2372 files changed, 559321 insertions, 48937 deletions

diff --git a/.mailmap b/.mailmap
index 97f7b4fb6139..4e83e7b52d15 100644
--- a/.mailmap
+++ b/.mailmap
@@ -32,6 +32,7 @@ Christoph Hellwig <hch@lst.de>
 Corey Minyard <minyard@acm.org>
 David Brownell <david-b@pacbell.net>
 David Woodhouse <dwmw2@shinybook.infradead.org>
+Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
 Domen Puncer <domen@coderock.org>
 Douglas Gilbert <dougg@torque.net>
 Ed L. Cashin <ecashin@coraid.com>
diff --git a/CREDITS b/CREDITS
index abe05a0be4ed..939da46a87fb 100644
--- a/CREDITS
+++ b/CREDITS
@@ -464,6 +464,11 @@ S: 1200 Goldenrod Dr.
 S: Nampa, Idaho 83686
 S: USA
 
+N: Dirk J. Brandewie
+E: dirk.j.brandewie@intel.com
+E: linux-wimax@intel.com
+D: Intel Wireless WiMAX Connection 2400 SDIO driver
+
 N: Derrick J. Brashear
 E: shadow@dementia.org
 W: http://www.dementia.org/~shadow
@@ -1681,7 +1686,7 @@ E: ajoshi@shell.unixbox.com
 D: fbdev hacking
 
 N: Jesper Juhl
-E: jesper.juhl@gmail.com
+E: jj@chaosbits.net
 D: Various fixes, cleanups and minor features all over the tree.
 D: Wrote initial version of the hdaps driver (since passed on to others).
 S: Lemnosvej 1, 3.tv
@@ -2119,6 +2124,11 @@ N: H.J. Lu
 E: hjl@gnu.ai.mit.edu
 D: GCC + libraries hacker
 
+N: Yanir Lubetkin
+E: yanirx.lubatkin@intel.com
+E: linux-wimax@intel.com
+D: Intel Wireless WiMAX Connection 2400 driver
+
 N: Michal Ludvig
 E: michal@logix.cz
 E: michal.ludvig@asterisk.co.nz
@@ -2693,6 +2703,13 @@ S: RR #5, 497 Pole Line Road
 S: Thunder Bay, Ontario
 S: CANADA P7C 5M9
 
+N: Inaky Perez-Gonzalez
+E: inaky.perez-gonzalez@intel.com
+E: linux-wimax@intel.com
+E: inakypg@yahoo.com
+D: WiMAX stack
+D: Intel Wireless WiMAX Connection 2400 driver
+
 N: Yuri Per
 E: yuri@pts.mipt.ru
 D: Some smbfs fixes
diff --git a/Documentation/ABI/testing/sysfs-devices-memory b/Documentation/ABI/testing/sysfs-devices-memory
index 7a16fe1e2270..9fe91c02ee40 100644
--- a/Documentation/ABI/testing/sysfs-devices-memory
+++ b/Documentation/ABI/testing/sysfs-devices-memory
@@ -6,7 +6,6 @@ Description:
 		internal state of the kernel memory blocks. Files could be
 		added or removed dynamically to represent hot-add/remove
 		operations.
-
 Users:		hotplug memory add/remove tools
 		https://w3.opensource.ibm.com/projects/powerpc-utils/
 
@@ -19,6 +18,56 @@ Description:
 		This is useful for a user-level agent to determine
 		identify removable sections of the memory before attempting
 		potentially expensive hot-remove memory operation
+Users:		hotplug memory remove tools
+		https://w3.opensource.ibm.com/projects/powerpc-utils/
+
+What:		/sys/devices/system/memory/memoryX/phys_device
+Date:		September 2008
+Contact:	Badari Pulavarty <pbadari@us.ibm.com>
+Description:
+		The file /sys/devices/system/memory/memoryX/phys_device
+		is read-only and is designed to show the name of physical
+		memory device.  Implementation is currently incomplete.
 
+What:		/sys/devices/system/memory/memoryX/phys_index
+Date:		September 2008
+Contact:	Badari Pulavarty <pbadari@us.ibm.com>
+Description:
+		The file /sys/devices/system/memory/memoryX/phys_index
+		is read-only and contains the section ID in hexadecimal
+		which is equivalent to decimal X contained in the
+		memory section directory name.
+
+What:		/sys/devices/system/memory/memoryX/state
+Date:		September 2008
+Contact:	Badari Pulavarty <pbadari@us.ibm.com>
+Description:
+		The file /sys/devices/system/memory/memoryX/state
+		is read-write.  When read, it's contents show the
+		online/offline state of the memory section.  When written,
+		root can toggle the the online/offline state of a removable
+		memory section (see removable file description above)
+		using the following commands.
+		# echo online > /sys/devices/system/memory/memoryX/state
+		# echo offline > /sys/devices/system/memory/memoryX/state
+
+		For example, if /sys/devices/system/memory/memory22/removable
+		contains a value of 1 and
+		/sys/devices/system/memory/memory22/state contains the
+		string "online" the following command can be executed by
+		by root to offline that section.
+		# echo offline > /sys/devices/system/memory/memory22/state
 Users:		hotplug memory remove tools
 		https://w3.opensource.ibm.com/projects/powerpc-utils/
+
+What:		/sys/devices/system/node/nodeX/memoryY
+Date:		September 2008
+Contact:	Gary Hade <garyhade@us.ibm.com>
+Description:
+		When CONFIG_NUMA is enabled
+		/sys/devices/system/node/nodeX/memoryY is a symbolic link that
+		points to the corresponding /sys/devices/system/memory/memoryY
+		memory section directory.  For example, the following symbolic
+		link is created for memory section 9 on node0.
+		/sys/devices/system/node/node0/memory9 -> ../../memory/memory9
+
diff --git a/Documentation/DMA-mapping.txt b/Documentation/DMA-mapping.txt
index c74fec8c2351..b2a4d6d244d9 100644
--- a/Documentation/DMA-mapping.txt
+++ b/Documentation/DMA-mapping.txt
@@ -26,7 +26,7 @@ mapped only for the time they are actually used and unmapped after the DMA
 transfer.
 
 The following API will work of course even on platforms where no such
-hardware exists, see e.g. include/asm-i386/pci.h for how it is implemented on
+hardware exists, see e.g. arch/x86/include/asm/pci.h for how it is implemented on
 top of the virt_to_bus interface.
 
 First of all, you should make sure
diff --git a/Documentation/DocBook/networking.tmpl b/Documentation/DocBook/networking.tmpl
index 627707a3cb9d..59ad69a9d777 100644
--- a/Documentation/DocBook/networking.tmpl
+++ b/Documentation/DocBook/networking.tmpl
@@ -74,6 +74,14 @@
 !Enet/sunrpc/rpcb_clnt.c
 !Enet/sunrpc/clnt.c
      </sect1>
+     <sect1><title>WiMAX</title>
+!Enet/wimax/op-msg.c
+!Enet/wimax/op-reset.c
+!Enet/wimax/op-rfkill.c
+!Enet/wimax/stack.c
+!Iinclude/net/wimax.h
+!Iinclude/linux/wimax.h
+     </sect1>
   </chapter>
 
   <chapter id="netdev">
diff --git a/Documentation/DocBook/uio-howto.tmpl b/Documentation/DocBook/uio-howto.tmpl
index df87d1b93605..b787e4721c90 100644
--- a/Documentation/DocBook/uio-howto.tmpl
+++ b/Documentation/DocBook/uio-howto.tmpl
@@ -42,6 +42,12 @@ GPL version 2.
 
 <revhistory>
 	<revision>
+	<revnumber>0.6</revnumber>
+	<date>2008-12-05</date>
+	<authorinitials>hjk</authorinitials>
+	<revremark>Added description of portio sysfs attributes.</revremark>
+	</revision>
+	<revision>
 	<revnumber>0.5</revnumber>
 	<date>2008-05-22</date>
 	<authorinitials>hjk</authorinitials>
@@ -318,6 +324,54 @@ interested in translating it, please email me
 offset = N * getpagesize();
 </programlisting>
 
+<para>
+	Sometimes there is hardware with memory-like regions that can not be
+	mapped with the technique described here, but there are still ways to
+	access them from userspace. The most common example are x86 ioports.
+	On x86 systems, userspace can access these ioports using
+	<function>ioperm()</function>, <function>iopl()</function>,
+	<function>inb()</function>, <function>outb()</function>, and similar
+	functions.
+</para>
+<para>
+	Since these ioport regions can not be mapped, they will not appear under
+	<filename>/sys/class/uio/uioX/maps/</filename> like the normal memory
+	described above. Without information about the port regions a hardware
+	has to offer, it becomes difficult for the userspace part of the
+	driver to find out which ports belong to which UIO device.
+</para>
+<para>
+	To address this situation, the new directory
+	<filename>/sys/class/uio/uioX/portio/</filename> was added. It only
+	exists if the driver wants to pass information about one or more port
+	regions to userspace. If that is the case, subdirectories named
+	<filename>port0</filename>, <filename>port1</filename>, and so on,
+	will appear underneath
+	<filename>/sys/class/uio/uioX/portio/</filename>.
+</para>
+<para>
+	Each <filename>portX/</filename> directory contains three read-only
+	files that show start, size, and type of the port region:
+</para>
+<itemizedlist>
+<listitem>
+	<para>
+	<filename>start</filename>: The first port of this region.
+	</para>
+</listitem>
+<listitem>
+	<para>
+	<filename>size</filename>: The number of ports in this region.
+	</para>
+</listitem>
+<listitem>
+	<para>
+	<filename>porttype</filename>: A string describing the type of port.
+	</para>
+</listitem>
+</itemizedlist>
+
+
 </sect1>
 </chapter>
 
@@ -339,12 +393,12 @@ offset = N * getpagesize();
 
 <itemizedlist>
 <listitem><para>
-<varname>char *name</varname>: Required. The name of your driver as
+<varname>const char *name</varname>: Required. The name of your driver as
 it will appear in sysfs. I recommend using the name of your module for this.
 </para></listitem>
 
 <listitem><para>
-<varname>char *version</varname>: Required. This string appears in
+<varname>const char *version</varname>: Required. This string appears in
 <filename>/sys/class/uio/uioX/version</filename>.
 </para></listitem>
 
@@ -356,6 +410,13 @@ See the description below for details.
 </para></listitem>
 
 <listitem><para>
+<varname>struct uio_port port[ MAX_UIO_PORTS_REGIONS ]</varname>: Required
+if you want to pass information about ioports to userspace. For each port
+region you need to fill one of the <varname>uio_port</varname> structures.
+See the description below for details.
+</para></listitem>
+
+<listitem><para>
 <varname>long irq</varname>: Required. If your hardware generates an
 interrupt, it's your modules task to determine the irq number during
 initialization. If you don't have a hardware generated interrupt but
@@ -448,6 +509,42 @@ Please do not touch the <varname>kobj</varname> element of
 <varname>struct uio_mem</varname>! It is used by the UIO framework
 to set up sysfs files for this mapping. Simply leave it alone.
 </para>
+
+<para>
+Sometimes, your device can have one or more port regions which can not be
+mapped to userspace. But if there are other possibilities for userspace to
+access these ports, it makes sense to make information about the ports
+available in sysfs. For each region, you have to set up a
+<varname>struct uio_port</varname> in the <varname>port[]</varname> array.
+Here's a description of the fields of <varname>struct uio_port</varname>:
+</para>
+
+<itemizedlist>
+<listitem><para>
+<varname>char *porttype</varname>: Required. Set this to one of the predefined
+constants. Use <varname>UIO_PORT_X86</varname> for the ioports found in x86
+architectures.
+</para></listitem>
+
+<listitem><para>
+<varname>unsigned long start</varname>: Required if the port region is used.
+Fill in the number of the first port of this region.
+</para></listitem>
+
+<listitem><para>
+<varname>unsigned long size</varname>: Fill in the number of ports in this
+region. If <varname>size</varname> is zero, the region is considered unused.
+Note that you <emphasis>must</emphasis> initialize <varname>size</varname>
+with zero for all unused regions.
+</para></listitem>
+</itemizedlist>
+
+<para>
+Please do not touch the <varname>portio</varname> element of
+<varname>struct uio_port</varname>! It is used internally by the UIO
+framework to set up sysfs files for this region. Simply leave it alone.
+</para>
+
 </sect1>
 
 <sect1 id="adding_irq_handler">
diff --git a/Documentation/PCI/pci.txt b/Documentation/PCI/pci.txt
index fd4907a2968c..7f6de6ea5b47 100644
--- a/Documentation/PCI/pci.txt
+++ b/Documentation/PCI/pci.txt
@@ -294,7 +294,8 @@ NOTE: pci_enable_device() can fail! Check the return value.
 
 pci_set_master() will enable DMA by setting the bus master bit
 in the PCI_COMMAND register. It also fixes the latency timer value if
-it's set to something bogus by the BIOS.
+it's set to something bogus by the BIOS.  pci_clear_master() will
+disable DMA by clearing the bus master bit.
 
 If the PCI device can use the PCI Memory-Write-Invalidate transaction,
 call pci_set_mwi().  This enables the PCI_COMMAND bit for Mem-Wr-Inval
diff --git a/Documentation/blackfin/00-INDEX b/Documentation/blackfin/00-INDEX
index 7cb3b356b249..d6840a91e1e1 100644
--- a/Documentation/blackfin/00-INDEX
+++ b/Documentation/blackfin/00-INDEX
@@ -9,3 +9,6 @@ cachefeatures.txt
 
 Filesystems
 	- Requirements for mounting the root file system.
+
+bfin-gpio-note.txt
+	- Notes in developing/using bfin-gpio driver.
diff --git a/Documentation/blackfin/bfin-gpio-notes.txt b/Documentation/blackfin/bfin-gpio-notes.txt
new file mode 100644
index 000000000000..9898c7ded7d3
--- /dev/null
+++ b/Documentation/blackfin/bfin-gpio-notes.txt
@@ -0,0 +1,71 @@
+/*
+ * File:         Documentation/blackfin/bfin-gpio-note.txt
+ * Based on:
+ * Author:
+ *
+ * Created:      $Id: bfin-gpio-note.txt 2008-11-24 16:42 grafyang $
+ * Description:  This file contains the notes in developing/using bfin-gpio.
+ *
+ *
+ * Rev:
+ *
+ * Modified:
+ *               Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ */
+
+
+1. Blackfin GPIO introduction
+
+    There are many GPIO pins on Blackfin. Most of these pins are muxed to
+    multi-functions. They can be configured as peripheral, or just as GPIO,
+    configured to input with interrupt enabled, or output.
+
+    For detailed information, please see "arch/blackfin/kernel/bfin_gpio.c",
+    or the relevant HRM.
+
+
+2. Avoiding resource conflict
+
+    Followed function groups are used to avoiding resource conflict,
+    - Use the pin as peripheral,
+	int peripheral_request(unsigned short per, const char *label);
+	int peripheral_request_list(const unsigned short per[], const char *label);
+	void peripheral_free(unsigned short per);
+	void peripheral_free_list(const unsigned short per[]);
+    - Use the pin as GPIO,
+	int bfin_gpio_request(unsigned gpio, const char *label);
+	void bfin_gpio_free(unsigned gpio);
+    - Use the pin as GPIO interrupt,
+	int bfin_gpio_irq_request(unsigned gpio, const char *label);
+	void bfin_gpio_irq_free(unsigned gpio);
+
+    The request functions will record the function state for a certain pin,
+    the free functions will clear it's function state.
+    Once a pin is requested, it can't be requested again before it is freed by
+    previous caller, otherwise kernel will dump stacks, and the request
+    function fail.
+    These functions are wrapped by other functions, most of the users need not
+    care.
+
+
+3. But there are some exceptions
+    - Kernel permit the identical GPIO be requested both as GPIO and GPIO
+    interrut.
+    Some drivers, like gpio-keys, need this behavior. Kernel only print out
+    warning messages like,
+	bfin-gpio: GPIO 24 is already reserved by gpio-keys: BTN0, and you are
+configuring it as IRQ!
+
+        Note: Consider the case that, if there are two drivers need the
+	identical GPIO, one of them use it as GPIO, the other use it as
+	GPIO interrupt. This will really cause resource conflict. So if
+	there is any abnormal driver behavior, please check the bfin-gpio
+	warning messages.
+
+    - Kernel permit the identical GPIO be requested from the same driver twice.
+
+
+
diff --git a/Documentation/dell_rbu.txt b/Documentation/dell_rbu.txt
index 2c0d631de0cf..c11b931f8f98 100644
--- a/Documentation/dell_rbu.txt
+++ b/Documentation/dell_rbu.txt
@@ -81,8 +81,8 @@ Until this step is completed the driver cannot be unloaded.
 Also echoing either mono ,packet or init in to image_type will free up the
 memory allocated by the driver.
 
-If an user by accident executes steps 1 and 3 above without executing step 2;
-it will make the /sys/class/firmware/dell_rbu/ entries to disappear.
+If a user by accident executes steps 1 and 3 above without executing step 2;
+it will make the /sys/class/firmware/dell_rbu/ entries disappear.
 The entries can be recreated by doing the following
 echo init > /sys/devices/platform/dell_rbu/image_type
 NOTE: echoing init in image_type does not change it original value.
diff --git a/Documentation/feature-removal-schedule.txt b/Documentation/feature-removal-schedule.txt
index df18d87c4837..5ddbe350487a 100644
--- a/Documentation/feature-removal-schedule.txt
+++ b/Documentation/feature-removal-schedule.txt
@@ -315,3 +315,23 @@ When:	2.6.29 (ideally) or 2.6.30 (more likely)
 Why:	Deprecated by the new (standard) device driver binding model. Use
 	i2c_driver->probe() and ->remove() instead.
 Who:	Jean Delvare <khali@linux-fr.org>
+
+---------------------------
+
+What:	fscher and fscpos drivers
+When:	June 2009
+Why:	Deprecated by the new fschmd driver.
+Who:	Hans de Goede <hdegoede@redhat.com>
+	Jean Delvare <khali@linux-fr.org>
+
+---------------------------
+
+What:	SELinux "compat_net" functionality
+When:	2.6.30 at the earliest
+Why:	In 2.6.18 the Secmark concept was introduced to replace the "compat_net"
+	network access control functionality of SELinux.  Secmark offers both
+	better performance and greater flexibility than the "compat_net"
+	mechanism.  Now that the major Linux distributions have moved to
+	Secmark, it is time to deprecate the older mechanism and start the
+	process of removing the old code.
+Who:	Paul Moore <paul.moore@hp.com>
diff --git a/Documentation/filesystems/Locking b/Documentation/filesystems/Locking
index ccec55394380..cfbfa15a46ba 100644
--- a/Documentation/filesystems/Locking
+++ b/Documentation/filesystems/Locking
@@ -397,7 +397,7 @@ prototypes:
 };
 
 locking rules:
-	All except ->poll() may block.
+	All may block.
 			BKL
 llseek:			no	(see below)
 read:			no
diff --git a/Documentation/filesystems/proc.txt b/Documentation/filesystems/proc.txt
index 71df353e367c..d105eb45282a 100644
--- a/Documentation/filesystems/proc.txt
+++ b/Documentation/filesystems/proc.txt
@@ -140,6 +140,7 @@ Table 1-1: Process specific entries in /proc
  statm		Process memory status information
  status		Process status in human readable form
  wchan		If CONFIG_KALLSYMS is set, a pre-decoded wchan
+ stack		Report full stack trace, enable via CONFIG_STACKTRACE
  smaps		Extension based on maps, the rss size for each mapped file
 ..............................................................................
 
@@ -1385,6 +1386,15 @@ swapcache reclaim.  Decreasing vfs_cache_pressure causes the kernel to prefer
 to retain dentry and inode caches.  Increasing vfs_cache_pressure beyond 100
 causes the kernel to prefer to reclaim dentries and inodes.
 
+dirty_background_bytes
+----------------------
+
+Contains the amount of dirty memory at which the pdflush background writeback
+daemon will start writeback.
+
+If dirty_background_bytes is written, dirty_background_ratio becomes a function
+of its value (dirty_background_bytes / the amount of dirtyable system memory).
+
 dirty_background_ratio
 ----------------------
 
@@ -1393,14 +1403,29 @@ pages + file cache, not including locked pages and HugePages), the number of
 pages at which the pdflush background writeback daemon will start writing out
 dirty data.
 
+If dirty_background_ratio is written, dirty_background_bytes becomes a function
+of its value (dirty_background_ratio * the amount of dirtyable system memory).
+
+dirty_bytes
+-----------
+
+Contains the amount of dirty memory at which a process generating disk writes
+will itself start writeback.
+
+If dirty_bytes is written, dirty_ratio becomes a function of its value
+(dirty_bytes / the amount of dirtyable system memory).
+
 dirty_ratio
------------------
+-----------
 
 Contains, as a percentage of the dirtyable system memory (free pages + mapped
 pages + file cache, not including locked pages and HugePages), the number of
 pages at which a process which is generating disk writes will itself start
 writing out dirty data.
 
+If dirty_ratio is written, dirty_bytes becomes a function of its value
+(dirty_ratio * the amount of dirtyable system memory).
+
 dirty_writeback_centisecs
 -------------------------
 
diff --git a/Documentation/hwmon/abituguru-datasheet b/Documentation/hwmon/abituguru-datasheet
index aef5a9b36846..4d184f2db0ea 100644
--- a/Documentation/hwmon/abituguru-datasheet
+++ b/Documentation/hwmon/abituguru-datasheet
@@ -74,7 +74,7 @@ a sensor.
 Notice that some banks have both a read and a write address this is how the
 uGuru determines if a read from or a write to the bank is taking place, thus
 when reading you should always use the read address and when writing the
-write address. The write address is always one (1) more then the read address.
+write address. The write address is always one (1) more than the read address.
 
 
 uGuru ready
@@ -224,7 +224,7 @@ Bit 3: Beep if alarm							(RW)
 Bit 4: 1 if alarm cause measured temp is over the warning threshold	(R)
 Bit 5: 1 if alarm cause measured volt is over the max threshold		(R)
 Bit 6: 1 if alarm cause measured volt is under the min threshold	(R)
-Bit 7: Volt sensor: Shutdown if alarm persist for more then 4 seconds	(RW)
+Bit 7: Volt sensor: Shutdown if alarm persist for more than 4 seconds	(RW)
        Temp sensor: Shutdown if temp is over the shutdown threshold	(RW)
 
 *  This bit is only honored/used by the uGuru if a temp sensor is connected
@@ -293,7 +293,7 @@ Byte 0:
 Alarm behaviour for the selected sensor. A 1 enables the described behaviour.
 Bit 0: Give an alarm if measured rpm is under the min threshold	(RW)
 Bit 3: Beep if alarm						(RW)
-Bit 7: Shutdown if alarm persist for more then 4 seconds	(RW)
+Bit 7: Shutdown if alarm persist for more than 4 seconds	(RW)
 
 Byte 1:
 min threshold (scale as bank 0x26)
diff --git a/Documentation/hwmon/adt7470 b/Documentation/hwmon/adt7470
index 75d13ca147cc..8ce4aa0a0f55 100644
--- a/Documentation/hwmon/adt7470
+++ b/Documentation/hwmon/adt7470
@@ -31,15 +31,11 @@ Each of the measured inputs (temperature, fan speed) has corresponding high/low
 limit values. The ADT7470 will signal an ALARM if any measured value exceeds
 either limit.
 
-The ADT7470 DOES NOT sample all inputs continuously.  A single pin on the
-ADT7470 is connected to a multitude of thermal diodes, but the chip must be
-instructed explicitly to read the multitude of diodes.  If you want to use
-automatic fan control mode, you must manually read any of the temperature
-sensors or the fan control algorithm will not run.  The chip WILL NOT DO THIS
-AUTOMATICALLY; this must be done from userspace.  This may be a bug in the chip
-design, given that many other AD chips take care of this.  The driver will not
-read the registers more often than once every 5 seconds.  Further,
-configuration data is only read once per minute.
+The ADT7470 samples all inputs continuously.  A kernel thread is started up for
+the purpose of periodically querying the temperature sensors, thus allowing the
+automatic fan pwm control to set the fan speed.  The driver will not read the
+registers more often than once every 5 seconds.  Further, configuration data is
+only read once per minute.
 
 Special Features
 ----------------
@@ -72,5 +68,6 @@ pwm#_auto_point2_temp.
 Notes
 -----
 
-As stated above, the temperature inputs must be read periodically from
-userspace in order for the automatic pwm algorithm to run.
+The temperature inputs no longer need to be read periodically from userspace in
+order for the automatic pwm algorithm to run.  This was the case for earlier
+versions of the driver.
diff --git a/Documentation/hwmon/f71882fg b/Documentation/hwmon/f71882fg
new file mode 100644
index 000000000000..a8321267b5b6
--- /dev/null
+++ b/Documentation/hwmon/f71882fg
@@ -0,0 +1,89 @@
+Kernel driver f71882fg
+======================
+
+Supported chips:
+  * Fintek F71882FG and F71883FG
+    Prefix: 'f71882fg'
+    Addresses scanned: none, address read from Super I/O config space
+    Datasheet: Available from the Fintek website
+  * Fintek F71862FG and F71863FG
+    Prefix: 'f71862fg'
+    Addresses scanned: none, address read from Super I/O config space
+    Datasheet: Available from the Fintek website
+  * Fintek F8000
+    Prefix: 'f8000'
+    Addresses scanned: none, address read from Super I/O config space
+    Datasheet: Not public
+
+Author: Hans de Goede <hdegoede@redhat.com>
+
+
+Description
+-----------
+
+Fintek F718xxFG/F8000 Super I/O chips include complete hardware monitoring
+capabilities. They can monitor up to 9 voltages (3 for the F8000), 4 fans and
+3 temperature sensors.
+
+These chips also have fan controlling features, using either DC or PWM, in
+three different modes (one manual, two automatic).
+
+The driver assumes that no more than one chip is present, which seems
+reasonable.
+
+
+Monitoring
+----------
+
+The Voltage, Fan and Temperature Monitoring uses the standard sysfs
+interface as documented in sysfs-interface, without any exceptions.
+
+
+Fan Control
+-----------
+
+Both PWM (pulse-width modulation) and DC fan speed control methods are
+supported. The right one to use depends on external circuitry on the
+motherboard, so the driver assumes that the BIOS set the method
+properly.
+
+There are 2 modes to specify the speed of the fan, PWM duty cycle (or DC
+voltage) mode, where 0-100% duty cycle (0-100% of 12V) is specified. And RPM
+mode where the actual RPM of the fan (as measured) is controlled and the speed
+gets specified as 0-100% of the fan#_full_speed file.
+
+Since both modes work in a 0-100% (mapped to 0-255) scale, there isn't a
+whole lot of a difference when modifying fan control settings. The only
+important difference is that in RPM mode the 0-100% controls the fan speed
+between 0-100% of fan#_full_speed. It is assumed that if the BIOS programs
+RPM mode, it will also set fan#_full_speed properly, if it does not then
+fan control will not work properly, unless you set a sane fan#_full_speed
+value yourself.
+
+Switching between these modes requires re-initializing a whole bunch of
+registers, so the mode which the BIOS has set is kept. The mode is
+printed when loading the driver.
+
+Three different fan control modes are supported; the mode number is written
+to the pwm#_enable file. Note that not all modes are supported on all
+chips, and some modes may only be available in RPM / PWM mode on the F8000.
+Writing an unsupported mode will result in an invalid parameter error.
+
+* 1: Manual mode
+  You ask for a specific PWM duty cycle / DC voltage or a specific % of
+  fan#_full_speed by writing to the pwm# file. This mode is only
+  available on the F8000 if the fan channel is in RPM mode.
+
+* 2: Normal auto mode
+  You can define a number of temperature/fan speed trip points, which % the
+  fan should run at at this temp and which temp a fan should follow using the
+  standard sysfs interface. The number and type of trip points is chip
+  depended, see which files are available in sysfs.
+  Fan/PWM channel 3 of the F8000 is always in this mode!
+
+* 3: Thermostat mode (Only available on the F8000 when in duty cycle mode)
+  The fan speed is regulated to keep the temp the fan is mapped to between
+  temp#_auto_point2_temp and temp#_auto_point3_temp.
+
+Both of the automatic modes require that pwm1 corresponds to fan1, pwm2 to
+fan2 and pwm3 to fan3.
diff --git a/Documentation/hwmon/it87 b/Documentation/hwmon/it87
index 042c0415140b..659315d98e00 100644
--- a/Documentation/hwmon/it87
+++ b/Documentation/hwmon/it87
@@ -26,6 +26,10 @@ Supported chips:
     Datasheet: Publicly available at the ITE website
                http://www.ite.com.tw/product_info/file/pc/IT8718F_V0.2.zip
                http://www.ite.com.tw/product_info/file/pc/IT8718F_V0%203_(for%20C%20version).zip
+  * IT8720F
+    Prefix: 'it8720'
+    Addresses scanned: from Super I/O config space (8 I/O ports)
+    Datasheet: Not yet publicly available.
   * SiS950   [clone of IT8705F]
     Prefix: 'it87'
     Addresses scanned: from Super I/O config space (8 I/O ports)
@@ -71,7 +75,7 @@ Description
 -----------
 
 This driver implements support for the IT8705F, IT8712F, IT8716F,
-IT8718F, IT8726F and SiS950 chips.
+IT8718F, IT8720F, IT8726F and SiS950 chips.
 
 These chips are 'Super I/O chips', supporting floppy disks, infrared ports,
 joysticks and other miscellaneous stuff. For hardware monitoring, they
@@ -84,19 +88,19 @@ the IT8716F and late IT8712F have 6. They are shared with other functions
 though, so the functionality may not be available on a given system.
 The driver dumbly assume it is there.
 
-The IT8718F also features VID inputs (up to 8 pins) but the value is
-stored in the Super-I/O configuration space. Due to technical limitations,
+The IT8718F and IT8720F also features VID inputs (up to 8 pins) but the value
+is stored in the Super-I/O configuration space. Due to technical limitations,
 this value can currently only be read once at initialization time, so
 the driver won't notice and report changes in the VID value. The two
 upper VID bits share their pins with voltage inputs (in5 and in6) so you
 can't have both on a given board.
 
-The IT8716F, IT8718F and later IT8712F revisions have support for
+The IT8716F, IT8718F, IT8720F and later IT8712F revisions have support for
 2 additional fans. The additional fans are supported by the driver.
 
-The IT8716F and IT8718F, and late IT8712F and IT8705F also have optional
-16-bit tachometer counters for fans 1 to 3. This is better (no more fan
-clock divider mess) but not compatible with the older chips and
+The IT8716F, IT8718F and IT8720F, and late IT8712F and IT8705F also have
+optional 16-bit tachometer counters for fans 1 to 3. This is better (no more
+fan clock divider mess) but not compatible with the older chips and
 revisions. The 16-bit tachometer mode is enabled by the driver when one
 of the above chips is detected.
 
@@ -122,7 +126,7 @@ zero'; this is important for negative voltage measurements. All voltage
 inputs can measure voltages between 0 and 4.08 volts, with a resolution of
 0.016 volt. The battery voltage in8 does not have limit registers.
 
-The VID lines (IT8712F/IT8716F/IT8718F) encode the core voltage value:
+The VID lines (IT8712F/IT8716F/IT8718F/IT8720F) encode the core voltage value:
 the voltage level your processor should work with. This is hardcoded by
 the mainboard and/or processor itself. It is a value in volts.
 
diff --git a/Documentation/hwmon/lm70 b/Documentation/hwmon/lm70
index 2bdd3feebf53..0d240291e3cc 100644
--- a/Documentation/hwmon/lm70
+++ b/Documentation/hwmon/lm70
@@ -1,9 +1,11 @@
 Kernel driver lm70
 ==================
 
-Supported chip:
+Supported chips:
   * National Semiconductor LM70
     Datasheet: http://www.national.com/pf/LM/LM70.html
+  * Texas Instruments TMP121/TMP123
+    Information: http://focus.ti.com/docs/prod/folders/print/tmp121.html
 
 Author:
         Kaiwan N Billimoria <kaiwan@designergraphix.com>
@@ -25,6 +27,14 @@ complement digital temperature (sent via the SIO line), is available in the
 driver for interpretation. This driver makes use of the kernel's in-core
 SPI support.
 
+As a real (in-tree) example of this "SPI protocol driver" interfacing
+with a "SPI master controller driver", see drivers/spi/spi_lm70llp.c
+and its associated documentation.
+
+The TMP121/TMP123 are very similar; main differences are 4 wire SPI inter-
+face (read only) and 13-bit temperature data (0.0625 degrees celsius reso-
+lution).
+
 Thanks to
 ---------
 Jean Delvare <khali@linux-fr.org> for mentoring the hwmon-side driver
diff --git a/Documentation/hwmon/lm85 b/Documentation/hwmon/lm85
index 400620741290..a13680871bc7 100644
--- a/Documentation/hwmon/lm85
+++ b/Documentation/hwmon/lm85
@@ -164,7 +164,7 @@ configured individually according to the following options.
                       temperature. (PWM value from 0 to 255)
 
 * pwm#_auto_pwm_minctl - this flags selects for temp#_auto_temp_off temperature
-                         the bahaviour of fans. Write 1 to let fans spinning at
+                         the behaviour of fans. Write 1 to let fans spinning at
 			 pwm#_auto_pwm_min or write 0 to let them off.
 
 NOTE: It has been reported that there is a bug in the LM85 that causes the flag
diff --git a/Documentation/hwmon/ltc4245 b/Documentation/hwmon/ltc4245
new file mode 100644
index 000000000000..bae7a3adc5d8
--- /dev/null
+++ b/Documentation/hwmon/ltc4245
@@ -0,0 +1,81 @@
+Kernel driver ltc4245
+=====================
+
+Supported chips:
+  * Linear Technology LTC4245
+    Prefix: 'ltc4245'
+    Addresses scanned: 0x20-0x3f
+    Datasheet:
+        http://www.linear.com/pc/downloadDocument.do?navId=H0,C1,C1003,C1006,C1140,P19392,D13517
+
+Author: Ira W. Snyder <iws@ovro.caltech.edu>
+
+
+Description
+-----------
+
+The LTC4245 controller allows a board to be safely inserted and removed
+from a live backplane in multiple supply systems such as CompactPCI and
+PCI Express.
+
+
+Usage Notes
+-----------
+
+This driver does not probe for LTC4245 devices, due to the fact that some
+of the possible addresses are unfriendly to probing. You will need to use
+the "force" parameter to tell the driver where to find the device.
+
+Example: the following will load the driver for an LTC4245 at address 0x23
+on I2C bus #1:
+$ modprobe ltc4245 force=1,0x23
+
+
+Sysfs entries
+-------------
+
+The LTC4245 has built-in limits for over and under current warnings. This
+makes it very likely that the reference circuit will be used.
+
+This driver uses the values in the datasheet to change the register values
+into the values specified in the sysfs-interface document. The current readings
+rely on the sense resistors listed in Table 2: "Sense Resistor Values".
+
+in1_input		12v input voltage (mV)
+in2_input		5v  input voltage (mV)
+in3_input		3v  input voltage (mV)
+in4_input		Vee (-12v) input voltage (mV)
+
+in1_min_alarm		12v input undervoltage alarm
+in2_min_alarm		5v  input undervoltage alarm
+in3_min_alarm		3v  input undervoltage alarm
+in4_min_alarm		Vee (-12v) input undervoltage alarm
+
+curr1_input		12v current (mA)
+curr2_input		5v  current (mA)
+curr3_input		3v  current (mA)
+curr4_input		Vee (-12v) current (mA)
+
+curr1_max_alarm		12v overcurrent alarm
+curr2_max_alarm		5v  overcurrent alarm
+curr3_max_alarm		3v  overcurrent alarm
+curr4_max_alarm		Vee (-12v) overcurrent alarm
+
+in5_input		12v output voltage (mV)
+in6_input		5v  output voltage (mV)
+in7_input		3v  output voltage (mV)
+in8_input		Vee (-12v) output voltage (mV)
+
+in5_min_alarm		12v output undervoltage alarm
+in6_min_alarm		5v  output undervoltage alarm
+in7_min_alarm		3v  output undervoltage alarm
+in8_min_alarm		Vee (-12v) output undervoltage alarm
+
+in9_input		GPIO #1 voltage data
+in10_input		GPIO #2 voltage data
+in11_input		GPIO #3 voltage data
+
+power1_input		12v power usage (mW)
+power2_input		5v  power usage (mW)
+power3_input		3v  power usage (mW)
+power4_input		Vee (-12v) power usage (mW)
diff --git a/Documentation/ide/warm-plug-howto.txt b/Documentation/ide/warm-plug-howto.txt
index d5885468b072..98152bcd515a 100644
--- a/Documentation/ide/warm-plug-howto.txt
+++ b/Documentation/ide/warm-plug-howto.txt
@@ -11,3 +11,8 @@ unplug old device(s) and plug new device(s)
 # echo -n "1" > /sys/class/ide_port/idex/scan
 
 done
+
+NOTE: please make sure that partitions are unmounted and that there are
+no other active references to devices before doing "delete_devices" step,
+also do not attempt "scan" step on devices currently in use -- otherwise
+results may be unpredictable and lead to data loss if you're unlucky
diff --git a/Documentation/input/walkera0701.txt b/Documentation/input/walkera0701.txt
new file mode 100644
index 000000000000..8f4289efc5c4
--- /dev/null
+++ b/Documentation/input/walkera0701.txt
@@ -0,0 +1,109 @@
+
+Walkera WK-0701 transmitter is supplied with a ready to fly Walkera
+helicopters such as HM36, HM37, HM60. The walkera0701 module enables to use
+this transmitter as joystick
+
+Devel homepage and download:
+http://zub.fei.tuke.sk/walkera-wk0701/
+
+or use cogito:
+cg-clone http://zub.fei.tuke.sk/GIT/walkera0701-joystick
+
+
+Connecting to PC:
+
+At back side of transmitter S-video connector can be found. Modulation
+pulses from processor to HF part can be found at pin 2 of this connector,
+pin 3 is GND. Between pin 3 and CPU 5k6 resistor can be found. To get
+modulation pulses to PC, signal pulses must be amplified.
+
+Cable: (walkera TX to parport)
+
+Walkera WK-0701 TX S-VIDEO connector:
+ (back side of TX)
+     __   __              S-video:                                  canon25
+    /  |_|  \             pin 2 (signal)              NPN           parport
+   / O 4 3 O \            pin 3 (GND)        LED        ________________  10 ACK
+  ( O 2   1 O )                                         | C
+   \   ___   /      2 ________________________|\|_____|/
+    | [___] |                                 |/|   B |\
+     -------        3 __________________________________|________________ 25 GND
+                                                          E
+
+
+I use green LED and BC109 NPN transistor.
+
+Software:
+
+Build kernel with walkera0701 module. Module walkera0701 need exclusive
+access to parport, modules like lp must be unloaded before loading
+walkera0701 module, check dmesg for error messages. Connect TX to PC by
+cable and run jstest /dev/input/js0 to see values from TX. If no value can
+be changed by TX "joystick", check output from /proc/interrupts. Value for
+(usually irq7) parport must increase if TX is on.
+
+
+
+Technical details:
+
+Driver use interrupt from parport ACK input bit to measure pulse length
+using hrtimers.
+
+Frame format:
+Based on walkera WK-0701 PCM Format description by Shaul Eizikovich.
+(downloaded from http://www.smartpropoplus.com/Docs/Walkera_Wk-0701_PCM.pdf)
+
+Signal pulses:
+                   (ANALOG)
+    SYNC      BIN   OCT
+  +---------+      +------+
+  |         |      |      |
+--+         +------+      +---
+
+Frame:
+ SYNC , BIN1, OCT1, BIN2, OCT2 ... BIN24, OCT24, BIN25, next frame SYNC ..
+
+pulse length:
+   Binary values:		Analog octal values:
+
+   288 uS Binary 0		318 uS       000
+   438 uS Binary 1		398 uS       001
+				478 uS       010
+				558 uS       011
+				638 uS       100
+  1306 uS SYNC			718 uS       101
+				798 uS       110
+				878 uS       111
+
+24 bin+oct values + 1 bin value = 24*4+1 bits  = 97 bits
+
+(Warning, pulses on ACK ar inverted by transistor, irq is rised up on sync
+to bin change or octal value to bin change).
+
+Binary data representations:
+
+One binary and octal value can be grouped to nibble. 24 nibbles + one binary
+values can be sampled between sync pulses.
+
+Values for first four channels (analog joystick values) can be found in
+first 10 nibbles. Analog value is represented by one sign bit and 9 bit
+absolute binary value. (10 bits per channel). Next nibble is checksum for
+first ten nibbles.
+
+Next nibbles 12 .. 21 represents four channels (not all channels can be
+directly controlled from TX). Binary representations ar the same as in first
+four channels. In nibbles 22 and 23 is a special magic number. Nibble 24 is
+checksum for nibbles 12..23.
+
+After last octal value for nibble 24 and next sync pulse one additional
+binary value can be sampled. This bit and magic number is not used in
+software driver. Some details about this magic numbers can be found in
+Walkera_Wk-0701_PCM.pdf.
+
+Checksum calculation:
+
+Summary of octal values in nibbles must be same as octal value in checksum
+nibble (only first 3 bits are used). Binary value for checksum nibble is
+calculated by sum of binary values in checked nibbles + sum of octal values
+in checked nibbles divided by 8. Only bit 0 of this sum is used.
+
diff --git a/Documentation/ioctl/ioctl-number.txt b/Documentation/ioctl/ioctl-number.txt
index 824699174436..f1d639903325 100644
--- a/Documentation/ioctl/ioctl-number.txt
+++ b/Documentation/ioctl/ioctl-number.txt
@@ -84,7 +84,7 @@ Code	Seq#	Include File		Comments
 'B'	C0-FF				advanced bbus
 					<mailto:maassen@uni-freiburg.de>
 'C'	all	linux/soundcard.h
-'D'	all	asm-s390/dasd.h
+'D'	all	arch/s390/include/asm/dasd.h
 'E'	all	linux/input.h
 'F'	all	linux/fb.h
 'H'	all	linux/hiddev.h
@@ -105,7 +105,7 @@ Code	Seq#	Include File		Comments
 'S'	80-81	scsi/scsi_ioctl.h	conflict!
 'S'	82-FF	scsi/scsi.h		conflict!
 'T'	all	linux/soundcard.h	conflict!
-'T'	all	asm-i386/ioctls.h	conflict!
+'T'	all	arch/x86/include/asm/ioctls.h	conflict!
 'U'	00-EF	linux/drivers/usb/usb.h
 'V'	all	linux/vt.h
 'W'	00-1F	linux/watchdog.h	conflict!
@@ -120,7 +120,7 @@ Code	Seq#	Include File		Comments
 					<mailto:natalia@nikhefk.nikhef.nl>
 'c'	00-7F	linux/comstats.h	conflict!
 'c'	00-7F	linux/coda.h		conflict!
-'c'	80-9F	asm-s390/chsc.h
+'c'	80-9F	arch/s390/include/asm/chsc.h
 'd'	00-FF	linux/char/drm/drm/h	conflict!
 'd'	00-DF	linux/video_decoder.h	conflict!
 'd'	F0-FF	linux/digi1.h
@@ -170,7 +170,7 @@ Code	Seq#	Include File		Comments
 					<mailto:oe@port.de>
 0x80	00-1F	linux/fb.h
 0x81	00-1F	linux/videotext.h
-0x89	00-06	asm-i386/sockios.h
+0x89	00-06	arch/x86/include/asm/sockios.h
 0x89	0B-DF	linux/sockios.h
 0x89	E0-EF	linux/sockios.h		SIOCPROTOPRIVATE range
 0x89	F0-FF	linux/sockios.h		SIOCDEVPRIVATE range
diff --git a/Documentation/kbuild/kbuild.txt b/Documentation/kbuild/kbuild.txt
index 51771847e816..923f9ddee8f6 100644
--- a/Documentation/kbuild/kbuild.txt
+++ b/Documentation/kbuild/kbuild.txt
@@ -124,3 +124,10 @@ KBUILD_EXTRA_SYMBOLS
 --------------------------------------------------
 For modules use symbols from another modules.
 See more details in modules.txt.
+
+ALLSOURCE_ARCHS
+--------------------------------------------------
+For tags/TAGS/cscope targets, you can specify more than one archs
+to be included in the databases, separated by blankspace. e.g.
+
+    $ make ALLSOURCE_ARCHS="x86 mips arm" tags
diff --git a/Documentation/kbuild/modules.txt b/Documentation/kbuild/modules.txt
index 1821c077b435..b1096da953c8 100644
--- a/Documentation/kbuild/modules.txt
+++ b/Documentation/kbuild/modules.txt
@@ -253,7 +253,7 @@ following files:
 
 		# Module specific targets
 		genbin:
-			echo "X" > 8123_bin_shipped
+			echo "X" > 8123_bin.o_shipped
 
 
 	In example 2, we are down to two fairly simple files and for simple
@@ -279,7 +279,7 @@ following files:
 
 		# Module specific targets
 		genbin:
-			echo "X" > 8123_bin_shipped
+			echo "X" > 8123_bin.o_shipped
 
 		endif
 
diff --git a/Documentation/kernel-doc-nano-HOWTO.txt b/Documentation/kernel-doc-nano-HOWTO.txt
index c6841eee9598..d73fbd2b2b45 100644
--- a/Documentation/kernel-doc-nano-HOWTO.txt
+++ b/Documentation/kernel-doc-nano-HOWTO.txt
@@ -71,6 +71,11 @@ The @argument descriptions must begin on the very next line following
 this opening short function description line, with no intervening
 empty comment lines.
 
+If a function parameter is "..." (varargs), it should be listed in
+kernel-doc notation as:
+ * @...: description
+
+
 Example kernel-doc data structure comment.
 
 /**
@@ -282,6 +287,32 @@ struct my_struct {
 };
 
 
+Including documentation blocks in source files
+----------------------------------------------
+
+To facilitate having source code and comments close together, you can
+include kernel-doc documentation blocks that are free-form comments
+instead of being kernel-doc for functions, structures, unions,
+enums, or typedefs.  This could be used for something like a
+theory of operation for a driver or library code, for example.
+
+This is done by using a DOC: section keyword with a section title.  E.g.:
+
+/**
+ * DOC: Theory of Operation
+ *
+ * The whizbang foobar is a dilly of a gizmo.  It can do whatever you
+ * want it to do, at any time.  It reads your mind.  Here's how it works.
+ *
+ * foo bar splat
+ *
+ * The only drawback to this gizmo is that is can sometimes damage
+ * hardware, software, or its subject(s).
+ */
+
+DOC: sections are used in SGML templates files as indicated below.
+
+
 How to make new SGML template files
 -----------------------------------
 
@@ -302,6 +333,9 @@ exported using EXPORT_SYMBOL.
 !F<filename> <function [functions...]> is replaced by the
 documentation, in <filename>, for the functions listed.
 
+!P<filename> <section title> is replaced by the contents of the DOC:
+section titled <section title> from <filename>.
+Spaces are allowed in <section title>; do not quote the <section title>.
 
 Tim.
 */ <twaugh@redhat.com>
diff --git a/Documentation/kernel-parameters.txt b/Documentation/kernel-parameters.txt
index a2d8805c03d5..532eacbbed62 100644
--- a/Documentation/kernel-parameters.txt
+++ b/Documentation/kernel-parameters.txt
@@ -91,6 +91,7 @@ parameter is applicable:
 	SUSPEND	System suspend states are enabled.
 	FTRACE	Function tracing enabled.
 	TS	Appropriate touchscreen support is enabled.
+	UMS	USB Mass Storage support is enabled.
 	USB	USB support is enabled.
 	USBHID	USB Human Interface Device support is enabled.
 	V4L	Video For Linux support is enabled.
@@ -469,8 +470,8 @@ and is between 256 and 4096 characters. It is defined in the file
 
 	clearcpuid=BITNUM [X86]
 			Disable CPUID feature X for the kernel. See
-			include/asm-x86/cpufeature.h for the valid bit numbers.
-			Note the Linux specific bits are not necessarily
+			arch/x86/include/asm/cpufeature.h for the valid bit
+			numbers. Note the Linux specific bits are not necessarily
 			stable over kernel options, but the vendor specific
 			ones should be.
 			Also note that user programs calling CPUID directly
@@ -551,6 +552,11 @@ and is between 256 and 4096 characters. It is defined in the file
 			not work reliably with all consoles, but is known
 			to work with serial and VGA consoles.
 
+	coredump_filter=
+			[KNL] Change the default value for
+			/proc/<pid>/coredump_filter.
+			See also Documentation/filesystems/proc.txt.
+
 	cpcihp_generic=	[HW,PCI] Generic port I/O CompactPCI driver
 			Format:
 			<first_slot>,<last_slot>,<port>,<enum_bit>[,<debug>]
@@ -913,6 +919,10 @@ and is between 256 and 4096 characters. It is defined in the file
 
 	inttest=	[IA64]
 
+	iomem=		Disable strict checking of access to MMIO memory
+		strict	regions from userspace.
+		relaxed
+
 	iommu=		[x86]
 		off
 		force
@@ -1117,6 +1127,8 @@ and is between 256 and 4096 characters. It is defined in the file
 			If there are multiple matching configurations changing
 			the same attribute, the last one is used.
 
+	lmb=debug	[KNL] Enable lmb debug messages.
+
 	load_ramdisk=	[RAM] List of ramdisks to load from floppy
 			See Documentation/blockdev/ramdisk.txt.
 
@@ -1569,6 +1581,10 @@ and is between 256 and 4096 characters. It is defined in the file
 
 	nr_uarts=	[SERIAL] maximum number of UARTs to be registered.
 
+	ohci1394_dma=early	[HW] enable debugging via the ohci1394 driver.
+			See Documentation/debugging-via-ohci1394.txt for more
+			info.
+
 	olpc_ec_timeout= [OLPC] ms delay when issuing EC commands
 			Rather than timing out after 20 ms if an EC
 			command is not properly ACKed, override the length
@@ -1793,10 +1809,10 @@ and is between 256 and 4096 characters. It is defined in the file
 			autoconfiguration.
 			Ranges are in pairs (memory base and size).
 
-	dynamic_printk
-			Enables pr_debug()/dev_dbg() calls if
-			CONFIG_DYNAMIC_PRINTK_DEBUG has been enabled. These can also
-			be switched on/off via <debugfs>/dynamic_printk/modules
+	dynamic_printk	Enables pr_debug()/dev_dbg() calls if
+			CONFIG_DYNAMIC_PRINTK_DEBUG has been enabled.
+			These can also be switched on/off via
+			<debugfs>/dynamic_printk/modules
 
 	print-fatal-signals=
 			[KNL] debug: print fatal signals
@@ -1884,7 +1900,7 @@ and is between 256 and 4096 characters. It is defined in the file
 
 	reboot=		[BUGS=X86-32,BUGS=ARM,BUGS=IA-64] Rebooting mode
 			Format: <reboot_mode>[,<reboot_mode2>[,...]]
-			See arch/*/kernel/reboot.c or arch/*/kernel/process.c			
+			See arch/*/kernel/reboot.c or arch/*/kernel/process.c
 
 	relax_domain_level=
 			[KNL, SMP] Set scheduler's default relax_domain_level.
@@ -2372,6 +2388,41 @@ and is between 256 and 4096 characters. It is defined in the file
 	usbhid.mousepoll=
 			[USBHID] The interval which mice are to be polled at.
 
+	usb-storage.delay_use=
+			[UMS] The delay in seconds before a new device is
+			scanned for Logical Units (default 5).
+
+	usb-storage.quirks=
+			[UMS] A list of quirks entries to supplement or
+			override the built-in unusual_devs list.  List
+			entries are separated by commas.  Each entry has
+			the form VID:PID:Flags where VID and PID are Vendor
+			and Product ID values (4-digit hex numbers) and
+			Flags is a set of characters, each corresponding
+			to a common usb-storage quirk flag as follows:
+				a = SANE_SENSE (collect more than 18 bytes
+					of sense data);
+				c = FIX_CAPACITY (decrease the reported
+					device capacity by one sector);
+				h = CAPACITY_HEURISTICS (decrease the
+					reported device capacity by one
+					sector if the number is odd);
+				i = IGNORE_DEVICE (don't bind to this
+					device);
+				l = NOT_LOCKABLE (don't try to lock and
+					unlock ejectable media);
+				m = MAX_SECTORS_64 (don't transfer more
+					than 64 sectors = 32 KB at a time);
+				o = CAPACITY_OK (accept the capacity
+					reported by the device);
+				r = IGNORE_RESIDUE (the device reports
+					bogus residue values);
+				s = SINGLE_LUN (the device has only one
+					Logical Unit);
+				w = NO_WP_DETECT (don't test whether the
+					medium is write-protected).
+			Example: quirks=0419:aaf5:rl,0421:0433:rc
+
 	add_efi_memmap	[EFI; x86-32,X86-64] Include EFI memory map in
 			kernel's map of available physical RAM.
 
@@ -2432,8 +2483,8 @@ and is between 256 and 4096 characters. It is defined in the file
 			Format:
 			<irq>,<irq_mask>,<io>,<full_duplex>,<do_sound>,<lockup_hack>[,<irq2>[,<irq3>[,<irq4>]]]
 
-	norandmaps	Don't use address space randomization
-			Equivalent to echo 0 > /proc/sys/kernel/randomize_va_space
+	norandmaps	Don't use address space randomization.  Equivalent to
+			echo 0 > /proc/sys/kernel/randomize_va_space
 
 ______________________________________________________________________
 
diff --git a/Documentation/kobject.txt b/Documentation/kobject.txt
index f5d2aad65a67..b2e374586bd8 100644
--- a/Documentation/kobject.txt
+++ b/Documentation/kobject.txt
@@ -118,8 +118,8 @@ the name of the kobject, call kobject_rename():
 
     int kobject_rename(struct kobject *kobj, const char *new_name);
 
-Note kobject_rename does perform any locking or have a solid notion of
-what names are valid so the provide must provide their own sanity checking
+kobject_rename does not perform any locking or have a solid notion of
+what names are valid so the caller must provide their own sanity checking
 and serialization.
 
 There is a function called kobject_set_name() but that is legacy cruft and
diff --git a/Documentation/kprobes.txt b/Documentation/kprobes.txt
index a79633d702bf..48b3de90eb1e 100644
--- a/Documentation/kprobes.txt
+++ b/Documentation/kprobes.txt
@@ -497,7 +497,10 @@ The first column provides the kernel address where the probe is inserted.
 The second column identifies the type of probe (k - kprobe, r - kretprobe
 and j - jprobe), while the third column specifies the symbol+offset of
 the probe. If the probed function belongs to a module, the module name
-is also specified.
+is also specified. Following columns show probe status. If the probe is on
+a virtual address that is no longer valid (module init sections, module
+virtual addresses that correspond to modules that've been unloaded),
+such probes are marked with [GONE].
 
 /debug/kprobes/enabled: Turn kprobes ON/OFF
 
diff --git a/Documentation/laptops/thinkpad-acpi.txt b/Documentation/laptops/thinkpad-acpi.txt
index 71f0fe1fc1b0..898b4987bb80 100644
--- a/Documentation/laptops/thinkpad-acpi.txt
+++ b/Documentation/laptops/thinkpad-acpi.txt
@@ -1475,7 +1475,7 @@ Sysfs interface changelog:
 
 0x020100:	Marker for thinkpad-acpi with hot key NVRAM polling
 		support.  If you must, use it to know you should not
-		start an userspace NVRAM poller (allows to detect when
+		start a userspace NVRAM poller (allows to detect when
 		NVRAM is compiled out by the user because it is
 		unneeded/undesired in the first place).
 0x020101:	Marker for thinkpad-acpi with hot key NVRAM polling
diff --git a/Documentation/magic-number.txt b/Documentation/magic-number.txt
index 95070028d15e..505f19607542 100644
--- a/Documentation/magic-number.txt
+++ b/Documentation/magic-number.txt
@@ -125,14 +125,14 @@ TRIDENT_CARD_MAGIC    0x5072696E  trident_card      sound/oss/trident.c
 ROUTER_MAGIC          0x524d4157  wan_device        include/linux/wanrouter.h
 SCC_MAGIC             0x52696368  gs_port           drivers/char/scc.h
 SAVEKMSG_MAGIC1       0x53415645  savekmsg          arch/*/amiga/config.c
-GDA_MAGIC             0x58464552  gda               include/asm-mips64/sn/gda.h
+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     include/asm-mips64/sn/klkernvars.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
 TRIDENT_STATE_MAGIC   0x63657373  trient_state      sound/oss/trident.c
 M3_CARD_MAGIC         0x646e6f50  m3_card           sound/oss/maestro3.c
@@ -158,7 +158,7 @@ CCB_MAGIC             0xf2691ad2  ccb               drivers/scsi/ncr53c8xx.c
 QUEUE_MAGIC_FREE      0xf7e1c9a3  queue_entry       drivers/scsi/arm/queue.c
 QUEUE_MAGIC_USED      0xf7e1cc33  queue_entry       drivers/scsi/arm/queue.c
 HTB_CMAGIC            0xFEFAFEF1  htb_class         net/sched/sch_htb.c
-NMI_MAGIC             0x48414d4d455201 nmi_s        include/asm-mips64/sn/nmi.h
+NMI_MAGIC             0x48414d4d455201 nmi_s        arch/mips/include/asm/sn/nmi.h
 
 Note that there are also defined special per-driver magic numbers in sound
 memory management. See include/sound/sndmagic.h for complete list of them. Many
diff --git a/Documentation/memory-hotplug.txt b/Documentation/memory-hotplug.txt
index 168117bd6ee8..4c2ecf537a4a 100644
--- a/Documentation/memory-hotplug.txt
+++ b/Documentation/memory-hotplug.txt
@@ -124,7 +124,7 @@ config options.
     This option can be kernel module too.
 
 --------------------------------
-3 sysfs files for memory hotplug
+4 sysfs files for memory hotplug
 --------------------------------
 All sections have their device information under /sys/devices/system/memory as
 
@@ -138,11 +138,12 @@ For example, assume 1GiB section size. A device for a memory starting at
 (0x100000000 / 1Gib = 4)
 This device covers address range [0x100000000 ... 0x140000000)
 
-Under each section, you can see 3 files.
+Under each section, you can see 4 files.
 
 /sys/devices/system/memory/memoryXXX/phys_index
 /sys/devices/system/memory/memoryXXX/phys_device
 /sys/devices/system/memory/memoryXXX/state
+/sys/devices/system/memory/memoryXXX/removable
 
 'phys_index' : read-only and contains section id, same as XXX.
 'state'      : read-write
@@ -150,10 +151,20 @@ Under each section, you can see 3 files.
                at write: user can specify "online", "offline" command
 'phys_device': read-only: designed to show the name of physical memory device.
                This is not well implemented now.
+'removable'  : read-only: contains an integer value indicating
+               whether the memory section is removable or not
+               removable.  A value of 1 indicates that the memory
+               section is removable and a value of 0 indicates that
+               it is not removable.
 
 NOTE:
   These directories/files appear after physical memory hotplug phase.
 
+If CONFIG_NUMA is enabled the
+/sys/devices/system/memory/memoryXXX memory section
+directories can also be accessed via symbolic links located in
+the /sys/devices/system/node/node* directories.  For example:
+/sys/devices/system/node/node0/memory9 -> ../../memory/memory9
 
 --------------------------------
 4. Physical memory hot-add phase
@@ -365,7 +376,6 @@ node if necessary.
   - allowing memory hot-add to ZONE_MOVABLE. maybe we need some switch like
     sysctl or new control file.
   - showing memory section and physical device relationship.
-  - showing memory section and node relationship (maybe good for NUMA)
   - showing memory section is under ZONE_MOVABLE or not
   - test and make it better memory offlining.
   - support HugeTLB page migration and offlining.
diff --git a/Documentation/mips/AU1xxx_IDE.README b/Documentation/mips/AU1xxx_IDE.README
index 25a6ed1aaa5b..f54962aea84d 100644
--- a/Documentation/mips/AU1xxx_IDE.README
+++ b/Documentation/mips/AU1xxx_IDE.README
@@ -44,7 +44,7 @@ FILES, CONFIGS AND COMPATABILITY
 
 Two files are introduced:
 
-  a) 'include/asm-mips/mach-au1x00/au1xxx_ide.h'
+  a) 'arch/mips/include/asm/mach-au1x00/au1xxx_ide.h'
      containes : struct _auide_hwif
                  timing parameters for PIO mode 0/1/2/3/4
                  timing parameters for MWDMA 0/1/2
diff --git a/Documentation/networking/rxrpc.txt b/Documentation/networking/rxrpc.txt
index c3669a3fb4af..60d05eb77c64 100644
--- a/Documentation/networking/rxrpc.txt
+++ b/Documentation/networking/rxrpc.txt
@@ -540,7 +540,7 @@ A client would issue an operation by:
      MSG_MORE should be set in msghdr::msg_flags on all but the last part of
      the request.  Multiple requests may be made simultaneously.
 
-     If a call is intended to go to a destination other then the default
+     If a call is intended to go to a destination other than the default
      specified through connect(), then msghdr::msg_name should be set on the
      first request message of that call.
 
diff --git a/Documentation/networking/tuntap.txt b/Documentation/networking/tuntap.txt
index 839cbb71388b..c0aab985bad9 100644
--- a/Documentation/networking/tuntap.txt
+++ b/Documentation/networking/tuntap.txt
@@ -118,7 +118,7 @@ As mentioned above, main purpose of TUN/TAP driver is tunneling.
 It is used by VTun (http://vtun.sourceforge.net).
 
 Another interesting application using TUN/TAP is pipsecd
-(http://perso.enst.fr/~beyssac/pipsec/), an userspace IPSec
+(http://perso.enst.fr/~beyssac/pipsec/), a userspace IPSec
 implementation that can use complete kernel routing (unlike FreeS/WAN).
 
 3. How does Virtual network device actually work ? 
diff --git a/Documentation/powerpc/cpu_features.txt b/Documentation/powerpc/cpu_features.txt
index 472739880e87..ffa4183fdb8b 100644
--- a/Documentation/powerpc/cpu_features.txt
+++ b/Documentation/powerpc/cpu_features.txt
@@ -31,7 +31,7 @@ anyways).
 
 After detecting the processor type, the kernel patches out sections of code
 that shouldn't be used by writing nop's over it. Using cpufeatures requires
-just 2 macros (found in include/asm-ppc/cputable.h), as seen in head.S
+just 2 macros (found in arch/powerpc/include/asm/cputable.h), as seen in head.S
 transfer_to_handler:
 
 	#ifdef CONFIG_ALTIVEC
diff --git a/Documentation/s390/Debugging390.txt b/Documentation/s390/Debugging390.txt
index d30a281c570f..10711d9f0788 100644
--- a/Documentation/s390/Debugging390.txt
+++ b/Documentation/s390/Debugging390.txt
@@ -1402,7 +1402,7 @@ Syscalls are implemented on Linux for S390 by the Supervisor call instruction (S
 possibilities of these as the instruction is made up of a  0xA opcode & the second byte being
 the syscall number. They are traced using the simple command.
 TR SVC  <Optional value or range>
-the syscalls are defined in linux/include/asm-s390/unistd.h
+the syscalls are defined in linux/arch/s390/include/asm/unistd.h
 e.g. to trace all file opens just do
 TR SVC 5 ( as this is the syscall number of open )
 
diff --git a/Documentation/s390/cds.txt b/Documentation/s390/cds.txt
index c4b7b2bd369a..480a78ef5a1e 100644
--- a/Documentation/s390/cds.txt
+++ b/Documentation/s390/cds.txt
@@ -98,7 +98,7 @@ platform. Some of the interface routines are specific to Linux/390 and some
 of them can be found on other Linux platforms implementations too.
 Miscellaneous function prototypes, data declarations, and macro definitions
 can be found in the architecture specific C header file
-linux/include/asm-s390/irq.h.
+linux/arch/s390/include/asm/irq.h.
 
 Overview of CDS interface concepts
 
diff --git a/Documentation/s390/s390dbf.txt b/Documentation/s390/s390dbf.txt
index e05420973698..2d10053dd97e 100644
--- a/Documentation/s390/s390dbf.txt
+++ b/Documentation/s390/s390dbf.txt
@@ -2,7 +2,7 @@ S390 Debug Feature
 ==================
 
 files: arch/s390/kernel/debug.c
-       include/asm-s390/debug.h
+       arch/s390/include/asm/debug.h
 
 Description:
 ------------
diff --git a/Documentation/scsi/ChangeLog.lpfc b/Documentation/scsi/ChangeLog.lpfc
index ae3f962a7cfc..ff19a52fe004 100644
--- a/Documentation/scsi/ChangeLog.lpfc
+++ b/Documentation/scsi/ChangeLog.lpfc
@@ -733,7 +733,7 @@ Changes from 20040920 to 20041018
 	  I/O completion path a little more, especially taking care of
 	  fast-pathing the non-error case.  Also removes tons of dead
 	  members and defines from lpfc_scsi.h - e.g. lpfc_target is down
-	  to nothing more then the lpfc_nodelist pointer.
+	  to nothing more than the lpfc_nodelist pointer.
 	* Added binary sysfs file to issue mbox commands
 	* Replaced #if __BIG_ENDIAN with #if __BIG_ENDIAN_BITFIELD for
 	  compatibility with the user space applications.
diff --git a/Documentation/scsi/ChangeLog.ncr53c8xx b/Documentation/scsi/ChangeLog.ncr53c8xx
index a9f721aeb11c..8b278c10edfd 100644
--- a/Documentation/scsi/ChangeLog.ncr53c8xx
+++ b/Documentation/scsi/ChangeLog.ncr53c8xx
@@ -19,7 +19,7 @@ Sun Sep 24 21:30 2000 Gerard Roudier (groudier@club-internet.fr)
 
 Wed Jul 26 23:30 2000 Gerard Roudier (groudier@club-internet.fr)
 	* version ncr53c8xx-3.4.1
-	- Provide OpenFirmare path through the proc FS on PPC.
+	- Provide OpenFirmware path through the proc FS on PPC.
 	- Remove trailing argument #2 from a couple of #undefs.
 
 Sun Jul 09 16:30 2000 Gerard Roudier (groudier@club-internet.fr)
diff --git a/Documentation/scsi/ChangeLog.sym53c8xx b/Documentation/scsi/ChangeLog.sym53c8xx
index ef985ec348e6..02ffbc1e8a84 100644
--- a/Documentation/scsi/ChangeLog.sym53c8xx
+++ b/Documentation/scsi/ChangeLog.sym53c8xx
@@ -81,7 +81,7 @@ Sun Sep 24 21:30 2000 Gerard Roudier (groudier@club-internet.fr)
 
 Wed Jul 26 23:30 2000 Gerard Roudier (groudier@club-internet.fr)
 	* version sym53c8xx-1.7.1
-	- Provide OpenFirmare path through the proc FS on PPC.
+	- Provide OpenFirmware path through the proc FS on PPC.
 	- Download of on-chip SRAM using memcpy_toio() doesn't work 
 	  on PPC. Restore previous method (MEMORY MOVE from SCRIPTS).
 	- Remove trailing argument #2 from a couple of #undefs.
diff --git a/Documentation/spi/spi-lm70llp b/Documentation/spi/spi-lm70llp
index 154bd02220b9..34a9cfd746bd 100644
--- a/Documentation/spi/spi-lm70llp
+++ b/Documentation/spi/spi-lm70llp
@@ -13,10 +13,20 @@ Description
 This driver provides glue code connecting a National Semiconductor LM70 LLP
 temperature sensor evaluation board to the kernel's SPI core subsystem.
 
+This is a SPI master controller driver. It can be used in conjunction with
+(layered under) the LM70 logical driver (a "SPI protocol driver").
 In effect, this driver turns the parallel port interface on the eval board
 into a SPI bus with a single device, which will be driven by the generic
 LM70 driver (drivers/hwmon/lm70.c).
 
+
+Hardware Interfacing
+--------------------
+The schematic for this particular board (the LM70EVAL-LLP) is
+available (on page 4) here:
+
+  http://www.national.com/appinfo/tempsensors/files/LM70LLPEVALmanual.pdf
+
 The hardware interfacing on the LM70 LLP eval board is as follows:
 
    Parallel                 LM70 LLP
diff --git a/Documentation/sysctl/vm.txt b/Documentation/sysctl/vm.txt
index d79eeda7a699..cd05994a49e6 100644
--- a/Documentation/sysctl/vm.txt
+++ b/Documentation/sysctl/vm.txt
@@ -41,7 +41,8 @@ Currently, these files are in /proc/sys/vm:
 
 ==============================================================
 
-dirty_ratio, dirty_background_ratio, dirty_expire_centisecs,
+dirty_bytes, dirty_ratio, dirty_background_bytes,
+dirty_background_ratio, dirty_expire_centisecs,
 dirty_writeback_centisecs, highmem_is_dirtyable,
 vfs_cache_pressure, laptop_mode, block_dump, swap_token_timeout,
 drop-caches, hugepages_treat_as_movable:
diff --git a/Documentation/usb/power-management.txt b/Documentation/usb/power-management.txt
index e48ea1d51010..ad642615ad4c 100644
--- a/Documentation/usb/power-management.txt
+++ b/Documentation/usb/power-management.txt
@@ -313,11 +313,13 @@ three of the methods listed above.  In addition, a driver indicates
 that it supports autosuspend by setting the .supports_autosuspend flag
 in its usb_driver structure.  It is then responsible for informing the
 USB core whenever one of its interfaces becomes busy or idle.  The
-driver does so by calling these three functions:
+driver does so by calling these five functions:
 
 	int  usb_autopm_get_interface(struct usb_interface *intf);
 	void usb_autopm_put_interface(struct usb_interface *intf);
 	int  usb_autopm_set_interface(struct usb_interface *intf);
+	int  usb_autopm_get_interface_async(struct usb_interface *intf);
+	void usb_autopm_put_interface_async(struct usb_interface *intf);
 
 The functions work by maintaining a counter in the usb_interface
 structure.  When intf->pm_usage_count is > 0 then the interface is
@@ -330,10 +332,12 @@ associated with the device itself rather than any of its interfaces.
 This field is used only by the USB core.)
 
 The driver owns intf->pm_usage_count; it can modify the value however
-and whenever it likes.  A nice aspect of the usb_autopm_* routines is
-that the changes they make are protected by the usb_device structure's
-PM mutex (udev->pm_mutex); however drivers may change pm_usage_count
-without holding the mutex.
+and whenever it likes.  A nice aspect of the non-async usb_autopm_*
+routines is that the changes they make are protected by the usb_device
+structure's PM mutex (udev->pm_mutex); however drivers may change
+pm_usage_count without holding the mutex.  Drivers using the async
+routines are responsible for their own synchronization and mutual
+exclusion.
 
 	usb_autopm_get_interface() increments pm_usage_count and
 	attempts an autoresume if the new value is > 0 and the
@@ -348,6 +352,14 @@ without holding the mutex.
 	is suspended, and it attempts an autosuspend if the value is
 	<= 0 and the device isn't suspended.
 
+	usb_autopm_get_interface_async() and
+	usb_autopm_put_interface_async() do almost the same things as
+	their non-async counterparts.  The differences are: they do
+	not acquire the PM mutex, and they use a workqueue to do their
+	jobs.  As a result they can be called in an atomic context,
+	such as an URB's completion handler, but when they return the
+	device will not generally not yet be in the desired state.
+
 There also are a couple of utility routines drivers can use:
 
 	usb_autopm_enable() sets pm_usage_cnt to 0 and then calls
diff --git a/Documentation/vm/unevictable-lru.txt b/Documentation/vm/unevictable-lru.txt
index 125eed560e5a..0706a7282a8c 100644
--- a/Documentation/vm/unevictable-lru.txt
+++ b/Documentation/vm/unevictable-lru.txt
@@ -137,13 +137,6 @@ shrink_page_list() where they will be detected when vmscan walks the reverse
 map in try_to_unmap().  If try_to_unmap() returns SWAP_MLOCK, shrink_page_list()
 will cull the page at that point.
 
-Note that for anonymous pages, shrink_page_list() attempts to add the page to
-the swap cache before it tries to unmap the page.  To avoid this unnecessary
-consumption of swap space, shrink_page_list() calls try_to_munlock() to check
-whether any VM_LOCKED vmas map the page without attempting to unmap the page.
-If try_to_munlock() returns SWAP_MLOCK, shrink_page_list() will cull the page
-without consuming swap space.  try_to_munlock() will be described below.
-
 To "cull" an unevictable page, vmscan simply puts the page back on the lru
 list using putback_lru_page()--the inverse operation to isolate_lru_page()--
 after dropping the page lock.  Because the condition which makes the page
@@ -190,8 +183,8 @@ several places:
    in the VM_LOCKED flag being set for the vma.
 3) in the fault path, if mlocked pages are "culled" in the fault path,
    and when a VM_LOCKED stack segment is expanded.
-4) as mentioned above, in vmscan:shrink_page_list() with attempting to
-   reclaim a page in a VM_LOCKED vma--via try_to_unmap() or try_to_munlock().
+4) as mentioned above, in vmscan:shrink_page_list() when attempting to
+   reclaim a page in a VM_LOCKED vma via try_to_unmap().
 
 Mlocked pages become unlocked and rescued from the unevictable list when:
 
@@ -260,9 +253,9 @@ mlock_fixup() filters several classes of "special" vmas:
 
 2) vmas mapping hugetlbfs page are already effectively pinned into memory.
    We don't need nor want to mlock() these pages.  However, to preserve the
-   prior behavior of mlock()--before the unevictable/mlock changes--mlock_fixup()
-   will call make_pages_present() in the hugetlbfs vma range to allocate the
-   huge pages and populate the ptes.
+   prior behavior of mlock()--before the unevictable/mlock changes--
+   mlock_fixup() will call make_pages_present() in the hugetlbfs vma range
+   to allocate the huge pages and populate the ptes.
 
 3) vmas with VM_DONTEXPAND|VM_RESERVED are generally user space mappings of
    kernel pages, such as the vdso page, relay channel pages, etc.  These pages
@@ -322,7 +315,7 @@ __mlock_vma_pages_range()--the same function used to mlock a vma range--
 passing a flag to indicate that munlock() is being performed.
 
 Because the vma access protections could have been changed to PROT_NONE after
-faulting in and mlocking some pages, get_user_pages() was unreliable for visiting
+faulting in and mlocking pages, get_user_pages() was unreliable for visiting
 these pages for munlocking.  Because we don't want to leave pages mlocked(),
 get_user_pages() was enhanced to accept a flag to ignore the permissions when
 fetching the pages--all of which should be resident as a result of previous
@@ -416,8 +409,8 @@ Mlocked Pages:  munmap()/exit()/exec() System Call Handling
 When unmapping an mlocked region of memory, whether by an explicit call to
 munmap() or via an internal unmap from exit() or exec() processing, we must
 munlock the pages if we're removing the last VM_LOCKED vma that maps the pages.
-Before the unevictable/mlock changes, mlocking did not mark the pages in any way,
-so unmapping them required no processing.
+Before the unevictable/mlock changes, mlocking did not mark the pages in any
+way, so unmapping them required no processing.
 
 To munlock a range of memory under the unevictable/mlock infrastructure, the
 munmap() hander and task address space tear down function call
@@ -517,12 +510,10 @@ couldn't be mlocked.
 Mlocked pages:  try_to_munlock() Reverse Map Scan
 
 TODO/FIXME:  a better name might be page_mlocked()--analogous to the
-page_referenced() reverse map walker--especially if we continue to call this
-from shrink_page_list().  See related TODO/FIXME below.
+page_referenced() reverse map walker.
 
-When munlock_vma_page()--see "Mlocked Pages:  munlock()/munlockall() System
-Call Handling" above--tries to munlock a page, or when shrink_page_list()
-encounters an anonymous page that is not yet in the swap cache, they need to
+When munlock_vma_page()--see "Mlocked Pages:  munlock()/munlockall()
+System Call Handling" above--tries to munlock a page, it needs to
 determine whether or not the page is mapped by any VM_LOCKED vma, without
 actually attempting to unmap all ptes from the page.  For this purpose, the
 unevictable/mlock infrastructure introduced a variant of try_to_unmap() called
@@ -535,10 +526,7 @@ for VM_LOCKED vmas.  When such a vma is found for anonymous pages and file
 pages mapped in linear VMAs, as in the try_to_unmap() case, the functions
 attempt to acquire the associated mmap semphore, mlock the page via
 mlock_vma_page() and return SWAP_MLOCK.  This effectively undoes the
-pre-clearing of the page's PG_mlocked done by munlock_vma_page() and informs
-shrink_page_list() that the anonymous page should be culled rather than added
-to the swap cache in preparation for a try_to_unmap() that will almost
-certainly fail.
+pre-clearing of the page's PG_mlocked done by munlock_vma_page.
 
 If try_to_unmap() is unable to acquire a VM_LOCKED vma's associated mmap
 semaphore, it will return SWAP_AGAIN.  This will allow shrink_page_list()
@@ -557,10 +545,7 @@ However, the scan can terminate when it encounters a VM_LOCKED vma and can
 successfully acquire the vma's mmap semphore for read and mlock the page.
 Although try_to_munlock() can be called many [very many!] times when
 munlock()ing a large region or tearing down a large address space that has been
-mlocked via mlockall(), overall this is a fairly rare event.  In addition,
-although shrink_page_list() calls try_to_munlock() for every anonymous page that
-it handles that is not yet in the swap cache, on average anonymous pages will
-have very short reverse map lists.
+mlocked via mlockall(), overall this is a fairly rare event.
 
 Mlocked Page:  Page Reclaim in shrink_*_list()
 
@@ -588,8 +573,8 @@ Some examples of these unevictable pages on the LRU lists are:
    munlock_vma_page() was forced to let the page back on to the normal
    LRU list for vmscan to handle.
 
-shrink_inactive_list() also culls any unevictable pages that it finds
-on the inactive lists, again diverting them to the appropriate zone's unevictable
+shrink_inactive_list() also culls any unevictable pages that it finds on
+the inactive lists, again diverting them to the appropriate zone's unevictable
 lru list.  shrink_inactive_list() should only see SHM_LOCKed pages that became
 SHM_LOCKed after shrink_active_list() had moved them to the inactive list, or
 pages mapped into VM_LOCKED vmas that munlock_vma_page() couldn't isolate from
@@ -597,19 +582,7 @@ the lru to recheck via try_to_munlock().  shrink_inactive_list() won't notice
 the latter, but will pass on to shrink_page_list().
 
 shrink_page_list() again culls obviously unevictable pages that it could
-encounter for similar reason to shrink_inactive_list().  As already discussed,
-shrink_page_list() proactively looks for anonymous pages that should have
-PG_mlocked set but don't--these would not be detected by page_evictable()--to
-avoid adding them to the swap cache unnecessarily.  File pages mapped into
+encounter for similar reason to shrink_inactive_list().  Pages mapped into
 VM_LOCKED vmas but without PG_mlocked set will make it all the way to
-try_to_unmap().  shrink_page_list() will divert them to the unevictable list when
-try_to_unmap() returns SWAP_MLOCK, as discussed above.
-
-TODO/FIXME:  If we can enhance the swap cache to reliably remove entries
-with page_count(page) > 2, as long as all ptes are mapped to the page and
-not the swap entry, we can probably remove the call to try_to_munlock() in
-shrink_page_list() and just remove the page from the swap cache when
-try_to_unmap() returns SWAP_MLOCK.   Currently, remove_exclusive_swap_page()
-doesn't seem to allow that.
-
-
+try_to_unmap().  shrink_page_list() will divert them to the unevictable list
+when try_to_unmap() returns SWAP_MLOCK, as discussed above.
diff --git a/Documentation/wimax/README.i2400m b/Documentation/wimax/README.i2400m
new file mode 100644
index 000000000000..7dffd8919cb0
--- /dev/null
+++ b/Documentation/wimax/README.i2400m
@@ -0,0 +1,260 @@
+
+   Driver for the Intel Wireless Wimax Connection 2400m
+
+   (C) 2008 Intel Corporation < linux-wimax@intel.com >
+
+   This provides a driver for the Intel Wireless WiMAX Connection 2400m
+   and a basic Linux kernel WiMAX stack.
+
+1. Requirements
+
+     * Linux installation with Linux kernel 2.6.22 or newer (if building
+       from a separate tree)
+     * Intel i2400m Echo Peak or Baxter Peak; this includes the Intel
+       Wireless WiMAX/WiFi Link 5x50 series.
+     * build tools:
+          + Linux kernel development package for the target kernel; to
+            build against your currently running kernel, you need to have
+            the kernel development package corresponding to the running
+            image installed (usually if your kernel is named
+            linux-VERSION, the development package is called
+            linux-dev-VERSION or linux-headers-VERSION).
+          + GNU C Compiler, make
+
+2. Compilation and installation
+
+2.1. Compilation of the drivers included in the kernel
+
+   Configure the kernel; to enable the WiMAX drivers select Drivers >
+   Networking Drivers > WiMAX device support. Enable all of them as
+   modules (easier).
+
+   If USB or SDIO are not enabled in the kernel configuration, the options
+   to build the i2400m USB or SDIO drivers will not show. Enable said
+   subsystems and go back to the WiMAX menu to enable the drivers.
+
+   Compile and install your kernel as usual.
+
+2.2. Compilation of the drivers distributed as an standalone module
+
+   To compile
+
+$ cd source/directory
+$ make
+
+   Once built you can load and unload using the provided load.sh script;
+   load.sh will load the modules, load.sh u will unload them.
+
+   To install in the default kernel directories (and enable auto loading
+   when the device is plugged):
+
+$ make install
+$ depmod -a
+
+   If your kernel development files are located in a non standard
+   directory or if you want to build for a kernel that is not the
+   currently running one, set KDIR to the right location:
+
+$ make KDIR=/path/to/kernel/dev/tree
+
+   For more information, please contact linux-wimax@intel.com.
+
+3. Installing the firmware
+
+   The firmware can be obtained from http://linuxwimax.org or might have
+   been supplied with your hardware.
+
+   It has to be installed in the target system:
+     *
+$ cp FIRMWAREFILE.sbcf /lib/firmware/i2400m-fw-BUSTYPE-1.3.sbcf
+
+     * NOTE: if your firmware came in an .rpm or .deb file, just install
+       it as normal, with the rpm (rpm -i FIRMWARE.rpm) or dpkg
+       (dpkg -i FIRMWARE.deb) commands. No further action is needed.
+     * BUSTYPE will be usb or sdio, depending on the hardware you have.
+       Each hardware type comes with its own firmware and will not work
+       with other types.
+
+4. Design
+
+   This package contains two major parts: a WiMAX kernel stack and a
+   driver for the Intel i2400m.
+
+   The WiMAX stack is designed to provide for common WiMAX control
+   services to current and future WiMAX devices from any vendor; please
+   see README.wimax for details.
+
+   The i2400m kernel driver is broken up in two main parts: the bus
+   generic driver and the bus-specific drivers. The bus generic driver
+   forms the drivercore and contain no knowledge of the actual method we
+   use to connect to the device. The bus specific drivers are just the
+   glue to connect the bus-generic driver and the device. Currently only
+   USB and SDIO are supported. See drivers/net/wimax/i2400m/i2400m.h for
+   more information.
+
+   The bus generic driver is logically broken up in two parts: OS-glue and
+   hardware-glue. The OS-glue interfaces with Linux. The hardware-glue
+   interfaces with the device on using an interface provided by the
+   bus-specific driver. The reason for this breakup is to be able to
+   easily reuse the hardware-glue to write drivers for other OSes; note
+   the hardware glue part is written as a native Linux driver; no
+   abstraction layers are used, so to port to another OS, the Linux kernel
+   API calls should be replaced with the target OS's.
+
+5. Usage
+
+   To load the driver, follow the instructions in the install section;
+   once the driver is loaded, plug in the device (unless it is permanently
+   plugged in). The driver will enumerate the device, upload the firmware
+   and output messages in the kernel log (dmesg, /var/log/messages or
+   /var/log/kern.log) such as:
+
+...
+i2400m_usb 5-4:1.0: firmware interface version 8.0.0
+i2400m_usb 5-4:1.0: WiMAX interface wmx0 (00:1d:e1:01:94:2c) ready
+
+   At this point the device is ready to work.
+
+   Current versions require the Intel WiMAX Network Service in userspace
+   to make things work. See the network service's README for instructions
+   on how to scan, connect and disconnect.
+
+5.1. Module parameters
+
+   Module parameters can be set at kernel or module load time or by
+   echoing values:
+
+$ echo VALUE > /sys/module/MODULENAME/parameters/PARAMETERNAME
+
+   To make changes permanent, for example, for the i2400m module, you can
+   also create a file named /etc/modprobe.d/i2400m containing:
+
+options i2400m idle_mode_disabled=1
+
+   To find which parameters are supported by a module, run:
+
+$ modinfo path/to/module.ko
+
+   During kernel bootup (if the driver is linked in the kernel), specify
+   the following to the kernel command line:
+
+i2400m.PARAMETER=VALUE
+
+5.1.1. i2400m: idle_mode_disabled
+
+   The i2400m module supports a parameter to disable idle mode. This
+   parameter, once set, will take effect only when the device is
+   reinitialized by the driver (eg: following a reset or a reconnect).
+
+5.2. Debug operations: debugfs entries
+
+   The driver will register debugfs entries that allow the user to tweak
+   debug settings. There are three main container directories where
+   entries are placed, which correspond to the three blocks a i2400m WiMAX
+   driver has:
+     * /sys/kernel/debug/wimax:DEVNAME/ for the generic WiMAX stack
+       controls
+     * /sys/kernel/debug/wimax:DEVNAME/i2400m for the i2400m generic
+       driver controls
+     * /sys/kernel/debug/wimax:DEVNAME/i2400m-usb (or -sdio) for the
+       bus-specific i2400m-usb or i2400m-sdio controls).
+
+   Of course, if debugfs is mounted in a directory other than
+   /sys/kernel/debug, those paths will change.
+
+5.2.1. Increasing debug output
+
+   The files named *dl_* indicate knobs for controlling the debug output
+   of different submodules:
+     *
+# find /sys/kernel/debug/wimax\:wmx0 -name \*dl_\*
+/sys/kernel/debug/wimax:wmx0/i2400m-usb/dl_tx
+/sys/kernel/debug/wimax:wmx0/i2400m-usb/dl_rx
+/sys/kernel/debug/wimax:wmx0/i2400m-usb/dl_notif
+/sys/kernel/debug/wimax:wmx0/i2400m-usb/dl_fw
+/sys/kernel/debug/wimax:wmx0/i2400m-usb/dl_usb
+/sys/kernel/debug/wimax:wmx0/i2400m/dl_tx
+/sys/kernel/debug/wimax:wmx0/i2400m/dl_rx
+/sys/kernel/debug/wimax:wmx0/i2400m/dl_rfkill
+/sys/kernel/debug/wimax:wmx0/i2400m/dl_netdev
+/sys/kernel/debug/wimax:wmx0/i2400m/dl_fw
+/sys/kernel/debug/wimax:wmx0/i2400m/dl_debugfs
+/sys/kernel/debug/wimax:wmx0/i2400m/dl_driver
+/sys/kernel/debug/wimax:wmx0/i2400m/dl_control
+/sys/kernel/debug/wimax:wmx0/wimax_dl_stack
+/sys/kernel/debug/wimax:wmx0/wimax_dl_op_rfkill
+/sys/kernel/debug/wimax:wmx0/wimax_dl_op_reset
+/sys/kernel/debug/wimax:wmx0/wimax_dl_op_msg
+/sys/kernel/debug/wimax:wmx0/wimax_dl_id_table
+/sys/kernel/debug/wimax:wmx0/wimax_dl_debugfs
+
+   By reading the file you can obtain the current value of said debug
+   level; by writing to it, you can set it.
+
+   To increase the debug level of, for example, the i2400m's generic TX
+   engine, just write:
+
+$ echo 3 > /sys/kernel/debug/wimax:wmx0/i2400m/dl_tx
+
+   Increasing numbers yield increasing debug information; for details of
+   what is printed and the available levels, check the source. The code
+   uses 0 for disabled and increasing values until 8.
+
+5.2.2. RX and TX statistics
+
+   The i2400m/rx_stats and i2400m/tx_stats provide statistics about the
+   data reception/delivery from the device:
+
+$ cat /sys/kernel/debug/wimax:wmx0/i2400m/rx_stats
+45 1 3 34 3104 48 480
+
+   The numbers reported are
+     * packets/RX-buffer: total, min, max
+     * RX-buffers: total RX buffers received, accumulated RX buffer size
+       in bytes, min size received, max size received
+
+   Thus, to find the average buffer size received, divide accumulated
+   RX-buffer / total RX-buffers.
+
+   To clear the statistics back to 0, write anything to the rx_stats file:
+
+$ echo 1 > /sys/kernel/debug/wimax:wmx0/i2400m_rx_stats
+
+   Likewise for TX.
+
+   Note the packets this debug file refers to are not network packet, but
+   packets in the sense of the device-specific protocol for communication
+   to the host. See drivers/net/wimax/i2400m/tx.c.
+
+5.2.3. Tracing messages received from user space
+
+   To echo messages received from user space into the trace pipe that the
+   i2400m driver creates, set the debug file i2400m/trace_msg_from_user to
+   1:
+     *
+$ echo 1 > /sys/kernel/debug/wimax:wmx0/i2400m/trace_msg_from_user
+
+5.2.4. Performing a device reset
+
+   By writing a 0, a 1 or a 2 to the file
+   /sys/kernel/debug/wimax:wmx0/reset, the driver performs a warm (without
+   disconnecting from the bus), cold (disconnecting from the bus) or bus
+   (bus specific) reset on the device.
+
+5.2.5. Asking the device to enter power saving mode
+
+   By writing any value to the /sys/kernel/debug/wimax:wmx0 file, the
+   device will attempt to enter power saving mode.
+
+6. Troubleshooting
+
+6.1. Driver complains about 'i2400m-fw-usb-1.2.sbcf: request failed'
+
+   If upon connecting the device, the following is output in the kernel
+   log:
+
+i2400m_usb 5-4:1.0: fw i2400m-fw-usb-1.3.sbcf: request failed: -2
+
+   This means that the driver cannot locate the firmware file named
+   /lib/firmware/i2400m-fw-usb-1.2.sbcf. Check that the file is present in
+   the right location.
diff --git a/Documentation/wimax/README.wimax b/Documentation/wimax/README.wimax
new file mode 100644
index 000000000000..b78c4378084e
--- /dev/null
+++ b/Documentation/wimax/README.wimax
@@ -0,0 +1,81 @@
+
+   Linux kernel WiMAX stack
+
+   (C) 2008 Intel Corporation < linux-wimax@intel.com >
+
+   This provides a basic Linux kernel WiMAX stack to provide a common
+   control API for WiMAX devices, usable from kernel and user space.
+
+1. Design
+
+   The WiMAX stack is designed to provide for common WiMAX control
+   services to current and future WiMAX devices from any vendor.
+
+   Because currently there is only one and we don't know what would be the
+   common services, the APIs it currently provides are very minimal.
+   However, it is done in such a way that it is easily extensible to
+   accommodate future requirements.
+
+   The stack works by embedding a struct wimax_dev in your device's
+   control structures. This provides a set of callbacks that the WiMAX
+   stack will call in order to implement control operations requested by
+   the user. As well, the stack provides API functions that the driver
+   calls to notify about changes of state in the device.
+
+   The stack exports the API calls needed to control the device to user
+   space using generic netlink as a marshalling mechanism. You can access
+   them using your own code or use the wrappers provided for your
+   convenience in libwimax (in the wimax-tools package).
+
+   For detailed information on the stack, please see
+   include/linux/wimax.h.
+
+2. Usage
+
+   For usage in a driver (registration, API, etc) please refer to the
+   instructions in the header file include/linux/wimax.h.
+
+   When a device is registered with the WiMAX stack, a set of debugfs
+   files will appear in /sys/kernel/debug/wimax:wmxX can tweak for
+   control.
+
+2.1. Obtaining debug information: debugfs entries
+
+   The WiMAX stack is compiled, by default, with debug messages that can
+   be used to diagnose issues. By default, said messages are disabled.
+
+   The drivers will register debugfs entries that allow the user to tweak
+   debug settings.
+
+   Each driver, when registering with the stack, will cause a debugfs
+   directory named wimax:DEVICENAME to be created; optionally, it might
+   create more subentries below it.
+
+2.1.1. Increasing debug output
+
+   The files named *dl_* indicate knobs for controlling the debug output
+   of different submodules of the WiMAX stack:
+     *
+# find /sys/kernel/debug/wimax\:wmx0 -name \*dl_\*
+/sys/kernel/debug/wimax:wmx0/wimax_dl_stack
+/sys/kernel/debug/wimax:wmx0/wimax_dl_op_rfkill
+/sys/kernel/debug/wimax:wmx0/wimax_dl_op_reset
+/sys/kernel/debug/wimax:wmx0/wimax_dl_op_msg
+/sys/kernel/debug/wimax:wmx0/wimax_dl_id_table
+/sys/kernel/debug/wimax:wmx0/wimax_dl_debugfs
+/sys/kernel/debug/wimax:wmx0/.... # other driver specific files
+
+       NOTE: Of course, if debugfs is mounted in a directory other than
+       /sys/kernel/debug, those paths will change.
+
+   By reading the file you can obtain the current value of said debug
+   level; by writing to it, you can set it.
+
+   To increase the debug level of, for example, the id-table submodule,
+   just write:
+
+$ echo 3 > /sys/kernel/debug/wimax:wmx0/wimax_dl_id_table
+
+   Increasing numbers yield increasing debug information; for details of
+   what is printed and the available levels, check the source. The code
+   uses 0 for disabled and increasing values until 8.
diff --git a/Documentation/x86/zero-page.txt b/Documentation/x86/zero-page.txt
index 169ad423a3d1..4f913857b8a2 100644
--- a/Documentation/x86/zero-page.txt
+++ b/Documentation/x86/zero-page.txt
@@ -3,7 +3,7 @@ protocol of kernel. These should be filled by bootloader or 16-bit
 real-mode setup code of the kernel. References/settings to it mainly
 are in:
 
-  include/asm-x86/bootparam.h
+  arch/x86/include/asm/bootparam.h
 
 
 Offset	Proto	Name		Meaning
diff --git a/MAINTAINERS b/MAINTAINERS
index 141aff67bd6d..a01884407fe4 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -616,7 +616,7 @@ M:	mkpetch@internode.on.net
 S:	Maintained
 
 ARM/TOSA MACHINE SUPPORT
-P:	Dmitry Baryshkov
+P:	Dmitry Eremin-Solenikov
 M:	dbaryshkov@gmail.com
 P:	Dirk Opfer
 M:	dirk@opfer-online.de
@@ -1024,16 +1024,17 @@ S:	Maintained
 BTTV VIDEO4LINUX DRIVER
 P:	Mauro Carvalho Chehab
 M:	mchehab@infradead.org
-M:	v4l-dvb-maintainer@linuxtv.org
+L:	linux-media@vger.kernel.org
 L:	video4linux-list@redhat.com
 W:	http://linuxtv.org
-T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/v4l-dvb.git
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 S:	Maintained
 
 CAFE CMOS INTEGRATED CAMERA CONTROLLER DRIVER
 P:	Jonathan Corbet
 M:	corbet@lwn.net
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 S:	Maintained
 
 CALGARY x86-64 IOMMU
@@ -1092,11 +1093,8 @@ S:	Maintained
 
 CHECKPATCH
 P:	Andy Whitcroft
-M:	apw@shadowen.org
-P:	Randy Dunlap
-M:	rdunlap@xenotime.net
-P:	Joel Schopp
-M:	jschopp@austin.ibm.com
+M:	apw@canonical.com
+L:	linux-kernel@vger.kernel.org
 S:	Supported
 
 CISCO 10G ETHERNET DRIVER
@@ -1264,7 +1262,8 @@ P:	Hans Verkuil, Andy Walls
 M:	hverkuil@xs4all.nl, awalls@radix.net
 L:	ivtv-devel@ivtvdriver.org
 L:	ivtv-users@ivtvdriver.org
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 W:	http://linuxtv.org
 S:	Maintained
 
@@ -1490,10 +1489,10 @@ S:	Maintained
 
 DVB SUBSYSTEM AND DRIVERS
 P:	LinuxTV.org Project
-M:	v4l-dvb-maintainer@linuxtv.org
+M:	linux-media@vger.kernel.org
 L:	linux-dvb@linuxtv.org (subscription required)
 W:	http://linuxtv.org/
-T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/v4l-dvb.git
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 S:	Maintained
 
 DZ DECSTATION DZ11 SERIAL DRIVER
@@ -1885,32 +1884,37 @@ S:	Maintained
 GSPCA FINEPIX SUBDRIVER
 P:	Frank Zago
 M:	frank@zago.net
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 S:	Maintained
 
 GSPCA M5602 SUBDRIVER
 P:	Erik Andren
 M:	erik.andren@gmail.com
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 S:	Maintained
 
 GSPCA PAC207 SONIXB SUBDRIVER
 P:	Hans de Goede
 M:	hdegoede@redhat.com
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 S:	Maintained
 
 GSPCA T613 SUBDRIVER
 P:	Leandro Costantino
 M:	lcostantino@gmail.com
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 S:	Maintained
 
 GSPCA USB WEBCAM DRIVER
 P:	Jean-Francois Moine
 M:	moinejf@free.fr
 W:	http://moinejf.free.fr
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 S:	Maintained
 
 HARDWARE MONITORING
@@ -2308,6 +2312,14 @@ W:	http://lists.sourceforge.net/mailman/listinfo/ipw2100-devel
 W:	http://ipw2200.sourceforge.net
 S:	Supported
 
+INTEL WIRELESS WIMAX CONNECTION 2400
+P:	Inaky Perez-Gonzalez
+M:	inaky.perez-gonzalez@intel.com
+M:	linux-wimax@intel.com
+L:	wimax@linuxwimax.org
+S:	Supported
+W:	http://linuxwimax.org
+
 INTEL WIRELESS WIFI LINK (iwlwifi)
 P:	Zhu Yi
 M:	yi.zhu@intel.com
@@ -2432,7 +2444,8 @@ P:	Hans Verkuil
 M:	hverkuil@xs4all.nl
 L:	ivtv-devel@ivtvdriver.org
 L:	ivtv-users@ivtvdriver.org
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 W:	http://www.ivtvdriver.org
 S:	Maintained
 
@@ -2985,6 +2998,7 @@ MUSB MULTIPOINT HIGH SPEED DUAL-ROLE CONTROLLER
 P:	Felipe Balbi
 M:	felipe.balbi@nokia.com
 L:	linux-usb@vger.kernel.org
+T:	git gitorious.org:/musb/mainline.git
 S:	Maintained
 
 MYRICOM MYRI-10G 10GbE DRIVER (MYRI10GE)
@@ -3191,7 +3205,8 @@ S:	Maintained
 OMNIVISION OV7670 SENSOR DRIVER
 P:	Jonathan Corbet
 M:	corbet@lwn.net
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 S:	Maintained
 
 ONENAND FLASH DRIVER
@@ -3473,8 +3488,9 @@ PVRUSB2 VIDEO4LINUX DRIVER
 P:	Mike Isely
 M:	isely@pobox.com
 L:	pvrusb2@isely.net	(subscribers-only)
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
 W:	http://www.isely.net/pvrusb2/
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 S:	Maintained
 
 PXA2xx/PXA3xx SUPPORT
@@ -3694,6 +3710,8 @@ S:	Supported
 SAA7146 VIDEO4LINUX-2 DRIVER
 P:	Michael Hunold
 M:	michael@mihu.de
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 W:	http://www.mihu.de/linux/saa7146
 S:	Maintained
 
@@ -3957,7 +3975,8 @@ S:	Maintained
 SOC-CAMERA V4L2 SUBSYSTEM
 P:	Guennadi Liakhovetski
 M:	g.liakhovetski@gmx.de
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 S:	Maintained
 
 SOEKRIS NET48XX LED SUPPORT
@@ -4232,9 +4251,10 @@ L:	tpmdd-devel@lists.sourceforge.net (moderated for non-subscribers)
 S:	Maintained
 
 TRIVIAL PATCHES
-P:	Jesper Juhl
+P:	Jiri Kosina
 M:	trivial@kernel.org
 L:	linux-kernel@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/jikos/trivial.git
 S:	Maintained
 
 TTY LAYER
@@ -4375,7 +4395,8 @@ USB ET61X[12]51 DRIVER
 P:	Luca Risolia
 M:	luca.risolia@studio.unibo.it
 L:	linux-usb@vger.kernel.org
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 W:	http://www.linux-projects.org
 S:	Maintained
 
@@ -4524,7 +4545,8 @@ USB SN9C1xx DRIVER
 P:	Luca Risolia
 M:	luca.risolia@studio.unibo.it
 L:	linux-usb@vger.kernel.org
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 W:	http://www.linux-projects.org
 S:	Maintained
 
@@ -4553,7 +4575,8 @@ USB VIDEO CLASS
 P:	Laurent Pinchart
 M:	laurent.pinchart@skynet.be
 L:	linux-uvc-devel@lists.berlios.de (subscribers-only)
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 W:	http://linux-uvc.berlios.de
 S:	Maintained
 
@@ -4561,7 +4584,8 @@ USB W996[87]CF DRIVER
 P:	Luca Risolia
 M:	luca.risolia@studio.unibo.it
 L:	linux-usb@vger.kernel.org
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 W:	http://www.linux-projects.org
 S:	Maintained
 
@@ -4575,7 +4599,8 @@ USB ZC0301 DRIVER
 P:	Luca Risolia
 M:	luca.risolia@studio.unibo.it
 L:	linux-usb@vger.kernel.org
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 W:	http://www.linux-projects.org
 S:	Maintained
 
@@ -4590,7 +4615,8 @@ USB ZR364XX DRIVER
 P:	Antoine Jacquet
 M:	royale@zerezo.com
 L:	linux-usb@vger.kernel.org
-L:	video4linux-list@redhat.com
+L:	linux-media@vger.kernel.org
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 W:	http://royale.zerezo.com/zr364xx/
 S:	Maintained
 
@@ -4659,10 +4685,10 @@ S:	Maintained
 VIDEO FOR LINUX (V4L)
 P:	Mauro Carvalho Chehab
 M:	mchehab@infradead.org
-M:	v4l-dvb-maintainer@linuxtv.org
+L:	linux-media@vger.kernel.org
 L:	video4linux-list@redhat.com
 W:	http://linuxtv.org
-T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/v4l-dvb.git
+T:	git kernel.org:/pub/scm/linux/kernel/git/mchehab/linux-2.6.git
 S:	Maintained
 
 VLAN (802.1Q)
@@ -4735,6 +4761,14 @@ M:	zaga@fly.cc.fer.hr
 L:	linux-scsi@vger.kernel.org
 S:	Maintained
 
+WIMAX STACK
+P:	Inaky Perez-Gonzalez
+M:	inaky.perez-gonzalez@intel.com
+M:	linux-wimax@intel.com
+L:	wimax@linuxwimax.org
+S:	Supported
+W:	http://linuxwimax.org
+
 WIMEDIA LLC PROTOCOL (WLP) SUBSYSTEM
 P:	David Vrabel
 M:	david.vrabel@csr.com
diff --git a/Makefile b/Makefile
index f9006663f01e..28331288341f 100644
--- a/Makefile
+++ b/Makefile
@@ -965,6 +965,7 @@ ifneq ($(KBUILD_SRC),)
 	    mkdir -p include2;                                          \
 	    ln -fsn $(srctree)/include/asm-$(SRCARCH) include2/asm;     \
 	fi
+	ln -fsn $(srctree) source
 endif
 
 # prepare2 creates a makefile if using a separate output directory
@@ -1008,7 +1009,7 @@ define check-symlink
 endef
 
 # We create the target directory of the symlink if it does
-# not exist so the test in chack-symlink works and we have a
+# not exist so the test in check-symlink works and we have a
 # directory for generated filesas used by some architectures.
 define create-symlink
 	if [ ! -L include/asm ]; then                                      \
diff --git a/arch/alpha/include/asm/Kbuild b/arch/alpha/include/asm/Kbuild
index b7c8f188b313..4dad27360576 100644
--- a/arch/alpha/include/asm/Kbuild
+++ b/arch/alpha/include/asm/Kbuild
@@ -9,3 +9,4 @@ unifdef-y += console.h
 unifdef-y += fpu.h
 unifdef-y += sysinfo.h
 unifdef-y += compiler.h
+unifdef-y += swab.h
diff --git a/arch/alpha/include/asm/atomic.h b/arch/alpha/include/asm/atomic.h
index ca88e54dec93..62b363584b2b 100644
--- a/arch/alpha/include/asm/atomic.h
+++ b/arch/alpha/include/asm/atomic.h
@@ -1,6 +1,7 @@
 #ifndef _ALPHA_ATOMIC_H
 #define _ALPHA_ATOMIC_H
 
+#include <linux/types.h>
 #include <asm/barrier.h>
 #include <asm/system.h>
 
@@ -13,14 +14,6 @@
  */
 
 
-/*
- * Counter is volatile to make sure gcc doesn't try to be clever
- * and move things around on us. We need to use _exactly_ the address
- * the user gave us, not some alias that contains the same information.
- */
-typedef struct { volatile int counter; } atomic_t;
-typedef struct { volatile long counter; } atomic64_t;
-
 #define ATOMIC_INIT(i)		( (atomic_t) { (i) } )
 #define ATOMIC64_INIT(i)	( (atomic64_t) { (i) } )
 
diff --git a/arch/alpha/include/asm/byteorder.h b/arch/alpha/include/asm/byteorder.h
index 58e958fc7f1b..6772f3168701 100644
--- a/arch/alpha/include/asm/byteorder.h
+++ b/arch/alpha/include/asm/byteorder.h
@@ -1,47 +1,7 @@
 #ifndef _ALPHA_BYTEORDER_H
 #define _ALPHA_BYTEORDER_H
 
-#include <asm/types.h>
-#include <linux/compiler.h>
-#include <asm/compiler.h>
-
-#ifdef __GNUC__
-
-static inline __attribute_const__ __u32 __arch__swab32(__u32 x)
-{
-	/*
-	 * Unfortunately, we can't use the 6 instruction sequence
-	 * on ev6 since the latency of the UNPKBW is 3, which is
-	 * pretty hard to hide.  Just in case a future implementation
-	 * has a lower latency, here's the sequence (also by Mike Burrows)
-	 *
-	 * UNPKBW a0, v0       v0: 00AA00BB00CC00DD
-	 * SLL v0, 24, a0      a0: BB00CC00DD000000
-	 * BIS v0, a0, a0      a0: BBAACCBBDDCC00DD
-	 * EXTWL a0, 6, v0     v0: 000000000000BBAA
-	 * ZAP a0, 0xf3, a0    a0: 00000000DDCC0000
-	 * ADDL a0, v0, v0     v0: ssssssssDDCCBBAA
-	 */
-
-	__u64 t0, t1, t2, t3;
-
-	t0 = __kernel_inslh(x, 7);	/* t0 : 0000000000AABBCC */
-	t1 = __kernel_inswl(x, 3);	/* t1 : 000000CCDD000000 */
-	t1 |= t0;			/* t1 : 000000CCDDAABBCC */
-	t2 = t1 >> 16;			/* t2 : 0000000000CCDDAA */
-	t0 = t1 & 0xFF00FF00;		/* t0 : 00000000DD00BB00 */
-	t3 = t2 & 0x00FF00FF;		/* t3 : 0000000000CC00AA */
-	t1 = t0 + t3;			/* t1 : ssssssssDDCCBBAA */
-
-	return t1;
-}
-
-#define __arch__swab32 __arch__swab32
-
-#endif /* __GNUC__ */
-
-#define __BYTEORDER_HAS_U64__
-
+#include <asm/swab.h>
 #include <linux/byteorder/little_endian.h>
 
 #endif /* _ALPHA_BYTEORDER_H */
diff --git a/arch/alpha/include/asm/swab.h b/arch/alpha/include/asm/swab.h
new file mode 100644
index 000000000000..68e7089e02d5
--- /dev/null
+++ b/arch/alpha/include/asm/swab.h
@@ -0,0 +1,42 @@
+#ifndef _ALPHA_SWAB_H
+#define _ALPHA_SWAB_H
+
+#include <asm/types.h>
+#include <linux/compiler.h>
+#include <asm/compiler.h>
+
+#ifdef __GNUC__
+
+static inline __attribute_const__ __u32 __arch_swab32(__u32 x)
+{
+	/*
+	 * Unfortunately, we can't use the 6 instruction sequence
+	 * on ev6 since the latency of the UNPKBW is 3, which is
+	 * pretty hard to hide.  Just in case a future implementation
+	 * has a lower latency, here's the sequence (also by Mike Burrows)
+	 *
+	 * UNPKBW a0, v0       v0: 00AA00BB00CC00DD
+	 * SLL v0, 24, a0      a0: BB00CC00DD000000
+	 * BIS v0, a0, a0      a0: BBAACCBBDDCC00DD
+	 * EXTWL a0, 6, v0     v0: 000000000000BBAA
+	 * ZAP a0, 0xf3, a0    a0: 00000000DDCC0000
+	 * ADDL a0, v0, v0     v0: ssssssssDDCCBBAA
+	 */
+
+	__u64 t0, t1, t2, t3;
+
+	t0 = __kernel_inslh(x, 7);	/* t0 : 0000000000AABBCC */
+	t1 = __kernel_inswl(x, 3);	/* t1 : 000000CCDD000000 */
+	t1 |= t0;			/* t1 : 000000CCDDAABBCC */
+	t2 = t1 >> 16;			/* t2 : 0000000000CCDDAA */
+	t0 = t1 & 0xFF00FF00;		/* t0 : 00000000DD00BB00 */
+	t3 = t2 & 0x00FF00FF;		/* t3 : 0000000000CC00AA */
+	t1 = t0 + t3;			/* t1 : ssssssssDDCCBBAA */
+
+	return t1;
+}
+#define __arch_swab32 __arch_swab32
+
+#endif /* __GNUC__ */
+
+#endif /* _ALPHA_SWAB_H */
diff --git a/arch/alpha/kernel/pci.c b/arch/alpha/kernel/pci.c
index ff8cb638472e..a3b938811400 100644
--- a/arch/alpha/kernel/pci.c
+++ b/arch/alpha/kernel/pci.c
@@ -320,24 +320,6 @@ pcibios_update_irq(struct pci_dev *dev, int irq)
 	pci_write_config_byte(dev, PCI_INTERRUPT_LINE, irq);
 }
 
-/* Most Alphas have straight-forward swizzling needs.  */
-
-u8 __init
-common_swizzle(struct pci_dev *dev, u8 *pinp)
-{
-	u8 pin = *pinp;
-
-	while (dev->bus->parent) {
-		pin = bridge_swizzle(pin, PCI_SLOT(dev->devfn));
-		/* Move up the chain of bridges. */
-		dev = dev->bus->self;
-        }
-	*pinp = pin;
-
-	/* The slot is the slot of the last bridge. */
-	return PCI_SLOT(dev->devfn);
-}
-
 void
 pcibios_resource_to_bus(struct pci_dev *dev, struct pci_bus_region *region,
 			 struct resource *res)
diff --git a/arch/alpha/kernel/pci_impl.h b/arch/alpha/kernel/pci_impl.h
index f8b74995a002..00edd04b585e 100644
--- a/arch/alpha/kernel/pci_impl.h
+++ b/arch/alpha/kernel/pci_impl.h
@@ -106,16 +106,11 @@ struct pci_iommu_arena;
  *   Where A = pin 1, B = pin 2 and so on and pin=0 = default = A.
  *   Thus, each swizzle is ((pin-1) + (device#-4)) % 4
  *
- *   The following code swizzles for exactly one bridge.  The routine
- *   common_swizzle below handles multiple bridges.  But there are a
- *   couple boards that do strange things, so we define this here.
+ *   pci_swizzle_interrupt_pin() swizzles for exactly one bridge.  The routine
+ *   pci_common_swizzle() handles multiple bridges.  But there are a
+ *   couple boards that do strange things.
  */
 
-static inline u8 bridge_swizzle(u8 pin, u8 slot) 
-{
-	return (((pin-1) + slot) % 4) + 1;
-}
-
 
 /* The following macro is used to implement the table-based irq mapping
    function for all single-bus Alphas.  */
@@ -184,7 +179,7 @@ extern int pci_probe_only;
 extern unsigned long alpha_agpgart_size;
 
 extern void common_init_pci(void);
-extern u8 common_swizzle(struct pci_dev *, u8 *);
+#define common_swizzle pci_common_swizzle
 extern struct pci_controller *alloc_pci_controller(void);
 extern struct resource *alloc_resource(void);
 
diff --git a/arch/alpha/kernel/sys_dp264.c b/arch/alpha/kernel/sys_dp264.c
index ab44c164d9d4..9c9d1fd4155f 100644
--- a/arch/alpha/kernel/sys_dp264.c
+++ b/arch/alpha/kernel/sys_dp264.c
@@ -481,7 +481,7 @@ monet_swizzle(struct pci_dev *dev, u8 *pinp)
 				slot = PCI_SLOT(dev->devfn);
 				break;
 			}
-			pin = bridge_swizzle(pin, PCI_SLOT(dev->devfn)) ;
+			pin = pci_swizzle_interrupt_pin(dev, pin);
 
 			/* Move up the chain of bridges.  */
 			dev = dev->bus->self;
diff --git a/arch/alpha/kernel/sys_eiger.c b/arch/alpha/kernel/sys_eiger.c
index 7ef3b6fb3700..baf60f36cbd7 100644
--- a/arch/alpha/kernel/sys_eiger.c
+++ b/arch/alpha/kernel/sys_eiger.c
@@ -204,7 +204,7 @@ eiger_swizzle(struct pci_dev *dev, u8 *pinp)
 			break;
 		}
 		/* Must be a card-based bridge.  */
-		pin = bridge_swizzle(pin, PCI_SLOT(dev->devfn));
+		pin = pci_swizzle_interrupt_pin(dev, pin);
 
 		/* Move up the chain of bridges.  */
 		dev = dev->bus->self;
diff --git a/arch/alpha/kernel/sys_miata.c b/arch/alpha/kernel/sys_miata.c
index 910b43cd63e8..61ccd95579ec 100644
--- a/arch/alpha/kernel/sys_miata.c
+++ b/arch/alpha/kernel/sys_miata.c
@@ -219,7 +219,7 @@ miata_swizzle(struct pci_dev *dev, u8 *pinp)
 				slot = PCI_SLOT(dev->devfn) + 9;
 				break;
 			}
-			pin = bridge_swizzle(pin, PCI_SLOT(dev->devfn));
+			pin = pci_swizzle_interrupt_pin(dev, pin);
 
 			/* Move up the chain of bridges.  */
 			dev = dev->bus->self;
diff --git a/arch/alpha/kernel/sys_noritake.c b/arch/alpha/kernel/sys_noritake.c
index eb2a1d63f484..538876b62449 100644
--- a/arch/alpha/kernel/sys_noritake.c
+++ b/arch/alpha/kernel/sys_noritake.c
@@ -257,7 +257,7 @@ noritake_swizzle(struct pci_dev *dev, u8 *pinp)
 				slot = PCI_SLOT(dev->devfn) + 15;
 				break;
 			}
-			pin = bridge_swizzle(pin, PCI_SLOT(dev->devfn)) ;
+			pin = pci_swizzle_interrupt_pin(dev, pin);
 
 			/* Move up the chain of bridges.  */
 			dev = dev->bus->self;
diff --git a/arch/alpha/kernel/sys_ruffian.c b/arch/alpha/kernel/sys_ruffian.c
index 5b99cf3cd69c..f15a329b6011 100644
--- a/arch/alpha/kernel/sys_ruffian.c
+++ b/arch/alpha/kernel/sys_ruffian.c
@@ -160,7 +160,7 @@ ruffian_swizzle(struct pci_dev *dev, u8 *pinp)
 				slot = PCI_SLOT(dev->devfn) + 10;
 				break;
 			}
-			pin = bridge_swizzle(pin, PCI_SLOT(dev->devfn));
+			pin = pci_swizzle_interrupt_pin(dev, pin);
 
 			/* Move up the chain of bridges.  */
 			dev = dev->bus->self;
diff --git a/arch/alpha/kernel/sys_sable.c b/arch/alpha/kernel/sys_sable.c
index a4555f497639..d232e42be018 100644
--- a/arch/alpha/kernel/sys_sable.c
+++ b/arch/alpha/kernel/sys_sable.c
@@ -425,7 +425,7 @@ lynx_swizzle(struct pci_dev *dev, u8 *pinp)
 				slot = PCI_SLOT(dev->devfn) + 11;
 				break;
 			}
-			pin = bridge_swizzle(pin, PCI_SLOT(dev->devfn)) ;
+			pin = pci_swizzle_interrupt_pin(dev, pin);
 
 			/* Move up the chain of bridges.  */
 			dev = dev->bus->self;
diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index d6ebe39934b5..dbfdf87f993f 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -1325,6 +1325,8 @@ source "drivers/regulator/Kconfig"
 
 source "drivers/uio/Kconfig"
 
+source "drivers/staging/Kconfig"
+
 endmenu
 
 source "fs/Kconfig"
diff --git a/arch/arm/include/asm/Kbuild b/arch/arm/include/asm/Kbuild
index 73237bd130a2..43b0b2ba392f 100644
--- a/arch/arm/include/asm/Kbuild
+++ b/arch/arm/include/asm/Kbuild
@@ -1,3 +1,4 @@
 include include/asm-generic/Kbuild.asm
 
 unifdef-y += hwcap.h
+unifdef-y += swab.h
diff --git a/arch/arm/include/asm/atomic.h b/arch/arm/include/asm/atomic.h
index 325f881ccb50..ee99723b3a6c 100644
--- a/arch/arm/include/asm/atomic.h
+++ b/arch/arm/include/asm/atomic.h
@@ -12,10 +12,9 @@
 #define __ASM_ARM_ATOMIC_H
 
 #include <linux/compiler.h>
+#include <linux/types.h>
 #include <asm/system.h>
 
-typedef struct { volatile int counter; } atomic_t;
-
 #define ATOMIC_INIT(i)	{ (i) }
 
 #ifdef __KERNEL__
diff --git a/arch/arm/include/asm/byteorder.h b/arch/arm/include/asm/byteorder.h
index 4fbfb22f65a0..c02b6fc28e1a 100644
--- a/arch/arm/include/asm/byteorder.h
+++ b/arch/arm/include/asm/byteorder.h
@@ -15,38 +15,7 @@
 #ifndef __ASM_ARM_BYTEORDER_H
 #define __ASM_ARM_BYTEORDER_H
 
-#include <linux/compiler.h>
-#include <asm/types.h>
-
-static inline __attribute_const__ __u32 ___arch__swab32(__u32 x)
-{
-	__u32 t;
-
-#ifndef __thumb__
-	if (!__builtin_constant_p(x)) {
-		/*
-		 * The compiler needs a bit of a hint here to always do the
-		 * right thing and not screw it up to different degrees
-		 * depending on the gcc version.
-		 */
-		asm ("eor\t%0, %1, %1, ror #16" : "=r" (t) : "r" (x));
-	} else
-#endif
-		t = x ^ ((x << 16) | (x >> 16)); /* eor r1,r0,r0,ror #16 */
-
-	x = (x << 24) | (x >> 8);		/* mov r0,r0,ror #8      */
-	t &= ~0x00FF0000;			/* bic r1,r1,#0x00FF0000 */
-	x ^= (t >> 8);				/* eor r0,r0,r1,lsr #8   */
-
-	return x;
-}
-
-#define __arch__swab32(x) ___arch__swab32(x)
-
-#if !defined(__STRICT_ANSI__) || defined(__KERNEL__)
-#  define __BYTEORDER_HAS_U64__
-#  define __SWAB_64_THRU_32__
-#endif
+#include <asm/swab.h>
 
 #ifdef __ARMEB__
 #include <linux/byteorder/big_endian.h>
diff --git a/arch/arm/include/asm/mach/pci.h b/arch/arm/include/asm/mach/pci.h
index 32da1ae17e06..a38bdc7afa34 100644
--- a/arch/arm/include/asm/mach/pci.h
+++ b/arch/arm/include/asm/mach/pci.h
@@ -42,7 +42,7 @@ struct pci_sys_data {
 /*
  * This is the standard PCI-PCI bridge swizzling algorithm.
  */
-u8 pci_std_swizzle(struct pci_dev *dev, u8 *pinp);
+#define pci_std_swizzle pci_common_swizzle
 
 /*
  * Call this with your hw_pci struct to initialise the PCI system.
diff --git a/arch/arm/include/asm/swab.h b/arch/arm/include/asm/swab.h
new file mode 100644
index 000000000000..27a689be0856
--- /dev/null
+++ b/arch/arm/include/asm/swab.h
@@ -0,0 +1,50 @@
+/*
+ *  arch/arm/include/asm/byteorder.h
+ *
+ * ARM Endian-ness.  In little endian mode, the data bus is connected such
+ * that byte accesses appear as:
+ *  0 = d0...d7, 1 = d8...d15, 2 = d16...d23, 3 = d24...d31
+ * and word accesses (data or instruction) appear as:
+ *  d0...d31
+ *
+ * When in big endian mode, byte accesses appear as:
+ *  0 = d24...d31, 1 = d16...d23, 2 = d8...d15, 3 = d0...d7
+ * and word accesses (data or instruction) appear as:
+ *  d0...d31
+ */
+#ifndef __ASM_ARM_SWAB_H
+#define __ASM_ARM_SWAB_H
+
+#include <linux/compiler.h>
+#include <asm/types.h>
+
+#if !defined(__STRICT_ANSI__) || defined(__KERNEL__)
+#  define __SWAB_64_THRU_32__
+#endif
+
+static inline __attribute_const__ __u32 __arch_swab32(__u32 x)
+{
+	__u32 t;
+
+#ifndef __thumb__
+	if (!__builtin_constant_p(x)) {
+		/*
+		 * The compiler needs a bit of a hint here to always do the
+		 * right thing and not screw it up to different degrees
+		 * depending on the gcc version.
+		 */
+		asm ("eor\t%0, %1, %1, ror #16" : "=r" (t) : "r" (x));
+	} else
+#endif
+		t = x ^ ((x << 16) | (x >> 16)); /* eor r1,r0,r0,ror #16 */
+
+	x = (x << 24) | (x >> 8);		/* mov r0,r0,ror #8      */
+	t &= ~0x00FF0000;			/* bic r1,r1,#0x00FF0000 */
+	x ^= (t >> 8);				/* eor r0,r0,r1,lsr #8   */
+
+	return x;
+}
+#define __arch_swab32 __arch_swab32
+
+#endif
+
diff --git a/arch/arm/kernel/bios32.c b/arch/arm/kernel/bios32.c
index 17a59b6e521f..809681900ec8 100644
--- a/arch/arm/kernel/bios32.c
+++ b/arch/arm/kernel/bios32.c
@@ -480,33 +480,6 @@ EXPORT_SYMBOL(pcibios_bus_to_resource);
 #endif
 
 /*
- * This is the standard PCI-PCI bridge swizzling algorithm:
- *
- *   Dev: 0  1  2  3
- *    A   A  B  C  D
- *    B   B  C  D  A
- *    C   C  D  A  B
- *    D   D  A  B  C
- *        ^^^^^^^^^^ irq pin on bridge
- */
-u8 __devinit pci_std_swizzle(struct pci_dev *dev, u8 *pinp)
-{
-	int pin = *pinp - 1;
-
-	while (dev->bus->self) {
-		pin = (pin + PCI_SLOT(dev->devfn)) & 3;
-		/*
-		 * move up the chain of bridges,
-		 * swizzling as we go.
-		 */
-		dev = dev->bus->self;
-	}
-	*pinp = pin + 1;
-
-	return PCI_SLOT(dev->devfn);
-}
-
-/*
  * Swizzle the device pin each time we cross a bridge.
  * This might update pin and returns the slot number.
  */
diff --git a/arch/arm/kernel/ecard.c b/arch/arm/kernel/ecard.c
index 60c079d85355..eed2f795e1b3 100644
--- a/arch/arm/kernel/ecard.c
+++ b/arch/arm/kernel/ecard.c
@@ -817,7 +817,7 @@ static struct expansion_card *__init ecard_alloc_card(int type, int slot)
 	ec->dma = NO_DMA;
 	ec->ops = &ecard_default_ops;
 
-	snprintf(ec->dev.bus_id, sizeof(ec->dev.bus_id), "ecard%d", slot);
+	dev_set_name(&ec->dev, "ecard%d", slot);
 	ec->dev.parent = NULL;
 	ec->dev.bus = &ecard_bus_type;
 	ec->dev.dma_mask = &ec->dma_mask;
diff --git a/arch/arm/kernel/kprobes.c b/arch/arm/kernel/kprobes.c
index 3f9abe0e9aff..f692efddd449 100644
--- a/arch/arm/kernel/kprobes.c
+++ b/arch/arm/kernel/kprobes.c
@@ -92,9 +92,7 @@ void __kprobes arch_disarm_kprobe(struct kprobe *p)
 void __kprobes arch_remove_kprobe(struct kprobe *p)
 {
 	if (p->ainsn.insn) {
-		mutex_lock(&kprobe_mutex);
 		free_insn_slot(p->ainsn.insn, 0);
-		mutex_unlock(&kprobe_mutex);
 		p->ainsn.insn = NULL;
 	}
 }
diff --git a/arch/arm/mach-aaec2000/core.c b/arch/arm/mach-aaec2000/core.c
index 50e13965dfed..b5c5fc6ba3a9 100644
--- a/arch/arm/mach-aaec2000/core.c
+++ b/arch/arm/mach-aaec2000/core.c
@@ -212,7 +212,7 @@ static struct clcd_board clcd_plat_data = {
 
 static struct amba_device clcd_device = {
 	.dev		= {
-		.bus_id			= "mb:16",
+		.init_name		= "mb:16",
 		.coherent_dma_mask	= ~0,
 		.platform_data		= &clcd_plat_data,
 	},
diff --git a/arch/arm/mach-ep93xx/core.c b/arch/arm/mach-ep93xx/core.c
index 4781f323703b..6d9152de6074 100644
--- a/arch/arm/mach-ep93xx/core.c
+++ b/arch/arm/mach-ep93xx/core.c
@@ -409,7 +409,7 @@ static struct amba_pl010_data ep93xx_uart_data = {
 
 static struct amba_device uart1_device = {
 	.dev		= {
-		.bus_id		= "apb:uart1",
+		.init_name	= "apb:uart1",
 		.platform_data	= &ep93xx_uart_data,
 	},
 	.res		= {
@@ -423,7 +423,7 @@ static struct amba_device uart1_device = {
 
 static struct amba_device uart2_device = {
 	.dev		= {
-		.bus_id		= "apb:uart2",
+		.init_name	= "apb:uart2",
 		.platform_data	= &ep93xx_uart_data,
 	},
 	.res		= {
@@ -437,7 +437,7 @@ static struct amba_device uart2_device = {
 
 static struct amba_device uart3_device = {
 	.dev		= {
-		.bus_id		= "apb:uart3",
+		.init_name	= "apb:uart3",
 		.platform_data	= &ep93xx_uart_data,
 	},
 	.res		= {
diff --git a/arch/arm/mach-integrator/core.c b/arch/arm/mach-integrator/core.c
index c89c949b4d45..6f8872913073 100644
--- a/arch/arm/mach-integrator/core.c
+++ b/arch/arm/mach-integrator/core.c
@@ -37,7 +37,7 @@ static struct amba_pl010_data integrator_uart_data;
 
 static struct amba_device rtc_device = {
 	.dev		= {
-		.bus_id	= "mb:15",
+		.init_name = "mb:15",
 	},
 	.res		= {
 		.start	= INTEGRATOR_RTC_BASE,
@@ -50,7 +50,7 @@ static struct amba_device rtc_device = {
 
 static struct amba_device uart0_device = {
 	.dev		= {
-		.bus_id	= "mb:16",
+		.init_name = "mb:16",
 		.platform_data = &integrator_uart_data,
 	},
 	.res		= {
@@ -64,7 +64,7 @@ static struct amba_device uart0_device = {
 
 static struct amba_device uart1_device = {
 	.dev		= {
-		.bus_id	= "mb:17",
+		.init_name = "mb:17",
 		.platform_data = &integrator_uart_data,
 	},
 	.res		= {
@@ -78,7 +78,7 @@ static struct amba_device uart1_device = {
 
 static struct amba_device kmi0_device = {
 	.dev		= {
-		.bus_id	= "mb:18",
+		.init_name = "mb:18",
 	},
 	.res		= {
 		.start	= KMI0_BASE,
@@ -91,7 +91,7 @@ static struct amba_device kmi0_device = {
 
 static struct amba_device kmi1_device = {
 	.dev		= {
-		.bus_id	= "mb:19",
+		.init_name = "mb:19",
 	},
 	.res		= {
 		.start	= KMI1_BASE,
diff --git a/arch/arm/mach-integrator/integrator_cp.c b/arch/arm/mach-integrator/integrator_cp.c
index 427c2d8dc123..4ac04055c2ea 100644
--- a/arch/arm/mach-integrator/integrator_cp.c
+++ b/arch/arm/mach-integrator/integrator_cp.c
@@ -407,7 +407,7 @@ static struct mmc_platform_data mmc_data = {
 
 static struct amba_device mmc_device = {
 	.dev		= {
-		.bus_id	= "mb:1c",
+		.init_name = "mb:1c",
 		.platform_data = &mmc_data,
 	},
 	.res		= {
@@ -421,7 +421,7 @@ static struct amba_device mmc_device = {
 
 static struct amba_device aaci_device = {
 	.dev		= {
-		.bus_id	= "mb:1d",
+		.init_name = "mb:1d",
 	},
 	.res		= {
 		.start	= INTCP_PA_AACI_BASE,
@@ -532,7 +532,7 @@ static struct clcd_board clcd_data = {
 
 static struct amba_device clcd_device = {
 	.dev		= {
-		.bus_id	= "mb:c0",
+		.init_name = "mb:c0",
 		.coherent_dma_mask = ~0,
 		.platform_data = &clcd_data,
 	},
diff --git a/arch/arm/mach-integrator/pci.c b/arch/arm/mach-integrator/pci.c
index af7d3ff013ec..2fdb95433f0a 100644
--- a/arch/arm/mach-integrator/pci.c
+++ b/arch/arm/mach-integrator/pci.c
@@ -63,13 +63,7 @@
  *
  * Where A = pin 1, B = pin 2 and so on and pin=0 = default = A.
  * Thus, each swizzle is ((pin-1) + (device#-4)) % 4
- *
- * The following code swizzles for exactly one bridge.  
  */
-static inline int bridge_swizzle(int pin, unsigned int slot) 
-{
-	return (pin + slot) & 3;
-}
 
 /*
  * This routine handles multiple bridges.
@@ -81,15 +75,14 @@ static u8 __init integrator_swizzle(struct pci_dev *dev, u8 *pinp)
 	if (pin == 0)
 		pin = 1;
 
-	pin -= 1;
 	while (dev->bus->self) {
-		pin = bridge_swizzle(pin, PCI_SLOT(dev->devfn));
+		pin = pci_swizzle_interrupt_pin(dev, pin);
 		/*
 		 * move up the chain of bridges, swizzling as we go.
 		 */
 		dev = dev->bus->self;
 	}
-	*pinp = pin + 1;
+	*pinp = pin;
 
 	return PCI_SLOT(dev->devfn);
 }
diff --git a/arch/arm/mach-lh7a40x/clcd.c b/arch/arm/mach-lh7a40x/clcd.c
index a2a543258fc3..c472b9e8b37c 100644
--- a/arch/arm/mach-lh7a40x/clcd.c
+++ b/arch/arm/mach-lh7a40x/clcd.c
@@ -207,7 +207,7 @@ static struct clcd_board clcd_platform_data = {
 static struct amba_device name##_device = {			\
 	.dev = {						\
 		.coherent_dma_mask = ~0,			\
-		.bus_id	= busid,				\
+		.init_name = busid,				\
 		.platform_data = plat,				\
 		},						\
 	.res = {						\
diff --git a/arch/arm/mach-netx/fb.c b/arch/arm/mach-netx/fb.c
index 8f1f992f002e..ea8fa8898fe8 100644
--- a/arch/arm/mach-netx/fb.c
+++ b/arch/arm/mach-netx/fb.c
@@ -91,7 +91,7 @@ void clk_put(struct clk *clk)
 
 static struct amba_device fb_device = {
 	.dev		= {
-		.bus_id	= "fb",
+		.init_name = "fb",
 		.coherent_dma_mask = ~0,
 	},
 	.res		= {
diff --git a/arch/arm/mach-pxa/include/mach/pxa930_rotary.h b/arch/arm/mach-pxa/include/mach/pxa930_rotary.h
new file mode 100644
index 000000000000..053587caffdd
--- /dev/null
+++ b/arch/arm/mach-pxa/include/mach/pxa930_rotary.h
@@ -0,0 +1,20 @@
+#ifndef __ASM_ARCH_PXA930_ROTARY_H
+#define __ASM_ARCH_PXA930_ROTARY_H
+
+/* NOTE:
+ *
+ * rotary can be either interpreted as a ralative input event (e.g.
+ * REL_WHEEL or REL_HWHEEL) or a specific key event (e.g. UP/DOWN
+ * or LEFT/RIGHT), depending on if up_key & down_key are assigned
+ * or rel_code is assigned a non-zero value. When all are non-zero,
+ * up_key and down_key will be preferred.
+ */
+struct pxa930_rotary_platform_data {
+	int	up_key;
+	int	down_key;
+	int	rel_code;
+};
+
+void __init pxa930_set_rotarykey_info(struct pxa930_rotary_platform_data *info);
+
+#endif /* __ASM_ARCH_PXA930_ROTARY_H */
diff --git a/arch/arm/mach-pxa/include/mach/pxa930_trkball.h b/arch/arm/mach-pxa/include/mach/pxa930_trkball.h
new file mode 100644
index 000000000000..5e0789bc4729
--- /dev/null
+++ b/arch/arm/mach-pxa/include/mach/pxa930_trkball.h
@@ -0,0 +1,10 @@
+#ifndef __ASM_ARCH_PXA930_TRKBALL_H
+#define __ASM_ARCH_PXA930_TRKBALL_H
+
+struct pxa930_trkball_platform_data {
+	int x_filter;
+	int y_filter;
+};
+
+#endif /* __ASM_ARCH_PXA930_TRKBALL_H */
+
diff --git a/arch/arm/mach-realview/core.h b/arch/arm/mach-realview/core.h
index 63be2abdc19c..44269b162d49 100644
--- a/arch/arm/mach-realview/core.h
+++ b/arch/arm/mach-realview/core.h
@@ -31,7 +31,7 @@
 static struct amba_device name##_device = {			\
 	.dev		= {					\
 		.coherent_dma_mask = ~0,			\
-		.bus_id	= busid,				\
+		.init_name = busid,				\
 		.platform_data = plat,				\
 	},							\
 	.res		= {					\
diff --git a/arch/arm/mach-s3c2410/include/mach/spi.h b/arch/arm/mach-s3c2410/include/mach/spi.h
index 774f3adfe8ad..1d300fb112b1 100644
--- a/arch/arm/mach-s3c2410/include/mach/spi.h
+++ b/arch/arm/mach-s3c2410/include/mach/spi.h
@@ -14,7 +14,7 @@
 #define __ASM_ARCH_SPI_H __FILE__
 
 struct s3c2410_spi_info {
-	unsigned long		 pin_cs;	/* simple gpio cs */
+	int			 pin_cs;	/* simple gpio cs */
 	unsigned int		 num_cs;	/* total chipselects */
 	int			 bus_num;       /* bus number to use. */
 
diff --git a/arch/arm/mach-versatile/core.h b/arch/arm/mach-versatile/core.h
index afcaa858eb1f..9d39886a8351 100644
--- a/arch/arm/mach-versatile/core.h
+++ b/arch/arm/mach-versatile/core.h
@@ -34,7 +34,7 @@ extern unsigned int mmc_status(struct device *dev);
 static struct amba_device name##_device = {			\
 	.dev		= {					\
 		.coherent_dma_mask = ~0,			\
-		.bus_id	= busid,				\
+		.init_name = busid,				\
 		.platform_data = plat,				\
 	},							\
 	.res		= {					\
diff --git a/arch/arm/plat-mxc/include/mach/usb.h b/arch/arm/plat-mxc/include/mach/usb.h
new file mode 100644
index 000000000000..2dacb3086f1c
--- /dev/null
+++ b/arch/arm/plat-mxc/include/mach/usb.h
@@ -0,0 +1,23 @@
+/*
+ *	Copyright (C) 2008 Darius Augulis <augulis.darius@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.
+ */
+
+#ifndef __ASM_ARCH_MXC_USB
+#define __ASM_ARCH_MXC_USB
+
+struct imxusb_platform_data {
+	int (*init)(struct device *);
+	int (*exit)(struct device *);
+};
+
+#endif /* __ASM_ARCH_MXC_USB */
diff --git a/arch/arm/plat-omap/include/mach/memory.h b/arch/arm/plat-omap/include/mach/memory.h
index 211c9f6619e9..d6b5ca6c7da2 100644
--- a/arch/arm/plat-omap/include/mach/memory.h
+++ b/arch/arm/plat-omap/include/mach/memory.h
@@ -59,7 +59,7 @@
 
 #define virt_to_lbus(x)		((x) - PAGE_OFFSET + OMAP1510_LB_OFFSET)
 #define lbus_to_virt(x)		((x) - OMAP1510_LB_OFFSET + PAGE_OFFSET)
-#define is_lbus_device(dev)	(cpu_is_omap15xx() && dev && (strncmp(dev->bus_id, "ohci", 4) == 0))
+#define is_lbus_device(dev)	(cpu_is_omap15xx() && dev && (strncmp(dev_name(dev), "ohci", 4) == 0))
 
 #define __arch_page_to_dma(dev, page)	({is_lbus_device(dev) ? \
 					(dma_addr_t)virt_to_lbus(page_address(page)) : \
diff --git a/arch/arm/plat-omap/usb.c b/arch/arm/plat-omap/usb.c
index 67ca1e216df7..add0485703b5 100644
--- a/arch/arm/plat-omap/usb.c
+++ b/arch/arm/plat-omap/usb.c
@@ -77,38 +77,6 @@
 
 /*-------------------------------------------------------------------------*/
 
-#if	defined(CONFIG_ARCH_OMAP_OTG) || defined(CONFIG_USB_MUSB_OTG)
-
-static struct otg_transceiver *xceiv;
-
-/**
- * otg_get_transceiver - find the (single) OTG transceiver driver
- *
- * Returns the transceiver driver, after getting a refcount to it; or
- * null if there is no such transceiver.  The caller is responsible for
- * releasing that count.
- */
-struct otg_transceiver *otg_get_transceiver(void)
-{
-	if (xceiv)
-		get_device(xceiv->dev);
-	return xceiv;
-}
-EXPORT_SYMBOL(otg_get_transceiver);
-
-int otg_set_transceiver(struct otg_transceiver *x)
-{
-	if (xceiv && x)
-		return -EBUSY;
-	xceiv = x;
-	return 0;
-}
-EXPORT_SYMBOL(otg_set_transceiver);
-
-#endif
-
-/*-------------------------------------------------------------------------*/
-
 #if defined(CONFIG_ARCH_OMAP_OTG) || defined(CONFIG_ARCH_OMAP15XX)
 
 static void omap2_usb_devconf_clear(u8 port, u32 mask)
diff --git a/arch/avr32/Kconfig b/arch/avr32/Kconfig
index 26eca87f6735..b189680d18b0 100644
--- a/arch/avr32/Kconfig
+++ b/arch/avr32/Kconfig
@@ -122,6 +122,24 @@ config BOARD_ATNGW100
 	bool "ATNGW100 Network Gateway"
 	select CPU_AT32AP7000
 
+config BOARD_HAMMERHEAD
+	bool "Hammerhead board"
+	select CPU_AT32AP7000
+	select USB_ARCH_HAS_HCD
+	help
+	  The Hammerhead platform is built around a AVR32 32-bit microcontroller from Atmel.
+	  It offers versatile peripherals, such as ethernet, usb device, usb host etc.
+
+	  The board also incooperates a power supply and is a Power over Ethernet (PoE) Powered
+	  Device (PD).
+
+	  Additonally, a Cyclone III FPGA from Altera is integrated on the board. The FPGA is
+	  mapped into the 32-bit AVR memory bus. The FPGA offers two DDR2 SDRAM interfaces, which
+	  will cover even the most exceptional need of memory bandwidth. Together with the onboard
+	  video decoder the board is ready for video processing.
+
+	  For more information see: http://www.miromico.com/hammerhead
+
 config BOARD_FAVR_32
 	bool "Favr-32 LCD-board"
 	select CPU_AT32AP7000
@@ -133,6 +151,7 @@ endchoice
 
 source "arch/avr32/boards/atstk1000/Kconfig"
 source "arch/avr32/boards/atngw100/Kconfig"
+source "arch/avr32/boards/hammerhead/Kconfig"
 source "arch/avr32/boards/favr-32/Kconfig"
 
 choice
diff --git a/arch/avr32/Makefile b/arch/avr32/Makefile
index b088e103e753..f3ef3bbf797c 100644
--- a/arch/avr32/Makefile
+++ b/arch/avr32/Makefile
@@ -33,6 +33,7 @@ head-y					+= arch/avr32/kernel/head.o
 core-y					+= $(machdirs)
 core-$(CONFIG_BOARD_ATSTK1000)		+= arch/avr32/boards/atstk1000/
 core-$(CONFIG_BOARD_ATNGW100)		+= arch/avr32/boards/atngw100/
+core-$(CONFIG_BOARD_HAMMERHEAD)		+= arch/avr32/boards/hammerhead/
 core-$(CONFIG_BOARD_FAVR_32)		+= arch/avr32/boards/favr-32/
 core-$(CONFIG_BOARD_MIMC200)		+= arch/avr32/boards/mimc200/
 core-$(CONFIG_LOADER_U_BOOT)		+= arch/avr32/boot/u-boot/
diff --git a/arch/avr32/boards/atngw100/setup.c b/arch/avr32/boards/atngw100/setup.c
index 32fb9ba0fbdf..05d3722fff18 100644
--- a/arch/avr32/boards/atngw100/setup.c
+++ b/arch/avr32/boards/atngw100/setup.c
@@ -19,8 +19,8 @@
 #include <linux/types.h>
 #include <linux/leds.h>
 #include <linux/spi/spi.h>
+#include <linux/atmel-mci.h>
 
-#include <asm/atmel-mci.h>
 #include <asm/io.h>
 #include <asm/setup.h>
 
diff --git a/arch/avr32/boards/atstk1000/atstk1002.c b/arch/avr32/boards/atstk1000/atstk1002.c
index 5c5cdf3b464f..1f33a106905c 100644
--- a/arch/avr32/boards/atstk1000/atstk1002.c
+++ b/arch/avr32/boards/atstk1000/atstk1002.c
@@ -16,12 +16,12 @@
 #include <linux/types.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/at73c213.h>
+#include <linux/atmel-mci.h>
 
 #include <video/atmel_lcdc.h>
 
 #include <asm/io.h>
 #include <asm/setup.h>
-#include <asm/atmel-mci.h>
 
 #include <mach/at32ap700x.h>
 #include <mach/board.h>
@@ -287,23 +287,7 @@ static int __init atstk1002_init(void)
 	 * ATSTK1000 uses 32-bit SDRAM interface. Reserve the
 	 * SDRAM-specific pins so that nobody messes with them.
 	 */
-	at32_reserve_pin(GPIO_PIN_PE(0));	/* DATA[16]	*/
-	at32_reserve_pin(GPIO_PIN_PE(1));	/* DATA[17]	*/
-	at32_reserve_pin(GPIO_PIN_PE(2));	/* DATA[18]	*/
-	at32_reserve_pin(GPIO_PIN_PE(3));	/* DATA[19]	*/
-	at32_reserve_pin(GPIO_PIN_PE(4));	/* DATA[20]	*/
-	at32_reserve_pin(GPIO_PIN_PE(5));	/* DATA[21]	*/
-	at32_reserve_pin(GPIO_PIN_PE(6));	/* DATA[22]	*/
-	at32_reserve_pin(GPIO_PIN_PE(7));	/* DATA[23]	*/
-	at32_reserve_pin(GPIO_PIN_PE(8));	/* DATA[24]	*/
-	at32_reserve_pin(GPIO_PIN_PE(9));	/* DATA[25]	*/
-	at32_reserve_pin(GPIO_PIN_PE(10));	/* DATA[26]	*/
-	at32_reserve_pin(GPIO_PIN_PE(11));	/* DATA[27]	*/
-	at32_reserve_pin(GPIO_PIN_PE(12));	/* DATA[28]	*/
-	at32_reserve_pin(GPIO_PIN_PE(13));	/* DATA[29]	*/
-	at32_reserve_pin(GPIO_PIN_PE(14));	/* DATA[30]	*/
-	at32_reserve_pin(GPIO_PIN_PE(15));	/* DATA[31]	*/
-	at32_reserve_pin(GPIO_PIN_PE(26));	/* SDCS		*/
+	at32_reserve_pin(GPIO_PIOE_BASE, ATMEL_EBI_PE_DATA_ALL);
 
 #ifdef CONFIG_BOARD_ATSTK1006
 	smc_set_timing(&nand_config, &nand_timing);
diff --git a/arch/avr32/boards/atstk1000/atstk1003.c b/arch/avr32/boards/atstk1000/atstk1003.c
index 134b566630b0..b3a23c88bcfe 100644
--- a/arch/avr32/boards/atstk1000/atstk1003.c
+++ b/arch/avr32/boards/atstk1000/atstk1003.c
@@ -17,9 +17,9 @@
 
 #include <linux/spi/at73c213.h>
 #include <linux/spi/spi.h>
+#include <linux/atmel-mci.h>
 
 #include <asm/setup.h>
-#include <asm/atmel-mci.h>
 
 #include <mach/at32ap700x.h>
 #include <mach/board.h>
@@ -131,23 +131,7 @@ static int __init atstk1003_init(void)
 	 * ATSTK1000 uses 32-bit SDRAM interface. Reserve the
 	 * SDRAM-specific pins so that nobody messes with them.
 	 */
-	at32_reserve_pin(GPIO_PIN_PE(0));	/* DATA[16]	*/
-	at32_reserve_pin(GPIO_PIN_PE(1));	/* DATA[17]	*/
-	at32_reserve_pin(GPIO_PIN_PE(2));	/* DATA[18]	*/
-	at32_reserve_pin(GPIO_PIN_PE(3));	/* DATA[19]	*/
-	at32_reserve_pin(GPIO_PIN_PE(4));	/* DATA[20]	*/
-	at32_reserve_pin(GPIO_PIN_PE(5));	/* DATA[21]	*/
-	at32_reserve_pin(GPIO_PIN_PE(6));	/* DATA[22]	*/
-	at32_reserve_pin(GPIO_PIN_PE(7));	/* DATA[23]	*/
-	at32_reserve_pin(GPIO_PIN_PE(8));	/* DATA[24]	*/
-	at32_reserve_pin(GPIO_PIN_PE(9));	/* DATA[25]	*/
-	at32_reserve_pin(GPIO_PIN_PE(10));	/* DATA[26]	*/
-	at32_reserve_pin(GPIO_PIN_PE(11));	/* DATA[27]	*/
-	at32_reserve_pin(GPIO_PIN_PE(12));	/* DATA[28]	*/
-	at32_reserve_pin(GPIO_PIN_PE(13));	/* DATA[29]	*/
-	at32_reserve_pin(GPIO_PIN_PE(14));	/* DATA[30]	*/
-	at32_reserve_pin(GPIO_PIN_PE(15));	/* DATA[31]	*/
-	at32_reserve_pin(GPIO_PIN_PE(26));	/* SDCS		*/
+	at32_reserve_pin(GPIO_PIOE_BASE, ATMEL_EBI_PE_DATA_ALL);
 
 #ifdef	CONFIG_BOARD_ATSTK100X_SW2_CUSTOM
 	at32_add_device_usart(1);
diff --git a/arch/avr32/boards/atstk1000/atstk1004.c b/arch/avr32/boards/atstk1000/atstk1004.c
index cb32eb844aa7..29b35aca96cd 100644
--- a/arch/avr32/boards/atstk1000/atstk1004.c
+++ b/arch/avr32/boards/atstk1000/atstk1004.c
@@ -17,11 +17,11 @@
 
 #include <linux/spi/at73c213.h>
 #include <linux/spi/spi.h>
+#include <linux/atmel-mci.h>
 
 #include <video/atmel_lcdc.h>
 
 #include <asm/setup.h>
-#include <asm/atmel-mci.h>
 
 #include <mach/at32ap700x.h>
 #include <mach/board.h>
diff --git a/arch/avr32/boards/favr-32/setup.c b/arch/avr32/boards/favr-32/setup.c
index 1ee4faf0742d..745c408c2ac5 100644
--- a/arch/avr32/boards/favr-32/setup.c
+++ b/arch/avr32/boards/favr-32/setup.c
@@ -17,6 +17,7 @@
 #include <linux/linkage.h>
 #include <linux/gpio.h>
 #include <linux/leds.h>
+#include <linux/atmel-mci.h>
 #include <linux/atmel-pwm-bl.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/ads7846.h>
@@ -79,6 +80,14 @@ static struct spi_board_info __initdata spi1_board_info[] = {
 	},
 };
 
+static struct mci_platform_data __initdata mci0_data = {
+	.slot[0] = {
+		.bus_width	= 4,
+		.detect_pin	= -ENODEV,
+		.wp_pin		= -ENODEV,
+	},
+};
+
 static struct fb_videomode __initdata lb104v03_modes[] = {
 	{
 		.name		= "640x480 @ 50",
@@ -307,28 +316,10 @@ static int __init favr32_init(void)
 	 * Favr-32 uses 32-bit SDRAM interface. Reserve the SDRAM-specific
 	 * pins so that nobody messes with them.
 	 */
-	at32_reserve_pin(GPIO_PIN_PE(0));	/* DATA[16]	*/
-	at32_reserve_pin(GPIO_PIN_PE(1));	/* DATA[17]	*/
-	at32_reserve_pin(GPIO_PIN_PE(2));	/* DATA[18]	*/
-	at32_reserve_pin(GPIO_PIN_PE(3));	/* DATA[19]	*/
-	at32_reserve_pin(GPIO_PIN_PE(4));	/* DATA[20]	*/
-	at32_reserve_pin(GPIO_PIN_PE(5));	/* DATA[21]	*/
-	at32_reserve_pin(GPIO_PIN_PE(6));	/* DATA[22]	*/
-	at32_reserve_pin(GPIO_PIN_PE(7));	/* DATA[23]	*/
-	at32_reserve_pin(GPIO_PIN_PE(8));	/* DATA[24]	*/
-	at32_reserve_pin(GPIO_PIN_PE(9));	/* DATA[25]	*/
-	at32_reserve_pin(GPIO_PIN_PE(10));	/* DATA[26]	*/
-	at32_reserve_pin(GPIO_PIN_PE(11));	/* DATA[27]	*/
-	at32_reserve_pin(GPIO_PIN_PE(12));	/* DATA[28]	*/
-	at32_reserve_pin(GPIO_PIN_PE(13));	/* DATA[29]	*/
-	at32_reserve_pin(GPIO_PIN_PE(14));	/* DATA[30]	*/
-	at32_reserve_pin(GPIO_PIN_PE(15));	/* DATA[31]	*/
-	at32_reserve_pin(GPIO_PIN_PE(26));	/* SDCS		*/
+	at32_reserve_pin(GPIO_PIOE_BASE, ATMEL_EBI_PE_DATA_ALL);
 
 	at32_select_gpio(GPIO_PIN_PB(3), 0);	/* IRQ from ADS7843 */
 
-	at32_add_system_devices();
-
 	at32_add_device_usart(0);
 
 	set_hw_addr(at32_add_device_eth(0, &eth_data[0]));
@@ -339,7 +330,7 @@ static int __init favr32_init(void)
 
 	at32_add_device_pwm(1 << atmel_pwm_bl_pdata.pwm_channel);
 	at32_add_device_spi(1, spi1_board_info, ARRAY_SIZE(spi1_board_info));
-	at32_add_device_mci(0, NULL);
+	at32_add_device_mci(0, &mci0_data);
 	at32_add_device_usba(0, NULL);
 	at32_add_device_lcdc(0, &favr32_lcdc_data, fbmem_start, fbmem_size, 0);
 
diff --git a/arch/avr32/boards/hammerhead/Kconfig b/arch/avr32/boards/hammerhead/Kconfig
new file mode 100644
index 000000000000..fda2331f9789
--- /dev/null
+++ b/arch/avr32/boards/hammerhead/Kconfig
@@ -0,0 +1,43 @@
+# Hammerhead customization
+
+if BOARD_HAMMERHEAD
+
+config BOARD_HAMMERHEAD_USB
+	bool "Philips ISP116x-hcd USB support"
+	help
+	  This enables USB support for Hammerheads internal ISP116x
+	  controller from Philips.
+
+	  Choose 'Y' here if you want to have your board USB driven.
+
+config BOARD_HAMMERHEAD_LCD
+	bool "Atmel AT91/AT32 LCD support"
+	help
+	  This enables LCD support for the Hammerhead board. You may
+	  also add support for framebuffer devices (AT91/AT32 LCD Controller)
+	  and framebuffer console support to get the most out of your LCD.
+
+	  Choose 'Y' here if you have ordered a Corona daugther board and
+	  want to have support for your Hantronix HDA-351T-LV LCD.
+
+config BOARD_HAMMERHEAD_SND
+	bool "Atmel AC97 Sound support"
+	help
+	  This enables Sound support for the Hammerhead board. You may
+	  also go trough the ALSA settings to get it working.
+
+	  Choose 'Y' here if you have ordered a Corona daugther board and
+	  want to make your board funky.
+
+config BOARD_HAMMERHEAD_FPGA
+	bool "Hammerhead FPGA Support"
+	default y
+	help
+	  This adds support for the Cyclone III FPGA from Altera
+	  found on Miromico's Hammerhead board.
+
+	  Choose 'Y' here if you want to have FPGA support enabled.
+	  You will have to choose the "Hammerhead FPGA Device Support" in
+	  Device Drivers->Misc to be able to use FPGA functionality.
+
+endif	# BOARD_ATNGW100
diff --git a/arch/avr32/boards/hammerhead/Makefile b/arch/avr32/boards/hammerhead/Makefile
new file mode 100644
index 000000000000..c740aa116755
--- /dev/null
+++ b/arch/avr32/boards/hammerhead/Makefile
@@ -0,0 +1 @@
+obj-y				+= setup.o flash.o
diff --git a/arch/avr32/boards/hammerhead/flash.c b/arch/avr32/boards/hammerhead/flash.c
new file mode 100644
index 000000000000..a98c6dd3a028
--- /dev/null
+++ b/arch/avr32/boards/hammerhead/flash.c
@@ -0,0 +1,377 @@
+/*
+ * Hammerhead board-specific flash initialization
+ *
+ * Copyright (C) 2008 Miromico AG
+ *
+ * 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/platform_device.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+#include <linux/mtd/physmap.h>
+#include <linux/usb/isp116x.h>
+#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+
+#include <mach/portmux.h>
+#include <mach/at32ap700x.h>
+#include <mach/smc.h>
+
+#include "../../mach-at32ap/clock.h"
+#include "flash.h"
+
+
+#define HAMMERHEAD_USB_PERIPH_GCLK0	0x40000000
+#define HAMMERHEAD_USB_PERIPH_CS2	0x02000000
+#define HAMMERHEAD_USB_PERIPH_EXTINT0	0x02000000
+
+#define HAMMERHEAD_FPGA_PERIPH_MOSI	0x00000002
+#define HAMMERHEAD_FPGA_PERIPH_SCK	0x00000020
+#define HAMMERHEAD_FPGA_PERIPH_EXTINT3	0x10000000
+
+static struct smc_timing flash_timing __initdata = {
+	.ncs_read_setup		= 0,
+	.nrd_setup		= 40,
+	.ncs_write_setup	= 0,
+	.nwe_setup		= 10,
+
+	.ncs_read_pulse		= 80,
+	.nrd_pulse		= 40,
+	.ncs_write_pulse	= 65,
+	.nwe_pulse		= 55,
+
+	.read_cycle		= 120,
+	.write_cycle		= 120,
+};
+
+static struct smc_config flash_config __initdata = {
+	.bus_width		= 2,
+	.nrd_controlled		= 1,
+	.nwe_controlled		= 1,
+	.byte_write		= 1,
+};
+
+static struct mtd_partition flash_parts[] = {
+	{
+		.name		= "u-boot",
+		.offset		= 0x00000000,
+		.size		= 0x00020000,           /* 128 KiB */
+		.mask_flags	= MTD_WRITEABLE,
+	},
+	{
+		.name		= "root",
+		.offset		= 0x00020000,
+		.size		= 0x007d0000,
+	},
+	{
+		.name		= "env",
+		.offset		= 0x007f0000,
+		.size		= 0x00010000,
+		.mask_flags	= MTD_WRITEABLE,
+	},
+};
+
+static struct physmap_flash_data flash_data = {
+	.width		= 2,
+	.nr_parts	= ARRAY_SIZE(flash_parts),
+	.parts		= flash_parts,
+};
+
+static struct resource flash_resource = {
+	.start		= 0x00000000,
+	.end		= 0x007fffff,
+	.flags		= IORESOURCE_MEM,
+};
+
+static struct platform_device flash_device = {
+	.name		= "physmap-flash",
+	.id		= 0,
+	.resource	= &flash_resource,
+	.num_resources	= 1,
+	.dev		= { .platform_data = &flash_data, },
+};
+
+#ifdef CONFIG_BOARD_HAMMERHEAD_USB
+
+static struct smc_timing isp1160_timing __initdata = {
+	.ncs_read_setup		= 75,
+	.nrd_setup		= 75,
+	.ncs_write_setup	= 75,
+	.nwe_setup		= 75,
+
+
+	/* We use conservative timing settings, as the minimal settings aren't
+	   stable. There may be room for tweaking. */
+	.ncs_read_pulse		= 75,  /* min. 33ns */
+	.nrd_pulse		= 75,  /* min. 33ns */
+	.ncs_write_pulse	= 75,  /* min. 26ns */
+	.nwe_pulse		= 75,  /* min. 26ns */
+
+	.read_cycle		= 225, /* min. 143ns */
+	.write_cycle		= 225, /* min. 136ns */
+};
+
+static struct smc_config isp1160_config __initdata = {
+	.bus_width		= 2,
+	.nrd_controlled		= 1,
+	.nwe_controlled		= 1,
+	.byte_write		= 0,
+};
+
+/*
+ * The platform delay function is only used to enforce the strange
+ * read to write delay. This can not be configured in the SMC. All other
+ * timings are controlled by the SMC (see timings obove)
+ * So in isp116x-hcd.c we should comment out USE_PLATFORM_DELAY
+ */
+void isp116x_delay(struct device *dev, int delay)
+{
+	if (delay > 150)
+		ndelay(delay - 150);
+}
+
+static struct  isp116x_platform_data isp1160_data = {
+	.sel15Kres		= 1,	/* use internal downstream resistors */
+	.oc_enable		= 0,	/* external overcurrent detection */
+	.int_edge_triggered	= 0,	/* interrupt is level triggered */
+	.int_act_high		= 0,	/* interrupt is active low */
+	.delay = isp116x_delay,		/* platform delay function */
+};
+
+static struct resource isp1160_resource[] = {
+	{
+		.start		= 0x08000000,
+		.end		= 0x08000001,
+		.flags		= IORESOURCE_MEM,
+	},
+	{
+		.start		= 0x08000002,
+		.end		= 0x08000003,
+		.flags		= IORESOURCE_MEM,
+	},
+	{
+		.start		= 64,
+		.flags		= IORESOURCE_IRQ,
+	},
+};
+
+static struct platform_device isp1160_device = {
+	.name		= "isp116x-hcd",
+	.id		= 0,
+	.resource	= isp1160_resource,
+	.num_resources	= 3,
+	.dev		= {
+		.platform_data = &isp1160_data,
+	},
+};
+#endif
+
+#ifdef CONFIG_BOARD_HAMMERHEAD_USB
+static int __init hammerhead_usbh_init(void)
+{
+	struct clk *gclk;
+	struct clk *osc;
+
+	int ret;
+
+	/* setup smc for usbh */
+	smc_set_timing(&isp1160_config, &isp1160_timing);
+	ret = smc_set_configuration(2, &isp1160_config);
+
+	if (ret < 0) {
+		printk(KERN_ERR
+		       "hammerhead: failed to set ISP1160 USBH timing\n");
+		return ret;
+	}
+
+	/* setup gclk0 to run from osc1 */
+	gclk = clk_get(NULL, "gclk0");
+	if (IS_ERR(gclk))
+		goto err_gclk;
+
+	osc = clk_get(NULL, "osc1");
+	if (IS_ERR(osc))
+		goto err_osc;
+
+	if (clk_set_parent(gclk, osc)) {
+		pr_debug("hammerhead: failed to set osc1 for USBH clock\n");
+		goto err_set_clk;
+	}
+
+	/* set clock to 6MHz */
+	clk_set_rate(gclk, 6000000);
+
+	/* and enable */
+	clk_enable(gclk);
+
+	/* select GCLK0 peripheral function */
+	at32_select_periph(GPIO_PIOA_BASE, HAMMERHEAD_USB_PERIPH_GCLK0,
+			   GPIO_PERIPH_A, 0);
+
+	/* enable CS2 peripheral function */
+	at32_select_periph(GPIO_PIOE_BASE, HAMMERHEAD_USB_PERIPH_CS2,
+			   GPIO_PERIPH_A, 0);
+
+	/* H_WAKEUP must be driven low */
+	at32_select_gpio(GPIO_PIN_PA(8), AT32_GPIOF_OUTPUT);
+
+	/* Select EXTINT0 for PB25 */
+	at32_select_periph(GPIO_PIOB_BASE, HAMMERHEAD_USB_PERIPH_EXTINT0,
+			   GPIO_PERIPH_A, 0);
+
+	/* register usbh device driver */
+	platform_device_register(&isp1160_device);
+
+ err_set_clk:
+	clk_put(osc);
+ err_osc:
+	clk_put(gclk);
+ err_gclk:
+	return ret;
+}
+#endif
+
+#ifdef CONFIG_BOARD_HAMMERHEAD_FPGA
+static struct smc_timing fpga_timing __initdata = {
+	.ncs_read_setup		= 16,
+	.nrd_setup		= 32,
+	.ncs_read_pulse		= 48,
+	.nrd_pulse		= 32,
+	.read_cycle		= 64,
+
+	.ncs_write_setup	= 16,
+	.nwe_setup		= 16,
+	.ncs_write_pulse	= 32,
+	.nwe_pulse		= 32,
+	.write_cycle		= 64,
+};
+
+static struct smc_config fpga_config __initdata = {
+	.bus_width		= 4,
+	.nrd_controlled		= 1,
+	.nwe_controlled		= 1,
+	.byte_write		= 0,
+};
+
+static struct resource hh_fpga0_resource[] = {
+	{
+		.start		= 0xffe00400,
+		.end		= 0xffe00400 + 0x3ff,
+		.flags		= IORESOURCE_MEM,
+	},
+	{
+		.start		= 4,
+		.end		= 4,
+		.flags		= IORESOURCE_IRQ,
+	},
+	{
+		.start		= 0x0c000000,
+		.end		= 0x0c000100,
+		.flags		= IORESOURCE_MEM,
+	},
+	{
+		.start		= 67,
+		.end		= 67,
+		.flags		= IORESOURCE_IRQ,
+	},
+};
+
+static u64 hh_fpga0_dma_mask = DMA_32BIT_MASK;
+static struct platform_device hh_fpga0_device = {
+	.name		= "hh_fpga",
+	.id		= 0,
+	.dev		= {
+		.dma_mask = &hh_fpga0_dma_mask,
+		.coherent_dma_mask = DMA_32BIT_MASK,
+	},
+	.resource	= hh_fpga0_resource,
+	.num_resources	= ARRAY_SIZE(hh_fpga0_resource),
+};
+
+static struct clk hh_fpga0_spi_clk = {
+	.name		= "spi_clk",
+	.dev		= &hh_fpga0_device.dev,
+	.mode		= pba_clk_mode,
+	.get_rate	= pba_clk_get_rate,
+	.index		= 1,
+};
+
+struct platform_device *__init at32_add_device_hh_fpga(void)
+{
+	/* Select peripheral functionallity for SPI SCK and MOSI */
+	at32_select_periph(GPIO_PIOB_BASE, HAMMERHEAD_FPGA_PERIPH_SCK,
+			   GPIO_PERIPH_B, 0);
+	at32_select_periph(GPIO_PIOB_BASE, HAMMERHEAD_FPGA_PERIPH_MOSI,
+			   GPIO_PERIPH_B, 0);
+
+	/* reserve all other needed gpio
+	 * We have on board pull ups, so there is no need
+	 * to enable gpio pull ups */
+	/* INIT_DONE (input) */
+	at32_select_gpio(GPIO_PIN_PB(0), 0);
+
+	/* nSTATUS (input) */
+	at32_select_gpio(GPIO_PIN_PB(2), 0);
+
+	/* nCONFIG (output, low) */
+	at32_select_gpio(GPIO_PIN_PB(3), AT32_GPIOF_OUTPUT);
+
+	/* CONF_DONE (input) */
+	at32_select_gpio(GPIO_PIN_PB(4), 0);
+
+	/* Select EXTINT3 for PB28 (Interrupt from FPGA) */
+	at32_select_periph(GPIO_PIOB_BASE, HAMMERHEAD_FPGA_PERIPH_EXTINT3,
+			   GPIO_PERIPH_A, 0);
+
+	/* Get our parent clock */
+	hh_fpga0_spi_clk.parent = clk_get(NULL, "pba");
+	clk_put(hh_fpga0_spi_clk.parent);
+
+	/* Register clock in at32 clock tree */
+	at32_clk_register(&hh_fpga0_spi_clk);
+
+	platform_device_register(&hh_fpga0_device);
+	return &hh_fpga0_device;
+}
+#endif
+
+/* This needs to be called after the SMC has been initialized */
+static int __init hammerhead_flash_init(void)
+{
+	int ret;
+
+	smc_set_timing(&flash_config, &flash_timing);
+	ret = smc_set_configuration(0, &flash_config);
+
+	if (ret < 0) {
+		printk(KERN_ERR "hammerhead: failed to set NOR flash timing\n");
+		return ret;
+	}
+
+	platform_device_register(&flash_device);
+
+#ifdef CONFIG_BOARD_HAMMERHEAD_USB
+	hammerhead_usbh_init();
+#endif
+
+#ifdef CONFIG_BOARD_HAMMERHEAD_FPGA
+	/* Setup SMC for FPGA interface */
+	smc_set_timing(&fpga_config, &fpga_timing);
+	ret = smc_set_configuration(3, &fpga_config);
+#endif
+
+
+	if (ret < 0) {
+		printk(KERN_ERR "hammerhead: failed to set FPGA timing\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+device_initcall(hammerhead_flash_init);
diff --git a/arch/avr32/boards/hammerhead/flash.h b/arch/avr32/boards/hammerhead/flash.h
new file mode 100644
index 000000000000..ea70c626587b
--- /dev/null
+++ b/arch/avr32/boards/hammerhead/flash.h
@@ -0,0 +1,6 @@
+#ifndef __BOARDS_HAMMERHEAD_FLASH_H
+#define __BOARDS_HAMMERHEAD_FLASH_H
+
+struct platform_device *at32_add_device_hh_fpga(void);
+
+#endif /* __BOARDS_HAMMERHEAD_FLASH_H */
diff --git a/arch/avr32/boards/hammerhead/setup.c b/arch/avr32/boards/hammerhead/setup.c
new file mode 100644
index 000000000000..4d2fe82b2029
--- /dev/null
+++ b/arch/avr32/boards/hammerhead/setup.c
@@ -0,0 +1,245 @@
+/*
+ * Board-specific setup code for the Miromico Hammerhead board
+ *
+ * Copyright (C) 2008 Miromico AG
+ *
+ * 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/atmel-mci.h>
+#include <linux/clk.h>
+#include <linux/fb.h>
+#include <linux/etherdevice.h>
+#include <linux/i2c.h>
+#include <linux/i2c-gpio.h>
+#include <linux/init.h>
+#include <linux/linkage.h>
+#include <linux/platform_device.h>
+#include <linux/types.h>
+#include <linux/spi/spi.h>
+
+#include <video/atmel_lcdc.h>
+
+#include <linux/io.h>
+#include <asm/setup.h>
+
+#include <mach/at32ap700x.h>
+#include <mach/board.h>
+#include <mach/init.h>
+#include <mach/portmux.h>
+
+#include "../../mach-at32ap/clock.h"
+#include "flash.h"
+
+/* Oscillator frequencies. These are board-specific */
+unsigned long at32_board_osc_rates[3] = {
+	[0] = 32768,	/* 32.768 kHz on RTC osc */
+	[1] = 25000000, /* 25MHz on osc0 */
+	[2] = 12000000,	/* 12 MHz on osc1 */
+};
+
+/* Initialized by bootloader-specific startup code. */
+struct tag *bootloader_tags __initdata;
+
+#ifdef CONFIG_BOARD_HAMMERHEAD_LCD
+static struct fb_videomode __initdata hda350tlv_modes[] = {
+	{
+		.name		= "320x240 @ 75",
+		.refresh	= 75,
+		.xres		= 320,
+		.yres		= 240,
+		.pixclock	= KHZ2PICOS(6891),
+
+		.left_margin	= 48,
+		.right_margin	= 18,
+		.upper_margin	= 18,
+		.lower_margin	= 4,
+		.hsync_len	= 20,
+		.vsync_len	= 2,
+
+		.sync		= 0,
+		.vmode		= FB_VMODE_NONINTERLACED,
+	},
+};
+
+static struct fb_monspecs __initdata hammerhead_hda350t_monspecs = {
+	.manufacturer		= "HAN",
+	.monitor		= "HDA350T-LV",
+	.modedb			= hda350tlv_modes,
+	.modedb_len		= ARRAY_SIZE(hda350tlv_modes),
+	.hfmin			= 14900,
+	.hfmax			= 22350,
+	.vfmin			= 60,
+	.vfmax			= 90,
+	.dclkmax		= 10000000,
+};
+
+struct atmel_lcdfb_info __initdata hammerhead_lcdc_data = {
+	.default_bpp		= 24,
+	.default_dmacon		= ATMEL_LCDC_DMAEN | ATMEL_LCDC_DMA2DEN,
+	.default_lcdcon2	= (ATMEL_LCDC_DISTYPE_TFT
+				   | ATMEL_LCDC_INVCLK
+				   | ATMEL_LCDC_CLKMOD_ALWAYSACTIVE
+				   | ATMEL_LCDC_MEMOR_BIG),
+	.default_monspecs	= &hammerhead_hda350t_monspecs,
+	.guard_time		= 2,
+};
+#endif
+
+static struct mci_platform_data __initdata mci0_data = {
+	.slot[0] = {
+		.bus_width	= 4,
+		.detect_pin	= -ENODEV,
+		.wp_pin		= -ENODEV,
+	},
+};
+
+struct eth_addr {
+	u8 addr[6];
+};
+
+static struct eth_addr __initdata hw_addr[1];
+static struct eth_platform_data __initdata eth_data[1];
+
+/*
+ * The next two functions should go away as the boot loader is
+ * supposed to initialize the macb address registers with a valid
+ * ethernet address. But we need to keep it around for a while until
+ * we can be reasonably sure the boot loader does this.
+ *
+ * The phy_id is ignored as the driver will probe for it.
+ */
+static int __init parse_tag_ethernet(struct tag *tag)
+{
+	int i = tag->u.ethernet.mac_index;
+
+	if (i < ARRAY_SIZE(hw_addr))
+		memcpy(hw_addr[i].addr, tag->u.ethernet.hw_address,
+		       sizeof(hw_addr[i].addr));
+
+	return 0;
+}
+__tagtable(ATAG_ETHERNET, parse_tag_ethernet);
+
+static void __init set_hw_addr(struct platform_device *pdev)
+{
+	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	const u8 *addr;
+	void __iomem *regs;
+	struct clk *pclk;
+
+	if (!res)
+		return;
+
+	if (pdev->id >= ARRAY_SIZE(hw_addr))
+		return;
+
+	addr = hw_addr[pdev->id].addr;
+
+	if (!is_valid_ether_addr(addr))
+		return;
+
+	/*
+	 * Since this is board-specific code, we'll cheat and use the
+	 * physical address directly as we happen to know that it's
+	 * the same as the virtual address.
+	 */
+	regs = (void __iomem __force *)res->start;
+	pclk = clk_get(&pdev->dev, "pclk");
+
+	if (!pclk)
+		return;
+
+	clk_enable(pclk);
+
+	__raw_writel((addr[3] << 24) | (addr[2] << 16) | (addr[1] << 8) |
+		     addr[0], regs + 0x98);
+	__raw_writel((addr[5] << 8) | addr[4], regs + 0x9c);
+
+	clk_disable(pclk);
+	clk_put(pclk);
+}
+
+void __init setup_board(void)
+{
+	at32_map_usart(1, 0);	/* USART 1: /dev/ttyS0, DB9 */
+	at32_setup_serial_console(0);
+}
+
+static struct i2c_gpio_platform_data i2c_gpio_data = {
+	.sda_pin		= GPIO_PIN_PA(6),
+	.scl_pin		= GPIO_PIN_PA(7),
+	.sda_is_open_drain	= 1,
+	.scl_is_open_drain	= 1,
+	.udelay			= 2,	/* close to 100 kHz */
+};
+
+static struct platform_device i2c_gpio_device = {
+	.name		= "i2c-gpio",
+	.id		= 0,
+	.dev		= { .platform_data = &i2c_gpio_data, },
+};
+
+static struct i2c_board_info __initdata i2c_info[] = {};
+
+#ifdef CONFIG_BOARD_HAMMERHEAD_SND
+static struct ac97c_platform_data ac97c_data = {
+	.reset_pin = GPIO_PIN_PA(16),
+};
+#endif
+
+static int __init hammerhead_init(void)
+{
+	/*
+	 * Hammerhead uses 32-bit SDRAM interface. Reserve the
+	 * SDRAM-specific pins so that nobody messes with them.
+	 */
+	at32_reserve_pin(GPIO_PIOE_BASE, ATMEL_EBI_PE_DATA_ALL);
+
+	at32_add_device_usart(0);
+
+	/* Reserve PB29 (GCLK3). This pin is used as clock source
+	 * for ETH PHY (25MHz). GCLK3 setup is done by U-Boot.
+	 */
+	at32_reserve_pin(GPIO_PIOB_BASE, (1<<29));
+
+	/*
+	 * Hammerhead uses only one ethernet port, so we don't set
+	 * address of second port
+	 */
+	set_hw_addr(at32_add_device_eth(0, &eth_data[0]));
+
+#ifdef CONFIG_BOARD_HAMMERHEAD_FPGA
+	at32_add_device_hh_fpga();
+#endif
+	at32_add_device_mci(0, &mci0_data);
+
+#ifdef CONFIG_BOARD_HAMMERHEAD_USB
+	at32_add_device_usba(0, NULL);
+#endif
+#ifdef CONFIG_BOARD_HAMMERHEAD_LCD
+	at32_add_device_lcdc(0, &hammerhead_lcdc_data, fbmem_start,
+			     fbmem_size, ATMEL_LCDC_PRI_24BIT);
+#endif
+
+	at32_select_gpio(i2c_gpio_data.sda_pin,
+			 AT32_GPIOF_MULTIDRV | AT32_GPIOF_OUTPUT |
+			 AT32_GPIOF_HIGH);
+	at32_select_gpio(i2c_gpio_data.scl_pin,
+			 AT32_GPIOF_MULTIDRV | AT32_GPIOF_OUTPUT |
+			 AT32_GPIOF_HIGH);
+	platform_device_register(&i2c_gpio_device);
+	i2c_register_board_info(0, i2c_info, ARRAY_SIZE(i2c_info));
+
+#ifdef CONFIG_BOARD_HAMMERHEAD_SND
+	at32_add_device_ac97c(0, &ac97c_data);
+#endif
+
+	/* Select the Touchscreen interrupt pin mode */
+	at32_select_periph(GPIO_PIOB_BASE, 0x08000000, GPIO_PERIPH_A, 0);
+
+	return 0;
+}
+
+postcore_initcall(hammerhead_init);
diff --git a/arch/avr32/boards/mimc200/setup.c b/arch/avr32/boards/mimc200/setup.c
index 397cbb8f44c8..2b58d61f0afc 100644
--- a/arch/avr32/boards/mimc200/setup.c
+++ b/arch/avr32/boards/mimc200/setup.c
@@ -24,7 +24,7 @@ extern struct atmel_lcdfb_info mimc200_lcdc_data;
 #include <video/atmel_lcdc.h>
 #include <linux/fb.h>
 
-#include <asm/atmel-mci.h>
+#include <linux/atmel-mci.h>
 #include <linux/io.h>
 #include <asm/setup.h>
 
@@ -207,8 +207,6 @@ static int __init mimc200_init(void)
 	 * reserve any pins for it.
 	 */
 
-	at32_add_system_devices();
-
 	at32_add_device_usart(0);
 	at32_add_device_usart(1);
 	at32_add_device_usart(2);
diff --git a/arch/avr32/configs/atngw100_defconfig b/arch/avr32/configs/atngw100_defconfig
index 541520912c55..164e2814ae78 100644
--- a/arch/avr32/configs/atngw100_defconfig
+++ b/arch/avr32/configs/atngw100_defconfig
@@ -892,7 +892,7 @@ CONFIG_DMA_ENGINE=y
 # DMA Clients
 #
 # CONFIG_NET_DMA is not set
-CONFIG_DMATEST=m
+# CONFIG_DMATEST is not set
 # CONFIG_UIO is not set
 
 #
diff --git a/arch/avr32/configs/atstk1002_defconfig b/arch/avr32/configs/atstk1002_defconfig
index 69fce6b6a783..c9dc64832a19 100644
--- a/arch/avr32/configs/atstk1002_defconfig
+++ b/arch/avr32/configs/atstk1002_defconfig
@@ -964,7 +964,7 @@ CONFIG_DMA_ENGINE=y
 # DMA Clients
 #
 # CONFIG_NET_DMA is not set
-CONFIG_DMATEST=m
+# CONFIG_DMATEST is not set
 # CONFIG_UIO is not set
 
 #
diff --git a/arch/avr32/configs/atstk1003_defconfig b/arch/avr32/configs/atstk1003_defconfig
index 5477ed3183b4..29ea1327b498 100644
--- a/arch/avr32/configs/atstk1003_defconfig
+++ b/arch/avr32/configs/atstk1003_defconfig
@@ -882,7 +882,7 @@ CONFIG_DMA_ENGINE=y
 # DMA Clients
 #
 # CONFIG_NET_DMA is not set
-CONFIG_DMATEST=m
+# CONFIG_DMATEST is not set
 # CONFIG_UIO is not set
 
 #
diff --git a/arch/avr32/configs/atstk1006_defconfig b/arch/avr32/configs/atstk1006_defconfig
index 6c45a3b77aa3..361c31c2af10 100644
--- a/arch/avr32/configs/atstk1006_defconfig
+++ b/arch/avr32/configs/atstk1006_defconfig
@@ -1014,7 +1014,7 @@ CONFIG_DMA_ENGINE=y
 # DMA Clients
 #
 # CONFIG_NET_DMA is not set
-CONFIG_DMATEST=m
+# CONFIG_DMATEST is not set
 # CONFIG_UIO is not set
 # CONFIG_STAGING is not set
 CONFIG_STAGING_EXCLUDE_BUILD=y
diff --git a/arch/avr32/configs/hammerhead_defconfig b/arch/avr32/configs/hammerhead_defconfig
new file mode 100644
index 000000000000..0d3d2982c8f5
--- /dev/null
+++ b/arch/avr32/configs/hammerhead_defconfig
@@ -0,0 +1,1467 @@
+#
+# Automatically generated make config: don't edit
+# Linux kernel version: 2.6.27
+# Tue Dec  9 15:37:30 2008
+#
+CONFIG_AVR32=y
+CONFIG_GENERIC_GPIO=y
+CONFIG_GENERIC_HARDIRQS=y
+CONFIG_STACKTRACE_SUPPORT=y
+CONFIG_LOCKDEP_SUPPORT=y
+CONFIG_TRACE_IRQFLAGS_SUPPORT=y
+CONFIG_HARDIRQS_SW_RESEND=y
+CONFIG_GENERIC_IRQ_PROBE=y
+CONFIG_RWSEM_GENERIC_SPINLOCK=y
+CONFIG_GENERIC_TIME=y
+CONFIG_GENERIC_CLOCKEVENTS=y
+# CONFIG_RWSEM_XCHGADD_ALGORITHM is not set
+# CONFIG_ARCH_HAS_ILOG2_U32 is not set
+# CONFIG_ARCH_HAS_ILOG2_U64 is not set
+CONFIG_GENERIC_HWEIGHT=y
+CONFIG_GENERIC_CALIBRATE_DELAY=y
+CONFIG_GENERIC_BUG=y
+CONFIG_DEFCONFIG_LIST="/lib/modules/$UNAME_RELEASE/.config"
+
+#
+# General setup
+#
+CONFIG_EXPERIMENTAL=y
+CONFIG_BROKEN_ON_SMP=y
+CONFIG_INIT_ENV_ARG_LIMIT=32
+CONFIG_LOCALVERSION=""
+# CONFIG_LOCALVERSION_AUTO is not set
+CONFIG_SWAP=y
+CONFIG_SYSVIPC=y
+CONFIG_SYSVIPC_SYSCTL=y
+CONFIG_POSIX_MQUEUE=y
+CONFIG_BSD_PROCESS_ACCT=y
+CONFIG_BSD_PROCESS_ACCT_V3=y
+# CONFIG_TASKSTATS is not set
+# CONFIG_AUDIT is not set
+# CONFIG_IKCONFIG is not set
+CONFIG_LOG_BUF_SHIFT=14
+# CONFIG_CGROUPS is not set
+# CONFIG_GROUP_SCHED is not set
+CONFIG_SYSFS_DEPRECATED=y
+CONFIG_SYSFS_DEPRECATED_V2=y
+# CONFIG_RELAY is not set
+# CONFIG_NAMESPACES is not set
+CONFIG_BLK_DEV_INITRD=y
+CONFIG_INITRAMFS_SOURCE=""
+CONFIG_CC_OPTIMIZE_FOR_SIZE=y
+CONFIG_SYSCTL=y
+CONFIG_EMBEDDED=y
+# CONFIG_SYSCTL_SYSCALL is not set
+CONFIG_KALLSYMS=y
+# CONFIG_KALLSYMS_ALL is not set
+# CONFIG_KALLSYMS_EXTRA_PASS is not set
+CONFIG_HOTPLUG=y
+CONFIG_PRINTK=y
+CONFIG_BUG=y
+CONFIG_ELF_CORE=y
+# CONFIG_COMPAT_BRK is not set
+# CONFIG_BASE_FULL is not set
+CONFIG_FUTEX=y
+CONFIG_ANON_INODES=y
+CONFIG_EPOLL=y
+CONFIG_SIGNALFD=y
+CONFIG_TIMERFD=y
+CONFIG_EVENTFD=y
+CONFIG_SHMEM=y
+CONFIG_AIO=y
+CONFIG_VM_EVENT_COUNTERS=y
+CONFIG_PCI_QUIRKS=y
+CONFIG_SLUB_DEBUG=y
+# CONFIG_SLAB is not set
+CONFIG_SLUB=y
+# CONFIG_SLOB is not set
+CONFIG_PROFILING=y
+# CONFIG_MARKERS is not set
+CONFIG_OPROFILE=m
+CONFIG_HAVE_OPROFILE=y
+CONFIG_KPROBES=y
+CONFIG_HAVE_KPROBES=y
+CONFIG_HAVE_CLK=y
+# CONFIG_HAVE_GENERIC_DMA_COHERENT is not set
+CONFIG_SLABINFO=y
+CONFIG_RT_MUTEXES=y
+# CONFIG_TINY_SHMEM is not set
+CONFIG_BASE_SMALL=1
+CONFIG_MODULES=y
+# CONFIG_MODULE_FORCE_LOAD is not set
+CONFIG_MODULE_UNLOAD=y
+CONFIG_MODULE_FORCE_UNLOAD=y
+# CONFIG_MODVERSIONS is not set
+# CONFIG_MODULE_SRCVERSION_ALL is not set
+CONFIG_KMOD=y
+CONFIG_BLOCK=y
+# CONFIG_LBD is not set
+# CONFIG_BLK_DEV_IO_TRACE is not set
+# CONFIG_LSF is not set
+# CONFIG_BLK_DEV_BSG is not set
+# CONFIG_BLK_DEV_INTEGRITY is not set
+
+#
+# IO Schedulers
+#
+CONFIG_IOSCHED_NOOP=y
+# CONFIG_IOSCHED_AS is not set
+# CONFIG_IOSCHED_DEADLINE is not set
+CONFIG_IOSCHED_CFQ=y
+# CONFIG_DEFAULT_AS is not set
+# CONFIG_DEFAULT_DEADLINE is not set
+CONFIG_DEFAULT_CFQ=y
+# CONFIG_DEFAULT_NOOP is not set
+CONFIG_DEFAULT_IOSCHED="cfq"
+CONFIG_CLASSIC_RCU=y
+# CONFIG_FREEZER is not set
+
+#
+# System Type and features
+#
+CONFIG_TICK_ONESHOT=y
+CONFIG_NO_HZ=y
+CONFIG_HIGH_RES_TIMERS=y
+CONFIG_GENERIC_CLOCKEVENTS_BUILD=y
+CONFIG_SUBARCH_AVR32B=y
+CONFIG_MMU=y
+CONFIG_PERFORMANCE_COUNTERS=y
+CONFIG_PLATFORM_AT32AP=y
+CONFIG_CPU_AT32AP700X=y
+CONFIG_CPU_AT32AP7000=y
+# CONFIG_BOARD_ATSTK1000 is not set
+# CONFIG_BOARD_ATNGW100 is not set
+CONFIG_BOARD_HAMMERHEAD=y
+# CONFIG_BOARD_FAVR_32 is not set
+# CONFIG_BOARD_MIMC200 is not set
+CONFIG_BOARD_HAMMERHEAD_USB=y
+CONFIG_BOARD_HAMMERHEAD_LCD=y
+CONFIG_BOARD_HAMMERHEAD_SND=y
+# CONFIG_BOARD_HAMMERHEAD_FPGA is not set
+CONFIG_LOADER_U_BOOT=y
+
+#
+# Atmel AVR32 AP options
+#
+# CONFIG_AP700X_32_BIT_SMC is not set
+CONFIG_AP700X_16_BIT_SMC=y
+# CONFIG_AP700X_8_BIT_SMC is not set
+CONFIG_LOAD_ADDRESS=0x10000000
+CONFIG_ENTRY_ADDRESS=0x90000000
+CONFIG_PHYS_OFFSET=0x10000000
+CONFIG_PREEMPT_NONE=y
+# CONFIG_PREEMPT_VOLUNTARY is not set
+# CONFIG_PREEMPT is not set
+CONFIG_QUICKLIST=y
+# CONFIG_HAVE_ARCH_BOOTMEM_NODE is not set
+# CONFIG_ARCH_HAVE_MEMORY_PRESENT is not set
+# CONFIG_NEED_NODE_MEMMAP_SIZE is not set
+CONFIG_ARCH_FLATMEM_ENABLE=y
+# CONFIG_ARCH_DISCONTIGMEM_ENABLE is not set
+# CONFIG_ARCH_SPARSEMEM_ENABLE is not set
+CONFIG_SELECT_MEMORY_MODEL=y
+CONFIG_FLATMEM_MANUAL=y
+# CONFIG_DISCONTIGMEM_MANUAL is not set
+# CONFIG_SPARSEMEM_MANUAL is not set
+CONFIG_FLATMEM=y
+CONFIG_FLAT_NODE_MEM_MAP=y
+CONFIG_PAGEFLAGS_EXTENDED=y
+CONFIG_SPLIT_PTLOCK_CPUS=4
+# CONFIG_RESOURCES_64BIT is not set
+# CONFIG_PHYS_ADDR_T_64BIT is not set
+CONFIG_ZONE_DMA_FLAG=0
+CONFIG_NR_QUICK=2
+CONFIG_VIRT_TO_BUS=y
+CONFIG_UNEVICTABLE_LRU=y
+# CONFIG_OWNERSHIP_TRACE is not set
+# CONFIG_NMI_DEBUGGING is not set
+# CONFIG_HZ_100 is not set
+CONFIG_HZ_250=y
+# CONFIG_HZ_300 is not set
+# CONFIG_HZ_1000 is not set
+CONFIG_HZ=250
+CONFIG_SCHED_HRTICK=y
+CONFIG_CMDLINE=""
+
+#
+# Power management options
+#
+# CONFIG_PM is not set
+CONFIG_ARCH_SUSPEND_POSSIBLE=y
+
+#
+# CPU Frequency scaling
+#
+CONFIG_CPU_FREQ=y
+CONFIG_CPU_FREQ_TABLE=y
+# CONFIG_CPU_FREQ_DEBUG is not set
+# CONFIG_CPU_FREQ_STAT is not set
+# CONFIG_CPU_FREQ_DEFAULT_GOV_PERFORMANCE is not set
+# CONFIG_CPU_FREQ_DEFAULT_GOV_POWERSAVE is not set
+# CONFIG_CPU_FREQ_DEFAULT_GOV_USERSPACE is not set
+CONFIG_CPU_FREQ_DEFAULT_GOV_ONDEMAND=y
+# CONFIG_CPU_FREQ_DEFAULT_GOV_CONSERVATIVE is not set
+CONFIG_CPU_FREQ_GOV_PERFORMANCE=y
+# CONFIG_CPU_FREQ_GOV_POWERSAVE is not set
+CONFIG_CPU_FREQ_GOV_USERSPACE=y
+CONFIG_CPU_FREQ_GOV_ONDEMAND=y
+# CONFIG_CPU_FREQ_GOV_CONSERVATIVE is not set
+CONFIG_CPU_FREQ_AT32AP=y
+
+#
+# Bus options
+#
+# CONFIG_ARCH_SUPPORTS_MSI is not set
+# CONFIG_PCCARD is not set
+
+#
+# Executable file formats
+#
+CONFIG_BINFMT_ELF=y
+# CONFIG_CORE_DUMP_DEFAULT_ELF_HEADERS is not set
+# CONFIG_HAVE_AOUT is not set
+# CONFIG_BINFMT_MISC is not set
+CONFIG_NET=y
+
+#
+# Networking options
+#
+CONFIG_PACKET=y
+CONFIG_PACKET_MMAP=y
+CONFIG_UNIX=y
+CONFIG_XFRM=y
+CONFIG_XFRM_USER=y
+# CONFIG_XFRM_SUB_POLICY is not set
+# CONFIG_XFRM_MIGRATE is not set
+# CONFIG_XFRM_STATISTICS is not set
+CONFIG_XFRM_IPCOMP=y
+CONFIG_NET_KEY=y
+# CONFIG_NET_KEY_MIGRATE is not set
+CONFIG_INET=y
+CONFIG_IP_MULTICAST=y
+CONFIG_IP_ADVANCED_ROUTER=y
+CONFIG_ASK_IP_FIB_HASH=y
+# CONFIG_IP_FIB_TRIE is not set
+CONFIG_IP_FIB_HASH=y
+# CONFIG_IP_MULTIPLE_TABLES is not set
+# CONFIG_IP_ROUTE_MULTIPATH is not set
+# CONFIG_IP_ROUTE_VERBOSE is not set
+CONFIG_IP_PNP=y
+CONFIG_IP_PNP_DHCP=y
+# CONFIG_IP_PNP_BOOTP is not set
+# CONFIG_IP_PNP_RARP is not set
+# CONFIG_NET_IPIP is not set
+# CONFIG_NET_IPGRE is not set
+CONFIG_IP_MROUTE=y
+CONFIG_IP_PIMSM_V1=y
+# CONFIG_IP_PIMSM_V2 is not set
+# CONFIG_ARPD is not set
+CONFIG_SYN_COOKIES=y
+CONFIG_INET_AH=y
+CONFIG_INET_ESP=y
+CONFIG_INET_IPCOMP=y
+CONFIG_INET_XFRM_TUNNEL=y
+CONFIG_INET_TUNNEL=y
+CONFIG_INET_XFRM_MODE_TRANSPORT=y
+CONFIG_INET_XFRM_MODE_TUNNEL=y
+CONFIG_INET_XFRM_MODE_BEET=y
+# CONFIG_INET_LRO is not set
+CONFIG_INET_DIAG=y
+CONFIG_INET_TCP_DIAG=y
+# CONFIG_TCP_CONG_ADVANCED is not set
+CONFIG_TCP_CONG_CUBIC=y
+CONFIG_DEFAULT_TCP_CONG="cubic"
+# CONFIG_TCP_MD5SIG is not set
+# CONFIG_IPV6 is not set
+# CONFIG_NETWORK_SECMARK is not set
+CONFIG_NETFILTER=y
+# CONFIG_NETFILTER_DEBUG is not set
+# CONFIG_NETFILTER_ADVANCED is not set
+
+#
+# Core Netfilter Configuration
+#
+CONFIG_NETFILTER_NETLINK=m
+CONFIG_NETFILTER_NETLINK_LOG=m
+CONFIG_NF_CONNTRACK=m
+CONFIG_NF_CONNTRACK_FTP=m
+CONFIG_NF_CONNTRACK_IRC=m
+CONFIG_NF_CONNTRACK_SIP=m
+CONFIG_NF_CT_NETLINK=m
+CONFIG_NETFILTER_XTABLES=y
+CONFIG_NETFILTER_XT_TARGET_MARK=m
+CONFIG_NETFILTER_XT_TARGET_NFLOG=m
+CONFIG_NETFILTER_XT_TARGET_TCPMSS=m
+CONFIG_NETFILTER_XT_MATCH_CONNTRACK=m
+CONFIG_NETFILTER_XT_MATCH_MARK=m
+CONFIG_NETFILTER_XT_MATCH_POLICY=m
+CONFIG_NETFILTER_XT_MATCH_STATE=m
+# CONFIG_IP_VS is not set
+
+#
+# IP: Netfilter Configuration
+#
+CONFIG_NF_DEFRAG_IPV4=m
+CONFIG_NF_CONNTRACK_IPV4=m
+CONFIG_NF_CONNTRACK_PROC_COMPAT=y
+CONFIG_IP_NF_IPTABLES=m
+CONFIG_IP_NF_FILTER=m
+CONFIG_IP_NF_TARGET_REJECT=m
+CONFIG_IP_NF_TARGET_LOG=m
+# CONFIG_IP_NF_TARGET_ULOG is not set
+CONFIG_NF_NAT=m
+CONFIG_NF_NAT_NEEDED=y
+CONFIG_IP_NF_TARGET_MASQUERADE=m
+CONFIG_NF_NAT_FTP=m
+CONFIG_NF_NAT_IRC=m
+# CONFIG_NF_NAT_TFTP is not set
+# CONFIG_NF_NAT_AMANDA is not set
+# CONFIG_NF_NAT_PPTP is not set
+# CONFIG_NF_NAT_H323 is not set
+CONFIG_NF_NAT_SIP=m
+CONFIG_IP_NF_MANGLE=m
+# CONFIG_IP_DCCP is not set
+# CONFIG_IP_SCTP is not set
+# CONFIG_TIPC is not set
+# CONFIG_ATM is not set
+# CONFIG_BRIDGE is not set
+# CONFIG_NET_DSA is not set
+# CONFIG_VLAN_8021Q is not set
+# CONFIG_DECNET is not set
+# CONFIG_LLC2 is not set
+# CONFIG_IPX is not set
+# CONFIG_ATALK is not set
+# CONFIG_X25 is not set
+# CONFIG_LAPB is not set
+# CONFIG_ECONET is not set
+# CONFIG_WAN_ROUTER is not set
+# CONFIG_NET_SCHED is not set
+
+#
+# Network testing
+#
+# CONFIG_NET_PKTGEN is not set
+# CONFIG_NET_TCPPROBE is not set
+# CONFIG_HAMRADIO is not set
+# CONFIG_CAN is not set
+# CONFIG_IRDA is not set
+# CONFIG_BT is not set
+# CONFIG_AF_RXRPC is not set
+# CONFIG_PHONET is not set
+CONFIG_WIRELESS=y
+# CONFIG_CFG80211 is not set
+# CONFIG_WIRELESS_OLD_REGULATORY is not set
+# CONFIG_WIRELESS_EXT is not set
+# CONFIG_MAC80211 is not set
+# CONFIG_IEEE80211 is not set
+# CONFIG_RFKILL is not set
+# CONFIG_NET_9P is not set
+
+#
+# Device Drivers
+#
+
+#
+# Generic Driver Options
+#
+CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
+CONFIG_STANDALONE=y
+# CONFIG_PREVENT_FIRMWARE_BUILD is not set
+# CONFIG_FW_LOADER is not set
+# CONFIG_DEBUG_DRIVER is not set
+# CONFIG_DEBUG_DEVRES is not set
+# CONFIG_SYS_HYPERVISOR is not set
+# CONFIG_CONNECTOR is not set
+CONFIG_MTD=y
+# CONFIG_MTD_DEBUG is not set
+# CONFIG_MTD_CONCAT is not set
+CONFIG_MTD_PARTITIONS=y
+# CONFIG_MTD_REDBOOT_PARTS is not set
+CONFIG_MTD_CMDLINE_PARTS=y
+# CONFIG_MTD_AR7_PARTS is not set
+
+#
+# User Modules And Translation Layers
+#
+CONFIG_MTD_CHAR=y
+CONFIG_MTD_BLKDEVS=y
+CONFIG_MTD_BLOCK=y
+# CONFIG_FTL is not set
+# CONFIG_NFTL is not set
+# CONFIG_INFTL is not set
+# CONFIG_RFD_FTL is not set
+# CONFIG_SSFDC is not set
+# CONFIG_MTD_OOPS is not set
+
+#
+# RAM/ROM/Flash chip drivers
+#
+CONFIG_MTD_CFI=y
+# CONFIG_MTD_JEDECPROBE is not set
+CONFIG_MTD_GEN_PROBE=y
+# CONFIG_MTD_CFI_ADV_OPTIONS is not set
+CONFIG_MTD_MAP_BANK_WIDTH_1=y
+CONFIG_MTD_MAP_BANK_WIDTH_2=y
+CONFIG_MTD_MAP_BANK_WIDTH_4=y
+# CONFIG_MTD_MAP_BANK_WIDTH_8 is not set
+# CONFIG_MTD_MAP_BANK_WIDTH_16 is not set
+# CONFIG_MTD_MAP_BANK_WIDTH_32 is not set
+CONFIG_MTD_CFI_I1=y
+CONFIG_MTD_CFI_I2=y
+# CONFIG_MTD_CFI_I4 is not set
+# CONFIG_MTD_CFI_I8 is not set
+# CONFIG_MTD_CFI_INTELEXT is not set
+CONFIG_MTD_CFI_AMDSTD=y
+# CONFIG_MTD_CFI_STAA is not set
+CONFIG_MTD_CFI_UTIL=y
+# CONFIG_MTD_RAM is not set
+# CONFIG_MTD_ROM is not set
+# CONFIG_MTD_ABSENT is not set
+
+#
+# Mapping drivers for chip access
+#
+# CONFIG_MTD_COMPLEX_MAPPINGS is not set
+CONFIG_MTD_PHYSMAP=y
+CONFIG_MTD_PHYSMAP_START=0x80000000
+CONFIG_MTD_PHYSMAP_LEN=0x0
+CONFIG_MTD_PHYSMAP_BANKWIDTH=2
+# CONFIG_MTD_PLATRAM is not set
+
+#
+# Self-contained MTD device drivers
+#
+CONFIG_MTD_DATAFLASH=y
+# CONFIG_MTD_DATAFLASH_WRITE_VERIFY is not set
+# CONFIG_MTD_DATAFLASH_OTP is not set
+# CONFIG_MTD_M25P80 is not set
+# CONFIG_MTD_SLRAM is not set
+# CONFIG_MTD_PHRAM is not set
+# CONFIG_MTD_MTDRAM is not set
+# CONFIG_MTD_BLOCK2MTD is not set
+
+#
+# Disk-On-Chip Device Drivers
+#
+# CONFIG_MTD_DOC2000 is not set
+# CONFIG_MTD_DOC2001 is not set
+# CONFIG_MTD_DOC2001PLUS is not set
+# CONFIG_MTD_NAND is not set
+# CONFIG_MTD_ONENAND is not set
+
+#
+# UBI - Unsorted block images
+#
+# CONFIG_MTD_UBI is not set
+# CONFIG_PARPORT is not set
+CONFIG_BLK_DEV=y
+# CONFIG_BLK_DEV_COW_COMMON is not set
+# CONFIG_BLK_DEV_LOOP is not set
+# CONFIG_BLK_DEV_NBD is not set
+# CONFIG_BLK_DEV_UB is not set
+CONFIG_BLK_DEV_RAM=m
+CONFIG_BLK_DEV_RAM_COUNT=16
+CONFIG_BLK_DEV_RAM_SIZE=4096
+# CONFIG_BLK_DEV_XIP is not set
+# CONFIG_CDROM_PKTCDVD is not set
+# CONFIG_ATA_OVER_ETH is not set
+CONFIG_MISC_DEVICES=y
+# CONFIG_ATMEL_PWM is not set
+CONFIG_ATMEL_TCLIB=y
+CONFIG_ATMEL_TCB_CLKSRC=y
+CONFIG_ATMEL_TCB_CLKSRC_BLOCK=0
+# CONFIG_EEPROM_93CX6 is not set
+# CONFIG_ATMEL_SSC is not set
+# CONFIG_ENCLOSURE_SERVICES is not set
+
+#
+# SCSI device support
+#
+# CONFIG_RAID_ATTRS is not set
+CONFIG_SCSI=m
+CONFIG_SCSI_DMA=y
+# CONFIG_SCSI_TGT is not set
+# CONFIG_SCSI_NETLINK is not set
+CONFIG_SCSI_PROC_FS=y
+
+#
+# SCSI support type (disk, tape, CD-ROM)
+#
+CONFIG_BLK_DEV_SD=m
+# CONFIG_CHR_DEV_ST is not set
+# CONFIG_CHR_DEV_OSST is not set
+# CONFIG_BLK_DEV_SR is not set
+# CONFIG_CHR_DEV_SG is not set
+# CONFIG_CHR_DEV_SCH is not set
+
+#
+# Some SCSI devices (e.g. CD jukebox) support multiple LUNs
+#
+# CONFIG_SCSI_MULTI_LUN is not set
+# CONFIG_SCSI_CONSTANTS is not set
+# CONFIG_SCSI_LOGGING is not set
+# CONFIG_SCSI_SCAN_ASYNC is not set
+CONFIG_SCSI_WAIT_SCAN=m
+
+#
+# SCSI Transports
+#
+# CONFIG_SCSI_SPI_ATTRS is not set
+# CONFIG_SCSI_FC_ATTRS is not set
+# CONFIG_SCSI_ISCSI_ATTRS is not set
+# CONFIG_SCSI_SAS_LIBSAS is not set
+# CONFIG_SCSI_SRP_ATTRS is not set
+CONFIG_SCSI_LOWLEVEL=y
+# CONFIG_ISCSI_TCP is not set
+# CONFIG_SCSI_DEBUG is not set
+# CONFIG_SCSI_DH is not set
+# CONFIG_ATA is not set
+# CONFIG_MD is not set
+CONFIG_NETDEVICES=y
+# CONFIG_DUMMY is not set
+# CONFIG_BONDING is not set
+# CONFIG_MACVLAN is not set
+# CONFIG_EQUALIZER is not set
+# CONFIG_TUN is not set
+# CONFIG_VETH is not set
+CONFIG_PHYLIB=y
+
+#
+# MII PHY device drivers
+#
+# CONFIG_MARVELL_PHY is not set
+# CONFIG_DAVICOM_PHY is not set
+# CONFIG_QSEMI_PHY is not set
+# CONFIG_LXT_PHY is not set
+# CONFIG_CICADA_PHY is not set
+# CONFIG_VITESSE_PHY is not set
+# CONFIG_SMSC_PHY is not set
+# CONFIG_BROADCOM_PHY is not set
+# CONFIG_ICPLUS_PHY is not set
+# CONFIG_REALTEK_PHY is not set
+# CONFIG_FIXED_PHY is not set
+# CONFIG_MDIO_BITBANG is not set
+CONFIG_NET_ETHERNET=y
+# CONFIG_MII is not set
+CONFIG_MACB=y
+# CONFIG_ENC28J60 is not set
+# CONFIG_IBM_NEW_EMAC_ZMII is not set
+# CONFIG_IBM_NEW_EMAC_RGMII is not set
+# CONFIG_IBM_NEW_EMAC_TAH is not set
+# CONFIG_IBM_NEW_EMAC_EMAC4 is not set
+# CONFIG_IBM_NEW_EMAC_NO_FLOW_CTRL is not set
+# CONFIG_IBM_NEW_EMAC_MAL_CLR_ICINTSTAT is not set
+# CONFIG_IBM_NEW_EMAC_MAL_COMMON_ERR is not set
+# CONFIG_B44 is not set
+# CONFIG_NETDEV_1000 is not set
+# CONFIG_NETDEV_10000 is not set
+
+#
+# Wireless LAN
+#
+# CONFIG_WLAN_PRE80211 is not set
+# CONFIG_WLAN_80211 is not set
+# CONFIG_IWLWIFI_LEDS is not set
+
+#
+# USB Network Adapters
+#
+# CONFIG_USB_CATC is not set
+# CONFIG_USB_KAWETH is not set
+# CONFIG_USB_PEGASUS is not set
+# CONFIG_USB_RTL8150 is not set
+# CONFIG_USB_USBNET is not set
+# CONFIG_WAN is not set
+# CONFIG_PPP is not set
+# CONFIG_SLIP is not set
+# CONFIG_NETCONSOLE is not set
+# CONFIG_NETPOLL is not set
+# CONFIG_NET_POLL_CONTROLLER is not set
+# CONFIG_ISDN is not set
+# CONFIG_PHONE is not set
+
+#
+# Input device support
+#
+CONFIG_INPUT=y
+CONFIG_INPUT_FF_MEMLESS=m
+# CONFIG_INPUT_POLLDEV is not set
+
+#
+# Userland interfaces
+#
+CONFIG_INPUT_MOUSEDEV=y
+CONFIG_INPUT_MOUSEDEV_PSAUX=y
+CONFIG_INPUT_MOUSEDEV_SCREEN_X=1024
+CONFIG_INPUT_MOUSEDEV_SCREEN_Y=768
+# CONFIG_INPUT_JOYDEV is not set
+CONFIG_INPUT_EVDEV=m
+# CONFIG_INPUT_EVBUG is not set
+
+#
+# Input Device Drivers
+#
+CONFIG_INPUT_KEYBOARD=y
+CONFIG_KEYBOARD_ATKBD=y
+# CONFIG_KEYBOARD_SUNKBD is not set
+# CONFIG_KEYBOARD_LKKBD is not set
+# CONFIG_KEYBOARD_XTKBD is not set
+# CONFIG_KEYBOARD_NEWTON is not set
+# CONFIG_KEYBOARD_STOWAWAY is not set
+# CONFIG_KEYBOARD_GPIO is not set
+CONFIG_INPUT_MOUSE=y
+CONFIG_MOUSE_PS2=y
+CONFIG_MOUSE_PS2_ALPS=y
+CONFIG_MOUSE_PS2_LOGIPS2PP=y
+CONFIG_MOUSE_PS2_SYNAPTICS=y
+CONFIG_MOUSE_PS2_LIFEBOOK=y
+CONFIG_MOUSE_PS2_TRACKPOINT=y
+# CONFIG_MOUSE_PS2_TOUCHKIT is not set
+# CONFIG_MOUSE_SERIAL is not set
+# CONFIG_MOUSE_APPLETOUCH is not set
+# CONFIG_MOUSE_BCM5974 is not set
+# CONFIG_MOUSE_VSXXXAA is not set
+# CONFIG_MOUSE_GPIO is not set
+# CONFIG_INPUT_JOYSTICK is not set
+# CONFIG_INPUT_TABLET is not set
+CONFIG_INPUT_TOUCHSCREEN=y
+# CONFIG_TOUCHSCREEN_ADS7846 is not set
+# CONFIG_TOUCHSCREEN_FUJITSU is not set
+# CONFIG_TOUCHSCREEN_GUNZE is not set
+# CONFIG_TOUCHSCREEN_ELO is not set
+# CONFIG_TOUCHSCREEN_MTOUCH is not set
+# CONFIG_TOUCHSCREEN_INEXIO is not set
+# CONFIG_TOUCHSCREEN_MK712 is not set
+# CONFIG_TOUCHSCREEN_PENMOUNT is not set
+# CONFIG_TOUCHSCREEN_TOUCHRIGHT is not set
+# CONFIG_TOUCHSCREEN_TOUCHWIN is not set
+# CONFIG_TOUCHSCREEN_USB_COMPOSITE is not set
+# CONFIG_TOUCHSCREEN_TOUCHIT213 is not set
+# CONFIG_INPUT_MISC is not set
+
+#
+# Hardware I/O ports
+#
+CONFIG_SERIO=y
+CONFIG_SERIO_I8042=y
+CONFIG_SERIO_SERPORT=y
+# CONFIG_SERIO_AT32PSIF is not set
+CONFIG_SERIO_LIBPS2=y
+# CONFIG_SERIO_RAW is not set
+# CONFIG_GAMEPORT is not set
+
+#
+# Character devices
+#
+CONFIG_VT=y
+CONFIG_CONSOLE_TRANSLATIONS=y
+CONFIG_VT_CONSOLE=y
+CONFIG_HW_CONSOLE=y
+# CONFIG_VT_HW_CONSOLE_BINDING is not set
+CONFIG_DEVKMEM=y
+# CONFIG_SERIAL_NONSTANDARD is not set
+
+#
+# Serial drivers
+#
+# CONFIG_SERIAL_8250 is not set
+
+#
+# Non-8250 serial port support
+#
+CONFIG_SERIAL_ATMEL=y
+CONFIG_SERIAL_ATMEL_CONSOLE=y
+CONFIG_SERIAL_ATMEL_PDC=y
+# CONFIG_SERIAL_ATMEL_TTYAT is not set
+CONFIG_SERIAL_CORE=y
+CONFIG_SERIAL_CORE_CONSOLE=y
+CONFIG_UNIX98_PTYS=y
+# CONFIG_LEGACY_PTYS is not set
+# CONFIG_IPMI_HANDLER is not set
+# CONFIG_HW_RANDOM is not set
+# CONFIG_R3964 is not set
+# CONFIG_RAW_DRIVER is not set
+# CONFIG_TCG_TPM is not set
+CONFIG_I2C=m
+CONFIG_I2C_BOARDINFO=y
+CONFIG_I2C_CHARDEV=m
+CONFIG_I2C_HELPER_AUTO=y
+CONFIG_I2C_ALGOBIT=m
+
+#
+# I2C Hardware Bus support
+#
+
+#
+# I2C system bus drivers (mostly embedded / system-on-chip)
+#
+CONFIG_I2C_GPIO=m
+# CONFIG_I2C_OCORES is not set
+# CONFIG_I2C_SIMTEC is not set
+
+#
+# External I2C/SMBus adapter drivers
+#
+# CONFIG_I2C_PARPORT_LIGHT is not set
+# CONFIG_I2C_TAOS_EVM is not set
+# CONFIG_I2C_TINY_USB is not set
+
+#
+# Other I2C/SMBus bus drivers
+#
+# CONFIG_I2C_PCA_PLATFORM is not set
+# CONFIG_I2C_STUB is not set
+
+#
+# Miscellaneous I2C Chip support
+#
+# CONFIG_DS1682 is not set
+# CONFIG_AT24 is not set
+# CONFIG_SENSORS_EEPROM is not set
+# CONFIG_SENSORS_PCF8574 is not set
+# CONFIG_PCF8575 is not set
+# CONFIG_SENSORS_PCA9539 is not set
+# CONFIG_SENSORS_PCF8591 is not set
+# CONFIG_TPS65010 is not set
+# CONFIG_SENSORS_MAX6875 is not set
+# CONFIG_SENSORS_TSL2550 is not set
+# CONFIG_I2C_DEBUG_CORE is not set
+# CONFIG_I2C_DEBUG_ALGO is not set
+# CONFIG_I2C_DEBUG_BUS is not set
+# CONFIG_I2C_DEBUG_CHIP is not set
+CONFIG_SPI=y
+# CONFIG_SPI_DEBUG is not set
+CONFIG_SPI_MASTER=y
+
+#
+# SPI Master Controller Drivers
+#
+CONFIG_SPI_ATMEL=y
+# CONFIG_SPI_BITBANG is not set
+
+#
+# SPI Protocol Masters
+#
+# CONFIG_SPI_AT25 is not set
+CONFIG_SPI_SPIDEV=m
+# CONFIG_SPI_TLE62X0 is not set
+CONFIG_ARCH_REQUIRE_GPIOLIB=y
+CONFIG_GPIOLIB=y
+# CONFIG_DEBUG_GPIO is not set
+# CONFIG_GPIO_SYSFS is not set
+
+#
+# I2C GPIO expanders:
+#
+# CONFIG_GPIO_MAX732X is not set
+# CONFIG_GPIO_PCA953X is not set
+# CONFIG_GPIO_PCF857X is not set
+
+#
+# PCI GPIO expanders:
+#
+
+#
+# SPI GPIO expanders:
+#
+# CONFIG_GPIO_MAX7301 is not set
+# CONFIG_GPIO_MCP23S08 is not set
+# CONFIG_W1 is not set
+# CONFIG_POWER_SUPPLY is not set
+# CONFIG_HWMON is not set
+# CONFIG_THERMAL is not set
+# CONFIG_THERMAL_HWMON is not set
+CONFIG_WATCHDOG=y
+# CONFIG_WATCHDOG_NOWAYOUT is not set
+
+#
+# Watchdog Device Drivers
+#
+# CONFIG_SOFT_WATCHDOG is not set
+CONFIG_AT32AP700X_WDT=y
+
+#
+# USB-based Watchdog Cards
+#
+# CONFIG_USBPCWATCHDOG is not set
+
+#
+# Sonics Silicon Backplane
+#
+CONFIG_SSB_POSSIBLE=y
+# CONFIG_SSB is not set
+
+#
+# Multifunction device drivers
+#
+# CONFIG_MFD_CORE is not set
+# CONFIG_MFD_SM501 is not set
+# CONFIG_HTC_PASIC3 is not set
+# CONFIG_MFD_TMIO is not set
+# CONFIG_MFD_WM8400 is not set
+# CONFIG_MFD_WM8350_I2C is not set
+
+#
+# Multimedia devices
+#
+
+#
+# Multimedia core support
+#
+# CONFIG_VIDEO_DEV is not set
+# CONFIG_DVB_CORE is not set
+# CONFIG_VIDEO_MEDIA is not set
+
+#
+# Multimedia drivers
+#
+# CONFIG_DAB is not set
+
+#
+# Graphics support
+#
+# CONFIG_VGASTATE is not set
+# CONFIG_VIDEO_OUTPUT_CONTROL is not set
+CONFIG_FB=y
+# CONFIG_FIRMWARE_EDID is not set
+# CONFIG_FB_DDC is not set
+# CONFIG_FB_BOOT_VESA_SUPPORT is not set
+CONFIG_FB_CFB_FILLRECT=y
+CONFIG_FB_CFB_COPYAREA=y
+CONFIG_FB_CFB_IMAGEBLIT=y
+# CONFIG_FB_CFB_REV_PIXELS_IN_BYTE is not set
+# CONFIG_FB_SYS_FILLRECT is not set
+# CONFIG_FB_SYS_COPYAREA is not set
+# CONFIG_FB_SYS_IMAGEBLIT is not set
+# CONFIG_FB_FOREIGN_ENDIAN is not set
+# CONFIG_FB_SYS_FOPS is not set
+# CONFIG_FB_SVGALIB is not set
+# CONFIG_FB_MACMODES is not set
+# CONFIG_FB_BACKLIGHT is not set
+# CONFIG_FB_MODE_HELPERS is not set
+# CONFIG_FB_TILEBLITTING is not set
+
+#
+# Frame buffer hardware drivers
+#
+# CONFIG_FB_S1D13XXX is not set
+CONFIG_FB_ATMEL=y
+# CONFIG_FB_VIRTUAL is not set
+# CONFIG_FB_METRONOME is not set
+# CONFIG_BACKLIGHT_LCD_SUPPORT is not set
+
+#
+# Display device support
+#
+# CONFIG_DISPLAY_SUPPORT is not set
+
+#
+# Console display driver support
+#
+CONFIG_DUMMY_CONSOLE=y
+CONFIG_FRAMEBUFFER_CONSOLE=y
+# CONFIG_FRAMEBUFFER_CONSOLE_DETECT_PRIMARY is not set
+# CONFIG_FRAMEBUFFER_CONSOLE_ROTATION is not set
+# CONFIG_FONTS is not set
+CONFIG_FONT_8x8=y
+CONFIG_FONT_8x16=y
+# CONFIG_LOGO is not set
+CONFIG_SOUND=m
+CONFIG_SOUND_OSS_CORE=y
+CONFIG_SND=m
+CONFIG_SND_TIMER=m
+CONFIG_SND_PCM=m
+CONFIG_SND_SEQUENCER=m
+# CONFIG_SND_SEQ_DUMMY is not set
+CONFIG_SND_OSSEMUL=y
+CONFIG_SND_MIXER_OSS=m
+CONFIG_SND_PCM_OSS=m
+CONFIG_SND_PCM_OSS_PLUGINS=y
+CONFIG_SND_SEQUENCER_OSS=y
+# CONFIG_SND_DYNAMIC_MINORS is not set
+# CONFIG_SND_SUPPORT_OLD_API is not set
+CONFIG_SND_VERBOSE_PROCFS=y
+# CONFIG_SND_VERBOSE_PRINTK is not set
+# CONFIG_SND_DEBUG is not set
+CONFIG_SND_DRIVERS=y
+# CONFIG_SND_DUMMY is not set
+# CONFIG_SND_VIRMIDI is not set
+# CONFIG_SND_MTPAV is not set
+# CONFIG_SND_SERIAL_U16550 is not set
+# CONFIG_SND_MPU401 is not set
+CONFIG_SND_SPI=y
+CONFIG_SND_USB=y
+# CONFIG_SND_USB_AUDIO is not set
+# CONFIG_SND_USB_CAIAQ is not set
+# CONFIG_SND_SOC is not set
+# CONFIG_SOUND_PRIME is not set
+CONFIG_HID_SUPPORT=y
+CONFIG_HID=y
+# CONFIG_HID_DEBUG is not set
+# CONFIG_HIDRAW is not set
+
+#
+# USB Input Devices
+#
+CONFIG_USB_HID=m
+# CONFIG_HID_PID is not set
+# CONFIG_USB_HIDDEV is not set
+
+#
+# USB HID Boot Protocol drivers
+#
+# CONFIG_USB_KBD is not set
+# CONFIG_USB_MOUSE is not set
+
+#
+# Special HID drivers
+#
+CONFIG_HID_COMPAT=y
+CONFIG_HID_A4TECH=m
+CONFIG_HID_APPLE=m
+CONFIG_HID_BELKIN=m
+CONFIG_HID_BRIGHT=m
+CONFIG_HID_CHERRY=m
+CONFIG_HID_CHICONY=m
+CONFIG_HID_CYPRESS=m
+CONFIG_HID_DELL=m
+CONFIG_HID_EZKEY=m
+CONFIG_HID_GYRATION=m
+CONFIG_HID_LOGITECH=m
+# CONFIG_LOGITECH_FF is not set
+# CONFIG_LOGIRUMBLEPAD2_FF is not set
+CONFIG_HID_MICROSOFT=m
+CONFIG_HID_MONTEREY=m
+CONFIG_HID_PANTHERLORD=m
+# CONFIG_PANTHERLORD_FF is not set
+CONFIG_HID_PETALYNX=m
+CONFIG_HID_SAMSUNG=m
+CONFIG_HID_SONY=m
+CONFIG_HID_SUNPLUS=m
+CONFIG_THRUSTMASTER_FF=m
+CONFIG_ZEROPLUS_FF=m
+CONFIG_USB_SUPPORT=y
+CONFIG_USB_ARCH_HAS_HCD=y
+# CONFIG_USB_ARCH_HAS_OHCI is not set
+# CONFIG_USB_ARCH_HAS_EHCI is not set
+CONFIG_USB=m
+# CONFIG_USB_DEBUG is not set
+# CONFIG_USB_ANNOUNCE_NEW_DEVICES is not set
+
+#
+# Miscellaneous USB options
+#
+CONFIG_USB_DEVICEFS=y
+# CONFIG_USB_DEVICE_CLASS is not set
+# CONFIG_USB_DYNAMIC_MINORS is not set
+# CONFIG_USB_OTG is not set
+# CONFIG_USB_OTG_WHITELIST is not set
+# CONFIG_USB_OTG_BLACKLIST_HUB is not set
+CONFIG_USB_MON=y
+
+#
+# USB Host Controller Drivers
+#
+# CONFIG_USB_C67X00_HCD is not set
+CONFIG_USB_ISP116X_HCD=m
+# CONFIG_USB_ISP1760_HCD is not set
+# CONFIG_USB_SL811_HCD is not set
+# CONFIG_USB_R8A66597_HCD is not set
+# CONFIG_USB_MUSB_HDRC is not set
+# CONFIG_USB_GADGET_MUSB_HDRC is not set
+
+#
+# USB Device Class drivers
+#
+# CONFIG_USB_ACM is not set
+# CONFIG_USB_PRINTER is not set
+# CONFIG_USB_WDM is not set
+# CONFIG_USB_TMC is not set
+
+#
+# NOTE: USB_STORAGE enables SCSI, and 'SCSI disk support'
+#
+
+#
+# may also be needed; see USB_STORAGE Help for more information
+#
+CONFIG_USB_STORAGE=m
+# CONFIG_USB_STORAGE_DEBUG is not set
+# CONFIG_USB_STORAGE_DATAFAB is not set
+# CONFIG_USB_STORAGE_FREECOM is not set
+# CONFIG_USB_STORAGE_ISD200 is not set
+# CONFIG_USB_STORAGE_DPCM is not set
+# CONFIG_USB_STORAGE_USBAT is not set
+# CONFIG_USB_STORAGE_SDDR09 is not set
+# CONFIG_USB_STORAGE_SDDR55 is not set
+# CONFIG_USB_STORAGE_JUMPSHOT is not set
+# CONFIG_USB_STORAGE_ALAUDA is not set
+# CONFIG_USB_STORAGE_ONETOUCH is not set
+# CONFIG_USB_STORAGE_KARMA is not set
+# CONFIG_USB_STORAGE_CYPRESS_ATACB is not set
+# CONFIG_USB_LIBUSUAL is not set
+
+#
+# USB Imaging devices
+#
+# CONFIG_USB_MDC800 is not set
+# CONFIG_USB_MICROTEK is not set
+
+#
+# USB port drivers
+#
+# CONFIG_USB_SERIAL is not set
+
+#
+# USB Miscellaneous drivers
+#
+# CONFIG_USB_EMI62 is not set
+# CONFIG_USB_EMI26 is not set
+# CONFIG_USB_ADUTUX is not set
+# CONFIG_USB_SEVSEG is not set
+# CONFIG_USB_RIO500 is not set
+# CONFIG_USB_LEGOTOWER is not set
+# CONFIG_USB_LCD is not set
+# CONFIG_USB_BERRY_CHARGE is not set
+# CONFIG_USB_LED is not set
+# CONFIG_USB_CYPRESS_CY7C63 is not set
+# CONFIG_USB_CYTHERM is not set
+# CONFIG_USB_PHIDGET is not set
+# CONFIG_USB_IDMOUSE is not set
+# CONFIG_USB_FTDI_ELAN is not set
+# CONFIG_USB_APPLEDISPLAY is not set
+# CONFIG_USB_LD is not set
+# CONFIG_USB_TRANCEVIBRATOR is not set
+# CONFIG_USB_IOWARRIOR is not set
+# CONFIG_USB_TEST is not set
+# CONFIG_USB_ISIGHTFW is not set
+# CONFIG_USB_VST is not set
+CONFIG_USB_GADGET=y
+# CONFIG_USB_GADGET_DEBUG is not set
+# CONFIG_USB_GADGET_DEBUG_FILES is not set
+CONFIG_USB_GADGET_VBUS_DRAW=2
+CONFIG_USB_GADGET_SELECTED=y
+# CONFIG_USB_GADGET_AT91 is not set
+CONFIG_USB_GADGET_ATMEL_USBA=y
+CONFIG_USB_ATMEL_USBA=y
+# CONFIG_USB_GADGET_FSL_USB2 is not set
+# CONFIG_USB_GADGET_LH7A40X is not set
+# CONFIG_USB_GADGET_OMAP is not set
+# CONFIG_USB_GADGET_PXA25X is not set
+# CONFIG_USB_GADGET_PXA27X is not set
+# CONFIG_USB_GADGET_S3C2410 is not set
+# CONFIG_USB_GADGET_M66592 is not set
+# CONFIG_USB_GADGET_AMD5536UDC is not set
+# CONFIG_USB_GADGET_FSL_QE is not set
+# CONFIG_USB_GADGET_NET2280 is not set
+# CONFIG_USB_GADGET_GOKU is not set
+# CONFIG_USB_GADGET_DUMMY_HCD is not set
+CONFIG_USB_GADGET_DUALSPEED=y
+CONFIG_USB_ZERO=m
+CONFIG_USB_ETH=m
+CONFIG_USB_ETH_RNDIS=y
+CONFIG_USB_GADGETFS=m
+CONFIG_USB_FILE_STORAGE=m
+# CONFIG_USB_FILE_STORAGE_TEST is not set
+CONFIG_USB_G_SERIAL=m
+# CONFIG_USB_MIDI_GADGET is not set
+# CONFIG_USB_G_PRINTER is not set
+# CONFIG_USB_CDC_COMPOSITE is not set
+CONFIG_MMC=m
+# CONFIG_MMC_DEBUG is not set
+# CONFIG_MMC_UNSAFE_RESUME is not set
+
+#
+# MMC/SD/SDIO Card Drivers
+#
+CONFIG_MMC_BLOCK=m
+CONFIG_MMC_BLOCK_BOUNCE=y
+# CONFIG_SDIO_UART is not set
+# CONFIG_MMC_TEST is not set
+
+#
+# MMC/SD/SDIO Host Controller Drivers
+#
+# CONFIG_MMC_SDHCI is not set
+CONFIG_MMC_ATMELMCI=m
+# CONFIG_MMC_SPI is not set
+# CONFIG_MEMSTICK is not set
+# CONFIG_NEW_LEDS is not set
+# CONFIG_ACCESSIBILITY is not set
+CONFIG_RTC_LIB=y
+CONFIG_RTC_CLASS=y
+CONFIG_RTC_HCTOSYS=y
+CONFIG_RTC_HCTOSYS_DEVICE="rtc0"
+# CONFIG_RTC_DEBUG is not set
+
+#
+# RTC interfaces
+#
+CONFIG_RTC_INTF_SYSFS=y
+CONFIG_RTC_INTF_PROC=y
+CONFIG_RTC_INTF_DEV=y
+# CONFIG_RTC_INTF_DEV_UIE_EMUL is not set
+# CONFIG_RTC_DRV_TEST is not set
+
+#
+# I2C RTC drivers
+#
+# CONFIG_RTC_DRV_DS1307 is not set
+# CONFIG_RTC_DRV_DS1374 is not set
+# CONFIG_RTC_DRV_DS1672 is not set
+# CONFIG_RTC_DRV_MAX6900 is not set
+# CONFIG_RTC_DRV_RS5C372 is not set
+# CONFIG_RTC_DRV_ISL1208 is not set
+# CONFIG_RTC_DRV_X1205 is not set
+# CONFIG_RTC_DRV_PCF8563 is not set
+# CONFIG_RTC_DRV_PCF8583 is not set
+# CONFIG_RTC_DRV_M41T80 is not set
+# CONFIG_RTC_DRV_S35390A is not set
+# CONFIG_RTC_DRV_FM3130 is not set
+
+#
+# SPI RTC drivers
+#
+# CONFIG_RTC_DRV_M41T94 is not set
+# CONFIG_RTC_DRV_DS1305 is not set
+# CONFIG_RTC_DRV_MAX6902 is not set
+# CONFIG_RTC_DRV_R9701 is not set
+# CONFIG_RTC_DRV_RS5C348 is not set
+# CONFIG_RTC_DRV_DS3234 is not set
+
+#
+# Platform RTC drivers
+#
+# CONFIG_RTC_DRV_DS1286 is not set
+# CONFIG_RTC_DRV_DS1511 is not set
+# CONFIG_RTC_DRV_DS1553 is not set
+# CONFIG_RTC_DRV_DS1742 is not set
+# CONFIG_RTC_DRV_STK17TA8 is not set
+# CONFIG_RTC_DRV_M48T86 is not set
+# CONFIG_RTC_DRV_M48T35 is not set
+# CONFIG_RTC_DRV_M48T59 is not set
+# CONFIG_RTC_DRV_BQ4802 is not set
+# CONFIG_RTC_DRV_V3020 is not set
+
+#
+# on-CPU RTC drivers
+#
+CONFIG_RTC_DRV_AT32AP700X=y
+# CONFIG_DMADEVICES is not set
+# CONFIG_UIO is not set
+# CONFIG_STAGING is not set
+
+#
+# File systems
+#
+CONFIG_EXT2_FS=m
+# CONFIG_EXT2_FS_XATTR is not set
+# CONFIG_EXT2_FS_XIP is not set
+# CONFIG_EXT3_FS is not set
+# CONFIG_EXT4_FS is not set
+# CONFIG_REISERFS_FS is not set
+# CONFIG_JFS_FS is not set
+# CONFIG_FS_POSIX_ACL is not set
+CONFIG_FILE_LOCKING=y
+# CONFIG_XFS_FS is not set
+# CONFIG_OCFS2_FS is not set
+# CONFIG_DNOTIFY is not set
+CONFIG_INOTIFY=y
+CONFIG_INOTIFY_USER=y
+# CONFIG_QUOTA is not set
+# CONFIG_AUTOFS_FS is not set
+# CONFIG_AUTOFS4_FS is not set
+# CONFIG_FUSE_FS is not set
+
+#
+# CD-ROM/DVD Filesystems
+#
+# CONFIG_ISO9660_FS is not set
+# CONFIG_UDF_FS is not set
+
+#
+# DOS/FAT/NT Filesystems
+#
+CONFIG_FAT_FS=y
+CONFIG_MSDOS_FS=y
+CONFIG_VFAT_FS=m
+CONFIG_FAT_DEFAULT_CODEPAGE=850
+CONFIG_FAT_DEFAULT_IOCHARSET="iso8859-1"
+# CONFIG_NTFS_FS is not set
+
+#
+# Pseudo filesystems
+#
+CONFIG_PROC_FS=y
+# CONFIG_PROC_KCORE is not set
+CONFIG_PROC_SYSCTL=y
+CONFIG_PROC_PAGE_MONITOR=y
+CONFIG_SYSFS=y
+CONFIG_TMPFS=y
+# CONFIG_TMPFS_POSIX_ACL is not set
+# CONFIG_HUGETLB_PAGE is not set
+CONFIG_CONFIGFS_FS=y
+
+#
+# Miscellaneous filesystems
+#
+# CONFIG_ADFS_FS is not set
+# CONFIG_AFFS_FS is not set
+# CONFIG_HFS_FS is not set
+# CONFIG_HFSPLUS_FS is not set
+# CONFIG_BEFS_FS is not set
+# CONFIG_BFS_FS is not set
+# CONFIG_EFS_FS is not set
+CONFIG_JFFS2_FS=y
+CONFIG_JFFS2_FS_DEBUG=0
+CONFIG_JFFS2_FS_WRITEBUFFER=y
+# CONFIG_JFFS2_FS_WBUF_VERIFY is not set
+# CONFIG_JFFS2_SUMMARY is not set
+# CONFIG_JFFS2_FS_XATTR is not set
+# CONFIG_JFFS2_COMPRESSION_OPTIONS is not set
+CONFIG_JFFS2_ZLIB=y
+# CONFIG_JFFS2_LZO is not set
+CONFIG_JFFS2_RTIME=y
+# CONFIG_JFFS2_RUBIN is not set
+# CONFIG_CRAMFS is not set
+# CONFIG_VXFS_FS is not set
+# CONFIG_MINIX_FS is not set
+# CONFIG_OMFS_FS is not set
+# CONFIG_HPFS_FS is not set
+# CONFIG_QNX4FS_FS is not set
+# CONFIG_ROMFS_FS is not set
+# CONFIG_SYSV_FS is not set
+# CONFIG_UFS_FS is not set
+CONFIG_NETWORK_FILESYSTEMS=y
+CONFIG_NFS_FS=y
+CONFIG_NFS_V3=y
+# CONFIG_NFS_V3_ACL is not set
+# CONFIG_NFS_V4 is not set
+CONFIG_ROOT_NFS=y
+# CONFIG_NFSD is not set
+CONFIG_LOCKD=y
+CONFIG_LOCKD_V4=y
+CONFIG_NFS_COMMON=y
+CONFIG_SUNRPC=y
+# CONFIG_SUNRPC_REGISTER_V4 is not set
+# CONFIG_RPCSEC_GSS_KRB5 is not set
+# CONFIG_RPCSEC_GSS_SPKM3 is not set
+# CONFIG_SMB_FS is not set
+# CONFIG_CIFS is not set
+# CONFIG_NCP_FS is not set
+# CONFIG_CODA_FS is not set
+# CONFIG_AFS_FS is not set
+
+#
+# Partition Types
+#
+# CONFIG_PARTITION_ADVANCED is not set
+CONFIG_MSDOS_PARTITION=y
+CONFIG_NLS=y
+CONFIG_NLS_DEFAULT="iso8859-1"
+CONFIG_NLS_CODEPAGE_437=m
+# CONFIG_NLS_CODEPAGE_737 is not set
+# CONFIG_NLS_CODEPAGE_775 is not set
+CONFIG_NLS_CODEPAGE_850=m
+# CONFIG_NLS_CODEPAGE_852 is not set
+# CONFIG_NLS_CODEPAGE_855 is not set
+# CONFIG_NLS_CODEPAGE_857 is not set
+# CONFIG_NLS_CODEPAGE_860 is not set
+# CONFIG_NLS_CODEPAGE_861 is not set
+# CONFIG_NLS_CODEPAGE_862 is not set
+# CONFIG_NLS_CODEPAGE_863 is not set
+# CONFIG_NLS_CODEPAGE_864 is not set
+# CONFIG_NLS_CODEPAGE_865 is not set
+# CONFIG_NLS_CODEPAGE_866 is not set
+# CONFIG_NLS_CODEPAGE_869 is not set
+# CONFIG_NLS_CODEPAGE_936 is not set
+# CONFIG_NLS_CODEPAGE_950 is not set
+# CONFIG_NLS_CODEPAGE_932 is not set
+# CONFIG_NLS_CODEPAGE_949 is not set
+# CONFIG_NLS_CODEPAGE_874 is not set
+# CONFIG_NLS_ISO8859_8 is not set
+# CONFIG_NLS_CODEPAGE_1250 is not set
+# CONFIG_NLS_CODEPAGE_1251 is not set
+# CONFIG_NLS_ASCII is not set
+CONFIG_NLS_ISO8859_1=m
+# CONFIG_NLS_ISO8859_2 is not set
+# CONFIG_NLS_ISO8859_3 is not set
+# CONFIG_NLS_ISO8859_4 is not set
+# CONFIG_NLS_ISO8859_5 is not set
+# CONFIG_NLS_ISO8859_6 is not set
+# CONFIG_NLS_ISO8859_7 is not set
+# CONFIG_NLS_ISO8859_9 is not set
+# CONFIG_NLS_ISO8859_13 is not set
+# CONFIG_NLS_ISO8859_14 is not set
+# CONFIG_NLS_ISO8859_15 is not set
+# CONFIG_NLS_KOI8_R is not set
+# CONFIG_NLS_KOI8_U is not set
+CONFIG_NLS_UTF8=m
+# CONFIG_DLM is not set
+
+#
+# Kernel hacking
+#
+# CONFIG_PRINTK_TIME is not set
+CONFIG_ENABLE_WARN_DEPRECATED=y
+CONFIG_ENABLE_MUST_CHECK=y
+CONFIG_FRAME_WARN=1024
+CONFIG_MAGIC_SYSRQ=y
+# CONFIG_UNUSED_SYMBOLS is not set
+# CONFIG_DEBUG_FS is not set
+# CONFIG_HEADERS_CHECK is not set
+CONFIG_DEBUG_KERNEL=y
+# CONFIG_DEBUG_SHIRQ is not set
+CONFIG_DETECT_SOFTLOCKUP=y
+# CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC is not set
+CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC_VALUE=0
+CONFIG_SCHED_DEBUG=y
+# CONFIG_SCHEDSTATS is not set
+# CONFIG_TIMER_STATS is not set
+# CONFIG_DEBUG_OBJECTS is not set
+# CONFIG_SLUB_DEBUG_ON is not set
+# CONFIG_SLUB_STATS is not set
+# CONFIG_DEBUG_RT_MUTEXES is not set
+# CONFIG_RT_MUTEX_TESTER is not set
+# CONFIG_DEBUG_SPINLOCK is not set
+# CONFIG_DEBUG_MUTEXES is not set
+# CONFIG_DEBUG_LOCK_ALLOC is not set
+# CONFIG_PROVE_LOCKING is not set
+# CONFIG_LOCK_STAT is not set
+# CONFIG_DEBUG_SPINLOCK_SLEEP is not set
+# CONFIG_DEBUG_LOCKING_API_SELFTESTS is not set
+# CONFIG_DEBUG_KOBJECT is not set
+CONFIG_DEBUG_BUGVERBOSE=y
+# CONFIG_DEBUG_INFO is not set
+# CONFIG_DEBUG_VM is not set
+# CONFIG_DEBUG_WRITECOUNT is not set
+# CONFIG_DEBUG_MEMORY_INIT is not set
+# CONFIG_DEBUG_LIST is not set
+# CONFIG_DEBUG_SG is not set
+CONFIG_FRAME_POINTER=y
+# CONFIG_BOOT_PRINTK_DELAY is not set
+# CONFIG_RCU_TORTURE_TEST is not set
+# CONFIG_RCU_CPU_STALL_DETECTOR is not set
+# CONFIG_KPROBES_SANITY_TEST is not set
+# CONFIG_BACKTRACE_SELF_TEST is not set
+# CONFIG_DEBUG_BLOCK_EXT_DEVT is not set
+# CONFIG_LKDTM is not set
+# CONFIG_FAULT_INJECTION is not set
+# CONFIG_DYNAMIC_PRINTK_DEBUG is not set
+# CONFIG_SAMPLES is not set
+
+#
+# Security options
+#
+# CONFIG_KEYS is not set
+# CONFIG_SECURITY is not set
+# CONFIG_SECURITYFS is not set
+# CONFIG_SECURITY_FILE_CAPABILITIES is not set
+CONFIG_CRYPTO=y
+
+#
+# Crypto core or helper
+#
+# CONFIG_CRYPTO_FIPS is not set
+CONFIG_CRYPTO_ALGAPI=y
+CONFIG_CRYPTO_AEAD=y
+CONFIG_CRYPTO_BLKCIPHER=y
+CONFIG_CRYPTO_HASH=y
+CONFIG_CRYPTO_RNG=y
+CONFIG_CRYPTO_MANAGER=y
+# CONFIG_CRYPTO_GF128MUL is not set
+# CONFIG_CRYPTO_NULL is not set
+# CONFIG_CRYPTO_CRYPTD is not set
+CONFIG_CRYPTO_AUTHENC=y
+# CONFIG_CRYPTO_TEST is not set
+
+#
+# Authenticated Encryption with Associated Data
+#
+# CONFIG_CRYPTO_CCM is not set
+# CONFIG_CRYPTO_GCM is not set
+# CONFIG_CRYPTO_SEQIV is not set
+
+#
+# Block modes
+#
+CONFIG_CRYPTO_CBC=y
+# CONFIG_CRYPTO_CTR is not set
+# CONFIG_CRYPTO_CTS is not set
+CONFIG_CRYPTO_ECB=m
+# CONFIG_CRYPTO_LRW is not set
+CONFIG_CRYPTO_PCBC=m
+# CONFIG_CRYPTO_XTS is not set
+
+#
+# Hash modes
+#
+CONFIG_CRYPTO_HMAC=y
+# CONFIG_CRYPTO_XCBC is not set
+
+#
+# Digest
+#
+# CONFIG_CRYPTO_CRC32C is not set
+# CONFIG_CRYPTO_MD4 is not set
+CONFIG_CRYPTO_MD5=y
+# CONFIG_CRYPTO_MICHAEL_MIC is not set
+# CONFIG_CRYPTO_RMD128 is not set
+# CONFIG_CRYPTO_RMD160 is not set
+# CONFIG_CRYPTO_RMD256 is not set
+# CONFIG_CRYPTO_RMD320 is not set
+CONFIG_CRYPTO_SHA1=y
+# CONFIG_CRYPTO_SHA256 is not set
+# CONFIG_CRYPTO_SHA512 is not set
+# CONFIG_CRYPTO_TGR192 is not set
+# CONFIG_CRYPTO_WP512 is not set
+
+#
+# Ciphers
+#
+# CONFIG_CRYPTO_AES is not set
+# CONFIG_CRYPTO_ANUBIS is not set
+CONFIG_CRYPTO_ARC4=m
+# CONFIG_CRYPTO_BLOWFISH is not set
+# CONFIG_CRYPTO_CAMELLIA is not set
+# CONFIG_CRYPTO_CAST5 is not set
+# CONFIG_CRYPTO_CAST6 is not set
+CONFIG_CRYPTO_DES=y
+# CONFIG_CRYPTO_FCRYPT is not set
+# CONFIG_CRYPTO_KHAZAD is not set
+# CONFIG_CRYPTO_SALSA20 is not set
+# CONFIG_CRYPTO_SEED is not set
+# CONFIG_CRYPTO_SERPENT is not set
+# CONFIG_CRYPTO_TEA is not set
+# CONFIG_CRYPTO_TWOFISH is not set
+
+#
+# Compression
+#
+CONFIG_CRYPTO_DEFLATE=y
+# CONFIG_CRYPTO_LZO is not set
+
+#
+# Random Number Generation
+#
+# CONFIG_CRYPTO_ANSI_CPRNG is not set
+CONFIG_CRYPTO_HW=y
+
+#
+# Library routines
+#
+CONFIG_BITREVERSE=y
+CONFIG_CRC_CCITT=m
+# CONFIG_CRC16 is not set
+# CONFIG_CRC_T10DIF is not set
+CONFIG_CRC_ITU_T=m
+CONFIG_CRC32=y
+CONFIG_CRC7=m
+# CONFIG_LIBCRC32C is not set
+CONFIG_ZLIB_INFLATE=y
+CONFIG_ZLIB_DEFLATE=y
+CONFIG_GENERIC_ALLOCATOR=y
+CONFIG_PLIST=y
+CONFIG_HAS_IOMEM=y
+CONFIG_HAS_IOPORT=y
+CONFIG_HAS_DMA=y
diff --git a/arch/avr32/include/asm/Kbuild b/arch/avr32/include/asm/Kbuild
index 3136628ba8d2..219822c8ad18 100644
--- a/arch/avr32/include/asm/Kbuild
+++ b/arch/avr32/include/asm/Kbuild
@@ -1,3 +1,4 @@
 include include/asm-generic/Kbuild.asm
 
+header-y	+= swab.h
 header-y	+= cachectl.h
diff --git a/arch/avr32/include/asm/atomic.h b/arch/avr32/include/asm/atomic.h
index 7ef3862a73d0..318815107748 100644
--- a/arch/avr32/include/asm/atomic.h
+++ b/arch/avr32/include/asm/atomic.h
@@ -14,9 +14,9 @@
 #ifndef __ASM_AVR32_ATOMIC_H
 #define __ASM_AVR32_ATOMIC_H
 
+#include <linux/types.h>
 #include <asm/system.h>
 
-typedef struct { volatile int counter; } atomic_t;
 #define ATOMIC_INIT(i)  { (i) }
 
 #define atomic_read(v)		((v)->counter)
diff --git a/arch/avr32/include/asm/byteorder.h b/arch/avr32/include/asm/byteorder.h
index 8e3af02076dd..2aba64b4e122 100644
--- a/arch/avr32/include/asm/byteorder.h
+++ b/arch/avr32/include/asm/byteorder.h
@@ -4,34 +4,7 @@
 #ifndef __ASM_AVR32_BYTEORDER_H
 #define __ASM_AVR32_BYTEORDER_H
 
-#include <asm/types.h>
-#include <linux/compiler.h>
+#include <asm/swab.h>
+#include <linux/byteorder/big_endian.h>
 
-#define __BIG_ENDIAN
-#define __SWAB_64_THRU_32__
-
-#ifdef __CHECKER__
-extern unsigned long __builtin_bswap_32(unsigned long x);
-extern unsigned short __builtin_bswap_16(unsigned short x);
-#endif
-
-/*
- * avr32-linux-gcc versions earlier than 4.2 improperly sign-extends
- * the result.
- */
-#if !(__GNUC__ == 4 && __GNUC_MINOR__ < 2)
-static inline __attribute_const__ __u16 __arch_swab16(__u16 val)
-{
-	return __builtin_bswap_16(val);
-}
-#define __arch_swab16 __arch_swab16
-
-static inline __attribute_const__ __u32 __arch_swab32(__u32 val)
-{
-	return __builtin_bswap_32(val);
-}
-#define __arch_swab32 __arch_swab32
-#endif
-
-#include <linux/byteorder.h>
 #endif /* __ASM_AVR32_BYTEORDER_H */
diff --git a/arch/avr32/include/asm/kdebug.h b/arch/avr32/include/asm/kdebug.h
index ca4f9542365a..f930ce286803 100644
--- a/arch/avr32/include/asm/kdebug.h
+++ b/arch/avr32/include/asm/kdebug.h
@@ -6,6 +6,7 @@ enum die_val {
 	DIE_BREAKPOINT,
 	DIE_SSTEP,
 	DIE_NMI,
+	DIE_OOPS,
 };
 
 #endif /* __ASM_AVR32_KDEBUG_H */
diff --git a/arch/avr32/include/asm/swab.h b/arch/avr32/include/asm/swab.h
new file mode 100644
index 000000000000..a14aa5b46d98
--- /dev/null
+++ b/arch/avr32/include/asm/swab.h
@@ -0,0 +1,35 @@
+/*
+ * AVR32 byteswapping functions.
+ */
+#ifndef __ASM_AVR32_SWAB_H
+#define __ASM_AVR32_SWAB_H
+
+#include <asm/types.h>
+#include <linux/compiler.h>
+
+#define __SWAB_64_THRU_32__
+
+#ifdef __CHECKER__
+extern unsigned long __builtin_bswap_32(unsigned long x);
+extern unsigned short __builtin_bswap_16(unsigned short x);
+#endif
+
+/*
+ * avr32-linux-gcc versions earlier than 4.2 improperly sign-extends
+ * the result.
+ */
+#if !(__GNUC__ == 4 && __GNUC_MINOR__ < 2)
+static inline __attribute_const__ __u16 __arch_swab16(__u16 val)
+{
+	return __builtin_bswap_16(val);
+}
+#define __arch_swab16 __arch_swab16
+
+static inline __attribute_const__ __u32 __arch_swab32(__u32 val)
+{
+	return __builtin_bswap_32(val);
+}
+#define __arch_swab32 __arch_swab32
+#endif
+
+#endif /* __ASM_AVR32_SWAB_H */
diff --git a/arch/avr32/include/asm/syscalls.h b/arch/avr32/include/asm/syscalls.h
new file mode 100644
index 000000000000..483d666c27c0
--- /dev/null
+++ b/arch/avr32/include/asm/syscalls.h
@@ -0,0 +1,39 @@
+/*
+ * syscalls.h - Linux syscall interfaces (arch-specific)
+ *
+ * Copyright (c) 2008 Jaswinder Singh
+ *
+ * This file is released under the GPLv2.
+ * See the file COPYING for more details.
+ */
+
+#ifndef _ASM_AVR32_SYSCALLS_H
+#define _ASM_AVR32_SYSCALLS_H
+
+#include <linux/compiler.h>
+#include <linux/linkage.h>
+#include <linux/types.h>
+#include <linux/signal.h>
+
+/* kernel/process.c */
+asmlinkage int sys_fork(struct pt_regs *);
+asmlinkage int sys_clone(unsigned long, unsigned long,
+			 unsigned long, unsigned long,
+			 struct pt_regs *);
+asmlinkage int sys_vfork(struct pt_regs *);
+asmlinkage int sys_execve(char __user *, char __user *__user *,
+			  char __user *__user *, struct pt_regs *);
+
+/* kernel/signal.c */
+asmlinkage int sys_sigaltstack(const stack_t __user *, stack_t __user *,
+			       struct pt_regs *);
+asmlinkage int sys_rt_sigreturn(struct pt_regs *);
+
+/* kernel/sys_avr32.c */
+asmlinkage long sys_mmap2(unsigned long, unsigned long, unsigned long,
+			  unsigned long, unsigned long, off_t);
+
+/* mm/cache.c */
+asmlinkage int sys_cacheflush(int, void __user *, size_t);
+
+#endif /* _ASM_AVR32_SYSCALLS_H */
diff --git a/arch/avr32/kernel/process.c b/arch/avr32/kernel/process.c
index 134d5302b6dd..43ae555ecb33 100644
--- a/arch/avr32/kernel/process.c
+++ b/arch/avr32/kernel/process.c
@@ -18,6 +18,7 @@
 
 #include <asm/sysreg.h>
 #include <asm/ocd.h>
+#include <asm/syscalls.h>
 
 #include <mach/pm.h>
 
diff --git a/arch/avr32/kernel/signal.c b/arch/avr32/kernel/signal.c
index c5b11f9067f1..803d7be0938f 100644
--- a/arch/avr32/kernel/signal.c
+++ b/arch/avr32/kernel/signal.c
@@ -19,6 +19,7 @@
 
 #include <asm/uaccess.h>
 #include <asm/ucontext.h>
+#include <asm/syscalls.h>
 
 #define _BLOCKABLE (~(sigmask(SIGKILL) | sigmask(SIGSTOP)))
 
diff --git a/arch/avr32/kernel/sys_avr32.c b/arch/avr32/kernel/sys_avr32.c
index 8e8911e55c8f..5d2daeaf356f 100644
--- a/arch/avr32/kernel/sys_avr32.c
+++ b/arch/avr32/kernel/sys_avr32.c
@@ -13,6 +13,7 @@
 
 #include <asm/mman.h>
 #include <asm/uaccess.h>
+#include <asm/syscalls.h>
 
 asmlinkage long sys_mmap2(unsigned long addr, unsigned long len,
 			  unsigned long prot, unsigned long flags,
diff --git a/arch/avr32/kernel/traps.c b/arch/avr32/kernel/traps.c
index 0d987373bc01..d547c8df157d 100644
--- a/arch/avr32/kernel/traps.c
+++ b/arch/avr32/kernel/traps.c
@@ -7,6 +7,7 @@
  */
 
 #include <linux/bug.h>
+#include <linux/hardirq.h>
 #include <linux/init.h>
 #include <linux/kallsyms.h>
 #include <linux/kdebug.h>
diff --git a/arch/avr32/mach-at32ap/at32ap700x.c b/arch/avr32/mach-at32ap/at32ap700x.c
index 066252eebf61..ea7bc1e8562b 100644
--- a/arch/avr32/mach-at32ap/at32ap700x.c
+++ b/arch/avr32/mach-at32ap/at32ap700x.c
@@ -15,8 +15,8 @@
 #include <linux/gpio.h>
 #include <linux/spi/spi.h>
 #include <linux/usb/atmel_usba_udc.h>
+#include <linux/atmel-mci.h>
 
-#include <asm/atmel-mci.h>
 #include <asm/io.h>
 #include <asm/irq.h>
 
@@ -421,7 +421,7 @@ static unsigned long hsb_clk_get_rate(struct clk *clk)
 	return bus_clk_get_rate(clk, shift);
 }
 
-static void pba_clk_mode(struct clk *clk, int enabled)
+void pba_clk_mode(struct clk *clk, int enabled)
 {
 	unsigned long flags;
 	u32 mask;
@@ -436,7 +436,7 @@ static void pba_clk_mode(struct clk *clk, int enabled)
 	spin_unlock_irqrestore(&pm_lock, flags);
 }
 
-static unsigned long pba_clk_get_rate(struct clk *clk)
+unsigned long pba_clk_get_rate(struct clk *clk)
 {
 	unsigned long cksel, shift = 0;
 
diff --git a/arch/avr32/mach-at32ap/clock.c b/arch/avr32/mach-at32ap/clock.c
index 138a00a2a2d0..442f08c5e641 100644
--- a/arch/avr32/mach-at32ap/clock.c
+++ b/arch/avr32/mach-at32ap/clock.c
@@ -198,7 +198,7 @@ dump_clock(struct clk *parent, struct clkinf *r)
 	unsigned	i;
 
 	/* skip clocks coupled to devices that aren't registered */
-	if (parent->dev && !parent->dev->bus_id[0] && !parent->users)
+	if (parent->dev && !dev_name(parent->dev) && !parent->users)
 		return;
 
 	/* <nest spaces> name <pad to end> */
@@ -214,7 +214,7 @@ dump_clock(struct clk *parent, struct clkinf *r)
 		parent->users ? "on" : "off",	/* NOTE: not-paranoid!! */
 		clk_get_rate(parent));
 	if (parent->dev)
-		seq_printf(r->s, ", for %s", parent->dev->bus_id);
+		seq_printf(r->s, ", for %s", dev_name(parent->dev));
 	seq_printf(r->s, "\n");
 
 	/* cost of this scan is small, but not linear... */
diff --git a/arch/avr32/mach-at32ap/clock.h b/arch/avr32/mach-at32ap/clock.h
index 623bf0e9a1e7..4c7ebbdc6dfa 100644
--- a/arch/avr32/mach-at32ap/clock.h
+++ b/arch/avr32/mach-at32ap/clock.h
@@ -30,3 +30,6 @@ struct clk {
 	u16		users;		/* Enabled if non-zero */
 	u16		index;		/* Sibling index */
 };
+
+unsigned long pba_clk_get_rate(struct clk *clk);
+void pba_clk_mode(struct clk *clk, int enabled);
diff --git a/arch/avr32/mach-at32ap/include/mach/at32ap700x.h b/arch/avr32/mach-at32ap/include/mach/at32ap700x.h
index a77d372f6f3e..5c4c971eed8e 100644
--- a/arch/avr32/mach-at32ap/include/mach/at32ap700x.h
+++ b/arch/avr32/mach-at32ap/include/mach/at32ap700x.h
@@ -211,4 +211,7 @@
 
 #define ATMEL_LCDC_ALT_15BIT	(ATMEL_LCDC_CONTROL | ATMEL_LCDC_ALT_15B_DATA)
 
+/* Bitmask for all EBI data (D16..D31) pins on port E */
+#define ATMEL_EBI_PE_DATA_ALL  (0x0000FFFF)
+
 #endif /* __ASM_ARCH_AT32AP700X_H__ */
diff --git a/arch/avr32/mach-at32ap/include/mach/portmux.h b/arch/avr32/mach-at32ap/include/mach/portmux.h
index 21c79373b53f..4873024e3b96 100644
--- a/arch/avr32/mach-at32ap/include/mach/portmux.h
+++ b/arch/avr32/mach-at32ap/include/mach/portmux.h
@@ -25,6 +25,6 @@ void at32_select_periph(unsigned int port, unsigned int pin,
 			unsigned int periph, unsigned long flags);
 void at32_select_gpio(unsigned int pin, unsigned long flags);
 void at32_deselect_pin(unsigned int pin);
-void at32_reserve_pin(unsigned int pin);
+void at32_reserve_pin(unsigned int port, u32 pin_mask);
 
 #endif /* __ASM_ARCH_PORTMUX_H__ */
diff --git a/arch/avr32/mach-at32ap/pio.c b/arch/avr32/mach-at32ap/pio.c
index ed81a8bcb22d..09a274c9d0b7 100644
--- a/arch/avr32/mach-at32ap/pio.c
+++ b/arch/avr32/mach-at32ap/pio.c
@@ -167,22 +167,29 @@ void at32_deselect_pin(unsigned int pin)
 }
 
 /* Reserve a pin, preventing anyone else from changing its configuration. */
-void __init at32_reserve_pin(unsigned int pin)
+void __init at32_reserve_pin(unsigned int port, u32 pin_mask)
 {
 	struct pio_device *pio;
-	unsigned int pin_index = pin & 0x1f;
 
-	pio = gpio_to_pio(pin);
+	/* assign and verify pio */
+	pio = gpio_to_pio(port);
 	if (unlikely(!pio)) {
-		printk("pio: invalid pin %u\n", pin);
+		printk(KERN_WARNING "pio: invalid port %u\n", port);
 		goto fail;
 	}
 
-	if (unlikely(test_and_set_bit(pin_index, &pio->pinmux_mask))) {
-		printk("%s: pin %u is busy\n", pio->name, pin_index);
+	/* Test if any of the requested pins is already muxed */
+	spin_lock(&pio_lock);
+	if (unlikely(pio->pinmux_mask & pin_mask)) {
+		printk(KERN_WARNING "%s: pin(s) busy (req. 0x%x, busy 0x%x)\n",
+		       pio->name, pin_mask, pio->pinmux_mask & pin_mask);
+		spin_unlock(&pio_lock);
 		goto fail;
 	}
 
+	/* Reserve pins */
+	pio->pinmux_mask |= pin_mask;
+	spin_unlock(&pio_lock);
 	return;
 
 fail:
diff --git a/arch/avr32/mm/cache.c b/arch/avr32/mm/cache.c
index 15a4e5e142c1..24a74d1ca7d9 100644
--- a/arch/avr32/mm/cache.c
+++ b/arch/avr32/mm/cache.c
@@ -13,6 +13,7 @@
 #include <asm/cachectl.h>
 #include <asm/processor.h>
 #include <asm/uaccess.h>
+#include <asm/syscalls.h>
 
 /*
  * If you attempt to flush anything more than this, you need superuser
diff --git a/arch/avr32/mm/init.c b/arch/avr32/mm/init.c
index fa92ff6d95f7..e819fa69a90e 100644
--- a/arch/avr32/mm/init.c
+++ b/arch/avr32/mm/init.c
@@ -97,7 +97,6 @@ void __init paging_init(void)
 
 	mem_map = NODE_DATA(0)->node_mem_map;
 
-	memset(zero_page, 0, PAGE_SIZE);
 	empty_zero_page = virt_to_page(zero_page);
 	flush_dcache_page(empty_zero_page);
 }
diff --git a/arch/blackfin/Kconfig b/arch/blackfin/Kconfig
index 29e71ed6b8a7..a949c4fbbddd 100644
--- a/arch/blackfin/Kconfig
+++ b/arch/blackfin/Kconfig
@@ -26,6 +26,7 @@ config BLACKFIN
 	default y
 	select HAVE_IDE
 	select HAVE_OPROFILE
+	select ARCH_WANT_OPTIONAL_GPIOLIB
 
 config ZONE_DMA
 	bool
@@ -59,10 +60,6 @@ config GENERIC_CALIBRATE_DELAY
 	bool
 	default y
 
-config HARDWARE_PM
-	def_bool y
-	depends on OPROFILE
-
 source "init/Kconfig"
 
 source "kernel/Kconfig.preempt"
@@ -77,6 +74,26 @@ choice
 	prompt "CPU"
 	default BF533
 
+config BF512
+	bool "BF512"
+	help
+	  BF512 Processor Support.
+
+config BF514
+	bool "BF514"
+	help
+	  BF514 Processor Support.
+
+config BF516
+	bool "BF516"
+	help
+	  BF516 Processor Support.
+
+config BF518
+	bool "BF518"
+	help
+	  BF518 Processor Support.
+
 config BF522
 	bool "BF522"
 	help
@@ -137,6 +154,16 @@ config BF537
 	help
 	  BF537 Processor Support.
 
+config BF538
+	bool "BF538"
+	help
+	  BF538 Processor Support.
+
+config BF539
+	bool "BF539"
+	help
+	  BF539 Processor Support.
+
 config BF542
 	bool "BF542"
 	help
@@ -169,28 +196,55 @@ config BF561
 
 endchoice
 
+config SMP
+	depends on BF561
+	bool "Symmetric multi-processing support"
+	---help---
+	  This enables support for systems with more than one CPU,
+	  like the dual core BF561. If you have a system with only one
+	  CPU, say N. If you have a system with more than one CPU, say Y.
+
+	  If you don't know what to do here, say N.
+
+config NR_CPUS
+	int
+	depends on SMP
+	default 2 if BF561
+
+config IRQ_PER_CPU
+	bool
+	depends on SMP
+	default y
+
+config TICK_SOURCE_SYSTMR0
+	bool
+	select BFIN_GPTIMERS
+	depends on SMP
+	default y
+
 config BF_REV_MIN
 	int
-	default 0 if (BF52x || BF54x)
+	default 0 if (BF51x || BF52x || BF54x)
 	default 2 if (BF537 || BF536 || BF534)
 	default 3 if (BF561 ||BF533 || BF532 || BF531)
+	default 4 if (BF538 || BF539)
 
 config BF_REV_MAX
 	int
-	default 2 if (BF52x || BF54x)
+	default 2 if (BF51x || BF52x || BF54x)
 	default 3 if (BF537 || BF536 || BF534)
-	default 5 if (BF561)
+	default 5 if (BF561 || BF538 || BF539)
 	default 6 if (BF533 || BF532 || BF531)
 
 choice
 	prompt "Silicon Rev"
-	default BF_REV_0_1 if (BF52x || BF54x)
+	default BF_REV_0_1 if (BF51x || BF52x || BF54x)
 	default BF_REV_0_2 if (BF534 || BF536 || BF537)
 	default BF_REV_0_3 if (BF531 || BF532 || BF533 || BF561)
 
 config BF_REV_0_0
 	bool "0.0"
-	depends on (BF52x || BF54x)
+	depends on (BF51x || BF52x || BF54x)
 
 config BF_REV_0_1
 	bool "0.1"
@@ -206,11 +260,11 @@ config BF_REV_0_3
 
 config BF_REV_0_4
 	bool "0.4"
-	depends on (BF561 || BF533 || BF532 || BF531)
+	depends on (BF561 || BF533 || BF532 || BF531 || BF538 || BF539)
 
 config BF_REV_0_5
 	bool "0.5"
-	depends on (BF561 || BF533 || BF532 || BF531)
+	depends on (BF561 || BF533 || BF532 || BF531 || BF538 || BF539)
 
 config BF_REV_0_6
 	bool "0.6"
@@ -224,6 +278,11 @@ config BF_REV_NONE
 
 endchoice
 
+config BF51x
+	bool
+	depends on (BF512 || BF514 || BF516 || BF518)
+	default y
+
 config BF52x
 	bool
 	depends on (BF522 || BF523 || BF524 || BF525 || BF526 || BF527)
@@ -258,7 +317,7 @@ config MEM_MT48LC16M16A2TG_75
 
 config MEM_MT48LC32M8A2_75
 	bool
-	depends on (BFIN537_STAMP || PNAV10)
+	depends on (BFIN537_STAMP || PNAV10 || BFIN538_EZKIT)
 	default y
 
 config MEM_MT48LC8M32B2B5_7
@@ -271,10 +330,17 @@ config MEM_MT48LC32M16A2TG_75
 	depends on (BFIN527_EZKIT || BFIN532_IP0X || BLACKSTAMP || BFIN526_EZBRD)
 	default y
 
+config MEM_MT48LC32M8A2_75
+	bool
+	depends on (BFIN518F_EZBRD)
+	default y
+
+source "arch/blackfin/mach-bf518/Kconfig"
 source "arch/blackfin/mach-bf527/Kconfig"
 source "arch/blackfin/mach-bf533/Kconfig"
 source "arch/blackfin/mach-bf561/Kconfig"
 source "arch/blackfin/mach-bf537/Kconfig"
+source "arch/blackfin/mach-bf538/Kconfig"
 source "arch/blackfin/mach-bf548/Kconfig"
 
 menu "Board customizations"
@@ -307,6 +373,7 @@ config BOOT_LOAD
 
 config ROM_BASE
 	hex "Kernel ROM Base"
+	depends on ROMKERNEL
 	default "0x20040000"
 	range 0x20000000 0x20400000 if !(BF54x || BF561)
 	range 0x20000000 0x30000000 if (BF54x || BF561)
@@ -318,7 +385,7 @@ config CLKIN_HZ
 	int "Frequency of the crystal on the board in Hz"
 	default "11059200" if BFIN533_STAMP
 	default "27000000" if BFIN533_EZKIT
-	default "25000000" if (BFIN537_STAMP || BFIN527_EZKIT || H8606_HVSISTEMAS || BLACKSTAMP || BFIN526_EZBRD)
+	default "25000000" if (BFIN537_STAMP || BFIN527_EZKIT || H8606_HVSISTEMAS || BLACKSTAMP || BFIN526_EZBRD || BFIN538_EZKIT || BFIN518F-EZBRD)
 	default "30000000" if BFIN561_EZKIT
 	default "24576000" if PNAV10
 	default "10000000" if BFIN532_IP0X
@@ -354,11 +421,11 @@ config VCO_MULT
 	range 1 64
 	default "22" if BFIN533_EZKIT
 	default "45" if BFIN533_STAMP
-	default "20" if (BFIN537_STAMP || BFIN527_EZKIT || BFIN548_EZKIT || BFIN548_BLUETECHNIX_CM)
+	default "20" if (BFIN537_STAMP || BFIN527_EZKIT || BFIN548_EZKIT || BFIN548_BLUETECHNIX_CM || BFIN538_EZKIT)
 	default "22" if BFIN533_BLUETECHNIX_CM
 	default "20" if (BFIN537_BLUETECHNIX_CM || BFIN527_BLUETECHNIX_CM || BFIN561_BLUETECHNIX_CM)
 	default "20" if BFIN561_EZKIT
-	default "16" if (H8606_HVSISTEMAS || BLACKSTAMP || BFIN526_EZBRD)
+	default "16" if (H8606_HVSISTEMAS || BLACKSTAMP || BFIN526_EZBRD || BFIN518F_EZBRD)
 	help
 	  This controls the frequency of the on-chip PLL. This can be between 1 and 64.
 	  PLL Frequency = (Crystal Frequency) * (this setting)
@@ -407,19 +474,70 @@ config MEM_MT46V32M16_5B
 	bool "MT46V32M16_5B"
 endchoice
 
-config MAX_MEM_SIZE
-	int "Max SDRAM Memory Size in MBytes"
-	depends on !MPU
-	default 512
+choice
+	prompt "DDR/SDRAM Timing"
+	depends on BFIN_KERNEL_CLOCK
+	default BFIN_KERNEL_CLOCK_MEMINIT_CALC
 	help
-	  This is the max memory size that the kernel will create CPLB
-	  tables for.  Your system will not be able to handle any more.
+	  This option allows you to specify Blackfin SDRAM/DDR Timing parameters
+	  The calculated SDRAM timing parameters may not be 100%
+	  accurate - This option is therefore marked experimental.
+
+config BFIN_KERNEL_CLOCK_MEMINIT_CALC
+	bool "Calculate Timings (EXPERIMENTAL)"
+	depends on EXPERIMENTAL
+
+config BFIN_KERNEL_CLOCK_MEMINIT_SPEC
+	bool "Provide accurate Timings based on target SCLK"
+	help
+	  Please consult the Blackfin Hardware Reference Manuals as well
+	  as the memory device datasheet.
+	  http://docs.blackfin.uclinux.org/doku.php?id=bfin:sdram
+endchoice
+
+menu "Memory Init Control"
+	depends on BFIN_KERNEL_CLOCK_MEMINIT_SPEC
+
+config MEM_DDRCTL0
+	depends on BF54x
+	hex "DDRCTL0"
+	default 0x0
+
+config MEM_DDRCTL1
+	depends on BF54x
+	hex "DDRCTL1"
+	default 0x0
+
+config MEM_DDRCTL2
+	depends on BF54x
+	hex "DDRCTL2"
+	default 0x0
+
+config MEM_EBIU_DDRQUE
+	depends on BF54x
+	hex "DDRQUE"
+	default 0x0
+
+config MEM_SDRRC
+	depends on !BF54x
+	hex "SDRRC"
+	default 0x0
+
+config MEM_SDGCTL
+	depends on !BF54x
+	hex "SDGCTL"
+	default 0x0
+endmenu
 
 #
 # Max & Min Speeds for various Chips
 #
 config MAX_VCO_HZ
 	int
+	default 400000000 if BF512
+	default 400000000 if BF514
+	default 400000000 if BF516
+	default 400000000 if BF518
 	default 600000000 if BF522
 	default 400000000 if BF523
 	default 400000000 if BF524
@@ -459,6 +577,7 @@ source kernel/Kconfig.hz
 
 config GENERIC_TIME
 	bool "Generic time"
+	depends on !SMP
 	default y
 
 config GENERIC_CLOCKEVENTS
@@ -533,6 +652,7 @@ endmenu
 
 
 menu "Blackfin Kernel Optimizations"
+	depends on !SMP
 
 comment "Memory Optimizations"
 
@@ -655,6 +775,17 @@ config APP_STACK_L1
 
 	  Currently only works with FLAT binaries.
 
+config EXCEPTION_L1_SCRATCH
+	bool "Locate exception stack in L1 Scratch Memory"
+	default n
+	depends on !APP_STACK_L1 && !SYSCALL_TAB_L1
+	help
+	  Whenever an exception occurs, use the L1 Scratch memory for
+	  stack storage.  You cannot place the stacks of FLAT binaries
+	  in L1 when using this option.
+
+	  If you don't use L1 Scratch, then you should say Y here.
+
 comment "Speed Optimizations"
 config BFIN_INS_LOWOVERHEAD
 	bool "ins[bwl] low overhead, higher interrupt latency"
@@ -684,7 +815,6 @@ config BFIN_INS_LOWOVERHEAD
 
 endmenu
 
-
 choice
 	prompt "Kernel executes from"
 	help
@@ -714,17 +844,9 @@ config BFIN_GPTIMERS
 	  To compile this driver as a module, choose M here: the module
 	  will be called gptimers.ko.
 
-config BFIN_DMA_5XX
-	bool "Enable DMA Support"
-	depends on (BF52x || BF53x || BF561 || BF54x)
-	default y
-	help
-	  DMA driver for BF5xx.
-
 choice
-	prompt "Uncached SDRAM region"
+	prompt "Uncached DMA region"
 	default DMA_UNCACHED_1M
-	depends on BFIN_DMA_5XX
 config DMA_UNCACHED_4M
 	bool "Enable 4M DMA region"
 config DMA_UNCACHED_2M
@@ -751,9 +873,11 @@ config BFIN_ICACHE_LOCK
 choice
 	prompt "Policy"
 	depends on BFIN_DCACHE
-	default BFIN_WB
+	default BFIN_WB if !SMP
+	default BFIN_WT if SMP
 config BFIN_WB
 	bool "Write back"
+	depends on !SMP
 	help
 	  Write Back Policy:
 	    Cached data will be written back to SDRAM only when needed.
@@ -790,7 +914,7 @@ endchoice
 
 config BFIN_L2_CACHEABLE
 	bool "Cache L2 SRAM"
-	depends on (BFIN_DCACHE || BFIN_ICACHE) && (BF54x || BF561)
+	depends on (BFIN_DCACHE || BFIN_ICACHE) && (BF54x || (BF561 && !SMP))
 	default n
 	help
 	  Select to make L2 SRAM cacheable in L1 data and instruction cache.
@@ -980,7 +1104,7 @@ config PM_WAKEUP_GPIO_NUMBER
 	int "GPIO number"
 	range 0 47
 	depends on PM_WAKEUP_BY_GPIO
-	default 2 if BFIN537_STAMP
+	default 2
 
 choice
 	prompt "GPIO Polarity"
@@ -1003,7 +1127,7 @@ comment "Possible Suspend Mem / Hibernate Wake-Up Sources"
 
 config PM_BFIN_WAKE_PH6
 	bool "Allow Wake-Up from on-chip PHY or PH6 GP"
-	depends on PM && (BF52x || BF534 || BF536 || BF537)
+	depends on PM && (BF51x || BF52x || BF534 || BF536 || BF537)
 	default n
 	help
 	  Enable PHY and PH6 GP Wake-Up (Voltage Regulator Power-Up)
@@ -1020,15 +1144,21 @@ menu "CPU Frequency scaling"
 
 source "drivers/cpufreq/Kconfig"
 
+config BFIN_CPU_FREQ
+	bool
+	depends on CPU_FREQ
+	select CPU_FREQ_TABLE
+	default y
+
 config CPU_VOLTAGE
 	bool "CPU Voltage scaling"
-	depends on EXPERIMENTAL	
+	depends on EXPERIMENTAL
 	depends on CPU_FREQ
 	default n
 	help
 	  Say Y here if you want CPU voltage scaling according to the CPU frequency.
 	  This option violates the PLL BYPASS recommendation in the Blackfin Processor
-	  manuals. There is a theoretical risk that during VDDINT transitions 
+	  manuals. There is a theoretical risk that during VDDINT transitions
 	  the PLL may unlock.
 
 endmenu
diff --git a/arch/blackfin/Kconfig.debug b/arch/blackfin/Kconfig.debug
index 3ad25983ec97..5f981d9ca625 100644
--- a/arch/blackfin/Kconfig.debug
+++ b/arch/blackfin/Kconfig.debug
@@ -2,8 +2,30 @@ menu "Kernel hacking"
 
 source "lib/Kconfig.debug"
 
+config DEBUG_STACKOVERFLOW
+	bool "Check for stack overflows"
+	depends on DEBUG_KERNEL
+	help
+	  This option will cause messages to be printed if free stack space
+	  drops below a certain limit.
+
+config DEBUG_STACK_USAGE
+	bool "Enable stack utilization instrumentation"
+	depends on DEBUG_KERNEL
+	help
+	  Enables the display of the minimum amount of free stack which each
+	  task has ever had available in the sysrq-T output.
+
+	  This option will slow down process creation somewhat.
+
 config HAVE_ARCH_KGDB
-       def_bool y
+	def_bool y
+
+config KGDB_TESTCASE
+	tristate "KGDB: for test case in expect"
+	default n
+	help
+	  This is a kgdb test case for automated testing.
 
 config DEBUG_VERBOSE
 	bool "Verbose fault messages"
@@ -182,11 +204,11 @@ config DEBUG_BFIN_HWTRACE_EXPAND_LEN
 	  4 for (2^4) 16k, or 4096 entries
 
 config DEBUG_BFIN_NO_KERN_HWTRACE
-	bool "Trace user apps (turn off hwtrace in kernel)"
+	bool "Turn off hwtrace in CPLB handlers"
 	depends on DEBUG_BFIN_HWTRACE_ON
-	default n
+	default y
 	help
-	  Some pieces of the kernel contain a lot of flow changes which can
+	  The CPLB error handler contains a lot of flow changes which can
 	  quickly fill up the hardware trace buffer.  When debugging crashes,
 	  the hardware trace may indicate that the problem lies in kernel
 	  space when in reality an application is buggy.
diff --git a/arch/blackfin/Makefile b/arch/blackfin/Makefile
index 6bf50977850c..e550c8d46066 100644
--- a/arch/blackfin/Makefile
+++ b/arch/blackfin/Makefile
@@ -21,6 +21,10 @@ KALLSYMS         += --symbol-prefix=_
 KBUILD_DEFCONFIG := BF537-STAMP_defconfig
 
 # setup the machine name and the machine dependent settings
+machine-$(CONFIG_BF512) := bf518
+machine-$(CONFIG_BF514) := bf518
+machine-$(CONFIG_BF516) := bf518
+machine-$(CONFIG_BF518) := bf518
 machine-$(CONFIG_BF522) := bf527
 machine-$(CONFIG_BF523) := bf527
 machine-$(CONFIG_BF524) := bf527
@@ -33,6 +37,8 @@ machine-$(CONFIG_BF533) := bf533
 machine-$(CONFIG_BF534) := bf537
 machine-$(CONFIG_BF536) := bf537
 machine-$(CONFIG_BF537) := bf537
+machine-$(CONFIG_BF538) := bf538
+machine-$(CONFIG_BF539) := bf538
 machine-$(CONFIG_BF542) := bf548
 machine-$(CONFIG_BF544) := bf548
 machine-$(CONFIG_BF547) := bf548
@@ -42,6 +48,10 @@ machine-$(CONFIG_BF561) := bf561
 MACHINE := $(machine-y)
 export MACHINE
 
+cpu-$(CONFIG_BF512) := bf512
+cpu-$(CONFIG_BF514) := bf514
+cpu-$(CONFIG_BF516) := bf516
+cpu-$(CONFIG_BF518) := bf518
 cpu-$(CONFIG_BF522) := bf522
 cpu-$(CONFIG_BF523) := bf523
 cpu-$(CONFIG_BF524) := bf524
@@ -54,6 +64,8 @@ cpu-$(CONFIG_BF533) := bf533
 cpu-$(CONFIG_BF534) := bf534
 cpu-$(CONFIG_BF536) := bf536
 cpu-$(CONFIG_BF537) := bf537
+cpu-$(CONFIG_BF538) := bf538
+cpu-$(CONFIG_BF539) := bf539
 cpu-$(CONFIG_BF542) := bf542
 cpu-$(CONFIG_BF544) := bf544
 cpu-$(CONFIG_BF547) := bf547
@@ -79,7 +91,7 @@ KBUILD_AFLAGS += -mcpu=$(cpu-y)-$(rev-y)
 CHECKFLAGS_SILICON = $(shell echo "" | $(CPP) $(KBUILD_CFLAGS) -dD - 2>/dev/null | awk '$$2 == "__SILICON_REVISION__" { print $$3 }')
 CHECKFLAGS += -D__SILICON_REVISION__=$(CHECKFLAGS_SILICON) -Dl1_text=__used__
 
-head-y   := arch/$(ARCH)/mach-$(MACHINE)/head.o arch/$(ARCH)/kernel/init_task.o
+head-y   := arch/$(ARCH)/kernel/init_task.o
 
 core-y   += arch/$(ARCH)/kernel/ arch/$(ARCH)/mm/ arch/$(ARCH)/mach-common/
 
@@ -95,10 +107,10 @@ else
 core-y	+= arch/$(ARCH)/kernel/cplb-nompu/
 endif
 
-libs-y   += arch/$(ARCH)/lib/
-
 drivers-$(CONFIG_OPROFILE) += arch/$(ARCH)/oprofile/
 
+libs-y   += arch/$(ARCH)/lib/
+
 machdirs	:= $(patsubst %,arch/blackfin/mach-%/, $(machine-y))
 
 KBUILD_CFLAGS += -Iarch/$(ARCH)/include/
diff --git a/arch/blackfin/configs/BF518F-EZBRD_defconfig b/arch/blackfin/configs/BF518F-EZBRD_defconfig
new file mode 100644
index 000000000000..e0b3f242b555
--- /dev/null
+++ b/arch/blackfin/configs/BF518F-EZBRD_defconfig
@@ -0,0 +1,1191 @@
+#
+# Automatically generated make config: don't edit
+# Linux kernel version: 2.6.28-rc2
+#
+# CONFIG_MMU is not set
+# CONFIG_FPU is not set
+CONFIG_RWSEM_GENERIC_SPINLOCK=y
+# CONFIG_RWSEM_XCHGADD_ALGORITHM is not set
+CONFIG_BLACKFIN=y
+CONFIG_ZONE_DMA=y
+CONFIG_GENERIC_FIND_NEXT_BIT=y
+CONFIG_GENERIC_HWEIGHT=y
+CONFIG_GENERIC_HARDIRQS=y
+CONFIG_GENERIC_IRQ_PROBE=y
+CONFIG_GENERIC_GPIO=y
+CONFIG_FORCE_MAX_ZONEORDER=14
+CONFIG_GENERIC_CALIBRATE_DELAY=y
+CONFIG_DEFCONFIG_LIST="/lib/modules/$UNAME_RELEASE/.config"
+
+#
+# General setup
+#
+CONFIG_EXPERIMENTAL=y
+CONFIG_BROKEN_ON_SMP=y
+CONFIG_INIT_ENV_ARG_LIMIT=32
+CONFIG_LOCALVERSION=""
+CONFIG_LOCALVERSION_AUTO=y
+CONFIG_SYSVIPC=y
+CONFIG_SYSVIPC_SYSCTL=y
+# CONFIG_POSIX_MQUEUE is not set
+# CONFIG_BSD_PROCESS_ACCT is not set
+# CONFIG_TASKSTATS is not set
+# CONFIG_AUDIT is not set
+CONFIG_IKCONFIG=y
+CONFIG_IKCONFIG_PROC=y
+CONFIG_LOG_BUF_SHIFT=14
+# CONFIG_CGROUPS is not set
+# CONFIG_GROUP_SCHED is not set
+# CONFIG_SYSFS_DEPRECATED_V2 is not set
+# CONFIG_RELAY is not set
+# CONFIG_NAMESPACES is not set
+CONFIG_BLK_DEV_INITRD=y
+CONFIG_INITRAMFS_SOURCE=""
+# CONFIG_CC_OPTIMIZE_FOR_SIZE is not set
+CONFIG_SYSCTL=y
+CONFIG_EMBEDDED=y
+CONFIG_UID16=y
+CONFIG_SYSCTL_SYSCALL=y
+CONFIG_KALLSYMS=y
+# CONFIG_KALLSYMS_ALL is not set
+# CONFIG_KALLSYMS_EXTRA_PASS is not set
+CONFIG_HOTPLUG=y
+CONFIG_PRINTK=y
+CONFIG_BUG=y
+# CONFIG_ELF_CORE is not set
+CONFIG_COMPAT_BRK=y
+CONFIG_BASE_FULL=y
+CONFIG_FUTEX=y
+CONFIG_ANON_INODES=y
+CONFIG_EPOLL=y
+CONFIG_SIGNALFD=y
+CONFIG_TIMERFD=y
+CONFIG_EVENTFD=y
+CONFIG_AIO=y
+CONFIG_VM_EVENT_COUNTERS=y
+CONFIG_SLAB=y
+# CONFIG_SLUB is not set
+# CONFIG_SLOB is not set
+# CONFIG_PROFILING is not set
+# CONFIG_MARKERS is not set
+CONFIG_HAVE_OPROFILE=y
+# CONFIG_HAVE_GENERIC_DMA_COHERENT is not set
+CONFIG_SLABINFO=y
+CONFIG_RT_MUTEXES=y
+CONFIG_TINY_SHMEM=y
+CONFIG_BASE_SMALL=0
+CONFIG_MODULES=y
+# CONFIG_MODULE_FORCE_LOAD is not set
+CONFIG_MODULE_UNLOAD=y
+# CONFIG_MODULE_FORCE_UNLOAD is not set
+# CONFIG_MODVERSIONS is not set
+# CONFIG_MODULE_SRCVERSION_ALL is not set
+CONFIG_KMOD=y
+CONFIG_BLOCK=y
+# CONFIG_LBD is not set
+# CONFIG_BLK_DEV_IO_TRACE is not set
+# CONFIG_LSF is not set
+# CONFIG_BLK_DEV_BSG is not set
+# CONFIG_BLK_DEV_INTEGRITY is not set
+
+#
+# IO Schedulers
+#
+CONFIG_IOSCHED_NOOP=y
+# CONFIG_IOSCHED_AS is not set
+# CONFIG_IOSCHED_DEADLINE is not set
+# CONFIG_IOSCHED_CFQ is not set
+# CONFIG_DEFAULT_AS is not set
+# CONFIG_DEFAULT_DEADLINE is not set
+# CONFIG_DEFAULT_CFQ is not set
+CONFIG_DEFAULT_NOOP=y
+CONFIG_DEFAULT_IOSCHED="noop"
+CONFIG_CLASSIC_RCU=y
+# CONFIG_PREEMPT_NONE is not set
+CONFIG_PREEMPT_VOLUNTARY=y
+# CONFIG_PREEMPT is not set
+# CONFIG_FREEZER is not set
+
+#
+# Blackfin Processor Options
+#
+
+#
+# Processor and Board Settings
+#
+# CONFIG_BF512 is not set
+# CONFIG_BF514 is not set
+# CONFIG_BF516 is not set
+CONFIG_BF518=y
+# CONFIG_BF522 is not set
+# CONFIG_BF523 is not set
+# CONFIG_BF524 is not set
+# CONFIG_BF525 is not set
+# CONFIG_BF526 is not set
+# CONFIG_BF527 is not set
+# CONFIG_BF531 is not set
+# CONFIG_BF532 is not set
+# CONFIG_BF533 is not set
+# CONFIG_BF534 is not set
+# CONFIG_BF536 is not set
+# CONFIG_BF537 is not set
+# CONFIG_BF538 is not set
+# CONFIG_BF539 is not set
+# CONFIG_BF542 is not set
+# CONFIG_BF544 is not set
+# CONFIG_BF547 is not set
+# CONFIG_BF548 is not set
+# CONFIG_BF549 is not set
+# CONFIG_BF561 is not set
+CONFIG_BF_REV_MIN=0
+CONFIG_BF_REV_MAX=2
+CONFIG_BF_REV_0_0=y
+# CONFIG_BF_REV_0_1 is not set
+# CONFIG_BF_REV_0_2 is not set
+# CONFIG_BF_REV_0_3 is not set
+# CONFIG_BF_REV_0_4 is not set
+# CONFIG_BF_REV_0_5 is not set
+# CONFIG_BF_REV_0_6 is not set
+# CONFIG_BF_REV_ANY is not set
+# CONFIG_BF_REV_NONE is not set
+CONFIG_BF51x=y
+CONFIG_BFIN518F_EZBRD=y
+
+#
+# BF518 Specific Configuration
+#
+
+#
+# Alternative Multiplexing Scheme
+#
+# CONFIG_BF518_SPORT0_PORTF is not set
+CONFIG_BF518_SPORT0_PORTG=y
+CONFIG_BF518_SPORT0_TSCLK_PG10=y
+# CONFIG_BF518_SPORT0_TSCLK_PG14 is not set
+CONFIG_BF518_UART1_PORTF=y
+# CONFIG_BF518_UART1_PORTG is not set
+
+#
+# Interrupt Priority Assignment
+#
+
+#
+# Priority
+#
+CONFIG_IRQ_PLL_WAKEUP=7
+CONFIG_IRQ_DMA0_ERROR=7
+CONFIG_IRQ_DMAR0_BLK=7
+CONFIG_IRQ_DMAR1_BLK=7
+CONFIG_IRQ_DMAR0_OVR=7
+CONFIG_IRQ_DMAR1_OVR=7
+CONFIG_IRQ_PPI_ERROR=7
+CONFIG_IRQ_MAC_ERROR=7
+CONFIG_IRQ_SPORT0_ERROR=7
+CONFIG_IRQ_SPORT1_ERROR=7
+CONFIG_IRQ_PTP_ERROR=7
+CONFIG_IRQ_UART0_ERROR=7
+CONFIG_IRQ_UART1_ERROR=7
+CONFIG_IRQ_RTC=8
+CONFIG_IRQ_PPI=8
+CONFIG_IRQ_SPORT0_RX=9
+CONFIG_IRQ_SPORT0_TX=9
+CONFIG_IRQ_SPORT1_RX=9
+CONFIG_IRQ_SPORT1_TX=9
+CONFIG_IRQ_TWI=10
+CONFIG_IRQ_SPI0=10
+CONFIG_IRQ_UART0_RX=10
+CONFIG_IRQ_UART0_TX=10
+CONFIG_IRQ_UART1_RX=10
+CONFIG_IRQ_UART1_TX=10
+CONFIG_IRQ_OPTSEC=11
+CONFIG_IRQ_CNT=11
+CONFIG_IRQ_MAC_RX=11
+CONFIG_IRQ_PORTH_INTA=11
+CONFIG_IRQ_MAC_TX=11
+CONFIG_IRQ_PORTH_INTB=11
+CONFIG_IRQ_TIMER0=12
+CONFIG_IRQ_TIMER1=12
+CONFIG_IRQ_TIMER2=12
+CONFIG_IRQ_TIMER3=12
+CONFIG_IRQ_TIMER4=12
+CONFIG_IRQ_TIMER5=12
+CONFIG_IRQ_TIMER6=12
+CONFIG_IRQ_TIMER7=12
+CONFIG_IRQ_PORTG_INTA=12
+CONFIG_IRQ_PORTG_INTB=12
+CONFIG_IRQ_MEM_DMA0=13
+CONFIG_IRQ_MEM_DMA1=13
+CONFIG_IRQ_WATCH=13
+CONFIG_IRQ_PORTF_INTA=13
+CONFIG_IRQ_PORTF_INTB=13
+CONFIG_IRQ_SPI0_ERROR=7
+CONFIG_IRQ_SPI1_ERROR=7
+CONFIG_IRQ_RSI_INT0=7
+CONFIG_IRQ_RSI_INT1=7
+CONFIG_IRQ_PWM_TRIP=10
+CONFIG_IRQ_PWM_SYNC=10
+CONFIG_IRQ_PTP_STAT=10
+
+#
+# Board customizations
+#
+# CONFIG_CMDLINE_BOOL is not set
+CONFIG_BOOT_LOAD=0x1000
+
+#
+# Clock/PLL Setup
+#
+CONFIG_CLKIN_HZ=25000000
+# CONFIG_BFIN_KERNEL_CLOCK is not set
+CONFIG_MAX_VCO_HZ=400000000
+CONFIG_MIN_VCO_HZ=50000000
+CONFIG_MAX_SCLK_HZ=133333333
+CONFIG_MIN_SCLK_HZ=27000000
+
+#
+# Kernel Timer/Scheduler
+#
+# CONFIG_HZ_100 is not set
+CONFIG_HZ_250=y
+# CONFIG_HZ_300 is not set
+# CONFIG_HZ_1000 is not set
+CONFIG_HZ=250
+# CONFIG_SCHED_HRTICK is not set
+CONFIG_GENERIC_TIME=y
+CONFIG_GENERIC_CLOCKEVENTS=y
+# CONFIG_CYCLES_CLOCKSOURCE is not set
+# CONFIG_NO_HZ is not set
+# CONFIG_HIGH_RES_TIMERS is not set
+CONFIG_GENERIC_CLOCKEVENTS_BUILD=y
+
+#
+# Misc
+#
+CONFIG_BFIN_SCRATCH_REG_RETN=y
+# CONFIG_BFIN_SCRATCH_REG_RETE is not set
+# CONFIG_BFIN_SCRATCH_REG_CYCLES is not set
+
+#
+# Blackfin Kernel Optimizations
+#
+
+#
+# Memory Optimizations
+#
+CONFIG_I_ENTRY_L1=y
+CONFIG_EXCPT_IRQ_SYSC_L1=y
+CONFIG_DO_IRQ_L1=y
+CONFIG_CORE_TIMER_IRQ_L1=y
+CONFIG_IDLE_L1=y
+# CONFIG_SCHEDULE_L1 is not set
+CONFIG_ARITHMETIC_OPS_L1=y
+CONFIG_ACCESS_OK_L1=y
+# CONFIG_MEMSET_L1 is not set
+# CONFIG_MEMCPY_L1 is not set
+# CONFIG_SYS_BFIN_SPINLOCK_L1 is not set
+# CONFIG_IP_CHECKSUM_L1 is not set
+CONFIG_CACHELINE_ALIGNED_L1=y
+# CONFIG_SYSCALL_TAB_L1 is not set
+# CONFIG_CPLB_SWITCH_TAB_L1 is not set
+CONFIG_APP_STACK_L1=y
+
+#
+# Speed Optimizations
+#
+CONFIG_BFIN_INS_LOWOVERHEAD=y
+CONFIG_RAMKERNEL=y
+# CONFIG_ROMKERNEL is not set
+CONFIG_SELECT_MEMORY_MODEL=y
+CONFIG_FLATMEM_MANUAL=y
+# CONFIG_DISCONTIGMEM_MANUAL is not set
+# CONFIG_SPARSEMEM_MANUAL is not set
+CONFIG_FLATMEM=y
+CONFIG_FLAT_NODE_MEM_MAP=y
+CONFIG_PAGEFLAGS_EXTENDED=y
+CONFIG_SPLIT_PTLOCK_CPUS=4
+# CONFIG_RESOURCES_64BIT is not set
+# CONFIG_PHYS_ADDR_T_64BIT is not set
+CONFIG_ZONE_DMA_FLAG=1
+CONFIG_VIRT_TO_BUS=y
+CONFIG_BFIN_GPTIMERS=y
+# CONFIG_DMA_UNCACHED_4M is not set
+# CONFIG_DMA_UNCACHED_2M is not set
+CONFIG_DMA_UNCACHED_1M=y
+# CONFIG_DMA_UNCACHED_NONE is not set
+
+#
+# Cache Support
+#
+CONFIG_BFIN_ICACHE=y
+CONFIG_BFIN_DCACHE=y
+# CONFIG_BFIN_DCACHE_BANKA is not set
+# CONFIG_BFIN_ICACHE_LOCK is not set
+CONFIG_BFIN_WB=y
+# CONFIG_BFIN_WT is not set
+# CONFIG_MPU is not set
+
+#
+# Asynchonous Memory Configuration
+#
+
+#
+# EBIU_AMGCTL Global Control
+#
+CONFIG_C_AMCKEN=y
+CONFIG_C_CDPRIO=y
+# CONFIG_C_AMBEN is not set
+# CONFIG_C_AMBEN_B0 is not set
+# CONFIG_C_AMBEN_B0_B1 is not set
+# CONFIG_C_AMBEN_B0_B1_B2 is not set
+CONFIG_C_AMBEN_ALL=y
+
+#
+# EBIU_AMBCTL Control
+#
+CONFIG_BANK_0=0x7BB0
+CONFIG_BANK_1=0x5554
+CONFIG_BANK_2=0x7BB0
+CONFIG_BANK_3=0xFFC0
+
+#
+# Bus options (PCI, PCMCIA, EISA, MCA, ISA)
+#
+# CONFIG_ARCH_SUPPORTS_MSI is not set
+# CONFIG_PCCARD is not set
+
+#
+# Executable file formats
+#
+CONFIG_BINFMT_ELF_FDPIC=y
+CONFIG_BINFMT_FLAT=y
+CONFIG_BINFMT_ZFLAT=y
+# CONFIG_BINFMT_SHARED_FLAT is not set
+# CONFIG_HAVE_AOUT is not set
+# CONFIG_BINFMT_MISC is not set
+
+#
+# Power management options
+#
+# CONFIG_PM is not set
+CONFIG_ARCH_SUSPEND_POSSIBLE=y
+# CONFIG_PM_WAKEUP_BY_GPIO is not set
+
+#
+# CPU Frequency scaling
+#
+# CONFIG_CPU_FREQ is not set
+CONFIG_NET=y
+
+#
+# Networking options
+#
+CONFIG_PACKET=y
+# CONFIG_PACKET_MMAP is not set
+CONFIG_UNIX=y
+CONFIG_XFRM=y
+# CONFIG_XFRM_USER is not set
+# CONFIG_XFRM_SUB_POLICY is not set
+# CONFIG_XFRM_MIGRATE is not set
+# CONFIG_XFRM_STATISTICS is not set
+# CONFIG_NET_KEY is not set
+CONFIG_INET=y
+# CONFIG_IP_MULTICAST is not set
+# CONFIG_IP_ADVANCED_ROUTER is not set
+CONFIG_IP_FIB_HASH=y
+CONFIG_IP_PNP=y
+# CONFIG_IP_PNP_DHCP is not set
+# CONFIG_IP_PNP_BOOTP is not set
+# CONFIG_IP_PNP_RARP is not set
+# CONFIG_NET_IPIP is not set
+# CONFIG_NET_IPGRE is not set
+# CONFIG_ARPD is not set
+CONFIG_SYN_COOKIES=y
+# CONFIG_INET_AH is not set
+# CONFIG_INET_ESP is not set
+# CONFIG_INET_IPCOMP is not set
+# CONFIG_INET_XFRM_TUNNEL is not set
+# CONFIG_INET_TUNNEL is not set
+CONFIG_INET_XFRM_MODE_TRANSPORT=y
+CONFIG_INET_XFRM_MODE_TUNNEL=y
+CONFIG_INET_XFRM_MODE_BEET=y
+# CONFIG_INET_LRO is not set
+CONFIG_INET_DIAG=y
+CONFIG_INET_TCP_DIAG=y
+# CONFIG_TCP_CONG_ADVANCED is not set
+CONFIG_TCP_CONG_CUBIC=y
+CONFIG_DEFAULT_TCP_CONG="cubic"
+# CONFIG_TCP_MD5SIG is not set
+# CONFIG_IPV6 is not set
+# CONFIG_NETLABEL is not set
+# CONFIG_NETWORK_SECMARK is not set
+# CONFIG_NETFILTER is not set
+# CONFIG_IP_DCCP is not set
+# CONFIG_IP_SCTP is not set
+# CONFIG_TIPC is not set
+# CONFIG_ATM is not set
+# CONFIG_BRIDGE is not set
+# CONFIG_NET_DSA is not set
+# CONFIG_VLAN_8021Q is not set
+# CONFIG_DECNET is not set
+# CONFIG_LLC2 is not set
+# CONFIG_IPX is not set
+# CONFIG_ATALK is not set
+# CONFIG_X25 is not set
+# CONFIG_LAPB is not set
+# CONFIG_ECONET is not set
+# CONFIG_WAN_ROUTER is not set
+# CONFIG_NET_SCHED is not set
+
+#
+# Network testing
+#
+# CONFIG_NET_PKTGEN is not set
+# CONFIG_HAMRADIO is not set
+# CONFIG_CAN is not set
+# CONFIG_IRDA is not set
+# CONFIG_BT is not set
+# CONFIG_AF_RXRPC is not set
+# CONFIG_PHONET is not set
+CONFIG_WIRELESS=y
+# CONFIG_CFG80211 is not set
+CONFIG_WIRELESS_OLD_REGULATORY=y
+# CONFIG_WIRELESS_EXT is not set
+# CONFIG_MAC80211 is not set
+# CONFIG_IEEE80211 is not set
+# CONFIG_RFKILL is not set
+# CONFIG_NET_9P is not set
+
+#
+# Device Drivers
+#
+
+#
+# Generic Driver Options
+#
+CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
+CONFIG_STANDALONE=y
+CONFIG_PREVENT_FIRMWARE_BUILD=y
+# CONFIG_FW_LOADER is not set
+# CONFIG_DEBUG_DRIVER is not set
+# CONFIG_DEBUG_DEVRES is not set
+# CONFIG_SYS_HYPERVISOR is not set
+# CONFIG_CONNECTOR is not set
+CONFIG_MTD=y
+# CONFIG_MTD_DEBUG is not set
+# CONFIG_MTD_CONCAT is not set
+CONFIG_MTD_PARTITIONS=y
+# CONFIG_MTD_REDBOOT_PARTS is not set
+# CONFIG_MTD_CMDLINE_PARTS is not set
+# CONFIG_MTD_AR7_PARTS is not set
+
+#
+# User Modules And Translation Layers
+#
+CONFIG_MTD_CHAR=y
+CONFIG_MTD_BLKDEVS=y
+CONFIG_MTD_BLOCK=y
+# CONFIG_FTL is not set
+# CONFIG_NFTL is not set
+# CONFIG_INFTL is not set
+# CONFIG_RFD_FTL is not set
+# CONFIG_SSFDC is not set
+# CONFIG_MTD_OOPS is not set
+
+#
+# RAM/ROM/Flash chip drivers
+#
+# CONFIG_MTD_CFI is not set
+CONFIG_MTD_JEDECPROBE=m
+CONFIG_MTD_GEN_PROBE=m
+# CONFIG_MTD_CFI_ADV_OPTIONS is not set
+CONFIG_MTD_MAP_BANK_WIDTH_1=y
+CONFIG_MTD_MAP_BANK_WIDTH_2=y
+CONFIG_MTD_MAP_BANK_WIDTH_4=y
+# CONFIG_MTD_MAP_BANK_WIDTH_8 is not set
+# CONFIG_MTD_MAP_BANK_WIDTH_16 is not set
+# CONFIG_MTD_MAP_BANK_WIDTH_32 is not set
+CONFIG_MTD_CFI_I1=y
+CONFIG_MTD_CFI_I2=y
+# CONFIG_MTD_CFI_I4 is not set
+# CONFIG_MTD_CFI_I8 is not set
+# CONFIG_MTD_CFI_INTELEXT is not set
+# CONFIG_MTD_CFI_AMDSTD is not set
+# CONFIG_MTD_CFI_STAA is not set
+CONFIG_MTD_RAM=y
+CONFIG_MTD_ROM=m
+# CONFIG_MTD_ABSENT is not set
+
+#
+# Mapping drivers for chip access
+#
+CONFIG_MTD_COMPLEX_MAPPINGS=y
+# CONFIG_MTD_PHYSMAP is not set
+# CONFIG_MTD_GPIO_ADDR is not set
+# CONFIG_MTD_UCLINUX is not set
+# CONFIG_MTD_PLATRAM is not set
+
+#
+# Self-contained MTD device drivers
+#
+# CONFIG_MTD_SLRAM is not set
+# CONFIG_MTD_PHRAM is not set
+# CONFIG_MTD_MTDRAM is not set
+# CONFIG_MTD_BLOCK2MTD is not set
+
+#
+# Disk-On-Chip Device Drivers
+#
+# CONFIG_MTD_DOC2000 is not set
+# CONFIG_MTD_DOC2001 is not set
+# CONFIG_MTD_DOC2001PLUS is not set
+# CONFIG_MTD_NAND is not set
+# CONFIG_MTD_ONENAND is not set
+
+#
+# UBI - Unsorted block images
+#
+# CONFIG_MTD_UBI is not set
+# CONFIG_PARPORT is not set
+CONFIG_BLK_DEV=y
+# CONFIG_BLK_DEV_COW_COMMON is not set
+# CONFIG_BLK_DEV_LOOP is not set
+# CONFIG_BLK_DEV_NBD is not set
+CONFIG_BLK_DEV_RAM=y
+CONFIG_BLK_DEV_RAM_COUNT=16
+CONFIG_BLK_DEV_RAM_SIZE=4096
+# CONFIG_BLK_DEV_XIP is not set
+# CONFIG_CDROM_PKTCDVD is not set
+# CONFIG_ATA_OVER_ETH is not set
+# CONFIG_BLK_DEV_HD is not set
+CONFIG_MISC_DEVICES=y
+# CONFIG_EEPROM_93CX6 is not set
+# CONFIG_ENCLOSURE_SERVICES is not set
+CONFIG_HAVE_IDE=y
+# CONFIG_IDE is not set
+
+#
+# SCSI device support
+#
+# CONFIG_RAID_ATTRS is not set
+# CONFIG_SCSI is not set
+# CONFIG_SCSI_DMA is not set
+# CONFIG_SCSI_NETLINK is not set
+# CONFIG_ATA is not set
+# CONFIG_MD is not set
+CONFIG_NETDEVICES=y
+# CONFIG_DUMMY is not set
+# CONFIG_BONDING is not set
+# CONFIG_MACVLAN is not set
+# CONFIG_EQUALIZER is not set
+# CONFIG_TUN is not set
+# CONFIG_VETH is not set
+CONFIG_PHYLIB=y
+
+#
+# MII PHY device drivers
+#
+# CONFIG_MARVELL_PHY is not set
+# CONFIG_DAVICOM_PHY is not set
+# CONFIG_QSEMI_PHY is not set
+# CONFIG_LXT_PHY is not set
+# CONFIG_CICADA_PHY is not set
+# CONFIG_VITESSE_PHY is not set
+# CONFIG_SMSC_PHY is not set
+# CONFIG_BROADCOM_PHY is not set
+# CONFIG_ICPLUS_PHY is not set
+# CONFIG_REALTEK_PHY is not set
+# CONFIG_FIXED_PHY is not set
+# CONFIG_MDIO_BITBANG is not set
+CONFIG_NET_ETHERNET=y
+CONFIG_MII=y
+# CONFIG_BFIN_MAC is not set
+# CONFIG_SMC91X is not set
+# CONFIG_SMSC911X is not set
+# CONFIG_DM9000 is not set
+# CONFIG_IBM_NEW_EMAC_ZMII is not set
+# CONFIG_IBM_NEW_EMAC_RGMII is not set
+# CONFIG_IBM_NEW_EMAC_TAH is not set
+# CONFIG_IBM_NEW_EMAC_EMAC4 is not set
+# CONFIG_IBM_NEW_EMAC_NO_FLOW_CTRL is not set
+# CONFIG_IBM_NEW_EMAC_MAL_CLR_ICINTSTAT is not set
+# CONFIG_IBM_NEW_EMAC_MAL_COMMON_ERR is not set
+# CONFIG_NETDEV_1000 is not set
+# CONFIG_NETDEV_10000 is not set
+
+#
+# Wireless LAN
+#
+# CONFIG_WLAN_PRE80211 is not set
+# CONFIG_WLAN_80211 is not set
+# CONFIG_IWLWIFI_LEDS is not set
+# CONFIG_WAN is not set
+# CONFIG_PPP is not set
+# CONFIG_SLIP is not set
+# CONFIG_NETCONSOLE is not set
+# CONFIG_NETPOLL is not set
+# CONFIG_NET_POLL_CONTROLLER is not set
+# CONFIG_ISDN is not set
+# CONFIG_PHONE is not set
+
+#
+# Input device support
+#
+CONFIG_INPUT=y
+# CONFIG_INPUT_FF_MEMLESS is not set
+# CONFIG_INPUT_POLLDEV is not set
+
+#
+# Userland interfaces
+#
+# CONFIG_INPUT_MOUSEDEV is not set
+# CONFIG_INPUT_JOYDEV is not set
+# CONFIG_INPUT_EVDEV is not set
+# CONFIG_INPUT_EVBUG is not set
+
+#
+# Input Device Drivers
+#
+# CONFIG_INPUT_KEYBOARD is not set
+# CONFIG_INPUT_MOUSE is not set
+# CONFIG_INPUT_JOYSTICK is not set
+# CONFIG_INPUT_TABLET is not set
+# CONFIG_INPUT_TOUCHSCREEN is not set
+CONFIG_INPUT_MISC=y
+# CONFIG_INPUT_UINPUT is not set
+# CONFIG_CONFIG_INPUT_PCF8574 is not set
+
+#
+# Hardware I/O ports
+#
+# CONFIG_SERIO is not set
+# CONFIG_GAMEPORT is not set
+
+#
+# Character devices
+#
+# CONFIG_AD9960 is not set
+# CONFIG_SPI_ADC_BF533 is not set
+# CONFIG_BF5xx_PPIFCD is not set
+# CONFIG_BFIN_SIMPLE_TIMER is not set
+# CONFIG_BF5xx_PPI is not set
+# CONFIG_BFIN_SPORT is not set
+# CONFIG_BFIN_TIMER_LATENCY is not set
+# CONFIG_TWI_LCD is not set
+CONFIG_BFIN_DMA_INTERFACE=m
+CONFIG_SIMPLE_GPIO=m
+CONFIG_VT=y
+CONFIG_CONSOLE_TRANSLATIONS=y
+CONFIG_VT_CONSOLE=y
+CONFIG_HW_CONSOLE=y
+# CONFIG_VT_HW_CONSOLE_BINDING is not set
+# CONFIG_DEVKMEM is not set
+# CONFIG_BFIN_JTAG_COMM is not set
+# CONFIG_SERIAL_NONSTANDARD is not set
+
+#
+# Serial drivers
+#
+# CONFIG_SERIAL_8250 is not set
+
+#
+# Non-8250 serial port support
+#
+CONFIG_SERIAL_BFIN=y
+CONFIG_SERIAL_BFIN_CONSOLE=y
+CONFIG_SERIAL_BFIN_DMA=y
+# CONFIG_SERIAL_BFIN_PIO is not set
+CONFIG_SERIAL_BFIN_UART0=y
+# CONFIG_BFIN_UART0_CTSRTS is not set
+# CONFIG_SERIAL_BFIN_UART1 is not set
+CONFIG_SERIAL_CORE=y
+CONFIG_SERIAL_CORE_CONSOLE=y
+# CONFIG_SERIAL_BFIN_SPORT is not set
+CONFIG_UNIX98_PTYS=y
+# CONFIG_LEGACY_PTYS is not set
+
+#
+# CAN, the car bus and industrial fieldbus
+#
+# CONFIG_CAN4LINUX is not set
+# CONFIG_IPMI_HANDLER is not set
+# CONFIG_HW_RANDOM is not set
+# CONFIG_R3964 is not set
+# CONFIG_RAW_DRIVER is not set
+# CONFIG_TCG_TPM is not set
+CONFIG_I2C=y
+CONFIG_I2C_BOARDINFO=y
+CONFIG_I2C_CHARDEV=y
+CONFIG_I2C_HELPER_AUTO=y
+
+#
+# I2C Hardware Bus support
+#
+
+#
+# I2C system bus drivers (mostly embedded / system-on-chip)
+#
+CONFIG_I2C_BLACKFIN_TWI=y
+CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=100
+# CONFIG_I2C_GPIO is not set
+# CONFIG_I2C_OCORES is not set
+# CONFIG_I2C_SIMTEC is not set
+
+#
+# External I2C/SMBus adapter drivers
+#
+# CONFIG_I2C_PARPORT_LIGHT is not set
+# CONFIG_I2C_TAOS_EVM is not set
+
+#
+# Other I2C/SMBus bus drivers
+#
+# CONFIG_I2C_PCA_PLATFORM is not set
+# CONFIG_I2C_STUB is not set
+
+#
+# Miscellaneous I2C Chip support
+#
+# CONFIG_DS1682 is not set
+# CONFIG_AT24 is not set
+# CONFIG_SENSORS_AD5252 is not set
+# CONFIG_SENSORS_EEPROM is not set
+# CONFIG_SENSORS_PCF8574 is not set
+# CONFIG_PCF8575 is not set
+# CONFIG_SENSORS_PCA9539 is not set
+# CONFIG_SENSORS_PCF8591 is not set
+# CONFIG_SENSORS_MAX6875 is not set
+# CONFIG_SENSORS_TSL2550 is not set
+# CONFIG_I2C_DEBUG_CORE is not set
+# CONFIG_I2C_DEBUG_ALGO is not set
+# CONFIG_I2C_DEBUG_BUS is not set
+# CONFIG_I2C_DEBUG_CHIP is not set
+# CONFIG_SPI is not set
+CONFIG_ARCH_WANT_OPTIONAL_GPIOLIB=y
+# CONFIG_GPIOLIB is not set
+# CONFIG_W1 is not set
+# CONFIG_POWER_SUPPLY is not set
+# CONFIG_HWMON is not set
+# CONFIG_THERMAL is not set
+# CONFIG_THERMAL_HWMON is not set
+CONFIG_WATCHDOG=y
+# CONFIG_WATCHDOG_NOWAYOUT is not set
+
+#
+# Watchdog Device Drivers
+#
+# CONFIG_SOFT_WATCHDOG is not set
+CONFIG_BFIN_WDT=y
+
+#
+# Multifunction device drivers
+#
+# CONFIG_MFD_CORE is not set
+# CONFIG_MFD_SM501 is not set
+# CONFIG_HTC_PASIC3 is not set
+# CONFIG_MFD_TMIO is not set
+# CONFIG_MFD_WM8400 is not set
+# CONFIG_MFD_WM8350_I2C is not set
+
+#
+# Multimedia devices
+#
+
+#
+# Multimedia core support
+#
+# CONFIG_VIDEO_DEV is not set
+# CONFIG_DVB_CORE is not set
+# CONFIG_VIDEO_MEDIA is not set
+
+#
+# Multimedia drivers
+#
+# CONFIG_DAB is not set
+
+#
+# Graphics support
+#
+# CONFIG_VGASTATE is not set
+# CONFIG_VIDEO_OUTPUT_CONTROL is not set
+# CONFIG_FB is not set
+# CONFIG_BACKLIGHT_LCD_SUPPORT is not set
+
+#
+# Display device support
+#
+# CONFIG_DISPLAY_SUPPORT is not set
+
+#
+# Console display driver support
+#
+CONFIG_DUMMY_CONSOLE=y
+# CONFIG_SOUND is not set
+# CONFIG_HID_SUPPORT is not set
+# CONFIG_USB_SUPPORT is not set
+# CONFIG_MMC is not set
+# CONFIG_MEMSTICK is not set
+# CONFIG_NEW_LEDS is not set
+# CONFIG_ACCESSIBILITY is not set
+CONFIG_RTC_LIB=y
+CONFIG_RTC_CLASS=y
+CONFIG_RTC_HCTOSYS=y
+CONFIG_RTC_HCTOSYS_DEVICE="rtc0"
+# CONFIG_RTC_DEBUG is not set
+
+#
+# RTC interfaces
+#
+CONFIG_RTC_INTF_SYSFS=y
+CONFIG_RTC_INTF_PROC=y
+CONFIG_RTC_INTF_DEV=y
+# CONFIG_RTC_INTF_DEV_UIE_EMUL is not set
+# CONFIG_RTC_DRV_TEST is not set
+
+#
+# I2C RTC drivers
+#
+# CONFIG_RTC_DRV_DS1307 is not set
+# CONFIG_RTC_DRV_DS1374 is not set
+# CONFIG_RTC_DRV_DS1672 is not set
+# CONFIG_RTC_DRV_MAX6900 is not set
+# CONFIG_RTC_DRV_RS5C372 is not set
+# CONFIG_RTC_DRV_ISL1208 is not set
+# CONFIG_RTC_DRV_X1205 is not set
+# CONFIG_RTC_DRV_PCF8563 is not set
+# CONFIG_RTC_DRV_PCF8583 is not set
+# CONFIG_RTC_DRV_M41T80 is not set
+# CONFIG_RTC_DRV_S35390A is not set
+# CONFIG_RTC_DRV_FM3130 is not set
+
+#
+# SPI RTC drivers
+#
+
+#
+# Platform RTC drivers
+#
+# CONFIG_RTC_DRV_DS1286 is not set
+# CONFIG_RTC_DRV_DS1511 is not set
+# CONFIG_RTC_DRV_DS1553 is not set
+# CONFIG_RTC_DRV_DS1742 is not set
+# CONFIG_RTC_DRV_STK17TA8 is not set
+# CONFIG_RTC_DRV_M48T86 is not set
+# CONFIG_RTC_DRV_M48T35 is not set
+# CONFIG_RTC_DRV_M48T59 is not set
+# CONFIG_RTC_DRV_BQ4802 is not set
+# CONFIG_RTC_DRV_V3020 is not set
+
+#
+# on-CPU RTC drivers
+#
+CONFIG_RTC_DRV_BFIN=y
+# CONFIG_DMADEVICES is not set
+# CONFIG_UIO is not set
+# CONFIG_STAGING is not set
+
+#
+# File systems
+#
+# CONFIG_EXT2_FS is not set
+# CONFIG_EXT3_FS is not set
+# CONFIG_EXT4_FS is not set
+# CONFIG_REISERFS_FS is not set
+# CONFIG_JFS_FS is not set
+# CONFIG_FS_POSIX_ACL is not set
+CONFIG_FILE_LOCKING=y
+# CONFIG_XFS_FS is not set
+# CONFIG_OCFS2_FS is not set
+# CONFIG_DNOTIFY is not set
+CONFIG_INOTIFY=y
+CONFIG_INOTIFY_USER=y
+# CONFIG_QUOTA is not set
+# CONFIG_AUTOFS_FS is not set
+# CONFIG_AUTOFS4_FS is not set
+# CONFIG_FUSE_FS is not set
+
+#
+# CD-ROM/DVD Filesystems
+#
+# CONFIG_ISO9660_FS is not set
+# CONFIG_UDF_FS is not set
+
+#
+# DOS/FAT/NT Filesystems
+#
+# CONFIG_MSDOS_FS is not set
+# CONFIG_VFAT_FS is not set
+# CONFIG_NTFS_FS is not set
+
+#
+# Pseudo filesystems
+#
+CONFIG_PROC_FS=y
+CONFIG_PROC_SYSCTL=y
+CONFIG_SYSFS=y
+# CONFIG_TMPFS is not set
+# CONFIG_HUGETLB_PAGE is not set
+# CONFIG_CONFIGFS_FS is not set
+
+#
+# Miscellaneous filesystems
+#
+# CONFIG_ADFS_FS is not set
+# CONFIG_AFFS_FS is not set
+# CONFIG_HFS_FS is not set
+# CONFIG_HFSPLUS_FS is not set
+# CONFIG_BEFS_FS is not set
+# CONFIG_BFS_FS is not set
+# CONFIG_EFS_FS is not set
+# CONFIG_YAFFS_FS is not set
+# CONFIG_JFFS2_FS is not set
+# CONFIG_CRAMFS is not set
+# CONFIG_VXFS_FS is not set
+# CONFIG_MINIX_FS is not set
+# CONFIG_OMFS_FS is not set
+# CONFIG_HPFS_FS is not set
+# CONFIG_QNX4FS_FS is not set
+# CONFIG_ROMFS_FS is not set
+# CONFIG_SYSV_FS is not set
+# CONFIG_UFS_FS is not set
+CONFIG_NETWORK_FILESYSTEMS=y
+CONFIG_NFS_FS=m
+CONFIG_NFS_V3=y
+# CONFIG_NFS_V3_ACL is not set
+# CONFIG_NFS_V4 is not set
+# CONFIG_NFSD is not set
+CONFIG_LOCKD=m
+CONFIG_LOCKD_V4=y
+CONFIG_NFS_COMMON=y
+CONFIG_SUNRPC=m
+# CONFIG_SUNRPC_REGISTER_V4 is not set
+# CONFIG_RPCSEC_GSS_KRB5 is not set
+# CONFIG_RPCSEC_GSS_SPKM3 is not set
+CONFIG_SMB_FS=m
+# CONFIG_SMB_NLS_DEFAULT is not set
+# CONFIG_CIFS is not set
+# CONFIG_NCP_FS is not set
+# CONFIG_CODA_FS is not set
+# CONFIG_AFS_FS is not set
+
+#
+# Partition Types
+#
+# CONFIG_PARTITION_ADVANCED is not set
+CONFIG_MSDOS_PARTITION=y
+CONFIG_NLS=y
+CONFIG_NLS_DEFAULT="iso8859-1"
+CONFIG_NLS_CODEPAGE_437=y
+# CONFIG_NLS_CODEPAGE_737 is not set
+# CONFIG_NLS_CODEPAGE_775 is not set
+# CONFIG_NLS_CODEPAGE_850 is not set
+# CONFIG_NLS_CODEPAGE_852 is not set
+# CONFIG_NLS_CODEPAGE_855 is not set
+# CONFIG_NLS_CODEPAGE_857 is not set
+# CONFIG_NLS_CODEPAGE_860 is not set
+# CONFIG_NLS_CODEPAGE_861 is not set
+# CONFIG_NLS_CODEPAGE_862 is not set
+# CONFIG_NLS_CODEPAGE_863 is not set
+# CONFIG_NLS_CODEPAGE_864 is not set
+# CONFIG_NLS_CODEPAGE_865 is not set
+# CONFIG_NLS_CODEPAGE_866 is not set
+# CONFIG_NLS_CODEPAGE_869 is not set
+# CONFIG_NLS_CODEPAGE_936 is not set
+# CONFIG_NLS_CODEPAGE_950 is not set
+# CONFIG_NLS_CODEPAGE_932 is not set
+# CONFIG_NLS_CODEPAGE_949 is not set
+# CONFIG_NLS_CODEPAGE_874 is not set
+# CONFIG_NLS_ISO8859_8 is not set
+# CONFIG_NLS_CODEPAGE_1250 is not set
+# CONFIG_NLS_CODEPAGE_1251 is not set
+# CONFIG_NLS_ASCII is not set
+CONFIG_NLS_ISO8859_1=y
+# CONFIG_NLS_ISO8859_2 is not set
+# CONFIG_NLS_ISO8859_3 is not set
+# CONFIG_NLS_ISO8859_4 is not set
+# CONFIG_NLS_ISO8859_5 is not set
+# CONFIG_NLS_ISO8859_6 is not set
+# CONFIG_NLS_ISO8859_7 is not set
+# CONFIG_NLS_ISO8859_9 is not set
+# CONFIG_NLS_ISO8859_13 is not set
+# CONFIG_NLS_ISO8859_14 is not set
+# CONFIG_NLS_ISO8859_15 is not set
+# CONFIG_NLS_KOI8_R is not set
+# CONFIG_NLS_KOI8_U is not set
+# CONFIG_NLS_UTF8 is not set
+# CONFIG_DLM is not set
+
+#
+# Kernel hacking
+#
+# CONFIG_PRINTK_TIME is not set
+CONFIG_ENABLE_WARN_DEPRECATED=y
+CONFIG_ENABLE_MUST_CHECK=y
+CONFIG_FRAME_WARN=1024
+# CONFIG_MAGIC_SYSRQ is not set
+# CONFIG_UNUSED_SYMBOLS is not set
+CONFIG_DEBUG_FS=y
+# CONFIG_HEADERS_CHECK is not set
+CONFIG_DEBUG_KERNEL=y
+# CONFIG_DEBUG_SHIRQ is not set
+CONFIG_DETECT_SOFTLOCKUP=y
+# CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC is not set
+CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC_VALUE=0
+CONFIG_SCHED_DEBUG=y
+# CONFIG_SCHEDSTATS is not set
+# CONFIG_TIMER_STATS is not set
+# CONFIG_DEBUG_OBJECTS is not set
+# CONFIG_DEBUG_SLAB is not set
+# CONFIG_DEBUG_RT_MUTEXES is not set
+# CONFIG_RT_MUTEX_TESTER is not set
+# CONFIG_DEBUG_SPINLOCK is not set
+# CONFIG_DEBUG_MUTEXES is not set
+# CONFIG_DEBUG_SPINLOCK_SLEEP is not set
+# CONFIG_DEBUG_LOCKING_API_SELFTESTS is not set
+# CONFIG_DEBUG_KOBJECT is not set
+# CONFIG_DEBUG_BUGVERBOSE is not set
+CONFIG_DEBUG_INFO=y
+# CONFIG_DEBUG_VM is not set
+# CONFIG_DEBUG_WRITECOUNT is not set
+# CONFIG_DEBUG_MEMORY_INIT is not set
+# CONFIG_DEBUG_LIST is not set
+# CONFIG_DEBUG_SG is not set
+# CONFIG_FRAME_POINTER is not set
+# CONFIG_BOOT_PRINTK_DELAY is not set
+# CONFIG_RCU_TORTURE_TEST is not set
+# CONFIG_RCU_CPU_STALL_DETECTOR is not set
+# CONFIG_BACKTRACE_SELF_TEST is not set
+# CONFIG_DEBUG_BLOCK_EXT_DEVT is not set
+# CONFIG_FAULT_INJECTION is not set
+CONFIG_SYSCTL_SYSCALL_CHECK=y
+# CONFIG_DYNAMIC_PRINTK_DEBUG is not set
+# CONFIG_SAMPLES is not set
+CONFIG_HAVE_ARCH_KGDB=y
+# CONFIG_KGDB is not set
+# CONFIG_DEBUG_STACKOVERFLOW is not set
+# CONFIG_DEBUG_STACK_USAGE is not set
+CONFIG_DEBUG_VERBOSE=y
+CONFIG_DEBUG_MMRS=y
+# CONFIG_DEBUG_HWERR is not set
+# CONFIG_DEBUG_DOUBLEFAULT is not set
+CONFIG_DEBUG_HUNT_FOR_ZERO=y
+CONFIG_DEBUG_BFIN_HWTRACE_ON=y
+CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION_OFF=y
+# CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION_ONE is not set
+# CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION_TWO is not set
+CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION=0
+# CONFIG_DEBUG_BFIN_HWTRACE_EXPAND is not set
+# CONFIG_DEBUG_BFIN_NO_KERN_HWTRACE is not set
+CONFIG_EARLY_PRINTK=y
+CONFIG_CPLB_INFO=y
+CONFIG_ACCESS_CHECK=y
+
+#
+# Security options
+#
+# CONFIG_KEYS is not set
+CONFIG_SECURITY=y
+# CONFIG_SECURITYFS is not set
+# CONFIG_SECURITY_NETWORK is not set
+# CONFIG_SECURITY_FILE_CAPABILITIES is not set
+CONFIG_SECURITY_DEFAULT_MMAP_MIN_ADDR=0
+CONFIG_CRYPTO=y
+
+#
+# Crypto core or helper
+#
+# CONFIG_CRYPTO_FIPS is not set
+# CONFIG_CRYPTO_MANAGER is not set
+# CONFIG_CRYPTO_GF128MUL is not set
+# CONFIG_CRYPTO_NULL is not set
+# CONFIG_CRYPTO_CRYPTD is not set
+# CONFIG_CRYPTO_AUTHENC is not set
+# CONFIG_CRYPTO_TEST is not set
+
+#
+# Authenticated Encryption with Associated Data
+#
+# CONFIG_CRYPTO_CCM is not set
+# CONFIG_CRYPTO_GCM is not set
+# CONFIG_CRYPTO_SEQIV is not set
+
+#
+# Block modes
+#
+# CONFIG_CRYPTO_CBC is not set
+# CONFIG_CRYPTO_CTR is not set
+# CONFIG_CRYPTO_CTS is not set
+# CONFIG_CRYPTO_ECB is not set
+# CONFIG_CRYPTO_LRW is not set
+# CONFIG_CRYPTO_PCBC is not set
+# CONFIG_CRYPTO_XTS is not set
+
+#
+# Hash modes
+#
+# CONFIG_CRYPTO_HMAC is not set
+# CONFIG_CRYPTO_XCBC is not set
+
+#
+# Digest
+#
+# CONFIG_CRYPTO_CRC32C is not set
+# CONFIG_CRYPTO_MD4 is not set
+# CONFIG_CRYPTO_MD5 is not set
+# CONFIG_CRYPTO_MICHAEL_MIC is not set
+# CONFIG_CRYPTO_RMD128 is not set
+# CONFIG_CRYPTO_RMD160 is not set
+# CONFIG_CRYPTO_RMD256 is not set
+# CONFIG_CRYPTO_RMD320 is not set
+# CONFIG_CRYPTO_SHA1 is not set
+# CONFIG_CRYPTO_SHA256 is not set
+# CONFIG_CRYPTO_SHA512 is not set
+# CONFIG_CRYPTO_TGR192 is not set
+# CONFIG_CRYPTO_WP512 is not set
+
+#
+# Ciphers
+#
+# CONFIG_CRYPTO_AES is not set
+# CONFIG_CRYPTO_ANUBIS is not set
+# CONFIG_CRYPTO_ARC4 is not set
+# CONFIG_CRYPTO_BLOWFISH is not set
+# CONFIG_CRYPTO_CAMELLIA is not set
+# CONFIG_CRYPTO_CAST5 is not set
+# CONFIG_CRYPTO_CAST6 is not set
+# CONFIG_CRYPTO_DES is not set
+# CONFIG_CRYPTO_FCRYPT is not set
+# CONFIG_CRYPTO_KHAZAD is not set
+# CONFIG_CRYPTO_SALSA20 is not set
+# CONFIG_CRYPTO_SEED is not set
+# CONFIG_CRYPTO_SERPENT is not set
+# CONFIG_CRYPTO_TEA is not set
+# CONFIG_CRYPTO_TWOFISH is not set
+
+#
+# Compression
+#
+# CONFIG_CRYPTO_DEFLATE is not set
+# CONFIG_CRYPTO_LZO is not set
+
+#
+# Random Number Generation
+#
+# CONFIG_CRYPTO_ANSI_CPRNG is not set
+CONFIG_CRYPTO_HW=y
+
+#
+# Library routines
+#
+CONFIG_BITREVERSE=y
+CONFIG_CRC_CCITT=m
+# CONFIG_CRC16 is not set
+# CONFIG_CRC_T10DIF is not set
+# CONFIG_CRC_ITU_T is not set
+CONFIG_CRC32=y
+# CONFIG_CRC7 is not set
+# CONFIG_LIBCRC32C is not set
+CONFIG_ZLIB_INFLATE=y
+CONFIG_PLIST=y
+CONFIG_HAS_IOMEM=y
+CONFIG_HAS_IOPORT=y
+CONFIG_HAS_DMA=y
diff --git a/arch/blackfin/configs/BF526-EZBRD_defconfig b/arch/blackfin/configs/BF526-EZBRD_defconfig
index 4443a47e516f..69f66c35b2a5 100644
--- a/arch/blackfin/configs/BF526-EZBRD_defconfig
+++ b/arch/blackfin/configs/BF526-EZBRD_defconfig
@@ -1,7 +1,6 @@
 #
 # Automatically generated make config: don't edit
-# Linux kernel version: 2.6.26.3
-# Thu Aug 28 16:49:53 2008
+# Linux kernel version: 2.6.28-rc2
 #
 # CONFIG_MMU is not set
 # CONFIG_FPU is not set
@@ -37,8 +36,7 @@ CONFIG_IKCONFIG_PROC=y
 CONFIG_LOG_BUF_SHIFT=14
 # CONFIG_CGROUPS is not set
 # CONFIG_GROUP_SCHED is not set
-CONFIG_SYSFS_DEPRECATED=y
-CONFIG_SYSFS_DEPRECATED_V2=y
+# CONFIG_SYSFS_DEPRECATED_V2 is not set
 # CONFIG_RELAY is not set
 # CONFIG_NAMESPACES is not set
 CONFIG_BLK_DEV_INITRD=y
@@ -48,13 +46,13 @@ CONFIG_SYSCTL=y
 CONFIG_EMBEDDED=y
 CONFIG_UID16=y
 CONFIG_SYSCTL_SYSCALL=y
-CONFIG_SYSCTL_SYSCALL_CHECK=y
 CONFIG_KALLSYMS=y
+# CONFIG_KALLSYMS_ALL is not set
 # CONFIG_KALLSYMS_EXTRA_PASS is not set
 CONFIG_HOTPLUG=y
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
 CONFIG_COMPAT_BRK=y
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
@@ -63,6 +61,7 @@ CONFIG_EPOLL=y
 CONFIG_SIGNALFD=y
 CONFIG_TIMERFD=y
 CONFIG_EVENTFD=y
+CONFIG_AIO=y
 CONFIG_VM_EVENT_COUNTERS=y
 CONFIG_SLAB=y
 # CONFIG_SLUB is not set
@@ -70,9 +69,7 @@ CONFIG_SLAB=y
 # CONFIG_PROFILING is not set
 # CONFIG_MARKERS is not set
 CONFIG_HAVE_OPROFILE=y
-# CONFIG_HAVE_KPROBES is not set
-# CONFIG_HAVE_KRETPROBES is not set
-# CONFIG_HAVE_DMA_ATTRS is not set
+# CONFIG_HAVE_GENERIC_DMA_COHERENT is not set
 CONFIG_SLABINFO=y
 CONFIG_RT_MUTEXES=y
 CONFIG_TINY_SHMEM=y
@@ -89,6 +86,7 @@ CONFIG_BLOCK=y
 # CONFIG_BLK_DEV_IO_TRACE is not set
 # CONFIG_LSF is not set
 # CONFIG_BLK_DEV_BSG is not set
+# CONFIG_BLK_DEV_INTEGRITY is not set
 
 #
 # IO Schedulers
@@ -106,6 +104,7 @@ CONFIG_CLASSIC_RCU=y
 # CONFIG_PREEMPT_NONE is not set
 CONFIG_PREEMPT_VOLUNTARY=y
 # CONFIG_PREEMPT is not set
+# CONFIG_FREEZER is not set
 
 #
 # Blackfin Processor Options
@@ -114,6 +113,10 @@ CONFIG_PREEMPT_VOLUNTARY=y
 #
 # Processor and Board Settings
 #
+# CONFIG_BF512 is not set
+# CONFIG_BF514 is not set
+# CONFIG_BF516 is not set
+# CONFIG_BF518 is not set
 # CONFIG_BF522 is not set
 # CONFIG_BF523 is not set
 # CONFIG_BF524 is not set
@@ -126,49 +129,27 @@ CONFIG_BF526=y
 # CONFIG_BF534 is not set
 # CONFIG_BF536 is not set
 # CONFIG_BF537 is not set
+# CONFIG_BF538 is not set
+# CONFIG_BF539 is not set
 # CONFIG_BF542 is not set
 # CONFIG_BF544 is not set
 # CONFIG_BF547 is not set
 # CONFIG_BF548 is not set
 # CONFIG_BF549 is not set
 # CONFIG_BF561 is not set
+CONFIG_BF_REV_MIN=0
+CONFIG_BF_REV_MAX=2
 CONFIG_BF_REV_0_0=y
 # CONFIG_BF_REV_0_1 is not set
 # CONFIG_BF_REV_0_2 is not set
 # CONFIG_BF_REV_0_3 is not set
 # CONFIG_BF_REV_0_4 is not set
 # CONFIG_BF_REV_0_5 is not set
+# CONFIG_BF_REV_0_6 is not set
 # CONFIG_BF_REV_ANY is not set
 # CONFIG_BF_REV_NONE is not set
 CONFIG_BF52x=y
 CONFIG_MEM_MT48LC32M16A2TG_75=y
-# CONFIG_BFIN527_EZKIT is not set
-# CONFIG_BFIN527_BLUETECHNIX_CM is not set
-CONFIG_BFIN526_EZBRD=y
-
-#
-# BF527 Specific Configuration
-#
-
-#
-# Alternative Multiplexing Scheme
-#
-# CONFIG_BF527_SPORT0_PORTF is not set
-CONFIG_BF527_SPORT0_PORTG=y
-CONFIG_BF527_SPORT0_TSCLK_PG10=y
-# CONFIG_BF527_SPORT0_TSCLK_PG14 is not set
-CONFIG_BF527_UART1_PORTF=y
-# CONFIG_BF527_UART1_PORTG is not set
-# CONFIG_BF527_NAND_D_PORTF is not set
-CONFIG_BF527_NAND_D_PORTH=y
-
-#
-# Interrupt Priority Assignment
-#
-
-#
-# Priority
-#
 CONFIG_IRQ_PLL_WAKEUP=7
 CONFIG_IRQ_DMA0_ERROR=7
 CONFIG_IRQ_DMAR0_BLK=7
@@ -188,7 +169,6 @@ CONFIG_IRQ_SPORT0_TX=9
 CONFIG_IRQ_SPORT1_RX=9
 CONFIG_IRQ_SPORT1_TX=9
 CONFIG_IRQ_TWI=10
-CONFIG_IRQ_SPI=10
 CONFIG_IRQ_UART0_RX=10
 CONFIG_IRQ_UART0_TX=10
 CONFIG_IRQ_UART1_RX=10
@@ -199,14 +179,14 @@ CONFIG_IRQ_MAC_RX=11
 CONFIG_IRQ_PORTH_INTA=11
 CONFIG_IRQ_MAC_TX=11
 CONFIG_IRQ_PORTH_INTB=11
-CONFIG_IRQ_TMR0=12
-CONFIG_IRQ_TMR1=12
-CONFIG_IRQ_TMR2=12
-CONFIG_IRQ_TMR3=12
-CONFIG_IRQ_TMR4=12
-CONFIG_IRQ_TMR5=12
-CONFIG_IRQ_TMR6=12
-CONFIG_IRQ_TMR7=12
+CONFIG_IRQ_TIMER0=12
+CONFIG_IRQ_TIMER1=12
+CONFIG_IRQ_TIMER2=12
+CONFIG_IRQ_TIMER3=12
+CONFIG_IRQ_TIMER4=12
+CONFIG_IRQ_TIMER5=12
+CONFIG_IRQ_TIMER6=12
+CONFIG_IRQ_TIMER7=12
 CONFIG_IRQ_PORTG_INTA=12
 CONFIG_IRQ_PORTG_INTB=12
 CONFIG_IRQ_MEM_DMA0=13
@@ -214,6 +194,34 @@ CONFIG_IRQ_MEM_DMA1=13
 CONFIG_IRQ_WATCH=13
 CONFIG_IRQ_PORTF_INTA=13
 CONFIG_IRQ_PORTF_INTB=13
+# CONFIG_BFIN527_EZKIT is not set
+# CONFIG_BFIN527_BLUETECHNIX_CM is not set
+CONFIG_BFIN526_EZBRD=y
+
+#
+# BF527 Specific Configuration
+#
+
+#
+# Alternative Multiplexing Scheme
+#
+# CONFIG_BF527_SPORT0_PORTF is not set
+CONFIG_BF527_SPORT0_PORTG=y
+CONFIG_BF527_SPORT0_TSCLK_PG10=y
+# CONFIG_BF527_SPORT0_TSCLK_PG14 is not set
+CONFIG_BF527_UART1_PORTF=y
+# CONFIG_BF527_UART1_PORTG is not set
+# CONFIG_BF527_NAND_D_PORTF is not set
+CONFIG_BF527_NAND_D_PORTH=y
+
+#
+# Interrupt Priority Assignment
+#
+
+#
+# Priority
+#
+CONFIG_IRQ_SPI=10
 CONFIG_IRQ_SPI_ERROR=7
 CONFIG_IRQ_NFC_ERROR=7
 CONFIG_IRQ_HDMA_ERROR=7
@@ -235,7 +243,6 @@ CONFIG_BOOT_LOAD=0x1000
 #
 CONFIG_CLKIN_HZ=25000000
 # CONFIG_BFIN_KERNEL_CLOCK is not set
-CONFIG_MAX_MEM_SIZE=512
 CONFIG_MAX_VCO_HZ=400000000
 CONFIG_MIN_VCO_HZ=50000000
 CONFIG_MAX_SCLK_HZ=133333333
@@ -253,16 +260,11 @@ CONFIG_HZ=250
 CONFIG_GENERIC_TIME=y
 CONFIG_GENERIC_CLOCKEVENTS=y
 # CONFIG_CYCLES_CLOCKSOURCE is not set
-# CONFIG_TICK_ONESHOT is not set
 # CONFIG_NO_HZ is not set
 # CONFIG_HIGH_RES_TIMERS is not set
 CONFIG_GENERIC_CLOCKEVENTS_BUILD=y
 
 #
-# Memory Setup
-#
-
-#
 # Misc
 #
 CONFIG_BFIN_SCRATCH_REG_RETN=y
@@ -291,6 +293,7 @@ CONFIG_ACCESS_OK_L1=y
 CONFIG_CACHELINE_ALIGNED_L1=y
 # CONFIG_SYSCALL_TAB_L1 is not set
 # CONFIG_CPLB_SWITCH_TAB_L1 is not set
+CONFIG_APP_STACK_L1=y
 
 #
 # Speed Optimizations
@@ -304,15 +307,13 @@ CONFIG_FLATMEM_MANUAL=y
 # CONFIG_SPARSEMEM_MANUAL is not set
 CONFIG_FLATMEM=y
 CONFIG_FLAT_NODE_MEM_MAP=y
-# CONFIG_SPARSEMEM_STATIC is not set
-# CONFIG_SPARSEMEM_VMEMMAP_ENABLE is not set
 CONFIG_PAGEFLAGS_EXTENDED=y
 CONFIG_SPLIT_PTLOCK_CPUS=4
 # CONFIG_RESOURCES_64BIT is not set
+# CONFIG_PHYS_ADDR_T_64BIT is not set
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_VIRT_TO_BUS=y
 CONFIG_BFIN_GPTIMERS=y
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_4M is not set
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
@@ -365,6 +366,7 @@ CONFIG_BINFMT_ELF_FDPIC=y
 CONFIG_BINFMT_FLAT=y
 CONFIG_BINFMT_ZFLAT=y
 # CONFIG_BINFMT_SHARED_FLAT is not set
+# CONFIG_HAVE_AOUT is not set
 # CONFIG_BINFMT_MISC is not set
 
 #
@@ -378,10 +380,6 @@ CONFIG_ARCH_SUSPEND_POSSIBLE=y
 # CPU Frequency scaling
 #
 # CONFIG_CPU_FREQ is not set
-
-#
-# Networking
-#
 CONFIG_NET=y
 
 #
@@ -432,6 +430,7 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_TIPC is not set
 # CONFIG_ATM is not set
 # CONFIG_BRIDGE is not set
+# CONFIG_NET_DSA is not set
 # CONFIG_VLAN_8021Q is not set
 # CONFIG_DECNET is not set
 # CONFIG_LLC2 is not set
@@ -452,11 +451,10 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_IRDA is not set
 # CONFIG_BT is not set
 # CONFIG_AF_RXRPC is not set
-
-#
-# Wireless
-#
+# CONFIG_PHONET is not set
+CONFIG_WIRELESS=y
 # CONFIG_CFG80211 is not set
+CONFIG_WIRELESS_OLD_REGULATORY=y
 # CONFIG_WIRELESS_EXT is not set
 # CONFIG_MAC80211 is not set
 # CONFIG_IEEE80211 is not set
@@ -474,6 +472,8 @@ CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
 CONFIG_STANDALONE=y
 CONFIG_PREVENT_FIRMWARE_BUILD=y
 # CONFIG_FW_LOADER is not set
+# CONFIG_DEBUG_DRIVER is not set
+# CONFIG_DEBUG_DEVRES is not set
 # CONFIG_SYS_HYPERVISOR is not set
 # CONFIG_CONNECTOR is not set
 CONFIG_MTD=y
@@ -534,7 +534,8 @@ CONFIG_MTD_COMPLEX_MAPPINGS=y
 # Self-contained MTD device drivers
 #
 # CONFIG_MTD_DATAFLASH is not set
-# CONFIG_MTD_M25P80 is not set
+CONFIG_MTD_M25P80=y
+CONFIG_M25PXX_USE_FAST_READ=y
 # CONFIG_MTD_SLRAM is not set
 # CONFIG_MTD_PHRAM is not set
 # CONFIG_MTD_MTDRAM is not set
@@ -579,6 +580,7 @@ CONFIG_BLK_DEV_RAM_SIZE=4096
 # CONFIG_BLK_DEV_XIP is not set
 # CONFIG_CDROM_PKTCDVD is not set
 # CONFIG_ATA_OVER_ETH is not set
+# CONFIG_BLK_DEV_HD is not set
 CONFIG_MISC_DEVICES=y
 # CONFIG_EEPROM_93CX6 is not set
 # CONFIG_ENCLOSURE_SERVICES is not set
@@ -595,7 +597,6 @@ CONFIG_HAVE_IDE=y
 # CONFIG_ATA is not set
 # CONFIG_MD is not set
 CONFIG_NETDEVICES=y
-# CONFIG_NETDEVICES_MULTIQUEUE is not set
 # CONFIG_DUMMY is not set
 # CONFIG_BONDING is not set
 # CONFIG_MACVLAN is not set
@@ -633,9 +634,10 @@ CONFIG_BFIN_MAC_RMII=y
 # CONFIG_IBM_NEW_EMAC_RGMII is not set
 # CONFIG_IBM_NEW_EMAC_TAH is not set
 # CONFIG_IBM_NEW_EMAC_EMAC4 is not set
-# CONFIG_B44 is not set
+# CONFIG_IBM_NEW_EMAC_NO_FLOW_CTRL is not set
+# CONFIG_IBM_NEW_EMAC_MAL_CLR_ICINTSTAT is not set
+# CONFIG_IBM_NEW_EMAC_MAL_COMMON_ERR is not set
 CONFIG_NETDEV_1000=y
-# CONFIG_E1000E_ENABLED is not set
 # CONFIG_AX88180 is not set
 CONFIG_NETDEV_10000=y
 
@@ -667,7 +669,7 @@ CONFIG_NETDEV_10000=y
 # Input device support
 #
 CONFIG_INPUT=y
-# CONFIG_INPUT_FF_MEMLESS is not set
+CONFIG_INPUT_FF_MEMLESS=m
 # CONFIG_INPUT_POLLDEV is not set
 
 #
@@ -692,8 +694,9 @@ CONFIG_INPUT_MISC=y
 # CONFIG_INPUT_KEYSPAN_REMOTE is not set
 # CONFIG_INPUT_POWERMATE is not set
 # CONFIG_INPUT_YEALINK is not set
+# CONFIG_INPUT_CM109 is not set
 # CONFIG_INPUT_UINPUT is not set
-# CONFIG_TWI_KEYPAD is not set
+# CONFIG_CONFIG_INPUT_PCF8574 is not set
 
 #
 # Hardware I/O ports
@@ -712,12 +715,15 @@ CONFIG_INPUT_MISC=y
 # CONFIG_BFIN_SPORT is not set
 # CONFIG_BFIN_TIMER_LATENCY is not set
 # CONFIG_TWI_LCD is not set
+CONFIG_BFIN_DMA_INTERFACE=m
 CONFIG_SIMPLE_GPIO=m
 CONFIG_VT=y
+CONFIG_CONSOLE_TRANSLATIONS=y
 CONFIG_VT_CONSOLE=y
 CONFIG_HW_CONSOLE=y
 # CONFIG_VT_HW_CONSOLE_BINDING is not set
-CONFIG_DEVKMEM=y
+# CONFIG_DEVKMEM is not set
+# CONFIG_BFIN_JTAG_COMM is not set
 # CONFIG_SERIAL_NONSTANDARD is not set
 
 #
@@ -760,25 +766,39 @@ CONFIG_I2C_HELPER_AUTO=y
 #
 # I2C Hardware Bus support
 #
+
+#
+# I2C system bus drivers (mostly embedded / system-on-chip)
+#
 CONFIG_I2C_BLACKFIN_TWI=y
-CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=50
+CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=100
 # CONFIG_I2C_GPIO is not set
 # CONFIG_I2C_OCORES is not set
-# CONFIG_I2C_PARPORT_LIGHT is not set
 # CONFIG_I2C_SIMTEC is not set
+
+#
+# External I2C/SMBus adapter drivers
+#
+# CONFIG_I2C_PARPORT_LIGHT is not set
 # CONFIG_I2C_TAOS_EVM is not set
-# CONFIG_I2C_STUB is not set
 # CONFIG_I2C_TINY_USB is not set
+
+#
+# Other I2C/SMBus bus drivers
+#
 # CONFIG_I2C_PCA_PLATFORM is not set
+# CONFIG_I2C_STUB is not set
 
 #
 # Miscellaneous I2C Chip support
 #
 # CONFIG_DS1682 is not set
+# CONFIG_AT24 is not set
 # CONFIG_SENSORS_AD5252 is not set
 # CONFIG_SENSORS_EEPROM is not set
 # CONFIG_SENSORS_PCF8574 is not set
 # CONFIG_PCF8575 is not set
+# CONFIG_SENSORS_PCA9539 is not set
 # CONFIG_SENSORS_PCF8591 is not set
 # CONFIG_SENSORS_MAX6875 is not set
 # CONFIG_SENSORS_TSL2550 is not set
@@ -787,12 +807,14 @@ CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=50
 # CONFIG_I2C_DEBUG_BUS is not set
 # CONFIG_I2C_DEBUG_CHIP is not set
 CONFIG_SPI=y
+# CONFIG_SPI_DEBUG is not set
 CONFIG_SPI_MASTER=y
 
 #
 # SPI Master Controller Drivers
 #
 CONFIG_SPI_BFIN=y
+# CONFIG_SPI_BFIN_LOCK is not set
 # CONFIG_SPI_BITBANG is not set
 
 #
@@ -801,11 +823,15 @@ CONFIG_SPI_BFIN=y
 # CONFIG_SPI_AT25 is not set
 # CONFIG_SPI_SPIDEV is not set
 # CONFIG_SPI_TLE62X0 is not set
+CONFIG_ARCH_WANT_OPTIONAL_GPIOLIB=y
+# CONFIG_GPIOLIB is not set
 # CONFIG_W1 is not set
 # CONFIG_POWER_SUPPLY is not set
 CONFIG_HWMON=y
 # CONFIG_HWMON_VID is not set
+# CONFIG_SENSORS_AD7414 is not set
 # CONFIG_SENSORS_AD7418 is not set
+# CONFIG_SENSORS_ADCXX is not set
 # CONFIG_SENSORS_ADM1021 is not set
 # CONFIG_SENSORS_ADM1025 is not set
 # CONFIG_SENSORS_ADM1026 is not set
@@ -834,6 +860,7 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_LM90 is not set
 # CONFIG_SENSORS_LM92 is not set
 # CONFIG_SENSORS_LM93 is not set
+# CONFIG_SENSORS_MAX1111 is not set
 # CONFIG_SENSORS_MAX1619 is not set
 # CONFIG_SENSORS_MAX6650 is not set
 # CONFIG_SENSORS_PC87360 is not set
@@ -871,16 +898,14 @@ CONFIG_BFIN_WDT=y
 # CONFIG_USBPCWATCHDOG is not set
 
 #
-# Sonics Silicon Backplane
-#
-CONFIG_SSB_POSSIBLE=y
-# CONFIG_SSB is not set
-
-#
 # Multifunction device drivers
 #
+# CONFIG_MFD_CORE is not set
 # CONFIG_MFD_SM501 is not set
 # CONFIG_HTC_PASIC3 is not set
+# CONFIG_MFD_TMIO is not set
+# CONFIG_MFD_WM8400 is not set
+# CONFIG_MFD_WM8350_I2C is not set
 
 #
 # Multimedia devices
@@ -915,15 +940,8 @@ CONFIG_SSB_POSSIBLE=y
 # Console display driver support
 #
 CONFIG_DUMMY_CONSOLE=y
-
-#
-# Sound
-#
 CONFIG_SOUND=m
-
-#
-# Advanced Linux Sound Architecture
-#
+CONFIG_SOUND_OSS_CORE=y
 CONFIG_SND=m
 CONFIG_SND_TIMER=m
 CONFIG_SND_PCM=m
@@ -937,56 +955,40 @@ CONFIG_SND_SUPPORT_OLD_API=y
 CONFIG_SND_VERBOSE_PROCFS=y
 # CONFIG_SND_VERBOSE_PRINTK is not set
 # CONFIG_SND_DEBUG is not set
-
-#
-# Generic devices
-#
+CONFIG_SND_DRIVERS=y
 # CONFIG_SND_DUMMY is not set
 # CONFIG_SND_MTPAV is not set
 # CONFIG_SND_SERIAL_U16550 is not set
 # CONFIG_SND_MPU401 is not set
-
-#
-# SPI devices
-#
+CONFIG_SND_SPI=y
 
 #
 # ALSA Blackfin devices
 #
 # CONFIG_SND_BLACKFIN_AD1836 is not set
-# CONFIG_SND_BFIN_AD73311 is not set
 # CONFIG_SND_BFIN_AD73322 is not set
-
-#
-# USB devices
-#
+CONFIG_SND_USB=y
 # CONFIG_SND_USB_AUDIO is not set
 # CONFIG_SND_USB_CAIAQ is not set
-
-#
-# System on Chip audio support
-#
 CONFIG_SND_SOC=m
+CONFIG_SND_SOC_AC97_BUS=y
 CONFIG_SND_BF5XX_I2S=m
 CONFIG_SND_BF5XX_SOC_SSM2602=m
-# CONFIG_SND_BF5XX_AC97 is not set
+# CONFIG_SND_BF5XX_SOC_AD73311 is not set
+CONFIG_SND_BF5XX_AC97=m
+CONFIG_SND_BF5XX_MMAP_SUPPORT=y
+# CONFIG_SND_BF5XX_MULTICHAN_SUPPORT is not set
 CONFIG_SND_BF5XX_SOC_SPORT=m
 CONFIG_SND_BF5XX_SOC_I2S=m
+CONFIG_SND_BF5XX_SOC_AC97=m
+CONFIG_SND_BF5XX_SOC_AD1980=m
 CONFIG_SND_BF5XX_SPORT_NUM=0
-
-#
-# ALSA SoC audio for Freescale SOCs
-#
-
-#
-# SoC Audio for the Texas Instruments OMAP
-#
+# CONFIG_SND_BF5XX_HAVE_COLD_RESET is not set
+# CONFIG_SND_SOC_ALL_CODECS is not set
+CONFIG_SND_SOC_AD1980=m
 CONFIG_SND_SOC_SSM2602=m
-
-#
-# Open Sound System
-#
 # CONFIG_SOUND_PRIME is not set
+CONFIG_AC97_BUS=m
 CONFIG_HID_SUPPORT=y
 CONFIG_HID=y
 # CONFIG_HID_DEBUG is not set
@@ -996,9 +998,36 @@ CONFIG_HID=y
 # USB Input Devices
 #
 CONFIG_USB_HID=y
-# CONFIG_USB_HIDINPUT_POWERBOOK is not set
-# CONFIG_HID_FF is not set
+# CONFIG_HID_PID is not set
 # CONFIG_USB_HIDDEV is not set
+
+#
+# Special HID drivers
+#
+CONFIG_HID_COMPAT=y
+CONFIG_HID_A4TECH=y
+CONFIG_HID_APPLE=y
+CONFIG_HID_BELKIN=y
+CONFIG_HID_BRIGHT=y
+CONFIG_HID_CHERRY=y
+CONFIG_HID_CHICONY=y
+CONFIG_HID_CYPRESS=y
+CONFIG_HID_DELL=y
+CONFIG_HID_EZKEY=y
+CONFIG_HID_GYRATION=y
+CONFIG_HID_LOGITECH=y
+# CONFIG_LOGITECH_FF is not set
+# CONFIG_LOGIRUMBLEPAD2_FF is not set
+CONFIG_HID_MICROSOFT=y
+CONFIG_HID_MONTEREY=y
+CONFIG_HID_PANTHERLORD=y
+# CONFIG_PANTHERLORD_FF is not set
+CONFIG_HID_PETALYNX=y
+CONFIG_HID_SAMSUNG=y
+CONFIG_HID_SONY=y
+CONFIG_HID_SUNPLUS=y
+CONFIG_THRUSTMASTER_FF=m
+CONFIG_ZEROPLUS_FF=m
 CONFIG_USB_SUPPORT=y
 CONFIG_USB_ARCH_HAS_HCD=y
 # CONFIG_USB_ARCH_HAS_OHCI is not set
@@ -1016,6 +1045,9 @@ CONFIG_USB_DEVICE_CLASS=y
 # CONFIG_USB_OTG is not set
 # CONFIG_USB_OTG_WHITELIST is not set
 CONFIG_USB_OTG_BLACKLIST_HUB=y
+CONFIG_USB_MON=y
+# CONFIG_USB_WUSB is not set
+# CONFIG_USB_WUSB_CBAF is not set
 
 #
 # USB Host Controller Drivers
@@ -1026,6 +1058,7 @@ CONFIG_USB_OTG_BLACKLIST_HUB=y
 # CONFIG_USB_ISP1362_HCD is not set
 # CONFIG_USB_SL811_HCD is not set
 # CONFIG_USB_R8A66597_HCD is not set
+# CONFIG_USB_HWA_HCD is not set
 CONFIG_USB_MUSB_HDRC=y
 CONFIG_USB_MUSB_SOC=y
 
@@ -1037,7 +1070,7 @@ CONFIG_USB_MUSB_HOST=y
 # CONFIG_USB_MUSB_OTG is not set
 CONFIG_USB_MUSB_HDRC_HCD=y
 CONFIG_MUSB_PIO_ONLY=y
-CONFIG_USB_MUSB_LOGLEVEL=0
+# CONFIG_USB_MUSB_DEBUG is not set
 
 #
 # USB Device Class drivers
@@ -1045,6 +1078,7 @@ CONFIG_USB_MUSB_LOGLEVEL=0
 # CONFIG_USB_ACM is not set
 # CONFIG_USB_PRINTER is not set
 # CONFIG_USB_WDM is not set
+# CONFIG_USB_TMC is not set
 
 #
 # NOTE: USB_STORAGE enables SCSI, and 'SCSI disk support'
@@ -1059,7 +1093,6 @@ CONFIG_USB_MUSB_LOGLEVEL=0
 # USB Imaging devices
 #
 # CONFIG_USB_MDC800 is not set
-CONFIG_USB_MON=y
 
 #
 # USB port drivers
@@ -1072,7 +1105,7 @@ CONFIG_USB_MON=y
 # CONFIG_USB_EMI62 is not set
 # CONFIG_USB_EMI26 is not set
 # CONFIG_USB_ADUTUX is not set
-# CONFIG_USB_AUERSWALD is not set
+# CONFIG_USB_SEVSEG is not set
 # CONFIG_USB_RIO500 is not set
 # CONFIG_USB_LEGOTOWER is not set
 # CONFIG_USB_LCD is not set
@@ -1089,6 +1122,7 @@ CONFIG_USB_MON=y
 # CONFIG_USB_TRANCEVIBRATOR is not set
 # CONFIG_USB_IOWARRIOR is not set
 # CONFIG_USB_ISIGHTFW is not set
+# CONFIG_USB_VST is not set
 # CONFIG_USB_GADGET is not set
 # CONFIG_MMC is not set
 # CONFIG_MEMSTICK is not set
@@ -1128,36 +1162,45 @@ CONFIG_RTC_INTF_DEV=y
 #
 # SPI RTC drivers
 #
+# CONFIG_RTC_DRV_M41T94 is not set
+# CONFIG_RTC_DRV_DS1305 is not set
 # CONFIG_RTC_DRV_MAX6902 is not set
 # CONFIG_RTC_DRV_R9701 is not set
 # CONFIG_RTC_DRV_RS5C348 is not set
+# CONFIG_RTC_DRV_DS3234 is not set
 
 #
 # Platform RTC drivers
 #
+# CONFIG_RTC_DRV_DS1286 is not set
 # CONFIG_RTC_DRV_DS1511 is not set
 # CONFIG_RTC_DRV_DS1553 is not set
 # CONFIG_RTC_DRV_DS1742 is not set
 # CONFIG_RTC_DRV_STK17TA8 is not set
 # CONFIG_RTC_DRV_M48T86 is not set
+# CONFIG_RTC_DRV_M48T35 is not set
 # CONFIG_RTC_DRV_M48T59 is not set
+# CONFIG_RTC_DRV_BQ4802 is not set
 # CONFIG_RTC_DRV_V3020 is not set
 
 #
 # on-CPU RTC drivers
 #
 CONFIG_RTC_DRV_BFIN=y
+# CONFIG_DMADEVICES is not set
 # CONFIG_UIO is not set
+# CONFIG_STAGING is not set
 
 #
 # File systems
 #
 # CONFIG_EXT2_FS is not set
 # CONFIG_EXT3_FS is not set
-# CONFIG_EXT4DEV_FS is not set
+# CONFIG_EXT4_FS is not set
 # CONFIG_REISERFS_FS is not set
 # CONFIG_JFS_FS is not set
 # CONFIG_FS_POSIX_ACL is not set
+CONFIG_FILE_LOCKING=y
 # CONFIG_XFS_FS is not set
 # CONFIG_OCFS2_FS is not set
 # CONFIG_DNOTIFY is not set
@@ -1225,6 +1268,7 @@ CONFIG_JFFS2_RTIME=y
 # CONFIG_CRAMFS is not set
 # CONFIG_VXFS_FS is not set
 # CONFIG_MINIX_FS is not set
+# CONFIG_OMFS_FS is not set
 # CONFIG_HPFS_FS is not set
 # CONFIG_QNX4FS_FS is not set
 # CONFIG_ROMFS_FS is not set
@@ -1240,7 +1284,7 @@ CONFIG_LOCKD=m
 CONFIG_LOCKD_V4=y
 CONFIG_NFS_COMMON=y
 CONFIG_SUNRPC=m
-# CONFIG_SUNRPC_BIND34 is not set
+# CONFIG_SUNRPC_REGISTER_V4 is not set
 # CONFIG_RPCSEC_GSS_KRB5 is not set
 # CONFIG_RPCSEC_GSS_SPKM3 is not set
 CONFIG_SMB_FS=m
@@ -1308,10 +1352,48 @@ CONFIG_FRAME_WARN=1024
 # CONFIG_UNUSED_SYMBOLS is not set
 CONFIG_DEBUG_FS=y
 # CONFIG_HEADERS_CHECK is not set
-# CONFIG_DEBUG_KERNEL is not set
+CONFIG_DEBUG_KERNEL=y
+# CONFIG_DEBUG_SHIRQ is not set
+CONFIG_DETECT_SOFTLOCKUP=y
+# CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC is not set
+CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC_VALUE=0
+CONFIG_SCHED_DEBUG=y
+# CONFIG_SCHEDSTATS is not set
+# CONFIG_TIMER_STATS is not set
+# CONFIG_DEBUG_OBJECTS is not set
+# CONFIG_DEBUG_SLAB is not set
+# CONFIG_DEBUG_RT_MUTEXES is not set
+# CONFIG_RT_MUTEX_TESTER is not set
+# CONFIG_DEBUG_SPINLOCK is not set
+# CONFIG_DEBUG_MUTEXES is not set
+# CONFIG_DEBUG_SPINLOCK_SLEEP is not set
+# CONFIG_DEBUG_LOCKING_API_SELFTESTS is not set
+# CONFIG_DEBUG_KOBJECT is not set
 # CONFIG_DEBUG_BUGVERBOSE is not set
+CONFIG_DEBUG_INFO=y
+# CONFIG_DEBUG_VM is not set
+# CONFIG_DEBUG_WRITECOUNT is not set
+# CONFIG_DEBUG_MEMORY_INIT is not set
+# CONFIG_DEBUG_LIST is not set
+# CONFIG_DEBUG_SG is not set
+# CONFIG_FRAME_POINTER is not set
+# CONFIG_BOOT_PRINTK_DELAY is not set
+# CONFIG_RCU_TORTURE_TEST is not set
+# CONFIG_RCU_CPU_STALL_DETECTOR is not set
+# CONFIG_BACKTRACE_SELF_TEST is not set
+# CONFIG_DEBUG_BLOCK_EXT_DEVT is not set
+# CONFIG_FAULT_INJECTION is not set
+CONFIG_SYSCTL_SYSCALL_CHECK=y
+# CONFIG_DYNAMIC_PRINTK_DEBUG is not set
 # CONFIG_SAMPLES is not set
+CONFIG_HAVE_ARCH_KGDB=y
+# CONFIG_KGDB is not set
+# CONFIG_DEBUG_STACKOVERFLOW is not set
+# CONFIG_DEBUG_STACK_USAGE is not set
+CONFIG_DEBUG_VERBOSE=y
 CONFIG_DEBUG_MMRS=y
+# CONFIG_DEBUG_HWERR is not set
+# CONFIG_DEBUG_DOUBLEFAULT is not set
 CONFIG_DEBUG_HUNT_FOR_ZERO=y
 CONFIG_DEBUG_BFIN_HWTRACE_ON=y
 CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION_OFF=y
@@ -1329,8 +1411,9 @@ CONFIG_ACCESS_CHECK=y
 #
 # CONFIG_KEYS is not set
 CONFIG_SECURITY=y
+# CONFIG_SECURITYFS is not set
 # CONFIG_SECURITY_NETWORK is not set
-# CONFIG_SECURITY_CAPABILITIES is not set
+# CONFIG_SECURITY_FILE_CAPABILITIES is not set
 # CONFIG_SECURITY_ROOTPLUG is not set
 CONFIG_SECURITY_DEFAULT_MMAP_MIN_ADDR=0
 CONFIG_CRYPTO=y
@@ -1338,6 +1421,7 @@ CONFIG_CRYPTO=y
 #
 # Crypto core or helper
 #
+# CONFIG_CRYPTO_FIPS is not set
 # CONFIG_CRYPTO_MANAGER is not set
 # CONFIG_CRYPTO_GF128MUL is not set
 # CONFIG_CRYPTO_NULL is not set
@@ -1376,6 +1460,10 @@ CONFIG_CRYPTO=y
 # CONFIG_CRYPTO_MD4 is not set
 # CONFIG_CRYPTO_MD5 is not set
 # CONFIG_CRYPTO_MICHAEL_MIC is not set
+# CONFIG_CRYPTO_RMD128 is not set
+# CONFIG_CRYPTO_RMD160 is not set
+# CONFIG_CRYPTO_RMD256 is not set
+# CONFIG_CRYPTO_RMD320 is not set
 # CONFIG_CRYPTO_SHA1 is not set
 # CONFIG_CRYPTO_SHA256 is not set
 # CONFIG_CRYPTO_SHA512 is not set
@@ -1406,15 +1494,20 @@ CONFIG_CRYPTO=y
 #
 # CONFIG_CRYPTO_DEFLATE is not set
 # CONFIG_CRYPTO_LZO is not set
+
+#
+# Random Number Generation
+#
+# CONFIG_CRYPTO_ANSI_CPRNG is not set
 CONFIG_CRYPTO_HW=y
 
 #
 # Library routines
 #
 CONFIG_BITREVERSE=y
-# CONFIG_GENERIC_FIND_FIRST_BIT is not set
 CONFIG_CRC_CCITT=m
 # CONFIG_CRC16 is not set
+# CONFIG_CRC_T10DIF is not set
 # CONFIG_CRC_ITU_T is not set
 CONFIG_CRC32=y
 # CONFIG_CRC7 is not set
diff --git a/arch/blackfin/configs/BF527-EZKIT_defconfig b/arch/blackfin/configs/BF527-EZKIT_defconfig
index 4a2a660a6b35..f92668af00b0 100644
--- a/arch/blackfin/configs/BF527-EZKIT_defconfig
+++ b/arch/blackfin/configs/BF527-EZKIT_defconfig
@@ -1,6 +1,6 @@
 #
 # Automatically generated make config: don't edit
-# Linux kernel version: 2.6.24.7
+# Linux kernel version: 2.6.28-rc2
 #
 # CONFIG_MMU is not set
 # CONFIG_FPU is not set
@@ -8,7 +8,6 @@ CONFIG_RWSEM_GENERIC_SPINLOCK=y
 # CONFIG_RWSEM_XCHGADD_ALGORITHM is not set
 CONFIG_BLACKFIN=y
 CONFIG_ZONE_DMA=y
-CONFIG_SEMAPHORE_SLEEPERS=y
 CONFIG_GENERIC_FIND_NEXT_BIT=y
 CONFIG_GENERIC_HWEIGHT=y
 CONFIG_GENERIC_HARDIRQS=y
@@ -31,18 +30,16 @@ CONFIG_SYSVIPC_SYSCTL=y
 # CONFIG_POSIX_MQUEUE is not set
 # CONFIG_BSD_PROCESS_ACCT is not set
 # CONFIG_TASKSTATS is not set
-# CONFIG_USER_NS is not set
-# CONFIG_PID_NS is not set
 # CONFIG_AUDIT is not set
 CONFIG_IKCONFIG=y
 CONFIG_IKCONFIG_PROC=y
 CONFIG_LOG_BUF_SHIFT=14
 # CONFIG_CGROUPS is not set
-CONFIG_FAIR_GROUP_SCHED=y
-CONFIG_FAIR_USER_SCHED=y
-# CONFIG_FAIR_CGROUP_SCHED is not set
-CONFIG_SYSFS_DEPRECATED=y
+# CONFIG_GROUP_SCHED is not set
+# CONFIG_SYSFS_DEPRECATED is not set
+# CONFIG_SYSFS_DEPRECATED_V2 is not set
 # CONFIG_RELAY is not set
+# CONFIG_NAMESPACES is not set
 CONFIG_BLK_DEV_INITRD=y
 CONFIG_INITRAMFS_SOURCE=""
 # CONFIG_CC_OPTIMIZE_FOR_SIZE is not set
@@ -51,26 +48,35 @@ CONFIG_EMBEDDED=y
 CONFIG_UID16=y
 CONFIG_SYSCTL_SYSCALL=y
 CONFIG_KALLSYMS=y
+# CONFIG_KALLSYMS_ALL is not set
 # CONFIG_KALLSYMS_EXTRA_PASS is not set
 CONFIG_HOTPLUG=y
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
+CONFIG_COMPAT_BRK=y
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
 CONFIG_EPOLL=y
 CONFIG_SIGNALFD=y
+CONFIG_TIMERFD=y
 CONFIG_EVENTFD=y
+CONFIG_AIO=y
 CONFIG_VM_EVENT_COUNTERS=y
 CONFIG_SLAB=y
 # CONFIG_SLUB is not set
 # CONFIG_SLOB is not set
+# CONFIG_PROFILING is not set
+# CONFIG_MARKERS is not set
+CONFIG_HAVE_OPROFILE=y
+# CONFIG_HAVE_GENERIC_DMA_COHERENT is not set
 CONFIG_SLABINFO=y
 CONFIG_RT_MUTEXES=y
 CONFIG_TINY_SHMEM=y
 CONFIG_BASE_SMALL=0
 CONFIG_MODULES=y
+# CONFIG_MODULE_FORCE_LOAD is not set
 CONFIG_MODULE_UNLOAD=y
 # CONFIG_MODULE_FORCE_UNLOAD is not set
 # CONFIG_MODVERSIONS is not set
@@ -81,6 +87,7 @@ CONFIG_BLOCK=y
 # CONFIG_BLK_DEV_IO_TRACE is not set
 # CONFIG_LSF is not set
 # CONFIG_BLK_DEV_BSG is not set
+# CONFIG_BLK_DEV_INTEGRITY is not set
 
 #
 # IO Schedulers
@@ -94,9 +101,11 @@ CONFIG_DEFAULT_AS=y
 # CONFIG_DEFAULT_CFQ is not set
 # CONFIG_DEFAULT_NOOP is not set
 CONFIG_DEFAULT_IOSCHED="anticipatory"
+CONFIG_CLASSIC_RCU=y
 # CONFIG_PREEMPT_NONE is not set
 CONFIG_PREEMPT_VOLUNTARY=y
 # CONFIG_PREEMPT is not set
+# CONFIG_FREEZER is not set
 
 #
 # Blackfin Processor Options
@@ -105,6 +114,10 @@ CONFIG_PREEMPT_VOLUNTARY=y
 #
 # Processor and Board Settings
 #
+# CONFIG_BF512 is not set
+# CONFIG_BF514 is not set
+# CONFIG_BF516 is not set
+# CONFIG_BF518 is not set
 # CONFIG_BF522 is not set
 # CONFIG_BF523 is not set
 # CONFIG_BF524 is not set
@@ -117,47 +130,27 @@ CONFIG_BF527=y
 # CONFIG_BF534 is not set
 # CONFIG_BF536 is not set
 # CONFIG_BF537 is not set
+# CONFIG_BF538 is not set
+# CONFIG_BF539 is not set
 # CONFIG_BF542 is not set
 # CONFIG_BF544 is not set
 # CONFIG_BF547 is not set
 # CONFIG_BF548 is not set
 # CONFIG_BF549 is not set
 # CONFIG_BF561 is not set
+CONFIG_BF_REV_MIN=0
+CONFIG_BF_REV_MAX=2
 CONFIG_BF_REV_0_0=y
 # CONFIG_BF_REV_0_1 is not set
 # CONFIG_BF_REV_0_2 is not set
 # CONFIG_BF_REV_0_3 is not set
 # CONFIG_BF_REV_0_4 is not set
 # CONFIG_BF_REV_0_5 is not set
+# CONFIG_BF_REV_0_6 is not set
 # CONFIG_BF_REV_ANY is not set
 # CONFIG_BF_REV_NONE is not set
 CONFIG_BF52x=y
 CONFIG_MEM_MT48LC32M16A2TG_75=y
-CONFIG_BFIN527_EZKIT=y
-
-#
-# BF527 Specific Configuration
-#
-
-#
-# Alternative Multiplexing Scheme
-#
-# CONFIG_BF527_SPORT0_PORTF is not set
-CONFIG_BF527_SPORT0_PORTG=y
-CONFIG_BF527_SPORT0_TSCLK_PG10=y
-# CONFIG_BF527_SPORT0_TSCLK_PG14 is not set
-CONFIG_BF527_UART1_PORTF=y
-# CONFIG_BF527_UART1_PORTG is not set
-# CONFIG_BF527_NAND_D_PORTF is not set
-CONFIG_BF527_NAND_D_PORTH=y
-
-#
-# Interrupt Priority Assignment
-#
-
-#
-# Priority
-#
 CONFIG_IRQ_PLL_WAKEUP=7
 CONFIG_IRQ_DMA0_ERROR=7
 CONFIG_IRQ_DMAR0_BLK=7
@@ -177,7 +170,6 @@ CONFIG_IRQ_SPORT0_TX=9
 CONFIG_IRQ_SPORT1_RX=9
 CONFIG_IRQ_SPORT1_TX=9
 CONFIG_IRQ_TWI=10
-CONFIG_IRQ_SPI=10
 CONFIG_IRQ_UART0_RX=10
 CONFIG_IRQ_UART0_TX=10
 CONFIG_IRQ_UART1_RX=10
@@ -188,14 +180,14 @@ CONFIG_IRQ_MAC_RX=11
 CONFIG_IRQ_PORTH_INTA=11
 CONFIG_IRQ_MAC_TX=11
 CONFIG_IRQ_PORTH_INTB=11
-CONFIG_IRQ_TMR0=12
-CONFIG_IRQ_TMR1=12
-CONFIG_IRQ_TMR2=12
-CONFIG_IRQ_TMR3=12
-CONFIG_IRQ_TMR4=12
-CONFIG_IRQ_TMR5=12
-CONFIG_IRQ_TMR6=12
-CONFIG_IRQ_TMR7=12
+CONFIG_IRQ_TIMER0=8
+CONFIG_IRQ_TIMER1=12
+CONFIG_IRQ_TIMER2=12
+CONFIG_IRQ_TIMER3=12
+CONFIG_IRQ_TIMER4=12
+CONFIG_IRQ_TIMER5=12
+CONFIG_IRQ_TIMER6=12
+CONFIG_IRQ_TIMER7=12
 CONFIG_IRQ_PORTG_INTA=12
 CONFIG_IRQ_PORTG_INTB=12
 CONFIG_IRQ_MEM_DMA0=13
@@ -203,6 +195,34 @@ CONFIG_IRQ_MEM_DMA1=13
 CONFIG_IRQ_WATCH=13
 CONFIG_IRQ_PORTF_INTA=13
 CONFIG_IRQ_PORTF_INTB=13
+CONFIG_BFIN527_EZKIT=y
+# CONFIG_BFIN527_BLUETECHNIX_CM is not set
+# CONFIG_BFIN526_EZBRD is not set
+
+#
+# BF527 Specific Configuration
+#
+
+#
+# Alternative Multiplexing Scheme
+#
+# CONFIG_BF527_SPORT0_PORTF is not set
+CONFIG_BF527_SPORT0_PORTG=y
+CONFIG_BF527_SPORT0_TSCLK_PG10=y
+# CONFIG_BF527_SPORT0_TSCLK_PG14 is not set
+CONFIG_BF527_UART1_PORTF=y
+# CONFIG_BF527_UART1_PORTG is not set
+# CONFIG_BF527_NAND_D_PORTF is not set
+CONFIG_BF527_NAND_D_PORTH=y
+
+#
+# Interrupt Priority Assignment
+#
+
+#
+# Priority
+#
+CONFIG_IRQ_SPI=10
 CONFIG_IRQ_SPI_ERROR=7
 CONFIG_IRQ_NFC_ERROR=7
 CONFIG_IRQ_HDMA_ERROR=7
@@ -224,7 +244,6 @@ CONFIG_BOOT_LOAD=0x1000
 #
 CONFIG_CLKIN_HZ=25000000
 # CONFIG_BFIN_KERNEL_CLOCK is not set
-CONFIG_MAX_MEM_SIZE=512
 CONFIG_MAX_VCO_HZ=600000000
 CONFIG_MIN_VCO_HZ=50000000
 CONFIG_MAX_SCLK_HZ=133333333
@@ -238,10 +257,10 @@ CONFIG_HZ_250=y
 # CONFIG_HZ_300 is not set
 # CONFIG_HZ_1000 is not set
 CONFIG_HZ=250
+# CONFIG_SCHED_HRTICK is not set
 CONFIG_GENERIC_TIME=y
 CONFIG_GENERIC_CLOCKEVENTS=y
 # CONFIG_CYCLES_CLOCKSOURCE is not set
-# CONFIG_TICK_ONESHOT is not set
 # CONFIG_NO_HZ is not set
 # CONFIG_HIGH_RES_TIMERS is not set
 CONFIG_GENERIC_CLOCKEVENTS_BUILD=y
@@ -275,6 +294,12 @@ CONFIG_ACCESS_OK_L1=y
 CONFIG_CACHELINE_ALIGNED_L1=y
 # CONFIG_SYSCALL_TAB_L1 is not set
 # CONFIG_CPLB_SWITCH_TAB_L1 is not set
+CONFIG_APP_STACK_L1=y
+
+#
+# Speed Optimizations
+#
+CONFIG_BFIN_INS_LOWOVERHEAD=y
 CONFIG_RAMKERNEL=y
 # CONFIG_ROMKERNEL is not set
 CONFIG_SELECT_MEMORY_MODEL=y
@@ -283,14 +308,13 @@ CONFIG_FLATMEM_MANUAL=y
 # CONFIG_SPARSEMEM_MANUAL is not set
 CONFIG_FLATMEM=y
 CONFIG_FLAT_NODE_MEM_MAP=y
-# CONFIG_SPARSEMEM_STATIC is not set
-# CONFIG_SPARSEMEM_VMEMMAP_ENABLE is not set
+CONFIG_PAGEFLAGS_EXTENDED=y
 CONFIG_SPLIT_PTLOCK_CPUS=4
 # CONFIG_RESOURCES_64BIT is not set
+# CONFIG_PHYS_ADDR_T_64BIT is not set
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_VIRT_TO_BUS=y
 CONFIG_BFIN_GPTIMERS=y
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_4M is not set
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
@@ -305,7 +329,6 @@ CONFIG_BFIN_DCACHE=y
 # CONFIG_BFIN_ICACHE_LOCK is not set
 # CONFIG_BFIN_WB is not set
 CONFIG_BFIN_WT=y
-CONFIG_L1_MAX_PIECE=16
 # CONFIG_MPU is not set
 
 #
@@ -334,7 +357,6 @@ CONFIG_BANK_3=0xFFC0
 #
 # Bus options (PCI, PCMCIA, EISA, MCA, ISA)
 #
-# CONFIG_PCI is not set
 # CONFIG_ARCH_SUPPORTS_MSI is not set
 # CONFIG_PCCARD is not set
 
@@ -345,23 +367,20 @@ CONFIG_BINFMT_ELF_FDPIC=y
 CONFIG_BINFMT_FLAT=y
 CONFIG_BINFMT_ZFLAT=y
 # CONFIG_BINFMT_SHARED_FLAT is not set
+# CONFIG_HAVE_AOUT is not set
 # CONFIG_BINFMT_MISC is not set
 
 #
 # Power management options
 #
 # CONFIG_PM is not set
-CONFIG_SUSPEND_UP_POSSIBLE=y
+CONFIG_ARCH_SUSPEND_POSSIBLE=y
 # CONFIG_PM_WAKEUP_BY_GPIO is not set
 
 #
 # CPU Frequency scaling
 #
 # CONFIG_CPU_FREQ is not set
-
-#
-# Networking
-#
 CONFIG_NET=y
 
 #
@@ -374,6 +393,7 @@ CONFIG_XFRM=y
 # CONFIG_XFRM_USER is not set
 # CONFIG_XFRM_SUB_POLICY is not set
 # CONFIG_XFRM_MIGRATE is not set
+# CONFIG_XFRM_STATISTICS is not set
 # CONFIG_NET_KEY is not set
 CONFIG_INET=y
 # CONFIG_IP_MULTICAST is not set
@@ -403,8 +423,6 @@ CONFIG_TCP_CONG_CUBIC=y
 CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_TCP_MD5SIG is not set
 # CONFIG_IPV6 is not set
-# CONFIG_INET6_XFRM_TUNNEL is not set
-# CONFIG_INET6_TUNNEL is not set
 # CONFIG_NETLABEL is not set
 # CONFIG_NETWORK_SECMARK is not set
 # CONFIG_NETFILTER is not set
@@ -413,6 +431,7 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_TIPC is not set
 # CONFIG_ATM is not set
 # CONFIG_BRIDGE is not set
+# CONFIG_NET_DSA is not set
 # CONFIG_VLAN_8021Q is not set
 # CONFIG_DECNET is not set
 # CONFIG_LLC2 is not set
@@ -429,6 +448,7 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 #
 # CONFIG_NET_PKTGEN is not set
 # CONFIG_HAMRADIO is not set
+# CONFIG_CAN is not set
 CONFIG_IRDA=m
 
 #
@@ -467,15 +487,6 @@ CONFIG_SIR_BFIN_DMA=y
 # CONFIG_KS959_DONGLE is not set
 
 #
-# Old SIR device drivers
-#
-# CONFIG_IRPORT_SIR is not set
-
-#
-# Old Serial dongle support
-#
-
-#
 # FIR device drivers
 #
 # CONFIG_USB_IRDA is not set
@@ -483,11 +494,10 @@ CONFIG_SIR_BFIN_DMA=y
 # CONFIG_MCS_FIR is not set
 # CONFIG_BT is not set
 # CONFIG_AF_RXRPC is not set
-
-#
-# Wireless
-#
+# CONFIG_PHONET is not set
+CONFIG_WIRELESS=y
 # CONFIG_CFG80211 is not set
+CONFIG_WIRELESS_OLD_REGULATORY=y
 # CONFIG_WIRELESS_EXT is not set
 # CONFIG_MAC80211 is not set
 # CONFIG_IEEE80211 is not set
@@ -505,6 +515,8 @@ CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
 CONFIG_STANDALONE=y
 CONFIG_PREVENT_FIRMWARE_BUILD=y
 # CONFIG_FW_LOADER is not set
+# CONFIG_DEBUG_DRIVER is not set
+# CONFIG_DEBUG_DEVRES is not set
 # CONFIG_SYS_HYPERVISOR is not set
 # CONFIG_CONNECTOR is not set
 CONFIG_MTD=y
@@ -513,6 +525,7 @@ CONFIG_MTD=y
 CONFIG_MTD_PARTITIONS=y
 # CONFIG_MTD_REDBOOT_PARTS is not set
 # CONFIG_MTD_CMDLINE_PARTS is not set
+# CONFIG_MTD_AR7_PARTS is not set
 
 #
 # User Modules And Translation Layers
@@ -556,6 +569,7 @@ CONFIG_MTD_ROM=m
 #
 CONFIG_MTD_COMPLEX_MAPPINGS=y
 # CONFIG_MTD_PHYSMAP is not set
+# CONFIG_MTD_GPIO_ADDR is not set
 # CONFIG_MTD_UCLINUX is not set
 # CONFIG_MTD_PLATRAM is not set
 
@@ -563,7 +577,8 @@ CONFIG_MTD_COMPLEX_MAPPINGS=y
 # Self-contained MTD device drivers
 #
 # CONFIG_MTD_DATAFLASH is not set
-# CONFIG_MTD_M25P80 is not set
+CONFIG_MTD_M25P80=y
+CONFIG_M25PXX_USE_FAST_READ=y
 # CONFIG_MTD_SLRAM is not set
 # CONFIG_MTD_PHRAM is not set
 # CONFIG_MTD_MTDRAM is not set
@@ -605,11 +620,14 @@ CONFIG_BLK_DEV=y
 CONFIG_BLK_DEV_RAM=y
 CONFIG_BLK_DEV_RAM_COUNT=16
 CONFIG_BLK_DEV_RAM_SIZE=4096
-CONFIG_BLK_DEV_RAM_BLOCKSIZE=1024
+# CONFIG_BLK_DEV_XIP is not set
 # CONFIG_CDROM_PKTCDVD is not set
 # CONFIG_ATA_OVER_ETH is not set
+# CONFIG_BLK_DEV_HD is not set
 CONFIG_MISC_DEVICES=y
 # CONFIG_EEPROM_93CX6 is not set
+# CONFIG_ENCLOSURE_SERVICES is not set
+CONFIG_HAVE_IDE=y
 # CONFIG_IDE is not set
 
 #
@@ -622,7 +640,6 @@ CONFIG_MISC_DEVICES=y
 # CONFIG_ATA is not set
 # CONFIG_MD is not set
 CONFIG_NETDEVICES=y
-# CONFIG_NETDEVICES_MULTIQUEUE is not set
 # CONFIG_DUMMY is not set
 # CONFIG_BONDING is not set
 # CONFIG_MACVLAN is not set
@@ -643,6 +660,7 @@ CONFIG_PHYLIB=y
 # CONFIG_SMSC_PHY is not set
 # CONFIG_BROADCOM_PHY is not set
 # CONFIG_ICPLUS_PHY is not set
+# CONFIG_REALTEK_PHY is not set
 # CONFIG_FIXED_PHY is not set
 # CONFIG_MDIO_BITBANG is not set
 CONFIG_NET_ETHERNET=y
@@ -655,11 +673,14 @@ CONFIG_BFIN_MAC_RMII=y
 # CONFIG_SMC91X is not set
 # CONFIG_SMSC911X is not set
 # CONFIG_DM9000 is not set
+# CONFIG_ENC28J60 is not set
 # CONFIG_IBM_NEW_EMAC_ZMII is not set
 # CONFIG_IBM_NEW_EMAC_RGMII is not set
 # CONFIG_IBM_NEW_EMAC_TAH is not set
 # CONFIG_IBM_NEW_EMAC_EMAC4 is not set
-# CONFIG_B44 is not set
+# CONFIG_IBM_NEW_EMAC_NO_FLOW_CTRL is not set
+# CONFIG_IBM_NEW_EMAC_MAL_CLR_ICINTSTAT is not set
+# CONFIG_IBM_NEW_EMAC_MAL_COMMON_ERR is not set
 CONFIG_NETDEV_1000=y
 # CONFIG_AX88180 is not set
 CONFIG_NETDEV_10000=y
@@ -669,6 +690,7 @@ CONFIG_NETDEV_10000=y
 #
 # CONFIG_WLAN_PRE80211 is not set
 # CONFIG_WLAN_80211 is not set
+# CONFIG_IWLWIFI_LEDS is not set
 
 #
 # USB Network Adapters
@@ -681,7 +703,6 @@ CONFIG_NETDEV_10000=y
 # CONFIG_WAN is not set
 # CONFIG_PPP is not set
 # CONFIG_SLIP is not set
-# CONFIG_SHAPER is not set
 # CONFIG_NETCONSOLE is not set
 # CONFIG_NETPOLL is not set
 # CONFIG_NET_POLL_CONTROLLER is not set
@@ -692,7 +713,7 @@ CONFIG_NETDEV_10000=y
 # Input device support
 #
 CONFIG_INPUT=y
-# CONFIG_INPUT_FF_MEMLESS is not set
+CONFIG_INPUT_FF_MEMLESS=m
 # CONFIG_INPUT_POLLDEV is not set
 
 #
@@ -717,8 +738,9 @@ CONFIG_INPUT_MISC=y
 # CONFIG_INPUT_KEYSPAN_REMOTE is not set
 # CONFIG_INPUT_POWERMATE is not set
 # CONFIG_INPUT_YEALINK is not set
+# CONFIG_INPUT_CM109 is not set
 # CONFIG_INPUT_UINPUT is not set
-# CONFIG_TWI_KEYPAD is not set
+# CONFIG_CONFIG_INPUT_PCF8574 is not set
 
 #
 # Hardware I/O ports
@@ -734,16 +756,18 @@ CONFIG_INPUT_MISC=y
 # CONFIG_BF5xx_PPIFCD is not set
 # CONFIG_BFIN_SIMPLE_TIMER is not set
 # CONFIG_BF5xx_PPI is not set
-CONFIG_BFIN_OTP=y
-# CONFIG_BFIN_OTP_WRITE_ENABLE is not set
 # CONFIG_BFIN_SPORT is not set
 # CONFIG_BFIN_TIMER_LATENCY is not set
 # CONFIG_TWI_LCD is not set
+CONFIG_BFIN_DMA_INTERFACE=m
 CONFIG_SIMPLE_GPIO=m
 CONFIG_VT=y
+CONFIG_CONSOLE_TRANSLATIONS=y
 CONFIG_VT_CONSOLE=y
 CONFIG_HW_CONSOLE=y
 # CONFIG_VT_HW_CONSOLE_BINDING is not set
+# CONFIG_DEVKMEM is not set
+# CONFIG_BFIN_JTAG_COMM is not set
 # CONFIG_SERIAL_NONSTANDARD is not set
 
 #
@@ -766,6 +790,8 @@ CONFIG_SERIAL_CORE_CONSOLE=y
 # CONFIG_SERIAL_BFIN_SPORT is not set
 CONFIG_UNIX98_PTYS=y
 # CONFIG_LEGACY_PTYS is not set
+CONFIG_BFIN_OTP=y
+# CONFIG_BFIN_OTP_WRITE_ENABLE is not set
 
 #
 # CAN, the car bus and industrial fieldbus
@@ -773,44 +799,49 @@ CONFIG_UNIX98_PTYS=y
 # CONFIG_CAN4LINUX is not set
 # CONFIG_IPMI_HANDLER is not set
 # CONFIG_HW_RANDOM is not set
-# CONFIG_GEN_RTC is not set
 # CONFIG_R3964 is not set
 # CONFIG_RAW_DRIVER is not set
 # CONFIG_TCG_TPM is not set
 CONFIG_I2C=y
 CONFIG_I2C_BOARDINFO=y
 CONFIG_I2C_CHARDEV=m
+CONFIG_I2C_HELPER_AUTO=y
 
 #
-# I2C Algorithms
+# I2C Hardware Bus support
 #
-# CONFIG_I2C_ALGOBIT is not set
-# CONFIG_I2C_ALGOPCF is not set
-# CONFIG_I2C_ALGOPCA is not set
 
 #
-# I2C Hardware Bus support
+# I2C system bus drivers (mostly embedded / system-on-chip)
 #
 CONFIG_I2C_BLACKFIN_TWI=m
-CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=50
+CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=100
 # CONFIG_I2C_GPIO is not set
 # CONFIG_I2C_OCORES is not set
-# CONFIG_I2C_PARPORT_LIGHT is not set
 # CONFIG_I2C_SIMTEC is not set
+
+#
+# External I2C/SMBus adapter drivers
+#
+# CONFIG_I2C_PARPORT_LIGHT is not set
 # CONFIG_I2C_TAOS_EVM is not set
-# CONFIG_I2C_STUB is not set
 # CONFIG_I2C_TINY_USB is not set
 
 #
+# Other I2C/SMBus bus drivers
+#
+# CONFIG_I2C_PCA_PLATFORM is not set
+# CONFIG_I2C_STUB is not set
+
+#
 # Miscellaneous I2C Chip support
 #
-# CONFIG_SENSORS_DS1337 is not set
-# CONFIG_SENSORS_DS1374 is not set
 # CONFIG_DS1682 is not set
+# CONFIG_AT24 is not set
 # CONFIG_SENSORS_AD5252 is not set
 # CONFIG_SENSORS_EEPROM is not set
 # CONFIG_SENSORS_PCF8574 is not set
-# CONFIG_SENSORS_PCF8575 is not set
+# CONFIG_PCF8575 is not set
 # CONFIG_SENSORS_PCA9539 is not set
 # CONFIG_SENSORS_PCF8591 is not set
 # CONFIG_SENSORS_MAX6875 is not set
@@ -819,17 +850,15 @@ CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=50
 # CONFIG_I2C_DEBUG_ALGO is not set
 # CONFIG_I2C_DEBUG_BUS is not set
 # CONFIG_I2C_DEBUG_CHIP is not set
-
-#
-# SPI support
-#
 CONFIG_SPI=y
+# CONFIG_SPI_DEBUG is not set
 CONFIG_SPI_MASTER=y
 
 #
 # SPI Master Controller Drivers
 #
 CONFIG_SPI_BFIN=y
+# CONFIG_SPI_BFIN_LOCK is not set
 # CONFIG_SPI_BITBANG is not set
 
 #
@@ -838,11 +867,15 @@ CONFIG_SPI_BFIN=y
 # CONFIG_SPI_AT25 is not set
 # CONFIG_SPI_SPIDEV is not set
 # CONFIG_SPI_TLE62X0 is not set
+CONFIG_ARCH_WANT_OPTIONAL_GPIOLIB=y
+# CONFIG_GPIOLIB is not set
 # CONFIG_W1 is not set
 # CONFIG_POWER_SUPPLY is not set
 CONFIG_HWMON=y
 # CONFIG_HWMON_VID is not set
+# CONFIG_SENSORS_AD7414 is not set
 # CONFIG_SENSORS_AD7418 is not set
+# CONFIG_SENSORS_ADCXX is not set
 # CONFIG_SENSORS_ADM1021 is not set
 # CONFIG_SENSORS_ADM1025 is not set
 # CONFIG_SENSORS_ADM1026 is not set
@@ -850,6 +883,7 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_ADM1031 is not set
 # CONFIG_SENSORS_ADM9240 is not set
 # CONFIG_SENSORS_ADT7470 is not set
+# CONFIG_SENSORS_ADT7473 is not set
 # CONFIG_SENSORS_ATXP1 is not set
 # CONFIG_SENSORS_DS1621 is not set
 # CONFIG_SENSORS_F71805F is not set
@@ -870,6 +904,7 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_LM90 is not set
 # CONFIG_SENSORS_LM92 is not set
 # CONFIG_SENSORS_LM93 is not set
+# CONFIG_SENSORS_MAX1111 is not set
 # CONFIG_SENSORS_MAX1619 is not set
 # CONFIG_SENSORS_MAX6650 is not set
 # CONFIG_SENSORS_PC87360 is not set
@@ -878,6 +913,7 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_SMSC47M1 is not set
 # CONFIG_SENSORS_SMSC47M192 is not set
 # CONFIG_SENSORS_SMSC47B397 is not set
+# CONFIG_SENSORS_ADS7828 is not set
 # CONFIG_SENSORS_THMC50 is not set
 # CONFIG_SENSORS_VT1211 is not set
 # CONFIG_SENSORS_W83781D is not set
@@ -885,9 +921,12 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_W83792D is not set
 # CONFIG_SENSORS_W83793 is not set
 # CONFIG_SENSORS_W83L785TS is not set
+# CONFIG_SENSORS_W83L786NG is not set
 # CONFIG_SENSORS_W83627HF is not set
 # CONFIG_SENSORS_W83627EHF is not set
 # CONFIG_HWMON_DEBUG_CHIP is not set
+# CONFIG_THERMAL is not set
+# CONFIG_THERMAL_HWMON is not set
 CONFIG_WATCHDOG=y
 # CONFIG_WATCHDOG_NOWAYOUT is not set
 
@@ -903,21 +942,29 @@ CONFIG_BFIN_WDT=y
 # CONFIG_USBPCWATCHDOG is not set
 
 #
-# Sonics Silicon Backplane
-#
-CONFIG_SSB_POSSIBLE=y
-# CONFIG_SSB is not set
-
-#
 # Multifunction device drivers
 #
+# CONFIG_MFD_CORE is not set
 # CONFIG_MFD_SM501 is not set
+# CONFIG_HTC_PASIC3 is not set
+# CONFIG_MFD_TMIO is not set
+# CONFIG_MFD_WM8400 is not set
+# CONFIG_MFD_WM8350_I2C is not set
 
 #
 # Multimedia devices
 #
+
+#
+# Multimedia core support
+#
 # CONFIG_VIDEO_DEV is not set
 # CONFIG_DVB_CORE is not set
+# CONFIG_VIDEO_MEDIA is not set
+
+#
+# Multimedia drivers
+#
 # CONFIG_DAB is not set
 
 #
@@ -928,6 +975,7 @@ CONFIG_SSB_POSSIBLE=y
 CONFIG_FB=y
 # CONFIG_FIRMWARE_EDID is not set
 # CONFIG_FB_DDC is not set
+# CONFIG_FB_BOOT_VESA_SUPPORT is not set
 CONFIG_FB_CFB_FILLRECT=y
 CONFIG_FB_CFB_COPYAREA=y
 CONFIG_FB_CFB_IMAGEBLIT=y
@@ -935,8 +983,8 @@ CONFIG_FB_CFB_IMAGEBLIT=y
 # CONFIG_FB_SYS_FILLRECT is not set
 # CONFIG_FB_SYS_COPYAREA is not set
 # CONFIG_FB_SYS_IMAGEBLIT is not set
+# CONFIG_FB_FOREIGN_ENDIAN is not set
 # CONFIG_FB_SYS_FOPS is not set
-CONFIG_FB_DEFERRED_IO=y
 # CONFIG_FB_SVGALIB is not set
 # CONFIG_FB_MACMODES is not set
 # CONFIG_FB_BACKLIGHT is not set
@@ -947,12 +995,18 @@ CONFIG_FB_DEFERRED_IO=y
 # Frame buffer hardware drivers
 #
 CONFIG_FB_BFIN_T350MCQB=y
+# CONFIG_FB_BFIN_LQ035Q1 is not set
 # CONFIG_FB_BFIN_7393 is not set
 # CONFIG_FB_S1D13XXX is not set
 # CONFIG_FB_VIRTUAL is not set
+# CONFIG_FB_METRONOME is not set
 CONFIG_BACKLIGHT_LCD_SUPPORT=y
 CONFIG_LCD_CLASS_DEVICE=m
 CONFIG_LCD_LTV350QV=m
+# CONFIG_LCD_ILI9320 is not set
+# CONFIG_LCD_TDO24M is not set
+# CONFIG_LCD_VGG2432A4 is not set
+# CONFIG_LCD_PLATFORM is not set
 CONFIG_BACKLIGHT_CLASS_DEVICE=m
 # CONFIG_BACKLIGHT_CORGI is not set
 
@@ -977,15 +1031,8 @@ CONFIG_LOGO=y
 # CONFIG_LOGO_LINUX_CLUT224 is not set
 # CONFIG_LOGO_BLACKFIN_VGA16 is not set
 CONFIG_LOGO_BLACKFIN_CLUT224=y
-
-#
-# Sound
-#
 CONFIG_SOUND=m
-
-#
-# Advanced Linux Sound Architecture
-#
+# CONFIG_SOUND_OSS_CORE is not set
 CONFIG_SND=m
 CONFIG_SND_TIMER=m
 CONFIG_SND_PCM=m
@@ -997,62 +1044,38 @@ CONFIG_SND_SUPPORT_OLD_API=y
 CONFIG_SND_VERBOSE_PROCFS=y
 # CONFIG_SND_VERBOSE_PRINTK is not set
 # CONFIG_SND_DEBUG is not set
-
-#
-# Generic devices
-#
+CONFIG_SND_DRIVERS=y
 # CONFIG_SND_DUMMY is not set
 # CONFIG_SND_MTPAV is not set
 # CONFIG_SND_SERIAL_U16550 is not set
 # CONFIG_SND_MPU401 is not set
-
-#
-# SPI devices
-#
+CONFIG_SND_SPI=y
 
 #
 # ALSA Blackfin devices
 #
 # CONFIG_SND_BLACKFIN_AD1836 is not set
-# CONFIG_SND_BLACKFIN_AD1836_TDM is not set
-# CONFIG_SND_BLACKFIN_AD1836_I2S is not set
-# CONFIG_SND_BLACKFIN_AD1836_MULSUB is not set
-# CONFIG_SND_BLACKFIN_AD1836_5P1 is not set
-# CONFIG_SND_BFIN_AD73311 is not set
 # CONFIG_SND_BFIN_AD73322 is not set
-
-#
-# USB devices
-#
+CONFIG_SND_USB=y
 # CONFIG_SND_USB_AUDIO is not set
 # CONFIG_SND_USB_CAIAQ is not set
-
-#
-# System on Chip audio support
-#
-CONFIG_SND_SOC_AC97_BUS=y
 CONFIG_SND_SOC=m
-CONFIG_SND_BF5XX_SOC=m
-CONFIG_SND_MMAP_SUPPORT=y
+CONFIG_SND_SOC_AC97_BUS=y
+CONFIG_SND_BF5XX_I2S=m
+CONFIG_SND_BF5XX_SOC_SSM2602=m
+# CONFIG_SND_BF5XX_SOC_AD73311 is not set
+CONFIG_SND_BF5XX_AC97=m
+CONFIG_SND_BF5XX_MMAP_SUPPORT=y
+# CONFIG_SND_BF5XX_MULTICHAN_SUPPORT is not set
+CONFIG_SND_BF5XX_SOC_SPORT=m
 CONFIG_SND_BF5XX_SOC_I2S=m
 CONFIG_SND_BF5XX_SOC_AC97=m
-# CONFIG_SND_BF5XX_SOC_WM8750 is not set
-# CONFIG_SND_BF5XX_SOC_WM8731 is not set
-CONFIG_SND_BF5XX_SOC_SSM2602=m
-CONFIG_SND_BF5XX_SOC_BF5xx=m
+CONFIG_SND_BF5XX_SOC_AD1980=m
 CONFIG_SND_BF5XX_SPORT_NUM=0
 # CONFIG_SND_BF5XX_HAVE_COLD_RESET is not set
-
-#
-# SoC Audio support for SuperH
-#
-CONFIG_SND_SOC_SSM2602=m
-# CONFIG_SND_SOC_SSM2602_SPI is not set
+# CONFIG_SND_SOC_ALL_CODECS is not set
 CONFIG_SND_SOC_AD1980=m
-
-#
-# Open Sound System
-#
+CONFIG_SND_SOC_SSM2602=m
 # CONFIG_SOUND_PRIME is not set
 CONFIG_AC97_BUS=m
 CONFIG_HID_SUPPORT=y
@@ -1064,15 +1087,43 @@ CONFIG_HID=y
 # USB Input Devices
 #
 CONFIG_USB_HID=y
-# CONFIG_USB_HIDINPUT_POWERBOOK is not set
-# CONFIG_HID_FF is not set
+# CONFIG_HID_PID is not set
 # CONFIG_USB_HIDDEV is not set
+
+#
+# Special HID drivers
+#
+CONFIG_HID_COMPAT=y
+CONFIG_HID_A4TECH=y
+CONFIG_HID_APPLE=y
+CONFIG_HID_BELKIN=y
+CONFIG_HID_BRIGHT=y
+CONFIG_HID_CHERRY=y
+CONFIG_HID_CHICONY=y
+CONFIG_HID_CYPRESS=y
+CONFIG_HID_DELL=y
+CONFIG_HID_EZKEY=y
+CONFIG_HID_GYRATION=y
+CONFIG_HID_LOGITECH=y
+# CONFIG_LOGITECH_FF is not set
+# CONFIG_LOGIRUMBLEPAD2_FF is not set
+CONFIG_HID_MICROSOFT=y
+CONFIG_HID_MONTEREY=y
+CONFIG_HID_PANTHERLORD=y
+# CONFIG_PANTHERLORD_FF is not set
+CONFIG_HID_PETALYNX=y
+CONFIG_HID_SAMSUNG=y
+CONFIG_HID_SONY=y
+CONFIG_HID_SUNPLUS=y
+CONFIG_THRUSTMASTER_FF=m
+CONFIG_ZEROPLUS_FF=m
 CONFIG_USB_SUPPORT=y
 CONFIG_USB_ARCH_HAS_HCD=y
 # CONFIG_USB_ARCH_HAS_OHCI is not set
 # CONFIG_USB_ARCH_HAS_EHCI is not set
 CONFIG_USB=y
 # CONFIG_USB_DEBUG is not set
+# CONFIG_USB_ANNOUNCE_NEW_DEVICES is not set
 
 #
 # Miscellaneous USB options
@@ -1083,15 +1134,20 @@ CONFIG_USB_DEVICE_CLASS=y
 # CONFIG_USB_OTG is not set
 # CONFIG_USB_OTG_WHITELIST is not set
 CONFIG_USB_OTG_BLACKLIST_HUB=y
+CONFIG_USB_MON=y
+# CONFIG_USB_WUSB is not set
+# CONFIG_USB_WUSB_CBAF is not set
 
 #
 # USB Host Controller Drivers
 #
+# CONFIG_USB_C67X00_HCD is not set
 # CONFIG_USB_ISP116X_HCD is not set
-# CONFIG_USB_ISP1362_HCD is not set
 # CONFIG_USB_ISP1760_HCD is not set
+# CONFIG_USB_ISP1362_HCD is not set
 # CONFIG_USB_SL811_HCD is not set
 # CONFIG_USB_R8A66597_HCD is not set
+# CONFIG_USB_HWA_HCD is not set
 CONFIG_USB_MUSB_HDRC=y
 CONFIG_USB_MUSB_SOC=y
 
@@ -1103,13 +1159,15 @@ CONFIG_USB_MUSB_HOST=y
 # CONFIG_USB_MUSB_OTG is not set
 CONFIG_USB_MUSB_HDRC_HCD=y
 CONFIG_MUSB_PIO_ONLY=y
-CONFIG_USB_MUSB_LOGLEVEL=0
+# CONFIG_USB_MUSB_DEBUG is not set
 
 #
 # USB Device Class drivers
 #
 # CONFIG_USB_ACM is not set
 # CONFIG_USB_PRINTER is not set
+# CONFIG_USB_WDM is not set
+# CONFIG_USB_TMC is not set
 
 #
 # NOTE: USB_STORAGE enables SCSI, and 'SCSI disk support'
@@ -1124,15 +1182,10 @@ CONFIG_USB_MUSB_LOGLEVEL=0
 # USB Imaging devices
 #
 # CONFIG_USB_MDC800 is not set
-CONFIG_USB_MON=y
 
 #
 # USB port drivers
 #
-
-#
-# USB Serial Converter support
-#
 # CONFIG_USB_SERIAL is not set
 
 #
@@ -1141,7 +1194,7 @@ CONFIG_USB_MON=y
 # CONFIG_USB_EMI62 is not set
 # CONFIG_USB_EMI26 is not set
 # CONFIG_USB_ADUTUX is not set
-# CONFIG_USB_AUERSWALD is not set
+# CONFIG_USB_SEVSEG is not set
 # CONFIG_USB_RIO500 is not set
 # CONFIG_USB_LEGOTOWER is not set
 # CONFIG_USB_LCD is not set
@@ -1157,17 +1210,13 @@ CONFIG_USB_MON=y
 # CONFIG_USB_LD is not set
 # CONFIG_USB_TRANCEVIBRATOR is not set
 # CONFIG_USB_IOWARRIOR is not set
-
-#
-# USB DSL modem support
-#
-
-#
-# USB Gadget Support
-#
+# CONFIG_USB_ISIGHTFW is not set
+# CONFIG_USB_VST is not set
 # CONFIG_USB_GADGET is not set
 # CONFIG_MMC is not set
+# CONFIG_MEMSTICK is not set
 # CONFIG_NEW_LEDS is not set
+# CONFIG_ACCESSIBILITY is not set
 CONFIG_RTC_LIB=y
 CONFIG_RTC_CLASS=y
 CONFIG_RTC_HCTOSYS=y
@@ -1196,51 +1245,57 @@ CONFIG_RTC_INTF_DEV=y
 # CONFIG_RTC_DRV_PCF8563 is not set
 # CONFIG_RTC_DRV_PCF8583 is not set
 # CONFIG_RTC_DRV_M41T80 is not set
+# CONFIG_RTC_DRV_S35390A is not set
+# CONFIG_RTC_DRV_FM3130 is not set
 
 #
 # SPI RTC drivers
 #
-# CONFIG_RTC_DRV_RS5C348 is not set
+# CONFIG_RTC_DRV_M41T94 is not set
+# CONFIG_RTC_DRV_DS1305 is not set
 # CONFIG_RTC_DRV_MAX6902 is not set
+# CONFIG_RTC_DRV_R9701 is not set
+# CONFIG_RTC_DRV_RS5C348 is not set
+# CONFIG_RTC_DRV_DS3234 is not set
 
 #
 # Platform RTC drivers
 #
+# CONFIG_RTC_DRV_DS1286 is not set
+# CONFIG_RTC_DRV_DS1511 is not set
 # CONFIG_RTC_DRV_DS1553 is not set
-# CONFIG_RTC_DRV_STK17TA8 is not set
 # CONFIG_RTC_DRV_DS1742 is not set
+# CONFIG_RTC_DRV_STK17TA8 is not set
 # CONFIG_RTC_DRV_M48T86 is not set
+# CONFIG_RTC_DRV_M48T35 is not set
 # CONFIG_RTC_DRV_M48T59 is not set
+# CONFIG_RTC_DRV_BQ4802 is not set
 # CONFIG_RTC_DRV_V3020 is not set
 
 #
 # on-CPU RTC drivers
 #
 CONFIG_RTC_DRV_BFIN=y
-
-#
-# Userspace I/O
-#
+# CONFIG_DMADEVICES is not set
 # CONFIG_UIO is not set
+# CONFIG_STAGING is not set
 
 #
 # File systems
 #
 # CONFIG_EXT2_FS is not set
 # CONFIG_EXT3_FS is not set
-# CONFIG_EXT4DEV_FS is not set
+# CONFIG_EXT4_FS is not set
 # CONFIG_REISERFS_FS is not set
 # CONFIG_JFS_FS is not set
 # CONFIG_FS_POSIX_ACL is not set
+CONFIG_FILE_LOCKING=y
 # CONFIG_XFS_FS is not set
-# CONFIG_GFS2_FS is not set
 # CONFIG_OCFS2_FS is not set
-# CONFIG_MINIX_FS is not set
-# CONFIG_ROMFS_FS is not set
+# CONFIG_DNOTIFY is not set
 CONFIG_INOTIFY=y
 CONFIG_INOTIFY_USER=y
 # CONFIG_QUOTA is not set
-# CONFIG_DNOTIFY is not set
 # CONFIG_AUTOFS_FS is not set
 # CONFIG_AUTOFS4_FS is not set
 # CONFIG_FUSE_FS is not set
@@ -1280,11 +1335,11 @@ CONFIG_SYSFS=y
 # CONFIG_EFS_FS is not set
 CONFIG_YAFFS_FS=m
 CONFIG_YAFFS_YAFFS1=y
+# CONFIG_YAFFS_9BYTE_TAGS is not set
 # CONFIG_YAFFS_DOES_ECC is not set
 CONFIG_YAFFS_YAFFS2=y
 CONFIG_YAFFS_AUTO_YAFFS2=y
 # CONFIG_YAFFS_DISABLE_LAZY_LOAD is not set
-CONFIG_YAFFS_CHECKPOINT_RESERVED_BLOCKS=10
 # CONFIG_YAFFS_DISABLE_WIDE_TNODES is not set
 # CONFIG_YAFFS_ALWAYS_CHECK_CHUNK_ERASED is not set
 CONFIG_YAFFS_SHORT_NAMES_IN_RAM=y
@@ -1301,8 +1356,11 @@ CONFIG_JFFS2_RTIME=y
 # CONFIG_JFFS2_RUBIN is not set
 # CONFIG_CRAMFS is not set
 # CONFIG_VXFS_FS is not set
+# CONFIG_MINIX_FS is not set
+# CONFIG_OMFS_FS is not set
 # CONFIG_HPFS_FS is not set
 # CONFIG_QNX4FS_FS is not set
+# CONFIG_ROMFS_FS is not set
 # CONFIG_SYSV_FS is not set
 # CONFIG_UFS_FS is not set
 CONFIG_NETWORK_FILESYSTEMS=y
@@ -1310,13 +1368,12 @@ CONFIG_NFS_FS=m
 CONFIG_NFS_V3=y
 # CONFIG_NFS_V3_ACL is not set
 # CONFIG_NFS_V4 is not set
-# CONFIG_NFS_DIRECTIO is not set
 # CONFIG_NFSD is not set
 CONFIG_LOCKD=m
 CONFIG_LOCKD_V4=y
 CONFIG_NFS_COMMON=y
 CONFIG_SUNRPC=m
-# CONFIG_SUNRPC_BIND34 is not set
+# CONFIG_SUNRPC_REGISTER_V4 is not set
 # CONFIG_RPCSEC_GSS_KRB5 is not set
 # CONFIG_RPCSEC_GSS_SPKM3 is not set
 CONFIG_SMB_FS=m
@@ -1372,9 +1429,6 @@ CONFIG_NLS_DEFAULT="iso8859-1"
 # CONFIG_NLS_KOI8_U is not set
 # CONFIG_NLS_UTF8 is not set
 # CONFIG_DLM is not set
-CONFIG_INSTRUMENTATION=y
-# CONFIG_PROFILING is not set
-# CONFIG_MARKERS is not set
 
 #
 # Kernel hacking
@@ -1382,14 +1436,53 @@ CONFIG_INSTRUMENTATION=y
 # CONFIG_PRINTK_TIME is not set
 CONFIG_ENABLE_WARN_DEPRECATED=y
 CONFIG_ENABLE_MUST_CHECK=y
+CONFIG_FRAME_WARN=1024
 # CONFIG_MAGIC_SYSRQ is not set
 # CONFIG_UNUSED_SYMBOLS is not set
 CONFIG_DEBUG_FS=y
 # CONFIG_HEADERS_CHECK is not set
-# CONFIG_DEBUG_KERNEL is not set
+CONFIG_DEBUG_KERNEL=y
+# CONFIG_DEBUG_SHIRQ is not set
+CONFIG_DETECT_SOFTLOCKUP=y
+# CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC is not set
+CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC_VALUE=0
+CONFIG_SCHED_DEBUG=y
+# CONFIG_SCHEDSTATS is not set
+# CONFIG_TIMER_STATS is not set
+# CONFIG_DEBUG_OBJECTS is not set
+# CONFIG_DEBUG_SLAB is not set
+# CONFIG_DEBUG_RT_MUTEXES is not set
+# CONFIG_RT_MUTEX_TESTER is not set
+# CONFIG_DEBUG_SPINLOCK is not set
+# CONFIG_DEBUG_MUTEXES is not set
+# CONFIG_DEBUG_SPINLOCK_SLEEP is not set
+# CONFIG_DEBUG_LOCKING_API_SELFTESTS is not set
+# CONFIG_DEBUG_KOBJECT is not set
 # CONFIG_DEBUG_BUGVERBOSE is not set
+CONFIG_DEBUG_INFO=y
+# CONFIG_DEBUG_VM is not set
+# CONFIG_DEBUG_WRITECOUNT is not set
+# CONFIG_DEBUG_MEMORY_INIT is not set
+# CONFIG_DEBUG_LIST is not set
+# CONFIG_DEBUG_SG is not set
+# CONFIG_FRAME_POINTER is not set
+# CONFIG_BOOT_PRINTK_DELAY is not set
+# CONFIG_RCU_TORTURE_TEST is not set
+# CONFIG_RCU_CPU_STALL_DETECTOR is not set
+# CONFIG_BACKTRACE_SELF_TEST is not set
+# CONFIG_DEBUG_BLOCK_EXT_DEVT is not set
+# CONFIG_FAULT_INJECTION is not set
+# CONFIG_SYSCTL_SYSCALL_CHECK is not set
+# CONFIG_DYNAMIC_PRINTK_DEBUG is not set
 # CONFIG_SAMPLES is not set
+CONFIG_HAVE_ARCH_KGDB=y
+# CONFIG_KGDB is not set
+# CONFIG_DEBUG_STACKOVERFLOW is not set
+# CONFIG_DEBUG_STACK_USAGE is not set
+CONFIG_DEBUG_VERBOSE=y
 CONFIG_DEBUG_MMRS=y
+# CONFIG_DEBUG_HWERR is not set
+# CONFIG_DEBUG_DOUBLEFAULT is not set
 CONFIG_DEBUG_HUNT_FOR_ZERO=y
 CONFIG_DEBUG_BFIN_HWTRACE_ON=y
 CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION_OFF=y
@@ -1407,10 +1500,95 @@ CONFIG_ACCESS_CHECK=y
 #
 # CONFIG_KEYS is not set
 CONFIG_SECURITY=y
+# CONFIG_SECURITYFS is not set
 # CONFIG_SECURITY_NETWORK is not set
-# CONFIG_SECURITY_CAPABILITIES is not set
+# CONFIG_SECURITY_FILE_CAPABILITIES is not set
 # CONFIG_SECURITY_ROOTPLUG is not set
-# CONFIG_CRYPTO is not set
+CONFIG_SECURITY_DEFAULT_MMAP_MIN_ADDR=0
+CONFIG_CRYPTO=y
+
+#
+# Crypto core or helper
+#
+# CONFIG_CRYPTO_FIPS is not set
+# CONFIG_CRYPTO_MANAGER is not set
+# CONFIG_CRYPTO_GF128MUL is not set
+# CONFIG_CRYPTO_NULL is not set
+# CONFIG_CRYPTO_CRYPTD is not set
+# CONFIG_CRYPTO_AUTHENC is not set
+# CONFIG_CRYPTO_TEST is not set
+
+#
+# Authenticated Encryption with Associated Data
+#
+# CONFIG_CRYPTO_CCM is not set
+# CONFIG_CRYPTO_GCM is not set
+# CONFIG_CRYPTO_SEQIV is not set
+
+#
+# Block modes
+#
+# CONFIG_CRYPTO_CBC is not set
+# CONFIG_CRYPTO_CTR is not set
+# CONFIG_CRYPTO_CTS is not set
+# CONFIG_CRYPTO_ECB is not set
+# CONFIG_CRYPTO_LRW is not set
+# CONFIG_CRYPTO_PCBC is not set
+# CONFIG_CRYPTO_XTS is not set
+
+#
+# Hash modes
+#
+# CONFIG_CRYPTO_HMAC is not set
+# CONFIG_CRYPTO_XCBC is not set
+
+#
+# Digest
+#
+# CONFIG_CRYPTO_CRC32C is not set
+# CONFIG_CRYPTO_MD4 is not set
+# CONFIG_CRYPTO_MD5 is not set
+# CONFIG_CRYPTO_MICHAEL_MIC is not set
+# CONFIG_CRYPTO_RMD128 is not set
+# CONFIG_CRYPTO_RMD160 is not set
+# CONFIG_CRYPTO_RMD256 is not set
+# CONFIG_CRYPTO_RMD320 is not set
+# CONFIG_CRYPTO_SHA1 is not set
+# CONFIG_CRYPTO_SHA256 is not set
+# CONFIG_CRYPTO_SHA512 is not set
+# CONFIG_CRYPTO_TGR192 is not set
+# CONFIG_CRYPTO_WP512 is not set
+
+#
+# Ciphers
+#
+# CONFIG_CRYPTO_AES is not set
+# CONFIG_CRYPTO_ANUBIS is not set
+# CONFIG_CRYPTO_ARC4 is not set
+# CONFIG_CRYPTO_BLOWFISH is not set
+# CONFIG_CRYPTO_CAMELLIA is not set
+# CONFIG_CRYPTO_CAST5 is not set
+# CONFIG_CRYPTO_CAST6 is not set
+# CONFIG_CRYPTO_DES is not set
+# CONFIG_CRYPTO_FCRYPT is not set
+# CONFIG_CRYPTO_KHAZAD is not set
+# CONFIG_CRYPTO_SALSA20 is not set
+# CONFIG_CRYPTO_SEED is not set
+# CONFIG_CRYPTO_SERPENT is not set
+# CONFIG_CRYPTO_TEA is not set
+# CONFIG_CRYPTO_TWOFISH is not set
+
+#
+# Compression
+#
+# CONFIG_CRYPTO_DEFLATE is not set
+# CONFIG_CRYPTO_LZO is not set
+
+#
+# Random Number Generation
+#
+# CONFIG_CRYPTO_ANSI_CPRNG is not set
+CONFIG_CRYPTO_HW=y
 
 #
 # Library routines
@@ -1418,6 +1596,7 @@ CONFIG_SECURITY=y
 CONFIG_BITREVERSE=y
 CONFIG_CRC_CCITT=m
 # CONFIG_CRC16 is not set
+# CONFIG_CRC_T10DIF is not set
 # CONFIG_CRC_ITU_T is not set
 CONFIG_CRC32=y
 # CONFIG_CRC7 is not set
diff --git a/arch/blackfin/configs/BF533-EZKIT_defconfig b/arch/blackfin/configs/BF533-EZKIT_defconfig
index deeb5e45effb..92afd988449b 100644
--- a/arch/blackfin/configs/BF533-EZKIT_defconfig
+++ b/arch/blackfin/configs/BF533-EZKIT_defconfig
@@ -1,6 +1,6 @@
 #
 # Automatically generated make config: don't edit
-# Linux kernel version: 2.6.24.7
+# Linux kernel version: 2.6.28-rc2
 #
 # CONFIG_MMU is not set
 # CONFIG_FPU is not set
@@ -8,7 +8,6 @@ CONFIG_RWSEM_GENERIC_SPINLOCK=y
 # CONFIG_RWSEM_XCHGADD_ALGORITHM is not set
 CONFIG_BLACKFIN=y
 CONFIG_ZONE_DMA=y
-CONFIG_SEMAPHORE_SLEEPERS=y
 CONFIG_GENERIC_FIND_NEXT_BIT=y
 CONFIG_GENERIC_HWEIGHT=y
 CONFIG_GENERIC_HARDIRQS=y
@@ -31,18 +30,16 @@ CONFIG_SYSVIPC_SYSCTL=y
 # CONFIG_POSIX_MQUEUE is not set
 # CONFIG_BSD_PROCESS_ACCT is not set
 # CONFIG_TASKSTATS is not set
-# CONFIG_USER_NS is not set
-# CONFIG_PID_NS is not set
 # CONFIG_AUDIT is not set
 CONFIG_IKCONFIG=y
 CONFIG_IKCONFIG_PROC=y
 CONFIG_LOG_BUF_SHIFT=14
 # CONFIG_CGROUPS is not set
-CONFIG_FAIR_GROUP_SCHED=y
-CONFIG_FAIR_USER_SCHED=y
-# CONFIG_FAIR_CGROUP_SCHED is not set
-CONFIG_SYSFS_DEPRECATED=y
+# CONFIG_GROUP_SCHED is not set
+# CONFIG_SYSFS_DEPRECATED is not set
+# CONFIG_SYSFS_DEPRECATED_V2 is not set
 # CONFIG_RELAY is not set
+# CONFIG_NAMESPACES is not set
 CONFIG_BLK_DEV_INITRD=y
 CONFIG_INITRAMFS_SOURCE=""
 # CONFIG_CC_OPTIMIZE_FOR_SIZE is not set
@@ -51,26 +48,35 @@ CONFIG_EMBEDDED=y
 CONFIG_UID16=y
 CONFIG_SYSCTL_SYSCALL=y
 CONFIG_KALLSYMS=y
+# CONFIG_KALLSYMS_ALL is not set
 # CONFIG_KALLSYMS_EXTRA_PASS is not set
 CONFIG_HOTPLUG=y
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
+CONFIG_COMPAT_BRK=y
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
 CONFIG_EPOLL=y
 CONFIG_SIGNALFD=y
+CONFIG_TIMERFD=y
 CONFIG_EVENTFD=y
+CONFIG_AIO=y
 CONFIG_VM_EVENT_COUNTERS=y
 CONFIG_SLAB=y
 # CONFIG_SLUB is not set
 # CONFIG_SLOB is not set
+# CONFIG_PROFILING is not set
+# CONFIG_MARKERS is not set
+CONFIG_HAVE_OPROFILE=y
+# CONFIG_HAVE_GENERIC_DMA_COHERENT is not set
 CONFIG_SLABINFO=y
 CONFIG_RT_MUTEXES=y
 CONFIG_TINY_SHMEM=y
 CONFIG_BASE_SMALL=0
 CONFIG_MODULES=y
+# CONFIG_MODULE_FORCE_LOAD is not set
 CONFIG_MODULE_UNLOAD=y
 # CONFIG_MODULE_FORCE_UNLOAD is not set
 # CONFIG_MODVERSIONS is not set
@@ -81,6 +87,7 @@ CONFIG_BLOCK=y
 # CONFIG_BLK_DEV_IO_TRACE is not set
 # CONFIG_LSF is not set
 # CONFIG_BLK_DEV_BSG is not set
+# CONFIG_BLK_DEV_INTEGRITY is not set
 
 #
 # IO Schedulers
@@ -94,9 +101,11 @@ CONFIG_DEFAULT_AS=y
 # CONFIG_DEFAULT_CFQ is not set
 # CONFIG_DEFAULT_NOOP is not set
 CONFIG_DEFAULT_IOSCHED="anticipatory"
+CONFIG_CLASSIC_RCU=y
 # CONFIG_PREEMPT_NONE is not set
 CONFIG_PREEMPT_VOLUNTARY=y
 # CONFIG_PREEMPT is not set
+CONFIG_FREEZER=y
 
 #
 # Blackfin Processor Options
@@ -105,6 +114,10 @@ CONFIG_PREEMPT_VOLUNTARY=y
 #
 # Processor and Board Settings
 #
+# CONFIG_BF512 is not set
+# CONFIG_BF514 is not set
+# CONFIG_BF516 is not set
+# CONFIG_BF518 is not set
 # CONFIG_BF522 is not set
 # CONFIG_BF523 is not set
 # CONFIG_BF524 is not set
@@ -117,24 +130,30 @@ CONFIG_BF533=y
 # CONFIG_BF534 is not set
 # CONFIG_BF536 is not set
 # CONFIG_BF537 is not set
+# CONFIG_BF538 is not set
+# CONFIG_BF539 is not set
 # CONFIG_BF542 is not set
 # CONFIG_BF544 is not set
 # CONFIG_BF547 is not set
 # CONFIG_BF548 is not set
 # CONFIG_BF549 is not set
 # CONFIG_BF561 is not set
+CONFIG_BF_REV_MIN=3
+CONFIG_BF_REV_MAX=6
 # CONFIG_BF_REV_0_0 is not set
 # CONFIG_BF_REV_0_1 is not set
 # CONFIG_BF_REV_0_2 is not set
 CONFIG_BF_REV_0_3=y
 # CONFIG_BF_REV_0_4 is not set
 # CONFIG_BF_REV_0_5 is not set
+# CONFIG_BF_REV_0_6 is not set
 # CONFIG_BF_REV_ANY is not set
 # CONFIG_BF_REV_NONE is not set
 CONFIG_BF53x=y
 CONFIG_MEM_MT48LC16M16A2TG_75=y
 CONFIG_BFIN533_EZKIT=y
 # CONFIG_BFIN533_STAMP is not set
+# CONFIG_BLACKSTAMP is not set
 # CONFIG_BFIN533_BLUETECHNIX_CM is not set
 # CONFIG_H8606_HVSISTEMAS is not set
 # CONFIG_BFIN532_IP0X is not set
@@ -187,7 +206,6 @@ CONFIG_BOOT_LOAD=0x1000
 #
 CONFIG_CLKIN_HZ=27000000
 # CONFIG_BFIN_KERNEL_CLOCK is not set
-CONFIG_MAX_MEM_SIZE=512
 CONFIG_MAX_VCO_HZ=750000000
 CONFIG_MIN_VCO_HZ=50000000
 CONFIG_MAX_SCLK_HZ=133333333
@@ -201,6 +219,7 @@ CONFIG_HZ_250=y
 # CONFIG_HZ_300 is not set
 # CONFIG_HZ_1000 is not set
 CONFIG_HZ=250
+CONFIG_SCHED_HRTICK=y
 CONFIG_GENERIC_TIME=y
 CONFIG_GENERIC_CLOCKEVENTS=y
 # CONFIG_CYCLES_CLOCKSOURCE is not set
@@ -238,6 +257,12 @@ CONFIG_SYS_BFIN_SPINLOCK_L1=y
 CONFIG_CACHELINE_ALIGNED_L1=y
 # CONFIG_SYSCALL_TAB_L1 is not set
 # CONFIG_CPLB_SWITCH_TAB_L1 is not set
+CONFIG_APP_STACK_L1=y
+
+#
+# Speed Optimizations
+#
+CONFIG_BFIN_INS_LOWOVERHEAD=y
 CONFIG_RAMKERNEL=y
 # CONFIG_ROMKERNEL is not set
 CONFIG_SELECT_MEMORY_MODEL=y
@@ -246,14 +271,13 @@ CONFIG_FLATMEM_MANUAL=y
 # CONFIG_SPARSEMEM_MANUAL is not set
 CONFIG_FLATMEM=y
 CONFIG_FLAT_NODE_MEM_MAP=y
-# CONFIG_SPARSEMEM_STATIC is not set
-# CONFIG_SPARSEMEM_VMEMMAP_ENABLE is not set
+CONFIG_PAGEFLAGS_EXTENDED=y
 CONFIG_SPLIT_PTLOCK_CPUS=4
 # CONFIG_RESOURCES_64BIT is not set
+# CONFIG_PHYS_ADDR_T_64BIT is not set
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_VIRT_TO_BUS=y
 # CONFIG_BFIN_GPTIMERS is not set
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_4M is not set
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
@@ -268,7 +292,6 @@ CONFIG_BFIN_DCACHE=y
 # CONFIG_BFIN_ICACHE_LOCK is not set
 # CONFIG_BFIN_WB is not set
 CONFIG_BFIN_WT=y
-CONFIG_L1_MAX_PIECE=16
 # CONFIG_MPU is not set
 
 #
@@ -297,7 +320,6 @@ CONFIG_BANK_3=0xAAC2
 #
 # Bus options (PCI, PCMCIA, EISA, MCA, ISA)
 #
-# CONFIG_PCI is not set
 # CONFIG_ARCH_SUPPORTS_MSI is not set
 # CONFIG_PCCARD is not set
 
@@ -308,29 +330,30 @@ CONFIG_BINFMT_ELF_FDPIC=y
 CONFIG_BINFMT_FLAT=y
 CONFIG_BINFMT_ZFLAT=y
 # CONFIG_BINFMT_SHARED_FLAT is not set
+# CONFIG_HAVE_AOUT is not set
 # CONFIG_BINFMT_MISC is not set
 
 #
 # Power management options
 #
 CONFIG_PM=y
-# CONFIG_PM_LEGACY is not set
 # CONFIG_PM_DEBUG is not set
 CONFIG_PM_SLEEP=y
-CONFIG_SUSPEND_UP_POSSIBLE=y
 CONFIG_SUSPEND=y
+CONFIG_SUSPEND_FREEZER=y
+CONFIG_ARCH_SUSPEND_POSSIBLE=y
 CONFIG_PM_BFIN_SLEEP_DEEPER=y
 # CONFIG_PM_BFIN_SLEEP is not set
 # CONFIG_PM_WAKEUP_BY_GPIO is not set
 
 #
-# CPU Frequency scaling
+# Possible Suspend Mem / Hibernate Wake-Up Sources
 #
-# CONFIG_CPU_FREQ is not set
 
 #
-# Networking
+# CPU Frequency scaling
 #
+# CONFIG_CPU_FREQ is not set
 CONFIG_NET=y
 
 #
@@ -343,6 +366,7 @@ CONFIG_XFRM=y
 # CONFIG_XFRM_USER is not set
 # CONFIG_XFRM_SUB_POLICY is not set
 # CONFIG_XFRM_MIGRATE is not set
+# CONFIG_XFRM_STATISTICS is not set
 # CONFIG_NET_KEY is not set
 CONFIG_INET=y
 # CONFIG_IP_MULTICAST is not set
@@ -372,8 +396,6 @@ CONFIG_TCP_CONG_CUBIC=y
 CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_TCP_MD5SIG is not set
 # CONFIG_IPV6 is not set
-# CONFIG_INET6_XFRM_TUNNEL is not set
-# CONFIG_INET6_TUNNEL is not set
 # CONFIG_NETLABEL is not set
 # CONFIG_NETWORK_SECMARK is not set
 # CONFIG_NETFILTER is not set
@@ -382,6 +404,7 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_TIPC is not set
 # CONFIG_ATM is not set
 # CONFIG_BRIDGE is not set
+# CONFIG_NET_DSA is not set
 # CONFIG_VLAN_8021Q is not set
 # CONFIG_DECNET is not set
 # CONFIG_LLC2 is not set
@@ -398,6 +421,7 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 #
 # CONFIG_NET_PKTGEN is not set
 # CONFIG_HAMRADIO is not set
+# CONFIG_CAN is not set
 CONFIG_IRDA=m
 
 #
@@ -430,24 +454,14 @@ CONFIG_IRTTY_SIR=m
 # CONFIG_DONGLE is not set
 
 #
-# Old SIR device drivers
-#
-# CONFIG_IRPORT_SIR is not set
-
-#
-# Old Serial dongle support
-#
-
-#
 # FIR device drivers
 #
 # CONFIG_BT is not set
 # CONFIG_AF_RXRPC is not set
-
-#
-# Wireless
-#
+# CONFIG_PHONET is not set
+CONFIG_WIRELESS=y
 # CONFIG_CFG80211 is not set
+CONFIG_WIRELESS_OLD_REGULATORY=y
 # CONFIG_WIRELESS_EXT is not set
 # CONFIG_MAC80211 is not set
 # CONFIG_IEEE80211 is not set
@@ -465,6 +479,8 @@ CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
 CONFIG_STANDALONE=y
 CONFIG_PREVENT_FIRMWARE_BUILD=y
 # CONFIG_FW_LOADER is not set
+# CONFIG_DEBUG_DRIVER is not set
+# CONFIG_DEBUG_DEVRES is not set
 # CONFIG_SYS_HYPERVISOR is not set
 # CONFIG_CONNECTOR is not set
 CONFIG_MTD=y
@@ -473,6 +489,7 @@ CONFIG_MTD=y
 CONFIG_MTD_PARTITIONS=y
 # CONFIG_MTD_REDBOOT_PARTS is not set
 # CONFIG_MTD_CMDLINE_PARTS is not set
+# CONFIG_MTD_AR7_PARTS is not set
 
 #
 # User Modules And Translation Layers
@@ -516,6 +533,7 @@ CONFIG_MTD_ROM=m
 #
 CONFIG_MTD_COMPLEX_MAPPINGS=y
 # CONFIG_MTD_PHYSMAP is not set
+# CONFIG_MTD_GPIO_ADDR is not set
 # CONFIG_MTD_UCLINUX is not set
 # CONFIG_MTD_PLATRAM is not set
 
@@ -550,11 +568,14 @@ CONFIG_BLK_DEV=y
 CONFIG_BLK_DEV_RAM=y
 CONFIG_BLK_DEV_RAM_COUNT=16
 CONFIG_BLK_DEV_RAM_SIZE=4096
-CONFIG_BLK_DEV_RAM_BLOCKSIZE=1024
+# CONFIG_BLK_DEV_XIP is not set
 # CONFIG_CDROM_PKTCDVD is not set
 # CONFIG_ATA_OVER_ETH is not set
+# CONFIG_BLK_DEV_HD is not set
 CONFIG_MISC_DEVICES=y
 # CONFIG_EEPROM_93CX6 is not set
+# CONFIG_ENCLOSURE_SERVICES is not set
+CONFIG_HAVE_IDE=y
 # CONFIG_IDE is not set
 
 #
@@ -567,7 +588,6 @@ CONFIG_MISC_DEVICES=y
 # CONFIG_ATA is not set
 # CONFIG_MD is not set
 CONFIG_NETDEVICES=y
-# CONFIG_NETDEVICES_MULTIQUEUE is not set
 # CONFIG_DUMMY is not set
 # CONFIG_BONDING is not set
 # CONFIG_MACVLAN is not set
@@ -580,11 +600,14 @@ CONFIG_MII=y
 CONFIG_SMC91X=y
 # CONFIG_SMSC911X is not set
 # CONFIG_DM9000 is not set
+# CONFIG_ENC28J60 is not set
 # CONFIG_IBM_NEW_EMAC_ZMII is not set
 # CONFIG_IBM_NEW_EMAC_RGMII is not set
 # CONFIG_IBM_NEW_EMAC_TAH is not set
 # CONFIG_IBM_NEW_EMAC_EMAC4 is not set
-# CONFIG_B44 is not set
+# CONFIG_IBM_NEW_EMAC_NO_FLOW_CTRL is not set
+# CONFIG_IBM_NEW_EMAC_MAL_CLR_ICINTSTAT is not set
+# CONFIG_IBM_NEW_EMAC_MAL_COMMON_ERR is not set
 CONFIG_NETDEV_1000=y
 # CONFIG_AX88180 is not set
 CONFIG_NETDEV_10000=y
@@ -594,10 +617,10 @@ CONFIG_NETDEV_10000=y
 #
 # CONFIG_WLAN_PRE80211 is not set
 # CONFIG_WLAN_80211 is not set
+# CONFIG_IWLWIFI_LEDS is not set
 # CONFIG_WAN is not set
 # CONFIG_PPP is not set
 # CONFIG_SLIP is not set
-# CONFIG_SHAPER is not set
 # CONFIG_NETCONSOLE is not set
 # CONFIG_NETPOLL is not set
 # CONFIG_NET_POLL_CONTROLLER is not set
@@ -645,8 +668,11 @@ CONFIG_INPUT_EVDEV=m
 # CONFIG_BF5xx_PPI is not set
 CONFIG_BFIN_SPORT=y
 # CONFIG_BFIN_TIMER_LATENCY is not set
+CONFIG_BFIN_DMA_INTERFACE=m
 CONFIG_SIMPLE_GPIO=m
 # CONFIG_VT is not set
+# CONFIG_DEVKMEM is not set
+# CONFIG_BFIN_JTAG_COMM is not set
 # CONFIG_SERIAL_NONSTANDARD is not set
 
 #
@@ -675,22 +701,19 @@ CONFIG_UNIX98_PTYS=y
 # CONFIG_CAN4LINUX is not set
 # CONFIG_IPMI_HANDLER is not set
 # CONFIG_HW_RANDOM is not set
-# CONFIG_GEN_RTC is not set
 # CONFIG_R3964 is not set
 # CONFIG_RAW_DRIVER is not set
 # CONFIG_TCG_TPM is not set
 # CONFIG_I2C is not set
-
-#
-# SPI support
-#
 CONFIG_SPI=y
+# CONFIG_SPI_DEBUG is not set
 CONFIG_SPI_MASTER=y
 
 #
 # SPI Master Controller Drivers
 #
 CONFIG_SPI_BFIN=y
+# CONFIG_SPI_BFIN_LOCK is not set
 # CONFIG_SPI_BITBANG is not set
 
 #
@@ -699,14 +722,18 @@ CONFIG_SPI_BFIN=y
 # CONFIG_SPI_AT25 is not set
 # CONFIG_SPI_SPIDEV is not set
 # CONFIG_SPI_TLE62X0 is not set
+CONFIG_ARCH_WANT_OPTIONAL_GPIOLIB=y
+# CONFIG_GPIOLIB is not set
 # CONFIG_W1 is not set
 # CONFIG_POWER_SUPPLY is not set
 CONFIG_HWMON=y
 # CONFIG_HWMON_VID is not set
+# CONFIG_SENSORS_ADCXX is not set
 # CONFIG_SENSORS_F71805F is not set
 # CONFIG_SENSORS_F71882FG is not set
 # CONFIG_SENSORS_IT87 is not set
 # CONFIG_SENSORS_LM70 is not set
+# CONFIG_SENSORS_MAX1111 is not set
 # CONFIG_SENSORS_PC87360 is not set
 # CONFIG_SENSORS_PC87427 is not set
 # CONFIG_SENSORS_SMSC47M1 is not set
@@ -715,6 +742,8 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_W83627HF is not set
 # CONFIG_SENSORS_W83627EHF is not set
 # CONFIG_HWMON_DEBUG_CHIP is not set
+# CONFIG_THERMAL is not set
+# CONFIG_THERMAL_HWMON is not set
 CONFIG_WATCHDOG=y
 # CONFIG_WATCHDOG_NOWAYOUT is not set
 
@@ -725,21 +754,28 @@ CONFIG_WATCHDOG=y
 CONFIG_BFIN_WDT=y
 
 #
-# Sonics Silicon Backplane
-#
-CONFIG_SSB_POSSIBLE=y
-# CONFIG_SSB is not set
-
-#
 # Multifunction device drivers
 #
+# CONFIG_MFD_CORE is not set
 # CONFIG_MFD_SM501 is not set
+# CONFIG_HTC_PASIC3 is not set
+# CONFIG_MFD_TMIO is not set
+# CONFIG_MFD_WM8400 is not set
 
 #
 # Multimedia devices
 #
+
+#
+# Multimedia core support
+#
 # CONFIG_VIDEO_DEV is not set
 # CONFIG_DVB_CORE is not set
+# CONFIG_VIDEO_MEDIA is not set
+
+#
+# Multimedia drivers
+#
 # CONFIG_DAB is not set
 
 #
@@ -754,18 +790,22 @@ CONFIG_SSB_POSSIBLE=y
 # Display device support
 #
 # CONFIG_DISPLAY_SUPPORT is not set
-
-#
-# Sound
-#
 # CONFIG_SOUND is not set
 CONFIG_HID_SUPPORT=y
 CONFIG_HID=m
 # CONFIG_HID_DEBUG is not set
 # CONFIG_HIDRAW is not set
+# CONFIG_HID_PID is not set
+
+#
+# Special HID drivers
+#
+CONFIG_HID_COMPAT=y
 # CONFIG_USB_SUPPORT is not set
 # CONFIG_MMC is not set
+# CONFIG_MEMSTICK is not set
 # CONFIG_NEW_LEDS is not set
+# CONFIG_ACCESSIBILITY is not set
 CONFIG_RTC_LIB=y
 CONFIG_RTC_CLASS=y
 CONFIG_RTC_HCTOSYS=y
@@ -784,47 +824,51 @@ CONFIG_RTC_INTF_DEV=y
 #
 # SPI RTC drivers
 #
-# CONFIG_RTC_DRV_RS5C348 is not set
+# CONFIG_RTC_DRV_M41T94 is not set
+# CONFIG_RTC_DRV_DS1305 is not set
 # CONFIG_RTC_DRV_MAX6902 is not set
+# CONFIG_RTC_DRV_R9701 is not set
+# CONFIG_RTC_DRV_RS5C348 is not set
+# CONFIG_RTC_DRV_DS3234 is not set
 
 #
 # Platform RTC drivers
 #
+# CONFIG_RTC_DRV_DS1286 is not set
+# CONFIG_RTC_DRV_DS1511 is not set
 # CONFIG_RTC_DRV_DS1553 is not set
-# CONFIG_RTC_DRV_STK17TA8 is not set
 # CONFIG_RTC_DRV_DS1742 is not set
+# CONFIG_RTC_DRV_STK17TA8 is not set
 # CONFIG_RTC_DRV_M48T86 is not set
+# CONFIG_RTC_DRV_M48T35 is not set
 # CONFIG_RTC_DRV_M48T59 is not set
+# CONFIG_RTC_DRV_BQ4802 is not set
 # CONFIG_RTC_DRV_V3020 is not set
 
 #
 # on-CPU RTC drivers
 #
 CONFIG_RTC_DRV_BFIN=y
-
-#
-# Userspace I/O
-#
+# CONFIG_DMADEVICES is not set
 # CONFIG_UIO is not set
+# CONFIG_STAGING is not set
 
 #
 # File systems
 #
 # CONFIG_EXT2_FS is not set
 # CONFIG_EXT3_FS is not set
-# CONFIG_EXT4DEV_FS is not set
+# CONFIG_EXT4_FS is not set
 # CONFIG_REISERFS_FS is not set
 # CONFIG_JFS_FS is not set
 # CONFIG_FS_POSIX_ACL is not set
+CONFIG_FILE_LOCKING=y
 # CONFIG_XFS_FS is not set
-# CONFIG_GFS2_FS is not set
 # CONFIG_OCFS2_FS is not set
-# CONFIG_MINIX_FS is not set
-# CONFIG_ROMFS_FS is not set
+# CONFIG_DNOTIFY is not set
 CONFIG_INOTIFY=y
 CONFIG_INOTIFY_USER=y
 # CONFIG_QUOTA is not set
-# CONFIG_DNOTIFY is not set
 # CONFIG_AUTOFS_FS is not set
 # CONFIG_AUTOFS4_FS is not set
 # CONFIG_FUSE_FS is not set
@@ -864,11 +908,11 @@ CONFIG_SYSFS=y
 # CONFIG_EFS_FS is not set
 CONFIG_YAFFS_FS=m
 CONFIG_YAFFS_YAFFS1=y
+# CONFIG_YAFFS_9BYTE_TAGS is not set
 # CONFIG_YAFFS_DOES_ECC is not set
 CONFIG_YAFFS_YAFFS2=y
 CONFIG_YAFFS_AUTO_YAFFS2=y
 # CONFIG_YAFFS_DISABLE_LAZY_LOAD is not set
-CONFIG_YAFFS_CHECKPOINT_RESERVED_BLOCKS=10
 # CONFIG_YAFFS_DISABLE_WIDE_TNODES is not set
 # CONFIG_YAFFS_ALWAYS_CHECK_CHUNK_ERASED is not set
 CONFIG_YAFFS_SHORT_NAMES_IN_RAM=y
@@ -885,8 +929,11 @@ CONFIG_JFFS2_RTIME=y
 # CONFIG_JFFS2_RUBIN is not set
 # CONFIG_CRAMFS is not set
 # CONFIG_VXFS_FS is not set
+# CONFIG_MINIX_FS is not set
+# CONFIG_OMFS_FS is not set
 # CONFIG_HPFS_FS is not set
 # CONFIG_QNX4FS_FS is not set
+# CONFIG_ROMFS_FS is not set
 # CONFIG_SYSV_FS is not set
 # CONFIG_UFS_FS is not set
 CONFIG_NETWORK_FILESYSTEMS=y
@@ -894,13 +941,12 @@ CONFIG_NFS_FS=m
 CONFIG_NFS_V3=y
 # CONFIG_NFS_V3_ACL is not set
 # CONFIG_NFS_V4 is not set
-# CONFIG_NFS_DIRECTIO is not set
 # CONFIG_NFSD is not set
 CONFIG_LOCKD=m
 CONFIG_LOCKD_V4=y
 CONFIG_NFS_COMMON=y
 CONFIG_SUNRPC=m
-# CONFIG_SUNRPC_BIND34 is not set
+# CONFIG_SUNRPC_REGISTER_V4 is not set
 # CONFIG_RPCSEC_GSS_KRB5 is not set
 # CONFIG_RPCSEC_GSS_SPKM3 is not set
 CONFIG_SMB_FS=m
@@ -956,9 +1002,6 @@ CONFIG_NLS_DEFAULT="iso8859-1"
 # CONFIG_NLS_KOI8_U is not set
 # CONFIG_NLS_UTF8 is not set
 # CONFIG_DLM is not set
-CONFIG_INSTRUMENTATION=y
-# CONFIG_PROFILING is not set
-# CONFIG_MARKERS is not set
 
 #
 # Kernel hacking
@@ -966,14 +1009,53 @@ CONFIG_INSTRUMENTATION=y
 # CONFIG_PRINTK_TIME is not set
 CONFIG_ENABLE_WARN_DEPRECATED=y
 CONFIG_ENABLE_MUST_CHECK=y
+CONFIG_FRAME_WARN=1024
 # CONFIG_MAGIC_SYSRQ is not set
 # CONFIG_UNUSED_SYMBOLS is not set
 CONFIG_DEBUG_FS=y
 # CONFIG_HEADERS_CHECK is not set
-# CONFIG_DEBUG_KERNEL is not set
+CONFIG_DEBUG_KERNEL=y
+# CONFIG_DEBUG_SHIRQ is not set
+CONFIG_DETECT_SOFTLOCKUP=y
+# CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC is not set
+CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC_VALUE=0
+CONFIG_SCHED_DEBUG=y
+# CONFIG_SCHEDSTATS is not set
+# CONFIG_TIMER_STATS is not set
+# CONFIG_DEBUG_OBJECTS is not set
+# CONFIG_DEBUG_SLAB is not set
+# CONFIG_DEBUG_RT_MUTEXES is not set
+# CONFIG_RT_MUTEX_TESTER is not set
+# CONFIG_DEBUG_SPINLOCK is not set
+# CONFIG_DEBUG_MUTEXES is not set
+# CONFIG_DEBUG_SPINLOCK_SLEEP is not set
+# CONFIG_DEBUG_LOCKING_API_SELFTESTS is not set
+# CONFIG_DEBUG_KOBJECT is not set
 # CONFIG_DEBUG_BUGVERBOSE is not set
+CONFIG_DEBUG_INFO=y
+# CONFIG_DEBUG_VM is not set
+# CONFIG_DEBUG_WRITECOUNT is not set
+# CONFIG_DEBUG_MEMORY_INIT is not set
+# CONFIG_DEBUG_LIST is not set
+# CONFIG_DEBUG_SG is not set
+# CONFIG_FRAME_POINTER is not set
+# CONFIG_BOOT_PRINTK_DELAY is not set
+# CONFIG_RCU_TORTURE_TEST is not set
+# CONFIG_RCU_CPU_STALL_DETECTOR is not set
+# CONFIG_BACKTRACE_SELF_TEST is not set
+# CONFIG_DEBUG_BLOCK_EXT_DEVT is not set
+# CONFIG_FAULT_INJECTION is not set
+# CONFIG_SYSCTL_SYSCALL_CHECK is not set
+# CONFIG_DYNAMIC_PRINTK_DEBUG is not set
 # CONFIG_SAMPLES is not set
+CONFIG_HAVE_ARCH_KGDB=y
+# CONFIG_KGDB is not set
+# CONFIG_DEBUG_STACKOVERFLOW is not set
+# CONFIG_DEBUG_STACK_USAGE is not set
+CONFIG_DEBUG_VERBOSE=y
 CONFIG_DEBUG_MMRS=y
+# CONFIG_DEBUG_HWERR is not set
+# CONFIG_DEBUG_DOUBLEFAULT is not set
 CONFIG_DEBUG_HUNT_FOR_ZERO=y
 CONFIG_DEBUG_BFIN_HWTRACE_ON=y
 CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION_OFF=y
@@ -991,9 +1073,94 @@ CONFIG_ACCESS_CHECK=y
 #
 # CONFIG_KEYS is not set
 CONFIG_SECURITY=y
+# CONFIG_SECURITYFS is not set
 # CONFIG_SECURITY_NETWORK is not set
-# CONFIG_SECURITY_CAPABILITIES is not set
-# CONFIG_CRYPTO is not set
+# CONFIG_SECURITY_FILE_CAPABILITIES is not set
+CONFIG_SECURITY_DEFAULT_MMAP_MIN_ADDR=0
+CONFIG_CRYPTO=y
+
+#
+# Crypto core or helper
+#
+# CONFIG_CRYPTO_FIPS is not set
+# CONFIG_CRYPTO_MANAGER is not set
+# CONFIG_CRYPTO_GF128MUL is not set
+# CONFIG_CRYPTO_NULL is not set
+# CONFIG_CRYPTO_CRYPTD is not set
+# CONFIG_CRYPTO_AUTHENC is not set
+# CONFIG_CRYPTO_TEST is not set
+
+#
+# Authenticated Encryption with Associated Data
+#
+# CONFIG_CRYPTO_CCM is not set
+# CONFIG_CRYPTO_GCM is not set
+# CONFIG_CRYPTO_SEQIV is not set
+
+#
+# Block modes
+#
+# CONFIG_CRYPTO_CBC is not set
+# CONFIG_CRYPTO_CTR is not set
+# CONFIG_CRYPTO_CTS is not set
+# CONFIG_CRYPTO_ECB is not set
+# CONFIG_CRYPTO_LRW is not set
+# CONFIG_CRYPTO_PCBC is not set
+# CONFIG_CRYPTO_XTS is not set
+
+#
+# Hash modes
+#
+# CONFIG_CRYPTO_HMAC is not set
+# CONFIG_CRYPTO_XCBC is not set
+
+#
+# Digest
+#
+# CONFIG_CRYPTO_CRC32C is not set
+# CONFIG_CRYPTO_MD4 is not set
+# CONFIG_CRYPTO_MD5 is not set
+# CONFIG_CRYPTO_MICHAEL_MIC is not set
+# CONFIG_CRYPTO_RMD128 is not set
+# CONFIG_CRYPTO_RMD160 is not set
+# CONFIG_CRYPTO_RMD256 is not set
+# CONFIG_CRYPTO_RMD320 is not set
+# CONFIG_CRYPTO_SHA1 is not set
+# CONFIG_CRYPTO_SHA256 is not set
+# CONFIG_CRYPTO_SHA512 is not set
+# CONFIG_CRYPTO_TGR192 is not set
+# CONFIG_CRYPTO_WP512 is not set
+
+#
+# Ciphers
+#
+# CONFIG_CRYPTO_AES is not set
+# CONFIG_CRYPTO_ANUBIS is not set
+# CONFIG_CRYPTO_ARC4 is not set
+# CONFIG_CRYPTO_BLOWFISH is not set
+# CONFIG_CRYPTO_CAMELLIA is not set
+# CONFIG_CRYPTO_CAST5 is not set
+# CONFIG_CRYPTO_CAST6 is not set
+# CONFIG_CRYPTO_DES is not set
+# CONFIG_CRYPTO_FCRYPT is not set
+# CONFIG_CRYPTO_KHAZAD is not set
+# CONFIG_CRYPTO_SALSA20 is not set
+# CONFIG_CRYPTO_SEED is not set
+# CONFIG_CRYPTO_SERPENT is not set
+# CONFIG_CRYPTO_TEA is not set
+# CONFIG_CRYPTO_TWOFISH is not set
+
+#
+# Compression
+#
+# CONFIG_CRYPTO_DEFLATE is not set
+# CONFIG_CRYPTO_LZO is not set
+
+#
+# Random Number Generation
+#
+# CONFIG_CRYPTO_ANSI_CPRNG is not set
+CONFIG_CRYPTO_HW=y
 
 #
 # Library routines
@@ -1001,6 +1168,7 @@ CONFIG_SECURITY=y
 CONFIG_BITREVERSE=y
 CONFIG_CRC_CCITT=m
 # CONFIG_CRC16 is not set
+# CONFIG_CRC_T10DIF is not set
 # CONFIG_CRC_ITU_T is not set
 CONFIG_CRC32=y
 # CONFIG_CRC7 is not set
diff --git a/arch/blackfin/configs/BF533-STAMP_defconfig b/arch/blackfin/configs/BF533-STAMP_defconfig
index c23267ed880b..49eabb41e9e5 100644
--- a/arch/blackfin/configs/BF533-STAMP_defconfig
+++ b/arch/blackfin/configs/BF533-STAMP_defconfig
@@ -1,6 +1,6 @@
 #
 # Automatically generated make config: don't edit
-# Linux kernel version: 2.6.24.7
+# Linux kernel version: 2.6.28-rc2
 #
 # CONFIG_MMU is not set
 # CONFIG_FPU is not set
@@ -8,7 +8,6 @@ CONFIG_RWSEM_GENERIC_SPINLOCK=y
 # CONFIG_RWSEM_XCHGADD_ALGORITHM is not set
 CONFIG_BLACKFIN=y
 CONFIG_ZONE_DMA=y
-CONFIG_SEMAPHORE_SLEEPERS=y
 CONFIG_GENERIC_FIND_NEXT_BIT=y
 CONFIG_GENERIC_HWEIGHT=y
 CONFIG_GENERIC_HARDIRQS=y
@@ -31,18 +30,16 @@ CONFIG_SYSVIPC_SYSCTL=y
 # CONFIG_POSIX_MQUEUE is not set
 # CONFIG_BSD_PROCESS_ACCT is not set
 # CONFIG_TASKSTATS is not set
-# CONFIG_USER_NS is not set
-# CONFIG_PID_NS is not set
 # CONFIG_AUDIT is not set
 CONFIG_IKCONFIG=y
 CONFIG_IKCONFIG_PROC=y
 CONFIG_LOG_BUF_SHIFT=14
 # CONFIG_CGROUPS is not set
-CONFIG_FAIR_GROUP_SCHED=y
-CONFIG_FAIR_USER_SCHED=y
-# CONFIG_FAIR_CGROUP_SCHED is not set
-CONFIG_SYSFS_DEPRECATED=y
+# CONFIG_GROUP_SCHED is not set
+# CONFIG_SYSFS_DEPRECATED is not set
+# CONFIG_SYSFS_DEPRECATED_V2 is not set
 # CONFIG_RELAY is not set
+# CONFIG_NAMESPACES is not set
 CONFIG_BLK_DEV_INITRD=y
 CONFIG_INITRAMFS_SOURCE=""
 # CONFIG_CC_OPTIMIZE_FOR_SIZE is not set
@@ -51,26 +48,35 @@ CONFIG_EMBEDDED=y
 CONFIG_UID16=y
 CONFIG_SYSCTL_SYSCALL=y
 CONFIG_KALLSYMS=y
+# CONFIG_KALLSYMS_ALL is not set
 # CONFIG_KALLSYMS_EXTRA_PASS is not set
 CONFIG_HOTPLUG=y
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
+CONFIG_COMPAT_BRK=y
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
 CONFIG_EPOLL=y
 CONFIG_SIGNALFD=y
+CONFIG_TIMERFD=y
 CONFIG_EVENTFD=y
+CONFIG_AIO=y
 CONFIG_VM_EVENT_COUNTERS=y
 CONFIG_SLAB=y
 # CONFIG_SLUB is not set
 # CONFIG_SLOB is not set
+# CONFIG_PROFILING is not set
+# CONFIG_MARKERS is not set
+CONFIG_HAVE_OPROFILE=y
+# CONFIG_HAVE_GENERIC_DMA_COHERENT is not set
 CONFIG_SLABINFO=y
 CONFIG_RT_MUTEXES=y
 CONFIG_TINY_SHMEM=y
 CONFIG_BASE_SMALL=0
 CONFIG_MODULES=y
+# CONFIG_MODULE_FORCE_LOAD is not set
 CONFIG_MODULE_UNLOAD=y
 # CONFIG_MODULE_FORCE_UNLOAD is not set
 # CONFIG_MODVERSIONS is not set
@@ -81,6 +87,7 @@ CONFIG_BLOCK=y
 # CONFIG_BLK_DEV_IO_TRACE is not set
 # CONFIG_LSF is not set
 # CONFIG_BLK_DEV_BSG is not set
+# CONFIG_BLK_DEV_INTEGRITY is not set
 
 #
 # IO Schedulers
@@ -94,9 +101,11 @@ CONFIG_DEFAULT_AS=y
 # CONFIG_DEFAULT_CFQ is not set
 # CONFIG_DEFAULT_NOOP is not set
 CONFIG_DEFAULT_IOSCHED="anticipatory"
+CONFIG_CLASSIC_RCU=y
 # CONFIG_PREEMPT_NONE is not set
 CONFIG_PREEMPT_VOLUNTARY=y
 # CONFIG_PREEMPT is not set
+CONFIG_FREEZER=y
 
 #
 # Blackfin Processor Options
@@ -105,6 +114,10 @@ CONFIG_PREEMPT_VOLUNTARY=y
 #
 # Processor and Board Settings
 #
+# CONFIG_BF512 is not set
+# CONFIG_BF514 is not set
+# CONFIG_BF516 is not set
+# CONFIG_BF518 is not set
 # CONFIG_BF522 is not set
 # CONFIG_BF523 is not set
 # CONFIG_BF524 is not set
@@ -117,24 +130,30 @@ CONFIG_BF533=y
 # CONFIG_BF534 is not set
 # CONFIG_BF536 is not set
 # CONFIG_BF537 is not set
+# CONFIG_BF538 is not set
+# CONFIG_BF539 is not set
 # CONFIG_BF542 is not set
 # CONFIG_BF544 is not set
 # CONFIG_BF547 is not set
 # CONFIG_BF548 is not set
 # CONFIG_BF549 is not set
 # CONFIG_BF561 is not set
+CONFIG_BF_REV_MIN=3
+CONFIG_BF_REV_MAX=6
 # CONFIG_BF_REV_0_0 is not set
 # CONFIG_BF_REV_0_1 is not set
 # CONFIG_BF_REV_0_2 is not set
 CONFIG_BF_REV_0_3=y
 # CONFIG_BF_REV_0_4 is not set
 # CONFIG_BF_REV_0_5 is not set
+# CONFIG_BF_REV_0_6 is not set
 # CONFIG_BF_REV_ANY is not set
 # CONFIG_BF_REV_NONE is not set
 CONFIG_BF53x=y
 CONFIG_MEM_MT48LC64M4A2FB_7E=y
 # CONFIG_BFIN533_EZKIT is not set
 CONFIG_BFIN533_STAMP=y
+# CONFIG_BLACKSTAMP is not set
 # CONFIG_BFIN533_BLUETECHNIX_CM is not set
 # CONFIG_H8606_HVSISTEMAS is not set
 # CONFIG_BFIN532_IP0X is not set
@@ -187,7 +206,6 @@ CONFIG_BOOT_LOAD=0x1000
 #
 CONFIG_CLKIN_HZ=11059200
 # CONFIG_BFIN_KERNEL_CLOCK is not set
-CONFIG_MAX_MEM_SIZE=512
 CONFIG_MAX_VCO_HZ=750000000
 CONFIG_MIN_VCO_HZ=50000000
 CONFIG_MAX_SCLK_HZ=133333333
@@ -201,6 +219,7 @@ CONFIG_HZ_250=y
 # CONFIG_HZ_300 is not set
 # CONFIG_HZ_1000 is not set
 CONFIG_HZ=250
+CONFIG_SCHED_HRTICK=y
 CONFIG_GENERIC_TIME=y
 CONFIG_GENERIC_CLOCKEVENTS=y
 # CONFIG_CYCLES_CLOCKSOURCE is not set
@@ -238,6 +257,12 @@ CONFIG_SYS_BFIN_SPINLOCK_L1=y
 CONFIG_CACHELINE_ALIGNED_L1=y
 # CONFIG_SYSCALL_TAB_L1 is not set
 # CONFIG_CPLB_SWITCH_TAB_L1 is not set
+CONFIG_APP_STACK_L1=y
+
+#
+# Speed Optimizations
+#
+CONFIG_BFIN_INS_LOWOVERHEAD=y
 CONFIG_RAMKERNEL=y
 # CONFIG_ROMKERNEL is not set
 CONFIG_SELECT_MEMORY_MODEL=y
@@ -246,14 +271,13 @@ CONFIG_FLATMEM_MANUAL=y
 # CONFIG_SPARSEMEM_MANUAL is not set
 CONFIG_FLATMEM=y
 CONFIG_FLAT_NODE_MEM_MAP=y
-# CONFIG_SPARSEMEM_STATIC is not set
-# CONFIG_SPARSEMEM_VMEMMAP_ENABLE is not set
+CONFIG_PAGEFLAGS_EXTENDED=y
 CONFIG_SPLIT_PTLOCK_CPUS=4
 # CONFIG_RESOURCES_64BIT is not set
+# CONFIG_PHYS_ADDR_T_64BIT is not set
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_VIRT_TO_BUS=y
 # CONFIG_BFIN_GPTIMERS is not set
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_4M is not set
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
@@ -268,7 +292,6 @@ CONFIG_BFIN_DCACHE=y
 # CONFIG_BFIN_ICACHE_LOCK is not set
 # CONFIG_BFIN_WB is not set
 CONFIG_BFIN_WT=y
-CONFIG_L1_MAX_PIECE=16
 # CONFIG_MPU is not set
 
 #
@@ -297,7 +320,6 @@ CONFIG_BANK_3=0xAAC2
 #
 # Bus options (PCI, PCMCIA, EISA, MCA, ISA)
 #
-# CONFIG_PCI is not set
 # CONFIG_ARCH_SUPPORTS_MSI is not set
 # CONFIG_PCCARD is not set
 
@@ -308,29 +330,30 @@ CONFIG_BINFMT_ELF_FDPIC=y
 CONFIG_BINFMT_FLAT=y
 CONFIG_BINFMT_ZFLAT=y
 # CONFIG_BINFMT_SHARED_FLAT is not set
+# CONFIG_HAVE_AOUT is not set
 # CONFIG_BINFMT_MISC is not set
 
 #
 # Power management options
 #
 CONFIG_PM=y
-# CONFIG_PM_LEGACY is not set
 # CONFIG_PM_DEBUG is not set
 CONFIG_PM_SLEEP=y
-CONFIG_SUSPEND_UP_POSSIBLE=y
 CONFIG_SUSPEND=y
+CONFIG_SUSPEND_FREEZER=y
+CONFIG_ARCH_SUSPEND_POSSIBLE=y
 CONFIG_PM_BFIN_SLEEP_DEEPER=y
 # CONFIG_PM_BFIN_SLEEP is not set
 # CONFIG_PM_WAKEUP_BY_GPIO is not set
 
 #
-# CPU Frequency scaling
+# Possible Suspend Mem / Hibernate Wake-Up Sources
 #
-# CONFIG_CPU_FREQ is not set
 
 #
-# Networking
+# CPU Frequency scaling
 #
+# CONFIG_CPU_FREQ is not set
 CONFIG_NET=y
 
 #
@@ -343,6 +366,7 @@ CONFIG_XFRM=y
 # CONFIG_XFRM_USER is not set
 # CONFIG_XFRM_SUB_POLICY is not set
 # CONFIG_XFRM_MIGRATE is not set
+# CONFIG_XFRM_STATISTICS is not set
 # CONFIG_NET_KEY is not set
 CONFIG_INET=y
 # CONFIG_IP_MULTICAST is not set
@@ -372,8 +396,6 @@ CONFIG_TCP_CONG_CUBIC=y
 CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_TCP_MD5SIG is not set
 # CONFIG_IPV6 is not set
-# CONFIG_INET6_XFRM_TUNNEL is not set
-# CONFIG_INET6_TUNNEL is not set
 # CONFIG_NETLABEL is not set
 # CONFIG_NETWORK_SECMARK is not set
 # CONFIG_NETFILTER is not set
@@ -382,6 +404,7 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_TIPC is not set
 # CONFIG_ATM is not set
 # CONFIG_BRIDGE is not set
+# CONFIG_NET_DSA is not set
 # CONFIG_VLAN_8021Q is not set
 # CONFIG_DECNET is not set
 # CONFIG_LLC2 is not set
@@ -398,6 +421,7 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 #
 # CONFIG_NET_PKTGEN is not set
 # CONFIG_HAMRADIO is not set
+# CONFIG_CAN is not set
 CONFIG_IRDA=m
 
 #
@@ -432,24 +456,14 @@ CONFIG_SIR_BFIN_DMA=y
 # CONFIG_DONGLE is not set
 
 #
-# Old SIR device drivers
-#
-# CONFIG_IRPORT_SIR is not set
-
-#
-# Old Serial dongle support
-#
-
-#
 # FIR device drivers
 #
 # CONFIG_BT is not set
 # CONFIG_AF_RXRPC is not set
-
-#
-# Wireless
-#
+# CONFIG_PHONET is not set
+CONFIG_WIRELESS=y
 # CONFIG_CFG80211 is not set
+CONFIG_WIRELESS_OLD_REGULATORY=y
 # CONFIG_WIRELESS_EXT is not set
 # CONFIG_MAC80211 is not set
 # CONFIG_IEEE80211 is not set
@@ -467,6 +481,8 @@ CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
 CONFIG_STANDALONE=y
 CONFIG_PREVENT_FIRMWARE_BUILD=y
 # CONFIG_FW_LOADER is not set
+# CONFIG_DEBUG_DRIVER is not set
+# CONFIG_DEBUG_DEVRES is not set
 # CONFIG_SYS_HYPERVISOR is not set
 # CONFIG_CONNECTOR is not set
 CONFIG_MTD=y
@@ -475,6 +491,7 @@ CONFIG_MTD=y
 CONFIG_MTD_PARTITIONS=y
 # CONFIG_MTD_REDBOOT_PARTS is not set
 CONFIG_MTD_CMDLINE_PARTS=y
+# CONFIG_MTD_AR7_PARTS is not set
 
 #
 # User Modules And Translation Layers
@@ -520,6 +537,7 @@ CONFIG_MTD_ROM=m
 CONFIG_MTD_COMPLEX_MAPPINGS=y
 # CONFIG_MTD_PHYSMAP is not set
 CONFIG_MTD_BFIN_ASYNC=m
+# CONFIG_MTD_GPIO_ADDR is not set
 # CONFIG_MTD_UCLINUX is not set
 # CONFIG_MTD_PLATRAM is not set
 
@@ -554,11 +572,14 @@ CONFIG_BLK_DEV=y
 CONFIG_BLK_DEV_RAM=y
 CONFIG_BLK_DEV_RAM_COUNT=16
 CONFIG_BLK_DEV_RAM_SIZE=4096
-CONFIG_BLK_DEV_RAM_BLOCKSIZE=1024
+# CONFIG_BLK_DEV_XIP is not set
 # CONFIG_CDROM_PKTCDVD is not set
 # CONFIG_ATA_OVER_ETH is not set
+# CONFIG_BLK_DEV_HD is not set
 CONFIG_MISC_DEVICES=y
 # CONFIG_EEPROM_93CX6 is not set
+# CONFIG_ENCLOSURE_SERVICES is not set
+CONFIG_HAVE_IDE=y
 # CONFIG_IDE is not set
 
 #
@@ -571,7 +592,6 @@ CONFIG_MISC_DEVICES=y
 # CONFIG_ATA is not set
 # CONFIG_MD is not set
 CONFIG_NETDEVICES=y
-# CONFIG_NETDEVICES_MULTIQUEUE is not set
 # CONFIG_DUMMY is not set
 # CONFIG_BONDING is not set
 # CONFIG_MACVLAN is not set
@@ -584,11 +604,14 @@ CONFIG_MII=y
 CONFIG_SMC91X=y
 # CONFIG_SMSC911X is not set
 # CONFIG_DM9000 is not set
+# CONFIG_ENC28J60 is not set
 # CONFIG_IBM_NEW_EMAC_ZMII is not set
 # CONFIG_IBM_NEW_EMAC_RGMII is not set
 # CONFIG_IBM_NEW_EMAC_TAH is not set
 # CONFIG_IBM_NEW_EMAC_EMAC4 is not set
-# CONFIG_B44 is not set
+# CONFIG_IBM_NEW_EMAC_NO_FLOW_CTRL is not set
+# CONFIG_IBM_NEW_EMAC_MAL_CLR_ICINTSTAT is not set
+# CONFIG_IBM_NEW_EMAC_MAL_COMMON_ERR is not set
 CONFIG_NETDEV_1000=y
 # CONFIG_AX88180 is not set
 CONFIG_NETDEV_10000=y
@@ -598,10 +621,10 @@ CONFIG_NETDEV_10000=y
 #
 # CONFIG_WLAN_PRE80211 is not set
 # CONFIG_WLAN_80211 is not set
+# CONFIG_IWLWIFI_LEDS is not set
 # CONFIG_WAN is not set
 # CONFIG_PPP is not set
 # CONFIG_SLIP is not set
-# CONFIG_SHAPER is not set
 # CONFIG_NETCONSOLE is not set
 # CONFIG_NETPOLL is not set
 # CONFIG_NET_POLL_CONTROLLER is not set
@@ -633,7 +656,7 @@ CONFIG_INPUT_EVDEV=m
 # CONFIG_INPUT_TOUCHSCREEN is not set
 CONFIG_INPUT_MISC=y
 # CONFIG_INPUT_UINPUT is not set
-CONFIG_TWI_KEYPAD=m
+CONFIG_CONFIG_INPUT_PCF8574=m
 
 #
 # Hardware I/O ports
@@ -652,8 +675,11 @@ CONFIG_TWI_KEYPAD=m
 CONFIG_BFIN_SPORT=y
 # CONFIG_BFIN_TIMER_LATENCY is not set
 CONFIG_TWI_LCD=m
+CONFIG_BFIN_DMA_INTERFACE=m
 CONFIG_SIMPLE_GPIO=m
 # CONFIG_VT is not set
+# CONFIG_DEVKMEM is not set
+# CONFIG_BFIN_JTAG_COMM is not set
 # CONFIG_SERIAL_NONSTANDARD is not set
 
 #
@@ -682,41 +708,46 @@ CONFIG_UNIX98_PTYS=y
 # CONFIG_CAN4LINUX is not set
 # CONFIG_IPMI_HANDLER is not set
 # CONFIG_HW_RANDOM is not set
-# CONFIG_GEN_RTC is not set
 # CONFIG_R3964 is not set
 # CONFIG_RAW_DRIVER is not set
 # CONFIG_TCG_TPM is not set
 CONFIG_I2C=m
 CONFIG_I2C_BOARDINFO=y
 CONFIG_I2C_CHARDEV=m
+CONFIG_I2C_HELPER_AUTO=y
 
 #
-# I2C Algorithms
+# I2C Hardware Bus support
 #
-CONFIG_I2C_ALGOBIT=m
-# CONFIG_I2C_ALGOPCF is not set
-# CONFIG_I2C_ALGOPCA is not set
 
 #
-# I2C Hardware Bus support
+# I2C system bus drivers (mostly embedded / system-on-chip)
 #
 # CONFIG_I2C_GPIO is not set
 # CONFIG_I2C_OCORES is not set
-# CONFIG_I2C_PARPORT_LIGHT is not set
 # CONFIG_I2C_SIMTEC is not set
+
+#
+# External I2C/SMBus adapter drivers
+#
+# CONFIG_I2C_PARPORT_LIGHT is not set
 # CONFIG_I2C_TAOS_EVM is not set
+
+#
+# Other I2C/SMBus bus drivers
+#
+# CONFIG_I2C_PCA_PLATFORM is not set
 # CONFIG_I2C_STUB is not set
 
 #
 # Miscellaneous I2C Chip support
 #
-# CONFIG_SENSORS_DS1337 is not set
-# CONFIG_SENSORS_DS1374 is not set
 # CONFIG_DS1682 is not set
+# CONFIG_AT24 is not set
 # CONFIG_SENSORS_AD5252 is not set
 # CONFIG_SENSORS_EEPROM is not set
 # CONFIG_SENSORS_PCF8574 is not set
-# CONFIG_SENSORS_PCF8575 is not set
+# CONFIG_PCF8575 is not set
 # CONFIG_SENSORS_PCA9539 is not set
 # CONFIG_SENSORS_PCF8591 is not set
 # CONFIG_SENSORS_MAX6875 is not set
@@ -725,17 +756,15 @@ CONFIG_I2C_ALGOBIT=m
 # CONFIG_I2C_DEBUG_ALGO is not set
 # CONFIG_I2C_DEBUG_BUS is not set
 # CONFIG_I2C_DEBUG_CHIP is not set
-
-#
-# SPI support
-#
 CONFIG_SPI=y
+# CONFIG_SPI_DEBUG is not set
 CONFIG_SPI_MASTER=y
 
 #
 # SPI Master Controller Drivers
 #
 CONFIG_SPI_BFIN=y
+# CONFIG_SPI_BFIN_LOCK is not set
 # CONFIG_SPI_BITBANG is not set
 
 #
@@ -744,11 +773,15 @@ CONFIG_SPI_BFIN=y
 # CONFIG_SPI_AT25 is not set
 # CONFIG_SPI_SPIDEV is not set
 # CONFIG_SPI_TLE62X0 is not set
+CONFIG_ARCH_WANT_OPTIONAL_GPIOLIB=y
+# CONFIG_GPIOLIB is not set
 # CONFIG_W1 is not set
 # CONFIG_POWER_SUPPLY is not set
 CONFIG_HWMON=y
 # CONFIG_HWMON_VID is not set
+# CONFIG_SENSORS_AD7414 is not set
 # CONFIG_SENSORS_AD7418 is not set
+# CONFIG_SENSORS_ADCXX is not set
 # CONFIG_SENSORS_ADM1021 is not set
 # CONFIG_SENSORS_ADM1025 is not set
 # CONFIG_SENSORS_ADM1026 is not set
@@ -756,6 +789,7 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_ADM1031 is not set
 # CONFIG_SENSORS_ADM9240 is not set
 # CONFIG_SENSORS_ADT7470 is not set
+# CONFIG_SENSORS_ADT7473 is not set
 # CONFIG_SENSORS_ATXP1 is not set
 # CONFIG_SENSORS_DS1621 is not set
 # CONFIG_SENSORS_F71805F is not set
@@ -776,6 +810,7 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_LM90 is not set
 # CONFIG_SENSORS_LM92 is not set
 # CONFIG_SENSORS_LM93 is not set
+# CONFIG_SENSORS_MAX1111 is not set
 # CONFIG_SENSORS_MAX1619 is not set
 # CONFIG_SENSORS_MAX6650 is not set
 # CONFIG_SENSORS_PC87360 is not set
@@ -784,6 +819,7 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_SMSC47M1 is not set
 # CONFIG_SENSORS_SMSC47M192 is not set
 # CONFIG_SENSORS_SMSC47B397 is not set
+# CONFIG_SENSORS_ADS7828 is not set
 # CONFIG_SENSORS_THMC50 is not set
 # CONFIG_SENSORS_VT1211 is not set
 # CONFIG_SENSORS_W83781D is not set
@@ -791,9 +827,12 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_W83792D is not set
 # CONFIG_SENSORS_W83793 is not set
 # CONFIG_SENSORS_W83L785TS is not set
+# CONFIG_SENSORS_W83L786NG is not set
 # CONFIG_SENSORS_W83627HF is not set
 # CONFIG_SENSORS_W83627EHF is not set
 # CONFIG_HWMON_DEBUG_CHIP is not set
+# CONFIG_THERMAL is not set
+# CONFIG_THERMAL_HWMON is not set
 CONFIG_WATCHDOG=y
 # CONFIG_WATCHDOG_NOWAYOUT is not set
 
@@ -804,21 +843,29 @@ CONFIG_WATCHDOG=y
 CONFIG_BFIN_WDT=y
 
 #
-# Sonics Silicon Backplane
-#
-CONFIG_SSB_POSSIBLE=y
-# CONFIG_SSB is not set
-
-#
 # Multifunction device drivers
 #
+# CONFIG_MFD_CORE is not set
 # CONFIG_MFD_SM501 is not set
+# CONFIG_HTC_PASIC3 is not set
+# CONFIG_MFD_TMIO is not set
+# CONFIG_MFD_WM8400 is not set
+# CONFIG_MFD_WM8350_I2C is not set
 
 #
 # Multimedia devices
 #
+
+#
+# Multimedia core support
+#
 # CONFIG_VIDEO_DEV is not set
 # CONFIG_DVB_CORE is not set
+# CONFIG_VIDEO_MEDIA is not set
+
+#
+# Multimedia drivers
+#
 # CONFIG_DAB is not set
 
 #
@@ -829,6 +876,7 @@ CONFIG_SSB_POSSIBLE=y
 CONFIG_FB=m
 CONFIG_FIRMWARE_EDID=y
 # CONFIG_FB_DDC is not set
+# CONFIG_FB_BOOT_VESA_SUPPORT is not set
 CONFIG_FB_CFB_FILLRECT=m
 CONFIG_FB_CFB_COPYAREA=m
 CONFIG_FB_CFB_IMAGEBLIT=m
@@ -836,8 +884,8 @@ CONFIG_FB_CFB_IMAGEBLIT=m
 # CONFIG_FB_SYS_FILLRECT is not set
 # CONFIG_FB_SYS_COPYAREA is not set
 # CONFIG_FB_SYS_IMAGEBLIT is not set
+# CONFIG_FB_FOREIGN_ENDIAN is not set
 # CONFIG_FB_SYS_FOPS is not set
-CONFIG_FB_DEFERRED_IO=y
 # CONFIG_FB_SVGALIB is not set
 # CONFIG_FB_MACMODES is not set
 # CONFIG_FB_BACKLIGHT is not set
@@ -848,6 +896,7 @@ CONFIG_FB_DEFERRED_IO=y
 # Frame buffer hardware drivers
 #
 # CONFIG_FB_BFIN_T350MCQB is not set
+# CONFIG_FB_BFIN_LQ035Q1 is not set
 CONFIG_FB_BFIN_7393=m
 CONFIG_NTSC=y
 # CONFIG_PAL is not set
@@ -859,6 +908,7 @@ CONFIG_ADV7393_1XMEM=y
 # CONFIG_ADV7393_2XMEM is not set
 # CONFIG_FB_S1D13XXX is not set
 # CONFIG_FB_VIRTUAL is not set
+# CONFIG_FB_METRONOME is not set
 # CONFIG_BACKLIGHT_LCD_SUPPORT is not set
 
 #
@@ -866,15 +916,8 @@ CONFIG_ADV7393_1XMEM=y
 #
 # CONFIG_DISPLAY_SUPPORT is not set
 # CONFIG_LOGO is not set
-
-#
-# Sound
-#
 CONFIG_SOUND=m
-
-#
-# Advanced Linux Sound Architecture
-#
+CONFIG_SOUND_OSS_CORE=y
 CONFIG_SND=m
 CONFIG_SND_TIMER=m
 CONFIG_SND_PCM=m
@@ -888,18 +931,12 @@ CONFIG_SND_SUPPORT_OLD_API=y
 CONFIG_SND_VERBOSE_PROCFS=y
 # CONFIG_SND_VERBOSE_PRINTK is not set
 # CONFIG_SND_DEBUG is not set
-
-#
-# Generic devices
-#
+CONFIG_SND_DRIVERS=y
 # CONFIG_SND_DUMMY is not set
 # CONFIG_SND_MTPAV is not set
 # CONFIG_SND_SERIAL_U16550 is not set
 # CONFIG_SND_MPU401 is not set
-
-#
-# SPI devices
-#
+CONFIG_SND_SPI=y
 
 #
 # ALSA Blackfin devices
@@ -911,46 +948,46 @@ CONFIG_SND_BLACKFIN_AD1836_MULSUB=y
 # CONFIG_SND_BLACKFIN_AD1836_5P1 is not set
 CONFIG_SND_BLACKFIN_SPORT=0
 CONFIG_SND_BLACKFIN_SPI_PFBIT=4
-CONFIG_SND_BFIN_AD73311=m
 CONFIG_SND_BFIN_SPORT=0
-CONFIG_SND_BFIN_AD73311_SE=4
 CONFIG_SND_BFIN_AD73322=m
 CONFIG_SND_BFIN_AD73322_SPORT0_SE=10
 CONFIG_SND_BFIN_AD73322_SPORT1_SE=14
 CONFIG_SND_BFIN_AD73322_RESET=12
-
-#
-# System on Chip audio support
-#
-CONFIG_SND_SOC_AC97_BUS=y
 CONFIG_SND_SOC=m
-CONFIG_SND_BF5XX_SOC=m
-CONFIG_SND_MMAP_SUPPORT=y
-CONFIG_SND_BF5XX_SOC_AC97=m
-# CONFIG_SND_BF5XX_SOC_WM8750 is not set
-# CONFIG_SND_BF5XX_SOC_WM8731 is not set
+CONFIG_SND_SOC_AC97_BUS=y
+CONFIG_SND_BF5XX_I2S=m
 # CONFIG_SND_BF5XX_SOC_SSM2602 is not set
-CONFIG_SND_BF5XX_SOC_BF5xx=m
+CONFIG_SND_BF5XX_SOC_AD73311=m
+CONFIG_SND_BFIN_AD73311_SE=4
+CONFIG_SND_BF5XX_AC97=m
+CONFIG_SND_BF5XX_MMAP_SUPPORT=y
+# CONFIG_SND_BF5XX_MULTICHAN_SUPPORT is not set
+CONFIG_SND_BF5XX_SOC_SPORT=m
+CONFIG_SND_BF5XX_SOC_I2S=m
+CONFIG_SND_BF5XX_SOC_AC97=m
+CONFIG_SND_BF5XX_SOC_AD1980=m
 CONFIG_SND_BF5XX_SPORT_NUM=0
 # CONFIG_SND_BF5XX_HAVE_COLD_RESET is not set
-
-#
-# SoC Audio support for SuperH
-#
+# CONFIG_SND_SOC_ALL_CODECS is not set
 CONFIG_SND_SOC_AD1980=m
-
-#
-# Open Sound System
-#
+CONFIG_SND_SOC_AD73311=m
 # CONFIG_SOUND_PRIME is not set
 CONFIG_AC97_BUS=m
 CONFIG_HID_SUPPORT=y
 CONFIG_HID=y
 # CONFIG_HID_DEBUG is not set
 # CONFIG_HIDRAW is not set
+# CONFIG_HID_PID is not set
+
+#
+# Special HID drivers
+#
+CONFIG_HID_COMPAT=y
 # CONFIG_USB_SUPPORT is not set
 # CONFIG_MMC is not set
+# CONFIG_MEMSTICK is not set
 # CONFIG_NEW_LEDS is not set
+# CONFIG_ACCESSIBILITY is not set
 CONFIG_RTC_LIB=y
 CONFIG_RTC_CLASS=y
 CONFIG_RTC_HCTOSYS=y
@@ -979,51 +1016,57 @@ CONFIG_RTC_INTF_DEV=y
 # CONFIG_RTC_DRV_PCF8563 is not set
 # CONFIG_RTC_DRV_PCF8583 is not set
 # CONFIG_RTC_DRV_M41T80 is not set
+# CONFIG_RTC_DRV_S35390A is not set
+# CONFIG_RTC_DRV_FM3130 is not set
 
 #
 # SPI RTC drivers
 #
-# CONFIG_RTC_DRV_RS5C348 is not set
+# CONFIG_RTC_DRV_M41T94 is not set
+# CONFIG_RTC_DRV_DS1305 is not set
 # CONFIG_RTC_DRV_MAX6902 is not set
+# CONFIG_RTC_DRV_R9701 is not set
+# CONFIG_RTC_DRV_RS5C348 is not set
+# CONFIG_RTC_DRV_DS3234 is not set
 
 #
 # Platform RTC drivers
 #
+# CONFIG_RTC_DRV_DS1286 is not set
+# CONFIG_RTC_DRV_DS1511 is not set
 # CONFIG_RTC_DRV_DS1553 is not set
-# CONFIG_RTC_DRV_STK17TA8 is not set
 # CONFIG_RTC_DRV_DS1742 is not set
+# CONFIG_RTC_DRV_STK17TA8 is not set
 # CONFIG_RTC_DRV_M48T86 is not set
+# CONFIG_RTC_DRV_M48T35 is not set
 # CONFIG_RTC_DRV_M48T59 is not set
+# CONFIG_RTC_DRV_BQ4802 is not set
 # CONFIG_RTC_DRV_V3020 is not set
 
 #
 # on-CPU RTC drivers
 #
 CONFIG_RTC_DRV_BFIN=y
-
-#
-# Userspace I/O
-#
+# CONFIG_DMADEVICES is not set
 # CONFIG_UIO is not set
+# CONFIG_STAGING is not set
 
 #
 # File systems
 #
 # CONFIG_EXT2_FS is not set
 # CONFIG_EXT3_FS is not set
-# CONFIG_EXT4DEV_FS is not set
+# CONFIG_EXT4_FS is not set
 # CONFIG_REISERFS_FS is not set
 # CONFIG_JFS_FS is not set
 # CONFIG_FS_POSIX_ACL is not set
+CONFIG_FILE_LOCKING=y
 # CONFIG_XFS_FS is not set
-# CONFIG_GFS2_FS is not set
 # CONFIG_OCFS2_FS is not set
-# CONFIG_MINIX_FS is not set
-# CONFIG_ROMFS_FS is not set
+# CONFIG_DNOTIFY is not set
 CONFIG_INOTIFY=y
 CONFIG_INOTIFY_USER=y
 # CONFIG_QUOTA is not set
-# CONFIG_DNOTIFY is not set
 # CONFIG_AUTOFS_FS is not set
 # CONFIG_AUTOFS4_FS is not set
 # CONFIG_FUSE_FS is not set
@@ -1063,11 +1106,11 @@ CONFIG_SYSFS=y
 # CONFIG_EFS_FS is not set
 CONFIG_YAFFS_FS=m
 CONFIG_YAFFS_YAFFS1=y
+# CONFIG_YAFFS_9BYTE_TAGS is not set
 # CONFIG_YAFFS_DOES_ECC is not set
 CONFIG_YAFFS_YAFFS2=y
 CONFIG_YAFFS_AUTO_YAFFS2=y
 # CONFIG_YAFFS_DISABLE_LAZY_LOAD is not set
-CONFIG_YAFFS_CHECKPOINT_RESERVED_BLOCKS=10
 # CONFIG_YAFFS_DISABLE_WIDE_TNODES is not set
 # CONFIG_YAFFS_ALWAYS_CHECK_CHUNK_ERASED is not set
 CONFIG_YAFFS_SHORT_NAMES_IN_RAM=y
@@ -1084,8 +1127,11 @@ CONFIG_JFFS2_RTIME=y
 # CONFIG_JFFS2_RUBIN is not set
 # CONFIG_CRAMFS is not set
 # CONFIG_VXFS_FS is not set
+# CONFIG_MINIX_FS is not set
+# CONFIG_OMFS_FS is not set
 # CONFIG_HPFS_FS is not set
 # CONFIG_QNX4FS_FS is not set
+# CONFIG_ROMFS_FS is not set
 # CONFIG_SYSV_FS is not set
 # CONFIG_UFS_FS is not set
 CONFIG_NETWORK_FILESYSTEMS=y
@@ -1093,13 +1139,12 @@ CONFIG_NFS_FS=m
 CONFIG_NFS_V3=y
 # CONFIG_NFS_V3_ACL is not set
 # CONFIG_NFS_V4 is not set
-# CONFIG_NFS_DIRECTIO is not set
 # CONFIG_NFSD is not set
 CONFIG_LOCKD=m
 CONFIG_LOCKD_V4=y
 CONFIG_NFS_COMMON=y
 CONFIG_SUNRPC=m
-# CONFIG_SUNRPC_BIND34 is not set
+# CONFIG_SUNRPC_REGISTER_V4 is not set
 # CONFIG_RPCSEC_GSS_KRB5 is not set
 # CONFIG_RPCSEC_GSS_SPKM3 is not set
 CONFIG_SMB_FS=m
@@ -1155,9 +1200,6 @@ CONFIG_NLS_DEFAULT="iso8859-1"
 # CONFIG_NLS_KOI8_U is not set
 # CONFIG_NLS_UTF8 is not set
 # CONFIG_DLM is not set
-CONFIG_INSTRUMENTATION=y
-# CONFIG_PROFILING is not set
-# CONFIG_MARKERS is not set
 
 #
 # Kernel hacking
@@ -1165,14 +1207,53 @@ CONFIG_INSTRUMENTATION=y
 # CONFIG_PRINTK_TIME is not set
 CONFIG_ENABLE_WARN_DEPRECATED=y
 CONFIG_ENABLE_MUST_CHECK=y
+CONFIG_FRAME_WARN=1024
 # CONFIG_MAGIC_SYSRQ is not set
 # CONFIG_UNUSED_SYMBOLS is not set
 CONFIG_DEBUG_FS=y
 # CONFIG_HEADERS_CHECK is not set
-# CONFIG_DEBUG_KERNEL is not set
+CONFIG_DEBUG_KERNEL=y
+# CONFIG_DEBUG_SHIRQ is not set
+CONFIG_DETECT_SOFTLOCKUP=y
+# CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC is not set
+CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC_VALUE=0
+CONFIG_SCHED_DEBUG=y
+# CONFIG_SCHEDSTATS is not set
+# CONFIG_TIMER_STATS is not set
+# CONFIG_DEBUG_OBJECTS is not set
+# CONFIG_DEBUG_SLAB is not set
+# CONFIG_DEBUG_RT_MUTEXES is not set
+# CONFIG_RT_MUTEX_TESTER is not set
+# CONFIG_DEBUG_SPINLOCK is not set
+# CONFIG_DEBUG_MUTEXES is not set
+# CONFIG_DEBUG_SPINLOCK_SLEEP is not set
+# CONFIG_DEBUG_LOCKING_API_SELFTESTS is not set
+# CONFIG_DEBUG_KOBJECT is not set
 # CONFIG_DEBUG_BUGVERBOSE is not set
+CONFIG_DEBUG_INFO=y
+# CONFIG_DEBUG_VM is not set
+# CONFIG_DEBUG_WRITECOUNT is not set
+# CONFIG_DEBUG_MEMORY_INIT is not set
+# CONFIG_DEBUG_LIST is not set
+# CONFIG_DEBUG_SG is not set
+# CONFIG_FRAME_POINTER is not set
+# CONFIG_BOOT_PRINTK_DELAY is not set
+# CONFIG_RCU_TORTURE_TEST is not set
+# CONFIG_RCU_CPU_STALL_DETECTOR is not set
+# CONFIG_BACKTRACE_SELF_TEST is not set
+# CONFIG_DEBUG_BLOCK_EXT_DEVT is not set
+# CONFIG_FAULT_INJECTION is not set
+# CONFIG_SYSCTL_SYSCALL_CHECK is not set
+# CONFIG_DYNAMIC_PRINTK_DEBUG is not set
 # CONFIG_SAMPLES is not set
+CONFIG_HAVE_ARCH_KGDB=y
+# CONFIG_KGDB is not set
+# CONFIG_DEBUG_STACKOVERFLOW is not set
+# CONFIG_DEBUG_STACK_USAGE is not set
+CONFIG_DEBUG_VERBOSE=y
 CONFIG_DEBUG_MMRS=y
+# CONFIG_DEBUG_HWERR is not set
+# CONFIG_DEBUG_DOUBLEFAULT is not set
 CONFIG_DEBUG_HUNT_FOR_ZERO=y
 CONFIG_DEBUG_BFIN_HWTRACE_ON=y
 CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION_OFF=y
@@ -1190,9 +1271,94 @@ CONFIG_ACCESS_CHECK=y
 #
 # CONFIG_KEYS is not set
 CONFIG_SECURITY=y
+# CONFIG_SECURITYFS is not set
 # CONFIG_SECURITY_NETWORK is not set
-# CONFIG_SECURITY_CAPABILITIES is not set
-# CONFIG_CRYPTO is not set
+# CONFIG_SECURITY_FILE_CAPABILITIES is not set
+CONFIG_SECURITY_DEFAULT_MMAP_MIN_ADDR=0
+CONFIG_CRYPTO=y
+
+#
+# Crypto core or helper
+#
+# CONFIG_CRYPTO_FIPS is not set
+# CONFIG_CRYPTO_MANAGER is not set
+# CONFIG_CRYPTO_GF128MUL is not set
+# CONFIG_CRYPTO_NULL is not set
+# CONFIG_CRYPTO_CRYPTD is not set
+# CONFIG_CRYPTO_AUTHENC is not set
+# CONFIG_CRYPTO_TEST is not set
+
+#
+# Authenticated Encryption with Associated Data
+#
+# CONFIG_CRYPTO_CCM is not set
+# CONFIG_CRYPTO_GCM is not set
+# CONFIG_CRYPTO_SEQIV is not set
+
+#
+# Block modes
+#
+# CONFIG_CRYPTO_CBC is not set
+# CONFIG_CRYPTO_CTR is not set
+# CONFIG_CRYPTO_CTS is not set
+# CONFIG_CRYPTO_ECB is not set
+# CONFIG_CRYPTO_LRW is not set
+# CONFIG_CRYPTO_PCBC is not set
+# CONFIG_CRYPTO_XTS is not set
+
+#
+# Hash modes
+#
+# CONFIG_CRYPTO_HMAC is not set
+# CONFIG_CRYPTO_XCBC is not set
+
+#
+# Digest
+#
+# CONFIG_CRYPTO_CRC32C is not set
+# CONFIG_CRYPTO_MD4 is not set
+# CONFIG_CRYPTO_MD5 is not set
+# CONFIG_CRYPTO_MICHAEL_MIC is not set
+# CONFIG_CRYPTO_RMD128 is not set
+# CONFIG_CRYPTO_RMD160 is not set
+# CONFIG_CRYPTO_RMD256 is not set
+# CONFIG_CRYPTO_RMD320 is not set
+# CONFIG_CRYPTO_SHA1 is not set
+# CONFIG_CRYPTO_SHA256 is not set
+# CONFIG_CRYPTO_SHA512 is not set
+# CONFIG_CRYPTO_TGR192 is not set
+# CONFIG_CRYPTO_WP512 is not set
+
+#
+# Ciphers
+#
+# CONFIG_CRYPTO_AES is not set
+# CONFIG_CRYPTO_ANUBIS is not set
+# CONFIG_CRYPTO_ARC4 is not set
+# CONFIG_CRYPTO_BLOWFISH is not set
+# CONFIG_CRYPTO_CAMELLIA is not set
+# CONFIG_CRYPTO_CAST5 is not set
+# CONFIG_CRYPTO_CAST6 is not set
+# CONFIG_CRYPTO_DES is not set
+# CONFIG_CRYPTO_FCRYPT is not set
+# CONFIG_CRYPTO_KHAZAD is not set
+# CONFIG_CRYPTO_SALSA20 is not set
+# CONFIG_CRYPTO_SEED is not set
+# CONFIG_CRYPTO_SERPENT is not set
+# CONFIG_CRYPTO_TEA is not set
+# CONFIG_CRYPTO_TWOFISH is not set
+
+#
+# Compression
+#
+# CONFIG_CRYPTO_DEFLATE is not set
+# CONFIG_CRYPTO_LZO is not set
+
+#
+# Random Number Generation
+#
+# CONFIG_CRYPTO_ANSI_CPRNG is not set
+CONFIG_CRYPTO_HW=y
 
 #
 # Library routines
@@ -1200,6 +1366,7 @@ CONFIG_SECURITY=y
 CONFIG_BITREVERSE=y
 CONFIG_CRC_CCITT=m
 # CONFIG_CRC16 is not set
+# CONFIG_CRC_T10DIF is not set
 # CONFIG_CRC_ITU_T is not set
 CONFIG_CRC32=y
 # CONFIG_CRC7 is not set
diff --git a/arch/blackfin/configs/BF537-STAMP_defconfig b/arch/blackfin/configs/BF537-STAMP_defconfig
index 63a0f854745c..332142f7f9b4 100644
--- a/arch/blackfin/configs/BF537-STAMP_defconfig
+++ b/arch/blackfin/configs/BF537-STAMP_defconfig
@@ -1,6 +1,7 @@
 #
 # Automatically generated make config: don't edit
-# Linux kernel version: 2.6.24.7
+# Linux kernel version: 2.6.28-rc2
+# Tue Dec 30 17:24:37 2008
 #
 # CONFIG_MMU is not set
 # CONFIG_FPU is not set
@@ -8,7 +9,6 @@ CONFIG_RWSEM_GENERIC_SPINLOCK=y
 # CONFIG_RWSEM_XCHGADD_ALGORITHM is not set
 CONFIG_BLACKFIN=y
 CONFIG_ZONE_DMA=y
-CONFIG_SEMAPHORE_SLEEPERS=y
 CONFIG_GENERIC_FIND_NEXT_BIT=y
 CONFIG_GENERIC_HWEIGHT=y
 CONFIG_GENERIC_HARDIRQS=y
@@ -31,18 +31,16 @@ CONFIG_SYSVIPC_SYSCTL=y
 # CONFIG_POSIX_MQUEUE is not set
 # CONFIG_BSD_PROCESS_ACCT is not set
 # CONFIG_TASKSTATS is not set
-# CONFIG_USER_NS is not set
-# CONFIG_PID_NS is not set
 # CONFIG_AUDIT is not set
 CONFIG_IKCONFIG=y
 CONFIG_IKCONFIG_PROC=y
 CONFIG_LOG_BUF_SHIFT=14
 # CONFIG_CGROUPS is not set
-CONFIG_FAIR_GROUP_SCHED=y
-CONFIG_FAIR_USER_SCHED=y
-# CONFIG_FAIR_CGROUP_SCHED is not set
-CONFIG_SYSFS_DEPRECATED=y
+# CONFIG_GROUP_SCHED is not set
+# CONFIG_SYSFS_DEPRECATED is not set
+# CONFIG_SYSFS_DEPRECATED_V2 is not set
 # CONFIG_RELAY is not set
+# CONFIG_NAMESPACES is not set
 CONFIG_BLK_DEV_INITRD=y
 CONFIG_INITRAMFS_SOURCE=""
 # CONFIG_CC_OPTIMIZE_FOR_SIZE is not set
@@ -51,26 +49,35 @@ CONFIG_EMBEDDED=y
 CONFIG_UID16=y
 CONFIG_SYSCTL_SYSCALL=y
 CONFIG_KALLSYMS=y
+# CONFIG_KALLSYMS_ALL is not set
 # CONFIG_KALLSYMS_EXTRA_PASS is not set
 CONFIG_HOTPLUG=y
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
+CONFIG_COMPAT_BRK=y
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
 CONFIG_EPOLL=y
 CONFIG_SIGNALFD=y
+CONFIG_TIMERFD=y
 CONFIG_EVENTFD=y
+CONFIG_AIO=y
 CONFIG_VM_EVENT_COUNTERS=y
 CONFIG_SLAB=y
 # CONFIG_SLUB is not set
 # CONFIG_SLOB is not set
+# CONFIG_PROFILING is not set
+# CONFIG_MARKERS is not set
+CONFIG_HAVE_OPROFILE=y
+# CONFIG_HAVE_GENERIC_DMA_COHERENT is not set
 CONFIG_SLABINFO=y
 CONFIG_RT_MUTEXES=y
 CONFIG_TINY_SHMEM=y
 CONFIG_BASE_SMALL=0
 CONFIG_MODULES=y
+# CONFIG_MODULE_FORCE_LOAD is not set
 CONFIG_MODULE_UNLOAD=y
 # CONFIG_MODULE_FORCE_UNLOAD is not set
 # CONFIG_MODVERSIONS is not set
@@ -81,6 +88,7 @@ CONFIG_BLOCK=y
 # CONFIG_BLK_DEV_IO_TRACE is not set
 # CONFIG_LSF is not set
 # CONFIG_BLK_DEV_BSG is not set
+# CONFIG_BLK_DEV_INTEGRITY is not set
 
 #
 # IO Schedulers
@@ -94,9 +102,11 @@ CONFIG_DEFAULT_AS=y
 # CONFIG_DEFAULT_CFQ is not set
 # CONFIG_DEFAULT_NOOP is not set
 CONFIG_DEFAULT_IOSCHED="anticipatory"
+CONFIG_CLASSIC_RCU=y
 # CONFIG_PREEMPT_NONE is not set
 CONFIG_PREEMPT_VOLUNTARY=y
 # CONFIG_PREEMPT is not set
+CONFIG_FREEZER=y
 
 #
 # Blackfin Processor Options
@@ -105,6 +115,10 @@ CONFIG_PREEMPT_VOLUNTARY=y
 #
 # Processor and Board Settings
 #
+# CONFIG_BF512 is not set
+# CONFIG_BF514 is not set
+# CONFIG_BF516 is not set
+# CONFIG_BF518 is not set
 # CONFIG_BF522 is not set
 # CONFIG_BF523 is not set
 # CONFIG_BF524 is not set
@@ -117,18 +131,23 @@ CONFIG_PREEMPT_VOLUNTARY=y
 # CONFIG_BF534 is not set
 # CONFIG_BF536 is not set
 CONFIG_BF537=y
+# CONFIG_BF538 is not set
+# CONFIG_BF539 is not set
 # CONFIG_BF542 is not set
 # CONFIG_BF544 is not set
 # CONFIG_BF547 is not set
 # CONFIG_BF548 is not set
 # CONFIG_BF549 is not set
 # CONFIG_BF561 is not set
+CONFIG_BF_REV_MIN=2
+CONFIG_BF_REV_MAX=3
 # CONFIG_BF_REV_0_0 is not set
 # CONFIG_BF_REV_0_1 is not set
 CONFIG_BF_REV_0_2=y
 # CONFIG_BF_REV_0_3 is not set
 # CONFIG_BF_REV_0_4 is not set
 # CONFIG_BF_REV_0_5 is not set
+# CONFIG_BF_REV_0_6 is not set
 # CONFIG_BF_REV_ANY is not set
 # CONFIG_BF_REV_NONE is not set
 CONFIG_BF53x=y
@@ -141,27 +160,28 @@ CONFIG_IRQ_SPORT0_TX=9
 CONFIG_IRQ_SPORT1_RX=9
 CONFIG_IRQ_SPORT1_TX=9
 CONFIG_IRQ_TWI=10
-CONFIG_IRQ_SPI=10
 CONFIG_IRQ_UART0_RX=10
 CONFIG_IRQ_UART0_TX=10
 CONFIG_IRQ_UART1_RX=10
 CONFIG_IRQ_UART1_TX=10
 CONFIG_IRQ_MAC_RX=11
 CONFIG_IRQ_MAC_TX=11
-CONFIG_IRQ_TMR0=12
-CONFIG_IRQ_TMR1=12
-CONFIG_IRQ_TMR2=12
-CONFIG_IRQ_TMR3=12
-CONFIG_IRQ_TMR4=12
-CONFIG_IRQ_TMR5=12
-CONFIG_IRQ_TMR6=12
-CONFIG_IRQ_TMR7=12
+CONFIG_IRQ_TIMER0=8
+CONFIG_IRQ_TIMER1=12
+CONFIG_IRQ_TIMER2=12
+CONFIG_IRQ_TIMER3=12
+CONFIG_IRQ_TIMER4=12
+CONFIG_IRQ_TIMER5=12
+CONFIG_IRQ_TIMER6=12
+CONFIG_IRQ_TIMER7=12
 CONFIG_IRQ_PORTG_INTB=12
 CONFIG_IRQ_MEM_DMA0=13
 CONFIG_IRQ_MEM_DMA1=13
 CONFIG_IRQ_WATCH=13
+CONFIG_IRQ_SPI=10
 CONFIG_BFIN537_STAMP=y
 # CONFIG_BFIN537_BLUETECHNIX_CM is not set
+# CONFIG_BFIN537_BLUETECHNIX_TCM is not set
 # CONFIG_PNAV10 is not set
 # CONFIG_CAMSIG_MINOTAUR is not set
 # CONFIG_GENERIC_BF537_BOARD is not set
@@ -194,7 +214,6 @@ CONFIG_BOOT_LOAD=0x1000
 #
 CONFIG_CLKIN_HZ=25000000
 # CONFIG_BFIN_KERNEL_CLOCK is not set
-CONFIG_MAX_MEM_SIZE=512
 CONFIG_MAX_VCO_HZ=600000000
 CONFIG_MIN_VCO_HZ=50000000
 CONFIG_MAX_SCLK_HZ=133333333
@@ -208,6 +227,7 @@ CONFIG_HZ_250=y
 # CONFIG_HZ_300 is not set
 # CONFIG_HZ_1000 is not set
 CONFIG_HZ=250
+CONFIG_SCHED_HRTICK=y
 CONFIG_GENERIC_TIME=y
 CONFIG_GENERIC_CLOCKEVENTS=y
 # CONFIG_CYCLES_CLOCKSOURCE is not set
@@ -245,6 +265,12 @@ CONFIG_SYS_BFIN_SPINLOCK_L1=y
 CONFIG_CACHELINE_ALIGNED_L1=y
 # CONFIG_SYSCALL_TAB_L1 is not set
 # CONFIG_CPLB_SWITCH_TAB_L1 is not set
+CONFIG_APP_STACK_L1=y
+
+#
+# Speed Optimizations
+#
+CONFIG_BFIN_INS_LOWOVERHEAD=y
 CONFIG_RAMKERNEL=y
 # CONFIG_ROMKERNEL is not set
 CONFIG_SELECT_MEMORY_MODEL=y
@@ -253,14 +279,13 @@ CONFIG_FLATMEM_MANUAL=y
 # CONFIG_SPARSEMEM_MANUAL is not set
 CONFIG_FLATMEM=y
 CONFIG_FLAT_NODE_MEM_MAP=y
-# CONFIG_SPARSEMEM_STATIC is not set
-# CONFIG_SPARSEMEM_VMEMMAP_ENABLE is not set
+CONFIG_PAGEFLAGS_EXTENDED=y
 CONFIG_SPLIT_PTLOCK_CPUS=4
 # CONFIG_RESOURCES_64BIT is not set
+# CONFIG_PHYS_ADDR_T_64BIT is not set
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_VIRT_TO_BUS=y
-# CONFIG_BFIN_GPTIMERS is not set
-CONFIG_BFIN_DMA_5XX=y
+CONFIG_BFIN_GPTIMERS=m
 # CONFIG_DMA_UNCACHED_4M is not set
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
@@ -275,7 +300,6 @@ CONFIG_BFIN_DCACHE=y
 # CONFIG_BFIN_ICACHE_LOCK is not set
 # CONFIG_BFIN_WB is not set
 CONFIG_BFIN_WT=y
-CONFIG_L1_MAX_PIECE=16
 # CONFIG_MPU is not set
 
 #
@@ -304,7 +328,6 @@ CONFIG_BANK_3=0x99B2
 #
 # Bus options (PCI, PCMCIA, EISA, MCA, ISA)
 #
-# CONFIG_PCI is not set
 # CONFIG_ARCH_SUPPORTS_MSI is not set
 # CONFIG_PCCARD is not set
 
@@ -315,29 +338,31 @@ CONFIG_BINFMT_ELF_FDPIC=y
 CONFIG_BINFMT_FLAT=y
 CONFIG_BINFMT_ZFLAT=y
 # CONFIG_BINFMT_SHARED_FLAT is not set
+# CONFIG_HAVE_AOUT is not set
 # CONFIG_BINFMT_MISC is not set
 
 #
 # Power management options
 #
 CONFIG_PM=y
-# CONFIG_PM_LEGACY is not set
 # CONFIG_PM_DEBUG is not set
 CONFIG_PM_SLEEP=y
-CONFIG_SUSPEND_UP_POSSIBLE=y
 CONFIG_SUSPEND=y
+CONFIG_SUSPEND_FREEZER=y
+CONFIG_ARCH_SUSPEND_POSSIBLE=y
 CONFIG_PM_BFIN_SLEEP_DEEPER=y
 # CONFIG_PM_BFIN_SLEEP is not set
 # CONFIG_PM_WAKEUP_BY_GPIO is not set
 
 #
-# CPU Frequency scaling
+# Possible Suspend Mem / Hibernate Wake-Up Sources
 #
-# CONFIG_CPU_FREQ is not set
+# CONFIG_PM_BFIN_WAKE_PH6 is not set
 
 #
-# Networking
+# CPU Frequency scaling
 #
+# CONFIG_CPU_FREQ is not set
 CONFIG_NET=y
 
 #
@@ -350,6 +375,7 @@ CONFIG_XFRM=y
 # CONFIG_XFRM_USER is not set
 # CONFIG_XFRM_SUB_POLICY is not set
 # CONFIG_XFRM_MIGRATE is not set
+# CONFIG_XFRM_STATISTICS is not set
 # CONFIG_NET_KEY is not set
 CONFIG_INET=y
 # CONFIG_IP_MULTICAST is not set
@@ -379,8 +405,6 @@ CONFIG_TCP_CONG_CUBIC=y
 CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_TCP_MD5SIG is not set
 # CONFIG_IPV6 is not set
-# CONFIG_INET6_XFRM_TUNNEL is not set
-# CONFIG_INET6_TUNNEL is not set
 # CONFIG_NETLABEL is not set
 # CONFIG_NETWORK_SECMARK is not set
 # CONFIG_NETFILTER is not set
@@ -389,6 +413,7 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_TIPC is not set
 # CONFIG_ATM is not set
 # CONFIG_BRIDGE is not set
+# CONFIG_NET_DSA is not set
 # CONFIG_VLAN_8021Q is not set
 # CONFIG_DECNET is not set
 # CONFIG_LLC2 is not set
@@ -405,6 +430,7 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 #
 # CONFIG_NET_PKTGEN is not set
 # CONFIG_HAMRADIO is not set
+# CONFIG_CAN is not set
 CONFIG_IRDA=m
 
 #
@@ -440,24 +466,14 @@ CONFIG_SIR_BFIN_DMA=y
 # CONFIG_DONGLE is not set
 
 #
-# Old SIR device drivers
-#
-# CONFIG_IRPORT_SIR is not set
-
-#
-# Old Serial dongle support
-#
-
-#
 # FIR device drivers
 #
 # CONFIG_BT is not set
 # CONFIG_AF_RXRPC is not set
-
-#
-# Wireless
-#
+# CONFIG_PHONET is not set
+CONFIG_WIRELESS=y
 # CONFIG_CFG80211 is not set
+CONFIG_WIRELESS_OLD_REGULATORY=y
 # CONFIG_WIRELESS_EXT is not set
 # CONFIG_MAC80211 is not set
 # CONFIG_IEEE80211 is not set
@@ -475,6 +491,8 @@ CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
 CONFIG_STANDALONE=y
 CONFIG_PREVENT_FIRMWARE_BUILD=y
 # CONFIG_FW_LOADER is not set
+# CONFIG_DEBUG_DRIVER is not set
+# CONFIG_DEBUG_DEVRES is not set
 # CONFIG_SYS_HYPERVISOR is not set
 # CONFIG_CONNECTOR is not set
 CONFIG_MTD=y
@@ -483,6 +501,7 @@ CONFIG_MTD=y
 CONFIG_MTD_PARTITIONS=y
 # CONFIG_MTD_REDBOOT_PARTS is not set
 CONFIG_MTD_CMDLINE_PARTS=y
+# CONFIG_MTD_AR7_PARTS is not set
 
 #
 # User Modules And Translation Layers
@@ -553,15 +572,11 @@ CONFIG_MTD_NAND=m
 # CONFIG_MTD_NAND_VERIFY_WRITE is not set
 # CONFIG_MTD_NAND_ECC_SMC is not set
 # CONFIG_MTD_NAND_MUSEUM_IDS is not set
-CONFIG_MTD_NAND_BFIN=m
-CONFIG_BFIN_NAND_BASE=0x20212000
-CONFIG_BFIN_NAND_CLE=2
-CONFIG_BFIN_NAND_ALE=1
-CONFIG_BFIN_NAND_READY=3
+# CONFIG_MTD_NAND_BFIN is not set
 CONFIG_MTD_NAND_IDS=m
 # CONFIG_MTD_NAND_DISKONCHIP is not set
 # CONFIG_MTD_NAND_NANDSIM is not set
-# CONFIG_MTD_NAND_PLATFORM is not set
+CONFIG_MTD_NAND_PLATFORM=m
 # CONFIG_MTD_ONENAND is not set
 
 #
@@ -576,11 +591,14 @@ CONFIG_BLK_DEV=y
 CONFIG_BLK_DEV_RAM=y
 CONFIG_BLK_DEV_RAM_COUNT=16
 CONFIG_BLK_DEV_RAM_SIZE=4096
-CONFIG_BLK_DEV_RAM_BLOCKSIZE=1024
+# CONFIG_BLK_DEV_XIP is not set
 # CONFIG_CDROM_PKTCDVD is not set
 # CONFIG_ATA_OVER_ETH is not set
+# CONFIG_BLK_DEV_HD is not set
 CONFIG_MISC_DEVICES=y
 # CONFIG_EEPROM_93CX6 is not set
+# CONFIG_ENCLOSURE_SERVICES is not set
+CONFIG_HAVE_IDE=y
 # CONFIG_IDE is not set
 
 #
@@ -593,7 +611,6 @@ CONFIG_MISC_DEVICES=y
 # CONFIG_ATA is not set
 # CONFIG_MD is not set
 CONFIG_NETDEVICES=y
-# CONFIG_NETDEVICES_MULTIQUEUE is not set
 # CONFIG_DUMMY is not set
 # CONFIG_BONDING is not set
 # CONFIG_MACVLAN is not set
@@ -614,6 +631,7 @@ CONFIG_PHYLIB=y
 CONFIG_SMSC_PHY=y
 # CONFIG_BROADCOM_PHY is not set
 # CONFIG_ICPLUS_PHY is not set
+# CONFIG_REALTEK_PHY is not set
 # CONFIG_FIXED_PHY is not set
 # CONFIG_MDIO_BITBANG is not set
 CONFIG_NET_ETHERNET=y
@@ -626,11 +644,14 @@ CONFIG_BFIN_RX_DESC_NUM=20
 # CONFIG_SMC91X is not set
 # CONFIG_SMSC911X is not set
 # CONFIG_DM9000 is not set
+# CONFIG_ENC28J60 is not set
 # CONFIG_IBM_NEW_EMAC_ZMII is not set
 # CONFIG_IBM_NEW_EMAC_RGMII is not set
 # CONFIG_IBM_NEW_EMAC_TAH is not set
 # CONFIG_IBM_NEW_EMAC_EMAC4 is not set
-# CONFIG_B44 is not set
+# CONFIG_IBM_NEW_EMAC_NO_FLOW_CTRL is not set
+# CONFIG_IBM_NEW_EMAC_MAL_CLR_ICINTSTAT is not set
+# CONFIG_IBM_NEW_EMAC_MAL_COMMON_ERR is not set
 CONFIG_NETDEV_1000=y
 # CONFIG_AX88180 is not set
 CONFIG_NETDEV_10000=y
@@ -640,10 +661,10 @@ CONFIG_NETDEV_10000=y
 #
 # CONFIG_WLAN_PRE80211 is not set
 # CONFIG_WLAN_80211 is not set
+# CONFIG_IWLWIFI_LEDS is not set
 # CONFIG_WAN is not set
 # CONFIG_PPP is not set
 # CONFIG_SLIP is not set
-# CONFIG_SHAPER is not set
 # CONFIG_NETCONSOLE is not set
 # CONFIG_NETPOLL is not set
 # CONFIG_NET_POLL_CONTROLLER is not set
@@ -675,12 +696,15 @@ CONFIG_INPUT_EVDEV=m
 # CONFIG_INPUT_TOUCHSCREEN is not set
 CONFIG_INPUT_MISC=y
 # CONFIG_INPUT_UINPUT is not set
-CONFIG_TWI_KEYPAD=m
+CONFIG_CONFIG_INPUT_PCF8574=m
 
 #
 # Hardware I/O ports
 #
-# CONFIG_SERIO is not set
+CONFIG_SERIO=y
+CONFIG_SERIO_SERPORT=y
+CONFIG_SERIO_LIBPS2=y
+# CONFIG_SERIO_RAW is not set
 # CONFIG_GAMEPORT is not set
 
 #
@@ -691,11 +715,14 @@ CONFIG_TWI_KEYPAD=m
 # CONFIG_BF5xx_PPIFCD is not set
 # CONFIG_BFIN_SIMPLE_TIMER is not set
 # CONFIG_BF5xx_PPI is not set
-CONFIG_BFIN_SPORT=y
+CONFIG_BFIN_SPORT=m
 # CONFIG_BFIN_TIMER_LATENCY is not set
 CONFIG_TWI_LCD=m
+CONFIG_BFIN_DMA_INTERFACE=m
 CONFIG_SIMPLE_GPIO=m
 # CONFIG_VT is not set
+# CONFIG_DEVKMEM is not set
+# CONFIG_BFIN_JTAG_COMM is not set
 # CONFIG_SERIAL_NONSTANDARD is not set
 
 #
@@ -727,48 +754,51 @@ CONFIG_CAN4LINUX=y
 #
 # linux embedded drivers
 #
-# CONFIG_CAN_MCF5282 is not set
-# CONFIG_CAN_UNCTWINCAN is not set
 CONFIG_CAN_BLACKFIN=m
 # CONFIG_IPMI_HANDLER is not set
 # CONFIG_HW_RANDOM is not set
-# CONFIG_GEN_RTC is not set
 # CONFIG_R3964 is not set
 # CONFIG_RAW_DRIVER is not set
 # CONFIG_TCG_TPM is not set
 CONFIG_I2C=m
 CONFIG_I2C_BOARDINFO=y
 CONFIG_I2C_CHARDEV=m
+CONFIG_I2C_HELPER_AUTO=y
 
 #
-# I2C Algorithms
+# I2C Hardware Bus support
 #
-# CONFIG_I2C_ALGOBIT is not set
-# CONFIG_I2C_ALGOPCF is not set
-# CONFIG_I2C_ALGOPCA is not set
 
 #
-# I2C Hardware Bus support
+# I2C system bus drivers (mostly embedded / system-on-chip)
 #
 CONFIG_I2C_BLACKFIN_TWI=m
-CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=50
+CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=100
 # CONFIG_I2C_GPIO is not set
 # CONFIG_I2C_OCORES is not set
-# CONFIG_I2C_PARPORT_LIGHT is not set
 # CONFIG_I2C_SIMTEC is not set
+
+#
+# External I2C/SMBus adapter drivers
+#
+# CONFIG_I2C_PARPORT_LIGHT is not set
 # CONFIG_I2C_TAOS_EVM is not set
+
+#
+# Other I2C/SMBus bus drivers
+#
+# CONFIG_I2C_PCA_PLATFORM is not set
 # CONFIG_I2C_STUB is not set
 
 #
 # Miscellaneous I2C Chip support
 #
-# CONFIG_SENSORS_DS1337 is not set
-# CONFIG_SENSORS_DS1374 is not set
 # CONFIG_DS1682 is not set
+# CONFIG_AT24 is not set
 CONFIG_SENSORS_AD5252=m
 # CONFIG_SENSORS_EEPROM is not set
 # CONFIG_SENSORS_PCF8574 is not set
-# CONFIG_SENSORS_PCF8575 is not set
+# CONFIG_PCF8575 is not set
 # CONFIG_SENSORS_PCA9539 is not set
 # CONFIG_SENSORS_PCF8591 is not set
 # CONFIG_SENSORS_MAX6875 is not set
@@ -777,17 +807,15 @@ CONFIG_SENSORS_AD5252=m
 # CONFIG_I2C_DEBUG_ALGO is not set
 # CONFIG_I2C_DEBUG_BUS is not set
 # CONFIG_I2C_DEBUG_CHIP is not set
-
-#
-# SPI support
-#
 CONFIG_SPI=y
+# CONFIG_SPI_DEBUG is not set
 CONFIG_SPI_MASTER=y
 
 #
 # SPI Master Controller Drivers
 #
 CONFIG_SPI_BFIN=y
+# CONFIG_SPI_BFIN_LOCK is not set
 # CONFIG_SPI_BITBANG is not set
 
 #
@@ -796,11 +824,15 @@ CONFIG_SPI_BFIN=y
 # CONFIG_SPI_AT25 is not set
 # CONFIG_SPI_SPIDEV is not set
 # CONFIG_SPI_TLE62X0 is not set
+CONFIG_ARCH_WANT_OPTIONAL_GPIOLIB=y
+# CONFIG_GPIOLIB is not set
 # CONFIG_W1 is not set
 # CONFIG_POWER_SUPPLY is not set
 CONFIG_HWMON=y
 # CONFIG_HWMON_VID is not set
+# CONFIG_SENSORS_AD7414 is not set
 # CONFIG_SENSORS_AD7418 is not set
+# CONFIG_SENSORS_ADCXX is not set
 # CONFIG_SENSORS_ADM1021 is not set
 # CONFIG_SENSORS_ADM1025 is not set
 # CONFIG_SENSORS_ADM1026 is not set
@@ -808,6 +840,7 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_ADM1031 is not set
 # CONFIG_SENSORS_ADM9240 is not set
 # CONFIG_SENSORS_ADT7470 is not set
+# CONFIG_SENSORS_ADT7473 is not set
 # CONFIG_SENSORS_ATXP1 is not set
 # CONFIG_SENSORS_DS1621 is not set
 # CONFIG_SENSORS_F71805F is not set
@@ -828,6 +861,7 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_LM90 is not set
 # CONFIG_SENSORS_LM92 is not set
 # CONFIG_SENSORS_LM93 is not set
+# CONFIG_SENSORS_MAX1111 is not set
 # CONFIG_SENSORS_MAX1619 is not set
 # CONFIG_SENSORS_MAX6650 is not set
 # CONFIG_SENSORS_PC87360 is not set
@@ -836,6 +870,7 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_SMSC47M1 is not set
 # CONFIG_SENSORS_SMSC47M192 is not set
 # CONFIG_SENSORS_SMSC47B397 is not set
+# CONFIG_SENSORS_ADS7828 is not set
 # CONFIG_SENSORS_THMC50 is not set
 # CONFIG_SENSORS_VT1211 is not set
 # CONFIG_SENSORS_W83781D is not set
@@ -843,9 +878,12 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_W83792D is not set
 # CONFIG_SENSORS_W83793 is not set
 # CONFIG_SENSORS_W83L785TS is not set
+# CONFIG_SENSORS_W83L786NG is not set
 # CONFIG_SENSORS_W83627HF is not set
 # CONFIG_SENSORS_W83627EHF is not set
 # CONFIG_HWMON_DEBUG_CHIP is not set
+# CONFIG_THERMAL is not set
+# CONFIG_THERMAL_HWMON is not set
 CONFIG_WATCHDOG=y
 # CONFIG_WATCHDOG_NOWAYOUT is not set
 
@@ -856,21 +894,29 @@ CONFIG_WATCHDOG=y
 CONFIG_BFIN_WDT=y
 
 #
-# Sonics Silicon Backplane
-#
-CONFIG_SSB_POSSIBLE=y
-# CONFIG_SSB is not set
-
-#
 # Multifunction device drivers
 #
+# CONFIG_MFD_CORE is not set
 # CONFIG_MFD_SM501 is not set
+# CONFIG_HTC_PASIC3 is not set
+# CONFIG_MFD_TMIO is not set
+# CONFIG_MFD_WM8400 is not set
+# CONFIG_MFD_WM8350_I2C is not set
 
 #
 # Multimedia devices
 #
+
+#
+# Multimedia core support
+#
 # CONFIG_VIDEO_DEV is not set
 # CONFIG_DVB_CORE is not set
+# CONFIG_VIDEO_MEDIA is not set
+
+#
+# Multimedia drivers
+#
 # CONFIG_DAB is not set
 
 #
@@ -881,6 +927,7 @@ CONFIG_SSB_POSSIBLE=y
 CONFIG_FB=m
 CONFIG_FIRMWARE_EDID=y
 # CONFIG_FB_DDC is not set
+# CONFIG_FB_BOOT_VESA_SUPPORT is not set
 CONFIG_FB_CFB_FILLRECT=m
 CONFIG_FB_CFB_COPYAREA=m
 CONFIG_FB_CFB_IMAGEBLIT=m
@@ -888,8 +935,8 @@ CONFIG_FB_CFB_IMAGEBLIT=m
 # CONFIG_FB_SYS_FILLRECT is not set
 # CONFIG_FB_SYS_COPYAREA is not set
 # CONFIG_FB_SYS_IMAGEBLIT is not set
+# CONFIG_FB_FOREIGN_ENDIAN is not set
 # CONFIG_FB_SYS_FOPS is not set
-CONFIG_FB_DEFERRED_IO=y
 # CONFIG_FB_SVGALIB is not set
 # CONFIG_FB_MACMODES is not set
 # CONFIG_FB_BACKLIGHT is not set
@@ -899,8 +946,12 @@ CONFIG_FB_DEFERRED_IO=y
 #
 # Frame buffer hardware drivers
 #
-# CONFIG_FB_HITACHI_TX09 is not set
 # CONFIG_FB_BFIN_T350MCQB is not set
+# CONFIG_FB_BFIN_LQ035Q1 is not set
+CONFIG_FB_BF537_LQ035=m
+CONFIG_LQ035_SLAVE_ADDR=0x58
+# CONFIG_FB_BFIN_LANDSCAPE is not set
+# CONFIG_FB_BFIN_BGR is not set
 CONFIG_FB_BFIN_7393=m
 CONFIG_NTSC=y
 # CONFIG_PAL is not set
@@ -910,15 +961,17 @@ CONFIG_NTSC=y
 # CONFIG_PAL_YCBCR is not set
 CONFIG_ADV7393_1XMEM=y
 # CONFIG_ADV7393_2XMEM is not set
-CONFIG_FB_BF537_LQ035=m
-CONFIG_LQ035_SLAVE_ADDR=0x58
-# CONFIG_FB_BFIN_LANDSCAPE is not set
-# CONFIG_FB_BFIN_BGR is not set
+# CONFIG_FB_HITACHI_TX09 is not set
 # CONFIG_FB_S1D13XXX is not set
 # CONFIG_FB_VIRTUAL is not set
+# CONFIG_FB_METRONOME is not set
 CONFIG_BACKLIGHT_LCD_SUPPORT=y
 CONFIG_LCD_CLASS_DEVICE=m
 # CONFIG_LCD_LTV350QV is not set
+# CONFIG_LCD_ILI9320 is not set
+# CONFIG_LCD_TDO24M is not set
+# CONFIG_LCD_VGG2432A4 is not set
+# CONFIG_LCD_PLATFORM is not set
 CONFIG_BACKLIGHT_CLASS_DEVICE=m
 CONFIG_BACKLIGHT_CORGI=m
 
@@ -927,15 +980,8 @@ CONFIG_BACKLIGHT_CORGI=m
 #
 # CONFIG_DISPLAY_SUPPORT is not set
 # CONFIG_LOGO is not set
-
-#
-# Sound
-#
 CONFIG_SOUND=m
-
-#
-# Advanced Linux Sound Architecture
-#
+CONFIG_SOUND_OSS_CORE=y
 CONFIG_SND=m
 CONFIG_SND_TIMER=m
 CONFIG_SND_PCM=m
@@ -949,18 +995,12 @@ CONFIG_SND_SUPPORT_OLD_API=y
 CONFIG_SND_VERBOSE_PROCFS=y
 # CONFIG_SND_VERBOSE_PRINTK is not set
 # CONFIG_SND_DEBUG is not set
-
-#
-# Generic devices
-#
+CONFIG_SND_DRIVERS=y
 # CONFIG_SND_DUMMY is not set
 # CONFIG_SND_MTPAV is not set
 # CONFIG_SND_SERIAL_U16550 is not set
 # CONFIG_SND_MPU401 is not set
-
-#
-# SPI devices
-#
+CONFIG_SND_SPI=y
 
 #
 # ALSA Blackfin devices
@@ -972,51 +1012,46 @@ CONFIG_SND_BLACKFIN_AD1836_MULSUB=y
 # CONFIG_SND_BLACKFIN_AD1836_5P1 is not set
 CONFIG_SND_BLACKFIN_SPORT=0
 CONFIG_SND_BLACKFIN_SPI_PFBIT=4
-CONFIG_SND_BFIN_AD73311=m
 CONFIG_SND_BFIN_SPORT=0
-CONFIG_SND_BFIN_AD73311_SE=4
 CONFIG_SND_BFIN_AD73322=m
 CONFIG_SND_BFIN_AD73322_SPORT0_SE=10
 CONFIG_SND_BFIN_AD73322_SPORT1_SE=14
 CONFIG_SND_BFIN_AD73322_RESET=12
-
-#
-# System on Chip audio support
-#
-CONFIG_SND_SOC_AC97_BUS=y
 CONFIG_SND_SOC=m
-CONFIG_SND_BF5XX_SOC=m
-CONFIG_SND_MMAP_SUPPORT=y
-CONFIG_SND_BF5XX_SOC_AC97=m
-# CONFIG_SND_BF5XX_SOC_WM8750 is not set
-# CONFIG_SND_BF5XX_SOC_WM8731 is not set
+CONFIG_SND_SOC_AC97_BUS=y
+CONFIG_SND_BF5XX_I2S=m
 # CONFIG_SND_BF5XX_SOC_SSM2602 is not set
-CONFIG_SND_BF5XX_SOC_BF5xx=m
+CONFIG_SND_BF5XX_SOC_AD73311=m
+CONFIG_SND_BFIN_AD73311_SE=4
+CONFIG_SND_BF5XX_AC97=m
+CONFIG_SND_BF5XX_MMAP_SUPPORT=y
+# CONFIG_SND_BF5XX_MULTICHAN_SUPPORT is not set
+CONFIG_SND_BF5XX_SOC_SPORT=m
+CONFIG_SND_BF5XX_SOC_I2S=m
+CONFIG_SND_BF5XX_SOC_AC97=m
+CONFIG_SND_BF5XX_SOC_AD1980=m
 CONFIG_SND_BF5XX_SPORT_NUM=0
 # CONFIG_SND_BF5XX_HAVE_COLD_RESET is not set
-
-#
-# SoC Audio support for SuperH
-#
+# CONFIG_SND_SOC_ALL_CODECS is not set
 CONFIG_SND_SOC_AD1980=m
-
-#
-# Open Sound System
-#
+CONFIG_SND_SOC_AD73311=m
 # CONFIG_SOUND_PRIME is not set
 CONFIG_AC97_BUS=m
 CONFIG_HID_SUPPORT=y
 CONFIG_HID=y
 # CONFIG_HID_DEBUG is not set
 # CONFIG_HIDRAW is not set
+# CONFIG_HID_PID is not set
+
+#
+# Special HID drivers
+#
+CONFIG_HID_COMPAT=y
 # CONFIG_USB_SUPPORT is not set
-# CONFIG_NO_DUMMY_DELAY is not set
-# CONFIG_DUMMY_DELAY_BANK0 is not set
-# CONFIG_DUMMY_DELAY_BANK1 is not set
-# CONFIG_DUMMY_DELAY_BANK2 is not set
-# CONFIG_DUMMY_DELAY_BANK3 is not set
 # CONFIG_MMC is not set
+# CONFIG_MEMSTICK is not set
 # CONFIG_NEW_LEDS is not set
+# CONFIG_ACCESSIBILITY is not set
 CONFIG_RTC_LIB=y
 CONFIG_RTC_CLASS=y
 CONFIG_RTC_HCTOSYS=y
@@ -1045,51 +1080,57 @@ CONFIG_RTC_INTF_DEV=y
 # CONFIG_RTC_DRV_PCF8563 is not set
 # CONFIG_RTC_DRV_PCF8583 is not set
 # CONFIG_RTC_DRV_M41T80 is not set
+# CONFIG_RTC_DRV_S35390A is not set
+# CONFIG_RTC_DRV_FM3130 is not set
 
 #
 # SPI RTC drivers
 #
-# CONFIG_RTC_DRV_RS5C348 is not set
+# CONFIG_RTC_DRV_M41T94 is not set
+# CONFIG_RTC_DRV_DS1305 is not set
 # CONFIG_RTC_DRV_MAX6902 is not set
+# CONFIG_RTC_DRV_R9701 is not set
+# CONFIG_RTC_DRV_RS5C348 is not set
+# CONFIG_RTC_DRV_DS3234 is not set
 
 #
 # Platform RTC drivers
 #
+# CONFIG_RTC_DRV_DS1286 is not set
+# CONFIG_RTC_DRV_DS1511 is not set
 # CONFIG_RTC_DRV_DS1553 is not set
-# CONFIG_RTC_DRV_STK17TA8 is not set
 # CONFIG_RTC_DRV_DS1742 is not set
+# CONFIG_RTC_DRV_STK17TA8 is not set
 # CONFIG_RTC_DRV_M48T86 is not set
+# CONFIG_RTC_DRV_M48T35 is not set
 # CONFIG_RTC_DRV_M48T59 is not set
+# CONFIG_RTC_DRV_BQ4802 is not set
 # CONFIG_RTC_DRV_V3020 is not set
 
 #
 # on-CPU RTC drivers
 #
 CONFIG_RTC_DRV_BFIN=y
-
-#
-# Userspace I/O
-#
+# CONFIG_DMADEVICES is not set
 # CONFIG_UIO is not set
+# CONFIG_STAGING is not set
 
 #
 # File systems
 #
 # CONFIG_EXT2_FS is not set
 # CONFIG_EXT3_FS is not set
-# CONFIG_EXT4DEV_FS is not set
+# CONFIG_EXT4_FS is not set
 # CONFIG_REISERFS_FS is not set
 # CONFIG_JFS_FS is not set
 # CONFIG_FS_POSIX_ACL is not set
+CONFIG_FILE_LOCKING=y
 # CONFIG_XFS_FS is not set
-# CONFIG_GFS2_FS is not set
 # CONFIG_OCFS2_FS is not set
-# CONFIG_MINIX_FS is not set
-# CONFIG_ROMFS_FS is not set
+# CONFIG_DNOTIFY is not set
 CONFIG_INOTIFY=y
 CONFIG_INOTIFY_USER=y
 # CONFIG_QUOTA is not set
-# CONFIG_DNOTIFY is not set
 # CONFIG_AUTOFS_FS is not set
 # CONFIG_AUTOFS4_FS is not set
 # CONFIG_FUSE_FS is not set
@@ -1129,11 +1170,11 @@ CONFIG_SYSFS=y
 # CONFIG_EFS_FS is not set
 CONFIG_YAFFS_FS=m
 CONFIG_YAFFS_YAFFS1=y
+# CONFIG_YAFFS_9BYTE_TAGS is not set
 # CONFIG_YAFFS_DOES_ECC is not set
 CONFIG_YAFFS_YAFFS2=y
 CONFIG_YAFFS_AUTO_YAFFS2=y
 # CONFIG_YAFFS_DISABLE_LAZY_LOAD is not set
-CONFIG_YAFFS_CHECKPOINT_RESERVED_BLOCKS=10
 # CONFIG_YAFFS_DISABLE_WIDE_TNODES is not set
 # CONFIG_YAFFS_ALWAYS_CHECK_CHUNK_ERASED is not set
 CONFIG_YAFFS_SHORT_NAMES_IN_RAM=y
@@ -1150,8 +1191,11 @@ CONFIG_JFFS2_RTIME=y
 # CONFIG_JFFS2_RUBIN is not set
 # CONFIG_CRAMFS is not set
 # CONFIG_VXFS_FS is not set
+# CONFIG_MINIX_FS is not set
+# CONFIG_OMFS_FS is not set
 # CONFIG_HPFS_FS is not set
 # CONFIG_QNX4FS_FS is not set
+# CONFIG_ROMFS_FS is not set
 # CONFIG_SYSV_FS is not set
 # CONFIG_UFS_FS is not set
 CONFIG_NETWORK_FILESYSTEMS=y
@@ -1159,13 +1203,12 @@ CONFIG_NFS_FS=m
 CONFIG_NFS_V3=y
 # CONFIG_NFS_V3_ACL is not set
 # CONFIG_NFS_V4 is not set
-# CONFIG_NFS_DIRECTIO is not set
 # CONFIG_NFSD is not set
 CONFIG_LOCKD=m
 CONFIG_LOCKD_V4=y
 CONFIG_NFS_COMMON=y
 CONFIG_SUNRPC=m
-# CONFIG_SUNRPC_BIND34 is not set
+# CONFIG_SUNRPC_REGISTER_V4 is not set
 # CONFIG_RPCSEC_GSS_KRB5 is not set
 # CONFIG_RPCSEC_GSS_SPKM3 is not set
 CONFIG_SMB_FS=m
@@ -1221,9 +1264,6 @@ CONFIG_NLS_DEFAULT="iso8859-1"
 # CONFIG_NLS_KOI8_U is not set
 # CONFIG_NLS_UTF8 is not set
 # CONFIG_DLM is not set
-CONFIG_INSTRUMENTATION=y
-# CONFIG_PROFILING is not set
-# CONFIG_MARKERS is not set
 
 #
 # Kernel hacking
@@ -1231,14 +1271,53 @@ CONFIG_INSTRUMENTATION=y
 # CONFIG_PRINTK_TIME is not set
 CONFIG_ENABLE_WARN_DEPRECATED=y
 CONFIG_ENABLE_MUST_CHECK=y
+CONFIG_FRAME_WARN=1024
 # CONFIG_MAGIC_SYSRQ is not set
 # CONFIG_UNUSED_SYMBOLS is not set
 CONFIG_DEBUG_FS=y
 # CONFIG_HEADERS_CHECK is not set
-# CONFIG_DEBUG_KERNEL is not set
+CONFIG_DEBUG_KERNEL=y
+# CONFIG_DEBUG_SHIRQ is not set
+CONFIG_DETECT_SOFTLOCKUP=y
+# CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC is not set
+CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC_VALUE=0
+CONFIG_SCHED_DEBUG=y
+# CONFIG_SCHEDSTATS is not set
+# CONFIG_TIMER_STATS is not set
+# CONFIG_DEBUG_OBJECTS is not set
+# CONFIG_DEBUG_SLAB is not set
+# CONFIG_DEBUG_RT_MUTEXES is not set
+# CONFIG_RT_MUTEX_TESTER is not set
+# CONFIG_DEBUG_SPINLOCK is not set
+# CONFIG_DEBUG_MUTEXES is not set
+# CONFIG_DEBUG_SPINLOCK_SLEEP is not set
+# CONFIG_DEBUG_LOCKING_API_SELFTESTS is not set
+# CONFIG_DEBUG_KOBJECT is not set
 # CONFIG_DEBUG_BUGVERBOSE is not set
+CONFIG_DEBUG_INFO=y
+# CONFIG_DEBUG_VM is not set
+# CONFIG_DEBUG_WRITECOUNT is not set
+# CONFIG_DEBUG_MEMORY_INIT is not set
+# CONFIG_DEBUG_LIST is not set
+# CONFIG_DEBUG_SG is not set
+# CONFIG_FRAME_POINTER is not set
+# CONFIG_BOOT_PRINTK_DELAY is not set
+# CONFIG_RCU_TORTURE_TEST is not set
+# CONFIG_RCU_CPU_STALL_DETECTOR is not set
+# CONFIG_BACKTRACE_SELF_TEST is not set
+# CONFIG_DEBUG_BLOCK_EXT_DEVT is not set
+# CONFIG_FAULT_INJECTION is not set
+# CONFIG_SYSCTL_SYSCALL_CHECK is not set
+# CONFIG_DYNAMIC_PRINTK_DEBUG is not set
 # CONFIG_SAMPLES is not set
+CONFIG_HAVE_ARCH_KGDB=y
+# CONFIG_KGDB is not set
+# CONFIG_DEBUG_STACKOVERFLOW is not set
+# CONFIG_DEBUG_STACK_USAGE is not set
+CONFIG_DEBUG_VERBOSE=y
 CONFIG_DEBUG_MMRS=y
+# CONFIG_DEBUG_HWERR is not set
+# CONFIG_DEBUG_DOUBLEFAULT is not set
 CONFIG_DEBUG_HUNT_FOR_ZERO=y
 CONFIG_DEBUG_BFIN_HWTRACE_ON=y
 CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION_OFF=y
@@ -1256,9 +1335,94 @@ CONFIG_ACCESS_CHECK=y
 #
 # CONFIG_KEYS is not set
 CONFIG_SECURITY=y
+# CONFIG_SECURITYFS is not set
 # CONFIG_SECURITY_NETWORK is not set
-# CONFIG_SECURITY_CAPABILITIES is not set
-# CONFIG_CRYPTO is not set
+# CONFIG_SECURITY_FILE_CAPABILITIES is not set
+CONFIG_SECURITY_DEFAULT_MMAP_MIN_ADDR=0
+CONFIG_CRYPTO=y
+
+#
+# Crypto core or helper
+#
+# CONFIG_CRYPTO_FIPS is not set
+# CONFIG_CRYPTO_MANAGER is not set
+# CONFIG_CRYPTO_GF128MUL is not set
+# CONFIG_CRYPTO_NULL is not set
+# CONFIG_CRYPTO_CRYPTD is not set
+# CONFIG_CRYPTO_AUTHENC is not set
+# CONFIG_CRYPTO_TEST is not set
+
+#
+# Authenticated Encryption with Associated Data
+#
+# CONFIG_CRYPTO_CCM is not set
+# CONFIG_CRYPTO_GCM is not set
+# CONFIG_CRYPTO_SEQIV is not set
+
+#
+# Block modes
+#
+# CONFIG_CRYPTO_CBC is not set
+# CONFIG_CRYPTO_CTR is not set
+# CONFIG_CRYPTO_CTS is not set
+# CONFIG_CRYPTO_ECB is not set
+# CONFIG_CRYPTO_LRW is not set
+# CONFIG_CRYPTO_PCBC is not set
+# CONFIG_CRYPTO_XTS is not set
+
+#
+# Hash modes
+#
+# CONFIG_CRYPTO_HMAC is not set
+# CONFIG_CRYPTO_XCBC is not set
+
+#
+# Digest
+#
+# CONFIG_CRYPTO_CRC32C is not set
+# CONFIG_CRYPTO_MD4 is not set
+# CONFIG_CRYPTO_MD5 is not set
+# CONFIG_CRYPTO_MICHAEL_MIC is not set
+# CONFIG_CRYPTO_RMD128 is not set
+# CONFIG_CRYPTO_RMD160 is not set
+# CONFIG_CRYPTO_RMD256 is not set
+# CONFIG_CRYPTO_RMD320 is not set
+# CONFIG_CRYPTO_SHA1 is not set
+# CONFIG_CRYPTO_SHA256 is not set
+# CONFIG_CRYPTO_SHA512 is not set
+# CONFIG_CRYPTO_TGR192 is not set
+# CONFIG_CRYPTO_WP512 is not set
+
+#
+# Ciphers
+#
+# CONFIG_CRYPTO_AES is not set
+# CONFIG_CRYPTO_ANUBIS is not set
+# CONFIG_CRYPTO_ARC4 is not set
+# CONFIG_CRYPTO_BLOWFISH is not set
+# CONFIG_CRYPTO_CAMELLIA is not set
+# CONFIG_CRYPTO_CAST5 is not set
+# CONFIG_CRYPTO_CAST6 is not set
+# CONFIG_CRYPTO_DES is not set
+# CONFIG_CRYPTO_FCRYPT is not set
+# CONFIG_CRYPTO_KHAZAD is not set
+# CONFIG_CRYPTO_SALSA20 is not set
+# CONFIG_CRYPTO_SEED is not set
+# CONFIG_CRYPTO_SERPENT is not set
+# CONFIG_CRYPTO_TEA is not set
+# CONFIG_CRYPTO_TWOFISH is not set
+
+#
+# Compression
+#
+# CONFIG_CRYPTO_DEFLATE is not set
+# CONFIG_CRYPTO_LZO is not set
+
+#
+# Random Number Generation
+#
+# CONFIG_CRYPTO_ANSI_CPRNG is not set
+CONFIG_CRYPTO_HW=y
 
 #
 # Library routines
@@ -1266,6 +1430,7 @@ CONFIG_SECURITY=y
 CONFIG_BITREVERSE=y
 CONFIG_CRC_CCITT=m
 # CONFIG_CRC16 is not set
+# CONFIG_CRC_T10DIF is not set
 # CONFIG_CRC_ITU_T is not set
 CONFIG_CRC32=y
 # CONFIG_CRC7 is not set
diff --git a/arch/blackfin/configs/BF538-EZKIT_defconfig b/arch/blackfin/configs/BF538-EZKIT_defconfig
new file mode 100644
index 000000000000..ed15934c67c2
--- /dev/null
+++ b/arch/blackfin/configs/BF538-EZKIT_defconfig
@@ -0,0 +1,1368 @@
+#
+# Automatically generated make config: don't edit
+# Linux kernel version: 2.6.28-rc2
+#
+# CONFIG_MMU is not set
+# CONFIG_FPU is not set
+CONFIG_RWSEM_GENERIC_SPINLOCK=y
+# CONFIG_RWSEM_XCHGADD_ALGORITHM is not set
+CONFIG_BLACKFIN=y
+CONFIG_ZONE_DMA=y
+CONFIG_GENERIC_FIND_NEXT_BIT=y
+CONFIG_GENERIC_HWEIGHT=y
+CONFIG_GENERIC_HARDIRQS=y
+CONFIG_GENERIC_IRQ_PROBE=y
+CONFIG_GENERIC_GPIO=y
+CONFIG_FORCE_MAX_ZONEORDER=14
+CONFIG_GENERIC_CALIBRATE_DELAY=y
+CONFIG_DEFCONFIG_LIST="/lib/modules/$UNAME_RELEASE/.config"
+
+#
+# General setup
+#
+CONFIG_EXPERIMENTAL=y
+CONFIG_BROKEN_ON_SMP=y
+CONFIG_INIT_ENV_ARG_LIMIT=32
+CONFIG_LOCALVERSION=""
+CONFIG_LOCALVERSION_AUTO=y
+CONFIG_SYSVIPC=y
+CONFIG_SYSVIPC_SYSCTL=y
+# CONFIG_POSIX_MQUEUE is not set
+# CONFIG_BSD_PROCESS_ACCT is not set
+# CONFIG_TASKSTATS is not set
+# CONFIG_AUDIT is not set
+CONFIG_IKCONFIG=y
+CONFIG_IKCONFIG_PROC=y
+CONFIG_LOG_BUF_SHIFT=14
+# CONFIG_CGROUPS is not set
+# CONFIG_GROUP_SCHED is not set
+# CONFIG_SYSFS_DEPRECATED_V2 is not set
+# CONFIG_RELAY is not set
+# CONFIG_NAMESPACES is not set
+CONFIG_BLK_DEV_INITRD=y
+CONFIG_INITRAMFS_SOURCE=""
+# CONFIG_CC_OPTIMIZE_FOR_SIZE is not set
+CONFIG_SYSCTL=y
+CONFIG_EMBEDDED=y
+CONFIG_UID16=y
+CONFIG_SYSCTL_SYSCALL=y
+CONFIG_KALLSYMS=y
+# CONFIG_KALLSYMS_ALL is not set
+# CONFIG_KALLSYMS_EXTRA_PASS is not set
+CONFIG_HOTPLUG=y
+CONFIG_PRINTK=y
+CONFIG_BUG=y
+# CONFIG_ELF_CORE is not set
+CONFIG_COMPAT_BRK=y
+CONFIG_BASE_FULL=y
+CONFIG_FUTEX=y
+CONFIG_ANON_INODES=y
+CONFIG_EPOLL=y
+CONFIG_SIGNALFD=y
+CONFIG_TIMERFD=y
+CONFIG_EVENTFD=y
+CONFIG_AIO=y
+CONFIG_VM_EVENT_COUNTERS=y
+CONFIG_SLAB=y
+# CONFIG_SLUB is not set
+# CONFIG_SLOB is not set
+# CONFIG_PROFILING is not set
+# CONFIG_MARKERS is not set
+CONFIG_HAVE_OPROFILE=y
+# CONFIG_HAVE_GENERIC_DMA_COHERENT is not set
+CONFIG_SLABINFO=y
+CONFIG_RT_MUTEXES=y
+CONFIG_TINY_SHMEM=y
+CONFIG_BASE_SMALL=0
+CONFIG_MODULES=y
+# CONFIG_MODULE_FORCE_LOAD is not set
+CONFIG_MODULE_UNLOAD=y
+# CONFIG_MODULE_FORCE_UNLOAD is not set
+# CONFIG_MODVERSIONS is not set
+# CONFIG_MODULE_SRCVERSION_ALL is not set
+CONFIG_KMOD=y
+CONFIG_BLOCK=y
+# CONFIG_LBD is not set
+# CONFIG_BLK_DEV_IO_TRACE is not set
+# CONFIG_LSF is not set
+# CONFIG_BLK_DEV_BSG is not set
+# CONFIG_BLK_DEV_INTEGRITY is not set
+
+#
+# IO Schedulers
+#
+CONFIG_IOSCHED_NOOP=y
+CONFIG_IOSCHED_AS=y
+# CONFIG_IOSCHED_DEADLINE is not set
+CONFIG_IOSCHED_CFQ=y
+CONFIG_DEFAULT_AS=y
+# CONFIG_DEFAULT_DEADLINE is not set
+# CONFIG_DEFAULT_CFQ is not set
+# CONFIG_DEFAULT_NOOP is not set
+CONFIG_DEFAULT_IOSCHED="anticipatory"
+CONFIG_CLASSIC_RCU=y
+# CONFIG_PREEMPT_NONE is not set
+CONFIG_PREEMPT_VOLUNTARY=y
+# CONFIG_PREEMPT is not set
+# CONFIG_FREEZER is not set
+
+#
+# Blackfin Processor Options
+#
+
+#
+# Processor and Board Settings
+#
+# CONFIG_BF512 is not set
+# CONFIG_BF514 is not set
+# CONFIG_BF516 is not set
+# CONFIG_BF518 is not set
+# CONFIG_BF522 is not set
+# CONFIG_BF523 is not set
+# CONFIG_BF524 is not set
+# CONFIG_BF525 is not set
+# CONFIG_BF526 is not set
+# CONFIG_BF527 is not set
+# CONFIG_BF531 is not set
+# CONFIG_BF532 is not set
+# CONFIG_BF533 is not set
+# CONFIG_BF534 is not set
+# CONFIG_BF536 is not set
+# CONFIG_BF537 is not set
+CONFIG_BF538=y
+# CONFIG_BF539 is not set
+# CONFIG_BF542 is not set
+# CONFIG_BF544 is not set
+# CONFIG_BF547 is not set
+# CONFIG_BF548 is not set
+# CONFIG_BF549 is not set
+# CONFIG_BF561 is not set
+CONFIG_BF_REV_MIN=4
+CONFIG_BF_REV_MAX=5
+# CONFIG_BF_REV_0_0 is not set
+# CONFIG_BF_REV_0_1 is not set
+# CONFIG_BF_REV_0_2 is not set
+# CONFIG_BF_REV_0_3 is not set
+CONFIG_BF_REV_0_4=y
+# CONFIG_BF_REV_0_5 is not set
+# CONFIG_BF_REV_0_6 is not set
+# CONFIG_BF_REV_ANY is not set
+# CONFIG_BF_REV_NONE is not set
+CONFIG_MEM_MT48LC32M8A2_75=y
+CONFIG_IRQ_PLL_WAKEUP=7
+CONFIG_IRQ_DMA0_ERROR=7
+CONFIG_IRQ_PPI_ERROR=7
+CONFIG_IRQ_SPORT0_ERROR=7
+CONFIG_IRQ_SPORT1_ERROR=7
+CONFIG_IRQ_UART0_ERROR=7
+CONFIG_IRQ_UART1_ERROR=7
+CONFIG_IRQ_RTC=8
+CONFIG_IRQ_PPI=8
+CONFIG_IRQ_SPORT0_RX=9
+CONFIG_IRQ_SPORT0_TX=9
+CONFIG_IRQ_SPORT1_RX=9
+CONFIG_IRQ_SPORT1_TX=9
+CONFIG_IRQ_SPI0=10
+CONFIG_IRQ_UART0_RX=10
+CONFIG_IRQ_UART0_TX=10
+CONFIG_IRQ_UART1_RX=10
+CONFIG_IRQ_UART1_TX=10
+CONFIG_IRQ_TIMER0=12
+CONFIG_IRQ_TIMER1=12
+CONFIG_IRQ_TIMER2=12
+CONFIG_IRQ_WATCH=13
+CONFIG_IRQ_PORTF_INTA=12
+CONFIG_IRQ_PORTF_INTB=12
+CONFIG_IRQ_SPI0_ERROR=7
+CONFIG_IRQ_SPI1_ERROR=7
+CONFIG_IRQ_DMA1_ERROR=7
+CONFIG_IRQ_CAN_RX=11
+CONFIG_IRQ_CAN_TX=11
+CONFIG_BFIN538_EZKIT=y
+
+#
+# BF538 Specific Configuration
+#
+
+#
+# Interrupt Priority Assignment
+#
+
+#
+# Priority
+#
+CONFIG_IRQ_MEM0_DMA0=13
+CONFIG_IRQ_MEM0_DMA1=13
+CONFIG_IRQ_SPORT2_ERROR=7
+CONFIG_IRQ_SPORT3_ERROR=7
+CONFIG_IRQ_SPI2_ERROR=7
+CONFIG_IRQ_UART2_ERROR=7
+CONFIG_IRQ_CAN_ERROR=7
+CONFIG_IRQ_SPORT2_RX=9
+CONFIG_IRQ_SPORT2_TX=9
+CONFIG_IRQ_SPORT3_RX=9
+CONFIG_IRQ_SPORT3_TX=9
+CONFIG_IRQ_SPI1=10
+CONFIG_IRQ_SPI2=10
+CONFIG_IRQ_UART2_RX=10
+CONFIG_IRQ_UART2_TX=10
+CONFIG_IRQ_TWI0=11
+CONFIG_IRQ_TWI1=11
+CONFIG_IRQ_MEM1_DMA0=13
+CONFIG_IRQ_MEM1_DMA1=13
+
+#
+# Board customizations
+#
+# CONFIG_CMDLINE_BOOL is not set
+CONFIG_BOOT_LOAD=0x1000
+
+#
+# Clock/PLL Setup
+#
+CONFIG_CLKIN_HZ=25000000
+# CONFIG_BFIN_KERNEL_CLOCK is not set
+CONFIG_MAX_VCO_HZ=533333333
+CONFIG_MIN_VCO_HZ=50000000
+CONFIG_MAX_SCLK_HZ=133333333
+CONFIG_MIN_SCLK_HZ=27000000
+
+#
+# Kernel Timer/Scheduler
+#
+# CONFIG_HZ_100 is not set
+CONFIG_HZ_250=y
+# CONFIG_HZ_300 is not set
+# CONFIG_HZ_1000 is not set
+CONFIG_HZ=250
+CONFIG_SCHED_HRTICK=y
+CONFIG_GENERIC_TIME=y
+CONFIG_GENERIC_CLOCKEVENTS=y
+# CONFIG_CYCLES_CLOCKSOURCE is not set
+CONFIG_TICK_ONESHOT=y
+# CONFIG_NO_HZ is not set
+CONFIG_HIGH_RES_TIMERS=y
+CONFIG_GENERIC_CLOCKEVENTS_BUILD=y
+
+#
+# Misc
+#
+CONFIG_BFIN_SCRATCH_REG_RETN=y
+# CONFIG_BFIN_SCRATCH_REG_RETE is not set
+# CONFIG_BFIN_SCRATCH_REG_CYCLES is not set
+
+#
+# Blackfin Kernel Optimizations
+#
+
+#
+# Memory Optimizations
+#
+CONFIG_I_ENTRY_L1=y
+CONFIG_EXCPT_IRQ_SYSC_L1=y
+CONFIG_DO_IRQ_L1=y
+CONFIG_CORE_TIMER_IRQ_L1=y
+CONFIG_IDLE_L1=y
+CONFIG_SCHEDULE_L1=y
+CONFIG_ARITHMETIC_OPS_L1=y
+CONFIG_ACCESS_OK_L1=y
+CONFIG_MEMSET_L1=y
+CONFIG_MEMCPY_L1=y
+CONFIG_SYS_BFIN_SPINLOCK_L1=y
+# CONFIG_IP_CHECKSUM_L1 is not set
+CONFIG_CACHELINE_ALIGNED_L1=y
+# CONFIG_SYSCALL_TAB_L1 is not set
+# CONFIG_CPLB_SWITCH_TAB_L1 is not set
+CONFIG_APP_STACK_L1=y
+
+#
+# Speed Optimizations
+#
+CONFIG_BFIN_INS_LOWOVERHEAD=y
+CONFIG_RAMKERNEL=y
+# CONFIG_ROMKERNEL is not set
+CONFIG_SELECT_MEMORY_MODEL=y
+CONFIG_FLATMEM_MANUAL=y
+# CONFIG_DISCONTIGMEM_MANUAL is not set
+# CONFIG_SPARSEMEM_MANUAL is not set
+CONFIG_FLATMEM=y
+CONFIG_FLAT_NODE_MEM_MAP=y
+CONFIG_PAGEFLAGS_EXTENDED=y
+CONFIG_SPLIT_PTLOCK_CPUS=4
+# CONFIG_RESOURCES_64BIT is not set
+# CONFIG_PHYS_ADDR_T_64BIT is not set
+CONFIG_ZONE_DMA_FLAG=1
+CONFIG_VIRT_TO_BUS=y
+CONFIG_BFIN_GPTIMERS=y
+# CONFIG_DMA_UNCACHED_4M is not set
+# CONFIG_DMA_UNCACHED_2M is not set
+CONFIG_DMA_UNCACHED_1M=y
+# CONFIG_DMA_UNCACHED_NONE is not set
+
+#
+# Cache Support
+#
+CONFIG_BFIN_ICACHE=y
+CONFIG_BFIN_DCACHE=y
+# CONFIG_BFIN_DCACHE_BANKA is not set
+# CONFIG_BFIN_ICACHE_LOCK is not set
+# CONFIG_BFIN_WB is not set
+CONFIG_BFIN_WT=y
+# CONFIG_MPU is not set
+
+#
+# Asynchonous Memory Configuration
+#
+
+#
+# EBIU_AMGCTL Global Control
+#
+CONFIG_C_AMCKEN=y
+CONFIG_C_CDPRIO=y
+# CONFIG_C_AMBEN is not set
+# CONFIG_C_AMBEN_B0 is not set
+# CONFIG_C_AMBEN_B0_B1 is not set
+# CONFIG_C_AMBEN_B0_B1_B2 is not set
+CONFIG_C_AMBEN_ALL=y
+
+#
+# EBIU_AMBCTL Control
+#
+CONFIG_BANK_0=0x7BB0
+CONFIG_BANK_1=0x7BB0
+CONFIG_BANK_2=0x7BB0
+CONFIG_BANK_3=0x99B2
+
+#
+# Bus options (PCI, PCMCIA, EISA, MCA, ISA)
+#
+# CONFIG_ARCH_SUPPORTS_MSI is not set
+# CONFIG_PCCARD is not set
+
+#
+# Executable file formats
+#
+CONFIG_BINFMT_ELF_FDPIC=y
+CONFIG_BINFMT_FLAT=y
+CONFIG_BINFMT_ZFLAT=y
+# CONFIG_BINFMT_SHARED_FLAT is not set
+# CONFIG_HAVE_AOUT is not set
+# CONFIG_BINFMT_MISC is not set
+
+#
+# Power management options
+#
+# CONFIG_PM is not set
+CONFIG_ARCH_SUSPEND_POSSIBLE=y
+# CONFIG_PM_WAKEUP_BY_GPIO is not set
+
+#
+# CPU Frequency scaling
+#
+# CONFIG_CPU_FREQ is not set
+CONFIG_NET=y
+
+#
+# Networking options
+#
+CONFIG_PACKET=y
+# CONFIG_PACKET_MMAP is not set
+CONFIG_UNIX=y
+CONFIG_XFRM=y
+# CONFIG_XFRM_USER is not set
+# CONFIG_XFRM_SUB_POLICY is not set
+# CONFIG_XFRM_MIGRATE is not set
+# CONFIG_XFRM_STATISTICS is not set
+# CONFIG_NET_KEY is not set
+CONFIG_INET=y
+# CONFIG_IP_MULTICAST is not set
+# CONFIG_IP_ADVANCED_ROUTER is not set
+CONFIG_IP_FIB_HASH=y
+CONFIG_IP_PNP=y
+# CONFIG_IP_PNP_DHCP is not set
+# CONFIG_IP_PNP_BOOTP is not set
+# CONFIG_IP_PNP_RARP is not set
+# CONFIG_NET_IPIP is not set
+# CONFIG_NET_IPGRE is not set
+# CONFIG_ARPD is not set
+CONFIG_SYN_COOKIES=y
+# CONFIG_INET_AH is not set
+# CONFIG_INET_ESP is not set
+# CONFIG_INET_IPCOMP is not set
+# CONFIG_INET_XFRM_TUNNEL is not set
+# CONFIG_INET_TUNNEL is not set
+CONFIG_INET_XFRM_MODE_TRANSPORT=y
+CONFIG_INET_XFRM_MODE_TUNNEL=y
+CONFIG_INET_XFRM_MODE_BEET=y
+# CONFIG_INET_LRO is not set
+CONFIG_INET_DIAG=y
+CONFIG_INET_TCP_DIAG=y
+# CONFIG_TCP_CONG_ADVANCED is not set
+CONFIG_TCP_CONG_CUBIC=y
+CONFIG_DEFAULT_TCP_CONG="cubic"
+# CONFIG_TCP_MD5SIG is not set
+# CONFIG_IPV6 is not set
+# CONFIG_NETLABEL is not set
+# CONFIG_NETWORK_SECMARK is not set
+# CONFIG_NETFILTER is not set
+# CONFIG_IP_DCCP is not set
+# CONFIG_IP_SCTP is not set
+# CONFIG_TIPC is not set
+# CONFIG_ATM is not set
+# CONFIG_BRIDGE is not set
+# CONFIG_NET_DSA is not set
+# CONFIG_VLAN_8021Q is not set
+# CONFIG_DECNET is not set
+# CONFIG_LLC2 is not set
+# CONFIG_IPX is not set
+# CONFIG_ATALK is not set
+# CONFIG_X25 is not set
+# CONFIG_LAPB is not set
+# CONFIG_ECONET is not set
+# CONFIG_WAN_ROUTER is not set
+# CONFIG_NET_SCHED is not set
+
+#
+# Network testing
+#
+# CONFIG_NET_PKTGEN is not set
+# CONFIG_HAMRADIO is not set
+# CONFIG_CAN is not set
+CONFIG_IRDA=m
+
+#
+# IrDA protocols
+#
+CONFIG_IRLAN=m
+CONFIG_IRCOMM=m
+# CONFIG_IRDA_ULTRA is not set
+
+#
+# IrDA options
+#
+CONFIG_IRDA_CACHE_LAST_LSAP=y
+# CONFIG_IRDA_FAST_RR is not set
+# CONFIG_IRDA_DEBUG is not set
+
+#
+# Infrared-port device drivers
+#
+
+#
+# SIR device drivers
+#
+CONFIG_IRTTY_SIR=m
+CONFIG_BFIN_SIR=m
+CONFIG_SIR_BFIN_DMA=y
+# CONFIG_SIR_BFIN_PIO is not set
+
+#
+# Dongle support
+#
+# CONFIG_DONGLE is not set
+
+#
+# FIR device drivers
+#
+# CONFIG_BT is not set
+# CONFIG_AF_RXRPC is not set
+# CONFIG_PHONET is not set
+CONFIG_WIRELESS=y
+# CONFIG_CFG80211 is not set
+CONFIG_WIRELESS_OLD_REGULATORY=y
+# CONFIG_WIRELESS_EXT is not set
+# CONFIG_MAC80211 is not set
+# CONFIG_IEEE80211 is not set
+# CONFIG_RFKILL is not set
+# CONFIG_NET_9P is not set
+
+#
+# Device Drivers
+#
+
+#
+# Generic Driver Options
+#
+CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
+CONFIG_STANDALONE=y
+CONFIG_PREVENT_FIRMWARE_BUILD=y
+# CONFIG_FW_LOADER is not set
+# CONFIG_DEBUG_DRIVER is not set
+# CONFIG_DEBUG_DEVRES is not set
+# CONFIG_SYS_HYPERVISOR is not set
+# CONFIG_CONNECTOR is not set
+CONFIG_MTD=y
+# CONFIG_MTD_DEBUG is not set
+# CONFIG_MTD_CONCAT is not set
+CONFIG_MTD_PARTITIONS=y
+# CONFIG_MTD_REDBOOT_PARTS is not set
+CONFIG_MTD_CMDLINE_PARTS=y
+# CONFIG_MTD_AR7_PARTS is not set
+
+#
+# User Modules And Translation Layers
+#
+CONFIG_MTD_CHAR=m
+CONFIG_MTD_BLKDEVS=y
+CONFIG_MTD_BLOCK=y
+# CONFIG_FTL is not set
+# CONFIG_NFTL is not set
+# CONFIG_INFTL is not set
+# CONFIG_RFD_FTL is not set
+# CONFIG_SSFDC is not set
+# CONFIG_MTD_OOPS is not set
+
+#
+# RAM/ROM/Flash chip drivers
+#
+CONFIG_MTD_CFI=m
+# CONFIG_MTD_JEDECPROBE is not set
+CONFIG_MTD_GEN_PROBE=m
+# CONFIG_MTD_CFI_ADV_OPTIONS is not set
+CONFIG_MTD_MAP_BANK_WIDTH_1=y
+CONFIG_MTD_MAP_BANK_WIDTH_2=y
+CONFIG_MTD_MAP_BANK_WIDTH_4=y
+# CONFIG_MTD_MAP_BANK_WIDTH_8 is not set
+# CONFIG_MTD_MAP_BANK_WIDTH_16 is not set
+# CONFIG_MTD_MAP_BANK_WIDTH_32 is not set
+CONFIG_MTD_CFI_I1=y
+CONFIG_MTD_CFI_I2=y
+# CONFIG_MTD_CFI_I4 is not set
+# CONFIG_MTD_CFI_I8 is not set
+# CONFIG_MTD_CFI_INTELEXT is not set
+CONFIG_MTD_CFI_AMDSTD=m
+# CONFIG_MTD_CFI_STAA is not set
+CONFIG_MTD_CFI_UTIL=m
+CONFIG_MTD_RAM=y
+CONFIG_MTD_ROM=m
+# CONFIG_MTD_ABSENT is not set
+
+#
+# Mapping drivers for chip access
+#
+# CONFIG_MTD_COMPLEX_MAPPINGS is not set
+CONFIG_MTD_PHYSMAP=m
+CONFIG_MTD_PHYSMAP_START=0x20000000
+CONFIG_MTD_PHYSMAP_LEN=0x0
+CONFIG_MTD_PHYSMAP_BANKWIDTH=2
+# CONFIG_MTD_UCLINUX is not set
+# CONFIG_MTD_PLATRAM is not set
+
+#
+# Self-contained MTD device drivers
+#
+# CONFIG_MTD_DATAFLASH is not set
+# CONFIG_MTD_M25P80 is not set
+# CONFIG_MTD_SLRAM is not set
+# CONFIG_MTD_PHRAM is not set
+# CONFIG_MTD_MTDRAM is not set
+# CONFIG_MTD_BLOCK2MTD is not set
+
+#
+# Disk-On-Chip Device Drivers
+#
+# CONFIG_MTD_DOC2000 is not set
+# CONFIG_MTD_DOC2001 is not set
+# CONFIG_MTD_DOC2001PLUS is not set
+CONFIG_MTD_NAND=m
+# CONFIG_MTD_NAND_VERIFY_WRITE is not set
+# CONFIG_MTD_NAND_ECC_SMC is not set
+# CONFIG_MTD_NAND_MUSEUM_IDS is not set
+CONFIG_MTD_NAND_BFIN=m
+CONFIG_BFIN_NAND_BASE=0x20212000
+CONFIG_BFIN_NAND_CLE=2
+CONFIG_BFIN_NAND_ALE=1
+CONFIG_BFIN_NAND_READY=3
+CONFIG_MTD_NAND_IDS=m
+# CONFIG_MTD_NAND_DISKONCHIP is not set
+# CONFIG_MTD_NAND_NANDSIM is not set
+# CONFIG_MTD_NAND_PLATFORM is not set
+# CONFIG_MTD_ONENAND is not set
+
+#
+# UBI - Unsorted block images
+#
+# CONFIG_MTD_UBI is not set
+# CONFIG_PARPORT is not set
+CONFIG_BLK_DEV=y
+# CONFIG_BLK_DEV_COW_COMMON is not set
+# CONFIG_BLK_DEV_LOOP is not set
+# CONFIG_BLK_DEV_NBD is not set
+CONFIG_BLK_DEV_RAM=y
+CONFIG_BLK_DEV_RAM_COUNT=16
+CONFIG_BLK_DEV_RAM_SIZE=4096
+# CONFIG_BLK_DEV_XIP is not set
+# CONFIG_CDROM_PKTCDVD is not set
+# CONFIG_ATA_OVER_ETH is not set
+# CONFIG_BLK_DEV_HD is not set
+# CONFIG_MISC_DEVICES is not set
+CONFIG_HAVE_IDE=y
+# CONFIG_IDE is not set
+
+#
+# SCSI device support
+#
+# CONFIG_RAID_ATTRS is not set
+# CONFIG_SCSI is not set
+# CONFIG_SCSI_DMA is not set
+# CONFIG_SCSI_NETLINK is not set
+# CONFIG_ATA is not set
+# CONFIG_MD is not set
+CONFIG_NETDEVICES=y
+# CONFIG_DUMMY is not set
+# CONFIG_BONDING is not set
+# CONFIG_MACVLAN is not set
+# CONFIG_EQUALIZER is not set
+# CONFIG_TUN is not set
+# CONFIG_VETH is not set
+CONFIG_PHYLIB=y
+
+#
+# MII PHY device drivers
+#
+# CONFIG_MARVELL_PHY is not set
+# CONFIG_DAVICOM_PHY is not set
+# CONFIG_QSEMI_PHY is not set
+# CONFIG_LXT_PHY is not set
+# CONFIG_CICADA_PHY is not set
+# CONFIG_VITESSE_PHY is not set
+CONFIG_SMSC_PHY=y
+# CONFIG_BROADCOM_PHY is not set
+# CONFIG_ICPLUS_PHY is not set
+# CONFIG_REALTEK_PHY is not set
+# CONFIG_FIXED_PHY is not set
+# CONFIG_MDIO_BITBANG is not set
+CONFIG_NET_ETHERNET=y
+CONFIG_MII=y
+CONFIG_SMC91X=y
+# CONFIG_SMSC911X is not set
+# CONFIG_DM9000 is not set
+# CONFIG_ENC28J60 is not set
+# CONFIG_IBM_NEW_EMAC_ZMII is not set
+# CONFIG_IBM_NEW_EMAC_RGMII is not set
+# CONFIG_IBM_NEW_EMAC_TAH is not set
+# CONFIG_IBM_NEW_EMAC_EMAC4 is not set
+# CONFIG_IBM_NEW_EMAC_NO_FLOW_CTRL is not set
+# CONFIG_IBM_NEW_EMAC_MAL_CLR_ICINTSTAT is not set
+# CONFIG_IBM_NEW_EMAC_MAL_COMMON_ERR is not set
+# CONFIG_NETDEV_1000 is not set
+# CONFIG_NETDEV_10000 is not set
+
+#
+# Wireless LAN
+#
+# CONFIG_WLAN_PRE80211 is not set
+# CONFIG_WLAN_80211 is not set
+# CONFIG_IWLWIFI_LEDS is not set
+# CONFIG_WAN is not set
+# CONFIG_PPP is not set
+# CONFIG_SLIP is not set
+# CONFIG_NETCONSOLE is not set
+# CONFIG_NETPOLL is not set
+# CONFIG_NET_POLL_CONTROLLER is not set
+# CONFIG_ISDN is not set
+# CONFIG_PHONE is not set
+
+#
+# Input device support
+#
+CONFIG_INPUT=y
+# CONFIG_INPUT_FF_MEMLESS is not set
+# CONFIG_INPUT_POLLDEV is not set
+
+#
+# Userland interfaces
+#
+# CONFIG_INPUT_MOUSEDEV is not set
+# CONFIG_INPUT_JOYDEV is not set
+CONFIG_INPUT_EVDEV=m
+# CONFIG_INPUT_EVBUG is not set
+
+#
+# Input Device Drivers
+#
+# CONFIG_INPUT_KEYBOARD is not set
+# CONFIG_INPUT_MOUSE is not set
+# CONFIG_INPUT_JOYSTICK is not set
+# CONFIG_INPUT_TABLET is not set
+CONFIG_INPUT_TOUCHSCREEN=y
+# CONFIG_TOUCHSCREEN_ADS7846 is not set
+# CONFIG_TOUCHSCREEN_AD7877 is not set
+# CONFIG_TOUCHSCREEN_AD7879_I2C is not set
+CONFIG_TOUCHSCREEN_AD7879_SPI=y
+CONFIG_TOUCHSCREEN_AD7879=m
+# CONFIG_TOUCHSCREEN_FUJITSU is not set
+# CONFIG_TOUCHSCREEN_GUNZE is not set
+# CONFIG_TOUCHSCREEN_ELO is not set
+# CONFIG_TOUCHSCREEN_MTOUCH is not set
+# CONFIG_TOUCHSCREEN_INEXIO is not set
+# CONFIG_TOUCHSCREEN_MK712 is not set
+# CONFIG_TOUCHSCREEN_PENMOUNT is not set
+# CONFIG_TOUCHSCREEN_TOUCHRIGHT is not set
+# CONFIG_TOUCHSCREEN_TOUCHWIN is not set
+# CONFIG_TOUCHSCREEN_TOUCHIT213 is not set
+CONFIG_INPUT_MISC=y
+# CONFIG_INPUT_UINPUT is not set
+# CONFIG_CONFIG_INPUT_PCF8574 is not set
+
+#
+# Hardware I/O ports
+#
+# CONFIG_SERIO is not set
+# CONFIG_GAMEPORT is not set
+
+#
+# Character devices
+#
+# CONFIG_AD9960 is not set
+# CONFIG_SPI_ADC_BF533 is not set
+# CONFIG_BF5xx_PPIFCD is not set
+# CONFIG_BFIN_SIMPLE_TIMER is not set
+# CONFIG_BF5xx_PPI is not set
+CONFIG_BFIN_SPORT=y
+# CONFIG_BFIN_TIMER_LATENCY is not set
+# CONFIG_TWI_LCD is not set
+CONFIG_BFIN_DMA_INTERFACE=m
+CONFIG_SIMPLE_GPIO=m
+# CONFIG_VT is not set
+# CONFIG_DEVKMEM is not set
+# CONFIG_BFIN_JTAG_COMM is not set
+# CONFIG_SERIAL_NONSTANDARD is not set
+
+#
+# Serial drivers
+#
+# CONFIG_SERIAL_8250 is not set
+
+#
+# Non-8250 serial port support
+#
+CONFIG_SERIAL_BFIN=y
+CONFIG_SERIAL_BFIN_CONSOLE=y
+CONFIG_SERIAL_BFIN_DMA=y
+# CONFIG_SERIAL_BFIN_PIO is not set
+CONFIG_SERIAL_BFIN_UART0=y
+# CONFIG_BFIN_UART0_CTSRTS is not set
+CONFIG_SERIAL_BFIN_UART1=y
+# CONFIG_BFIN_UART1_CTSRTS is not set
+CONFIG_SERIAL_BFIN_UART2=y
+# CONFIG_BFIN_UART2_CTSRTS is not set
+CONFIG_SERIAL_CORE=y
+CONFIG_SERIAL_CORE_CONSOLE=y
+# CONFIG_SERIAL_BFIN_SPORT is not set
+CONFIG_UNIX98_PTYS=y
+# CONFIG_LEGACY_PTYS is not set
+
+#
+# CAN, the car bus and industrial fieldbus
+#
+# CONFIG_CAN4LINUX is not set
+# CONFIG_IPMI_HANDLER is not set
+# CONFIG_HW_RANDOM is not set
+# CONFIG_R3964 is not set
+# CONFIG_RAW_DRIVER is not set
+# CONFIG_TCG_TPM is not set
+CONFIG_I2C=y
+CONFIG_I2C_BOARDINFO=y
+# CONFIG_I2C_CHARDEV is not set
+CONFIG_I2C_HELPER_AUTO=y
+
+#
+# I2C Hardware Bus support
+#
+
+#
+# I2C system bus drivers (mostly embedded / system-on-chip)
+#
+CONFIG_I2C_BLACKFIN_TWI=y
+CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=100
+# CONFIG_I2C_GPIO is not set
+# CONFIG_I2C_OCORES is not set
+# CONFIG_I2C_SIMTEC is not set
+
+#
+# External I2C/SMBus adapter drivers
+#
+# CONFIG_I2C_PARPORT_LIGHT is not set
+# CONFIG_I2C_TAOS_EVM is not set
+
+#
+# Other I2C/SMBus bus drivers
+#
+# CONFIG_I2C_PCA_PLATFORM is not set
+# CONFIG_I2C_STUB is not set
+
+#
+# Miscellaneous I2C Chip support
+#
+# CONFIG_DS1682 is not set
+# CONFIG_AT24 is not set
+# CONFIG_SENSORS_AD5252 is not set
+# CONFIG_SENSORS_EEPROM is not set
+# CONFIG_SENSORS_PCF8574 is not set
+# CONFIG_PCF8575 is not set
+# CONFIG_SENSORS_PCA9539 is not set
+# CONFIG_SENSORS_PCF8591 is not set
+# CONFIG_SENSORS_MAX6875 is not set
+# CONFIG_SENSORS_TSL2550 is not set
+# CONFIG_I2C_DEBUG_CORE is not set
+# CONFIG_I2C_DEBUG_ALGO is not set
+# CONFIG_I2C_DEBUG_BUS is not set
+# CONFIG_I2C_DEBUG_CHIP is not set
+CONFIG_SPI=y
+# CONFIG_SPI_DEBUG is not set
+CONFIG_SPI_MASTER=y
+
+#
+# SPI Master Controller Drivers
+#
+CONFIG_SPI_BFIN=y
+# CONFIG_SPI_BFIN_LOCK is not set
+# CONFIG_SPI_BITBANG is not set
+
+#
+# SPI Protocol Masters
+#
+# CONFIG_SPI_AT25 is not set
+# CONFIG_SPI_SPIDEV is not set
+# CONFIG_SPI_TLE62X0 is not set
+CONFIG_ARCH_WANT_OPTIONAL_GPIOLIB=y
+# CONFIG_GPIOLIB is not set
+# CONFIG_W1 is not set
+# CONFIG_POWER_SUPPLY is not set
+CONFIG_HWMON=y
+# CONFIG_HWMON_VID is not set
+# CONFIG_SENSORS_AD7414 is not set
+# CONFIG_SENSORS_AD7418 is not set
+# CONFIG_SENSORS_ADCXX is not set
+# CONFIG_SENSORS_ADM1021 is not set
+# CONFIG_SENSORS_ADM1025 is not set
+# CONFIG_SENSORS_ADM1026 is not set
+# CONFIG_SENSORS_ADM1029 is not set
+# CONFIG_SENSORS_ADM1031 is not set
+# CONFIG_SENSORS_ADM9240 is not set
+# CONFIG_SENSORS_ADT7470 is not set
+# CONFIG_SENSORS_ADT7473 is not set
+# CONFIG_SENSORS_ATXP1 is not set
+# CONFIG_SENSORS_DS1621 is not set
+# CONFIG_SENSORS_F71805F is not set
+# CONFIG_SENSORS_F71882FG is not set
+# CONFIG_SENSORS_F75375S is not set
+# CONFIG_SENSORS_GL518SM is not set
+# CONFIG_SENSORS_GL520SM is not set
+# CONFIG_SENSORS_IT87 is not set
+# CONFIG_SENSORS_LM63 is not set
+# CONFIG_SENSORS_LM70 is not set
+# CONFIG_SENSORS_LM75 is not set
+# CONFIG_SENSORS_LM77 is not set
+# CONFIG_SENSORS_LM78 is not set
+# CONFIG_SENSORS_LM80 is not set
+# CONFIG_SENSORS_LM83 is not set
+# CONFIG_SENSORS_LM85 is not set
+# CONFIG_SENSORS_LM87 is not set
+# CONFIG_SENSORS_LM90 is not set
+# CONFIG_SENSORS_LM92 is not set
+# CONFIG_SENSORS_LM93 is not set
+# CONFIG_SENSORS_MAX1111 is not set
+# CONFIG_SENSORS_MAX1619 is not set
+# CONFIG_SENSORS_MAX6650 is not set
+# CONFIG_SENSORS_PC87360 is not set
+# CONFIG_SENSORS_PC87427 is not set
+# CONFIG_SENSORS_DME1737 is not set
+# CONFIG_SENSORS_SMSC47M1 is not set
+# CONFIG_SENSORS_SMSC47M192 is not set
+# CONFIG_SENSORS_SMSC47B397 is not set
+# CONFIG_SENSORS_ADS7828 is not set
+# CONFIG_SENSORS_THMC50 is not set
+# CONFIG_SENSORS_VT1211 is not set
+# CONFIG_SENSORS_W83781D is not set
+# CONFIG_SENSORS_W83791D is not set
+# CONFIG_SENSORS_W83792D is not set
+# CONFIG_SENSORS_W83793 is not set
+# CONFIG_SENSORS_W83L785TS is not set
+# CONFIG_SENSORS_W83L786NG is not set
+# CONFIG_SENSORS_W83627HF is not set
+# CONFIG_SENSORS_W83627EHF is not set
+# CONFIG_HWMON_DEBUG_CHIP is not set
+# CONFIG_THERMAL is not set
+# CONFIG_THERMAL_HWMON is not set
+CONFIG_WATCHDOG=y
+# CONFIG_WATCHDOG_NOWAYOUT is not set
+
+#
+# Watchdog Device Drivers
+#
+# CONFIG_SOFT_WATCHDOG is not set
+CONFIG_BFIN_WDT=y
+
+#
+# Multifunction device drivers
+#
+# CONFIG_MFD_CORE is not set
+# CONFIG_MFD_SM501 is not set
+# CONFIG_HTC_PASIC3 is not set
+# CONFIG_MFD_TMIO is not set
+# CONFIG_MFD_WM8400 is not set
+# CONFIG_MFD_WM8350_I2C is not set
+
+#
+# Multimedia devices
+#
+
+#
+# Multimedia core support
+#
+# CONFIG_VIDEO_DEV is not set
+# CONFIG_DVB_CORE is not set
+# CONFIG_VIDEO_MEDIA is not set
+
+#
+# Multimedia drivers
+#
+# CONFIG_DAB is not set
+
+#
+# Graphics support
+#
+# CONFIG_VGASTATE is not set
+# CONFIG_VIDEO_OUTPUT_CONTROL is not set
+CONFIG_FB=m
+# CONFIG_FIRMWARE_EDID is not set
+# CONFIG_FB_DDC is not set
+# CONFIG_FB_BOOT_VESA_SUPPORT is not set
+CONFIG_FB_CFB_FILLRECT=m
+CONFIG_FB_CFB_COPYAREA=m
+CONFIG_FB_CFB_IMAGEBLIT=m
+# CONFIG_FB_CFB_REV_PIXELS_IN_BYTE is not set
+# CONFIG_FB_SYS_FILLRECT is not set
+# CONFIG_FB_SYS_COPYAREA is not set
+# CONFIG_FB_SYS_IMAGEBLIT is not set
+# CONFIG_FB_FOREIGN_ENDIAN is not set
+# CONFIG_FB_SYS_FOPS is not set
+# CONFIG_FB_SVGALIB is not set
+# CONFIG_FB_MACMODES is not set
+# CONFIG_FB_BACKLIGHT is not set
+# CONFIG_FB_MODE_HELPERS is not set
+# CONFIG_FB_TILEBLITTING is not set
+
+#
+# Frame buffer hardware drivers
+#
+# CONFIG_FB_BFIN_T350MCQB is not set
+CONFIG_FB_BFIN_LQ035Q1=m
+# CONFIG_FB_BFIN_7393 is not set
+# CONFIG_FB_S1D13XXX is not set
+# CONFIG_FB_VIRTUAL is not set
+# CONFIG_FB_METRONOME is not set
+# CONFIG_BACKLIGHT_LCD_SUPPORT is not set
+
+#
+# Display device support
+#
+# CONFIG_DISPLAY_SUPPORT is not set
+# CONFIG_LOGO is not set
+# CONFIG_SOUND is not set
+CONFIG_HID_SUPPORT=y
+CONFIG_HID=y
+# CONFIG_HID_DEBUG is not set
+# CONFIG_HIDRAW is not set
+# CONFIG_HID_PID is not set
+
+#
+# Special HID drivers
+#
+CONFIG_HID_COMPAT=y
+# CONFIG_USB_SUPPORT is not set
+# CONFIG_MMC is not set
+# CONFIG_MEMSTICK is not set
+# CONFIG_NEW_LEDS is not set
+# CONFIG_ACCESSIBILITY is not set
+CONFIG_RTC_LIB=y
+CONFIG_RTC_CLASS=y
+CONFIG_RTC_HCTOSYS=y
+CONFIG_RTC_HCTOSYS_DEVICE="rtc0"
+# CONFIG_RTC_DEBUG is not set
+
+#
+# RTC interfaces
+#
+CONFIG_RTC_INTF_SYSFS=y
+CONFIG_RTC_INTF_PROC=y
+CONFIG_RTC_INTF_DEV=y
+# CONFIG_RTC_INTF_DEV_UIE_EMUL is not set
+# CONFIG_RTC_DRV_TEST is not set
+
+#
+# I2C RTC drivers
+#
+# CONFIG_RTC_DRV_DS1307 is not set
+# CONFIG_RTC_DRV_DS1374 is not set
+# CONFIG_RTC_DRV_DS1672 is not set
+# CONFIG_RTC_DRV_MAX6900 is not set
+# CONFIG_RTC_DRV_RS5C372 is not set
+# CONFIG_RTC_DRV_ISL1208 is not set
+# CONFIG_RTC_DRV_X1205 is not set
+# CONFIG_RTC_DRV_PCF8563 is not set
+# CONFIG_RTC_DRV_PCF8583 is not set
+# CONFIG_RTC_DRV_M41T80 is not set
+# CONFIG_RTC_DRV_S35390A is not set
+# CONFIG_RTC_DRV_FM3130 is not set
+
+#
+# SPI RTC drivers
+#
+# CONFIG_RTC_DRV_M41T94 is not set
+# CONFIG_RTC_DRV_DS1305 is not set
+# CONFIG_RTC_DRV_MAX6902 is not set
+# CONFIG_RTC_DRV_R9701 is not set
+# CONFIG_RTC_DRV_RS5C348 is not set
+# CONFIG_RTC_DRV_DS3234 is not set
+
+#
+# Platform RTC drivers
+#
+# CONFIG_RTC_DRV_DS1286 is not set
+# CONFIG_RTC_DRV_DS1511 is not set
+# CONFIG_RTC_DRV_DS1553 is not set
+# CONFIG_RTC_DRV_DS1742 is not set
+# CONFIG_RTC_DRV_STK17TA8 is not set
+# CONFIG_RTC_DRV_M48T86 is not set
+# CONFIG_RTC_DRV_M48T35 is not set
+# CONFIG_RTC_DRV_M48T59 is not set
+# CONFIG_RTC_DRV_BQ4802 is not set
+# CONFIG_RTC_DRV_V3020 is not set
+
+#
+# on-CPU RTC drivers
+#
+CONFIG_RTC_DRV_BFIN=y
+# CONFIG_DMADEVICES is not set
+# CONFIG_UIO is not set
+# CONFIG_STAGING is not set
+
+#
+# File systems
+#
+# CONFIG_EXT2_FS is not set
+# CONFIG_EXT3_FS is not set
+# CONFIG_EXT4_FS is not set
+# CONFIG_REISERFS_FS is not set
+# CONFIG_JFS_FS is not set
+# CONFIG_FS_POSIX_ACL is not set
+CONFIG_FILE_LOCKING=y
+# CONFIG_XFS_FS is not set
+# CONFIG_OCFS2_FS is not set
+# CONFIG_DNOTIFY is not set
+CONFIG_INOTIFY=y
+CONFIG_INOTIFY_USER=y
+# CONFIG_QUOTA is not set
+# CONFIG_AUTOFS_FS is not set
+# CONFIG_AUTOFS4_FS is not set
+# CONFIG_FUSE_FS is not set
+
+#
+# CD-ROM/DVD Filesystems
+#
+# CONFIG_ISO9660_FS is not set
+# CONFIG_UDF_FS is not set
+
+#
+# DOS/FAT/NT Filesystems
+#
+# CONFIG_MSDOS_FS is not set
+# CONFIG_VFAT_FS is not set
+# CONFIG_NTFS_FS is not set
+
+#
+# Pseudo filesystems
+#
+CONFIG_PROC_FS=y
+CONFIG_PROC_SYSCTL=y
+CONFIG_SYSFS=y
+# CONFIG_TMPFS is not set
+# CONFIG_HUGETLB_PAGE is not set
+# CONFIG_CONFIGFS_FS is not set
+
+#
+# Miscellaneous filesystems
+#
+# CONFIG_ADFS_FS is not set
+# CONFIG_AFFS_FS is not set
+# CONFIG_HFS_FS is not set
+# CONFIG_HFSPLUS_FS is not set
+# CONFIG_BEFS_FS is not set
+# CONFIG_BFS_FS is not set
+# CONFIG_EFS_FS is not set
+CONFIG_YAFFS_FS=m
+CONFIG_YAFFS_YAFFS1=y
+# CONFIG_YAFFS_9BYTE_TAGS is not set
+# CONFIG_YAFFS_DOES_ECC is not set
+CONFIG_YAFFS_YAFFS2=y
+CONFIG_YAFFS_AUTO_YAFFS2=y
+# CONFIG_YAFFS_DISABLE_LAZY_LOAD is not set
+# CONFIG_YAFFS_DISABLE_WIDE_TNODES is not set
+# CONFIG_YAFFS_ALWAYS_CHECK_CHUNK_ERASED is not set
+CONFIG_YAFFS_SHORT_NAMES_IN_RAM=y
+CONFIG_JFFS2_FS=m
+CONFIG_JFFS2_FS_DEBUG=0
+CONFIG_JFFS2_FS_WRITEBUFFER=y
+# CONFIG_JFFS2_FS_WBUF_VERIFY is not set
+# CONFIG_JFFS2_SUMMARY is not set
+# CONFIG_JFFS2_FS_XATTR is not set
+# CONFIG_JFFS2_COMPRESSION_OPTIONS is not set
+CONFIG_JFFS2_ZLIB=y
+# CONFIG_JFFS2_LZO is not set
+CONFIG_JFFS2_RTIME=y
+# CONFIG_JFFS2_RUBIN is not set
+# CONFIG_CRAMFS is not set
+# CONFIG_VXFS_FS is not set
+# CONFIG_MINIX_FS is not set
+# CONFIG_OMFS_FS is not set
+# CONFIG_HPFS_FS is not set
+# CONFIG_QNX4FS_FS is not set
+# CONFIG_ROMFS_FS is not set
+# CONFIG_SYSV_FS is not set
+# CONFIG_UFS_FS is not set
+CONFIG_NETWORK_FILESYSTEMS=y
+CONFIG_NFS_FS=m
+CONFIG_NFS_V3=y
+# CONFIG_NFS_V3_ACL is not set
+# CONFIG_NFS_V4 is not set
+# CONFIG_NFSD is not set
+CONFIG_LOCKD=m
+CONFIG_LOCKD_V4=y
+CONFIG_NFS_COMMON=y
+CONFIG_SUNRPC=m
+# CONFIG_SUNRPC_REGISTER_V4 is not set
+# CONFIG_RPCSEC_GSS_KRB5 is not set
+# CONFIG_RPCSEC_GSS_SPKM3 is not set
+CONFIG_SMB_FS=m
+# CONFIG_SMB_NLS_DEFAULT is not set
+# CONFIG_CIFS is not set
+# CONFIG_NCP_FS is not set
+# CONFIG_CODA_FS is not set
+# CONFIG_AFS_FS is not set
+
+#
+# Partition Types
+#
+# CONFIG_PARTITION_ADVANCED is not set
+CONFIG_MSDOS_PARTITION=y
+CONFIG_NLS=m
+CONFIG_NLS_DEFAULT="iso8859-1"
+# CONFIG_NLS_CODEPAGE_437 is not set
+# CONFIG_NLS_CODEPAGE_737 is not set
+# CONFIG_NLS_CODEPAGE_775 is not set
+# CONFIG_NLS_CODEPAGE_850 is not set
+# CONFIG_NLS_CODEPAGE_852 is not set
+# CONFIG_NLS_CODEPAGE_855 is not set
+# CONFIG_NLS_CODEPAGE_857 is not set
+# CONFIG_NLS_CODEPAGE_860 is not set
+# CONFIG_NLS_CODEPAGE_861 is not set
+# CONFIG_NLS_CODEPAGE_862 is not set
+# CONFIG_NLS_CODEPAGE_863 is not set
+# CONFIG_NLS_CODEPAGE_864 is not set
+# CONFIG_NLS_CODEPAGE_865 is not set
+# CONFIG_NLS_CODEPAGE_866 is not set
+# CONFIG_NLS_CODEPAGE_869 is not set
+# CONFIG_NLS_CODEPAGE_936 is not set
+# CONFIG_NLS_CODEPAGE_950 is not set
+# CONFIG_NLS_CODEPAGE_932 is not set
+# CONFIG_NLS_CODEPAGE_949 is not set
+# CONFIG_NLS_CODEPAGE_874 is not set
+# CONFIG_NLS_ISO8859_8 is not set
+# CONFIG_NLS_CODEPAGE_1250 is not set
+# CONFIG_NLS_CODEPAGE_1251 is not set
+# CONFIG_NLS_ASCII is not set
+# CONFIG_NLS_ISO8859_1 is not set
+# CONFIG_NLS_ISO8859_2 is not set
+# CONFIG_NLS_ISO8859_3 is not set
+# CONFIG_NLS_ISO8859_4 is not set
+# CONFIG_NLS_ISO8859_5 is not set
+# CONFIG_NLS_ISO8859_6 is not set
+# CONFIG_NLS_ISO8859_7 is not set
+# CONFIG_NLS_ISO8859_9 is not set
+# CONFIG_NLS_ISO8859_13 is not set
+# CONFIG_NLS_ISO8859_14 is not set
+# CONFIG_NLS_ISO8859_15 is not set
+# CONFIG_NLS_KOI8_R is not set
+# CONFIG_NLS_KOI8_U is not set
+# CONFIG_NLS_UTF8 is not set
+# CONFIG_DLM is not set
+
+#
+# Kernel hacking
+#
+# CONFIG_PRINTK_TIME is not set
+CONFIG_ENABLE_WARN_DEPRECATED=y
+CONFIG_ENABLE_MUST_CHECK=y
+CONFIG_FRAME_WARN=1024
+# CONFIG_MAGIC_SYSRQ is not set
+# CONFIG_UNUSED_SYMBOLS is not set
+CONFIG_DEBUG_FS=y
+# CONFIG_HEADERS_CHECK is not set
+CONFIG_DEBUG_KERNEL=y
+# CONFIG_DEBUG_SHIRQ is not set
+CONFIG_DETECT_SOFTLOCKUP=y
+# CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC is not set
+CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC_VALUE=0
+CONFIG_SCHED_DEBUG=y
+# CONFIG_SCHEDSTATS is not set
+# CONFIG_TIMER_STATS is not set
+# CONFIG_DEBUG_OBJECTS is not set
+# CONFIG_DEBUG_SLAB is not set
+# CONFIG_DEBUG_RT_MUTEXES is not set
+# CONFIG_RT_MUTEX_TESTER is not set
+# CONFIG_DEBUG_SPINLOCK is not set
+# CONFIG_DEBUG_MUTEXES is not set
+# CONFIG_DEBUG_SPINLOCK_SLEEP is not set
+# CONFIG_DEBUG_LOCKING_API_SELFTESTS is not set
+# CONFIG_DEBUG_KOBJECT is not set
+# CONFIG_DEBUG_BUGVERBOSE is not set
+CONFIG_DEBUG_INFO=y
+# CONFIG_DEBUG_VM is not set
+# CONFIG_DEBUG_WRITECOUNT is not set
+# CONFIG_DEBUG_MEMORY_INIT is not set
+# CONFIG_DEBUG_LIST is not set
+# CONFIG_DEBUG_SG is not set
+# CONFIG_FRAME_POINTER is not set
+# CONFIG_BOOT_PRINTK_DELAY is not set
+# CONFIG_RCU_TORTURE_TEST is not set
+# CONFIG_RCU_CPU_STALL_DETECTOR is not set
+# CONFIG_BACKTRACE_SELF_TEST is not set
+# CONFIG_DEBUG_BLOCK_EXT_DEVT is not set
+# CONFIG_FAULT_INJECTION is not set
+CONFIG_SYSCTL_SYSCALL_CHECK=y
+# CONFIG_DYNAMIC_PRINTK_DEBUG is not set
+# CONFIG_SAMPLES is not set
+CONFIG_HAVE_ARCH_KGDB=y
+# CONFIG_KGDB is not set
+# CONFIG_DEBUG_STACKOVERFLOW is not set
+# CONFIG_DEBUG_STACK_USAGE is not set
+CONFIG_DEBUG_VERBOSE=y
+CONFIG_DEBUG_MMRS=y
+# CONFIG_DEBUG_HWERR is not set
+# CONFIG_DEBUG_DOUBLEFAULT is not set
+CONFIG_DEBUG_HUNT_FOR_ZERO=y
+CONFIG_DEBUG_BFIN_HWTRACE_ON=y
+CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION_OFF=y
+# CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION_ONE is not set
+# CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION_TWO is not set
+CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION=0
+# CONFIG_DEBUG_BFIN_HWTRACE_EXPAND is not set
+# CONFIG_DEBUG_BFIN_NO_KERN_HWTRACE is not set
+CONFIG_EARLY_PRINTK=y
+CONFIG_CPLB_INFO=y
+CONFIG_ACCESS_CHECK=y
+
+#
+# Security options
+#
+# CONFIG_KEYS is not set
+CONFIG_SECURITY=y
+# CONFIG_SECURITYFS is not set
+# CONFIG_SECURITY_NETWORK is not set
+# CONFIG_SECURITY_FILE_CAPABILITIES is not set
+CONFIG_SECURITY_DEFAULT_MMAP_MIN_ADDR=0
+CONFIG_CRYPTO=y
+
+#
+# Crypto core or helper
+#
+# CONFIG_CRYPTO_FIPS is not set
+# CONFIG_CRYPTO_MANAGER is not set
+# CONFIG_CRYPTO_GF128MUL is not set
+# CONFIG_CRYPTO_NULL is not set
+# CONFIG_CRYPTO_CRYPTD is not set
+# CONFIG_CRYPTO_AUTHENC is not set
+# CONFIG_CRYPTO_TEST is not set
+
+#
+# Authenticated Encryption with Associated Data
+#
+# CONFIG_CRYPTO_CCM is not set
+# CONFIG_CRYPTO_GCM is not set
+# CONFIG_CRYPTO_SEQIV is not set
+
+#
+# Block modes
+#
+# CONFIG_CRYPTO_CBC is not set
+# CONFIG_CRYPTO_CTR is not set
+# CONFIG_CRYPTO_CTS is not set
+# CONFIG_CRYPTO_ECB is not set
+# CONFIG_CRYPTO_LRW is not set
+# CONFIG_CRYPTO_PCBC is not set
+# CONFIG_CRYPTO_XTS is not set
+
+#
+# Hash modes
+#
+# CONFIG_CRYPTO_HMAC is not set
+# CONFIG_CRYPTO_XCBC is not set
+
+#
+# Digest
+#
+# CONFIG_CRYPTO_CRC32C is not set
+# CONFIG_CRYPTO_MD4 is not set
+# CONFIG_CRYPTO_MD5 is not set
+# CONFIG_CRYPTO_MICHAEL_MIC is not set
+# CONFIG_CRYPTO_RMD128 is not set
+# CONFIG_CRYPTO_RMD160 is not set
+# CONFIG_CRYPTO_RMD256 is not set
+# CONFIG_CRYPTO_RMD320 is not set
+# CONFIG_CRYPTO_SHA1 is not set
+# CONFIG_CRYPTO_SHA256 is not set
+# CONFIG_CRYPTO_SHA512 is not set
+# CONFIG_CRYPTO_TGR192 is not set
+# CONFIG_CRYPTO_WP512 is not set
+
+#
+# Ciphers
+#
+# CONFIG_CRYPTO_AES is not set
+# CONFIG_CRYPTO_ANUBIS is not set
+# CONFIG_CRYPTO_ARC4 is not set
+# CONFIG_CRYPTO_BLOWFISH is not set
+# CONFIG_CRYPTO_CAMELLIA is not set
+# CONFIG_CRYPTO_CAST5 is not set
+# CONFIG_CRYPTO_CAST6 is not set
+# CONFIG_CRYPTO_DES is not set
+# CONFIG_CRYPTO_FCRYPT is not set
+# CONFIG_CRYPTO_KHAZAD is not set
+# CONFIG_CRYPTO_SALSA20 is not set
+# CONFIG_CRYPTO_SEED is not set
+# CONFIG_CRYPTO_SERPENT is not set
+# CONFIG_CRYPTO_TEA is not set
+# CONFIG_CRYPTO_TWOFISH is not set
+
+#
+# Compression
+#
+# CONFIG_CRYPTO_DEFLATE is not set
+# CONFIG_CRYPTO_LZO is not set
+
+#
+# Random Number Generation
+#
+# CONFIG_CRYPTO_ANSI_CPRNG is not set
+CONFIG_CRYPTO_HW=y
+
+#
+# Library routines
+#
+CONFIG_BITREVERSE=y
+CONFIG_CRC_CCITT=m
+# CONFIG_CRC16 is not set
+# CONFIG_CRC_T10DIF is not set
+# CONFIG_CRC_ITU_T is not set
+CONFIG_CRC32=y
+# CONFIG_CRC7 is not set
+# CONFIG_LIBCRC32C is not set
+CONFIG_ZLIB_INFLATE=y
+CONFIG_ZLIB_DEFLATE=m
+CONFIG_PLIST=y
+CONFIG_HAS_IOMEM=y
+CONFIG_HAS_IOPORT=y
+CONFIG_HAS_DMA=y
diff --git a/arch/blackfin/configs/BF548-EZKIT_defconfig b/arch/blackfin/configs/BF548-EZKIT_defconfig
index bf63660815b9..d4ed9ce1f62f 100644
--- a/arch/blackfin/configs/BF548-EZKIT_defconfig
+++ b/arch/blackfin/configs/BF548-EZKIT_defconfig
@@ -1,6 +1,6 @@
 #
 # Automatically generated make config: don't edit
-# Linux kernel version: 2.6.24.7
+# Linux kernel version: 2.6.28-rc2
 #
 # CONFIG_MMU is not set
 # CONFIG_FPU is not set
@@ -8,7 +8,6 @@ CONFIG_RWSEM_GENERIC_SPINLOCK=y
 # CONFIG_RWSEM_XCHGADD_ALGORITHM is not set
 CONFIG_BLACKFIN=y
 CONFIG_ZONE_DMA=y
-CONFIG_SEMAPHORE_SLEEPERS=y
 CONFIG_GENERIC_FIND_NEXT_BIT=y
 CONFIG_GENERIC_HWEIGHT=y
 CONFIG_GENERIC_HARDIRQS=y
@@ -31,18 +30,16 @@ CONFIG_SYSVIPC_SYSCTL=y
 # CONFIG_POSIX_MQUEUE is not set
 # CONFIG_BSD_PROCESS_ACCT is not set
 # CONFIG_TASKSTATS is not set
-# CONFIG_USER_NS is not set
-# CONFIG_PID_NS is not set
 # CONFIG_AUDIT is not set
 CONFIG_IKCONFIG=y
 CONFIG_IKCONFIG_PROC=y
 CONFIG_LOG_BUF_SHIFT=14
 # CONFIG_CGROUPS is not set
-CONFIG_FAIR_GROUP_SCHED=y
-CONFIG_FAIR_USER_SCHED=y
-# CONFIG_FAIR_CGROUP_SCHED is not set
-CONFIG_SYSFS_DEPRECATED=y
+# CONFIG_GROUP_SCHED is not set
+# CONFIG_SYSFS_DEPRECATED is not set
+# CONFIG_SYSFS_DEPRECATED_V2 is not set
 # CONFIG_RELAY is not set
+# CONFIG_NAMESPACES is not set
 CONFIG_BLK_DEV_INITRD=y
 CONFIG_INITRAMFS_SOURCE=""
 # CONFIG_CC_OPTIMIZE_FOR_SIZE is not set
@@ -51,26 +48,35 @@ CONFIG_EMBEDDED=y
 CONFIG_UID16=y
 CONFIG_SYSCTL_SYSCALL=y
 CONFIG_KALLSYMS=y
+# CONFIG_KALLSYMS_ALL is not set
 # CONFIG_KALLSYMS_EXTRA_PASS is not set
 CONFIG_HOTPLUG=y
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
+CONFIG_COMPAT_BRK=y
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
 CONFIG_EPOLL=y
 CONFIG_SIGNALFD=y
+CONFIG_TIMERFD=y
 CONFIG_EVENTFD=y
+CONFIG_AIO=y
 CONFIG_VM_EVENT_COUNTERS=y
 CONFIG_SLAB=y
 # CONFIG_SLUB is not set
 # CONFIG_SLOB is not set
+# CONFIG_PROFILING is not set
+# CONFIG_MARKERS is not set
+CONFIG_HAVE_OPROFILE=y
+# CONFIG_HAVE_GENERIC_DMA_COHERENT is not set
 CONFIG_SLABINFO=y
 CONFIG_RT_MUTEXES=y
 CONFIG_TINY_SHMEM=y
 CONFIG_BASE_SMALL=0
 CONFIG_MODULES=y
+# CONFIG_MODULE_FORCE_LOAD is not set
 CONFIG_MODULE_UNLOAD=y
 # CONFIG_MODULE_FORCE_UNLOAD is not set
 # CONFIG_MODVERSIONS is not set
@@ -81,6 +87,7 @@ CONFIG_BLOCK=y
 # CONFIG_BLK_DEV_IO_TRACE is not set
 # CONFIG_LSF is not set
 # CONFIG_BLK_DEV_BSG is not set
+# CONFIG_BLK_DEV_INTEGRITY is not set
 
 #
 # IO Schedulers
@@ -94,9 +101,11 @@ CONFIG_DEFAULT_AS=y
 # CONFIG_DEFAULT_CFQ is not set
 # CONFIG_DEFAULT_NOOP is not set
 CONFIG_DEFAULT_IOSCHED="anticipatory"
+CONFIG_CLASSIC_RCU=y
 # CONFIG_PREEMPT_NONE is not set
 CONFIG_PREEMPT_VOLUNTARY=y
 # CONFIG_PREEMPT is not set
+# CONFIG_FREEZER is not set
 
 #
 # Blackfin Processor Options
@@ -105,6 +114,10 @@ CONFIG_PREEMPT_VOLUNTARY=y
 #
 # Processor and Board Settings
 #
+# CONFIG_BF512 is not set
+# CONFIG_BF514 is not set
+# CONFIG_BF516 is not set
+# CONFIG_BF518 is not set
 # CONFIG_BF522 is not set
 # CONFIG_BF523 is not set
 # CONFIG_BF524 is not set
@@ -117,18 +130,23 @@ CONFIG_PREEMPT_VOLUNTARY=y
 # CONFIG_BF534 is not set
 # CONFIG_BF536 is not set
 # CONFIG_BF537 is not set
+# CONFIG_BF538 is not set
+# CONFIG_BF539 is not set
 # CONFIG_BF542 is not set
 # CONFIG_BF544 is not set
 # CONFIG_BF547 is not set
 CONFIG_BF548=y
 # CONFIG_BF549 is not set
 # CONFIG_BF561 is not set
+CONFIG_BF_REV_MIN=0
+CONFIG_BF_REV_MAX=2
 CONFIG_BF_REV_0_0=y
 # CONFIG_BF_REV_0_1 is not set
 # CONFIG_BF_REV_0_2 is not set
 # CONFIG_BF_REV_0_3 is not set
 # CONFIG_BF_REV_0_4 is not set
 # CONFIG_BF_REV_0_5 is not set
+# CONFIG_BF_REV_0_6 is not set
 # CONFIG_BF_REV_ANY is not set
 # CONFIG_BF_REV_NONE is not set
 CONFIG_BF54x=y
@@ -138,15 +156,12 @@ CONFIG_IRQ_SPORT0_RX=9
 CONFIG_IRQ_SPORT0_TX=9
 CONFIG_IRQ_SPORT1_RX=9
 CONFIG_IRQ_SPORT1_TX=9
+CONFIG_IRQ_SPI0=10
 CONFIG_IRQ_UART0_RX=10
 CONFIG_IRQ_UART0_TX=10
 CONFIG_IRQ_UART1_RX=10
 CONFIG_IRQ_UART1_TX=10
 CONFIG_IRQ_CNT=8
-CONFIG_IRQ_USB_INT0=11
-CONFIG_IRQ_USB_INT1=11
-CONFIG_IRQ_USB_INT2=11
-CONFIG_IRQ_USB_DMA=11
 CONFIG_IRQ_TIMER0=11
 CONFIG_IRQ_TIMER1=11
 CONFIG_IRQ_TIMER2=11
@@ -155,9 +170,21 @@ CONFIG_IRQ_TIMER4=11
 CONFIG_IRQ_TIMER5=11
 CONFIG_IRQ_TIMER6=11
 CONFIG_IRQ_TIMER7=11
+CONFIG_IRQ_USB_INT0=11
+CONFIG_IRQ_USB_INT1=11
+CONFIG_IRQ_USB_INT2=11
+CONFIG_IRQ_USB_DMA=11
 CONFIG_IRQ_TIMER8=11
 CONFIG_IRQ_TIMER9=11
 CONFIG_IRQ_TIMER10=11
+CONFIG_IRQ_SPORT2_RX=9
+CONFIG_IRQ_SPORT2_TX=9
+CONFIG_IRQ_SPORT3_RX=9
+CONFIG_IRQ_SPORT3_TX=9
+CONFIG_IRQ_SPI1=10
+CONFIG_IRQ_SPI2=10
+CONFIG_IRQ_TWI0=11
+CONFIG_IRQ_TWI1=11
 CONFIG_BFIN548_EZKIT=y
 # CONFIG_BFIN548_BLUETECHNIX_CM is not set
 
@@ -180,7 +207,6 @@ CONFIG_IRQ_SPORT1_ERR=7
 CONFIG_IRQ_SPI0_ERR=7
 CONFIG_IRQ_UART0_ERR=7
 CONFIG_IRQ_EPPI0=8
-CONFIG_IRQ_SPI0=10
 CONFIG_IRQ_PINT0=12
 CONFIG_IRQ_PINT1=12
 CONFIG_IRQ_MDMAS0=13
@@ -195,18 +221,10 @@ CONFIG_IRQ_SPI2_ERR=7
 CONFIG_IRQ_UART1_ERR=7
 CONFIG_IRQ_UART2_ERR=7
 CONFIG_IRQ_CAN0_ERR=7
-CONFIG_IRQ_SPORT2_RX=9
-CONFIG_IRQ_SPORT2_TX=9
-CONFIG_IRQ_SPORT3_RX=9
-CONFIG_IRQ_SPORT3_TX=9
 CONFIG_IRQ_EPPI1=9
 CONFIG_IRQ_EPPI2=9
-CONFIG_IRQ_SPI1=10
-CONFIG_IRQ_SPI2=10
 CONFIG_IRQ_ATAPI_RX=10
 CONFIG_IRQ_ATAPI_TX=10
-CONFIG_IRQ_TWI0=11
-CONFIG_IRQ_TWI1=11
 CONFIG_IRQ_CAN0_RX=11
 CONFIG_IRQ_CAN0_TX=11
 CONFIG_IRQ_MDMAS2=13
@@ -260,7 +278,6 @@ CONFIG_BOOT_LOAD=0x1000
 #
 CONFIG_CLKIN_HZ=25000000
 # CONFIG_BFIN_KERNEL_CLOCK is not set
-CONFIG_MAX_MEM_SIZE=512
 CONFIG_MAX_VCO_HZ=600000000
 CONFIG_MIN_VCO_HZ=50000000
 CONFIG_MAX_SCLK_HZ=133333333
@@ -274,10 +291,10 @@ CONFIG_HZ_250=y
 # CONFIG_HZ_300 is not set
 # CONFIG_HZ_1000 is not set
 CONFIG_HZ=250
+# CONFIG_SCHED_HRTICK is not set
 CONFIG_GENERIC_TIME=y
 CONFIG_GENERIC_CLOCKEVENTS=y
 # CONFIG_CYCLES_CLOCKSOURCE is not set
-# CONFIG_TICK_ONESHOT is not set
 # CONFIG_NO_HZ is not set
 # CONFIG_HIGH_RES_TIMERS is not set
 CONFIG_GENERIC_CLOCKEVENTS_BUILD=y
@@ -311,6 +328,12 @@ CONFIG_ACCESS_OK_L1=y
 CONFIG_CACHELINE_ALIGNED_L1=y
 # CONFIG_SYSCALL_TAB_L1 is not set
 # CONFIG_CPLB_SWITCH_TAB_L1 is not set
+CONFIG_APP_STACK_L1=y
+
+#
+# Speed Optimizations
+#
+CONFIG_BFIN_INS_LOWOVERHEAD=y
 CONFIG_RAMKERNEL=y
 # CONFIG_ROMKERNEL is not set
 CONFIG_SELECT_MEMORY_MODEL=y
@@ -319,14 +342,13 @@ CONFIG_FLATMEM_MANUAL=y
 # CONFIG_SPARSEMEM_MANUAL is not set
 CONFIG_FLATMEM=y
 CONFIG_FLAT_NODE_MEM_MAP=y
-# CONFIG_SPARSEMEM_STATIC is not set
-# CONFIG_SPARSEMEM_VMEMMAP_ENABLE is not set
+CONFIG_PAGEFLAGS_EXTENDED=y
 CONFIG_SPLIT_PTLOCK_CPUS=4
 # CONFIG_RESOURCES_64BIT is not set
+# CONFIG_PHYS_ADDR_T_64BIT is not set
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_VIRT_TO_BUS=y
 # CONFIG_BFIN_GPTIMERS is not set
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_4M is not set
 CONFIG_DMA_UNCACHED_2M=y
 # CONFIG_DMA_UNCACHED_1M is not set
@@ -341,7 +363,7 @@ CONFIG_BFIN_DCACHE=y
 # CONFIG_BFIN_ICACHE_LOCK is not set
 # CONFIG_BFIN_WB is not set
 CONFIG_BFIN_WT=y
-CONFIG_L1_MAX_PIECE=16
+# CONFIG_BFIN_L2_CACHEABLE is not set
 # CONFIG_MPU is not set
 
 #
@@ -373,7 +395,6 @@ CONFIG_EBIU_FCTLVAL=0x6
 #
 # Bus options (PCI, PCMCIA, EISA, MCA, ISA)
 #
-# CONFIG_PCI is not set
 # CONFIG_ARCH_SUPPORTS_MSI is not set
 # CONFIG_PCCARD is not set
 
@@ -384,23 +405,20 @@ CONFIG_BINFMT_ELF_FDPIC=y
 CONFIG_BINFMT_FLAT=y
 CONFIG_BINFMT_ZFLAT=y
 # CONFIG_BINFMT_SHARED_FLAT is not set
+# CONFIG_HAVE_AOUT is not set
 # CONFIG_BINFMT_MISC is not set
 
 #
 # Power management options
 #
 # CONFIG_PM is not set
-CONFIG_SUSPEND_UP_POSSIBLE=y
+CONFIG_ARCH_SUSPEND_POSSIBLE=y
 # CONFIG_PM_WAKEUP_BY_GPIO is not set
 
 #
 # CPU Frequency scaling
 #
 # CONFIG_CPU_FREQ is not set
-
-#
-# Networking
-#
 CONFIG_NET=y
 
 #
@@ -413,6 +431,7 @@ CONFIG_XFRM=y
 # CONFIG_XFRM_USER is not set
 # CONFIG_XFRM_SUB_POLICY is not set
 # CONFIG_XFRM_MIGRATE is not set
+# CONFIG_XFRM_STATISTICS is not set
 # CONFIG_NET_KEY is not set
 CONFIG_INET=y
 # CONFIG_IP_MULTICAST is not set
@@ -442,8 +461,6 @@ CONFIG_TCP_CONG_CUBIC=y
 CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_TCP_MD5SIG is not set
 # CONFIG_IPV6 is not set
-# CONFIG_INET6_XFRM_TUNNEL is not set
-# CONFIG_INET6_TUNNEL is not set
 # CONFIG_NETLABEL is not set
 # CONFIG_NETWORK_SECMARK is not set
 # CONFIG_NETFILTER is not set
@@ -452,6 +469,7 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_TIPC is not set
 # CONFIG_ATM is not set
 # CONFIG_BRIDGE is not set
+# CONFIG_NET_DSA is not set
 # CONFIG_VLAN_8021Q is not set
 # CONFIG_DECNET is not set
 # CONFIG_LLC2 is not set
@@ -468,6 +486,7 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 #
 # CONFIG_NET_PKTGEN is not set
 # CONFIG_HAMRADIO is not set
+# CONFIG_CAN is not set
 CONFIG_IRDA=m
 
 #
@@ -493,9 +512,9 @@ CONFIG_IRCOMM=m
 #
 CONFIG_IRTTY_SIR=m
 CONFIG_BFIN_SIR=m
+CONFIG_BFIN_SIR3=y
 # CONFIG_BFIN_SIR0 is not set
 # CONFIG_BFIN_SIR2 is not set
-CONFIG_BFIN_SIR3=y
 CONFIG_SIR_BFIN_DMA=y
 # CONFIG_SIR_BFIN_PIO is not set
 
@@ -508,15 +527,6 @@ CONFIG_SIR_BFIN_DMA=y
 # CONFIG_KS959_DONGLE is not set
 
 #
-# Old SIR device drivers
-#
-# CONFIG_IRPORT_SIR is not set
-
-#
-# Old Serial dongle support
-#
-
-#
 # FIR device drivers
 #
 # CONFIG_USB_IRDA is not set
@@ -524,11 +534,10 @@ CONFIG_SIR_BFIN_DMA=y
 # CONFIG_MCS_FIR is not set
 # CONFIG_BT is not set
 # CONFIG_AF_RXRPC is not set
-
-#
-# Wireless
-#
+# CONFIG_PHONET is not set
+CONFIG_WIRELESS=y
 # CONFIG_CFG80211 is not set
+CONFIG_WIRELESS_OLD_REGULATORY=y
 # CONFIG_WIRELESS_EXT is not set
 # CONFIG_MAC80211 is not set
 # CONFIG_IEEE80211 is not set
@@ -546,6 +555,8 @@ CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
 CONFIG_STANDALONE=y
 CONFIG_PREVENT_FIRMWARE_BUILD=y
 # CONFIG_FW_LOADER is not set
+# CONFIG_DEBUG_DRIVER is not set
+# CONFIG_DEBUG_DEVRES is not set
 # CONFIG_SYS_HYPERVISOR is not set
 # CONFIG_CONNECTOR is not set
 CONFIG_MTD=y
@@ -554,6 +565,7 @@ CONFIG_MTD=y
 CONFIG_MTD_PARTITIONS=y
 # CONFIG_MTD_REDBOOT_PARTS is not set
 CONFIG_MTD_CMDLINE_PARTS=y
+# CONFIG_MTD_AR7_PARTS is not set
 
 #
 # User Modules And Translation Layers
@@ -601,6 +613,7 @@ CONFIG_MTD_PHYSMAP=y
 CONFIG_MTD_PHYSMAP_START=0x20000000
 CONFIG_MTD_PHYSMAP_LEN=0
 CONFIG_MTD_PHYSMAP_BANKWIDTH=2
+# CONFIG_MTD_GPIO_ADDR is not set
 # CONFIG_MTD_UCLINUX is not set
 # CONFIG_MTD_PLATRAM is not set
 
@@ -608,7 +621,8 @@ CONFIG_MTD_PHYSMAP_BANKWIDTH=2
 # Self-contained MTD device drivers
 #
 # CONFIG_MTD_DATAFLASH is not set
-# CONFIG_MTD_M25P80 is not set
+CONFIG_MTD_M25P80=y
+CONFIG_M25PXX_USE_FAST_READ=y
 # CONFIG_MTD_SLRAM is not set
 # CONFIG_MTD_PHRAM is not set
 # CONFIG_MTD_MTDRAM is not set
@@ -648,11 +662,14 @@ CONFIG_BLK_DEV=y
 CONFIG_BLK_DEV_RAM=y
 CONFIG_BLK_DEV_RAM_COUNT=16
 CONFIG_BLK_DEV_RAM_SIZE=4096
-CONFIG_BLK_DEV_RAM_BLOCKSIZE=1024
+# CONFIG_BLK_DEV_XIP is not set
 # CONFIG_CDROM_PKTCDVD is not set
 # CONFIG_ATA_OVER_ETH is not set
+# CONFIG_BLK_DEV_HD is not set
 CONFIG_MISC_DEVICES=y
 # CONFIG_EEPROM_93CX6 is not set
+# CONFIG_ENCLOSURE_SERVICES is not set
+CONFIG_HAVE_IDE=y
 # CONFIG_IDE is not set
 
 #
@@ -696,13 +713,16 @@ CONFIG_SCSI_WAIT_SCAN=m
 CONFIG_SCSI_LOWLEVEL=y
 # CONFIG_ISCSI_TCP is not set
 # CONFIG_SCSI_DEBUG is not set
+# CONFIG_SCSI_DH is not set
 CONFIG_ATA=y
 # CONFIG_ATA_NONSTANDARD is not set
+CONFIG_SATA_PMP=y
+CONFIG_ATA_SFF=y
+# CONFIG_SATA_MV is not set
 # CONFIG_PATA_PLATFORM is not set
 CONFIG_PATA_BF54X=y
 # CONFIG_MD is not set
 CONFIG_NETDEVICES=y
-# CONFIG_NETDEVICES_MULTIQUEUE is not set
 # CONFIG_DUMMY is not set
 # CONFIG_BONDING is not set
 # CONFIG_MACVLAN is not set
@@ -715,11 +735,14 @@ CONFIG_MII=y
 # CONFIG_SMC91X is not set
 CONFIG_SMSC911X=y
 # CONFIG_DM9000 is not set
+# CONFIG_ENC28J60 is not set
 # CONFIG_IBM_NEW_EMAC_ZMII is not set
 # CONFIG_IBM_NEW_EMAC_RGMII is not set
 # CONFIG_IBM_NEW_EMAC_TAH is not set
 # CONFIG_IBM_NEW_EMAC_EMAC4 is not set
-# CONFIG_B44 is not set
+# CONFIG_IBM_NEW_EMAC_NO_FLOW_CTRL is not set
+# CONFIG_IBM_NEW_EMAC_MAL_CLR_ICINTSTAT is not set
+# CONFIG_IBM_NEW_EMAC_MAL_COMMON_ERR is not set
 CONFIG_NETDEV_1000=y
 # CONFIG_AX88180 is not set
 CONFIG_NETDEV_10000=y
@@ -729,6 +752,7 @@ CONFIG_NETDEV_10000=y
 #
 # CONFIG_WLAN_PRE80211 is not set
 # CONFIG_WLAN_80211 is not set
+# CONFIG_IWLWIFI_LEDS is not set
 
 #
 # USB Network Adapters
@@ -741,7 +765,6 @@ CONFIG_NETDEV_10000=y
 # CONFIG_WAN is not set
 # CONFIG_PPP is not set
 # CONFIG_SLIP is not set
-# CONFIG_SHAPER is not set
 # CONFIG_NETCONSOLE is not set
 # CONFIG_NETPOLL is not set
 # CONFIG_NET_POLL_CONTROLLER is not set
@@ -752,7 +775,7 @@ CONFIG_NETDEV_10000=y
 # Input device support
 #
 CONFIG_INPUT=y
-# CONFIG_INPUT_FF_MEMLESS is not set
+CONFIG_INPUT_FF_MEMLESS=m
 # CONFIG_INPUT_POLLDEV is not set
 
 #
@@ -776,30 +799,37 @@ CONFIG_INPUT_KEYBOARD=y
 # CONFIG_KEYBOARD_GPIO is not set
 CONFIG_KEYBOARD_BFIN=y
 # CONFIG_KEYBOARD_OPENCORES is not set
+# CONFIG_KEYBOARD_ADP5588 is not set
 # CONFIG_INPUT_MOUSE is not set
 # CONFIG_INPUT_JOYSTICK is not set
 # CONFIG_INPUT_TABLET is not set
 CONFIG_INPUT_TOUCHSCREEN=y
 # CONFIG_TOUCHSCREEN_ADS7846 is not set
 CONFIG_TOUCHSCREEN_AD7877=m
+# CONFIG_TOUCHSCREEN_AD7879_I2C is not set
+# CONFIG_TOUCHSCREEN_AD7879_SPI is not set
+# CONFIG_TOUCHSCREEN_AD7879 is not set
 # CONFIG_TOUCHSCREEN_FUJITSU is not set
 # CONFIG_TOUCHSCREEN_GUNZE is not set
 # CONFIG_TOUCHSCREEN_ELO is not set
 # CONFIG_TOUCHSCREEN_MTOUCH is not set
+# CONFIG_TOUCHSCREEN_INEXIO is not set
 # CONFIG_TOUCHSCREEN_MK712 is not set
 # CONFIG_TOUCHSCREEN_PENMOUNT is not set
 # CONFIG_TOUCHSCREEN_TOUCHRIGHT is not set
 # CONFIG_TOUCHSCREEN_TOUCHWIN is not set
-# CONFIG_TOUCHSCREEN_UCB1400 is not set
+# CONFIG_TOUCHSCREEN_WM97XX is not set
 # CONFIG_TOUCHSCREEN_USB_COMPOSITE is not set
+# CONFIG_TOUCHSCREEN_TOUCHIT213 is not set
 CONFIG_INPUT_MISC=y
 # CONFIG_INPUT_ATI_REMOTE is not set
 # CONFIG_INPUT_ATI_REMOTE2 is not set
 # CONFIG_INPUT_KEYSPAN_REMOTE is not set
 # CONFIG_INPUT_POWERMATE is not set
 # CONFIG_INPUT_YEALINK is not set
+# CONFIG_INPUT_CM109 is not set
 # CONFIG_INPUT_UINPUT is not set
-# CONFIG_TWI_KEYPAD is not set
+# CONFIG_CONFIG_INPUT_PCF8574 is not set
 
 #
 # Hardware I/O ports
@@ -815,16 +845,18 @@ CONFIG_INPUT_MISC=y
 # CONFIG_BF5xx_PPIFCD is not set
 # CONFIG_BFIN_SIMPLE_TIMER is not set
 # CONFIG_BF5xx_PPI is not set
-CONFIG_BFIN_OTP=y
-# CONFIG_BFIN_OTP_WRITE_ENABLE is not set
 # CONFIG_BFIN_SPORT is not set
 # CONFIG_BFIN_TIMER_LATENCY is not set
 # CONFIG_TWI_LCD is not set
+CONFIG_BFIN_DMA_INTERFACE=m
 CONFIG_SIMPLE_GPIO=m
 CONFIG_VT=y
+CONFIG_CONSOLE_TRANSLATIONS=y
 CONFIG_VT_CONSOLE=y
 CONFIG_HW_CONSOLE=y
 # CONFIG_VT_HW_CONSOLE_BINDING is not set
+# CONFIG_DEVKMEM is not set
+# CONFIG_BFIN_JTAG_COMM is not set
 # CONFIG_SERIAL_NONSTANDARD is not set
 
 #
@@ -849,6 +881,8 @@ CONFIG_SERIAL_CORE_CONSOLE=y
 # CONFIG_SERIAL_BFIN_SPORT is not set
 CONFIG_UNIX98_PTYS=y
 # CONFIG_LEGACY_PTYS is not set
+CONFIG_BFIN_OTP=y
+# CONFIG_BFIN_OTP_WRITE_ENABLE is not set
 
 #
 # CAN, the car bus and industrial fieldbus
@@ -856,44 +890,49 @@ CONFIG_UNIX98_PTYS=y
 # CONFIG_CAN4LINUX is not set
 # CONFIG_IPMI_HANDLER is not set
 # CONFIG_HW_RANDOM is not set
-# CONFIG_GEN_RTC is not set
 # CONFIG_R3964 is not set
 # CONFIG_RAW_DRIVER is not set
 # CONFIG_TCG_TPM is not set
 CONFIG_I2C=y
 CONFIG_I2C_BOARDINFO=y
 CONFIG_I2C_CHARDEV=y
+CONFIG_I2C_HELPER_AUTO=y
 
 #
-# I2C Algorithms
+# I2C Hardware Bus support
 #
-# CONFIG_I2C_ALGOBIT is not set
-# CONFIG_I2C_ALGOPCF is not set
-# CONFIG_I2C_ALGOPCA is not set
 
 #
-# I2C Hardware Bus support
+# I2C system bus drivers (mostly embedded / system-on-chip)
 #
 CONFIG_I2C_BLACKFIN_TWI=y
-CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=50
+CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=100
 # CONFIG_I2C_GPIO is not set
 # CONFIG_I2C_OCORES is not set
-# CONFIG_I2C_PARPORT_LIGHT is not set
 # CONFIG_I2C_SIMTEC is not set
+
+#
+# External I2C/SMBus adapter drivers
+#
+# CONFIG_I2C_PARPORT_LIGHT is not set
 # CONFIG_I2C_TAOS_EVM is not set
-# CONFIG_I2C_STUB is not set
 # CONFIG_I2C_TINY_USB is not set
 
 #
+# Other I2C/SMBus bus drivers
+#
+# CONFIG_I2C_PCA_PLATFORM is not set
+# CONFIG_I2C_STUB is not set
+
+#
 # Miscellaneous I2C Chip support
 #
-# CONFIG_SENSORS_DS1337 is not set
-# CONFIG_SENSORS_DS1374 is not set
 # CONFIG_DS1682 is not set
+# CONFIG_AT24 is not set
 # CONFIG_SENSORS_AD5252 is not set
 # CONFIG_SENSORS_EEPROM is not set
 # CONFIG_SENSORS_PCF8574 is not set
-# CONFIG_SENSORS_PCF8575 is not set
+# CONFIG_PCF8575 is not set
 # CONFIG_SENSORS_PCA9539 is not set
 # CONFIG_SENSORS_PCF8591 is not set
 # CONFIG_SENSORS_MAX6875 is not set
@@ -902,17 +941,15 @@ CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=50
 # CONFIG_I2C_DEBUG_ALGO is not set
 # CONFIG_I2C_DEBUG_BUS is not set
 # CONFIG_I2C_DEBUG_CHIP is not set
-
-#
-# SPI support
-#
 CONFIG_SPI=y
+# CONFIG_SPI_DEBUG is not set
 CONFIG_SPI_MASTER=y
 
 #
 # SPI Master Controller Drivers
 #
 CONFIG_SPI_BFIN=y
+# CONFIG_SPI_BFIN_LOCK is not set
 # CONFIG_SPI_BITBANG is not set
 
 #
@@ -921,11 +958,15 @@ CONFIG_SPI_BFIN=y
 # CONFIG_SPI_AT25 is not set
 # CONFIG_SPI_SPIDEV is not set
 # CONFIG_SPI_TLE62X0 is not set
+CONFIG_ARCH_WANT_OPTIONAL_GPIOLIB=y
+# CONFIG_GPIOLIB is not set
 # CONFIG_W1 is not set
 # CONFIG_POWER_SUPPLY is not set
 CONFIG_HWMON=y
 # CONFIG_HWMON_VID is not set
+# CONFIG_SENSORS_AD7414 is not set
 # CONFIG_SENSORS_AD7418 is not set
+# CONFIG_SENSORS_ADCXX is not set
 # CONFIG_SENSORS_ADM1021 is not set
 # CONFIG_SENSORS_ADM1025 is not set
 # CONFIG_SENSORS_ADM1026 is not set
@@ -933,6 +974,7 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_ADM1031 is not set
 # CONFIG_SENSORS_ADM9240 is not set
 # CONFIG_SENSORS_ADT7470 is not set
+# CONFIG_SENSORS_ADT7473 is not set
 # CONFIG_SENSORS_ATXP1 is not set
 # CONFIG_SENSORS_DS1621 is not set
 # CONFIG_SENSORS_F71805F is not set
@@ -953,6 +995,7 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_LM90 is not set
 # CONFIG_SENSORS_LM92 is not set
 # CONFIG_SENSORS_LM93 is not set
+# CONFIG_SENSORS_MAX1111 is not set
 # CONFIG_SENSORS_MAX1619 is not set
 # CONFIG_SENSORS_MAX6650 is not set
 # CONFIG_SENSORS_PC87360 is not set
@@ -961,6 +1004,7 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_SMSC47M1 is not set
 # CONFIG_SENSORS_SMSC47M192 is not set
 # CONFIG_SENSORS_SMSC47B397 is not set
+# CONFIG_SENSORS_ADS7828 is not set
 # CONFIG_SENSORS_THMC50 is not set
 # CONFIG_SENSORS_VT1211 is not set
 # CONFIG_SENSORS_W83781D is not set
@@ -968,9 +1012,12 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_W83792D is not set
 # CONFIG_SENSORS_W83793 is not set
 # CONFIG_SENSORS_W83L785TS is not set
+# CONFIG_SENSORS_W83L786NG is not set
 # CONFIG_SENSORS_W83627HF is not set
 # CONFIG_SENSORS_W83627EHF is not set
 # CONFIG_HWMON_DEBUG_CHIP is not set
+# CONFIG_THERMAL is not set
+# CONFIG_THERMAL_HWMON is not set
 CONFIG_WATCHDOG=y
 # CONFIG_WATCHDOG_NOWAYOUT is not set
 
@@ -986,23 +1033,30 @@ CONFIG_BFIN_WDT=y
 # CONFIG_USBPCWATCHDOG is not set
 
 #
-# Sonics Silicon Backplane
-#
-CONFIG_SSB_POSSIBLE=y
-# CONFIG_SSB is not set
-
-#
 # Multifunction device drivers
 #
+# CONFIG_MFD_CORE is not set
 # CONFIG_MFD_SM501 is not set
+# CONFIG_HTC_PASIC3 is not set
+# CONFIG_MFD_TMIO is not set
+# CONFIG_MFD_WM8400 is not set
+# CONFIG_MFD_WM8350_I2C is not set
 
 #
 # Multimedia devices
 #
+
+#
+# Multimedia core support
+#
 # CONFIG_VIDEO_DEV is not set
 # CONFIG_DVB_CORE is not set
+# CONFIG_VIDEO_MEDIA is not set
+
+#
+# Multimedia drivers
+#
 # CONFIG_DAB is not set
-# CONFIG_USB_DABUSB is not set
 
 #
 # Graphics support
@@ -1012,6 +1066,7 @@ CONFIG_SSB_POSSIBLE=y
 CONFIG_FB=y
 CONFIG_FIRMWARE_EDID=y
 # CONFIG_FB_DDC is not set
+# CONFIG_FB_BOOT_VESA_SUPPORT is not set
 CONFIG_FB_CFB_FILLRECT=y
 CONFIG_FB_CFB_COPYAREA=y
 CONFIG_FB_CFB_IMAGEBLIT=y
@@ -1019,8 +1074,8 @@ CONFIG_FB_CFB_IMAGEBLIT=y
 # CONFIG_FB_SYS_FILLRECT is not set
 # CONFIG_FB_SYS_COPYAREA is not set
 # CONFIG_FB_SYS_IMAGEBLIT is not set
+# CONFIG_FB_FOREIGN_ENDIAN is not set
 # CONFIG_FB_SYS_FOPS is not set
-CONFIG_FB_DEFERRED_IO=y
 # CONFIG_FB_SVGALIB is not set
 # CONFIG_FB_MACMODES is not set
 # CONFIG_FB_BACKLIGHT is not set
@@ -1032,9 +1087,11 @@ CONFIG_FB_DEFERRED_IO=y
 #
 CONFIG_FB_BF54X_LQ043=y
 # CONFIG_FB_BFIN_T350MCQB is not set
+# CONFIG_FB_BFIN_LQ035Q1 is not set
 # CONFIG_FB_BFIN_7393 is not set
 # CONFIG_FB_S1D13XXX is not set
 # CONFIG_FB_VIRTUAL is not set
+# CONFIG_FB_METRONOME is not set
 # CONFIG_BACKLIGHT_LCD_SUPPORT is not set
 
 #
@@ -1066,15 +1123,8 @@ CONFIG_LOGO=y
 # CONFIG_LOGO_LINUX_CLUT224 is not set
 # CONFIG_LOGO_BLACKFIN_VGA16 is not set
 CONFIG_LOGO_BLACKFIN_CLUT224=y
-
-#
-# Sound
-#
 CONFIG_SOUND=y
-
-#
-# Advanced Linux Sound Architecture
-#
+CONFIG_SOUND_OSS_CORE=y
 CONFIG_SND=y
 CONFIG_SND_TIMER=y
 CONFIG_SND_PCM=y
@@ -1088,56 +1138,35 @@ CONFIG_SND_SUPPORT_OLD_API=y
 CONFIG_SND_VERBOSE_PROCFS=y
 # CONFIG_SND_VERBOSE_PRINTK is not set
 # CONFIG_SND_DEBUG is not set
-
-#
-# Generic devices
-#
+CONFIG_SND_DRIVERS=y
 # CONFIG_SND_DUMMY is not set
 # CONFIG_SND_MTPAV is not set
 # CONFIG_SND_SERIAL_U16550 is not set
 # CONFIG_SND_MPU401 is not set
-
-#
-# SPI devices
-#
+CONFIG_SND_SPI=y
 
 #
 # ALSA Blackfin devices
 #
 # CONFIG_SND_BLACKFIN_AD1836 is not set
-# CONFIG_SND_BFIN_AD73311 is not set
 # CONFIG_SND_BFIN_AD73322 is not set
-
-#
-# USB devices
-#
+CONFIG_SND_USB=y
 # CONFIG_SND_USB_AUDIO is not set
 # CONFIG_SND_USB_CAIAQ is not set
-
-#
-# System on Chip audio support
-#
-CONFIG_SND_SOC_AC97_BUS=y
 CONFIG_SND_SOC=y
-CONFIG_SND_BF5XX_SOC=y
-CONFIG_SND_MMAP_SUPPORT=y
+CONFIG_SND_SOC_AC97_BUS=y
+# CONFIG_SND_BF5XX_I2S is not set
+CONFIG_SND_BF5XX_AC97=y
+CONFIG_SND_BF5XX_MMAP_SUPPORT=y
+# CONFIG_SND_BF5XX_MULTICHAN_SUPPORT is not set
+CONFIG_SND_BF5XX_SOC_SPORT=y
 CONFIG_SND_BF5XX_SOC_AC97=y
-CONFIG_SND_BF5XX_SOC_BF548_EZKIT=y
-# CONFIG_SND_BF5XX_SOC_WM8750 is not set
-# CONFIG_SND_BF5XX_SOC_WM8731 is not set
-# CONFIG_SND_BF5XX_SOC_SSM2602 is not set
+CONFIG_SND_BF5XX_SOC_AD1980=y
 CONFIG_SND_BF5XX_SPORT_NUM=0
 CONFIG_SND_BF5XX_HAVE_COLD_RESET=y
 CONFIG_SND_BF5XX_RESET_GPIO_NUM=19
-
-#
-# SoC Audio support for SuperH
-#
+# CONFIG_SND_SOC_ALL_CODECS is not set
 CONFIG_SND_SOC_AD1980=y
-
-#
-# Open Sound System
-#
 # CONFIG_SOUND_PRIME is not set
 CONFIG_AC97_BUS=y
 CONFIG_HID_SUPPORT=y
@@ -1149,15 +1178,43 @@ CONFIG_HID=y
 # USB Input Devices
 #
 CONFIG_USB_HID=y
-# CONFIG_USB_HIDINPUT_POWERBOOK is not set
-# CONFIG_HID_FF is not set
+# CONFIG_HID_PID is not set
 # CONFIG_USB_HIDDEV is not set
+
+#
+# Special HID drivers
+#
+CONFIG_HID_COMPAT=y
+CONFIG_HID_A4TECH=y
+CONFIG_HID_APPLE=y
+CONFIG_HID_BELKIN=y
+CONFIG_HID_BRIGHT=y
+CONFIG_HID_CHERRY=y
+CONFIG_HID_CHICONY=y
+CONFIG_HID_CYPRESS=y
+CONFIG_HID_DELL=y
+CONFIG_HID_EZKEY=y
+CONFIG_HID_GYRATION=y
+CONFIG_HID_LOGITECH=y
+# CONFIG_LOGITECH_FF is not set
+# CONFIG_LOGIRUMBLEPAD2_FF is not set
+CONFIG_HID_MICROSOFT=y
+CONFIG_HID_MONTEREY=y
+CONFIG_HID_PANTHERLORD=y
+# CONFIG_PANTHERLORD_FF is not set
+CONFIG_HID_PETALYNX=y
+CONFIG_HID_SAMSUNG=y
+CONFIG_HID_SONY=y
+CONFIG_HID_SUNPLUS=y
+CONFIG_THRUSTMASTER_FF=m
+CONFIG_ZEROPLUS_FF=m
 CONFIG_USB_SUPPORT=y
 CONFIG_USB_ARCH_HAS_HCD=y
 # CONFIG_USB_ARCH_HAS_OHCI is not set
 # CONFIG_USB_ARCH_HAS_EHCI is not set
 CONFIG_USB=y
 # CONFIG_USB_DEBUG is not set
+# CONFIG_USB_ANNOUNCE_NEW_DEVICES is not set
 
 #
 # Miscellaneous USB options
@@ -1168,15 +1225,20 @@ CONFIG_USB_DEVICE_CLASS=y
 # CONFIG_USB_OTG is not set
 # CONFIG_USB_OTG_WHITELIST is not set
 CONFIG_USB_OTG_BLACKLIST_HUB=y
+CONFIG_USB_MON=y
+# CONFIG_USB_WUSB is not set
+# CONFIG_USB_WUSB_CBAF is not set
 
 #
 # USB Host Controller Drivers
 #
+# CONFIG_USB_C67X00_HCD is not set
 # CONFIG_USB_ISP116X_HCD is not set
-# CONFIG_USB_ISP1362_HCD is not set
 # CONFIG_USB_ISP1760_HCD is not set
+# CONFIG_USB_ISP1362_HCD is not set
 # CONFIG_USB_SL811_HCD is not set
 # CONFIG_USB_R8A66597_HCD is not set
+# CONFIG_USB_HWA_HCD is not set
 CONFIG_USB_MUSB_HDRC=y
 CONFIG_USB_MUSB_SOC=y
 
@@ -1190,13 +1252,15 @@ CONFIG_USB_MUSB_HDRC_HCD=y
 # CONFIG_MUSB_PIO_ONLY is not set
 CONFIG_USB_INVENTRA_DMA=y
 # CONFIG_USB_TI_CPPI_DMA is not set
-CONFIG_USB_MUSB_LOGLEVEL=0
+# CONFIG_USB_MUSB_DEBUG is not set
 
 #
 # USB Device Class drivers
 #
 # CONFIG_USB_ACM is not set
 # CONFIG_USB_PRINTER is not set
+# CONFIG_USB_WDM is not set
+# CONFIG_USB_TMC is not set
 
 #
 # NOTE: USB_STORAGE enables SCSI, and 'SCSI disk support'
@@ -1218,6 +1282,7 @@ CONFIG_USB_STORAGE=m
 # CONFIG_USB_STORAGE_ALAUDA is not set
 # CONFIG_USB_STORAGE_ONETOUCH is not set
 # CONFIG_USB_STORAGE_KARMA is not set
+# CONFIG_USB_STORAGE_CYPRESS_ATACB is not set
 # CONFIG_USB_LIBUSUAL is not set
 
 #
@@ -1225,15 +1290,10 @@ CONFIG_USB_STORAGE=m
 #
 # CONFIG_USB_MDC800 is not set
 # CONFIG_USB_MICROTEK is not set
-CONFIG_USB_MON=y
 
 #
 # USB port drivers
 #
-
-#
-# USB Serial Converter support
-#
 # CONFIG_USB_SERIAL is not set
 
 #
@@ -1242,7 +1302,7 @@ CONFIG_USB_MON=y
 # CONFIG_USB_EMI62 is not set
 # CONFIG_USB_EMI26 is not set
 # CONFIG_USB_ADUTUX is not set
-# CONFIG_USB_AUERSWALD is not set
+# CONFIG_USB_SEVSEG is not set
 # CONFIG_USB_RIO500 is not set
 # CONFIG_USB_LEGOTOWER is not set
 # CONFIG_USB_LCD is not set
@@ -1258,34 +1318,31 @@ CONFIG_USB_MON=y
 # CONFIG_USB_LD is not set
 # CONFIG_USB_TRANCEVIBRATOR is not set
 # CONFIG_USB_IOWARRIOR is not set
-
-#
-# USB DSL modem support
-#
-
-#
-# USB Gadget Support
-#
+# CONFIG_USB_ISIGHTFW is not set
+# CONFIG_USB_VST is not set
 # CONFIG_USB_GADGET is not set
 CONFIG_MMC=m
 # CONFIG_MMC_DEBUG is not set
 # CONFIG_MMC_UNSAFE_RESUME is not set
 
 #
-# MMC/SD Card Drivers
+# MMC/SD/SDIO Card Drivers
 #
 CONFIG_MMC_BLOCK=m
 CONFIG_MMC_BLOCK_BOUNCE=y
 # CONFIG_SDIO_UART is not set
+# CONFIG_MMC_TEST is not set
 
 #
-# MMC/SD Host Controller Drivers
+# MMC/SD/SDIO Host Controller Drivers
 #
+# CONFIG_MMC_SDHCI is not set
 CONFIG_SDH_BFIN=m
 # CONFIG_SDH_BFIN_MISSING_CMD_PULLUP_WORKAROUND is not set
 # CONFIG_MMC_SPI is not set
-# CONFIG_SPI_MMC is not set
+# CONFIG_MEMSTICK is not set
 # CONFIG_NEW_LEDS is not set
+# CONFIG_ACCESSIBILITY is not set
 CONFIG_RTC_LIB=y
 CONFIG_RTC_CLASS=y
 CONFIG_RTC_HCTOSYS=y
@@ -1314,32 +1371,40 @@ CONFIG_RTC_INTF_DEV=y
 # CONFIG_RTC_DRV_PCF8563 is not set
 # CONFIG_RTC_DRV_PCF8583 is not set
 # CONFIG_RTC_DRV_M41T80 is not set
+# CONFIG_RTC_DRV_S35390A is not set
+# CONFIG_RTC_DRV_FM3130 is not set
 
 #
 # SPI RTC drivers
 #
-# CONFIG_RTC_DRV_RS5C348 is not set
+# CONFIG_RTC_DRV_M41T94 is not set
+# CONFIG_RTC_DRV_DS1305 is not set
 # CONFIG_RTC_DRV_MAX6902 is not set
+# CONFIG_RTC_DRV_R9701 is not set
+# CONFIG_RTC_DRV_RS5C348 is not set
+# CONFIG_RTC_DRV_DS3234 is not set
 
 #
 # Platform RTC drivers
 #
+# CONFIG_RTC_DRV_DS1286 is not set
+# CONFIG_RTC_DRV_DS1511 is not set
 # CONFIG_RTC_DRV_DS1553 is not set
-# CONFIG_RTC_DRV_STK17TA8 is not set
 # CONFIG_RTC_DRV_DS1742 is not set
+# CONFIG_RTC_DRV_STK17TA8 is not set
 # CONFIG_RTC_DRV_M48T86 is not set
+# CONFIG_RTC_DRV_M48T35 is not set
 # CONFIG_RTC_DRV_M48T59 is not set
+# CONFIG_RTC_DRV_BQ4802 is not set
 # CONFIG_RTC_DRV_V3020 is not set
 
 #
 # on-CPU RTC drivers
 #
 CONFIG_RTC_DRV_BFIN=y
-
-#
-# Userspace I/O
-#
+# CONFIG_DMADEVICES is not set
 # CONFIG_UIO is not set
+# CONFIG_STAGING is not set
 
 #
 # File systems
@@ -1352,22 +1417,20 @@ CONFIG_EXT3_FS=y
 CONFIG_EXT3_FS_XATTR=y
 # CONFIG_EXT3_FS_POSIX_ACL is not set
 # CONFIG_EXT3_FS_SECURITY is not set
-# CONFIG_EXT4DEV_FS is not set
+# CONFIG_EXT4_FS is not set
 CONFIG_JBD=y
 # CONFIG_JBD_DEBUG is not set
 CONFIG_FS_MBCACHE=y
 # CONFIG_REISERFS_FS is not set
 # CONFIG_JFS_FS is not set
 # CONFIG_FS_POSIX_ACL is not set
+CONFIG_FILE_LOCKING=y
 # CONFIG_XFS_FS is not set
-# CONFIG_GFS2_FS is not set
 # CONFIG_OCFS2_FS is not set
-# CONFIG_MINIX_FS is not set
-# CONFIG_ROMFS_FS is not set
+# CONFIG_DNOTIFY is not set
 CONFIG_INOTIFY=y
 CONFIG_INOTIFY_USER=y
 # CONFIG_QUOTA is not set
-# CONFIG_DNOTIFY is not set
 # CONFIG_AUTOFS_FS is not set
 # CONFIG_AUTOFS4_FS is not set
 # CONFIG_FUSE_FS is not set
@@ -1414,11 +1477,11 @@ CONFIG_SYSFS=y
 # CONFIG_EFS_FS is not set
 CONFIG_YAFFS_FS=m
 CONFIG_YAFFS_YAFFS1=y
+# CONFIG_YAFFS_9BYTE_TAGS is not set
 # CONFIG_YAFFS_DOES_ECC is not set
 CONFIG_YAFFS_YAFFS2=y
 CONFIG_YAFFS_AUTO_YAFFS2=y
 # CONFIG_YAFFS_DISABLE_LAZY_LOAD is not set
-CONFIG_YAFFS_CHECKPOINT_RESERVED_BLOCKS=10
 # CONFIG_YAFFS_DISABLE_WIDE_TNODES is not set
 # CONFIG_YAFFS_ALWAYS_CHECK_CHUNK_ERASED is not set
 CONFIG_YAFFS_SHORT_NAMES_IN_RAM=y
@@ -1435,8 +1498,11 @@ CONFIG_JFFS2_RTIME=y
 # CONFIG_JFFS2_RUBIN is not set
 # CONFIG_CRAMFS is not set
 # CONFIG_VXFS_FS is not set
+# CONFIG_MINIX_FS is not set
+# CONFIG_OMFS_FS is not set
 # CONFIG_HPFS_FS is not set
 # CONFIG_QNX4FS_FS is not set
+# CONFIG_ROMFS_FS is not set
 # CONFIG_SYSV_FS is not set
 # CONFIG_UFS_FS is not set
 CONFIG_NETWORK_FILESYSTEMS=y
@@ -1444,18 +1510,16 @@ CONFIG_NFS_FS=m
 CONFIG_NFS_V3=y
 # CONFIG_NFS_V3_ACL is not set
 # CONFIG_NFS_V4 is not set
-# CONFIG_NFS_DIRECTIO is not set
 CONFIG_NFSD=m
 CONFIG_NFSD_V3=y
 # CONFIG_NFSD_V3_ACL is not set
 # CONFIG_NFSD_V4 is not set
-CONFIG_NFSD_TCP=y
 CONFIG_LOCKD=m
 CONFIG_LOCKD_V4=y
 CONFIG_EXPORTFS=m
 CONFIG_NFS_COMMON=y
 CONFIG_SUNRPC=m
-# CONFIG_SUNRPC_BIND34 is not set
+# CONFIG_SUNRPC_REGISTER_V4 is not set
 # CONFIG_RPCSEC_GSS_KRB5 is not set
 # CONFIG_RPCSEC_GSS_SPKM3 is not set
 CONFIG_SMB_FS=m
@@ -1533,9 +1597,6 @@ CONFIG_NLS_KOI8_R=m
 CONFIG_NLS_KOI8_U=m
 CONFIG_NLS_UTF8=m
 # CONFIG_DLM is not set
-CONFIG_INSTRUMENTATION=y
-# CONFIG_PROFILING is not set
-# CONFIG_MARKERS is not set
 
 #
 # Kernel hacking
@@ -1543,14 +1604,53 @@ CONFIG_INSTRUMENTATION=y
 # CONFIG_PRINTK_TIME is not set
 CONFIG_ENABLE_WARN_DEPRECATED=y
 CONFIG_ENABLE_MUST_CHECK=y
+CONFIG_FRAME_WARN=1024
 # CONFIG_MAGIC_SYSRQ is not set
 # CONFIG_UNUSED_SYMBOLS is not set
 CONFIG_DEBUG_FS=y
 # CONFIG_HEADERS_CHECK is not set
-# CONFIG_DEBUG_KERNEL is not set
+CONFIG_DEBUG_KERNEL=y
+# CONFIG_DEBUG_SHIRQ is not set
+CONFIG_DETECT_SOFTLOCKUP=y
+# CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC is not set
+CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC_VALUE=0
+CONFIG_SCHED_DEBUG=y
+# CONFIG_SCHEDSTATS is not set
+# CONFIG_TIMER_STATS is not set
+# CONFIG_DEBUG_OBJECTS is not set
+# CONFIG_DEBUG_SLAB is not set
+# CONFIG_DEBUG_RT_MUTEXES is not set
+# CONFIG_RT_MUTEX_TESTER is not set
+# CONFIG_DEBUG_SPINLOCK is not set
+# CONFIG_DEBUG_MUTEXES is not set
+# CONFIG_DEBUG_SPINLOCK_SLEEP is not set
+# CONFIG_DEBUG_LOCKING_API_SELFTESTS is not set
+# CONFIG_DEBUG_KOBJECT is not set
 # CONFIG_DEBUG_BUGVERBOSE is not set
+CONFIG_DEBUG_INFO=y
+# CONFIG_DEBUG_VM is not set
+# CONFIG_DEBUG_WRITECOUNT is not set
+# CONFIG_DEBUG_MEMORY_INIT is not set
+# CONFIG_DEBUG_LIST is not set
+# CONFIG_DEBUG_SG is not set
+# CONFIG_FRAME_POINTER is not set
+# CONFIG_BOOT_PRINTK_DELAY is not set
+# CONFIG_RCU_TORTURE_TEST is not set
+# CONFIG_RCU_CPU_STALL_DETECTOR is not set
+# CONFIG_BACKTRACE_SELF_TEST is not set
+# CONFIG_DEBUG_BLOCK_EXT_DEVT is not set
+# CONFIG_FAULT_INJECTION is not set
+# CONFIG_SYSCTL_SYSCALL_CHECK is not set
+# CONFIG_DYNAMIC_PRINTK_DEBUG is not set
 # CONFIG_SAMPLES is not set
+CONFIG_HAVE_ARCH_KGDB=y
+# CONFIG_KGDB is not set
+# CONFIG_DEBUG_STACKOVERFLOW is not set
+# CONFIG_DEBUG_STACK_USAGE is not set
+CONFIG_DEBUG_VERBOSE=y
 CONFIG_DEBUG_MMRS=y
+# CONFIG_DEBUG_HWERR is not set
+# CONFIG_DEBUG_DOUBLEFAULT is not set
 CONFIG_DEBUG_HUNT_FOR_ZERO=y
 CONFIG_DEBUG_BFIN_HWTRACE_ON=y
 CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION_OFF=y
@@ -1568,10 +1668,95 @@ CONFIG_ACCESS_CHECK=y
 #
 # CONFIG_KEYS is not set
 CONFIG_SECURITY=y
+# CONFIG_SECURITYFS is not set
 # CONFIG_SECURITY_NETWORK is not set
-# CONFIG_SECURITY_CAPABILITIES is not set
+# CONFIG_SECURITY_FILE_CAPABILITIES is not set
 # CONFIG_SECURITY_ROOTPLUG is not set
-# CONFIG_CRYPTO is not set
+CONFIG_SECURITY_DEFAULT_MMAP_MIN_ADDR=0
+CONFIG_CRYPTO=y
+
+#
+# Crypto core or helper
+#
+# CONFIG_CRYPTO_FIPS is not set
+# CONFIG_CRYPTO_MANAGER is not set
+# CONFIG_CRYPTO_GF128MUL is not set
+# CONFIG_CRYPTO_NULL is not set
+# CONFIG_CRYPTO_CRYPTD is not set
+# CONFIG_CRYPTO_AUTHENC is not set
+# CONFIG_CRYPTO_TEST is not set
+
+#
+# Authenticated Encryption with Associated Data
+#
+# CONFIG_CRYPTO_CCM is not set
+# CONFIG_CRYPTO_GCM is not set
+# CONFIG_CRYPTO_SEQIV is not set
+
+#
+# Block modes
+#
+# CONFIG_CRYPTO_CBC is not set
+# CONFIG_CRYPTO_CTR is not set
+# CONFIG_CRYPTO_CTS is not set
+# CONFIG_CRYPTO_ECB is not set
+# CONFIG_CRYPTO_LRW is not set
+# CONFIG_CRYPTO_PCBC is not set
+# CONFIG_CRYPTO_XTS is not set
+
+#
+# Hash modes
+#
+# CONFIG_CRYPTO_HMAC is not set
+# CONFIG_CRYPTO_XCBC is not set
+
+#
+# Digest
+#
+# CONFIG_CRYPTO_CRC32C is not set
+# CONFIG_CRYPTO_MD4 is not set
+# CONFIG_CRYPTO_MD5 is not set
+# CONFIG_CRYPTO_MICHAEL_MIC is not set
+# CONFIG_CRYPTO_RMD128 is not set
+# CONFIG_CRYPTO_RMD160 is not set
+# CONFIG_CRYPTO_RMD256 is not set
+# CONFIG_CRYPTO_RMD320 is not set
+# CONFIG_CRYPTO_SHA1 is not set
+# CONFIG_CRYPTO_SHA256 is not set
+# CONFIG_CRYPTO_SHA512 is not set
+# CONFIG_CRYPTO_TGR192 is not set
+# CONFIG_CRYPTO_WP512 is not set
+
+#
+# Ciphers
+#
+# CONFIG_CRYPTO_AES is not set
+# CONFIG_CRYPTO_ANUBIS is not set
+# CONFIG_CRYPTO_ARC4 is not set
+# CONFIG_CRYPTO_BLOWFISH is not set
+# CONFIG_CRYPTO_CAMELLIA is not set
+# CONFIG_CRYPTO_CAST5 is not set
+# CONFIG_CRYPTO_CAST6 is not set
+# CONFIG_CRYPTO_DES is not set
+# CONFIG_CRYPTO_FCRYPT is not set
+# CONFIG_CRYPTO_KHAZAD is not set
+# CONFIG_CRYPTO_SALSA20 is not set
+# CONFIG_CRYPTO_SEED is not set
+# CONFIG_CRYPTO_SERPENT is not set
+# CONFIG_CRYPTO_TEA is not set
+# CONFIG_CRYPTO_TWOFISH is not set
+
+#
+# Compression
+#
+# CONFIG_CRYPTO_DEFLATE is not set
+# CONFIG_CRYPTO_LZO is not set
+
+#
+# Random Number Generation
+#
+# CONFIG_CRYPTO_ANSI_CPRNG is not set
+CONFIG_CRYPTO_HW=y
 
 #
 # Library routines
@@ -1579,6 +1764,7 @@ CONFIG_SECURITY=y
 CONFIG_BITREVERSE=y
 CONFIG_CRC_CCITT=m
 # CONFIG_CRC16 is not set
+# CONFIG_CRC_T10DIF is not set
 # CONFIG_CRC_ITU_T is not set
 CONFIG_CRC32=y
 # CONFIG_CRC7 is not set
diff --git a/arch/blackfin/configs/BF561-EZKIT_defconfig b/arch/blackfin/configs/BF561-EZKIT_defconfig
index 3c70d6230a12..1ecb7a38c905 100644
--- a/arch/blackfin/configs/BF561-EZKIT_defconfig
+++ b/arch/blackfin/configs/BF561-EZKIT_defconfig
@@ -1,6 +1,6 @@
 #
 # Automatically generated make config: don't edit
-# Linux kernel version: 2.6.24.7
+# Linux kernel version: 2.6.28-rc2
 #
 # CONFIG_MMU is not set
 # CONFIG_FPU is not set
@@ -8,7 +8,6 @@ CONFIG_RWSEM_GENERIC_SPINLOCK=y
 # CONFIG_RWSEM_XCHGADD_ALGORITHM is not set
 CONFIG_BLACKFIN=y
 CONFIG_ZONE_DMA=y
-CONFIG_SEMAPHORE_SLEEPERS=y
 CONFIG_GENERIC_FIND_NEXT_BIT=y
 CONFIG_GENERIC_HWEIGHT=y
 CONFIG_GENERIC_HARDIRQS=y
@@ -31,18 +30,16 @@ CONFIG_SYSVIPC_SYSCTL=y
 # CONFIG_POSIX_MQUEUE is not set
 # CONFIG_BSD_PROCESS_ACCT is not set
 # CONFIG_TASKSTATS is not set
-# CONFIG_USER_NS is not set
-# CONFIG_PID_NS is not set
 # CONFIG_AUDIT is not set
 CONFIG_IKCONFIG=y
 CONFIG_IKCONFIG_PROC=y
 CONFIG_LOG_BUF_SHIFT=14
 # CONFIG_CGROUPS is not set
-CONFIG_FAIR_GROUP_SCHED=y
-CONFIG_FAIR_USER_SCHED=y
-# CONFIG_FAIR_CGROUP_SCHED is not set
-CONFIG_SYSFS_DEPRECATED=y
+# CONFIG_GROUP_SCHED is not set
+# CONFIG_SYSFS_DEPRECATED is not set
+# CONFIG_SYSFS_DEPRECATED_V2 is not set
 # CONFIG_RELAY is not set
+# CONFIG_NAMESPACES is not set
 CONFIG_BLK_DEV_INITRD=y
 CONFIG_INITRAMFS_SOURCE=""
 # CONFIG_CC_OPTIMIZE_FOR_SIZE is not set
@@ -51,26 +48,35 @@ CONFIG_EMBEDDED=y
 CONFIG_UID16=y
 CONFIG_SYSCTL_SYSCALL=y
 CONFIG_KALLSYMS=y
+# CONFIG_KALLSYMS_ALL is not set
 # CONFIG_KALLSYMS_EXTRA_PASS is not set
 CONFIG_HOTPLUG=y
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
+CONFIG_COMPAT_BRK=y
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
 CONFIG_EPOLL=y
 CONFIG_SIGNALFD=y
+CONFIG_TIMERFD=y
 CONFIG_EVENTFD=y
+CONFIG_AIO=y
 CONFIG_VM_EVENT_COUNTERS=y
 CONFIG_SLAB=y
 # CONFIG_SLUB is not set
 # CONFIG_SLOB is not set
+# CONFIG_PROFILING is not set
+# CONFIG_MARKERS is not set
+CONFIG_HAVE_OPROFILE=y
+# CONFIG_HAVE_GENERIC_DMA_COHERENT is not set
 CONFIG_SLABINFO=y
 CONFIG_RT_MUTEXES=y
 CONFIG_TINY_SHMEM=y
 CONFIG_BASE_SMALL=0
 CONFIG_MODULES=y
+# CONFIG_MODULE_FORCE_LOAD is not set
 CONFIG_MODULE_UNLOAD=y
 # CONFIG_MODULE_FORCE_UNLOAD is not set
 # CONFIG_MODVERSIONS is not set
@@ -81,6 +87,7 @@ CONFIG_BLOCK=y
 # CONFIG_BLK_DEV_IO_TRACE is not set
 # CONFIG_LSF is not set
 # CONFIG_BLK_DEV_BSG is not set
+# CONFIG_BLK_DEV_INTEGRITY is not set
 
 #
 # IO Schedulers
@@ -94,9 +101,11 @@ CONFIG_DEFAULT_AS=y
 # CONFIG_DEFAULT_CFQ is not set
 # CONFIG_DEFAULT_NOOP is not set
 CONFIG_DEFAULT_IOSCHED="anticipatory"
+CONFIG_CLASSIC_RCU=y
 # CONFIG_PREEMPT_NONE is not set
 CONFIG_PREEMPT_VOLUNTARY=y
 # CONFIG_PREEMPT is not set
+# CONFIG_FREEZER is not set
 
 #
 # Blackfin Processor Options
@@ -105,6 +114,10 @@ CONFIG_PREEMPT_VOLUNTARY=y
 #
 # Processor and Board Settings
 #
+# CONFIG_BF512 is not set
+# CONFIG_BF514 is not set
+# CONFIG_BF516 is not set
+# CONFIG_BF518 is not set
 # CONFIG_BF522 is not set
 # CONFIG_BF523 is not set
 # CONFIG_BF524 is not set
@@ -117,24 +130,38 @@ CONFIG_PREEMPT_VOLUNTARY=y
 # CONFIG_BF534 is not set
 # CONFIG_BF536 is not set
 # CONFIG_BF537 is not set
+# CONFIG_BF538 is not set
+# CONFIG_BF539 is not set
 # CONFIG_BF542 is not set
 # CONFIG_BF544 is not set
 # CONFIG_BF547 is not set
 # CONFIG_BF548 is not set
 # CONFIG_BF549 is not set
 CONFIG_BF561=y
+# CONFIG_SMP is not set
+CONFIG_BF_REV_MIN=3
+CONFIG_BF_REV_MAX=5
 # CONFIG_BF_REV_0_0 is not set
 # CONFIG_BF_REV_0_1 is not set
 # CONFIG_BF_REV_0_2 is not set
 CONFIG_BF_REV_0_3=y
 # CONFIG_BF_REV_0_4 is not set
 # CONFIG_BF_REV_0_5 is not set
+# CONFIG_BF_REV_0_6 is not set
 # CONFIG_BF_REV_ANY is not set
 # CONFIG_BF_REV_NONE is not set
 CONFIG_MEM_MT48LC16M16A2TG_75=y
 CONFIG_IRQ_PLL_WAKEUP=7
 CONFIG_IRQ_SPORT0_ERROR=7
 CONFIG_IRQ_SPORT1_ERROR=7
+CONFIG_IRQ_TIMER0=10
+CONFIG_IRQ_TIMER1=10
+CONFIG_IRQ_TIMER2=10
+CONFIG_IRQ_TIMER3=10
+CONFIG_IRQ_TIMER4=10
+CONFIG_IRQ_TIMER5=10
+CONFIG_IRQ_TIMER6=10
+CONFIG_IRQ_TIMER7=10
 CONFIG_IRQ_SPI_ERROR=7
 CONFIG_BFIN561_EZKIT=y
 # CONFIG_BFIN561_TEPLA is not set
@@ -148,10 +175,6 @@ CONFIG_BFIN561_EZKIT=y
 #
 # Core B Support
 #
-
-#
-# Core B Support
-#
 CONFIG_BF561_COREB=y
 CONFIG_BF561_COREB_RESET=y
 
@@ -193,14 +216,6 @@ CONFIG_IRQ_DMA2_8=9
 CONFIG_IRQ_DMA2_9=9
 CONFIG_IRQ_DMA2_10=9
 CONFIG_IRQ_DMA2_11=9
-CONFIG_IRQ_TIMER0=10
-CONFIG_IRQ_TIMER1=10
-CONFIG_IRQ_TIMER2=10
-CONFIG_IRQ_TIMER3=10
-CONFIG_IRQ_TIMER4=10
-CONFIG_IRQ_TIMER5=10
-CONFIG_IRQ_TIMER6=10
-CONFIG_IRQ_TIMER7=10
 CONFIG_IRQ_TIMER8=10
 CONFIG_IRQ_TIMER9=10
 CONFIG_IRQ_TIMER10=10
@@ -230,7 +245,6 @@ CONFIG_BOOT_LOAD=0x1000
 #
 CONFIG_CLKIN_HZ=30000000
 # CONFIG_BFIN_KERNEL_CLOCK is not set
-CONFIG_MAX_MEM_SIZE=512
 CONFIG_MAX_VCO_HZ=600000000
 CONFIG_MIN_VCO_HZ=50000000
 CONFIG_MAX_SCLK_HZ=133333333
@@ -244,6 +258,7 @@ CONFIG_HZ_250=y
 # CONFIG_HZ_300 is not set
 # CONFIG_HZ_1000 is not set
 CONFIG_HZ=250
+CONFIG_SCHED_HRTICK=y
 CONFIG_GENERIC_TIME=y
 CONFIG_GENERIC_CLOCKEVENTS=y
 # CONFIG_CYCLES_CLOCKSOURCE is not set
@@ -281,6 +296,12 @@ CONFIG_SYS_BFIN_SPINLOCK_L1=y
 CONFIG_CACHELINE_ALIGNED_L1=y
 # CONFIG_SYSCALL_TAB_L1 is not set
 # CONFIG_CPLB_SWITCH_TAB_L1 is not set
+CONFIG_APP_STACK_L1=y
+
+#
+# Speed Optimizations
+#
+CONFIG_BFIN_INS_LOWOVERHEAD=y
 CONFIG_RAMKERNEL=y
 # CONFIG_ROMKERNEL is not set
 CONFIG_SELECT_MEMORY_MODEL=y
@@ -289,14 +310,13 @@ CONFIG_FLATMEM_MANUAL=y
 # CONFIG_SPARSEMEM_MANUAL is not set
 CONFIG_FLATMEM=y
 CONFIG_FLAT_NODE_MEM_MAP=y
-# CONFIG_SPARSEMEM_STATIC is not set
-# CONFIG_SPARSEMEM_VMEMMAP_ENABLE is not set
+CONFIG_PAGEFLAGS_EXTENDED=y
 CONFIG_SPLIT_PTLOCK_CPUS=4
 # CONFIG_RESOURCES_64BIT is not set
+# CONFIG_PHYS_ADDR_T_64BIT is not set
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_VIRT_TO_BUS=y
 # CONFIG_BFIN_GPTIMERS is not set
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_4M is not set
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
@@ -311,7 +331,7 @@ CONFIG_BFIN_DCACHE=y
 # CONFIG_BFIN_ICACHE_LOCK is not set
 # CONFIG_BFIN_WB is not set
 CONFIG_BFIN_WT=y
-CONFIG_L1_MAX_PIECE=16
+# CONFIG_BFIN_L2_CACHEABLE is not set
 # CONFIG_MPU is not set
 
 #
@@ -344,7 +364,6 @@ CONFIG_BANK_3=0xAAC2
 #
 # Bus options (PCI, PCMCIA, EISA, MCA, ISA)
 #
-# CONFIG_PCI is not set
 # CONFIG_ARCH_SUPPORTS_MSI is not set
 # CONFIG_PCCARD is not set
 
@@ -355,23 +374,20 @@ CONFIG_BINFMT_ELF_FDPIC=y
 CONFIG_BINFMT_FLAT=y
 CONFIG_BINFMT_ZFLAT=y
 # CONFIG_BINFMT_SHARED_FLAT is not set
+# CONFIG_HAVE_AOUT is not set
 # CONFIG_BINFMT_MISC is not set
 
 #
 # Power management options
 #
 # CONFIG_PM is not set
-CONFIG_SUSPEND_UP_POSSIBLE=y
+CONFIG_ARCH_SUSPEND_POSSIBLE=y
 # CONFIG_PM_WAKEUP_BY_GPIO is not set
 
 #
 # CPU Frequency scaling
 #
 # CONFIG_CPU_FREQ is not set
-
-#
-# Networking
-#
 CONFIG_NET=y
 
 #
@@ -384,6 +400,7 @@ CONFIG_XFRM=y
 # CONFIG_XFRM_USER is not set
 # CONFIG_XFRM_SUB_POLICY is not set
 # CONFIG_XFRM_MIGRATE is not set
+# CONFIG_XFRM_STATISTICS is not set
 # CONFIG_NET_KEY is not set
 CONFIG_INET=y
 # CONFIG_IP_MULTICAST is not set
@@ -413,8 +430,6 @@ CONFIG_TCP_CONG_CUBIC=y
 CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_TCP_MD5SIG is not set
 # CONFIG_IPV6 is not set
-# CONFIG_INET6_XFRM_TUNNEL is not set
-# CONFIG_INET6_TUNNEL is not set
 # CONFIG_NETLABEL is not set
 # CONFIG_NETWORK_SECMARK is not set
 # CONFIG_NETFILTER is not set
@@ -423,6 +438,7 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_TIPC is not set
 # CONFIG_ATM is not set
 # CONFIG_BRIDGE is not set
+# CONFIG_NET_DSA is not set
 # CONFIG_VLAN_8021Q is not set
 # CONFIG_DECNET is not set
 # CONFIG_LLC2 is not set
@@ -439,6 +455,7 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 #
 # CONFIG_NET_PKTGEN is not set
 # CONFIG_HAMRADIO is not set
+# CONFIG_CAN is not set
 CONFIG_IRDA=m
 
 #
@@ -471,24 +488,14 @@ CONFIG_IRTTY_SIR=m
 # CONFIG_DONGLE is not set
 
 #
-# Old SIR device drivers
-#
-# CONFIG_IRPORT_SIR is not set
-
-#
-# Old Serial dongle support
-#
-
-#
 # FIR device drivers
 #
 # CONFIG_BT is not set
 # CONFIG_AF_RXRPC is not set
-
-#
-# Wireless
-#
+# CONFIG_PHONET is not set
+CONFIG_WIRELESS=y
 # CONFIG_CFG80211 is not set
+CONFIG_WIRELESS_OLD_REGULATORY=y
 # CONFIG_WIRELESS_EXT is not set
 # CONFIG_MAC80211 is not set
 # CONFIG_IEEE80211 is not set
@@ -506,6 +513,8 @@ CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
 CONFIG_STANDALONE=y
 CONFIG_PREVENT_FIRMWARE_BUILD=y
 # CONFIG_FW_LOADER is not set
+# CONFIG_DEBUG_DRIVER is not set
+# CONFIG_DEBUG_DEVRES is not set
 # CONFIG_SYS_HYPERVISOR is not set
 # CONFIG_CONNECTOR is not set
 CONFIG_MTD=y
@@ -514,6 +523,7 @@ CONFIG_MTD=y
 CONFIG_MTD_PARTITIONS=y
 # CONFIG_MTD_REDBOOT_PARTS is not set
 CONFIG_MTD_CMDLINE_PARTS=y
+# CONFIG_MTD_AR7_PARTS is not set
 
 #
 # User Modules And Translation Layers
@@ -595,11 +605,14 @@ CONFIG_BLK_DEV=y
 CONFIG_BLK_DEV_RAM=y
 CONFIG_BLK_DEV_RAM_COUNT=16
 CONFIG_BLK_DEV_RAM_SIZE=4096
-CONFIG_BLK_DEV_RAM_BLOCKSIZE=1024
+# CONFIG_BLK_DEV_XIP is not set
 # CONFIG_CDROM_PKTCDVD is not set
 # CONFIG_ATA_OVER_ETH is not set
+# CONFIG_BLK_DEV_HD is not set
 CONFIG_MISC_DEVICES=y
 # CONFIG_EEPROM_93CX6 is not set
+# CONFIG_ENCLOSURE_SERVICES is not set
+CONFIG_HAVE_IDE=y
 # CONFIG_IDE is not set
 
 #
@@ -612,7 +625,6 @@ CONFIG_MISC_DEVICES=y
 # CONFIG_ATA is not set
 # CONFIG_MD is not set
 CONFIG_NETDEVICES=y
-# CONFIG_NETDEVICES_MULTIQUEUE is not set
 # CONFIG_DUMMY is not set
 # CONFIG_BONDING is not set
 # CONFIG_MACVLAN is not set
@@ -625,11 +637,14 @@ CONFIG_MII=y
 CONFIG_SMC91X=y
 # CONFIG_SMSC911X is not set
 # CONFIG_DM9000 is not set
+# CONFIG_ENC28J60 is not set
 # CONFIG_IBM_NEW_EMAC_ZMII is not set
 # CONFIG_IBM_NEW_EMAC_RGMII is not set
 # CONFIG_IBM_NEW_EMAC_TAH is not set
 # CONFIG_IBM_NEW_EMAC_EMAC4 is not set
-# CONFIG_B44 is not set
+# CONFIG_IBM_NEW_EMAC_NO_FLOW_CTRL is not set
+# CONFIG_IBM_NEW_EMAC_MAL_CLR_ICINTSTAT is not set
+# CONFIG_IBM_NEW_EMAC_MAL_COMMON_ERR is not set
 CONFIG_NETDEV_1000=y
 # CONFIG_AX88180 is not set
 CONFIG_NETDEV_10000=y
@@ -639,10 +654,10 @@ CONFIG_NETDEV_10000=y
 #
 # CONFIG_WLAN_PRE80211 is not set
 # CONFIG_WLAN_80211 is not set
+# CONFIG_IWLWIFI_LEDS is not set
 # CONFIG_WAN is not set
 # CONFIG_PPP is not set
 # CONFIG_SLIP is not set
-# CONFIG_SHAPER is not set
 # CONFIG_NETCONSOLE is not set
 # CONFIG_NETPOLL is not set
 # CONFIG_NET_POLL_CONTROLLER is not set
@@ -690,8 +705,11 @@ CONFIG_INPUT_EVDEV=m
 # CONFIG_BF5xx_PPI is not set
 # CONFIG_BFIN_SPORT is not set
 # CONFIG_BFIN_TIMER_LATENCY is not set
+CONFIG_BFIN_DMA_INTERFACE=m
 CONFIG_SIMPLE_GPIO=m
 # CONFIG_VT is not set
+# CONFIG_DEVKMEM is not set
+# CONFIG_BFIN_JTAG_COMM is not set
 # CONFIG_SERIAL_NONSTANDARD is not set
 
 #
@@ -720,22 +738,19 @@ CONFIG_UNIX98_PTYS=y
 # CONFIG_CAN4LINUX is not set
 # CONFIG_IPMI_HANDLER is not set
 # CONFIG_HW_RANDOM is not set
-# CONFIG_GEN_RTC is not set
 # CONFIG_R3964 is not set
 # CONFIG_RAW_DRIVER is not set
 # CONFIG_TCG_TPM is not set
 # CONFIG_I2C is not set
-
-#
-# SPI support
-#
 CONFIG_SPI=y
+# CONFIG_SPI_DEBUG is not set
 CONFIG_SPI_MASTER=y
 
 #
 # SPI Master Controller Drivers
 #
 CONFIG_SPI_BFIN=y
+# CONFIG_SPI_BFIN_LOCK is not set
 # CONFIG_SPI_BITBANG is not set
 
 #
@@ -744,14 +759,18 @@ CONFIG_SPI_BFIN=y
 # CONFIG_SPI_AT25 is not set
 # CONFIG_SPI_SPIDEV is not set
 # CONFIG_SPI_TLE62X0 is not set
+CONFIG_ARCH_WANT_OPTIONAL_GPIOLIB=y
+# CONFIG_GPIOLIB is not set
 # CONFIG_W1 is not set
 # CONFIG_POWER_SUPPLY is not set
 CONFIG_HWMON=y
 # CONFIG_HWMON_VID is not set
+# CONFIG_SENSORS_ADCXX is not set
 # CONFIG_SENSORS_F71805F is not set
 # CONFIG_SENSORS_F71882FG is not set
 # CONFIG_SENSORS_IT87 is not set
 # CONFIG_SENSORS_LM70 is not set
+# CONFIG_SENSORS_MAX1111 is not set
 # CONFIG_SENSORS_PC87360 is not set
 # CONFIG_SENSORS_PC87427 is not set
 # CONFIG_SENSORS_SMSC47M1 is not set
@@ -760,6 +779,8 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_W83627HF is not set
 # CONFIG_SENSORS_W83627EHF is not set
 # CONFIG_HWMON_DEBUG_CHIP is not set
+# CONFIG_THERMAL is not set
+# CONFIG_THERMAL_HWMON is not set
 CONFIG_WATCHDOG=y
 # CONFIG_WATCHDOG_NOWAYOUT is not set
 
@@ -770,21 +791,28 @@ CONFIG_WATCHDOG=y
 CONFIG_BFIN_WDT=y
 
 #
-# Sonics Silicon Backplane
-#
-CONFIG_SSB_POSSIBLE=y
-# CONFIG_SSB is not set
-
-#
 # Multifunction device drivers
 #
+# CONFIG_MFD_CORE is not set
 # CONFIG_MFD_SM501 is not set
+# CONFIG_HTC_PASIC3 is not set
+# CONFIG_MFD_TMIO is not set
+# CONFIG_MFD_WM8400 is not set
 
 #
 # Multimedia devices
 #
+
+#
+# Multimedia core support
+#
 # CONFIG_VIDEO_DEV is not set
 # CONFIG_DVB_CORE is not set
+# CONFIG_VIDEO_MEDIA is not set
+
+#
+# Multimedia drivers
+#
 # CONFIG_DAB is not set
 
 #
@@ -799,43 +827,43 @@ CONFIG_SSB_POSSIBLE=y
 # Display device support
 #
 # CONFIG_DISPLAY_SUPPORT is not set
-
-#
-# Sound
-#
 # CONFIG_SOUND is not set
 CONFIG_HID_SUPPORT=y
 CONFIG_HID=m
 # CONFIG_HID_DEBUG is not set
 # CONFIG_HIDRAW is not set
+# CONFIG_HID_PID is not set
+
+#
+# Special HID drivers
+#
+CONFIG_HID_COMPAT=y
 # CONFIG_USB_SUPPORT is not set
 # CONFIG_MMC is not set
+# CONFIG_MEMSTICK is not set
 # CONFIG_NEW_LEDS is not set
+# CONFIG_ACCESSIBILITY is not set
 # CONFIG_RTC_CLASS is not set
-
-#
-# Userspace I/O
-#
+# CONFIG_DMADEVICES is not set
 # CONFIG_UIO is not set
+# CONFIG_STAGING is not set
 
 #
 # File systems
 #
 # CONFIG_EXT2_FS is not set
 # CONFIG_EXT3_FS is not set
-# CONFIG_EXT4DEV_FS is not set
+# CONFIG_EXT4_FS is not set
 # CONFIG_REISERFS_FS is not set
 # CONFIG_JFS_FS is not set
 # CONFIG_FS_POSIX_ACL is not set
+CONFIG_FILE_LOCKING=y
 # CONFIG_XFS_FS is not set
-# CONFIG_GFS2_FS is not set
 # CONFIG_OCFS2_FS is not set
-# CONFIG_MINIX_FS is not set
-# CONFIG_ROMFS_FS is not set
+# CONFIG_DNOTIFY is not set
 CONFIG_INOTIFY=y
 CONFIG_INOTIFY_USER=y
 # CONFIG_QUOTA is not set
-# CONFIG_DNOTIFY is not set
 # CONFIG_AUTOFS_FS is not set
 # CONFIG_AUTOFS4_FS is not set
 # CONFIG_FUSE_FS is not set
@@ -875,11 +903,11 @@ CONFIG_SYSFS=y
 # CONFIG_EFS_FS is not set
 CONFIG_YAFFS_FS=m
 CONFIG_YAFFS_YAFFS1=y
+# CONFIG_YAFFS_9BYTE_TAGS is not set
 # CONFIG_YAFFS_DOES_ECC is not set
 CONFIG_YAFFS_YAFFS2=y
 CONFIG_YAFFS_AUTO_YAFFS2=y
 # CONFIG_YAFFS_DISABLE_LAZY_LOAD is not set
-CONFIG_YAFFS_CHECKPOINT_RESERVED_BLOCKS=10
 # CONFIG_YAFFS_DISABLE_WIDE_TNODES is not set
 # CONFIG_YAFFS_ALWAYS_CHECK_CHUNK_ERASED is not set
 CONFIG_YAFFS_SHORT_NAMES_IN_RAM=y
@@ -896,8 +924,11 @@ CONFIG_JFFS2_RTIME=y
 # CONFIG_JFFS2_RUBIN is not set
 # CONFIG_CRAMFS is not set
 # CONFIG_VXFS_FS is not set
+# CONFIG_MINIX_FS is not set
+# CONFIG_OMFS_FS is not set
 # CONFIG_HPFS_FS is not set
 # CONFIG_QNX4FS_FS is not set
+# CONFIG_ROMFS_FS is not set
 # CONFIG_SYSV_FS is not set
 # CONFIG_UFS_FS is not set
 CONFIG_NETWORK_FILESYSTEMS=y
@@ -905,13 +936,12 @@ CONFIG_NFS_FS=m
 CONFIG_NFS_V3=y
 # CONFIG_NFS_V3_ACL is not set
 # CONFIG_NFS_V4 is not set
-# CONFIG_NFS_DIRECTIO is not set
 # CONFIG_NFSD is not set
 CONFIG_LOCKD=m
 CONFIG_LOCKD_V4=y
 CONFIG_NFS_COMMON=y
 CONFIG_SUNRPC=m
-# CONFIG_SUNRPC_BIND34 is not set
+# CONFIG_SUNRPC_REGISTER_V4 is not set
 # CONFIG_RPCSEC_GSS_KRB5 is not set
 # CONFIG_RPCSEC_GSS_SPKM3 is not set
 CONFIG_SMB_FS=m
@@ -967,9 +997,6 @@ CONFIG_NLS_DEFAULT="iso8859-1"
 # CONFIG_NLS_KOI8_U is not set
 # CONFIG_NLS_UTF8 is not set
 # CONFIG_DLM is not set
-CONFIG_INSTRUMENTATION=y
-# CONFIG_PROFILING is not set
-# CONFIG_MARKERS is not set
 
 #
 # Kernel hacking
@@ -977,14 +1004,53 @@ CONFIG_INSTRUMENTATION=y
 # CONFIG_PRINTK_TIME is not set
 CONFIG_ENABLE_WARN_DEPRECATED=y
 CONFIG_ENABLE_MUST_CHECK=y
+CONFIG_FRAME_WARN=1024
 # CONFIG_MAGIC_SYSRQ is not set
 # CONFIG_UNUSED_SYMBOLS is not set
 CONFIG_DEBUG_FS=y
 # CONFIG_HEADERS_CHECK is not set
-# CONFIG_DEBUG_KERNEL is not set
+CONFIG_DEBUG_KERNEL=y
+# CONFIG_DEBUG_SHIRQ is not set
+CONFIG_DETECT_SOFTLOCKUP=y
+# CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC is not set
+CONFIG_BOOTPARAM_SOFTLOCKUP_PANIC_VALUE=0
+CONFIG_SCHED_DEBUG=y
+# CONFIG_SCHEDSTATS is not set
+# CONFIG_TIMER_STATS is not set
+# CONFIG_DEBUG_OBJECTS is not set
+# CONFIG_DEBUG_SLAB is not set
+# CONFIG_DEBUG_RT_MUTEXES is not set
+# CONFIG_RT_MUTEX_TESTER is not set
+# CONFIG_DEBUG_SPINLOCK is not set
+# CONFIG_DEBUG_MUTEXES is not set
+# CONFIG_DEBUG_SPINLOCK_SLEEP is not set
+# CONFIG_DEBUG_LOCKING_API_SELFTESTS is not set
+# CONFIG_DEBUG_KOBJECT is not set
 # CONFIG_DEBUG_BUGVERBOSE is not set
+CONFIG_DEBUG_INFO=y
+# CONFIG_DEBUG_VM is not set
+# CONFIG_DEBUG_WRITECOUNT is not set
+# CONFIG_DEBUG_MEMORY_INIT is not set
+# CONFIG_DEBUG_LIST is not set
+# CONFIG_DEBUG_SG is not set
+# CONFIG_FRAME_POINTER is not set
+# CONFIG_BOOT_PRINTK_DELAY is not set
+# CONFIG_RCU_TORTURE_TEST is not set
+# CONFIG_RCU_CPU_STALL_DETECTOR is not set
+# CONFIG_BACKTRACE_SELF_TEST is not set
+# CONFIG_DEBUG_BLOCK_EXT_DEVT is not set
+# CONFIG_FAULT_INJECTION is not set
+# CONFIG_SYSCTL_SYSCALL_CHECK is not set
+# CONFIG_DYNAMIC_PRINTK_DEBUG is not set
 # CONFIG_SAMPLES is not set
+CONFIG_HAVE_ARCH_KGDB=y
+# CONFIG_KGDB is not set
+# CONFIG_DEBUG_STACKOVERFLOW is not set
+# CONFIG_DEBUG_STACK_USAGE is not set
+CONFIG_DEBUG_VERBOSE=y
 CONFIG_DEBUG_MMRS=y
+# CONFIG_DEBUG_HWERR is not set
+# CONFIG_DEBUG_DOUBLEFAULT is not set
 CONFIG_DEBUG_HUNT_FOR_ZERO=y
 CONFIG_DEBUG_BFIN_HWTRACE_ON=y
 CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION_OFF=y
@@ -994,7 +1060,6 @@ CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION=0
 # CONFIG_DEBUG_BFIN_HWTRACE_EXPAND is not set
 # CONFIG_DEBUG_BFIN_NO_KERN_HWTRACE is not set
 CONFIG_EARLY_PRINTK=y
-# CONFIG_DUAL_CORE_TEST_MODULE is not set
 CONFIG_CPLB_INFO=y
 CONFIG_ACCESS_CHECK=y
 
@@ -1003,9 +1068,94 @@ CONFIG_ACCESS_CHECK=y
 #
 # CONFIG_KEYS is not set
 CONFIG_SECURITY=y
+# CONFIG_SECURITYFS is not set
 # CONFIG_SECURITY_NETWORK is not set
-# CONFIG_SECURITY_CAPABILITIES is not set
-# CONFIG_CRYPTO is not set
+# CONFIG_SECURITY_FILE_CAPABILITIES is not set
+CONFIG_SECURITY_DEFAULT_MMAP_MIN_ADDR=0
+CONFIG_CRYPTO=y
+
+#
+# Crypto core or helper
+#
+# CONFIG_CRYPTO_FIPS is not set
+# CONFIG_CRYPTO_MANAGER is not set
+# CONFIG_CRYPTO_GF128MUL is not set
+# CONFIG_CRYPTO_NULL is not set
+# CONFIG_CRYPTO_CRYPTD is not set
+# CONFIG_CRYPTO_AUTHENC is not set
+# CONFIG_CRYPTO_TEST is not set
+
+#
+# Authenticated Encryption with Associated Data
+#
+# CONFIG_CRYPTO_CCM is not set
+# CONFIG_CRYPTO_GCM is not set
+# CONFIG_CRYPTO_SEQIV is not set
+
+#
+# Block modes
+#
+# CONFIG_CRYPTO_CBC is not set
+# CONFIG_CRYPTO_CTR is not set
+# CONFIG_CRYPTO_CTS is not set
+# CONFIG_CRYPTO_ECB is not set
+# CONFIG_CRYPTO_LRW is not set
+# CONFIG_CRYPTO_PCBC is not set
+# CONFIG_CRYPTO_XTS is not set
+
+#
+# Hash modes
+#
+# CONFIG_CRYPTO_HMAC is not set
+# CONFIG_CRYPTO_XCBC is not set
+
+#
+# Digest
+#
+# CONFIG_CRYPTO_CRC32C is not set
+# CONFIG_CRYPTO_MD4 is not set
+# CONFIG_CRYPTO_MD5 is not set
+# CONFIG_CRYPTO_MICHAEL_MIC is not set
+# CONFIG_CRYPTO_RMD128 is not set
+# CONFIG_CRYPTO_RMD160 is not set
+# CONFIG_CRYPTO_RMD256 is not set
+# CONFIG_CRYPTO_RMD320 is not set
+# CONFIG_CRYPTO_SHA1 is not set
+# CONFIG_CRYPTO_SHA256 is not set
+# CONFIG_CRYPTO_SHA512 is not set
+# CONFIG_CRYPTO_TGR192 is not set
+# CONFIG_CRYPTO_WP512 is not set
+
+#
+# Ciphers
+#
+# CONFIG_CRYPTO_AES is not set
+# CONFIG_CRYPTO_ANUBIS is not set
+# CONFIG_CRYPTO_ARC4 is not set
+# CONFIG_CRYPTO_BLOWFISH is not set
+# CONFIG_CRYPTO_CAMELLIA is not set
+# CONFIG_CRYPTO_CAST5 is not set
+# CONFIG_CRYPTO_CAST6 is not set
+# CONFIG_CRYPTO_DES is not set
+# CONFIG_CRYPTO_FCRYPT is not set
+# CONFIG_CRYPTO_KHAZAD is not set
+# CONFIG_CRYPTO_SALSA20 is not set
+# CONFIG_CRYPTO_SEED is not set
+# CONFIG_CRYPTO_SERPENT is not set
+# CONFIG_CRYPTO_TEA is not set
+# CONFIG_CRYPTO_TWOFISH is not set
+
+#
+# Compression
+#
+# CONFIG_CRYPTO_DEFLATE is not set
+# CONFIG_CRYPTO_LZO is not set
+
+#
+# Random Number Generation
+#
+# CONFIG_CRYPTO_ANSI_CPRNG is not set
+CONFIG_CRYPTO_HW=y
 
 #
 # Library routines
@@ -1013,6 +1163,7 @@ CONFIG_SECURITY=y
 CONFIG_BITREVERSE=y
 CONFIG_CRC_CCITT=m
 # CONFIG_CRC16 is not set
+# CONFIG_CRC_T10DIF is not set
 # CONFIG_CRC_ITU_T is not set
 CONFIG_CRC32=y
 # CONFIG_CRC7 is not set
diff --git a/arch/blackfin/configs/BlackStamp_defconfig b/arch/blackfin/configs/BlackStamp_defconfig
index 2921f9952d5f..9683b2e13097 100644
--- a/arch/blackfin/configs/BlackStamp_defconfig
+++ b/arch/blackfin/configs/BlackStamp_defconfig
@@ -53,7 +53,7 @@ CONFIG_KALLSYMS=y
 CONFIG_HOTPLUG=y
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
 CONFIG_COMPAT_BRK=y
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
@@ -276,7 +276,6 @@ CONFIG_SPLIT_PTLOCK_CPUS=4
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_VIRT_TO_BUS=y
 CONFIG_BFIN_GPTIMERS=y
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_4M is not set
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
diff --git a/arch/blackfin/configs/CM-BF527_defconfig b/arch/blackfin/configs/CM-BF527_defconfig
index b6a14635fb91..a041e7eba770 100644
--- a/arch/blackfin/configs/CM-BF527_defconfig
+++ b/arch/blackfin/configs/CM-BF527_defconfig
@@ -42,7 +42,7 @@ CONFIG_LOG_BUF_SHIFT=14
 CONFIG_FAIR_GROUP_SCHED=y
 CONFIG_FAIR_USER_SCHED=y
 # CONFIG_FAIR_CGROUP_SCHED is not set
-CONFIG_SYSFS_DEPRECATED=y
+# CONFIG_SYSFS_DEPRECATED is not set
 # CONFIG_RELAY is not set
 CONFIG_BLK_DEV_INITRD=y
 CONFIG_INITRAMFS_SOURCE=""
@@ -56,7 +56,7 @@ CONFIG_KALLSYMS=y
 CONFIG_HOTPLUG=y
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
@@ -190,14 +190,14 @@ CONFIG_IRQ_MAC_RX=11
 CONFIG_IRQ_PORTH_INTA=11
 CONFIG_IRQ_MAC_TX=11
 CONFIG_IRQ_PORTH_INTB=11
-CONFIG_IRQ_TMR0=12
-CONFIG_IRQ_TMR1=12
-CONFIG_IRQ_TMR2=12
-CONFIG_IRQ_TMR3=12
-CONFIG_IRQ_TMR4=12
-CONFIG_IRQ_TMR5=12
-CONFIG_IRQ_TMR6=12
-CONFIG_IRQ_TMR7=12
+CONFIG_IRQ_TIMER0=12
+CONFIG_IRQ_TIMER1=12
+CONFIG_IRQ_TIMER2=12
+CONFIG_IRQ_TIMER3=12
+CONFIG_IRQ_TIMER4=12
+CONFIG_IRQ_TIMER5=12
+CONFIG_IRQ_TIMER6=12
+CONFIG_IRQ_TIMER7=12
 CONFIG_IRQ_PORTG_INTA=12
 CONFIG_IRQ_PORTG_INTB=12
 CONFIG_IRQ_MEM_DMA0=13
@@ -292,7 +292,6 @@ CONFIG_SPLIT_PTLOCK_CPUS=4
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_VIRT_TO_BUS=y
 CONFIG_BFIN_GPTIMERS=y
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_4M is not set
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
@@ -650,6 +649,7 @@ CONFIG_BFIN_OTP=y
 # CONFIG_TWI_LCD is not set
 CONFIG_SIMPLE_GPIO=m
 # CONFIG_VT is not set
+# CONFIG_DEVKMEM is not set
 # CONFIG_SERIAL_NONSTANDARD is not set
 
 #
@@ -699,7 +699,7 @@ CONFIG_I2C_CHARDEV=m
 # I2C Hardware Bus support
 #
 CONFIG_I2C_BLACKFIN_TWI=m
-CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=50
+CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=100
 # CONFIG_I2C_GPIO is not set
 # CONFIG_I2C_OCORES is not set
 # CONFIG_I2C_PARPORT_LIGHT is not set
diff --git a/arch/blackfin/configs/CM-BF533_defconfig b/arch/blackfin/configs/CM-BF533_defconfig
index c3ba9066b935..085211b9e4e4 100644
--- a/arch/blackfin/configs/CM-BF533_defconfig
+++ b/arch/blackfin/configs/CM-BF533_defconfig
@@ -42,7 +42,7 @@ CONFIG_SYSVIPC_SYSCTL=y
 CONFIG_IKCONFIG=y
 CONFIG_IKCONFIG_PROC=y
 CONFIG_LOG_BUF_SHIFT=14
-CONFIG_SYSFS_DEPRECATED=y
+# CONFIG_SYSFS_DEPRECATED is not set
 # CONFIG_RELAY is not set
 # CONFIG_BLK_DEV_INITRD is not set
 # CONFIG_CC_OPTIMIZE_FOR_SIZE is not set
@@ -55,7 +55,7 @@ CONFIG_KALLSYMS=y
 # CONFIG_HOTPLUG is not set
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
@@ -254,7 +254,6 @@ CONFIG_SPLIT_PTLOCK_CPUS=4
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_LARGE_ALLOCS=y
 # CONFIG_BFIN_GPTIMERS is not set
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
 # CONFIG_DMA_UNCACHED_NONE is not set
@@ -598,6 +597,7 @@ CONFIG_NETDEV_10000=y
 CONFIG_BFIN_SPORT=y
 # CONFIG_BFIN_TIMER_LATENCY is not set
 # CONFIG_VT is not set
+# CONFIG_DEVKMEM is not set
 # CONFIG_SERIAL_NONSTANDARD is not set
 
 #
diff --git a/arch/blackfin/configs/CM-BF537E_defconfig b/arch/blackfin/configs/CM-BF537E_defconfig
index cdc6b7feb59e..750203e27a46 100644
--- a/arch/blackfin/configs/CM-BF537E_defconfig
+++ b/arch/blackfin/configs/CM-BF537E_defconfig
@@ -42,7 +42,7 @@ CONFIG_SYSVIPC_SYSCTL=y
 CONFIG_IKCONFIG=y
 CONFIG_IKCONFIG_PROC=y
 CONFIG_LOG_BUF_SHIFT=14
-CONFIG_SYSFS_DEPRECATED=y
+# CONFIG_SYSFS_DEPRECATED is not set
 # CONFIG_RELAY is not set
 # CONFIG_BLK_DEV_INITRD is not set
 # CONFIG_CC_OPTIMIZE_FOR_SIZE is not set
@@ -55,7 +55,7 @@ CONFIG_KALLSYMS=y
 # CONFIG_HOTPLUG is not set
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
@@ -157,14 +157,14 @@ CONFIG_IRQ_UART1_RX=10
 CONFIG_IRQ_UART1_TX=10
 CONFIG_IRQ_MAC_RX=11
 CONFIG_IRQ_MAC_TX=11
-CONFIG_IRQ_TMR0=12
-CONFIG_IRQ_TMR1=12
-CONFIG_IRQ_TMR2=12
-CONFIG_IRQ_TMR3=12
-CONFIG_IRQ_TMR4=12
-CONFIG_IRQ_TMR5=12
-CONFIG_IRQ_TMR6=12
-CONFIG_IRQ_TMR7=12
+CONFIG_IRQ_TIMER0=12
+CONFIG_IRQ_TIMER1=12
+CONFIG_IRQ_TIMER2=12
+CONFIG_IRQ_TIMER3=12
+CONFIG_IRQ_TIMER4=12
+CONFIG_IRQ_TIMER5=12
+CONFIG_IRQ_TIMER6=12
+CONFIG_IRQ_TIMER7=12
 CONFIG_IRQ_PORTG_INTB=12
 CONFIG_IRQ_MEM_DMA0=13
 CONFIG_IRQ_MEM_DMA1=13
@@ -262,7 +262,6 @@ CONFIG_SPLIT_PTLOCK_CPUS=4
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_LARGE_ALLOCS=y
 # CONFIG_BFIN_GPTIMERS is not set
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
 # CONFIG_DMA_UNCACHED_NONE is not set
@@ -627,6 +626,7 @@ CONFIG_NETDEV_10000=y
 CONFIG_BFIN_SPORT=y
 # CONFIG_BFIN_TIMER_LATENCY is not set
 # CONFIG_VT is not set
+# CONFIG_DEVKMEM is not set
 # CONFIG_SERIAL_NONSTANDARD is not set
 
 #
diff --git a/arch/blackfin/configs/CM-BF537U_defconfig b/arch/blackfin/configs/CM-BF537U_defconfig
index f074bdcd1ce5..dec8a7d5cc0e 100644
--- a/arch/blackfin/configs/CM-BF537U_defconfig
+++ b/arch/blackfin/configs/CM-BF537U_defconfig
@@ -42,7 +42,7 @@ CONFIG_SYSVIPC_SYSCTL=y
 CONFIG_IKCONFIG=y
 CONFIG_IKCONFIG_PROC=y
 CONFIG_LOG_BUF_SHIFT=14
-CONFIG_SYSFS_DEPRECATED=y
+# CONFIG_SYSFS_DEPRECATED is not set
 # CONFIG_RELAY is not set
 # CONFIG_BLK_DEV_INITRD is not set
 # CONFIG_CC_OPTIMIZE_FOR_SIZE is not set
@@ -55,7 +55,7 @@ CONFIG_KALLSYMS=y
 # CONFIG_HOTPLUG is not set
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
@@ -157,14 +157,14 @@ CONFIG_IRQ_UART1_RX=10
 CONFIG_IRQ_UART1_TX=10
 CONFIG_IRQ_MAC_RX=11
 CONFIG_IRQ_MAC_TX=11
-CONFIG_IRQ_TMR0=12
-CONFIG_IRQ_TMR1=12
-CONFIG_IRQ_TMR2=12
-CONFIG_IRQ_TMR3=12
-CONFIG_IRQ_TMR4=12
-CONFIG_IRQ_TMR5=12
-CONFIG_IRQ_TMR6=12
-CONFIG_IRQ_TMR7=12
+CONFIG_IRQ_TIMER0=12
+CONFIG_IRQ_TIMER1=12
+CONFIG_IRQ_TIMER2=12
+CONFIG_IRQ_TIMER3=12
+CONFIG_IRQ_TIMER4=12
+CONFIG_IRQ_TIMER5=12
+CONFIG_IRQ_TIMER6=12
+CONFIG_IRQ_TIMER7=12
 CONFIG_IRQ_PORTG_INTB=12
 CONFIG_IRQ_MEM_DMA0=13
 CONFIG_IRQ_MEM_DMA1=13
@@ -262,7 +262,6 @@ CONFIG_SPLIT_PTLOCK_CPUS=4
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_LARGE_ALLOCS=y
 # CONFIG_BFIN_GPTIMERS is not set
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
 # CONFIG_DMA_UNCACHED_NONE is not set
@@ -607,6 +606,7 @@ CONFIG_NETDEV_10000=y
 CONFIG_BFIN_SPORT=y
 # CONFIG_BFIN_TIMER_LATENCY is not set
 # CONFIG_VT is not set
+# CONFIG_DEVKMEM is not set
 # CONFIG_SERIAL_NONSTANDARD is not set
 
 #
diff --git a/arch/blackfin/configs/CM-BF548_defconfig b/arch/blackfin/configs/CM-BF548_defconfig
index 5c44fdb8e6e3..efd68bc78f35 100644
--- a/arch/blackfin/configs/CM-BF548_defconfig
+++ b/arch/blackfin/configs/CM-BF548_defconfig
@@ -41,7 +41,7 @@ CONFIG_LOG_BUF_SHIFT=14
 CONFIG_FAIR_GROUP_SCHED=y
 CONFIG_FAIR_USER_SCHED=y
 # CONFIG_FAIR_CGROUP_SCHED is not set
-CONFIG_SYSFS_DEPRECATED=y
+# CONFIG_SYSFS_DEPRECATED is not set
 # CONFIG_RELAY is not set
 CONFIG_BLK_DEV_INITRD=y
 CONFIG_INITRAMFS_SOURCE=""
@@ -55,7 +55,7 @@ CONFIG_KALLSYMS=y
 CONFIG_HOTPLUG=y
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
@@ -325,7 +325,6 @@ CONFIG_SPLIT_PTLOCK_CPUS=4
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_VIRT_TO_BUS=y
 # CONFIG_BFIN_GPTIMERS is not set
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
 # CONFIG_DMA_UNCACHED_NONE is not set
@@ -544,7 +543,7 @@ CONFIG_MTD_RAM=y
 CONFIG_MTD_COMPLEX_MAPPINGS=y
 CONFIG_MTD_PHYSMAP=y
 CONFIG_MTD_PHYSMAP_START=0x20000000
-CONFIG_MTD_PHYSMAP_LEN=0x800000
+CONFIG_MTD_PHYSMAP_LEN=0
 CONFIG_MTD_PHYSMAP_BANKWIDTH=2
 # CONFIG_MTD_UCLINUX is not set
 # CONFIG_MTD_PLATRAM is not set
@@ -732,6 +731,7 @@ CONFIG_BFIN_OTP=y
 # CONFIG_TWI_LCD is not set
 # CONFIG_SIMPLE_GPIO is not set
 # CONFIG_VT is not set
+# CONFIG_DEVKMEM is not set
 # CONFIG_SERIAL_NONSTANDARD is not set
 
 #
@@ -782,7 +782,7 @@ CONFIG_I2C_CHARDEV=y
 # I2C Hardware Bus support
 #
 CONFIG_I2C_BLACKFIN_TWI=y
-CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=50
+CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=100
 # CONFIG_I2C_GPIO is not set
 # CONFIG_I2C_OCORES is not set
 # CONFIG_I2C_PARPORT_LIGHT is not set
diff --git a/arch/blackfin/configs/CM-BF561_defconfig b/arch/blackfin/configs/CM-BF561_defconfig
index 086fe5dda495..346bc7af8f42 100644
--- a/arch/blackfin/configs/CM-BF561_defconfig
+++ b/arch/blackfin/configs/CM-BF561_defconfig
@@ -42,7 +42,7 @@ CONFIG_LOG_BUF_SHIFT=14
 CONFIG_FAIR_GROUP_SCHED=y
 CONFIG_FAIR_USER_SCHED=y
 # CONFIG_FAIR_CGROUP_SCHED is not set
-CONFIG_SYSFS_DEPRECATED=y
+# CONFIG_SYSFS_DEPRECATED is not set
 # CONFIG_RELAY is not set
 # CONFIG_BLK_DEV_INITRD is not set
 # CONFIG_CC_OPTIMIZE_FOR_SIZE is not set
@@ -55,7 +55,7 @@ CONFIG_KALLSYMS=y
 # CONFIG_HOTPLUG is not set
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
@@ -300,7 +300,6 @@ CONFIG_ZONE_DMA_FLAG=1
 CONFIG_VIRT_TO_BUS=y
 CONFIG_LARGE_ALLOCS=y
 # CONFIG_BFIN_GPTIMERS is not set
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
 # CONFIG_DMA_UNCACHED_NONE is not set
@@ -612,6 +611,7 @@ CONFIG_NETDEV_10000=y
 # CONFIG_BFIN_TIMER_LATENCY is not set
 # CONFIG_SIMPLE_GPIO is not set
 # CONFIG_VT is not set
+# CONFIG_DEVKMEM is not set
 # CONFIG_SERIAL_NONSTANDARD is not set
 
 #
diff --git a/arch/blackfin/configs/H8606_defconfig b/arch/blackfin/configs/H8606_defconfig
index 1fc31f1b762b..5d3901d23fd1 100644
--- a/arch/blackfin/configs/H8606_defconfig
+++ b/arch/blackfin/configs/H8606_defconfig
@@ -54,7 +54,7 @@ CONFIG_KALLSYMS=y
 CONFIG_HOTPLUG=y
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
@@ -250,7 +250,6 @@ CONFIG_SPLIT_PTLOCK_CPUS=4
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_LARGE_ALLOCS=y
 CONFIG_BFIN_GPTIMERS=y
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
 # CONFIG_DMA_UNCACHED_NONE is not set
diff --git a/arch/blackfin/configs/IP0X_defconfig b/arch/blackfin/configs/IP0X_defconfig
index 285d2241df26..e66f5daaa828 100644
--- a/arch/blackfin/configs/IP0X_defconfig
+++ b/arch/blackfin/configs/IP0X_defconfig
@@ -55,7 +55,7 @@ CONFIG_KALLSYMS=y
 # CONFIG_HOTPLUG is not set
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
@@ -262,7 +262,6 @@ CONFIG_SPLIT_PTLOCK_CPUS=4
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_LARGE_ALLOCS=y
 # CONFIG_BFIN_GPTIMERS is not set
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
 # CONFIG_DMA_UNCACHED_NONE is not set
diff --git a/arch/blackfin/configs/PNAV-10_defconfig b/arch/blackfin/configs/PNAV-10_defconfig
index bffca7de65d4..ce5dde9de9db 100644
--- a/arch/blackfin/configs/PNAV-10_defconfig
+++ b/arch/blackfin/configs/PNAV-10_defconfig
@@ -1,6 +1,6 @@
 #
 # Automatically generated make config: don't edit
-# Linux kernel version: 2.6.22.12
+# Linux kernel version: 2.6.28-rc2
 #
 # CONFIG_MMU is not set
 # CONFIG_FPU is not set
@@ -8,41 +8,37 @@ CONFIG_RWSEM_GENERIC_SPINLOCK=y
 # CONFIG_RWSEM_XCHGADD_ALGORITHM is not set
 CONFIG_BLACKFIN=y
 CONFIG_ZONE_DMA=y
-CONFIG_SEMAPHORE_SLEEPERS=y
 CONFIG_GENERIC_FIND_NEXT_BIT=y
 CONFIG_GENERIC_HWEIGHT=y
 CONFIG_GENERIC_HARDIRQS=y
 CONFIG_GENERIC_IRQ_PROBE=y
-CONFIG_GENERIC_TIME=y
 CONFIG_GENERIC_GPIO=y
 CONFIG_FORCE_MAX_ZONEORDER=14
 CONFIG_GENERIC_CALIBRATE_DELAY=y
 CONFIG_DEFCONFIG_LIST="/lib/modules/$UNAME_RELEASE/.config"
 
 #
-# Code maturity level options
+# General setup
 #
 CONFIG_EXPERIMENTAL=y
 CONFIG_BROKEN_ON_SMP=y
 CONFIG_INIT_ENV_ARG_LIMIT=32
-
-#
-# General setup
-#
 CONFIG_LOCALVERSION=""
 CONFIG_LOCALVERSION_AUTO=y
 CONFIG_SYSVIPC=y
-# CONFIG_IPC_NS is not set
 CONFIG_SYSVIPC_SYSCTL=y
 # CONFIG_POSIX_MQUEUE is not set
 # CONFIG_BSD_PROCESS_ACCT is not set
 # CONFIG_TASKSTATS is not set
-# CONFIG_UTS_NS is not set
 # CONFIG_AUDIT is not set
 # CONFIG_IKCONFIG is not set
 CONFIG_LOG_BUF_SHIFT=14
-CONFIG_SYSFS_DEPRECATED=y
+# CONFIG_CGROUPS is not set
+# CONFIG_GROUP_SCHED is not set
+# CONFIG_SYSFS_DEPRECATED is not set
+# CONFIG_SYSFS_DEPRECATED_V2 is not set
 # CONFIG_RELAY is not set
+# CONFIG_NAMESPACES is not set
 # CONFIG_BLK_DEV_INITRD is not set
 # CONFIG_CC_OPTIMIZE_FOR_SIZE is not set
 CONFIG_SYSCTL=y
@@ -54,40 +50,41 @@ CONFIG_KALLSYMS=y
 CONFIG_HOTPLUG=y
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
+CONFIG_COMPAT_BRK=y
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
 CONFIG_EPOLL=y
 CONFIG_SIGNALFD=y
+CONFIG_TIMERFD=y
 CONFIG_EVENTFD=y
+CONFIG_AIO=y
 CONFIG_VM_EVENT_COUNTERS=y
-CONFIG_BIG_ORDER_ALLOC_NOFAIL_MAGIC=9
-# CONFIG_NP2 is not set
 CONFIG_SLAB=y
 # CONFIG_SLUB is not set
 # CONFIG_SLOB is not set
+# CONFIG_PROFILING is not set
+# CONFIG_MARKERS is not set
+CONFIG_HAVE_OPROFILE=y
+# CONFIG_HAVE_GENERIC_DMA_COHERENT is not set
+CONFIG_SLABINFO=y
 CONFIG_RT_MUTEXES=y
 CONFIG_TINY_SHMEM=y
 CONFIG_BASE_SMALL=0
-
-#
-# Loadable module support
-#
 CONFIG_MODULES=y
+# CONFIG_MODULE_FORCE_LOAD is not set
 CONFIG_MODULE_UNLOAD=y
 # CONFIG_MODULE_FORCE_UNLOAD is not set
 # CONFIG_MODVERSIONS is not set
 # CONFIG_MODULE_SRCVERSION_ALL is not set
 CONFIG_KMOD=y
-
-#
-# Block layer
-#
 CONFIG_BLOCK=y
 # CONFIG_LBD is not set
 # CONFIG_BLK_DEV_IO_TRACE is not set
 # CONFIG_LSF is not set
+# CONFIG_BLK_DEV_BSG is not set
+# CONFIG_BLK_DEV_INTEGRITY is not set
 
 #
 # IO Schedulers
@@ -101,9 +98,11 @@ CONFIG_DEFAULT_AS=y
 # CONFIG_DEFAULT_CFQ is not set
 # CONFIG_DEFAULT_NOOP is not set
 CONFIG_DEFAULT_IOSCHED="anticipatory"
+CONFIG_CLASSIC_RCU=y
 # CONFIG_PREEMPT_NONE is not set
 CONFIG_PREEMPT_VOLUNTARY=y
 # CONFIG_PREEMPT is not set
+# CONFIG_FREEZER is not set
 
 #
 # Blackfin Processor Options
@@ -112,8 +111,15 @@ CONFIG_PREEMPT_VOLUNTARY=y
 #
 # Processor and Board Settings
 #
+# CONFIG_BF512 is not set
+# CONFIG_BF514 is not set
+# CONFIG_BF516 is not set
+# CONFIG_BF518 is not set
 # CONFIG_BF522 is not set
+# CONFIG_BF523 is not set
+# CONFIG_BF524 is not set
 # CONFIG_BF525 is not set
+# CONFIG_BF526 is not set
 # CONFIG_BF527 is not set
 # CONFIG_BF531 is not set
 # CONFIG_BF532 is not set
@@ -121,22 +127,26 @@ CONFIG_PREEMPT_VOLUNTARY=y
 # CONFIG_BF534 is not set
 # CONFIG_BF536 is not set
 CONFIG_BF537=y
+# CONFIG_BF538 is not set
+# CONFIG_BF539 is not set
 # CONFIG_BF542 is not set
 # CONFIG_BF544 is not set
 # CONFIG_BF547 is not set
 # CONFIG_BF548 is not set
 # CONFIG_BF549 is not set
 # CONFIG_BF561 is not set
+CONFIG_BF_REV_MIN=2
+CONFIG_BF_REV_MAX=3
 # CONFIG_BF_REV_0_0 is not set
 # CONFIG_BF_REV_0_1 is not set
 CONFIG_BF_REV_0_2=y
 # CONFIG_BF_REV_0_3 is not set
 # CONFIG_BF_REV_0_4 is not set
 # CONFIG_BF_REV_0_5 is not set
+# CONFIG_BF_REV_0_6 is not set
 # CONFIG_BF_REV_ANY is not set
 # CONFIG_BF_REV_NONE is not set
 CONFIG_BF53x=y
-CONFIG_BFIN_SINGLE_CORE=y
 CONFIG_MEM_MT48LC32M8A2_75=y
 CONFIG_IRQ_PLL_WAKEUP=7
 CONFIG_IRQ_RTC=8
@@ -146,28 +156,30 @@ CONFIG_IRQ_SPORT0_TX=9
 CONFIG_IRQ_SPORT1_RX=9
 CONFIG_IRQ_SPORT1_TX=9
 CONFIG_IRQ_TWI=10
-CONFIG_IRQ_SPI=10
 CONFIG_IRQ_UART0_RX=10
 CONFIG_IRQ_UART0_TX=10
 CONFIG_IRQ_UART1_RX=10
 CONFIG_IRQ_UART1_TX=10
 CONFIG_IRQ_MAC_RX=11
 CONFIG_IRQ_MAC_TX=11
-CONFIG_IRQ_TMR0=12
-CONFIG_IRQ_TMR1=12
-CONFIG_IRQ_TMR2=12
-CONFIG_IRQ_TMR3=12
-CONFIG_IRQ_TMR4=12
-CONFIG_IRQ_TMR5=12
-CONFIG_IRQ_TMR6=12
-CONFIG_IRQ_TMR7=12
+CONFIG_IRQ_TIMER0=12
+CONFIG_IRQ_TIMER1=12
+CONFIG_IRQ_TIMER2=12
+CONFIG_IRQ_TIMER3=12
+CONFIG_IRQ_TIMER4=12
+CONFIG_IRQ_TIMER5=12
+CONFIG_IRQ_TIMER6=12
+CONFIG_IRQ_TIMER7=12
 CONFIG_IRQ_PORTG_INTB=12
 CONFIG_IRQ_MEM_DMA0=13
 CONFIG_IRQ_MEM_DMA1=13
 CONFIG_IRQ_WATCH=13
+CONFIG_IRQ_SPI=10
 # CONFIG_BFIN537_STAMP is not set
 # CONFIG_BFIN537_BLUETECHNIX_CM is not set
+# CONFIG_BFIN537_BLUETECHNIX_TCM is not set
 CONFIG_PNAV10=y
+# CONFIG_CAMSIG_MINOTAUR is not set
 # CONFIG_GENERIC_BF537_BOARD is not set
 
 #
@@ -191,6 +203,7 @@ CONFIG_IRQ_PROG_INTA=12
 # Board customizations
 #
 # CONFIG_CMDLINE_BOOL is not set
+CONFIG_BOOT_LOAD=0x1000
 
 #
 # Clock/PLL Setup
@@ -199,7 +212,7 @@ CONFIG_CLKIN_HZ=24576000
 # CONFIG_BFIN_KERNEL_CLOCK is not set
 CONFIG_MAX_VCO_HZ=600000000
 CONFIG_MIN_VCO_HZ=50000000
-CONFIG_MAX_SCLK_HZ=133000000
+CONFIG_MAX_SCLK_HZ=133333333
 CONFIG_MIN_SCLK_HZ=27000000
 
 #
@@ -210,13 +223,17 @@ CONFIG_HZ_250=y
 # CONFIG_HZ_300 is not set
 # CONFIG_HZ_1000 is not set
 CONFIG_HZ=250
+# CONFIG_SCHED_HRTICK is not set
+CONFIG_GENERIC_TIME=y
+CONFIG_GENERIC_CLOCKEVENTS=y
+# CONFIG_CYCLES_CLOCKSOURCE is not set
+# CONFIG_NO_HZ is not set
+# CONFIG_HIGH_RES_TIMERS is not set
+CONFIG_GENERIC_CLOCKEVENTS_BUILD=y
 
 #
-# Memory Setup
+# Misc
 #
-CONFIG_MAX_MEM_SIZE=64
-CONFIG_MEM_ADD_WIDTH=10
-CONFIG_BOOT_LOAD=0x1000
 CONFIG_BFIN_SCRATCH_REG_RETN=y
 # CONFIG_BFIN_SCRATCH_REG_RETE is not set
 # CONFIG_BFIN_SCRATCH_REG_CYCLES is not set
@@ -243,6 +260,12 @@ CONFIG_IP_CHECKSUM_L1=y
 CONFIG_CACHELINE_ALIGNED_L1=y
 CONFIG_SYSCALL_TAB_L1=y
 CONFIG_CPLB_SWITCH_TAB_L1=y
+CONFIG_APP_STACK_L1=y
+
+#
+# Speed Optimizations
+#
+CONFIG_BFIN_INS_LOWOVERHEAD=y
 CONFIG_RAMKERNEL=y
 # CONFIG_ROMKERNEL is not set
 CONFIG_SELECT_MEMORY_MODEL=y
@@ -251,13 +274,14 @@ CONFIG_FLATMEM_MANUAL=y
 # CONFIG_SPARSEMEM_MANUAL is not set
 CONFIG_FLATMEM=y
 CONFIG_FLAT_NODE_MEM_MAP=y
-# CONFIG_SPARSEMEM_STATIC is not set
+CONFIG_PAGEFLAGS_EXTENDED=y
 CONFIG_SPLIT_PTLOCK_CPUS=4
 # CONFIG_RESOURCES_64BIT is not set
+# CONFIG_PHYS_ADDR_T_64BIT is not set
 CONFIG_ZONE_DMA_FLAG=1
-CONFIG_LARGE_ALLOCS=y
-# CONFIG_BFIN_GPTIMERS is not set
-CONFIG_BFIN_DMA_5XX=y
+CONFIG_VIRT_TO_BUS=y
+CONFIG_BFIN_GPTIMERS=y
+# CONFIG_DMA_UNCACHED_4M is not set
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
 # CONFIG_DMA_UNCACHED_NONE is not set
@@ -271,7 +295,7 @@ CONFIG_BFIN_DCACHE=y
 # CONFIG_BFIN_ICACHE_LOCK is not set
 CONFIG_BFIN_WB=y
 # CONFIG_BFIN_WT is not set
-CONFIG_L1_MAX_PIECE=16
+# CONFIG_MPU is not set
 
 #
 # Asynchonous Memory Configuration
@@ -299,12 +323,7 @@ CONFIG_BANK_3=0x99B2
 #
 # Bus options (PCI, PCMCIA, EISA, MCA, ISA)
 #
-# CONFIG_PCI is not set
 # CONFIG_ARCH_SUPPORTS_MSI is not set
-
-#
-# PCCARD (PCMCIA/CardBus) support
-#
 # CONFIG_PCCARD is not set
 
 #
@@ -314,21 +333,20 @@ CONFIG_BINFMT_ELF_FDPIC=y
 CONFIG_BINFMT_FLAT=y
 CONFIG_BINFMT_ZFLAT=y
 # CONFIG_BINFMT_SHARED_FLAT is not set
+# CONFIG_HAVE_AOUT is not set
 # CONFIG_BINFMT_MISC is not set
 
 #
 # Power management options
 #
 # CONFIG_PM is not set
+CONFIG_ARCH_SUSPEND_POSSIBLE=y
+# CONFIG_PM_WAKEUP_BY_GPIO is not set
 
 #
 # CPU Frequency scaling
 #
 # CONFIG_CPU_FREQ is not set
-
-#
-# Networking
-#
 CONFIG_NET=y
 
 #
@@ -341,6 +359,7 @@ CONFIG_XFRM=y
 # CONFIG_XFRM_USER is not set
 # CONFIG_XFRM_SUB_POLICY is not set
 # CONFIG_XFRM_MIGRATE is not set
+# CONFIG_XFRM_STATISTICS is not set
 # CONFIG_NET_KEY is not set
 CONFIG_INET=y
 # CONFIG_IP_MULTICAST is not set
@@ -362,6 +381,7 @@ CONFIG_SYN_COOKIES=y
 CONFIG_INET_XFRM_MODE_TRANSPORT=y
 CONFIG_INET_XFRM_MODE_TUNNEL=y
 CONFIG_INET_XFRM_MODE_BEET=y
+# CONFIG_INET_LRO is not set
 CONFIG_INET_DIAG=y
 CONFIG_INET_TCP_DIAG=y
 # CONFIG_TCP_CONG_ADVANCED is not set
@@ -369,8 +389,6 @@ CONFIG_TCP_CONG_CUBIC=y
 CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_TCP_MD5SIG is not set
 # CONFIG_IPV6 is not set
-# CONFIG_INET6_XFRM_TUNNEL is not set
-# CONFIG_INET6_TUNNEL is not set
 # CONFIG_NETLABEL is not set
 # CONFIG_NETWORK_SECMARK is not set
 # CONFIG_NETFILTER is not set
@@ -379,6 +397,7 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_TIPC is not set
 # CONFIG_ATM is not set
 # CONFIG_BRIDGE is not set
+# CONFIG_NET_DSA is not set
 # CONFIG_VLAN_8021Q is not set
 # CONFIG_DECNET is not set
 # CONFIG_LLC2 is not set
@@ -388,10 +407,6 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 # CONFIG_LAPB is not set
 # CONFIG_ECONET is not set
 # CONFIG_WAN_ROUTER is not set
-
-#
-# QoS and/or fair queueing
-#
 # CONFIG_NET_SCHED is not set
 
 #
@@ -399,18 +414,19 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 #
 # CONFIG_NET_PKTGEN is not set
 # CONFIG_HAMRADIO is not set
+# CONFIG_CAN is not set
 # CONFIG_IRDA is not set
 # CONFIG_BT is not set
 # CONFIG_AF_RXRPC is not set
-
-#
-# Wireless
-#
+# CONFIG_PHONET is not set
+CONFIG_WIRELESS=y
 # CONFIG_CFG80211 is not set
+CONFIG_WIRELESS_OLD_REGULATORY=y
 # CONFIG_WIRELESS_EXT is not set
 # CONFIG_MAC80211 is not set
 # CONFIG_IEEE80211 is not set
 # CONFIG_RFKILL is not set
+# CONFIG_NET_9P is not set
 
 #
 # Device Drivers
@@ -419,14 +435,11 @@ CONFIG_DEFAULT_TCP_CONG="cubic"
 #
 # Generic Driver Options
 #
+CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
 CONFIG_STANDALONE=y
 CONFIG_PREVENT_FIRMWARE_BUILD=y
 # CONFIG_FW_LOADER is not set
 # CONFIG_SYS_HYPERVISOR is not set
-
-#
-# Connector - unified userspace <-> kernelspace linker
-#
 # CONFIG_CONNECTOR is not set
 CONFIG_MTD=y
 # CONFIG_MTD_DEBUG is not set
@@ -434,6 +447,7 @@ CONFIG_MTD=y
 CONFIG_MTD_PARTITIONS=y
 # CONFIG_MTD_REDBOOT_PARTS is not set
 # CONFIG_MTD_CMDLINE_PARTS is not set
+# CONFIG_MTD_AR7_PARTS is not set
 
 #
 # User Modules And Translation Layers
@@ -446,6 +460,7 @@ CONFIG_MTD_BLOCK=y
 # CONFIG_INFTL is not set
 # CONFIG_RFD_FTL is not set
 # CONFIG_SSFDC is not set
+# CONFIG_MTD_OOPS is not set
 
 #
 # RAM/ROM/Flash chip drivers
@@ -470,7 +485,7 @@ CONFIG_MTD_RAM=y
 # Mapping drivers for chip access
 #
 CONFIG_MTD_COMPLEX_MAPPINGS=y
-# CONFIG_MTD_BF5xx is not set
+# CONFIG_MTD_GPIO_ADDR is not set
 CONFIG_MTD_UCLINUX=y
 # CONFIG_MTD_PLATRAM is not set
 
@@ -509,33 +524,22 @@ CONFIG_MTD_NAND_IDS=y
 # UBI - Unsorted block images
 #
 # CONFIG_MTD_UBI is not set
-
-#
-# Parallel port support
-#
 # CONFIG_PARPORT is not set
-
-#
-# Plug and Play support
-#
-# CONFIG_PNPACPI is not set
-
-#
-# Block devices
-#
+CONFIG_BLK_DEV=y
 # CONFIG_BLK_DEV_COW_COMMON is not set
 # CONFIG_BLK_DEV_LOOP is not set
 # CONFIG_BLK_DEV_NBD is not set
 CONFIG_BLK_DEV_RAM=y
 CONFIG_BLK_DEV_RAM_COUNT=16
 CONFIG_BLK_DEV_RAM_SIZE=4096
-CONFIG_BLK_DEV_RAM_BLOCKSIZE=1024
+# CONFIG_BLK_DEV_XIP is not set
 # CONFIG_CDROM_PKTCDVD is not set
 # CONFIG_ATA_OVER_ETH is not set
-
-#
-# Misc devices
-#
+# CONFIG_BLK_DEV_HD is not set
+CONFIG_MISC_DEVICES=y
+# CONFIG_EEPROM_93CX6 is not set
+# CONFIG_ENCLOSURE_SERVICES is not set
+CONFIG_HAVE_IDE=y
 # CONFIG_IDE is not set
 
 #
@@ -543,22 +547,17 @@ CONFIG_BLK_DEV_RAM_BLOCKSIZE=1024
 #
 # CONFIG_RAID_ATTRS is not set
 # CONFIG_SCSI is not set
+# CONFIG_SCSI_DMA is not set
 # CONFIG_SCSI_NETLINK is not set
 # CONFIG_ATA is not set
-
-#
-# Multi-device support (RAID and LVM)
-#
 # CONFIG_MD is not set
-
-#
-# Network device support
-#
 CONFIG_NETDEVICES=y
 # CONFIG_DUMMY is not set
 # CONFIG_BONDING is not set
+# CONFIG_MACVLAN is not set
 # CONFIG_EQUALIZER is not set
 # CONFIG_TUN is not set
+# CONFIG_VETH is not set
 CONFIG_PHYLIB=y
 
 #
@@ -572,46 +571,45 @@ CONFIG_PHYLIB=y
 # CONFIG_VITESSE_PHY is not set
 # CONFIG_SMSC_PHY is not set
 # CONFIG_BROADCOM_PHY is not set
+# CONFIG_ICPLUS_PHY is not set
+# CONFIG_REALTEK_PHY is not set
 # CONFIG_FIXED_PHY is not set
-
-#
-# Ethernet (10 or 100Mbit)
-#
+# CONFIG_MDIO_BITBANG is not set
 CONFIG_NET_ETHERNET=y
 CONFIG_MII=y
-# CONFIG_SMC91X is not set
 CONFIG_BFIN_MAC=y
 # CONFIG_BFIN_MAC_USE_L1 is not set
 CONFIG_BFIN_TX_DESC_NUM=100
 CONFIG_BFIN_RX_DESC_NUM=100
 CONFIG_BFIN_MAC_RMII=y
+# CONFIG_SMC91X is not set
 # CONFIG_SMSC911X is not set
 # CONFIG_DM9000 is not set
+# CONFIG_ENC28J60 is not set
+# CONFIG_IBM_NEW_EMAC_ZMII is not set
+# CONFIG_IBM_NEW_EMAC_RGMII is not set
+# CONFIG_IBM_NEW_EMAC_TAH is not set
+# CONFIG_IBM_NEW_EMAC_EMAC4 is not set
+# CONFIG_IBM_NEW_EMAC_NO_FLOW_CTRL is not set
+# CONFIG_IBM_NEW_EMAC_MAL_CLR_ICINTSTAT is not set
+# CONFIG_IBM_NEW_EMAC_MAL_COMMON_ERR is not set
 CONFIG_NETDEV_1000=y
-CONFIG_NETDEV_10000=y
 # CONFIG_AX88180 is not set
+CONFIG_NETDEV_10000=y
 
 #
 # Wireless LAN
 #
 # CONFIG_WLAN_PRE80211 is not set
 # CONFIG_WLAN_80211 is not set
+# CONFIG_IWLWIFI_LEDS is not set
 # CONFIG_WAN is not set
 # CONFIG_PPP is not set
 # CONFIG_SLIP is not set
-# CONFIG_SHAPER is not set
 # CONFIG_NETCONSOLE is not set
 # CONFIG_NETPOLL is not set
 # CONFIG_NET_POLL_CONTROLLER is not set
-
-#
-# ISDN subsystem
-#
 # CONFIG_ISDN is not set
-
-#
-# Telephony Support
-#
 # CONFIG_PHONE is not set
 
 #
@@ -626,9 +624,6 @@ CONFIG_INPUT=y
 #
 # CONFIG_INPUT_MOUSEDEV is not set
 # CONFIG_INPUT_JOYDEV is not set
-CONFIG_INPUT_TSDEV=y
-CONFIG_INPUT_TSDEV_SCREEN_X=240
-CONFIG_INPUT_TSDEV_SCREEN_Y=320
 CONFIG_INPUT_EVDEV=y
 # CONFIG_INPUT_EVBUG is not set
 
@@ -642,24 +637,29 @@ CONFIG_INPUT_EVDEV=y
 CONFIG_INPUT_TOUCHSCREEN=y
 # CONFIG_TOUCHSCREEN_ADS7846 is not set
 CONFIG_TOUCHSCREEN_AD7877=y
+# CONFIG_TOUCHSCREEN_AD7879_I2C is not set
+# CONFIG_TOUCHSCREEN_AD7879_SPI is not set
+# CONFIG_TOUCHSCREEN_AD7879 is not set
+# CONFIG_TOUCHSCREEN_FUJITSU is not set
 # CONFIG_TOUCHSCREEN_GUNZE is not set
 # CONFIG_TOUCHSCREEN_ELO is not set
 # CONFIG_TOUCHSCREEN_MTOUCH is not set
+# CONFIG_TOUCHSCREEN_INEXIO is not set
 # CONFIG_TOUCHSCREEN_MK712 is not set
 # CONFIG_TOUCHSCREEN_PENMOUNT is not set
 # CONFIG_TOUCHSCREEN_TOUCHRIGHT is not set
 # CONFIG_TOUCHSCREEN_TOUCHWIN is not set
-# CONFIG_TOUCHSCREEN_UCB1400 is not set
 # CONFIG_TOUCHSCREEN_USB_COMPOSITE is not set
+# CONFIG_TOUCHSCREEN_TOUCHIT213 is not set
 CONFIG_INPUT_MISC=y
 # CONFIG_INPUT_ATI_REMOTE is not set
 # CONFIG_INPUT_ATI_REMOTE2 is not set
 # CONFIG_INPUT_KEYSPAN_REMOTE is not set
 # CONFIG_INPUT_POWERMATE is not set
 # CONFIG_INPUT_YEALINK is not set
+# CONFIG_INPUT_CM109 is not set
 CONFIG_INPUT_UINPUT=y
-# CONFIG_BF53X_PFBUTTONS is not set
-# CONFIG_TWI_KEYPAD is not set
+# CONFIG_CONFIG_INPUT_PCF8574 is not set
 
 #
 # Hardware I/O ports
@@ -672,18 +672,17 @@ CONFIG_INPUT_UINPUT=y
 #
 # CONFIG_AD9960 is not set
 # CONFIG_SPI_ADC_BF533 is not set
-# CONFIG_BF5xx_PFLAGS is not set
 # CONFIG_BF5xx_PPIFCD is not set
 # CONFIG_BFIN_SIMPLE_TIMER is not set
 # CONFIG_BF5xx_PPI is not set
 CONFIG_BFIN_SPORT=y
 # CONFIG_BFIN_TIMER_LATENCY is not set
 CONFIG_TWI_LCD=m
-CONFIG_TWI_LCD_SLAVE_ADDR=34
-# CONFIG_AD5304 is not set
-# CONFIG_BF5xx_TEA5764 is not set
-# CONFIG_BF5xx_FBDMA is not set
+CONFIG_BFIN_DMA_INTERFACE=m
+# CONFIG_SIMPLE_GPIO is not set
 # CONFIG_VT is not set
+CONFIG_DEVKMEM=y
+# CONFIG_BFIN_JTAG_COMM is not set
 # CONFIG_SERIAL_NONSTANDARD is not set
 
 #
@@ -716,68 +715,59 @@ CONFIG_CAN4LINUX=y
 #
 # linux embedded drivers
 #
-# CONFIG_CAN_MCF5282 is not set
-# CONFIG_CAN_UNCTWINCAN is not set
 CONFIG_CAN_BLACKFIN=m
-
-#
-# IPMI
-#
 # CONFIG_IPMI_HANDLER is not set
-# CONFIG_WATCHDOG is not set
 CONFIG_HW_RANDOM=y
-# CONFIG_GEN_RTC is not set
 # CONFIG_R3964 is not set
 # CONFIG_RAW_DRIVER is not set
-
-#
-# TPM devices
-#
 # CONFIG_TCG_TPM is not set
 CONFIG_I2C=y
 CONFIG_I2C_BOARDINFO=y
 CONFIG_I2C_CHARDEV=y
+CONFIG_I2C_HELPER_AUTO=y
 
 #
-# I2C Algorithms
+# I2C Hardware Bus support
 #
-# CONFIG_I2C_ALGOBIT is not set
-# CONFIG_I2C_ALGOPCF is not set
-# CONFIG_I2C_ALGOPCA is not set
 
 #
-# I2C Hardware Bus support
+# I2C system bus drivers (mostly embedded / system-on-chip)
 #
-# CONFIG_I2C_BLACKFIN_GPIO is not set
 CONFIG_I2C_BLACKFIN_TWI=y
-CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=50
+CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=100
 # CONFIG_I2C_GPIO is not set
 # CONFIG_I2C_OCORES is not set
-# CONFIG_I2C_PARPORT_LIGHT is not set
 # CONFIG_I2C_SIMTEC is not set
+
+#
+# External I2C/SMBus adapter drivers
+#
+# CONFIG_I2C_PARPORT_LIGHT is not set
+# CONFIG_I2C_TAOS_EVM is not set
+
+#
+# Other I2C/SMBus bus drivers
+#
+# CONFIG_I2C_PCA_PLATFORM is not set
 # CONFIG_I2C_STUB is not set
 
 #
 # Miscellaneous I2C Chip support
 #
-# CONFIG_SENSORS_DS1337 is not set
-# CONFIG_SENSORS_DS1374 is not set
+# CONFIG_DS1682 is not set
+# CONFIG_AT24 is not set
 # CONFIG_SENSORS_AD5252 is not set
 # CONFIG_SENSORS_EEPROM is not set
 CONFIG_SENSORS_PCF8574=m
-CONFIG_SENSORS_PCF8575=y
-# CONFIG_SENSORS_PCA9543 is not set
+# CONFIG_PCF8575 is not set
 # CONFIG_SENSORS_PCA9539 is not set
 # CONFIG_SENSORS_PCF8591 is not set
 # CONFIG_SENSORS_MAX6875 is not set
+# CONFIG_SENSORS_TSL2550 is not set
 # CONFIG_I2C_DEBUG_CORE is not set
 # CONFIG_I2C_DEBUG_ALGO is not set
 # CONFIG_I2C_DEBUG_BUS is not set
 # CONFIG_I2C_DEBUG_CHIP is not set
-
-#
-# SPI support
-#
 CONFIG_SPI=y
 CONFIG_SPI_MASTER=y
 
@@ -785,6 +775,7 @@ CONFIG_SPI_MASTER=y
 # SPI Master Controller Drivers
 #
 CONFIG_SPI_BFIN=y
+# CONFIG_SPI_BFIN_LOCK is not set
 # CONFIG_SPI_BITBANG is not set
 
 #
@@ -792,27 +783,29 @@ CONFIG_SPI_BFIN=y
 #
 # CONFIG_SPI_AT25 is not set
 # CONFIG_SPI_SPIDEV is not set
-
-#
-# Dallas's 1-wire bus
-#
+# CONFIG_SPI_TLE62X0 is not set
+CONFIG_ARCH_WANT_OPTIONAL_GPIOLIB=y
+# CONFIG_GPIOLIB is not set
 # CONFIG_W1 is not set
+# CONFIG_POWER_SUPPLY is not set
 CONFIG_HWMON=y
 # CONFIG_HWMON_VID is not set
-# CONFIG_SENSORS_ABITUGURU is not set
+# CONFIG_SENSORS_AD7414 is not set
 # CONFIG_SENSORS_AD7418 is not set
+# CONFIG_SENSORS_ADCXX is not set
 # CONFIG_SENSORS_ADM1021 is not set
 # CONFIG_SENSORS_ADM1025 is not set
 # CONFIG_SENSORS_ADM1026 is not set
 # CONFIG_SENSORS_ADM1029 is not set
 # CONFIG_SENSORS_ADM1031 is not set
 # CONFIG_SENSORS_ADM9240 is not set
-# CONFIG_SENSORS_ASB100 is not set
+# CONFIG_SENSORS_ADT7470 is not set
+# CONFIG_SENSORS_ADT7473 is not set
 # CONFIG_SENSORS_ATXP1 is not set
 # CONFIG_SENSORS_DS1621 is not set
 # CONFIG_SENSORS_F71805F is not set
-# CONFIG_SENSORS_FSCHER is not set
-# CONFIG_SENSORS_FSCPOS is not set
+# CONFIG_SENSORS_F71882FG is not set
+# CONFIG_SENSORS_F75375S is not set
 # CONFIG_SENSORS_GL518SM is not set
 # CONFIG_SENSORS_GL520SM is not set
 # CONFIG_SENSORS_IT87 is not set
@@ -827,58 +820,76 @@ CONFIG_HWMON=y
 # CONFIG_SENSORS_LM87 is not set
 # CONFIG_SENSORS_LM90 is not set
 # CONFIG_SENSORS_LM92 is not set
+# CONFIG_SENSORS_LM93 is not set
+# CONFIG_SENSORS_MAX1111 is not set
 # CONFIG_SENSORS_MAX1619 is not set
 # CONFIG_SENSORS_MAX6650 is not set
 # CONFIG_SENSORS_PC87360 is not set
 # CONFIG_SENSORS_PC87427 is not set
+# CONFIG_SENSORS_DME1737 is not set
 # CONFIG_SENSORS_SMSC47M1 is not set
 # CONFIG_SENSORS_SMSC47M192 is not set
 # CONFIG_SENSORS_SMSC47B397 is not set
+# CONFIG_SENSORS_ADS7828 is not set
+# CONFIG_SENSORS_THMC50 is not set
 # CONFIG_SENSORS_VT1211 is not set
 # CONFIG_SENSORS_W83781D is not set
 # CONFIG_SENSORS_W83791D is not set
 # CONFIG_SENSORS_W83792D is not set
 # CONFIG_SENSORS_W83793 is not set
 # CONFIG_SENSORS_W83L785TS is not set
+# CONFIG_SENSORS_W83L786NG is not set
 # CONFIG_SENSORS_W83627HF is not set
 # CONFIG_SENSORS_W83627EHF is not set
 # CONFIG_HWMON_DEBUG_CHIP is not set
+# CONFIG_THERMAL is not set
+# CONFIG_THERMAL_HWMON is not set
+# CONFIG_WATCHDOG is not set
 
 #
 # Multifunction device drivers
 #
+# CONFIG_MFD_CORE is not set
 # CONFIG_MFD_SM501 is not set
+# CONFIG_HTC_PASIC3 is not set
+# CONFIG_MFD_TMIO is not set
+# CONFIG_MFD_WM8400 is not set
+# CONFIG_MFD_WM8350_I2C is not set
 
 #
 # Multimedia devices
 #
+
+#
+# Multimedia core support
+#
 # CONFIG_VIDEO_DEV is not set
 # CONFIG_DVB_CORE is not set
-CONFIG_DAB=y
+# CONFIG_VIDEO_MEDIA is not set
 
 #
-# Graphics support
+# Multimedia drivers
 #
-CONFIG_BACKLIGHT_LCD_SUPPORT=y
-CONFIG_BACKLIGHT_CLASS_DEVICE=y
-CONFIG_LCD_CLASS_DEVICE=y
+CONFIG_DAB=y
 
 #
-# Display device support
+# Graphics support
 #
-# CONFIG_DISPLAY_SUPPORT is not set
 # CONFIG_VGASTATE is not set
+# CONFIG_VIDEO_OUTPUT_CONTROL is not set
 CONFIG_FB=y
 CONFIG_FIRMWARE_EDID=y
 # CONFIG_FB_DDC is not set
+# CONFIG_FB_BOOT_VESA_SUPPORT is not set
 CONFIG_FB_CFB_FILLRECT=y
 CONFIG_FB_CFB_COPYAREA=y
 CONFIG_FB_CFB_IMAGEBLIT=y
+# CONFIG_FB_CFB_REV_PIXELS_IN_BYTE is not set
 # CONFIG_FB_SYS_FILLRECT is not set
 # CONFIG_FB_SYS_COPYAREA is not set
 # CONFIG_FB_SYS_IMAGEBLIT is not set
+# CONFIG_FB_FOREIGN_ENDIAN is not set
 # CONFIG_FB_SYS_FOPS is not set
-CONFIG_FB_DEFERRED_IO=y
 # CONFIG_FB_SVGALIB is not set
 # CONFIG_FB_MACMODES is not set
 # CONFIG_FB_BACKLIGHT is not set
@@ -888,25 +899,34 @@ CONFIG_FB_DEFERRED_IO=y
 #
 # Frame buffer hardware drivers
 #
-# CONFIG_FB_BFIN_7171 is not set
-# CONFIG_FB_BFIN_7393 is not set
+# CONFIG_FB_BFIN_T350MCQB is not set
+# CONFIG_FB_BFIN_LQ035Q1 is not set
 CONFIG_FB_BF537_LQ035=y
 CONFIG_LQ035_SLAVE_ADDR=0x58
 CONFIG_FB_BFIN_LANDSCAPE=y
 # CONFIG_FB_BFIN_BGR is not set
-# CONFIG_FB_BFIN_T350MCQB is not set
+# CONFIG_FB_BFIN_7393 is not set
+# CONFIG_FB_HITACHI_TX09 is not set
 # CONFIG_FB_S1D13XXX is not set
 # CONFIG_FB_VIRTUAL is not set
-# CONFIG_LOGO is not set
+# CONFIG_FB_METRONOME is not set
+CONFIG_BACKLIGHT_LCD_SUPPORT=y
+CONFIG_LCD_CLASS_DEVICE=y
+# CONFIG_LCD_LTV350QV is not set
+# CONFIG_LCD_ILI9320 is not set
+# CONFIG_LCD_TDO24M is not set
+# CONFIG_LCD_VGG2432A4 is not set
+# CONFIG_LCD_PLATFORM is not set
+CONFIG_BACKLIGHT_CLASS_DEVICE=y
+# CONFIG_BACKLIGHT_CORGI is not set
 
 #
-# Sound
+# Display device support
 #
+# CONFIG_DISPLAY_SUPPORT is not set
+# CONFIG_LOGO is not set
 CONFIG_SOUND=y
-
-#
-# Advanced Linux Sound Architecture
-#
+CONFIG_SOUND_OSS_CORE=y
 CONFIG_SND=m
 # CONFIG_SND_SEQUENCER is not set
 # CONFIG_SND_MIXER_OSS is not set
@@ -916,46 +936,30 @@ CONFIG_SND=m
 # CONFIG_SND_VERBOSE_PROCFS is not set
 # CONFIG_SND_VERBOSE_PRINTK is not set
 # CONFIG_SND_DEBUG is not set
-
-#
-# Generic devices
-#
+CONFIG_SND_DRIVERS=y
 # CONFIG_SND_DUMMY is not set
 # CONFIG_SND_MTPAV is not set
 # CONFIG_SND_SERIAL_U16550 is not set
 # CONFIG_SND_MPU401 is not set
+CONFIG_SND_SPI=y
 
 #
 # ALSA Blackfin devices
 #
 # CONFIG_SND_BLACKFIN_AD1836 is not set
-# CONFIG_SND_BFIN_AD73311 is not set
-
-#
-# System on Chip audio support
-#
+# CONFIG_SND_BFIN_AD73322 is not set
 # CONFIG_SND_SOC is not set
-
-#
-# Open Sound System
-#
 CONFIG_SOUND_PRIME=y
-# CONFIG_OSS_OBSOLETE is not set
-# CONFIG_SOUND_MSNDCLAS is not set
-# CONFIG_SOUND_MSNDPIN is not set
-
-#
-# HID Devices
-#
+CONFIG_HID_SUPPORT=y
 # CONFIG_HID is not set
-
-#
-# USB support
-#
+# CONFIG_HID_PID is not set
+CONFIG_USB_SUPPORT=y
 CONFIG_USB_ARCH_HAS_HCD=y
 # CONFIG_USB_ARCH_HAS_OHCI is not set
 # CONFIG_USB_ARCH_HAS_EHCI is not set
 # CONFIG_USB is not set
+# CONFIG_USB_OTG_WHITELIST is not set
+# CONFIG_USB_OTG_BLACKLIST_HUB is not set
 
 #
 # Enable Host or Gadget support to see Inventra options
@@ -964,37 +968,11 @@ CONFIG_USB_ARCH_HAS_HCD=y
 #
 # NOTE: USB_STORAGE enables SCSI, and 'SCSI disk support'
 #
-
-#
-# USB Gadget Support
-#
 # CONFIG_USB_GADGET is not set
 # CONFIG_MMC is not set
-
-#
-# LED devices
-#
+# CONFIG_MEMSTICK is not set
 # CONFIG_NEW_LEDS is not set
-
-#
-# LED drivers
-#
-
-#
-# LED Triggers
-#
-
-#
-# InfiniBand support
-#
-
-#
-# EDAC - error detection and reporting (RAS) (EXPERIMENTAL)
-#
-
-#
-# Real Time Clock
-#
+# CONFIG_ACCESSIBILITY is not set
 CONFIG_RTC_LIB=y
 CONFIG_RTC_CLASS=y
 CONFIG_RTC_HCTOSYS=y
@@ -1014,6 +992,7 @@ CONFIG_RTC_INTF_DEV=y
 # I2C RTC drivers
 #
 # CONFIG_RTC_DRV_DS1307 is not set
+# CONFIG_RTC_DRV_DS1374 is not set
 # CONFIG_RTC_DRV_DS1672 is not set
 # CONFIG_RTC_DRV_MAX6900 is not set
 # CONFIG_RTC_DRV_RS5C372 is not set
@@ -1021,43 +1000,41 @@ CONFIG_RTC_INTF_DEV=y
 # CONFIG_RTC_DRV_X1205 is not set
 # CONFIG_RTC_DRV_PCF8563 is not set
 # CONFIG_RTC_DRV_PCF8583 is not set
+# CONFIG_RTC_DRV_M41T80 is not set
+# CONFIG_RTC_DRV_S35390A is not set
+# CONFIG_RTC_DRV_FM3130 is not set
 
 #
 # SPI RTC drivers
 #
-# CONFIG_RTC_DRV_RS5C348 is not set
+# CONFIG_RTC_DRV_M41T94 is not set
+# CONFIG_RTC_DRV_DS1305 is not set
 # CONFIG_RTC_DRV_MAX6902 is not set
+# CONFIG_RTC_DRV_R9701 is not set
+# CONFIG_RTC_DRV_RS5C348 is not set
+# CONFIG_RTC_DRV_DS3234 is not set
 
 #
 # Platform RTC drivers
 #
+# CONFIG_RTC_DRV_DS1286 is not set
+# CONFIG_RTC_DRV_DS1511 is not set
 # CONFIG_RTC_DRV_DS1553 is not set
 # CONFIG_RTC_DRV_DS1742 is not set
+# CONFIG_RTC_DRV_STK17TA8 is not set
 # CONFIG_RTC_DRV_M48T86 is not set
+# CONFIG_RTC_DRV_M48T35 is not set
+# CONFIG_RTC_DRV_M48T59 is not set
+# CONFIG_RTC_DRV_BQ4802 is not set
 # CONFIG_RTC_DRV_V3020 is not set
 
 #
 # on-CPU RTC drivers
 #
 CONFIG_RTC_DRV_BFIN=y
-
-#
-# DMA Engine support
-#
-# CONFIG_DMA_ENGINE is not set
-
-#
-# DMA Clients
-#
-
-#
-# DMA Devices
-#
-
-#
-# PBX support
-#
-# CONFIG_PBX is not set
+# CONFIG_DMADEVICES is not set
+# CONFIG_UIO is not set
+# CONFIG_STAGING is not set
 
 #
 # File systems
@@ -1067,20 +1044,18 @@ CONFIG_EXT2_FS_XATTR=y
 # CONFIG_EXT2_FS_POSIX_ACL is not set
 # CONFIG_EXT2_FS_SECURITY is not set
 # CONFIG_EXT3_FS is not set
-# CONFIG_EXT4DEV_FS is not set
+# CONFIG_EXT4_FS is not set
 CONFIG_FS_MBCACHE=y
 # CONFIG_REISERFS_FS is not set
 # CONFIG_JFS_FS is not set
 # CONFIG_FS_POSIX_ACL is not set
+CONFIG_FILE_LOCKING=y
 # CONFIG_XFS_FS is not set
-# CONFIG_GFS2_FS is not set
 # CONFIG_OCFS2_FS is not set
-# CONFIG_MINIX_FS is not set
-# CONFIG_ROMFS_FS is not set
+# CONFIG_DNOTIFY is not set
 CONFIG_INOTIFY=y
 CONFIG_INOTIFY_USER=y
 # CONFIG_QUOTA is not set
-# CONFIG_DNOTIFY is not set
 # CONFIG_AUTOFS_FS is not set
 # CONFIG_AUTOFS4_FS is not set
 # CONFIG_FUSE_FS is not set
@@ -1106,7 +1081,6 @@ CONFIG_PROC_SYSCTL=y
 CONFIG_SYSFS=y
 # CONFIG_TMPFS is not set
 # CONFIG_HUGETLB_PAGE is not set
-CONFIG_RAMFS=y
 # CONFIG_CONFIGFS_FS is not set
 
 #
@@ -1121,36 +1095,35 @@ CONFIG_RAMFS=y
 # CONFIG_EFS_FS is not set
 CONFIG_YAFFS_FS=y
 CONFIG_YAFFS_YAFFS1=y
+# CONFIG_YAFFS_9BYTE_TAGS is not set
 # CONFIG_YAFFS_DOES_ECC is not set
 CONFIG_YAFFS_YAFFS2=y
 CONFIG_YAFFS_AUTO_YAFFS2=y
 # CONFIG_YAFFS_DISABLE_LAZY_LOAD is not set
-CONFIG_YAFFS_CHECKPOINT_RESERVED_BLOCKS=10
 # CONFIG_YAFFS_DISABLE_WIDE_TNODES is not set
 # CONFIG_YAFFS_ALWAYS_CHECK_CHUNK_ERASED is not set
 CONFIG_YAFFS_SHORT_NAMES_IN_RAM=y
 # CONFIG_JFFS2_FS is not set
 # CONFIG_CRAMFS is not set
 # CONFIG_VXFS_FS is not set
+# CONFIG_MINIX_FS is not set
+# CONFIG_OMFS_FS is not set
 # CONFIG_HPFS_FS is not set
 # CONFIG_QNX4FS_FS is not set
+# CONFIG_ROMFS_FS is not set
 # CONFIG_SYSV_FS is not set
 # CONFIG_UFS_FS is not set
-
-#
-# Network File Systems
-#
+CONFIG_NETWORK_FILESYSTEMS=y
 CONFIG_NFS_FS=m
 CONFIG_NFS_V3=y
 # CONFIG_NFS_V3_ACL is not set
 # CONFIG_NFS_V4 is not set
-# CONFIG_NFS_DIRECTIO is not set
 # CONFIG_NFSD is not set
 CONFIG_LOCKD=m
 CONFIG_LOCKD_V4=y
 CONFIG_NFS_COMMON=y
 CONFIG_SUNRPC=m
-# CONFIG_SUNRPC_BIND34 is not set
+# CONFIG_SUNRPC_REGISTER_V4 is not set
 # CONFIG_RPCSEC_GSS_KRB5 is not set
 # CONFIG_RPCSEC_GSS_SPKM3 is not set
 CONFIG_SMB_FS=m
@@ -1159,17 +1132,12 @@ CONFIG_SMB_FS=m
 # CONFIG_NCP_FS is not set
 # CONFIG_CODA_FS is not set
 # CONFIG_AFS_FS is not set
-# CONFIG_9P_FS is not set
 
 #
 # Partition Types
 #
 # CONFIG_PARTITION_ADVANCED is not set
 CONFIG_MSDOS_PARTITION=y
-
-#
-# Native Language Support
-#
 CONFIG_NLS=m
 CONFIG_NLS_DEFAULT="iso8859-1"
 # CONFIG_NLS_CODEPAGE_437 is not set
@@ -1210,29 +1178,30 @@ CONFIG_NLS_DEFAULT="iso8859-1"
 # CONFIG_NLS_KOI8_R is not set
 # CONFIG_NLS_KOI8_U is not set
 # CONFIG_NLS_UTF8 is not set
-
-#
-# Distributed Lock Manager
-#
 # CONFIG_DLM is not set
 
 #
-# Profiling support
-#
-# CONFIG_PROFILING is not set
-
-#
 # Kernel hacking
 #
 # CONFIG_PRINTK_TIME is not set
+CONFIG_ENABLE_WARN_DEPRECATED=y
 CONFIG_ENABLE_MUST_CHECK=y
+CONFIG_FRAME_WARN=1024
 # CONFIG_MAGIC_SYSRQ is not set
 # CONFIG_UNUSED_SYMBOLS is not set
 # CONFIG_DEBUG_FS is not set
 # CONFIG_HEADERS_CHECK is not set
 # CONFIG_DEBUG_KERNEL is not set
 # CONFIG_DEBUG_BUGVERBOSE is not set
+# CONFIG_DEBUG_MEMORY_INIT is not set
+# CONFIG_RCU_CPU_STALL_DETECTOR is not set
+# CONFIG_SYSCTL_SYSCALL_CHECK is not set
+# CONFIG_DYNAMIC_PRINTK_DEBUG is not set
+# CONFIG_SAMPLES is not set
+CONFIG_HAVE_ARCH_KGDB=y
+CONFIG_DEBUG_VERBOSE=y
 # CONFIG_DEBUG_MMRS is not set
+# CONFIG_DEBUG_DOUBLEFAULT is not set
 # CONFIG_DEBUG_HUNT_FOR_ZERO is not set
 CONFIG_DEBUG_BFIN_HWTRACE_ON=y
 CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION_OFF=y
@@ -1250,13 +1219,94 @@ CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION=0
 #
 # CONFIG_KEYS is not set
 CONFIG_SECURITY=y
+# CONFIG_SECURITYFS is not set
 # CONFIG_SECURITY_NETWORK is not set
-CONFIG_SECURITY_CAPABILITIES=y
+# CONFIG_SECURITY_FILE_CAPABILITIES is not set
+CONFIG_SECURITY_DEFAULT_MMAP_MIN_ADDR=0
+CONFIG_CRYPTO=y
+
+#
+# Crypto core or helper
+#
+# CONFIG_CRYPTO_FIPS is not set
+# CONFIG_CRYPTO_MANAGER is not set
+# CONFIG_CRYPTO_GF128MUL is not set
+# CONFIG_CRYPTO_NULL is not set
+# CONFIG_CRYPTO_CRYPTD is not set
+# CONFIG_CRYPTO_AUTHENC is not set
+# CONFIG_CRYPTO_TEST is not set
+
+#
+# Authenticated Encryption with Associated Data
+#
+# CONFIG_CRYPTO_CCM is not set
+# CONFIG_CRYPTO_GCM is not set
+# CONFIG_CRYPTO_SEQIV is not set
+
+#
+# Block modes
+#
+# CONFIG_CRYPTO_CBC is not set
+# CONFIG_CRYPTO_CTR is not set
+# CONFIG_CRYPTO_CTS is not set
+# CONFIG_CRYPTO_ECB is not set
+# CONFIG_CRYPTO_LRW is not set
+# CONFIG_CRYPTO_PCBC is not set
+# CONFIG_CRYPTO_XTS is not set
+
+#
+# Hash modes
+#
+# CONFIG_CRYPTO_HMAC is not set
+# CONFIG_CRYPTO_XCBC is not set
+
+#
+# Digest
+#
+# CONFIG_CRYPTO_CRC32C is not set
+# CONFIG_CRYPTO_MD4 is not set
+# CONFIG_CRYPTO_MD5 is not set
+# CONFIG_CRYPTO_MICHAEL_MIC is not set
+# CONFIG_CRYPTO_RMD128 is not set
+# CONFIG_CRYPTO_RMD160 is not set
+# CONFIG_CRYPTO_RMD256 is not set
+# CONFIG_CRYPTO_RMD320 is not set
+# CONFIG_CRYPTO_SHA1 is not set
+# CONFIG_CRYPTO_SHA256 is not set
+# CONFIG_CRYPTO_SHA512 is not set
+# CONFIG_CRYPTO_TGR192 is not set
+# CONFIG_CRYPTO_WP512 is not set
+
+#
+# Ciphers
+#
+# CONFIG_CRYPTO_AES is not set
+# CONFIG_CRYPTO_ANUBIS is not set
+# CONFIG_CRYPTO_ARC4 is not set
+# CONFIG_CRYPTO_BLOWFISH is not set
+# CONFIG_CRYPTO_CAMELLIA is not set
+# CONFIG_CRYPTO_CAST5 is not set
+# CONFIG_CRYPTO_CAST6 is not set
+# CONFIG_CRYPTO_DES is not set
+# CONFIG_CRYPTO_FCRYPT is not set
+# CONFIG_CRYPTO_KHAZAD is not set
+# CONFIG_CRYPTO_SALSA20 is not set
+# CONFIG_CRYPTO_SEED is not set
+# CONFIG_CRYPTO_SERPENT is not set
+# CONFIG_CRYPTO_TEA is not set
+# CONFIG_CRYPTO_TWOFISH is not set
+
+#
+# Compression
+#
+# CONFIG_CRYPTO_DEFLATE is not set
+# CONFIG_CRYPTO_LZO is not set
 
 #
-# Cryptographic options
+# Random Number Generation
 #
-# CONFIG_CRYPTO is not set
+# CONFIG_CRYPTO_ANSI_CPRNG is not set
+CONFIG_CRYPTO_HW=y
 
 #
 # Library routines
@@ -1264,8 +1314,10 @@ CONFIG_SECURITY_CAPABILITIES=y
 CONFIG_BITREVERSE=y
 CONFIG_CRC_CCITT=m
 # CONFIG_CRC16 is not set
+# CONFIG_CRC_T10DIF is not set
 # CONFIG_CRC_ITU_T is not set
 CONFIG_CRC32=y
+# CONFIG_CRC7 is not set
 # CONFIG_LIBCRC32C is not set
 CONFIG_ZLIB_INFLATE=y
 CONFIG_PLIST=y
diff --git a/arch/blackfin/configs/SRV1_defconfig b/arch/blackfin/configs/SRV1_defconfig
index b1309f878fcd..7c8250d6fa66 100644
--- a/arch/blackfin/configs/SRV1_defconfig
+++ b/arch/blackfin/configs/SRV1_defconfig
@@ -59,7 +59,7 @@ CONFIG_KALLSYMS_ALL=y
 CONFIG_HOTPLUG=y
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
@@ -172,14 +172,14 @@ CONFIG_IRQ_UART1_RX=10
 CONFIG_IRQ_UART1_TX=10
 CONFIG_IRQ_MAC_RX=11
 CONFIG_IRQ_MAC_TX=11
-CONFIG_IRQ_TMR0=12
-CONFIG_IRQ_TMR1=12
-CONFIG_IRQ_TMR2=12
-CONFIG_IRQ_TMR3=12
-CONFIG_IRQ_TMR4=12
-CONFIG_IRQ_TMR5=12
-CONFIG_IRQ_TMR6=12
-CONFIG_IRQ_TMR7=12
+CONFIG_IRQ_TIMER0=12
+CONFIG_IRQ_TIMER1=12
+CONFIG_IRQ_TIMER2=12
+CONFIG_IRQ_TIMER3=12
+CONFIG_IRQ_TIMER4=12
+CONFIG_IRQ_TIMER5=12
+CONFIG_IRQ_TIMER6=12
+CONFIG_IRQ_TIMER7=12
 CONFIG_IRQ_PORTG_INTB=12
 CONFIG_IRQ_MEM_DMA0=13
 CONFIG_IRQ_MEM_DMA1=13
@@ -271,7 +271,6 @@ CONFIG_SPLIT_PTLOCK_CPUS=4
 # CONFIG_RESOURCES_64BIT is not set
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_LARGE_ALLOCS=y
-CONFIG_BFIN_DMA_5XX=y
 CONFIG_DMA_UNCACHED_2M=y
 # CONFIG_DMA_UNCACHED_1M is not set
 # CONFIG_DMA_UNCACHED_NONE is not set
@@ -786,7 +785,7 @@ CONFIG_I2C_CHARDEV=y
 #
 # CONFIG_I2C_BLACKFIN_GPIO is not set
 CONFIG_I2C_BLACKFIN_TWI=y
-CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=50
+CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ=100
 # CONFIG_I2C_GPIO is not set
 # CONFIG_I2C_OCORES is not set
 # CONFIG_I2C_PARPORT_LIGHT is not set
diff --git a/arch/blackfin/configs/TCM-BF537_defconfig b/arch/blackfin/configs/TCM-BF537_defconfig
index c482ee171f9e..9af522c7dadf 100644
--- a/arch/blackfin/configs/TCM-BF537_defconfig
+++ b/arch/blackfin/configs/TCM-BF537_defconfig
@@ -1,7 +1,7 @@
 #
 # Automatically generated make config: don't edit
-# Linux kernel version: 2.6.24.7
-# Thu Jul 31 00:53:15 2008
+# Linux kernel version: 2.6.28-rc2
+# Tue Jan  6 09:22:17 2009
 #
 # CONFIG_MMU is not set
 # CONFIG_FPU is not set
@@ -9,7 +9,6 @@ CONFIG_RWSEM_GENERIC_SPINLOCK=y
 # CONFIG_RWSEM_XCHGADD_ALGORITHM is not set
 CONFIG_BLACKFIN=y
 CONFIG_ZONE_DMA=y
-CONFIG_SEMAPHORE_SLEEPERS=y
 CONFIG_GENERIC_FIND_NEXT_BIT=y
 CONFIG_GENERIC_HWEIGHT=y
 CONFIG_GENERIC_HARDIRQS=y
@@ -30,17 +29,14 @@ CONFIG_LOCALVERSION_AUTO=y
 CONFIG_SYSVIPC=y
 CONFIG_SYSVIPC_SYSCTL=y
 # CONFIG_BSD_PROCESS_ACCT is not set
-# CONFIG_USER_NS is not set
-# CONFIG_PID_NS is not set
 CONFIG_IKCONFIG=y
 CONFIG_IKCONFIG_PROC=y
 CONFIG_LOG_BUF_SHIFT=14
 # CONFIG_CGROUPS is not set
-CONFIG_FAIR_GROUP_SCHED=y
-CONFIG_FAIR_USER_SCHED=y
-# CONFIG_FAIR_CGROUP_SCHED is not set
-CONFIG_SYSFS_DEPRECATED=y
+# CONFIG_GROUP_SCHED is not set
+# CONFIG_SYSFS_DEPRECATED_V2 is not set
 # CONFIG_RELAY is not set
+# CONFIG_NAMESPACES is not set
 # CONFIG_BLK_DEV_INITRD is not set
 # CONFIG_CC_OPTIMIZE_FOR_SIZE is not set
 CONFIG_SYSCTL=y
@@ -52,22 +48,30 @@ CONFIG_KALLSYMS=y
 # CONFIG_HOTPLUG is not set
 CONFIG_PRINTK=y
 CONFIG_BUG=y
-CONFIG_ELF_CORE=y
+# CONFIG_ELF_CORE is not set
+CONFIG_COMPAT_BRK=y
 CONFIG_BASE_FULL=y
 CONFIG_FUTEX=y
 CONFIG_ANON_INODES=y
 CONFIG_EPOLL=y
 CONFIG_SIGNALFD=y
+CONFIG_TIMERFD=y
 CONFIG_EVENTFD=y
+CONFIG_AIO=y
 CONFIG_VM_EVENT_COUNTERS=y
 CONFIG_SLAB=y
 # CONFIG_SLUB is not set
 # CONFIG_SLOB is not set
+# CONFIG_PROFILING is not set
+# CONFIG_MARKERS is not set
+CONFIG_HAVE_OPROFILE=y
+# CONFIG_HAVE_GENERIC_DMA_COHERENT is not set
 CONFIG_SLABINFO=y
 CONFIG_RT_MUTEXES=y
 CONFIG_TINY_SHMEM=y
 CONFIG_BASE_SMALL=0
 CONFIG_MODULES=y
+# CONFIG_MODULE_FORCE_LOAD is not set
 CONFIG_MODULE_UNLOAD=y
 # CONFIG_MODULE_FORCE_UNLOAD is not set
 # CONFIG_MODVERSIONS is not set
@@ -78,6 +82,7 @@ CONFIG_BLOCK=y
 # CONFIG_BLK_DEV_IO_TRACE is not set
 # CONFIG_LSF is not set
 # CONFIG_BLK_DEV_BSG is not set
+# CONFIG_BLK_DEV_INTEGRITY is not set
 
 #
 # IO Schedulers
@@ -91,9 +96,11 @@ CONFIG_IOSCHED_CFQ=y
 # CONFIG_DEFAULT_CFQ is not set
 CONFIG_DEFAULT_NOOP=y
 CONFIG_DEFAULT_IOSCHED="noop"
+CONFIG_CLASSIC_RCU=y
 CONFIG_PREEMPT_NONE=y
 # CONFIG_PREEMPT_VOLUNTARY is not set
 # CONFIG_PREEMPT is not set
+# CONFIG_FREEZER is not set
 
 #
 # Blackfin Processor Options
@@ -102,6 +109,10 @@ CONFIG_PREEMPT_NONE=y
 #
 # Processor and Board Settings
 #
+# CONFIG_BF512 is not set
+# CONFIG_BF514 is not set
+# CONFIG_BF516 is not set
+# CONFIG_BF518 is not set
 # CONFIG_BF522 is not set
 # CONFIG_BF523 is not set
 # CONFIG_BF524 is not set
@@ -114,18 +125,23 @@ CONFIG_PREEMPT_NONE=y
 # CONFIG_BF534 is not set
 # CONFIG_BF536 is not set
 CONFIG_BF537=y
+# CONFIG_BF538 is not set
+# CONFIG_BF539 is not set
 # CONFIG_BF542 is not set
 # CONFIG_BF544 is not set
 # CONFIG_BF547 is not set
 # CONFIG_BF548 is not set
 # CONFIG_BF549 is not set
 # CONFIG_BF561 is not set
+CONFIG_BF_REV_MIN=2
+CONFIG_BF_REV_MAX=3
 # CONFIG_BF_REV_0_0 is not set
 # CONFIG_BF_REV_0_1 is not set
 CONFIG_BF_REV_0_2=y
 # CONFIG_BF_REV_0_3 is not set
 # CONFIG_BF_REV_0_4 is not set
 # CONFIG_BF_REV_0_5 is not set
+# CONFIG_BF_REV_0_6 is not set
 # CONFIG_BF_REV_ANY is not set
 # CONFIG_BF_REV_NONE is not set
 CONFIG_BF53x=y
@@ -137,25 +153,25 @@ CONFIG_IRQ_SPORT0_TX=9
 CONFIG_IRQ_SPORT1_RX=9
 CONFIG_IRQ_SPORT1_TX=9
 CONFIG_IRQ_TWI=10
-CONFIG_IRQ_SPI=10
 CONFIG_IRQ_UART0_RX=10
 CONFIG_IRQ_UART0_TX=10
 CONFIG_IRQ_UART1_RX=10
 CONFIG_IRQ_UART1_TX=10
 CONFIG_IRQ_MAC_RX=11
 CONFIG_IRQ_MAC_TX=11
-CONFIG_IRQ_TMR0=12
-CONFIG_IRQ_TMR1=12
-CONFIG_IRQ_TMR2=12
-CONFIG_IRQ_TMR3=12
-CONFIG_IRQ_TMR4=12
-CONFIG_IRQ_TMR5=12
-CONFIG_IRQ_TMR6=12
-CONFIG_IRQ_TMR7=12
+CONFIG_IRQ_TIMER0=12
+CONFIG_IRQ_TIMER1=12
+CONFIG_IRQ_TIMER2=12
+CONFIG_IRQ_TIMER3=12
+CONFIG_IRQ_TIMER4=12
+CONFIG_IRQ_TIMER5=12
+CONFIG_IRQ_TIMER6=12
+CONFIG_IRQ_TIMER7=12
 CONFIG_IRQ_PORTG_INTB=12
 CONFIG_IRQ_MEM_DMA0=13
 CONFIG_IRQ_MEM_DMA1=13
 CONFIG_IRQ_WATCH=13
+CONFIG_IRQ_SPI=10
 # CONFIG_BFIN537_STAMP is not set
 # CONFIG_BFIN537_BLUETECHNIX_CM is not set
 CONFIG_BFIN537_BLUETECHNIX_TCM=y
@@ -191,7 +207,6 @@ CONFIG_BOOT_LOAD=0x1000
 #
 CONFIG_CLKIN_HZ=25000000
 # CONFIG_BFIN_KERNEL_CLOCK is not set
-CONFIG_MAX_MEM_SIZE=32
 CONFIG_MAX_VCO_HZ=600000000
 CONFIG_MIN_VCO_HZ=50000000
 CONFIG_MAX_SCLK_HZ=133333333
@@ -205,10 +220,10 @@ CONFIG_HZ_250=y
 # CONFIG_HZ_300 is not set
 # CONFIG_HZ_1000 is not set
 CONFIG_HZ=250
+# CONFIG_SCHED_HRTICK is not set
 CONFIG_GENERIC_TIME=y
 CONFIG_GENERIC_CLOCKEVENTS=y
 # CONFIG_CYCLES_CLOCKSOURCE is not set
-# CONFIG_TICK_ONESHOT is not set
 # CONFIG_NO_HZ is not set
 # CONFIG_HIGH_RES_TIMERS is not set
 CONFIG_GENERIC_CLOCKEVENTS_BUILD=y
@@ -242,6 +257,12 @@ CONFIG_IP_CHECKSUM_L1=y
 CONFIG_CACHELINE_ALIGNED_L1=y
 CONFIG_SYSCALL_TAB_L1=y
 CONFIG_CPLB_SWITCH_TAB_L1=y
+CONFIG_APP_STACK_L1=y
+
+#
+# Speed Optimizations
+#
+CONFIG_BFIN_INS_LOWOVERHEAD=y
 CONFIG_RAMKERNEL=y
 # CONFIG_ROMKERNEL is not set
 CONFIG_SELECT_MEMORY_MODEL=y
@@ -250,14 +271,13 @@ CONFIG_FLATMEM_MANUAL=y
 # CONFIG_SPARSEMEM_MANUAL is not set
 CONFIG_FLATMEM=y
 CONFIG_FLAT_NODE_MEM_MAP=y
-# CONFIG_SPARSEMEM_STATIC is not set
-# CONFIG_SPARSEMEM_VMEMMAP_ENABLE is not set
+CONFIG_PAGEFLAGS_EXTENDED=y
 CONFIG_SPLIT_PTLOCK_CPUS=4
 # CONFIG_RESOURCES_64BIT is not set
+# CONFIG_PHYS_ADDR_T_64BIT is not set
 CONFIG_ZONE_DMA_FLAG=1
 CONFIG_VIRT_TO_BUS=y
 # CONFIG_BFIN_GPTIMERS is not set
-CONFIG_BFIN_DMA_5XX=y
 # CONFIG_DMA_UNCACHED_4M is not set
 # CONFIG_DMA_UNCACHED_2M is not set
 CONFIG_DMA_UNCACHED_1M=y
@@ -300,7 +320,6 @@ CONFIG_BANK_3=0xFFC2
 #
 # Bus options (PCI, PCMCIA, EISA, MCA, ISA)
 #
-# CONFIG_PCI is not set
 # CONFIG_ARCH_SUPPORTS_MSI is not set
 
 #
@@ -310,23 +329,20 @@ CONFIG_BINFMT_ELF_FDPIC=y
 CONFIG_BINFMT_FLAT=y
 CONFIG_BINFMT_ZFLAT=y
 CONFIG_BINFMT_SHARED_FLAT=y
+# CONFIG_HAVE_AOUT is not set
 # CONFIG_BINFMT_MISC is not set
 
 #
 # Power management options
 #
 # CONFIG_PM is not set
-CONFIG_SUSPEND_UP_POSSIBLE=y
+CONFIG_ARCH_SUSPEND_POSSIBLE=y
 # CONFIG_PM_WAKEUP_BY_GPIO is not set
 
 #
 # CPU Frequency scaling
 #
 # CONFIG_CPU_FREQ is not set
-
-#
-# Networking
-#
 # CONFIG_NET is not set
 
 #
@@ -345,6 +361,7 @@ CONFIG_MTD=y
 CONFIG_MTD_PARTITIONS=y
 # CONFIG_MTD_REDBOOT_PARTS is not set
 # CONFIG_MTD_CMDLINE_PARTS is not set
+# CONFIG_MTD_AR7_PARTS is not set
 
 #
 # User Modules And Translation Layers
@@ -362,8 +379,10 @@ CONFIG_MTD_BLOCK=y
 #
 # RAM/ROM/Flash chip drivers
 #
-# CONFIG_MTD_CFI is not set
+CONFIG_MTD_CFI=y
 # CONFIG_MTD_JEDECPROBE is not set
+CONFIG_MTD_GEN_PROBE=y
+# CONFIG_MTD_CFI_ADV_OPTIONS is not set
 CONFIG_MTD_MAP_BANK_WIDTH_1=y
 CONFIG_MTD_MAP_BANK_WIDTH_2=y
 CONFIG_MTD_MAP_BANK_WIDTH_4=y
@@ -374,6 +393,10 @@ CONFIG_MTD_CFI_I1=y
 CONFIG_MTD_CFI_I2=y
 # CONFIG_MTD_CFI_I4 is not set
 # CONFIG_MTD_CFI_I8 is not set
+CONFIG_MTD_CFI_INTELEXT=y
+# CONFIG_MTD_CFI_AMDSTD is not set
+# CONFIG_MTD_CFI_STAA is not set
+CONFIG_MTD_CFI_UTIL=y
 CONFIG_MTD_RAM=y
 # CONFIG_MTD_ROM is not set
 # CONFIG_MTD_ABSENT is not set
@@ -381,8 +404,9 @@ CONFIG_MTD_RAM=y
 #
 # Mapping drivers for chip access
 #
-# CONFIG_MTD_COMPLEX_MAPPINGS is not set
-# CONFIG_MTD_GPIO_ADDR is not set
+CONFIG_MTD_COMPLEX_MAPPINGS=y
+# CONFIG_MTD_PHYSMAP is not set
+CONFIG_MTD_GPIO_ADDR=y
 CONFIG_MTD_UCLINUX=y
 # CONFIG_MTD_PLATRAM is not set
 
@@ -416,10 +440,13 @@ CONFIG_BLK_DEV=y
 CONFIG_BLK_DEV_RAM=y
 CONFIG_BLK_DEV_RAM_COUNT=16
 CONFIG_BLK_DEV_RAM_SIZE=4096
-CONFIG_BLK_DEV_RAM_BLOCKSIZE=1024
+# CONFIG_BLK_DEV_XIP is not set
 # CONFIG_CDROM_PKTCDVD is not set
+# CONFIG_BLK_DEV_HD is not set
 CONFIG_MISC_DEVICES=y
 # CONFIG_EEPROM_93CX6 is not set
+# CONFIG_ENCLOSURE_SERVICES is not set
+CONFIG_HAVE_IDE=y
 # CONFIG_IDE is not set
 
 #
@@ -454,8 +481,11 @@ CONFIG_MISC_DEVICES=y
 # CONFIG_BF5xx_PPI is not set
 CONFIG_BFIN_SPORT=y
 # CONFIG_BFIN_TIMER_LATENCY is not set
+CONFIG_BFIN_DMA_INTERFACE=m
 # CONFIG_SIMPLE_GPIO is not set
 # CONFIG_VT is not set
+# CONFIG_DEVKMEM is not set
+# CONFIG_BFIN_JTAG_COMM is not set
 # CONFIG_SERIAL_NONSTANDARD is not set
 
 #
@@ -486,15 +516,10 @@ CONFIG_UNIX98_PTYS=y
 # CONFIG_CAN4LINUX is not set
 # CONFIG_IPMI_HANDLER is not set
 # CONFIG_HW_RANDOM is not set
-# CONFIG_GEN_RTC is not set
 # CONFIG_R3964 is not set
 # CONFIG_RAW_DRIVER is not set
 # CONFIG_TCG_TPM is not set
 # CONFIG_I2C is not set
-
-#
-# SPI support
-#
 CONFIG_SPI=y
 CONFIG_SPI_MASTER=y
 
@@ -502,6 +527,7 @@ CONFIG_SPI_MASTER=y
 # SPI Master Controller Drivers
 #
 CONFIG_SPI_BFIN=y
+# CONFIG_SPI_BFIN_LOCK is not set
 # CONFIG_SPI_BITBANG is not set
 
 #
@@ -510,9 +536,13 @@ CONFIG_SPI_BFIN=y
 # CONFIG_SPI_AT25 is not set
 # CONFIG_SPI_SPIDEV is not set
 # CONFIG_SPI_TLE62X0 is not set
+CONFIG_ARCH_WANT_OPTIONAL_GPIOLIB=y
+# CONFIG_GPIOLIB is not set
 # CONFIG_W1 is not set
 # CONFIG_POWER_SUPPLY is not set
 # CONFIG_HWMON is not set
+# CONFIG_THERMAL is not set
+# CONFIG_THERMAL_HWMON is not set
 CONFIG_WATCHDOG=y
 # CONFIG_WATCHDOG_NOWAYOUT is not set
 
@@ -523,20 +553,27 @@ CONFIG_WATCHDOG=y
 CONFIG_BFIN_WDT=y
 
 #
-# Sonics Silicon Backplane
-#
-CONFIG_SSB_POSSIBLE=y
-# CONFIG_SSB is not set
-
-#
 # Multifunction device drivers
 #
+# CONFIG_MFD_CORE is not set
 # CONFIG_MFD_SM501 is not set
+# CONFIG_HTC_PASIC3 is not set
+# CONFIG_MFD_TMIO is not set
+# CONFIG_MFD_WM8400 is not set
 
 #
 # Multimedia devices
 #
+
+#
+# Multimedia core support
+#
 # CONFIG_VIDEO_DEV is not set
+# CONFIG_VIDEO_MEDIA is not set
+
+#
+# Multimedia drivers
+#
 # CONFIG_DAB is not set
 
 #
@@ -551,20 +588,16 @@ CONFIG_SSB_POSSIBLE=y
 # Display device support
 #
 # CONFIG_DISPLAY_SUPPORT is not set
-
-#
-# Sound
-#
 # CONFIG_SOUND is not set
 # CONFIG_USB_SUPPORT is not set
 # CONFIG_MMC is not set
+# CONFIG_MEMSTICK is not set
 # CONFIG_NEW_LEDS is not set
+# CONFIG_ACCESSIBILITY is not set
 # CONFIG_RTC_CLASS is not set
-
-#
-# Userspace I/O
-#
+# CONFIG_DMADEVICES is not set
 # CONFIG_UIO is not set
+# CONFIG_STAGING is not set
 
 #
 # File systems
@@ -574,19 +607,17 @@ CONFIG_EXT2_FS_XATTR=y
 # CONFIG_EXT2_FS_POSIX_ACL is not set
 # CONFIG_EXT2_FS_SECURITY is not set
 # CONFIG_EXT3_FS is not set
-# CONFIG_EXT4DEV_FS is not set
+# CONFIG_EXT4_FS is not set
 CONFIG_FS_MBCACHE=y
 # CONFIG_REISERFS_FS is not set
 # CONFIG_JFS_FS is not set
 # CONFIG_FS_POSIX_ACL is not set
+CONFIG_FILE_LOCKING=y
 # CONFIG_XFS_FS is not set
-# CONFIG_GFS2_FS is not set
-# CONFIG_MINIX_FS is not set
-# CONFIG_ROMFS_FS is not set
+# CONFIG_DNOTIFY is not set
 CONFIG_INOTIFY=y
 CONFIG_INOTIFY_USER=y
 # CONFIG_QUOTA is not set
-# CONFIG_DNOTIFY is not set
 # CONFIG_AUTOFS_FS is not set
 # CONFIG_AUTOFS4_FS is not set
 # CONFIG_FUSE_FS is not set
@@ -628,8 +659,11 @@ CONFIG_SYSFS=y
 # CONFIG_JFFS2_FS is not set
 # CONFIG_CRAMFS is not set
 # CONFIG_VXFS_FS is not set
+# CONFIG_MINIX_FS is not set
+# CONFIG_OMFS_FS is not set
 # CONFIG_HPFS_FS is not set
 # CONFIG_QNX4FS_FS is not set
+# CONFIG_ROMFS_FS is not set
 # CONFIG_SYSV_FS is not set
 # CONFIG_UFS_FS is not set
 
@@ -639,7 +673,6 @@ CONFIG_SYSFS=y
 # CONFIG_PARTITION_ADVANCED is not set
 CONFIG_MSDOS_PARTITION=y
 # CONFIG_NLS is not set
-# CONFIG_INSTRUMENTATION is not set
 
 #
 # Kernel hacking
@@ -647,14 +680,22 @@ CONFIG_MSDOS_PARTITION=y
 # CONFIG_PRINTK_TIME is not set
 CONFIG_ENABLE_WARN_DEPRECATED=y
 CONFIG_ENABLE_MUST_CHECK=y
+CONFIG_FRAME_WARN=1024
 # CONFIG_MAGIC_SYSRQ is not set
 # CONFIG_UNUSED_SYMBOLS is not set
 CONFIG_DEBUG_FS=y
 # CONFIG_HEADERS_CHECK is not set
 # CONFIG_DEBUG_KERNEL is not set
 # CONFIG_DEBUG_BUGVERBOSE is not set
+# CONFIG_DEBUG_MEMORY_INIT is not set
+# CONFIG_RCU_CPU_STALL_DETECTOR is not set
+# CONFIG_SYSCTL_SYSCALL_CHECK is not set
+# CONFIG_DYNAMIC_PRINTK_DEBUG is not set
 # CONFIG_SAMPLES is not set
+CONFIG_HAVE_ARCH_KGDB=y
+CONFIG_DEBUG_VERBOSE=y
 CONFIG_DEBUG_MMRS=y
+# CONFIG_DEBUG_DOUBLEFAULT is not set
 CONFIG_DEBUG_HUNT_FOR_ZERO=y
 CONFIG_DEBUG_BFIN_HWTRACE_ON=y
 CONFIG_DEBUG_BFIN_HWTRACE_COMPRESSION_OFF=y
@@ -671,9 +712,8 @@ CONFIG_ACCESS_CHECK=y
 # Security options
 #
 # CONFIG_KEYS is not set
-CONFIG_SECURITY=y
-# CONFIG_SECURITY_NETWORK is not set
-CONFIG_SECURITY_CAPABILITIES=y
+# CONFIG_SECURITY is not set
+# CONFIG_SECURITYFS is not set
 # CONFIG_SECURITY_FILE_CAPABILITIES is not set
 # CONFIG_CRYPTO is not set
 
@@ -682,6 +722,7 @@ CONFIG_SECURITY_CAPABILITIES=y
 #
 # CONFIG_CRC_CCITT is not set
 # CONFIG_CRC16 is not set
+# CONFIG_CRC_T10DIF is not set
 # CONFIG_CRC_ITU_T is not set
 # CONFIG_CRC32 is not set
 # CONFIG_CRC7 is not set
diff --git a/arch/blackfin/include/asm/Kbuild b/arch/blackfin/include/asm/Kbuild
index 606ecfdcc962..d0d1ac435544 100644
--- a/arch/blackfin/include/asm/Kbuild
+++ b/arch/blackfin/include/asm/Kbuild
@@ -1,3 +1,4 @@
 include include/asm-generic/Kbuild.asm
 
 unifdef-y += fixed_code.h
+unifdef-y += swab.h
diff --git a/arch/blackfin/include/asm/atomic.h b/arch/blackfin/include/asm/atomic.h
index 7cf508718605..94b2a9b19451 100644
--- a/arch/blackfin/include/asm/atomic.h
+++ b/arch/blackfin/include/asm/atomic.h
@@ -1,6 +1,7 @@
 #ifndef __ARCH_BLACKFIN_ATOMIC__
 #define __ARCH_BLACKFIN_ATOMIC__
 
+#include <linux/types.h>
 #include <asm/system.h>	/* local_irq_XXX() */
 
 /*
@@ -13,108 +14,160 @@
  * Tony Kou (tonyko@lineo.ca)   Lineo Inc.   2001
  */
 
-typedef struct {
-	int counter;
-} atomic_t;
 #define ATOMIC_INIT(i)	{ (i) }
-
-#define atomic_read(v)		((v)->counter)
 #define atomic_set(v, i)	(((v)->counter) = i)
 
-static __inline__ void atomic_add(int i, atomic_t * v)
+#ifdef CONFIG_SMP
+
+#define atomic_read(v)	__raw_uncached_fetch_asm(&(v)->counter)
+
+asmlinkage int __raw_uncached_fetch_asm(const volatile int *ptr);
+
+asmlinkage int __raw_atomic_update_asm(volatile int *ptr, int value);
+
+asmlinkage int __raw_atomic_clear_asm(volatile int *ptr, int value);
+
+asmlinkage int __raw_atomic_set_asm(volatile int *ptr, int value);
+
+asmlinkage int __raw_atomic_xor_asm(volatile int *ptr, int value);
+
+asmlinkage int __raw_atomic_test_asm(const volatile int *ptr, int value);
+
+static inline void atomic_add(int i, atomic_t *v)
+{
+	__raw_atomic_update_asm(&v->counter, i);
+}
+
+static inline void atomic_sub(int i, atomic_t *v)
+{
+	__raw_atomic_update_asm(&v->counter, -i);
+}
+
+static inline int atomic_add_return(int i, atomic_t *v)
+{
+	return __raw_atomic_update_asm(&v->counter, i);
+}
+
+static inline int atomic_sub_return(int i, atomic_t *v)
+{
+	return __raw_atomic_update_asm(&v->counter, -i);
+}
+
+static inline void atomic_inc(volatile atomic_t *v)
+{
+	__raw_atomic_update_asm(&v->counter, 1);
+}
+
+static inline void atomic_dec(volatile atomic_t *v)
+{
+	__raw_atomic_update_asm(&v->counter, -1);
+}
+
+static inline void atomic_clear_mask(int mask, atomic_t *v)
+{
+	__raw_atomic_clear_asm(&v->counter, mask);
+}
+
+static inline void atomic_set_mask(int mask, atomic_t *v)
+{
+	__raw_atomic_set_asm(&v->counter, mask);
+}
+
+static inline int atomic_test_mask(int mask, atomic_t *v)
+{
+	return __raw_atomic_test_asm(&v->counter, mask);
+}
+
+/* Atomic operations are already serializing */
+#define smp_mb__before_atomic_dec()    barrier()
+#define smp_mb__after_atomic_dec() barrier()
+#define smp_mb__before_atomic_inc()    barrier()
+#define smp_mb__after_atomic_inc() barrier()
+
+#else /* !CONFIG_SMP */
+
+#define atomic_read(v)	((v)->counter)
+
+static inline void atomic_add(int i, atomic_t *v)
 {
 	long flags;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	v->counter += i;
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
 
-static __inline__ void atomic_sub(int i, atomic_t * v)
+static inline void atomic_sub(int i, atomic_t *v)
 {
 	long flags;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	v->counter -= i;
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 }
 
-static inline int atomic_add_return(int i, atomic_t * v)
+static inline int atomic_add_return(int i, atomic_t *v)
 {
 	int __temp = 0;
 	long flags;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	v->counter += i;
 	__temp = v->counter;
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 
 	return __temp;
 }
 
-#define atomic_add_negative(a, v)	(atomic_add_return((a), (v)) < 0)
-static inline int atomic_sub_return(int i, atomic_t * v)
+static inline int atomic_sub_return(int i, atomic_t *v)
 {
 	int __temp = 0;
 	long flags;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	v->counter -= i;
 	__temp = v->counter;
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 	return __temp;
 }
 
-static __inline__ void atomic_inc(volatile atomic_t * v)
+static inline void atomic_inc(volatile atomic_t *v)
 {
 	long flags;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	v->counter++;
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
 
-#define atomic_cmpxchg(v, o, n) ((int)cmpxchg(&((v)->counter), (o), (n)))
-#define atomic_xchg(v, new) (xchg(&((v)->counter), new))
-
-#define atomic_add_unless(v, a, u)				\
-({								\
-	int c, old;						\
-	c = atomic_read(v);					\
-	while (c != (u) && (old = atomic_cmpxchg((v), c, c + (a))) != c) \
-		c = old;					\
-	c != (u);						\
-})
-#define atomic_inc_not_zero(v) atomic_add_unless((v), 1, 0)
-
-static __inline__ void atomic_dec(volatile atomic_t * v)
+static inline void atomic_dec(volatile atomic_t *v)
 {
 	long flags;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	v->counter--;
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
 
-static __inline__ void atomic_clear_mask(unsigned int mask, atomic_t * v)
+static inline void atomic_clear_mask(unsigned int mask, atomic_t *v)
 {
 	long flags;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	v->counter &= ~mask;
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
 
-static __inline__ void atomic_set_mask(unsigned int mask, atomic_t * v)
+static inline void atomic_set_mask(unsigned int mask, atomic_t *v)
 {
 	long flags;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	v->counter |= mask;
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
 
 /* Atomic operations are already serializing */
@@ -123,9 +176,25 @@ static __inline__ void atomic_set_mask(unsigned int mask, atomic_t * v)
 #define smp_mb__before_atomic_inc()    barrier()
 #define smp_mb__after_atomic_inc() barrier()
 
+#endif /* !CONFIG_SMP */
+
+#define atomic_add_negative(a, v)	(atomic_add_return((a), (v)) < 0)
 #define atomic_dec_return(v) atomic_sub_return(1,(v))
 #define atomic_inc_return(v) atomic_add_return(1,(v))
 
+#define atomic_cmpxchg(v, o, n) ((int)cmpxchg(&((v)->counter), (o), (n)))
+#define atomic_xchg(v, new) (xchg(&((v)->counter), new))
+
+#define atomic_add_unless(v, a, u)				\
+({								\
+	int c, old;						\
+	c = atomic_read(v);					\
+	while (c != (u) && (old = atomic_cmpxchg((v), c, c + (a))) != c) \
+		c = old;					\
+	c != (u);						\
+})
+#define atomic_inc_not_zero(v) atomic_add_unless((v), 1, 0)
+
 /*
  * atomic_inc_and_test - increment and test
  * @v: pointer of type atomic_t
diff --git a/arch/blackfin/include/asm/bfin-global.h b/arch/blackfin/include/asm/bfin-global.h
index 77295666c34b..daffc0684e75 100644
--- a/arch/blackfin/include/asm/bfin-global.h
+++ b/arch/blackfin/include/asm/bfin-global.h
@@ -47,6 +47,9 @@
 # define DMA_UNCACHED_REGION (0)
 #endif
 
+extern void bfin_setup_caches(unsigned int cpu);
+extern void bfin_setup_cpudata(unsigned int cpu);
+
 extern unsigned long get_cclk(void);
 extern unsigned long get_sclk(void);
 extern unsigned long sclk_to_usecs(unsigned long sclk);
@@ -58,8 +61,6 @@ extern void dump_bfin_trace_buffer(void);
 
 /* init functions only */
 extern int init_arch_irq(void);
-extern void bfin_icache_init(void);
-extern void bfin_dcache_init(void);
 extern void init_exception_vectors(void);
 extern void program_IAR(void);
 
@@ -110,7 +111,7 @@ extern unsigned long memory_mtd_start, memory_mtd_end, mtd_size;
 
 #ifdef CONFIG_BFIN_ICACHE_LOCK
 extern void cache_grab_lock(int way);
-extern void cache_lock(int way);
+extern void bfin_cache_lock(int way);
 #endif
 
 #endif
diff --git a/arch/blackfin/include/asm/bfin5xx_spi.h b/arch/blackfin/include/asm/bfin5xx_spi.h
index 9fa19158e38d..1306e6b22946 100644
--- a/arch/blackfin/include/asm/bfin5xx_spi.h
+++ b/arch/blackfin/include/asm/bfin5xx_spi.h
@@ -1,22 +1,12 @@
-/************************************************************
-
-* Copyright (C) 2006-2008, Analog Devices. All Rights Reserved
-*
-* FILE bfin5xx_spi.h
-* PROGRAMMER(S): Luke Yang (Analog Devices Inc.)
-*
-*
-* DATE OF CREATION: March. 10th 2006
-*
-* SYNOPSIS:
-*
-* DESCRIPTION: header file for SPI controller driver for Blackfin5xx.
-**************************************************************
-
-* MODIFICATION HISTORY:
-* March 10, 2006  bfin5xx_spi.h Created. (Luke Yang)
-
-************************************************************/
+/*
+ * Blackfin On-Chip SPI Driver
+ *
+ * Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Licensed under the GPL-2 or later.
+ */
 
 #ifndef _SPI_CHANNEL_H_
 #define _SPI_CHANNEL_H_
diff --git a/arch/blackfin/include/asm/bfin_sdh.h b/arch/blackfin/include/asm/bfin_sdh.h
new file mode 100644
index 000000000000..d61d5497c590
--- /dev/null
+++ b/arch/blackfin/include/asm/bfin_sdh.h
@@ -0,0 +1,19 @@
+/*
+ * bfin_sdh.h - Blackfin SDH definitions
+ *
+ * Copyright 2008 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#ifndef __BFIN_SDH_H__
+#define __BFIN_SDH_H__
+
+struct bfin_sd_host {
+	int dma_chan;
+	int irq_int0;
+	int irq_int1;
+	u16 pin_req[7];
+};
+
+#endif
diff --git a/arch/blackfin/include/asm/bfin_sport.h b/arch/blackfin/include/asm/bfin_sport.h
index c76ed8def302..fe88a2c19213 100644
--- a/arch/blackfin/include/asm/bfin_sport.h
+++ b/arch/blackfin/include/asm/bfin_sport.h
@@ -120,9 +120,6 @@ struct sport_register {
 #define SPORT_IOC_MAGIC		'P'
 #define SPORT_IOC_CONFIG	_IOWR('P', 0x01, struct sport_config)
 
-/* Test purpose */
-#define ENABLE_AD73311		_IOWR('P', 0x02, int)
-
 struct sport_dev {
 	struct cdev cdev;	/* Char device structure */
 
diff --git a/arch/blackfin/include/asm/bfrom.h b/arch/blackfin/include/asm/bfrom.h
index cfe8024c3b2f..9e4be5e5e767 100644
--- a/arch/blackfin/include/asm/bfrom.h
+++ b/arch/blackfin/include/asm/bfrom.h
@@ -43,6 +43,11 @@ __attribute__((__noreturn__))
 static inline void bfrom_SoftReset(void *new_stack)
 {
 	while (1)
+		/*
+		 * We don't declare the SP as clobbered on purpose, since
+		 * it confuses the heck out of the compiler, and this function
+		 * never returns
+		 */
 		__asm__ __volatile__(
 			"sp = %[stack];"
 			"jump (%[bfrom_syscontrol]);"
diff --git a/arch/blackfin/include/asm/bitops.h b/arch/blackfin/include/asm/bitops.h
index c428e4106f89..21b036eadab1 100644
--- a/arch/blackfin/include/asm/bitops.h
+++ b/arch/blackfin/include/asm/bitops.h
@@ -7,7 +7,6 @@
 
 #include <linux/compiler.h>
 #include <asm/byteorder.h>	/* swab32 */
-#include <asm/system.h>		/* save_flags */
 
 #ifdef __KERNEL__
 
@@ -20,80 +19,107 @@
 #include <asm-generic/bitops/sched.h>
 #include <asm-generic/bitops/ffz.h>
 
-static __inline__ void set_bit(int nr, volatile unsigned long *addr)
+#ifdef CONFIG_SMP
+
+#include <linux/linkage.h>
+
+asmlinkage int __raw_bit_set_asm(volatile unsigned long *addr, int nr);
+
+asmlinkage int __raw_bit_clear_asm(volatile unsigned long *addr, int nr);
+
+asmlinkage int __raw_bit_toggle_asm(volatile unsigned long *addr, int nr);
+
+asmlinkage int __raw_bit_test_set_asm(volatile unsigned long *addr, int nr);
+
+asmlinkage int __raw_bit_test_clear_asm(volatile unsigned long *addr, int nr);
+
+asmlinkage int __raw_bit_test_toggle_asm(volatile unsigned long *addr, int nr);
+
+asmlinkage int __raw_bit_test_asm(const volatile unsigned long *addr, int nr);
+
+static inline void set_bit(int nr, volatile unsigned long *addr)
 {
-	int *a = (int *)addr;
-	int mask;
-	unsigned long flags;
+	volatile unsigned long *a = addr + (nr >> 5);
+	__raw_bit_set_asm(a, nr & 0x1f);
+}
 
-	a += nr >> 5;
-	mask = 1 << (nr & 0x1f);
-	local_irq_save(flags);
-	*a |= mask;
-	local_irq_restore(flags);
+static inline void clear_bit(int nr, volatile unsigned long *addr)
+{
+	volatile unsigned long *a = addr + (nr >> 5);
+	__raw_bit_clear_asm(a, nr & 0x1f);
 }
 
-static __inline__ void __set_bit(int nr, volatile unsigned long *addr)
+static inline void change_bit(int nr, volatile unsigned long *addr)
 {
-	int *a = (int *)addr;
-	int mask;
+	volatile unsigned long *a = addr + (nr >> 5);
+	__raw_bit_toggle_asm(a, nr & 0x1f);
+}
 
-	a += nr >> 5;
-	mask = 1 << (nr & 0x1f);
-	*a |= mask;
+static inline int test_bit(int nr, const volatile unsigned long *addr)
+{
+	volatile const unsigned long *a = addr + (nr >> 5);
+	return __raw_bit_test_asm(a, nr & 0x1f) != 0;
 }
 
-/*
- * clear_bit() doesn't provide any barrier for the compiler.
- */
-#define smp_mb__before_clear_bit()	barrier()
-#define smp_mb__after_clear_bit()	barrier()
+static inline int test_and_set_bit(int nr, volatile unsigned long *addr)
+{
+	volatile unsigned long *a = addr + (nr >> 5);
+	return __raw_bit_test_set_asm(a, nr & 0x1f);
+}
+
+static inline int test_and_clear_bit(int nr, volatile unsigned long *addr)
+{
+	volatile unsigned long *a = addr + (nr >> 5);
+	return __raw_bit_test_clear_asm(a, nr & 0x1f);
+}
+
+static inline int test_and_change_bit(int nr, volatile unsigned long *addr)
+{
+	volatile unsigned long *a = addr + (nr >> 5);
+	return __raw_bit_test_toggle_asm(a, nr & 0x1f);
+}
+
+#else /* !CONFIG_SMP */
+
+#include <asm/system.h>		/* save_flags */
 
-static __inline__ void clear_bit(int nr, volatile unsigned long *addr)
+static inline void set_bit(int nr, volatile unsigned long *addr)
 {
 	int *a = (int *)addr;
 	int mask;
 	unsigned long flags;
 	a += nr >> 5;
 	mask = 1 << (nr & 0x1f);
-	local_irq_save(flags);
-	*a &= ~mask;
-	local_irq_restore(flags);
+	local_irq_save_hw(flags);
+	*a |= mask;
+	local_irq_restore_hw(flags);
 }
 
-static __inline__ void __clear_bit(int nr, volatile unsigned long *addr)
+static inline void clear_bit(int nr, volatile unsigned long *addr)
 {
 	int *a = (int *)addr;
 	int mask;
-
+	unsigned long flags;
 	a += nr >> 5;
 	mask = 1 << (nr & 0x1f);
+	local_irq_save_hw(flags);
 	*a &= ~mask;
+	local_irq_restore_hw(flags);
 }
 
-static __inline__ void change_bit(int nr, volatile unsigned long *addr)
+static inline void change_bit(int nr, volatile unsigned long *addr)
 {
 	int mask, flags;
 	unsigned long *ADDR = (unsigned long *)addr;
 
 	ADDR += nr >> 5;
 	mask = 1 << (nr & 31);
-	local_irq_save(flags);
-	*ADDR ^= mask;
-	local_irq_restore(flags);
-}
-
-static __inline__ void __change_bit(int nr, volatile unsigned long *addr)
-{
-	int mask;
-	unsigned long *ADDR = (unsigned long *)addr;
-
-	ADDR += nr >> 5;
-	mask = 1 << (nr & 31);
+	local_irq_save_hw(flags);
 	*ADDR ^= mask;
+	local_irq_restore_hw(flags);
 }
 
-static __inline__ int test_and_set_bit(int nr, void *addr)
+static inline int test_and_set_bit(int nr, volatile unsigned long *addr)
 {
 	int mask, retval;
 	volatile unsigned int *a = (volatile unsigned int *)addr;
@@ -101,27 +127,31 @@ static __inline__ int test_and_set_bit(int nr, void *addr)
 
 	a += nr >> 5;
 	mask = 1 << (nr & 0x1f);
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	retval = (mask & *a) != 0;
 	*a |= mask;
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 	return retval;
 }
 
-static __inline__ int __test_and_set_bit(int nr, volatile unsigned long *addr)
+static inline int test_and_clear_bit(int nr, volatile unsigned long *addr)
 {
 	int mask, retval;
 	volatile unsigned int *a = (volatile unsigned int *)addr;
+	unsigned long flags;
 
 	a += nr >> 5;
 	mask = 1 << (nr & 0x1f);
+	local_irq_save_hw(flags);
 	retval = (mask & *a) != 0;
-	*a |= mask;
+	*a &= ~mask;
+	local_irq_restore_hw(flags);
+
 	return retval;
 }
 
-static __inline__ int test_and_clear_bit(int nr, volatile unsigned long *addr)
+static inline int test_and_change_bit(int nr, volatile unsigned long *addr)
 {
 	int mask, retval;
 	volatile unsigned int *a = (volatile unsigned int *)addr;
@@ -129,15 +159,52 @@ static __inline__ int test_and_clear_bit(int nr, volatile unsigned long *addr)
 
 	a += nr >> 5;
 	mask = 1 << (nr & 0x1f);
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	retval = (mask & *a) != 0;
+	*a ^= mask;
+	local_irq_restore_hw(flags);
+	return retval;
+}
+
+#endif /* CONFIG_SMP */
+
+/*
+ * clear_bit() doesn't provide any barrier for the compiler.
+ */
+#define smp_mb__before_clear_bit()	barrier()
+#define smp_mb__after_clear_bit()	barrier()
+
+static inline void __set_bit(int nr, volatile unsigned long *addr)
+{
+	int *a = (int *)addr;
+	int mask;
+
+	a += nr >> 5;
+	mask = 1 << (nr & 0x1f);
+	*a |= mask;
+}
+
+static inline void __clear_bit(int nr, volatile unsigned long *addr)
+{
+	int *a = (int *)addr;
+	int mask;
+
+	a += nr >> 5;
+	mask = 1 << (nr & 0x1f);
 	*a &= ~mask;
-	local_irq_restore(flags);
+}
 
-	return retval;
+static inline void __change_bit(int nr, volatile unsigned long *addr)
+{
+	int mask;
+	unsigned long *ADDR = (unsigned long *)addr;
+
+	ADDR += nr >> 5;
+	mask = 1 << (nr & 31);
+	*ADDR ^= mask;
 }
 
-static __inline__ int __test_and_clear_bit(int nr, volatile unsigned long *addr)
+static inline int __test_and_set_bit(int nr, volatile unsigned long *addr)
 {
 	int mask, retval;
 	volatile unsigned int *a = (volatile unsigned int *)addr;
@@ -145,26 +212,23 @@ static __inline__ int __test_and_clear_bit(int nr, volatile unsigned long *addr)
 	a += nr >> 5;
 	mask = 1 << (nr & 0x1f);
 	retval = (mask & *a) != 0;
-	*a &= ~mask;
+	*a |= mask;
 	return retval;
 }
 
-static __inline__ int test_and_change_bit(int nr, volatile unsigned long *addr)
+static inline int __test_and_clear_bit(int nr, volatile unsigned long *addr)
 {
 	int mask, retval;
 	volatile unsigned int *a = (volatile unsigned int *)addr;
-	unsigned long flags;
 
 	a += nr >> 5;
 	mask = 1 << (nr & 0x1f);
-	local_irq_save(flags);
 	retval = (mask & *a) != 0;
-	*a ^= mask;
-	local_irq_restore(flags);
+	*a &= ~mask;
 	return retval;
 }
 
-static __inline__ int __test_and_change_bit(int nr,
+static inline int __test_and_change_bit(int nr,
 					    volatile unsigned long *addr)
 {
 	int mask, retval;
@@ -177,16 +241,7 @@ static __inline__ int __test_and_change_bit(int nr,
 	return retval;
 }
 
-/*
- * This routine doesn't need to be atomic.
- */
-static __inline__ int __constant_test_bit(int nr, const void *addr)
-{
-	return ((1UL << (nr & 31)) &
-		(((const volatile unsigned int *)addr)[nr >> 5])) != 0;
-}
-
-static __inline__ int __test_bit(int nr, const void *addr)
+static inline int __test_bit(int nr, const void *addr)
 {
 	int *a = (int *)addr;
 	int mask;
@@ -196,10 +251,16 @@ static __inline__ int __test_bit(int nr, const void *addr)
 	return ((mask & *a) != 0);
 }
 
-#define test_bit(nr,addr) \
-(__builtin_constant_p(nr) ? \
- __constant_test_bit((nr),(addr)) : \
- __test_bit((nr),(addr)))
+#ifndef CONFIG_SMP
+/*
+ * This routine doesn't need irq save and restore ops in UP
+ * context.
+ */
+static inline int test_bit(int nr, const void *addr)
+{
+	return __test_bit(nr, addr);
+}
+#endif
 
 #include <asm-generic/bitops/find.h>
 #include <asm-generic/bitops/hweight.h>
diff --git a/arch/blackfin/include/asm/blackfin.h b/arch/blackfin/include/asm/blackfin.h
index 8749b0e321ab..8bb2cb139756 100644
--- a/arch/blackfin/include/asm/blackfin.h
+++ b/arch/blackfin/include/asm/blackfin.h
@@ -6,11 +6,6 @@
 #ifndef _BLACKFIN_H_
 #define _BLACKFIN_H_
 
-#define LO(con32) ((con32) & 0xFFFF)
-#define lo(con32) ((con32) & 0xFFFF)
-#define HI(con32) (((con32) >> 16) & 0xFFFF)
-#define hi(con32) (((con32) >> 16) & 0xFFFF)
-
 #include <mach/anomaly.h>
 
 #ifndef __ASSEMBLY__
@@ -65,6 +60,11 @@ static inline void CSYNC(void)
 
 #else  /* __ASSEMBLY__ */
 
+#define LO(con32) ((con32) & 0xFFFF)
+#define lo(con32) ((con32) & 0xFFFF)
+#define HI(con32) (((con32) >> 16) & 0xFFFF)
+#define hi(con32) (((con32) >> 16) & 0xFFFF)
+
 /* SSYNC & CSYNC implementations for assembly files */
 
 #define ssync(x) SSYNC(x)
diff --git a/arch/blackfin/include/asm/byteorder.h b/arch/blackfin/include/asm/byteorder.h
index 6a673d42da18..b9e797a497b4 100644
--- a/arch/blackfin/include/asm/byteorder.h
+++ b/arch/blackfin/include/asm/byteorder.h
@@ -1,48 +1,7 @@
 #ifndef _BLACKFIN_BYTEORDER_H
 #define _BLACKFIN_BYTEORDER_H
 
-#include <asm/types.h>
-#include <linux/compiler.h>
-
-#ifdef __GNUC__
-
-static __inline__ __attribute_const__ __u32 ___arch__swahb32(__u32 xx)
-{
-	__u32 tmp;
-	__asm__("%1 = %0 >> 8 (V);\n\t"
-		"%0 = %0 << 8 (V);\n\t"
-		"%0 = %0 | %1;\n\t"
-		: "+d"(xx), "=&d"(tmp));
-	return xx;
-}
-
-static __inline__ __attribute_const__ __u32 ___arch__swahw32(__u32 xx)
-{
-	__u32 rv;
-	__asm__("%0 = PACK(%1.L, %1.H);\n\t": "=d"(rv): "d"(xx));
-	return rv;
-}
-
-#define __arch__swahb32(x) ___arch__swahb32(x)
-#define __arch__swahw32(x) ___arch__swahw32(x)
-#define __arch__swab32(x) ___arch__swahb32(___arch__swahw32(x))
-
-static __inline__ __attribute_const__ __u16 ___arch__swab16(__u16 xx)
-{
-	__u32 xw = xx;
-	__asm__("%0 <<= 8;\n	%0.L = %0.L + %0.H (NS);\n": "+d"(xw));
-	return (__u16)xw;
-}
-
-#define __arch__swab16(x) ___arch__swab16(x)
-
-#endif
-
-#if defined(__GNUC__) && !defined(__STRICT_ANSI__) || defined(__KERNEL__)
-#  define __BYTEORDER_HAS_U64__
-#  define __SWAB_64_THRU_32__
-#endif
-
+#include <asm/swab.h>
 #include <linux/byteorder/little_endian.h>
 
 #endif				/* _BLACKFIN_BYTEORDER_H */
diff --git a/arch/blackfin/include/asm/cache.h b/arch/blackfin/include/asm/cache.h
index 023d72133b5a..86637814cf25 100644
--- a/arch/blackfin/include/asm/cache.h
+++ b/arch/blackfin/include/asm/cache.h
@@ -12,6 +12,11 @@
 #define L1_CACHE_BYTES	(1 << L1_CACHE_SHIFT)
 #define SMP_CACHE_BYTES	L1_CACHE_BYTES
 
+#ifdef CONFIG_SMP
+#define __cacheline_aligned
+#else
+#define ____cacheline_aligned
+
 /*
  * Put cacheline_aliged data to L1 data memory
  */
@@ -21,9 +26,33 @@
 		__section__(".data_l1.cacheline_aligned")))
 #endif
 
+#endif
+
 /*
  * largest L1 which this arch supports
  */
 #define L1_CACHE_SHIFT_MAX	5
 
+#if defined(CONFIG_SMP) && \
+    !defined(CONFIG_BFIN_CACHE_COHERENT) && \
+    defined(CONFIG_BFIN_DCACHE)
+#define __ARCH_SYNC_CORE_DCACHE
+#ifndef __ASSEMBLY__
+asmlinkage void __raw_smp_mark_barrier_asm(void);
+asmlinkage void __raw_smp_check_barrier_asm(void);
+
+static inline void smp_mark_barrier(void)
+{
+	__raw_smp_mark_barrier_asm();
+}
+static inline void smp_check_barrier(void)
+{
+	__raw_smp_check_barrier_asm();
+}
+
+void resync_core_dcache(void);
+#endif
+#endif
+
+
 #endif
diff --git a/arch/blackfin/include/asm/cacheflush.h b/arch/blackfin/include/asm/cacheflush.h
index 4403415583fa..1b040f5b4feb 100644
--- a/arch/blackfin/include/asm/cacheflush.h
+++ b/arch/blackfin/include/asm/cacheflush.h
@@ -35,6 +35,7 @@ extern void blackfin_icache_flush_range(unsigned long start_address, unsigned lo
 extern void blackfin_dcache_flush_range(unsigned long start_address, unsigned long end_address);
 extern void blackfin_dcache_invalidate_range(unsigned long start_address, unsigned long end_address);
 extern void blackfin_dflush_page(void *page);
+extern void blackfin_invalidate_entire_dcache(void);
 
 #define flush_dcache_mmap_lock(mapping)		do { } while (0)
 #define flush_dcache_mmap_unlock(mapping)	do { } while (0)
@@ -44,12 +45,20 @@ extern void blackfin_dflush_page(void *page);
 #define flush_cache_vmap(start, end)		do { } while (0)
 #define flush_cache_vunmap(start, end)		do { } while (0)
 
+#ifdef CONFIG_SMP
+#define flush_icache_range_others(start, end)	\
+	smp_icache_flush_range_others((start), (end))
+#else
+#define flush_icache_range_others(start, end)	do { } while (0)
+#endif
+
 static inline void flush_icache_range(unsigned start, unsigned end)
 {
 #if defined(CONFIG_BFIN_DCACHE) && defined(CONFIG_BFIN_ICACHE)
 
 # if defined(CONFIG_BFIN_WT)
 	blackfin_icache_flush_range((start), (end));
+	flush_icache_range_others(start, end);
 # else
 	blackfin_icache_dcache_flush_range((start), (end));
 # endif
@@ -58,6 +67,7 @@ static inline void flush_icache_range(unsigned start, unsigned end)
 
 # if defined(CONFIG_BFIN_ICACHE)
 	blackfin_icache_flush_range((start), (end));
+	flush_icache_range_others(start, end);
 # endif
 # if defined(CONFIG_BFIN_DCACHE)
 	blackfin_dcache_flush_range((start), (end));
@@ -66,10 +76,12 @@ static inline void flush_icache_range(unsigned start, unsigned end)
 #endif
 }
 
-#define copy_to_user_page(vma, page, vaddr, dst, src, len) \
-do { memcpy(dst, src, len); \
-     flush_icache_range ((unsigned) (dst), (unsigned) (dst) + (len)); \
+#define copy_to_user_page(vma, page, vaddr, dst, src, len)		\
+do { memcpy(dst, src, len);						\
+     flush_icache_range((unsigned) (dst), (unsigned) (dst) + (len));	\
+     flush_icache_range_others((unsigned long) (dst), (unsigned long) (dst) + (len));\
 } while (0)
+
 #define copy_from_user_page(vma, page, vaddr, dst, src, len)	memcpy(dst, src, len)
 
 #if defined(CONFIG_BFIN_DCACHE)
@@ -82,7 +94,7 @@ do { memcpy(dst, src, len); \
 # define flush_dcache_page(page)			blackfin_dflush_page(page_address(page))
 #else
 # define flush_dcache_range(start,end)		do { } while (0)
-# define flush_dcache_page(page)			do { } while (0)
+# define flush_dcache_page(page)		do { } while (0)
 #endif
 
 extern unsigned long reserved_mem_dcache_on;
diff --git a/arch/blackfin/include/asm/checksum.h b/arch/blackfin/include/asm/checksum.h
index 6f6af2b8e9e0..f67289a0d8d2 100644
--- a/arch/blackfin/include/asm/checksum.h
+++ b/arch/blackfin/include/asm/checksum.h
@@ -78,7 +78,8 @@ csum_tcpudp_nofold(__be32 saddr, __be32 daddr, unsigned short len,
                  "%0 = %0 + %4;\n\t"
                  "NOP;\n\t"
  		 : "=d" (sum)
-		 : "d" (daddr), "d" (saddr), "d" ((ntohs(len)<<16)+proto*256), "d" (1), "0"(sum));
+		 : "d" (daddr), "d" (saddr), "d" ((ntohs(len)<<16)+proto*256), "d" (1), "0"(sum)
+		 : "CC");
 
 	return (sum);
 }
diff --git a/arch/blackfin/include/asm/context.S b/arch/blackfin/include/asm/context.S
index c0e630edfb9a..16561ab18b38 100644
--- a/arch/blackfin/include/asm/context.S
+++ b/arch/blackfin/include/asm/context.S
@@ -303,9 +303,14 @@
 	RETI = [sp++];
 	RETS = [sp++];
 
-	p0.h = _irq_flags;
-	p0.l = _irq_flags;
+#ifdef CONFIG_SMP
+	GET_PDA(p0, r0);
+	r0 = [p0 + PDA_IRQFLAGS];
+#else
+	p0.h = _bfin_irq_flags;
+	p0.l = _bfin_irq_flags;
 	r0 = [p0];
+#endif
 	sti r0;
 
 	sp += 4;	/* Skip Reserved */
@@ -353,3 +358,41 @@
 	csync;
 .endm
 
+.macro save_context_cplb
+	[--sp] = (R7:0, P5:0);
+	[--sp] = fp;
+
+	[--sp] = a0.x;
+	[--sp] = a0.w;
+	[--sp] = a1.x;
+	[--sp] = a1.w;
+
+	[--sp] = LC0;
+	[--sp] = LC1;
+	[--sp] = LT0;
+	[--sp] = LT1;
+	[--sp] = LB0;
+	[--sp] = LB1;
+
+	[--sp] = RETS;
+.endm
+
+.macro restore_context_cplb
+	RETS = [sp++];
+
+	LB1 = [sp++];
+	LB0 = [sp++];
+	LT1 = [sp++];
+	LT0 = [sp++];
+	LC1 = [sp++];
+	LC0 = [sp++];
+
+	a1.w = [sp++];
+	a1.x = [sp++];
+	a0.w = [sp++];
+	a0.x = [sp++];
+
+	fp = [sp++];
+
+	(R7:0, P5:0) = [SP++];
+.endm
diff --git a/arch/blackfin/include/asm/cplb-mpu.h b/arch/blackfin/include/asm/cplb-mpu.h
deleted file mode 100644
index 75c67b99d607..000000000000
--- a/arch/blackfin/include/asm/cplb-mpu.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- * File:         include/asm-blackfin/cplbinit.h
- * Based on:
- * Author:
- *
- * Created:
- * Description:
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-#ifndef __ASM_BFIN_CPLB_MPU_H
-#define __ASM_BFIN_CPLB_MPU_H
-
-struct cplb_entry {
-	unsigned long data, addr;
-};
-
-struct mem_region {
-	unsigned long start, end;
-	unsigned long dcplb_data;
-	unsigned long icplb_data;
-};
-
-extern struct cplb_entry dcplb_tbl[MAX_CPLBS];
-extern struct cplb_entry icplb_tbl[MAX_CPLBS];
-extern int first_switched_icplb;
-extern int first_mask_dcplb;
-extern int first_switched_dcplb;
-
-extern int nr_dcplb_miss, nr_icplb_miss, nr_icplb_supv_miss, nr_dcplb_prot;
-extern int nr_cplb_flush;
-
-extern int page_mask_order;
-extern int page_mask_nelts;
-
-extern unsigned long *current_rwx_mask;
-
-extern void flush_switched_cplbs(void);
-extern void set_mask_dcplbs(unsigned long *);
-
-extern void __noreturn panic_cplb_error(int seqstat, struct pt_regs *);
-
-#endif /* __ASM_BFIN_CPLB_MPU_H */
diff --git a/arch/blackfin/include/asm/cplb.h b/arch/blackfin/include/asm/cplb.h
index 9e8b4035fcec..ad566ff9ad16 100644
--- a/arch/blackfin/include/asm/cplb.h
+++ b/arch/blackfin/include/asm/cplb.h
@@ -30,7 +30,6 @@
 #ifndef _CPLB_H
 #define _CPLB_H
 
-#include <asm/blackfin.h>
 #include <mach/anomaly.h>
 
 #define SDRAM_IGENERIC    (CPLB_L1_CHBL | CPLB_USER_RD | CPLB_VALID | CPLB_PORTPRIO)
@@ -55,13 +54,24 @@
 #endif
 
 #define L1_DMEMORY       (CPLB_LOCK | CPLB_COMMON)
+
+#ifdef CONFIG_SMP
+#define L2_ATTR           (INITIAL_T | I_CPLB | D_CPLB)
+#define L2_IMEMORY         (CPLB_COMMON | CPLB_LOCK)
+#define L2_DMEMORY         (CPLB_COMMON | CPLB_LOCK)
+
+#else
 #ifdef CONFIG_BFIN_L2_CACHEABLE
 #define L2_IMEMORY        (SDRAM_IGENERIC)
 #define L2_DMEMORY        (SDRAM_DGENERIC)
 #else
 #define L2_IMEMORY        (CPLB_COMMON)
 #define L2_DMEMORY        (CPLB_COMMON)
-#endif
+#endif /* CONFIG_BFIN_L2_CACHEABLE */
+
+#define L2_ATTR           (INITIAL_T | SWITCH_T | I_CPLB | D_CPLB)
+#endif /* CONFIG_SMP */
+
 #define SDRAM_DNON_CHBL  (CPLB_COMMON)
 #define SDRAM_EBIU       (CPLB_COMMON)
 #define SDRAM_OOPS       (CPLB_VALID | ANOMALY_05000158_WORKAROUND | CPLB_LOCK | CPLB_DIRTY)
@@ -71,14 +81,7 @@
 #define SIZE_1M 0x00100000      /* 1M */
 #define SIZE_4M 0x00400000      /* 4M */
 
-#ifdef CONFIG_MPU
 #define MAX_CPLBS 16
-#else
-#define MAX_CPLBS (16 * 2)
-#endif
-
-#define ASYNC_MEMORY_CPLB_COVERAGE	((ASYNC_BANK0_SIZE + ASYNC_BANK1_SIZE + \
-				 ASYNC_BANK2_SIZE + ASYNC_BANK3_SIZE) / SIZE_4M)
 
 #define CPLB_ENABLE_ICACHE_P	0
 #define CPLB_ENABLE_DCACHE_P	1
@@ -113,4 +116,8 @@
 #define CPLB_INOCACHE   	CPLB_USER_RD | CPLB_VALID
 #define CPLB_IDOCACHE   	CPLB_INOCACHE | CPLB_L1_CHBL
 
+#define FAULT_RW        (1 << 16)
+#define FAULT_USERSUPV  (1 << 17)
+#define FAULT_CPLBBITS  0x0000ffff
+
 #endif				/* _CPLB_H */
diff --git a/arch/blackfin/include/asm/cplbinit.h b/arch/blackfin/include/asm/cplbinit.h
index f845b41147ba..05b14a631d0c 100644
--- a/arch/blackfin/include/asm/cplbinit.h
+++ b/arch/blackfin/include/asm/cplbinit.h
@@ -32,61 +32,56 @@
 
 #include <asm/blackfin.h>
 #include <asm/cplb.h>
+#include <linux/threads.h>
 
-#ifdef CONFIG_MPU
-
-#include <asm/cplb-mpu.h>
-
+#ifdef CONFIG_CPLB_SWITCH_TAB_L1
+# define PDT_ATTR __attribute__((l1_data))
 #else
+# define PDT_ATTR
+#endif
 
-#define INITIAL_T 0x1
-#define SWITCH_T  0x2
-#define I_CPLB    0x4
-#define D_CPLB    0x8
-
-#define IN_KERNEL 1
-
-enum
-{ZERO_P, L1I_MEM, L1D_MEM, SDRAM_KERN , SDRAM_RAM_MTD, SDRAM_DMAZ, RES_MEM, ASYNC_MEM, L2_MEM};
-
-struct cplb_desc {
-	u32 start; /* start address */
-	u32 end; /* end address */
-	u32 psize; /* prefered size if any otherwise 1MB or 4MB*/
-	u16 attr;/* attributes */
-	u16 i_conf;/* I-CPLB DATA */
-	u16 d_conf;/* D-CPLB DATA */
-	u16 valid;/* valid */
-	const s8 name[30];/* name */
+struct cplb_entry {
+	unsigned long data, addr;
 };
 
-struct cplb_tab {
-  u_long *tab;
-	u16 pos;
-	u16 size;
+struct cplb_boundary {
+	unsigned long eaddr; /* End of this region.  */
+	unsigned long data; /* CPLB data value.  */
 };
 
-extern u_long icplb_table[];
-extern u_long dcplb_table[];
+extern struct cplb_boundary dcplb_bounds[];
+extern struct cplb_boundary icplb_bounds[];
+extern int dcplb_nr_bounds, icplb_nr_bounds;
 
-/* Till here we are discussing about the static memory management model.
- * However, the operating envoronments commonly define more CPLB
- * descriptors to cover the entire addressable memory than will fit into
- * the available on-chip 16 CPLB MMRs. When this happens, the below table
- * will be used which will hold all the potentially required CPLB descriptors
- *
- * This is how Page descriptor Table is implemented in uClinux/Blackfin.
- */
+extern struct cplb_entry dcplb_tbl[NR_CPUS][MAX_CPLBS];
+extern struct cplb_entry icplb_tbl[NR_CPUS][MAX_CPLBS];
+extern int first_switched_icplb;
+extern int first_switched_dcplb;
 
-extern u_long ipdt_table[];
-extern u_long dpdt_table[];
-#ifdef CONFIG_CPLB_INFO
-extern u_long ipdt_swapcount_table[];
-extern u_long dpdt_swapcount_table[];
-#endif
+extern int nr_dcplb_miss[], nr_icplb_miss[], nr_icplb_supv_miss[];
+extern int nr_dcplb_prot[], nr_cplb_flush[];
+
+#ifdef CONFIG_MPU
+
+extern int first_mask_dcplb;
+
+extern int page_mask_order;
+extern int page_mask_nelts;
+
+extern unsigned long *current_rwx_mask[NR_CPUS];
+
+extern void flush_switched_cplbs(unsigned int);
+extern void set_mask_dcplbs(unsigned long *, unsigned int);
+
+extern void __noreturn panic_cplb_error(int seqstat, struct pt_regs *);
 
 #endif /* CONFIG_MPU */
 
-extern void generate_cplb_tables(void);
+extern void bfin_icache_init(struct cplb_entry *icplb_tbl);
+extern void bfin_dcache_init(struct cplb_entry *icplb_tbl);
 
+#if defined(CONFIG_BFIN_DCACHE) || defined(CONFIG_BFIN_ICACHE)
+extern void generate_cplb_tables_all(void);
+extern void generate_cplb_tables_cpu(unsigned int cpu);
+#endif
 #endif
diff --git a/arch/blackfin/include/asm/cpu.h b/arch/blackfin/include/asm/cpu.h
new file mode 100644
index 000000000000..c2594ef877f6
--- /dev/null
+++ b/arch/blackfin/include/asm/cpu.h
@@ -0,0 +1,41 @@
+/*
+ * File:         arch/blackfin/include/asm/cpu.h.
+ * Author:       Philippe Gerum <rpm@xenomai.org>
+ *
+ *               Copyright 2007 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 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 the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#ifndef __ASM_BLACKFIN_CPU_H
+#define __ASM_BLACKFIN_CPU_H
+
+#include <linux/percpu.h>
+
+struct task_struct;
+
+struct blackfin_cpudata {
+	struct cpu cpu;
+	struct task_struct *idle;
+	unsigned int imemctl;
+	unsigned int dmemctl;
+	unsigned long loops_per_jiffy;
+	unsigned long dcache_invld_count;
+};
+
+DECLARE_PER_CPU(struct blackfin_cpudata, cpu_data);
+
+#endif
diff --git a/arch/blackfin/include/asm/dma.h b/arch/blackfin/include/asm/dma.h
index 6509733bb0f6..e4f7b8043f02 100644
--- a/arch/blackfin/include/asm/dma.h
+++ b/arch/blackfin/include/asm/dma.h
@@ -1,44 +1,17 @@
 /*
- * File:         include/asm-blackfin/simple_bf533_dma.h
- * Based on:     none - original work
- * Author:       LG Soft India
- *               Copyright (C) 2004-2005 Analog Devices Inc.
- * Created:      Tue Sep 21 2004
- * Description:  This file contains the major Data structures and constants
- * 		 used for DMA Implementation in BF533
- * Modified:
+ * dma.h - Blackfin DMA defines/structures/etc...
  *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, 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; see the file COPYING.
- * If not, write to the Free Software Foundation,
- * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ * Copyright 2004-2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
  */
 
 #ifndef _BLACKFIN_DMA_H_
 #define _BLACKFIN_DMA_H_
 
-#include <asm/io.h>
-#include <linux/slab.h>
-#include <asm/irq.h>
-#include <asm/signal.h>
-
-#include <linux/kernel.h>
-#include <mach/dma.h>
-#include <linux/mm.h>
 #include <linux/interrupt.h>
+#include <mach/dma.h>
 #include <asm/blackfin.h>
+#include <asm/page.h>
 
 #define MAX_DMA_ADDRESS PAGE_OFFSET
 
@@ -79,7 +52,7 @@ enum dma_chan_status {
 #define DMA_SYNC_RESTART	1
 
 struct dmasg {
-	unsigned long next_desc_addr;
+	void *next_desc_addr;
 	unsigned long start_addr;
 	unsigned short cfg;
 	unsigned short x_count;
@@ -89,7 +62,7 @@ struct dmasg {
 } __attribute__((packed));
 
 struct dma_register {
-	unsigned long next_desc_ptr;	/* DMA Next Descriptor Pointer register */
+	void *next_desc_ptr;	/* DMA Next Descriptor Pointer register */
 	unsigned long start_addr;	/* DMA Start address  register */
 
 	unsigned short cfg;	/* DMA Configuration register */
@@ -109,7 +82,7 @@ struct dma_register {
 	short y_modify;	/* DMA y_modify register */
 	unsigned short dummy5;
 
-	unsigned long curr_desc_ptr;	/* DMA Current Descriptor Pointer
+	void *curr_desc_ptr;	/* DMA Current Descriptor Pointer
 					   register */
 	unsigned long curr_addr_ptr;	/* DMA Current Address Pointer
 						   register */
@@ -131,19 +104,15 @@ struct dma_register {
 
 };
 
-typedef irqreturn_t(*dma_interrupt_t) (int irq, void *dev_id);
-
+struct mutex;
 struct dma_channel {
 	struct mutex dmalock;
-	char *device_id;
+	const char *device_id;
 	enum dma_chan_status chan_status;
-	struct dma_register *regs;
+	volatile struct dma_register *regs;
 	struct dmasg *sg;		/* large mode descriptor */
-	unsigned int ctrl_num;	/* controller number */
-	dma_interrupt_t irq_callback;
+	unsigned int irq;
 	void *data;
-	unsigned int dma_enable_flag;
-	unsigned int loopback_flag;
 #ifdef CONFIG_PM
 	unsigned short saved_peripheral_map;
 #endif
@@ -157,49 +126,132 @@ void blackfin_dma_resume(void);
 /*******************************************************************************
 *	DMA API's
 *******************************************************************************/
-/* functions to set register mode */
-void set_dma_start_addr(unsigned int channel, unsigned long addr);
-void set_dma_next_desc_addr(unsigned int channel, unsigned long addr);
-void set_dma_curr_desc_addr(unsigned int channel, unsigned long addr);
-void set_dma_x_count(unsigned int channel, unsigned short x_count);
-void set_dma_x_modify(unsigned int channel, short x_modify);
-void set_dma_y_count(unsigned int channel, unsigned short y_count);
-void set_dma_y_modify(unsigned int channel, short y_modify);
-void set_dma_config(unsigned int channel, unsigned short config);
-unsigned short set_bfin_dma_config(char direction, char flow_mode,
-				   char intr_mode, char dma_mode, char width,
-				   char syncmode);
-void set_dma_curr_addr(unsigned int channel, unsigned long addr);
-
-/* get curr status for polling */
-unsigned short get_dma_curr_irqstat(unsigned int channel);
-unsigned short get_dma_curr_xcount(unsigned int channel);
-unsigned short get_dma_curr_ycount(unsigned int channel);
-unsigned long get_dma_next_desc_ptr(unsigned int channel);
-unsigned long get_dma_curr_desc_ptr(unsigned int channel);
-unsigned long get_dma_curr_addr(unsigned int channel);
-
-/* set large DMA mode descriptor */
-void set_dma_sg(unsigned int channel, struct dmasg *sg, int nr_sg);
-
-/* check if current channel is in use */
-int dma_channel_active(unsigned int channel);
-
-/* common functions must be called in any mode */
+extern struct dma_channel dma_ch[MAX_DMA_CHANNELS];
+extern struct dma_register *dma_io_base_addr[MAX_DMA_CHANNELS];
+extern int channel2irq(unsigned int channel);
+
+static inline void set_dma_start_addr(unsigned int channel, unsigned long addr)
+{
+	dma_ch[channel].regs->start_addr = addr;
+}
+static inline void set_dma_next_desc_addr(unsigned int channel, void *addr)
+{
+	dma_ch[channel].regs->next_desc_ptr = addr;
+}
+static inline void set_dma_curr_desc_addr(unsigned int channel, void *addr)
+{
+	dma_ch[channel].regs->curr_desc_ptr = addr;
+}
+static inline void set_dma_x_count(unsigned int channel, unsigned short x_count)
+{
+	dma_ch[channel].regs->x_count = x_count;
+}
+static inline void set_dma_y_count(unsigned int channel, unsigned short y_count)
+{
+	dma_ch[channel].regs->y_count = y_count;
+}
+static inline void set_dma_x_modify(unsigned int channel, short x_modify)
+{
+	dma_ch[channel].regs->x_modify = x_modify;
+}
+static inline void set_dma_y_modify(unsigned int channel, short y_modify)
+{
+	dma_ch[channel].regs->y_modify = y_modify;
+}
+static inline void set_dma_config(unsigned int channel, unsigned short config)
+{
+	dma_ch[channel].regs->cfg = config;
+}
+static inline void set_dma_curr_addr(unsigned int channel, unsigned long addr)
+{
+	dma_ch[channel].regs->curr_addr_ptr = addr;
+}
+
+static inline unsigned short
+set_bfin_dma_config(char direction, char flow_mode,
+		    char intr_mode, char dma_mode, char width, char syncmode)
+{
+	return (direction << 1) | (width << 2) | (dma_mode << 4) |
+		(intr_mode << 6) | (flow_mode << 12) | (syncmode << 5);
+}
+
+static inline unsigned short get_dma_curr_irqstat(unsigned int channel)
+{
+	return dma_ch[channel].regs->irq_status;
+}
+static inline unsigned short get_dma_curr_xcount(unsigned int channel)
+{
+	return dma_ch[channel].regs->curr_x_count;
+}
+static inline unsigned short get_dma_curr_ycount(unsigned int channel)
+{
+	return dma_ch[channel].regs->curr_y_count;
+}
+static inline void *get_dma_next_desc_ptr(unsigned int channel)
+{
+	return dma_ch[channel].regs->next_desc_ptr;
+}
+static inline void *get_dma_curr_desc_ptr(unsigned int channel)
+{
+	return dma_ch[channel].regs->curr_desc_ptr;
+}
+static inline unsigned short get_dma_config(unsigned int channel)
+{
+	return dma_ch[channel].regs->cfg;
+}
+static inline unsigned long get_dma_curr_addr(unsigned int channel)
+{
+	return dma_ch[channel].regs->curr_addr_ptr;
+}
+
+static inline void set_dma_sg(unsigned int channel, struct dmasg *sg, int ndsize)
+{
+	dma_ch[channel].regs->cfg =
+		(dma_ch[channel].regs->cfg & ~(0xf << 8)) |
+		((ndsize & 0xf) << 8);
+	dma_ch[channel].regs->next_desc_ptr = sg;
+}
+
+static inline int dma_channel_active(unsigned int channel)
+{
+	if (dma_ch[channel].chan_status == DMA_CHANNEL_FREE)
+		return 0;
+	else
+		return 1;
+}
+
+static inline void disable_dma(unsigned int channel)
+{
+	dma_ch[channel].regs->cfg &= ~DMAEN;
+	SSYNC();
+	dma_ch[channel].chan_status = DMA_CHANNEL_REQUESTED;
+}
+static inline void enable_dma(unsigned int channel)
+{
+	dma_ch[channel].regs->curr_x_count = 0;
+	dma_ch[channel].regs->curr_y_count = 0;
+	dma_ch[channel].regs->cfg |= DMAEN;
+	dma_ch[channel].chan_status = DMA_CHANNEL_ENABLED;
+}
 void free_dma(unsigned int channel);
-int dma_channel_active(unsigned int channel); /* check if a channel is in use */
-void disable_dma(unsigned int channel);
-void enable_dma(unsigned int channel);
-int request_dma(unsigned int channel, char *device_id);
-int set_dma_callback(unsigned int channel, dma_interrupt_t callback,
-		     void *data);
-void dma_disable_irq(unsigned int channel);
-void dma_enable_irq(unsigned int channel);
-void clear_dma_irqstat(unsigned int channel);
+int request_dma(unsigned int channel, const char *device_id);
+int set_dma_callback(unsigned int channel, irq_handler_t callback, void *data);
+
+static inline void dma_disable_irq(unsigned int channel)
+{
+	disable_irq(dma_ch[channel].irq);
+}
+static inline void dma_enable_irq(unsigned int channel)
+{
+	enable_irq(dma_ch[channel].irq);
+}
+static inline void clear_dma_irqstat(unsigned int channel)
+{
+	dma_ch[channel].regs->irq_status = DMA_DONE | DMA_ERR;
+}
+
 void *dma_memcpy(void *dest, const void *src, size_t count);
 void *safe_dma_memcpy(void *dest, const void *src, size_t count);
-
-extern int channel2irq(unsigned int channel);
-extern struct dma_register *dma_io_base_addr[MAX_BLACKFIN_DMA_CHANNEL];
+void blackfin_dma_early_init(void);
 
 #endif
diff --git a/arch/blackfin/include/asm/entry.h b/arch/blackfin/include/asm/entry.h
index c4f721e0d00d..b30a2968e274 100644
--- a/arch/blackfin/include/asm/entry.h
+++ b/arch/blackfin/include/asm/entry.h
@@ -27,6 +27,14 @@
 #define SAVE_ALL_SYS		save_context_no_interrupts
 /* This is used for all normal interrupts.  It saves a minimum of registers
    to the stack, loads the IRQ number, and jumps to common code.  */
+#ifdef CONFIG_IPIPE
+# define LOAD_IPIPE_IPEND \
+	P0.l = lo(IPEND); \
+	P0.h = hi(IPEND); \
+	R1 = [P0];
+#else
+# define LOAD_IPIPE_IPEND
+#endif
 #define INTERRUPT_ENTRY(N)						\
     [--sp] = SYSCFG;							\
 									\
@@ -34,6 +42,7 @@
     [--sp] = R0;	/*orig_r0*/					\
     [--sp] = (R7:0,P5:0);						\
     R0 = (N);								\
+    LOAD_IPIPE_IPEND							\
     jump __common_int_entry;
 
 /* For timer interrupts, we need to save IPEND, since the user_mode
@@ -53,9 +62,11 @@
 /* This one pushes RETI without using CLI.  Interrupts are enabled.  */
 #define SAVE_CONTEXT_SYSCALL	save_context_syscall
 #define SAVE_CONTEXT		save_context_with_interrupts
+#define SAVE_CONTEXT_CPLB	save_context_cplb
 
 #define RESTORE_ALL_SYS		restore_context_no_interrupts
 #define RESTORE_CONTEXT		restore_context_with_interrupts
+#define RESTORE_CONTEXT_CPLB	restore_context_cplb
 
 #endif				/* __ASSEMBLY__ */
 #endif				/* __BFIN_ENTRY_H */
diff --git a/arch/blackfin/include/asm/gpio.h b/arch/blackfin/include/asm/gpio.h
index ad33ac271fd9..9477d82fcad2 100644
--- a/arch/blackfin/include/asm/gpio.h
+++ b/arch/blackfin/include/asm/gpio.h
@@ -84,11 +84,14 @@
 #ifndef __ARCH_BLACKFIN_GPIO_H__
 #define __ARCH_BLACKFIN_GPIO_H__
 
-#define gpio_bank(x) ((x) >> 4)
-#define gpio_bit(x)  (1<<((x) & 0xF))
-#define gpio_sub_n(x) ((x) & 0xF)
+#define gpio_bank(x) 	((x) >> 4)
+#define gpio_bit(x)  	(1<<((x) & 0xF))
+#define gpio_sub_n(x) 	((x) & 0xF)
 
-#define GPIO_BANKSIZE 16
+#define GPIO_BANKSIZE 	16
+#define GPIO_BANK_NUM 	DIV_ROUND_UP(MAX_BLACKFIN_GPIOS, GPIO_BANKSIZE)
+
+#include <mach/gpio.h>
 
 #define	GPIO_0	0
 #define	GPIO_1	1
@@ -139,151 +142,9 @@
 #define	GPIO_46	46
 #define	GPIO_47	47
 
-
 #define PERIPHERAL_USAGE 1
 #define GPIO_USAGE 0
 
-#ifdef BF533_FAMILY
-#define MAX_BLACKFIN_GPIOS 16
-
-#define	GPIO_PF0	0
-#define	GPIO_PF1	1
-#define	GPIO_PF2	2
-#define	GPIO_PF3	3
-#define	GPIO_PF4	4
-#define	GPIO_PF5	5
-#define	GPIO_PF6	6
-#define	GPIO_PF7	7
-#define	GPIO_PF8	8
-#define	GPIO_PF9	9
-#define	GPIO_PF10	10
-#define	GPIO_PF11	11
-#define	GPIO_PF12	12
-#define	GPIO_PF13	13
-#define	GPIO_PF14	14
-#define	GPIO_PF15	15
-
-#endif
-
-#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
-#define MAX_BLACKFIN_GPIOS 48
-
-#define	GPIO_PF0	0
-#define	GPIO_PF1	1
-#define	GPIO_PF2	2
-#define	GPIO_PF3	3
-#define	GPIO_PF4	4
-#define	GPIO_PF5	5
-#define	GPIO_PF6	6
-#define	GPIO_PF7	7
-#define	GPIO_PF8	8
-#define	GPIO_PF9	9
-#define	GPIO_PF10	10
-#define	GPIO_PF11	11
-#define	GPIO_PF12	12
-#define	GPIO_PF13	13
-#define	GPIO_PF14	14
-#define	GPIO_PF15	15
-#define	GPIO_PG0	16
-#define	GPIO_PG1	17
-#define	GPIO_PG2	18
-#define	GPIO_PG3	19
-#define	GPIO_PG4	20
-#define	GPIO_PG5	21
-#define	GPIO_PG6	22
-#define	GPIO_PG7	23
-#define	GPIO_PG8	24
-#define	GPIO_PG9	25
-#define	GPIO_PG10      	26
-#define	GPIO_PG11      	27
-#define	GPIO_PG12      	28
-#define	GPIO_PG13      	29
-#define	GPIO_PG14      	30
-#define	GPIO_PG15      	31
-#define	GPIO_PH0	32
-#define	GPIO_PH1	33
-#define	GPIO_PH2	34
-#define	GPIO_PH3	35
-#define	GPIO_PH4	36
-#define	GPIO_PH5	37
-#define	GPIO_PH6	38
-#define	GPIO_PH7	39
-#define	GPIO_PH8	40
-#define	GPIO_PH9	41
-#define	GPIO_PH10      	42
-#define	GPIO_PH11      	43
-#define	GPIO_PH12      	44
-#define	GPIO_PH13      	45
-#define	GPIO_PH14      	46
-#define	GPIO_PH15      	47
-
-#define PORT_F GPIO_PF0
-#define PORT_G GPIO_PG0
-#define PORT_H GPIO_PH0
-
-#endif
-
-#ifdef BF548_FAMILY
-#include <mach/gpio.h>
-#endif
-
-#ifdef BF561_FAMILY
-#define MAX_BLACKFIN_GPIOS 48
-
-#define	GPIO_PF0	0
-#define	GPIO_PF1	1
-#define	GPIO_PF2	2
-#define	GPIO_PF3	3
-#define	GPIO_PF4	4
-#define	GPIO_PF5	5
-#define	GPIO_PF6	6
-#define	GPIO_PF7	7
-#define	GPIO_PF8	8
-#define	GPIO_PF9	9
-#define	GPIO_PF10	10
-#define	GPIO_PF11	11
-#define	GPIO_PF12	12
-#define	GPIO_PF13	13
-#define	GPIO_PF14	14
-#define	GPIO_PF15	15
-#define	GPIO_PF16	16
-#define	GPIO_PF17	17
-#define	GPIO_PF18	18
-#define	GPIO_PF19	19
-#define	GPIO_PF20	20
-#define	GPIO_PF21	21
-#define	GPIO_PF22	22
-#define	GPIO_PF23	23
-#define	GPIO_PF24	24
-#define	GPIO_PF25	25
-#define	GPIO_PF26	26
-#define	GPIO_PF27	27
-#define	GPIO_PF28	28
-#define	GPIO_PF29	29
-#define	GPIO_PF30	30
-#define	GPIO_PF31	31
-#define	GPIO_PF32	32
-#define	GPIO_PF33	33
-#define	GPIO_PF34	34
-#define	GPIO_PF35	35
-#define	GPIO_PF36	36
-#define	GPIO_PF37	37
-#define	GPIO_PF38	38
-#define	GPIO_PF39	39
-#define	GPIO_PF40	40
-#define	GPIO_PF41	41
-#define	GPIO_PF42	42
-#define	GPIO_PF43	43
-#define	GPIO_PF44	44
-#define	GPIO_PF45	45
-#define	GPIO_PF46	46
-#define	GPIO_PF47	47
-
-#define PORT_FIO0 GPIO_0
-#define PORT_FIO1 GPIO_16
-#define PORT_FIO2 GPIO_32
-#endif
-
 #ifndef __ASSEMBLY__
 
 /***********************************************************
@@ -425,20 +286,77 @@ struct gpio_port_s {
 * MODIFICATION HISTORY :
 **************************************************************/
 
-int gpio_request(unsigned, const char *);
-void gpio_free(unsigned);
-
-void gpio_set_value(unsigned gpio, int arg);
-int gpio_get_value(unsigned gpio);
+int bfin_gpio_request(unsigned gpio, const char *label);
+void bfin_gpio_free(unsigned gpio);
+int bfin_gpio_irq_request(unsigned gpio, const char *label);
+void bfin_gpio_irq_free(unsigned gpio);
+int bfin_gpio_direction_input(unsigned gpio);
+int bfin_gpio_direction_output(unsigned gpio, int value);
+int bfin_gpio_get_value(unsigned gpio);
+void bfin_gpio_set_value(unsigned gpio, int value);
 
 #ifndef BF548_FAMILY
-#define gpio_set_value(gpio, value)	set_gpio_data(gpio, value)
+#define bfin_gpio_set_value(gpio, value)    set_gpio_data(gpio, value)
 #endif
 
-int gpio_direction_input(unsigned gpio);
-int gpio_direction_output(unsigned gpio, int value);
+#ifdef CONFIG_GPIOLIB
+#include <asm-generic/gpio.h>		/* cansleep wrappers */
+
+static inline int gpio_get_value(unsigned int gpio)
+{
+	if (gpio < MAX_BLACKFIN_GPIOS)
+		return bfin_gpio_get_value(gpio);
+	else
+		return __gpio_get_value(gpio);
+}
+
+static inline void gpio_set_value(unsigned int gpio, int value)
+{
+	if (gpio < MAX_BLACKFIN_GPIOS)
+		bfin_gpio_set_value(gpio, value);
+	else
+		__gpio_set_value(gpio, value);
+}
+
+static inline int gpio_cansleep(unsigned int gpio)
+{
+	return __gpio_cansleep(gpio);
+}
+
+#else /* !CONFIG_GPIOLIB */
+
+static inline int gpio_request(unsigned gpio, const char *label)
+{
+	return bfin_gpio_request(gpio, label);
+}
+
+static inline void gpio_free(unsigned gpio)
+{
+	return bfin_gpio_free(gpio);
+}
+
+static inline int gpio_direction_input(unsigned gpio)
+{
+	return bfin_gpio_direction_input(gpio);
+}
+
+static inline int gpio_direction_output(unsigned gpio, int value)
+{
+	return bfin_gpio_direction_output(gpio, value);
+}
+
+static inline int gpio_get_value(unsigned gpio)
+{
+	return bfin_gpio_get_value(gpio);
+}
+
+static inline void gpio_set_value(unsigned gpio, int value)
+{
+	return bfin_gpio_set_value(gpio, value);
+}
 
 #include <asm-generic/gpio.h>		/* cansleep wrappers */
+#endif	/* !CONFIG_GPIOLIB */
 #include <asm/irq.h>
 
 static inline int gpio_to_irq(unsigned gpio)
diff --git a/arch/blackfin/include/asm/hardirq.h b/arch/blackfin/include/asm/hardirq.h
index b6b19f1b9dab..717181a1749b 100644
--- a/arch/blackfin/include/asm/hardirq.h
+++ b/arch/blackfin/include/asm/hardirq.h
@@ -42,4 +42,6 @@ typedef struct {
 
 #define __ARCH_IRQ_EXIT_IRQS_DISABLED	1
 
+extern void ack_bad_irq(unsigned int irq);
+
 #endif
diff --git a/arch/blackfin/include/asm/io.h b/arch/blackfin/include/asm/io.h
index 7dc77a21fdf3..63b2d8c78570 100644
--- a/arch/blackfin/include/asm/io.h
+++ b/arch/blackfin/include/asm/io.h
@@ -94,12 +94,12 @@ static inline unsigned int readl(const volatile void __iomem *addr)
 #define outw_p(x,addr) outw(x,addr)
 #define outl_p(x,addr) outl(x,addr)
 
-#define ioread8_rep(a,d,c)	insb(a,d,c)
-#define ioread16_rep(a,d,c)	insw(a,d,c)
-#define ioread32_rep(a,d,c)	insl(a,d,c)
-#define iowrite8_rep(a,s,c)	outsb(a,s,c)
-#define iowrite16_rep(a,s,c)	outsw(a,s,c)
-#define iowrite32_rep(a,s,c)	outsl(a,s,c)
+#define ioread8_rep(a,d,c)	readsb(a,d,c)
+#define ioread16_rep(a,d,c)	readsw(a,d,c)
+#define ioread32_rep(a,d,c)	readsl(a,d,c)
+#define iowrite8_rep(a,s,c)	writesb(a,s,c)
+#define iowrite16_rep(a,s,c)	writesw(a,s,c)
+#define iowrite32_rep(a,s,c)	writesl(a,s,c)
 
 #define ioread8(X)			readb(X)
 #define ioread16(X)			readw(X)
@@ -108,6 +108,8 @@ static inline unsigned int readl(const volatile void __iomem *addr)
 #define iowrite16(val,X)		writew(val,X)
 #define iowrite32(val,X)		writel(val,X)
 
+#define mmiowb() wmb()
+
 #define IO_SPACE_LIMIT 0xffffffff
 
 /* Values for nocacheflag and cmode */
diff --git a/arch/blackfin/include/asm/ipipe.h b/arch/blackfin/include/asm/ipipe.h
new file mode 100644
index 000000000000..76f53d8b9a0d
--- /dev/null
+++ b/arch/blackfin/include/asm/ipipe.h
@@ -0,0 +1,278 @@
+/*   -*- linux-c -*-
+ *   include/asm-blackfin/ipipe.h
+ *
+ *   Copyright (C) 2002-2007 Philippe Gerum.
+ *
+ *   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, Inc., 675 Mass Ave, Cambridge MA 02139,
+ *   USA; either version 2 of the License, or (at your option) any later
+ *   version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef __ASM_BLACKFIN_IPIPE_H
+#define __ASM_BLACKFIN_IPIPE_H
+
+#ifdef CONFIG_IPIPE
+
+#include <linux/cpumask.h>
+#include <linux/list.h>
+#include <linux/threads.h>
+#include <linux/irq.h>
+#include <linux/ipipe_percpu.h>
+#include <asm/ptrace.h>
+#include <asm/irq.h>
+#include <asm/bitops.h>
+#include <asm/atomic.h>
+#include <asm/traps.h>
+
+#define IPIPE_ARCH_STRING     "1.8-00"
+#define IPIPE_MAJOR_NUMBER    1
+#define IPIPE_MINOR_NUMBER    8
+#define IPIPE_PATCH_NUMBER    0
+
+#ifdef CONFIG_SMP
+#error "I-pipe/blackfin: SMP not implemented"
+#else /* !CONFIG_SMP */
+#define ipipe_processor_id()	0
+#endif	/* CONFIG_SMP */
+
+#define prepare_arch_switch(next)		\
+do {						\
+	ipipe_schedule_notify(current, next);	\
+	local_irq_disable_hw();			\
+} while (0)
+
+#define task_hijacked(p)						\
+	({								\
+		int __x__ = ipipe_current_domain != ipipe_root_domain;	\
+		/* We would need to clear the SYNC flag for the root domain */ \
+		/* over the current processor in SMP mode. */		\
+		local_irq_enable_hw(); __x__;				\
+	})
+
+struct ipipe_domain;
+
+struct ipipe_sysinfo {
+
+	int ncpus;		/* Number of CPUs on board */
+	u64 cpufreq;		/* CPU frequency (in Hz) */
+
+	/* Arch-dependent block */
+
+	struct {
+		unsigned tmirq;	/* Timer tick IRQ */
+		u64 tmfreq;	/* Timer frequency */
+	} archdep;
+};
+
+#define ipipe_read_tsc(t)					\
+	({							\
+	unsigned long __cy2;					\
+	__asm__ __volatile__ ("1: %0 = CYCLES2\n"		\
+				"%1 = CYCLES\n"			\
+				"%2 = CYCLES2\n"		\
+				"CC = %2 == %0\n"		\
+				"if ! CC jump 1b\n"		\
+				: "=r" (((unsigned long *)&t)[1]),	\
+				  "=r" (((unsigned long *)&t)[0]),	\
+				  "=r" (__cy2)				\
+				: /*no input*/ : "CC");			\
+	t;								\
+	})
+
+#define ipipe_cpu_freq()	__ipipe_core_clock
+#define ipipe_tsc2ns(_t)	(((unsigned long)(_t)) * __ipipe_freq_scale)
+#define ipipe_tsc2us(_t)	(ipipe_tsc2ns(_t) / 1000 + 1)
+
+/* Private interface -- Internal use only */
+
+#define __ipipe_check_platform()	do { } while (0)
+
+#define __ipipe_init_platform()		do { } while (0)
+
+extern atomic_t __ipipe_irq_lvdepth[IVG15 + 1];
+
+extern unsigned long __ipipe_irq_lvmask;
+
+extern struct ipipe_domain ipipe_root;
+
+/* enable/disable_irqdesc _must_ be used in pairs. */
+
+void __ipipe_enable_irqdesc(struct ipipe_domain *ipd,
+			    unsigned irq);
+
+void __ipipe_disable_irqdesc(struct ipipe_domain *ipd,
+			     unsigned irq);
+
+#define __ipipe_enable_irq(irq)		(irq_desc[irq].chip->unmask(irq))
+
+#define __ipipe_disable_irq(irq)	(irq_desc[irq].chip->mask(irq))
+
+#define __ipipe_lock_root()					\
+	set_bit(IPIPE_ROOTLOCK_FLAG, &ipipe_root_domain->flags)
+
+#define __ipipe_unlock_root()					\
+	clear_bit(IPIPE_ROOTLOCK_FLAG, &ipipe_root_domain->flags)
+
+void __ipipe_enable_pipeline(void);
+
+#define __ipipe_hook_critical_ipi(ipd) do { } while (0)
+
+#define __ipipe_sync_pipeline(syncmask)					\
+	do {								\
+		struct ipipe_domain *ipd = ipipe_current_domain;	\
+		if (likely(ipd != ipipe_root_domain || !test_bit(IPIPE_ROOTLOCK_FLAG, &ipd->flags))) \
+			__ipipe_sync_stage(syncmask);			\
+	} while (0)
+
+void __ipipe_handle_irq(unsigned irq, struct pt_regs *regs);
+
+int __ipipe_get_irq_priority(unsigned irq);
+
+int __ipipe_get_irqthread_priority(unsigned irq);
+
+void __ipipe_stall_root_raw(void);
+
+void __ipipe_unstall_root_raw(void);
+
+void __ipipe_serial_debug(const char *fmt, ...);
+
+DECLARE_PER_CPU(struct pt_regs, __ipipe_tick_regs);
+
+extern unsigned long __ipipe_core_clock;
+
+extern unsigned long __ipipe_freq_scale;
+
+extern unsigned long __ipipe_irq_tail_hook;
+
+static inline unsigned long __ipipe_ffnz(unsigned long ul)
+{
+	return ffs(ul) - 1;
+}
+
+#define __ipipe_run_irqtail()  /* Must be a macro */			\
+	do {								\
+		asmlinkage void __ipipe_call_irqtail(void);		\
+		unsigned long __pending;				\
+		CSYNC();					\
+		__pending = bfin_read_IPEND();				\
+		if (__pending & 0x8000) {				\
+			__pending &= ~0x8010;				\
+			if (__pending && (__pending & (__pending - 1)) == 0) \
+				__ipipe_call_irqtail();			\
+		}							\
+	} while (0)
+
+#define __ipipe_run_isr(ipd, irq)					\
+	do {								\
+		if (ipd == ipipe_root_domain) {				\
+			/*						\
+			 * Note: the I-pipe implements a threaded interrupt model on \
+			 * this arch for Linux external IRQs. The interrupt handler we \
+			 * call here only wakes up the associated IRQ thread. \
+			 */						\
+			if (ipipe_virtual_irq_p(irq)) {			\
+				/* No irqtail here; virtual interrupts have no effect \
+				   on IPEND so there is no need for processing \
+				   deferral. */				\
+				local_irq_enable_nohead(ipd);		\
+				ipd->irqs[irq].handler(irq, ipd->irqs[irq].cookie); \
+				local_irq_disable_nohead(ipd);		\
+			} else						\
+				/*					\
+				 * No need to run the irqtail here either; \
+				 * we can't be preempted by hw IRQs, so	\
+				 * non-Linux IRQs cannot stack over the short \
+				 * thread wakeup code. Which in turn means \
+				 * that no irqtail condition could be pending \
+				 * for domains above Linux in the pipeline. \
+				 */					\
+				ipd->irqs[irq].handler(irq, &__raw_get_cpu_var(__ipipe_tick_regs)); \
+		} else {						\
+			__clear_bit(IPIPE_SYNC_FLAG, &ipipe_cpudom_var(ipd, status)); \
+			local_irq_enable_nohead(ipd);			\
+			ipd->irqs[irq].handler(irq, ipd->irqs[irq].cookie); \
+			/* Attempt to exit the outer interrupt level before \
+			 * starting the deferred IRQ processing. */	\
+			local_irq_disable_nohead(ipd);			\
+			__ipipe_run_irqtail();				\
+			__set_bit(IPIPE_SYNC_FLAG, &ipipe_cpudom_var(ipd, status)); \
+		}							\
+	} while (0)
+
+#define __ipipe_syscall_watched_p(p, sc)	\
+	(((p)->flags & PF_EVNOTIFY) || (unsigned long)sc >= NR_syscalls)
+
+void ipipe_init_irq_threads(void);
+
+int ipipe_start_irq_thread(unsigned irq, struct irq_desc *desc);
+
+#define IS_SYSIRQ(irq)		((irq) > IRQ_CORETMR && (irq) <= SYS_IRQS)
+#define IS_GPIOIRQ(irq)		((irq) >= GPIO_IRQ_BASE && (irq) < NR_IRQS)
+
+#define IRQ_SYSTMR		IRQ_TIMER0
+#define IRQ_PRIOTMR		CONFIG_IRQ_TIMER0
+
+#if defined(CONFIG_BF531) || defined(CONFIG_BF532) || defined(CONFIG_BF533)
+#define PRIO_GPIODEMUX(irq)	CONFIG_PFA
+#elif defined(CONFIG_BF534) || defined(CONFIG_BF536) || defined(CONFIG_BF537)
+#define PRIO_GPIODEMUX(irq)	CONFIG_IRQ_PROG_INTA
+#elif defined(CONFIG_BF52x)
+#define PRIO_GPIODEMUX(irq)	((irq) == IRQ_PORTF_INTA ? CONFIG_IRQ_PORTF_INTA : \
+				 (irq) == IRQ_PORTG_INTA ? CONFIG_IRQ_PORTG_INTA : \
+				 (irq) == IRQ_PORTH_INTA ? CONFIG_IRQ_PORTH_INTA : \
+				 -1)
+#elif defined(CONFIG_BF561)
+#define PRIO_GPIODEMUX(irq)	((irq) == IRQ_PROG0_INTA ? CONFIG_IRQ_PROG0_INTA : \
+				 (irq) == IRQ_PROG1_INTA ? CONFIG_IRQ_PROG1_INTA : \
+				 (irq) == IRQ_PROG2_INTA ? CONFIG_IRQ_PROG2_INTA : \
+				 -1)
+#define bfin_write_TIMER_DISABLE(val)	bfin_write_TMRS8_DISABLE(val)
+#define bfin_write_TIMER_ENABLE(val)	bfin_write_TMRS8_ENABLE(val)
+#define bfin_write_TIMER_STATUS(val)	bfin_write_TMRS8_STATUS(val)
+#define bfin_read_TIMER_STATUS()	bfin_read_TMRS8_STATUS()
+#elif defined(CONFIG_BF54x)
+#define PRIO_GPIODEMUX(irq)	((irq) == IRQ_PINT0 ? CONFIG_IRQ_PINT0 : \
+				 (irq) == IRQ_PINT1 ? CONFIG_IRQ_PINT1 : \
+				 (irq) == IRQ_PINT2 ? CONFIG_IRQ_PINT2 : \
+				 (irq) == IRQ_PINT3 ? CONFIG_IRQ_PINT3 : \
+				 -1)
+#define bfin_write_TIMER_DISABLE(val)	bfin_write_TIMER_DISABLE0(val)
+#define bfin_write_TIMER_ENABLE(val)	bfin_write_TIMER_ENABLE0(val)
+#define bfin_write_TIMER_STATUS(val)	bfin_write_TIMER_STATUS0(val)
+#define bfin_read_TIMER_STATUS(val)	bfin_read_TIMER_STATUS0(val)
+#else
+# error "no PRIO_GPIODEMUX() for this part"
+#endif
+
+#define __ipipe_root_tick_p(regs)	((regs->ipend & 0x10) != 0)
+
+#else /* !CONFIG_IPIPE */
+
+#define task_hijacked(p)		0
+#define ipipe_trap_notify(t, r)  	0
+
+#define __ipipe_stall_root_raw()	do { } while (0)
+#define __ipipe_unstall_root_raw()	do { } while (0)
+
+#define ipipe_init_irq_threads()		do { } while (0)
+#define ipipe_start_irq_thread(irq, desc)	0
+
+#define IRQ_SYSTMR		IRQ_CORETMR
+#define IRQ_PRIOTMR		IRQ_CORETMR
+
+#define __ipipe_root_tick_p(regs)	1
+
+#endif /* !CONFIG_IPIPE */
+
+#endif	/* !__ASM_BLACKFIN_IPIPE_H */
diff --git a/arch/blackfin/include/asm/ipipe_base.h b/arch/blackfin/include/asm/ipipe_base.h
new file mode 100644
index 000000000000..cb1025aeabcf
--- /dev/null
+++ b/arch/blackfin/include/asm/ipipe_base.h
@@ -0,0 +1,80 @@
+/*   -*- linux-c -*-
+ *   include/asm-blackfin/_baseipipe.h
+ *
+ *   Copyright (C) 2007 Philippe Gerum.
+ *
+ *   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, Inc., 675 Mass Ave, Cambridge MA 02139,
+ *   USA; either version 2 of the License, or (at your option) any later
+ *   version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef __ASM_BLACKFIN_IPIPE_BASE_H
+#define __ASM_BLACKFIN_IPIPE_BASE_H
+
+#ifdef CONFIG_IPIPE
+
+#define IPIPE_NR_XIRQS		NR_IRQS
+#define IPIPE_IRQ_ISHIFT	5	/* 2^5 for 32bits arch. */
+
+/* Blackfin-specific, global domain flags */
+#define IPIPE_ROOTLOCK_FLAG	1	/* Lock pipeline for root */
+
+ /* Blackfin traps -- i.e. exception vector numbers */
+#define IPIPE_NR_FAULTS		52 /* We leave a gap after VEC_ILL_RES. */
+/* Pseudo-vectors used for kernel events */
+#define IPIPE_FIRST_EVENT	IPIPE_NR_FAULTS
+#define IPIPE_EVENT_SYSCALL	(IPIPE_FIRST_EVENT)
+#define IPIPE_EVENT_SCHEDULE	(IPIPE_FIRST_EVENT + 1)
+#define IPIPE_EVENT_SIGWAKE	(IPIPE_FIRST_EVENT + 2)
+#define IPIPE_EVENT_SETSCHED	(IPIPE_FIRST_EVENT + 3)
+#define IPIPE_EVENT_INIT	(IPIPE_FIRST_EVENT + 4)
+#define IPIPE_EVENT_EXIT	(IPIPE_FIRST_EVENT + 5)
+#define IPIPE_EVENT_CLEANUP	(IPIPE_FIRST_EVENT + 6)
+#define IPIPE_LAST_EVENT	IPIPE_EVENT_CLEANUP
+#define IPIPE_NR_EVENTS		(IPIPE_LAST_EVENT + 1)
+
+#define IPIPE_TIMER_IRQ		IRQ_CORETMR
+
+#ifndef __ASSEMBLY__
+
+#include <linux/bitops.h>
+
+extern int test_bit(int nr, const void *addr);
+
+
+extern unsigned long __ipipe_root_status; /* Alias to ipipe_root_cpudom_var(status) */
+
+static inline void __ipipe_stall_root(void)
+{
+	volatile unsigned long *p = &__ipipe_root_status;
+	set_bit(0, p);
+}
+
+static inline unsigned long __ipipe_test_and_stall_root(void)
+{
+	volatile unsigned long *p = &__ipipe_root_status;
+	return test_and_set_bit(0, p);
+}
+
+static inline unsigned long __ipipe_test_root(void)
+{
+	const unsigned long *p = &__ipipe_root_status;
+	return test_bit(0, p);
+}
+
+#endif /* !__ASSEMBLY__ */
+
+#endif /* CONFIG_IPIPE */
+
+#endif /* !__ASM_BLACKFIN_IPIPE_BASE_H */
diff --git a/arch/blackfin/include/asm/irq.h b/arch/blackfin/include/asm/irq.h
index 89f59e18af93..3d977909ce7d 100644
--- a/arch/blackfin/include/asm/irq.h
+++ b/arch/blackfin/include/asm/irq.h
@@ -17,56 +17,272 @@
 #ifndef _BFIN_IRQ_H_
 #define _BFIN_IRQ_H_
 
+/* SYS_IRQS and NR_IRQS are defined in <mach-bf5xx/irq.h>*/
 #include <mach/irq.h>
-#include <asm/ptrace.h>
-
-/*******************************************************************************
- *****   INTRODUCTION ***********
- *   On the Blackfin, the interrupt structure allows remmapping of the hardware
- *   levels.
- * - I'm going to assume that the H/W level is going to stay at the default
- *   settings. If someone wants to go through and abstart this out, feel free
- *   to mod the interrupt numbering scheme.
- * - I'm abstracting the interrupts so that uClinux does not know anything
- *   about the H/W levels. If you want to change the H/W AND keep the abstracted
- *   levels that uClinux sees, you should be able to do most of it here.
- * - I've left the "abstract" numbering sparce in case someone wants to pull the
- *   interrupts apart (just the TX/RX for the various devices)
- *******************************************************************************/
+#include <asm/pda.h>
+#include <asm/processor.h>
 
-/* SYS_IRQS and NR_IRQS are defined in <mach-bf5xx/irq.h>*/
+#ifdef CONFIG_SMP
+/* Forward decl needed due to cdef inter dependencies */
+static inline uint32_t __pure bfin_dspid(void);
+# define blackfin_core_id() (bfin_dspid() & 0xff)
+# define bfin_irq_flags cpu_pda[blackfin_core_id()].imask
+#else
+extern unsigned long bfin_irq_flags;
+#endif
 
-/*
- * Machine specific interrupt sources.
- *
- * Adding an interrupt service routine for a source with this bit
- * set indicates a special machine specific interrupt source.
- * The machine specific files define these sources.
- *
- * The IRQ_MACHSPEC bit is now gone - the only thing it did was to
- * introduce unnecessary overhead.
- *
- * All interrupt handling is actually machine specific so it is better
- * to use function pointers, as used by the Sparc port, and select the
- * interrupt handling functions when initializing the kernel. This way
- * we save some unnecessary overhead at run-time.
- *                                                      01/11/97 - Jes
- */
+#ifdef CONFIG_IPIPE
+
+#include <linux/ipipe_trace.h>
+
+void __ipipe_unstall_root(void);
+
+void __ipipe_restore_root(unsigned long flags);
+
+#ifdef CONFIG_DEBUG_HWERR
+# define __all_masked_irq_flags 0x3f
+# define __save_and_cli_hw(x) \
+	__asm__ __volatile__( \
+		"cli %0;" \
+		"sti %1;" \
+		: "=&d"(x) \
+		: "d" (0x3F) \
+	)
+#else
+# define __all_masked_irq_flags 0x1f
+# define __save_and_cli_hw(x) \
+	__asm__ __volatile__( \
+		"cli %0;" \
+		: "=&d"(x) \
+	)
+#endif
+
+#define irqs_enabled_from_flags_hw(x)	((x) != __all_masked_irq_flags)
+#define raw_irqs_disabled_flags(flags)	(!irqs_enabled_from_flags_hw(flags))
+#define local_test_iflag_hw(x)		irqs_enabled_from_flags_hw(x)
 
-extern void ack_bad_irq(unsigned int irq);
+#define local_save_flags(x)						\
+	do {								\
+		(x) = __ipipe_test_root() ? \
+			__all_masked_irq_flags : bfin_irq_flags; \
+	} while (0)
 
-static __inline__ int irq_canonicalize(int irq)
+#define local_irq_save(x)				\
+	do {						\
+		(x) = __ipipe_test_and_stall_root();	\
+	} while (0)
+
+#define local_irq_restore(x)	__ipipe_restore_root(x)
+#define local_irq_disable()	__ipipe_stall_root()
+#define local_irq_enable()	__ipipe_unstall_root()
+#define irqs_disabled()		__ipipe_test_root()
+
+#define local_save_flags_hw(x) \
+	__asm__ __volatile__( \
+		"cli %0;" \
+		"sti %0;" \
+		: "=d"(x) \
+	)
+
+#define	irqs_disabled_hw()				\
+	({						\
+		unsigned long flags;			\
+		local_save_flags_hw(flags);		\
+		!irqs_enabled_from_flags_hw(flags);	\
+	})
+
+static inline unsigned long raw_mangle_irq_bits(int virt, unsigned long real)
 {
-	return irq;
+	/* Merge virtual and real interrupt mask bits into a single
+	   32bit word. */
+	return (real & ~(1 << 31)) | ((virt != 0) << 31);
+}
+
+static inline int raw_demangle_irq_bits(unsigned long *x)
+{
+	int virt = (*x & (1 << 31)) != 0;
+	*x &= ~(1L << 31);
+	return virt;
 }
 
-/* count of spurious interrupts */
-/* extern volatile unsigned int num_spurious; */
+#ifdef CONFIG_IPIPE_TRACE_IRQSOFF
+
+#define local_irq_disable_hw()						\
+	do {								\
+		int _tmp_dummy;						\
+		if (!irqs_disabled_hw())				\
+			ipipe_trace_begin(0x80000000);			\
+		__asm__ __volatile__ ("cli %0;" : "=d" (_tmp_dummy) : );	\
+	} while (0)
+
+#define local_irq_enable_hw()						\
+	do {								\
+		if (irqs_disabled_hw())					\
+			ipipe_trace_end(0x80000000);			\
+		__asm__ __volatile__ ("sti %0;" : : "d"(bfin_irq_flags));	\
+	} while (0)
+
+#define local_irq_save_hw(x)				\
+	do {						\
+		__save_and_cli_hw(x);			\
+		if (local_test_iflag_hw(x))		\
+			ipipe_trace_begin(0x80000001);	\
+	} while (0)
+
+#define local_irq_restore_hw(x)				\
+	do {						\
+		if (local_test_iflag_hw(x)) {		\
+			ipipe_trace_end(0x80000001);	\
+			local_irq_enable_hw_notrace();	\
+		}					\
+	} while (0)
+
+#define local_irq_disable_hw_notrace()					\
+	do {								\
+		int _tmp_dummy;						\
+		__asm__ __volatile__ ("cli %0;" : "=d" (_tmp_dummy) : );	\
+	} while (0)
+
+#define local_irq_enable_hw_notrace() \
+	__asm__ __volatile__( \
+		"sti %0;" \
+		: \
+		: "d"(bfin_irq_flags) \
+	)
 
-#ifndef NO_IRQ
-#define NO_IRQ ((unsigned int)(-1))
+#define local_irq_save_hw_notrace(x) __save_and_cli_hw(x)
+
+#define local_irq_restore_hw_notrace(x)			\
+	do {						\
+		if (local_test_iflag_hw(x))		\
+			local_irq_enable_hw_notrace();	\
+	} while (0)
+
+#else /* CONFIG_IPIPE_TRACE_IRQSOFF */
+
+#define local_irq_enable_hw() \
+	__asm__ __volatile__( \
+		"sti %0;" \
+		: \
+		: "d"(bfin_irq_flags) \
+	)
+
+#define local_irq_disable_hw()			\
+	do {					\
+		int _tmp_dummy;			\
+		__asm__ __volatile__ (		\
+			"cli %0;"		\
+			: "=d" (_tmp_dummy));	\
+	} while (0)
+
+#define local_irq_restore_hw(x) \
+	do { \
+		if (irqs_enabled_from_flags_hw(x)) \
+			local_irq_enable_hw(); \
+	} while (0)
+
+#define local_irq_save_hw(x)		__save_and_cli_hw(x)
+
+#define local_irq_disable_hw_notrace()	local_irq_disable_hw()
+#define local_irq_enable_hw_notrace()	local_irq_enable_hw()
+#define local_irq_save_hw_notrace(x)	local_irq_save_hw(x)
+#define local_irq_restore_hw_notrace(x)	local_irq_restore_hw(x)
+
+#endif  /* CONFIG_IPIPE_TRACE_IRQSOFF */
+
+#else /* !CONFIG_IPIPE */
+
+/*
+ * Interrupt configuring macros.
+ */
+#define local_irq_disable() \
+	do { \
+		int __tmp_dummy; \
+		__asm__ __volatile__( \
+			"cli %0;" \
+			: "=d" (__tmp_dummy) \
+		); \
+	} while (0)
+
+#define local_irq_enable() \
+	__asm__ __volatile__( \
+		"sti %0;" \
+		: \
+		: "d" (bfin_irq_flags) \
+	)
+
+#ifdef CONFIG_DEBUG_HWERR
+# define __save_and_cli(x) \
+	__asm__ __volatile__( \
+		"cli %0;" \
+		"sti %1;" \
+		: "=&d" (x) \
+		: "d" (0x3F) \
+	)
+#else
+# define __save_and_cli(x) \
+	__asm__ __volatile__( \
+		"cli %0;" \
+		: "=&d" (x) \
+	)
 #endif
 
-#define SIC_SYSIRQ(irq)	(irq - (IRQ_CORETMR + 1))
+#define local_save_flags(x) \
+	__asm__ __volatile__( \
+		"cli %0;" \
+		"sti %0;" \
+		: "=d" (x) \
+	)
+
+#ifdef CONFIG_DEBUG_HWERR
+#define irqs_enabled_from_flags(x) (((x) & ~0x3f) != 0)
+#else
+#define irqs_enabled_from_flags(x) ((x) != 0x1f)
+#endif
+
+#define local_irq_restore(x) \
+	do { \
+		if (irqs_enabled_from_flags(x)) \
+			local_irq_enable(); \
+	} while (0)
+
+/* For spinlocks etc */
+#define local_irq_save(x) __save_and_cli(x)
+
+#define irqs_disabled()				\
+({						\
+	unsigned long flags;			\
+	local_save_flags(flags);		\
+	!irqs_enabled_from_flags(flags);	\
+})
+
+#define local_irq_save_hw(x)		local_irq_save(x)
+#define local_irq_restore_hw(x)		local_irq_restore(x)
+#define local_irq_enable_hw()		local_irq_enable()
+#define local_irq_disable_hw()		local_irq_disable()
+#define irqs_disabled_hw()		irqs_disabled()
+
+#endif /* !CONFIG_IPIPE */
+
+#if ANOMALY_05000244 && defined(CONFIG_BFIN_ICACHE)
+# define NOP_PAD_ANOMALY_05000244 "nop; nop;"
+#else
+# define NOP_PAD_ANOMALY_05000244
+#endif
+
+#define idle_with_irq_disabled() \
+	__asm__ __volatile__( \
+		NOP_PAD_ANOMALY_05000244 \
+		".align 8;" \
+		"sti %0;" \
+		"idle;" \
+		: \
+		: "d" (bfin_irq_flags) \
+	)
+
+static inline int irq_canonicalize(int irq)
+{
+	return irq;
+}
 
 #endif				/* _BFIN_IRQ_H_ */
diff --git a/arch/blackfin/include/asm/l1layout.h b/arch/blackfin/include/asm/l1layout.h
index c13ded777828..79dbefaa5bef 100644
--- a/arch/blackfin/include/asm/l1layout.h
+++ b/arch/blackfin/include/asm/l1layout.h
@@ -8,6 +8,7 @@
 
 #include <asm/blackfin.h>
 
+#ifndef CONFIG_SMP
 #ifndef __ASSEMBLY__
 
 /* Data that is "mapped" into the process VM at the start of the L1 scratch
@@ -24,8 +25,10 @@ struct l1_scratch_task_info
 };
 
 /* A pointer to the structure in memory.  */
-#define L1_SCRATCH_TASK_INFO ((struct l1_scratch_task_info *)L1_SCRATCH_START)
+#define L1_SCRATCH_TASK_INFO ((struct l1_scratch_task_info *)\
+						get_l1_scratch_start())
 
 #endif
+#endif
 
 #endif
diff --git a/arch/blackfin/mach-bf527/include/mach/mem_init.h b/arch/blackfin/include/asm/mem_init.h
index cbe03f4a5698..255a9316ad36 100644
--- a/arch/blackfin/mach-bf527/include/mach/mem_init.h
+++ b/arch/blackfin/include/asm/mem_init.h
@@ -1,35 +1,20 @@
 /*
- * File:         include/asm-blackfin/mach-bf527/mem_init.h
- * Based on:
- * Author:
+ * arch/blackfin/include/asm/mem_init.h - reprogram clocks / memory
  *
- * Created:
- * Description:
+ * Copyright 2004-2008 Analog Devices Inc.
  *
- * Rev:
- *
- * Modified:
- *               Copyright 2004-2007 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, 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; see the file COPYING.
- * If not, write to the Free Software Foundation,
- * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ * Licensed under the GPL-2 or later.
  */
 
-#if (CONFIG_MEM_MT48LC16M16A2TG_75 || CONFIG_MEM_MT48LC64M4A2FB_7E || CONFIG_MEM_MT48LC16M8A2TG_75 || CONFIG_MEM_GENERIC_BOARD || CONFIG_MEM_MT48LC32M8A2_75 || CONFIG_MEM_MT48LC32M16A2TG_75)
+#if defined(EBIU_SDGCTL)
+#if defined(CONFIG_MEM_MT48LC16M16A2TG_75) || \
+    defined(CONFIG_MEM_MT48LC64M4A2FB_7E) || \
+    defined(CONFIG_MEM_MT48LC16M8A2TG_75) || \
+    defined(CONFIG_MEM_GENERIC_BOARD) || \
+    defined(CONFIG_MEM_MT48LC32M8A2_75) || \
+    defined(CONFIG_MEM_MT48LC8M32B2B5_7) || \
+    defined(CONFIG_MEM_MT48LC32M16A2TG_75) || \
+    defined(CONFIG_MEM_MT48LC32M8A2_75)
 #if (CONFIG_SCLK_HZ > 119402985)
 #define SDRAM_tRP       TRP_2
 #define SDRAM_tRP_num   2
@@ -104,53 +89,114 @@
 #endif
 #endif
 
-#if (CONFIG_MEM_MT48LC16M16A2TG_75)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-#if (CONFIG_MEM_MT48LC16M8A2TG_75)
+#if defined(CONFIG_MEM_MT48LC16M8A2TG_75) || \
+    defined(CONFIG_MEM_MT48LC8M32B2B5_7)
   /*SDRAM INFORMATION: */
 #define SDRAM_Tref  64		/* Refresh period in milliseconds   */
 #define SDRAM_NRA   4096	/* Number of row addresses in SDRAM */
 #define SDRAM_CL    CL_3
 #endif
 
-#if (CONFIG_MEM_MT48LC32M8A2_75)
+#if defined(CONFIG_MEM_MT48LC32M8A2_75) || \
+    defined(CONFIG_MEM_MT48LC64M4A2FB_7E) || \
+    defined(CONFIG_MEM_GENERIC_BOARD) || \
+    defined(CONFIG_MEM_MT48LC32M16A2TG_75) || \
+    defined(CONFIG_MEM_MT48LC16M16A2TG_75) || \
+    defined(CONFIG_MEM_MT48LC32M8A2_75)
   /*SDRAM INFORMATION: */
 #define SDRAM_Tref  64		/* Refresh period in milliseconds   */
 #define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
 #define SDRAM_CL    CL_3
 #endif
 
-#if (CONFIG_MEM_MT48LC64M4A2FB_7E)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
+
+#ifdef CONFIG_BFIN_KERNEL_CLOCK_MEMINIT_CALC
+/* Equation from section 17 (p17-46) of BF533 HRM */
+#define mem_SDRRC       (((CONFIG_SCLK_HZ / 1000) * SDRAM_Tref) / SDRAM_NRA) - (SDRAM_tRAS_num + SDRAM_tRP_num)
+
+/* Enable SCLK Out */
+#define mem_SDGCTL        (0x80000000 | SCTLE | SDRAM_CL | SDRAM_tRAS | SDRAM_tRP | SDRAM_tRCD | SDRAM_tWR | PSS)
+#else
+#define mem_SDRRC 	CONFIG_MEM_SDRRC
+#define mem_SDGCTL	CONFIG_MEM_SDGCTL
+#endif
 #endif
 
-#if (CONFIG_MEM_GENERIC_BOARD)
-  /*SDRAM INFORMATION: Modify this for your board */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
+
+#if defined(EBIU_DDRCTL0)
+#define MIN_DDR_SCLK(x)	(x*(CONFIG_SCLK_HZ/1000/1000)/1000 + 1)
+#define MAX_DDR_SCLK(x)	(x*(CONFIG_SCLK_HZ/1000/1000)/1000)
+#define DDR_CLK_HZ(x)	(1000*1000*1000/x)
+
+#if defined(CONFIG_MEM_MT46V32M16_6T)
+#define DDR_SIZE	DEVSZ_512
+#define DDR_WIDTH	DEVWD_16
+#define DDR_MAX_tCK	13
+
+#define DDR_tRC		DDR_TRC(MIN_DDR_SCLK(60))
+#define DDR_tRAS	DDR_TRAS(MIN_DDR_SCLK(42))
+#define DDR_tRP		DDR_TRP(MIN_DDR_SCLK(15))
+#define DDR_tRFC	DDR_TRFC(MIN_DDR_SCLK(72))
+#define DDR_tREFI	DDR_TREFI(MAX_DDR_SCLK(7800))
+
+#define DDR_tRCD	DDR_TRCD(MIN_DDR_SCLK(15))
+#define DDR_tWTR	DDR_TWTR(1)
+#define DDR_tMRD	DDR_TMRD(MIN_DDR_SCLK(12))
+#define DDR_tWR		DDR_TWR(MIN_DDR_SCLK(15))
 #endif
 
-#if (CONFIG_MEM_MT48LC32M16A2TG_75)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
+#if defined(CONFIG_MEM_MT46V32M16_5B)
+#define DDR_SIZE	DEVSZ_512
+#define DDR_WIDTH	DEVWD_16
+#define DDR_MAX_tCK	13
+
+#define DDR_tRC		DDR_TRC(MIN_DDR_SCLK(55))
+#define DDR_tRAS	DDR_TRAS(MIN_DDR_SCLK(40))
+#define DDR_tRP		DDR_TRP(MIN_DDR_SCLK(15))
+#define DDR_tRFC	DDR_TRFC(MIN_DDR_SCLK(70))
+#define DDR_tREFI	DDR_TREFI(MAX_DDR_SCLK(7800))
+
+#define DDR_tRCD	DDR_TRCD(MIN_DDR_SCLK(15))
+#define DDR_tWTR	DDR_TWTR(2)
+#define DDR_tMRD	DDR_TMRD(MIN_DDR_SCLK(10))
+#define DDR_tWR		DDR_TWR(MIN_DDR_SCLK(15))
 #endif
 
-/* Equation from section 17 (p17-46) of BF533 HRM */
-#define mem_SDRRC       (((CONFIG_SCLK_HZ / 1000) * SDRAM_Tref) / SDRAM_NRA) - (SDRAM_tRAS_num + SDRAM_tRP_num)
+#if defined(CONFIG_MEM_GENERIC_BOARD)
+#define DDR_SIZE	DEVSZ_512
+#define DDR_WIDTH	DEVWD_16
+#define DDR_MAX_tCK	13
 
-/* Enable SCLK Out */
-#define mem_SDGCTL        (SCTLE | SDRAM_CL | SDRAM_tRAS | SDRAM_tRP | SDRAM_tRCD | SDRAM_tWR | PSS)
+#define DDR_tRCD	DDR_TRCD(3)
+#define DDR_tWTR	DDR_TWTR(2)
+#define DDR_tWR		DDR_TWR(2)
+#define DDR_tMRD	DDR_TMRD(2)
+#define DDR_tRP		DDR_TRP(3)
+#define DDR_tRAS	DDR_TRAS(7)
+#define DDR_tRC		DDR_TRC(10)
+#define DDR_tRFC	DDR_TRFC(12)
+#define DDR_tREFI	DDR_TREFI(1288)
+#endif
+
+#if (CONFIG_SCLK_HZ < DDR_CLK_HZ(DDR_MAX_tCK))
+# error "CONFIG_SCLK_HZ is too small (<DDR_CLK_HZ(DDR_MAX_tCK) Hz)."
+#elif(CONFIG_SCLK_HZ <= 133333333)
+# define	DDR_CL		CL_2
+#else
+# error "CONFIG_SCLK_HZ is too large (>133333333 Hz)."
+#endif
+
+#ifdef CONFIG_BFIN_KERNEL_CLOCK_MEMINIT_CALC
+#define mem_DDRCTL0	(DDR_tRP | DDR_tRAS | DDR_tRC | DDR_tRFC | DDR_tREFI)
+#define mem_DDRCTL1	(DDR_DATWIDTH | EXTBANK_1 | DDR_SIZE | DDR_WIDTH | DDR_tWTR \
+			| DDR_tMRD | DDR_tWR | DDR_tRCD)
+#define mem_DDRCTL2	DDR_CL
+#else
+#define mem_DDRCTL0	CONFIG_MEM_DDRCTL0
+#define mem_DDRCTL1	CONFIG_MEM_DDRCTL1
+#define mem_DDRCTL2	CONFIG_MEM_DDRCTL2
+#endif
+#endif
 
 #if defined CONFIG_CLKIN_HALF
 #define CLKIN_HALF       1
@@ -165,6 +211,13 @@
 #endif
 
 /***************************************Currently Not Being Used *********************************/
+
+#if defined(CONFIG_FLASH_SPEED_BWAT) && \
+defined(CONFIG_FLASH_SPEED_BRAT) && \
+defined(CONFIG_FLASH_SPEED_BHT) && \
+defined(CONFIG_FLASH_SPEED_BST) && \
+defined(CONFIG_FLASH_SPEED_BTT)
+
 #define flash_EBIU_AMBCTL_WAT  ((CONFIG_FLASH_SPEED_BWAT * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
 #define flash_EBIU_AMBCTL_RAT  ((CONFIG_FLASH_SPEED_BRAT * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
 #define flash_EBIU_AMBCTL_HT   ((CONFIG_FLASH_SPEED_BHT  * 4) / (4000000000 / CONFIG_SCLK_HZ))
@@ -308,3 +361,4 @@
 #define flash_EBIU_AMBCTL0  \
 	(flash_EBIU_AMBCTL0_WAT | flash_EBIU_AMBCTL0_RAT | flash_EBIU_AMBCTL0_HT | \
 	 flash_EBIU_AMBCTL0_ST | flash_EBIU_AMBCTL0_TT | CONFIG_FLASH_SPEED_RDYEN)
+#endif
diff --git a/arch/blackfin/include/asm/mem_map.h b/arch/blackfin/include/asm/mem_map.h
index 88d04a707708..e92b31051bb7 100644
--- a/arch/blackfin/include/asm/mem_map.h
+++ b/arch/blackfin/include/asm/mem_map.h
@@ -9,4 +9,79 @@
 
 #include <mach/mem_map.h>
 
+#ifndef __ASSEMBLY__
+
+#ifdef CONFIG_SMP
+static inline ulong get_l1_scratch_start_cpu(int cpu)
+{
+	return (cpu) ? COREB_L1_SCRATCH_START : COREA_L1_SCRATCH_START;
+}
+static inline ulong get_l1_code_start_cpu(int cpu)
+{
+	return (cpu) ? COREB_L1_CODE_START : COREA_L1_CODE_START;
+}
+static inline ulong get_l1_data_a_start_cpu(int cpu)
+{
+	return (cpu) ? COREB_L1_DATA_A_START : COREA_L1_DATA_A_START;
+}
+static inline ulong get_l1_data_b_start_cpu(int cpu)
+{
+	return (cpu) ? COREB_L1_DATA_B_START : COREA_L1_DATA_B_START;
+}
+
+static inline ulong get_l1_scratch_start(void)
+{
+	return get_l1_scratch_start_cpu(blackfin_core_id());
+}
+static inline ulong get_l1_code_start(void)
+{
+	return get_l1_code_start_cpu(blackfin_core_id());
+}
+static inline ulong get_l1_data_a_start(void)
+{
+	return get_l1_data_a_start_cpu(blackfin_core_id());
+}
+static inline ulong get_l1_data_b_start(void)
+{
+	return get_l1_data_b_start_cpu(blackfin_core_id());
+}
+
+#else /* !CONFIG_SMP */
+
+static inline ulong get_l1_scratch_start_cpu(int cpu)
+{
+	return L1_SCRATCH_START;
+}
+static inline ulong get_l1_code_start_cpu(int cpu)
+{
+	return L1_CODE_START;
+}
+static inline ulong get_l1_data_a_start_cpu(int cpu)
+{
+	return L1_DATA_A_START;
+}
+static inline ulong get_l1_data_b_start_cpu(int cpu)
+{
+	return L1_DATA_B_START;
+}
+static inline ulong get_l1_scratch_start(void)
+{
+	return get_l1_scratch_start_cpu(0);
+}
+static inline ulong get_l1_code_start(void)
+{
+	return  get_l1_code_start_cpu(0);
+}
+static inline ulong get_l1_data_a_start(void)
+{
+	return get_l1_data_a_start_cpu(0);
+}
+static inline ulong get_l1_data_b_start(void)
+{
+	return get_l1_data_b_start_cpu(0);
+}
+
+#endif /* CONFIG_SMP */
+#endif /* __ASSEMBLY__ */
+
 #endif				/* _MEM_MAP_H_ */
diff --git a/arch/blackfin/include/asm/mmu_context.h b/arch/blackfin/include/asm/mmu_context.h
index 35593dda2a4d..944e29faae48 100644
--- a/arch/blackfin/include/asm/mmu_context.h
+++ b/arch/blackfin/include/asm/mmu_context.h
@@ -37,6 +37,10 @@
 #include <asm/pgalloc.h>
 #include <asm/cplbinit.h>
 
+/* Note: L1 stacks are CPU-private things, so we bluntly disable this
+   feature in SMP mode, and use the per-CPU scratch SRAM bank only to
+   store the PDA instead. */
+
 extern void *current_l1_stack_save;
 extern int nr_l1stack_tasks;
 extern void *l1_stack_base;
@@ -88,12 +92,15 @@ activate_l1stack(struct mm_struct *mm, unsigned long sp_base)
 static inline void switch_mm(struct mm_struct *prev_mm, struct mm_struct *next_mm,
 			     struct task_struct *tsk)
 {
+#ifdef CONFIG_MPU
+	unsigned int cpu = smp_processor_id();
+#endif
 	if (prev_mm == next_mm)
 		return;
 #ifdef CONFIG_MPU
-	if (prev_mm->context.page_rwx_mask == current_rwx_mask) {
-		flush_switched_cplbs();
-		set_mask_dcplbs(next_mm->context.page_rwx_mask);
+	if (prev_mm->context.page_rwx_mask == current_rwx_mask[cpu]) {
+		flush_switched_cplbs(cpu);
+		set_mask_dcplbs(next_mm->context.page_rwx_mask, cpu);
 	}
 #endif
 
@@ -138,9 +145,10 @@ static inline void protect_page(struct mm_struct *mm, unsigned long addr,
 
 static inline void update_protections(struct mm_struct *mm)
 {
-	if (mm->context.page_rwx_mask == current_rwx_mask) {
-		flush_switched_cplbs();
-		set_mask_dcplbs(mm->context.page_rwx_mask);
+	unsigned int cpu = smp_processor_id();
+	if (mm->context.page_rwx_mask == current_rwx_mask[cpu]) {
+		flush_switched_cplbs(cpu);
+		set_mask_dcplbs(mm->context.page_rwx_mask, cpu);
 	}
 }
 #endif
@@ -165,6 +173,9 @@ init_new_context(struct task_struct *tsk, struct mm_struct *mm)
 static inline void destroy_context(struct mm_struct *mm)
 {
 	struct sram_list_struct *tmp;
+#ifdef CONFIG_MPU
+	unsigned int cpu = smp_processor_id();
+#endif
 
 #ifdef CONFIG_APP_STACK_L1
 	if (current_l1_stack_save == mm->context.l1_stack_save)
@@ -179,8 +190,8 @@ static inline void destroy_context(struct mm_struct *mm)
 		kfree(tmp);
 	}
 #ifdef CONFIG_MPU
-	if (current_rwx_mask == mm->context.page_rwx_mask)
-		current_rwx_mask = NULL;
+	if (current_rwx_mask[cpu] == mm->context.page_rwx_mask)
+		current_rwx_mask[cpu] = NULL;
 	free_pages((unsigned long)mm->context.page_rwx_mask, page_mask_order);
 #endif
 }
diff --git a/arch/blackfin/include/asm/mutex-dec.h b/arch/blackfin/include/asm/mutex-dec.h
new file mode 100644
index 000000000000..0134151656af
--- /dev/null
+++ b/arch/blackfin/include/asm/mutex-dec.h
@@ -0,0 +1,112 @@
+/*
+ * include/asm-generic/mutex-dec.h
+ *
+ * Generic implementation of the mutex fastpath, based on atomic
+ * decrement/increment.
+ */
+#ifndef _ASM_GENERIC_MUTEX_DEC_H
+#define _ASM_GENERIC_MUTEX_DEC_H
+
+/**
+ *  __mutex_fastpath_lock - try to take the lock by moving the count
+ *                          from 1 to a 0 value
+ *  @count: pointer of type atomic_t
+ *  @fail_fn: function to call if the original value was not 1
+ *
+ * Change the count from 1 to a value lower than 1, and call <fail_fn> if
+ * it wasn't 1 originally. This function MUST leave the value lower than
+ * 1 even when the "1" assertion wasn't true.
+ */
+static inline void
+__mutex_fastpath_lock(atomic_t *count, fastcall void (*fail_fn)(atomic_t *))
+{
+	if (unlikely(atomic_dec_return(count) < 0))
+		fail_fn(count);
+	else
+		smp_mb();
+}
+
+/**
+ *  __mutex_fastpath_lock_retval - try to take the lock by moving the count
+ *                                 from 1 to a 0 value
+ *  @count: pointer of type atomic_t
+ *  @fail_fn: function to call if the original value was not 1
+ *
+ * Change the count from 1 to a value lower than 1, and call <fail_fn> if
+ * it wasn't 1 originally. This function returns 0 if the fastpath succeeds,
+ * or anything the slow path function returns.
+ */
+static inline int
+__mutex_fastpath_lock_retval(atomic_t *count, fastcall int (*fail_fn)(atomic_t *))
+{
+	if (unlikely(atomic_dec_return(count) < 0))
+		return fail_fn(count);
+	else {
+		smp_mb();
+		return 0;
+	}
+}
+
+/**
+ *  __mutex_fastpath_unlock - try to promote the count from 0 to 1
+ *  @count: pointer of type atomic_t
+ *  @fail_fn: function to call if the original value was not 0
+ *
+ * Try to promote the count from 0 to 1. If it wasn't 0, call <fail_fn>.
+ * In the failure case, this function is allowed to either set the value to
+ * 1, or to set it to a value lower than 1.
+ *
+ * If the implementation sets it to a value of lower than 1, then the
+ * __mutex_slowpath_needs_to_unlock() macro needs to return 1, it needs
+ * to return 0 otherwise.
+ */
+static inline void
+__mutex_fastpath_unlock(atomic_t *count, fastcall void (*fail_fn)(atomic_t *))
+{
+	smp_mb();
+	if (unlikely(atomic_inc_return(count) <= 0))
+		fail_fn(count);
+}
+
+#define __mutex_slowpath_needs_to_unlock()		1
+
+/**
+ * __mutex_fastpath_trylock - try to acquire the mutex, without waiting
+ *
+ *  @count: pointer of type atomic_t
+ *  @fail_fn: fallback function
+ *
+ * Change the count from 1 to a value lower than 1, and return 0 (failure)
+ * if it wasn't 1 originally, or return 1 (success) otherwise. This function
+ * MUST leave the value lower than 1 even when the "1" assertion wasn't true.
+ * Additionally, if the value was < 0 originally, this function must not leave
+ * it to 0 on failure.
+ *
+ * If the architecture has no effective trylock variant, it should call the
+ * <fail_fn> spinlock-based trylock variant unconditionally.
+ */
+static inline int
+__mutex_fastpath_trylock(atomic_t *count, int (*fail_fn)(atomic_t *))
+{
+	/*
+	 * We have two variants here. The cmpxchg based one is the best one
+	 * because it never induce a false contention state.  It is included
+	 * here because architectures using the inc/dec algorithms over the
+	 * xchg ones are much more likely to support cmpxchg natively.
+	 *
+	 * If not we fall back to the spinlock based variant - that is
+	 * just as efficient (and simpler) as a 'destructive' probing of
+	 * the mutex state would be.
+	 */
+#ifdef __HAVE_ARCH_CMPXCHG
+	if (likely(atomic_cmpxchg(count, 1, 0) == 1)) {
+		smp_mb();
+		return 1;
+	}
+	return 0;
+#else
+	return fail_fn(count);
+#endif
+}
+
+#endif
diff --git a/arch/blackfin/include/asm/mutex.h b/arch/blackfin/include/asm/mutex.h
index 458c1f7fbc18..5d399256bf06 100644
--- a/arch/blackfin/include/asm/mutex.h
+++ b/arch/blackfin/include/asm/mutex.h
@@ -6,4 +6,67 @@
  * implementation. (see asm-generic/mutex-xchg.h for details)
  */
 
+#ifndef _ASM_MUTEX_H
+#define _ASM_MUTEX_H
+
+#ifndef CONFIG_SMP
 #include <asm-generic/mutex-dec.h>
+#else
+
+static inline void
+__mutex_fastpath_lock(atomic_t *count, void (*fail_fn)(atomic_t *))
+{
+	if (unlikely(atomic_dec_return(count) < 0))
+		fail_fn(count);
+	else
+		smp_mb();
+}
+
+static inline int
+__mutex_fastpath_lock_retval(atomic_t *count, int (*fail_fn)(atomic_t *))
+{
+	if (unlikely(atomic_dec_return(count) < 0))
+		return fail_fn(count);
+	else {
+		smp_mb();
+		return 0;
+	}
+}
+
+static inline void
+__mutex_fastpath_unlock(atomic_t *count, void (*fail_fn)(atomic_t *))
+{
+	smp_mb();
+	if (unlikely(atomic_inc_return(count) <= 0))
+		fail_fn(count);
+}
+
+#define __mutex_slowpath_needs_to_unlock()		1
+
+static inline int
+__mutex_fastpath_trylock(atomic_t *count, int (*fail_fn)(atomic_t *))
+{
+	/*
+	 * We have two variants here. The cmpxchg based one is the best one
+	 * because it never induce a false contention state.  It is included
+	 * here because architectures using the inc/dec algorithms over the
+	 * xchg ones are much more likely to support cmpxchg natively.
+	 *
+	 * If not we fall back to the spinlock based variant - that is
+	 * just as efficient (and simpler) as a 'destructive' probing of
+	 * the mutex state would be.
+	 */
+#ifdef __HAVE_ARCH_CMPXCHG
+	if (likely(atomic_cmpxchg(count, 1, 0) == 1)) {
+		smp_mb();
+		return 1;
+	}
+	return 0;
+#else
+	return fail_fn(count);
+#endif
+}
+
+#endif
+
+#endif
diff --git a/arch/blackfin/include/asm/pda.h b/arch/blackfin/include/asm/pda.h
new file mode 100644
index 000000000000..bd8d4a7efeb2
--- /dev/null
+++ b/arch/blackfin/include/asm/pda.h
@@ -0,0 +1,70 @@
+/*
+ * File:         arch/blackfin/include/asm/pda.h
+ * Author:       Philippe Gerum <rpm@xenomai.org>
+ *
+ *               Copyright 2007 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 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 the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#ifndef _ASM_BLACKFIN_PDA_H
+#define _ASM_BLACKFIN_PDA_H
+
+#include <mach/anomaly.h>
+
+#ifndef __ASSEMBLY__
+
+struct blackfin_pda {			/* Per-processor Data Area */
+	struct blackfin_pda *next;
+
+	unsigned long syscfg;
+#ifdef CONFIG_SMP
+	unsigned long imask;		/* Current IMASK value */
+#endif
+
+	unsigned long *ipdt;		/* Start of switchable I-CPLB table */
+	unsigned long *ipdt_swapcount;	/* Number of swaps in ipdt */
+	unsigned long *dpdt;		/* Start of switchable D-CPLB table */
+	unsigned long *dpdt_swapcount;	/* Number of swaps in dpdt */
+
+	/*
+	 * Single instructions can have multiple faults, which
+	 * need to be handled by traps.c, in irq5. We store
+	 * the exception cause to ensure we don't miss a
+	 * double fault condition
+	 */
+	unsigned long ex_iptr;
+	unsigned long ex_optr;
+	unsigned long ex_buf[4];
+	unsigned long ex_imask;		/* Saved imask from exception */
+	unsigned long *ex_stack;	/* Exception stack space */
+
+#ifdef ANOMALY_05000261
+	unsigned long last_cplb_fault_retx;
+#endif
+	unsigned long dcplb_fault_addr;
+	unsigned long icplb_fault_addr;
+	unsigned long retx;
+	unsigned long seqstat;
+};
+
+extern struct blackfin_pda cpu_pda[];
+
+void reserve_pda(void);
+
+#endif	/* __ASSEMBLY__ */
+
+#endif /* _ASM_BLACKFIN_PDA_H */
diff --git a/arch/blackfin/include/asm/percpu.h b/arch/blackfin/include/asm/percpu.h
index 78dd61f6b39f..797c0c165069 100644
--- a/arch/blackfin/include/asm/percpu.h
+++ b/arch/blackfin/include/asm/percpu.h
@@ -3,4 +3,14 @@
 
 #include <asm-generic/percpu.h>
 
-#endif				/* __ARCH_BLACKFIN_PERCPU__ */
+#ifdef CONFIG_MODULES
+#define PERCPU_MODULE_RESERVE 8192
+#else
+#define PERCPU_MODULE_RESERVE 0
+#endif
+
+#define PERCPU_ENOUGH_ROOM \
+	(ALIGN(__per_cpu_end - __per_cpu_start, SMP_CACHE_BYTES) + \
+	 PERCPU_MODULE_RESERVE)
+
+#endif	/* __ARCH_BLACKFIN_PERCPU__ */
diff --git a/arch/blackfin/include/asm/pgtable.h b/arch/blackfin/include/asm/pgtable.h
index f11684e4ade7..783c8f7f8f8c 100644
--- a/arch/blackfin/include/asm/pgtable.h
+++ b/arch/blackfin/include/asm/pgtable.h
@@ -29,6 +29,7 @@ typedef pte_t *pte_addr_t;
 #define PAGE_COPY		__pgprot(0)	/* these mean nothing to NO_MM */
 #define PAGE_READONLY		__pgprot(0)	/* these mean nothing to NO_MM */
 #define PAGE_KERNEL		__pgprot(0)	/* these mean nothing to NO_MM */
+#define pgprot_noncached(prot)	(prot)
 
 extern void paging_init(void);
 
diff --git a/arch/blackfin/include/asm/processor.h b/arch/blackfin/include/asm/processor.h
index e3e9b41fa8db..0eece23b41c7 100644
--- a/arch/blackfin/include/asm/processor.h
+++ b/arch/blackfin/include/asm/processor.h
@@ -24,6 +24,14 @@ static inline void wrusp(unsigned long usp)
 	__asm__ __volatile__("usp = %0;\n\t"::"da"(usp));
 }
 
+static inline unsigned long __get_SP(void)
+{
+	unsigned long sp;
+
+	__asm__ __volatile__("%0 = sp;\n\t" : "=da"(sp));
+	return sp;
+}
+
 /*
  * User space process size: 1st byte beyond user address space.
  * Fairly meaningless on nommu.  Parts of user programs can be scattered
@@ -57,6 +65,7 @@ struct thread_struct {
  * pass the data segment into user programs if it exists,
  * it can't hurt anything as far as I can tell
  */
+#ifndef CONFIG_SMP
 #define start_thread(_regs, _pc, _usp)					\
 do {									\
 	set_fs(USER_DS);						\
@@ -70,6 +79,16 @@ do {									\
 		sizeof(*L1_SCRATCH_TASK_INFO));				\
 	wrusp(_usp);							\
 } while(0)
+#else
+#define start_thread(_regs, _pc, _usp)					\
+do {									\
+	set_fs(USER_DS);						\
+	(_regs)->pc = (_pc);						\
+	if (current->mm)						\
+		(_regs)->p5 = current->mm->start_data;			\
+	wrusp(_usp);							\
+} while (0)
+#endif
 
 /* Forward declaration, a strange C thing */
 struct task_struct;
@@ -106,7 +125,8 @@ unsigned long get_wchan(struct task_struct *p);
 	eip; })
 #define	KSTK_ESP(tsk)	((tsk) == current ? rdusp() : (tsk)->thread.usp)
 
-#define cpu_relax()    	barrier()
+#define cpu_relax()    	smp_mb()
+
 
 /* Get the Silicon Revision of the chip */
 static inline uint32_t __pure bfin_revid(void)
@@ -137,7 +157,11 @@ static inline uint32_t __pure bfin_revid(void)
 static inline uint16_t __pure bfin_cpuid(void)
 {
 	return (bfin_read_CHIPID() & CHIPID_FAMILY) >> 12;
+}
 
+static inline uint32_t __pure bfin_dspid(void)
+{
+	return bfin_read_DSPID();
 }
 
 static inline uint32_t __pure bfin_compiled_revid(void)
@@ -154,6 +178,8 @@ static inline uint32_t __pure bfin_compiled_revid(void)
 	return 4;
 #elif defined(CONFIG_BF_REV_0_5)
 	return 5;
+#elif defined(CONFIG_BF_REV_0_6)
+	return 6;
 #elif defined(CONFIG_BF_REV_ANY)
 	return 0xffff;
 #else
diff --git a/arch/blackfin/include/asm/reboot.h b/arch/blackfin/include/asm/reboot.h
index 6d448b5f5985..4856d62b7467 100644
--- a/arch/blackfin/include/asm/reboot.h
+++ b/arch/blackfin/include/asm/reboot.h
@@ -1,7 +1,7 @@
 /*
- * include/asm-blackfin/reboot.h - shutdown/reboot header
+ * reboot.h - shutdown/reboot header
  *
- * Copyright 2004-2007 Analog Devices Inc.
+ * Copyright 2004-2008 Analog Devices Inc.
  *
  * Licensed under the GPL-2 or later.
  */
diff --git a/arch/blackfin/include/asm/rwlock.h b/arch/blackfin/include/asm/rwlock.h
new file mode 100644
index 000000000000..4a724b378971
--- /dev/null
+++ b/arch/blackfin/include/asm/rwlock.h
@@ -0,0 +1,6 @@
+#ifndef _ASM_BLACKFIN_RWLOCK_H
+#define _ASM_BLACKFIN_RWLOCK_H
+
+#define RW_LOCK_BIAS	0x01000000
+
+#endif
diff --git a/arch/blackfin/include/asm/serial.h b/arch/blackfin/include/asm/serial.h
index 994dd869558c..3a47606c858b 100644
--- a/arch/blackfin/include/asm/serial.h
+++ b/arch/blackfin/include/asm/serial.h
@@ -3,3 +3,4 @@
  */
 
 #define SERIAL_EXTRA_IRQ_FLAGS IRQF_TRIGGER_HIGH
+#define BASE_BAUD (1843200 / 16)
diff --git a/arch/blackfin/include/asm/smp.h b/arch/blackfin/include/asm/smp.h
new file mode 100644
index 000000000000..118deeeae7c0
--- /dev/null
+++ b/arch/blackfin/include/asm/smp.h
@@ -0,0 +1,44 @@
+/*
+ * File:         arch/blackfin/include/asm/smp.h
+ * Author:       Philippe Gerum <rpm@xenomai.org>
+ *
+ *               Copyright 2007 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 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 the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#ifndef __ASM_BLACKFIN_SMP_H
+#define __ASM_BLACKFIN_SMP_H
+
+#include <linux/kernel.h>
+#include <linux/threads.h>
+#include <linux/cpumask.h>
+#include <linux/cache.h>
+#include <asm/blackfin.h>
+#include <mach/smp.h>
+
+#define raw_smp_processor_id()  blackfin_core_id()
+
+extern char coreb_trampoline_start, coreb_trampoline_end;
+
+struct corelock_slot {
+	int lock;
+};
+
+void smp_icache_flush_range_others(unsigned long start,
+				   unsigned long end);
+
+#endif /* !__ASM_BLACKFIN_SMP_H */
diff --git a/arch/blackfin/include/asm/spinlock.h b/arch/blackfin/include/asm/spinlock.h
index 64e908a50646..0249ac319476 100644
--- a/arch/blackfin/include/asm/spinlock.h
+++ b/arch/blackfin/include/asm/spinlock.h
@@ -1,6 +1,89 @@
 #ifndef __BFIN_SPINLOCK_H
 #define __BFIN_SPINLOCK_H
 
-#error blackfin architecture does not support SMP spin lock yet
+#include <asm/atomic.h>
 
-#endif
+asmlinkage int __raw_spin_is_locked_asm(volatile int *ptr);
+asmlinkage void __raw_spin_lock_asm(volatile int *ptr);
+asmlinkage int __raw_spin_trylock_asm(volatile int *ptr);
+asmlinkage void __raw_spin_unlock_asm(volatile int *ptr);
+asmlinkage void __raw_read_lock_asm(volatile int *ptr);
+asmlinkage int __raw_read_trylock_asm(volatile int *ptr);
+asmlinkage void __raw_read_unlock_asm(volatile int *ptr);
+asmlinkage void __raw_write_lock_asm(volatile int *ptr);
+asmlinkage int __raw_write_trylock_asm(volatile int *ptr);
+asmlinkage void __raw_write_unlock_asm(volatile int *ptr);
+
+static inline int __raw_spin_is_locked(raw_spinlock_t *lock)
+{
+	return __raw_spin_is_locked_asm(&lock->lock);
+}
+
+static inline void __raw_spin_lock(raw_spinlock_t *lock)
+{
+	__raw_spin_lock_asm(&lock->lock);
+}
+
+#define __raw_spin_lock_flags(lock, flags) __raw_spin_lock(lock)
+
+static inline int __raw_spin_trylock(raw_spinlock_t *lock)
+{
+	return __raw_spin_trylock_asm(&lock->lock);
+}
+
+static inline void __raw_spin_unlock(raw_spinlock_t *lock)
+{
+	__raw_spin_unlock_asm(&lock->lock);
+}
+
+static inline void __raw_spin_unlock_wait(raw_spinlock_t *lock)
+{
+	while (__raw_spin_is_locked(lock))
+		cpu_relax();
+}
+
+static inline int __raw_read_can_lock(raw_rwlock_t *rw)
+{
+	return __raw_uncached_fetch_asm(&rw->lock) > 0;
+}
+
+static inline int __raw_write_can_lock(raw_rwlock_t *rw)
+{
+	return __raw_uncached_fetch_asm(&rw->lock) == RW_LOCK_BIAS;
+}
+
+static inline void __raw_read_lock(raw_rwlock_t *rw)
+{
+	__raw_read_lock_asm(&rw->lock);
+}
+
+static inline int __raw_read_trylock(raw_rwlock_t *rw)
+{
+	return __raw_read_trylock_asm(&rw->lock);
+}
+
+static inline void __raw_read_unlock(raw_rwlock_t *rw)
+{
+	__raw_read_unlock_asm(&rw->lock);
+}
+
+static inline void __raw_write_lock(raw_rwlock_t *rw)
+{
+	__raw_write_lock_asm(&rw->lock);
+}
+
+static inline int __raw_write_trylock(raw_rwlock_t *rw)
+{
+	return __raw_write_trylock_asm(&rw->lock);
+}
+
+static inline void __raw_write_unlock(raw_rwlock_t *rw)
+{
+	__raw_write_unlock_asm(&rw->lock);
+}
+
+#define _raw_spin_relax(lock)  	cpu_relax()
+#define _raw_read_relax(lock)	cpu_relax()
+#define _raw_write_relax(lock)	cpu_relax()
+
+#endif /*  !__BFIN_SPINLOCK_H */
diff --git a/arch/blackfin/include/asm/spinlock_types.h b/arch/blackfin/include/asm/spinlock_types.h
new file mode 100644
index 000000000000..b1e3c4c7b382
--- /dev/null
+++ b/arch/blackfin/include/asm/spinlock_types.h
@@ -0,0 +1,22 @@
+#ifndef __ASM_SPINLOCK_TYPES_H
+#define __ASM_SPINLOCK_TYPES_H
+
+#ifndef __LINUX_SPINLOCK_TYPES_H
+# error "please don't include this file directly"
+#endif
+
+#include <asm/rwlock.h>
+
+typedef struct {
+	volatile unsigned int lock;
+} raw_spinlock_t;
+
+#define __RAW_SPIN_LOCK_UNLOCKED	{ 0 }
+
+typedef struct {
+	volatile unsigned int lock;
+} raw_rwlock_t;
+
+#define __RAW_RW_LOCK_UNLOCKED		{ RW_LOCK_BIAS }
+
+#endif
diff --git a/arch/blackfin/include/asm/swab.h b/arch/blackfin/include/asm/swab.h
new file mode 100644
index 000000000000..69a051b612bd
--- /dev/null
+++ b/arch/blackfin/include/asm/swab.h
@@ -0,0 +1,48 @@
+#ifndef _BLACKFIN_SWAB_H
+#define _BLACKFIN_SWAB_H
+
+#include <asm/types.h>
+#include <linux/compiler.h>
+
+#if defined(__GNUC__) && !defined(__STRICT_ANSI__) || defined(__KERNEL__)
+#  define __SWAB_64_THRU_32__
+#endif
+
+#ifdef __GNUC__
+
+static __inline__ __attribute_const__ __u32 __arch_swahb32(__u32 xx)
+{
+	__u32 tmp;
+	__asm__("%1 = %0 >> 8 (V);\n\t"
+		"%0 = %0 << 8 (V);\n\t"
+		"%0 = %0 | %1;\n\t"
+		: "+d"(xx), "=&d"(tmp));
+	return xx;
+}
+#define __arch_swahb32 __arch_swahb32
+
+static __inline__ __attribute_const__ __u32 __arch_swahw32(__u32 xx)
+{
+	__u32 rv;
+	__asm__("%0 = PACK(%1.L, %1.H);\n\t": "=d"(rv): "d"(xx));
+	return rv;
+}
+#define __arch_swahw32 __arch_swahw32
+
+static __inline__ __attribute_const__ __u32 __arch_swab32(__u32 xx)
+{
+	return __arch_swahb32(__arch_swahw32(xx));
+}
+#define __arch_swab32 __arch_swab32
+
+static __inline__ __attribute_const__ __u16 __arch_swab16(__u16 xx)
+{
+	__u32 xw = xx;
+	__asm__("%0 <<= 8;\n	%0.L = %0.L + %0.H (NS);\n": "+d"(xw));
+	return (__u16)xw;
+}
+#define __arch_swab16 __arch_swab16
+
+#endif /* __GNUC__ */
+
+#endif				/* _BLACKFIN_SWAB_H */
diff --git a/arch/blackfin/include/asm/system.h b/arch/blackfin/include/asm/system.h
index 8f1627d8bf09..a4c8254bec55 100644
--- a/arch/blackfin/include/asm/system.h
+++ b/arch/blackfin/include/asm/system.h
@@ -37,114 +37,98 @@
 #include <linux/linkage.h>
 #include <linux/compiler.h>
 #include <mach/anomaly.h>
+#include <asm/pda.h>
+#include <asm/processor.h>
+#include <asm/irq.h>
 
 /*
- * Interrupt configuring macros.
+ * Force strict CPU ordering.
  */
+#define nop()  __asm__ __volatile__ ("nop;\n\t" : : )
+#define mb()   __asm__ __volatile__ (""   : : : "memory")
+#define rmb()  __asm__ __volatile__ (""   : : : "memory")
+#define wmb()  __asm__ __volatile__ (""   : : : "memory")
+#define set_mb(var, value) do { (void) xchg(&var, value); } while (0)
+#define read_barrier_depends() 		do { } while(0)
 
-extern unsigned long irq_flags;
-
-#define local_irq_enable() \
-	__asm__ __volatile__( \
-		"sti %0;" \
-		: \
-		: "d" (irq_flags) \
-	)
-
-#define local_irq_disable() \
-	do { \
-		int __tmp_dummy; \
-		__asm__ __volatile__( \
-			"cli %0;" \
-			: "=d" (__tmp_dummy) \
-		); \
-	} while (0)
-
-#if ANOMALY_05000244 && defined(CONFIG_BFIN_ICACHE)
-# define NOP_PAD_ANOMALY_05000244 "nop; nop;"
-#else
-# define NOP_PAD_ANOMALY_05000244
-#endif
-
-#define idle_with_irq_disabled() \
-	__asm__ __volatile__( \
-		NOP_PAD_ANOMALY_05000244 \
-		".align 8;" \
-		"sti %0;" \
-		"idle;" \
-		: \
-		: "d" (irq_flags) \
-	)
-
-#ifdef CONFIG_DEBUG_HWERR
-# define __save_and_cli(x) \
-	__asm__ __volatile__( \
-		"cli %0;" \
-		"sti %1;" \
-		: "=&d" (x) \
-		: "d" (0x3F) \
-	)
-#else
-# define __save_and_cli(x) \
-	__asm__ __volatile__( \
-		"cli %0;" \
-		: "=&d" (x) \
-	)
-#endif
-
-#define local_save_flags(x) \
-	__asm__ __volatile__( \
-		"cli %0;" \
-		"sti %0;" \
-		: "=d" (x) \
-	)
+#ifdef CONFIG_SMP
+asmlinkage unsigned long __raw_xchg_1_asm(volatile void *ptr, unsigned long value);
+asmlinkage unsigned long __raw_xchg_2_asm(volatile void *ptr, unsigned long value);
+asmlinkage unsigned long __raw_xchg_4_asm(volatile void *ptr, unsigned long value);
+asmlinkage unsigned long __raw_cmpxchg_1_asm(volatile void *ptr,
+					unsigned long new, unsigned long old);
+asmlinkage unsigned long __raw_cmpxchg_2_asm(volatile void *ptr,
+					unsigned long new, unsigned long old);
+asmlinkage unsigned long __raw_cmpxchg_4_asm(volatile void *ptr,
+					unsigned long new, unsigned long old);
+
+#ifdef __ARCH_SYNC_CORE_DCACHE
+# define smp_mb()	do { barrier(); smp_check_barrier(); smp_mark_barrier(); } while (0)
+# define smp_rmb()	do { barrier(); smp_check_barrier(); } while (0)
+# define smp_wmb()	do { barrier(); smp_mark_barrier(); } while (0)
+#define smp_read_barrier_depends()	do { barrier(); smp_check_barrier(); } while (0)
 
-#ifdef CONFIG_DEBUG_HWERR
-#define irqs_enabled_from_flags(x) (((x) & ~0x3f) != 0)
 #else
-#define irqs_enabled_from_flags(x) ((x) != 0x1f)
+# define smp_mb()	barrier()
+# define smp_rmb()	barrier()
+# define smp_wmb()	barrier()
+#define smp_read_barrier_depends()	barrier()
 #endif
 
-#define local_irq_restore(x) \
-	do { \
-		if (irqs_enabled_from_flags(x)) \
-			local_irq_enable(); \
-	} while (0)
+static inline unsigned long __xchg(unsigned long x, volatile void *ptr,
+				   int size)
+{
+	unsigned long tmp;
 
-/* For spinlocks etc */
-#define local_irq_save(x) __save_and_cli(x)
+	switch (size) {
+	case 1:
+		tmp = __raw_xchg_1_asm(ptr, x);
+		break;
+	case 2:
+		tmp = __raw_xchg_2_asm(ptr, x);
+		break;
+	case 4:
+		tmp = __raw_xchg_4_asm(ptr, x);
+		break;
+	}
 
-#define	irqs_disabled()				\
-({						\
-	unsigned long flags;			\
-	local_save_flags(flags);		\
-	!irqs_enabled_from_flags(flags);	\
-})
+	return tmp;
+}
 
 /*
- * Force strict CPU ordering.
+ * Atomic compare and exchange.  Compare OLD with MEM, if identical,
+ * store NEW in MEM.  Return the initial value in MEM.  Success is
+ * indicated by comparing RETURN with OLD.
  */
-#define nop()  asm volatile ("nop;\n\t"::)
-#define mb()   asm volatile (""   : : :"memory")
-#define rmb()  asm volatile (""   : : :"memory")
-#define wmb()  asm volatile (""   : : :"memory")
-#define set_mb(var, value) do { (void) xchg(&var, value); } while (0)
+static inline unsigned long __cmpxchg(volatile void *ptr, unsigned long old,
+				      unsigned long new, int size)
+{
+	unsigned long tmp;
 
-#define read_barrier_depends() 		do { } while(0)
+	switch (size) {
+	case 1:
+		tmp = __raw_cmpxchg_1_asm(ptr, new, old);
+		break;
+	case 2:
+		tmp = __raw_cmpxchg_2_asm(ptr, new, old);
+		break;
+	case 4:
+		tmp = __raw_cmpxchg_4_asm(ptr, new, old);
+		break;
+	}
+
+	return tmp;
+}
+#define cmpxchg(ptr, o, n) \
+	((__typeof__(*(ptr)))__cmpxchg((ptr), (unsigned long)(o), \
+		(unsigned long)(n), sizeof(*(ptr))))
+
+#else /* !CONFIG_SMP */
 
-#ifdef CONFIG_SMP
-#define smp_mb()	mb()
-#define smp_rmb()	rmb()
-#define smp_wmb()	wmb()
-#define smp_read_barrier_depends()	read_barrier_depends()
-#else
 #define smp_mb()	barrier()
 #define smp_rmb()	barrier()
 #define smp_wmb()	barrier()
 #define smp_read_barrier_depends()	do { } while(0)
-#endif
-
-#define xchg(ptr,x) ((__typeof__(*(ptr)))__xchg((unsigned long)(x),(ptr),sizeof(*(ptr))))
 
 struct __xchg_dummy {
 	unsigned long a[100];
@@ -157,7 +141,7 @@ static inline unsigned long __xchg(unsigned long x, volatile void *ptr,
 	unsigned long tmp = 0;
 	unsigned long flags = 0;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 
 	switch (size) {
 	case 1:
@@ -179,7 +163,7 @@ static inline unsigned long __xchg(unsigned long x, volatile void *ptr,
 			 : "=&d" (tmp) : "d" (x), "m" (*__xg(ptr)) : "memory");
 		break;
 	}
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 	return tmp;
 }
 
@@ -194,9 +178,12 @@ static inline unsigned long __xchg(unsigned long x, volatile void *ptr,
 			(unsigned long)(n), sizeof(*(ptr))))
 #define cmpxchg64_local(ptr, o, n) __cmpxchg64_local_generic((ptr), (o), (n))
 
-#ifndef CONFIG_SMP
 #include <asm-generic/cmpxchg.h>
-#endif
+
+#endif /* !CONFIG_SMP */
+
+#define xchg(ptr, x) ((__typeof__(*(ptr)))__xchg((unsigned long)(x), (ptr), sizeof(*(ptr))))
+#define tas(ptr) ((void)xchg((ptr), 1))
 
 #define prepare_to_switch()     do { } while(0)
 
@@ -205,10 +192,12 @@ static inline unsigned long __xchg(unsigned long x, volatile void *ptr,
  * ptr isn't the current task, in which case it does nothing.
  */
 
-#include <asm/blackfin.h>
+#include <asm/l1layout.h>
+#include <asm/mem_map.h>
 
 asmlinkage struct task_struct *resume(struct task_struct *prev, struct task_struct *next);
 
+#ifndef CONFIG_SMP
 #define switch_to(prev,next,last) \
 do {    \
 	memcpy (&task_thread_info(prev)->l1_task_info, L1_SCRATCH_TASK_INFO, \
@@ -217,5 +206,11 @@ do {    \
 		sizeof *L1_SCRATCH_TASK_INFO); \
 	(last) = resume (prev, next);   \
 } while (0)
+#else
+#define switch_to(prev, next, last) \
+do {    \
+	(last) = resume(prev, next);   \
+} while (0)
+#endif
 
-#endif				/* _BLACKFIN_SYSTEM_H */
+#endif	/* _BLACKFIN_SYSTEM_H */
diff --git a/arch/blackfin/include/asm/thread_info.h b/arch/blackfin/include/asm/thread_info.h
index 642769329d12..e721ce55956c 100644
--- a/arch/blackfin/include/asm/thread_info.h
+++ b/arch/blackfin/include/asm/thread_info.h
@@ -44,6 +44,7 @@
  */
 #define THREAD_SIZE_ORDER	1
 #define THREAD_SIZE		8192	/* 2 pages */
+#define STACK_WARN		(THREAD_SIZE/8)
 
 #ifndef __ASSEMBLY__
 
@@ -62,7 +63,9 @@ struct thread_info {
 	int preempt_count;	/* 0 => preemptable, <0 => BUG */
 	mm_segment_t addr_limit;	/* address limit */
 	struct restart_block restart_block;
+#ifndef CONFIG_SMP
 	struct l1_scratch_task_info l1_task_info;
+#endif
 };
 
 /*
@@ -90,7 +93,7 @@ __attribute_const__
 static inline struct thread_info *current_thread_info(void)
 {
 	struct thread_info *ti;
-      __asm__("%0 = sp;": "=&d"(ti):
+      __asm__("%0 = sp;" : "=da"(ti) :
 	);
 	return (struct thread_info *)((long)ti & ~((long)THREAD_SIZE-1));
 }
diff --git a/arch/blackfin/include/asm/uaccess.h b/arch/blackfin/include/asm/uaccess.h
index d928b8099056..3248033531e6 100644
--- a/arch/blackfin/include/asm/uaccess.h
+++ b/arch/blackfin/include/asm/uaccess.h
@@ -149,54 +149,42 @@ static inline int bad_user_access_length(void)
 		 : /* no outputs */			\
 		 :"d" (x),"a" (__ptr(p)) : "memory")
 
-#define get_user(x,p)							\
-	({								\
-		int _err = 0;						\
-		typeof(*(p)) *_p = (p);					\
-		if (!access_ok(VERIFY_READ, _p, sizeof(*(_p)))) {	\
-			_err = -EFAULT;					\
-		}							\
-		else {							\
-		switch (sizeof(*(_p))) {				\
-		case 1:							\
-			__get_user_asm(x, _p, B,(Z));			\
-			break;						\
-		case 2:							\
-			__get_user_asm(x, _p, W,(Z));			\
-			break;						\
-		case 4:							\
-			__get_user_asm(x, _p,  , );			\
-			break;						\
-		case 8: {						\
-			unsigned long _xl, _xh;				\
-			__get_user_asm(_xl, ((unsigned long *)_p)+0,  , ); \
-			__get_user_asm(_xh, ((unsigned long *)_p)+1,  , ); \
-			((unsigned long *)&x)[0] = _xl;			\
-			((unsigned long *)&x)[1] = _xh;			\
-		} break;						\
-		default:						\
-			x = 0;						\
-			printk(KERN_INFO "get_user_bad: %s:%d %s\n",    \
-			       __FILE__, __LINE__, __func__);	\
-			_err = __get_user_bad();			\
-			break;						\
-		}							\
-		}							\
-		_err;							\
-	})
+#define get_user(x, ptr)					\
+({								\
+	int _err = 0;						\
+	unsigned long _val = 0;					\
+	const typeof(*(ptr)) __user *_p = (ptr);		\
+	const size_t ptr_size = sizeof(*(_p));			\
+	if (likely(access_ok(VERIFY_READ, _p, ptr_size))) {	\
+		BUILD_BUG_ON(ptr_size >= 8);			\
+		switch (ptr_size) {				\
+		case 1:						\
+			__get_user_asm(_val, _p, B,(Z));	\
+			break;					\
+		case 2:						\
+			__get_user_asm(_val, _p, W,(Z));	\
+			break;					\
+		case 4:						\
+			__get_user_asm(_val, _p,  , );		\
+			break;					\
+		}						\
+	} else							\
+		_err = -EFAULT;					\
+	x = (typeof(*(ptr)))_val;				\
+	_err;							\
+})
 
 #define __get_user(x,p) get_user(x,p)
 
 #define __get_user_bad() (bad_user_access_length(), (-EFAULT))
 
-#define __get_user_asm(x,p,bhw,option)				\
-	{							\
-		unsigned long _tmp;				\
-		__asm__ ("%0 =" #bhw "[%1]"#option";\n\t"	\
-			 : "=d" (_tmp)				\
-			 : "a" (__ptr(p)));			\
-		(x) = (__typeof__(*(p))) _tmp;			\
-	}
+#define __get_user_asm(x, ptr, bhw, option)	\
+({						\
+	__asm__ __volatile__ (			\
+		"%0 =" #bhw "[%1]" #option ";"	\
+		: "=d" (x)			\
+		: "a" (__ptr(ptr)));		\
+})
 
 #define __copy_from_user(to, from, n) copy_from_user(to, from, n)
 #define __copy_to_user(to, from, n) copy_to_user(to, from, n)
@@ -209,8 +197,8 @@ static inline int bad_user_access_length(void)
 #define copy_from_user_ret(to,from,n,retval) ({ if (copy_from_user(to,from,n))\
                                                    return retval; })
 
-static inline long copy_from_user(void *to,
-				  const void __user * from, unsigned long n)
+static inline unsigned long __must_check
+copy_from_user(void *to, const void __user *from, unsigned long n)
 {
 	if (access_ok(VERIFY_READ, from, n))
 		memcpy(to, from, n);
@@ -219,8 +207,8 @@ static inline long copy_from_user(void *to,
 	return 0;
 }
 
-static inline long copy_to_user(void *to,
-				const void __user * from, unsigned long n)
+static inline unsigned long __must_check
+copy_to_user(void *to, const void __user *from, unsigned long n)
 {
 	if (access_ok(VERIFY_WRITE, to, n))
 		memcpy(to, from, n);
@@ -233,8 +221,8 @@ static inline long copy_to_user(void *to,
  * Copy a null terminated string from userspace.
  */
 
-static inline long strncpy_from_user(char *dst,
-                                     const char *src, long count)
+static inline long __must_check
+strncpy_from_user(char *dst, const char *src, long count)
 {
 	char *tmp;
 	if (!access_ok(VERIFY_READ, src, 1))
@@ -260,7 +248,8 @@ static inline long strnlen_user(const char *src, long n)
  * Zero Userspace
  */
 
-static inline unsigned long __clear_user(void *to, unsigned long n)
+static inline unsigned long __must_check
+__clear_user(void *to, unsigned long n)
 {
 	memset(to, 0, n);
 	return 0;
diff --git a/arch/blackfin/include/asm/xor.h b/arch/blackfin/include/asm/xor.h
new file mode 100644
index 000000000000..c82eb12a5b18
--- /dev/null
+++ b/arch/blackfin/include/asm/xor.h
@@ -0,0 +1 @@
+#include <asm-generic/xor.h>
diff --git a/arch/blackfin/kernel/Makefile b/arch/blackfin/kernel/Makefile
index 606adc78aa85..38a233374f07 100644
--- a/arch/blackfin/kernel/Makefile
+++ b/arch/blackfin/kernel/Makefile
@@ -7,7 +7,7 @@ extra-y := init_task.o vmlinux.lds
 obj-y := \
 	entry.o process.o bfin_ksyms.o ptrace.o setup.o signal.o \
 	sys_bfin.o traps.o irqchip.o dma-mapping.o flat.o \
-	fixed_code.o reboot.o bfin_gpio.o
+	fixed_code.o reboot.o bfin_gpio.o bfin_dma_5xx.o
 
 ifeq ($(CONFIG_GENERIC_CLOCKEVENTS),y)
     obj-y += time-ts.o
@@ -15,8 +15,11 @@ else
     obj-y += time.o
 endif
 
+obj-$(CONFIG_IPIPE)                  += ipipe.o
+obj-$(CONFIG_IPIPE_TRACE_MCOUNT)     += mcount.o
 obj-$(CONFIG_BFIN_GPTIMERS)          += gptimers.o
+obj-$(CONFIG_CPLB_INFO)              += cplbinfo.o
 obj-$(CONFIG_MODULES)                += module.o
-obj-$(CONFIG_BFIN_DMA_5XX)           += bfin_dma_5xx.o
 obj-$(CONFIG_KGDB)                   += kgdb.o
+obj-$(CONFIG_KGDB_TESTCASE)          += kgdb_test.o
 obj-$(CONFIG_EARLY_PRINTK)           += early_printk.o
diff --git a/arch/blackfin/kernel/asm-offsets.c b/arch/blackfin/kernel/asm-offsets.c
index 9bb85dd5ccb3..b5df9459d6d5 100644
--- a/arch/blackfin/kernel/asm-offsets.c
+++ b/arch/blackfin/kernel/asm-offsets.c
@@ -56,6 +56,9 @@ int main(void)
 	/* offsets into the thread struct */
 	DEFINE(THREAD_KSP, offsetof(struct thread_struct, ksp));
 	DEFINE(THREAD_USP, offsetof(struct thread_struct, usp));
+	DEFINE(THREAD_SR, offsetof(struct thread_struct, seqstat));
+	DEFINE(PT_SR, offsetof(struct thread_struct, seqstat));
+	DEFINE(THREAD_ESP0, offsetof(struct thread_struct, esp0));
 	DEFINE(THREAD_PC, offsetof(struct thread_struct, pc));
 	DEFINE(KERNEL_STACK_SIZE, THREAD_SIZE);
 
@@ -128,5 +131,31 @@ int main(void)
 	DEFINE(SIGSEGV, SIGSEGV);
 	DEFINE(SIGTRAP, SIGTRAP);
 
+	/* PDA management (in L1 scratchpad) */
+	DEFINE(PDA_SYSCFG, offsetof(struct blackfin_pda, syscfg));
+#ifdef CONFIG_SMP
+	DEFINE(PDA_IRQFLAGS, offsetof(struct blackfin_pda, imask));
+#endif
+	DEFINE(PDA_IPDT, offsetof(struct blackfin_pda, ipdt));
+	DEFINE(PDA_IPDT_SWAPCOUNT, offsetof(struct blackfin_pda, ipdt_swapcount));
+	DEFINE(PDA_DPDT, offsetof(struct blackfin_pda, dpdt));
+	DEFINE(PDA_DPDT_SWAPCOUNT, offsetof(struct blackfin_pda, dpdt_swapcount));
+	DEFINE(PDA_EXIPTR, offsetof(struct blackfin_pda, ex_iptr));
+	DEFINE(PDA_EXOPTR, offsetof(struct blackfin_pda, ex_optr));
+	DEFINE(PDA_EXBUF, offsetof(struct blackfin_pda, ex_buf));
+	DEFINE(PDA_EXIMASK, offsetof(struct blackfin_pda, ex_imask));
+	DEFINE(PDA_EXSTACK, offsetof(struct blackfin_pda, ex_stack));
+#ifdef ANOMALY_05000261
+	DEFINE(PDA_LFRETX, offsetof(struct blackfin_pda, last_cplb_fault_retx));
+#endif
+	DEFINE(PDA_DCPLB, offsetof(struct blackfin_pda, dcplb_fault_addr));
+	DEFINE(PDA_ICPLB, offsetof(struct blackfin_pda, icplb_fault_addr));
+	DEFINE(PDA_RETX, offsetof(struct blackfin_pda, retx));
+	DEFINE(PDA_SEQSTAT, offsetof(struct blackfin_pda, seqstat));
+#ifdef CONFIG_SMP
+	/* Inter-core lock (in L2 SRAM) */
+	DEFINE(SIZEOF_CORELOCK, sizeof(struct corelock_slot));
+#endif
+
 	return 0;
 }
diff --git a/arch/blackfin/kernel/bfin_dma_5xx.c b/arch/blackfin/kernel/bfin_dma_5xx.c
index 339293d677cc..07e02c0d1c07 100644
--- a/arch/blackfin/kernel/bfin_dma_5xx.c
+++ b/arch/blackfin/kernel/bfin_dma_5xx.c
@@ -1,63 +1,27 @@
 /*
- * File:         arch/blackfin/kernel/bfin_dma_5xx.c
- * Based on:
- * Author:
+ * bfin_dma_5xx.c - Blackfin DMA implementation
  *
- * Created:
- * Description:  This file contains the simple DMA Implementation for Blackfin
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ * Copyright 2004-2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
  */
 
 #include <linux/errno.h>
-#include <linux/module.h>
-#include <linux/sched.h>
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/param.h>
+#include <linux/proc_fs.h>
+#include <linux/sched.h>
+#include <linux/seq_file.h>
+#include <linux/spinlock.h>
 
 #include <asm/blackfin.h>
-#include <asm/dma.h>
 #include <asm/cacheflush.h>
+#include <asm/dma.h>
+#include <asm/uaccess.h>
 
-/* Remove unused code not exported by symbol or internally called */
-#define REMOVE_DEAD_CODE
-
-/**************************************************************************
- * Global Variables
-***************************************************************************/
-
-static struct dma_channel dma_ch[MAX_BLACKFIN_DMA_CHANNEL];
-
-/*------------------------------------------------------------------------------
- *       Set the Buffer Clear bit in the Configuration register of specific DMA
- *       channel. This will stop the descriptor based DMA operation.
- *-----------------------------------------------------------------------------*/
-static void clear_dma_buffer(unsigned int channel)
-{
-	dma_ch[channel].regs->cfg |= RESTART;
-	SSYNC();
-	dma_ch[channel].regs->cfg &= ~RESTART;
-	SSYNC();
-}
+struct dma_channel dma_ch[MAX_DMA_CHANNELS];
+EXPORT_SYMBOL(dma_ch);
 
 static int __init blackfin_dma_init(void)
 {
@@ -65,32 +29,67 @@ static int __init blackfin_dma_init(void)
 
 	printk(KERN_INFO "Blackfin DMA Controller\n");
 
-	for (i = 0; i < MAX_BLACKFIN_DMA_CHANNEL; i++) {
+	for (i = 0; i < MAX_DMA_CHANNELS; i++) {
 		dma_ch[i].chan_status = DMA_CHANNEL_FREE;
 		dma_ch[i].regs = dma_io_base_addr[i];
 		mutex_init(&(dma_ch[i].dmalock));
 	}
 	/* Mark MEMDMA Channel 0 as requested since we're using it internally */
-	dma_ch[CH_MEM_STREAM0_DEST].chan_status = DMA_CHANNEL_REQUESTED;
-	dma_ch[CH_MEM_STREAM0_SRC].chan_status = DMA_CHANNEL_REQUESTED;
+	request_dma(CH_MEM_STREAM0_DEST, "Blackfin dma_memcpy");
+	request_dma(CH_MEM_STREAM0_SRC, "Blackfin dma_memcpy");
 
 #if defined(CONFIG_DEB_DMA_URGENT)
 	bfin_write_EBIU_DDRQUE(bfin_read_EBIU_DDRQUE()
 			 | DEB1_URGENT | DEB2_URGENT | DEB3_URGENT);
 #endif
+
 	return 0;
 }
-
 arch_initcall(blackfin_dma_init);
 
-/*------------------------------------------------------------------------------
- *	Request the specific DMA channel from the system.
- *-----------------------------------------------------------------------------*/
-int request_dma(unsigned int channel, char *device_id)
+#ifdef CONFIG_PROC_FS
+static int proc_dma_show(struct seq_file *m, void *v)
 {
+	int i;
+
+	for (i = 0; i < MAX_DMA_CHANNELS; ++i)
+		if (dma_ch[i].chan_status != DMA_CHANNEL_FREE)
+			seq_printf(m, "%2d: %s\n", i, dma_ch[i].device_id);
+
+	return 0;
+}
 
+static int proc_dma_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, proc_dma_show, NULL);
+}
+
+static const struct file_operations proc_dma_operations = {
+	.open		= proc_dma_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int __init proc_dma_init(void)
+{
+	return proc_create("dma", 0, NULL, &proc_dma_operations) != NULL;
+}
+late_initcall(proc_dma_init);
+#endif
+
+/**
+ *	request_dma - request a DMA channel
+ *
+ * Request the specific DMA channel from the system if it's available.
+ */
+int request_dma(unsigned int channel, const char *device_id)
+{
 	pr_debug("request_dma() : BEGIN \n");
 
+	if (device_id == NULL)
+		printk(KERN_WARNING "request_dma(%u): no device_id given\n", channel);
+
 #if defined(CONFIG_BF561) && ANOMALY_05000182
 	if (channel >= CH_IMEM_STREAM0_DEST && channel <= CH_IMEM_STREAM1_DEST) {
 		if (get_cclk() > 500000000) {
@@ -129,60 +128,63 @@ int request_dma(unsigned int channel, char *device_id)
 #endif
 
 	dma_ch[channel].device_id = device_id;
-	dma_ch[channel].irq_callback = NULL;
+	dma_ch[channel].irq = 0;
 
 	/* This is to be enabled by putting a restriction -
 	 * you have to request DMA, before doing any operations on
 	 * descriptor/channel
 	 */
 	pr_debug("request_dma() : END  \n");
-	return channel;
+	return 0;
 }
 EXPORT_SYMBOL(request_dma);
 
-int set_dma_callback(unsigned int channel, dma_interrupt_t callback, void *data)
+int set_dma_callback(unsigned int channel, irq_handler_t callback, void *data)
 {
-	int ret_irq = 0;
-
 	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
+	       && channel < MAX_DMA_CHANNELS));
 
 	if (callback != NULL) {
-		int ret_val;
-		ret_irq = channel2irq(channel);
+		int ret;
+		unsigned int irq = channel2irq(channel);
 
-		dma_ch[channel].data = data;
+		ret = request_irq(irq, callback, IRQF_DISABLED,
+			dma_ch[channel].device_id, data);
+		if (ret)
+			return ret;
 
-		ret_val =
-		    request_irq(ret_irq, (void *)callback, IRQF_DISABLED,
-				dma_ch[channel].device_id, data);
-		if (ret_val) {
-			printk(KERN_NOTICE
-			       "Request irq in DMA engine failed.\n");
-			return -EPERM;
-		}
-		dma_ch[channel].irq_callback = callback;
+		dma_ch[channel].irq = irq;
+		dma_ch[channel].data = data;
 	}
 	return 0;
 }
 EXPORT_SYMBOL(set_dma_callback);
 
-void free_dma(unsigned int channel)
+/**
+ *	clear_dma_buffer - clear DMA fifos for specified channel
+ *
+ * Set the Buffer Clear bit in the Configuration register of specific DMA
+ * channel. This will stop the descriptor based DMA operation.
+ */
+static void clear_dma_buffer(unsigned int channel)
 {
-	int ret_irq;
+	dma_ch[channel].regs->cfg |= RESTART;
+	SSYNC();
+	dma_ch[channel].regs->cfg &= ~RESTART;
+}
 
+void free_dma(unsigned int channel)
+{
 	pr_debug("freedma() : BEGIN \n");
 	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
+	       && channel < MAX_DMA_CHANNELS));
 
 	/* Halt the DMA */
 	disable_dma(channel);
 	clear_dma_buffer(channel);
 
-	if (dma_ch[channel].irq_callback != NULL) {
-		ret_irq = channel2irq(channel);
-		free_irq(ret_irq, dma_ch[channel].data);
-	}
+	if (dma_ch[channel].irq)
+		free_irq(dma_ch[channel].irq, dma_ch[channel].data);
 
 	/* Clear the DMA Variable in the Channel */
 	mutex_lock(&(dma_ch[channel].dmalock));
@@ -193,294 +195,15 @@ void free_dma(unsigned int channel)
 }
 EXPORT_SYMBOL(free_dma);
 
-void dma_enable_irq(unsigned int channel)
-{
-	int ret_irq;
-
-	pr_debug("dma_enable_irq() : BEGIN \n");
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	ret_irq = channel2irq(channel);
-	enable_irq(ret_irq);
-}
-EXPORT_SYMBOL(dma_enable_irq);
-
-void dma_disable_irq(unsigned int channel)
-{
-	int ret_irq;
-
-	pr_debug("dma_disable_irq() : BEGIN \n");
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	ret_irq = channel2irq(channel);
-	disable_irq(ret_irq);
-}
-EXPORT_SYMBOL(dma_disable_irq);
-
-int dma_channel_active(unsigned int channel)
-{
-	if (dma_ch[channel].chan_status == DMA_CHANNEL_FREE) {
-		return 0;
-	} else {
-		return 1;
-	}
-}
-EXPORT_SYMBOL(dma_channel_active);
-
-/*------------------------------------------------------------------------------
-*	stop the specific DMA channel.
-*-----------------------------------------------------------------------------*/
-void disable_dma(unsigned int channel)
-{
-	pr_debug("stop_dma() : BEGIN \n");
-
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->cfg &= ~DMAEN;	/* Clean the enable bit */
-	SSYNC();
-	dma_ch[channel].chan_status = DMA_CHANNEL_REQUESTED;
-	/* Needs to be enabled Later */
-	pr_debug("stop_dma() : END \n");
-	return;
-}
-EXPORT_SYMBOL(disable_dma);
-
-void enable_dma(unsigned int channel)
-{
-	pr_debug("enable_dma() : BEGIN \n");
-
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].chan_status = DMA_CHANNEL_ENABLED;
-	dma_ch[channel].regs->curr_x_count = 0;
-	dma_ch[channel].regs->curr_y_count = 0;
-
-	dma_ch[channel].regs->cfg |= DMAEN;	/* Set the enable bit */
-	SSYNC();
-	pr_debug("enable_dma() : END \n");
-	return;
-}
-EXPORT_SYMBOL(enable_dma);
-
-/*------------------------------------------------------------------------------
-*		Set the Start Address register for the specific DMA channel
-* 		This function can be used for register based DMA,
-*		to setup the start address
-*		addr:		Starting address of the DMA Data to be transferred.
-*-----------------------------------------------------------------------------*/
-void set_dma_start_addr(unsigned int channel, unsigned long addr)
-{
-	pr_debug("set_dma_start_addr() : BEGIN \n");
-
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->start_addr = addr;
-	SSYNC();
-	pr_debug("set_dma_start_addr() : END\n");
-}
-EXPORT_SYMBOL(set_dma_start_addr);
-
-void set_dma_next_desc_addr(unsigned int channel, unsigned long addr)
-{
-	pr_debug("set_dma_next_desc_addr() : BEGIN \n");
-
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->next_desc_ptr = addr;
-	SSYNC();
-	pr_debug("set_dma_next_desc_addr() : END\n");
-}
-EXPORT_SYMBOL(set_dma_next_desc_addr);
-
-void set_dma_curr_desc_addr(unsigned int channel, unsigned long addr)
-{
-	pr_debug("set_dma_curr_desc_addr() : BEGIN \n");
-
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->curr_desc_ptr = addr;
-	SSYNC();
-	pr_debug("set_dma_curr_desc_addr() : END\n");
-}
-EXPORT_SYMBOL(set_dma_curr_desc_addr);
-
-void set_dma_x_count(unsigned int channel, unsigned short x_count)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->x_count = x_count;
-	SSYNC();
-}
-EXPORT_SYMBOL(set_dma_x_count);
-
-void set_dma_y_count(unsigned int channel, unsigned short y_count)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->y_count = y_count;
-	SSYNC();
-}
-EXPORT_SYMBOL(set_dma_y_count);
-
-void set_dma_x_modify(unsigned int channel, short x_modify)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->x_modify = x_modify;
-	SSYNC();
-}
-EXPORT_SYMBOL(set_dma_x_modify);
-
-void set_dma_y_modify(unsigned int channel, short y_modify)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->y_modify = y_modify;
-	SSYNC();
-}
-EXPORT_SYMBOL(set_dma_y_modify);
-
-void set_dma_config(unsigned int channel, unsigned short config)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->cfg = config;
-	SSYNC();
-}
-EXPORT_SYMBOL(set_dma_config);
-
-unsigned short
-set_bfin_dma_config(char direction, char flow_mode,
-		    char intr_mode, char dma_mode, char width, char syncmode)
-{
-	unsigned short config;
-
-	config =
-	    ((direction << 1) | (width << 2) | (dma_mode << 4) |
-	     (intr_mode << 6) | (flow_mode << 12) | (syncmode << 5));
-	return config;
-}
-EXPORT_SYMBOL(set_bfin_dma_config);
-
-void set_dma_sg(unsigned int channel, struct dmasg *sg, int nr_sg)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->cfg |= ((nr_sg & 0x0F) << 8);
-
-	dma_ch[channel].regs->next_desc_ptr = (unsigned int)sg;
-
-	SSYNC();
-}
-EXPORT_SYMBOL(set_dma_sg);
-
-void set_dma_curr_addr(unsigned int channel, unsigned long addr)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->curr_addr_ptr = addr;
-	SSYNC();
-}
-EXPORT_SYMBOL(set_dma_curr_addr);
-
-/*------------------------------------------------------------------------------
- *	Get the DMA status of a specific DMA channel from the system.
- *-----------------------------------------------------------------------------*/
-unsigned short get_dma_curr_irqstat(unsigned int channel)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	return dma_ch[channel].regs->irq_status;
-}
-EXPORT_SYMBOL(get_dma_curr_irqstat);
-
-/*------------------------------------------------------------------------------
- *	Clear the DMA_DONE bit in DMA status. Stop the DMA completion interrupt.
- *-----------------------------------------------------------------------------*/
-void clear_dma_irqstat(unsigned int channel)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-	dma_ch[channel].regs->irq_status |= 3;
-}
-EXPORT_SYMBOL(clear_dma_irqstat);
-
-/*------------------------------------------------------------------------------
- *	Get current DMA xcount of a specific DMA channel from the system.
- *-----------------------------------------------------------------------------*/
-unsigned short get_dma_curr_xcount(unsigned int channel)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	return dma_ch[channel].regs->curr_x_count;
-}
-EXPORT_SYMBOL(get_dma_curr_xcount);
-
-/*------------------------------------------------------------------------------
- *	Get current DMA ycount of a specific DMA channel from the system.
- *-----------------------------------------------------------------------------*/
-unsigned short get_dma_curr_ycount(unsigned int channel)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	return dma_ch[channel].regs->curr_y_count;
-}
-EXPORT_SYMBOL(get_dma_curr_ycount);
-
-unsigned long get_dma_next_desc_ptr(unsigned int channel)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	      && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	return dma_ch[channel].regs->next_desc_ptr;
-}
-EXPORT_SYMBOL(get_dma_next_desc_ptr);
-
-unsigned long get_dma_curr_desc_ptr(unsigned int channel)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	      && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	return dma_ch[channel].regs->curr_desc_ptr;
-}
-EXPORT_SYMBOL(get_dma_curr_desc_ptr);
-
-unsigned long get_dma_curr_addr(unsigned int channel)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	      && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	return dma_ch[channel].regs->curr_addr_ptr;
-}
-EXPORT_SYMBOL(get_dma_curr_addr);
-
 #ifdef CONFIG_PM
+# ifndef MAX_DMA_SUSPEND_CHANNELS
+#  define MAX_DMA_SUSPEND_CHANNELS MAX_DMA_CHANNELS
+# endif
 int blackfin_dma_suspend(void)
 {
 	int i;
 
-#ifdef CONFIG_BF561	/* IMDMA channels doesn't have a PERIPHERAL_MAP */
-	for (i = 0; i <= CH_MEM_STREAM3_SRC; i++) {
-#else
-	for (i = 0; i < MAX_BLACKFIN_DMA_CHANNEL; i++) {
-#endif
+	for (i = 0; i < MAX_DMA_SUSPEND_CHANNELS; ++i) {
 		if (dma_ch[i].chan_status == DMA_CHANNEL_ENABLED) {
 			printk(KERN_ERR "DMA Channel %d failed to suspend\n", i);
 			return -EBUSY;
@@ -495,388 +218,201 @@ int blackfin_dma_suspend(void)
 void blackfin_dma_resume(void)
 {
 	int i;
-
-#ifdef CONFIG_BF561	/* IMDMA channels doesn't have a PERIPHERAL_MAP */
-	for (i = 0; i <= CH_MEM_STREAM3_SRC; i++)
-#else
-	for (i = 0; i < MAX_BLACKFIN_DMA_CHANNEL; i++)
-#endif
+	for (i = 0; i < MAX_DMA_SUSPEND_CHANNELS; ++i)
 		dma_ch[i].regs->peripheral_map = dma_ch[i].saved_peripheral_map;
 }
 #endif
 
-static void *__dma_memcpy(void *dest, const void *src, size_t size)
+/**
+ *	blackfin_dma_early_init - minimal DMA init
+ *
+ * Setup a few DMA registers so we can safely do DMA transfers early on in
+ * the kernel booting process.  Really this just means using dma_memcpy().
+ */
+void __init blackfin_dma_early_init(void)
 {
-	int direction;	/* 1 - address decrease, 0 - address increase */
-	int flag_align;	/* 1 - address aligned,  0 - address unaligned */
-	int flag_2D;	/* 1 - 2D DMA needed,	 0 - 1D DMA needed */
-	unsigned long flags;
-
-	if (size <= 0)
-		return NULL;
-
-	local_irq_save(flags);
-
-	if ((unsigned long)src < memory_end)
-		blackfin_dcache_flush_range((unsigned int)src,
-					    (unsigned int)(src + size));
-
-	if ((unsigned long)dest < memory_end)
-		blackfin_dcache_invalidate_range((unsigned int)dest,
-						 (unsigned int)(dest + size));
-
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	if ((unsigned long)src < (unsigned long)dest)
-		direction = 1;
-	else
-		direction = 0;
-
-	if ((((unsigned long)dest % 2) == 0) && (((unsigned long)src % 2) == 0)
-	    && ((size % 2) == 0))
-		flag_align = 1;
-	else
-		flag_align = 0;
-
-	if (size > 0x10000)	/* size > 64K */
-		flag_2D = 1;
-	else
-		flag_2D = 0;
-
-	/* Setup destination and source start address */
-	if (direction) {
-		if (flag_align) {
-			bfin_write_MDMA_D0_START_ADDR(dest + size - 2);
-			bfin_write_MDMA_S0_START_ADDR(src + size - 2);
-		} else {
-			bfin_write_MDMA_D0_START_ADDR(dest + size - 1);
-			bfin_write_MDMA_S0_START_ADDR(src + size - 1);
-		}
-	} else {
-		bfin_write_MDMA_D0_START_ADDR(dest);
-		bfin_write_MDMA_S0_START_ADDR(src);
-	}
-
-	/* Setup destination and source xcount */
-	if (flag_2D) {
-		if (flag_align) {
-			bfin_write_MDMA_D0_X_COUNT(1024 / 2);
-			bfin_write_MDMA_S0_X_COUNT(1024 / 2);
-		} else {
-			bfin_write_MDMA_D0_X_COUNT(1024);
-			bfin_write_MDMA_S0_X_COUNT(1024);
-		}
-		bfin_write_MDMA_D0_Y_COUNT(size >> 10);
-		bfin_write_MDMA_S0_Y_COUNT(size >> 10);
-	} else {
-		if (flag_align) {
-			bfin_write_MDMA_D0_X_COUNT(size / 2);
-			bfin_write_MDMA_S0_X_COUNT(size / 2);
-		} else {
-			bfin_write_MDMA_D0_X_COUNT(size);
-			bfin_write_MDMA_S0_X_COUNT(size);
-		}
-	}
-
-	/* Setup destination and source xmodify and ymodify */
-	if (direction) {
-		if (flag_align) {
-			bfin_write_MDMA_D0_X_MODIFY(-2);
-			bfin_write_MDMA_S0_X_MODIFY(-2);
-			if (flag_2D) {
-				bfin_write_MDMA_D0_Y_MODIFY(-2);
-				bfin_write_MDMA_S0_Y_MODIFY(-2);
-			}
-		} else {
-			bfin_write_MDMA_D0_X_MODIFY(-1);
-			bfin_write_MDMA_S0_X_MODIFY(-1);
-			if (flag_2D) {
-				bfin_write_MDMA_D0_Y_MODIFY(-1);
-				bfin_write_MDMA_S0_Y_MODIFY(-1);
-			}
-		}
-	} else {
-		if (flag_align) {
-			bfin_write_MDMA_D0_X_MODIFY(2);
-			bfin_write_MDMA_S0_X_MODIFY(2);
-			if (flag_2D) {
-				bfin_write_MDMA_D0_Y_MODIFY(2);
-				bfin_write_MDMA_S0_Y_MODIFY(2);
-			}
-		} else {
-			bfin_write_MDMA_D0_X_MODIFY(1);
-			bfin_write_MDMA_S0_X_MODIFY(1);
-			if (flag_2D) {
-				bfin_write_MDMA_D0_Y_MODIFY(1);
-				bfin_write_MDMA_S0_Y_MODIFY(1);
-			}
-		}
-	}
-
-	/* Enable source DMA */
-	if (flag_2D) {
-		if (flag_align) {
-			bfin_write_MDMA_S0_CONFIG(DMAEN | DMA2D | WDSIZE_16);
-			bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | DMA2D | WDSIZE_16);
-		} else {
-			bfin_write_MDMA_S0_CONFIG(DMAEN | DMA2D);
-			bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | DMA2D);
-		}
-	} else {
-		if (flag_align) {
-			bfin_write_MDMA_S0_CONFIG(DMAEN | WDSIZE_16);
-			bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | WDSIZE_16);
-		} else {
-			bfin_write_MDMA_S0_CONFIG(DMAEN);
-			bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN);
-		}
-	}
-
-	SSYNC();
-
-	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE))
-		;
-
-	bfin_write_MDMA_D0_IRQ_STATUS(bfin_read_MDMA_D0_IRQ_STATUS() |
-				      (DMA_DONE | DMA_ERR));
-
 	bfin_write_MDMA_S0_CONFIG(0);
-	bfin_write_MDMA_D0_CONFIG(0);
-
-	local_irq_restore(flags);
-
-	return dest;
 }
 
-void *dma_memcpy(void *dest, const void *src, size_t size)
-{
-	size_t bulk;
-	size_t rest;
-	void * addr;
-
-	bulk = (size >> 16) << 16;
-	rest = size - bulk;
-	if (bulk)
-		__dma_memcpy(dest, src, bulk);
-	addr = __dma_memcpy(dest+bulk, src+bulk, rest);
-	return addr;
-}
-EXPORT_SYMBOL(dma_memcpy);
-
-void *safe_dma_memcpy(void *dest, const void *src, size_t size)
-{
-	void *addr;
-	addr = dma_memcpy(dest, src, size);
-	return addr;
-}
-EXPORT_SYMBOL(safe_dma_memcpy);
-
-void dma_outsb(unsigned long addr, const void *buf, unsigned short len)
+/**
+ *	__dma_memcpy - program the MDMA registers
+ *
+ * Actually program MDMA0 and wait for the transfer to finish.  Disable IRQs
+ * while programming registers so that everything is fully configured.  Wait
+ * for DMA to finish with IRQs enabled.  If interrupted, the initial DMA_DONE
+ * check will make sure we don't clobber any existing transfer.
+ */
+static void __dma_memcpy(u32 daddr, s16 dmod, u32 saddr, s16 smod, size_t cnt, u32 conf)
 {
+	static DEFINE_SPINLOCK(mdma_lock);
 	unsigned long flags;
 
-	local_irq_save(flags);
-
-	blackfin_dcache_flush_range((unsigned int)buf,
-			 (unsigned int)(buf) + len);
+	spin_lock_irqsave(&mdma_lock, flags);
+
+	if (bfin_read_MDMA_S0_CONFIG())
+		while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE))
+			continue;
+
+	if (conf & DMA2D) {
+		/* For larger bit sizes, we've already divided down cnt so it
+		 * is no longer a multiple of 64k.  So we have to break down
+		 * the limit here so it is a multiple of the incoming size.
+		 * There is no limitation here in terms of total size other
+		 * than the hardware though as the bits lost in the shift are
+		 * made up by MODIFY (== we can hit the whole address space).
+		 * X: (2^(16 - 0)) * 1 == (2^(16 - 1)) * 2 == (2^(16 - 2)) * 4
+		 */
+		u32 shift = abs(dmod) >> 1;
+		size_t ycnt = cnt >> (16 - shift);
+		cnt = 1 << (16 - shift);
+		bfin_write_MDMA_D0_Y_COUNT(ycnt);
+		bfin_write_MDMA_S0_Y_COUNT(ycnt);
+		bfin_write_MDMA_D0_Y_MODIFY(dmod);
+		bfin_write_MDMA_S0_Y_MODIFY(smod);
+	}
 
-	bfin_write_MDMA_D0_START_ADDR(addr);
-	bfin_write_MDMA_D0_X_COUNT(len);
-	bfin_write_MDMA_D0_X_MODIFY(0);
+	bfin_write_MDMA_D0_START_ADDR(daddr);
+	bfin_write_MDMA_D0_X_COUNT(cnt);
+	bfin_write_MDMA_D0_X_MODIFY(dmod);
 	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
 
-	bfin_write_MDMA_S0_START_ADDR(buf);
-	bfin_write_MDMA_S0_X_COUNT(len);
-	bfin_write_MDMA_S0_X_MODIFY(1);
+	bfin_write_MDMA_S0_START_ADDR(saddr);
+	bfin_write_MDMA_S0_X_COUNT(cnt);
+	bfin_write_MDMA_S0_X_MODIFY(smod);
 	bfin_write_MDMA_S0_IRQ_STATUS(DMA_DONE | DMA_ERR);
 
-	bfin_write_MDMA_S0_CONFIG(DMAEN | WDSIZE_8);
-	bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | WDSIZE_8);
+	bfin_write_MDMA_S0_CONFIG(DMAEN | conf);
+	bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | conf);
+
+	spin_unlock_irqrestore(&mdma_lock, flags);
 
 	SSYNC();
 
-	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE));
+	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE))
+		if (bfin_read_MDMA_S0_CONFIG())
+			continue;
+		else
+			return;
 
 	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
 
 	bfin_write_MDMA_S0_CONFIG(0);
 	bfin_write_MDMA_D0_CONFIG(0);
-	local_irq_restore(flags);
-
 }
-EXPORT_SYMBOL(dma_outsb);
-
 
-void dma_insb(unsigned long addr, void *buf, unsigned short len)
+/**
+ *	_dma_memcpy - translate C memcpy settings into MDMA settings
+ *
+ * Handle all the high level steps before we touch the MDMA registers.  So
+ * handle direction, tweaking of sizes, and formatting of addresses.
+ */
+static void *_dma_memcpy(void *pdst, const void *psrc, size_t size)
 {
-	unsigned long flags;
-
-	blackfin_dcache_invalidate_range((unsigned int)buf,
-			 (unsigned int)(buf) + len);
-
-	local_irq_save(flags);
-	bfin_write_MDMA_D0_START_ADDR(buf);
-	bfin_write_MDMA_D0_X_COUNT(len);
-	bfin_write_MDMA_D0_X_MODIFY(1);
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_START_ADDR(addr);
-	bfin_write_MDMA_S0_X_COUNT(len);
-	bfin_write_MDMA_S0_X_MODIFY(0);
-	bfin_write_MDMA_S0_IRQ_STATUS(DMA_DONE | DMA_ERR);
+	u32 conf, shift;
+	s16 mod;
+	unsigned long dst = (unsigned long)pdst;
+	unsigned long src = (unsigned long)psrc;
 
-	bfin_write_MDMA_S0_CONFIG(DMAEN | WDSIZE_8);
-	bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | WDSIZE_8);
+	if (size == 0)
+		return NULL;
 
-	SSYNC();
+	if (dst % 4 == 0 && src % 4 == 0 && size % 4 == 0) {
+		conf = WDSIZE_32;
+		shift = 2;
+	} else if (dst % 2 == 0 && src % 2 == 0 && size % 2 == 0) {
+		conf = WDSIZE_16;
+		shift = 1;
+	} else {
+		conf = WDSIZE_8;
+		shift = 0;
+	}
 
-	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE));
+	/* If the two memory regions have a chance of overlapping, make
+	 * sure the memcpy still works as expected.  Do this by having the
+	 * copy run backwards instead.
+	 */
+	mod = 1 << shift;
+	if (src < dst) {
+		mod *= -1;
+		dst += size + mod;
+		src += size + mod;
+	}
+	size >>= shift;
 
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
+	if (size > 0x10000)
+		conf |= DMA2D;
 
-	bfin_write_MDMA_S0_CONFIG(0);
-	bfin_write_MDMA_D0_CONFIG(0);
-	local_irq_restore(flags);
+	__dma_memcpy(dst, mod, src, mod, size, conf);
 
+	return pdst;
 }
-EXPORT_SYMBOL(dma_insb);
 
-void dma_outsw(unsigned long addr, const void  *buf, unsigned short len)
+/**
+ *	dma_memcpy - DMA memcpy under mutex lock
+ *
+ * Do not check arguments before starting the DMA memcpy.  Break the transfer
+ * up into two pieces.  The first transfer is in multiples of 64k and the
+ * second transfer is the piece smaller than 64k.
+ */
+void *dma_memcpy(void *pdst, const void *psrc, size_t size)
 {
-	unsigned long flags;
-
-	local_irq_save(flags);
+	unsigned long dst = (unsigned long)pdst;
+	unsigned long src = (unsigned long)psrc;
+	size_t bulk, rest;
 
-	blackfin_dcache_flush_range((unsigned int)buf,
-			 (unsigned int)(buf) + len * sizeof(short));
+	if (bfin_addr_dcachable(src))
+		blackfin_dcache_flush_range(src, src + size);
 
-	bfin_write_MDMA_D0_START_ADDR(addr);
-	bfin_write_MDMA_D0_X_COUNT(len);
-	bfin_write_MDMA_D0_X_MODIFY(0);
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_START_ADDR(buf);
-	bfin_write_MDMA_S0_X_COUNT(len);
-	bfin_write_MDMA_S0_X_MODIFY(2);
-	bfin_write_MDMA_S0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(DMAEN | WDSIZE_16);
-	bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | WDSIZE_16);
-
-	SSYNC();
-
-	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE));
-
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(0);
-	bfin_write_MDMA_D0_CONFIG(0);
-	local_irq_restore(flags);
+	if (bfin_addr_dcachable(dst))
+		blackfin_dcache_invalidate_range(dst, dst + size);
 
+	bulk = size & ~0xffff;
+	rest = size - bulk;
+	if (bulk)
+		_dma_memcpy(pdst, psrc, bulk);
+	_dma_memcpy(pdst + bulk, psrc + bulk, rest);
+	return pdst;
 }
-EXPORT_SYMBOL(dma_outsw);
+EXPORT_SYMBOL(dma_memcpy);
 
-void dma_insw(unsigned long addr, void *buf, unsigned short len)
+/**
+ *	safe_dma_memcpy - DMA memcpy w/argument checking
+ *
+ * Verify arguments are safe before heading to dma_memcpy().
+ */
+void *safe_dma_memcpy(void *dst, const void *src, size_t size)
 {
-	unsigned long flags;
-
-	blackfin_dcache_invalidate_range((unsigned int)buf,
-			 (unsigned int)(buf) + len * sizeof(short));
-
-	local_irq_save(flags);
-
-	bfin_write_MDMA_D0_START_ADDR(buf);
-	bfin_write_MDMA_D0_X_COUNT(len);
-	bfin_write_MDMA_D0_X_MODIFY(2);
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_START_ADDR(addr);
-	bfin_write_MDMA_S0_X_COUNT(len);
-	bfin_write_MDMA_S0_X_MODIFY(0);
-	bfin_write_MDMA_S0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(DMAEN | WDSIZE_16);
-	bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | WDSIZE_16);
-
-	SSYNC();
-
-	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE));
-
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(0);
-	bfin_write_MDMA_D0_CONFIG(0);
-	local_irq_restore(flags);
-
+	if (!access_ok(VERIFY_WRITE, dst, size))
+		return NULL;
+	if (!access_ok(VERIFY_READ, src, size))
+		return NULL;
+	return dma_memcpy(dst, src, size);
 }
-EXPORT_SYMBOL(dma_insw);
+EXPORT_SYMBOL(safe_dma_memcpy);
 
-void dma_outsl(unsigned long addr, const void *buf, unsigned short len)
+static void _dma_out(unsigned long addr, unsigned long buf, unsigned short len,
+                     u16 size, u16 dma_size)
 {
-	unsigned long flags;
-
-	local_irq_save(flags);
-
-	blackfin_dcache_flush_range((unsigned int)buf,
-			 (unsigned int)(buf) + len * sizeof(long));
-
-	bfin_write_MDMA_D0_START_ADDR(addr);
-	bfin_write_MDMA_D0_X_COUNT(len);
-	bfin_write_MDMA_D0_X_MODIFY(0);
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_START_ADDR(buf);
-	bfin_write_MDMA_S0_X_COUNT(len);
-	bfin_write_MDMA_S0_X_MODIFY(4);
-	bfin_write_MDMA_S0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(DMAEN | WDSIZE_32);
-	bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | WDSIZE_32);
-
-	SSYNC();
-
-	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE));
-
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(0);
-	bfin_write_MDMA_D0_CONFIG(0);
-	local_irq_restore(flags);
-
+	blackfin_dcache_flush_range(buf, buf + len * size);
+	__dma_memcpy(addr, 0, buf, size, len, dma_size);
 }
-EXPORT_SYMBOL(dma_outsl);
 
-void dma_insl(unsigned long addr, void *buf, unsigned short len)
+static void _dma_in(unsigned long addr, unsigned long buf, unsigned short len,
+                    u16 size, u16 dma_size)
 {
-	unsigned long flags;
-
-	blackfin_dcache_invalidate_range((unsigned int)buf,
-			 (unsigned int)(buf) + len * sizeof(long));
-
-	local_irq_save(flags);
-
-	bfin_write_MDMA_D0_START_ADDR(buf);
-	bfin_write_MDMA_D0_X_COUNT(len);
-	bfin_write_MDMA_D0_X_MODIFY(4);
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_START_ADDR(addr);
-	bfin_write_MDMA_S0_X_COUNT(len);
-	bfin_write_MDMA_S0_X_MODIFY(0);
-	bfin_write_MDMA_S0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(DMAEN | WDSIZE_32);
-	bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | WDSIZE_32);
-
-	SSYNC();
-
-	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE));
-
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(0);
-	bfin_write_MDMA_D0_CONFIG(0);
-	local_irq_restore(flags);
-
+	blackfin_dcache_invalidate_range(buf, buf + len * size);
+	__dma_memcpy(buf, size, addr, 0, len, dma_size);
 }
-EXPORT_SYMBOL(dma_insl);
+
+#define MAKE_DMA_IO(io, bwl, isize, dmasize, cnst) \
+void dma_##io##s##bwl(unsigned long addr, cnst void *buf, unsigned short len) \
+{ \
+	_dma_##io(addr, (unsigned long)buf, len, isize, WDSIZE_##dmasize); \
+} \
+EXPORT_SYMBOL(dma_##io##s##bwl)
+MAKE_DMA_IO(out, b, 1,  8, const);
+MAKE_DMA_IO(in,  b, 1,  8, );
+MAKE_DMA_IO(out, w, 2, 16, const);
+MAKE_DMA_IO(in,  w, 2, 16, );
+MAKE_DMA_IO(out, l, 4, 32, const);
+MAKE_DMA_IO(in,  l, 4, 32, );
diff --git a/arch/blackfin/kernel/bfin_gpio.c b/arch/blackfin/kernel/bfin_gpio.c
index 5c0800adb4dd..4c14331978f6 100644
--- a/arch/blackfin/kernel/bfin_gpio.c
+++ b/arch/blackfin/kernel/bfin_gpio.c
@@ -119,28 +119,28 @@ enum {
 #define AWA_DUMMY_READ(...)  do { } while (0)
 #endif
 
-#ifdef BF533_FAMILY
-static struct gpio_port_t *gpio_bankb[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
+#if defined(BF533_FAMILY) || defined(BF538_FAMILY)
+static struct gpio_port_t *gpio_bankb[] = {
 	(struct gpio_port_t *) FIO_FLAG_D,
 };
 #endif
 
-#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
-static struct gpio_port_t *gpio_bankb[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY) || defined(BF518_FAMILY)
+static struct gpio_port_t *gpio_bankb[] = {
 	(struct gpio_port_t *) PORTFIO,
 	(struct gpio_port_t *) PORTGIO,
 	(struct gpio_port_t *) PORTHIO,
 };
 
-static unsigned short *port_fer[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
+static unsigned short *port_fer[] = {
 	(unsigned short *) PORTF_FER,
 	(unsigned short *) PORTG_FER,
 	(unsigned short *) PORTH_FER,
 };
 #endif
 
-#ifdef BF527_FAMILY
-static unsigned short *port_mux[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
+#if defined(BF527_FAMILY) || defined(BF518_FAMILY)
+static unsigned short *port_mux[] = {
 	(unsigned short *) PORTF_MUX,
 	(unsigned short *) PORTG_MUX,
 	(unsigned short *) PORTH_MUX,
@@ -155,7 +155,7 @@ u8 pmux_offset[][16] =
 #endif
 
 #ifdef BF561_FAMILY
-static struct gpio_port_t *gpio_bankb[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
+static struct gpio_port_t *gpio_bankb[] = {
 	(struct gpio_port_t *) FIO0_FLAG_D,
 	(struct gpio_port_t *) FIO1_FLAG_D,
 	(struct gpio_port_t *) FIO2_FLAG_D,
@@ -163,7 +163,7 @@ static struct gpio_port_t *gpio_bankb[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
 #endif
 
 #ifdef BF548_FAMILY
-static struct gpio_port_t *gpio_array[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
+static struct gpio_port_t *gpio_array[] = {
 	(struct gpio_port_t *)PORTA_FER,
 	(struct gpio_port_t *)PORTB_FER,
 	(struct gpio_port_t *)PORTC_FER,
@@ -177,8 +177,9 @@ static struct gpio_port_t *gpio_array[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
 };
 #endif
 
-static unsigned short reserved_gpio_map[gpio_bank(MAX_BLACKFIN_GPIOS)];
+static unsigned short reserved_gpio_map[GPIO_BANK_NUM];
 static unsigned short reserved_peri_map[gpio_bank(MAX_RESOURCES)];
+static unsigned short reserved_gpio_irq_map[GPIO_BANK_NUM];
 
 #define RESOURCE_LABEL_SIZE 	16
 
@@ -188,48 +189,46 @@ static struct str_ident {
 
 #if defined(CONFIG_PM)
 #if defined(CONFIG_BF54x)
-static struct gpio_port_s gpio_bank_saved[gpio_bank(MAX_BLACKFIN_GPIOS)];
+static struct gpio_port_s gpio_bank_saved[GPIO_BANK_NUM];
 #else
-static unsigned short wakeup_map[gpio_bank(MAX_BLACKFIN_GPIOS)];
+static unsigned short wakeup_map[GPIO_BANK_NUM];
 static unsigned char wakeup_flags_map[MAX_BLACKFIN_GPIOS];
-static struct gpio_port_s gpio_bank_saved[gpio_bank(MAX_BLACKFIN_GPIOS)];
+static struct gpio_port_s gpio_bank_saved[GPIO_BANK_NUM];
 
 #ifdef BF533_FAMILY
-static unsigned int sic_iwr_irqs[gpio_bank(MAX_BLACKFIN_GPIOS)] = {IRQ_PROG_INTB};
+static unsigned int sic_iwr_irqs[] = {IRQ_PROG_INTB};
 #endif
 
 #ifdef BF537_FAMILY
-static unsigned int sic_iwr_irqs[gpio_bank(MAX_BLACKFIN_GPIOS)] = {IRQ_PROG_INTB, IRQ_PORTG_INTB, IRQ_MAC_TX};
+static unsigned int sic_iwr_irqs[] = {IRQ_PROG_INTB, IRQ_PORTG_INTB, IRQ_MAC_TX};
+#endif
+
+#ifdef BF538_FAMILY
+static unsigned int sic_iwr_irqs[] = {IRQ_PORTF_INTB};
 #endif
 
-#ifdef BF527_FAMILY
-static unsigned int sic_iwr_irqs[gpio_bank(MAX_BLACKFIN_GPIOS)] = {IRQ_PORTF_INTB, IRQ_PORTG_INTB, IRQ_PORTH_INTB};
+#if defined(BF527_FAMILY) || defined(BF518_FAMILY)
+static unsigned int sic_iwr_irqs[] = {IRQ_PORTF_INTB, IRQ_PORTG_INTB, IRQ_PORTH_INTB};
 #endif
 
 #ifdef BF561_FAMILY
-static unsigned int sic_iwr_irqs[gpio_bank(MAX_BLACKFIN_GPIOS)] = {IRQ_PROG0_INTB, IRQ_PROG1_INTB, IRQ_PROG2_INTB};
+static unsigned int sic_iwr_irqs[] = {IRQ_PROG0_INTB, IRQ_PROG1_INTB, IRQ_PROG2_INTB};
 #endif
 #endif
 #endif /* CONFIG_PM */
 
-#if defined(BF548_FAMILY)
 inline int check_gpio(unsigned gpio)
 {
+#if defined(BF548_FAMILY)
 	if (gpio == GPIO_PB15 || gpio == GPIO_PC14 || gpio == GPIO_PC15
 	    || gpio == GPIO_PH14 || gpio == GPIO_PH15
-	    || gpio == GPIO_PJ14 || gpio == GPIO_PJ15
-	    || gpio >= MAX_BLACKFIN_GPIOS)
+	    || gpio == GPIO_PJ14 || gpio == GPIO_PJ15)
 		return -EINVAL;
-	return 0;
-}
-#else
-inline int check_gpio(unsigned gpio)
-{
+#endif
 	if (gpio >= MAX_BLACKFIN_GPIOS)
 		return -EINVAL;
 	return 0;
 }
-#endif
 
 static void gpio_error(unsigned gpio)
 {
@@ -258,35 +257,30 @@ static int cmp_label(unsigned short ident, const char *label)
 	}
 
 	if (label)
-		return strncmp(str_ident[ident].name,
-				 label, strlen(label));
+		return strcmp(str_ident[ident].name, label);
 	else
 		return -EINVAL;
 }
 
-#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
 static void port_setup(unsigned gpio, unsigned short usage)
 {
-	if (!check_gpio(gpio)) {
-		if (usage == GPIO_USAGE)
-			*port_fer[gpio_bank(gpio)] &= ~gpio_bit(gpio);
-		else
-			*port_fer[gpio_bank(gpio)] |= gpio_bit(gpio);
-		SSYNC();
-	}
-}
+	if (check_gpio(gpio))
+		return;
+
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY) || defined(BF518_FAMILY)
+	if (usage == GPIO_USAGE)
+		*port_fer[gpio_bank(gpio)] &= ~gpio_bit(gpio);
+	else
+		*port_fer[gpio_bank(gpio)] |= gpio_bit(gpio);
+	SSYNC();
 #elif defined(BF548_FAMILY)
-static void port_setup(unsigned gpio, unsigned short usage)
-{
 	if (usage == GPIO_USAGE)
 		gpio_array[gpio_bank(gpio)]->port_fer &= ~gpio_bit(gpio);
 	else
 		gpio_array[gpio_bank(gpio)]->port_fer |= gpio_bit(gpio);
 	SSYNC();
-}
-#else
-# define port_setup(...)  do { } while (0)
 #endif
+}
 
 #ifdef BF537_FAMILY
 static struct {
@@ -379,7 +373,7 @@ inline u16 get_portmux(unsigned short portno)
 
 	return (pmux >> (2 * gpio_sub_n(portno)) & 0x3);
 }
-#elif defined(BF527_FAMILY)
+#elif defined(BF527_FAMILY) || defined(BF518_FAMILY)
 inline void portmux_setup(unsigned short portno, unsigned short function)
 {
 	u16 pmux, ident = P_IDENT(portno);
@@ -428,13 +422,13 @@ arch_initcall(bfin_gpio_init);
 void set_gpio_ ## name(unsigned gpio, unsigned short arg) \
 { \
 	unsigned long flags; \
-	local_irq_save(flags); \
+	local_irq_save_hw(flags); \
 	if (arg) \
 		gpio_bankb[gpio_bank(gpio)]->name |= gpio_bit(gpio); \
 	else \
 		gpio_bankb[gpio_bank(gpio)]->name &= ~gpio_bit(gpio); \
 	AWA_DUMMY_READ(name); \
-	local_irq_restore(flags); \
+	local_irq_restore_hw(flags); \
 } \
 EXPORT_SYMBOL(set_gpio_ ## name);
 
@@ -450,13 +444,13 @@ SET_GPIO(both)
 void set_gpio_ ## name(unsigned gpio, unsigned short arg) \
 { \
 	unsigned long flags; \
-	local_irq_save(flags); \
+	local_irq_save_hw(flags); \
 	if (arg) \
 		gpio_bankb[gpio_bank(gpio)]->name ## _set = gpio_bit(gpio); \
 	else \
 		gpio_bankb[gpio_bank(gpio)]->name ## _clear = gpio_bit(gpio); \
 	AWA_DUMMY_READ(name); \
-	local_irq_restore(flags); \
+	local_irq_restore_hw(flags); \
 } \
 EXPORT_SYMBOL(set_gpio_ ## name);
 #else
@@ -479,10 +473,10 @@ SET_GPIO_SC(data)
 void set_gpio_toggle(unsigned gpio)
 {
 	unsigned long flags;
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	gpio_bankb[gpio_bank(gpio)]->toggle = gpio_bit(gpio);
 	AWA_DUMMY_READ(toggle);
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
 #else
 void set_gpio_toggle(unsigned gpio)
@@ -500,10 +494,10 @@ EXPORT_SYMBOL(set_gpio_toggle);
 void set_gpiop_ ## name(unsigned gpio, unsigned short arg) \
 { \
 	unsigned long flags; \
-	local_irq_save(flags); \
+	local_irq_save_hw(flags); \
 	gpio_bankb[gpio_bank(gpio)]->name = arg; \
 	AWA_DUMMY_READ(name); \
-	local_irq_restore(flags); \
+	local_irq_restore_hw(flags); \
 } \
 EXPORT_SYMBOL(set_gpiop_ ## name);
 #else
@@ -531,10 +525,10 @@ unsigned short get_gpio_ ## name(unsigned gpio) \
 { \
 	unsigned long flags; \
 	unsigned short ret; \
-	local_irq_save(flags); \
+	local_irq_save_hw(flags); \
 	ret = 0x01 & (gpio_bankb[gpio_bank(gpio)]->name >> gpio_sub_n(gpio)); \
 	AWA_DUMMY_READ(name); \
-	local_irq_restore(flags); \
+	local_irq_restore_hw(flags); \
 	return ret; \
 } \
 EXPORT_SYMBOL(get_gpio_ ## name);
@@ -564,10 +558,10 @@ unsigned short get_gpiop_ ## name(unsigned gpio) \
 { \
 	unsigned long flags; \
 	unsigned short ret; \
-	local_irq_save(flags); \
+	local_irq_save_hw(flags); \
 	ret = (gpio_bankb[gpio_bank(gpio)]->name); \
 	AWA_DUMMY_READ(name); \
-	local_irq_restore(flags); \
+	local_irq_restore_hw(flags); \
 	return ret; \
 } \
 EXPORT_SYMBOL(get_gpiop_ ## name);
@@ -617,10 +611,10 @@ int gpio_pm_wakeup_request(unsigned gpio, unsigned char type)
 	if ((check_gpio(gpio) < 0) || !type)
 		return -EINVAL;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	wakeup_map[gpio_bank(gpio)] |= gpio_bit(gpio);
 	wakeup_flags_map[gpio] = type;
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 	return 0;
 }
@@ -633,11 +627,11 @@ void gpio_pm_wakeup_free(unsigned gpio)
 	if (check_gpio(gpio) < 0)
 		return;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 
 	wakeup_map[gpio_bank(gpio)] &= ~gpio_bit(gpio);
 
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
 EXPORT_SYMBOL(gpio_pm_wakeup_free);
 
@@ -679,7 +673,7 @@ u32 bfin_pm_standby_setup(void)
 		gpio_bankb[bank]->maskb = 0;
 
 		if (mask) {
-#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY) || defined(BF518_FAMILY)
 			gpio_bank_saved[bank].fer   = *port_fer[bank];
 #endif
 			gpio_bank_saved[bank].inen  = gpio_bankb[bank]->inen;
@@ -724,7 +718,7 @@ void bfin_pm_standby_restore(void)
 		bank = gpio_bank(i);
 
 		if (mask) {
-#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY) || defined(BF518_FAMILY)
 			*port_fer[bank]   	= gpio_bank_saved[bank].fer;
 #endif
 			gpio_bankb[bank]->inen  = gpio_bank_saved[bank].inen;
@@ -750,9 +744,9 @@ void bfin_gpio_pm_hibernate_suspend(void)
 	for (i = 0; i < MAX_BLACKFIN_GPIOS; i += GPIO_BANKSIZE) {
 		bank = gpio_bank(i);
 
-#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY) || defined(BF518_FAMILY)
 			gpio_bank_saved[bank].fer   = *port_fer[bank];
-#ifdef BF527_FAMILY
+#if defined(BF527_FAMILY) || defined(BF518_FAMILY)
 			gpio_bank_saved[bank].mux   = *port_mux[bank];
 #else
 			if (bank == 0)
@@ -778,8 +772,8 @@ void bfin_gpio_pm_hibernate_restore(void)
 	for (i = 0; i < MAX_BLACKFIN_GPIOS; i += GPIO_BANKSIZE) {
 			bank = gpio_bank(i);
 
-#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
-#ifdef BF527_FAMILY
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY) || defined(BF518_FAMILY)
+#if defined(BF527_FAMILY) || defined(BF518_FAMILY)
 			*port_mux[bank] = gpio_bank_saved[bank].mux;
 #else
 			if (bank == 0)
@@ -873,7 +867,6 @@ EXPORT_SYMBOL(get_gpio_dir);
 * MODIFICATION HISTORY :
 **************************************************************/
 
-#ifdef BF548_FAMILY
 int peripheral_request(unsigned short per, const char *label)
 {
 	unsigned long flags;
@@ -889,31 +882,35 @@ int peripheral_request(unsigned short per, const char *label)
 	if (!(per & P_DEFINED))
 		return -ENODEV;
 
-	if (check_gpio(ident) < 0)
-		return -EINVAL;
+	local_irq_save_hw(flags);
 
-	local_irq_save(flags);
-
-	if (unlikely(reserved_gpio_map[gpio_bank(ident)] & gpio_bit(ident))) {
+	/* If a pin can be muxed as either GPIO or peripheral, make
+	 * sure it is not already a GPIO pin when we request it.
+	 */
+	if (unlikely(!check_gpio(ident) &&
+	    reserved_gpio_map[gpio_bank(ident)] & gpio_bit(ident))) {
 		dump_stack();
 		printk(KERN_ERR
-		    "%s: Peripheral %d is already reserved as GPIO by %s !\n",
+		       "%s: Peripheral %d is already reserved as GPIO by %s !\n",
 		       __func__, ident, get_label(ident));
-		local_irq_restore(flags);
+		local_irq_restore_hw(flags);
 		return -EBUSY;
 	}
 
 	if (unlikely(reserved_peri_map[gpio_bank(ident)] & gpio_bit(ident))) {
 
-		u16 funct = get_portmux(ident);
-
 		/*
 		 * Pin functions like AMC address strobes my
 		 * be requested and used by several drivers
 		 */
 
-		if (!((per & P_MAYSHARE) && (funct == P_FUNCT2MUX(per)))) {
+#ifdef BF548_FAMILY
+		u16 funct = get_portmux(ident);
 
+		if (!((per & P_MAYSHARE) && (funct == P_FUNCT2MUX(per)))) {
+#else
+		if (!(per & P_MAYSHARE)) {
+#endif
 			/*
 			 * Allow that the identical pin function can
 			 * be requested from the same driver twice
@@ -926,7 +923,7 @@ int peripheral_request(unsigned short per, const char *label)
 			printk(KERN_ERR
 			       "%s: Peripheral %d function %d is already reserved by %s !\n",
 			       __func__, ident, P_FUNCT2MUX(per), get_label(ident));
-			local_irq_restore(flags);
+			local_irq_restore_hw(flags);
 			return -EBUSY;
 		}
 	}
@@ -934,89 +931,19 @@ int peripheral_request(unsigned short per, const char *label)
  anyway:
 	reserved_peri_map[gpio_bank(ident)] |= gpio_bit(ident);
 
+#ifdef BF548_FAMILY
 	portmux_setup(ident, P_FUNCT2MUX(per));
-	port_setup(ident, PERIPHERAL_USAGE);
-
-	local_irq_restore(flags);
-	set_label(ident, label);
-
-	return 0;
-}
-EXPORT_SYMBOL(peripheral_request);
 #else
-
-int peripheral_request(unsigned short per, const char *label)
-{
-	unsigned long flags;
-	unsigned short ident = P_IDENT(per);
-
-	/*
-	 * Don't cares are pins with only one dedicated function
-	 */
-
-	if (per & P_DONTCARE)
-		return 0;
-
-	if (!(per & P_DEFINED))
-		return -ENODEV;
-
-	local_irq_save(flags);
-
-	if (!check_gpio(ident)) {
-
-		if (unlikely(reserved_gpio_map[gpio_bank(ident)] & gpio_bit(ident))) {
-			dump_stack();
-			printk(KERN_ERR
-			       "%s: Peripheral %d is already reserved as GPIO by %s !\n",
-			       __func__, ident, get_label(ident));
-			local_irq_restore(flags);
-			return -EBUSY;
-		}
-
-	}
-
-	if (unlikely(reserved_peri_map[gpio_bank(ident)] & gpio_bit(ident))) {
-
-		/*
-		 * Pin functions like AMC address strobes my
-		 * be requested and used by several drivers
-		 */
-
-		if (!(per & P_MAYSHARE)) {
-
-			/*
-			 * Allow that the identical pin function can
-			 * be requested from the same driver twice
-			 */
-
-			if (cmp_label(ident, label) == 0)
-				goto anyway;
-
-			dump_stack();
-			printk(KERN_ERR
-			       "%s: Peripheral %d function %d is already"
-			       " reserved by %s !\n",
-			       __func__, ident, P_FUNCT2MUX(per),
-				get_label(ident));
-			local_irq_restore(flags);
-			return -EBUSY;
-		}
-
-	}
-
- anyway:
 	portmux_setup(per, P_FUNCT2MUX(per));
-
+#endif
 	port_setup(ident, PERIPHERAL_USAGE);
 
-	reserved_peri_map[gpio_bank(ident)] |= gpio_bit(ident);
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 	set_label(ident, label);
 
 	return 0;
 }
 EXPORT_SYMBOL(peripheral_request);
-#endif
 
 int peripheral_request_list(const unsigned short per[], const char *label)
 {
@@ -1053,10 +980,10 @@ void peripheral_free(unsigned short per)
 	if (check_gpio(ident) < 0)
 		return;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 
 	if (unlikely(!(reserved_peri_map[gpio_bank(ident)] & gpio_bit(ident)))) {
-		local_irq_restore(flags);
+		local_irq_restore_hw(flags);
 		return;
 	}
 
@@ -1067,7 +994,7 @@ void peripheral_free(unsigned short per)
 
 	set_label(ident, "free");
 
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
 EXPORT_SYMBOL(peripheral_free);
 
@@ -1094,14 +1021,14 @@ EXPORT_SYMBOL(peripheral_free_list);
 * MODIFICATION HISTORY :
 **************************************************************/
 
-int gpio_request(unsigned gpio, const char *label)
+int bfin_gpio_request(unsigned gpio, const char *label)
 {
 	unsigned long flags;
 
 	if (check_gpio(gpio) < 0)
 		return -EINVAL;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 
 	/*
 	 * Allow that the identical GPIO can
@@ -1110,15 +1037,15 @@ int gpio_request(unsigned gpio, const char *label)
 	 */
 
 	if (cmp_label(gpio, label) == 0) {
-		local_irq_restore(flags);
+		local_irq_restore_hw(flags);
 		return 0;
 	}
 
 	if (unlikely(reserved_gpio_map[gpio_bank(gpio)] & gpio_bit(gpio))) {
 		dump_stack();
 		printk(KERN_ERR "bfin-gpio: GPIO %d is already reserved by %s !\n",
-			 gpio, get_label(gpio));
-		local_irq_restore(flags);
+		       gpio, get_label(gpio));
+		local_irq_restore_hw(flags);
 		return -EBUSY;
 	}
 	if (unlikely(reserved_peri_map[gpio_bank(gpio)] & gpio_bit(gpio))) {
@@ -1126,34 +1053,37 @@ int gpio_request(unsigned gpio, const char *label)
 		printk(KERN_ERR
 		       "bfin-gpio: GPIO %d is already reserved as Peripheral by %s !\n",
 		       gpio, get_label(gpio));
-		local_irq_restore(flags);
+		local_irq_restore_hw(flags);
 		return -EBUSY;
 	}
+	if (unlikely(reserved_gpio_irq_map[gpio_bank(gpio)] & gpio_bit(gpio)))
+		printk(KERN_NOTICE "bfin-gpio: GPIO %d is already reserved as gpio-irq!"
+		       " (Documentation/blackfin/bfin-gpio-notes.txt)\n", gpio);
 
 	reserved_gpio_map[gpio_bank(gpio)] |= gpio_bit(gpio);
+	set_label(gpio, label);
 
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 	port_setup(gpio, GPIO_USAGE);
-	set_label(gpio, label);
 
 	return 0;
 }
-EXPORT_SYMBOL(gpio_request);
+EXPORT_SYMBOL(bfin_gpio_request);
 
-void gpio_free(unsigned gpio)
+void bfin_gpio_free(unsigned gpio)
 {
 	unsigned long flags;
 
 	if (check_gpio(gpio) < 0)
 		return;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 
 	if (unlikely(!(reserved_gpio_map[gpio_bank(gpio)] & gpio_bit(gpio)))) {
 		dump_stack();
 		gpio_error(gpio);
-		local_irq_restore(flags);
+		local_irq_restore_hw(flags);
 		return;
 	}
 
@@ -1161,13 +1091,76 @@ void gpio_free(unsigned gpio)
 
 	set_label(gpio, "free");
 
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
+}
+EXPORT_SYMBOL(bfin_gpio_free);
+
+int bfin_gpio_irq_request(unsigned gpio, const char *label)
+{
+	unsigned long flags;
+
+	if (check_gpio(gpio) < 0)
+		return -EINVAL;
+
+	local_irq_save_hw(flags);
+
+	if (unlikely(reserved_gpio_irq_map[gpio_bank(gpio)] & gpio_bit(gpio))) {
+		dump_stack();
+		printk(KERN_ERR
+		       "bfin-gpio: GPIO %d is already reserved as gpio-irq !\n",
+		       gpio);
+		local_irq_restore_hw(flags);
+		return -EBUSY;
+	}
+	if (unlikely(reserved_peri_map[gpio_bank(gpio)] & gpio_bit(gpio))) {
+		dump_stack();
+		printk(KERN_ERR
+		       "bfin-gpio: GPIO %d is already reserved as Peripheral by %s !\n",
+		       gpio, get_label(gpio));
+		local_irq_restore_hw(flags);
+		return -EBUSY;
+	}
+	if (unlikely(reserved_gpio_map[gpio_bank(gpio)] & gpio_bit(gpio)))
+		printk(KERN_NOTICE "bfin-gpio: GPIO %d is already reserved by %s! "
+		       "(Documentation/blackfin/bfin-gpio-notes.txt)\n",
+		       gpio, get_label(gpio));
+
+	reserved_gpio_irq_map[gpio_bank(gpio)] |= gpio_bit(gpio);
+	set_label(gpio, label);
+
+	local_irq_restore_hw(flags);
+
+	port_setup(gpio, GPIO_USAGE);
+
+	return 0;
+}
+
+void bfin_gpio_irq_free(unsigned gpio)
+{
+	unsigned long flags;
+
+	if (check_gpio(gpio) < 0)
+		return;
+
+	local_irq_save_hw(flags);
+
+	if (unlikely(!(reserved_gpio_irq_map[gpio_bank(gpio)] & gpio_bit(gpio)))) {
+		dump_stack();
+		gpio_error(gpio);
+		local_irq_restore_hw(flags);
+		return;
+	}
+
+	reserved_gpio_irq_map[gpio_bank(gpio)] &= ~gpio_bit(gpio);
+
+	set_label(gpio, "free");
+
+	local_irq_restore_hw(flags);
 }
-EXPORT_SYMBOL(gpio_free);
 
 
 #ifdef BF548_FAMILY
-int gpio_direction_input(unsigned gpio)
+int bfin_gpio_direction_input(unsigned gpio)
 {
 	unsigned long flags;
 
@@ -1176,16 +1169,16 @@ int gpio_direction_input(unsigned gpio)
 		return -EINVAL;
 	}
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	gpio_array[gpio_bank(gpio)]->port_dir_clear = gpio_bit(gpio);
 	gpio_array[gpio_bank(gpio)]->port_inen |= gpio_bit(gpio);
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 	return 0;
 }
-EXPORT_SYMBOL(gpio_direction_input);
+EXPORT_SYMBOL(bfin_gpio_direction_input);
 
-int gpio_direction_output(unsigned gpio, int value)
+int bfin_gpio_direction_output(unsigned gpio, int value)
 {
 	unsigned long flags;
 
@@ -1194,30 +1187,30 @@ int gpio_direction_output(unsigned gpio, int value)
 		return -EINVAL;
 	}
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	gpio_array[gpio_bank(gpio)]->port_inen &= ~gpio_bit(gpio);
 	gpio_set_value(gpio, value);
 	gpio_array[gpio_bank(gpio)]->port_dir_set = gpio_bit(gpio);
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 	return 0;
 }
-EXPORT_SYMBOL(gpio_direction_output);
+EXPORT_SYMBOL(bfin_gpio_direction_output);
 
-void gpio_set_value(unsigned gpio, int arg)
+void bfin_gpio_set_value(unsigned gpio, int arg)
 {
 	if (arg)
 		gpio_array[gpio_bank(gpio)]->port_set = gpio_bit(gpio);
 	else
 		gpio_array[gpio_bank(gpio)]->port_clear = gpio_bit(gpio);
 }
-EXPORT_SYMBOL(gpio_set_value);
+EXPORT_SYMBOL(bfin_gpio_set_value);
 
-int gpio_get_value(unsigned gpio)
+int bfin_gpio_get_value(unsigned gpio)
 {
 	return (1 & (gpio_array[gpio_bank(gpio)]->port_data >> gpio_sub_n(gpio)));
 }
-EXPORT_SYMBOL(gpio_get_value);
+EXPORT_SYMBOL(bfin_gpio_get_value);
 
 void bfin_gpio_irq_prepare(unsigned gpio)
 {
@@ -1225,34 +1218,34 @@ void bfin_gpio_irq_prepare(unsigned gpio)
 
 	port_setup(gpio, GPIO_USAGE);
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	gpio_array[gpio_bank(gpio)]->port_dir_clear = gpio_bit(gpio);
 	gpio_array[gpio_bank(gpio)]->port_inen |= gpio_bit(gpio);
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
 
 #else
 
-int gpio_get_value(unsigned gpio)
+int bfin_gpio_get_value(unsigned gpio)
 {
 	unsigned long flags;
 	int ret;
 
 	if (unlikely(get_gpio_edge(gpio))) {
-		local_irq_save(flags);
+		local_irq_save_hw(flags);
 		set_gpio_edge(gpio, 0);
 		ret = get_gpio_data(gpio);
 		set_gpio_edge(gpio, 1);
-		local_irq_restore(flags);
+		local_irq_restore_hw(flags);
 
 		return ret;
 	} else
 		return get_gpio_data(gpio);
 }
-EXPORT_SYMBOL(gpio_get_value);
+EXPORT_SYMBOL(bfin_gpio_get_value);
 
 
-int gpio_direction_input(unsigned gpio)
+int bfin_gpio_direction_input(unsigned gpio)
 {
 	unsigned long flags;
 
@@ -1261,17 +1254,17 @@ int gpio_direction_input(unsigned gpio)
 		return -EINVAL;
 	}
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	gpio_bankb[gpio_bank(gpio)]->dir &= ~gpio_bit(gpio);
 	gpio_bankb[gpio_bank(gpio)]->inen |= gpio_bit(gpio);
 	AWA_DUMMY_READ(inen);
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 	return 0;
 }
-EXPORT_SYMBOL(gpio_direction_input);
+EXPORT_SYMBOL(bfin_gpio_direction_input);
 
-int gpio_direction_output(unsigned gpio, int value)
+int bfin_gpio_direction_output(unsigned gpio, int value)
 {
 	unsigned long flags;
 
@@ -1280,7 +1273,7 @@ int gpio_direction_output(unsigned gpio, int value)
 		return -EINVAL;
 	}
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	gpio_bankb[gpio_bank(gpio)]->inen &= ~gpio_bit(gpio);
 
 	if (value)
@@ -1290,11 +1283,11 @@ int gpio_direction_output(unsigned gpio, int value)
 
 	gpio_bankb[gpio_bank(gpio)]->dir |= gpio_bit(gpio);
 	AWA_DUMMY_READ(dir);
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 	return 0;
 }
-EXPORT_SYMBOL(gpio_direction_output);
+EXPORT_SYMBOL(bfin_gpio_direction_output);
 
 /* If we are booting from SPI and our board lacks a strong enough pull up,
  * the core can reset and execute the bootrom faster than the resistor can
@@ -1327,14 +1320,17 @@ void bfin_gpio_irq_prepare(unsigned gpio)
 static int gpio_proc_read(char *buf, char **start, off_t offset,
 			  int len, int *unused_i, void *unused_v)
 {
-	int c, outlen = 0;
+	int c, irq, gpio, outlen = 0;
 
 	for (c = 0; c < MAX_RESOURCES; c++) {
-		if (!check_gpio(c) && (reserved_gpio_map[gpio_bank(c)] & gpio_bit(c)))
-			len = sprintf(buf, "GPIO_%d: %s \t\tGPIO %s\n", c,
-				 get_label(c), get_gpio_dir(c) ? "OUTPUT" : "INPUT");
+		irq = reserved_gpio_irq_map[gpio_bank(c)] & gpio_bit(c);
+		gpio = reserved_gpio_map[gpio_bank(c)] & gpio_bit(c);
+		if (!check_gpio(c) && (gpio || irq))
+			len = sprintf(buf, "GPIO_%d: \t%s%s \t\tGPIO %s\n", c,
+				 get_label(c), (gpio && irq) ? " *" : "",
+				 get_gpio_dir(c) ? "OUTPUT" : "INPUT");
 		else if (reserved_peri_map[gpio_bank(c)] & gpio_bit(c))
-			len = sprintf(buf, "GPIO_%d: %s \t\tPeripheral\n", c, get_label(c));
+			len = sprintf(buf, "GPIO_%d: \t%s \t\tPeripheral\n", c, get_label(c));
 		else
 			continue;
 		buf += len;
@@ -1354,3 +1350,57 @@ static __init int gpio_register_proc(void)
 }
 __initcall(gpio_register_proc);
 #endif
+
+#ifdef CONFIG_GPIOLIB
+int bfin_gpiolib_direction_input(struct gpio_chip *chip, unsigned gpio)
+{
+	return bfin_gpio_direction_input(gpio);
+}
+
+int bfin_gpiolib_direction_output(struct gpio_chip *chip, unsigned gpio, int level)
+{
+	return bfin_gpio_direction_output(gpio, level);
+}
+
+int bfin_gpiolib_get_value(struct gpio_chip *chip, unsigned gpio)
+{
+	return bfin_gpio_get_value(gpio);
+}
+
+void bfin_gpiolib_set_value(struct gpio_chip *chip, unsigned gpio, int value)
+{
+#ifdef BF548_FAMILY
+	return bfin_gpio_set_value(gpio, value);
+#else
+	return set_gpio_data(gpio, value);
+#endif
+}
+
+int bfin_gpiolib_gpio_request(struct gpio_chip *chip, unsigned gpio)
+{
+	return bfin_gpio_request(gpio, chip->label);
+}
+
+void bfin_gpiolib_gpio_free(struct gpio_chip *chip, unsigned gpio)
+{
+	return bfin_gpio_free(gpio);
+}
+
+static struct gpio_chip bfin_chip = {
+	.label			= "Blackfin-GPIOlib",
+	.direction_input	= bfin_gpiolib_direction_input,
+	.get			= bfin_gpiolib_get_value,
+	.direction_output	= bfin_gpiolib_direction_output,
+	.set			= bfin_gpiolib_set_value,
+	.request		= bfin_gpiolib_gpio_request,
+	.free			= bfin_gpiolib_gpio_free,
+	.base			= 0,
+	.ngpio			= MAX_BLACKFIN_GPIOS,
+};
+
+static int __init bfin_gpiolib_setup(void)
+{
+	return gpiochip_add(&bfin_chip);
+}
+arch_initcall(bfin_gpiolib_setup);
+#endif
diff --git a/arch/blackfin/kernel/bfin_ksyms.c b/arch/blackfin/kernel/bfin_ksyms.c
index 4367330909b2..01f917d58b59 100644
--- a/arch/blackfin/kernel/bfin_ksyms.c
+++ b/arch/blackfin/kernel/bfin_ksyms.c
@@ -1,52 +1,25 @@
 /*
- * File:         arch/blackfin/kernel/bfin_ksyms.c
- * Based on:     none - original work
- * Author:
+ * arch/blackfin/kernel/bfin_ksyms.c - exports for random symbols
  *
- * Created:
- * Description:
+ * Copyright 2004-2008 Analog Devices Inc.
  *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ * Licensed under the GPL-2 or later.
  */
 
 #include <linux/module.h>
-#include <linux/irq.h>
 #include <linux/uaccess.h>
 
-#include <asm/checksum.h>
 #include <asm/cacheflush.h>
 
-/* platform dependent support */
-
-EXPORT_SYMBOL(__ioremap);
-
-EXPORT_SYMBOL(ip_fast_csum);
-
-EXPORT_SYMBOL(kernel_thread);
-
-EXPORT_SYMBOL(is_in_rom);
+/* Allow people to have their own Blackfin exception handler in a module */
 EXPORT_SYMBOL(bfin_return_from_exception);
 
-/* Networking helper routines. */
-EXPORT_SYMBOL(csum_partial_copy);
+/* All the Blackfin cache functions: mach-common/cache.S */
+EXPORT_SYMBOL(blackfin_dcache_invalidate_range);
+EXPORT_SYMBOL(blackfin_icache_dcache_flush_range);
+EXPORT_SYMBOL(blackfin_icache_flush_range);
+EXPORT_SYMBOL(blackfin_dcache_flush_range);
+EXPORT_SYMBOL(blackfin_dflush_page);
 
 /* The following are special because they're not called
  * explicitly (the C compiler generates them).  Fortunately,
@@ -74,8 +47,6 @@ extern void __modsi3(void);
 extern void __muldi3(void);
 extern void __udivsi3(void);
 extern void __umodsi3(void);
-
-/* gcc lib functions */
 EXPORT_SYMBOL(__ashldi3);
 EXPORT_SYMBOL(__ashrdi3);
 EXPORT_SYMBOL(__umulsi3_highpart);
@@ -87,6 +58,7 @@ EXPORT_SYMBOL(__muldi3);
 EXPORT_SYMBOL(__udivsi3);
 EXPORT_SYMBOL(__umodsi3);
 
+/* Input/output symbols: lib/{in,out}s.S */
 EXPORT_SYMBOL(outsb);
 EXPORT_SYMBOL(insb);
 EXPORT_SYMBOL(outsw);
@@ -96,20 +68,39 @@ EXPORT_SYMBOL(insw_8);
 EXPORT_SYMBOL(outsl);
 EXPORT_SYMBOL(insl);
 EXPORT_SYMBOL(insl_16);
-EXPORT_SYMBOL(irq_flags);
-EXPORT_SYMBOL(iounmap);
-EXPORT_SYMBOL(blackfin_dcache_invalidate_range);
-EXPORT_SYMBOL(blackfin_icache_dcache_flush_range);
-EXPORT_SYMBOL(blackfin_icache_flush_range);
-EXPORT_SYMBOL(blackfin_dcache_flush_range);
-EXPORT_SYMBOL(blackfin_dflush_page);
 
-EXPORT_SYMBOL(csum_partial);
-EXPORT_SYMBOL(__init_begin);
-EXPORT_SYMBOL(__init_end);
-EXPORT_SYMBOL(_ebss_l1);
-EXPORT_SYMBOL(_stext_l1);
-EXPORT_SYMBOL(_etext_l1);
-EXPORT_SYMBOL(_sdata_l1);
-EXPORT_SYMBOL(_ebss_b_l1);
-EXPORT_SYMBOL(_sdata_b_l1);
+#ifdef CONFIG_SMP
+EXPORT_SYMBOL(__raw_atomic_update_asm);
+EXPORT_SYMBOL(__raw_atomic_clear_asm);
+EXPORT_SYMBOL(__raw_atomic_set_asm);
+EXPORT_SYMBOL(__raw_atomic_xor_asm);
+EXPORT_SYMBOL(__raw_atomic_test_asm);
+EXPORT_SYMBOL(__raw_xchg_1_asm);
+EXPORT_SYMBOL(__raw_xchg_2_asm);
+EXPORT_SYMBOL(__raw_xchg_4_asm);
+EXPORT_SYMBOL(__raw_cmpxchg_1_asm);
+EXPORT_SYMBOL(__raw_cmpxchg_2_asm);
+EXPORT_SYMBOL(__raw_cmpxchg_4_asm);
+EXPORT_SYMBOL(__raw_spin_is_locked_asm);
+EXPORT_SYMBOL(__raw_spin_lock_asm);
+EXPORT_SYMBOL(__raw_spin_trylock_asm);
+EXPORT_SYMBOL(__raw_spin_unlock_asm);
+EXPORT_SYMBOL(__raw_read_lock_asm);
+EXPORT_SYMBOL(__raw_read_trylock_asm);
+EXPORT_SYMBOL(__raw_read_unlock_asm);
+EXPORT_SYMBOL(__raw_write_lock_asm);
+EXPORT_SYMBOL(__raw_write_trylock_asm);
+EXPORT_SYMBOL(__raw_write_unlock_asm);
+EXPORT_SYMBOL(__raw_bit_set_asm);
+EXPORT_SYMBOL(__raw_bit_clear_asm);
+EXPORT_SYMBOL(__raw_bit_toggle_asm);
+EXPORT_SYMBOL(__raw_bit_test_asm);
+EXPORT_SYMBOL(__raw_bit_test_set_asm);
+EXPORT_SYMBOL(__raw_bit_test_clear_asm);
+EXPORT_SYMBOL(__raw_bit_test_toggle_asm);
+EXPORT_SYMBOL(__raw_uncached_fetch_asm);
+#ifdef __ARCH_SYNC_CORE_DCACHE
+EXPORT_SYMBOL(__raw_smp_mark_barrier_asm);
+EXPORT_SYMBOL(__raw_smp_check_barrier_asm);
+#endif
+#endif
diff --git a/arch/blackfin/kernel/cplb-mpu/Makefile b/arch/blackfin/kernel/cplb-mpu/Makefile
index 286b69357f97..7d70d3bf3212 100644
--- a/arch/blackfin/kernel/cplb-mpu/Makefile
+++ b/arch/blackfin/kernel/cplb-mpu/Makefile
@@ -4,5 +4,7 @@
 
 obj-y := cplbinit.o cacheinit.o cplbmgr.o
 
-obj-$(CONFIG_CPLB_INFO) += cplbinfo.o
-
+CFLAGS_cplbmgr.o := -ffixed-I0 -ffixed-I1 -ffixed-I2 -ffixed-I3 \
+		    -ffixed-L0 -ffixed-L1 -ffixed-L2 -ffixed-L3 \
+		    -ffixed-M0 -ffixed-M1 -ffixed-M2 -ffixed-M3 \
+		    -ffixed-B0 -ffixed-B1 -ffixed-B2 -ffixed-B3
diff --git a/arch/blackfin/kernel/cplb-mpu/cacheinit.c b/arch/blackfin/kernel/cplb-mpu/cacheinit.c
index a8b712a24c59..c6ff947f9d37 100644
--- a/arch/blackfin/kernel/cplb-mpu/cacheinit.c
+++ b/arch/blackfin/kernel/cplb-mpu/cacheinit.c
@@ -25,7 +25,7 @@
 #include <asm/cplbinit.h>
 
 #if defined(CONFIG_BFIN_ICACHE)
-void __init bfin_icache_init(void)
+void __cpuinit bfin_icache_init(struct cplb_entry *icplb_tbl)
 {
 	unsigned long ctrl;
 	int i;
@@ -43,7 +43,7 @@ void __init bfin_icache_init(void)
 #endif
 
 #if defined(CONFIG_BFIN_DCACHE)
-void __init bfin_dcache_init(void)
+void __cpuinit bfin_dcache_init(struct cplb_entry *dcplb_tbl)
 {
 	unsigned long ctrl;
 	int i;
diff --git a/arch/blackfin/kernel/cplb-mpu/cplbinfo.c b/arch/blackfin/kernel/cplb-mpu/cplbinfo.c
deleted file mode 100644
index 822beefa3a4b..000000000000
--- a/arch/blackfin/kernel/cplb-mpu/cplbinfo.c
+++ /dev/null
@@ -1,136 +0,0 @@
-/*
- * File:         arch/blackfin/mach-common/cplbinfo.c
- * Based on:
- * Author:       Sonic Zhang <sonic.zhang@analog.com>
- *
- * Created:      Jan. 2005
- * Description:  Display CPLB status
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/proc_fs.h>
-#include <linux/uaccess.h>
-
-#include <asm/current.h>
-#include <asm/system.h>
-#include <asm/cplb.h>
-#include <asm/cplbinit.h>
-#include <asm/blackfin.h>
-
-static char page_size_string_table[][4] = { "1K", "4K", "1M", "4M" };
-
-static char *cplb_print_entry(char *buf, struct cplb_entry *tbl, int switched)
-{
-	int i;
-	buf += sprintf(buf, "Index\tAddress\t\tData\tSize\tU/RD\tU/WR\tS/WR\tSwitch\n");
-	for (i = 0; i < MAX_CPLBS; i++) {
-		unsigned long data = tbl[i].data;
-		unsigned long addr = tbl[i].addr;
-		if (!(data & CPLB_VALID))
-			continue;
-
-		buf +=
-		    sprintf(buf,
-			    "%d\t0x%08lx\t%06lx\t%s\t%c\t%c\t%c\t%c\n",
-			    i, addr, data,
-			    page_size_string_table[(data & 0x30000) >> 16],
-			    (data & CPLB_USER_RD) ? 'Y' : 'N',
-			    (data & CPLB_USER_WR) ? 'Y' : 'N',
-			    (data & CPLB_SUPV_WR) ? 'Y' : 'N',
-			    i < switched ? 'N' : 'Y');
-	}
-	buf += sprintf(buf, "\n");
-
-	return buf;
-}
-
-int cplbinfo_proc_output(char *buf)
-{
-	char *p;
-
-	p = buf;
-
-	p += sprintf(p, "------------------ CPLB Information ------------------\n\n");
-
-	if (bfin_read_IMEM_CONTROL() & ENICPLB) {
-		p += sprintf(p, "Instruction CPLB entry:\n");
-		p = cplb_print_entry(p, icplb_tbl, first_switched_icplb);
-	} else
-		p += sprintf(p, "Instruction CPLB is disabled.\n\n");
-
-	if (1 || bfin_read_DMEM_CONTROL() & ENDCPLB) {
-		p += sprintf(p, "Data CPLB entry:\n");
-		p = cplb_print_entry(p, dcplb_tbl, first_switched_dcplb);
-	} else
-		p += sprintf(p, "Data CPLB is disabled.\n");
-
-	p += sprintf(p, "ICPLB miss: %d\nICPLB supervisor miss: %d\n",
-		     nr_icplb_miss, nr_icplb_supv_miss);
-	p += sprintf(p, "DCPLB miss: %d\nDCPLB protection fault:%d\n",
-		     nr_dcplb_miss, nr_dcplb_prot);
-	p += sprintf(p, "CPLB flushes: %d\n",
-		     nr_cplb_flush);
-
-	return p - buf;
-}
-
-static int cplbinfo_read_proc(char *page, char **start, off_t off,
-			      int count, int *eof, void *data)
-{
-	int len;
-
-	len = cplbinfo_proc_output(page);
-	if (len <= off + count)
-		*eof = 1;
-	*start = page + off;
-	len -= off;
-	if (len > count)
-		len = count;
-	if (len < 0)
-		len = 0;
-	return len;
-}
-
-static int __init cplbinfo_init(void)
-{
-	struct proc_dir_entry *entry;
-
-	entry = create_proc_entry("cplbinfo", 0, NULL);
-	if (!entry)
-		return -ENOMEM;
-
-	entry->read_proc = cplbinfo_read_proc;
-	entry->data = NULL;
-
-	return 0;
-}
-
-static void __exit cplbinfo_exit(void)
-{
-	remove_proc_entry("cplbinfo", NULL);
-}
-
-module_init(cplbinfo_init);
-module_exit(cplbinfo_exit);
diff --git a/arch/blackfin/kernel/cplb-mpu/cplbinit.c b/arch/blackfin/kernel/cplb-mpu/cplbinit.c
index 55af729f8495..bdb958486e76 100644
--- a/arch/blackfin/kernel/cplb-mpu/cplbinit.c
+++ b/arch/blackfin/kernel/cplb-mpu/cplbinit.c
@@ -25,18 +25,19 @@
 #include <asm/blackfin.h>
 #include <asm/cplb.h>
 #include <asm/cplbinit.h>
+#include <asm/mem_map.h>
 
 #if ANOMALY_05000263
 # error the MPU will not function safely while Anomaly 05000263 applies
 #endif
 
-struct cplb_entry icplb_tbl[MAX_CPLBS];
-struct cplb_entry dcplb_tbl[MAX_CPLBS];
+struct cplb_entry icplb_tbl[NR_CPUS][MAX_CPLBS];
+struct cplb_entry dcplb_tbl[NR_CPUS][MAX_CPLBS];
 
 int first_switched_icplb, first_switched_dcplb;
 int first_mask_dcplb;
 
-void __init generate_cplb_tables(void)
+void __init generate_cplb_tables_cpu(unsigned int cpu)
 {
 	int i_d, i_i;
 	unsigned long addr;
@@ -55,15 +56,16 @@ void __init generate_cplb_tables(void)
 	d_cache |= CPLB_L1_AOW | CPLB_WT;
 #endif
 #endif
+
 	i_d = i_i = 0;
 
 	/* Set up the zero page.  */
-	dcplb_tbl[i_d].addr = 0;
-	dcplb_tbl[i_d++].data = SDRAM_OOPS | PAGE_SIZE_1KB;
+	dcplb_tbl[cpu][i_d].addr = 0;
+	dcplb_tbl[cpu][i_d++].data = SDRAM_OOPS | PAGE_SIZE_1KB;
 
 #if 0
-	icplb_tbl[i_i].addr = 0;
-	icplb_tbl[i_i++].data = i_cache | CPLB_USER_RD | PAGE_SIZE_4KB;
+	icplb_tbl[cpu][i_i].addr = 0;
+	icplb_tbl[cpu][i_i++].data = i_cache | CPLB_USER_RD | PAGE_SIZE_4KB;
 #endif
 
 	/* Cover kernel memory with 4M pages.  */
@@ -72,28 +74,28 @@ void __init generate_cplb_tables(void)
 	i_data = i_cache | CPLB_VALID | CPLB_PORTPRIO | PAGE_SIZE_4MB;
 
 	for (; addr < memory_start; addr += 4 * 1024 * 1024) {
-		dcplb_tbl[i_d].addr = addr;
-		dcplb_tbl[i_d++].data = d_data;
-		icplb_tbl[i_i].addr = addr;
-		icplb_tbl[i_i++].data = i_data | (addr == 0 ? CPLB_USER_RD : 0);
+		dcplb_tbl[cpu][i_d].addr = addr;
+		dcplb_tbl[cpu][i_d++].data = d_data;
+		icplb_tbl[cpu][i_i].addr = addr;
+		icplb_tbl[cpu][i_i++].data = i_data | (addr == 0 ? CPLB_USER_RD : 0);
 	}
 
 	/* Cover L1 memory.  One 4M area for code and data each is enough.  */
 #if L1_DATA_A_LENGTH > 0 || L1_DATA_B_LENGTH > 0
-	dcplb_tbl[i_d].addr = L1_DATA_A_START;
-	dcplb_tbl[i_d++].data = L1_DMEMORY | PAGE_SIZE_4MB;
+	dcplb_tbl[cpu][i_d].addr = get_l1_data_a_start_cpu(cpu);
+	dcplb_tbl[cpu][i_d++].data = L1_DMEMORY | PAGE_SIZE_4MB;
 #endif
 #if L1_CODE_LENGTH > 0
-	icplb_tbl[i_i].addr = L1_CODE_START;
-	icplb_tbl[i_i++].data = L1_IMEMORY | PAGE_SIZE_4MB;
+	icplb_tbl[cpu][i_i].addr = get_l1_code_start_cpu(cpu);
+	icplb_tbl[cpu][i_i++].data = L1_IMEMORY | PAGE_SIZE_4MB;
 #endif
 
 	/* Cover L2 memory */
 #if L2_LENGTH > 0
-	dcplb_tbl[i_d].addr = L2_START;
-	dcplb_tbl[i_d++].data = L2_DMEMORY | PAGE_SIZE_1MB;
-	icplb_tbl[i_i].addr = L2_START;
-	icplb_tbl[i_i++].data = L2_IMEMORY | PAGE_SIZE_1MB;
+	dcplb_tbl[cpu][i_d].addr = L2_START;
+	dcplb_tbl[cpu][i_d++].data = L2_DMEMORY | PAGE_SIZE_1MB;
+	icplb_tbl[cpu][i_i].addr = L2_START;
+	icplb_tbl[cpu][i_i++].data = L2_IMEMORY | PAGE_SIZE_1MB;
 #endif
 
 	first_mask_dcplb = i_d;
@@ -101,7 +103,11 @@ void __init generate_cplb_tables(void)
 	first_switched_icplb = i_i;
 
 	while (i_d < MAX_CPLBS)
-		dcplb_tbl[i_d++].data = 0;
+		dcplb_tbl[cpu][i_d++].data = 0;
 	while (i_i < MAX_CPLBS)
-		icplb_tbl[i_i++].data = 0;
+		icplb_tbl[cpu][i_i++].data = 0;
+}
+
+void generate_cplb_tables_all(void)
+{
 }
diff --git a/arch/blackfin/kernel/cplb-mpu/cplbmgr.c b/arch/blackfin/kernel/cplb-mpu/cplbmgr.c
index baa52e261f0d..87463ce87f5a 100644
--- a/arch/blackfin/kernel/cplb-mpu/cplbmgr.c
+++ b/arch/blackfin/kernel/cplb-mpu/cplbmgr.c
@@ -25,15 +25,21 @@
 #include <asm/cplbinit.h>
 #include <asm/mmu_context.h>
 
-#define FAULT_RW	(1 << 16)
-#define FAULT_USERSUPV	(1 << 17)
+/*
+ * WARNING
+ *
+ * This file is compiled with certain -ffixed-reg options.  We have to
+ * make sure not to call any functions here that could clobber these
+ * registers.
+ */
 
 int page_mask_nelts;
 int page_mask_order;
-unsigned long *current_rwx_mask;
+unsigned long *current_rwx_mask[NR_CPUS];
 
-int nr_dcplb_miss, nr_icplb_miss, nr_icplb_supv_miss, nr_dcplb_prot;
-int nr_cplb_flush;
+int nr_dcplb_miss[NR_CPUS], nr_icplb_miss[NR_CPUS];
+int nr_icplb_supv_miss[NR_CPUS], nr_dcplb_prot[NR_CPUS];
+int nr_cplb_flush[NR_CPUS];
 
 static inline void disable_dcplb(void)
 {
@@ -98,42 +104,42 @@ static inline int write_permitted(int status, unsigned long data)
 }
 
 /* Counters to implement round-robin replacement.  */
-static int icplb_rr_index, dcplb_rr_index;
+static int icplb_rr_index[NR_CPUS], dcplb_rr_index[NR_CPUS];
 
 /*
  * Find an ICPLB entry to be evicted and return its index.
  */
-static int evict_one_icplb(void)
+static int evict_one_icplb(unsigned int cpu)
 {
 	int i;
 	for (i = first_switched_icplb; i < MAX_CPLBS; i++)
-		if ((icplb_tbl[i].data & CPLB_VALID) == 0)
+		if ((icplb_tbl[cpu][i].data & CPLB_VALID) == 0)
 			return i;
-	i = first_switched_icplb + icplb_rr_index;
+	i = first_switched_icplb + icplb_rr_index[cpu];
 	if (i >= MAX_CPLBS) {
 		i -= MAX_CPLBS - first_switched_icplb;
-		icplb_rr_index -= MAX_CPLBS - first_switched_icplb;
+		icplb_rr_index[cpu] -= MAX_CPLBS - first_switched_icplb;
 	}
-	icplb_rr_index++;
+	icplb_rr_index[cpu]++;
 	return i;
 }
 
-static int evict_one_dcplb(void)
+static int evict_one_dcplb(unsigned int cpu)
 {
 	int i;
 	for (i = first_switched_dcplb; i < MAX_CPLBS; i++)
-		if ((dcplb_tbl[i].data & CPLB_VALID) == 0)
+		if ((dcplb_tbl[cpu][i].data & CPLB_VALID) == 0)
 			return i;
-	i = first_switched_dcplb + dcplb_rr_index;
+	i = first_switched_dcplb + dcplb_rr_index[cpu];
 	if (i >= MAX_CPLBS) {
 		i -= MAX_CPLBS - first_switched_dcplb;
-		dcplb_rr_index -= MAX_CPLBS - first_switched_dcplb;
+		dcplb_rr_index[cpu] -= MAX_CPLBS - first_switched_dcplb;
 	}
-	dcplb_rr_index++;
+	dcplb_rr_index[cpu]++;
 	return i;
 }
 
-static noinline int dcplb_miss(void)
+static noinline int dcplb_miss(unsigned int cpu)
 {
 	unsigned long addr = bfin_read_DCPLB_FAULT_ADDR();
 	int status = bfin_read_DCPLB_STATUS();
@@ -141,7 +147,7 @@ static noinline int dcplb_miss(void)
 	int idx;
 	unsigned long d_data;
 
-	nr_dcplb_miss++;
+	nr_dcplb_miss[cpu]++;
 
 	d_data = CPLB_SUPV_WR | CPLB_VALID | CPLB_DIRTY | PAGE_SIZE_4KB;
 #ifdef CONFIG_BFIN_DCACHE
@@ -168,25 +174,25 @@ static noinline int dcplb_miss(void)
 	} else if (addr >= _ramend) {
 	    d_data |= CPLB_USER_RD | CPLB_USER_WR;
 	} else {
-		mask = current_rwx_mask;
+		mask = current_rwx_mask[cpu];
 		if (mask) {
 			int page = addr >> PAGE_SHIFT;
-			int offs = page >> 5;
+			int idx = page >> 5;
 			int bit = 1 << (page & 31);
 
-			if (mask[offs] & bit)
+			if (mask[idx] & bit)
 				d_data |= CPLB_USER_RD;
 
 			mask += page_mask_nelts;
-			if (mask[offs] & bit)
+			if (mask[idx] & bit)
 				d_data |= CPLB_USER_WR;
 		}
 	}
-	idx = evict_one_dcplb();
+	idx = evict_one_dcplb(cpu);
 
 	addr &= PAGE_MASK;
-	dcplb_tbl[idx].addr = addr;
-	dcplb_tbl[idx].data = d_data;
+	dcplb_tbl[cpu][idx].addr = addr;
+	dcplb_tbl[cpu][idx].data = d_data;
 
 	disable_dcplb();
 	bfin_write32(DCPLB_DATA0 + idx * 4, d_data);
@@ -196,21 +202,21 @@ static noinline int dcplb_miss(void)
 	return 0;
 }
 
-static noinline int icplb_miss(void)
+static noinline int icplb_miss(unsigned int cpu)
 {
 	unsigned long addr = bfin_read_ICPLB_FAULT_ADDR();
 	int status = bfin_read_ICPLB_STATUS();
 	int idx;
 	unsigned long i_data;
 
-	nr_icplb_miss++;
+	nr_icplb_miss[cpu]++;
 
 	/* If inside the uncached DMA region, fault.  */
 	if (addr >= _ramend - DMA_UNCACHED_REGION && addr < _ramend)
 		return CPLB_PROT_VIOL;
 
 	if (status & FAULT_USERSUPV)
-		nr_icplb_supv_miss++;
+		nr_icplb_supv_miss[cpu]++;
 
 	/*
 	 * First, try to find a CPLB that matches this address.  If we
@@ -218,8 +224,8 @@ static noinline int icplb_miss(void)
 	 * that the instruction crosses a page boundary.
 	 */
 	for (idx = first_switched_icplb; idx < MAX_CPLBS; idx++) {
-		if (icplb_tbl[idx].data & CPLB_VALID) {
-			unsigned long this_addr = icplb_tbl[idx].addr;
+		if (icplb_tbl[cpu][idx].data & CPLB_VALID) {
+			unsigned long this_addr = icplb_tbl[cpu][idx].addr;
 			if (this_addr <= addr && this_addr + PAGE_SIZE > addr) {
 				addr += PAGE_SIZE;
 				break;
@@ -257,23 +263,23 @@ static noinline int icplb_miss(void)
 		 * Otherwise, check the x bitmap of the current process.
 		 */
 		if (!(status & FAULT_USERSUPV)) {
-			unsigned long *mask = current_rwx_mask;
+			unsigned long *mask = current_rwx_mask[cpu];
 
 			if (mask) {
 				int page = addr >> PAGE_SHIFT;
-				int offs = page >> 5;
+				int idx = page >> 5;
 				int bit = 1 << (page & 31);
 
 				mask += 2 * page_mask_nelts;
-				if (mask[offs] & bit)
+				if (mask[idx] & bit)
 					i_data |= CPLB_USER_RD;
 			}
 		}
 	}
-	idx = evict_one_icplb();
+	idx = evict_one_icplb(cpu);
 	addr &= PAGE_MASK;
-	icplb_tbl[idx].addr = addr;
-	icplb_tbl[idx].data = i_data;
+	icplb_tbl[cpu][idx].addr = addr;
+	icplb_tbl[cpu][idx].data = i_data;
 
 	disable_icplb();
 	bfin_write32(ICPLB_DATA0 + idx * 4, i_data);
@@ -283,19 +289,19 @@ static noinline int icplb_miss(void)
 	return 0;
 }
 
-static noinline int dcplb_protection_fault(void)
+static noinline int dcplb_protection_fault(unsigned int cpu)
 {
 	int status = bfin_read_DCPLB_STATUS();
 
-	nr_dcplb_prot++;
+	nr_dcplb_prot[cpu]++;
 
 	if (status & FAULT_RW) {
 		int idx = faulting_cplb_index(status);
-		unsigned long data = dcplb_tbl[idx].data;
+		unsigned long data = dcplb_tbl[cpu][idx].data;
 		if (!(data & CPLB_WT) && !(data & CPLB_DIRTY) &&
 		    write_permitted(status, data)) {
 			data |= CPLB_DIRTY;
-			dcplb_tbl[idx].data = data;
+			dcplb_tbl[cpu][idx].data = data;
 			bfin_write32(DCPLB_DATA0 + idx * 4, data);
 			return 0;
 		}
@@ -306,44 +312,45 @@ static noinline int dcplb_protection_fault(void)
 int cplb_hdr(int seqstat, struct pt_regs *regs)
 {
 	int cause = seqstat & 0x3f;
+	unsigned int cpu = smp_processor_id();
 	switch (cause) {
 	case 0x23:
-		return dcplb_protection_fault();
+		return dcplb_protection_fault(cpu);
 	case 0x2C:
-		return icplb_miss();
+		return icplb_miss(cpu);
 	case 0x26:
-		return dcplb_miss();
+		return dcplb_miss(cpu);
 	default:
 		return 1;
 	}
 }
 
-void flush_switched_cplbs(void)
+void flush_switched_cplbs(unsigned int cpu)
 {
 	int i;
 	unsigned long flags;
 
-	nr_cplb_flush++;
+	nr_cplb_flush[cpu]++;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	disable_icplb();
 	for (i = first_switched_icplb; i < MAX_CPLBS; i++) {
-		icplb_tbl[i].data = 0;
+		icplb_tbl[cpu][i].data = 0;
 		bfin_write32(ICPLB_DATA0 + i * 4, 0);
 	}
 	enable_icplb();
 
 	disable_dcplb();
 	for (i = first_switched_dcplb; i < MAX_CPLBS; i++) {
-		dcplb_tbl[i].data = 0;
+		dcplb_tbl[cpu][i].data = 0;
 		bfin_write32(DCPLB_DATA0 + i * 4, 0);
 	}
 	enable_dcplb();
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 }
 
-void set_mask_dcplbs(unsigned long *masks)
+void set_mask_dcplbs(unsigned long *masks, unsigned int cpu)
 {
 	int i;
 	unsigned long addr = (unsigned long)masks;
@@ -351,12 +358,12 @@ void set_mask_dcplbs(unsigned long *masks)
 	unsigned long flags;
 
 	if (!masks) {
-		current_rwx_mask = masks;
+		current_rwx_mask[cpu] = masks;
 		return;
 	}
 
-	local_irq_save(flags);
-	current_rwx_mask = masks;
+	local_irq_save_hw(flags);
+	current_rwx_mask[cpu] = masks;
 
 	d_data = CPLB_SUPV_WR | CPLB_VALID | CPLB_DIRTY | PAGE_SIZE_4KB;
 #ifdef CONFIG_BFIN_DCACHE
@@ -368,12 +375,12 @@ void set_mask_dcplbs(unsigned long *masks)
 
 	disable_dcplb();
 	for (i = first_mask_dcplb; i < first_switched_dcplb; i++) {
-		dcplb_tbl[i].addr = addr;
-		dcplb_tbl[i].data = d_data;
+		dcplb_tbl[cpu][i].addr = addr;
+		dcplb_tbl[cpu][i].data = d_data;
 		bfin_write32(DCPLB_DATA0 + i * 4, d_data);
 		bfin_write32(DCPLB_ADDR0 + i * 4, addr);
 		addr += PAGE_SIZE;
 	}
 	enable_dcplb();
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
diff --git a/arch/blackfin/kernel/cplb-nompu/Makefile b/arch/blackfin/kernel/cplb-nompu/Makefile
index d36ea9b5382e..7d70d3bf3212 100644
--- a/arch/blackfin/kernel/cplb-nompu/Makefile
+++ b/arch/blackfin/kernel/cplb-nompu/Makefile
@@ -2,7 +2,9 @@
 # arch/blackfin/kernel/cplb-nompu/Makefile
 #
 
-obj-y := cplbinit.o cacheinit.o cplbhdlr.o cplbmgr.o
-
-obj-$(CONFIG_CPLB_INFO) += cplbinfo.o
+obj-y := cplbinit.o cacheinit.o cplbmgr.o
 
+CFLAGS_cplbmgr.o := -ffixed-I0 -ffixed-I1 -ffixed-I2 -ffixed-I3 \
+		    -ffixed-L0 -ffixed-L1 -ffixed-L2 -ffixed-L3 \
+		    -ffixed-M0 -ffixed-M1 -ffixed-M2 -ffixed-M3 \
+		    -ffixed-B0 -ffixed-B1 -ffixed-B2 -ffixed-B3
diff --git a/arch/blackfin/kernel/cplb-nompu/cacheinit.c b/arch/blackfin/kernel/cplb-nompu/cacheinit.c
index bd0831592c2c..c6ff947f9d37 100644
--- a/arch/blackfin/kernel/cplb-nompu/cacheinit.c
+++ b/arch/blackfin/kernel/cplb-nompu/cacheinit.c
@@ -25,19 +25,15 @@
 #include <asm/cplbinit.h>
 
 #if defined(CONFIG_BFIN_ICACHE)
-void __init bfin_icache_init(void)
+void __cpuinit bfin_icache_init(struct cplb_entry *icplb_tbl)
 {
-	unsigned long *table = icplb_table;
 	unsigned long ctrl;
 	int i;
 
+	SSYNC();
 	for (i = 0; i < MAX_CPLBS; i++) {
-		unsigned long addr = *table++;
-		unsigned long data = *table++;
-		if (addr == (unsigned long)-1)
-			break;
-		bfin_write32(ICPLB_ADDR0 + i * 4, addr);
-		bfin_write32(ICPLB_DATA0 + i * 4, data);
+		bfin_write32(ICPLB_ADDR0 + i * 4, icplb_tbl[i].addr);
+		bfin_write32(ICPLB_DATA0 + i * 4, icplb_tbl[i].data);
 	}
 	ctrl = bfin_read_IMEM_CONTROL();
 	ctrl |= IMC | ENICPLB;
@@ -47,20 +43,17 @@ void __init bfin_icache_init(void)
 #endif
 
 #if defined(CONFIG_BFIN_DCACHE)
-void __init bfin_dcache_init(void)
+void __cpuinit bfin_dcache_init(struct cplb_entry *dcplb_tbl)
 {
-	unsigned long *table = dcplb_table;
 	unsigned long ctrl;
 	int i;
 
+	SSYNC();
 	for (i = 0; i < MAX_CPLBS; i++) {
-		unsigned long addr = *table++;
-		unsigned long data = *table++;
-		if (addr == (unsigned long)-1)
-			break;
-		bfin_write32(DCPLB_ADDR0 + i * 4, addr);
-		bfin_write32(DCPLB_DATA0 + i * 4, data);
+		bfin_write32(DCPLB_ADDR0 + i * 4, dcplb_tbl[i].addr);
+		bfin_write32(DCPLB_DATA0 + i * 4, dcplb_tbl[i].data);
 	}
+
 	ctrl = bfin_read_DMEM_CONTROL();
 	ctrl |= DMEM_CNTR;
 	bfin_write_DMEM_CONTROL(ctrl);
diff --git a/arch/blackfin/kernel/cplb-nompu/cplbhdlr.S b/arch/blackfin/kernel/cplb-nompu/cplbhdlr.S
deleted file mode 100644
index ecbabc0a1fed..000000000000
--- a/arch/blackfin/kernel/cplb-nompu/cplbhdlr.S
+++ /dev/null
@@ -1,130 +0,0 @@
-/*
- * File:         arch/blackfin/mach-common/cplbhdlr.S
- * Based on:
- * Author:       LG Soft India
- *
- * Created:      ?
- * Description:  CPLB exception handler
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#include <linux/linkage.h>
-#include <asm/cplb.h>
-#include <asm/entry.h>
-
-#ifdef CONFIG_EXCPT_IRQ_SYSC_L1
-.section .l1.text
-#else
-.text
-#endif
-
-.type _cplb_mgr, STT_FUNC;
-.type _panic_cplb_error, STT_FUNC;
-
-.align 2
-
-ENTRY(__cplb_hdr)
-	R2 = SEQSTAT;
-
-	/* Mask the contents of SEQSTAT and leave only EXCAUSE in R2 */
-	R2 <<= 26;
-	R2 >>= 26;
-
-	R1 = 0x23; /* Data access CPLB protection violation */
-	CC = R2 == R1;
-	IF !CC JUMP .Lnot_data_write;
-	R0 = 2;		/* is a write to data space*/
-	JUMP .Lis_icplb_miss;
-
-.Lnot_data_write:
-	R1 = 0x2C; /* CPLB miss on an instruction fetch */
-	CC = R2 == R1;
-	R0 = 0;		/* is_data_miss == False*/
-	IF CC JUMP .Lis_icplb_miss;
-
-	R1 = 0x26;
-	CC = R2 == R1;
-	IF !CC JUMP .Lunknown;
-
-	R0 = 1;		/* is_data_miss == True*/
-
-.Lis_icplb_miss:
-
-#if defined(CONFIG_BFIN_ICACHE) || defined(CONFIG_BFIN_DCACHE)
-# if defined(CONFIG_BFIN_ICACHE) && !defined(CONFIG_BFIN_DCACHE)
-	R1 = CPLB_ENABLE_ICACHE;
-# endif
-# if !defined(CONFIG_BFIN_ICACHE) && defined(CONFIG_BFIN_DCACHE)
-	R1 = CPLB_ENABLE_DCACHE;
-# endif
-# if defined(CONFIG_BFIN_ICACHE) && defined(CONFIG_BFIN_DCACHE)
-	R1 = CPLB_ENABLE_DCACHE | CPLB_ENABLE_ICACHE;
-# endif
-#else
-	R1 = 0;
-#endif
-
-	[--SP] = RETS;
-	CALL _cplb_mgr;
-	RETS = [SP++];
-	CC = R0 == 0;
-	IF !CC JUMP .Lnot_replaced;
-	RTS;
-
-/*
- * Diagnostic exception handlers
- */
-.Lunknown:
-	R0 = CPLB_UNKNOWN_ERR;
-	JUMP .Lcplb_error;
-
-.Lnot_replaced:
-	CC = R0 == CPLB_NO_UNLOCKED;
-	IF !CC JUMP .Lnext_check;
-	R0 = CPLB_NO_UNLOCKED;
-	JUMP .Lcplb_error;
-
-.Lnext_check:
-	CC = R0 == CPLB_NO_ADDR_MATCH;
-	IF !CC JUMP .Lnext_check2;
-	R0 = CPLB_NO_ADDR_MATCH;
-	JUMP .Lcplb_error;
-
-.Lnext_check2:
-	CC = R0 == CPLB_PROT_VIOL;
-	IF !CC JUMP .Lstrange_return_from_cplb_mgr;
-	R0 = CPLB_PROT_VIOL;
-	JUMP .Lcplb_error;
-
-.Lstrange_return_from_cplb_mgr:
-	IDLE;
-	CSYNC;
-	JUMP .Lstrange_return_from_cplb_mgr;
-
-.Lcplb_error:
-	R1 = sp;
-	SP += -12;
-	call _panic_cplb_error;
-	SP += 12;
-	JUMP.L _handle_bad_cplb;
-
-ENDPROC(__cplb_hdr)
diff --git a/arch/blackfin/kernel/cplb-nompu/cplbinfo.c b/arch/blackfin/kernel/cplb-nompu/cplbinfo.c
deleted file mode 100644
index 1e74f0b97996..000000000000
--- a/arch/blackfin/kernel/cplb-nompu/cplbinfo.c
+++ /dev/null
@@ -1,195 +0,0 @@
-/*
- * File:         arch/blackfin/mach-common/cplbinfo.c
- * Based on:
- * Author:       Sonic Zhang <sonic.zhang@analog.com>
- *
- * Created:      Jan. 2005
- * Description:  Display CPLB status
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/proc_fs.h>
-#include <linux/uaccess.h>
-
-#include <asm/cplbinit.h>
-#include <asm/blackfin.h>
-
-#define CPLB_I 1
-#define CPLB_D 2
-
-#define SYNC_SYS    SSYNC()
-#define SYNC_CORE   CSYNC()
-
-#define CPLB_BIT_PAGESIZE 0x30000
-
-static int page_size_table[4] = {
-	0x00000400,		/* 1K */
-	0x00001000,		/* 4K */
-	0x00100000,		/* 1M */
-	0x00400000		/* 4M */
-};
-
-static char page_size_string_table[][4] = { "1K", "4K", "1M", "4M" };
-
-static int cplb_find_entry(unsigned long *cplb_addr,
-			   unsigned long *cplb_data, unsigned long addr,
-			   unsigned long data)
-{
-	int ii;
-
-	for (ii = 0; ii < 16; ii++)
-		if (addr >= cplb_addr[ii] && addr < cplb_addr[ii] +
-		    page_size_table[(cplb_data[ii] & CPLB_BIT_PAGESIZE) >> 16]
-			&& (cplb_data[ii] == data))
-			return ii;
-
-	return -1;
-}
-
-static char *cplb_print_entry(char *buf, int type)
-{
-	unsigned long *p_addr = dpdt_table;
-	unsigned long *p_data = dpdt_table + 1;
-	unsigned long *p_icount = dpdt_swapcount_table;
-	unsigned long *p_ocount = dpdt_swapcount_table + 1;
-	unsigned long *cplb_addr = (unsigned long *)DCPLB_ADDR0;
-	unsigned long *cplb_data = (unsigned long *)DCPLB_DATA0;
-	int entry = 0, used_cplb = 0;
-
-	if (type == CPLB_I) {
-		buf += sprintf(buf, "Instruction CPLB entry:\n");
-		p_addr = ipdt_table;
-		p_data = ipdt_table + 1;
-		p_icount = ipdt_swapcount_table;
-		p_ocount = ipdt_swapcount_table + 1;
-		cplb_addr = (unsigned long *)ICPLB_ADDR0;
-		cplb_data = (unsigned long *)ICPLB_DATA0;
-	} else
-		buf += sprintf(buf, "Data CPLB entry:\n");
-
-	buf += sprintf(buf, "Address\t\tData\tSize\tValid\tLocked\tSwapin\tiCount\toCount\n");
-
-	while (*p_addr != 0xffffffff) {
-		entry = cplb_find_entry(cplb_addr, cplb_data, *p_addr, *p_data);
-		if (entry >= 0)
-			used_cplb |= 1 << entry;
-
-		buf +=
-		    sprintf(buf,
-			    "0x%08lx\t0x%05lx\t%s\t%c\t%c\t%2d\t%ld\t%ld\n",
-			    *p_addr, *p_data,
-			    page_size_string_table[(*p_data & 0x30000) >> 16],
-			    (*p_data & CPLB_VALID) ? 'Y' : 'N',
-			    (*p_data & CPLB_LOCK) ? 'Y' : 'N', entry, *p_icount,
-			    *p_ocount);
-
-		p_addr += 2;
-		p_data += 2;
-		p_icount += 2;
-		p_ocount += 2;
-	}
-
-	if (used_cplb != 0xffff) {
-		buf += sprintf(buf, "Unused/mismatched CPLBs:\n");
-
-		for (entry = 0; entry < 16; entry++)
-			if (0 == ((1 << entry) & used_cplb)) {
-				int flags = cplb_data[entry];
-				buf +=
-				    sprintf(buf,
-					    "%2d: 0x%08lx\t0x%05x\t%s\t%c\t%c\n",
-					    entry, cplb_addr[entry], flags,
-					    page_size_string_table[(flags &
-								    0x30000) >>
-								   16],
-					    (flags & CPLB_VALID) ? 'Y' : 'N',
-					    (flags & CPLB_LOCK) ? 'Y' : 'N');
-			}
-	}
-
-	buf += sprintf(buf, "\n");
-
-	return buf;
-}
-
-static int cplbinfo_proc_output(char *buf)
-{
-	char *p;
-
-	p = buf;
-
-	p += sprintf(p, "------------------ CPLB Information ------------------\n\n");
-
-	if (bfin_read_IMEM_CONTROL() & ENICPLB)
-		p = cplb_print_entry(p, CPLB_I);
-	else
-		p += sprintf(p, "Instruction CPLB is disabled.\n\n");
-
-	if (bfin_read_DMEM_CONTROL() & ENDCPLB)
-		p = cplb_print_entry(p, CPLB_D);
-	else
-		p += sprintf(p, "Data CPLB is disabled.\n");
-
-	return p - buf;
-}
-
-static int cplbinfo_read_proc(char *page, char **start, off_t off,
-			      int count, int *eof, void *data)
-{
-	int len;
-
-	len = cplbinfo_proc_output(page);
-	if (len <= off + count)
-		*eof = 1;
-	*start = page + off;
-	len -= off;
-	if (len > count)
-		len = count;
-	if (len < 0)
-		len = 0;
-	return len;
-}
-
-static int __init cplbinfo_init(void)
-{
-	struct proc_dir_entry *entry;
-
-	entry = create_proc_entry("cplbinfo", 0, NULL);
-	if (!entry)
-		return -ENOMEM;
-
-	entry->read_proc = cplbinfo_read_proc;
-	entry->data = NULL;
-
-	return 0;
-}
-
-static void __exit cplbinfo_exit(void)
-{
-	remove_proc_entry("cplbinfo", NULL);
-}
-
-module_init(cplbinfo_init);
-module_exit(cplbinfo_exit);
diff --git a/arch/blackfin/kernel/cplb-nompu/cplbinit.c b/arch/blackfin/kernel/cplb-nompu/cplbinit.c
index 2debc900e246..0e28f7595733 100644
--- a/arch/blackfin/kernel/cplb-nompu/cplbinit.c
+++ b/arch/blackfin/kernel/cplb-nompu/cplbinit.c
@@ -20,445 +20,152 @@
  * to the Free Software Foundation, Inc.,
  * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  */
+
 #include <linux/module.h>
 
 #include <asm/blackfin.h>
 #include <asm/cacheflush.h>
 #include <asm/cplb.h>
 #include <asm/cplbinit.h>
+#include <asm/mem_map.h>
 
-#define CPLB_MEM CONFIG_MAX_MEM_SIZE
-
-/*
-* Number of required data CPLB switchtable entries
-* MEMSIZE / 4 (we mostly install 4M page size CPLBs
-* approx 16 for smaller 1MB page size CPLBs for allignment purposes
-* 1 for L1 Data Memory
-* possibly 1 for L2 Data Memory
-* 1 for CONFIG_DEBUG_HUNT_FOR_ZERO
-* 1 for ASYNC Memory
-*/
-#define MAX_SWITCH_D_CPLBS (((CPLB_MEM / 4) + 16 + 1 + 1 + 1 \
-				 + ASYNC_MEMORY_CPLB_COVERAGE) * 2)
-
-/*
-* Number of required instruction CPLB switchtable entries
-* MEMSIZE / 4 (we mostly install 4M page size CPLBs
-* approx 12 for smaller 1MB page size CPLBs for allignment purposes
-* 1 for L1 Instruction Memory
-* possibly 1 for L2 Instruction Memory
-* 1 for CONFIG_DEBUG_HUNT_FOR_ZERO
-*/
-#define MAX_SWITCH_I_CPLBS (((CPLB_MEM / 4) + 12 + 1 + 1 + 1) * 2)
-
-
-u_long icplb_table[MAX_CPLBS + 1];
-u_long dcplb_table[MAX_CPLBS + 1];
-
-#ifdef CONFIG_CPLB_SWITCH_TAB_L1
-# define PDT_ATTR __attribute__((l1_data))
-#else
-# define PDT_ATTR
-#endif
-
-u_long ipdt_table[MAX_SWITCH_I_CPLBS + 1] PDT_ATTR;
-u_long dpdt_table[MAX_SWITCH_D_CPLBS + 1] PDT_ATTR;
+struct cplb_entry icplb_tbl[NR_CPUS][MAX_CPLBS] PDT_ATTR;
+struct cplb_entry dcplb_tbl[NR_CPUS][MAX_CPLBS] PDT_ATTR;
 
-#ifdef CONFIG_CPLB_INFO
-u_long ipdt_swapcount_table[MAX_SWITCH_I_CPLBS] PDT_ATTR;
-u_long dpdt_swapcount_table[MAX_SWITCH_D_CPLBS] PDT_ATTR;
-#endif
+int first_switched_icplb PDT_ATTR;
+int first_switched_dcplb PDT_ATTR;
 
-struct s_cplb {
-	struct cplb_tab init_i;
-	struct cplb_tab init_d;
-	struct cplb_tab switch_i;
-	struct cplb_tab switch_d;
-};
+struct cplb_boundary dcplb_bounds[9] PDT_ATTR;
+struct cplb_boundary icplb_bounds[7] PDT_ATTR;
 
-#if defined(CONFIG_BFIN_DCACHE) || defined(CONFIG_BFIN_ICACHE)
-static struct cplb_desc cplb_data[] = {
-	{
-		.start = 0,
-		.end = SIZE_1K,
-		.psize = SIZE_1K,
-		.attr = INITIAL_T | SWITCH_T | I_CPLB | D_CPLB,
-		.i_conf = SDRAM_OOPS,
-		.d_conf = SDRAM_OOPS,
-#if defined(CONFIG_DEBUG_HUNT_FOR_ZERO)
-		.valid = 1,
-#else
-		.valid = 0,
-#endif
-		.name = "Zero Pointer Guard Page",
-	},
-	{
-		.start = L1_CODE_START,
-		.end = L1_CODE_START + L1_CODE_LENGTH,
-		.psize = SIZE_4M,
-		.attr = INITIAL_T | SWITCH_T | I_CPLB,
-		.i_conf = L1_IMEMORY,
-		.d_conf = 0,
-		.valid = 1,
-		.name = "L1 I-Memory",
-	},
-	{
-		.start = L1_DATA_A_START,
-		.end = L1_DATA_B_START + L1_DATA_B_LENGTH,
-		.psize = SIZE_4M,
-		.attr = INITIAL_T | SWITCH_T | D_CPLB,
-		.i_conf = 0,
-		.d_conf = L1_DMEMORY,
-#if ((L1_DATA_A_LENGTH > 0) || (L1_DATA_B_LENGTH > 0))
-		.valid = 1,
-#else
-		.valid = 0,
-#endif
-		.name = "L1 D-Memory",
-	},
-	{
-		.start = 0,
-		.end = 0,  /* dynamic */
-		.psize = 0,
-		.attr = INITIAL_T | SWITCH_T | I_CPLB | D_CPLB,
-		.i_conf = SDRAM_IGENERIC,
-		.d_conf = SDRAM_DGENERIC,
-		.valid = 1,
-		.name = "Kernel Memory",
-	},
-	{
-		.start = 0, /* dynamic */
-		.end = 0, /* dynamic */
-		.psize = 0,
-		.attr = INITIAL_T | SWITCH_T | D_CPLB,
-		.i_conf = SDRAM_IGENERIC,
-		.d_conf = SDRAM_DNON_CHBL,
-		.valid = 1,
-		.name = "uClinux MTD Memory",
-	},
-	{
-		.start = 0, /* dynamic */
-		.end = 0,   /* dynamic */
-		.psize = SIZE_1M,
-		.attr = INITIAL_T | SWITCH_T | D_CPLB,
-		.d_conf = SDRAM_DNON_CHBL,
-		.valid = 1,
-		.name = "Uncached DMA Zone",
-	},
-	{
-		.start = 0, /* dynamic */
-		.end = 0, /* dynamic */
-		.psize = 0,
-		.attr = SWITCH_T | D_CPLB,
-		.i_conf = 0, /* dynamic */
-		.d_conf = 0, /* dynamic */
-		.valid = 1,
-		.name = "Reserved Memory",
-	},
-	{
-		.start = ASYNC_BANK0_BASE,
-		.end = ASYNC_BANK3_BASE + ASYNC_BANK3_SIZE,
-		.psize = 0,
-		.attr = SWITCH_T | D_CPLB,
-		.d_conf = SDRAM_EBIU,
-		.valid = 1,
-		.name = "Asynchronous Memory Banks",
-	},
-	{
-		.start = L2_START,
-		.end = L2_START + L2_LENGTH,
-		.psize = SIZE_1M,
-		.attr = SWITCH_T | I_CPLB | D_CPLB,
-		.i_conf = L2_IMEMORY,
-		.d_conf = L2_DMEMORY,
-		.valid = (L2_LENGTH > 0),
-		.name = "L2 Memory",
-	},
-	{
-		.start = BOOT_ROM_START,
-		.end = BOOT_ROM_START + BOOT_ROM_LENGTH,
-		.psize = SIZE_1M,
-		.attr = SWITCH_T | I_CPLB | D_CPLB,
-		.i_conf = SDRAM_IGENERIC,
-		.d_conf = SDRAM_DGENERIC,
-		.valid = 1,
-		.name = "On-Chip BootROM",
-	},
-};
+int icplb_nr_bounds PDT_ATTR;
+int dcplb_nr_bounds PDT_ATTR;
 
-static u16 __init lock_kernel_check(u32 start, u32 end)
+void __init generate_cplb_tables_cpu(unsigned int cpu)
 {
-	if (start >= (u32)_end || end <= (u32)_stext)
-		return 0;
+	int i_d, i_i;
+	unsigned long addr;
 
-	/* This cplb block overlapped with kernel area. */
-	return IN_KERNEL;
-}
+	struct cplb_entry *d_tbl = dcplb_tbl[cpu];
+	struct cplb_entry *i_tbl = icplb_tbl[cpu];
 
-static unsigned short __init
-fill_cplbtab(struct cplb_tab *table,
-	     unsigned long start, unsigned long end,
-	     unsigned long block_size, unsigned long cplb_data)
-{
-	int i;
+	printk(KERN_INFO "NOMPU: setting up cplb tables\n");
 
-	switch (block_size) {
-	case SIZE_4M:
-		i = 3;
-		break;
-	case SIZE_1M:
-		i = 2;
-		break;
-	case SIZE_4K:
-		i = 1;
-		break;
-	case SIZE_1K:
-	default:
-		i = 0;
-		break;
-	}
-
-	cplb_data = (cplb_data & ~(3 << 16)) | (i << 16);
-
-	while ((start < end) && (table->pos < table->size)) {
+	i_d = i_i = 0;
 
-		table->tab[table->pos++] = start;
+	/* Set up the zero page.  */
+	d_tbl[i_d].addr = 0;
+	d_tbl[i_d++].data = SDRAM_OOPS | PAGE_SIZE_1KB;
 
-		if (lock_kernel_check(start, start + block_size) == IN_KERNEL)
-			table->tab[table->pos++] =
-			    cplb_data | CPLB_LOCK | CPLB_DIRTY;
-		else
-			table->tab[table->pos++] = cplb_data;
+	/* Cover kernel memory with 4M pages.  */
+	addr = 0;
 
-		start += block_size;
+	for (; addr < memory_start; addr += 4 * 1024 * 1024) {
+		d_tbl[i_d].addr = addr;
+		d_tbl[i_d++].data = SDRAM_DGENERIC | PAGE_SIZE_4MB;
+		i_tbl[i_i].addr = addr;
+		i_tbl[i_i++].data = SDRAM_IGENERIC | PAGE_SIZE_4MB;
 	}
-	return 0;
-}
 
-static unsigned short __init
-close_cplbtab(struct cplb_tab *table)
-{
-
-	while (table->pos < table->size) {
-
-		table->tab[table->pos++] = 0;
-		table->tab[table->pos++] = 0; /* !CPLB_VALID */
+	/* Cover L1 memory.  One 4M area for code and data each is enough.  */
+	if (L1_DATA_A_LENGTH || L1_DATA_B_LENGTH) {
+		d_tbl[i_d].addr = L1_DATA_A_START;
+		d_tbl[i_d++].data = L1_DMEMORY | PAGE_SIZE_4MB;
 	}
-	return 0;
-}
+	i_tbl[i_i].addr = L1_CODE_START;
+	i_tbl[i_i++].data = L1_IMEMORY | PAGE_SIZE_4MB;
 
-/* helper function */
-static void __init
-__fill_code_cplbtab(struct cplb_tab *t, int i, u32 a_start, u32 a_end)
-{
-	if (cplb_data[i].psize) {
-		fill_cplbtab(t,
-				cplb_data[i].start,
-				cplb_data[i].end,
-				cplb_data[i].psize,
-				cplb_data[i].i_conf);
-	} else {
-#if defined(CONFIG_BFIN_ICACHE)
-		if (ANOMALY_05000263 && i == SDRAM_KERN) {
-			fill_cplbtab(t,
-					cplb_data[i].start,
-					cplb_data[i].end,
-					SIZE_4M,
-					cplb_data[i].i_conf);
-		} else
-#endif
-		{
-			fill_cplbtab(t,
-					cplb_data[i].start,
-					a_start,
-					SIZE_1M,
-					cplb_data[i].i_conf);
-			fill_cplbtab(t,
-					a_start,
-					a_end,
-					SIZE_4M,
-					cplb_data[i].i_conf);
-			fill_cplbtab(t, a_end,
-					cplb_data[i].end,
-					SIZE_1M,
-					cplb_data[i].i_conf);
-		}
-	}
-}
+	first_switched_dcplb = i_d;
+	first_switched_icplb = i_i;
 
-static void __init
-__fill_data_cplbtab(struct cplb_tab *t, int i, u32 a_start, u32 a_end)
-{
-	if (cplb_data[i].psize) {
-		fill_cplbtab(t,
-				cplb_data[i].start,
-				cplb_data[i].end,
-				cplb_data[i].psize,
-				cplb_data[i].d_conf);
-	} else {
-		fill_cplbtab(t,
-				cplb_data[i].start,
-				a_start, SIZE_1M,
-				cplb_data[i].d_conf);
-		fill_cplbtab(t, a_start,
-				a_end, SIZE_4M,
-				cplb_data[i].d_conf);
-		fill_cplbtab(t, a_end,
-				cplb_data[i].end,
-				SIZE_1M,
-				cplb_data[i].d_conf);
-	}
+	BUG_ON(first_switched_dcplb > MAX_CPLBS);
+	BUG_ON(first_switched_icplb > MAX_CPLBS);
+
+	while (i_d < MAX_CPLBS)
+		d_tbl[i_d++].data = 0;
+	while (i_i < MAX_CPLBS)
+		i_tbl[i_i++].data = 0;
 }
 
-void __init generate_cplb_tables(void)
+void __init generate_cplb_tables_all(void)
 {
+	int i_d, i_i;
 
-	u16 i, j, process;
-	u32 a_start, a_end, as, ae, as_1m;
-
-	struct cplb_tab *t_i = NULL;
-	struct cplb_tab *t_d = NULL;
-	struct s_cplb cplb;
-
-	printk(KERN_INFO "NOMPU: setting up cplb tables for global access\n");
-
-	cplb.init_i.size = MAX_CPLBS;
-	cplb.init_d.size = MAX_CPLBS;
-	cplb.switch_i.size = MAX_SWITCH_I_CPLBS;
-	cplb.switch_d.size = MAX_SWITCH_D_CPLBS;
-
-	cplb.init_i.pos = 0;
-	cplb.init_d.pos = 0;
-	cplb.switch_i.pos = 0;
-	cplb.switch_d.pos = 0;
-
-	cplb.init_i.tab = icplb_table;
-	cplb.init_d.tab = dcplb_table;
-	cplb.switch_i.tab = ipdt_table;
-	cplb.switch_d.tab = dpdt_table;
-
-	cplb_data[SDRAM_KERN].end = memory_end;
-
+	i_d = 0;
+	/* Normal RAM, including MTD FS.  */
 #ifdef CONFIG_MTD_UCLINUX
-	cplb_data[SDRAM_RAM_MTD].start = memory_mtd_start;
-	cplb_data[SDRAM_RAM_MTD].end = memory_mtd_start + mtd_size;
-	cplb_data[SDRAM_RAM_MTD].valid = mtd_size > 0;
-# if defined(CONFIG_ROMFS_FS)
-	cplb_data[SDRAM_RAM_MTD].attr |= I_CPLB;
-
-	/*
-	 * The ROMFS_FS size is often not multiple of 1MB.
-	 * This can cause multiple CPLB sets covering the same memory area.
-	 * This will then cause multiple CPLB hit exceptions.
-	 * Workaround: We ensure a contiguous memory area by extending the kernel
-	 * memory section over the mtd section.
-	 * For ROMFS_FS memory must be covered with ICPLBs anyways.
-	 * So there is no difference between kernel and mtd memory setup.
-	 */
-
-	cplb_data[SDRAM_KERN].end = memory_mtd_start + mtd_size;;
-	cplb_data[SDRAM_RAM_MTD].valid = 0;
-
-# endif
+	dcplb_bounds[i_d].eaddr = memory_mtd_start + mtd_size;
 #else
-	cplb_data[SDRAM_RAM_MTD].valid = 0;
+	dcplb_bounds[i_d].eaddr = memory_end;
 #endif
+	dcplb_bounds[i_d++].data = SDRAM_DGENERIC;
+	/* DMA uncached region.  */
+	if (DMA_UNCACHED_REGION) {
+		dcplb_bounds[i_d].eaddr = _ramend;
+		dcplb_bounds[i_d++].data = SDRAM_DNON_CHBL;
+	}
+	if (_ramend != physical_mem_end) {
+		/* Reserved memory.  */
+		dcplb_bounds[i_d].eaddr = physical_mem_end;
+		dcplb_bounds[i_d++].data = (reserved_mem_dcache_on ?
+					    SDRAM_DGENERIC : SDRAM_DNON_CHBL);
+	}
+	/* Addressing hole up to the async bank.  */
+	dcplb_bounds[i_d].eaddr = ASYNC_BANK0_BASE;
+	dcplb_bounds[i_d++].data = 0;
+	/* ASYNC banks.  */
+	dcplb_bounds[i_d].eaddr = ASYNC_BANK3_BASE + ASYNC_BANK3_SIZE;
+	dcplb_bounds[i_d++].data = SDRAM_EBIU;
+	/* Addressing hole up to BootROM.  */
+	dcplb_bounds[i_d].eaddr = BOOT_ROM_START;
+	dcplb_bounds[i_d++].data = 0;
+	/* BootROM -- largest one should be less than 1 meg.  */
+	dcplb_bounds[i_d].eaddr = BOOT_ROM_START + (1 * 1024 * 1024);
+	dcplb_bounds[i_d++].data = SDRAM_DGENERIC;
+	if (L2_LENGTH) {
+		/* Addressing hole up to L2 SRAM.  */
+		dcplb_bounds[i_d].eaddr = L2_START;
+		dcplb_bounds[i_d++].data = 0;
+		/* L2 SRAM.  */
+		dcplb_bounds[i_d].eaddr = L2_START + L2_LENGTH;
+		dcplb_bounds[i_d++].data = L2_DMEMORY;
+	}
+	dcplb_nr_bounds = i_d;
+	BUG_ON(dcplb_nr_bounds > ARRAY_SIZE(dcplb_bounds));
 
-	cplb_data[SDRAM_DMAZ].start = _ramend - DMA_UNCACHED_REGION;
-	cplb_data[SDRAM_DMAZ].end = _ramend;
-
-	cplb_data[RES_MEM].start = _ramend;
-	cplb_data[RES_MEM].end = physical_mem_end;
-
-	if (reserved_mem_dcache_on)
-		cplb_data[RES_MEM].d_conf = SDRAM_DGENERIC;
-	else
-		cplb_data[RES_MEM].d_conf = SDRAM_DNON_CHBL;
-
-	if (reserved_mem_icache_on)
-		cplb_data[RES_MEM].i_conf = SDRAM_IGENERIC;
-	else
-		cplb_data[RES_MEM].i_conf = SDRAM_INON_CHBL;
-
-	for (i = ZERO_P; i < ARRAY_SIZE(cplb_data); ++i) {
-		if (!cplb_data[i].valid)
-			continue;
-
-		as_1m = cplb_data[i].start % SIZE_1M;
-
-		/* We need to make sure all sections are properly 1M aligned
-		 * However between Kernel Memory and the Kernel mtd section, depending on the
-		 * rootfs size, there can be overlapping memory areas.
-		 */
-
-		if (as_1m && i != L1I_MEM && i != L1D_MEM) {
+	i_i = 0;
+	/* Normal RAM, including MTD FS.  */
 #ifdef CONFIG_MTD_UCLINUX
-			if (i == SDRAM_RAM_MTD) {
-				if ((cplb_data[SDRAM_KERN].end + 1) > cplb_data[SDRAM_RAM_MTD].start)
-					cplb_data[SDRAM_RAM_MTD].start = (cplb_data[i].start & (-2*SIZE_1M)) + SIZE_1M;
-				else
-					cplb_data[SDRAM_RAM_MTD].start = (cplb_data[i].start & (-2*SIZE_1M));
-			} else
+	icplb_bounds[i_i].eaddr = memory_mtd_start + mtd_size;
+#else
+	icplb_bounds[i_i].eaddr = memory_end;
 #endif
-				printk(KERN_WARNING "Unaligned Start of %s at 0x%X\n",
-				       cplb_data[i].name, cplb_data[i].start);
-		}
-
-		as = cplb_data[i].start % SIZE_4M;
-		ae = cplb_data[i].end % SIZE_4M;
-
-		if (as)
-			a_start = cplb_data[i].start + (SIZE_4M - (as));
-		else
-			a_start = cplb_data[i].start;
-
-		a_end = cplb_data[i].end - ae;
-
-		for (j = INITIAL_T; j <= SWITCH_T; j++) {
-
-			switch (j) {
-			case INITIAL_T:
-				if (cplb_data[i].attr & INITIAL_T) {
-					t_i = &cplb.init_i;
-					t_d = &cplb.init_d;
-					process = 1;
-				} else
-					process = 0;
-				break;
-			case SWITCH_T:
-				if (cplb_data[i].attr & SWITCH_T) {
-					t_i = &cplb.switch_i;
-					t_d = &cplb.switch_d;
-					process = 1;
-				} else
-					process = 0;
-				break;
-			default:
-					process = 0;
-				break;
-			}
-
-			if (!process)
-				continue;
-			if (cplb_data[i].attr & I_CPLB)
-				__fill_code_cplbtab(t_i, i, a_start, a_end);
-
-			if (cplb_data[i].attr & D_CPLB)
-				__fill_data_cplbtab(t_d, i, a_start, a_end);
-		}
+	icplb_bounds[i_i++].data = SDRAM_IGENERIC;
+	/* DMA uncached region.  */
+	if (DMA_UNCACHED_REGION) {
+		icplb_bounds[i_i].eaddr = _ramend;
+		icplb_bounds[i_i++].data = 0;
 	}
-
-/* close tables */
-
-	close_cplbtab(&cplb.init_i);
-	close_cplbtab(&cplb.init_d);
-
-	cplb.init_i.tab[cplb.init_i.pos] = -1;
-	cplb.init_d.tab[cplb.init_d.pos] = -1;
-	cplb.switch_i.tab[cplb.switch_i.pos] = -1;
-	cplb.switch_d.tab[cplb.switch_d.pos] = -1;
-
+	if (_ramend != physical_mem_end) {
+		/* Reserved memory.  */
+		icplb_bounds[i_i].eaddr = physical_mem_end;
+		icplb_bounds[i_i++].data = (reserved_mem_icache_on ?
+					    SDRAM_IGENERIC : SDRAM_INON_CHBL);
+	}
+	/* Addressing hole up to BootROM.  */
+	icplb_bounds[i_i].eaddr = BOOT_ROM_START;
+	icplb_bounds[i_i++].data = 0;
+	/* BootROM -- largest one should be less than 1 meg.  */
+	icplb_bounds[i_i].eaddr = BOOT_ROM_START + (1 * 1024 * 1024);
+	icplb_bounds[i_i++].data = SDRAM_IGENERIC;
+	if (L2_LENGTH) {
+		/* Addressing hole up to L2 SRAM, including the async bank.  */
+		icplb_bounds[i_i].eaddr = L2_START;
+		icplb_bounds[i_i++].data = 0;
+		/* L2 SRAM.  */
+		icplb_bounds[i_i].eaddr = L2_START + L2_LENGTH;
+		icplb_bounds[i_i++].data = L2_IMEMORY;
+	}
+	icplb_nr_bounds = i_i;
+	BUG_ON(icplb_nr_bounds > ARRAY_SIZE(icplb_bounds));
 }
-
-#endif
-
diff --git a/arch/blackfin/kernel/cplb-nompu/cplbmgr.S b/arch/blackfin/kernel/cplb-nompu/cplbmgr.S
deleted file mode 100644
index f5cf3accef37..000000000000
--- a/arch/blackfin/kernel/cplb-nompu/cplbmgr.S
+++ /dev/null
@@ -1,646 +0,0 @@
-/*
- * File:         arch/blackfin/mach-common/cplbmgtr.S
- * Based on:
- * Author:       LG Soft India
- *
- * Created:      ?
- * Description:  CPLB replacement routine for CPLB mismatch
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-/* Usage: int _cplb_mgr(is_data_miss,int enable_cache)
- * is_data_miss==2 => Mark as Dirty, write to the clean data page
- * is_data_miss==1 => Replace a data CPLB.
- * is_data_miss==0 => Replace an instruction CPLB.
- *
- * Returns:
- * CPLB_RELOADED	=> Successfully updated CPLB table.
- * CPLB_NO_UNLOCKED	=> All CPLBs are locked, so cannot be evicted.
- *			   This indicates that the CPLBs in the configuration
- *			   tablei are badly configured, as this should never
- *			   occur.
- * CPLB_NO_ADDR_MATCH	=> The address being accessed, that triggered the
- *			   exception, is not covered by any of the CPLBs in
- *			   the configuration table. The application is
- *			   presumably misbehaving.
- * CPLB_PROT_VIOL	=> The address being accessed, that triggered the
- *			   exception, was not a first-write to a clean Write
- *			   Back Data page, and so presumably is a genuine
- *			   violation of the page's protection attributes.
- *			   The application is misbehaving.
- */
-
-#include <linux/linkage.h>
-#include <asm/blackfin.h>
-#include <asm/cplb.h>
-
-#ifdef CONFIG_EXCPT_IRQ_SYSC_L1
-.section .l1.text
-#else
-.text
-#endif
-
-.align 2;
-ENTRY(_cplb_mgr)
-
-	[--SP]=( R7:4,P5:3 );
-
-	CC = R0 == 2;
-	IF CC JUMP .Ldcplb_write;
-
-	CC = R0 == 0;
-	IF !CC JUMP .Ldcplb_miss_compare;
-
-	/* ICPLB Miss Exception. We need to choose one of the
-	* currently-installed CPLBs, and replace it with one
-	* from the configuration table.
-	*/
-
-	/* A multi-word instruction can cross a page boundary. This means the
-	 * first part of the instruction can be in a valid page, but the
-	 * second part is not, and hence generates the instruction miss.
-	 * However, the fault address is for the start of the instruction,
-	 * not the part that's in the bad page. Therefore, we have to check
-	 * whether the fault address applies to a page that is already present
-	 * in the table.
-	 */
-
-	P4.L = LO(ICPLB_FAULT_ADDR);
-	P4.H = HI(ICPLB_FAULT_ADDR);
-
-	P1 = 16;
-	P5.L = _page_size_table;
-	P5.H = _page_size_table;
-
-	P0.L = LO(ICPLB_DATA0);
-	P0.H = HI(ICPLB_DATA0);
-	R4 = [P4];		/* Get faulting address*/
-	R6 = 64;		/* Advance past the fault address, which*/
-	R6 = R6 + R4;		/* we'll use if we find a match*/
-	R3 = ((16 << 8) | 2);	/* Extract mask, two bits at posn 16 */
-
-	R5 = 0;
-.Lisearch:
-
-	R1 = [P0-0x100];	/* Address for this CPLB */
-
-	R0 = [P0++];		/* Info for this CPLB*/
-	CC = BITTST(R0,0);	/* Is the CPLB valid?*/
-	IF !CC JUMP .Lnomatch;	/* Skip it, if not.*/
-	CC = R4 < R1(IU);	/* If fault address less than page start*/
-	IF CC JUMP .Lnomatch;	/* then skip this one.*/
-	R2 = EXTRACT(R0,R3.L) (Z);	/* Get page size*/
-	P1 = R2;
-	P1 = P5 + (P1<<2);	/* index into page-size table*/
-	R2 = [P1];		/* Get the page size*/
-	R1 = R1 + R2;		/* and add to page start, to get page end*/
-	CC = R4 < R1(IU);	/* and see whether fault addr is in page.*/
-	IF !CC R4 = R6;		/* If so, advance the address and finish loop.*/
-	IF !CC JUMP .Lisearch_done;
-.Lnomatch:
-	/* Go around again*/
-	R5 += 1;
-	CC = BITTST(R5, 4);	/* i.e CC = R5 >= 16*/
-	IF !CC JUMP .Lisearch;
-
-.Lisearch_done:
-	I0 = R4;		/* Fault address we'll search for*/
-
-	/* set up pointers */
-	P0.L = LO(ICPLB_DATA0);
-	P0.H = HI(ICPLB_DATA0);
-
-	/* The replacement procedure for ICPLBs */
-
-	P4.L = LO(IMEM_CONTROL);
-	P4.H = HI(IMEM_CONTROL);
-
-	/* Turn off CPLBs while we work, necessary according to HRM before
-	 * modifying CPLB descriptors
-	 */
-	R5 = [P4];		/* Control Register*/
-	BITCLR(R5,ENICPLB_P);
-	CLI R1;
-	SSYNC;		/* SSYNC required before writing to IMEM_CONTROL. */
-	.align 8;
-	[P4] = R5;
-	SSYNC;
-	STI R1;
-
-	R1 = -1;		/* end point comparison */
-	R3 = 16;		/* counter */
-
-	/* Search through CPLBs for first non-locked entry */
-	/* Overwrite it by moving everyone else up by 1 */
-.Licheck_lock:
-	R0 = [P0++];
-	R3 = R3 + R1;
-	CC = R3 == R1;
-	IF CC JUMP .Lall_locked;
-	CC = BITTST(R0, 0);		/* an invalid entry is good */
-	IF !CC JUMP .Lifound_victim;
-	CC = BITTST(R0,1);		/* but a locked entry isn't */
-	IF CC JUMP .Licheck_lock;
-
-.Lifound_victim:
-#ifdef CONFIG_CPLB_INFO
-	R7 = [P0 - 0x104];
-	P2.L = _ipdt_table;
-	P2.H = _ipdt_table;
-	P3.L = _ipdt_swapcount_table;
-	P3.H = _ipdt_swapcount_table;
-	P3 += -4;
-.Licount:
-	R2 = [P2];	/* address from config table */
-	P2 += 8;
-	P3 += 8;
-	CC = R2==-1;
-	IF CC JUMP .Licount_done;
-	CC = R7==R2;
-	IF !CC JUMP .Licount;
-	R7 = [P3];
-	R7 += 1;
-	[P3] = R7;
-	CSYNC;
-.Licount_done:
-#endif
-	LC0=R3;
-	LSETUP(.Lis_move,.Lie_move) LC0;
-.Lis_move:
-	R0 = [P0];
-	[P0 - 4] = R0;
-	R0 = [P0 - 0x100];
-	[P0-0x104] = R0;
-.Lie_move:
-	P0+=4;
-
-	/* Clear ICPLB_DATA15, in case we don't find a replacement
-	 * otherwise, we would have a duplicate entry, and will crash
-	 */
-	R0 = 0;
-	[P0 - 4] = R0;
-
-	/* We've made space in the ICPLB table, so that ICPLB15
-	 * is now free to be overwritten. Next, we have to determine
-	 * which CPLB we need to install, from the configuration
-	 * table. This is a matter of getting the start-of-page
-	 * addresses and page-lengths from the config table, and
-	 * determining whether the fault address falls within that
-	 * range.
- 	 */
-
-	P2.L = _ipdt_table;
-	P2.H = _ipdt_table;
-#ifdef	CONFIG_CPLB_INFO
-	P3.L = _ipdt_swapcount_table;
-	P3.H = _ipdt_swapcount_table;
-	P3 += -8;
-#endif
-	P0.L = _page_size_table;
-	P0.H = _page_size_table;
-
-	/* Retrieve our fault address (which may have been advanced
-	 * because the faulting instruction crossed a page boundary).
-	 */
-
-	R0 = I0;
-
-	/* An extraction pattern, to get the page-size bits from
-	 * the CPLB data entry. Bits 16-17, so two bits at posn 16.
-	 */
-
-	R1 = ((16<<8)|2);
-.Linext:	R4 = [P2++];	/* address from config table */
-	R2 = [P2++];	/* data from config table */
-#ifdef	CONFIG_CPLB_INFO
-	P3 += 8;
-#endif
-
-	CC = R4 == -1;	/* End of config table*/
-	IF CC JUMP .Lno_page_in_table;
-
-	/* See if failed address > start address */
-	CC = R4 <= R0(IU);
-	IF !CC JUMP .Linext;
-
-	/* extract page size (17:16)*/
-	R3 = EXTRACT(R2, R1.L) (Z);
-
-	/* add page size to addr to get range */
-
-	P5 = R3;
-	P5 = P0 + (P5 << 2);	/* scaled, for int access*/
-	R3 = [P5];
-	R3 = R3 + R4;
-
-	/* See if failed address < (start address + page size) */
-	CC = R0 < R3(IU);
-	IF !CC JUMP .Linext;
-
-	/* We've found a CPLB in the config table that covers
-	 * the faulting address, so install this CPLB into the
-	 * last entry of the table.
-	 */
-
-	P1.L = LO(ICPLB_DATA15);		/* ICPLB_DATA15 */
-	P1.H = HI(ICPLB_DATA15);
-	[P1] = R2;
-	[P1-0x100] = R4;
-#ifdef	CONFIG_CPLB_INFO
-	R3 = [P3];
-	R3 += 1;
-	[P3] = R3;
-#endif
-
-	/* P4 points to IMEM_CONTROL, and R5 contains its old
-	 * value, after we disabled ICPLBS. Re-enable them.
-	 */
-
-	BITSET(R5,ENICPLB_P);
-	CLI R2;
-	SSYNC;		/* SSYNC required before writing to IMEM_CONTROL. */
-	.align 8;
-	[P4] = R5;
-	SSYNC;
-	STI R2;
-
-	( R7:4,P5:3 ) = [SP++];
-	R0 = CPLB_RELOADED;
-	RTS;
-
-/* FAILED CASES*/
-.Lno_page_in_table:
-	R0 = CPLB_NO_ADDR_MATCH;
-	JUMP .Lfail_ret;
-
-.Lall_locked:
-	R0 = CPLB_NO_UNLOCKED;
-	JUMP .Lfail_ret;
-
-.Lprot_violation:
-	R0 = CPLB_PROT_VIOL;
-
-.Lfail_ret:
-	/* Make sure we turn protection/cache back on, even in the failing case */
-	BITSET(R5,ENICPLB_P);
-	CLI R2;
-	SSYNC;          /* SSYNC required before writing to IMEM_CONTROL. */
-	.align 8;
-	[P4] = R5;
-	SSYNC;
-	STI R2;
-
-	( R7:4,P5:3 ) = [SP++];
-	RTS;
-
-.Ldcplb_write:
-
-	/* if a DCPLB is marked as write-back (CPLB_WT==0), and
-	 * it is clean (CPLB_DIRTY==0), then a write to the
-	 * CPLB's page triggers a protection violation. We have to
-	 * mark the CPLB as dirty, to indicate that there are
-	 * pending writes associated with the CPLB.
-	 */
-
-	P4.L = LO(DCPLB_STATUS);
-	P4.H = HI(DCPLB_STATUS);
-	P3.L = LO(DCPLB_DATA0);
-	P3.H = HI(DCPLB_DATA0);
-	R5 = [P4];
-
-	/* A protection violation can be caused by more than just writes
-	 * to a clean WB page, so we have to ensure that:
-	 * - It's a write
-	 * - to a clean WB page
-	 * - and is allowed in the mode the access occurred.
-	 */
-
-	CC = BITTST(R5, 16);	/* ensure it was a write*/
-	IF !CC JUMP .Lprot_violation;
-
-	/* to check the rest, we have to retrieve the DCPLB.*/
-
-	/* The low half of DCPLB_STATUS is a bit mask*/
-
-	R2 = R5.L (Z);	/* indicating which CPLB triggered the event.*/
-	R3 = 30;	/* so we can use this to determine the offset*/
-	R2.L = SIGNBITS R2;
-	R2 = R2.L (Z);	/* into the DCPLB table.*/
-	R3 = R3 - R2;
-	P4 = R3;
-	P3 = P3 + (P4<<2);
-	R3 = [P3];	/* Retrieve the CPLB*/
-
-	/* Now we can check whether it's a clean WB page*/
-
-	CC = BITTST(R3, 14);	/* 0==WB, 1==WT*/
-	IF CC JUMP .Lprot_violation;
-	CC = BITTST(R3, 7);	/* 0 == clean, 1 == dirty*/
-	IF CC JUMP .Lprot_violation;
-
-	/* Check whether the write is allowed in the mode that was active.*/
-
-	R2 = 1<<3;		/* checking write in user mode*/
-	CC = BITTST(R5, 17);	/* 0==was user, 1==was super*/
-	R5 = CC;
-	R2 <<= R5;		/* if was super, check write in super mode*/
-	R2 = R3 & R2;
-	CC = R2 == 0;
-	IF CC JUMP .Lprot_violation;
-
-	/* It's a genuine write-to-clean-page.*/
-
-	BITSET(R3, 7);		/* mark as dirty*/
-	[P3] = R3;		/* and write back.*/
-	NOP;
-	CSYNC;
-	( R7:4,P5:3 ) = [SP++];
-	R0 = CPLB_RELOADED;
-	RTS;
-
-.Ldcplb_miss_compare:
-
-	/* Data CPLB Miss event. We need to choose a CPLB to
-	 * evict, and then locate a new CPLB to install from the
-	 * config table, that covers the faulting address.
-	 */
-
-	P1.L = LO(DCPLB_DATA15);
-	P1.H = HI(DCPLB_DATA15);
-
-	P4.L = LO(DCPLB_FAULT_ADDR);
-	P4.H = HI(DCPLB_FAULT_ADDR);
-	R4 = [P4];
-	I0 = R4;
-
-	/* The replacement procedure for DCPLBs*/
-
-	R6 = R1;	/* Save for later*/
-
-	/* Turn off CPLBs while we work.*/
-	P4.L = LO(DMEM_CONTROL);
-	P4.H = HI(DMEM_CONTROL);
-	R5 = [P4];
-	BITCLR(R5,ENDCPLB_P);
-	CLI R0;
-	SSYNC;		/* SSYNC required before writing to DMEM_CONTROL. */
-	.align 8;
-	[P4] = R5;
-	SSYNC;
-	STI R0;
-
-	/* Start looking for a CPLB to evict. Our order of preference
-	 * is: invalid CPLBs, clean CPLBs, dirty CPLBs. Locked CPLBs
-	 * are no good.
-	 */
-
-	I1.L = LO(DCPLB_DATA0);
-	I1.H = HI(DCPLB_DATA0);
-	P1 = 2;
-	P2 = 16;
-	I2.L = _dcplb_preference;
-	I2.H = _dcplb_preference;
-	LSETUP(.Lsdsearch1, .Ledsearch1) LC0 = P1;
-.Lsdsearch1:
-	R0 = [I2++];		/* Get the bits we're interested in*/
-	P0 = I1;		/* Go back to start of table*/
-	LSETUP (.Lsdsearch2, .Ledsearch2) LC1 = P2;
-.Lsdsearch2:
-	R1 = [P0++];		/* Fetch each installed CPLB in turn*/
-	R2 = R1 & R0;		/* and test for interesting bits.*/
-	CC = R2 == 0;		/* If none are set, it'll do.*/
-	IF !CC JUMP .Lskip_stack_check;
-
-	R2 = [P0 - 0x104]; 	/* R2 - PageStart */
-	P3.L = _page_size_table; /* retrieve end address */
-	P3.H = _page_size_table; /* retrieve end address */
-	R3 = 0x1002;		/* 16th - position, 2 bits -length */
-#if ANOMALY_05000209
-	nop;			/* Anomaly 05000209 */
-#endif
-	R7 = EXTRACT(R1,R3.l);
-	R7 = R7 << 2;		/* Page size index offset */
-	P5 = R7;
-	P3 = P3 + P5;
-	R7 = [P3];		/* page size in bytes */
-
-	R7 = R2 + R7;		/* R7 - PageEnd */
-	R4 = SP; 		/* Test SP is in range */
-
-	CC = R7 < R4;		/* if PageEnd < SP */
-	IF CC JUMP .Ldfound_victim;
-	R3 = 0x284;		/* stack length from start of trap till
-				 * the point.
-				 * 20 stack locations for future modifications
-				 */
-	R4 = R4 + R3;
-	CC = R4 < R2;		/* if SP + stacklen < PageStart */
-	IF CC JUMP .Ldfound_victim;
-.Lskip_stack_check:
-
-.Ledsearch2: NOP;
-.Ledsearch1: NOP;
-
-	/* If we got here, we didn't find a DCPLB we considered
-	 * replacable, which means all of them were locked.
-	 */
-
-	JUMP .Lall_locked;
-.Ldfound_victim:
-
-#ifdef CONFIG_CPLB_INFO
-	R7 = [P0 - 0x104];
-	P2.L = _dpdt_table;
-	P2.H = _dpdt_table;
-	P3.L = _dpdt_swapcount_table;
-	P3.H = _dpdt_swapcount_table;
-	P3 += -4;
-.Ldicount:
-	R2 = [P2];
-	P2 += 8;
-	P3 += 8;
-	CC = R2==-1;
-	IF CC JUMP .Ldicount_done;
-	CC = R7==R2;
-	IF !CC JUMP .Ldicount;
-	R7 = [P3];
-	R7 += 1;
-	[P3] = R7;
-.Ldicount_done:
-#endif
-
-	/* Clean down the hardware loops*/
-	R2 = 0;
-	LC1 = R2;
-	LC0 = R2;
-
-	/* There's a suitable victim in [P0-4] (because we've
-	 * advanced already).
-	 */
-
-.LDdoverwrite:
-
-	/* [P0-4] is a suitable victim CPLB, so we want to
-	 * overwrite it by moving all the following CPLBs
-	 * one space closer to the start.
-	 */
-
-	R1.L = LO(DCPLB_DATA16);		/* DCPLB_DATA15 + 4 */
-	R1.H = HI(DCPLB_DATA16);
-	R0 = P0;
-
-	/* If the victim happens to be in DCPLB15,
-	 * we don't need to move anything.
-	 */
-
-	CC = R1 == R0;
-	IF CC JUMP .Lde_moved;
-	R1 = R1 - R0;
-	R1 >>= 2;
-	P1 = R1;
-	LSETUP(.Lds_move, .Lde_move) LC0=P1;
-.Lds_move:
-	R0 = [P0++];	/* move data */
-	[P0 - 8] = R0;
-	R0 = [P0-0x104]	/* move address */
-.Lde_move:
-	 [P0-0x108] = R0;
-
-.Lde_moved:
-	NOP;
-
-	/* Clear DCPLB_DATA15, in case we don't find a replacement
-	 * otherwise, we would have a duplicate entry, and will crash
-	 */
-	R0 = 0;
-	[P0 - 0x4] = R0;
-
-	/* We've now made space in DCPLB15 for the new CPLB to be
-	 * installed. The next stage is to locate a CPLB in the
-	 * config table that covers the faulting address.
-	 */
-
-	R0 = I0;		/* Our faulting address */
-
-	P2.L = _dpdt_table;
-	P2.H = _dpdt_table;
-#ifdef	CONFIG_CPLB_INFO
-	P3.L = _dpdt_swapcount_table;
-	P3.H = _dpdt_swapcount_table;
-	P3 += -8;
-#endif
-
-	P1.L = _page_size_table;
-	P1.H = _page_size_table;
-
-	/* An extraction pattern, to retrieve bits 17:16.*/
-
-	R1 = (16<<8)|2;
-.Ldnext:	R4 = [P2++];	/* address */
-	R2 = [P2++];	/* data */
-#ifdef	CONFIG_CPLB_INFO
-	P3 += 8;
-#endif
-
-	CC = R4 == -1;
-	IF CC JUMP .Lno_page_in_table;
-
-	/* See if failed address > start address */
-	CC = R4 <= R0(IU);
-	IF !CC JUMP .Ldnext;
-
-	/* extract page size (17:16)*/
-	R3 = EXTRACT(R2, R1.L) (Z);
-
-	/* add page size to addr to get range */
-
-	P5 = R3;
-	P5 = P1 + (P5 << 2);
-	R3 = [P5];
-	R3 = R3 + R4;
-
-	/* See if failed address < (start address + page size) */
-	CC = R0 < R3(IU);
-	IF !CC JUMP .Ldnext;
-
-	/* We've found the CPLB that should be installed, so
-	 * write it into CPLB15, masking off any caching bits
-	 * if necessary.
-	 */
-
-	P1.L = LO(DCPLB_DATA15);
-	P1.H = HI(DCPLB_DATA15);
-
-	/* If the DCPLB has cache bits set, but caching hasn't
-	 * been enabled, then we want to mask off the cache-in-L1
-	 * bit before installing. Moreover, if caching is off, we
-	 * also want to ensure that the DCPLB has WT mode set, rather
-	 * than WB, since WB pages still trigger first-write exceptions
-	 * even when not caching is off, and the page isn't marked as
-	 * cachable. Finally, we could mark the page as clean, not dirty,
-	 * but we choose to leave that decision to the user; if the user
-	 * chooses to have a CPLB pre-defined as dirty, then they always
-	 * pay the cost of flushing during eviction, but don't pay the
-	 * cost of first-write exceptions to mark the page as dirty.
-	 */
-
-#ifdef CONFIG_BFIN_WT
-	BITSET(R6, 14);		/* Set WT*/
-#endif
-
-	[P1] = R2;
-	[P1-0x100] = R4;
-#ifdef	CONFIG_CPLB_INFO
-	R3 = [P3];
-	R3 += 1;
-	[P3] = R3;
-#endif
-
-	/* We've installed the CPLB, so re-enable CPLBs. P4
-	 * points to DMEM_CONTROL, and R5 is the value we
-	 * last wrote to it, when we were disabling CPLBs.
-	 */
-
-	BITSET(R5,ENDCPLB_P);
-	CLI R2;
-	.align 8;
-	[P4] = R5;
-	SSYNC;
-	STI R2;
-
-	( R7:4,P5:3 ) = [SP++];
-	R0 = CPLB_RELOADED;
-	RTS;
-ENDPROC(_cplb_mgr)
-
-.data
-.align 4;
-_page_size_table:
-.byte4	0x00000400;	/* 1K */
-.byte4	0x00001000;	/* 4K */
-.byte4	0x00100000;	/* 1M */
-.byte4	0x00400000;	/* 4M */
-
-.align 4;
-_dcplb_preference:
-.byte4	0x00000001;	/* valid bit */
-.byte4	0x00000002;	/* lock bit */
diff --git a/arch/blackfin/kernel/cplb-nompu/cplbmgr.c b/arch/blackfin/kernel/cplb-nompu/cplbmgr.c
new file mode 100644
index 000000000000..376249ab2694
--- /dev/null
+++ b/arch/blackfin/kernel/cplb-nompu/cplbmgr.c
@@ -0,0 +1,283 @@
+/*
+ * File:         arch/blackfin/kernel/cplb-nompu-c/cplbmgr.c
+ * Based on:     arch/blackfin/kernel/cplb-mpu/cplbmgr.c
+ * Author:       Michael McTernan <mmcternan@airvana.com>
+ *
+ * Created:      01Nov2008
+ * Description:  CPLB miss handler.
+ *
+ * Modified:
+ *               Copyright 2008 Airvana Inc.
+ *               Copyright 2004-2007 Analog Devices Inc.
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <asm/blackfin.h>
+#include <asm/cplbinit.h>
+#include <asm/cplb.h>
+#include <asm/mmu_context.h>
+
+/*
+ * WARNING
+ *
+ * This file is compiled with certain -ffixed-reg options.  We have to
+ * make sure not to call any functions here that could clobber these
+ * registers.
+ */
+
+int nr_dcplb_miss[NR_CPUS], nr_icplb_miss[NR_CPUS];
+int nr_dcplb_supv_miss[NR_CPUS], nr_icplb_supv_miss[NR_CPUS];
+int nr_cplb_flush[NR_CPUS], nr_dcplb_prot[NR_CPUS];
+
+#ifdef CONFIG_EXCPT_IRQ_SYSC_L1
+#define MGR_ATTR __attribute__((l1_text))
+#else
+#define MGR_ATTR
+#endif
+
+/*
+ * We're in an exception handler.  The normal cli nop nop workaround
+ * isn't going to do very much, as the only thing that can interrupt
+ * us is an NMI, and the cli isn't going to stop that.
+ */
+#define NOWA_SSYNC __asm__ __volatile__ ("ssync;")
+
+/* Anomaly handlers provide SSYNCs, so avoid extra if anomaly is present */
+#if ANOMALY_05000125
+
+#define bfin_write_DMEM_CONTROL_SSYNC(v)    bfin_write_DMEM_CONTROL(v)
+#define bfin_write_IMEM_CONTROL_SSYNC(v)    bfin_write_IMEM_CONTROL(v)
+
+#else
+
+#define bfin_write_DMEM_CONTROL_SSYNC(v) \
+    do { NOWA_SSYNC; bfin_write_DMEM_CONTROL(v); NOWA_SSYNC; } while (0)
+#define bfin_write_IMEM_CONTROL_SSYNC(v) \
+    do { NOWA_SSYNC; bfin_write_IMEM_CONTROL(v); NOWA_SSYNC; } while (0)
+
+#endif
+
+static inline void write_dcplb_data(int cpu, int idx, unsigned long data,
+				    unsigned long addr)
+{
+	unsigned long ctrl = bfin_read_DMEM_CONTROL();
+	bfin_write_DMEM_CONTROL_SSYNC(ctrl & ~ENDCPLB);
+	bfin_write32(DCPLB_DATA0 + idx * 4, data);
+	bfin_write32(DCPLB_ADDR0 + idx * 4, addr);
+	bfin_write_DMEM_CONTROL_SSYNC(ctrl);
+
+#ifdef CONFIG_CPLB_INFO
+	dcplb_tbl[cpu][idx].addr = addr;
+	dcplb_tbl[cpu][idx].data = data;
+#endif
+}
+
+static inline void write_icplb_data(int cpu, int idx, unsigned long data,
+				    unsigned long addr)
+{
+	unsigned long ctrl = bfin_read_IMEM_CONTROL();
+
+	bfin_write_IMEM_CONTROL_SSYNC(ctrl & ~ENICPLB);
+	bfin_write32(ICPLB_DATA0 + idx * 4, data);
+	bfin_write32(ICPLB_ADDR0 + idx * 4, addr);
+	bfin_write_IMEM_CONTROL_SSYNC(ctrl);
+
+#ifdef CONFIG_CPLB_INFO
+	icplb_tbl[cpu][idx].addr = addr;
+	icplb_tbl[cpu][idx].data = data;
+#endif
+}
+
+/*
+ * Given the contents of the status register, return the index of the
+ * CPLB that caused the fault.
+ */
+static inline int faulting_cplb_index(int status)
+{
+	int signbits = __builtin_bfin_norm_fr1x32(status & 0xFFFF);
+	return 30 - signbits;
+}
+
+/*
+ * Given the contents of the status register and the DCPLB_DATA contents,
+ * return true if a write access should be permitted.
+ */
+static inline int write_permitted(int status, unsigned long data)
+{
+	if (status & FAULT_USERSUPV)
+		return !!(data & CPLB_SUPV_WR);
+	else
+		return !!(data & CPLB_USER_WR);
+}
+
+/* Counters to implement round-robin replacement.  */
+static int icplb_rr_index[NR_CPUS] PDT_ATTR;
+static int dcplb_rr_index[NR_CPUS] PDT_ATTR;
+
+/*
+ * Find an ICPLB entry to be evicted and return its index.
+ */
+static int evict_one_icplb(int cpu)
+{
+	int i = first_switched_icplb + icplb_rr_index[cpu];
+	if (i >= MAX_CPLBS) {
+		i -= MAX_CPLBS - first_switched_icplb;
+		icplb_rr_index[cpu] -= MAX_CPLBS - first_switched_icplb;
+	}
+	icplb_rr_index[cpu]++;
+	return i;
+}
+
+static int evict_one_dcplb(int cpu)
+{
+	int i = first_switched_dcplb + dcplb_rr_index[cpu];
+	if (i >= MAX_CPLBS) {
+		i -= MAX_CPLBS - first_switched_dcplb;
+		dcplb_rr_index[cpu] -= MAX_CPLBS - first_switched_dcplb;
+	}
+	dcplb_rr_index[cpu]++;
+	return i;
+}
+
+MGR_ATTR static int icplb_miss(int cpu)
+{
+	unsigned long addr = bfin_read_ICPLB_FAULT_ADDR();
+	int status = bfin_read_ICPLB_STATUS();
+	int idx;
+	unsigned long i_data, base, addr1, eaddr;
+
+	nr_icplb_miss[cpu]++;
+	if (unlikely(status & FAULT_USERSUPV))
+		nr_icplb_supv_miss[cpu]++;
+
+	base = 0;
+	for (idx = 0; idx < icplb_nr_bounds; idx++) {
+		eaddr = icplb_bounds[idx].eaddr;
+		if (addr < eaddr)
+			break;
+		base = eaddr;
+	}
+	if (unlikely(idx == icplb_nr_bounds))
+		return CPLB_NO_ADDR_MATCH;
+
+	i_data = icplb_bounds[idx].data;
+	if (unlikely(i_data == 0))
+		return CPLB_NO_ADDR_MATCH;
+
+	addr1 = addr & ~(SIZE_4M - 1);
+	addr &= ~(SIZE_1M - 1);
+	i_data |= PAGE_SIZE_1MB;
+	if (addr1 >= base && (addr1 + SIZE_4M) <= eaddr) {
+		/*
+		 * This works because
+		 * (PAGE_SIZE_4MB & PAGE_SIZE_1MB) == PAGE_SIZE_1MB.
+		 */
+		i_data |= PAGE_SIZE_4MB;
+		addr = addr1;
+	}
+
+	/* Pick entry to evict */
+	idx = evict_one_icplb(cpu);
+
+	write_icplb_data(cpu, idx, i_data, addr);
+
+	return CPLB_RELOADED;
+}
+
+MGR_ATTR static int dcplb_miss(int cpu)
+{
+	unsigned long addr = bfin_read_DCPLB_FAULT_ADDR();
+	int status = bfin_read_DCPLB_STATUS();
+	int idx;
+	unsigned long d_data, base, addr1, eaddr;
+
+	nr_dcplb_miss[cpu]++;
+	if (unlikely(status & FAULT_USERSUPV))
+		nr_dcplb_supv_miss[cpu]++;
+
+	base = 0;
+	for (idx = 0; idx < dcplb_nr_bounds; idx++) {
+		eaddr = dcplb_bounds[idx].eaddr;
+		if (addr < eaddr)
+			break;
+		base = eaddr;
+	}
+	if (unlikely(idx == dcplb_nr_bounds))
+		return CPLB_NO_ADDR_MATCH;
+
+	d_data = dcplb_bounds[idx].data;
+	if (unlikely(d_data == 0))
+		return CPLB_NO_ADDR_MATCH;
+
+	addr1 = addr & ~(SIZE_4M - 1);
+	addr &= ~(SIZE_1M - 1);
+	d_data |= PAGE_SIZE_1MB;
+	if (addr1 >= base && (addr1 + SIZE_4M) <= eaddr) {
+		/*
+		 * This works because
+		 * (PAGE_SIZE_4MB & PAGE_SIZE_1MB) == PAGE_SIZE_1MB.
+		 */
+		d_data |= PAGE_SIZE_4MB;
+		addr = addr1;
+	}
+
+	/* Pick entry to evict */
+	idx = evict_one_dcplb(cpu);
+
+	write_dcplb_data(cpu, idx, d_data, addr);
+
+	return CPLB_RELOADED;
+}
+
+MGR_ATTR static noinline int dcplb_protection_fault(int cpu)
+{
+	int status = bfin_read_DCPLB_STATUS();
+
+	nr_dcplb_prot[cpu]++;
+
+	if (likely(status & FAULT_RW)) {
+		int idx = faulting_cplb_index(status);
+		unsigned long regaddr = DCPLB_DATA0 + idx * 4;
+		unsigned long data = bfin_read32(regaddr);
+
+		/* Check if fault is to dirty a clean page */
+		if (!(data & CPLB_WT) && !(data & CPLB_DIRTY) &&
+		    write_permitted(status, data)) {
+
+			dcplb_tbl[cpu][idx].data = data;
+			bfin_write32(regaddr, data);
+			return CPLB_RELOADED;
+		}
+	}
+
+	return CPLB_PROT_VIOL;
+}
+
+MGR_ATTR int cplb_hdr(int seqstat, struct pt_regs *regs)
+{
+	int cause = seqstat & 0x3f;
+	unsigned int cpu = smp_processor_id();
+	switch (cause) {
+	case 0x2C:
+		return icplb_miss(cpu);
+	case 0x26:
+		return dcplb_miss(cpu);
+	default:
+		if (unlikely(cause == 0x23))
+			return dcplb_protection_fault(cpu);
+
+		return CPLB_UNKNOWN_ERR;
+	}
+}
diff --git a/arch/blackfin/kernel/cplbinfo.c b/arch/blackfin/kernel/cplbinfo.c
new file mode 100644
index 000000000000..64d78300dd08
--- /dev/null
+++ b/arch/blackfin/kernel/cplbinfo.c
@@ -0,0 +1,177 @@
+/*
+ * arch/blackfin/kernel/cplbinfo.c - display CPLB status
+ *
+ * Copyright 2004-2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/ctype.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+
+#include <asm/cplbinit.h>
+#include <asm/blackfin.h>
+
+static char const page_strtbl[][3] = { "1K", "4K", "1M", "4M" };
+#define page(flags)    (((flags) & 0x30000) >> 16)
+#define strpage(flags) page_strtbl[page(flags)]
+
+struct cplbinfo_data {
+	loff_t pos;
+	char cplb_type;
+	u32 mem_control;
+	struct cplb_entry *tbl;
+	int switched;
+};
+
+static void cplbinfo_print_header(struct seq_file *m)
+{
+	seq_printf(m, "Index\tAddress\t\tData\tSize\tU/RD\tU/WR\tS/WR\tSwitch\n");
+}
+
+static int cplbinfo_nomore(struct cplbinfo_data *cdata)
+{
+	return cdata->pos >= MAX_CPLBS;
+}
+
+static int cplbinfo_show(struct seq_file *m, void *p)
+{
+	struct cplbinfo_data *cdata;
+	unsigned long data, addr;
+	loff_t pos;
+
+	cdata = p;
+	pos = cdata->pos;
+	addr = cdata->tbl[pos].addr;
+	data = cdata->tbl[pos].data;
+
+	seq_printf(m,
+		"%d\t0x%08lx\t%05lx\t%s\t%c\t%c\t%c\t%c\n",
+		(int)pos, addr, data, strpage(data),
+		(data & CPLB_USER_RD) ? 'Y' : 'N',
+		(data & CPLB_USER_WR) ? 'Y' : 'N',
+		(data & CPLB_SUPV_WR) ? 'Y' : 'N',
+		pos < cdata->switched ? 'N' : 'Y');
+
+	return 0;
+}
+
+static void cplbinfo_seq_init(struct cplbinfo_data *cdata, unsigned int cpu)
+{
+	if (cdata->cplb_type == 'I') {
+		cdata->mem_control = bfin_read_IMEM_CONTROL();
+		cdata->tbl = icplb_tbl[cpu];
+		cdata->switched = first_switched_icplb;
+	} else {
+		cdata->mem_control = bfin_read_DMEM_CONTROL();
+		cdata->tbl = dcplb_tbl[cpu];
+		cdata->switched = first_switched_dcplb;
+	}
+}
+
+static void *cplbinfo_start(struct seq_file *m, loff_t *pos)
+{
+	struct cplbinfo_data *cdata = m->private;
+
+	if (!*pos) {
+		seq_printf(m, "%cCPLBs are %sabled: 0x%x\n", cdata->cplb_type,
+			(cdata->mem_control & ENDCPLB ? "en" : "dis"),
+			cdata->mem_control);
+		cplbinfo_print_header(m);
+	} else if (cplbinfo_nomore(cdata))
+		return NULL;
+
+	get_cpu();
+	return cdata;
+}
+
+static void *cplbinfo_next(struct seq_file *m, void *p, loff_t *pos)
+{
+	struct cplbinfo_data *cdata = p;
+	cdata->pos = ++(*pos);
+	if (cplbinfo_nomore(cdata))
+		return NULL;
+	else
+		return cdata;
+}
+
+static void cplbinfo_stop(struct seq_file *m, void *p)
+{
+	put_cpu();
+}
+
+static const struct seq_operations cplbinfo_sops = {
+	.start = cplbinfo_start,
+	.next  = cplbinfo_next,
+	.stop  = cplbinfo_stop,
+	.show  = cplbinfo_show,
+};
+
+static int cplbinfo_open(struct inode *inode, struct file *file)
+{
+	char buf[256], *path, *p;
+	unsigned int cpu;
+	char *s_cpu, *s_cplb;
+	int ret;
+	struct seq_file *m;
+	struct cplbinfo_data *cdata;
+
+	path = d_path(&file->f_path, buf, sizeof(buf));
+	if (IS_ERR(path))
+		return PTR_ERR(path);
+	s_cpu = strstr(path, "/cpu");
+	s_cplb = strrchr(path, '/');
+	if (!s_cpu || !s_cplb)
+		return -EINVAL;
+
+	cpu = simple_strtoul(s_cpu + 4, &p, 10);
+	if (!cpu_online(cpu))
+		return -ENODEV;
+
+	ret = seq_open_private(file, &cplbinfo_sops, sizeof(*cdata));
+	if (ret)
+		return ret;
+	m = file->private_data;
+	cdata = m->private;
+
+	cdata->pos = 0;
+	cdata->cplb_type = toupper(s_cplb[1]);
+	cplbinfo_seq_init(cdata, cpu);
+
+	return 0;
+}
+
+static const struct file_operations cplbinfo_fops = {
+	.open    = cplbinfo_open,
+	.read    = seq_read,
+	.llseek  = seq_lseek,
+	.release = seq_release_private,
+};
+
+static int __init cplbinfo_init(void)
+{
+	struct proc_dir_entry *cplb_dir, *cpu_dir;
+	char buf[10];
+	unsigned int cpu;
+
+	cplb_dir = proc_mkdir("cplbinfo", NULL);
+	if (!cplb_dir)
+		return -ENOMEM;
+
+	for_each_possible_cpu(cpu) {
+		sprintf(buf, "cpu%i", cpu);
+		cpu_dir = proc_mkdir(buf, cplb_dir);
+		if (!cpu_dir)
+			return -ENOMEM;
+
+		proc_create("icplb", S_IRUGO, cpu_dir, &cplbinfo_fops);
+		proc_create("dcplb", S_IRUGO, cpu_dir, &cplbinfo_fops);
+	}
+
+	return 0;
+}
+late_initcall(cplbinfo_init);
diff --git a/arch/blackfin/kernel/early_printk.c b/arch/blackfin/kernel/early_printk.c
index 1f4e3d2e0901..c8ad051742e2 100644
--- a/arch/blackfin/kernel/early_printk.c
+++ b/arch/blackfin/kernel/early_printk.c
@@ -105,10 +105,10 @@ static struct console * __init earlyserial_init(char *buf)
 		cflag |= CS5;
 		break;
 	case 6:
-		cflag |= CS5;
+		cflag |= CS6;
 		break;
 	case 7:
-		cflag |= CS5;
+		cflag |= CS7;
 		break;
 	default:
 		cflag |= CS8;
diff --git a/arch/blackfin/kernel/entry.S b/arch/blackfin/kernel/entry.S
index faea88ebb2ef..a9cfba9946b5 100644
--- a/arch/blackfin/kernel/entry.S
+++ b/arch/blackfin/kernel/entry.S
@@ -30,6 +30,7 @@
 #include <linux/linkage.h>
 #include <asm/thread_info.h>
 #include <asm/errno.h>
+#include <asm/blackfin.h>
 #include <asm/asm-offsets.h>
 
 #include <asm/context.S>
@@ -41,6 +42,10 @@
 #endif
 
 ENTRY(_ret_from_fork)
+#ifdef CONFIG_IPIPE
+	[--sp] = reti; 		/* IRQs on. */
+	SP += 4;
+#endif /* CONFIG_IPIPE */
 	SP += -12;
 	call _schedule_tail;
 	SP += 12;
diff --git a/arch/blackfin/kernel/fixed_code.S b/arch/blackfin/kernel/fixed_code.S
index 4b03ba025488..0d2d9e0968c8 100644
--- a/arch/blackfin/kernel/fixed_code.S
+++ b/arch/blackfin/kernel/fixed_code.S
@@ -8,10 +8,12 @@
  * BF561 SMP).
  */
 #include <linux/linkage.h>
+#include <linux/init.h>
 #include <linux/unistd.h>
 #include <asm/entry.h>
 
-.text
+__INIT
+
 ENTRY(_fixed_code_start)
 
 .align 16
@@ -144,3 +146,5 @@ ENTRY(_safe_user_instruction)
 ENDPROC(_safe_user_instruction)
 
 ENTRY(_fixed_code_end)
+
+__FINIT
diff --git a/arch/blackfin/kernel/ipipe.c b/arch/blackfin/kernel/ipipe.c
new file mode 100644
index 000000000000..339be5a3ae6a
--- /dev/null
+++ b/arch/blackfin/kernel/ipipe.c
@@ -0,0 +1,428 @@
+/* -*- linux-c -*-
+ * linux/arch/blackfin/kernel/ipipe.c
+ *
+ * Copyright (C) 2005-2007 Philippe Gerum.
+ *
+ * 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, Inc., 675 Mass Ave, Cambridge MA 02139,
+ * USA; 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.
+ *
+ * Architecture-dependent I-pipe support for the Blackfin.
+ */
+
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/percpu.h>
+#include <linux/bitops.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/kthread.h>
+#include <asm/unistd.h>
+#include <asm/system.h>
+#include <asm/atomic.h>
+#include <asm/io.h>
+
+static int create_irq_threads;
+
+DEFINE_PER_CPU(struct pt_regs, __ipipe_tick_regs);
+
+static DEFINE_PER_CPU(unsigned long, pending_irqthread_mask);
+
+static DEFINE_PER_CPU(int [IVG13 + 1], pending_irq_count);
+
+asmlinkage void asm_do_IRQ(unsigned int irq, struct pt_regs *regs);
+
+static void __ipipe_no_irqtail(void);
+
+unsigned long __ipipe_irq_tail_hook = (unsigned long)&__ipipe_no_irqtail;
+EXPORT_SYMBOL(__ipipe_irq_tail_hook);
+
+unsigned long __ipipe_core_clock;
+EXPORT_SYMBOL(__ipipe_core_clock);
+
+unsigned long __ipipe_freq_scale;
+EXPORT_SYMBOL(__ipipe_freq_scale);
+
+atomic_t __ipipe_irq_lvdepth[IVG15 + 1];
+
+unsigned long __ipipe_irq_lvmask = __all_masked_irq_flags;
+EXPORT_SYMBOL(__ipipe_irq_lvmask);
+
+static void __ipipe_ack_irq(unsigned irq, struct irq_desc *desc)
+{
+	desc->ipipe_ack(irq, desc);
+}
+
+/*
+ * __ipipe_enable_pipeline() -- We are running on the boot CPU, hw
+ * interrupts are off, and secondary CPUs are still lost in space.
+ */
+void __ipipe_enable_pipeline(void)
+{
+	unsigned irq;
+
+	__ipipe_core_clock = get_cclk(); /* Fetch this once. */
+	__ipipe_freq_scale = 1000000000UL / __ipipe_core_clock;
+
+	for (irq = 0; irq < NR_IRQS; ++irq)
+		ipipe_virtualize_irq(ipipe_root_domain,
+				     irq,
+				     (ipipe_irq_handler_t)&asm_do_IRQ,
+				     NULL,
+				     &__ipipe_ack_irq,
+				     IPIPE_HANDLE_MASK | IPIPE_PASS_MASK);
+}
+
+/*
+ * __ipipe_handle_irq() -- IPIPE's generic IRQ handler. An optimistic
+ * interrupt protection log is maintained here for each domain. Hw
+ * interrupts are masked on entry.
+ */
+void __ipipe_handle_irq(unsigned irq, struct pt_regs *regs)
+{
+	struct ipipe_domain *this_domain, *next_domain;
+	struct list_head *head, *pos;
+	int m_ack, s = -1;
+
+	/*
+	 * Software-triggered IRQs do not need any ack.  The contents
+	 * of the register frame should only be used when processing
+	 * the timer interrupt, but not for handling any other
+	 * interrupt.
+	 */
+	m_ack = (regs == NULL || irq == IRQ_SYSTMR || irq == IRQ_CORETMR);
+
+	this_domain = ipipe_current_domain;
+
+	if (unlikely(test_bit(IPIPE_STICKY_FLAG, &this_domain->irqs[irq].control)))
+		head = &this_domain->p_link;
+	else {
+		head = __ipipe_pipeline.next;
+		next_domain = list_entry(head, struct ipipe_domain, p_link);
+		if (likely(test_bit(IPIPE_WIRED_FLAG, &next_domain->irqs[irq].control))) {
+			if (!m_ack && next_domain->irqs[irq].acknowledge != NULL)
+				next_domain->irqs[irq].acknowledge(irq, irq_desc + irq);
+			if (test_bit(IPIPE_ROOTLOCK_FLAG, &ipipe_root_domain->flags))
+				s = __test_and_set_bit(IPIPE_STALL_FLAG,
+						       &ipipe_root_cpudom_var(status));
+			__ipipe_dispatch_wired(next_domain, irq);
+				goto finalize;
+			return;
+		}
+	}
+
+	/* Ack the interrupt. */
+
+	pos = head;
+
+	while (pos != &__ipipe_pipeline) {
+		next_domain = list_entry(pos, struct ipipe_domain, p_link);
+		/*
+		 * For each domain handling the incoming IRQ, mark it
+		 * as pending in its log.
+		 */
+		if (test_bit(IPIPE_HANDLE_FLAG, &next_domain->irqs[irq].control)) {
+			/*
+			 * Domains that handle this IRQ are polled for
+			 * acknowledging it by decreasing priority
+			 * order. The interrupt must be made pending
+			 * _first_ in the domain's status flags before
+			 * the PIC is unlocked.
+			 */
+			__ipipe_set_irq_pending(next_domain, irq);
+
+			if (!m_ack && next_domain->irqs[irq].acknowledge != NULL) {
+				next_domain->irqs[irq].acknowledge(irq, irq_desc + irq);
+				m_ack = 1;
+			}
+		}
+
+		/*
+		 * If the domain does not want the IRQ to be passed
+		 * down the interrupt pipe, exit the loop now.
+		 */
+		if (!test_bit(IPIPE_PASS_FLAG, &next_domain->irqs[irq].control))
+			break;
+
+		pos = next_domain->p_link.next;
+	}
+
+	/*
+	 * Now walk the pipeline, yielding control to the highest
+	 * priority domain that has pending interrupt(s) or
+	 * immediately to the current domain if the interrupt has been
+	 * marked as 'sticky'. This search does not go beyond the
+	 * current domain in the pipeline. We also enforce the
+	 * additional root stage lock (blackfin-specific). */
+
+	if (test_bit(IPIPE_ROOTLOCK_FLAG, &ipipe_root_domain->flags))
+		s = __test_and_set_bit(IPIPE_STALL_FLAG,
+				       &ipipe_root_cpudom_var(status));
+finalize:
+
+	__ipipe_walk_pipeline(head);
+
+	if (!s)
+		__clear_bit(IPIPE_STALL_FLAG,
+			    &ipipe_root_cpudom_var(status));
+}
+
+int __ipipe_check_root(void)
+{
+	return ipipe_root_domain_p;
+}
+
+void __ipipe_enable_irqdesc(struct ipipe_domain *ipd, unsigned irq)
+{
+	struct irq_desc *desc = irq_desc + irq;
+	int prio = desc->ic_prio;
+
+	desc->depth = 0;
+	if (ipd != &ipipe_root &&
+	    atomic_inc_return(&__ipipe_irq_lvdepth[prio]) == 1)
+		__set_bit(prio, &__ipipe_irq_lvmask);
+}
+EXPORT_SYMBOL(__ipipe_enable_irqdesc);
+
+void __ipipe_disable_irqdesc(struct ipipe_domain *ipd, unsigned irq)
+{
+	struct irq_desc *desc = irq_desc + irq;
+	int prio = desc->ic_prio;
+
+	if (ipd != &ipipe_root &&
+	    atomic_dec_and_test(&__ipipe_irq_lvdepth[prio]))
+		__clear_bit(prio, &__ipipe_irq_lvmask);
+}
+EXPORT_SYMBOL(__ipipe_disable_irqdesc);
+
+void __ipipe_stall_root_raw(void)
+{
+	/*
+	 * This code is called by the ins{bwl} routines (see
+	 * arch/blackfin/lib/ins.S), which are heavily used by the
+	 * network stack. It masks all interrupts but those handled by
+	 * non-root domains, so that we keep decent network transfer
+	 * rates for Linux without inducing pathological jitter for
+	 * the real-time domain.
+	 */
+	__asm__ __volatile__ ("sti %0;" : : "d"(__ipipe_irq_lvmask));
+
+	__set_bit(IPIPE_STALL_FLAG,
+		  &ipipe_root_cpudom_var(status));
+}
+
+void __ipipe_unstall_root_raw(void)
+{
+	__clear_bit(IPIPE_STALL_FLAG,
+		    &ipipe_root_cpudom_var(status));
+
+	__asm__ __volatile__ ("sti %0;" : : "d"(bfin_irq_flags));
+}
+
+int __ipipe_syscall_root(struct pt_regs *regs)
+{
+	unsigned long flags;
+
+	/* We need to run the IRQ tail hook whenever we don't
+	 * propagate a syscall to higher domains, because we know that
+	 * important operations might be pending there (e.g. Xenomai
+	 * deferred rescheduling). */
+
+	if (!__ipipe_syscall_watched_p(current, regs->orig_p0)) {
+		void (*hook)(void) = (void (*)(void))__ipipe_irq_tail_hook;
+		hook();
+		return 0;
+	}
+
+	/*
+	 * This routine either returns:
+	 * 0 -- if the syscall is to be passed to Linux;
+	 * 1 -- if the syscall should not be passed to Linux, and no
+	 * tail work should be performed;
+	 * -1 -- if the syscall should not be passed to Linux but the
+	 * tail work has to be performed (for handling signals etc).
+	 */
+
+	if (__ipipe_event_monitored_p(IPIPE_EVENT_SYSCALL) &&
+	    __ipipe_dispatch_event(IPIPE_EVENT_SYSCALL, regs) > 0) {
+		if (ipipe_root_domain_p && !in_atomic()) {
+			/*
+			 * Sync pending VIRQs before _TIF_NEED_RESCHED
+			 * is tested.
+			 */
+			local_irq_save_hw(flags);
+			if ((ipipe_root_cpudom_var(irqpend_himask) & IPIPE_IRQMASK_VIRT) != 0)
+				__ipipe_sync_pipeline(IPIPE_IRQMASK_VIRT);
+			local_irq_restore_hw(flags);
+			return -1;
+		}
+		return 1;
+	}
+
+	return 0;
+}
+
+unsigned long ipipe_critical_enter(void (*syncfn) (void))
+{
+	unsigned long flags;
+
+	local_irq_save_hw(flags);
+
+	return flags;
+}
+
+void ipipe_critical_exit(unsigned long flags)
+{
+	local_irq_restore_hw(flags);
+}
+
+static void __ipipe_no_irqtail(void)
+{
+}
+
+int ipipe_get_sysinfo(struct ipipe_sysinfo *info)
+{
+	info->ncpus = num_online_cpus();
+	info->cpufreq = ipipe_cpu_freq();
+	info->archdep.tmirq = IPIPE_TIMER_IRQ;
+	info->archdep.tmfreq = info->cpufreq;
+
+	return 0;
+}
+
+/*
+ * ipipe_trigger_irq() -- Push the interrupt at front of the pipeline
+ * just like if it has been actually received from a hw source. Also
+ * works for virtual interrupts.
+ */
+int ipipe_trigger_irq(unsigned irq)
+{
+	unsigned long flags;
+
+	if (irq >= IPIPE_NR_IRQS ||
+	    (ipipe_virtual_irq_p(irq)
+	     && !test_bit(irq - IPIPE_VIRQ_BASE, &__ipipe_virtual_irq_map)))
+		return -EINVAL;
+
+	local_irq_save_hw(flags);
+
+	__ipipe_handle_irq(irq, NULL);
+
+	local_irq_restore_hw(flags);
+
+	return 1;
+}
+
+/* Move Linux IRQ to threads. */
+
+static int do_irqd(void *__desc)
+{
+	struct irq_desc *desc = __desc;
+	unsigned irq = desc - irq_desc;
+	int thrprio = desc->thr_prio;
+	int thrmask = 1 << thrprio;
+	int cpu = smp_processor_id();
+	cpumask_t cpumask;
+
+	sigfillset(&current->blocked);
+	current->flags |= PF_NOFREEZE;
+	cpumask = cpumask_of_cpu(cpu);
+	set_cpus_allowed(current, cpumask);
+	ipipe_setscheduler_root(current, SCHED_FIFO, 50 + thrprio);
+
+	while (!kthread_should_stop()) {
+		local_irq_disable();
+		if (!(desc->status & IRQ_SCHEDULED)) {
+			set_current_state(TASK_INTERRUPTIBLE);
+resched:
+			local_irq_enable();
+			schedule();
+			local_irq_disable();
+		}
+		__set_current_state(TASK_RUNNING);
+		/*
+		 * If higher priority interrupt servers are ready to
+		 * run, reschedule immediately. We need this for the
+		 * GPIO demux IRQ handler to unmask the interrupt line
+		 * _last_, after all GPIO IRQs have run.
+		 */
+		if (per_cpu(pending_irqthread_mask, cpu) & ~(thrmask|(thrmask-1)))
+			goto resched;
+		if (--per_cpu(pending_irq_count[thrprio], cpu) == 0)
+			per_cpu(pending_irqthread_mask, cpu) &= ~thrmask;
+		desc->status &= ~IRQ_SCHEDULED;
+		desc->thr_handler(irq, &__raw_get_cpu_var(__ipipe_tick_regs));
+		local_irq_enable();
+	}
+	__set_current_state(TASK_RUNNING);
+	return 0;
+}
+
+static void kick_irqd(unsigned irq, void *cookie)
+{
+	struct irq_desc *desc = irq_desc + irq;
+	int thrprio = desc->thr_prio;
+	int thrmask = 1 << thrprio;
+	int cpu = smp_processor_id();
+
+	if (!(desc->status & IRQ_SCHEDULED)) {
+		desc->status |= IRQ_SCHEDULED;
+		per_cpu(pending_irqthread_mask, cpu) |= thrmask;
+		++per_cpu(pending_irq_count[thrprio], cpu);
+		wake_up_process(desc->thread);
+	}
+}
+
+int ipipe_start_irq_thread(unsigned irq, struct irq_desc *desc)
+{
+	if (desc->thread || !create_irq_threads)
+		return 0;
+
+	desc->thread = kthread_create(do_irqd, desc, "IRQ %d", irq);
+	if (desc->thread == NULL) {
+		printk(KERN_ERR "irqd: could not create IRQ thread %d!\n", irq);
+		return -ENOMEM;
+	}
+
+	wake_up_process(desc->thread);
+
+	desc->thr_handler = ipipe_root_domain->irqs[irq].handler;
+	ipipe_root_domain->irqs[irq].handler = &kick_irqd;
+
+	return 0;
+}
+
+void __init ipipe_init_irq_threads(void)
+{
+	unsigned irq;
+	struct irq_desc *desc;
+
+	create_irq_threads = 1;
+
+	for (irq = 0; irq < NR_IRQS; irq++) {
+		desc = irq_desc + irq;
+		if (desc->action != NULL ||
+			(desc->status & IRQ_NOREQUEST) != 0)
+			ipipe_start_irq_thread(irq, desc);
+	}
+}
+
+EXPORT_SYMBOL(show_stack);
+
+#ifdef CONFIG_IPIPE_TRACE_MCOUNT
+void notrace _mcount(void);
+EXPORT_SYMBOL(_mcount);
+#endif /* CONFIG_IPIPE_TRACE_MCOUNT */
diff --git a/arch/blackfin/kernel/irqchip.c b/arch/blackfin/kernel/irqchip.c
index 07402f57c9de..ab8209cbbad0 100644
--- a/arch/blackfin/kernel/irqchip.c
+++ b/arch/blackfin/kernel/irqchip.c
@@ -36,7 +36,7 @@
 #include <linux/irq.h>
 #include <asm/trace.h>
 
-static unsigned long irq_err_count;
+static atomic_t irq_err_count;
 static spinlock_t irq_controller_lock;
 
 /*
@@ -48,10 +48,9 @@ void dummy_mask_unmask_irq(unsigned int irq)
 
 void ack_bad_irq(unsigned int irq)
 {
-	irq_err_count += 1;
+	atomic_inc(&irq_err_count);
 	printk(KERN_ERR "IRQ: spurious interrupt %d\n", irq);
 }
-EXPORT_SYMBOL(ack_bad_irq);
 
 static struct irq_chip bad_chip = {
 	.ack = dummy_mask_unmask_irq,
@@ -72,7 +71,7 @@ static struct irq_desc bad_irq_desc = {
 
 int show_interrupts(struct seq_file *p, void *v)
 {
-	int i = *(loff_t *) v;
+	int i = *(loff_t *) v, j;
 	struct irqaction *action;
 	unsigned long flags;
 
@@ -80,19 +79,20 @@ int show_interrupts(struct seq_file *p, void *v)
 		spin_lock_irqsave(&irq_desc[i].lock, flags);
 		action = irq_desc[i].action;
 		if (!action)
-			goto unlock;
-
-		seq_printf(p, "%3d: %10u ", i, kstat_irqs(i));
+			goto skip;
+		seq_printf(p, "%3d: ", i);
+		for_each_online_cpu(j)
+			seq_printf(p, "%10u ", kstat_cpu(j).irqs[i]);
+		seq_printf(p, " %8s", irq_desc[i].chip->name);
 		seq_printf(p, "  %s", action->name);
 		for (action = action->next; action; action = action->next)
-			seq_printf(p, ", %s", action->name);
+			seq_printf(p, "  %s", action->name);
 
 		seq_putc(p, '\n');
- unlock:
+ skip:
 		spin_unlock_irqrestore(&irq_desc[i].lock, flags);
-	} else if (i == NR_IRQS) {
-		seq_printf(p, "Err: %10lu\n", irq_err_count);
-	}
+	} else if (i == NR_IRQS)
+		seq_printf(p, "Err: %10u\n",  atomic_read(&irq_err_count));
 	return 0;
 }
 
@@ -101,7 +101,6 @@ int show_interrupts(struct seq_file *p, void *v)
  * come via this function.  Instead, they should provide their
  * own 'handler'
  */
-
 #ifdef CONFIG_DO_IRQ_L1
 __attribute__((l1_text))
 #endif
@@ -109,8 +108,9 @@ asmlinkage void asm_do_IRQ(unsigned int irq, struct pt_regs *regs)
 {
 	struct pt_regs *old_regs;
 	struct irq_desc *desc = irq_desc + irq;
+#ifndef CONFIG_IPIPE
 	unsigned short pending, other_ints;
-
+#endif
 	old_regs = set_irq_regs(regs);
 
 	/*
@@ -121,9 +121,24 @@ asmlinkage void asm_do_IRQ(unsigned int irq, struct pt_regs *regs)
 		desc = &bad_irq_desc;
 
 	irq_enter();
-
+#ifdef CONFIG_DEBUG_STACKOVERFLOW
+	/* Debugging check for stack overflow: is there less than STACK_WARN free? */
+	{
+		long sp;
+
+		sp = __get_SP() & (THREAD_SIZE-1);
+
+		if (unlikely(sp < (sizeof(struct thread_info) + STACK_WARN))) {
+			dump_stack();
+			printk(KERN_EMERG "%s: possible stack overflow while handling irq %i "
+					" only %ld bytes free\n",
+				__func__, irq, sp - sizeof(struct thread_info));
+		}
+	}
+#endif
 	generic_handle_irq(irq);
 
+#ifndef CONFIG_IPIPE	/* Useless and bugous over the I-pipe: IRQs are threaded. */
 	/* If we're the only interrupt running (ignoring IRQ15 which is for
 	   syscalls), lower our priority to IRQ14 so that softirqs run at
 	   that level.  If there's another, lower-level interrupt, irq_exit
@@ -133,6 +148,7 @@ asmlinkage void asm_do_IRQ(unsigned int irq, struct pt_regs *regs)
 	other_ints = pending & (pending - 1);
 	if (other_ints == 0)
 		lower_to_irq14();
+#endif /* !CONFIG_IPIPE */
 	irq_exit();
 
 	set_irq_regs(old_regs);
diff --git a/arch/blackfin/kernel/kgdb.c b/arch/blackfin/kernel/kgdb.c
index b795a207742c..b163f6d3330d 100644
--- a/arch/blackfin/kernel/kgdb.c
+++ b/arch/blackfin/kernel/kgdb.c
@@ -34,9 +34,14 @@ int gdb_bfin_vector = -1;
 #error change the definition of slavecpulocks
 #endif
 
-#ifdef CONFIG_BFIN_WDT
-# error "Please unselect blackfin watchdog driver before build KGDB."
-#endif
+#define IN_MEM(addr, size, l1_addr, l1_size) \
+({ \
+	unsigned long __addr = (unsigned long)(addr); \
+	(l1_size && __addr >= l1_addr && __addr + (size) <= l1_addr + l1_size); \
+})
+#define ASYNC_BANK_SIZE \
+	(ASYNC_BANK0_SIZE + ASYNC_BANK1_SIZE + \
+	 ASYNC_BANK2_SIZE + ASYNC_BANK3_SIZE)
 
 void pt_regs_to_gdb_regs(unsigned long *gdb_regs, struct pt_regs *regs)
 {
@@ -105,7 +110,7 @@ void pt_regs_to_gdb_regs(unsigned long *gdb_regs, struct pt_regs *regs)
  * Extracts ebp, esp and eip values understandable by gdb from the values
  * saved by switch_to.
  * thread.esp points to ebp. flags and ebp are pushed in switch_to hence esp
- * prior to entering switch_to is 8 greater then the value that is saved.
+ * prior to entering switch_to is 8 greater than the value that is saved.
  * If switch_to changes, change following code appropriately.
  */
 void sleeping_thread_to_gdb_regs(unsigned long *gdb_regs, struct task_struct *p)
@@ -219,6 +224,7 @@ int bfin_set_hw_break(unsigned long addr, int len, enum kgdb_bptype type)
 		if (bfin_type == breakinfo[breakno].type
 			&& !breakinfo[breakno].occupied) {
 			breakinfo[breakno].occupied = 1;
+			breakinfo[breakno].skip = 0;
 			breakinfo[breakno].enabled = 1;
 			breakinfo[breakno].addr = addr;
 			breakinfo[breakno].dataacc = dataacc;
@@ -363,12 +369,12 @@ void kgdb_passive_cpu_callback(void *info)
 
 void kgdb_roundup_cpus(unsigned long flags)
 {
-	smp_call_function(kgdb_passive_cpu_callback, NULL, 0, 0);
+	smp_call_function(kgdb_passive_cpu_callback, NULL, 0);
 }
 
 void kgdb_roundup_cpu(int cpu, unsigned long flags)
 {
-	smp_call_function_single(cpu, kgdb_passive_cpu_callback, NULL, 0, 0);
+	smp_call_function_single(cpu, kgdb_passive_cpu_callback, NULL, 0);
 }
 #endif
 
@@ -385,10 +391,8 @@ int kgdb_arch_handle_exception(int vector, int signo,
 			       struct pt_regs *regs)
 {
 	long addr;
-	long breakno;
 	char *ptr;
 	int newPC;
-	int wp_status;
 	int i;
 
 	switch (remcom_in_buffer[0]) {
@@ -426,17 +430,6 @@ int kgdb_arch_handle_exception(int vector, int signo,
 			kgdb_single_step = i + 1;
 		}
 
-		if (vector == VEC_WATCH) {
-			wp_status = bfin_read_WPSTAT();
-			for (breakno = 0; breakno < HW_WATCHPOINT_NUM; breakno++) {
-				if (wp_status & (1 << breakno)) {
-					breakinfo->skip = 1;
-					break;
-				}
-			}
-			bfin_write_WPSTAT(0);
-		}
-
 		bfin_correct_hw_break();
 
 		return 0;
@@ -478,57 +471,32 @@ static int validate_memory_access_address(unsigned long addr, int size)
 		return 0;
 	if (addr >= SYSMMR_BASE)
 		return 0;
-	if (addr >= ASYNC_BANK0_BASE
-	   && addr + size <= ASYNC_BANK3_BASE + ASYNC_BANK3_SIZE)
+	if (IN_MEM(addr, size, ASYNC_BANK0_BASE, ASYNC_BANK_SIZE))
 		return 0;
 	if (cpu == 0) {
-		if (addr >= L1_SCRATCH_START
-		   && (addr + size <= L1_SCRATCH_START + L1_SCRATCH_LENGTH))
+		if (IN_MEM(addr, size, L1_SCRATCH_START, L1_SCRATCH_LENGTH))
 			return 0;
-#if L1_CODE_LENGTH != 0
-		if (addr >= L1_CODE_START
-		   && (addr + size <= L1_CODE_START + L1_CODE_LENGTH))
+		if (IN_MEM(addr, size, L1_CODE_START, L1_CODE_LENGTH))
 			return 0;
-#endif
-#if L1_DATA_A_LENGTH != 0
-		if (addr >= L1_DATA_A_START
-		   && (addr + size <= L1_DATA_A_START + L1_DATA_A_LENGTH))
+		if (IN_MEM(addr, size, L1_DATA_A_START, L1_DATA_A_LENGTH))
 			return 0;
-#endif
-#if L1_DATA_B_LENGTH != 0
-		if (addr >= L1_DATA_B_START
-		   && (addr + size <= L1_DATA_B_START + L1_DATA_B_LENGTH))
+		if (IN_MEM(addr, size, L1_DATA_B_START, L1_DATA_B_LENGTH))
 			return 0;
-#endif
 #ifdef CONFIG_SMP
 	} else if (cpu == 1) {
-		if (addr >= COREB_L1_SCRATCH_START
-		   && (addr + size <= COREB_L1_SCRATCH_START
-		   + L1_SCRATCH_LENGTH))
+		if (IN_MEM(addr, size, COREB_L1_SCRATCH_START, L1_SCRATCH_LENGTH))
 			return 0;
-# if L1_CODE_LENGTH != 0
-		if (addr >= COREB_L1_CODE_START
-		   && (addr + size <= COREB_L1_CODE_START + L1_CODE_LENGTH))
+		if (IN_MEM(addr, size, COREB_L1_CODE_START, L1_CODE_LENGTH))
 			return 0;
-# endif
-# if L1_DATA_A_LENGTH != 0
-		if (addr >= COREB_L1_DATA_A_START
-		   && (addr + size <= COREB_L1_DATA_A_START + L1_DATA_A_LENGTH))
+		if (IN_MEM(addr, size, COREB_L1_DATA_A_START, L1_DATA_A_LENGTH))
 			return 0;
-# endif
-# if L1_DATA_B_LENGTH != 0
-		if (addr >= COREB_L1_DATA_B_START
-		   && (addr + size <= COREB_L1_DATA_B_START + L1_DATA_B_LENGTH))
+		if (IN_MEM(addr, size, COREB_L1_DATA_B_START, L1_DATA_B_LENGTH))
 			return 0;
-# endif
 #endif
 	}
 
-#if L2_LENGTH != 0
-	if (addr >= L2_START
-	   && addr + size <= L2_START + L2_LENGTH)
+	if (IN_MEM(addr, size, L2_START, L2_LENGTH))
 		return 0;
-#endif
 
 	return EFAULT;
 }
@@ -582,12 +550,9 @@ int kgdb_mem2hex(char *mem, char *buf, int count)
 		default:
 			err = EFAULT;
 		}
-	} else if (cpu == 0 && (unsigned int)mem >= L1_CODE_START &&
-		(unsigned int)(mem + count) <= L1_CODE_START + L1_CODE_LENGTH
+	} else if ((cpu == 0 && IN_MEM(mem, count, L1_CODE_START, L1_CODE_LENGTH))
 #ifdef CONFIG_SMP
-		|| cpu == 1 && (unsigned int)mem >= COREB_L1_CODE_START &&
-		(unsigned int)(mem + count) <=
-		COREB_L1_CODE_START + L1_CODE_LENGTH
+		|| (cpu == 1 && IN_MEM(mem, count, COREB_L1_CODE_START, L1_CODE_LENGTH))
 #endif
 		) {
 		/* access L1 instruction SRAM*/
@@ -658,12 +623,9 @@ int kgdb_ebin2mem(char *buf, char *mem, int count)
 		default:
 			return EFAULT;
 		}
-	} else if (cpu == 0 && (unsigned int)mem >= L1_CODE_START &&
-		(unsigned int)(mem + count) < L1_CODE_START + L1_CODE_LENGTH
+	} else if ((cpu == 0 && IN_MEM(mem, count, L1_CODE_START, L1_CODE_LENGTH))
 #ifdef CONFIG_SMP
-		|| cpu == 1 && (unsigned int)mem >= COREB_L1_CODE_START &&
-		(unsigned int)(mem + count) <=
-		COREB_L1_CODE_START + L1_CODE_LENGTH
+		|| (cpu == 1 && IN_MEM(mem, count, COREB_L1_CODE_START, L1_CODE_LENGTH))
 #endif
 		) {
 		/* access L1 instruction SRAM */
@@ -723,12 +685,9 @@ int kgdb_hex2mem(char *buf, char *mem, int count)
 		default:
 			return EFAULT;
 		}
-	} else if (cpu == 0 && (unsigned int)mem >= L1_CODE_START &&
-		(unsigned int)(mem + count) <= L1_CODE_START + L1_CODE_LENGTH
+	} else if ((cpu == 0 && IN_MEM(mem, count, L1_CODE_START, L1_CODE_LENGTH))
 #ifdef CONFIG_SMP
-		|| cpu == 1 && (unsigned int)mem >= COREB_L1_CODE_START &&
-		(unsigned int)(mem + count) <=
-		COREB_L1_CODE_START + L1_CODE_LENGTH
+		|| (cpu == 1 && IN_MEM(mem, count, COREB_L1_CODE_START, L1_CODE_LENGTH))
 #endif
 		) {
 		/* access L1 instruction SRAM */
@@ -745,24 +704,16 @@ int kgdb_validate_break_address(unsigned long addr)
 
 	if (addr >= 0x1000 && (addr + BREAK_INSTR_SIZE) <= physical_mem_end)
 		return 0;
-	if (addr >= ASYNC_BANK0_BASE
-	   && addr + BREAK_INSTR_SIZE <= ASYNC_BANK3_BASE + ASYNC_BANK3_BASE)
+	if (IN_MEM(addr, BREAK_INSTR_SIZE, ASYNC_BANK0_BASE, ASYNC_BANK_SIZE))
 		return 0;
-#if L1_CODE_LENGTH != 0
-	if (cpu == 0 && addr >= L1_CODE_START
-	   && addr + BREAK_INSTR_SIZE <= L1_CODE_START + L1_CODE_LENGTH)
+	if (cpu == 0 && IN_MEM(addr, BREAK_INSTR_SIZE, L1_CODE_START, L1_CODE_LENGTH))
 		return 0;
-# ifdef CONFIG_SMP
-	else if (cpu == 1 && addr >= COREB_L1_CODE_START
-	   && addr + BREAK_INSTR_SIZE <= COREB_L1_CODE_START + L1_CODE_LENGTH)
+#ifdef CONFIG_SMP
+	else if (cpu == 1 && IN_MEM(addr, BREAK_INSTR_SIZE, COREB_L1_CODE_START, L1_CODE_LENGTH))
 		return 0;
-# endif
 #endif
-#if L2_LENGTH != 0
-	if (addr >= L2_START
-	   && addr + BREAK_INSTR_SIZE <= L2_START + L2_LENGTH)
+	if (IN_MEM(addr, BREAK_INSTR_SIZE, L2_START, L2_LENGTH))
 		return 0;
-#endif
 
 	return EFAULT;
 }
@@ -772,13 +723,9 @@ int kgdb_arch_set_breakpoint(unsigned long addr, char *saved_instr)
 	int err;
 	int cpu = raw_smp_processor_id();
 
-	if ((cpu == 0 && (unsigned int)addr >= L1_CODE_START
-		&& (unsigned int)(addr + BREAK_INSTR_SIZE)
-		< L1_CODE_START + L1_CODE_LENGTH)
+	if ((cpu == 0 && IN_MEM(addr, BREAK_INSTR_SIZE, L1_CODE_START, L1_CODE_LENGTH))
 #ifdef CONFIG_SMP
-		|| (cpu == 1 && (unsigned int)addr >= COREB_L1_CODE_START
-		&& (unsigned int)(addr + BREAK_INSTR_SIZE)
-		< COREB_L1_CODE_START + L1_CODE_LENGTH)
+		|| (cpu == 1 && IN_MEM(addr, BREAK_INSTR_SIZE, COREB_L1_CODE_START, L1_CODE_LENGTH))
 #endif
 		) {
 		/* access L1 instruction SRAM */
@@ -804,9 +751,7 @@ int kgdb_arch_set_breakpoint(unsigned long addr, char *saved_instr)
 
 int kgdb_arch_remove_breakpoint(unsigned long addr, char *bundle)
 {
-	if ((unsigned int)addr >= L1_CODE_START &&
-		(unsigned int)(addr + BREAK_INSTR_SIZE) <
-			L1_CODE_START + L1_CODE_LENGTH) {
+	if (IN_MEM(addr, BREAK_INSTR_SIZE, L1_CODE_START, L1_CODE_LENGTH)) {
 		/* access L1 instruction SRAM */
 		if (dma_memcpy((void *)addr, bundle, BREAK_INSTR_SIZE) == NULL)
 			return -EFAULT;
diff --git a/arch/blackfin/kernel/kgdb_test.c b/arch/blackfin/kernel/kgdb_test.c
new file mode 100644
index 000000000000..3dba9c17304a
--- /dev/null
+++ b/arch/blackfin/kernel/kgdb_test.c
@@ -0,0 +1,123 @@
+/*
+ * arch/blackfin/kernel/kgdb_test.c - Blackfin kgdb tests
+ *
+ * Copyright 2005-2008 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/proc_fs.h>
+
+#include <asm/current.h>
+#include <asm/uaccess.h>
+#include <asm/system.h>
+
+#include <asm/blackfin.h>
+
+static char cmdline[256];
+static unsigned long len;
+
+static int num1 __attribute__((l1_data));
+
+void kgdb_l1_test(void) __attribute__((l1_text));
+
+void kgdb_l1_test(void)
+{
+	printk(KERN_ALERT "L1(before change) : data variable addr = 0x%p, data value is %d\n", &num1, num1);
+	printk(KERN_ALERT "L1 : code function addr = 0x%p\n", kgdb_l1_test);
+	num1 = num1 + 10 ;
+	printk(KERN_ALERT "L1(after change) : data variable addr = 0x%p, data value is %d\n", &num1, num1);
+	return ;
+}
+#if L2_LENGTH
+
+static int num2 __attribute__((l2));
+void kgdb_l2_test(void) __attribute__((l2));
+
+void kgdb_l2_test(void)
+{
+	printk(KERN_ALERT "L2(before change) : data variable addr = 0x%p, data value is %d\n", &num2, num2);
+	printk(KERN_ALERT "L2 : code function addr = 0x%p\n", kgdb_l2_test);
+	num2 = num2 + 20 ;
+	printk(KERN_ALERT "L2(after change) : data variable addr = 0x%p, data value is %d\n", &num2, num2);
+	return ;
+}
+
+#endif
+
+
+int kgdb_test(char *name, int len, int count, int z)
+{
+	printk(KERN_DEBUG "kgdb name(%d): %s, %d, %d\n", len, name, count, z);
+	count = z;
+	return count;
+}
+
+static int test_proc_output(char *buf)
+{
+	kgdb_test("hello world!", 12, 0x55, 0x10);
+	kgdb_l1_test();
+	#if L2_LENGTH
+	kgdb_l2_test();
+	#endif
+
+	return 0;
+}
+
+static int test_read_proc(char *page, char **start, off_t off,
+				 int count, int *eof, void *data)
+{
+	int len;
+
+	len = test_proc_output(page);
+	if (len <= off+count)
+		*eof = 1;
+	*start = page + off;
+	len -= off;
+	if (len > count)
+		len = count;
+	if (len < 0)
+		len = 0;
+	return len;
+}
+
+static int test_write_proc(struct file *file, const char *buffer,
+				 unsigned long count, void *data)
+{
+	if (count >= 256)
+		len = 255;
+	else
+		len = count;
+
+	memcpy(cmdline, buffer, count);
+	cmdline[len] = 0;
+
+	return len;
+}
+
+static int __init kgdbtest_init(void)
+{
+	struct proc_dir_entry *entry;
+
+	entry = create_proc_entry("kgdbtest", 0, NULL);
+	if (entry == NULL)
+		return -ENOMEM;
+
+	entry->read_proc = test_read_proc;
+	entry->write_proc = test_write_proc;
+	entry->data = NULL;
+
+	return 0;
+}
+
+static void __exit kgdbtest_exit(void)
+{
+	remove_proc_entry("kgdbtest", NULL);
+}
+
+module_init(kgdbtest_init);
+module_exit(kgdbtest_exit);
+MODULE_LICENSE("GPL");
diff --git a/arch/blackfin/kernel/mcount.S b/arch/blackfin/kernel/mcount.S
new file mode 100644
index 000000000000..edcfb3865f46
--- /dev/null
+++ b/arch/blackfin/kernel/mcount.S
@@ -0,0 +1,70 @@
+/*
+ * linux/arch/blackfin/mcount.S
+ *
+ * Copyright (C) 2006 Analog Devices Inc.
+ *
+ * 2007/04/12 Save index, length, modify and base registers. --rpm
+ */
+
+#include <linux/linkage.h>
+#include <asm/blackfin.h>
+
+.text
+
+.align 4 	/* just in case */
+
+ENTRY(__mcount)
+	[--sp] = i0;
+	[--sp] = i1;
+	[--sp] = i2;
+	[--sp] = i3;
+	[--sp] = l0;
+	[--sp] = l1;
+	[--sp] = l2;
+	[--sp] = l3;
+	[--sp] = m0;
+	[--sp] = m1;
+	[--sp] = m2;
+	[--sp] = m3;
+	[--sp] = b0;
+	[--sp] = b1;
+	[--sp] = b2;
+	[--sp] = b3;
+	[--sp] = ( r7:0, p5:0 );
+	[--sp] = ASTAT;
+
+	p1.L = _ipipe_trace_enable;
+	p1.H = _ipipe_trace_enable;
+	r7 = [p1];
+	CC = r7 == 0;
+	if CC jump out;
+	link 0x10;
+	r0 = 0x0;
+	[sp + 0xc] = r0; /* v */
+	r0 = 0x0;	/* type: IPIPE_TRACE_FN */
+	r1 = rets;
+	p0 = [fp];	/* p0: Prior FP */
+	r2 = [p0 + 4];	/* r2: Prior RETS */
+	call ___ipipe_trace;
+	unlink;
+out:
+	ASTAT = [sp++];
+	( r7:0, p5:0 ) = [sp++];
+	b3 = [sp++];
+	b2 = [sp++];
+	b1 = [sp++];
+	b0 = [sp++];
+	m3 = [sp++];
+	m2 = [sp++];
+	m1 = [sp++];
+	m0 = [sp++];
+	l3 = [sp++];
+	l2 = [sp++];
+	l1 = [sp++];
+	l0 = [sp++];
+	i3 = [sp++];
+	i2 = [sp++];
+	i1 = [sp++];
+	i0 = [sp++];
+	rts;
+ENDPROC(__mcount)
diff --git a/arch/blackfin/kernel/module.c b/arch/blackfin/kernel/module.c
index e1bebc80a5bf..1bd7f2d018a8 100644
--- a/arch/blackfin/kernel/module.c
+++ b/arch/blackfin/kernel/module.c
@@ -37,111 +37,6 @@
 #include <asm/dma.h>
 #include <asm/cacheflush.h>
 
-/*
- * handle arithmetic relocations.
- * See binutils/bfd/elf32-bfin.c for more details
- */
-#define RELOC_STACK_SIZE 100
-static uint32_t reloc_stack[RELOC_STACK_SIZE];
-static unsigned int reloc_stack_tos;
-
-#define is_reloc_stack_empty() ((reloc_stack_tos > 0)?0:1)
-
-static void reloc_stack_push(uint32_t value)
-{
-	reloc_stack[reloc_stack_tos++] = value;
-}
-
-static uint32_t reloc_stack_pop(void)
-{
-	return reloc_stack[--reloc_stack_tos];
-}
-
-static uint32_t reloc_stack_operate(unsigned int oper, struct module *mod)
-{
-	uint32_t value;
-
-	switch (oper) {
-	case R_add:
-		value = reloc_stack[reloc_stack_tos - 2] +
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_sub:
-		value = reloc_stack[reloc_stack_tos - 2] -
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_mult:
-		value = reloc_stack[reloc_stack_tos - 2] *
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_div:
-		value = reloc_stack[reloc_stack_tos - 2] /
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_mod:
-		value = reloc_stack[reloc_stack_tos - 2] %
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_lshift:
-		value = reloc_stack[reloc_stack_tos - 2] <<
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_rshift:
-		value = reloc_stack[reloc_stack_tos - 2] >>
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_and:
-		value = reloc_stack[reloc_stack_tos - 2] &
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_or:
-		value = reloc_stack[reloc_stack_tos - 2] |
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_xor:
-		value = reloc_stack[reloc_stack_tos - 2] ^
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_land:
-		value = reloc_stack[reloc_stack_tos - 2] &&
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_lor:
-		value = reloc_stack[reloc_stack_tos - 2] ||
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_neg:
-		value = -reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos--;
-		break;
-	case R_comp:
-		value = ~reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 1;
-		break;
-	default:
-		printk(KERN_WARNING "module %s: unhandled reloction\n",
-				mod->name);
-		return 0;
-	}
-
-	/* now push the new value back on stack */
-	reloc_stack_push(value);
-
-	return value;
-}
-
 void *module_alloc(unsigned long size)
 {
 	if (size == 0)
@@ -334,16 +229,18 @@ apply_relocate_add(Elf_Shdr * sechdrs, const char *strtab,
 		   undefined symbols have been resolved. */
 		sym = (Elf32_Sym *) sechdrs[symindex].sh_addr
 		    + ELF32_R_SYM(rel[i].r_info);
-		if (is_reloc_stack_empty()) {
-			value = sym->st_value;
-		} else {
-			value = reloc_stack_pop();
-		}
+		value = sym->st_value;
 		value += rel[i].r_addend;
 		pr_debug("location is %x, value is %x type is %d \n",
 			 (unsigned int) location32, value,
 			 ELF32_R_TYPE(rel[i].r_info));
-
+#ifdef CONFIG_SMP
+		if ((unsigned long)location16 >= COREB_L1_DATA_A_START) {
+			printk(KERN_ERR "module %s: cannot relocate in L1: %u (SMP kernel)",
+				       mod->name, ELF32_R_TYPE(rel[i].r_info));
+			return -ENOEXEC;
+		}
+#endif
 		switch (ELF32_R_TYPE(rel[i].r_info)) {
 
 		case R_pcrel24:
@@ -355,6 +252,12 @@ apply_relocate_add(Elf_Shdr * sechdrs, const char *strtab,
 			location32 = (uint32_t *) location16;
 			value -= (uint32_t) location32;
 			value >>= 1;
+			if ((value & 0xFF000000) != 0 &&
+			    (value & 0xFF000000) != 0xFF000000) {
+				printk(KERN_ERR "module %s: relocation overflow\n",
+				       mod->name);
+				return -ENOEXEC;
+			}
 			pr_debug("value is %x, before %x-%x after %x-%x\n", value,
 			       *location16, *(location16 + 1),
 			       (*location16 & 0xff00) | (value >> 16 & 0x00ff),
@@ -399,28 +302,6 @@ apply_relocate_add(Elf_Shdr * sechdrs, const char *strtab,
 			pr_debug("before %x after %x\n", *location32, value);
 			*location32 = value;
 			break;
-		case R_push:
-			reloc_stack_push(value);
-			break;
-		case R_const:
-			reloc_stack_push(rel[i].r_addend);
-			break;
-		case R_add:
-		case R_sub:
-		case R_mult:
-		case R_div:
-		case R_mod:
-		case R_lshift:
-		case R_rshift:
-		case R_and:
-		case R_or:
-		case R_xor:
-		case R_land:
-		case R_lor:
-		case R_neg:
-		case R_comp:
-			reloc_stack_operate(ELF32_R_TYPE(rel[i].r_info), mod);
-			break;
 		default:
 			printk(KERN_ERR "module %s: Unknown relocation: %u\n",
 			       mod->name, ELF32_R_TYPE(rel[i].r_info));
@@ -436,6 +317,7 @@ module_finalize(const Elf_Ehdr * hdr,
 {
 	unsigned int i, strindex = 0, symindex = 0;
 	char *secstrings;
+	long err = 0;
 
 	secstrings = (void *)hdr + sechdrs[hdr->e_shstrndx].sh_offset;
 
@@ -460,8 +342,10 @@ module_finalize(const Elf_Ehdr * hdr,
 		    (strcmp(".rela.l1.text", secstrings + sechdrs[i].sh_name) == 0) ||
 		    ((strcmp(".rela.text", secstrings + sechdrs[i].sh_name) == 0) &&
 			(hdr->e_flags & (EF_BFIN_CODE_IN_L1|EF_BFIN_CODE_IN_L2))))) {
-			apply_relocate_add((Elf_Shdr *) sechdrs, strtab,
+			err = apply_relocate_add((Elf_Shdr *) sechdrs, strtab,
 					   symindex, i, mod);
+			if (err < 0)
+				return -ENOEXEC;
 		}
 	}
 	return 0;
diff --git a/arch/blackfin/kernel/process.c b/arch/blackfin/kernel/process.c
index 0c3ea118b657..33e2e8993f7f 100644
--- a/arch/blackfin/kernel/process.c
+++ b/arch/blackfin/kernel/process.c
@@ -39,6 +39,7 @@
 
 #include <asm/blackfin.h>
 #include <asm/fixed_code.h>
+#include <asm/mem_map.h>
 
 asmlinkage void ret_from_fork(void);
 
@@ -81,11 +82,14 @@ void cpu_idle(void)__attribute__((l1_text));
  */
 static void default_idle(void)
 {
-	local_irq_disable();
+#ifdef CONFIG_IPIPE
+	ipipe_suspend_domain();
+#endif
+	local_irq_disable_hw();
 	if (!need_resched())
 		idle_with_irq_disabled();
 
-	local_irq_enable();
+	local_irq_enable_hw();
 }
 
 /*
@@ -154,6 +158,7 @@ pid_t kernel_thread(int (*fn) (void *), void *arg, unsigned long flags)
 	return do_fork(flags | CLONE_VM | CLONE_UNTRACED, 0, &regs, 0, NULL,
 		       NULL);
 }
+EXPORT_SYMBOL(kernel_thread);
 
 void flush_thread(void)
 {
@@ -170,6 +175,13 @@ asmlinkage int bfin_clone(struct pt_regs *regs)
 	unsigned long clone_flags;
 	unsigned long newsp;
 
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	if (current->rt.nr_cpus_allowed == num_possible_cpus()) {
+		current->cpus_allowed = cpumask_of_cpu(smp_processor_id());
+		current->rt.nr_cpus_allowed = 1;
+	}
+#endif
+
 	/* syscall2 puts clone_flags in r0 and usp in r1 */
 	clone_flags = regs->r0;
 	newsp = regs->r1;
@@ -337,22 +349,22 @@ int _access_ok(unsigned long addr, unsigned long size)
 	if (addr >= (unsigned long)__init_begin &&
 	    addr + size <= (unsigned long)__init_end)
 		return 1;
-	if (addr >= L1_SCRATCH_START
-	    && addr + size <= L1_SCRATCH_START + L1_SCRATCH_LENGTH)
+	if (addr >= get_l1_scratch_start()
+	    && addr + size <= get_l1_scratch_start() + L1_SCRATCH_LENGTH)
 		return 1;
 #if L1_CODE_LENGTH != 0
-	if (addr >= L1_CODE_START + (_etext_l1 - _stext_l1)
-	    && addr + size <= L1_CODE_START + L1_CODE_LENGTH)
+	if (addr >= get_l1_code_start() + (_etext_l1 - _stext_l1)
+	    && addr + size <= get_l1_code_start() + L1_CODE_LENGTH)
 		return 1;
 #endif
 #if L1_DATA_A_LENGTH != 0
-	if (addr >= L1_DATA_A_START + (_ebss_l1 - _sdata_l1)
-	    && addr + size <= L1_DATA_A_START + L1_DATA_A_LENGTH)
+	if (addr >= get_l1_data_a_start() + (_ebss_l1 - _sdata_l1)
+	    && addr + size <= get_l1_data_a_start() + L1_DATA_A_LENGTH)
 		return 1;
 #endif
 #if L1_DATA_B_LENGTH != 0
-	if (addr >= L1_DATA_B_START + (_ebss_b_l1 - _sdata_b_l1)
-	    && addr + size <= L1_DATA_B_START + L1_DATA_B_LENGTH)
+	if (addr >= get_l1_data_b_start() + (_ebss_b_l1 - _sdata_b_l1)
+	    && addr + size <= get_l1_data_b_start() + L1_DATA_B_LENGTH)
 		return 1;
 #endif
 #if L2_LENGTH != 0
diff --git a/arch/blackfin/kernel/ptrace.c b/arch/blackfin/kernel/ptrace.c
index 140bf00e9974..d2d388536630 100644
--- a/arch/blackfin/kernel/ptrace.c
+++ b/arch/blackfin/kernel/ptrace.c
@@ -45,6 +45,7 @@
 #include <asm/asm-offsets.h>
 #include <asm/dma.h>
 #include <asm/fixed_code.h>
+#include <asm/mem_map.h>
 
 #define TEXT_OFFSET 0
 /*
@@ -80,10 +81,12 @@ static inline struct pt_regs *get_user_regs(struct task_struct *task)
 /*
  * Get all user integer registers.
  */
-static inline int ptrace_getregs(struct task_struct *tsk, void __user * uregs)
+static inline int ptrace_getregs(struct task_struct *tsk, void __user *uregs)
 {
-	struct pt_regs *regs = get_user_regs(tsk);
-	return copy_to_user(uregs, regs, sizeof(struct pt_regs)) ? -EFAULT : 0;
+	struct pt_regs regs;
+	memcpy(&regs, get_user_regs(tsk), sizeof(regs));
+	regs.usp = tsk->thread.usp;
+	return copy_to_user(uregs, &regs, sizeof(struct pt_regs)) ? -EFAULT : 0;
 }
 
 /* Mapping from PT_xxx to the stack offset at which the register is
@@ -220,8 +223,8 @@ long arch_ptrace(struct task_struct *child, long request, long addr, long data)
 				break;
 			pr_debug("ptrace: user address is valid\n");
 
-			if (L1_CODE_LENGTH != 0 && addr >= L1_CODE_START
-			    && addr + sizeof(tmp) <= L1_CODE_START + L1_CODE_LENGTH) {
+			if (L1_CODE_LENGTH != 0 && addr >= get_l1_code_start()
+			    && addr + sizeof(tmp) <= get_l1_code_start() + L1_CODE_LENGTH) {
 				safe_dma_memcpy (&tmp, (const void *)(addr), sizeof(tmp));
 				copied = sizeof(tmp);
 
@@ -300,8 +303,8 @@ long arch_ptrace(struct task_struct *child, long request, long addr, long data)
 				break;
 			pr_debug("ptrace: user address is valid\n");
 
-			if (L1_CODE_LENGTH != 0 && addr >= L1_CODE_START
-			    && addr + sizeof(data) <= L1_CODE_START + L1_CODE_LENGTH) {
+			if (L1_CODE_LENGTH != 0 && addr >= get_l1_code_start()
+			    && addr + sizeof(data) <= get_l1_code_start() + L1_CODE_LENGTH) {
 				safe_dma_memcpy ((void *)(addr), &data, sizeof(data));
 				copied = sizeof(data);
 
diff --git a/arch/blackfin/kernel/reboot.c b/arch/blackfin/kernel/reboot.c
index ae97ca407b0d..eeee8cb43360 100644
--- a/arch/blackfin/kernel/reboot.c
+++ b/arch/blackfin/kernel/reboot.c
@@ -21,7 +21,7 @@
  * the core reset.
  */
 __attribute__((l1_text))
-static void bfin_reset(void)
+static void _bfin_reset(void)
 {
 	/* Wait for completion of "system" events such as cache line
 	 * line fills so that we avoid infinite stalls later on as
@@ -66,6 +66,18 @@ static void bfin_reset(void)
 	}
 }
 
+static void bfin_reset(void)
+{
+	if (ANOMALY_05000353 || ANOMALY_05000386)
+		_bfin_reset();
+	else
+		/* the bootrom checks to see how it was reset and will
+		 * automatically perform a software reset for us when
+		 * it starts executing boot
+		 */
+		asm("raise 1;");
+}
+
 __attribute__((weak))
 void native_machine_restart(char *cmd)
 {
@@ -75,14 +87,10 @@ void machine_restart(char *cmd)
 {
 	native_machine_restart(cmd);
 	local_irq_disable();
-	if (ANOMALY_05000353 || ANOMALY_05000386)
-		bfin_reset();
+	if (smp_processor_id())
+		smp_call_function((void *)bfin_reset, 0, 1);
 	else
-		/* the bootrom checks to see how it was reset and will
-		 * automatically perform a software reset for us when
-		 * it starts executing boot
-		 */
-		asm("raise 1;");
+		bfin_reset();
 }
 
 __attribute__((weak))
diff --git a/arch/blackfin/kernel/setup.c b/arch/blackfin/kernel/setup.c
index 71a9a8c53cea..b2a811347b65 100644
--- a/arch/blackfin/kernel/setup.c
+++ b/arch/blackfin/kernel/setup.c
@@ -13,6 +13,7 @@
 #include <linux/bootmem.h>
 #include <linux/seq_file.h>
 #include <linux/cpu.h>
+#include <linux/mm.h>
 #include <linux/module.h>
 #include <linux/tty.h>
 #include <linux/pfn.h>
@@ -26,11 +27,10 @@
 #include <asm/blackfin.h>
 #include <asm/cplbinit.h>
 #include <asm/div64.h>
+#include <asm/cpu.h>
 #include <asm/fixed_code.h>
 #include <asm/early_printk.h>
 
-static DEFINE_PER_CPU(struct cpu, cpu_devices);
-
 u16 _bfin_swrst;
 EXPORT_SYMBOL(_bfin_swrst);
 
@@ -79,27 +79,68 @@ static struct change_member *change_point[2*BFIN_MEMMAP_MAX] __initdata;
 static struct bfin_memmap_entry *overlap_list[BFIN_MEMMAP_MAX] __initdata;
 static struct bfin_memmap_entry new_map[BFIN_MEMMAP_MAX] __initdata;
 
-void __init bfin_cache_init(void)
-{
+DEFINE_PER_CPU(struct blackfin_cpudata, cpu_data);
+
+static int early_init_clkin_hz(char *buf);
+
 #if defined(CONFIG_BFIN_DCACHE) || defined(CONFIG_BFIN_ICACHE)
-	generate_cplb_tables();
+void __init generate_cplb_tables(void)
+{
+	unsigned int cpu;
+
+	generate_cplb_tables_all();
+	/* Generate per-CPU I&D CPLB tables */
+	for (cpu = 0; cpu < num_possible_cpus(); ++cpu)
+		generate_cplb_tables_cpu(cpu);
+}
 #endif
 
+void __cpuinit bfin_setup_caches(unsigned int cpu)
+{
 #ifdef CONFIG_BFIN_ICACHE
-	bfin_icache_init();
-	printk(KERN_INFO "Instruction Cache Enabled\n");
+	bfin_icache_init(icplb_tbl[cpu]);
 #endif
 
 #ifdef CONFIG_BFIN_DCACHE
-	bfin_dcache_init();
-	printk(KERN_INFO "Data Cache Enabled"
+	bfin_dcache_init(dcplb_tbl[cpu]);
+#endif
+
+	/*
+	 * In cache coherence emulation mode, we need to have the
+	 * D-cache enabled before running any atomic operation which
+	 * might invove cache invalidation (i.e. spinlock, rwlock).
+	 * So printk's are deferred until then.
+	 */
+#ifdef CONFIG_BFIN_ICACHE
+	printk(KERN_INFO "Instruction Cache Enabled for CPU%u\n", cpu);
+#endif
+#ifdef CONFIG_BFIN_DCACHE
+	printk(KERN_INFO "Data Cache Enabled for CPU%u"
 # if defined CONFIG_BFIN_WB
 		" (write-back)"
 # elif defined CONFIG_BFIN_WT
 		" (write-through)"
 # endif
-		"\n");
+		"\n", cpu);
+#endif
+}
+
+void __cpuinit bfin_setup_cpudata(unsigned int cpu)
+{
+	struct blackfin_cpudata *cpudata = &per_cpu(cpu_data, cpu);
+
+	cpudata->idle = current;
+	cpudata->loops_per_jiffy = loops_per_jiffy;
+	cpudata->imemctl = bfin_read_IMEM_CONTROL();
+	cpudata->dmemctl = bfin_read_DMEM_CONTROL();
+}
+
+void __init bfin_cache_init(void)
+{
+#if defined(CONFIG_BFIN_DCACHE) || defined(CONFIG_BFIN_ICACHE)
+	generate_cplb_tables();
 #endif
+	bfin_setup_caches(0);
 }
 
 void __init bfin_relocate_l1_mem(void)
@@ -109,6 +150,8 @@ void __init bfin_relocate_l1_mem(void)
 	unsigned long l1_data_b_length;
 	unsigned long l2_length;
 
+	blackfin_dma_early_init();
+
 	l1_code_length = _etext_l1 - _stext_l1;
 	if (l1_code_length > L1_CODE_LENGTH)
 		panic("L1 Instruction SRAM Overflow\n");
@@ -230,7 +273,7 @@ static int __init sanitize_memmap(struct bfin_memmap_entry *map, int *pnr_map)
 	/* record all known change-points (starting and ending addresses),
 	   omitting those that are for empty memory regions */
 	chgidx = 0;
-	for (i = 0; i < old_nr; i++)	{
+	for (i = 0; i < old_nr; i++) {
 		if (map[i].size != 0) {
 			change_point[chgidx]->addr = map[i].addr;
 			change_point[chgidx++]->pentry = &map[i];
@@ -238,13 +281,13 @@ static int __init sanitize_memmap(struct bfin_memmap_entry *map, int *pnr_map)
 			change_point[chgidx++]->pentry = &map[i];
 		}
 	}
-	chg_nr = chgidx;    	/* true number of change-points */
+	chg_nr = chgidx;	/* true number of change-points */
 
 	/* sort change-point list by memory addresses (low -> high) */
 	still_changing = 1;
-	while (still_changing)	{
+	while (still_changing) {
 		still_changing = 0;
-		for (i = 1; i < chg_nr; i++)  {
+		for (i = 1; i < chg_nr; i++) {
 			/* if <current_addr> > <last_addr>, swap */
 			/* or, if current=<start_addr> & last=<end_addr>, swap */
 			if ((change_point[i]->addr < change_point[i-1]->addr) ||
@@ -261,10 +304,10 @@ static int __init sanitize_memmap(struct bfin_memmap_entry *map, int *pnr_map)
 	}
 
 	/* create a new memmap, removing overlaps */
-	overlap_entries = 0;	 /* number of entries in the overlap table */
-	new_entry = 0;	 /* index for creating new memmap entries */
-	last_type = 0;		 /* start with undefined memory type */
-	last_addr = 0;		 /* start with 0 as last starting address */
+	overlap_entries = 0;	/* number of entries in the overlap table */
+	new_entry = 0;		/* index for creating new memmap entries */
+	last_type = 0;		/* start with undefined memory type */
+	last_addr = 0;		/* start with 0 as last starting address */
 	/* loop through change-points, determining affect on the new memmap */
 	for (chgidx = 0; chgidx < chg_nr; chgidx++) {
 		/* keep track of all overlapping memmap entries */
@@ -286,14 +329,14 @@ static int __init sanitize_memmap(struct bfin_memmap_entry *map, int *pnr_map)
 			if (overlap_list[i]->type > current_type)
 				current_type = overlap_list[i]->type;
 		/* continue building up new memmap based on this information */
-		if (current_type != last_type)	{
+		if (current_type != last_type) {
 			if (last_type != 0) {
 				new_map[new_entry].size =
 					change_point[chgidx]->addr - last_addr;
 				/* move forward only if the new size was non-zero */
 				if (new_map[new_entry].size != 0)
 					if (++new_entry >= BFIN_MEMMAP_MAX)
-						break; 	/* no more space left for new entries */
+						break;	/* no more space left for new entries */
 			}
 			if (current_type != 0) {
 				new_map[new_entry].addr = change_point[chgidx]->addr;
@@ -303,9 +346,9 @@ static int __init sanitize_memmap(struct bfin_memmap_entry *map, int *pnr_map)
 			last_type = current_type;
 		}
 	}
-	new_nr = new_entry;   /* retain count for new entries */
+	new_nr = new_entry;	/* retain count for new entries */
 
-	/* copy new  mapping into original location */
+	/* copy new mapping into original location */
 	memcpy(map, new_map, new_nr*sizeof(struct bfin_memmap_entry));
 	*pnr_map = new_nr;
 
@@ -361,7 +404,6 @@ static __init int parse_memmap(char *arg)
  *  - "memmap=XXX[KkmM][@][$]XXX[KkmM]" defines a memory region
  *       @ from <start> to <start>+<mem>, type RAM
  *       $ from <start> to <start>+<mem>, type RESERVED
- *
  */
 static __init void parse_cmdline_early(char *cmdline_p)
 {
@@ -383,14 +425,15 @@ static __init void parse_cmdline_early(char *cmdline_p)
 					if (*to != ' ') {
 						if (*to == '$'
 						    || *(to + 1) == '$')
-							reserved_mem_dcache_on =
-							    1;
+							reserved_mem_dcache_on = 1;
 						if (*to == '#'
 						    || *(to + 1) == '#')
-							reserved_mem_icache_on =
-							    1;
+							reserved_mem_icache_on = 1;
 					}
 				}
+			} else if (!memcmp(to, "clkin_hz=", 9)) {
+				to += 9;
+				early_init_clkin_hz(to);
 			} else if (!memcmp(to, "earlyprintk=", 12)) {
 				to += 12;
 				setup_early_printk(to);
@@ -417,9 +460,8 @@ static __init void parse_cmdline_early(char *cmdline_p)
  *	[_ramend - DMA_UNCACHED_REGION,
  *		_ramend]:			uncached DMA region
  *  [_ramend, physical_mem_end]:	memory not managed by kernel
- *
  */
-static __init void  memory_setup(void)
+static __init void memory_setup(void)
 {
 #ifdef CONFIG_MTD_UCLINUX
 	unsigned long mtd_phys = 0;
@@ -436,7 +478,7 @@ static __init void  memory_setup(void)
 	memory_end = _ramend - DMA_UNCACHED_REGION;
 
 #ifdef CONFIG_MPU
-	/* Round up to multiple of 4MB.  */
+	/* Round up to multiple of 4MB */
 	memory_start = (_ramstart + 0x3fffff) & ~0x3fffff;
 #else
 	memory_start = PAGE_ALIGN(_ramstart);
@@ -616,7 +658,7 @@ static __init void setup_bootmem_allocator(void)
 	end_pfn = memory_end >> PAGE_SHIFT;
 
 	/*
-	 * give all the memory to the bootmap allocator,  tell it to put the
+	 * give all the memory to the bootmap allocator, tell it to put the
 	 * boot mem_map at the start of memory.
 	 */
 	bootmap_size = init_bootmem_node(NODE_DATA(0),
@@ -791,7 +833,11 @@ void __init setup_arch(char **cmdline_p)
 	bfin_write_SWRST(_bfin_swrst | DOUBLE_FAULT);
 #endif
 
+#ifdef CONFIG_SMP
+	if (_bfin_swrst & SWRST_DBL_FAULT_A) {
+#else
 	if (_bfin_swrst & RESET_DOUBLE) {
+#endif
 		printk(KERN_EMERG "Recovering from DOUBLE FAULT event\n");
 #ifdef CONFIG_DEBUG_DOUBLEFAULT
 		/* We assume the crashing kernel, and the current symbol table match */
@@ -823,9 +869,12 @@ void __init setup_arch(char **cmdline_p)
 			if (bfin_compiled_revid() == -1)
 				printk(KERN_ERR "Warning: Compiled for Rev none, but running on Rev %d\n",
 				       bfin_revid());
-			else if (bfin_compiled_revid() != 0xffff)
+			else if (bfin_compiled_revid() != 0xffff) {
 				printk(KERN_ERR "Warning: Compiled for Rev %d, but running on Rev %d\n",
 				       bfin_compiled_revid(), bfin_revid());
+				if (bfin_compiled_revid() > bfin_revid())
+					panic("Error: you are missing anomaly workarounds for this rev\n");
+			}
 		}
 		if (bfin_revid() < CONFIG_BF_REV_MIN || bfin_revid() > CONFIG_BF_REV_MAX)
 			printk(KERN_ERR "Warning: Unsupported Chip Revision ADSP-%s Rev 0.%d detected\n",
@@ -835,7 +884,7 @@ void __init setup_arch(char **cmdline_p)
 	printk(KERN_INFO "Blackfin Linux support by http://blackfin.uclinux.org/\n");
 
 	printk(KERN_INFO "Processor Speed: %lu MHz core clock and %lu MHz System Clock\n",
-	       cclk / 1000000,  sclk / 1000000);
+	       cclk / 1000000, sclk / 1000000);
 
 	if (ANOMALY_05000273 && (cclk >> 1) <= sclk)
 		printk("\n\n\nANOMALY_05000273: CCLK must be >= 2*SCLK !!!\n\n\n");
@@ -867,18 +916,21 @@ void __init setup_arch(char **cmdline_p)
 	BUG_ON((char *)&safe_user_instruction - (char *)&fixed_code_start
 		!= SAFE_USER_INSTRUCTION - FIXED_CODE_START);
 
+#ifdef CONFIG_SMP
+	platform_init_cpus();
+#endif
 	init_exception_vectors();
-	bfin_cache_init();
+	bfin_cache_init();	/* Initialize caches for the boot CPU */
 }
 
 static int __init topology_init(void)
 {
-	int cpu;
+	unsigned int cpu;
+	/* Record CPU-private information for the boot processor. */
+	bfin_setup_cpudata(0);
 
 	for_each_possible_cpu(cpu) {
-		struct cpu *c = &per_cpu(cpu_devices, cpu);
-
-		register_cpu(c, cpu);
+		register_cpu(&per_cpu(cpu_data, cpu).cpu, cpu);
 	}
 
 	return 0;
@@ -886,36 +938,54 @@ static int __init topology_init(void)
 
 subsys_initcall(topology_init);
 
+/* Get the input clock frequency */
+static u_long cached_clkin_hz = CONFIG_CLKIN_HZ;
+static u_long get_clkin_hz(void)
+{
+	return cached_clkin_hz;
+}
+static int __init early_init_clkin_hz(char *buf)
+{
+	cached_clkin_hz = simple_strtoul(buf, NULL, 0);
+#ifdef BFIN_KERNEL_CLOCK
+	if (cached_clkin_hz != CONFIG_CLKIN_HZ)
+		panic("cannot change clkin_hz when reprogramming clocks");
+#endif
+	return 1;
+}
+early_param("clkin_hz=", early_init_clkin_hz);
+
 /* Get the voltage input multiplier */
-static u_long cached_vco_pll_ctl, cached_vco;
 static u_long get_vco(void)
 {
-	u_long msel;
+	static u_long cached_vco;
+	u_long msel, pll_ctl;
 
-	u_long pll_ctl = bfin_read_PLL_CTL();
-	if (pll_ctl == cached_vco_pll_ctl)
+	/* The assumption here is that VCO never changes at runtime.
+	 * If, someday, we support that, then we'll have to change this.
+	 */
+	if (cached_vco)
 		return cached_vco;
-	else
-		cached_vco_pll_ctl = pll_ctl;
 
+	pll_ctl = bfin_read_PLL_CTL();
 	msel = (pll_ctl >> 9) & 0x3F;
 	if (0 == msel)
 		msel = 64;
 
-	cached_vco = CONFIG_CLKIN_HZ;
+	cached_vco = get_clkin_hz();
 	cached_vco >>= (1 & pll_ctl);	/* DF bit */
 	cached_vco *= msel;
 	return cached_vco;
 }
 
 /* Get the Core clock */
-static u_long cached_cclk_pll_div, cached_cclk;
 u_long get_cclk(void)
 {
+	static u_long cached_cclk_pll_div, cached_cclk;
 	u_long csel, ssel;
 
 	if (bfin_read_PLL_STAT() & 0x1)
-		return CONFIG_CLKIN_HZ;
+		return get_clkin_hz();
 
 	ssel = bfin_read_PLL_DIV();
 	if (ssel == cached_cclk_pll_div)
@@ -934,21 +1004,21 @@ u_long get_cclk(void)
 EXPORT_SYMBOL(get_cclk);
 
 /* Get the System clock */
-static u_long cached_sclk_pll_div, cached_sclk;
 u_long get_sclk(void)
 {
+	static u_long cached_sclk;
 	u_long ssel;
 
-	if (bfin_read_PLL_STAT() & 0x1)
-		return CONFIG_CLKIN_HZ;
-
-	ssel = bfin_read_PLL_DIV();
-	if (ssel == cached_sclk_pll_div)
+	/* The assumption here is that SCLK never changes at runtime.
+	 * If, someday, we support that, then we'll have to change this.
+	 */
+	if (cached_sclk)
 		return cached_sclk;
-	else
-		cached_sclk_pll_div = ssel;
 
-	ssel &= 0xf;
+	if (bfin_read_PLL_STAT() & 0x1)
+		return get_clkin_hz();
+
+	ssel = bfin_read_PLL_DIV() & 0xf;
 	if (0 == ssel) {
 		printk(KERN_WARNING "Invalid System Clock\n");
 		ssel = 1;
@@ -982,17 +1052,18 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 {
 	char *cpu, *mmu, *fpu, *vendor, *cache;
 	uint32_t revid;
-
-	u_long cclk = 0, sclk = 0;
+	int cpu_num = *(unsigned int *)v;
+	u_long sclk, cclk;
 	u_int icache_size = BFIN_ICACHESIZE / 1024, dcache_size = 0, dsup_banks = 0;
+	struct blackfin_cpudata *cpudata = &per_cpu(cpu_data, cpu_num);
 
 	cpu = CPU;
 	mmu = "none";
 	fpu = "none";
 	revid = bfin_revid();
 
-	cclk = get_cclk();
 	sclk = get_sclk();
+	cclk = get_cclk();
 
 	switch (bfin_read_CHIPID() & CHIPID_MANUFACTURE) {
 	case 0xca:
@@ -1003,10 +1074,7 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 		break;
 	}
 
-	seq_printf(m, "processor\t: %d\n"
-		"vendor_id\t: %s\n",
-		*(unsigned int *)v,
-		vendor);
+	seq_printf(m, "processor\t: %d\n" "vendor_id\t: %s\n", cpu_num, vendor);
 
 	if (CPUID == bfin_cpuid())
 		seq_printf(m, "cpu family\t: 0x%04x\n", CPUID);
@@ -1029,12 +1097,12 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 		sclk/1000000, sclk%1000000);
 	seq_printf(m, "bogomips\t: %lu.%02lu\n"
 		"Calibration\t: %lu loops\n",
-		(loops_per_jiffy * HZ) / 500000,
-		((loops_per_jiffy * HZ) / 5000) % 100,
-		(loops_per_jiffy * HZ));
+		(cpudata->loops_per_jiffy * HZ) / 500000,
+		((cpudata->loops_per_jiffy * HZ) / 5000) % 100,
+		(cpudata->loops_per_jiffy * HZ));
 
 	/* Check Cache configutation */
-	switch (bfin_read_DMEM_CONTROL() & (1 << DMC0_P | 1 << DMC1_P)) {
+	switch (cpudata->dmemctl & (1 << DMC0_P | 1 << DMC1_P)) {
 	case ACACHE_BSRAM:
 		cache = "dbank-A/B\t: cache/sram";
 		dcache_size = 16;
@@ -1058,10 +1126,10 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 	}
 
 	/* Is it turned on? */
-	if ((bfin_read_DMEM_CONTROL() & (ENDCPLB | DMC_ENABLE)) != (ENDCPLB | DMC_ENABLE))
+	if ((cpudata->dmemctl & (ENDCPLB | DMC_ENABLE)) != (ENDCPLB | DMC_ENABLE))
 		dcache_size = 0;
 
-	if ((bfin_read_IMEM_CONTROL() & (IMC | ENICPLB)) != (IMC | ENICPLB))
+	if ((cpudata->imemctl & (IMC | ENICPLB)) != (IMC | ENICPLB))
 		icache_size = 0;
 
 	seq_printf(m, "cache size\t: %d KB(L1 icache) "
@@ -1086,8 +1154,11 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 		   "dcache setup\t: %d Super-banks/%d Sub-banks/%d Ways, %d Lines/Way\n",
 		   dsup_banks, BFIN_DSUBBANKS, BFIN_DWAYS,
 		   BFIN_DLINES);
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	seq_printf(m, "SMP Dcache Flushes\t: %lu\n\n", cpudata->dcache_invld_count);
+#endif
 #ifdef CONFIG_BFIN_ICACHE_LOCK
-	switch ((bfin_read_IMEM_CONTROL() >> 3) & WAYALL_L) {
+	switch ((cpudata->imemctl >> 3) & WAYALL_L) {
 	case WAY0_L:
 		seq_printf(m, "Way0 Locked-Down\n");
 		break;
@@ -1137,6 +1208,12 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 		seq_printf(m, "No Ways are locked\n");
 	}
 #endif
+
+	if (cpu_num != num_possible_cpus() - 1)
+		return 0;
+
+	if (L2_LENGTH)
+		seq_printf(m, "L2 SRAM\t\t: %dKB\n", L2_LENGTH/0x400);
 	seq_printf(m, "board name\t: %s\n", bfin_board_name);
 	seq_printf(m, "board memory\t: %ld kB (0x%p -> 0x%p)\n",
 		 physical_mem_end >> 10, (void *)0, (void *)physical_mem_end);
@@ -1144,6 +1221,7 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 		((int)memory_end - (int)_stext) >> 10,
 		_stext,
 		(void *)memory_end);
+	seq_printf(m, "\n");
 
 	return 0;
 }
diff --git a/arch/blackfin/kernel/time.c b/arch/blackfin/kernel/time.c
index eb2352320454..172b4c588467 100644
--- a/arch/blackfin/kernel/time.c
+++ b/arch/blackfin/kernel/time.c
@@ -1,32 +1,11 @@
 /*
- * File:         arch/blackfin/kernel/time.c
- * Based on:     none - original work
- * Author:
+ * arch/blackfin/kernel/time.c
  *
- * Created:
- * Description:  This file contains the bfin-specific time handling details.
- *               Most of the stuff is located in the machine specific files.
- *		 FIXME: (This file is subject for removal)
+ * This file contains the Blackfin-specific time handling details.
+ * Most of the stuff is located in the machine specific files.
  *
- * Modified:
- *               Copyright 2004-2008 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ * Copyright 2004-2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
  */
 
 #include <linux/module.h>
@@ -34,23 +13,43 @@
 #include <linux/interrupt.h>
 #include <linux/time.h>
 #include <linux/irq.h>
+#include <linux/delay.h>
 
 #include <asm/blackfin.h>
 #include <asm/time.h>
+#include <asm/gptimers.h>
 
 /* This is an NTP setting */
 #define	TICK_SIZE (tick_nsec / 1000)
 
-static void time_sched_init(irq_handler_t timer_routine);
-static unsigned long gettimeoffset(void);
-
 static struct irqaction bfin_timer_irq = {
-	.name = "BFIN Timer Tick",
+	.name = "Blackfin Timer Tick",
+#ifdef CONFIG_IRQ_PER_CPU
+	.flags = IRQF_DISABLED | IRQF_PERCPU,
+#else
 	.flags = IRQF_DISABLED
+#endif
 };
 
-static void
-time_sched_init(irq_handler_t timer_routine)
+#if defined(CONFIG_TICK_SOURCE_SYSTMR0) || defined(CONFIG_IPIPE)
+void __init setup_system_timer0(void)
+{
+	/* Power down the core timer, just to play safe. */
+	bfin_write_TCNTL(0);
+
+	disable_gptimers(TIMER0bit);
+	set_gptimer_status(0, TIMER_STATUS_TRUN0);
+	while (get_gptimer_status(0) & TIMER_STATUS_TRUN0)
+		udelay(10);
+
+	set_gptimer_config(0, 0x59); /* IRQ enable, periodic, PWM_OUT, SCLKed, OUT PAD disabled */
+	set_gptimer_period(TIMER0_id, get_sclk() / HZ);
+	set_gptimer_pwidth(TIMER0_id, 1);
+	SSYNC();
+	enable_gptimers(TIMER0bit);
+}
+#else
+void __init setup_core_timer(void)
 {
 	u32 tcount;
 
@@ -58,10 +57,8 @@ time_sched_init(irq_handler_t timer_routine)
 	bfin_write_TCNTL(1);
 	CSYNC();
 
-	/*
-	 * the TSCALE prescaler counter.
-	 */
-	bfin_write_TSCALE((TIME_SCALE - 1));
+	/* the TSCALE prescaler counter */
+	bfin_write_TSCALE(TIME_SCALE - 1);
 
 	tcount = ((get_cclk() / (HZ * TIME_SCALE)) - 1);
 	bfin_write_TPERIOD(tcount);
@@ -71,35 +68,52 @@ time_sched_init(irq_handler_t timer_routine)
 	CSYNC();
 
 	bfin_write_TCNTL(7);
+}
+#endif
 
-	bfin_timer_irq.handler = (irq_handler_t)timer_routine;
-	/* call setup_irq instead of request_irq because request_irq calls
-	 * kmalloc which has not been initialized yet
-	 */
+static void __init
+time_sched_init(irqreturn_t(*timer_routine) (int, void *))
+{
+#if defined(CONFIG_TICK_SOURCE_SYSTMR0) || defined(CONFIG_IPIPE)
+	setup_system_timer0();
+	bfin_timer_irq.handler = timer_routine;
+	setup_irq(IRQ_TIMER0, &bfin_timer_irq);
+#else
+	setup_core_timer();
+	bfin_timer_irq.handler = timer_routine;
 	setup_irq(IRQ_CORETMR, &bfin_timer_irq);
+#endif
 }
 
 /*
  * Should return useconds since last timer tick
  */
+#ifndef CONFIG_GENERIC_TIME
 static unsigned long gettimeoffset(void)
 {
 	unsigned long offset;
 	unsigned long clocks_per_jiffy;
 
+#if defined(CONFIG_TICK_SOURCE_SYSTMR0) || defined(CONFIG_IPIPE)
+	clocks_per_jiffy = bfin_read_TIMER0_PERIOD();
+	offset = bfin_read_TIMER0_COUNTER() / \
+		(((clocks_per_jiffy + 1) * HZ) / USEC_PER_SEC);
+
+	if ((get_gptimer_status(0) & TIMER_STATUS_TIMIL0) && offset < (100000 / HZ / 2))
+		offset += (USEC_PER_SEC / HZ);
+#else
 	clocks_per_jiffy = bfin_read_TPERIOD();
-	offset =
-	    (clocks_per_jiffy -
-	     bfin_read_TCOUNT()) / (((clocks_per_jiffy + 1) * HZ) /
-				    USEC_PER_SEC);
+	offset = (clocks_per_jiffy - bfin_read_TCOUNT()) / \
+		(((clocks_per_jiffy + 1) * HZ) / USEC_PER_SEC);
 
 	/* Check if we just wrapped the counters and maybe missed a tick */
 	if ((bfin_read_ILAT() & (1 << IRQ_CORETMR))
-	    && (offset < (100000 / HZ / 2)))
+		&& (offset < (100000 / HZ / 2)))
 		offset += (USEC_PER_SEC / HZ);
-
+#endif
 	return offset;
 }
+#endif
 
 static inline int set_rtc_mmss(unsigned long nowtime)
 {
@@ -111,43 +125,49 @@ static inline int set_rtc_mmss(unsigned long nowtime)
  * as well as call the "do_timer()" routine every clocktick
  */
 #ifdef CONFIG_CORE_TIMER_IRQ_L1
-irqreturn_t timer_interrupt(int irq, void *dummy)__attribute__((l1_text));
+__attribute__((l1_text))
 #endif
-
 irqreturn_t timer_interrupt(int irq, void *dummy)
 {
 	/* last time the cmos clock got updated */
 	static long last_rtc_update;
 
 	write_seqlock(&xtime_lock);
-
-	do_timer(1);
-
-	profile_tick(CPU_PROFILING);
-
-	/*
-	 * If we have an externally synchronized Linux clock, then update
-	 * CMOS clock accordingly every ~11 minutes. Set_rtc_mmss() has to be
-	 * called as close as possible to 500 ms before the new second starts.
-	 */
-
-	if (ntp_synced() &&
-	    xtime.tv_sec > last_rtc_update + 660 &&
-	    (xtime.tv_nsec / NSEC_PER_USEC) >=
-	    500000 - ((unsigned)TICK_SIZE) / 2
-	    && (xtime.tv_nsec / NSEC_PER_USEC) <=
-	    500000 + ((unsigned)TICK_SIZE) / 2) {
-		if (set_rtc_mmss(xtime.tv_sec) == 0)
-			last_rtc_update = xtime.tv_sec;
-		else
-			/* Do it again in 60s. */
-			last_rtc_update = xtime.tv_sec - 600;
+#if defined(CONFIG_TICK_SOURCE_SYSTMR0) && !defined(CONFIG_IPIPE)
+/* FIXME: Here TIMIL0 is not set when IPIPE enabled, why? */
+	if (get_gptimer_status(0) & TIMER_STATUS_TIMIL0) {
+#endif
+		do_timer(1);
+
+		/*
+		 * If we have an externally synchronized Linux clock, then update
+		 * CMOS clock accordingly every ~11 minutes. Set_rtc_mmss() has to be
+		 * called as close as possible to 500 ms before the new second starts.
+		 */
+		if (ntp_synced() &&
+		    xtime.tv_sec > last_rtc_update + 660 &&
+		    (xtime.tv_nsec / NSEC_PER_USEC) >=
+		    500000 - ((unsigned)TICK_SIZE) / 2
+		    && (xtime.tv_nsec / NSEC_PER_USEC) <=
+		    500000 + ((unsigned)TICK_SIZE) / 2) {
+			if (set_rtc_mmss(xtime.tv_sec) == 0)
+				last_rtc_update = xtime.tv_sec;
+			else
+				/* Do it again in 60s. */
+				last_rtc_update = xtime.tv_sec - 600;
+		}
+#if defined(CONFIG_TICK_SOURCE_SYSTMR0) && !defined(CONFIG_IPIPE)
+		set_gptimer_status(0, TIMER_STATUS_TIMIL0);
 	}
+#endif
 	write_sequnlock(&xtime_lock);
 
-#ifndef CONFIG_SMP
+#ifdef CONFIG_IPIPE
+	update_root_process_times(get_irq_regs());
+#else
 	update_process_times(user_mode(get_irq_regs()));
 #endif
+	profile_tick(CPU_PROFILING);
 
 	return IRQ_HANDLED;
 }
diff --git a/arch/blackfin/kernel/traps.c b/arch/blackfin/kernel/traps.c
index bef025b07443..17d8e4172896 100644
--- a/arch/blackfin/kernel/traps.c
+++ b/arch/blackfin/kernel/traps.c
@@ -75,16 +75,6 @@ void __init trap_init(void)
 	CSYNC();
 }
 
-/*
- * Used to save the RETX, SEQSTAT, I/D CPLB FAULT ADDR
- * values across the transition from exception to IRQ5.
- * We put these in L1, so they are going to be in a valid
- * location during exception context
- */
-__attribute__((l1_data))
-unsigned long saved_retx, saved_seqstat,
-	saved_icplb_fault_addr, saved_dcplb_fault_addr;
-
 static void decode_address(char *buf, unsigned long address)
 {
 #ifdef CONFIG_DEBUG_VERBOSE
@@ -211,18 +201,18 @@ asmlinkage void double_fault_c(struct pt_regs *fp)
 	printk(KERN_EMERG "\n" KERN_EMERG "Double Fault\n");
 #ifdef CONFIG_DEBUG_DOUBLEFAULT_PRINT
 	if (((long)fp->seqstat &  SEQSTAT_EXCAUSE) == VEC_UNCOV) {
+		unsigned int cpu = smp_processor_id();
 		char buf[150];
-		decode_address(buf, saved_retx);
+		decode_address(buf, cpu_pda[cpu].retx);
 		printk(KERN_EMERG "While handling exception (EXCAUSE = 0x%x) at %s:\n",
-			(int)saved_seqstat & SEQSTAT_EXCAUSE, buf);
-		decode_address(buf, saved_dcplb_fault_addr);
+			(unsigned int)cpu_pda[cpu].seqstat & SEQSTAT_EXCAUSE, buf);
+		decode_address(buf, cpu_pda[cpu].dcplb_fault_addr);
 		printk(KERN_NOTICE "   DCPLB_FAULT_ADDR: %s\n", buf);
-		decode_address(buf, saved_icplb_fault_addr);
+		decode_address(buf, cpu_pda[cpu].icplb_fault_addr);
 		printk(KERN_NOTICE "   ICPLB_FAULT_ADDR: %s\n", buf);
 
 		decode_address(buf, fp->retx);
-		printk(KERN_NOTICE "The instruction at %s caused a double exception\n",
-			buf);
+		printk(KERN_NOTICE "The instruction at %s caused a double exception\n", buf);
 	} else
 #endif
 	{
@@ -240,6 +230,9 @@ asmlinkage void trap_c(struct pt_regs *fp)
 #ifdef CONFIG_DEBUG_BFIN_HWTRACE_ON
 	int j;
 #endif
+#ifdef CONFIG_DEBUG_HUNT_FOR_ZERO
+	unsigned int cpu = smp_processor_id();
+#endif
 	int sig = 0;
 	siginfo_t info;
 	unsigned long trapnr = fp->seqstat & SEQSTAT_EXCAUSE;
@@ -417,7 +410,7 @@ asmlinkage void trap_c(struct pt_regs *fp)
 		info.si_code = ILL_CPLB_MULHIT;
 		sig = SIGSEGV;
 #ifdef CONFIG_DEBUG_HUNT_FOR_ZERO
-		if (saved_dcplb_fault_addr < FIXED_CODE_START)
+		if (cpu_pda[cpu].dcplb_fault_addr < FIXED_CODE_START)
 			verbose_printk(KERN_NOTICE "NULL pointer access\n");
 		else
 #endif
@@ -471,7 +464,7 @@ asmlinkage void trap_c(struct pt_regs *fp)
 		info.si_code = ILL_CPLB_MULHIT;
 		sig = SIGSEGV;
 #ifdef CONFIG_DEBUG_HUNT_FOR_ZERO
-		if (saved_icplb_fault_addr < FIXED_CODE_START)
+		if (cpu_pda[cpu].icplb_fault_addr < FIXED_CODE_START)
 			verbose_printk(KERN_NOTICE "Jump to NULL address\n");
 		else
 #endif
@@ -584,10 +577,15 @@ asmlinkage void trap_c(struct pt_regs *fp)
 		}
 	}
 
-	info.si_signo = sig;
-	info.si_errno = 0;
-	info.si_addr = (void __user *)fp->pc;
-	force_sig_info(sig, &info, current);
+#ifdef CONFIG_IPIPE
+	if (!ipipe_trap_notify(fp->seqstat & 0x3f, fp))
+#endif
+	{
+		info.si_signo = sig;
+		info.si_errno = 0;
+		info.si_addr = (void __user *)fp->pc;
+		force_sig_info(sig, &info, current);
+	}
 
 	trace_buffer_restore(j);
 	return;
@@ -656,13 +654,13 @@ static bool get_instruction(unsigned short *val, unsigned short *address)
 	return false;
 }
 
-/* 
+/*
  * decode the instruction if we are printing out the trace, as it
  * makes things easier to follow, without running it through objdump
  * These are the normal instructions which cause change of flow, which
  * would be at the source of the trace buffer
  */
-#ifdef CONFIG_DEBUG_VERBOSE
+#if defined(CONFIG_DEBUG_VERBOSE) && defined(CONFIG_DEBUG_BFIN_HWTRACE_ON)
 static void decode_instruction(unsigned short *address)
 {
 	unsigned short opcode;
@@ -846,7 +844,7 @@ void show_stack(struct task_struct *task, unsigned long *stack)
 	}
 	if (fp) {
 		frame = fp;
-		printk(" FP: (0x%p)\n", fp);
+		printk(KERN_NOTICE " FP: (0x%p)\n", fp);
 	} else
 		frame = 0;
 
@@ -960,6 +958,7 @@ void dump_bfin_process(struct pt_regs *fp)
 		else
 			verbose_printk(KERN_NOTICE "COMM= invalid\n");
 
+		printk(KERN_NOTICE "CPU = %d\n", current_thread_info()->cpu);
 		if (!((unsigned long)current->mm & 0x3) && (unsigned long)current->mm >= FIXED_CODE_START)
 			verbose_printk(KERN_NOTICE  "TEXT = 0x%p-0x%p        DATA = 0x%p-0x%p\n"
 				KERN_NOTICE " BSS = 0x%p-0x%p  USER-STACK = 0x%p\n"
@@ -1053,6 +1052,7 @@ void show_regs(struct pt_regs *fp)
 	struct irqaction *action;
 	unsigned int i;
 	unsigned long flags;
+	unsigned int cpu = smp_processor_id();
 
 	verbose_printk(KERN_NOTICE "\n" KERN_NOTICE "SEQUENCER STATUS:\t\t%s\n", print_tainted());
 	verbose_printk(KERN_NOTICE " SEQSTAT: %08lx  IPEND: %04lx  SYSCFG: %04lx\n",
@@ -1112,9 +1112,9 @@ unlock:
 
 	if (((long)fp->seqstat &  SEQSTAT_EXCAUSE) &&
 	    (((long)fp->seqstat & SEQSTAT_EXCAUSE) != VEC_HWERR)) {
-		decode_address(buf, saved_dcplb_fault_addr);
+		decode_address(buf, cpu_pda[cpu].dcplb_fault_addr);
 		verbose_printk(KERN_NOTICE "DCPLB_FAULT_ADDR: %s\n", buf);
-		decode_address(buf, saved_icplb_fault_addr);
+		decode_address(buf, cpu_pda[cpu].icplb_fault_addr);
 		verbose_printk(KERN_NOTICE "ICPLB_FAULT_ADDR: %s\n", buf);
 	}
 
@@ -1153,20 +1153,21 @@ unlock:
 asmlinkage int sys_bfin_spinlock(int *spinlock)__attribute__((l1_text));
 #endif
 
-asmlinkage int sys_bfin_spinlock(int *spinlock)
+static DEFINE_SPINLOCK(bfin_spinlock_lock);
+
+asmlinkage int sys_bfin_spinlock(int *p)
 {
-	int ret = 0;
-	int tmp = 0;
+	int ret, tmp = 0;
 
-	local_irq_disable();
-	ret = get_user(tmp, spinlock);
-	if (ret == 0) {
-		if (tmp)
+	spin_lock(&bfin_spinlock_lock);	/* This would also hold kernel preemption. */
+	ret = get_user(tmp, p);
+	if (likely(ret == 0)) {
+		if (unlikely(tmp))
 			ret = 1;
-		tmp = 1;
-		put_user(tmp, spinlock);
+		else
+			put_user(1, p);
 	}
-	local_irq_enable();
+	spin_unlock(&bfin_spinlock_lock);
 	return ret;
 }
 
diff --git a/arch/blackfin/kernel/vmlinux.lds.S b/arch/blackfin/kernel/vmlinux.lds.S
index 7d12c6692a65..4b4341da0585 100644
--- a/arch/blackfin/kernel/vmlinux.lds.S
+++ b/arch/blackfin/kernel/vmlinux.lds.S
@@ -68,6 +68,8 @@ SECTIONS
 		__etext = .;
 	}
 
+	NOTES
+
 	/* Just in case the first read only is a 32-bit access */
 	RO_DATA(4)
 
@@ -109,7 +111,6 @@ SECTIONS
 #endif
 
 		DATA_DATA
-		*(.data.*)
 		CONSTRUCTORS
 
 		/* make sure the init_task is aligned to the
@@ -161,12 +162,14 @@ SECTIONS
 		*(.con_initcall.init)
 		___con_initcall_end = .;
 	}
+	PERCPU(4)
 	SECURITY_INIT
 	.init.ramfs :
 	{
 		. = ALIGN(4);
 		___initramfs_start = .;
 		*(.init.ramfs)
+		. = ALIGN(4);
 		___initramfs_end = .;
 	}
 
@@ -212,7 +215,7 @@ SECTIONS
 		__ebss_b_l1 = .;
 	}
 
-	__l2_lma_start = .;
+	__l2_lma_start = LOADADDR(.data_b_l1) + SIZEOF(.data_b_l1);
 
 	.text_data_l2 L2_START : AT(LOADADDR(.data_b_l1) + SIZEOF(.data_b_l1))
 	{
@@ -240,7 +243,7 @@ SECTIONS
 	/* Force trailing alignment of our init section so that when we
 	 * free our init memory, we don't leave behind a partial page.
 	 */
-	. = LOADADDR(.data_b_l1) + SIZEOF(.data_b_l1);
+	. = LOADADDR(.text_data_l2) + SIZEOF(.text_data_l2);
 	. = ALIGN(PAGE_SIZE);
 	___init_end = .;
 
diff --git a/arch/blackfin/lib/checksum.c b/arch/blackfin/lib/checksum.c
index 5c87505165d3..762a7f02970a 100644
--- a/arch/blackfin/lib/checksum.c
+++ b/arch/blackfin/lib/checksum.c
@@ -29,6 +29,7 @@
  * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  */
 
+#include <linux/module.h>
 #include <net/checksum.h>
 #include <asm/checksum.h>
 
@@ -76,6 +77,7 @@ __sum16 ip_fast_csum(unsigned char *iph, unsigned int ihl)
 {
 	return (__force __sum16)~do_csum(iph, ihl * 4);
 }
+EXPORT_SYMBOL(ip_fast_csum);
 
 /*
  * computes the checksum of a memory block at buff, length len,
@@ -104,6 +106,7 @@ __wsum csum_partial(const void *buff, int len, __wsum sum)
 
 	return sum;
 }
+EXPORT_SYMBOL(csum_partial);
 
 /*
  * this routine is used for miscellaneous IP-like checksums, mainly
@@ -137,3 +140,4 @@ __wsum csum_partial_copy(const void *src, void *dst, int len, __wsum sum)
 	memcpy(dst, src, len);
 	return csum_partial(dst, len, sum);
 }
+EXPORT_SYMBOL(csum_partial_copy);
diff --git a/arch/blackfin/lib/ins.S b/arch/blackfin/lib/ins.S
index d60554dce87b..1863a6ba507c 100644
--- a/arch/blackfin/lib/ins.S
+++ b/arch/blackfin/lib/ins.S
@@ -1,31 +1,9 @@
 /*
- * File:         arch/blackfin/lib/ins.S
- * Based on:
- * Author:       Bas Vermeulen <bas@buyways.nl>
+ * arch/blackfin/lib/ins.S - ins{bwl} using hardware loops
  *
- * Created:      Tue Mar 22 15:27:24 CEST 2005
- * Description:  Implementation of ins{bwl} for BlackFin processors using zero overhead loops.
- *
- * Modified:
- *               Copyright 2004-2008 Analog Devices Inc.
- *               Copyright (C) 2005 Bas Vermeulen, BuyWays BV <bas@buyways.nl>
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ * Copyright 2004-2008 Analog Devices Inc.
+ * Copyright (C) 2005 Bas Vermeulen, BuyWays BV <bas@buyways.nl>
+ * Licensed under the GPL-2 or later.
  */
 
 #include <linux/linkage.h>
@@ -33,6 +11,46 @@
 
 .align 2
 
+#ifdef CONFIG_IPIPE
+# define DO_CLI \
+	[--sp] = rets; \
+	[--sp] = (P5:0); \
+	sp += -12; \
+	call ___ipipe_stall_root_raw; \
+	sp += 12; \
+	(P5:0) = [sp++];
+# define CLI_INNER_NOP
+#else
+# define DO_CLI cli R3;
+# define CLI_INNER_NOP nop; nop; nop;
+#endif
+
+#ifdef CONFIG_IPIPE
+# define DO_STI \
+	sp += -12; \
+	call ___ipipe_unstall_root_raw; \
+	sp += 12; \
+2:	rets = [sp++];
+#else
+# define DO_STI 2: sti R3;
+#endif
+
+#ifdef CONFIG_BFIN_INS_LOWOVERHEAD
+# define CLI_OUTER DO_CLI;
+# define STI_OUTER DO_STI;
+# define CLI_INNER 1:
+# if ANOMALY_05000416
+#  define STI_INNER nop; 2: nop;
+# else
+#  define STI_INNER 2:
+# endif
+#else
+# define CLI_OUTER
+# define STI_OUTER
+# define CLI_INNER 1: DO_CLI; CLI_INNER_NOP;
+# define STI_INNER DO_STI;
+#endif
+
 /*
  * Reads on the Blackfin are speculative. In Blackfin terms, this means they
  * can be interrupted at any time (even after they have been issued on to the
@@ -53,170 +71,48 @@
  * buffers in/out of FIFOs.
  */
 
-ENTRY(_insl)
-#ifdef CONFIG_BFIN_INS_LOWOVERHEAD
-	P0 = R0;	/* P0 = port */
-	cli R3;
-	P1 = R1;	/* P1 = address */
-	P2 = R2;	/* P2 = count */
-	SSYNC;
-	LSETUP( .Llong_loop_s, .Llong_loop_e) LC0 = P2;
-.Llong_loop_s:  R0 = [P0];
-		[P1++] = R0;
-		NOP;
-.Llong_loop_e: 	NOP;
-	sti R3;
-	RTS;
-#else
-	P0 = R0;	/* P0 = port */
-	P1 = R1;	/* P1 = address */
-	P2 = R2;	/* P2 = count */
-	SSYNC;
-	LSETUP( .Llong_loop_s, .Llong_loop_e) LC0 = P2;
-.Llong_loop_s:
-	CLI R3;
-	NOP; NOP; NOP;
-	R0 = [P0];
-	[P1++] = R0;
-.Llong_loop_e:
-	STI R3;
+#define COMMON_INS(func, ops) \
+ENTRY(_ins##func) \
+	P0 = R0;	/* P0 = port */ \
+	CLI_OUTER;	/* 3 instructions before first read access */ \
+	P1 = R1;	/* P1 = address */ \
+	P2 = R2;	/* P2 = count */ \
+	SSYNC; \
+ \
+	LSETUP(1f, 2f) LC0 = P2; \
+	CLI_INNER; \
+	ops; \
+	STI_INNER; \
+ \
+	STI_OUTER; \
+	RTS; \
+ENDPROC(_ins##func)
 
-	RTS;
-#endif
-ENDPROC(_insl)
-
-ENTRY(_insw)
-#ifdef CONFIG_BFIN_INS_LOWOVERHEAD
-	P0 = R0;	/* P0 = port */
-	cli R3;
-	P1 = R1;	/* P1 = address */
-	P2 = R2;	/* P2 = count */
-	SSYNC;
-	LSETUP( .Lword_loop_s, .Lword_loop_e) LC0 = P2;
-.Lword_loop_s:  R0 = W[P0];
-		W[P1++] = R0;
-		NOP;
-.Lword_loop_e: 	NOP;
-	sti R3;
-	RTS;
-#else
-	P0 = R0;	/* P0 = port */
-	P1 = R1;	/* P1 = address */
-	P2 = R2;	/* P2 = count */
-	SSYNC;
-	LSETUP( .Lword_loop_s, .Lword_loop_e) LC0 = P2;
-.Lword_loop_s:
-	CLI R3;
-	NOP; NOP; NOP;
-	R0 = W[P0];
-	W[P1++] = R0;
-.Lword_loop_e:
-	STI R3;
-	RTS;
-
-#endif
-ENDPROC(_insw)
-
-ENTRY(_insw_8)
-#ifdef CONFIG_BFIN_INS_LOWOVERHEAD
-	P0 = R0;	/* P0 = port */
-	cli R3;
-	P1 = R1;	/* P1 = address */
-	P2 = R2;	/* P2 = count */
-	SSYNC;
-	LSETUP( .Lword8_loop_s, .Lword8_loop_e) LC0 = P2;
-.Lword8_loop_s:  R0 = W[P0];
-		B[P1++] = R0;
-		R0 = R0 >> 8;
-		B[P1++] = R0;
-		NOP;
-.Lword8_loop_e: NOP;
-	sti R3;
-	RTS;
-#else
-	P0 = R0;	/* P0 = port */
-	P1 = R1;	/* P1 = address */
-	P2 = R2;	/* P2 = count */
-	SSYNC;
-	LSETUP( .Lword8_loop_s, .Lword8_loop_e) LC0 = P2;
-.Lword8_loop_s:
-	CLI R3;
-	NOP; NOP; NOP;
-	R0 = W[P0];
-	B[P1++] = R0;
-	R0 = R0 >> 8;
-	B[P1++] = R0;
-	NOP;
-.Lword8_loop_e:
-	STI R3;
+COMMON_INS(l, \
+	R0 = [P0]; \
+	[P1++] = R0; \
+)
 
-	RTS;
-#endif
-ENDPROC(_insw_8)
+COMMON_INS(w, \
+	R0 = W[P0]; \
+	W[P1++] = R0; \
+)
 
-ENTRY(_insb)
-#ifdef CONFIG_BFIN_INS_LOWOVERHEAD
-	P0 = R0;	/* P0 = port */
-	cli R3;
-	P1 = R1;	/* P1 = address */
-	P2 = R2;	/* P2 = count */
-	SSYNC;
-	LSETUP( .Lbyte_loop_s, .Lbyte_loop_e) LC0 = P2;
-.Lbyte_loop_s:  R0 = B[P0];
-		B[P1++] = R0;
-		NOP;
-.Lbyte_loop_e:  NOP;
-	sti R3;
-	RTS;
-#else
-	P0 = R0;        /* P0 = port */
-	P1 = R1;        /* P1 = address */
-	P2 = R2;        /* P2 = count */
-	SSYNC;
-	LSETUP( .Lbyte_loop_s, .Lbyte_loop_e) LC0 = P2;
-.Lbyte_loop_s:
-	CLI R3;
-	NOP; NOP; NOP;
-	R0 = B[P0];
-	B[P1++] = R0;
-.Lbyte_loop_e:
-	STI R3;
+COMMON_INS(w_8, \
+	R0 = W[P0]; \
+	B[P1++] = R0; \
+	R0 = R0 >> 8; \
+	B[P1++] = R0; \
+)
 
-	RTS;
-#endif
-ENDPROC(_insb)
+COMMON_INS(b, \
+	R0 = B[P0]; \
+	B[P1++] = R0; \
+)
 
-ENTRY(_insl_16)
-#ifdef CONFIG_BFIN_INS_LOWOVERHEAD
-	P0 = R0;	/* P0 = port */
-	cli R3;
-	P1 = R1;	/* P1 = address */
-	P2 = R2;	/* P2 = count */
-	SSYNC;
-	LSETUP( .Llong16_loop_s, .Llong16_loop_e) LC0 = P2;
-.Llong16_loop_s:  R0 = [P0];
-		  W[P1++] = R0;
-		  R0 = R0 >> 16;
-		  W[P1++] = R0;
-		  NOP;
-.Llong16_loop_e:  NOP;
-	sti R3;
-	RTS;
-#else
-	P0 = R0;	/* P0 = port */
-	P1 = R1;	/* P1 = address */
-	P2 = R2;	/* P2 = count */
-	SSYNC;
-	LSETUP( .Llong16_loop_s, .Llong16_loop_e) LC0 = P2;
-.Llong16_loop_s:
-	CLI R3;
-	NOP; NOP; NOP;
-	R0 = [P0];
-	W[P1++] = R0;
-	R0 = R0 >> 16;
-	W[P1++] = R0;
-.Llong16_loop_e:
-	STI R3;
-	RTS;
-#endif
-ENDPROC(_insl_16)
+COMMON_INS(l_16, \
+	R0 = [P0]; \
+	W[P1++] = R0; \
+	R0 = R0 >> 16; \
+	W[P1++] = R0; \
+)
diff --git a/arch/blackfin/lib/muldi3.S b/arch/blackfin/lib/muldi3.S
new file mode 100644
index 000000000000..abde120ee230
--- /dev/null
+++ b/arch/blackfin/lib/muldi3.S
@@ -0,0 +1,68 @@
+.align 2
+.global ___muldi3;
+.type ___muldi3, STT_FUNC;
+
+#ifdef CONFIG_ARITHMETIC_OPS_L1
+.section .l1.text
+#else
+.text
+#endif
+
+/*
+	   R1:R0 * R3:R2
+	 = R1.h:R1.l:R0.h:R0.l * R3.h:R3.l:R2.h:R2.l
+[X]	 = (R1.h * R3.h) * 2^96
+[X]	   + (R1.h * R3.l + R1.l * R3.h) * 2^80
+[X]	   + (R1.h * R2.h + R1.l * R3.l + R3.h * R0.h) * 2^64
+[T1]	   + (R1.h * R2.l + R3.h * R0.l + R1.l * R2.h + R3.l * R0.h) * 2^48
+[T2]	   + (R1.l * R2.l + R3.l * R0.l + R0.h * R2.h) * 2^32
+[T3]	   + (R0.l * R2.h + R2.l * R0.h) * 2^16
+[T4]	   + (R0.l * R2.l)
+
+	We can discard the first three lines marked "X" since we produce
+	only a 64 bit result.  So, we need ten 16-bit multiplies.
+
+	Individual mul-acc results:
+[E1]	 =  R1.h * R2.l + R3.h * R0.l + R1.l * R2.h + R3.l * R0.h
+[E2]	 =  R1.l * R2.l + R3.l * R0.l + R0.h * R2.h
+[E3]	 =  R0.l * R2.h + R2.l * R0.h
+[E4]	 =  R0.l * R2.l
+
+	We also need to add high parts from lower-level results to higher ones:
+	E[n]c = E[n] + (E[n+1]c >> 16), where E4c := E4
+
+	One interesting property is that all parts of the result that depend
+	on the sign of the multiplication are discarded.  Those would be the
+	multiplications involving R1.h and R3.h, but only the top 16 bit of
+	the 32 bit result depend on the sign, and since R1.h and R3.h only
+	occur in E1, the top half of these results is cut off.
+	So, we can just use FU mode for all of the 16-bit multiplies, and
+	ignore questions of when to use mixed mode.  */
+
+___muldi3:
+	/* [SP] technically is part of the caller's frame, but we can
+	   use it as scratch space.  */
+	A0 = R2.H * R1.L, A1 = R2.L * R1.H (FU) || R3 = [SP + 12];	/* E1 */
+	A0 += R3.H * R0.L, A1 += R3.L * R0.H (FU) || [SP] = R4;		/* E1 */
+	A0 += A1;							/* E1 */
+	R4 = A0.w;
+	A0 = R0.l * R3.l (FU);						/* E2 */
+	A0 += R2.l * R1.l (FU);						/* E2 */
+
+	A1 = R2.L * R0.L (FU);						/* E4 */
+	R3 = A1.w;
+	A1 = A1 >> 16;							/* E3c */
+	A0 += R2.H * R0.H, A1 += R2.L * R0.H (FU);			/* E2, E3c */
+	A1 += R0.L * R2.H (FU);						/* E3c */
+	R0 = A1.w;
+	A1 = A1 >> 16;							/* E2c */
+	A0 += A1;							/* E2c */
+	R1 = A0.w;
+
+	/* low(result) = low(E3c):low(E4) */
+	R0 = PACK (R0.l, R3.l);
+	/* high(result) = E2c + (E1 << 16) */
+	R1.h = R1.h + R4.l (NS) || R4 = [SP];
+	RTS;
+
+.size ___muldi3, .-___muldi3
diff --git a/arch/blackfin/lib/muldi3.c b/arch/blackfin/lib/muldi3.c
deleted file mode 100644
index 303d0c6a6dba..000000000000
--- a/arch/blackfin/lib/muldi3.c
+++ /dev/null
@@ -1,99 +0,0 @@
-/*
- * File:         arch/blackfin/lib/muldi3.c
- * Based on:
- * Author:
- *
- * Created:
- * Description:
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#ifndef SI_TYPE_SIZE
-#define SI_TYPE_SIZE 32
-#endif
-#define __ll_b (1L << (SI_TYPE_SIZE / 2))
-#define __ll_lowpart(t) ((usitype) (t) % __ll_b)
-#define __ll_highpart(t) ((usitype) (t) / __ll_b)
-#define BITS_PER_UNIT 8
-
-#if !defined(umul_ppmm)
-#define umul_ppmm(w1, w0, u, v)						\
-  do {									\
-    usitype __x0, __x1, __x2, __x3;					\
-    usitype __ul, __vl, __uh, __vh;					\
-									\
-    __ul = __ll_lowpart (u);						\
-    __uh = __ll_highpart (u);						\
-    __vl = __ll_lowpart (v);						\
-    __vh = __ll_highpart (v);						\
-									\
-    __x0 = (usitype) __ul * __vl;					\
-    __x1 = (usitype) __ul * __vh;					\
-    __x2 = (usitype) __uh * __vl;					\
-    __x3 = (usitype) __uh * __vh;					\
-									\
-    __x1 += __ll_highpart (__x0);/* this can't give carry */		\
-    __x1 += __x2;		/* but this indeed can */		\
-    if (__x1 < __x2)		/* did we get it? */			\
-      __x3 += __ll_b;		/* yes, add it in the proper pos. */	\
-									\
-    (w1) = __x3 + __ll_highpart (__x1);					\
-    (w0) = __ll_lowpart (__x1) * __ll_b + __ll_lowpart (__x0);		\
-  } while (0)
-#endif
-
-#if !defined(__umulsidi3)
-#define __umulsidi3(u, v) 						\
-  ({diunion __w;                                                        \
-       umul_ppmm (__w.s.high, __w.s.low, u, v);                         \
-           __w.ll; })
-#endif
-
-typedef unsigned int usitype __attribute__ ((mode(SI)));
-typedef int sitype __attribute__ ((mode(SI)));
-typedef int ditype __attribute__ ((mode(DI)));
-typedef int word_type __attribute__ ((mode(__word__)));
-
-struct distruct {
-	sitype low, high;
-};
-typedef union {
-	struct distruct s;
-	ditype ll;
-} diunion;
-
-#ifdef CONFIG_ARITHMETIC_OPS_L1
-ditype __muldi3(ditype u, ditype v)__attribute__((l1_text));
-#endif
-
-ditype __muldi3(ditype u, ditype v)
-{
-	diunion w;
-	diunion uu, vv;
-
-	uu.ll = u, vv.ll = v;
-	w.ll = __umulsidi3(uu.s.low, vv.s.low);
-	w.s.high += ((usitype) uu.s.low * (usitype) vv.s.high
-		     + (usitype) uu.s.high * (usitype) vv.s.low);
-
-	return w.ll;
-}
diff --git a/arch/blackfin/mach-bf518/Kconfig b/arch/blackfin/mach-bf518/Kconfig
new file mode 100644
index 000000000000..f397ede006bf
--- /dev/null
+++ b/arch/blackfin/mach-bf518/Kconfig
@@ -0,0 +1,233 @@
+if (BF51x)
+
+source "arch/blackfin/mach-bf518/boards/Kconfig"
+
+menu "BF518 Specific Configuration"
+
+comment "Alternative Multiplexing Scheme"
+
+choice
+	prompt "SPORT0"
+	default BF518_SPORT0_PORTG
+	help
+	  Select PORT used for SPORT0. See Hardware Reference Manual
+
+config BF518_SPORT0_PORTF
+	bool "PORT F"
+	help
+	  PORT F
+
+config BF518_SPORT0_PORTG
+	bool "PORT G"
+	help
+	  PORT G
+endchoice
+
+choice
+	prompt "SPORT0 TSCLK Location"
+	depends on BF518_SPORT0_PORTG
+	default BF518_SPORT0_TSCLK_PG10
+	help
+	  Select PIN used for SPORT0_TSCLK. See Hardware Reference Manual
+
+config BF518_SPORT0_TSCLK_PG10
+	bool "PORT PG10"
+	help
+	  PORT PG10
+
+config BF518_SPORT0_TSCLK_PG14
+	bool "PORT PG14"
+	help
+	  PORT PG14
+endchoice
+
+choice
+	prompt "UART1"
+	default BF518_UART1_PORTF
+	help
+	  Select PORT used for UART1. See Hardware Reference Manual
+
+config BF518_UART1_PORTF
+	bool "PORT F"
+	help
+	  PORT F
+
+config BF518_UART1_PORTG
+	bool "PORT G"
+	help
+	  PORT G
+endchoice
+
+comment "Interrupt Priority Assignment"
+menu "Priority"
+
+config IRQ_PLL_WAKEUP
+	int "IRQ_PLL_WAKEUP"
+	default 7
+config IRQ_DMA0_ERROR
+	int "IRQ_DMA0_ERROR"
+	default 7
+config IRQ_DMAR0_BLK
+	int "IRQ_DMAR0_BLK"
+	default 7
+config IRQ_DMAR1_BLK
+	int "IRQ_DMAR1_BLK"
+	default 7
+config IRQ_DMAR0_OVR
+	int "IRQ_DMAR0_OVR"
+	default 7
+config IRQ_DMAR1_OVR
+	int "IRQ_DMAR1_OVR"
+	default 7
+config IRQ_PPI_ERROR
+	int "IRQ_PPI_ERROR"
+	default 7
+config IRQ_MAC_ERROR
+	int "IRQ_MAC_ERROR"
+	default 7
+config IRQ_SPORT0_ERROR
+	int "IRQ_SPORT0_ERROR"
+	default 7
+config IRQ_SPORT1_ERROR
+	int "IRQ_SPORT1_ERROR"
+	default 7
+config IRQ_PTP_ERROR
+	int "IRQ_PTP_ERROR"
+	default 7
+config IRQ_UART0_ERROR
+	int "IRQ_UART0_ERROR"
+	default 7
+config IRQ_UART1_ERROR
+	int "IRQ_UART1_ERROR"
+	default 7
+config IRQ_RTC
+	int "IRQ_RTC"
+	default 8
+config IRQ_PPI
+	int "IRQ_PPI"
+	default 8
+config IRQ_SPORT0_RX
+	int "IRQ_SPORT0_RX"
+	default 9
+config IRQ_SPORT0_TX
+	int "IRQ_SPORT0_TX"
+	default 9
+config IRQ_SPORT1_RX
+	int "IRQ_SPORT1_RX"
+	default 9
+config IRQ_SPORT1_TX
+	int "IRQ_SPORT1_TX"
+	default 9
+config IRQ_TWI
+	int "IRQ_TWI"
+	default 10
+config IRQ_SPI0
+	int "IRQ_SPI"
+	default 10
+config IRQ_UART0_RX
+	int "IRQ_UART0_RX"
+	default 10
+config IRQ_UART0_TX
+	int "IRQ_UART0_TX"
+	default 10
+config IRQ_UART1_RX
+	int "IRQ_UART1_RX"
+	default 10
+config IRQ_UART1_TX
+	int "IRQ_UART1_TX"
+	default 10
+config IRQ_OPTSEC
+	int "IRQ_OPTSEC"
+	default 11
+config IRQ_CNT
+	int "IRQ_CNT"
+	default 11
+config IRQ_MAC_RX
+	int "IRQ_MAC_RX"
+	default 11
+config IRQ_PORTH_INTA
+	int "IRQ_PORTH_INTA"
+	default 11
+config IRQ_MAC_TX
+	int "IRQ_MAC_TX/NFC"
+	default 11
+config IRQ_PORTH_INTB
+	int "IRQ_PORTH_INTB"
+	default 11
+config IRQ_TIMER0
+	int "IRQ_TIMER0"
+	default 8
+config IRQ_TIMER1
+	int "IRQ_TIMER1"
+	default 12
+config IRQ_TIMER2
+	int "IRQ_TIMER2"
+	default 12
+config IRQ_TIMER3
+	int "IRQ_TIMER3"
+	default 12
+config IRQ_TIMER4
+	int "IRQ_TIMER4"
+	default 12
+config IRQ_TIMER5
+	int "IRQ_TIMER5"
+	default 12
+config IRQ_TIMER6
+	int "IRQ_TIMER6"
+	default 12
+config IRQ_TIMER7
+	int "IRQ_TIMER7"
+	default 12
+config IRQ_PORTG_INTA
+	int "IRQ_PORTG_INTA"
+	default 12
+config IRQ_PORTG_INTB
+	int "IRQ_PORTG_INTB"
+	default 12
+config IRQ_MEM_DMA0
+	int "IRQ_MEM_DMA0"
+	default 13
+config IRQ_MEM_DMA1
+	int "IRQ_MEM_DMA1"
+	default 13
+config IRQ_WATCH
+	int "IRQ_WATCH"
+	default 13
+config IRQ_PORTF_INTA
+	int "IRQ_PORTF_INTA"
+	default 13
+config IRQ_PORTF_INTB
+	int "IRQ_PORTF_INTB"
+	default 13
+config IRQ_SPI0_ERROR
+	int "IRQ_SPI0_ERROR"
+	default 7
+config IRQ_SPI1_ERROR
+	int "IRQ_SPI1_ERROR"
+	default 7
+config IRQ_RSI_INT0
+	int "IRQ_RSI_INT0"
+	default 7
+config IRQ_RSI_INT1
+	int "IRQ_RSI_INT1"
+	default 7
+config IRQ_PWM_TRIP
+	int "IRQ_PWM_TRIP"
+	default 10
+config IRQ_PWM_SYNC
+	int "IRQ_PWM_SYNC"
+	default 10
+config IRQ_PTP_STAT
+	int "IRQ_PTP_STAT"
+	default 10
+
+	help
+	  Enter the priority numbers between 7-13 ONLY.  Others are Reserved.
+	  This applies to all the above.  It is not recommended to assign the
+	  highest priority number 7 to UART or any other device.
+
+endmenu
+
+endmenu
+
+endif
diff --git a/arch/blackfin/mach-bf518/Makefile b/arch/blackfin/mach-bf518/Makefile
new file mode 100644
index 000000000000..168a193f9f9a
--- /dev/null
+++ b/arch/blackfin/mach-bf518/Makefile
@@ -0,0 +1,5 @@
+#
+# arch/blackfin/mach-bf518/Makefile
+#
+
+obj-y := ints-priority.o dma.o
diff --git a/arch/blackfin/mach-bf518/boards/Kconfig b/arch/blackfin/mach-bf518/boards/Kconfig
new file mode 100644
index 000000000000..96163514ed22
--- /dev/null
+++ b/arch/blackfin/mach-bf518/boards/Kconfig
@@ -0,0 +1,12 @@
+choice
+	prompt "System type"
+	default BFIN518F_EZBRD
+	help
+	  Select your board!
+
+config BFIN518F_EZBRD
+	bool "BF518F-EZBRD"
+	help
+	  BF518-EZBRD board support.
+
+endchoice
diff --git a/arch/blackfin/mach-bf518/boards/Makefile b/arch/blackfin/mach-bf518/boards/Makefile
new file mode 100644
index 000000000000..172e859c3a7f
--- /dev/null
+++ b/arch/blackfin/mach-bf518/boards/Makefile
@@ -0,0 +1,5 @@
+#
+# arch/blackfin/mach-bf518/boards/Makefile
+#
+
+obj-$(CONFIG_BFIN518F_EZBRD)            += ezbrd.o
diff --git a/arch/blackfin/mach-bf518/boards/ezbrd.c b/arch/blackfin/mach-bf518/boards/ezbrd.c
new file mode 100644
index 000000000000..15f1351c8645
--- /dev/null
+++ b/arch/blackfin/mach-bf518/boards/ezbrd.c
@@ -0,0 +1,669 @@
+/*
+ * File:         arch/blackfin/mach-bf518/boards/ezbrd.c
+ * Based on:     arch/blackfin/mach-bf527/boards/ezbrd.c
+ * Author:       Bryan Wu <cooloney@kernel.org>
+ *
+ * Created:
+ * Description:
+ *
+ * Modified:
+ *               Copyright 2005 National ICT Australia (NICTA)
+ *               Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+#include <linux/mtd/physmap.h>
+#include <linux/spi/spi.h>
+#include <linux/spi/flash.h>
+
+#include <linux/i2c.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <asm/dma.h>
+#include <asm/bfin5xx_spi.h>
+#include <asm/reboot.h>
+#include <asm/portmux.h>
+#include <asm/dpmc.h>
+#include <asm/bfin_sdh.h>
+#include <linux/spi/ad7877.h>
+
+/*
+ * Name the Board for the /proc/cpuinfo
+ */
+const char bfin_board_name[] = "ADI BF518F-EZBRD";
+
+/*
+ *  Driver needs to know address, irq and flag pin.
+ */
+
+#if defined(CONFIG_MTD_PHYSMAP) || defined(CONFIG_MTD_PHYSMAP_MODULE)
+static struct mtd_partition ezbrd_partitions[] = {
+	{
+		.name       = "bootloader(nor)",
+		.size       = 0x40000,
+		.offset     = 0,
+	}, {
+		.name       = "linux kernel(nor)",
+		.size       = 0x1C0000,
+		.offset     = MTDPART_OFS_APPEND,
+	}, {
+		.name       = "file system(nor)",
+		.size       = MTDPART_SIZ_FULL,
+		.offset     = MTDPART_OFS_APPEND,
+	}
+};
+
+static struct physmap_flash_data ezbrd_flash_data = {
+	.width      = 2,
+	.parts      = ezbrd_partitions,
+	.nr_parts   = ARRAY_SIZE(ezbrd_partitions),
+};
+
+static struct resource ezbrd_flash_resource = {
+	.start = 0x20000000,
+	.end   = 0x203fffff,
+	.flags = IORESOURCE_MEM,
+};
+
+static struct platform_device ezbrd_flash_device = {
+	.name          = "physmap-flash",
+	.id            = 0,
+	.dev = {
+		.platform_data = &ezbrd_flash_data,
+	},
+	.num_resources = 1,
+	.resource      = &ezbrd_flash_resource,
+};
+#endif
+
+#if defined(CONFIG_RTC_DRV_BFIN) || defined(CONFIG_RTC_DRV_BFIN_MODULE)
+static struct platform_device rtc_device = {
+	.name = "rtc-bfin",
+	.id   = -1,
+};
+#endif
+
+#if defined(CONFIG_BFIN_MAC) || defined(CONFIG_BFIN_MAC_MODULE)
+static struct platform_device bfin_mac_device = {
+	.name = "bfin_mac",
+};
+#endif
+
+#if defined(CONFIG_MTD_M25P80) \
+	|| defined(CONFIG_MTD_M25P80_MODULE)
+static struct mtd_partition bfin_spi_flash_partitions[] = {
+	{
+		.name = "bootloader(spi)",
+		.size = 0x00040000,
+		.offset = 0,
+		.mask_flags = MTD_CAP_ROM
+	}, {
+		.name = "linux kernel(spi)",
+		.size = MTDPART_SIZ_FULL,
+		.offset = MTDPART_OFS_APPEND,
+	}
+};
+
+static struct flash_platform_data bfin_spi_flash_data = {
+	.name = "m25p80",
+	.parts = bfin_spi_flash_partitions,
+	.nr_parts = ARRAY_SIZE(bfin_spi_flash_partitions),
+	.type = "m25p16",
+};
+
+/* SPI flash chip (m25p64) */
+static struct bfin5xx_spi_chip spi_flash_chip_info = {
+	.enable_dma = 0,         /* use dma transfer with this chip*/
+	.bits_per_word = 8,
+};
+#endif
+
+#if defined(CONFIG_SPI_ADC_BF533) \
+	|| defined(CONFIG_SPI_ADC_BF533_MODULE)
+/* SPI ADC chip */
+static struct bfin5xx_spi_chip spi_adc_chip_info = {
+	.enable_dma = 1,         /* use dma transfer with this chip*/
+	.bits_per_word = 16,
+};
+#endif
+
+#if defined(CONFIG_SPI_MMC) || defined(CONFIG_SPI_MMC_MODULE)
+static struct bfin5xx_spi_chip spi_mmc_chip_info = {
+	.enable_dma = 1,
+	.bits_per_word = 8,
+};
+#endif
+
+#if defined(CONFIG_PBX)
+static struct bfin5xx_spi_chip spi_si3xxx_chip_info = {
+	.ctl_reg	= 0x4, /* send zero */
+	.enable_dma	= 0,
+	.bits_per_word	= 8,
+	.cs_change_per_word = 1,
+};
+#endif
+
+#if defined(CONFIG_TOUCHSCREEN_AD7877) || defined(CONFIG_TOUCHSCREEN_AD7877_MODULE)
+static struct bfin5xx_spi_chip spi_ad7877_chip_info = {
+	.enable_dma = 0,
+	.bits_per_word = 16,
+};
+
+static const struct ad7877_platform_data bfin_ad7877_ts_info = {
+	.model			= 7877,
+	.vref_delay_usecs	= 50,	/* internal, no capacitor */
+	.x_plate_ohms		= 419,
+	.y_plate_ohms		= 486,
+	.pressure_max		= 1000,
+	.pressure_min		= 0,
+	.stopacq_polarity 	= 1,
+	.first_conversion_delay = 3,
+	.acquisition_time 	= 1,
+	.averaging 		= 1,
+	.pen_down_acc_interval 	= 1,
+};
+#endif
+
+#if defined(CONFIG_SND_SOC_WM8731) || defined(CONFIG_SND_SOC_WM8731_MODULE) \
+	 && defined(CONFIG_SND_SOC_WM8731_SPI)
+static struct bfin5xx_spi_chip spi_wm8731_chip_info = {
+	.enable_dma = 0,
+	.bits_per_word = 16,
+};
+#endif
+
+#if defined(CONFIG_SPI_SPIDEV) || defined(CONFIG_SPI_SPIDEV_MODULE)
+static struct bfin5xx_spi_chip spidev_chip_info = {
+	.enable_dma = 0,
+	.bits_per_word = 8,
+};
+#endif
+
+static struct spi_board_info bfin_spi_board_info[] __initdata = {
+#if defined(CONFIG_MTD_M25P80) \
+	|| defined(CONFIG_MTD_M25P80_MODULE)
+	{
+		/* the modalias must be the same as spi device driver name */
+		.modalias = "m25p80", /* Name of spi_driver for this device */
+		.max_speed_hz = 25000000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num = 0, /* Framework bus number */
+		.chip_select = 1, /* Framework chip select. On STAMP537 it is SPISSEL1*/
+		.platform_data = &bfin_spi_flash_data,
+		.controller_data = &spi_flash_chip_info,
+		.mode = SPI_MODE_3,
+	},
+#endif
+
+#if defined(CONFIG_SPI_ADC_BF533) \
+	|| defined(CONFIG_SPI_ADC_BF533_MODULE)
+	{
+		.modalias = "bfin_spi_adc", /* Name of spi_driver for this device */
+		.max_speed_hz = 6250000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num = 0, /* Framework bus number */
+		.chip_select = 1, /* Framework chip select. */
+		.platform_data = NULL, /* No spi_driver specific config */
+		.controller_data = &spi_adc_chip_info,
+	},
+#endif
+
+#if defined(CONFIG_SPI_MMC) || defined(CONFIG_SPI_MMC_MODULE)
+	{
+		.modalias = "spi_mmc_dummy",
+		.max_speed_hz = 25000000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num = 0,
+		.chip_select = 0,
+		.platform_data = NULL,
+		.controller_data = &spi_mmc_chip_info,
+		.mode = SPI_MODE_3,
+	},
+	{
+		.modalias = "spi_mmc",
+		.max_speed_hz = 25000000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num = 0,
+		.chip_select = CONFIG_SPI_MMC_CS_CHAN,
+		.platform_data = NULL,
+		.controller_data = &spi_mmc_chip_info,
+		.mode = SPI_MODE_3,
+	},
+#endif
+#if defined(CONFIG_PBX)
+	{
+		.modalias = "fxs-spi",
+		.max_speed_hz = 12500000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num = 0,
+		.chip_select = 8 - CONFIG_J11_JUMPER,
+		.controller_data = &spi_si3xxx_chip_info,
+		.mode = SPI_MODE_3,
+	},
+	{
+		.modalias = "fxo-spi",
+		.max_speed_hz = 12500000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num = 0,
+		.chip_select = 8 - CONFIG_J19_JUMPER,
+		.controller_data = &spi_si3xxx_chip_info,
+		.mode = SPI_MODE_3,
+	},
+#endif
+#if defined(CONFIG_TOUCHSCREEN_AD7877) || defined(CONFIG_TOUCHSCREEN_AD7877_MODULE)
+	{
+		.modalias		= "ad7877",
+		.platform_data		= &bfin_ad7877_ts_info,
+		.irq			= IRQ_PF8,
+		.max_speed_hz	= 12500000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num	= 0,
+		.chip_select  = 2,
+		.controller_data = &spi_ad7877_chip_info,
+	},
+#endif
+#if defined(CONFIG_SND_SOC_WM8731) || defined(CONFIG_SND_SOC_WM8731_MODULE) \
+	 && defined(CONFIG_SND_SOC_WM8731_SPI)
+	{
+		.modalias	= "wm8731",
+		.max_speed_hz	= 3125000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num	= 0,
+		.chip_select    = 5,
+		.controller_data = &spi_wm8731_chip_info,
+		.mode = SPI_MODE_0,
+	},
+#endif
+#if defined(CONFIG_SPI_SPIDEV) || defined(CONFIG_SPI_SPIDEV_MODULE)
+	{
+		.modalias = "spidev",
+		.max_speed_hz = 3125000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num = 0,
+		.chip_select = 1,
+		.controller_data = &spidev_chip_info,
+	},
+#endif
+#if defined(CONFIG_FB_BFIN_LQ035Q1) || defined(CONFIG_FB_BFIN_LQ035Q1_MODULE)
+	{
+		.modalias = "bfin-lq035q1-spi",
+		.max_speed_hz = 20000000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num = 0,
+		.chip_select = 1,
+		.controller_data = &lq035q1_spi_chip_info,
+		.mode = SPI_CPHA | SPI_CPOL,
+	},
+#endif
+};
+
+/* SPI controller data */
+#if defined(CONFIG_SPI_BFIN) || defined(CONFIG_SPI_BFIN_MODULE)
+/* SPI (0) */
+static struct bfin5xx_spi_master bfin_spi0_info = {
+	.num_chipselect = 5,
+	.enable_dma = 1,  /* master has the ability to do dma transfer */
+	.pin_req = {P_SPI0_SCK, P_SPI0_MISO, P_SPI0_MOSI, 0},
+};
+
+static struct resource bfin_spi0_resource[] = {
+	[0] = {
+		.start = SPI0_REGBASE,
+		.end   = SPI0_REGBASE + 0xFF,
+		.flags = IORESOURCE_MEM,
+		},
+	[1] = {
+		.start = CH_SPI0,
+		.end   = CH_SPI0,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static struct platform_device bfin_spi0_device = {
+	.name = "bfin-spi",
+	.id = 0, /* Bus number */
+	.num_resources = ARRAY_SIZE(bfin_spi0_resource),
+	.resource = bfin_spi0_resource,
+	.dev = {
+		.platform_data = &bfin_spi0_info, /* Passed to driver */
+	},
+};
+
+/* SPI (1) */
+static struct bfin5xx_spi_master bfin_spi1_info = {
+	.num_chipselect = 5,
+	.enable_dma = 1,  /* master has the ability to do dma transfer */
+	.pin_req = {P_SPI1_SCK, P_SPI1_MISO, P_SPI1_MOSI, 0},
+};
+
+static struct resource bfin_spi1_resource[] = {
+	[0] = {
+		.start = SPI1_REGBASE,
+		.end   = SPI1_REGBASE + 0xFF,
+		.flags = IORESOURCE_MEM,
+		},
+	[1] = {
+		.start = CH_SPI1,
+		.end   = CH_SPI1,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static struct platform_device bfin_spi1_device = {
+	.name = "bfin-spi",
+	.id = 1, /* Bus number */
+	.num_resources = ARRAY_SIZE(bfin_spi1_resource),
+	.resource = bfin_spi1_resource,
+	.dev = {
+		.platform_data = &bfin_spi1_info, /* Passed to driver */
+	},
+};
+#endif  /* spi master and devices */
+
+#if defined(CONFIG_SERIAL_BFIN) || defined(CONFIG_SERIAL_BFIN_MODULE)
+static struct resource bfin_uart_resources[] = {
+#ifdef CONFIG_SERIAL_BFIN_UART0
+	{
+		.start = 0xFFC00400,
+		.end = 0xFFC004FF,
+		.flags = IORESOURCE_MEM,
+	},
+#endif
+#ifdef CONFIG_SERIAL_BFIN_UART1
+	{
+		.start = 0xFFC02000,
+		.end = 0xFFC020FF,
+		.flags = IORESOURCE_MEM,
+	},
+#endif
+};
+
+static struct platform_device bfin_uart_device = {
+	.name = "bfin-uart",
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_uart_resources),
+	.resource = bfin_uart_resources,
+};
+#endif
+
+#if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
+#ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
+	{
+		.start = 0xFFC00400,
+		.end = 0xFFC004FF,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
+#endif
+#ifdef CONFIG_BFIN_SIR1
+static struct resource bfin_sir1_resources[] = {
+	{
+		.start = 0xFFC02000,
+		.end = 0xFFC020FF,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = IRQ_UART1_RX,
+		.end = IRQ_UART1_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART1_RX,
+		.end = CH_UART1_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
+static struct platform_device bfin_sir1_device = {
+	.name = "bfin_sir",
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_sir1_resources),
+	.resource = bfin_sir1_resources,
+};
+#endif
+#endif
+
+#if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
+static struct resource bfin_twi0_resource[] = {
+	[0] = {
+		.start = TWI0_REGBASE,
+		.end   = TWI0_REGBASE,
+		.flags = IORESOURCE_MEM,
+	},
+	[1] = {
+		.start = IRQ_TWI,
+		.end   = IRQ_TWI,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static struct platform_device i2c_bfin_twi_device = {
+	.name = "i2c-bfin-twi",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_twi0_resource),
+	.resource = bfin_twi0_resource,
+};
+#endif
+
+#ifdef CONFIG_I2C_BOARDINFO
+static struct i2c_board_info __initdata bfin_i2c_board_info[] = {
+#if defined(CONFIG_TWI_LCD) || defined(CONFIG_TWI_LCD_MODULE)
+	{
+		I2C_BOARD_INFO("pcf8574_lcd", 0x22),
+	},
+#endif
+#if defined(CONFIG_TWI_KEYPAD) || defined(CONFIG_TWI_KEYPAD_MODULE)
+	{
+		I2C_BOARD_INFO("pcf8574_keypad", 0x27),
+		.irq = IRQ_PF8,
+	},
+#endif
+};
+#endif
+
+#if defined(CONFIG_SERIAL_BFIN_SPORT) || defined(CONFIG_SERIAL_BFIN_SPORT_MODULE)
+static struct platform_device bfin_sport0_uart_device = {
+	.name = "bfin-sport-uart",
+	.id = 0,
+};
+
+static struct platform_device bfin_sport1_uart_device = {
+	.name = "bfin-sport-uart",
+	.id = 1,
+};
+#endif
+
+#if defined(CONFIG_KEYBOARD_GPIO) || defined(CONFIG_KEYBOARD_GPIO_MODULE)
+#include <linux/input.h>
+#include <linux/gpio_keys.h>
+
+static struct gpio_keys_button bfin_gpio_keys_table[] = {
+	{BTN_0, GPIO_PG0, 1, "gpio-keys: BTN0"},
+	{BTN_1, GPIO_PG13, 1, "gpio-keys: BTN1"},
+};
+
+static struct gpio_keys_platform_data bfin_gpio_keys_data = {
+	.buttons        = bfin_gpio_keys_table,
+	.nbuttons       = ARRAY_SIZE(bfin_gpio_keys_table),
+};
+
+static struct platform_device bfin_device_gpiokeys = {
+	.name      = "gpio-keys",
+	.dev = {
+		.platform_data = &bfin_gpio_keys_data,
+	},
+};
+#endif
+
+#if defined(CONFIG_SDH_BFIN) || defined(CONFIG_SDH_BFIN_MODULE)
+
+static struct bfin_sd_host bfin_sdh_data = {
+	.dma_chan = CH_RSI,
+	.irq_int0 = IRQ_RSI_INT0,
+	.pin_req = {P_RSI_DATA0, P_RSI_DATA1, P_RSI_DATA2, P_RSI_DATA3, P_RSI_CMD, P_RSI_CLK, 0},
+};
+
+static struct platform_device bf51x_sdh_device = {
+	.name = "bfin-sdh",
+	.id = 0,
+	.dev = {
+		.platform_data = &bfin_sdh_data,
+	},
+};
+#endif
+
+static struct resource bfin_gpios_resources = {
+	.start = 0,
+	.end   = MAX_BLACKFIN_GPIOS - 1,
+	.flags = IORESOURCE_IRQ,
+};
+
+static struct platform_device bfin_gpios_device = {
+	.name = "simple-gpio",
+	.id = -1,
+	.num_resources = 1,
+	.resource = &bfin_gpios_resources,
+};
+
+static const unsigned int cclk_vlev_datasheet[] =
+{
+	VRPAIR(VLEV_100, 400000000),
+	VRPAIR(VLEV_105, 426000000),
+	VRPAIR(VLEV_110, 500000000),
+	VRPAIR(VLEV_115, 533000000),
+	VRPAIR(VLEV_120, 600000000),
+};
+
+static struct bfin_dpmc_platform_data bfin_dmpc_vreg_data = {
+	.tuple_tab = cclk_vlev_datasheet,
+	.tabsize = ARRAY_SIZE(cclk_vlev_datasheet),
+	.vr_settling_time = 25 /* us */,
+};
+
+static struct platform_device bfin_dpmc = {
+	.name = "bfin dpmc",
+	.dev = {
+		.platform_data = &bfin_dmpc_vreg_data,
+	},
+};
+
+static struct platform_device *stamp_devices[] __initdata = {
+
+	&bfin_dpmc,
+
+#if defined(CONFIG_RTC_DRV_BFIN) || defined(CONFIG_RTC_DRV_BFIN_MODULE)
+	&rtc_device,
+#endif
+
+#if defined(CONFIG_BFIN_MAC) || defined(CONFIG_BFIN_MAC_MODULE)
+	&bfin_mac_device,
+#endif
+
+#if defined(CONFIG_SPI_BFIN) || defined(CONFIG_SPI_BFIN_MODULE)
+	&bfin_spi0_device,
+	&bfin_spi1_device,
+#endif
+
+#if defined(CONFIG_SERIAL_BFIN) || defined(CONFIG_SERIAL_BFIN_MODULE)
+	&bfin_uart_device,
+#endif
+
+#if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#ifdef CONFIG_BFIN_SIR1
+	&bfin_sir1_device,
+#endif
+#endif
+
+#if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
+	&i2c_bfin_twi_device,
+#endif
+
+#if defined(CONFIG_SERIAL_BFIN_SPORT) || defined(CONFIG_SERIAL_BFIN_SPORT_MODULE)
+	&bfin_sport0_uart_device,
+	&bfin_sport1_uart_device,
+#endif
+
+#if defined(CONFIG_KEYBOARD_GPIO) || defined(CONFIG_KEYBOARD_GPIO_MODULE)
+	&bfin_device_gpiokeys,
+#endif
+
+#if defined(CONFIG_SDH_BFIN) || defined(CONFIG_SDH_BFIN_MODULE)
+	&bf51x_sdh_device,
+#endif
+
+#if defined(CONFIG_MTD_PHYSMAP) || defined(CONFIG_MTD_PHYSMAP_MODULE)
+	&ezbrd_flash_device,
+#endif
+
+	&bfin_gpios_device,
+};
+
+static int __init ezbrd_init(void)
+{
+	printk(KERN_INFO "%s(): registering device resources\n", __func__);
+
+#ifdef CONFIG_I2C_BOARDINFO
+	i2c_register_board_info(0, bfin_i2c_board_info,
+				ARRAY_SIZE(bfin_i2c_board_info));
+#endif
+
+	platform_add_devices(stamp_devices, ARRAY_SIZE(stamp_devices));
+	spi_register_board_info(bfin_spi_board_info, ARRAY_SIZE(bfin_spi_board_info));
+	return 0;
+}
+
+arch_initcall(ezbrd_init);
+
+void native_machine_restart(char *cmd)
+{
+	/* workaround reboot hang when booting from SPI */
+	if ((bfin_read_SYSCR() & 0x7) == 0x3)
+		bfin_gpio_reset_spi0_ssel1();
+}
+
+void bfin_get_ether_addr(char *addr)
+{
+	/* the MAC is stored in OTP memory page 0xDF */
+	u32 ret;
+	u64 otp_mac;
+	u32 (*otp_read)(u32 page, u32 flags, u64 *page_content) = (void *)0xEF00001A;
+
+	ret = otp_read(0xDF, 0x00, &otp_mac);
+	if (!(ret & 0x1)) {
+		char *otp_mac_p = (char *)&otp_mac;
+		for (ret = 0; ret < 6; ++ret)
+			addr[ret] = otp_mac_p[5 - ret];
+	}
+}
+EXPORT_SYMBOL(bfin_get_ether_addr);
diff --git a/arch/blackfin/mach-bf518/dma.c b/arch/blackfin/mach-bf518/dma.c
new file mode 100644
index 000000000000..698e88ca5104
--- /dev/null
+++ b/arch/blackfin/mach-bf518/dma.c
@@ -0,0 +1,118 @@
+/*
+ * File:         arch/blackfin/mach-bf518/dma.c
+ * Based on:
+ * Author:       Bryan Wu <cooloney@kernel.org>
+ *
+ * Created:
+ * Description:  This file contains the simple DMA Implementation for Blackfin
+ *
+ * Modified:
+ *               Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+#include <linux/module.h>
+
+#include <asm/blackfin.h>
+#include <asm/dma.h>
+
+struct dma_register *dma_io_base_addr[MAX_DMA_CHANNELS] = {
+	(struct dma_register *) DMA0_NEXT_DESC_PTR,
+	(struct dma_register *) DMA1_NEXT_DESC_PTR,
+	(struct dma_register *) DMA2_NEXT_DESC_PTR,
+	(struct dma_register *) DMA3_NEXT_DESC_PTR,
+	(struct dma_register *) DMA4_NEXT_DESC_PTR,
+	(struct dma_register *) DMA5_NEXT_DESC_PTR,
+	(struct dma_register *) DMA6_NEXT_DESC_PTR,
+	(struct dma_register *) DMA7_NEXT_DESC_PTR,
+	(struct dma_register *) DMA8_NEXT_DESC_PTR,
+	(struct dma_register *) DMA9_NEXT_DESC_PTR,
+	(struct dma_register *) DMA10_NEXT_DESC_PTR,
+	(struct dma_register *) DMA11_NEXT_DESC_PTR,
+	(struct dma_register *) MDMA_S0_NEXT_DESC_PTR,
+	(struct dma_register *) MDMA_D0_NEXT_DESC_PTR,
+	(struct dma_register *) MDMA_S1_NEXT_DESC_PTR,
+	(struct dma_register *) MDMA_D1_NEXT_DESC_PTR,
+};
+EXPORT_SYMBOL(dma_io_base_addr);
+
+int channel2irq(unsigned int channel)
+{
+	int ret_irq = -1;
+
+	switch (channel) {
+	case CH_PPI:
+		ret_irq = IRQ_PPI;
+		break;
+
+	case CH_EMAC_RX:
+		ret_irq = IRQ_MAC_RX;
+		break;
+
+	case CH_EMAC_TX:
+		ret_irq = IRQ_MAC_TX;
+		break;
+
+	case CH_UART1_RX:
+		ret_irq = IRQ_UART1_RX;
+		break;
+
+	case CH_UART1_TX:
+		ret_irq = IRQ_UART1_TX;
+		break;
+
+	case CH_SPORT0_RX:
+		ret_irq = IRQ_SPORT0_RX;
+		break;
+
+	case CH_SPORT0_TX:
+		ret_irq = IRQ_SPORT0_TX;
+		break;
+
+	case CH_SPORT1_RX:
+		ret_irq = IRQ_SPORT1_RX;
+		break;
+
+	case CH_SPORT1_TX:
+		ret_irq = IRQ_SPORT1_TX;
+		break;
+
+	case CH_SPI0:
+		ret_irq = IRQ_SPI0;
+		break;
+
+	case CH_UART0_RX:
+		ret_irq = IRQ_UART0_RX;
+		break;
+
+	case CH_UART0_TX:
+		ret_irq = IRQ_UART0_TX;
+		break;
+
+	case CH_MEM_STREAM0_SRC:
+	case CH_MEM_STREAM0_DEST:
+		ret_irq = IRQ_MEM_DMA0;
+		break;
+
+	case CH_MEM_STREAM1_SRC:
+	case CH_MEM_STREAM1_DEST:
+		ret_irq = IRQ_MEM_DMA1;
+		break;
+	}
+	return ret_irq;
+}
diff --git a/arch/blackfin/mach-bf518/include/mach/anomaly.h b/arch/blackfin/mach-bf518/include/mach/anomaly.h
new file mode 100644
index 000000000000..e5b4bef0edae
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/anomaly.h
@@ -0,0 +1,79 @@
+/*
+ * File: include/asm-blackfin/mach-bf518/anomaly.h
+ * Bugs: Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Copyright (C) 2004-2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
+ */
+
+/* This file shoule be up to date with:
+ *  - ????
+ */
+
+#ifndef _MACH_ANOMALY_H_
+#define _MACH_ANOMALY_H_
+
+/* Multi-Issue Instruction with dsp32shiftimm in slot1 and P-reg Store in slot2 Not Supported */
+#define ANOMALY_05000074 (1)
+/* Rx.H Cannot Be Used to Access 16-bit System MMR Registers */
+#define ANOMALY_05000122 (1)
+/* False Hardware Error from an Access in the Shadow of a Conditional Branch */
+#define ANOMALY_05000245 (1)
+/* Sensitivity To Noise with Slow Input Edge Rates on External SPORT TX and RX Clocks */
+#define ANOMALY_05000265 (1)
+/* False Hardware Errors Caused by Fetches at the Boundary of Reserved Memory */
+#define ANOMALY_05000310 (1)
+/* PPI Underflow Error Goes Undetected in ITU-R 656 Mode */
+#define ANOMALY_05000366 (1)
+/* Lockbox SESR Firmware Does Not Save/Restore Full Context */
+#define ANOMALY_05000405 (1)
+/* Lockbox Firmware Memory Cleanup Routine Does not Clear Registers */
+#define ANOMALY_05000408 (1)
+/* Speculative Fetches Can Cause Undesired External FIFO Operations */
+#define ANOMALY_05000416 (1)
+/* TWI Fall Time (Tof) May Violate the Minimum I2C Specification */
+#define ANOMALY_05000421 (1)
+/* TWI Input Capacitance (Ci) May Violate the Maximum I2C Specification */
+#define ANOMALY_05000422 (1)
+/* Speculative Fetches of Indirect-Pointer Instructions Can Cause False Hardware Errors */
+#define ANOMALY_05000426 (1)
+/* Software System Reset Corrupts PLL_LOCKCNT Register */
+#define ANOMALY_05000430 (1)
+/* Incorrect Use of Stack in Lockbox Firmware During Authentication */
+#define ANOMALY_05000431 (1)
+/* Certain SIC Registers are not Reset After Soft or Core Double Fault Reset */
+#define ANOMALY_05000435 (1)
+/* PORTx_DRIVE and PORTx_HYSTERESIS Registers Read Back Incorrect Values */
+#define ANOMALY_05000438 (1)
+/* Preboot Cannot be Used to Program the PLL_DIV Register */
+#define ANOMALY_05000439 (1)
+/* bfrom_SysControl() Cannot be Used to Write the PLL_DIV Register */
+#define ANOMALY_05000440 (1)
+/* IFLUSH Instruction at End of Hardware Loop Causes Infinite Stall */
+#define ANOMALY_05000443 (1)
+/* Incorrect L1 Instruction Bank B Memory Map Location */
+#define ANOMALY_05000444 (1)
+
+/* Anomalies that don't exist on this proc */
+#define ANOMALY_05000125 (0)
+#define ANOMALY_05000158 (0)
+#define ANOMALY_05000183 (0)
+#define ANOMALY_05000198 (0)
+#define ANOMALY_05000230 (0)
+#define ANOMALY_05000244 (0)
+#define ANOMALY_05000261 (0)
+#define ANOMALY_05000263 (0)
+#define ANOMALY_05000266 (0)
+#define ANOMALY_05000273 (0)
+#define ANOMALY_05000285 (0)
+#define ANOMALY_05000307 (0)
+#define ANOMALY_05000311 (0)
+#define ANOMALY_05000312 (0)
+#define ANOMALY_05000323 (0)
+#define ANOMALY_05000353 (0)
+#define ANOMALY_05000363 (0)
+#define ANOMALY_05000386 (0)
+#define ANOMALY_05000412 (0)
+#define ANOMALY_05000432 (0)
+
+#endif
diff --git a/arch/blackfin/mach-bf518/include/mach/bf518.h b/arch/blackfin/mach-bf518/include/mach/bf518.h
new file mode 100644
index 000000000000..78da1a07ee73
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/bf518.h
@@ -0,0 +1,132 @@
+/*
+ * File:         include/asm-blackfin/mach-bf518/bf518.h
+ * Based on:	include/asm-blackfin/mach-bf527/bf527.h
+ * Author:	Michael Hennerich (michael.hennerich@analog.com)
+ *
+ * Created:
+ * Description:  SYSTEM MMR REGISTER AND MEMORY MAP FOR ADSP-BF518
+ *
+ * Modified:
+ *               Copyright 2004-2007 Analog Devices Inc.
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#ifndef __MACH_BF518_H__
+#define __MACH_BF518_H__
+
+#define OFFSET_(x) ((x) & 0x0000FFFF)
+
+/*some misc defines*/
+#define IMASK_IVG15		0x8000
+#define IMASK_IVG14		0x4000
+#define IMASK_IVG13		0x2000
+#define IMASK_IVG12		0x1000
+
+#define IMASK_IVG11		0x0800
+#define IMASK_IVG10		0x0400
+#define IMASK_IVG9		0x0200
+#define IMASK_IVG8		0x0100
+
+#define IMASK_IVG7		0x0080
+#define IMASK_IVGTMR		0x0040
+#define IMASK_IVGHW		0x0020
+
+/***************************/
+
+#define BFIN_DSUBBANKS	4
+#define BFIN_DWAYS		2
+#define BFIN_DLINES		64
+#define BFIN_ISUBBANKS	4
+#define BFIN_IWAYS		4
+#define BFIN_ILINES		32
+
+#define WAY0_L			0x1
+#define WAY1_L			0x2
+#define WAY01_L			0x3
+#define WAY2_L			0x4
+#define WAY02_L			0x5
+#define	WAY12_L			0x6
+#define	WAY012_L		0x7
+
+#define	WAY3_L			0x8
+#define	WAY03_L			0x9
+#define	WAY13_L			0xA
+#define	WAY013_L		0xB
+
+#define	WAY32_L			0xC
+#define	WAY320_L		0xD
+#define	WAY321_L		0xE
+#define	WAYALL_L		0xF
+
+#define DMC_ENABLE (2<<2)	/*yes, 2, not 1 */
+
+/********************************* EBIU Settings ************************************/
+#define AMBCTL0VAL	((CONFIG_BANK_1 << 16) | CONFIG_BANK_0)
+#define AMBCTL1VAL	((CONFIG_BANK_3 << 16) | CONFIG_BANK_2)
+
+#ifdef CONFIG_C_AMBEN_ALL
+#define V_AMBEN AMBEN_ALL
+#endif
+#ifdef CONFIG_C_AMBEN
+#define V_AMBEN 0x0
+#endif
+#ifdef CONFIG_C_AMBEN_B0
+#define V_AMBEN AMBEN_B0
+#endif
+#ifdef CONFIG_C_AMBEN_B0_B1
+#define V_AMBEN AMBEN_B0_B1
+#endif
+#ifdef CONFIG_C_AMBEN_B0_B1_B2
+#define V_AMBEN AMBEN_B0_B1_B2
+#endif
+#ifdef CONFIG_C_AMCKEN
+#define V_AMCKEN AMCKEN
+#else
+#define V_AMCKEN 0x0
+#endif
+#ifdef CONFIG_C_CDPRIO
+#define V_CDPRIO 0x100
+#else
+#define V_CDPRIO 0x0
+#endif
+
+#define AMGCTLVAL	(V_AMBEN | V_AMCKEN | V_CDPRIO)
+
+#ifdef CONFIG_BF518
+#define CPU "BF518"
+#define CPUID 0x27e8
+#endif
+#ifdef CONFIG_BF516
+#define CPU "BF516"
+#define CPUID 0x27e8
+#endif
+#ifdef CONFIG_BF514
+#define CPU "BF514"
+#define CPUID 0x27e8
+#endif
+#ifdef CONFIG_BF512
+#define CPU "BF512"
+#define CPUID 0x27e8
+#endif
+
+#ifndef CPU
+#error "Unknown CPU type - This kernel doesn't seem to be configured properly"
+#endif
+
+#endif				/* __MACH_BF518_H__  */
diff --git a/arch/blackfin/mach-bf518/include/mach/bfin_serial_5xx.h b/arch/blackfin/mach-bf518/include/mach/bfin_serial_5xx.h
new file mode 100644
index 000000000000..b50a63b975a2
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/bfin_serial_5xx.h
@@ -0,0 +1,169 @@
+/*
+ * file:        include/asm-blackfin/mach-bf518/bfin_serial_5xx.h
+ * based on:
+ * author:
+ *
+ * created:
+ * description:
+ *	blackfin serial driver head file
+ * rev:
+ *
+ * modified:
+ *
+ *
+ * bugs:         enter bugs at http://blackfin.uclinux.org/
+ *
+ * this program is free software; you can redistribute it and/or modify
+ * it under the terms of the gnu general public license as published by
+ * the free software foundation; either version 2, 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; see the file copying.
+ * if not, write to the free software foundation,
+ * 59 temple place - suite 330, boston, ma 02111-1307, usa.
+ */
+
+#include <linux/serial.h>
+#include <asm/dma.h>
+#include <asm/portmux.h>
+
+#define UART_GET_CHAR(uart)     bfin_read16(((uart)->port.membase + OFFSET_RBR))
+#define UART_GET_DLL(uart)	bfin_read16(((uart)->port.membase + OFFSET_DLL))
+#define UART_GET_IER(uart)      bfin_read16(((uart)->port.membase + OFFSET_IER))
+#define UART_GET_DLH(uart)	bfin_read16(((uart)->port.membase + OFFSET_DLH))
+#define UART_GET_IIR(uart)      bfin_read16(((uart)->port.membase + OFFSET_IIR))
+#define UART_GET_LCR(uart)      bfin_read16(((uart)->port.membase + OFFSET_LCR))
+#define UART_GET_GCTL(uart)     bfin_read16(((uart)->port.membase + OFFSET_GCTL))
+
+#define UART_PUT_CHAR(uart, v)   bfin_write16(((uart)->port.membase + OFFSET_THR), v)
+#define UART_PUT_DLL(uart, v)    bfin_write16(((uart)->port.membase + OFFSET_DLL), v)
+#define UART_PUT_IER(uart, v)    bfin_write16(((uart)->port.membase + OFFSET_IER), v)
+#define UART_SET_IER(uart, v)    UART_PUT_IER(uart, UART_GET_IER(uart) | (v))
+#define UART_CLEAR_IER(uart, v)  UART_PUT_IER(uart, UART_GET_IER(uart) & ~(v))
+#define UART_PUT_DLH(uart, v)    bfin_write16(((uart)->port.membase + OFFSET_DLH), v)
+#define UART_PUT_LCR(uart, v)    bfin_write16(((uart)->port.membase + OFFSET_LCR), v)
+#define UART_PUT_GCTL(uart, v)   bfin_write16(((uart)->port.membase + OFFSET_GCTL), v)
+
+#define UART_SET_DLAB(uart)     do { UART_PUT_LCR(uart, UART_GET_LCR(uart) | DLAB); SSYNC(); } while (0)
+#define UART_CLEAR_DLAB(uart)   do { UART_PUT_LCR(uart, UART_GET_LCR(uart) & ~DLAB); SSYNC(); } while (0)
+
+#define UART_GET_CTS(x) gpio_get_value(x->cts_pin)
+#define UART_SET_RTS(x) gpio_set_value(x->rts_pin, 1)
+#define UART_CLEAR_RTS(x) gpio_set_value(x->rts_pin, 0)
+#define UART_ENABLE_INTS(x, v) UART_PUT_IER(x, v)
+#define UART_DISABLE_INTS(x) UART_PUT_IER(x, 0)
+
+#if defined(CONFIG_BFIN_UART0_CTSRTS) || defined(CONFIG_BFIN_UART1_CTSRTS)
+# define CONFIG_SERIAL_BFIN_CTSRTS
+
+# ifndef CONFIG_UART0_CTS_PIN
+#  define CONFIG_UART0_CTS_PIN -1
+# endif
+
+# ifndef CONFIG_UART0_RTS_PIN
+#  define CONFIG_UART0_RTS_PIN -1
+# endif
+
+# ifndef CONFIG_UART1_CTS_PIN
+#  define CONFIG_UART1_CTS_PIN -1
+# endif
+
+# ifndef CONFIG_UART1_RTS_PIN
+#  define CONFIG_UART1_RTS_PIN -1
+# endif
+#endif
+
+#define BFIN_UART_TX_FIFO_SIZE	2
+
+/*
+ * The pin configuration is different from schematic
+ */
+struct bfin_serial_port {
+	struct uart_port port;
+	unsigned int old_status;
+	unsigned int lsr;
+#ifdef CONFIG_SERIAL_BFIN_DMA
+	int tx_done;
+	int tx_count;
+	struct circ_buf rx_dma_buf;
+	struct timer_list rx_dma_timer;
+	int rx_dma_nrows;
+	unsigned int tx_dma_channel;
+	unsigned int rx_dma_channel;
+	struct work_struct tx_dma_workqueue;
+#endif
+#ifdef CONFIG_SERIAL_BFIN_CTSRTS
+	struct timer_list cts_timer;
+	int cts_pin;
+	int rts_pin;
+#endif
+};
+
+/* The hardware clears the LSR bits upon read, so we need to cache
+ * some of the more fun bits in software so they don't get lost
+ * when checking the LSR in other code paths (TX).
+ */
+static inline unsigned int UART_GET_LSR(struct bfin_serial_port *uart)
+{
+	unsigned int lsr = bfin_read16(uart->port.membase + OFFSET_LSR);
+	uart->lsr |= (lsr & (BI|FE|PE|OE));
+	return lsr | uart->lsr;
+}
+
+static inline void UART_CLEAR_LSR(struct bfin_serial_port *uart)
+{
+	uart->lsr = 0;
+	bfin_write16(uart->port.membase + OFFSET_LSR, -1);
+}
+
+struct bfin_serial_res {
+	unsigned long uart_base_addr;
+	int uart_irq;
+#ifdef CONFIG_SERIAL_BFIN_DMA
+	unsigned int uart_tx_dma_channel;
+	unsigned int uart_rx_dma_channel;
+#endif
+#ifdef CONFIG_SERIAL_BFIN_CTSRTS
+	int uart_cts_pin;
+	int uart_rts_pin;
+#endif
+};
+
+struct bfin_serial_res bfin_serial_resource[] = {
+#ifdef CONFIG_SERIAL_BFIN_UART0
+	{
+	 0xFFC00400,
+	 IRQ_UART0_RX,
+#ifdef CONFIG_SERIAL_BFIN_DMA
+	 CH_UART0_TX,
+	 CH_UART0_RX,
+#endif
+#ifdef CONFIG_BFIN_UART0_CTSRTS
+	 CONFIG_UART0_CTS_PIN,
+	 CONFIG_UART0_RTS_PIN,
+#endif
+	 },
+#endif
+#ifdef CONFIG_SERIAL_BFIN_UART1
+	{
+	 0xFFC02000,
+	 IRQ_UART1_RX,
+#ifdef CONFIG_SERIAL_BFIN_DMA
+	 CH_UART1_TX,
+	 CH_UART1_RX,
+#endif
+#ifdef CONFIG_BFIN_UART1_CTSRTS
+	 CONFIG_UART1_CTS_PIN,
+	 CONFIG_UART1_RTS_PIN,
+#endif
+	 },
+#endif
+};
+
+#define DRIVER_NAME "bfin-uart"
diff --git a/arch/blackfin/mach-bf518/include/mach/blackfin.h b/arch/blackfin/mach-bf518/include/mach/blackfin.h
new file mode 100644
index 000000000000..d1a2b9ca6227
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/blackfin.h
@@ -0,0 +1,105 @@
+/*
+ * File:         include/asm-blackfin/mach-bf518/blackfin.h
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:
+ *
+ * Rev:
+ *
+ * Modified:
+ *
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, 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; see the file COPYING.
+ * If not, write to the Free Software Foundation,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef _MACH_BLACKFIN_H_
+#define _MACH_BLACKFIN_H_
+
+#define BF518_FAMILY
+
+#include "bf518.h"
+#include "mem_map.h"
+#include "defBF512.h"
+#include "anomaly.h"
+
+#if defined(CONFIG_BF518)
+#include "defBF518.h"
+#endif
+
+#if defined(CONFIG_BF516)
+#include "defBF516.h"
+#endif
+
+#if defined(CONFIG_BF514)
+#include "defBF514.h"
+#endif
+
+#if defined(CONFIG_BF512)
+#include "defBF512.h"
+#endif
+
+#if !defined(__ASSEMBLY__)
+#include "cdefBF512.h"
+
+#if defined(CONFIG_BF518)
+#include "cdefBF518.h"
+#endif
+
+#if defined(CONFIG_BF516)
+#include "cdefBF516.h"
+#endif
+
+#if defined(CONFIG_BF514)
+#include "cdefBF514.h"
+#endif
+#endif
+
+/* UART_IIR Register */
+#define STATUS(x)	((x << 1) & 0x06)
+#define STATUS_P1	0x02
+#define STATUS_P0	0x01
+
+#define BFIN_UART_NR_PORTS	2
+
+#define OFFSET_THR              0x00	/* Transmit Holding register            */
+#define OFFSET_RBR              0x00	/* Receive Buffer register              */
+#define OFFSET_DLL              0x00	/* Divisor Latch (Low-Byte)             */
+#define OFFSET_IER              0x04	/* Interrupt Enable Register            */
+#define OFFSET_DLH              0x04	/* Divisor Latch (High-Byte)            */
+#define OFFSET_IIR              0x08	/* Interrupt Identification Register    */
+#define OFFSET_LCR              0x0C	/* Line Control Register                */
+#define OFFSET_MCR              0x10	/* Modem Control Register               */
+#define OFFSET_LSR              0x14	/* Line Status Register                 */
+#define OFFSET_MSR              0x18	/* Modem Status Register                */
+#define OFFSET_SCR              0x1C	/* SCR Scratch Register                 */
+#define OFFSET_GCTL             0x24	/* Global Control Register              */
+
+/* DPMC*/
+#define bfin_read_STOPCK_OFF() bfin_read_STOPCK()
+#define bfin_write_STOPCK_OFF(val) bfin_write_STOPCK(val)
+#define STOPCK_OFF STOPCK
+
+/* PLL_DIV Masks													*/
+#define CCLK_DIV1 CSEL_DIV1	/*          CCLK = VCO / 1                                  */
+#define CCLK_DIV2 CSEL_DIV2	/*          CCLK = VCO / 2                                  */
+#define CCLK_DIV4 CSEL_DIV4	/*          CCLK = VCO / 4                                  */
+#define CCLK_DIV8 CSEL_DIV8	/*          CCLK = VCO / 8                                  */
+
+#endif
diff --git a/arch/blackfin/mach-bf518/include/mach/cdefBF512.h b/arch/blackfin/mach-bf518/include/mach/cdefBF512.h
new file mode 100644
index 000000000000..820c13c4daaa
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/cdefBF512.h
@@ -0,0 +1,46 @@
+/*
+ * File:         include/asm-blackfin/mach-bf518/cdefbf512.h
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:  system mmr register map
+ *
+ * Rev:
+ *
+ * Modified:
+ *
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, 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; see the file COPYING.
+ * If not, write to the Free Software Foundation,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef _CDEF_BF512_H
+#define _CDEF_BF512_H
+
+/* include all Core registers and bit definitions */
+#include "defBF512.h"
+
+/* include core specific register pointer definitions */
+#include <asm/cdef_LPBlackfin.h>
+
+/* SYSTEM & MMR ADDRESS DEFINITIONS FOR ADSP-BF512 */
+
+/* include cdefBF51x_base.h for the set of #defines that are common to all ADSP-BF51x processors */
+#include "cdefBF51x_base.h"
+
+#endif /* _CDEF_BF512_H */
diff --git a/arch/blackfin/mach-bf518/include/mach/cdefBF514.h b/arch/blackfin/mach-bf518/include/mach/cdefBF514.h
new file mode 100644
index 000000000000..9521e178fb28
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/cdefBF514.h
@@ -0,0 +1,48 @@
+/*
+ * File:         include/asm-blackfin/mach-bf518/cdefbf514.h
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:  system mmr register map
+ *
+ * Rev:
+ *
+ * Modified:
+ *
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, 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; see the file COPYING.
+ * If not, write to the Free Software Foundation,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef _CDEF_BF514_H
+#define _CDEF_BF514_H
+
+/* include all Core registers and bit definitions */
+#include "defBF514.h"
+
+/* include core specific register pointer definitions */
+#include <asm/cdef_LPBlackfin.h>
+
+/* SYSTEM & MMR ADDRESS DEFINITIONS FOR ADSP-BF514 */
+
+/* include cdefBF51x_base.h for the set of #defines that are common to all ADSP-BF51x processors */
+#include "cdefBF51x_base.h"
+
+/* The following are the #defines needed by ADSP-BF514 that are not in the common header */
+
+#endif /* _CDEF_BF514_H */
diff --git a/arch/blackfin/mach-bf518/include/mach/cdefBF516.h b/arch/blackfin/mach-bf518/include/mach/cdefBF516.h
new file mode 100644
index 000000000000..4e26ccfcef97
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/cdefBF516.h
@@ -0,0 +1,213 @@
+/*
+ * File:         include/asm-blackfin/mach-bf518/cdefbf516.h
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:  system mmr register map
+ *
+ * Rev:
+ *
+ * Modified:
+ *
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, 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; see the file COPYING.
+ * If not, write to the Free Software Foundation,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef _CDEF_BF516_H
+#define _CDEF_BF516_H
+
+/* include all Core registers and bit definitions */
+#include "defBF516.h"
+
+/* include core specific register pointer definitions */
+#include <asm/cdef_LPBlackfin.h>
+
+/* SYSTEM & MMR ADDRESS DEFINITIONS FOR ADSP-BF516 */
+
+/* include cdefBF51x_base.h for the set of #defines that are common to all ADSP-BF51x processors */
+#include "cdefBF51x_base.h"
+
+/* The following are the #defines needed by ADSP-BF516 that are not in the common header */
+
+/* 10/100 Ethernet Controller	(0xFFC03000 - 0xFFC031FF) */
+
+#define bfin_read_EMAC_OPMODE()			bfin_read32(EMAC_OPMODE)
+#define bfin_write_EMAC_OPMODE(val)		bfin_write32(EMAC_OPMODE, val)
+#define bfin_read_EMAC_ADDRLO()			bfin_read32(EMAC_ADDRLO)
+#define bfin_write_EMAC_ADDRLO(val)		bfin_write32(EMAC_ADDRLO, val)
+#define bfin_read_EMAC_ADDRHI()			bfin_read32(EMAC_ADDRHI)
+#define bfin_write_EMAC_ADDRHI(val)		bfin_write32(EMAC_ADDRHI, val)
+#define bfin_read_EMAC_HASHLO()			bfin_read32(EMAC_HASHLO)
+#define bfin_write_EMAC_HASHLO(val)		bfin_write32(EMAC_HASHLO, val)
+#define bfin_read_EMAC_HASHHI()			bfin_read32(EMAC_HASHHI)
+#define bfin_write_EMAC_HASHHI(val)		bfin_write32(EMAC_HASHHI, val)
+#define bfin_read_EMAC_STAADD()			bfin_read32(EMAC_STAADD)
+#define bfin_write_EMAC_STAADD(val)		bfin_write32(EMAC_STAADD, val)
+#define bfin_read_EMAC_STADAT()			bfin_read32(EMAC_STADAT)
+#define bfin_write_EMAC_STADAT(val)		bfin_write32(EMAC_STADAT, val)
+#define bfin_read_EMAC_FLC()			bfin_read32(EMAC_FLC)
+#define bfin_write_EMAC_FLC(val)		bfin_write32(EMAC_FLC, val)
+#define bfin_read_EMAC_VLAN1()			bfin_read32(EMAC_VLAN1)
+#define bfin_write_EMAC_VLAN1(val)		bfin_write32(EMAC_VLAN1, val)
+#define bfin_read_EMAC_VLAN2()			bfin_read32(EMAC_VLAN2)
+#define bfin_write_EMAC_VLAN2(val)		bfin_write32(EMAC_VLAN2, val)
+#define bfin_read_EMAC_WKUP_CTL()		bfin_read32(EMAC_WKUP_CTL)
+#define bfin_write_EMAC_WKUP_CTL(val)		bfin_write32(EMAC_WKUP_CTL, val)
+#define bfin_read_EMAC_WKUP_FFMSK0()		bfin_read32(EMAC_WKUP_FFMSK0)
+#define bfin_write_EMAC_WKUP_FFMSK0(val)	bfin_write32(EMAC_WKUP_FFMSK0, val)
+#define bfin_read_EMAC_WKUP_FFMSK1()		bfin_read32(EMAC_WKUP_FFMSK1)
+#define bfin_write_EMAC_WKUP_FFMSK1(val)	bfin_write32(EMAC_WKUP_FFMSK1, val)
+#define bfin_read_EMAC_WKUP_FFMSK2()		bfin_read32(EMAC_WKUP_FFMSK2)
+#define bfin_write_EMAC_WKUP_FFMSK2(val)	bfin_write32(EMAC_WKUP_FFMSK2, val)
+#define bfin_read_EMAC_WKUP_FFMSK3()		bfin_read32(EMAC_WKUP_FFMSK3)
+#define bfin_write_EMAC_WKUP_FFMSK3(val)	bfin_write32(EMAC_WKUP_FFMSK3, val)
+#define bfin_read_EMAC_WKUP_FFCMD()		bfin_read32(EMAC_WKUP_FFCMD)
+#define bfin_write_EMAC_WKUP_FFCMD(val)		bfin_write32(EMAC_WKUP_FFCMD, val)
+#define bfin_read_EMAC_WKUP_FFOFF()		bfin_read32(EMAC_WKUP_FFOFF)
+#define bfin_write_EMAC_WKUP_FFOFF(val)		bfin_write32(EMAC_WKUP_FFOFF, val)
+#define bfin_read_EMAC_WKUP_FFCRC0()		bfin_read32(EMAC_WKUP_FFCRC0)
+#define bfin_write_EMAC_WKUP_FFCRC0(val)	bfin_write32(EMAC_WKUP_FFCRC0, val)
+#define bfin_read_EMAC_WKUP_FFCRC1()		bfin_read32(EMAC_WKUP_FFCRC1)
+#define bfin_write_EMAC_WKUP_FFCRC1(val)	bfin_write32(EMAC_WKUP_FFCRC1, val)
+
+#define bfin_read_EMAC_SYSCTL()			bfin_read32(EMAC_SYSCTL)
+#define bfin_write_EMAC_SYSCTL(val)		bfin_write32(EMAC_SYSCTL, val)
+#define bfin_read_EMAC_SYSTAT()			bfin_read32(EMAC_SYSTAT)
+#define bfin_write_EMAC_SYSTAT(val)		bfin_write32(EMAC_SYSTAT, val)
+#define bfin_read_EMAC_RX_STAT()		bfin_read32(EMAC_RX_STAT)
+#define bfin_write_EMAC_RX_STAT(val)		bfin_write32(EMAC_RX_STAT, val)
+#define bfin_read_EMAC_RX_STKY()		bfin_read32(EMAC_RX_STKY)
+#define bfin_write_EMAC_RX_STKY(val)		bfin_write32(EMAC_RX_STKY, val)
+#define bfin_read_EMAC_RX_IRQE()		bfin_read32(EMAC_RX_IRQE)
+#define bfin_write_EMAC_RX_IRQE(val)		bfin_write32(EMAC_RX_IRQE, val)
+#define bfin_read_EMAC_TX_STAT()		bfin_read32(EMAC_TX_STAT)
+#define bfin_write_EMAC_TX_STAT(val)		bfin_write32(EMAC_TX_STAT, val)
+#define bfin_read_EMAC_TX_STKY()		bfin_read32(EMAC_TX_STKY)
+#define bfin_write_EMAC_TX_STKY(val)		bfin_write32(EMAC_TX_STKY, val)
+#define bfin_read_EMAC_TX_IRQE()		bfin_read32(EMAC_TX_IRQE)
+#define bfin_write_EMAC_TX_IRQE(val)		bfin_write32(EMAC_TX_IRQE, val)
+
+#define bfin_read_EMAC_MMC_CTL()		bfin_read32(EMAC_MMC_CTL)
+#define bfin_write_EMAC_MMC_CTL(val)		bfin_write32(EMAC_MMC_CTL, val)
+#define bfin_read_EMAC_MMC_RIRQS()		bfin_read32(EMAC_MMC_RIRQS)
+#define bfin_write_EMAC_MMC_RIRQS(val)		bfin_write32(EMAC_MMC_RIRQS, val)
+#define bfin_read_EMAC_MMC_RIRQE()		bfin_read32(EMAC_MMC_RIRQE)
+#define bfin_write_EMAC_MMC_RIRQE(val)		bfin_write32(EMAC_MMC_RIRQE, val)
+#define bfin_read_EMAC_MMC_TIRQS()		bfin_read32(EMAC_MMC_TIRQS)
+#define bfin_write_EMAC_MMC_TIRQS(val)		bfin_write32(EMAC_MMC_TIRQS, val)
+#define bfin_read_EMAC_MMC_TIRQE()		bfin_read32(EMAC_MMC_TIRQE)
+#define bfin_write_EMAC_MMC_TIRQE(val)		bfin_write32(EMAC_MMC_TIRQE, val)
+
+#define bfin_read_EMAC_RXC_OK()			bfin_read32(EMAC_RXC_OK)
+#define bfin_write_EMAC_RXC_OK(val)		bfin_write32(EMAC_RXC_OK, val)
+#define bfin_read_EMAC_RXC_FCS()		bfin_read32(EMAC_RXC_FCS)
+#define bfin_write_EMAC_RXC_FCS(val)		bfin_write32(EMAC_RXC_FCS, val)
+#define bfin_read_EMAC_RXC_ALIGN()		bfin_read32(EMAC_RXC_ALIGN)
+#define bfin_write_EMAC_RXC_ALIGN(val)		bfin_write32(EMAC_RXC_ALIGN, val)
+#define bfin_read_EMAC_RXC_OCTET()		bfin_read32(EMAC_RXC_OCTET)
+#define bfin_write_EMAC_RXC_OCTET(val)		bfin_write32(EMAC_RXC_OCTET, val)
+#define bfin_read_EMAC_RXC_DMAOVF()		bfin_read32(EMAC_RXC_DMAOVF)
+#define bfin_write_EMAC_RXC_DMAOVF(val)		bfin_write32(EMAC_RXC_DMAOVF, val)
+#define bfin_read_EMAC_RXC_UNICST()		bfin_read32(EMAC_RXC_UNICST)
+#define bfin_write_EMAC_RXC_UNICST(val)		bfin_write32(EMAC_RXC_UNICST, val)
+#define bfin_read_EMAC_RXC_MULTI()		bfin_read32(EMAC_RXC_MULTI)
+#define bfin_write_EMAC_RXC_MULTI(val)		bfin_write32(EMAC_RXC_MULTI, val)
+#define bfin_read_EMAC_RXC_BROAD()		bfin_read32(EMAC_RXC_BROAD)
+#define bfin_write_EMAC_RXC_BROAD(val)		bfin_write32(EMAC_RXC_BROAD, val)
+#define bfin_read_EMAC_RXC_LNERRI()		bfin_read32(EMAC_RXC_LNERRI)
+#define bfin_write_EMAC_RXC_LNERRI(val)		bfin_write32(EMAC_RXC_LNERRI, val)
+#define bfin_read_EMAC_RXC_LNERRO()		bfin_read32(EMAC_RXC_LNERRO)
+#define bfin_write_EMAC_RXC_LNERRO(val)		bfin_write32(EMAC_RXC_LNERRO, val)
+#define bfin_read_EMAC_RXC_LONG()		bfin_read32(EMAC_RXC_LONG)
+#define bfin_write_EMAC_RXC_LONG(val)		bfin_write32(EMAC_RXC_LONG, val)
+#define bfin_read_EMAC_RXC_MACCTL()		bfin_read32(EMAC_RXC_MACCTL)
+#define bfin_write_EMAC_RXC_MACCTL(val)		bfin_write32(EMAC_RXC_MACCTL, val)
+#define bfin_read_EMAC_RXC_OPCODE()		bfin_read32(EMAC_RXC_OPCODE)
+#define bfin_write_EMAC_RXC_OPCODE(val)		bfin_write32(EMAC_RXC_OPCODE, val)
+#define bfin_read_EMAC_RXC_PAUSE()		bfin_read32(EMAC_RXC_PAUSE)
+#define bfin_write_EMAC_RXC_PAUSE(val)		bfin_write32(EMAC_RXC_PAUSE, val)
+#define bfin_read_EMAC_RXC_ALLFRM()		bfin_read32(EMAC_RXC_ALLFRM)
+#define bfin_write_EMAC_RXC_ALLFRM(val)		bfin_write32(EMAC_RXC_ALLFRM, val)
+#define bfin_read_EMAC_RXC_ALLOCT()		bfin_read32(EMAC_RXC_ALLOCT)
+#define bfin_write_EMAC_RXC_ALLOCT(val)		bfin_write32(EMAC_RXC_ALLOCT, val)
+#define bfin_read_EMAC_RXC_TYPED()		bfin_read32(EMAC_RXC_TYPED)
+#define bfin_write_EMAC_RXC_TYPED(val)		bfin_write32(EMAC_RXC_TYPED, val)
+#define bfin_read_EMAC_RXC_SHORT()		bfin_read32(EMAC_RXC_SHORT)
+#define bfin_write_EMAC_RXC_SHORT(val)		bfin_write32(EMAC_RXC_SHORT, val)
+#define bfin_read_EMAC_RXC_EQ64()		bfin_read32(EMAC_RXC_EQ64)
+#define bfin_write_EMAC_RXC_EQ64(val)		bfin_write32(EMAC_RXC_EQ64, val)
+#define bfin_read_EMAC_RXC_LT128()		bfin_read32(EMAC_RXC_LT128)
+#define bfin_write_EMAC_RXC_LT128(val)		bfin_write32(EMAC_RXC_LT128, val)
+#define bfin_read_EMAC_RXC_LT256()		bfin_read32(EMAC_RXC_LT256)
+#define bfin_write_EMAC_RXC_LT256(val)		bfin_write32(EMAC_RXC_LT256, val)
+#define bfin_read_EMAC_RXC_LT512()		bfin_read32(EMAC_RXC_LT512)
+#define bfin_write_EMAC_RXC_LT512(val)		bfin_write32(EMAC_RXC_LT512, val)
+#define bfin_read_EMAC_RXC_LT1024()		bfin_read32(EMAC_RXC_LT1024)
+#define bfin_write_EMAC_RXC_LT1024(val)		bfin_write32(EMAC_RXC_LT1024, val)
+#define bfin_read_EMAC_RXC_GE1024()		bfin_read32(EMAC_RXC_GE1024)
+#define bfin_write_EMAC_RXC_GE1024(val)		bfin_write32(EMAC_RXC_GE1024, val)
+
+#define bfin_read_EMAC_TXC_OK()			bfin_read32(EMAC_TXC_OK)
+#define bfin_write_EMAC_TXC_OK(val)		bfin_write32(EMAC_TXC_OK, val)
+#define bfin_read_EMAC_TXC_1COL()		bfin_read32(EMAC_TXC_1COL)
+#define bfin_write_EMAC_TXC_1COL(val)		bfin_write32(EMAC_TXC_1COL, val)
+#define bfin_read_EMAC_TXC_GT1COL()		bfin_read32(EMAC_TXC_GT1COL)
+#define bfin_write_EMAC_TXC_GT1COL(val)		bfin_write32(EMAC_TXC_GT1COL, val)
+#define bfin_read_EMAC_TXC_OCTET()		bfin_read32(EMAC_TXC_OCTET)
+#define bfin_write_EMAC_TXC_OCTET(val)		bfin_write32(EMAC_TXC_OCTET, val)
+#define bfin_read_EMAC_TXC_DEFER()		bfin_read32(EMAC_TXC_DEFER)
+#define bfin_write_EMAC_TXC_DEFER(val)		bfin_write32(EMAC_TXC_DEFER, val)
+#define bfin_read_EMAC_TXC_LATECL()		bfin_read32(EMAC_TXC_LATECL)
+#define bfin_write_EMAC_TXC_LATECL(val)		bfin_write32(EMAC_TXC_LATECL, val)
+#define bfin_read_EMAC_TXC_XS_COL()		bfin_read32(EMAC_TXC_XS_COL)
+#define bfin_write_EMAC_TXC_XS_COL(val)		bfin_write32(EMAC_TXC_XS_COL, val)
+#define bfin_read_EMAC_TXC_DMAUND()		bfin_read32(EMAC_TXC_DMAUND)
+#define bfin_write_EMAC_TXC_DMAUND(val)		bfin_write32(EMAC_TXC_DMAUND, val)
+#define bfin_read_EMAC_TXC_CRSERR()		bfin_read32(EMAC_TXC_CRSERR)
+#define bfin_write_EMAC_TXC_CRSERR(val)		bfin_write32(EMAC_TXC_CRSERR, val)
+#define bfin_read_EMAC_TXC_UNICST()		bfin_read32(EMAC_TXC_UNICST)
+#define bfin_write_EMAC_TXC_UNICST(val)		bfin_write32(EMAC_TXC_UNICST, val)
+#define bfin_read_EMAC_TXC_MULTI()		bfin_read32(EMAC_TXC_MULTI)
+#define bfin_write_EMAC_TXC_MULTI(val)		bfin_write32(EMAC_TXC_MULTI, val)
+#define bfin_read_EMAC_TXC_BROAD()		bfin_read32(EMAC_TXC_BROAD)
+#define bfin_write_EMAC_TXC_BROAD(val)		bfin_write32(EMAC_TXC_BROAD, val)
+#define bfin_read_EMAC_TXC_XS_DFR()		bfin_read32(EMAC_TXC_XS_DFR)
+#define bfin_write_EMAC_TXC_XS_DFR(val)		bfin_write32(EMAC_TXC_XS_DFR, val)
+#define bfin_read_EMAC_TXC_MACCTL()		bfin_read32(EMAC_TXC_MACCTL)
+#define bfin_write_EMAC_TXC_MACCTL(val)		bfin_write32(EMAC_TXC_MACCTL, val)
+#define bfin_read_EMAC_TXC_ALLFRM()		bfin_read32(EMAC_TXC_ALLFRM)
+#define bfin_write_EMAC_TXC_ALLFRM(val)		bfin_write32(EMAC_TXC_ALLFRM, val)
+#define bfin_read_EMAC_TXC_ALLOCT()		bfin_read32(EMAC_TXC_ALLOCT)
+#define bfin_write_EMAC_TXC_ALLOCT(val)		bfin_write32(EMAC_TXC_ALLOCT, val)
+#define bfin_read_EMAC_TXC_EQ64()		bfin_read32(EMAC_TXC_EQ64)
+#define bfin_write_EMAC_TXC_EQ64(val)		bfin_write32(EMAC_TXC_EQ64, val)
+#define bfin_read_EMAC_TXC_LT128()		bfin_read32(EMAC_TXC_LT128)
+#define bfin_write_EMAC_TXC_LT128(val)		bfin_write32(EMAC_TXC_LT128, val)
+#define bfin_read_EMAC_TXC_LT256()		bfin_read32(EMAC_TXC_LT256)
+#define bfin_write_EMAC_TXC_LT256(val)		bfin_write32(EMAC_TXC_LT256, val)
+#define bfin_read_EMAC_TXC_LT512()		bfin_read32(EMAC_TXC_LT512)
+#define bfin_write_EMAC_TXC_LT512(val)		bfin_write32(EMAC_TXC_LT512, val)
+#define bfin_read_EMAC_TXC_LT1024()		bfin_read32(EMAC_TXC_LT1024)
+#define bfin_write_EMAC_TXC_LT1024(val)		bfin_write32(EMAC_TXC_LT1024, val)
+#define bfin_read_EMAC_TXC_GE1024()		bfin_read32(EMAC_TXC_GE1024)
+#define bfin_write_EMAC_TXC_GE1024(val)		bfin_write32(EMAC_TXC_GE1024, val)
+#define bfin_read_EMAC_TXC_ABORT()		bfin_read32(EMAC_TXC_ABORT)
+#define bfin_write_EMAC_TXC_ABORT(val)		bfin_write32(EMAC_TXC_ABORT, val)
+
+#endif /* _CDEF_BF516_H */
diff --git a/arch/blackfin/mach-bf518/include/mach/cdefBF518.h b/arch/blackfin/mach-bf518/include/mach/cdefBF518.h
new file mode 100644
index 000000000000..bafb370cfb3c
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/cdefBF518.h
@@ -0,0 +1,282 @@
+/*
+ * File:         include/asm-blackfin/mach-bf518/cdefbf518.h
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:  system mmr register map
+ *
+ * Rev:
+ *
+ * Modified:
+ *
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, 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; see the file COPYING.
+ * If not, write to the Free Software Foundation,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef _CDEF_BF518_H
+#define _CDEF_BF518_H
+
+/* include all Core registers and bit definitions */
+#include "defBF518.h"
+
+/* include core specific register pointer definitions */
+#include <asm/cdef_LPBlackfin.h>
+
+/* SYSTEM & MMR ADDRESS DEFINITIONS FOR ADSP-BF518 */
+
+/* include cdefBF51x_base.h for the set of #defines that are common to all ADSP-BF51x processors */
+#include "cdefBF51x_base.h"
+
+/* The following are the #defines needed by ADSP-BF518 that are not in the common header */
+
+
+/* 10/100 Ethernet Controller	(0xFFC03000 - 0xFFC031FF) */
+
+#define bfin_read_EMAC_OPMODE()			bfin_read32(EMAC_OPMODE)
+#define bfin_write_EMAC_OPMODE(val)		bfin_write32(EMAC_OPMODE, val)
+#define bfin_read_EMAC_ADDRLO()			bfin_read32(EMAC_ADDRLO)
+#define bfin_write_EMAC_ADDRLO(val)		bfin_write32(EMAC_ADDRLO, val)
+#define bfin_read_EMAC_ADDRHI()			bfin_read32(EMAC_ADDRHI)
+#define bfin_write_EMAC_ADDRHI(val)		bfin_write32(EMAC_ADDRHI, val)
+#define bfin_read_EMAC_HASHLO()			bfin_read32(EMAC_HASHLO)
+#define bfin_write_EMAC_HASHLO(val)		bfin_write32(EMAC_HASHLO, val)
+#define bfin_read_EMAC_HASHHI()			bfin_read32(EMAC_HASHHI)
+#define bfin_write_EMAC_HASHHI(val)		bfin_write32(EMAC_HASHHI, val)
+#define bfin_read_EMAC_STAADD()			bfin_read32(EMAC_STAADD)
+#define bfin_write_EMAC_STAADD(val)		bfin_write32(EMAC_STAADD, val)
+#define bfin_read_EMAC_STADAT()			bfin_read32(EMAC_STADAT)
+#define bfin_write_EMAC_STADAT(val)		bfin_write32(EMAC_STADAT, val)
+#define bfin_read_EMAC_FLC()			bfin_read32(EMAC_FLC)
+#define bfin_write_EMAC_FLC(val)		bfin_write32(EMAC_FLC, val)
+#define bfin_read_EMAC_VLAN1()			bfin_read32(EMAC_VLAN1)
+#define bfin_write_EMAC_VLAN1(val)		bfin_write32(EMAC_VLAN1, val)
+#define bfin_read_EMAC_VLAN2()			bfin_read32(EMAC_VLAN2)
+#define bfin_write_EMAC_VLAN2(val)		bfin_write32(EMAC_VLAN2, val)
+#define bfin_read_EMAC_WKUP_CTL()		bfin_read32(EMAC_WKUP_CTL)
+#define bfin_write_EMAC_WKUP_CTL(val)		bfin_write32(EMAC_WKUP_CTL, val)
+#define bfin_read_EMAC_WKUP_FFMSK0()		bfin_read32(EMAC_WKUP_FFMSK0)
+#define bfin_write_EMAC_WKUP_FFMSK0(val)	bfin_write32(EMAC_WKUP_FFMSK0, val)
+#define bfin_read_EMAC_WKUP_FFMSK1()		bfin_read32(EMAC_WKUP_FFMSK1)
+#define bfin_write_EMAC_WKUP_FFMSK1(val)	bfin_write32(EMAC_WKUP_FFMSK1, val)
+#define bfin_read_EMAC_WKUP_FFMSK2()		bfin_read32(EMAC_WKUP_FFMSK2)
+#define bfin_write_EMAC_WKUP_FFMSK2(val)	bfin_write32(EMAC_WKUP_FFMSK2, val)
+#define bfin_read_EMAC_WKUP_FFMSK3()		bfin_read32(EMAC_WKUP_FFMSK3)
+#define bfin_write_EMAC_WKUP_FFMSK3(val)	bfin_write32(EMAC_WKUP_FFMSK3, val)
+#define bfin_read_EMAC_WKUP_FFCMD()		bfin_read32(EMAC_WKUP_FFCMD)
+#define bfin_write_EMAC_WKUP_FFCMD(val)		bfin_write32(EMAC_WKUP_FFCMD, val)
+#define bfin_read_EMAC_WKUP_FFOFF()		bfin_read32(EMAC_WKUP_FFOFF)
+#define bfin_write_EMAC_WKUP_FFOFF(val)		bfin_write32(EMAC_WKUP_FFOFF, val)
+#define bfin_read_EMAC_WKUP_FFCRC0()		bfin_read32(EMAC_WKUP_FFCRC0)
+#define bfin_write_EMAC_WKUP_FFCRC0(val)	bfin_write32(EMAC_WKUP_FFCRC0, val)
+#define bfin_read_EMAC_WKUP_FFCRC1()		bfin_read32(EMAC_WKUP_FFCRC1)
+#define bfin_write_EMAC_WKUP_FFCRC1(val)	bfin_write32(EMAC_WKUP_FFCRC1, val)
+
+#define bfin_read_EMAC_SYSCTL()			bfin_read32(EMAC_SYSCTL)
+#define bfin_write_EMAC_SYSCTL(val)		bfin_write32(EMAC_SYSCTL, val)
+#define bfin_read_EMAC_SYSTAT()			bfin_read32(EMAC_SYSTAT)
+#define bfin_write_EMAC_SYSTAT(val)		bfin_write32(EMAC_SYSTAT, val)
+#define bfin_read_EMAC_RX_STAT()		bfin_read32(EMAC_RX_STAT)
+#define bfin_write_EMAC_RX_STAT(val)		bfin_write32(EMAC_RX_STAT, val)
+#define bfin_read_EMAC_RX_STKY()		bfin_read32(EMAC_RX_STKY)
+#define bfin_write_EMAC_RX_STKY(val)		bfin_write32(EMAC_RX_STKY, val)
+#define bfin_read_EMAC_RX_IRQE()		bfin_read32(EMAC_RX_IRQE)
+#define bfin_write_EMAC_RX_IRQE(val)		bfin_write32(EMAC_RX_IRQE, val)
+#define bfin_read_EMAC_TX_STAT()		bfin_read32(EMAC_TX_STAT)
+#define bfin_write_EMAC_TX_STAT(val)		bfin_write32(EMAC_TX_STAT, val)
+#define bfin_read_EMAC_TX_STKY()		bfin_read32(EMAC_TX_STKY)
+#define bfin_write_EMAC_TX_STKY(val)		bfin_write32(EMAC_TX_STKY, val)
+#define bfin_read_EMAC_TX_IRQE()		bfin_read32(EMAC_TX_IRQE)
+#define bfin_write_EMAC_TX_IRQE(val)		bfin_write32(EMAC_TX_IRQE, val)
+
+#define bfin_read_EMAC_MMC_CTL()		bfin_read32(EMAC_MMC_CTL)
+#define bfin_write_EMAC_MMC_CTL(val)		bfin_write32(EMAC_MMC_CTL, val)
+#define bfin_read_EMAC_MMC_RIRQS()		bfin_read32(EMAC_MMC_RIRQS)
+#define bfin_write_EMAC_MMC_RIRQS(val)		bfin_write32(EMAC_MMC_RIRQS, val)
+#define bfin_read_EMAC_MMC_RIRQE()		bfin_read32(EMAC_MMC_RIRQE)
+#define bfin_write_EMAC_MMC_RIRQE(val)		bfin_write32(EMAC_MMC_RIRQE, val)
+#define bfin_read_EMAC_MMC_TIRQS()		bfin_read32(EMAC_MMC_TIRQS)
+#define bfin_write_EMAC_MMC_TIRQS(val)		bfin_write32(EMAC_MMC_TIRQS, val)
+#define bfin_read_EMAC_MMC_TIRQE()		bfin_read32(EMAC_MMC_TIRQE)
+#define bfin_write_EMAC_MMC_TIRQE(val)		bfin_write32(EMAC_MMC_TIRQE, val)
+
+#define bfin_read_EMAC_RXC_OK()			bfin_read32(EMAC_RXC_OK)
+#define bfin_write_EMAC_RXC_OK(val)		bfin_write32(EMAC_RXC_OK, val)
+#define bfin_read_EMAC_RXC_FCS()		bfin_read32(EMAC_RXC_FCS)
+#define bfin_write_EMAC_RXC_FCS(val)		bfin_write32(EMAC_RXC_FCS, val)
+#define bfin_read_EMAC_RXC_ALIGN()		bfin_read32(EMAC_RXC_ALIGN)
+#define bfin_write_EMAC_RXC_ALIGN(val)		bfin_write32(EMAC_RXC_ALIGN, val)
+#define bfin_read_EMAC_RXC_OCTET()		bfin_read32(EMAC_RXC_OCTET)
+#define bfin_write_EMAC_RXC_OCTET(val)		bfin_write32(EMAC_RXC_OCTET, val)
+#define bfin_read_EMAC_RXC_DMAOVF()		bfin_read32(EMAC_RXC_DMAOVF)
+#define bfin_write_EMAC_RXC_DMAOVF(val)		bfin_write32(EMAC_RXC_DMAOVF, val)
+#define bfin_read_EMAC_RXC_UNICST()		bfin_read32(EMAC_RXC_UNICST)
+#define bfin_write_EMAC_RXC_UNICST(val)		bfin_write32(EMAC_RXC_UNICST, val)
+#define bfin_read_EMAC_RXC_MULTI()		bfin_read32(EMAC_RXC_MULTI)
+#define bfin_write_EMAC_RXC_MULTI(val)		bfin_write32(EMAC_RXC_MULTI, val)
+#define bfin_read_EMAC_RXC_BROAD()		bfin_read32(EMAC_RXC_BROAD)
+#define bfin_write_EMAC_RXC_BROAD(val)		bfin_write32(EMAC_RXC_BROAD, val)
+#define bfin_read_EMAC_RXC_LNERRI()		bfin_read32(EMAC_RXC_LNERRI)
+#define bfin_write_EMAC_RXC_LNERRI(val)		bfin_write32(EMAC_RXC_LNERRI, val)
+#define bfin_read_EMAC_RXC_LNERRO()		bfin_read32(EMAC_RXC_LNERRO)
+#define bfin_write_EMAC_RXC_LNERRO(val)		bfin_write32(EMAC_RXC_LNERRO, val)
+#define bfin_read_EMAC_RXC_LONG()		bfin_read32(EMAC_RXC_LONG)
+#define bfin_write_EMAC_RXC_LONG(val)		bfin_write32(EMAC_RXC_LONG, val)
+#define bfin_read_EMAC_RXC_MACCTL()		bfin_read32(EMAC_RXC_MACCTL)
+#define bfin_write_EMAC_RXC_MACCTL(val)		bfin_write32(EMAC_RXC_MACCTL, val)
+#define bfin_read_EMAC_RXC_OPCODE()		bfin_read32(EMAC_RXC_OPCODE)
+#define bfin_write_EMAC_RXC_OPCODE(val)		bfin_write32(EMAC_RXC_OPCODE, val)
+#define bfin_read_EMAC_RXC_PAUSE()		bfin_read32(EMAC_RXC_PAUSE)
+#define bfin_write_EMAC_RXC_PAUSE(val)		bfin_write32(EMAC_RXC_PAUSE, val)
+#define bfin_read_EMAC_RXC_ALLFRM()		bfin_read32(EMAC_RXC_ALLFRM)
+#define bfin_write_EMAC_RXC_ALLFRM(val)		bfin_write32(EMAC_RXC_ALLFRM, val)
+#define bfin_read_EMAC_RXC_ALLOCT()		bfin_read32(EMAC_RXC_ALLOCT)
+#define bfin_write_EMAC_RXC_ALLOCT(val)		bfin_write32(EMAC_RXC_ALLOCT, val)
+#define bfin_read_EMAC_RXC_TYPED()		bfin_read32(EMAC_RXC_TYPED)
+#define bfin_write_EMAC_RXC_TYPED(val)		bfin_write32(EMAC_RXC_TYPED, val)
+#define bfin_read_EMAC_RXC_SHORT()		bfin_read32(EMAC_RXC_SHORT)
+#define bfin_write_EMAC_RXC_SHORT(val)		bfin_write32(EMAC_RXC_SHORT, val)
+#define bfin_read_EMAC_RXC_EQ64()		bfin_read32(EMAC_RXC_EQ64)
+#define bfin_write_EMAC_RXC_EQ64(val)		bfin_write32(EMAC_RXC_EQ64, val)
+#define bfin_read_EMAC_RXC_LT128()		bfin_read32(EMAC_RXC_LT128)
+#define bfin_write_EMAC_RXC_LT128(val)		bfin_write32(EMAC_RXC_LT128, val)
+#define bfin_read_EMAC_RXC_LT256()		bfin_read32(EMAC_RXC_LT256)
+#define bfin_write_EMAC_RXC_LT256(val)		bfin_write32(EMAC_RXC_LT256, val)
+#define bfin_read_EMAC_RXC_LT512()		bfin_read32(EMAC_RXC_LT512)
+#define bfin_write_EMAC_RXC_LT512(val)		bfin_write32(EMAC_RXC_LT512, val)
+#define bfin_read_EMAC_RXC_LT1024()		bfin_read32(EMAC_RXC_LT1024)
+#define bfin_write_EMAC_RXC_LT1024(val)		bfin_write32(EMAC_RXC_LT1024, val)
+#define bfin_read_EMAC_RXC_GE1024()		bfin_read32(EMAC_RXC_GE1024)
+#define bfin_write_EMAC_RXC_GE1024(val)		bfin_write32(EMAC_RXC_GE1024, val)
+
+#define bfin_read_EMAC_TXC_OK()			bfin_read32(EMAC_TXC_OK)
+#define bfin_write_EMAC_TXC_OK(val)		bfin_write32(EMAC_TXC_OK, val)
+#define bfin_read_EMAC_TXC_1COL()		bfin_read32(EMAC_TXC_1COL)
+#define bfin_write_EMAC_TXC_1COL(val)		bfin_write32(EMAC_TXC_1COL, val)
+#define bfin_read_EMAC_TXC_GT1COL()		bfin_read32(EMAC_TXC_GT1COL)
+#define bfin_write_EMAC_TXC_GT1COL(val)		bfin_write32(EMAC_TXC_GT1COL, val)
+#define bfin_read_EMAC_TXC_OCTET()		bfin_read32(EMAC_TXC_OCTET)
+#define bfin_write_EMAC_TXC_OCTET(val)		bfin_write32(EMAC_TXC_OCTET, val)
+#define bfin_read_EMAC_TXC_DEFER()		bfin_read32(EMAC_TXC_DEFER)
+#define bfin_write_EMAC_TXC_DEFER(val)		bfin_write32(EMAC_TXC_DEFER, val)
+#define bfin_read_EMAC_TXC_LATECL()		bfin_read32(EMAC_TXC_LATECL)
+#define bfin_write_EMAC_TXC_LATECL(val)		bfin_write32(EMAC_TXC_LATECL, val)
+#define bfin_read_EMAC_TXC_XS_COL()		bfin_read32(EMAC_TXC_XS_COL)
+#define bfin_write_EMAC_TXC_XS_COL(val)		bfin_write32(EMAC_TXC_XS_COL, val)
+#define bfin_read_EMAC_TXC_DMAUND()		bfin_read32(EMAC_TXC_DMAUND)
+#define bfin_write_EMAC_TXC_DMAUND(val)		bfin_write32(EMAC_TXC_DMAUND, val)
+#define bfin_read_EMAC_TXC_CRSERR()		bfin_read32(EMAC_TXC_CRSERR)
+#define bfin_write_EMAC_TXC_CRSERR(val)		bfin_write32(EMAC_TXC_CRSERR, val)
+#define bfin_read_EMAC_TXC_UNICST()		bfin_read32(EMAC_TXC_UNICST)
+#define bfin_write_EMAC_TXC_UNICST(val)		bfin_write32(EMAC_TXC_UNICST, val)
+#define bfin_read_EMAC_TXC_MULTI()		bfin_read32(EMAC_TXC_MULTI)
+#define bfin_write_EMAC_TXC_MULTI(val)		bfin_write32(EMAC_TXC_MULTI, val)
+#define bfin_read_EMAC_TXC_BROAD()		bfin_read32(EMAC_TXC_BROAD)
+#define bfin_write_EMAC_TXC_BROAD(val)		bfin_write32(EMAC_TXC_BROAD, val)
+#define bfin_read_EMAC_TXC_XS_DFR()		bfin_read32(EMAC_TXC_XS_DFR)
+#define bfin_write_EMAC_TXC_XS_DFR(val)		bfin_write32(EMAC_TXC_XS_DFR, val)
+#define bfin_read_EMAC_TXC_MACCTL()		bfin_read32(EMAC_TXC_MACCTL)
+#define bfin_write_EMAC_TXC_MACCTL(val)		bfin_write32(EMAC_TXC_MACCTL, val)
+#define bfin_read_EMAC_TXC_ALLFRM()		bfin_read32(EMAC_TXC_ALLFRM)
+#define bfin_write_EMAC_TXC_ALLFRM(val)		bfin_write32(EMAC_TXC_ALLFRM, val)
+#define bfin_read_EMAC_TXC_ALLOCT()		bfin_read32(EMAC_TXC_ALLOCT)
+#define bfin_write_EMAC_TXC_ALLOCT(val)		bfin_write32(EMAC_TXC_ALLOCT, val)
+#define bfin_read_EMAC_TXC_EQ64()		bfin_read32(EMAC_TXC_EQ64)
+#define bfin_write_EMAC_TXC_EQ64(val)		bfin_write32(EMAC_TXC_EQ64, val)
+#define bfin_read_EMAC_TXC_LT128()		bfin_read32(EMAC_TXC_LT128)
+#define bfin_write_EMAC_TXC_LT128(val)		bfin_write32(EMAC_TXC_LT128, val)
+#define bfin_read_EMAC_TXC_LT256()		bfin_read32(EMAC_TXC_LT256)
+#define bfin_write_EMAC_TXC_LT256(val)		bfin_write32(EMAC_TXC_LT256, val)
+#define bfin_read_EMAC_TXC_LT512()		bfin_read32(EMAC_TXC_LT512)
+#define bfin_write_EMAC_TXC_LT512(val)		bfin_write32(EMAC_TXC_LT512, val)
+#define bfin_read_EMAC_TXC_LT1024()		bfin_read32(EMAC_TXC_LT1024)
+#define bfin_write_EMAC_TXC_LT1024(val)		bfin_write32(EMAC_TXC_LT1024, val)
+#define bfin_read_EMAC_TXC_GE1024()		bfin_read32(EMAC_TXC_GE1024)
+#define bfin_write_EMAC_TXC_GE1024(val)		bfin_write32(EMAC_TXC_GE1024, val)
+#define bfin_read_EMAC_TXC_ABORT()		bfin_read32(EMAC_TXC_ABORT)
+#define bfin_write_EMAC_TXC_ABORT(val)		bfin_write32(EMAC_TXC_ABORT, val)
+
+/* Removable Storage Interface Registers */
+
+#define bfin_read_RSI_PWR_CTL()        bfin_read16(RSI_PWR_CONTROL)
+#define bfin_write_RSI_PWR_CTL(val)    bfin_write16(RSI_PWR_CONTROL, val)
+#define bfin_read_RSI_CLK_CTL()	       bfin_read16(RSI_CLK_CONTROL)
+#define bfin_write_RSI_CLK_CTL(val)    bfin_write16(RSI_CLK_CONTROL, val)
+#define bfin_read_RSI_ARGUMENT()       bfin_read32(RSI_ARGUMENT)
+#define bfin_write_RSI_ARGUMENT(val)   bfin_write32(RSI_ARGUMENT, val)
+#define bfin_read_RSI_COMMAND()        bfin_read16(RSI_COMMAND)
+#define bfin_write_RSI_COMMAND(val)    bfin_write16(RSI_COMMAND, val)
+#define bfin_read_RSI_RESP_CMD()       bfin_read16(RSI_RESP_CMD)
+#define bfin_write_RSI_RESP_CMD(val)   bfin_write16(RSI_RESP_CMD, val)
+#define bfin_read_RSI_RESPONSE0()      bfin_read32(RSI_RESPONSE0)
+#define bfin_write_RSI_RESPONSE0(val)  bfin_write32(RSI_RESPONSE0, val)
+#define bfin_read_RSI_RESPONSE1()      bfin_read32(RSI_RESPONSE1)
+#define bfin_write_RSI_RESPONSE1(val)  bfin_write32(RSI_RESPONSE1, val)
+#define bfin_read_RSI_RESPONSE2()      bfin_read32(RSI_RESPONSE2)
+#define bfin_write_RSI_RESPONSE2(val)  bfin_write32(RSI_RESPONSE2, val)
+#define bfin_read_RSI_RESPONSE3()      bfin_read32(RSI_RESPONSE3)
+#define bfin_write_RSI_RESPONSE3(val)  bfin_write32(RSI_RESPONSE3, val)
+#define bfin_read_RSI_DATA_TIMER()     bfin_read32(RSI_DATA_TIMER)
+#define bfin_write_RSI_DATA_TIMER(val) bfin_write32(RSI_DATA_TIMER, val)
+#define bfin_read_RSI_DATA_LGTH()      bfin_read16(RSI_DATA_LGTH)
+#define bfin_write_RSI_DATA_LGTH(val)  bfin_write16(RSI_DATA_LGTH, val)
+#define bfin_read_RSI_DATA_CTL()       bfin_read16(RSI_DATA_CONTROL)
+#define bfin_write_RSI_DATA_CTL(val)   bfin_write16(RSI_DATA_CONTROL, val)
+#define bfin_read_RSI_DATA_CNT()       bfin_read16(RSI_DATA_CNT)
+#define bfin_write_RSI_DATA_CNT(val)   bfin_write16(RSI_DATA_CNT, val)
+#define bfin_read_RSI_STATUS()         bfin_read32(RSI_STATUS)
+#define bfin_write_RSI_STATUS(val)     bfin_write32(RSI_STATUS, val)
+#define bfin_read_RSI_STATUS_CLR()     bfin_read16(RSI_STATUSCL)
+#define bfin_write_RSI_STATUS_CLR(val) bfin_write16(RSI_STATUSCL, val)
+#define bfin_read_RSI_MASK0()          bfin_read32(RSI_MASK0)
+#define bfin_write_RSI_MASK0(val)      bfin_write32(RSI_MASK0, val)
+#define bfin_read_RSI_MASK1()          bfin_read32(RSI_MASK1)
+#define bfin_write_RSI_MASK1(val)      bfin_write32(RSI_MASK1, val)
+#define bfin_read_RSI_FIFO_CNT()       bfin_read16(RSI_FIFO_CNT)
+#define bfin_write_RSI_FIFO_CNT(val)   bfin_write16(RSI_FIFO_CNT, val)
+#define bfin_read_RSI_CEATA_CTL()      bfin_read16(RSI_CEATA_CONTROL)
+#define bfin_write_RSI_CEATA_CTL(val)  bfin_write16(RSI_CEATA_CONTROL, val)
+#define bfin_read_RSI_FIFO()           bfin_read32(RSI_FIFO)
+#define bfin_write_RSI_FIFO(val)       bfin_write32(RSI_FIFO, val)
+#define bfin_read_RSI_E_STATUS()       bfin_read16(RSI_ESTAT)
+#define bfin_write_RSI_E_STATUS(val)   bfin_write16(RSI_ESTAT, val)
+#define bfin_read_RSI_E_MASK()         bfin_read16(RSI_EMASK)
+#define bfin_write_RSI_E_MASK(val)     bfin_write16(RSI_EMASK, val)
+#define bfin_read_RSI_CFG()            bfin_read16(RSI_CONFIG)
+#define bfin_write_RSI_CFG(val)        bfin_write16(RSI_CONFIG, val)
+#define bfin_read_RSI_RD_WAIT_EN()     bfin_read16(RSI_RD_WAIT_EN)
+#define bfin_write_RSI_RD_WAIT_EN(val) bfin_write16(RSI_RD_WAIT_EN, val)
+#define bfin_read_RSI_PID0()           bfin_read16(RSI_PID0)
+#define bfin_write_RSI_PID0(val)       bfin_write16(RSI_PID0, val)
+#define bfin_read_RSI_PID1()           bfin_read16(RSI_PID1)
+#define bfin_write_RSI_PID1(val)       bfin_write16(RSI_PID1, val)
+#define bfin_read_RSI_PID2()           bfin_read16(RSI_PID2)
+#define bfin_write_RSI_PID2(val)       bfin_write16(RSI_PID2, val)
+#define bfin_read_RSI_PID3()           bfin_read16(RSI_PID3)
+#define bfin_write_RSI_PID3(val)       bfin_write16(RSI_PID3, val)
+#define bfin_read_RSI_PID4()           bfin_read16(RSI_PID4)
+#define bfin_write_RSI_PID4(val)       bfin_write16(RSI_PID4, val)
+#define bfin_read_RSI_PID5()           bfin_read16(RSI_PID5)
+#define bfin_write_RSI_PID5(val)       bfin_write16(RSI_PID5, val)
+#define bfin_read_RSI_PID6()           bfin_read16(RSI_PID6)
+#define bfin_write_RSI_PID6(val)       bfin_write16(RSI_PID6, val)
+#define bfin_read_RSI_PID7()           bfin_read16(RSI_PID7)
+#define bfin_write_RSI_PID7(val)       bfin_write16(RSI_PID7, val)
+
+
+#endif /* _CDEF_BF518_H */
diff --git a/arch/blackfin/mach-bf518/include/mach/cdefBF51x_base.h b/arch/blackfin/mach-bf518/include/mach/cdefBF51x_base.h
new file mode 100644
index 000000000000..ee3d4733369c
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/cdefBF51x_base.h
@@ -0,0 +1,1208 @@
+/*
+ * File:         include/asm-blackfin/mach-bf518/cdefBF51x_base.h
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:
+ *
+ * Rev:
+ *
+ * Modified:
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, 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; see the file COPYING.
+ * If not, write to the Free Software Foundation,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef _CDEF_BF52X_H
+#define _CDEF_BF52X_H
+
+#include <asm/blackfin.h>
+
+#include "defBF51x_base.h"
+
+/* Include core specific register pointer definitions 								*/
+#include <asm/cdef_LPBlackfin.h>
+
+/* ==== begin from cdefBF534.h ==== */
+
+/* Clock and System Control	(0xFFC00000 - 0xFFC000FF)								*/
+#define bfin_read_PLL_CTL()			bfin_read16(PLL_CTL)
+#define bfin_read_PLL_DIV()			bfin_read16(PLL_DIV)
+#define bfin_write_PLL_DIV(val)			bfin_write16(PLL_DIV, val)
+#define bfin_read_VR_CTL()			bfin_read16(VR_CTL)
+#define bfin_read_PLL_STAT()			bfin_read16(PLL_STAT)
+#define bfin_write_PLL_STAT(val)		bfin_write16(PLL_STAT, val)
+#define bfin_read_PLL_LOCKCNT()			bfin_read16(PLL_LOCKCNT)
+#define bfin_write_PLL_LOCKCNT(val)		bfin_write16(PLL_LOCKCNT, val)
+#define bfin_read_CHIPID()			bfin_read32(CHIPID)
+#define bfin_write_CHIPID(val)			bfin_write32(CHIPID, val)
+
+
+/* System Interrupt Controller (0xFFC00100 - 0xFFC001FF)							*/
+#define bfin_read_SWRST()			bfin_read16(SWRST)
+#define bfin_write_SWRST(val)			bfin_write16(SWRST, val)
+#define bfin_read_SYSCR()			bfin_read16(SYSCR)
+#define bfin_write_SYSCR(val)			bfin_write16(SYSCR, val)
+
+#define bfin_read_SIC_RVECT()			bfin_read32(SIC_RVECT)
+#define bfin_write_SIC_RVECT(val)		bfin_write32(SIC_RVECT, val)
+#define bfin_read_SIC_IMASK0()			bfin_read32(SIC_IMASK0)
+#define bfin_write_SIC_IMASK0(val)		bfin_write32(SIC_IMASK0, val)
+#define bfin_read_SIC_IMASK(x)			bfin_read32(SIC_IMASK0 + (x << 6))
+#define bfin_write_SIC_IMASK(x, val)		bfin_write32((SIC_IMASK0 + (x << 6)), val)
+
+#define bfin_read_SIC_IAR0()			bfin_read32(SIC_IAR0)
+#define bfin_write_SIC_IAR0(val)		bfin_write32(SIC_IAR0, val)
+#define bfin_read_SIC_IAR1()			bfin_read32(SIC_IAR1)
+#define bfin_write_SIC_IAR1(val)		bfin_write32(SIC_IAR1, val)
+#define bfin_read_SIC_IAR2()			bfin_read32(SIC_IAR2)
+#define bfin_write_SIC_IAR2(val)		bfin_write32(SIC_IAR2, val)
+#define bfin_read_SIC_IAR3()			bfin_read32(SIC_IAR3)
+#define bfin_write_SIC_IAR3(val)		bfin_write32(SIC_IAR3, val)
+
+#define bfin_read_SIC_ISR0()			bfin_read32(SIC_ISR0)
+#define bfin_write_SIC_ISR0(val)		bfin_write32(SIC_ISR0, val)
+#define bfin_read_SIC_ISR(x)			bfin_read32(SIC_ISR0 + (x << 6))
+#define bfin_write_SIC_ISR(x, val)		bfin_write32((SIC_ISR0 + (x << 6)), val)
+
+#define bfin_read_SIC_IWR0()			bfin_read32(SIC_IWR0)
+#define bfin_write_SIC_IWR0(val)		bfin_write32(SIC_IWR0, val)
+#define bfin_read_SIC_IWR(x)			bfin_read32(SIC_IWR0 + (x << 6))
+#define bfin_write_SIC_IWR(x, val)		bfin_write32((SIC_IWR0 + (x << 6)), val)
+
+/* SIC Additions to ADSP-BF51x (0xFFC0014C - 0xFFC00162) */
+
+#define bfin_read_SIC_IMASK1()			bfin_read32(SIC_IMASK1)
+#define bfin_write_SIC_IMASK1(val)		bfin_write32(SIC_IMASK1, val)
+#define bfin_read_SIC_IAR4()			bfin_read32(SIC_IAR4)
+#define bfin_write_SIC_IAR4(val)		bfin_write32(SIC_IAR4, val)
+#define bfin_read_SIC_IAR5()			bfin_read32(SIC_IAR5)
+#define bfin_write_SIC_IAR5(val)		bfin_write32(SIC_IAR5, val)
+#define bfin_read_SIC_IAR6()			bfin_read32(SIC_IAR6)
+#define bfin_write_SIC_IAR6(val)		bfin_write32(SIC_IAR6, val)
+#define bfin_read_SIC_IAR7()			bfin_read32(SIC_IAR7)
+#define bfin_write_SIC_IAR7(val)		bfin_write32(SIC_IAR7, val)
+#define bfin_read_SIC_ISR1()			bfin_read32(SIC_ISR1)
+#define bfin_write_SIC_ISR1(val)		bfin_write32(SIC_ISR1, val)
+#define bfin_read_SIC_IWR1()			bfin_read32(SIC_IWR1)
+#define bfin_write_SIC_IWR1(val)		bfin_write32(SIC_IWR1, val)
+
+/* Watchdog Timer		(0xFFC00200 - 0xFFC002FF)									*/
+#define bfin_read_WDOG_CTL()			bfin_read16(WDOG_CTL)
+#define bfin_write_WDOG_CTL(val)		bfin_write16(WDOG_CTL, val)
+#define bfin_read_WDOG_CNT()			bfin_read32(WDOG_CNT)
+#define bfin_write_WDOG_CNT(val)		bfin_write32(WDOG_CNT, val)
+#define bfin_read_WDOG_STAT()			bfin_read32(WDOG_STAT)
+#define bfin_write_WDOG_STAT(val)		bfin_write32(WDOG_STAT, val)
+
+
+/* Real Time Clock		(0xFFC00300 - 0xFFC003FF)									*/
+#define bfin_read_RTC_STAT()			bfin_read32(RTC_STAT)
+#define bfin_write_RTC_STAT(val)		bfin_write32(RTC_STAT, val)
+#define bfin_read_RTC_ICTL()			bfin_read16(RTC_ICTL)
+#define bfin_write_RTC_ICTL(val)		bfin_write16(RTC_ICTL, val)
+#define bfin_read_RTC_ISTAT()			bfin_read16(RTC_ISTAT)
+#define bfin_write_RTC_ISTAT(val)		bfin_write16(RTC_ISTAT, val)
+#define bfin_read_RTC_SWCNT()			bfin_read16(RTC_SWCNT)
+#define bfin_write_RTC_SWCNT(val)		bfin_write16(RTC_SWCNT, val)
+#define bfin_read_RTC_ALARM()			bfin_read32(RTC_ALARM)
+#define bfin_write_RTC_ALARM(val)		bfin_write32(RTC_ALARM, val)
+#define bfin_read_RTC_FAST()			bfin_read16(RTC_FAST)
+#define bfin_write_RTC_FAST(val)		bfin_write16(RTC_FAST, val)
+#define bfin_read_RTC_PREN()			bfin_read16(RTC_PREN)
+#define bfin_write_RTC_PREN(val)		bfin_write16(RTC_PREN, val)
+
+
+/* UART0 Controller		(0xFFC00400 - 0xFFC004FF)									*/
+#define bfin_read_UART0_THR()			bfin_read16(UART0_THR)
+#define bfin_write_UART0_THR(val)		bfin_write16(UART0_THR, val)
+#define bfin_read_UART0_RBR()			bfin_read16(UART0_RBR)
+#define bfin_write_UART0_RBR(val)		bfin_write16(UART0_RBR, val)
+#define bfin_read_UART0_DLL()			bfin_read16(UART0_DLL)
+#define bfin_write_UART0_DLL(val)		bfin_write16(UART0_DLL, val)
+#define bfin_read_UART0_IER()			bfin_read16(UART0_IER)
+#define bfin_write_UART0_IER(val)		bfin_write16(UART0_IER, val)
+#define bfin_read_UART0_DLH()			bfin_read16(UART0_DLH)
+#define bfin_write_UART0_DLH(val)		bfin_write16(UART0_DLH, val)
+#define bfin_read_UART0_IIR()			bfin_read16(UART0_IIR)
+#define bfin_write_UART0_IIR(val)		bfin_write16(UART0_IIR, val)
+#define bfin_read_UART0_LCR()			bfin_read16(UART0_LCR)
+#define bfin_write_UART0_LCR(val)		bfin_write16(UART0_LCR, val)
+#define bfin_read_UART0_MCR()			bfin_read16(UART0_MCR)
+#define bfin_write_UART0_MCR(val)		bfin_write16(UART0_MCR, val)
+#define bfin_read_UART0_LSR()			bfin_read16(UART0_LSR)
+#define bfin_write_UART0_LSR(val)		bfin_write16(UART0_LSR, val)
+#define bfin_read_UART0_MSR()			bfin_read16(UART0_MSR)
+#define bfin_write_UART0_MSR(val)		bfin_write16(UART0_MSR, val)
+#define bfin_read_UART0_SCR()			bfin_read16(UART0_SCR)
+#define bfin_write_UART0_SCR(val)		bfin_write16(UART0_SCR, val)
+#define bfin_read_UART0_GCTL()			bfin_read16(UART0_GCTL)
+#define bfin_write_UART0_GCTL(val)		bfin_write16(UART0_GCTL, val)
+
+
+/* SPI Controller		(0xFFC00500 - 0xFFC005FF)									*/
+#define bfin_read_SPI_CTL()			bfin_read16(SPI_CTL)
+#define bfin_write_SPI_CTL(val)			bfin_write16(SPI_CTL, val)
+#define bfin_read_SPI_FLG()			bfin_read16(SPI_FLG)
+#define bfin_write_SPI_FLG(val)			bfin_write16(SPI_FLG, val)
+#define bfin_read_SPI_STAT()			bfin_read16(SPI_STAT)
+#define bfin_write_SPI_STAT(val)		bfin_write16(SPI_STAT, val)
+#define bfin_read_SPI_TDBR()			bfin_read16(SPI_TDBR)
+#define bfin_write_SPI_TDBR(val)		bfin_write16(SPI_TDBR, val)
+#define bfin_read_SPI_RDBR()			bfin_read16(SPI_RDBR)
+#define bfin_write_SPI_RDBR(val)		bfin_write16(SPI_RDBR, val)
+#define bfin_read_SPI_BAUD()			bfin_read16(SPI_BAUD)
+#define bfin_write_SPI_BAUD(val)		bfin_write16(SPI_BAUD, val)
+#define bfin_read_SPI_SHADOW()			bfin_read16(SPI_SHADOW)
+#define bfin_write_SPI_SHADOW(val)		bfin_write16(SPI_SHADOW, val)
+
+
+/* TIMER0-7 Registers		(0xFFC00600 - 0xFFC006FF)								*/
+#define bfin_read_TIMER0_CONFIG()		bfin_read16(TIMER0_CONFIG)
+#define bfin_write_TIMER0_CONFIG(val)		bfin_write16(TIMER0_CONFIG, val)
+#define bfin_read_TIMER0_COUNTER()		bfin_read32(TIMER0_COUNTER)
+#define bfin_write_TIMER0_COUNTER(val)		bfin_write32(TIMER0_COUNTER, val)
+#define bfin_read_TIMER0_PERIOD()		bfin_read32(TIMER0_PERIOD)
+#define bfin_write_TIMER0_PERIOD(val)		bfin_write32(TIMER0_PERIOD, val)
+#define bfin_read_TIMER0_WIDTH()		bfin_read32(TIMER0_WIDTH)
+#define bfin_write_TIMER0_WIDTH(val)		bfin_write32(TIMER0_WIDTH, val)
+
+#define bfin_read_TIMER1_CONFIG()		bfin_read16(TIMER1_CONFIG)
+#define bfin_write_TIMER1_CONFIG(val)		bfin_write16(TIMER1_CONFIG, val)
+#define bfin_read_TIMER1_COUNTER()		bfin_read32(TIMER1_COUNTER)
+#define bfin_write_TIMER1_COUNTER(val)		bfin_write32(TIMER1_COUNTER, val)
+#define bfin_read_TIMER1_PERIOD()		bfin_read32(TIMER1_PERIOD)
+#define bfin_write_TIMER1_PERIOD(val)		bfin_write32(TIMER1_PERIOD, val)
+#define bfin_read_TIMER1_WIDTH()		bfin_read32(TIMER1_WIDTH)
+#define bfin_write_TIMER1_WIDTH(val)		bfin_write32(TIMER1_WIDTH, val)
+
+#define bfin_read_TIMER2_CONFIG()		bfin_read16(TIMER2_CONFIG)
+#define bfin_write_TIMER2_CONFIG(val)		bfin_write16(TIMER2_CONFIG, val)
+#define bfin_read_TIMER2_COUNTER()		bfin_read32(TIMER2_COUNTER)
+#define bfin_write_TIMER2_COUNTER(val)		bfin_write32(TIMER2_COUNTER, val)
+#define bfin_read_TIMER2_PERIOD()		bfin_read32(TIMER2_PERIOD)
+#define bfin_write_TIMER2_PERIOD(val)		bfin_write32(TIMER2_PERIOD, val)
+#define bfin_read_TIMER2_WIDTH()		bfin_read32(TIMER2_WIDTH)
+#define bfin_write_TIMER2_WIDTH(val)		bfin_write32(TIMER2_WIDTH, val)
+
+#define bfin_read_TIMER3_CONFIG()		bfin_read16(TIMER3_CONFIG)
+#define bfin_write_TIMER3_CONFIG(val)		bfin_write16(TIMER3_CONFIG, val)
+#define bfin_read_TIMER3_COUNTER()		bfin_read32(TIMER3_COUNTER)
+#define bfin_write_TIMER3_COUNTER(val)		bfin_write32(TIMER3_COUNTER, val)
+#define bfin_read_TIMER3_PERIOD()		bfin_read32(TIMER3_PERIOD)
+#define bfin_write_TIMER3_PERIOD(val)		bfin_write32(TIMER3_PERIOD, val)
+#define bfin_read_TIMER3_WIDTH()		bfin_read32(TIMER3_WIDTH)
+#define bfin_write_TIMER3_WIDTH(val)		bfin_write32(TIMER3_WIDTH, val)
+
+#define bfin_read_TIMER4_CONFIG()		bfin_read16(TIMER4_CONFIG)
+#define bfin_write_TIMER4_CONFIG(val)		bfin_write16(TIMER4_CONFIG, val)
+#define bfin_read_TIMER4_COUNTER()		bfin_read32(TIMER4_COUNTER)
+#define bfin_write_TIMER4_COUNTER(val)		bfin_write32(TIMER4_COUNTER, val)
+#define bfin_read_TIMER4_PERIOD()		bfin_read32(TIMER4_PERIOD)
+#define bfin_write_TIMER4_PERIOD(val)		bfin_write32(TIMER4_PERIOD, val)
+#define bfin_read_TIMER4_WIDTH()		bfin_read32(TIMER4_WIDTH)
+#define bfin_write_TIMER4_WIDTH(val)		bfin_write32(TIMER4_WIDTH, val)
+
+#define bfin_read_TIMER5_CONFIG()		bfin_read16(TIMER5_CONFIG)
+#define bfin_write_TIMER5_CONFIG(val)		bfin_write16(TIMER5_CONFIG, val)
+#define bfin_read_TIMER5_COUNTER()		bfin_read32(TIMER5_COUNTER)
+#define bfin_write_TIMER5_COUNTER(val)		bfin_write32(TIMER5_COUNTER, val)
+#define bfin_read_TIMER5_PERIOD()		bfin_read32(TIMER5_PERIOD)
+#define bfin_write_TIMER5_PERIOD(val)		bfin_write32(TIMER5_PERIOD, val)
+#define bfin_read_TIMER5_WIDTH()		bfin_read32(TIMER5_WIDTH)
+#define bfin_write_TIMER5_WIDTH(val)		bfin_write32(TIMER5_WIDTH, val)
+
+#define bfin_read_TIMER6_CONFIG()		bfin_read16(TIMER6_CONFIG)
+#define bfin_write_TIMER6_CONFIG(val)		bfin_write16(TIMER6_CONFIG, val)
+#define bfin_read_TIMER6_COUNTER()		bfin_read32(TIMER6_COUNTER)
+#define bfin_write_TIMER6_COUNTER(val)		bfin_write32(TIMER6_COUNTER, val)
+#define bfin_read_TIMER6_PERIOD()		bfin_read32(TIMER6_PERIOD)
+#define bfin_write_TIMER6_PERIOD(val)		bfin_write32(TIMER6_PERIOD, val)
+#define bfin_read_TIMER6_WIDTH()		bfin_read32(TIMER6_WIDTH)
+#define bfin_write_TIMER6_WIDTH(val)		bfin_write32(TIMER6_WIDTH, val)
+
+#define bfin_read_TIMER7_CONFIG()		bfin_read16(TIMER7_CONFIG)
+#define bfin_write_TIMER7_CONFIG(val)		bfin_write16(TIMER7_CONFIG, val)
+#define bfin_read_TIMER7_COUNTER()		bfin_read32(TIMER7_COUNTER)
+#define bfin_write_TIMER7_COUNTER(val)		bfin_write32(TIMER7_COUNTER, val)
+#define bfin_read_TIMER7_PERIOD()		bfin_read32(TIMER7_PERIOD)
+#define bfin_write_TIMER7_PERIOD(val)		bfin_write32(TIMER7_PERIOD, val)
+#define bfin_read_TIMER7_WIDTH()		bfin_read32(TIMER7_WIDTH)
+#define bfin_write_TIMER7_WIDTH(val)		bfin_write32(TIMER7_WIDTH, val)
+
+#define bfin_read_TIMER_ENABLE()		bfin_read16(TIMER_ENABLE)
+#define bfin_write_TIMER_ENABLE(val)		bfin_write16(TIMER_ENABLE, val)
+#define bfin_read_TIMER_DISABLE()		bfin_read16(TIMER_DISABLE)
+#define bfin_write_TIMER_DISABLE(val)		bfin_write16(TIMER_DISABLE, val)
+#define bfin_read_TIMER_STATUS()		bfin_read32(TIMER_STATUS)
+#define bfin_write_TIMER_STATUS(val)		bfin_write32(TIMER_STATUS, val)
+
+
+/* General Purpose I/O Port F (0xFFC00700 - 0xFFC007FF)								*/
+#define bfin_read_PORTFIO()			bfin_read16(PORTFIO)
+#define bfin_write_PORTFIO(val)			bfin_write16(PORTFIO, val)
+#define bfin_read_PORTFIO_CLEAR()		bfin_read16(PORTFIO_CLEAR)
+#define bfin_write_PORTFIO_CLEAR(val)		bfin_write16(PORTFIO_CLEAR, val)
+#define bfin_read_PORTFIO_SET()			bfin_read16(PORTFIO_SET)
+#define bfin_write_PORTFIO_SET(val)		bfin_write16(PORTFIO_SET, val)
+#define bfin_read_PORTFIO_TOGGLE()		bfin_read16(PORTFIO_TOGGLE)
+#define bfin_write_PORTFIO_TOGGLE(val)		bfin_write16(PORTFIO_TOGGLE, val)
+#define bfin_read_PORTFIO_MASKA()		bfin_read16(PORTFIO_MASKA)
+#define bfin_write_PORTFIO_MASKA(val)		bfin_write16(PORTFIO_MASKA, val)
+#define bfin_read_PORTFIO_MASKA_CLEAR()		bfin_read16(PORTFIO_MASKA_CLEAR)
+#define bfin_write_PORTFIO_MASKA_CLEAR(val)	bfin_write16(PORTFIO_MASKA_CLEAR, val)
+#define bfin_read_PORTFIO_MASKA_SET()		bfin_read16(PORTFIO_MASKA_SET)
+#define bfin_write_PORTFIO_MASKA_SET(val)	bfin_write16(PORTFIO_MASKA_SET, val)
+#define bfin_read_PORTFIO_MASKA_TOGGLE()	bfin_read16(PORTFIO_MASKA_TOGGLE)
+#define bfin_write_PORTFIO_MASKA_TOGGLE(val)	bfin_write16(PORTFIO_MASKA_TOGGLE, val)
+#define bfin_read_PORTFIO_MASKB()		bfin_read16(PORTFIO_MASKB)
+#define bfin_write_PORTFIO_MASKB(val)		bfin_write16(PORTFIO_MASKB, val)
+#define bfin_read_PORTFIO_MASKB_CLEAR()		bfin_read16(PORTFIO_MASKB_CLEAR)
+#define bfin_write_PORTFIO_MASKB_CLEAR(val)	bfin_write16(PORTFIO_MASKB_CLEAR, val)
+#define bfin_read_PORTFIO_MASKB_SET()		bfin_read16(PORTFIO_MASKB_SET)
+#define bfin_write_PORTFIO_MASKB_SET(val)	bfin_write16(PORTFIO_MASKB_SET, val)
+#define bfin_read_PORTFIO_MASKB_TOGGLE()	bfin_read16(PORTFIO_MASKB_TOGGLE)
+#define bfin_write_PORTFIO_MASKB_TOGGLE(val)	bfin_write16(PORTFIO_MASKB_TOGGLE, val)
+#define bfin_read_PORTFIO_DIR()			bfin_read16(PORTFIO_DIR)
+#define bfin_write_PORTFIO_DIR(val)		bfin_write16(PORTFIO_DIR, val)
+#define bfin_read_PORTFIO_POLAR()		bfin_read16(PORTFIO_POLAR)
+#define bfin_write_PORTFIO_POLAR(val)		bfin_write16(PORTFIO_POLAR, val)
+#define bfin_read_PORTFIO_EDGE()		bfin_read16(PORTFIO_EDGE)
+#define bfin_write_PORTFIO_EDGE(val)		bfin_write16(PORTFIO_EDGE, val)
+#define bfin_read_PORTFIO_BOTH()		bfin_read16(PORTFIO_BOTH)
+#define bfin_write_PORTFIO_BOTH(val)		bfin_write16(PORTFIO_BOTH, val)
+#define bfin_read_PORTFIO_INEN()		bfin_read16(PORTFIO_INEN)
+#define bfin_write_PORTFIO_INEN(val)		bfin_write16(PORTFIO_INEN, val)
+
+
+/* SPORT0 Controller		(0xFFC00800 - 0xFFC008FF)								*/
+#define bfin_read_SPORT0_TCR1()			bfin_read16(SPORT0_TCR1)
+#define bfin_write_SPORT0_TCR1(val)		bfin_write16(SPORT0_TCR1, val)
+#define bfin_read_SPORT0_TCR2()			bfin_read16(SPORT0_TCR2)
+#define bfin_write_SPORT0_TCR2(val)		bfin_write16(SPORT0_TCR2, val)
+#define bfin_read_SPORT0_TCLKDIV()		bfin_read16(SPORT0_TCLKDIV)
+#define bfin_write_SPORT0_TCLKDIV(val)		bfin_write16(SPORT0_TCLKDIV, val)
+#define bfin_read_SPORT0_TFSDIV()		bfin_read16(SPORT0_TFSDIV)
+#define bfin_write_SPORT0_TFSDIV(val)		bfin_write16(SPORT0_TFSDIV, val)
+#define bfin_read_SPORT0_TX()			bfin_read32(SPORT0_TX)
+#define bfin_write_SPORT0_TX(val)		bfin_write32(SPORT0_TX, val)
+#define bfin_read_SPORT0_RX()			bfin_read32(SPORT0_RX)
+#define bfin_write_SPORT0_RX(val)		bfin_write32(SPORT0_RX, val)
+#define bfin_read_SPORT0_TX32()			bfin_read32(SPORT0_TX32)
+#define bfin_write_SPORT0_TX32(val)		bfin_write32(SPORT0_TX32, val)
+#define bfin_read_SPORT0_RX32()			bfin_read32(SPORT0_RX32)
+#define bfin_write_SPORT0_RX32(val)		bfin_write32(SPORT0_RX32, val)
+#define bfin_read_SPORT0_TX16()			bfin_read16(SPORT0_TX16)
+#define bfin_write_SPORT0_TX16(val)		bfin_write16(SPORT0_TX16, val)
+#define bfin_read_SPORT0_RX16()			bfin_read16(SPORT0_RX16)
+#define bfin_write_SPORT0_RX16(val)		bfin_write16(SPORT0_RX16, val)
+#define bfin_read_SPORT0_RCR1()			bfin_read16(SPORT0_RCR1)
+#define bfin_write_SPORT0_RCR1(val)		bfin_write16(SPORT0_RCR1, val)
+#define bfin_read_SPORT0_RCR2()			bfin_read16(SPORT0_RCR2)
+#define bfin_write_SPORT0_RCR2(val)		bfin_write16(SPORT0_RCR2, val)
+#define bfin_read_SPORT0_RCLKDIV()		bfin_read16(SPORT0_RCLKDIV)
+#define bfin_write_SPORT0_RCLKDIV(val)		bfin_write16(SPORT0_RCLKDIV, val)
+#define bfin_read_SPORT0_RFSDIV()		bfin_read16(SPORT0_RFSDIV)
+#define bfin_write_SPORT0_RFSDIV(val)		bfin_write16(SPORT0_RFSDIV, val)
+#define bfin_read_SPORT0_STAT()			bfin_read16(SPORT0_STAT)
+#define bfin_write_SPORT0_STAT(val)		bfin_write16(SPORT0_STAT, val)
+#define bfin_read_SPORT0_CHNL()			bfin_read16(SPORT0_CHNL)
+#define bfin_write_SPORT0_CHNL(val)		bfin_write16(SPORT0_CHNL, val)
+#define bfin_read_SPORT0_MCMC1()		bfin_read16(SPORT0_MCMC1)
+#define bfin_write_SPORT0_MCMC1(val)		bfin_write16(SPORT0_MCMC1, val)
+#define bfin_read_SPORT0_MCMC2()		bfin_read16(SPORT0_MCMC2)
+#define bfin_write_SPORT0_MCMC2(val)		bfin_write16(SPORT0_MCMC2, val)
+#define bfin_read_SPORT0_MTCS0()		bfin_read32(SPORT0_MTCS0)
+#define bfin_write_SPORT0_MTCS0(val)		bfin_write32(SPORT0_MTCS0, val)
+#define bfin_read_SPORT0_MTCS1()		bfin_read32(SPORT0_MTCS1)
+#define bfin_write_SPORT0_MTCS1(val)		bfin_write32(SPORT0_MTCS1, val)
+#define bfin_read_SPORT0_MTCS2()		bfin_read32(SPORT0_MTCS2)
+#define bfin_write_SPORT0_MTCS2(val)		bfin_write32(SPORT0_MTCS2, val)
+#define bfin_read_SPORT0_MTCS3()		bfin_read32(SPORT0_MTCS3)
+#define bfin_write_SPORT0_MTCS3(val)		bfin_write32(SPORT0_MTCS3, val)
+#define bfin_read_SPORT0_MRCS0()		bfin_read32(SPORT0_MRCS0)
+#define bfin_write_SPORT0_MRCS0(val)		bfin_write32(SPORT0_MRCS0, val)
+#define bfin_read_SPORT0_MRCS1()		bfin_read32(SPORT0_MRCS1)
+#define bfin_write_SPORT0_MRCS1(val)		bfin_write32(SPORT0_MRCS1, val)
+#define bfin_read_SPORT0_MRCS2()		bfin_read32(SPORT0_MRCS2)
+#define bfin_write_SPORT0_MRCS2(val)		bfin_write32(SPORT0_MRCS2, val)
+#define bfin_read_SPORT0_MRCS3()		bfin_read32(SPORT0_MRCS3)
+#define bfin_write_SPORT0_MRCS3(val)		bfin_write32(SPORT0_MRCS3, val)
+
+
+/* SPORT1 Controller		(0xFFC00900 - 0xFFC009FF)								*/
+#define bfin_read_SPORT1_TCR1()			bfin_read16(SPORT1_TCR1)
+#define bfin_write_SPORT1_TCR1(val)		bfin_write16(SPORT1_TCR1, val)
+#define bfin_read_SPORT1_TCR2()			bfin_read16(SPORT1_TCR2)
+#define bfin_write_SPORT1_TCR2(val)		bfin_write16(SPORT1_TCR2, val)
+#define bfin_read_SPORT1_TCLKDIV()		bfin_read16(SPORT1_TCLKDIV)
+#define bfin_write_SPORT1_TCLKDIV(val)		bfin_write16(SPORT1_TCLKDIV, val)
+#define bfin_read_SPORT1_TFSDIV()		bfin_read16(SPORT1_TFSDIV)
+#define bfin_write_SPORT1_TFSDIV(val)		bfin_write16(SPORT1_TFSDIV, val)
+#define bfin_read_SPORT1_TX()			bfin_read32(SPORT1_TX)
+#define bfin_write_SPORT1_TX(val)		bfin_write32(SPORT1_TX, val)
+#define bfin_read_SPORT1_RX()			bfin_read32(SPORT1_RX)
+#define bfin_write_SPORT1_RX(val)		bfin_write32(SPORT1_RX, val)
+#define bfin_read_SPORT1_TX32()			bfin_read32(SPORT1_TX32)
+#define bfin_write_SPORT1_TX32(val)		bfin_write32(SPORT1_TX32, val)
+#define bfin_read_SPORT1_RX32()			bfin_read32(SPORT1_RX32)
+#define bfin_write_SPORT1_RX32(val)		bfin_write32(SPORT1_RX32, val)
+#define bfin_read_SPORT1_TX16()			bfin_read16(SPORT1_TX16)
+#define bfin_write_SPORT1_TX16(val)		bfin_write16(SPORT1_TX16, val)
+#define bfin_read_SPORT1_RX16()			bfin_read16(SPORT1_RX16)
+#define bfin_write_SPORT1_RX16(val)		bfin_write16(SPORT1_RX16, val)
+#define bfin_read_SPORT1_RCR1()			bfin_read16(SPORT1_RCR1)
+#define bfin_write_SPORT1_RCR1(val)		bfin_write16(SPORT1_RCR1, val)
+#define bfin_read_SPORT1_RCR2()			bfin_read16(SPORT1_RCR2)
+#define bfin_write_SPORT1_RCR2(val)		bfin_write16(SPORT1_RCR2, val)
+#define bfin_read_SPORT1_RCLKDIV()		bfin_read16(SPORT1_RCLKDIV)
+#define bfin_write_SPORT1_RCLKDIV(val)		bfin_write16(SPORT1_RCLKDIV, val)
+#define bfin_read_SPORT1_RFSDIV()		bfin_read16(SPORT1_RFSDIV)
+#define bfin_write_SPORT1_RFSDIV(val)		bfin_write16(SPORT1_RFSDIV, val)
+#define bfin_read_SPORT1_STAT()			bfin_read16(SPORT1_STAT)
+#define bfin_write_SPORT1_STAT(val)		bfin_write16(SPORT1_STAT, val)
+#define bfin_read_SPORT1_CHNL()			bfin_read16(SPORT1_CHNL)
+#define bfin_write_SPORT1_CHNL(val)		bfin_write16(SPORT1_CHNL, val)
+#define bfin_read_SPORT1_MCMC1()		bfin_read16(SPORT1_MCMC1)
+#define bfin_write_SPORT1_MCMC1(val)		bfin_write16(SPORT1_MCMC1, val)
+#define bfin_read_SPORT1_MCMC2()		bfin_read16(SPORT1_MCMC2)
+#define bfin_write_SPORT1_MCMC2(val)		bfin_write16(SPORT1_MCMC2, val)
+#define bfin_read_SPORT1_MTCS0()		bfin_read32(SPORT1_MTCS0)
+#define bfin_write_SPORT1_MTCS0(val)		bfin_write32(SPORT1_MTCS0, val)
+#define bfin_read_SPORT1_MTCS1()		bfin_read32(SPORT1_MTCS1)
+#define bfin_write_SPORT1_MTCS1(val)		bfin_write32(SPORT1_MTCS1, val)
+#define bfin_read_SPORT1_MTCS2()		bfin_read32(SPORT1_MTCS2)
+#define bfin_write_SPORT1_MTCS2(val)		bfin_write32(SPORT1_MTCS2, val)
+#define bfin_read_SPORT1_MTCS3()		bfin_read32(SPORT1_MTCS3)
+#define bfin_write_SPORT1_MTCS3(val)		bfin_write32(SPORT1_MTCS3, val)
+#define bfin_read_SPORT1_MRCS0()		bfin_read32(SPORT1_MRCS0)
+#define bfin_write_SPORT1_MRCS0(val)		bfin_write32(SPORT1_MRCS0, val)
+#define bfin_read_SPORT1_MRCS1()		bfin_read32(SPORT1_MRCS1)
+#define bfin_write_SPORT1_MRCS1(val)		bfin_write32(SPORT1_MRCS1, val)
+#define bfin_read_SPORT1_MRCS2()		bfin_read32(SPORT1_MRCS2)
+#define bfin_write_SPORT1_MRCS2(val)		bfin_write32(SPORT1_MRCS2, val)
+#define bfin_read_SPORT1_MRCS3()		bfin_read32(SPORT1_MRCS3)
+#define bfin_write_SPORT1_MRCS3(val)		bfin_write32(SPORT1_MRCS3, val)
+
+
+/* External Bus Interface Unit (0xFFC00A00 - 0xFFC00AFF)							*/
+#define bfin_read_EBIU_AMGCTL()			bfin_read16(EBIU_AMGCTL)
+#define bfin_write_EBIU_AMGCTL(val)		bfin_write16(EBIU_AMGCTL, val)
+#define bfin_read_EBIU_AMBCTL0()		bfin_read32(EBIU_AMBCTL0)
+#define bfin_write_EBIU_AMBCTL0(val)		bfin_write32(EBIU_AMBCTL0, val)
+#define bfin_read_EBIU_AMBCTL1()		bfin_read32(EBIU_AMBCTL1)
+#define bfin_write_EBIU_AMBCTL1(val)		bfin_write32(EBIU_AMBCTL1, val)
+#define bfin_read_EBIU_SDGCTL()			bfin_read32(EBIU_SDGCTL)
+#define bfin_write_EBIU_SDGCTL(val)		bfin_write32(EBIU_SDGCTL, val)
+#define bfin_read_EBIU_SDBCTL()			bfin_read16(EBIU_SDBCTL)
+#define bfin_write_EBIU_SDBCTL(val)		bfin_write16(EBIU_SDBCTL, val)
+#define bfin_read_EBIU_SDRRC()			bfin_read16(EBIU_SDRRC)
+#define bfin_write_EBIU_SDRRC(val)		bfin_write16(EBIU_SDRRC, val)
+#define bfin_read_EBIU_SDSTAT()			bfin_read16(EBIU_SDSTAT)
+#define bfin_write_EBIU_SDSTAT(val)		bfin_write16(EBIU_SDSTAT, val)
+
+
+/* DMA Traffic Control Registers													*/
+#define bfin_read_DMA_TC_PER()			bfin_read16(DMA_TC_PER)
+#define bfin_write_DMA_TC_PER(val)		bfin_write16(DMA_TC_PER, val)
+#define bfin_read_DMA_TC_CNT()			bfin_read16(DMA_TC_CNT)
+#define bfin_write_DMA_TC_CNT(val)		bfin_write16(DMA_TC_CNT, val)
+
+/* Alternate deprecated register names (below) provided for backwards code compatibility */
+#define bfin_read_DMA_TCPER()			bfin_read16(DMA_TCPER)
+#define bfin_write_DMA_TCPER(val)		bfin_write16(DMA_TCPER, val)
+#define bfin_read_DMA_TCCNT()			bfin_read16(DMA_TCCNT)
+#define bfin_write_DMA_TCCNT(val)		bfin_write16(DMA_TCCNT, val)
+
+/* DMA Controller																	*/
+#define bfin_read_DMA0_CONFIG()			bfin_read16(DMA0_CONFIG)
+#define bfin_write_DMA0_CONFIG(val)		bfin_write16(DMA0_CONFIG, val)
+#define bfin_read_DMA0_NEXT_DESC_PTR()		bfin_read32(DMA0_NEXT_DESC_PTR)
+#define bfin_write_DMA0_NEXT_DESC_PTR(val)	bfin_write32(DMA0_NEXT_DESC_PTR, val)
+#define bfin_read_DMA0_START_ADDR()		bfin_read32(DMA0_START_ADDR)
+#define bfin_write_DMA0_START_ADDR(val)		bfin_write32(DMA0_START_ADDR, val)
+#define bfin_read_DMA0_X_COUNT()		bfin_read16(DMA0_X_COUNT)
+#define bfin_write_DMA0_X_COUNT(val)		bfin_write16(DMA0_X_COUNT, val)
+#define bfin_read_DMA0_Y_COUNT()		bfin_read16(DMA0_Y_COUNT)
+#define bfin_write_DMA0_Y_COUNT(val)		bfin_write16(DMA0_Y_COUNT, val)
+#define bfin_read_DMA0_X_MODIFY()		bfin_read16(DMA0_X_MODIFY)
+#define bfin_write_DMA0_X_MODIFY(val)		bfin_write16(DMA0_X_MODIFY, val)
+#define bfin_read_DMA0_Y_MODIFY()		bfin_read16(DMA0_Y_MODIFY)
+#define bfin_write_DMA0_Y_MODIFY(val)		bfin_write16(DMA0_Y_MODIFY, val)
+#define bfin_read_DMA0_CURR_DESC_PTR()		bfin_read32(DMA0_CURR_DESC_PTR)
+#define bfin_write_DMA0_CURR_DESC_PTR(val)	bfin_write32(DMA0_CURR_DESC_PTR, val)
+#define bfin_read_DMA0_CURR_ADDR()		bfin_read32(DMA0_CURR_ADDR)
+#define bfin_write_DMA0_CURR_ADDR(val)		bfin_write32(DMA0_CURR_ADDR, val)
+#define bfin_read_DMA0_CURR_X_COUNT()		bfin_read16(DMA0_CURR_X_COUNT)
+#define bfin_write_DMA0_CURR_X_COUNT(val)	bfin_write16(DMA0_CURR_X_COUNT, val)
+#define bfin_read_DMA0_CURR_Y_COUNT()		bfin_read16(DMA0_CURR_Y_COUNT)
+#define bfin_write_DMA0_CURR_Y_COUNT(val)	bfin_write16(DMA0_CURR_Y_COUNT, val)
+#define bfin_read_DMA0_IRQ_STATUS()		bfin_read16(DMA0_IRQ_STATUS)
+#define bfin_write_DMA0_IRQ_STATUS(val)		bfin_write16(DMA0_IRQ_STATUS, val)
+#define bfin_read_DMA0_PERIPHERAL_MAP()		bfin_read16(DMA0_PERIPHERAL_MAP)
+#define bfin_write_DMA0_PERIPHERAL_MAP(val)	bfin_write16(DMA0_PERIPHERAL_MAP, val)
+
+#define bfin_read_DMA1_CONFIG()			bfin_read16(DMA1_CONFIG)
+#define bfin_write_DMA1_CONFIG(val)		bfin_write16(DMA1_CONFIG, val)
+#define bfin_read_DMA1_NEXT_DESC_PTR()		bfin_read32(DMA1_NEXT_DESC_PTR)
+#define bfin_write_DMA1_NEXT_DESC_PTR(val)	bfin_write32(DMA1_NEXT_DESC_PTR, val)
+#define bfin_read_DMA1_START_ADDR()		bfin_read32(DMA1_START_ADDR)
+#define bfin_write_DMA1_START_ADDR(val)		bfin_write32(DMA1_START_ADDR, val)
+#define bfin_read_DMA1_X_COUNT()		bfin_read16(DMA1_X_COUNT)
+#define bfin_write_DMA1_X_COUNT(val)		bfin_write16(DMA1_X_COUNT, val)
+#define bfin_read_DMA1_Y_COUNT()		bfin_read16(DMA1_Y_COUNT)
+#define bfin_write_DMA1_Y_COUNT(val)		bfin_write16(DMA1_Y_COUNT, val)
+#define bfin_read_DMA1_X_MODIFY()		bfin_read16(DMA1_X_MODIFY)
+#define bfin_write_DMA1_X_MODIFY(val)		bfin_write16(DMA1_X_MODIFY, val)
+#define bfin_read_DMA1_Y_MODIFY()		bfin_read16(DMA1_Y_MODIFY)
+#define bfin_write_DMA1_Y_MODIFY(val)		bfin_write16(DMA1_Y_MODIFY, val)
+#define bfin_read_DMA1_CURR_DESC_PTR()		bfin_read32(DMA1_CURR_DESC_PTR)
+#define bfin_write_DMA1_CURR_DESC_PTR(val)	bfin_write32(DMA1_CURR_DESC_PTR, val)
+#define bfin_read_DMA1_CURR_ADDR()		bfin_read32(DMA1_CURR_ADDR)
+#define bfin_write_DMA1_CURR_ADDR(val)		bfin_write32(DMA1_CURR_ADDR, val)
+#define bfin_read_DMA1_CURR_X_COUNT()		bfin_read16(DMA1_CURR_X_COUNT)
+#define bfin_write_DMA1_CURR_X_COUNT(val)	bfin_write16(DMA1_CURR_X_COUNT, val)
+#define bfin_read_DMA1_CURR_Y_COUNT()		bfin_read16(DMA1_CURR_Y_COUNT)
+#define bfin_write_DMA1_CURR_Y_COUNT(val)	bfin_write16(DMA1_CURR_Y_COUNT, val)
+#define bfin_read_DMA1_IRQ_STATUS()		bfin_read16(DMA1_IRQ_STATUS)
+#define bfin_write_DMA1_IRQ_STATUS(val)		bfin_write16(DMA1_IRQ_STATUS, val)
+#define bfin_read_DMA1_PERIPHERAL_MAP()		bfin_read16(DMA1_PERIPHERAL_MAP)
+#define bfin_write_DMA1_PERIPHERAL_MAP(val)	bfin_write16(DMA1_PERIPHERAL_MAP, val)
+
+#define bfin_read_DMA2_CONFIG()			bfin_read16(DMA2_CONFIG)
+#define bfin_write_DMA2_CONFIG(val)		bfin_write16(DMA2_CONFIG, val)
+#define bfin_read_DMA2_NEXT_DESC_PTR()		bfin_read32(DMA2_NEXT_DESC_PTR)
+#define bfin_write_DMA2_NEXT_DESC_PTR(val)	bfin_write32(DMA2_NEXT_DESC_PTR, val)
+#define bfin_read_DMA2_START_ADDR()		bfin_read32(DMA2_START_ADDR)
+#define bfin_write_DMA2_START_ADDR(val)		bfin_write32(DMA2_START_ADDR, val)
+#define bfin_read_DMA2_X_COUNT()		bfin_read16(DMA2_X_COUNT)
+#define bfin_write_DMA2_X_COUNT(val)		bfin_write16(DMA2_X_COUNT, val)
+#define bfin_read_DMA2_Y_COUNT()		bfin_read16(DMA2_Y_COUNT)
+#define bfin_write_DMA2_Y_COUNT(val)		bfin_write16(DMA2_Y_COUNT, val)
+#define bfin_read_DMA2_X_MODIFY()		bfin_read16(DMA2_X_MODIFY)
+#define bfin_write_DMA2_X_MODIFY(val)		bfin_write16(DMA2_X_MODIFY, val)
+#define bfin_read_DMA2_Y_MODIFY()		bfin_read16(DMA2_Y_MODIFY)
+#define bfin_write_DMA2_Y_MODIFY(val)		bfin_write16(DMA2_Y_MODIFY, val)
+#define bfin_read_DMA2_CURR_DESC_PTR()		bfin_read32(DMA2_CURR_DESC_PTR)
+#define bfin_write_DMA2_CURR_DESC_PTR(val)	bfin_write32(DMA2_CURR_DESC_PTR, val)
+#define bfin_read_DMA2_CURR_ADDR()		bfin_read32(DMA2_CURR_ADDR)
+#define bfin_write_DMA2_CURR_ADDR(val)		bfin_write32(DMA2_CURR_ADDR, val)
+#define bfin_read_DMA2_CURR_X_COUNT()		bfin_read16(DMA2_CURR_X_COUNT)
+#define bfin_write_DMA2_CURR_X_COUNT(val)	bfin_write16(DMA2_CURR_X_COUNT, val)
+#define bfin_read_DMA2_CURR_Y_COUNT()		bfin_read16(DMA2_CURR_Y_COUNT)
+#define bfin_write_DMA2_CURR_Y_COUNT(val)	bfin_write16(DMA2_CURR_Y_COUNT, val)
+#define bfin_read_DMA2_IRQ_STATUS()		bfin_read16(DMA2_IRQ_STATUS)
+#define bfin_write_DMA2_IRQ_STATUS(val)		bfin_write16(DMA2_IRQ_STATUS, val)
+#define bfin_read_DMA2_PERIPHERAL_MAP()		bfin_read16(DMA2_PERIPHERAL_MAP)
+#define bfin_write_DMA2_PERIPHERAL_MAP(val)	bfin_write16(DMA2_PERIPHERAL_MAP, val)
+
+#define bfin_read_DMA3_CONFIG()			bfin_read16(DMA3_CONFIG)
+#define bfin_write_DMA3_CONFIG(val)		bfin_write16(DMA3_CONFIG, val)
+#define bfin_read_DMA3_NEXT_DESC_PTR()		bfin_read32(DMA3_NEXT_DESC_PTR)
+#define bfin_write_DMA3_NEXT_DESC_PTR(val)	bfin_write32(DMA3_NEXT_DESC_PTR, val)
+#define bfin_read_DMA3_START_ADDR()		bfin_read32(DMA3_START_ADDR)
+#define bfin_write_DMA3_START_ADDR(val)		bfin_write32(DMA3_START_ADDR, val)
+#define bfin_read_DMA3_X_COUNT()		bfin_read16(DMA3_X_COUNT)
+#define bfin_write_DMA3_X_COUNT(val)		bfin_write16(DMA3_X_COUNT, val)
+#define bfin_read_DMA3_Y_COUNT()		bfin_read16(DMA3_Y_COUNT)
+#define bfin_write_DMA3_Y_COUNT(val)		bfin_write16(DMA3_Y_COUNT, val)
+#define bfin_read_DMA3_X_MODIFY()		bfin_read16(DMA3_X_MODIFY)
+#define bfin_write_DMA3_X_MODIFY(val)		bfin_write16(DMA3_X_MODIFY, val)
+#define bfin_read_DMA3_Y_MODIFY()		bfin_read16(DMA3_Y_MODIFY)
+#define bfin_write_DMA3_Y_MODIFY(val)		bfin_write16(DMA3_Y_MODIFY, val)
+#define bfin_read_DMA3_CURR_DESC_PTR()		bfin_read32(DMA3_CURR_DESC_PTR)
+#define bfin_write_DMA3_CURR_DESC_PTR(val)	bfin_write32(DMA3_CURR_DESC_PTR, val)
+#define bfin_read_DMA3_CURR_ADDR()		bfin_read32(DMA3_CURR_ADDR)
+#define bfin_write_DMA3_CURR_ADDR(val)		bfin_write32(DMA3_CURR_ADDR, val)
+#define bfin_read_DMA3_CURR_X_COUNT()		bfin_read16(DMA3_CURR_X_COUNT)
+#define bfin_write_DMA3_CURR_X_COUNT(val)	bfin_write16(DMA3_CURR_X_COUNT, val)
+#define bfin_read_DMA3_CURR_Y_COUNT()		bfin_read16(DMA3_CURR_Y_COUNT)
+#define bfin_write_DMA3_CURR_Y_COUNT(val)	bfin_write16(DMA3_CURR_Y_COUNT, val)
+#define bfin_read_DMA3_IRQ_STATUS()		bfin_read16(DMA3_IRQ_STATUS)
+#define bfin_write_DMA3_IRQ_STATUS(val)		bfin_write16(DMA3_IRQ_STATUS, val)
+#define bfin_read_DMA3_PERIPHERAL_MAP()		bfin_read16(DMA3_PERIPHERAL_MAP)
+#define bfin_write_DMA3_PERIPHERAL_MAP(val)	bfin_write16(DMA3_PERIPHERAL_MAP, val)
+
+#define bfin_read_DMA4_CONFIG()			bfin_read16(DMA4_CONFIG)
+#define bfin_write_DMA4_CONFIG(val)		bfin_write16(DMA4_CONFIG, val)
+#define bfin_read_DMA4_NEXT_DESC_PTR()		bfin_read32(DMA4_NEXT_DESC_PTR)
+#define bfin_write_DMA4_NEXT_DESC_PTR(val)	bfin_write32(DMA4_NEXT_DESC_PTR, val)
+#define bfin_read_DMA4_START_ADDR()		bfin_read32(DMA4_START_ADDR)
+#define bfin_write_DMA4_START_ADDR(val)		bfin_write32(DMA4_START_ADDR, val)
+#define bfin_read_DMA4_X_COUNT()		bfin_read16(DMA4_X_COUNT)
+#define bfin_write_DMA4_X_COUNT(val)		bfin_write16(DMA4_X_COUNT, val)
+#define bfin_read_DMA4_Y_COUNT()		bfin_read16(DMA4_Y_COUNT)
+#define bfin_write_DMA4_Y_COUNT(val)		bfin_write16(DMA4_Y_COUNT, val)
+#define bfin_read_DMA4_X_MODIFY()		bfin_read16(DMA4_X_MODIFY)
+#define bfin_write_DMA4_X_MODIFY(val)		bfin_write16(DMA4_X_MODIFY, val)
+#define bfin_read_DMA4_Y_MODIFY()		bfin_read16(DMA4_Y_MODIFY)
+#define bfin_write_DMA4_Y_MODIFY(val)		bfin_write16(DMA4_Y_MODIFY, val)
+#define bfin_read_DMA4_CURR_DESC_PTR()		bfin_read32(DMA4_CURR_DESC_PTR)
+#define bfin_write_DMA4_CURR_DESC_PTR(val)	bfin_write32(DMA4_CURR_DESC_PTR, val)
+#define bfin_read_DMA4_CURR_ADDR()		bfin_read32(DMA4_CURR_ADDR)
+#define bfin_write_DMA4_CURR_ADDR(val)		bfin_write32(DMA4_CURR_ADDR, val)
+#define bfin_read_DMA4_CURR_X_COUNT()		bfin_read16(DMA4_CURR_X_COUNT)
+#define bfin_write_DMA4_CURR_X_COUNT(val)	bfin_write16(DMA4_CURR_X_COUNT, val)
+#define bfin_read_DMA4_CURR_Y_COUNT()		bfin_read16(DMA4_CURR_Y_COUNT)
+#define bfin_write_DMA4_CURR_Y_COUNT(val)	bfin_write16(DMA4_CURR_Y_COUNT, val)
+#define bfin_read_DMA4_IRQ_STATUS()		bfin_read16(DMA4_IRQ_STATUS)
+#define bfin_write_DMA4_IRQ_STATUS(val)		bfin_write16(DMA4_IRQ_STATUS, val)
+#define bfin_read_DMA4_PERIPHERAL_MAP()		bfin_read16(DMA4_PERIPHERAL_MAP)
+#define bfin_write_DMA4_PERIPHERAL_MAP(val)	bfin_write16(DMA4_PERIPHERAL_MAP, val)
+
+#define bfin_read_DMA5_CONFIG()			bfin_read16(DMA5_CONFIG)
+#define bfin_write_DMA5_CONFIG(val)		bfin_write16(DMA5_CONFIG, val)
+#define bfin_read_DMA5_NEXT_DESC_PTR()		bfin_read32(DMA5_NEXT_DESC_PTR)
+#define bfin_write_DMA5_NEXT_DESC_PTR(val)	bfin_write32(DMA5_NEXT_DESC_PTR, val)
+#define bfin_read_DMA5_START_ADDR()		bfin_read32(DMA5_START_ADDR)
+#define bfin_write_DMA5_START_ADDR(val)		bfin_write32(DMA5_START_ADDR, val)
+#define bfin_read_DMA5_X_COUNT()		bfin_read16(DMA5_X_COUNT)
+#define bfin_write_DMA5_X_COUNT(val)		bfin_write16(DMA5_X_COUNT, val)
+#define bfin_read_DMA5_Y_COUNT()		bfin_read16(DMA5_Y_COUNT)
+#define bfin_write_DMA5_Y_COUNT(val)		bfin_write16(DMA5_Y_COUNT, val)
+#define bfin_read_DMA5_X_MODIFY()		bfin_read16(DMA5_X_MODIFY)
+#define bfin_write_DMA5_X_MODIFY(val)		bfin_write16(DMA5_X_MODIFY, val)
+#define bfin_read_DMA5_Y_MODIFY()		bfin_read16(DMA5_Y_MODIFY)
+#define bfin_write_DMA5_Y_MODIFY(val)		bfin_write16(DMA5_Y_MODIFY, val)
+#define bfin_read_DMA5_CURR_DESC_PTR()		bfin_read32(DMA5_CURR_DESC_PTR)
+#define bfin_write_DMA5_CURR_DESC_PTR(val)	bfin_write32(DMA5_CURR_DESC_PTR, val)
+#define bfin_read_DMA5_CURR_ADDR()		bfin_read32(DMA5_CURR_ADDR)
+#define bfin_write_DMA5_CURR_ADDR(val)		bfin_write32(DMA5_CURR_ADDR, val)
+#define bfin_read_DMA5_CURR_X_COUNT()		bfin_read16(DMA5_CURR_X_COUNT)
+#define bfin_write_DMA5_CURR_X_COUNT(val)	bfin_write16(DMA5_CURR_X_COUNT, val)
+#define bfin_read_DMA5_CURR_Y_COUNT()		bfin_read16(DMA5_CURR_Y_COUNT)
+#define bfin_write_DMA5_CURR_Y_COUNT(val)	bfin_write16(DMA5_CURR_Y_COUNT, val)
+#define bfin_read_DMA5_IRQ_STATUS()		bfin_read16(DMA5_IRQ_STATUS)
+#define bfin_write_DMA5_IRQ_STATUS(val)		bfin_write16(DMA5_IRQ_STATUS, val)
+#define bfin_read_DMA5_PERIPHERAL_MAP()		bfin_read16(DMA5_PERIPHERAL_MAP)
+#define bfin_write_DMA5_PERIPHERAL_MAP(val)	bfin_write16(DMA5_PERIPHERAL_MAP, val)
+
+#define bfin_read_DMA6_CONFIG()			bfin_read16(DMA6_CONFIG)
+#define bfin_write_DMA6_CONFIG(val)		bfin_write16(DMA6_CONFIG, val)
+#define bfin_read_DMA6_NEXT_DESC_PTR()		bfin_read32(DMA6_NEXT_DESC_PTR)
+#define bfin_write_DMA6_NEXT_DESC_PTR(val)	bfin_write32(DMA6_NEXT_DESC_PTR, val)
+#define bfin_read_DMA6_START_ADDR()		bfin_read32(DMA6_START_ADDR)
+#define bfin_write_DMA6_START_ADDR(val)		bfin_write32(DMA6_START_ADDR, val)
+#define bfin_read_DMA6_X_COUNT()		bfin_read16(DMA6_X_COUNT)
+#define bfin_write_DMA6_X_COUNT(val)		bfin_write16(DMA6_X_COUNT, val)
+#define bfin_read_DMA6_Y_COUNT()		bfin_read16(DMA6_Y_COUNT)
+#define bfin_write_DMA6_Y_COUNT(val)		bfin_write16(DMA6_Y_COUNT, val)
+#define bfin_read_DMA6_X_MODIFY()		bfin_read16(DMA6_X_MODIFY)
+#define bfin_write_DMA6_X_MODIFY(val)		bfin_write16(DMA6_X_MODIFY, val)
+#define bfin_read_DMA6_Y_MODIFY()		bfin_read16(DMA6_Y_MODIFY)
+#define bfin_write_DMA6_Y_MODIFY(val)		bfin_write16(DMA6_Y_MODIFY, val)
+#define bfin_read_DMA6_CURR_DESC_PTR()		bfin_read32(DMA6_CURR_DESC_PTR)
+#define bfin_write_DMA6_CURR_DESC_PTR(val)	bfin_write32(DMA6_CURR_DESC_PTR, val)
+#define bfin_read_DMA6_CURR_ADDR()		bfin_read32(DMA6_CURR_ADDR)
+#define bfin_write_DMA6_CURR_ADDR(val)		bfin_write32(DMA6_CURR_ADDR, val)
+#define bfin_read_DMA6_CURR_X_COUNT()		bfin_read16(DMA6_CURR_X_COUNT)
+#define bfin_write_DMA6_CURR_X_COUNT(val)	bfin_write16(DMA6_CURR_X_COUNT, val)
+#define bfin_read_DMA6_CURR_Y_COUNT()		bfin_read16(DMA6_CURR_Y_COUNT)
+#define bfin_write_DMA6_CURR_Y_COUNT(val)	bfin_write16(DMA6_CURR_Y_COUNT, val)
+#define bfin_read_DMA6_IRQ_STATUS()		bfin_read16(DMA6_IRQ_STATUS)
+#define bfin_write_DMA6_IRQ_STATUS(val)		bfin_write16(DMA6_IRQ_STATUS, val)
+#define bfin_read_DMA6_PERIPHERAL_MAP()		bfin_read16(DMA6_PERIPHERAL_MAP)
+#define bfin_write_DMA6_PERIPHERAL_MAP(val)	bfin_write16(DMA6_PERIPHERAL_MAP, val)
+
+#define bfin_read_DMA7_CONFIG()			bfin_read16(DMA7_CONFIG)
+#define bfin_write_DMA7_CONFIG(val)		bfin_write16(DMA7_CONFIG, val)
+#define bfin_read_DMA7_NEXT_DESC_PTR()		bfin_read32(DMA7_NEXT_DESC_PTR)
+#define bfin_write_DMA7_NEXT_DESC_PTR(val)	bfin_write32(DMA7_NEXT_DESC_PTR, val)
+#define bfin_read_DMA7_START_ADDR()		bfin_read32(DMA7_START_ADDR)
+#define bfin_write_DMA7_START_ADDR(val)		bfin_write32(DMA7_START_ADDR, val)
+#define bfin_read_DMA7_X_COUNT()		bfin_read16(DMA7_X_COUNT)
+#define bfin_write_DMA7_X_COUNT(val)		bfin_write16(DMA7_X_COUNT, val)
+#define bfin_read_DMA7_Y_COUNT()		bfin_read16(DMA7_Y_COUNT)
+#define bfin_write_DMA7_Y_COUNT(val)		bfin_write16(DMA7_Y_COUNT, val)
+#define bfin_read_DMA7_X_MODIFY()		bfin_read16(DMA7_X_MODIFY)
+#define bfin_write_DMA7_X_MODIFY(val)		bfin_write16(DMA7_X_MODIFY, val)
+#define bfin_read_DMA7_Y_MODIFY()		bfin_read16(DMA7_Y_MODIFY)
+#define bfin_write_DMA7_Y_MODIFY(val)		bfin_write16(DMA7_Y_MODIFY, val)
+#define bfin_read_DMA7_CURR_DESC_PTR()		bfin_read32(DMA7_CURR_DESC_PTR)
+#define bfin_write_DMA7_CURR_DESC_PTR(val)	bfin_write32(DMA7_CURR_DESC_PTR, val)
+#define bfin_read_DMA7_CURR_ADDR()		bfin_read32(DMA7_CURR_ADDR)
+#define bfin_write_DMA7_CURR_ADDR(val)		bfin_write32(DMA7_CURR_ADDR, val)
+#define bfin_read_DMA7_CURR_X_COUNT()		bfin_read16(DMA7_CURR_X_COUNT)
+#define bfin_write_DMA7_CURR_X_COUNT(val)	bfin_write16(DMA7_CURR_X_COUNT, val)
+#define bfin_read_DMA7_CURR_Y_COUNT()		bfin_read16(DMA7_CURR_Y_COUNT)
+#define bfin_write_DMA7_CURR_Y_COUNT(val)	bfin_write16(DMA7_CURR_Y_COUNT, val)
+#define bfin_read_DMA7_IRQ_STATUS()		bfin_read16(DMA7_IRQ_STATUS)
+#define bfin_write_DMA7_IRQ_STATUS(val)		bfin_write16(DMA7_IRQ_STATUS, val)
+#define bfin_read_DMA7_PERIPHERAL_MAP()		bfin_read16(DMA7_PERIPHERAL_MAP)
+#define bfin_write_DMA7_PERIPHERAL_MAP(val)	bfin_write16(DMA7_PERIPHERAL_MAP, val)
+
+#define bfin_read_DMA8_CONFIG()			bfin_read16(DMA8_CONFIG)
+#define bfin_write_DMA8_CONFIG(val)		bfin_write16(DMA8_CONFIG, val)
+#define bfin_read_DMA8_NEXT_DESC_PTR()		bfin_read32(DMA8_NEXT_DESC_PTR)
+#define bfin_write_DMA8_NEXT_DESC_PTR(val)	bfin_write32(DMA8_NEXT_DESC_PTR, val)
+#define bfin_read_DMA8_START_ADDR()		bfin_read32(DMA8_START_ADDR)
+#define bfin_write_DMA8_START_ADDR(val)		bfin_write32(DMA8_START_ADDR, val)
+#define bfin_read_DMA8_X_COUNT()		bfin_read16(DMA8_X_COUNT)
+#define bfin_write_DMA8_X_COUNT(val)		bfin_write16(DMA8_X_COUNT, val)
+#define bfin_read_DMA8_Y_COUNT()		bfin_read16(DMA8_Y_COUNT)
+#define bfin_write_DMA8_Y_COUNT(val)		bfin_write16(DMA8_Y_COUNT, val)
+#define bfin_read_DMA8_X_MODIFY()		bfin_read16(DMA8_X_MODIFY)
+#define bfin_write_DMA8_X_MODIFY(val)		bfin_write16(DMA8_X_MODIFY, val)
+#define bfin_read_DMA8_Y_MODIFY()		bfin_read16(DMA8_Y_MODIFY)
+#define bfin_write_DMA8_Y_MODIFY(val)		bfin_write16(DMA8_Y_MODIFY, val)
+#define bfin_read_DMA8_CURR_DESC_PTR()		bfin_read32(DMA8_CURR_DESC_PTR)
+#define bfin_write_DMA8_CURR_DESC_PTR(val)	bfin_write32(DMA8_CURR_DESC_PTR, val)
+#define bfin_read_DMA8_CURR_ADDR()		bfin_read32(DMA8_CURR_ADDR)
+#define bfin_write_DMA8_CURR_ADDR(val)		bfin_write32(DMA8_CURR_ADDR, val)
+#define bfin_read_DMA8_CURR_X_COUNT()		bfin_read16(DMA8_CURR_X_COUNT)
+#define bfin_write_DMA8_CURR_X_COUNT(val)	bfin_write16(DMA8_CURR_X_COUNT, val)
+#define bfin_read_DMA8_CURR_Y_COUNT()		bfin_read16(DMA8_CURR_Y_COUNT)
+#define bfin_write_DMA8_CURR_Y_COUNT(val)	bfin_write16(DMA8_CURR_Y_COUNT, val)
+#define bfin_read_DMA8_IRQ_STATUS()		bfin_read16(DMA8_IRQ_STATUS)
+#define bfin_write_DMA8_IRQ_STATUS(val)		bfin_write16(DMA8_IRQ_STATUS, val)
+#define bfin_read_DMA8_PERIPHERAL_MAP()		bfin_read16(DMA8_PERIPHERAL_MAP)
+#define bfin_write_DMA8_PERIPHERAL_MAP(val)	bfin_write16(DMA8_PERIPHERAL_MAP, val)
+
+#define bfin_read_DMA9_CONFIG()			bfin_read16(DMA9_CONFIG)
+#define bfin_write_DMA9_CONFIG(val)		bfin_write16(DMA9_CONFIG, val)
+#define bfin_read_DMA9_NEXT_DESC_PTR()		bfin_read32(DMA9_NEXT_DESC_PTR)
+#define bfin_write_DMA9_NEXT_DESC_PTR(val)	bfin_write32(DMA9_NEXT_DESC_PTR, val)
+#define bfin_read_DMA9_START_ADDR()		bfin_read32(DMA9_START_ADDR)
+#define bfin_write_DMA9_START_ADDR(val)		bfin_write32(DMA9_START_ADDR, val)
+#define bfin_read_DMA9_X_COUNT()		bfin_read16(DMA9_X_COUNT)
+#define bfin_write_DMA9_X_COUNT(val)		bfin_write16(DMA9_X_COUNT, val)
+#define bfin_read_DMA9_Y_COUNT()		bfin_read16(DMA9_Y_COUNT)
+#define bfin_write_DMA9_Y_COUNT(val)		bfin_write16(DMA9_Y_COUNT, val)
+#define bfin_read_DMA9_X_MODIFY()		bfin_read16(DMA9_X_MODIFY)
+#define bfin_write_DMA9_X_MODIFY(val)		bfin_write16(DMA9_X_MODIFY, val)
+#define bfin_read_DMA9_Y_MODIFY()		bfin_read16(DMA9_Y_MODIFY)
+#define bfin_write_DMA9_Y_MODIFY(val)		bfin_write16(DMA9_Y_MODIFY, val)
+#define bfin_read_DMA9_CURR_DESC_PTR()		bfin_read32(DMA9_CURR_DESC_PTR)
+#define bfin_write_DMA9_CURR_DESC_PTR(val)	bfin_write32(DMA9_CURR_DESC_PTR, val)
+#define bfin_read_DMA9_CURR_ADDR()		bfin_read32(DMA9_CURR_ADDR)
+#define bfin_write_DMA9_CURR_ADDR(val)		bfin_write32(DMA9_CURR_ADDR, val)
+#define bfin_read_DMA9_CURR_X_COUNT()		bfin_read16(DMA9_CURR_X_COUNT)
+#define bfin_write_DMA9_CURR_X_COUNT(val)	bfin_write16(DMA9_CURR_X_COUNT, val)
+#define bfin_read_DMA9_CURR_Y_COUNT()		bfin_read16(DMA9_CURR_Y_COUNT)
+#define bfin_write_DMA9_CURR_Y_COUNT(val)	bfin_write16(DMA9_CURR_Y_COUNT, val)
+#define bfin_read_DMA9_IRQ_STATUS()		bfin_read16(DMA9_IRQ_STATUS)
+#define bfin_write_DMA9_IRQ_STATUS(val)		bfin_write16(DMA9_IRQ_STATUS, val)
+#define bfin_read_DMA9_PERIPHERAL_MAP()		bfin_read16(DMA9_PERIPHERAL_MAP)
+#define bfin_write_DMA9_PERIPHERAL_MAP(val)	bfin_write16(DMA9_PERIPHERAL_MAP, val)
+
+#define bfin_read_DMA10_CONFIG()		bfin_read16(DMA10_CONFIG)
+#define bfin_write_DMA10_CONFIG(val)		bfin_write16(DMA10_CONFIG, val)
+#define bfin_read_DMA10_NEXT_DESC_PTR()		bfin_read32(DMA10_NEXT_DESC_PTR)
+#define bfin_write_DMA10_NEXT_DESC_PTR(val)	bfin_write32(DMA10_NEXT_DESC_PTR, val)
+#define bfin_read_DMA10_START_ADDR()		bfin_read32(DMA10_START_ADDR)
+#define bfin_write_DMA10_START_ADDR(val)	bfin_write32(DMA10_START_ADDR, val)
+#define bfin_read_DMA10_X_COUNT()		bfin_read16(DMA10_X_COUNT)
+#define bfin_write_DMA10_X_COUNT(val)		bfin_write16(DMA10_X_COUNT, val)
+#define bfin_read_DMA10_Y_COUNT()		bfin_read16(DMA10_Y_COUNT)
+#define bfin_write_DMA10_Y_COUNT(val)		bfin_write16(DMA10_Y_COUNT, val)
+#define bfin_read_DMA10_X_MODIFY()		bfin_read16(DMA10_X_MODIFY)
+#define bfin_write_DMA10_X_MODIFY(val)		bfin_write16(DMA10_X_MODIFY, val)
+#define bfin_read_DMA10_Y_MODIFY()		bfin_read16(DMA10_Y_MODIFY)
+#define bfin_write_DMA10_Y_MODIFY(val)		bfin_write16(DMA10_Y_MODIFY, val)
+#define bfin_read_DMA10_CURR_DESC_PTR()		bfin_read32(DMA10_CURR_DESC_PTR)
+#define bfin_write_DMA10_CURR_DESC_PTR(val)	bfin_write32(DMA10_CURR_DESC_PTR, val)
+#define bfin_read_DMA10_CURR_ADDR()		bfin_read32(DMA10_CURR_ADDR)
+#define bfin_write_DMA10_CURR_ADDR(val)		bfin_write32(DMA10_CURR_ADDR, val)
+#define bfin_read_DMA10_CURR_X_COUNT()		bfin_read16(DMA10_CURR_X_COUNT)
+#define bfin_write_DMA10_CURR_X_COUNT(val)	bfin_write16(DMA10_CURR_X_COUNT, val)
+#define bfin_read_DMA10_CURR_Y_COUNT()		bfin_read16(DMA10_CURR_Y_COUNT)
+#define bfin_write_DMA10_CURR_Y_COUNT(val)	bfin_write16(DMA10_CURR_Y_COUNT, val)
+#define bfin_read_DMA10_IRQ_STATUS()		bfin_read16(DMA10_IRQ_STATUS)
+#define bfin_write_DMA10_IRQ_STATUS(val)	bfin_write16(DMA10_IRQ_STATUS, val)
+#define bfin_read_DMA10_PERIPHERAL_MAP()	bfin_read16(DMA10_PERIPHERAL_MAP)
+#define bfin_write_DMA10_PERIPHERAL_MAP(val)	bfin_write16(DMA10_PERIPHERAL_MAP, val)
+
+#define bfin_read_DMA11_CONFIG()		bfin_read16(DMA11_CONFIG)
+#define bfin_write_DMA11_CONFIG(val)		bfin_write16(DMA11_CONFIG, val)
+#define bfin_read_DMA11_NEXT_DESC_PTR()		bfin_read32(DMA11_NEXT_DESC_PTR)
+#define bfin_write_DMA11_NEXT_DESC_PTR(val)	bfin_write32(DMA11_NEXT_DESC_PTR, val)
+#define bfin_read_DMA11_START_ADDR()		bfin_read32(DMA11_START_ADDR)
+#define bfin_write_DMA11_START_ADDR(val)	bfin_write32(DMA11_START_ADDR, val)
+#define bfin_read_DMA11_X_COUNT()		bfin_read16(DMA11_X_COUNT)
+#define bfin_write_DMA11_X_COUNT(val)		bfin_write16(DMA11_X_COUNT, val)
+#define bfin_read_DMA11_Y_COUNT()		bfin_read16(DMA11_Y_COUNT)
+#define bfin_write_DMA11_Y_COUNT(val)		bfin_write16(DMA11_Y_COUNT, val)
+#define bfin_read_DMA11_X_MODIFY()		bfin_read16(DMA11_X_MODIFY)
+#define bfin_write_DMA11_X_MODIFY(val)		bfin_write16(DMA11_X_MODIFY, val)
+#define bfin_read_DMA11_Y_MODIFY()		bfin_read16(DMA11_Y_MODIFY)
+#define bfin_write_DMA11_Y_MODIFY(val)		bfin_write16(DMA11_Y_MODIFY, val)
+#define bfin_read_DMA11_CURR_DESC_PTR()		bfin_read32(DMA11_CURR_DESC_PTR)
+#define bfin_write_DMA11_CURR_DESC_PTR(val)	bfin_write32(DMA11_CURR_DESC_PTR, val)
+#define bfin_read_DMA11_CURR_ADDR()		bfin_read32(DMA11_CURR_ADDR)
+#define bfin_write_DMA11_CURR_ADDR(val)		bfin_write32(DMA11_CURR_ADDR, val)
+#define bfin_read_DMA11_CURR_X_COUNT()		bfin_read16(DMA11_CURR_X_COUNT)
+#define bfin_write_DMA11_CURR_X_COUNT(val)	bfin_write16(DMA11_CURR_X_COUNT, val)
+#define bfin_read_DMA11_CURR_Y_COUNT()		bfin_read16(DMA11_CURR_Y_COUNT)
+#define bfin_write_DMA11_CURR_Y_COUNT(val)	bfin_write16(DMA11_CURR_Y_COUNT, val)
+#define bfin_read_DMA11_IRQ_STATUS()		bfin_read16(DMA11_IRQ_STATUS)
+#define bfin_write_DMA11_IRQ_STATUS(val)	bfin_write16(DMA11_IRQ_STATUS, val)
+#define bfin_read_DMA11_PERIPHERAL_MAP()	bfin_read16(DMA11_PERIPHERAL_MAP)
+#define bfin_write_DMA11_PERIPHERAL_MAP(val)	bfin_write16(DMA11_PERIPHERAL_MAP, val)
+
+#define bfin_read_MDMA_D0_CONFIG()		bfin_read16(MDMA_D0_CONFIG)
+#define bfin_write_MDMA_D0_CONFIG(val)		bfin_write16(MDMA_D0_CONFIG, val)
+#define bfin_read_MDMA_D0_NEXT_DESC_PTR()	bfin_read32(MDMA_D0_NEXT_DESC_PTR)
+#define bfin_write_MDMA_D0_NEXT_DESC_PTR(val)	bfin_write32(MDMA_D0_NEXT_DESC_PTR, val)
+#define bfin_read_MDMA_D0_START_ADDR()		bfin_read32(MDMA_D0_START_ADDR)
+#define bfin_write_MDMA_D0_START_ADDR(val)	bfin_write32(MDMA_D0_START_ADDR, val)
+#define bfin_read_MDMA_D0_X_COUNT()		bfin_read16(MDMA_D0_X_COUNT)
+#define bfin_write_MDMA_D0_X_COUNT(val)		bfin_write16(MDMA_D0_X_COUNT, val)
+#define bfin_read_MDMA_D0_Y_COUNT()		bfin_read16(MDMA_D0_Y_COUNT)
+#define bfin_write_MDMA_D0_Y_COUNT(val)		bfin_write16(MDMA_D0_Y_COUNT, val)
+#define bfin_read_MDMA_D0_X_MODIFY()		bfin_read16(MDMA_D0_X_MODIFY)
+#define bfin_write_MDMA_D0_X_MODIFY(val)	bfin_write16(MDMA_D0_X_MODIFY, val)
+#define bfin_read_MDMA_D0_Y_MODIFY()		bfin_read16(MDMA_D0_Y_MODIFY)
+#define bfin_write_MDMA_D0_Y_MODIFY(val)	bfin_write16(MDMA_D0_Y_MODIFY, val)
+#define bfin_read_MDMA_D0_CURR_DESC_PTR()	bfin_read32(MDMA_D0_CURR_DESC_PTR)
+#define bfin_write_MDMA_D0_CURR_DESC_PTR(val)	bfin_write32(MDMA_D0_CURR_DESC_PTR, val)
+#define bfin_read_MDMA_D0_CURR_ADDR()		bfin_read32(MDMA_D0_CURR_ADDR)
+#define bfin_write_MDMA_D0_CURR_ADDR(val)	bfin_write32(MDMA_D0_CURR_ADDR, val)
+#define bfin_read_MDMA_D0_CURR_X_COUNT()	bfin_read16(MDMA_D0_CURR_X_COUNT)
+#define bfin_write_MDMA_D0_CURR_X_COUNT(val)	bfin_write16(MDMA_D0_CURR_X_COUNT, val)
+#define bfin_read_MDMA_D0_CURR_Y_COUNT()	bfin_read16(MDMA_D0_CURR_Y_COUNT)
+#define bfin_write_MDMA_D0_CURR_Y_COUNT(val)	bfin_write16(MDMA_D0_CURR_Y_COUNT, val)
+#define bfin_read_MDMA_D0_IRQ_STATUS()		bfin_read16(MDMA_D0_IRQ_STATUS)
+#define bfin_write_MDMA_D0_IRQ_STATUS(val)	bfin_write16(MDMA_D0_IRQ_STATUS, val)
+#define bfin_read_MDMA_D0_PERIPHERAL_MAP()	bfin_read16(MDMA_D0_PERIPHERAL_MAP)
+#define bfin_write_MDMA_D0_PERIPHERAL_MAP(val)	bfin_write16(MDMA_D0_PERIPHERAL_MAP, val)
+
+#define bfin_read_MDMA_S0_CONFIG()		bfin_read16(MDMA_S0_CONFIG)
+#define bfin_write_MDMA_S0_CONFIG(val)		bfin_write16(MDMA_S0_CONFIG, val)
+#define bfin_read_MDMA_S0_NEXT_DESC_PTR()	bfin_read32(MDMA_S0_NEXT_DESC_PTR)
+#define bfin_write_MDMA_S0_NEXT_DESC_PTR(val)	bfin_write32(MDMA_S0_NEXT_DESC_PTR, val)
+#define bfin_read_MDMA_S0_START_ADDR()		bfin_read32(MDMA_S0_START_ADDR)
+#define bfin_write_MDMA_S0_START_ADDR(val)	bfin_write32(MDMA_S0_START_ADDR, val)
+#define bfin_read_MDMA_S0_X_COUNT()		bfin_read16(MDMA_S0_X_COUNT)
+#define bfin_write_MDMA_S0_X_COUNT(val)		bfin_write16(MDMA_S0_X_COUNT, val)
+#define bfin_read_MDMA_S0_Y_COUNT()		bfin_read16(MDMA_S0_Y_COUNT)
+#define bfin_write_MDMA_S0_Y_COUNT(val)		bfin_write16(MDMA_S0_Y_COUNT, val)
+#define bfin_read_MDMA_S0_X_MODIFY()		bfin_read16(MDMA_S0_X_MODIFY)
+#define bfin_write_MDMA_S0_X_MODIFY(val)	bfin_write16(MDMA_S0_X_MODIFY, val)
+#define bfin_read_MDMA_S0_Y_MODIFY()		bfin_read16(MDMA_S0_Y_MODIFY)
+#define bfin_write_MDMA_S0_Y_MODIFY(val)	bfin_write16(MDMA_S0_Y_MODIFY, val)
+#define bfin_read_MDMA_S0_CURR_DESC_PTR()	bfin_read32(MDMA_S0_CURR_DESC_PTR)
+#define bfin_write_MDMA_S0_CURR_DESC_PTR(val)	bfin_write32(MDMA_S0_CURR_DESC_PTR, val)
+#define bfin_read_MDMA_S0_CURR_ADDR()		bfin_read32(MDMA_S0_CURR_ADDR)
+#define bfin_write_MDMA_S0_CURR_ADDR(val)	bfin_write32(MDMA_S0_CURR_ADDR, val)
+#define bfin_read_MDMA_S0_CURR_X_COUNT()	bfin_read16(MDMA_S0_CURR_X_COUNT)
+#define bfin_write_MDMA_S0_CURR_X_COUNT(val)	bfin_write16(MDMA_S0_CURR_X_COUNT, val)
+#define bfin_read_MDMA_S0_CURR_Y_COUNT()	bfin_read16(MDMA_S0_CURR_Y_COUNT)
+#define bfin_write_MDMA_S0_CURR_Y_COUNT(val)	bfin_write16(MDMA_S0_CURR_Y_COUNT, val)
+#define bfin_read_MDMA_S0_IRQ_STATUS()		bfin_read16(MDMA_S0_IRQ_STATUS)
+#define bfin_write_MDMA_S0_IRQ_STATUS(val)	bfin_write16(MDMA_S0_IRQ_STATUS, val)
+#define bfin_read_MDMA_S0_PERIPHERAL_MAP()	bfin_read16(MDMA_S0_PERIPHERAL_MAP)
+#define bfin_write_MDMA_S0_PERIPHERAL_MAP(val)	bfin_write16(MDMA_S0_PERIPHERAL_MAP, val)
+
+#define bfin_read_MDMA_D1_CONFIG()		bfin_read16(MDMA_D1_CONFIG)
+#define bfin_write_MDMA_D1_CONFIG(val)		bfin_write16(MDMA_D1_CONFIG, val)
+#define bfin_read_MDMA_D1_NEXT_DESC_PTR()	bfin_read32(MDMA_D1_NEXT_DESC_PTR)
+#define bfin_write_MDMA_D1_NEXT_DESC_PTR(val)	bfin_write32(MDMA_D1_NEXT_DESC_PTR, val)
+#define bfin_read_MDMA_D1_START_ADDR()		bfin_read32(MDMA_D1_START_ADDR)
+#define bfin_write_MDMA_D1_START_ADDR(val)	bfin_write32(MDMA_D1_START_ADDR, val)
+#define bfin_read_MDMA_D1_X_COUNT()		bfin_read16(MDMA_D1_X_COUNT)
+#define bfin_write_MDMA_D1_X_COUNT(val)		bfin_write16(MDMA_D1_X_COUNT, val)
+#define bfin_read_MDMA_D1_Y_COUNT()		bfin_read16(MDMA_D1_Y_COUNT)
+#define bfin_write_MDMA_D1_Y_COUNT(val)		bfin_write16(MDMA_D1_Y_COUNT, val)
+#define bfin_read_MDMA_D1_X_MODIFY()		bfin_read16(MDMA_D1_X_MODIFY)
+#define bfin_write_MDMA_D1_X_MODIFY(val)	bfin_write16(MDMA_D1_X_MODIFY, val)
+#define bfin_read_MDMA_D1_Y_MODIFY()		bfin_read16(MDMA_D1_Y_MODIFY)
+#define bfin_write_MDMA_D1_Y_MODIFY(val)	bfin_write16(MDMA_D1_Y_MODIFY, val)
+#define bfin_read_MDMA_D1_CURR_DESC_PTR()	bfin_read32(MDMA_D1_CURR_DESC_PTR)
+#define bfin_write_MDMA_D1_CURR_DESC_PTR(val)	bfin_write32(MDMA_D1_CURR_DESC_PTR, val)
+#define bfin_read_MDMA_D1_CURR_ADDR()		bfin_read32(MDMA_D1_CURR_ADDR)
+#define bfin_write_MDMA_D1_CURR_ADDR(val)	bfin_write32(MDMA_D1_CURR_ADDR, val)
+#define bfin_read_MDMA_D1_CURR_X_COUNT()	bfin_read16(MDMA_D1_CURR_X_COUNT)
+#define bfin_write_MDMA_D1_CURR_X_COUNT(val)	bfin_write16(MDMA_D1_CURR_X_COUNT, val)
+#define bfin_read_MDMA_D1_CURR_Y_COUNT()	bfin_read16(MDMA_D1_CURR_Y_COUNT)
+#define bfin_write_MDMA_D1_CURR_Y_COUNT(val)	bfin_write16(MDMA_D1_CURR_Y_COUNT, val)
+#define bfin_read_MDMA_D1_IRQ_STATUS()		bfin_read16(MDMA_D1_IRQ_STATUS)
+#define bfin_write_MDMA_D1_IRQ_STATUS(val)	bfin_write16(MDMA_D1_IRQ_STATUS, val)
+#define bfin_read_MDMA_D1_PERIPHERAL_MAP()	bfin_read16(MDMA_D1_PERIPHERAL_MAP)
+#define bfin_write_MDMA_D1_PERIPHERAL_MAP(val)	bfin_write16(MDMA_D1_PERIPHERAL_MAP, val)
+
+#define bfin_read_MDMA_S1_CONFIG()		bfin_read16(MDMA_S1_CONFIG)
+#define bfin_write_MDMA_S1_CONFIG(val)		bfin_write16(MDMA_S1_CONFIG, val)
+#define bfin_read_MDMA_S1_NEXT_DESC_PTR()	bfin_read32(MDMA_S1_NEXT_DESC_PTR)
+#define bfin_write_MDMA_S1_NEXT_DESC_PTR(val)	bfin_write32(MDMA_S1_NEXT_DESC_PTR, val)
+#define bfin_read_MDMA_S1_START_ADDR()		bfin_read32(MDMA_S1_START_ADDR)
+#define bfin_write_MDMA_S1_START_ADDR(val)	bfin_write32(MDMA_S1_START_ADDR, val)
+#define bfin_read_MDMA_S1_X_COUNT()		bfin_read16(MDMA_S1_X_COUNT)
+#define bfin_write_MDMA_S1_X_COUNT(val)		bfin_write16(MDMA_S1_X_COUNT, val)
+#define bfin_read_MDMA_S1_Y_COUNT()		bfin_read16(MDMA_S1_Y_COUNT)
+#define bfin_write_MDMA_S1_Y_COUNT(val)		bfin_write16(MDMA_S1_Y_COUNT, val)
+#define bfin_read_MDMA_S1_X_MODIFY()		bfin_read16(MDMA_S1_X_MODIFY)
+#define bfin_write_MDMA_S1_X_MODIFY(val)	bfin_write16(MDMA_S1_X_MODIFY, val)
+#define bfin_read_MDMA_S1_Y_MODIFY()		bfin_read16(MDMA_S1_Y_MODIFY)
+#define bfin_write_MDMA_S1_Y_MODIFY(val)	bfin_write16(MDMA_S1_Y_MODIFY, val)
+#define bfin_read_MDMA_S1_CURR_DESC_PTR()	bfin_read32(MDMA_S1_CURR_DESC_PTR)
+#define bfin_write_MDMA_S1_CURR_DESC_PTR(val)	bfin_write32(MDMA_S1_CURR_DESC_PTR, val)
+#define bfin_read_MDMA_S1_CURR_ADDR()		bfin_read32(MDMA_S1_CURR_ADDR)
+#define bfin_write_MDMA_S1_CURR_ADDR(val)	bfin_write32(MDMA_S1_CURR_ADDR, val)
+#define bfin_read_MDMA_S1_CURR_X_COUNT()	bfin_read16(MDMA_S1_CURR_X_COUNT)
+#define bfin_write_MDMA_S1_CURR_X_COUNT(val)	bfin_write16(MDMA_S1_CURR_X_COUNT, val)
+#define bfin_read_MDMA_S1_CURR_Y_COUNT()	bfin_read16(MDMA_S1_CURR_Y_COUNT)
+#define bfin_write_MDMA_S1_CURR_Y_COUNT(val)	bfin_write16(MDMA_S1_CURR_Y_COUNT, val)
+#define bfin_read_MDMA_S1_IRQ_STATUS()		bfin_read16(MDMA_S1_IRQ_STATUS)
+#define bfin_write_MDMA_S1_IRQ_STATUS(val)	bfin_write16(MDMA_S1_IRQ_STATUS, val)
+#define bfin_read_MDMA_S1_PERIPHERAL_MAP()	bfin_read16(MDMA_S1_PERIPHERAL_MAP)
+#define bfin_write_MDMA_S1_PERIPHERAL_MAP(val)	bfin_write16(MDMA_S1_PERIPHERAL_MAP, val)
+
+
+/* Parallel Peripheral Interface (0xFFC01000 - 0xFFC010FF)							*/
+#define bfin_read_PPI_CONTROL()			bfin_read16(PPI_CONTROL)
+#define bfin_write_PPI_CONTROL(val)		bfin_write16(PPI_CONTROL, val)
+#define bfin_read_PPI_STATUS()			bfin_read16(PPI_STATUS)
+#define bfin_write_PPI_STATUS(val)		bfin_write16(PPI_STATUS, val)
+#define bfin_read_PPI_DELAY()			bfin_read16(PPI_DELAY)
+#define bfin_write_PPI_DELAY(val)		bfin_write16(PPI_DELAY, val)
+#define bfin_read_PPI_COUNT()			bfin_read16(PPI_COUNT)
+#define bfin_write_PPI_COUNT(val)		bfin_write16(PPI_COUNT, val)
+#define bfin_read_PPI_FRAME()			bfin_read16(PPI_FRAME)
+#define bfin_write_PPI_FRAME(val)		bfin_write16(PPI_FRAME, val)
+
+
+/* Two-Wire Interface		(0xFFC01400 - 0xFFC014FF)								*/
+
+/* General Purpose I/O Port G (0xFFC01500 - 0xFFC015FF)								*/
+#define bfin_read_PORTGIO()			bfin_read16(PORTGIO)
+#define bfin_write_PORTGIO(val)			bfin_write16(PORTGIO, val)
+#define bfin_read_PORTGIO_CLEAR()		bfin_read16(PORTGIO_CLEAR)
+#define bfin_write_PORTGIO_CLEAR(val)		bfin_write16(PORTGIO_CLEAR, val)
+#define bfin_read_PORTGIO_SET()			bfin_read16(PORTGIO_SET)
+#define bfin_write_PORTGIO_SET(val)		bfin_write16(PORTGIO_SET, val)
+#define bfin_read_PORTGIO_TOGGLE()		bfin_read16(PORTGIO_TOGGLE)
+#define bfin_write_PORTGIO_TOGGLE(val)		bfin_write16(PORTGIO_TOGGLE, val)
+#define bfin_read_PORTGIO_MASKA()		bfin_read16(PORTGIO_MASKA)
+#define bfin_write_PORTGIO_MASKA(val)		bfin_write16(PORTGIO_MASKA, val)
+#define bfin_read_PORTGIO_MASKA_CLEAR()		bfin_read16(PORTGIO_MASKA_CLEAR)
+#define bfin_write_PORTGIO_MASKA_CLEAR(val)	bfin_write16(PORTGIO_MASKA_CLEAR, val)
+#define bfin_read_PORTGIO_MASKA_SET()		bfin_read16(PORTGIO_MASKA_SET)
+#define bfin_write_PORTGIO_MASKA_SET(val)	bfin_write16(PORTGIO_MASKA_SET, val)
+#define bfin_read_PORTGIO_MASKA_TOGGLE()	bfin_read16(PORTGIO_MASKA_TOGGLE)
+#define bfin_write_PORTGIO_MASKA_TOGGLE(val)	bfin_write16(PORTGIO_MASKA_TOGGLE, val)
+#define bfin_read_PORTGIO_MASKB()		bfin_read16(PORTGIO_MASKB)
+#define bfin_write_PORTGIO_MASKB(val)		bfin_write16(PORTGIO_MASKB, val)
+#define bfin_read_PORTGIO_MASKB_CLEAR()		bfin_read16(PORTGIO_MASKB_CLEAR)
+#define bfin_write_PORTGIO_MASKB_CLEAR(val)	bfin_write16(PORTGIO_MASKB_CLEAR, val)
+#define bfin_read_PORTGIO_MASKB_SET()		bfin_read16(PORTGIO_MASKB_SET)
+#define bfin_write_PORTGIO_MASKB_SET(val)	bfin_write16(PORTGIO_MASKB_SET, val)
+#define bfin_read_PORTGIO_MASKB_TOGGLE()	bfin_read16(PORTGIO_MASKB_TOGGLE)
+#define bfin_write_PORTGIO_MASKB_TOGGLE(val)	bfin_write16(PORTGIO_MASKB_TOGGLE, val)
+#define bfin_read_PORTGIO_DIR()			bfin_read16(PORTGIO_DIR)
+#define bfin_write_PORTGIO_DIR(val)		bfin_write16(PORTGIO_DIR, val)
+#define bfin_read_PORTGIO_POLAR()		bfin_read16(PORTGIO_POLAR)
+#define bfin_write_PORTGIO_POLAR(val)		bfin_write16(PORTGIO_POLAR, val)
+#define bfin_read_PORTGIO_EDGE()		bfin_read16(PORTGIO_EDGE)
+#define bfin_write_PORTGIO_EDGE(val)		bfin_write16(PORTGIO_EDGE, val)
+#define bfin_read_PORTGIO_BOTH()		bfin_read16(PORTGIO_BOTH)
+#define bfin_write_PORTGIO_BOTH(val)		bfin_write16(PORTGIO_BOTH, val)
+#define bfin_read_PORTGIO_INEN()		bfin_read16(PORTGIO_INEN)
+#define bfin_write_PORTGIO_INEN(val)		bfin_write16(PORTGIO_INEN, val)
+
+
+/* General Purpose I/O Port H (0xFFC01700 - 0xFFC017FF)								*/
+#define bfin_read_PORTHIO()			bfin_read16(PORTHIO)
+#define bfin_write_PORTHIO(val)			bfin_write16(PORTHIO, val)
+#define bfin_read_PORTHIO_CLEAR()		bfin_read16(PORTHIO_CLEAR)
+#define bfin_write_PORTHIO_CLEAR(val)		bfin_write16(PORTHIO_CLEAR, val)
+#define bfin_read_PORTHIO_SET()			bfin_read16(PORTHIO_SET)
+#define bfin_write_PORTHIO_SET(val)		bfin_write16(PORTHIO_SET, val)
+#define bfin_read_PORTHIO_TOGGLE()		bfin_read16(PORTHIO_TOGGLE)
+#define bfin_write_PORTHIO_TOGGLE(val)		bfin_write16(PORTHIO_TOGGLE, val)
+#define bfin_read_PORTHIO_MASKA()		bfin_read16(PORTHIO_MASKA)
+#define bfin_write_PORTHIO_MASKA(val)		bfin_write16(PORTHIO_MASKA, val)
+#define bfin_read_PORTHIO_MASKA_CLEAR()		bfin_read16(PORTHIO_MASKA_CLEAR)
+#define bfin_write_PORTHIO_MASKA_CLEAR(val)	bfin_write16(PORTHIO_MASKA_CLEAR, val)
+#define bfin_read_PORTHIO_MASKA_SET()		bfin_read16(PORTHIO_MASKA_SET)
+#define bfin_write_PORTHIO_MASKA_SET(val)	bfin_write16(PORTHIO_MASKA_SET, val)
+#define bfin_read_PORTHIO_MASKA_TOGGLE()	bfin_read16(PORTHIO_MASKA_TOGGLE)
+#define bfin_write_PORTHIO_MASKA_TOGGLE(val)	bfin_write16(PORTHIO_MASKA_TOGGLE, val)
+#define bfin_read_PORTHIO_MASKB()		bfin_read16(PORTHIO_MASKB)
+#define bfin_write_PORTHIO_MASKB(val)		bfin_write16(PORTHIO_MASKB, val)
+#define bfin_read_PORTHIO_MASKB_CLEAR()		bfin_read16(PORTHIO_MASKB_CLEAR)
+#define bfin_write_PORTHIO_MASKB_CLEAR(val)	bfin_write16(PORTHIO_MASKB_CLEAR, val)
+#define bfin_read_PORTHIO_MASKB_SET()		bfin_read16(PORTHIO_MASKB_SET)
+#define bfin_write_PORTHIO_MASKB_SET(val)	bfin_write16(PORTHIO_MASKB_SET, val)
+#define bfin_read_PORTHIO_MASKB_TOGGLE()	bfin_read16(PORTHIO_MASKB_TOGGLE)
+#define bfin_write_PORTHIO_MASKB_TOGGLE(val)	bfin_write16(PORTHIO_MASKB_TOGGLE, val)
+#define bfin_read_PORTHIO_DIR()			bfin_read16(PORTHIO_DIR)
+#define bfin_write_PORTHIO_DIR(val)		bfin_write16(PORTHIO_DIR, val)
+#define bfin_read_PORTHIO_POLAR()		bfin_read16(PORTHIO_POLAR)
+#define bfin_write_PORTHIO_POLAR(val)		bfin_write16(PORTHIO_POLAR, val)
+#define bfin_read_PORTHIO_EDGE()		bfin_read16(PORTHIO_EDGE)
+#define bfin_write_PORTHIO_EDGE(val)		bfin_write16(PORTHIO_EDGE, val)
+#define bfin_read_PORTHIO_BOTH()		bfin_read16(PORTHIO_BOTH)
+#define bfin_write_PORTHIO_BOTH(val)		bfin_write16(PORTHIO_BOTH, val)
+#define bfin_read_PORTHIO_INEN()		bfin_read16(PORTHIO_INEN)
+#define bfin_write_PORTHIO_INEN(val)		bfin_write16(PORTHIO_INEN, val)
+
+
+/* UART1 Controller		(0xFFC02000 - 0xFFC020FF)								*/
+#define bfin_read_UART1_THR()			bfin_read16(UART1_THR)
+#define bfin_write_UART1_THR(val)		bfin_write16(UART1_THR, val)
+#define bfin_read_UART1_RBR()			bfin_read16(UART1_RBR)
+#define bfin_write_UART1_RBR(val)		bfin_write16(UART1_RBR, val)
+#define bfin_read_UART1_DLL()			bfin_read16(UART1_DLL)
+#define bfin_write_UART1_DLL(val)		bfin_write16(UART1_DLL, val)
+#define bfin_read_UART1_IER()			bfin_read16(UART1_IER)
+#define bfin_write_UART1_IER(val)		bfin_write16(UART1_IER, val)
+#define bfin_read_UART1_DLH()			bfin_read16(UART1_DLH)
+#define bfin_write_UART1_DLH(val)		bfin_write16(UART1_DLH, val)
+#define bfin_read_UART1_IIR()			bfin_read16(UART1_IIR)
+#define bfin_write_UART1_IIR(val)		bfin_write16(UART1_IIR, val)
+#define bfin_read_UART1_LCR()			bfin_read16(UART1_LCR)
+#define bfin_write_UART1_LCR(val)		bfin_write16(UART1_LCR, val)
+#define bfin_read_UART1_MCR()			bfin_read16(UART1_MCR)
+#define bfin_write_UART1_MCR(val)		bfin_write16(UART1_MCR, val)
+#define bfin_read_UART1_LSR()			bfin_read16(UART1_LSR)
+#define bfin_write_UART1_LSR(val)		bfin_write16(UART1_LSR, val)
+#define bfin_read_UART1_MSR()			bfin_read16(UART1_MSR)
+#define bfin_write_UART1_MSR(val)		bfin_write16(UART1_MSR, val)
+#define bfin_read_UART1_SCR()			bfin_read16(UART1_SCR)
+#define bfin_write_UART1_SCR(val)		bfin_write16(UART1_SCR, val)
+#define bfin_read_UART1_GCTL()			bfin_read16(UART1_GCTL)
+#define bfin_write_UART1_GCTL(val)		bfin_write16(UART1_GCTL, val)
+
+/* Omit CAN register sets from the cdefBF534.h (CAN is not in the ADSP-BF51x processor) */
+
+/* Pin Control Registers	(0xFFC03200 - 0xFFC032FF)								*/
+#define bfin_read_PORTF_FER()			bfin_read16(PORTF_FER)
+#define bfin_write_PORTF_FER(val)		bfin_write16(PORTF_FER, val)
+#define bfin_read_PORTG_FER()			bfin_read16(PORTG_FER)
+#define bfin_write_PORTG_FER(val)		bfin_write16(PORTG_FER, val)
+#define bfin_read_PORTH_FER()			bfin_read16(PORTH_FER)
+#define bfin_write_PORTH_FER(val)		bfin_write16(PORTH_FER, val)
+#define bfin_read_PORT_MUX()			bfin_read16(PORT_MUX)
+#define bfin_write_PORT_MUX(val)		bfin_write16(PORT_MUX, val)
+
+
+/* Handshake MDMA Registers	(0xFFC03300 - 0xFFC033FF)								*/
+#define bfin_read_HMDMA0_CONTROL()		bfin_read16(HMDMA0_CONTROL)
+#define bfin_write_HMDMA0_CONTROL(val)		bfin_write16(HMDMA0_CONTROL, val)
+#define bfin_read_HMDMA0_ECINIT()		bfin_read16(HMDMA0_ECINIT)
+#define bfin_write_HMDMA0_ECINIT(val)		bfin_write16(HMDMA0_ECINIT, val)
+#define bfin_read_HMDMA0_BCINIT()		bfin_read16(HMDMA0_BCINIT)
+#define bfin_write_HMDMA0_BCINIT(val)		bfin_write16(HMDMA0_BCINIT, val)
+#define bfin_read_HMDMA0_ECURGENT()		bfin_read16(HMDMA0_ECURGENT)
+#define bfin_write_HMDMA0_ECURGENT(val)		bfin_write16(HMDMA0_ECURGENT, val)
+#define bfin_read_HMDMA0_ECOVERFLOW()		bfin_read16(HMDMA0_ECOVERFLOW)
+#define bfin_write_HMDMA0_ECOVERFLOW(val)	bfin_write16(HMDMA0_ECOVERFLOW, val)
+#define bfin_read_HMDMA0_ECOUNT()		bfin_read16(HMDMA0_ECOUNT)
+#define bfin_write_HMDMA0_ECOUNT(val)		bfin_write16(HMDMA0_ECOUNT, val)
+#define bfin_read_HMDMA0_BCOUNT()		bfin_read16(HMDMA0_BCOUNT)
+#define bfin_write_HMDMA0_BCOUNT(val)		bfin_write16(HMDMA0_BCOUNT, val)
+
+#define bfin_read_HMDMA1_CONTROL()		bfin_read16(HMDMA1_CONTROL)
+#define bfin_write_HMDMA1_CONTROL(val)		bfin_write16(HMDMA1_CONTROL, val)
+#define bfin_read_HMDMA1_ECINIT()		bfin_read16(HMDMA1_ECINIT)
+#define bfin_write_HMDMA1_ECINIT(val)		bfin_write16(HMDMA1_ECINIT, val)
+#define bfin_read_HMDMA1_BCINIT()		bfin_read16(HMDMA1_BCINIT)
+#define bfin_write_HMDMA1_BCINIT(val)		bfin_write16(HMDMA1_BCINIT, val)
+#define bfin_read_HMDMA1_ECURGENT()		bfin_read16(HMDMA1_ECURGENT)
+#define bfin_write_HMDMA1_ECURGENT(val)		bfin_write16(HMDMA1_ECURGENT, val)
+#define bfin_read_HMDMA1_ECOVERFLOW()		bfin_read16(HMDMA1_ECOVERFLOW)
+#define bfin_write_HMDMA1_ECOVERFLOW(val)	bfin_write16(HMDMA1_ECOVERFLOW, val)
+#define bfin_read_HMDMA1_ECOUNT()		bfin_read16(HMDMA1_ECOUNT)
+#define bfin_write_HMDMA1_ECOUNT(val)		bfin_write16(HMDMA1_ECOUNT, val)
+#define bfin_read_HMDMA1_BCOUNT()		bfin_read16(HMDMA1_BCOUNT)
+#define bfin_write_HMDMA1_BCOUNT(val)		bfin_write16(HMDMA1_BCOUNT, val)
+
+/* ==== end from cdefBF534.h ==== */
+
+/* GPIO PIN mux (0xFFC03210 - OxFFC03288) */
+
+#define bfin_read_PORTF_MUX()			bfin_read16(PORTF_MUX)
+#define bfin_write_PORTF_MUX(val)		bfin_write16(PORTF_MUX, val)
+#define bfin_read_PORTG_MUX()			bfin_read16(PORTG_MUX)
+#define bfin_write_PORTG_MUX(val)		bfin_write16(PORTG_MUX, val)
+#define bfin_read_PORTH_MUX()			bfin_read16(PORTH_MUX)
+#define bfin_write_PORTH_MUX(val)		bfin_write16(PORTH_MUX, val)
+
+#define bfin_read_PORTF_DRIVE()			bfin_read16(PORTF_DRIVE)
+#define bfin_write_PORTF_DRIVE(val)		bfin_write16(PORTF_DRIVE, val)
+#define bfin_read_PORTG_DRIVE()			bfin_read16(PORTG_DRIVE)
+#define bfin_write_PORTG_DRIVE(val)		bfin_write16(PORTG_DRIVE, val)
+#define bfin_read_PORTH_DRIVE()			bfin_read16(PORTH_DRIVE)
+#define bfin_write_PORTH_DRIVE(val)		bfin_write16(PORTH_DRIVE, val)
+#define bfin_read_PORTF_SLEW()			bfin_read16(PORTF_SLEW)
+#define bfin_write_PORTF_SLEW(val)		bfin_write16(PORTF_SLEW, val)
+#define bfin_read_PORTG_SLEW()			bfin_read16(PORTG_SLEW)
+#define bfin_write_PORTG_SLEW(val)		bfin_write16(PORTG_SLEW, val)
+#define bfin_read_PORTH_SLEW()			bfin_read16(PORTH_SLEW)
+#define bfin_write_PORTH_SLEW(val)		bfin_write16(PORTH_SLEW, val)
+#define bfin_read_PORTF_HYSTERISIS()		bfin_read16(PORTF_HYSTERISIS)
+#define bfin_write_PORTF_HYSTERISIS(val)	bfin_write16(PORTF_HYSTERISIS, val)
+#define bfin_read_PORTG_HYSTERISIS()		bfin_read16(PORTG_HYSTERISIS)
+#define bfin_write_PORTG_HYSTERISIS(val)	bfin_write16(PORTG_HYSTERISIS, val)
+#define bfin_read_PORTH_HYSTERISIS()		bfin_read16(PORTH_HYSTERISIS)
+#define bfin_write_PORTH_HYSTERISIS(val)	bfin_write16(PORTH_HYSTERISIS, val)
+#define bfin_read_MISCPORT_DRIVE()		bfin_read16(MISCPORT_DRIVE)
+#define bfin_write_MISCPORT_DRIVE(val)		bfin_write16(MISCPORT_DRIVE, val)
+#define bfin_read_MISCPORT_SLEW()		bfin_read16(MISCPORT_SLEW)
+#define bfin_write_MISCPORT_SLEW(val)		bfin_write16(MISCPORT_SLEW, val)
+#define bfin_read_MISCPORT_HYSTERISIS()		bfin_read16(MISCPORT_HYSTERISIS)
+#define bfin_write_MISCPORT_HYSTERISIS(val)	bfin_write16(MISCPORT_HYSTERISIS, val)
+
+/* HOST Port Registers */
+
+#define bfin_read_HOST_CONTROL()		bfin_read16(HOST_CONTROL)
+#define bfin_write_HOST_CONTROL(val)		bfin_write16(HOST_CONTROL, val)
+#define bfin_read_HOST_STATUS()			bfin_read16(HOST_STATUS)
+#define bfin_write_HOST_STATUS(val)		bfin_write16(HOST_STATUS, val)
+#define bfin_read_HOST_TIMEOUT()		bfin_read16(HOST_TIMEOUT)
+#define bfin_write_HOST_TIMEOUT(val)		bfin_write16(HOST_TIMEOUT, val)
+
+/* Counter Registers */
+
+#define bfin_read_CNT_CONFIG()			bfin_read16(CNT_CONFIG)
+#define bfin_write_CNT_CONFIG(val)		bfin_write16(CNT_CONFIG, val)
+#define bfin_read_CNT_IMASK()			bfin_read16(CNT_IMASK)
+#define bfin_write_CNT_IMASK(val)		bfin_write16(CNT_IMASK, val)
+#define bfin_read_CNT_STATUS()			bfin_read16(CNT_STATUS)
+#define bfin_write_CNT_STATUS(val)		bfin_write16(CNT_STATUS, val)
+#define bfin_read_CNT_COMMAND()			bfin_read16(CNT_COMMAND)
+#define bfin_write_CNT_COMMAND(val)		bfin_write16(CNT_COMMAND, val)
+#define bfin_read_CNT_DEBOUNCE()		bfin_read16(CNT_DEBOUNCE)
+#define bfin_write_CNT_DEBOUNCE(val)		bfin_write16(CNT_DEBOUNCE, val)
+#define bfin_read_CNT_COUNTER()			bfin_read32(CNT_COUNTER)
+#define bfin_write_CNT_COUNTER(val)		bfin_write32(CNT_COUNTER, val)
+#define bfin_read_CNT_MAX()			bfin_read32(CNT_MAX)
+#define bfin_write_CNT_MAX(val)			bfin_write32(CNT_MAX, val)
+#define bfin_read_CNT_MIN()			bfin_read32(CNT_MIN)
+#define bfin_write_CNT_MIN(val)			bfin_write32(CNT_MIN, val)
+
+/* OTP/FUSE Registers */
+
+#define bfin_read_OTP_CONTROL()			bfin_read16(OTP_CONTROL)
+#define bfin_write_OTP_CONTROL(val)		bfin_write16(OTP_CONTROL, val)
+#define bfin_read_OTP_BEN()			bfin_read16(OTP_BEN)
+#define bfin_write_OTP_BEN(val)			bfin_write16(OTP_BEN, val)
+#define bfin_read_OTP_STATUS()			bfin_read16(OTP_STATUS)
+#define bfin_write_OTP_STATUS(val)		bfin_write16(OTP_STATUS, val)
+#define bfin_read_OTP_TIMING()			bfin_read32(OTP_TIMING)
+#define bfin_write_OTP_TIMING(val)		bfin_write32(OTP_TIMING, val)
+
+/* Security Registers */
+
+#define bfin_read_SECURE_SYSSWT()		bfin_read32(SECURE_SYSSWT)
+#define bfin_write_SECURE_SYSSWT(val)		bfin_write32(SECURE_SYSSWT, val)
+#define bfin_read_SECURE_CONTROL()		bfin_read16(SECURE_CONTROL)
+#define bfin_write_SECURE_CONTROL(val)		bfin_write16(SECURE_CONTROL, val)
+#define bfin_read_SECURE_STATUS()		bfin_read16(SECURE_STATUS)
+#define bfin_write_SECURE_STATUS(val)		bfin_write16(SECURE_STATUS, val)
+
+/* OTP Read/Write Data Buffer Registers */
+
+#define bfin_read_OTP_DATA0()			bfin_read32(OTP_DATA0)
+#define bfin_write_OTP_DATA0(val)		bfin_write32(OTP_DATA0, val)
+#define bfin_read_OTP_DATA1()			bfin_read32(OTP_DATA1)
+#define bfin_write_OTP_DATA1(val)		bfin_write32(OTP_DATA1, val)
+#define bfin_read_OTP_DATA2()			bfin_read32(OTP_DATA2)
+#define bfin_write_OTP_DATA2(val)		bfin_write32(OTP_DATA2, val)
+#define bfin_read_OTP_DATA3()			bfin_read32(OTP_DATA3)
+#define bfin_write_OTP_DATA3(val)		bfin_write32(OTP_DATA3, val)
+
+/* NFC Registers */
+
+#define bfin_read_NFC_CTL()			bfin_read16(NFC_CTL)
+#define bfin_write_NFC_CTL(val)			bfin_write16(NFC_CTL, val)
+#define bfin_read_NFC_STAT()			bfin_read16(NFC_STAT)
+#define bfin_write_NFC_STAT(val)		bfin_write16(NFC_STAT, val)
+#define bfin_read_NFC_IRQSTAT()			bfin_read16(NFC_IRQSTAT)
+#define bfin_write_NFC_IRQSTAT(val)		bfin_write16(NFC_IRQSTAT, val)
+#define bfin_read_NFC_IRQMASK()			bfin_read16(NFC_IRQMASK)
+#define bfin_write_NFC_IRQMASK(val)		bfin_write16(NFC_IRQMASK, val)
+#define bfin_read_NFC_ECC0()			bfin_read16(NFC_ECC0)
+#define bfin_write_NFC_ECC0(val)		bfin_write16(NFC_ECC0, val)
+#define bfin_read_NFC_ECC1()			bfin_read16(NFC_ECC1)
+#define bfin_write_NFC_ECC1(val)		bfin_write16(NFC_ECC1, val)
+#define bfin_read_NFC_ECC2()			bfin_read16(NFC_ECC2)
+#define bfin_write_NFC_ECC2(val)		bfin_write16(NFC_ECC2, val)
+#define bfin_read_NFC_ECC3()			bfin_read16(NFC_ECC3)
+#define bfin_write_NFC_ECC3(val)		bfin_write16(NFC_ECC3, val)
+#define bfin_read_NFC_COUNT()			bfin_read16(NFC_COUNT)
+#define bfin_write_NFC_COUNT(val)		bfin_write16(NFC_COUNT, val)
+#define bfin_read_NFC_RST()			bfin_read16(NFC_RST)
+#define bfin_write_NFC_RST(val)			bfin_write16(NFC_RST, val)
+#define bfin_read_NFC_PGCTL()			bfin_read16(NFC_PGCTL)
+#define bfin_write_NFC_PGCTL(val)		bfin_write16(NFC_PGCTL, val)
+#define bfin_read_NFC_READ()			bfin_read16(NFC_READ)
+#define bfin_write_NFC_READ(val)		bfin_write16(NFC_READ, val)
+#define bfin_read_NFC_ADDR()			bfin_read16(NFC_ADDR)
+#define bfin_write_NFC_ADDR(val)		bfin_write16(NFC_ADDR, val)
+#define bfin_read_NFC_CMD()			bfin_read16(NFC_CMD)
+#define bfin_write_NFC_CMD(val)			bfin_write16(NFC_CMD, val)
+#define bfin_read_NFC_DATA_WR()			bfin_read16(NFC_DATA_WR)
+#define bfin_write_NFC_DATA_WR(val)		bfin_write16(NFC_DATA_WR, val)
+#define bfin_read_NFC_DATA_RD()			bfin_read16(NFC_DATA_RD)
+#define bfin_write_NFC_DATA_RD(val)		bfin_write16(NFC_DATA_RD, val)
+
+/* These need to be last due to the cdef/linux inter-dependencies */
+#include <asm/irq.h>
+
+/* Writing to PLL_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_PLL_CTL(unsigned int val)
+{
+	unsigned long flags, iwr0, iwr1;
+
+	if (val == bfin_read_PLL_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr0 = bfin_read32(SIC_IWR0);
+	iwr1 = bfin_read32(SIC_IWR1);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SIC_IWR0, IWR_ENABLE(0));
+	bfin_write32(SIC_IWR1, 0);
+
+	bfin_write16(PLL_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SIC_IWR0, iwr0);
+	bfin_write32(SIC_IWR1, iwr1);
+	local_irq_restore_hw(flags);
+}
+
+/* Writing to VR_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_VR_CTL(unsigned int val)
+{
+	unsigned long flags, iwr0, iwr1;
+
+	if (val == bfin_read_VR_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr0 = bfin_read32(SIC_IWR0);
+	iwr1 = bfin_read32(SIC_IWR1);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SIC_IWR0, IWR_ENABLE(0));
+	bfin_write32(SIC_IWR1, 0);
+
+	bfin_write16(VR_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SIC_IWR0, iwr0);
+	bfin_write32(SIC_IWR1, iwr1);
+	local_irq_restore_hw(flags);
+}
+
+#endif /* _CDEF_BF52X_H */
diff --git a/arch/blackfin/mach-bf518/include/mach/defBF512.h b/arch/blackfin/mach-bf518/include/mach/defBF512.h
new file mode 100644
index 000000000000..a96ca90154dd
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/defBF512.h
@@ -0,0 +1,42 @@
+/*
+ * File:         include/asm-blackfin/mach-bf518/defBF512.h
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:
+ *
+ * Rev:
+ *
+ * Modified:
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, 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; see the file COPYING.
+ * If not, write to the Free Software Foundation,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef _DEF_BF512_H
+#define _DEF_BF512_H
+
+/* Include all Core registers and bit definitions */
+#include <asm/def_LPBlackfin.h>
+
+/* SYSTEM & MMR ADDRESS DEFINITIONS FOR ADSP-BF512 */
+
+/* Include defBF51x_base.h for the set of #defines that are common to all ADSP-BF51x processors */
+#include "defBF51x_base.h"
+
+#endif /* _DEF_BF512_H */
diff --git a/arch/blackfin/mach-bf518/include/mach/defBF514.h b/arch/blackfin/mach-bf518/include/mach/defBF514.h
new file mode 100644
index 000000000000..543f2913b3f5
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/defBF514.h
@@ -0,0 +1,113 @@
+/*
+ * File:         include/asm-blackfin/mach-bf518/defBF514.h
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:
+ *
+ * Rev:
+ *
+ * Modified:
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, 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; see the file COPYING.
+ * If not, write to the Free Software Foundation,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef _DEF_BF514_H
+#define _DEF_BF514_H
+
+/* Include all Core registers and bit definitions */
+#include <asm/def_LPBlackfin.h>
+
+/* SYSTEM & MMR ADDRESS DEFINITIONS FOR ADSP-BF514 */
+
+/* Include defBF51x_base.h for the set of #defines that are common to all ADSP-BF51x processors */
+#include "defBF51x_base.h"
+
+/* The following are the #defines needed by ADSP-BF514 that are not in the common header */
+
+/* SDH Registers */
+
+#define SDH_PWR_CTL                    0xFFC03900 /* SDH Power Control */
+#define SDH_CLK_CTL                    0xFFC03904 /* SDH Clock Control */
+#define SDH_ARGUMENT                   0xFFC03908 /* SDH Argument */
+#define SDH_COMMAND                    0xFFC0390C /* SDH Command */
+#define SDH_RESP_CMD                   0xFFC03910 /* SDH Response Command */
+#define SDH_RESPONSE0                  0xFFC03914 /* SDH Response0 */
+#define SDH_RESPONSE1                  0xFFC03918 /* SDH Response1 */
+#define SDH_RESPONSE2                  0xFFC0391C /* SDH Response2 */
+#define SDH_RESPONSE3                  0xFFC03920 /* SDH Response3 */
+#define SDH_DATA_TIMER                 0xFFC03924 /* SDH Data Timer */
+#define SDH_DATA_LGTH                  0xFFC03928 /* SDH Data Length */
+#define SDH_DATA_CTL                   0xFFC0392C /* SDH Data Control */
+#define SDH_DATA_CNT                   0xFFC03930 /* SDH Data Counter */
+#define SDH_STATUS                     0xFFC03934 /* SDH Status */
+#define SDH_STATUS_CLR                 0xFFC03938 /* SDH Status Clear */
+#define SDH_MASK0                      0xFFC0393C /* SDH Interrupt0 Mask */
+#define SDH_MASK1                      0xFFC03940 /* SDH Interrupt1 Mask */
+#define SDH_FIFO_CNT                   0xFFC03948 /* SDH FIFO Counter */
+#define SDH_FIFO                       0xFFC03980 /* SDH Data FIFO */
+#define SDH_E_STATUS                   0xFFC039C0 /* SDH Exception Status */
+#define SDH_E_MASK                     0xFFC039C4 /* SDH Exception Mask */
+#define SDH_CFG                        0xFFC039C8 /* SDH Configuration */
+#define SDH_RD_WAIT_EN                 0xFFC039CC /* SDH Read Wait Enable */
+#define SDH_PID0                       0xFFC039D0 /* SDH Peripheral Identification0 */
+#define SDH_PID1                       0xFFC039D4 /* SDH Peripheral Identification1 */
+#define SDH_PID2                       0xFFC039D8 /* SDH Peripheral Identification2 */
+#define SDH_PID3                       0xFFC039DC /* SDH Peripheral Identification3 */
+#define SDH_PID4                       0xFFC039E0 /* SDH Peripheral Identification4 */
+#define SDH_PID5                       0xFFC039E4 /* SDH Peripheral Identification5 */
+#define SDH_PID6                       0xFFC039E8 /* SDH Peripheral Identification6 */
+#define SDH_PID7                       0xFFC039EC /* SDH Peripheral Identification7 */
+
+/* Removable Storage Interface Registers */
+
+#define RSI_PWR_CONTROL                0xFFC03800 /* RSI Power Control Register */
+#define RSI_CLK_CONTROL                0xFFC03804 /* RSI Clock Control Register */
+#define RSI_ARGUMENT                   0xFFC03808 /* RSI Argument Register */
+#define RSI_COMMAND                    0xFFC0380C /* RSI Command Register */
+#define RSI_RESP_CMD                   0xFFC03810 /* RSI Response Command Register */
+#define RSI_RESPONSE0                  0xFFC03814 /* RSI Response Register */
+#define RSI_RESPONSE1                  0xFFC03818 /* RSI Response Register */
+#define RSI_RESPONSE2                  0xFFC0381C /* RSI Response Register */
+#define RSI_RESPONSE3                  0xFFC03820 /* RSI Response Register */
+#define RSI_DATA_TIMER                 0xFFC03824 /* RSI Data Timer Register */
+#define RSI_DATA_LGTH                  0xFFC03828 /* RSI Data Length Register */
+#define RSI_DATA_CONTROL               0xFFC0382C /* RSI Data Control Register */
+#define RSI_DATA_CNT                   0xFFC03830 /* RSI Data Counter Register */
+#define RSI_STATUS                     0xFFC03834 /* RSI Status Register */
+#define RSI_STATUSCL                   0xFFC03838 /* RSI Status Clear Register */
+#define RSI_MASK0                      0xFFC0383C /* RSI Interrupt 0 Mask Register */
+#define RSI_MASK1                      0xFFC03840 /* RSI Interrupt 1 Mask Register */
+#define RSI_FIFO_CNT                   0xFFC03848 /* RSI FIFO Counter Register */
+#define RSI_CEATA_CONTROL              0xFFC0384C /* RSI CEATA Register */
+#define RSI_FIFO                       0xFFC03880 /* RSI Data FIFO Register */
+#define RSI_ESTAT                      0xFFC038C0 /* RSI Exception Status Register */
+#define RSI_EMASK                      0xFFC038C4 /* RSI Exception Mask Register */
+#define RSI_CONFIG                     0xFFC038C8 /* RSI Configuration Register */
+#define RSI_RD_WAIT_EN                 0xFFC038CC /* RSI Read Wait Enable Register */
+#define RSI_PID0                       0xFFC03FE0 /* RSI Peripheral ID Register 0 */
+#define RSI_PID1                       0xFFC03FE4 /* RSI Peripheral ID Register 1 */
+#define RSI_PID2                       0xFFC03FE8 /* RSI Peripheral ID Register 2 */
+#define RSI_PID3                       0xFFC03FEC /* RSI Peripheral ID Register 3 */
+#define RSI_PID4                       0xFFC03FF0 /* RSI Peripheral ID Register 4 */
+#define RSI_PID5                       0xFFC03FF4 /* RSI Peripheral ID Register 5 */
+#define RSI_PID6                       0xFFC03FF8 /* RSI Peripheral ID Register 6 */
+#define RSI_PID7                       0xFFC03FFC /* RSI Peripheral ID Register 7 */
+
+#endif /* _DEF_BF514_H */
diff --git a/arch/blackfin/mach-bf518/include/mach/defBF516.h b/arch/blackfin/mach-bf518/include/mach/defBF516.h
new file mode 100644
index 000000000000..149a269306c5
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/defBF516.h
@@ -0,0 +1,490 @@
+/*
+ * File:         include/asm-blackfin/mach-bf518/defBF516.h
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:
+ *
+ * Rev:
+ *
+ * Modified:
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, 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; see the file COPYING.
+ * If not, write to the Free Software Foundation,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef _DEF_BF516_H
+#define _DEF_BF516_H
+
+/* Include all Core registers and bit definitions */
+#include <asm/def_LPBlackfin.h>
+
+/* SYSTEM & MMR ADDRESS DEFINITIONS FOR ADSP-BF516 */
+
+/* Include defBF51x_base.h for the set of #defines that are common to all ADSP-BF51x processors */
+#include "defBF51x_base.h"
+
+/* The following are the #defines needed by ADSP-BF516 that are not in the common header */
+/* 10/100 Ethernet Controller	(0xFFC03000 - 0xFFC031FF) */
+
+#define EMAC_OPMODE             0xFFC03000       /* Operating Mode Register                              */
+#define EMAC_ADDRLO             0xFFC03004       /* Address Low (32 LSBs) Register                       */
+#define EMAC_ADDRHI             0xFFC03008       /* Address High (16 MSBs) Register                      */
+#define EMAC_HASHLO             0xFFC0300C       /* Multicast Hash Table Low (Bins 31-0) Register        */
+#define EMAC_HASHHI             0xFFC03010       /* Multicast Hash Table High (Bins 63-32) Register      */
+#define EMAC_STAADD             0xFFC03014       /* Station Management Address Register                  */
+#define EMAC_STADAT             0xFFC03018       /* Station Management Data Register                     */
+#define EMAC_FLC                0xFFC0301C       /* Flow Control Register                                */
+#define EMAC_VLAN1              0xFFC03020       /* VLAN1 Tag Register                                   */
+#define EMAC_VLAN2              0xFFC03024       /* VLAN2 Tag Register                                   */
+#define EMAC_WKUP_CTL           0xFFC0302C       /* Wake-Up Control/Status Register                      */
+#define EMAC_WKUP_FFMSK0        0xFFC03030       /* Wake-Up Frame Filter 0 Byte Mask Register            */
+#define EMAC_WKUP_FFMSK1        0xFFC03034       /* Wake-Up Frame Filter 1 Byte Mask Register            */
+#define EMAC_WKUP_FFMSK2        0xFFC03038       /* Wake-Up Frame Filter 2 Byte Mask Register            */
+#define EMAC_WKUP_FFMSK3        0xFFC0303C       /* Wake-Up Frame Filter 3 Byte Mask Register            */
+#define EMAC_WKUP_FFCMD         0xFFC03040       /* Wake-Up Frame Filter Commands Register               */
+#define EMAC_WKUP_FFOFF         0xFFC03044       /* Wake-Up Frame Filter Offsets Register                */
+#define EMAC_WKUP_FFCRC0        0xFFC03048       /* Wake-Up Frame Filter 0,1 CRC-16 Register             */
+#define EMAC_WKUP_FFCRC1        0xFFC0304C       /* Wake-Up Frame Filter 2,3 CRC-16 Register             */
+
+#define EMAC_SYSCTL             0xFFC03060       /* EMAC System Control Register                         */
+#define EMAC_SYSTAT             0xFFC03064       /* EMAC System Status Register                          */
+#define EMAC_RX_STAT            0xFFC03068       /* RX Current Frame Status Register                     */
+#define EMAC_RX_STKY            0xFFC0306C       /* RX Sticky Frame Status Register                      */
+#define EMAC_RX_IRQE            0xFFC03070       /* RX Frame Status Interrupt Enables Register           */
+#define EMAC_TX_STAT            0xFFC03074       /* TX Current Frame Status Register                     */
+#define EMAC_TX_STKY            0xFFC03078       /* TX Sticky Frame Status Register                      */
+#define EMAC_TX_IRQE            0xFFC0307C       /* TX Frame Status Interrupt Enables Register           */
+
+#define EMAC_MMC_CTL            0xFFC03080       /* MMC Counter Control Register                         */
+#define EMAC_MMC_RIRQS          0xFFC03084       /* MMC RX Interrupt Status Register                     */
+#define EMAC_MMC_RIRQE          0xFFC03088       /* MMC RX Interrupt Enables Register                    */
+#define EMAC_MMC_TIRQS          0xFFC0308C       /* MMC TX Interrupt Status Register                     */
+#define EMAC_MMC_TIRQE          0xFFC03090       /* MMC TX Interrupt Enables Register                    */
+
+#define EMAC_RXC_OK             0xFFC03100       /* RX Frame Successful Count                            */
+#define EMAC_RXC_FCS            0xFFC03104       /* RX Frame FCS Failure Count                           */
+#define EMAC_RXC_ALIGN          0xFFC03108       /* RX Alignment Error Count                             */
+#define EMAC_RXC_OCTET          0xFFC0310C       /* RX Octets Successfully Received Count                */
+#define EMAC_RXC_DMAOVF         0xFFC03110       /* Internal MAC Sublayer Error RX Frame Count           */
+#define EMAC_RXC_UNICST         0xFFC03114       /* Unicast RX Frame Count                               */
+#define EMAC_RXC_MULTI          0xFFC03118       /* Multicast RX Frame Count                             */
+#define EMAC_RXC_BROAD          0xFFC0311C       /* Broadcast RX Frame Count                             */
+#define EMAC_RXC_LNERRI         0xFFC03120       /* RX Frame In Range Error Count                        */
+#define EMAC_RXC_LNERRO         0xFFC03124       /* RX Frame Out Of Range Error Count                    */
+#define EMAC_RXC_LONG           0xFFC03128       /* RX Frame Too Long Count                              */
+#define EMAC_RXC_MACCTL         0xFFC0312C       /* MAC Control RX Frame Count                           */
+#define EMAC_RXC_OPCODE         0xFFC03130       /* Unsupported Op-Code RX Frame Count                   */
+#define EMAC_RXC_PAUSE          0xFFC03134       /* MAC Control Pause RX Frame Count                     */
+#define EMAC_RXC_ALLFRM         0xFFC03138       /* Overall RX Frame Count                               */
+#define EMAC_RXC_ALLOCT         0xFFC0313C       /* Overall RX Octet Count                               */
+#define EMAC_RXC_TYPED          0xFFC03140       /* Type/Length Consistent RX Frame Count                */
+#define EMAC_RXC_SHORT          0xFFC03144       /* RX Frame Fragment Count - Byte Count x < 64          */
+#define EMAC_RXC_EQ64           0xFFC03148       /* Good RX Frame Count - Byte Count x = 64              */
+#define EMAC_RXC_LT128          0xFFC0314C       /* Good RX Frame Count - Byte Count  64 < x < 128       */
+#define EMAC_RXC_LT256          0xFFC03150       /* Good RX Frame Count - Byte Count 128 <= x < 256      */
+#define EMAC_RXC_LT512          0xFFC03154       /* Good RX Frame Count - Byte Count 256 <= x < 512      */
+#define EMAC_RXC_LT1024         0xFFC03158       /* Good RX Frame Count - Byte Count 512 <= x < 1024     */
+#define EMAC_RXC_GE1024         0xFFC0315C       /* Good RX Frame Count - Byte Count x >= 1024           */
+
+#define EMAC_TXC_OK             0xFFC03180       /* TX Frame Successful Count                             */
+#define EMAC_TXC_1COL           0xFFC03184       /* TX Frames Successful After Single Collision Count     */
+#define EMAC_TXC_GT1COL         0xFFC03188       /* TX Frames Successful After Multiple Collisions Count  */
+#define EMAC_TXC_OCTET          0xFFC0318C       /* TX Octets Successfully Received Count                 */
+#define EMAC_TXC_DEFER          0xFFC03190       /* TX Frame Delayed Due To Busy Count                    */
+#define EMAC_TXC_LATECL         0xFFC03194       /* Late TX Collisions Count                              */
+#define EMAC_TXC_XS_COL         0xFFC03198       /* TX Frame Failed Due To Excessive Collisions Count     */
+#define EMAC_TXC_DMAUND         0xFFC0319C       /* Internal MAC Sublayer Error TX Frame Count            */
+#define EMAC_TXC_CRSERR         0xFFC031A0       /* Carrier Sense Deasserted During TX Frame Count        */
+#define EMAC_TXC_UNICST         0xFFC031A4       /* Unicast TX Frame Count                                */
+#define EMAC_TXC_MULTI          0xFFC031A8       /* Multicast TX Frame Count                              */
+#define EMAC_TXC_BROAD          0xFFC031AC       /* Broadcast TX Frame Count                              */
+#define EMAC_TXC_XS_DFR         0xFFC031B0       /* TX Frames With Excessive Deferral Count               */
+#define EMAC_TXC_MACCTL         0xFFC031B4       /* MAC Control TX Frame Count                            */
+#define EMAC_TXC_ALLFRM         0xFFC031B8       /* Overall TX Frame Count                                */
+#define EMAC_TXC_ALLOCT         0xFFC031BC       /* Overall TX Octet Count                                */
+#define EMAC_TXC_EQ64           0xFFC031C0       /* Good TX Frame Count - Byte Count x = 64               */
+#define EMAC_TXC_LT128          0xFFC031C4       /* Good TX Frame Count - Byte Count  64 < x < 128        */
+#define EMAC_TXC_LT256          0xFFC031C8       /* Good TX Frame Count - Byte Count 128 <= x < 256       */
+#define EMAC_TXC_LT512          0xFFC031CC       /* Good TX Frame Count - Byte Count 256 <= x < 512       */
+#define EMAC_TXC_LT1024         0xFFC031D0       /* Good TX Frame Count - Byte Count 512 <= x < 1024      */
+#define EMAC_TXC_GE1024         0xFFC031D4       /* Good TX Frame Count - Byte Count x >= 1024            */
+#define EMAC_TXC_ABORT          0xFFC031D8       /* Total TX Frames Aborted Count                         */
+
+/* Listing for IEEE-Supported Count Registers */
+
+#define FramesReceivedOK                EMAC_RXC_OK        /* RX Frame Successful Count                            */
+#define FrameCheckSequenceErrors        EMAC_RXC_FCS       /* RX Frame FCS Failure Count                           */
+#define AlignmentErrors                 EMAC_RXC_ALIGN     /* RX Alignment Error Count                             */
+#define OctetsReceivedOK                EMAC_RXC_OCTET     /* RX Octets Successfully Received Count                */
+#define FramesLostDueToIntMACRcvError   EMAC_RXC_DMAOVF    /* Internal MAC Sublayer Error RX Frame Count           */
+#define UnicastFramesReceivedOK         EMAC_RXC_UNICST    /* Unicast RX Frame Count                               */
+#define MulticastFramesReceivedOK       EMAC_RXC_MULTI     /* Multicast RX Frame Count                             */
+#define BroadcastFramesReceivedOK       EMAC_RXC_BROAD     /* Broadcast RX Frame Count                             */
+#define InRangeLengthErrors             EMAC_RXC_LNERRI    /* RX Frame In Range Error Count                        */
+#define OutOfRangeLengthField           EMAC_RXC_LNERRO    /* RX Frame Out Of Range Error Count                    */
+#define FrameTooLongErrors              EMAC_RXC_LONG      /* RX Frame Too Long Count                              */
+#define MACControlFramesReceived        EMAC_RXC_MACCTL    /* MAC Control RX Frame Count                           */
+#define UnsupportedOpcodesReceived      EMAC_RXC_OPCODE    /* Unsupported Op-Code RX Frame Count                   */
+#define PAUSEMACCtrlFramesReceived      EMAC_RXC_PAUSE     /* MAC Control Pause RX Frame Count                     */
+#define FramesReceivedAll               EMAC_RXC_ALLFRM    /* Overall RX Frame Count                               */
+#define OctetsReceivedAll               EMAC_RXC_ALLOCT    /* Overall RX Octet Count                               */
+#define TypedFramesReceived             EMAC_RXC_TYPED     /* Type/Length Consistent RX Frame Count                */
+#define FramesLenLt64Received           EMAC_RXC_SHORT     /* RX Frame Fragment Count - Byte Count x < 64          */
+#define FramesLenEq64Received           EMAC_RXC_EQ64      /* Good RX Frame Count - Byte Count x = 64              */
+#define FramesLen65_127Received         EMAC_RXC_LT128     /* Good RX Frame Count - Byte Count  64 < x < 128       */
+#define FramesLen128_255Received        EMAC_RXC_LT256     /* Good RX Frame Count - Byte Count 128 <= x < 256      */
+#define FramesLen256_511Received        EMAC_RXC_LT512     /* Good RX Frame Count - Byte Count 256 <= x < 512      */
+#define FramesLen512_1023Received       EMAC_RXC_LT1024    /* Good RX Frame Count - Byte Count 512 <= x < 1024     */
+#define FramesLen1024_MaxReceived       EMAC_RXC_GE1024    /* Good RX Frame Count - Byte Count x >= 1024           */
+
+#define FramesTransmittedOK             EMAC_TXC_OK        /* TX Frame Successful Count                            */
+#define SingleCollisionFrames           EMAC_TXC_1COL      /* TX Frames Successful After Single Collision Count    */
+#define MultipleCollisionFrames         EMAC_TXC_GT1COL    /* TX Frames Successful After Multiple Collisions Count */
+#define OctetsTransmittedOK             EMAC_TXC_OCTET     /* TX Octets Successfully Received Count                */
+#define FramesWithDeferredXmissions     EMAC_TXC_DEFER     /* TX Frame Delayed Due To Busy Count                   */
+#define LateCollisions                  EMAC_TXC_LATECL    /* Late TX Collisions Count                             */
+#define FramesAbortedDueToXSColls       EMAC_TXC_XS_COL    /* TX Frame Failed Due To Excessive Collisions Count    */
+#define FramesLostDueToIntMacXmitError  EMAC_TXC_DMAUND    /* Internal MAC Sublayer Error TX Frame Count           */
+#define CarrierSenseErrors              EMAC_TXC_CRSERR    /* Carrier Sense Deasserted During TX Frame Count       */
+#define UnicastFramesXmittedOK          EMAC_TXC_UNICST    /* Unicast TX Frame Count                               */
+#define MulticastFramesXmittedOK        EMAC_TXC_MULTI     /* Multicast TX Frame Count                             */
+#define BroadcastFramesXmittedOK        EMAC_TXC_BROAD     /* Broadcast TX Frame Count                             */
+#define FramesWithExcessiveDeferral     EMAC_TXC_XS_DFR    /* TX Frames With Excessive Deferral Count              */
+#define MACControlFramesTransmitted     EMAC_TXC_MACCTL    /* MAC Control TX Frame Count                           */
+#define FramesTransmittedAll            EMAC_TXC_ALLFRM    /* Overall TX Frame Count                               */
+#define OctetsTransmittedAll            EMAC_TXC_ALLOCT    /* Overall TX Octet Count                               */
+#define FramesLenEq64Transmitted        EMAC_TXC_EQ64      /* Good TX Frame Count - Byte Count x = 64              */
+#define FramesLen65_127Transmitted      EMAC_TXC_LT128     /* Good TX Frame Count - Byte Count  64 < x < 128       */
+#define FramesLen128_255Transmitted     EMAC_TXC_LT256     /* Good TX Frame Count - Byte Count 128 <= x < 256      */
+#define FramesLen256_511Transmitted     EMAC_TXC_LT512     /* Good TX Frame Count - Byte Count 256 <= x < 512      */
+#define FramesLen512_1023Transmitted    EMAC_TXC_LT1024    /* Good TX Frame Count - Byte Count 512 <= x < 1024     */
+#define FramesLen1024_MaxTransmitted    EMAC_TXC_GE1024    /* Good TX Frame Count - Byte Count x >= 1024           */
+#define TxAbortedFrames                 EMAC_TXC_ABORT     /* Total TX Frames Aborted Count                        */
+
+/***********************************************************************************
+** System MMR Register Bits And Macros
+**
+** Disclaimer:	All macros are intended to make C and Assembly code more readable.
+**				Use these macros carefully, as any that do left shifts for field
+**				depositing will result in the lower order bits being destroyed.  Any
+**				macro that shifts left to properly position the bit-field should be
+**				used as part of an OR to initialize a register and NOT as a dynamic
+**				modifier UNLESS the lower order bits are saved and ORed back in when
+**				the macro is used.
+*************************************************************************************/
+
+/************************  ETHERNET 10/100 CONTROLLER MASKS  ************************/
+
+/* EMAC_OPMODE Masks */
+
+#define	RE                 0x00000001     /* Receiver Enable                                    */
+#define	ASTP               0x00000002     /* Enable Automatic Pad Stripping On RX Frames        */
+#define	HU                 0x00000010     /* Hash Filter Unicast Address                        */
+#define	HM                 0x00000020     /* Hash Filter Multicast Address                      */
+#define	PAM                0x00000040     /* Pass-All-Multicast Mode Enable                     */
+#define	PR                 0x00000080     /* Promiscuous Mode Enable                            */
+#define	IFE                0x00000100     /* Inverse Filtering Enable                           */
+#define	DBF                0x00000200     /* Disable Broadcast Frame Reception                  */
+#define	PBF                0x00000400     /* Pass Bad Frames Enable                             */
+#define	PSF                0x00000800     /* Pass Short Frames Enable                           */
+#define	RAF                0x00001000     /* Receive-All Mode                                   */
+#define	TE                 0x00010000     /* Transmitter Enable                                 */
+#define	DTXPAD             0x00020000     /* Disable Automatic TX Padding                       */
+#define	DTXCRC             0x00040000     /* Disable Automatic TX CRC Generation                */
+#define	DC                 0x00080000     /* Deferral Check                                     */
+#define	BOLMT              0x00300000     /* Back-Off Limit                                     */
+#define	BOLMT_10           0x00000000     /*		10-bit range                            */
+#define	BOLMT_8            0x00100000     /*		8-bit range                             */
+#define	BOLMT_4            0x00200000     /*		4-bit range                             */
+#define	BOLMT_1            0x00300000     /*		1-bit range                             */
+#define	DRTY               0x00400000     /* Disable TX Retry On Collision                      */
+#define	LCTRE              0x00800000     /* Enable TX Retry On Late Collision                  */
+#define	RMII               0x01000000     /* RMII/MII* Mode                                     */
+#define	RMII_10            0x02000000     /* Speed Select for RMII Port (10MBit/100MBit*)       */
+#define	FDMODE             0x04000000     /* Duplex Mode Enable (Full/Half*)                    */
+#define	LB                 0x08000000     /* Internal Loopback Enable                           */
+#define	DRO                0x10000000     /* Disable Receive Own Frames (Half-Duplex Mode)      */
+
+/* EMAC_STAADD Masks */
+
+#define	STABUSY            0x00000001     /* Initiate Station Mgt Reg Access / STA Busy Stat    */
+#define	STAOP              0x00000002     /* Station Management Operation Code (Write/Read*)    */
+#define	STADISPRE          0x00000004     /* Disable Preamble Generation                        */
+#define	STAIE              0x00000008     /* Station Mgt. Transfer Done Interrupt Enable        */
+#define	REGAD              0x000007C0     /* STA Register Address                               */
+#define	PHYAD              0x0000F800     /* PHY Device Address                                 */
+
+#define	SET_REGAD(x) (((x)&0x1F)<<  6 )   /* Set STA Register Address                           */
+#define	SET_PHYAD(x) (((x)&0x1F)<< 11 )   /* Set PHY Device Address                             */
+
+/* EMAC_STADAT Mask */
+
+#define	STADATA            0x0000FFFF     /* Station Management Data                            */
+
+/* EMAC_FLC Masks */
+
+#define	FLCBUSY            0x00000001     /* Send Flow Ctrl Frame / Flow Ctrl Busy Status       */
+#define	FLCE               0x00000002     /* Flow Control Enable                                */
+#define	PCF                0x00000004     /* Pass Control Frames                                */
+#define	BKPRSEN            0x00000008     /* Enable Backpressure                                */
+#define	FLCPAUSE           0xFFFF0000     /* Pause Time                                         */
+
+#define	SET_FLCPAUSE(x) (((x)&0xFFFF)<< 16) /* Set Pause Time                                   */
+
+/* EMAC_WKUP_CTL Masks */
+
+#define	CAPWKFRM           0x00000001    /* Capture Wake-Up Frames                              */
+#define	MPKE               0x00000002    /* Magic Packet Enable                                 */
+#define	RWKE               0x00000004    /* Remote Wake-Up Frame Enable                         */
+#define	GUWKE              0x00000008    /* Global Unicast Wake Enable                          */
+#define	MPKS               0x00000020    /* Magic Packet Received Status                        */
+#define	RWKS               0x00000F00    /* Wake-Up Frame Received Status, Filters 3:0          */
+
+/* EMAC_WKUP_FFCMD Masks */
+
+#define	WF0_E              0x00000001    /* Enable Wake-Up Filter 0                              */
+#define	WF0_T              0x00000008    /* Wake-Up Filter 0 Addr Type (Multicast/Unicast*)      */
+#define	WF1_E              0x00000100    /* Enable Wake-Up Filter 1                              */
+#define	WF1_T              0x00000800    /* Wake-Up Filter 1 Addr Type (Multicast/Unicast*)      */
+#define	WF2_E              0x00010000    /* Enable Wake-Up Filter 2                              */
+#define	WF2_T              0x00080000    /* Wake-Up Filter 2 Addr Type (Multicast/Unicast*)      */
+#define	WF3_E              0x01000000    /* Enable Wake-Up Filter 3                              */
+#define	WF3_T              0x08000000    /* Wake-Up Filter 3 Addr Type (Multicast/Unicast*)      */
+
+/* EMAC_WKUP_FFOFF Masks */
+
+#define	WF0_OFF            0x000000FF    /* Wake-Up Filter 0 Pattern Offset                      */
+#define	WF1_OFF            0x0000FF00    /* Wake-Up Filter 1 Pattern Offset                      */
+#define	WF2_OFF            0x00FF0000    /* Wake-Up Filter 2 Pattern Offset                      */
+#define	WF3_OFF            0xFF000000    /* Wake-Up Filter 3 Pattern Offset                      */
+
+#define	SET_WF0_OFF(x) (((x)&0xFF)<<  0 ) /* Set Wake-Up Filter 0 Byte Offset                    */
+#define	SET_WF1_OFF(x) (((x)&0xFF)<<  8 ) /* Set Wake-Up Filter 1 Byte Offset                    */
+#define	SET_WF2_OFF(x) (((x)&0xFF)<< 16 ) /* Set Wake-Up Filter 2 Byte Offset                    */
+#define	SET_WF3_OFF(x) (((x)&0xFF)<< 24 ) /* Set Wake-Up Filter 3 Byte Offset                    */
+/* Set ALL Offsets */
+#define	SET_WF_OFFS(x0,x1,x2,x3) (SET_WF0_OFF((x0))|SET_WF1_OFF((x1))|SET_WF2_OFF((x2))|SET_WF3_OFF((x3)))
+
+/* EMAC_WKUP_FFCRC0 Masks */
+
+#define	WF0_CRC           0x0000FFFF    /* Wake-Up Filter 0 Pattern CRC                           */
+#define	WF1_CRC           0xFFFF0000    /* Wake-Up Filter 1 Pattern CRC                           */
+
+#define	SET_WF0_CRC(x) (((x)&0xFFFF)<<   0 ) /* Set Wake-Up Filter 0 Target CRC                   */
+#define	SET_WF1_CRC(x) (((x)&0xFFFF)<<  16 ) /* Set Wake-Up Filter 1 Target CRC                   */
+
+/* EMAC_WKUP_FFCRC1 Masks */
+
+#define	WF2_CRC           0x0000FFFF    /* Wake-Up Filter 2 Pattern CRC                           */
+#define	WF3_CRC           0xFFFF0000    /* Wake-Up Filter 3 Pattern CRC                           */
+
+#define	SET_WF2_CRC(x) (((x)&0xFFFF)<<   0 ) /* Set Wake-Up Filter 2 Target CRC                   */
+#define	SET_WF3_CRC(x) (((x)&0xFFFF)<<  16 ) /* Set Wake-Up Filter 3 Target CRC                   */
+
+/* EMAC_SYSCTL Masks */
+
+#define	PHYIE             0x00000001    /* PHY_INT Interrupt Enable                               */
+#define	RXDWA             0x00000002    /* Receive Frame DMA Word Alignment (Odd/Even*)           */
+#define	RXCKS             0x00000004    /* Enable RX Frame TCP/UDP Checksum Computation           */
+#define	TXDWA             0x00000010    /* Transmit Frame DMA Word Alignment (Odd/Even*)          */
+#define	MDCDIV            0x00003F00    /* SCLK:MDC Clock Divisor [MDC=SCLK/(2*(N+1))]            */
+
+#define	SET_MDCDIV(x) (((x)&0x3F)<< 8)   /* Set MDC Clock Divisor                                 */
+
+/* EMAC_SYSTAT Masks */
+
+#define	PHYINT            0x00000001    /* PHY_INT Interrupt Status                               */
+#define	MMCINT            0x00000002    /* MMC Counter Interrupt Status                           */
+#define	RXFSINT           0x00000004    /* RX Frame-Status Interrupt Status                       */
+#define	TXFSINT           0x00000008    /* TX Frame-Status Interrupt Status                       */
+#define	WAKEDET           0x00000010    /* Wake-Up Detected Status                                */
+#define	RXDMAERR          0x00000020    /* RX DMA Direction Error Status                          */
+#define	TXDMAERR          0x00000040    /* TX DMA Direction Error Status                          */
+#define	STMDONE           0x00000080    /* Station Mgt. Transfer Done Interrupt Status            */
+
+/* EMAC_RX_STAT, EMAC_RX_STKY, and EMAC_RX_IRQE Masks */
+
+#define	RX_FRLEN          0x000007FF    /* Frame Length In Bytes                                  */
+#define	RX_COMP           0x00001000    /* RX Frame Complete                                      */
+#define	RX_OK             0x00002000    /* RX Frame Received With No Errors                       */
+#define	RX_LONG           0x00004000    /* RX Frame Too Long Error                                */
+#define	RX_ALIGN          0x00008000    /* RX Frame Alignment Error                               */
+#define	RX_CRC            0x00010000    /* RX Frame CRC Error                                     */
+#define	RX_LEN            0x00020000    /* RX Frame Length Error                                  */
+#define	RX_FRAG           0x00040000    /* RX Frame Fragment Error                                */
+#define	RX_ADDR           0x00080000    /* RX Frame Address Filter Failed Error                   */
+#define	RX_DMAO           0x00100000    /* RX Frame DMA Overrun Error                             */
+#define	RX_PHY            0x00200000    /* RX Frame PHY Error                                     */
+#define	RX_LATE           0x00400000    /* RX Frame Late Collision Error                          */
+#define	RX_RANGE          0x00800000    /* RX Frame Length Field Out of Range Error               */
+#define	RX_MULTI          0x01000000    /* RX Multicast Frame Indicator                           */
+#define	RX_BROAD          0x02000000    /* RX Broadcast Frame Indicator                           */
+#define	RX_CTL            0x04000000    /* RX Control Frame Indicator                             */
+#define	RX_UCTL           0x08000000    /* Unsupported RX Control Frame Indicator                 */
+#define	RX_TYPE           0x10000000    /* RX Typed Frame Indicator                               */
+#define	RX_VLAN1          0x20000000    /* RX VLAN1 Frame Indicator                               */
+#define	RX_VLAN2          0x40000000    /* RX VLAN2 Frame Indicator                               */
+#define	RX_ACCEPT         0x80000000    /* RX Frame Accepted Indicator                            */
+
+/*  EMAC_TX_STAT, EMAC_TX_STKY, and EMAC_TX_IRQE Masks  */
+
+#define	TX_COMP           0x00000001    /* TX Frame Complete                                      */
+#define	TX_OK             0x00000002    /* TX Frame Sent With No Errors                           */
+#define	TX_ECOLL          0x00000004    /* TX Frame Excessive Collision Error                     */
+#define	TX_LATE           0x00000008    /* TX Frame Late Collision Error                          */
+#define	TX_DMAU           0x00000010    /* TX Frame DMA Underrun Error (STAT)                     */
+#define	TX_MACE           0x00000010    /* Internal MAC Error Detected (STKY and IRQE)            */
+#define	TX_EDEFER         0x00000020    /* TX Frame Excessive Deferral Error                      */
+#define	TX_BROAD          0x00000040    /* TX Broadcast Frame Indicator                           */
+#define	TX_MULTI          0x00000080    /* TX Multicast Frame Indicator                           */
+#define	TX_CCNT           0x00000F00    /* TX Frame Collision Count                               */
+#define	TX_DEFER          0x00001000    /* TX Frame Deferred Indicator                            */
+#define	TX_CRS            0x00002000    /* TX Frame Carrier Sense Not Asserted Error              */
+#define	TX_LOSS           0x00004000    /* TX Frame Carrier Lost During TX Error                  */
+#define	TX_RETRY          0x00008000    /* TX Frame Successful After Retry                        */
+#define	TX_FRLEN          0x07FF0000    /* TX Frame Length (Bytes)                                */
+
+/* EMAC_MMC_CTL Masks */
+#define	RSTC              0x00000001    /* Reset All Counters                                     */
+#define	CROLL             0x00000002    /* Counter Roll-Over Enable                               */
+#define	CCOR              0x00000004    /* Counter Clear-On-Read Mode Enable                      */
+#define	MMCE              0x00000008    /* Enable MMC Counter Operation                           */
+
+/* EMAC_MMC_RIRQS and EMAC_MMC_RIRQE Masks */
+#define	RX_OK_CNT         0x00000001    /* RX Frames Received With No Errors                      */
+#define	RX_FCS_CNT        0x00000002    /* RX Frames W/Frame Check Sequence Errors                */
+#define	RX_ALIGN_CNT      0x00000004    /* RX Frames With Alignment Errors                        */
+#define	RX_OCTET_CNT      0x00000008    /* RX Octets Received OK                                  */
+#define	RX_LOST_CNT       0x00000010    /* RX Frames Lost Due To Internal MAC RX Error            */
+#define	RX_UNI_CNT        0x00000020    /* Unicast RX Frames Received OK                          */
+#define	RX_MULTI_CNT      0x00000040    /* Multicast RX Frames Received OK                        */
+#define	RX_BROAD_CNT      0x00000080    /* Broadcast RX Frames Received OK                        */
+#define	RX_IRL_CNT        0x00000100    /* RX Frames With In-Range Length Errors                  */
+#define	RX_ORL_CNT        0x00000200    /* RX Frames With Out-Of-Range Length Errors              */
+#define	RX_LONG_CNT       0x00000400    /* RX Frames With Frame Too Long Errors                   */
+#define	RX_MACCTL_CNT     0x00000800    /* MAC Control RX Frames Received                         */
+#define	RX_OPCODE_CTL     0x00001000    /* Unsupported Op-Code RX Frames Received                 */
+#define	RX_PAUSE_CNT      0x00002000    /* PAUSEMAC Control RX Frames Received                    */
+#define	RX_ALLF_CNT       0x00004000    /* All RX Frames Received                                 */
+#define	RX_ALLO_CNT       0x00008000    /* All RX Octets Received                                 */
+#define	RX_TYPED_CNT      0x00010000    /* Typed RX Frames Received                               */
+#define	RX_SHORT_CNT      0x00020000    /* RX Frame Fragments (< 64 Bytes) Received               */
+#define	RX_EQ64_CNT       0x00040000    /* 64-Byte RX Frames Received                             */
+#define	RX_LT128_CNT      0x00080000    /* 65-127-Byte RX Frames Received                         */
+#define	RX_LT256_CNT      0x00100000    /* 128-255-Byte RX Frames Received                        */
+#define	RX_LT512_CNT      0x00200000    /* 256-511-Byte RX Frames Received                        */
+#define	RX_LT1024_CNT     0x00400000    /* 512-1023-Byte RX Frames Received                       */
+#define	RX_GE1024_CNT     0x00800000    /* 1024-Max-Byte RX Frames Received                       */
+
+/* EMAC_MMC_TIRQS and EMAC_MMC_TIRQE Masks  */
+
+#define	TX_OK_CNT         0x00000001    /* TX Frames Sent OK                                      */
+#define	TX_SCOLL_CNT      0x00000002    /* TX Frames With Single Collisions                       */
+#define	TX_MCOLL_CNT      0x00000004    /* TX Frames With Multiple Collisions                     */
+#define	TX_OCTET_CNT      0x00000008    /* TX Octets Sent OK                                      */
+#define	TX_DEFER_CNT      0x00000010    /* TX Frames With Deferred Transmission                   */
+#define	TX_LATE_CNT       0x00000020    /* TX Frames With Late Collisions                         */
+#define	TX_ABORTC_CNT     0x00000040    /* TX Frames Aborted Due To Excess Collisions             */
+#define	TX_LOST_CNT       0x00000080    /* TX Frames Lost Due To Internal MAC TX Error            */
+#define	TX_CRS_CNT        0x00000100    /* TX Frames With Carrier Sense Errors                    */
+#define	TX_UNI_CNT        0x00000200    /* Unicast TX Frames Sent                                 */
+#define	TX_MULTI_CNT      0x00000400    /* Multicast TX Frames Sent                               */
+#define	TX_BROAD_CNT      0x00000800    /* Broadcast TX Frames Sent                               */
+#define	TX_EXDEF_CTL      0x00001000    /* TX Frames With Excessive Deferral                      */
+#define	TX_MACCTL_CNT     0x00002000    /* MAC Control TX Frames Sent                             */
+#define	TX_ALLF_CNT       0x00004000    /* All TX Frames Sent                                     */
+#define	TX_ALLO_CNT       0x00008000    /* All TX Octets Sent                                     */
+#define	TX_EQ64_CNT       0x00010000    /* 64-Byte TX Frames Sent                                 */
+#define	TX_LT128_CNT      0x00020000    /* 65-127-Byte TX Frames Sent                             */
+#define	TX_LT256_CNT      0x00040000    /* 128-255-Byte TX Frames Sent                            */
+#define	TX_LT512_CNT      0x00080000    /* 256-511-Byte TX Frames Sent                            */
+#define	TX_LT1024_CNT     0x00100000    /* 512-1023-Byte TX Frames Sent                           */
+#define	TX_GE1024_CNT     0x00200000    /* 1024-Max-Byte TX Frames Sent                           */
+#define	TX_ABORT_CNT      0x00400000    /* TX Frames Aborted                                      */
+
+/* SDH Registers */
+
+#define SDH_PWR_CTL                    0xFFC03900 /* SDH Power Control */
+#define SDH_CLK_CTL                    0xFFC03904 /* SDH Clock Control */
+#define SDH_ARGUMENT                   0xFFC03908 /* SDH Argument */
+#define SDH_COMMAND                    0xFFC0390C /* SDH Command */
+#define SDH_RESP_CMD                   0xFFC03910 /* SDH Response Command */
+#define SDH_RESPONSE0                  0xFFC03914 /* SDH Response0 */
+#define SDH_RESPONSE1                  0xFFC03918 /* SDH Response1 */
+#define SDH_RESPONSE2                  0xFFC0391C /* SDH Response2 */
+#define SDH_RESPONSE3                  0xFFC03920 /* SDH Response3 */
+#define SDH_DATA_TIMER                 0xFFC03924 /* SDH Data Timer */
+#define SDH_DATA_LGTH                  0xFFC03928 /* SDH Data Length */
+#define SDH_DATA_CTL                   0xFFC0392C /* SDH Data Control */
+#define SDH_DATA_CNT                   0xFFC03930 /* SDH Data Counter */
+#define SDH_STATUS                     0xFFC03934 /* SDH Status */
+#define SDH_STATUS_CLR                 0xFFC03938 /* SDH Status Clear */
+#define SDH_MASK0                      0xFFC0393C /* SDH Interrupt0 Mask */
+#define SDH_MASK1                      0xFFC03940 /* SDH Interrupt1 Mask */
+#define SDH_FIFO_CNT                   0xFFC03948 /* SDH FIFO Counter */
+#define SDH_FIFO                       0xFFC03980 /* SDH Data FIFO */
+#define SDH_E_STATUS                   0xFFC039C0 /* SDH Exception Status */
+#define SDH_E_MASK                     0xFFC039C4 /* SDH Exception Mask */
+#define SDH_CFG                        0xFFC039C8 /* SDH Configuration */
+#define SDH_RD_WAIT_EN                 0xFFC039CC /* SDH Read Wait Enable */
+#define SDH_PID0                       0xFFC039D0 /* SDH Peripheral Identification0 */
+#define SDH_PID1                       0xFFC039D4 /* SDH Peripheral Identification1 */
+#define SDH_PID2                       0xFFC039D8 /* SDH Peripheral Identification2 */
+#define SDH_PID3                       0xFFC039DC /* SDH Peripheral Identification3 */
+#define SDH_PID4                       0xFFC039E0 /* SDH Peripheral Identification4 */
+#define SDH_PID5                       0xFFC039E4 /* SDH Peripheral Identification5 */
+#define SDH_PID6                       0xFFC039E8 /* SDH Peripheral Identification6 */
+#define SDH_PID7                       0xFFC039EC /* SDH Peripheral Identification7 */
+
+/* Removable Storage Interface Registers */
+
+#define RSI_PWR_CONTROL                0xFFC03800 /* RSI Power Control Register */
+#define RSI_CLK_CONTROL                0xFFC03804 /* RSI Clock Control Register */
+#define RSI_ARGUMENT                   0xFFC03808 /* RSI Argument Register */
+#define RSI_COMMAND                    0xFFC0380C /* RSI Command Register */
+#define RSI_RESP_CMD                   0xFFC03810 /* RSI Response Command Register */
+#define RSI_RESPONSE0                  0xFFC03814 /* RSI Response Register */
+#define RSI_RESPONSE1                  0xFFC03818 /* RSI Response Register */
+#define RSI_RESPONSE2                  0xFFC0381C /* RSI Response Register */
+#define RSI_RESPONSE3                  0xFFC03820 /* RSI Response Register */
+#define RSI_DATA_TIMER                 0xFFC03824 /* RSI Data Timer Register */
+#define RSI_DATA_LGTH                  0xFFC03828 /* RSI Data Length Register */
+#define RSI_DATA_CONTROL               0xFFC0382C /* RSI Data Control Register */
+#define RSI_DATA_CNT                   0xFFC03830 /* RSI Data Counter Register */
+#define RSI_STATUS                     0xFFC03834 /* RSI Status Register */
+#define RSI_STATUSCL                   0xFFC03838 /* RSI Status Clear Register */
+#define RSI_MASK0                      0xFFC0383C /* RSI Interrupt 0 Mask Register */
+#define RSI_MASK1                      0xFFC03840 /* RSI Interrupt 1 Mask Register */
+#define RSI_FIFO_CNT                   0xFFC03848 /* RSI FIFO Counter Register */
+#define RSI_CEATA_CONTROL              0xFFC0384C /* RSI CEATA Register */
+#define RSI_FIFO                       0xFFC03880 /* RSI Data FIFO Register */
+#define RSI_ESTAT                      0xFFC038C0 /* RSI Exception Status Register */
+#define RSI_EMASK                      0xFFC038C4 /* RSI Exception Mask Register */
+#define RSI_CONFIG                     0xFFC038C8 /* RSI Configuration Register */
+#define RSI_RD_WAIT_EN                 0xFFC038CC /* RSI Read Wait Enable Register */
+#define RSI_PID0                       0xFFC03FE0 /* RSI Peripheral ID Register 0 */
+#define RSI_PID1                       0xFFC03FE4 /* RSI Peripheral ID Register 1 */
+#define RSI_PID2                       0xFFC03FE8 /* RSI Peripheral ID Register 2 */
+#define RSI_PID3                       0xFFC03FEC /* RSI Peripheral ID Register 3 */
+#define RSI_PID4                       0xFFC03FF0 /* RSI Peripheral ID Register 4 */
+#define RSI_PID5                       0xFFC03FF4 /* RSI Peripheral ID Register 5 */
+#define RSI_PID6                       0xFFC03FF8 /* RSI Peripheral ID Register 6 */
+#define RSI_PID7                       0xFFC03FFC /* RSI Peripheral ID Register 7 */
+
+#endif /* _DEF_BF516_H */
diff --git a/arch/blackfin/mach-bf518/include/mach/defBF518.h b/arch/blackfin/mach-bf518/include/mach/defBF518.h
new file mode 100644
index 000000000000..6e982abf4ede
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/defBF518.h
@@ -0,0 +1,651 @@
+/*
+ * File:         include/asm-blackfin/mach-bf518/defBF518.h
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:
+ *
+ * Rev:
+ *
+ * Modified:
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, 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; see the file COPYING.
+ * If not, write to the Free Software Foundation,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef _DEF_BF518_H
+#define _DEF_BF518_H
+
+/* Include all Core registers and bit definitions */
+#include <asm/def_LPBlackfin.h>
+
+/* SYSTEM & MMR ADDRESS DEFINITIONS FOR ADSP-BF518 */
+
+/* Include defBF51x_base.h for the set of #defines that are common to all ADSP-BF51x processors */
+#include "defBF51x_base.h"
+
+/* The following are the #defines needed by ADSP-BF518 that are not in the common header */
+/* 10/100 Ethernet Controller	(0xFFC03000 - 0xFFC031FF) */
+
+#define EMAC_OPMODE             0xFFC03000       /* Operating Mode Register                              */
+#define EMAC_ADDRLO             0xFFC03004       /* Address Low (32 LSBs) Register                       */
+#define EMAC_ADDRHI             0xFFC03008       /* Address High (16 MSBs) Register                      */
+#define EMAC_HASHLO             0xFFC0300C       /* Multicast Hash Table Low (Bins 31-0) Register        */
+#define EMAC_HASHHI             0xFFC03010       /* Multicast Hash Table High (Bins 63-32) Register      */
+#define EMAC_STAADD             0xFFC03014       /* Station Management Address Register                  */
+#define EMAC_STADAT             0xFFC03018       /* Station Management Data Register                     */
+#define EMAC_FLC                0xFFC0301C       /* Flow Control Register                                */
+#define EMAC_VLAN1              0xFFC03020       /* VLAN1 Tag Register                                   */
+#define EMAC_VLAN2              0xFFC03024       /* VLAN2 Tag Register                                   */
+#define EMAC_WKUP_CTL           0xFFC0302C       /* Wake-Up Control/Status Register                      */
+#define EMAC_WKUP_FFMSK0        0xFFC03030       /* Wake-Up Frame Filter 0 Byte Mask Register            */
+#define EMAC_WKUP_FFMSK1        0xFFC03034       /* Wake-Up Frame Filter 1 Byte Mask Register            */
+#define EMAC_WKUP_FFMSK2        0xFFC03038       /* Wake-Up Frame Filter 2 Byte Mask Register            */
+#define EMAC_WKUP_FFMSK3        0xFFC0303C       /* Wake-Up Frame Filter 3 Byte Mask Register            */
+#define EMAC_WKUP_FFCMD         0xFFC03040       /* Wake-Up Frame Filter Commands Register               */
+#define EMAC_WKUP_FFOFF         0xFFC03044       /* Wake-Up Frame Filter Offsets Register                */
+#define EMAC_WKUP_FFCRC0        0xFFC03048       /* Wake-Up Frame Filter 0,1 CRC-16 Register             */
+#define EMAC_WKUP_FFCRC1        0xFFC0304C       /* Wake-Up Frame Filter 2,3 CRC-16 Register             */
+
+#define EMAC_SYSCTL             0xFFC03060       /* EMAC System Control Register                         */
+#define EMAC_SYSTAT             0xFFC03064       /* EMAC System Status Register                          */
+#define EMAC_RX_STAT            0xFFC03068       /* RX Current Frame Status Register                     */
+#define EMAC_RX_STKY            0xFFC0306C       /* RX Sticky Frame Status Register                      */
+#define EMAC_RX_IRQE            0xFFC03070       /* RX Frame Status Interrupt Enables Register           */
+#define EMAC_TX_STAT            0xFFC03074       /* TX Current Frame Status Register                     */
+#define EMAC_TX_STKY            0xFFC03078       /* TX Sticky Frame Status Register                      */
+#define EMAC_TX_IRQE            0xFFC0307C       /* TX Frame Status Interrupt Enables Register           */
+
+#define EMAC_MMC_CTL            0xFFC03080       /* MMC Counter Control Register                         */
+#define EMAC_MMC_RIRQS          0xFFC03084       /* MMC RX Interrupt Status Register                     */
+#define EMAC_MMC_RIRQE          0xFFC03088       /* MMC RX Interrupt Enables Register                    */
+#define EMAC_MMC_TIRQS          0xFFC0308C       /* MMC TX Interrupt Status Register                     */
+#define EMAC_MMC_TIRQE          0xFFC03090       /* MMC TX Interrupt Enables Register                    */
+
+#define EMAC_RXC_OK             0xFFC03100       /* RX Frame Successful Count                            */
+#define EMAC_RXC_FCS            0xFFC03104       /* RX Frame FCS Failure Count                           */
+#define EMAC_RXC_ALIGN          0xFFC03108       /* RX Alignment Error Count                             */
+#define EMAC_RXC_OCTET          0xFFC0310C       /* RX Octets Successfully Received Count                */
+#define EMAC_RXC_DMAOVF         0xFFC03110       /* Internal MAC Sublayer Error RX Frame Count           */
+#define EMAC_RXC_UNICST         0xFFC03114       /* Unicast RX Frame Count                               */
+#define EMAC_RXC_MULTI          0xFFC03118       /* Multicast RX Frame Count                             */
+#define EMAC_RXC_BROAD          0xFFC0311C       /* Broadcast RX Frame Count                             */
+#define EMAC_RXC_LNERRI         0xFFC03120       /* RX Frame In Range Error Count                        */
+#define EMAC_RXC_LNERRO         0xFFC03124       /* RX Frame Out Of Range Error Count                    */
+#define EMAC_RXC_LONG           0xFFC03128       /* RX Frame Too Long Count                              */
+#define EMAC_RXC_MACCTL         0xFFC0312C       /* MAC Control RX Frame Count                           */
+#define EMAC_RXC_OPCODE         0xFFC03130       /* Unsupported Op-Code RX Frame Count                   */
+#define EMAC_RXC_PAUSE          0xFFC03134       /* MAC Control Pause RX Frame Count                     */
+#define EMAC_RXC_ALLFRM         0xFFC03138       /* Overall RX Frame Count                               */
+#define EMAC_RXC_ALLOCT         0xFFC0313C       /* Overall RX Octet Count                               */
+#define EMAC_RXC_TYPED          0xFFC03140       /* Type/Length Consistent RX Frame Count                */
+#define EMAC_RXC_SHORT          0xFFC03144       /* RX Frame Fragment Count - Byte Count x < 64          */
+#define EMAC_RXC_EQ64           0xFFC03148       /* Good RX Frame Count - Byte Count x = 64              */
+#define EMAC_RXC_LT128          0xFFC0314C       /* Good RX Frame Count - Byte Count  64 < x < 128       */
+#define EMAC_RXC_LT256          0xFFC03150       /* Good RX Frame Count - Byte Count 128 <= x < 256      */
+#define EMAC_RXC_LT512          0xFFC03154       /* Good RX Frame Count - Byte Count 256 <= x < 512      */
+#define EMAC_RXC_LT1024         0xFFC03158       /* Good RX Frame Count - Byte Count 512 <= x < 1024     */
+#define EMAC_RXC_GE1024         0xFFC0315C       /* Good RX Frame Count - Byte Count x >= 1024           */
+
+#define EMAC_TXC_OK             0xFFC03180       /* TX Frame Successful Count                             */
+#define EMAC_TXC_1COL           0xFFC03184       /* TX Frames Successful After Single Collision Count     */
+#define EMAC_TXC_GT1COL         0xFFC03188       /* TX Frames Successful After Multiple Collisions Count  */
+#define EMAC_TXC_OCTET          0xFFC0318C       /* TX Octets Successfully Received Count                 */
+#define EMAC_TXC_DEFER          0xFFC03190       /* TX Frame Delayed Due To Busy Count                    */
+#define EMAC_TXC_LATECL         0xFFC03194       /* Late TX Collisions Count                              */
+#define EMAC_TXC_XS_COL         0xFFC03198       /* TX Frame Failed Due To Excessive Collisions Count     */
+#define EMAC_TXC_DMAUND         0xFFC0319C       /* Internal MAC Sublayer Error TX Frame Count            */
+#define EMAC_TXC_CRSERR         0xFFC031A0       /* Carrier Sense Deasserted During TX Frame Count        */
+#define EMAC_TXC_UNICST         0xFFC031A4       /* Unicast TX Frame Count                                */
+#define EMAC_TXC_MULTI          0xFFC031A8       /* Multicast TX Frame Count                              */
+#define EMAC_TXC_BROAD          0xFFC031AC       /* Broadcast TX Frame Count                              */
+#define EMAC_TXC_XS_DFR         0xFFC031B0       /* TX Frames With Excessive Deferral Count               */
+#define EMAC_TXC_MACCTL         0xFFC031B4       /* MAC Control TX Frame Count                            */
+#define EMAC_TXC_ALLFRM         0xFFC031B8       /* Overall TX Frame Count                                */
+#define EMAC_TXC_ALLOCT         0xFFC031BC       /* Overall TX Octet Count                                */
+#define EMAC_TXC_EQ64           0xFFC031C0       /* Good TX Frame Count - Byte Count x = 64               */
+#define EMAC_TXC_LT128          0xFFC031C4       /* Good TX Frame Count - Byte Count  64 < x < 128        */
+#define EMAC_TXC_LT256          0xFFC031C8       /* Good TX Frame Count - Byte Count 128 <= x < 256       */
+#define EMAC_TXC_LT512          0xFFC031CC       /* Good TX Frame Count - Byte Count 256 <= x < 512       */
+#define EMAC_TXC_LT1024         0xFFC031D0       /* Good TX Frame Count - Byte Count 512 <= x < 1024      */
+#define EMAC_TXC_GE1024         0xFFC031D4       /* Good TX Frame Count - Byte Count x >= 1024            */
+#define EMAC_TXC_ABORT          0xFFC031D8       /* Total TX Frames Aborted Count                         */
+
+/* Listing for IEEE-Supported Count Registers */
+
+#define FramesReceivedOK                EMAC_RXC_OK        /* RX Frame Successful Count                            */
+#define FrameCheckSequenceErrors        EMAC_RXC_FCS       /* RX Frame FCS Failure Count                           */
+#define AlignmentErrors                 EMAC_RXC_ALIGN     /* RX Alignment Error Count                             */
+#define OctetsReceivedOK                EMAC_RXC_OCTET     /* RX Octets Successfully Received Count                */
+#define FramesLostDueToIntMACRcvError   EMAC_RXC_DMAOVF    /* Internal MAC Sublayer Error RX Frame Count           */
+#define UnicastFramesReceivedOK         EMAC_RXC_UNICST    /* Unicast RX Frame Count                               */
+#define MulticastFramesReceivedOK       EMAC_RXC_MULTI     /* Multicast RX Frame Count                             */
+#define BroadcastFramesReceivedOK       EMAC_RXC_BROAD     /* Broadcast RX Frame Count                             */
+#define InRangeLengthErrors             EMAC_RXC_LNERRI    /* RX Frame In Range Error Count                        */
+#define OutOfRangeLengthField           EMAC_RXC_LNERRO    /* RX Frame Out Of Range Error Count                    */
+#define FrameTooLongErrors              EMAC_RXC_LONG      /* RX Frame Too Long Count                              */
+#define MACControlFramesReceived        EMAC_RXC_MACCTL    /* MAC Control RX Frame Count                           */
+#define UnsupportedOpcodesReceived      EMAC_RXC_OPCODE    /* Unsupported Op-Code RX Frame Count                   */
+#define PAUSEMACCtrlFramesReceived      EMAC_RXC_PAUSE     /* MAC Control Pause RX Frame Count                     */
+#define FramesReceivedAll               EMAC_RXC_ALLFRM    /* Overall RX Frame Count                               */
+#define OctetsReceivedAll               EMAC_RXC_ALLOCT    /* Overall RX Octet Count                               */
+#define TypedFramesReceived             EMAC_RXC_TYPED     /* Type/Length Consistent RX Frame Count                */
+#define FramesLenLt64Received           EMAC_RXC_SHORT     /* RX Frame Fragment Count - Byte Count x < 64          */
+#define FramesLenEq64Received           EMAC_RXC_EQ64      /* Good RX Frame Count - Byte Count x = 64              */
+#define FramesLen65_127Received         EMAC_RXC_LT128     /* Good RX Frame Count - Byte Count  64 < x < 128       */
+#define FramesLen128_255Received        EMAC_RXC_LT256     /* Good RX Frame Count - Byte Count 128 <= x < 256      */
+#define FramesLen256_511Received        EMAC_RXC_LT512     /* Good RX Frame Count - Byte Count 256 <= x < 512      */
+#define FramesLen512_1023Received       EMAC_RXC_LT1024    /* Good RX Frame Count - Byte Count 512 <= x < 1024     */
+#define FramesLen1024_MaxReceived       EMAC_RXC_GE1024    /* Good RX Frame Count - Byte Count x >= 1024           */
+
+#define FramesTransmittedOK             EMAC_TXC_OK        /* TX Frame Successful Count                            */
+#define SingleCollisionFrames           EMAC_TXC_1COL      /* TX Frames Successful After Single Collision Count    */
+#define MultipleCollisionFrames         EMAC_TXC_GT1COL    /* TX Frames Successful After Multiple Collisions Count */
+#define OctetsTransmittedOK             EMAC_TXC_OCTET     /* TX Octets Successfully Received Count                */
+#define FramesWithDeferredXmissions     EMAC_TXC_DEFER     /* TX Frame Delayed Due To Busy Count                   */
+#define LateCollisions                  EMAC_TXC_LATECL    /* Late TX Collisions Count                             */
+#define FramesAbortedDueToXSColls       EMAC_TXC_XS_COL    /* TX Frame Failed Due To Excessive Collisions Count    */
+#define FramesLostDueToIntMacXmitError  EMAC_TXC_DMAUND    /* Internal MAC Sublayer Error TX Frame Count           */
+#define CarrierSenseErrors              EMAC_TXC_CRSERR    /* Carrier Sense Deasserted During TX Frame Count       */
+#define UnicastFramesXmittedOK          EMAC_TXC_UNICST    /* Unicast TX Frame Count                               */
+#define MulticastFramesXmittedOK        EMAC_TXC_MULTI     /* Multicast TX Frame Count                             */
+#define BroadcastFramesXmittedOK        EMAC_TXC_BROAD     /* Broadcast TX Frame Count                             */
+#define FramesWithExcessiveDeferral     EMAC_TXC_XS_DFR    /* TX Frames With Excessive Deferral Count              */
+#define MACControlFramesTransmitted     EMAC_TXC_MACCTL    /* MAC Control TX Frame Count                           */
+#define FramesTransmittedAll            EMAC_TXC_ALLFRM    /* Overall TX Frame Count                               */
+#define OctetsTransmittedAll            EMAC_TXC_ALLOCT    /* Overall TX Octet Count                               */
+#define FramesLenEq64Transmitted        EMAC_TXC_EQ64      /* Good TX Frame Count - Byte Count x = 64              */
+#define FramesLen65_127Transmitted      EMAC_TXC_LT128     /* Good TX Frame Count - Byte Count  64 < x < 128       */
+#define FramesLen128_255Transmitted     EMAC_TXC_LT256     /* Good TX Frame Count - Byte Count 128 <= x < 256      */
+#define FramesLen256_511Transmitted     EMAC_TXC_LT512     /* Good TX Frame Count - Byte Count 256 <= x < 512      */
+#define FramesLen512_1023Transmitted    EMAC_TXC_LT1024    /* Good TX Frame Count - Byte Count 512 <= x < 1024     */
+#define FramesLen1024_MaxTransmitted    EMAC_TXC_GE1024    /* Good TX Frame Count - Byte Count x >= 1024           */
+#define TxAbortedFrames                 EMAC_TXC_ABORT     /* Total TX Frames Aborted Count                        */
+
+/***********************************************************************************
+** System MMR Register Bits And Macros
+**
+** Disclaimer:	All macros are intended to make C and Assembly code more readable.
+**				Use these macros carefully, as any that do left shifts for field
+**				depositing will result in the lower order bits being destroyed.  Any
+**				macro that shifts left to properly position the bit-field should be
+**				used as part of an OR to initialize a register and NOT as a dynamic
+**				modifier UNLESS the lower order bits are saved and ORed back in when
+**				the macro is used.
+*************************************************************************************/
+
+/************************  ETHERNET 10/100 CONTROLLER MASKS  ************************/
+
+/* EMAC_OPMODE Masks */
+
+#define	RE                 0x00000001     /* Receiver Enable                                    */
+#define	ASTP               0x00000002     /* Enable Automatic Pad Stripping On RX Frames        */
+#define	HU                 0x00000010     /* Hash Filter Unicast Address                        */
+#define	HM                 0x00000020     /* Hash Filter Multicast Address                      */
+#define	PAM                0x00000040     /* Pass-All-Multicast Mode Enable                     */
+#define	PR                 0x00000080     /* Promiscuous Mode Enable                            */
+#define	IFE                0x00000100     /* Inverse Filtering Enable                           */
+#define	DBF                0x00000200     /* Disable Broadcast Frame Reception                  */
+#define	PBF                0x00000400     /* Pass Bad Frames Enable                             */
+#define	PSF                0x00000800     /* Pass Short Frames Enable                           */
+#define	RAF                0x00001000     /* Receive-All Mode                                   */
+#define	TE                 0x00010000     /* Transmitter Enable                                 */
+#define	DTXPAD             0x00020000     /* Disable Automatic TX Padding                       */
+#define	DTXCRC             0x00040000     /* Disable Automatic TX CRC Generation                */
+#define	DC                 0x00080000     /* Deferral Check                                     */
+#define	BOLMT              0x00300000     /* Back-Off Limit                                     */
+#define	BOLMT_10           0x00000000     /*		10-bit range                            */
+#define	BOLMT_8            0x00100000     /*		8-bit range                             */
+#define	BOLMT_4            0x00200000     /*		4-bit range                             */
+#define	BOLMT_1            0x00300000     /*		1-bit range                             */
+#define	DRTY               0x00400000     /* Disable TX Retry On Collision                      */
+#define	LCTRE              0x00800000     /* Enable TX Retry On Late Collision                  */
+#define	RMII               0x01000000     /* RMII/MII* Mode                                     */
+#define	RMII_10            0x02000000     /* Speed Select for RMII Port (10MBit/100MBit*)       */
+#define	FDMODE             0x04000000     /* Duplex Mode Enable (Full/Half*)                    */
+#define	LB                 0x08000000     /* Internal Loopback Enable                           */
+#define	DRO                0x10000000     /* Disable Receive Own Frames (Half-Duplex Mode)      */
+
+/* EMAC_STAADD Masks */
+
+#define	STABUSY            0x00000001     /* Initiate Station Mgt Reg Access / STA Busy Stat    */
+#define	STAOP              0x00000002     /* Station Management Operation Code (Write/Read*)    */
+#define	STADISPRE          0x00000004     /* Disable Preamble Generation                        */
+#define	STAIE              0x00000008     /* Station Mgt. Transfer Done Interrupt Enable        */
+#define	REGAD              0x000007C0     /* STA Register Address                               */
+#define	PHYAD              0x0000F800     /* PHY Device Address                                 */
+
+#define	SET_REGAD(x) (((x)&0x1F)<<  6 )   /* Set STA Register Address                           */
+#define	SET_PHYAD(x) (((x)&0x1F)<< 11 )   /* Set PHY Device Address                             */
+
+/* EMAC_STADAT Mask */
+
+#define	STADATA            0x0000FFFF     /* Station Management Data                            */
+
+/* EMAC_FLC Masks */
+
+#define	FLCBUSY            0x00000001     /* Send Flow Ctrl Frame / Flow Ctrl Busy Status       */
+#define	FLCE               0x00000002     /* Flow Control Enable                                */
+#define	PCF                0x00000004     /* Pass Control Frames                                */
+#define	BKPRSEN            0x00000008     /* Enable Backpressure                                */
+#define	FLCPAUSE           0xFFFF0000     /* Pause Time                                         */
+
+#define	SET_FLCPAUSE(x) (((x)&0xFFFF)<< 16) /* Set Pause Time                                   */
+
+/* EMAC_WKUP_CTL Masks */
+
+#define	CAPWKFRM           0x00000001    /* Capture Wake-Up Frames                              */
+#define	MPKE               0x00000002    /* Magic Packet Enable                                 */
+#define	RWKE               0x00000004    /* Remote Wake-Up Frame Enable                         */
+#define	GUWKE              0x00000008    /* Global Unicast Wake Enable                          */
+#define	MPKS               0x00000020    /* Magic Packet Received Status                        */
+#define	RWKS               0x00000F00    /* Wake-Up Frame Received Status, Filters 3:0          */
+
+/* EMAC_WKUP_FFCMD Masks */
+
+#define	WF0_E              0x00000001    /* Enable Wake-Up Filter 0                              */
+#define	WF0_T              0x00000008    /* Wake-Up Filter 0 Addr Type (Multicast/Unicast*)      */
+#define	WF1_E              0x00000100    /* Enable Wake-Up Filter 1                              */
+#define	WF1_T              0x00000800    /* Wake-Up Filter 1 Addr Type (Multicast/Unicast*)      */
+#define	WF2_E              0x00010000    /* Enable Wake-Up Filter 2                              */
+#define	WF2_T              0x00080000    /* Wake-Up Filter 2 Addr Type (Multicast/Unicast*)      */
+#define	WF3_E              0x01000000    /* Enable Wake-Up Filter 3                              */
+#define	WF3_T              0x08000000    /* Wake-Up Filter 3 Addr Type (Multicast/Unicast*)      */
+
+/* EMAC_WKUP_FFOFF Masks */
+
+#define	WF0_OFF            0x000000FF    /* Wake-Up Filter 0 Pattern Offset                      */
+#define	WF1_OFF            0x0000FF00    /* Wake-Up Filter 1 Pattern Offset                      */
+#define	WF2_OFF            0x00FF0000    /* Wake-Up Filter 2 Pattern Offset                      */
+#define	WF3_OFF            0xFF000000    /* Wake-Up Filter 3 Pattern Offset                      */
+
+#define	SET_WF0_OFF(x) (((x)&0xFF)<<  0 ) /* Set Wake-Up Filter 0 Byte Offset                    */
+#define	SET_WF1_OFF(x) (((x)&0xFF)<<  8 ) /* Set Wake-Up Filter 1 Byte Offset                    */
+#define	SET_WF2_OFF(x) (((x)&0xFF)<< 16 ) /* Set Wake-Up Filter 2 Byte Offset                    */
+#define	SET_WF3_OFF(x) (((x)&0xFF)<< 24 ) /* Set Wake-Up Filter 3 Byte Offset                    */
+/* Set ALL Offsets */
+#define	SET_WF_OFFS(x0,x1,x2,x3) (SET_WF0_OFF((x0))|SET_WF1_OFF((x1))|SET_WF2_OFF((x2))|SET_WF3_OFF((x3)))
+
+/* EMAC_WKUP_FFCRC0 Masks */
+
+#define	WF0_CRC           0x0000FFFF    /* Wake-Up Filter 0 Pattern CRC                           */
+#define	WF1_CRC           0xFFFF0000    /* Wake-Up Filter 1 Pattern CRC                           */
+
+#define	SET_WF0_CRC(x) (((x)&0xFFFF)<<   0 ) /* Set Wake-Up Filter 0 Target CRC                   */
+#define	SET_WF1_CRC(x) (((x)&0xFFFF)<<  16 ) /* Set Wake-Up Filter 1 Target CRC                   */
+
+/* EMAC_WKUP_FFCRC1 Masks */
+
+#define	WF2_CRC           0x0000FFFF    /* Wake-Up Filter 2 Pattern CRC                           */
+#define	WF3_CRC           0xFFFF0000    /* Wake-Up Filter 3 Pattern CRC                           */
+
+#define	SET_WF2_CRC(x) (((x)&0xFFFF)<<   0 ) /* Set Wake-Up Filter 2 Target CRC                   */
+#define	SET_WF3_CRC(x) (((x)&0xFFFF)<<  16 ) /* Set Wake-Up Filter 3 Target CRC                   */
+
+/* EMAC_SYSCTL Masks */
+
+#define	PHYIE             0x00000001    /* PHY_INT Interrupt Enable                               */
+#define	RXDWA             0x00000002    /* Receive Frame DMA Word Alignment (Odd/Even*)           */
+#define	RXCKS             0x00000004    /* Enable RX Frame TCP/UDP Checksum Computation           */
+#define	TXDWA             0x00000010    /* Transmit Frame DMA Word Alignment (Odd/Even*)          */
+#define	MDCDIV            0x00003F00    /* SCLK:MDC Clock Divisor [MDC=SCLK/(2*(N+1))]            */
+
+#define	SET_MDCDIV(x) (((x)&0x3F)<< 8)   /* Set MDC Clock Divisor                                 */
+
+/* EMAC_SYSTAT Masks */
+
+#define	PHYINT            0x00000001    /* PHY_INT Interrupt Status                               */
+#define	MMCINT            0x00000002    /* MMC Counter Interrupt Status                           */
+#define	RXFSINT           0x00000004    /* RX Frame-Status Interrupt Status                       */
+#define	TXFSINT           0x00000008    /* TX Frame-Status Interrupt Status                       */
+#define	WAKEDET           0x00000010    /* Wake-Up Detected Status                                */
+#define	RXDMAERR          0x00000020    /* RX DMA Direction Error Status                          */
+#define	TXDMAERR          0x00000040    /* TX DMA Direction Error Status                          */
+#define	STMDONE           0x00000080    /* Station Mgt. Transfer Done Interrupt Status            */
+
+/* EMAC_RX_STAT, EMAC_RX_STKY, and EMAC_RX_IRQE Masks */
+
+#define	RX_FRLEN          0x000007FF    /* Frame Length In Bytes                                  */
+#define	RX_COMP           0x00001000    /* RX Frame Complete                                      */
+#define	RX_OK             0x00002000    /* RX Frame Received With No Errors                       */
+#define	RX_LONG           0x00004000    /* RX Frame Too Long Error                                */
+#define	RX_ALIGN          0x00008000    /* RX Frame Alignment Error                               */
+#define	RX_CRC            0x00010000    /* RX Frame CRC Error                                     */
+#define	RX_LEN            0x00020000    /* RX Frame Length Error                                  */
+#define	RX_FRAG           0x00040000    /* RX Frame Fragment Error                                */
+#define	RX_ADDR           0x00080000    /* RX Frame Address Filter Failed Error                   */
+#define	RX_DMAO           0x00100000    /* RX Frame DMA Overrun Error                             */
+#define	RX_PHY            0x00200000    /* RX Frame PHY Error                                     */
+#define	RX_LATE           0x00400000    /* RX Frame Late Collision Error                          */
+#define	RX_RANGE          0x00800000    /* RX Frame Length Field Out of Range Error               */
+#define	RX_MULTI          0x01000000    /* RX Multicast Frame Indicator                           */
+#define	RX_BROAD          0x02000000    /* RX Broadcast Frame Indicator                           */
+#define	RX_CTL            0x04000000    /* RX Control Frame Indicator                             */
+#define	RX_UCTL           0x08000000    /* Unsupported RX Control Frame Indicator                 */
+#define	RX_TYPE           0x10000000    /* RX Typed Frame Indicator                               */
+#define	RX_VLAN1          0x20000000    /* RX VLAN1 Frame Indicator                               */
+#define	RX_VLAN2          0x40000000    /* RX VLAN2 Frame Indicator                               */
+#define	RX_ACCEPT         0x80000000    /* RX Frame Accepted Indicator                            */
+
+/*  EMAC_TX_STAT, EMAC_TX_STKY, and EMAC_TX_IRQE Masks  */
+
+#define	TX_COMP           0x00000001    /* TX Frame Complete                                      */
+#define	TX_OK             0x00000002    /* TX Frame Sent With No Errors                           */
+#define	TX_ECOLL          0x00000004    /* TX Frame Excessive Collision Error                     */
+#define	TX_LATE           0x00000008    /* TX Frame Late Collision Error                          */
+#define	TX_DMAU           0x00000010    /* TX Frame DMA Underrun Error (STAT)                     */
+#define	TX_MACE           0x00000010    /* Internal MAC Error Detected (STKY and IRQE)            */
+#define	TX_EDEFER         0x00000020    /* TX Frame Excessive Deferral Error                      */
+#define	TX_BROAD          0x00000040    /* TX Broadcast Frame Indicator                           */
+#define	TX_MULTI          0x00000080    /* TX Multicast Frame Indicator                           */
+#define	TX_CCNT           0x00000F00    /* TX Frame Collision Count                               */
+#define	TX_DEFER          0x00001000    /* TX Frame Deferred Indicator                            */
+#define	TX_CRS            0x00002000    /* TX Frame Carrier Sense Not Asserted Error              */
+#define	TX_LOSS           0x00004000    /* TX Frame Carrier Lost During TX Error                  */
+#define	TX_RETRY          0x00008000    /* TX Frame Successful After Retry                        */
+#define	TX_FRLEN          0x07FF0000    /* TX Frame Length (Bytes)                                */
+
+/* EMAC_MMC_CTL Masks */
+#define	RSTC              0x00000001    /* Reset All Counters                                     */
+#define	CROLL             0x00000002    /* Counter Roll-Over Enable                               */
+#define	CCOR              0x00000004    /* Counter Clear-On-Read Mode Enable                      */
+#define	MMCE              0x00000008    /* Enable MMC Counter Operation                           */
+
+/* EMAC_MMC_RIRQS and EMAC_MMC_RIRQE Masks */
+#define	RX_OK_CNT         0x00000001    /* RX Frames Received With No Errors                      */
+#define	RX_FCS_CNT        0x00000002    /* RX Frames W/Frame Check Sequence Errors                */
+#define	RX_ALIGN_CNT      0x00000004    /* RX Frames With Alignment Errors                        */
+#define	RX_OCTET_CNT      0x00000008    /* RX Octets Received OK                                  */
+#define	RX_LOST_CNT       0x00000010    /* RX Frames Lost Due To Internal MAC RX Error            */
+#define	RX_UNI_CNT        0x00000020    /* Unicast RX Frames Received OK                          */
+#define	RX_MULTI_CNT      0x00000040    /* Multicast RX Frames Received OK                        */
+#define	RX_BROAD_CNT      0x00000080    /* Broadcast RX Frames Received OK                        */
+#define	RX_IRL_CNT        0x00000100    /* RX Frames With In-Range Length Errors                  */
+#define	RX_ORL_CNT        0x00000200    /* RX Frames With Out-Of-Range Length Errors              */
+#define	RX_LONG_CNT       0x00000400    /* RX Frames With Frame Too Long Errors                   */
+#define	RX_MACCTL_CNT     0x00000800    /* MAC Control RX Frames Received                         */
+#define	RX_OPCODE_CTL     0x00001000    /* Unsupported Op-Code RX Frames Received                 */
+#define	RX_PAUSE_CNT      0x00002000    /* PAUSEMAC Control RX Frames Received                    */
+#define	RX_ALLF_CNT       0x00004000    /* All RX Frames Received                                 */
+#define	RX_ALLO_CNT       0x00008000    /* All RX Octets Received                                 */
+#define	RX_TYPED_CNT      0x00010000    /* Typed RX Frames Received                               */
+#define	RX_SHORT_CNT      0x00020000    /* RX Frame Fragments (< 64 Bytes) Received               */
+#define	RX_EQ64_CNT       0x00040000    /* 64-Byte RX Frames Received                             */
+#define	RX_LT128_CNT      0x00080000    /* 65-127-Byte RX Frames Received                         */
+#define	RX_LT256_CNT      0x00100000    /* 128-255-Byte RX Frames Received                        */
+#define	RX_LT512_CNT      0x00200000    /* 256-511-Byte RX Frames Received                        */
+#define	RX_LT1024_CNT     0x00400000    /* 512-1023-Byte RX Frames Received                       */
+#define	RX_GE1024_CNT     0x00800000    /* 1024-Max-Byte RX Frames Received                       */
+
+/* EMAC_MMC_TIRQS and EMAC_MMC_TIRQE Masks  */
+
+#define	TX_OK_CNT         0x00000001    /* TX Frames Sent OK                                      */
+#define	TX_SCOLL_CNT      0x00000002    /* TX Frames With Single Collisions                       */
+#define	TX_MCOLL_CNT      0x00000004    /* TX Frames With Multiple Collisions                     */
+#define	TX_OCTET_CNT      0x00000008    /* TX Octets Sent OK                                      */
+#define	TX_DEFER_CNT      0x00000010    /* TX Frames With Deferred Transmission                   */
+#define	TX_LATE_CNT       0x00000020    /* TX Frames With Late Collisions                         */
+#define	TX_ABORTC_CNT     0x00000040    /* TX Frames Aborted Due To Excess Collisions             */
+#define	TX_LOST_CNT       0x00000080    /* TX Frames Lost Due To Internal MAC TX Error            */
+#define	TX_CRS_CNT        0x00000100    /* TX Frames With Carrier Sense Errors                    */
+#define	TX_UNI_CNT        0x00000200    /* Unicast TX Frames Sent                                 */
+#define	TX_MULTI_CNT      0x00000400    /* Multicast TX Frames Sent                               */
+#define	TX_BROAD_CNT      0x00000800    /* Broadcast TX Frames Sent                               */
+#define	TX_EXDEF_CTL      0x00001000    /* TX Frames With Excessive Deferral                      */
+#define	TX_MACCTL_CNT     0x00002000    /* MAC Control TX Frames Sent                             */
+#define	TX_ALLF_CNT       0x00004000    /* All TX Frames Sent                                     */
+#define	TX_ALLO_CNT       0x00008000    /* All TX Octets Sent                                     */
+#define	TX_EQ64_CNT       0x00010000    /* 64-Byte TX Frames Sent                                 */
+#define	TX_LT128_CNT      0x00020000    /* 65-127-Byte TX Frames Sent                             */
+#define	TX_LT256_CNT      0x00040000    /* 128-255-Byte TX Frames Sent                            */
+#define	TX_LT512_CNT      0x00080000    /* 256-511-Byte TX Frames Sent                            */
+#define	TX_LT1024_CNT     0x00100000    /* 512-1023-Byte TX Frames Sent                           */
+#define	TX_GE1024_CNT     0x00200000    /* 1024-Max-Byte TX Frames Sent                           */
+#define	TX_ABORT_CNT      0x00400000    /* TX Frames Aborted                                      */
+
+/* SDH Registers */
+
+#define SDH_PWR_CTL                    0xFFC03900 /* SDH Power Control */
+#define SDH_CLK_CTL                    0xFFC03904 /* SDH Clock Control */
+#define SDH_ARGUMENT                   0xFFC03908 /* SDH Argument */
+#define SDH_COMMAND                    0xFFC0390C /* SDH Command */
+#define SDH_RESP_CMD                   0xFFC03910 /* SDH Response Command */
+#define SDH_RESPONSE0                  0xFFC03914 /* SDH Response0 */
+#define SDH_RESPONSE1                  0xFFC03918 /* SDH Response1 */
+#define SDH_RESPONSE2                  0xFFC0391C /* SDH Response2 */
+#define SDH_RESPONSE3                  0xFFC03920 /* SDH Response3 */
+#define SDH_DATA_TIMER                 0xFFC03924 /* SDH Data Timer */
+#define SDH_DATA_LGTH                  0xFFC03928 /* SDH Data Length */
+#define SDH_DATA_CTL                   0xFFC0392C /* SDH Data Control */
+#define SDH_DATA_CNT                   0xFFC03930 /* SDH Data Counter */
+#define SDH_STATUS                     0xFFC03934 /* SDH Status */
+#define SDH_STATUS_CLR                 0xFFC03938 /* SDH Status Clear */
+#define SDH_MASK0                      0xFFC0393C /* SDH Interrupt0 Mask */
+#define SDH_MASK1                      0xFFC03940 /* SDH Interrupt1 Mask */
+#define SDH_FIFO_CNT                   0xFFC03948 /* SDH FIFO Counter */
+#define SDH_FIFO                       0xFFC03980 /* SDH Data FIFO */
+#define SDH_E_STATUS                   0xFFC039C0 /* SDH Exception Status */
+#define SDH_E_MASK                     0xFFC039C4 /* SDH Exception Mask */
+#define SDH_CFG                        0xFFC039C8 /* SDH Configuration */
+#define SDH_RD_WAIT_EN                 0xFFC039CC /* SDH Read Wait Enable */
+#define SDH_PID0                       0xFFC039D0 /* SDH Peripheral Identification0 */
+#define SDH_PID1                       0xFFC039D4 /* SDH Peripheral Identification1 */
+#define SDH_PID2                       0xFFC039D8 /* SDH Peripheral Identification2 */
+#define SDH_PID3                       0xFFC039DC /* SDH Peripheral Identification3 */
+#define SDH_PID4                       0xFFC039E0 /* SDH Peripheral Identification4 */
+#define SDH_PID5                       0xFFC039E4 /* SDH Peripheral Identification5 */
+#define SDH_PID6                       0xFFC039E8 /* SDH Peripheral Identification6 */
+#define SDH_PID7                       0xFFC039EC /* SDH Peripheral Identification7 */
+
+/* Removable Storage Interface Registers */
+
+#define RSI_PWR_CONTROL                0xFFC03800 /* RSI Power Control Register */
+#define RSI_CLK_CONTROL                0xFFC03804 /* RSI Clock Control Register */
+#define RSI_ARGUMENT                   0xFFC03808 /* RSI Argument Register */
+#define RSI_COMMAND                    0xFFC0380C /* RSI Command Register */
+#define RSI_RESP_CMD                   0xFFC03810 /* RSI Response Command Register */
+#define RSI_RESPONSE0                  0xFFC03814 /* RSI Response Register */
+#define RSI_RESPONSE1                  0xFFC03818 /* RSI Response Register */
+#define RSI_RESPONSE2                  0xFFC0381C /* RSI Response Register */
+#define RSI_RESPONSE3                  0xFFC03820 /* RSI Response Register */
+#define RSI_DATA_TIMER                 0xFFC03824 /* RSI Data Timer Register */
+#define RSI_DATA_LGTH                  0xFFC03828 /* RSI Data Length Register */
+#define RSI_DATA_CONTROL               0xFFC0382C /* RSI Data Control Register */
+#define RSI_DATA_CNT                   0xFFC03830 /* RSI Data Counter Register */
+#define RSI_STATUS                     0xFFC03834 /* RSI Status Register */
+#define RSI_STATUSCL                   0xFFC03838 /* RSI Status Clear Register */
+#define RSI_MASK0                      0xFFC0383C /* RSI Interrupt 0 Mask Register */
+#define RSI_MASK1                      0xFFC03840 /* RSI Interrupt 1 Mask Register */
+#define RSI_FIFO_CNT                   0xFFC03848 /* RSI FIFO Counter Register */
+#define RSI_CEATA_CONTROL              0xFFC0384C /* RSI CEATA Register */
+#define RSI_FIFO                       0xFFC03880 /* RSI Data FIFO Register */
+#define RSI_ESTAT                      0xFFC038C0 /* RSI Exception Status Register */
+#define RSI_EMASK                      0xFFC038C4 /* RSI Exception Mask Register */
+#define RSI_CONFIG                     0xFFC038C8 /* RSI Configuration Register */
+#define RSI_RD_WAIT_EN                 0xFFC038CC /* RSI Read Wait Enable Register */
+#define RSI_PID0                       0xFFC03FE0 /* RSI Peripheral ID Register 0 */
+#define RSI_PID1                       0xFFC03FE4 /* RSI Peripheral ID Register 1 */
+#define RSI_PID2                       0xFFC03FE8 /* RSI Peripheral ID Register 2 */
+#define RSI_PID3                       0xFFC03FEC /* RSI Peripheral ID Register 3 */
+#define RSI_PID4                       0xFFC03FF0 /* RSI Peripheral ID Register 4 */
+#define RSI_PID5                       0xFFC03FF4 /* RSI Peripheral ID Register 5 */
+#define RSI_PID6                       0xFFC03FF8 /* RSI Peripheral ID Register 6 */
+#define RSI_PID7                       0xFFC03FFC /* RSI Peripheral ID Register 7 */
+
+/* PTP TSYNC Registers */
+
+#define EMAC_PTP_CTL                   0xFFC030A0 /* PTP Block Control */
+#define EMAC_PTP_IE                    0xFFC030A4 /* PTP Block Interrupt Enable */
+#define EMAC_PTP_ISTAT                 0xFFC030A8 /* PTP Block Interrupt Status */
+#define EMAC_PTP_FOFF                  0xFFC030AC /* PTP Filter offset Register */
+#define EMAC_PTP_FV1                   0xFFC030B0 /* PTP Filter Value Register 1 */
+#define EMAC_PTP_FV2                   0xFFC030B4 /* PTP Filter Value Register 2 */
+#define EMAC_PTP_FV3                   0xFFC030B8 /* PTP Filter Value Register 3 */
+#define EMAC_PTP_ADDEND                0xFFC030BC /* PTP Addend for Frequency Compensation */
+#define EMAC_PTP_ACCR                  0xFFC030C0 /* PTP Accumulator for Frequency Compensation */
+#define EMAC_PTP_OFFSET                0xFFC030C4 /* PTP Time Offset Register */
+#define EMAC_PTP_TIMELO                0xFFC030C8 /* PTP Precision Clock Time Low */
+#define EMAC_PTP_TIMEHI                0xFFC030CC /* PTP Precision Clock Time High */
+#define EMAC_PTP_RXSNAPLO              0xFFC030D0 /* PTP Receive Snapshot Register Low */
+#define EMAC_PTP_RXSNAPHI              0xFFC030D4 /* PTP Receive Snapshot Register High */
+#define EMAC_PTP_TXSNAPLO              0xFFC030D8 /* PTP Transmit Snapshot Register Low */
+#define EMAC_PTP_TXSNAPHI              0xFFC030DC /* PTP Transmit Snapshot Register High */
+#define EMAC_PTP_ALARMLO               0xFFC030E0 /* PTP Alarm time Low */
+#define EMAC_PTP_ALARMHI               0xFFC030E4 /* PTP Alarm time High */
+#define EMAC_PTP_ID_OFF                0xFFC030E8 /* PTP Capture ID offset register */
+#define EMAC_PTP_ID_SNAP               0xFFC030EC /* PTP Capture ID register */
+#define EMAC_PTP_PPS_STARTLO           0xFFC030F0 /* PPS Start Time Low */
+#define EMAC_PTP_PPS_STARTHI           0xFFC030F4 /* PPS Start Time High */
+#define EMAC_PTP_PPS_PERIOD            0xFFC030F8 /* PPS Count Register */
+
+/* ********************************************************** */
+/*     SINGLE BIT MACRO PAIRS (bit mask and negated one)      */
+/*     and MULTI BIT READ MACROS                              */
+/* ********************************************************** */
+
+/* Bit masks for SDH_COMMAND */
+
+#define                   CMD_IDX  0x3f       /* Command Index */
+#define                   CMD_RSP  0x40       /* Response */
+#define                 CMD_L_RSP  0x80       /* Long Response */
+#define                 CMD_INT_E  0x100      /* Command Interrupt */
+#define                CMD_PEND_E  0x200      /* Command Pending */
+#define                     CMD_E  0x400      /* Command Enable */
+
+/* Bit masks for SDH_PWR_CTL */
+
+#define                    PWR_ON  0x3        /* Power On */
+#if 0
+#define                       TBD  0x3c       /* TBD */
+#endif
+#define                 SD_CMD_OD  0x40       /* Open Drain Output */
+#define                   ROD_CTL  0x80       /* Rod Control */
+
+/* Bit masks for SDH_CLK_CTL */
+
+#define                    CLKDIV  0xff       /* MC_CLK Divisor */
+#define                     CLK_E  0x100      /* MC_CLK Bus Clock Enable */
+#define                  PWR_SV_E  0x200      /* Power Save Enable */
+#define             CLKDIV_BYPASS  0x400      /* Bypass Divisor */
+#define                  WIDE_BUS  0x800      /* Wide Bus Mode Enable */
+
+/* Bit masks for SDH_RESP_CMD */
+
+#define                  RESP_CMD  0x3f       /* Response Command */
+
+/* Bit masks for SDH_DATA_CTL */
+
+#define                     DTX_E  0x1        /* Data Transfer Enable */
+#define                   DTX_DIR  0x2        /* Data Transfer Direction */
+#define                  DTX_MODE  0x4        /* Data Transfer Mode */
+#define                 DTX_DMA_E  0x8        /* Data Transfer DMA Enable */
+#define              DTX_BLK_LGTH  0xf0       /* Data Transfer Block Length */
+
+/* Bit masks for SDH_STATUS */
+
+#define              CMD_CRC_FAIL  0x1        /* CMD CRC Fail */
+#define              DAT_CRC_FAIL  0x2        /* Data CRC Fail */
+#define               CMD_TIME_OUT  0x4        /* CMD Time Out */
+#define               DAT_TIME_OUT  0x8        /* Data Time Out */
+#define               TX_UNDERRUN  0x10       /* Transmit Underrun */
+#define                RX_OVERRUN  0x20       /* Receive Overrun */
+#define              CMD_RESP_END  0x40       /* CMD Response End */
+#define                  CMD_SENT  0x80       /* CMD Sent */
+#define                   DAT_END  0x100      /* Data End */
+#define             START_BIT_ERR  0x200      /* Start Bit Error */
+#define               DAT_BLK_END  0x400      /* Data Block End */
+#define                   CMD_ACT  0x800      /* CMD Active */
+#define                    TX_ACT  0x1000     /* Transmit Active */
+#define                    RX_ACT  0x2000     /* Receive Active */
+#define              TX_FIFO_STAT  0x4000     /* Transmit FIFO Status */
+#define              RX_FIFO_STAT  0x8000     /* Receive FIFO Status */
+#define              TX_FIFO_FULL  0x10000    /* Transmit FIFO Full */
+#define              RX_FIFO_FULL  0x20000    /* Receive FIFO Full */
+#define              TX_FIFO_ZERO  0x40000    /* Transmit FIFO Empty */
+#define               RX_DAT_ZERO  0x80000    /* Receive FIFO Empty */
+#define                TX_DAT_RDY  0x100000   /* Transmit Data Available */
+#define               RX_FIFO_RDY  0x200000   /* Receive Data Available */
+
+/* Bit masks for SDH_STATUS_CLR */
+
+#define         CMD_CRC_FAIL_STAT  0x1        /* CMD CRC Fail Status */
+#define         DAT_CRC_FAIL_STAT  0x2        /* Data CRC Fail Status */
+#define          CMD_TIMEOUT_STAT  0x4        /* CMD Time Out Status */
+#define          DAT_TIMEOUT_STAT  0x8        /* Data Time Out status */
+#define          TX_UNDERRUN_STAT  0x10       /* Transmit Underrun Status */
+#define           RX_OVERRUN_STAT  0x20       /* Receive Overrun Status */
+#define         CMD_RESP_END_STAT  0x40       /* CMD Response End Status */
+#define             CMD_SENT_STAT  0x80       /* CMD Sent Status */
+#define              DAT_END_STAT  0x100      /* Data End Status */
+#define        START_BIT_ERR_STAT  0x200      /* Start Bit Error Status */
+#define          DAT_BLK_END_STAT  0x400      /* Data Block End Status */
+
+/* Bit masks for SDH_MASK0 */
+
+#define         CMD_CRC_FAIL_MASK  0x1        /* CMD CRC Fail Mask */
+#define         DAT_CRC_FAIL_MASK  0x2        /* Data CRC Fail Mask */
+#define          CMD_TIMEOUT_MASK  0x4        /* CMD Time Out Mask */
+#define          DAT_TIMEOUT_MASK  0x8        /* Data Time Out Mask */
+#define          TX_UNDERRUN_MASK  0x10       /* Transmit Underrun Mask */
+#define           RX_OVERRUN_MASK  0x20       /* Receive Overrun Mask */
+#define         CMD_RESP_END_MASK  0x40       /* CMD Response End Mask */
+#define             CMD_SENT_MASK  0x80       /* CMD Sent Mask */
+#define              DAT_END_MASK  0x100      /* Data End Mask */
+#define        START_BIT_ERR_MASK  0x200      /* Start Bit Error Mask */
+#define          DAT_BLK_END_MASK  0x400      /* Data Block End Mask */
+#define              CMD_ACT_MASK  0x800      /* CMD Active Mask */
+#define               TX_ACT_MASK  0x1000     /* Transmit Active Mask */
+#define               RX_ACT_MASK  0x2000     /* Receive Active Mask */
+#define         TX_FIFO_STAT_MASK  0x4000     /* Transmit FIFO Status Mask */
+#define         RX_FIFO_STAT_MASK  0x8000     /* Receive FIFO Status Mask */
+#define         TX_FIFO_FULL_MASK  0x10000    /* Transmit FIFO Full Mask */
+#define         RX_FIFO_FULL_MASK  0x20000    /* Receive FIFO Full Mask */
+#define         TX_FIFO_ZERO_MASK  0x40000    /* Transmit FIFO Empty Mask */
+#define          RX_DAT_ZERO_MASK  0x80000    /* Receive FIFO Empty Mask */
+#define           TX_DAT_RDY_MASK  0x100000   /* Transmit Data Available Mask */
+#define          RX_FIFO_RDY_MASK  0x200000   /* Receive Data Available Mask */
+
+/* Bit masks for SDH_FIFO_CNT */
+
+#define                FIFO_COUNT  0x7fff     /* FIFO Count */
+
+/* Bit masks for SDH_E_STATUS */
+
+#define              SDIO_INT_DET  0x2        /* SDIO Int Detected */
+#define               SD_CARD_DET  0x10       /* SD Card Detect */
+
+/* Bit masks for SDH_E_MASK */
+
+#define                  SDIO_MSK  0x2        /* Mask SDIO Int Detected */
+#define                   SCD_MSK  0x40       /* Mask Card Detect */
+
+/* Bit masks for SDH_CFG */
+
+#define                   CLKS_EN  0x1        /* Clocks Enable */
+#define                      SD4E  0x4        /* SDIO 4-Bit Enable */
+#define                       MWE  0x8        /* Moving Window Enable */
+#define                    SD_RST  0x10       /* SDMMC Reset */
+#define                 PUP_SDDAT  0x20       /* Pull-up SD_DAT */
+#define                PUP_SDDAT3  0x40       /* Pull-up SD_DAT3 */
+#define                 PD_SDDAT3  0x80       /* Pull-down SD_DAT3 */
+
+/* Bit masks for SDH_RD_WAIT_EN */
+
+#define                       RWR  0x1        /* Read Wait Request */
+
+#endif /* _DEF_BF518_H */
diff --git a/arch/blackfin/mach-bf518/include/mach/defBF51x_base.h b/arch/blackfin/mach-bf518/include/mach/defBF51x_base.h
new file mode 100644
index 000000000000..1bec8d1c2a73
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/defBF51x_base.h
@@ -0,0 +1,1940 @@
+/*
+ * File:         include/asm-blackfin/mach-bf518/defBF51x_base.h
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:
+ *
+ * Rev:
+ *
+ * Modified:
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, 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; see the file COPYING.
+ * If not, write to the Free Software Foundation,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef _DEF_BF51X_H
+#define _DEF_BF51X_H
+
+
+/* ************************************************************** */
+/*   SYSTEM & MMR ADDRESS DEFINITIONS COMMON TO ALL ADSP-BF51x    */
+/* ************************************************************** */
+
+/* Clock and System Control	(0xFFC00000 - 0xFFC000FF)								*/
+#define PLL_CTL				0xFFC00000	/* PLL Control Register						*/
+#define PLL_DIV				0xFFC00004	/* PLL Divide Register						*/
+#define VR_CTL				0xFFC00008	/* Voltage Regulator Control Register				*/
+#define PLL_STAT			0xFFC0000C	/* PLL Status Register						*/
+#define PLL_LOCKCNT			0xFFC00010	/* PLL Lock Count Register					*/
+#define CHIPID				0xFFC00014	/* Device ID Register */
+
+/* System Interrupt Controller (0xFFC00100 - 0xFFC001FF)								*/
+#define SWRST				0xFFC00100	/* Software Reset Register					*/
+#define SYSCR				0xFFC00104	/* System Configuration Register				*/
+#define SIC_RVECT			0xFFC00108	/* Interrupt Reset Vector Address Register			*/
+
+#define SIC_IMASK0			0xFFC0010C	/* Interrupt Mask Register					*/
+#define SIC_IAR0			0xFFC00110	/* Interrupt Assignment Register 0				*/
+#define SIC_IAR1			0xFFC00114	/* Interrupt Assignment Register 1				*/
+#define SIC_IAR2			0xFFC00118	/* Interrupt Assignment Register 2				*/
+#define SIC_IAR3			0xFFC0011C	/* Interrupt Assignment Register 3				*/
+#define SIC_ISR0			0xFFC00120	/* Interrupt Status Register					*/
+#define SIC_IWR0			0xFFC00124	/* Interrupt Wakeup Register					*/
+
+/* SIC Additions to ADSP-BF51x (0xFFC0014C - 0xFFC00162) */
+#define SIC_IMASK1                      0xFFC0014C     /* Interrupt Mask register of SIC2 */
+#define SIC_IAR4                        0xFFC00150     /* Interrupt Assignment register4 */
+#define SIC_IAR5                        0xFFC00154     /* Interrupt Assignment register5 */
+#define SIC_IAR6                        0xFFC00158     /* Interrupt Assignment register6 */
+#define SIC_IAR7                        0xFFC0015C     /* Interrupt Assignment register7 */
+#define SIC_ISR1                        0xFFC00160     /* Interrupt Statur register */
+#define SIC_IWR1                        0xFFC00164     /* Interrupt Wakeup register */
+
+
+/* Watchdog Timer			(0xFFC00200 - 0xFFC002FF)								*/
+#define WDOG_CTL			0xFFC00200	/* Watchdog Control Register				*/
+#define WDOG_CNT			0xFFC00204	/* Watchdog Count Register					*/
+#define WDOG_STAT			0xFFC00208	/* Watchdog Status Register					*/
+
+
+/* Real Time Clock		(0xFFC00300 - 0xFFC003FF)									*/
+#define RTC_STAT			0xFFC00300	/* RTC Status Register						*/
+#define RTC_ICTL			0xFFC00304	/* RTC Interrupt Control Register			*/
+#define RTC_ISTAT			0xFFC00308	/* RTC Interrupt Status Register			*/
+#define RTC_SWCNT			0xFFC0030C	/* RTC Stopwatch Count Register				*/
+#define RTC_ALARM			0xFFC00310	/* RTC Alarm Time Register					*/
+#define RTC_FAST			0xFFC00314	/* RTC Prescaler Enable Register			*/
+#define RTC_PREN			0xFFC00314	/* RTC Prescaler Enable Alternate Macro		*/
+
+
+/* UART0 Controller		(0xFFC00400 - 0xFFC004FF)									*/
+#define UART0_THR			0xFFC00400	/* Transmit Holding register				*/
+#define UART0_RBR			0xFFC00400	/* Receive Buffer register					*/
+#define UART0_DLL			0xFFC00400	/* Divisor Latch (Low-Byte)					*/
+#define UART0_IER			0xFFC00404	/* Interrupt Enable Register				*/
+#define UART0_DLH			0xFFC00404	/* Divisor Latch (High-Byte)				*/
+#define UART0_IIR			0xFFC00408	/* Interrupt Identification Register		*/
+#define UART0_LCR			0xFFC0040C	/* Line Control Register					*/
+#define UART0_MCR			0xFFC00410	/* Modem Control Register					*/
+#define UART0_LSR			0xFFC00414	/* Line Status Register						*/
+#define UART0_MSR			0xFFC00418	/* Modem Status Register					*/
+#define UART0_SCR			0xFFC0041C	/* SCR Scratch Register						*/
+#define UART0_GCTL			0xFFC00424	/* Global Control Register					*/
+
+/* SPI0 Controller			(0xFFC00500 - 0xFFC005FF)							*/
+#define SPI0_REGBASE			0xFFC00500
+#define SPI0_CTL			0xFFC00500	/* SPI Control Register						*/
+#define SPI0_FLG			0xFFC00504	/* SPI Flag register						*/
+#define SPI0_STAT			0xFFC00508	/* SPI Status register						*/
+#define SPI0_TDBR			0xFFC0050C	/* SPI Transmit Data Buffer Register				*/
+#define SPI0_RDBR			0xFFC00510	/* SPI Receive Data Buffer Register				*/
+#define SPI0_BAUD			0xFFC00514	/* SPI Baud rate Register					*/
+#define SPI0_SHADOW			0xFFC00518	/* SPI_RDBR Shadow Register					*/
+
+/* SPI1 Controller			(0xFFC03400 - 0xFFC034FF)							*/
+#define SPI1_REGBASE			0xFFC03400
+#define SPI1_CTL			0xFFC03400	/* SPI Control Register						*/
+#define SPI1_FLG			0xFFC03404	/* SPI Flag register						*/
+#define SPI1_STAT			0xFFC03408	/* SPI Status register						*/
+#define SPI1_TDBR			0xFFC0340C	/* SPI Transmit Data Buffer Register				*/
+#define SPI1_RDBR			0xFFC03410	/* SPI Receive Data Buffer Register				*/
+#define SPI1_BAUD			0xFFC03414	/* SPI Baud rate Register					*/
+#define SPI1_SHADOW			0xFFC03418	/* SPI_RDBR Shadow Register					*/
+
+/* TIMER0-7 Registers		(0xFFC00600 - 0xFFC006FF)								*/
+#define TIMER0_CONFIG		0xFFC00600	/* Timer 0 Configuration Register			*/
+#define TIMER0_COUNTER		0xFFC00604	/* Timer 0 Counter Register					*/
+#define TIMER0_PERIOD		0xFFC00608	/* Timer 0 Period Register					*/
+#define TIMER0_WIDTH		0xFFC0060C	/* Timer 0 Width Register					*/
+
+#define TIMER1_CONFIG		0xFFC00610	/* Timer 1 Configuration Register  			*/
+#define TIMER1_COUNTER		0xFFC00614	/* Timer 1 Counter Register        			*/
+#define TIMER1_PERIOD		0xFFC00618	/* Timer 1 Period Register         			*/
+#define TIMER1_WIDTH		0xFFC0061C	/* Timer 1 Width Register          			*/
+
+#define TIMER2_CONFIG		0xFFC00620	/* Timer 2 Configuration Register  			*/
+#define TIMER2_COUNTER		0xFFC00624	/* Timer 2 Counter Register        			*/
+#define TIMER2_PERIOD		0xFFC00628	/* Timer 2 Period Register         			*/
+#define TIMER2_WIDTH		0xFFC0062C	/* Timer 2 Width Register          			*/
+
+#define TIMER3_CONFIG		0xFFC00630	/* Timer 3 Configuration Register			*/
+#define TIMER3_COUNTER		0xFFC00634	/* Timer 3 Counter Register					*/
+#define TIMER3_PERIOD		0xFFC00638	/* Timer 3 Period Register					*/
+#define TIMER3_WIDTH		0xFFC0063C	/* Timer 3 Width Register					*/
+
+#define TIMER4_CONFIG		0xFFC00640	/* Timer 4 Configuration Register  			*/
+#define TIMER4_COUNTER		0xFFC00644	/* Timer 4 Counter Register        			*/
+#define TIMER4_PERIOD		0xFFC00648	/* Timer 4 Period Register         			*/
+#define TIMER4_WIDTH		0xFFC0064C	/* Timer 4 Width Register          			*/
+
+#define TIMER5_CONFIG		0xFFC00650	/* Timer 5 Configuration Register  			*/
+#define TIMER5_COUNTER		0xFFC00654	/* Timer 5 Counter Register        			*/
+#define TIMER5_PERIOD		0xFFC00658	/* Timer 5 Period Register         			*/
+#define TIMER5_WIDTH		0xFFC0065C	/* Timer 5 Width Register          			*/
+
+#define TIMER6_CONFIG		0xFFC00660	/* Timer 6 Configuration Register  			*/
+#define TIMER6_COUNTER		0xFFC00664	/* Timer 6 Counter Register        			*/
+#define TIMER6_PERIOD		0xFFC00668	/* Timer 6 Period Register         			*/
+#define TIMER6_WIDTH		0xFFC0066C	/* Timer 6 Width Register          			*/
+
+#define TIMER7_CONFIG		0xFFC00670	/* Timer 7 Configuration Register  			*/
+#define TIMER7_COUNTER		0xFFC00674	/* Timer 7 Counter Register        			*/
+#define TIMER7_PERIOD		0xFFC00678	/* Timer 7 Period Register         			*/
+#define TIMER7_WIDTH		0xFFC0067C	/* Timer 7 Width Register       			*/
+
+#define TIMER_ENABLE		0xFFC00680	/* Timer Enable Register					*/
+#define TIMER_DISABLE		0xFFC00684	/* Timer Disable Register					*/
+#define TIMER_STATUS		0xFFC00688	/* Timer Status Register					*/
+
+/* General Purpose I/O Port F (0xFFC00700 - 0xFFC007FF)												*/
+#define PORTFIO					0xFFC00700	/* Port F I/O Pin State Specify Register				*/
+#define PORTFIO_CLEAR			0xFFC00704	/* Port F I/O Peripheral Interrupt Clear Register		*/
+#define PORTFIO_SET				0xFFC00708	/* Port F I/O Peripheral Interrupt Set Register			*/
+#define PORTFIO_TOGGLE			0xFFC0070C	/* Port F I/O Pin State Toggle Register					*/
+#define PORTFIO_MASKA			0xFFC00710	/* Port F I/O Mask State Specify Interrupt A Register	*/
+#define PORTFIO_MASKA_CLEAR		0xFFC00714	/* Port F I/O Mask Disable Interrupt A Register			*/
+#define PORTFIO_MASKA_SET		0xFFC00718	/* Port F I/O Mask Enable Interrupt A Register			*/
+#define PORTFIO_MASKA_TOGGLE	0xFFC0071C	/* Port F I/O Mask Toggle Enable Interrupt A Register	*/
+#define PORTFIO_MASKB			0xFFC00720	/* Port F I/O Mask State Specify Interrupt B Register	*/
+#define PORTFIO_MASKB_CLEAR		0xFFC00724	/* Port F I/O Mask Disable Interrupt B Register			*/
+#define PORTFIO_MASKB_SET		0xFFC00728	/* Port F I/O Mask Enable Interrupt B Register			*/
+#define PORTFIO_MASKB_TOGGLE	0xFFC0072C	/* Port F I/O Mask Toggle Enable Interrupt B Register	*/
+#define PORTFIO_DIR				0xFFC00730	/* Port F I/O Direction Register						*/
+#define PORTFIO_POLAR			0xFFC00734	/* Port F I/O Source Polarity Register					*/
+#define PORTFIO_EDGE			0xFFC00738	/* Port F I/O Source Sensitivity Register				*/
+#define PORTFIO_BOTH			0xFFC0073C	/* Port F I/O Set on BOTH Edges Register				*/
+#define PORTFIO_INEN			0xFFC00740	/* Port F I/O Input Enable Register 					*/
+
+/* SPORT0 Controller		(0xFFC00800 - 0xFFC008FF)										*/
+#define SPORT0_TCR1			0xFFC00800	/* SPORT0 Transmit Configuration 1 Register			*/
+#define SPORT0_TCR2			0xFFC00804	/* SPORT0 Transmit Configuration 2 Register			*/
+#define SPORT0_TCLKDIV		0xFFC00808	/* SPORT0 Transmit Clock Divider					*/
+#define SPORT0_TFSDIV		0xFFC0080C	/* SPORT0 Transmit Frame Sync Divider				*/
+#define SPORT0_TX			0xFFC00810	/* SPORT0 TX Data Register							*/
+#define SPORT0_RX			0xFFC00818	/* SPORT0 RX Data Register							*/
+#define SPORT0_RCR1			0xFFC00820	/* SPORT0 Transmit Configuration 1 Register			*/
+#define SPORT0_RCR2			0xFFC00824	/* SPORT0 Transmit Configuration 2 Register			*/
+#define SPORT0_RCLKDIV		0xFFC00828	/* SPORT0 Receive Clock Divider						*/
+#define SPORT0_RFSDIV		0xFFC0082C	/* SPORT0 Receive Frame Sync Divider				*/
+#define SPORT0_STAT			0xFFC00830	/* SPORT0 Status Register							*/
+#define SPORT0_CHNL			0xFFC00834	/* SPORT0 Current Channel Register					*/
+#define SPORT0_MCMC1		0xFFC00838	/* SPORT0 Multi-Channel Configuration Register 1	*/
+#define SPORT0_MCMC2		0xFFC0083C	/* SPORT0 Multi-Channel Configuration Register 2	*/
+#define SPORT0_MTCS0		0xFFC00840	/* SPORT0 Multi-Channel Transmit Select Register 0	*/
+#define SPORT0_MTCS1		0xFFC00844	/* SPORT0 Multi-Channel Transmit Select Register 1	*/
+#define SPORT0_MTCS2		0xFFC00848	/* SPORT0 Multi-Channel Transmit Select Register 2	*/
+#define SPORT0_MTCS3		0xFFC0084C	/* SPORT0 Multi-Channel Transmit Select Register 3	*/
+#define SPORT0_MRCS0		0xFFC00850	/* SPORT0 Multi-Channel Receive Select Register 0	*/
+#define SPORT0_MRCS1		0xFFC00854	/* SPORT0 Multi-Channel Receive Select Register 1	*/
+#define SPORT0_MRCS2		0xFFC00858	/* SPORT0 Multi-Channel Receive Select Register 2	*/
+#define SPORT0_MRCS3		0xFFC0085C	/* SPORT0 Multi-Channel Receive Select Register 3	*/
+
+/* SPORT1 Controller		(0xFFC00900 - 0xFFC009FF)										*/
+#define SPORT1_TCR1			0xFFC00900	/* SPORT1 Transmit Configuration 1 Register			*/
+#define SPORT1_TCR2			0xFFC00904	/* SPORT1 Transmit Configuration 2 Register			*/
+#define SPORT1_TCLKDIV		0xFFC00908	/* SPORT1 Transmit Clock Divider					*/
+#define SPORT1_TFSDIV		0xFFC0090C	/* SPORT1 Transmit Frame Sync Divider				*/
+#define SPORT1_TX			0xFFC00910	/* SPORT1 TX Data Register							*/
+#define SPORT1_RX			0xFFC00918	/* SPORT1 RX Data Register							*/
+#define SPORT1_RCR1			0xFFC00920	/* SPORT1 Transmit Configuration 1 Register			*/
+#define SPORT1_RCR2			0xFFC00924	/* SPORT1 Transmit Configuration 2 Register			*/
+#define SPORT1_RCLKDIV		0xFFC00928	/* SPORT1 Receive Clock Divider						*/
+#define SPORT1_RFSDIV		0xFFC0092C	/* SPORT1 Receive Frame Sync Divider				*/
+#define SPORT1_STAT			0xFFC00930	/* SPORT1 Status Register							*/
+#define SPORT1_CHNL			0xFFC00934	/* SPORT1 Current Channel Register					*/
+#define SPORT1_MCMC1		0xFFC00938	/* SPORT1 Multi-Channel Configuration Register 1	*/
+#define SPORT1_MCMC2		0xFFC0093C	/* SPORT1 Multi-Channel Configuration Register 2	*/
+#define SPORT1_MTCS0		0xFFC00940	/* SPORT1 Multi-Channel Transmit Select Register 0	*/
+#define SPORT1_MTCS1		0xFFC00944	/* SPORT1 Multi-Channel Transmit Select Register 1	*/
+#define SPORT1_MTCS2		0xFFC00948	/* SPORT1 Multi-Channel Transmit Select Register 2	*/
+#define SPORT1_MTCS3		0xFFC0094C	/* SPORT1 Multi-Channel Transmit Select Register 3	*/
+#define SPORT1_MRCS0		0xFFC00950	/* SPORT1 Multi-Channel Receive Select Register 0	*/
+#define SPORT1_MRCS1		0xFFC00954	/* SPORT1 Multi-Channel Receive Select Register 1	*/
+#define SPORT1_MRCS2		0xFFC00958	/* SPORT1 Multi-Channel Receive Select Register 2	*/
+#define SPORT1_MRCS3		0xFFC0095C	/* SPORT1 Multi-Channel Receive Select Register 3	*/
+
+/* External Bus Interface Unit (0xFFC00A00 - 0xFFC00AFF)								*/
+#define EBIU_AMGCTL			0xFFC00A00	/* Asynchronous Memory Global Control Register	*/
+#define EBIU_AMBCTL0		0xFFC00A04	/* Asynchronous Memory Bank Control Register 0	*/
+#define EBIU_AMBCTL1		0xFFC00A08	/* Asynchronous Memory Bank Control Register 1	*/
+#define EBIU_SDGCTL			0xFFC00A10	/* SDRAM Global Control Register				*/
+#define EBIU_SDBCTL			0xFFC00A14	/* SDRAM Bank Control Register					*/
+#define EBIU_SDRRC			0xFFC00A18	/* SDRAM Refresh Rate Control Register			*/
+#define EBIU_SDSTAT			0xFFC00A1C	/* SDRAM Status Register						*/
+
+/* DMA Traffic Control Registers													*/
+#define DMA_TC_PER			0xFFC00B0C	/* Traffic Control Periods Register			*/
+#define DMA_TC_CNT			0xFFC00B10	/* Traffic Control Current Counts Register	*/
+
+/* Alternate deprecated register names (below) provided for backwards code compatibility */
+#define DMA_TCPER			0xFFC00B0C	/* Traffic Control Periods Register			*/
+#define DMA_TCCNT			0xFFC00B10	/* Traffic Control Current Counts Register	*/
+
+/* DMA Controller (0xFFC00C00 - 0xFFC00FFF)															*/
+#define DMA0_NEXT_DESC_PTR		0xFFC00C00	/* DMA Channel 0 Next Descriptor Pointer Register		*/
+#define DMA0_START_ADDR			0xFFC00C04	/* DMA Channel 0 Start Address Register					*/
+#define DMA0_CONFIG				0xFFC00C08	/* DMA Channel 0 Configuration Register					*/
+#define DMA0_X_COUNT			0xFFC00C10	/* DMA Channel 0 X Count Register						*/
+#define DMA0_X_MODIFY			0xFFC00C14	/* DMA Channel 0 X Modify Register						*/
+#define DMA0_Y_COUNT			0xFFC00C18	/* DMA Channel 0 Y Count Register						*/
+#define DMA0_Y_MODIFY			0xFFC00C1C	/* DMA Channel 0 Y Modify Register						*/
+#define DMA0_CURR_DESC_PTR		0xFFC00C20	/* DMA Channel 0 Current Descriptor Pointer Register	*/
+#define DMA0_CURR_ADDR			0xFFC00C24	/* DMA Channel 0 Current Address Register				*/
+#define DMA0_IRQ_STATUS			0xFFC00C28	/* DMA Channel 0 Interrupt/Status Register				*/
+#define DMA0_PERIPHERAL_MAP		0xFFC00C2C	/* DMA Channel 0 Peripheral Map Register				*/
+#define DMA0_CURR_X_COUNT		0xFFC00C30	/* DMA Channel 0 Current X Count Register				*/
+#define DMA0_CURR_Y_COUNT		0xFFC00C38	/* DMA Channel 0 Current Y Count Register				*/
+
+#define DMA1_NEXT_DESC_PTR		0xFFC00C40	/* DMA Channel 1 Next Descriptor Pointer Register		*/
+#define DMA1_START_ADDR			0xFFC00C44	/* DMA Channel 1 Start Address Register					*/
+#define DMA1_CONFIG				0xFFC00C48	/* DMA Channel 1 Configuration Register					*/
+#define DMA1_X_COUNT			0xFFC00C50	/* DMA Channel 1 X Count Register						*/
+#define DMA1_X_MODIFY			0xFFC00C54	/* DMA Channel 1 X Modify Register						*/
+#define DMA1_Y_COUNT			0xFFC00C58	/* DMA Channel 1 Y Count Register						*/
+#define DMA1_Y_MODIFY			0xFFC00C5C	/* DMA Channel 1 Y Modify Register						*/
+#define DMA1_CURR_DESC_PTR		0xFFC00C60	/* DMA Channel 1 Current Descriptor Pointer Register	*/
+#define DMA1_CURR_ADDR			0xFFC00C64	/* DMA Channel 1 Current Address Register				*/
+#define DMA1_IRQ_STATUS			0xFFC00C68	/* DMA Channel 1 Interrupt/Status Register				*/
+#define DMA1_PERIPHERAL_MAP		0xFFC00C6C	/* DMA Channel 1 Peripheral Map Register				*/
+#define DMA1_CURR_X_COUNT		0xFFC00C70	/* DMA Channel 1 Current X Count Register				*/
+#define DMA1_CURR_Y_COUNT		0xFFC00C78	/* DMA Channel 1 Current Y Count Register				*/
+
+#define DMA2_NEXT_DESC_PTR		0xFFC00C80	/* DMA Channel 2 Next Descriptor Pointer Register		*/
+#define DMA2_START_ADDR			0xFFC00C84	/* DMA Channel 2 Start Address Register					*/
+#define DMA2_CONFIG				0xFFC00C88	/* DMA Channel 2 Configuration Register					*/
+#define DMA2_X_COUNT			0xFFC00C90	/* DMA Channel 2 X Count Register						*/
+#define DMA2_X_MODIFY			0xFFC00C94	/* DMA Channel 2 X Modify Register						*/
+#define DMA2_Y_COUNT			0xFFC00C98	/* DMA Channel 2 Y Count Register						*/
+#define DMA2_Y_MODIFY			0xFFC00C9C	/* DMA Channel 2 Y Modify Register						*/
+#define DMA2_CURR_DESC_PTR		0xFFC00CA0	/* DMA Channel 2 Current Descriptor Pointer Register	*/
+#define DMA2_CURR_ADDR			0xFFC00CA4	/* DMA Channel 2 Current Address Register				*/
+#define DMA2_IRQ_STATUS			0xFFC00CA8	/* DMA Channel 2 Interrupt/Status Register				*/
+#define DMA2_PERIPHERAL_MAP		0xFFC00CAC	/* DMA Channel 2 Peripheral Map Register				*/
+#define DMA2_CURR_X_COUNT		0xFFC00CB0	/* DMA Channel 2 Current X Count Register				*/
+#define DMA2_CURR_Y_COUNT		0xFFC00CB8	/* DMA Channel 2 Current Y Count Register				*/
+
+#define DMA3_NEXT_DESC_PTR		0xFFC00CC0	/* DMA Channel 3 Next Descriptor Pointer Register		*/
+#define DMA3_START_ADDR			0xFFC00CC4	/* DMA Channel 3 Start Address Register					*/
+#define DMA3_CONFIG				0xFFC00CC8	/* DMA Channel 3 Configuration Register					*/
+#define DMA3_X_COUNT			0xFFC00CD0	/* DMA Channel 3 X Count Register						*/
+#define DMA3_X_MODIFY			0xFFC00CD4	/* DMA Channel 3 X Modify Register						*/
+#define DMA3_Y_COUNT			0xFFC00CD8	/* DMA Channel 3 Y Count Register						*/
+#define DMA3_Y_MODIFY			0xFFC00CDC	/* DMA Channel 3 Y Modify Register						*/
+#define DMA3_CURR_DESC_PTR		0xFFC00CE0	/* DMA Channel 3 Current Descriptor Pointer Register	*/
+#define DMA3_CURR_ADDR			0xFFC00CE4	/* DMA Channel 3 Current Address Register				*/
+#define DMA3_IRQ_STATUS			0xFFC00CE8	/* DMA Channel 3 Interrupt/Status Register				*/
+#define DMA3_PERIPHERAL_MAP		0xFFC00CEC	/* DMA Channel 3 Peripheral Map Register				*/
+#define DMA3_CURR_X_COUNT		0xFFC00CF0	/* DMA Channel 3 Current X Count Register				*/
+#define DMA3_CURR_Y_COUNT		0xFFC00CF8	/* DMA Channel 3 Current Y Count Register				*/
+
+#define DMA4_NEXT_DESC_PTR		0xFFC00D00	/* DMA Channel 4 Next Descriptor Pointer Register		*/
+#define DMA4_START_ADDR			0xFFC00D04	/* DMA Channel 4 Start Address Register					*/
+#define DMA4_CONFIG				0xFFC00D08	/* DMA Channel 4 Configuration Register					*/
+#define DMA4_X_COUNT			0xFFC00D10	/* DMA Channel 4 X Count Register						*/
+#define DMA4_X_MODIFY			0xFFC00D14	/* DMA Channel 4 X Modify Register						*/
+#define DMA4_Y_COUNT			0xFFC00D18	/* DMA Channel 4 Y Count Register						*/
+#define DMA4_Y_MODIFY			0xFFC00D1C	/* DMA Channel 4 Y Modify Register						*/
+#define DMA4_CURR_DESC_PTR		0xFFC00D20	/* DMA Channel 4 Current Descriptor Pointer Register	*/
+#define DMA4_CURR_ADDR			0xFFC00D24	/* DMA Channel 4 Current Address Register				*/
+#define DMA4_IRQ_STATUS			0xFFC00D28	/* DMA Channel 4 Interrupt/Status Register				*/
+#define DMA4_PERIPHERAL_MAP		0xFFC00D2C	/* DMA Channel 4 Peripheral Map Register				*/
+#define DMA4_CURR_X_COUNT		0xFFC00D30	/* DMA Channel 4 Current X Count Register				*/
+#define DMA4_CURR_Y_COUNT		0xFFC00D38	/* DMA Channel 4 Current Y Count Register				*/
+
+#define DMA5_NEXT_DESC_PTR		0xFFC00D40	/* DMA Channel 5 Next Descriptor Pointer Register		*/
+#define DMA5_START_ADDR			0xFFC00D44	/* DMA Channel 5 Start Address Register					*/
+#define DMA5_CONFIG				0xFFC00D48	/* DMA Channel 5 Configuration Register					*/
+#define DMA5_X_COUNT			0xFFC00D50	/* DMA Channel 5 X Count Register						*/
+#define DMA5_X_MODIFY			0xFFC00D54	/* DMA Channel 5 X Modify Register						*/
+#define DMA5_Y_COUNT			0xFFC00D58	/* DMA Channel 5 Y Count Register						*/
+#define DMA5_Y_MODIFY			0xFFC00D5C	/* DMA Channel 5 Y Modify Register						*/
+#define DMA5_CURR_DESC_PTR		0xFFC00D60	/* DMA Channel 5 Current Descriptor Pointer Register	*/
+#define DMA5_CURR_ADDR			0xFFC00D64	/* DMA Channel 5 Current Address Register				*/
+#define DMA5_IRQ_STATUS			0xFFC00D68	/* DMA Channel 5 Interrupt/Status Register				*/
+#define DMA5_PERIPHERAL_MAP		0xFFC00D6C	/* DMA Channel 5 Peripheral Map Register				*/
+#define DMA5_CURR_X_COUNT		0xFFC00D70	/* DMA Channel 5 Current X Count Register				*/
+#define DMA5_CURR_Y_COUNT		0xFFC00D78	/* DMA Channel 5 Current Y Count Register				*/
+
+#define DMA6_NEXT_DESC_PTR		0xFFC00D80	/* DMA Channel 6 Next Descriptor Pointer Register		*/
+#define DMA6_START_ADDR			0xFFC00D84	/* DMA Channel 6 Start Address Register					*/
+#define DMA6_CONFIG				0xFFC00D88	/* DMA Channel 6 Configuration Register					*/
+#define DMA6_X_COUNT			0xFFC00D90	/* DMA Channel 6 X Count Register						*/
+#define DMA6_X_MODIFY			0xFFC00D94	/* DMA Channel 6 X Modify Register						*/
+#define DMA6_Y_COUNT			0xFFC00D98	/* DMA Channel 6 Y Count Register						*/
+#define DMA6_Y_MODIFY			0xFFC00D9C	/* DMA Channel 6 Y Modify Register						*/
+#define DMA6_CURR_DESC_PTR		0xFFC00DA0	/* DMA Channel 6 Current Descriptor Pointer Register	*/
+#define DMA6_CURR_ADDR			0xFFC00DA4	/* DMA Channel 6 Current Address Register				*/
+#define DMA6_IRQ_STATUS			0xFFC00DA8	/* DMA Channel 6 Interrupt/Status Register				*/
+#define DMA6_PERIPHERAL_MAP		0xFFC00DAC	/* DMA Channel 6 Peripheral Map Register				*/
+#define DMA6_CURR_X_COUNT		0xFFC00DB0	/* DMA Channel 6 Current X Count Register				*/
+#define DMA6_CURR_Y_COUNT		0xFFC00DB8	/* DMA Channel 6 Current Y Count Register				*/
+
+#define DMA7_NEXT_DESC_PTR		0xFFC00DC0	/* DMA Channel 7 Next Descriptor Pointer Register		*/
+#define DMA7_START_ADDR			0xFFC00DC4	/* DMA Channel 7 Start Address Register					*/
+#define DMA7_CONFIG				0xFFC00DC8	/* DMA Channel 7 Configuration Register					*/
+#define DMA7_X_COUNT			0xFFC00DD0	/* DMA Channel 7 X Count Register						*/
+#define DMA7_X_MODIFY			0xFFC00DD4	/* DMA Channel 7 X Modify Register						*/
+#define DMA7_Y_COUNT			0xFFC00DD8	/* DMA Channel 7 Y Count Register						*/
+#define DMA7_Y_MODIFY			0xFFC00DDC	/* DMA Channel 7 Y Modify Register						*/
+#define DMA7_CURR_DESC_PTR		0xFFC00DE0	/* DMA Channel 7 Current Descriptor Pointer Register	*/
+#define DMA7_CURR_ADDR			0xFFC00DE4	/* DMA Channel 7 Current Address Register				*/
+#define DMA7_IRQ_STATUS			0xFFC00DE8	/* DMA Channel 7 Interrupt/Status Register				*/
+#define DMA7_PERIPHERAL_MAP		0xFFC00DEC	/* DMA Channel 7 Peripheral Map Register				*/
+#define DMA7_CURR_X_COUNT		0xFFC00DF0	/* DMA Channel 7 Current X Count Register				*/
+#define DMA7_CURR_Y_COUNT		0xFFC00DF8	/* DMA Channel 7 Current Y Count Register				*/
+
+#define DMA8_NEXT_DESC_PTR		0xFFC00E00	/* DMA Channel 8 Next Descriptor Pointer Register		*/
+#define DMA8_START_ADDR			0xFFC00E04	/* DMA Channel 8 Start Address Register					*/
+#define DMA8_CONFIG				0xFFC00E08	/* DMA Channel 8 Configuration Register					*/
+#define DMA8_X_COUNT			0xFFC00E10	/* DMA Channel 8 X Count Register						*/
+#define DMA8_X_MODIFY			0xFFC00E14	/* DMA Channel 8 X Modify Register						*/
+#define DMA8_Y_COUNT			0xFFC00E18	/* DMA Channel 8 Y Count Register						*/
+#define DMA8_Y_MODIFY			0xFFC00E1C	/* DMA Channel 8 Y Modify Register						*/
+#define DMA8_CURR_DESC_PTR		0xFFC00E20	/* DMA Channel 8 Current Descriptor Pointer Register	*/
+#define DMA8_CURR_ADDR			0xFFC00E24	/* DMA Channel 8 Current Address Register				*/
+#define DMA8_IRQ_STATUS			0xFFC00E28	/* DMA Channel 8 Interrupt/Status Register				*/
+#define DMA8_PERIPHERAL_MAP		0xFFC00E2C	/* DMA Channel 8 Peripheral Map Register				*/
+#define DMA8_CURR_X_COUNT		0xFFC00E30	/* DMA Channel 8 Current X Count Register				*/
+#define DMA8_CURR_Y_COUNT		0xFFC00E38	/* DMA Channel 8 Current Y Count Register				*/
+
+#define DMA9_NEXT_DESC_PTR		0xFFC00E40	/* DMA Channel 9 Next Descriptor Pointer Register		*/
+#define DMA9_START_ADDR			0xFFC00E44	/* DMA Channel 9 Start Address Register					*/
+#define DMA9_CONFIG				0xFFC00E48	/* DMA Channel 9 Configuration Register					*/
+#define DMA9_X_COUNT			0xFFC00E50	/* DMA Channel 9 X Count Register						*/
+#define DMA9_X_MODIFY			0xFFC00E54	/* DMA Channel 9 X Modify Register						*/
+#define DMA9_Y_COUNT			0xFFC00E58	/* DMA Channel 9 Y Count Register						*/
+#define DMA9_Y_MODIFY			0xFFC00E5C	/* DMA Channel 9 Y Modify Register						*/
+#define DMA9_CURR_DESC_PTR		0xFFC00E60	/* DMA Channel 9 Current Descriptor Pointer Register	*/
+#define DMA9_CURR_ADDR			0xFFC00E64	/* DMA Channel 9 Current Address Register				*/
+#define DMA9_IRQ_STATUS			0xFFC00E68	/* DMA Channel 9 Interrupt/Status Register				*/
+#define DMA9_PERIPHERAL_MAP		0xFFC00E6C	/* DMA Channel 9 Peripheral Map Register				*/
+#define DMA9_CURR_X_COUNT		0xFFC00E70	/* DMA Channel 9 Current X Count Register				*/
+#define DMA9_CURR_Y_COUNT		0xFFC00E78	/* DMA Channel 9 Current Y Count Register				*/
+
+#define DMA10_NEXT_DESC_PTR		0xFFC00E80	/* DMA Channel 10 Next Descriptor Pointer Register		*/
+#define DMA10_START_ADDR		0xFFC00E84	/* DMA Channel 10 Start Address Register				*/
+#define DMA10_CONFIG			0xFFC00E88	/* DMA Channel 10 Configuration Register				*/
+#define DMA10_X_COUNT			0xFFC00E90	/* DMA Channel 10 X Count Register						*/
+#define DMA10_X_MODIFY			0xFFC00E94	/* DMA Channel 10 X Modify Register						*/
+#define DMA10_Y_COUNT			0xFFC00E98	/* DMA Channel 10 Y Count Register						*/
+#define DMA10_Y_MODIFY			0xFFC00E9C	/* DMA Channel 10 Y Modify Register						*/
+#define DMA10_CURR_DESC_PTR		0xFFC00EA0	/* DMA Channel 10 Current Descriptor Pointer Register	*/
+#define DMA10_CURR_ADDR			0xFFC00EA4	/* DMA Channel 10 Current Address Register				*/
+#define DMA10_IRQ_STATUS		0xFFC00EA8	/* DMA Channel 10 Interrupt/Status Register				*/
+#define DMA10_PERIPHERAL_MAP	0xFFC00EAC	/* DMA Channel 10 Peripheral Map Register				*/
+#define DMA10_CURR_X_COUNT		0xFFC00EB0	/* DMA Channel 10 Current X Count Register				*/
+#define DMA10_CURR_Y_COUNT		0xFFC00EB8	/* DMA Channel 10 Current Y Count Register				*/
+
+#define DMA11_NEXT_DESC_PTR		0xFFC00EC0	/* DMA Channel 11 Next Descriptor Pointer Register		*/
+#define DMA11_START_ADDR		0xFFC00EC4	/* DMA Channel 11 Start Address Register				*/
+#define DMA11_CONFIG			0xFFC00EC8	/* DMA Channel 11 Configuration Register				*/
+#define DMA11_X_COUNT			0xFFC00ED0	/* DMA Channel 11 X Count Register						*/
+#define DMA11_X_MODIFY			0xFFC00ED4	/* DMA Channel 11 X Modify Register						*/
+#define DMA11_Y_COUNT			0xFFC00ED8	/* DMA Channel 11 Y Count Register						*/
+#define DMA11_Y_MODIFY			0xFFC00EDC	/* DMA Channel 11 Y Modify Register						*/
+#define DMA11_CURR_DESC_PTR		0xFFC00EE0	/* DMA Channel 11 Current Descriptor Pointer Register	*/
+#define DMA11_CURR_ADDR			0xFFC00EE4	/* DMA Channel 11 Current Address Register				*/
+#define DMA11_IRQ_STATUS		0xFFC00EE8	/* DMA Channel 11 Interrupt/Status Register				*/
+#define DMA11_PERIPHERAL_MAP	0xFFC00EEC	/* DMA Channel 11 Peripheral Map Register				*/
+#define DMA11_CURR_X_COUNT		0xFFC00EF0	/* DMA Channel 11 Current X Count Register				*/
+#define DMA11_CURR_Y_COUNT		0xFFC00EF8	/* DMA Channel 11 Current Y Count Register				*/
+
+#define MDMA_D0_NEXT_DESC_PTR	0xFFC00F00	/* MemDMA Stream 0 Destination Next Descriptor Pointer Register		*/
+#define MDMA_D0_START_ADDR		0xFFC00F04	/* MemDMA Stream 0 Destination Start Address Register				*/
+#define MDMA_D0_CONFIG			0xFFC00F08	/* MemDMA Stream 0 Destination Configuration Register				*/
+#define MDMA_D0_X_COUNT			0xFFC00F10	/* MemDMA Stream 0 Destination X Count Register						*/
+#define MDMA_D0_X_MODIFY		0xFFC00F14	/* MemDMA Stream 0 Destination X Modify Register					*/
+#define MDMA_D0_Y_COUNT			0xFFC00F18	/* MemDMA Stream 0 Destination Y Count Register						*/
+#define MDMA_D0_Y_MODIFY		0xFFC00F1C	/* MemDMA Stream 0 Destination Y Modify Register					*/
+#define MDMA_D0_CURR_DESC_PTR	0xFFC00F20	/* MemDMA Stream 0 Destination Current Descriptor Pointer Register	*/
+#define MDMA_D0_CURR_ADDR		0xFFC00F24	/* MemDMA Stream 0 Destination Current Address Register				*/
+#define MDMA_D0_IRQ_STATUS		0xFFC00F28	/* MemDMA Stream 0 Destination Interrupt/Status Register			*/
+#define MDMA_D0_PERIPHERAL_MAP	0xFFC00F2C	/* MemDMA Stream 0 Destination Peripheral Map Register				*/
+#define MDMA_D0_CURR_X_COUNT	0xFFC00F30	/* MemDMA Stream 0 Destination Current X Count Register				*/
+#define MDMA_D0_CURR_Y_COUNT	0xFFC00F38	/* MemDMA Stream 0 Destination Current Y Count Register				*/
+
+#define MDMA_S0_NEXT_DESC_PTR	0xFFC00F40	/* MemDMA Stream 0 Source Next Descriptor Pointer Register			*/
+#define MDMA_S0_START_ADDR		0xFFC00F44	/* MemDMA Stream 0 Source Start Address Register					*/
+#define MDMA_S0_CONFIG			0xFFC00F48	/* MemDMA Stream 0 Source Configuration Register					*/
+#define MDMA_S0_X_COUNT			0xFFC00F50	/* MemDMA Stream 0 Source X Count Register							*/
+#define MDMA_S0_X_MODIFY		0xFFC00F54	/* MemDMA Stream 0 Source X Modify Register							*/
+#define MDMA_S0_Y_COUNT			0xFFC00F58	/* MemDMA Stream 0 Source Y Count Register							*/
+#define MDMA_S0_Y_MODIFY		0xFFC00F5C	/* MemDMA Stream 0 Source Y Modify Register							*/
+#define MDMA_S0_CURR_DESC_PTR	0xFFC00F60	/* MemDMA Stream 0 Source Current Descriptor Pointer Register		*/
+#define MDMA_S0_CURR_ADDR		0xFFC00F64	/* MemDMA Stream 0 Source Current Address Register					*/
+#define MDMA_S0_IRQ_STATUS		0xFFC00F68	/* MemDMA Stream 0 Source Interrupt/Status Register					*/
+#define MDMA_S0_PERIPHERAL_MAP	0xFFC00F6C	/* MemDMA Stream 0 Source Peripheral Map Register					*/
+#define MDMA_S0_CURR_X_COUNT	0xFFC00F70	/* MemDMA Stream 0 Source Current X Count Register					*/
+#define MDMA_S0_CURR_Y_COUNT	0xFFC00F78	/* MemDMA Stream 0 Source Current Y Count Register					*/
+
+#define MDMA_D1_NEXT_DESC_PTR	0xFFC00F80	/* MemDMA Stream 1 Destination Next Descriptor Pointer Register		*/
+#define MDMA_D1_START_ADDR		0xFFC00F84	/* MemDMA Stream 1 Destination Start Address Register				*/
+#define MDMA_D1_CONFIG			0xFFC00F88	/* MemDMA Stream 1 Destination Configuration Register				*/
+#define MDMA_D1_X_COUNT			0xFFC00F90	/* MemDMA Stream 1 Destination X Count Register						*/
+#define MDMA_D1_X_MODIFY		0xFFC00F94	/* MemDMA Stream 1 Destination X Modify Register					*/
+#define MDMA_D1_Y_COUNT			0xFFC00F98	/* MemDMA Stream 1 Destination Y Count Register						*/
+#define MDMA_D1_Y_MODIFY		0xFFC00F9C	/* MemDMA Stream 1 Destination Y Modify Register					*/
+#define MDMA_D1_CURR_DESC_PTR	0xFFC00FA0	/* MemDMA Stream 1 Destination Current Descriptor Pointer Register	*/
+#define MDMA_D1_CURR_ADDR		0xFFC00FA4	/* MemDMA Stream 1 Destination Current Address Register				*/
+#define MDMA_D1_IRQ_STATUS		0xFFC00FA8	/* MemDMA Stream 1 Destination Interrupt/Status Register			*/
+#define MDMA_D1_PERIPHERAL_MAP	0xFFC00FAC	/* MemDMA Stream 1 Destination Peripheral Map Register				*/
+#define MDMA_D1_CURR_X_COUNT	0xFFC00FB0	/* MemDMA Stream 1 Destination Current X Count Register				*/
+#define MDMA_D1_CURR_Y_COUNT	0xFFC00FB8	/* MemDMA Stream 1 Destination Current Y Count Register				*/
+
+#define MDMA_S1_NEXT_DESC_PTR	0xFFC00FC0	/* MemDMA Stream 1 Source Next Descriptor Pointer Register			*/
+#define MDMA_S1_START_ADDR		0xFFC00FC4	/* MemDMA Stream 1 Source Start Address Register					*/
+#define MDMA_S1_CONFIG			0xFFC00FC8	/* MemDMA Stream 1 Source Configuration Register					*/
+#define MDMA_S1_X_COUNT			0xFFC00FD0	/* MemDMA Stream 1 Source X Count Register							*/
+#define MDMA_S1_X_MODIFY		0xFFC00FD4	/* MemDMA Stream 1 Source X Modify Register							*/
+#define MDMA_S1_Y_COUNT			0xFFC00FD8	/* MemDMA Stream 1 Source Y Count Register							*/
+#define MDMA_S1_Y_MODIFY		0xFFC00FDC	/* MemDMA Stream 1 Source Y Modify Register							*/
+#define MDMA_S1_CURR_DESC_PTR	0xFFC00FE0	/* MemDMA Stream 1 Source Current Descriptor Pointer Register		*/
+#define MDMA_S1_CURR_ADDR		0xFFC00FE4	/* MemDMA Stream 1 Source Current Address Register					*/
+#define MDMA_S1_IRQ_STATUS		0xFFC00FE8	/* MemDMA Stream 1 Source Interrupt/Status Register					*/
+#define MDMA_S1_PERIPHERAL_MAP	0xFFC00FEC	/* MemDMA Stream 1 Source Peripheral Map Register					*/
+#define MDMA_S1_CURR_X_COUNT	0xFFC00FF0	/* MemDMA Stream 1 Source Current X Count Register					*/
+#define MDMA_S1_CURR_Y_COUNT	0xFFC00FF8	/* MemDMA Stream 1 Source Current Y Count Register					*/
+
+
+/* Parallel Peripheral Interface (0xFFC01000 - 0xFFC010FF)				*/
+#define PPI_CONTROL			0xFFC01000	/* PPI Control Register			*/
+#define PPI_STATUS			0xFFC01004	/* PPI Status Register			*/
+#define PPI_COUNT			0xFFC01008	/* PPI Transfer Count Register	*/
+#define PPI_DELAY			0xFFC0100C	/* PPI Delay Count Register		*/
+#define PPI_FRAME			0xFFC01010	/* PPI Frame Length Register	*/
+
+
+/* Two-Wire Interface		(0xFFC01400 - 0xFFC014FF)								*/
+#define TWI0_REGBASE			0xFFC01400
+#define TWI_CLKDIV			0xFFC01400	/* Serial Clock Divider Register			*/
+#define TWI_CONTROL			0xFFC01404	/* TWI Control Register						*/
+#define TWI_SLAVE_CTL		0xFFC01408	/* Slave Mode Control Register				*/
+#define TWI_SLAVE_STAT		0xFFC0140C	/* Slave Mode Status Register				*/
+#define TWI_SLAVE_ADDR		0xFFC01410	/* Slave Mode Address Register				*/
+#define TWI_MASTER_CTL		0xFFC01414	/* Master Mode Control Register				*/
+#define TWI_MASTER_STAT		0xFFC01418	/* Master Mode Status Register				*/
+#define TWI_MASTER_ADDR		0xFFC0141C	/* Master Mode Address Register				*/
+#define TWI_INT_STAT		0xFFC01420	/* TWI Interrupt Status Register			*/
+#define TWI_INT_MASK		0xFFC01424	/* TWI Master Interrupt Mask Register		*/
+#define TWI_FIFO_CTL		0xFFC01428	/* FIFO Control Register					*/
+#define TWI_FIFO_STAT		0xFFC0142C	/* FIFO Status Register						*/
+#define TWI_XMT_DATA8		0xFFC01480	/* FIFO Transmit Data Single Byte Register	*/
+#define TWI_XMT_DATA16		0xFFC01484	/* FIFO Transmit Data Double Byte Register	*/
+#define TWI_RCV_DATA8		0xFFC01488	/* FIFO Receive Data Single Byte Register	*/
+#define TWI_RCV_DATA16		0xFFC0148C	/* FIFO Receive Data Double Byte Register	*/
+
+
+/* General Purpose I/O Port G (0xFFC01500 - 0xFFC015FF)												*/
+#define PORTGIO					0xFFC01500	/* Port G I/O Pin State Specify Register				*/
+#define PORTGIO_CLEAR			0xFFC01504	/* Port G I/O Peripheral Interrupt Clear Register		*/
+#define PORTGIO_SET				0xFFC01508	/* Port G I/O Peripheral Interrupt Set Register			*/
+#define PORTGIO_TOGGLE			0xFFC0150C	/* Port G I/O Pin State Toggle Register					*/
+#define PORTGIO_MASKA			0xFFC01510	/* Port G I/O Mask State Specify Interrupt A Register	*/
+#define PORTGIO_MASKA_CLEAR		0xFFC01514	/* Port G I/O Mask Disable Interrupt A Register			*/
+#define PORTGIO_MASKA_SET		0xFFC01518	/* Port G I/O Mask Enable Interrupt A Register			*/
+#define PORTGIO_MASKA_TOGGLE	0xFFC0151C	/* Port G I/O Mask Toggle Enable Interrupt A Register	*/
+#define PORTGIO_MASKB			0xFFC01520	/* Port G I/O Mask State Specify Interrupt B Register	*/
+#define PORTGIO_MASKB_CLEAR		0xFFC01524	/* Port G I/O Mask Disable Interrupt B Register			*/
+#define PORTGIO_MASKB_SET		0xFFC01528	/* Port G I/O Mask Enable Interrupt B Register			*/
+#define PORTGIO_MASKB_TOGGLE	0xFFC0152C	/* Port G I/O Mask Toggle Enable Interrupt B Register	*/
+#define PORTGIO_DIR				0xFFC01530	/* Port G I/O Direction Register						*/
+#define PORTGIO_POLAR			0xFFC01534	/* Port G I/O Source Polarity Register					*/
+#define PORTGIO_EDGE			0xFFC01538	/* Port G I/O Source Sensitivity Register				*/
+#define PORTGIO_BOTH			0xFFC0153C	/* Port G I/O Set on BOTH Edges Register				*/
+#define PORTGIO_INEN			0xFFC01540	/* Port G I/O Input Enable Register						*/
+
+
+/* General Purpose I/O Port H (0xFFC01700 - 0xFFC017FF)												*/
+#define PORTHIO					0xFFC01700	/* Port H I/O Pin State Specify Register				*/
+#define PORTHIO_CLEAR			0xFFC01704	/* Port H I/O Peripheral Interrupt Clear Register		*/
+#define PORTHIO_SET				0xFFC01708	/* Port H I/O Peripheral Interrupt Set Register			*/
+#define PORTHIO_TOGGLE			0xFFC0170C	/* Port H I/O Pin State Toggle Register					*/
+#define PORTHIO_MASKA			0xFFC01710	/* Port H I/O Mask State Specify Interrupt A Register	*/
+#define PORTHIO_MASKA_CLEAR		0xFFC01714	/* Port H I/O Mask Disable Interrupt A Register			*/
+#define PORTHIO_MASKA_SET		0xFFC01718	/* Port H I/O Mask Enable Interrupt A Register			*/
+#define PORTHIO_MASKA_TOGGLE	0xFFC0171C	/* Port H I/O Mask Toggle Enable Interrupt A Register	*/
+#define PORTHIO_MASKB			0xFFC01720	/* Port H I/O Mask State Specify Interrupt B Register	*/
+#define PORTHIO_MASKB_CLEAR		0xFFC01724	/* Port H I/O Mask Disable Interrupt B Register			*/
+#define PORTHIO_MASKB_SET		0xFFC01728	/* Port H I/O Mask Enable Interrupt B Register			*/
+#define PORTHIO_MASKB_TOGGLE	0xFFC0172C	/* Port H I/O Mask Toggle Enable Interrupt B Register	*/
+#define PORTHIO_DIR				0xFFC01730	/* Port H I/O Direction Register						*/
+#define PORTHIO_POLAR			0xFFC01734	/* Port H I/O Source Polarity Register					*/
+#define PORTHIO_EDGE			0xFFC01738	/* Port H I/O Source Sensitivity Register				*/
+#define PORTHIO_BOTH			0xFFC0173C	/* Port H I/O Set on BOTH Edges Register				*/
+#define PORTHIO_INEN			0xFFC01740	/* Port H I/O Input Enable Register						*/
+
+
+/* UART1 Controller		(0xFFC02000 - 0xFFC020FF)								*/
+#define UART1_THR			0xFFC02000	/* Transmit Holding register			*/
+#define UART1_RBR			0xFFC02000	/* Receive Buffer register				*/
+#define UART1_DLL			0xFFC02000	/* Divisor Latch (Low-Byte)				*/
+#define UART1_IER			0xFFC02004	/* Interrupt Enable Register			*/
+#define UART1_DLH			0xFFC02004	/* Divisor Latch (High-Byte)			*/
+#define UART1_IIR			0xFFC02008	/* Interrupt Identification Register	*/
+#define UART1_LCR			0xFFC0200C	/* Line Control Register				*/
+#define UART1_MCR			0xFFC02010	/* Modem Control Register				*/
+#define UART1_LSR			0xFFC02014	/* Line Status Register					*/
+#define UART1_MSR			0xFFC02018	/* Modem Status Register				*/
+#define UART1_SCR			0xFFC0201C	/* SCR Scratch Register					*/
+#define UART1_GCTL			0xFFC02024	/* Global Control Register				*/
+
+
+/* Pin Control Registers	(0xFFC03200 - 0xFFC032FF)											*/
+#define PORTF_FER			0xFFC03200	/* Port F Function Enable Register (Alternate/Flag*)	*/
+#define PORTG_FER			0xFFC03204	/* Port G Function Enable Register (Alternate/Flag*)	*/
+#define PORTH_FER			0xFFC03208	/* Port H Function Enable Register (Alternate/Flag*)	*/
+#define BFIN_PORT_MUX			0xFFC0320C	/* Port Multiplexer Control Register					*/
+
+
+/* Handshake MDMA Registers	(0xFFC03300 - 0xFFC033FF)										*/
+#define HMDMA0_CONTROL		0xFFC03300	/* Handshake MDMA0 Control Register					*/
+#define HMDMA0_ECINIT		0xFFC03304	/* HMDMA0 Initial Edge Count Register				*/
+#define HMDMA0_BCINIT		0xFFC03308	/* HMDMA0 Initial Block Count Register				*/
+#define HMDMA0_ECURGENT		0xFFC0330C	/* HMDMA0 Urgent Edge Count Threshhold Register		*/
+#define HMDMA0_ECOVERFLOW	0xFFC03310	/* HMDMA0 Edge Count Overflow Interrupt Register	*/
+#define HMDMA0_ECOUNT		0xFFC03314	/* HMDMA0 Current Edge Count Register				*/
+#define HMDMA0_BCOUNT		0xFFC03318	/* HMDMA0 Current Block Count Register				*/
+
+#define HMDMA1_CONTROL		0xFFC03340	/* Handshake MDMA1 Control Register					*/
+#define HMDMA1_ECINIT		0xFFC03344	/* HMDMA1 Initial Edge Count Register				*/
+#define HMDMA1_BCINIT		0xFFC03348	/* HMDMA1 Initial Block Count Register				*/
+#define HMDMA1_ECURGENT		0xFFC0334C	/* HMDMA1 Urgent Edge Count Threshhold Register		*/
+#define HMDMA1_ECOVERFLOW	0xFFC03350	/* HMDMA1 Edge Count Overflow Interrupt Register	*/
+#define HMDMA1_ECOUNT		0xFFC03354	/* HMDMA1 Current Edge Count Register				*/
+#define HMDMA1_BCOUNT		0xFFC03358	/* HMDMA1 Current Block Count Register				*/
+
+
+/* GPIO PIN mux (0xFFC03210 - OxFFC03288) */
+#define PORTF_MUX               0xFFC03210      /* Port F mux control */
+#define PORTG_MUX               0xFFC03214      /* Port G mux control */
+#define PORTH_MUX               0xFFC03218      /* Port H mux control */
+#define PORTF_DRIVE             0xFFC03220      /* Port F drive strength control */
+#define PORTG_DRIVE             0xFFC03224      /* Port G drive strength control */
+#define PORTH_DRIVE             0xFFC03228      /* Port H drive strength control */
+#define PORTF_SLEW              0xFFC03230      /* Port F slew control */
+#define PORTG_SLEW              0xFFC03234      /* Port G slew control */
+#define PORTH_SLEW              0xFFC03238      /* Port H slew control */
+#define PORTF_HYSTERISIS        0xFFC03240      /* Port F Schmitt trigger control */
+#define PORTG_HYSTERISIS        0xFFC03244      /* Port G Schmitt trigger control */
+#define PORTH_HYSTERISIS        0xFFC03248      /* Port H Schmitt trigger control */
+#define MISCPORT_DRIVE          0xFFC03280      /* Misc Port drive strength control */
+#define MISCPORT_SLEW           0xFFC03284      /* Misc Port slew control */
+#define MISCPORT_HYSTERISIS     0xFFC03288      /* Misc Port Schmitt trigger control */
+
+
+/***********************************************************************************
+** System MMR Register Bits And Macros
+**
+** Disclaimer:	All macros are intended to make C and Assembly code more readable.
+**				Use these macros carefully, as any that do left shifts for field
+**				depositing will result in the lower order bits being destroyed.  Any
+**				macro that shifts left to properly position the bit-field should be
+**				used as part of an OR to initialize a register and NOT as a dynamic
+**				modifier UNLESS the lower order bits are saved and ORed back in when
+**				the macro is used.
+*************************************************************************************/
+/*
+** ********************* PLL AND RESET MASKS ****************************************/
+/* PLL_CTL Masks																	*/
+#define DF				0x0001	/* 0: PLL = CLKIN, 1: PLL = CLKIN/2					*/
+#define PLL_OFF			0x0002	/* PLL Not Powered									*/
+#define STOPCK			0x0008	/* Core Clock Off									*/
+#define PDWN			0x0020	/* Enter Deep Sleep Mode							*/
+#define	IN_DELAY		0x0040	/* Add 200ps Delay To EBIU Input Latches			*/
+#define	OUT_DELAY		0x0080	/* Add 200ps Delay To EBIU Output Signals			*/
+#define BYPASS			0x0100	/* Bypass the PLL									*/
+#define	MSEL			0x7E00	/* Multiplier Select For CCLK/VCO Factors			*/
+/* PLL_CTL Macros (Only Use With Logic OR While Setting Lower Order Bits)			*/
+#define	SET_MSEL(x)		(((x)&0x3F) << 0x9)	/* Set MSEL = 0-63 --> VCO = CLKIN*MSEL		*/
+
+/* PLL_DIV Masks														*/
+#define SSEL			0x000F	/* System Select						*/
+#define	CSEL			0x0030	/* Core Select							*/
+#define CSEL_DIV1		0x0000	/* 		CCLK = VCO / 1					*/
+#define CSEL_DIV2		0x0010	/* 		CCLK = VCO / 2					*/
+#define	CSEL_DIV4		0x0020	/* 		CCLK = VCO / 4					*/
+#define	CSEL_DIV8		0x0030	/* 		CCLK = VCO / 8					*/
+/* PLL_DIV Macros														*/
+#define SET_SSEL(x)		((x)&0xF)		/* Set SSEL = 0-15 --> SCLK = VCO/SSEL	*/
+
+/* VR_CTL Masks	*/
+#define	FREQ			0x3000	/* Switching Oscillator Frequency For Regulator	*/
+#define	HIBERNATE		0x0000	/* 		Powerdown/Bypass On-Board Regulation	*/
+
+#define	VLEV			0x00F0	/* Internal Voltage Level					*/
+#define	VLEV_085 		0x0060	/* 		VLEV = 0.85 V (-5% - +10% Accuracy)	*/
+#define	VLEV_090		0x0070	/* 		VLEV = 0.90 V (-5% - +10% Accuracy)	*/
+#define	VLEV_095		0x0080	/* 		VLEV = 0.95 V (-5% - +10% Accuracy)	*/
+#define	VLEV_100		0x0090	/* 		VLEV = 1.00 V (-5% - +10% Accuracy)	*/
+#define	VLEV_105		0x00A0	/* 		VLEV = 1.05 V (-5% - +10% Accuracy)	*/
+#define	VLEV_110		0x00B0	/* 		VLEV = 1.10 V (-5% - +10% Accuracy)	*/
+#define	VLEV_115		0x00C0	/* 		VLEV = 1.15 V (-5% - +10% Accuracy)	*/
+#define	VLEV_120		0x00D0	/* 		VLEV = 1.20 V (-5% - +10% Accuracy)	*/
+#define	VLEV_125		0x00E0	/* 		VLEV = 1.25 V (-5% - +10% Accuracy)	*/
+#define	VLEV_130		0x00F0	/* 		VLEV = 1.30 V (-5% - +10% Accuracy)	*/
+
+#define	WAKE			0x0100	/* Enable RTC/Reset Wakeup From Hibernate	*/
+#define	USBWE			0x0200	/* Enable USB Wakeup From Hibernate			*/
+#define	PHYWE			0x0400	/* Enable PHY Wakeup From Hibernate			*/
+#define	CLKBUFOE		0x4000	/* CLKIN Buffer Output Enable */
+#define	PHYCLKOE		CLKBUFOE	/* Alternative legacy name for the above */
+#define	SCKELOW		0x8000	/* Enable Drive CKE Low During Reset		*/
+
+/* PLL_STAT Masks																	*/
+#define ACTIVE_PLLENABLED	0x0001	/* Processor In Active Mode With PLL Enabled	*/
+#define	FULL_ON				0x0002	/* Processor In Full On Mode					*/
+#define ACTIVE_PLLDISABLED	0x0004	/* Processor In Active Mode With PLL Disabled	*/
+#define	PLL_LOCKED			0x0020	/* PLL_LOCKCNT Has Been Reached					*/
+
+/* CHIPID Masks */
+#define CHIPID_VERSION         0xF0000000
+#define CHIPID_FAMILY          0x0FFFF000
+#define CHIPID_MANUFACTURE     0x00000FFE
+
+/* SWRST Masks																		*/
+#define SYSTEM_RESET		0x0007	/* Initiates A System Software Reset			*/
+#define	DOUBLE_FAULT		0x0008	/* Core Double Fault Causes Reset				*/
+#define RESET_DOUBLE		0x2000	/* SW Reset Generated By Core Double-Fault		*/
+#define RESET_WDOG			0x4000	/* SW Reset Generated By Watchdog Timer			*/
+#define RESET_SOFTWARE		0x8000	/* SW Reset Occurred Since Last Read Of SWRST	*/
+
+/* SYSCR Masks																				*/
+#define BMODE				0x0007	/* Boot Mode - Latched During HW Reset From Mode Pins	*/
+#define	NOBOOT				0x0010	/* Execute From L1 or ASYNC Bank 0 When BMODE = 0		*/
+
+
+/* *************  SYSTEM INTERRUPT CONTROLLER MASKS *************************************/
+/* Peripheral Masks For SIC_ISR, SIC_IWR, SIC_IMASK										*/
+
+#if 0
+#define IRQ_PLL_WAKEUP	0x00000001	/* PLL Wakeup Interrupt			 					*/
+
+#define IRQ_ERROR1      0x00000002  /* Error Interrupt (DMA, DMARx Block, DMARx Overflow) */
+#define IRQ_ERROR2      0x00000004  /* Error Interrupt (CAN, Ethernet, SPORTx, PPI, SPI, UARTx) */
+#define IRQ_RTC			0x00000008	/* Real Time Clock Interrupt 						*/
+#define IRQ_DMA0		0x00000010	/* DMA Channel 0 (PPI) Interrupt 					*/
+#define IRQ_DMA3		0x00000020	/* DMA Channel 3 (SPORT0 RX) Interrupt 				*/
+#define IRQ_DMA4		0x00000040	/* DMA Channel 4 (SPORT0 TX) Interrupt 				*/
+#define IRQ_DMA5		0x00000080	/* DMA Channel 5 (SPORT1 RX) Interrupt 				*/
+
+#define IRQ_DMA6		0x00000100	/* DMA Channel 6 (SPORT1 TX) Interrupt 		 		*/
+#define IRQ_TWI			0x00000200	/* TWI Interrupt									*/
+#define IRQ_DMA7		0x00000400	/* DMA Channel 7 (SPI) Interrupt 					*/
+#define IRQ_DMA8		0x00000800	/* DMA Channel 8 (UART0 RX) Interrupt 				*/
+#define IRQ_DMA9		0x00001000	/* DMA Channel 9 (UART0 TX) Interrupt 				*/
+#define IRQ_DMA10		0x00002000	/* DMA Channel 10 (UART1 RX) Interrupt 				*/
+#define IRQ_DMA11		0x00004000	/* DMA Channel 11 (UART1 TX) Interrupt 				*/
+#define IRQ_CAN_RX		0x00008000	/* CAN Receive Interrupt 							*/
+
+#define IRQ_CAN_TX		0x00010000	/* CAN Transmit Interrupt  							*/
+#define IRQ_DMA1		0x00020000	/* DMA Channel 1 (Ethernet RX) Interrupt 			*/
+#define IRQ_PFA_PORTH	0x00020000	/* PF Port H (PF47:32) Interrupt A 					*/
+#define IRQ_DMA2		0x00040000	/* DMA Channel 2 (Ethernet TX) Interrupt 			*/
+#define IRQ_PFB_PORTH	0x00040000	/* PF Port H (PF47:32) Interrupt B 					*/
+#define IRQ_TIMER0		0x00080000	/* Timer 0 Interrupt								*/
+#define IRQ_TIMER1		0x00100000	/* Timer 1 Interrupt 								*/
+#define IRQ_TIMER2		0x00200000	/* Timer 2 Interrupt 								*/
+#define IRQ_TIMER3		0x00400000	/* Timer 3 Interrupt 								*/
+#define IRQ_TIMER4		0x00800000	/* Timer 4 Interrupt 								*/
+
+#define IRQ_TIMER5		0x01000000	/* Timer 5 Interrupt 								*/
+#define IRQ_TIMER6		0x02000000	/* Timer 6 Interrupt 								*/
+#define IRQ_TIMER7		0x04000000	/* Timer 7 Interrupt 								*/
+#define IRQ_PFA_PORTFG	0x08000000	/* PF Ports F&G (PF31:0) Interrupt A 				*/
+#define IRQ_PFB_PORTF	0x80000000	/* PF Port F (PF15:0) Interrupt B 					*/
+#define IRQ_DMA12		0x20000000	/* DMA Channels 12 (MDMA1 Source) RX Interrupt 		*/
+#define IRQ_DMA13		0x20000000	/* DMA Channels 13 (MDMA1 Destination) TX Interrupt */
+#define IRQ_DMA14		0x40000000	/* DMA Channels 14 (MDMA0 Source) RX Interrupt 		*/
+#define IRQ_DMA15		0x40000000	/* DMA Channels 15 (MDMA0 Destination) TX Interrupt */
+#define IRQ_WDOG		0x80000000	/* Software Watchdog Timer Interrupt 				*/
+#define IRQ_PFB_PORTG	0x10000000	/* PF Port G (PF31:16) Interrupt B 					*/
+#endif
+
+/* SIC_IAR0 Macros															*/
+#define P0_IVG(x)		(((x)&0xF)-7)			/* Peripheral #0 assigned IVG #x 	*/
+#define P1_IVG(x)		(((x)&0xF)-7) << 0x4	/* Peripheral #1 assigned IVG #x 	*/
+#define P2_IVG(x)		(((x)&0xF)-7) << 0x8	/* Peripheral #2 assigned IVG #x 	*/
+#define P3_IVG(x)		(((x)&0xF)-7) << 0xC	/* Peripheral #3 assigned IVG #x	*/
+#define P4_IVG(x)		(((x)&0xF)-7) << 0x10	/* Peripheral #4 assigned IVG #x	*/
+#define P5_IVG(x)		(((x)&0xF)-7) << 0x14	/* Peripheral #5 assigned IVG #x	*/
+#define P6_IVG(x)		(((x)&0xF)-7) << 0x18	/* Peripheral #6 assigned IVG #x	*/
+#define P7_IVG(x)		(((x)&0xF)-7) << 0x1C	/* Peripheral #7 assigned IVG #x	*/
+
+/* SIC_IAR1 Macros															*/
+#define P8_IVG(x)		(((x)&0xF)-7)			/* Peripheral #8 assigned IVG #x 	*/
+#define P9_IVG(x)		(((x)&0xF)-7) << 0x4	/* Peripheral #9 assigned IVG #x 	*/
+#define P10_IVG(x)		(((x)&0xF)-7) << 0x8	/* Peripheral #10 assigned IVG #x	*/
+#define P11_IVG(x)		(((x)&0xF)-7) << 0xC	/* Peripheral #11 assigned IVG #x 	*/
+#define P12_IVG(x)		(((x)&0xF)-7) << 0x10	/* Peripheral #12 assigned IVG #x	*/
+#define P13_IVG(x)		(((x)&0xF)-7) << 0x14	/* Peripheral #13 assigned IVG #x	*/
+#define P14_IVG(x)		(((x)&0xF)-7) << 0x18	/* Peripheral #14 assigned IVG #x	*/
+#define P15_IVG(x)		(((x)&0xF)-7) << 0x1C	/* Peripheral #15 assigned IVG #x	*/
+
+/* SIC_IAR2 Macros															*/
+#define P16_IVG(x)		(((x)&0xF)-7)			/* Peripheral #16 assigned IVG #x	*/
+#define P17_IVG(x)		(((x)&0xF)-7) << 0x4	/* Peripheral #17 assigned IVG #x	*/
+#define P18_IVG(x)		(((x)&0xF)-7) << 0x8	/* Peripheral #18 assigned IVG #x	*/
+#define P19_IVG(x)		(((x)&0xF)-7) << 0xC	/* Peripheral #19 assigned IVG #x	*/
+#define P20_IVG(x)		(((x)&0xF)-7) << 0x10	/* Peripheral #20 assigned IVG #x	*/
+#define P21_IVG(x)		(((x)&0xF)-7) << 0x14	/* Peripheral #21 assigned IVG #x	*/
+#define P22_IVG(x)		(((x)&0xF)-7) << 0x18	/* Peripheral #22 assigned IVG #x	*/
+#define P23_IVG(x)		(((x)&0xF)-7) << 0x1C	/* Peripheral #23 assigned IVG #x	*/
+
+/* SIC_IAR3 Macros															*/
+#define P24_IVG(x)		(((x)&0xF)-7)			/* Peripheral #24 assigned IVG #x	*/
+#define P25_IVG(x)		(((x)&0xF)-7) << 0x4	/* Peripheral #25 assigned IVG #x	*/
+#define P26_IVG(x)		(((x)&0xF)-7) << 0x8	/* Peripheral #26 assigned IVG #x	*/
+#define P27_IVG(x)		(((x)&0xF)-7) << 0xC	/* Peripheral #27 assigned IVG #x	*/
+#define P28_IVG(x)		(((x)&0xF)-7) << 0x10	/* Peripheral #28 assigned IVG #x	*/
+#define P29_IVG(x)		(((x)&0xF)-7) << 0x14	/* Peripheral #29 assigned IVG #x	*/
+#define P30_IVG(x)		(((x)&0xF)-7) << 0x18	/* Peripheral #30 assigned IVG #x	*/
+#define P31_IVG(x)		(((x)&0xF)-7) << 0x1C	/* Peripheral #31 assigned IVG #x	*/
+
+
+/* SIC_IMASK Masks																		*/
+#define SIC_UNMASK_ALL	0x00000000					/* Unmask all peripheral interrupts	*/
+#define SIC_MASK_ALL	0xFFFFFFFF					/* Mask all peripheral interrupts	*/
+#define SIC_MASK(x)		(1 << ((x)&0x1F))					/* Mask Peripheral #x interrupt		*/
+#define SIC_UNMASK(x)	(0xFFFFFFFF ^ (1 << ((x)&0x1F)))	/* Unmask Peripheral #x interrupt	*/
+
+/* SIC_IWR Masks																		*/
+#define IWR_DISABLE_ALL	0x00000000					/* Wakeup Disable all peripherals	*/
+#define IWR_ENABLE_ALL	0xFFFFFFFF					/* Wakeup Enable all peripherals	*/
+#define IWR_ENABLE(x)	(1 << ((x)&0x1F))					/* Wakeup Enable Peripheral #x		*/
+#define IWR_DISABLE(x)	(0xFFFFFFFF ^ (1 << ((x)&0x1F))) 	/* Wakeup Disable Peripheral #x		*/
+
+
+/* ********* WATCHDOG TIMER MASKS ******************** */
+
+/* Watchdog Timer WDOG_CTL Register Masks */
+
+#define WDEV(x) (((x)<<1) & 0x0006) /* event generated on roll over */
+#define WDEV_RESET 0x0000 /* generate reset event on roll over */
+#define WDEV_NMI 0x0002 /* generate NMI event on roll over */
+#define WDEV_GPI 0x0004 /* generate GP IRQ on roll over */
+#define WDEV_NONE 0x0006 /* no event on roll over */
+#define WDEN 0x0FF0 /* enable watchdog */
+#define WDDIS 0x0AD0 /* disable watchdog */
+#define WDRO 0x8000 /* watchdog rolled over latch */
+
+/* depreciated WDOG_CTL Register Masks for legacy code */
+
+
+#define ICTL WDEV
+#define ENABLE_RESET WDEV_RESET
+#define WDOG_RESET WDEV_RESET
+#define ENABLE_NMI WDEV_NMI
+#define WDOG_NMI WDEV_NMI
+#define ENABLE_GPI WDEV_GPI
+#define WDOG_GPI WDEV_GPI
+#define DISABLE_EVT WDEV_NONE
+#define WDOG_NONE WDEV_NONE
+
+#define TMR_EN WDEN
+#define TMR_DIS WDDIS
+#define TRO WDRO
+#define ICTL_P0 0x01
+ #define ICTL_P1 0x02
+#define TRO_P 0x0F
+
+
+
+/* ***************  REAL TIME CLOCK MASKS  **************************/
+/* RTC_STAT and RTC_ALARM Masks										*/
+#define	RTC_SEC				0x0000003F	/* Real-Time Clock Seconds	*/
+#define	RTC_MIN				0x00000FC0	/* Real-Time Clock Minutes	*/
+#define	RTC_HR				0x0001F000	/* Real-Time Clock Hours	*/
+#define	RTC_DAY				0xFFFE0000	/* Real-Time Clock Days		*/
+
+/* RTC_ALARM Macro			z=day		y=hr	x=min	w=sec		*/
+#define SET_ALARM(z,y,x,w)	((((z)&0x7FFF)<<0x11)|(((y)&0x1F)<<0xC)|(((x)&0x3F)<<0x6)|((w)&0x3F))
+
+/* RTC_ICTL and RTC_ISTAT Masks																		*/
+#define	STOPWATCH			0x0001		/* Stopwatch Interrupt Enable								*/
+#define	ALARM				0x0002		/* Alarm Interrupt Enable									*/
+#define	SECOND				0x0004		/* Seconds (1 Hz) Interrupt Enable							*/
+#define	MINUTE				0x0008		/* Minutes Interrupt Enable									*/
+#define	HOUR				0x0010		/* Hours Interrupt Enable									*/
+#define	DAY					0x0020		/* 24 Hours (Days) Interrupt Enable							*/
+#define	DAY_ALARM			0x0040		/* Day Alarm (Day, Hour, Minute, Second) Interrupt Enable	*/
+#define	WRITE_PENDING		0x4000		/* Write Pending Status										*/
+#define	WRITE_COMPLETE		0x8000		/* Write Complete Interrupt Enable							*/
+
+/* RTC_FAST / RTC_PREN Mask												*/
+#define PREN				0x0001	/* Enable Prescaler, RTC Runs @1 Hz	*/
+
+
+/* ************** UART CONTROLLER MASKS *************************/
+/* UARTx_LCR Masks												*/
+#define WLS(x)		(((x)-5) & 0x03)	/* Word Length Select */
+#define STB			0x04				/* Stop Bits			*/
+#define PEN			0x08				/* Parity Enable		*/
+#define EPS			0x10				/* Even Parity Select	*/
+#define STP			0x20				/* Stick Parity			*/
+#define SB			0x40				/* Set Break			*/
+#define DLAB		0x80				/* Divisor Latch Access	*/
+
+/* UARTx_MCR Mask										*/
+#define LOOP_ENA	0x10	/* Loopback Mode Enable */
+#define LOOP_ENA_P	0x04
+
+/* UARTx_LSR Masks										*/
+#define DR			0x01	/* Data Ready				*/
+#define OE			0x02	/* Overrun Error			*/
+#define PE			0x04	/* Parity Error				*/
+#define FE			0x08	/* Framing Error			*/
+#define BI			0x10	/* Break Interrupt			*/
+#define THRE		0x20	/* THR Empty				*/
+#define TEMT		0x40	/* TSR and UART_THR Empty	*/
+
+/* UARTx_IER Masks															*/
+#define ERBFI		0x01		/* Enable Receive Buffer Full Interrupt		*/
+#define ETBEI		0x02		/* Enable Transmit Buffer Empty Interrupt	*/
+#define ELSI		0x04		/* Enable RX Status Interrupt				*/
+
+/* UARTx_IIR Masks														*/
+#define NINT		0x01		/* Pending Interrupt					*/
+#define IIR_TX_READY    0x02		/* UART_THR empty                               */
+#define IIR_RX_READY    0x04		/* Receive data ready                           */
+#define IIR_LINE_CHANGE 0x06		/* Receive line status    			*/
+#define IIR_STATUS	0x06		/* Highest Priority Pending Interrupt	*/
+
+/* UARTx_GCTL Masks													*/
+#define UCEN		0x01		/* Enable UARTx Clocks				*/
+#define IREN		0x02		/* Enable IrDA Mode					*/
+#define TPOLC		0x04		/* IrDA TX Polarity Change			*/
+#define RPOLC		0x08		/* IrDA RX Polarity Change			*/
+#define FPE			0x10		/* Force Parity Error On Transmit	*/
+#define FFE			0x20		/* Force Framing Error On Transmit	*/
+
+
+/* ***********  SERIAL PERIPHERAL INTERFACE (SPI) MASKS  ****************************/
+/* SPI_CTL Masks																	*/
+#define	TIMOD		0x0003		/* Transfer Initiate Mode							*/
+#define RDBR_CORE	0x0000		/* 		RDBR Read Initiates, IRQ When RDBR Full		*/
+#define	TDBR_CORE	0x0001		/* 		TDBR Write Initiates, IRQ When TDBR Empty	*/
+#define RDBR_DMA	0x0002		/* 		DMA Read, DMA Until FIFO Empty				*/
+#define TDBR_DMA	0x0003		/* 		DMA Write, DMA Until FIFO Full				*/
+#define SZ			0x0004		/* Send Zero (When TDBR Empty, Send Zero/Last*)		*/
+#define GM			0x0008		/* Get More (When RDBR Full, Overwrite/Discard*)	*/
+#define PSSE		0x0010		/* Slave-Select Input Enable						*/
+#define EMISO		0x0020		/* Enable MISO As Output							*/
+#define SIZE		0x0100		/* Size of Words (16/8* Bits)						*/
+#define LSBF		0x0200		/* LSB First										*/
+#define CPHA		0x0400		/* Clock Phase										*/
+#define CPOL		0x0800		/* Clock Polarity									*/
+#define MSTR		0x1000		/* Master/Slave*									*/
+#define WOM			0x2000		/* Write Open Drain Master							*/
+#define SPE			0x4000		/* SPI Enable										*/
+
+/* SPI_FLG Masks																	*/
+#define FLS1		0x0002		/* Enables SPI_FLOUT1 as SPI Slave-Select Output	*/
+#define FLS2		0x0004		/* Enables SPI_FLOUT2 as SPI Slave-Select Output	*/
+#define FLS3		0x0008		/* Enables SPI_FLOUT3 as SPI Slave-Select Output	*/
+#define FLS4		0x0010		/* Enables SPI_FLOUT4 as SPI Slave-Select Output	*/
+#define FLS5		0x0020		/* Enables SPI_FLOUT5 as SPI Slave-Select Output	*/
+#define FLS6		0x0040		/* Enables SPI_FLOUT6 as SPI Slave-Select Output	*/
+#define FLS7		0x0080		/* Enables SPI_FLOUT7 as SPI Slave-Select Output	*/
+#define FLG1		0xFDFF		/* Activates SPI_FLOUT1 							*/
+#define FLG2		0xFBFF		/* Activates SPI_FLOUT2								*/
+#define FLG3		0xF7FF		/* Activates SPI_FLOUT3								*/
+#define FLG4		0xEFFF		/* Activates SPI_FLOUT4								*/
+#define FLG5		0xDFFF		/* Activates SPI_FLOUT5								*/
+#define FLG6		0xBFFF		/* Activates SPI_FLOUT6								*/
+#define FLG7		0x7FFF		/* Activates SPI_FLOUT7								*/
+
+/* SPI_STAT Masks																				*/
+#define SPIF		0x0001		/* SPI Finished (Single-Word Transfer Complete)					*/
+#define MODF		0x0002		/* Mode Fault Error (Another Device Tried To Become Master)		*/
+#define TXE			0x0004		/* Transmission Error (Data Sent With No New Data In TDBR)		*/
+#define TXS			0x0008		/* SPI_TDBR Data Buffer Status (Full/Empty*)					*/
+#define RBSY		0x0010		/* Receive Error (Data Received With RDBR Full)					*/
+#define RXS			0x0020		/* SPI_RDBR Data Buffer Status (Full/Empty*)					*/
+#define TXCOL		0x0040		/* Transmit Collision Error (Corrupt Data May Have Been Sent)	*/
+
+
+/*  ****************  GENERAL PURPOSE TIMER MASKS  **********************/
+/* TIMER_ENABLE Masks													*/
+#define TIMEN0			0x0001		/* Enable Timer 0					*/
+#define TIMEN1			0x0002		/* Enable Timer 1					*/
+#define TIMEN2			0x0004		/* Enable Timer 2					*/
+#define TIMEN3			0x0008		/* Enable Timer 3					*/
+#define TIMEN4			0x0010		/* Enable Timer 4					*/
+#define TIMEN5			0x0020		/* Enable Timer 5					*/
+#define TIMEN6			0x0040		/* Enable Timer 6					*/
+#define TIMEN7			0x0080		/* Enable Timer 7					*/
+
+/* TIMER_DISABLE Masks													*/
+#define TIMDIS0			TIMEN0		/* Disable Timer 0					*/
+#define TIMDIS1			TIMEN1		/* Disable Timer 1					*/
+#define TIMDIS2			TIMEN2		/* Disable Timer 2					*/
+#define TIMDIS3			TIMEN3		/* Disable Timer 3					*/
+#define TIMDIS4			TIMEN4		/* Disable Timer 4					*/
+#define TIMDIS5			TIMEN5		/* Disable Timer 5					*/
+#define TIMDIS6			TIMEN6		/* Disable Timer 6					*/
+#define TIMDIS7			TIMEN7		/* Disable Timer 7					*/
+
+/* TIMER_STATUS Masks													*/
+#define TIMIL0			0x00000001	/* Timer 0 Interrupt				*/
+#define TIMIL1			0x00000002	/* Timer 1 Interrupt				*/
+#define TIMIL2			0x00000004	/* Timer 2 Interrupt				*/
+#define TIMIL3			0x00000008	/* Timer 3 Interrupt				*/
+#define TOVF_ERR0		0x00000010	/* Timer 0 Counter Overflow			*/
+#define TOVF_ERR1		0x00000020	/* Timer 1 Counter Overflow			*/
+#define TOVF_ERR2		0x00000040	/* Timer 2 Counter Overflow			*/
+#define TOVF_ERR3		0x00000080	/* Timer 3 Counter Overflow			*/
+#define TRUN0			0x00001000	/* Timer 0 Slave Enable Status		*/
+#define TRUN1			0x00002000	/* Timer 1 Slave Enable Status		*/
+#define TRUN2			0x00004000	/* Timer 2 Slave Enable Status		*/
+#define TRUN3			0x00008000	/* Timer 3 Slave Enable Status		*/
+#define TIMIL4			0x00010000	/* Timer 4 Interrupt				*/
+#define TIMIL5			0x00020000	/* Timer 5 Interrupt				*/
+#define TIMIL6			0x00040000	/* Timer 6 Interrupt				*/
+#define TIMIL7			0x00080000	/* Timer 7 Interrupt				*/
+#define TOVF_ERR4		0x00100000	/* Timer 4 Counter Overflow			*/
+#define TOVF_ERR5		0x00200000	/* Timer 5 Counter Overflow			*/
+#define TOVF_ERR6		0x00400000	/* Timer 6 Counter Overflow			*/
+#define TOVF_ERR7		0x00800000	/* Timer 7 Counter Overflow			*/
+#define TRUN4			0x10000000	/* Timer 4 Slave Enable Status		*/
+#define TRUN5			0x20000000	/* Timer 5 Slave Enable Status		*/
+#define TRUN6			0x40000000	/* Timer 6 Slave Enable Status		*/
+#define TRUN7			0x80000000	/* Timer 7 Slave Enable Status		*/
+
+/* Alternate Deprecated Macros Provided For Backwards Code Compatibility */
+#define TOVL_ERR0 TOVF_ERR0
+#define TOVL_ERR1 TOVF_ERR1
+#define TOVL_ERR2 TOVF_ERR2
+#define TOVL_ERR3 TOVF_ERR3
+#define TOVL_ERR4 TOVF_ERR4
+#define TOVL_ERR5 TOVF_ERR5
+#define TOVL_ERR6 TOVF_ERR6
+#define TOVL_ERR7 TOVF_ERR7
+
+/* TIMERx_CONFIG Masks													*/
+#define PWM_OUT			0x0001	/* Pulse-Width Modulation Output Mode	*/
+#define WDTH_CAP		0x0002	/* Width Capture Input Mode				*/
+#define EXT_CLK			0x0003	/* External Clock Mode					*/
+#define PULSE_HI		0x0004	/* Action Pulse (Positive/Negative*)	*/
+#define PERIOD_CNT		0x0008	/* Period Count							*/
+#define IRQ_ENA			0x0010	/* Interrupt Request Enable				*/
+#define TIN_SEL			0x0020	/* Timer Input Select					*/
+#define OUT_DIS			0x0040	/* Output Pad Disable					*/
+#define CLK_SEL			0x0080	/* Timer Clock Select					*/
+#define TOGGLE_HI		0x0100	/* PWM_OUT PULSE_HI Toggle Mode			*/
+#define EMU_RUN			0x0200	/* Emulation Behavior Select			*/
+#define ERR_TYP			0xC000	/* Error Type							*/
+
+
+/* ******************   GPIO PORTS F, G, H MASKS  ***********************/
+/*  General Purpose IO (0xFFC00700 - 0xFFC007FF)  Masks 				*/
+/* Port F Masks 														*/
+#define PF0		0x0001
+#define PF1		0x0002
+#define PF2		0x0004
+#define PF3		0x0008
+#define PF4		0x0010
+#define PF5		0x0020
+#define PF6		0x0040
+#define PF7		0x0080
+#define PF8		0x0100
+#define PF9		0x0200
+#define PF10	0x0400
+#define PF11	0x0800
+#define PF12	0x1000
+#define PF13	0x2000
+#define PF14	0x4000
+#define PF15	0x8000
+
+/* Port G Masks															*/
+#define PG0		0x0001
+#define PG1		0x0002
+#define PG2		0x0004
+#define PG3		0x0008
+#define PG4		0x0010
+#define PG5		0x0020
+#define PG6		0x0040
+#define PG7		0x0080
+#define PG8		0x0100
+#define PG9		0x0200
+#define PG10	0x0400
+#define PG11	0x0800
+#define PG12	0x1000
+#define PG13	0x2000
+#define PG14	0x4000
+#define PG15	0x8000
+
+/* Port H Masks															*/
+#define PH0		0x0001
+#define PH1		0x0002
+#define PH2		0x0004
+#define PH3		0x0008
+#define PH4		0x0010
+#define PH5		0x0020
+#define PH6		0x0040
+#define PH7		0x0080
+
+
+/* *******************  SERIAL PORT MASKS  **************************************/
+/* SPORTx_TCR1 Masks															*/
+#define TSPEN		0x0001		/* Transmit Enable								*/
+#define ITCLK		0x0002		/* Internal Transmit Clock Select				*/
+#define DTYPE_NORM	0x0004		/* Data Format Normal							*/
+#define DTYPE_ULAW	0x0008		/* Compand Using u-Law							*/
+#define DTYPE_ALAW	0x000C		/* Compand Using A-Law							*/
+#define TLSBIT		0x0010		/* Transmit Bit Order							*/
+#define ITFS		0x0200		/* Internal Transmit Frame Sync Select			*/
+#define TFSR		0x0400		/* Transmit Frame Sync Required Select			*/
+#define DITFS		0x0800		/* Data-Independent Transmit Frame Sync Select	*/
+#define LTFS		0x1000		/* Low Transmit Frame Sync Select				*/
+#define LATFS		0x2000		/* Late Transmit Frame Sync Select				*/
+#define TCKFE		0x4000		/* Clock Falling Edge Select					*/
+
+/* SPORTx_TCR2 Masks and Macro													*/
+#define SLEN(x)		((x)&0x1F)	/* SPORT TX Word Length (2 - 31)				*/
+#define TXSE		0x0100		/* TX Secondary Enable							*/
+#define TSFSE		0x0200		/* Transmit Stereo Frame Sync Enable			*/
+#define TRFST		0x0400		/* Left/Right Order (1 = Right Channel 1st)		*/
+
+/* SPORTx_RCR1 Masks															*/
+#define RSPEN		0x0001		/* Receive Enable 								*/
+#define IRCLK		0x0002		/* Internal Receive Clock Select 				*/
+#define DTYPE_NORM	0x0004		/* Data Format Normal							*/
+#define DTYPE_ULAW	0x0008		/* Compand Using u-Law							*/
+#define DTYPE_ALAW	0x000C		/* Compand Using A-Law							*/
+#define RLSBIT		0x0010		/* Receive Bit Order							*/
+#define IRFS		0x0200		/* Internal Receive Frame Sync Select 			*/
+#define RFSR		0x0400		/* Receive Frame Sync Required Select 			*/
+#define LRFS		0x1000		/* Low Receive Frame Sync Select 				*/
+#define LARFS		0x2000		/* Late Receive Frame Sync Select 				*/
+#define RCKFE		0x4000		/* Clock Falling Edge Select 					*/
+
+/* SPORTx_RCR2 Masks															*/
+#define SLEN(x)		((x)&0x1F)	/* SPORT RX Word Length (2 - 31)				*/
+#define RXSE		0x0100		/* RX Secondary Enable							*/
+#define RSFSE		0x0200		/* RX Stereo Frame Sync Enable					*/
+#define RRFST		0x0400		/* Right-First Data Order 						*/
+
+/* SPORTx_STAT Masks															*/
+#define RXNE		0x0001		/* Receive FIFO Not Empty Status				*/
+#define RUVF		0x0002		/* Sticky Receive Underflow Status				*/
+#define ROVF		0x0004		/* Sticky Receive Overflow Status				*/
+#define TXF			0x0008		/* Transmit FIFO Full Status					*/
+#define TUVF		0x0010		/* Sticky Transmit Underflow Status				*/
+#define TOVF		0x0020		/* Sticky Transmit Overflow Status				*/
+#define TXHRE		0x0040		/* Transmit Hold Register Empty					*/
+
+/* SPORTx_MCMC1 Macros															*/
+#define SP_WOFF(x)	((x) & 0x3FF) 	/* Multichannel Window Offset Field			*/
+
+/* Only use WSIZE Macro With Logic OR While Setting Lower Order Bits						*/
+#define SP_WSIZE(x)	(((((x)>>0x3)-1)&0xF) << 0xC)	/* Multichannel Window Size = (x/8)-1	*/
+
+/* SPORTx_MCMC2 Masks															*/
+#define REC_BYPASS	0x0000		/* Bypass Mode (No Clock Recovery)				*/
+#define REC_2FROM4	0x0002		/* Recover 2 MHz Clock from 4 MHz Clock			*/
+#define REC_8FROM16	0x0003		/* Recover 8 MHz Clock from 16 MHz Clock		*/
+#define MCDTXPE		0x0004 		/* Multichannel DMA Transmit Packing			*/
+#define MCDRXPE		0x0008 		/* Multichannel DMA Receive Packing				*/
+#define MCMEN		0x0010 		/* Multichannel Frame Mode Enable				*/
+#define FSDR		0x0080 		/* Multichannel Frame Sync to Data Relationship	*/
+#define MFD_0		0x0000		/* Multichannel Frame Delay = 0					*/
+#define MFD_1		0x1000		/* Multichannel Frame Delay = 1					*/
+#define MFD_2		0x2000		/* Multichannel Frame Delay = 2					*/
+#define MFD_3		0x3000		/* Multichannel Frame Delay = 3					*/
+#define MFD_4		0x4000		/* Multichannel Frame Delay = 4					*/
+#define MFD_5		0x5000		/* Multichannel Frame Delay = 5					*/
+#define MFD_6		0x6000		/* Multichannel Frame Delay = 6					*/
+#define MFD_7		0x7000		/* Multichannel Frame Delay = 7					*/
+#define MFD_8		0x8000		/* Multichannel Frame Delay = 8					*/
+#define MFD_9		0x9000		/* Multichannel Frame Delay = 9					*/
+#define MFD_10		0xA000		/* Multichannel Frame Delay = 10				*/
+#define MFD_11		0xB000		/* Multichannel Frame Delay = 11				*/
+#define MFD_12		0xC000		/* Multichannel Frame Delay = 12				*/
+#define MFD_13		0xD000		/* Multichannel Frame Delay = 13				*/
+#define MFD_14		0xE000		/* Multichannel Frame Delay = 14				*/
+#define MFD_15		0xF000		/* Multichannel Frame Delay = 15				*/
+
+
+/* *********************  ASYNCHRONOUS MEMORY CONTROLLER MASKS  *************************/
+/* EBIU_AMGCTL Masks																	*/
+#define AMCKEN			0x0001		/* Enable CLKOUT									*/
+#define	AMBEN_NONE		0x0000		/* All Banks Disabled								*/
+#define AMBEN_B0		0x0002		/* Enable Async Memory Bank 0 only					*/
+#define AMBEN_B0_B1		0x0004		/* Enable Async Memory Banks 0 & 1 only				*/
+#define AMBEN_B0_B1_B2	0x0006		/* Enable Async Memory Banks 0, 1, and 2			*/
+#define AMBEN_ALL		0x0008		/* Enable Async Memory Banks (all) 0, 1, 2, and 3	*/
+
+/* EBIU_AMBCTL0 Masks																	*/
+#define B0RDYEN			0x00000001  /* Bank 0 (B0) RDY Enable							*/
+#define B0RDYPOL		0x00000002  /* B0 RDY Active High								*/
+#define B0TT_1			0x00000004  /* B0 Transition Time (Read to Write) = 1 cycle		*/
+#define B0TT_2			0x00000008  /* B0 Transition Time (Read to Write) = 2 cycles	*/
+#define B0TT_3			0x0000000C  /* B0 Transition Time (Read to Write) = 3 cycles	*/
+#define B0TT_4			0x00000000  /* B0 Transition Time (Read to Write) = 4 cycles	*/
+#define B0ST_1			0x00000010  /* B0 Setup Time (AOE to Read/Write) = 1 cycle		*/
+#define B0ST_2			0x00000020  /* B0 Setup Time (AOE to Read/Write) = 2 cycles		*/
+#define B0ST_3			0x00000030  /* B0 Setup Time (AOE to Read/Write) = 3 cycles		*/
+#define B0ST_4			0x00000000  /* B0 Setup Time (AOE to Read/Write) = 4 cycles		*/
+#define B0HT_1			0x00000040  /* B0 Hold Time (~Read/Write to ~AOE) = 1 cycle		*/
+#define B0HT_2			0x00000080  /* B0 Hold Time (~Read/Write to ~AOE) = 2 cycles	*/
+#define B0HT_3			0x000000C0  /* B0 Hold Time (~Read/Write to ~AOE) = 3 cycles	*/
+#define B0HT_0			0x00000000  /* B0 Hold Time (~Read/Write to ~AOE) = 0 cycles	*/
+#define B0RAT_1			0x00000100  /* B0 Read Access Time = 1 cycle					*/
+#define B0RAT_2			0x00000200  /* B0 Read Access Time = 2 cycles					*/
+#define B0RAT_3			0x00000300  /* B0 Read Access Time = 3 cycles					*/
+#define B0RAT_4			0x00000400  /* B0 Read Access Time = 4 cycles					*/
+#define B0RAT_5			0x00000500  /* B0 Read Access Time = 5 cycles					*/
+#define B0RAT_6			0x00000600  /* B0 Read Access Time = 6 cycles					*/
+#define B0RAT_7			0x00000700  /* B0 Read Access Time = 7 cycles					*/
+#define B0RAT_8			0x00000800  /* B0 Read Access Time = 8 cycles					*/
+#define B0RAT_9			0x00000900  /* B0 Read Access Time = 9 cycles					*/
+#define B0RAT_10		0x00000A00  /* B0 Read Access Time = 10 cycles					*/
+#define B0RAT_11		0x00000B00  /* B0 Read Access Time = 11 cycles					*/
+#define B0RAT_12		0x00000C00  /* B0 Read Access Time = 12 cycles					*/
+#define B0RAT_13		0x00000D00  /* B0 Read Access Time = 13 cycles					*/
+#define B0RAT_14		0x00000E00  /* B0 Read Access Time = 14 cycles					*/
+#define B0RAT_15		0x00000F00  /* B0 Read Access Time = 15 cycles					*/
+#define B0WAT_1			0x00001000  /* B0 Write Access Time = 1 cycle					*/
+#define B0WAT_2			0x00002000  /* B0 Write Access Time = 2 cycles					*/
+#define B0WAT_3			0x00003000  /* B0 Write Access Time = 3 cycles					*/
+#define B0WAT_4			0x00004000  /* B0 Write Access Time = 4 cycles					*/
+#define B0WAT_5			0x00005000  /* B0 Write Access Time = 5 cycles					*/
+#define B0WAT_6			0x00006000  /* B0 Write Access Time = 6 cycles					*/
+#define B0WAT_7			0x00007000  /* B0 Write Access Time = 7 cycles					*/
+#define B0WAT_8			0x00008000  /* B0 Write Access Time = 8 cycles					*/
+#define B0WAT_9			0x00009000  /* B0 Write Access Time = 9 cycles					*/
+#define B0WAT_10		0x0000A000  /* B0 Write Access Time = 10 cycles					*/
+#define B0WAT_11		0x0000B000  /* B0 Write Access Time = 11 cycles					*/
+#define B0WAT_12		0x0000C000  /* B0 Write Access Time = 12 cycles					*/
+#define B0WAT_13		0x0000D000  /* B0 Write Access Time = 13 cycles					*/
+#define B0WAT_14		0x0000E000  /* B0 Write Access Time = 14 cycles					*/
+#define B0WAT_15		0x0000F000  /* B0 Write Access Time = 15 cycles					*/
+
+#define B1RDYEN			0x00010000  /* Bank 1 (B1) RDY Enable                       	*/
+#define B1RDYPOL		0x00020000  /* B1 RDY Active High                           	*/
+#define B1TT_1			0x00040000  /* B1 Transition Time (Read to Write) = 1 cycle 	*/
+#define B1TT_2			0x00080000  /* B1 Transition Time (Read to Write) = 2 cycles	*/
+#define B1TT_3			0x000C0000  /* B1 Transition Time (Read to Write) = 3 cycles	*/
+#define B1TT_4			0x00000000  /* B1 Transition Time (Read to Write) = 4 cycles	*/
+#define B1ST_1			0x00100000  /* B1 Setup Time (AOE to Read/Write) = 1 cycle  	*/
+#define B1ST_2			0x00200000  /* B1 Setup Time (AOE to Read/Write) = 2 cycles 	*/
+#define B1ST_3			0x00300000  /* B1 Setup Time (AOE to Read/Write) = 3 cycles 	*/
+#define B1ST_4			0x00000000  /* B1 Setup Time (AOE to Read/Write) = 4 cycles 	*/
+#define B1HT_1			0x00400000  /* B1 Hold Time (~Read/Write to ~AOE) = 1 cycle 	*/
+#define B1HT_2			0x00800000  /* B1 Hold Time (~Read/Write to ~AOE) = 2 cycles	*/
+#define B1HT_3			0x00C00000  /* B1 Hold Time (~Read/Write to ~AOE) = 3 cycles	*/
+#define B1HT_0			0x00000000  /* B1 Hold Time (~Read/Write to ~AOE) = 0 cycles	*/
+#define B1RAT_1			0x01000000  /* B1 Read Access Time = 1 cycle					*/
+#define B1RAT_2			0x02000000  /* B1 Read Access Time = 2 cycles					*/
+#define B1RAT_3			0x03000000  /* B1 Read Access Time = 3 cycles					*/
+#define B1RAT_4			0x04000000  /* B1 Read Access Time = 4 cycles					*/
+#define B1RAT_5			0x05000000  /* B1 Read Access Time = 5 cycles					*/
+#define B1RAT_6			0x06000000  /* B1 Read Access Time = 6 cycles					*/
+#define B1RAT_7			0x07000000  /* B1 Read Access Time = 7 cycles					*/
+#define B1RAT_8			0x08000000  /* B1 Read Access Time = 8 cycles					*/
+#define B1RAT_9			0x09000000  /* B1 Read Access Time = 9 cycles					*/
+#define B1RAT_10		0x0A000000  /* B1 Read Access Time = 10 cycles					*/
+#define B1RAT_11		0x0B000000  /* B1 Read Access Time = 11 cycles					*/
+#define B1RAT_12		0x0C000000  /* B1 Read Access Time = 12 cycles					*/
+#define B1RAT_13		0x0D000000  /* B1 Read Access Time = 13 cycles					*/
+#define B1RAT_14		0x0E000000  /* B1 Read Access Time = 14 cycles					*/
+#define B1RAT_15		0x0F000000  /* B1 Read Access Time = 15 cycles					*/
+#define B1WAT_1			0x10000000  /* B1 Write Access Time = 1 cycle					*/
+#define B1WAT_2			0x20000000  /* B1 Write Access Time = 2 cycles					*/
+#define B1WAT_3			0x30000000  /* B1 Write Access Time = 3 cycles					*/
+#define B1WAT_4			0x40000000  /* B1 Write Access Time = 4 cycles					*/
+#define B1WAT_5			0x50000000  /* B1 Write Access Time = 5 cycles					*/
+#define B1WAT_6			0x60000000  /* B1 Write Access Time = 6 cycles					*/
+#define B1WAT_7			0x70000000  /* B1 Write Access Time = 7 cycles					*/
+#define B1WAT_8			0x80000000  /* B1 Write Access Time = 8 cycles					*/
+#define B1WAT_9			0x90000000  /* B1 Write Access Time = 9 cycles					*/
+#define B1WAT_10		0xA0000000  /* B1 Write Access Time = 10 cycles					*/
+#define B1WAT_11		0xB0000000  /* B1 Write Access Time = 11 cycles					*/
+#define B1WAT_12		0xC0000000  /* B1 Write Access Time = 12 cycles					*/
+#define B1WAT_13		0xD0000000  /* B1 Write Access Time = 13 cycles					*/
+#define B1WAT_14		0xE0000000  /* B1 Write Access Time = 14 cycles					*/
+#define B1WAT_15		0xF0000000  /* B1 Write Access Time = 15 cycles					*/
+
+/* EBIU_AMBCTL1 Masks																	*/
+#define B2RDYEN			0x00000001  /* Bank 2 (B2) RDY Enable							*/
+#define B2RDYPOL		0x00000002  /* B2 RDY Active High								*/
+#define B2TT_1			0x00000004  /* B2 Transition Time (Read to Write) = 1 cycle		*/
+#define B2TT_2			0x00000008  /* B2 Transition Time (Read to Write) = 2 cycles	*/
+#define B2TT_3			0x0000000C  /* B2 Transition Time (Read to Write) = 3 cycles	*/
+#define B2TT_4			0x00000000  /* B2 Transition Time (Read to Write) = 4 cycles	*/
+#define B2ST_1			0x00000010  /* B2 Setup Time (AOE to Read/Write) = 1 cycle		*/
+#define B2ST_2			0x00000020  /* B2 Setup Time (AOE to Read/Write) = 2 cycles		*/
+#define B2ST_3			0x00000030  /* B2 Setup Time (AOE to Read/Write) = 3 cycles		*/
+#define B2ST_4			0x00000000  /* B2 Setup Time (AOE to Read/Write) = 4 cycles		*/
+#define B2HT_1			0x00000040  /* B2 Hold Time (~Read/Write to ~AOE) = 1 cycle		*/
+#define B2HT_2			0x00000080  /* B2 Hold Time (~Read/Write to ~AOE) = 2 cycles	*/
+#define B2HT_3			0x000000C0  /* B2 Hold Time (~Read/Write to ~AOE) = 3 cycles	*/
+#define B2HT_0			0x00000000  /* B2 Hold Time (~Read/Write to ~AOE) = 0 cycles	*/
+#define B2RAT_1			0x00000100  /* B2 Read Access Time = 1 cycle					*/
+#define B2RAT_2			0x00000200  /* B2 Read Access Time = 2 cycles					*/
+#define B2RAT_3			0x00000300  /* B2 Read Access Time = 3 cycles					*/
+#define B2RAT_4			0x00000400  /* B2 Read Access Time = 4 cycles					*/
+#define B2RAT_5			0x00000500  /* B2 Read Access Time = 5 cycles					*/
+#define B2RAT_6			0x00000600  /* B2 Read Access Time = 6 cycles					*/
+#define B2RAT_7			0x00000700  /* B2 Read Access Time = 7 cycles					*/
+#define B2RAT_8			0x00000800  /* B2 Read Access Time = 8 cycles					*/
+#define B2RAT_9			0x00000900  /* B2 Read Access Time = 9 cycles					*/
+#define B2RAT_10		0x00000A00  /* B2 Read Access Time = 10 cycles					*/
+#define B2RAT_11		0x00000B00  /* B2 Read Access Time = 11 cycles					*/
+#define B2RAT_12		0x00000C00  /* B2 Read Access Time = 12 cycles					*/
+#define B2RAT_13		0x00000D00  /* B2 Read Access Time = 13 cycles					*/
+#define B2RAT_14		0x00000E00  /* B2 Read Access Time = 14 cycles					*/
+#define B2RAT_15		0x00000F00  /* B2 Read Access Time = 15 cycles					*/
+#define B2WAT_1			0x00001000  /* B2 Write Access Time = 1 cycle					*/
+#define B2WAT_2			0x00002000  /* B2 Write Access Time = 2 cycles					*/
+#define B2WAT_3			0x00003000  /* B2 Write Access Time = 3 cycles					*/
+#define B2WAT_4			0x00004000  /* B2 Write Access Time = 4 cycles					*/
+#define B2WAT_5			0x00005000  /* B2 Write Access Time = 5 cycles					*/
+#define B2WAT_6			0x00006000  /* B2 Write Access Time = 6 cycles					*/
+#define B2WAT_7			0x00007000  /* B2 Write Access Time = 7 cycles					*/
+#define B2WAT_8			0x00008000  /* B2 Write Access Time = 8 cycles					*/
+#define B2WAT_9			0x00009000  /* B2 Write Access Time = 9 cycles					*/
+#define B2WAT_10		0x0000A000  /* B2 Write Access Time = 10 cycles					*/
+#define B2WAT_11		0x0000B000  /* B2 Write Access Time = 11 cycles					*/
+#define B2WAT_12		0x0000C000  /* B2 Write Access Time = 12 cycles					*/
+#define B2WAT_13		0x0000D000  /* B2 Write Access Time = 13 cycles					*/
+#define B2WAT_14		0x0000E000  /* B2 Write Access Time = 14 cycles					*/
+#define B2WAT_15		0x0000F000  /* B2 Write Access Time = 15 cycles					*/
+
+#define B3RDYEN			0x00010000  /* Bank 3 (B3) RDY Enable							*/
+#define B3RDYPOL		0x00020000  /* B3 RDY Active High								*/
+#define B3TT_1			0x00040000  /* B3 Transition Time (Read to Write) = 1 cycle		*/
+#define B3TT_2			0x00080000  /* B3 Transition Time (Read to Write) = 2 cycles	*/
+#define B3TT_3			0x000C0000  /* B3 Transition Time (Read to Write) = 3 cycles	*/
+#define B3TT_4			0x00000000  /* B3 Transition Time (Read to Write) = 4 cycles	*/
+#define B3ST_1			0x00100000  /* B3 Setup Time (AOE to Read/Write) = 1 cycle		*/
+#define B3ST_2			0x00200000  /* B3 Setup Time (AOE to Read/Write) = 2 cycles		*/
+#define B3ST_3			0x00300000  /* B3 Setup Time (AOE to Read/Write) = 3 cycles		*/
+#define B3ST_4			0x00000000  /* B3 Setup Time (AOE to Read/Write) = 4 cycles		*/
+#define B3HT_1			0x00400000  /* B3 Hold Time (~Read/Write to ~AOE) = 1 cycle		*/
+#define B3HT_2			0x00800000  /* B3 Hold Time (~Read/Write to ~AOE) = 2 cycles	*/
+#define B3HT_3			0x00C00000  /* B3 Hold Time (~Read/Write to ~AOE) = 3 cycles	*/
+#define B3HT_0			0x00000000  /* B3 Hold Time (~Read/Write to ~AOE) = 0 cycles	*/
+#define B3RAT_1			0x01000000  /* B3 Read Access Time = 1 cycle					*/
+#define B3RAT_2			0x02000000  /* B3 Read Access Time = 2 cycles					*/
+#define B3RAT_3			0x03000000  /* B3 Read Access Time = 3 cycles					*/
+#define B3RAT_4			0x04000000  /* B3 Read Access Time = 4 cycles					*/
+#define B3RAT_5			0x05000000  /* B3 Read Access Time = 5 cycles					*/
+#define B3RAT_6			0x06000000  /* B3 Read Access Time = 6 cycles					*/
+#define B3RAT_7			0x07000000  /* B3 Read Access Time = 7 cycles					*/
+#define B3RAT_8			0x08000000  /* B3 Read Access Time = 8 cycles					*/
+#define B3RAT_9			0x09000000  /* B3 Read Access Time = 9 cycles					*/
+#define B3RAT_10		0x0A000000  /* B3 Read Access Time = 10 cycles					*/
+#define B3RAT_11		0x0B000000  /* B3 Read Access Time = 11 cycles					*/
+#define B3RAT_12		0x0C000000  /* B3 Read Access Time = 12 cycles					*/
+#define B3RAT_13		0x0D000000  /* B3 Read Access Time = 13 cycles					*/
+#define B3RAT_14		0x0E000000  /* B3 Read Access Time = 14 cycles					*/
+#define B3RAT_15		0x0F000000  /* B3 Read Access Time = 15 cycles					*/
+#define B3WAT_1			0x10000000  /* B3 Write Access Time = 1 cycle					*/
+#define B3WAT_2			0x20000000  /* B3 Write Access Time = 2 cycles					*/
+#define B3WAT_3			0x30000000  /* B3 Write Access Time = 3 cycles					*/
+#define B3WAT_4			0x40000000  /* B3 Write Access Time = 4 cycles					*/
+#define B3WAT_5			0x50000000  /* B3 Write Access Time = 5 cycles					*/
+#define B3WAT_6			0x60000000  /* B3 Write Access Time = 6 cycles					*/
+#define B3WAT_7			0x70000000  /* B3 Write Access Time = 7 cycles					*/
+#define B3WAT_8			0x80000000  /* B3 Write Access Time = 8 cycles					*/
+#define B3WAT_9			0x90000000  /* B3 Write Access Time = 9 cycles					*/
+#define B3WAT_10		0xA0000000  /* B3 Write Access Time = 10 cycles					*/
+#define B3WAT_11		0xB0000000  /* B3 Write Access Time = 11 cycles					*/
+#define B3WAT_12		0xC0000000  /* B3 Write Access Time = 12 cycles					*/
+#define B3WAT_13		0xD0000000  /* B3 Write Access Time = 13 cycles					*/
+#define B3WAT_14		0xE0000000  /* B3 Write Access Time = 14 cycles					*/
+#define B3WAT_15		0xF0000000  /* B3 Write Access Time = 15 cycles					*/
+
+
+/* **********************  SDRAM CONTROLLER MASKS  **********************************************/
+/* EBIU_SDGCTL Masks																			*/
+#define SCTLE			0x00000001	/* Enable SDRAM Signals										*/
+#define CL_2			0x00000008	/* SDRAM CAS Latency = 2 cycles								*/
+#define CL_3			0x0000000C	/* SDRAM CAS Latency = 3 cycles								*/
+#define PASR_ALL		0x00000000	/* All 4 SDRAM Banks Refreshed In Self-Refresh				*/
+#define PASR_B0_B1		0x00000010	/* SDRAM Banks 0 and 1 Are Refreshed In Self-Refresh		*/
+#define PASR_B0			0x00000020	/* Only SDRAM Bank 0 Is Refreshed In Self-Refresh			*/
+#define TRAS_1			0x00000040	/* SDRAM tRAS = 1 cycle										*/
+#define TRAS_2			0x00000080	/* SDRAM tRAS = 2 cycles									*/
+#define TRAS_3			0x000000C0	/* SDRAM tRAS = 3 cycles									*/
+#define TRAS_4			0x00000100	/* SDRAM tRAS = 4 cycles									*/
+#define TRAS_5			0x00000140	/* SDRAM tRAS = 5 cycles									*/
+#define TRAS_6			0x00000180	/* SDRAM tRAS = 6 cycles									*/
+#define TRAS_7			0x000001C0	/* SDRAM tRAS = 7 cycles									*/
+#define TRAS_8			0x00000200	/* SDRAM tRAS = 8 cycles									*/
+#define TRAS_9			0x00000240	/* SDRAM tRAS = 9 cycles									*/
+#define TRAS_10			0x00000280	/* SDRAM tRAS = 10 cycles									*/
+#define TRAS_11			0x000002C0	/* SDRAM tRAS = 11 cycles									*/
+#define TRAS_12			0x00000300	/* SDRAM tRAS = 12 cycles									*/
+#define TRAS_13			0x00000340	/* SDRAM tRAS = 13 cycles									*/
+#define TRAS_14			0x00000380	/* SDRAM tRAS = 14 cycles									*/
+#define TRAS_15			0x000003C0	/* SDRAM tRAS = 15 cycles									*/
+#define TRP_1			0x00000800	/* SDRAM tRP = 1 cycle										*/
+#define TRP_2			0x00001000	/* SDRAM tRP = 2 cycles										*/
+#define TRP_3			0x00001800	/* SDRAM tRP = 3 cycles										*/
+#define TRP_4			0x00002000	/* SDRAM tRP = 4 cycles										*/
+#define TRP_5			0x00002800	/* SDRAM tRP = 5 cycles										*/
+#define TRP_6			0x00003000	/* SDRAM tRP = 6 cycles										*/
+#define TRP_7			0x00003800	/* SDRAM tRP = 7 cycles										*/
+#define TRCD_1			0x00008000	/* SDRAM tRCD = 1 cycle										*/
+#define TRCD_2			0x00010000	/* SDRAM tRCD = 2 cycles									*/
+#define TRCD_3			0x00018000	/* SDRAM tRCD = 3 cycles									*/
+#define TRCD_4			0x00020000	/* SDRAM tRCD = 4 cycles									*/
+#define TRCD_5			0x00028000	/* SDRAM tRCD = 5 cycles									*/
+#define TRCD_6			0x00030000	/* SDRAM tRCD = 6 cycles									*/
+#define TRCD_7			0x00038000	/* SDRAM tRCD = 7 cycles									*/
+#define TWR_1			0x00080000	/* SDRAM tWR = 1 cycle										*/
+#define TWR_2			0x00100000	/* SDRAM tWR = 2 cycles										*/
+#define TWR_3			0x00180000	/* SDRAM tWR = 3 cycles										*/
+#define PUPSD			0x00200000	/* Power-Up Start Delay (15 SCLK Cycles Delay)				*/
+#define PSM				0x00400000	/* Power-Up Sequence (Mode Register Before/After* Refresh)	*/
+#define PSS				0x00800000	/* Enable Power-Up Sequence on Next SDRAM Access			*/
+#define SRFS			0x01000000	/* Enable SDRAM Self-Refresh Mode							*/
+#define EBUFE			0x02000000	/* Enable External Buffering Timing							*/
+#define FBBRW			0x04000000	/* Enable Fast Back-To-Back Read To Write					*/
+#define EMREN			0x10000000	/* Extended Mode Register Enable							*/
+#define TCSR			0x20000000	/* Temp-Compensated Self-Refresh Value (85/45* Deg C)		*/
+#define CDDBG			0x40000000	/* Tristate SDRAM Controls During Bus Grant					*/
+
+/* EBIU_SDBCTL Masks																		*/
+#define EBE				0x0001		/* Enable SDRAM External Bank							*/
+#define EBSZ_16			0x0000		/* SDRAM External Bank Size = 16MB	*/
+#define EBSZ_32			0x0002		/* SDRAM External Bank Size = 32MB	*/
+#define EBSZ_64			0x0004		/* SDRAM External Bank Size = 64MB	*/
+#define EBSZ_128		0x0006		/* SDRAM External Bank Size = 128MB		*/
+#define EBSZ_256		0x0008		/* SDRAM External Bank Size = 256MB 	*/
+#define EBSZ_512		0x000A		/* SDRAM External Bank Size = 512MB		*/
+#define EBCAW_8			0x0000		/* SDRAM External Bank Column Address Width = 8 Bits	*/
+#define EBCAW_9			0x0010		/* SDRAM External Bank Column Address Width = 9 Bits	*/
+#define EBCAW_10		0x0020		/* SDRAM External Bank Column Address Width = 10 Bits	*/
+#define EBCAW_11		0x0030		/* SDRAM External Bank Column Address Width = 11 Bits	*/
+
+/* EBIU_SDSTAT Masks														*/
+#define SDCI			0x0001		/* SDRAM Controller Idle 				*/
+#define SDSRA			0x0002		/* SDRAM Self-Refresh Active			*/
+#define SDPUA			0x0004		/* SDRAM Power-Up Active 				*/
+#define SDRS			0x0008		/* SDRAM Will Power-Up On Next Access	*/
+#define SDEASE			0x0010		/* SDRAM EAB Sticky Error Status		*/
+#define BGSTAT			0x0020		/* Bus Grant Status						*/
+
+
+/* **************************  DMA CONTROLLER MASKS  ********************************/
+/* DMAx_CONFIG, MDMA_yy_CONFIG Masks												*/
+#define DMAEN			0x0001		/* DMA Channel Enable							*/
+#define WNR				0x0002		/* Channel Direction (W/R*)						*/
+#define WDSIZE_8		0x0000		/* Transfer Word Size = 8						*/
+#define WDSIZE_16		0x0004		/* Transfer Word Size = 16						*/
+#define WDSIZE_32		0x0008		/* Transfer Word Size = 32						*/
+#define DMA2D			0x0010		/* DMA Mode (2D/1D*)							*/
+#define RESTART			0x0020		/* DMA Buffer Clear								*/
+#define DI_SEL			0x0040		/* Data Interrupt Timing Select					*/
+#define DI_EN			0x0080		/* Data Interrupt Enable						*/
+#define NDSIZE_0		0x0000		/* Next Descriptor Size = 0 (Stop/Autobuffer)	*/
+#define NDSIZE_1		0x0100		/* Next Descriptor Size = 1						*/
+#define NDSIZE_2		0x0200		/* Next Descriptor Size = 2						*/
+#define NDSIZE_3		0x0300		/* Next Descriptor Size = 3						*/
+#define NDSIZE_4		0x0400		/* Next Descriptor Size = 4						*/
+#define NDSIZE_5		0x0500		/* Next Descriptor Size = 5						*/
+#define NDSIZE_6		0x0600		/* Next Descriptor Size = 6						*/
+#define NDSIZE_7		0x0700		/* Next Descriptor Size = 7						*/
+#define NDSIZE_8		0x0800		/* Next Descriptor Size = 8						*/
+#define NDSIZE_9		0x0900		/* Next Descriptor Size = 9						*/
+#define NDSIZE	        	0x0900	/* Next Descriptor Size */
+#define DMAFLOW	        	0x7000	/* Flow Control */
+#define DMAFLOW_STOP		0x0000		/* Stop Mode									*/
+#define DMAFLOW_AUTO		0x1000		/* Autobuffer Mode								*/
+#define DMAFLOW_ARRAY		0x4000		/* Descriptor Array Mode						*/
+#define DMAFLOW_SMALL		0x6000		/* Small Model Descriptor List Mode				*/
+#define DMAFLOW_LARGE		0x7000		/* Large Model Descriptor List Mode				*/
+
+/* DMAx_PERIPHERAL_MAP, MDMA_yy_PERIPHERAL_MAP Masks								*/
+#define CTYPE			0x0040	/* DMA Channel Type Indicator (Memory/Peripheral*)	*/
+#define PMAP			0xF000	/* Peripheral Mapped To This Channel				*/
+#define PMAP_PPI		0x0000	/* 		PPI Port DMA								*/
+#define	PMAP_EMACRX		0x1000	/* 		Ethernet Receive DMA						*/
+#define PMAP_EMACTX		0x2000	/* 		Ethernet Transmit DMA						*/
+#define PMAP_SPORT0RX	0x3000	/* 		SPORT0 Receive DMA							*/
+#define PMAP_SPORT0TX	0x4000	/* 		SPORT0 Transmit DMA							*/
+#define PMAP_SPORT1RX	0x5000	/* 		SPORT1 Receive DMA							*/
+#define PMAP_SPORT1TX	0x6000	/* 		SPORT1 Transmit DMA							*/
+#define PMAP_SPI		0x7000	/* 		SPI Port DMA								*/
+#define PMAP_UART0RX	0x8000	/* 		UART0 Port Receive DMA						*/
+#define PMAP_UART0TX	0x9000	/* 		UART0 Port Transmit DMA						*/
+#define	PMAP_UART1RX	0xA000	/* 		UART1 Port Receive DMA						*/
+#define	PMAP_UART1TX	0xB000	/* 		UART1 Port Transmit DMA						*/
+
+/* DMAx_IRQ_STATUS, MDMA_yy_IRQ_STATUS Masks						*/
+#define DMA_DONE		0x0001	/* DMA Completion Interrupt Status	*/
+#define DMA_ERR			0x0002	/* DMA Error Interrupt Status		*/
+#define DFETCH			0x0004	/* DMA Descriptor Fetch Indicator	*/
+#define DMA_RUN			0x0008	/* DMA Channel Running Indicator	*/
+
+
+/*  ************  PARALLEL PERIPHERAL INTERFACE (PPI) MASKS *************/
+/*  PPI_CONTROL Masks													*/
+#define PORT_EN			0x0001		/* PPI Port Enable					*/
+#define PORT_DIR		0x0002		/* PPI Port Direction				*/
+#define XFR_TYPE		0x000C		/* PPI Transfer Type				*/
+#define PORT_CFG		0x0030		/* PPI Port Configuration			*/
+#define FLD_SEL			0x0040		/* PPI Active Field Select			*/
+#define PACK_EN			0x0080		/* PPI Packing Mode					*/
+#define DMA32			0x0100		/* PPI 32-bit DMA Enable			*/
+#define SKIP_EN			0x0200		/* PPI Skip Element Enable			*/
+#define SKIP_EO			0x0400		/* PPI Skip Even/Odd Elements		*/
+#define DLEN_8			0x0000		/* Data Length = 8 Bits				*/
+#define DLEN_10			0x0800		/* Data Length = 10 Bits			*/
+#define DLEN_11			0x1000		/* Data Length = 11 Bits			*/
+#define DLEN_12			0x1800		/* Data Length = 12 Bits			*/
+#define DLEN_13			0x2000		/* Data Length = 13 Bits			*/
+#define DLEN_14			0x2800		/* Data Length = 14 Bits			*/
+#define DLEN_15			0x3000		/* Data Length = 15 Bits			*/
+#define DLEN_16			0x3800		/* Data Length = 16 Bits			*/
+#define DLENGTH			0x3800		/* PPI Data Length  */
+#define POLC			0x4000		/* PPI Clock Polarity				*/
+#define POLS			0x8000		/* PPI Frame Sync Polarity			*/
+
+/* PPI_STATUS Masks														*/
+#define FLD				0x0400		/* Field Indicator					*/
+#define FT_ERR			0x0800		/* Frame Track Error				*/
+#define OVR				0x1000		/* FIFO Overflow Error				*/
+#define UNDR			0x2000		/* FIFO Underrun Error				*/
+#define ERR_DET			0x4000		/* Error Detected Indicator			*/
+#define ERR_NCOR		0x8000		/* Error Not Corrected Indicator	*/
+
+
+/*  ********************  TWO-WIRE INTERFACE (TWI) MASKS  ***********************/
+/* TWI_CLKDIV Macros (Use: *pTWI_CLKDIV = CLKLOW(x)|CLKHI(y);  )				*/
+#define	CLKLOW(x)	((x) & 0xFF)		/* Periods Clock Is Held Low			*/
+#define CLKHI(y)	(((y)&0xFF)<<0x8)	/* Periods Before New Clock Low			*/
+
+/* TWI_PRESCALE Masks															*/
+#define	PRESCALE	0x007F		/* SCLKs Per Internal Time Reference (10MHz)	*/
+#define	TWI_ENA		0x0080		/* TWI Enable									*/
+#define	SCCB		0x0200		/* SCCB Compatibility Enable					*/
+
+/* TWI_SLAVE_CTRL Masks															*/
+#define	SEN			0x0001		/* Slave Enable									*/
+#define	SADD_LEN	0x0002		/* Slave Address Length							*/
+#define	STDVAL		0x0004		/* Slave Transmit Data Valid					*/
+#define	NAK			0x0008		/* NAK/ACK* Generated At Conclusion Of Transfer */
+#define	GEN			0x0010		/* General Call Adrress Matching Enabled		*/
+
+/* TWI_SLAVE_STAT Masks															*/
+#define	SDIR		0x0001		/* Slave Transfer Direction (Transmit/Receive*)	*/
+#define GCALL		0x0002		/* General Call Indicator						*/
+
+/* TWI_MASTER_CTRL Masks													*/
+#define	MEN			0x0001		/* Master Mode Enable						*/
+#define	MADD_LEN	0x0002		/* Master Address Length					*/
+#define	MDIR		0x0004		/* Master Transmit Direction (RX/TX*)		*/
+#define	FAST		0x0008		/* Use Fast Mode Timing Specs				*/
+#define	STOP		0x0010		/* Issue Stop Condition						*/
+#define	RSTART		0x0020		/* Repeat Start or Stop* At End Of Transfer	*/
+#define	DCNT		0x3FC0		/* Data Bytes To Transfer					*/
+#define	SDAOVR		0x4000		/* Serial Data Override						*/
+#define	SCLOVR		0x8000		/* Serial Clock Override					*/
+
+/* TWI_MASTER_STAT Masks														*/
+#define	MPROG		0x0001		/* Master Transfer In Progress					*/
+#define	LOSTARB		0x0002		/* Lost Arbitration Indicator (Xfer Aborted)	*/
+#define	ANAK		0x0004		/* Address Not Acknowledged						*/
+#define	DNAK		0x0008		/* Data Not Acknowledged						*/
+#define	BUFRDERR	0x0010		/* Buffer Read Error							*/
+#define	BUFWRERR	0x0020		/* Buffer Write Error							*/
+#define	SDASEN		0x0040		/* Serial Data Sense							*/
+#define	SCLSEN		0x0080		/* Serial Clock Sense							*/
+#define	BUSBUSY		0x0100		/* Bus Busy Indicator							*/
+
+/* TWI_INT_SRC and TWI_INT_ENABLE Masks						*/
+#define	SINIT		0x0001		/* Slave Transfer Initiated	*/
+#define	SCOMP		0x0002		/* Slave Transfer Complete	*/
+#define	SERR		0x0004		/* Slave Transfer Error		*/
+#define	SOVF		0x0008		/* Slave Overflow			*/
+#define	MCOMP		0x0010		/* Master Transfer Complete	*/
+#define	MERR		0x0020		/* Master Transfer Error	*/
+#define	XMTSERV		0x0040		/* Transmit FIFO Service	*/
+#define	RCVSERV		0x0080		/* Receive FIFO Service		*/
+
+/* TWI_FIFO_CTRL Masks												*/
+#define	XMTFLUSH	0x0001		/* Transmit Buffer Flush			*/
+#define	RCVFLUSH	0x0002		/* Receive Buffer Flush				*/
+#define	XMTINTLEN	0x0004		/* Transmit Buffer Interrupt Length	*/
+#define	RCVINTLEN	0x0008		/* Receive Buffer Interrupt Length	*/
+
+/* TWI_FIFO_STAT Masks															*/
+#define	XMTSTAT		0x0003		/* Transmit FIFO Status							*/
+#define	XMT_EMPTY	0x0000		/* 		Transmit FIFO Empty						*/
+#define	XMT_HALF	0x0001		/* 		Transmit FIFO Has 1 Byte To Write		*/
+#define	XMT_FULL	0x0003		/* 		Transmit FIFO Full (2 Bytes To Write)	*/
+
+#define	RCVSTAT		0x000C		/* Receive FIFO Status							*/
+#define	RCV_EMPTY	0x0000		/* 		Receive FIFO Empty						*/
+#define	RCV_HALF	0x0004		/* 		Receive FIFO Has 1 Byte To Read			*/
+#define	RCV_FULL	0x000C		/* 		Receive FIFO Full (2 Bytes To Read)		*/
+
+
+/*  *******************  PIN CONTROL REGISTER MASKS  ************************/
+/* PORT_MUX Masks															*/
+#define	PJSE			0x0001			/* Port J SPI/SPORT Enable			*/
+#define	PJSE_SPORT		0x0000			/* 		Enable TFS0/DT0PRI			*/
+#define	PJSE_SPI		0x0001			/* 		Enable SPI_SSEL3:2			*/
+
+#define	PJCE(x)			(((x)&0x3)<<1)	/* Port J CAN/SPI/SPORT Enable		*/
+#define	PJCE_SPORT		0x0000			/* 		Enable DR0SEC/DT0SEC		*/
+#define	PJCE_CAN		0x0002			/* 		Enable CAN RX/TX			*/
+#define	PJCE_SPI		0x0004			/* 		Enable SPI_SSEL7			*/
+
+#define	PFDE			0x0008			/* Port F DMA Request Enable		*/
+#define	PFDE_UART		0x0000			/* 		Enable UART0 RX/TX			*/
+#define	PFDE_DMA		0x0008			/* 		Enable DMAR1:0				*/
+
+#define	PFTE			0x0010			/* Port F Timer Enable				*/
+#define	PFTE_UART		0x0000			/*		Enable UART1 RX/TX			*/
+#define	PFTE_TIMER		0x0010			/* 		Enable TMR7:6				*/
+
+#define	PFS6E			0x0020			/* Port F SPI SSEL 6 Enable			*/
+#define	PFS6E_TIMER		0x0000			/*		Enable TMR5					*/
+#define	PFS6E_SPI		0x0020			/* 		Enable SPI_SSEL6			*/
+
+#define	PFS5E			0x0040			/* Port F SPI SSEL 5 Enable			*/
+#define	PFS5E_TIMER		0x0000			/*		Enable TMR4					*/
+#define	PFS5E_SPI		0x0040			/* 		Enable SPI_SSEL5			*/
+
+#define	PFS4E			0x0080			/* Port F SPI SSEL 4 Enable			*/
+#define	PFS4E_TIMER		0x0000			/*		Enable TMR3					*/
+#define	PFS4E_SPI		0x0080			/* 		Enable SPI_SSEL4			*/
+
+#define	PFFE			0x0100			/* Port F PPI Frame Sync Enable		*/
+#define	PFFE_TIMER		0x0000			/* 		Enable TMR2					*/
+#define	PFFE_PPI		0x0100			/* 		Enable PPI FS3				*/
+
+#define	PGSE			0x0200			/* Port G SPORT1 Secondary Enable	*/
+#define	PGSE_PPI		0x0000			/* 		Enable PPI D9:8				*/
+#define	PGSE_SPORT		0x0200			/* 		Enable DR1SEC/DT1SEC		*/
+
+#define	PGRE			0x0400			/* Port G SPORT1 Receive Enable		*/
+#define	PGRE_PPI		0x0000			/* 		Enable PPI D12:10			*/
+#define	PGRE_SPORT		0x0400			/* 		Enable DR1PRI/RFS1/RSCLK1	*/
+
+#define	PGTE			0x0800			/* Port G SPORT1 Transmit Enable	*/
+#define	PGTE_PPI		0x0000			/* 		Enable PPI D15:13			*/
+#define	PGTE_SPORT		0x0800			/* 		Enable DT1PRI/TFS1/TSCLK1	*/
+
+
+/*  ******************  HANDSHAKE DMA (HDMA) MASKS  *********************/
+/* HDMAx_CTL Masks														*/
+#define	HMDMAEN		0x0001	/* Enable Handshake DMA 0/1					*/
+#define	REP			0x0002	/* HDMA Request Polarity					*/
+#define	UTE			0x0004	/* Urgency Threshold Enable					*/
+#define	OIE			0x0010	/* Overflow Interrupt Enable				*/
+#define	BDIE		0x0020	/* Block Done Interrupt Enable				*/
+#define	MBDI		0x0040	/* Mask Block Done IRQ If Pending ECNT		*/
+#define	DRQ			0x0300	/* HDMA Request Type						*/
+#define	DRQ_NONE	0x0000	/* 		No Request							*/
+#define	DRQ_SINGLE	0x0100	/* 		Channels Request Single				*/
+#define	DRQ_MULTI	0x0200	/* 		Channels Request Multi (Default)	*/
+#define	DRQ_URGENT	0x0300	/* 		Channels Request Multi Urgent		*/
+#define	RBC			0x1000	/* Reload BCNT With IBCNT					*/
+#define	PS			0x2000	/* HDMA Pin Status							*/
+#define	OI			0x4000	/* Overflow Interrupt Generated				*/
+#define	BDI			0x8000	/* Block Done Interrupt Generated			*/
+
+/* entry addresses of the user-callable Boot ROM functions */
+
+#define _BOOTROM_RESET 0xEF000000
+#define _BOOTROM_FINAL_INIT 0xEF000002
+#define _BOOTROM_DO_MEMORY_DMA 0xEF000006
+#define _BOOTROM_BOOT_DXE_FLASH 0xEF000008
+#define _BOOTROM_BOOT_DXE_SPI 0xEF00000A
+#define _BOOTROM_BOOT_DXE_TWI 0xEF00000C
+#define _BOOTROM_GET_DXE_ADDRESS_FLASH 0xEF000010
+#define _BOOTROM_GET_DXE_ADDRESS_SPI 0xEF000012
+#define _BOOTROM_GET_DXE_ADDRESS_TWI 0xEF000014
+
+/* Alternate Deprecated Macros Provided For Backwards Code Compatibility */
+#define	PGDE_UART   PFDE_UART
+#define	PGDE_DMA    PFDE_DMA
+#define	CKELOW		SCKELOW
+
+/* HOST Port Registers */
+
+#define                     HOST_CONTROL  0xffc03400   /* HOST Control Register */
+#define                      HOST_STATUS  0xffc03404   /* HOST Status Register */
+#define                     HOST_TIMEOUT  0xffc03408   /* HOST Acknowledge Mode Timeout Register */
+
+/* Counter Registers */
+
+#define                       CNT_CONFIG  0xffc03500   /* Configuration Register */
+#define                        CNT_IMASK  0xffc03504   /* Interrupt Mask Register */
+#define                       CNT_STATUS  0xffc03508   /* Status Register */
+#define                      CNT_COMMAND  0xffc0350c   /* Command Register */
+#define                     CNT_DEBOUNCE  0xffc03510   /* Debounce Register */
+#define                      CNT_COUNTER  0xffc03514   /* Counter Register */
+#define                          CNT_MAX  0xffc03518   /* Maximal Count Register */
+#define                          CNT_MIN  0xffc0351c   /* Minimal Count Register */
+
+/* OTP/FUSE Registers */
+
+#define                      OTP_CONTROL  0xffc03600   /* OTP/Fuse Control Register */
+#define                          OTP_BEN  0xffc03604   /* OTP/Fuse Byte Enable */
+#define                       OTP_STATUS  0xffc03608   /* OTP/Fuse Status */
+#define                       OTP_TIMING  0xffc0360c   /* OTP/Fuse Access Timing */
+
+/* Security Registers */
+
+#define                    SECURE_SYSSWT  0xffc03620   /* Secure System Switches */
+#define                   SECURE_CONTROL  0xffc03624   /* Secure Control */
+#define                    SECURE_STATUS  0xffc03628   /* Secure Status */
+
+/* OTP Read/Write Data Buffer Registers */
+
+#define                        OTP_DATA0  0xffc03680   /* OTP/Fuse Data (OTP_DATA0-3) accesses the fuse read write buffer */
+#define                        OTP_DATA1  0xffc03684   /* OTP/Fuse Data (OTP_DATA0-3) accesses the fuse read write buffer */
+#define                        OTP_DATA2  0xffc03688   /* OTP/Fuse Data (OTP_DATA0-3) accesses the fuse read write buffer */
+#define                        OTP_DATA3  0xffc0368c   /* OTP/Fuse Data (OTP_DATA0-3) accesses the fuse read write buffer */
+
+/* Motor Control PWM Registers */
+
+#define                         PWM_CTRL  0xffc03700   /* PWM Control Register */
+#define                         PWM_STAT  0xffc03704   /* PWM Status Register */
+#define                           PWM_TM  0xffc03708   /* PWM Period Register */
+#define                           PWM_DT  0xffc0370c   /* PWM Dead Time Register */
+#define                         PWM_GATE  0xffc03710   /* PWM Chopping Control */
+#define                          PWM_CHA  0xffc03714   /* PWM Channel A Duty Control */
+#define                          PWM_CHB  0xffc03718   /* PWM Channel B Duty Control */
+#define                          PWM_CHC  0xffc0371c   /* PWM Channel C Duty Control */
+#define                          PWM_SEG  0xffc03720   /* PWM Crossover and Output Enable */
+#define                       PWM_SYNCWT  0xffc03724   /* PWM Sync Pluse Width Control */
+#define                         PWM_CHAL  0xffc03728   /* PWM Channel AL Duty Control (SR mode only) */
+#define                         PWM_CHBL  0xffc0372c   /* PWM Channel BL Duty Control (SR mode only) */
+#define                         PWM_CHCL  0xffc03730   /* PWM Channel CL Duty Control (SR mode only) */
+#define                          PWM_LSI  0xffc03734   /* PWM Low Side Invert (SR mode only) */
+#define                        PWM_STAT2  0xffc03738   /* PWM Status Register 2 */
+
+
+/* ********************************************************** */
+/*     SINGLE BIT MACRO PAIRS (bit mask and negated one)      */
+/*     and MULTI BIT READ MACROS                              */
+/* ********************************************************** */
+
+/* Bit masks for HOST_CONTROL */
+
+#define                   HOST_CNTR_HOST_EN  0x1        /* Host Enable */
+#define                  HOST_CNTR_nHOST_EN  0x0
+#define                  HOST_CNTR_HOST_END  0x2        /* Host Endianess */
+#define                 HOST_CNTR_nHOST_END  0x0
+#define                 HOST_CNTR_DATA_SIZE  0x4        /* Data Size */
+#define                HOST_CNTR_nDATA_SIZE  0x0
+#define                  HOST_CNTR_HOST_RST  0x8        /* Host Reset */
+#define                 HOST_CNTR_nHOST_RST  0x0
+#define                  HOST_CNTR_HRDY_OVR  0x20       /* Host Ready Override */
+#define                 HOST_CNTR_nHRDY_OVR  0x0
+#define                  HOST_CNTR_INT_MODE  0x40       /* Interrupt Mode */
+#define                 HOST_CNTR_nINT_MODE  0x0
+#define                     HOST_CNTR_BT_EN  0x80       /* Bus Timeout Enable */
+#define                   HOST_CNTR_ nBT_EN  0x0
+#define                       HOST_CNTR_EHW  0x100      /* Enable Host Write */
+#define                      HOST_CNTR_nEHW  0x0
+#define                       HOST_CNTR_EHR  0x200      /* Enable Host Read */
+#define                      HOST_CNTR_nEHR  0x0
+#define                       HOST_CNTR_BDR  0x400      /* Burst DMA Requests */
+#define                      HOST_CNTR_nBDR  0x0
+
+/* Bit masks for HOST_STATUS */
+
+#define                     HOST_STAT_READY  0x1        /* DMA Ready */
+#define                    HOST_STAT_nREADY  0x0
+#define                  HOST_STAT_FIFOFULL  0x2        /* FIFO Full */
+#define                 HOST_STAT_nFIFOFULL  0x0
+#define                 HOST_STAT_FIFOEMPTY  0x4        /* FIFO Empty */
+#define                HOST_STAT_nFIFOEMPTY  0x0
+#define                  HOST_STAT_COMPLETE  0x8        /* DMA Complete */
+#define                 HOST_STAT_nCOMPLETE  0x0
+#define                      HOST_STAT_HSHK  0x10       /* Host Handshake */
+#define                     HOST_STAT_nHSHK  0x0
+#define                   HOST_STAT_TIMEOUT  0x20       /* Host Timeout */
+#define                  HOST_STAT_nTIMEOUT  0x0
+#define                      HOST_STAT_HIRQ  0x40       /* Host Interrupt Request */
+#define                     HOST_STAT_nHIRQ  0x0
+#define                HOST_STAT_ALLOW_CNFG  0x80       /* Allow New Configuration */
+#define               HOST_STAT_nALLOW_CNFG  0x0
+#define                   HOST_STAT_DMA_DIR  0x100      /* DMA Direction */
+#define                  HOST_STAT_nDMA_DIR  0x0
+#define                       HOST_STAT_BTE  0x200      /* Bus Timeout Enabled */
+#define                      HOST_STAT_nBTE  0x0
+#define               HOST_STAT_HOSTRD_DONE  0x8000     /* Host Read Completion Interrupt */
+#define              HOST_STAT_nHOSTRD_DONE  0x0
+
+/* Bit masks for HOST_TIMEOUT */
+
+#define             HOST_COUNT_TIMEOUT  0x7ff      /* Host Timeout count */
+
+/* Bit masks for CNT_CONFIG */
+
+#define                      CNTE  0x1        /* Counter Enable */
+#define                     nCNTE  0x0
+#define                      DEBE  0x2        /* Debounce Enable */
+#define                     nDEBE  0x0
+#define                    CDGINV  0x10       /* CDG Pin Polarity Invert */
+#define                   nCDGINV  0x0
+#define                    CUDINV  0x20       /* CUD Pin Polarity Invert */
+#define                   nCUDINV  0x0
+#define                    CZMINV  0x40       /* CZM Pin Polarity Invert */
+#define                   nCZMINV  0x0
+#define                   CNTMODE  0x700      /* Counter Operating Mode */
+#define                      ZMZC  0x800      /* CZM Zeroes Counter Enable */
+#define                     nZMZC  0x0
+#define                   BNDMODE  0x3000     /* Boundary register Mode */
+#define                    INPDIS  0x8000     /* CUG and CDG Input Disable */
+#define                   nINPDIS  0x0
+
+/* Bit masks for CNT_IMASK */
+
+#define                      ICIE  0x1        /* Illegal Gray/Binary Code Interrupt Enable */
+#define                     nICIE  0x0
+#define                      UCIE  0x2        /* Up count Interrupt Enable */
+#define                     nUCIE  0x0
+#define                      DCIE  0x4        /* Down count Interrupt Enable */
+#define                     nDCIE  0x0
+#define                    MINCIE  0x8        /* Min Count Interrupt Enable */
+#define                   nMINCIE  0x0
+#define                    MAXCIE  0x10       /* Max Count Interrupt Enable */
+#define                   nMAXCIE  0x0
+#define                   COV31IE  0x20       /* Bit 31 Overflow Interrupt Enable */
+#define                  nCOV31IE  0x0
+#define                   COV15IE  0x40       /* Bit 15 Overflow Interrupt Enable */
+#define                  nCOV15IE  0x0
+#define                   CZEROIE  0x80       /* Count to Zero Interrupt Enable */
+#define                  nCZEROIE  0x0
+#define                     CZMIE  0x100      /* CZM Pin Interrupt Enable */
+#define                    nCZMIE  0x0
+#define                    CZMEIE  0x200      /* CZM Error Interrupt Enable */
+#define                   nCZMEIE  0x0
+#define                    CZMZIE  0x400      /* CZM Zeroes Counter Interrupt Enable */
+#define                   nCZMZIE  0x0
+
+/* Bit masks for CNT_STATUS */
+
+#define                      ICII  0x1        /* Illegal Gray/Binary Code Interrupt Identifier */
+#define                     nICII  0x0
+#define                      UCII  0x2        /* Up count Interrupt Identifier */
+#define                     nUCII  0x0
+#define                      DCII  0x4        /* Down count Interrupt Identifier */
+#define                     nDCII  0x0
+#define                    MINCII  0x8        /* Min Count Interrupt Identifier */
+#define                   nMINCII  0x0
+#define                    MAXCII  0x10       /* Max Count Interrupt Identifier */
+#define                   nMAXCII  0x0
+#define                   COV31II  0x20       /* Bit 31 Overflow Interrupt Identifier */
+#define                  nCOV31II  0x0
+#define                   COV15II  0x40       /* Bit 15 Overflow Interrupt Identifier */
+#define                  nCOV15II  0x0
+#define                   CZEROII  0x80       /* Count to Zero Interrupt Identifier */
+#define                  nCZEROII  0x0
+#define                     CZMII  0x100      /* CZM Pin Interrupt Identifier */
+#define                    nCZMII  0x0
+#define                    CZMEII  0x200      /* CZM Error Interrupt Identifier */
+#define                   nCZMEII  0x0
+#define                    CZMZII  0x400      /* CZM Zeroes Counter Interrupt Identifier */
+#define                   nCZMZII  0x0
+
+/* Bit masks for CNT_COMMAND */
+
+#define                    W1LCNT  0xf        /* Load Counter Register */
+#define                    W1LMIN  0xf0       /* Load Min Register */
+#define                    W1LMAX  0xf00      /* Load Max Register */
+#define                  W1ZMONCE  0x1000     /* Enable CZM Clear Counter Once */
+#define                 nW1ZMONCE  0x0
+
+/* Bit masks for CNT_DEBOUNCE */
+
+#define                 DPRESCALE  0xf        /* Load Counter Register */
+
+/* CNT_COMMAND bit field options */
+
+#define W1LCNT_ZERO   0x0001   /* write 1 to load CNT_COUNTER with zero */
+#define W1LCNT_MIN    0x0004   /* write 1 to load CNT_COUNTER from CNT_MIN */
+#define W1LCNT_MAX    0x0008   /* write 1 to load CNT_COUNTER from CNT_MAX */
+
+#define W1LMIN_ZERO   0x0010   /* write 1 to load CNT_MIN with zero */
+#define W1LMIN_CNT    0x0020   /* write 1 to load CNT_MIN from CNT_COUNTER */
+#define W1LMIN_MAX    0x0080   /* write 1 to load CNT_MIN from CNT_MAX */
+
+#define W1LMAX_ZERO   0x0100   /* write 1 to load CNT_MAX with zero */
+#define W1LMAX_CNT    0x0200   /* write 1 to load CNT_MAX from CNT_COUNTER */
+#define W1LMAX_MIN    0x0400   /* write 1 to load CNT_MAX from CNT_MIN */
+
+/* CNT_CONFIG bit field options */
+
+#define CNTMODE_QUADENC  0x0000  /* quadrature encoder mode */
+#define CNTMODE_BINENC   0x0100  /* binary encoder mode */
+#define CNTMODE_UDCNT    0x0200  /* up/down counter mode */
+#define CNTMODE_DIRCNT   0x0400  /* direction counter mode */
+#define CNTMODE_DIRTMR   0x0500  /* direction timer mode */
+
+#define BNDMODE_COMP     0x0000  /* boundary compare mode */
+#define BNDMODE_ZERO     0x1000  /* boundary compare and zero mode */
+#define BNDMODE_CAPT     0x2000  /* boundary capture mode */
+#define BNDMODE_AEXT     0x3000  /* boundary auto-extend mode */
+
+/* Bit masks for OTP_CONTROL */
+
+#define                FUSE_FADDR  0x1ff      /* OTP/Fuse Address */
+#define                      FIEN  0x800      /* OTP/Fuse Interrupt Enable */
+#define                     nFIEN  0x0
+#define                  FTESTDEC  0x1000     /* OTP/Fuse Test Decoder */
+#define                 nFTESTDEC  0x0
+#define                   FWRTEST  0x2000     /* OTP/Fuse Write Test */
+#define                  nFWRTEST  0x0
+#define                     FRDEN  0x4000     /* OTP/Fuse Read Enable */
+#define                    nFRDEN  0x0
+#define                     FWREN  0x8000     /* OTP/Fuse Write Enable */
+#define                    nFWREN  0x0
+
+/* Bit masks for OTP_BEN */
+
+#define                      FBEN  0xffff     /* OTP/Fuse Byte Enable */
+
+/* Bit masks for OTP_STATUS */
+
+#define                     FCOMP  0x1        /* OTP/Fuse Access Complete */
+#define                    nFCOMP  0x0
+#define                    FERROR  0x2        /* OTP/Fuse Access Error */
+#define                   nFERROR  0x0
+#define                  MMRGLOAD  0x10       /* Memory Mapped Register Gasket Load */
+#define                 nMMRGLOAD  0x0
+#define                  MMRGLOCK  0x20       /* Memory Mapped Register Gasket Lock */
+#define                 nMMRGLOCK  0x0
+#define                    FPGMEN  0x40       /* OTP/Fuse Program Enable */
+#define                   nFPGMEN  0x0
+
+/* Bit masks for OTP_TIMING */
+
+#define                   USECDIV  0xff       /* Micro Second Divider */
+#define                   READACC  0x7f00     /* Read Access Time */
+#define                   CPUMPRL  0x38000    /* Charge Pump Release Time */
+#define                   CPUMPSU  0xc0000    /* Charge Pump Setup Time */
+#define                   CPUMPHD  0xf00000   /* Charge Pump Hold Time */
+#define                   PGMTIME  0xff000000 /* Program Time */
+
+/* Bit masks for SECURE_SYSSWT */
+
+#define                   EMUDABL  0x1        /* Emulation Disable. */
+#define                  nEMUDABL  0x0
+#define                   RSTDABL  0x2        /* Reset Disable */
+#define                  nRSTDABL  0x0
+#define                   L1IDABL  0x1c       /* L1 Instruction Memory Disable. */
+#define                  L1DADABL  0xe0       /* L1 Data Bank A Memory Disable. */
+#define                  L1DBDABL  0x700      /* L1 Data Bank B Memory Disable. */
+#define                   DMA0OVR  0x800      /* DMA0 Memory Access Override */
+#define                  nDMA0OVR  0x0
+#define                   DMA1OVR  0x1000     /* DMA1 Memory Access Override */
+#define                  nDMA1OVR  0x0
+#define                    EMUOVR  0x4000     /* Emulation Override */
+#define                   nEMUOVR  0x0
+#define                    OTPSEN  0x8000     /* OTP Secrets Enable. */
+#define                   nOTPSEN  0x0
+#define                    L2DABL  0x70000    /* L2 Memory Disable. */
+
+/* Bit masks for SECURE_CONTROL */
+
+#define                   SECURE0  0x1        /* SECURE 0 */
+#define                  nSECURE0  0x0
+#define                   SECURE1  0x2        /* SECURE 1 */
+#define                  nSECURE1  0x0
+#define                   SECURE2  0x4        /* SECURE 2 */
+#define                  nSECURE2  0x0
+#define                   SECURE3  0x8        /* SECURE 3 */
+#define                  nSECURE3  0x0
+
+/* Bit masks for SECURE_STATUS */
+
+#define                   SECMODE  0x3        /* Secured Mode Control State */
+#define                       NMI  0x4        /* Non Maskable Interrupt */
+#define                      nNMI  0x0
+#define                   AFVALID  0x8        /* Authentication Firmware Valid */
+#define                  nAFVALID  0x0
+#define                    AFEXIT  0x10       /* Authentication Firmware Exit */
+#define                   nAFEXIT  0x0
+#define                   SECSTAT  0xe0       /* Secure Status */
+
+
+
+#endif /* _DEF_BF51X_H */
diff --git a/arch/blackfin/mach-bf518/include/mach/dma.h b/arch/blackfin/mach-bf518/include/mach/dma.h
new file mode 100644
index 000000000000..bbd33c1706e2
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/dma.h
@@ -0,0 +1,33 @@
+/* mach/dma.h - arch-specific DMA defines
+ *
+ * Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#ifndef _MACH_DMA_H_
+#define _MACH_DMA_H_
+
+#define MAX_DMA_CHANNELS 16
+
+#define CH_PPI 			0	/* PPI receive/transmit */
+#define CH_EMAC_RX 		1	/* Ethernet MAC receive */
+#define CH_EMAC_TX 		2	/* Ethernet MAC transmit */
+#define CH_SPORT0_RX 		3	/* SPORT0 receive */
+#define CH_SPORT0_TX 		4	/* SPORT0 transmit */
+#define CH_RSI 			4	/* RSI */
+#define CH_SPORT1_RX 		5	/* SPORT1 receive */
+#define CH_SPI1 		5	/* SPI1 transmit/receive */
+#define CH_SPORT1_TX 		6	/* SPORT1 transmit */
+#define CH_SPI0 		7	/* SPI0 transmit/receive */
+#define CH_UART0_RX 		8	/* UART0 receive */
+#define CH_UART0_TX 		9	/* UART0 transmit */
+#define CH_UART1_RX 		10	/* UART1 receive */
+#define CH_UART1_TX 		11	/* UART1 transmit */
+
+#define CH_MEM_STREAM0_SRC 	12	/* RX */
+#define CH_MEM_STREAM0_DEST	13	/* TX */
+#define CH_MEM_STREAM1_SRC 	14	/* RX */
+#define CH_MEM_STREAM1_DEST	15	/* TX */
+
+#endif
diff --git a/arch/blackfin/mach-bf518/include/mach/gpio.h b/arch/blackfin/mach-bf518/include/mach/gpio.h
new file mode 100644
index 000000000000..9757683c3948
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/gpio.h
@@ -0,0 +1,60 @@
+/*
+ * File: arch/blackfin/mach-bf518/include/mach/gpio.h
+ * Bugs: Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Copyright (C) 2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
+ */
+
+
+#ifndef _MACH_GPIO_H_
+#define _MACH_GPIO_H_
+
+#define MAX_BLACKFIN_GPIOS 40
+
+#define	GPIO_PF0	0
+#define	GPIO_PF1	1
+#define	GPIO_PF2	2
+#define	GPIO_PF3	3
+#define	GPIO_PF4	4
+#define	GPIO_PF5	5
+#define	GPIO_PF6	6
+#define	GPIO_PF7	7
+#define	GPIO_PF8	8
+#define	GPIO_PF9	9
+#define	GPIO_PF10	10
+#define	GPIO_PF11	11
+#define	GPIO_PF12	12
+#define	GPIO_PF13	13
+#define	GPIO_PF14	14
+#define	GPIO_PF15	15
+#define	GPIO_PG0	16
+#define	GPIO_PG1	17
+#define	GPIO_PG2	18
+#define	GPIO_PG3	19
+#define	GPIO_PG4	20
+#define	GPIO_PG5	21
+#define	GPIO_PG6	22
+#define	GPIO_PG7	23
+#define	GPIO_PG8	24
+#define	GPIO_PG9	25
+#define	GPIO_PG10      	26
+#define	GPIO_PG11      	27
+#define	GPIO_PG12      	28
+#define	GPIO_PG13      	29
+#define	GPIO_PG14      	30
+#define	GPIO_PG15      	31
+#define	GPIO_PH0	32
+#define	GPIO_PH1	33
+#define	GPIO_PH2	34
+#define	GPIO_PH3	35
+#define	GPIO_PH4	36
+#define	GPIO_PH5	37
+#define	GPIO_PH6	38
+#define	GPIO_PH7	39
+
+#define PORT_F GPIO_PF0
+#define PORT_G GPIO_PG0
+#define PORT_H GPIO_PH0
+
+#endif /* _MACH_GPIO_H_ */
diff --git a/arch/blackfin/mach-bf518/include/mach/irq.h b/arch/blackfin/mach-bf518/include/mach/irq.h
new file mode 100644
index 000000000000..3ff0f093313d
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/irq.h
@@ -0,0 +1,260 @@
+/*
+ * file:	include/asm-blackfin/mach-bf518/irq.h
+ * based on:	include/asm-blackfin/mach-bf527/irq.h
+ * author:	Michael Hennerich (michael.hennerich@analog.com)
+ *
+ * created:
+ * description:
+ *	system mmr register map
+ * rev:
+ *
+ * modified:
+ *
+ *
+ * bugs:         enter bugs at http://blackfin.uclinux.org/
+ *
+ * this program is free software; you can redistribute it and/or modify
+ * it under the terms of the gnu general public license as published by
+ * the free software foundation; either version 2, 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; see the file copying.
+ * if not, write to the free software foundation,
+ * 59 temple place - suite 330, boston, ma 02111-1307, usa.
+ */
+
+#ifndef _BF518_IRQ_H_
+#define _BF518_IRQ_H_
+
+/*
+ * Interrupt source definitions
+	Event Source    Core Event Name
+	Core        Emulation               **
+	Events         (highest priority)  EMU         0
+	Reset                   RST         1
+	NMI                     NMI         2
+	Exception               EVX         3
+	Reserved                --          4
+	Hardware Error          IVHW        5
+	Core Timer              IVTMR       6 *
+
+	.....
+
+	 Software Interrupt 1    IVG14       31
+	 Software Interrupt 2    --
+	 (lowest priority)  IVG15       32 *
+*/
+
+#define NR_PERI_INTS    (2 * 32)
+
+/* The ABSTRACT IRQ definitions */
+/** the first seven of the following are fixed, the rest you change if you need to **/
+#define IRQ_EMU			0	/* Emulation */
+#define IRQ_RST			1	/* reset */
+#define IRQ_NMI			2	/* Non Maskable */
+#define IRQ_EVX			3	/* Exception */
+#define IRQ_UNUSED		4	/* - unused interrupt */
+#define IRQ_HWERR		5	/* Hardware Error */
+#define IRQ_CORETMR		6	/* Core timer */
+
+#define BFIN_IRQ(x)		((x) + 7)
+
+#define IRQ_PLL_WAKEUP		BFIN_IRQ(0)	/* PLL Wakeup Interrupt */
+#define IRQ_DMA0_ERROR		BFIN_IRQ(1)	/* DMA Error 0 (generic) */
+#define IRQ_DMAR0_BLK		BFIN_IRQ(2)	/* DMAR0 Block Interrupt */
+#define IRQ_DMAR1_BLK		BFIN_IRQ(3)	/* DMAR1 Block Interrupt */
+#define IRQ_DMAR0_OVR		BFIN_IRQ(4)	/* DMAR0 Overflow Error */
+#define IRQ_DMAR1_OVR		BFIN_IRQ(5)	/* DMAR1 Overflow Error */
+#define IRQ_PPI_ERROR		BFIN_IRQ(6)	/* PPI Error */
+#define IRQ_MAC_ERROR		BFIN_IRQ(7)	/* MAC Status */
+#define IRQ_SPORT0_ERROR	BFIN_IRQ(8)	/* SPORT0 Status */
+#define IRQ_SPORT1_ERROR	BFIN_IRQ(9)	/* SPORT1 Status */
+#define IRQ_PTP_ERROR		BFIN_IRQ(10)	/* PTP Error Interrupt */
+#define IRQ_UART0_ERROR		BFIN_IRQ(12)	/* UART0 Status */
+#define IRQ_UART1_ERROR		BFIN_IRQ(13)	/* UART1 Status */
+#define IRQ_RTC			BFIN_IRQ(14)	/* RTC */
+#define IRQ_PPI      		BFIN_IRQ(15)	/* DMA Channel 0 (PPI) */
+#define IRQ_SPORT0_RX		BFIN_IRQ(16)	/* DMA 3 Channel (SPORT0 RX) */
+#define IRQ_SPORT0_TX		BFIN_IRQ(17)	/* DMA 4 Channel (SPORT0 TX) */
+#define IRQ_RSI			BFIN_IRQ(17)	/* DMA 4 Channel (RSI) */
+#define IRQ_SPORT1_RX		BFIN_IRQ(18)	/* DMA 5 Channel (SPORT1 RX/SPI) */
+#define IRQ_SPI1		BFIN_IRQ(18)	/* DMA 5 Channel (SPI1) */
+#define IRQ_SPORT1_TX		BFIN_IRQ(19)	/* DMA 6 Channel (SPORT1 TX) */
+#define IRQ_TWI      		BFIN_IRQ(20)	/* TWI */
+#define IRQ_SPI0     		BFIN_IRQ(21)	/* DMA 7 Channel (SPI0) */
+#define IRQ_UART0_RX 		BFIN_IRQ(22)	/* DMA8 Channel (UART0 RX) */
+#define IRQ_UART0_TX 		BFIN_IRQ(23)	/* DMA9 Channel (UART0 TX) */
+#define IRQ_UART1_RX 		BFIN_IRQ(24)	/* DMA10 Channel (UART1 RX) */
+#define IRQ_UART1_TX 		BFIN_IRQ(25)	/* DMA11 Channel (UART1 TX) */
+#define IRQ_OPTSEC   		BFIN_IRQ(26)	/* OTPSEC Interrupt */
+#define IRQ_CNT   		BFIN_IRQ(27)	/* GP Counter */
+#define IRQ_MAC_RX   		BFIN_IRQ(28)	/* DMA1 Channel (MAC RX) */
+#define IRQ_PORTH_INTA   	BFIN_IRQ(29)	/* Port H Interrupt A */
+#define IRQ_MAC_TX		BFIN_IRQ(30)	/* DMA2 Channel (MAC TX) */
+#define IRQ_PORTH_INTB		BFIN_IRQ(31)	/* Port H Interrupt B */
+#define IRQ_TIMER0		BFIN_IRQ(32)	/* Timer 0 */
+#define IRQ_TIMER1		BFIN_IRQ(33)	/* Timer 1 */
+#define IRQ_TIMER2		BFIN_IRQ(34)	/* Timer 2 */
+#define IRQ_TIMER3		BFIN_IRQ(35)	/* Timer 3 */
+#define IRQ_TIMER4		BFIN_IRQ(36)	/* Timer 4 */
+#define IRQ_TIMER5		BFIN_IRQ(37)	/* Timer 5 */
+#define IRQ_TIMER6		BFIN_IRQ(38)	/* Timer 6 */
+#define IRQ_TIMER7		BFIN_IRQ(39)	/* Timer 7 */
+#define IRQ_PORTG_INTA		BFIN_IRQ(40)	/* Port G Interrupt A */
+#define IRQ_PORTG_INTB		BFIN_IRQ(41)	/* Port G Interrupt B */
+#define IRQ_MEM_DMA0		BFIN_IRQ(42)	/* MDMA Stream 0 */
+#define IRQ_MEM_DMA1		BFIN_IRQ(43)	/* MDMA Stream 1 */
+#define IRQ_WATCH		BFIN_IRQ(44)	/* Software Watchdog Timer */
+#define IRQ_PORTF_INTA		BFIN_IRQ(45)	/* Port F Interrupt A */
+#define IRQ_PORTF_INTB		BFIN_IRQ(46)	/* Port F Interrupt B */
+#define IRQ_SPI0_ERROR		BFIN_IRQ(47)	/* SPI0 Status */
+#define IRQ_SPI1_ERROR		BFIN_IRQ(48)	/* SPI1 Error */
+#define IRQ_RSI_INT0		BFIN_IRQ(51)	/* RSI Interrupt0 */
+#define IRQ_RSI_INT1		BFIN_IRQ(52)	/* RSI Interrupt1 */
+#define IRQ_PWM_TRIP		BFIN_IRQ(53)	/* PWM Trip Interrupt */
+#define IRQ_PWM_SYNC		BFIN_IRQ(54)	/* PWM Sync Interrupt */
+#define IRQ_PTP_STAT		BFIN_IRQ(55)	/* PTP Stat Interrupt */
+
+#define SYS_IRQS        	BFIN_IRQ(63)	/* 70 */
+
+#define IRQ_PF0         71
+#define IRQ_PF1         72
+#define IRQ_PF2         73
+#define IRQ_PF3         74
+#define IRQ_PF4         75
+#define IRQ_PF5         76
+#define IRQ_PF6         77
+#define IRQ_PF7         78
+#define IRQ_PF8         79
+#define IRQ_PF9         80
+#define IRQ_PF10        81
+#define IRQ_PF11        82
+#define IRQ_PF12        83
+#define IRQ_PF13        84
+#define IRQ_PF14        85
+#define IRQ_PF15        86
+
+#define IRQ_PG0         87
+#define IRQ_PG1         88
+#define IRQ_PG2         89
+#define IRQ_PG3         90
+#define IRQ_PG4         91
+#define IRQ_PG5         92
+#define IRQ_PG6         93
+#define IRQ_PG7         94
+#define IRQ_PG8         95
+#define IRQ_PG9         96
+#define IRQ_PG10        97
+#define IRQ_PG11        98
+#define IRQ_PG12        99
+#define IRQ_PG13        100
+#define IRQ_PG14        101
+#define IRQ_PG15        102
+
+#define IRQ_PH0         103
+#define IRQ_PH1         104
+#define IRQ_PH2         105
+#define IRQ_PH3         106
+#define IRQ_PH4         107
+#define IRQ_PH5         108
+#define IRQ_PH6         109
+#define IRQ_PH7         110
+#define IRQ_PH8         111
+#define IRQ_PH9         112
+#define IRQ_PH10        113
+#define IRQ_PH11        114
+#define IRQ_PH12        115
+#define IRQ_PH13        116
+#define IRQ_PH14        117
+#define IRQ_PH15        118
+
+#define GPIO_IRQ_BASE	IRQ_PF0
+
+#define NR_IRQS     (IRQ_PH15 + 1)
+
+#define IVG7            7
+#define IVG8            8
+#define IVG9            9
+#define IVG10           10
+#define IVG11           11
+#define IVG12           12
+#define IVG13           13
+#define IVG14           14
+#define IVG15           15
+
+/* IAR0 BIT FIELDS */
+#define IRQ_PLL_WAKEUP_POS	0
+#define IRQ_DMA0_ERROR_POS	4
+#define IRQ_DMAR0_BLK_POS 	8
+#define IRQ_DMAR1_BLK_POS 	12
+#define IRQ_DMAR0_OVR_POS 	16
+#define IRQ_DMAR1_OVR_POS 	20
+#define IRQ_PPI_ERROR_POS 	24
+#define IRQ_MAC_ERROR_POS 	28
+
+/* IAR1 BIT FIELDS */
+#define IRQ_SPORT0_ERROR_POS	0
+#define IRQ_SPORT1_ERROR_POS	4
+#define IRQ_PTP_ERROR_POS	8
+#define IRQ_UART0_ERROR_POS 	16
+#define IRQ_UART1_ERROR_POS 	20
+#define IRQ_RTC_POS         	24
+#define IRQ_PPI_POS         	28
+
+/* IAR2 BIT FIELDS */
+#define IRQ_SPORT0_RX_POS	0
+#define IRQ_SPORT0_TX_POS	4
+#define IRQ_RSI_POS		4
+#define IRQ_SPORT1_RX_POS	8
+#define IRQ_SPI1_POS		8
+#define IRQ_SPORT1_TX_POS	12
+#define IRQ_TWI_POS      	16
+#define IRQ_SPI0_POS      	20
+#define IRQ_UART0_RX_POS 	24
+#define IRQ_UART0_TX_POS 	28
+
+/* IAR3 BIT FIELDS */
+#define IRQ_UART1_RX_POS  	0
+#define IRQ_UART1_TX_POS  	4
+#define IRQ_OPTSEC_POS    	8
+#define IRQ_CNT_POS       	12
+#define IRQ_MAC_RX_POS    	16
+#define IRQ_PORTH_INTA_POS	20
+#define IRQ_MAC_TX_POS    	24
+#define IRQ_PORTH_INTB_POS	28
+
+/* IAR4 BIT FIELDS */
+#define IRQ_TIMER0_POS		0
+#define IRQ_TIMER1_POS		4
+#define IRQ_TIMER2_POS		8
+#define IRQ_TIMER3_POS		12
+#define IRQ_TIMER4_POS		16
+#define IRQ_TIMER5_POS		20
+#define IRQ_TIMER6_POS		24
+#define IRQ_TIMER7_POS		28
+
+/* IAR5 BIT FIELDS */
+#define IRQ_PORTG_INTA_POS	0
+#define IRQ_PORTG_INTB_POS	4
+#define IRQ_MEM_DMA0_POS  	8
+#define IRQ_MEM_DMA1_POS  	12
+#define IRQ_WATCH_POS     	16
+#define IRQ_PORTF_INTA_POS	20
+#define IRQ_PORTF_INTB_POS	24
+#define IRQ_SPI0_ERROR_POS 	28
+
+/* IAR6 BIT FIELDS */
+#define IRQ_SPI1_ERROR_POS  	0
+#define IRQ_RSI_INT0_POS   	12
+#define IRQ_RSI_INT1_POS   	16
+#define IRQ_PWM_TRIP_POS   	20
+#define IRQ_PWM_SYNC_POS   	24
+#define IRQ_PTP_STAT_POS    	28
+
+#endif				/* _BF518_IRQ_H_ */
diff --git a/arch/blackfin/mach-bf518/include/mach/mem_map.h b/arch/blackfin/mach-bf518/include/mach/mem_map.h
new file mode 100644
index 000000000000..62bcc781bfaa
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/mem_map.h
@@ -0,0 +1,108 @@
+/*
+ * file:         include/asm-blackfin/mach-bf518/mem_map.h
+ * based on:	include/asm-blackfin/mach-bf527/mem_map.h
+ * author:	Bryan Wu <cooloney@kernel.org>
+ *
+ * created:
+ * description:
+ *	Memory MAP Common header file for blackfin BF518/6/4/2 of processors.
+ * rev:
+ *
+ * modified:
+ *
+ * bugs:         enter bugs at http://blackfin.uclinux.org/
+ *
+ * this program is free software; you can redistribute it and/or modify
+ * it under the terms of the gnu general public license as published by
+ * the free software foundation; either version 2, 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; see the file copying.
+ * if not, write to the free software foundation,
+ * 59 temple place - suite 330, boston, ma 02111-1307, usa.
+ */
+
+#ifndef _MEM_MAP_518_H_
+#define _MEM_MAP_518_H_
+
+#define COREMMR_BASE           0xFFE00000	/* Core MMRs */
+#define SYSMMR_BASE            0xFFC00000	/* System MMRs */
+
+/* Async Memory Banks */
+#define ASYNC_BANK3_BASE	0x20300000	/* Async Bank 3 */
+#define ASYNC_BANK3_SIZE	0x00100000	/* 1M */
+#define ASYNC_BANK2_BASE	0x20200000	/* Async Bank 2 */
+#define ASYNC_BANK2_SIZE	0x00100000	/* 1M */
+#define ASYNC_BANK1_BASE	0x20100000	/* Async Bank 1 */
+#define ASYNC_BANK1_SIZE	0x00100000	/* 1M */
+#define ASYNC_BANK0_BASE	0x20000000	/* Async Bank 0 */
+#define ASYNC_BANK0_SIZE	0x00100000	/* 1M */
+
+/* Boot ROM Memory */
+
+#define BOOT_ROM_START		0xEF000000
+#define BOOT_ROM_LENGTH		0x8000
+
+/* Level 1 Memory */
+
+/* Memory Map for ADSP-BF518/6/4/2 processors */
+
+#ifdef CONFIG_BFIN_ICACHE
+#define BFIN_ICACHESIZE		(16 * 1024)
+#else
+#define BFIN_ICACHESIZE		(0)
+#endif
+
+#define L1_CODE_START		0xFFA00000
+#define L1_DATA_A_START		0xFF800000
+#define L1_DATA_B_START		0xFF900000
+
+#define L1_CODE_LENGTH		0xC000
+
+#ifdef CONFIG_BFIN_DCACHE
+
+#ifdef CONFIG_BFIN_DCACHE_BANKA
+#define DMEM_CNTR (ACACHE_BSRAM | ENDCPLB | PORT_PREF0)
+#define L1_DATA_A_LENGTH	(0x8000 - 0x4000)
+#define L1_DATA_B_LENGTH	0x8000
+#define BFIN_DCACHESIZE		(16 * 1024)
+#define BFIN_DSUPBANKS		1
+#else
+#define DMEM_CNTR (ACACHE_BCACHE | ENDCPLB | PORT_PREF0)
+#define L1_DATA_A_LENGTH	(0x8000 - 0x4000)
+#define L1_DATA_B_LENGTH	(0x8000 - 0x4000)
+#define BFIN_DCACHESIZE		(32 * 1024)
+#define BFIN_DSUPBANKS		2
+#endif
+
+#else
+#define DMEM_CNTR (ASRAM_BSRAM | ENDCPLB | PORT_PREF0)
+#define L1_DATA_A_LENGTH	0x8000
+#define L1_DATA_B_LENGTH	0x8000
+#define BFIN_DCACHESIZE		0
+#define BFIN_DSUPBANKS		0
+#endif				/*CONFIG_BFIN_DCACHE */
+
+/* Level 2 Memory - none */
+
+#define L2_START	0
+#define L2_LENGTH	0
+
+/* Scratch Pad Memory */
+
+#define L1_SCRATCH_START	0xFFB00000
+#define L1_SCRATCH_LENGTH	0x1000
+
+#define GET_PDA_SAFE(preg)		\
+	preg.l = _cpu_pda;		\
+	preg.h = _cpu_pda;
+
+#define GET_PDA(preg, dreg)	GET_PDA_SAFE(preg)
+
+#endif				/* _MEM_MAP_518_H_ */
diff --git a/arch/blackfin/mach-bf518/include/mach/portmux.h b/arch/blackfin/mach-bf518/include/mach/portmux.h
new file mode 100644
index 000000000000..ac16d54734d4
--- /dev/null
+++ b/arch/blackfin/mach-bf518/include/mach/portmux.h
@@ -0,0 +1,188 @@
+#ifndef _MACH_PORTMUX_H_
+#define _MACH_PORTMUX_H_
+
+#define MAX_RESOURCES 	MAX_BLACKFIN_GPIOS
+
+/* EMAC MII/RMII Port Mux */
+#define P_MII0_ETxD2	(P_DEFINED | P_IDENT(GPIO_PF0) | P_FUNCT(0))
+#define P_MII0_ERxD2	(P_DEFINED | P_IDENT(GPIO_PF1) | P_FUNCT(0))
+#define P_MII0_ETxD3	(P_DEFINED | P_IDENT(GPIO_PF2) | P_FUNCT(0))
+#define P_MII0_ERxD3	(P_DEFINED | P_IDENT(GPIO_PF3) | P_FUNCT(0))
+#define P_MII0_ERxCLK	(P_DEFINED | P_IDENT(GPIO_PF4) | P_FUNCT(0))
+#define P_MII0_ERxDV	(P_DEFINED | P_IDENT(GPIO_PF5) | P_FUNCT(0))
+#define P_MII0_COL	(P_DEFINED | P_IDENT(GPIO_PF6) | P_FUNCT(0))
+
+#define P_MII0_MDC	(P_DEFINED | P_IDENT(GPIO_PF8) | P_FUNCT(0))
+#define P_MII0_MDIO	(P_DEFINED | P_IDENT(GPIO_PF9) | P_FUNCT(0))
+#define P_MII0_ETxD0	(P_DEFINED | P_IDENT(GPIO_PF10) | P_FUNCT(0))
+#define P_MII0_ERxD0	(P_DEFINED | P_IDENT(GPIO_PF11) | P_FUNCT(0))
+#define P_MII0_ETxD1	(P_DEFINED | P_IDENT(GPIO_PF12) | P_FUNCT(0))
+#define P_MII0_ERxD1	(P_DEFINED | P_IDENT(GPIO_PF13) | P_FUNCT(0))
+#define P_MII0_ETxEN	(P_DEFINED | P_IDENT(GPIO_PF14) | P_FUNCT(0))
+#define P_MII0_PHYINT	(P_DEFINED | P_IDENT(GPIO_PF15) | P_FUNCT(0))
+#define P_MII0_CRS	(P_DEFINED | P_IDENT(GPIO_PG0) | P_FUNCT(0))
+#define P_MII0_ERxER	(P_DEFINED | P_IDENT(GPIO_PG1) | P_FUNCT(0))
+#define P_MII0_TxCLK	(P_DEFINED | P_IDENT(GPIO_PG2) | P_FUNCT(0))
+
+#define P_MII0 {\
+	P_MII0_ETxD0, \
+	P_MII0_ETxD1, \
+	P_MII0_ETxD2, \
+	P_MII0_ETxD3, \
+	P_MII0_ETxEN, \
+	P_MII0_TxCLK, \
+	P_MII0_PHYINT, \
+	P_MII0_COL, \
+	P_MII0_ERxD0, \
+	P_MII0_ERxD1, \
+	P_MII0_ERxD2, \
+	P_MII0_ERxD3, \
+	P_MII0_ERxDV, \
+	P_MII0_ERxCLK, \
+	P_MII0_ERxER, \
+	P_MII0_CRS, \
+	P_MII0_MDC, \
+	P_MII0_MDIO, 0}
+
+#define P_RMII0 {\
+	P_MII0_ETxD0, \
+	P_MII0_ETxD1, \
+	P_MII0_ETxEN, \
+	P_MII0_ERxD0, \
+	P_MII0_ERxD1, \
+	P_MII0_ERxER, \
+	P_MII0_TxCLK, \
+	P_MII0_PHYINT, \
+	P_MII0_CRS, \
+	P_MII0_MDC, \
+	P_MII0_MDIO, 0}
+
+/* PPI Port Mux */
+#define P_PPI0_D0	(P_DEFINED | P_IDENT(GPIO_PF0) | P_FUNCT(1))
+#define P_PPI0_D1	(P_DEFINED | P_IDENT(GPIO_PF1) | P_FUNCT(1))
+#define P_PPI0_D2	(P_DEFINED | P_IDENT(GPIO_PF2) | P_FUNCT(1))
+#define P_PPI0_D3	(P_DEFINED | P_IDENT(GPIO_PF3) | P_FUNCT(1))
+#define P_PPI0_D4	(P_DEFINED | P_IDENT(GPIO_PF4) | P_FUNCT(1))
+#define P_PPI0_D5	(P_DEFINED | P_IDENT(GPIO_PF5) | P_FUNCT(1))
+#define P_PPI0_D6	(P_DEFINED | P_IDENT(GPIO_PF6) | P_FUNCT(1))
+#define P_PPI0_D7	(P_DEFINED | P_IDENT(GPIO_PF7) | P_FUNCT(1))
+#define P_PPI0_D8	(P_DEFINED | P_IDENT(GPIO_PF8) | P_FUNCT(1))
+#define P_PPI0_D9	(P_DEFINED | P_IDENT(GPIO_PF9) | P_FUNCT(1))
+#define P_PPI0_D10	(P_DEFINED | P_IDENT(GPIO_PF10) | P_FUNCT(1))
+#define P_PPI0_D11	(P_DEFINED | P_IDENT(GPIO_PF11) | P_FUNCT(1))
+#define P_PPI0_D12	(P_DEFINED | P_IDENT(GPIO_PF12) | P_FUNCT(1))
+#define P_PPI0_D13	(P_DEFINED | P_IDENT(GPIO_PF13) | P_FUNCT(1))
+#define P_PPI0_D14	(P_DEFINED | P_IDENT(GPIO_PF14) | P_FUNCT(1))
+#define P_PPI0_D15	(P_DEFINED | P_IDENT(GPIO_PF15) | P_FUNCT(1))
+
+#define P_PPI0_CLK	(P_DEFINED | P_IDENT(GPIO_PG12) | P_FUNCT(1))
+#define P_PPI0_FS1	(P_DEFINED | P_IDENT(GPIO_PG13) | P_FUNCT(1))
+#define P_PPI0_FS2	(P_DEFINED | P_IDENT(GPIO_PG14) | P_FUNCT(1))
+#define P_PPI0_FS3	(P_DEFINED | P_IDENT(GPIO_PG15) | P_FUNCT(1))
+
+/* SPI Port Mux */
+#define P_SPI0_SS	(P_DEFINED | P_IDENT(GPIO_PG11) | P_FUNCT(0))
+#define P_SPI0_SCK	(P_DEFINED | P_IDENT(GPIO_PG12) | P_FUNCT(0))
+#define P_SPI0_MISO	(P_DEFINED | P_IDENT(GPIO_PG13) | P_FUNCT(0))
+#define P_SPI0_MOSI	(P_DEFINED | P_IDENT(GPIO_PG14) | P_FUNCT(0))
+
+#define P_SPI0_SSEL1	(P_DEFINED | P_IDENT(GPIO_PF7) | P_FUNCT(0))
+#define P_SPI0_SSEL2	(P_DEFINED | P_IDENT(GPIO_PG15) | P_FUNCT(0))
+#define P_SPI0_SSEL3	(P_DEFINED | P_IDENT(GPIO_PH4) | P_FUNCT(2))
+#define P_SPI0_SSEL4	(P_DEFINED | P_IDENT(GPIO_PG10) | P_FUNCT(2))
+#define P_SPI0_SSEL5	(P_DEFINED | P_IDENT(GPIO_PG3) | P_FUNCT(2))
+
+#define P_SPI1_SS	(P_DEFINED | P_IDENT(GPIO_PH0) | P_FUNCT(1))
+#define P_SPI1_SCK	(P_DEFINED | P_IDENT(GPIO_PH1) | P_FUNCT(1))
+#define P_SPI1_MISO	(P_DEFINED | P_IDENT(GPIO_PH2) | P_FUNCT(1))
+#define P_SPI1_MOSI	(P_DEFINED | P_IDENT(GPIO_PH3) | P_FUNCT(1))
+
+#define P_SPI1_SSEL1	(P_DEFINED | P_IDENT(GPIO_PH6) | P_FUNCT(2))
+#define P_SPI1_SSEL2	(P_DEFINED | P_IDENT(GPIO_PF0) | P_FUNCT(2))
+#define P_SPI1_SSEL3	(P_DEFINED | P_IDENT(GPIO_PG0) | P_FUNCT(2))
+#define P_SPI1_SSEL4	(P_DEFINED | P_IDENT(GPIO_PF8) | P_FUNCT(2))
+#define P_SPI1_SSEL5	(P_DEFINED | P_IDENT(GPIO_PG11) | P_FUNCT(2))
+
+/* SPORT Port Mux */
+#define P_SPORT0_DRPRI	(P_DEFINED | P_IDENT(GPIO_PG3) | P_FUNCT(0))
+#define P_SPORT0_RSCLK	(P_DEFINED | P_IDENT(GPIO_PG4) | P_FUNCT(0))
+#define P_SPORT0_RFS	(P_DEFINED | P_IDENT(GPIO_PG5) | P_FUNCT(0))
+#define P_SPORT0_TFS	(P_DEFINED | P_IDENT(GPIO_PG6) | P_FUNCT(0))
+#define P_SPORT0_DTPRI	(P_DEFINED | P_IDENT(GPIO_PG7) | P_FUNCT(0))
+#define P_SPORT0_TSCLK	(P_DEFINED | P_IDENT(GPIO_PG8) | P_FUNCT(0))
+#define P_SPORT0_DTSEC	(P_DEFINED | P_IDENT(GPIO_PG9) | P_FUNCT(0))
+#define P_SPORT0_DRSEC	(P_DEFINED | P_IDENT(GPIO_PG10) | P_FUNCT(0))
+
+#define P_SPORT1_DRPRI	(P_DEFINED | P_IDENT(GPIO_PH0) | P_FUNCT(0))
+#define P_SPORT1_RFS	(P_DEFINED | P_IDENT(GPIO_PH1) | P_FUNCT(0))
+#define P_SPORT1_RSCLK	(P_DEFINED | P_IDENT(GPIO_PH2) | P_FUNCT(0))
+#define P_SPORT1_DTPRI	(P_DEFINED | P_IDENT(GPIO_PH3) | P_FUNCT(0))
+#define P_SPORT1_TFS	(P_DEFINED | P_IDENT(GPIO_PH4) | P_FUNCT(0))
+#define P_SPORT1_TSCLK	(P_DEFINED | P_IDENT(GPIO_PH5) | P_FUNCT(0))
+#define P_SPORT1_DTSEC	(P_DEFINED | P_IDENT(GPIO_PH6) | P_FUNCT(0))
+#define P_SPORT1_DRSEC	(P_DEFINED | P_IDENT(GPIO_PH7) | P_FUNCT(0))
+
+/* UART Port Mux */
+#define P_UART0_TX	(P_DEFINED | P_IDENT(GPIO_PG9) | P_FUNCT(1))
+#define P_UART0_RX	(P_DEFINED | P_IDENT(GPIO_PG10) | P_FUNCT(1))
+
+#define P_UART1_TX	(P_DEFINED | P_IDENT(GPIO_PH6) | P_FUNCT(1))
+#define P_UART1_RX	(P_DEFINED | P_IDENT(GPIO_PH7) | P_FUNCT(1))
+
+/* Timer */
+#define P_TMRCLK	(P_DEFINED | P_IDENT(GPIO_PG5) | P_FUNCT(2))
+#define P_TMR0		(P_DEFINED | P_IDENT(GPIO_PG6) | P_FUNCT(2))
+#define P_TMR1		(P_DEFINED | P_IDENT(GPIO_PG7) | P_FUNCT(2))
+#define P_TMR2		(P_DEFINED | P_IDENT(GPIO_PF9) | P_FUNCT(2))
+#define P_TMR3		(P_DEFINED | P_IDENT(GPIO_PF10) | P_FUNCT(2))
+#define P_TMR4		(P_DEFINED | P_IDENT(GPIO_PG9) | P_FUNCT(2))
+#define P_TMR5		(P_DEFINED | P_IDENT(GPIO_PG4) | P_FUNCT(2))
+#define P_TMR6		(P_DEFINED | P_IDENT(GPIO_PG8) | P_FUNCT(2))
+#define P_TMR7		(P_DEFINED | P_IDENT(GPIO_PH7) | P_FUNCT(2))
+
+/* DMA */
+#define P_DMAR1		(P_DEFINED | P_IDENT(GPIO_PG1) | P_FUNCT(1))
+#define P_DMAR0		(P_DEFINED | P_IDENT(GPIO_PG2) | P_FUNCT(1))
+
+/* TWI */
+#define P_TWI0_SCL	(P_DONTCARE)
+#define P_TWI0_SDA	(P_DONTCARE)
+
+/* PWM */
+#define P_PWM0_AH		(P_DEFINED | P_IDENT(GPIO_PF1) | P_FUNCT(2))
+#define P_PWM0_AL		(P_DEFINED | P_IDENT(GPIO_PF2) | P_FUNCT(2))
+#define P_PWM0_BH		(P_DEFINED | P_IDENT(GPIO_PF3) | P_FUNCT(2))
+#define P_PWM0_BL		(P_DEFINED | P_IDENT(GPIO_PF4) | P_FUNCT(2))
+#define P_PWM0_CH		(P_DEFINED | P_IDENT(GPIO_PF5) | P_FUNCT(2))
+#define P_PWM0_CL		(P_DEFINED | P_IDENT(GPIO_PF6) | P_FUNCT(2))
+#define P_PWM0_SYNC		(P_DEFINED | P_IDENT(GPIO_PF7) | P_FUNCT(2))
+
+#define P_PWM1_AH		(P_DEFINED | P_IDENT(GPIO_PF11) | P_FUNCT(2))
+#define P_PWM1_AL		(P_DEFINED | P_IDENT(GPIO_PF12) | P_FUNCT(2))
+#define P_PWM1_BH		(P_DEFINED | P_IDENT(GPIO_PF13) | P_FUNCT(2))
+#define P_PWM1_BL		(P_DEFINED | P_IDENT(GPIO_PF14) | P_FUNCT(2))
+#define P_PWM1_CH		(P_DEFINED | P_IDENT(GPIO_PG1) | P_FUNCT(2))
+#define P_PWM1_CL		(P_DEFINED | P_IDENT(GPIO_PG2) | P_FUNCT(2))
+#define P_PWM1_SYNC		(P_DEFINED | P_IDENT(GPIO_PF15) | P_FUNCT(2))
+
+#define P_PWM_TRIPB		(P_DEFINED | P_IDENT(GPIO_PG14) | P_FUNCT(2))
+
+/* RSI */
+#define P_RSI_DATA0		(P_DEFINED | P_IDENT(GPIO_PG3) | P_FUNCT(1))
+#define P_RSI_DATA1		(P_DEFINED | P_IDENT(GPIO_PG4) | P_FUNCT(1))
+#define P_RSI_DATA2		(P_DEFINED | P_IDENT(GPIO_PG5) | P_FUNCT(1))
+#define P_RSI_DATA3		(P_DEFINED | P_IDENT(GPIO_PG6) | P_FUNCT(1))
+#define P_RSI_DATA4		(P_DEFINED | P_IDENT(GPIO_PH0) | P_FUNCT(2))
+#define P_RSI_DATA5		(P_DEFINED | P_IDENT(GPIO_PH1) | P_FUNCT(2))
+#define P_RSI_DATA6		(P_DEFINED | P_IDENT(GPIO_PH2) | P_FUNCT(2))
+#define P_RSI_DATA7		(P_DEFINED | P_IDENT(GPIO_PH3) | P_FUNCT(2))
+#define P_RSI_CMD		(P_DEFINED | P_IDENT(GPIO_PG7) | P_FUNCT(1))
+#define P_RSI_CLK		(P_DEFINED | P_IDENT(GPIO_PG8) | P_FUNCT(1))
+
+/* PTP */
+#define P_PTP_PPS		(P_DEFINED | P_IDENT(GPIO_PG12) | P_FUNCT(2))
+#define P_PTP_CLKOUT		(P_DEFINED | P_IDENT(GPIO_PG13) | P_FUNCT(2))
+
+#define P_HWAIT		(P_DEFINED | P_IDENT(GPIO_PG000000000) | P_FUNCT(1))
+
+#endif				/* _MACH_PORTMUX_H_ */
diff --git a/arch/blackfin/mach-bf518/ints-priority.c b/arch/blackfin/mach-bf518/ints-priority.c
new file mode 100644
index 000000000000..3151fd5501ca
--- /dev/null
+++ b/arch/blackfin/mach-bf518/ints-priority.c
@@ -0,0 +1,99 @@
+/*
+ * File:         arch/blackfin/mach-bf518/ints-priority.c
+ * Based on:     arch/blackfin/mach-bf527/ints-priority.c
+ * Author:       Bryan Wu <cooloney@kernel.org>
+ *
+ * Created:
+ * Description:  Set up the interrupt priorities
+ *
+ * Modified:
+ *               Copyright 2004-2007 Analog Devices Inc.
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#include <linux/module.h>
+#include <linux/irq.h>
+#include <asm/blackfin.h>
+
+void __init program_IAR(void)
+{
+	/* Program the IAR0 Register with the configured priority */
+	bfin_write_SIC_IAR0(((CONFIG_IRQ_PLL_WAKEUP - 7) << IRQ_PLL_WAKEUP_POS) |
+			((CONFIG_IRQ_DMA0_ERROR - 7) << IRQ_DMA0_ERROR_POS) |
+			((CONFIG_IRQ_DMAR0_BLK - 7) << IRQ_DMAR0_BLK_POS) |
+			((CONFIG_IRQ_DMAR1_BLK - 7) << IRQ_DMAR1_BLK_POS) |
+			((CONFIG_IRQ_DMAR0_OVR - 7) << IRQ_DMAR0_OVR_POS) |
+			((CONFIG_IRQ_DMAR1_OVR - 7) << IRQ_DMAR1_OVR_POS) |
+			((CONFIG_IRQ_PPI_ERROR - 7) << IRQ_PPI_ERROR_POS) |
+			((CONFIG_IRQ_MAC_ERROR - 7) << IRQ_MAC_ERROR_POS));
+
+
+	bfin_write_SIC_IAR1(((CONFIG_IRQ_SPORT0_ERROR - 7) << IRQ_SPORT0_ERROR_POS) |
+			((CONFIG_IRQ_SPORT1_ERROR - 7) << IRQ_SPORT1_ERROR_POS) |
+			((CONFIG_IRQ_PTP_ERROR - 7) << IRQ_PTP_ERROR_POS) |
+			((CONFIG_IRQ_UART0_ERROR - 7) << IRQ_UART0_ERROR_POS) |
+			((CONFIG_IRQ_UART1_ERROR - 7) << IRQ_UART1_ERROR_POS) |
+			((CONFIG_IRQ_RTC - 7) << IRQ_RTC_POS) |
+			((CONFIG_IRQ_PPI - 7) << IRQ_PPI_POS));
+
+	bfin_write_SIC_IAR2(((CONFIG_IRQ_SPORT0_RX - 7) << IRQ_SPORT0_RX_POS) |
+			((CONFIG_IRQ_SPORT0_TX - 7) << IRQ_SPORT0_TX_POS) |
+			((CONFIG_IRQ_SPORT1_RX - 7) << IRQ_SPORT1_RX_POS) |
+			((CONFIG_IRQ_SPORT1_TX - 7) << IRQ_SPORT1_TX_POS) |
+			((CONFIG_IRQ_TWI - 7) << IRQ_TWI_POS) |
+			((CONFIG_IRQ_SPI0 - 7) << IRQ_SPI0_POS) |
+			((CONFIG_IRQ_UART0_RX - 7) << IRQ_UART0_RX_POS) |
+			((CONFIG_IRQ_UART0_TX - 7) << IRQ_UART0_TX_POS));
+
+	bfin_write_SIC_IAR3(((CONFIG_IRQ_UART1_RX - 7) << IRQ_UART1_RX_POS) |
+			((CONFIG_IRQ_UART1_TX - 7) << IRQ_UART1_TX_POS) |
+			((CONFIG_IRQ_OPTSEC - 7) << IRQ_OPTSEC_POS) |
+			((CONFIG_IRQ_CNT - 7) << IRQ_CNT_POS) |
+			((CONFIG_IRQ_MAC_RX - 7) << IRQ_MAC_RX_POS) |
+			((CONFIG_IRQ_PORTH_INTA - 7) << IRQ_PORTH_INTA_POS) |
+			((CONFIG_IRQ_MAC_TX - 7) << IRQ_MAC_TX_POS) |
+			((CONFIG_IRQ_PORTH_INTB - 7) << IRQ_PORTH_INTB_POS));
+
+	bfin_write_SIC_IAR4(((CONFIG_IRQ_TIMER0 - 7) << IRQ_TIMER0_POS) |
+			((CONFIG_IRQ_TIMER1 - 7) << IRQ_TIMER1_POS) |
+			((CONFIG_IRQ_TIMER2 - 7) << IRQ_TIMER2_POS) |
+			((CONFIG_IRQ_TIMER3 - 7) << IRQ_TIMER3_POS) |
+			((CONFIG_IRQ_TIMER4 - 7) << IRQ_TIMER4_POS) |
+			((CONFIG_IRQ_TIMER5 - 7) << IRQ_TIMER5_POS) |
+			((CONFIG_IRQ_TIMER6 - 7) << IRQ_TIMER6_POS) |
+			((CONFIG_IRQ_TIMER7 - 7) << IRQ_TIMER7_POS));
+
+	bfin_write_SIC_IAR5(((CONFIG_IRQ_PORTG_INTA - 7) << IRQ_PORTG_INTA_POS) |
+			((CONFIG_IRQ_PORTG_INTB - 7) << IRQ_PORTG_INTB_POS) |
+			((CONFIG_IRQ_MEM_DMA0 - 7) << IRQ_MEM_DMA0_POS) |
+			((CONFIG_IRQ_MEM_DMA1 - 7) << IRQ_MEM_DMA1_POS) |
+			((CONFIG_IRQ_WATCH - 7) << IRQ_WATCH_POS) |
+			((CONFIG_IRQ_PORTF_INTA - 7) << IRQ_PORTF_INTA_POS) |
+			((CONFIG_IRQ_PORTF_INTB - 7) << IRQ_PORTF_INTB_POS) |
+			((CONFIG_IRQ_SPI0_ERROR - 7) << IRQ_SPI0_ERROR_POS));
+
+	bfin_write_SIC_IAR6(((CONFIG_IRQ_SPI1_ERROR - 7) << IRQ_SPI1_ERROR_POS) |
+			((CONFIG_IRQ_RSI_INT0 - 7) << IRQ_RSI_INT0_POS) |
+			((CONFIG_IRQ_RSI_INT1 - 7) << IRQ_RSI_INT1_POS) |
+			((CONFIG_IRQ_PWM_TRIP - 7) << IRQ_PWM_TRIP_POS) |
+			((CONFIG_IRQ_PWM_SYNC - 7) << IRQ_PWM_SYNC_POS) |
+			((CONFIG_IRQ_PTP_STAT - 7) << IRQ_PTP_STAT_POS));
+
+	SSYNC();
+}
diff --git a/arch/blackfin/mach-bf527/Kconfig b/arch/blackfin/mach-bf527/Kconfig
index 3cde4beeb214..8438ec6d6679 100644
--- a/arch/blackfin/mach-bf527/Kconfig
+++ b/arch/blackfin/mach-bf527/Kconfig
@@ -168,29 +168,29 @@ config IRQ_MAC_TX
 config IRQ_PORTH_INTB
 	int "IRQ_PORTH_INTB"
 	default 11
-config IRQ_TMR0
-	int "IRQ_TMR0"
-	default 12
-config IRQ_TMR1
-	int "IRQ_TMR1"
+config IRQ_TIMER0
+	int "IRQ_TIMER0"
+	default 8
+config IRQ_TIMER1
+	int "IRQ_TIMER1"
 	default 12
-config IRQ_TMR2
-	int "IRQ_TMR2"
+config IRQ_TIMER2
+	int "IRQ_TIMER2"
 	default 12
-config IRQ_TMR3
-	int "IRQ_TMR3"
+config IRQ_TIMER3
+	int "IRQ_TIMER3"
 	default 12
-config IRQ_TMR4
-	int "IRQ_TMR4"
+config IRQ_TIMER4
+	int "IRQ_TIMER4"
 	default 12
-config IRQ_TMR5
-	int "IRQ_TMR5"
+config IRQ_TIMER5
+	int "IRQ_TIMER5"
 	default 12
-config IRQ_TMR6
-	int "IRQ_TMR6"
+config IRQ_TIMER6
+	int "IRQ_TIMER6"
 	default 12
-config IRQ_TMR7
-	int "IRQ_TMR7"
+config IRQ_TIMER7
+	int "IRQ_TIMER7"
 	default 12
 config IRQ_PORTG_INTA
 	int "IRQ_PORTG_INTA"
diff --git a/arch/blackfin/mach-bf527/Makefile b/arch/blackfin/mach-bf527/Makefile
index 4eddb580319c..4a6cdafab8ce 100644
--- a/arch/blackfin/mach-bf527/Makefile
+++ b/arch/blackfin/mach-bf527/Makefile
@@ -2,6 +2,4 @@
 # arch/blackfin/mach-bf527/Makefile
 #
 
-extra-y := head.o
-
 obj-y := ints-priority.o dma.o
diff --git a/arch/blackfin/mach-bf527/boards/cm_bf527.c b/arch/blackfin/mach-bf527/boards/cm_bf527.c
index 9ea440bbb13d..a2c3578f4b6c 100644
--- a/arch/blackfin/mach-bf527/boards/cm_bf527.c
+++ b/arch/blackfin/mach-bf527/boards/cm_bf527.c
@@ -61,51 +61,40 @@ const char bfin_board_name[] = "Bluetechnix CM-BF527";
  *  Driver needs to know address, irq and flag pin.
  */
 
-#define ISP1761_BASE       0x203C0000
-#define ISP1761_IRQ        IRQ_PF7
-
 #if defined(CONFIG_USB_ISP1760_HCD) || defined(CONFIG_USB_ISP1760_HCD_MODULE)
-static struct resource bfin_isp1761_resources[] = {
+#include <linux/usb/isp1760.h>
+static struct resource bfin_isp1760_resources[] = {
 	[0] = {
-		.name	= "isp1761-regs",
-		.start  = ISP1761_BASE + 0x00000000,
-		.end    = ISP1761_BASE + 0x000fffff,
+		.start  = 0x203C0000,
+		.end    = 0x203C0000 + 0x000fffff,
 		.flags  = IORESOURCE_MEM,
 	},
 	[1] = {
-		.start  = ISP1761_IRQ,
-		.end    = ISP1761_IRQ,
+		.start  = IRQ_PF7,
+		.end    = IRQ_PF7,
 		.flags  = IORESOURCE_IRQ,
 	},
 };
 
-static struct platform_device bfin_isp1761_device = {
-	.name           = "isp1761",
-	.id             = 0,
-	.num_resources  = ARRAY_SIZE(bfin_isp1761_resources),
-	.resource       = bfin_isp1761_resources,
+static struct isp1760_platform_data isp1760_priv = {
+	.is_isp1761 = 0,
+	.port1_disable = 0,
+	.bus_width_16 = 1,
+	.port1_otg = 0,
+	.analog_oc = 0,
+	.dack_polarity_high = 0,
+	.dreq_polarity_high = 0,
 };
 
-static struct platform_device *bfin_isp1761_devices[] = {
-	&bfin_isp1761_device,
+static struct platform_device bfin_isp1760_device = {
+	.name           = "isp1760-hcd",
+	.id             = 0,
+	.dev = {
+		.platform_data = &isp1760_priv,
+	},
+	.num_resources  = ARRAY_SIZE(bfin_isp1760_resources),
+	.resource       = bfin_isp1760_resources,
 };
-
-int __init bfin_isp1761_init(void)
-{
-	unsigned int num_devices = ARRAY_SIZE(bfin_isp1761_devices);
-
-	printk(KERN_INFO "%s(): registering device resources\n", __func__);
-	set_irq_type(ISP1761_IRQ, IRQF_TRIGGER_FALLING);
-
-	return platform_add_devices(bfin_isp1761_devices, num_devices);
-}
-
-void __exit bfin_isp1761_exit(void)
-{
-	platform_device_unregister(&bfin_isp1761_device);
-}
-
-arch_initcall(bfin_isp1761_init);
 #endif
 
 #if defined(CONFIG_USB_MUSB_HDRC) || defined(CONFIG_USB_MUSB_HDRC_MODULE)
@@ -132,8 +121,8 @@ static struct musb_hdrc_config musb_config = {
 	.dyn_fifo	= 0,
 	.soft_con	= 1,
 	.dma		= 1,
-	.num_eps	= 7,
-	.dma_channels	= 7,
+	.num_eps	= 8,
+	.dma_channels	= 8,
 	.gpio_vrsel	= GPIO_PF11,
 };
 
@@ -728,30 +717,59 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
 #endif
 #ifdef CONFIG_BFIN_SIR1
+static struct resource bfin_sir1_resources[] = {
 	{
 		.start = 0xFFC02000,
 		.end = 0xFFC020FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART1_RX,
+		.end = IRQ_UART1_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART1_RX,
+		.end = CH_UART1_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir1_device = {
 	.name = "bfin_sir",
-	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_sir1_resources),
+	.resource = bfin_sir1_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
 static struct resource bfin_twi0_resource[] = {
@@ -885,6 +903,10 @@ static struct platform_device *stamp_devices[] __initdata = {
 	&isp1362_hcd_device,
 #endif
 
+#if defined(CONFIG_USB_ISP1760_HCD) || defined(CONFIG_USB_ISP1760_HCD_MODULE)
+	&bfin_isp1760_device,
+#endif
+
 #if defined(CONFIG_USB_MUSB_HDRC) || defined(CONFIG_USB_MUSB_HDRC_MODULE)
 	&musb_device,
 #endif
@@ -918,7 +940,12 @@ static struct platform_device *stamp_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#ifdef CONFIG_BFIN_SIR1
+	&bfin_sir1_device,
+#endif
 #endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
diff --git a/arch/blackfin/mach-bf527/boards/ezbrd.c b/arch/blackfin/mach-bf527/boards/ezbrd.c
index 36c87b6fbdec..0314bd3355eb 100644
--- a/arch/blackfin/mach-bf527/boards/ezbrd.c
+++ b/arch/blackfin/mach-bf527/boards/ezbrd.c
@@ -51,7 +51,7 @@
 /*
  * Name the Board for the /proc/cpuinfo
  */
-const char bfin_board_name[] = "BF526-EZBRD";
+const char bfin_board_name[] = "ADI BF526-EZBRD";
 
 /*
  *  Driver needs to know address, irq and flag pin.
@@ -81,8 +81,8 @@ static struct musb_hdrc_config musb_config = {
 	.dyn_fifo	= 0,
 	.soft_con	= 1,
 	.dma		= 1,
-	.num_eps	= 7,
-	.dma_channels	= 7,
+	.num_eps	= 8,
+	.dma_channels	= 8,
 	.gpio_vrsel	= GPIO_PG13,
 };
 
@@ -288,6 +288,30 @@ static const struct ad7877_platform_data bfin_ad7877_ts_info = {
 };
 #endif
 
+#if defined(CONFIG_TOUCHSCREEN_AD7879) || defined(CONFIG_TOUCHSCREEN_AD7879_MODULE)
+#include <linux/spi/ad7879.h>
+static const struct ad7879_platform_data bfin_ad7879_ts_info = {
+	.model			= 7879,	/* Model = AD7879 */
+	.x_plate_ohms		= 620,	/* 620 Ohm from the touch datasheet */
+	.pressure_max		= 10000,
+	.pressure_min		= 0,
+	.first_conversion_delay = 3,	/* wait 512us before do a first conversion */
+	.acquisition_time 	= 1,	/* 4us acquisition time per sample */
+	.median			= 2,	/* do 8 measurements */
+	.averaging 		= 1,	/* take the average of 4 middle samples */
+	.pen_down_acc_interval 	= 255,	/* 9.4 ms */
+	.gpio_output		= 1,	/* configure AUX/VBAT/GPIO as GPIO output */
+	.gpio_default 		= 1,	/* During initialization set GPIO = HIGH */
+};
+#endif
+
+#if defined(CONFIG_TOUCHSCREEN_AD7879_SPI) || defined(CONFIG_TOUCHSCREEN_AD7879_SPI_MODULE)
+static struct bfin5xx_spi_chip spi_ad7879_chip_info = {
+	.enable_dma = 0,
+	.bits_per_word = 16,
+};
+#endif
+
 #if defined(CONFIG_SND_SOC_WM8731) || defined(CONFIG_SND_SOC_WM8731_MODULE) \
 	 && defined(CONFIG_SND_SOC_WM8731_SPI)
 static struct bfin5xx_spi_chip spi_wm8731_chip_info = {
@@ -386,6 +410,18 @@ static struct spi_board_info bfin_spi_board_info[] __initdata = {
 		.controller_data = &spi_ad7877_chip_info,
 	},
 #endif
+#if defined(CONFIG_TOUCHSCREEN_AD7879_SPI) || defined(CONFIG_TOUCHSCREEN_AD7879_SPI_MODULE)
+	{
+		.modalias = "ad7879",
+		.platform_data = &bfin_ad7879_ts_info,
+		.irq = IRQ_PG0,
+		.max_speed_hz = 5000000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num = 0,
+		.chip_select = 5,
+		.controller_data = &spi_ad7879_chip_info,
+		.mode = SPI_CPHA | SPI_CPOL,
+	},
+#endif
 #if defined(CONFIG_SND_SOC_WM8731) || defined(CONFIG_SND_SOC_WM8731_MODULE) \
 	 && defined(CONFIG_SND_SOC_WM8731_SPI)
 	{
@@ -478,30 +514,59 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
 #endif
 #ifdef CONFIG_BFIN_SIR1
+static struct resource bfin_sir1_resources[] = {
 	{
 		.start = 0xFFC02000,
 		.end = 0xFFC020FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART1_RX,
+		.end = IRQ_UART1_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART1_RX,
+		.end = CH_UART1_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir1_device = {
 	.name = "bfin_sir",
-	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_sir1_resources),
+	.resource = bfin_sir1_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
 static struct resource bfin_twi0_resource[] = {
@@ -671,7 +736,12 @@ static struct platform_device *stamp_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#ifdef CONFIG_BFIN_SIR1
+	&bfin_sir1_device,
+#endif
 #endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
diff --git a/arch/blackfin/mach-bf527/boards/ezkit.c b/arch/blackfin/mach-bf527/boards/ezkit.c
index 8ee2b744e234..9454fb7b18c3 100644
--- a/arch/blackfin/mach-bf527/boards/ezkit.c
+++ b/arch/blackfin/mach-bf527/boards/ezkit.c
@@ -54,57 +54,46 @@
 /*
  * Name the Board for the /proc/cpuinfo
  */
-const char bfin_board_name[] = "ADDS-BF527-EZKIT";
+const char bfin_board_name[] = "ADI BF527-EZKIT";
 
 /*
  *  Driver needs to know address, irq and flag pin.
  */
 
-#define ISP1761_BASE       0x203C0000
-#define ISP1761_IRQ        IRQ_PF7
-
 #if defined(CONFIG_USB_ISP1760_HCD) || defined(CONFIG_USB_ISP1760_HCD_MODULE)
-static struct resource bfin_isp1761_resources[] = {
+#include <linux/usb/isp1760.h>
+static struct resource bfin_isp1760_resources[] = {
 	[0] = {
-		.name	= "isp1761-regs",
-		.start  = ISP1761_BASE + 0x00000000,
-		.end    = ISP1761_BASE + 0x000fffff,
+		.start  = 0x203C0000,
+		.end    = 0x203C0000 + 0x000fffff,
 		.flags  = IORESOURCE_MEM,
 	},
 	[1] = {
-		.start  = ISP1761_IRQ,
-		.end    = ISP1761_IRQ,
+		.start  = IRQ_PF7,
+		.end    = IRQ_PF7,
 		.flags  = IORESOURCE_IRQ,
 	},
 };
 
-static struct platform_device bfin_isp1761_device = {
-	.name           = "isp1761",
-	.id             = 0,
-	.num_resources  = ARRAY_SIZE(bfin_isp1761_resources),
-	.resource       = bfin_isp1761_resources,
+static struct isp1760_platform_data isp1760_priv = {
+	.is_isp1761 = 0,
+	.port1_disable = 0,
+	.bus_width_16 = 1,
+	.port1_otg = 0,
+	.analog_oc = 0,
+	.dack_polarity_high = 0,
+	.dreq_polarity_high = 0,
 };
 
-static struct platform_device *bfin_isp1761_devices[] = {
-	&bfin_isp1761_device,
+static struct platform_device bfin_isp1760_device = {
+	.name           = "isp1760-hcd",
+	.id             = 0,
+	.dev = {
+		.platform_data = &isp1760_priv,
+	},
+	.num_resources  = ARRAY_SIZE(bfin_isp1760_resources),
+	.resource       = bfin_isp1760_resources,
 };
-
-int __init bfin_isp1761_init(void)
-{
-	unsigned int num_devices = ARRAY_SIZE(bfin_isp1761_devices);
-
-	printk(KERN_INFO "%s(): registering device resources\n", __func__);
-	set_irq_type(ISP1761_IRQ, IRQF_TRIGGER_FALLING);
-
-	return platform_add_devices(bfin_isp1761_devices, num_devices);
-}
-
-void __exit bfin_isp1761_exit(void)
-{
-	platform_device_unregister(&bfin_isp1761_device);
-}
-
-arch_initcall(bfin_isp1761_init);
 #endif
 
 #if defined(CONFIG_USB_MUSB_HDRC) || defined(CONFIG_USB_MUSB_HDRC_MODULE)
@@ -131,8 +120,8 @@ static struct musb_hdrc_config musb_config = {
 	.dyn_fifo	= 0,
 	.soft_con	= 1,
 	.dma		= 1,
-	.num_eps	= 7,
-	.dma_channels	= 7,
+	.num_eps	= 8,
+	.dma_channels	= 8,
 	.gpio_vrsel	= GPIO_PG13,
 };
 
@@ -515,13 +504,6 @@ static struct bfin5xx_spi_chip ad9960_spi_chip_info = {
 };
 #endif
 
-#if defined(CONFIG_SPI_MMC) || defined(CONFIG_SPI_MMC_MODULE)
-static struct bfin5xx_spi_chip spi_mmc_chip_info = {
-	.enable_dma = 1,
-	.bits_per_word = 8,
-};
-#endif
-
 #if defined(CONFIG_PBX)
 static struct bfin5xx_spi_chip spi_si3xxx_chip_info = {
 	.ctl_reg	= 0x4, /* send zero */
@@ -552,6 +534,30 @@ static const struct ad7877_platform_data bfin_ad7877_ts_info = {
 };
 #endif
 
+#if defined(CONFIG_TOUCHSCREEN_AD7879) || defined(CONFIG_TOUCHSCREEN_AD7879_MODULE)
+#include <linux/spi/ad7879.h>
+static const struct ad7879_platform_data bfin_ad7879_ts_info = {
+	.model			= 7879,	/* Model = AD7879 */
+	.x_plate_ohms		= 620,	/* 620 Ohm from the touch datasheet */
+	.pressure_max		= 10000,
+	.pressure_min		= 0,
+	.first_conversion_delay = 3,	/* wait 512us before do a first conversion */
+	.acquisition_time 	= 1,	/* 4us acquisition time per sample */
+	.median			= 2,	/* do 8 measurements */
+	.averaging 		= 1,	/* take the average of 4 middle samples */
+	.pen_down_acc_interval 	= 255,	/* 9.4 ms */
+	.gpio_output		= 1,	/* configure AUX/VBAT/GPIO as GPIO output */
+	.gpio_default 		= 1,	/* During initialization set GPIO = HIGH */
+};
+#endif
+
+#if defined(CONFIG_TOUCHSCREEN_AD7879_SPI) || defined(CONFIG_TOUCHSCREEN_AD7879_SPI_MODULE)
+static struct bfin5xx_spi_chip spi_ad7879_chip_info = {
+	.enable_dma = 0,
+	.bits_per_word = 16,
+};
+#endif
+
 #if defined(CONFIG_SND_SOC_WM8731) || defined(CONFIG_SND_SOC_WM8731_MODULE) \
 	 && defined(CONFIG_SND_SOC_WM8731_SPI)
 static struct bfin5xx_spi_chip spi_wm8731_chip_info = {
@@ -613,26 +619,6 @@ static struct spi_board_info bfin_spi_board_info[] __initdata = {
 		.controller_data = &ad9960_spi_chip_info,
 	},
 #endif
-#if defined(CONFIG_SPI_MMC) || defined(CONFIG_SPI_MMC_MODULE)
-	{
-		.modalias = "spi_mmc_dummy",
-		.max_speed_hz = 25000000,     /* max spi clock (SCK) speed in HZ */
-		.bus_num = 0,
-		.chip_select = 0,
-		.platform_data = NULL,
-		.controller_data = &spi_mmc_chip_info,
-		.mode = SPI_MODE_3,
-	},
-	{
-		.modalias = "spi_mmc",
-		.max_speed_hz = 25000000,     /* max spi clock (SCK) speed in HZ */
-		.bus_num = 0,
-		.chip_select = CONFIG_SPI_MMC_CS_CHAN,
-		.platform_data = NULL,
-		.controller_data = &spi_mmc_chip_info,
-		.mode = SPI_MODE_3,
-	},
-#endif
 #if defined(CONFIG_PBX)
 	{
 		.modalias = "fxs-spi",
@@ -662,6 +648,18 @@ static struct spi_board_info bfin_spi_board_info[] __initdata = {
 		.controller_data = &spi_ad7877_chip_info,
 	},
 #endif
+#if defined(CONFIG_TOUCHSCREEN_AD7879_SPI) || defined(CONFIG_TOUCHSCREEN_AD7879_SPI_MODULE)
+	{
+		.modalias = "ad7879",
+		.platform_data = &bfin_ad7879_ts_info,
+		.irq = IRQ_PF8,
+		.max_speed_hz = 5000000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num = 0,
+		.chip_select = 3,
+		.controller_data = &spi_ad7879_chip_info,
+		.mode = SPI_CPHA | SPI_CPOL,
+	},
+#endif
 #if defined(CONFIG_SND_SOC_WM8731) || defined(CONFIG_SND_SOC_WM8731_MODULE) \
 	 && defined(CONFIG_SND_SOC_WM8731_SPI)
 	{
@@ -756,30 +754,59 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
 #endif
 #ifdef CONFIG_BFIN_SIR1
+static struct resource bfin_sir1_resources[] = {
 	{
 		.start = 0xFFC02000,
 		.end = 0xFFC020FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART1_RX,
+		.end = IRQ_UART1_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART1_RX,
+		.end = CH_UART1_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir1_device = {
 	.name = "bfin_sir",
-	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_sir1_resources),
+	.resource = bfin_sir1_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
 static struct resource bfin_twi0_resource[] = {
@@ -944,6 +971,10 @@ static struct platform_device *stamp_devices[] __initdata = {
 	&isp1362_hcd_device,
 #endif
 
+#if defined(CONFIG_USB_ISP1760_HCD) || defined(CONFIG_USB_ISP1760_HCD_MODULE)
+	&bfin_isp1760_device,
+#endif
+
 #if defined(CONFIG_USB_MUSB_HDRC) || defined(CONFIG_USB_MUSB_HDRC_MODULE)
 	&musb_device,
 #endif
@@ -985,7 +1016,12 @@ static struct platform_device *stamp_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#ifdef CONFIG_BFIN_SIR1
+	&bfin_sir1_device,
+#endif
 #endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
diff --git a/arch/blackfin/mach-bf527/dma.c b/arch/blackfin/mach-bf527/dma.c
index dfd080cda787..231877578243 100644
--- a/arch/blackfin/mach-bf527/dma.c
+++ b/arch/blackfin/mach-bf527/dma.c
@@ -31,7 +31,7 @@
 #include <asm/blackfin.h>
 #include <asm/dma.h>
 
-struct dma_register *dma_io_base_addr[MAX_BLACKFIN_DMA_CHANNEL] = {
+struct dma_register *dma_io_base_addr[MAX_DMA_CHANNELS] = {
 	(struct dma_register *) DMA0_NEXT_DESC_PTR,
 	(struct dma_register *) DMA1_NEXT_DESC_PTR,
 	(struct dma_register *) DMA2_NEXT_DESC_PTR,
diff --git a/arch/blackfin/mach-bf527/head.S b/arch/blackfin/mach-bf527/head.S
deleted file mode 100644
index 0eb1da85db73..000000000000
--- a/arch/blackfin/mach-bf527/head.S
+++ /dev/null
@@ -1,146 +0,0 @@
-/*
- * File:         arch/blackfin/mach-bf527/head.S
- * Based on:     arch/blackfin/mach-bf533/head.S
- * Author:       Jeff Dionne <jeff@uclinux.org> COPYRIGHT 1998 D. Jeff Dionne
- *
- * Created:      1998
- * Description:  Startup code for Blackfin BF537
- *
- * Modified:
- *               Copyright 2004-2007 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#include <linux/linkage.h>
-#include <linux/init.h>
-#include <asm/blackfin.h>
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-#include <asm/clocks.h>
-#include <mach/mem_init.h>
-#endif
-
-.section .l1.text
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-ENTRY(_start_dma_code)
-
-	/* Enable PHY CLK buffer output */
-	p0.h = hi(VR_CTL);
-	p0.l = lo(VR_CTL);
-	r0.l = w[p0];
-	bitset(r0, 14);
-	w[p0] = r0.l;
-	ssync;
-
-	p0.h = hi(SIC_IWR0);
-	p0.l = lo(SIC_IWR0);
-	r0.l = 0x1;
-	r0.h = 0x0;
-	[p0] = r0;
-	SSYNC;
-
-	/*
-	 *  Set PLL_CTL
-	 *   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
-	 *   - [8]     = BYPASS    : BYPASS the PLL, run CLKIN into CCLK/SCLK
-	 *   - [7]     = output delay (add 200ps of delay to mem signals)
-	 *   - [6]     = input delay (add 200ps of input delay to mem signals)
-	 *   - [5]     = PDWN      : 1=All Clocks off
-	 *   - [3]     = STOPCK    : 1=Core Clock off
-	 *   - [1]     = PLL_OFF   : 1=Disable Power to PLL
-	 *   - [0]     = DF        : 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
-	 *   all other bits set to zero
-	 */
-
-	p0.h = hi(PLL_LOCKCNT);
-	p0.l = lo(PLL_LOCKCNT);
-	r0 = 0x300(Z);
-	w[p0] = r0.l;
-	ssync;
-
-	P2.H = hi(EBIU_SDGCTL);
-	P2.L = lo(EBIU_SDGCTL);
-	R0 = [P2];
-	BITSET (R0, 24);
-	[P2] = R0;
-	SSYNC;
-
-	r0 = CONFIG_VCO_MULT & 63;       /* Load the VCO multiplier         */
-	r0 = r0 << 9;                    /* Shift it over,                  */
-	r1 = CLKIN_HALF;                 /* Do we need to divide CLKIN by 2?*/
-	r0 = r1 | r0;
-	r1 = PLL_BYPASS;                 /* Bypass the PLL?                 */
-	r1 = r1 << 8;                    /* Shift it over                   */
-	r0 = r1 | r0;                    /* add them all together           */
-#ifdef ANOMALY_05000265
-	BITSET(r0, 15);                  /* Add 250 mV of hysteresis to SPORT input pins */
-#endif
-
-	p0.h = hi(PLL_CTL);
-	p0.l = lo(PLL_CTL);              /* Load the address                */
-	cli r2;                          /* Disable interrupts              */
-	ssync;
-	w[p0] = r0.l;                    /* Set the value                   */
-	idle;                            /* Wait for the PLL to stablize    */
-	sti r2;                          /* Enable interrupts               */
-
-.Lcheck_again:
-	p0.h = hi(PLL_STAT);
-	p0.l = lo(PLL_STAT);
-	R0 = W[P0](Z);
-	CC = BITTST(R0,5);
-	if ! CC jump .Lcheck_again;
-
-	/* Configure SCLK & CCLK Dividers */
-	r0 = (CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
-	p0.h = hi(PLL_DIV);
-	p0.l = lo(PLL_DIV);
-	w[p0] = r0.l;
-	ssync;
-
-	p0.l = lo(EBIU_SDRRC);
-	p0.h = hi(EBIU_SDRRC);
-	r0 = mem_SDRRC;
-	w[p0] = r0.l;
-	ssync;
-
-	P2.H = hi(EBIU_SDGCTL);
-	P2.L = lo(EBIU_SDGCTL);
-	R0 = [P2];
-	BITCLR (R0, 24);
-	p0.h = hi(EBIU_SDSTAT);
-	p0.l = lo(EBIU_SDSTAT);
-	r2.l = w[p0];
-	cc = bittst(r2,3);
-	if !cc jump .Lskip;
-	NOP;
-	BITSET (R0, 23);
-.Lskip:
-	[P2] = R0;
-	SSYNC;
-
-	R0.L = lo(mem_SDGCTL);
-	R0.H = hi(mem_SDGCTL);
-	R1 = [p2];
-	R1 = R1 | R0;
-	[P2] = R1;
-	SSYNC;
-
-	RTS;
-ENDPROC(_start_dma_code)
-#endif /* CONFIG_BFIN_KERNEL_CLOCK */
diff --git a/arch/blackfin/mach-bf527/include/mach/anomaly.h b/arch/blackfin/mach-bf527/include/mach/anomaly.h
index 62373e61c585..035e8d835058 100644
--- a/arch/blackfin/mach-bf527/include/mach/anomaly.h
+++ b/arch/blackfin/mach-bf527/include/mach/anomaly.h
@@ -28,7 +28,7 @@
 /* Multi-Issue Instruction with dsp32shiftimm in slot1 and P-reg Store in slot2 Not Supported */
 #define ANOMALY_05000074 (1)
 /* DMA_RUN Bit Is Not Valid after a Peripheral Receive Channel DMA Stops */
-#define ANOMALY_05000119 (1)
+#define ANOMALY_05000119 (1)	/* note: brokenness is noted in documentation, not anomaly sheet */
 /* Rx.H Cannot Be Used to Access 16-bit System MMR Registers */
 #define ANOMALY_05000122 (1)
 /* Spurious Hardware Error from an Access in the Shadow of a Conditional Branch */
@@ -37,8 +37,6 @@
 #define ANOMALY_05000265 (1)
 /* False Hardware Errors Caused by Fetches at the Boundary of Reserved Memory */
 #define ANOMALY_05000310 (1)
-/* Errors when SSYNC, CSYNC, or Loads to LT, LB and LC Registers Are Interrupted */
-#define ANOMALY_05000312 (ANOMALY_BF527)
 /* PPI Is Level-Sensitive on First Transfer In Single Frame Sync Modes */
 #define ANOMALY_05000313 (__SILICON_REVISION__ < 2)
 /* Incorrect Access of OTP_STATUS During otp_write() Function */
@@ -153,6 +151,10 @@
 #define ANOMALY_05000430 (ANOMALY_BF527 && __SILICON_REVISION__ > 1)
 /* bfrom_SysControl() Does Not Clear SIC_IWR1 Before Executing PLL Programming Sequence */
 #define ANOMALY_05000432 (ANOMALY_BF526)
+/* Certain SIC Registers are not Reset After Soft or Core Double Fault Reset */
+#define ANOMALY_05000435 ((ANOMALY_BF526 && __SILICON_REVISION__ < 1) || ANOMALY_BF527)
+/* IFLUSH Instruction at End of Hardware Loop Causes Infinite Stall */
+#define ANOMALY_05000443 (1)
 
 /* Anomalies that don't exist on this proc */
 #define ANOMALY_05000125 (0)
@@ -168,7 +170,9 @@
 #define ANOMALY_05000285 (0)
 #define ANOMALY_05000307 (0)
 #define ANOMALY_05000311 (0)
+#define ANOMALY_05000312 (0)
 #define ANOMALY_05000323 (0)
 #define ANOMALY_05000363 (0)
+#define ANOMALY_05000412 (0)
 
 #endif
diff --git a/arch/blackfin/mach-bf527/include/mach/bf527.h b/arch/blackfin/mach-bf527/include/mach/bf527.h
index 144f08d3f8ea..3832aab11e9a 100644
--- a/arch/blackfin/mach-bf527/include/mach/bf527.h
+++ b/arch/blackfin/mach-bf527/include/mach/bf527.h
@@ -110,7 +110,7 @@
 
 #ifdef CONFIG_BF527
 #define CPU "BF527"
-#define CPUID 0x27e4
+#define CPUID 0x27e0
 #endif
 #ifdef CONFIG_BF526
 #define CPU "BF526"
@@ -118,7 +118,7 @@
 #endif
 #ifdef CONFIG_BF525
 #define CPU "BF525"
-#define CPUID 0x27e4
+#define CPUID 0x27e0
 #endif
 #ifdef CONFIG_BF524
 #define CPU "BF524"
@@ -126,7 +126,7 @@
 #endif
 #ifdef CONFIG_BF523
 #define CPU "BF523"
-#define CPUID 0x27e4
+#define CPUID 0x27e0
 #endif
 #ifdef CONFIG_BF522
 #define CPU "BF522"
@@ -134,7 +134,7 @@
 #endif
 
 #ifndef CPU
-#error Unknown CPU type - This kernel doesn't seem to be configured properly
+#error "Unknown CPU type - This kernel doesn't seem to be configured properly"
 #endif
 
 #endif				/* __MACH_BF527_H__  */
diff --git a/arch/blackfin/mach-bf527/include/mach/bfin_sir.h b/arch/blackfin/mach-bf527/include/mach/bfin_sir.h
deleted file mode 100644
index cfd8ad4f1f2c..000000000000
--- a/arch/blackfin/mach-bf527/include/mach/bfin_sir.h
+++ /dev/null
@@ -1,142 +0,0 @@
-/*
- * Blackfin Infra-red Driver
- *
- * Copyright 2006-2008 Analog Devices Inc.
- *
- * Enter bugs at http://blackfin.uclinux.org/
- *
- * Licensed under the GPL-2 or later.
- *
- */
-
-#include <linux/serial.h>
-#include <asm/dma.h>
-#include <asm/portmux.h>
-
-#define SIR_UART_GET_CHAR(port)   bfin_read16((port)->membase + OFFSET_RBR)
-#define SIR_UART_GET_DLL(port)    bfin_read16((port)->membase + OFFSET_DLL)
-#define SIR_UART_GET_IER(port)    bfin_read16((port)->membase + OFFSET_IER)
-#define SIR_UART_GET_DLH(port)    bfin_read16((port)->membase + OFFSET_DLH)
-#define SIR_UART_GET_IIR(port)    bfin_read16((port)->membase + OFFSET_IIR)
-#define SIR_UART_GET_LCR(port)    bfin_read16((port)->membase + OFFSET_LCR)
-#define SIR_UART_GET_GCTL(port)   bfin_read16((port)->membase + OFFSET_GCTL)
-
-#define SIR_UART_PUT_CHAR(port, v) bfin_write16(((port)->membase + OFFSET_THR), v)
-#define SIR_UART_PUT_DLL(port, v)  bfin_write16(((port)->membase + OFFSET_DLL), v)
-#define SIR_UART_PUT_IER(port, v)  bfin_write16(((port)->membase + OFFSET_IER), v)
-#define SIR_UART_PUT_DLH(port, v)  bfin_write16(((port)->membase + OFFSET_DLH), v)
-#define SIR_UART_PUT_LCR(port, v)  bfin_write16(((port)->membase + OFFSET_LCR), v)
-#define SIR_UART_PUT_GCTL(port, v) bfin_write16(((port)->membase + OFFSET_GCTL), v)
-
-#ifdef CONFIG_SIR_BFIN_DMA
-struct dma_rx_buf {
-	char *buf;
-	int head;
-	int tail;
-	};
-#endif /* CONFIG_SIR_BFIN_DMA */
-
-struct bfin_sir_port {
-	unsigned char __iomem   *membase;
-	unsigned int            irq;
-	unsigned int            lsr;
-	unsigned long           clk;
-	struct net_device       *dev;
-#ifdef CONFIG_SIR_BFIN_DMA
-	int                     tx_done;
-	struct dma_rx_buf       rx_dma_buf;
-	struct timer_list       rx_dma_timer;
-	int                     rx_dma_nrows;
-#endif /* CONFIG_SIR_BFIN_DMA */
-	unsigned int            tx_dma_channel;
-	unsigned int            rx_dma_channel;
-};
-
-struct bfin_sir_port sir_ports[BFIN_UART_NR_PORTS];
-
-struct bfin_sir_port_res {
-	unsigned long   base_addr;
-	int             irq;
-	unsigned int    rx_dma_channel;
-	unsigned int    tx_dma_channel;
-};
-
-struct bfin_sir_port_res bfin_sir_port_resource[] = {
-#ifdef CONFIG_BFIN_SIR0
-	{
-	0xFFC00400,
-	IRQ_UART0_RX,
-	CH_UART0_RX,
-	CH_UART0_TX,
-	},
-#endif
-#ifdef CONFIG_BFIN_SIR1
-	{
-	0xFFC02000,
-	IRQ_UART1_RX,
-	CH_UART1_RX,
-	CH_UART1_TX,
-	},
-#endif
-};
-
-int nr_sirs = ARRAY_SIZE(bfin_sir_port_resource);
-
-struct bfin_sir_self {
-	struct bfin_sir_port    *sir_port;
-	spinlock_t              lock;
-	unsigned int            open;
-	int                     speed;
-	int                     newspeed;
-
-	struct sk_buff          *txskb;
-	struct sk_buff          *rxskb;
-	struct net_device_stats stats;
-	struct device           *dev;
-	struct irlap_cb         *irlap;
-	struct qos_info         qos;
-
-	iobuff_t                tx_buff;
-	iobuff_t                rx_buff;
-
-	struct work_struct      work;
-	int                     mtt;
-};
-
-static inline unsigned int SIR_UART_GET_LSR(struct bfin_sir_port *port)
-{
-	unsigned int lsr = bfin_read16(port->membase + OFFSET_LSR);
-	port->lsr |= (lsr & (BI|FE|PE|OE));
-	return lsr | port->lsr;
-}
-
-static inline void SIR_UART_CLEAR_LSR(struct bfin_sir_port *port)
-{
-	port->lsr = 0;
-	bfin_read16(port->membase + OFFSET_LSR);
-}
-
-#define DRIVER_NAME "bfin_sir"
-
-static int bfin_sir_hw_init(void)
-{
-	int ret = -ENODEV;
-#ifdef CONFIG_BFIN_SIR0
-	ret = peripheral_request(P_UART0_TX, DRIVER_NAME);
-	if (ret)
-		return ret;
-	ret = peripheral_request(P_UART0_RX, DRIVER_NAME);
-	if (ret)
-		return ret;
-#endif
-
-#ifdef CONFIG_BFIN_SIR1
-	ret = peripheral_request(P_UART1_TX, DRIVER_NAME);
-	if (ret)
-		return ret;
-	ret = peripheral_request(P_UART1_RX, DRIVER_NAME);
-	if (ret)
-		return ret;
-#endif
-	return ret;
-}
diff --git a/arch/blackfin/mach-bf527/include/mach/cdefBF52x_base.h b/arch/blackfin/mach-bf527/include/mach/cdefBF52x_base.h
index 9a814b9a12b9..1fe76d8e0403 100644
--- a/arch/blackfin/mach-bf527/include/mach/cdefBF52x_base.h
+++ b/arch/blackfin/mach-bf527/include/mach/cdefBF52x_base.h
@@ -31,7 +31,6 @@
 #ifndef _CDEF_BF52X_H
 #define _CDEF_BF52X_H
 
-#include <asm/system.h>
 #include <asm/blackfin.h>
 
 #include "defBF52x_base.h"
@@ -43,57 +42,9 @@
 
 /* Clock and System Control	(0xFFC00000 - 0xFFC000FF)								*/
 #define bfin_read_PLL_CTL()			bfin_read16(PLL_CTL)
-/* Writing to PLL_CTL initiates a PLL relock sequence. */
-static __inline__ void bfin_write_PLL_CTL(unsigned int val)
-{
-	unsigned long flags, iwr0, iwr1;
-
-	if (val == bfin_read_PLL_CTL())
-		return;
-
-	local_irq_save(flags);
-	/* Enable the PLL Wakeup bit in SIC IWR */
-	iwr0 = bfin_read32(SIC_IWR0);
-	iwr1 = bfin_read32(SIC_IWR1);
-	/* Only allow PPL Wakeup) */
-	bfin_write32(SIC_IWR0, IWR_ENABLE(0));
-	bfin_write32(SIC_IWR1, 0);
-
-	bfin_write16(PLL_CTL, val);
-	SSYNC();
-	asm("IDLE;");
-
-	bfin_write32(SIC_IWR0, iwr0);
-	bfin_write32(SIC_IWR1, iwr1);
-	local_irq_restore(flags);
-}
 #define bfin_read_PLL_DIV()			bfin_read16(PLL_DIV)
 #define bfin_write_PLL_DIV(val)			bfin_write16(PLL_DIV, val)
 #define bfin_read_VR_CTL()			bfin_read16(VR_CTL)
-/* Writing to VR_CTL initiates a PLL relock sequence. */
-static __inline__ void bfin_write_VR_CTL(unsigned int val)
-{
-	unsigned long flags, iwr0, iwr1;
-
-	if (val == bfin_read_VR_CTL())
-		return;
-
-	local_irq_save(flags);
-	/* Enable the PLL Wakeup bit in SIC IWR */
-	iwr0 = bfin_read32(SIC_IWR0);
-	iwr1 = bfin_read32(SIC_IWR1);
-	/* Only allow PPL Wakeup) */
-	bfin_write32(SIC_IWR0, IWR_ENABLE(0));
-	bfin_write32(SIC_IWR1, 0);
-
-	bfin_write16(VR_CTL, val);
-	SSYNC();
-	asm("IDLE;");
-
-	bfin_write32(SIC_IWR0, iwr0);
-	bfin_write32(SIC_IWR1, iwr1);
-	local_irq_restore(flags);
-}
 #define bfin_read_PLL_STAT()			bfin_read16(PLL_STAT)
 #define bfin_write_PLL_STAT(val)		bfin_write16(PLL_STAT, val)
 #define bfin_read_PLL_LOCKCNT()			bfin_read16(PLL_LOCKCNT)
@@ -1201,4 +1152,57 @@ static __inline__ void bfin_write_VR_CTL(unsigned int val)
 #define bfin_read_NFC_DATA_RD()			bfin_read16(NFC_DATA_RD)
 #define bfin_write_NFC_DATA_RD(val)		bfin_write16(NFC_DATA_RD, val)
 
+/* These need to be last due to the cdef/linux inter-dependencies */
+#include <asm/irq.h>
+
+/* Writing to PLL_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_PLL_CTL(unsigned int val)
+{
+	unsigned long flags, iwr0, iwr1;
+
+	if (val == bfin_read_PLL_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr0 = bfin_read32(SIC_IWR0);
+	iwr1 = bfin_read32(SIC_IWR1);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SIC_IWR0, IWR_ENABLE(0));
+	bfin_write32(SIC_IWR1, 0);
+
+	bfin_write16(PLL_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SIC_IWR0, iwr0);
+	bfin_write32(SIC_IWR1, iwr1);
+	local_irq_restore_hw(flags);
+}
+
+/* Writing to VR_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_VR_CTL(unsigned int val)
+{
+	unsigned long flags, iwr0, iwr1;
+
+	if (val == bfin_read_VR_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr0 = bfin_read32(SIC_IWR0);
+	iwr1 = bfin_read32(SIC_IWR1);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SIC_IWR0, IWR_ENABLE(0));
+	bfin_write32(SIC_IWR1, 0);
+
+	bfin_write16(VR_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SIC_IWR0, iwr0);
+	bfin_write32(SIC_IWR1, iwr1);
+	local_irq_restore_hw(flags);
+}
+
 #endif /* _CDEF_BF52X_H */
diff --git a/arch/blackfin/mach-bf527/include/mach/dma.h b/arch/blackfin/mach-bf527/include/mach/dma.h
index 49dd693223e8..eb287da101a2 100644
--- a/arch/blackfin/mach-bf527/include/mach/dma.h
+++ b/arch/blackfin/mach-bf527/include/mach/dma.h
@@ -1,38 +1,14 @@
-/*
- * file:        include/asm-blackfin/mach-bf527/dma.h
- * based on:	include/asm-blackfin/mach-bf537/dma.h
- * author:	Michael Hennerich (michael.hennerich@analog.com)
+/* mach/dma.h - arch-specific DMA defines
  *
- * created:
- * description:
- *	system DMA map
- * rev:
+ * Copyright 2004-2008 Analog Devices Inc.
  *
- * modified:
- *
- *
- * bugs:         enter bugs at http://blackfin.uclinux.org/
- *
- * this program is free software; you can redistribute it and/or modify
- * it under the terms of the gnu general public license as published by
- * the free software foundation; either version 2, 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; see the file copying.
- * if not, write to the free software foundation,
- * 59 temple place - suite 330, boston, ma 02111-1307, usa.
+ * Licensed under the GPL-2 or later.
  */
 
 #ifndef _MACH_DMA_H_
 #define _MACH_DMA_H_
 
-#define MAX_BLACKFIN_DMA_CHANNEL 16
+#define MAX_DMA_CHANNELS 16
 
 #define CH_PPI 			0	/* PPI receive/transmit or NFC */
 #define CH_EMAC_RX 		1	/* Ethernet MAC receive or HOSTDP */
diff --git a/arch/blackfin/mach-bf527/include/mach/gpio.h b/arch/blackfin/mach-bf527/include/mach/gpio.h
new file mode 100644
index 000000000000..06b6eebf0d49
--- /dev/null
+++ b/arch/blackfin/mach-bf527/include/mach/gpio.h
@@ -0,0 +1,68 @@
+/*
+ * File: arch/blackfin/mach-bf527/include/mach/gpio.h
+ * Bugs: Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Copyright (C) 2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
+ */
+
+
+#ifndef _MACH_GPIO_H_
+#define _MACH_GPIO_H_
+
+#define MAX_BLACKFIN_GPIOS 48
+
+#define	GPIO_PF0	0
+#define	GPIO_PF1	1
+#define	GPIO_PF2	2
+#define	GPIO_PF3	3
+#define	GPIO_PF4	4
+#define	GPIO_PF5	5
+#define	GPIO_PF6	6
+#define	GPIO_PF7	7
+#define	GPIO_PF8	8
+#define	GPIO_PF9	9
+#define	GPIO_PF10	10
+#define	GPIO_PF11	11
+#define	GPIO_PF12	12
+#define	GPIO_PF13	13
+#define	GPIO_PF14	14
+#define	GPIO_PF15	15
+#define	GPIO_PG0	16
+#define	GPIO_PG1	17
+#define	GPIO_PG2	18
+#define	GPIO_PG3	19
+#define	GPIO_PG4	20
+#define	GPIO_PG5	21
+#define	GPIO_PG6	22
+#define	GPIO_PG7	23
+#define	GPIO_PG8	24
+#define	GPIO_PG9	25
+#define	GPIO_PG10      	26
+#define	GPIO_PG11      	27
+#define	GPIO_PG12      	28
+#define	GPIO_PG13      	29
+#define	GPIO_PG14      	30
+#define	GPIO_PG15      	31
+#define	GPIO_PH0	32
+#define	GPIO_PH1	33
+#define	GPIO_PH2	34
+#define	GPIO_PH3	35
+#define	GPIO_PH4	36
+#define	GPIO_PH5	37
+#define	GPIO_PH6	38
+#define	GPIO_PH7	39
+#define	GPIO_PH8	40
+#define	GPIO_PH9	41
+#define	GPIO_PH10      	42
+#define	GPIO_PH11      	43
+#define	GPIO_PH12      	44
+#define	GPIO_PH13      	45
+#define	GPIO_PH14      	46
+#define	GPIO_PH15      	47
+
+#define PORT_F GPIO_PF0
+#define PORT_G GPIO_PG0
+#define PORT_H GPIO_PH0
+
+#endif /* _MACH_GPIO_H_ */
diff --git a/arch/blackfin/mach-bf527/include/mach/irq.h b/arch/blackfin/mach-bf527/include/mach/irq.h
index 4e2b3f2020e5..8ea660d8151f 100644
--- a/arch/blackfin/mach-bf527/include/mach/irq.h
+++ b/arch/blackfin/mach-bf527/include/mach/irq.h
@@ -96,14 +96,14 @@
 #define IRQ_MAC_TX		BFIN_IRQ(30)	/* DMA2 Channel (MAC TX/NAND) */
 #define IRQ_NFC			BFIN_IRQ(30)	/* DMA2 Channel (MAC TX/NAND) */
 #define IRQ_PORTH_INTB		BFIN_IRQ(31)	/* Port H Interrupt B */
-#define IRQ_TMR0		BFIN_IRQ(32)	/* Timer 0 */
-#define IRQ_TMR1		BFIN_IRQ(33)	/* Timer 1 */
-#define IRQ_TMR2		BFIN_IRQ(34)	/* Timer 2 */
-#define IRQ_TMR3		BFIN_IRQ(35)	/* Timer 3 */
-#define IRQ_TMR4		BFIN_IRQ(36)	/* Timer 4 */
-#define IRQ_TMR5		BFIN_IRQ(37)	/* Timer 5 */
-#define IRQ_TMR6		BFIN_IRQ(38)	/* Timer 6 */
-#define IRQ_TMR7		BFIN_IRQ(39)	/* Timer 7 */
+#define IRQ_TIMER0		BFIN_IRQ(32)	/* Timer 0 */
+#define IRQ_TIMER1		BFIN_IRQ(33)	/* Timer 1 */
+#define IRQ_TIMER2		BFIN_IRQ(34)	/* Timer 2 */
+#define IRQ_TIMER3		BFIN_IRQ(35)	/* Timer 3 */
+#define IRQ_TIMER4		BFIN_IRQ(36)	/* Timer 4 */
+#define IRQ_TIMER5		BFIN_IRQ(37)	/* Timer 5 */
+#define IRQ_TIMER6		BFIN_IRQ(38)	/* Timer 6 */
+#define IRQ_TIMER7		BFIN_IRQ(39)	/* Timer 7 */
 #define IRQ_PORTG_INTA		BFIN_IRQ(40)	/* Port G Interrupt A */
 #define IRQ_PORTG_INTB		BFIN_IRQ(41)	/* Port G Interrupt B */
 #define IRQ_MEM_DMA0		BFIN_IRQ(42)	/* MDMA Stream 0 */
@@ -227,14 +227,14 @@
 #define IRQ_PORTH_INTB_POS	28
 
 /* IAR4 BIT FIELDS */
-#define IRQ_TMR0_POS		0
-#define IRQ_TMR1_POS		4
-#define IRQ_TMR2_POS		8
-#define IRQ_TMR3_POS		12
-#define IRQ_TMR4_POS		16
-#define IRQ_TMR5_POS		20
-#define IRQ_TMR6_POS		24
-#define IRQ_TMR7_POS		28
+#define IRQ_TIMER0_POS		0
+#define IRQ_TIMER1_POS		4
+#define IRQ_TIMER2_POS		8
+#define IRQ_TIMER3_POS		12
+#define IRQ_TIMER4_POS		16
+#define IRQ_TIMER5_POS		20
+#define IRQ_TIMER6_POS		24
+#define IRQ_TIMER7_POS		28
 
 /* IAR5 BIT FIELDS */
 #define IRQ_PORTG_INTA_POS	0
diff --git a/arch/blackfin/mach-bf527/include/mach/mem_map.h b/arch/blackfin/mach-bf527/include/mach/mem_map.h
index ef46dc991cd4..019e0017ad81 100644
--- a/arch/blackfin/mach-bf527/include/mach/mem_map.h
+++ b/arch/blackfin/mach-bf527/include/mach/mem_map.h
@@ -99,4 +99,10 @@
 #define L1_SCRATCH_START	0xFFB00000
 #define L1_SCRATCH_LENGTH	0x1000
 
+#define GET_PDA_SAFE(preg)		\
+	preg.l = _cpu_pda;		\
+	preg.h = _cpu_pda;
+
+#define GET_PDA(preg, dreg)	GET_PDA_SAFE(preg)
+
 #endif				/* _MEM_MAP_527_H_ */
diff --git a/arch/blackfin/mach-bf527/ints-priority.c b/arch/blackfin/mach-bf527/ints-priority.c
index 8a2367403d2b..f8c8acd73e30 100644
--- a/arch/blackfin/mach-bf527/ints-priority.c
+++ b/arch/blackfin/mach-bf527/ints-priority.c
@@ -69,14 +69,14 @@ void __init program_IAR(void)
 			((CONFIG_IRQ_MAC_TX - 7) << IRQ_MAC_TX_POS) |
 			((CONFIG_IRQ_PORTH_INTB - 7) << IRQ_PORTH_INTB_POS));
 
-	bfin_write_SIC_IAR4(((CONFIG_IRQ_TMR0 - 7) << IRQ_TMR0_POS) |
-			((CONFIG_IRQ_TMR1 - 7) << IRQ_TMR1_POS) |
-			((CONFIG_IRQ_TMR2 - 7) << IRQ_TMR2_POS) |
-			((CONFIG_IRQ_TMR3 - 7) << IRQ_TMR3_POS) |
-			((CONFIG_IRQ_TMR4 - 7) << IRQ_TMR4_POS) |
-			((CONFIG_IRQ_TMR5 - 7) << IRQ_TMR5_POS) |
-			((CONFIG_IRQ_TMR6 - 7) << IRQ_TMR6_POS) |
-			((CONFIG_IRQ_TMR7 - 7) << IRQ_TMR7_POS));
+	bfin_write_SIC_IAR4(((CONFIG_IRQ_TIMER0 - 7) << IRQ_TIMER0_POS) |
+			((CONFIG_IRQ_TIMER1 - 7) << IRQ_TIMER1_POS) |
+			((CONFIG_IRQ_TIMER2 - 7) << IRQ_TIMER2_POS) |
+			((CONFIG_IRQ_TIMER3 - 7) << IRQ_TIMER3_POS) |
+			((CONFIG_IRQ_TIMER4 - 7) << IRQ_TIMER4_POS) |
+			((CONFIG_IRQ_TIMER5 - 7) << IRQ_TIMER5_POS) |
+			((CONFIG_IRQ_TIMER6 - 7) << IRQ_TIMER6_POS) |
+			((CONFIG_IRQ_TIMER7 - 7) << IRQ_TIMER7_POS));
 
 	bfin_write_SIC_IAR5(((CONFIG_IRQ_PORTG_INTA - 7) << IRQ_PORTG_INTA_POS) |
 			((CONFIG_IRQ_PORTG_INTB - 7) << IRQ_PORTG_INTB_POS) |
diff --git a/arch/blackfin/mach-bf533/Kconfig b/arch/blackfin/mach-bf533/Kconfig
index 76beb75f12da..14427de7d77f 100644
--- a/arch/blackfin/mach-bf533/Kconfig
+++ b/arch/blackfin/mach-bf533/Kconfig
@@ -59,7 +59,7 @@ config DMA7_UARTTX
 	default 10
 config TIMER0
 	int "TIMER0"
-	default 11
+	default 8
 config TIMER1
 	int "TIMER1"
 	default 11
diff --git a/arch/blackfin/mach-bf533/Makefile b/arch/blackfin/mach-bf533/Makefile
index aa9f2647ee0c..874840f76028 100644
--- a/arch/blackfin/mach-bf533/Makefile
+++ b/arch/blackfin/mach-bf533/Makefile
@@ -2,6 +2,4 @@
 # arch/blackfin/mach-bf533/Makefile
 #
 
-extra-y := head.o
-
 obj-y := ints-priority.o dma.o
diff --git a/arch/blackfin/mach-bf533/boards/H8606.c b/arch/blackfin/mach-bf533/boards/H8606.c
index 72ac3ac8ef76..0c66bf44cfab 100644
--- a/arch/blackfin/mach-bf533/boards/H8606.c
+++ b/arch/blackfin/mach-bf533/boards/H8606.c
@@ -313,23 +313,33 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir0_device = {
 	.name = "bfin_sir",
 	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_SERIAL_8250) || defined(CONFIG_SERIAL_8250_MODULE)
 
@@ -431,7 +441,9 @@ static struct platform_device *h8606_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
 #endif
 
 #if defined(CONFIG_KEYBOARD_OPENCORES) || defined(CONFIG_KEYBOARD_OPENCORES_MODULE)
diff --git a/arch/blackfin/mach-bf533/boards/blackstamp.c b/arch/blackfin/mach-bf533/boards/blackstamp.c
index d064ded87719..6ee607c259ac 100644
--- a/arch/blackfin/mach-bf533/boards/blackstamp.c
+++ b/arch/blackfin/mach-bf533/boards/blackstamp.c
@@ -212,23 +212,33 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir0_device = {
 	.name = "bfin_sir",
 	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_SERIAL_BFIN_SPORT) || defined(CONFIG_SERIAL_BFIN_SPORT_MODULE)
 static struct platform_device bfin_sport0_uart_device = {
@@ -353,7 +363,9 @@ static struct platform_device *stamp_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
 #endif
 
 #if defined(CONFIG_SERIAL_BFIN_SPORT) || defined(CONFIG_SERIAL_BFIN_SPORT_MODULE)
diff --git a/arch/blackfin/mach-bf533/boards/cm_bf533.c b/arch/blackfin/mach-bf533/boards/cm_bf533.c
index 575843f6d9ef..e7061c7e8c42 100644
--- a/arch/blackfin/mach-bf533/boards/cm_bf533.c
+++ b/arch/blackfin/mach-bf533/boards/cm_bf533.c
@@ -219,6 +219,19 @@ static struct platform_device smc91x_device = {
 };
 #endif
 
+static struct resource bfin_gpios_resources = {
+	.start = 0,
+	.end   = MAX_BLACKFIN_GPIOS - 1,
+	.flags = IORESOURCE_IRQ,
+};
+
+static struct platform_device bfin_gpios_device = {
+	.name = "simple-gpio",
+	.id = -1,
+	.num_resources = 1,
+	.resource = &bfin_gpios_resources,
+};
+
 #if defined(CONFIG_SERIAL_BFIN) || defined(CONFIG_SERIAL_BFIN_MODULE)
 static struct resource bfin_uart_resources[] = {
 	{
@@ -237,23 +250,33 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir0_device = {
 	.name = "bfin_sir",
 	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_SERIAL_BFIN_SPORT) || defined(CONFIG_SERIAL_BFIN_SPORT_MODULE)
 static struct platform_device bfin_sport0_uart_device = {
@@ -342,7 +365,9 @@ static struct platform_device *cm_bf533_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
 #endif
 
 #if defined(CONFIG_SERIAL_BFIN_SPORT) || defined(CONFIG_SERIAL_BFIN_SPORT_MODULE)
@@ -365,6 +390,8 @@ static struct platform_device *cm_bf533_devices[] __initdata = {
 #if defined(CONFIG_SPI_BFIN) || defined(CONFIG_SPI_BFIN_MODULE)
 	&bfin_spi0_device,
 #endif
+
+	&bfin_gpios_device,
 };
 
 static int __init cm_bf533_init(void)
diff --git a/arch/blackfin/mach-bf533/boards/ezkit.c b/arch/blackfin/mach-bf533/boards/ezkit.c
index cc2e7eeb1d5a..08cd0969de47 100644
--- a/arch/blackfin/mach-bf533/boards/ezkit.c
+++ b/arch/blackfin/mach-bf533/boards/ezkit.c
@@ -46,7 +46,7 @@
 /*
  * Name the Board for the /proc/cpuinfo
  */
-const char bfin_board_name[] = "ADDS-BF533-EZKIT";
+const char bfin_board_name[] = "ADI BF533-EZKIT";
 
 #if defined(CONFIG_RTC_DRV_BFIN) || defined(CONFIG_RTC_DRV_BFIN_MODULE)
 static struct platform_device rtc_device = {
@@ -236,23 +236,33 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir0_device = {
 	.name = "bfin_sir",
 	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_KEYBOARD_GPIO) || defined(CONFIG_KEYBOARD_GPIO_MODULE)
 #include <linux/input.h>
@@ -363,7 +373,9 @@ static struct platform_device *ezkit_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
 #endif
 
 #if defined(CONFIG_KEYBOARD_GPIO) || defined(CONFIG_KEYBOARD_GPIO_MODULE)
diff --git a/arch/blackfin/mach-bf533/boards/generic_board.c b/arch/blackfin/mach-bf533/boards/generic_board.c
index 82b1f6a60e3f..986eeec53b1f 100644
--- a/arch/blackfin/mach-bf533/boards/generic_board.c
+++ b/arch/blackfin/mach-bf533/boards/generic_board.c
@@ -72,6 +72,35 @@ static struct platform_device smc91x_device = {
 };
 #endif
 
+#if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
+#ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
+	{
+		.start = 0xFFC00400,
+		.end = 0xFFC004FF,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
+#endif
+#endif
+
 static struct platform_device *generic_board_devices[] __initdata = {
 #if defined(CONFIG_RTC_DRV_BFIN) || defined(CONFIG_RTC_DRV_BFIN_MODULE)
 	&rtc_device,
@@ -80,6 +109,12 @@ static struct platform_device *generic_board_devices[] __initdata = {
 #if defined(CONFIG_SMC91X) || defined(CONFIG_SMC91X_MODULE)
 	&smc91x_device,
 #endif
+
+#if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#endif
 };
 
 static int __init generic_board_init(void)
diff --git a/arch/blackfin/mach-bf533/boards/ip0x.c b/arch/blackfin/mach-bf533/boards/ip0x.c
index 5864892de314..e30b1b7d1442 100644
--- a/arch/blackfin/mach-bf533/boards/ip0x.c
+++ b/arch/blackfin/mach-bf533/boards/ip0x.c
@@ -197,23 +197,33 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir0_device = {
 	.name = "bfin_sir",
 	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_USB_ISP1362_HCD) || defined(CONFIG_USB_ISP1362_HCD_MODULE)
 static struct resource isp1362_hcd_resources[] = {
@@ -272,7 +282,9 @@ static struct platform_device *ip0x_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
 #endif
 
 #if defined(CONFIG_USB_ISP1362_HCD) || defined(CONFIG_USB_ISP1362_HCD_MODULE)
diff --git a/arch/blackfin/mach-bf533/boards/stamp.c b/arch/blackfin/mach-bf533/boards/stamp.c
index 050ffca53530..07f9ad1e189c 100644
--- a/arch/blackfin/mach-bf533/boards/stamp.c
+++ b/arch/blackfin/mach-bf533/boards/stamp.c
@@ -49,7 +49,7 @@
 /*
  * Name the Board for the /proc/cpuinfo
  */
-const char bfin_board_name[] = "ADDS-BF533-STAMP";
+const char bfin_board_name[] = "ADI BF533-STAMP";
 
 #if defined(CONFIG_RTC_DRV_BFIN) || defined(CONFIG_RTC_DRV_BFIN_MODULE)
 static struct platform_device rtc_device = {
@@ -118,7 +118,7 @@ static struct mtd_partition stamp_partitions[] = {
 		.offset = 0,
 	}, {
 		.name   = "linux kernel(nor)",
-		.size   = 0xE0000,
+		.size   = 0x180000,
 		.offset = MTDPART_OFS_APPEND,
 	}, {
 		.name   = "file system(nor)",
@@ -169,7 +169,7 @@ static struct mtd_partition bfin_spi_flash_partitions[] = {
 		.mask_flags = MTD_CAP_ROM
 	}, {
 		.name = "linux kernel(spi)",
-		.size = 0xe0000,
+		.size = 0x180000,
 		.offset = MTDPART_OFS_APPEND,
 	}, {
 		.name = "file system(spi)",
@@ -216,13 +216,6 @@ static struct bfin5xx_spi_chip spi_si3xxx_chip_info = {
 };
 #endif
 
-#if defined(CONFIG_SPI_MMC) || defined(CONFIG_SPI_MMC_MODULE)
-static struct bfin5xx_spi_chip spi_mmc_chip_info = {
-	.enable_dma = 1,
-	.bits_per_word = 8,
-};
-#endif
-
 #if defined(CONFIG_SPI_SPIDEV) || defined(CONFIG_SPI_SPIDEV_MODULE)
 static struct bfin5xx_spi_chip spidev_chip_info = {
 	.enable_dma = 0,
@@ -265,27 +258,6 @@ static struct spi_board_info bfin_spi_board_info[] __initdata = {
 	},
 #endif
 
-#if defined(CONFIG_SPI_MMC) || defined(CONFIG_SPI_MMC_MODULE)
-	{
-		.modalias = "spi_mmc_dummy",
-		.max_speed_hz = 20000000,     /* max spi clock (SCK) speed in HZ */
-		.bus_num = 0,
-		.chip_select = 0,
-		.platform_data = NULL,
-		.controller_data = &spi_mmc_chip_info,
-		.mode = SPI_MODE_3,
-	},
-	{
-		.modalias = "spi_mmc",
-		.max_speed_hz = 20000000,     /* max spi clock (SCK) speed in HZ */
-		.bus_num = 0,
-		.chip_select = CONFIG_SPI_MMC_CS_CHAN,
-		.platform_data = NULL,
-		.controller_data = &spi_mmc_chip_info,
-		.mode = SPI_MODE_3,
-	},
-#endif
-
 #if defined(CONFIG_PBX)
 	{
 		.modalias = "fxs-spi",
@@ -373,23 +345,33 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir0_device = {
 	.name = "bfin_sir",
 	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_SERIAL_BFIN_SPORT) || defined(CONFIG_SERIAL_BFIN_SPORT_MODULE)
 static struct platform_device bfin_sport0_uart_device = {
@@ -537,7 +519,9 @@ static struct platform_device *stamp_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
 #endif
 
 #if defined(CONFIG_SERIAL_BFIN_SPORT) || defined(CONFIG_SERIAL_BFIN_SPORT_MODULE)
diff --git a/arch/blackfin/mach-bf533/dma.c b/arch/blackfin/mach-bf533/dma.c
index 28655c1cb7dc..0a6eb8f24d98 100644
--- a/arch/blackfin/mach-bf533/dma.c
+++ b/arch/blackfin/mach-bf533/dma.c
@@ -31,7 +31,7 @@
 #include <asm/blackfin.h>
 #include <asm/dma.h>
 
-struct dma_register *dma_io_base_addr[MAX_BLACKFIN_DMA_CHANNEL] = {
+struct dma_register *dma_io_base_addr[MAX_DMA_CHANNELS] = {
 	(struct dma_register *) DMA0_NEXT_DESC_PTR,
 	(struct dma_register *) DMA1_NEXT_DESC_PTR,
 	(struct dma_register *) DMA2_NEXT_DESC_PTR,
diff --git a/arch/blackfin/mach-bf533/head.S b/arch/blackfin/mach-bf533/head.S
deleted file mode 100644
index 9fc95aaca439..000000000000
--- a/arch/blackfin/mach-bf533/head.S
+++ /dev/null
@@ -1,137 +0,0 @@
-/*
- * File:         arch/blackfin/mach-bf533/head.S
- * Based on:
- * Author:       Jeff Dionne <jeff@uclinux.org> COPYRIGHT 1998 D. Jeff Dionne
- *
- * Created:      1998
- * Description:  bf533 startup file
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#include <linux/linkage.h>
-#include <linux/init.h>
-#include <asm/blackfin.h>
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-#include <asm/clocks.h>
-#include <mach/mem_init.h>
-#endif
-
-.section .l1.text
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-ENTRY(_start_dma_code)
-	p0.h = hi(SIC_IWR);
-	p0.l = lo(SIC_IWR);
-	r0.l = 0x1;
-	r0.h = 0x0;
-	[p0] = r0;
-	SSYNC;
-
-	/*
-	 *  Set PLL_CTL
-	 *   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
-	 *   - [8]     = BYPASS    : BYPASS the PLL, run CLKIN into CCLK/SCLK
-	 *   - [7]     = output delay (add 200ps of delay to mem signals)
-	 *   - [6]     = input delay (add 200ps of input delay to mem signals)
-	 *   - [5]     = PDWN      : 1=All Clocks off
-	 *   - [3]     = STOPCK    : 1=Core Clock off
-	 *   - [1]     = PLL_OFF   : 1=Disable Power to PLL
-	 *   - [0]     = DF        : 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
-	 *   all other bits set to zero
-	 */
-
-	p0.h = hi(PLL_LOCKCNT);
-	p0.l = lo(PLL_LOCKCNT);
-	r0 = 0x300(Z);
-	w[p0] = r0.l;
-	ssync;
-
-	P2.H = hi(EBIU_SDGCTL);
-	P2.L = lo(EBIU_SDGCTL);
-	R0 = [P2];
-	BITSET (R0, 24);
-	[P2] = R0;
-	SSYNC;
-
-	r0 = CONFIG_VCO_MULT & 63;       /* Load the VCO multiplier         */
-	r0 = r0 << 9;                    /* Shift it over,                  */
-	r1 = CLKIN_HALF;                 /* Do we need to divide CLKIN by 2?*/
-	r0 = r1 | r0;
-	r1 = PLL_BYPASS;                 /* Bypass the PLL?                 */
-	r1 = r1 << 8;                    /* Shift it over                   */
-	r0 = r1 | r0;                    /* add them all together           */
-#ifdef ANOMALY_05000265
-	BITSET(r0, 15);                  /* Add 250 mV of hysteresis to SPORT input pins */
-#endif
-
-	p0.h = hi(PLL_CTL);
-	p0.l = lo(PLL_CTL);              /* Load the address                */
-	cli r2;                          /* Disable interrupts              */
-	ssync;
-	w[p0] = r0.l;                    /* Set the value                   */
-	idle;                            /* Wait for the PLL to stablize    */
-	sti r2;                          /* Enable interrupts               */
-
-.Lcheck_again:
-	p0.h = hi(PLL_STAT);
-	p0.l = lo(PLL_STAT);
-	R0 = W[P0](Z);
-	CC = BITTST(R0,5);
-	if ! CC jump .Lcheck_again;
-
-	/* Configure SCLK & CCLK Dividers */
-	r0 = (CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
-	p0.h = hi(PLL_DIV);
-	p0.l = lo(PLL_DIV);
-	w[p0] = r0.l;
-	ssync;
-
-	p0.l = lo(EBIU_SDRRC);
-	p0.h = hi(EBIU_SDRRC);
-	r0 = mem_SDRRC;
-	w[p0] = r0.l;
-	ssync;
-
-	P2.H = hi(EBIU_SDGCTL);
-	P2.L = lo(EBIU_SDGCTL);
-	R0 = [P2];
-	BITCLR (R0, 24);
-	p0.h = hi(EBIU_SDSTAT);
-	p0.l = lo(EBIU_SDSTAT);
-	r2.l = w[p0];
-	cc = bittst(r2,3);
-	if !cc jump .Lskip;
-	NOP;
-	BITSET (R0, 23);
-.Lskip:
-	[P2] = R0;
-	SSYNC;
-
-	R0.L = lo(mem_SDGCTL);
-	R0.H = hi(mem_SDGCTL);
-	R1 = [p2];
-	R1 = R1 | R0;
-	[P2] = R1;
-	SSYNC;
-
-	RTS;
-ENDPROC(_start_dma_code)
-#endif /* CONFIG_BFIN_KERNEL_CLOCK */
diff --git a/arch/blackfin/mach-bf533/include/mach/anomaly.h b/arch/blackfin/mach-bf533/include/mach/anomaly.h
index f544fc56959a..0d3a03429fb9 100644
--- a/arch/blackfin/mach-bf533/include/mach/anomaly.h
+++ b/arch/blackfin/mach-bf533/include/mach/anomaly.h
@@ -7,7 +7,7 @@
  */
 
 /* This file shoule be up to date with:
- *  - Revision D, 06/18/2008; ADSP-BF531/BF532/BF533 Blackfin Processor Anomaly List
+ *  - Revision E, 09/18/2008; ADSP-BF531/BF532/BF533 Blackfin Processor Anomaly List
  */
 
 #ifndef _MACH_ANOMALY_H_
@@ -194,6 +194,12 @@
 #define ANOMALY_05000403 (1)
 /* Speculative Fetches Can Cause Undesired External FIFO Operations */
 #define ANOMALY_05000416 (1)
+/* Multichannel SPORT Channel Misalignment Under Specific Configuration */
+#define ANOMALY_05000425 (1)
+/* Speculative Fetches of Indirect-Pointer Instructions Can Cause False Hardware Errors */
+#define ANOMALY_05000426 (1)
+/* IFLUSH Instruction at End of Hardware Loop Causes Infinite Stall */
+#define ANOMALY_05000443 (1)
 
 /* These anomalies have been "phased" out of analog.com anomaly sheets and are
  * here to show running on older silicon just isn't feasible.
@@ -273,5 +279,8 @@
 #define ANOMALY_05000323 (0)
 #define ANOMALY_05000353 (1)
 #define ANOMALY_05000386 (1)
+#define ANOMALY_05000412 (0)
+#define ANOMALY_05000432 (0)
+#define ANOMALY_05000435 (0)
 
 #endif
diff --git a/arch/blackfin/mach-bf533/include/mach/bf533.h b/arch/blackfin/mach-bf533/include/mach/bf533.h
index dfc8c1ad2d7a..cf4427cd3f72 100644
--- a/arch/blackfin/mach-bf533/include/mach/bf533.h
+++ b/arch/blackfin/mach-bf533/include/mach/bf533.h
@@ -145,7 +145,7 @@
 #endif
 #ifdef CONFIG_BF532
 #define CPU "BF532"
-#define CPUID 0x275A
+#define CPUID 0x27a5
 #endif
 #ifdef CONFIG_BF531
 #define CPU "BF531"
@@ -153,7 +153,7 @@
 #endif
 
 #ifndef CPU
-#error Unknown CPU type - This kernel doesn't seem to be configured properly
+#error "Unknown CPU type - This kernel doesn't seem to be configured properly"
 #endif
 
 #endif				/* __MACH_BF533_H__  */
diff --git a/arch/blackfin/mach-bf533/include/mach/bfin_sir.h b/arch/blackfin/mach-bf533/include/mach/bfin_sir.h
deleted file mode 100644
index 9bb87e9e2e9b..000000000000
--- a/arch/blackfin/mach-bf533/include/mach/bfin_sir.h
+++ /dev/null
@@ -1,125 +0,0 @@
-/*
- * Blackfin Infra-red Driver
- *
- * Copyright 2006-2008 Analog Devices Inc.
- *
- * Enter bugs at http://blackfin.uclinux.org/
- *
- * Licensed under the GPL-2 or later.
- *
- */
-
-#include <linux/serial.h>
-#include <asm/dma.h>
-#include <asm/portmux.h>
-
-#define SIR_UART_GET_CHAR(port)   bfin_read16((port)->membase + OFFSET_RBR)
-#define SIR_UART_GET_DLL(port)    bfin_read16((port)->membase + OFFSET_DLL)
-#define SIR_UART_GET_IER(port)    bfin_read16((port)->membase + OFFSET_IER)
-#define SIR_UART_GET_DLH(port)    bfin_read16((port)->membase + OFFSET_DLH)
-#define SIR_UART_GET_IIR(port)    bfin_read16((port)->membase + OFFSET_IIR)
-#define SIR_UART_GET_LCR(port)    bfin_read16((port)->membase + OFFSET_LCR)
-#define SIR_UART_GET_GCTL(port)   bfin_read16((port)->membase + OFFSET_GCTL)
-
-#define SIR_UART_PUT_CHAR(port, v) bfin_write16(((port)->membase + OFFSET_THR), v)
-#define SIR_UART_PUT_DLL(port, v)  bfin_write16(((port)->membase + OFFSET_DLL), v)
-#define SIR_UART_PUT_IER(port, v)  bfin_write16(((port)->membase + OFFSET_IER), v)
-#define SIR_UART_PUT_DLH(port, v)  bfin_write16(((port)->membase + OFFSET_DLH), v)
-#define SIR_UART_PUT_LCR(port, v)  bfin_write16(((port)->membase + OFFSET_LCR), v)
-#define SIR_UART_PUT_GCTL(port, v) bfin_write16(((port)->membase + OFFSET_GCTL), v)
-
-#ifdef CONFIG_SIR_BFIN_DMA
-struct dma_rx_buf {
-	char *buf;
-	int head;
-	int tail;
-	};
-#endif /* CONFIG_SIR_BFIN_DMA */
-
-struct bfin_sir_port {
-	unsigned char __iomem   *membase;
-	unsigned int            irq;
-	unsigned int            lsr;
-	unsigned long           clk;
-	struct net_device       *dev;
-#ifdef CONFIG_SIR_BFIN_DMA
-	int                     tx_done;
-	struct dma_rx_buf       rx_dma_buf;
-	struct timer_list       rx_dma_timer;
-	int                     rx_dma_nrows;
-#endif /* CONFIG_SIR_BFIN_DMA */
-	unsigned int            tx_dma_channel;
-	unsigned int            rx_dma_channel;
-};
-
-struct bfin_sir_port sir_ports[BFIN_UART_NR_PORTS];
-
-struct bfin_sir_port_res {
-	unsigned long   base_addr;
-	int             irq;
-	unsigned int    rx_dma_channel;
-	unsigned int    tx_dma_channel;
-};
-
-struct bfin_sir_port_res bfin_sir_port_resource[] = {
-#ifdef CONFIG_BFIN_SIR0
-	{
-	0xFFC00400,
-	IRQ_UART_RX,
-	CH_UART_RX,
-	CH_UART_TX,
-	},
-#endif
-};
-
-int nr_sirs = ARRAY_SIZE(bfin_sir_port_resource);
-
-struct bfin_sir_self {
-	struct bfin_sir_port    *sir_port;
-	spinlock_t              lock;
-	unsigned int            open;
-	int                     speed;
-	int                     newspeed;
-
-	struct sk_buff          *txskb;
-	struct sk_buff          *rxskb;
-	struct net_device_stats stats;
-	struct device           *dev;
-	struct irlap_cb         *irlap;
-	struct qos_info         qos;
-
-	iobuff_t                tx_buff;
-	iobuff_t                rx_buff;
-
-	struct work_struct      work;
-	int                     mtt;
-};
-
-static inline unsigned int SIR_UART_GET_LSR(struct bfin_sir_port *port)
-{
-	unsigned int lsr = bfin_read16(port->membase + OFFSET_LSR);
-	port->lsr |= (lsr & (BI|FE|PE|OE));
-	return lsr | port->lsr;
-}
-
-static inline void SIR_UART_CLEAR_LSR(struct bfin_sir_port *port)
-{
-	port->lsr = 0;
-	bfin_read16(port->membase + OFFSET_LSR);
-}
-
-#define DRIVER_NAME "bfin_sir"
-
-static int bfin_sir_hw_init(void)
-{
-	int ret = -ENODEV;
-#ifdef CONFIG_BFIN_SIR0
-	ret = peripheral_request(P_UART0_TX, DRIVER_NAME);
-	if (ret)
-		return ret;
-	ret = peripheral_request(P_UART0_RX, DRIVER_NAME);
-	if (ret)
-		return ret;
-#endif
-	return ret;
-}
diff --git a/arch/blackfin/mach-bf533/include/mach/blackfin.h b/arch/blackfin/mach-bf533/include/mach/blackfin.h
index d80971b4e3aa..045184f81a29 100644
--- a/arch/blackfin/mach-bf533/include/mach/blackfin.h
+++ b/arch/blackfin/mach-bf533/include/mach/blackfin.h
@@ -44,6 +44,13 @@
 
 #define BFIN_UART_NR_PORTS      1
 
+#define CH_UART_RX CH_UART0_RX
+#define CH_UART_TX CH_UART0_TX
+
+#define IRQ_UART_ERROR IRQ_UART0_ERROR
+#define IRQ_UART_RX    IRQ_UART0_RX
+#define IRQ_UART_TX    IRQ_UART0_TX
+
 #define OFFSET_THR              0x00	/* Transmit Holding register            */
 #define OFFSET_RBR              0x00	/* Receive Buffer register              */
 #define OFFSET_DLL              0x00	/* Divisor Latch (Low-Byte)             */
diff --git a/arch/blackfin/mach-bf533/include/mach/cdefBF532.h b/arch/blackfin/mach-bf533/include/mach/cdefBF532.h
index 3d8978a52c17..bbc3c8386d48 100644
--- a/arch/blackfin/mach-bf533/include/mach/cdefBF532.h
+++ b/arch/blackfin/mach-bf533/include/mach/cdefBF532.h
@@ -39,31 +39,8 @@
 /*include core specific register pointer definitions*/
 #include <asm/cdef_LPBlackfin.h>
 
-#include <asm/system.h>
-
 /* Clock and System Control (0xFFC0 0400-0xFFC0 07FF) */
 #define bfin_read_PLL_CTL()                  bfin_read16(PLL_CTL)
-/* Writing to PLL_CTL initiates a PLL relock sequence. */
-static __inline__ void bfin_write_PLL_CTL(unsigned int val)
-{
-	unsigned long flags, iwr;
-
-	if (val == bfin_read_PLL_CTL())
-		return;
-
-	local_irq_save(flags);
-	/* Enable the PLL Wakeup bit in SIC IWR */
-	iwr = bfin_read32(SIC_IWR);
-	/* Only allow PPL Wakeup) */
-	bfin_write32(SIC_IWR, IWR_ENABLE(0));
-
-	bfin_write16(PLL_CTL, val);
-	SSYNC();
-	asm("IDLE;");
-
-	bfin_write32(SIC_IWR, iwr);
-	local_irq_restore(flags);
-}
 #define bfin_read_PLL_STAT()                 bfin_read16(PLL_STAT)
 #define bfin_write_PLL_STAT(val)             bfin_write16(PLL_STAT,val)
 #define bfin_read_PLL_LOCKCNT()              bfin_read16(PLL_LOCKCNT)
@@ -72,27 +49,6 @@ static __inline__ void bfin_write_PLL_CTL(unsigned int val)
 #define bfin_read_PLL_DIV()                  bfin_read16(PLL_DIV)
 #define bfin_write_PLL_DIV(val)              bfin_write16(PLL_DIV,val)
 #define bfin_read_VR_CTL()                   bfin_read16(VR_CTL)
-/* Writing to VR_CTL initiates a PLL relock sequence. */
-static __inline__ void bfin_write_VR_CTL(unsigned int val)
-{
-	unsigned long flags, iwr;
-
-	if (val == bfin_read_VR_CTL())
-		return;
-
-	local_irq_save(flags);
-	/* Enable the PLL Wakeup bit in SIC IWR */
-	iwr = bfin_read32(SIC_IWR);
-	/* Only allow PPL Wakeup) */
-	bfin_write32(SIC_IWR, IWR_ENABLE(0));
-
-	bfin_write16(VR_CTL, val);
-	SSYNC();
-	asm("IDLE;");
-
-	bfin_write32(SIC_IWR, iwr);
-	local_irq_restore(flags);
-}
 
 /* System Interrupt Controller (0xFFC0 0C00-0xFFC0 0FFF) */
 #define bfin_read_SWRST()                    bfin_read16(SWRST)
@@ -178,50 +134,6 @@ static __inline__ void bfin_write_VR_CTL(unsigned int val)
 #define bfin_read_FIO_MASKB_T()              bfin_read16(FIO_MASKB_T)
 #define bfin_write_FIO_MASKB_T(val)          bfin_write16(FIO_MASKB_T,val)
 
-
-#if ANOMALY_05000311
-#define BFIN_WRITE_FIO_FLAG(name) \
-static __inline__ void bfin_write_FIO_FLAG_ ## name (unsigned short val)\
-{\
-	unsigned long flags;\
-	local_irq_save(flags);\
-	bfin_write16(FIO_FLAG_ ## name,val);\
-	bfin_read_CHIPID();\
-	local_irq_restore(flags);\
-}
-BFIN_WRITE_FIO_FLAG(D)
-BFIN_WRITE_FIO_FLAG(C)
-BFIN_WRITE_FIO_FLAG(S)
-BFIN_WRITE_FIO_FLAG(T)
-
-#define BFIN_READ_FIO_FLAG(name) \
-static __inline__ unsigned short bfin_read_FIO_FLAG_ ## name (void)\
-{\
-	unsigned long flags;\
-	unsigned short ret;\
-	local_irq_save(flags);\
-	ret = bfin_read16(FIO_FLAG_ ## name);\
-	bfin_read_CHIPID();\
-	local_irq_restore(flags);\
-	return ret;\
-}
-BFIN_READ_FIO_FLAG(D)
-BFIN_READ_FIO_FLAG(C)
-BFIN_READ_FIO_FLAG(S)
-BFIN_READ_FIO_FLAG(T)
-
-#else
-#define bfin_write_FIO_FLAG_D(val)           bfin_write16(FIO_FLAG_D,val)
-#define bfin_write_FIO_FLAG_C(val)           bfin_write16(FIO_FLAG_C,val)
-#define bfin_write_FIO_FLAG_S(val)           bfin_write16(FIO_FLAG_S,val)
-#define bfin_write_FIO_FLAG_T(val)           bfin_write16(FIO_FLAG_T,val)
-#define bfin_read_FIO_FLAG_T()               bfin_read16(FIO_FLAG_T)
-#define bfin_read_FIO_FLAG_C()               bfin_read16(FIO_FLAG_C)
-#define bfin_read_FIO_FLAG_S()               bfin_read16(FIO_FLAG_S)
-#define bfin_read_FIO_FLAG_D()               bfin_read16(FIO_FLAG_D)
-#endif
-
-
 /* DMA Controller */
 #define bfin_read_DMA0_CONFIG()              bfin_read16(DMA0_CONFIG)
 #define bfin_write_DMA0_CONFIG(val)          bfin_write16(DMA0_CONFIG,val)
@@ -764,4 +676,93 @@ BFIN_READ_FIO_FLAG(T)
 #define bfin_read_PPI_FRAME()                bfin_read16(PPI_FRAME)
 #define bfin_write_PPI_FRAME(val)            bfin_write16(PPI_FRAME,val)
 
+/* These need to be last due to the cdef/linux inter-dependencies */
+#include <asm/irq.h>
+
+#if ANOMALY_05000311
+#define BFIN_WRITE_FIO_FLAG(name) \
+static inline void bfin_write_FIO_FLAG_##name(unsigned short val) \
+{ \
+	unsigned long flags; \
+	local_irq_save_hw(flags); \
+	bfin_write16(FIO_FLAG_##name, val); \
+	bfin_read_CHIPID(); \
+	local_irq_restore_hw(flags); \
+}
+BFIN_WRITE_FIO_FLAG(D)
+BFIN_WRITE_FIO_FLAG(C)
+BFIN_WRITE_FIO_FLAG(S)
+BFIN_WRITE_FIO_FLAG(T)
+
+#define BFIN_READ_FIO_FLAG(name) \
+static inline u16 bfin_read_FIO_FLAG_##name(void) \
+{ \
+	unsigned long flags; \
+	u16 ret; \
+	local_irq_save_hw(flags); \
+	ret = bfin_read16(FIO_FLAG_##name); \
+	bfin_read_CHIPID(); \
+	local_irq_restore_hw(flags); \
+	return ret; \
+}
+BFIN_READ_FIO_FLAG(D)
+BFIN_READ_FIO_FLAG(C)
+BFIN_READ_FIO_FLAG(S)
+BFIN_READ_FIO_FLAG(T)
+
+#else
+#define bfin_write_FIO_FLAG_D(val)           bfin_write16(FIO_FLAG_D, val)
+#define bfin_write_FIO_FLAG_C(val)           bfin_write16(FIO_FLAG_C, val)
+#define bfin_write_FIO_FLAG_S(val)           bfin_write16(FIO_FLAG_S, val)
+#define bfin_write_FIO_FLAG_T(val)           bfin_write16(FIO_FLAG_T, val)
+#define bfin_read_FIO_FLAG_T()               bfin_read16(FIO_FLAG_T)
+#define bfin_read_FIO_FLAG_C()               bfin_read16(FIO_FLAG_C)
+#define bfin_read_FIO_FLAG_S()               bfin_read16(FIO_FLAG_S)
+#define bfin_read_FIO_FLAG_D()               bfin_read16(FIO_FLAG_D)
+#endif
+
+/* Writing to PLL_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_PLL_CTL(unsigned int val)
+{
+	unsigned long flags, iwr;
+
+	if (val == bfin_read_PLL_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr = bfin_read32(SIC_IWR);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SIC_IWR, IWR_ENABLE(0));
+
+	bfin_write16(PLL_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SIC_IWR, iwr);
+	local_irq_restore_hw(flags);
+}
+
+/* Writing to VR_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_VR_CTL(unsigned int val)
+{
+	unsigned long flags, iwr;
+
+	if (val == bfin_read_VR_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr = bfin_read32(SIC_IWR);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SIC_IWR, IWR_ENABLE(0));
+
+	bfin_write16(VR_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SIC_IWR, iwr);
+	local_irq_restore_hw(flags);
+}
+
 #endif				/* _CDEF_BF532_H */
diff --git a/arch/blackfin/mach-bf533/include/mach/dma.h b/arch/blackfin/mach-bf533/include/mach/dma.h
index bd9d5e94307d..fb34934c5ba8 100644
--- a/arch/blackfin/mach-bf533/include/mach/dma.h
+++ b/arch/blackfin/mach-bf533/include/mach/dma.h
@@ -1,42 +1,14 @@
-/*****************************************************************************
-*
-*        BF-533/2/1 Specific Declarations
-*
-****************************************************************************/
-/*
- * File:         include/asm-blackfin/mach-bf533/dma.h
- * Based on:
- * Author:
+/* mach/dma.h - arch-specific DMA defines
  *
- * Created:
- * Description:
+ * Copyright 2004-2008 Analog Devices Inc.
  *
- * Rev:
- *
- * Modified:
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, 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; see the file COPYING.
- * If not, write to the Free Software Foundation,
- * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ * Licensed under the GPL-2 or later.
  */
 
 #ifndef _MACH_DMA_H_
 #define _MACH_DMA_H_
 
-#define MAX_BLACKFIN_DMA_CHANNEL 12
+#define MAX_DMA_CHANNELS 12
 
 #define CH_PPI          0
 #define CH_SPORT0_RX    1
@@ -44,8 +16,8 @@
 #define CH_SPORT1_RX    3
 #define CH_SPORT1_TX    4
 #define CH_SPI          5
-#define CH_UART_RX      6
-#define CH_UART_TX      7
+#define CH_UART0_RX     6
+#define CH_UART0_TX     7
 #define CH_MEM_STREAM0_DEST     8	 /* TX */
 #define CH_MEM_STREAM0_SRC      9	 /* RX */
 #define CH_MEM_STREAM1_DEST     10	 /* TX */
diff --git a/arch/blackfin/mach-bf533/include/mach/gpio.h b/arch/blackfin/mach-bf533/include/mach/gpio.h
new file mode 100644
index 000000000000..e45c17077aff
--- /dev/null
+++ b/arch/blackfin/mach-bf533/include/mach/gpio.h
@@ -0,0 +1,34 @@
+/*
+ * File: arch/blackfin/mach-bf533/include/mach/gpio.h
+ * Bugs: Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Copyright (C) 2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
+ */
+
+
+#ifndef _MACH_GPIO_H_
+#define _MACH_GPIO_H_
+
+#define MAX_BLACKFIN_GPIOS 16
+
+#define	GPIO_PF0	0
+#define	GPIO_PF1	1
+#define	GPIO_PF2	2
+#define	GPIO_PF3	3
+#define	GPIO_PF4	4
+#define	GPIO_PF5	5
+#define	GPIO_PF6	6
+#define	GPIO_PF7	7
+#define	GPIO_PF8	8
+#define	GPIO_PF9	9
+#define	GPIO_PF10	10
+#define	GPIO_PF11	11
+#define	GPIO_PF12	12
+#define	GPIO_PF13	13
+#define	GPIO_PF14	14
+#define	GPIO_PF15	15
+
+#define PORT_F GPIO_PF0
+
+#endif /* _MACH_GPIO_H_ */
diff --git a/arch/blackfin/mach-bf533/include/mach/irq.h b/arch/blackfin/mach-bf533/include/mach/irq.h
index 5aa38e5da6b7..db1e346cd1aa 100644
--- a/arch/blackfin/mach-bf533/include/mach/irq.h
+++ b/arch/blackfin/mach-bf533/include/mach/irq.h
@@ -90,19 +90,19 @@ Core        Emulation               **
 #define	IRQ_SPORT0_ERROR	10	/*SPORT0 Error Interrupt */
 #define	IRQ_SPORT1_ERROR	11	/*SPORT1 Error Interrupt */
 #define	IRQ_SPI_ERROR		12	/*SPI Error Interrupt */
-#define	IRQ_UART_ERROR		13	/*UART Error Interrupt */
+#define	IRQ_UART0_ERROR		13	/*UART Error Interrupt */
 #define	IRQ_RTC			14	/*RTC Interrupt */
 #define	IRQ_PPI			15	/*DMA0 Interrupt (PPI) */
 #define	IRQ_SPORT0_RX		16	/*DMA1 Interrupt (SPORT0 RX) */
 #define	IRQ_SPORT0_TX		17	/*DMA2 Interrupt (SPORT0 TX) */
 #define	IRQ_SPORT1_RX		18	/*DMA3 Interrupt (SPORT1 RX) */
 #define	IRQ_SPORT1_TX		19	/*DMA4 Interrupt (SPORT1 TX) */
-#define 	IRQ_SPI			20	/*DMA5 Interrupt (SPI) */
-#define	IRQ_UART_RX		21	/*DMA6 Interrupt (UART RX) */
-#define	IRQ_UART_TX		22	/*DMA7 Interrupt (UART TX) */
-#define	IRQ_TMR0		23	/*Timer 0 */
-#define	IRQ_TMR1		24	/*Timer 1 */
-#define	IRQ_TMR2		25	/*Timer 2 */
+#define	IRQ_SPI			20	/*DMA5 Interrupt (SPI) */
+#define	IRQ_UART0_RX		21	/*DMA6 Interrupt (UART RX) */
+#define	IRQ_UART0_TX		22	/*DMA7 Interrupt (UART TX) */
+#define	IRQ_TIMER0		23	/*Timer 0 */
+#define	IRQ_TIMER1		24	/*Timer 1 */
+#define	IRQ_TIMER2		25	/*Timer 2 */
 #define	IRQ_PROG_INTA		26	/*Programmable Flags A (8) */
 #define	IRQ_PROG_INTB		27	/*Programmable Flags B (8) */
 #define	IRQ_MEM_DMA0		28	/*DMA8/9 Interrupt (Memory DMA Stream 0) */
diff --git a/arch/blackfin/mach-bf533/include/mach/mem_init.h b/arch/blackfin/mach-bf533/include/mach/mem_init.h
deleted file mode 100644
index ed2034bf10ec..000000000000
--- a/arch/blackfin/mach-bf533/include/mach/mem_init.h
+++ /dev/null
@@ -1,297 +0,0 @@
-/*
- * File:         include/asm-blackfin/mach-bf533/mem_init.h
- * Based on:
- * Author:
- *
- * Created:
- * Description:
- *
- * Rev:
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, 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; see the file COPYING.
- * If not, write to the Free Software Foundation,
- * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-#if (CONFIG_MEM_MT48LC16M16A2TG_75 || CONFIG_MEM_MT48LC64M4A2FB_7E || \
-     CONFIG_MEM_MT48LC32M16A2TG_75 || CONFIG_MEM_GENERIC_BOARD)
-#if (CONFIG_SCLK_HZ > 119402985)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_7
-#define SDRAM_tRAS_num  7
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 104477612) && (CONFIG_SCLK_HZ <= 119402985)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_6
-#define SDRAM_tRAS_num  6
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 89552239) && (CONFIG_SCLK_HZ <= 104477612)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_5
-#define SDRAM_tRAS_num  5
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 74626866) && (CONFIG_SCLK_HZ <= 89552239)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_4
-#define SDRAM_tRAS_num  4
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 66666667) && (CONFIG_SCLK_HZ <= 74626866)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_3
-#define SDRAM_tRAS_num  3
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 59701493) && (CONFIG_SCLK_HZ <= 66666667)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_4
-#define SDRAM_tRAS_num  3
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 44776119) && (CONFIG_SCLK_HZ <= 59701493)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_3
-#define SDRAM_tRAS_num  3
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 29850746) && (CONFIG_SCLK_HZ <= 44776119)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_2
-#define SDRAM_tRAS_num  2
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ <= 29850746)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_1
-#define SDRAM_tRAS_num  1
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#endif
-
-#if (CONFIG_MEM_MT48LC16M16A2TG_75)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-#if (CONFIG_MEM_MT48LC64M4A2FB_7E)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-#if (CONFIG_MEM_MT48LC32M16A2TG_75)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-#if (CONFIG_MEM_GENERIC_BOARD)
-  /*SDRAM INFORMATION: Modify this for your board */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-/* Equation from section 17 (p17-46) of BF533 HRM */
-#define mem_SDRRC       (((CONFIG_SCLK_HZ / 1000) * SDRAM_Tref)  / SDRAM_NRA) - (SDRAM_tRAS_num + SDRAM_tRP_num)
-
-/* Enable SCLK Out */
-#define mem_SDGCTL        (SCTLE | SDRAM_CL | SDRAM_tRAS | SDRAM_tRP | SDRAM_tRCD | SDRAM_tWR | PSS)
-
-#if defined CONFIG_CLKIN_HALF
-#define CLKIN_HALF       1
-#else
-#define CLKIN_HALF       0
-#endif
-
-#if defined CONFIG_PLL_BYPASS
-#define PLL_BYPASS      1
-#else
-#define PLL_BYPASS       0
-#endif
-
-/***************************************Currently Not Being Used *********************************/
-#define flash_EBIU_AMBCTL_WAT  ((CONFIG_FLASH_SPEED_BWAT * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-#define flash_EBIU_AMBCTL_RAT  ((CONFIG_FLASH_SPEED_BRAT * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-#define flash_EBIU_AMBCTL_HT   ((CONFIG_FLASH_SPEED_BHT  * 4) / (4000000000 / CONFIG_SCLK_HZ))
-#define flash_EBIU_AMBCTL_ST   ((CONFIG_FLASH_SPEED_BST  * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-#define flash_EBIU_AMBCTL_TT   ((CONFIG_FLASH_SPEED_BTT  * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-
-#if (flash_EBIU_AMBCTL_TT > 3)
-#define flash_EBIU_AMBCTL0_TT   B0TT_4
-#endif
-#if (flash_EBIU_AMBCTL_TT == 3)
-#define flash_EBIU_AMBCTL0_TT   B0TT_3
-#endif
-#if (flash_EBIU_AMBCTL_TT == 2)
-#define flash_EBIU_AMBCTL0_TT   B0TT_2
-#endif
-#if (flash_EBIU_AMBCTL_TT < 2)
-#define flash_EBIU_AMBCTL0_TT   B0TT_1
-#endif
-
-#if (flash_EBIU_AMBCTL_ST > 3)
-#define flash_EBIU_AMBCTL0_ST   B0ST_4
-#endif
-#if (flash_EBIU_AMBCTL_ST == 3)
-#define flash_EBIU_AMBCTL0_ST   B0ST_3
-#endif
-#if (flash_EBIU_AMBCTL_ST == 2)
-#define flash_EBIU_AMBCTL0_ST   B0ST_2
-#endif
-#if (flash_EBIU_AMBCTL_ST < 2)
-#define flash_EBIU_AMBCTL0_ST   B0ST_1
-#endif
-
-#if (flash_EBIU_AMBCTL_HT > 2)
-#define flash_EBIU_AMBCTL0_HT   B0HT_3
-#endif
-#if (flash_EBIU_AMBCTL_HT == 2)
-#define flash_EBIU_AMBCTL0_HT   B0HT_2
-#endif
-#if (flash_EBIU_AMBCTL_HT == 1)
-#define flash_EBIU_AMBCTL0_HT   B0HT_1
-#endif
-#if (flash_EBIU_AMBCTL_HT == 0 && CONFIG_FLASH_SPEED_BHT == 0)
-#define flash_EBIU_AMBCTL0_HT   B0HT_0
-#endif
-#if (flash_EBIU_AMBCTL_HT == 0 && CONFIG_FLASH_SPEED_BHT != 0)
-#define flash_EBIU_AMBCTL0_HT   B0HT_1
-#endif
-
-#if (flash_EBIU_AMBCTL_WAT > 14)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_15
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 14)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_14
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 13)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_13
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 12)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_12
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 11)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_11
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 10)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_10
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 9)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_9
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 8)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_8
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 7)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_7
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 6)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_6
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 5)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_5
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 4)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_4
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 3)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_3
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 2)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_2
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 1)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_1
-#endif
-
-#if (flash_EBIU_AMBCTL_RAT > 14)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_15
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 14)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_14
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 13)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_13
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 12)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_12
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 11)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_11
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 10)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_10
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 9)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_9
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 8)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_8
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 7)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_7
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 6)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_6
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 5)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_5
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 4)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_4
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 3)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_3
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 2)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_2
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 1)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_1
-#endif
-
-#define flash_EBIU_AMBCTL0  \
-	(flash_EBIU_AMBCTL0_WAT | flash_EBIU_AMBCTL0_RAT | flash_EBIU_AMBCTL0_HT | \
-	 flash_EBIU_AMBCTL0_ST | flash_EBIU_AMBCTL0_TT | CONFIG_FLASH_SPEED_RDYEN)
diff --git a/arch/blackfin/mach-bf533/include/mach/mem_map.h b/arch/blackfin/mach-bf533/include/mach/mem_map.h
index 581fc6eea789..fc33b7cb9937 100644
--- a/arch/blackfin/mach-bf533/include/mach/mem_map.h
+++ b/arch/blackfin/mach-bf533/include/mach/mem_map.h
@@ -168,4 +168,10 @@
 #define L1_SCRATCH_START	0xFFB00000
 #define L1_SCRATCH_LENGTH	0x1000
 
+#define GET_PDA_SAFE(preg)		\
+	preg.l = _cpu_pda;		\
+	preg.h = _cpu_pda;
+
+#define GET_PDA(preg, dreg)	GET_PDA_SAFE(preg)
+
 #endif				/* _MEM_MAP_533_H_ */
diff --git a/arch/blackfin/mach-bf537/Kconfig b/arch/blackfin/mach-bf537/Kconfig
index 8255374c04aa..bbc08fd4f122 100644
--- a/arch/blackfin/mach-bf537/Kconfig
+++ b/arch/blackfin/mach-bf537/Kconfig
@@ -64,29 +64,29 @@ config IRQ_MAC_RX
 config IRQ_MAC_TX
 	int "IRQ_MAC_TX"
 	default 11
-config IRQ_TMR0
-	int "IRQ_TMR0"
-	default 12
-config IRQ_TMR1
-	int "IRQ_TMR1"
+config IRQ_TIMER0
+	int "IRQ_TIMER0"
+	default 8
+config IRQ_TIMER1
+	int "IRQ_TIMER1"
 	default 12
-config IRQ_TMR2
-	int "IRQ_TMR2"
+config IRQ_TIMER2
+	int "IRQ_TIMER2"
 	default 12
-config IRQ_TMR3
-	int "IRQ_TMR3"
+config IRQ_TIMER3
+	int "IRQ_TIMER3"
 	default 12
-config IRQ_TMR4
-	int "IRQ_TMR4"
+config IRQ_TIMER4
+	int "IRQ_TIMER4"
 	default 12
-config IRQ_TMR5
-	int "IRQ_TMR5"
+config IRQ_TIMER5
+	int "IRQ_TIMER5"
 	default 12
-config IRQ_TMR6
-	int "IRQ_TMR6"
+config IRQ_TIMER6
+	int "IRQ_TIMER6"
 	default 12
-config IRQ_TMR7
-	int "IRQ_TMR7"
+config IRQ_TIMER7
+	int "IRQ_TIMER7"
 	default 12
 config IRQ_PROG_INTA
 	int "IRQ_PROG_INTA"
diff --git a/arch/blackfin/mach-bf537/Makefile b/arch/blackfin/mach-bf537/Makefile
index 68e5478e95a9..56994b675f9c 100644
--- a/arch/blackfin/mach-bf537/Makefile
+++ b/arch/blackfin/mach-bf537/Makefile
@@ -2,6 +2,4 @@
 # arch/blackfin/mach-bf537/Makefile
 #
 
-extra-y := head.o
-
 obj-y := ints-priority.o dma.o
diff --git a/arch/blackfin/mach-bf537/boards/cm_bf537.c b/arch/blackfin/mach-bf537/boards/cm_bf537.c
index dde14720b0ea..6ac8e4d5bd38 100644
--- a/arch/blackfin/mach-bf537/boards/cm_bf537.c
+++ b/arch/blackfin/mach-bf537/boards/cm_bf537.c
@@ -308,6 +308,19 @@ static struct platform_device net2272_bfin_device = {
 };
 #endif
 
+static struct resource bfin_gpios_resources = {
+	.start = 0,
+	.end   = MAX_BLACKFIN_GPIOS - 1,
+	.flags = IORESOURCE_IRQ,
+};
+
+static struct platform_device bfin_gpios_device = {
+	.name = "simple-gpio",
+	.id = -1,
+	.num_resources = 1,
+	.resource = &bfin_gpios_resources,
+};
+
 #if defined(CONFIG_MTD_GPIO_ADDR) || defined(CONFIG_MTD_GPIO_ADDR_MODULE)
 static struct mtd_partition cm_partitions[] = {
 	{
@@ -379,30 +392,57 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
 #endif
 #ifdef CONFIG_BFIN_SIR1
+static struct resource bfin_sir1_resources[] = {
 	{
 		.start = 0xFFC02000,
 		.end = 0xFFC020FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART1_RX,
+		.end = IRQ_UART1_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART1_RX,
+		.end = CH_UART1_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
-
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir1_device = {
 	.name = "bfin_sir",
-	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_sir1_resources),
+	.resource = bfin_sir1_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
 static struct resource bfin_twi0_resource[] = {
@@ -525,7 +565,12 @@ static struct platform_device *cm_bf537_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#ifdef CONFIG_BFIN_SIR1
+	&bfin_sir1_device,
+#endif
 #endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
@@ -564,6 +609,8 @@ static struct platform_device *cm_bf537_devices[] __initdata = {
 #if defined(CONFIG_MTD_GPIO_ADDR) || defined(CONFIG_MTD_GPIO_ADDR_MODULE)
 	&cm_flash_device,
 #endif
+
+	&bfin_gpios_device,
 };
 
 static int __init cm_bf537_init(void)
diff --git a/arch/blackfin/mach-bf537/boards/generic_board.c b/arch/blackfin/mach-bf537/boards/generic_board.c
index 78a13d5bfd55..dd6e6bfb98ea 100644
--- a/arch/blackfin/mach-bf537/boards/generic_board.c
+++ b/arch/blackfin/mach-bf537/boards/generic_board.c
@@ -50,57 +50,46 @@
 /*
  * Name the Board for the /proc/cpuinfo
  */
-const char bfin_board_name[] = "GENERIC Board";
+const char bfin_board_name[] = "UNKNOWN BOARD";
 
 /*
  *  Driver needs to know address, irq and flag pin.
  */
 
-#define ISP1761_BASE       0x203C0000
-#define ISP1761_IRQ        IRQ_PF7
-
 #if defined(CONFIG_USB_ISP1760_HCD) || defined(CONFIG_USB_ISP1760_HCD_MODULE)
-static struct resource bfin_isp1761_resources[] = {
+#include <linux/usb/isp1760.h>
+static struct resource bfin_isp1760_resources[] = {
 	[0] = {
-		.name	= "isp1761-regs",
-		.start  = ISP1761_BASE + 0x00000000,
-		.end    = ISP1761_BASE + 0x000fffff,
+		.start  = 0x203C0000,
+		.end    = 0x203C0000 + 0x000fffff,
 		.flags  = IORESOURCE_MEM,
 	},
 	[1] = {
-		.start  = ISP1761_IRQ,
-		.end    = ISP1761_IRQ,
+		.start  = IRQ_PF7,
+		.end    = IRQ_PF7,
 		.flags  = IORESOURCE_IRQ,
 	},
 };
 
-static struct platform_device bfin_isp1761_device = {
-	.name           = "isp1761",
-	.id             = 0,
-	.num_resources  = ARRAY_SIZE(bfin_isp1761_resources),
-	.resource       = bfin_isp1761_resources,
+static struct isp1760_platform_data isp1760_priv = {
+	.is_isp1761 = 0,
+	.port1_disable = 0,
+	.bus_width_16 = 1,
+	.port1_otg = 0,
+	.analog_oc = 0,
+	.dack_polarity_high = 0,
+	.dreq_polarity_high = 0,
 };
 
-static struct platform_device *bfin_isp1761_devices[] = {
-	&bfin_isp1761_device,
+static struct platform_device bfin_isp1760_device = {
+	.name           = "isp1760-hcd",
+	.id             = 0,
+	.dev = {
+		.platform_data = &isp1760_priv,
+	},
+	.num_resources  = ARRAY_SIZE(bfin_isp1760_resources),
+	.resource       = bfin_isp1760_resources,
 };
-
-int __init bfin_isp1761_init(void)
-{
-	unsigned int num_devices = ARRAY_SIZE(bfin_isp1761_devices);
-
-	printk(KERN_INFO "%s(): registering device resources\n", __func__);
-	set_irq_type(ISP1761_IRQ, IRQF_TRIGGER_FALLING);
-
-	return platform_add_devices(bfin_isp1761_devices, num_devices);
-}
-
-void __exit bfin_isp1761_exit(void)
-{
-	platform_device_unregister(&bfin_isp1761_device);
-}
-
-arch_initcall(bfin_isp1761_init);
 #endif
 
 #if defined(CONFIG_BFIN_CFPCMCIA) || defined(CONFIG_BFIN_CFPCMCIA_MODULE)
@@ -559,30 +548,59 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
 #endif
 #ifdef CONFIG_BFIN_SIR1
+static struct resource bfin_sir1_resources[] = {
 	{
 		.start = 0xFFC02000,
 		.end = 0xFFC020FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART1_RX,
+		.end = IRQ_UART1_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART1_RX,
+		.end = CH_UART1_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir1_device = {
 	.name = "bfin_sir",
-	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_sir1_resources),
+	.resource = bfin_sir1_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
 static struct resource bfin_twi0_resource[] = {
@@ -651,6 +669,10 @@ static struct platform_device *stamp_devices[] __initdata = {
 	&net2272_bfin_device,
 #endif
 
+#if defined(CONFIG_USB_ISP1760_HCD) || defined(CONFIG_USB_ISP1760_HCD_MODULE)
+	&bfin_isp1760_device,
+#endif
+
 #if defined(CONFIG_SPI_BFIN) || defined(CONFIG_SPI_BFIN_MODULE)
 	&bfin_spi0_device,
 #endif
@@ -668,7 +690,12 @@ static struct platform_device *stamp_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#ifdef CONFIG_BFIN_SIR1
+	&bfin_sir1_device,
+#endif
 #endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
diff --git a/arch/blackfin/mach-bf537/boards/minotaur.c b/arch/blackfin/mach-bf537/boards/minotaur.c
index 48c4cd2d1be6..bb795341cb17 100644
--- a/arch/blackfin/mach-bf537/boards/minotaur.c
+++ b/arch/blackfin/mach-bf537/boards/minotaur.c
@@ -226,30 +226,59 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
 #endif
 #ifdef CONFIG_BFIN_SIR1
+static struct resource bfin_sir1_resources[] = {
 	{
 		.start = 0xFFC02000,
 		.end = 0xFFC020FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART1_RX,
+		.end = IRQ_UART1_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART1_RX,
+		.end = CH_UART1_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir1_device = {
 	.name = "bfin_sir",
-	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_sir1_resources),
+	.resource = bfin_sir1_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
 static struct resource bfin_twi0_resource[] = {
@@ -311,7 +340,12 @@ static struct platform_device *minotaur_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#ifdef CONFIG_BFIN_SIR1
+	&bfin_sir1_device,
+#endif
 #endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
diff --git a/arch/blackfin/mach-bf537/boards/pnav10.c b/arch/blackfin/mach-bf537/boards/pnav10.c
index f9174c11cbd4..89de94f4545d 100644
--- a/arch/blackfin/mach-bf537/boards/pnav10.c
+++ b/arch/blackfin/mach-bf537/boards/pnav10.c
@@ -49,7 +49,7 @@
 /*
  * Name the Board for the /proc/cpuinfo
  */
-const char bfin_board_name[] = "PNAV-1.0";
+const char bfin_board_name[] = "ADI PNAV-1.0";
 
 /*
  *  Driver needs to know address, irq and flag pin.
@@ -453,30 +453,59 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
 #endif
 #ifdef CONFIG_BFIN_SIR1
+static struct resource bfin_sir1_resources[] = {
 	{
 		.start = 0xFFC02000,
 		.end = 0xFFC020FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART1_RX,
+		.end = IRQ_UART1_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART1_RX,
+		.end = CH_UART1_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir1_device = {
 	.name = "bfin_sir",
-	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_sir1_resources),
+	.resource = bfin_sir1_resources,
 };
 #endif
+#endif
 
 static struct platform_device *stamp_devices[] __initdata = {
 #if defined(CONFIG_BFIN_CFPCMCIA) || defined(CONFIG_BFIN_CFPCMCIA_MODULE)
@@ -520,7 +549,12 @@ static struct platform_device *stamp_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#ifdef CONFIG_BFIN_SIR1
+	&bfin_sir1_device,
+#endif
 #endif
 };
 
diff --git a/arch/blackfin/mach-bf537/boards/stamp.c b/arch/blackfin/mach-bf537/boards/stamp.c
index 8d394393201f..d812e2514a2f 100644
--- a/arch/blackfin/mach-bf537/boards/stamp.c
+++ b/arch/blackfin/mach-bf537/boards/stamp.c
@@ -46,6 +46,7 @@
 #include <linux/interrupt.h>
 #include <linux/i2c.h>
 #include <linux/usb/sl811.h>
+#include <linux/spi/mmc_spi.h>
 #include <asm/dma.h>
 #include <asm/bfin5xx_spi.h>
 #include <asm/reboot.h>
@@ -55,57 +56,46 @@
 /*
  * Name the Board for the /proc/cpuinfo
  */
-const char bfin_board_name[] = "ADDS-BF537-STAMP";
+const char bfin_board_name[] = "ADI BF537-STAMP";
 
 /*
  *  Driver needs to know address, irq and flag pin.
  */
 
-#define ISP1761_BASE       0x203C0000
-#define ISP1761_IRQ        IRQ_PF7
-
 #if defined(CONFIG_USB_ISP1760_HCD) || defined(CONFIG_USB_ISP1760_HCD_MODULE)
-static struct resource bfin_isp1761_resources[] = {
+#include <linux/usb/isp1760.h>
+static struct resource bfin_isp1760_resources[] = {
 	[0] = {
-		.name	= "isp1761-regs",
-		.start  = ISP1761_BASE + 0x00000000,
-		.end    = ISP1761_BASE + 0x000fffff,
+		.start  = 0x203C0000,
+		.end    = 0x203C0000 + 0x000fffff,
 		.flags  = IORESOURCE_MEM,
 	},
 	[1] = {
-		.start  = ISP1761_IRQ,
-		.end    = ISP1761_IRQ,
-		.flags  = IORESOURCE_IRQ,
+		.start  = IRQ_PF7,
+		.end    = IRQ_PF7,
+		.flags  = IORESOURCE_IRQ | IORESOURCE_IRQ_LOWLEVEL,
 	},
 };
 
-static struct platform_device bfin_isp1761_device = {
-	.name           = "isp1761",
-	.id             = 0,
-	.num_resources  = ARRAY_SIZE(bfin_isp1761_resources),
-	.resource       = bfin_isp1761_resources,
+static struct isp1760_platform_data isp1760_priv = {
+	.is_isp1761 = 0,
+	.port1_disable = 0,
+	.bus_width_16 = 1,
+	.port1_otg = 0,
+	.analog_oc = 0,
+	.dack_polarity_high = 0,
+	.dreq_polarity_high = 0,
 };
 
-static struct platform_device *bfin_isp1761_devices[] = {
-	&bfin_isp1761_device,
+static struct platform_device bfin_isp1760_device = {
+	.name           = "isp1760-hcd",
+	.id             = 0,
+	.dev = {
+		.platform_data = &isp1760_priv,
+	},
+	.num_resources  = ARRAY_SIZE(bfin_isp1760_resources),
+	.resource       = bfin_isp1760_resources,
 };
-
-int __init bfin_isp1761_init(void)
-{
-	unsigned int num_devices = ARRAY_SIZE(bfin_isp1761_devices);
-
-	printk(KERN_INFO "%s(): registering device resources\n", __func__);
-	set_irq_type(ISP1761_IRQ, IRQF_TRIGGER_FALLING);
-
-	return platform_add_devices(bfin_isp1761_devices, num_devices);
-}
-
-void __exit bfin_isp1761_exit(void)
-{
-	platform_device_unregister(&bfin_isp1761_device);
-}
-
-arch_initcall(bfin_isp1761_init);
 #endif
 
 #if defined(CONFIG_KEYBOARD_GPIO) || defined(CONFIG_KEYBOARD_GPIO_MODULE)
@@ -443,11 +433,11 @@ static struct mtd_partition stamp_partitions[] = {
 		.offset     = 0,
 	}, {
 		.name       = "linux kernel(nor)",
-		.size       = 0xE0000,
+		.size       = 0x180000,
 		.offset     = MTDPART_OFS_APPEND,
 	}, {
 		.name       = "file system(nor)",
-		.size       = 0x400000 - 0x40000 - 0xE0000 - 0x10000,
+		.size       = 0x400000 - 0x40000 - 0x180000 - 0x10000,
 		.offset     = MTDPART_OFS_APPEND,
 	}, {
 		.name       = "MAC Address(nor)",
@@ -490,7 +480,7 @@ static struct mtd_partition bfin_spi_flash_partitions[] = {
 		.mask_flags = MTD_CAP_ROM
 	}, {
 		.name = "linux kernel(spi)",
-		.size = 0xe0000,
+		.size = 0x180000,
 		.offset = MTDPART_OFS_APPEND,
 	}, {
 		.name = "file system(spi)",
@@ -503,7 +493,7 @@ static struct flash_platform_data bfin_spi_flash_data = {
 	.name = "m25p80",
 	.parts = bfin_spi_flash_partitions,
 	.nr_parts = ARRAY_SIZE(bfin_spi_flash_partitions),
-	.type = "m25p64",
+	/* .type = "m25p64", */
 };
 
 /* SPI flash chip (m25p64) */
@@ -537,9 +527,29 @@ static struct bfin5xx_spi_chip ad9960_spi_chip_info = {
 };
 #endif
 
-#if defined(CONFIG_SPI_MMC) || defined(CONFIG_SPI_MMC_MODULE)
-static struct bfin5xx_spi_chip spi_mmc_chip_info = {
-	.enable_dma = 1,
+#if defined(CONFIG_MMC_SPI) || defined(CONFIG_MMC_SPI_MODULE)
+#define MMC_SPI_CARD_DETECT_INT IRQ_PF5
+
+static int bfin_mmc_spi_init(struct device *dev,
+	irqreturn_t (*detect_int)(int, void *), void *data)
+{
+	return request_irq(MMC_SPI_CARD_DETECT_INT, detect_int,
+		IRQF_TRIGGER_FALLING, "mmc-spi-detect", data);
+}
+
+static void bfin_mmc_spi_exit(struct device *dev, void *data)
+{
+	free_irq(MMC_SPI_CARD_DETECT_INT, data);
+}
+
+static struct mmc_spi_platform_data bfin_mmc_spi_pdata = {
+	.init = bfin_mmc_spi_init,
+	.exit = bfin_mmc_spi_exit,
+	.detect_delay = 100, /* msecs */
+};
+
+static struct bfin5xx_spi_chip  mmc_spi_chip_info = {
+	.enable_dma = 0,
 	.bits_per_word = 8,
 };
 #endif
@@ -613,6 +623,14 @@ static struct bfin5xx_spi_chip lq035q1_spi_chip_info = {
 };
 #endif
 
+#if defined(CONFIG_ENC28J60) || defined(CONFIG_ENC28J60_MODULE)
+static struct bfin5xx_spi_chip enc28j60_spi_chip_info = {
+	.enable_dma	= 1,
+	.bits_per_word	= 8,
+	.cs_gpio = GPIO_PF10,
+};
+#endif
+
 #if defined(CONFIG_MTD_DATAFLASH) \
 	|| defined(CONFIG_MTD_DATAFLASH_MODULE)
 
@@ -624,7 +642,7 @@ static struct mtd_partition bfin_spi_dataflash_partitions[] = {
 		.mask_flags = MTD_CAP_ROM
 	}, {
 		.name = "linux kernel(spi)",
-		.size = 0xe0000,
+		.size = 0x180000,
 		.offset = MTDPART_OFS_APPEND,
 	}, {
 		.name = "file system(spi)",
@@ -703,23 +721,14 @@ static struct spi_board_info bfin_spi_board_info[] __initdata = {
 		.controller_data = &ad9960_spi_chip_info,
 	},
 #endif
-#if defined(CONFIG_SPI_MMC) || defined(CONFIG_SPI_MMC_MODULE)
+#if defined(CONFIG_MMC_SPI) || defined(CONFIG_MMC_SPI_MODULE)
 	{
-		.modalias = "spi_mmc_dummy",
+		.modalias = "mmc_spi",
 		.max_speed_hz = 20000000,     /* max spi clock (SCK) speed in HZ */
 		.bus_num = 0,
-		.chip_select = 0,
-		.platform_data = NULL,
-		.controller_data = &spi_mmc_chip_info,
-		.mode = SPI_MODE_3,
-	},
-	{
-		.modalias = "spi_mmc",
-		.max_speed_hz = 20000000,     /* max spi clock (SCK) speed in HZ */
-		.bus_num = 0,
-		.chip_select = CONFIG_SPI_MMC_CS_CHAN,
-		.platform_data = NULL,
-		.controller_data = &spi_mmc_chip_info,
+		.chip_select = 4,
+		.platform_data = &bfin_mmc_spi_pdata,
+		.controller_data = &mmc_spi_chip_info,
 		.mode = SPI_MODE_3,
 	},
 #endif
@@ -783,6 +792,17 @@ static struct spi_board_info bfin_spi_board_info[] __initdata = {
 		.mode = SPI_CPHA | SPI_CPOL,
 	},
 #endif
+#if defined(CONFIG_ENC28J60) || defined(CONFIG_ENC28J60_MODULE)
+	{
+		.modalias = "enc28j60",
+		.max_speed_hz = 20000000,     /* max spi clock (SCK) speed in HZ */
+		.irq = IRQ_PF6,
+		.bus_num = 0,
+		.chip_select = 0,	/* GPIO controlled SSEL */
+		.controller_data = &enc28j60_spi_chip_info,
+		.mode = SPI_MODE_0,
+	},
+#endif
 };
 
 #if defined(CONFIG_SPI_BFIN) || defined(CONFIG_SPI_BFIN_MODULE)
@@ -885,30 +905,59 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
 #endif
 #ifdef CONFIG_BFIN_SIR1
+static struct resource bfin_sir1_resources[] = {
 	{
 		.start = 0xFFC02000,
 		.end = 0xFFC020FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART1_RX,
+		.end = IRQ_UART1_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART1_RX,
+		.end = CH_UART1_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir1_device = {
 	.name = "bfin_sir",
-	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_sir1_resources),
+	.resource = bfin_sir1_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
 static struct resource bfin_twi0_resource[] = {
@@ -932,6 +981,93 @@ static struct platform_device i2c_bfin_twi_device = {
 };
 #endif
 
+#if defined(CONFIG_KEYBOARD_ADP5588) || defined(CONFIG_KEYBOARD_ADP5588_MODULE)
+#include <linux/input.h>
+#include <linux/i2c/adp5588_keys.h>
+static const unsigned short adp5588_keymap[ADP5588_KEYMAPSIZE] = {
+	[0]	 = KEY_GRAVE,
+	[1]	 = KEY_1,
+	[2]	 = KEY_2,
+	[3]	 = KEY_3,
+	[4]	 = KEY_4,
+	[5]	 = KEY_5,
+	[6]	 = KEY_6,
+	[7]	 = KEY_7,
+	[8]	 = KEY_8,
+	[9]	 = KEY_9,
+	[10]	 = KEY_0,
+	[11]	 = KEY_MINUS,
+	[12]	 = KEY_EQUAL,
+	[13]	 = KEY_BACKSLASH,
+	[15]	 = KEY_KP0,
+	[16]	 = KEY_Q,
+	[17]	 = KEY_W,
+	[18]	 = KEY_E,
+	[19]	 = KEY_R,
+	[20]	 = KEY_T,
+	[21]	 = KEY_Y,
+	[22]	 = KEY_U,
+	[23]	 = KEY_I,
+	[24]	 = KEY_O,
+	[25]	 = KEY_P,
+	[26]	 = KEY_LEFTBRACE,
+	[27]	 = KEY_RIGHTBRACE,
+	[29]	 = KEY_KP1,
+	[30]	 = KEY_KP2,
+	[31]	 = KEY_KP3,
+	[32]	 = KEY_A,
+	[33]	 = KEY_S,
+	[34]	 = KEY_D,
+	[35]	 = KEY_F,
+	[36]	 = KEY_G,
+	[37]	 = KEY_H,
+	[38]	 = KEY_J,
+	[39]	 = KEY_K,
+	[40]	 = KEY_L,
+	[41]	 = KEY_SEMICOLON,
+	[42]	 = KEY_APOSTROPHE,
+	[43]	 = KEY_BACKSLASH,
+	[45]	 = KEY_KP4,
+	[46]	 = KEY_KP5,
+	[47]	 = KEY_KP6,
+	[48]	 = KEY_102ND,
+	[49]	 = KEY_Z,
+	[50]	 = KEY_X,
+	[51]	 = KEY_C,
+	[52]	 = KEY_V,
+	[53]	 = KEY_B,
+	[54]	 = KEY_N,
+	[55]	 = KEY_M,
+	[56]	 = KEY_COMMA,
+	[57]	 = KEY_DOT,
+	[58]	 = KEY_SLASH,
+	[60]	 = KEY_KPDOT,
+	[61]	 = KEY_KP7,
+	[62]	 = KEY_KP8,
+	[63]	 = KEY_KP9,
+	[64]	 = KEY_SPACE,
+	[65]	 = KEY_BACKSPACE,
+	[66]	 = KEY_TAB,
+	[67]	 = KEY_KPENTER,
+	[68]	 = KEY_ENTER,
+	[69]	 = KEY_ESC,
+	[70]	 = KEY_DELETE,
+	[74]	 = KEY_KPMINUS,
+	[76]	 = KEY_UP,
+	[77]	 = KEY_DOWN,
+	[78]	 = KEY_RIGHT,
+	[79]	 = KEY_LEFT,
+};
+
+static struct adp5588_kpad_platform_data adp5588_kpad_data = {
+	.rows		= 8,
+	.cols		= 10,
+	.keymap		= adp5588_keymap,
+	.keymapsize	= ARRAY_SIZE(adp5588_keymap),
+	.repeat		= 0,
+};
+#endif
+
 #ifdef CONFIG_I2C_BOARDINFO
 static struct i2c_board_info __initdata bfin_i2c_board_info[] = {
 #if defined(CONFIG_JOYSTICK_AD7142) || defined(CONFIG_JOYSTICK_AD7142_MODULE)
@@ -958,6 +1094,13 @@ static struct i2c_board_info __initdata bfin_i2c_board_info[] = {
 		.platform_data = (void *)&bfin_ad7879_ts_info,
 	},
 #endif
+#if defined(CONFIG_KEYBOARD_ADP5588) || defined(CONFIG_KEYBOARD_ADP5588_MODULE)
+	{
+		I2C_BOARD_INFO("adp5588-keys", 0x34),
+		.irq = IRQ_PG0,
+		.platform_data = (void *)&adp5588_kpad_data,
+	},
+#endif
 };
 #endif
 
@@ -1057,6 +1200,10 @@ static struct platform_device *stamp_devices[] __initdata = {
 	&isp1362_hcd_device,
 #endif
 
+#if defined(CONFIG_USB_ISP1760_HCD) || defined(CONFIG_USB_ISP1760_HCD_MODULE)
+	&bfin_isp1760_device,
+#endif
+
 #if defined(CONFIG_SMC91X) || defined(CONFIG_SMC91X_MODULE)
 	&smc91x_device,
 #endif
@@ -1098,7 +1245,12 @@ static struct platform_device *stamp_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#ifdef CONFIG_BFIN_SIR1
+	&bfin_sir1_device,
+#endif
 #endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
diff --git a/arch/blackfin/mach-bf537/boards/tcm_bf537.c b/arch/blackfin/mach-bf537/boards/tcm_bf537.c
index d5ff705a5129..2f4b066153c5 100644
--- a/arch/blackfin/mach-bf537/boards/tcm_bf537.c
+++ b/arch/blackfin/mach-bf537/boards/tcm_bf537.c
@@ -308,6 +308,19 @@ static struct platform_device net2272_bfin_device = {
 };
 #endif
 
+static struct resource bfin_gpios_resources = {
+	.start = 0,
+	.end   = MAX_BLACKFIN_GPIOS - 1,
+	.flags = IORESOURCE_IRQ,
+};
+
+static struct platform_device bfin_gpios_device = {
+	.name = "simple-gpio",
+	.id = -1,
+	.num_resources = 1,
+	.resource = &bfin_gpios_resources,
+};
+
 #if defined(CONFIG_MTD_GPIO_ADDR) || defined(CONFIG_MTD_GPIO_ADDR_MODULE)
 static struct mtd_partition cm_partitions[] = {
 	{
@@ -379,30 +392,59 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
 #endif
 #ifdef CONFIG_BFIN_SIR1
+static struct resource bfin_sir1_resources[] = {
 	{
 		.start = 0xFFC02000,
 		.end = 0xFFC020FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART1_RX,
+		.end = IRQ_UART1_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART1_RX,
+		.end = CH_UART1_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir1_device = {
 	.name = "bfin_sir",
-	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_sir1_resources),
+	.resource = bfin_sir1_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
 static struct resource bfin_twi0_resource[] = {
@@ -525,7 +567,12 @@ static struct platform_device *cm_bf537_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#ifdef CONFIG_BFIN_SIR1
+	&bfin_sir1_device,
+#endif
 #endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
@@ -564,6 +611,8 @@ static struct platform_device *cm_bf537_devices[] __initdata = {
 #if defined(CONFIG_MTD_GPIO_ADDR) || defined(CONFIG_MTD_GPIO_ADDR_MODULE)
 	&cm_flash_device,
 #endif
+
+	&bfin_gpios_device,
 };
 
 static int __init cm_bf537_init(void)
diff --git a/arch/blackfin/mach-bf537/dma.c b/arch/blackfin/mach-bf537/dma.c
index 4edb363ff99c..81185051de91 100644
--- a/arch/blackfin/mach-bf537/dma.c
+++ b/arch/blackfin/mach-bf537/dma.c
@@ -31,7 +31,7 @@
 #include <asm/blackfin.h>
 #include <asm/dma.h>
 
-struct dma_register *dma_io_base_addr[MAX_BLACKFIN_DMA_CHANNEL] = {
+struct dma_register *dma_io_base_addr[MAX_DMA_CHANNELS] = {
 	(struct dma_register *) DMA0_NEXT_DESC_PTR,
 	(struct dma_register *) DMA1_NEXT_DESC_PTR,
 	(struct dma_register *) DMA2_NEXT_DESC_PTR,
diff --git a/arch/blackfin/mach-bf537/head.S b/arch/blackfin/mach-bf537/head.S
deleted file mode 100644
index f5c94bf80e3b..000000000000
--- a/arch/blackfin/mach-bf537/head.S
+++ /dev/null
@@ -1,146 +0,0 @@
-/*
- * File:         arch/blackfin/mach-bf537/head.S
- * Based on:     arch/blackfin/mach-bf533/head.S
- * Author:       Jeff Dionne <jeff@uclinux.org> COPYRIGHT 1998 D. Jeff Dionne
- *
- * Created:      1998
- * Description:  Startup code for Blackfin BF537
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#include <linux/linkage.h>
-#include <linux/init.h>
-#include <asm/blackfin.h>
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-#include <asm/clocks.h>
-#include <mach/mem_init.h>
-#endif
-
-.section .l1.text
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-ENTRY(_start_dma_code)
-
-	/* Enable PHY CLK buffer output */
-	p0.h = hi(VR_CTL);
-	p0.l = lo(VR_CTL);
-	r0.l = w[p0];
-	bitset(r0, 14);
-	w[p0] = r0.l;
-	ssync;
-
-	p0.h = hi(SIC_IWR);
-	p0.l = lo(SIC_IWR);
-	r0.l = 0x1;
-	r0.h = 0x0;
-	[p0] = r0;
-	SSYNC;
-
-	/*
-	 *  Set PLL_CTL
-	 *   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
-	 *   - [8]     = BYPASS    : BYPASS the PLL, run CLKIN into CCLK/SCLK
-	 *   - [7]     = output delay (add 200ps of delay to mem signals)
-	 *   - [6]     = input delay (add 200ps of input delay to mem signals)
-	 *   - [5]     = PDWN      : 1=All Clocks off
-	 *   - [3]     = STOPCK    : 1=Core Clock off
-	 *   - [1]     = PLL_OFF   : 1=Disable Power to PLL
-	 *   - [0]     = DF        : 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
-	 *   all other bits set to zero
-	 */
-
-	p0.h = hi(PLL_LOCKCNT);
-	p0.l = lo(PLL_LOCKCNT);
-	r0 = 0x300(Z);
-	w[p0] = r0.l;
-	ssync;
-
-	P2.H = hi(EBIU_SDGCTL);
-	P2.L = lo(EBIU_SDGCTL);
-	R0 = [P2];
-	BITSET (R0, 24);
-	[P2] = R0;
-	SSYNC;
-
-	r0 = CONFIG_VCO_MULT & 63;       /* Load the VCO multiplier         */
-	r0 = r0 << 9;                    /* Shift it over,                  */
-	r1 = CLKIN_HALF;                 /* Do we need to divide CLKIN by 2?*/
-	r0 = r1 | r0;
-	r1 = PLL_BYPASS;                 /* Bypass the PLL?                 */
-	r1 = r1 << 8;                    /* Shift it over                   */
-	r0 = r1 | r0;                    /* add them all together           */
-#ifdef ANOMALY_05000265
-	BITSET(r0, 15);                  /* Add 250 mV of hysteresis to SPORT input pins */
-#endif
-
-	p0.h = hi(PLL_CTL);
-	p0.l = lo(PLL_CTL);              /* Load the address                */
-	cli r2;                          /* Disable interrupts              */
-	ssync;
-	w[p0] = r0.l;                    /* Set the value                   */
-	idle;                            /* Wait for the PLL to stablize    */
-	sti r2;                          /* Enable interrupts               */
-
-.Lcheck_again:
-	p0.h = hi(PLL_STAT);
-	p0.l = lo(PLL_STAT);
-	R0 = W[P0](Z);
-	CC = BITTST(R0,5);
-	if ! CC jump .Lcheck_again;
-
-	/* Configure SCLK & CCLK Dividers */
-	r0 = (CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
-	p0.h = hi(PLL_DIV);
-	p0.l = lo(PLL_DIV);
-	w[p0] = r0.l;
-	ssync;
-
-	p0.l = lo(EBIU_SDRRC);
-	p0.h = hi(EBIU_SDRRC);
-	r0 = mem_SDRRC;
-	w[p0] = r0.l;
-	ssync;
-
-	P2.H = hi(EBIU_SDGCTL);
-	P2.L = lo(EBIU_SDGCTL);
-	R0 = [P2];
-	BITCLR (R0, 24);
-	p0.h = hi(EBIU_SDSTAT);
-	p0.l = lo(EBIU_SDSTAT);
-	r2.l = w[p0];
-	cc = bittst(r2,3);
-	if !cc jump .Lskip;
-	NOP;
-	BITSET (R0, 23);
-.Lskip:
-	[P2] = R0;
-	SSYNC;
-
-	R0.L = lo(mem_SDGCTL);
-	R0.H = hi(mem_SDGCTL);
-	R1 = [p2];
-	R1 = R1 | R0;
-	[P2] = R1;
-	SSYNC;
-
-	RTS;
-ENDPROC(_start_dma_code)
-#endif /* CONFIG_BFIN_KERNEL_CLOCK */
diff --git a/arch/blackfin/mach-bf537/include/mach/anomaly.h b/arch/blackfin/mach-bf537/include/mach/anomaly.h
index c68992494f9e..9cb39121d1cb 100644
--- a/arch/blackfin/mach-bf537/include/mach/anomaly.h
+++ b/arch/blackfin/mach-bf537/include/mach/anomaly.h
@@ -7,7 +7,7 @@
  */
 
 /* This file shoule be up to date with:
- *  - Revision C, 02/08/2008; ADSP-BF534/ADSP-BF536/ADSP-BF537 Blackfin Processor Anomaly List
+ *  - Revision D, 09/18/2008; ADSP-BF534/ADSP-BF536/ADSP-BF537 Blackfin Processor Anomaly List
  */
 
 #ifndef _MACH_ANOMALY_H_
@@ -148,6 +148,14 @@
 #define ANOMALY_05000402 (__SILICON_REVISION__ >= 5)
 /* Level-Sensitive External GPIO Wakeups May Cause Indefinite Stall */
 #define ANOMALY_05000403 (1)
+/* Speculative Fetches Can Cause Undesired External FIFO Operations */
+#define ANOMALY_05000416 (1)
+/* Multichannel SPORT Channel Misalignment Under Specific Configuration */
+#define ANOMALY_05000425 (1)
+/* Speculative Fetches of Indirect-Pointer Instructions Can Cause False Hardware Errors */
+#define ANOMALY_05000426 (1)
+/* IFLUSH Instruction at End of Hardware Loop Causes Infinite Stall */
+#define ANOMALY_05000443 (1)
 
 /* Anomalies that don't exist on this proc */
 #define ANOMALY_05000125 (0)
@@ -161,5 +169,8 @@
 #define ANOMALY_05000353 (1)
 #define ANOMALY_05000363 (0)
 #define ANOMALY_05000386 (1)
+#define ANOMALY_05000412 (0)
+#define ANOMALY_05000432 (0)
+#define ANOMALY_05000435 (0)
 
 #endif
diff --git a/arch/blackfin/mach-bf537/include/mach/bf537.h b/arch/blackfin/mach-bf537/include/mach/bf537.h
index 24d5c9d42323..f194a848ae8e 100644
--- a/arch/blackfin/mach-bf537/include/mach/bf537.h
+++ b/arch/blackfin/mach-bf537/include/mach/bf537.h
@@ -133,7 +133,7 @@
 #endif
 
 #ifndef CPU
-#error Unknown CPU type - This kernel doesn't seem to be configured properly
+#error "Unknown CPU type - This kernel doesn't seem to be configured properly"
 #endif
 
 #endif				/* __MACH_BF537_H__  */
diff --git a/arch/blackfin/mach-bf537/include/mach/bfin_sir.h b/arch/blackfin/mach-bf537/include/mach/bfin_sir.h
deleted file mode 100644
index cfd8ad4f1f2c..000000000000
--- a/arch/blackfin/mach-bf537/include/mach/bfin_sir.h
+++ /dev/null
@@ -1,142 +0,0 @@
-/*
- * Blackfin Infra-red Driver
- *
- * Copyright 2006-2008 Analog Devices Inc.
- *
- * Enter bugs at http://blackfin.uclinux.org/
- *
- * Licensed under the GPL-2 or later.
- *
- */
-
-#include <linux/serial.h>
-#include <asm/dma.h>
-#include <asm/portmux.h>
-
-#define SIR_UART_GET_CHAR(port)   bfin_read16((port)->membase + OFFSET_RBR)
-#define SIR_UART_GET_DLL(port)    bfin_read16((port)->membase + OFFSET_DLL)
-#define SIR_UART_GET_IER(port)    bfin_read16((port)->membase + OFFSET_IER)
-#define SIR_UART_GET_DLH(port)    bfin_read16((port)->membase + OFFSET_DLH)
-#define SIR_UART_GET_IIR(port)    bfin_read16((port)->membase + OFFSET_IIR)
-#define SIR_UART_GET_LCR(port)    bfin_read16((port)->membase + OFFSET_LCR)
-#define SIR_UART_GET_GCTL(port)   bfin_read16((port)->membase + OFFSET_GCTL)
-
-#define SIR_UART_PUT_CHAR(port, v) bfin_write16(((port)->membase + OFFSET_THR), v)
-#define SIR_UART_PUT_DLL(port, v)  bfin_write16(((port)->membase + OFFSET_DLL), v)
-#define SIR_UART_PUT_IER(port, v)  bfin_write16(((port)->membase + OFFSET_IER), v)
-#define SIR_UART_PUT_DLH(port, v)  bfin_write16(((port)->membase + OFFSET_DLH), v)
-#define SIR_UART_PUT_LCR(port, v)  bfin_write16(((port)->membase + OFFSET_LCR), v)
-#define SIR_UART_PUT_GCTL(port, v) bfin_write16(((port)->membase + OFFSET_GCTL), v)
-
-#ifdef CONFIG_SIR_BFIN_DMA
-struct dma_rx_buf {
-	char *buf;
-	int head;
-	int tail;
-	};
-#endif /* CONFIG_SIR_BFIN_DMA */
-
-struct bfin_sir_port {
-	unsigned char __iomem   *membase;
-	unsigned int            irq;
-	unsigned int            lsr;
-	unsigned long           clk;
-	struct net_device       *dev;
-#ifdef CONFIG_SIR_BFIN_DMA
-	int                     tx_done;
-	struct dma_rx_buf       rx_dma_buf;
-	struct timer_list       rx_dma_timer;
-	int                     rx_dma_nrows;
-#endif /* CONFIG_SIR_BFIN_DMA */
-	unsigned int            tx_dma_channel;
-	unsigned int            rx_dma_channel;
-};
-
-struct bfin_sir_port sir_ports[BFIN_UART_NR_PORTS];
-
-struct bfin_sir_port_res {
-	unsigned long   base_addr;
-	int             irq;
-	unsigned int    rx_dma_channel;
-	unsigned int    tx_dma_channel;
-};
-
-struct bfin_sir_port_res bfin_sir_port_resource[] = {
-#ifdef CONFIG_BFIN_SIR0
-	{
-	0xFFC00400,
-	IRQ_UART0_RX,
-	CH_UART0_RX,
-	CH_UART0_TX,
-	},
-#endif
-#ifdef CONFIG_BFIN_SIR1
-	{
-	0xFFC02000,
-	IRQ_UART1_RX,
-	CH_UART1_RX,
-	CH_UART1_TX,
-	},
-#endif
-};
-
-int nr_sirs = ARRAY_SIZE(bfin_sir_port_resource);
-
-struct bfin_sir_self {
-	struct bfin_sir_port    *sir_port;
-	spinlock_t              lock;
-	unsigned int            open;
-	int                     speed;
-	int                     newspeed;
-
-	struct sk_buff          *txskb;
-	struct sk_buff          *rxskb;
-	struct net_device_stats stats;
-	struct device           *dev;
-	struct irlap_cb         *irlap;
-	struct qos_info         qos;
-
-	iobuff_t                tx_buff;
-	iobuff_t                rx_buff;
-
-	struct work_struct      work;
-	int                     mtt;
-};
-
-static inline unsigned int SIR_UART_GET_LSR(struct bfin_sir_port *port)
-{
-	unsigned int lsr = bfin_read16(port->membase + OFFSET_LSR);
-	port->lsr |= (lsr & (BI|FE|PE|OE));
-	return lsr | port->lsr;
-}
-
-static inline void SIR_UART_CLEAR_LSR(struct bfin_sir_port *port)
-{
-	port->lsr = 0;
-	bfin_read16(port->membase + OFFSET_LSR);
-}
-
-#define DRIVER_NAME "bfin_sir"
-
-static int bfin_sir_hw_init(void)
-{
-	int ret = -ENODEV;
-#ifdef CONFIG_BFIN_SIR0
-	ret = peripheral_request(P_UART0_TX, DRIVER_NAME);
-	if (ret)
-		return ret;
-	ret = peripheral_request(P_UART0_RX, DRIVER_NAME);
-	if (ret)
-		return ret;
-#endif
-
-#ifdef CONFIG_BFIN_SIR1
-	ret = peripheral_request(P_UART1_TX, DRIVER_NAME);
-	if (ret)
-		return ret;
-	ret = peripheral_request(P_UART1_RX, DRIVER_NAME);
-	if (ret)
-		return ret;
-#endif
-	return ret;
-}
diff --git a/arch/blackfin/mach-bf537/include/mach/blackfin.h b/arch/blackfin/mach-bf537/include/mach/blackfin.h
index cffc786b2a2b..7d6069c886f1 100644
--- a/arch/blackfin/mach-bf537/include/mach/blackfin.h
+++ b/arch/blackfin/mach-bf537/include/mach/blackfin.h
@@ -82,7 +82,7 @@
 #define STATUS_P1	0x02
 #define STATUS_P0	0x01
 
-/* DMA Channnel */
+/* DMA Channel */
 #define bfin_read_CH_UART_RX() bfin_read_CH_UART0_RX()
 #define bfin_write_CH_UART_RX(val) bfin_write_CH_UART0_RX(val)
 #define CH_UART_RX CH_UART0_RX
diff --git a/arch/blackfin/mach-bf537/include/mach/cdefBF534.h b/arch/blackfin/mach-bf537/include/mach/cdefBF534.h
index 88d491cd9f36..5f8b5f845be6 100644
--- a/arch/blackfin/mach-bf537/include/mach/cdefBF534.h
+++ b/arch/blackfin/mach-bf537/include/mach/cdefBF534.h
@@ -40,55 +40,11 @@
 /* Include core specific register pointer definitions 								*/
 #include <asm/cdef_LPBlackfin.h>
 
-#include <asm/system.h>
-
 /* Clock and System Control	(0xFFC00000 - 0xFFC000FF)								*/
 #define bfin_read_PLL_CTL()                  bfin_read16(PLL_CTL)
-/* Writing to PLL_CTL initiates a PLL relock sequence. */
-static __inline__ void bfin_write_PLL_CTL(unsigned int val)
-{
-	unsigned long flags, iwr;
-
-	if (val == bfin_read_PLL_CTL())
-		return;
-
-	local_irq_save(flags);
-	/* Enable the PLL Wakeup bit in SIC IWR */
-	iwr = bfin_read32(SIC_IWR);
-	/* Only allow PPL Wakeup) */
-	bfin_write32(SIC_IWR, IWR_ENABLE(0));
-
-	bfin_write16(PLL_CTL, val);
-	SSYNC();
-	asm("IDLE;");
-
-	bfin_write32(SIC_IWR, iwr);
-	local_irq_restore(flags);
-}
 #define bfin_read_PLL_DIV()                  bfin_read16(PLL_DIV)
 #define bfin_write_PLL_DIV(val)              bfin_write16(PLL_DIV,val)
 #define bfin_read_VR_CTL()                   bfin_read16(VR_CTL)
-/* Writing to VR_CTL initiates a PLL relock sequence. */
-static __inline__ void bfin_write_VR_CTL(unsigned int val)
-{
-	unsigned long flags, iwr;
-
-	if (val == bfin_read_VR_CTL())
-		return;
-
-	local_irq_save(flags);
-	/* Enable the PLL Wakeup bit in SIC IWR */
-	iwr = bfin_read32(SIC_IWR);
-	/* Only allow PPL Wakeup) */
-	bfin_write32(SIC_IWR, IWR_ENABLE(0));
-
-	bfin_write16(VR_CTL, val);
-	SSYNC();
-	asm("IDLE;");
-
-	bfin_write32(SIC_IWR, iwr);
-	local_irq_restore(flags);
-}
 #define bfin_read_PLL_STAT()                 bfin_read16(PLL_STAT)
 #define bfin_write_PLL_STAT(val)             bfin_write16(PLL_STAT,val)
 #define bfin_read_PLL_LOCKCNT()              bfin_read16(PLL_LOCKCNT)
@@ -1816,4 +1772,51 @@ static __inline__ void bfin_write_VR_CTL(unsigned int val)
 #define bfin_read_HMDMA1_BCOUNT()            bfin_read16(HMDMA1_BCOUNT)
 #define bfin_write_HMDMA1_BCOUNT(val)        bfin_write16(HMDMA1_BCOUNT,val)
 
+/* These need to be last due to the cdef/linux inter-dependencies */
+#include <asm/irq.h>
+
+/* Writing to PLL_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_PLL_CTL(unsigned int val)
+{
+	unsigned long flags, iwr;
+
+	if (val == bfin_read_PLL_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr = bfin_read32(SIC_IWR);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SIC_IWR, IWR_ENABLE(0));
+
+	bfin_write16(PLL_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SIC_IWR, iwr);
+	local_irq_restore_hw(flags);
+}
+
+/* Writing to VR_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_VR_CTL(unsigned int val)
+{
+	unsigned long flags, iwr;
+
+	if (val == bfin_read_VR_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr = bfin_read32(SIC_IWR);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SIC_IWR, IWR_ENABLE(0));
+
+	bfin_write16(VR_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SIC_IWR, iwr);
+	local_irq_restore_hw(flags);
+}
+
 #endif				/* _CDEF_BF534_H */
diff --git a/arch/blackfin/mach-bf537/include/mach/dma.h b/arch/blackfin/mach-bf537/include/mach/dma.h
index 7a964040870a..5ae83b1183a1 100644
--- a/arch/blackfin/mach-bf537/include/mach/dma.h
+++ b/arch/blackfin/mach-bf537/include/mach/dma.h
@@ -1,38 +1,14 @@
-/*
- * file:         include/asm-blackfin/mach-bf537/dma.h
- * based on:
- * author:
+/* mach/dma.h - arch-specific DMA defines
  *
- * created:
- * description:
- *	system mmr register map
- * rev:
+ * Copyright 2004-2008 Analog Devices Inc.
  *
- * modified:
- *
- *
- * bugs:         enter bugs at http://blackfin.uclinux.org/
- *
- * this program is free software; you can redistribute it and/or modify
- * it under the terms of the gnu general public license as published by
- * the free software foundation; either version 2, 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; see the file copying.
- * if not, write to the free software foundation,
- * 59 temple place - suite 330, boston, ma 02111-1307, usa.
+ * Licensed under the GPL-2 or later.
  */
 
 #ifndef _MACH_DMA_H_
 #define _MACH_DMA_H_
 
-#define MAX_BLACKFIN_DMA_CHANNEL 16
+#define MAX_DMA_CHANNELS 16
 
 #define CH_PPI 			    0
 #define CH_EMAC_RX 		    1
diff --git a/arch/blackfin/mach-bf537/include/mach/gpio.h b/arch/blackfin/mach-bf537/include/mach/gpio.h
new file mode 100644
index 000000000000..d77a31e45a30
--- /dev/null
+++ b/arch/blackfin/mach-bf537/include/mach/gpio.h
@@ -0,0 +1,68 @@
+/*
+ * File: arch/blackfin/mach-bf537/include/mach/gpio.h
+ * Bugs: Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Copyright (C) 2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
+ */
+
+
+#ifndef _MACH_GPIO_H_
+#define _MACH_GPIO_H_
+
+#define MAX_BLACKFIN_GPIOS 48
+
+#define	GPIO_PF0	0
+#define	GPIO_PF1	1
+#define	GPIO_PF2	2
+#define	GPIO_PF3	3
+#define	GPIO_PF4	4
+#define	GPIO_PF5	5
+#define	GPIO_PF6	6
+#define	GPIO_PF7	7
+#define	GPIO_PF8	8
+#define	GPIO_PF9	9
+#define	GPIO_PF10	10
+#define	GPIO_PF11	11
+#define	GPIO_PF12	12
+#define	GPIO_PF13	13
+#define	GPIO_PF14	14
+#define	GPIO_PF15	15
+#define	GPIO_PG0	16
+#define	GPIO_PG1	17
+#define	GPIO_PG2	18
+#define	GPIO_PG3	19
+#define	GPIO_PG4	20
+#define	GPIO_PG5	21
+#define	GPIO_PG6	22
+#define	GPIO_PG7	23
+#define	GPIO_PG8	24
+#define	GPIO_PG9	25
+#define	GPIO_PG10      	26
+#define	GPIO_PG11      	27
+#define	GPIO_PG12      	28
+#define	GPIO_PG13      	29
+#define	GPIO_PG14      	30
+#define	GPIO_PG15      	31
+#define	GPIO_PH0	32
+#define	GPIO_PH1	33
+#define	GPIO_PH2	34
+#define	GPIO_PH3	35
+#define	GPIO_PH4	36
+#define	GPIO_PH5	37
+#define	GPIO_PH6	38
+#define	GPIO_PH7	39
+#define	GPIO_PH8	40
+#define	GPIO_PH9	41
+#define	GPIO_PH10      	42
+#define	GPIO_PH11      	43
+#define	GPIO_PH12      	44
+#define	GPIO_PH13      	45
+#define	GPIO_PH14      	46
+#define	GPIO_PH15      	47
+
+#define PORT_F GPIO_PF0
+#define PORT_G GPIO_PG0
+#define PORT_H GPIO_PH0
+
+#endif /* _MACH_GPIO_H_ */
diff --git a/arch/blackfin/mach-bf537/include/mach/irq.h b/arch/blackfin/mach-bf537/include/mach/irq.h
index 2e68a8a1e730..b2a71d5d4e5f 100644
--- a/arch/blackfin/mach-bf537/include/mach/irq.h
+++ b/arch/blackfin/mach-bf537/include/mach/irq.h
@@ -82,14 +82,14 @@
 #define IRQ_CAN_TX          23	/*CAN Transmit Interrupt */
 #define IRQ_MAC_RX          24	/*DMA1 (Ethernet RX) Interrupt */
 #define IRQ_MAC_TX          25	/*DMA2 (Ethernet TX) Interrupt */
-#define IRQ_TMR0            26	/*Timer 0 */
-#define IRQ_TMR1            27	/*Timer 1 */
-#define IRQ_TMR2            28	/*Timer 2 */
-#define IRQ_TMR3            29	/*Timer 3 */
-#define IRQ_TMR4            30	/*Timer 4 */
-#define IRQ_TMR5            31	/*Timer 5 */
-#define IRQ_TMR6            32	/*Timer 6 */
-#define IRQ_TMR7            33	/*Timer 7 */
+#define IRQ_TIMER0            26	/*Timer 0 */
+#define IRQ_TIMER1            27	/*Timer 1 */
+#define IRQ_TIMER2            28	/*Timer 2 */
+#define IRQ_TIMER3            29	/*Timer 3 */
+#define IRQ_TIMER4            30	/*Timer 4 */
+#define IRQ_TIMER5            31	/*Timer 5 */
+#define IRQ_TIMER6            32	/*Timer 6 */
+#define IRQ_TIMER7            33	/*Timer 7 */
 #define IRQ_PROG_INTA       34	/* PF Ports F&G (PF15:0) Interrupt A */
 #define IRQ_PORTG_INTB      35	/* PF Port G (PF15:0) Interrupt B */
 #define IRQ_MEM_DMA0        36	/*(Memory DMA Stream 0) */
@@ -195,16 +195,16 @@
 #define IRQ_CAN_TX_POS      0
 #define IRQ_MAC_RX_POS      4
 #define IRQ_MAC_TX_POS      8
-#define IRQ_TMR0_POS        12
-#define IRQ_TMR1_POS        16
-#define IRQ_TMR2_POS        20
-#define IRQ_TMR3_POS        24
-#define IRQ_TMR4_POS        28
+#define IRQ_TIMER0_POS        12
+#define IRQ_TIMER1_POS        16
+#define IRQ_TIMER2_POS        20
+#define IRQ_TIMER3_POS        24
+#define IRQ_TIMER4_POS        28
 
 /* IAR3 BIT FIELDS*/
-#define IRQ_TMR5_POS        0
-#define IRQ_TMR6_POS        4
-#define IRQ_TMR7_POS        8
+#define IRQ_TIMER5_POS        0
+#define IRQ_TIMER6_POS        4
+#define IRQ_TIMER7_POS        8
 #define IRQ_PROG_INTA_POS   12
 #define IRQ_PORTG_INTB_POS   16
 #define IRQ_MEM_DMA0_POS    20
diff --git a/arch/blackfin/mach-bf537/include/mach/mem_init.h b/arch/blackfin/mach-bf537/include/mach/mem_init.h
deleted file mode 100644
index f67698f670ca..000000000000
--- a/arch/blackfin/mach-bf537/include/mach/mem_init.h
+++ /dev/null
@@ -1,303 +0,0 @@
-/*
- * File:         include/asm-blackfin/mach-bf537/mem_init.h
- * Based on:
- * Author:
- *
- * Created:
- * Description:
- *
- * Rev:
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, 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; see the file COPYING.
- * If not, write to the Free Software Foundation,
- * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-#if (CONFIG_MEM_MT48LC16M16A2TG_75 || CONFIG_MEM_MT48LC64M4A2FB_7E || CONFIG_MEM_MT48LC16M8A2TG_75 || CONFIG_MEM_GENERIC_BOARD || CONFIG_MEM_MT48LC32M8A2_75)
-#if (CONFIG_SCLK_HZ > 119402985)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_7
-#define SDRAM_tRAS_num  7
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 104477612) && (CONFIG_SCLK_HZ <= 119402985)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_6
-#define SDRAM_tRAS_num  6
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 89552239) && (CONFIG_SCLK_HZ <= 104477612)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_5
-#define SDRAM_tRAS_num  5
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 74626866) && (CONFIG_SCLK_HZ <= 89552239)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_4
-#define SDRAM_tRAS_num  4
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 66666667) && (CONFIG_SCLK_HZ <= 74626866)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_3
-#define SDRAM_tRAS_num  3
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 59701493) && (CONFIG_SCLK_HZ <= 66666667)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_4
-#define SDRAM_tRAS_num  3
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 44776119) && (CONFIG_SCLK_HZ <= 59701493)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_3
-#define SDRAM_tRAS_num  3
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 29850746) && (CONFIG_SCLK_HZ <= 44776119)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_2
-#define SDRAM_tRAS_num  2
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ <= 29850746)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_1
-#define SDRAM_tRAS_num  1
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#endif
-
-#if (CONFIG_MEM_MT48LC16M16A2TG_75)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-#if (CONFIG_MEM_MT48LC16M8A2TG_75)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   4096	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-#if (CONFIG_MEM_MT48LC32M8A2_75)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-#if (CONFIG_MEM_MT48LC64M4A2FB_7E)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-#if (CONFIG_MEM_GENERIC_BOARD)
-  /*SDRAM INFORMATION: Modify this for your board */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-/* Equation from section 17 (p17-46) of BF533 HRM */
-#define mem_SDRRC       (((CONFIG_SCLK_HZ / 1000) * SDRAM_Tref) / SDRAM_NRA) - (SDRAM_tRAS_num + SDRAM_tRP_num)
-
-/* Enable SCLK Out */
-#define mem_SDGCTL        (SCTLE | SDRAM_CL | SDRAM_tRAS | SDRAM_tRP | SDRAM_tRCD | SDRAM_tWR | PSS)
-
-#if defined CONFIG_CLKIN_HALF
-#define CLKIN_HALF       1
-#else
-#define CLKIN_HALF       0
-#endif
-
-#if defined CONFIG_PLL_BYPASS
-#define PLL_BYPASS      1
-#else
-#define PLL_BYPASS       0
-#endif
-
-/***************************************Currently Not Being Used *********************************/
-#define flash_EBIU_AMBCTL_WAT  ((CONFIG_FLASH_SPEED_BWAT * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-#define flash_EBIU_AMBCTL_RAT  ((CONFIG_FLASH_SPEED_BRAT * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-#define flash_EBIU_AMBCTL_HT   ((CONFIG_FLASH_SPEED_BHT  * 4) / (4000000000 / CONFIG_SCLK_HZ))
-#define flash_EBIU_AMBCTL_ST   ((CONFIG_FLASH_SPEED_BST  * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-#define flash_EBIU_AMBCTL_TT   ((CONFIG_FLASH_SPEED_BTT  * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-
-#if (flash_EBIU_AMBCTL_TT > 3)
-#define flash_EBIU_AMBCTL0_TT   B0TT_4
-#endif
-#if (flash_EBIU_AMBCTL_TT == 3)
-#define flash_EBIU_AMBCTL0_TT   B0TT_3
-#endif
-#if (flash_EBIU_AMBCTL_TT == 2)
-#define flash_EBIU_AMBCTL0_TT   B0TT_2
-#endif
-#if (flash_EBIU_AMBCTL_TT < 2)
-#define flash_EBIU_AMBCTL0_TT   B0TT_1
-#endif
-
-#if (flash_EBIU_AMBCTL_ST > 3)
-#define flash_EBIU_AMBCTL0_ST   B0ST_4
-#endif
-#if (flash_EBIU_AMBCTL_ST == 3)
-#define flash_EBIU_AMBCTL0_ST   B0ST_3
-#endif
-#if (flash_EBIU_AMBCTL_ST == 2)
-#define flash_EBIU_AMBCTL0_ST   B0ST_2
-#endif
-#if (flash_EBIU_AMBCTL_ST < 2)
-#define flash_EBIU_AMBCTL0_ST   B0ST_1
-#endif
-
-#if (flash_EBIU_AMBCTL_HT > 2)
-#define flash_EBIU_AMBCTL0_HT   B0HT_3
-#endif
-#if (flash_EBIU_AMBCTL_HT == 2)
-#define flash_EBIU_AMBCTL0_HT   B0HT_2
-#endif
-#if (flash_EBIU_AMBCTL_HT == 1)
-#define flash_EBIU_AMBCTL0_HT   B0HT_1
-#endif
-#if (flash_EBIU_AMBCTL_HT == 0 && CONFIG_FLASH_SPEED_BHT == 0)
-#define flash_EBIU_AMBCTL0_HT   B0HT_0
-#endif
-#if (flash_EBIU_AMBCTL_HT == 0 && CONFIG_FLASH_SPEED_BHT != 0)
-#define flash_EBIU_AMBCTL0_HT   B0HT_1
-#endif
-
-#if (flash_EBIU_AMBCTL_WAT > 14)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_15
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 14)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_14
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 13)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_13
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 12)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_12
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 11)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_11
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 10)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_10
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 9)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_9
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 8)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_8
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 7)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_7
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 6)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_6
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 5)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_5
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 4)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_4
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 3)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_3
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 2)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_2
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 1)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_1
-#endif
-
-#if (flash_EBIU_AMBCTL_RAT > 14)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_15
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 14)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_14
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 13)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_13
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 12)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_12
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 11)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_11
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 10)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_10
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 9)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_9
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 8)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_8
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 7)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_7
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 6)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_6
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 5)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_5
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 4)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_4
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 3)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_3
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 2)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_2
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 1)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_1
-#endif
-
-#define flash_EBIU_AMBCTL0  \
-	(flash_EBIU_AMBCTL0_WAT | flash_EBIU_AMBCTL0_RAT | flash_EBIU_AMBCTL0_HT | \
-	 flash_EBIU_AMBCTL0_ST | flash_EBIU_AMBCTL0_TT | CONFIG_FLASH_SPEED_RDYEN)
diff --git a/arch/blackfin/mach-bf537/include/mach/mem_map.h b/arch/blackfin/mach-bf537/include/mach/mem_map.h
index 5078b669431f..f9010c4b4bf3 100644
--- a/arch/blackfin/mach-bf537/include/mach/mem_map.h
+++ b/arch/blackfin/mach-bf537/include/mach/mem_map.h
@@ -176,4 +176,10 @@
 #define L1_SCRATCH_START	0xFFB00000
 #define L1_SCRATCH_LENGTH	0x1000
 
+#define GET_PDA_SAFE(preg)		\
+	preg.l = _cpu_pda;		\
+	preg.h = _cpu_pda;
+
+#define GET_PDA(preg, dreg)	GET_PDA_SAFE(preg)
+
 #endif				/* _MEM_MAP_537_H_ */
diff --git a/arch/blackfin/mach-bf537/ints-priority.c b/arch/blackfin/mach-bf537/ints-priority.c
index b1300b3f1812..51c48087e03b 100644
--- a/arch/blackfin/mach-bf537/ints-priority.c
+++ b/arch/blackfin/mach-bf537/ints-priority.c
@@ -55,15 +55,15 @@ void __init program_IAR(void)
 	bfin_write_SIC_IAR2(((CONFIG_IRQ_CAN_TX - 7) << IRQ_CAN_TX_POS) |
 			    ((CONFIG_IRQ_MAC_RX - 7) << IRQ_MAC_RX_POS) |
 			    ((CONFIG_IRQ_MAC_TX - 7) << IRQ_MAC_TX_POS) |
-			    ((CONFIG_IRQ_TMR0 - 7) << IRQ_TMR0_POS) |
-			    ((CONFIG_IRQ_TMR1 - 7) << IRQ_TMR1_POS) |
-			    ((CONFIG_IRQ_TMR2 - 7) << IRQ_TMR2_POS) |
-			    ((CONFIG_IRQ_TMR3 - 7) << IRQ_TMR3_POS) |
-			    ((CONFIG_IRQ_TMR4 - 7) << IRQ_TMR4_POS));
+			    ((CONFIG_IRQ_TIMER0 - 7) << IRQ_TIMER0_POS) |
+			    ((CONFIG_IRQ_TIMER1 - 7) << IRQ_TIMER1_POS) |
+			    ((CONFIG_IRQ_TIMER2 - 7) << IRQ_TIMER2_POS) |
+			    ((CONFIG_IRQ_TIMER3 - 7) << IRQ_TIMER3_POS) |
+			    ((CONFIG_IRQ_TIMER4 - 7) << IRQ_TIMER4_POS));
 
-	bfin_write_SIC_IAR3(((CONFIG_IRQ_TMR5 - 7) << IRQ_TMR5_POS) |
-			    ((CONFIG_IRQ_TMR6 - 7) << IRQ_TMR6_POS) |
-			    ((CONFIG_IRQ_TMR7 - 7) << IRQ_TMR7_POS) |
+	bfin_write_SIC_IAR3(((CONFIG_IRQ_TIMER5 - 7) << IRQ_TIMER5_POS) |
+			    ((CONFIG_IRQ_TIMER6 - 7) << IRQ_TIMER6_POS) |
+			    ((CONFIG_IRQ_TIMER7 - 7) << IRQ_TIMER7_POS) |
 			    ((CONFIG_IRQ_PROG_INTA - 7) << IRQ_PROG_INTA_POS) |
 			    ((CONFIG_IRQ_PORTG_INTB - 7) << IRQ_PORTG_INTB_POS) |
 			    ((CONFIG_IRQ_MEM_DMA0 - 7) << IRQ_MEM_DMA0_POS) |
diff --git a/arch/blackfin/mach-bf538/Kconfig b/arch/blackfin/mach-bf538/Kconfig
new file mode 100644
index 000000000000..f068c3523cdc
--- /dev/null
+++ b/arch/blackfin/mach-bf538/Kconfig
@@ -0,0 +1,164 @@
+if (BF538 || BF539)
+
+source "arch/blackfin/mach-bf538/boards/Kconfig"
+
+menu "BF538 Specific Configuration"
+
+comment "Interrupt Priority Assignment"
+menu "Priority"
+
+config IRQ_PLL_WAKEUP
+	int "IRQ_PLL_WAKEUP"
+	default 7
+config IRQ_DMA0_ERROR
+	int "IRQ_DMA0_ERROR"
+	default 7
+config IRQ_PPI_ERROR
+	int "IRQ_PPI_ERROR"
+	default 7
+config IRQ_SPORT0_ERROR
+	int "IRQ_SPORT0_ERROR"
+	default 7
+config IRQ_SPORT1_ERROR
+	int "IRQ_SPORT1_ERROR"
+	default 7
+config IRQ_SPI0_ERROR
+	int "IRQ_SPI0_ERROR"
+	default 7
+config IRQ_UART0_ERROR
+	int "IRQ_UART0_ERROR"
+	default 7
+config IRQ_RTC
+	int "IRQ_RTC"
+	default 8
+config IRQ_PPI
+	int "IRQ_PPI"
+	default 8
+config IRQ_SPORT0_RX
+	int "IRQ_SPORT0_RX"
+	default 9
+config IRQ_SPORT0_TX
+	int "IRQ_SPORT0_TX"
+	default 9
+config IRQ_SPORT1_RX
+	int "IRQ_SPORT1_RX"
+	default 9
+config IRQ_SPORT1_TX
+	int "IRQ_SPORT1_TX"
+	default 9
+config IRQ_SPI0
+	int "IRQ_SPI0"
+	default 10
+config IRQ_UART0_RX
+	int "IRQ_UART0_RX"
+	default 10
+config IRQ_UART0_TX
+	int "IRQ_UART0_TX"
+	default 10
+config IRQ_TIMER0
+	int "IRQ_TIMER0"
+	default 8
+config IRQ_TIMER1
+	int "IRQ_TIMER1"
+	default 11
+config IRQ_TIMER2
+	int "IRQ_TIMER2"
+	default 11
+config IRQ_PORTF_INTA
+	int "IRQ_PORTF_INTA"
+	default 12
+config IRQ_PORTF_INTB
+	int "IRQ_PORTF_INTB"
+	default 12
+config IRQ_MEM0_DMA0
+	int "IRQ_MEM0_DMA0"
+	default 13
+config IRQ_MEM0_DMA1
+	int "IRQ_MEM0_DMA1"
+	default 13
+config IRQ_WATCH
+	int "IRQ_WATCH"
+	default 13
+config IRQ_DMA1_ERROR
+	int "IRQ_DMA1_ERROR"
+	default 7
+config IRQ_SPORT2_ERROR
+	int "IRQ_SPORT2_ERROR"
+	default 7
+config IRQ_SPORT3_ERROR
+	int "IRQ_SPORT3_ERROR"
+	default 7
+config IRQ_SPI1_ERROR
+	int "IRQ_SPI1_ERROR"
+	default 7
+config IRQ_SPI2_ERROR
+	int "IRQ_SPI2_ERROR"
+	default 7
+config IRQ_UART1_ERROR
+	int "IRQ_UART1_ERROR"
+	default 7
+config IRQ_UART2_ERROR
+	int "IRQ_UART2_ERROR"
+	default 7
+config IRQ_CAN_ERROR
+	int "IRQ_CAN_ERROR"
+	default 7
+config IRQ_SPORT2_RX
+	int "IRQ_SPORT2_RX"
+	default 9
+config IRQ_SPORT2_TX
+	int "IRQ_SPORT2_TX"
+	default 9
+config IRQ_SPORT3_RX
+	int "IRQ_SPORT3_RX"
+	default 9
+config IRQ_SPORT3_TX
+	int "IRQ_SPORT3_TX"
+	default 9
+config IRQ_SPI1
+	int "IRQ_SPI1"
+	default 10
+config IRQ_SPI2
+	int "IRQ_SPI2"
+	default 10
+config IRQ_UART1_RX
+	int "IRQ_UART1_RX"
+	default 10
+config IRQ_UART1_TX
+	int "IRQ_UART1_TX"
+	default 10
+config IRQ_UART2_RX
+	int "IRQ_UART2_RX"
+	default 10
+config IRQ_UART2_TX
+	int "IRQ_UART2_TX"
+	default 10
+config IRQ_TWI0
+	int "IRQ_TWI0"
+	default 11
+config IRQ_TWI1
+	int "IRQ_TWI1"
+	default 11
+config IRQ_CAN_RX
+	int "IRQ_CAN_RX"
+	default 11
+config IRQ_CAN_TX
+	int "IRQ_CAN_TX"
+	default 11
+config IRQ_MEM1_DMA0
+	int "IRQ_MEM1_DMA0"
+	default 13
+config IRQ_MEM1_DMA1
+	int "IRQ_MEM1_DMA1"
+	default 13
+
+	help
+	  Enter the priority numbers between 7-13 ONLY.  Others are Reserved.
+	  This applies to all the above.  It is not recommended to assign the
+	  highest priority number 7 to UART or any other device.
+
+endmenu
+
+endmenu
+
+endif
diff --git a/arch/blackfin/mach-bf538/Makefile b/arch/blackfin/mach-bf538/Makefile
new file mode 100644
index 000000000000..8cd2719684db
--- /dev/null
+++ b/arch/blackfin/mach-bf538/Makefile
@@ -0,0 +1,5 @@
+#
+# arch/blackfin/mach-bf538/Makefile
+#
+
+obj-y := ints-priority.o dma.o
diff --git a/arch/blackfin/mach-bf538/boards/Kconfig b/arch/blackfin/mach-bf538/boards/Kconfig
new file mode 100644
index 000000000000..215249ba58bb
--- /dev/null
+++ b/arch/blackfin/mach-bf538/boards/Kconfig
@@ -0,0 +1,12 @@
+choice
+	prompt "System type"
+	default BFIN538_EZKIT
+	help
+	  Select your board!
+
+config BFIN538_EZKIT
+	bool "BF538-EZKIT"
+	help
+	  BF538-EZKIT-LITE board support.
+
+endchoice
diff --git a/arch/blackfin/mach-bf538/boards/Makefile b/arch/blackfin/mach-bf538/boards/Makefile
new file mode 100644
index 000000000000..6143b320d585
--- /dev/null
+++ b/arch/blackfin/mach-bf538/boards/Makefile
@@ -0,0 +1,5 @@
+#
+# arch/blackfin/mach-bf538/boards/Makefile
+#
+
+obj-$(CONFIG_BFIN538_EZKIT)            += ezkit.o
diff --git a/arch/blackfin/mach-bf538/boards/ezkit.c b/arch/blackfin/mach-bf538/boards/ezkit.c
new file mode 100644
index 000000000000..e37cb9378884
--- /dev/null
+++ b/arch/blackfin/mach-bf538/boards/ezkit.c
@@ -0,0 +1,606 @@
+/*
+ * File:         arch/blackfin/mach-bf538/boards/ezkit.c
+ * Based on:     arch/blackfin/mach-bf537/boards/ezkit.c
+ * Author:       Aidan Williams <aidan@nicta.com.au>
+ *
+ * Created:
+ * Description:
+ *
+ * Modified:
+ *               Copyright 2005 National ICT Australia (NICTA)
+ *               Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+#include <linux/spi/spi.h>
+#include <linux/spi/flash.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <asm/bfin5xx_spi.h>
+#include <asm/dma.h>
+#include <asm/gpio.h>
+#include <asm/nand.h>
+#include <asm/portmux.h>
+#include <asm/dpmc.h>
+#include <linux/input.h>
+
+/*
+ * Name the Board for the /proc/cpuinfo
+ */
+const char bfin_board_name[] = "ADI BF538-EZKIT";
+
+/*
+ *  Driver needs to know address, irq and flag pin.
+ */
+
+
+#if defined(CONFIG_RTC_DRV_BFIN) || defined(CONFIG_RTC_DRV_BFIN_MODULE)
+static struct platform_device rtc_device = {
+	.name = "rtc-bfin",
+	.id   = -1,
+};
+#endif
+
+#if defined(CONFIG_SERIAL_BFIN) || defined(CONFIG_SERIAL_BFIN_MODULE)
+static struct resource bfin_uart_resources[] = {
+#ifdef CONFIG_SERIAL_BFIN_UART0
+	{
+		.start = 0xFFC00400,
+		.end = 0xFFC004FF,
+		.flags = IORESOURCE_MEM,
+	},
+#endif
+#ifdef CONFIG_SERIAL_BFIN_UART1
+	{
+		.start = 0xFFC02000,
+		.end = 0xFFC020FF,
+		.flags = IORESOURCE_MEM,
+	},
+#endif
+#ifdef CONFIG_SERIAL_BFIN_UART2
+	{
+		.start = 0xFFC02100,
+		.end = 0xFFC021FF,
+		.flags = IORESOURCE_MEM,
+	},
+#endif
+};
+
+static struct platform_device bfin_uart_device = {
+	.name = "bfin-uart",
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_uart_resources),
+	.resource = bfin_uart_resources,
+};
+#endif
+
+#if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
+#ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
+	{
+		.start = 0xFFC00400,
+		.end = 0xFFC004FF,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
+#endif
+#ifdef CONFIG_BFIN_SIR1
+static struct resource bfin_sir1_resources[] = {
+	{
+		.start = 0xFFC02000,
+		.end = 0xFFC020FF,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = IRQ_UART1_RX,
+		.end = IRQ_UART1_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART1_RX,
+		.end = CH_UART1_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+static struct platform_device bfin_sir1_device = {
+	.name = "bfin_sir",
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_sir1_resources),
+	.resource = bfin_sir1_resources,
+};
+#endif
+#ifdef CONFIG_BFIN_SIR2
+static struct resource bfin_sir2_resources[] = {
+	{
+		.start = 0xFFC02100,
+		.end = 0xFFC021FF,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = IRQ_UART2_RX,
+		.end = IRQ_UART2_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART2_RX,
+		.end = CH_UART2_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+static struct platform_device bfin_sir2_device = {
+	.name = "bfin_sir",
+	.id = 2,
+	.num_resources = ARRAY_SIZE(bfin_sir2_resources),
+	.resource = bfin_sir2_resources,
+};
+#endif
+#endif
+
+/*
+ *  USB-LAN EzExtender board
+ *  Driver needs to know address, irq and flag pin.
+ */
+#if defined(CONFIG_SMC91X) || defined(CONFIG_SMC91X_MODULE)
+static struct resource smc91x_resources[] = {
+	{
+		.name = "smc91x-regs",
+		.start = 0x20310300,
+		.end = 0x20310300 + 16,
+		.flags = IORESOURCE_MEM,
+	}, {
+		.start = IRQ_PF0,
+		.end = IRQ_PF0,
+		.flags = IORESOURCE_IRQ | IORESOURCE_IRQ_HIGHLEVEL,
+	},
+};
+static struct platform_device smc91x_device = {
+	.name = "smc91x",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(smc91x_resources),
+	.resource = smc91x_resources,
+};
+#endif
+
+#if defined(CONFIG_SPI_BFIN) || defined(CONFIG_SPI_BFIN_MODULE)
+/* all SPI peripherals info goes here */
+#if defined(CONFIG_MTD_M25P80) \
+	|| defined(CONFIG_MTD_M25P80_MODULE)
+/* SPI flash chip (m25p16) */
+static struct mtd_partition bfin_spi_flash_partitions[] = {
+	{
+		.name = "bootloader(spi)",
+		.size = 0x00040000,
+		.offset = 0,
+		.mask_flags = MTD_CAP_ROM
+	}, {
+		.name = "linux kernel(spi)",
+		.size = 0x1c0000,
+		.offset = 0x40000
+	}
+};
+
+static struct flash_platform_data bfin_spi_flash_data = {
+	.name = "m25p80",
+	.parts = bfin_spi_flash_partitions,
+	.nr_parts = ARRAY_SIZE(bfin_spi_flash_partitions),
+	.type = "m25p16",
+};
+
+static struct bfin5xx_spi_chip spi_flash_chip_info = {
+	.enable_dma = 0,         /* use dma transfer with this chip*/
+	.bits_per_word = 8,
+	.cs_change_per_word = 0,
+};
+#endif
+
+#if defined(CONFIG_TOUCHSCREEN_AD7879) || defined(CONFIG_TOUCHSCREEN_AD7879_MODULE)
+#include <linux/spi/ad7879.h>
+static const struct ad7879_platform_data bfin_ad7879_ts_info = {
+	.model			= 7879,	/* Model = AD7879 */
+	.x_plate_ohms		= 620,	/* 620 Ohm from the touch datasheet */
+	.pressure_max		= 10000,
+	.pressure_min		= 0,
+	.first_conversion_delay = 3,	/* wait 512us before do a first conversion */
+	.acquisition_time 	= 1,	/* 4us acquisition time per sample */
+	.median			= 2,	/* do 8 measurements */
+	.averaging 		= 1,	/* take the average of 4 middle samples */
+	.pen_down_acc_interval 	= 255,	/* 9.4 ms */
+	.gpio_output		= 1,	/* configure AUX/VBAT/GPIO as GPIO output */
+	.gpio_default 		= 1,	/* During initialization set GPIO = HIGH */
+};
+#endif
+
+#if defined(CONFIG_TOUCHSCREEN_AD7879_SPI) || defined(CONFIG_TOUCHSCREEN_AD7879_SPI_MODULE)
+static struct bfin5xx_spi_chip spi_ad7879_chip_info = {
+	.enable_dma = 0,
+	.bits_per_word = 16,
+};
+#endif
+
+#if defined(CONFIG_FB_BFIN_LQ035Q1) || defined(CONFIG_FB_BFIN_LQ035Q1_MODULE)
+#include <asm/bfin-lq035q1.h>
+
+static struct bfin_lq035q1fb_disp_info bfin_lq035q1_data = {
+	.mode = 	LQ035_NORM | LQ035_RGB | LQ035_RL | LQ035_TB,
+	.use_bl = 	0,	/* let something else control the LCD Blacklight */
+	.gpio_bl =	GPIO_PF7,
+};
+
+static struct resource bfin_lq035q1_resources[] = {
+	{
+		.start = IRQ_PPI_ERROR,
+		.end = IRQ_PPI_ERROR,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static struct platform_device bfin_lq035q1_device = {
+	.name		= "bfin-lq035q1",
+	.id		= -1,
+	.num_resources 	= ARRAY_SIZE(bfin_lq035q1_resources),
+	.resource 	= bfin_lq035q1_resources,
+	.dev		= {
+		.platform_data = &bfin_lq035q1_data,
+	},
+};
+#endif
+
+#if defined(CONFIG_SPI_SPIDEV) || defined(CONFIG_SPI_SPIDEV_MODULE)
+static struct bfin5xx_spi_chip spidev_chip_info = {
+	.enable_dma = 0,
+	.bits_per_word = 8,
+};
+#endif
+
+#if defined(CONFIG_FB_BFIN_LQ035Q1) || defined(CONFIG_FB_BFIN_LQ035Q1_MODULE)
+static struct bfin5xx_spi_chip lq035q1_spi_chip_info = {
+	.enable_dma	= 0,
+	.bits_per_word	= 8,
+};
+#endif
+
+static struct spi_board_info bf538_spi_board_info[] __initdata = {
+#if defined(CONFIG_MTD_M25P80) \
+	|| defined(CONFIG_MTD_M25P80_MODULE)
+	{
+		/* the modalias must be the same as spi device driver name */
+		.modalias = "m25p80", /* Name of spi_driver for this device */
+		.max_speed_hz = 25000000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num = 0, /* Framework bus number */
+		.chip_select = 1, /* SPI_SSEL1*/
+		.platform_data = &bfin_spi_flash_data,
+		.controller_data = &spi_flash_chip_info,
+		.mode = SPI_MODE_3,
+	},
+#endif
+#if defined(CONFIG_TOUCHSCREEN_AD7879_SPI) || defined(CONFIG_TOUCHSCREEN_AD7879_SPI_MODULE)
+	{
+		.modalias = "ad7879",
+		.platform_data = &bfin_ad7879_ts_info,
+		.irq = IRQ_PF3,
+		.max_speed_hz = 5000000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num = 0,
+		.chip_select = 1,
+		.controller_data = &spi_ad7879_chip_info,
+		.mode = SPI_CPHA | SPI_CPOL,
+	},
+#endif
+#if defined(CONFIG_FB_BFIN_LQ035Q1) || defined(CONFIG_FB_BFIN_LQ035Q1_MODULE)
+	{
+		.modalias = "bfin-lq035q1-spi",
+		.max_speed_hz = 20000000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num = 0,
+		.chip_select = 2,
+		.controller_data = &lq035q1_spi_chip_info,
+		.mode = SPI_CPHA | SPI_CPOL,
+	},
+#endif
+#if defined(CONFIG_SPI_SPIDEV) || defined(CONFIG_SPI_SPIDEV_MODULE)
+	{
+		.modalias = "spidev",
+		.max_speed_hz = 3125000,     /* max spi clock (SCK) speed in HZ */
+		.bus_num = 0,
+		.chip_select = 1,
+		.controller_data = &spidev_chip_info,
+	},
+#endif
+};
+
+/* SPI (0) */
+static struct resource bfin_spi0_resource[] = {
+	[0] = {
+		.start = SPI0_REGBASE,
+		.end   = SPI0_REGBASE + 0xFF,
+		.flags = IORESOURCE_MEM,
+	},
+	[1] = {
+		.start = CH_SPI0,
+		.end   = CH_SPI0,
+		.flags = IORESOURCE_IRQ,
+	}
+};
+
+/* SPI (1) */
+static struct resource bfin_spi1_resource[] = {
+	[0] = {
+		.start = SPI1_REGBASE,
+		.end   = SPI1_REGBASE + 0xFF,
+		.flags = IORESOURCE_MEM,
+	},
+	[1] = {
+		.start = CH_SPI1,
+		.end   = CH_SPI1,
+		.flags = IORESOURCE_IRQ,
+	}
+};
+
+/* SPI (2) */
+static struct resource bfin_spi2_resource[] = {
+	[0] = {
+		.start = SPI2_REGBASE,
+		.end   = SPI2_REGBASE + 0xFF,
+		.flags = IORESOURCE_MEM,
+	},
+	[1] = {
+		.start = CH_SPI2,
+		.end   = CH_SPI2,
+		.flags = IORESOURCE_IRQ,
+	}
+};
+
+/* SPI controller data */
+static struct bfin5xx_spi_master bf538_spi_master_info0 = {
+	.num_chipselect = 8,
+	.enable_dma = 1,  /* master has the ability to do dma transfer */
+	.pin_req = {P_SPI0_SCK, P_SPI0_MISO, P_SPI0_MOSI, 0},
+};
+
+static struct platform_device bf538_spi_master0 = {
+	.name = "bfin-spi",
+	.id = 0, /* Bus number */
+	.num_resources = ARRAY_SIZE(bfin_spi0_resource),
+	.resource = bfin_spi0_resource,
+	.dev = {
+		.platform_data = &bf538_spi_master_info0, /* Passed to driver */
+		},
+};
+
+static struct bfin5xx_spi_master bf538_spi_master_info1 = {
+	.num_chipselect = 8,
+	.enable_dma = 1,  /* master has the ability to do dma transfer */
+	.pin_req = {P_SPI1_SCK, P_SPI1_MISO, P_SPI1_MOSI, 0},
+};
+
+static struct platform_device bf538_spi_master1 = {
+	.name = "bfin-spi",
+	.id = 1, /* Bus number */
+	.num_resources = ARRAY_SIZE(bfin_spi1_resource),
+	.resource = bfin_spi1_resource,
+	.dev = {
+		.platform_data = &bf538_spi_master_info1, /* Passed to driver */
+		},
+};
+
+static struct bfin5xx_spi_master bf538_spi_master_info2 = {
+	.num_chipselect = 8,
+	.enable_dma = 1,  /* master has the ability to do dma transfer */
+	.pin_req = {P_SPI2_SCK, P_SPI2_MISO, P_SPI2_MOSI, 0},
+};
+
+static struct platform_device bf538_spi_master2 = {
+	.name = "bfin-spi",
+	.id = 2, /* Bus number */
+	.num_resources = ARRAY_SIZE(bfin_spi2_resource),
+	.resource = bfin_spi2_resource,
+	.dev = {
+		.platform_data = &bf538_spi_master_info2, /* Passed to driver */
+		},
+};
+
+#endif  /* spi master and devices */
+
+#if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
+static struct resource bfin_twi0_resource[] = {
+	[0] = {
+		.start = TWI0_REGBASE,
+		.end   = TWI0_REGBASE + 0xFF,
+		.flags = IORESOURCE_MEM,
+	},
+	[1] = {
+		.start = IRQ_TWI0,
+		.end   = IRQ_TWI0,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static struct platform_device i2c_bfin_twi0_device = {
+	.name = "i2c-bfin-twi",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_twi0_resource),
+	.resource = bfin_twi0_resource,
+};
+
+#if !defined(CONFIG_BF542)	/* The BF542 only has 1 TWI */
+static struct resource bfin_twi1_resource[] = {
+	[0] = {
+		.start = TWI1_REGBASE,
+		.end   = TWI1_REGBASE + 0xFF,
+		.flags = IORESOURCE_MEM,
+	},
+	[1] = {
+		.start = IRQ_TWI1,
+		.end   = IRQ_TWI1,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static struct platform_device i2c_bfin_twi1_device = {
+	.name = "i2c-bfin-twi",
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_twi1_resource),
+	.resource = bfin_twi1_resource,
+};
+#endif
+#endif
+
+static struct resource bfin_gpios_resources = {
+	.start = 0,
+	.end   = MAX_BLACKFIN_GPIOS - 1,
+	.flags = IORESOURCE_IRQ,
+};
+
+static struct platform_device bfin_gpios_device = {
+	.name = "simple-gpio",
+	.id = -1,
+	.num_resources = 1,
+	.resource = &bfin_gpios_resources,
+};
+
+#if defined(CONFIG_KEYBOARD_GPIO) || defined(CONFIG_KEYBOARD_GPIO_MODULE)
+#include <linux/gpio_keys.h>
+
+static struct gpio_keys_button bfin_gpio_keys_table[] = {
+	{BTN_0, GPIO_PC7, 1, "gpio-keys: BTN0"},
+};
+
+static struct gpio_keys_platform_data bfin_gpio_keys_data = {
+	.buttons        = bfin_gpio_keys_table,
+	.nbuttons       = ARRAY_SIZE(bfin_gpio_keys_table),
+};
+
+static struct platform_device bfin_device_gpiokeys = {
+	.name      = "gpio-keys",
+	.dev = {
+		.platform_data = &bfin_gpio_keys_data,
+	},
+};
+#endif
+
+static const unsigned int cclk_vlev_datasheet[] =
+{
+/*
+ * Internal VLEV BF538SBBC1533
+ ****temporarily using these values until data sheet is updated
+ */
+	VRPAIR(VLEV_100, 150000000),
+	VRPAIR(VLEV_100, 250000000),
+	VRPAIR(VLEV_110, 276000000),
+	VRPAIR(VLEV_115, 301000000),
+	VRPAIR(VLEV_120, 525000000),
+	VRPAIR(VLEV_125, 550000000),
+	VRPAIR(VLEV_130, 600000000),
+};
+
+static struct bfin_dpmc_platform_data bfin_dmpc_vreg_data = {
+	.tuple_tab = cclk_vlev_datasheet,
+	.tabsize = ARRAY_SIZE(cclk_vlev_datasheet),
+	.vr_settling_time = 25 /* us */,
+};
+
+static struct platform_device bfin_dpmc = {
+	.name = "bfin dpmc",
+	.dev = {
+		.platform_data = &bfin_dmpc_vreg_data,
+	},
+};
+
+static struct platform_device *cm_bf538_devices[] __initdata = {
+
+	&bfin_dpmc,
+
+#if defined(CONFIG_RTC_DRV_BFIN) || defined(CONFIG_RTC_DRV_BFIN_MODULE)
+	&rtc_device,
+#endif
+
+#if defined(CONFIG_SERIAL_BFIN) || defined(CONFIG_SERIAL_BFIN_MODULE)
+	&bfin_uart_device,
+#endif
+
+#if defined(CONFIG_SPI_BFIN) || defined(CONFIG_SPI_BFIN_MODULE)
+	&bf538_spi_master0,
+	&bf538_spi_master1,
+	&bf538_spi_master2,
+#endif
+
+#if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
+	&i2c_bfin_twi0_device,
+	&i2c_bfin_twi1_device,
+#endif
+
+#if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#ifdef CONFIG_BFIN_SIR1
+	&bfin_sir1_device,
+#endif
+#ifdef CONFIG_BFIN_SIR2
+	&bfin_sir2_device,
+#endif
+#endif
+
+#if defined(CONFIG_SMC91X) || defined(CONFIG_SMC91X_MODULE)
+	&smc91x_device,
+#endif
+
+#if defined(CONFIG_FB_BFIN_LQ035Q1) || defined(CONFIG_FB_BFIN_LQ035Q1_MODULE)
+	&bfin_lq035q1_device,
+#endif
+
+#if defined(CONFIG_KEYBOARD_GPIO) || defined(CONFIG_KEYBOARD_GPIO_MODULE)
+	&bfin_device_gpiokeys,
+#endif
+
+	&bfin_gpios_device,
+};
+
+static int __init ezkit_init(void)
+{
+	printk(KERN_INFO "%s(): registering device resources\n", __func__);
+	platform_add_devices(cm_bf538_devices, ARRAY_SIZE(cm_bf538_devices));
+
+#if defined(CONFIG_SPI_BFIN) || defined(CONFIG_SPI_BFIN_MODULE)
+	spi_register_board_info(bf538_spi_board_info,
+			ARRAY_SIZE(bf538_spi_board_info));
+#endif
+
+	return 0;
+}
+
+arch_initcall(ezkit_init);
diff --git a/arch/blackfin/mach-bf538/dma.c b/arch/blackfin/mach-bf538/dma.c
new file mode 100644
index 000000000000..d6837fbf94ea
--- /dev/null
+++ b/arch/blackfin/mach-bf538/dma.c
@@ -0,0 +1,161 @@
+/*
+ * File:         arch/blackfin/mach-bf538/dma.c
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:  This file contains the simple DMA Implementation for Blackfin
+ *
+ * Modified:
+ *               Copyright 2008 Analog Devices Inc.
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+#include <linux/module.h>
+
+#include <asm/blackfin.h>
+#include <asm/dma.h>
+
+struct dma_register *dma_io_base_addr[MAX_DMA_CHANNELS] = {
+	(struct dma_register *) DMA0_NEXT_DESC_PTR,
+	(struct dma_register *) DMA1_NEXT_DESC_PTR,
+	(struct dma_register *) DMA2_NEXT_DESC_PTR,
+	(struct dma_register *) DMA3_NEXT_DESC_PTR,
+	(struct dma_register *) DMA4_NEXT_DESC_PTR,
+	(struct dma_register *) DMA5_NEXT_DESC_PTR,
+	(struct dma_register *) DMA6_NEXT_DESC_PTR,
+	(struct dma_register *) DMA7_NEXT_DESC_PTR,
+	(struct dma_register *) DMA8_NEXT_DESC_PTR,
+	(struct dma_register *) DMA9_NEXT_DESC_PTR,
+	(struct dma_register *) DMA10_NEXT_DESC_PTR,
+	(struct dma_register *) DMA11_NEXT_DESC_PTR,
+	(struct dma_register *) DMA12_NEXT_DESC_PTR,
+	(struct dma_register *) DMA13_NEXT_DESC_PTR,
+	(struct dma_register *) DMA14_NEXT_DESC_PTR,
+	(struct dma_register *) DMA15_NEXT_DESC_PTR,
+	(struct dma_register *) DMA16_NEXT_DESC_PTR,
+	(struct dma_register *) DMA17_NEXT_DESC_PTR,
+	(struct dma_register *) DMA18_NEXT_DESC_PTR,
+	(struct dma_register *) DMA19_NEXT_DESC_PTR,
+	(struct dma_register *) MDMA0_D0_NEXT_DESC_PTR,
+	(struct dma_register *) MDMA0_S0_NEXT_DESC_PTR,
+	(struct dma_register *) MDMA0_D1_NEXT_DESC_PTR,
+	(struct dma_register *) MDMA0_S1_NEXT_DESC_PTR,
+	(struct dma_register *) MDMA1_D0_NEXT_DESC_PTR,
+	(struct dma_register *) MDMA1_S0_NEXT_DESC_PTR,
+	(struct dma_register *) MDMA1_D1_NEXT_DESC_PTR,
+	(struct dma_register *) MDMA1_S1_NEXT_DESC_PTR,
+};
+EXPORT_SYMBOL(dma_io_base_addr);
+
+int channel2irq(unsigned int channel)
+{
+	int ret_irq = -1;
+
+	switch (channel) {
+	case CH_PPI:
+		ret_irq = IRQ_PPI;
+		break;
+
+	case CH_UART0_RX:
+		ret_irq = IRQ_UART0_RX;
+		break;
+
+	case CH_UART0_TX:
+		ret_irq = IRQ_UART0_TX;
+		break;
+
+	case CH_UART1_RX:
+		ret_irq = IRQ_UART1_RX;
+		break;
+
+	case CH_UART1_TX:
+		ret_irq = IRQ_UART1_TX;
+		break;
+
+	case CH_UART2_RX:
+		ret_irq = IRQ_UART2_RX;
+		break;
+
+	case CH_UART2_TX:
+		ret_irq = IRQ_UART2_TX;
+		break;
+
+	case CH_SPORT0_RX:
+		ret_irq = IRQ_SPORT0_RX;
+		break;
+
+	case CH_SPORT0_TX:
+		ret_irq = IRQ_SPORT0_TX;
+		break;
+
+	case CH_SPORT1_RX:
+		ret_irq = IRQ_SPORT1_RX;
+		break;
+
+	case CH_SPORT1_TX:
+		ret_irq = IRQ_SPORT1_TX;
+		break;
+
+	case CH_SPORT2_RX:
+		ret_irq = IRQ_SPORT2_RX;
+		break;
+
+	case CH_SPORT2_TX:
+		ret_irq = IRQ_SPORT2_TX;
+		break;
+
+	case CH_SPORT3_RX:
+		ret_irq = IRQ_SPORT3_RX;
+		break;
+
+	case CH_SPORT3_TX:
+		ret_irq = IRQ_SPORT3_TX;
+		break;
+
+	case CH_SPI0:
+		ret_irq = IRQ_SPI0;
+		break;
+
+	case CH_SPI1:
+		ret_irq = IRQ_SPI1;
+		break;
+
+	case CH_SPI2:
+		ret_irq = IRQ_SPI2;
+		break;
+
+	case CH_MEM_STREAM0_SRC:
+	case CH_MEM_STREAM0_DEST:
+		ret_irq = IRQ_MEM0_DMA0;
+		break;
+	case CH_MEM_STREAM1_SRC:
+	case CH_MEM_STREAM1_DEST:
+		ret_irq = IRQ_MEM0_DMA1;
+		break;
+	case CH_MEM_STREAM2_SRC:
+	case CH_MEM_STREAM2_DEST:
+		ret_irq = IRQ_MEM1_DMA0;
+		break;
+	case CH_MEM_STREAM3_SRC:
+	case CH_MEM_STREAM3_DEST:
+		ret_irq = IRQ_MEM1_DMA1;
+		break;
+	}
+	return ret_irq;
+}
diff --git a/arch/blackfin/mach-bf538/include/mach/anomaly.h b/arch/blackfin/mach-bf538/include/mach/anomaly.h
new file mode 100644
index 000000000000..e130b4f8a05d
--- /dev/null
+++ b/arch/blackfin/mach-bf538/include/mach/anomaly.h
@@ -0,0 +1,132 @@
+/*
+ * File: include/asm-blackfin/mach-bf538/anomaly.h
+ * Bugs: Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Copyright (C) 2004-2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
+ */
+
+/* This file shoule be up to date with:
+ *  - Revision G, 09/18/2008; ADSP-BF538/BF538F Blackfin Processor Anomaly List
+ *  - Revision L, 09/18/2008; ADSP-BF539/BF539F Blackfin Processor Anomaly List
+ */
+
+#ifndef _MACH_ANOMALY_H_
+#define _MACH_ANOMALY_H_
+
+#if __SILICON_REVISION__ < 4
+# error will not work on BF538 silicon version 0.0, 0.1, 0.2, or 0.3
+#endif
+
+/* Multi-Issue Instruction with dsp32shiftimm in slot1 and P-reg Store in slot2 Not Supported */
+#define ANOMALY_05000074 (1)
+/* DMA_RUN Bit Is Not Valid after a Peripheral Receive Channel DMA Stops */
+#define ANOMALY_05000119 (1)
+/* Rx.H Cannot Be Used to Access 16-bit System MMR Registers */
+#define ANOMALY_05000122 (1)
+/* PPI Data Lengths between 8 and 16 Do Not Zero Out Upper Bits */
+#define ANOMALY_05000166 (1)
+/* PPI_COUNT Cannot Be Programmed to 0 in General Purpose TX or RX Modes */
+#define ANOMALY_05000179 (1)
+/* PPI_DELAY Not Functional in PPI Modes with 0 Frame Syncs */
+#define ANOMALY_05000180 (1)
+/* False I/O Pin Interrupts on Edge-Sensitive Inputs When Polarity Setting Is Changed */
+#define ANOMALY_05000193 (1)
+/* Current DMA Address Shows Wrong Value During Carry Fix */
+#define ANOMALY_05000199 (__SILICON_REVISION__ < 4)
+/* NMI Event at Boot Time Results in Unpredictable State */
+#define ANOMALY_05000219 (1)
+/* SPI Slave Boot Mode Modifies Registers from Reset Value */
+#define ANOMALY_05000229 (1)
+/* PPI_FS3 Is Not Driven in 2 or 3 Internal Frame Sync Transmit Modes */
+#define ANOMALY_05000233 (1)
+/* If i-cache is on, CSYNC/SSYNC/IDLE around Change of Control causes failures */
+#define ANOMALY_05000244 (__SILICON_REVISION__ < 3)
+/* Spurious Hardware Error from an Access in the Shadow of a Conditional Branch */
+#define ANOMALY_05000245 (1)
+/* Maximum External Clock Speed for Timers */
+#define ANOMALY_05000253 (1)
+/* DCPLB_FAULT_ADDR MMR register may be corrupted */
+#define ANOMALY_05000261 (__SILICON_REVISION__ < 3)
+/* High I/O Activity Causes Output Voltage of Internal Voltage Regulator (Vddint) to Decrease */
+#define ANOMALY_05000270 (__SILICON_REVISION__ < 4)
+/* Certain Data Cache Writethrough Modes Fail for Vddint <= 0.9V */
+#define ANOMALY_05000272 (1)
+/* Writes to Synchronous SDRAM Memory May Be Lost */
+#define ANOMALY_05000273 (__SILICON_REVISION__ < 4)
+/* Writes to an I/O Data Register One SCLK Cycle after an Edge Is Detected May Clear Interrupt */
+#define ANOMALY_05000277 (__SILICON_REVISION__ < 4)
+/* Disabling Peripherals with DMA Running May Cause DMA System Instability */
+#define ANOMALY_05000278 (__SILICON_REVISION__ < 4)
+/* False Hardware Error Exception when ISR Context Is Not Restored */
+#define ANOMALY_05000281 (__SILICON_REVISION__ < 4)
+/* Memory DMA Corruption with 32-Bit Data and Traffic Control */
+#define ANOMALY_05000282 (__SILICON_REVISION__ < 4)
+/* System MMR Write Is Stalled Indefinitely when Killed in a Particular Stage */
+#define ANOMALY_05000283 (__SILICON_REVISION__ < 4)
+/* SPORTs May Receive Bad Data If FIFOs Fill Up */
+#define ANOMALY_05000288 (__SILICON_REVISION__ < 4)
+/* Reads from CAN Mailbox and Acceptance Mask Area Can Fail */
+#define ANOMALY_05000291 (__SILICON_REVISION__ < 4)
+/* Hibernate Leakage Current Is Higher Than Specified */
+#define ANOMALY_05000293 (__SILICON_REVISION__ < 4)
+/* Timer Pin Limitations for PPI TX Modes with External Frame Syncs */
+#define ANOMALY_05000294 (1)
+/* Memory-To-Memory DMA Source/Destination Descriptors Must Be in Same Memory Space */
+#define ANOMALY_05000301 (__SILICON_REVISION__ < 4)
+/* SSYNCs After Writes To CAN/DMA MMR Registers Are Not Always Handled Correctly */
+#define ANOMALY_05000304 (__SILICON_REVISION__ < 4)
+/* SCKELOW Bit Does Not Maintain State Through Hibernate */
+#define ANOMALY_05000307 (__SILICON_REVISION__ < 4)
+/* False Hardware Errors Caused by Fetches at the Boundary of Reserved Memory */
+#define ANOMALY_05000310 (1)
+/* Errors when SSYNC, CSYNC, or Loads to LT, LB and LC Registers Are Interrupted */
+#define ANOMALY_05000312 (__SILICON_REVISION__ < 5)
+/* PPI Is Level-Sensitive on First Transfer */
+#define ANOMALY_05000313 (__SILICON_REVISION__ < 4)
+/* Killed System MMR Write Completes Erroneously on Next System MMR Access */
+#define ANOMALY_05000315 (__SILICON_REVISION__ < 4)
+/* PFx Glitch on Write to FIO_FLAG_D or FIO_FLAG_T */
+#define ANOMALY_05000318 (__SILICON_REVISION__ < 4)
+/* Regulator Programming Blocked when Hibernate Wakeup Source Remains Active */
+#define ANOMALY_05000355 (__SILICON_REVISION__ < 5)
+/* Serial Port (SPORT) Multichannel Transmit Failure when Channel 0 Is Disabled */
+#define ANOMALY_05000357 (__SILICON_REVISION__ < 5)
+/* PPI Underflow Error Goes Undetected in ITU-R 656 Mode */
+#define ANOMALY_05000366 (1)
+/* Possible RETS Register Corruption when Subroutine Is under 5 Cycles in Duration */
+#define ANOMALY_05000371 (__SILICON_REVISION__ < 5)
+/* Entering Hibernate State with Peripheral Wakeups Enabled Draws Excess Current */
+#define ANOMALY_05000374 (__SILICON_REVISION__ == 4)
+/* New Feature: Open-Drain GPIO Outputs on PC1 and PC4 (Not Available on Older Silicon) */
+#define ANOMALY_05000375 (__SILICON_REVISION__ < 4)
+/* SSYNC Stalls Processor when Executed from Non-Cacheable Memory */
+#define ANOMALY_05000402 (__SILICON_REVISION__ < 4)
+/* Level-Sensitive External GPIO Wakeups May Cause Indefinite Stall */
+#define ANOMALY_05000403 (1)
+/* Speculative Fetches Can Cause Undesired External FIFO Operations */
+#define ANOMALY_05000416 (1)
+/* Multichannel SPORT Channel Misalignment Under Specific Configuration */
+#define ANOMALY_05000425 (1)
+/* Speculative Fetches of Indirect-Pointer Instructions Can Cause False Hardware Errors */
+#define ANOMALY_05000426 (1)
+/* Specific GPIO Pins May Change State when Entering Hibernate */
+#define ANOMALY_05000436 (__SILICON_REVISION__ > 3)
+/* IFLUSH Instruction at End of Hardware Loop Causes Infinite Stall */
+#define ANOMALY_05000443 (1)
+
+/* Anomalies that don't exist on this proc */
+#define ANOMALY_05000158 (0)
+#define ANOMALY_05000198 (0)
+#define ANOMALY_05000230 (0)
+#define ANOMALY_05000263 (0)
+#define ANOMALY_05000311 (0)
+#define ANOMALY_05000323 (0)
+#define ANOMALY_05000353 (1)
+#define ANOMALY_05000363 (0)
+#define ANOMALY_05000386 (1)
+#define ANOMALY_05000412 (0)
+#define ANOMALY_05000432 (0)
+#define ANOMALY_05000435 (0)
+
+#endif
diff --git a/arch/blackfin/mach-bf538/include/mach/bf538.h b/arch/blackfin/mach-bf538/include/mach/bf538.h
new file mode 100644
index 000000000000..9c8abb307908
--- /dev/null
+++ b/arch/blackfin/mach-bf538/include/mach/bf538.h
@@ -0,0 +1,124 @@
+/*
+ * File:         include/asm-blackfin/mach-bf538/bf538.h
+ * Based on:	include/asm-blackfin/mach-bf537/bf537.h
+ * Author:	Michael Hennerich (michael.hennerich@analog.com)
+ *
+ * Created:
+ * Description:  SYSTEM MMR REGISTER AND MEMORY MAP FOR ADSP-BF527
+ *
+ * Modified:
+ *               Copyright 2004-2007 Analog Devices Inc.
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#ifndef __MACH_BF538_H__
+#define __MACH_BF538_H__
+
+#define OFFSET_(x) ((x) & 0x0000FFFF)
+
+/*some misc defines*/
+#define IMASK_IVG15		0x8000
+#define IMASK_IVG14		0x4000
+#define IMASK_IVG13		0x2000
+#define IMASK_IVG12		0x1000
+
+#define IMASK_IVG11		0x0800
+#define IMASK_IVG10		0x0400
+#define IMASK_IVG9		0x0200
+#define IMASK_IVG8		0x0100
+
+#define IMASK_IVG7		0x0080
+#define IMASK_IVGTMR	0x0040
+#define IMASK_IVGHW		0x0020
+
+/***************************/
+
+#define BFIN_DSUBBANKS	4
+#define BFIN_DWAYS		2
+#define BFIN_DLINES		64
+#define BFIN_ISUBBANKS	4
+#define BFIN_IWAYS		4
+#define BFIN_ILINES		32
+
+#define WAY0_L			0x1
+#define WAY1_L			0x2
+#define WAY01_L			0x3
+#define WAY2_L			0x4
+#define WAY02_L			0x5
+#define	WAY12_L			0x6
+#define	WAY012_L		0x7
+
+#define	WAY3_L			0x8
+#define	WAY03_L			0x9
+#define	WAY13_L			0xA
+#define	WAY013_L		0xB
+
+#define	WAY32_L			0xC
+#define	WAY320_L		0xD
+#define	WAY321_L		0xE
+#define	WAYALL_L		0xF
+
+#define DMC_ENABLE (2<<2)	/*yes, 2, not 1 */
+
+/********************************* EBIU Settings ************************************/
+#define AMBCTL0VAL	((CONFIG_BANK_1 << 16) | CONFIG_BANK_0)
+#define AMBCTL1VAL	((CONFIG_BANK_3 << 16) | CONFIG_BANK_2)
+
+#ifdef CONFIG_C_AMBEN_ALL
+#define V_AMBEN AMBEN_ALL
+#endif
+#ifdef CONFIG_C_AMBEN
+#define V_AMBEN 0x0
+#endif
+#ifdef CONFIG_C_AMBEN_B0
+#define V_AMBEN AMBEN_B0
+#endif
+#ifdef CONFIG_C_AMBEN_B0_B1
+#define V_AMBEN AMBEN_B0_B1
+#endif
+#ifdef CONFIG_C_AMBEN_B0_B1_B2
+#define V_AMBEN AMBEN_B0_B1_B2
+#endif
+#ifdef CONFIG_C_AMCKEN
+#define V_AMCKEN AMCKEN
+#else
+#define V_AMCKEN 0x0
+#endif
+#ifdef CONFIG_C_CDPRIO
+#define V_CDPRIO 0x100
+#else
+#define V_CDPRIO 0x0
+#endif
+
+#define AMGCTLVAL	(V_AMBEN | V_AMCKEN | V_CDPRIO)
+
+#ifdef CONFIG_BF538
+#define CPU "BF538"
+#define CPUID 0x27C4
+#endif
+#ifdef CONFIG_BF539
+#define CPU "BF539"
+#define CPUID 0x27C4	/* FXIME:? */
+#endif
+
+#ifndef CPU
+#error "Unknown CPU type - This kernel doesn't seem to be configured properly"
+#endif
+
+#endif				/* __MACH_BF538_H__  */
diff --git a/arch/blackfin/mach-bf538/include/mach/bfin_serial_5xx.h b/arch/blackfin/mach-bf538/include/mach/bfin_serial_5xx.h
new file mode 100644
index 000000000000..40503b6b89a3
--- /dev/null
+++ b/arch/blackfin/mach-bf538/include/mach/bfin_serial_5xx.h
@@ -0,0 +1,183 @@
+/*
+ * file:         include/asm-blackfin/mach-bf538/bfin_serial_5xx.h
+ * based on:
+ * author:
+ *
+ * created:
+ * description:
+ *	blackfin serial driver header files
+ * rev:
+ *
+ * modified:
+ *
+ *
+ * bugs:         enter bugs at http://blackfin.uclinux.org/
+ *
+ * this program is free software; you can redistribute it and/or modify
+ * it under the terms of the gnu general public license as published by
+ * the free software foundation; either version 2, 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; see the file copying.
+ * if not, write to the free software foundation,
+ * 59 temple place - suite 330, boston, ma 02111-1307, usa.
+ */
+
+#include <linux/serial.h>
+#include <asm/dma.h>
+#include <asm/portmux.h>
+
+#define UART_GET_CHAR(uart)     bfin_read16(((uart)->port.membase + OFFSET_RBR))
+#define UART_GET_DLL(uart)	bfin_read16(((uart)->port.membase + OFFSET_DLL))
+#define UART_GET_IER(uart)      bfin_read16(((uart)->port.membase + OFFSET_IER))
+#define UART_GET_DLH(uart)	bfin_read16(((uart)->port.membase + OFFSET_DLH))
+#define UART_GET_IIR(uart)      bfin_read16(((uart)->port.membase + OFFSET_IIR))
+#define UART_GET_LCR(uart)      bfin_read16(((uart)->port.membase + OFFSET_LCR))
+#define UART_GET_GCTL(uart)     bfin_read16(((uart)->port.membase + OFFSET_GCTL))
+
+#define UART_PUT_CHAR(uart, v)   bfin_write16(((uart)->port.membase + OFFSET_THR), v)
+#define UART_PUT_DLL(uart, v)    bfin_write16(((uart)->port.membase + OFFSET_DLL), v)
+#define UART_PUT_IER(uart, v)    bfin_write16(((uart)->port.membase + OFFSET_IER), v)
+#define UART_SET_IER(uart, v)    UART_PUT_IER(uart, UART_GET_IER(uart) | (v))
+#define UART_CLEAR_IER(uart, v)  UART_PUT_IER(uart, UART_GET_IER(uart) & ~(v))
+#define UART_PUT_DLH(uart, v)    bfin_write16(((uart)->port.membase + OFFSET_DLH), v)
+#define UART_PUT_LCR(uart, v)    bfin_write16(((uart)->port.membase + OFFSET_LCR), v)
+#define UART_PUT_GCTL(uart, v)   bfin_write16(((uart)->port.membase + OFFSET_GCTL), v)
+
+#define UART_SET_DLAB(uart)     do { UART_PUT_LCR(uart, UART_GET_LCR(uart) | DLAB); SSYNC(); } while (0)
+#define UART_CLEAR_DLAB(uart)   do { UART_PUT_LCR(uart, UART_GET_LCR(uart) & ~DLAB); SSYNC(); } while (0)
+
+#define UART_GET_CTS(x) gpio_get_value(x->cts_pin)
+#define UART_SET_RTS(x) gpio_set_value(x->rts_pin, 1)
+#define UART_CLEAR_RTS(x) gpio_set_value(x->rts_pin, 0)
+#define UART_ENABLE_INTS(x, v) UART_PUT_IER(x, v)
+#define UART_DISABLE_INTS(x) UART_PUT_IER(x, 0)
+
+#if defined(CONFIG_BFIN_UART0_CTSRTS) || defined(CONFIG_BFIN_UART1_CTSRTS)
+# define CONFIG_SERIAL_BFIN_CTSRTS
+
+# ifndef CONFIG_UART0_CTS_PIN
+#  define CONFIG_UART0_CTS_PIN -1
+# endif
+
+# ifndef CONFIG_UART0_RTS_PIN
+#  define CONFIG_UART0_RTS_PIN -1
+# endif
+
+# ifndef CONFIG_UART1_CTS_PIN
+#  define CONFIG_UART1_CTS_PIN -1
+# endif
+
+# ifndef CONFIG_UART1_RTS_PIN
+#  define CONFIG_UART1_RTS_PIN -1
+# endif
+#endif
+
+#define BFIN_UART_TX_FIFO_SIZE	2
+
+/*
+ * The pin configuration is different from schematic
+ */
+struct bfin_serial_port {
+	struct uart_port	port;
+	unsigned int		old_status;
+	unsigned int lsr;
+#ifdef CONFIG_SERIAL_BFIN_DMA
+	int			tx_done;
+	int			tx_count;
+	struct circ_buf		rx_dma_buf;
+	struct timer_list       rx_dma_timer;
+	int			rx_dma_nrows;
+	unsigned int		tx_dma_channel;
+	unsigned int		rx_dma_channel;
+	struct work_struct	tx_dma_workqueue;
+#endif
+#ifdef CONFIG_SERIAL_BFIN_CTSRTS
+	struct timer_list	cts_timer;
+	int		cts_pin;
+	int		rts_pin;
+#endif
+};
+
+/* The hardware clears the LSR bits upon read, so we need to cache
+ * some of the more fun bits in software so they don't get lost
+ * when checking the LSR in other code paths (TX).
+ */
+static inline unsigned int UART_GET_LSR(struct bfin_serial_port *uart)
+{
+	unsigned int lsr = bfin_read16(uart->port.membase + OFFSET_LSR);
+	uart->lsr |= (lsr & (BI|FE|PE|OE));
+	return lsr | uart->lsr;
+}
+
+static inline void UART_CLEAR_LSR(struct bfin_serial_port *uart)
+{
+	uart->lsr = 0;
+	bfin_write16(uart->port.membase + OFFSET_LSR, -1);
+}
+
+struct bfin_serial_res {
+	unsigned long	uart_base_addr;
+	int		uart_irq;
+#ifdef CONFIG_SERIAL_BFIN_DMA
+	unsigned int	uart_tx_dma_channel;
+	unsigned int	uart_rx_dma_channel;
+#endif
+#ifdef CONFIG_SERIAL_BFIN_CTSRTS
+	int	uart_cts_pin;
+	int	uart_rts_pin;
+#endif
+};
+
+struct bfin_serial_res bfin_serial_resource[] = {
+#ifdef CONFIG_SERIAL_BFIN_UART0
+	{
+	0xFFC00400,
+	IRQ_UART0_RX,
+#ifdef CONFIG_SERIAL_BFIN_DMA
+	CH_UART0_TX,
+	CH_UART0_RX,
+#endif
+#ifdef CONFIG_BFIN_UART0_CTSRTS
+	CONFIG_UART0_CTS_PIN,
+	CONFIG_UART0_RTS_PIN,
+#endif
+	},
+#endif
+#ifdef CONFIG_SERIAL_BFIN_UART1
+	{
+	0xFFC02000,
+	IRQ_UART1_RX,
+#ifdef CONFIG_SERIAL_BFIN_DMA
+	CH_UART1_TX,
+	CH_UART1_RX,
+#endif
+#ifdef CONFIG_BFIN_UART1_CTSRTS
+	CONFIG_UART1_CTS_PIN,
+	CONFIG_UART1_RTS_PIN,
+#endif
+	},
+#endif
+#ifdef CONFIG_SERIAL_BFIN_UART2
+	{
+	0xFFC02100,
+	IRQ_UART2_RX,
+#ifdef CONFIG_SERIAL_BFIN_DMA
+	CH_UART2_TX,
+	CH_UART2_RX,
+#endif
+#ifdef CONFIG_BFIN_UART2_CTSRTS
+	CONFIG_UART2_CTS_PIN,
+	CONFIG_UART2_RTS_PIN,
+#endif
+	},
+#endif
+};
+
+#define DRIVER_NAME "bfin-uart"
diff --git a/arch/blackfin/mach-bf538/include/mach/blackfin.h b/arch/blackfin/mach-bf538/include/mach/blackfin.h
new file mode 100644
index 000000000000..ea25371a922b
--- /dev/null
+++ b/arch/blackfin/mach-bf538/include/mach/blackfin.h
@@ -0,0 +1,101 @@
+/*
+ * File:         include/asm-blackfin/mach-bf538/blackfin.h
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:
+ *
+ * Rev:
+ *
+ * Modified:
+ *
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, 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; see the file COPYING.
+ * If not, write to the Free Software Foundation,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef _MACH_BLACKFIN_H_
+#define _MACH_BLACKFIN_H_
+
+#define BF538_FAMILY
+
+#include "bf538.h"
+#include "mem_map.h"
+#include "defBF539.h"
+#include "anomaly.h"
+
+
+#if !defined(__ASSEMBLY__)
+#include "cdefBF538.h"
+
+#if defined(CONFIG_BF539)
+#include "cdefBF539.h"
+#endif
+#endif
+
+/* UART_IIR Register */
+#define STATUS(x)	((x << 1) & 0x06)
+#define STATUS_P1	0x02
+#define STATUS_P0	0x01
+
+#define BFIN_UART_NR_PORTS	3
+
+#define OFFSET_THR              0x00	/* Transmit Holding register            */
+#define OFFSET_RBR              0x00	/* Receive Buffer register              */
+#define OFFSET_DLL              0x00	/* Divisor Latch (Low-Byte)             */
+#define OFFSET_IER              0x04	/* Interrupt Enable Register            */
+#define OFFSET_DLH              0x04	/* Divisor Latch (High-Byte)            */
+#define OFFSET_IIR              0x08	/* Interrupt Identification Register    */
+#define OFFSET_LCR              0x0C	/* Line Control Register                */
+#define OFFSET_MCR              0x10	/* Modem Control Register               */
+#define OFFSET_LSR              0x14	/* Line Status Register                 */
+#define OFFSET_MSR              0x18	/* Modem Status Register                */
+#define OFFSET_SCR              0x1C	/* SCR Scratch Register                 */
+#define OFFSET_GCTL             0x24	/* Global Control Register              */
+
+
+#define bfin_write_MDMA_D0_IRQ_STATUS	bfin_write_MDMA0_D0_IRQ_STATUS
+#define bfin_write_MDMA_D0_START_ADDR   bfin_write_MDMA0_D0_START_ADDR
+#define bfin_write_MDMA_S0_START_ADDR   bfin_write_MDMA0_S0_START_ADDR
+#define bfin_write_MDMA_D0_X_COUNT      bfin_write_MDMA0_D0_X_COUNT
+#define bfin_write_MDMA_S0_X_COUNT      bfin_write_MDMA0_S0_X_COUNT
+#define bfin_write_MDMA_D0_Y_COUNT      bfin_write_MDMA0_D0_Y_COUNT
+#define bfin_write_MDMA_S0_Y_COUNT      bfin_write_MDMA0_S0_Y_COUNT
+#define bfin_write_MDMA_D0_X_MODIFY     bfin_write_MDMA0_D0_X_MODIFY
+#define bfin_write_MDMA_S0_X_MODIFY     bfin_write_MDMA0_S0_X_MODIFY
+#define bfin_write_MDMA_D0_Y_MODIFY     bfin_write_MDMA0_D0_Y_MODIFY
+#define bfin_write_MDMA_S0_Y_MODIFY     bfin_write_MDMA0_S0_Y_MODIFY
+#define bfin_write_MDMA_S0_CONFIG       bfin_write_MDMA0_S0_CONFIG
+#define bfin_write_MDMA_D0_CONFIG       bfin_write_MDMA0_D0_CONFIG
+#define bfin_read_MDMA_S0_CONFIG        bfin_read_MDMA0_S0_CONFIG
+#define bfin_read_MDMA_D0_IRQ_STATUS    bfin_read_MDMA0_D0_IRQ_STATUS
+#define bfin_write_MDMA_S0_IRQ_STATUS   bfin_write_MDMA0_S0_IRQ_STATUS
+
+
+/* DPMC*/
+#define bfin_read_STOPCK_OFF() bfin_read_STOPCK()
+#define bfin_write_STOPCK_OFF(val) bfin_write_STOPCK(val)
+#define STOPCK_OFF STOPCK
+
+/* PLL_DIV Masks													*/
+#define CCLK_DIV1 CSEL_DIV1	/*          CCLK = VCO / 1                                  */
+#define CCLK_DIV2 CSEL_DIV2	/*          CCLK = VCO / 2                                  */
+#define CCLK_DIV4 CSEL_DIV4	/*          CCLK = VCO / 4                                  */
+#define CCLK_DIV8 CSEL_DIV8	/*          CCLK = VCO / 8                                  */
+
+#endif
diff --git a/arch/blackfin/mach-bf538/include/mach/cdefBF538.h b/arch/blackfin/mach-bf538/include/mach/cdefBF538.h
new file mode 100644
index 000000000000..241725bc6988
--- /dev/null
+++ b/arch/blackfin/mach-bf538/include/mach/cdefBF538.h
@@ -0,0 +1,2108 @@
+/*
+ * File:         include/asm-blackfin/mach-bf538/cdefBF538.h
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:
+ *
+ * Rev:
+ *
+ * Modified:
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, 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; see the file COPYING.
+ * If not, write to the Free Software Foundation,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef _CDEF_BF538_H
+#define _CDEF_BF538_H
+
+#include <asm/blackfin.h>
+
+/*include all Core registers and bit definitions*/
+#include "defBF539.h"
+
+/*include core specific register pointer definitions*/
+#include <asm/cdef_LPBlackfin.h>
+
+#define bfin_writePTR(addr, val) bfin_write32(addr, val)
+
+#define bfin_read_PLL_CTL()            bfin_read16(PLL_CTL)
+#define bfin_read_PLL_DIV()            bfin_read16(PLL_DIV)
+#define bfin_write_PLL_DIV(val)        bfin_write16(PLL_DIV, val)
+#define bfin_read_VR_CTL()             bfin_read16(VR_CTL)
+#define bfin_read_PLL_STAT()           bfin_read16(PLL_STAT)
+#define bfin_write_PLL_STAT(val)       bfin_write16(PLL_STAT, val)
+#define bfin_read_PLL_LOCKCNT()        bfin_read16(PLL_LOCKCNT)
+#define bfin_write_PLL_LOCKCNT(val)    bfin_write16(PLL_LOCKCNT, val)
+#define bfin_read_CHIPID()             bfin_read32(CHIPID)
+#define bfin_write_CHIPID(val)         bfin_write32(CHIPID, val)
+#define bfin_read_SWRST()              bfin_read16(SWRST)
+#define bfin_write_SWRST(val)          bfin_write16(SWRST, val)
+#define bfin_read_SYSCR()              bfin_read16(SYSCR)
+#define bfin_write_SYSCR(val)          bfin_write16(SYSCR, val)
+#define bfin_read_SIC_RVECT()          bfin_readPTR(SIC_RVECT)
+#define bfin_write_SIC_RVECT(val)      bfin_writePTR(SIC_RVECT, val)
+#define bfin_read_SIC_IMASK0()         bfin_read32(SIC_IMASK0)
+#define bfin_write_SIC_IMASK0(val)     bfin_write32(SIC_IMASK0, val)
+#define bfin_read_SIC_IMASK1()         bfin_read32(SIC_IMASK1)
+#define bfin_write_SIC_IMASK1(val)     bfin_write32(SIC_IMASK1, val)
+#define bfin_read_SIC_IMASK(x)	       bfin_read32(SIC_IMASK0 + x * (SIC_IMASK1 - SIC_IMASK0))
+#define bfin_write_SIC_IMASK(x, val)   bfin_write32(SIC_IMASK0 + x * (SIC_IMASK1 - SIC_IMASK0), val)
+#define bfin_read_SIC_ISR0()           bfin_read32(SIC_ISR0)
+#define bfin_write_SIC_ISR0(val)       bfin_write32(SIC_ISR0, val)
+#define bfin_read_SIC_ISR1()           bfin_read32(SIC_ISR1)
+#define bfin_write_SIC_ISR1(val)       bfin_write32(SIC_ISR1, val)
+#define bfin_read_SIC_ISR(x)	       bfin_read32(SIC_ISR0 + x * (SIC_ISR1 - SIC_ISR0))
+#define bfin_write_SIC_ISR(x, val)     bfin_write32(SIC_ISR0 + x * (SIC_ISR1 - SIC_ISR0), val)
+#define bfin_read_SIC_IWR0()           bfin_read32(SIC_IWR0)
+#define bfin_write_SIC_IWR0(val)       bfin_write32(SIC_IWR0, val)
+#define bfin_read_SIC_IWR1()           bfin_read32(SIC_IWR1)
+#define bfin_write_SIC_IWR1(val)       bfin_write32(SIC_IWR1, val)
+#define bfin_read_SIC_IWR(x)	       bfin_read32(SIC_IWR0 + x * (SIC_IWR1 - SIC_IWR0))
+#define bfin_write_SIC_IWR(x, val)     bfin_write32((SIC_IWR0 + x * (SIC_IWR1 - SIC_IWR0), val)
+#define bfin_read_SIC_IAR0()           bfin_read32(SIC_IAR0)
+#define bfin_write_SIC_IAR0(val)       bfin_write32(SIC_IAR0, val)
+#define bfin_read_SIC_IAR1()           bfin_read32(SIC_IAR1)
+#define bfin_write_SIC_IAR1(val)       bfin_write32(SIC_IAR1, val)
+#define bfin_read_SIC_IAR2()           bfin_read32(SIC_IAR2)
+#define bfin_write_SIC_IAR2(val)       bfin_write32(SIC_IAR2, val)
+#define bfin_read_SIC_IAR3()           bfin_read32(SIC_IAR3)
+#define bfin_write_SIC_IAR3(val)       bfin_write32(SIC_IAR3, val)
+#define bfin_read_SIC_IAR4()           bfin_read32(SIC_IAR4)
+#define bfin_write_SIC_IAR4(val)       bfin_write32(SIC_IAR4, val)
+#define bfin_read_SIC_IAR5()           bfin_read32(SIC_IAR5)
+#define bfin_write_SIC_IAR5(val)       bfin_write32(SIC_IAR5, val)
+#define bfin_read_SIC_IAR6()           bfin_read32(SIC_IAR6)
+#define bfin_write_SIC_IAR6(val)       bfin_write32(SIC_IAR6, val)
+#define bfin_read_WDOG_CTL()           bfin_read16(WDOG_CTL)
+#define bfin_write_WDOG_CTL(val)       bfin_write16(WDOG_CTL, val)
+#define bfin_read_WDOG_CNT()           bfin_read32(WDOG_CNT)
+#define bfin_write_WDOG_CNT(val)       bfin_write32(WDOG_CNT, val)
+#define bfin_read_WDOG_STAT()          bfin_read32(WDOG_STAT)
+#define bfin_write_WDOG_STAT(val)      bfin_write32(WDOG_STAT, val)
+#define bfin_read_RTC_STAT()           bfin_read32(RTC_STAT)
+#define bfin_write_RTC_STAT(val)       bfin_write32(RTC_STAT, val)
+#define bfin_read_RTC_ICTL()           bfin_read16(RTC_ICTL)
+#define bfin_write_RTC_ICTL(val)       bfin_write16(RTC_ICTL, val)
+#define bfin_read_RTC_ISTAT()          bfin_read16(RTC_ISTAT)
+#define bfin_write_RTC_ISTAT(val)      bfin_write16(RTC_ISTAT, val)
+#define bfin_read_RTC_SWCNT()          bfin_read16(RTC_SWCNT)
+#define bfin_write_RTC_SWCNT(val)      bfin_write16(RTC_SWCNT, val)
+#define bfin_read_RTC_ALARM()          bfin_read32(RTC_ALARM)
+#define bfin_write_RTC_ALARM(val)      bfin_write32(RTC_ALARM, val)
+#define bfin_read_RTC_PREN()           bfin_read16(RTC_PREN)
+#define bfin_write_RTC_PREN(val)       bfin_write16(RTC_PREN, val)
+#define bfin_read_UART0_THR()          bfin_read16(UART0_THR)
+#define bfin_write_UART0_THR(val)      bfin_write16(UART0_THR, val)
+#define bfin_read_UART0_RBR()          bfin_read16(UART0_RBR)
+#define bfin_write_UART0_RBR(val)      bfin_write16(UART0_RBR, val)
+#define bfin_read_UART0_DLL()          bfin_read16(UART0_DLL)
+#define bfin_write_UART0_DLL(val)      bfin_write16(UART0_DLL, val)
+#define bfin_read_UART0_DLH()          bfin_read16(UART0_DLH)
+#define bfin_write_UART0_DLH(val)      bfin_write16(UART0_DLH, val)
+#define bfin_read_UART0_IER()          bfin_read16(UART0_IER)
+#define bfin_write_UART0_IER(val)      bfin_write16(UART0_IER, val)
+#define bfin_read_UART0_IIR()          bfin_read16(UART0_IIR)
+#define bfin_write_UART0_IIR(val)      bfin_write16(UART0_IIR, val)
+#define bfin_read_UART0_LCR()          bfin_read16(UART0_LCR)
+#define bfin_write_UART0_LCR(val)      bfin_write16(UART0_LCR, val)
+#define bfin_read_UART0_MCR()          bfin_read16(UART0_MCR)
+#define bfin_write_UART0_MCR(val)      bfin_write16(UART0_MCR, val)
+#define bfin_read_UART0_LSR()          bfin_read16(UART0_LSR)
+#define bfin_write_UART0_LSR(val)      bfin_write16(UART0_LSR, val)
+#define bfin_read_UART0_SCR()          bfin_read16(UART0_SCR)
+#define bfin_write_UART0_SCR(val)      bfin_write16(UART0_SCR, val)
+#define bfin_read_UART0_GCTL()         bfin_read16(UART0_GCTL)
+#define bfin_write_UART0_GCTL(val)     bfin_write16(UART0_GCTL, val)
+#define bfin_read_UART1_THR()          bfin_read16(UART1_THR)
+#define bfin_write_UART1_THR(val)      bfin_write16(UART1_THR, val)
+#define bfin_read_UART1_RBR()          bfin_read16(UART1_RBR)
+#define bfin_write_UART1_RBR(val)      bfin_write16(UART1_RBR, val)
+#define bfin_read_UART1_DLL()          bfin_read16(UART1_DLL)
+#define bfin_write_UART1_DLL(val)      bfin_write16(UART1_DLL, val)
+#define bfin_read_UART1_DLH()          bfin_read16(UART1_DLH)
+#define bfin_write_UART1_DLH(val)      bfin_write16(UART1_DLH, val)
+#define bfin_read_UART1_IER()          bfin_read16(UART1_IER)
+#define bfin_write_UART1_IER(val)      bfin_write16(UART1_IER, val)
+#define bfin_read_UART1_IIR()          bfin_read16(UART1_IIR)
+#define bfin_write_UART1_IIR(val)      bfin_write16(UART1_IIR, val)
+#define bfin_read_UART1_LCR()          bfin_read16(UART1_LCR)
+#define bfin_write_UART1_LCR(val)      bfin_write16(UART1_LCR, val)
+#define bfin_read_UART1_MCR()          bfin_read16(UART1_MCR)
+#define bfin_write_UART1_MCR(val)      bfin_write16(UART1_MCR, val)
+#define bfin_read_UART1_LSR()          bfin_read16(UART1_LSR)
+#define bfin_write_UART1_LSR(val)      bfin_write16(UART1_LSR, val)
+#define bfin_read_UART1_SCR()          bfin_read16(UART1_SCR)
+#define bfin_write_UART1_SCR(val)      bfin_write16(UART1_SCR, val)
+#define bfin_read_UART1_GCTL()         bfin_read16(UART1_GCTL)
+#define bfin_write_UART1_GCTL(val)     bfin_write16(UART1_GCTL, val)
+#define bfin_read_UART2_THR()          bfin_read16(UART2_THR)
+#define bfin_write_UART2_THR(val)      bfin_write16(UART2_THR, val)
+#define bfin_read_UART2_RBR()          bfin_read16(UART2_RBR)
+#define bfin_write_UART2_RBR(val)      bfin_write16(UART2_RBR, val)
+#define bfin_read_UART2_DLL()          bfin_read16(UART2_DLL)
+#define bfin_write_UART2_DLL(val)      bfin_write16(UART2_DLL, val)
+#define bfin_read_UART2_DLH()          bfin_read16(UART2_DLH)
+#define bfin_write_UART2_DLH(val)      bfin_write16(UART2_DLH, val)
+#define bfin_read_UART2_IER()          bfin_read16(UART2_IER)
+#define bfin_write_UART2_IER(val)      bfin_write16(UART2_IER, val)
+#define bfin_read_UART2_IIR()          bfin_read16(UART2_IIR)
+#define bfin_write_UART2_IIR(val)      bfin_write16(UART2_IIR, val)
+#define bfin_read_UART2_LCR()          bfin_read16(UART2_LCR)
+#define bfin_write_UART2_LCR(val)      bfin_write16(UART2_LCR, val)
+#define bfin_read_UART2_MCR()          bfin_read16(UART2_MCR)
+#define bfin_write_UART2_MCR(val)      bfin_write16(UART2_MCR, val)
+#define bfin_read_UART2_LSR()          bfin_read16(UART2_LSR)
+#define bfin_write_UART2_LSR(val)      bfin_write16(UART2_LSR, val)
+#define bfin_read_UART2_SCR()          bfin_read16(UART2_SCR)
+#define bfin_write_UART2_SCR(val)      bfin_write16(UART2_SCR, val)
+#define bfin_read_UART2_GCTL()         bfin_read16(UART2_GCTL)
+#define bfin_write_UART2_GCTL(val)     bfin_write16(UART2_GCTL, val)
+#define bfin_read_SPI0_CTL()           bfin_read16(SPI0_CTL)
+#define bfin_write_SPI0_CTL(val)       bfin_write16(SPI0_CTL, val)
+#define bfin_read_SPI0_FLG()           bfin_read16(SPI0_FLG)
+#define bfin_write_SPI0_FLG(val)       bfin_write16(SPI0_FLG, val)
+#define bfin_read_SPI0_STAT()          bfin_read16(SPI0_STAT)
+#define bfin_write_SPI0_STAT(val)      bfin_write16(SPI0_STAT, val)
+#define bfin_read_SPI0_TDBR()          bfin_read16(SPI0_TDBR)
+#define bfin_write_SPI0_TDBR(val)      bfin_write16(SPI0_TDBR, val)
+#define bfin_read_SPI0_RDBR()          bfin_read16(SPI0_RDBR)
+#define bfin_write_SPI0_RDBR(val)      bfin_write16(SPI0_RDBR, val)
+#define bfin_read_SPI0_BAUD()          bfin_read16(SPI0_BAUD)
+#define bfin_write_SPI0_BAUD(val)      bfin_write16(SPI0_BAUD, val)
+#define bfin_read_SPI0_SHADOW()        bfin_read16(SPI0_SHADOW)
+#define bfin_write_SPI0_SHADOW(val)    bfin_write16(SPI0_SHADOW, val)
+#define bfin_read_SPI1_CTL()           bfin_read16(SPI1_CTL)
+#define bfin_write_SPI1_CTL(val)       bfin_write16(SPI1_CTL, val)
+#define bfin_read_SPI1_FLG()           bfin_read16(SPI1_FLG)
+#define bfin_write_SPI1_FLG(val)       bfin_write16(SPI1_FLG, val)
+#define bfin_read_SPI1_STAT()          bfin_read16(SPI1_STAT)
+#define bfin_write_SPI1_STAT(val)      bfin_write16(SPI1_STAT, val)
+#define bfin_read_SPI1_TDBR()          bfin_read16(SPI1_TDBR)
+#define bfin_write_SPI1_TDBR(val)      bfin_write16(SPI1_TDBR, val)
+#define bfin_read_SPI1_RDBR()          bfin_read16(SPI1_RDBR)
+#define bfin_write_SPI1_RDBR(val)      bfin_write16(SPI1_RDBR, val)
+#define bfin_read_SPI1_BAUD()          bfin_read16(SPI1_BAUD)
+#define bfin_write_SPI1_BAUD(val)      bfin_write16(SPI1_BAUD, val)
+#define bfin_read_SPI1_SHADOW()        bfin_read16(SPI1_SHADOW)
+#define bfin_write_SPI1_SHADOW(val)    bfin_write16(SPI1_SHADOW, val)
+#define bfin_read_SPI2_CTL()           bfin_read16(SPI2_CTL)
+#define bfin_write_SPI2_CTL(val)       bfin_write16(SPI2_CTL, val)
+#define bfin_read_SPI2_FLG()           bfin_read16(SPI2_FLG)
+#define bfin_write_SPI2_FLG(val)       bfin_write16(SPI2_FLG, val)
+#define bfin_read_SPI2_STAT()          bfin_read16(SPI2_STAT)
+#define bfin_write_SPI2_STAT(val)      bfin_write16(SPI2_STAT, val)
+#define bfin_read_SPI2_TDBR()          bfin_read16(SPI2_TDBR)
+#define bfin_write_SPI2_TDBR(val)      bfin_write16(SPI2_TDBR, val)
+#define bfin_read_SPI2_RDBR()          bfin_read16(SPI2_RDBR)
+#define bfin_write_SPI2_RDBR(val)      bfin_write16(SPI2_RDBR, val)
+#define bfin_read_SPI2_BAUD()          bfin_read16(SPI2_BAUD)
+#define bfin_write_SPI2_BAUD(val)      bfin_write16(SPI2_BAUD, val)
+#define bfin_read_SPI2_SHADOW()        bfin_read16(SPI2_SHADOW)
+#define bfin_write_SPI2_SHADOW(val)    bfin_write16(SPI2_SHADOW, val)
+#define bfin_read_TIMER0_CONFIG()      bfin_read16(TIMER0_CONFIG)
+#define bfin_write_TIMER0_CONFIG(val)  bfin_write16(TIMER0_CONFIG, val)
+#define bfin_read_TIMER0_COUNTER()     bfin_read32(TIMER0_COUNTER)
+#define bfin_write_TIMER0_COUNTER(val) bfin_write32(TIMER0_COUNTER, val)
+#define bfin_read_TIMER0_PERIOD()      bfin_read32(TIMER0_PERIOD)
+#define bfin_write_TIMER0_PERIOD(val)  bfin_write32(TIMER0_PERIOD, val)
+#define bfin_read_TIMER0_WIDTH()       bfin_read32(TIMER0_WIDTH)
+#define bfin_write_TIMER0_WIDTH(val)   bfin_write32(TIMER0_WIDTH, val)
+#define bfin_read_TIMER1_CONFIG()      bfin_read16(TIMER1_CONFIG)
+#define bfin_write_TIMER1_CONFIG(val)  bfin_write16(TIMER1_CONFIG, val)
+#define bfin_read_TIMER1_COUNTER()     bfin_read32(TIMER1_COUNTER)
+#define bfin_write_TIMER1_COUNTER(val) bfin_write32(TIMER1_COUNTER, val)
+#define bfin_read_TIMER1_PERIOD()      bfin_read32(TIMER1_PERIOD)
+#define bfin_write_TIMER1_PERIOD(val)  bfin_write32(TIMER1_PERIOD, val)
+#define bfin_read_TIMER1_WIDTH()       bfin_read32(TIMER1_WIDTH)
+#define bfin_write_TIMER1_WIDTH(val)   bfin_write32(TIMER1_WIDTH, val)
+#define bfin_read_TIMER2_CONFIG()      bfin_read16(TIMER2_CONFIG)
+#define bfin_write_TIMER2_CONFIG(val)  bfin_write16(TIMER2_CONFIG, val)
+#define bfin_read_TIMER2_COUNTER()     bfin_read32(TIMER2_COUNTER)
+#define bfin_write_TIMER2_COUNTER(val) bfin_write32(TIMER2_COUNTER, val)
+#define bfin_read_TIMER2_PERIOD()      bfin_read32(TIMER2_PERIOD)
+#define bfin_write_TIMER2_PERIOD(val)  bfin_write32(TIMER2_PERIOD, val)
+#define bfin_read_TIMER2_WIDTH()       bfin_read32(TIMER2_WIDTH)
+#define bfin_write_TIMER2_WIDTH(val)   bfin_write32(TIMER2_WIDTH, val)
+#define bfin_read_TIMER_ENABLE()       bfin_read16(TIMER_ENABLE)
+#define bfin_write_TIMER_ENABLE(val)   bfin_write16(TIMER_ENABLE, val)
+#define bfin_read_TIMER_DISABLE()      bfin_read16(TIMER_DISABLE)
+#define bfin_write_TIMER_DISABLE(val)  bfin_write16(TIMER_DISABLE, val)
+#define bfin_read_TIMER_STATUS()       bfin_read16(TIMER_STATUS)
+#define bfin_write_TIMER_STATUS(val)   bfin_write16(TIMER_STATUS, val)
+#define bfin_read_SPORT0_TCR1()        bfin_read16(SPORT0_TCR1)
+#define bfin_write_SPORT0_TCR1(val)    bfin_write16(SPORT0_TCR1, val)
+#define bfin_read_SPORT0_TCR2()        bfin_read16(SPORT0_TCR2)
+#define bfin_write_SPORT0_TCR2(val)    bfin_write16(SPORT0_TCR2, val)
+#define bfin_read_SPORT0_TCLKDIV()     bfin_read16(SPORT0_TCLKDIV)
+#define bfin_write_SPORT0_TCLKDIV(val) bfin_write16(SPORT0_TCLKDIV, val)
+#define bfin_read_SPORT0_TFSDIV()      bfin_read16(SPORT0_TFSDIV)
+#define bfin_write_SPORT0_TFSDIV(val)  bfin_write16(SPORT0_TFSDIV, val)
+#define bfin_read_SPORT0_TX()          bfin_read32(SPORT0_TX)
+#define bfin_write_SPORT0_TX(val)      bfin_write32(SPORT0_TX, val)
+#define bfin_read_SPORT0_RX()          bfin_read32(SPORT0_RX)
+#define bfin_write_SPORT0_RX(val)      bfin_write32(SPORT0_RX, val)
+#define bfin_read_SPORT0_RCR1()        bfin_read16(SPORT0_RCR1)
+#define bfin_write_SPORT0_RCR1(val)    bfin_write16(SPORT0_RCR1, val)
+#define bfin_read_SPORT0_RCR2()        bfin_read16(SPORT0_RCR2)
+#define bfin_write_SPORT0_RCR2(val)    bfin_write16(SPORT0_RCR2, val)
+#define bfin_read_SPORT0_RCLKDIV()     bfin_read16(SPORT0_RCLKDIV)
+#define bfin_write_SPORT0_RCLKDIV(val) bfin_write16(SPORT0_RCLKDIV, val)
+#define bfin_read_SPORT0_RFSDIV()      bfin_read16(SPORT0_RFSDIV)
+#define bfin_write_SPORT0_RFSDIV(val)  bfin_write16(SPORT0_RFSDIV, val)
+#define bfin_read_SPORT0_STAT()        bfin_read16(SPORT0_STAT)
+#define bfin_write_SPORT0_STAT(val)    bfin_write16(SPORT0_STAT, val)
+#define bfin_read_SPORT0_CHNL()        bfin_read16(SPORT0_CHNL)
+#define bfin_write_SPORT0_CHNL(val)    bfin_write16(SPORT0_CHNL, val)
+#define bfin_read_SPORT0_MCMC1()       bfin_read16(SPORT0_MCMC1)
+#define bfin_write_SPORT0_MCMC1(val)   bfin_write16(SPORT0_MCMC1, val)
+#define bfin_read_SPORT0_MCMC2()       bfin_read16(SPORT0_MCMC2)
+#define bfin_write_SPORT0_MCMC2(val)   bfin_write16(SPORT0_MCMC2, val)
+#define bfin_read_SPORT0_MTCS0()       bfin_read32(SPORT0_MTCS0)
+#define bfin_write_SPORT0_MTCS0(val)   bfin_write32(SPORT0_MTCS0, val)
+#define bfin_read_SPORT0_MTCS1()       bfin_read32(SPORT0_MTCS1)
+#define bfin_write_SPORT0_MTCS1(val)   bfin_write32(SPORT0_MTCS1, val)
+#define bfin_read_SPORT0_MTCS2()       bfin_read32(SPORT0_MTCS2)
+#define bfin_write_SPORT0_MTCS2(val)   bfin_write32(SPORT0_MTCS2, val)
+#define bfin_read_SPORT0_MTCS3()       bfin_read32(SPORT0_MTCS3)
+#define bfin_write_SPORT0_MTCS3(val)   bfin_write32(SPORT0_MTCS3, val)
+#define bfin_read_SPORT0_MRCS0()       bfin_read32(SPORT0_MRCS0)
+#define bfin_write_SPORT0_MRCS0(val)   bfin_write32(SPORT0_MRCS0, val)
+#define bfin_read_SPORT0_MRCS1()       bfin_read32(SPORT0_MRCS1)
+#define bfin_write_SPORT0_MRCS1(val)   bfin_write32(SPORT0_MRCS1, val)
+#define bfin_read_SPORT0_MRCS2()       bfin_read32(SPORT0_MRCS2)
+#define bfin_write_SPORT0_MRCS2(val)   bfin_write32(SPORT0_MRCS2, val)
+#define bfin_read_SPORT0_MRCS3()       bfin_read32(SPORT0_MRCS3)
+#define bfin_write_SPORT0_MRCS3(val)   bfin_write32(SPORT0_MRCS3, val)
+#define bfin_read_SPORT1_TCR1()        bfin_read16(SPORT1_TCR1)
+#define bfin_write_SPORT1_TCR1(val)    bfin_write16(SPORT1_TCR1, val)
+#define bfin_read_SPORT1_TCR2()        bfin_read16(SPORT1_TCR2)
+#define bfin_write_SPORT1_TCR2(val)    bfin_write16(SPORT1_TCR2, val)
+#define bfin_read_SPORT1_TCLKDIV()     bfin_read16(SPORT1_TCLKDIV)
+#define bfin_write_SPORT1_TCLKDIV(val) bfin_write16(SPORT1_TCLKDIV, val)
+#define bfin_read_SPORT1_TFSDIV()      bfin_read16(SPORT1_TFSDIV)
+#define bfin_write_SPORT1_TFSDIV(val)  bfin_write16(SPORT1_TFSDIV, val)
+#define bfin_read_SPORT1_TX()          bfin_read32(SPORT1_TX)
+#define bfin_write_SPORT1_TX(val)      bfin_write32(SPORT1_TX, val)
+#define bfin_read_SPORT1_RX()          bfin_read32(SPORT1_RX)
+#define bfin_write_SPORT1_RX(val)      bfin_write32(SPORT1_RX, val)
+#define bfin_read_SPORT1_RCR1()        bfin_read16(SPORT1_RCR1)
+#define bfin_write_SPORT1_RCR1(val)    bfin_write16(SPORT1_RCR1, val)
+#define bfin_read_SPORT1_RCR2()        bfin_read16(SPORT1_RCR2)
+#define bfin_write_SPORT1_RCR2(val)    bfin_write16(SPORT1_RCR2, val)
+#define bfin_read_SPORT1_RCLKDIV()     bfin_read16(SPORT1_RCLKDIV)
+#define bfin_write_SPORT1_RCLKDIV(val) bfin_write16(SPORT1_RCLKDIV, val)
+#define bfin_read_SPORT1_RFSDIV()      bfin_read16(SPORT1_RFSDIV)
+#define bfin_write_SPORT1_RFSDIV(val)  bfin_write16(SPORT1_RFSDIV, val)
+#define bfin_read_SPORT1_STAT()        bfin_read16(SPORT1_STAT)
+#define bfin_write_SPORT1_STAT(val)    bfin_write16(SPORT1_STAT, val)
+#define bfin_read_SPORT1_CHNL()        bfin_read16(SPORT1_CHNL)
+#define bfin_write_SPORT1_CHNL(val)    bfin_write16(SPORT1_CHNL, val)
+#define bfin_read_SPORT1_MCMC1()       bfin_read16(SPORT1_MCMC1)
+#define bfin_write_SPORT1_MCMC1(val)   bfin_write16(SPORT1_MCMC1, val)
+#define bfin_read_SPORT1_MCMC2()       bfin_read16(SPORT1_MCMC2)
+#define bfin_write_SPORT1_MCMC2(val)   bfin_write16(SPORT1_MCMC2, val)
+#define bfin_read_SPORT1_MTCS0()       bfin_read32(SPORT1_MTCS0)
+#define bfin_write_SPORT1_MTCS0(val)   bfin_write32(SPORT1_MTCS0, val)
+#define bfin_read_SPORT1_MTCS1()       bfin_read32(SPORT1_MTCS1)
+#define bfin_write_SPORT1_MTCS1(val)   bfin_write32(SPORT1_MTCS1, val)
+#define bfin_read_SPORT1_MTCS2()       bfin_read32(SPORT1_MTCS2)
+#define bfin_write_SPORT1_MTCS2(val)   bfin_write32(SPORT1_MTCS2, val)
+#define bfin_read_SPORT1_MTCS3()       bfin_read32(SPORT1_MTCS3)
+#define bfin_write_SPORT1_MTCS3(val)   bfin_write32(SPORT1_MTCS3, val)
+#define bfin_read_SPORT1_MRCS0()       bfin_read32(SPORT1_MRCS0)
+#define bfin_write_SPORT1_MRCS0(val)   bfin_write32(SPORT1_MRCS0, val)
+#define bfin_read_SPORT1_MRCS1()       bfin_read32(SPORT1_MRCS1)
+#define bfin_write_SPORT1_MRCS1(val)   bfin_write32(SPORT1_MRCS1, val)
+#define bfin_read_SPORT1_MRCS2()       bfin_read32(SPORT1_MRCS2)
+#define bfin_write_SPORT1_MRCS2(val)   bfin_write32(SPORT1_MRCS2, val)
+#define bfin_read_SPORT1_MRCS3()       bfin_read32(SPORT1_MRCS3)
+#define bfin_write_SPORT1_MRCS3(val)   bfin_write32(SPORT1_MRCS3, val)
+#define bfin_read_SPORT2_TCR1()        bfin_read16(SPORT2_TCR1)
+#define bfin_write_SPORT2_TCR1(val)    bfin_write16(SPORT2_TCR1, val)
+#define bfin_read_SPORT2_TCR2()        bfin_read16(SPORT2_TCR2)
+#define bfin_write_SPORT2_TCR2(val)    bfin_write16(SPORT2_TCR2, val)
+#define bfin_read_SPORT2_TCLKDIV()     bfin_read16(SPORT2_TCLKDIV)
+#define bfin_write_SPORT2_TCLKDIV(val) bfin_write16(SPORT2_TCLKDIV, val)
+#define bfin_read_SPORT2_TFSDIV()      bfin_read16(SPORT2_TFSDIV)
+#define bfin_write_SPORT2_TFSDIV(val)  bfin_write16(SPORT2_TFSDIV, val)
+#define bfin_read_SPORT2_TX()          bfin_read32(SPORT2_TX)
+#define bfin_write_SPORT2_TX(val)      bfin_write32(SPORT2_TX, val)
+#define bfin_read_SPORT2_RX()          bfin_read32(SPORT2_RX)
+#define bfin_write_SPORT2_RX(val)      bfin_write32(SPORT2_RX, val)
+#define bfin_read_SPORT2_RCR1()        bfin_read16(SPORT2_RCR1)
+#define bfin_write_SPORT2_RCR1(val)    bfin_write16(SPORT2_RCR1, val)
+#define bfin_read_SPORT2_RCR2()        bfin_read16(SPORT2_RCR2)
+#define bfin_write_SPORT2_RCR2(val)    bfin_write16(SPORT2_RCR2, val)
+#define bfin_read_SPORT2_RCLKDIV()     bfin_read16(SPORT2_RCLKDIV)
+#define bfin_write_SPORT2_RCLKDIV(val) bfin_write16(SPORT2_RCLKDIV, val)
+#define bfin_read_SPORT2_RFSDIV()      bfin_read16(SPORT2_RFSDIV)
+#define bfin_write_SPORT2_RFSDIV(val)  bfin_write16(SPORT2_RFSDIV, val)
+#define bfin_read_SPORT2_STAT()        bfin_read16(SPORT2_STAT)
+#define bfin_write_SPORT2_STAT(val)    bfin_write16(SPORT2_STAT, val)
+#define bfin_read_SPORT2_CHNL()        bfin_read16(SPORT2_CHNL)
+#define bfin_write_SPORT2_CHNL(val)    bfin_write16(SPORT2_CHNL, val)
+#define bfin_read_SPORT2_MCMC1()       bfin_read16(SPORT2_MCMC1)
+#define bfin_write_SPORT2_MCMC1(val)   bfin_write16(SPORT2_MCMC1, val)
+#define bfin_read_SPORT2_MCMC2()       bfin_read16(SPORT2_MCMC2)
+#define bfin_write_SPORT2_MCMC2(val)   bfin_write16(SPORT2_MCMC2, val)
+#define bfin_read_SPORT2_MTCS0()       bfin_read32(SPORT2_MTCS0)
+#define bfin_write_SPORT2_MTCS0(val)   bfin_write32(SPORT2_MTCS0, val)
+#define bfin_read_SPORT2_MTCS1()       bfin_read32(SPORT2_MTCS1)
+#define bfin_write_SPORT2_MTCS1(val)   bfin_write32(SPORT2_MTCS1, val)
+#define bfin_read_SPORT2_MTCS2()       bfin_read32(SPORT2_MTCS2)
+#define bfin_write_SPORT2_MTCS2(val)   bfin_write32(SPORT2_MTCS2, val)
+#define bfin_read_SPORT2_MTCS3()       bfin_read32(SPORT2_MTCS3)
+#define bfin_write_SPORT2_MTCS3(val)   bfin_write32(SPORT2_MTCS3, val)
+#define bfin_read_SPORT2_MRCS0()       bfin_read32(SPORT2_MRCS0)
+#define bfin_write_SPORT2_MRCS0(val)   bfin_write32(SPORT2_MRCS0, val)
+#define bfin_read_SPORT2_MRCS1()       bfin_read32(SPORT2_MRCS1)
+#define bfin_write_SPORT2_MRCS1(val)   bfin_write32(SPORT2_MRCS1, val)
+#define bfin_read_SPORT2_MRCS2()       bfin_read32(SPORT2_MRCS2)
+#define bfin_write_SPORT2_MRCS2(val)   bfin_write32(SPORT2_MRCS2, val)
+#define bfin_read_SPORT2_MRCS3()       bfin_read32(SPORT2_MRCS3)
+#define bfin_write_SPORT2_MRCS3(val)   bfin_write32(SPORT2_MRCS3, val)
+#define bfin_read_SPORT3_TCR1()        bfin_read16(SPORT3_TCR1)
+#define bfin_write_SPORT3_TCR1(val)    bfin_write16(SPORT3_TCR1, val)
+#define bfin_read_SPORT3_TCR2()        bfin_read16(SPORT3_TCR2)
+#define bfin_write_SPORT3_TCR2(val)    bfin_write16(SPORT3_TCR2, val)
+#define bfin_read_SPORT3_TCLKDIV()     bfin_read16(SPORT3_TCLKDIV)
+#define bfin_write_SPORT3_TCLKDIV(val) bfin_write16(SPORT3_TCLKDIV, val)
+#define bfin_read_SPORT3_TFSDIV()      bfin_read16(SPORT3_TFSDIV)
+#define bfin_write_SPORT3_TFSDIV(val)  bfin_write16(SPORT3_TFSDIV, val)
+#define bfin_read_SPORT3_TX()          bfin_read32(SPORT3_TX)
+#define bfin_write_SPORT3_TX(val)      bfin_write32(SPORT3_TX, val)
+#define bfin_read_SPORT3_RX()          bfin_read32(SPORT3_RX)
+#define bfin_write_SPORT3_RX(val)      bfin_write32(SPORT3_RX, val)
+#define bfin_read_SPORT3_RCR1()        bfin_read16(SPORT3_RCR1)
+#define bfin_write_SPORT3_RCR1(val)    bfin_write16(SPORT3_RCR1, val)
+#define bfin_read_SPORT3_RCR2()        bfin_read16(SPORT3_RCR2)
+#define bfin_write_SPORT3_RCR2(val)    bfin_write16(SPORT3_RCR2, val)
+#define bfin_read_SPORT3_RCLKDIV()     bfin_read16(SPORT3_RCLKDIV)
+#define bfin_write_SPORT3_RCLKDIV(val) bfin_write16(SPORT3_RCLKDIV, val)
+#define bfin_read_SPORT3_RFSDIV()      bfin_read16(SPORT3_RFSDIV)
+#define bfin_write_SPORT3_RFSDIV(val)  bfin_write16(SPORT3_RFSDIV, val)
+#define bfin_read_SPORT3_STAT()        bfin_read16(SPORT3_STAT)
+#define bfin_write_SPORT3_STAT(val)    bfin_write16(SPORT3_STAT, val)
+#define bfin_read_SPORT3_CHNL()        bfin_read16(SPORT3_CHNL)
+#define bfin_write_SPORT3_CHNL(val)    bfin_write16(SPORT3_CHNL, val)
+#define bfin_read_SPORT3_MCMC1()       bfin_read16(SPORT3_MCMC1)
+#define bfin_write_SPORT3_MCMC1(val)   bfin_write16(SPORT3_MCMC1, val)
+#define bfin_read_SPORT3_MCMC2()       bfin_read16(SPORT3_MCMC2)
+#define bfin_write_SPORT3_MCMC2(val)   bfin_write16(SPORT3_MCMC2, val)
+#define bfin_read_SPORT3_MTCS0()       bfin_read32(SPORT3_MTCS0)
+#define bfin_write_SPORT3_MTCS0(val)   bfin_write32(SPORT3_MTCS0, val)
+#define bfin_read_SPORT3_MTCS1()       bfin_read32(SPORT3_MTCS1)
+#define bfin_write_SPORT3_MTCS1(val)   bfin_write32(SPORT3_MTCS1, val)
+#define bfin_read_SPORT3_MTCS2()       bfin_read32(SPORT3_MTCS2)
+#define bfin_write_SPORT3_MTCS2(val)   bfin_write32(SPORT3_MTCS2, val)
+#define bfin_read_SPORT3_MTCS3()       bfin_read32(SPORT3_MTCS3)
+#define bfin_write_SPORT3_MTCS3(val)   bfin_write32(SPORT3_MTCS3, val)
+#define bfin_read_SPORT3_MRCS0()       bfin_read32(SPORT3_MRCS0)
+#define bfin_write_SPORT3_MRCS0(val)   bfin_write32(SPORT3_MRCS0, val)
+#define bfin_read_SPORT3_MRCS1()       bfin_read32(SPORT3_MRCS1)
+#define bfin_write_SPORT3_MRCS1(val)   bfin_write32(SPORT3_MRCS1, val)
+#define bfin_read_SPORT3_MRCS2()       bfin_read32(SPORT3_MRCS2)
+#define bfin_write_SPORT3_MRCS2(val)   bfin_write32(SPORT3_MRCS2, val)
+#define bfin_read_SPORT3_MRCS3()       bfin_read32(SPORT3_MRCS3)
+#define bfin_write_SPORT3_MRCS3(val)   bfin_write32(SPORT3_MRCS3, val)
+#define bfin_read_PORTFIO()            bfin_read16(PORTFIO)
+#define bfin_write_PORTFIO(val)        bfin_write16(PORTFIO, val)
+#define bfin_read_PORTFIO_CLEAR()      bfin_read16(PORTFIO_CLEAR)
+#define bfin_write_PORTFIO_CLEAR(val)  bfin_write16(PORTFIO_CLEAR, val)
+#define bfin_read_PORTFIO_SET()        bfin_read16(PORTFIO_SET)
+#define bfin_write_PORTFIO_SET(val)    bfin_write16(PORTFIO_SET, val)
+#define bfin_read_PORTFIO_TOGGLE()     bfin_read16(PORTFIO_TOGGLE)
+#define bfin_write_PORTFIO_TOGGLE(val) bfin_write16(PORTFIO_TOGGLE, val)
+#define bfin_read_PORTFIO_MASKA()      bfin_read16(PORTFIO_MASKA)
+#define bfin_write_PORTFIO_MASKA(val)  bfin_write16(PORTFIO_MASKA, val)
+#define bfin_read_PORTFIO_MASKA_CLEAR() bfin_read16(PORTFIO_MASKA_CLEAR)
+#define bfin_write_PORTFIO_MASKA_CLEAR(val) bfin_write16(PORTFIO_MASKA_CLEAR, val)
+#define bfin_read_PORTFIO_MASKA_SET()  bfin_read16(PORTFIO_MASKA_SET)
+#define bfin_write_PORTFIO_MASKA_SET(val) bfin_write16(PORTFIO_MASKA_SET, val)
+#define bfin_read_PORTFIO_MASKA_TOGGLE() bfin_read16(PORTFIO_MASKA_TOGGLE)
+#define bfin_write_PORTFIO_MASKA_TOGGLE(val) bfin_write16(PORTFIO_MASKA_TOGGLE, val)
+#define bfin_read_PORTFIO_MASKB()      bfin_read16(PORTFIO_MASKB)
+#define bfin_write_PORTFIO_MASKB(val)  bfin_write16(PORTFIO_MASKB, val)
+#define bfin_read_PORTFIO_MASKB_CLEAR() bfin_read16(PORTFIO_MASKB_CLEAR)
+#define bfin_write_PORTFIO_MASKB_CLEAR(val) bfin_write16(PORTFIO_MASKB_CLEAR, val)
+#define bfin_read_PORTFIO_MASKB_SET()  bfin_read16(PORTFIO_MASKB_SET)
+#define bfin_write_PORTFIO_MASKB_SET(val) bfin_write16(PORTFIO_MASKB_SET, val)
+#define bfin_read_PORTFIO_MASKB_TOGGLE() bfin_read16(PORTFIO_MASKB_TOGGLE)
+#define bfin_write_PORTFIO_MASKB_TOGGLE(val) bfin_write16(PORTFIO_MASKB_TOGGLE, val)
+#define bfin_read_PORTFIO_DIR()        bfin_read16(PORTFIO_DIR)
+#define bfin_write_PORTFIO_DIR(val)    bfin_write16(PORTFIO_DIR, val)
+#define bfin_read_PORTFIO_POLAR()      bfin_read16(PORTFIO_POLAR)
+#define bfin_write_PORTFIO_POLAR(val)  bfin_write16(PORTFIO_POLAR, val)
+#define bfin_read_PORTFIO_EDGE()       bfin_read16(PORTFIO_EDGE)
+#define bfin_write_PORTFIO_EDGE(val)   bfin_write16(PORTFIO_EDGE, val)
+#define bfin_read_PORTFIO_BOTH()       bfin_read16(PORTFIO_BOTH)
+#define bfin_write_PORTFIO_BOTH(val)   bfin_write16(PORTFIO_BOTH, val)
+#define bfin_read_PORTFIO_INEN()       bfin_read16(PORTFIO_INEN)
+#define bfin_write_PORTFIO_INEN(val)   bfin_write16(PORTFIO_INEN, val)
+#define bfin_read_PORTCIO_FER()        bfin_read16(PORTCIO_FER)
+#define bfin_write_PORTCIO_FER(val)    bfin_write16(PORTCIO_FER, val)
+#define bfin_read_PORTCIO()            bfin_read16(PORTCIO)
+#define bfin_write_PORTCIO(val)        bfin_write16(PORTCIO, val)
+#define bfin_read_PORTCIO_CLEAR()      bfin_read16(PORTCIO_CLEAR)
+#define bfin_write_PORTCIO_CLEAR(val)  bfin_write16(PORTCIO_CLEAR, val)
+#define bfin_read_PORTCIO_SET()        bfin_read16(PORTCIO_SET)
+#define bfin_write_PORTCIO_SET(val)    bfin_write16(PORTCIO_SET, val)
+#define bfin_read_PORTCIO_TOGGLE()     bfin_read16(PORTCIO_TOGGLE)
+#define bfin_write_PORTCIO_TOGGLE(val) bfin_write16(PORTCIO_TOGGLE, val)
+#define bfin_read_PORTCIO_DIR()        bfin_read16(PORTCIO_DIR)
+#define bfin_write_PORTCIO_DIR(val)    bfin_write16(PORTCIO_DIR, val)
+#define bfin_read_PORTCIO_INEN()       bfin_read16(PORTCIO_INEN)
+#define bfin_write_PORTCIO_INEN(val)   bfin_write16(PORTCIO_INEN, val)
+#define bfin_read_PORTDIO_FER()        bfin_read16(PORTDIO_FER)
+#define bfin_write_PORTDIO_FER(val)    bfin_write16(PORTDIO_FER, val)
+#define bfin_read_PORTDIO()            bfin_read16(PORTDIO)
+#define bfin_write_PORTDIO(val)        bfin_write16(PORTDIO, val)
+#define bfin_read_PORTDIO_CLEAR()      bfin_read16(PORTDIO_CLEAR)
+#define bfin_write_PORTDIO_CLEAR(val)  bfin_write16(PORTDIO_CLEAR, val)
+#define bfin_read_PORTDIO_SET()        bfin_read16(PORTDIO_SET)
+#define bfin_write_PORTDIO_SET(val)    bfin_write16(PORTDIO_SET, val)
+#define bfin_read_PORTDIO_TOGGLE()     bfin_read16(PORTDIO_TOGGLE)
+#define bfin_write_PORTDIO_TOGGLE(val) bfin_write16(PORTDIO_TOGGLE, val)
+#define bfin_read_PORTDIO_DIR()        bfin_read16(PORTDIO_DIR)
+#define bfin_write_PORTDIO_DIR(val)    bfin_write16(PORTDIO_DIR, val)
+#define bfin_read_PORTDIO_INEN()       bfin_read16(PORTDIO_INEN)
+#define bfin_write_PORTDIO_INEN(val)   bfin_write16(PORTDIO_INEN, val)
+#define bfin_read_PORTEIO_FER()        bfin_read16(PORTEIO_FER)
+#define bfin_write_PORTEIO_FER(val)    bfin_write16(PORTEIO_FER, val)
+#define bfin_read_PORTEIO()            bfin_read16(PORTEIO)
+#define bfin_write_PORTEIO(val)        bfin_write16(PORTEIO, val)
+#define bfin_read_PORTEIO_CLEAR()      bfin_read16(PORTEIO_CLEAR)
+#define bfin_write_PORTEIO_CLEAR(val)  bfin_write16(PORTEIO_CLEAR, val)
+#define bfin_read_PORTEIO_SET()        bfin_read16(PORTEIO_SET)
+#define bfin_write_PORTEIO_SET(val)    bfin_write16(PORTEIO_SET, val)
+#define bfin_read_PORTEIO_TOGGLE()     bfin_read16(PORTEIO_TOGGLE)
+#define bfin_write_PORTEIO_TOGGLE(val) bfin_write16(PORTEIO_TOGGLE, val)
+#define bfin_read_PORTEIO_DIR()        bfin_read16(PORTEIO_DIR)
+#define bfin_write_PORTEIO_DIR(val)    bfin_write16(PORTEIO_DIR, val)
+#define bfin_read_PORTEIO_INEN()       bfin_read16(PORTEIO_INEN)
+#define bfin_write_PORTEIO_INEN(val)   bfin_write16(PORTEIO_INEN, val)
+#define bfin_read_EBIU_AMGCTL()        bfin_read16(EBIU_AMGCTL)
+#define bfin_write_EBIU_AMGCTL(val)    bfin_write16(EBIU_AMGCTL, val)
+#define bfin_read_EBIU_AMBCTL0()       bfin_read32(EBIU_AMBCTL0)
+#define bfin_write_EBIU_AMBCTL0(val)   bfin_write32(EBIU_AMBCTL0, val)
+#define bfin_read_EBIU_AMBCTL1()       bfin_read32(EBIU_AMBCTL1)
+#define bfin_write_EBIU_AMBCTL1(val)   bfin_write32(EBIU_AMBCTL1, val)
+#define bfin_read_EBIU_SDGCTL()        bfin_read32(EBIU_SDGCTL)
+#define bfin_write_EBIU_SDGCTL(val)    bfin_write32(EBIU_SDGCTL, val)
+#define bfin_read_EBIU_SDBCTL()        bfin_read16(EBIU_SDBCTL)
+#define bfin_write_EBIU_SDBCTL(val)    bfin_write16(EBIU_SDBCTL, val)
+#define bfin_read_EBIU_SDRRC()         bfin_read16(EBIU_SDRRC)
+#define bfin_write_EBIU_SDRRC(val)     bfin_write16(EBIU_SDRRC, val)
+#define bfin_read_EBIU_SDSTAT()        bfin_read16(EBIU_SDSTAT)
+#define bfin_write_EBIU_SDSTAT(val)    bfin_write16(EBIU_SDSTAT, val)
+#define bfin_read_DMA0_TC_PER()        bfin_read16(DMA0_TC_PER)
+#define bfin_write_DMA0_TC_PER(val)    bfin_write16(DMA0_TC_PER, val)
+#define bfin_read_DMA0_TC_CNT()        bfin_read16(DMA0_TC_CNT)
+#define bfin_write_DMA0_TC_CNT(val)    bfin_write16(DMA0_TC_CNT, val)
+#define bfin_read_DMA0_NEXT_DESC_PTR() bfin_readPTR(DMA0_NEXT_DESC_PTR)
+#define bfin_write_DMA0_NEXT_DESC_PTR(val) bfin_writePTR(DMA0_NEXT_DESC_PTR, val)
+#define bfin_read_DMA0_START_ADDR()    bfin_readPTR(DMA0_START_ADDR)
+#define bfin_write_DMA0_START_ADDR(val) bfin_writePTR(DMA0_START_ADDR, val)
+#define bfin_read_DMA0_CONFIG()        bfin_read16(DMA0_CONFIG)
+#define bfin_write_DMA0_CONFIG(val)    bfin_write16(DMA0_CONFIG, val)
+#define bfin_read_DMA0_X_COUNT()       bfin_read16(DMA0_X_COUNT)
+#define bfin_write_DMA0_X_COUNT(val)   bfin_write16(DMA0_X_COUNT, val)
+#define bfin_read_DMA0_X_MODIFY()      bfin_read16(DMA0_X_MODIFY)
+#define bfin_write_DMA0_X_MODIFY(val)  bfin_write16(DMA0_X_MODIFY, val)
+#define bfin_read_DMA0_Y_COUNT()       bfin_read16(DMA0_Y_COUNT)
+#define bfin_write_DMA0_Y_COUNT(val)   bfin_write16(DMA0_Y_COUNT, val)
+#define bfin_read_DMA0_Y_MODIFY()      bfin_read16(DMA0_Y_MODIFY)
+#define bfin_write_DMA0_Y_MODIFY(val)  bfin_write16(DMA0_Y_MODIFY, val)
+#define bfin_read_DMA0_CURR_DESC_PTR() bfin_readPTR(DMA0_CURR_DESC_PTR)
+#define bfin_write_DMA0_CURR_DESC_PTR(val) bfin_writePTR(DMA0_CURR_DESC_PTR, val)
+#define bfin_read_DMA0_CURR_ADDR()     bfin_readPTR(DMA0_CURR_ADDR)
+#define bfin_write_DMA0_CURR_ADDR(val) bfin_writePTR(DMA0_CURR_ADDR, val)
+#define bfin_read_DMA0_IRQ_STATUS()    bfin_read16(DMA0_IRQ_STATUS)
+#define bfin_write_DMA0_IRQ_STATUS(val) bfin_write16(DMA0_IRQ_STATUS, val)
+#define bfin_read_DMA0_PERIPHERAL_MAP() bfin_read16(DMA0_PERIPHERAL_MAP)
+#define bfin_write_DMA0_PERIPHERAL_MAP(val) bfin_write16(DMA0_PERIPHERAL_MAP, val)
+#define bfin_read_DMA0_CURR_X_COUNT()  bfin_read16(DMA0_CURR_X_COUNT)
+#define bfin_write_DMA0_CURR_X_COUNT(val) bfin_write16(DMA0_CURR_X_COUNT, val)
+#define bfin_read_DMA0_CURR_Y_COUNT()  bfin_read16(DMA0_CURR_Y_COUNT)
+#define bfin_write_DMA0_CURR_Y_COUNT(val) bfin_write16(DMA0_CURR_Y_COUNT, val)
+#define bfin_read_DMA1_NEXT_DESC_PTR() bfin_readPTR(DMA1_NEXT_DESC_PTR)
+#define bfin_write_DMA1_NEXT_DESC_PTR(val) bfin_writePTR(DMA1_NEXT_DESC_PTR, val)
+#define bfin_read_DMA1_START_ADDR()    bfin_readPTR(DMA1_START_ADDR)
+#define bfin_write_DMA1_START_ADDR(val) bfin_writePTR(DMA1_START_ADDR, val)
+#define bfin_read_DMA1_CONFIG()        bfin_read16(DMA1_CONFIG)
+#define bfin_write_DMA1_CONFIG(val)    bfin_write16(DMA1_CONFIG, val)
+#define bfin_read_DMA1_X_COUNT()       bfin_read16(DMA1_X_COUNT)
+#define bfin_write_DMA1_X_COUNT(val)   bfin_write16(DMA1_X_COUNT, val)
+#define bfin_read_DMA1_X_MODIFY()      bfin_read16(DMA1_X_MODIFY)
+#define bfin_write_DMA1_X_MODIFY(val)  bfin_write16(DMA1_X_MODIFY, val)
+#define bfin_read_DMA1_Y_COUNT()       bfin_read16(DMA1_Y_COUNT)
+#define bfin_write_DMA1_Y_COUNT(val)   bfin_write16(DMA1_Y_COUNT, val)
+#define bfin_read_DMA1_Y_MODIFY()      bfin_read16(DMA1_Y_MODIFY)
+#define bfin_write_DMA1_Y_MODIFY(val)  bfin_write16(DMA1_Y_MODIFY, val)
+#define bfin_read_DMA1_CURR_DESC_PTR() bfin_readPTR(DMA1_CURR_DESC_PTR)
+#define bfin_write_DMA1_CURR_DESC_PTR(val) bfin_writePTR(DMA1_CURR_DESC_PTR, val)
+#define bfin_read_DMA1_CURR_ADDR()     bfin_readPTR(DMA1_CURR_ADDR)
+#define bfin_write_DMA1_CURR_ADDR(val) bfin_writePTR(DMA1_CURR_ADDR, val)
+#define bfin_read_DMA1_IRQ_STATUS()    bfin_read16(DMA1_IRQ_STATUS)
+#define bfin_write_DMA1_IRQ_STATUS(val) bfin_write16(DMA1_IRQ_STATUS, val)
+#define bfin_read_DMA1_PERIPHERAL_MAP() bfin_read16(DMA1_PERIPHERAL_MAP)
+#define bfin_write_DMA1_PERIPHERAL_MAP(val) bfin_write16(DMA1_PERIPHERAL_MAP, val)
+#define bfin_read_DMA1_CURR_X_COUNT()  bfin_read16(DMA1_CURR_X_COUNT)
+#define bfin_write_DMA1_CURR_X_COUNT(val) bfin_write16(DMA1_CURR_X_COUNT, val)
+#define bfin_read_DMA1_CURR_Y_COUNT()  bfin_read16(DMA1_CURR_Y_COUNT)
+#define bfin_write_DMA1_CURR_Y_COUNT(val) bfin_write16(DMA1_CURR_Y_COUNT, val)
+#define bfin_read_DMA2_NEXT_DESC_PTR() bfin_readPTR(DMA2_NEXT_DESC_PTR)
+#define bfin_write_DMA2_NEXT_DESC_PTR(val) bfin_writePTR(DMA2_NEXT_DESC_PTR, val)
+#define bfin_read_DMA2_START_ADDR()    bfin_readPTR(DMA2_START_ADDR)
+#define bfin_write_DMA2_START_ADDR(val) bfin_writePTR(DMA2_START_ADDR, val)
+#define bfin_read_DMA2_CONFIG()        bfin_read16(DMA2_CONFIG)
+#define bfin_write_DMA2_CONFIG(val)    bfin_write16(DMA2_CONFIG, val)
+#define bfin_read_DMA2_X_COUNT()       bfin_read16(DMA2_X_COUNT)
+#define bfin_write_DMA2_X_COUNT(val)   bfin_write16(DMA2_X_COUNT, val)
+#define bfin_read_DMA2_X_MODIFY()      bfin_read16(DMA2_X_MODIFY)
+#define bfin_write_DMA2_X_MODIFY(val)  bfin_write16(DMA2_X_MODIFY, val)
+#define bfin_read_DMA2_Y_COUNT()       bfin_read16(DMA2_Y_COUNT)
+#define bfin_write_DMA2_Y_COUNT(val)   bfin_write16(DMA2_Y_COUNT, val)
+#define bfin_read_DMA2_Y_MODIFY()      bfin_read16(DMA2_Y_MODIFY)
+#define bfin_write_DMA2_Y_MODIFY(val)  bfin_write16(DMA2_Y_MODIFY, val)
+#define bfin_read_DMA2_CURR_DESC_PTR() bfin_readPTR(DMA2_CURR_DESC_PTR)
+#define bfin_write_DMA2_CURR_DESC_PTR(val) bfin_writePTR(DMA2_CURR_DESC_PTR, val)
+#define bfin_read_DMA2_CURR_ADDR()     bfin_readPTR(DMA2_CURR_ADDR)
+#define bfin_write_DMA2_CURR_ADDR(val) bfin_writePTR(DMA2_CURR_ADDR, val)
+#define bfin_read_DMA2_IRQ_STATUS()    bfin_read16(DMA2_IRQ_STATUS)
+#define bfin_write_DMA2_IRQ_STATUS(val) bfin_write16(DMA2_IRQ_STATUS, val)
+#define bfin_read_DMA2_PERIPHERAL_MAP() bfin_read16(DMA2_PERIPHERAL_MAP)
+#define bfin_write_DMA2_PERIPHERAL_MAP(val) bfin_write16(DMA2_PERIPHERAL_MAP, val)
+#define bfin_read_DMA2_CURR_X_COUNT()  bfin_read16(DMA2_CURR_X_COUNT)
+#define bfin_write_DMA2_CURR_X_COUNT(val) bfin_write16(DMA2_CURR_X_COUNT, val)
+#define bfin_read_DMA2_CURR_Y_COUNT()  bfin_read16(DMA2_CURR_Y_COUNT)
+#define bfin_write_DMA2_CURR_Y_COUNT(val) bfin_write16(DMA2_CURR_Y_COUNT, val)
+#define bfin_read_DMA3_NEXT_DESC_PTR() bfin_readPTR(DMA3_NEXT_DESC_PTR)
+#define bfin_write_DMA3_NEXT_DESC_PTR(val) bfin_writePTR(DMA3_NEXT_DESC_PTR, val)
+#define bfin_read_DMA3_START_ADDR()    bfin_readPTR(DMA3_START_ADDR)
+#define bfin_write_DMA3_START_ADDR(val) bfin_writePTR(DMA3_START_ADDR, val)
+#define bfin_read_DMA3_CONFIG()        bfin_read16(DMA3_CONFIG)
+#define bfin_write_DMA3_CONFIG(val)    bfin_write16(DMA3_CONFIG, val)
+#define bfin_read_DMA3_X_COUNT()       bfin_read16(DMA3_X_COUNT)
+#define bfin_write_DMA3_X_COUNT(val)   bfin_write16(DMA3_X_COUNT, val)
+#define bfin_read_DMA3_X_MODIFY()      bfin_read16(DMA3_X_MODIFY)
+#define bfin_write_DMA3_X_MODIFY(val)  bfin_write16(DMA3_X_MODIFY, val)
+#define bfin_read_DMA3_Y_COUNT()       bfin_read16(DMA3_Y_COUNT)
+#define bfin_write_DMA3_Y_COUNT(val)   bfin_write16(DMA3_Y_COUNT, val)
+#define bfin_read_DMA3_Y_MODIFY()      bfin_read16(DMA3_Y_MODIFY)
+#define bfin_write_DMA3_Y_MODIFY(val)  bfin_write16(DMA3_Y_MODIFY, val)
+#define bfin_read_DMA3_CURR_DESC_PTR() bfin_readPTR(DMA3_CURR_DESC_PTR)
+#define bfin_write_DMA3_CURR_DESC_PTR(val) bfin_writePTR(DMA3_CURR_DESC_PTR, val)
+#define bfin_read_DMA3_CURR_ADDR()     bfin_readPTR(DMA3_CURR_ADDR)
+#define bfin_write_DMA3_CURR_ADDR(val) bfin_writePTR(DMA3_CURR_ADDR, val)
+#define bfin_read_DMA3_IRQ_STATUS()    bfin_read16(DMA3_IRQ_STATUS)
+#define bfin_write_DMA3_IRQ_STATUS(val) bfin_write16(DMA3_IRQ_STATUS, val)
+#define bfin_read_DMA3_PERIPHERAL_MAP() bfin_read16(DMA3_PERIPHERAL_MAP)
+#define bfin_write_DMA3_PERIPHERAL_MAP(val) bfin_write16(DMA3_PERIPHERAL_MAP, val)
+#define bfin_read_DMA3_CURR_X_COUNT()  bfin_read16(DMA3_CURR_X_COUNT)
+#define bfin_write_DMA3_CURR_X_COUNT(val) bfin_write16(DMA3_CURR_X_COUNT, val)
+#define bfin_read_DMA3_CURR_Y_COUNT()  bfin_read16(DMA3_CURR_Y_COUNT)
+#define bfin_write_DMA3_CURR_Y_COUNT(val) bfin_write16(DMA3_CURR_Y_COUNT, val)
+#define bfin_read_DMA4_NEXT_DESC_PTR() bfin_readPTR(DMA4_NEXT_DESC_PTR)
+#define bfin_write_DMA4_NEXT_DESC_PTR(val) bfin_writePTR(DMA4_NEXT_DESC_PTR, val)
+#define bfin_read_DMA4_START_ADDR()    bfin_readPTR(DMA4_START_ADDR)
+#define bfin_write_DMA4_START_ADDR(val) bfin_writePTR(DMA4_START_ADDR, val)
+#define bfin_read_DMA4_CONFIG()        bfin_read16(DMA4_CONFIG)
+#define bfin_write_DMA4_CONFIG(val)    bfin_write16(DMA4_CONFIG, val)
+#define bfin_read_DMA4_X_COUNT()       bfin_read16(DMA4_X_COUNT)
+#define bfin_write_DMA4_X_COUNT(val)   bfin_write16(DMA4_X_COUNT, val)
+#define bfin_read_DMA4_X_MODIFY()      bfin_read16(DMA4_X_MODIFY)
+#define bfin_write_DMA4_X_MODIFY(val)  bfin_write16(DMA4_X_MODIFY, val)
+#define bfin_read_DMA4_Y_COUNT()       bfin_read16(DMA4_Y_COUNT)
+#define bfin_write_DMA4_Y_COUNT(val)   bfin_write16(DMA4_Y_COUNT, val)
+#define bfin_read_DMA4_Y_MODIFY()      bfin_read16(DMA4_Y_MODIFY)
+#define bfin_write_DMA4_Y_MODIFY(val)  bfin_write16(DMA4_Y_MODIFY, val)
+#define bfin_read_DMA4_CURR_DESC_PTR() bfin_readPTR(DMA4_CURR_DESC_PTR)
+#define bfin_write_DMA4_CURR_DESC_PTR(val) bfin_writePTR(DMA4_CURR_DESC_PTR, val)
+#define bfin_read_DMA4_CURR_ADDR()     bfin_readPTR(DMA4_CURR_ADDR)
+#define bfin_write_DMA4_CURR_ADDR(val) bfin_writePTR(DMA4_CURR_ADDR, val)
+#define bfin_read_DMA4_IRQ_STATUS()    bfin_read16(DMA4_IRQ_STATUS)
+#define bfin_write_DMA4_IRQ_STATUS(val) bfin_write16(DMA4_IRQ_STATUS, val)
+#define bfin_read_DMA4_PERIPHERAL_MAP() bfin_read16(DMA4_PERIPHERAL_MAP)
+#define bfin_write_DMA4_PERIPHERAL_MAP(val) bfin_write16(DMA4_PERIPHERAL_MAP, val)
+#define bfin_read_DMA4_CURR_X_COUNT()  bfin_read16(DMA4_CURR_X_COUNT)
+#define bfin_write_DMA4_CURR_X_COUNT(val) bfin_write16(DMA4_CURR_X_COUNT, val)
+#define bfin_read_DMA4_CURR_Y_COUNT()  bfin_read16(DMA4_CURR_Y_COUNT)
+#define bfin_write_DMA4_CURR_Y_COUNT(val) bfin_write16(DMA4_CURR_Y_COUNT, val)
+#define bfin_read_DMA5_NEXT_DESC_PTR() bfin_readPTR(DMA5_NEXT_DESC_PTR)
+#define bfin_write_DMA5_NEXT_DESC_PTR(val) bfin_writePTR(DMA5_NEXT_DESC_PTR, val)
+#define bfin_read_DMA5_START_ADDR()    bfin_readPTR(DMA5_START_ADDR)
+#define bfin_write_DMA5_START_ADDR(val) bfin_writePTR(DMA5_START_ADDR, val)
+#define bfin_read_DMA5_CONFIG()        bfin_read16(DMA5_CONFIG)
+#define bfin_write_DMA5_CONFIG(val)    bfin_write16(DMA5_CONFIG, val)
+#define bfin_read_DMA5_X_COUNT()       bfin_read16(DMA5_X_COUNT)
+#define bfin_write_DMA5_X_COUNT(val)   bfin_write16(DMA5_X_COUNT, val)
+#define bfin_read_DMA5_X_MODIFY()      bfin_read16(DMA5_X_MODIFY)
+#define bfin_write_DMA5_X_MODIFY(val)  bfin_write16(DMA5_X_MODIFY, val)
+#define bfin_read_DMA5_Y_COUNT()       bfin_read16(DMA5_Y_COUNT)
+#define bfin_write_DMA5_Y_COUNT(val)   bfin_write16(DMA5_Y_COUNT, val)
+#define bfin_read_DMA5_Y_MODIFY()      bfin_read16(DMA5_Y_MODIFY)
+#define bfin_write_DMA5_Y_MODIFY(val)  bfin_write16(DMA5_Y_MODIFY, val)
+#define bfin_read_DMA5_CURR_DESC_PTR() bfin_readPTR(DMA5_CURR_DESC_PTR)
+#define bfin_write_DMA5_CURR_DESC_PTR(val) bfin_writePTR(DMA5_CURR_DESC_PTR, val)
+#define bfin_read_DMA5_CURR_ADDR()     bfin_readPTR(DMA5_CURR_ADDR)
+#define bfin_write_DMA5_CURR_ADDR(val) bfin_writePTR(DMA5_CURR_ADDR, val)
+#define bfin_read_DMA5_IRQ_STATUS()    bfin_read16(DMA5_IRQ_STATUS)
+#define bfin_write_DMA5_IRQ_STATUS(val) bfin_write16(DMA5_IRQ_STATUS, val)
+#define bfin_read_DMA5_PERIPHERAL_MAP() bfin_read16(DMA5_PERIPHERAL_MAP)
+#define bfin_write_DMA5_PERIPHERAL_MAP(val) bfin_write16(DMA5_PERIPHERAL_MAP, val)
+#define bfin_read_DMA5_CURR_X_COUNT()  bfin_read16(DMA5_CURR_X_COUNT)
+#define bfin_write_DMA5_CURR_X_COUNT(val) bfin_write16(DMA5_CURR_X_COUNT, val)
+#define bfin_read_DMA5_CURR_Y_COUNT()  bfin_read16(DMA5_CURR_Y_COUNT)
+#define bfin_write_DMA5_CURR_Y_COUNT(val) bfin_write16(DMA5_CURR_Y_COUNT, val)
+#define bfin_read_DMA6_NEXT_DESC_PTR() bfin_readPTR(DMA6_NEXT_DESC_PTR)
+#define bfin_write_DMA6_NEXT_DESC_PTR(val) bfin_writePTR(DMA6_NEXT_DESC_PTR, val)
+#define bfin_read_DMA6_START_ADDR()    bfin_readPTR(DMA6_START_ADDR)
+#define bfin_write_DMA6_START_ADDR(val) bfin_writePTR(DMA6_START_ADDR, val)
+#define bfin_read_DMA6_CONFIG()        bfin_read16(DMA6_CONFIG)
+#define bfin_write_DMA6_CONFIG(val)    bfin_write16(DMA6_CONFIG, val)
+#define bfin_read_DMA6_X_COUNT()       bfin_read16(DMA6_X_COUNT)
+#define bfin_write_DMA6_X_COUNT(val)   bfin_write16(DMA6_X_COUNT, val)
+#define bfin_read_DMA6_X_MODIFY()      bfin_read16(DMA6_X_MODIFY)
+#define bfin_write_DMA6_X_MODIFY(val)  bfin_write16(DMA6_X_MODIFY, val)
+#define bfin_read_DMA6_Y_COUNT()       bfin_read16(DMA6_Y_COUNT)
+#define bfin_write_DMA6_Y_COUNT(val)   bfin_write16(DMA6_Y_COUNT, val)
+#define bfin_read_DMA6_Y_MODIFY()      bfin_read16(DMA6_Y_MODIFY)
+#define bfin_write_DMA6_Y_MODIFY(val)  bfin_write16(DMA6_Y_MODIFY, val)
+#define bfin_read_DMA6_CURR_DESC_PTR() bfin_readPTR(DMA6_CURR_DESC_PTR)
+#define bfin_write_DMA6_CURR_DESC_PTR(val) bfin_writePTR(DMA6_CURR_DESC_PTR, val)
+#define bfin_read_DMA6_CURR_ADDR()     bfin_readPTR(DMA6_CURR_ADDR)
+#define bfin_write_DMA6_CURR_ADDR(val) bfin_writePTR(DMA6_CURR_ADDR, val)
+#define bfin_read_DMA6_IRQ_STATUS()    bfin_read16(DMA6_IRQ_STATUS)
+#define bfin_write_DMA6_IRQ_STATUS(val) bfin_write16(DMA6_IRQ_STATUS, val)
+#define bfin_read_DMA6_PERIPHERAL_MAP() bfin_read16(DMA6_PERIPHERAL_MAP)
+#define bfin_write_DMA6_PERIPHERAL_MAP(val) bfin_write16(DMA6_PERIPHERAL_MAP, val)
+#define bfin_read_DMA6_CURR_X_COUNT()  bfin_read16(DMA6_CURR_X_COUNT)
+#define bfin_write_DMA6_CURR_X_COUNT(val) bfin_write16(DMA6_CURR_X_COUNT, val)
+#define bfin_read_DMA6_CURR_Y_COUNT()  bfin_read16(DMA6_CURR_Y_COUNT)
+#define bfin_write_DMA6_CURR_Y_COUNT(val) bfin_write16(DMA6_CURR_Y_COUNT, val)
+#define bfin_read_DMA7_NEXT_DESC_PTR() bfin_readPTR(DMA7_NEXT_DESC_PTR)
+#define bfin_write_DMA7_NEXT_DESC_PTR(val) bfin_writePTR(DMA7_NEXT_DESC_PTR, val)
+#define bfin_read_DMA7_START_ADDR()    bfin_readPTR(DMA7_START_ADDR)
+#define bfin_write_DMA7_START_ADDR(val) bfin_writePTR(DMA7_START_ADDR, val)
+#define bfin_read_DMA7_CONFIG()        bfin_read16(DMA7_CONFIG)
+#define bfin_write_DMA7_CONFIG(val)    bfin_write16(DMA7_CONFIG, val)
+#define bfin_read_DMA7_X_COUNT()       bfin_read16(DMA7_X_COUNT)
+#define bfin_write_DMA7_X_COUNT(val)   bfin_write16(DMA7_X_COUNT, val)
+#define bfin_read_DMA7_X_MODIFY()      bfin_read16(DMA7_X_MODIFY)
+#define bfin_write_DMA7_X_MODIFY(val)  bfin_write16(DMA7_X_MODIFY, val)
+#define bfin_read_DMA7_Y_COUNT()       bfin_read16(DMA7_Y_COUNT)
+#define bfin_write_DMA7_Y_COUNT(val)   bfin_write16(DMA7_Y_COUNT, val)
+#define bfin_read_DMA7_Y_MODIFY()      bfin_read16(DMA7_Y_MODIFY)
+#define bfin_write_DMA7_Y_MODIFY(val)  bfin_write16(DMA7_Y_MODIFY, val)
+#define bfin_read_DMA7_CURR_DESC_PTR() bfin_readPTR(DMA7_CURR_DESC_PTR)
+#define bfin_write_DMA7_CURR_DESC_PTR(val) bfin_writePTR(DMA7_CURR_DESC_PTR, val)
+#define bfin_read_DMA7_CURR_ADDR()     bfin_readPTR(DMA7_CURR_ADDR)
+#define bfin_write_DMA7_CURR_ADDR(val) bfin_writePTR(DMA7_CURR_ADDR, val)
+#define bfin_read_DMA7_IRQ_STATUS()    bfin_read16(DMA7_IRQ_STATUS)
+#define bfin_write_DMA7_IRQ_STATUS(val) bfin_write16(DMA7_IRQ_STATUS, val)
+#define bfin_read_DMA7_PERIPHERAL_MAP() bfin_read16(DMA7_PERIPHERAL_MAP)
+#define bfin_write_DMA7_PERIPHERAL_MAP(val) bfin_write16(DMA7_PERIPHERAL_MAP, val)
+#define bfin_read_DMA7_CURR_X_COUNT()  bfin_read16(DMA7_CURR_X_COUNT)
+#define bfin_write_DMA7_CURR_X_COUNT(val) bfin_write16(DMA7_CURR_X_COUNT, val)
+#define bfin_read_DMA7_CURR_Y_COUNT()  bfin_read16(DMA7_CURR_Y_COUNT)
+#define bfin_write_DMA7_CURR_Y_COUNT(val) bfin_write16(DMA7_CURR_Y_COUNT, val)
+#define bfin_read_DMA1_TC_PER()        bfin_read16(DMA1_TC_PER)
+#define bfin_write_DMA1_TC_PER(val)    bfin_write16(DMA1_TC_PER, val)
+#define bfin_read_DMA1_TC_CNT()        bfin_read16(DMA1_TC_CNT)
+#define bfin_write_DMA1_TC_CNT(val)    bfin_write16(DMA1_TC_CNT, val)
+#define bfin_read_DMA8_NEXT_DESC_PTR() bfin_readPTR(DMA8_NEXT_DESC_PTR)
+#define bfin_write_DMA8_NEXT_DESC_PTR(val) bfin_writePTR(DMA8_NEXT_DESC_PTR, val)
+#define bfin_read_DMA8_START_ADDR()    bfin_readPTR(DMA8_START_ADDR)
+#define bfin_write_DMA8_START_ADDR(val) bfin_writePTR(DMA8_START_ADDR, val)
+#define bfin_read_DMA8_CONFIG()        bfin_read16(DMA8_CONFIG)
+#define bfin_write_DMA8_CONFIG(val)    bfin_write16(DMA8_CONFIG, val)
+#define bfin_read_DMA8_X_COUNT()       bfin_read16(DMA8_X_COUNT)
+#define bfin_write_DMA8_X_COUNT(val)   bfin_write16(DMA8_X_COUNT, val)
+#define bfin_read_DMA8_X_MODIFY()      bfin_read16(DMA8_X_MODIFY)
+#define bfin_write_DMA8_X_MODIFY(val)  bfin_write16(DMA8_X_MODIFY, val)
+#define bfin_read_DMA8_Y_COUNT()       bfin_read16(DMA8_Y_COUNT)
+#define bfin_write_DMA8_Y_COUNT(val)   bfin_write16(DMA8_Y_COUNT, val)
+#define bfin_read_DMA8_Y_MODIFY()      bfin_read16(DMA8_Y_MODIFY)
+#define bfin_write_DMA8_Y_MODIFY(val)  bfin_write16(DMA8_Y_MODIFY, val)
+#define bfin_read_DMA8_CURR_DESC_PTR() bfin_readPTR(DMA8_CURR_DESC_PTR)
+#define bfin_write_DMA8_CURR_DESC_PTR(val) bfin_writePTR(DMA8_CURR_DESC_PTR, val)
+#define bfin_read_DMA8_CURR_ADDR()     bfin_readPTR(DMA8_CURR_ADDR)
+#define bfin_write_DMA8_CURR_ADDR(val) bfin_writePTR(DMA8_CURR_ADDR, val)
+#define bfin_read_DMA8_IRQ_STATUS()    bfin_read16(DMA8_IRQ_STATUS)
+#define bfin_write_DMA8_IRQ_STATUS(val) bfin_write16(DMA8_IRQ_STATUS, val)
+#define bfin_read_DMA8_PERIPHERAL_MAP() bfin_read16(DMA8_PERIPHERAL_MAP)
+#define bfin_write_DMA8_PERIPHERAL_MAP(val) bfin_write16(DMA8_PERIPHERAL_MAP, val)
+#define bfin_read_DMA8_CURR_X_COUNT()  bfin_read16(DMA8_CURR_X_COUNT)
+#define bfin_write_DMA8_CURR_X_COUNT(val) bfin_write16(DMA8_CURR_X_COUNT, val)
+#define bfin_read_DMA8_CURR_Y_COUNT()  bfin_read16(DMA8_CURR_Y_COUNT)
+#define bfin_write_DMA8_CURR_Y_COUNT(val) bfin_write16(DMA8_CURR_Y_COUNT, val)
+#define bfin_read_DMA9_NEXT_DESC_PTR() bfin_readPTR(DMA9_NEXT_DESC_PTR)
+#define bfin_write_DMA9_NEXT_DESC_PTR(val) bfin_writePTR(DMA9_NEXT_DESC_PTR, val)
+#define bfin_read_DMA9_START_ADDR()    bfin_readPTR(DMA9_START_ADDR)
+#define bfin_write_DMA9_START_ADDR(val) bfin_writePTR(DMA9_START_ADDR, val)
+#define bfin_read_DMA9_CONFIG()        bfin_read16(DMA9_CONFIG)
+#define bfin_write_DMA9_CONFIG(val)    bfin_write16(DMA9_CONFIG, val)
+#define bfin_read_DMA9_X_COUNT()       bfin_read16(DMA9_X_COUNT)
+#define bfin_write_DMA9_X_COUNT(val)   bfin_write16(DMA9_X_COUNT, val)
+#define bfin_read_DMA9_X_MODIFY()      bfin_read16(DMA9_X_MODIFY)
+#define bfin_write_DMA9_X_MODIFY(val)  bfin_write16(DMA9_X_MODIFY, val)
+#define bfin_read_DMA9_Y_COUNT()       bfin_read16(DMA9_Y_COUNT)
+#define bfin_write_DMA9_Y_COUNT(val)   bfin_write16(DMA9_Y_COUNT, val)
+#define bfin_read_DMA9_Y_MODIFY()      bfin_read16(DMA9_Y_MODIFY)
+#define bfin_write_DMA9_Y_MODIFY(val)  bfin_write16(DMA9_Y_MODIFY, val)
+#define bfin_read_DMA9_CURR_DESC_PTR() bfin_readPTR(DMA9_CURR_DESC_PTR)
+#define bfin_write_DMA9_CURR_DESC_PTR(val) bfin_writePTR(DMA9_CURR_DESC_PTR, val)
+#define bfin_read_DMA9_CURR_ADDR()     bfin_readPTR(DMA9_CURR_ADDR)
+#define bfin_write_DMA9_CURR_ADDR(val) bfin_writePTR(DMA9_CURR_ADDR, val)
+#define bfin_read_DMA9_IRQ_STATUS()    bfin_read16(DMA9_IRQ_STATUS)
+#define bfin_write_DMA9_IRQ_STATUS(val) bfin_write16(DMA9_IRQ_STATUS, val)
+#define bfin_read_DMA9_PERIPHERAL_MAP() bfin_read16(DMA9_PERIPHERAL_MAP)
+#define bfin_write_DMA9_PERIPHERAL_MAP(val) bfin_write16(DMA9_PERIPHERAL_MAP, val)
+#define bfin_read_DMA9_CURR_X_COUNT()  bfin_read16(DMA9_CURR_X_COUNT)
+#define bfin_write_DMA9_CURR_X_COUNT(val) bfin_write16(DMA9_CURR_X_COUNT, val)
+#define bfin_read_DMA9_CURR_Y_COUNT()  bfin_read16(DMA9_CURR_Y_COUNT)
+#define bfin_write_DMA9_CURR_Y_COUNT(val) bfin_write16(DMA9_CURR_Y_COUNT, val)
+#define bfin_read_DMA10_NEXT_DESC_PTR() bfin_readPTR(DMA10_NEXT_DESC_PTR)
+#define bfin_write_DMA10_NEXT_DESC_PTR(val) bfin_writePTR(DMA10_NEXT_DESC_PTR, val)
+#define bfin_read_DMA10_START_ADDR()   bfin_readPTR(DMA10_START_ADDR)
+#define bfin_write_DMA10_START_ADDR(val) bfin_writePTR(DMA10_START_ADDR, val)
+#define bfin_read_DMA10_CONFIG()       bfin_read16(DMA10_CONFIG)
+#define bfin_write_DMA10_CONFIG(val)   bfin_write16(DMA10_CONFIG, val)
+#define bfin_read_DMA10_X_COUNT()      bfin_read16(DMA10_X_COUNT)
+#define bfin_write_DMA10_X_COUNT(val)  bfin_write16(DMA10_X_COUNT, val)
+#define bfin_read_DMA10_X_MODIFY()     bfin_read16(DMA10_X_MODIFY)
+#define bfin_write_DMA10_X_MODIFY(val) bfin_write16(DMA10_X_MODIFY, val)
+#define bfin_read_DMA10_Y_COUNT()      bfin_read16(DMA10_Y_COUNT)
+#define bfin_write_DMA10_Y_COUNT(val)  bfin_write16(DMA10_Y_COUNT, val)
+#define bfin_read_DMA10_Y_MODIFY()     bfin_read16(DMA10_Y_MODIFY)
+#define bfin_write_DMA10_Y_MODIFY(val) bfin_write16(DMA10_Y_MODIFY, val)
+#define bfin_read_DMA10_CURR_DESC_PTR() bfin_readPTR(DMA10_CURR_DESC_PTR)
+#define bfin_write_DMA10_CURR_DESC_PTR(val) bfin_writePTR(DMA10_CURR_DESC_PTR, val)
+#define bfin_read_DMA10_CURR_ADDR()    bfin_readPTR(DMA10_CURR_ADDR)
+#define bfin_write_DMA10_CURR_ADDR(val) bfin_writePTR(DMA10_CURR_ADDR, val)
+#define bfin_read_DMA10_IRQ_STATUS()   bfin_read16(DMA10_IRQ_STATUS)
+#define bfin_write_DMA10_IRQ_STATUS(val) bfin_write16(DMA10_IRQ_STATUS, val)
+#define bfin_read_DMA10_PERIPHERAL_MAP() bfin_read16(DMA10_PERIPHERAL_MAP)
+#define bfin_write_DMA10_PERIPHERAL_MAP(val) bfin_write16(DMA10_PERIPHERAL_MAP, val)
+#define bfin_read_DMA10_CURR_X_COUNT() bfin_read16(DMA10_CURR_X_COUNT)
+#define bfin_write_DMA10_CURR_X_COUNT(val) bfin_write16(DMA10_CURR_X_COUNT, val)
+#define bfin_read_DMA10_CURR_Y_COUNT() bfin_read16(DMA10_CURR_Y_COUNT)
+#define bfin_write_DMA10_CURR_Y_COUNT(val) bfin_write16(DMA10_CURR_Y_COUNT, val)
+#define bfin_read_DMA11_NEXT_DESC_PTR() bfin_readPTR(DMA11_NEXT_DESC_PTR)
+#define bfin_write_DMA11_NEXT_DESC_PTR(val) bfin_writePTR(DMA11_NEXT_DESC_PTR, val)
+#define bfin_read_DMA11_START_ADDR()   bfin_readPTR(DMA11_START_ADDR)
+#define bfin_write_DMA11_START_ADDR(val) bfin_writePTR(DMA11_START_ADDR, val)
+#define bfin_read_DMA11_CONFIG()       bfin_read16(DMA11_CONFIG)
+#define bfin_write_DMA11_CONFIG(val)   bfin_write16(DMA11_CONFIG, val)
+#define bfin_read_DMA11_X_COUNT()      bfin_read16(DMA11_X_COUNT)
+#define bfin_write_DMA11_X_COUNT(val)  bfin_write16(DMA11_X_COUNT, val)
+#define bfin_read_DMA11_X_MODIFY()     bfin_read16(DMA11_X_MODIFY)
+#define bfin_write_DMA11_X_MODIFY(val) bfin_write16(DMA11_X_MODIFY, val)
+#define bfin_read_DMA11_Y_COUNT()      bfin_read16(DMA11_Y_COUNT)
+#define bfin_write_DMA11_Y_COUNT(val)  bfin_write16(DMA11_Y_COUNT, val)
+#define bfin_read_DMA11_Y_MODIFY()     bfin_read16(DMA11_Y_MODIFY)
+#define bfin_write_DMA11_Y_MODIFY(val) bfin_write16(DMA11_Y_MODIFY, val)
+#define bfin_read_DMA11_CURR_DESC_PTR() bfin_readPTR(DMA11_CURR_DESC_PTR)
+#define bfin_write_DMA11_CURR_DESC_PTR(val) bfin_writePTR(DMA11_CURR_DESC_PTR, val)
+#define bfin_read_DMA11_CURR_ADDR()    bfin_readPTR(DMA11_CURR_ADDR)
+#define bfin_write_DMA11_CURR_ADDR(val) bfin_writePTR(DMA11_CURR_ADDR, val)
+#define bfin_read_DMA11_IRQ_STATUS()   bfin_read16(DMA11_IRQ_STATUS)
+#define bfin_write_DMA11_IRQ_STATUS(val) bfin_write16(DMA11_IRQ_STATUS, val)
+#define bfin_read_DMA11_PERIPHERAL_MAP() bfin_read16(DMA11_PERIPHERAL_MAP)
+#define bfin_write_DMA11_PERIPHERAL_MAP(val) bfin_write16(DMA11_PERIPHERAL_MAP, val)
+#define bfin_read_DMA11_CURR_X_COUNT() bfin_read16(DMA11_CURR_X_COUNT)
+#define bfin_write_DMA11_CURR_X_COUNT(val) bfin_write16(DMA11_CURR_X_COUNT, val)
+#define bfin_read_DMA11_CURR_Y_COUNT() bfin_read16(DMA11_CURR_Y_COUNT)
+#define bfin_write_DMA11_CURR_Y_COUNT(val) bfin_write16(DMA11_CURR_Y_COUNT, val)
+#define bfin_read_DMA12_NEXT_DESC_PTR() bfin_readPTR(DMA12_NEXT_DESC_PTR)
+#define bfin_write_DMA12_NEXT_DESC_PTR(val) bfin_writePTR(DMA12_NEXT_DESC_PTR, val)
+#define bfin_read_DMA12_START_ADDR()   bfin_readPTR(DMA12_START_ADDR)
+#define bfin_write_DMA12_START_ADDR(val) bfin_writePTR(DMA12_START_ADDR, val)
+#define bfin_read_DMA12_CONFIG()       bfin_read16(DMA12_CONFIG)
+#define bfin_write_DMA12_CONFIG(val)   bfin_write16(DMA12_CONFIG, val)
+#define bfin_read_DMA12_X_COUNT()      bfin_read16(DMA12_X_COUNT)
+#define bfin_write_DMA12_X_COUNT(val)  bfin_write16(DMA12_X_COUNT, val)
+#define bfin_read_DMA12_X_MODIFY()     bfin_read16(DMA12_X_MODIFY)
+#define bfin_write_DMA12_X_MODIFY(val) bfin_write16(DMA12_X_MODIFY, val)
+#define bfin_read_DMA12_Y_COUNT()      bfin_read16(DMA12_Y_COUNT)
+#define bfin_write_DMA12_Y_COUNT(val)  bfin_write16(DMA12_Y_COUNT, val)
+#define bfin_read_DMA12_Y_MODIFY()     bfin_read16(DMA12_Y_MODIFY)
+#define bfin_write_DMA12_Y_MODIFY(val) bfin_write16(DMA12_Y_MODIFY, val)
+#define bfin_read_DMA12_CURR_DESC_PTR() bfin_readPTR(DMA12_CURR_DESC_PTR)
+#define bfin_write_DMA12_CURR_DESC_PTR(val) bfin_writePTR(DMA12_CURR_DESC_PTR, val)
+#define bfin_read_DMA12_CURR_ADDR()    bfin_readPTR(DMA12_CURR_ADDR)
+#define bfin_write_DMA12_CURR_ADDR(val) bfin_writePTR(DMA12_CURR_ADDR, val)
+#define bfin_read_DMA12_IRQ_STATUS()   bfin_read16(DMA12_IRQ_STATUS)
+#define bfin_write_DMA12_IRQ_STATUS(val) bfin_write16(DMA12_IRQ_STATUS, val)
+#define bfin_read_DMA12_PERIPHERAL_MAP() bfin_read16(DMA12_PERIPHERAL_MAP)
+#define bfin_write_DMA12_PERIPHERAL_MAP(val) bfin_write16(DMA12_PERIPHERAL_MAP, val)
+#define bfin_read_DMA12_CURR_X_COUNT() bfin_read16(DMA12_CURR_X_COUNT)
+#define bfin_write_DMA12_CURR_X_COUNT(val) bfin_write16(DMA12_CURR_X_COUNT, val)
+#define bfin_read_DMA12_CURR_Y_COUNT() bfin_read16(DMA12_CURR_Y_COUNT)
+#define bfin_write_DMA12_CURR_Y_COUNT(val) bfin_write16(DMA12_CURR_Y_COUNT, val)
+#define bfin_read_DMA13_NEXT_DESC_PTR() bfin_readPTR(DMA13_NEXT_DESC_PTR)
+#define bfin_write_DMA13_NEXT_DESC_PTR(val) bfin_writePTR(DMA13_NEXT_DESC_PTR, val)
+#define bfin_read_DMA13_START_ADDR()   bfin_readPTR(DMA13_START_ADDR)
+#define bfin_write_DMA13_START_ADDR(val) bfin_writePTR(DMA13_START_ADDR, val)
+#define bfin_read_DMA13_CONFIG()       bfin_read16(DMA13_CONFIG)
+#define bfin_write_DMA13_CONFIG(val)   bfin_write16(DMA13_CONFIG, val)
+#define bfin_read_DMA13_X_COUNT()      bfin_read16(DMA13_X_COUNT)
+#define bfin_write_DMA13_X_COUNT(val)  bfin_write16(DMA13_X_COUNT, val)
+#define bfin_read_DMA13_X_MODIFY()     bfin_read16(DMA13_X_MODIFY)
+#define bfin_write_DMA13_X_MODIFY(val) bfin_write16(DMA13_X_MODIFY, val)
+#define bfin_read_DMA13_Y_COUNT()      bfin_read16(DMA13_Y_COUNT)
+#define bfin_write_DMA13_Y_COUNT(val)  bfin_write16(DMA13_Y_COUNT, val)
+#define bfin_read_DMA13_Y_MODIFY()     bfin_read16(DMA13_Y_MODIFY)
+#define bfin_write_DMA13_Y_MODIFY(val) bfin_write16(DMA13_Y_MODIFY, val)
+#define bfin_read_DMA13_CURR_DESC_PTR() bfin_readPTR(DMA13_CURR_DESC_PTR)
+#define bfin_write_DMA13_CURR_DESC_PTR(val) bfin_writePTR(DMA13_CURR_DESC_PTR, val)
+#define bfin_read_DMA13_CURR_ADDR()    bfin_readPTR(DMA13_CURR_ADDR)
+#define bfin_write_DMA13_CURR_ADDR(val) bfin_writePTR(DMA13_CURR_ADDR, val)
+#define bfin_read_DMA13_IRQ_STATUS()   bfin_read16(DMA13_IRQ_STATUS)
+#define bfin_write_DMA13_IRQ_STATUS(val) bfin_write16(DMA13_IRQ_STATUS, val)
+#define bfin_read_DMA13_PERIPHERAL_MAP() bfin_read16(DMA13_PERIPHERAL_MAP)
+#define bfin_write_DMA13_PERIPHERAL_MAP(val) bfin_write16(DMA13_PERIPHERAL_MAP, val)
+#define bfin_read_DMA13_CURR_X_COUNT() bfin_read16(DMA13_CURR_X_COUNT)
+#define bfin_write_DMA13_CURR_X_COUNT(val) bfin_write16(DMA13_CURR_X_COUNT, val)
+#define bfin_read_DMA13_CURR_Y_COUNT() bfin_read16(DMA13_CURR_Y_COUNT)
+#define bfin_write_DMA13_CURR_Y_COUNT(val) bfin_write16(DMA13_CURR_Y_COUNT, val)
+#define bfin_read_DMA14_NEXT_DESC_PTR() bfin_readPTR(DMA14_NEXT_DESC_PTR)
+#define bfin_write_DMA14_NEXT_DESC_PTR(val) bfin_writePTR(DMA14_NEXT_DESC_PTR, val)
+#define bfin_read_DMA14_START_ADDR()   bfin_readPTR(DMA14_START_ADDR)
+#define bfin_write_DMA14_START_ADDR(val) bfin_writePTR(DMA14_START_ADDR, val)
+#define bfin_read_DMA14_CONFIG()       bfin_read16(DMA14_CONFIG)
+#define bfin_write_DMA14_CONFIG(val)   bfin_write16(DMA14_CONFIG, val)
+#define bfin_read_DMA14_X_COUNT()      bfin_read16(DMA14_X_COUNT)
+#define bfin_write_DMA14_X_COUNT(val)  bfin_write16(DMA14_X_COUNT, val)
+#define bfin_read_DMA14_X_MODIFY()     bfin_read16(DMA14_X_MODIFY)
+#define bfin_write_DMA14_X_MODIFY(val) bfin_write16(DMA14_X_MODIFY, val)
+#define bfin_read_DMA14_Y_COUNT()      bfin_read16(DMA14_Y_COUNT)
+#define bfin_write_DMA14_Y_COUNT(val)  bfin_write16(DMA14_Y_COUNT, val)
+#define bfin_read_DMA14_Y_MODIFY()     bfin_read16(DMA14_Y_MODIFY)
+#define bfin_write_DMA14_Y_MODIFY(val) bfin_write16(DMA14_Y_MODIFY, val)
+#define bfin_read_DMA14_CURR_DESC_PTR() bfin_readPTR(DMA14_CURR_DESC_PTR)
+#define bfin_write_DMA14_CURR_DESC_PTR(val) bfin_writePTR(DMA14_CURR_DESC_PTR, val)
+#define bfin_read_DMA14_CURR_ADDR()    bfin_readPTR(DMA14_CURR_ADDR)
+#define bfin_write_DMA14_CURR_ADDR(val) bfin_writePTR(DMA14_CURR_ADDR, val)
+#define bfin_read_DMA14_IRQ_STATUS()   bfin_read16(DMA14_IRQ_STATUS)
+#define bfin_write_DMA14_IRQ_STATUS(val) bfin_write16(DMA14_IRQ_STATUS, val)
+#define bfin_read_DMA14_PERIPHERAL_MAP() bfin_read16(DMA14_PERIPHERAL_MAP)
+#define bfin_write_DMA14_PERIPHERAL_MAP(val) bfin_write16(DMA14_PERIPHERAL_MAP, val)
+#define bfin_read_DMA14_CURR_X_COUNT() bfin_read16(DMA14_CURR_X_COUNT)
+#define bfin_write_DMA14_CURR_X_COUNT(val) bfin_write16(DMA14_CURR_X_COUNT, val)
+#define bfin_read_DMA14_CURR_Y_COUNT() bfin_read16(DMA14_CURR_Y_COUNT)
+#define bfin_write_DMA14_CURR_Y_COUNT(val) bfin_write16(DMA14_CURR_Y_COUNT, val)
+#define bfin_read_DMA15_NEXT_DESC_PTR() bfin_readPTR(DMA15_NEXT_DESC_PTR)
+#define bfin_write_DMA15_NEXT_DESC_PTR(val) bfin_writePTR(DMA15_NEXT_DESC_PTR, val)
+#define bfin_read_DMA15_START_ADDR()   bfin_readPTR(DMA15_START_ADDR)
+#define bfin_write_DMA15_START_ADDR(val) bfin_writePTR(DMA15_START_ADDR, val)
+#define bfin_read_DMA15_CONFIG()       bfin_read16(DMA15_CONFIG)
+#define bfin_write_DMA15_CONFIG(val)   bfin_write16(DMA15_CONFIG, val)
+#define bfin_read_DMA15_X_COUNT()      bfin_read16(DMA15_X_COUNT)
+#define bfin_write_DMA15_X_COUNT(val)  bfin_write16(DMA15_X_COUNT, val)
+#define bfin_read_DMA15_X_MODIFY()     bfin_read16(DMA15_X_MODIFY)
+#define bfin_write_DMA15_X_MODIFY(val) bfin_write16(DMA15_X_MODIFY, val)
+#define bfin_read_DMA15_Y_COUNT()      bfin_read16(DMA15_Y_COUNT)
+#define bfin_write_DMA15_Y_COUNT(val)  bfin_write16(DMA15_Y_COUNT, val)
+#define bfin_read_DMA15_Y_MODIFY()     bfin_read16(DMA15_Y_MODIFY)
+#define bfin_write_DMA15_Y_MODIFY(val) bfin_write16(DMA15_Y_MODIFY, val)
+#define bfin_read_DMA15_CURR_DESC_PTR() bfin_readPTR(DMA15_CURR_DESC_PTR)
+#define bfin_write_DMA15_CURR_DESC_PTR(val) bfin_writePTR(DMA15_CURR_DESC_PTR, val)
+#define bfin_read_DMA15_CURR_ADDR()    bfin_readPTR(DMA15_CURR_ADDR)
+#define bfin_write_DMA15_CURR_ADDR(val) bfin_writePTR(DMA15_CURR_ADDR, val)
+#define bfin_read_DMA15_IRQ_STATUS()   bfin_read16(DMA15_IRQ_STATUS)
+#define bfin_write_DMA15_IRQ_STATUS(val) bfin_write16(DMA15_IRQ_STATUS, val)
+#define bfin_read_DMA15_PERIPHERAL_MAP() bfin_read16(DMA15_PERIPHERAL_MAP)
+#define bfin_write_DMA15_PERIPHERAL_MAP(val) bfin_write16(DMA15_PERIPHERAL_MAP, val)
+#define bfin_read_DMA15_CURR_X_COUNT() bfin_read16(DMA15_CURR_X_COUNT)
+#define bfin_write_DMA15_CURR_X_COUNT(val) bfin_write16(DMA15_CURR_X_COUNT, val)
+#define bfin_read_DMA15_CURR_Y_COUNT() bfin_read16(DMA15_CURR_Y_COUNT)
+#define bfin_write_DMA15_CURR_Y_COUNT(val) bfin_write16(DMA15_CURR_Y_COUNT, val)
+#define bfin_read_DMA16_NEXT_DESC_PTR() bfin_readPTR(DMA16_NEXT_DESC_PTR)
+#define bfin_write_DMA16_NEXT_DESC_PTR(val) bfin_writePTR(DMA16_NEXT_DESC_PTR, val)
+#define bfin_read_DMA16_START_ADDR()   bfin_readPTR(DMA16_START_ADDR)
+#define bfin_write_DMA16_START_ADDR(val) bfin_writePTR(DMA16_START_ADDR, val)
+#define bfin_read_DMA16_CONFIG()       bfin_read16(DMA16_CONFIG)
+#define bfin_write_DMA16_CONFIG(val)   bfin_write16(DMA16_CONFIG, val)
+#define bfin_read_DMA16_X_COUNT()      bfin_read16(DMA16_X_COUNT)
+#define bfin_write_DMA16_X_COUNT(val)  bfin_write16(DMA16_X_COUNT, val)
+#define bfin_read_DMA16_X_MODIFY()     bfin_read16(DMA16_X_MODIFY)
+#define bfin_write_DMA16_X_MODIFY(val) bfin_write16(DMA16_X_MODIFY, val)
+#define bfin_read_DMA16_Y_COUNT()      bfin_read16(DMA16_Y_COUNT)
+#define bfin_write_DMA16_Y_COUNT(val)  bfin_write16(DMA16_Y_COUNT, val)
+#define bfin_read_DMA16_Y_MODIFY()     bfin_read16(DMA16_Y_MODIFY)
+#define bfin_write_DMA16_Y_MODIFY(val) bfin_write16(DMA16_Y_MODIFY, val)
+#define bfin_read_DMA16_CURR_DESC_PTR() bfin_readPTR(DMA16_CURR_DESC_PTR)
+#define bfin_write_DMA16_CURR_DESC_PTR(val) bfin_writePTR(DMA16_CURR_DESC_PTR, val)
+#define bfin_read_DMA16_CURR_ADDR()    bfin_readPTR(DMA16_CURR_ADDR)
+#define bfin_write_DMA16_CURR_ADDR(val) bfin_writePTR(DMA16_CURR_ADDR, val)
+#define bfin_read_DMA16_IRQ_STATUS()   bfin_read16(DMA16_IRQ_STATUS)
+#define bfin_write_DMA16_IRQ_STATUS(val) bfin_write16(DMA16_IRQ_STATUS, val)
+#define bfin_read_DMA16_PERIPHERAL_MAP() bfin_read16(DMA16_PERIPHERAL_MAP)
+#define bfin_write_DMA16_PERIPHERAL_MAP(val) bfin_write16(DMA16_PERIPHERAL_MAP, val)
+#define bfin_read_DMA16_CURR_X_COUNT() bfin_read16(DMA16_CURR_X_COUNT)
+#define bfin_write_DMA16_CURR_X_COUNT(val) bfin_write16(DMA16_CURR_X_COUNT, val)
+#define bfin_read_DMA16_CURR_Y_COUNT() bfin_read16(DMA16_CURR_Y_COUNT)
+#define bfin_write_DMA16_CURR_Y_COUNT(val) bfin_write16(DMA16_CURR_Y_COUNT, val)
+#define bfin_read_DMA17_NEXT_DESC_PTR() bfin_readPTR(DMA17_NEXT_DESC_PTR)
+#define bfin_write_DMA17_NEXT_DESC_PTR(val) bfin_writePTR(DMA17_NEXT_DESC_PTR, val)
+#define bfin_read_DMA17_START_ADDR()   bfin_readPTR(DMA17_START_ADDR)
+#define bfin_write_DMA17_START_ADDR(val) bfin_writePTR(DMA17_START_ADDR, val)
+#define bfin_read_DMA17_CONFIG()       bfin_read16(DMA17_CONFIG)
+#define bfin_write_DMA17_CONFIG(val)   bfin_write16(DMA17_CONFIG, val)
+#define bfin_read_DMA17_X_COUNT()      bfin_read16(DMA17_X_COUNT)
+#define bfin_write_DMA17_X_COUNT(val)  bfin_write16(DMA17_X_COUNT, val)
+#define bfin_read_DMA17_X_MODIFY()     bfin_read16(DMA17_X_MODIFY)
+#define bfin_write_DMA17_X_MODIFY(val) bfin_write16(DMA17_X_MODIFY, val)
+#define bfin_read_DMA17_Y_COUNT()      bfin_read16(DMA17_Y_COUNT)
+#define bfin_write_DMA17_Y_COUNT(val)  bfin_write16(DMA17_Y_COUNT, val)
+#define bfin_read_DMA17_Y_MODIFY()     bfin_read16(DMA17_Y_MODIFY)
+#define bfin_write_DMA17_Y_MODIFY(val) bfin_write16(DMA17_Y_MODIFY, val)
+#define bfin_read_DMA17_CURR_DESC_PTR() bfin_readPTR(DMA17_CURR_DESC_PTR)
+#define bfin_write_DMA17_CURR_DESC_PTR(val) bfin_writePTR(DMA17_CURR_DESC_PTR, val)
+#define bfin_read_DMA17_CURR_ADDR()    bfin_readPTR(DMA17_CURR_ADDR)
+#define bfin_write_DMA17_CURR_ADDR(val) bfin_writePTR(DMA17_CURR_ADDR, val)
+#define bfin_read_DMA17_IRQ_STATUS()   bfin_read16(DMA17_IRQ_STATUS)
+#define bfin_write_DMA17_IRQ_STATUS(val) bfin_write16(DMA17_IRQ_STATUS, val)
+#define bfin_read_DMA17_PERIPHERAL_MAP() bfin_read16(DMA17_PERIPHERAL_MAP)
+#define bfin_write_DMA17_PERIPHERAL_MAP(val) bfin_write16(DMA17_PERIPHERAL_MAP, val)
+#define bfin_read_DMA17_CURR_X_COUNT() bfin_read16(DMA17_CURR_X_COUNT)
+#define bfin_write_DMA17_CURR_X_COUNT(val) bfin_write16(DMA17_CURR_X_COUNT, val)
+#define bfin_read_DMA17_CURR_Y_COUNT() bfin_read16(DMA17_CURR_Y_COUNT)
+#define bfin_write_DMA17_CURR_Y_COUNT(val) bfin_write16(DMA17_CURR_Y_COUNT, val)
+#define bfin_read_DMA18_NEXT_DESC_PTR() bfin_readPTR(DMA18_NEXT_DESC_PTR)
+#define bfin_write_DMA18_NEXT_DESC_PTR(val) bfin_writePTR(DMA18_NEXT_DESC_PTR, val)
+#define bfin_read_DMA18_START_ADDR()   bfin_readPTR(DMA18_START_ADDR)
+#define bfin_write_DMA18_START_ADDR(val) bfin_writePTR(DMA18_START_ADDR, val)
+#define bfin_read_DMA18_CONFIG()       bfin_read16(DMA18_CONFIG)
+#define bfin_write_DMA18_CONFIG(val)   bfin_write16(DMA18_CONFIG, val)
+#define bfin_read_DMA18_X_COUNT()      bfin_read16(DMA18_X_COUNT)
+#define bfin_write_DMA18_X_COUNT(val)  bfin_write16(DMA18_X_COUNT, val)
+#define bfin_read_DMA18_X_MODIFY()     bfin_read16(DMA18_X_MODIFY)
+#define bfin_write_DMA18_X_MODIFY(val) bfin_write16(DMA18_X_MODIFY, val)
+#define bfin_read_DMA18_Y_COUNT()      bfin_read16(DMA18_Y_COUNT)
+#define bfin_write_DMA18_Y_COUNT(val)  bfin_write16(DMA18_Y_COUNT, val)
+#define bfin_read_DMA18_Y_MODIFY()     bfin_read16(DMA18_Y_MODIFY)
+#define bfin_write_DMA18_Y_MODIFY(val) bfin_write16(DMA18_Y_MODIFY, val)
+#define bfin_read_DMA18_CURR_DESC_PTR() bfin_readPTR(DMA18_CURR_DESC_PTR)
+#define bfin_write_DMA18_CURR_DESC_PTR(val) bfin_writePTR(DMA18_CURR_DESC_PTR, val)
+#define bfin_read_DMA18_CURR_ADDR()    bfin_readPTR(DMA18_CURR_ADDR)
+#define bfin_write_DMA18_CURR_ADDR(val) bfin_writePTR(DMA18_CURR_ADDR, val)
+#define bfin_read_DMA18_IRQ_STATUS()   bfin_read16(DMA18_IRQ_STATUS)
+#define bfin_write_DMA18_IRQ_STATUS(val) bfin_write16(DMA18_IRQ_STATUS, val)
+#define bfin_read_DMA18_PERIPHERAL_MAP() bfin_read16(DMA18_PERIPHERAL_MAP)
+#define bfin_write_DMA18_PERIPHERAL_MAP(val) bfin_write16(DMA18_PERIPHERAL_MAP, val)
+#define bfin_read_DMA18_CURR_X_COUNT() bfin_read16(DMA18_CURR_X_COUNT)
+#define bfin_write_DMA18_CURR_X_COUNT(val) bfin_write16(DMA18_CURR_X_COUNT, val)
+#define bfin_read_DMA18_CURR_Y_COUNT() bfin_read16(DMA18_CURR_Y_COUNT)
+#define bfin_write_DMA18_CURR_Y_COUNT(val) bfin_write16(DMA18_CURR_Y_COUNT, val)
+#define bfin_read_DMA19_NEXT_DESC_PTR() bfin_readPTR(DMA19_NEXT_DESC_PTR)
+#define bfin_write_DMA19_NEXT_DESC_PTR(val) bfin_writePTR(DMA19_NEXT_DESC_PTR, val)
+#define bfin_read_DMA19_START_ADDR()   bfin_readPTR(DMA19_START_ADDR)
+#define bfin_write_DMA19_START_ADDR(val) bfin_writePTR(DMA19_START_ADDR, val)
+#define bfin_read_DMA19_CONFIG()       bfin_read16(DMA19_CONFIG)
+#define bfin_write_DMA19_CONFIG(val)   bfin_write16(DMA19_CONFIG, val)
+#define bfin_read_DMA19_X_COUNT()      bfin_read16(DMA19_X_COUNT)
+#define bfin_write_DMA19_X_COUNT(val)  bfin_write16(DMA19_X_COUNT, val)
+#define bfin_read_DMA19_X_MODIFY()     bfin_read16(DMA19_X_MODIFY)
+#define bfin_write_DMA19_X_MODIFY(val) bfin_write16(DMA19_X_MODIFY, val)
+#define bfin_read_DMA19_Y_COUNT()      bfin_read16(DMA19_Y_COUNT)
+#define bfin_write_DMA19_Y_COUNT(val)  bfin_write16(DMA19_Y_COUNT, val)
+#define bfin_read_DMA19_Y_MODIFY()     bfin_read16(DMA19_Y_MODIFY)
+#define bfin_write_DMA19_Y_MODIFY(val) bfin_write16(DMA19_Y_MODIFY, val)
+#define bfin_read_DMA19_CURR_DESC_PTR() bfin_readPTR(DMA19_CURR_DESC_PTR)
+#define bfin_write_DMA19_CURR_DESC_PTR(val) bfin_writePTR(DMA19_CURR_DESC_PTR, val)
+#define bfin_read_DMA19_CURR_ADDR()    bfin_readPTR(DMA19_CURR_ADDR)
+#define bfin_write_DMA19_CURR_ADDR(val) bfin_writePTR(DMA19_CURR_ADDR, val)
+#define bfin_read_DMA19_IRQ_STATUS()   bfin_read16(DMA19_IRQ_STATUS)
+#define bfin_write_DMA19_IRQ_STATUS(val) bfin_write16(DMA19_IRQ_STATUS, val)
+#define bfin_read_DMA19_PERIPHERAL_MAP() bfin_read16(DMA19_PERIPHERAL_MAP)
+#define bfin_write_DMA19_PERIPHERAL_MAP(val) bfin_write16(DMA19_PERIPHERAL_MAP, val)
+#define bfin_read_DMA19_CURR_X_COUNT() bfin_read16(DMA19_CURR_X_COUNT)
+#define bfin_write_DMA19_CURR_X_COUNT(val) bfin_write16(DMA19_CURR_X_COUNT, val)
+#define bfin_read_DMA19_CURR_Y_COUNT() bfin_read16(DMA19_CURR_Y_COUNT)
+#define bfin_write_DMA19_CURR_Y_COUNT(val) bfin_write16(DMA19_CURR_Y_COUNT, val)
+#define bfin_read_MDMA0_D0_NEXT_DESC_PTR() bfin_readPTR(MDMA0_D0_NEXT_DESC_PTR)
+#define bfin_write_MDMA0_D0_NEXT_DESC_PTR(val) bfin_writePTR(MDMA0_D0_NEXT_DESC_PTR, val)
+#define bfin_read_MDMA0_D0_START_ADDR() bfin_readPTR(MDMA0_D0_START_ADDR)
+#define bfin_write_MDMA0_D0_START_ADDR(val) bfin_writePTR(MDMA0_D0_START_ADDR, val)
+#define bfin_read_MDMA0_D0_CONFIG()    bfin_read16(MDMA0_D0_CONFIG)
+#define bfin_write_MDMA0_D0_CONFIG(val) bfin_write16(MDMA0_D0_CONFIG, val)
+#define bfin_read_MDMA0_D0_X_COUNT()   bfin_read16(MDMA0_D0_X_COUNT)
+#define bfin_write_MDMA0_D0_X_COUNT(val) bfin_write16(MDMA0_D0_X_COUNT, val)
+#define bfin_read_MDMA0_D0_X_MODIFY()  bfin_read16(MDMA0_D0_X_MODIFY)
+#define bfin_write_MDMA0_D0_X_MODIFY(val) bfin_write16(MDMA0_D0_X_MODIFY, val)
+#define bfin_read_MDMA0_D0_Y_COUNT()   bfin_read16(MDMA0_D0_Y_COUNT)
+#define bfin_write_MDMA0_D0_Y_COUNT(val) bfin_write16(MDMA0_D0_Y_COUNT, val)
+#define bfin_read_MDMA0_D0_Y_MODIFY()  bfin_read16(MDMA0_D0_Y_MODIFY)
+#define bfin_write_MDMA0_D0_Y_MODIFY(val) bfin_write16(MDMA0_D0_Y_MODIFY, val)
+#define bfin_read_MDMA0_D0_CURR_DESC_PTR() bfin_readPTR(MDMA0_D0_CURR_DESC_PTR)
+#define bfin_write_MDMA0_D0_CURR_DESC_PTR(val) bfin_writePTR(MDMA0_D0_CURR_DESC_PTR, val)
+#define bfin_read_MDMA0_D0_CURR_ADDR() bfin_readPTR(MDMA0_D0_CURR_ADDR)
+#define bfin_write_MDMA0_D0_CURR_ADDR(val) bfin_writePTR(MDMA0_D0_CURR_ADDR, val)
+#define bfin_read_MDMA0_D0_IRQ_STATUS() bfin_read16(MDMA0_D0_IRQ_STATUS)
+#define bfin_write_MDMA0_D0_IRQ_STATUS(val) bfin_write16(MDMA0_D0_IRQ_STATUS, val)
+#define bfin_read_MDMA0_D0_PERIPHERAL_MAP() bfin_read16(MDMA0_D0_PERIPHERAL_MAP)
+#define bfin_write_MDMA0_D0_PERIPHERAL_MAP(val) bfin_write16(MDMA0_D0_PERIPHERAL_MAP, val)
+#define bfin_read_MDMA0_D0_CURR_X_COUNT() bfin_read16(MDMA0_D0_CURR_X_COUNT)
+#define bfin_write_MDMA0_D0_CURR_X_COUNT(val) bfin_write16(MDMA0_D0_CURR_X_COUNT, val)
+#define bfin_read_MDMA0_D0_CURR_Y_COUNT() bfin_read16(MDMA0_D0_CURR_Y_COUNT)
+#define bfin_write_MDMA0_D0_CURR_Y_COUNT(val) bfin_write16(MDMA0_D0_CURR_Y_COUNT, val)
+#define bfin_read_MDMA0_S0_NEXT_DESC_PTR() bfin_readPTR(MDMA0_S0_NEXT_DESC_PTR)
+#define bfin_write_MDMA0_S0_NEXT_DESC_PTR(val) bfin_writePTR(MDMA0_S0_NEXT_DESC_PTR, val)
+#define bfin_read_MDMA0_S0_START_ADDR() bfin_readPTR(MDMA0_S0_START_ADDR)
+#define bfin_write_MDMA0_S0_START_ADDR(val) bfin_writePTR(MDMA0_S0_START_ADDR, val)
+#define bfin_read_MDMA0_S0_CONFIG()    bfin_read16(MDMA0_S0_CONFIG)
+#define bfin_write_MDMA0_S0_CONFIG(val) bfin_write16(MDMA0_S0_CONFIG, val)
+#define bfin_read_MDMA0_S0_X_COUNT()   bfin_read16(MDMA0_S0_X_COUNT)
+#define bfin_write_MDMA0_S0_X_COUNT(val) bfin_write16(MDMA0_S0_X_COUNT, val)
+#define bfin_read_MDMA0_S0_X_MODIFY()  bfin_read16(MDMA0_S0_X_MODIFY)
+#define bfin_write_MDMA0_S0_X_MODIFY(val) bfin_write16(MDMA0_S0_X_MODIFY, val)
+#define bfin_read_MDMA0_S0_Y_COUNT()   bfin_read16(MDMA0_S0_Y_COUNT)
+#define bfin_write_MDMA0_S0_Y_COUNT(val) bfin_write16(MDMA0_S0_Y_COUNT, val)
+#define bfin_read_MDMA0_S0_Y_MODIFY()  bfin_read16(MDMA0_S0_Y_MODIFY)
+#define bfin_write_MDMA0_S0_Y_MODIFY(val) bfin_write16(MDMA0_S0_Y_MODIFY, val)
+#define bfin_read_MDMA0_S0_CURR_DESC_PTR() bfin_readPTR(MDMA0_S0_CURR_DESC_PTR)
+#define bfin_write_MDMA0_S0_CURR_DESC_PTR(val) bfin_writePTR(MDMA0_S0_CURR_DESC_PTR, val)
+#define bfin_read_MDMA0_S0_CURR_ADDR() bfin_readPTR(MDMA0_S0_CURR_ADDR)
+#define bfin_write_MDMA0_S0_CURR_ADDR(val) bfin_writePTR(MDMA0_S0_CURR_ADDR, val)
+#define bfin_read_MDMA0_S0_IRQ_STATUS() bfin_read16(MDMA0_S0_IRQ_STATUS)
+#define bfin_write_MDMA0_S0_IRQ_STATUS(val) bfin_write16(MDMA0_S0_IRQ_STATUS, val)
+#define bfin_read_MDMA0_S0_PERIPHERAL_MAP() bfin_read16(MDMA0_S0_PERIPHERAL_MAP)
+#define bfin_write_MDMA0_S0_PERIPHERAL_MAP(val) bfin_write16(MDMA0_S0_PERIPHERAL_MAP, val)
+#define bfin_read_MDMA0_S0_CURR_X_COUNT() bfin_read16(MDMA0_S0_CURR_X_COUNT)
+#define bfin_write_MDMA0_S0_CURR_X_COUNT(val) bfin_write16(MDMA0_S0_CURR_X_COUNT, val)
+#define bfin_read_MDMA0_S0_CURR_Y_COUNT() bfin_read16(MDMA0_S0_CURR_Y_COUNT)
+#define bfin_write_MDMA0_S0_CURR_Y_COUNT(val) bfin_write16(MDMA0_S0_CURR_Y_COUNT, val)
+#define bfin_read_MDMA0_D1_NEXT_DESC_PTR() bfin_readPTR(MDMA0_D1_NEXT_DESC_PTR)
+#define bfin_write_MDMA0_D1_NEXT_DESC_PTR(val) bfin_writePTR(MDMA0_D1_NEXT_DESC_PTR, val)
+#define bfin_read_MDMA0_D1_START_ADDR() bfin_readPTR(MDMA0_D1_START_ADDR)
+#define bfin_write_MDMA0_D1_START_ADDR(val) bfin_writePTR(MDMA0_D1_START_ADDR, val)
+#define bfin_read_MDMA0_D1_CONFIG()    bfin_read16(MDMA0_D1_CONFIG)
+#define bfin_write_MDMA0_D1_CONFIG(val) bfin_write16(MDMA0_D1_CONFIG, val)
+#define bfin_read_MDMA0_D1_X_COUNT()   bfin_read16(MDMA0_D1_X_COUNT)
+#define bfin_write_MDMA0_D1_X_COUNT(val) bfin_write16(MDMA0_D1_X_COUNT, val)
+#define bfin_read_MDMA0_D1_X_MODIFY()  bfin_read16(MDMA0_D1_X_MODIFY)
+#define bfin_write_MDMA0_D1_X_MODIFY(val) bfin_write16(MDMA0_D1_X_MODIFY, val)
+#define bfin_read_MDMA0_D1_Y_COUNT()   bfin_read16(MDMA0_D1_Y_COUNT)
+#define bfin_write_MDMA0_D1_Y_COUNT(val) bfin_write16(MDMA0_D1_Y_COUNT, val)
+#define bfin_read_MDMA0_D1_Y_MODIFY()  bfin_read16(MDMA0_D1_Y_MODIFY)
+#define bfin_write_MDMA0_D1_Y_MODIFY(val) bfin_write16(MDMA0_D1_Y_MODIFY, val)
+#define bfin_read_MDMA0_D1_CURR_DESC_PTR() bfin_readPTR(MDMA0_D1_CURR_DESC_PTR)
+#define bfin_write_MDMA0_D1_CURR_DESC_PTR(val) bfin_writePTR(MDMA0_D1_CURR_DESC_PTR, val)
+#define bfin_read_MDMA0_D1_CURR_ADDR() bfin_readPTR(MDMA0_D1_CURR_ADDR)
+#define bfin_write_MDMA0_D1_CURR_ADDR(val) bfin_writePTR(MDMA0_D1_CURR_ADDR, val)
+#define bfin_read_MDMA0_D1_IRQ_STATUS() bfin_read16(MDMA0_D1_IRQ_STATUS)
+#define bfin_write_MDMA0_D1_IRQ_STATUS(val) bfin_write16(MDMA0_D1_IRQ_STATUS, val)
+#define bfin_read_MDMA0_D1_PERIPHERAL_MAP() bfin_read16(MDMA0_D1_PERIPHERAL_MAP)
+#define bfin_write_MDMA0_D1_PERIPHERAL_MAP(val) bfin_write16(MDMA0_D1_PERIPHERAL_MAP, val)
+#define bfin_read_MDMA0_D1_CURR_X_COUNT() bfin_read16(MDMA0_D1_CURR_X_COUNT)
+#define bfin_write_MDMA0_D1_CURR_X_COUNT(val) bfin_write16(MDMA0_D1_CURR_X_COUNT, val)
+#define bfin_read_MDMA0_D1_CURR_Y_COUNT() bfin_read16(MDMA0_D1_CURR_Y_COUNT)
+#define bfin_write_MDMA0_D1_CURR_Y_COUNT(val) bfin_write16(MDMA0_D1_CURR_Y_COUNT, val)
+#define bfin_read_MDMA0_S1_NEXT_DESC_PTR() bfin_readPTR(MDMA0_S1_NEXT_DESC_PTR)
+#define bfin_write_MDMA0_S1_NEXT_DESC_PTR(val) bfin_writePTR(MDMA0_S1_NEXT_DESC_PTR, val)
+#define bfin_read_MDMA0_S1_START_ADDR() bfin_readPTR(MDMA0_S1_START_ADDR)
+#define bfin_write_MDMA0_S1_START_ADDR(val) bfin_writePTR(MDMA0_S1_START_ADDR, val)
+#define bfin_read_MDMA0_S1_CONFIG()    bfin_read16(MDMA0_S1_CONFIG)
+#define bfin_write_MDMA0_S1_CONFIG(val) bfin_write16(MDMA0_S1_CONFIG, val)
+#define bfin_read_MDMA0_S1_X_COUNT()   bfin_read16(MDMA0_S1_X_COUNT)
+#define bfin_write_MDMA0_S1_X_COUNT(val) bfin_write16(MDMA0_S1_X_COUNT, val)
+#define bfin_read_MDMA0_S1_X_MODIFY()  bfin_read16(MDMA0_S1_X_MODIFY)
+#define bfin_write_MDMA0_S1_X_MODIFY(val) bfin_write16(MDMA0_S1_X_MODIFY, val)
+#define bfin_read_MDMA0_S1_Y_COUNT()   bfin_read16(MDMA0_S1_Y_COUNT)
+#define bfin_write_MDMA0_S1_Y_COUNT(val) bfin_write16(MDMA0_S1_Y_COUNT, val)
+#define bfin_read_MDMA0_S1_Y_MODIFY()  bfin_read16(MDMA0_S1_Y_MODIFY)
+#define bfin_write_MDMA0_S1_Y_MODIFY(val) bfin_write16(MDMA0_S1_Y_MODIFY, val)
+#define bfin_read_MDMA0_S1_CURR_DESC_PTR() bfin_readPTR(MDMA0_S1_CURR_DESC_PTR)
+#define bfin_write_MDMA0_S1_CURR_DESC_PTR(val) bfin_writePTR(MDMA0_S1_CURR_DESC_PTR, val)
+#define bfin_read_MDMA0_S1_CURR_ADDR() bfin_readPTR(MDMA0_S1_CURR_ADDR)
+#define bfin_write_MDMA0_S1_CURR_ADDR(val) bfin_writePTR(MDMA0_S1_CURR_ADDR, val)
+#define bfin_read_MDMA0_S1_IRQ_STATUS() bfin_read16(MDMA0_S1_IRQ_STATUS)
+#define bfin_write_MDMA0_S1_IRQ_STATUS(val) bfin_write16(MDMA0_S1_IRQ_STATUS, val)
+#define bfin_read_MDMA0_S1_PERIPHERAL_MAP() bfin_read16(MDMA0_S1_PERIPHERAL_MAP)
+#define bfin_write_MDMA0_S1_PERIPHERAL_MAP(val) bfin_write16(MDMA0_S1_PERIPHERAL_MAP, val)
+#define bfin_read_MDMA0_S1_CURR_X_COUNT() bfin_read16(MDMA0_S1_CURR_X_COUNT)
+#define bfin_write_MDMA0_S1_CURR_X_COUNT(val) bfin_write16(MDMA0_S1_CURR_X_COUNT, val)
+#define bfin_read_MDMA0_S1_CURR_Y_COUNT() bfin_read16(MDMA0_S1_CURR_Y_COUNT)
+#define bfin_write_MDMA0_S1_CURR_Y_COUNT(val) bfin_write16(MDMA0_S1_CURR_Y_COUNT, val)
+#define bfin_read_MDMA1_D0_NEXT_DESC_PTR() bfin_readPTR(MDMA1_D0_NEXT_DESC_PTR)
+#define bfin_write_MDMA1_D0_NEXT_DESC_PTR(val) bfin_writePTR(MDMA1_D0_NEXT_DESC_PTR, val)
+#define bfin_read_MDMA1_D0_START_ADDR() bfin_readPTR(MDMA1_D0_START_ADDR)
+#define bfin_write_MDMA1_D0_START_ADDR(val) bfin_writePTR(MDMA1_D0_START_ADDR, val)
+#define bfin_read_MDMA1_D0_CONFIG()    bfin_read16(MDMA1_D0_CONFIG)
+#define bfin_write_MDMA1_D0_CONFIG(val) bfin_write16(MDMA1_D0_CONFIG, val)
+#define bfin_read_MDMA1_D0_X_COUNT()   bfin_read16(MDMA1_D0_X_COUNT)
+#define bfin_write_MDMA1_D0_X_COUNT(val) bfin_write16(MDMA1_D0_X_COUNT, val)
+#define bfin_read_MDMA1_D0_X_MODIFY()  bfin_read16(MDMA1_D0_X_MODIFY)
+#define bfin_write_MDMA1_D0_X_MODIFY(val) bfin_write16(MDMA1_D0_X_MODIFY, val)
+#define bfin_read_MDMA1_D0_Y_COUNT()   bfin_read16(MDMA1_D0_Y_COUNT)
+#define bfin_write_MDMA1_D0_Y_COUNT(val) bfin_write16(MDMA1_D0_Y_COUNT, val)
+#define bfin_read_MDMA1_D0_Y_MODIFY()  bfin_read16(MDMA1_D0_Y_MODIFY)
+#define bfin_write_MDMA1_D0_Y_MODIFY(val) bfin_write16(MDMA1_D0_Y_MODIFY, val)
+#define bfin_read_MDMA1_D0_CURR_DESC_PTR() bfin_readPTR(MDMA1_D0_CURR_DESC_PTR)
+#define bfin_write_MDMA1_D0_CURR_DESC_PTR(val) bfin_writePTR(MDMA1_D0_CURR_DESC_PTR, val)
+#define bfin_read_MDMA1_D0_CURR_ADDR() bfin_readPTR(MDMA1_D0_CURR_ADDR)
+#define bfin_write_MDMA1_D0_CURR_ADDR(val) bfin_writePTR(MDMA1_D0_CURR_ADDR, val)
+#define bfin_read_MDMA1_D0_IRQ_STATUS() bfin_read16(MDMA1_D0_IRQ_STATUS)
+#define bfin_write_MDMA1_D0_IRQ_STATUS(val) bfin_write16(MDMA1_D0_IRQ_STATUS, val)
+#define bfin_read_MDMA1_D0_PERIPHERAL_MAP() bfin_read16(MDMA1_D0_PERIPHERAL_MAP)
+#define bfin_write_MDMA1_D0_PERIPHERAL_MAP(val) bfin_write16(MDMA1_D0_PERIPHERAL_MAP, val)
+#define bfin_read_MDMA1_D0_CURR_X_COUNT() bfin_read16(MDMA1_D0_CURR_X_COUNT)
+#define bfin_write_MDMA1_D0_CURR_X_COUNT(val) bfin_write16(MDMA1_D0_CURR_X_COUNT, val)
+#define bfin_read_MDMA1_D0_CURR_Y_COUNT() bfin_read16(MDMA1_D0_CURR_Y_COUNT)
+#define bfin_write_MDMA1_D0_CURR_Y_COUNT(val) bfin_write16(MDMA1_D0_CURR_Y_COUNT, val)
+#define bfin_read_MDMA1_S0_NEXT_DESC_PTR() bfin_readPTR(MDMA1_S0_NEXT_DESC_PTR)
+#define bfin_write_MDMA1_S0_NEXT_DESC_PTR(val) bfin_writePTR(MDMA1_S0_NEXT_DESC_PTR, val)
+#define bfin_read_MDMA1_S0_START_ADDR() bfin_readPTR(MDMA1_S0_START_ADDR)
+#define bfin_write_MDMA1_S0_START_ADDR(val) bfin_writePTR(MDMA1_S0_START_ADDR, val)
+#define bfin_read_MDMA1_S0_CONFIG()    bfin_read16(MDMA1_S0_CONFIG)
+#define bfin_write_MDMA1_S0_CONFIG(val) bfin_write16(MDMA1_S0_CONFIG, val)
+#define bfin_read_MDMA1_S0_X_COUNT()   bfin_read16(MDMA1_S0_X_COUNT)
+#define bfin_write_MDMA1_S0_X_COUNT(val) bfin_write16(MDMA1_S0_X_COUNT, val)
+#define bfin_read_MDMA1_S0_X_MODIFY()  bfin_read16(MDMA1_S0_X_MODIFY)
+#define bfin_write_MDMA1_S0_X_MODIFY(val) bfin_write16(MDMA1_S0_X_MODIFY, val)
+#define bfin_read_MDMA1_S0_Y_COUNT()   bfin_read16(MDMA1_S0_Y_COUNT)
+#define bfin_write_MDMA1_S0_Y_COUNT(val) bfin_write16(MDMA1_S0_Y_COUNT, val)
+#define bfin_read_MDMA1_S0_Y_MODIFY()  bfin_read16(MDMA1_S0_Y_MODIFY)
+#define bfin_write_MDMA1_S0_Y_MODIFY(val) bfin_write16(MDMA1_S0_Y_MODIFY, val)
+#define bfin_read_MDMA1_S0_CURR_DESC_PTR() bfin_readPTR(MDMA1_S0_CURR_DESC_PTR)
+#define bfin_write_MDMA1_S0_CURR_DESC_PTR(val) bfin_writePTR(MDMA1_S0_CURR_DESC_PTR, val)
+#define bfin_read_MDMA1_S0_CURR_ADDR() bfin_readPTR(MDMA1_S0_CURR_ADDR)
+#define bfin_write_MDMA1_S0_CURR_ADDR(val) bfin_writePTR(MDMA1_S0_CURR_ADDR, val)
+#define bfin_read_MDMA1_S0_IRQ_STATUS() bfin_read16(MDMA1_S0_IRQ_STATUS)
+#define bfin_write_MDMA1_S0_IRQ_STATUS(val) bfin_write16(MDMA1_S0_IRQ_STATUS, val)
+#define bfin_read_MDMA1_S0_PERIPHERAL_MAP() bfin_read16(MDMA1_S0_PERIPHERAL_MAP)
+#define bfin_write_MDMA1_S0_PERIPHERAL_MAP(val) bfin_write16(MDMA1_S0_PERIPHERAL_MAP, val)
+#define bfin_read_MDMA1_S0_CURR_X_COUNT() bfin_read16(MDMA1_S0_CURR_X_COUNT)
+#define bfin_write_MDMA1_S0_CURR_X_COUNT(val) bfin_write16(MDMA1_S0_CURR_X_COUNT, val)
+#define bfin_read_MDMA1_S0_CURR_Y_COUNT() bfin_read16(MDMA1_S0_CURR_Y_COUNT)
+#define bfin_write_MDMA1_S0_CURR_Y_COUNT(val) bfin_write16(MDMA1_S0_CURR_Y_COUNT, val)
+#define bfin_read_MDMA1_D1_NEXT_DESC_PTR() bfin_readPTR(MDMA1_D1_NEXT_DESC_PTR)
+#define bfin_write_MDMA1_D1_NEXT_DESC_PTR(val) bfin_writePTR(MDMA1_D1_NEXT_DESC_PTR, val)
+#define bfin_read_MDMA1_D1_START_ADDR() bfin_readPTR(MDMA1_D1_START_ADDR)
+#define bfin_write_MDMA1_D1_START_ADDR(val) bfin_writePTR(MDMA1_D1_START_ADDR, val)
+#define bfin_read_MDMA1_D1_CONFIG()    bfin_read16(MDMA1_D1_CONFIG)
+#define bfin_write_MDMA1_D1_CONFIG(val) bfin_write16(MDMA1_D1_CONFIG, val)
+#define bfin_read_MDMA1_D1_X_COUNT()   bfin_read16(MDMA1_D1_X_COUNT)
+#define bfin_write_MDMA1_D1_X_COUNT(val) bfin_write16(MDMA1_D1_X_COUNT, val)
+#define bfin_read_MDMA1_D1_X_MODIFY()  bfin_read16(MDMA1_D1_X_MODIFY)
+#define bfin_write_MDMA1_D1_X_MODIFY(val) bfin_write16(MDMA1_D1_X_MODIFY, val)
+#define bfin_read_MDMA1_D1_Y_COUNT()   bfin_read16(MDMA1_D1_Y_COUNT)
+#define bfin_write_MDMA1_D1_Y_COUNT(val) bfin_write16(MDMA1_D1_Y_COUNT, val)
+#define bfin_read_MDMA1_D1_Y_MODIFY()  bfin_read16(MDMA1_D1_Y_MODIFY)
+#define bfin_write_MDMA1_D1_Y_MODIFY(val) bfin_write16(MDMA1_D1_Y_MODIFY, val)
+#define bfin_read_MDMA1_D1_CURR_DESC_PTR() bfin_readPTR(MDMA1_D1_CURR_DESC_PTR)
+#define bfin_write_MDMA1_D1_CURR_DESC_PTR(val) bfin_writePTR(MDMA1_D1_CURR_DESC_PTR, val)
+#define bfin_read_MDMA1_D1_CURR_ADDR() bfin_readPTR(MDMA1_D1_CURR_ADDR)
+#define bfin_write_MDMA1_D1_CURR_ADDR(val) bfin_writePTR(MDMA1_D1_CURR_ADDR, val)
+#define bfin_read_MDMA1_D1_IRQ_STATUS() bfin_read16(MDMA1_D1_IRQ_STATUS)
+#define bfin_write_MDMA1_D1_IRQ_STATUS(val) bfin_write16(MDMA1_D1_IRQ_STATUS, val)
+#define bfin_read_MDMA1_D1_PERIPHERAL_MAP() bfin_read16(MDMA1_D1_PERIPHERAL_MAP)
+#define bfin_write_MDMA1_D1_PERIPHERAL_MAP(val) bfin_write16(MDMA1_D1_PERIPHERAL_MAP, val)
+#define bfin_read_MDMA1_D1_CURR_X_COUNT() bfin_read16(MDMA1_D1_CURR_X_COUNT)
+#define bfin_write_MDMA1_D1_CURR_X_COUNT(val) bfin_write16(MDMA1_D1_CURR_X_COUNT, val)
+#define bfin_read_MDMA1_D1_CURR_Y_COUNT() bfin_read16(MDMA1_D1_CURR_Y_COUNT)
+#define bfin_write_MDMA1_D1_CURR_Y_COUNT(val) bfin_write16(MDMA1_D1_CURR_Y_COUNT, val)
+#define bfin_read_MDMA1_S1_NEXT_DESC_PTR() bfin_readPTR(MDMA1_S1_NEXT_DESC_PTR)
+#define bfin_write_MDMA1_S1_NEXT_DESC_PTR(val) bfin_writePTR(MDMA1_S1_NEXT_DESC_PTR, val)
+#define bfin_read_MDMA1_S1_START_ADDR() bfin_readPTR(MDMA1_S1_START_ADDR)
+#define bfin_write_MDMA1_S1_START_ADDR(val) bfin_writePTR(MDMA1_S1_START_ADDR, val)
+#define bfin_read_MDMA1_S1_CONFIG()    bfin_read16(MDMA1_S1_CONFIG)
+#define bfin_write_MDMA1_S1_CONFIG(val) bfin_write16(MDMA1_S1_CONFIG, val)
+#define bfin_read_MDMA1_S1_X_COUNT()   bfin_read16(MDMA1_S1_X_COUNT)
+#define bfin_write_MDMA1_S1_X_COUNT(val) bfin_write16(MDMA1_S1_X_COUNT, val)
+#define bfin_read_MDMA1_S1_X_MODIFY()  bfin_read16(MDMA1_S1_X_MODIFY)
+#define bfin_write_MDMA1_S1_X_MODIFY(val) bfin_write16(MDMA1_S1_X_MODIFY, val)
+#define bfin_read_MDMA1_S1_Y_COUNT()   bfin_read16(MDMA1_S1_Y_COUNT)
+#define bfin_write_MDMA1_S1_Y_COUNT(val) bfin_write16(MDMA1_S1_Y_COUNT, val)
+#define bfin_read_MDMA1_S1_Y_MODIFY()  bfin_read16(MDMA1_S1_Y_MODIFY)
+#define bfin_write_MDMA1_S1_Y_MODIFY(val) bfin_write16(MDMA1_S1_Y_MODIFY, val)
+#define bfin_read_MDMA1_S1_CURR_DESC_PTR() bfin_readPTR(MDMA1_S1_CURR_DESC_PTR)
+#define bfin_write_MDMA1_S1_CURR_DESC_PTR(val) bfin_writePTR(MDMA1_S1_CURR_DESC_PTR, val)
+#define bfin_read_MDMA1_S1_CURR_ADDR() bfin_readPTR(MDMA1_S1_CURR_ADDR)
+#define bfin_write_MDMA1_S1_CURR_ADDR(val) bfin_writePTR(MDMA1_S1_CURR_ADDR, val)
+#define bfin_read_MDMA1_S1_IRQ_STATUS() bfin_read16(MDMA1_S1_IRQ_STATUS)
+#define bfin_write_MDMA1_S1_IRQ_STATUS(val) bfin_write16(MDMA1_S1_IRQ_STATUS, val)
+#define bfin_read_MDMA1_S1_PERIPHERAL_MAP() bfin_read16(MDMA1_S1_PERIPHERAL_MAP)
+#define bfin_write_MDMA1_S1_PERIPHERAL_MAP(val) bfin_write16(MDMA1_S1_PERIPHERAL_MAP, val)
+#define bfin_read_MDMA1_S1_CURR_X_COUNT() bfin_read16(MDMA1_S1_CURR_X_COUNT)
+#define bfin_write_MDMA1_S1_CURR_X_COUNT(val) bfin_write16(MDMA1_S1_CURR_X_COUNT, val)
+#define bfin_read_MDMA1_S1_CURR_Y_COUNT() bfin_read16(MDMA1_S1_CURR_Y_COUNT)
+#define bfin_write_MDMA1_S1_CURR_Y_COUNT(val) bfin_write16(MDMA1_S1_CURR_Y_COUNT, val)
+#define bfin_read_PPI_CONTROL()        bfin_read16(PPI_CONTROL)
+#define bfin_write_PPI_CONTROL(val)    bfin_write16(PPI_CONTROL, val)
+#define bfin_read_PPI_STATUS()         bfin_read16(PPI_STATUS)
+#define bfin_write_PPI_STATUS(val)     bfin_write16(PPI_STATUS, val)
+#define bfin_read_PPI_DELAY()          bfin_read16(PPI_DELAY)
+#define bfin_write_PPI_DELAY(val)      bfin_write16(PPI_DELAY, val)
+#define bfin_read_PPI_COUNT()          bfin_read16(PPI_COUNT)
+#define bfin_write_PPI_COUNT(val)      bfin_write16(PPI_COUNT, val)
+#define bfin_read_PPI_FRAME()          bfin_read16(PPI_FRAME)
+#define bfin_write_PPI_FRAME(val)      bfin_write16(PPI_FRAME, val)
+#define bfin_read_TWI0_CLKDIV()        bfin_read16(TWI0_CLKDIV)
+#define bfin_write_TWI0_CLKDIV(val)    bfin_write16(TWI0_CLKDIV, val)
+#define bfin_read_TWI0_CONTROL()       bfin_read16(TWI0_CONTROL)
+#define bfin_write_TWI0_CONTROL(val)   bfin_write16(TWI0_CONTROL, val)
+#define bfin_read_TWI0_SLAVE_CTRL()    bfin_read16(TWI0_SLAVE_CTRL)
+#define bfin_write_TWI0_SLAVE_CTRL(val) bfin_write16(TWI0_SLAVE_CTRL, val)
+#define bfin_read_TWI0_SLAVE_STAT()    bfin_read16(TWI0_SLAVE_STAT)
+#define bfin_write_TWI0_SLAVE_STAT(val) bfin_write16(TWI0_SLAVE_STAT, val)
+#define bfin_read_TWI0_SLAVE_ADDR()    bfin_read16(TWI0_SLAVE_ADDR)
+#define bfin_write_TWI0_SLAVE_ADDR(val) bfin_write16(TWI0_SLAVE_ADDR, val)
+#define bfin_read_TWI0_MASTER_CTL()    bfin_read16(TWI0_MASTER_CTL)
+#define bfin_write_TWI0_MASTER_CTL(val) bfin_write16(TWI0_MASTER_CTL, val)
+#define bfin_read_TWI0_MASTER_STAT()   bfin_read16(TWI0_MASTER_STAT)
+#define bfin_write_TWI0_MASTER_STAT(val) bfin_write16(TWI0_MASTER_STAT, val)
+#define bfin_read_TWI0_MASTER_ADDR()   bfin_read16(TWI0_MASTER_ADDR)
+#define bfin_write_TWI0_MASTER_ADDR(val) bfin_write16(TWI0_MASTER_ADDR, val)
+#define bfin_read_TWI0_INT_STAT()      bfin_read16(TWI0_INT_STAT)
+#define bfin_write_TWI0_INT_STAT(val)  bfin_write16(TWI0_INT_STAT, val)
+#define bfin_read_TWI0_INT_MASK()      bfin_read16(TWI0_INT_MASK)
+#define bfin_write_TWI0_INT_MASK(val)  bfin_write16(TWI0_INT_MASK, val)
+#define bfin_read_TWI0_FIFO_CTL()      bfin_read16(TWI0_FIFO_CTL)
+#define bfin_write_TWI0_FIFO_CTL(val)  bfin_write16(TWI0_FIFO_CTL, val)
+#define bfin_read_TWI0_FIFO_STAT()     bfin_read16(TWI0_FIFO_STAT)
+#define bfin_write_TWI0_FIFO_STAT(val) bfin_write16(TWI0_FIFO_STAT, val)
+#define bfin_read_TWI0_XMT_DATA8()     bfin_read16(TWI0_XMT_DATA8)
+#define bfin_write_TWI0_XMT_DATA8(val) bfin_write16(TWI0_XMT_DATA8, val)
+#define bfin_read_TWI0_XMT_DATA16()    bfin_read16(TWI0_XMT_DATA16)
+#define bfin_write_TWI0_XMT_DATA16(val) bfin_write16(TWI0_XMT_DATA16, val)
+#define bfin_read_TWI0_RCV_DATA8()     bfin_read16(TWI0_RCV_DATA8)
+#define bfin_write_TWI0_RCV_DATA8(val) bfin_write16(TWI0_RCV_DATA8, val)
+#define bfin_read_TWI0_RCV_DATA16()    bfin_read16(TWI0_RCV_DATA16)
+#define bfin_write_TWI0_RCV_DATA16(val) bfin_write16(TWI0_RCV_DATA16, val)
+#define bfin_read_TWI1_CLKDIV()        bfin_read16(TWI1_CLKDIV)
+#define bfin_write_TWI1_CLKDIV(val)    bfin_write16(TWI1_CLKDIV, val)
+#define bfin_read_TWI1_CONTROL()       bfin_read16(TWI1_CONTROL)
+#define bfin_write_TWI1_CONTROL(val)   bfin_write16(TWI1_CONTROL, val)
+#define bfin_read_TWI1_SLAVE_CTRL()    bfin_read16(TWI1_SLAVE_CTRL)
+#define bfin_write_TWI1_SLAVE_CTRL(val) bfin_write16(TWI1_SLAVE_CTRL, val)
+#define bfin_read_TWI1_SLAVE_STAT()    bfin_read16(TWI1_SLAVE_STAT)
+#define bfin_write_TWI1_SLAVE_STAT(val) bfin_write16(TWI1_SLAVE_STAT, val)
+#define bfin_read_TWI1_SLAVE_ADDR()    bfin_read16(TWI1_SLAVE_ADDR)
+#define bfin_write_TWI1_SLAVE_ADDR(val) bfin_write16(TWI1_SLAVE_ADDR, val)
+#define bfin_read_TWI1_MASTER_CTL()    bfin_read16(TWI1_MASTER_CTL)
+#define bfin_write_TWI1_MASTER_CTL(val) bfin_write16(TWI1_MASTER_CTL, val)
+#define bfin_read_TWI1_MASTER_STAT()   bfin_read16(TWI1_MASTER_STAT)
+#define bfin_write_TWI1_MASTER_STAT(val) bfin_write16(TWI1_MASTER_STAT, val)
+#define bfin_read_TWI1_MASTER_ADDR()   bfin_read16(TWI1_MASTER_ADDR)
+#define bfin_write_TWI1_MASTER_ADDR(val) bfin_write16(TWI1_MASTER_ADDR, val)
+#define bfin_read_TWI1_INT_STAT()      bfin_read16(TWI1_INT_STAT)
+#define bfin_write_TWI1_INT_STAT(val)  bfin_write16(TWI1_INT_STAT, val)
+#define bfin_read_TWI1_INT_MASK()      bfin_read16(TWI1_INT_MASK)
+#define bfin_write_TWI1_INT_MASK(val)  bfin_write16(TWI1_INT_MASK, val)
+#define bfin_read_TWI1_FIFO_CTL()      bfin_read16(TWI1_FIFO_CTL)
+#define bfin_write_TWI1_FIFO_CTL(val)  bfin_write16(TWI1_FIFO_CTL, val)
+#define bfin_read_TWI1_FIFO_STAT()     bfin_read16(TWI1_FIFO_STAT)
+#define bfin_write_TWI1_FIFO_STAT(val) bfin_write16(TWI1_FIFO_STAT, val)
+#define bfin_read_TWI1_XMT_DATA8()     bfin_read16(TWI1_XMT_DATA8)
+#define bfin_write_TWI1_XMT_DATA8(val) bfin_write16(TWI1_XMT_DATA8, val)
+#define bfin_read_TWI1_XMT_DATA16()    bfin_read16(TWI1_XMT_DATA16)
+#define bfin_write_TWI1_XMT_DATA16(val) bfin_write16(TWI1_XMT_DATA16, val)
+#define bfin_read_TWI1_RCV_DATA8()     bfin_read16(TWI1_RCV_DATA8)
+#define bfin_write_TWI1_RCV_DATA8(val) bfin_write16(TWI1_RCV_DATA8, val)
+#define bfin_read_TWI1_RCV_DATA16()    bfin_read16(TWI1_RCV_DATA16)
+#define bfin_write_TWI1_RCV_DATA16(val) bfin_write16(TWI1_RCV_DATA16, val)
+#define bfin_read_CAN_MC1()            bfin_read16(CAN_MC1)
+#define bfin_write_CAN_MC1(val)        bfin_write16(CAN_MC1, val)
+#define bfin_read_CAN_MD1()            bfin_read16(CAN_MD1)
+#define bfin_write_CAN_MD1(val)        bfin_write16(CAN_MD1, val)
+#define bfin_read_CAN_TRS1()           bfin_read16(CAN_TRS1)
+#define bfin_write_CAN_TRS1(val)       bfin_write16(CAN_TRS1, val)
+#define bfin_read_CAN_TRR1()           bfin_read16(CAN_TRR1)
+#define bfin_write_CAN_TRR1(val)       bfin_write16(CAN_TRR1, val)
+#define bfin_read_CAN_TA1()            bfin_read16(CAN_TA1)
+#define bfin_write_CAN_TA1(val)        bfin_write16(CAN_TA1, val)
+#define bfin_read_CAN_AA1()            bfin_read16(CAN_AA1)
+#define bfin_write_CAN_AA1(val)        bfin_write16(CAN_AA1, val)
+#define bfin_read_CAN_RMP1()           bfin_read16(CAN_RMP1)
+#define bfin_write_CAN_RMP1(val)       bfin_write16(CAN_RMP1, val)
+#define bfin_read_CAN_RML1()           bfin_read16(CAN_RML1)
+#define bfin_write_CAN_RML1(val)       bfin_write16(CAN_RML1, val)
+#define bfin_read_CAN_MBTIF1()         bfin_read16(CAN_MBTIF1)
+#define bfin_write_CAN_MBTIF1(val)     bfin_write16(CAN_MBTIF1, val)
+#define bfin_read_CAN_MBRIF1()         bfin_read16(CAN_MBRIF1)
+#define bfin_write_CAN_MBRIF1(val)     bfin_write16(CAN_MBRIF1, val)
+#define bfin_read_CAN_MBIM1()          bfin_read16(CAN_MBIM1)
+#define bfin_write_CAN_MBIM1(val)      bfin_write16(CAN_MBIM1, val)
+#define bfin_read_CAN_RFH1()           bfin_read16(CAN_RFH1)
+#define bfin_write_CAN_RFH1(val)       bfin_write16(CAN_RFH1, val)
+#define bfin_read_CAN_OPSS1()          bfin_read16(CAN_OPSS1)
+#define bfin_write_CAN_OPSS1(val)      bfin_write16(CAN_OPSS1, val)
+#define bfin_read_CAN_MC2()            bfin_read16(CAN_MC2)
+#define bfin_write_CAN_MC2(val)        bfin_write16(CAN_MC2, val)
+#define bfin_read_CAN_MD2()            bfin_read16(CAN_MD2)
+#define bfin_write_CAN_MD2(val)        bfin_write16(CAN_MD2, val)
+#define bfin_read_CAN_TRS2()           bfin_read16(CAN_TRS2)
+#define bfin_write_CAN_TRS2(val)       bfin_write16(CAN_TRS2, val)
+#define bfin_read_CAN_TRR2()           bfin_read16(CAN_TRR2)
+#define bfin_write_CAN_TRR2(val)       bfin_write16(CAN_TRR2, val)
+#define bfin_read_CAN_TA2()            bfin_read16(CAN_TA2)
+#define bfin_write_CAN_TA2(val)        bfin_write16(CAN_TA2, val)
+#define bfin_read_CAN_AA2()            bfin_read16(CAN_AA2)
+#define bfin_write_CAN_AA2(val)        bfin_write16(CAN_AA2, val)
+#define bfin_read_CAN_RMP2()           bfin_read16(CAN_RMP2)
+#define bfin_write_CAN_RMP2(val)       bfin_write16(CAN_RMP2, val)
+#define bfin_read_CAN_RML2()           bfin_read16(CAN_RML2)
+#define bfin_write_CAN_RML2(val)       bfin_write16(CAN_RML2, val)
+#define bfin_read_CAN_MBTIF2()         bfin_read16(CAN_MBTIF2)
+#define bfin_write_CAN_MBTIF2(val)     bfin_write16(CAN_MBTIF2, val)
+#define bfin_read_CAN_MBRIF2()         bfin_read16(CAN_MBRIF2)
+#define bfin_write_CAN_MBRIF2(val)     bfin_write16(CAN_MBRIF2, val)
+#define bfin_read_CAN_MBIM2()          bfin_read16(CAN_MBIM2)
+#define bfin_write_CAN_MBIM2(val)      bfin_write16(CAN_MBIM2, val)
+#define bfin_read_CAN_RFH2()           bfin_read16(CAN_RFH2)
+#define bfin_write_CAN_RFH2(val)       bfin_write16(CAN_RFH2, val)
+#define bfin_read_CAN_OPSS2()          bfin_read16(CAN_OPSS2)
+#define bfin_write_CAN_OPSS2(val)      bfin_write16(CAN_OPSS2, val)
+#define bfin_read_CAN_CLOCK()          bfin_read16(CAN_CLOCK)
+#define bfin_write_CAN_CLOCK(val)      bfin_write16(CAN_CLOCK, val)
+#define bfin_read_CAN_TIMING()         bfin_read16(CAN_TIMING)
+#define bfin_write_CAN_TIMING(val)     bfin_write16(CAN_TIMING, val)
+#define bfin_read_CAN_DEBUG()          bfin_read16(CAN_DEBUG)
+#define bfin_write_CAN_DEBUG(val)      bfin_write16(CAN_DEBUG, val)
+#define bfin_read_CAN_STATUS()         bfin_read16(CAN_STATUS)
+#define bfin_write_CAN_STATUS(val)     bfin_write16(CAN_STATUS, val)
+#define bfin_read_CAN_CEC()            bfin_read16(CAN_CEC)
+#define bfin_write_CAN_CEC(val)        bfin_write16(CAN_CEC, val)
+#define bfin_read_CAN_GIS()            bfin_read16(CAN_GIS)
+#define bfin_write_CAN_GIS(val)        bfin_write16(CAN_GIS, val)
+#define bfin_read_CAN_GIM()            bfin_read16(CAN_GIM)
+#define bfin_write_CAN_GIM(val)        bfin_write16(CAN_GIM, val)
+#define bfin_read_CAN_GIF()            bfin_read16(CAN_GIF)
+#define bfin_write_CAN_GIF(val)        bfin_write16(CAN_GIF, val)
+#define bfin_read_CAN_CONTROL()        bfin_read16(CAN_CONTROL)
+#define bfin_write_CAN_CONTROL(val)    bfin_write16(CAN_CONTROL, val)
+#define bfin_read_CAN_INTR()           bfin_read16(CAN_INTR)
+#define bfin_write_CAN_INTR(val)       bfin_write16(CAN_INTR, val)
+#define bfin_read_CAN_VERSION()        bfin_read16(CAN_VERSION)
+#define bfin_write_CAN_VERSION(val)    bfin_write16(CAN_VERSION, val)
+#define bfin_read_CAN_MBTD()           bfin_read16(CAN_MBTD)
+#define bfin_write_CAN_MBTD(val)       bfin_write16(CAN_MBTD, val)
+#define bfin_read_CAN_EWR()            bfin_read16(CAN_EWR)
+#define bfin_write_CAN_EWR(val)        bfin_write16(CAN_EWR, val)
+#define bfin_read_CAN_ESR()            bfin_read16(CAN_ESR)
+#define bfin_write_CAN_ESR(val)        bfin_write16(CAN_ESR, val)
+#define bfin_read_CAN_UCREG()          bfin_read16(CAN_UCREG)
+#define bfin_write_CAN_UCREG(val)      bfin_write16(CAN_UCREG, val)
+#define bfin_read_CAN_UCCNT()          bfin_read16(CAN_UCCNT)
+#define bfin_write_CAN_UCCNT(val)      bfin_write16(CAN_UCCNT, val)
+#define bfin_read_CAN_UCRC()           bfin_read16(CAN_UCRC)
+#define bfin_write_CAN_UCRC(val)       bfin_write16(CAN_UCRC, val)
+#define bfin_read_CAN_UCCNF()          bfin_read16(CAN_UCCNF)
+#define bfin_write_CAN_UCCNF(val)      bfin_write16(CAN_UCCNF, val)
+#define bfin_read_CAN_VERSION2()       bfin_read16(CAN_VERSION2)
+#define bfin_write_CAN_VERSION2(val)   bfin_write16(CAN_VERSION2, val)
+#define bfin_read_CAN_AM00L()          bfin_read16(CAN_AM00L)
+#define bfin_write_CAN_AM00L(val)      bfin_write16(CAN_AM00L, val)
+#define bfin_read_CAN_AM00H()          bfin_read16(CAN_AM00H)
+#define bfin_write_CAN_AM00H(val)      bfin_write16(CAN_AM00H, val)
+#define bfin_read_CAN_AM01L()          bfin_read16(CAN_AM01L)
+#define bfin_write_CAN_AM01L(val)      bfin_write16(CAN_AM01L, val)
+#define bfin_read_CAN_AM01H()          bfin_read16(CAN_AM01H)
+#define bfin_write_CAN_AM01H(val)      bfin_write16(CAN_AM01H, val)
+#define bfin_read_CAN_AM02L()          bfin_read16(CAN_AM02L)
+#define bfin_write_CAN_AM02L(val)      bfin_write16(CAN_AM02L, val)
+#define bfin_read_CAN_AM02H()          bfin_read16(CAN_AM02H)
+#define bfin_write_CAN_AM02H(val)      bfin_write16(CAN_AM02H, val)
+#define bfin_read_CAN_AM03L()          bfin_read16(CAN_AM03L)
+#define bfin_write_CAN_AM03L(val)      bfin_write16(CAN_AM03L, val)
+#define bfin_read_CAN_AM03H()          bfin_read16(CAN_AM03H)
+#define bfin_write_CAN_AM03H(val)      bfin_write16(CAN_AM03H, val)
+#define bfin_read_CAN_AM04L()          bfin_read16(CAN_AM04L)
+#define bfin_write_CAN_AM04L(val)      bfin_write16(CAN_AM04L, val)
+#define bfin_read_CAN_AM04H()          bfin_read16(CAN_AM04H)
+#define bfin_write_CAN_AM04H(val)      bfin_write16(CAN_AM04H, val)
+#define bfin_read_CAN_AM05L()          bfin_read16(CAN_AM05L)
+#define bfin_write_CAN_AM05L(val)      bfin_write16(CAN_AM05L, val)
+#define bfin_read_CAN_AM05H()          bfin_read16(CAN_AM05H)
+#define bfin_write_CAN_AM05H(val)      bfin_write16(CAN_AM05H, val)
+#define bfin_read_CAN_AM06L()          bfin_read16(CAN_AM06L)
+#define bfin_write_CAN_AM06L(val)      bfin_write16(CAN_AM06L, val)
+#define bfin_read_CAN_AM06H()          bfin_read16(CAN_AM06H)
+#define bfin_write_CAN_AM06H(val)      bfin_write16(CAN_AM06H, val)
+#define bfin_read_CAN_AM07L()          bfin_read16(CAN_AM07L)
+#define bfin_write_CAN_AM07L(val)      bfin_write16(CAN_AM07L, val)
+#define bfin_read_CAN_AM07H()          bfin_read16(CAN_AM07H)
+#define bfin_write_CAN_AM07H(val)      bfin_write16(CAN_AM07H, val)
+#define bfin_read_CAN_AM08L()          bfin_read16(CAN_AM08L)
+#define bfin_write_CAN_AM08L(val)      bfin_write16(CAN_AM08L, val)
+#define bfin_read_CAN_AM08H()          bfin_read16(CAN_AM08H)
+#define bfin_write_CAN_AM08H(val)      bfin_write16(CAN_AM08H, val)
+#define bfin_read_CAN_AM09L()          bfin_read16(CAN_AM09L)
+#define bfin_write_CAN_AM09L(val)      bfin_write16(CAN_AM09L, val)
+#define bfin_read_CAN_AM09H()          bfin_read16(CAN_AM09H)
+#define bfin_write_CAN_AM09H(val)      bfin_write16(CAN_AM09H, val)
+#define bfin_read_CAN_AM10L()          bfin_read16(CAN_AM10L)
+#define bfin_write_CAN_AM10L(val)      bfin_write16(CAN_AM10L, val)
+#define bfin_read_CAN_AM10H()          bfin_read16(CAN_AM10H)
+#define bfin_write_CAN_AM10H(val)      bfin_write16(CAN_AM10H, val)
+#define bfin_read_CAN_AM11L()          bfin_read16(CAN_AM11L)
+#define bfin_write_CAN_AM11L(val)      bfin_write16(CAN_AM11L, val)
+#define bfin_read_CAN_AM11H()          bfin_read16(CAN_AM11H)
+#define bfin_write_CAN_AM11H(val)      bfin_write16(CAN_AM11H, val)
+#define bfin_read_CAN_AM12L()          bfin_read16(CAN_AM12L)
+#define bfin_write_CAN_AM12L(val)      bfin_write16(CAN_AM12L, val)
+#define bfin_read_CAN_AM12H()          bfin_read16(CAN_AM12H)
+#define bfin_write_CAN_AM12H(val)      bfin_write16(CAN_AM12H, val)
+#define bfin_read_CAN_AM13L()          bfin_read16(CAN_AM13L)
+#define bfin_write_CAN_AM13L(val)      bfin_write16(CAN_AM13L, val)
+#define bfin_read_CAN_AM13H()          bfin_read16(CAN_AM13H)
+#define bfin_write_CAN_AM13H(val)      bfin_write16(CAN_AM13H, val)
+#define bfin_read_CAN_AM14L()          bfin_read16(CAN_AM14L)
+#define bfin_write_CAN_AM14L(val)      bfin_write16(CAN_AM14L, val)
+#define bfin_read_CAN_AM14H()          bfin_read16(CAN_AM14H)
+#define bfin_write_CAN_AM14H(val)      bfin_write16(CAN_AM14H, val)
+#define bfin_read_CAN_AM15L()          bfin_read16(CAN_AM15L)
+#define bfin_write_CAN_AM15L(val)      bfin_write16(CAN_AM15L, val)
+#define bfin_read_CAN_AM15H()          bfin_read16(CAN_AM15H)
+#define bfin_write_CAN_AM15H(val)      bfin_write16(CAN_AM15H, val)
+#define bfin_read_CAN_AM16L()          bfin_read16(CAN_AM16L)
+#define bfin_write_CAN_AM16L(val)      bfin_write16(CAN_AM16L, val)
+#define bfin_read_CAN_AM16H()          bfin_read16(CAN_AM16H)
+#define bfin_write_CAN_AM16H(val)      bfin_write16(CAN_AM16H, val)
+#define bfin_read_CAN_AM17L()          bfin_read16(CAN_AM17L)
+#define bfin_write_CAN_AM17L(val)      bfin_write16(CAN_AM17L, val)
+#define bfin_read_CAN_AM17H()          bfin_read16(CAN_AM17H)
+#define bfin_write_CAN_AM17H(val)      bfin_write16(CAN_AM17H, val)
+#define bfin_read_CAN_AM18L()          bfin_read16(CAN_AM18L)
+#define bfin_write_CAN_AM18L(val)      bfin_write16(CAN_AM18L, val)
+#define bfin_read_CAN_AM18H()          bfin_read16(CAN_AM18H)
+#define bfin_write_CAN_AM18H(val)      bfin_write16(CAN_AM18H, val)
+#define bfin_read_CAN_AM19L()          bfin_read16(CAN_AM19L)
+#define bfin_write_CAN_AM19L(val)      bfin_write16(CAN_AM19L, val)
+#define bfin_read_CAN_AM19H()          bfin_read16(CAN_AM19H)
+#define bfin_write_CAN_AM19H(val)      bfin_write16(CAN_AM19H, val)
+#define bfin_read_CAN_AM20L()          bfin_read16(CAN_AM20L)
+#define bfin_write_CAN_AM20L(val)      bfin_write16(CAN_AM20L, val)
+#define bfin_read_CAN_AM20H()          bfin_read16(CAN_AM20H)
+#define bfin_write_CAN_AM20H(val)      bfin_write16(CAN_AM20H, val)
+#define bfin_read_CAN_AM21L()          bfin_read16(CAN_AM21L)
+#define bfin_write_CAN_AM21L(val)      bfin_write16(CAN_AM21L, val)
+#define bfin_read_CAN_AM21H()          bfin_read16(CAN_AM21H)
+#define bfin_write_CAN_AM21H(val)      bfin_write16(CAN_AM21H, val)
+#define bfin_read_CAN_AM22L()          bfin_read16(CAN_AM22L)
+#define bfin_write_CAN_AM22L(val)      bfin_write16(CAN_AM22L, val)
+#define bfin_read_CAN_AM22H()          bfin_read16(CAN_AM22H)
+#define bfin_write_CAN_AM22H(val)      bfin_write16(CAN_AM22H, val)
+#define bfin_read_CAN_AM23L()          bfin_read16(CAN_AM23L)
+#define bfin_write_CAN_AM23L(val)      bfin_write16(CAN_AM23L, val)
+#define bfin_read_CAN_AM23H()          bfin_read16(CAN_AM23H)
+#define bfin_write_CAN_AM23H(val)      bfin_write16(CAN_AM23H, val)
+#define bfin_read_CAN_AM24L()          bfin_read16(CAN_AM24L)
+#define bfin_write_CAN_AM24L(val)      bfin_write16(CAN_AM24L, val)
+#define bfin_read_CAN_AM24H()          bfin_read16(CAN_AM24H)
+#define bfin_write_CAN_AM24H(val)      bfin_write16(CAN_AM24H, val)
+#define bfin_read_CAN_AM25L()          bfin_read16(CAN_AM25L)
+#define bfin_write_CAN_AM25L(val)      bfin_write16(CAN_AM25L, val)
+#define bfin_read_CAN_AM25H()          bfin_read16(CAN_AM25H)
+#define bfin_write_CAN_AM25H(val)      bfin_write16(CAN_AM25H, val)
+#define bfin_read_CAN_AM26L()          bfin_read16(CAN_AM26L)
+#define bfin_write_CAN_AM26L(val)      bfin_write16(CAN_AM26L, val)
+#define bfin_read_CAN_AM26H()          bfin_read16(CAN_AM26H)
+#define bfin_write_CAN_AM26H(val)      bfin_write16(CAN_AM26H, val)
+#define bfin_read_CAN_AM27L()          bfin_read16(CAN_AM27L)
+#define bfin_write_CAN_AM27L(val)      bfin_write16(CAN_AM27L, val)
+#define bfin_read_CAN_AM27H()          bfin_read16(CAN_AM27H)
+#define bfin_write_CAN_AM27H(val)      bfin_write16(CAN_AM27H, val)
+#define bfin_read_CAN_AM28L()          bfin_read16(CAN_AM28L)
+#define bfin_write_CAN_AM28L(val)      bfin_write16(CAN_AM28L, val)
+#define bfin_read_CAN_AM28H()          bfin_read16(CAN_AM28H)
+#define bfin_write_CAN_AM28H(val)      bfin_write16(CAN_AM28H, val)
+#define bfin_read_CAN_AM29L()          bfin_read16(CAN_AM29L)
+#define bfin_write_CAN_AM29L(val)      bfin_write16(CAN_AM29L, val)
+#define bfin_read_CAN_AM29H()          bfin_read16(CAN_AM29H)
+#define bfin_write_CAN_AM29H(val)      bfin_write16(CAN_AM29H, val)
+#define bfin_read_CAN_AM30L()          bfin_read16(CAN_AM30L)
+#define bfin_write_CAN_AM30L(val)      bfin_write16(CAN_AM30L, val)
+#define bfin_read_CAN_AM30H()          bfin_read16(CAN_AM30H)
+#define bfin_write_CAN_AM30H(val)      bfin_write16(CAN_AM30H, val)
+#define bfin_read_CAN_AM31L()          bfin_read16(CAN_AM31L)
+#define bfin_write_CAN_AM31L(val)      bfin_write16(CAN_AM31L, val)
+#define bfin_read_CAN_AM31H()          bfin_read16(CAN_AM31H)
+#define bfin_write_CAN_AM31H(val)      bfin_write16(CAN_AM31H, val)
+#define bfin_read_CAN_MB00_DATA0()     bfin_read16(CAN_MB00_DATA0)
+#define bfin_write_CAN_MB00_DATA0(val) bfin_write16(CAN_MB00_DATA0, val)
+#define bfin_read_CAN_MB00_DATA1()     bfin_read16(CAN_MB00_DATA1)
+#define bfin_write_CAN_MB00_DATA1(val) bfin_write16(CAN_MB00_DATA1, val)
+#define bfin_read_CAN_MB00_DATA2()     bfin_read16(CAN_MB00_DATA2)
+#define bfin_write_CAN_MB00_DATA2(val) bfin_write16(CAN_MB00_DATA2, val)
+#define bfin_read_CAN_MB00_DATA3()     bfin_read16(CAN_MB00_DATA3)
+#define bfin_write_CAN_MB00_DATA3(val) bfin_write16(CAN_MB00_DATA3, val)
+#define bfin_read_CAN_MB00_LENGTH()    bfin_read16(CAN_MB00_LENGTH)
+#define bfin_write_CAN_MB00_LENGTH(val) bfin_write16(CAN_MB00_LENGTH, val)
+#define bfin_read_CAN_MB00_TIMESTAMP() bfin_read16(CAN_MB00_TIMESTAMP)
+#define bfin_write_CAN_MB00_TIMESTAMP(val) bfin_write16(CAN_MB00_TIMESTAMP, val)
+#define bfin_read_CAN_MB00_ID0()       bfin_read16(CAN_MB00_ID0)
+#define bfin_write_CAN_MB00_ID0(val)   bfin_write16(CAN_MB00_ID0, val)
+#define bfin_read_CAN_MB00_ID1()       bfin_read16(CAN_MB00_ID1)
+#define bfin_write_CAN_MB00_ID1(val)   bfin_write16(CAN_MB00_ID1, val)
+#define bfin_read_CAN_MB01_DATA0()     bfin_read16(CAN_MB01_DATA0)
+#define bfin_write_CAN_MB01_DATA0(val) bfin_write16(CAN_MB01_DATA0, val)
+#define bfin_read_CAN_MB01_DATA1()     bfin_read16(CAN_MB01_DATA1)
+#define bfin_write_CAN_MB01_DATA1(val) bfin_write16(CAN_MB01_DATA1, val)
+#define bfin_read_CAN_MB01_DATA2()     bfin_read16(CAN_MB01_DATA2)
+#define bfin_write_CAN_MB01_DATA2(val) bfin_write16(CAN_MB01_DATA2, val)
+#define bfin_read_CAN_MB01_DATA3()     bfin_read16(CAN_MB01_DATA3)
+#define bfin_write_CAN_MB01_DATA3(val) bfin_write16(CAN_MB01_DATA3, val)
+#define bfin_read_CAN_MB01_LENGTH()    bfin_read16(CAN_MB01_LENGTH)
+#define bfin_write_CAN_MB01_LENGTH(val) bfin_write16(CAN_MB01_LENGTH, val)
+#define bfin_read_CAN_MB01_TIMESTAMP() bfin_read16(CAN_MB01_TIMESTAMP)
+#define bfin_write_CAN_MB01_TIMESTAMP(val) bfin_write16(CAN_MB01_TIMESTAMP, val)
+#define bfin_read_CAN_MB01_ID0()       bfin_read16(CAN_MB01_ID0)
+#define bfin_write_CAN_MB01_ID0(val)   bfin_write16(CAN_MB01_ID0, val)
+#define bfin_read_CAN_MB01_ID1()       bfin_read16(CAN_MB01_ID1)
+#define bfin_write_CAN_MB01_ID1(val)   bfin_write16(CAN_MB01_ID1, val)
+#define bfin_read_CAN_MB02_DATA0()     bfin_read16(CAN_MB02_DATA0)
+#define bfin_write_CAN_MB02_DATA0(val) bfin_write16(CAN_MB02_DATA0, val)
+#define bfin_read_CAN_MB02_DATA1()     bfin_read16(CAN_MB02_DATA1)
+#define bfin_write_CAN_MB02_DATA1(val) bfin_write16(CAN_MB02_DATA1, val)
+#define bfin_read_CAN_MB02_DATA2()     bfin_read16(CAN_MB02_DATA2)
+#define bfin_write_CAN_MB02_DATA2(val) bfin_write16(CAN_MB02_DATA2, val)
+#define bfin_read_CAN_MB02_DATA3()     bfin_read16(CAN_MB02_DATA3)
+#define bfin_write_CAN_MB02_DATA3(val) bfin_write16(CAN_MB02_DATA3, val)
+#define bfin_read_CAN_MB02_LENGTH()    bfin_read16(CAN_MB02_LENGTH)
+#define bfin_write_CAN_MB02_LENGTH(val) bfin_write16(CAN_MB02_LENGTH, val)
+#define bfin_read_CAN_MB02_TIMESTAMP() bfin_read16(CAN_MB02_TIMESTAMP)
+#define bfin_write_CAN_MB02_TIMESTAMP(val) bfin_write16(CAN_MB02_TIMESTAMP, val)
+#define bfin_read_CAN_MB02_ID0()       bfin_read16(CAN_MB02_ID0)
+#define bfin_write_CAN_MB02_ID0(val)   bfin_write16(CAN_MB02_ID0, val)
+#define bfin_read_CAN_MB02_ID1()       bfin_read16(CAN_MB02_ID1)
+#define bfin_write_CAN_MB02_ID1(val)   bfin_write16(CAN_MB02_ID1, val)
+#define bfin_read_CAN_MB03_DATA0()     bfin_read16(CAN_MB03_DATA0)
+#define bfin_write_CAN_MB03_DATA0(val) bfin_write16(CAN_MB03_DATA0, val)
+#define bfin_read_CAN_MB03_DATA1()     bfin_read16(CAN_MB03_DATA1)
+#define bfin_write_CAN_MB03_DATA1(val) bfin_write16(CAN_MB03_DATA1, val)
+#define bfin_read_CAN_MB03_DATA2()     bfin_read16(CAN_MB03_DATA2)
+#define bfin_write_CAN_MB03_DATA2(val) bfin_write16(CAN_MB03_DATA2, val)
+#define bfin_read_CAN_MB03_DATA3()     bfin_read16(CAN_MB03_DATA3)
+#define bfin_write_CAN_MB03_DATA3(val) bfin_write16(CAN_MB03_DATA3, val)
+#define bfin_read_CAN_MB03_LENGTH()    bfin_read16(CAN_MB03_LENGTH)
+#define bfin_write_CAN_MB03_LENGTH(val) bfin_write16(CAN_MB03_LENGTH, val)
+#define bfin_read_CAN_MB03_TIMESTAMP() bfin_read16(CAN_MB03_TIMESTAMP)
+#define bfin_write_CAN_MB03_TIMESTAMP(val) bfin_write16(CAN_MB03_TIMESTAMP, val)
+#define bfin_read_CAN_MB03_ID0()       bfin_read16(CAN_MB03_ID0)
+#define bfin_write_CAN_MB03_ID0(val)   bfin_write16(CAN_MB03_ID0, val)
+#define bfin_read_CAN_MB03_ID1()       bfin_read16(CAN_MB03_ID1)
+#define bfin_write_CAN_MB03_ID1(val)   bfin_write16(CAN_MB03_ID1, val)
+#define bfin_read_CAN_MB04_DATA0()     bfin_read16(CAN_MB04_DATA0)
+#define bfin_write_CAN_MB04_DATA0(val) bfin_write16(CAN_MB04_DATA0, val)
+#define bfin_read_CAN_MB04_DATA1()     bfin_read16(CAN_MB04_DATA1)
+#define bfin_write_CAN_MB04_DATA1(val) bfin_write16(CAN_MB04_DATA1, val)
+#define bfin_read_CAN_MB04_DATA2()     bfin_read16(CAN_MB04_DATA2)
+#define bfin_write_CAN_MB04_DATA2(val) bfin_write16(CAN_MB04_DATA2, val)
+#define bfin_read_CAN_MB04_DATA3()     bfin_read16(CAN_MB04_DATA3)
+#define bfin_write_CAN_MB04_DATA3(val) bfin_write16(CAN_MB04_DATA3, val)
+#define bfin_read_CAN_MB04_LENGTH()    bfin_read16(CAN_MB04_LENGTH)
+#define bfin_write_CAN_MB04_LENGTH(val) bfin_write16(CAN_MB04_LENGTH, val)
+#define bfin_read_CAN_MB04_TIMESTAMP() bfin_read16(CAN_MB04_TIMESTAMP)
+#define bfin_write_CAN_MB04_TIMESTAMP(val) bfin_write16(CAN_MB04_TIMESTAMP, val)
+#define bfin_read_CAN_MB04_ID0()       bfin_read16(CAN_MB04_ID0)
+#define bfin_write_CAN_MB04_ID0(val)   bfin_write16(CAN_MB04_ID0, val)
+#define bfin_read_CAN_MB04_ID1()       bfin_read16(CAN_MB04_ID1)
+#define bfin_write_CAN_MB04_ID1(val)   bfin_write16(CAN_MB04_ID1, val)
+#define bfin_read_CAN_MB05_DATA0()     bfin_read16(CAN_MB05_DATA0)
+#define bfin_write_CAN_MB05_DATA0(val) bfin_write16(CAN_MB05_DATA0, val)
+#define bfin_read_CAN_MB05_DATA1()     bfin_read16(CAN_MB05_DATA1)
+#define bfin_write_CAN_MB05_DATA1(val) bfin_write16(CAN_MB05_DATA1, val)
+#define bfin_read_CAN_MB05_DATA2()     bfin_read16(CAN_MB05_DATA2)
+#define bfin_write_CAN_MB05_DATA2(val) bfin_write16(CAN_MB05_DATA2, val)
+#define bfin_read_CAN_MB05_DATA3()     bfin_read16(CAN_MB05_DATA3)
+#define bfin_write_CAN_MB05_DATA3(val) bfin_write16(CAN_MB05_DATA3, val)
+#define bfin_read_CAN_MB05_LENGTH()    bfin_read16(CAN_MB05_LENGTH)
+#define bfin_write_CAN_MB05_LENGTH(val) bfin_write16(CAN_MB05_LENGTH, val)
+#define bfin_read_CAN_MB05_TIMESTAMP() bfin_read16(CAN_MB05_TIMESTAMP)
+#define bfin_write_CAN_MB05_TIMESTAMP(val) bfin_write16(CAN_MB05_TIMESTAMP, val)
+#define bfin_read_CAN_MB05_ID0()       bfin_read16(CAN_MB05_ID0)
+#define bfin_write_CAN_MB05_ID0(val)   bfin_write16(CAN_MB05_ID0, val)
+#define bfin_read_CAN_MB05_ID1()       bfin_read16(CAN_MB05_ID1)
+#define bfin_write_CAN_MB05_ID1(val)   bfin_write16(CAN_MB05_ID1, val)
+#define bfin_read_CAN_MB06_DATA0()     bfin_read16(CAN_MB06_DATA0)
+#define bfin_write_CAN_MB06_DATA0(val) bfin_write16(CAN_MB06_DATA0, val)
+#define bfin_read_CAN_MB06_DATA1()     bfin_read16(CAN_MB06_DATA1)
+#define bfin_write_CAN_MB06_DATA1(val) bfin_write16(CAN_MB06_DATA1, val)
+#define bfin_read_CAN_MB06_DATA2()     bfin_read16(CAN_MB06_DATA2)
+#define bfin_write_CAN_MB06_DATA2(val) bfin_write16(CAN_MB06_DATA2, val)
+#define bfin_read_CAN_MB06_DATA3()     bfin_read16(CAN_MB06_DATA3)
+#define bfin_write_CAN_MB06_DATA3(val) bfin_write16(CAN_MB06_DATA3, val)
+#define bfin_read_CAN_MB06_LENGTH()    bfin_read16(CAN_MB06_LENGTH)
+#define bfin_write_CAN_MB06_LENGTH(val) bfin_write16(CAN_MB06_LENGTH, val)
+#define bfin_read_CAN_MB06_TIMESTAMP() bfin_read16(CAN_MB06_TIMESTAMP)
+#define bfin_write_CAN_MB06_TIMESTAMP(val) bfin_write16(CAN_MB06_TIMESTAMP, val)
+#define bfin_read_CAN_MB06_ID0()       bfin_read16(CAN_MB06_ID0)
+#define bfin_write_CAN_MB06_ID0(val)   bfin_write16(CAN_MB06_ID0, val)
+#define bfin_read_CAN_MB06_ID1()       bfin_read16(CAN_MB06_ID1)
+#define bfin_write_CAN_MB06_ID1(val)   bfin_write16(CAN_MB06_ID1, val)
+#define bfin_read_CAN_MB07_DATA0()     bfin_read16(CAN_MB07_DATA0)
+#define bfin_write_CAN_MB07_DATA0(val) bfin_write16(CAN_MB07_DATA0, val)
+#define bfin_read_CAN_MB07_DATA1()     bfin_read16(CAN_MB07_DATA1)
+#define bfin_write_CAN_MB07_DATA1(val) bfin_write16(CAN_MB07_DATA1, val)
+#define bfin_read_CAN_MB07_DATA2()     bfin_read16(CAN_MB07_DATA2)
+#define bfin_write_CAN_MB07_DATA2(val) bfin_write16(CAN_MB07_DATA2, val)
+#define bfin_read_CAN_MB07_DATA3()     bfin_read16(CAN_MB07_DATA3)
+#define bfin_write_CAN_MB07_DATA3(val) bfin_write16(CAN_MB07_DATA3, val)
+#define bfin_read_CAN_MB07_LENGTH()    bfin_read16(CAN_MB07_LENGTH)
+#define bfin_write_CAN_MB07_LENGTH(val) bfin_write16(CAN_MB07_LENGTH, val)
+#define bfin_read_CAN_MB07_TIMESTAMP() bfin_read16(CAN_MB07_TIMESTAMP)
+#define bfin_write_CAN_MB07_TIMESTAMP(val) bfin_write16(CAN_MB07_TIMESTAMP, val)
+#define bfin_read_CAN_MB07_ID0()       bfin_read16(CAN_MB07_ID0)
+#define bfin_write_CAN_MB07_ID0(val)   bfin_write16(CAN_MB07_ID0, val)
+#define bfin_read_CAN_MB07_ID1()       bfin_read16(CAN_MB07_ID1)
+#define bfin_write_CAN_MB07_ID1(val)   bfin_write16(CAN_MB07_ID1, val)
+#define bfin_read_CAN_MB08_DATA0()     bfin_read16(CAN_MB08_DATA0)
+#define bfin_write_CAN_MB08_DATA0(val) bfin_write16(CAN_MB08_DATA0, val)
+#define bfin_read_CAN_MB08_DATA1()     bfin_read16(CAN_MB08_DATA1)
+#define bfin_write_CAN_MB08_DATA1(val) bfin_write16(CAN_MB08_DATA1, val)
+#define bfin_read_CAN_MB08_DATA2()     bfin_read16(CAN_MB08_DATA2)
+#define bfin_write_CAN_MB08_DATA2(val) bfin_write16(CAN_MB08_DATA2, val)
+#define bfin_read_CAN_MB08_DATA3()     bfin_read16(CAN_MB08_DATA3)
+#define bfin_write_CAN_MB08_DATA3(val) bfin_write16(CAN_MB08_DATA3, val)
+#define bfin_read_CAN_MB08_LENGTH()    bfin_read16(CAN_MB08_LENGTH)
+#define bfin_write_CAN_MB08_LENGTH(val) bfin_write16(CAN_MB08_LENGTH, val)
+#define bfin_read_CAN_MB08_TIMESTAMP() bfin_read16(CAN_MB08_TIMESTAMP)
+#define bfin_write_CAN_MB08_TIMESTAMP(val) bfin_write16(CAN_MB08_TIMESTAMP, val)
+#define bfin_read_CAN_MB08_ID0()       bfin_read16(CAN_MB08_ID0)
+#define bfin_write_CAN_MB08_ID0(val)   bfin_write16(CAN_MB08_ID0, val)
+#define bfin_read_CAN_MB08_ID1()       bfin_read16(CAN_MB08_ID1)
+#define bfin_write_CAN_MB08_ID1(val)   bfin_write16(CAN_MB08_ID1, val)
+#define bfin_read_CAN_MB09_DATA0()     bfin_read16(CAN_MB09_DATA0)
+#define bfin_write_CAN_MB09_DATA0(val) bfin_write16(CAN_MB09_DATA0, val)
+#define bfin_read_CAN_MB09_DATA1()     bfin_read16(CAN_MB09_DATA1)
+#define bfin_write_CAN_MB09_DATA1(val) bfin_write16(CAN_MB09_DATA1, val)
+#define bfin_read_CAN_MB09_DATA2()     bfin_read16(CAN_MB09_DATA2)
+#define bfin_write_CAN_MB09_DATA2(val) bfin_write16(CAN_MB09_DATA2, val)
+#define bfin_read_CAN_MB09_DATA3()     bfin_read16(CAN_MB09_DATA3)
+#define bfin_write_CAN_MB09_DATA3(val) bfin_write16(CAN_MB09_DATA3, val)
+#define bfin_read_CAN_MB09_LENGTH()    bfin_read16(CAN_MB09_LENGTH)
+#define bfin_write_CAN_MB09_LENGTH(val) bfin_write16(CAN_MB09_LENGTH, val)
+#define bfin_read_CAN_MB09_TIMESTAMP() bfin_read16(CAN_MB09_TIMESTAMP)
+#define bfin_write_CAN_MB09_TIMESTAMP(val) bfin_write16(CAN_MB09_TIMESTAMP, val)
+#define bfin_read_CAN_MB09_ID0()       bfin_read16(CAN_MB09_ID0)
+#define bfin_write_CAN_MB09_ID0(val)   bfin_write16(CAN_MB09_ID0, val)
+#define bfin_read_CAN_MB09_ID1()       bfin_read16(CAN_MB09_ID1)
+#define bfin_write_CAN_MB09_ID1(val)   bfin_write16(CAN_MB09_ID1, val)
+#define bfin_read_CAN_MB10_DATA0()     bfin_read16(CAN_MB10_DATA0)
+#define bfin_write_CAN_MB10_DATA0(val) bfin_write16(CAN_MB10_DATA0, val)
+#define bfin_read_CAN_MB10_DATA1()     bfin_read16(CAN_MB10_DATA1)
+#define bfin_write_CAN_MB10_DATA1(val) bfin_write16(CAN_MB10_DATA1, val)
+#define bfin_read_CAN_MB10_DATA2()     bfin_read16(CAN_MB10_DATA2)
+#define bfin_write_CAN_MB10_DATA2(val) bfin_write16(CAN_MB10_DATA2, val)
+#define bfin_read_CAN_MB10_DATA3()     bfin_read16(CAN_MB10_DATA3)
+#define bfin_write_CAN_MB10_DATA3(val) bfin_write16(CAN_MB10_DATA3, val)
+#define bfin_read_CAN_MB10_LENGTH()    bfin_read16(CAN_MB10_LENGTH)
+#define bfin_write_CAN_MB10_LENGTH(val) bfin_write16(CAN_MB10_LENGTH, val)
+#define bfin_read_CAN_MB10_TIMESTAMP() bfin_read16(CAN_MB10_TIMESTAMP)
+#define bfin_write_CAN_MB10_TIMESTAMP(val) bfin_write16(CAN_MB10_TIMESTAMP, val)
+#define bfin_read_CAN_MB10_ID0()       bfin_read16(CAN_MB10_ID0)
+#define bfin_write_CAN_MB10_ID0(val)   bfin_write16(CAN_MB10_ID0, val)
+#define bfin_read_CAN_MB10_ID1()       bfin_read16(CAN_MB10_ID1)
+#define bfin_write_CAN_MB10_ID1(val)   bfin_write16(CAN_MB10_ID1, val)
+#define bfin_read_CAN_MB11_DATA0()     bfin_read16(CAN_MB11_DATA0)
+#define bfin_write_CAN_MB11_DATA0(val) bfin_write16(CAN_MB11_DATA0, val)
+#define bfin_read_CAN_MB11_DATA1()     bfin_read16(CAN_MB11_DATA1)
+#define bfin_write_CAN_MB11_DATA1(val) bfin_write16(CAN_MB11_DATA1, val)
+#define bfin_read_CAN_MB11_DATA2()     bfin_read16(CAN_MB11_DATA2)
+#define bfin_write_CAN_MB11_DATA2(val) bfin_write16(CAN_MB11_DATA2, val)
+#define bfin_read_CAN_MB11_DATA3()     bfin_read16(CAN_MB11_DATA3)
+#define bfin_write_CAN_MB11_DATA3(val) bfin_write16(CAN_MB11_DATA3, val)
+#define bfin_read_CAN_MB11_LENGTH()    bfin_read16(CAN_MB11_LENGTH)
+#define bfin_write_CAN_MB11_LENGTH(val) bfin_write16(CAN_MB11_LENGTH, val)
+#define bfin_read_CAN_MB11_TIMESTAMP() bfin_read16(CAN_MB11_TIMESTAMP)
+#define bfin_write_CAN_MB11_TIMESTAMP(val) bfin_write16(CAN_MB11_TIMESTAMP, val)
+#define bfin_read_CAN_MB11_ID0()       bfin_read16(CAN_MB11_ID0)
+#define bfin_write_CAN_MB11_ID0(val)   bfin_write16(CAN_MB11_ID0, val)
+#define bfin_read_CAN_MB11_ID1()       bfin_read16(CAN_MB11_ID1)
+#define bfin_write_CAN_MB11_ID1(val)   bfin_write16(CAN_MB11_ID1, val)
+#define bfin_read_CAN_MB12_DATA0()     bfin_read16(CAN_MB12_DATA0)
+#define bfin_write_CAN_MB12_DATA0(val) bfin_write16(CAN_MB12_DATA0, val)
+#define bfin_read_CAN_MB12_DATA1()     bfin_read16(CAN_MB12_DATA1)
+#define bfin_write_CAN_MB12_DATA1(val) bfin_write16(CAN_MB12_DATA1, val)
+#define bfin_read_CAN_MB12_DATA2()     bfin_read16(CAN_MB12_DATA2)
+#define bfin_write_CAN_MB12_DATA2(val) bfin_write16(CAN_MB12_DATA2, val)
+#define bfin_read_CAN_MB12_DATA3()     bfin_read16(CAN_MB12_DATA3)
+#define bfin_write_CAN_MB12_DATA3(val) bfin_write16(CAN_MB12_DATA3, val)
+#define bfin_read_CAN_MB12_LENGTH()    bfin_read16(CAN_MB12_LENGTH)
+#define bfin_write_CAN_MB12_LENGTH(val) bfin_write16(CAN_MB12_LENGTH, val)
+#define bfin_read_CAN_MB12_TIMESTAMP() bfin_read16(CAN_MB12_TIMESTAMP)
+#define bfin_write_CAN_MB12_TIMESTAMP(val) bfin_write16(CAN_MB12_TIMESTAMP, val)
+#define bfin_read_CAN_MB12_ID0()       bfin_read16(CAN_MB12_ID0)
+#define bfin_write_CAN_MB12_ID0(val)   bfin_write16(CAN_MB12_ID0, val)
+#define bfin_read_CAN_MB12_ID1()       bfin_read16(CAN_MB12_ID1)
+#define bfin_write_CAN_MB12_ID1(val)   bfin_write16(CAN_MB12_ID1, val)
+#define bfin_read_CAN_MB13_DATA0()     bfin_read16(CAN_MB13_DATA0)
+#define bfin_write_CAN_MB13_DATA0(val) bfin_write16(CAN_MB13_DATA0, val)
+#define bfin_read_CAN_MB13_DATA1()     bfin_read16(CAN_MB13_DATA1)
+#define bfin_write_CAN_MB13_DATA1(val) bfin_write16(CAN_MB13_DATA1, val)
+#define bfin_read_CAN_MB13_DATA2()     bfin_read16(CAN_MB13_DATA2)
+#define bfin_write_CAN_MB13_DATA2(val) bfin_write16(CAN_MB13_DATA2, val)
+#define bfin_read_CAN_MB13_DATA3()     bfin_read16(CAN_MB13_DATA3)
+#define bfin_write_CAN_MB13_DATA3(val) bfin_write16(CAN_MB13_DATA3, val)
+#define bfin_read_CAN_MB13_LENGTH()    bfin_read16(CAN_MB13_LENGTH)
+#define bfin_write_CAN_MB13_LENGTH(val) bfin_write16(CAN_MB13_LENGTH, val)
+#define bfin_read_CAN_MB13_TIMESTAMP() bfin_read16(CAN_MB13_TIMESTAMP)
+#define bfin_write_CAN_MB13_TIMESTAMP(val) bfin_write16(CAN_MB13_TIMESTAMP, val)
+#define bfin_read_CAN_MB13_ID0()       bfin_read16(CAN_MB13_ID0)
+#define bfin_write_CAN_MB13_ID0(val)   bfin_write16(CAN_MB13_ID0, val)
+#define bfin_read_CAN_MB13_ID1()       bfin_read16(CAN_MB13_ID1)
+#define bfin_write_CAN_MB13_ID1(val)   bfin_write16(CAN_MB13_ID1, val)
+#define bfin_read_CAN_MB14_DATA0()     bfin_read16(CAN_MB14_DATA0)
+#define bfin_write_CAN_MB14_DATA0(val) bfin_write16(CAN_MB14_DATA0, val)
+#define bfin_read_CAN_MB14_DATA1()     bfin_read16(CAN_MB14_DATA1)
+#define bfin_write_CAN_MB14_DATA1(val) bfin_write16(CAN_MB14_DATA1, val)
+#define bfin_read_CAN_MB14_DATA2()     bfin_read16(CAN_MB14_DATA2)
+#define bfin_write_CAN_MB14_DATA2(val) bfin_write16(CAN_MB14_DATA2, val)
+#define bfin_read_CAN_MB14_DATA3()     bfin_read16(CAN_MB14_DATA3)
+#define bfin_write_CAN_MB14_DATA3(val) bfin_write16(CAN_MB14_DATA3, val)
+#define bfin_read_CAN_MB14_LENGTH()    bfin_read16(CAN_MB14_LENGTH)
+#define bfin_write_CAN_MB14_LENGTH(val) bfin_write16(CAN_MB14_LENGTH, val)
+#define bfin_read_CAN_MB14_TIMESTAMP() bfin_read16(CAN_MB14_TIMESTAMP)
+#define bfin_write_CAN_MB14_TIMESTAMP(val) bfin_write16(CAN_MB14_TIMESTAMP, val)
+#define bfin_read_CAN_MB14_ID0()       bfin_read16(CAN_MB14_ID0)
+#define bfin_write_CAN_MB14_ID0(val)   bfin_write16(CAN_MB14_ID0, val)
+#define bfin_read_CAN_MB14_ID1()       bfin_read16(CAN_MB14_ID1)
+#define bfin_write_CAN_MB14_ID1(val)   bfin_write16(CAN_MB14_ID1, val)
+#define bfin_read_CAN_MB15_DATA0()     bfin_read16(CAN_MB15_DATA0)
+#define bfin_write_CAN_MB15_DATA0(val) bfin_write16(CAN_MB15_DATA0, val)
+#define bfin_read_CAN_MB15_DATA1()     bfin_read16(CAN_MB15_DATA1)
+#define bfin_write_CAN_MB15_DATA1(val) bfin_write16(CAN_MB15_DATA1, val)
+#define bfin_read_CAN_MB15_DATA2()     bfin_read16(CAN_MB15_DATA2)
+#define bfin_write_CAN_MB15_DATA2(val) bfin_write16(CAN_MB15_DATA2, val)
+#define bfin_read_CAN_MB15_DATA3()     bfin_read16(CAN_MB15_DATA3)
+#define bfin_write_CAN_MB15_DATA3(val) bfin_write16(CAN_MB15_DATA3, val)
+#define bfin_read_CAN_MB15_LENGTH()    bfin_read16(CAN_MB15_LENGTH)
+#define bfin_write_CAN_MB15_LENGTH(val) bfin_write16(CAN_MB15_LENGTH, val)
+#define bfin_read_CAN_MB15_TIMESTAMP() bfin_read16(CAN_MB15_TIMESTAMP)
+#define bfin_write_CAN_MB15_TIMESTAMP(val) bfin_write16(CAN_MB15_TIMESTAMP, val)
+#define bfin_read_CAN_MB15_ID0()       bfin_read16(CAN_MB15_ID0)
+#define bfin_write_CAN_MB15_ID0(val)   bfin_write16(CAN_MB15_ID0, val)
+#define bfin_read_CAN_MB15_ID1()       bfin_read16(CAN_MB15_ID1)
+#define bfin_write_CAN_MB15_ID1(val)   bfin_write16(CAN_MB15_ID1, val)
+#define bfin_read_CAN_MB16_DATA0()     bfin_read16(CAN_MB16_DATA0)
+#define bfin_write_CAN_MB16_DATA0(val) bfin_write16(CAN_MB16_DATA0, val)
+#define bfin_read_CAN_MB16_DATA1()     bfin_read16(CAN_MB16_DATA1)
+#define bfin_write_CAN_MB16_DATA1(val) bfin_write16(CAN_MB16_DATA1, val)
+#define bfin_read_CAN_MB16_DATA2()     bfin_read16(CAN_MB16_DATA2)
+#define bfin_write_CAN_MB16_DATA2(val) bfin_write16(CAN_MB16_DATA2, val)
+#define bfin_read_CAN_MB16_DATA3()     bfin_read16(CAN_MB16_DATA3)
+#define bfin_write_CAN_MB16_DATA3(val) bfin_write16(CAN_MB16_DATA3, val)
+#define bfin_read_CAN_MB16_LENGTH()    bfin_read16(CAN_MB16_LENGTH)
+#define bfin_write_CAN_MB16_LENGTH(val) bfin_write16(CAN_MB16_LENGTH, val)
+#define bfin_read_CAN_MB16_TIMESTAMP() bfin_read16(CAN_MB16_TIMESTAMP)
+#define bfin_write_CAN_MB16_TIMESTAMP(val) bfin_write16(CAN_MB16_TIMESTAMP, val)
+#define bfin_read_CAN_MB16_ID0()       bfin_read16(CAN_MB16_ID0)
+#define bfin_write_CAN_MB16_ID0(val)   bfin_write16(CAN_MB16_ID0, val)
+#define bfin_read_CAN_MB16_ID1()       bfin_read16(CAN_MB16_ID1)
+#define bfin_write_CAN_MB16_ID1(val)   bfin_write16(CAN_MB16_ID1, val)
+#define bfin_read_CAN_MB17_DATA0()     bfin_read16(CAN_MB17_DATA0)
+#define bfin_write_CAN_MB17_DATA0(val) bfin_write16(CAN_MB17_DATA0, val)
+#define bfin_read_CAN_MB17_DATA1()     bfin_read16(CAN_MB17_DATA1)
+#define bfin_write_CAN_MB17_DATA1(val) bfin_write16(CAN_MB17_DATA1, val)
+#define bfin_read_CAN_MB17_DATA2()     bfin_read16(CAN_MB17_DATA2)
+#define bfin_write_CAN_MB17_DATA2(val) bfin_write16(CAN_MB17_DATA2, val)
+#define bfin_read_CAN_MB17_DATA3()     bfin_read16(CAN_MB17_DATA3)
+#define bfin_write_CAN_MB17_DATA3(val) bfin_write16(CAN_MB17_DATA3, val)
+#define bfin_read_CAN_MB17_LENGTH()    bfin_read16(CAN_MB17_LENGTH)
+#define bfin_write_CAN_MB17_LENGTH(val) bfin_write16(CAN_MB17_LENGTH, val)
+#define bfin_read_CAN_MB17_TIMESTAMP() bfin_read16(CAN_MB17_TIMESTAMP)
+#define bfin_write_CAN_MB17_TIMESTAMP(val) bfin_write16(CAN_MB17_TIMESTAMP, val)
+#define bfin_read_CAN_MB17_ID0()       bfin_read16(CAN_MB17_ID0)
+#define bfin_write_CAN_MB17_ID0(val)   bfin_write16(CAN_MB17_ID0, val)
+#define bfin_read_CAN_MB17_ID1()       bfin_read16(CAN_MB17_ID1)
+#define bfin_write_CAN_MB17_ID1(val)   bfin_write16(CAN_MB17_ID1, val)
+#define bfin_read_CAN_MB18_DATA0()     bfin_read16(CAN_MB18_DATA0)
+#define bfin_write_CAN_MB18_DATA0(val) bfin_write16(CAN_MB18_DATA0, val)
+#define bfin_read_CAN_MB18_DATA1()     bfin_read16(CAN_MB18_DATA1)
+#define bfin_write_CAN_MB18_DATA1(val) bfin_write16(CAN_MB18_DATA1, val)
+#define bfin_read_CAN_MB18_DATA2()     bfin_read16(CAN_MB18_DATA2)
+#define bfin_write_CAN_MB18_DATA2(val) bfin_write16(CAN_MB18_DATA2, val)
+#define bfin_read_CAN_MB18_DATA3()     bfin_read16(CAN_MB18_DATA3)
+#define bfin_write_CAN_MB18_DATA3(val) bfin_write16(CAN_MB18_DATA3, val)
+#define bfin_read_CAN_MB18_LENGTH()    bfin_read16(CAN_MB18_LENGTH)
+#define bfin_write_CAN_MB18_LENGTH(val) bfin_write16(CAN_MB18_LENGTH, val)
+#define bfin_read_CAN_MB18_TIMESTAMP() bfin_read16(CAN_MB18_TIMESTAMP)
+#define bfin_write_CAN_MB18_TIMESTAMP(val) bfin_write16(CAN_MB18_TIMESTAMP, val)
+#define bfin_read_CAN_MB18_ID0()       bfin_read16(CAN_MB18_ID0)
+#define bfin_write_CAN_MB18_ID0(val)   bfin_write16(CAN_MB18_ID0, val)
+#define bfin_read_CAN_MB18_ID1()       bfin_read16(CAN_MB18_ID1)
+#define bfin_write_CAN_MB18_ID1(val)   bfin_write16(CAN_MB18_ID1, val)
+#define bfin_read_CAN_MB19_DATA0()     bfin_read16(CAN_MB19_DATA0)
+#define bfin_write_CAN_MB19_DATA0(val) bfin_write16(CAN_MB19_DATA0, val)
+#define bfin_read_CAN_MB19_DATA1()     bfin_read16(CAN_MB19_DATA1)
+#define bfin_write_CAN_MB19_DATA1(val) bfin_write16(CAN_MB19_DATA1, val)
+#define bfin_read_CAN_MB19_DATA2()     bfin_read16(CAN_MB19_DATA2)
+#define bfin_write_CAN_MB19_DATA2(val) bfin_write16(CAN_MB19_DATA2, val)
+#define bfin_read_CAN_MB19_DATA3()     bfin_read16(CAN_MB19_DATA3)
+#define bfin_write_CAN_MB19_DATA3(val) bfin_write16(CAN_MB19_DATA3, val)
+#define bfin_read_CAN_MB19_LENGTH()    bfin_read16(CAN_MB19_LENGTH)
+#define bfin_write_CAN_MB19_LENGTH(val) bfin_write16(CAN_MB19_LENGTH, val)
+#define bfin_read_CAN_MB19_TIMESTAMP() bfin_read16(CAN_MB19_TIMESTAMP)
+#define bfin_write_CAN_MB19_TIMESTAMP(val) bfin_write16(CAN_MB19_TIMESTAMP, val)
+#define bfin_read_CAN_MB19_ID0()       bfin_read16(CAN_MB19_ID0)
+#define bfin_write_CAN_MB19_ID0(val)   bfin_write16(CAN_MB19_ID0, val)
+#define bfin_read_CAN_MB19_ID1()       bfin_read16(CAN_MB19_ID1)
+#define bfin_write_CAN_MB19_ID1(val)   bfin_write16(CAN_MB19_ID1, val)
+#define bfin_read_CAN_MB20_DATA0()     bfin_read16(CAN_MB20_DATA0)
+#define bfin_write_CAN_MB20_DATA0(val) bfin_write16(CAN_MB20_DATA0, val)
+#define bfin_read_CAN_MB20_DATA1()     bfin_read16(CAN_MB20_DATA1)
+#define bfin_write_CAN_MB20_DATA1(val) bfin_write16(CAN_MB20_DATA1, val)
+#define bfin_read_CAN_MB20_DATA2()     bfin_read16(CAN_MB20_DATA2)
+#define bfin_write_CAN_MB20_DATA2(val) bfin_write16(CAN_MB20_DATA2, val)
+#define bfin_read_CAN_MB20_DATA3()     bfin_read16(CAN_MB20_DATA3)
+#define bfin_write_CAN_MB20_DATA3(val) bfin_write16(CAN_MB20_DATA3, val)
+#define bfin_read_CAN_MB20_LENGTH()    bfin_read16(CAN_MB20_LENGTH)
+#define bfin_write_CAN_MB20_LENGTH(val) bfin_write16(CAN_MB20_LENGTH, val)
+#define bfin_read_CAN_MB20_TIMESTAMP() bfin_read16(CAN_MB20_TIMESTAMP)
+#define bfin_write_CAN_MB20_TIMESTAMP(val) bfin_write16(CAN_MB20_TIMESTAMP, val)
+#define bfin_read_CAN_MB20_ID0()       bfin_read16(CAN_MB20_ID0)
+#define bfin_write_CAN_MB20_ID0(val)   bfin_write16(CAN_MB20_ID0, val)
+#define bfin_read_CAN_MB20_ID1()       bfin_read16(CAN_MB20_ID1)
+#define bfin_write_CAN_MB20_ID1(val)   bfin_write16(CAN_MB20_ID1, val)
+#define bfin_read_CAN_MB21_DATA0()     bfin_read16(CAN_MB21_DATA0)
+#define bfin_write_CAN_MB21_DATA0(val) bfin_write16(CAN_MB21_DATA0, val)
+#define bfin_read_CAN_MB21_DATA1()     bfin_read16(CAN_MB21_DATA1)
+#define bfin_write_CAN_MB21_DATA1(val) bfin_write16(CAN_MB21_DATA1, val)
+#define bfin_read_CAN_MB21_DATA2()     bfin_read16(CAN_MB21_DATA2)
+#define bfin_write_CAN_MB21_DATA2(val) bfin_write16(CAN_MB21_DATA2, val)
+#define bfin_read_CAN_MB21_DATA3()     bfin_read16(CAN_MB21_DATA3)
+#define bfin_write_CAN_MB21_DATA3(val) bfin_write16(CAN_MB21_DATA3, val)
+#define bfin_read_CAN_MB21_LENGTH()    bfin_read16(CAN_MB21_LENGTH)
+#define bfin_write_CAN_MB21_LENGTH(val) bfin_write16(CAN_MB21_LENGTH, val)
+#define bfin_read_CAN_MB21_TIMESTAMP() bfin_read16(CAN_MB21_TIMESTAMP)
+#define bfin_write_CAN_MB21_TIMESTAMP(val) bfin_write16(CAN_MB21_TIMESTAMP, val)
+#define bfin_read_CAN_MB21_ID0()       bfin_read16(CAN_MB21_ID0)
+#define bfin_write_CAN_MB21_ID0(val)   bfin_write16(CAN_MB21_ID0, val)
+#define bfin_read_CAN_MB21_ID1()       bfin_read16(CAN_MB21_ID1)
+#define bfin_write_CAN_MB21_ID1(val)   bfin_write16(CAN_MB21_ID1, val)
+#define bfin_read_CAN_MB22_DATA0()     bfin_read16(CAN_MB22_DATA0)
+#define bfin_write_CAN_MB22_DATA0(val) bfin_write16(CAN_MB22_DATA0, val)
+#define bfin_read_CAN_MB22_DATA1()     bfin_read16(CAN_MB22_DATA1)
+#define bfin_write_CAN_MB22_DATA1(val) bfin_write16(CAN_MB22_DATA1, val)
+#define bfin_read_CAN_MB22_DATA2()     bfin_read16(CAN_MB22_DATA2)
+#define bfin_write_CAN_MB22_DATA2(val) bfin_write16(CAN_MB22_DATA2, val)
+#define bfin_read_CAN_MB22_DATA3()     bfin_read16(CAN_MB22_DATA3)
+#define bfin_write_CAN_MB22_DATA3(val) bfin_write16(CAN_MB22_DATA3, val)
+#define bfin_read_CAN_MB22_LENGTH()    bfin_read16(CAN_MB22_LENGTH)
+#define bfin_write_CAN_MB22_LENGTH(val) bfin_write16(CAN_MB22_LENGTH, val)
+#define bfin_read_CAN_MB22_TIMESTAMP() bfin_read16(CAN_MB22_TIMESTAMP)
+#define bfin_write_CAN_MB22_TIMESTAMP(val) bfin_write16(CAN_MB22_TIMESTAMP, val)
+#define bfin_read_CAN_MB22_ID0()       bfin_read16(CAN_MB22_ID0)
+#define bfin_write_CAN_MB22_ID0(val)   bfin_write16(CAN_MB22_ID0, val)
+#define bfin_read_CAN_MB22_ID1()       bfin_read16(CAN_MB22_ID1)
+#define bfin_write_CAN_MB22_ID1(val)   bfin_write16(CAN_MB22_ID1, val)
+#define bfin_read_CAN_MB23_DATA0()     bfin_read16(CAN_MB23_DATA0)
+#define bfin_write_CAN_MB23_DATA0(val) bfin_write16(CAN_MB23_DATA0, val)
+#define bfin_read_CAN_MB23_DATA1()     bfin_read16(CAN_MB23_DATA1)
+#define bfin_write_CAN_MB23_DATA1(val) bfin_write16(CAN_MB23_DATA1, val)
+#define bfin_read_CAN_MB23_DATA2()     bfin_read16(CAN_MB23_DATA2)
+#define bfin_write_CAN_MB23_DATA2(val) bfin_write16(CAN_MB23_DATA2, val)
+#define bfin_read_CAN_MB23_DATA3()     bfin_read16(CAN_MB23_DATA3)
+#define bfin_write_CAN_MB23_DATA3(val) bfin_write16(CAN_MB23_DATA3, val)
+#define bfin_read_CAN_MB23_LENGTH()    bfin_read16(CAN_MB23_LENGTH)
+#define bfin_write_CAN_MB23_LENGTH(val) bfin_write16(CAN_MB23_LENGTH, val)
+#define bfin_read_CAN_MB23_TIMESTAMP() bfin_read16(CAN_MB23_TIMESTAMP)
+#define bfin_write_CAN_MB23_TIMESTAMP(val) bfin_write16(CAN_MB23_TIMESTAMP, val)
+#define bfin_read_CAN_MB23_ID0()       bfin_read16(CAN_MB23_ID0)
+#define bfin_write_CAN_MB23_ID0(val)   bfin_write16(CAN_MB23_ID0, val)
+#define bfin_read_CAN_MB23_ID1()       bfin_read16(CAN_MB23_ID1)
+#define bfin_write_CAN_MB23_ID1(val)   bfin_write16(CAN_MB23_ID1, val)
+#define bfin_read_CAN_MB24_DATA0()     bfin_read16(CAN_MB24_DATA0)
+#define bfin_write_CAN_MB24_DATA0(val) bfin_write16(CAN_MB24_DATA0, val)
+#define bfin_read_CAN_MB24_DATA1()     bfin_read16(CAN_MB24_DATA1)
+#define bfin_write_CAN_MB24_DATA1(val) bfin_write16(CAN_MB24_DATA1, val)
+#define bfin_read_CAN_MB24_DATA2()     bfin_read16(CAN_MB24_DATA2)
+#define bfin_write_CAN_MB24_DATA2(val) bfin_write16(CAN_MB24_DATA2, val)
+#define bfin_read_CAN_MB24_DATA3()     bfin_read16(CAN_MB24_DATA3)
+#define bfin_write_CAN_MB24_DATA3(val) bfin_write16(CAN_MB24_DATA3, val)
+#define bfin_read_CAN_MB24_LENGTH()    bfin_read16(CAN_MB24_LENGTH)
+#define bfin_write_CAN_MB24_LENGTH(val) bfin_write16(CAN_MB24_LENGTH, val)
+#define bfin_read_CAN_MB24_TIMESTAMP() bfin_read16(CAN_MB24_TIMESTAMP)
+#define bfin_write_CAN_MB24_TIMESTAMP(val) bfin_write16(CAN_MB24_TIMESTAMP, val)
+#define bfin_read_CAN_MB24_ID0()       bfin_read16(CAN_MB24_ID0)
+#define bfin_write_CAN_MB24_ID0(val)   bfin_write16(CAN_MB24_ID0, val)
+#define bfin_read_CAN_MB24_ID1()       bfin_read16(CAN_MB24_ID1)
+#define bfin_write_CAN_MB24_ID1(val)   bfin_write16(CAN_MB24_ID1, val)
+#define bfin_read_CAN_MB25_DATA0()     bfin_read16(CAN_MB25_DATA0)
+#define bfin_write_CAN_MB25_DATA0(val) bfin_write16(CAN_MB25_DATA0, val)
+#define bfin_read_CAN_MB25_DATA1()     bfin_read16(CAN_MB25_DATA1)
+#define bfin_write_CAN_MB25_DATA1(val) bfin_write16(CAN_MB25_DATA1, val)
+#define bfin_read_CAN_MB25_DATA2()     bfin_read16(CAN_MB25_DATA2)
+#define bfin_write_CAN_MB25_DATA2(val) bfin_write16(CAN_MB25_DATA2, val)
+#define bfin_read_CAN_MB25_DATA3()     bfin_read16(CAN_MB25_DATA3)
+#define bfin_write_CAN_MB25_DATA3(val) bfin_write16(CAN_MB25_DATA3, val)
+#define bfin_read_CAN_MB25_LENGTH()    bfin_read16(CAN_MB25_LENGTH)
+#define bfin_write_CAN_MB25_LENGTH(val) bfin_write16(CAN_MB25_LENGTH, val)
+#define bfin_read_CAN_MB25_TIMESTAMP() bfin_read16(CAN_MB25_TIMESTAMP)
+#define bfin_write_CAN_MB25_TIMESTAMP(val) bfin_write16(CAN_MB25_TIMESTAMP, val)
+#define bfin_read_CAN_MB25_ID0()       bfin_read16(CAN_MB25_ID0)
+#define bfin_write_CAN_MB25_ID0(val)   bfin_write16(CAN_MB25_ID0, val)
+#define bfin_read_CAN_MB25_ID1()       bfin_read16(CAN_MB25_ID1)
+#define bfin_write_CAN_MB25_ID1(val)   bfin_write16(CAN_MB25_ID1, val)
+#define bfin_read_CAN_MB26_DATA0()     bfin_read16(CAN_MB26_DATA0)
+#define bfin_write_CAN_MB26_DATA0(val) bfin_write16(CAN_MB26_DATA0, val)
+#define bfin_read_CAN_MB26_DATA1()     bfin_read16(CAN_MB26_DATA1)
+#define bfin_write_CAN_MB26_DATA1(val) bfin_write16(CAN_MB26_DATA1, val)
+#define bfin_read_CAN_MB26_DATA2()     bfin_read16(CAN_MB26_DATA2)
+#define bfin_write_CAN_MB26_DATA2(val) bfin_write16(CAN_MB26_DATA2, val)
+#define bfin_read_CAN_MB26_DATA3()     bfin_read16(CAN_MB26_DATA3)
+#define bfin_write_CAN_MB26_DATA3(val) bfin_write16(CAN_MB26_DATA3, val)
+#define bfin_read_CAN_MB26_LENGTH()    bfin_read16(CAN_MB26_LENGTH)
+#define bfin_write_CAN_MB26_LENGTH(val) bfin_write16(CAN_MB26_LENGTH, val)
+#define bfin_read_CAN_MB26_TIMESTAMP() bfin_read16(CAN_MB26_TIMESTAMP)
+#define bfin_write_CAN_MB26_TIMESTAMP(val) bfin_write16(CAN_MB26_TIMESTAMP, val)
+#define bfin_read_CAN_MB26_ID0()       bfin_read16(CAN_MB26_ID0)
+#define bfin_write_CAN_MB26_ID0(val)   bfin_write16(CAN_MB26_ID0, val)
+#define bfin_read_CAN_MB26_ID1()       bfin_read16(CAN_MB26_ID1)
+#define bfin_write_CAN_MB26_ID1(val)   bfin_write16(CAN_MB26_ID1, val)
+#define bfin_read_CAN_MB27_DATA0()     bfin_read16(CAN_MB27_DATA0)
+#define bfin_write_CAN_MB27_DATA0(val) bfin_write16(CAN_MB27_DATA0, val)
+#define bfin_read_CAN_MB27_DATA1()     bfin_read16(CAN_MB27_DATA1)
+#define bfin_write_CAN_MB27_DATA1(val) bfin_write16(CAN_MB27_DATA1, val)
+#define bfin_read_CAN_MB27_DATA2()     bfin_read16(CAN_MB27_DATA2)
+#define bfin_write_CAN_MB27_DATA2(val) bfin_write16(CAN_MB27_DATA2, val)
+#define bfin_read_CAN_MB27_DATA3()     bfin_read16(CAN_MB27_DATA3)
+#define bfin_write_CAN_MB27_DATA3(val) bfin_write16(CAN_MB27_DATA3, val)
+#define bfin_read_CAN_MB27_LENGTH()    bfin_read16(CAN_MB27_LENGTH)
+#define bfin_write_CAN_MB27_LENGTH(val) bfin_write16(CAN_MB27_LENGTH, val)
+#define bfin_read_CAN_MB27_TIMESTAMP() bfin_read16(CAN_MB27_TIMESTAMP)
+#define bfin_write_CAN_MB27_TIMESTAMP(val) bfin_write16(CAN_MB27_TIMESTAMP, val)
+#define bfin_read_CAN_MB27_ID0()       bfin_read16(CAN_MB27_ID0)
+#define bfin_write_CAN_MB27_ID0(val)   bfin_write16(CAN_MB27_ID0, val)
+#define bfin_read_CAN_MB27_ID1()       bfin_read16(CAN_MB27_ID1)
+#define bfin_write_CAN_MB27_ID1(val)   bfin_write16(CAN_MB27_ID1, val)
+#define bfin_read_CAN_MB28_DATA0()     bfin_read16(CAN_MB28_DATA0)
+#define bfin_write_CAN_MB28_DATA0(val) bfin_write16(CAN_MB28_DATA0, val)
+#define bfin_read_CAN_MB28_DATA1()     bfin_read16(CAN_MB28_DATA1)
+#define bfin_write_CAN_MB28_DATA1(val) bfin_write16(CAN_MB28_DATA1, val)
+#define bfin_read_CAN_MB28_DATA2()     bfin_read16(CAN_MB28_DATA2)
+#define bfin_write_CAN_MB28_DATA2(val) bfin_write16(CAN_MB28_DATA2, val)
+#define bfin_read_CAN_MB28_DATA3()     bfin_read16(CAN_MB28_DATA3)
+#define bfin_write_CAN_MB28_DATA3(val) bfin_write16(CAN_MB28_DATA3, val)
+#define bfin_read_CAN_MB28_LENGTH()    bfin_read16(CAN_MB28_LENGTH)
+#define bfin_write_CAN_MB28_LENGTH(val) bfin_write16(CAN_MB28_LENGTH, val)
+#define bfin_read_CAN_MB28_TIMESTAMP() bfin_read16(CAN_MB28_TIMESTAMP)
+#define bfin_write_CAN_MB28_TIMESTAMP(val) bfin_write16(CAN_MB28_TIMESTAMP, val)
+#define bfin_read_CAN_MB28_ID0()       bfin_read16(CAN_MB28_ID0)
+#define bfin_write_CAN_MB28_ID0(val)   bfin_write16(CAN_MB28_ID0, val)
+#define bfin_read_CAN_MB28_ID1()       bfin_read16(CAN_MB28_ID1)
+#define bfin_write_CAN_MB28_ID1(val)   bfin_write16(CAN_MB28_ID1, val)
+#define bfin_read_CAN_MB29_DATA0()     bfin_read16(CAN_MB29_DATA0)
+#define bfin_write_CAN_MB29_DATA0(val) bfin_write16(CAN_MB29_DATA0, val)
+#define bfin_read_CAN_MB29_DATA1()     bfin_read16(CAN_MB29_DATA1)
+#define bfin_write_CAN_MB29_DATA1(val) bfin_write16(CAN_MB29_DATA1, val)
+#define bfin_read_CAN_MB29_DATA2()     bfin_read16(CAN_MB29_DATA2)
+#define bfin_write_CAN_MB29_DATA2(val) bfin_write16(CAN_MB29_DATA2, val)
+#define bfin_read_CAN_MB29_DATA3()     bfin_read16(CAN_MB29_DATA3)
+#define bfin_write_CAN_MB29_DATA3(val) bfin_write16(CAN_MB29_DATA3, val)
+#define bfin_read_CAN_MB29_LENGTH()    bfin_read16(CAN_MB29_LENGTH)
+#define bfin_write_CAN_MB29_LENGTH(val) bfin_write16(CAN_MB29_LENGTH, val)
+#define bfin_read_CAN_MB29_TIMESTAMP() bfin_read16(CAN_MB29_TIMESTAMP)
+#define bfin_write_CAN_MB29_TIMESTAMP(val) bfin_write16(CAN_MB29_TIMESTAMP, val)
+#define bfin_read_CAN_MB29_ID0()       bfin_read16(CAN_MB29_ID0)
+#define bfin_write_CAN_MB29_ID0(val)   bfin_write16(CAN_MB29_ID0, val)
+#define bfin_read_CAN_MB29_ID1()       bfin_read16(CAN_MB29_ID1)
+#define bfin_write_CAN_MB29_ID1(val)   bfin_write16(CAN_MB29_ID1, val)
+#define bfin_read_CAN_MB30_DATA0()     bfin_read16(CAN_MB30_DATA0)
+#define bfin_write_CAN_MB30_DATA0(val) bfin_write16(CAN_MB30_DATA0, val)
+#define bfin_read_CAN_MB30_DATA1()     bfin_read16(CAN_MB30_DATA1)
+#define bfin_write_CAN_MB30_DATA1(val) bfin_write16(CAN_MB30_DATA1, val)
+#define bfin_read_CAN_MB30_DATA2()     bfin_read16(CAN_MB30_DATA2)
+#define bfin_write_CAN_MB30_DATA2(val) bfin_write16(CAN_MB30_DATA2, val)
+#define bfin_read_CAN_MB30_DATA3()     bfin_read16(CAN_MB30_DATA3)
+#define bfin_write_CAN_MB30_DATA3(val) bfin_write16(CAN_MB30_DATA3, val)
+#define bfin_read_CAN_MB30_LENGTH()    bfin_read16(CAN_MB30_LENGTH)
+#define bfin_write_CAN_MB30_LENGTH(val) bfin_write16(CAN_MB30_LENGTH, val)
+#define bfin_read_CAN_MB30_TIMESTAMP() bfin_read16(CAN_MB30_TIMESTAMP)
+#define bfin_write_CAN_MB30_TIMESTAMP(val) bfin_write16(CAN_MB30_TIMESTAMP, val)
+#define bfin_read_CAN_MB30_ID0()       bfin_read16(CAN_MB30_ID0)
+#define bfin_write_CAN_MB30_ID0(val)   bfin_write16(CAN_MB30_ID0, val)
+#define bfin_read_CAN_MB30_ID1()       bfin_read16(CAN_MB30_ID1)
+#define bfin_write_CAN_MB30_ID1(val)   bfin_write16(CAN_MB30_ID1, val)
+#define bfin_read_CAN_MB31_DATA0()     bfin_read16(CAN_MB31_DATA0)
+#define bfin_write_CAN_MB31_DATA0(val) bfin_write16(CAN_MB31_DATA0, val)
+#define bfin_read_CAN_MB31_DATA1()     bfin_read16(CAN_MB31_DATA1)
+#define bfin_write_CAN_MB31_DATA1(val) bfin_write16(CAN_MB31_DATA1, val)
+#define bfin_read_CAN_MB31_DATA2()     bfin_read16(CAN_MB31_DATA2)
+#define bfin_write_CAN_MB31_DATA2(val) bfin_write16(CAN_MB31_DATA2, val)
+#define bfin_read_CAN_MB31_DATA3()     bfin_read16(CAN_MB31_DATA3)
+#define bfin_write_CAN_MB31_DATA3(val) bfin_write16(CAN_MB31_DATA3, val)
+#define bfin_read_CAN_MB31_LENGTH()    bfin_read16(CAN_MB31_LENGTH)
+#define bfin_write_CAN_MB31_LENGTH(val) bfin_write16(CAN_MB31_LENGTH, val)
+#define bfin_read_CAN_MB31_TIMESTAMP() bfin_read16(CAN_MB31_TIMESTAMP)
+#define bfin_write_CAN_MB31_TIMESTAMP(val) bfin_write16(CAN_MB31_TIMESTAMP, val)
+#define bfin_read_CAN_MB31_ID0()       bfin_read16(CAN_MB31_ID0)
+#define bfin_write_CAN_MB31_ID0(val)   bfin_write16(CAN_MB31_ID0, val)
+#define bfin_read_CAN_MB31_ID1()       bfin_read16(CAN_MB31_ID1)
+#define bfin_write_CAN_MB31_ID1(val)   bfin_write16(CAN_MB31_ID1, val)
+
+/* These need to be last due to the cdef/linux inter-dependencies */
+#include <asm/irq.h>
+
+/* Writing to PLL_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_PLL_CTL(unsigned int val)
+{
+	unsigned long flags, iwr0, iwr1;
+
+	if (val == bfin_read_PLL_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr0 = bfin_read32(SIC_IWR0);
+	iwr1 = bfin_read32(SIC_IWR1);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SIC_IWR0, IWR_ENABLE(0));
+	bfin_write32(SIC_IWR1, 0);
+
+	bfin_write16(PLL_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SIC_IWR0, iwr0);
+	bfin_write32(SIC_IWR1, iwr1);
+	local_irq_restore_hw(flags);
+}
+
+/* Writing to VR_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_VR_CTL(unsigned int val)
+{
+	unsigned long flags, iwr0, iwr1;
+
+	if (val == bfin_read_VR_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr0 = bfin_read32(SIC_IWR0);
+	iwr1 = bfin_read32(SIC_IWR1);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SIC_IWR0, IWR_ENABLE(0));
+	bfin_write32(SIC_IWR1, 0);
+
+	bfin_write16(VR_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SIC_IWR0, iwr0);
+	bfin_write32(SIC_IWR1, iwr1);
+	local_irq_restore_hw(flags);
+}
+
+#endif
diff --git a/arch/blackfin/mach-bf538/include/mach/cdefBF539.h b/arch/blackfin/mach-bf538/include/mach/cdefBF539.h
new file mode 100644
index 000000000000..198c4bbc8e5d
--- /dev/null
+++ b/arch/blackfin/mach-bf538/include/mach/cdefBF539.h
@@ -0,0 +1,240 @@
+/* DO NOT EDIT THIS FILE
+ * Automatically generated by generate-cdef-headers.xsl
+ * DO NOT EDIT THIS FILE
+ */
+
+#ifndef _CDEF_BF539_H
+#define _CDEF_BF539_H
+
+/* Include MMRs Common to BF538 								*/
+#include "cdefBF538.h"
+
+
+#define bfin_read_MXVR_CONFIG()        bfin_read16(MXVR_CONFIG)
+#define bfin_write_MXVR_CONFIG(val)    bfin_write16(MXVR_CONFIG, val)
+#define bfin_read_MXVR_PLL_CTL_0()     bfin_read32(MXVR_PLL_CTL_0)
+#define bfin_write_MXVR_PLL_CTL_0(val) bfin_write32(MXVR_PLL_CTL_0, val)
+#define bfin_read_MXVR_STATE_0()       bfin_read32(MXVR_STATE_0)
+#define bfin_write_MXVR_STATE_0(val)   bfin_write32(MXVR_STATE_0, val)
+#define bfin_read_MXVR_STATE_1()       bfin_read32(MXVR_STATE_1)
+#define bfin_write_MXVR_STATE_1(val)   bfin_write32(MXVR_STATE_1, val)
+#define bfin_read_MXVR_INT_STAT_0()    bfin_read32(MXVR_INT_STAT_0)
+#define bfin_write_MXVR_INT_STAT_0(val) bfin_write32(MXVR_INT_STAT_0, val)
+#define bfin_read_MXVR_INT_STAT_1()    bfin_read32(MXVR_INT_STAT_1)
+#define bfin_write_MXVR_INT_STAT_1(val) bfin_write32(MXVR_INT_STAT_1, val)
+#define bfin_read_MXVR_INT_EN_0()      bfin_read32(MXVR_INT_EN_0)
+#define bfin_write_MXVR_INT_EN_0(val)  bfin_write32(MXVR_INT_EN_0, val)
+#define bfin_read_MXVR_INT_EN_1()      bfin_read32(MXVR_INT_EN_1)
+#define bfin_write_MXVR_INT_EN_1(val)  bfin_write32(MXVR_INT_EN_1, val)
+#define bfin_read_MXVR_POSITION()      bfin_read16(MXVR_POSITION)
+#define bfin_write_MXVR_POSITION(val)  bfin_write16(MXVR_POSITION, val)
+#define bfin_read_MXVR_MAX_POSITION()  bfin_read16(MXVR_MAX_POSITION)
+#define bfin_write_MXVR_MAX_POSITION(val) bfin_write16(MXVR_MAX_POSITION, val)
+#define bfin_read_MXVR_DELAY()         bfin_read16(MXVR_DELAY)
+#define bfin_write_MXVR_DELAY(val)     bfin_write16(MXVR_DELAY, val)
+#define bfin_read_MXVR_MAX_DELAY()     bfin_read16(MXVR_MAX_DELAY)
+#define bfin_write_MXVR_MAX_DELAY(val) bfin_write16(MXVR_MAX_DELAY, val)
+#define bfin_read_MXVR_LADDR()         bfin_read32(MXVR_LADDR)
+#define bfin_write_MXVR_LADDR(val)     bfin_write32(MXVR_LADDR, val)
+#define bfin_read_MXVR_GADDR()         bfin_read16(MXVR_GADDR)
+#define bfin_write_MXVR_GADDR(val)     bfin_write16(MXVR_GADDR, val)
+#define bfin_read_MXVR_AADDR()         bfin_read32(MXVR_AADDR)
+#define bfin_write_MXVR_AADDR(val)     bfin_write32(MXVR_AADDR, val)
+#define bfin_read_MXVR_ALLOC_0()       bfin_read32(MXVR_ALLOC_0)
+#define bfin_write_MXVR_ALLOC_0(val)   bfin_write32(MXVR_ALLOC_0, val)
+#define bfin_read_MXVR_ALLOC_1()       bfin_read32(MXVR_ALLOC_1)
+#define bfin_write_MXVR_ALLOC_1(val)   bfin_write32(MXVR_ALLOC_1, val)
+#define bfin_read_MXVR_ALLOC_2()       bfin_read32(MXVR_ALLOC_2)
+#define bfin_write_MXVR_ALLOC_2(val)   bfin_write32(MXVR_ALLOC_2, val)
+#define bfin_read_MXVR_ALLOC_3()       bfin_read32(MXVR_ALLOC_3)
+#define bfin_write_MXVR_ALLOC_3(val)   bfin_write32(MXVR_ALLOC_3, val)
+#define bfin_read_MXVR_ALLOC_4()       bfin_read32(MXVR_ALLOC_4)
+#define bfin_write_MXVR_ALLOC_4(val)   bfin_write32(MXVR_ALLOC_4, val)
+#define bfin_read_MXVR_ALLOC_5()       bfin_read32(MXVR_ALLOC_5)
+#define bfin_write_MXVR_ALLOC_5(val)   bfin_write32(MXVR_ALLOC_5, val)
+#define bfin_read_MXVR_ALLOC_6()       bfin_read32(MXVR_ALLOC_6)
+#define bfin_write_MXVR_ALLOC_6(val)   bfin_write32(MXVR_ALLOC_6, val)
+#define bfin_read_MXVR_ALLOC_7()       bfin_read32(MXVR_ALLOC_7)
+#define bfin_write_MXVR_ALLOC_7(val)   bfin_write32(MXVR_ALLOC_7, val)
+#define bfin_read_MXVR_ALLOC_8()       bfin_read32(MXVR_ALLOC_8)
+#define bfin_write_MXVR_ALLOC_8(val)   bfin_write32(MXVR_ALLOC_8, val)
+#define bfin_read_MXVR_ALLOC_9()       bfin_read32(MXVR_ALLOC_9)
+#define bfin_write_MXVR_ALLOC_9(val)   bfin_write32(MXVR_ALLOC_9, val)
+#define bfin_read_MXVR_ALLOC_10()      bfin_read32(MXVR_ALLOC_10)
+#define bfin_write_MXVR_ALLOC_10(val)  bfin_write32(MXVR_ALLOC_10, val)
+#define bfin_read_MXVR_ALLOC_11()      bfin_read32(MXVR_ALLOC_11)
+#define bfin_write_MXVR_ALLOC_11(val)  bfin_write32(MXVR_ALLOC_11, val)
+#define bfin_read_MXVR_ALLOC_12()      bfin_read32(MXVR_ALLOC_12)
+#define bfin_write_MXVR_ALLOC_12(val)  bfin_write32(MXVR_ALLOC_12, val)
+#define bfin_read_MXVR_ALLOC_13()      bfin_read32(MXVR_ALLOC_13)
+#define bfin_write_MXVR_ALLOC_13(val)  bfin_write32(MXVR_ALLOC_13, val)
+#define bfin_read_MXVR_ALLOC_14()      bfin_read32(MXVR_ALLOC_14)
+#define bfin_write_MXVR_ALLOC_14(val)  bfin_write32(MXVR_ALLOC_14, val)
+#define bfin_read_MXVR_SYNC_LCHAN_0()  bfin_read32(MXVR_SYNC_LCHAN_0)
+#define bfin_write_MXVR_SYNC_LCHAN_0(val) bfin_write32(MXVR_SYNC_LCHAN_0, val)
+#define bfin_read_MXVR_SYNC_LCHAN_1()  bfin_read32(MXVR_SYNC_LCHAN_1)
+#define bfin_write_MXVR_SYNC_LCHAN_1(val) bfin_write32(MXVR_SYNC_LCHAN_1, val)
+#define bfin_read_MXVR_SYNC_LCHAN_2()  bfin_read32(MXVR_SYNC_LCHAN_2)
+#define bfin_write_MXVR_SYNC_LCHAN_2(val) bfin_write32(MXVR_SYNC_LCHAN_2, val)
+#define bfin_read_MXVR_SYNC_LCHAN_3()  bfin_read32(MXVR_SYNC_LCHAN_3)
+#define bfin_write_MXVR_SYNC_LCHAN_3(val) bfin_write32(MXVR_SYNC_LCHAN_3, val)
+#define bfin_read_MXVR_SYNC_LCHAN_4()  bfin_read32(MXVR_SYNC_LCHAN_4)
+#define bfin_write_MXVR_SYNC_LCHAN_4(val) bfin_write32(MXVR_SYNC_LCHAN_4, val)
+#define bfin_read_MXVR_SYNC_LCHAN_5()  bfin_read32(MXVR_SYNC_LCHAN_5)
+#define bfin_write_MXVR_SYNC_LCHAN_5(val) bfin_write32(MXVR_SYNC_LCHAN_5, val)
+#define bfin_read_MXVR_SYNC_LCHAN_6()  bfin_read32(MXVR_SYNC_LCHAN_6)
+#define bfin_write_MXVR_SYNC_LCHAN_6(val) bfin_write32(MXVR_SYNC_LCHAN_6, val)
+#define bfin_read_MXVR_SYNC_LCHAN_7()  bfin_read32(MXVR_SYNC_LCHAN_7)
+#define bfin_write_MXVR_SYNC_LCHAN_7(val) bfin_write32(MXVR_SYNC_LCHAN_7, val)
+#define bfin_read_MXVR_DMA0_CONFIG()   bfin_read32(MXVR_DMA0_CONFIG)
+#define bfin_write_MXVR_DMA0_CONFIG(val) bfin_write32(MXVR_DMA0_CONFIG, val)
+#define bfin_read_MXVR_DMA0_START_ADDR() bfin_readPTR(MXVR_DMA0_START_ADDR)
+#define bfin_write_MXVR_DMA0_START_ADDR(val) bfin_writePTR(MXVR_DMA0_START_ADDR, val)
+#define bfin_read_MXVR_DMA0_COUNT()    bfin_read16(MXVR_DMA0_COUNT)
+#define bfin_write_MXVR_DMA0_COUNT(val) bfin_write16(MXVR_DMA0_COUNT, val)
+#define bfin_read_MXVR_DMA0_CURR_ADDR() bfin_readPTR(MXVR_DMA0_CURR_ADDR)
+#define bfin_write_MXVR_DMA0_CURR_ADDR(val) bfin_writePTR(MXVR_DMA0_CURR_ADDR, val)
+#define bfin_read_MXVR_DMA0_CURR_COUNT() bfin_read16(MXVR_DMA0_CURR_COUNT)
+#define bfin_write_MXVR_DMA0_CURR_COUNT(val) bfin_write16(MXVR_DMA0_CURR_COUNT, val)
+#define bfin_read_MXVR_DMA1_CONFIG()   bfin_read32(MXVR_DMA1_CONFIG)
+#define bfin_write_MXVR_DMA1_CONFIG(val) bfin_write32(MXVR_DMA1_CONFIG, val)
+#define bfin_read_MXVR_DMA1_START_ADDR() bfin_readPTR(MXVR_DMA1_START_ADDR)
+#define bfin_write_MXVR_DMA1_START_ADDR(val) bfin_writePTR(MXVR_DMA1_START_ADDR, val)
+#define bfin_read_MXVR_DMA1_COUNT()    bfin_read16(MXVR_DMA1_COUNT)
+#define bfin_write_MXVR_DMA1_COUNT(val) bfin_write16(MXVR_DMA1_COUNT, val)
+#define bfin_read_MXVR_DMA1_CURR_ADDR() bfin_readPTR(MXVR_DMA1_CURR_ADDR)
+#define bfin_write_MXVR_DMA1_CURR_ADDR(val) bfin_writePTR(MXVR_DMA1_CURR_ADDR, val)
+#define bfin_read_MXVR_DMA1_CURR_COUNT() bfin_read16(MXVR_DMA1_CURR_COUNT)
+#define bfin_write_MXVR_DMA1_CURR_COUNT(val) bfin_write16(MXVR_DMA1_CURR_COUNT, val)
+#define bfin_read_MXVR_DMA2_CONFIG()   bfin_read32(MXVR_DMA2_CONFIG)
+#define bfin_write_MXVR_DMA2_CONFIG(val) bfin_write32(MXVR_DMA2_CONFIG, val)
+#define bfin_read_MXVR_DMA2_START_ADDR() bfin_readPTR(MXVR_DMA2_START_ADDR)
+#define bfin_write_MXVR_DMA2_START_ADDR(val) bfin_writePTR(MXVR_DMA2_START_ADDR, val)
+#define bfin_read_MXVR_DMA2_COUNT()    bfin_read16(MXVR_DMA2_COUNT)
+#define bfin_write_MXVR_DMA2_COUNT(val) bfin_write16(MXVR_DMA2_COUNT, val)
+#define bfin_read_MXVR_DMA2_CURR_ADDR() bfin_readPTR(MXVR_DMA2_CURR_ADDR)
+#define bfin_write_MXVR_DMA2_CURR_ADDR(val) bfin_writePTR(MXVR_DMA2_CURR_ADDR, val)
+#define bfin_read_MXVR_DMA2_CURR_COUNT() bfin_read16(MXVR_DMA2_CURR_COUNT)
+#define bfin_write_MXVR_DMA2_CURR_COUNT(val) bfin_write16(MXVR_DMA2_CURR_COUNT, val)
+#define bfin_read_MXVR_DMA3_CONFIG()   bfin_read32(MXVR_DMA3_CONFIG)
+#define bfin_write_MXVR_DMA3_CONFIG(val) bfin_write32(MXVR_DMA3_CONFIG, val)
+#define bfin_read_MXVR_DMA3_START_ADDR() bfin_readPTR(MXVR_DMA3_START_ADDR)
+#define bfin_write_MXVR_DMA3_START_ADDR(val) bfin_writePTR(MXVR_DMA3_START_ADDR, val)
+#define bfin_read_MXVR_DMA3_COUNT()    bfin_read16(MXVR_DMA3_COUNT)
+#define bfin_write_MXVR_DMA3_COUNT(val) bfin_write16(MXVR_DMA3_COUNT, val)
+#define bfin_read_MXVR_DMA3_CURR_ADDR() bfin_readPTR(MXVR_DMA3_CURR_ADDR)
+#define bfin_write_MXVR_DMA3_CURR_ADDR(val) bfin_writePTR(MXVR_DMA3_CURR_ADDR, val)
+#define bfin_read_MXVR_DMA3_CURR_COUNT() bfin_read16(MXVR_DMA3_CURR_COUNT)
+#define bfin_write_MXVR_DMA3_CURR_COUNT(val) bfin_write16(MXVR_DMA3_CURR_COUNT, val)
+#define bfin_read_MXVR_DMA4_CONFIG()   bfin_read32(MXVR_DMA4_CONFIG)
+#define bfin_write_MXVR_DMA4_CONFIG(val) bfin_write32(MXVR_DMA4_CONFIG, val)
+#define bfin_read_MXVR_DMA4_START_ADDR() bfin_readPTR(MXVR_DMA4_START_ADDR)
+#define bfin_write_MXVR_DMA4_START_ADDR(val) bfin_writePTR(MXVR_DMA4_START_ADDR, val)
+#define bfin_read_MXVR_DMA4_COUNT()    bfin_read16(MXVR_DMA4_COUNT)
+#define bfin_write_MXVR_DMA4_COUNT(val) bfin_write16(MXVR_DMA4_COUNT, val)
+#define bfin_read_MXVR_DMA4_CURR_ADDR() bfin_readPTR(MXVR_DMA4_CURR_ADDR)
+#define bfin_write_MXVR_DMA4_CURR_ADDR(val) bfin_writePTR(MXVR_DMA4_CURR_ADDR, val)
+#define bfin_read_MXVR_DMA4_CURR_COUNT() bfin_read16(MXVR_DMA4_CURR_COUNT)
+#define bfin_write_MXVR_DMA4_CURR_COUNT(val) bfin_write16(MXVR_DMA4_CURR_COUNT, val)
+#define bfin_read_MXVR_DMA5_CONFIG()   bfin_read32(MXVR_DMA5_CONFIG)
+#define bfin_write_MXVR_DMA5_CONFIG(val) bfin_write32(MXVR_DMA5_CONFIG, val)
+#define bfin_read_MXVR_DMA5_START_ADDR() bfin_readPTR(MXVR_DMA5_START_ADDR)
+#define bfin_write_MXVR_DMA5_START_ADDR(val) bfin_writePTR(MXVR_DMA5_START_ADDR, val)
+#define bfin_read_MXVR_DMA5_COUNT()    bfin_read16(MXVR_DMA5_COUNT)
+#define bfin_write_MXVR_DMA5_COUNT(val) bfin_write16(MXVR_DMA5_COUNT, val)
+#define bfin_read_MXVR_DMA5_CURR_ADDR() bfin_readPTR(MXVR_DMA5_CURR_ADDR)
+#define bfin_write_MXVR_DMA5_CURR_ADDR(val) bfin_writePTR(MXVR_DMA5_CURR_ADDR, val)
+#define bfin_read_MXVR_DMA5_CURR_COUNT() bfin_read16(MXVR_DMA5_CURR_COUNT)
+#define bfin_write_MXVR_DMA5_CURR_COUNT(val) bfin_write16(MXVR_DMA5_CURR_COUNT, val)
+#define bfin_read_MXVR_DMA6_CONFIG()   bfin_read32(MXVR_DMA6_CONFIG)
+#define bfin_write_MXVR_DMA6_CONFIG(val) bfin_write32(MXVR_DMA6_CONFIG, val)
+#define bfin_read_MXVR_DMA6_START_ADDR() bfin_readPTR(MXVR_DMA6_START_ADDR)
+#define bfin_write_MXVR_DMA6_START_ADDR(val) bfin_writePTR(MXVR_DMA6_START_ADDR, val)
+#define bfin_read_MXVR_DMA6_COUNT()    bfin_read16(MXVR_DMA6_COUNT)
+#define bfin_write_MXVR_DMA6_COUNT(val) bfin_write16(MXVR_DMA6_COUNT, val)
+#define bfin_read_MXVR_DMA6_CURR_ADDR() bfin_readPTR(MXVR_DMA6_CURR_ADDR)
+#define bfin_write_MXVR_DMA6_CURR_ADDR(val) bfin_writePTR(MXVR_DMA6_CURR_ADDR, val)
+#define bfin_read_MXVR_DMA6_CURR_COUNT() bfin_read16(MXVR_DMA6_CURR_COUNT)
+#define bfin_write_MXVR_DMA6_CURR_COUNT(val) bfin_write16(MXVR_DMA6_CURR_COUNT, val)
+#define bfin_read_MXVR_DMA7_CONFIG()   bfin_read32(MXVR_DMA7_CONFIG)
+#define bfin_write_MXVR_DMA7_CONFIG(val) bfin_write32(MXVR_DMA7_CONFIG, val)
+#define bfin_read_MXVR_DMA7_START_ADDR() bfin_readPTR(MXVR_DMA7_START_ADDR)
+#define bfin_write_MXVR_DMA7_START_ADDR(val) bfin_writePTR(MXVR_DMA7_START_ADDR, val)
+#define bfin_read_MXVR_DMA7_COUNT()    bfin_read16(MXVR_DMA7_COUNT)
+#define bfin_write_MXVR_DMA7_COUNT(val) bfin_write16(MXVR_DMA7_COUNT, val)
+#define bfin_read_MXVR_DMA7_CURR_ADDR() bfin_readPTR(MXVR_DMA7_CURR_ADDR)
+#define bfin_write_MXVR_DMA7_CURR_ADDR(val) bfin_writePTR(MXVR_DMA7_CURR_ADDR, val)
+#define bfin_read_MXVR_DMA7_CURR_COUNT() bfin_read16(MXVR_DMA7_CURR_COUNT)
+#define bfin_write_MXVR_DMA7_CURR_COUNT(val) bfin_write16(MXVR_DMA7_CURR_COUNT, val)
+#define bfin_read_MXVR_AP_CTL()        bfin_read16(MXVR_AP_CTL)
+#define bfin_write_MXVR_AP_CTL(val)    bfin_write16(MXVR_AP_CTL, val)
+#define bfin_read_MXVR_APRB_START_ADDR() bfin_readPTR(MXVR_APRB_START_ADDR)
+#define bfin_write_MXVR_APRB_START_ADDR(val) bfin_writePTR(MXVR_APRB_START_ADDR, val)
+#define bfin_read_MXVR_APRB_CURR_ADDR() bfin_readPTR(MXVR_APRB_CURR_ADDR)
+#define bfin_write_MXVR_APRB_CURR_ADDR(val) bfin_writePTR(MXVR_APRB_CURR_ADDR, val)
+#define bfin_read_MXVR_APTB_START_ADDR() bfin_readPTR(MXVR_APTB_START_ADDR)
+#define bfin_write_MXVR_APTB_START_ADDR(val) bfin_writePTR(MXVR_APTB_START_ADDR, val)
+#define bfin_read_MXVR_APTB_CURR_ADDR() bfin_readPTR(MXVR_APTB_CURR_ADDR)
+#define bfin_write_MXVR_APTB_CURR_ADDR(val) bfin_writePTR(MXVR_APTB_CURR_ADDR, val)
+#define bfin_read_MXVR_CM_CTL()        bfin_read32(MXVR_CM_CTL)
+#define bfin_write_MXVR_CM_CTL(val)    bfin_write32(MXVR_CM_CTL, val)
+#define bfin_read_MXVR_CMRB_START_ADDR() bfin_readPTR(MXVR_CMRB_START_ADDR)
+#define bfin_write_MXVR_CMRB_START_ADDR(val) bfin_writePTR(MXVR_CMRB_START_ADDR, val)
+#define bfin_read_MXVR_CMRB_CURR_ADDR() bfin_readPTR(MXVR_CMRB_CURR_ADDR)
+#define bfin_write_MXVR_CMRB_CURR_ADDR(val) bfin_writePTR(MXVR_CMRB_CURR_ADDR, val)
+#define bfin_read_MXVR_CMTB_START_ADDR() bfin_readPTR(MXVR_CMTB_START_ADDR)
+#define bfin_write_MXVR_CMTB_START_ADDR(val) bfin_writePTR(MXVR_CMTB_START_ADDR, val)
+#define bfin_read_MXVR_CMTB_CURR_ADDR() bfin_readPTR(MXVR_CMTB_CURR_ADDR)
+#define bfin_write_MXVR_CMTB_CURR_ADDR(val) bfin_writePTR(MXVR_CMTB_CURR_ADDR, val)
+#define bfin_read_MXVR_RRDB_START_ADDR() bfin_readPTR(MXVR_RRDB_START_ADDR)
+#define bfin_write_MXVR_RRDB_START_ADDR(val) bfin_writePTR(MXVR_RRDB_START_ADDR, val)
+#define bfin_read_MXVR_RRDB_CURR_ADDR() bfin_readPTR(MXVR_RRDB_CURR_ADDR)
+#define bfin_write_MXVR_RRDB_CURR_ADDR(val) bfin_writePTR(MXVR_RRDB_CURR_ADDR, val)
+#define bfin_read_MXVR_PAT_DATA_0()    bfin_read32(MXVR_PAT_DATA_0)
+#define bfin_write_MXVR_PAT_DATA_0(val) bfin_write32(MXVR_PAT_DATA_0, val)
+#define bfin_read_MXVR_PAT_EN_0()      bfin_read32(MXVR_PAT_EN_0)
+#define bfin_write_MXVR_PAT_EN_0(val)  bfin_write32(MXVR_PAT_EN_0, val)
+#define bfin_read_MXVR_PAT_DATA_1()    bfin_read32(MXVR_PAT_DATA_1)
+#define bfin_write_MXVR_PAT_DATA_1(val) bfin_write32(MXVR_PAT_DATA_1, val)
+#define bfin_read_MXVR_PAT_EN_1()      bfin_read32(MXVR_PAT_EN_1)
+#define bfin_write_MXVR_PAT_EN_1(val)  bfin_write32(MXVR_PAT_EN_1, val)
+#define bfin_read_MXVR_FRAME_CNT_0()   bfin_read16(MXVR_FRAME_CNT_0)
+#define bfin_write_MXVR_FRAME_CNT_0(val) bfin_write16(MXVR_FRAME_CNT_0, val)
+#define bfin_read_MXVR_FRAME_CNT_1()   bfin_read16(MXVR_FRAME_CNT_1)
+#define bfin_write_MXVR_FRAME_CNT_1(val) bfin_write16(MXVR_FRAME_CNT_1, val)
+#define bfin_read_MXVR_ROUTING_0()     bfin_read32(MXVR_ROUTING_0)
+#define bfin_write_MXVR_ROUTING_0(val) bfin_write32(MXVR_ROUTING_0, val)
+#define bfin_read_MXVR_ROUTING_1()     bfin_read32(MXVR_ROUTING_1)
+#define bfin_write_MXVR_ROUTING_1(val) bfin_write32(MXVR_ROUTING_1, val)
+#define bfin_read_MXVR_ROUTING_2()     bfin_read32(MXVR_ROUTING_2)
+#define bfin_write_MXVR_ROUTING_2(val) bfin_write32(MXVR_ROUTING_2, val)
+#define bfin_read_MXVR_ROUTING_3()     bfin_read32(MXVR_ROUTING_3)
+#define bfin_write_MXVR_ROUTING_3(val) bfin_write32(MXVR_ROUTING_3, val)
+#define bfin_read_MXVR_ROUTING_4()     bfin_read32(MXVR_ROUTING_4)
+#define bfin_write_MXVR_ROUTING_4(val) bfin_write32(MXVR_ROUTING_4, val)
+#define bfin_read_MXVR_ROUTING_5()     bfin_read32(MXVR_ROUTING_5)
+#define bfin_write_MXVR_ROUTING_5(val) bfin_write32(MXVR_ROUTING_5, val)
+#define bfin_read_MXVR_ROUTING_6()     bfin_read32(MXVR_ROUTING_6)
+#define bfin_write_MXVR_ROUTING_6(val) bfin_write32(MXVR_ROUTING_6, val)
+#define bfin_read_MXVR_ROUTING_7()     bfin_read32(MXVR_ROUTING_7)
+#define bfin_write_MXVR_ROUTING_7(val) bfin_write32(MXVR_ROUTING_7, val)
+#define bfin_read_MXVR_ROUTING_8()     bfin_read32(MXVR_ROUTING_8)
+#define bfin_write_MXVR_ROUTING_8(val) bfin_write32(MXVR_ROUTING_8, val)
+#define bfin_read_MXVR_ROUTING_9()     bfin_read32(MXVR_ROUTING_9)
+#define bfin_write_MXVR_ROUTING_9(val) bfin_write32(MXVR_ROUTING_9, val)
+#define bfin_read_MXVR_ROUTING_10()    bfin_read32(MXVR_ROUTING_10)
+#define bfin_write_MXVR_ROUTING_10(val) bfin_write32(MXVR_ROUTING_10, val)
+#define bfin_read_MXVR_ROUTING_11()    bfin_read32(MXVR_ROUTING_11)
+#define bfin_write_MXVR_ROUTING_11(val) bfin_write32(MXVR_ROUTING_11, val)
+#define bfin_read_MXVR_ROUTING_12()    bfin_read32(MXVR_ROUTING_12)
+#define bfin_write_MXVR_ROUTING_12(val) bfin_write32(MXVR_ROUTING_12, val)
+#define bfin_read_MXVR_ROUTING_13()    bfin_read32(MXVR_ROUTING_13)
+#define bfin_write_MXVR_ROUTING_13(val) bfin_write32(MXVR_ROUTING_13, val)
+#define bfin_read_MXVR_ROUTING_14()    bfin_read32(MXVR_ROUTING_14)
+#define bfin_write_MXVR_ROUTING_14(val) bfin_write32(MXVR_ROUTING_14, val)
+#define bfin_read_MXVR_PLL_CTL_1()     bfin_read32(MXVR_PLL_CTL_1)
+#define bfin_write_MXVR_PLL_CTL_1(val) bfin_write32(MXVR_PLL_CTL_1, val)
+#define bfin_read_MXVR_BLOCK_CNT()     bfin_read16(MXVR_BLOCK_CNT)
+#define bfin_write_MXVR_BLOCK_CNT(val) bfin_write16(MXVR_BLOCK_CNT, val)
+
+#endif /* _CDEF_BF539_H */
diff --git a/arch/blackfin/mach-bf538/include/mach/defBF539.h b/arch/blackfin/mach-bf538/include/mach/defBF539.h
new file mode 100644
index 000000000000..6adbfcc65a35
--- /dev/null
+++ b/arch/blackfin/mach-bf538/include/mach/defBF539.h
@@ -0,0 +1,4243 @@
+/************************************************************************
+ *
+ * This file is subject to the terms and conditions of the GNU Public
+ * License. See the file "COPYING" in the main directory of this archive
+ * for more details.
+ *
+ * Non-GPL License also available as part of VisualDSP++
+ * http://www.analog.com/processors/resources/crosscore/visualDspDevSoftware.html
+ *
+ * (c) Copyright 2001-2005 Analog Devices, Inc. All rights reserved
+ *
+ * This file under source code control, please send bugs or changes to:
+ * dsptools.support@analog.com
+ *
+ ************************************************************************/
+/*
+ * File:         include/asm-blackfin/mach-bf538/defBF539.h
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:
+ *
+ * Rev:
+ *
+ * Modified:
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, 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; see the file COPYING.
+ * If not, write to the Free Software Foundation,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+/* SYSTEM & MM REGISTER BIT & ADDRESS DEFINITIONS FOR ADSP-BF538/9 */
+
+#ifndef _DEF_BF539_H
+#define _DEF_BF539_H
+
+/* include all Core registers and bit definitions */
+#include <asm/def_LPBlackfin.h>
+
+
+/*********************************************************************************** */
+/* System MMR Register Map */
+/*********************************************************************************** */
+/* Clock/Regulator Control (0xFFC00000 - 0xFFC000FF) */
+#define	PLL_CTL			0xFFC00000	/* PLL Control register (16-bit) */
+#define	PLL_DIV			0xFFC00004	/* PLL Divide Register (16-bit) */
+#define	VR_CTL			0xFFC00008	/* Voltage Regulator Control Register (16-bit) */
+#define	PLL_STAT		0xFFC0000C	/* PLL Status register (16-bit) */
+#define	PLL_LOCKCNT		0xFFC00010	/* PLL Lock	Count register (16-bit) */
+#define	CHIPID			0xFFC00014	/* Chip	ID Register */
+
+/* CHIPID Masks */
+#define CHIPID_VERSION         0xF0000000
+#define CHIPID_FAMILY          0x0FFFF000
+#define CHIPID_MANUFACTURE     0x00000FFE
+
+/* System Interrupt Controller (0xFFC00100 - 0xFFC001FF) */
+#define	SWRST			0xFFC00100  /* Software	Reset Register (16-bit) */
+#define	SYSCR			0xFFC00104  /* System Configuration registe */
+#define	SIC_IMASK0		0xFFC0010C  /* Interrupt Mask Register */
+#define	SIC_IAR0		0xFFC00110  /* Interrupt Assignment Register 0 */
+#define	SIC_IAR1		0xFFC00114  /* Interrupt Assignment Register 1 */
+#define	SIC_IAR2		0xFFC00118  /* Interrupt Assignment Register 2 */
+#define	SIC_IAR3			0xFFC0011C	/* Interrupt Assignment	Register 3 */
+#define	SIC_ISR0			0xFFC00120  /* Interrupt Status	Register */
+#define	SIC_IWR0			0xFFC00124  /* Interrupt Wakeup	Register */
+#define	SIC_IMASK1			0xFFC00128	/* Interrupt Mask Register 1 */
+#define	SIC_ISR1			0xFFC0012C	/* Interrupt Status Register 1 */
+#define	SIC_IWR1			0xFFC00130	/* Interrupt Wakeup Register 1 */
+#define	SIC_IAR4			0xFFC00134	/* Interrupt Assignment	Register 4 */
+#define	SIC_IAR5			0xFFC00138	/* Interrupt Assignment	Register 5 */
+#define	SIC_IAR6			0xFFC0013C	/* Interrupt Assignment	Register 6 */
+
+
+/* Watchdog Timer (0xFFC00200 -	0xFFC002FF) */
+#define	WDOG_CTL	0xFFC00200  /* Watchdog	Control	Register */
+#define	WDOG_CNT	0xFFC00204  /* Watchdog	Count Register */
+#define	WDOG_STAT	0xFFC00208  /* Watchdog	Status Register */
+
+
+/* Real	Time Clock (0xFFC00300 - 0xFFC003FF) */
+#define	RTC_STAT	0xFFC00300  /* RTC Status Register */
+#define	RTC_ICTL	0xFFC00304  /* RTC Interrupt Control Register */
+#define	RTC_ISTAT	0xFFC00308  /* RTC Interrupt Status Register */
+#define	RTC_SWCNT	0xFFC0030C  /* RTC Stopwatch Count Register */
+#define	RTC_ALARM	0xFFC00310  /* RTC Alarm Time Register */
+#define	RTC_FAST	0xFFC00314  /* RTC Prescaler Enable Register */
+#define	RTC_PREN		0xFFC00314  /* RTC Prescaler Enable Register (alternate	macro) */
+
+
+/* UART0 Controller (0xFFC00400	- 0xFFC004FF) */
+#define	UART0_THR	      0xFFC00400  /* Transmit Holding register */
+#define	UART0_RBR	      0xFFC00400  /* Receive Buffer register */
+#define	UART0_DLL	      0xFFC00400  /* Divisor Latch (Low-Byte) */
+#define	UART0_IER	      0xFFC00404  /* Interrupt Enable Register */
+#define	UART0_DLH	      0xFFC00404  /* Divisor Latch (High-Byte) */
+#define	UART0_IIR	      0xFFC00408  /* Interrupt Identification Register */
+#define	UART0_LCR	      0xFFC0040C  /* Line Control Register */
+#define	UART0_MCR			 0xFFC00410  /*	Modem Control Register */
+#define	UART0_LSR	      0xFFC00414  /* Line Status Register */
+#define	UART0_SCR	      0xFFC0041C  /* SCR Scratch Register */
+#define	UART0_GCTL		     0xFFC00424	 /* Global Control Register */
+
+
+/* SPI0	Controller (0xFFC00500 - 0xFFC005FF) */
+
+#define	SPI0_CTL			0xFFC00500  /* SPI0 Control Register */
+#define	SPI0_FLG			0xFFC00504  /* SPI0 Flag register */
+#define	SPI0_STAT			0xFFC00508  /* SPI0 Status register */
+#define	SPI0_TDBR			0xFFC0050C  /* SPI0 Transmit Data Buffer Register */
+#define	SPI0_RDBR			0xFFC00510  /* SPI0 Receive Data Buffer	Register */
+#define	SPI0_BAUD			0xFFC00514  /* SPI0 Baud rate Register */
+#define	SPI0_SHADOW			0xFFC00518  /* SPI0_RDBR Shadow	Register */
+#define SPI0_REGBASE			SPI0_CTL
+
+
+/* TIMER 0, 1, 2 Registers (0xFFC00600 - 0xFFC006FF) */
+#define	TIMER0_CONFIG			0xFFC00600     /* Timer	0 Configuration	Register */
+#define	TIMER0_COUNTER				0xFFC00604     /* Timer	0 Counter Register */
+#define	TIMER0_PERIOD			0xFFC00608     /* Timer	0 Period Register */
+#define	TIMER0_WIDTH			0xFFC0060C     /* Timer	0 Width	Register */
+
+#define	TIMER1_CONFIG			0xFFC00610	/*  Timer 1 Configuration Register   */
+#define	TIMER1_COUNTER			0xFFC00614	/*  Timer 1 Counter Register	     */
+#define	TIMER1_PERIOD			0xFFC00618	/*  Timer 1 Period Register	     */
+#define	TIMER1_WIDTH			0xFFC0061C	/*  Timer 1 Width Register	     */
+
+#define	TIMER2_CONFIG			0xFFC00620	/* Timer 2 Configuration Register   */
+#define	TIMER2_COUNTER			0xFFC00624	/* Timer 2 Counter Register	    */
+#define	TIMER2_PERIOD			0xFFC00628	/* Timer 2 Period Register	    */
+#define	TIMER2_WIDTH			0xFFC0062C	/* Timer 2 Width Register	    */
+
+#define	TIMER_ENABLE				0xFFC00640	/* Timer Enable	Register */
+#define	TIMER_DISABLE				0xFFC00644	/* Timer Disable Register */
+#define	TIMER_STATUS				0xFFC00648	/* Timer Status	Register */
+
+
+/* Programmable	Flags (0xFFC00700 - 0xFFC007FF) */
+#define	FIO_FLAG_D				0xFFC00700  /* Flag Mask to directly specify state of pins */
+#define	FIO_FLAG_C			0xFFC00704  /* Peripheral Interrupt Flag Register (clear) */
+#define	FIO_FLAG_S			0xFFC00708  /* Peripheral Interrupt Flag Register (set) */
+#define	FIO_FLAG_T					0xFFC0070C  /* Flag Mask to directly toggle state of pins */
+#define	FIO_MASKA_D			0xFFC00710  /* Flag Mask Interrupt A Register (set directly) */
+#define	FIO_MASKA_C			0xFFC00714  /* Flag Mask Interrupt A Register (clear) */
+#define	FIO_MASKA_S			0xFFC00718  /* Flag Mask Interrupt A Register (set) */
+#define	FIO_MASKA_T			0xFFC0071C  /* Flag Mask Interrupt A Register (toggle) */
+#define	FIO_MASKB_D			0xFFC00720  /* Flag Mask Interrupt B Register (set directly) */
+#define	FIO_MASKB_C			0xFFC00724  /* Flag Mask Interrupt B Register (clear) */
+#define	FIO_MASKB_S			0xFFC00728  /* Flag Mask Interrupt B Register (set) */
+#define	FIO_MASKB_T			0xFFC0072C  /* Flag Mask Interrupt B Register (toggle) */
+#define	FIO_DIR				0xFFC00730  /* Peripheral Flag Direction Register */
+#define	FIO_POLAR			0xFFC00734  /* Flag Source Polarity Register */
+#define	FIO_EDGE			0xFFC00738  /* Flag Source Sensitivity Register */
+#define	FIO_BOTH			0xFFC0073C  /* Flag Set	on BOTH	Edges Register */
+#define	FIO_INEN					0xFFC00740  /* Flag Input Enable Register  */
+
+
+/* SPORT0 Controller (0xFFC00800 - 0xFFC008FF) */
+#define	SPORT0_TCR1				0xFFC00800  /* SPORT0 Transmit Configuration 1 Register */
+#define	SPORT0_TCR2				0xFFC00804  /* SPORT0 Transmit Configuration 2 Register */
+#define	SPORT0_TCLKDIV			0xFFC00808  /* SPORT0 Transmit Clock Divider */
+#define	SPORT0_TFSDIV			0xFFC0080C  /* SPORT0 Transmit Frame Sync Divider */
+#define	SPORT0_TX			0xFFC00810  /* SPORT0 TX Data Register */
+#define	SPORT0_RX			0xFFC00818  /* SPORT0 RX Data Register */
+#define	SPORT0_RCR1				0xFFC00820  /* SPORT0 Transmit Configuration 1 Register */
+#define	SPORT0_RCR2				0xFFC00824  /* SPORT0 Transmit Configuration 2 Register */
+#define	SPORT0_RCLKDIV			0xFFC00828  /* SPORT0 Receive Clock Divider */
+#define	SPORT0_RFSDIV			0xFFC0082C  /* SPORT0 Receive Frame Sync Divider */
+#define	SPORT0_STAT			0xFFC00830  /* SPORT0 Status Register */
+#define	SPORT0_CHNL			0xFFC00834  /* SPORT0 Current Channel Register */
+#define	SPORT0_MCMC1			0xFFC00838  /* SPORT0 Multi-Channel Configuration Register 1 */
+#define	SPORT0_MCMC2			0xFFC0083C  /* SPORT0 Multi-Channel Configuration Register 2 */
+#define	SPORT0_MTCS0			0xFFC00840  /* SPORT0 Multi-Channel Transmit Select Register 0 */
+#define	SPORT0_MTCS1			0xFFC00844  /* SPORT0 Multi-Channel Transmit Select Register 1 */
+#define	SPORT0_MTCS2			0xFFC00848  /* SPORT0 Multi-Channel Transmit Select Register 2 */
+#define	SPORT0_MTCS3			0xFFC0084C  /* SPORT0 Multi-Channel Transmit Select Register 3 */
+#define	SPORT0_MRCS0			0xFFC00850  /* SPORT0 Multi-Channel Receive Select Register 0 */
+#define	SPORT0_MRCS1			0xFFC00854  /* SPORT0 Multi-Channel Receive Select Register 1 */
+#define	SPORT0_MRCS2			0xFFC00858  /* SPORT0 Multi-Channel Receive Select Register 2 */
+#define	SPORT0_MRCS3			0xFFC0085C  /* SPORT0 Multi-Channel Receive Select Register 3 */
+
+
+/* SPORT1 Controller (0xFFC00900 - 0xFFC009FF) */
+#define	SPORT1_TCR1				0xFFC00900  /* SPORT1 Transmit Configuration 1 Register */
+#define	SPORT1_TCR2				0xFFC00904  /* SPORT1 Transmit Configuration 2 Register */
+#define	SPORT1_TCLKDIV			0xFFC00908  /* SPORT1 Transmit Clock Divider */
+#define	SPORT1_TFSDIV			0xFFC0090C  /* SPORT1 Transmit Frame Sync Divider */
+#define	SPORT1_TX			0xFFC00910  /* SPORT1 TX Data Register */
+#define	SPORT1_RX			0xFFC00918  /* SPORT1 RX Data Register */
+#define	SPORT1_RCR1				0xFFC00920  /* SPORT1 Transmit Configuration 1 Register */
+#define	SPORT1_RCR2				0xFFC00924  /* SPORT1 Transmit Configuration 2 Register */
+#define	SPORT1_RCLKDIV			0xFFC00928  /* SPORT1 Receive Clock Divider */
+#define	SPORT1_RFSDIV			0xFFC0092C  /* SPORT1 Receive Frame Sync Divider */
+#define	SPORT1_STAT			0xFFC00930  /* SPORT1 Status Register */
+#define	SPORT1_CHNL			0xFFC00934  /* SPORT1 Current Channel Register */
+#define	SPORT1_MCMC1			0xFFC00938  /* SPORT1 Multi-Channel Configuration Register 1 */
+#define	SPORT1_MCMC2			0xFFC0093C  /* SPORT1 Multi-Channel Configuration Register 2 */
+#define	SPORT1_MTCS0			0xFFC00940  /* SPORT1 Multi-Channel Transmit Select Register 0 */
+#define	SPORT1_MTCS1			0xFFC00944  /* SPORT1 Multi-Channel Transmit Select Register 1 */
+#define	SPORT1_MTCS2			0xFFC00948  /* SPORT1 Multi-Channel Transmit Select Register 2 */
+#define	SPORT1_MTCS3			0xFFC0094C  /* SPORT1 Multi-Channel Transmit Select Register 3 */
+#define	SPORT1_MRCS0			0xFFC00950  /* SPORT1 Multi-Channel Receive Select Register 0 */
+#define	SPORT1_MRCS1			0xFFC00954  /* SPORT1 Multi-Channel Receive Select Register 1 */
+#define	SPORT1_MRCS2			0xFFC00958  /* SPORT1 Multi-Channel Receive Select Register 2 */
+#define	SPORT1_MRCS3			0xFFC0095C  /* SPORT1 Multi-Channel Receive Select Register 3 */
+
+
+/* External Bus	Interface Unit (0xFFC00A00 - 0xFFC00AFF) */
+/* Asynchronous	Memory Controller  */
+#define	EBIU_AMGCTL			0xFFC00A00  /* Asynchronous Memory Global Control Register */
+#define	EBIU_AMBCTL0		0xFFC00A04  /* Asynchronous Memory Bank	Control	Register 0 */
+#define	EBIU_AMBCTL1		0xFFC00A08  /* Asynchronous Memory Bank	Control	Register 1 */
+
+/* SDRAM Controller */
+#define	EBIU_SDGCTL			0xFFC00A10  /* SDRAM Global Control Register */
+#define	EBIU_SDBCTL			0xFFC00A14  /* SDRAM Bank Control Register */
+#define	EBIU_SDRRC			0xFFC00A18  /* SDRAM Refresh Rate Control Register */
+#define	EBIU_SDSTAT			0xFFC00A1C  /* SDRAM Status Register */
+
+
+
+/* DMA Controller 0 Traffic Control Registers (0xFFC00B00 - 0xFFC00BFF) */
+
+#define	DMAC0_TC_PER			0xFFC00B0C	/* DMA Controller 0 Traffic Control Periods Register */
+#define	DMAC0_TC_CNT			0xFFC00B10	/* DMA Controller 0 Traffic Control Current Counts Register */
+
+/* Alternate deprecated	register names (below) provided	for backwards code compatibility */
+#define	DMA0_TCPER			DMAC0_TC_PER
+#define	DMA0_TCCNT			DMAC0_TC_CNT
+
+
+/* DMA Controller 0 (0xFFC00C00	- 0xFFC00FFF)							 */
+
+#define	DMA0_NEXT_DESC_PTR		0xFFC00C00	/* DMA Channel 0 Next Descriptor Pointer Register */
+#define	DMA0_START_ADDR			0xFFC00C04	/* DMA Channel 0 Start Address Register */
+#define	DMA0_CONFIG				0xFFC00C08	/* DMA Channel 0 Configuration Register */
+#define	DMA0_X_COUNT			0xFFC00C10	/* DMA Channel 0 X Count Register */
+#define	DMA0_X_MODIFY			0xFFC00C14	/* DMA Channel 0 X Modify Register */
+#define	DMA0_Y_COUNT			0xFFC00C18	/* DMA Channel 0 Y Count Register */
+#define	DMA0_Y_MODIFY			0xFFC00C1C	/* DMA Channel 0 Y Modify Register */
+#define	DMA0_CURR_DESC_PTR		0xFFC00C20	/* DMA Channel 0 Current Descriptor Pointer Register */
+#define	DMA0_CURR_ADDR			0xFFC00C24	/* DMA Channel 0 Current Address Register */
+#define	DMA0_IRQ_STATUS			0xFFC00C28	/* DMA Channel 0 Interrupt/Status Register */
+#define	DMA0_PERIPHERAL_MAP		0xFFC00C2C	/* DMA Channel 0 Peripheral Map	Register */
+#define	DMA0_CURR_X_COUNT		0xFFC00C30	/* DMA Channel 0 Current X Count Register */
+#define	DMA0_CURR_Y_COUNT		0xFFC00C38	/* DMA Channel 0 Current Y Count Register */
+
+#define	DMA1_NEXT_DESC_PTR		0xFFC00C40	/* DMA Channel 1 Next Descriptor Pointer Register */
+#define	DMA1_START_ADDR			0xFFC00C44	/* DMA Channel 1 Start Address Register */
+#define	DMA1_CONFIG				0xFFC00C48	/* DMA Channel 1 Configuration Register */
+#define	DMA1_X_COUNT			0xFFC00C50	/* DMA Channel 1 X Count Register */
+#define	DMA1_X_MODIFY			0xFFC00C54	/* DMA Channel 1 X Modify Register */
+#define	DMA1_Y_COUNT			0xFFC00C58	/* DMA Channel 1 Y Count Register */
+#define	DMA1_Y_MODIFY			0xFFC00C5C	/* DMA Channel 1 Y Modify Register */
+#define	DMA1_CURR_DESC_PTR		0xFFC00C60	/* DMA Channel 1 Current Descriptor Pointer Register */
+#define	DMA1_CURR_ADDR			0xFFC00C64	/* DMA Channel 1 Current Address Register */
+#define	DMA1_IRQ_STATUS			0xFFC00C68	/* DMA Channel 1 Interrupt/Status Register */
+#define	DMA1_PERIPHERAL_MAP		0xFFC00C6C	/* DMA Channel 1 Peripheral Map	Register */
+#define	DMA1_CURR_X_COUNT		0xFFC00C70	/* DMA Channel 1 Current X Count Register */
+#define	DMA1_CURR_Y_COUNT		0xFFC00C78	/* DMA Channel 1 Current Y Count Register */
+
+#define	DMA2_NEXT_DESC_PTR		0xFFC00C80	/* DMA Channel 2 Next Descriptor Pointer Register */
+#define	DMA2_START_ADDR			0xFFC00C84	/* DMA Channel 2 Start Address Register */
+#define	DMA2_CONFIG				0xFFC00C88	/* DMA Channel 2 Configuration Register */
+#define	DMA2_X_COUNT			0xFFC00C90	/* DMA Channel 2 X Count Register */
+#define	DMA2_X_MODIFY			0xFFC00C94	/* DMA Channel 2 X Modify Register */
+#define	DMA2_Y_COUNT			0xFFC00C98	/* DMA Channel 2 Y Count Register */
+#define	DMA2_Y_MODIFY			0xFFC00C9C	/* DMA Channel 2 Y Modify Register */
+#define	DMA2_CURR_DESC_PTR		0xFFC00CA0	/* DMA Channel 2 Current Descriptor Pointer Register */
+#define	DMA2_CURR_ADDR			0xFFC00CA4	/* DMA Channel 2 Current Address Register */
+#define	DMA2_IRQ_STATUS			0xFFC00CA8	/* DMA Channel 2 Interrupt/Status Register */
+#define	DMA2_PERIPHERAL_MAP		0xFFC00CAC	/* DMA Channel 2 Peripheral Map	Register */
+#define	DMA2_CURR_X_COUNT		0xFFC00CB0	/* DMA Channel 2 Current X Count Register */
+#define	DMA2_CURR_Y_COUNT		0xFFC00CB8	/* DMA Channel 2 Current Y Count Register */
+
+#define	DMA3_NEXT_DESC_PTR		0xFFC00CC0	/* DMA Channel 3 Next Descriptor Pointer Register */
+#define	DMA3_START_ADDR			0xFFC00CC4	/* DMA Channel 3 Start Address Register */
+#define	DMA3_CONFIG				0xFFC00CC8	/* DMA Channel 3 Configuration Register */
+#define	DMA3_X_COUNT			0xFFC00CD0	/* DMA Channel 3 X Count Register */
+#define	DMA3_X_MODIFY			0xFFC00CD4	/* DMA Channel 3 X Modify Register */
+#define	DMA3_Y_COUNT			0xFFC00CD8	/* DMA Channel 3 Y Count Register */
+#define	DMA3_Y_MODIFY			0xFFC00CDC	/* DMA Channel 3 Y Modify Register */
+#define	DMA3_CURR_DESC_PTR		0xFFC00CE0	/* DMA Channel 3 Current Descriptor Pointer Register */
+#define	DMA3_CURR_ADDR			0xFFC00CE4	/* DMA Channel 3 Current Address Register */
+#define	DMA3_IRQ_STATUS			0xFFC00CE8	/* DMA Channel 3 Interrupt/Status Register */
+#define	DMA3_PERIPHERAL_MAP		0xFFC00CEC	/* DMA Channel 3 Peripheral Map	Register */
+#define	DMA3_CURR_X_COUNT		0xFFC00CF0	/* DMA Channel 3 Current X Count Register */
+#define	DMA3_CURR_Y_COUNT		0xFFC00CF8	/* DMA Channel 3 Current Y Count Register */
+
+#define	DMA4_NEXT_DESC_PTR		0xFFC00D00	/* DMA Channel 4 Next Descriptor Pointer Register */
+#define	DMA4_START_ADDR			0xFFC00D04	/* DMA Channel 4 Start Address Register */
+#define	DMA4_CONFIG				0xFFC00D08	/* DMA Channel 4 Configuration Register */
+#define	DMA4_X_COUNT			0xFFC00D10	/* DMA Channel 4 X Count Register */
+#define	DMA4_X_MODIFY			0xFFC00D14	/* DMA Channel 4 X Modify Register */
+#define	DMA4_Y_COUNT			0xFFC00D18	/* DMA Channel 4 Y Count Register */
+#define	DMA4_Y_MODIFY			0xFFC00D1C	/* DMA Channel 4 Y Modify Register */
+#define	DMA4_CURR_DESC_PTR		0xFFC00D20	/* DMA Channel 4 Current Descriptor Pointer Register */
+#define	DMA4_CURR_ADDR			0xFFC00D24	/* DMA Channel 4 Current Address Register */
+#define	DMA4_IRQ_STATUS			0xFFC00D28	/* DMA Channel 4 Interrupt/Status Register */
+#define	DMA4_PERIPHERAL_MAP		0xFFC00D2C	/* DMA Channel 4 Peripheral Map	Register */
+#define	DMA4_CURR_X_COUNT		0xFFC00D30	/* DMA Channel 4 Current X Count Register */
+#define	DMA4_CURR_Y_COUNT		0xFFC00D38	/* DMA Channel 4 Current Y Count Register */
+
+#define	DMA5_NEXT_DESC_PTR		0xFFC00D40	/* DMA Channel 5 Next Descriptor Pointer Register */
+#define	DMA5_START_ADDR			0xFFC00D44	/* DMA Channel 5 Start Address Register */
+#define	DMA5_CONFIG				0xFFC00D48	/* DMA Channel 5 Configuration Register */
+#define	DMA5_X_COUNT			0xFFC00D50	/* DMA Channel 5 X Count Register */
+#define	DMA5_X_MODIFY			0xFFC00D54	/* DMA Channel 5 X Modify Register */
+#define	DMA5_Y_COUNT			0xFFC00D58	/* DMA Channel 5 Y Count Register */
+#define	DMA5_Y_MODIFY			0xFFC00D5C	/* DMA Channel 5 Y Modify Register */
+#define	DMA5_CURR_DESC_PTR		0xFFC00D60	/* DMA Channel 5 Current Descriptor Pointer Register */
+#define	DMA5_CURR_ADDR			0xFFC00D64	/* DMA Channel 5 Current Address Register */
+#define	DMA5_IRQ_STATUS			0xFFC00D68	/* DMA Channel 5 Interrupt/Status Register */
+#define	DMA5_PERIPHERAL_MAP		0xFFC00D6C	/* DMA Channel 5 Peripheral Map	Register */
+#define	DMA5_CURR_X_COUNT		0xFFC00D70	/* DMA Channel 5 Current X Count Register */
+#define	DMA5_CURR_Y_COUNT		0xFFC00D78	/* DMA Channel 5 Current Y Count Register */
+
+#define	DMA6_NEXT_DESC_PTR		0xFFC00D80	/* DMA Channel 6 Next Descriptor Pointer Register */
+#define	DMA6_START_ADDR			0xFFC00D84	/* DMA Channel 6 Start Address Register */
+#define	DMA6_CONFIG				0xFFC00D88	/* DMA Channel 6 Configuration Register */
+#define	DMA6_X_COUNT			0xFFC00D90	/* DMA Channel 6 X Count Register */
+#define	DMA6_X_MODIFY			0xFFC00D94	/* DMA Channel 6 X Modify Register */
+#define	DMA6_Y_COUNT			0xFFC00D98	/* DMA Channel 6 Y Count Register */
+#define	DMA6_Y_MODIFY			0xFFC00D9C	/* DMA Channel 6 Y Modify Register */
+#define	DMA6_CURR_DESC_PTR		0xFFC00DA0	/* DMA Channel 6 Current Descriptor Pointer Register */
+#define	DMA6_CURR_ADDR			0xFFC00DA4	/* DMA Channel 6 Current Address Register */
+#define	DMA6_IRQ_STATUS			0xFFC00DA8	/* DMA Channel 6 Interrupt/Status Register */
+#define	DMA6_PERIPHERAL_MAP		0xFFC00DAC	/* DMA Channel 6 Peripheral Map	Register */
+#define	DMA6_CURR_X_COUNT		0xFFC00DB0	/* DMA Channel 6 Current X Count Register */
+#define	DMA6_CURR_Y_COUNT		0xFFC00DB8	/* DMA Channel 6 Current Y Count Register */
+
+#define	DMA7_NEXT_DESC_PTR		0xFFC00DC0	/* DMA Channel 7 Next Descriptor Pointer Register */
+#define	DMA7_START_ADDR			0xFFC00DC4	/* DMA Channel 7 Start Address Register */
+#define	DMA7_CONFIG				0xFFC00DC8	/* DMA Channel 7 Configuration Register */
+#define	DMA7_X_COUNT			0xFFC00DD0	/* DMA Channel 7 X Count Register */
+#define	DMA7_X_MODIFY			0xFFC00DD4	/* DMA Channel 7 X Modify Register */
+#define	DMA7_Y_COUNT			0xFFC00DD8	/* DMA Channel 7 Y Count Register */
+#define	DMA7_Y_MODIFY			0xFFC00DDC	/* DMA Channel 7 Y Modify Register */
+#define	DMA7_CURR_DESC_PTR		0xFFC00DE0	/* DMA Channel 7 Current Descriptor Pointer Register */
+#define	DMA7_CURR_ADDR			0xFFC00DE4	/* DMA Channel 7 Current Address Register */
+#define	DMA7_IRQ_STATUS			0xFFC00DE8	/* DMA Channel 7 Interrupt/Status Register */
+#define	DMA7_PERIPHERAL_MAP		0xFFC00DEC	/* DMA Channel 7 Peripheral Map	Register */
+#define	DMA7_CURR_X_COUNT		0xFFC00DF0	/* DMA Channel 7 Current X Count Register */
+#define	DMA7_CURR_Y_COUNT		0xFFC00DF8	/* DMA Channel 7 Current Y Count Register */
+
+#define	MDMA0_D0_NEXT_DESC_PTR	0xFFC00E00	/* MemDMA0 Stream 0 Destination	Next Descriptor	Pointer	Register */
+#define	MDMA0_D0_START_ADDR		0xFFC00E04	/* MemDMA0 Stream 0 Destination	Start Address Register */
+#define	MDMA0_D0_CONFIG			0xFFC00E08	/* MemDMA0 Stream 0 Destination	Configuration Register */
+#define	MDMA0_D0_X_COUNT		0xFFC00E10	/* MemDMA0 Stream 0 Destination	X Count	Register */
+#define	MDMA0_D0_X_MODIFY		0xFFC00E14	/* MemDMA0 Stream 0 Destination	X Modify Register */
+#define	MDMA0_D0_Y_COUNT		0xFFC00E18	/* MemDMA0 Stream 0 Destination	Y Count	Register */
+#define	MDMA0_D0_Y_MODIFY		0xFFC00E1C	/* MemDMA0 Stream 0 Destination	Y Modify Register */
+#define	MDMA0_D0_CURR_DESC_PTR	0xFFC00E20	/* MemDMA0 Stream 0 Destination	Current	Descriptor Pointer Register */
+#define	MDMA0_D0_CURR_ADDR		0xFFC00E24	/* MemDMA0 Stream 0 Destination	Current	Address	Register */
+#define	MDMA0_D0_IRQ_STATUS		0xFFC00E28	/* MemDMA0 Stream 0 Destination	Interrupt/Status Register */
+#define	MDMA0_D0_PERIPHERAL_MAP	0xFFC00E2C	/* MemDMA0 Stream 0 Destination	Peripheral Map Register */
+#define	MDMA0_D0_CURR_X_COUNT	0xFFC00E30	/* MemDMA0 Stream 0 Destination	Current	X Count	Register */
+#define	MDMA0_D0_CURR_Y_COUNT	0xFFC00E38	/* MemDMA0 Stream 0 Destination	Current	Y Count	Register */
+
+#define	MDMA0_S0_NEXT_DESC_PTR	0xFFC00E40	/* MemDMA0 Stream 0 Source Next	Descriptor Pointer Register */
+#define	MDMA0_S0_START_ADDR		0xFFC00E44	/* MemDMA0 Stream 0 Source Start Address Register */
+#define	MDMA0_S0_CONFIG			0xFFC00E48	/* MemDMA0 Stream 0 Source Configuration Register */
+#define	MDMA0_S0_X_COUNT		0xFFC00E50	/* MemDMA0 Stream 0 Source X Count Register */
+#define	MDMA0_S0_X_MODIFY		0xFFC00E54	/* MemDMA0 Stream 0 Source X Modify Register */
+#define	MDMA0_S0_Y_COUNT		0xFFC00E58	/* MemDMA0 Stream 0 Source Y Count Register */
+#define	MDMA0_S0_Y_MODIFY		0xFFC00E5C	/* MemDMA0 Stream 0 Source Y Modify Register */
+#define	MDMA0_S0_CURR_DESC_PTR	0xFFC00E60	/* MemDMA0 Stream 0 Source Current Descriptor Pointer Register */
+#define	MDMA0_S0_CURR_ADDR		0xFFC00E64	/* MemDMA0 Stream 0 Source Current Address Register */
+#define	MDMA0_S0_IRQ_STATUS		0xFFC00E68	/* MemDMA0 Stream 0 Source Interrupt/Status Register */
+#define	MDMA0_S0_PERIPHERAL_MAP	0xFFC00E6C	/* MemDMA0 Stream 0 Source Peripheral Map Register */
+#define	MDMA0_S0_CURR_X_COUNT	0xFFC00E70	/* MemDMA0 Stream 0 Source Current X Count Register */
+#define	MDMA0_S0_CURR_Y_COUNT	0xFFC00E78	/* MemDMA0 Stream 0 Source Current Y Count Register */
+
+#define	MDMA0_D1_NEXT_DESC_PTR	0xFFC00E80	/* MemDMA0 Stream 1 Destination	Next Descriptor	Pointer	Register */
+#define	MDMA0_D1_START_ADDR		0xFFC00E84	/* MemDMA0 Stream 1 Destination	Start Address Register */
+#define	MDMA0_D1_CONFIG			0xFFC00E88	/* MemDMA0 Stream 1 Destination	Configuration Register */
+#define	MDMA0_D1_X_COUNT		0xFFC00E90	/* MemDMA0 Stream 1 Destination	X Count	Register */
+#define	MDMA0_D1_X_MODIFY		0xFFC00E94	/* MemDMA0 Stream 1 Destination	X Modify Register */
+#define	MDMA0_D1_Y_COUNT		0xFFC00E98	/* MemDMA0 Stream 1 Destination	Y Count	Register */
+#define	MDMA0_D1_Y_MODIFY		0xFFC00E9C	/* MemDMA0 Stream 1 Destination	Y Modify Register */
+#define	MDMA0_D1_CURR_DESC_PTR	0xFFC00EA0	/* MemDMA0 Stream 1 Destination	Current	Descriptor Pointer Register */
+#define	MDMA0_D1_CURR_ADDR		0xFFC00EA4	/* MemDMA0 Stream 1 Destination	Current	Address	Register */
+#define	MDMA0_D1_IRQ_STATUS		0xFFC00EA8	/* MemDMA0 Stream 1 Destination	Interrupt/Status Register */
+#define	MDMA0_D1_PERIPHERAL_MAP	0xFFC00EAC	/* MemDMA0 Stream 1 Destination	Peripheral Map Register */
+#define	MDMA0_D1_CURR_X_COUNT	0xFFC00EB0	/* MemDMA0 Stream 1 Destination	Current	X Count	Register */
+#define	MDMA0_D1_CURR_Y_COUNT	0xFFC00EB8	/* MemDMA0 Stream 1 Destination	Current	Y Count	Register */
+
+#define	MDMA0_S1_NEXT_DESC_PTR	0xFFC00EC0	/* MemDMA0 Stream 1 Source Next	Descriptor Pointer Register */
+#define	MDMA0_S1_START_ADDR		0xFFC00EC4	/* MemDMA0 Stream 1 Source Start Address Register */
+#define	MDMA0_S1_CONFIG			0xFFC00EC8	/* MemDMA0 Stream 1 Source Configuration Register */
+#define	MDMA0_S1_X_COUNT		0xFFC00ED0	/* MemDMA0 Stream 1 Source X Count Register */
+#define	MDMA0_S1_X_MODIFY		0xFFC00ED4	/* MemDMA0 Stream 1 Source X Modify Register */
+#define	MDMA0_S1_Y_COUNT		0xFFC00ED8	/* MemDMA0 Stream 1 Source Y Count Register */
+#define	MDMA0_S1_Y_MODIFY		0xFFC00EDC	/* MemDMA0 Stream 1 Source Y Modify Register */
+#define	MDMA0_S1_CURR_DESC_PTR	0xFFC00EE0	/* MemDMA0 Stream 1 Source Current Descriptor Pointer Register */
+#define	MDMA0_S1_CURR_ADDR		0xFFC00EE4	/* MemDMA0 Stream 1 Source Current Address Register */
+#define	MDMA0_S1_IRQ_STATUS		0xFFC00EE8	/* MemDMA0 Stream 1 Source Interrupt/Status Register */
+#define	MDMA0_S1_PERIPHERAL_MAP	0xFFC00EEC	/* MemDMA0 Stream 1 Source Peripheral Map Register */
+#define	MDMA0_S1_CURR_X_COUNT	0xFFC00EF0	/* MemDMA0 Stream 1 Source Current X Count Register */
+#define	MDMA0_S1_CURR_Y_COUNT	0xFFC00EF8	/* MemDMA0 Stream 1 Source Current Y Count Register */
+
+
+/* Parallel Peripheral Interface (PPI) (0xFFC01000 - 0xFFC010FF) */
+#define	PPI_CONTROL			0xFFC01000	/* PPI Control Register */
+#define	PPI_STATUS			0xFFC01004	/* PPI Status Register */
+#define	PPI_COUNT			0xFFC01008	/* PPI Transfer	Count Register */
+#define	PPI_DELAY			0xFFC0100C	/* PPI Delay Count Register */
+#define	PPI_FRAME			0xFFC01010	/* PPI Frame Length Register */
+
+
+/* Two-Wire Interface 0	(0xFFC01400 - 0xFFC014FF)			 */
+#define	TWI0_CLKDIV			0xFFC01400	/* Serial Clock	Divider	Register */
+#define	TWI0_CONTROL		0xFFC01404	/* TWI0	Master Internal	Time Reference Register */
+#define	TWI0_SLAVE_CTRL		0xFFC01408	/* Slave Mode Control Register */
+#define	TWI0_SLAVE_STAT		0xFFC0140C	/* Slave Mode Status Register */
+#define	TWI0_SLAVE_ADDR		0xFFC01410	/* Slave Mode Address Register */
+#define	TWI0_MASTER_CTRL	0xFFC01414	/* Master Mode Control Register */
+#define	TWI0_MASTER_STAT	0xFFC01418	/* Master Mode Status Register */
+#define	TWI0_MASTER_ADDR	0xFFC0141C	/* Master Mode Address Register */
+#define	TWI0_INT_STAT		0xFFC01420	/* TWI0	Master Interrupt Register */
+#define	TWI0_INT_MASK		0xFFC01424	/* TWI0	Master Interrupt Mask Register */
+#define	TWI0_FIFO_CTRL		0xFFC01428	/* FIFO	Control	Register */
+#define	TWI0_FIFO_STAT		0xFFC0142C	/* FIFO	Status Register */
+#define	TWI0_XMT_DATA8		0xFFC01480	/* FIFO	Transmit Data Single Byte Register */
+#define	TWI0_XMT_DATA16		0xFFC01484	/* FIFO	Transmit Data Double Byte Register */
+#define	TWI0_RCV_DATA8		0xFFC01488	/* FIFO	Receive	Data Single Byte Register */
+#define	TWI0_RCV_DATA16		0xFFC0148C	/* FIFO	Receive	Data Double Byte Register */
+
+#define TWI0_REGBASE		TWI0_CLKDIV
+
+/* the following are for backwards compatibility */
+#define	TWI0_PRESCALE	 TWI0_CONTROL
+#define	TWI0_INT_SRC	 TWI0_INT_STAT
+#define	TWI0_INT_ENABLE	 TWI0_INT_MASK
+
+
+/* General-Purpose Ports  (0xFFC01500 -	0xFFC015FF)	 */
+
+/* GPIO	Port C Register	Names */
+#define	GPIO_C_CNFG			0xFFC01500	/* GPIO	Pin Port C Configuration Register */
+#define	GPIO_C_D			0xFFC01510	/* GPIO	Pin Port C Data	Register */
+#define	GPIO_C_C			0xFFC01520	/* Clear GPIO Pin Port C Register */
+#define	GPIO_C_S			0xFFC01530	/* Set GPIO Pin	Port C Register */
+#define	GPIO_C_T			0xFFC01540	/* Toggle GPIO Pin Port	C Register */
+#define	GPIO_C_DIR			0xFFC01550	/* GPIO	Pin Port C Direction Register */
+#define	GPIO_C_INEN			0xFFC01560	/* GPIO	Pin Port C Input Enable	Register */
+
+/* GPIO	Port D Register	Names */
+#define	GPIO_D_CNFG			0xFFC01504	/* GPIO	Pin Port D Configuration Register */
+#define	GPIO_D_D			0xFFC01514	/* GPIO	Pin Port D Data	Register */
+#define	GPIO_D_C			0xFFC01524	/* Clear GPIO Pin Port D Register */
+#define	GPIO_D_S			0xFFC01534	/* Set GPIO Pin	Port D Register */
+#define	GPIO_D_T			0xFFC01544	/* Toggle GPIO Pin Port	D Register */
+#define	GPIO_D_DIR			0xFFC01554	/* GPIO	Pin Port D Direction Register */
+#define	GPIO_D_INEN			0xFFC01564	/* GPIO	Pin Port D Input Enable	Register */
+
+/* GPIO	Port E Register	Names */
+#define	GPIO_E_CNFG			0xFFC01508	/* GPIO	Pin Port E Configuration Register */
+#define	GPIO_E_D			0xFFC01518	/* GPIO	Pin Port E Data	Register */
+#define	GPIO_E_C			0xFFC01528	/* Clear GPIO Pin Port E Register */
+#define	GPIO_E_S			0xFFC01538	/* Set GPIO Pin	Port E Register */
+#define	GPIO_E_T			0xFFC01548	/* Toggle GPIO Pin Port	E Register */
+#define	GPIO_E_DIR			0xFFC01558	/* GPIO	Pin Port E Direction Register */
+#define	GPIO_E_INEN			0xFFC01568	/* GPIO	Pin Port E Input Enable	Register */
+
+/* DMA Controller 1 Traffic Control Registers (0xFFC01B00 - 0xFFC01BFF) */
+
+#define	DMAC1_TC_PER			0xFFC01B0C	/* DMA Controller 1 Traffic Control Periods Register */
+#define	DMAC1_TC_CNT			0xFFC01B10	/* DMA Controller 1 Traffic Control Current Counts Register */
+
+/* Alternate deprecated	register names (below) provided	for backwards code compatibility */
+#define	DMA1_TCPER			DMAC1_TC_PER
+#define	DMA1_TCCNT			DMAC1_TC_CNT
+
+
+/* DMA Controller 1 (0xFFC01C00	- 0xFFC01FFF)							 */
+#define	DMA8_NEXT_DESC_PTR		0xFFC01C00	/* DMA Channel 8 Next Descriptor Pointer Register */
+#define	DMA8_START_ADDR			0xFFC01C04	/* DMA Channel 8 Start Address Register */
+#define	DMA8_CONFIG				0xFFC01C08	/* DMA Channel 8 Configuration Register */
+#define	DMA8_X_COUNT			0xFFC01C10	/* DMA Channel 8 X Count Register */
+#define	DMA8_X_MODIFY			0xFFC01C14	/* DMA Channel 8 X Modify Register */
+#define	DMA8_Y_COUNT			0xFFC01C18	/* DMA Channel 8 Y Count Register */
+#define	DMA8_Y_MODIFY			0xFFC01C1C	/* DMA Channel 8 Y Modify Register */
+#define	DMA8_CURR_DESC_PTR		0xFFC01C20	/* DMA Channel 8 Current Descriptor Pointer Register */
+#define	DMA8_CURR_ADDR			0xFFC01C24	/* DMA Channel 8 Current Address Register */
+#define	DMA8_IRQ_STATUS			0xFFC01C28	/* DMA Channel 8 Interrupt/Status Register */
+#define	DMA8_PERIPHERAL_MAP		0xFFC01C2C	/* DMA Channel 8 Peripheral Map	Register */
+#define	DMA8_CURR_X_COUNT		0xFFC01C30	/* DMA Channel 8 Current X Count Register */
+#define	DMA8_CURR_Y_COUNT		0xFFC01C38	/* DMA Channel 8 Current Y Count Register */
+
+#define	DMA9_NEXT_DESC_PTR		0xFFC01C40	/* DMA Channel 9 Next Descriptor Pointer Register */
+#define	DMA9_START_ADDR			0xFFC01C44	/* DMA Channel 9 Start Address Register */
+#define	DMA9_CONFIG				0xFFC01C48	/* DMA Channel 9 Configuration Register */
+#define	DMA9_X_COUNT			0xFFC01C50	/* DMA Channel 9 X Count Register */
+#define	DMA9_X_MODIFY			0xFFC01C54	/* DMA Channel 9 X Modify Register */
+#define	DMA9_Y_COUNT			0xFFC01C58	/* DMA Channel 9 Y Count Register */
+#define	DMA9_Y_MODIFY			0xFFC01C5C	/* DMA Channel 9 Y Modify Register */
+#define	DMA9_CURR_DESC_PTR		0xFFC01C60	/* DMA Channel 9 Current Descriptor Pointer Register */
+#define	DMA9_CURR_ADDR			0xFFC01C64	/* DMA Channel 9 Current Address Register */
+#define	DMA9_IRQ_STATUS			0xFFC01C68	/* DMA Channel 9 Interrupt/Status Register */
+#define	DMA9_PERIPHERAL_MAP		0xFFC01C6C	/* DMA Channel 9 Peripheral Map	Register */
+#define	DMA9_CURR_X_COUNT		0xFFC01C70	/* DMA Channel 9 Current X Count Register */
+#define	DMA9_CURR_Y_COUNT		0xFFC01C78	/* DMA Channel 9 Current Y Count Register */
+
+#define	DMA10_NEXT_DESC_PTR		0xFFC01C80	/* DMA Channel 10 Next Descriptor Pointer Register */
+#define	DMA10_START_ADDR		0xFFC01C84	/* DMA Channel 10 Start	Address	Register */
+#define	DMA10_CONFIG			0xFFC01C88	/* DMA Channel 10 Configuration	Register */
+#define	DMA10_X_COUNT			0xFFC01C90	/* DMA Channel 10 X Count Register */
+#define	DMA10_X_MODIFY			0xFFC01C94	/* DMA Channel 10 X Modify Register */
+#define	DMA10_Y_COUNT			0xFFC01C98	/* DMA Channel 10 Y Count Register */
+#define	DMA10_Y_MODIFY			0xFFC01C9C	/* DMA Channel 10 Y Modify Register */
+#define	DMA10_CURR_DESC_PTR		0xFFC01CA0	/* DMA Channel 10 Current Descriptor Pointer Register */
+#define	DMA10_CURR_ADDR			0xFFC01CA4	/* DMA Channel 10 Current Address Register */
+#define	DMA10_IRQ_STATUS		0xFFC01CA8	/* DMA Channel 10 Interrupt/Status Register */
+#define	DMA10_PERIPHERAL_MAP	0xFFC01CAC	/* DMA Channel 10 Peripheral Map Register */
+#define	DMA10_CURR_X_COUNT		0xFFC01CB0	/* DMA Channel 10 Current X Count Register */
+#define	DMA10_CURR_Y_COUNT		0xFFC01CB8	/* DMA Channel 10 Current Y Count Register */
+
+#define	DMA11_NEXT_DESC_PTR		0xFFC01CC0	/* DMA Channel 11 Next Descriptor Pointer Register */
+#define	DMA11_START_ADDR		0xFFC01CC4	/* DMA Channel 11 Start	Address	Register */
+#define	DMA11_CONFIG			0xFFC01CC8	/* DMA Channel 11 Configuration	Register */
+#define	DMA11_X_COUNT			0xFFC01CD0	/* DMA Channel 11 X Count Register */
+#define	DMA11_X_MODIFY			0xFFC01CD4	/* DMA Channel 11 X Modify Register */
+#define	DMA11_Y_COUNT			0xFFC01CD8	/* DMA Channel 11 Y Count Register */
+#define	DMA11_Y_MODIFY			0xFFC01CDC	/* DMA Channel 11 Y Modify Register */
+#define	DMA11_CURR_DESC_PTR		0xFFC01CE0	/* DMA Channel 11 Current Descriptor Pointer Register */
+#define	DMA11_CURR_ADDR			0xFFC01CE4	/* DMA Channel 11 Current Address Register */
+#define	DMA11_IRQ_STATUS		0xFFC01CE8	/* DMA Channel 11 Interrupt/Status Register */
+#define	DMA11_PERIPHERAL_MAP	0xFFC01CEC	/* DMA Channel 11 Peripheral Map Register */
+#define	DMA11_CURR_X_COUNT		0xFFC01CF0	/* DMA Channel 11 Current X Count Register */
+#define	DMA11_CURR_Y_COUNT		0xFFC01CF8	/* DMA Channel 11 Current Y Count Register */
+
+#define	DMA12_NEXT_DESC_PTR		0xFFC01D00	/* DMA Channel 12 Next Descriptor Pointer Register */
+#define	DMA12_START_ADDR		0xFFC01D04	/* DMA Channel 12 Start	Address	Register */
+#define	DMA12_CONFIG			0xFFC01D08	/* DMA Channel 12 Configuration	Register */
+#define	DMA12_X_COUNT			0xFFC01D10	/* DMA Channel 12 X Count Register */
+#define	DMA12_X_MODIFY			0xFFC01D14	/* DMA Channel 12 X Modify Register */
+#define	DMA12_Y_COUNT			0xFFC01D18	/* DMA Channel 12 Y Count Register */
+#define	DMA12_Y_MODIFY			0xFFC01D1C	/* DMA Channel 12 Y Modify Register */
+#define	DMA12_CURR_DESC_PTR		0xFFC01D20	/* DMA Channel 12 Current Descriptor Pointer Register */
+#define	DMA12_CURR_ADDR			0xFFC01D24	/* DMA Channel 12 Current Address Register */
+#define	DMA12_IRQ_STATUS		0xFFC01D28	/* DMA Channel 12 Interrupt/Status Register */
+#define	DMA12_PERIPHERAL_MAP	0xFFC01D2C	/* DMA Channel 12 Peripheral Map Register */
+#define	DMA12_CURR_X_COUNT		0xFFC01D30	/* DMA Channel 12 Current X Count Register */
+#define	DMA12_CURR_Y_COUNT		0xFFC01D38	/* DMA Channel 12 Current Y Count Register */
+
+#define	DMA13_NEXT_DESC_PTR		0xFFC01D40	/* DMA Channel 13 Next Descriptor Pointer Register */
+#define	DMA13_START_ADDR		0xFFC01D44	/* DMA Channel 13 Start	Address	Register */
+#define	DMA13_CONFIG			0xFFC01D48	/* DMA Channel 13 Configuration	Register */
+#define	DMA13_X_COUNT			0xFFC01D50	/* DMA Channel 13 X Count Register */
+#define	DMA13_X_MODIFY			0xFFC01D54	/* DMA Channel 13 X Modify Register */
+#define	DMA13_Y_COUNT			0xFFC01D58	/* DMA Channel 13 Y Count Register */
+#define	DMA13_Y_MODIFY			0xFFC01D5C	/* DMA Channel 13 Y Modify Register */
+#define	DMA13_CURR_DESC_PTR		0xFFC01D60	/* DMA Channel 13 Current Descriptor Pointer Register */
+#define	DMA13_CURR_ADDR			0xFFC01D64	/* DMA Channel 13 Current Address Register */
+#define	DMA13_IRQ_STATUS		0xFFC01D68	/* DMA Channel 13 Interrupt/Status Register */
+#define	DMA13_PERIPHERAL_MAP	0xFFC01D6C	/* DMA Channel 13 Peripheral Map Register */
+#define	DMA13_CURR_X_COUNT		0xFFC01D70	/* DMA Channel 13 Current X Count Register */
+#define	DMA13_CURR_Y_COUNT		0xFFC01D78	/* DMA Channel 13 Current Y Count Register */
+
+#define	DMA14_NEXT_DESC_PTR		0xFFC01D80	/* DMA Channel 14 Next Descriptor Pointer Register */
+#define	DMA14_START_ADDR		0xFFC01D84	/* DMA Channel 14 Start	Address	Register */
+#define	DMA14_CONFIG			0xFFC01D88	/* DMA Channel 14 Configuration	Register */
+#define	DMA14_X_COUNT			0xFFC01D90	/* DMA Channel 14 X Count Register */
+#define	DMA14_X_MODIFY			0xFFC01D94	/* DMA Channel 14 X Modify Register */
+#define	DMA14_Y_COUNT			0xFFC01D98	/* DMA Channel 14 Y Count Register */
+#define	DMA14_Y_MODIFY			0xFFC01D9C	/* DMA Channel 14 Y Modify Register */
+#define	DMA14_CURR_DESC_PTR		0xFFC01DA0	/* DMA Channel 14 Current Descriptor Pointer Register */
+#define	DMA14_CURR_ADDR			0xFFC01DA4	/* DMA Channel 14 Current Address Register */
+#define	DMA14_IRQ_STATUS		0xFFC01DA8	/* DMA Channel 14 Interrupt/Status Register */
+#define	DMA14_PERIPHERAL_MAP	0xFFC01DAC	/* DMA Channel 14 Peripheral Map Register */
+#define	DMA14_CURR_X_COUNT		0xFFC01DB0	/* DMA Channel 14 Current X Count Register */
+#define	DMA14_CURR_Y_COUNT		0xFFC01DB8	/* DMA Channel 14 Current Y Count Register */
+
+#define	DMA15_NEXT_DESC_PTR		0xFFC01DC0	/* DMA Channel 15 Next Descriptor Pointer Register */
+#define	DMA15_START_ADDR		0xFFC01DC4	/* DMA Channel 15 Start	Address	Register */
+#define	DMA15_CONFIG			0xFFC01DC8	/* DMA Channel 15 Configuration	Register */
+#define	DMA15_X_COUNT			0xFFC01DD0	/* DMA Channel 15 X Count Register */
+#define	DMA15_X_MODIFY			0xFFC01DD4	/* DMA Channel 15 X Modify Register */
+#define	DMA15_Y_COUNT			0xFFC01DD8	/* DMA Channel 15 Y Count Register */
+#define	DMA15_Y_MODIFY			0xFFC01DDC	/* DMA Channel 15 Y Modify Register */
+#define	DMA15_CURR_DESC_PTR		0xFFC01DE0	/* DMA Channel 15 Current Descriptor Pointer Register */
+#define	DMA15_CURR_ADDR			0xFFC01DE4	/* DMA Channel 15 Current Address Register */
+#define	DMA15_IRQ_STATUS		0xFFC01DE8	/* DMA Channel 15 Interrupt/Status Register */
+#define	DMA15_PERIPHERAL_MAP	0xFFC01DEC	/* DMA Channel 15 Peripheral Map Register */
+#define	DMA15_CURR_X_COUNT		0xFFC01DF0	/* DMA Channel 15 Current X Count Register */
+#define	DMA15_CURR_Y_COUNT		0xFFC01DF8	/* DMA Channel 15 Current Y Count Register */
+
+#define	DMA16_NEXT_DESC_PTR		0xFFC01E00	/* DMA Channel 16 Next Descriptor Pointer Register */
+#define	DMA16_START_ADDR		0xFFC01E04	/* DMA Channel 16 Start	Address	Register */
+#define	DMA16_CONFIG			0xFFC01E08	/* DMA Channel 16 Configuration	Register */
+#define	DMA16_X_COUNT			0xFFC01E10	/* DMA Channel 16 X Count Register */
+#define	DMA16_X_MODIFY			0xFFC01E14	/* DMA Channel 16 X Modify Register */
+#define	DMA16_Y_COUNT			0xFFC01E18	/* DMA Channel 16 Y Count Register */
+#define	DMA16_Y_MODIFY			0xFFC01E1C	/* DMA Channel 16 Y Modify Register */
+#define	DMA16_CURR_DESC_PTR		0xFFC01E20	/* DMA Channel 16 Current Descriptor Pointer Register */
+#define	DMA16_CURR_ADDR			0xFFC01E24	/* DMA Channel 16 Current Address Register */
+#define	DMA16_IRQ_STATUS		0xFFC01E28	/* DMA Channel 16 Interrupt/Status Register */
+#define	DMA16_PERIPHERAL_MAP	0xFFC01E2C	/* DMA Channel 16 Peripheral Map Register */
+#define	DMA16_CURR_X_COUNT		0xFFC01E30	/* DMA Channel 16 Current X Count Register */
+#define	DMA16_CURR_Y_COUNT		0xFFC01E38	/* DMA Channel 16 Current Y Count Register */
+
+#define	DMA17_NEXT_DESC_PTR		0xFFC01E40	/* DMA Channel 17 Next Descriptor Pointer Register */
+#define	DMA17_START_ADDR		0xFFC01E44	/* DMA Channel 17 Start	Address	Register */
+#define	DMA17_CONFIG			0xFFC01E48	/* DMA Channel 17 Configuration	Register */
+#define	DMA17_X_COUNT			0xFFC01E50	/* DMA Channel 17 X Count Register */
+#define	DMA17_X_MODIFY			0xFFC01E54	/* DMA Channel 17 X Modify Register */
+#define	DMA17_Y_COUNT			0xFFC01E58	/* DMA Channel 17 Y Count Register */
+#define	DMA17_Y_MODIFY			0xFFC01E5C	/* DMA Channel 17 Y Modify Register */
+#define	DMA17_CURR_DESC_PTR		0xFFC01E60	/* DMA Channel 17 Current Descriptor Pointer Register */
+#define	DMA17_CURR_ADDR			0xFFC01E64	/* DMA Channel 17 Current Address Register */
+#define	DMA17_IRQ_STATUS		0xFFC01E68	/* DMA Channel 17 Interrupt/Status Register */
+#define	DMA17_PERIPHERAL_MAP	0xFFC01E6C	/* DMA Channel 17 Peripheral Map Register */
+#define	DMA17_CURR_X_COUNT		0xFFC01E70	/* DMA Channel 17 Current X Count Register */
+#define	DMA17_CURR_Y_COUNT		0xFFC01E78	/* DMA Channel 17 Current Y Count Register */
+
+#define	DMA18_NEXT_DESC_PTR		0xFFC01E80	/* DMA Channel 18 Next Descriptor Pointer Register */
+#define	DMA18_START_ADDR		0xFFC01E84	/* DMA Channel 18 Start	Address	Register */
+#define	DMA18_CONFIG			0xFFC01E88	/* DMA Channel 18 Configuration	Register */
+#define	DMA18_X_COUNT			0xFFC01E90	/* DMA Channel 18 X Count Register */
+#define	DMA18_X_MODIFY			0xFFC01E94	/* DMA Channel 18 X Modify Register */
+#define	DMA18_Y_COUNT			0xFFC01E98	/* DMA Channel 18 Y Count Register */
+#define	DMA18_Y_MODIFY			0xFFC01E9C	/* DMA Channel 18 Y Modify Register */
+#define	DMA18_CURR_DESC_PTR		0xFFC01EA0	/* DMA Channel 18 Current Descriptor Pointer Register */
+#define	DMA18_CURR_ADDR			0xFFC01EA4	/* DMA Channel 18 Current Address Register */
+#define	DMA18_IRQ_STATUS		0xFFC01EA8	/* DMA Channel 18 Interrupt/Status Register */
+#define	DMA18_PERIPHERAL_MAP	0xFFC01EAC	/* DMA Channel 18 Peripheral Map Register */
+#define	DMA18_CURR_X_COUNT		0xFFC01EB0	/* DMA Channel 18 Current X Count Register */
+#define	DMA18_CURR_Y_COUNT		0xFFC01EB8	/* DMA Channel 18 Current Y Count Register */
+
+#define	DMA19_NEXT_DESC_PTR		0xFFC01EC0	/* DMA Channel 19 Next Descriptor Pointer Register */
+#define	DMA19_START_ADDR		0xFFC01EC4	/* DMA Channel 19 Start	Address	Register */
+#define	DMA19_CONFIG			0xFFC01EC8	/* DMA Channel 19 Configuration	Register */
+#define	DMA19_X_COUNT			0xFFC01ED0	/* DMA Channel 19 X Count Register */
+#define	DMA19_X_MODIFY			0xFFC01ED4	/* DMA Channel 19 X Modify Register */
+#define	DMA19_Y_COUNT			0xFFC01ED8	/* DMA Channel 19 Y Count Register */
+#define	DMA19_Y_MODIFY			0xFFC01EDC	/* DMA Channel 19 Y Modify Register */
+#define	DMA19_CURR_DESC_PTR		0xFFC01EE0	/* DMA Channel 19 Current Descriptor Pointer Register */
+#define	DMA19_CURR_ADDR			0xFFC01EE4	/* DMA Channel 19 Current Address Register */
+#define	DMA19_IRQ_STATUS		0xFFC01EE8	/* DMA Channel 19 Interrupt/Status Register */
+#define	DMA19_PERIPHERAL_MAP	0xFFC01EEC	/* DMA Channel 19 Peripheral Map Register */
+#define	DMA19_CURR_X_COUNT		0xFFC01EF0	/* DMA Channel 19 Current X Count Register */
+#define	DMA19_CURR_Y_COUNT		0xFFC01EF8	/* DMA Channel 19 Current Y Count Register */
+
+#define	MDMA1_D0_NEXT_DESC_PTR	0xFFC01F00	/* MemDMA1 Stream 0 Destination	Next Descriptor	Pointer	Register */
+#define	MDMA1_D0_START_ADDR		0xFFC01F04	/* MemDMA1 Stream 0 Destination	Start Address Register */
+#define	MDMA1_D0_CONFIG			0xFFC01F08	/* MemDMA1 Stream 0 Destination	Configuration Register */
+#define	MDMA1_D0_X_COUNT		0xFFC01F10	/* MemDMA1 Stream 0 Destination	X Count	Register */
+#define	MDMA1_D0_X_MODIFY		0xFFC01F14	/* MemDMA1 Stream 0 Destination	X Modify Register */
+#define	MDMA1_D0_Y_COUNT		0xFFC01F18	/* MemDMA1 Stream 0 Destination	Y Count	Register */
+#define	MDMA1_D0_Y_MODIFY		0xFFC01F1C	/* MemDMA1 Stream 0 Destination	Y Modify Register */
+#define	MDMA1_D0_CURR_DESC_PTR	0xFFC01F20	/* MemDMA1 Stream 0 Destination	Current	Descriptor Pointer Register */
+#define	MDMA1_D0_CURR_ADDR		0xFFC01F24	/* MemDMA1 Stream 0 Destination	Current	Address	Register */
+#define	MDMA1_D0_IRQ_STATUS		0xFFC01F28	/* MemDMA1 Stream 0 Destination	Interrupt/Status Register */
+#define	MDMA1_D0_PERIPHERAL_MAP	0xFFC01F2C	/* MemDMA1 Stream 0 Destination	Peripheral Map Register */
+#define	MDMA1_D0_CURR_X_COUNT	0xFFC01F30	/* MemDMA1 Stream 0 Destination	Current	X Count	Register */
+#define	MDMA1_D0_CURR_Y_COUNT	0xFFC01F38	/* MemDMA1 Stream 0 Destination	Current	Y Count	Register */
+
+#define	MDMA1_S0_NEXT_DESC_PTR	0xFFC01F40	/* MemDMA1 Stream 0 Source Next	Descriptor Pointer Register */
+#define	MDMA1_S0_START_ADDR		0xFFC01F44	/* MemDMA1 Stream 0 Source Start Address Register */
+#define	MDMA1_S0_CONFIG			0xFFC01F48	/* MemDMA1 Stream 0 Source Configuration Register */
+#define	MDMA1_S0_X_COUNT		0xFFC01F50	/* MemDMA1 Stream 0 Source X Count Register */
+#define	MDMA1_S0_X_MODIFY		0xFFC01F54	/* MemDMA1 Stream 0 Source X Modify Register */
+#define	MDMA1_S0_Y_COUNT		0xFFC01F58	/* MemDMA1 Stream 0 Source Y Count Register */
+#define	MDMA1_S0_Y_MODIFY		0xFFC01F5C	/* MemDMA1 Stream 0 Source Y Modify Register */
+#define	MDMA1_S0_CURR_DESC_PTR	0xFFC01F60	/* MemDMA1 Stream 0 Source Current Descriptor Pointer Register */
+#define	MDMA1_S0_CURR_ADDR		0xFFC01F64	/* MemDMA1 Stream 0 Source Current Address Register */
+#define	MDMA1_S0_IRQ_STATUS		0xFFC01F68	/* MemDMA1 Stream 0 Source Interrupt/Status Register */
+#define	MDMA1_S0_PERIPHERAL_MAP	0xFFC01F6C	/* MemDMA1 Stream 0 Source Peripheral Map Register */
+#define	MDMA1_S0_CURR_X_COUNT	0xFFC01F70	/* MemDMA1 Stream 0 Source Current X Count Register */
+#define	MDMA1_S0_CURR_Y_COUNT	0xFFC01F78	/* MemDMA1 Stream 0 Source Current Y Count Register */
+
+#define	MDMA1_D1_NEXT_DESC_PTR	0xFFC01F80	/* MemDMA1 Stream 1 Destination	Next Descriptor	Pointer	Register */
+#define	MDMA1_D1_START_ADDR		0xFFC01F84	/* MemDMA1 Stream 1 Destination	Start Address Register */
+#define	MDMA1_D1_CONFIG			0xFFC01F88	/* MemDMA1 Stream 1 Destination	Configuration Register */
+#define	MDMA1_D1_X_COUNT		0xFFC01F90	/* MemDMA1 Stream 1 Destination	X Count	Register */
+#define	MDMA1_D1_X_MODIFY		0xFFC01F94	/* MemDMA1 Stream 1 Destination	X Modify Register */
+#define	MDMA1_D1_Y_COUNT		0xFFC01F98	/* MemDMA1 Stream 1 Destination	Y Count	Register */
+#define	MDMA1_D1_Y_MODIFY		0xFFC01F9C	/* MemDMA1 Stream 1 Destination	Y Modify Register */
+#define	MDMA1_D1_CURR_DESC_PTR	0xFFC01FA0	/* MemDMA1 Stream 1 Destination	Current	Descriptor Pointer Register */
+#define	MDMA1_D1_CURR_ADDR		0xFFC01FA4	/* MemDMA1 Stream 1 Destination	Current	Address	Register */
+#define	MDMA1_D1_IRQ_STATUS		0xFFC01FA8	/* MemDMA1 Stream 1 Destination	Interrupt/Status Register */
+#define	MDMA1_D1_PERIPHERAL_MAP	0xFFC01FAC	/* MemDMA1 Stream 1 Destination	Peripheral Map Register */
+#define	MDMA1_D1_CURR_X_COUNT	0xFFC01FB0	/* MemDMA1 Stream 1 Destination	Current	X Count	Register */
+#define	MDMA1_D1_CURR_Y_COUNT	0xFFC01FB8	/* MemDMA1 Stream 1 Destination	Current	Y Count	Register */
+
+#define	MDMA1_S1_NEXT_DESC_PTR	0xFFC01FC0	/* MemDMA1 Stream 1 Source Next	Descriptor Pointer Register */
+#define	MDMA1_S1_START_ADDR		0xFFC01FC4	/* MemDMA1 Stream 1 Source Start Address Register */
+#define	MDMA1_S1_CONFIG			0xFFC01FC8	/* MemDMA1 Stream 1 Source Configuration Register */
+#define	MDMA1_S1_X_COUNT		0xFFC01FD0	/* MemDMA1 Stream 1 Source X Count Register */
+#define	MDMA1_S1_X_MODIFY		0xFFC01FD4	/* MemDMA1 Stream 1 Source X Modify Register */
+#define	MDMA1_S1_Y_COUNT		0xFFC01FD8	/* MemDMA1 Stream 1 Source Y Count Register */
+#define	MDMA1_S1_Y_MODIFY		0xFFC01FDC	/* MemDMA1 Stream 1 Source Y Modify Register */
+#define	MDMA1_S1_CURR_DESC_PTR	0xFFC01FE0	/* MemDMA1 Stream 1 Source Current Descriptor Pointer Register */
+#define	MDMA1_S1_CURR_ADDR		0xFFC01FE4	/* MemDMA1 Stream 1 Source Current Address Register */
+#define	MDMA1_S1_IRQ_STATUS		0xFFC01FE8	/* MemDMA1 Stream 1 Source Interrupt/Status Register */
+#define	MDMA1_S1_PERIPHERAL_MAP	0xFFC01FEC	/* MemDMA1 Stream 1 Source Peripheral Map Register */
+#define	MDMA1_S1_CURR_X_COUNT	0xFFC01FF0	/* MemDMA1 Stream 1 Source Current X Count Register */
+#define	MDMA1_S1_CURR_Y_COUNT	0xFFC01FF8	/* MemDMA1 Stream 1 Source Current Y Count Register */
+
+
+/* UART1 Controller		(0xFFC02000 - 0xFFC020FF)	 */
+#define	UART1_THR			0xFFC02000	/* Transmit Holding register */
+#define	UART1_RBR			0xFFC02000	/* Receive Buffer register */
+#define	UART1_DLL			0xFFC02000	/* Divisor Latch (Low-Byte) */
+#define	UART1_IER			0xFFC02004	/* Interrupt Enable Register */
+#define	UART1_DLH			0xFFC02004	/* Divisor Latch (High-Byte) */
+#define	UART1_IIR			0xFFC02008	/* Interrupt Identification Register */
+#define	UART1_LCR			0xFFC0200C	/* Line	Control	Register */
+#define	UART1_MCR			0xFFC02010	/* Modem Control Register */
+#define	UART1_LSR			0xFFC02014	/* Line	Status Register */
+#define	UART1_SCR			0xFFC0201C	/* SCR Scratch Register */
+#define	UART1_GCTL			0xFFC02024	/* Global Control Register */
+
+
+/* UART2 Controller		(0xFFC02100 - 0xFFC021FF)	 */
+#define	UART2_THR			0xFFC02100	/* Transmit Holding register */
+#define	UART2_RBR			0xFFC02100	/* Receive Buffer register */
+#define	UART2_DLL			0xFFC02100	/* Divisor Latch (Low-Byte) */
+#define	UART2_IER			0xFFC02104	/* Interrupt Enable Register */
+#define	UART2_DLH			0xFFC02104	/* Divisor Latch (High-Byte) */
+#define	UART2_IIR			0xFFC02108	/* Interrupt Identification Register */
+#define	UART2_LCR			0xFFC0210C	/* Line	Control	Register */
+#define	UART2_MCR			0xFFC02110	/* Modem Control Register */
+#define	UART2_LSR			0xFFC02114	/* Line	Status Register */
+#define	UART2_SCR			0xFFC0211C	/* SCR Scratch Register */
+#define	UART2_GCTL			0xFFC02124	/* Global Control Register */
+
+
+/* Two-Wire Interface 1	(0xFFC02200 - 0xFFC022FF)			 */
+#define	TWI1_CLKDIV			0xFFC02200	/* Serial Clock	Divider	Register */
+#define	TWI1_CONTROL		0xFFC02204	/* TWI1	Master Internal	Time Reference Register */
+#define	TWI1_SLAVE_CTRL		0xFFC02208	/* Slave Mode Control Register */
+#define	TWI1_SLAVE_STAT		0xFFC0220C	/* Slave Mode Status Register */
+#define	TWI1_SLAVE_ADDR		0xFFC02210	/* Slave Mode Address Register */
+#define	TWI1_MASTER_CTRL	0xFFC02214	/* Master Mode Control Register */
+#define	TWI1_MASTER_STAT	0xFFC02218	/* Master Mode Status Register */
+#define	TWI1_MASTER_ADDR	0xFFC0221C	/* Master Mode Address Register */
+#define	TWI1_INT_STAT		0xFFC02220	/* TWI1	Master Interrupt Register */
+#define	TWI1_INT_MASK		0xFFC02224	/* TWI1	Master Interrupt Mask Register */
+#define	TWI1_FIFO_CTRL		0xFFC02228	/* FIFO	Control	Register */
+#define	TWI1_FIFO_STAT		0xFFC0222C	/* FIFO	Status Register */
+#define	TWI1_XMT_DATA8		0xFFC02280	/* FIFO	Transmit Data Single Byte Register */
+#define	TWI1_XMT_DATA16		0xFFC02284	/* FIFO	Transmit Data Double Byte Register */
+#define	TWI1_RCV_DATA8		0xFFC02288	/* FIFO	Receive	Data Single Byte Register */
+#define	TWI1_RCV_DATA16		0xFFC0228C	/* FIFO	Receive	Data Double Byte Register */
+#define TWI1_REGBASE		TWI1_CLKDIV
+
+
+/* the following are for backwards compatibility */
+#define	TWI1_PRESCALE	  TWI1_CONTROL
+#define	TWI1_INT_SRC	  TWI1_INT_STAT
+#define	TWI1_INT_ENABLE	  TWI1_INT_MASK
+
+
+/* SPI1	Controller		(0xFFC02300 - 0xFFC023FF)	 */
+#define	SPI1_CTL			0xFFC02300  /* SPI1 Control Register */
+#define	SPI1_FLG			0xFFC02304  /* SPI1 Flag register */
+#define	SPI1_STAT			0xFFC02308  /* SPI1 Status register */
+#define	SPI1_TDBR			0xFFC0230C  /* SPI1 Transmit Data Buffer Register */
+#define	SPI1_RDBR			0xFFC02310  /* SPI1 Receive Data Buffer	Register */
+#define	SPI1_BAUD			0xFFC02314  /* SPI1 Baud rate Register */
+#define	SPI1_SHADOW			0xFFC02318  /* SPI1_RDBR Shadow	Register */
+#define SPI1_REGBASE			SPI1_CTL
+
+/* SPI2	Controller		(0xFFC02400 - 0xFFC024FF)	 */
+#define	SPI2_CTL			0xFFC02400  /* SPI2 Control Register */
+#define	SPI2_FLG			0xFFC02404  /* SPI2 Flag register */
+#define	SPI2_STAT			0xFFC02408  /* SPI2 Status register */
+#define	SPI2_TDBR			0xFFC0240C  /* SPI2 Transmit Data Buffer Register */
+#define	SPI2_RDBR			0xFFC02410  /* SPI2 Receive Data Buffer	Register */
+#define	SPI2_BAUD			0xFFC02414  /* SPI2 Baud rate Register */
+#define	SPI2_SHADOW			0xFFC02418  /* SPI2_RDBR Shadow	Register */
+#define SPI2_REGBASE			SPI2_CTL
+
+/* SPORT2 Controller		(0xFFC02500 - 0xFFC025FF)			 */
+#define	SPORT2_TCR1			0xFFC02500	/* SPORT2 Transmit Configuration 1 Register */
+#define	SPORT2_TCR2			0xFFC02504	/* SPORT2 Transmit Configuration 2 Register */
+#define	SPORT2_TCLKDIV		0xFFC02508	/* SPORT2 Transmit Clock Divider */
+#define	SPORT2_TFSDIV		0xFFC0250C	/* SPORT2 Transmit Frame Sync Divider */
+#define	SPORT2_TX			0xFFC02510	/* SPORT2 TX Data Register */
+#define	SPORT2_RX			0xFFC02518	/* SPORT2 RX Data Register */
+#define	SPORT2_RCR1			0xFFC02520	/* SPORT2 Transmit Configuration 1 Register */
+#define	SPORT2_RCR2			0xFFC02524	/* SPORT2 Transmit Configuration 2 Register */
+#define	SPORT2_RCLKDIV		0xFFC02528	/* SPORT2 Receive Clock	Divider */
+#define	SPORT2_RFSDIV		0xFFC0252C	/* SPORT2 Receive Frame	Sync Divider */
+#define	SPORT2_STAT			0xFFC02530	/* SPORT2 Status Register */
+#define	SPORT2_CHNL			0xFFC02534	/* SPORT2 Current Channel Register */
+#define	SPORT2_MCMC1		0xFFC02538	/* SPORT2 Multi-Channel	Configuration Register 1 */
+#define	SPORT2_MCMC2		0xFFC0253C	/* SPORT2 Multi-Channel	Configuration Register 2 */
+#define	SPORT2_MTCS0		0xFFC02540	/* SPORT2 Multi-Channel	Transmit Select	Register 0 */
+#define	SPORT2_MTCS1		0xFFC02544	/* SPORT2 Multi-Channel	Transmit Select	Register 1 */
+#define	SPORT2_MTCS2		0xFFC02548	/* SPORT2 Multi-Channel	Transmit Select	Register 2 */
+#define	SPORT2_MTCS3		0xFFC0254C	/* SPORT2 Multi-Channel	Transmit Select	Register 3 */
+#define	SPORT2_MRCS0		0xFFC02550	/* SPORT2 Multi-Channel	Receive	Select Register	0 */
+#define	SPORT2_MRCS1		0xFFC02554	/* SPORT2 Multi-Channel	Receive	Select Register	1 */
+#define	SPORT2_MRCS2		0xFFC02558	/* SPORT2 Multi-Channel	Receive	Select Register	2 */
+#define	SPORT2_MRCS3		0xFFC0255C	/* SPORT2 Multi-Channel	Receive	Select Register	3 */
+
+
+/* SPORT3 Controller		(0xFFC02600 - 0xFFC026FF)			 */
+#define	SPORT3_TCR1			0xFFC02600	/* SPORT3 Transmit Configuration 1 Register */
+#define	SPORT3_TCR2			0xFFC02604	/* SPORT3 Transmit Configuration 2 Register */
+#define	SPORT3_TCLKDIV		0xFFC02608	/* SPORT3 Transmit Clock Divider */
+#define	SPORT3_TFSDIV		0xFFC0260C	/* SPORT3 Transmit Frame Sync Divider */
+#define	SPORT3_TX			0xFFC02610	/* SPORT3 TX Data Register */
+#define	SPORT3_RX			0xFFC02618	/* SPORT3 RX Data Register */
+#define	SPORT3_RCR1			0xFFC02620	/* SPORT3 Transmit Configuration 1 Register */
+#define	SPORT3_RCR2			0xFFC02624	/* SPORT3 Transmit Configuration 2 Register */
+#define	SPORT3_RCLKDIV		0xFFC02628	/* SPORT3 Receive Clock	Divider */
+#define	SPORT3_RFSDIV		0xFFC0262C	/* SPORT3 Receive Frame	Sync Divider */
+#define	SPORT3_STAT			0xFFC02630	/* SPORT3 Status Register */
+#define	SPORT3_CHNL			0xFFC02634	/* SPORT3 Current Channel Register */
+#define	SPORT3_MCMC1		0xFFC02638	/* SPORT3 Multi-Channel	Configuration Register 1 */
+#define	SPORT3_MCMC2		0xFFC0263C	/* SPORT3 Multi-Channel	Configuration Register 2 */
+#define	SPORT3_MTCS0		0xFFC02640	/* SPORT3 Multi-Channel	Transmit Select	Register 0 */
+#define	SPORT3_MTCS1		0xFFC02644	/* SPORT3 Multi-Channel	Transmit Select	Register 1 */
+#define	SPORT3_MTCS2		0xFFC02648	/* SPORT3 Multi-Channel	Transmit Select	Register 2 */
+#define	SPORT3_MTCS3		0xFFC0264C	/* SPORT3 Multi-Channel	Transmit Select	Register 3 */
+#define	SPORT3_MRCS0		0xFFC02650	/* SPORT3 Multi-Channel	Receive	Select Register	0 */
+#define	SPORT3_MRCS1		0xFFC02654	/* SPORT3 Multi-Channel	Receive	Select Register	1 */
+#define	SPORT3_MRCS2		0xFFC02658	/* SPORT3 Multi-Channel	Receive	Select Register	2 */
+#define	SPORT3_MRCS3		0xFFC0265C	/* SPORT3 Multi-Channel	Receive	Select Register	3 */
+
+
+/* Media Transceiver (MXVR)   (0xFFC02700 - 0xFFC028FF) */
+
+#define	MXVR_CONFIG	      0xFFC02700  /* MXVR Configuration	Register */
+#define	MXVR_PLL_CTL_0	      0xFFC02704  /* MXVR Phase	Lock Loop Control Register 0 */
+
+#define	MXVR_STATE_0	      0xFFC02708  /* MXVR State	Register 0 */
+#define	MXVR_STATE_1	      0xFFC0270C  /* MXVR State	Register 1 */
+
+#define	MXVR_INT_STAT_0	      0xFFC02710  /* MXVR Interrupt Status Register 0 */
+#define	MXVR_INT_STAT_1	      0xFFC02714  /* MXVR Interrupt Status Register 1 */
+
+#define	MXVR_INT_EN_0	      0xFFC02718  /* MXVR Interrupt Enable Register 0 */
+#define	MXVR_INT_EN_1	      0xFFC0271C  /* MXVR Interrupt Enable Register 1 */
+
+#define	MXVR_POSITION	      0xFFC02720  /* MXVR Node Position	Register */
+#define	MXVR_MAX_POSITION     0xFFC02724  /* MXVR Maximum Node Position	Register */
+
+#define	MXVR_DELAY	      0xFFC02728  /* MXVR Node Frame Delay Register */
+#define	MXVR_MAX_DELAY	      0xFFC0272C  /* MXVR Maximum Node Frame Delay Register */
+
+#define	MXVR_LADDR	      0xFFC02730  /* MXVR Logical Address Register */
+#define	MXVR_GADDR	      0xFFC02734  /* MXVR Group	Address	Register */
+#define	MXVR_AADDR	      0xFFC02738  /* MXVR Alternate Address Register */
+
+#define	MXVR_ALLOC_0	      0xFFC0273C  /* MXVR Allocation Table Register 0 */
+#define	MXVR_ALLOC_1	      0xFFC02740  /* MXVR Allocation Table Register 1 */
+#define	MXVR_ALLOC_2	      0xFFC02744  /* MXVR Allocation Table Register 2 */
+#define	MXVR_ALLOC_3	      0xFFC02748  /* MXVR Allocation Table Register 3 */
+#define	MXVR_ALLOC_4	      0xFFC0274C  /* MXVR Allocation Table Register 4 */
+#define	MXVR_ALLOC_5	      0xFFC02750  /* MXVR Allocation Table Register 5 */
+#define	MXVR_ALLOC_6	      0xFFC02754  /* MXVR Allocation Table Register 6 */
+#define	MXVR_ALLOC_7	      0xFFC02758  /* MXVR Allocation Table Register 7 */
+#define	MXVR_ALLOC_8	      0xFFC0275C  /* MXVR Allocation Table Register 8 */
+#define	MXVR_ALLOC_9	      0xFFC02760  /* MXVR Allocation Table Register 9 */
+#define	MXVR_ALLOC_10	      0xFFC02764  /* MXVR Allocation Table Register 10 */
+#define	MXVR_ALLOC_11	      0xFFC02768  /* MXVR Allocation Table Register 11 */
+#define	MXVR_ALLOC_12	      0xFFC0276C  /* MXVR Allocation Table Register 12 */
+#define	MXVR_ALLOC_13	      0xFFC02770  /* MXVR Allocation Table Register 13 */
+#define	MXVR_ALLOC_14	      0xFFC02774  /* MXVR Allocation Table Register 14 */
+
+#define	MXVR_SYNC_LCHAN_0     0xFFC02778  /* MXVR Sync Data Logical Channel Assign Register 0 */
+#define	MXVR_SYNC_LCHAN_1     0xFFC0277C  /* MXVR Sync Data Logical Channel Assign Register 1 */
+#define	MXVR_SYNC_LCHAN_2     0xFFC02780  /* MXVR Sync Data Logical Channel Assign Register 2 */
+#define	MXVR_SYNC_LCHAN_3     0xFFC02784  /* MXVR Sync Data Logical Channel Assign Register 3 */
+#define	MXVR_SYNC_LCHAN_4     0xFFC02788  /* MXVR Sync Data Logical Channel Assign Register 4 */
+#define	MXVR_SYNC_LCHAN_5     0xFFC0278C  /* MXVR Sync Data Logical Channel Assign Register 5 */
+#define	MXVR_SYNC_LCHAN_6     0xFFC02790  /* MXVR Sync Data Logical Channel Assign Register 6 */
+#define	MXVR_SYNC_LCHAN_7     0xFFC02794  /* MXVR Sync Data Logical Channel Assign Register 7 */
+
+#define	MXVR_DMA0_CONFIG      0xFFC02798  /* MXVR Sync Data DMA0 Config	Register */
+#define	MXVR_DMA0_START_ADDR  0xFFC0279C  /* MXVR Sync Data DMA0 Start Address Register */
+#define	MXVR_DMA0_COUNT	      0xFFC027A0  /* MXVR Sync Data DMA0 Loop Count Register */
+#define	MXVR_DMA0_CURR_ADDR   0xFFC027A4  /* MXVR Sync Data DMA0 Current Address Register */
+#define	MXVR_DMA0_CURR_COUNT  0xFFC027A8  /* MXVR Sync Data DMA0 Current Loop Count Register */
+
+#define	MXVR_DMA1_CONFIG      0xFFC027AC  /* MXVR Sync Data DMA1 Config	Register */
+#define	MXVR_DMA1_START_ADDR  0xFFC027B0  /* MXVR Sync Data DMA1 Start Address Register */
+#define	MXVR_DMA1_COUNT	      0xFFC027B4  /* MXVR Sync Data DMA1 Loop Count Register */
+#define	MXVR_DMA1_CURR_ADDR   0xFFC027B8  /* MXVR Sync Data DMA1 Current Address Register */
+#define	MXVR_DMA1_CURR_COUNT  0xFFC027BC  /* MXVR Sync Data DMA1 Current Loop Count Register */
+
+#define	MXVR_DMA2_CONFIG      0xFFC027C0  /* MXVR Sync Data DMA2 Config	Register */
+#define	MXVR_DMA2_START_ADDR  0xFFC027C4  /* MXVR Sync Data DMA2 Start Address Register */
+#define	MXVR_DMA2_COUNT	      0xFFC027C8  /* MXVR Sync Data DMA2 Loop Count Register */
+#define	MXVR_DMA2_CURR_ADDR   0xFFC027CC  /* MXVR Sync Data DMA2 Current Address Register */
+#define	MXVR_DMA2_CURR_COUNT  0xFFC027D0  /* MXVR Sync Data DMA2 Current Loop Count Register */
+
+#define	MXVR_DMA3_CONFIG      0xFFC027D4  /* MXVR Sync Data DMA3 Config	Register */
+#define	MXVR_DMA3_START_ADDR  0xFFC027D8  /* MXVR Sync Data DMA3 Start Address Register */
+#define	MXVR_DMA3_COUNT	      0xFFC027DC  /* MXVR Sync Data DMA3 Loop Count Register */
+#define	MXVR_DMA3_CURR_ADDR   0xFFC027E0  /* MXVR Sync Data DMA3 Current Address Register */
+#define	MXVR_DMA3_CURR_COUNT  0xFFC027E4  /* MXVR Sync Data DMA3 Current Loop Count Register */
+
+#define	MXVR_DMA4_CONFIG      0xFFC027E8  /* MXVR Sync Data DMA4 Config	Register */
+#define	MXVR_DMA4_START_ADDR  0xFFC027EC  /* MXVR Sync Data DMA4 Start Address Register */
+#define	MXVR_DMA4_COUNT	      0xFFC027F0  /* MXVR Sync Data DMA4 Loop Count Register */
+#define	MXVR_DMA4_CURR_ADDR   0xFFC027F4  /* MXVR Sync Data DMA4 Current Address Register */
+#define	MXVR_DMA4_CURR_COUNT  0xFFC027F8  /* MXVR Sync Data DMA4 Current Loop Count Register */
+
+#define	MXVR_DMA5_CONFIG      0xFFC027FC  /* MXVR Sync Data DMA5 Config	Register */
+#define	MXVR_DMA5_START_ADDR  0xFFC02800  /* MXVR Sync Data DMA5 Start Address Register */
+#define	MXVR_DMA5_COUNT	      0xFFC02804  /* MXVR Sync Data DMA5 Loop Count Register */
+#define	MXVR_DMA5_CURR_ADDR   0xFFC02808  /* MXVR Sync Data DMA5 Current Address Register */
+#define	MXVR_DMA5_CURR_COUNT  0xFFC0280C  /* MXVR Sync Data DMA5 Current Loop Count Register */
+
+#define	MXVR_DMA6_CONFIG      0xFFC02810  /* MXVR Sync Data DMA6 Config	Register */
+#define	MXVR_DMA6_START_ADDR  0xFFC02814  /* MXVR Sync Data DMA6 Start Address Register */
+#define	MXVR_DMA6_COUNT	      0xFFC02818  /* MXVR Sync Data DMA6 Loop Count Register */
+#define	MXVR_DMA6_CURR_ADDR   0xFFC0281C  /* MXVR Sync Data DMA6 Current Address Register */
+#define	MXVR_DMA6_CURR_COUNT  0xFFC02820  /* MXVR Sync Data DMA6 Current Loop Count Register */
+
+#define	MXVR_DMA7_CONFIG      0xFFC02824  /* MXVR Sync Data DMA7 Config	Register */
+#define	MXVR_DMA7_START_ADDR  0xFFC02828  /* MXVR Sync Data DMA7 Start Address Register */
+#define	MXVR_DMA7_COUNT	      0xFFC0282C  /* MXVR Sync Data DMA7 Loop Count Register */
+#define	MXVR_DMA7_CURR_ADDR   0xFFC02830  /* MXVR Sync Data DMA7 Current Address Register */
+#define	MXVR_DMA7_CURR_COUNT  0xFFC02834  /* MXVR Sync Data DMA7 Current Loop Count Register */
+
+#define	MXVR_AP_CTL	      0xFFC02838  /* MXVR Async	Packet Control Register */
+#define	MXVR_APRB_START_ADDR  0xFFC0283C  /* MXVR Async	Packet RX Buffer Start Addr Register */
+#define	MXVR_APRB_CURR_ADDR   0xFFC02840  /* MXVR Async	Packet RX Buffer Current Addr Register */
+#define	MXVR_APTB_START_ADDR  0xFFC02844  /* MXVR Async	Packet TX Buffer Start Addr Register */
+#define	MXVR_APTB_CURR_ADDR   0xFFC02848  /* MXVR Async	Packet TX Buffer Current Addr Register */
+
+#define	MXVR_CM_CTL	      0xFFC0284C  /* MXVR Control Message Control Register */
+#define	MXVR_CMRB_START_ADDR  0xFFC02850  /* MXVR Control Message RX Buffer Start Addr Register */
+#define	MXVR_CMRB_CURR_ADDR   0xFFC02854  /* MXVR Control Message RX Buffer Current Address */
+#define	MXVR_CMTB_START_ADDR  0xFFC02858  /* MXVR Control Message TX Buffer Start Addr Register */
+#define	MXVR_CMTB_CURR_ADDR   0xFFC0285C  /* MXVR Control Message TX Buffer Current Address */
+
+#define	MXVR_RRDB_START_ADDR  0xFFC02860  /* MXVR Remote Read Buffer Start Addr	Register */
+#define	MXVR_RRDB_CURR_ADDR   0xFFC02864  /* MXVR Remote Read Buffer Current Addr Register */
+
+#define	MXVR_PAT_DATA_0	      0xFFC02868  /* MXVR Pattern Data Register	0 */
+#define	MXVR_PAT_EN_0	      0xFFC0286C  /* MXVR Pattern Enable Register 0 */
+#define	MXVR_PAT_DATA_1	      0xFFC02870  /* MXVR Pattern Data Register	1 */
+#define	MXVR_PAT_EN_1	      0xFFC02874  /* MXVR Pattern Enable Register 1 */
+
+#define	MXVR_FRAME_CNT_0      0xFFC02878  /* MXVR Frame	Counter	0 */
+#define	MXVR_FRAME_CNT_1      0xFFC0287C  /* MXVR Frame	Counter	1 */
+
+#define	MXVR_ROUTING_0	      0xFFC02880  /* MXVR Routing Table	Register 0 */
+#define	MXVR_ROUTING_1	      0xFFC02884  /* MXVR Routing Table	Register 1 */
+#define	MXVR_ROUTING_2	      0xFFC02888  /* MXVR Routing Table	Register 2 */
+#define	MXVR_ROUTING_3	      0xFFC0288C  /* MXVR Routing Table	Register 3 */
+#define	MXVR_ROUTING_4	      0xFFC02890  /* MXVR Routing Table	Register 4 */
+#define	MXVR_ROUTING_5	      0xFFC02894  /* MXVR Routing Table	Register 5 */
+#define	MXVR_ROUTING_6	      0xFFC02898  /* MXVR Routing Table	Register 6 */
+#define	MXVR_ROUTING_7	      0xFFC0289C  /* MXVR Routing Table	Register 7 */
+#define	MXVR_ROUTING_8	      0xFFC028A0  /* MXVR Routing Table	Register 8 */
+#define	MXVR_ROUTING_9	      0xFFC028A4  /* MXVR Routing Table	Register 9 */
+#define	MXVR_ROUTING_10	      0xFFC028A8  /* MXVR Routing Table	Register 10 */
+#define	MXVR_ROUTING_11	      0xFFC028AC  /* MXVR Routing Table	Register 11 */
+#define	MXVR_ROUTING_12	      0xFFC028B0  /* MXVR Routing Table	Register 12 */
+#define	MXVR_ROUTING_13	      0xFFC028B4  /* MXVR Routing Table	Register 13 */
+#define	MXVR_ROUTING_14	      0xFFC028B8  /* MXVR Routing Table	Register 14 */
+
+#define	MXVR_PLL_CTL_1	      0xFFC028BC  /* MXVR Phase	Lock Loop Control Register 1 */
+#define	MXVR_BLOCK_CNT	      0xFFC028C0  /* MXVR Block	Counter */
+#define	MXVR_PLL_CTL_2	      0xFFC028C4  /* MXVR Phase	Lock Loop Control Register 2 */
+
+
+/* CAN Controller		(0xFFC02A00 - 0xFFC02FFF)				 */
+/* For Mailboxes 0-15											 */
+#define	CAN_MC1				0xFFC02A00	/* Mailbox config reg 1	 */
+#define	CAN_MD1				0xFFC02A04	/* Mailbox direction reg 1 */
+#define	CAN_TRS1			0xFFC02A08	/* Transmit Request Set	reg 1 */
+#define	CAN_TRR1			0xFFC02A0C	/* Transmit Request Reset reg 1 */
+#define	CAN_TA1				0xFFC02A10	/* Transmit Acknowledge	reg 1 */
+#define	CAN_AA1				0xFFC02A14	/* Transmit Abort Acknowledge reg 1 */
+#define	CAN_RMP1			0xFFC02A18	/* Receive Message Pending reg 1 */
+#define	CAN_RML1			0xFFC02A1C	/* Receive Message Lost	reg 1 */
+#define	CAN_MBTIF1			0xFFC02A20	/* Mailbox Transmit Interrupt Flag reg 1 */
+#define	CAN_MBRIF1			0xFFC02A24	/* Mailbox Receive  Interrupt Flag reg 1 */
+#define	CAN_MBIM1			0xFFC02A28	/* Mailbox Interrupt Mask reg 1 */
+#define	CAN_RFH1			0xFFC02A2C	/* Remote Frame	Handling reg 1 */
+#define	CAN_OPSS1			0xFFC02A30	/* Overwrite Protection	Single Shot Xmission reg 1 */
+
+/* For Mailboxes 16-31											 */
+#define	CAN_MC2				0xFFC02A40	/* Mailbox config reg 2	 */
+#define	CAN_MD2				0xFFC02A44	/* Mailbox direction reg 2 */
+#define	CAN_TRS2			0xFFC02A48	/* Transmit Request Set	reg 2 */
+#define	CAN_TRR2			0xFFC02A4C	/* Transmit Request Reset reg 2 */
+#define	CAN_TA2				0xFFC02A50	/* Transmit Acknowledge	reg 2 */
+#define	CAN_AA2				0xFFC02A54	/* Transmit Abort Acknowledge reg 2 */
+#define	CAN_RMP2			0xFFC02A58	/* Receive Message Pending reg 2 */
+#define	CAN_RML2			0xFFC02A5C	/* Receive Message Lost	reg 2 */
+#define	CAN_MBTIF2			0xFFC02A60	/* Mailbox Transmit Interrupt Flag reg 2 */
+#define	CAN_MBRIF2			0xFFC02A64	/* Mailbox Receive  Interrupt Flag reg 2 */
+#define	CAN_MBIM2			0xFFC02A68	/* Mailbox Interrupt Mask reg 2 */
+#define	CAN_RFH2			0xFFC02A6C	/* Remote Frame	Handling reg 2 */
+#define	CAN_OPSS2			0xFFC02A70	/* Overwrite Protection	Single Shot Xmission reg 2 */
+
+#define	CAN_CLOCK			0xFFC02A80	/* Bit Timing Configuration register 0 */
+#define	CAN_TIMING			0xFFC02A84	/* Bit Timing Configuration register 1 */
+
+#define	CAN_DEBUG			0xFFC02A88	/* Debug Register		 */
+/* the following is for	backwards compatibility */
+#define	CAN_CNF		 CAN_DEBUG
+
+#define	CAN_STATUS			0xFFC02A8C	/* Global Status Register */
+#define	CAN_CEC				0xFFC02A90	/* Error Counter Register */
+#define	CAN_GIS				0xFFC02A94	/* Global Interrupt Status Register */
+#define	CAN_GIM				0xFFC02A98	/* Global Interrupt Mask Register */
+#define	CAN_GIF				0xFFC02A9C	/* Global Interrupt Flag Register */
+#define	CAN_CONTROL			0xFFC02AA0	/* Master Control Register */
+#define	CAN_INTR			0xFFC02AA4	/* Interrupt Pending Register */
+#define	CAN_MBTD			0xFFC02AAC	/* Mailbox Temporary Disable Feature */
+#define	CAN_EWR				0xFFC02AB0	/* Programmable	Warning	Level */
+#define	CAN_ESR				0xFFC02AB4	/* Error Status	Register */
+#define	CAN_UCCNT			0xFFC02AC4	/* Universal Counter	 */
+#define	CAN_UCRC			0xFFC02AC8	/* Universal Counter Reload/Capture Register */
+#define	CAN_UCCNF			0xFFC02ACC	/* Universal Counter Configuration Register */
+
+/* Mailbox Acceptance Masks					 */
+#define	CAN_AM00L			0xFFC02B00	/* Mailbox 0 Low Acceptance Mask */
+#define	CAN_AM00H			0xFFC02B04	/* Mailbox 0 High Acceptance Mask */
+#define	CAN_AM01L			0xFFC02B08	/* Mailbox 1 Low Acceptance Mask */
+#define	CAN_AM01H			0xFFC02B0C	/* Mailbox 1 High Acceptance Mask */
+#define	CAN_AM02L			0xFFC02B10	/* Mailbox 2 Low Acceptance Mask */
+#define	CAN_AM02H			0xFFC02B14	/* Mailbox 2 High Acceptance Mask */
+#define	CAN_AM03L			0xFFC02B18	/* Mailbox 3 Low Acceptance Mask */
+#define	CAN_AM03H			0xFFC02B1C	/* Mailbox 3 High Acceptance Mask */
+#define	CAN_AM04L			0xFFC02B20	/* Mailbox 4 Low Acceptance Mask */
+#define	CAN_AM04H			0xFFC02B24	/* Mailbox 4 High Acceptance Mask */
+#define	CAN_AM05L			0xFFC02B28	/* Mailbox 5 Low Acceptance Mask */
+#define	CAN_AM05H			0xFFC02B2C	/* Mailbox 5 High Acceptance Mask */
+#define	CAN_AM06L			0xFFC02B30	/* Mailbox 6 Low Acceptance Mask */
+#define	CAN_AM06H			0xFFC02B34	/* Mailbox 6 High Acceptance Mask */
+#define	CAN_AM07L			0xFFC02B38	/* Mailbox 7 Low Acceptance Mask */
+#define	CAN_AM07H			0xFFC02B3C	/* Mailbox 7 High Acceptance Mask */
+#define	CAN_AM08L			0xFFC02B40	/* Mailbox 8 Low Acceptance Mask */
+#define	CAN_AM08H			0xFFC02B44	/* Mailbox 8 High Acceptance Mask */
+#define	CAN_AM09L			0xFFC02B48	/* Mailbox 9 Low Acceptance Mask */
+#define	CAN_AM09H			0xFFC02B4C	/* Mailbox 9 High Acceptance Mask */
+#define	CAN_AM10L			0xFFC02B50	/* Mailbox 10 Low Acceptance Mask */
+#define	CAN_AM10H			0xFFC02B54	/* Mailbox 10 High Acceptance Mask */
+#define	CAN_AM11L			0xFFC02B58	/* Mailbox 11 Low Acceptance Mask */
+#define	CAN_AM11H			0xFFC02B5C	/* Mailbox 11 High Acceptance Mask */
+#define	CAN_AM12L			0xFFC02B60	/* Mailbox 12 Low Acceptance Mask */
+#define	CAN_AM12H			0xFFC02B64	/* Mailbox 12 High Acceptance Mask */
+#define	CAN_AM13L			0xFFC02B68	/* Mailbox 13 Low Acceptance Mask */
+#define	CAN_AM13H			0xFFC02B6C	/* Mailbox 13 High Acceptance Mask */
+#define	CAN_AM14L			0xFFC02B70	/* Mailbox 14 Low Acceptance Mask */
+#define	CAN_AM14H			0xFFC02B74	/* Mailbox 14 High Acceptance Mask */
+#define	CAN_AM15L			0xFFC02B78	/* Mailbox 15 Low Acceptance Mask */
+#define	CAN_AM15H			0xFFC02B7C	/* Mailbox 15 High Acceptance Mask */
+
+#define	CAN_AM16L			0xFFC02B80	/* Mailbox 16 Low Acceptance Mask */
+#define	CAN_AM16H			0xFFC02B84	/* Mailbox 16 High Acceptance Mask */
+#define	CAN_AM17L			0xFFC02B88	/* Mailbox 17 Low Acceptance Mask */
+#define	CAN_AM17H			0xFFC02B8C	/* Mailbox 17 High Acceptance Mask */
+#define	CAN_AM18L			0xFFC02B90	/* Mailbox 18 Low Acceptance Mask */
+#define	CAN_AM18H			0xFFC02B94	/* Mailbox 18 High Acceptance Mask */
+#define	CAN_AM19L			0xFFC02B98	/* Mailbox 19 Low Acceptance Mask */
+#define	CAN_AM19H			0xFFC02B9C	/* Mailbox 19 High Acceptance Mask */
+#define	CAN_AM20L			0xFFC02BA0	/* Mailbox 20 Low Acceptance Mask */
+#define	CAN_AM20H			0xFFC02BA4	/* Mailbox 20 High Acceptance Mask */
+#define	CAN_AM21L			0xFFC02BA8	/* Mailbox 21 Low Acceptance Mask */
+#define	CAN_AM21H			0xFFC02BAC	/* Mailbox 21 High Acceptance Mask */
+#define	CAN_AM22L			0xFFC02BB0	/* Mailbox 22 Low Acceptance Mask */
+#define	CAN_AM22H			0xFFC02BB4	/* Mailbox 22 High Acceptance Mask */
+#define	CAN_AM23L			0xFFC02BB8	/* Mailbox 23 Low Acceptance Mask */
+#define	CAN_AM23H			0xFFC02BBC	/* Mailbox 23 High Acceptance Mask */
+#define	CAN_AM24L			0xFFC02BC0	/* Mailbox 24 Low Acceptance Mask */
+#define	CAN_AM24H			0xFFC02BC4	/* Mailbox 24 High Acceptance Mask */
+#define	CAN_AM25L			0xFFC02BC8	/* Mailbox 25 Low Acceptance Mask */
+#define	CAN_AM25H			0xFFC02BCC	/* Mailbox 25 High Acceptance Mask */
+#define	CAN_AM26L			0xFFC02BD0	/* Mailbox 26 Low Acceptance Mask */
+#define	CAN_AM26H			0xFFC02BD4	/* Mailbox 26 High Acceptance Mask */
+#define	CAN_AM27L			0xFFC02BD8	/* Mailbox 27 Low Acceptance Mask */
+#define	CAN_AM27H			0xFFC02BDC	/* Mailbox 27 High Acceptance Mask */
+#define	CAN_AM28L			0xFFC02BE0	/* Mailbox 28 Low Acceptance Mask */
+#define	CAN_AM28H			0xFFC02BE4	/* Mailbox 28 High Acceptance Mask */
+#define	CAN_AM29L			0xFFC02BE8	/* Mailbox 29 Low Acceptance Mask */
+#define	CAN_AM29H			0xFFC02BEC	/* Mailbox 29 High Acceptance Mask */
+#define	CAN_AM30L			0xFFC02BF0	/* Mailbox 30 Low Acceptance Mask */
+#define	CAN_AM30H			0xFFC02BF4	/* Mailbox 30 High Acceptance Mask */
+#define	CAN_AM31L			0xFFC02BF8	/* Mailbox 31 Low Acceptance Mask */
+#define	CAN_AM31H			0xFFC02BFC	/* Mailbox 31 High Acceptance Mask */
+
+/* CAN Acceptance Mask Macros */
+#define	CAN_AM_L(x)			(CAN_AM00L+((x)*0x8))
+#define	CAN_AM_H(x)			(CAN_AM00H+((x)*0x8))
+
+/* Mailbox Registers									 */
+#define	CAN_MB00_DATA0		0xFFC02C00	/* Mailbox 0 Data Word 0 [15:0]	Register */
+#define	CAN_MB00_DATA1		0xFFC02C04	/* Mailbox 0 Data Word 1 [31:16] Register */
+#define	CAN_MB00_DATA2		0xFFC02C08	/* Mailbox 0 Data Word 2 [47:32] Register */
+#define	CAN_MB00_DATA3		0xFFC02C0C	/* Mailbox 0 Data Word 3 [63:48] Register */
+#define	CAN_MB00_LENGTH		0xFFC02C10	/* Mailbox 0 Data Length Code Register */
+#define	CAN_MB00_TIMESTAMP	0xFFC02C14	/* Mailbox 0 Time Stamp	Value Register */
+#define	CAN_MB00_ID0		0xFFC02C18	/* Mailbox 0 Identifier	Low Register */
+#define	CAN_MB00_ID1		0xFFC02C1C	/* Mailbox 0 Identifier	High Register */
+
+#define	CAN_MB01_DATA0		0xFFC02C20	/* Mailbox 1 Data Word 0 [15:0]	Register */
+#define	CAN_MB01_DATA1		0xFFC02C24	/* Mailbox 1 Data Word 1 [31:16] Register */
+#define	CAN_MB01_DATA2		0xFFC02C28	/* Mailbox 1 Data Word 2 [47:32] Register */
+#define	CAN_MB01_DATA3		0xFFC02C2C	/* Mailbox 1 Data Word 3 [63:48] Register */
+#define	CAN_MB01_LENGTH		0xFFC02C30	/* Mailbox 1 Data Length Code Register */
+#define	CAN_MB01_TIMESTAMP	0xFFC02C34	/* Mailbox 1 Time Stamp	Value Register */
+#define	CAN_MB01_ID0		0xFFC02C38	/* Mailbox 1 Identifier	Low Register */
+#define	CAN_MB01_ID1		0xFFC02C3C	/* Mailbox 1 Identifier	High Register */
+
+#define	CAN_MB02_DATA0		0xFFC02C40	/* Mailbox 2 Data Word 0 [15:0]	Register */
+#define	CAN_MB02_DATA1		0xFFC02C44	/* Mailbox 2 Data Word 1 [31:16] Register */
+#define	CAN_MB02_DATA2		0xFFC02C48	/* Mailbox 2 Data Word 2 [47:32] Register */
+#define	CAN_MB02_DATA3		0xFFC02C4C	/* Mailbox 2 Data Word 3 [63:48] Register */
+#define	CAN_MB02_LENGTH		0xFFC02C50	/* Mailbox 2 Data Length Code Register */
+#define	CAN_MB02_TIMESTAMP	0xFFC02C54	/* Mailbox 2 Time Stamp	Value Register */
+#define	CAN_MB02_ID0		0xFFC02C58	/* Mailbox 2 Identifier	Low Register */
+#define	CAN_MB02_ID1		0xFFC02C5C	/* Mailbox 2 Identifier	High Register */
+
+#define	CAN_MB03_DATA0		0xFFC02C60	/* Mailbox 3 Data Word 0 [15:0]	Register */
+#define	CAN_MB03_DATA1		0xFFC02C64	/* Mailbox 3 Data Word 1 [31:16] Register */
+#define	CAN_MB03_DATA2		0xFFC02C68	/* Mailbox 3 Data Word 2 [47:32] Register */
+#define	CAN_MB03_DATA3		0xFFC02C6C	/* Mailbox 3 Data Word 3 [63:48] Register */
+#define	CAN_MB03_LENGTH		0xFFC02C70	/* Mailbox 3 Data Length Code Register */
+#define	CAN_MB03_TIMESTAMP	0xFFC02C74	/* Mailbox 3 Time Stamp	Value Register */
+#define	CAN_MB03_ID0		0xFFC02C78	/* Mailbox 3 Identifier	Low Register */
+#define	CAN_MB03_ID1		0xFFC02C7C	/* Mailbox 3 Identifier	High Register */
+
+#define	CAN_MB04_DATA0		0xFFC02C80	/* Mailbox 4 Data Word 0 [15:0]	Register */
+#define	CAN_MB04_DATA1		0xFFC02C84	/* Mailbox 4 Data Word 1 [31:16] Register */
+#define	CAN_MB04_DATA2		0xFFC02C88	/* Mailbox 4 Data Word 2 [47:32] Register */
+#define	CAN_MB04_DATA3		0xFFC02C8C	/* Mailbox 4 Data Word 3 [63:48] Register */
+#define	CAN_MB04_LENGTH		0xFFC02C90	/* Mailbox 4 Data Length Code Register */
+#define	CAN_MB04_TIMESTAMP	0xFFC02C94	/* Mailbox 4 Time Stamp	Value Register */
+#define	CAN_MB04_ID0		0xFFC02C98	/* Mailbox 4 Identifier	Low Register */
+#define	CAN_MB04_ID1		0xFFC02C9C	/* Mailbox 4 Identifier	High Register */
+
+#define	CAN_MB05_DATA0		0xFFC02CA0	/* Mailbox 5 Data Word 0 [15:0]	Register */
+#define	CAN_MB05_DATA1		0xFFC02CA4	/* Mailbox 5 Data Word 1 [31:16] Register */
+#define	CAN_MB05_DATA2		0xFFC02CA8	/* Mailbox 5 Data Word 2 [47:32] Register */
+#define	CAN_MB05_DATA3		0xFFC02CAC	/* Mailbox 5 Data Word 3 [63:48] Register */
+#define	CAN_MB05_LENGTH		0xFFC02CB0	/* Mailbox 5 Data Length Code Register */
+#define	CAN_MB05_TIMESTAMP	0xFFC02CB4	/* Mailbox 5 Time Stamp	Value Register */
+#define	CAN_MB05_ID0		0xFFC02CB8	/* Mailbox 5 Identifier	Low Register */
+#define	CAN_MB05_ID1		0xFFC02CBC	/* Mailbox 5 Identifier	High Register */
+
+#define	CAN_MB06_DATA0		0xFFC02CC0	/* Mailbox 6 Data Word 0 [15:0]	Register */
+#define	CAN_MB06_DATA1		0xFFC02CC4	/* Mailbox 6 Data Word 1 [31:16] Register */
+#define	CAN_MB06_DATA2		0xFFC02CC8	/* Mailbox 6 Data Word 2 [47:32] Register */
+#define	CAN_MB06_DATA3		0xFFC02CCC	/* Mailbox 6 Data Word 3 [63:48] Register */
+#define	CAN_MB06_LENGTH		0xFFC02CD0	/* Mailbox 6 Data Length Code Register */
+#define	CAN_MB06_TIMESTAMP	0xFFC02CD4	/* Mailbox 6 Time Stamp	Value Register */
+#define	CAN_MB06_ID0		0xFFC02CD8	/* Mailbox 6 Identifier	Low Register */
+#define	CAN_MB06_ID1		0xFFC02CDC	/* Mailbox 6 Identifier	High Register */
+
+#define	CAN_MB07_DATA0		0xFFC02CE0	/* Mailbox 7 Data Word 0 [15:0]	Register */
+#define	CAN_MB07_DATA1		0xFFC02CE4	/* Mailbox 7 Data Word 1 [31:16] Register */
+#define	CAN_MB07_DATA2		0xFFC02CE8	/* Mailbox 7 Data Word 2 [47:32] Register */
+#define	CAN_MB07_DATA3		0xFFC02CEC	/* Mailbox 7 Data Word 3 [63:48] Register */
+#define	CAN_MB07_LENGTH		0xFFC02CF0	/* Mailbox 7 Data Length Code Register */
+#define	CAN_MB07_TIMESTAMP	0xFFC02CF4	/* Mailbox 7 Time Stamp	Value Register */
+#define	CAN_MB07_ID0		0xFFC02CF8	/* Mailbox 7 Identifier	Low Register */
+#define	CAN_MB07_ID1		0xFFC02CFC	/* Mailbox 7 Identifier	High Register */
+
+#define	CAN_MB08_DATA0		0xFFC02D00	/* Mailbox 8 Data Word 0 [15:0]	Register */
+#define	CAN_MB08_DATA1		0xFFC02D04	/* Mailbox 8 Data Word 1 [31:16] Register */
+#define	CAN_MB08_DATA2		0xFFC02D08	/* Mailbox 8 Data Word 2 [47:32] Register */
+#define	CAN_MB08_DATA3		0xFFC02D0C	/* Mailbox 8 Data Word 3 [63:48] Register */
+#define	CAN_MB08_LENGTH		0xFFC02D10	/* Mailbox 8 Data Length Code Register */
+#define	CAN_MB08_TIMESTAMP	0xFFC02D14	/* Mailbox 8 Time Stamp	Value Register */
+#define	CAN_MB08_ID0		0xFFC02D18	/* Mailbox 8 Identifier	Low Register */
+#define	CAN_MB08_ID1		0xFFC02D1C	/* Mailbox 8 Identifier	High Register */
+
+#define	CAN_MB09_DATA0		0xFFC02D20	/* Mailbox 9 Data Word 0 [15:0]	Register */
+#define	CAN_MB09_DATA1		0xFFC02D24	/* Mailbox 9 Data Word 1 [31:16] Register */
+#define	CAN_MB09_DATA2		0xFFC02D28	/* Mailbox 9 Data Word 2 [47:32] Register */
+#define	CAN_MB09_DATA3		0xFFC02D2C	/* Mailbox 9 Data Word 3 [63:48] Register */
+#define	CAN_MB09_LENGTH		0xFFC02D30	/* Mailbox 9 Data Length Code Register */
+#define	CAN_MB09_TIMESTAMP	0xFFC02D34	/* Mailbox 9 Time Stamp	Value Register */
+#define	CAN_MB09_ID0		0xFFC02D38	/* Mailbox 9 Identifier	Low Register */
+#define	CAN_MB09_ID1		0xFFC02D3C	/* Mailbox 9 Identifier	High Register */
+
+#define	CAN_MB10_DATA0		0xFFC02D40	/* Mailbox 10 Data Word	0 [15:0] Register */
+#define	CAN_MB10_DATA1		0xFFC02D44	/* Mailbox 10 Data Word	1 [31:16] Register */
+#define	CAN_MB10_DATA2		0xFFC02D48	/* Mailbox 10 Data Word	2 [47:32] Register */
+#define	CAN_MB10_DATA3		0xFFC02D4C	/* Mailbox 10 Data Word	3 [63:48] Register */
+#define	CAN_MB10_LENGTH		0xFFC02D50	/* Mailbox 10 Data Length Code Register */
+#define	CAN_MB10_TIMESTAMP	0xFFC02D54	/* Mailbox 10 Time Stamp Value Register */
+#define	CAN_MB10_ID0		0xFFC02D58	/* Mailbox 10 Identifier Low Register */
+#define	CAN_MB10_ID1		0xFFC02D5C	/* Mailbox 10 Identifier High Register */
+
+#define	CAN_MB11_DATA0		0xFFC02D60	/* Mailbox 11 Data Word	0 [15:0] Register */
+#define	CAN_MB11_DATA1		0xFFC02D64	/* Mailbox 11 Data Word	1 [31:16] Register */
+#define	CAN_MB11_DATA2		0xFFC02D68	/* Mailbox 11 Data Word	2 [47:32] Register */
+#define	CAN_MB11_DATA3		0xFFC02D6C	/* Mailbox 11 Data Word	3 [63:48] Register */
+#define	CAN_MB11_LENGTH		0xFFC02D70	/* Mailbox 11 Data Length Code Register */
+#define	CAN_MB11_TIMESTAMP	0xFFC02D74	/* Mailbox 11 Time Stamp Value Register */
+#define	CAN_MB11_ID0		0xFFC02D78	/* Mailbox 11 Identifier Low Register */
+#define	CAN_MB11_ID1		0xFFC02D7C	/* Mailbox 11 Identifier High Register */
+
+#define	CAN_MB12_DATA0		0xFFC02D80	/* Mailbox 12 Data Word	0 [15:0] Register */
+#define	CAN_MB12_DATA1		0xFFC02D84	/* Mailbox 12 Data Word	1 [31:16] Register */
+#define	CAN_MB12_DATA2		0xFFC02D88	/* Mailbox 12 Data Word	2 [47:32] Register */
+#define	CAN_MB12_DATA3		0xFFC02D8C	/* Mailbox 12 Data Word	3 [63:48] Register */
+#define	CAN_MB12_LENGTH		0xFFC02D90	/* Mailbox 12 Data Length Code Register */
+#define	CAN_MB12_TIMESTAMP	0xFFC02D94	/* Mailbox 12 Time Stamp Value Register */
+#define	CAN_MB12_ID0		0xFFC02D98	/* Mailbox 12 Identifier Low Register */
+#define	CAN_MB12_ID1		0xFFC02D9C	/* Mailbox 12 Identifier High Register */
+
+#define	CAN_MB13_DATA0		0xFFC02DA0	/* Mailbox 13 Data Word	0 [15:0] Register */
+#define	CAN_MB13_DATA1		0xFFC02DA4	/* Mailbox 13 Data Word	1 [31:16] Register */
+#define	CAN_MB13_DATA2		0xFFC02DA8	/* Mailbox 13 Data Word	2 [47:32] Register */
+#define	CAN_MB13_DATA3		0xFFC02DAC	/* Mailbox 13 Data Word	3 [63:48] Register */
+#define	CAN_MB13_LENGTH		0xFFC02DB0	/* Mailbox 13 Data Length Code Register */
+#define	CAN_MB13_TIMESTAMP	0xFFC02DB4	/* Mailbox 13 Time Stamp Value Register */
+#define	CAN_MB13_ID0		0xFFC02DB8	/* Mailbox 13 Identifier Low Register */
+#define	CAN_MB13_ID1		0xFFC02DBC	/* Mailbox 13 Identifier High Register */
+
+#define	CAN_MB14_DATA0		0xFFC02DC0	/* Mailbox 14 Data Word	0 [15:0] Register */
+#define	CAN_MB14_DATA1		0xFFC02DC4	/* Mailbox 14 Data Word	1 [31:16] Register */
+#define	CAN_MB14_DATA2		0xFFC02DC8	/* Mailbox 14 Data Word	2 [47:32] Register */
+#define	CAN_MB14_DATA3		0xFFC02DCC	/* Mailbox 14 Data Word	3 [63:48] Register */
+#define	CAN_MB14_LENGTH		0xFFC02DD0	/* Mailbox 14 Data Length Code Register */
+#define	CAN_MB14_TIMESTAMP	0xFFC02DD4	/* Mailbox 14 Time Stamp Value Register */
+#define	CAN_MB14_ID0		0xFFC02DD8	/* Mailbox 14 Identifier Low Register */
+#define	CAN_MB14_ID1		0xFFC02DDC	/* Mailbox 14 Identifier High Register */
+
+#define	CAN_MB15_DATA0		0xFFC02DE0	/* Mailbox 15 Data Word	0 [15:0] Register */
+#define	CAN_MB15_DATA1		0xFFC02DE4	/* Mailbox 15 Data Word	1 [31:16] Register */
+#define	CAN_MB15_DATA2		0xFFC02DE8	/* Mailbox 15 Data Word	2 [47:32] Register */
+#define	CAN_MB15_DATA3		0xFFC02DEC	/* Mailbox 15 Data Word	3 [63:48] Register */
+#define	CAN_MB15_LENGTH		0xFFC02DF0	/* Mailbox 15 Data Length Code Register */
+#define	CAN_MB15_TIMESTAMP	0xFFC02DF4	/* Mailbox 15 Time Stamp Value Register */
+#define	CAN_MB15_ID0		0xFFC02DF8	/* Mailbox 15 Identifier Low Register */
+#define	CAN_MB15_ID1		0xFFC02DFC	/* Mailbox 15 Identifier High Register */
+
+#define	CAN_MB16_DATA0		0xFFC02E00	/* Mailbox 16 Data Word	0 [15:0] Register */
+#define	CAN_MB16_DATA1		0xFFC02E04	/* Mailbox 16 Data Word	1 [31:16] Register */
+#define	CAN_MB16_DATA2		0xFFC02E08	/* Mailbox 16 Data Word	2 [47:32] Register */
+#define	CAN_MB16_DATA3		0xFFC02E0C	/* Mailbox 16 Data Word	3 [63:48] Register */
+#define	CAN_MB16_LENGTH		0xFFC02E10	/* Mailbox 16 Data Length Code Register */
+#define	CAN_MB16_TIMESTAMP	0xFFC02E14	/* Mailbox 16 Time Stamp Value Register */
+#define	CAN_MB16_ID0		0xFFC02E18	/* Mailbox 16 Identifier Low Register */
+#define	CAN_MB16_ID1		0xFFC02E1C	/* Mailbox 16 Identifier High Register */
+
+#define	CAN_MB17_DATA0		0xFFC02E20	/* Mailbox 17 Data Word	0 [15:0] Register */
+#define	CAN_MB17_DATA1		0xFFC02E24	/* Mailbox 17 Data Word	1 [31:16] Register */
+#define	CAN_MB17_DATA2		0xFFC02E28	/* Mailbox 17 Data Word	2 [47:32] Register */
+#define	CAN_MB17_DATA3		0xFFC02E2C	/* Mailbox 17 Data Word	3 [63:48] Register */
+#define	CAN_MB17_LENGTH		0xFFC02E30	/* Mailbox 17 Data Length Code Register */
+#define	CAN_MB17_TIMESTAMP	0xFFC02E34	/* Mailbox 17 Time Stamp Value Register */
+#define	CAN_MB17_ID0		0xFFC02E38	/* Mailbox 17 Identifier Low Register */
+#define	CAN_MB17_ID1		0xFFC02E3C	/* Mailbox 17 Identifier High Register */
+
+#define	CAN_MB18_DATA0		0xFFC02E40	/* Mailbox 18 Data Word	0 [15:0] Register */
+#define	CAN_MB18_DATA1		0xFFC02E44	/* Mailbox 18 Data Word	1 [31:16] Register */
+#define	CAN_MB18_DATA2		0xFFC02E48	/* Mailbox 18 Data Word	2 [47:32] Register */
+#define	CAN_MB18_DATA3		0xFFC02E4C	/* Mailbox 18 Data Word	3 [63:48] Register */
+#define	CAN_MB18_LENGTH		0xFFC02E50	/* Mailbox 18 Data Length Code Register */
+#define	CAN_MB18_TIMESTAMP	0xFFC02E54	/* Mailbox 18 Time Stamp Value Register */
+#define	CAN_MB18_ID0		0xFFC02E58	/* Mailbox 18 Identifier Low Register */
+#define	CAN_MB18_ID1		0xFFC02E5C	/* Mailbox 18 Identifier High Register */
+
+#define	CAN_MB19_DATA0		0xFFC02E60	/* Mailbox 19 Data Word	0 [15:0] Register */
+#define	CAN_MB19_DATA1		0xFFC02E64	/* Mailbox 19 Data Word	1 [31:16] Register */
+#define	CAN_MB19_DATA2		0xFFC02E68	/* Mailbox 19 Data Word	2 [47:32] Register */
+#define	CAN_MB19_DATA3		0xFFC02E6C	/* Mailbox 19 Data Word	3 [63:48] Register */
+#define	CAN_MB19_LENGTH		0xFFC02E70	/* Mailbox 19 Data Length Code Register */
+#define	CAN_MB19_TIMESTAMP	0xFFC02E74	/* Mailbox 19 Time Stamp Value Register */
+#define	CAN_MB19_ID0		0xFFC02E78	/* Mailbox 19 Identifier Low Register */
+#define	CAN_MB19_ID1		0xFFC02E7C	/* Mailbox 19 Identifier High Register */
+
+#define	CAN_MB20_DATA0		0xFFC02E80	/* Mailbox 20 Data Word	0 [15:0] Register */
+#define	CAN_MB20_DATA1		0xFFC02E84	/* Mailbox 20 Data Word	1 [31:16] Register */
+#define	CAN_MB20_DATA2		0xFFC02E88	/* Mailbox 20 Data Word	2 [47:32] Register */
+#define	CAN_MB20_DATA3		0xFFC02E8C	/* Mailbox 20 Data Word	3 [63:48] Register */
+#define	CAN_MB20_LENGTH		0xFFC02E90	/* Mailbox 20 Data Length Code Register */
+#define	CAN_MB20_TIMESTAMP	0xFFC02E94	/* Mailbox 20 Time Stamp Value Register */
+#define	CAN_MB20_ID0		0xFFC02E98	/* Mailbox 20 Identifier Low Register */
+#define	CAN_MB20_ID1		0xFFC02E9C	/* Mailbox 20 Identifier High Register */
+
+#define	CAN_MB21_DATA0		0xFFC02EA0	/* Mailbox 21 Data Word	0 [15:0] Register */
+#define	CAN_MB21_DATA1		0xFFC02EA4	/* Mailbox 21 Data Word	1 [31:16] Register */
+#define	CAN_MB21_DATA2		0xFFC02EA8	/* Mailbox 21 Data Word	2 [47:32] Register */
+#define	CAN_MB21_DATA3		0xFFC02EAC	/* Mailbox 21 Data Word	3 [63:48] Register */
+#define	CAN_MB21_LENGTH		0xFFC02EB0	/* Mailbox 21 Data Length Code Register */
+#define	CAN_MB21_TIMESTAMP	0xFFC02EB4	/* Mailbox 21 Time Stamp Value Register */
+#define	CAN_MB21_ID0		0xFFC02EB8	/* Mailbox 21 Identifier Low Register */
+#define	CAN_MB21_ID1		0xFFC02EBC	/* Mailbox 21 Identifier High Register */
+
+#define	CAN_MB22_DATA0		0xFFC02EC0	/* Mailbox 22 Data Word	0 [15:0] Register */
+#define	CAN_MB22_DATA1		0xFFC02EC4	/* Mailbox 22 Data Word	1 [31:16] Register */
+#define	CAN_MB22_DATA2		0xFFC02EC8	/* Mailbox 22 Data Word	2 [47:32] Register */
+#define	CAN_MB22_DATA3		0xFFC02ECC	/* Mailbox 22 Data Word	3 [63:48] Register */
+#define	CAN_MB22_LENGTH		0xFFC02ED0	/* Mailbox 22 Data Length Code Register */
+#define	CAN_MB22_TIMESTAMP	0xFFC02ED4	/* Mailbox 22 Time Stamp Value Register */
+#define	CAN_MB22_ID0		0xFFC02ED8	/* Mailbox 22 Identifier Low Register */
+#define	CAN_MB22_ID1		0xFFC02EDC	/* Mailbox 22 Identifier High Register */
+
+#define	CAN_MB23_DATA0		0xFFC02EE0	/* Mailbox 23 Data Word	0 [15:0] Register */
+#define	CAN_MB23_DATA1		0xFFC02EE4	/* Mailbox 23 Data Word	1 [31:16] Register */
+#define	CAN_MB23_DATA2		0xFFC02EE8	/* Mailbox 23 Data Word	2 [47:32] Register */
+#define	CAN_MB23_DATA3		0xFFC02EEC	/* Mailbox 23 Data Word	3 [63:48] Register */
+#define	CAN_MB23_LENGTH		0xFFC02EF0	/* Mailbox 23 Data Length Code Register */
+#define	CAN_MB23_TIMESTAMP	0xFFC02EF4	/* Mailbox 23 Time Stamp Value Register */
+#define	CAN_MB23_ID0		0xFFC02EF8	/* Mailbox 23 Identifier Low Register */
+#define	CAN_MB23_ID1		0xFFC02EFC	/* Mailbox 23 Identifier High Register */
+
+#define	CAN_MB24_DATA0		0xFFC02F00	/* Mailbox 24 Data Word	0 [15:0] Register */
+#define	CAN_MB24_DATA1		0xFFC02F04	/* Mailbox 24 Data Word	1 [31:16] Register */
+#define	CAN_MB24_DATA2		0xFFC02F08	/* Mailbox 24 Data Word	2 [47:32] Register */
+#define	CAN_MB24_DATA3		0xFFC02F0C	/* Mailbox 24 Data Word	3 [63:48] Register */
+#define	CAN_MB24_LENGTH		0xFFC02F10	/* Mailbox 24 Data Length Code Register */
+#define	CAN_MB24_TIMESTAMP	0xFFC02F14	/* Mailbox 24 Time Stamp Value Register */
+#define	CAN_MB24_ID0		0xFFC02F18	/* Mailbox 24 Identifier Low Register */
+#define	CAN_MB24_ID1		0xFFC02F1C	/* Mailbox 24 Identifier High Register */
+
+#define	CAN_MB25_DATA0		0xFFC02F20	/* Mailbox 25 Data Word	0 [15:0] Register */
+#define	CAN_MB25_DATA1		0xFFC02F24	/* Mailbox 25 Data Word	1 [31:16] Register */
+#define	CAN_MB25_DATA2		0xFFC02F28	/* Mailbox 25 Data Word	2 [47:32] Register */
+#define	CAN_MB25_DATA3		0xFFC02F2C	/* Mailbox 25 Data Word	3 [63:48] Register */
+#define	CAN_MB25_LENGTH		0xFFC02F30	/* Mailbox 25 Data Length Code Register */
+#define	CAN_MB25_TIMESTAMP	0xFFC02F34	/* Mailbox 25 Time Stamp Value Register */
+#define	CAN_MB25_ID0		0xFFC02F38	/* Mailbox 25 Identifier Low Register */
+#define	CAN_MB25_ID1		0xFFC02F3C	/* Mailbox 25 Identifier High Register */
+
+#define	CAN_MB26_DATA0		0xFFC02F40	/* Mailbox 26 Data Word	0 [15:0] Register */
+#define	CAN_MB26_DATA1		0xFFC02F44	/* Mailbox 26 Data Word	1 [31:16] Register */
+#define	CAN_MB26_DATA2		0xFFC02F48	/* Mailbox 26 Data Word	2 [47:32] Register */
+#define	CAN_MB26_DATA3		0xFFC02F4C	/* Mailbox 26 Data Word	3 [63:48] Register */
+#define	CAN_MB26_LENGTH		0xFFC02F50	/* Mailbox 26 Data Length Code Register */
+#define	CAN_MB26_TIMESTAMP	0xFFC02F54	/* Mailbox 26 Time Stamp Value Register */
+#define	CAN_MB26_ID0		0xFFC02F58	/* Mailbox 26 Identifier Low Register */
+#define	CAN_MB26_ID1		0xFFC02F5C	/* Mailbox 26 Identifier High Register */
+
+#define	CAN_MB27_DATA0		0xFFC02F60	/* Mailbox 27 Data Word	0 [15:0] Register */
+#define	CAN_MB27_DATA1		0xFFC02F64	/* Mailbox 27 Data Word	1 [31:16] Register */
+#define	CAN_MB27_DATA2		0xFFC02F68	/* Mailbox 27 Data Word	2 [47:32] Register */
+#define	CAN_MB27_DATA3		0xFFC02F6C	/* Mailbox 27 Data Word	3 [63:48] Register */
+#define	CAN_MB27_LENGTH		0xFFC02F70	/* Mailbox 27 Data Length Code Register */
+#define	CAN_MB27_TIMESTAMP	0xFFC02F74	/* Mailbox 27 Time Stamp Value Register */
+#define	CAN_MB27_ID0		0xFFC02F78	/* Mailbox 27 Identifier Low Register */
+#define	CAN_MB27_ID1		0xFFC02F7C	/* Mailbox 27 Identifier High Register */
+
+#define	CAN_MB28_DATA0		0xFFC02F80	/* Mailbox 28 Data Word	0 [15:0] Register */
+#define	CAN_MB28_DATA1		0xFFC02F84	/* Mailbox 28 Data Word	1 [31:16] Register */
+#define	CAN_MB28_DATA2		0xFFC02F88	/* Mailbox 28 Data Word	2 [47:32] Register */
+#define	CAN_MB28_DATA3		0xFFC02F8C	/* Mailbox 28 Data Word	3 [63:48] Register */
+#define	CAN_MB28_LENGTH		0xFFC02F90	/* Mailbox 28 Data Length Code Register */
+#define	CAN_MB28_TIMESTAMP	0xFFC02F94	/* Mailbox 28 Time Stamp Value Register */
+#define	CAN_MB28_ID0		0xFFC02F98	/* Mailbox 28 Identifier Low Register */
+#define	CAN_MB28_ID1		0xFFC02F9C	/* Mailbox 28 Identifier High Register */
+
+#define	CAN_MB29_DATA0		0xFFC02FA0	/* Mailbox 29 Data Word	0 [15:0] Register */
+#define	CAN_MB29_DATA1		0xFFC02FA4	/* Mailbox 29 Data Word	1 [31:16] Register */
+#define	CAN_MB29_DATA2		0xFFC02FA8	/* Mailbox 29 Data Word	2 [47:32] Register */
+#define	CAN_MB29_DATA3		0xFFC02FAC	/* Mailbox 29 Data Word	3 [63:48] Register */
+#define	CAN_MB29_LENGTH		0xFFC02FB0	/* Mailbox 29 Data Length Code Register */
+#define	CAN_MB29_TIMESTAMP	0xFFC02FB4	/* Mailbox 29 Time Stamp Value Register */
+#define	CAN_MB29_ID0		0xFFC02FB8	/* Mailbox 29 Identifier Low Register */
+#define	CAN_MB29_ID1		0xFFC02FBC	/* Mailbox 29 Identifier High Register */
+
+#define	CAN_MB30_DATA0		0xFFC02FC0	/* Mailbox 30 Data Word	0 [15:0] Register */
+#define	CAN_MB30_DATA1		0xFFC02FC4	/* Mailbox 30 Data Word	1 [31:16] Register */
+#define	CAN_MB30_DATA2		0xFFC02FC8	/* Mailbox 30 Data Word	2 [47:32] Register */
+#define	CAN_MB30_DATA3		0xFFC02FCC	/* Mailbox 30 Data Word	3 [63:48] Register */
+#define	CAN_MB30_LENGTH		0xFFC02FD0	/* Mailbox 30 Data Length Code Register */
+#define	CAN_MB30_TIMESTAMP	0xFFC02FD4	/* Mailbox 30 Time Stamp Value Register */
+#define	CAN_MB30_ID0		0xFFC02FD8	/* Mailbox 30 Identifier Low Register */
+#define	CAN_MB30_ID1		0xFFC02FDC	/* Mailbox 30 Identifier High Register */
+
+#define	CAN_MB31_DATA0		0xFFC02FE0	/* Mailbox 31 Data Word	0 [15:0] Register */
+#define	CAN_MB31_DATA1		0xFFC02FE4	/* Mailbox 31 Data Word	1 [31:16] Register */
+#define	CAN_MB31_DATA2		0xFFC02FE8	/* Mailbox 31 Data Word	2 [47:32] Register */
+#define	CAN_MB31_DATA3		0xFFC02FEC	/* Mailbox 31 Data Word	3 [63:48] Register */
+#define	CAN_MB31_LENGTH		0xFFC02FF0	/* Mailbox 31 Data Length Code Register */
+#define	CAN_MB31_TIMESTAMP	0xFFC02FF4	/* Mailbox 31 Time Stamp Value Register */
+#define	CAN_MB31_ID0		0xFFC02FF8	/* Mailbox 31 Identifier Low Register */
+#define	CAN_MB31_ID1		0xFFC02FFC	/* Mailbox 31 Identifier High Register */
+
+/* CAN Mailbox Area Macros */
+#define	CAN_MB_ID1(x)		(CAN_MB00_ID1+((x)*0x20))
+#define	CAN_MB_ID0(x)		(CAN_MB00_ID0+((x)*0x20))
+#define	CAN_MB_TIMESTAMP(x)	(CAN_MB00_TIMESTAMP+((x)*0x20))
+#define	CAN_MB_LENGTH(x)	(CAN_MB00_LENGTH+((x)*0x20))
+#define	CAN_MB_DATA3(x)		(CAN_MB00_DATA3+((x)*0x20))
+#define	CAN_MB_DATA2(x)		(CAN_MB00_DATA2+((x)*0x20))
+#define	CAN_MB_DATA1(x)		(CAN_MB00_DATA1+((x)*0x20))
+#define	CAN_MB_DATA0(x)		(CAN_MB00_DATA0+((x)*0x20))
+
+
+/*********************************************************************************** */
+/* System MMR Register Bits and	Macros */
+/******************************************************************************* */
+
+/* ********************* PLL AND RESET MASKS ************************ */
+/* PLL_CTL Masks */
+#define	PLL_CLKIN			0x0000	/* Pass	CLKIN to PLL */
+#define	PLL_CLKIN_DIV2		0x0001	/* Pass	CLKIN/2	to PLL */
+#define	DF					0x0001	 /* 0: PLL = CLKIN, 1: PLL = CLKIN/2 */
+#define	PLL_OFF				0x0002	/* Shut	off PLL	clocks */
+
+#define	STOPCK				0x0008	/* Core	Clock Off		 */
+#define	PDWN				0x0020	/* Put the PLL in a Deep Sleep state */
+#define	IN_DELAY			0x0014	/* EBIU	Input Delay Select */
+#define	OUT_DELAY			0x00C0	/* EBIU	Output Delay Select */
+#define	BYPASS				0x0100	/* Bypass the PLL */
+#define	MSEL			0x7E00	/* Multiplier Select For CCLK/VCO Factors */
+
+/* PLL_CTL Macros				 */
+#ifdef _MISRA_RULES
+#define	SET_MSEL(x)		(((x)&0x3Fu) <<	0x9)	/* Set MSEL = 0-63 --> VCO = CLKIN*MSEL */
+#define	SET_OUT_DELAY(x)	(((x)&0x03u) <<	0x6)
+#define	SET_IN_DELAY(x)		((((x)&0x02u) << 0x3) |	(((x)&0x01u) <<	0x2))
+#else
+#define	SET_MSEL(x)		(((x)&0x3F) << 0x9)	/* Set MSEL = 0-63 --> VCO = CLKIN*MSEL */
+#define	SET_OUT_DELAY(x)	(((x)&0x03) << 0x6)
+#define	SET_IN_DELAY(x)		((((x)&0x02) <<	0x3) | (((x)&0x01) << 0x2))
+#endif /* _MISRA_RULES */
+
+/* PLL_DIV Masks */
+#define	SSEL				0x000F	/* System Select */
+#define	CSEL				0x0030	/* Core	Select */
+#define	CSEL_DIV1		0x0000	/*		CCLK = VCO / 1 */
+#define	CSEL_DIV2		0x0010	/*		CCLK = VCO / 2 */
+#define	CSEL_DIV4		0x0020	/*		CCLK = VCO / 4 */
+#define	CSEL_DIV8		0x0030	/*		CCLK = VCO / 8 */
+
+#define	SCLK_DIV(x)			(x)		/* SCLK	= VCO /	x */
+
+/* PLL_DIV Macros							 */
+#ifdef _MISRA_RULES
+#define	SET_SSEL(x)			((x)&0xFu)	/* Set SSEL = 0-15 --> SCLK = VCO/SSEL */
+#else
+#define	SET_SSEL(x)			((x)&0xF)	/* Set SSEL = 0-15 --> SCLK = VCO/SSEL */
+#endif /* _MISRA_RULES */
+
+/* PLL_STAT Masks										 */
+#define	ACTIVE_PLLENABLED	0x0001	/* Processor In	Active Mode With PLL Enabled */
+#define	FULL_ON				0x0002	/* Processor In	Full On	Mode */
+#define	ACTIVE_PLLDISABLED	0x0004	/* Processor In	Active Mode With PLL Disabled */
+#define	PLL_LOCKED			0x0020	/* PLL_LOCKCNT Has Been	Reached */
+
+/* VR_CTL Masks										 */
+#define	FREQ			0x0003	/* Switching Oscillator	Frequency For Regulator */
+#define	HIBERNATE		0x0000	/*		Powerdown/Bypass On-Board Regulation */
+#define	FREQ_333		0x0001	/*		Switching Frequency Is 333 kHz */
+#define	FREQ_667		0x0002	/*		Switching Frequency Is 667 kHz */
+#define	FREQ_1000		0x0003	/*		Switching Frequency Is 1 MHz */
+
+#define	GAIN			0x000C	/* Voltage Level Gain */
+#define	GAIN_5			0x0000	/*		GAIN = 5 */
+#define	GAIN_10			0x0004	/*		GAIN = 10 */
+#define	GAIN_20			0x0008	/*		GAIN = 20 */
+#define	GAIN_50			0x000C	/*		GAIN = 50 */
+
+#define	VLEV			0x00F0	/* Internal Voltage Level - Only Program Values	Within Specifications */
+#define	VLEV_100		0x0090	/*	VLEV = 1.00 V (See Datasheet for Regulator Tolerance) */
+#define	VLEV_105		0x00A0	/*	VLEV = 1.05 V (See Datasheet for Regulator Tolerance) */
+#define	VLEV_110		0x00B0	/*	VLEV = 1.10 V (See Datasheet for Regulator Tolerance) */
+#define	VLEV_115		0x00C0	/*	VLEV = 1.15 V (See Datasheet for Regulator Tolerance) */
+#define	VLEV_120		0x00D0	/*	VLEV = 1.20 V (See Datasheet for Regulator Tolerance) */
+#define	VLEV_125		0x00E0	/*	VLEV = 1.25 V (See Datasheet for Regulator Tolerance) */
+#define	VLEV_130		0x00F0	/*	VLEV = 1.30 V (See Datasheet for Regulator Tolerance) */
+
+#define	WAKE			0x0100	/* Enable RTC/Reset Wakeup From	Hibernate */
+#define	CANWE			0x0200	/* Enable CAN Wakeup From Hibernate */
+#define	MXVRWE			0x0400	/* Enable MXVR Wakeup From Hibernate */
+#define	SCKELOW			0x8000	/* Do Not Drive	SCKE High During Reset After Hibernate */
+
+/* SWRST Mask */
+#define	SYSTEM_RESET	0x0007	/* Initiates A System Software Reset */
+#define	DOUBLE_FAULT	0x0008	/* Core	Double Fault Causes Reset */
+#define	RESET_DOUBLE	0x2000	/* SW Reset Generated By Core Double-Fault */
+#define	RESET_WDOG		0x4000	/* SW Reset Generated By Watchdog Timer */
+#define	RESET_SOFTWARE	0x8000	/* SW Reset Occurred Since Last	Read Of	SWRST */
+
+/* SYSCR Masks													 */
+#define	BMODE			0x0006	/* Boot	Mode - Latched During HW Reset From Mode Pins */
+#define	NOBOOT			0x0010	/* Execute From	L1 or ASYNC Bank 0 When	BMODE =	0 */
+
+
+/* *************  SYSTEM INTERRUPT CONTROLLER MASKS ***************** */
+
+/* Peripheral Masks For	SIC0_ISR, SIC0_IWR, SIC0_IMASK */
+#define	PLL_WAKEUP_IRQ		0x00000001	/* PLL Wakeup Interrupt	Request */
+#define	DMAC0_ERR_IRQ		0x00000002	/* DMA Controller 0 Error Interrupt Request */
+#define	PPI_ERR_IRQ		0x00000004	/* PPI Error Interrupt Request */
+#define	SPORT0_ERR_IRQ		0x00000008	/* SPORT0 Error	Interrupt Request */
+#define	SPORT1_ERR_IRQ		0x00000010	/* SPORT1 Error	Interrupt Request */
+#define	SPI0_ERR_IRQ		0x00000020	/* SPI0	Error Interrupt	Request */
+#define	UART0_ERR_IRQ		0x00000040	/* UART0 Error Interrupt Request */
+#define	RTC_IRQ			0x00000080	/* Real-Time Clock Interrupt Request */
+#define	DMA0_IRQ		0x00000100	/* DMA Channel 0 (PPI) Interrupt Request */
+#define	DMA1_IRQ		0x00000200	/* DMA Channel 1 (SPORT0 RX) Interrupt Request */
+#define	DMA2_IRQ		0x00000400	/* DMA Channel 2 (SPORT0 TX) Interrupt Request */
+#define	DMA3_IRQ		0x00000800	/* DMA Channel 3 (SPORT1 RX) Interrupt Request */
+#define	DMA4_IRQ		0x00001000	/* DMA Channel 4 (SPORT1 TX) Interrupt Request */
+#define	DMA5_IRQ		0x00002000	/* DMA Channel 5 (SPI) Interrupt Request */
+#define	DMA6_IRQ		0x00004000	/* DMA Channel 6 (UART RX) Interrupt Request */
+#define	DMA7_IRQ		0x00008000	/* DMA Channel 7 (UART TX) Interrupt Request */
+#define	TIMER0_IRQ		0x00010000	/* Timer 0 Interrupt Request */
+#define	TIMER1_IRQ		0x00020000	/* Timer 1 Interrupt Request */
+#define	TIMER2_IRQ		0x00040000	/* Timer 2 Interrupt Request */
+#define	PFA_IRQ			0x00080000	/* Programmable	Flag Interrupt Request A */
+#define	PFB_IRQ			0x00100000	/* Programmable	Flag Interrupt Request B */
+#define	MDMA0_0_IRQ		0x00200000	/* MemDMA0 Stream 0 Interrupt Request */
+#define	MDMA0_1_IRQ		0x00400000	/* MemDMA0 Stream 1 Interrupt Request */
+#define	WDOG_IRQ		0x00800000	/* Software Watchdog Timer Interrupt Request */
+#define	DMAC1_ERR_IRQ		0x01000000	/* DMA Controller 1 Error Interrupt Request */
+#define	SPORT2_ERR_IRQ		0x02000000	/* SPORT2 Error	Interrupt Request */
+#define	SPORT3_ERR_IRQ		0x04000000	/* SPORT3 Error	Interrupt Request */
+#define	MXVR_SD_IRQ		0x08000000	/* MXVR	Synchronous Data Interrupt Request */
+#define	SPI1_ERR_IRQ		0x10000000	/* SPI1	Error Interrupt	Request */
+#define	SPI2_ERR_IRQ		0x20000000	/* SPI2	Error Interrupt	Request */
+#define	UART1_ERR_IRQ		0x40000000	/* UART1 Error Interrupt Request */
+#define	UART2_ERR_IRQ		0x80000000	/* UART2 Error Interrupt Request */
+
+/* the following are for backwards compatibility */
+#define	DMA0_ERR_IRQ		DMAC0_ERR_IRQ
+#define	DMA1_ERR_IRQ		DMAC1_ERR_IRQ
+
+
+/* Peripheral Masks For	SIC_ISR1, SIC_IWR1, SIC_IMASK1	 */
+#define	CAN_ERR_IRQ			0x00000001	/* CAN Error Interrupt Request */
+#define	DMA8_IRQ			0x00000002	/* DMA Channel 8 (SPORT2 RX) Interrupt Request */
+#define	DMA9_IRQ			0x00000004	/* DMA Channel 9 (SPORT2 TX) Interrupt Request */
+#define	DMA10_IRQ			0x00000008	/* DMA Channel 10 (SPORT3 RX) Interrupt	Request */
+#define	DMA11_IRQ			0x00000010	/* DMA Channel 11 (SPORT3 TX) Interrupt	Request */
+#define	DMA12_IRQ			0x00000020	/* DMA Channel 12 Interrupt Request */
+#define	DMA13_IRQ			0x00000040	/* DMA Channel 13 Interrupt Request */
+#define	DMA14_IRQ			0x00000080	/* DMA Channel 14 (SPI1) Interrupt Request */
+#define	DMA15_IRQ			0x00000100	/* DMA Channel 15 (SPI2) Interrupt Request */
+#define	DMA16_IRQ			0x00000200	/* DMA Channel 16 (UART1 RX) Interrupt Request */
+#define	DMA17_IRQ			0x00000400	/* DMA Channel 17 (UART1 TX) Interrupt Request */
+#define	DMA18_IRQ			0x00000800	/* DMA Channel 18 (UART2 RX) Interrupt Request */
+#define	DMA19_IRQ			0x00001000	/* DMA Channel 19 (UART2 TX) Interrupt Request */
+#define	TWI0_IRQ			0x00002000	/* TWI0	Interrupt Request */
+#define	TWI1_IRQ			0x00004000	/* TWI1	Interrupt Request */
+#define	CAN_RX_IRQ			0x00008000	/* CAN Receive Interrupt Request */
+#define	CAN_TX_IRQ			0x00010000	/* CAN Transmit	Interrupt Request */
+#define	MDMA1_0_IRQ			0x00020000	/* MemDMA1 Stream 0 Interrupt Request */
+#define	MDMA1_1_IRQ			0x00040000	/* MemDMA1 Stream 1 Interrupt Request */
+#define	MXVR_STAT_IRQ			0x00080000	/* MXVR	Status Interrupt Request */
+#define	MXVR_CM_IRQ			0x00100000	/* MXVR	Control	Message	Interrupt Request */
+#define	MXVR_AP_IRQ			0x00200000	/* MXVR	Asynchronous Packet Interrupt */
+
+/* the following are for backwards compatibility */
+#define	MDMA0_IRQ		MDMA1_0_IRQ
+#define	MDMA1_IRQ		MDMA1_1_IRQ
+
+#ifdef _MISRA_RULES
+#define	_MF15 0xFu
+#define	_MF7 7u
+#else
+#define	_MF15 0xF
+#define	_MF7 7
+#endif /* _MISRA_RULES */
+
+/* SIC_IMASKx Masks											 */
+#define	SIC_UNMASK_ALL	0x00000000					/* Unmask all peripheral interrupts */
+#define	SIC_MASK_ALL	0xFFFFFFFF					/* Mask	all peripheral interrupts */
+#ifdef _MISRA_RULES
+#define	SIC_MASK(x)		(1 << ((x)&0x1Fu))					/* Mask	Peripheral #x interrupt */
+#define	SIC_UNMASK(x)	(0xFFFFFFFFu ^ (1 << ((x)&0x1Fu)))	/* Unmask Peripheral #x	interrupt */
+#else
+#define	SIC_MASK(x)		(1 << ((x)&0x1F))					/* Mask	Peripheral #x interrupt */
+#define	SIC_UNMASK(x)	(0xFFFFFFFF ^ (1 << ((x)&0x1F)))	/* Unmask Peripheral #x	interrupt */
+#endif /* _MISRA_RULES */
+
+/* SIC_IWRx Masks											 */
+#define	IWR_DISABLE_ALL	0x00000000					/* Wakeup Disable all peripherals */
+#define	IWR_ENABLE_ALL	0xFFFFFFFF					/* Wakeup Enable all peripherals */
+#ifdef _MISRA_RULES
+#define	IWR_ENABLE(x)	(1 << ((x)&0x1Fu))					/* Wakeup Enable Peripheral #x */
+#define	IWR_DISABLE(x)	(0xFFFFFFFFu ^ (1 << ((x)&0x1Fu)))	/* Wakeup Disable Peripheral #x */
+#else
+#define	IWR_ENABLE(x)	(1 << ((x)&0x1F))					/* Wakeup Enable Peripheral #x */
+#define	IWR_DISABLE(x)	(0xFFFFFFFF ^ (1 << ((x)&0x1F)))	/* Wakeup Disable Peripheral #x */
+#endif /* _MISRA_RULES */
+
+
+/* ********* WATCHDOG TIMER MASKS ******************** */
+/* Watchdog Timer WDOG_CTL Register Masks */
+#ifdef _MISRA_RULES
+#define	WDEV(x)			(((x)<<1) & 0x0006u)	/* event generated on roll over */
+#else
+#define	WDEV(x)			(((x)<<1) & 0x0006)	/* event generated on roll over */
+#endif /* _MISRA_RULES */
+#define	WDEV_RESET		0x0000				/* generate reset event	on roll	over */
+#define	WDEV_NMI		0x0002				/* generate NMI	event on roll over */
+#define	WDEV_GPI		0x0004				/* generate GP IRQ on roll over */
+#define	WDEV_NONE		0x0006				/* no event on roll over */
+#define	WDEN			0x0FF0				/* enable watchdog */
+#define	WDDIS			0x0AD0				/* disable watchdog */
+#define	WDRO			0x8000				/* watchdog rolled over	latch */
+
+/* deprecated WDOG_CTL Register	Masks for legacy code */
+#define	ICTL WDEV
+#define	ENABLE_RESET	WDEV_RESET
+#define	WDOG_RESET		WDEV_RESET
+#define	ENABLE_NMI		WDEV_NMI
+#define	WDOG_NMI		WDEV_NMI
+#define	ENABLE_GPI		WDEV_GPI
+#define	WDOG_GPI		WDEV_GPI
+#define	DISABLE_EVT	WDEV_NONE
+#define	WDOG_NONE		WDEV_NONE
+
+#define	TMR_EN			WDEN
+#define	WDOG_DISABLE		WDDIS
+#define	TRO			WDRO
+
+#define	ICTL_P0			0x01
+#define	ICTL_P1			0x02
+#define	TRO_P			0x0F
+
+
+/* ***************  REAL TIME CLOCK MASKS  **************************/
+/* RTC_STAT and	RTC_ALARM register */
+#define	RTSEC		0x0000003F	/* Real-Time Clock Seconds */
+#define	RTMIN		0x00000FC0	/* Real-Time Clock Minutes */
+#define	RTHR		0x0001F000	/* Real-Time Clock Hours */
+#define	RTDAY		0xFFFE0000	/* Real-Time Clock Days */
+
+/* RTC_ICTL register */
+#define	SWIE		0x0001		/* Stopwatch Interrupt Enable */
+#define	AIE			0x0002		/* Alarm Interrupt Enable */
+#define	SIE			0x0004		/* Seconds (1 Hz) Interrupt Enable */
+#define	MIE			0x0008		/* Minutes Interrupt Enable */
+#define	HIE			0x0010		/* Hours Interrupt Enable */
+#define	DIE			0x0020		/* 24 Hours (Days) Interrupt Enable */
+#define	DAIE		0x0040		/* Day Alarm (Day, Hour, Minute, Second) Interrupt Enable */
+#define	WCIE		0x8000		/* Write Complete Interrupt Enable */
+
+/* RTC_ISTAT register */
+#define	SWEF		0x0001		/* Stopwatch Event Flag */
+#define	AEF			0x0002		/* Alarm Event Flag */
+#define	SEF			0x0004		/* Seconds (1 Hz) Event	Flag */
+#define	MEF			0x0008		/* Minutes Event Flag */
+#define	HEF			0x0010		/* Hours Event Flag */
+#define	DEF			0x0020		/* 24 Hours (Days) Event Flag */
+#define	DAEF		0x0040		/* Day Alarm (Day, Hour, Minute, Second) Event Flag */
+#define	WPS			0x4000		/* Write Pending Status	(RO) */
+#define	WCOM		0x8000		/* Write Complete */
+
+/* RTC_FAST Mask (RTC_PREN Mask) */
+#define	ENABLE_PRESCALE	     0x00000001	 /* Enable prescaler so	RTC runs at 1 Hz */
+#define	PREN		     0x00000001
+		/* ** Must be set after	power-up for proper operation of RTC */
+
+/* Deprecated RTC_STAT and RTC_ALARM Masks			 */
+#define	RTC_SEC			RTSEC	/* Real-Time Clock Seconds */
+#define	RTC_MIN			RTMIN	/* Real-Time Clock Minutes */
+#define	RTC_HR			RTHR	/* Real-Time Clock Hours */
+#define	RTC_DAY			RTDAY	/* Real-Time Clock Days */
+
+/* Deprecated RTC_ICTL/RTC_ISTAT Masks											 */
+#define	STOPWATCH		SWIE		/* Stopwatch Interrupt Enable	 */
+#define	ALARM			AIE		/* Alarm Interrupt Enable		 */
+#define	SECOND			SIE		/* Seconds (1 Hz) Interrupt Enable */
+#define	MINUTE			MIE		/* Minutes Interrupt Enable		 */
+#define	HOUR			HIE		/* Hours Interrupt Enable		 */
+#define	DAY				DIE		/* 24 Hours (Days) Interrupt Enable */
+#define	DAY_ALARM		DAIE		/* Day Alarm (Day, Hour, Minute, Second) Interrupt Enable */
+#define	WRITE_COMPLETE	WCIE		/* Write Complete Interrupt Enable */
+
+
+/* ***************************** UART CONTROLLER MASKS ********************** */
+/* UARTx_LCR Register */
+#ifdef _MISRA_RULES
+#define	WLS(x)		(((x)-5u) & 0x03u)	/* Word	Length Select */
+#else
+#define	WLS(x)		(((x)-5) & 0x03)	/* Word	Length Select */
+#endif /* _MISRA_RULES */
+#define	STB			0x04				/* Stop	Bits */
+#define	PEN			0x08				/* Parity Enable */
+#define	EPS			0x10				/* Even	Parity Select */
+#define	STP			0x20				/* Stick Parity */
+#define	SB			0x40				/* Set Break */
+#define	DLAB		0x80				/* Divisor Latch Access */
+
+#define	DLAB_P		0x07
+#define	SB_P		0x06
+#define	STP_P		0x05
+#define	EPS_P		0x04
+#define	PEN_P		0x03
+#define	STB_P		0x02
+#define	WLS_P1		0x01
+#define	WLS_P0		0x00
+
+/* UARTx_MCR Register */
+#define	LOOP_ENA	0x10	/* Loopback Mode Enable */
+#define	LOOP_ENA_P	0x04
+/* Deprecated UARTx_MCR	Mask			 */
+
+/* UARTx_LSR Register */
+#define	DR			0x01	/* Data	Ready */
+#define	OE			0x02	/* Overrun Error */
+#define	PE			0x04	/* Parity Error */
+#define	FE			0x08	/* Framing Error */
+#define	BI			0x10	/* Break Interrupt */
+#define	THRE		0x20	/* THR Empty */
+#define	TEMT		0x40	/* TSR and UART_THR Empty */
+
+#define	TEMP_P		0x06
+#define	THRE_P		0x05
+#define	BI_P		0x04
+#define	FE_P		0x03
+#define	PE_P		0x02
+#define	OE_P		0x01
+#define	DR_P		0x00
+
+/* UARTx_IER Register */
+#define	ERBFI		0x01		/* Enable Receive Buffer Full Interrupt */
+#define	ETBEI		0x02		/* Enable Transmit Buffer Empty	Interrupt */
+#define	ELSI		0x04		/* Enable RX Status Interrupt */
+
+#define	ELSI_P		0x02
+#define	ETBEI_P		0x01
+#define	ERBFI_P		0x00
+
+/* UARTx_IIR Register */
+#define	NINT		0x01
+#define	STATUS_P1	0x02
+#define	STATUS_P0	0x01
+#define	NINT_P		0x00
+
+/* UARTx_GCTL Register */
+#define	UCEN		0x01		/* Enable UARTx	Clocks */
+#define	IREN		0x02		/* Enable IrDA Mode */
+#define	TPOLC		0x04		/* IrDA	TX Polarity Change */
+#define	RPOLC		0x08		/* IrDA	RX Polarity Change */
+#define	FPE			0x10		/* Force Parity	Error On Transmit */
+#define	FFE			0x20		/* Force Framing Error On Transmit */
+
+#define	FFE_P		0x05
+#define	FPE_P		0x04
+#define	RPOLC_P		0x03
+#define	TPOLC_P		0x02
+#define	IREN_P		0x01
+#define	UCEN_P		0x00
+
+
+/* **********  SERIAL PORT MASKS  ********************** */
+/* SPORTx_TCR1 Masks */
+#define	TSPEN		0x0001	/* TX enable  */
+#define	ITCLK		0x0002	/* Internal TX Clock Select  */
+#define	TDTYPE		0x000C	/* TX Data Formatting Select */
+#define	DTYPE_NORM	0x0000		/* Data	Format Normal */
+#define	DTYPE_ULAW	0x0008		/* Compand Using u-Law */
+#define	DTYPE_ALAW	0x000C		/* Compand Using A-Law */
+#define	TLSBIT		0x0010	/* TX Bit Order */
+#define	ITFS		0x0200	/* Internal TX Frame Sync Select  */
+#define	TFSR		0x0400	/* TX Frame Sync Required Select  */
+#define	DITFS		0x0800	/* Data	Independent TX Frame Sync Select  */
+#define	LTFS		0x1000	/* Low TX Frame	Sync Select  */
+#define	LATFS		0x2000	/* Late	TX Frame Sync Select  */
+#define	TCKFE		0x4000	/* TX Clock Falling Edge Select */
+/* SPORTx_RCR1 Deprecated Masks								 */
+#define	TULAW		DTYPE_ULAW		/* Compand Using u-Law */
+#define	TALAW		DTYPE_ALAW			/* Compand Using A-Law */
+
+/* SPORTx_TCR2 Masks */
+#ifdef _MISRA_RULES
+#define	SLEN(x)		((x)&0x1Fu)	/* SPORT TX Word Length	(2 - 31) */
+#else
+#define	SLEN(x)		((x)&0x1F)	/* SPORT TX Word Length	(2 - 31) */
+#endif /* _MISRA_RULES */
+#define	TXSE		0x0100	/*TX Secondary Enable */
+#define	TSFSE		0x0200	/*TX Stereo Frame Sync Enable */
+#define	TRFST		0x0400	/*TX Right-First Data Order  */
+
+/* SPORTx_RCR1 Masks */
+#define	RSPEN		0x0001	/* RX enable  */
+#define	IRCLK		0x0002	/* Internal RX Clock Select  */
+#define	RDTYPE		0x000C	/* RX Data Formatting Select */
+#define	DTYPE_NORM	0x0000		/* no companding */
+#define	DTYPE_ULAW	0x0008		/* Compand Using u-Law */
+#define	DTYPE_ALAW	0x000C		/* Compand Using A-Law */
+#define	RLSBIT		0x0010	/* RX Bit Order */
+#define	IRFS		0x0200	/* Internal RX Frame Sync Select  */
+#define	RFSR		0x0400	/* RX Frame Sync Required Select  */
+#define	LRFS		0x1000	/* Low RX Frame	Sync Select  */
+#define	LARFS		0x2000	/* Late	RX Frame Sync Select  */
+#define	RCKFE		0x4000	/* RX Clock Falling Edge Select */
+/* SPORTx_RCR1 Deprecated Masks								 */
+#define	RULAW		DTYPE_ULAW		/* Compand Using u-Law */
+#define	RALAW		DTYPE_ALAW			/* Compand Using A-Law */
+
+/* SPORTx_RCR2 Masks */
+#ifdef _MISRA_RULES
+#define	SLEN(x)		((x)&0x1Fu)	/* SPORT RX Word Length	(2 - 31) */
+#else
+#define	SLEN(x)		((x)&0x1F)	/* SPORT RX Word Length	(2 - 31) */
+#endif /* _MISRA_RULES */
+#define	RXSE		0x0100	/*RX Secondary Enable */
+#define	RSFSE		0x0200	/*RX Stereo Frame Sync Enable */
+#define	RRFST		0x0400	/*Right-First Data Order  */
+
+/*SPORTx_STAT Masks */
+#define	RXNE		0x0001		/*RX FIFO Not Empty Status */
+#define	RUVF		0x0002		/*RX Underflow Status */
+#define	ROVF		0x0004		/*RX Overflow Status */
+#define	TXF			0x0008		/*TX FIFO Full Status */
+#define	TUVF		0x0010		/*TX Underflow Status */
+#define	TOVF		0x0020		/*TX Overflow Status */
+#define	TXHRE		0x0040		/*TX Hold Register Empty */
+
+/*SPORTx_MCMC1 Masks */
+#define	WOFF			0x000003FF	/*Multichannel Window Offset Field */
+/* SPORTx_MCMC1	Macros								 */
+#ifdef _MISRA_RULES
+#define	SET_WOFF(x)		((x) & 0x3FFu)	/* Multichannel	Window Offset Field */
+/* Only	use SET_WSIZE Macro With Logic OR While	Setting	Lower Order Bits */
+#define	SET_WSIZE(x)	(((((x)>>0x3)-1u)&0xFu)	<< 0xC)	/* Multichannel	Window Size = (x/8)-1 */
+#else
+#define	SET_WOFF(x)		((x) & 0x3FF)	/* Multichannel	Window Offset Field */
+/* Only	use SET_WSIZE Macro With Logic OR While	Setting	Lower Order Bits */
+#define	SET_WSIZE(x)	(((((x)>>0x3)-1)&0xF) << 0xC)	/* Multichannel	Window Size = (x/8)-1 */
+#endif /* _MISRA_RULES */
+
+
+/*SPORTx_MCMC2 Masks */
+#define	MCCRM		0x0003	/*Multichannel Clock Recovery Mode */
+#define	REC_BYPASS	0x0000		/* Bypass Mode (No Clock Recovery) */
+#define	REC_2FROM4	0x0002		/* Recover 2 MHz Clock from 4 MHz Clock */
+#define	REC_8FROM16	0x0003		/* Recover 8 MHz Clock from 16 MHz Clock */
+#define	MCDTXPE		0x0004	/*Multichannel DMA Transmit Packing */
+#define	MCDRXPE		0x0008	/*Multichannel DMA Receive Packing */
+#define	MCMEN		0x0010	/*Multichannel Frame Mode Enable */
+#define	FSDR		0x0080	/*Multichannel Frame Sync to Data Relationship */
+#define	MFD			0xF000	/*Multichannel Frame Delay    */
+#define	MFD_0		0x0000		/* Multichannel	Frame Delay = 0 */
+#define	MFD_1		0x1000		/* Multichannel	Frame Delay = 1 */
+#define	MFD_2		0x2000		/* Multichannel	Frame Delay = 2 */
+#define	MFD_3		0x3000		/* Multichannel	Frame Delay = 3 */
+#define	MFD_4		0x4000		/* Multichannel	Frame Delay = 4 */
+#define	MFD_5		0x5000		/* Multichannel	Frame Delay = 5 */
+#define	MFD_6		0x6000		/* Multichannel	Frame Delay = 6 */
+#define	MFD_7		0x7000		/* Multichannel	Frame Delay = 7 */
+#define	MFD_8		0x8000		/* Multichannel	Frame Delay = 8 */
+#define	MFD_9		0x9000		/* Multichannel	Frame Delay = 9 */
+#define	MFD_10		0xA000		/* Multichannel	Frame Delay = 10 */
+#define	MFD_11		0xB000		/* Multichannel	Frame Delay = 11 */
+#define	MFD_12		0xC000		/* Multichannel	Frame Delay = 12 */
+#define	MFD_13		0xD000		/* Multichannel	Frame Delay = 13 */
+#define	MFD_14		0xE000		/* Multichannel	Frame Delay = 14 */
+#define	MFD_15		0xF000		/* Multichannel	Frame Delay = 15 */
+
+
+/*  *********  PARALLEL	PERIPHERAL INTERFACE (PPI) MASKS ****************   */
+/*  PPI_CONTROL	Masks	      */
+#define	PORT_EN		0x0001	/* PPI Port Enable  */
+#define	PORT_DIR	0x0002	/* PPI Port Direction	    */
+#define	XFR_TYPE	0x000C	/* PPI Transfer	Type  */
+#define	PORT_CFG	0x0030	/* PPI Port Configuration */
+#define	FLD_SEL		0x0040	/* PPI Active Field Select */
+#define	PACK_EN		0x0080	/* PPI Packing Mode */
+/* previous versions of	defBF539.h erroneously included	DMA32 (PPI 32-bit DMA Enable) */
+#define	SKIP_EN		0x0200	/* PPI Skip Element Enable */
+#define	SKIP_EO		0x0400	/* PPI Skip Even/Odd Elements */
+#define	DLENGTH		0x3800	/* PPI Data Length  */
+#define	DLEN_8		0x0	     /*	PPI Data Length	mask for DLEN=8 */
+#define	DLEN_10		0x0800		/* Data	Length = 10 Bits */
+#define	DLEN_11		0x1000		/* Data	Length = 11 Bits */
+#define	DLEN_12		0x1800		/* Data	Length = 12 Bits */
+#define	DLEN_13		0x2000		/* Data	Length = 13 Bits */
+#define	DLEN_14		0x2800		/* Data	Length = 14 Bits */
+#define	DLEN_15		0x3000		/* Data	Length = 15 Bits */
+#define	DLEN_16		0x3800		/* Data	Length = 16 Bits */
+#ifdef _MISRA_RULES
+#define	DLEN(x)		((((x)-9u) & 0x07u) << 11)  /* PPI Data	Length (only works for x=10-->x=16) */
+#else
+#define	DLEN(x)		((((x)-9) & 0x07) << 11)  /* PPI Data Length (only works for x=10-->x=16) */
+#endif /* _MISRA_RULES */
+#define	POL			0xC000	/* PPI Signal Polarities       */
+#define	POLC		0x4000		/* PPI Clock Polarity */
+#define	POLS		0x8000		/* PPI Frame Sync Polarity */
+
+
+/* PPI_STATUS Masks					     */
+#define	FLD			0x0400	/* Field Indicator   */
+#define	FT_ERR		0x0800	/* Frame Track Error */
+#define	OVR			0x1000	/* FIFO	Overflow Error */
+#define	UNDR		0x2000	/* FIFO	Underrun Error */
+#define	ERR_DET		0x4000	/* Error Detected Indicator */
+#define	ERR_NCOR	0x8000	/* Error Not Corrected Indicator */
+
+
+/* **********  DMA CONTROLLER MASKS  ***********************/
+/* DMAx_CONFIG,	MDMA_yy_CONFIG Masks */
+#define	DMAEN		0x0001	/* Channel Enable */
+#define	WNR			0x0002	/* Channel Direction (W/R*) */
+#define	WDSIZE_8	0x0000	/* Word	Size 8 bits */
+#define	WDSIZE_16	0x0004	/* Word	Size 16	bits */
+#define	WDSIZE_32	0x0008	/* Word	Size 32	bits */
+#define	DMA2D		0x0010	/* 2D/1D* Mode */
+#define	RESTART		0x0020	/* Restart */
+#define	DI_SEL		0x0040	/* Data	Interrupt Select */
+#define	DI_EN		0x0080	/* Data	Interrupt Enable */
+#define	NDSIZE		0x0900	/* Next	Descriptor Size */
+#define	NDSIZE_0	0x0000	/* Next	Descriptor Size	= 0 (Stop/Autobuffer) */
+#define	NDSIZE_1	0x0100	/* Next	Descriptor Size	= 1 */
+#define	NDSIZE_2	0x0200	/* Next	Descriptor Size	= 2 */
+#define	NDSIZE_3	0x0300	/* Next	Descriptor Size	= 3 */
+#define	NDSIZE_4	0x0400	/* Next	Descriptor Size	= 4 */
+#define	NDSIZE_5	0x0500	/* Next	Descriptor Size	= 5 */
+#define	NDSIZE_6	0x0600	/* Next	Descriptor Size	= 6 */
+#define	NDSIZE_7	0x0700	/* Next	Descriptor Size	= 7 */
+#define	NDSIZE_8	0x0800	/* Next	Descriptor Size	= 8 */
+#define	NDSIZE_9	0x0900	/* Next	Descriptor Size	= 9 */
+
+#define DMAFLOW			0x7000	/* Flow Control */
+#define DMAFLOW_STOP		0x0000	/* Stop Mode */
+#define DMAFLOW_AUTO		0x1000	/* Autobuffer Mode */
+#define DMAFLOW_ARRAY		0x4000	/* Descriptor Array Mode */
+#define DMAFLOW_SMALL		0x6000	/* Small Model Descriptor List Mode */
+#define DMAFLOW_LARGE		0x7000	/* Large Model Descriptor List Mode */
+
+#define	DMAEN_P		0x0		/* Channel Enable */
+#define	WNR_P		0x1		/* Channel Direction (W/R*) */
+#define	DMA2D_P		0x4		/* 2D/1D* Mode */
+#define	RESTART_P	0x5		/* Restart */
+#define	DI_SEL_P	0x6		/* Data	Interrupt Select */
+#define	DI_EN_P		0x7		/* Data	Interrupt Enable */
+
+/* DMAx_IRQ_STATUS, MDMA_yy_IRQ_STATUS Masks */
+#define	DMA_DONE	0x0001	/* DMA Done Indicator */
+#define	DMA_ERR		0x0002	/* DMA Error Indicator */
+#define	DFETCH		0x0004	/* Descriptor Fetch Indicator */
+#define	DMA_RUN		0x0008	/* DMA Running Indicator */
+
+#define	DMA_DONE_P	0x0		/* DMA Done Indicator */
+#define	DMA_ERR_P	0x1		/* DMA Error Indicator */
+#define	DFETCH_P	0x2		/* Descriptor Fetch Indicator */
+#define	DMA_RUN_P	0x3		/* DMA Running Indicator */
+
+/* DMAx_PERIPHERAL_MAP,	MDMA_yy_PERIPHERAL_MAP Masks */
+
+#define	CTYPE			0x0040	/* DMA Channel Type Indicator */
+#define	CTYPE_P			0x6		/* DMA Channel Type Indicator BIT POSITION */
+#define	PCAP8			0x0080	/* DMA 8-bit Operation Indicator   */
+#define	PCAP16			0x0100	/* DMA 16-bit Operation	Indicator */
+#define	PCAP32			0x0200	/* DMA 32-bit Operation	Indicator */
+#define	PCAPWR			0x0400	/* DMA Write Operation Indicator */
+#define	PCAPRD			0x0800	/* DMA Read Operation Indicator */
+#define	PMAP			0xF000	/* DMA Peripheral Map Field */
+
+/* PMAP	Encodings For DMA Controller 0 */
+#define	PMAP_PPI		0x0000	/* PMAP	PPI Port DMA */
+#define	PMAP_SPORT0RX	0x1000	/* PMAP	SPORT0 Receive DMA */
+#define	PMAP_SPORT0TX	0x2000	/* PMAP	SPORT0 Transmit	DMA */
+#define	PMAP_SPORT1RX	0x3000	/* PMAP	SPORT1 Receive DMA */
+#define	PMAP_SPORT1TX	0x4000	/* PMAP	SPORT1 Transmit	DMA */
+#define	PMAP_SPI0		0x5000	/* PMAP	SPI DMA */
+#define	PMAP_UART0RX		0x6000	/* PMAP	UART Receive DMA */
+#define	PMAP_UART0TX		0x7000	/* PMAP	UART Transmit DMA */
+
+/* PMAP	Encodings For DMA Controller 1 */
+#define	PMAP_SPORT2RX	    0x0000  /* PMAP SPORT2 Receive DMA */
+#define	PMAP_SPORT2TX	    0x1000  /* PMAP SPORT2 Transmit DMA */
+#define	PMAP_SPORT3RX	    0x2000  /* PMAP SPORT3 Receive DMA */
+#define	PMAP_SPORT3TX	    0x3000  /* PMAP SPORT3 Transmit DMA */
+#define	PMAP_SPI1	    0x6000  /* PMAP SPI1 DMA */
+#define	PMAP_SPI2	    0x7000  /* PMAP SPI2 DMA */
+#define	PMAP_UART1RX	    0x8000  /* PMAP UART1 Receive DMA */
+#define	PMAP_UART1TX	    0x9000  /* PMAP UART1 Transmit DMA */
+#define	PMAP_UART2RX	    0xA000  /* PMAP UART2 Receive DMA */
+#define	PMAP_UART2TX	    0xB000  /* PMAP UART2 Transmit DMA */
+
+
+/*  *************  GENERAL PURPOSE TIMER MASKS	******************** */
+/* PWM Timer bit definitions */
+/* TIMER_ENABLE	Register */
+#define	TIMEN0			0x0001	/* Enable Timer	0 */
+#define	TIMEN1			0x0002	/* Enable Timer	1 */
+#define	TIMEN2			0x0004	/* Enable Timer	2 */
+
+#define	TIMEN0_P		0x00
+#define	TIMEN1_P		0x01
+#define	TIMEN2_P		0x02
+
+/* TIMER_DISABLE Register */
+#define	TIMDIS0			0x0001	/* Disable Timer 0 */
+#define	TIMDIS1			0x0002	/* Disable Timer 1 */
+#define	TIMDIS2			0x0004	/* Disable Timer 2 */
+
+#define	TIMDIS0_P		0x00
+#define	TIMDIS1_P		0x01
+#define	TIMDIS2_P		0x02
+
+/* TIMER_STATUS	Register */
+#define	TIMIL0			0x0001	/* Timer 0 Interrupt */
+#define	TIMIL1			0x0002	/* Timer 1 Interrupt */
+#define	TIMIL2			0x0004	/* Timer 2 Interrupt */
+#define	TOVF_ERR0		0x0010	/* Timer 0 Counter Overflow */
+#define	TOVF_ERR1		0x0020	/* Timer 1 Counter Overflow */
+#define	TOVF_ERR2		0x0040	/* Timer 2 Counter Overflow */
+#define	TRUN0			0x1000	/* Timer 0 Slave Enable	Status */
+#define	TRUN1			0x2000	/* Timer 1 Slave Enable	Status */
+#define	TRUN2			0x4000	/* Timer 2 Slave Enable	Status */
+
+#define	TIMIL0_P		0x00
+#define	TIMIL1_P		0x01
+#define	TIMIL2_P		0x02
+#define	TOVF_ERR0_P		0x04
+#define	TOVF_ERR1_P		0x05
+#define	TOVF_ERR2_P		0x06
+#define	TRUN0_P			0x0C
+#define	TRUN1_P			0x0D
+#define	TRUN2_P			0x0E
+
+/* Alternate Deprecated	Macros Provided	For Backwards Code Compatibility */
+#define	TOVL_ERR0		TOVF_ERR0
+#define	TOVL_ERR1		TOVF_ERR1
+#define	TOVL_ERR2		TOVF_ERR2
+#define	TOVL_ERR0_P		TOVF_ERR0_P
+#define	TOVL_ERR1_P	TOVF_ERR1_P
+#define	TOVL_ERR2_P	TOVF_ERR2_P
+
+/* TIMERx_CONFIG Registers */
+#define	PWM_OUT			0x0001
+#define	WDTH_CAP		0x0002
+#define	EXT_CLK			0x0003
+#define	PULSE_HI		0x0004
+#define	PERIOD_CNT		0x0008
+#define	IRQ_ENA			0x0010
+#define	TIN_SEL			0x0020
+#define	OUT_DIS			0x0040
+#define	CLK_SEL			0x0080
+#define	TOGGLE_HI		0x0100
+#define	EMU_RUN			0x0200
+#ifdef _MISRA_RULES
+#define	ERR_TYP(x)		(((x) &	0x03u) << 14)
+#else
+#define	ERR_TYP(x)		(((x) &	0x03) << 14)
+#endif /* _MISRA_RULES */
+
+#define	TMODE_P0		0x00
+#define	TMODE_P1		0x01
+#define	PULSE_HI_P		0x02
+#define	PERIOD_CNT_P	0x03
+#define	IRQ_ENA_P		0x04
+#define	TIN_SEL_P		0x05
+#define	OUT_DIS_P		0x06
+#define	CLK_SEL_P		0x07
+#define	TOGGLE_HI_P		0x08
+#define	EMU_RUN_P		0x09
+#define	ERR_TYP_P0		0x0E
+#define	ERR_TYP_P1		0x0F
+
+
+/*/ ******************	 GENERAL-PURPOSE I/O  ********************* */
+/*  Flag I/O (FIO_) Masks */
+#define	PF0			0x0001
+#define	PF1			0x0002
+#define	PF2			0x0004
+#define	PF3			0x0008
+#define	PF4			0x0010
+#define	PF5			0x0020
+#define	PF6			0x0040
+#define	PF7			0x0080
+#define	PF8			0x0100
+#define	PF9			0x0200
+#define	PF10		0x0400
+#define	PF11		0x0800
+#define	PF12		0x1000
+#define	PF13		0x2000
+#define	PF14		0x4000
+#define	PF15		0x8000
+
+/*  PORT F BIT POSITIONS */
+#define	PF0_P		0x0
+#define	PF1_P		0x1
+#define	PF2_P		0x2
+#define	PF3_P		0x3
+#define	PF4_P		0x4
+#define	PF5_P		0x5
+#define	PF6_P		0x6
+#define	PF7_P		0x7
+#define	PF8_P		0x8
+#define	PF9_P		0x9
+#define	PF10_P		0xA
+#define	PF11_P		0xB
+#define	PF12_P		0xC
+#define	PF13_P		0xD
+#define	PF14_P		0xE
+#define	PF15_P		0xF
+
+
+/*******************   GPIO MASKS  *********************/
+/* Port	C Masks */
+#define	PC0		0x0001
+#define	PC1		0x0002
+#define	PC4		0x0010
+#define	PC5		0x0020
+#define	PC6		0x0040
+#define	PC7		0x0080
+#define	PC8		0x0100
+#define	PC9		0x0200
+/* Port	C Bit Positions */
+#define	PC0_P	0x0
+#define	PC1_P	0x1
+#define	PC4_P	0x4
+#define	PC5_P	0x5
+#define	PC6_P	0x6
+#define	PC7_P	0x7
+#define	PC8_P	0x8
+#define	PC9_P	0x9
+
+/* Port	D */
+#define	PD0		0x0001
+#define	PD1		0x0002
+#define	PD2		0x0004
+#define	PD3		0x0008
+#define	PD4		0x0010
+#define	PD5		0x0020
+#define	PD6		0x0040
+#define	PD7		0x0080
+#define	PD8		0x0100
+#define	PD9		0x0200
+#define	PD10	0x0400
+#define	PD11	0x0800
+#define	PD12	0x1000
+#define	PD13	0x2000
+#define	PD14	0x4000
+#define	PD15	0x8000
+/* Port	D Bit Positions */
+#define	PD0_P	0x0
+#define	PD1_P	0x1
+#define	PD2_P	0x2
+#define	PD3_P	0x3
+#define	PD4_P	0x4
+#define	PD5_P	0x5
+#define	PD6_P	0x6
+#define	PD7_P	0x7
+#define	PD8_P	0x8
+#define	PD9_P	0x9
+#define	PD10_P	0xA
+#define	PD11_P	0xB
+#define	PD12_P	0xC
+#define	PD13_P	0xD
+#define	PD14_P	0xE
+#define	PD15_P	0xF
+
+/* Port	E */
+#define	PE0		0x0001
+#define	PE1		0x0002
+#define	PE2		0x0004
+#define	PE3		0x0008
+#define	PE4		0x0010
+#define	PE5		0x0020
+#define	PE6		0x0040
+#define	PE7		0x0080
+#define	PE8		0x0100
+#define	PE9		0x0200
+#define	PE10	0x0400
+#define	PE11	0x0800
+#define	PE12	0x1000
+#define	PE13	0x2000
+#define	PE14	0x4000
+#define	PE15	0x8000
+/* Port	E Bit Positions */
+#define	PE0_P	0x0
+#define	PE1_P	0x1
+#define	PE2_P	0x2
+#define	PE3_P	0x3
+#define	PE4_P	0x4
+#define	PE5_P	0x5
+#define	PE6_P	0x6
+#define	PE7_P	0x7
+#define	PE8_P	0x8
+#define	PE9_P	0x9
+#define	PE10_P	0xA
+#define	PE11_P	0xB
+#define	PE12_P	0xC
+#define	PE13_P	0xD
+#define	PE14_P	0xE
+#define	PE15_P	0xF
+
+
+/* ***********	SERIAL PERIPHERAL INTERFACE (SPI) MASKS	 **************** */
+/* SPIx_CTL Masks */
+#define	TIMOD		0x0003		/* Transfer Initiate Mode */
+#define	RDBR_CORE	0x0000		/*		RDBR Read Initiates, IRQ When RDBR Full */
+#define	TDBR_CORE	0x0001		/*		TDBR Write Initiates, IRQ When TDBR Empty */
+#define	RDBR_DMA	0x0002		/*		DMA Read, DMA Until FIFO Empty */
+#define	TDBR_DMA	0x0003		/*		DMA Write, DMA Until FIFO Full */
+#define	SZ			0x0004		/* Send	Zero (When TDBR	Empty, Send Zero/Last*) */
+#define	GM			0x0008		/* Get More (When RDBR Full, Overwrite/Discard*) */
+#define	PSSE		0x0010		/* Slave-Select	Input Enable */
+#define	EMISO		0x0020		/* Enable MISO As Output */
+#define	SIZE		0x0100		/* Size	of Words (16/8*	Bits) */
+#define	LSBF		0x0200		/* LSB First			 */
+#define	CPHA		0x0400		/* Clock Phase			 */
+#define	CPOL		0x0800		/* Clock Polarity		 */
+#define	MSTR		0x1000		/* Master/Slave*		 */
+#define	WOM			0x2000		/* Write Open Drain Master */
+#define	SPE			0x4000		/* SPI Enable			 */
+
+/* SPIx_FLG Masks */
+#define	FLS1	0x0002	/* Enables (=1)	SPI_FLOUT1 as flag output for SPI Slave-select */
+#define	FLS2	0x0004	/* Enables (=1)	SPI_FLOUT2 as flag output for SPI Slave-select */
+#define	FLS3	0x0008	/* Enables (=1)	SPI_FLOUT3 as flag output for SPI Slave-select */
+#define	FLS4	0x0010	/* Enables (=1)	SPI_FLOUT4 as flag output for SPI Slave-select */
+#define	FLS5	0x0020	/* Enables (=1)	SPI_FLOUT5 as flag output for SPI Slave-select */
+#define	FLS6	0x0040	/* Enables (=1)	SPI_FLOUT6 as flag output for SPI Slave-select */
+#define	FLS7	0x0080	/* Enables (=1)	SPI_FLOUT7 as flag output for SPI Slave-select */
+
+#define	FLG1	0x0200	/* Activates (=0) SPI_FLOUT1 as	flag output for	SPI Slave-select  */
+#define	FLG2	0x0400	/* Activates (=0) SPI_FLOUT2 as	flag output for	SPI Slave-select */
+#define	FLG3	0x0800	/* Activates (=0) SPI_FLOUT3 as	flag output for	SPI Slave-select  */
+#define	FLG4	0x1000	/* Activates (=0) SPI_FLOUT4 as	flag output for	SPI Slave-select  */
+#define	FLG5	0x2000	/* Activates (=0) SPI_FLOUT5 as	flag output for	SPI Slave-select  */
+#define	FLG6	0x4000	/* Activates (=0) SPI_FLOUT6 as	flag output for	SPI Slave-select  */
+#define	FLG7	0x8000	/* Activates (=0) SPI_FLOUT7 as	flag output for	SPI Slave-select */
+
+/* SPIx_FLG Bit	Positions */
+#define	FLS1_P	0x0001	/* Enables (=1)	SPI_FLOUT1 as flag output for SPI Slave-select */
+#define	FLS2_P	0x0002	/* Enables (=1)	SPI_FLOUT2 as flag output for SPI Slave-select */
+#define	FLS3_P	0x0003	/* Enables (=1)	SPI_FLOUT3 as flag output for SPI Slave-select */
+#define	FLS4_P	0x0004	/* Enables (=1)	SPI_FLOUT4 as flag output for SPI Slave-select */
+#define	FLS5_P	0x0005	/* Enables (=1)	SPI_FLOUT5 as flag output for SPI Slave-select */
+#define	FLS6_P	0x0006	/* Enables (=1)	SPI_FLOUT6 as flag output for SPI Slave-select */
+#define	FLS7_P	0x0007	/* Enables (=1)	SPI_FLOUT7 as flag output for SPI Slave-select */
+#define	FLG1_P	0x0009	/* Activates (=0) SPI_FLOUT1 as	flag output for	SPI Slave-select  */
+#define	FLG2_P	0x000A	/* Activates (=0) SPI_FLOUT2 as	flag output for	SPI Slave-select */
+#define	FLG3_P	0x000B	/* Activates (=0) SPI_FLOUT3 as	flag output for	SPI Slave-select  */
+#define	FLG4_P	0x000C	/* Activates (=0) SPI_FLOUT4 as	flag output for	SPI Slave-select  */
+#define	FLG5_P	0x000D	/* Activates (=0) SPI_FLOUT5 as	flag output for	SPI Slave-select  */
+#define	FLG6_P	0x000E	/* Activates (=0) SPI_FLOUT6 as	flag output for	SPI Slave-select  */
+#define	FLG7_P	0x000F	/* Activates (=0) SPI_FLOUT7 as	flag output for	SPI Slave-select */
+
+/* SPIx_STAT Masks */
+#define	SPIF	0x0001	/* Set (=1) when SPI single-word transfer complete */
+#define	MODF	0x0002	/* Set (=1) in a master	device when some other device tries to become master */
+#define	TXE		0x0004	/* Set (=1) when transmission occurs with no new data in SPI_TDBR */
+#define	TXS		0x0008	/* SPI_TDBR Data Buffer	Status (0=Empty, 1=Full) */
+#define	RBSY	0x0010	/* Set (=1) when data is received with RDBR full */
+#define	RXS		0x0020	/* SPI_RDBR Data Buffer	Status (0=Empty, 1=Full)  */
+#define	TXCOL	0x0040	/* When	set (=1), corrupt data may have	been transmitted  */
+
+/* SPIx_FLG Masks										 */
+#define	FLG1E	0xFDFF		/* Activates SPI_FLOUT1	 */
+#define	FLG2E	0xFBFF		/* Activates SPI_FLOUT2	 */
+#define	FLG3E	0xF7FF		/* Activates SPI_FLOUT3	 */
+#define	FLG4E	0xEFFF		/* Activates SPI_FLOUT4	 */
+#define	FLG5E	0xDFFF		/* Activates SPI_FLOUT5	 */
+#define	FLG6E	0xBFFF		/* Activates SPI_FLOUT6	 */
+#define	FLG7E	0x7FFF		/* Activates SPI_FLOUT7	 */
+
+
+/* *********************  ASYNCHRONOUS MEMORY CONTROLLER MASKS	************* */
+/* EBIU_AMGCTL Masks */
+#define	AMCKEN		0x0001	/* Enable CLKOUT */
+#define	AMBEN_NONE	0x0000	/* All Banks Disabled */
+#define	AMBEN_B0	0x0002	/* Enable Asynchronous Memory Bank 0 only */
+#define	AMBEN_B0_B1	0x0004	/* Enable Asynchronous Memory Banks 0 &	1 only */
+#define	AMBEN_B0_B1_B2	0x0006	/* Enable Asynchronous Memory Banks 0, 1, and 2 */
+#define	AMBEN_ALL	0x0008	/* Enable Asynchronous Memory Banks (all) 0, 1,	2, and 3 */
+#define	CDPRIO		0x0100	/* DMA has priority over core for for external accesses */
+
+/* EBIU_AMGCTL Bit Positions */
+#define	AMCKEN_P		0x0000	/* Enable CLKOUT */
+#define	AMBEN_P0		0x0001	/* Asynchronous	Memory Enable, 000 - banks 0-3 disabled, 001 - Bank 0 enabled */
+#define	AMBEN_P1		0x0002	/* Asynchronous	Memory Enable, 010 - banks 0&1 enabled,	 011 - banks 0-3 enabled */
+#define	AMBEN_P2		0x0003	/* Asynchronous	Memory Enable, 1xx - All banks (bank 0,	1, 2, and 3) enabled */
+
+/* EBIU_AMBCTL0	Masks */
+#define	B0RDYEN			0x00000001  /* Bank 0 RDY Enable, 0=disable, 1=enable */
+#define	B0RDYPOL		0x00000002  /* Bank 0 RDY Active high, 0=active	low, 1=active high */
+#define	B0TT_1			0x00000004  /* Bank 0 Transition Time from Read	to Write = 1 cycle */
+#define	B0TT_2			0x00000008  /* Bank 0 Transition Time from Read	to Write = 2 cycles */
+#define	B0TT_3			0x0000000C  /* Bank 0 Transition Time from Read	to Write = 3 cycles */
+#define	B0TT_4			0x00000000  /* Bank 0 Transition Time from Read	to Write = 4 cycles */
+#define	B0ST_1			0x00000010  /* Bank 0 Setup Time from AOE asserted to Read/Write asserted=1 cycle */
+#define	B0ST_2			0x00000020  /* Bank 0 Setup Time from AOE asserted to Read/Write asserted=2 cycles */
+#define	B0ST_3			0x00000030  /* Bank 0 Setup Time from AOE asserted to Read/Write asserted=3 cycles */
+#define	B0ST_4			0x00000000  /* Bank 0 Setup Time from AOE asserted to Read/Write asserted=4 cycles */
+#define	B0HT_1			0x00000040  /* Bank 0 Hold Time	from Read/Write	deasserted to AOE deasserted = 1 cycle */
+#define	B0HT_2			0x00000080  /* Bank 0 Hold Time	from Read/Write	deasserted to AOE deasserted = 2 cycles */
+#define	B0HT_3			0x000000C0  /* Bank 0 Hold Time	from Read/Write	deasserted to AOE deasserted = 3 cycles */
+#define	B0HT_0			0x00000000  /* Bank 0 Hold Time	from Read/Write	deasserted to AOE deasserted = 0 cycles */
+#define	B0RAT_1			0x00000100  /* Bank 0 Read Access Time = 1 cycle */
+#define	B0RAT_2			0x00000200  /* Bank 0 Read Access Time = 2 cycles */
+#define	B0RAT_3			0x00000300  /* Bank 0 Read Access Time = 3 cycles */
+#define	B0RAT_4			0x00000400  /* Bank 0 Read Access Time = 4 cycles */
+#define	B0RAT_5			0x00000500  /* Bank 0 Read Access Time = 5 cycles */
+#define	B0RAT_6			0x00000600  /* Bank 0 Read Access Time = 6 cycles */
+#define	B0RAT_7			0x00000700  /* Bank 0 Read Access Time = 7 cycles */
+#define	B0RAT_8			0x00000800  /* Bank 0 Read Access Time = 8 cycles */
+#define	B0RAT_9			0x00000900  /* Bank 0 Read Access Time = 9 cycles */
+#define	B0RAT_10		0x00000A00  /* Bank 0 Read Access Time = 10 cycles */
+#define	B0RAT_11		0x00000B00  /* Bank 0 Read Access Time = 11 cycles */
+#define	B0RAT_12		0x00000C00  /* Bank 0 Read Access Time = 12 cycles */
+#define	B0RAT_13		0x00000D00  /* Bank 0 Read Access Time = 13 cycles */
+#define	B0RAT_14		0x00000E00  /* Bank 0 Read Access Time = 14 cycles */
+#define	B0RAT_15		0x00000F00  /* Bank 0 Read Access Time = 15 cycles */
+#define	B0WAT_1			0x00001000  /* Bank 0 Write Access Time	= 1 cycle */
+#define	B0WAT_2			0x00002000  /* Bank 0 Write Access Time	= 2 cycles */
+#define	B0WAT_3			0x00003000  /* Bank 0 Write Access Time	= 3 cycles */
+#define	B0WAT_4			0x00004000  /* Bank 0 Write Access Time	= 4 cycles */
+#define	B0WAT_5			0x00005000  /* Bank 0 Write Access Time	= 5 cycles */
+#define	B0WAT_6			0x00006000  /* Bank 0 Write Access Time	= 6 cycles */
+#define	B0WAT_7			0x00007000  /* Bank 0 Write Access Time	= 7 cycles */
+#define	B0WAT_8			0x00008000  /* Bank 0 Write Access Time	= 8 cycles */
+#define	B0WAT_9			0x00009000  /* Bank 0 Write Access Time	= 9 cycles */
+#define	B0WAT_10		0x0000A000  /* Bank 0 Write Access Time	= 10 cycles */
+#define	B0WAT_11		0x0000B000  /* Bank 0 Write Access Time	= 11 cycles */
+#define	B0WAT_12		0x0000C000  /* Bank 0 Write Access Time	= 12 cycles */
+#define	B0WAT_13		0x0000D000  /* Bank 0 Write Access Time	= 13 cycles */
+#define	B0WAT_14		0x0000E000  /* Bank 0 Write Access Time	= 14 cycles */
+#define	B0WAT_15		0x0000F000  /* Bank 0 Write Access Time	= 15 cycles */
+#define	B1RDYEN			0x00010000  /* Bank 1 RDY enable, 0=disable, 1=enable */
+#define	B1RDYPOL		0x00020000  /* Bank 1 RDY Active high, 0=active	low, 1=active high */
+#define	B1TT_1			0x00040000  /* Bank 1 Transition Time from Read	to Write = 1 cycle */
+#define	B1TT_2			0x00080000  /* Bank 1 Transition Time from Read	to Write = 2 cycles */
+#define	B1TT_3			0x000C0000  /* Bank 1 Transition Time from Read	to Write = 3 cycles */
+#define	B1TT_4			0x00000000  /* Bank 1 Transition Time from Read	to Write = 4 cycles */
+#define	B1ST_1			0x00100000  /* Bank 1 Setup Time from AOE asserted to Read or Write asserted = 1 cycle */
+#define	B1ST_2			0x00200000  /* Bank 1 Setup Time from AOE asserted to Read or Write asserted = 2 cycles */
+#define	B1ST_3			0x00300000  /* Bank 1 Setup Time from AOE asserted to Read or Write asserted = 3 cycles */
+#define	B1ST_4			0x00000000  /* Bank 1 Setup Time from AOE asserted to Read or Write asserted = 4 cycles */
+#define	B1HT_1			0x00400000  /* Bank 1 Hold Time	from Read or Write deasserted to AOE deasserted	= 1 cycle */
+#define	B1HT_2			0x00800000  /* Bank 1 Hold Time	from Read or Write deasserted to AOE deasserted	= 2 cycles */
+#define	B1HT_3			0x00C00000  /* Bank 1 Hold Time	from Read or Write deasserted to AOE deasserted	= 3 cycles */
+#define	B1HT_0			0x00000000  /* Bank 1 Hold Time	from Read or Write deasserted to AOE deasserted	= 0 cycles */
+#define	B1RAT_1			0x01000000  /* Bank 1 Read Access Time = 1 cycle */
+#define	B1RAT_2			0x02000000  /* Bank 1 Read Access Time = 2 cycles */
+#define	B1RAT_3			0x03000000  /* Bank 1 Read Access Time = 3 cycles */
+#define	B1RAT_4			0x04000000  /* Bank 1 Read Access Time = 4 cycles */
+#define	B1RAT_5			0x05000000  /* Bank 1 Read Access Time = 5 cycles */
+#define	B1RAT_6			0x06000000  /* Bank 1 Read Access Time = 6 cycles */
+#define	B1RAT_7			0x07000000  /* Bank 1 Read Access Time = 7 cycles */
+#define	B1RAT_8			0x08000000  /* Bank 1 Read Access Time = 8 cycles */
+#define	B1RAT_9			0x09000000  /* Bank 1 Read Access Time = 9 cycles */
+#define	B1RAT_10		0x0A000000  /* Bank 1 Read Access Time = 10 cycles */
+#define	B1RAT_11		0x0B000000  /* Bank 1 Read Access Time = 11 cycles */
+#define	B1RAT_12		0x0C000000  /* Bank 1 Read Access Time = 12 cycles */
+#define	B1RAT_13		0x0D000000  /* Bank 1 Read Access Time = 13 cycles */
+#define	B1RAT_14		0x0E000000  /* Bank 1 Read Access Time = 14 cycles */
+#define	B1RAT_15		0x0F000000  /* Bank 1 Read Access Time = 15 cycles */
+#define	B1WAT_1			0x10000000 /* Bank 1 Write Access Time = 1 cycle */
+#define	B1WAT_2			0x20000000  /* Bank 1 Write Access Time	= 2 cycles */
+#define	B1WAT_3			0x30000000  /* Bank 1 Write Access Time	= 3 cycles */
+#define	B1WAT_4			0x40000000  /* Bank 1 Write Access Time	= 4 cycles */
+#define	B1WAT_5			0x50000000  /* Bank 1 Write Access Time	= 5 cycles */
+#define	B1WAT_6			0x60000000  /* Bank 1 Write Access Time	= 6 cycles */
+#define	B1WAT_7			0x70000000  /* Bank 1 Write Access Time	= 7 cycles */
+#define	B1WAT_8			0x80000000  /* Bank 1 Write Access Time	= 8 cycles */
+#define	B1WAT_9			0x90000000  /* Bank 1 Write Access Time	= 9 cycles */
+#define	B1WAT_10		0xA0000000  /* Bank 1 Write Access Time	= 10 cycles */
+#define	B1WAT_11		0xB0000000  /* Bank 1 Write Access Time	= 11 cycles */
+#define	B1WAT_12		0xC0000000  /* Bank 1 Write Access Time	= 12 cycles */
+#define	B1WAT_13		0xD0000000  /* Bank 1 Write Access Time	= 13 cycles */
+#define	B1WAT_14		0xE0000000  /* Bank 1 Write Access Time	= 14 cycles */
+#define	B1WAT_15		0xF0000000  /* Bank 1 Write Access Time	= 15 cycles */
+
+/* EBIU_AMBCTL1	Masks */
+#define	B2RDYEN			0x00000001  /* Bank 2 RDY Enable, 0=disable, 1=enable */
+#define	B2RDYPOL		0x00000002  /* Bank 2 RDY Active high, 0=active	low, 1=active high */
+#define	B2TT_1			0x00000004  /* Bank 2 Transition Time from Read	to Write = 1 cycle */
+#define	B2TT_2			0x00000008  /* Bank 2 Transition Time from Read	to Write = 2 cycles */
+#define	B2TT_3			0x0000000C  /* Bank 2 Transition Time from Read	to Write = 3 cycles */
+#define	B2TT_4			0x00000000  /* Bank 2 Transition Time from Read	to Write = 4 cycles */
+#define	B2ST_1			0x00000010  /* Bank 2 Setup Time from AOE asserted to Read or Write asserted = 1 cycle */
+#define	B2ST_2			0x00000020  /* Bank 2 Setup Time from AOE asserted to Read or Write asserted = 2 cycles */
+#define	B2ST_3			0x00000030  /* Bank 2 Setup Time from AOE asserted to Read or Write asserted = 3 cycles */
+#define	B2ST_4			0x00000000  /* Bank 2 Setup Time from AOE asserted to Read or Write asserted = 4 cycles */
+#define	B2HT_1			0x00000040  /* Bank 2 Hold Time	from Read or Write deasserted to AOE deasserted	= 1 cycle */
+#define	B2HT_2			0x00000080  /* Bank 2 Hold Time	from Read or Write deasserted to AOE deasserted	= 2 cycles */
+#define	B2HT_3			0x000000C0  /* Bank 2 Hold Time	from Read or Write deasserted to AOE deasserted	= 3 cycles */
+#define	B2HT_0			0x00000000  /* Bank 2 Hold Time	from Read or Write deasserted to AOE deasserted	= 0 cycles */
+#define	B2RAT_1			0x00000100  /* Bank 2 Read Access Time = 1 cycle */
+#define	B2RAT_2			0x00000200  /* Bank 2 Read Access Time = 2 cycles */
+#define	B2RAT_3			0x00000300  /* Bank 2 Read Access Time = 3 cycles */
+#define	B2RAT_4			0x00000400  /* Bank 2 Read Access Time = 4 cycles */
+#define	B2RAT_5			0x00000500  /* Bank 2 Read Access Time = 5 cycles */
+#define	B2RAT_6			0x00000600  /* Bank 2 Read Access Time = 6 cycles */
+#define	B2RAT_7			0x00000700  /* Bank 2 Read Access Time = 7 cycles */
+#define	B2RAT_8			0x00000800  /* Bank 2 Read Access Time = 8 cycles */
+#define	B2RAT_9			0x00000900  /* Bank 2 Read Access Time = 9 cycles */
+#define	B2RAT_10		0x00000A00  /* Bank 2 Read Access Time = 10 cycles */
+#define	B2RAT_11		0x00000B00  /* Bank 2 Read Access Time = 11 cycles */
+#define	B2RAT_12		0x00000C00  /* Bank 2 Read Access Time = 12 cycles */
+#define	B2RAT_13		0x00000D00  /* Bank 2 Read Access Time = 13 cycles */
+#define	B2RAT_14		0x00000E00  /* Bank 2 Read Access Time = 14 cycles */
+#define	B2RAT_15		0x00000F00  /* Bank 2 Read Access Time = 15 cycles */
+#define	B2WAT_1			0x00001000  /* Bank 2 Write Access Time	= 1 cycle */
+#define	B2WAT_2			0x00002000  /* Bank 2 Write Access Time	= 2 cycles */
+#define	B2WAT_3			0x00003000  /* Bank 2 Write Access Time	= 3 cycles */
+#define	B2WAT_4			0x00004000  /* Bank 2 Write Access Time	= 4 cycles */
+#define	B2WAT_5			0x00005000  /* Bank 2 Write Access Time	= 5 cycles */
+#define	B2WAT_6			0x00006000  /* Bank 2 Write Access Time	= 6 cycles */
+#define	B2WAT_7			0x00007000  /* Bank 2 Write Access Time	= 7 cycles */
+#define	B2WAT_8			0x00008000  /* Bank 2 Write Access Time	= 8 cycles */
+#define	B2WAT_9			0x00009000  /* Bank 2 Write Access Time	= 9 cycles */
+#define	B2WAT_10		0x0000A000  /* Bank 2 Write Access Time	= 10 cycles */
+#define	B2WAT_11		0x0000B000  /* Bank 2 Write Access Time	= 11 cycles */
+#define	B2WAT_12		0x0000C000  /* Bank 2 Write Access Time	= 12 cycles */
+#define	B2WAT_13		0x0000D000  /* Bank 2 Write Access Time	= 13 cycles */
+#define	B2WAT_14		0x0000E000  /* Bank 2 Write Access Time	= 14 cycles */
+#define	B2WAT_15		0x0000F000  /* Bank 2 Write Access Time	= 15 cycles */
+#define	B3RDYEN			0x00010000  /* Bank 3 RDY enable, 0=disable, 1=enable */
+#define	B3RDYPOL		0x00020000  /* Bank 3 RDY Active high, 0=active	low, 1=active high */
+#define	B3TT_1			0x00040000  /* Bank 3 Transition Time from Read	to Write = 1 cycle */
+#define	B3TT_2			0x00080000  /* Bank 3 Transition Time from Read	to Write = 2 cycles */
+#define	B3TT_3			0x000C0000  /* Bank 3 Transition Time from Read	to Write = 3 cycles */
+#define	B3TT_4			0x00000000  /* Bank 3 Transition Time from Read	to Write = 4 cycles */
+#define	B3ST_1			0x00100000  /* Bank 3 Setup Time from AOE asserted to Read or Write asserted = 1 cycle */
+#define	B3ST_2			0x00200000  /* Bank 3 Setup Time from AOE asserted to Read or Write asserted = 2 cycles */
+#define	B3ST_3			0x00300000  /* Bank 3 Setup Time from AOE asserted to Read or Write asserted = 3 cycles */
+#define	B3ST_4			0x00000000  /* Bank 3 Setup Time from AOE asserted to Read or Write asserted = 4 cycles */
+#define	B3HT_1			0x00400000  /* Bank 3 Hold Time	from Read or Write deasserted to AOE deasserted	= 1 cycle */
+#define	B3HT_2			0x00800000  /* Bank 3 Hold Time	from Read or Write deasserted to AOE deasserted	= 2 cycles */
+#define	B3HT_3			0x00C00000  /* Bank 3 Hold Time	from Read or Write deasserted to AOE deasserted	= 3 cycles */
+#define	B3HT_0			0x00000000  /* Bank 3 Hold Time	from Read or Write deasserted to AOE deasserted	= 0 cycles */
+#define	B3RAT_1			0x01000000 /* Bank 3 Read Access Time =	1 cycle */
+#define	B3RAT_2			0x02000000  /* Bank 3 Read Access Time = 2 cycles */
+#define	B3RAT_3			0x03000000  /* Bank 3 Read Access Time = 3 cycles */
+#define	B3RAT_4			0x04000000  /* Bank 3 Read Access Time = 4 cycles */
+#define	B3RAT_5			0x05000000  /* Bank 3 Read Access Time = 5 cycles */
+#define	B3RAT_6			0x06000000  /* Bank 3 Read Access Time = 6 cycles */
+#define	B3RAT_7			0x07000000  /* Bank 3 Read Access Time = 7 cycles */
+#define	B3RAT_8			0x08000000  /* Bank 3 Read Access Time = 8 cycles */
+#define	B3RAT_9			0x09000000  /* Bank 3 Read Access Time = 9 cycles */
+#define	B3RAT_10		0x0A000000  /* Bank 3 Read Access Time = 10 cycles */
+#define	B3RAT_11		0x0B000000  /* Bank 3 Read Access Time = 11 cycles */
+#define	B3RAT_12		0x0C000000  /* Bank 3 Read Access Time = 12 cycles */
+#define	B3RAT_13		0x0D000000  /* Bank 3 Read Access Time = 13 cycles */
+#define	B3RAT_14		0x0E000000  /* Bank 3 Read Access Time = 14 cycles */
+#define	B3RAT_15		0x0F000000  /* Bank 3 Read Access Time = 15 cycles */
+#define	B3WAT_1			0x10000000 /* Bank 3 Write Access Time = 1 cycle */
+#define	B3WAT_2			0x20000000  /* Bank 3 Write Access Time	= 2 cycles */
+#define	B3WAT_3			0x30000000  /* Bank 3 Write Access Time	= 3 cycles */
+#define	B3WAT_4			0x40000000  /* Bank 3 Write Access Time	= 4 cycles */
+#define	B3WAT_5			0x50000000  /* Bank 3 Write Access Time	= 5 cycles */
+#define	B3WAT_6			0x60000000  /* Bank 3 Write Access Time	= 6 cycles */
+#define	B3WAT_7			0x70000000  /* Bank 3 Write Access Time	= 7 cycles */
+#define	B3WAT_8			0x80000000  /* Bank 3 Write Access Time	= 8 cycles */
+#define	B3WAT_9			0x90000000  /* Bank 3 Write Access Time	= 9 cycles */
+#define	B3WAT_10		0xA0000000  /* Bank 3 Write Access Time	= 10 cycles */
+#define	B3WAT_11		0xB0000000  /* Bank 3 Write Access Time	= 11 cycles */
+#define	B3WAT_12		0xC0000000  /* Bank 3 Write Access Time	= 12 cycles */
+#define	B3WAT_13		0xD0000000  /* Bank 3 Write Access Time	= 13 cycles */
+#define	B3WAT_14		0xE0000000  /* Bank 3 Write Access Time	= 14 cycles */
+#define	B3WAT_15		0xF0000000  /* Bank 3 Write Access Time	= 15 cycles */
+
+/* **********************  SDRAM CONTROLLER MASKS  *************************** */
+/* EBIU_SDGCTL Masks */
+#define	SCTLE			0x00000001 /* Enable SCLK[0], /SRAS, /SCAS, /SWE, SDQM[3:0] */
+#define	CL_2			0x00000008 /* SDRAM CAS	latency	= 2 cycles */
+#define	CL_3			0x0000000C /* SDRAM CAS	latency	= 3 cycles */
+#define	PFE				0x00000010 /* Enable SDRAM prefetch */
+#define	PFP				0x00000020 /* Prefetch has priority over AMC requests */
+#define	PASR_ALL		0x00000000	/* All 4 SDRAM Banks Refreshed In Self-Refresh */
+#define	PASR_B0_B1		0x00000010	/* SDRAM Banks 0 and 1 Are Refreshed In	Self-Refresh */
+#define	PASR_B0			0x00000020	/* Only	SDRAM Bank 0 Is	Refreshed In Self-Refresh */
+#define	TRAS_1			0x00000040 /* SDRAM tRAS = 1 cycle */
+#define	TRAS_2			0x00000080 /* SDRAM tRAS = 2 cycles */
+#define	TRAS_3			0x000000C0 /* SDRAM tRAS = 3 cycles */
+#define	TRAS_4			0x00000100 /* SDRAM tRAS = 4 cycles */
+#define	TRAS_5			0x00000140 /* SDRAM tRAS = 5 cycles */
+#define	TRAS_6			0x00000180 /* SDRAM tRAS = 6 cycles */
+#define	TRAS_7			0x000001C0 /* SDRAM tRAS = 7 cycles */
+#define	TRAS_8			0x00000200 /* SDRAM tRAS = 8 cycles */
+#define	TRAS_9			0x00000240 /* SDRAM tRAS = 9 cycles */
+#define	TRAS_10			0x00000280 /* SDRAM tRAS = 10 cycles */
+#define	TRAS_11			0x000002C0 /* SDRAM tRAS = 11 cycles */
+#define	TRAS_12			0x00000300 /* SDRAM tRAS = 12 cycles */
+#define	TRAS_13			0x00000340 /* SDRAM tRAS = 13 cycles */
+#define	TRAS_14			0x00000380 /* SDRAM tRAS = 14 cycles */
+#define	TRAS_15			0x000003C0 /* SDRAM tRAS = 15 cycles */
+#define	TRP_1			0x00000800 /* SDRAM tRP	= 1 cycle */
+#define	TRP_2			0x00001000 /* SDRAM tRP	= 2 cycles */
+#define	TRP_3			0x00001800 /* SDRAM tRP	= 3 cycles */
+#define	TRP_4			0x00002000 /* SDRAM tRP	= 4 cycles */
+#define	TRP_5			0x00002800 /* SDRAM tRP	= 5 cycles */
+#define	TRP_6			0x00003000 /* SDRAM tRP	= 6 cycles */
+#define	TRP_7			0x00003800 /* SDRAM tRP	= 7 cycles */
+#define	TRCD_1			0x00008000 /* SDRAM tRCD = 1 cycle */
+#define	TRCD_2			0x00010000 /* SDRAM tRCD = 2 cycles */
+#define	TRCD_3			0x00018000 /* SDRAM tRCD = 3 cycles */
+#define	TRCD_4			0x00020000 /* SDRAM tRCD = 4 cycles */
+#define	TRCD_5			0x00028000 /* SDRAM tRCD = 5 cycles */
+#define	TRCD_6			0x00030000 /* SDRAM tRCD = 6 cycles */
+#define	TRCD_7			0x00038000 /* SDRAM tRCD = 7 cycles */
+#define	TWR_1			0x00080000 /* SDRAM tWR	= 1 cycle */
+#define	TWR_2			0x00100000 /* SDRAM tWR	= 2 cycles */
+#define	TWR_3			0x00180000 /* SDRAM tWR	= 3 cycles */
+#define	PUPSD			0x00200000 /*Power-up start delay */
+#define	PSM				0x00400000 /* SDRAM power-up sequence =	Precharge, mode	register set, 8	CBR refresh cycles */
+#define	PSS				0x00800000 /* enable SDRAM power-up sequence on	next SDRAM access */
+#define	SRFS			0x01000000 /* Start SDRAM self-refresh mode */
+#define	EBUFE			0x02000000 /* Enable external buffering	timing */
+#define	FBBRW			0x04000000 /* Fast back-to-back	read write enable */
+#define	EMREN			0x10000000 /* Extended mode register enable */
+#define	TCSR			0x20000000 /* Temp compensated self refresh value 85 deg C */
+#define	CDDBG			0x40000000 /* Tristate SDRAM controls during bus grant */
+
+/* EBIU_SDBCTL Masks */
+#define	EBE				0x00000001 /* Enable SDRAM external bank */
+#define	EBSZ_16			0x00000000 /* SDRAM external bank size = 16MB */
+#define	EBSZ_32			0x00000002 /* SDRAM external bank size = 32MB */
+#define	EBSZ_64			0x00000004 /* SDRAM external bank size = 64MB */
+#define	EBSZ_128		0x00000006 /* SDRAM external bank size = 128MB */
+#define	EBSZ_256		0x00000008 /* SDRAM External Bank Size = 256MB */
+#define	EBSZ_512		0x0000000A /* SDRAM External Bank Size = 512MB */
+#define	EBCAW_8			0x00000000 /* SDRAM external bank column address width = 8 bits */
+#define	EBCAW_9			0x00000010 /* SDRAM external bank column address width = 9 bits */
+#define	EBCAW_10		0x00000020 /* SDRAM external bank column address width = 9 bits */
+#define	EBCAW_11		0x00000030 /* SDRAM external bank column address width = 9 bits */
+
+/* EBIU_SDSTAT Masks */
+#define	SDCI			0x00000001 /* SDRAM controller is idle */
+#define	SDSRA			0x00000002 /* SDRAM SDRAM self refresh is active */
+#define	SDPUA			0x00000004 /* SDRAM power up active  */
+#define	SDRS			0x00000008 /* SDRAM is in reset	state */
+#define	SDEASE			0x00000010 /* SDRAM EAB	sticky error status - W1C */
+#define	BGSTAT			0x00000020 /* Bus granted */
+
+
+/*  ********************  TWO-WIRE INTERFACE (TWIx) MASKS  ***********************/
+/* TWIx_CLKDIV Macros (Use: *pTWIx_CLKDIV = CLKLOW(x)|CLKHI(y);	 ) */
+#ifdef _MISRA_RULES
+#define	CLKLOW(x)	((x) & 0xFFu)		/* Periods Clock Is Held Low */
+#define	CLKHI(y)	(((y)&0xFFu)<<0x8)	/* Periods Before New Clock Low */
+#else
+#define	CLKLOW(x)	((x) & 0xFF)		/* Periods Clock Is Held Low */
+#define	CLKHI(y)	(((y)&0xFF)<<0x8)	/* Periods Before New Clock Low */
+#endif /* _MISRA_RULES */
+
+/* TWIx_PRESCALE Masks								 */
+#define	PRESCALE	0x007F		/* SCLKs Per Internal Time Reference (10MHz) */
+#define	TWI_ENA		0x0080		/* TWI Enable		 */
+#define	SCCB		0x0200		/* SCCB	Compatibility Enable */
+
+/* TWIx_SLAVE_CTRL Masks								 */
+#define	SEN			0x0001		/* Slave Enable		 */
+#define	SADD_LEN	0x0002		/* Slave Address Length */
+#define	STDVAL		0x0004		/* Slave Transmit Data Valid */
+#define	NAK			0x0008		/* NAK/ACK* Generated At Conclusion Of Transfer */
+#define	GEN			0x0010		/* General Call	Adrress	Matching Enabled */
+
+/* TWIx_SLAVE_STAT Masks								 */
+#define	SDIR		0x0001		/* Slave Transfer Direction (Transmit/Receive*) */
+#define	GCALL		0x0002		/* General Call	Indicator */
+
+/* TWIx_MASTER_CTRL Masks						 */
+#define	MEN			0x0001		/* Master Mode Enable */
+#define	MADD_LEN	0x0002		/* Master Address Length */
+#define	MDIR		0x0004		/* Master Transmit Direction (RX/TX*) */
+#define	FAST		0x0008		/* Use Fast Mode Timing	Specs */
+#define	STOP		0x0010		/* Issue Stop Condition */
+#define	RSTART		0x0020		/* Repeat Start	or Stop* At End	Of Transfer */
+#define	DCNT		0x3FC0		/* Data	Bytes To Transfer */
+#define	SDAOVR		0x4000		/* Serial Data Override */
+#define	SCLOVR		0x8000		/* Serial Clock	Override */
+
+/* TWIx_MASTER_STAT Masks							 */
+#define	MPROG		0x0001		/* Master Transfer In Progress */
+#define	LOSTARB		0x0002		/* Lost	Arbitration Indicator (Xfer Aborted) */
+#define	ANAK		0x0004		/* Address Not Acknowledged */
+#define	DNAK		0x0008		/* Data	Not Acknowledged */
+#define	BUFRDERR	0x0010		/* Buffer Read Error */
+#define	BUFWRERR	0x0020		/* Buffer Write	Error */
+#define	SDASEN		0x0040		/* Serial Data Sense */
+#define	SCLSEN		0x0080		/* Serial Clock	Sense */
+#define	BUSBUSY		0x0100		/* Bus Busy Indicator */
+
+/* TWIx_INT_SRC	and TWIx_INT_ENABLE Masks */
+#define	SINIT		0x0001		/* Slave Transfer Initiated */
+#define	SCOMP		0x0002		/* Slave Transfer Complete */
+#define	SERR		0x0004		/* Slave Transfer Error */
+#define	SOVF		0x0008		/* Slave Overflow */
+#define	MCOMP		0x0010		/* Master Transfer Complete */
+#define	MERR		0x0020		/* Master Transfer Error */
+#define	XMTSERV		0x0040		/* Transmit FIFO Service */
+#define	RCVSERV		0x0080		/* Receive FIFO	Service */
+
+/* TWIx_FIFO_CTRL Masks					 */
+#define	XMTFLUSH	0x0001		/* Transmit Buffer Flush */
+#define	RCVFLUSH	0x0002		/* Receive Buffer Flush */
+#define	XMTINTLEN	0x0004		/* Transmit Buffer Interrupt Length */
+#define	RCVINTLEN	0x0008		/* Receive Buffer Interrupt Length */
+
+/* TWIx_FIFO_STAT Masks								 */
+#define	XMTSTAT		0x0003		/* Transmit FIFO Status */
+#define	XMT_EMPTY	0x0000		/*		Transmit FIFO Empty */
+#define	XMT_HALF	0x0001		/*		Transmit FIFO Has 1 Byte To Write */
+#define	XMT_FULL	0x0003		/*		Transmit FIFO Full (2 Bytes To Write) */
+
+#define	RCVSTAT		0x000C		/* Receive FIFO	Status */
+#define	RCV_EMPTY	0x0000		/*		Receive	FIFO Empty */
+#define	RCV_HALF	0x0004		/*		Receive	FIFO Has 1 Byte	To Read */
+#define	RCV_FULL	0x000C		/*		Receive	FIFO Full (2 Bytes To Read) */
+
+
+/********************************* MXVR	MASKS ****************************************/
+
+/* MXVR_CONFIG Masks */
+
+#define	MXVREN	  0x00000001lu
+#define	MMSM	  0x00000002lu
+#define	ACTIVE	  0x00000004lu
+#define	SDELAY	  0x00000008lu
+#define	NCMRXEN	  0x00000010lu
+#define	RWRRXEN	  0x00000020lu
+#define	MTXEN	  0x00000040lu
+#define	MTXON	  0x00000080lu /*legacy*/
+#define	MTXONB	  0x00000080lu
+#define	EPARITY	  0x00000100lu
+#define	MSB	  0x00001E00lu
+#define	APRXEN	  0x00002000lu
+#define	WAKEUP	  0x00004000lu
+#define	LMECH	  0x00008000lu
+
+#ifdef _MISRA_RULES
+#define	SET_MSB(x)     (((x)&0xFu) << 0x9)
+#else
+#define	SET_MSB(x)     (((x)&0xF) << 0x9)
+#endif /* _MISRA_RULES */
+
+
+/* MXVR_PLL_CTL_0 Masks */
+
+#define	MXTALCEN  0x00000001lu
+#define	MXTALFEN  0x00000002lu
+#define	MPLLMS	  0x00000008lu
+#define	MXTALMUL  0x00000030lu
+#define	MPLLEN	  0x00000040lu
+#define	MPLLEN0	  0x00000040lu /* legacy */
+#define	MPLLEN1	  0x00000080lu /* legacy */
+#define	MMCLKEN	  0x00000100lu
+#define	MMCLKMUL  0x00001E00lu
+#define	MPLLRSTB  0x00002000lu
+#define	MPLLRSTB0 0x00002000lu /* legacy */
+#define	MPLLRSTB1 0x00004000lu /* legacy */
+#define	MBCLKEN	  0x00010000lu
+#define	MBCLKDIV  0x001E0000lu
+#define	MPLLCDR	  0x00200000lu
+#define	MPLLCDR0  0x00200000lu /* legacy */
+#define	MPLLCDR1  0x00400000lu /* legacy */
+#define	INVRX	  0x00800000lu
+#define	MFSEN	  0x01000000lu
+#define	MFSDIV	  0x1E000000lu
+#define	MFSSEL	  0x60000000lu
+#define	MFSSYNC	  0x80000000lu
+
+#define	MXTALMUL_256FS	 0x00000000lu /* legacy */
+#define	MXTALMUL_384FS	 0x00000010lu /* legacy */
+#define	MXTALMUL_512FS	 0x00000020lu /* legacy */
+#define	MXTALMUL_1024FS	 0x00000030lu
+
+#define	MMCLKMUL_1024FS	 0x00000000lu
+#define	MMCLKMUL_512FS	 0x00000200lu
+#define	MMCLKMUL_256FS	 0x00000400lu
+#define	MMCLKMUL_128FS	 0x00000600lu
+#define	MMCLKMUL_64FS	 0x00000800lu
+#define	MMCLKMUL_32FS	 0x00000A00lu
+#define	MMCLKMUL_16FS	 0x00000C00lu
+#define	MMCLKMUL_8FS	 0x00000E00lu
+#define	MMCLKMUL_4FS	 0x00001000lu
+#define	MMCLKMUL_2FS	 0x00001200lu
+#define	MMCLKMUL_1FS	 0x00001400lu
+#define	MMCLKMUL_1536FS	 0x00001A00lu
+#define	MMCLKMUL_768FS	 0x00001C00lu
+#define	MMCLKMUL_384FS	 0x00001E00lu
+
+#define	MBCLKDIV_DIV2	 0x00020000lu
+#define	MBCLKDIV_DIV4	 0x00040000lu
+#define	MBCLKDIV_DIV8	 0x00060000lu
+#define	MBCLKDIV_DIV16	 0x00080000lu
+#define	MBCLKDIV_DIV32	 0x000A0000lu
+#define	MBCLKDIV_DIV64	 0x000C0000lu
+#define	MBCLKDIV_DIV128	 0x000E0000lu
+#define	MBCLKDIV_DIV256	 0x00100000lu
+#define	MBCLKDIV_DIV512	 0x00120000lu
+#define	MBCLKDIV_DIV1024 0x00140000lu
+
+#define	MFSDIV_DIV2	 0x02000000lu
+#define	MFSDIV_DIV4	 0x04000000lu
+#define	MFSDIV_DIV8	 0x06000000lu
+#define	MFSDIV_DIV16	 0x08000000lu
+#define	MFSDIV_DIV32	 0x0A000000lu
+#define	MFSDIV_DIV64	 0x0C000000lu
+#define	MFSDIV_DIV128	 0x0E000000lu
+#define	MFSDIV_DIV256	 0x10000000lu
+#define	MFSDIV_DIV512	 0x12000000lu
+#define	MFSDIV_DIV1024	 0x14000000lu
+
+#define	MFSSEL_CLOCK	 0x00000000lu
+#define	MFSSEL_PULSE_HI	 0x20000000lu
+#define	MFSSEL_PULSE_LO	 0x40000000lu
+
+
+/* MXVR_PLL_CTL_1 Masks */
+
+#define	MSTO	   0x00000001lu
+#define	MSTO0	   0x00000001lu	/* legacy */
+#define	MHOGGD	   0x00000004lu
+#define	MHOGGD0	   0x00000004lu	/* legacy */
+#define	MHOGGD1	   0x00000008lu	/* legacy */
+#define	MSHAPEREN  0x00000010lu
+#define	MSHAPEREN0 0x00000010lu	/* legacy */
+#define	MSHAPEREN1 0x00000020lu	/* legacy */
+#define	MPLLCNTEN  0x00008000lu
+#define	MPLLCNT	   0xFFFF0000lu
+
+#ifdef _MISRA_RULES
+#define	SET_MPLLCNT(x)	   (((x)&0xFFFFu) << 0x10)
+#else
+#define	SET_MPLLCNT(x)	   (((x)&0xFFFF) << 0x10)
+#endif /* _MISRA_RULES */
+
+
+/* MXVR_PLL_CTL_2 Masks */
+
+#define	MSHAPERSEL 0x00000007lu
+#define	MCPSEL	   0x000000E0lu
+
+/* MXVR_INT_STAT_0 Masks */
+
+#define	NI2A  0x00000001lu
+#define	NA2I  0x00000002lu
+#define	SBU2L 0x00000004lu
+#define	SBL2U 0x00000008lu
+#define	PRU   0x00000010lu
+#define	MPRU  0x00000020lu
+#define	DRU   0x00000040lu
+#define	MDRU  0x00000080lu
+#define	SBU   0x00000100lu
+#define	ATU   0x00000200lu
+#define	FCZ0  0x00000400lu
+#define	FCZ1  0x00000800lu
+#define	PERR  0x00001000lu
+#define	MH2L  0x00002000lu
+#define	ML2H  0x00004000lu
+#define	WUP   0x00008000lu
+#define	FU2L  0x00010000lu
+#define	FL2U  0x00020000lu
+#define	BU2L  0x00040000lu
+#define	BL2U  0x00080000lu
+#define	PCZ   0x00400000lu
+#define	FERR  0x00800000lu
+#define	CMR   0x01000000lu
+#define	CMROF 0x02000000lu
+#define	CMTS  0x04000000lu
+#define	CMTC  0x08000000lu
+#define	RWRC  0x10000000lu
+#define	BCZ   0x20000000lu
+#define	BMERR 0x40000000lu
+#define	DERR  0x80000000lu
+
+
+/* MXVR_INT_EN_0 Masks */
+
+#define	NI2AEN	NI2A
+#define	NA2IEN	NA2I
+#define	SBU2LEN	SBU2L
+#define	SBL2UEN	SBL2U
+#define	PRUEN	PRU
+#define	MPRUEN	MPRU
+#define	DRUEN	DRU
+#define	MDRUEN	MDRU
+#define	SBUEN	SBU
+#define	ATUEN	ATU
+#define	FCZ0EN	FCZ0
+#define	FCZ1EN	FCZ1
+#define	PERREN	PERR
+#define	MH2LEN	MH2L
+#define	ML2HEN	ML2H
+#define	WUPEN	WUP
+#define	FU2LEN	FU2L
+#define	FL2UEN	FL2U
+#define	BU2LEN	BU2L
+#define	BL2UEN	BL2U
+#define	PCZEN	PCZ
+#define	FERREN	FERR
+#define	CMREN	CMR
+#define	CMROFEN	CMROF
+#define	CMTSEN	CMTS
+#define	CMTCEN	CMTC
+#define	RWRCEN	RWRC
+#define	BCZEN	BCZ
+#define	BMERREN	BMERR
+#define	DERREN	DERR
+
+
+/* MXVR_INT_STAT_1 Masks */
+
+#define	APR   0x00000004lu
+#define	APROF 0x00000008lu
+#define	APTS  0x00000040lu
+#define	APTC  0x00000080lu
+#define	APRCE 0x00000400lu
+#define	APRPE 0x00000800lu
+
+#define	HDONE0 0x00000001lu
+#define	DONE0  0x00000002lu
+#define	HDONE1 0x00000010lu
+#define	DONE1  0x00000020lu
+#define	HDONE2 0x00000100lu
+#define	DONE2  0x00000200lu
+#define	HDONE3 0x00001000lu
+#define	DONE3  0x00002000lu
+#define	HDONE4 0x00010000lu
+#define	DONE4  0x00020000lu
+#define	HDONE5 0x00100000lu
+#define	DONE5  0x00200000lu
+#define	HDONE6 0x01000000lu
+#define	DONE6  0x02000000lu
+#define	HDONE7 0x10000000lu
+#define	DONE7  0x20000000lu
+
+#define	DONEX(x) (0x00000002 <<	(4 * (x)))
+#define	HDONEX(x) (0x00000001 << (4 * (x)))
+
+
+/* MXVR_INT_EN_1 Masks */
+
+#define	APREN	APR
+#define	APROFEN	APROF
+#define	APTSEN	APTS
+#define	APTCEN	APTC
+#define	APRCEEN	APRCE
+#define	APRPEEN	APRPE
+
+#define	HDONEEN0 HDONE0
+#define	DONEEN0	 DONE0
+#define	HDONEEN1 HDONE1
+#define	DONEEN1	 DONE1
+#define	HDONEEN2 HDONE2
+#define	DONEEN2	 DONE2
+#define	HDONEEN3 HDONE3
+#define	DONEEN3	 DONE3
+#define	HDONEEN4 HDONE4
+#define	DONEEN4	 DONE4
+#define	HDONEEN5 HDONE5
+#define	DONEEN5	 DONE5
+#define	HDONEEN6 HDONE6
+#define	DONEEN6	 DONE6
+#define	HDONEEN7 HDONE7
+#define	DONEEN7	 DONE7
+
+#define	DONEENX(x) (0x00000002 << (4 * (x)))
+#define	HDONEENX(x) (0x00000001	<< (4 *	(x)))
+
+
+/* MXVR_STATE_0	Masks */
+
+#define	NACT	 0x00000001lu
+#define	SBLOCK	 0x00000002lu
+#define	PFDLOCK	 0x00000004lu
+#define	PFDLOCK0 0x00000004lu /* legacy */
+#define	PDD	 0x00000008lu
+#define	PDD0	 0x00000008lu /* legacy */
+#define	PVCO	 0x00000010lu
+#define	PVCO0	 0x00000010lu /* legacy */
+#define	PFDLOCK1 0x00000020lu /* legacy */
+#define	PDD1	 0x00000040lu /* legacy */
+#define	PVCO1	 0x00000080lu /* legacy */
+#define	APBSY	 0x00000100lu
+#define	APARB	 0x00000200lu
+#define	APTX	 0x00000400lu
+#define	APRX	 0x00000800lu
+#define	CMBSY	 0x00001000lu
+#define	CMARB	 0x00002000lu
+#define	CMTX	 0x00004000lu
+#define	CMRX	 0x00008000lu
+#define	MRXONB	 0x00010000lu
+#define	RGSIP	 0x00020000lu
+#define	DALIP	 0x00040000lu
+#define	ALIP	 0x00080000lu
+#define	RRDIP	 0x00100000lu
+#define	RWRIP	 0x00200000lu
+#define	FLOCK	 0x00400000lu
+#define	BLOCK	 0x00800000lu
+#define	RSB	 0x0F000000lu
+#define	DERRNUM	 0xF0000000lu
+
+
+/* MXVR_STATE_1	Masks */
+
+#define	STXNUMB	    0x0000000Flu
+#define	SRXNUMB	    0x000000F0lu
+#define	APCONT	    0x00000100lu
+#define	DMAACTIVEX  0x00FF0000lu
+#define	DMAACTIVE0  0x00010000lu
+#define	DMAACTIVE1  0x00020000lu
+#define	DMAACTIVE2  0x00040000lu
+#define	DMAACTIVE3  0x00080000lu
+#define	DMAACTIVE4  0x00100000lu
+#define	DMAACTIVE5  0x00200000lu
+#define	DMAACTIVE6  0x00400000lu
+#define	DMAACTIVE7  0x00800000lu
+#define	DMAPMENX    0xFF000000lu
+#define	DMAPMEN0    0x01000000lu
+#define	DMAPMEN1    0x02000000lu
+#define	DMAPMEN2    0x04000000lu
+#define	DMAPMEN3    0x08000000lu
+#define	DMAPMEN4    0x10000000lu
+#define	DMAPMEN5    0x20000000lu
+#define	DMAPMEN6    0x40000000lu
+#define	DMAPMEN7    0x80000000lu
+
+
+/* MXVR_POSITION Masks */
+
+#define	PVALID	     0x8000
+#define	POSITION     0x003F
+
+
+/* MXVR_MAX_POSITION Masks */
+
+#define	MPVALID	     0x8000
+#define	MPOSITION    0x003F
+
+
+/* MXVR_DELAY Masks */
+
+#define	DVALID	     0x8000
+#define	DELAY	     0x003F
+
+
+/* MXVR_MAX_DELAY Masks */
+
+#define	MDVALID	     0x8000
+#define	MDELAY	     0x003F
+
+
+/* MXVR_LADDR Masks */
+
+#define	LVALID	     0x80000000lu
+#define	LADDR	     0x0000FFFFlu
+
+
+/* MXVR_GADDR Masks */
+
+#define	GVALID	     0x8000
+#define	GADDRL	     0x00FF
+
+
+/* MXVR_AADDR Masks */
+
+#define	AVALID	     0x80000000lu
+#define	AADDR	     0x0000FFFFlu
+
+
+/* MXVR_ALLOC_0	Masks */
+
+#define	CIU0	     0x00000080lu
+#define	CIU1	     0x00008000lu
+#define	CIU2	     0x00800000lu
+#define	CIU3	     0x80000000lu
+
+#define	CL0	     0x0000007Flu
+#define	CL1	     0x00007F00lu
+#define	CL2	     0x007F0000lu
+#define	CL3	     0x7F000000lu
+
+
+/* MXVR_ALLOC_1	Masks */
+
+#define	CIU4	     0x00000080lu
+#define	CIU5	     0x00008000lu
+#define	CIU6	     0x00800000lu
+#define	CIU7	     0x80000000lu
+
+#define	CL4	     0x0000007Flu
+#define	CL5	     0x00007F00lu
+#define	CL6	     0x007F0000lu
+#define	CL7	     0x7F000000lu
+
+
+/* MXVR_ALLOC_2	Masks */
+
+#define	CIU8	     0x00000080lu
+#define	CIU9	     0x00008000lu
+#define	CIU10	     0x00800000lu
+#define	CIU11	     0x80000000lu
+
+#define	CL8	     0x0000007Flu
+#define	CL9	     0x00007F00lu
+#define	CL10	     0x007F0000lu
+#define	CL11	     0x7F000000lu
+
+
+/* MXVR_ALLOC_3	Masks */
+
+#define	CIU12	     0x00000080lu
+#define	CIU13	     0x00008000lu
+#define	CIU14	     0x00800000lu
+#define	CIU15	     0x80000000lu
+
+#define	CL12	     0x0000007Flu
+#define	CL13	     0x00007F00lu
+#define	CL14	     0x007F0000lu
+#define	CL15	     0x7F000000lu
+
+
+/* MXVR_ALLOC_4	Masks */
+
+#define	CIU16	     0x00000080lu
+#define	CIU17	     0x00008000lu
+#define	CIU18	     0x00800000lu
+#define	CIU19	     0x80000000lu
+
+#define	CL16	     0x0000007Flu
+#define	CL17	     0x00007F00lu
+#define	CL18	     0x007F0000lu
+#define	CL19	     0x7F000000lu
+
+
+/* MXVR_ALLOC_5	Masks */
+
+#define	CIU20	     0x00000080lu
+#define	CIU21	     0x00008000lu
+#define	CIU22	     0x00800000lu
+#define	CIU23	     0x80000000lu
+
+#define	CL20	     0x0000007Flu
+#define	CL21	     0x00007F00lu
+#define	CL22	     0x007F0000lu
+#define	CL23	     0x7F000000lu
+
+
+/* MXVR_ALLOC_6	Masks */
+
+#define	CIU24	     0x00000080lu
+#define	CIU25	     0x00008000lu
+#define	CIU26	     0x00800000lu
+#define	CIU27	     0x80000000lu
+
+#define	CL24	     0x0000007Flu
+#define	CL25	     0x00007F00lu
+#define	CL26	     0x007F0000lu
+#define	CL27	     0x7F000000lu
+
+
+/* MXVR_ALLOC_7	Masks */
+
+#define	CIU28	     0x00000080lu
+#define	CIU29	     0x00008000lu
+#define	CIU30	     0x00800000lu
+#define	CIU31	     0x80000000lu
+
+#define	CL28	     0x0000007Flu
+#define	CL29	     0x00007F00lu
+#define	CL30	     0x007F0000lu
+#define	CL31	     0x7F000000lu
+
+
+/* MXVR_ALLOC_8	Masks */
+
+#define	CIU32	     0x00000080lu
+#define	CIU33	     0x00008000lu
+#define	CIU34	     0x00800000lu
+#define	CIU35	     0x80000000lu
+
+#define	CL32	     0x0000007Flu
+#define	CL33	     0x00007F00lu
+#define	CL34	     0x007F0000lu
+#define	CL35	     0x7F000000lu
+
+
+/* MXVR_ALLOC_9	Masks */
+
+#define	CIU36	     0x00000080lu
+#define	CIU37	     0x00008000lu
+#define	CIU38	     0x00800000lu
+#define	CIU39	     0x80000000lu
+
+#define	CL36	     0x0000007Flu
+#define	CL37	     0x00007F00lu
+#define	CL38	     0x007F0000lu
+#define	CL39	     0x7F000000lu
+
+
+/* MXVR_ALLOC_10 Masks */
+
+#define	CIU40	     0x00000080lu
+#define	CIU41	     0x00008000lu
+#define	CIU42	     0x00800000lu
+#define	CIU43	     0x80000000lu
+
+#define	CL40	     0x0000007Flu
+#define	CL41	     0x00007F00lu
+#define	CL42	     0x007F0000lu
+#define	CL43	     0x7F000000lu
+
+
+/* MXVR_ALLOC_11 Masks */
+
+#define	CIU44	     0x00000080lu
+#define	CIU45	     0x00008000lu
+#define	CIU46	     0x00800000lu
+#define	CIU47	     0x80000000lu
+
+#define	CL44	     0x0000007Flu
+#define	CL45	     0x00007F00lu
+#define	CL46	     0x007F0000lu
+#define	CL47	     0x7F000000lu
+
+
+/* MXVR_ALLOC_12 Masks */
+
+#define	CIU48	     0x00000080lu
+#define	CIU49	     0x00008000lu
+#define	CIU50	     0x00800000lu
+#define	CIU51	     0x80000000lu
+
+#define	CL48	     0x0000007Flu
+#define	CL49	     0x00007F00lu
+#define	CL50	     0x007F0000lu
+#define	CL51	     0x7F000000lu
+
+
+/* MXVR_ALLOC_13 Masks */
+
+#define	CIU52	     0x00000080lu
+#define	CIU53	     0x00008000lu
+#define	CIU54	     0x00800000lu
+#define	CIU55	     0x80000000lu
+
+#define	CL52	     0x0000007Flu
+#define	CL53	     0x00007F00lu
+#define	CL54	     0x007F0000lu
+#define	CL55	     0x7F000000lu
+
+
+/* MXVR_ALLOC_14 Masks */
+
+#define	CIU56	     0x00000080lu
+#define	CIU57	     0x00008000lu
+#define	CIU58	     0x00800000lu
+#define	CIU59	     0x80000000lu
+
+#define	CL56	     0x0000007Flu
+#define	CL57	     0x00007F00lu
+#define	CL58	     0x007F0000lu
+#define	CL59	     0x7F000000lu
+
+
+/* MXVR_SYNC_LCHAN_0 Masks */
+
+#define	LCHANPC0     0x0000000Flu
+#define	LCHANPC1     0x000000F0lu
+#define	LCHANPC2     0x00000F00lu
+#define	LCHANPC3     0x0000F000lu
+#define	LCHANPC4     0x000F0000lu
+#define	LCHANPC5     0x00F00000lu
+#define	LCHANPC6     0x0F000000lu
+#define	LCHANPC7     0xF0000000lu
+
+
+/* MXVR_SYNC_LCHAN_1 Masks */
+
+#define	LCHANPC8     0x0000000Flu
+#define	LCHANPC9     0x000000F0lu
+#define	LCHANPC10    0x00000F00lu
+#define	LCHANPC11    0x0000F000lu
+#define	LCHANPC12    0x000F0000lu
+#define	LCHANPC13    0x00F00000lu
+#define	LCHANPC14    0x0F000000lu
+#define	LCHANPC15    0xF0000000lu
+
+
+/* MXVR_SYNC_LCHAN_2 Masks */
+
+#define	LCHANPC16    0x0000000Flu
+#define	LCHANPC17    0x000000F0lu
+#define	LCHANPC18    0x00000F00lu
+#define	LCHANPC19    0x0000F000lu
+#define	LCHANPC20    0x000F0000lu
+#define	LCHANPC21    0x00F00000lu
+#define	LCHANPC22    0x0F000000lu
+#define	LCHANPC23    0xF0000000lu
+
+
+/* MXVR_SYNC_LCHAN_3 Masks */
+
+#define	LCHANPC24    0x0000000Flu
+#define	LCHANPC25    0x000000F0lu
+#define	LCHANPC26    0x00000F00lu
+#define	LCHANPC27    0x0000F000lu
+#define	LCHANPC28    0x000F0000lu
+#define	LCHANPC29    0x00F00000lu
+#define	LCHANPC30    0x0F000000lu
+#define	LCHANPC31    0xF0000000lu
+
+
+/* MXVR_SYNC_LCHAN_4 Masks */
+
+#define	LCHANPC32    0x0000000Flu
+#define	LCHANPC33    0x000000F0lu
+#define	LCHANPC34    0x00000F00lu
+#define	LCHANPC35    0x0000F000lu
+#define	LCHANPC36    0x000F0000lu
+#define	LCHANPC37    0x00F00000lu
+#define	LCHANPC38    0x0F000000lu
+#define	LCHANPC39    0xF0000000lu
+
+
+/* MXVR_SYNC_LCHAN_5 Masks */
+
+#define	LCHANPC40    0x0000000Flu
+#define	LCHANPC41    0x000000F0lu
+#define	LCHANPC42    0x00000F00lu
+#define	LCHANPC43    0x0000F000lu
+#define	LCHANPC44    0x000F0000lu
+#define	LCHANPC45    0x00F00000lu
+#define	LCHANPC46    0x0F000000lu
+#define	LCHANPC47    0xF0000000lu
+
+
+/* MXVR_SYNC_LCHAN_6 Masks */
+
+#define	LCHANPC48    0x0000000Flu
+#define	LCHANPC49    0x000000F0lu
+#define	LCHANPC50    0x00000F00lu
+#define	LCHANPC51    0x0000F000lu
+#define	LCHANPC52    0x000F0000lu
+#define	LCHANPC53    0x00F00000lu
+#define	LCHANPC54    0x0F000000lu
+#define	LCHANPC55    0xF0000000lu
+
+
+/* MXVR_SYNC_LCHAN_7 Masks */
+
+#define	LCHANPC56    0x0000000Flu
+#define	LCHANPC57    0x000000F0lu
+#define	LCHANPC58    0x00000F00lu
+#define	LCHANPC59    0x0000F000lu
+
+
+/* MXVR_DMAx_CONFIG Masks */
+
+#define	MDMAEN	    0x00000001lu
+#define	DD	    0x00000002lu
+#define	LCHAN	    0x000003C0lu
+#define	BITSWAPEN   0x00000400lu
+#define	BYSWAPEN    0x00000800lu
+#define	MFLOW	    0x00007000lu
+#define	FIXEDPM	    0x00080000lu
+#define	STARTPAT    0x00300000lu
+#define	STOPPAT	    0x00C00000lu
+#define	COUNTPOS    0x1C000000lu
+
+#define	DD_TX	    0x00000000lu
+#define	DD_RX	    0x00000002lu
+
+#define	LCHAN_0	    0x00000000lu
+#define	LCHAN_1	    0x00000040lu
+#define	LCHAN_2	    0x00000080lu
+#define	LCHAN_3	    0x000000C0lu
+#define	LCHAN_4	    0x00000100lu
+#define	LCHAN_5	    0x00000140lu
+#define	LCHAN_6	    0x00000180lu
+#define	LCHAN_7	    0x000001C0lu
+
+#define	MFLOW_STOP  0x00000000lu
+#define	MFLOW_AUTO  0x00001000lu
+#define	MFLOW_PVC   0x00002000lu
+#define	MFLOW_PSS   0x00003000lu
+#define	MFLOW_PFC   0x00004000lu
+
+#define	STARTPAT_0  0x00000000lu
+#define	STARTPAT_1  0x00100000lu
+
+#define	STOPPAT_0   0x00000000lu
+#define	STOPPAT_1   0x00400000lu
+
+#define	COUNTPOS_0  0x00000000lu
+#define	COUNTPOS_1  0x04000000lu
+#define	COUNTPOS_2  0x08000000lu
+#define	COUNTPOS_3  0x0C000000lu
+#define	COUNTPOS_4  0x10000000lu
+#define	COUNTPOS_5  0x14000000lu
+#define	COUNTPOS_6  0x18000000lu
+#define	COUNTPOS_7  0x1C000000lu
+
+
+/* MXVR_AP_CTL Masks */
+
+#define	STARTAP	   0x00000001lu
+#define	CANCELAP   0x00000002lu
+#define	RESETAP	   0x00000004lu
+#define	APRBE0	   0x00004000lu
+#define	APRBE1	   0x00008000lu
+#define	APRBEX	   0x0000C000lu
+
+
+/* MXVR_CM_CTL Masks */
+
+#define	STARTCM	   0x00000001lu
+#define	CANCELCM   0x00000002lu
+#define	CMRBEX	   0xFFFF0000lu
+#define	CMRBE0	   0x00010000lu
+#define	CMRBE1	   0x00020000lu
+#define	CMRBE2	   0x00040000lu
+#define	CMRBE3	   0x00080000lu
+#define	CMRBE4	   0x00100000lu
+#define	CMRBE5	   0x00200000lu
+#define	CMRBE6	   0x00400000lu
+#define	CMRBE7	   0x00800000lu
+#define	CMRBE8	   0x01000000lu
+#define	CMRBE9	   0x02000000lu
+#define	CMRBE10	   0x04000000lu
+#define	CMRBE11	   0x08000000lu
+#define	CMRBE12	   0x10000000lu
+#define	CMRBE13	   0x20000000lu
+#define	CMRBE14	   0x40000000lu
+#define	CMRBE15	   0x80000000lu
+
+
+/* MXVR_PAT_DATA_x Masks */
+
+#define	MATCH_DATA_0 0x000000FFlu
+#define	MATCH_DATA_1 0x0000FF00lu
+#define	MATCH_DATA_2 0x00FF0000lu
+#define	MATCH_DATA_3 0xFF000000lu
+
+
+
+/* MXVR_PAT_EN_x Masks */
+
+#define	MATCH_EN_0_0 0x00000001lu
+#define	MATCH_EN_0_1 0x00000002lu
+#define	MATCH_EN_0_2 0x00000004lu
+#define	MATCH_EN_0_3 0x00000008lu
+#define	MATCH_EN_0_4 0x00000010lu
+#define	MATCH_EN_0_5 0x00000020lu
+#define	MATCH_EN_0_6 0x00000040lu
+#define	MATCH_EN_0_7 0x00000080lu
+
+#define	MATCH_EN_1_0 0x00000100lu
+#define	MATCH_EN_1_1 0x00000200lu
+#define	MATCH_EN_1_2 0x00000400lu
+#define	MATCH_EN_1_3 0x00000800lu
+#define	MATCH_EN_1_4 0x00001000lu
+#define	MATCH_EN_1_5 0x00002000lu
+#define	MATCH_EN_1_6 0x00004000lu
+#define	MATCH_EN_1_7 0x00008000lu
+
+#define	MATCH_EN_2_0 0x00010000lu
+#define	MATCH_EN_2_1 0x00020000lu
+#define	MATCH_EN_2_2 0x00040000lu
+#define	MATCH_EN_2_3 0x00080000lu
+#define	MATCH_EN_2_4 0x00100000lu
+#define	MATCH_EN_2_5 0x00200000lu
+#define	MATCH_EN_2_6 0x00400000lu
+#define	MATCH_EN_2_7 0x00800000lu
+
+#define	MATCH_EN_3_0 0x01000000lu
+#define	MATCH_EN_3_1 0x02000000lu
+#define	MATCH_EN_3_2 0x04000000lu
+#define	MATCH_EN_3_3 0x08000000lu
+#define	MATCH_EN_3_4 0x10000000lu
+#define	MATCH_EN_3_5 0x20000000lu
+#define	MATCH_EN_3_6 0x40000000lu
+#define	MATCH_EN_3_7 0x80000000lu
+
+
+/* MXVR_ROUTING_0 Masks */
+
+#define	MUTE_CH0	0x00000080lu
+#define	MUTE_CH1	0x00008000lu
+#define	MUTE_CH2	0x00800000lu
+#define	MUTE_CH3	0x80000000lu
+
+#define	TX_CH0		0x0000007Flu
+#define	TX_CH1		0x00007F00lu
+#define	TX_CH2		0x007F0000lu
+#define	TX_CH3		0x7F000000lu
+
+
+/* MXVR_ROUTING_1 Masks */
+
+#define	MUTE_CH4	0x00000080lu
+#define	MUTE_CH5	0x00008000lu
+#define	MUTE_CH6	0x00800000lu
+#define	MUTE_CH7	0x80000000lu
+
+#define	TX_CH4		0x0000007Flu
+#define	TX_CH5		0x00007F00lu
+#define	TX_CH6		0x007F0000lu
+#define	TX_CH7		0x7F000000lu
+
+
+/* MXVR_ROUTING_2 Masks */
+
+#define	MUTE_CH8	0x00000080lu
+#define	MUTE_CH9	0x00008000lu
+#define	MUTE_CH10	0x00800000lu
+#define	MUTE_CH11	0x80000000lu
+
+#define	TX_CH8		0x0000007Flu
+#define	TX_CH9		0x00007F00lu
+#define	TX_CH10		0x007F0000lu
+#define	TX_CH11		0x7F000000lu
+
+/* MXVR_ROUTING_3 Masks */
+
+#define	MUTE_CH12	0x00000080lu
+#define	MUTE_CH13	0x00008000lu
+#define	MUTE_CH14	0x00800000lu
+#define	MUTE_CH15	0x80000000lu
+
+#define	TX_CH12		0x0000007Flu
+#define	TX_CH13		0x00007F00lu
+#define	TX_CH14		0x007F0000lu
+#define	TX_CH15		0x7F000000lu
+
+
+/* MXVR_ROUTING_4 Masks */
+
+#define	MUTE_CH16	0x00000080lu
+#define	MUTE_CH17	0x00008000lu
+#define	MUTE_CH18	0x00800000lu
+#define	MUTE_CH19	0x80000000lu
+
+#define	TX_CH16		0x0000007Flu
+#define	TX_CH17		0x00007F00lu
+#define	TX_CH18		0x007F0000lu
+#define	TX_CH19		0x7F000000lu
+
+
+/* MXVR_ROUTING_5 Masks */
+
+#define	MUTE_CH20	0x00000080lu
+#define	MUTE_CH21	0x00008000lu
+#define	MUTE_CH22	0x00800000lu
+#define	MUTE_CH23	0x80000000lu
+
+#define	TX_CH20		0x0000007Flu
+#define	TX_CH21		0x00007F00lu
+#define	TX_CH22		0x007F0000lu
+#define	TX_CH23		0x7F000000lu
+
+
+/* MXVR_ROUTING_6 Masks */
+
+#define	MUTE_CH24	0x00000080lu
+#define	MUTE_CH25	0x00008000lu
+#define	MUTE_CH26	0x00800000lu
+#define	MUTE_CH27	0x80000000lu
+
+#define	TX_CH24		0x0000007Flu
+#define	TX_CH25		0x00007F00lu
+#define	TX_CH26		0x007F0000lu
+#define	TX_CH27		0x7F000000lu
+
+
+/* MXVR_ROUTING_7 Masks */
+
+#define	MUTE_CH28	0x00000080lu
+#define	MUTE_CH29	0x00008000lu
+#define	MUTE_CH30	0x00800000lu
+#define	MUTE_CH31	0x80000000lu
+
+#define	TX_CH28		0x0000007Flu
+#define	TX_CH29		0x00007F00lu
+#define	TX_CH30		0x007F0000lu
+#define	TX_CH31		0x7F000000lu
+
+
+/* MXVR_ROUTING_8 Masks */
+
+#define	MUTE_CH32	0x00000080lu
+#define	MUTE_CH33	0x00008000lu
+#define	MUTE_CH34	0x00800000lu
+#define	MUTE_CH35	0x80000000lu
+
+#define	TX_CH32		0x0000007Flu
+#define	TX_CH33		0x00007F00lu
+#define	TX_CH34		0x007F0000lu
+#define	TX_CH35		0x7F000000lu
+
+
+/* MXVR_ROUTING_9 Masks */
+
+#define	MUTE_CH36	0x00000080lu
+#define	MUTE_CH37	0x00008000lu
+#define	MUTE_CH38	0x00800000lu
+#define	MUTE_CH39	0x80000000lu
+
+#define	TX_CH36		0x0000007Flu
+#define	TX_CH37		0x00007F00lu
+#define	TX_CH38		0x007F0000lu
+#define	TX_CH39		0x7F000000lu
+
+
+/* MXVR_ROUTING_10 Masks */
+
+#define	MUTE_CH40	0x00000080lu
+#define	MUTE_CH41	0x00008000lu
+#define	MUTE_CH42	0x00800000lu
+#define	MUTE_CH43	0x80000000lu
+
+#define	TX_CH40		0x0000007Flu
+#define	TX_CH41		0x00007F00lu
+#define	TX_CH42		0x007F0000lu
+#define	TX_CH43		0x7F000000lu
+
+
+/* MXVR_ROUTING_11 Masks */
+
+#define	MUTE_CH44	0x00000080lu
+#define	MUTE_CH45	0x00008000lu
+#define	MUTE_CH46	0x00800000lu
+#define	MUTE_CH47	0x80000000lu
+
+#define	TX_CH44		0x0000007Flu
+#define	TX_CH45		0x00007F00lu
+#define	TX_CH46		0x007F0000lu
+#define	TX_CH47		0x7F000000lu
+
+
+/* MXVR_ROUTING_12 Masks */
+
+#define	MUTE_CH48	0x00000080lu
+#define	MUTE_CH49	0x00008000lu
+#define	MUTE_CH50	0x00800000lu
+#define	MUTE_CH51	0x80000000lu
+
+#define	TX_CH48		0x0000007Flu
+#define	TX_CH49		0x00007F00lu
+#define	TX_CH50		0x007F0000lu
+#define	TX_CH51		0x7F000000lu
+
+
+/* MXVR_ROUTING_13 Masks */
+
+#define	MUTE_CH52	0x00000080lu
+#define	MUTE_CH53	0x00008000lu
+#define	MUTE_CH54	0x00800000lu
+#define	MUTE_CH55	0x80000000lu
+
+#define	TX_CH52		0x0000007Flu
+#define	TX_CH53		0x00007F00lu
+#define	TX_CH54		0x007F0000lu
+#define	TX_CH55		0x7F000000lu
+
+
+/* MXVR_ROUTING_14 Masks */
+
+#define	MUTE_CH56	0x00000080lu
+#define	MUTE_CH57	0x00008000lu
+#define	MUTE_CH58	0x00800000lu
+#define	MUTE_CH59	0x80000000lu
+
+#define	TX_CH56		0x0000007Flu
+#define	TX_CH57		0x00007F00lu
+#define	TX_CH58		0x007F0000lu
+#define	TX_CH59		0x7F000000lu
+
+
+/* Control Message Receive Buffer (CMRB) Address Offsets */
+
+#define	CMRB_STRIDE	  0x00000016lu
+
+#define	CMRB_DST_OFFSET	  0x00000000lu
+#define	CMRB_SRC_OFFSET	  0x00000002lu
+#define	CMRB_DATA_OFFSET  0x00000005lu
+
+
+/* Control Message Transmit Buffer (CMTB) Address Offsets */
+
+#define	CMTB_PRIO_OFFSET    0x00000000lu
+#define	CMTB_DST_OFFSET	    0x00000002lu
+#define	CMTB_SRC_OFFSET	    0x00000004lu
+#define	CMTB_TYPE_OFFSET    0x00000006lu
+#define	CMTB_DATA_OFFSET    0x00000007lu
+
+#define	CMTB_ANSWER_OFFSET  0x0000000Alu
+
+#define	CMTB_STAT_N_OFFSET  0x00000018lu
+#define	CMTB_STAT_A_OFFSET  0x00000016lu
+#define	CMTB_STAT_D_OFFSET  0x0000000Elu
+#define	CMTB_STAT_R_OFFSET  0x00000014lu
+#define	CMTB_STAT_W_OFFSET  0x00000014lu
+#define	CMTB_STAT_G_OFFSET  0x00000014lu
+
+
+/* Asynchronous	Packet Receive Buffer (APRB) Address Offsets */
+
+#define	APRB_STRIDE	  0x00000400lu
+
+#define	APRB_DST_OFFSET	  0x00000000lu
+#define	APRB_LEN_OFFSET	  0x00000002lu
+#define	APRB_SRC_OFFSET	  0x00000004lu
+#define	APRB_DATA_OFFSET  0x00000006lu
+
+
+/* Asynchronous	Packet Transmit	Buffer (APTB) Address Offsets */
+
+#define	APTB_PRIO_OFFSET  0x00000000lu
+#define	APTB_DST_OFFSET	  0x00000002lu
+#define	APTB_LEN_OFFSET	  0x00000004lu
+#define	APTB_SRC_OFFSET	  0x00000006lu
+#define	APTB_DATA_OFFSET  0x00000008lu
+
+
+/* Remote Read Buffer (RRDB) Address Offsets */
+
+#define	RRDB_WADDR_OFFSET 0x00000100lu
+#define	RRDB_WLEN_OFFSET  0x00000101lu
+
+
+
+/* ************	 CONTROLLER AREA NETWORK (CAN) MASKS  ***************/
+/* CAN_CONTROL Masks					 */
+#define	SRS			0x0001	/* Software Reset */
+#define	DNM			0x0002	/* Device Net Mode */
+#define	ABO			0x0004	/* Auto-Bus On Enable */
+#define	WBA			0x0010	/* Wake-Up On CAN Bus Activity Enable */
+#define	SMR			0x0020	/* Sleep Mode Request */
+#define	CSR			0x0040	/* CAN Suspend Mode Request */
+#define	CCR			0x0080	/* CAN Configuration Mode Request */
+
+/* CAN_STATUS Masks					 */
+#define	WT			0x0001	/* TX Warning Flag */
+#define	WR			0x0002	/* RX Warning Flag */
+#define	EP			0x0004	/* Error Passive Mode */
+#define	EBO			0x0008	/* Error Bus Off Mode */
+#define	CSA			0x0040	/* Suspend Mode	Acknowledge */
+#define	CCA			0x0080	/* Configuration Mode Acknowledge */
+#define	MBPTR		0x1F00	/* Mailbox Pointer */
+#define	TRM			0x4000	/* Transmit Mode */
+#define	REC			0x8000	/* Receive Mode */
+
+/* CAN_CLOCK Masks		 */
+#define	BRP			0x03FF	/* Bit-Rate Pre-Scaler */
+
+/* CAN_TIMING Masks				 */
+#define	TSEG1		0x000F	/* Time	Segment	1 */
+#define	TSEG2		0x0070	/* Time	Segment	2 */
+#define	SAM			0x0080	/* Sampling */
+#define	SJW			0x0300	/* Synchronization Jump	Width */
+
+/* CAN_DEBUG Masks				 */
+#define	DEC			0x0001	/* Disable CAN Error Counters */
+#define	DRI			0x0002	/* Disable CAN RX Input */
+#define	DTO			0x0004	/* Disable CAN TX Output */
+#define	DIL			0x0008	/* Disable CAN Internal	Loop */
+#define	MAA			0x0010	/* Mode	Auto-Acknowledge Enable */
+#define	MRB			0x0020	/* Mode	Read Back Enable */
+#define	CDE			0x8000	/* CAN Debug Enable */
+
+/* CAN_CEC Masks			 */
+#define	RXECNT		0x00FF	/* Receive Error Counter */
+#define	TXECNT		0xFF00	/* Transmit Error Counter */
+
+/* CAN_INTR Masks				 */
+#define	MBRIRQ	0x0001	/* Mailbox Receive Interrupt */
+#define	MBRIF		MBRIRQ	/* legacy */
+#define	MBTIRQ	0x0002	/* Mailbox Transmit Interrupt */
+#define	MBTIF		MBTIRQ	/* legacy */
+#define	GIRQ		0x0004	/* Global Interrupt */
+#define	SMACK		0x0008	/* Sleep Mode Acknowledge */
+#define	CANTX		0x0040	/* CAN TX Bus Value */
+#define	CANRX		0x0080	/* CAN RX Bus Value */
+
+/* CAN_MBxx_ID1	and CAN_MBxx_ID0 Masks			 */
+#define	DFC			0xFFFF	/* Data	Filtering Code (If Enabled) (ID0) */
+#define	EXTID_LO	0xFFFF	/* Lower 16 Bits of Extended Identifier	(ID0) */
+#define	EXTID_HI	0x0003	/* Upper 2 Bits	of Extended Identifier (ID1) */
+#define	BASEID		0x1FFC	/* Base	Identifier	 */
+#define	IDE			0x2000	/* Identifier Extension */
+#define	RTR			0x4000	/* Remote Frame	Transmission Request */
+#define	AME			0x8000	/* Acceptance Mask Enable */
+
+/* CAN_MBxx_TIMESTAMP Masks */
+#define	TSV			0xFFFF	/* Timestamp */
+
+/* CAN_MBxx_LENGTH Masks */
+#define	DLC			0x000F	/* Data	Length Code */
+
+/* CAN_AMxxH and CAN_AMxxL Masks					 */
+#define	DFM			0xFFFF	/* Data	Field Mask (If Enabled)	(CAN_AMxxL) */
+#define	EXTID_LO	0xFFFF	/* Lower 16 Bits of Extended Identifier	(CAN_AMxxL) */
+#define	EXTID_HI	0x0003	/* Upper 2 Bits	of Extended Identifier (CAN_AMxxH) */
+#define	BASEID		0x1FFC	/* Base	Identifier		 */
+#define	AMIDE		0x2000	/* Acceptance Mask ID Extension	Enable */
+#define	FMD			0x4000	/* Full	Mask Data Field	Enable */
+#define	FDF			0x8000	/* Filter On Data Field	Enable */
+
+/* CAN_MC1 Masks		 */
+#define	MC0			0x0001	/* Enable Mailbox 0 */
+#define	MC1			0x0002	/* Enable Mailbox 1 */
+#define	MC2			0x0004	/* Enable Mailbox 2 */
+#define	MC3			0x0008	/* Enable Mailbox 3 */
+#define	MC4			0x0010	/* Enable Mailbox 4 */
+#define	MC5			0x0020	/* Enable Mailbox 5 */
+#define	MC6			0x0040	/* Enable Mailbox 6 */
+#define	MC7			0x0080	/* Enable Mailbox 7 */
+#define	MC8			0x0100	/* Enable Mailbox 8 */
+#define	MC9			0x0200	/* Enable Mailbox 9 */
+#define	MC10		0x0400	/* Enable Mailbox 10 */
+#define	MC11		0x0800	/* Enable Mailbox 11 */
+#define	MC12		0x1000	/* Enable Mailbox 12 */
+#define	MC13		0x2000	/* Enable Mailbox 13 */
+#define	MC14		0x4000	/* Enable Mailbox 14 */
+#define	MC15		0x8000	/* Enable Mailbox 15 */
+
+/* CAN_MC2 Masks		 */
+#define	MC16		0x0001	/* Enable Mailbox 16 */
+#define	MC17		0x0002	/* Enable Mailbox 17 */
+#define	MC18		0x0004	/* Enable Mailbox 18 */
+#define	MC19		0x0008	/* Enable Mailbox 19 */
+#define	MC20		0x0010	/* Enable Mailbox 20 */
+#define	MC21		0x0020	/* Enable Mailbox 21 */
+#define	MC22		0x0040	/* Enable Mailbox 22 */
+#define	MC23		0x0080	/* Enable Mailbox 23 */
+#define	MC24		0x0100	/* Enable Mailbox 24 */
+#define	MC25		0x0200	/* Enable Mailbox 25 */
+#define	MC26		0x0400	/* Enable Mailbox 26 */
+#define	MC27		0x0800	/* Enable Mailbox 27 */
+#define	MC28		0x1000	/* Enable Mailbox 28 */
+#define	MC29		0x2000	/* Enable Mailbox 29 */
+#define	MC30		0x4000	/* Enable Mailbox 30 */
+#define	MC31		0x8000	/* Enable Mailbox 31 */
+
+/* CAN_MD1 Masks					 */
+#define	MD0			0x0001	/* Enable Mailbox 0 For	Receive */
+#define	MD1			0x0002	/* Enable Mailbox 1 For	Receive */
+#define	MD2			0x0004	/* Enable Mailbox 2 For	Receive */
+#define	MD3			0x0008	/* Enable Mailbox 3 For	Receive */
+#define	MD4			0x0010	/* Enable Mailbox 4 For	Receive */
+#define	MD5			0x0020	/* Enable Mailbox 5 For	Receive */
+#define	MD6			0x0040	/* Enable Mailbox 6 For	Receive */
+#define	MD7			0x0080	/* Enable Mailbox 7 For	Receive */
+#define	MD8			0x0100	/* Enable Mailbox 8 For	Receive */
+#define	MD9			0x0200	/* Enable Mailbox 9 For	Receive */
+#define	MD10		0x0400	/* Enable Mailbox 10 For Receive */
+#define	MD11		0x0800	/* Enable Mailbox 11 For Receive */
+#define	MD12		0x1000	/* Enable Mailbox 12 For Receive */
+#define	MD13		0x2000	/* Enable Mailbox 13 For Receive */
+#define	MD14		0x4000	/* Enable Mailbox 14 For Receive */
+#define	MD15		0x8000	/* Enable Mailbox 15 For Receive */
+
+/* CAN_MD2 Masks					 */
+#define	MD16		0x0001	/* Enable Mailbox 16 For Receive */
+#define	MD17		0x0002	/* Enable Mailbox 17 For Receive */
+#define	MD18		0x0004	/* Enable Mailbox 18 For Receive */
+#define	MD19		0x0008	/* Enable Mailbox 19 For Receive */
+#define	MD20		0x0010	/* Enable Mailbox 20 For Receive */
+#define	MD21		0x0020	/* Enable Mailbox 21 For Receive */
+#define	MD22		0x0040	/* Enable Mailbox 22 For Receive */
+#define	MD23		0x0080	/* Enable Mailbox 23 For Receive */
+#define	MD24		0x0100	/* Enable Mailbox 24 For Receive */
+#define	MD25		0x0200	/* Enable Mailbox 25 For Receive */
+#define	MD26		0x0400	/* Enable Mailbox 26 For Receive */
+#define	MD27		0x0800	/* Enable Mailbox 27 For Receive */
+#define	MD28		0x1000	/* Enable Mailbox 28 For Receive */
+#define	MD29		0x2000	/* Enable Mailbox 29 For Receive */
+#define	MD30		0x4000	/* Enable Mailbox 30 For Receive */
+#define	MD31		0x8000	/* Enable Mailbox 31 For Receive */
+
+/* CAN_RMP1 Masks					 */
+#define	RMP0		0x0001	/* RX Message Pending In Mailbox 0 */
+#define	RMP1		0x0002	/* RX Message Pending In Mailbox 1 */
+#define	RMP2		0x0004	/* RX Message Pending In Mailbox 2 */
+#define	RMP3		0x0008	/* RX Message Pending In Mailbox 3 */
+#define	RMP4		0x0010	/* RX Message Pending In Mailbox 4 */
+#define	RMP5		0x0020	/* RX Message Pending In Mailbox 5 */
+#define	RMP6		0x0040	/* RX Message Pending In Mailbox 6 */
+#define	RMP7		0x0080	/* RX Message Pending In Mailbox 7 */
+#define	RMP8		0x0100	/* RX Message Pending In Mailbox 8 */
+#define	RMP9		0x0200	/* RX Message Pending In Mailbox 9 */
+#define	RMP10		0x0400	/* RX Message Pending In Mailbox 10 */
+#define	RMP11		0x0800	/* RX Message Pending In Mailbox 11 */
+#define	RMP12		0x1000	/* RX Message Pending In Mailbox 12 */
+#define	RMP13		0x2000	/* RX Message Pending In Mailbox 13 */
+#define	RMP14		0x4000	/* RX Message Pending In Mailbox 14 */
+#define	RMP15		0x8000	/* RX Message Pending In Mailbox 15 */
+
+/* CAN_RMP2 Masks					 */
+#define	RMP16		0x0001	/* RX Message Pending In Mailbox 16 */
+#define	RMP17		0x0002	/* RX Message Pending In Mailbox 17 */
+#define	RMP18		0x0004	/* RX Message Pending In Mailbox 18 */
+#define	RMP19		0x0008	/* RX Message Pending In Mailbox 19 */
+#define	RMP20		0x0010	/* RX Message Pending In Mailbox 20 */
+#define	RMP21		0x0020	/* RX Message Pending In Mailbox 21 */
+#define	RMP22		0x0040	/* RX Message Pending In Mailbox 22 */
+#define	RMP23		0x0080	/* RX Message Pending In Mailbox 23 */
+#define	RMP24		0x0100	/* RX Message Pending In Mailbox 24 */
+#define	RMP25		0x0200	/* RX Message Pending In Mailbox 25 */
+#define	RMP26		0x0400	/* RX Message Pending In Mailbox 26 */
+#define	RMP27		0x0800	/* RX Message Pending In Mailbox 27 */
+#define	RMP28		0x1000	/* RX Message Pending In Mailbox 28 */
+#define	RMP29		0x2000	/* RX Message Pending In Mailbox 29 */
+#define	RMP30		0x4000	/* RX Message Pending In Mailbox 30 */
+#define	RMP31		0x8000	/* RX Message Pending In Mailbox 31 */
+
+/* CAN_RML1 Masks					 */
+#define	RML0		0x0001	/* RX Message Lost In Mailbox 0 */
+#define	RML1		0x0002	/* RX Message Lost In Mailbox 1 */
+#define	RML2		0x0004	/* RX Message Lost In Mailbox 2 */
+#define	RML3		0x0008	/* RX Message Lost In Mailbox 3 */
+#define	RML4		0x0010	/* RX Message Lost In Mailbox 4 */
+#define	RML5		0x0020	/* RX Message Lost In Mailbox 5 */
+#define	RML6		0x0040	/* RX Message Lost In Mailbox 6 */
+#define	RML7		0x0080	/* RX Message Lost In Mailbox 7 */
+#define	RML8		0x0100	/* RX Message Lost In Mailbox 8 */
+#define	RML9		0x0200	/* RX Message Lost In Mailbox 9 */
+#define	RML10		0x0400	/* RX Message Lost In Mailbox 10 */
+#define	RML11		0x0800	/* RX Message Lost In Mailbox 11 */
+#define	RML12		0x1000	/* RX Message Lost In Mailbox 12 */
+#define	RML13		0x2000	/* RX Message Lost In Mailbox 13 */
+#define	RML14		0x4000	/* RX Message Lost In Mailbox 14 */
+#define	RML15		0x8000	/* RX Message Lost In Mailbox 15 */
+
+/* CAN_RML2 Masks					 */
+#define	RML16		0x0001	/* RX Message Lost In Mailbox 16 */
+#define	RML17		0x0002	/* RX Message Lost In Mailbox 17 */
+#define	RML18		0x0004	/* RX Message Lost In Mailbox 18 */
+#define	RML19		0x0008	/* RX Message Lost In Mailbox 19 */
+#define	RML20		0x0010	/* RX Message Lost In Mailbox 20 */
+#define	RML21		0x0020	/* RX Message Lost In Mailbox 21 */
+#define	RML22		0x0040	/* RX Message Lost In Mailbox 22 */
+#define	RML23		0x0080	/* RX Message Lost In Mailbox 23 */
+#define	RML24		0x0100	/* RX Message Lost In Mailbox 24 */
+#define	RML25		0x0200	/* RX Message Lost In Mailbox 25 */
+#define	RML26		0x0400	/* RX Message Lost In Mailbox 26 */
+#define	RML27		0x0800	/* RX Message Lost In Mailbox 27 */
+#define	RML28		0x1000	/* RX Message Lost In Mailbox 28 */
+#define	RML29		0x2000	/* RX Message Lost In Mailbox 29 */
+#define	RML30		0x4000	/* RX Message Lost In Mailbox 30 */
+#define	RML31		0x8000	/* RX Message Lost In Mailbox 31 */
+
+/* CAN_OPSS1 Masks													 */
+#define	OPSS0		0x0001	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	0 */
+#define	OPSS1		0x0002	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	1 */
+#define	OPSS2		0x0004	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	2 */
+#define	OPSS3		0x0008	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	3 */
+#define	OPSS4		0x0010	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	4 */
+#define	OPSS5		0x0020	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	5 */
+#define	OPSS6		0x0040	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	6 */
+#define	OPSS7		0x0080	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	7 */
+#define	OPSS8		0x0100	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	8 */
+#define	OPSS9		0x0200	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	9 */
+#define	OPSS10		0x0400	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	10 */
+#define	OPSS11		0x0800	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	11 */
+#define	OPSS12		0x1000	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	12 */
+#define	OPSS13		0x2000	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	13 */
+#define	OPSS14		0x4000	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	14 */
+#define	OPSS15		0x8000	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	15 */
+
+/* CAN_OPSS2 Masks													 */
+#define	OPSS16		0x0001	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	16 */
+#define	OPSS17		0x0002	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	17 */
+#define	OPSS18		0x0004	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	18 */
+#define	OPSS19		0x0008	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	19 */
+#define	OPSS20		0x0010	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	20 */
+#define	OPSS21		0x0020	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	21 */
+#define	OPSS22		0x0040	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	22 */
+#define	OPSS23		0x0080	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	23 */
+#define	OPSS24		0x0100	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	24 */
+#define	OPSS25		0x0200	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	25 */
+#define	OPSS26		0x0400	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	26 */
+#define	OPSS27		0x0800	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	27 */
+#define	OPSS28		0x1000	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	28 */
+#define	OPSS29		0x2000	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	29 */
+#define	OPSS30		0x4000	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	30 */
+#define	OPSS31		0x8000	/* Enable RX Overwrite Protection or TX	Single-Shot For	Mailbox	31 */
+
+/* CAN_TRR1 Masks							 */
+#define	TRR0		0x0001	/* Deny	But Don't Lock Access To Mailbox 0 */
+#define	TRR1		0x0002	/* Deny	But Don't Lock Access To Mailbox 1 */
+#define	TRR2		0x0004	/* Deny	But Don't Lock Access To Mailbox 2 */
+#define	TRR3		0x0008	/* Deny	But Don't Lock Access To Mailbox 3 */
+#define	TRR4		0x0010	/* Deny	But Don't Lock Access To Mailbox 4 */
+#define	TRR5		0x0020	/* Deny	But Don't Lock Access To Mailbox 5 */
+#define	TRR6		0x0040	/* Deny	But Don't Lock Access To Mailbox 6 */
+#define	TRR7		0x0080	/* Deny	But Don't Lock Access To Mailbox 7 */
+#define	TRR8		0x0100	/* Deny	But Don't Lock Access To Mailbox 8 */
+#define	TRR9		0x0200	/* Deny	But Don't Lock Access To Mailbox 9 */
+#define	TRR10		0x0400	/* Deny	But Don't Lock Access To Mailbox 10 */
+#define	TRR11		0x0800	/* Deny	But Don't Lock Access To Mailbox 11 */
+#define	TRR12		0x1000	/* Deny	But Don't Lock Access To Mailbox 12 */
+#define	TRR13		0x2000	/* Deny	But Don't Lock Access To Mailbox 13 */
+#define	TRR14		0x4000	/* Deny	But Don't Lock Access To Mailbox 14 */
+#define	TRR15		0x8000	/* Deny	But Don't Lock Access To Mailbox 15 */
+
+/* CAN_TRR2 Masks							 */
+#define	TRR16		0x0001	/* Deny	But Don't Lock Access To Mailbox 16 */
+#define	TRR17		0x0002	/* Deny	But Don't Lock Access To Mailbox 17 */
+#define	TRR18		0x0004	/* Deny	But Don't Lock Access To Mailbox 18 */
+#define	TRR19		0x0008	/* Deny	But Don't Lock Access To Mailbox 19 */
+#define	TRR20		0x0010	/* Deny	But Don't Lock Access To Mailbox 20 */
+#define	TRR21		0x0020	/* Deny	But Don't Lock Access To Mailbox 21 */
+#define	TRR22		0x0040	/* Deny	But Don't Lock Access To Mailbox 22 */
+#define	TRR23		0x0080	/* Deny	But Don't Lock Access To Mailbox 23 */
+#define	TRR24		0x0100	/* Deny	But Don't Lock Access To Mailbox 24 */
+#define	TRR25		0x0200	/* Deny	But Don't Lock Access To Mailbox 25 */
+#define	TRR26		0x0400	/* Deny	But Don't Lock Access To Mailbox 26 */
+#define	TRR27		0x0800	/* Deny	But Don't Lock Access To Mailbox 27 */
+#define	TRR28		0x1000	/* Deny	But Don't Lock Access To Mailbox 28 */
+#define	TRR29		0x2000	/* Deny	But Don't Lock Access To Mailbox 29 */
+#define	TRR30		0x4000	/* Deny	But Don't Lock Access To Mailbox 30 */
+#define	TRR31		0x8000	/* Deny	But Don't Lock Access To Mailbox 31 */
+
+/* CAN_TRS1 Masks						 */
+#define	TRS0		0x0001	/* Remote Frame	Request	For Mailbox 0 */
+#define	TRS1		0x0002	/* Remote Frame	Request	For Mailbox 1 */
+#define	TRS2		0x0004	/* Remote Frame	Request	For Mailbox 2 */
+#define	TRS3		0x0008	/* Remote Frame	Request	For Mailbox 3 */
+#define	TRS4		0x0010	/* Remote Frame	Request	For Mailbox 4 */
+#define	TRS5		0x0020	/* Remote Frame	Request	For Mailbox 5 */
+#define	TRS6		0x0040	/* Remote Frame	Request	For Mailbox 6 */
+#define	TRS7		0x0080	/* Remote Frame	Request	For Mailbox 7 */
+#define	TRS8		0x0100	/* Remote Frame	Request	For Mailbox 8 */
+#define	TRS9		0x0200	/* Remote Frame	Request	For Mailbox 9 */
+#define	TRS10		0x0400	/* Remote Frame	Request	For Mailbox 10 */
+#define	TRS11		0x0800	/* Remote Frame	Request	For Mailbox 11 */
+#define	TRS12		0x1000	/* Remote Frame	Request	For Mailbox 12 */
+#define	TRS13		0x2000	/* Remote Frame	Request	For Mailbox 13 */
+#define	TRS14		0x4000	/* Remote Frame	Request	For Mailbox 14 */
+#define	TRS15		0x8000	/* Remote Frame	Request	For Mailbox 15 */
+
+/* CAN_TRS2 Masks						 */
+#define	TRS16		0x0001	/* Remote Frame	Request	For Mailbox 16 */
+#define	TRS17		0x0002	/* Remote Frame	Request	For Mailbox 17 */
+#define	TRS18		0x0004	/* Remote Frame	Request	For Mailbox 18 */
+#define	TRS19		0x0008	/* Remote Frame	Request	For Mailbox 19 */
+#define	TRS20		0x0010	/* Remote Frame	Request	For Mailbox 20 */
+#define	TRS21		0x0020	/* Remote Frame	Request	For Mailbox 21 */
+#define	TRS22		0x0040	/* Remote Frame	Request	For Mailbox 22 */
+#define	TRS23		0x0080	/* Remote Frame	Request	For Mailbox 23 */
+#define	TRS24		0x0100	/* Remote Frame	Request	For Mailbox 24 */
+#define	TRS25		0x0200	/* Remote Frame	Request	For Mailbox 25 */
+#define	TRS26		0x0400	/* Remote Frame	Request	For Mailbox 26 */
+#define	TRS27		0x0800	/* Remote Frame	Request	For Mailbox 27 */
+#define	TRS28		0x1000	/* Remote Frame	Request	For Mailbox 28 */
+#define	TRS29		0x2000	/* Remote Frame	Request	For Mailbox 29 */
+#define	TRS30		0x4000	/* Remote Frame	Request	For Mailbox 30 */
+#define	TRS31		0x8000	/* Remote Frame	Request	For Mailbox 31 */
+
+/* CAN_AA1 Masks					 */
+#define	AA0			0x0001	/* Aborted Message In Mailbox 0 */
+#define	AA1			0x0002	/* Aborted Message In Mailbox 1 */
+#define	AA2			0x0004	/* Aborted Message In Mailbox 2 */
+#define	AA3			0x0008	/* Aborted Message In Mailbox 3 */
+#define	AA4			0x0010	/* Aborted Message In Mailbox 4 */
+#define	AA5			0x0020	/* Aborted Message In Mailbox 5 */
+#define	AA6			0x0040	/* Aborted Message In Mailbox 6 */
+#define	AA7			0x0080	/* Aborted Message In Mailbox 7 */
+#define	AA8			0x0100	/* Aborted Message In Mailbox 8 */
+#define	AA9			0x0200	/* Aborted Message In Mailbox 9 */
+#define	AA10		0x0400	/* Aborted Message In Mailbox 10 */
+#define	AA11		0x0800	/* Aborted Message In Mailbox 11 */
+#define	AA12		0x1000	/* Aborted Message In Mailbox 12 */
+#define	AA13		0x2000	/* Aborted Message In Mailbox 13 */
+#define	AA14		0x4000	/* Aborted Message In Mailbox 14 */
+#define	AA15		0x8000	/* Aborted Message In Mailbox 15 */
+
+/* CAN_AA2 Masks					 */
+#define	AA16		0x0001	/* Aborted Message In Mailbox 16 */
+#define	AA17		0x0002	/* Aborted Message In Mailbox 17 */
+#define	AA18		0x0004	/* Aborted Message In Mailbox 18 */
+#define	AA19		0x0008	/* Aborted Message In Mailbox 19 */
+#define	AA20		0x0010	/* Aborted Message In Mailbox 20 */
+#define	AA21		0x0020	/* Aborted Message In Mailbox 21 */
+#define	AA22		0x0040	/* Aborted Message In Mailbox 22 */
+#define	AA23		0x0080	/* Aborted Message In Mailbox 23 */
+#define	AA24		0x0100	/* Aborted Message In Mailbox 24 */
+#define	AA25		0x0200	/* Aborted Message In Mailbox 25 */
+#define	AA26		0x0400	/* Aborted Message In Mailbox 26 */
+#define	AA27		0x0800	/* Aborted Message In Mailbox 27 */
+#define	AA28		0x1000	/* Aborted Message In Mailbox 28 */
+#define	AA29		0x2000	/* Aborted Message In Mailbox 29 */
+#define	AA30		0x4000	/* Aborted Message In Mailbox 30 */
+#define	AA31		0x8000	/* Aborted Message In Mailbox 31 */
+
+/* CAN_TA1 Masks						 */
+#define	TA0			0x0001	/* Transmit Successful From Mailbox 0 */
+#define	TA1			0x0002	/* Transmit Successful From Mailbox 1 */
+#define	TA2			0x0004	/* Transmit Successful From Mailbox 2 */
+#define	TA3			0x0008	/* Transmit Successful From Mailbox 3 */
+#define	TA4			0x0010	/* Transmit Successful From Mailbox 4 */
+#define	TA5			0x0020	/* Transmit Successful From Mailbox 5 */
+#define	TA6			0x0040	/* Transmit Successful From Mailbox 6 */
+#define	TA7			0x0080	/* Transmit Successful From Mailbox 7 */
+#define	TA8			0x0100	/* Transmit Successful From Mailbox 8 */
+#define	TA9			0x0200	/* Transmit Successful From Mailbox 9 */
+#define	TA10		0x0400	/* Transmit Successful From Mailbox 10 */
+#define	TA11		0x0800	/* Transmit Successful From Mailbox 11 */
+#define	TA12		0x1000	/* Transmit Successful From Mailbox 12 */
+#define	TA13		0x2000	/* Transmit Successful From Mailbox 13 */
+#define	TA14		0x4000	/* Transmit Successful From Mailbox 14 */
+#define	TA15		0x8000	/* Transmit Successful From Mailbox 15 */
+
+/* CAN_TA2 Masks						 */
+#define	TA16		0x0001	/* Transmit Successful From Mailbox 16 */
+#define	TA17		0x0002	/* Transmit Successful From Mailbox 17 */
+#define	TA18		0x0004	/* Transmit Successful From Mailbox 18 */
+#define	TA19		0x0008	/* Transmit Successful From Mailbox 19 */
+#define	TA20		0x0010	/* Transmit Successful From Mailbox 20 */
+#define	TA21		0x0020	/* Transmit Successful From Mailbox 21 */
+#define	TA22		0x0040	/* Transmit Successful From Mailbox 22 */
+#define	TA23		0x0080	/* Transmit Successful From Mailbox 23 */
+#define	TA24		0x0100	/* Transmit Successful From Mailbox 24 */
+#define	TA25		0x0200	/* Transmit Successful From Mailbox 25 */
+#define	TA26		0x0400	/* Transmit Successful From Mailbox 26 */
+#define	TA27		0x0800	/* Transmit Successful From Mailbox 27 */
+#define	TA28		0x1000	/* Transmit Successful From Mailbox 28 */
+#define	TA29		0x2000	/* Transmit Successful From Mailbox 29 */
+#define	TA30		0x4000	/* Transmit Successful From Mailbox 30 */
+#define	TA31		0x8000	/* Transmit Successful From Mailbox 31 */
+
+/* CAN_MBTD Masks					 */
+#define	TDPTR		0x001F	/* Mailbox To Temporarily Disable */
+#define	TDA			0x0040	/* Temporary Disable Acknowledge */
+#define	TDR			0x0080	/* Temporary Disable Request */
+
+/* CAN_RFH1 Masks											 */
+#define	RFH0		0x0001	/* Enable Automatic Remote Frame Handling For Mailbox 0 */
+#define	RFH1		0x0002	/* Enable Automatic Remote Frame Handling For Mailbox 1 */
+#define	RFH2		0x0004	/* Enable Automatic Remote Frame Handling For Mailbox 2 */
+#define	RFH3		0x0008	/* Enable Automatic Remote Frame Handling For Mailbox 3 */
+#define	RFH4		0x0010	/* Enable Automatic Remote Frame Handling For Mailbox 4 */
+#define	RFH5		0x0020	/* Enable Automatic Remote Frame Handling For Mailbox 5 */
+#define	RFH6		0x0040	/* Enable Automatic Remote Frame Handling For Mailbox 6 */
+#define	RFH7		0x0080	/* Enable Automatic Remote Frame Handling For Mailbox 7 */
+#define	RFH8		0x0100	/* Enable Automatic Remote Frame Handling For Mailbox 8 */
+#define	RFH9		0x0200	/* Enable Automatic Remote Frame Handling For Mailbox 9 */
+#define	RFH10		0x0400	/* Enable Automatic Remote Frame Handling For Mailbox 10 */
+#define	RFH11		0x0800	/* Enable Automatic Remote Frame Handling For Mailbox 11 */
+#define	RFH12		0x1000	/* Enable Automatic Remote Frame Handling For Mailbox 12 */
+#define	RFH13		0x2000	/* Enable Automatic Remote Frame Handling For Mailbox 13 */
+#define	RFH14		0x4000	/* Enable Automatic Remote Frame Handling For Mailbox 14 */
+#define	RFH15		0x8000	/* Enable Automatic Remote Frame Handling For Mailbox 15 */
+
+/* CAN_RFH2 Masks											 */
+#define	RFH16		0x0001	/* Enable Automatic Remote Frame Handling For Mailbox 16 */
+#define	RFH17		0x0002	/* Enable Automatic Remote Frame Handling For Mailbox 17 */
+#define	RFH18		0x0004	/* Enable Automatic Remote Frame Handling For Mailbox 18 */
+#define	RFH19		0x0008	/* Enable Automatic Remote Frame Handling For Mailbox 19 */
+#define	RFH20		0x0010	/* Enable Automatic Remote Frame Handling For Mailbox 20 */
+#define	RFH21		0x0020	/* Enable Automatic Remote Frame Handling For Mailbox 21 */
+#define	RFH22		0x0040	/* Enable Automatic Remote Frame Handling For Mailbox 22 */
+#define	RFH23		0x0080	/* Enable Automatic Remote Frame Handling For Mailbox 23 */
+#define	RFH24		0x0100	/* Enable Automatic Remote Frame Handling For Mailbox 24 */
+#define	RFH25		0x0200	/* Enable Automatic Remote Frame Handling For Mailbox 25 */
+#define	RFH26		0x0400	/* Enable Automatic Remote Frame Handling For Mailbox 26 */
+#define	RFH27		0x0800	/* Enable Automatic Remote Frame Handling For Mailbox 27 */
+#define	RFH28		0x1000	/* Enable Automatic Remote Frame Handling For Mailbox 28 */
+#define	RFH29		0x2000	/* Enable Automatic Remote Frame Handling For Mailbox 29 */
+#define	RFH30		0x4000	/* Enable Automatic Remote Frame Handling For Mailbox 30 */
+#define	RFH31		0x8000	/* Enable Automatic Remote Frame Handling For Mailbox 31 */
+
+/* CAN_MBTIF1 Masks						 */
+#define	MBTIF0		0x0001	/* TX Interrupt	Active In Mailbox 0 */
+#define	MBTIF1		0x0002	/* TX Interrupt	Active In Mailbox 1 */
+#define	MBTIF2		0x0004	/* TX Interrupt	Active In Mailbox 2 */
+#define	MBTIF3		0x0008	/* TX Interrupt	Active In Mailbox 3 */
+#define	MBTIF4		0x0010	/* TX Interrupt	Active In Mailbox 4 */
+#define	MBTIF5		0x0020	/* TX Interrupt	Active In Mailbox 5 */
+#define	MBTIF6		0x0040	/* TX Interrupt	Active In Mailbox 6 */
+#define	MBTIF7		0x0080	/* TX Interrupt	Active In Mailbox 7 */
+#define	MBTIF8		0x0100	/* TX Interrupt	Active In Mailbox 8 */
+#define	MBTIF9		0x0200	/* TX Interrupt	Active In Mailbox 9 */
+#define	MBTIF10		0x0400	/* TX Interrupt	Active In Mailbox 10 */
+#define	MBTIF11		0x0800	/* TX Interrupt	Active In Mailbox 11 */
+#define	MBTIF12		0x1000	/* TX Interrupt	Active In Mailbox 12 */
+#define	MBTIF13		0x2000	/* TX Interrupt	Active In Mailbox 13 */
+#define	MBTIF14		0x4000	/* TX Interrupt	Active In Mailbox 14 */
+#define	MBTIF15		0x8000	/* TX Interrupt	Active In Mailbox 15 */
+
+/* CAN_MBTIF2 Masks						 */
+#define	MBTIF16		0x0001	/* TX Interrupt	Active In Mailbox 16 */
+#define	MBTIF17		0x0002	/* TX Interrupt	Active In Mailbox 17 */
+#define	MBTIF18		0x0004	/* TX Interrupt	Active In Mailbox 18 */
+#define	MBTIF19		0x0008	/* TX Interrupt	Active In Mailbox 19 */
+#define	MBTIF20		0x0010	/* TX Interrupt	Active In Mailbox 20 */
+#define	MBTIF21		0x0020	/* TX Interrupt	Active In Mailbox 21 */
+#define	MBTIF22		0x0040	/* TX Interrupt	Active In Mailbox 22 */
+#define	MBTIF23		0x0080	/* TX Interrupt	Active In Mailbox 23 */
+#define	MBTIF24		0x0100	/* TX Interrupt	Active In Mailbox 24 */
+#define	MBTIF25		0x0200	/* TX Interrupt	Active In Mailbox 25 */
+#define	MBTIF26		0x0400	/* TX Interrupt	Active In Mailbox 26 */
+#define	MBTIF27		0x0800	/* TX Interrupt	Active In Mailbox 27 */
+#define	MBTIF28		0x1000	/* TX Interrupt	Active In Mailbox 28 */
+#define	MBTIF29		0x2000	/* TX Interrupt	Active In Mailbox 29 */
+#define	MBTIF30		0x4000	/* TX Interrupt	Active In Mailbox 30 */
+#define	MBTIF31		0x8000	/* TX Interrupt	Active In Mailbox 31 */
+
+/* CAN_MBRIF1 Masks						 */
+#define	MBRIF0		0x0001	/* RX Interrupt	Active In Mailbox 0 */
+#define	MBRIF1		0x0002	/* RX Interrupt	Active In Mailbox 1 */
+#define	MBRIF2		0x0004	/* RX Interrupt	Active In Mailbox 2 */
+#define	MBRIF3		0x0008	/* RX Interrupt	Active In Mailbox 3 */
+#define	MBRIF4		0x0010	/* RX Interrupt	Active In Mailbox 4 */
+#define	MBRIF5		0x0020	/* RX Interrupt	Active In Mailbox 5 */
+#define	MBRIF6		0x0040	/* RX Interrupt	Active In Mailbox 6 */
+#define	MBRIF7		0x0080	/* RX Interrupt	Active In Mailbox 7 */
+#define	MBRIF8		0x0100	/* RX Interrupt	Active In Mailbox 8 */
+#define	MBRIF9		0x0200	/* RX Interrupt	Active In Mailbox 9 */
+#define	MBRIF10		0x0400	/* RX Interrupt	Active In Mailbox 10 */
+#define	MBRIF11		0x0800	/* RX Interrupt	Active In Mailbox 11 */
+#define	MBRIF12		0x1000	/* RX Interrupt	Active In Mailbox 12 */
+#define	MBRIF13		0x2000	/* RX Interrupt	Active In Mailbox 13 */
+#define	MBRIF14		0x4000	/* RX Interrupt	Active In Mailbox 14 */
+#define	MBRIF15		0x8000	/* RX Interrupt	Active In Mailbox 15 */
+
+/* CAN_MBRIF2 Masks						 */
+#define	MBRIF16		0x0001	/* RX Interrupt	Active In Mailbox 16 */
+#define	MBRIF17		0x0002	/* RX Interrupt	Active In Mailbox 17 */
+#define	MBRIF18		0x0004	/* RX Interrupt	Active In Mailbox 18 */
+#define	MBRIF19		0x0008	/* RX Interrupt	Active In Mailbox 19 */
+#define	MBRIF20		0x0010	/* RX Interrupt	Active In Mailbox 20 */
+#define	MBRIF21		0x0020	/* RX Interrupt	Active In Mailbox 21 */
+#define	MBRIF22		0x0040	/* RX Interrupt	Active In Mailbox 22 */
+#define	MBRIF23		0x0080	/* RX Interrupt	Active In Mailbox 23 */
+#define	MBRIF24		0x0100	/* RX Interrupt	Active In Mailbox 24 */
+#define	MBRIF25		0x0200	/* RX Interrupt	Active In Mailbox 25 */
+#define	MBRIF26		0x0400	/* RX Interrupt	Active In Mailbox 26 */
+#define	MBRIF27		0x0800	/* RX Interrupt	Active In Mailbox 27 */
+#define	MBRIF28		0x1000	/* RX Interrupt	Active In Mailbox 28 */
+#define	MBRIF29		0x2000	/* RX Interrupt	Active In Mailbox 29 */
+#define	MBRIF30		0x4000	/* RX Interrupt	Active In Mailbox 30 */
+#define	MBRIF31		0x8000	/* RX Interrupt	Active In Mailbox 31 */
+
+/* CAN_MBIM1 Masks					 */
+#define	MBIM0		0x0001	/* Enable Interrupt For	Mailbox	0 */
+#define	MBIM1		0x0002	/* Enable Interrupt For	Mailbox	1 */
+#define	MBIM2		0x0004	/* Enable Interrupt For	Mailbox	2 */
+#define	MBIM3		0x0008	/* Enable Interrupt For	Mailbox	3 */
+#define	MBIM4		0x0010	/* Enable Interrupt For	Mailbox	4 */
+#define	MBIM5		0x0020	/* Enable Interrupt For	Mailbox	5 */
+#define	MBIM6		0x0040	/* Enable Interrupt For	Mailbox	6 */
+#define	MBIM7		0x0080	/* Enable Interrupt For	Mailbox	7 */
+#define	MBIM8		0x0100	/* Enable Interrupt For	Mailbox	8 */
+#define	MBIM9		0x0200	/* Enable Interrupt For	Mailbox	9 */
+#define	MBIM10		0x0400	/* Enable Interrupt For	Mailbox	10 */
+#define	MBIM11		0x0800	/* Enable Interrupt For	Mailbox	11 */
+#define	MBIM12		0x1000	/* Enable Interrupt For	Mailbox	12 */
+#define	MBIM13		0x2000	/* Enable Interrupt For	Mailbox	13 */
+#define	MBIM14		0x4000	/* Enable Interrupt For	Mailbox	14 */
+#define	MBIM15		0x8000	/* Enable Interrupt For	Mailbox	15 */
+
+/* CAN_MBIM2 Masks					 */
+#define	MBIM16		0x0001	/* Enable Interrupt For	Mailbox	16 */
+#define	MBIM17		0x0002	/* Enable Interrupt For	Mailbox	17 */
+#define	MBIM18		0x0004	/* Enable Interrupt For	Mailbox	18 */
+#define	MBIM19		0x0008	/* Enable Interrupt For	Mailbox	19 */
+#define	MBIM20		0x0010	/* Enable Interrupt For	Mailbox	20 */
+#define	MBIM21		0x0020	/* Enable Interrupt For	Mailbox	21 */
+#define	MBIM22		0x0040	/* Enable Interrupt For	Mailbox	22 */
+#define	MBIM23		0x0080	/* Enable Interrupt For	Mailbox	23 */
+#define	MBIM24		0x0100	/* Enable Interrupt For	Mailbox	24 */
+#define	MBIM25		0x0200	/* Enable Interrupt For	Mailbox	25 */
+#define	MBIM26		0x0400	/* Enable Interrupt For	Mailbox	26 */
+#define	MBIM27		0x0800	/* Enable Interrupt For	Mailbox	27 */
+#define	MBIM28		0x1000	/* Enable Interrupt For	Mailbox	28 */
+#define	MBIM29		0x2000	/* Enable Interrupt For	Mailbox	29 */
+#define	MBIM30		0x4000	/* Enable Interrupt For	Mailbox	30 */
+#define	MBIM31		0x8000	/* Enable Interrupt For	Mailbox	31 */
+
+/* CAN_GIM Masks									 */
+#define	EWTIM		0x0001	/* Enable TX Error Count Interrupt */
+#define	EWRIM		0x0002	/* Enable RX Error Count Interrupt */
+#define	EPIM		0x0004	/* Enable Error-Passive	Mode Interrupt */
+#define	BOIM		0x0008	/* Enable Bus Off Interrupt */
+#define	WUIM		0x0010	/* Enable Wake-Up Interrupt */
+#define	UIAIM		0x0020	/* Enable Access To Unimplemented Address Interrupt */
+#define	AAIM		0x0040	/* Enable Abort	Acknowledge Interrupt */
+#define	RMLIM		0x0080	/* Enable RX Message Lost Interrupt */
+#define	UCEIM		0x0100	/* Enable Universal Counter Overflow Interrupt */
+#define	EXTIM		0x0200	/* Enable External Trigger Output Interrupt */
+#define	ADIM		0x0400	/* Enable Access Denied	Interrupt */
+
+/* CAN_GIS Masks								 */
+#define	EWTIS		0x0001	/* TX Error Count IRQ Status */
+#define	EWRIS		0x0002	/* RX Error Count IRQ Status */
+#define	EPIS		0x0004	/* Error-Passive Mode IRQ Status */
+#define	BOIS		0x0008	/* Bus Off IRQ Status */
+#define	WUIS		0x0010	/* Wake-Up IRQ Status */
+#define	UIAIS		0x0020	/* Access To Unimplemented Address IRQ Status */
+#define	AAIS		0x0040	/* Abort Acknowledge IRQ Status */
+#define	RMLIS		0x0080	/* RX Message Lost IRQ Status */
+#define	UCEIS		0x0100	/* Universal Counter Overflow IRQ Status */
+#define	EXTIS		0x0200	/* External Trigger Output IRQ Status */
+#define	ADIS		0x0400	/* Access Denied IRQ Status */
+
+/* CAN_GIF Masks								 */
+#define	EWTIF		0x0001	/* TX Error Count IRQ Flag */
+#define	EWRIF		0x0002	/* RX Error Count IRQ Flag */
+#define	EPIF		0x0004	/* Error-Passive Mode IRQ Flag */
+#define	BOIF		0x0008	/* Bus Off IRQ Flag	 */
+#define	WUIF		0x0010	/* Wake-Up IRQ Flag	 */
+#define	UIAIF		0x0020	/* Access To Unimplemented Address IRQ Flag */
+#define	AAIF		0x0040	/* Abort Acknowledge IRQ Flag */
+#define	RMLIF		0x0080	/* RX Message Lost IRQ Flag */
+#define	UCEIF		0x0100	/* Universal Counter Overflow IRQ Flag */
+#define	EXTIF		0x0200	/* External Trigger Output IRQ Flag */
+#define	ADIF		0x0400	/* Access Denied IRQ Flag */
+
+/* CAN_UCCNF Masks								 */
+#define	UCCNF		0x000F	/* Universal Counter Mode */
+#define	UC_STAMP	0x0001	/*		Timestamp Mode */
+#define	UC_WDOG		0x0002	/*		Watchdog Mode */
+#define	UC_AUTOTX	0x0003	/*		Auto-Transmit Mode */
+#define	UC_ERROR	0x0006	/*		CAN Error Frame	Count */
+#define	UC_OVER		0x0007	/*		CAN Overload Frame Count */
+#define	UC_LOST		0x0008	/*		Arbitration Lost During	TX Count */
+#define	UC_AA		0x0009	/*		TX Abort Count */
+#define	UC_TA		0x000A	/*		TX Successful Count */
+#define	UC_REJECT	0x000B	/*		RX Message Rejected Count */
+#define	UC_RML		0x000C	/*		RX Message Lost	Count */
+#define	UC_RX		0x000D	/*		Total Successful RX Messages Count */
+#define	UC_RMP		0x000E	/*		Successful RX W/Matching ID Count */
+#define	UC_ALL		0x000F	/*		Correct	Message	On CAN Bus Line	Count */
+#define	UCRC		0x0020	/* Universal Counter Reload/Clear */
+#define	UCCT		0x0040	/* Universal Counter CAN Trigger */
+#define	UCE			0x0080	/* Universal Counter Enable */
+
+/* CAN_ESR Masks			 */
+#define	ACKE		0x0004	/* Acknowledge Error */
+#define	SER			0x0008	/* Stuff Error */
+#define	CRCE		0x0010	/* CRC Error */
+#define	SA0			0x0020	/* Stuck At Dominant Error */
+#define	BEF			0x0040	/* Bit Error Flag */
+#define	FER			0x0080	/* Form	Error Flag */
+
+/* CAN_EWR Masks					 */
+#define	EWLREC		0x00FF	/* RX Error Count Limit	(For EWRIS) */
+#define	EWLTEC		0xFF00	/* TX Error Count Limit	(For EWTIS) */
+
+#endif /* _DEF_BF539_H */
diff --git a/arch/blackfin/mach-bf538/include/mach/dma.h b/arch/blackfin/mach-bf538/include/mach/dma.h
new file mode 100644
index 000000000000..eb05cacbf4d3
--- /dev/null
+++ b/arch/blackfin/mach-bf538/include/mach/dma.h
@@ -0,0 +1,41 @@
+/* mach/dma.h - arch-specific DMA defines
+ *
+ * Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#ifndef _MACH_DMA_H_
+#define _MACH_DMA_H_
+
+#define CH_PPI			0
+#define CH_SPORT0_RX		1
+#define CH_SPORT0_TX		2
+#define CH_SPORT1_RX		3
+#define CH_SPORT1_TX		4
+#define CH_SPI0			5
+#define CH_UART0_RX		6
+#define CH_UART0_TX		7
+#define CH_SPORT2_RX		8
+#define CH_SPORT2_TX		9
+#define CH_SPORT3_RX		10
+#define CH_SPORT3_TX		11
+#define CH_SPI1			14
+#define CH_SPI2			15
+#define CH_UART1_RX		16
+#define CH_UART1_TX		17
+#define CH_UART2_RX		18
+#define CH_UART2_TX		19
+
+#define CH_MEM_STREAM0_DEST	20
+#define CH_MEM_STREAM0_SRC	21
+#define CH_MEM_STREAM1_DEST	22
+#define CH_MEM_STREAM1_SRC	23
+#define CH_MEM_STREAM2_DEST	24
+#define CH_MEM_STREAM2_SRC	25
+#define CH_MEM_STREAM3_DEST	26
+#define CH_MEM_STREAM3_SRC	27
+
+#define MAX_DMA_CHANNELS 28
+
+#endif
diff --git a/arch/blackfin/mach-bf538/include/mach/gpio.h b/arch/blackfin/mach-bf538/include/mach/gpio.h
new file mode 100644
index 000000000000..30f4f723f7cc
--- /dev/null
+++ b/arch/blackfin/mach-bf538/include/mach/gpio.h
@@ -0,0 +1,79 @@
+/*
+ * File: arch/blackfin/mach-bf538/include/mach/gpio.h
+ * Bugs: Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Copyright (C) 2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
+ */
+
+
+#ifndef _MACH_GPIO_H_
+#define _MACH_GPIO_H_
+
+	/* FIXME:
+	 * For now only support PORTF GPIOs.
+	 * PORT C,D and E are for peripheral usage only
+	 */
+#define MAX_BLACKFIN_GPIOS 16
+
+#define	GPIO_PF0	0	/* PF */
+#define	GPIO_PF1	1
+#define	GPIO_PF2	2
+#define	GPIO_PF3	3
+#define	GPIO_PF4	4
+#define	GPIO_PF5	5
+#define	GPIO_PF6	6
+#define	GPIO_PF7	7
+#define	GPIO_PF8	8
+#define	GPIO_PF9	9
+#define	GPIO_PF10	10
+#define	GPIO_PF11	11
+#define	GPIO_PF12	12
+#define	GPIO_PF13	13
+#define	GPIO_PF14	14
+#define	GPIO_PF15	15
+#define	GPIO_PC0	16	/* PC */
+#define	GPIO_PC1	17
+#define	GPIO_PC4	20
+#define	GPIO_PC5	21
+#define	GPIO_PC6	22
+#define	GPIO_PC7	23
+#define	GPIO_PC8	24
+#define	GPIO_PC9	25
+#define	GPIO_PD0	32	/* PD */
+#define	GPIO_PD1	33
+#define	GPIO_PD2	34
+#define	GPIO_PD3	35
+#define	GPIO_PD4	36
+#define	GPIO_PD5	37
+#define	GPIO_PD6	38
+#define	GPIO_PD7	39
+#define	GPIO_PD8	40
+#define	GPIO_PD9	41
+#define	GPIO_PD10      	42
+#define	GPIO_PD11      	43
+#define	GPIO_PD12      	44
+#define	GPIO_PD13      	45
+#define	GPIO_PE0	48	/* PE */
+#define	GPIO_PE1	49
+#define	GPIO_PE2	50
+#define	GPIO_PE3	51
+#define	GPIO_PE4	52
+#define	GPIO_PE5	53
+#define	GPIO_PE6	54
+#define	GPIO_PE7	55
+#define	GPIO_PE8	56
+#define	GPIO_PE9	57
+#define	GPIO_PE10      	58
+#define	GPIO_PE11      	59
+#define	GPIO_PE12      	60
+#define	GPIO_PE13      	61
+#define	GPIO_PE14      	62
+#define	GPIO_PE15      	63
+
+#define PORT_F GPIO_PF0
+#define PORT_C GPIO_PC0
+#define PORT_D GPIO_PD0
+#define PORT_E GPIO_PE0
+
+#endif /* _MACH_GPIO_H_ */
diff --git a/arch/blackfin/mach-bf538/include/mach/irq.h b/arch/blackfin/mach-bf538/include/mach/irq.h
new file mode 100644
index 000000000000..fdc87fe2c174
--- /dev/null
+++ b/arch/blackfin/mach-bf538/include/mach/irq.h
@@ -0,0 +1,211 @@
+/*
+ * file:	include/asm-blackfin/mach-bf538/irq.h
+ * based on:	include/asm-blackfin/mach-bf537/irq.h
+ * author:	Michael Hennerich (michael.hennerich@analog.com)
+ *
+ * created:
+ * description:
+ *	system mmr register map
+ * rev:
+ *
+ * modified:
+ *
+ *
+ * bugs:         enter bugs at http://blackfin.uclinux.org/
+ *
+ * this program is free software; you can redistribute it and/or modify
+ * it under the terms of the gnu general public license as published by
+ * the free software foundation; either version 2, 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; see the file copying.
+ * if not, write to the free software foundation,
+ * 59 temple place - suite 330, boston, ma 02111-1307, usa.
+ */
+
+#ifndef _BF538_IRQ_H_
+#define _BF538_IRQ_H_
+
+/*
+ * Interrupt source definitions
+	Event Source    Core Event Name
+	Core        Emulation               **
+	Events         (highest priority)  EMU         0
+	Reset                   RST         1
+	NMI                     NMI         2
+	Exception               EVX         3
+	Reserved                --          4
+	Hardware Error          IVHW        5
+	Core Timer              IVTMR       6 *
+
+	.....
+
+	 Software Interrupt 1    IVG14       31
+	 Software Interrupt 2    --
+	 (lowest priority)  IVG15       32 *
+*/
+
+#define NR_PERI_INTS    (2 * 32)
+
+/* The ABSTRACT IRQ definitions */
+/** the first seven of the following are fixed, the rest you change if you need to **/
+#define IRQ_EMU			0	/* Emulation */
+#define IRQ_RST			1	/* reset */
+#define IRQ_NMI			2	/* Non Maskable */
+#define IRQ_EVX			3	/* Exception */
+#define IRQ_UNUSED		4	/* - unused interrupt */
+#define IRQ_HWERR		5	/* Hardware Error */
+#define IRQ_CORETMR		6	/* Core timer */
+
+#define BFIN_IRQ(x)		((x) + 7)
+
+#define IRQ_PLL_WAKEUP		BFIN_IRQ(0)	/* PLL Wakeup Interrupt */
+#define IRQ_DMA0_ERROR		BFIN_IRQ(1)	/* DMA Error 0 (generic) */
+#define IRQ_PPI_ERROR		BFIN_IRQ(2)	/* PPI Error */
+#define IRQ_SPORT0_ERROR	BFIN_IRQ(3)	/* SPORT0 Status */
+#define IRQ_SPORT1_ERROR	BFIN_IRQ(4)	/* SPORT1 Status */
+#define IRQ_SPI0_ERROR		BFIN_IRQ(5)	/* SPI0 Status */
+#define IRQ_UART0_ERROR		BFIN_IRQ(6)	/* UART0 Status */
+#define IRQ_RTC			BFIN_IRQ(7)	/* RTC */
+#define IRQ_PPI			BFIN_IRQ(8)	/* DMA Channel 0 (PPI) */
+#define IRQ_SPORT0_RX		BFIN_IRQ(9)	/* DMA 1 Channel (SPORT0 RX) */
+#define IRQ_SPORT0_TX		BFIN_IRQ(10)	/* DMA 2 Channel (SPORT0 TX) */
+#define IRQ_SPORT1_RX		BFIN_IRQ(11)	/* DMA 3 Channel (SPORT1 RX) */
+#define IRQ_SPORT1_TX		BFIN_IRQ(12)	/* DMA 4 Channel (SPORT1 TX) */
+#define IRQ_SPI0		BFIN_IRQ(13)	/* DMA 5 Channel (SPI0) */
+#define IRQ_UART0_RX		BFIN_IRQ(14)	/* DMA 6 Channel (UART0 RX) */
+#define IRQ_UART0_TX		BFIN_IRQ(15)	/* DMA 7 Channel (UART0 TX) */
+#define IRQ_TIMER0		BFIN_IRQ(16)	/* Timer 0 */
+#define IRQ_TIMER1		BFIN_IRQ(17)	/* Timer 1 */
+#define IRQ_TIMER2		BFIN_IRQ(18)	/* Timer 2 */
+#define IRQ_PORTF_INTA		BFIN_IRQ(19)	/* Port F Interrupt A */
+#define IRQ_PORTF_INTB		BFIN_IRQ(20)	/* Port F Interrupt B */
+#define IRQ_MEM0_DMA0		BFIN_IRQ(21)	/* MDMA0 Stream 0 */
+#define IRQ_MEM0_DMA1		BFIN_IRQ(22)	/* MDMA0 Stream 1 */
+#define IRQ_WATCH		BFIN_IRQ(23)	/* Software Watchdog Timer */
+#define IRQ_DMA1_ERROR		BFIN_IRQ(24)	/* DMA Error 1 (generic) */
+#define IRQ_SPORT2_ERROR	BFIN_IRQ(25)	/* SPORT2 Status */
+#define IRQ_SPORT3_ERROR	BFIN_IRQ(26)	/* SPORT3 Status */
+#define IRQ_SPI1_ERROR		BFIN_IRQ(28)	/* SPI1 Status */
+#define IRQ_SPI2_ERROR		BFIN_IRQ(29)	/* SPI2 Status */
+#define IRQ_UART1_ERROR		BFIN_IRQ(30)	/* UART1 Status */
+#define IRQ_UART2_ERROR		BFIN_IRQ(31)	/* UART2 Status */
+#define IRQ_CAN_ERROR		BFIN_IRQ(32)	/* CAN Status (Error) Interrupt */
+#define IRQ_SPORT2_RX		BFIN_IRQ(33)	/* DMA 8 Channel (SPORT2 RX) */
+#define IRQ_SPORT2_TX		BFIN_IRQ(34)	/* DMA 9 Channel (SPORT2 TX) */
+#define IRQ_SPORT3_RX		BFIN_IRQ(35)	/* DMA 10 Channel (SPORT3 RX) */
+#define IRQ_SPORT3_TX		BFIN_IRQ(36)	/* DMA 11 Channel (SPORT3 TX) */
+#define IRQ_SPI1		BFIN_IRQ(39)	/* DMA 14 Channel (SPI1) */
+#define IRQ_SPI2		BFIN_IRQ(40)	/* DMA 15 Channel (SPI2) */
+#define IRQ_UART1_RX		BFIN_IRQ(41)	/* DMA 16 Channel (UART1 RX) */
+#define IRQ_UART1_TX		BFIN_IRQ(42)	/* DMA 17 Channel (UART1 TX) */
+#define IRQ_UART2_RX		BFIN_IRQ(43)	/* DMA 18 Channel (UART2 RX) */
+#define IRQ_UART2_TX		BFIN_IRQ(44)	/* DMA 19 Channel (UART2 TX) */
+#define IRQ_TWI0		BFIN_IRQ(45)	/* TWI0 */
+#define IRQ_TWI1		BFIN_IRQ(46)	/* TWI1 */
+#define IRQ_CAN_RX		BFIN_IRQ(47)	/* CAN Receive Interrupt */
+#define IRQ_CAN_TX		BFIN_IRQ(48)	/* CAN Transmit Interrupt */
+#define IRQ_MEM1_DMA0		BFIN_IRQ(49)	/* MDMA1 Stream 0 */
+#define IRQ_MEM1_DMA1		BFIN_IRQ(50)	/* MDMA1 Stream 1 */
+
+#define SYS_IRQS		BFIN_IRQ(63)	/* 70 */
+
+#define IRQ_PF0         71
+#define IRQ_PF1         72
+#define IRQ_PF2         73
+#define IRQ_PF3         74
+#define IRQ_PF4         75
+#define IRQ_PF5         76
+#define IRQ_PF6         77
+#define IRQ_PF7         78
+#define IRQ_PF8         79
+#define IRQ_PF9         80
+#define IRQ_PF10        81
+#define IRQ_PF11        82
+#define IRQ_PF12        83
+#define IRQ_PF13        84
+#define IRQ_PF14        85
+#define IRQ_PF15        86
+
+#define GPIO_IRQ_BASE	IRQ_PF0
+
+#define NR_IRQS     (IRQ_PF15+1)
+
+#define IVG7            7
+#define IVG8            8
+#define IVG9            9
+#define IVG10           10
+#define IVG11           11
+#define IVG12           12
+#define IVG13           13
+#define IVG14           14
+#define IVG15           15
+
+/* IAR0 BIT FIELDS */
+#define IRQ_PLL_WAKEUP_POS	0
+#define IRQ_DMA0_ERROR_POS	4
+#define IRQ_PPI_ERROR_POS	8
+#define IRQ_SPORT0_ERROR_POS	12
+#define IRQ_SPORT1_ERROR_POS	16
+#define IRQ_SPI0_ERROR_POS	20
+#define IRQ_UART0_ERROR_POS	24
+#define IRQ_RTC_POS		28
+
+/* IAR1 BIT FIELDS */
+#define IRQ_PPI_POS		0
+#define IRQ_SPORT0_RX_POS	4
+#define IRQ_SPORT0_TX_POS	8
+#define IRQ_SPORT1_RX_POS	12
+#define IRQ_SPORT1_TX_POS	16
+#define IRQ_SPI0_POS		20
+#define IRQ_UART0_RX_POS	24
+#define IRQ_UART0_TX_POS	28
+
+/* IAR2 BIT FIELDS */
+#define IRQ_TIMER0_POS		0
+#define IRQ_TIMER1_POS		4
+#define IRQ_TIMER2_POS		8
+#define IRQ_PORTF_INTA_POS	12
+#define IRQ_PORTF_INTB_POS	16
+#define IRQ_MEM0_DMA0_POS	20
+#define IRQ_MEM0_DMA1_POS	24
+#define IRQ_WATCH_POS		28
+
+/* IAR3 BIT FIELDS */
+#define IRQ_DMA1_ERROR_POS	0
+#define IRQ_SPORT2_ERROR_POS	4
+#define IRQ_SPORT3_ERROR_POS	8
+#define IRQ_SPI1_ERROR_POS	16
+#define IRQ_SPI2_ERROR_POS	20
+#define IRQ_UART1_ERROR_POS	24
+#define IRQ_UART2_ERROR_POS	28
+
+/* IAR4 BIT FIELDS */
+#define IRQ_CAN_ERROR_POS	0
+#define IRQ_SPORT2_RX_POS	4
+#define IRQ_SPORT2_TX_POS	8
+#define IRQ_SPORT3_RX_POS	12
+#define IRQ_SPORT3_TX_POS	16
+#define IRQ_SPI1_POS		28
+
+/* IAR5 BIT FIELDS */
+#define IRQ_SPI2_POS		0
+#define IRQ_UART1_RX_POS	4
+#define IRQ_UART1_TX_POS	8
+#define IRQ_UART2_RX_POS	12
+#define IRQ_UART2_TX_POS	16
+#define IRQ_TWI0_POS		20
+#define IRQ_TWI1_POS		24
+#define IRQ_CAN_RX_POS		28
+
+/* IAR6 BIT FIELDS */
+#define IRQ_CAN_TX_POS		0
+#define IRQ_MEM1_DMA0_POS	4
+#define IRQ_MEM1_DMA1_POS	8
+#endif				/* _BF538_IRQ_H_ */
diff --git a/arch/blackfin/mach-bf538/include/mach/mem_map.h b/arch/blackfin/mach-bf538/include/mach/mem_map.h
new file mode 100644
index 000000000000..76811966690e
--- /dev/null
+++ b/arch/blackfin/mach-bf538/include/mach/mem_map.h
@@ -0,0 +1,113 @@
+/*
+ * File:         include/asm-blackfin/mach-bf538/mem_map.h
+ * Based on:
+ * Author:
+ *
+ * Created:
+ * Description:
+ *
+ * Rev:
+ *
+ * Modified:
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, 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; see the file COPYING.
+ * If not, write to the Free Software Foundation,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef _MEM_MAP_538_H_
+#define _MEM_MAP_538_H_
+
+#define COREMMR_BASE           0xFFE00000	 /* Core MMRs */
+#define SYSMMR_BASE            0xFFC00000	 /* System MMRs */
+
+/* Async Memory Banks */
+#define ASYNC_BANK3_BASE	0x20300000	 /* Async Bank 3 */
+#define ASYNC_BANK3_SIZE	0x00100000	/* 1M */
+#define ASYNC_BANK2_BASE	0x20200000	 /* Async Bank 2 */
+#define ASYNC_BANK2_SIZE	0x00100000	/* 1M */
+#define ASYNC_BANK1_BASE	0x20100000	 /* Async Bank 1 */
+#define ASYNC_BANK1_SIZE	0x00100000	/* 1M */
+#define ASYNC_BANK0_BASE	0x20000000	 /* Async Bank 0 */
+#define ASYNC_BANK0_SIZE	0x00100000	/* 1M */
+
+/* Boot ROM Memory */
+
+#define BOOT_ROM_START		0xEF000000
+#define BOOT_ROM_LENGTH		0x400
+
+/* Level 1 Memory */
+
+#ifdef CONFIG_BFIN_ICACHE
+#define BFIN_ICACHESIZE	(16*1024)
+#else
+#define BFIN_ICACHESIZE	(0*1024)
+#endif
+
+/* Memory Map for ADSP-BF538/9 processors */
+
+#define L1_CODE_START       0xFFA00000
+#define L1_DATA_A_START     0xFF800000
+#define L1_DATA_B_START     0xFF900000
+
+#ifdef CONFIG_BFIN_ICACHE
+#define L1_CODE_LENGTH      (0x14000 - 0x4000)
+#else
+#define L1_CODE_LENGTH      0x14000
+#endif
+
+#ifdef CONFIG_BFIN_DCACHE
+
+#ifdef CONFIG_BFIN_DCACHE_BANKA
+#define DMEM_CNTR (ACACHE_BSRAM | ENDCPLB | PORT_PREF0)
+#define L1_DATA_A_LENGTH      (0x8000 - 0x4000)
+#define L1_DATA_B_LENGTH      0x8000
+#define BFIN_DCACHESIZE	(16*1024)
+#define BFIN_DSUPBANKS	1
+#else
+#define DMEM_CNTR (ACACHE_BCACHE | ENDCPLB | PORT_PREF0)
+#define L1_DATA_A_LENGTH      (0x8000 - 0x4000)
+#define L1_DATA_B_LENGTH      (0x8000 - 0x4000)
+#define BFIN_DCACHESIZE	(32*1024)
+#define BFIN_DSUPBANKS	2
+#endif
+
+#else
+#define DMEM_CNTR (ASRAM_BSRAM | ENDCPLB | PORT_PREF0)
+#define L1_DATA_A_LENGTH      0x8000
+#define L1_DATA_B_LENGTH      0x8000
+#define BFIN_DCACHESIZE	(0*1024)
+#define BFIN_DSUPBANKS	0
+#endif /*CONFIG_BFIN_DCACHE*/
+
+
+/* Level 2 Memory - none */
+
+#define L2_START	0
+#define L2_LENGTH	0
+
+/* Scratch Pad Memory */
+
+#define L1_SCRATCH_START	0xFFB00000
+#define L1_SCRATCH_LENGTH	0x1000
+
+#define GET_PDA_SAFE(preg)		\
+	preg.l = _cpu_pda;		\
+	preg.h = _cpu_pda;
+
+#define GET_PDA(preg, dreg)	GET_PDA_SAFE(preg)
+
+#endif				/* _MEM_MAP_538_H_ */
diff --git a/arch/blackfin/mach-bf538/include/mach/portmux.h b/arch/blackfin/mach-bf538/include/mach/portmux.h
new file mode 100644
index 000000000000..1e031b588b47
--- /dev/null
+++ b/arch/blackfin/mach-bf538/include/mach/portmux.h
@@ -0,0 +1,106 @@
+#ifndef _MACH_PORTMUX_H_
+#define _MACH_PORTMUX_H_
+
+#define MAX_RESOURCES	MAX_BLACKFIN_GPIOS
+
+#define P_TMR2		(P_DONTCARE)
+#define P_TMR1		(P_DONTCARE)
+#define P_TMR0		(P_DONTCARE)
+#define P_TMRCLK	(P_DONTCARE)
+#define P_PPI0_CLK	(P_DONTCARE)
+#define P_PPI0_FS1	(P_DONTCARE)
+#define P_PPI0_FS2	(P_DONTCARE)
+
+#define P_TWI0_SCL	(P_DONTCARE)
+#define P_TWI0_SDA	(P_DONTCARE)
+#define P_TWI1_SCL	(P_DONTCARE)
+#define P_TWI1_SDA	(P_DONTCARE)
+
+#define P_SPORT1_TSCLK	(P_DONTCARE)
+#define P_SPORT1_RSCLK	(P_DONTCARE)
+#define P_SPORT0_TSCLK	(P_DONTCARE)
+#define P_SPORT0_RSCLK	(P_DONTCARE)
+#define P_SPORT1_DRSEC	(P_DONTCARE)
+#define P_SPORT1_RFS	(P_DONTCARE)
+#define P_SPORT1_DTPRI	(P_DONTCARE)
+#define P_SPORT1_DTSEC	(P_DONTCARE)
+#define P_SPORT1_TFS	(P_DONTCARE)
+#define P_SPORT1_DRPRI	(P_DONTCARE)
+#define P_SPORT0_DRSEC	(P_DONTCARE)
+#define P_SPORT0_RFS	(P_DONTCARE)
+#define P_SPORT0_DTPRI	(P_DONTCARE)
+#define P_SPORT0_DTSEC	(P_DONTCARE)
+#define P_SPORT0_TFS	(P_DONTCARE)
+#define P_SPORT0_DRPRI	(P_DONTCARE)
+
+#define P_UART0_RX	(P_DONTCARE)
+#define P_UART0_TX	(P_DONTCARE)
+
+#define P_SPI0_MOSI	(P_DONTCARE)
+#define P_SPI0_MISO	(P_DONTCARE)
+#define P_SPI0_SCK	(P_DONTCARE)
+
+#define P_PPI0_D0	(P_DONTCARE)
+#define P_PPI0_D1	(P_DONTCARE)
+#define P_PPI0_D2	(P_DONTCARE)
+#define P_PPI0_D3	(P_DONTCARE)
+
+#define P_CAN0_TX	(P_DEFINED | P_IDENT(GPIO_PC0))
+#define P_CAN0_RX	(P_DEFINED | P_IDENT(GPIO_PC1))
+
+#define P_SPI1_MOSI	(P_DEFINED | P_IDENT(GPIO_PD0))
+#define P_SPI1_MISO	(P_DEFINED | P_IDENT(GPIO_PD1))
+#define P_SPI1_SCK	(P_DEFINED | P_IDENT(GPIO_PD2))
+#define P_SPI1_SS	(P_DEFINED | P_IDENT(GPIO_PD3))
+#define P_SPI1_SSEL1	(P_DEFINED | P_IDENT(GPIO_PD4))
+#define P_SPI2_MOSI	(P_DEFINED | P_IDENT(GPIO_PD5))
+#define P_SPI2_MISO	(P_DEFINED | P_IDENT(GPIO_PD6))
+#define P_SPI2_SCK	(P_DEFINED | P_IDENT(GPIO_PD7))
+#define P_SPI2_SS	(P_DEFINED | P_IDENT(GPIO_PD8))
+#define P_SPI2_SSEL1	(P_DEFINED | P_IDENT(GPIO_PD9))
+#define P_UART1_RX	(P_DEFINED | P_IDENT(GPIO_PD10))
+#define P_UART1_TX	(P_DEFINED | P_IDENT(GPIO_PD11))
+#define P_UART2_RX	(P_DEFINED | P_IDENT(GPIO_PD12))
+#define P_UART2_TX	(P_DEFINED | P_IDENT(GPIO_PD13))
+
+#define P_SPORT2_RSCLK	(P_DEFINED | P_IDENT(GPIO_PE0))
+#define P_SPORT2_RFS	(P_DEFINED | P_IDENT(GPIO_PE1))
+#define P_SPORT2_DRPRI	(P_DEFINED | P_IDENT(GPIO_PE2))
+#define P_SPORT2_DRSEC	(P_DEFINED | P_IDENT(GPIO_PE3))
+#define P_SPORT2_TSCLK	(P_DEFINED | P_IDENT(GPIO_PE4))
+#define P_SPORT2_TFS	(P_DEFINED | P_IDENT(GPIO_PE5))
+#define P_SPORT2_DTPRI	(P_DEFINED | P_IDENT(GPIO_PE6))
+#define P_SPORT2_DTSEC	(P_DEFINED | P_IDENT(GPIO_PE7))
+#define P_SPORT3_RSCLK	(P_DEFINED | P_IDENT(GPIO_PE8))
+#define P_SPORT3_RFS	(P_DEFINED | P_IDENT(GPIO_PE9))
+#define P_SPORT3_DRPRI	(P_DEFINED | P_IDENT(GPIO_PE10))
+#define P_SPORT3_DRSEC	(P_DEFINED | P_IDENT(GPIO_PE11))
+#define P_SPORT3_TSCLK	(P_DEFINED | P_IDENT(GPIO_PE12))
+#define P_SPORT3_TFS	(P_DEFINED | P_IDENT(GPIO_PE13))
+#define P_SPORT3_DTPRI	(P_DEFINED | P_IDENT(GPIO_PE14))
+#define P_SPORT3_DTSEC	(P_DEFINED | P_IDENT(GPIO_PE15))
+
+#define P_PPI0_FS3	(P_DEFINED | P_IDENT(GPIO_PF3))
+#define P_PPI0_D15	(P_DEFINED | P_IDENT(GPIO_PF4))
+#define P_PPI0_D14	(P_DEFINED | P_IDENT(GPIO_PF5))
+#define P_PPI0_D13	(P_DEFINED | P_IDENT(GPIO_PF6))
+#define P_PPI0_D12	(P_DEFINED | P_IDENT(GPIO_PF7))
+#define P_PPI0_D11	(P_DEFINED | P_IDENT(GPIO_PF8))
+#define P_PPI0_D10	(P_DEFINED | P_IDENT(GPIO_PF9))
+#define P_PPI0_D9	(P_DEFINED | P_IDENT(GPIO_PF10))
+#define P_PPI0_D8	(P_DEFINED | P_IDENT(GPIO_PF11))
+
+#define P_PPI0_D4	(P_DEFINED | P_IDENT(GPIO_PF15))
+#define P_PPI0_D5	(P_DEFINED | P_IDENT(GPIO_PF14))
+#define P_PPI0_D6	(P_DEFINED | P_IDENT(GPIO_PF13))
+#define P_PPI0_D7	(P_DEFINED | P_IDENT(GPIO_PF12))
+#define P_SPI0_SSEL7	(P_DEFINED | P_IDENT(GPIO_PF7))
+#define P_SPI0_SSEL6	(P_DEFINED | P_IDENT(GPIO_PF6))
+#define P_SPI0_SSEL5	(P_DEFINED | P_IDENT(GPIO_PF5))
+#define P_SPI0_SSEL4	(P_DEFINED | P_IDENT(GPIO_PF4))
+#define P_SPI0_SSEL3	(P_DEFINED | P_IDENT(GPIO_PF3))
+#define P_SPI0_SSEL2	(P_DEFINED | P_IDENT(GPIO_PF2))
+#define P_SPI0_SSEL1	(P_DEFINED | P_IDENT(GPIO_PF1))
+#define P_SPI0_SS	(P_DEFINED | P_IDENT(GPIO_PF0))
+
+#endif /* _MACH_PORTMUX_H_ */
diff --git a/arch/blackfin/mach-bf538/ints-priority.c b/arch/blackfin/mach-bf538/ints-priority.c
new file mode 100644
index 000000000000..70d17e550e05
--- /dev/null
+++ b/arch/blackfin/mach-bf538/ints-priority.c
@@ -0,0 +1,94 @@
+/*
+ * File:         arch/blackfin/mach-bf538/ints-priority.c
+ * Based on:     arch/blackfin/mach-bf533/ints-priority.c
+ * Author:       Michael Hennerich
+ *
+ * Created:
+ * Description:  Set up the interrupt priorities
+ *
+ * Modified:
+ *               Copyright 2008 Analog Devices Inc.
+ *
+ * Bugs:         Enter bugs at http://blackfin.uclinux.org/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#include <linux/module.h>
+#include <linux/irq.h>
+#include <asm/blackfin.h>
+
+void __init program_IAR(void)
+{
+
+	/* Program the IAR0 Register with the configured priority */
+	bfin_write_SIC_IAR0(((CONFIG_IRQ_PLL_WAKEUP - 7) << IRQ_PLL_WAKEUP_POS) |
+			((CONFIG_IRQ_DMA0_ERROR - 7) << IRQ_DMA0_ERROR_POS) |
+			((CONFIG_IRQ_PPI_ERROR - 7) << IRQ_PPI_ERROR_POS) |
+			((CONFIG_IRQ_SPORT0_ERROR - 7) << IRQ_SPORT0_ERROR_POS) |
+			((CONFIG_IRQ_SPORT1_ERROR - 7) << IRQ_SPORT1_ERROR_POS) |
+			((CONFIG_IRQ_SPI0_ERROR - 7) << IRQ_SPI0_ERROR_POS) |
+			((CONFIG_IRQ_UART0_ERROR - 7) << IRQ_UART0_ERROR_POS) |
+			((CONFIG_IRQ_RTC - 7) << IRQ_RTC_POS));
+
+	bfin_write_SIC_IAR1(((CONFIG_IRQ_PPI - 7) << IRQ_PPI_POS) |
+			((CONFIG_IRQ_SPORT0_RX - 7) << IRQ_SPORT0_RX_POS) |
+			((CONFIG_IRQ_SPORT0_TX - 7) << IRQ_SPORT0_TX_POS) |
+			((CONFIG_IRQ_SPORT1_RX - 7) << IRQ_SPORT1_RX_POS) |
+			((CONFIG_IRQ_SPORT1_TX - 7) << IRQ_SPORT1_TX_POS) |
+			((CONFIG_IRQ_SPI0 - 7) << IRQ_SPI0_POS) |
+			((CONFIG_IRQ_UART0_RX - 7) << IRQ_UART0_RX_POS) |
+			((CONFIG_IRQ_UART0_TX - 7) << IRQ_UART0_TX_POS));
+
+	bfin_write_SIC_IAR2(((CONFIG_IRQ_TIMER0 - 7) << IRQ_TIMER0_POS) |
+			((CONFIG_IRQ_TIMER1 - 7) << IRQ_TIMER1_POS) |
+			((CONFIG_IRQ_TIMER2 - 7) << IRQ_TIMER2_POS) |
+			((CONFIG_IRQ_PORTF_INTA - 7) << IRQ_PORTF_INTA_POS) |
+			((CONFIG_IRQ_PORTF_INTB - 7) << IRQ_PORTF_INTB_POS) |
+			((CONFIG_IRQ_MEM0_DMA0 - 7) << IRQ_MEM0_DMA0_POS) |
+			((CONFIG_IRQ_MEM0_DMA1 - 7) << IRQ_MEM0_DMA1_POS) |
+			((CONFIG_IRQ_WATCH - 7) << IRQ_WATCH_POS));
+
+	bfin_write_SIC_IAR3(((CONFIG_IRQ_DMA1_ERROR - 7) << IRQ_DMA1_ERROR_POS) |
+			((CONFIG_IRQ_SPORT2_ERROR - 7) << IRQ_SPORT2_ERROR_POS) |
+			((CONFIG_IRQ_SPORT3_ERROR - 7) << IRQ_SPORT3_ERROR_POS) |
+			((CONFIG_IRQ_SPI1_ERROR - 7) << IRQ_SPI1_ERROR_POS) |
+			((CONFIG_IRQ_SPI2_ERROR - 7) << IRQ_SPI2_ERROR_POS) |
+			((CONFIG_IRQ_UART1_ERROR - 7) << IRQ_UART1_ERROR_POS) |
+			((CONFIG_IRQ_UART2_ERROR - 7) << IRQ_UART2_ERROR_POS));
+
+	bfin_write_SIC_IAR4(((CONFIG_IRQ_CAN_ERROR - 7) << IRQ_CAN_ERROR_POS) |
+			((CONFIG_IRQ_SPORT2_RX - 7) << IRQ_SPORT2_RX_POS) |
+			((CONFIG_IRQ_SPORT2_TX - 7) << IRQ_SPORT2_TX_POS) |
+			((CONFIG_IRQ_SPORT3_RX - 7) << IRQ_SPORT3_RX_POS) |
+			((CONFIG_IRQ_SPORT3_TX - 7) << IRQ_SPORT3_TX_POS) |
+			((CONFIG_IRQ_SPI1 - 7) << IRQ_SPI1_POS));
+
+	bfin_write_SIC_IAR5(((CONFIG_IRQ_SPI2 - 7) << IRQ_SPI2_POS) |
+			((CONFIG_IRQ_UART1_RX - 7) << IRQ_UART1_RX_POS) |
+			((CONFIG_IRQ_UART1_TX - 7) << IRQ_UART1_TX_POS) |
+			((CONFIG_IRQ_UART2_RX - 7) << IRQ_UART2_RX_POS) |
+			((CONFIG_IRQ_UART2_TX - 7) << IRQ_UART2_TX_POS) |
+			((CONFIG_IRQ_TWI0 - 7) << IRQ_TWI0_POS) |
+			((CONFIG_IRQ_TWI1 - 7) << IRQ_TWI1_POS) |
+			((CONFIG_IRQ_CAN_RX - 7) << IRQ_CAN_RX_POS));
+
+	bfin_write_SIC_IAR6(((CONFIG_IRQ_CAN_TX - 7) << IRQ_CAN_TX_POS) |
+			((CONFIG_IRQ_MEM1_DMA0 - 7) << IRQ_MEM1_DMA0_POS) |
+			((CONFIG_IRQ_MEM1_DMA1 - 7) << IRQ_MEM1_DMA1_POS));
+
+	SSYNC();
+}
diff --git a/arch/blackfin/mach-bf548/Kconfig b/arch/blackfin/mach-bf548/Kconfig
index 1bfcd8f646ab..dcf657159051 100644
--- a/arch/blackfin/mach-bf548/Kconfig
+++ b/arch/blackfin/mach-bf548/Kconfig
@@ -250,7 +250,7 @@ config IRQ_OTPSEC
 	default 11
 config IRQ_TIMER0
 	int "IRQ_TIMER0"
-	default 11
+	default 8
 config IRQ_TIMER1
 	int "IRQ_TIMER1"
 	default 11
diff --git a/arch/blackfin/mach-bf548/Makefile b/arch/blackfin/mach-bf548/Makefile
index 68e5478e95a9..56994b675f9c 100644
--- a/arch/blackfin/mach-bf548/Makefile
+++ b/arch/blackfin/mach-bf548/Makefile
@@ -2,6 +2,4 @@
 # arch/blackfin/mach-bf537/Makefile
 #
 
-extra-y := head.o
-
 obj-y := ints-priority.o dma.o
diff --git a/arch/blackfin/mach-bf548/boards/cm_bf548.c b/arch/blackfin/mach-bf548/boards/cm_bf548.c
index 24192aaa9275..f53ad682530b 100644
--- a/arch/blackfin/mach-bf548/boards/cm_bf548.c
+++ b/arch/blackfin/mach-bf548/boards/cm_bf548.c
@@ -32,6 +32,7 @@
 #include <linux/platform_device.h>
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/partitions.h>
+#include <linux/mtd/physmap.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/flash.h>
 #include <linux/irq.h>
@@ -42,6 +43,7 @@
 #include <asm/gpio.h>
 #include <asm/nand.h>
 #include <asm/portmux.h>
+#include <asm/bfin_sdh.h>
 #include <mach/bf54x_keys.h>
 #include <asm/dpmc.h>
 #include <linux/input.h>
@@ -186,44 +188,107 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
 #endif
 #ifdef CONFIG_BFIN_SIR1
+static struct resource bfin_sir1_resources[] = {
 	{
 		.start = 0xFFC02000,
 		.end = 0xFFC020FF,
 		.flags = IORESOURCE_MEM,
 	},
+	{
+		.start = IRQ_UART1_RX,
+		.end = IRQ_UART1_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART1_RX,
+		.end = CH_UART1_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+static struct platform_device bfin_sir1_device = {
+	.name = "bfin_sir",
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_sir1_resources),
+	.resource = bfin_sir1_resources,
+};
 #endif
 #ifdef CONFIG_BFIN_SIR2
+static struct resource bfin_sir2_resources[] = {
 	{
 		.start = 0xFFC02100,
 		.end = 0xFFC021FF,
 		.flags = IORESOURCE_MEM,
 	},
+	{
+		.start = IRQ_UART2_RX,
+		.end = IRQ_UART2_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART2_RX,
+		.end = CH_UART2_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+static struct platform_device bfin_sir2_device = {
+	.name = "bfin_sir",
+	.id = 2,
+	.num_resources = ARRAY_SIZE(bfin_sir2_resources),
+	.resource = bfin_sir2_resources,
+};
 #endif
 #ifdef CONFIG_BFIN_SIR3
+static struct resource bfin_sir3_resources[] = {
 	{
 		.start = 0xFFC03100,
 		.end = 0xFFC031FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART3_RX,
+		.end = IRQ_UART3_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART3_RX,
+		.end = CH_UART3_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
-
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir3_device = {
 	.name = "bfin_sir",
-	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.id = 3,
+	.num_resources = ARRAY_SIZE(bfin_sir3_resources),
+	.resource = bfin_sir3_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_SMSC911X) || defined(CONFIG_SMSC911X_MODULE)
 static struct resource smsc911x_resources[] = {
@@ -271,8 +336,8 @@ static struct musb_hdrc_config musb_config = {
 	.dyn_fifo	= 0,
 	.soft_con	= 1,
 	.dma		= 1,
-	.num_eps	= 7,
-	.dma_channels	= 7,
+	.num_eps	= 8,
+	.dma_channels	= 8,
 	.gpio_vrsel	= GPIO_PH6,
 };
 
@@ -302,6 +367,19 @@ static struct platform_device musb_device = {
 };
 #endif
 
+static struct resource bfin_gpios_resources = {
+	.start = 0,
+	.end   = MAX_BLACKFIN_GPIOS - 1,
+	.flags = IORESOURCE_IRQ,
+};
+
+static struct platform_device bfin_gpios_device = {
+	.name = "simple-gpio",
+	.id = -1,
+	.num_resources = 1,
+	.resource = &bfin_gpios_resources,
+};
+
 #if defined(CONFIG_PATA_BF54X) || defined(CONFIG_PATA_BF54X_MODULE)
 static struct resource bfin_atapi_resources[] = {
 	{
@@ -372,9 +450,58 @@ static struct platform_device bf5xx_nand_device = {
 #endif
 
 #if defined(CONFIG_SDH_BFIN) || defined(CONFIG_SDH_BFIN_MODULE)
+static struct bfin_sd_host bfin_sdh_data = {
+	.dma_chan = CH_SDH,
+	.irq_int0 = IRQ_SDH_MASK0,
+	.pin_req = {P_SD_D0, P_SD_D1, P_SD_D2, P_SD_D3, P_SD_CLK, P_SD_CMD, 0},
+};
+
 static struct platform_device bf54x_sdh_device = {
 	.name = "bfin-sdh",
 	.id = 0,
+	.dev = {
+		.platform_data = &bfin_sdh_data,
+	},
+};
+#endif
+
+#if defined(CONFIG_MTD_PHYSMAP) || defined(CONFIG_MTD_PHYSMAP_MODULE)
+static struct mtd_partition para_partitions[] = {
+	{
+		.name       = "bootloader(nor)",
+		.size       = 0x40000,
+		.offset     = 0,
+	}, {
+		.name       = "linux kernel(nor)",
+		.size       = 0x400000,
+		.offset     = MTDPART_OFS_APPEND,
+	}, {
+		.name       = "file system(nor)",
+		.size       = MTDPART_SIZ_FULL,
+		.offset     = MTDPART_OFS_APPEND,
+	}
+};
+
+static struct physmap_flash_data para_flash_data = {
+	.width      = 2,
+	.parts      = para_partitions,
+	.nr_parts   = ARRAY_SIZE(para_partitions),
+};
+
+static struct resource para_flash_resource = {
+	.start = 0x20000000,
+	.end   = 0x207fffff,
+	.flags = IORESOURCE_MEM,
+};
+
+static struct platform_device para_flash_device = {
+	.name          = "physmap-flash",
+	.id            = 0,
+	.dev = {
+		.platform_data = &para_flash_data,
+	},
+	.num_resources = 1,
+	.resource      = &para_flash_resource,
 };
 #endif
 
@@ -642,7 +769,18 @@ static struct platform_device *cm_bf548_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#ifdef CONFIG_BFIN_SIR1
+	&bfin_sir1_device,
+#endif
+#ifdef CONFIG_BFIN_SIR2
+	&bfin_sir2_device,
+#endif
+#ifdef CONFIG_BFIN_SIR3
+	&bfin_sir3_device,
+#endif
 #endif
 
 #if defined(CONFIG_FB_BF54X_LQ043) || defined(CONFIG_FB_BF54X_LQ043_MODULE)
@@ -679,7 +817,7 @@ static struct platform_device *cm_bf548_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
-/*	&i2c_bfin_twi0_device, */
+	&i2c_bfin_twi0_device,
 #if !defined(CONFIG_BF542)
 	&i2c_bfin_twi1_device,
 #endif
@@ -688,6 +826,12 @@ static struct platform_device *cm_bf548_devices[] __initdata = {
 #if defined(CONFIG_KEYBOARD_GPIO) || defined(CONFIG_KEYBOARD_GPIO_MODULE)
 	&bfin_device_gpiokeys,
 #endif
+
+#if defined(CONFIG_MTD_PHYSMAP) || defined(CONFIG_MTD_PHYSMAP_MODULE)
+	&para_flash_device,
+#endif
+
+	&bfin_gpios_device,
 };
 
 static int __init cm_bf548_init(void)
diff --git a/arch/blackfin/mach-bf548/boards/ezkit.c b/arch/blackfin/mach-bf548/boards/ezkit.c
index 5288187a3ace..309c16014cae 100644
--- a/arch/blackfin/mach-bf548/boards/ezkit.c
+++ b/arch/blackfin/mach-bf548/boards/ezkit.c
@@ -45,6 +45,7 @@
 #include <asm/nand.h>
 #include <asm/dpmc.h>
 #include <asm/portmux.h>
+#include <asm/bfin_sdh.h>
 #include <mach/bf54x_keys.h>
 #include <linux/input.h>
 #include <linux/spi/ad7877.h>
@@ -52,16 +53,16 @@
 /*
  * Name the Board for the /proc/cpuinfo
  */
-const char bfin_board_name[] = "ADSP-BF548-EZKIT";
+const char bfin_board_name[] = "ADI BF548-EZKIT";
 
 /*
  *  Driver needs to know address, irq and flag pin.
  */
 
 #if defined(CONFIG_USB_ISP1760_HCD) || defined(CONFIG_USB_ISP1760_HCD_MODULE)
-static struct resource bfin_isp1761_resources[] = {
+#include <linux/usb/isp1760.h>
+static struct resource bfin_isp1760_resources[] = {
 	[0] = {
-		.name	= "isp1761-regs",
 		.start  = 0x2C0C0000,
 		.end    = 0x2C0C0000 + 0xfffff,
 		.flags  = IORESOURCE_MEM,
@@ -73,32 +74,25 @@ static struct resource bfin_isp1761_resources[] = {
 	},
 };
 
-static struct platform_device bfin_isp1761_device = {
-	.name           = "isp1761",
-	.id             = 0,
-	.num_resources  = ARRAY_SIZE(bfin_isp1761_resources),
-	.resource       = bfin_isp1761_resources,
+static struct isp1760_platform_data isp1760_priv = {
+	.is_isp1761 = 0,
+	.port1_disable = 0,
+	.bus_width_16 = 1,
+	.port1_otg = 0,
+	.analog_oc = 0,
+	.dack_polarity_high = 0,
+	.dreq_polarity_high = 0,
 };
 
-static struct platform_device *bfin_isp1761_devices[] = {
-	&bfin_isp1761_device,
+static struct platform_device bfin_isp1760_device = {
+	.name           = "isp1760-hcd",
+	.id             = 0,
+	.dev = {
+		.platform_data = &isp1760_priv,
+	},
+	.num_resources  = ARRAY_SIZE(bfin_isp1760_resources),
+	.resource       = bfin_isp1760_resources,
 };
-
-int __init bfin_isp1761_init(void)
-{
-	unsigned int num_devices = ARRAY_SIZE(bfin_isp1761_devices);
-
-	printk(KERN_INFO "%s(): registering device resources\n", __func__);
-	set_irq_type(bfin_isp1761_resources[1].start, IRQF_TRIGGER_FALLING);
-
-	return platform_add_devices(bfin_isp1761_devices, num_devices);
-}
-
-void __exit bfin_isp1761_exit(void)
-{
-	platform_device_unregister(&bfin_isp1761_device);
-}
-arch_initcall(bfin_isp1761_init);
 #endif
 
 #if defined(CONFIG_FB_BF54X_LQ043) || defined(CONFIG_FB_BF54X_LQ043_MODULE)
@@ -262,44 +256,107 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
 #endif
 #ifdef CONFIG_BFIN_SIR1
+static struct resource bfin_sir1_resources[] = {
 	{
 		.start = 0xFFC02000,
 		.end = 0xFFC020FF,
 		.flags = IORESOURCE_MEM,
 	},
+	{
+		.start = IRQ_UART1_RX,
+		.end = IRQ_UART1_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART1_RX,
+		.end = CH_UART1_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+static struct platform_device bfin_sir1_device = {
+	.name = "bfin_sir",
+	.id = 1,
+	.num_resources = ARRAY_SIZE(bfin_sir1_resources),
+	.resource = bfin_sir1_resources,
+};
 #endif
 #ifdef CONFIG_BFIN_SIR2
+static struct resource bfin_sir2_resources[] = {
 	{
 		.start = 0xFFC02100,
 		.end = 0xFFC021FF,
 		.flags = IORESOURCE_MEM,
 	},
+	{
+		.start = IRQ_UART2_RX,
+		.end = IRQ_UART2_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART2_RX,
+		.end = CH_UART2_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+static struct platform_device bfin_sir2_device = {
+	.name = "bfin_sir",
+	.id = 2,
+	.num_resources = ARRAY_SIZE(bfin_sir2_resources),
+	.resource = bfin_sir2_resources,
+};
 #endif
 #ifdef CONFIG_BFIN_SIR3
+static struct resource bfin_sir3_resources[] = {
 	{
 		.start = 0xFFC03100,
 		.end = 0xFFC031FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART3_RX,
+		.end = IRQ_UART3_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART3_RX,
+		.end = CH_UART3_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
-
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir3_device = {
 	.name = "bfin_sir",
-	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.id = 3,
+	.num_resources = ARRAY_SIZE(bfin_sir3_resources),
+	.resource = bfin_sir3_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_SMSC911X) || defined(CONFIG_SMSC911X_MODULE)
 static struct resource smsc911x_resources[] = {
@@ -347,8 +404,8 @@ static struct musb_hdrc_config musb_config = {
 	.dyn_fifo	= 0,
 	.soft_con	= 1,
 	.dma		= 1,
-	.num_eps	= 7,
-	.dma_channels	= 7,
+	.num_eps	= 8,
+	.dma_channels	= 8,
 	.gpio_vrsel	= GPIO_PE7,
 };
 
@@ -448,9 +505,19 @@ static struct platform_device bf5xx_nand_device = {
 #endif
 
 #if defined(CONFIG_SDH_BFIN) || defined(CONFIG_SDH_BFIN_MODULE)
+
+static struct bfin_sd_host bfin_sdh_data = {
+	.dma_chan = CH_SDH,
+	.irq_int0 = IRQ_SDH_MASK0,
+	.pin_req = {P_SD_D0, P_SD_D1, P_SD_D2, P_SD_D3, P_SD_CLK, P_SD_CMD, 0},
+};
+
 static struct platform_device bf54x_sdh_device = {
 	.name = "bfin-sdh",
 	.id = 0,
+	.dev = {
+		.platform_data = &bfin_sdh_data,
+	},
 };
 #endif
 
@@ -589,7 +656,7 @@ static struct spi_board_info bfin_spi_board_info[] __initdata = {
 {
 	.modalias		= "ad7877",
 	.platform_data		= &bfin_ad7877_ts_info,
-	.irq			= IRQ_PJ11,	/* newer boards (Rev 1.4+) use IRQ_PB4 */
+	.irq			= IRQ_PB4,	/* old boards (<=Rev 1.3) use IRQ_PJ11 */
 	.max_speed_hz		= 12500000,     /* max spi clock (SCK) speed in HZ */
 	.bus_num		= 0,
 	.chip_select  		= 2,
@@ -812,7 +879,18 @@ static struct platform_device *ezkit_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#ifdef CONFIG_BFIN_SIR1
+	&bfin_sir1_device,
+#endif
+#ifdef CONFIG_BFIN_SIR2
+	&bfin_sir2_device,
+#endif
+#ifdef CONFIG_BFIN_SIR3
+	&bfin_sir3_device,
+#endif
 #endif
 
 #if defined(CONFIG_FB_BF54X_LQ043) || defined(CONFIG_FB_BF54X_LQ043_MODULE)
@@ -827,6 +905,10 @@ static struct platform_device *ezkit_devices[] __initdata = {
 	&musb_device,
 #endif
 
+#if defined(CONFIG_USB_ISP1760_HCD) || defined(CONFIG_USB_ISP1760_HCD_MODULE)
+	&bfin_isp1760_device,
+#endif
+
 #if defined(CONFIG_PATA_BF54X) || defined(CONFIG_PATA_BF54X_MODULE)
 	&bfin_atapi_device,
 #endif
diff --git a/arch/blackfin/mach-bf548/dma.c b/arch/blackfin/mach-bf548/dma.c
index 74730eb8ae1b..535980652bf6 100644
--- a/arch/blackfin/mach-bf548/dma.c
+++ b/arch/blackfin/mach-bf548/dma.c
@@ -32,7 +32,7 @@
 #include <asm/blackfin.h>
 #include <asm/dma.h>
 
-struct dma_register *dma_io_base_addr[MAX_BLACKFIN_DMA_CHANNEL] = {
+struct dma_register *dma_io_base_addr[MAX_DMA_CHANNELS] = {
 	(struct dma_register *) DMA0_NEXT_DESC_PTR,
 	(struct dma_register *) DMA1_NEXT_DESC_PTR,
 	(struct dma_register *) DMA2_NEXT_DESC_PTR,
diff --git a/arch/blackfin/mach-bf548/head.S b/arch/blackfin/mach-bf548/head.S
deleted file mode 100644
index 93b361dff27b..000000000000
--- a/arch/blackfin/mach-bf548/head.S
+++ /dev/null
@@ -1,158 +0,0 @@
-/*
- * File:         arch/blackfin/mach-bf548/head.S
- * Based on:     arch/blackfin/mach-bf537/head.S
- * Author:       Jeff Dionne <jeff@uclinux.org> COPYRIGHT 1998 D. Jeff Dionne
- *
- * Created:      1998
- * Description:  Startup code for Blackfin BF548
- *
- * Modified:
- *               Copyright 2004-2007 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#include <linux/linkage.h>
-#include <linux/init.h>
-#include <asm/blackfin.h>
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-#include <asm/clocks.h>
-#include <mach/mem_init.h>
-#endif
-
-.section .l1.text
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-ENTRY(_start_dma_code)
-
-	/* Enable PHY CLK buffer output */
-	p0.h = hi(VR_CTL);
-	p0.l = lo(VR_CTL);
-	r0.l = w[p0];
-	bitset(r0, 14);
-	w[p0] = r0.l;
-	ssync;
-
-	p0.h = hi(SIC_IWR0);
-	p0.l = lo(SIC_IWR0);
-	r0.l = 0x1;
-	r0.h = 0x0;
-	[p0] = r0;
-	SSYNC;
-
-	/*
-	 *  Set PLL_CTL
-	 *   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
-	 *   - [8]     = BYPASS    : BYPASS the PLL, run CLKIN into CCLK/SCLK
-	 *   - [7]     = output delay (add 200ps of delay to mem signals)
-	 *   - [6]     = input delay (add 200ps of input delay to mem signals)
-	 *   - [5]     = PDWN      : 1=All Clocks off
-	 *   - [3]     = STOPCK    : 1=Core Clock off
-	 *   - [1]     = PLL_OFF   : 1=Disable Power to PLL
-	 *   - [0]     = DF        : 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
-	 *   all other bits set to zero
-	 */
-
-	p0.h = hi(PLL_LOCKCNT);
-	p0.l = lo(PLL_LOCKCNT);
-	r0 = 0x300(Z);
-	w[p0] = r0.l;
-	ssync;
-
-	/* enable self refresh via SRREQ */
-	P2.H = hi(EBIU_RSTCTL);
-	P2.L = lo(EBIU_RSTCTL);
-	R0 = [P2];
-	BITSET (R0, 3);
-	[P2] = R0;
-	SSYNC;
-
-	/* wait for SRACK bit to be set */
-.LSRR_MODE:
-	R0 = [P2];
-	CC = BITTST(R0, 4);
-	if !CC JUMP .LSRR_MODE;
-
-	r0 = CONFIG_VCO_MULT & 63;       /* Load the VCO multiplier         */
-	r0 = r0 << 9;                    /* Shift it over,                  */
-	r1 = CLKIN_HALF;                 /* Do we need to divide CLKIN by 2?*/
-	r0 = r1 | r0;
-	r1 = PLL_BYPASS;                 /* Bypass the PLL?                 */
-	r1 = r1 << 8;                    /* Shift it over                   */
-	r0 = r1 | r0;                    /* add them all together           */
-#ifdef ANOMALY_05000265
-	BITSET(r0, 15);                  /* Add 250 mV of hysteresis to SPORT input pins */
-#endif
-
-	p0.h = hi(PLL_CTL);
-	p0.l = lo(PLL_CTL);              /* Load the address                */
-	cli r2;                          /* Disable interrupts              */
-	ssync;
-	w[p0] = r0.l;                    /* Set the value                   */
-	idle;                            /* Wait for the PLL to stablize    */
-	sti r2;                          /* Enable interrupts               */
-
-.Lcheck_again:
-	p0.h = hi(PLL_STAT);
-	p0.l = lo(PLL_STAT);
-	R0 = W[P0](Z);
-	CC = BITTST(R0,5);
-	if ! CC jump .Lcheck_again;
-
-	/* Configure SCLK & CCLK Dividers */
-	r0 = (CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
-	p0.h = hi(PLL_DIV);
-	p0.l = lo(PLL_DIV);
-	w[p0] = r0.l;
-	ssync;
-
-	/* disable self refresh by clearing SRREQ */
-	P2.H = hi(EBIU_RSTCTL);
-	P2.L = lo(EBIU_RSTCTL);
-	R0 = [P2];
-	CC = BITTST(R0, 0);
-	if CC jump .Lskipddrrst;
-	BITSET (R0, 0);
-.Lskipddrrst:
-	BITCLR (R0, 3);
-	[P2] = R0;
-	SSYNC;
-
-	p0.l = lo(EBIU_DDRCTL0);
-	p0.h = hi(EBIU_DDRCTL0);
-	r0.l = lo(mem_DDRCTL0);
-	r0.h = hi(mem_DDRCTL0);
-	[p0] = r0;
-	ssync;
-
-	p0.l = lo(EBIU_DDRCTL1);
-	p0.h = hi(EBIU_DDRCTL1);
-	r0.l = lo(mem_DDRCTL1);
-	r0.h = hi(mem_DDRCTL1);
-	[p0] = r0;
-	ssync;
-
-	p0.l = lo(EBIU_DDRCTL2);
-	p0.h = hi(EBIU_DDRCTL2);
-	r0.l = lo(mem_DDRCTL2);
-	r0.h = hi(mem_DDRCTL2);
-	[p0] = r0;
-	ssync;
-
-	RTS;
-ENDPROC(_start_dma_code)
-#endif /* CONFIG_BFIN_KERNEL_CLOCK */
diff --git a/arch/blackfin/mach-bf548/include/mach/anomaly.h b/arch/blackfin/mach-bf548/include/mach/anomaly.h
index 816b09278f62..3b5430999f4f 100644
--- a/arch/blackfin/mach-bf548/include/mach/anomaly.h
+++ b/arch/blackfin/mach-bf548/include/mach/anomaly.h
@@ -157,6 +157,8 @@
 #define ANOMALY_05000429 (__SILICON_REVISION__ < 2)
 /* Software System Reset Corrupts PLL_LOCKCNT Register */
 #define ANOMALY_05000430 (__SILICON_REVISION__ >= 2)
+/* IFLUSH Instruction at End of Hardware Loop Causes Infinite Stall */
+#define ANOMALY_05000443 (1)
 
 /* Anomalies that don't exist on this proc */
 #define ANOMALY_05000125 (0)
@@ -173,5 +175,8 @@
 #define ANOMALY_05000311 (0)
 #define ANOMALY_05000323 (0)
 #define ANOMALY_05000363 (0)
+#define ANOMALY_05000412 (0)
+#define ANOMALY_05000432 (0)
+#define ANOMALY_05000435 (0)
 
 #endif
diff --git a/arch/blackfin/mach-bf548/include/mach/bf548.h b/arch/blackfin/mach-bf548/include/mach/bf548.h
index 49f9b403d458..f0e569984810 100644
--- a/arch/blackfin/mach-bf548/include/mach/bf548.h
+++ b/arch/blackfin/mach-bf548/include/mach/bf548.h
@@ -122,7 +122,7 @@
 #endif
 
 #ifndef CPU
-#error Unknown CPU type - This kernel doesn't seem to be configured properly
+#error "Unknown CPU type - This kernel doesn't seem to be configured properly"
 #endif
 
 #endif	/* __MACH_BF48_H__  */
diff --git a/arch/blackfin/mach-bf548/include/mach/bfin_sir.h b/arch/blackfin/mach-bf548/include/mach/bfin_sir.h
deleted file mode 100644
index c41f9cf00268..000000000000
--- a/arch/blackfin/mach-bf548/include/mach/bfin_sir.h
+++ /dev/null
@@ -1,166 +0,0 @@
-/*
- * Blackfin Infra-red Driver
- *
- * Copyright 2006-2008 Analog Devices Inc.
- *
- * Enter bugs at http://blackfin.uclinux.org/
- *
- * Licensed under the GPL-2 or later.
- *
- */
-
-#include <linux/serial.h>
-#include <asm/dma.h>
-#include <asm/portmux.h>
-
-#define SIR_UART_GET_CHAR(port)   bfin_read16((port)->membase + OFFSET_RBR)
-#define SIR_UART_GET_DLL(port)    bfin_read16((port)->membase + OFFSET_DLL)
-#define SIR_UART_GET_IER(port)    bfin_read16((port)->membase + OFFSET_IER_SET)
-#define SIR_UART_GET_DLH(port)    bfin_read16((port)->membase + OFFSET_DLH)
-#define SIR_UART_GET_LCR(port)    bfin_read16((port)->membase + OFFSET_LCR)
-#define SIR_UART_GET_LSR(port)    bfin_read16((port)->membase + OFFSET_LSR)
-#define SIR_UART_GET_GCTL(port)   bfin_read16((port)->membase + OFFSET_GCTL)
-
-#define SIR_UART_PUT_CHAR(port, v) bfin_write16(((port)->membase + OFFSET_THR), v)
-#define SIR_UART_PUT_DLL(port, v)  bfin_write16(((port)->membase + OFFSET_DLL), v)
-#define SIR_UART_SET_IER(port, v)  bfin_write16(((port)->membase + OFFSET_IER_SET), v)
-#define SIR_UART_CLEAR_IER(port, v)  bfin_write16(((port)->membase + OFFSET_IER_CLEAR), v)
-#define SIR_UART_PUT_DLH(port, v)  bfin_write16(((port)->membase + OFFSET_DLH), v)
-#define SIR_UART_PUT_LSR(port, v)  bfin_write16(((port)->membase + OFFSET_LSR), v)
-#define SIR_UART_PUT_LCR(port, v)  bfin_write16(((port)->membase + OFFSET_LCR), v)
-#define SIR_UART_CLEAR_LSR(port)  bfin_write16(((port)->membase + OFFSET_LSR), -1)
-#define SIR_UART_PUT_GCTL(port, v) bfin_write16(((port)->membase + OFFSET_GCTL), v)
-
-#ifdef CONFIG_SIR_BFIN_DMA
-struct dma_rx_buf {
-	char *buf;
-	int head;
-	int tail;
-	};
-#endif /* CONFIG_SIR_BFIN_DMA */
-
-struct bfin_sir_port {
-	unsigned char __iomem   *membase;
-	unsigned int            irq;
-	unsigned int            lsr;
-	unsigned long           clk;
-	struct net_device       *dev;
-#ifdef CONFIG_SIR_BFIN_DMA
-	int                     tx_done;
-	struct dma_rx_buf       rx_dma_buf;
-	struct timer_list       rx_dma_timer;
-	int                     rx_dma_nrows;
-#endif /* CONFIG_SIR_BFIN_DMA */
-	unsigned int            tx_dma_channel;
-	unsigned int            rx_dma_channel;
-};
-
-struct bfin_sir_port sir_ports[BFIN_UART_NR_PORTS];
-
-struct bfin_sir_port_res {
-	unsigned long   base_addr;
-	int             irq;
-	unsigned int    rx_dma_channel;
-	unsigned int    tx_dma_channel;
-};
-
-struct bfin_sir_port_res bfin_sir_port_resource[] = {
-#ifdef CONFIG_BFIN_SIR0
-	{
-	0xFFC00400,
-	IRQ_UART0_RX,
-	CH_UART0_RX,
-	CH_UART0_TX,
-	},
-#endif
-#ifdef CONFIG_BFIN_SIR1
-	{
-	0xFFC02000,
-	IRQ_UART1_RX,
-	CH_UART1_RX,
-	CH_UART1_TX,
-	},
-#endif
-#ifdef CONFIG_BFIN_SIR2
-	{
-	0xFFC02100,
-	IRQ_UART2_RX,
-	CH_UART2_RX,
-	CH_UART2_TX,
-	},
-#endif
-#ifdef CONFIG_BFIN_SIR3
-	{
-	0xFFC03100,
-	IRQ_UART3_RX,
-	CH_UART3_RX,
-	CH_UART3_TX,
-	},
-#endif
-};
-
-int nr_sirs = ARRAY_SIZE(bfin_sir_port_resource);
-
-struct bfin_sir_self {
-	struct bfin_sir_port    *sir_port;
-	spinlock_t              lock;
-	unsigned int            open;
-	int                     speed;
-	int                     newspeed;
-
-	struct sk_buff          *txskb;
-	struct sk_buff          *rxskb;
-	struct net_device_stats stats;
-	struct device           *dev;
-	struct irlap_cb         *irlap;
-	struct qos_info         qos;
-
-	iobuff_t                tx_buff;
-	iobuff_t                rx_buff;
-
-	struct work_struct      work;
-	int                     mtt;
-};
-
-#define DRIVER_NAME "bfin_sir"
-
-static int bfin_sir_hw_init(void)
-{
-	int ret = -ENODEV;
-#ifdef CONFIG_BFIN_SIR0
-	ret = peripheral_request(P_UART0_TX, DRIVER_NAME);
-	if (ret)
-		return ret;
-	ret = peripheral_request(P_UART0_RX, DRIVER_NAME);
-	if (ret)
-		return ret;
-#endif
-
-#ifdef CONFIG_BFIN_SIR1
-	ret = peripheral_request(P_UART1_TX, DRIVER_NAME);
-	if (ret)
-		return ret;
-	ret = peripheral_request(P_UART1_RX, DRIVER_NAME);
-	if (ret)
-		return ret;
-#endif
-
-#ifdef CONFIG_BFIN_SIR2
-	ret = peripheral_request(P_UART2_TX, DRIVER_NAME);
-	if (ret)
-		return ret;
-	ret = peripheral_request(P_UART2_RX, DRIVER_NAME);
-	if (ret)
-		return ret;
-#endif
-
-#ifdef CONFIG_BFIN_SIR3
-	ret = peripheral_request(P_UART3_TX, DRIVER_NAME);
-	if (ret)
-		return ret;
-	ret = peripheral_request(P_UART3_RX, DRIVER_NAME);
-	if (ret)
-		return ret;
-#endif
-	return ret;
-}
diff --git a/arch/blackfin/mach-bf548/include/mach/blackfin.h b/arch/blackfin/mach-bf548/include/mach/blackfin.h
index d6ee74ac0460..0c0e3e2c3c21 100644
--- a/arch/blackfin/mach-bf548/include/mach/blackfin.h
+++ b/arch/blackfin/mach-bf548/include/mach/blackfin.h
@@ -111,7 +111,7 @@
 
 /* UART 0*/
 
-/* DMA Channnel */
+/* DMA Channel */
 #define bfin_read_CH_UART_RX()		bfin_read_CH_UART1_RX()
 #define bfin_write_CH_UART_RX(val)	bfin_write_CH_UART1_RX(val)
 #define bfin_read_CH_UART_TX()		bfin_read_CH_UART1_TX()
diff --git a/arch/blackfin/mach-bf548/include/mach/cdefBF54x_base.h b/arch/blackfin/mach-bf548/include/mach/cdefBF54x_base.h
index 57ac8cb9b1f6..6e636c418cb0 100644
--- a/arch/blackfin/mach-bf548/include/mach/cdefBF54x_base.h
+++ b/arch/blackfin/mach-bf548/include/mach/cdefBF54x_base.h
@@ -34,7 +34,6 @@
 #include <asm/blackfin.h>
 
 #include "defBF54x_base.h"
-#include <asm/system.h>
 
 /* ************************************************************** */
 /* SYSTEM & MMR ADDRESS DEFINITIONS COMMON TO ALL ADSP-BF54x    */
@@ -43,63 +42,9 @@
 /* PLL Registers */
 
 #define bfin_read_PLL_CTL()		bfin_read16(PLL_CTL)
-/* Writing to PLL_CTL initiates a PLL relock sequence. */
-static __inline__ void bfin_write_PLL_CTL(unsigned int val)
-{
-	unsigned long flags, iwr0, iwr1, iwr2;
-
-	if (val == bfin_read_PLL_CTL())
-		return;
-
-	local_irq_save(flags);
-	/* Enable the PLL Wakeup bit in SIC IWR */
-	iwr0 = bfin_read32(SIC_IWR0);
-	iwr1 = bfin_read32(SIC_IWR1);
-	iwr2 = bfin_read32(SIC_IWR2);
-	/* Only allow PPL Wakeup) */
-	bfin_write32(SIC_IWR0, IWR_ENABLE(0));
-	bfin_write32(SIC_IWR1, 0);
-	bfin_write32(SIC_IWR2, 0);
-
-	bfin_write16(PLL_CTL, val);
-	SSYNC();
-	asm("IDLE;");
-
-	bfin_write32(SIC_IWR0, iwr0);
-	bfin_write32(SIC_IWR1, iwr1);
-	bfin_write32(SIC_IWR2, iwr2);
-	local_irq_restore(flags);
-}
 #define bfin_read_PLL_DIV()		bfin_read16(PLL_DIV)
 #define bfin_write_PLL_DIV(val)		bfin_write16(PLL_DIV, val)
 #define bfin_read_VR_CTL()		bfin_read16(VR_CTL)
-/* Writing to VR_CTL initiates a PLL relock sequence. */
-static __inline__ void bfin_write_VR_CTL(unsigned int val)
-{
-	unsigned long flags, iwr0, iwr1, iwr2;
-
-	if (val == bfin_read_VR_CTL())
-		return;
-
-	local_irq_save(flags);
-	/* Enable the PLL Wakeup bit in SIC IWR */
-	iwr0 = bfin_read32(SIC_IWR0);
-	iwr1 = bfin_read32(SIC_IWR1);
-	iwr2 = bfin_read32(SIC_IWR2);
-	/* Only allow PPL Wakeup) */
-	bfin_write32(SIC_IWR0, IWR_ENABLE(0));
-	bfin_write32(SIC_IWR1, 0);
-	bfin_write32(SIC_IWR2, 0);
-
-	bfin_write16(VR_CTL, val);
-	SSYNC();
-	asm("IDLE;");
-
-	bfin_write32(SIC_IWR0, iwr0);
-	bfin_write32(SIC_IWR1, iwr1);
-	bfin_write32(SIC_IWR2, iwr2);
-	local_irq_restore(flags);
-}
 #define bfin_read_PLL_STAT()		bfin_read16(PLL_STAT)
 #define bfin_write_PLL_STAT(val)	bfin_write16(PLL_STAT, val)
 #define bfin_read_PLL_LOCKCNT()		bfin_read16(PLL_LOCKCNT)
@@ -2746,5 +2691,64 @@ static __inline__ void bfin_write_VR_CTL(unsigned int val)
 #define bfin_read_PINT3_IRQ		bfin_read_PINT3_REQUEST
 #define bfin_write_PINT3_IRQ		bfin_write_PINT3_REQUEST
 
+/* These need to be last due to the cdef/linux inter-dependencies */
+#include <asm/irq.h>
+
+/* Writing to PLL_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_PLL_CTL(unsigned int val)
+{
+	unsigned long flags, iwr0, iwr1, iwr2;
+
+	if (val == bfin_read_PLL_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr0 = bfin_read32(SIC_IWR0);
+	iwr1 = bfin_read32(SIC_IWR1);
+	iwr2 = bfin_read32(SIC_IWR2);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SIC_IWR0, IWR_ENABLE(0));
+	bfin_write32(SIC_IWR1, 0);
+	bfin_write32(SIC_IWR2, 0);
+
+	bfin_write16(PLL_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SIC_IWR0, iwr0);
+	bfin_write32(SIC_IWR1, iwr1);
+	bfin_write32(SIC_IWR2, iwr2);
+	local_irq_restore_hw(flags);
+}
+
+/* Writing to VR_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_VR_CTL(unsigned int val)
+{
+	unsigned long flags, iwr0, iwr1, iwr2;
+
+	if (val == bfin_read_VR_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr0 = bfin_read32(SIC_IWR0);
+	iwr1 = bfin_read32(SIC_IWR1);
+	iwr2 = bfin_read32(SIC_IWR2);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SIC_IWR0, IWR_ENABLE(0));
+	bfin_write32(SIC_IWR1, 0);
+	bfin_write32(SIC_IWR2, 0);
+
+	bfin_write16(VR_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SIC_IWR0, iwr0);
+	bfin_write32(SIC_IWR1, iwr1);
+	bfin_write32(SIC_IWR2, iwr2);
+	local_irq_restore_hw(flags);
+}
+
 #endif /* _CDEF_BF54X_H */
 
diff --git a/arch/blackfin/mach-bf548/include/mach/dma.h b/arch/blackfin/mach-bf548/include/mach/dma.h
index 36a2ef7e7849..a30d242c7398 100644
--- a/arch/blackfin/mach-bf548/include/mach/dma.h
+++ b/arch/blackfin/mach-bf548/include/mach/dma.h
@@ -1,32 +1,8 @@
-/*
- * file:         include/asm-blackfin/mach-bf548/dma.h
- * based on:
- * author:
+/* mach/dma.h - arch-specific DMA defines
  *
- * created:
- * description:
- *	system mmr register map
- * rev:
+ * Copyright 2004-2008 Analog Devices Inc.
  *
- * modified:
- *
- *
- * bugs:         enter bugs at http://blackfin.uclinux.org/
- *
- * this program is free software; you can redistribute it and/or modify
- * it under the terms of the gnu general public license as published by
- * the free software foundation; either version 2, 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; see the file copying.
- * if not, write to the free software foundation,
- * 59 temple place - suite 330, boston, ma 02111-1307, usa.
+ * Licensed under the GPL-2 or later.
  */
 
 #ifndef _MACH_DMA_H_
@@ -71,6 +47,6 @@
 #define CH_MEM_STREAM3_DEST	30
 #define CH_MEM_STREAM3_SRC	31
 
-#define MAX_BLACKFIN_DMA_CHANNEL 32
+#define MAX_DMA_CHANNELS 32
 
 #endif
diff --git a/arch/blackfin/mach-bf548/include/mach/irq.h b/arch/blackfin/mach-bf548/include/mach/irq.h
index ad380d1f5872..60299a71e090 100644
--- a/arch/blackfin/mach-bf548/include/mach/irq.h
+++ b/arch/blackfin/mach-bf548/include/mach/irq.h
@@ -158,7 +158,7 @@ Events         (highest priority)  EMU         0
 #define IRQ_PINT2		BFIN_IRQ(94)	/* PINT2 Interrupt */
 #define IRQ_PINT3		BFIN_IRQ(95)	/* PINT3 Interrupt */
 
-#define SYS_IRQS        	IRQ_PINT3
+#define SYS_IRQS		IRQ_PINT3
 
 #define BFIN_PA_IRQ(x)		((x) + SYS_IRQS + 1)
 #define IRQ_PA0			BFIN_PA_IRQ(0)
diff --git a/arch/blackfin/mach-bf548/include/mach/mem_init.h b/arch/blackfin/mach-bf548/include/mach/mem_init.h
deleted file mode 100644
index ab0b863eee66..000000000000
--- a/arch/blackfin/mach-bf548/include/mach/mem_init.h
+++ /dev/null
@@ -1,255 +0,0 @@
-/*
- * File:         include/asm-blackfin/mach-bf548/mem_init.h
- * Based on:
- * Author:
- *
- * Created:
- * Description:
- *
- * Rev:
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, 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; see the file COPYING.
- * If not, write to the Free Software Foundation,
- * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-#define MIN_DDR_SCLK(x)	(x*(CONFIG_SCLK_HZ/1000/1000)/1000 + 1)
-#define MAX_DDR_SCLK(x)	(x*(CONFIG_SCLK_HZ/1000/1000)/1000)
-#define DDR_CLK_HZ(x)	(1000*1000*1000/x)
-
-#if (CONFIG_MEM_MT46V32M16_6T)
-#define DDR_SIZE	DEVSZ_512
-#define DDR_WIDTH	DEVWD_16
-#define DDR_MAX_tCK	13
-
-#define DDR_tRC		DDR_TRC(MIN_DDR_SCLK(60))
-#define DDR_tRAS	DDR_TRAS(MIN_DDR_SCLK(42))
-#define DDR_tRP		DDR_TRP(MIN_DDR_SCLK(15))
-#define DDR_tRFC	DDR_TRFC(MIN_DDR_SCLK(72))
-#define DDR_tREFI	DDR_TREFI(MAX_DDR_SCLK(7800))
-
-#define DDR_tRCD	DDR_TRCD(MIN_DDR_SCLK(15))
-#define DDR_tWTR	DDR_TWTR(1)
-#define DDR_tMRD	DDR_TMRD(MIN_DDR_SCLK(12))
-#define DDR_tWR		DDR_TWR(MIN_DDR_SCLK(15))
-#endif
-
-#if (CONFIG_MEM_MT46V32M16_5B)
-#define DDR_SIZE	DEVSZ_512
-#define DDR_WIDTH	DEVWD_16
-#define DDR_MAX_tCK	13
-
-#define DDR_tRC		DDR_TRC(MIN_DDR_SCLK(55))
-#define DDR_tRAS	DDR_TRAS(MIN_DDR_SCLK(40))
-#define DDR_tRP		DDR_TRP(MIN_DDR_SCLK(15))
-#define DDR_tRFC	DDR_TRFC(MIN_DDR_SCLK(70))
-#define DDR_tREFI	DDR_TREFI(MAX_DDR_SCLK(7800))
-
-#define DDR_tRCD	DDR_TRCD(MIN_DDR_SCLK(15))
-#define DDR_tWTR	DDR_TWTR(2)
-#define DDR_tMRD	DDR_TMRD(MIN_DDR_SCLK(10))
-#define DDR_tWR		DDR_TWR(MIN_DDR_SCLK(15))
-#endif
-
-#if (CONFIG_MEM_GENERIC_BOARD)
-#define DDR_SIZE	DEVSZ_512
-#define DDR_WIDTH	DEVWD_16
-#define DDR_MAX_tCK	13
-
-#define DDR_tRCD	DDR_TRCD(3)
-#define DDR_tWTR	DDR_TWTR(2)
-#define DDR_tWR		DDR_TWR(2)
-#define DDR_tMRD	DDR_TMRD(2)
-#define DDR_tRP		DDR_TRP(3)
-#define DDR_tRAS	DDR_TRAS(7)
-#define DDR_tRC		DDR_TRC(10)
-#define DDR_tRFC	DDR_TRFC(12)
-#define DDR_tREFI	DDR_TREFI(1288)
-#endif
-
-#if (CONFIG_SCLK_HZ < DDR_CLK_HZ(DDR_MAX_tCK))
-# error "CONFIG_SCLK_HZ is too small (<DDR_CLK_HZ(DDR_MAX_tCK) Hz)."
-#elif(CONFIG_SCLK_HZ <= 133333333)
-# define	DDR_CL		CL_2
-#else
-# error "CONFIG_SCLK_HZ is too large (>133333333 Hz)."
-#endif
-
-
-#define mem_DDRCTL0	(DDR_tRP | DDR_tRAS | DDR_tRC | DDR_tRFC | DDR_tREFI)
-#define mem_DDRCTL1	(DDR_DATWIDTH | EXTBANK_1 | DDR_SIZE | DDR_WIDTH | DDR_tWTR \
-			| DDR_tMRD | DDR_tWR | DDR_tRCD)
-#define mem_DDRCTL2	DDR_CL
-
-
-#if defined CONFIG_CLKIN_HALF
-#define CLKIN_HALF       1
-#else
-#define CLKIN_HALF       0
-#endif
-
-#if defined CONFIG_PLL_BYPASS
-#define PLL_BYPASS      1
-#else
-#define PLL_BYPASS       0
-#endif
-
-/***************************************Currently Not Being Used *********************************/
-#define flash_EBIU_AMBCTL_WAT  ((CONFIG_FLASH_SPEED_BWAT * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-#define flash_EBIU_AMBCTL_RAT  ((CONFIG_FLASH_SPEED_BRAT * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-#define flash_EBIU_AMBCTL_HT   ((CONFIG_FLASH_SPEED_BHT  * 4) / (4000000000 / CONFIG_SCLK_HZ))
-#define flash_EBIU_AMBCTL_ST   ((CONFIG_FLASH_SPEED_BST  * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-#define flash_EBIU_AMBCTL_TT   ((CONFIG_FLASH_SPEED_BTT  * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-
-#if (flash_EBIU_AMBCTL_TT > 3)
-#define flash_EBIU_AMBCTL0_TT   B0TT_4
-#endif
-#if (flash_EBIU_AMBCTL_TT == 3)
-#define flash_EBIU_AMBCTL0_TT   B0TT_3
-#endif
-#if (flash_EBIU_AMBCTL_TT == 2)
-#define flash_EBIU_AMBCTL0_TT   B0TT_2
-#endif
-#if (flash_EBIU_AMBCTL_TT < 2)
-#define flash_EBIU_AMBCTL0_TT   B0TT_1
-#endif
-
-#if (flash_EBIU_AMBCTL_ST > 3)
-#define flash_EBIU_AMBCTL0_ST   B0ST_4
-#endif
-#if (flash_EBIU_AMBCTL_ST == 3)
-#define flash_EBIU_AMBCTL0_ST   B0ST_3
-#endif
-#if (flash_EBIU_AMBCTL_ST == 2)
-#define flash_EBIU_AMBCTL0_ST   B0ST_2
-#endif
-#if (flash_EBIU_AMBCTL_ST < 2)
-#define flash_EBIU_AMBCTL0_ST   B0ST_1
-#endif
-
-#if (flash_EBIU_AMBCTL_HT > 2)
-#define flash_EBIU_AMBCTL0_HT   B0HT_3
-#endif
-#if (flash_EBIU_AMBCTL_HT == 2)
-#define flash_EBIU_AMBCTL0_HT   B0HT_2
-#endif
-#if (flash_EBIU_AMBCTL_HT == 1)
-#define flash_EBIU_AMBCTL0_HT   B0HT_1
-#endif
-#if (flash_EBIU_AMBCTL_HT == 0 && CONFIG_FLASH_SPEED_BHT == 0)
-#define flash_EBIU_AMBCTL0_HT   B0HT_0
-#endif
-#if (flash_EBIU_AMBCTL_HT == 0 && CONFIG_FLASH_SPEED_BHT != 0)
-#define flash_EBIU_AMBCTL0_HT   B0HT_1
-#endif
-
-#if (flash_EBIU_AMBCTL_WAT > 14)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_15
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 14)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_14
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 13)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_13
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 12)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_12
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 11)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_11
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 10)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_10
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 9)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_9
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 8)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_8
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 7)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_7
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 6)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_6
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 5)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_5
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 4)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_4
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 3)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_3
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 2)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_2
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 1)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_1
-#endif
-
-#if (flash_EBIU_AMBCTL_RAT > 14)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_15
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 14)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_14
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 13)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_13
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 12)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_12
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 11)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_11
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 10)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_10
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 9)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_9
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 8)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_8
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 7)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_7
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 6)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_6
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 5)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_5
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 4)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_4
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 3)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_3
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 2)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_2
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 1)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_1
-#endif
-
-#define flash_EBIU_AMBCTL0  \
-	(flash_EBIU_AMBCTL0_WAT | flash_EBIU_AMBCTL0_RAT | flash_EBIU_AMBCTL0_HT | \
-	 flash_EBIU_AMBCTL0_ST | flash_EBIU_AMBCTL0_TT | CONFIG_FLASH_SPEED_RDYEN)
diff --git a/arch/blackfin/mach-bf548/include/mach/mem_map.h b/arch/blackfin/mach-bf548/include/mach/mem_map.h
index a2228428dc06..70b9c1194024 100644
--- a/arch/blackfin/mach-bf548/include/mach/mem_map.h
+++ b/arch/blackfin/mach-bf548/include/mach/mem_map.h
@@ -108,4 +108,10 @@
 #define L1_SCRATCH_START	0xFFB00000
 #define L1_SCRATCH_LENGTH	0x1000
 
+#define GET_PDA_SAFE(preg)		\
+	preg.l = _cpu_pda;		\
+	preg.h = _cpu_pda;
+
+#define GET_PDA(preg, dreg)	GET_PDA_SAFE(preg)
+
 #endif/* _MEM_MAP_548_H_ */
diff --git a/arch/blackfin/mach-bf561/Kconfig b/arch/blackfin/mach-bf561/Kconfig
index 3f4895450bea..638ec38ca470 100644
--- a/arch/blackfin/mach-bf561/Kconfig
+++ b/arch/blackfin/mach-bf561/Kconfig
@@ -4,9 +4,9 @@ source "arch/blackfin/mach-bf561/boards/Kconfig"
 
 menu "BF561 Specific Configuration"
 
-comment "Core B Support"
+if (!SMP)
 
-menu "Core B Support"
+comment "Core B Support"
 
 config BF561_COREB
 	bool "Enable Core B support"
@@ -25,7 +25,7 @@ config BF561_COREB_RESET
 	  0 is set, and will reset PC to 0xff600000 when
 	  COREB_SRAM_INIT is cleared.
 
-endmenu
+endif
 
 comment "Interrupt Priority Assignment"
 
@@ -138,7 +138,7 @@ config IRQ_DMA2_11
 	default 9
 config IRQ_TIMER0
 	int "TIMER 0  Interrupt"
-	default 10
+	default 8
 config IRQ_TIMER1
 	int "TIMER 1  Interrupt"
 	default 10
diff --git a/arch/blackfin/mach-bf561/Makefile b/arch/blackfin/mach-bf561/Makefile
index f39235a55783..59e18afe28c6 100644
--- a/arch/blackfin/mach-bf561/Makefile
+++ b/arch/blackfin/mach-bf561/Makefile
@@ -2,8 +2,7 @@
 # arch/blackfin/mach-bf561/Makefile
 #
 
-extra-y := head.o
-
 obj-y := ints-priority.o dma.o
 
 obj-$(CONFIG_BF561_COREB) += coreb.o
+obj-$(CONFIG_SMP)  += smp.o secondary.o atomic.o
diff --git a/arch/blackfin/mach-bf561/atomic.S b/arch/blackfin/mach-bf561/atomic.S
new file mode 100644
index 000000000000..9439bc6bd01f
--- /dev/null
+++ b/arch/blackfin/mach-bf561/atomic.S
@@ -0,0 +1,919 @@
+/*
+ * File:         arch/blackfin/mach-bf561/atomic.S
+ * Author:       Philippe Gerum <rpm@xenomai.org>
+ *
+ *               Copyright 2007 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 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 the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#include <linux/linkage.h>
+#include <asm/blackfin.h>
+#include <asm/cache.h>
+#include <asm/asm-offsets.h>
+#include <asm/rwlock.h>
+#include <asm/cplb.h>
+
+.text
+
+.macro coreslot_loadaddr reg:req
+	\reg\().l = _corelock;
+	\reg\().h = _corelock;
+.endm
+
+/*
+ * r0 = address of atomic data to flush and invalidate (32bit).
+ *
+ * Clear interrupts and return the old mask.
+ * We assume that no atomic data can span cachelines.
+ *
+ * Clobbers: r2:0, p0
+ */
+ENTRY(_get_core_lock)
+	r1 = -L1_CACHE_BYTES;
+	r1 = r0 & r1;
+	cli r0;
+	coreslot_loadaddr p0;
+.Lretry_corelock:
+	testset (p0);
+	if cc jump .Ldone_corelock;
+	SSYNC(r2);
+	jump .Lretry_corelock
+.Ldone_corelock:
+	p0 = r1;
+	CSYNC(r2);
+	flushinv[p0];
+	SSYNC(r2);
+	rts;
+ENDPROC(_get_core_lock)
+
+/*
+ * r0 = address of atomic data in uncacheable memory region (32bit).
+ *
+ * Clear interrupts and return the old mask.
+ *
+ * Clobbers: r0, p0
+ */
+ENTRY(_get_core_lock_noflush)
+	cli r0;
+	coreslot_loadaddr p0;
+.Lretry_corelock_noflush:
+	testset (p0);
+	if cc jump .Ldone_corelock_noflush;
+	SSYNC(r2);
+	jump .Lretry_corelock_noflush
+.Ldone_corelock_noflush:
+	rts;
+ENDPROC(_get_core_lock_noflush)
+
+/*
+ * r0 = interrupt mask to restore.
+ * r1 = address of atomic data to flush and invalidate (32bit).
+ *
+ * Interrupts are masked on entry (see _get_core_lock).
+ * Clobbers: r2:0, p0
+ */
+ENTRY(_put_core_lock)
+	/* Write-through cache assumed, so no flush needed here. */
+	coreslot_loadaddr p0;
+	r1 = 0;
+	[p0] = r1;
+	SSYNC(r2);
+	sti r0;
+	rts;
+ENDPROC(_put_core_lock)
+
+#ifdef __ARCH_SYNC_CORE_DCACHE
+
+ENTRY(___raw_smp_mark_barrier_asm)
+	[--sp] = rets;
+	[--sp] = ( r7:5 );
+	[--sp] = r0;
+	[--sp] = p1;
+	[--sp] = p0;
+	call _get_core_lock_noflush;
+
+	/*
+	 * Calculate current core mask
+	 */
+	GET_CPUID(p1, r7);
+	r6 = 1;
+	r6 <<= r7;
+
+	/*
+	 * Set bit of other cores in barrier mask. Don't change current core bit.
+	 */
+	p1.l = _barrier_mask;
+	p1.h = _barrier_mask;
+	r7 = [p1];
+	r5 = r7 & r6;
+	r7 = ~r6;
+	cc = r5 == 0;
+	if cc jump 1f;
+	r7 = r7 | r6;
+1:
+	[p1] = r7;
+	SSYNC(r2);
+
+	call _put_core_lock;
+	p0 = [sp++];
+	p1 = [sp++];
+	r0 = [sp++];
+	( r7:5 ) = [sp++];
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_smp_mark_barrier_asm)
+
+ENTRY(___raw_smp_check_barrier_asm)
+	[--sp] = rets;
+	[--sp] = ( r7:5 );
+	[--sp] = r0;
+	[--sp] = p1;
+	[--sp] = p0;
+	call _get_core_lock_noflush;
+
+	/*
+	 * Calculate current core mask
+	 */
+	GET_CPUID(p1, r7);
+	r6 = 1;
+	r6 <<= r7;
+
+	/*
+	 * Clear current core bit in barrier mask if it is set.
+	 */
+	p1.l = _barrier_mask;
+	p1.h = _barrier_mask;
+	r7 = [p1];
+	r5 = r7 & r6;
+	cc = r5 == 0;
+	if cc jump 1f;
+	r6 = ~r6;
+	r7 = r7 & r6;
+	[p1] = r7;
+	SSYNC(r2);
+
+	call _put_core_lock;
+
+	/*
+	 * Invalidate the entire D-cache of current core.
+	 */
+	sp += -12;
+	call _resync_core_dcache
+	sp += 12;
+	jump 2f;
+1:
+	call _put_core_lock;
+2:
+	p0 = [sp++];
+	p1 = [sp++];
+	r0 = [sp++];
+	( r7:5 ) = [sp++];
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_smp_check_barrier_asm)
+
+/*
+ * r0 = irqflags
+ * r1 = address of atomic data
+ *
+ * Clobbers: r2:0, p1:0
+ */
+_start_lock_coherent:
+
+	[--sp] = rets;
+	[--sp] = ( r7:6 );
+	r7 = r0;
+	p1 = r1;
+
+	/*
+	 * Determine whether the atomic data was previously
+	 * owned by another CPU (=r6).
+	 */
+	GET_CPUID(p0, r2);
+	r1 = 1;
+	r1 <<= r2;
+	r2 = ~r1;
+
+	r1 = [p1];
+	r1 >>= 28;   /* CPU fingerprints are stored in the high nibble. */
+	r6 = r1 & r2;
+	r1 = [p1];
+	r1 <<= 4;
+	r1 >>= 4;
+	[p1] = r1;
+
+	/*
+	 * Release the core lock now, but keep IRQs disabled while we are
+	 * performing the remaining housekeeping chores for the current CPU.
+	 */
+	coreslot_loadaddr p0;
+	r1 = 0;
+	[p0] = r1;
+
+	/*
+	 * If another CPU has owned the same atomic section before us,
+	 * then our D-cached copy of the shared data protected by the
+	 * current spin/write_lock may be obsolete.
+	 */
+	cc = r6 == 0;
+	if cc jump .Lcache_synced
+
+	/*
+	 * Invalidate the entire D-cache of the current core.
+	 */
+	sp += -12;
+	call _resync_core_dcache
+	sp += 12;
+
+.Lcache_synced:
+	SSYNC(r2);
+	sti r7;
+	( r7:6 ) = [sp++];
+	rets = [sp++];
+	rts
+
+/*
+ * r0 = irqflags
+ * r1 = address of atomic data
+ *
+ * Clobbers: r2:0, p1:0
+ */
+_end_lock_coherent:
+
+	p1 = r1;
+	GET_CPUID(p0, r2);
+	r2 += 28;
+	r1 = 1;
+	r1 <<= r2;
+	r2 = [p1];
+	r2 = r1 | r2;
+	[p1] = r2;
+	r1 = p1;
+	jump _put_core_lock;
+
+#endif /* __ARCH_SYNC_CORE_DCACHE */
+
+/*
+ * r0 = &spinlock->lock
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_spin_is_locked_asm)
+	p1 = r0;
+	[--sp] = rets;
+	call _get_core_lock;
+	r3 = [p1];
+	cc = bittst( r3, 0 );
+	r3 = cc;
+	r1 = p1;
+	call _put_core_lock;
+	rets = [sp++];
+	r0 = r3;
+	rts;
+ENDPROC(___raw_spin_is_locked_asm)
+
+/*
+ * r0 = &spinlock->lock
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_spin_lock_asm)
+	p1 = r0;
+	[--sp] = rets;
+.Lretry_spinlock:
+	call _get_core_lock;
+	r1 = p1;
+	r2 = [p1];
+	cc = bittst( r2, 0 );
+	if cc jump .Lbusy_spinlock
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	r3 = p1;
+	bitset ( r2, 0 ); /* Raise the lock bit. */
+	[p1] = r2;
+	call _start_lock_coherent
+#else
+	r2 = 1;
+	[p1] = r2;
+	call _put_core_lock;
+#endif
+	rets = [sp++];
+	rts;
+
+.Lbusy_spinlock:
+	/* We don't touch the atomic area if busy, so that flush
+	   will behave like nop in _put_core_lock. */
+	call _put_core_lock;
+	SSYNC(r2);
+	r0 = p1;
+	jump .Lretry_spinlock
+ENDPROC(___raw_spin_lock_asm)
+
+/*
+ * r0 = &spinlock->lock
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_spin_trylock_asm)
+	p1 = r0;
+	[--sp] = rets;
+	call _get_core_lock;
+	r1 = p1;
+	r3 = [p1];
+	cc = bittst( r3, 0 );
+	if cc jump .Lfailed_trylock
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	bitset ( r3, 0 ); /* Raise the lock bit. */
+	[p1] = r3;
+	call _start_lock_coherent
+#else
+	r2 = 1;
+	[p1] = r2;
+	call _put_core_lock;
+#endif
+	r0 = 1;
+	rets = [sp++];
+	rts;
+.Lfailed_trylock:
+	call _put_core_lock;
+	r0 = 0;
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_spin_trylock_asm)
+
+/*
+ * r0 = &spinlock->lock
+ *
+ * Clobbers: r2:0, p1:0
+ */
+ENTRY(___raw_spin_unlock_asm)
+	p1 = r0;
+	[--sp] = rets;
+	call _get_core_lock;
+	r2 = [p1];
+	bitclr ( r2, 0 );
+	[p1] = r2;
+	r1 = p1;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	call _end_lock_coherent
+#else
+	call _put_core_lock;
+#endif
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_spin_unlock_asm)
+
+/*
+ * r0 = &rwlock->lock
+ *
+ * Clobbers: r2:0, p1:0
+ */
+ENTRY(___raw_read_lock_asm)
+	p1 = r0;
+	[--sp] = rets;
+	call _get_core_lock;
+.Lrdlock_try:
+	r1 = [p1];
+	r1 += -1;
+	[p1] = r1;
+	cc = r1 < 0;
+	if cc jump .Lrdlock_failed
+	r1 = p1;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	call _start_lock_coherent
+#else
+	call _put_core_lock;
+#endif
+	rets = [sp++];
+	rts;
+
+.Lrdlock_failed:
+	r1 += 1;
+	[p1] = r1;
+.Lrdlock_wait:
+	r1 = p1;
+	call _put_core_lock;
+	SSYNC(r2);
+	r0 = p1;
+	call _get_core_lock;
+	r1 = [p1];
+	cc = r1 < 2;
+	if cc jump .Lrdlock_wait;
+	jump .Lrdlock_try
+ENDPROC(___raw_read_lock_asm)
+
+/*
+ * r0 = &rwlock->lock
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_read_trylock_asm)
+	p1 = r0;
+	[--sp] = rets;
+	call _get_core_lock;
+	r1 = [p1];
+	cc = r1 <= 0;
+	if cc jump .Lfailed_tryrdlock;
+	r1 += -1;
+	[p1] = r1;
+	r1 = p1;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	call _start_lock_coherent
+#else
+	call _put_core_lock;
+#endif
+	rets = [sp++];
+	r0 = 1;
+	rts;
+.Lfailed_tryrdlock:
+	r1 = p1;
+	call _put_core_lock;
+	rets = [sp++];
+	r0 = 0;
+	rts;
+ENDPROC(___raw_read_trylock_asm)
+
+/*
+ * r0 = &rwlock->lock
+ *
+ * Note: Processing controlled by a reader lock should not have
+ * any side-effect on cache issues with the other core, so we
+ * just release the core lock and exit (no _end_lock_coherent).
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_read_unlock_asm)
+	p1 = r0;
+	[--sp] = rets;
+	call _get_core_lock;
+	r1 = [p1];
+	r1 += 1;
+	[p1] = r1;
+	r1 = p1;
+	call _put_core_lock;
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_read_unlock_asm)
+
+/*
+ * r0 = &rwlock->lock
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_write_lock_asm)
+	p1 = r0;
+	r3.l = lo(RW_LOCK_BIAS);
+	r3.h = hi(RW_LOCK_BIAS);
+	[--sp] = rets;
+	call _get_core_lock;
+.Lwrlock_try:
+	r1 = [p1];
+	r1 = r1 - r3;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	r2 = r1;
+	r2 <<= 4;
+	r2 >>= 4;
+	cc = r2 == 0;
+#else
+	cc = r1 == 0;
+#endif
+	if !cc jump .Lwrlock_wait
+	[p1] = r1;
+	r1 = p1;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	call _start_lock_coherent
+#else
+	call _put_core_lock;
+#endif
+	rets = [sp++];
+	rts;
+
+.Lwrlock_wait:
+	r1 = p1;
+	call _put_core_lock;
+	SSYNC(r2);
+	r0 = p1;
+	call _get_core_lock;
+	r1 = [p1];
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	r1 <<= 4;
+	r1 >>= 4;
+#endif
+	cc = r1 == r3;
+	if !cc jump .Lwrlock_wait;
+	jump .Lwrlock_try
+ENDPROC(___raw_write_lock_asm)
+
+/*
+ * r0 = &rwlock->lock
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_write_trylock_asm)
+	p1 = r0;
+	[--sp] = rets;
+	call _get_core_lock;
+	r1 = [p1];
+	r2.l = lo(RW_LOCK_BIAS);
+	r2.h = hi(RW_LOCK_BIAS);
+	cc = r1 == r2;
+	if !cc jump .Lfailed_trywrlock;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	r1 >>= 28;
+	r1 <<= 28;
+#else
+	r1 = 0;
+#endif
+	[p1] = r1;
+	r1 = p1;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	call _start_lock_coherent
+#else
+	call _put_core_lock;
+#endif
+	rets = [sp++];
+	r0 = 1;
+	rts;
+
+.Lfailed_trywrlock:
+	r1 = p1;
+	call _put_core_lock;
+	rets = [sp++];
+	r0 = 0;
+	rts;
+ENDPROC(___raw_write_trylock_asm)
+
+/*
+ * r0 = &rwlock->lock
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_write_unlock_asm)
+	p1 = r0;
+	r3.l = lo(RW_LOCK_BIAS);
+	r3.h = hi(RW_LOCK_BIAS);
+	[--sp] = rets;
+	call _get_core_lock;
+	r1 = [p1];
+	r1 = r1 + r3;
+	[p1] = r1;
+	r1 = p1;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	call _end_lock_coherent
+#else
+	call _put_core_lock;
+#endif
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_write_unlock_asm)
+
+/*
+ * r0 = ptr
+ * r1 = value
+ *
+ * Add a signed value to a 32bit word and return the new value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_atomic_update_asm)
+	p1 = r0;
+	r3 = r1;
+	[--sp] = rets;
+	call _get_core_lock;
+	r2 = [p1];
+	r3 = r3 + r2;
+	[p1] = r3;
+	r1 = p1;
+	call _put_core_lock;
+	r0 = r3;
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_atomic_update_asm)
+
+/*
+ * r0 = ptr
+ * r1 = mask
+ *
+ * Clear the mask bits from a 32bit word and return the old 32bit value
+ * atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_atomic_clear_asm)
+	p1 = r0;
+	r3 = ~r1;
+	[--sp] = rets;
+	call _get_core_lock;
+	r2 = [p1];
+	r3 = r2 & r3;
+	[p1] = r3;
+	r3 = r2;
+	r1 = p1;
+	call _put_core_lock;
+	r0 = r3;
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_atomic_clear_asm)
+
+/*
+ * r0 = ptr
+ * r1 = mask
+ *
+ * Set the mask bits into a 32bit word and return the old 32bit value
+ * atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_atomic_set_asm)
+	p1 = r0;
+	r3 = r1;
+	[--sp] = rets;
+	call _get_core_lock;
+	r2 = [p1];
+	r3 = r2 | r3;
+	[p1] = r3;
+	r3 = r2;
+	r1 = p1;
+	call _put_core_lock;
+	r0 = r3;
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_atomic_set_asm)
+
+/*
+ * r0 = ptr
+ * r1 = mask
+ *
+ * XOR the mask bits with a 32bit word and return the old 32bit value
+ * atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_atomic_xor_asm)
+	p1 = r0;
+	r3 = r1;
+	[--sp] = rets;
+	call _get_core_lock;
+	r2 = [p1];
+	r3 = r2 ^ r3;
+	[p1] = r3;
+	r3 = r2;
+	r1 = p1;
+	call _put_core_lock;
+	r0 = r3;
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_atomic_xor_asm)
+
+/*
+ * r0 = ptr
+ * r1 = mask
+ *
+ * Perform a logical AND between the mask bits and a 32bit word, and
+ * return the masked value. We need this on this architecture in
+ * order to invalidate the local cache before testing.
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_atomic_test_asm)
+	p1 = r0;
+	r3 = r1;
+	r1 = -L1_CACHE_BYTES;
+	r1 = r0 & r1;
+	p0 = r1;
+	flushinv[p0];
+	SSYNC(r2);
+	r0 = [p1];
+	r0 = r0 & r3;
+	rts;
+ENDPROC(___raw_atomic_test_asm)
+
+/*
+ * r0 = ptr
+ * r1 = value
+ *
+ * Swap *ptr with value and return the old 32bit value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+#define	__do_xchg(src, dst) 		\
+	p1 = r0;			\
+	r3 = r1;			\
+	[--sp] = rets;			\
+	call _get_core_lock;		\
+	r2 = src;			\
+	dst = r3;			\
+	r3 = r2;			\
+	r1 = p1;			\
+	call _put_core_lock;		\
+	r0 = r3;			\
+	rets = [sp++];			\
+	rts;
+
+ENTRY(___raw_xchg_1_asm)
+	__do_xchg(b[p1] (z), b[p1])
+ENDPROC(___raw_xchg_1_asm)
+
+ENTRY(___raw_xchg_2_asm)
+	__do_xchg(w[p1] (z), w[p1])
+ENDPROC(___raw_xchg_2_asm)
+
+ENTRY(___raw_xchg_4_asm)
+	__do_xchg([p1], [p1])
+ENDPROC(___raw_xchg_4_asm)
+
+/*
+ * r0 = ptr
+ * r1 = new
+ * r2 = old
+ *
+ * Swap *ptr with new if *ptr == old and return the previous *ptr
+ * value atomically.
+ *
+ * Clobbers: r3:0, p1:0
+ */
+#define	__do_cmpxchg(src, dst) 		\
+	[--sp] = rets;			\
+	[--sp] = r4;			\
+	p1 = r0;			\
+	r3 = r1;			\
+	r4 = r2;			\
+	call _get_core_lock;		\
+	r2 = src;			\
+	cc = r2 == r4;			\
+	if !cc jump 1f;			\
+	dst = r3;			\
+     1: r3 = r2;			\
+	r1 = p1;			\
+	call _put_core_lock;		\
+	r0 = r3;			\
+	r4 = [sp++];			\
+	rets = [sp++];			\
+	rts;
+
+ENTRY(___raw_cmpxchg_1_asm)
+	__do_cmpxchg(b[p1] (z), b[p1])
+ENDPROC(___raw_cmpxchg_1_asm)
+
+ENTRY(___raw_cmpxchg_2_asm)
+	__do_cmpxchg(w[p1] (z), w[p1])
+ENDPROC(___raw_cmpxchg_2_asm)
+
+ENTRY(___raw_cmpxchg_4_asm)
+	__do_cmpxchg([p1], [p1])
+ENDPROC(___raw_cmpxchg_4_asm)
+
+/*
+ * r0 = ptr
+ * r1 = bitnr
+ *
+ * Set a bit in a 32bit word and return the old 32bit value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_bit_set_asm)
+	r2 = r1;
+	r1 = 1;
+	r1 <<= r2;
+	jump ___raw_atomic_set_asm
+ENDPROC(___raw_bit_set_asm)
+
+/*
+ * r0 = ptr
+ * r1 = bitnr
+ *
+ * Clear a bit in a 32bit word and return the old 32bit value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_bit_clear_asm)
+	r2 = r1;
+	r1 = 1;
+	r1 <<= r2;
+	jump ___raw_atomic_clear_asm
+ENDPROC(___raw_bit_clear_asm)
+
+/*
+ * r0 = ptr
+ * r1 = bitnr
+ *
+ * Toggle a bit in a 32bit word and return the old 32bit value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_bit_toggle_asm)
+	r2 = r1;
+	r1 = 1;
+	r1 <<= r2;
+	jump ___raw_atomic_xor_asm
+ENDPROC(___raw_bit_toggle_asm)
+
+/*
+ * r0 = ptr
+ * r1 = bitnr
+ *
+ * Test-and-set a bit in a 32bit word and return the old bit value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_bit_test_set_asm)
+	[--sp] = rets;
+	[--sp] = r1;
+	call ___raw_bit_set_asm
+	r1 = [sp++];
+	r2 = 1;
+	r2 <<= r1;
+	r0 = r0 & r2;
+	cc = r0 == 0;
+	if cc jump 1f
+	r0 = 1;
+1:
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_bit_test_set_asm)
+
+/*
+ * r0 = ptr
+ * r1 = bitnr
+ *
+ * Test-and-clear a bit in a 32bit word and return the old bit value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_bit_test_clear_asm)
+	[--sp] = rets;
+	[--sp] = r1;
+	call ___raw_bit_clear_asm
+	r1 = [sp++];
+	r2 = 1;
+	r2 <<= r1;
+	r0 = r0 & r2;
+	cc = r0 == 0;
+	if cc jump 1f
+	r0 = 1;
+1:
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_bit_test_clear_asm)
+
+/*
+ * r0 = ptr
+ * r1 = bitnr
+ *
+ * Test-and-toggle a bit in a 32bit word,
+ * and return the old bit value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_bit_test_toggle_asm)
+	[--sp] = rets;
+	[--sp] = r1;
+	call ___raw_bit_toggle_asm
+	r1 = [sp++];
+	r2 = 1;
+	r2 <<= r1;
+	r0 = r0 & r2;
+	cc = r0 == 0;
+	if cc jump 1f
+	r0 = 1;
+1:
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_bit_test_toggle_asm)
+
+/*
+ * r0 = ptr
+ * r1 = bitnr
+ *
+ * Test a bit in a 32bit word and return its value.
+ * We need this on this architecture in order to invalidate
+ * the local cache before testing.
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_bit_test_asm)
+	r2 = r1;
+	r1 = 1;
+	r1 <<= r2;
+	jump ___raw_atomic_test_asm
+ENDPROC(___raw_bit_test_asm)
+
+/*
+ * r0 = ptr
+ *
+ * Fetch and return an uncached 32bit value.
+ *
+ * Clobbers: r2:0, p1:0
+ */
+ENTRY(___raw_uncached_fetch_asm)
+	p1 = r0;
+	r1 = -L1_CACHE_BYTES;
+	r1 = r0 & r1;
+	p0 = r1;
+	flushinv[p0];
+	SSYNC(r2);
+	r0 = [p1];
+	rts;
+ENDPROC(___raw_uncached_fetch_asm)
diff --git a/arch/blackfin/mach-bf561/boards/cm_bf561.c b/arch/blackfin/mach-bf561/boards/cm_bf561.c
index 8f40990eea2f..6880d1ebfe60 100644
--- a/arch/blackfin/mach-bf561/boards/cm_bf561.c
+++ b/arch/blackfin/mach-bf561/boards/cm_bf561.c
@@ -230,6 +230,19 @@ static struct platform_device smc91x_device = {
 };
 #endif
 
+static struct resource bfin_gpios_resources = {
+	.start = 0,
+	.end   = MAX_BLACKFIN_GPIOS - 1,
+	.flags = IORESOURCE_IRQ,
+};
+
+static struct platform_device bfin_gpios_device = {
+	.name = "simple-gpio",
+	.id = -1,
+	.num_resources = 1,
+	.resource = &bfin_gpios_resources,
+};
+
 #if defined(CONFIG_USB_ISP1362_HCD) || defined(CONFIG_USB_ISP1362_HCD_MODULE)
 static struct resource isp1362_hcd_resources[] = {
 	{
@@ -287,23 +300,33 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir0_device = {
 	.name = "bfin_sir",
 	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_PATA_PLATFORM) || defined(CONFIG_PATA_PLATFORM_MODULE)
 #define PATA_INT	IRQ_PF46
@@ -382,7 +405,9 @@ static struct platform_device *cm_bf561_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
 #endif
 
 #if defined(CONFIG_USB_ISP1362_HCD) || defined(CONFIG_USB_ISP1362_HCD_MODULE)
@@ -400,6 +425,8 @@ static struct platform_device *cm_bf561_devices[] __initdata = {
 #if defined(CONFIG_PATA_PLATFORM) || defined(CONFIG_PATA_PLATFORM_MODULE)
 	&bfin_pata_device,
 #endif
+
+	&bfin_gpios_device,
 };
 
 static int __init cm_bf561_init(void)
diff --git a/arch/blackfin/mach-bf561/boards/ezkit.c b/arch/blackfin/mach-bf561/boards/ezkit.c
index 50b4cdceccfe..0e2178a1aec5 100644
--- a/arch/blackfin/mach-bf561/boards/ezkit.c
+++ b/arch/blackfin/mach-bf561/boards/ezkit.c
@@ -43,53 +43,42 @@
 /*
  * Name the Board for the /proc/cpuinfo
  */
-const char bfin_board_name[] = "ADDS-BF561-EZKIT";
-
-#define ISP1761_BASE       0x2C0F0000
-#define ISP1761_IRQ        IRQ_PF10
+const char bfin_board_name[] = "ADI BF561-EZKIT";
 
 #if defined(CONFIG_USB_ISP1760_HCD) || defined(CONFIG_USB_ISP1760_HCD_MODULE)
-static struct resource bfin_isp1761_resources[] = {
-	{
-		.name	= "isp1761-regs",
-		.start  = ISP1761_BASE + 0x00000000,
-		.end    = ISP1761_BASE + 0x000fffff,
+#include <linux/usb/isp1760.h>
+static struct resource bfin_isp1760_resources[] = {
+	[0] = {
+		.start  = 0x2C0F0000,
+		.end    = 0x203C0000 + 0xfffff,
 		.flags  = IORESOURCE_MEM,
 	},
-	{
-		.start  = ISP1761_IRQ,
-		.end    = ISP1761_IRQ,
+	[1] = {
+		.start  = IRQ_PF10,
+		.end    = IRQ_PF10,
 		.flags  = IORESOURCE_IRQ,
 	},
 };
 
-static struct platform_device bfin_isp1761_device = {
-	.name           = "isp1761",
-	.id             = 0,
-	.num_resources  = ARRAY_SIZE(bfin_isp1761_resources),
-	.resource       = bfin_isp1761_resources,
+static struct isp1760_platform_data isp1760_priv = {
+	.is_isp1761 = 0,
+	.port1_disable = 0,
+	.bus_width_16 = 1,
+	.port1_otg = 0,
+	.analog_oc = 0,
+	.dack_polarity_high = 0,
+	.dreq_polarity_high = 0,
 };
 
-static struct platform_device *bfin_isp1761_devices[] = {
-	&bfin_isp1761_device,
+static struct platform_device bfin_isp1760_device = {
+	.name           = "isp1760-hcd",
+	.id             = 0,
+	.dev = {
+		.platform_data = &isp1760_priv,
+	},
+	.num_resources  = ARRAY_SIZE(bfin_isp1760_resources),
+	.resource       = bfin_isp1760_resources,
 };
-
-int __init bfin_isp1761_init(void)
-{
-	unsigned int num_devices = ARRAY_SIZE(bfin_isp1761_devices);
-
-	printk(KERN_INFO "%s(): registering device resources\n", __func__);
-	set_irq_type(ISP1761_IRQ, IRQF_TRIGGER_FALLING);
-
-	return platform_add_devices(bfin_isp1761_devices, num_devices);
-}
-
-void __exit bfin_isp1761_exit(void)
-{
-	platform_device_unregister(&bfin_isp1761_device);
-}
-
-arch_initcall(bfin_isp1761_init);
 #endif
 
 #if defined(CONFIG_USB_ISP1362_HCD) || defined(CONFIG_USB_ISP1362_HCD_MODULE)
@@ -221,23 +210,33 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir0_device = {
 	.name = "bfin_sir",
 	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_MTD_PHYSMAP) || defined(CONFIG_MTD_PHYSMAP_MODULE)
 static struct mtd_partition ezkit_partitions[] = {
@@ -449,6 +448,10 @@ static struct platform_device *ezkit_devices[] __initdata = {
 	&net2272_bfin_device,
 #endif
 
+#if defined(CONFIG_USB_ISP1760_HCD) || defined(CONFIG_USB_ISP1760_HCD_MODULE)
+	&bfin_isp1760_device,
+#endif
+
 #if defined(CONFIG_SPI_BFIN) || defined(CONFIG_SPI_BFIN_MODULE)
 	&bfin_spi0_device,
 #endif
@@ -458,7 +461,9 @@ static struct platform_device *ezkit_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
 #endif
 
 #if defined(CONFIG_KEYBOARD_GPIO) || defined(CONFIG_KEYBOARD_GPIO_MODULE)
diff --git a/arch/blackfin/mach-bf561/boards/generic_board.c b/arch/blackfin/mach-bf561/boards/generic_board.c
index 2faa0072d614..0ba366a0e696 100644
--- a/arch/blackfin/mach-bf561/boards/generic_board.c
+++ b/arch/blackfin/mach-bf561/boards/generic_board.c
@@ -62,10 +62,45 @@ static struct platform_device smc91x_device = {
 };
 #endif
 
+#if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
+#ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
+	{
+		.start = 0xFFC00400,
+		.end = 0xFFC004FF,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
+#endif
+#endif
+
 static struct platform_device *generic_board_devices[] __initdata = {
 #if defined(CONFIG_SMC91X) || defined(CONFIG_SMC91X_MODULE)
 	&smc91x_device,
 #endif
+
+#if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#endif
 };
 
 static int __init generic_board_init(void)
diff --git a/arch/blackfin/mach-bf561/boards/tepla.c b/arch/blackfin/mach-bf561/boards/tepla.c
index c9174b39f98d..6f77dbe952f5 100644
--- a/arch/blackfin/mach-bf561/boards/tepla.c
+++ b/arch/blackfin/mach-bf561/boards/tepla.c
@@ -44,8 +44,42 @@ static struct platform_device smc91x_device = {
 	.resource      = smc91x_resources,
 };
 
+#if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
+#ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
+	{
+		.start = 0xFFC00400,
+		.end = 0xFFC004FF,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
+#endif
+#endif
+
 static struct platform_device *tepla_devices[] __initdata = {
 	&smc91x_device,
+#if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#endif
 };
 
 static int __init tepla_init(void)
diff --git a/arch/blackfin/mach-bf561/dma.c b/arch/blackfin/mach-bf561/dma.c
index 24415eb82698..42b0037afe61 100644
--- a/arch/blackfin/mach-bf561/dma.c
+++ b/arch/blackfin/mach-bf561/dma.c
@@ -31,7 +31,7 @@
 #include <asm/blackfin.h>
 #include <asm/dma.h>
 
-struct dma_register *dma_io_base_addr[MAX_BLACKFIN_DMA_CHANNEL] = {
+struct dma_register *dma_io_base_addr[MAX_DMA_CHANNELS] = {
 	(struct dma_register *) DMA1_0_NEXT_DESC_PTR,
 	(struct dma_register *) DMA1_1_NEXT_DESC_PTR,
 	(struct dma_register *) DMA1_2_NEXT_DESC_PTR,
diff --git a/arch/blackfin/mach-bf561/head.S b/arch/blackfin/mach-bf561/head.S
deleted file mode 100644
index 31a777a9e699..000000000000
--- a/arch/blackfin/mach-bf561/head.S
+++ /dev/null
@@ -1,136 +0,0 @@
-/*
- * File:         arch/blackfin/mach-bf561/head.S
- * Based on:     arch/blackfin/mach-bf533/head.S
- * Author:
- *
- * Created:
- * Description:  BF561 startup file
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#include <linux/linkage.h>
-#include <linux/init.h>
-#include <asm/blackfin.h>
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-#include <asm/clocks.h>
-#include <mach/mem_init.h>
-#endif
-
-.section .l1.text
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-ENTRY(_start_dma_code)
-	p0.h = hi(SICA_IWR0);
-	p0.l = lo(SICA_IWR0);
-	r0.l = 0x1;
-	[p0] = r0;
-	SSYNC;
-
-	/*
-	 *  Set PLL_CTL
-	 *   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
-	 *   - [8]     = BYPASS    : BYPASS the PLL, run CLKIN into CCLK/SCLK
-	 *   - [7]     = output delay (add 200ps of delay to mem signals)
-	 *   - [6]     = input delay (add 200ps of input delay to mem signals)
-	 *   - [5]     = PDWN      : 1=All Clocks off
-	 *   - [3]     = STOPCK    : 1=Core Clock off
-	 *   - [1]     = PLL_OFF   : 1=Disable Power to PLL
-	 *   - [0]     = DF        : 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
-	 *   all other bits set to zero
-	 */
-
-	p0.h = hi(PLL_LOCKCNT);
-	p0.l = lo(PLL_LOCKCNT);
-	r0 = 0x300(Z);
-	w[p0] = r0.l;
-	ssync;
-
-	P2.H = hi(EBIU_SDGCTL);
-	P2.L = lo(EBIU_SDGCTL);
-	R0 = [P2];
-	BITSET (R0, 24);
-	[P2] = R0;
-	SSYNC;
-
-	r0 = CONFIG_VCO_MULT & 63;       /* Load the VCO multiplier         */
-	r0 = r0 << 9;                    /* Shift it over,                  */
-	r1 = CLKIN_HALF;                 /* Do we need to divide CLKIN by 2?*/
-	r0 = r1 | r0;
-	r1 = PLL_BYPASS;                 /* Bypass the PLL?                 */
-	r1 = r1 << 8;                    /* Shift it over                   */
-	r0 = r1 | r0;                    /* add them all together           */
-#ifdef ANOMALY_05000265
-	BITSET(r0, 15);                  /* Add 250 mV of hysteresis to SPORT input pins */
-#endif
-
-	p0.h = hi(PLL_CTL);
-	p0.l = lo(PLL_CTL);              /* Load the address                */
-	cli r2;                          /* Disable interrupts              */
-	ssync;
-	w[p0] = r0.l;                    /* Set the value                   */
-	idle;                            /* Wait for the PLL to stablize    */
-	sti r2;                          /* Enable interrupts               */
-
-.Lcheck_again:
-	p0.h = hi(PLL_STAT);
-	p0.l = lo(PLL_STAT);
-	R0 = W[P0](Z);
-	CC = BITTST(R0,5);
-	if ! CC jump .Lcheck_again;
-
-	/* Configure SCLK & CCLK Dividers */
-	r0 = (CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
-	p0.h = hi(PLL_DIV);
-	p0.l = lo(PLL_DIV);
-	w[p0] = r0.l;
-	ssync;
-
-	p0.l = lo(EBIU_SDRRC);
-	p0.h = hi(EBIU_SDRRC);
-	r0 = mem_SDRRC;
-	w[p0] = r0.l;
-	ssync;
-
-	P2.H = hi(EBIU_SDGCTL);
-	P2.L = lo(EBIU_SDGCTL);
-	R0 = [P2];
-	BITCLR (R0, 24);
-	p0.h = hi(EBIU_SDSTAT);
-	p0.l = lo(EBIU_SDSTAT);
-	r2.l = w[p0];
-	cc = bittst(r2,3);
-	if !cc jump .Lskip;
-	NOP;
-	BITSET (R0, 23);
-.Lskip:
-	[P2] = R0;
-	SSYNC;
-
-	R0.L = lo(mem_SDGCTL);
-	R0.H = hi(mem_SDGCTL);
-	R1 = [p2];
-	R1 = R1 | R0;
-	[P2] = R1;
-	SSYNC;
-
-	RTS;
-ENDPROC(_start_dma_code)
-#endif /* CONFIG_BFIN_KERNEL_CLOCK */
diff --git a/arch/blackfin/mach-bf561/include/mach/anomaly.h b/arch/blackfin/mach-bf561/include/mach/anomaly.h
index 22990df04ae1..1a9e17562821 100644
--- a/arch/blackfin/mach-bf561/include/mach/anomaly.h
+++ b/arch/blackfin/mach-bf561/include/mach/anomaly.h
@@ -7,7 +7,7 @@
  */
 
 /* This file shoule be up to date with:
- *  - Revision P, 02/08/2008; ADSP-BF561 Blackfin Processor Anomaly List
+ *  - Revision Q, 11/07/2008; ADSP-BF561 Blackfin Processor Anomaly List
  */
 
 #ifndef _MACH_ANOMALY_H_
@@ -264,6 +264,18 @@
 #define ANOMALY_05000371 (1)
 /* Level-Sensitive External GPIO Wakeups May Cause Indefinite Stall */
 #define ANOMALY_05000403 (1)
+/* TESTSET Instruction Causes Data Corruption with Writeback Data Cache Enabled */
+#define ANOMALY_05000412 (1)
+/* Speculative Fetches Can Cause Undesired External FIFO Operations */
+#define ANOMALY_05000416 (1)
+/* Multichannel SPORT Channel Misalignment Under Specific Configuration */
+#define ANOMALY_05000425 (1)
+/* Speculative Fetches of Indirect-Pointer Instructions Can Cause False Hardware Errors */
+#define ANOMALY_05000426 (1)
+/* Lost/Corrupted L2/L3 Memory Write after Speculative L2 Memory Read by Core B */
+#define ANOMALY_05000428 (__SILICON_REVISION__ > 3)
+/* IFLUSH Instruction at End of Hardware Loop Causes Infinite Stall */
+#define ANOMALY_05000443 (1)
 
 /* Anomalies that don't exist on this proc */
 #define ANOMALY_05000158 (0)
@@ -272,5 +284,7 @@
 #define ANOMALY_05000311 (0)
 #define ANOMALY_05000353 (1)
 #define ANOMALY_05000386 (1)
+#define ANOMALY_05000432 (0)
+#define ANOMALY_05000435 (0)
 
 #endif
diff --git a/arch/blackfin/mach-bf561/include/mach/bf561.h b/arch/blackfin/mach-bf561/include/mach/bf561.h
index 18b1b3a223ab..9968362a2ee4 100644
--- a/arch/blackfin/mach-bf561/include/mach/bf561.h
+++ b/arch/blackfin/mach-bf561/include/mach/bf561.h
@@ -215,7 +215,7 @@
 #endif
 
 #ifndef CPU
-#error Unknown CPU type - This kernel doesn't seem to be configured properly
+#error "Unknown CPU type - This kernel doesn't seem to be configured properly"
 #endif
 
 #endif				/* __MACH_BF561_H__  */
diff --git a/arch/blackfin/mach-bf561/include/mach/bfin_sir.h b/arch/blackfin/mach-bf561/include/mach/bfin_sir.h
deleted file mode 100644
index 9bb87e9e2e9b..000000000000
--- a/arch/blackfin/mach-bf561/include/mach/bfin_sir.h
+++ /dev/null
@@ -1,125 +0,0 @@
-/*
- * Blackfin Infra-red Driver
- *
- * Copyright 2006-2008 Analog Devices Inc.
- *
- * Enter bugs at http://blackfin.uclinux.org/
- *
- * Licensed under the GPL-2 or later.
- *
- */
-
-#include <linux/serial.h>
-#include <asm/dma.h>
-#include <asm/portmux.h>
-
-#define SIR_UART_GET_CHAR(port)   bfin_read16((port)->membase + OFFSET_RBR)
-#define SIR_UART_GET_DLL(port)    bfin_read16((port)->membase + OFFSET_DLL)
-#define SIR_UART_GET_IER(port)    bfin_read16((port)->membase + OFFSET_IER)
-#define SIR_UART_GET_DLH(port)    bfin_read16((port)->membase + OFFSET_DLH)
-#define SIR_UART_GET_IIR(port)    bfin_read16((port)->membase + OFFSET_IIR)
-#define SIR_UART_GET_LCR(port)    bfin_read16((port)->membase + OFFSET_LCR)
-#define SIR_UART_GET_GCTL(port)   bfin_read16((port)->membase + OFFSET_GCTL)
-
-#define SIR_UART_PUT_CHAR(port, v) bfin_write16(((port)->membase + OFFSET_THR), v)
-#define SIR_UART_PUT_DLL(port, v)  bfin_write16(((port)->membase + OFFSET_DLL), v)
-#define SIR_UART_PUT_IER(port, v)  bfin_write16(((port)->membase + OFFSET_IER), v)
-#define SIR_UART_PUT_DLH(port, v)  bfin_write16(((port)->membase + OFFSET_DLH), v)
-#define SIR_UART_PUT_LCR(port, v)  bfin_write16(((port)->membase + OFFSET_LCR), v)
-#define SIR_UART_PUT_GCTL(port, v) bfin_write16(((port)->membase + OFFSET_GCTL), v)
-
-#ifdef CONFIG_SIR_BFIN_DMA
-struct dma_rx_buf {
-	char *buf;
-	int head;
-	int tail;
-	};
-#endif /* CONFIG_SIR_BFIN_DMA */
-
-struct bfin_sir_port {
-	unsigned char __iomem   *membase;
-	unsigned int            irq;
-	unsigned int            lsr;
-	unsigned long           clk;
-	struct net_device       *dev;
-#ifdef CONFIG_SIR_BFIN_DMA
-	int                     tx_done;
-	struct dma_rx_buf       rx_dma_buf;
-	struct timer_list       rx_dma_timer;
-	int                     rx_dma_nrows;
-#endif /* CONFIG_SIR_BFIN_DMA */
-	unsigned int            tx_dma_channel;
-	unsigned int            rx_dma_channel;
-};
-
-struct bfin_sir_port sir_ports[BFIN_UART_NR_PORTS];
-
-struct bfin_sir_port_res {
-	unsigned long   base_addr;
-	int             irq;
-	unsigned int    rx_dma_channel;
-	unsigned int    tx_dma_channel;
-};
-
-struct bfin_sir_port_res bfin_sir_port_resource[] = {
-#ifdef CONFIG_BFIN_SIR0
-	{
-	0xFFC00400,
-	IRQ_UART_RX,
-	CH_UART_RX,
-	CH_UART_TX,
-	},
-#endif
-};
-
-int nr_sirs = ARRAY_SIZE(bfin_sir_port_resource);
-
-struct bfin_sir_self {
-	struct bfin_sir_port    *sir_port;
-	spinlock_t              lock;
-	unsigned int            open;
-	int                     speed;
-	int                     newspeed;
-
-	struct sk_buff          *txskb;
-	struct sk_buff          *rxskb;
-	struct net_device_stats stats;
-	struct device           *dev;
-	struct irlap_cb         *irlap;
-	struct qos_info         qos;
-
-	iobuff_t                tx_buff;
-	iobuff_t                rx_buff;
-
-	struct work_struct      work;
-	int                     mtt;
-};
-
-static inline unsigned int SIR_UART_GET_LSR(struct bfin_sir_port *port)
-{
-	unsigned int lsr = bfin_read16(port->membase + OFFSET_LSR);
-	port->lsr |= (lsr & (BI|FE|PE|OE));
-	return lsr | port->lsr;
-}
-
-static inline void SIR_UART_CLEAR_LSR(struct bfin_sir_port *port)
-{
-	port->lsr = 0;
-	bfin_read16(port->membase + OFFSET_LSR);
-}
-
-#define DRIVER_NAME "bfin_sir"
-
-static int bfin_sir_hw_init(void)
-{
-	int ret = -ENODEV;
-#ifdef CONFIG_BFIN_SIR0
-	ret = peripheral_request(P_UART0_TX, DRIVER_NAME);
-	if (ret)
-		return ret;
-	ret = peripheral_request(P_UART0_RX, DRIVER_NAME);
-	if (ret)
-		return ret;
-#endif
-	return ret;
-}
diff --git a/arch/blackfin/mach-bf561/include/mach/blackfin.h b/arch/blackfin/mach-bf561/include/mach/blackfin.h
index 0ea8666e6764..f79f6626b7ec 100644
--- a/arch/blackfin/mach-bf561/include/mach/blackfin.h
+++ b/arch/blackfin/mach-bf561/include/mach/blackfin.h
@@ -66,8 +66,12 @@
 
 #define bfin_read_SIC_IMASK(x)		bfin_read32(SICA_IMASK0 + (x << 2))
 #define bfin_write_SIC_IMASK(x, val)	bfin_write32((SICA_IMASK0 + (x << 2)), val)
+#define bfin_read_SICB_IMASK(x)		bfin_read32(SICB_IMASK0 + (x << 2))
+#define bfin_write_SICB_IMASK(x, val)	bfin_write32((SICB_IMASK0 + (x << 2)), val)
 #define bfin_read_SIC_ISR(x)		bfin_read32(SICA_ISR0 + (x << 2))
 #define bfin_write_SIC_ISR(x, val)	bfin_write32((SICA_ISR0 + (x << 2)), val)
+#define bfin_read_SICB_ISR(x)		bfin_read32(SICB_ISR0 + (x << 2))
+#define bfin_write_SICB_ISR(x, val)	bfin_write32((SICB_ISR0 + (x << 2)), val)
 
 #define BFIN_UART_NR_PORTS      1
 
diff --git a/arch/blackfin/mach-bf561/include/mach/cdefBF561.h b/arch/blackfin/mach-bf561/include/mach/cdefBF561.h
index c14d63402e70..95d609f11c97 100644
--- a/arch/blackfin/mach-bf561/include/mach/cdefBF561.h
+++ b/arch/blackfin/mach-bf561/include/mach/cdefBF561.h
@@ -39,65 +39,15 @@
 /*include core specific register pointer definitions*/
 #include <asm/cdef_LPBlackfin.h>
 
-#include <asm/system.h>
-
 /*********************************************************************************** */
 /* System MMR Register Map */
 /*********************************************************************************** */
 
 /* Clock and System Control (0xFFC00000 - 0xFFC000FF) */
 #define bfin_read_PLL_CTL()                  bfin_read16(PLL_CTL)
-/* Writing to PLL_CTL initiates a PLL relock sequence. */
-static __inline__ void bfin_write_PLL_CTL(unsigned int val)
-{
-	unsigned long flags, iwr0, iwr1;
-
-	if (val == bfin_read_PLL_CTL())
-		return;
-
-	local_irq_save(flags);
-	/* Enable the PLL Wakeup bit in SIC IWR */
-	iwr0 = bfin_read32(SICA_IWR0);
-	iwr1 = bfin_read32(SICA_IWR1);
-	/* Only allow PPL Wakeup) */
-	bfin_write32(SICA_IWR0, IWR_ENABLE(0));
-	bfin_write32(SICA_IWR1, 0);
-
-	bfin_write16(PLL_CTL, val);
-	SSYNC();
-	asm("IDLE;");
-
-	bfin_write32(SICA_IWR0, iwr0);
-	bfin_write32(SICA_IWR1, iwr1);
-	local_irq_restore(flags);
-}
 #define bfin_read_PLL_DIV()                  bfin_read16(PLL_DIV)
 #define bfin_write_PLL_DIV(val)              bfin_write16(PLL_DIV,val)
 #define bfin_read_VR_CTL()                   bfin_read16(VR_CTL)
-/* Writing to VR_CTL initiates a PLL relock sequence. */
-static __inline__ void bfin_write_VR_CTL(unsigned int val)
-{
-	unsigned long flags, iwr0, iwr1;
-
-	if (val == bfin_read_VR_CTL())
-		return;
-
-	local_irq_save(flags);
-	/* Enable the PLL Wakeup bit in SIC IWR */
-	iwr0 = bfin_read32(SICA_IWR0);
-	iwr1 = bfin_read32(SICA_IWR1);
-	/* Only allow PPL Wakeup) */
-	bfin_write32(SICA_IWR0, IWR_ENABLE(0));
-	bfin_write32(SICA_IWR1, 0);
-
-	bfin_write16(VR_CTL, val);
-	SSYNC();
-	asm("IDLE;");
-
-	bfin_write32(SICA_IWR0, iwr0);
-	bfin_write32(SICA_IWR1, iwr1);
-	local_irq_restore(flags);
-}
 #define bfin_read_PLL_STAT()                 bfin_read16(PLL_STAT)
 #define bfin_write_PLL_STAT(val)             bfin_write16(PLL_STAT,val)
 #define bfin_read_PLL_LOCKCNT()              bfin_read16(PLL_LOCKCNT)
@@ -1576,4 +1526,57 @@ static __inline__ void bfin_write_VR_CTL(unsigned int val)
 #define bfin_read_MDMA_D0_START_ADDR()  bfin_read_MDMA1_D0_START_ADDR()
 #define bfin_write_MDMA_D0_START_ADDR(val) bfin_write_MDMA1_D0_START_ADDR(val)
 
+/* These need to be last due to the cdef/linux inter-dependencies */
+#include <asm/irq.h>
+
+/* Writing to PLL_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_PLL_CTL(unsigned int val)
+{
+	unsigned long flags, iwr0, iwr1;
+
+	if (val == bfin_read_PLL_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr0 = bfin_read32(SICA_IWR0);
+	iwr1 = bfin_read32(SICA_IWR1);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SICA_IWR0, IWR_ENABLE(0));
+	bfin_write32(SICA_IWR1, 0);
+
+	bfin_write16(PLL_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SICA_IWR0, iwr0);
+	bfin_write32(SICA_IWR1, iwr1);
+	local_irq_restore_hw(flags);
+}
+
+/* Writing to VR_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_VR_CTL(unsigned int val)
+{
+	unsigned long flags, iwr0, iwr1;
+
+	if (val == bfin_read_VR_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr0 = bfin_read32(SICA_IWR0);
+	iwr1 = bfin_read32(SICA_IWR1);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SICA_IWR0, IWR_ENABLE(0));
+	bfin_write32(SICA_IWR1, 0);
+
+	bfin_write16(VR_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SICA_IWR0, iwr0);
+	bfin_write32(SICA_IWR1, iwr1);
+	local_irq_restore_hw(flags);
+}
+
 #endif				/* _CDEF_BF561_H */
diff --git a/arch/blackfin/mach-bf561/include/mach/defBF561.h b/arch/blackfin/mach-bf561/include/mach/defBF561.h
index 4eca2026bb92..d7c509759659 100644
--- a/arch/blackfin/mach-bf561/include/mach/defBF561.h
+++ b/arch/blackfin/mach-bf561/include/mach/defBF561.h
@@ -912,6 +912,9 @@
 #define ACTIVE_PLLDISABLED	0x0004	/* Processor In Active Mode With PLL Disabled   */
 #define	PLL_LOCKED			0x0020	/* PLL_LOCKCNT Has Been Reached                                 */
 
+/* SICA_SYSCR Masks */
+#define COREB_SRAM_INIT		0x0020
+
 /* SWRST Mask */
 #define SYSTEM_RESET           0x0007	/* Initiates a system software reset */
 #define DOUBLE_FAULT_A         0x0008	/* Core A Double Fault Causes Reset */
diff --git a/arch/blackfin/mach-bf561/include/mach/dma.h b/arch/blackfin/mach-bf561/include/mach/dma.h
index 8bc46cd89a02..13647c71f1c7 100644
--- a/arch/blackfin/mach-bf561/include/mach/dma.h
+++ b/arch/blackfin/mach-bf561/include/mach/dma.h
@@ -1,13 +1,17 @@
-/*****************************************************************************
-*
-*        BF-533/2/1 Specific Declarations
-*
-****************************************************************************/
+/* mach/dma.h - arch-specific DMA defines
+ *
+ * Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
 
 #ifndef _MACH_DMA_H_
 #define _MACH_DMA_H_
 
-#define MAX_BLACKFIN_DMA_CHANNEL 36
+#define MAX_DMA_CHANNELS 36
+
+/* [#4267] IMDMA channels have no PERIPHERAL_MAP MMR */
+#define MAX_DMA_SUSPEND_CHANNELS 32
 
 #define CH_PPI0			0
 #define CH_PPI			(CH_PPI0)
diff --git a/arch/blackfin/mach-bf561/include/mach/gpio.h b/arch/blackfin/mach-bf561/include/mach/gpio.h
new file mode 100644
index 000000000000..7882f79e1ade
--- /dev/null
+++ b/arch/blackfin/mach-bf561/include/mach/gpio.h
@@ -0,0 +1,68 @@
+/*
+ * File: arch/blackfin/mach-bf561/include/mach/gpio.h
+ * Bugs: Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Copyright (C) 2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
+ */
+
+
+#ifndef _MACH_GPIO_H_
+#define _MACH_GPIO_H_
+
+#define MAX_BLACKFIN_GPIOS 48
+
+#define	GPIO_PF0	0
+#define	GPIO_PF1	1
+#define	GPIO_PF2	2
+#define	GPIO_PF3	3
+#define	GPIO_PF4	4
+#define	GPIO_PF5	5
+#define	GPIO_PF6	6
+#define	GPIO_PF7	7
+#define	GPIO_PF8	8
+#define	GPIO_PF9	9
+#define	GPIO_PF10	10
+#define	GPIO_PF11	11
+#define	GPIO_PF12	12
+#define	GPIO_PF13	13
+#define	GPIO_PF14	14
+#define	GPIO_PF15	15
+#define	GPIO_PF16	16
+#define	GPIO_PF17	17
+#define	GPIO_PF18	18
+#define	GPIO_PF19	19
+#define	GPIO_PF20	20
+#define	GPIO_PF21	21
+#define	GPIO_PF22	22
+#define	GPIO_PF23	23
+#define	GPIO_PF24	24
+#define	GPIO_PF25	25
+#define	GPIO_PF26	26
+#define	GPIO_PF27	27
+#define	GPIO_PF28	28
+#define	GPIO_PF29	29
+#define	GPIO_PF30	30
+#define	GPIO_PF31	31
+#define	GPIO_PF32	32
+#define	GPIO_PF33	33
+#define	GPIO_PF34	34
+#define	GPIO_PF35	35
+#define	GPIO_PF36	36
+#define	GPIO_PF37	37
+#define	GPIO_PF38	38
+#define	GPIO_PF39	39
+#define	GPIO_PF40	40
+#define	GPIO_PF41	41
+#define	GPIO_PF42	42
+#define	GPIO_PF43	43
+#define	GPIO_PF44	44
+#define	GPIO_PF45	45
+#define	GPIO_PF46	46
+#define	GPIO_PF47	47
+
+#define PORT_FIO0 GPIO_0
+#define PORT_FIO1 GPIO_16
+#define PORT_FIO2 GPIO_32
+
+#endif /* _MACH_GPIO_H_ */
diff --git a/arch/blackfin/mach-bf561/include/mach/mem_init.h b/arch/blackfin/mach-bf561/include/mach/mem_init.h
deleted file mode 100644
index e163260bca18..000000000000
--- a/arch/blackfin/mach-bf561/include/mach/mem_init.h
+++ /dev/null
@@ -1,295 +0,0 @@
-/*
- * File:         include/asm-blackfin/mach-bf561/mem_init.h
- * Based on:
- * Author:
- *
- * Created:
- * Description:
- *
- * Rev:
- *
- * Modified:
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, 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; see the file COPYING.
- * If not, write to the Free Software Foundation,
- * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-#if (CONFIG_MEM_MT48LC16M16A2TG_75 || CONFIG_MEM_MT48LC64M4A2FB_7E || CONFIG_MEM_GENERIC_BOARD || CONFIG_MEM_MT48LC8M32B2B5_7)
-#if (CONFIG_SCLK_HZ > 119402985)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_7
-#define SDRAM_tRAS_num  7
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 104477612) && (CONFIG_SCLK_HZ <= 119402985)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_6
-#define SDRAM_tRAS_num  6
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 89552239) && (CONFIG_SCLK_HZ <= 104477612)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_5
-#define SDRAM_tRAS_num  5
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 74626866) && (CONFIG_SCLK_HZ <= 89552239)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_4
-#define SDRAM_tRAS_num  4
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 66666667) && (CONFIG_SCLK_HZ <= 74626866)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_3
-#define SDRAM_tRAS_num  3
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 59701493) && (CONFIG_SCLK_HZ <= 66666667)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_4
-#define SDRAM_tRAS_num  3
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 44776119) && (CONFIG_SCLK_HZ <= 59701493)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_3
-#define SDRAM_tRAS_num  3
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 29850746) && (CONFIG_SCLK_HZ <= 44776119)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_2
-#define SDRAM_tRAS_num  2
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ <= 29850746)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_1
-#define SDRAM_tRAS_num  1
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#endif
-
-#if (CONFIG_MEM_MT48LC16M16A2TG_75)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-#if (CONFIG_MEM_MT48LC64M4A2FB_7E)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-#if (CONFIG_MEM_MT48LC8M32B2B5_7)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   4096	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-#if (CONFIG_MEM_GENERIC_BOARD)
-  /*SDRAM INFORMATION: Modify this for your board */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-/* Equation from section 17 (p17-46) of BF533 HRM */
-#define mem_SDRRC       (((CONFIG_SCLK_HZ / 1000) * SDRAM_Tref) / SDRAM_NRA) - (SDRAM_tRAS_num + SDRAM_tRP_num)
-
-/* Enable SCLK Out */
-#define mem_SDGCTL        (SCTLE | SDRAM_CL | SDRAM_tRAS | SDRAM_tRP | SDRAM_tRCD | SDRAM_tWR | PSS)
-
-#if defined CONFIG_CLKIN_HALF
-#define CLKIN_HALF       1
-#else
-#define CLKIN_HALF       0
-#endif
-
-#if defined CONFIG_PLL_BYPASS
-#define PLL_BYPASS      1
-#else
-#define PLL_BYPASS       0
-#endif
-
-/***************************************Currently Not Being Used *********************************/
-#define flash_EBIU_AMBCTL_WAT  ((CONFIG_FLASH_SPEED_BWAT * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-#define flash_EBIU_AMBCTL_RAT  ((CONFIG_FLASH_SPEED_BRAT * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-#define flash_EBIU_AMBCTL_HT   ((CONFIG_FLASH_SPEED_BHT  * 4) / (4000000000 / CONFIG_SCLK_HZ))
-#define flash_EBIU_AMBCTL_ST   ((CONFIG_FLASH_SPEED_BST  * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-#define flash_EBIU_AMBCTL_TT   ((CONFIG_FLASH_SPEED_BTT  * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-
-#if (flash_EBIU_AMBCTL_TT > 3)
-#define flash_EBIU_AMBCTL0_TT   B0TT_4
-#endif
-#if (flash_EBIU_AMBCTL_TT == 3)
-#define flash_EBIU_AMBCTL0_TT   B0TT_3
-#endif
-#if (flash_EBIU_AMBCTL_TT == 2)
-#define flash_EBIU_AMBCTL0_TT   B0TT_2
-#endif
-#if (flash_EBIU_AMBCTL_TT < 2)
-#define flash_EBIU_AMBCTL0_TT   B0TT_1
-#endif
-
-#if (flash_EBIU_AMBCTL_ST > 3)
-#define flash_EBIU_AMBCTL0_ST   B0ST_4
-#endif
-#if (flash_EBIU_AMBCTL_ST == 3)
-#define flash_EBIU_AMBCTL0_ST   B0ST_3
-#endif
-#if (flash_EBIU_AMBCTL_ST == 2)
-#define flash_EBIU_AMBCTL0_ST   B0ST_2
-#endif
-#if (flash_EBIU_AMBCTL_ST < 2)
-#define flash_EBIU_AMBCTL0_ST   B0ST_1
-#endif
-
-#if (flash_EBIU_AMBCTL_HT > 2)
-#define flash_EBIU_AMBCTL0_HT   B0HT_3
-#endif
-#if (flash_EBIU_AMBCTL_HT == 2)
-#define flash_EBIU_AMBCTL0_HT   B0HT_2
-#endif
-#if (flash_EBIU_AMBCTL_HT == 1)
-#define flash_EBIU_AMBCTL0_HT   B0HT_1
-#endif
-#if (flash_EBIU_AMBCTL_HT == 0 && CONFIG_FLASH_SPEED_BHT == 0)
-#define flash_EBIU_AMBCTL0_HT   B0HT_0
-#endif
-#if (flash_EBIU_AMBCTL_HT == 0 && CONFIG_FLASH_SPEED_BHT != 0)
-#define flash_EBIU_AMBCTL0_HT   B0HT_1
-#endif
-
-#if (flash_EBIU_AMBCTL_WAT > 14)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_15
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 14)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_14
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 13)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_13
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 12)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_12
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 11)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_11
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 10)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_10
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 9)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_9
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 8)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_8
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 7)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_7
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 6)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_6
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 5)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_5
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 4)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_4
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 3)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_3
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 2)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_2
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 1)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_1
-#endif
-
-#if (flash_EBIU_AMBCTL_RAT > 14)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_15
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 14)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_14
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 13)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_13
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 12)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_12
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 11)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_11
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 10)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_10
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 9)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_9
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 8)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_8
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 7)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_7
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 6)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_6
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 5)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_5
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 4)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_4
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 3)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_3
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 2)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_2
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 1)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_1
-#endif
-
-#define flash_EBIU_AMBCTL0  \
-	(flash_EBIU_AMBCTL0_WAT | flash_EBIU_AMBCTL0_RAT | flash_EBIU_AMBCTL0_HT | \
-	 flash_EBIU_AMBCTL0_ST | flash_EBIU_AMBCTL0_TT | CONFIG_FLASH_SPEED_RDYEN)
diff --git a/arch/blackfin/mach-bf561/include/mach/mem_map.h b/arch/blackfin/mach-bf561/include/mach/mem_map.h
index f1d4c0637bd2..419dffdc96eb 100644
--- a/arch/blackfin/mach-bf561/include/mach/mem_map.h
+++ b/arch/blackfin/mach-bf561/include/mach/mem_map.h
@@ -85,4 +85,84 @@
 #define L1_SCRATCH_START	COREA_L1_SCRATCH_START
 #define L1_SCRATCH_LENGTH	0x1000
 
+#ifdef __ASSEMBLY__
+
+/*
+ * The following macros both return the address of the PDA for the
+ * current core.
+ *
+ * In its first safe (and hairy) form, the macro neither clobbers any
+ * register aside of the output Preg, nor uses the stack, since it
+ * could be called with an invalid stack pointer, or the current stack
+ * space being uncovered by any CPLB (e.g. early exception handling).
+ *
+ * The constraints on the second form are a bit relaxed, and the code
+ * is allowed to use the specified Dreg for determining the PDA
+ * address to be returned into Preg.
+ */
+#ifdef CONFIG_SMP
+#define GET_PDA_SAFE(preg)		\
+	preg.l = lo(DSPID);		\
+	preg.h = hi(DSPID);		\
+	preg = [preg];			\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	if cc jump 2f;			\
+	cc = preg == 0x0;		\
+	preg.l = _cpu_pda;		\
+	preg.h = _cpu_pda;		\
+	if !cc jump 3f;			\
+1:					\
+	/* preg = 0x0; */		\
+	cc = !cc; /* restore cc to 0 */	\
+	jump 4f;			\
+2:					\
+	cc = preg == 0x0;		\
+	preg.l = _cpu_pda;		\
+	preg.h = _cpu_pda;		\
+	if cc jump 4f;			\
+	/* preg = 0x1000000; */		\
+	cc = !cc; /* restore cc to 1 */	\
+3:					\
+	preg = [preg];			\
+4:
+
+#define GET_PDA(preg, dreg)		\
+	preg.l = lo(DSPID);		\
+	preg.h = hi(DSPID);		\
+	dreg = [preg];			\
+	preg.l = _cpu_pda;		\
+	preg.h = _cpu_pda;		\
+	cc = bittst(dreg, 0);		\
+	if !cc jump 1f;			\
+	preg = [preg];			\
+1:					\
+
+#define GET_CPUID(preg, dreg)		\
+	preg.l = lo(DSPID);		\
+	preg.h = hi(DSPID);		\
+	dreg = [preg];			\
+	dreg = ROT dreg BY -1;		\
+	dreg = CC;
+
+#else
+#define GET_PDA_SAFE(preg)		\
+	preg.l = _cpu_pda;		\
+	preg.h = _cpu_pda;
+
+#define GET_PDA(preg, dreg)	GET_PDA_SAFE(preg)
+#endif /* CONFIG_SMP */
+
+#endif /* __ASSEMBLY__ */
+
 #endif				/* _MEM_MAP_533_H_ */
diff --git a/arch/blackfin/mach-bf561/include/mach/smp.h b/arch/blackfin/mach-bf561/include/mach/smp.h
new file mode 100644
index 000000000000..f9e65ebe81b2
--- /dev/null
+++ b/arch/blackfin/mach-bf561/include/mach/smp.h
@@ -0,0 +1,22 @@
+#ifndef _MACH_BF561_SMP
+#define _MACH_BF561_SMP
+
+struct task_struct;
+
+void platform_init_cpus(void);
+
+void platform_prepare_cpus(unsigned int max_cpus);
+
+int platform_boot_secondary(unsigned int cpu, struct task_struct *idle);
+
+void platform_secondary_init(unsigned int cpu);
+
+void platform_request_ipi(int (*handler)(int, void *));
+
+void platform_send_ipi(cpumask_t callmap);
+
+void platform_send_ipi_cpu(unsigned int cpu);
+
+void platform_clear_ipi(unsigned int cpu);
+
+#endif /* !_MACH_BF561_SMP */
diff --git a/arch/blackfin/mach-bf561/secondary.S b/arch/blackfin/mach-bf561/secondary.S
new file mode 100644
index 000000000000..35280f06b7b6
--- /dev/null
+++ b/arch/blackfin/mach-bf561/secondary.S
@@ -0,0 +1,215 @@
+/*
+ * File:         arch/blackfin/mach-bf561/secondary.S
+ * Based on:     arch/blackfin/mach-bf561/head.S
+ * Author:       Philippe Gerum <rpm@xenomai.org>
+ *
+ *               Copyright 2007 Analog Devices Inc.
+ *
+ * Description:  BF561 coreB bootstrap file
+ *
+ * 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 the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#include <linux/linkage.h>
+#include <linux/init.h>
+#include <asm/blackfin.h>
+#include <asm/asm-offsets.h>
+
+__INIT
+
+/* Lay the initial stack into the L1 scratch area of Core B */
+#define INITIAL_STACK	(COREB_L1_SCRATCH_START + L1_SCRATCH_LENGTH - 12)
+
+ENTRY(_coreb_trampoline_start)
+	/* Set the SYSCFG register */
+	R0 = 0x36;
+	SYSCFG = R0; /*Enable Cycle Counter and Nesting Of Interrupts(3rd Bit)*/
+	R0 = 0;
+
+	/*Clear Out All the data and pointer  Registers*/
+	R1 = R0;
+	R2 = R0;
+	R3 = R0;
+	R4 = R0;
+	R5 = R0;
+	R6 = R0;
+	R7 = R0;
+
+	P0 = R0;
+	P1 = R0;
+	P2 = R0;
+	P3 = R0;
+	P4 = R0;
+	P5 = R0;
+
+	LC0 = r0;
+	LC1 = r0;
+	L0 = r0;
+	L1 = r0;
+	L2 = r0;
+	L3 = r0;
+
+	/* Clear Out All the DAG Registers*/
+	B0 = r0;
+	B1 = r0;
+	B2 = r0;
+	B3 = r0;
+
+	I0 = r0;
+	I1 = r0;
+	I2 = r0;
+	I3 = r0;
+
+	M0 = r0;
+	M1 = r0;
+	M2 = r0;
+	M3 = r0;
+
+	/* Turn off the icache */
+	p0.l = LO(IMEM_CONTROL);
+	p0.h = HI(IMEM_CONTROL);
+	R1 = [p0];
+	R0 = ~ENICPLB;
+	R0 = R0 & R1;
+
+	/* Anomaly 05000125 */
+#ifdef ANOMALY_05000125
+	CLI R2;
+	SSYNC;
+#endif
+	[p0] = R0;
+	SSYNC;
+#ifdef ANOMALY_05000125
+	STI R2;
+#endif
+
+	/* Turn off the dcache */
+	p0.l = LO(DMEM_CONTROL);
+	p0.h = HI(DMEM_CONTROL);
+	R1 = [p0];
+	R0 = ~ENDCPLB;
+	R0 = R0 & R1;
+
+	/* Anomaly 05000125 */
+#ifdef ANOMALY_05000125
+	CLI R2;
+	SSYNC;
+#endif
+	[p0] = R0;
+	SSYNC;
+#ifdef ANOMALY_05000125
+	STI R2;
+#endif
+
+	/* in case of double faults, save a few things */
+	p0.l = _init_retx_coreb;
+	p0.h = _init_retx_coreb;
+	R0 = RETX;
+	[P0] = R0;
+
+#ifdef CONFIG_DEBUG_DOUBLEFAULT
+	/* Only save these if we are storing them,
+	 * This happens here, since L1 gets clobbered
+	 * below
+	 */
+	GET_PDA(p0, r0);
+	r7 = [p0 + PDA_RETX];
+	p1.l = _init_saved_retx_coreb;
+	p1.h = _init_saved_retx_coreb;
+	[p1] = r7;
+
+	r7 = [p0 + PDA_DCPLB];
+	p1.l = _init_saved_dcplb_fault_addr_coreb;
+	p1.h = _init_saved_dcplb_fault_addr_coreb;
+	[p1] = r7;
+
+	r7 = [p0 + PDA_ICPLB];
+	p1.l = _init_saved_icplb_fault_addr_coreb;
+	p1.h = _init_saved_icplb_fault_addr_coreb;
+	[p1] = r7;
+
+	r7 = [p0 + PDA_SEQSTAT];
+	p1.l = _init_saved_seqstat_coreb;
+	p1.h = _init_saved_seqstat_coreb;
+	[p1] = r7;
+#endif
+
+	/* Initialize stack pointer */
+	sp.l = lo(INITIAL_STACK);
+	sp.h = hi(INITIAL_STACK);
+	fp = sp;
+	usp = sp;
+
+	/* This section keeps the processor in supervisor mode
+	 * during core B startup.  Branches to the idle task.
+	 */
+
+	/* EVT15 = _real_start */
+
+	p0.l = lo(EVT15);
+	p0.h = hi(EVT15);
+	p1.l = _coreb_start;
+	p1.h = _coreb_start;
+	[p0] = p1;
+	csync;
+
+	p0.l = lo(IMASK);
+	p0.h = hi(IMASK);
+	p1.l = IMASK_IVG15;
+	p1.h = 0x0;
+	[p0] = p1;
+	csync;
+
+	raise 15;
+	p0.l = .LWAIT_HERE;
+	p0.h = .LWAIT_HERE;
+	reti = p0;
+#if defined(ANOMALY_05000281)
+	nop; nop; nop;
+#endif
+	rti;
+
+.LWAIT_HERE:
+	jump .LWAIT_HERE;
+ENDPROC(_coreb_trampoline_start)
+ENTRY(_coreb_trampoline_end)
+
+ENTRY(_coreb_start)
+	[--sp] = reti;
+
+	p0.l = lo(WDOGB_CTL);
+	p0.h = hi(WDOGB_CTL);
+	r0 = 0xAD6(z);
+	w[p0] = r0;	/* Clear the watchdog. */
+	ssync;
+
+	/*
+	 * switch to IDLE stack.
+	 */
+	p0.l = _secondary_stack;
+	p0.h = _secondary_stack;
+	sp = [p0];
+	usp = sp;
+	fp = sp;
+	sp += -12;
+	call _init_pda
+	sp += 12;
+	call _secondary_start_kernel;
+.L_exit:
+	jump.s	.L_exit;
+ENDPROC(_coreb_start)
+
+__FINIT
diff --git a/arch/blackfin/mach-bf561/smp.c b/arch/blackfin/mach-bf561/smp.c
new file mode 100644
index 000000000000..9b27e698c0b2
--- /dev/null
+++ b/arch/blackfin/mach-bf561/smp.c
@@ -0,0 +1,167 @@
+/*
+ * File:         arch/blackfin/mach-bf561/smp.c
+ * Author:       Philippe Gerum <rpm@xenomai.org>
+ *
+ *               Copyright 2007 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 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 the file COPYING, or 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/sched.h>
+#include <linux/delay.h>
+#include <asm/smp.h>
+#include <asm/dma.h>
+
+static DEFINE_SPINLOCK(boot_lock);
+
+static cpumask_t cpu_callin_map;
+
+/*
+ * platform_init_cpus() - Tell the world about how many cores we
+ * have. This is called while setting up the architecture support
+ * (setup_arch()), so don't be too demanding here with respect to
+ * available kernel services.
+ */
+
+void __init platform_init_cpus(void)
+{
+	cpu_set(0, cpu_possible_map); /* CoreA */
+	cpu_set(1, cpu_possible_map); /* CoreB */
+}
+
+void __init platform_prepare_cpus(unsigned int max_cpus)
+{
+	int len;
+
+	len = &coreb_trampoline_end - &coreb_trampoline_start + 1;
+	BUG_ON(len > L1_CODE_LENGTH);
+
+	dma_memcpy((void *)COREB_L1_CODE_START, &coreb_trampoline_start, len);
+
+	/* Both cores ought to be present on a bf561! */
+	cpu_set(0, cpu_present_map); /* CoreA */
+	cpu_set(1, cpu_present_map); /* CoreB */
+
+	printk(KERN_INFO "CoreB bootstrap code to SRAM %p via DMA.\n", (void *)COREB_L1_CODE_START);
+}
+
+int __init setup_profiling_timer(unsigned int multiplier) /* not supported */
+{
+	return -EINVAL;
+}
+
+void __cpuinit platform_secondary_init(unsigned int cpu)
+{
+	local_irq_disable();
+
+	/* Clone setup for peripheral interrupt sources from CoreA. */
+	bfin_write_SICB_IMASK0(bfin_read_SICA_IMASK0());
+	bfin_write_SICB_IMASK1(bfin_read_SICA_IMASK1());
+	SSYNC();
+
+	/* Clone setup for IARs from CoreA. */
+	bfin_write_SICB_IAR0(bfin_read_SICA_IAR0());
+	bfin_write_SICB_IAR1(bfin_read_SICA_IAR1());
+	bfin_write_SICB_IAR2(bfin_read_SICA_IAR2());
+	bfin_write_SICB_IAR3(bfin_read_SICA_IAR3());
+	bfin_write_SICB_IAR4(bfin_read_SICA_IAR4());
+	bfin_write_SICB_IAR5(bfin_read_SICA_IAR5());
+	bfin_write_SICB_IAR6(bfin_read_SICA_IAR6());
+	bfin_write_SICB_IAR7(bfin_read_SICA_IAR7());
+	SSYNC();
+
+	local_irq_enable();
+
+	/* Calibrate loops per jiffy value. */
+	calibrate_delay();
+
+	/* Store CPU-private information to the cpu_data array. */
+	bfin_setup_cpudata(cpu);
+
+	/* We are done with local CPU inits, unblock the boot CPU. */
+	cpu_set(cpu, cpu_callin_map);
+	spin_lock(&boot_lock);
+	spin_unlock(&boot_lock);
+}
+
+int __cpuinit platform_boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	unsigned long timeout;
+
+	/* CoreB already running?! */
+	BUG_ON((bfin_read_SICA_SYSCR() & COREB_SRAM_INIT) == 0);
+
+	printk(KERN_INFO "Booting Core B.\n");
+
+	spin_lock(&boot_lock);
+
+	/* Kick CoreB, which should start execution from CORE_SRAM_BASE. */
+	SSYNC();
+	bfin_write_SICA_SYSCR(bfin_read_SICA_SYSCR() & ~COREB_SRAM_INIT);
+	SSYNC();
+
+	timeout = jiffies + 1 * HZ;
+	while (time_before(jiffies, timeout)) {
+		if (cpu_isset(cpu, cpu_callin_map))
+			break;
+		udelay(100);
+		barrier();
+	}
+
+	spin_unlock(&boot_lock);
+
+	return cpu_isset(cpu, cpu_callin_map) ? 0 : -ENOSYS;
+}
+
+void __init platform_request_ipi(irq_handler_t handler)
+{
+	int ret;
+
+	ret = request_irq(IRQ_SUPPLE_0, handler, IRQF_DISABLED,
+			  "SMP interrupt", handler);
+	if (ret)
+		panic("Cannot request supplemental interrupt 0 for IPI service\n");
+}
+
+void platform_send_ipi(cpumask_t callmap)
+{
+	unsigned int cpu;
+
+	for_each_cpu_mask(cpu, callmap) {
+		BUG_ON(cpu >= 2);
+		SSYNC();
+		bfin_write_SICB_SYSCR(bfin_read_SICB_SYSCR() | (1 << (6 + cpu)));
+		SSYNC();
+	}
+}
+
+void platform_send_ipi_cpu(unsigned int cpu)
+{
+	BUG_ON(cpu >= 2);
+	SSYNC();
+	bfin_write_SICB_SYSCR(bfin_read_SICB_SYSCR() | (1 << (6 + cpu)));
+	SSYNC();
+}
+
+void platform_clear_ipi(unsigned int cpu)
+{
+	BUG_ON(cpu >= 2);
+	SSYNC();
+	bfin_write_SICB_SYSCR(bfin_read_SICB_SYSCR() | (1 << (10 + cpu)));
+	SSYNC();
+}
diff --git a/arch/blackfin/mach-common/Makefile b/arch/blackfin/mach-common/Makefile
index e6ed57c56d4b..1f3228ed713f 100644
--- a/arch/blackfin/mach-common/Makefile
+++ b/arch/blackfin/mach-common/Makefile
@@ -3,10 +3,12 @@
 #
 
 obj-y := \
-	cache.o entry.o head.o \
+	cache.o cache-c.o entry.o head.o \
 	interrupt.o irqpanic.o arch_checks.o ints-priority.o
 
 obj-$(CONFIG_BFIN_ICACHE_LOCK) += lock.o
 obj-$(CONFIG_PM)          += pm.o dpmc_modes.o
 obj-$(CONFIG_CPU_FREQ)    += cpufreq.o
 obj-$(CONFIG_CPU_VOLTAGE) += dpmc.o
+obj-$(CONFIG_SMP)         += smp.o
+obj-$(CONFIG_BFIN_KERNEL_CLOCK) += clocks-init.o
diff --git a/arch/blackfin/mach-common/cache-c.c b/arch/blackfin/mach-common/cache-c.c
new file mode 100644
index 000000000000..e6ab1f815123
--- /dev/null
+++ b/arch/blackfin/mach-common/cache-c.c
@@ -0,0 +1,24 @@
+/*
+ * Blackfin cache control code (simpler control-style functions)
+ *
+ * Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <asm/blackfin.h>
+
+/* Invalidate the Entire Data cache by
+ * clearing DMC[1:0] bits
+ */
+void blackfin_invalidate_entire_dcache(void)
+{
+	u32 dmem = bfin_read_DMEM_CONTROL();
+	SSYNC();
+	bfin_write_DMEM_CONTROL(dmem & ~0xc);
+	SSYNC();
+	bfin_write_DMEM_CONTROL(dmem);
+	SSYNC();
+}
diff --git a/arch/blackfin/mach-common/cache.S b/arch/blackfin/mach-common/cache.S
index a028e9450419..3c98dacbf289 100644
--- a/arch/blackfin/mach-common/cache.S
+++ b/arch/blackfin/mach-common/cache.S
@@ -49,13 +49,17 @@
 .ifnb \optflushins
 	\optflushins [P0];
 .endif
+#if ANOMALY_05000443
 .ifb \optnopins
 2:
 .endif
 	\flushins [P0++];
 .ifnb \optnopins
-2: \optnopins;
+2:	\optnopins;
 .endif
+#else
+2:	\flushins [P0++];
+#endif
 
 	RTS;
 .endm
diff --git a/arch/blackfin/mach-common/clocks-init.c b/arch/blackfin/mach-common/clocks-init.c
new file mode 100644
index 000000000000..5d182abefc7b
--- /dev/null
+++ b/arch/blackfin/mach-common/clocks-init.c
@@ -0,0 +1,93 @@
+/*
+ * arch/blackfin/mach-common/clocks-init.c - reprogram clocks / memory
+ *
+ * Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/linkage.h>
+#include <linux/init.h>
+#include <asm/blackfin.h>
+
+#include <asm/dma.h>
+#include <asm/clocks.h>
+#include <asm/mem_init.h>
+
+#define PLL_CTL_VAL \
+	(((CONFIG_VCO_MULT & 63) << 9) | CLKIN_HALF | \
+	 (PLL_BYPASS << 8) | (ANOMALY_05000265 ? 0x8000 : 0))
+
+__attribute__((l1_text))
+static void do_sync(void)
+{
+	__builtin_bfin_ssync();
+}
+
+__attribute__((l1_text))
+void init_clocks(void)
+{
+	/* Kill any active DMAs as they may trigger external memory accesses
+	 * in the middle of reprogramming things, and that'll screw us up.
+	 * For example, any automatic DMAs left by U-Boot for splash screens.
+	 */
+	size_t i;
+	for (i = 0; i < MAX_DMA_CHANNELS; ++i) {
+		struct dma_register *dma = dma_io_base_addr[i];
+		dma->cfg = 0;
+	}
+
+	do_sync();
+
+#ifdef SIC_IWR0
+	bfin_write_SIC_IWR0(IWR_ENABLE(0));
+# ifdef SIC_IWR1
+	/* BF52x system reset does not properly reset SIC_IWR1 which
+	 * will screw up the bootrom as it relies on MDMA0/1 waking it
+	 * up from IDLE instructions.  See this report for more info:
+	 * http://blackfin.uclinux.org/gf/tracker/4323
+	 */
+	if (ANOMALY_05000435)
+		bfin_write_SIC_IWR1(IWR_ENABLE(10) | IWR_ENABLE(11));
+	else
+		bfin_write_SIC_IWR1(IWR_DISABLE_ALL);
+# endif
+# ifdef SIC_IWR2
+	bfin_write_SIC_IWR2(IWR_DISABLE_ALL);
+# endif
+#else
+	bfin_write_SIC_IWR(IWR_ENABLE(0));
+#endif
+	do_sync();
+#ifdef EBIU_SDGCTL
+	bfin_write_EBIU_SDGCTL(bfin_read_EBIU_SDGCTL() | SRFS);
+	do_sync();
+#endif
+
+#ifdef CLKBUFOE
+	bfin_write16(VR_CTL, bfin_read_VR_CTL() | CLKBUFOE);
+	do_sync();
+	__asm__ __volatile__("IDLE;");
+#endif
+	bfin_write_PLL_LOCKCNT(0x300);
+	do_sync();
+	bfin_write16(PLL_CTL, PLL_CTL_VAL);
+	__asm__ __volatile__("IDLE;");
+	bfin_write_PLL_DIV(CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
+#ifdef EBIU_SDGCTL
+	bfin_write_EBIU_SDRRC(mem_SDRRC);
+	bfin_write_EBIU_SDGCTL(mem_SDGCTL);
+#else
+	bfin_write_EBIU_RSTCTL(bfin_read_EBIU_RSTCTL() & ~(SRREQ));
+	do_sync();
+	bfin_write_EBIU_RSTCTL(bfin_read_EBIU_RSTCTL() | 0x1);
+	bfin_write_EBIU_DDRCTL0(mem_DDRCTL0);
+	bfin_write_EBIU_DDRCTL1(mem_DDRCTL1);
+	bfin_write_EBIU_DDRCTL2(mem_DDRCTL2);
+#ifdef CONFIG_MEM_EBIU_DDRQUE
+	bfin_write_EBIU_DDRQUE(CONFIG_MEM_EBIU_DDRQUE);
+#endif
+#endif
+	do_sync();
+	bfin_read16(0);
+}
diff --git a/arch/blackfin/mach-common/cpufreq.c b/arch/blackfin/mach-common/cpufreq.c
index dda5443b37ed..72e16605ca09 100644
--- a/arch/blackfin/mach-common/cpufreq.c
+++ b/arch/blackfin/mach-common/cpufreq.c
@@ -104,7 +104,7 @@ static int bfin_target(struct cpufreq_policy *policy,
 		 cclk_hz, target_freq, freqs.old);
 
 	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 		plldiv = (bfin_read_PLL_DIV() & SSEL) | dpm_state_table[index].csel;
 		tscale = dpm_state_table[index].tscale;
 		bfin_write_PLL_DIV(plldiv);
@@ -112,10 +112,10 @@ static int bfin_target(struct cpufreq_policy *policy,
 		bfin_write_TSCALE(tscale);
 		cycles = get_cycles();
 		SSYNC();
-	cycles += 10; /* ~10 cycles we loose after get_cycles() */
+	cycles += 10; /* ~10 cycles we lose after get_cycles() */
 	__bfin_cycles_off += (cycles << __bfin_cycles_mod) - (cycles << index);
 	__bfin_cycles_mod = index;
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 	/* TODO: just test case for cycles clock source, remove later */
 	pr_debug("cpufreq: done\n");
 	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
diff --git a/arch/blackfin/mach-common/dpmc_modes.S b/arch/blackfin/mach-common/dpmc_modes.S
index ad5431e2cd05..4da50bcd9300 100644
--- a/arch/blackfin/mach-common/dpmc_modes.S
+++ b/arch/blackfin/mach-common/dpmc_modes.S
@@ -247,7 +247,8 @@ ENTRY(_unset_dram_srfs)
 ENDPROC(_unset_dram_srfs)
 
 ENTRY(_set_sic_iwr)
-#if defined(CONFIG_BF54x) || defined(CONFIG_BF52x)  || defined(CONFIG_BF561)
+#if defined(CONFIG_BF54x) || defined(CONFIG_BF52x) || defined(CONFIG_BF561) || \
+	defined(CONFIG_BF538) || defined(CONFIG_BF539) || defined(CONFIG_BF51x)
 	P0.H = hi(SIC_IWR0);
 	P0.L = lo(SIC_IWR0);
 	P1.H = hi(SIC_IWR1);
diff --git a/arch/blackfin/mach-common/entry.S b/arch/blackfin/mach-common/entry.S
index bde6dc4e2614..fae774651374 100644
--- a/arch/blackfin/mach-common/entry.S
+++ b/arch/blackfin/mach-common/entry.S
@@ -36,6 +36,7 @@
 #include <linux/init.h>
 #include <linux/linkage.h>
 #include <linux/unistd.h>
+#include <linux/threads.h>
 #include <asm/blackfin.h>
 #include <asm/errno.h>
 #include <asm/fixed_code.h>
@@ -75,11 +76,11 @@ ENTRY(_ex_workaround_261)
 	 * handle it.
 	 */
 	P4 = R7;	/* Store EXCAUSE */
-	p5.l = _last_cplb_fault_retx;
-	p5.h = _last_cplb_fault_retx;
-	r7 = [p5];
+
+	GET_PDA(p5, r7);
+	r7 = [p5 + PDA_LFRETX];
 	r6 = retx;
-	[p5] = r6;
+	[p5 + PDA_LFRETX] = r6;
 	cc = r6 == r7;
 	if !cc jump _bfin_return_from_exception;
 	/* fall through */
@@ -111,24 +112,21 @@ ENTRY(_ex_dcplb_viol)
 ENTRY(_ex_dcplb_miss)
 ENTRY(_ex_icplb_miss)
 	(R7:6,P5:4) = [sp++];
-	ASTAT = [sp++];
-	SAVE_ALL_SYS
-#ifdef CONFIG_MPU
+	/* We leave the previously pushed ASTAT on the stack.  */
+	SAVE_CONTEXT_CPLB
+
 	/* We must load R1 here, _before_ DEBUG_HWTRACE_SAVE, since that
 	 * will change the stack pointer.  */
 	R0 = SEQSTAT;
 	R1 = SP;
-#endif
+
 	DEBUG_HWTRACE_SAVE(p5, r7)
-#ifdef CONFIG_MPU
+
 	sp += -12;
 	call _cplb_hdr;
 	sp += 12;
 	CC = R0 == 0;
 	IF !CC JUMP _handle_bad_cplb;
-#else
-	call __cplb_hdr;
-#endif
 
 #ifdef CONFIG_DEBUG_DOUBLEFAULT
 	/* While we were processing this, did we double fault? */
@@ -142,7 +140,8 @@ ENTRY(_ex_icplb_miss)
 #endif
 
 	DEBUG_HWTRACE_RESTORE(p5, r7)
-	RESTORE_ALL_SYS
+	RESTORE_CONTEXT_CPLB
+	ASTAT = [SP++];
 	SP = EX_SCRATCH_REG;
 	rtx;
 ENDPROC(_ex_icplb_miss)
@@ -297,9 +296,8 @@ ENTRY(_handle_bad_cplb)
 	 * the stack to get ready so, we can fall through - we
 	 * need to make a CPLB exception look like a normal exception
 	 */
-
-	RESTORE_ALL_SYS
-	[--sp] = ASTAT;
+	RESTORE_CONTEXT_CPLB
+	/* ASTAT is still on the stack, where it is needed.  */
 	[--sp] = (R7:6,P5:4);
 
 ENTRY(_ex_replaceable)
@@ -324,7 +322,9 @@ ENTRY(_ex_trap_c)
 	[p4] = p5;
 	csync;
 
+	GET_PDA(p5, r6);
 #ifndef CONFIG_DEBUG_DOUBLEFAULT
+
 	/*
 	 * Save these registers, as they are only valid in exception context
 	 *  (where we are now - as soon as we defer to IRQ5, they can change)
@@ -335,29 +335,25 @@ ENTRY(_ex_trap_c)
 	p4.l = lo(DCPLB_FAULT_ADDR);
 	p4.h = hi(DCPLB_FAULT_ADDR);
 	r7 = [p4];
-	p5.h = _saved_dcplb_fault_addr;
-	p5.l = _saved_dcplb_fault_addr;
-	[p5] = r7;
+	[p5 + PDA_DCPLB] = r7;
 
-	r7 = [p4 + (ICPLB_FAULT_ADDR - DCPLB_FAULT_ADDR)];
-	p5.h = _saved_icplb_fault_addr;
-	p5.l = _saved_icplb_fault_addr;
-	[p5] = r7;
+	p4.l = lo(ICPLB_FAULT_ADDR);
+	p4.h = hi(ICPLB_FAULT_ADDR);
+	r6 = [p4];
+	[p5 + PDA_ICPLB] = r6;
 
 	r6 = retx;
-	p4.l = _saved_retx;
-	p4.h = _saved_retx;
-	[p4] = r6;
+	[p5 + PDA_RETX] = r6;
 #endif
 	r6 = SYSCFG;
-	[p4 + 4] = r6;
+	[p5 + PDA_SYSCFG] = r6;
 	BITCLR(r6, 0);
 	SYSCFG = r6;
 
 	/* Disable all interrupts, but make sure level 5 is enabled so
 	 * we can switch to that level.  Save the old mask.  */
 	cli r6;
-	[p4 + 8] = r6;
+	[p5 + PDA_EXIMASK] = r6;
 
 	p4.l = lo(SAFE_USER_INSTRUCTION);
 	p4.h = hi(SAFE_USER_INSTRUCTION);
@@ -371,9 +367,10 @@ ENTRY(_ex_trap_c)
 ENDPROC(_ex_trap_c)
 
 /* We just realized we got an exception, while we were processing a different
- * exception. This is a unrecoverable event, so crash
+ * exception. This is a unrecoverable event, so crash.
+ * Note: this cannot be ENTRY() as we jump here with "if cc jump" ...
  */
-ENTRY(_double_fault)
+_double_fault:
 	/* Turn caches & protection off, to ensure we don't get any more
 	 * double exceptions
 	 */
@@ -424,17 +421,16 @@ ENDPROC(_double_fault)
 ENTRY(_exception_to_level5)
 	SAVE_ALL_SYS
 
-	p4.l = _saved_retx;
-	p4.h = _saved_retx;
-	r6 = [p4];
+	GET_PDA(p4, r7);        /* Fetch current PDA */
+	r6 = [p4 + PDA_RETX];
 	[sp + PT_PC] = r6;
 
-	r6 = [p4 + 4];
+	r6 = [p4 + PDA_SYSCFG];
 	[sp + PT_SYSCFG] = r6;
 
 	/* Restore interrupt mask.  We haven't pushed RETI, so this
 	 * doesn't enable interrupts until we return from this handler.  */
-	r6 = [p4 + 8];
+	r6 = [p4 + PDA_EXIMASK];
 	sti r6;
 
 	/* Restore the hardware error vector.  */
@@ -478,8 +474,8 @@ ENTRY(_trap) /* Exception: 4th entry into system event table(supervisor mode)*/
 	 * scratch register (for want of a better option).
 	 */
 	EX_SCRATCH_REG = sp;
-	sp.l = _exception_stack_top;
-	sp.h = _exception_stack_top;
+	GET_PDA_SAFE(sp);
+	sp = [sp + PDA_EXSTACK]
 	/* Try to deal with syscalls quickly.  */
 	[--sp] = ASTAT;
 	[--sp] = (R7:6,P5:4);
@@ -501,27 +497,22 @@ ENTRY(_trap) /* Exception: 4th entry into system event table(supervisor mode)*/
 	 * but they are not very interesting, so don't save them
 	 */
 
+	GET_PDA(p5, r7);
 	p4.l = lo(DCPLB_FAULT_ADDR);
 	p4.h = hi(DCPLB_FAULT_ADDR);
 	r7 = [p4];
-	p5.h = _saved_dcplb_fault_addr;
-	p5.l = _saved_dcplb_fault_addr;
-	[p5] = r7;
+	[p5 + PDA_DCPLB] = r7;
 
-	r7 = [p4 + (ICPLB_FAULT_ADDR - DCPLB_FAULT_ADDR)];
-	p5.h = _saved_icplb_fault_addr;
-	p5.l = _saved_icplb_fault_addr;
-	[p5] = r7;
+	p4.l = lo(ICPLB_FAULT_ADDR);
+	p4.h = hi(ICPLB_FAULT_ADDR);
+	r7 = [p4];
+	[p5 + PDA_ICPLB] = r7;
 
-	p4.l = _saved_retx;
-	p4.h = _saved_retx;
 	r6 = retx;
-	[p4] = r6;
+	[p5 + PDA_RETX] = r6;
 
 	r7 = SEQSTAT;		/* reason code is in bit 5:0 */
-	p4.l = _saved_seqstat;
-	p4.h = _saved_seqstat;
-	[p4] = r7;
+	[p5 + PDA_SEQSTAT] = r7;
 #else
 	r7 = SEQSTAT;           /* reason code is in bit 5:0 */
 #endif
@@ -546,11 +537,11 @@ ENTRY(_kernel_execve)
 	p0 = sp;
 	r3 = SIZEOF_PTREGS / 4;
 	r4 = 0(x);
-0:
+.Lclear_regs:
 	[p0++] = r4;
 	r3 += -1;
 	cc = r3 == 0;
-	if !cc jump 0b (bp);
+	if !cc jump .Lclear_regs (bp);
 
 	p0 = sp;
 	sp += -16;
@@ -558,7 +549,7 @@ ENTRY(_kernel_execve)
 	call _do_execve;
 	SP += 16;
 	cc = r0 == 0;
-	if ! cc jump 1f;
+	if ! cc jump .Lexecve_failed;
 	/* Success.  Copy our temporary pt_regs to the top of the kernel
 	 * stack and do a normal exception return.
 	 */
@@ -574,12 +565,12 @@ ENTRY(_kernel_execve)
 	p0 = fp;
 	r4 = [p0--];
 	r3 = SIZEOF_PTREGS / 4;
-0:
+.Lcopy_regs:
 	r4 = [p0--];
 	[p1--] = r4;
 	r3 += -1;
 	cc = r3 == 0;
-	if ! cc jump 0b (bp);
+	if ! cc jump .Lcopy_regs (bp);
 
 	r0 = (KERNEL_STACK_SIZE - SIZEOF_PTREGS) (z);
 	p1 = r0;
@@ -591,7 +582,7 @@ ENTRY(_kernel_execve)
 
 	RESTORE_CONTEXT;
 	rti;
-1:
+.Lexecve_failed:
 	unlink;
 	rts;
 ENDPROC(_kernel_execve)
@@ -925,9 +916,14 @@ _schedule_and_signal_from_int:
 	p1 = rets;
 	[sp + PT_RESERVED] = p1;
 
-	p0.l = _irq_flags;
-	p0.h = _irq_flags;
+#ifdef CONFIG_SMP
+	GET_PDA(p0, r0); 	/* Fetch current PDA (can't migrate to other CPU here) */
+	r0 = [p0 + PDA_IRQFLAGS];
+#else
+	p0.l = _bfin_irq_flags;
+	p0.h = _bfin_irq_flags;
 	r0 = [p0];
+#endif
 	sti r0;
 
 	r0 = sp;
@@ -1539,14 +1535,18 @@ ENTRY(_sys_call_table)
 	.endr
 END(_sys_call_table)
 
-_exception_stack:
-	.rept 1024
-	.long 0;
+#ifdef CONFIG_EXCEPTION_L1_SCRATCH
+/* .section .l1.bss.scratch */
+.set _exception_stack_top, L1_SCRATCH_START + L1_SCRATCH_LENGTH
+#else
+#ifdef CONFIG_SYSCALL_TAB_L1
+.section .l1.bss
+#else
+.bss
+#endif
+ENTRY(_exception_stack)
+	.rept 1024 * NR_CPUS
+	.long 0
 	.endr
 _exception_stack_top:
-
-#if ANOMALY_05000261
-/* Used by the assembly entry point to work around an anomaly.  */
-_last_cplb_fault_retx:
-	.long 0;
 #endif
diff --git a/arch/blackfin/mach-common/head.S b/arch/blackfin/mach-common/head.S
index f123a62e2451..e1e42c029e15 100644
--- a/arch/blackfin/mach-common/head.S
+++ b/arch/blackfin/mach-common/head.S
@@ -13,6 +13,7 @@
 #include <asm/blackfin.h>
 #include <asm/thread_info.h>
 #include <asm/trace.h>
+#include <asm/asm-offsets.h>
 
 __INIT
 
@@ -111,33 +112,26 @@ ENTRY(__start)
 	 * This happens here, since L1 gets clobbered
 	 * below
 	 */
-	p0.l = _saved_retx;
-	p0.h = _saved_retx;
+	GET_PDA(p0, r0);
+	r7 = [p0 + PDA_RETX];
 	p1.l = _init_saved_retx;
 	p1.h = _init_saved_retx;
-	r0 = [p0];
-	[p1] = r0;
+	[p1] = r7;
 
-	p0.l = _saved_dcplb_fault_addr;
-	p0.h = _saved_dcplb_fault_addr;
+	r7 = [p0 + PDA_DCPLB];
 	p1.l = _init_saved_dcplb_fault_addr;
 	p1.h = _init_saved_dcplb_fault_addr;
-	r0 = [p0];
-	[p1] = r0;
+	[p1] = r7;
 
-	p0.l = _saved_icplb_fault_addr;
-	p0.h = _saved_icplb_fault_addr;
+	r7 = [p0 + PDA_ICPLB];
 	p1.l = _init_saved_icplb_fault_addr;
 	p1.h = _init_saved_icplb_fault_addr;
-	r0 = [p0];
-	[p1] = r0;
+	[p1] = r7;
 
-	p0.l = _saved_seqstat;
-	p0.h = _saved_seqstat;
+	r7 = [p0 + PDA_SEQSTAT];
 	p1.l = _init_saved_seqstat;
 	p1.h = _init_saved_seqstat;
-	r0 = [p0];
-	[p1] = r0;
+	[p1] = r7;
 #endif
 
 	/* Initialize stack pointer */
@@ -153,7 +147,7 @@ ENTRY(__start)
 	/* Put The Code for PLL Programming and SDRAM Programming in L1 ISRAM */
 	call _bfin_relocate_l1_mem;
 #ifdef CONFIG_BFIN_KERNEL_CLOCK
-	call _start_dma_code;
+	call _init_clocks;
 #endif
 
 	/* This section keeps the processor in supervisor mode
@@ -170,12 +164,8 @@ ENTRY(__start)
 	[p0] = p1;
 	csync;
 
-	p0.l = lo(IMASK);
-	p0.h = hi(IMASK);
-	p1.l = IMASK_IVG15;
-	p1.h = 0x0;
-	[p0] = p1;
-	csync;
+	r0 = EVT_IVG15 (z);
+	sti r0;
 
 	raise 15;
 	p0.l = .LWAIT_HERE;
@@ -195,6 +185,19 @@ ENDPROC(__start)
 # define WDOG_CTL WDOGA_CTL
 #endif
 
+ENTRY(__init_clear_bss)
+	r2 = r2 - r1;
+	cc = r2 == 0;
+	if cc jump .L_bss_done;
+	r2 >>= 2;
+	p1 = r1;
+	p2 = r2;
+	lsetup (1f, 1f) lc0 = p2;
+1:	[p1++] = r0;
+.L_bss_done:
+	rts;
+ENDPROC(__init_clear_bss)
+
 ENTRY(_real_start)
 	/* Enable nested interrupts */
 	[--sp] = reti;
@@ -206,87 +209,34 @@ ENTRY(_real_start)
 	w[p0] = r0;
 	ssync;
 
+	r0 = 0 (x);
+	/* Zero out all of the fun bss regions */
 #if L1_DATA_A_LENGTH > 0
 	r1.l = __sbss_l1;
 	r1.h = __sbss_l1;
 	r2.l = __ebss_l1;
 	r2.h = __ebss_l1;
-	r0 = 0 (z);
-	r2 = r2 - r1;
-	cc = r2 == 0;
-	if cc jump .L_a_l1_done;
-	r2 >>= 2;
-	p1 = r1;
-	p2 = r2;
-	lsetup (.L_clear_a_l1, .L_clear_a_l1 ) lc0 = p2;
-.L_clear_a_l1:
-	[p1++] = r0;
-.L_a_l1_done:
+	call __init_clear_bss
 #endif
-
 #if L1_DATA_B_LENGTH > 0
 	r1.l = __sbss_b_l1;
 	r1.h = __sbss_b_l1;
 	r2.l = __ebss_b_l1;
 	r2.h = __ebss_b_l1;
-	r0 = 0 (z);
-	r2 = r2 - r1;
-	cc = r2 == 0;
-	if cc jump .L_b_l1_done;
-	r2 >>= 2;
-	p1 = r1;
-	p2 = r2;
-	lsetup (.L_clear_b_l1, .L_clear_b_l1 ) lc0 = p2;
-.L_clear_b_l1:
-	[p1++] = r0;
-.L_b_l1_done:
+	call __init_clear_bss
 #endif
-
 #if L2_LENGTH > 0
 	r1.l = __sbss_l2;
 	r1.h = __sbss_l2;
 	r2.l = __ebss_l2;
 	r2.h = __ebss_l2;
-	r0 = 0 (z);
-	r2 = r2 - r1;
-	cc = r2 == 0;
-	if cc jump .L_l2_done;
-	r2 >>= 2;
-	p1 = r1;
-	p2 = r2;
-	lsetup (.L_clear_l2, .L_clear_l2 ) lc0 = p2;
-.L_clear_l2:
-	[p1++] = r0;
-.L_l2_done:
+	call __init_clear_bss
 #endif
-
-	/* Zero out the bss region
-	 * Note: this will fail if bss is 0 bytes ...
-	 */
-	r0 = 0 (z);
 	r1.l = ___bss_start;
 	r1.h = ___bss_start;
 	r2.l = ___bss_stop;
 	r2.h = ___bss_stop;
-	r2 = r2 - r1;
-	r2 >>= 2;
-	p1 = r1;
-	p2 = r2;
-	lsetup (.L_clear_bss, .L_clear_bss) lc0 = p2;
-.L_clear_bss:
-	[p1++] = r0;
-
-	/* In case there is a NULL pointer reference,
-	 * zero out region before stext
-	 */
-	p1 = r0;
-	r2.l = __stext;
-	r2.h = __stext;
-	r2 >>= 2;
-	p2 = r2;
-	lsetup (.L_clear_zero, .L_clear_zero) lc0 = p2;
-.L_clear_zero:
-	[p1++] = r0;
+	call __init_clear_bss
 
 	/* Pass the u-boot arguments to the global value command line */
 	R0 = R7;
@@ -299,6 +249,9 @@ ENTRY(_real_start)
 	sp = sp + p1;
 	usp = sp;
 	fp = sp;
+	sp += -12;
+	call _init_pda
+	sp += 12;
 	jump.l _start_kernel;
 ENDPROC(_real_start)
 
diff --git a/arch/blackfin/mach-common/interrupt.S b/arch/blackfin/mach-common/interrupt.S
index 4a2ec7a9675a..473df0f7fa78 100644
--- a/arch/blackfin/mach-common/interrupt.S
+++ b/arch/blackfin/mach-common/interrupt.S
@@ -129,8 +129,15 @@ __common_int_entry:
 #endif
 	r1 =  sp;
 	SP += -12;
+#ifdef CONFIG_IPIPE
+	call ___ipipe_grab_irq
+	SP += 12;
+	cc = r0 == 0;
+	if cc jump .Lcommon_restore_context;
+#else /* CONFIG_IPIPE */
 	call _do_irq;
 	SP += 12;
+#endif /* CONFIG_IPIPE */
 	call _return_from_int;
 .Lcommon_restore_context:
 	RESTORE_CONTEXT
@@ -152,15 +159,6 @@ ENTRY(_evt_ivhw)
 1:
 #endif
 
-#ifdef CONFIG_HARDWARE_PM
-	r7 = [sp + PT_SEQSTAT];
-	r7 = r7 >>> 0xe;
-	r6 = 0x1F;
-	r7 = r7 & r6;
-	r5 = 0x12;
-	cc = r7 == r5;
-	if cc jump .Lcall_do_ovf; /* deal with performance counter overflow */
-#endif
 	# We are going to dump something out, so make sure we print IPEND properly
 	p2.l = lo(IPEND);
 	p2.h = hi(IPEND);
@@ -192,17 +190,6 @@ ENTRY(_evt_ivhw)
 .Lcommon_restore_all_sys:
 	RESTORE_ALL_SYS
 	rti;
-
-#ifdef CONFIG_HARDWARE_PM
-.Lcall_do_ovf:
-
-	SP += -12;
-	call _pm_overflow;
-	SP += 12;
-
-	jump .Lcommon_restore_all_sys;
-#endif
-
 ENDPROC(_evt_ivhw)
 
 /* Interrupt routine for evt2 (NMI).
@@ -245,3 +232,56 @@ ENTRY(_evt_system_call)
 	call _system_call;
 	jump .Lcommon_restore_context;
 ENDPROC(_evt_system_call)
+
+#ifdef CONFIG_IPIPE
+ENTRY(___ipipe_call_irqtail)
+	r0.l = 1f;
+	r0.h = 1f;
+	reti = r0;
+	rti;
+1:
+	[--sp] = rets;
+	[--sp] = ( r7:4, p5:3 );
+	p0.l = ___ipipe_irq_tail_hook;
+	p0.h = ___ipipe_irq_tail_hook;
+	p0 = [p0];
+	sp += -12;
+	call (p0);
+	sp += 12;
+	( r7:4, p5:3 ) = [sp++];
+	rets = [sp++];
+
+	[--sp] = reti;
+	reti = [sp++];          /* IRQs are off. */
+	r0.h = 3f;
+	r0.l = 3f;
+	p0.l = lo(EVT14);
+	p0.h = hi(EVT14);
+	[p0] = r0;
+	csync;
+	r0 = 0x401f;
+	sti r0;
+	raise 14;
+	[--sp] = reti;          /* IRQs on. */
+2:
+	jump 2b;                /* Likely paranoid. */
+3:
+	sp += 4;                /* Discard saved RETI */
+	r0.h = _evt14_softirq;
+	r0.l = _evt14_softirq;
+	p0.l = lo(EVT14);
+	p0.h = hi(EVT14);
+	[p0] = r0;
+	csync;
+	p0.l = _bfin_irq_flags;
+	p0.h = _bfin_irq_flags;
+	r0 = [p0];
+	sti r0;
+#if 0 /* FIXME: this actually raises scheduling latencies */
+	/* Reenable interrupts */
+	[--sp] = reti;
+	r0 = [sp++];
+#endif
+	rts;
+ENDPROC(___ipipe_call_irqtail)
+#endif /* CONFIG_IPIPE */
diff --git a/arch/blackfin/mach-common/ints-priority.c b/arch/blackfin/mach-common/ints-priority.c
index 34e8a726ffda..1bba6030dce9 100644
--- a/arch/blackfin/mach-common/ints-priority.c
+++ b/arch/blackfin/mach-common/ints-priority.c
@@ -1,9 +1,6 @@
 /*
  * File:         arch/blackfin/mach-common/ints-priority.c
- * Based on:
- * Author:
  *
- * Created:      ?
  * Description:  Set up the interrupt priorities
  *
  * Modified:
@@ -37,6 +34,9 @@
 #include <linux/kernel_stat.h>
 #include <linux/seq_file.h>
 #include <linux/irq.h>
+#ifdef CONFIG_IPIPE
+#include <linux/ipipe.h>
+#endif
 #ifdef CONFIG_KGDB
 #include <linux/kgdb.h>
 #endif
@@ -45,6 +45,8 @@
 #include <asm/gpio.h>
 #include <asm/irq_handler.h>
 
+#define SIC_SYSIRQ(irq)	(irq - (IRQ_CORETMR + 1))
+
 #ifdef BF537_FAMILY
 # define BF537_GENERIC_ERROR_INT_DEMUX
 #else
@@ -58,13 +60,16 @@
  * -
  */
 
+#ifndef CONFIG_SMP
 /* Initialize this to an actual value to force it into the .data
  * section so that we know it is properly initialized at entry into
  * the kernel but before bss is initialized to zero (which is where
  * it would live otherwise).  The 0x1f magic represents the IRQs we
  * cannot actually mask out in hardware.
  */
-unsigned long irq_flags = 0x1f;
+unsigned long bfin_irq_flags = 0x1f;
+EXPORT_SYMBOL(bfin_irq_flags);
+#endif
 
 /* The number of spurious interrupts */
 atomic_t num_spurious;
@@ -103,12 +108,14 @@ static void __init search_IAR(void)
 		for (irqn = 0; irqn < NR_PERI_INTS; irqn++) {
 			int iar_shift = (irqn & 7) * 4;
 				if (ivg == (0xf &
-#ifndef CONFIG_BF52x
+#if defined(CONFIG_BF52x) || defined(CONFIG_BF538) \
+	|| defined(CONFIG_BF539) || defined(CONFIG_BF51x)
 			     bfin_read32((unsigned long *)SIC_IAR0 +
-					 (irqn >> 3)) >> iar_shift)) {
+					 ((irqn % 32) >> 3) + ((irqn / 32) *
+					 ((SIC_IAR4 - SIC_IAR0) / 4))) >> iar_shift)) {
 #else
 			     bfin_read32((unsigned long *)SIC_IAR0 +
-					 ((irqn%32) >> 3) + ((irqn / 32) * 16)) >> iar_shift)) {
+					 (irqn >> 3)) >> iar_shift)) {
 #endif
 				ivg_table[irq_pos].irqno = IVG7 + irqn;
 				ivg_table[irq_pos].isrflag = 1 << (irqn % 32);
@@ -130,25 +137,25 @@ static void bfin_ack_noop(unsigned int irq)
 
 static void bfin_core_mask_irq(unsigned int irq)
 {
-	irq_flags &= ~(1 << irq);
-	if (!irqs_disabled())
-		local_irq_enable();
+	bfin_irq_flags &= ~(1 << irq);
+	if (!irqs_disabled_hw())
+		local_irq_enable_hw();
 }
 
 static void bfin_core_unmask_irq(unsigned int irq)
 {
-	irq_flags |= 1 << irq;
+	bfin_irq_flags |= 1 << irq;
 	/*
 	 * If interrupts are enabled, IMASK must contain the same value
-	 * as irq_flags.  Make sure that invariant holds.  If interrupts
+	 * as bfin_irq_flags.  Make sure that invariant holds.  If interrupts
 	 * are currently disabled we need not do anything; one of the
 	 * callers will take care of setting IMASK to the proper value
 	 * when reenabling interrupts.
-	 * local_irq_enable just does "STI irq_flags", so it's exactly
+	 * local_irq_enable just does "STI bfin_irq_flags", so it's exactly
 	 * what we need.
 	 */
-	if (!irqs_disabled())
-		local_irq_enable();
+	if (!irqs_disabled_hw())
+		local_irq_enable_hw();
 	return;
 }
 
@@ -163,8 +170,11 @@ static void bfin_internal_mask_irq(unsigned int irq)
 	mask_bit = SIC_SYSIRQ(irq) % 32;
 	bfin_write_SIC_IMASK(mask_bank, bfin_read_SIC_IMASK(mask_bank) &
 			     ~(1 << mask_bit));
+#ifdef CONFIG_SMP
+	bfin_write_SICB_IMASK(mask_bank, bfin_read_SICB_IMASK(mask_bank) &
+			     ~(1 << mask_bit));
+#endif
 #endif
-	SSYNC();
 }
 
 static void bfin_internal_unmask_irq(unsigned int irq)
@@ -178,14 +188,17 @@ static void bfin_internal_unmask_irq(unsigned int irq)
 	mask_bit = SIC_SYSIRQ(irq) % 32;
 	bfin_write_SIC_IMASK(mask_bank, bfin_read_SIC_IMASK(mask_bank) |
 			     (1 << mask_bit));
+#ifdef CONFIG_SMP
+	bfin_write_SICB_IMASK(mask_bank, bfin_read_SICB_IMASK(mask_bank) |
+			     (1 << mask_bit));
+#endif
 #endif
-	SSYNC();
 }
 
 #ifdef CONFIG_PM
 int bfin_internal_set_wake(unsigned int irq, unsigned int state)
 {
-	unsigned bank, bit, wakeup = 0;
+	u32 bank, bit, wakeup = 0;
 	unsigned long flags;
 	bank = SIC_SYSIRQ(irq) / 32;
 	bit = SIC_SYSIRQ(irq) % 32;
@@ -225,7 +238,7 @@ int bfin_internal_set_wake(unsigned int irq, unsigned int state)
 	break;
 	}
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 
 	if (state) {
 		bfin_sic_iwr[bank] |= (1 << bit);
@@ -236,7 +249,7 @@ int bfin_internal_set_wake(unsigned int irq, unsigned int state)
 		vr_wakeup  &= ~wakeup;
 	}
 
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 	return 0;
 }
@@ -262,6 +275,19 @@ static struct irq_chip bfin_internal_irqchip = {
 #endif
 };
 
+static void bfin_handle_irq(unsigned irq)
+{
+#ifdef CONFIG_IPIPE
+	struct pt_regs regs;    /* Contents not used. */
+	ipipe_trace_irq_entry(irq);
+	__ipipe_handle_irq(irq, &regs);
+	ipipe_trace_irq_exit(irq);
+#else /* !CONFIG_IPIPE */
+	struct irq_desc *desc = irq_desc + irq;
+	desc->handle_irq(irq, desc);
+#endif  /* !CONFIG_IPIPE */
+}
+
 #ifdef BF537_GENERIC_ERROR_INT_DEMUX
 static int error_int_mask;
 
@@ -292,8 +318,6 @@ static void bfin_demux_error_irq(unsigned int int_err_irq,
 {
 	int irq = 0;
 
-	SSYNC();
-
 #if (defined(CONFIG_BF537) || defined(CONFIG_BF536))
 	if (bfin_read_EMAC_SYSTAT() & EMAC_ERR_MASK)
 		irq = IRQ_MAC_ERROR;
@@ -317,10 +341,9 @@ static void bfin_demux_error_irq(unsigned int int_err_irq,
 		irq = IRQ_UART1_ERROR;
 
 	if (irq) {
-		if (error_int_mask & (1L << (irq - IRQ_PPI_ERROR))) {
-			struct irq_desc *desc = irq_desc + irq;
-			desc->handle_irq(irq, desc);
-		} else {
+		if (error_int_mask & (1L << (irq - IRQ_PPI_ERROR)))
+			bfin_handle_irq(irq);
+		else {
 
 			switch (irq) {
 			case IRQ_PPI_ERROR:
@@ -366,62 +389,57 @@ static void bfin_demux_error_irq(unsigned int int_err_irq,
 
 static inline void bfin_set_irq_handler(unsigned irq, irq_flow_handler_t handle)
 {
+#ifdef CONFIG_IPIPE
+	_set_irq_handler(irq, handle_edge_irq);
+#else
 	struct irq_desc *desc = irq_desc + irq;
 	/* May not call generic set_irq_handler() due to spinlock
 	   recursion. */
 	desc->handle_irq = handle;
+#endif
 }
 
-#if !defined(CONFIG_BF54x)
-
-static unsigned short gpio_enabled[gpio_bank(MAX_BLACKFIN_GPIOS)];
-static unsigned short gpio_edge_triggered[gpio_bank(MAX_BLACKFIN_GPIOS)];
-
+static DECLARE_BITMAP(gpio_enabled, MAX_BLACKFIN_GPIOS);
 extern void bfin_gpio_irq_prepare(unsigned gpio);
 
+#if !defined(CONFIG_BF54x)
+
 static void bfin_gpio_ack_irq(unsigned int irq)
 {
-	u16 gpionr = irq - IRQ_PF0;
-
-	if (gpio_edge_triggered[gpio_bank(gpionr)] & gpio_bit(gpionr)) {
-		set_gpio_data(gpionr, 0);
-		SSYNC();
-	}
+	/* AFAIK ack_irq in case mask_ack is provided
+	 * get's only called for edge sense irqs
+	 */
+	set_gpio_data(irq_to_gpio(irq), 0);
 }
 
 static void bfin_gpio_mask_ack_irq(unsigned int irq)
 {
-	u16 gpionr = irq - IRQ_PF0;
+	struct irq_desc *desc = irq_desc + irq;
+	u32 gpionr = irq_to_gpio(irq);
 
-	if (gpio_edge_triggered[gpio_bank(gpionr)] & gpio_bit(gpionr)) {
+	if (desc->handle_irq == handle_edge_irq)
 		set_gpio_data(gpionr, 0);
-		SSYNC();
-	}
 
 	set_gpio_maska(gpionr, 0);
-	SSYNC();
 }
 
 static void bfin_gpio_mask_irq(unsigned int irq)
 {
-	set_gpio_maska(irq - IRQ_PF0, 0);
-	SSYNC();
+	set_gpio_maska(irq_to_gpio(irq), 0);
 }
 
 static void bfin_gpio_unmask_irq(unsigned int irq)
 {
-	set_gpio_maska(irq - IRQ_PF0, 1);
-	SSYNC();
+	set_gpio_maska(irq_to_gpio(irq), 1);
 }
 
 static unsigned int bfin_gpio_irq_startup(unsigned int irq)
 {
-	u16 gpionr = irq - IRQ_PF0;
+	u32 gpionr = irq_to_gpio(irq);
 
-	if (!(gpio_enabled[gpio_bank(gpionr)] & gpio_bit(gpionr)))
+	if (__test_and_set_bit(gpionr, gpio_enabled))
 		bfin_gpio_irq_prepare(gpionr);
 
-	gpio_enabled[gpio_bank(gpionr)] |= gpio_bit(gpionr);
 	bfin_gpio_unmask_irq(irq);
 
 	return 0;
@@ -429,29 +447,39 @@ static unsigned int bfin_gpio_irq_startup(unsigned int irq)
 
 static void bfin_gpio_irq_shutdown(unsigned int irq)
 {
+	u32 gpionr = irq_to_gpio(irq);
+
 	bfin_gpio_mask_irq(irq);
-	gpio_enabled[gpio_bank(irq - IRQ_PF0)] &= ~gpio_bit(irq - IRQ_PF0);
+	__clear_bit(gpionr, gpio_enabled);
+	bfin_gpio_irq_free(gpionr);
 }
 
 static int bfin_gpio_irq_type(unsigned int irq, unsigned int type)
 {
-	u16 gpionr = irq - IRQ_PF0;
+	int ret;
+	char buf[16];
+	u32 gpionr = irq_to_gpio(irq);
 
 	if (type == IRQ_TYPE_PROBE) {
 		/* only probe unenabled GPIO interrupt lines */
-		if (gpio_enabled[gpio_bank(gpionr)] & gpio_bit(gpionr))
+		if (__test_bit(gpionr, gpio_enabled))
 			return 0;
 		type = IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING;
 	}
 
 	if (type & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING |
 		    IRQ_TYPE_LEVEL_HIGH | IRQ_TYPE_LEVEL_LOW)) {
-		if (!(gpio_enabled[gpio_bank(gpionr)] & gpio_bit(gpionr)))
+
+		snprintf(buf, 16, "gpio-irq%d", irq);
+		ret = bfin_gpio_irq_request(gpionr, buf);
+		if (ret)
+			return ret;
+
+		if (__test_and_set_bit(gpionr, gpio_enabled))
 			bfin_gpio_irq_prepare(gpionr);
 
-		gpio_enabled[gpio_bank(gpionr)] |= gpio_bit(gpionr);
 	} else {
-		gpio_enabled[gpio_bank(gpionr)] &= ~gpio_bit(gpionr);
+		__clear_bit(gpionr, gpio_enabled);
 		return 0;
 	}
 
@@ -472,17 +500,13 @@ static int bfin_gpio_irq_type(unsigned int irq, unsigned int type)
 	if (type & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING)) {
 		set_gpio_edge(gpionr, 1);
 		set_gpio_inen(gpionr, 1);
-		gpio_edge_triggered[gpio_bank(gpionr)] |= gpio_bit(gpionr);
 		set_gpio_data(gpionr, 0);
 
 	} else {
 		set_gpio_edge(gpionr, 0);
-		gpio_edge_triggered[gpio_bank(gpionr)] &= ~gpio_bit(gpionr);
 		set_gpio_inen(gpionr, 1);
 	}
 
-	SSYNC();
-
 	if (type & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING))
 		bfin_set_irq_handler(irq, handle_edge_irq);
 	else
@@ -505,22 +529,6 @@ int bfin_gpio_set_wake(unsigned int irq, unsigned int state)
 }
 #endif
 
-static struct irq_chip bfin_gpio_irqchip = {
-	.name = "GPIO",
-	.ack = bfin_gpio_ack_irq,
-	.mask = bfin_gpio_mask_irq,
-	.mask_ack = bfin_gpio_mask_ack_irq,
-	.unmask = bfin_gpio_unmask_irq,
-	.disable = bfin_gpio_mask_irq,
-	.enable = bfin_gpio_unmask_irq,
-	.set_type = bfin_gpio_irq_type,
-	.startup = bfin_gpio_irq_startup,
-	.shutdown = bfin_gpio_irq_shutdown,
-#ifdef CONFIG_PM
-	.set_wake = bfin_gpio_set_wake,
-#endif
-};
-
 static void bfin_demux_gpio_irq(unsigned int inta_irq,
 				struct irq_desc *desc)
 {
@@ -537,7 +545,11 @@ static void bfin_demux_gpio_irq(unsigned int inta_irq,
 		irq = IRQ_PH0;
 		break;
 # endif
-#elif defined(CONFIG_BF52x)
+#elif defined(CONFIG_BF538) || defined(CONFIG_BF539)
+	case IRQ_PORTF_INTA:
+		irq = IRQ_PF0;
+		break;
+#elif defined(CONFIG_BF52x) || defined(CONFIG_BF51x)
 	case IRQ_PORTF_INTA:
 		irq = IRQ_PF0;
 		break;
@@ -567,30 +579,22 @@ static void bfin_demux_gpio_irq(unsigned int inta_irq,
 		for (i = 0; i < MAX_BLACKFIN_GPIOS; i += GPIO_BANKSIZE) {
 			irq += i;
 
-			mask = get_gpiop_data(i) &
-				(gpio_enabled[gpio_bank(i)] &
-				get_gpiop_maska(i));
+			mask = get_gpiop_data(i) & get_gpiop_maska(i);
 
 			while (mask) {
-				if (mask & 1) {
-					desc = irq_desc + irq;
-					desc->handle_irq(irq, desc);
-				}
+				if (mask & 1)
+					bfin_handle_irq(irq);
 				irq++;
 				mask >>= 1;
 			}
 		}
 	} else {
 			gpio = irq_to_gpio(irq);
-			mask = get_gpiop_data(gpio) &
-				(gpio_enabled[gpio_bank(gpio)] &
-				get_gpiop_maska(gpio));
+			mask = get_gpiop_data(gpio) & get_gpiop_maska(gpio);
 
 			do {
-				if (mask & 1) {
-					desc = irq_desc + irq;
-					desc->handle_irq(irq, desc);
-				}
+				if (mask & 1)
+					bfin_handle_irq(irq);
 				irq++;
 				mask >>= 1;
 			} while (mask);
@@ -612,10 +616,6 @@ static void bfin_demux_gpio_irq(unsigned int inta_irq,
 static unsigned char irq2pint_lut[NR_PINTS];
 static unsigned char pint2irq_lut[NR_PINT_SYS_IRQS * NR_PINT_BITS];
 
-static unsigned int gpio_both_edge_triggered[NR_PINT_SYS_IRQS];
-static unsigned short gpio_enabled[gpio_bank(MAX_BLACKFIN_GPIOS)];
-
-
 struct pin_int_t {
 	unsigned int mask_set;
 	unsigned int mask_clear;
@@ -636,12 +636,9 @@ static struct pin_int_t *pint[NR_PINT_SYS_IRQS] = {
 	(struct pin_int_t *)PINT3_MASK_SET,
 };
 
-extern void bfin_gpio_irq_prepare(unsigned gpio);
-
-inline unsigned short get_irq_base(u8 bank, u8 bmap)
+inline unsigned int get_irq_base(u32 bank, u8 bmap)
 {
-
-	u16 irq_base;
+	unsigned int irq_base;
 
 	if (bank < 2) {		/*PA-PB */
 		irq_base = IRQ_PA0 + bmap * 16;
@@ -650,7 +647,6 @@ inline unsigned short get_irq_base(u8 bank, u8 bmap)
 	}
 
 	return irq_base;
-
 }
 
 	/* Whenever PINTx_ASSIGN is altered init_pint_lut() must be executed! */
@@ -677,20 +673,18 @@ void init_pint_lut(void)
 
 			pint2irq_lut[bit_pos] = irq_base - SYS_IRQS;
 			irq2pint_lut[irq_base - SYS_IRQS] = bit_pos;
-
 		}
-
 	}
-
 }
 
 static void bfin_gpio_ack_irq(unsigned int irq)
 {
-	u8 pint_val = irq2pint_lut[irq - SYS_IRQS];
+	struct irq_desc *desc = irq_desc + irq;
+	u32 pint_val = irq2pint_lut[irq - SYS_IRQS];
 	u32 pintbit = PINT_BIT(pint_val);
-	u8 bank = PINT_2_BANK(pint_val);
+	u32 bank = PINT_2_BANK(pint_val);
 
-	if (unlikely(gpio_both_edge_triggered[bank] & pintbit)) {
+	if ((desc->status & IRQ_TYPE_SENSE_MASK) == IRQ_TYPE_EDGE_BOTH) {
 		if (pint[bank]->invert_set & pintbit)
 			pint[bank]->invert_clear = pintbit;
 		else
@@ -698,16 +692,16 @@ static void bfin_gpio_ack_irq(unsigned int irq)
 	}
 	pint[bank]->request = pintbit;
 
-	SSYNC();
 }
 
 static void bfin_gpio_mask_ack_irq(unsigned int irq)
 {
-	u8 pint_val = irq2pint_lut[irq - SYS_IRQS];
+	struct irq_desc *desc = irq_desc + irq;
+	u32 pint_val = irq2pint_lut[irq - SYS_IRQS];
 	u32 pintbit = PINT_BIT(pint_val);
-	u8 bank = PINT_2_BANK(pint_val);
+	u32 bank = PINT_2_BANK(pint_val);
 
-	if (unlikely(gpio_both_edge_triggered[bank] & pintbit)) {
+	if ((desc->status & IRQ_TYPE_SENSE_MASK) == IRQ_TYPE_EDGE_BOTH) {
 		if (pint[bank]->invert_set & pintbit)
 			pint[bank]->invert_clear = pintbit;
 		else
@@ -716,32 +710,29 @@ static void bfin_gpio_mask_ack_irq(unsigned int irq)
 
 	pint[bank]->request = pintbit;
 	pint[bank]->mask_clear = pintbit;
-	SSYNC();
 }
 
 static void bfin_gpio_mask_irq(unsigned int irq)
 {
-	u8 pint_val = irq2pint_lut[irq - SYS_IRQS];
+	u32 pint_val = irq2pint_lut[irq - SYS_IRQS];
 
 	pint[PINT_2_BANK(pint_val)]->mask_clear = PINT_BIT(pint_val);
-	SSYNC();
 }
 
 static void bfin_gpio_unmask_irq(unsigned int irq)
 {
-	u8 pint_val = irq2pint_lut[irq - SYS_IRQS];
+	u32 pint_val = irq2pint_lut[irq - SYS_IRQS];
 	u32 pintbit = PINT_BIT(pint_val);
-	u8 bank = PINT_2_BANK(pint_val);
+	u32 bank = PINT_2_BANK(pint_val);
 
 	pint[bank]->request = pintbit;
 	pint[bank]->mask_set = pintbit;
-	SSYNC();
 }
 
 static unsigned int bfin_gpio_irq_startup(unsigned int irq)
 {
-	u16 gpionr = irq_to_gpio(irq);
-	u8 pint_val = irq2pint_lut[irq - SYS_IRQS];
+	u32 gpionr = irq_to_gpio(irq);
+	u32 pint_val = irq2pint_lut[irq - SYS_IRQS];
 
 	if (pint_val == IRQ_NOT_AVAIL) {
 		printk(KERN_ERR
@@ -750,10 +741,9 @@ static unsigned int bfin_gpio_irq_startup(unsigned int irq)
 		return -ENODEV;
 	}
 
-	if (!(gpio_enabled[gpio_bank(gpionr)] & gpio_bit(gpionr)))
+	if (__test_and_set_bit(gpionr, gpio_enabled))
 		bfin_gpio_irq_prepare(gpionr);
 
-	gpio_enabled[gpio_bank(gpionr)] |= gpio_bit(gpionr);
 	bfin_gpio_unmask_irq(irq);
 
 	return 0;
@@ -761,38 +751,45 @@ static unsigned int bfin_gpio_irq_startup(unsigned int irq)
 
 static void bfin_gpio_irq_shutdown(unsigned int irq)
 {
-	u16 gpionr = irq_to_gpio(irq);
+	u32 gpionr = irq_to_gpio(irq);
 
 	bfin_gpio_mask_irq(irq);
-	gpio_enabled[gpio_bank(gpionr)] &= ~gpio_bit(gpionr);
+	__clear_bit(gpionr, gpio_enabled);
+	bfin_gpio_irq_free(gpionr);
 }
 
 static int bfin_gpio_irq_type(unsigned int irq, unsigned int type)
 {
-
-	u16 gpionr = irq_to_gpio(irq);
-	u8 pint_val = irq2pint_lut[irq - SYS_IRQS];
+	int ret;
+	char buf[16];
+	u32 gpionr = irq_to_gpio(irq);
+	u32 pint_val = irq2pint_lut[irq - SYS_IRQS];
 	u32 pintbit = PINT_BIT(pint_val);
-	u8 bank = PINT_2_BANK(pint_val);
+	u32 bank = PINT_2_BANK(pint_val);
 
 	if (pint_val == IRQ_NOT_AVAIL)
 		return -ENODEV;
 
 	if (type == IRQ_TYPE_PROBE) {
 		/* only probe unenabled GPIO interrupt lines */
-		if (gpio_enabled[gpio_bank(gpionr)] & gpio_bit(gpionr))
+		if (__test_bit(gpionr, gpio_enabled))
 			return 0;
 		type = IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING;
 	}
 
 	if (type & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING |
 		    IRQ_TYPE_LEVEL_HIGH | IRQ_TYPE_LEVEL_LOW)) {
-		if (!(gpio_enabled[gpio_bank(gpionr)] & gpio_bit(gpionr)))
+
+		snprintf(buf, 16, "gpio-irq%d", irq);
+		ret = bfin_gpio_irq_request(gpionr, buf);
+		if (ret)
+			return ret;
+
+		if (__test_and_set_bit(gpionr, gpio_enabled))
 			bfin_gpio_irq_prepare(gpionr);
 
-		gpio_enabled[gpio_bank(gpionr)] |= gpio_bit(gpionr);
 	} else {
-		gpio_enabled[gpio_bank(gpionr)] &= ~gpio_bit(gpionr);
+		__clear_bit(gpionr, gpio_enabled);
 		return 0;
 	}
 
@@ -803,15 +800,10 @@ static int bfin_gpio_irq_type(unsigned int irq, unsigned int type)
 
 	if ((type & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING))
 	    == (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING)) {
-
-		gpio_both_edge_triggered[bank] |= pintbit;
-
 		if (gpio_get_value(gpionr))
 			pint[bank]->invert_set = pintbit;
 		else
 			pint[bank]->invert_clear = pintbit;
-	} else {
-		gpio_both_edge_triggered[bank] &= ~pintbit;
 	}
 
 	if (type & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING)) {
@@ -822,8 +814,6 @@ static int bfin_gpio_irq_type(unsigned int irq, unsigned int type)
 		bfin_set_irq_handler(irq, handle_level_irq);
 	}
 
-	SSYNC();
-
 	return 0;
 }
 
@@ -834,7 +824,7 @@ u32 pint_wakeup_masks[NR_PINT_SYS_IRQS];
 int bfin_gpio_set_wake(unsigned int irq, unsigned int state)
 {
 	u32 pint_irq;
-	u8 pint_val = irq2pint_lut[irq - SYS_IRQS];
+	u32 pint_val = irq2pint_lut[irq - SYS_IRQS];
 	u32 bank = PINT_2_BANK(pint_val);
 	u32 pintbit = PINT_BIT(pint_val);
 
@@ -895,26 +885,10 @@ void bfin_pm_restore(void)
 }
 #endif
 
-static struct irq_chip bfin_gpio_irqchip = {
-	.name = "GPIO",
-	.ack = bfin_gpio_ack_irq,
-	.mask = bfin_gpio_mask_irq,
-	.mask_ack = bfin_gpio_mask_ack_irq,
-	.unmask = bfin_gpio_unmask_irq,
-	.disable = bfin_gpio_mask_irq,
-	.enable = bfin_gpio_unmask_irq,
-	.set_type = bfin_gpio_irq_type,
-	.startup = bfin_gpio_irq_startup,
-	.shutdown = bfin_gpio_irq_shutdown,
-#ifdef CONFIG_PM
-	.set_wake = bfin_gpio_set_wake,
-#endif
-};
-
 static void bfin_demux_gpio_irq(unsigned int inta_irq,
 				struct irq_desc *desc)
 {
-	u8 bank, pint_val;
+	u32 bank, pint_val;
 	u32 request, irq;
 
 	switch (inta_irq) {
@@ -941,8 +915,7 @@ static void bfin_demux_gpio_irq(unsigned int inta_irq,
 	while (request) {
 		if (request & 1) {
 			irq = pint2irq_lut[pint_val] + SYS_IRQS;
-			desc = irq_desc + irq;
-			desc->handle_irq(irq, desc);
+			bfin_handle_irq(irq);
 		}
 		pint_val++;
 		request >>= 1;
@@ -951,10 +924,24 @@ static void bfin_demux_gpio_irq(unsigned int inta_irq,
 }
 #endif
 
-void __init init_exception_vectors(void)
-{
-	SSYNC();
+static struct irq_chip bfin_gpio_irqchip = {
+	.name = "GPIO",
+	.ack = bfin_gpio_ack_irq,
+	.mask = bfin_gpio_mask_irq,
+	.mask_ack = bfin_gpio_mask_ack_irq,
+	.unmask = bfin_gpio_unmask_irq,
+	.disable = bfin_gpio_mask_irq,
+	.enable = bfin_gpio_unmask_irq,
+	.set_type = bfin_gpio_irq_type,
+	.startup = bfin_gpio_irq_startup,
+	.shutdown = bfin_gpio_irq_shutdown,
+#ifdef CONFIG_PM
+	.set_wake = bfin_gpio_set_wake,
+#endif
+};
 
+void __cpuinit init_exception_vectors(void)
+{
 	/* cannot program in software:
 	 * evt0 - emulation (jtag)
 	 * evt1 - reset
@@ -979,17 +966,23 @@ void __init init_exception_vectors(void)
  * This function should be called during kernel startup to initialize
  * the BFin IRQ handling routines.
  */
+
 int __init init_arch_irq(void)
 {
 	int irq;
 	unsigned long ilat = 0;
 	/*  Disable all the peripheral intrs  - page 4-29 HW Ref manual */
-#if defined(CONFIG_BF54x) || defined(CONFIG_BF52x) || defined(CONFIG_BF561)
+#if defined(CONFIG_BF54x) || defined(CONFIG_BF52x) || defined(CONFIG_BF561) \
+	|| defined(BF538_FAMILY) || defined(CONFIG_BF51x)
 	bfin_write_SIC_IMASK0(SIC_UNMASK_ALL);
 	bfin_write_SIC_IMASK1(SIC_UNMASK_ALL);
 # ifdef CONFIG_BF54x
 	bfin_write_SIC_IMASK2(SIC_UNMASK_ALL);
 # endif
+# ifdef CONFIG_SMP
+	bfin_write_SICB_IMASK0(SIC_UNMASK_ALL);
+	bfin_write_SICB_IMASK1(SIC_UNMASK_ALL);
+# endif
 #else
 	bfin_write_SIC_IMASK(SIC_UNMASK_ALL);
 #endif
@@ -1029,7 +1022,7 @@ int __init init_arch_irq(void)
 		case IRQ_PINT1:
 		case IRQ_PINT2:
 		case IRQ_PINT3:
-#elif defined(CONFIG_BF52x)
+#elif defined(CONFIG_BF52x) || defined(CONFIG_BF51x)
 		case IRQ_PORTF_INTA:
 		case IRQ_PORTG_INTA:
 		case IRQ_PORTH_INTA:
@@ -1037,18 +1030,41 @@ int __init init_arch_irq(void)
 		case IRQ_PROG0_INTA:
 		case IRQ_PROG1_INTA:
 		case IRQ_PROG2_INTA:
+#elif defined(CONFIG_BF538) || defined(CONFIG_BF539)
+		case IRQ_PORTF_INTA:
 #endif
+
 			set_irq_chained_handler(irq,
 						bfin_demux_gpio_irq);
 			break;
 #ifdef BF537_GENERIC_ERROR_INT_DEMUX
 		case IRQ_GENERIC_ERROR:
-			set_irq_handler(irq, bfin_demux_error_irq);
-
+			set_irq_chained_handler(irq, bfin_demux_error_irq);
+			break;
+#endif
+#if defined(CONFIG_TICK_SOURCE_SYSTMR0) || defined(CONFIG_IPIPE)
+		case IRQ_TIMER0:
+			set_irq_handler(irq, handle_percpu_irq);
+			break;
+#endif
+#ifdef CONFIG_SMP
+		case IRQ_SUPPLE_0:
+		case IRQ_SUPPLE_1:
+			set_irq_handler(irq, handle_percpu_irq);
 			break;
 #endif
 		default:
+#ifdef CONFIG_IPIPE
+	/*
+	 * We want internal interrupt sources to be masked, because
+	 * ISRs may trigger interrupts recursively (e.g. DMA), but
+	 * interrupts are _not_ masked at CPU level. So let's handle
+	 * them as level interrupts.
+	 */
+			set_irq_handler(irq, handle_level_irq);
+#else /* !CONFIG_IPIPE */
 			set_irq_handler(irq, handle_simple_irq);
+#endif /* !CONFIG_IPIPE */
 			break;
 		}
 	}
@@ -1073,7 +1089,7 @@ int __init init_arch_irq(void)
 	CSYNC();
 
 	printk(KERN_INFO "Configuring Blackfin Priority Driven Interrupts\n");
-	/* IMASK=xxx is equivalent to STI xx or irq_flags=xx,
+	/* IMASK=xxx is equivalent to STI xx or bfin_irq_flags=xx,
 	 * local_irq_enable()
 	 */
 	program_IAR();
@@ -1081,19 +1097,23 @@ int __init init_arch_irq(void)
 	search_IAR();
 
 	/* Enable interrupts IVG7-15 */
-	irq_flags = irq_flags | IMASK_IVG15 |
+	bfin_irq_flags |= IMASK_IVG15 |
 	    IMASK_IVG14 | IMASK_IVG13 | IMASK_IVG12 | IMASK_IVG11 |
 	    IMASK_IVG10 | IMASK_IVG9 | IMASK_IVG8 | IMASK_IVG7 | IMASK_IVGHW;
 
-#if defined(CONFIG_BF54x) || defined(CONFIG_BF52x) || defined(CONFIG_BF561)
+#if defined(CONFIG_BF54x) || defined(CONFIG_BF52x) || defined(CONFIG_BF561) \
+	|| defined(BF538_FAMILY) || defined(CONFIG_BF51x)
 	bfin_write_SIC_IWR0(IWR_DISABLE_ALL);
-#if defined(CONFIG_BF52x)
-	/* BF52x system reset does not properly reset SIC_IWR1 which
+#if defined(CONFIG_BF52x) || defined(CONFIG_BF51x)
+	/* BF52x/BF51x system reset does not properly reset SIC_IWR1 which
 	 * will screw up the bootrom as it relies on MDMA0/1 waking it
 	 * up from IDLE instructions.  See this report for more info:
 	 * http://blackfin.uclinux.org/gf/tracker/4323
 	 */
-	bfin_write_SIC_IWR1(IWR_ENABLE(10) | IWR_ENABLE(11));
+	if (ANOMALY_05000435)
+		bfin_write_SIC_IWR1(IWR_ENABLE(10) | IWR_ENABLE(11));
+	else
+		bfin_write_SIC_IWR1(IWR_DISABLE_ALL);
 #else
 	bfin_write_SIC_IWR1(IWR_DISABLE_ALL);
 #endif
@@ -1104,6 +1124,14 @@ int __init init_arch_irq(void)
 	bfin_write_SIC_IWR(IWR_DISABLE_ALL);
 #endif
 
+#ifdef CONFIG_IPIPE
+	for (irq = 0; irq < NR_IRQS; irq++) {
+		struct irq_desc *desc = irq_desc + irq;
+		desc->ic_prio = __ipipe_get_irq_priority(irq);
+		desc->thr_prio = __ipipe_get_irqthread_priority(irq);
+	}
+#endif /* CONFIG_IPIPE */
+
 	return 0;
 }
 
@@ -1117,11 +1145,20 @@ void do_irq(int vec, struct pt_regs *fp)
 	} else {
 		struct ivgx *ivg = ivg7_13[vec - IVG7].ifirst;
 		struct ivgx *ivg_stop = ivg7_13[vec - IVG7].istop;
-#if defined(CONFIG_BF54x) || defined(CONFIG_BF52x) || defined(CONFIG_BF561)
+#if defined(CONFIG_BF54x) || defined(CONFIG_BF52x) || defined(CONFIG_BF561) \
+	|| defined(BF538_FAMILY) || defined(CONFIG_BF51x)
 		unsigned long sic_status[3];
 
-		sic_status[0] = bfin_read_SIC_ISR0() & bfin_read_SIC_IMASK0();
-		sic_status[1] = bfin_read_SIC_ISR1() & bfin_read_SIC_IMASK1();
+		if (smp_processor_id()) {
+#ifdef CONFIG_SMP
+			/* This will be optimized out in UP mode. */
+			sic_status[0] = bfin_read_SICB_ISR0() & bfin_read_SICB_IMASK0();
+			sic_status[1] = bfin_read_SICB_ISR1() & bfin_read_SICB_IMASK1();
+#endif
+		} else {
+			sic_status[0] = bfin_read_SIC_ISR0() & bfin_read_SIC_IMASK0();
+			sic_status[1] = bfin_read_SIC_ISR1() & bfin_read_SIC_IMASK1();
+		}
 #ifdef CONFIG_BF54x
 		sic_status[2] = bfin_read_SIC_ISR2() & bfin_read_SIC_IMASK2();
 #endif
@@ -1150,3 +1187,161 @@ void do_irq(int vec, struct pt_regs *fp)
 	}
 	asm_do_IRQ(vec, fp);
 }
+
+#ifdef CONFIG_IPIPE
+
+int __ipipe_get_irq_priority(unsigned irq)
+{
+	int ient, prio;
+
+	if (irq <= IRQ_CORETMR)
+		return irq;
+
+	for (ient = 0; ient < NR_PERI_INTS; ient++) {
+		struct ivgx *ivg = ivg_table + ient;
+		if (ivg->irqno == irq) {
+			for (prio = 0; prio <= IVG13-IVG7; prio++) {
+				if (ivg7_13[prio].ifirst <= ivg &&
+				    ivg7_13[prio].istop > ivg)
+					return IVG7 + prio;
+			}
+		}
+	}
+
+	return IVG15;
+}
+
+int __ipipe_get_irqthread_priority(unsigned irq)
+{
+	int ient, prio;
+	int demux_irq;
+
+	/* The returned priority value is rescaled to [0..IVG13+1]
+	 * with 0 being the lowest effective priority level. */
+
+	if (irq <= IRQ_CORETMR)
+		return IVG13 - irq + 1;
+
+	/* GPIO IRQs are given the priority of the demux
+	 * interrupt. */
+	if (IS_GPIOIRQ(irq)) {
+#if defined(CONFIG_BF54x)
+		u32 bank = PINT_2_BANK(irq2pint_lut[irq - SYS_IRQS]);
+		demux_irq = (bank == 0 ? IRQ_PINT0 :
+				bank == 1 ? IRQ_PINT1 :
+				bank == 2 ? IRQ_PINT2 :
+				IRQ_PINT3);
+#elif defined(CONFIG_BF561)
+		demux_irq = (irq >= IRQ_PF32 ? IRQ_PROG2_INTA :
+				irq >= IRQ_PF16 ? IRQ_PROG1_INTA :
+				IRQ_PROG0_INTA);
+#elif defined(CONFIG_BF52x)
+		demux_irq = (irq >= IRQ_PH0 ? IRQ_PORTH_INTA :
+				irq >= IRQ_PG0 ? IRQ_PORTG_INTA :
+				IRQ_PORTF_INTA);
+#else
+		demux_irq = irq;
+#endif
+		return IVG13 - PRIO_GPIODEMUX(demux_irq) + 1;
+	}
+
+	/* The GPIO demux interrupt is given a lower priority
+	 * than the GPIO IRQs, so that its threaded handler
+	 * unmasks the interrupt line after the decoded IRQs
+	 * have been processed. */
+	prio = PRIO_GPIODEMUX(irq);
+	/* demux irq? */
+	if (prio != -1)
+		return IVG13 - prio;
+
+	for (ient = 0; ient < NR_PERI_INTS; ient++) {
+		struct ivgx *ivg = ivg_table + ient;
+		if (ivg->irqno == irq) {
+			for (prio = 0; prio <= IVG13-IVG7; prio++) {
+				if (ivg7_13[prio].ifirst <= ivg &&
+				    ivg7_13[prio].istop > ivg)
+					return IVG7 - prio;
+			}
+		}
+	}
+
+	return 0;
+}
+
+/* Hw interrupts are disabled on entry (check SAVE_CONTEXT). */
+#ifdef CONFIG_DO_IRQ_L1
+__attribute__((l1_text))
+#endif
+asmlinkage int __ipipe_grab_irq(int vec, struct pt_regs *regs)
+{
+	struct ivgx *ivg_stop = ivg7_13[vec-IVG7].istop;
+	struct ivgx *ivg = ivg7_13[vec-IVG7].ifirst;
+	int irq;
+
+	if (likely(vec == EVT_IVTMR_P)) {
+		irq = IRQ_CORETMR;
+		goto handle_irq;
+	}
+
+	SSYNC();
+
+#if defined(CONFIG_BF54x) || defined(CONFIG_BF52x) || defined(CONFIG_BF561)
+	{
+		unsigned long sic_status[3];
+
+		sic_status[0] = bfin_read_SIC_ISR0() & bfin_read_SIC_IMASK0();
+		sic_status[1] = bfin_read_SIC_ISR1() & bfin_read_SIC_IMASK1();
+#ifdef CONFIG_BF54x
+		sic_status[2] = bfin_read_SIC_ISR2() & bfin_read_SIC_IMASK2();
+#endif
+		for (;; ivg++) {
+			if (ivg >= ivg_stop) {
+				atomic_inc(&num_spurious);
+				return 0;
+			}
+			if (sic_status[(ivg->irqno - IVG7) / 32] & ivg->isrflag)
+				break;
+		}
+	}
+#else
+	{
+		unsigned long sic_status;
+
+		sic_status = bfin_read_SIC_IMASK() & bfin_read_SIC_ISR();
+
+		for (;; ivg++) {
+			if (ivg >= ivg_stop) {
+				atomic_inc(&num_spurious);
+				return 0;
+			} else if (sic_status & ivg->isrflag)
+				break;
+		}
+	}
+#endif
+
+	irq = ivg->irqno;
+
+	if (irq == IRQ_SYSTMR) {
+		bfin_write_TIMER_STATUS(1); /* Latch TIMIL0 */
+		/* This is basically what we need from the register frame. */
+		__raw_get_cpu_var(__ipipe_tick_regs).ipend = regs->ipend;
+		__raw_get_cpu_var(__ipipe_tick_regs).pc = regs->pc;
+		if (!ipipe_root_domain_p)
+			__raw_get_cpu_var(__ipipe_tick_regs).ipend |= 0x10;
+		else
+			__raw_get_cpu_var(__ipipe_tick_regs).ipend &= ~0x10;
+	}
+
+handle_irq:
+
+	ipipe_trace_irq_entry(irq);
+	__ipipe_handle_irq(irq, regs);
+       ipipe_trace_irq_exit(irq);
+
+       if (ipipe_root_domain_p)
+		return !test_bit(IPIPE_STALL_FLAG, &ipipe_root_cpudom_var(status));
+
+       return 0;
+}
+
+#endif /* CONFIG_IPIPE */
diff --git a/arch/blackfin/mach-common/irqpanic.c b/arch/blackfin/mach-common/irqpanic.c
index 606ded9ff4e1..05004df0f78b 100644
--- a/arch/blackfin/mach-common/irqpanic.c
+++ b/arch/blackfin/mach-common/irqpanic.c
@@ -33,8 +33,6 @@
 #include <asm/traps.h>
 #include <asm/blackfin.h>
 
-#include "../oprofile/op_blackfin.h"
-
 #ifdef CONFIG_DEBUG_ICACHE_CHECK
 #define L1_ICACHE_START 0xffa10000
 #define L1_ICACHE_END   0xffa13fff
@@ -134,13 +132,3 @@ asmlinkage void irq_panic(int reason, struct pt_regs *regs)
 #endif
 
 }
-
-#ifdef CONFIG_HARDWARE_PM
-/*
- * call the handler of Performance overflow
- */
-asmlinkage void pm_overflow(int irq, struct pt_regs *regs)
-{
-	pm_overflow_handler(irq, regs);
-}
-#endif
diff --git a/arch/blackfin/mach-common/lock.S b/arch/blackfin/mach-common/lock.S
index 9daf01201e9f..6c5f5f0ea7fe 100644
--- a/arch/blackfin/mach-common/lock.S
+++ b/arch/blackfin/mach-common/lock.S
@@ -160,7 +160,7 @@ ENDPROC(_cache_grab_lock)
  * R0 - Which way to be locked
  */
 
-ENTRY(_cache_lock)
+ENTRY(_bfin_cache_lock)
 
 	[--SP]=( R7:0,P5:0 );
 
@@ -184,7 +184,7 @@ ENTRY(_cache_lock)
 
 	( R7:0,P5:0 ) = [SP++];
 	RTS;
-ENDPROC(_cache_lock)
+ENDPROC(_bfin_cache_lock)
 
 /* Invalidate the Entire Instruction cache by
  * disabling IMC bit
diff --git a/arch/blackfin/mach-common/pm.c b/arch/blackfin/mach-common/pm.c
index e28c6af1f415..d3d70fd67c16 100644
--- a/arch/blackfin/mach-common/pm.c
+++ b/arch/blackfin/mach-common/pm.c
@@ -71,7 +71,7 @@ void bfin_pm_suspend_standby_enter(void)
 	gpio_pm_wakeup_request(CONFIG_PM_WAKEUP_GPIO_NUMBER, WAKEUP_TYPE);
 #endif
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	bfin_pm_standby_setup();
 
 #ifdef CONFIG_PM_BFIN_SLEEP_DEEPER
@@ -82,15 +82,19 @@ void bfin_pm_suspend_standby_enter(void)
 
 	bfin_pm_standby_restore();
 
-#if defined(CONFIG_BF54x) || defined(CONFIG_BF52x)  || defined(CONFIG_BF561)
+#if defined(CONFIG_BF54x) || defined(CONFIG_BF52x)  || defined(CONFIG_BF561) || \
+	defined(CONFIG_BF538) || defined(CONFIG_BF539) || defined(CONFIG_BF51x)
 	bfin_write_SIC_IWR0(IWR_DISABLE_ALL);
-#if defined(CONFIG_BF52x)
+#if defined(CONFIG_BF52x) || defined(CONFIG_BF51x)
 	/* BF52x system reset does not properly reset SIC_IWR1 which
 	 * will screw up the bootrom as it relies on MDMA0/1 waking it
 	 * up from IDLE instructions.  See this report for more info:
 	 * http://blackfin.uclinux.org/gf/tracker/4323
 	 */
-	bfin_write_SIC_IWR1(IWR_ENABLE(10) | IWR_ENABLE(11));
+	if (ANOMALY_05000435)
+		bfin_write_SIC_IWR1(IWR_ENABLE(10) | IWR_ENABLE(11));
+	else
+		bfin_write_SIC_IWR1(IWR_DISABLE_ALL);
 #else
 	bfin_write_SIC_IWR1(IWR_DISABLE_ALL);
 #endif
@@ -101,7 +105,7 @@ void bfin_pm_suspend_standby_enter(void)
 	bfin_write_SIC_IWR(IWR_DISABLE_ALL);
 #endif
 
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
 
 int bf53x_suspend_l1_mem(unsigned char *memptr)
@@ -245,12 +249,12 @@ int bfin_pm_suspend_mem_enter(void)
 	wakeup |= GPWE;
 #endif
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 
 	ret = blackfin_dma_suspend();
 
 	if (ret) {
-		local_irq_restore(flags);
+		local_irq_restore_hw(flags);
 		kfree(memptr);
 		return ret;
 	}
@@ -271,7 +275,7 @@ int bfin_pm_suspend_mem_enter(void)
 	bfin_gpio_pm_hibernate_restore();
 	blackfin_dma_resume();
 
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 	kfree(memptr);
 
 	return 0;
diff --git a/arch/blackfin/mach-common/smp.c b/arch/blackfin/mach-common/smp.c
new file mode 100644
index 000000000000..77c992847094
--- /dev/null
+++ b/arch/blackfin/mach-common/smp.c
@@ -0,0 +1,476 @@
+/*
+ * File:         arch/blackfin/kernel/smp.c
+ * Author:       Philippe Gerum <rpm@xenomai.org>
+ * IPI management based on arch/arm/kernel/smp.c.
+ *
+ *               Copyright 2007 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 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 the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/spinlock.h>
+#include <linux/sched.h>
+#include <linux/interrupt.h>
+#include <linux/cache.h>
+#include <linux/profile.h>
+#include <linux/errno.h>
+#include <linux/mm.h>
+#include <linux/cpu.h>
+#include <linux/smp.h>
+#include <linux/seq_file.h>
+#include <linux/irq.h>
+#include <asm/atomic.h>
+#include <asm/cacheflush.h>
+#include <asm/mmu_context.h>
+#include <asm/pgtable.h>
+#include <asm/pgalloc.h>
+#include <asm/processor.h>
+#include <asm/ptrace.h>
+#include <asm/cpu.h>
+#include <linux/err.h>
+
+struct corelock_slot corelock __attribute__ ((__section__(".l2.bss")));
+
+void __cpuinitdata *init_retx_coreb, *init_saved_retx_coreb,
+	*init_saved_seqstat_coreb, *init_saved_icplb_fault_addr_coreb,
+	*init_saved_dcplb_fault_addr_coreb;
+
+cpumask_t cpu_possible_map;
+EXPORT_SYMBOL(cpu_possible_map);
+
+cpumask_t cpu_online_map;
+EXPORT_SYMBOL(cpu_online_map);
+
+#define BFIN_IPI_RESCHEDULE   0
+#define BFIN_IPI_CALL_FUNC    1
+#define BFIN_IPI_CPU_STOP     2
+
+struct blackfin_flush_data {
+	unsigned long start;
+	unsigned long end;
+};
+
+void *secondary_stack;
+
+
+struct smp_call_struct {
+	void (*func)(void *info);
+	void *info;
+	int wait;
+	cpumask_t pending;
+	cpumask_t waitmask;
+};
+
+static struct blackfin_flush_data smp_flush_data;
+
+static DEFINE_SPINLOCK(stop_lock);
+
+struct ipi_message {
+	struct list_head list;
+	unsigned long type;
+	struct smp_call_struct call_struct;
+};
+
+struct ipi_message_queue {
+	struct list_head head;
+	spinlock_t lock;
+	unsigned long count;
+};
+
+static DEFINE_PER_CPU(struct ipi_message_queue, ipi_msg_queue);
+
+static void ipi_cpu_stop(unsigned int cpu)
+{
+	spin_lock(&stop_lock);
+	printk(KERN_CRIT "CPU%u: stopping\n", cpu);
+	dump_stack();
+	spin_unlock(&stop_lock);
+
+	cpu_clear(cpu, cpu_online_map);
+
+	local_irq_disable();
+
+	while (1)
+		SSYNC();
+}
+
+static void ipi_flush_icache(void *info)
+{
+	struct blackfin_flush_data *fdata = info;
+
+	/* Invalidate the memory holding the bounds of the flushed region. */
+	blackfin_dcache_invalidate_range((unsigned long)fdata,
+					 (unsigned long)fdata + sizeof(*fdata));
+
+	blackfin_icache_flush_range(fdata->start, fdata->end);
+}
+
+static void ipi_call_function(unsigned int cpu, struct ipi_message *msg)
+{
+	int wait;
+	void (*func)(void *info);
+	void *info;
+	func = msg->call_struct.func;
+	info = msg->call_struct.info;
+	wait = msg->call_struct.wait;
+	cpu_clear(cpu, msg->call_struct.pending);
+	func(info);
+	if (wait)
+		cpu_clear(cpu, msg->call_struct.waitmask);
+	else
+		kfree(msg);
+}
+
+static irqreturn_t ipi_handler(int irq, void *dev_instance)
+{
+	struct ipi_message *msg, *mg;
+	struct ipi_message_queue *msg_queue;
+	unsigned int cpu = smp_processor_id();
+
+	platform_clear_ipi(cpu);
+
+	msg_queue = &__get_cpu_var(ipi_msg_queue);
+	msg_queue->count++;
+
+	spin_lock(&msg_queue->lock);
+	list_for_each_entry_safe(msg, mg, &msg_queue->head, list) {
+		list_del(&msg->list);
+		switch (msg->type) {
+		case BFIN_IPI_RESCHEDULE:
+			/* That's the easiest one; leave it to
+			 * return_from_int. */
+			kfree(msg);
+			break;
+		case BFIN_IPI_CALL_FUNC:
+			ipi_call_function(cpu, msg);
+			break;
+		case BFIN_IPI_CPU_STOP:
+			ipi_cpu_stop(cpu);
+			kfree(msg);
+			break;
+		default:
+			printk(KERN_CRIT "CPU%u: Unknown IPI message \
+			0x%lx\n", cpu, msg->type);
+			kfree(msg);
+			break;
+		}
+	}
+	spin_unlock(&msg_queue->lock);
+	return IRQ_HANDLED;
+}
+
+static void ipi_queue_init(void)
+{
+	unsigned int cpu;
+	struct ipi_message_queue *msg_queue;
+	for_each_possible_cpu(cpu) {
+		msg_queue = &per_cpu(ipi_msg_queue, cpu);
+		INIT_LIST_HEAD(&msg_queue->head);
+		spin_lock_init(&msg_queue->lock);
+		msg_queue->count = 0;
+	}
+}
+
+int smp_call_function(void (*func)(void *info), void *info, int wait)
+{
+	unsigned int cpu;
+	cpumask_t callmap;
+	unsigned long flags;
+	struct ipi_message_queue *msg_queue;
+	struct ipi_message *msg;
+
+	callmap = cpu_online_map;
+	cpu_clear(smp_processor_id(), callmap);
+	if (cpus_empty(callmap))
+		return 0;
+
+	msg = kmalloc(sizeof(*msg), GFP_ATOMIC);
+	INIT_LIST_HEAD(&msg->list);
+	msg->call_struct.func = func;
+	msg->call_struct.info = info;
+	msg->call_struct.wait = wait;
+	msg->call_struct.pending = callmap;
+	msg->call_struct.waitmask = callmap;
+	msg->type = BFIN_IPI_CALL_FUNC;
+
+	for_each_cpu_mask(cpu, callmap) {
+		msg_queue = &per_cpu(ipi_msg_queue, cpu);
+		spin_lock_irqsave(&msg_queue->lock, flags);
+		list_add(&msg->list, &msg_queue->head);
+		spin_unlock_irqrestore(&msg_queue->lock, flags);
+		platform_send_ipi_cpu(cpu);
+	}
+	if (wait) {
+		while (!cpus_empty(msg->call_struct.waitmask))
+			blackfin_dcache_invalidate_range(
+				(unsigned long)(&msg->call_struct.waitmask),
+				(unsigned long)(&msg->call_struct.waitmask));
+		kfree(msg);
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(smp_call_function);
+
+int smp_call_function_single(int cpuid, void (*func) (void *info), void *info,
+				int wait)
+{
+	unsigned int cpu = cpuid;
+	cpumask_t callmap;
+	unsigned long flags;
+	struct ipi_message_queue *msg_queue;
+	struct ipi_message *msg;
+
+	if (cpu_is_offline(cpu))
+		return 0;
+	cpus_clear(callmap);
+	cpu_set(cpu, callmap);
+
+	msg = kmalloc(sizeof(*msg), GFP_ATOMIC);
+	INIT_LIST_HEAD(&msg->list);
+	msg->call_struct.func = func;
+	msg->call_struct.info = info;
+	msg->call_struct.wait = wait;
+	msg->call_struct.pending = callmap;
+	msg->call_struct.waitmask = callmap;
+	msg->type = BFIN_IPI_CALL_FUNC;
+
+	msg_queue = &per_cpu(ipi_msg_queue, cpu);
+	spin_lock_irqsave(&msg_queue->lock, flags);
+	list_add(&msg->list, &msg_queue->head);
+	spin_unlock_irqrestore(&msg_queue->lock, flags);
+	platform_send_ipi_cpu(cpu);
+
+	if (wait) {
+		while (!cpus_empty(msg->call_struct.waitmask))
+			blackfin_dcache_invalidate_range(
+				(unsigned long)(&msg->call_struct.waitmask),
+				(unsigned long)(&msg->call_struct.waitmask));
+		kfree(msg);
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(smp_call_function_single);
+
+void smp_send_reschedule(int cpu)
+{
+	unsigned long flags;
+	struct ipi_message_queue *msg_queue;
+	struct ipi_message *msg;
+
+	if (cpu_is_offline(cpu))
+		return;
+
+	msg = kmalloc(sizeof(*msg), GFP_ATOMIC);
+	memset(msg, 0, sizeof(msg));
+	INIT_LIST_HEAD(&msg->list);
+	msg->type = BFIN_IPI_RESCHEDULE;
+
+	msg_queue = &per_cpu(ipi_msg_queue, cpu);
+	spin_lock_irqsave(&msg_queue->lock, flags);
+	list_add(&msg->list, &msg_queue->head);
+	spin_unlock_irqrestore(&msg_queue->lock, flags);
+	platform_send_ipi_cpu(cpu);
+
+	return;
+}
+
+void smp_send_stop(void)
+{
+	unsigned int cpu;
+	cpumask_t callmap;
+	unsigned long flags;
+	struct ipi_message_queue *msg_queue;
+	struct ipi_message *msg;
+
+	callmap = cpu_online_map;
+	cpu_clear(smp_processor_id(), callmap);
+	if (cpus_empty(callmap))
+		return;
+
+	msg = kmalloc(sizeof(*msg), GFP_ATOMIC);
+	memset(msg, 0, sizeof(msg));
+	INIT_LIST_HEAD(&msg->list);
+	msg->type = BFIN_IPI_CPU_STOP;
+
+	for_each_cpu_mask(cpu, callmap) {
+		msg_queue = &per_cpu(ipi_msg_queue, cpu);
+		spin_lock_irqsave(&msg_queue->lock, flags);
+		list_add(&msg->list, &msg_queue->head);
+		spin_unlock_irqrestore(&msg_queue->lock, flags);
+		platform_send_ipi_cpu(cpu);
+	}
+	return;
+}
+
+int __cpuinit __cpu_up(unsigned int cpu)
+{
+	struct task_struct *idle;
+	int ret;
+
+	idle = fork_idle(cpu);
+	if (IS_ERR(idle)) {
+		printk(KERN_ERR "CPU%u: fork() failed\n", cpu);
+		return PTR_ERR(idle);
+	}
+
+	secondary_stack = task_stack_page(idle) + THREAD_SIZE;
+	smp_wmb();
+
+	ret = platform_boot_secondary(cpu, idle);
+
+	if (ret) {
+		cpu_clear(cpu, cpu_present_map);
+		printk(KERN_CRIT "CPU%u: processor failed to boot (%d)\n", cpu, ret);
+		free_task(idle);
+	} else
+		cpu_set(cpu, cpu_online_map);
+
+	secondary_stack = NULL;
+
+	return ret;
+}
+
+static void __cpuinit setup_secondary(unsigned int cpu)
+{
+#if !(defined(CONFIG_TICK_SOURCE_SYSTMR0) || defined(CONFIG_IPIPE))
+	struct irq_desc *timer_desc;
+#endif
+	unsigned long ilat;
+
+	bfin_write_IMASK(0);
+	CSYNC();
+	ilat = bfin_read_ILAT();
+	CSYNC();
+	bfin_write_ILAT(ilat);
+	CSYNC();
+
+	/* Reserve the PDA space for the secondary CPU. */
+	reserve_pda();
+
+	/* Enable interrupt levels IVG7-15. IARs have been already
+	 * programmed by the boot CPU.  */
+	bfin_irq_flags |= IMASK_IVG15 |
+	    IMASK_IVG14 | IMASK_IVG13 | IMASK_IVG12 | IMASK_IVG11 |
+	    IMASK_IVG10 | IMASK_IVG9 | IMASK_IVG8 | IMASK_IVG7 | IMASK_IVGHW;
+
+#if defined(CONFIG_TICK_SOURCE_SYSTMR0) || defined(CONFIG_IPIPE)
+	/* Power down the core timer, just to play safe. */
+	bfin_write_TCNTL(0);
+
+	/* system timer0 has been setup by CoreA. */
+#else
+	timer_desc = irq_desc + IRQ_CORETMR;
+	setup_core_timer();
+	timer_desc->chip->enable(IRQ_CORETMR);
+#endif
+}
+
+void __cpuinit secondary_start_kernel(void)
+{
+	unsigned int cpu = smp_processor_id();
+	struct mm_struct *mm = &init_mm;
+
+	if (_bfin_swrst & SWRST_DBL_FAULT_B) {
+		printk(KERN_EMERG "CoreB Recovering from DOUBLE FAULT event\n");
+#ifdef CONFIG_DEBUG_DOUBLEFAULT
+		printk(KERN_EMERG " While handling exception (EXCAUSE = 0x%x) at %pF\n",
+			(int)init_saved_seqstat_coreb & SEQSTAT_EXCAUSE, init_saved_retx_coreb);
+		printk(KERN_NOTICE "   DCPLB_FAULT_ADDR: %pF\n", init_saved_dcplb_fault_addr_coreb);
+		printk(KERN_NOTICE "   ICPLB_FAULT_ADDR: %pF\n", init_saved_icplb_fault_addr_coreb);
+#endif
+		printk(KERN_NOTICE " The instruction at %pF caused a double exception\n",
+			init_retx_coreb);
+	}
+
+	/*
+	 * We want the D-cache to be enabled early, in case the atomic
+	 * support code emulates cache coherence (see
+	 * __ARCH_SYNC_CORE_DCACHE).
+	 */
+	init_exception_vectors();
+
+	bfin_setup_caches(cpu);
+
+	local_irq_disable();
+
+	/* Attach the new idle task to the global mm. */
+	atomic_inc(&mm->mm_users);
+	atomic_inc(&mm->mm_count);
+	current->active_mm = mm;
+	BUG_ON(current->mm);	/* Can't be, but better be safe than sorry. */
+
+	preempt_disable();
+
+	setup_secondary(cpu);
+
+	local_irq_enable();
+
+	platform_secondary_init(cpu);
+
+	cpu_idle();
+}
+
+void __init smp_prepare_boot_cpu(void)
+{
+}
+
+void __init smp_prepare_cpus(unsigned int max_cpus)
+{
+	platform_prepare_cpus(max_cpus);
+	ipi_queue_init();
+	platform_request_ipi(&ipi_handler);
+}
+
+void __init smp_cpus_done(unsigned int max_cpus)
+{
+	unsigned long bogosum = 0;
+	unsigned int cpu;
+
+	for_each_online_cpu(cpu)
+		bogosum += per_cpu(cpu_data, cpu).loops_per_jiffy;
+
+	printk(KERN_INFO "SMP: Total of %d processors activated "
+	       "(%lu.%02lu BogoMIPS).\n",
+	       num_online_cpus(),
+	       bogosum / (500000/HZ),
+	       (bogosum / (5000/HZ)) % 100);
+}
+
+void smp_icache_flush_range_others(unsigned long start, unsigned long end)
+{
+	smp_flush_data.start = start;
+	smp_flush_data.end = end;
+
+	if (smp_call_function(&ipi_flush_icache, &smp_flush_data, 1))
+		printk(KERN_WARNING "SMP: failed to run I-cache flush request on other CPUs\n");
+}
+EXPORT_SYMBOL_GPL(smp_icache_flush_range_others);
+
+#ifdef __ARCH_SYNC_CORE_DCACHE
+unsigned long barrier_mask __attribute__ ((__section__(".l2.bss")));
+
+void resync_core_dcache(void)
+{
+	unsigned int cpu = get_cpu();
+	blackfin_invalidate_entire_dcache();
+	++per_cpu(cpu_data, cpu).dcache_invld_count;
+	put_cpu();
+}
+EXPORT_SYMBOL(resync_core_dcache);
+#endif
diff --git a/arch/blackfin/mm/init.c b/arch/blackfin/mm/init.c
index bc240abb8745..d0532b72bba5 100644
--- a/arch/blackfin/mm/init.c
+++ b/arch/blackfin/mm/init.c
@@ -31,7 +31,8 @@
 #include <linux/bootmem.h>
 #include <linux/uaccess.h>
 #include <asm/bfin-global.h>
-#include <asm/l1layout.h>
+#include <asm/pda.h>
+#include <asm/cplbinit.h>
 #include "blackfin_sram.h"
 
 /*
@@ -53,6 +54,11 @@ static unsigned long empty_bad_page;
 
 unsigned long empty_zero_page;
 
+extern unsigned long exception_stack[NR_CPUS][1024];
+
+struct blackfin_pda cpu_pda[NR_CPUS];
+EXPORT_SYMBOL(cpu_pda);
+
 /*
  * paging_init() continues the virtual memory environment setup which
  * was begun by the code in arch/head.S.
@@ -98,6 +104,32 @@ void __init paging_init(void)
 	}
 }
 
+asmlinkage void init_pda(void)
+{
+	unsigned int cpu = raw_smp_processor_id();
+
+	/* Initialize the PDA fields holding references to other parts
+	   of the memory. The content of such memory is still
+	   undefined at the time of the call, we are only setting up
+	   valid pointers to it. */
+	memset(&cpu_pda[cpu], 0, sizeof(cpu_pda[cpu]));
+
+	cpu_pda[0].next = &cpu_pda[1];
+	cpu_pda[1].next = &cpu_pda[0];
+
+	cpu_pda[cpu].ex_stack = exception_stack[cpu + 1];
+
+#ifdef CONFIG_SMP
+	cpu_pda[cpu].imask = 0x1f;
+#endif
+}
+
+void __cpuinit reserve_pda(void)
+{
+	printk(KERN_INFO "PDA for CPU%u reserved at %p\n", smp_processor_id(),
+					&cpu_pda[smp_processor_id()]);
+}
+
 void __init mem_init(void)
 {
 	unsigned int codek = 0, datak = 0, initk = 0;
@@ -141,21 +173,13 @@ void __init mem_init(void)
 
 static int __init sram_init(void)
 {
-	unsigned long tmp;
-
 	/* Initialize the blackfin L1 Memory. */
 	bfin_sram_init();
 
-	/* Allocate this once; never free it.  We assume this gives us a
-	   pointer to the start of L1 scratchpad memory; panic if it
-	   doesn't.  */
-	tmp = (unsigned long)l1sram_alloc(sizeof(struct l1_scratch_task_info));
-	if (tmp != (unsigned long)L1_SCRATCH_TASK_INFO) {
-		printk(KERN_EMERG "mem_init(): Did not get the right address from l1sram_alloc: %08lx != %08lx\n",
-			tmp, (unsigned long)L1_SCRATCH_TASK_INFO);
-		panic("No L1, time to give up\n");
-	}
-
+	/* Reserve the PDA space for the boot CPU right after we
+	 * initialized the scratch memory allocator.
+	 */
+	reserve_pda();
 	return 0;
 }
 pure_initcall(sram_init);
diff --git a/arch/blackfin/mm/sram-alloc.c b/arch/blackfin/mm/sram-alloc.c
index cc6f336e7313..834cab7438a8 100644
--- a/arch/blackfin/mm/sram-alloc.c
+++ b/arch/blackfin/mm/sram-alloc.c
@@ -39,10 +39,13 @@
 #include <linux/spinlock.h>
 #include <linux/rtc.h>
 #include <asm/blackfin.h>
+#include <asm/mem_map.h>
 #include "blackfin_sram.h"
 
-static spinlock_t l1sram_lock, l1_data_sram_lock, l1_inst_sram_lock;
-static spinlock_t l2_sram_lock;
+static DEFINE_PER_CPU(spinlock_t, l1sram_lock) ____cacheline_aligned_in_smp;
+static DEFINE_PER_CPU(spinlock_t, l1_data_sram_lock) ____cacheline_aligned_in_smp;
+static DEFINE_PER_CPU(spinlock_t, l1_inst_sram_lock) ____cacheline_aligned_in_smp;
+static spinlock_t l2_sram_lock ____cacheline_aligned_in_smp;
 
 /* the data structure for L1 scratchpad and DATA SRAM */
 struct sram_piece {
@@ -52,18 +55,22 @@ struct sram_piece {
 	struct sram_piece *next;
 };
 
-static struct sram_piece free_l1_ssram_head, used_l1_ssram_head;
+static DEFINE_PER_CPU(struct sram_piece, free_l1_ssram_head);
+static DEFINE_PER_CPU(struct sram_piece, used_l1_ssram_head);
 
 #if L1_DATA_A_LENGTH != 0
-static struct sram_piece free_l1_data_A_sram_head, used_l1_data_A_sram_head;
+static DEFINE_PER_CPU(struct sram_piece, free_l1_data_A_sram_head);
+static DEFINE_PER_CPU(struct sram_piece, used_l1_data_A_sram_head);
 #endif
 
 #if L1_DATA_B_LENGTH != 0
-static struct sram_piece free_l1_data_B_sram_head, used_l1_data_B_sram_head;
+static DEFINE_PER_CPU(struct sram_piece, free_l1_data_B_sram_head);
+static DEFINE_PER_CPU(struct sram_piece, used_l1_data_B_sram_head);
 #endif
 
 #if L1_CODE_LENGTH != 0
-static struct sram_piece free_l1_inst_sram_head, used_l1_inst_sram_head;
+static DEFINE_PER_CPU(struct sram_piece, free_l1_inst_sram_head);
+static DEFINE_PER_CPU(struct sram_piece, used_l1_inst_sram_head);
 #endif
 
 #if L2_LENGTH != 0
@@ -75,102 +82,117 @@ static struct kmem_cache *sram_piece_cache;
 /* L1 Scratchpad SRAM initialization function */
 static void __init l1sram_init(void)
 {
-	free_l1_ssram_head.next =
-		kmem_cache_alloc(sram_piece_cache, GFP_KERNEL);
-	if (!free_l1_ssram_head.next) {
-		printk(KERN_INFO "Failed to initialize Scratchpad data SRAM\n");
-		return;
+	unsigned int cpu;
+	for (cpu = 0; cpu < num_possible_cpus(); ++cpu) {
+		per_cpu(free_l1_ssram_head, cpu).next =
+			kmem_cache_alloc(sram_piece_cache, GFP_KERNEL);
+		if (!per_cpu(free_l1_ssram_head, cpu).next) {
+			printk(KERN_INFO "Fail to initialize Scratchpad data SRAM.\n");
+			return;
+		}
+
+		per_cpu(free_l1_ssram_head, cpu).next->paddr = (void *)get_l1_scratch_start_cpu(cpu);
+		per_cpu(free_l1_ssram_head, cpu).next->size = L1_SCRATCH_LENGTH;
+		per_cpu(free_l1_ssram_head, cpu).next->pid = 0;
+		per_cpu(free_l1_ssram_head, cpu).next->next = NULL;
+
+		per_cpu(used_l1_ssram_head, cpu).next = NULL;
+
+		/* mutex initialize */
+		spin_lock_init(&per_cpu(l1sram_lock, cpu));
+		printk(KERN_INFO "Blackfin Scratchpad data SRAM: %d KB\n",
+			L1_SCRATCH_LENGTH >> 10);
 	}
-
-	free_l1_ssram_head.next->paddr = (void *)L1_SCRATCH_START;
-	free_l1_ssram_head.next->size = L1_SCRATCH_LENGTH;
-	free_l1_ssram_head.next->pid = 0;
-	free_l1_ssram_head.next->next = NULL;
-
-	used_l1_ssram_head.next = NULL;
-
-	/* mutex initialize */
-	spin_lock_init(&l1sram_lock);
-
-	printk(KERN_INFO "Blackfin Scratchpad data SRAM: %d KB\n",
-	       L1_SCRATCH_LENGTH >> 10);
 }
 
 static void __init l1_data_sram_init(void)
 {
+#if L1_DATA_A_LENGTH != 0 || L1_DATA_B_LENGTH != 0
+	unsigned int cpu;
+#endif
 #if L1_DATA_A_LENGTH != 0
-	free_l1_data_A_sram_head.next =
-		kmem_cache_alloc(sram_piece_cache, GFP_KERNEL);
-	if (!free_l1_data_A_sram_head.next) {
-		printk(KERN_INFO "Failed to initialize L1 Data A SRAM\n");
-		return;
+	for (cpu = 0; cpu < num_possible_cpus(); ++cpu) {
+		per_cpu(free_l1_data_A_sram_head, cpu).next =
+			kmem_cache_alloc(sram_piece_cache, GFP_KERNEL);
+		if (!per_cpu(free_l1_data_A_sram_head, cpu).next) {
+			printk(KERN_INFO "Fail to initialize L1 Data A SRAM.\n");
+			return;
+		}
+
+		per_cpu(free_l1_data_A_sram_head, cpu).next->paddr =
+			(void *)get_l1_data_a_start_cpu(cpu) + (_ebss_l1 - _sdata_l1);
+		per_cpu(free_l1_data_A_sram_head, cpu).next->size =
+			L1_DATA_A_LENGTH - (_ebss_l1 - _sdata_l1);
+		per_cpu(free_l1_data_A_sram_head, cpu).next->pid = 0;
+		per_cpu(free_l1_data_A_sram_head, cpu).next->next = NULL;
+
+		per_cpu(used_l1_data_A_sram_head, cpu).next = NULL;
+
+		printk(KERN_INFO "Blackfin L1 Data A SRAM: %d KB (%d KB free)\n",
+			L1_DATA_A_LENGTH >> 10,
+			per_cpu(free_l1_data_A_sram_head, cpu).next->size >> 10);
 	}
-
-	free_l1_data_A_sram_head.next->paddr =
-		(void *)L1_DATA_A_START + (_ebss_l1 - _sdata_l1);
-	free_l1_data_A_sram_head.next->size =
-		L1_DATA_A_LENGTH - (_ebss_l1 - _sdata_l1);
-	free_l1_data_A_sram_head.next->pid = 0;
-	free_l1_data_A_sram_head.next->next = NULL;
-
-	used_l1_data_A_sram_head.next = NULL;
-
-	printk(KERN_INFO "Blackfin L1 Data A SRAM: %d KB (%d KB free)\n",
-		L1_DATA_A_LENGTH >> 10,
-		free_l1_data_A_sram_head.next->size >> 10);
 #endif
 #if L1_DATA_B_LENGTH != 0
-	free_l1_data_B_sram_head.next =
-		kmem_cache_alloc(sram_piece_cache, GFP_KERNEL);
-	if (!free_l1_data_B_sram_head.next) {
-		printk(KERN_INFO "Failed to initialize L1 Data B SRAM\n");
-		return;
+	for (cpu = 0; cpu < num_possible_cpus(); ++cpu) {
+		per_cpu(free_l1_data_B_sram_head, cpu).next =
+			kmem_cache_alloc(sram_piece_cache, GFP_KERNEL);
+		if (!per_cpu(free_l1_data_B_sram_head, cpu).next) {
+			printk(KERN_INFO "Fail to initialize L1 Data B SRAM.\n");
+			return;
+		}
+
+		per_cpu(free_l1_data_B_sram_head, cpu).next->paddr =
+			(void *)get_l1_data_b_start_cpu(cpu) + (_ebss_b_l1 - _sdata_b_l1);
+		per_cpu(free_l1_data_B_sram_head, cpu).next->size =
+			L1_DATA_B_LENGTH - (_ebss_b_l1 - _sdata_b_l1);
+		per_cpu(free_l1_data_B_sram_head, cpu).next->pid = 0;
+		per_cpu(free_l1_data_B_sram_head, cpu).next->next = NULL;
+
+		per_cpu(used_l1_data_B_sram_head, cpu).next = NULL;
+
+		printk(KERN_INFO "Blackfin L1 Data B SRAM: %d KB (%d KB free)\n",
+			L1_DATA_B_LENGTH >> 10,
+			per_cpu(free_l1_data_B_sram_head, cpu).next->size >> 10);
+		/* mutex initialize */
 	}
-
-	free_l1_data_B_sram_head.next->paddr =
-		(void *)L1_DATA_B_START + (_ebss_b_l1 - _sdata_b_l1);
-	free_l1_data_B_sram_head.next->size =
-		L1_DATA_B_LENGTH - (_ebss_b_l1 - _sdata_b_l1);
-	free_l1_data_B_sram_head.next->pid = 0;
-	free_l1_data_B_sram_head.next->next = NULL;
-
-	used_l1_data_B_sram_head.next = NULL;
-
-	printk(KERN_INFO "Blackfin L1 Data B SRAM: %d KB (%d KB free)\n",
-		L1_DATA_B_LENGTH >> 10,
-		free_l1_data_B_sram_head.next->size >> 10);
 #endif
 
-	/* mutex initialize */
-	spin_lock_init(&l1_data_sram_lock);
+#if L1_DATA_A_LENGTH != 0 || L1_DATA_B_LENGTH != 0
+	for (cpu = 0; cpu < num_possible_cpus(); ++cpu)
+		spin_lock_init(&per_cpu(l1_data_sram_lock, cpu));
+#endif
 }
 
 static void __init l1_inst_sram_init(void)
 {
 #if L1_CODE_LENGTH != 0
-	free_l1_inst_sram_head.next =
-		kmem_cache_alloc(sram_piece_cache, GFP_KERNEL);
-	if (!free_l1_inst_sram_head.next) {
-		printk(KERN_INFO "Failed to initialize L1 Instruction SRAM\n");
-		return;
+	unsigned int cpu;
+	for (cpu = 0; cpu < num_possible_cpus(); ++cpu) {
+		per_cpu(free_l1_inst_sram_head, cpu).next =
+			kmem_cache_alloc(sram_piece_cache, GFP_KERNEL);
+		if (!per_cpu(free_l1_inst_sram_head, cpu).next) {
+			printk(KERN_INFO "Failed to initialize L1 Instruction SRAM\n");
+			return;
+		}
+
+		per_cpu(free_l1_inst_sram_head, cpu).next->paddr =
+			(void *)get_l1_code_start_cpu(cpu) + (_etext_l1 - _stext_l1);
+		per_cpu(free_l1_inst_sram_head, cpu).next->size =
+			L1_CODE_LENGTH - (_etext_l1 - _stext_l1);
+		per_cpu(free_l1_inst_sram_head, cpu).next->pid = 0;
+		per_cpu(free_l1_inst_sram_head, cpu).next->next = NULL;
+
+		per_cpu(used_l1_inst_sram_head, cpu).next = NULL;
+
+		printk(KERN_INFO "Blackfin L1 Instruction SRAM: %d KB (%d KB free)\n",
+			L1_CODE_LENGTH >> 10,
+			per_cpu(free_l1_inst_sram_head, cpu).next->size >> 10);
+
+		/* mutex initialize */
+		spin_lock_init(&per_cpu(l1_inst_sram_lock, cpu));
 	}
-
-	free_l1_inst_sram_head.next->paddr =
-		(void *)L1_CODE_START + (_etext_l1 - _stext_l1);
-	free_l1_inst_sram_head.next->size =
-		L1_CODE_LENGTH - (_etext_l1 - _stext_l1);
-	free_l1_inst_sram_head.next->pid = 0;
-	free_l1_inst_sram_head.next->next = NULL;
-
-	used_l1_inst_sram_head.next = NULL;
-
-	printk(KERN_INFO "Blackfin L1 Instruction SRAM: %d KB (%d KB free)\n",
-		L1_CODE_LENGTH >> 10,
-		free_l1_inst_sram_head.next->size >> 10);
 #endif
-
-	/* mutex initialize */
-	spin_lock_init(&l1_inst_sram_lock);
 }
 
 static void __init l2_sram_init(void)
@@ -179,7 +201,7 @@ static void __init l2_sram_init(void)
 	free_l2_sram_head.next =
 		kmem_cache_alloc(sram_piece_cache, GFP_KERNEL);
 	if (!free_l2_sram_head.next) {
-		printk(KERN_INFO "Failed to initialize L2 SRAM\n");
+		printk(KERN_INFO "Fail to initialize L2 SRAM.\n");
 		return;
 	}
 
@@ -200,6 +222,7 @@ static void __init l2_sram_init(void)
 	/* mutex initialize */
 	spin_lock_init(&l2_sram_lock);
 }
+
 void __init bfin_sram_init(void)
 {
 	sram_piece_cache = kmem_cache_create("sram_piece_cache",
@@ -353,20 +376,20 @@ int sram_free(const void *addr)
 {
 
 #if L1_CODE_LENGTH != 0
-	if (addr >= (void *)L1_CODE_START
-		 && addr < (void *)(L1_CODE_START + L1_CODE_LENGTH))
+	if (addr >= (void *)get_l1_code_start()
+		 && addr < (void *)(get_l1_code_start() + L1_CODE_LENGTH))
 		return l1_inst_sram_free(addr);
 	else
 #endif
 #if L1_DATA_A_LENGTH != 0
-	if (addr >= (void *)L1_DATA_A_START
-		 && addr < (void *)(L1_DATA_A_START + L1_DATA_A_LENGTH))
+	if (addr >= (void *)get_l1_data_a_start()
+		 && addr < (void *)(get_l1_data_a_start() + L1_DATA_A_LENGTH))
 		return l1_data_A_sram_free(addr);
 	else
 #endif
 #if L1_DATA_B_LENGTH != 0
-	if (addr >= (void *)L1_DATA_B_START
-		 && addr < (void *)(L1_DATA_B_START + L1_DATA_B_LENGTH))
+	if (addr >= (void *)get_l1_data_b_start()
+		 && addr < (void *)(get_l1_data_b_start() + L1_DATA_B_LENGTH))
 		return l1_data_B_sram_free(addr);
 	else
 #endif
@@ -384,17 +407,20 @@ void *l1_data_A_sram_alloc(size_t size)
 {
 	unsigned long flags;
 	void *addr = NULL;
+	unsigned int cpu;
 
+	cpu = get_cpu();
 	/* add mutex operation */
-	spin_lock_irqsave(&l1_data_sram_lock, flags);
+	spin_lock_irqsave(&per_cpu(l1_data_sram_lock, cpu), flags);
 
 #if L1_DATA_A_LENGTH != 0
-	addr = _sram_alloc(size, &free_l1_data_A_sram_head,
-			&used_l1_data_A_sram_head);
+	addr = _sram_alloc(size, &per_cpu(free_l1_data_A_sram_head, cpu),
+			&per_cpu(used_l1_data_A_sram_head, cpu));
 #endif
 
 	/* add mutex operation */
-	spin_unlock_irqrestore(&l1_data_sram_lock, flags);
+	spin_unlock_irqrestore(&per_cpu(l1_data_sram_lock, cpu), flags);
+	put_cpu();
 
 	pr_debug("Allocated address in l1_data_A_sram_alloc is 0x%lx+0x%lx\n",
 		 (long unsigned int)addr, size);
@@ -407,19 +433,22 @@ int l1_data_A_sram_free(const void *addr)
 {
 	unsigned long flags;
 	int ret;
+	unsigned int cpu;
 
+	cpu = get_cpu();
 	/* add mutex operation */
-	spin_lock_irqsave(&l1_data_sram_lock, flags);
+	spin_lock_irqsave(&per_cpu(l1_data_sram_lock, cpu), flags);
 
 #if L1_DATA_A_LENGTH != 0
-	ret = _sram_free(addr, &free_l1_data_A_sram_head,
-			&used_l1_data_A_sram_head);
+	ret = _sram_free(addr, &per_cpu(free_l1_data_A_sram_head, cpu),
+			&per_cpu(used_l1_data_A_sram_head, cpu));
 #else
 	ret = -1;
 #endif
 
 	/* add mutex operation */
-	spin_unlock_irqrestore(&l1_data_sram_lock, flags);
+	spin_unlock_irqrestore(&per_cpu(l1_data_sram_lock, cpu), flags);
+	put_cpu();
 
 	return ret;
 }
@@ -430,15 +459,18 @@ void *l1_data_B_sram_alloc(size_t size)
 #if L1_DATA_B_LENGTH != 0
 	unsigned long flags;
 	void *addr;
+	unsigned int cpu;
 
+	cpu = get_cpu();
 	/* add mutex operation */
-	spin_lock_irqsave(&l1_data_sram_lock, flags);
+	spin_lock_irqsave(&per_cpu(l1_data_sram_lock, cpu), flags);
 
-	addr = _sram_alloc(size, &free_l1_data_B_sram_head,
-			&used_l1_data_B_sram_head);
+	addr = _sram_alloc(size, &per_cpu(free_l1_data_B_sram_head, cpu),
+			&per_cpu(used_l1_data_B_sram_head, cpu));
 
 	/* add mutex operation */
-	spin_unlock_irqrestore(&l1_data_sram_lock, flags);
+	spin_unlock_irqrestore(&per_cpu(l1_data_sram_lock, cpu), flags);
+	put_cpu();
 
 	pr_debug("Allocated address in l1_data_B_sram_alloc is 0x%lx+0x%lx\n",
 		 (long unsigned int)addr, size);
@@ -455,15 +487,18 @@ int l1_data_B_sram_free(const void *addr)
 #if L1_DATA_B_LENGTH != 0
 	unsigned long flags;
 	int ret;
+	unsigned int cpu;
 
+	cpu = get_cpu();
 	/* add mutex operation */
-	spin_lock_irqsave(&l1_data_sram_lock, flags);
+	spin_lock_irqsave(&per_cpu(l1_data_sram_lock, cpu), flags);
 
-	ret = _sram_free(addr, &free_l1_data_B_sram_head,
-			&used_l1_data_B_sram_head);
+	ret = _sram_free(addr, &per_cpu(free_l1_data_B_sram_head, cpu),
+			&per_cpu(used_l1_data_B_sram_head, cpu));
 
 	/* add mutex operation */
-	spin_unlock_irqrestore(&l1_data_sram_lock, flags);
+	spin_unlock_irqrestore(&per_cpu(l1_data_sram_lock, cpu), flags);
+	put_cpu();
 
 	return ret;
 #else
@@ -509,15 +544,18 @@ void *l1_inst_sram_alloc(size_t size)
 #if L1_CODE_LENGTH != 0
 	unsigned long flags;
 	void *addr;
+	unsigned int cpu;
 
+	cpu = get_cpu();
 	/* add mutex operation */
-	spin_lock_irqsave(&l1_inst_sram_lock, flags);
+	spin_lock_irqsave(&per_cpu(l1_inst_sram_lock, cpu), flags);
 
-	addr = _sram_alloc(size, &free_l1_inst_sram_head,
-			&used_l1_inst_sram_head);
+	addr = _sram_alloc(size, &per_cpu(free_l1_inst_sram_head, cpu),
+			&per_cpu(used_l1_inst_sram_head, cpu));
 
 	/* add mutex operation */
-	spin_unlock_irqrestore(&l1_inst_sram_lock, flags);
+	spin_unlock_irqrestore(&per_cpu(l1_inst_sram_lock, cpu), flags);
+	put_cpu();
 
 	pr_debug("Allocated address in l1_inst_sram_alloc is 0x%lx+0x%lx\n",
 		 (long unsigned int)addr, size);
@@ -534,15 +572,18 @@ int l1_inst_sram_free(const void *addr)
 #if L1_CODE_LENGTH != 0
 	unsigned long flags;
 	int ret;
+	unsigned int cpu;
 
+	cpu = get_cpu();
 	/* add mutex operation */
-	spin_lock_irqsave(&l1_inst_sram_lock, flags);
+	spin_lock_irqsave(&per_cpu(l1_inst_sram_lock, cpu), flags);
 
-	ret = _sram_free(addr, &free_l1_inst_sram_head,
-			&used_l1_inst_sram_head);
+	ret = _sram_free(addr, &per_cpu(free_l1_inst_sram_head, cpu),
+			&per_cpu(used_l1_inst_sram_head, cpu));
 
 	/* add mutex operation */
-	spin_unlock_irqrestore(&l1_inst_sram_lock, flags);
+	spin_unlock_irqrestore(&per_cpu(l1_inst_sram_lock, cpu), flags);
+	put_cpu();
 
 	return ret;
 #else
@@ -556,15 +597,18 @@ void *l1sram_alloc(size_t size)
 {
 	unsigned long flags;
 	void *addr;
+	unsigned int cpu;
 
+	cpu = get_cpu();
 	/* add mutex operation */
-	spin_lock_irqsave(&l1sram_lock, flags);
+	spin_lock_irqsave(&per_cpu(l1sram_lock, cpu), flags);
 
-	addr = _sram_alloc(size, &free_l1_ssram_head,
-			&used_l1_ssram_head);
+	addr = _sram_alloc(size, &per_cpu(free_l1_ssram_head, cpu),
+			&per_cpu(used_l1_ssram_head, cpu));
 
 	/* add mutex operation */
-	spin_unlock_irqrestore(&l1sram_lock, flags);
+	spin_unlock_irqrestore(&per_cpu(l1sram_lock, cpu), flags);
+	put_cpu();
 
 	return addr;
 }
@@ -574,15 +618,18 @@ void *l1sram_alloc_max(size_t *psize)
 {
 	unsigned long flags;
 	void *addr;
+	unsigned int cpu;
 
+	cpu = get_cpu();
 	/* add mutex operation */
-	spin_lock_irqsave(&l1sram_lock, flags);
+	spin_lock_irqsave(&per_cpu(l1sram_lock, cpu), flags);
 
-	addr = _sram_alloc_max(&free_l1_ssram_head,
-			&used_l1_ssram_head, psize);
+	addr = _sram_alloc_max(&per_cpu(free_l1_ssram_head, cpu),
+			&per_cpu(used_l1_ssram_head, cpu), psize);
 
 	/* add mutex operation */
-	spin_unlock_irqrestore(&l1sram_lock, flags);
+	spin_unlock_irqrestore(&per_cpu(l1sram_lock, cpu), flags);
+	put_cpu();
 
 	return addr;
 }
@@ -592,15 +639,18 @@ int l1sram_free(const void *addr)
 {
 	unsigned long flags;
 	int ret;
+	unsigned int cpu;
 
+	cpu = get_cpu();
 	/* add mutex operation */
-	spin_lock_irqsave(&l1sram_lock, flags);
+	spin_lock_irqsave(&per_cpu(l1sram_lock, cpu), flags);
 
-	ret = _sram_free(addr, &free_l1_ssram_head,
-			&used_l1_ssram_head);
+	ret = _sram_free(addr, &per_cpu(free_l1_ssram_head, cpu),
+			&per_cpu(used_l1_ssram_head, cpu));
 
 	/* add mutex operation */
-	spin_unlock_irqrestore(&l1sram_lock, flags);
+	spin_unlock_irqrestore(&per_cpu(l1sram_lock, cpu), flags);
+	put_cpu();
 
 	return ret;
 }
@@ -761,33 +811,36 @@ static int sram_proc_read(char *buf, char **start, off_t offset, int count,
 		int *eof, void *data)
 {
 	int len = 0;
+	unsigned int cpu;
 
-	if (_sram_proc_read(buf, &len, count, "Scratchpad",
-			&free_l1_ssram_head, &used_l1_ssram_head))
-		goto not_done;
+	for (cpu = 0; cpu < num_possible_cpus(); ++cpu) {
+		if (_sram_proc_read(buf, &len, count, "Scratchpad",
+			&per_cpu(free_l1_ssram_head, cpu), &per_cpu(used_l1_ssram_head, cpu)))
+			goto not_done;
 #if L1_DATA_A_LENGTH != 0
-	if (_sram_proc_read(buf, &len, count, "L1 Data A",
-			&free_l1_data_A_sram_head,
-			&used_l1_data_A_sram_head))
-		goto not_done;
+		if (_sram_proc_read(buf, &len, count, "L1 Data A",
+			&per_cpu(free_l1_data_A_sram_head, cpu),
+			&per_cpu(used_l1_data_A_sram_head, cpu)))
+			goto not_done;
 #endif
 #if L1_DATA_B_LENGTH != 0
-	if (_sram_proc_read(buf, &len, count, "L1 Data B",
-			&free_l1_data_B_sram_head,
-			&used_l1_data_B_sram_head))
-		goto not_done;
+		if (_sram_proc_read(buf, &len, count, "L1 Data B",
+			&per_cpu(free_l1_data_B_sram_head, cpu),
+			&per_cpu(used_l1_data_B_sram_head, cpu)))
+			goto not_done;
 #endif
 #if L1_CODE_LENGTH != 0
-	if (_sram_proc_read(buf, &len, count, "L1 Instruction",
-			&free_l1_inst_sram_head, &used_l1_inst_sram_head))
-		goto not_done;
+		if (_sram_proc_read(buf, &len, count, "L1 Instruction",
+			&per_cpu(free_l1_inst_sram_head, cpu),
+			&per_cpu(used_l1_inst_sram_head, cpu)))
+			goto not_done;
 #endif
+	}
 #if L2_LENGTH != 0
-	if (_sram_proc_read(buf, &len, count, "L2",
-			&free_l2_sram_head, &used_l2_sram_head))
+	if (_sram_proc_read(buf, &len, count, "L2", &free_l2_sram_head,
+		&used_l2_sram_head))
 		goto not_done;
 #endif
-
 	*eof = 1;
  not_done:
 	return len;
diff --git a/arch/blackfin/oprofile/Makefile b/arch/blackfin/oprofile/Makefile
index 634e300d67e2..c70af3a01297 100644
--- a/arch/blackfin/oprofile/Makefile
+++ b/arch/blackfin/oprofile/Makefile
@@ -10,5 +10,4 @@ DRIVER_OBJS := $(addprefix ../../../drivers/oprofile/, \
 		oprofilefs.o oprofile_stats.o \
 		timer_int.o )
 
-oprofile-y := $(DRIVER_OBJS) common.o
-oprofile-$(CONFIG_HARDWARE_PM) += op_model_bf533.o
+oprofile-y := $(DRIVER_OBJS) bfin_oprofile.o
diff --git a/arch/blackfin/oprofile/bfin_oprofile.c b/arch/blackfin/oprofile/bfin_oprofile.c
new file mode 100644
index 000000000000..c3b9713b23f8
--- /dev/null
+++ b/arch/blackfin/oprofile/bfin_oprofile.c
@@ -0,0 +1,18 @@
+/*
+ * bfin_oprofile.c - Blackfin oprofile code
+ *
+ * Copyright 2004-2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/oprofile.h>
+#include <linux/init.h>
+
+int __init oprofile_arch_init(struct oprofile_operations *ops)
+{
+	return -1;
+}
+
+void oprofile_arch_exit(void)
+{
+}
diff --git a/arch/blackfin/oprofile/common.c b/arch/blackfin/oprofile/common.c
deleted file mode 100644
index 0f6d303a8891..000000000000
--- a/arch/blackfin/oprofile/common.c
+++ /dev/null
@@ -1,168 +0,0 @@
-/*
- * File:         arch/blackfin/oprofile/common.c
- * Based on:     arch/alpha/oprofile/common.c
- * Author:       Anton Blanchard <anton@au.ibm.com>
- *
- * Created:
- * Description:
- *
- * Modified:
- *               Copyright (C) 2004 Anton Blanchard <anton@au.ibm.com>, IBM
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#include <linux/oprofile.h>
-#include <linux/init.h>
-#include <linux/smp.h>
-#include <linux/errno.h>
-#include <linux/mutex.h>
-#include <linux/ptrace.h>
-#include <linux/irq.h>
-#include <linux/io.h>
-
-#include <asm/system.h>
-#include <asm/blackfin.h>
-
-#include "op_blackfin.h"
-
-#define BFIN_533_ID  0xE5040003
-#define BFIN_537_ID  0xE5040002
-
-static int pfmon_enabled;
-static struct mutex pfmon_lock;
-
-struct op_bfin533_model *model;
-
-struct op_counter_config ctr[OP_MAX_COUNTER];
-
-static int op_bfin_setup(void)
-{
-	int ret;
-
-	/* Pre-compute the values to stuff in the hardware registers.  */
-	spin_lock(&oprofilefs_lock);
-	ret = model->reg_setup(ctr);
-	spin_unlock(&oprofilefs_lock);
-
-	return ret;
-}
-
-static void op_bfin_shutdown(void)
-{
-#if 0
-	/* what is the difference between shutdown and stop? */
-#endif
-}
-
-static int op_bfin_start(void)
-{
-	int ret = -EBUSY;
-
-	printk(KERN_INFO "KSDBG:in %s\n", __func__);
-	mutex_lock(&pfmon_lock);
-	if (!pfmon_enabled) {
-		ret = model->start(ctr);
-		pfmon_enabled = !ret;
-	}
-	mutex_unlock(&pfmon_lock);
-
-	return ret;
-}
-
-static void op_bfin_stop(void)
-{
-	mutex_lock(&pfmon_lock);
-	if (pfmon_enabled) {
-		model->stop();
-		pfmon_enabled = 0;
-	}
-	mutex_unlock(&pfmon_lock);
-}
-
-static int op_bfin_create_files(struct super_block *sb, struct dentry *root)
-{
-	int i;
-
-	for (i = 0; i < model->num_counters; ++i) {
-		struct dentry *dir;
-		char buf[3];
-		printk(KERN_INFO "Oprofile: creating files... \n");
-
-		snprintf(buf, sizeof buf, "%d", i);
-		dir = oprofilefs_mkdir(sb, root, buf);
-
-		oprofilefs_create_ulong(sb, dir, "enabled", &ctr[i].enabled);
-		oprofilefs_create_ulong(sb, dir, "event", &ctr[i].event);
-		oprofilefs_create_ulong(sb, dir, "count", &ctr[i].count);
-		/*
-		 * We dont support per counter user/kernel selection, but
-		 * we leave the entries because userspace expects them
-		 */
-		oprofilefs_create_ulong(sb, dir, "kernel", &ctr[i].kernel);
-		oprofilefs_create_ulong(sb, dir, "user", &ctr[i].user);
-		oprofilefs_create_ulong(sb, dir, "unit_mask",
-					&ctr[i].unit_mask);
-	}
-
-	return 0;
-}
-int __init oprofile_arch_init(struct oprofile_operations *ops)
-{
-#ifdef CONFIG_HARDWARE_PM
-	unsigned int dspid;
-
-	mutex_init(&pfmon_lock);
-
-	dspid = bfin_read_DSPID();
-
-	printk(KERN_INFO "Oprofile got the cpu id is 0x%x. \n", dspid);
-
-	switch (dspid) {
-	case BFIN_533_ID:
-		model = &op_model_bfin533;
-		model->num_counters = 2;
-		break;
-	case BFIN_537_ID:
-		model = &op_model_bfin533;
-		model->num_counters = 2;
-		break;
-	default:
-		return -ENODEV;
-	}
-
-	ops->cpu_type = model->name;
-	ops->create_files = op_bfin_create_files;
-	ops->setup = op_bfin_setup;
-	ops->shutdown = op_bfin_shutdown;
-	ops->start = op_bfin_start;
-	ops->stop = op_bfin_stop;
-
-	printk(KERN_INFO "oprofile: using %s performance monitoring.\n",
-	       ops->cpu_type);
-
-	return 0;
-#else
-	return -1;
-#endif
-}
-
-void oprofile_arch_exit(void)
-{
-}
diff --git a/arch/blackfin/oprofile/op_blackfin.h b/arch/blackfin/oprofile/op_blackfin.h
deleted file mode 100644
index 05dd08c9d154..000000000000
--- a/arch/blackfin/oprofile/op_blackfin.h
+++ /dev/null
@@ -1,98 +0,0 @@
-/*
- * File:         arch/blackfin/oprofile/op_blackfin.h
- * Based on:
- * Author:       Anton Blanchard <anton@au.ibm.com>
- *
- * Created:
- * Description:
- *
- * Modified:
- *               Copyright (C) 2004 Anton Blanchard <anton@au.ibm.com>, IBM
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#ifndef OP_BLACKFIN_H
-#define OP_BLACKFIN_H 1
-
-#define OP_MAX_COUNTER 2
-
-#include <asm/blackfin.h>
-
-/* Per-counter configuration as set via oprofilefs.  */
-struct op_counter_config {
-	unsigned long valid;
-	unsigned long enabled;
-	unsigned long event;
-	unsigned long count;
-	unsigned long kernel;
-	unsigned long user;
-	unsigned long unit_mask;
-};
-
-/* System-wide configuration as set via oprofilefs.  */
-struct op_system_config {
-	unsigned long enable_kernel;
-	unsigned long enable_user;
-};
-
-/* Per-arch configuration */
-struct op_bfin533_model {
-	int (*reg_setup) (struct op_counter_config *);
-	int (*start) (struct op_counter_config *);
-	void (*stop) (void);
-	int num_counters;
-	char *name;
-};
-
-extern struct op_bfin533_model op_model_bfin533;
-
-static inline unsigned int ctr_read(void)
-{
-	unsigned int tmp;
-
-	tmp = bfin_read_PFCTL();
-	CSYNC();
-
-	return tmp;
-}
-
-static inline void ctr_write(unsigned int val)
-{
-	bfin_write_PFCTL(val);
-	CSYNC();
-}
-
-static inline void count_read(unsigned int *count)
-{
-	count[0] = bfin_read_PFCNTR0();
-	count[1] = bfin_read_PFCNTR1();
-	CSYNC();
-}
-
-static inline void count_write(unsigned int *count)
-{
-	bfin_write_PFCNTR0(count[0]);
-	bfin_write_PFCNTR1(count[1]);
-	CSYNC();
-}
-
-extern int pm_overflow_handler(int irq, struct pt_regs *regs);
-
-#endif
diff --git a/arch/blackfin/oprofile/op_model_bf533.c b/arch/blackfin/oprofile/op_model_bf533.c
deleted file mode 100644
index d1c698bb9ee5..000000000000
--- a/arch/blackfin/oprofile/op_model_bf533.c
+++ /dev/null
@@ -1,161 +0,0 @@
-/*
- * File:         arch/blackfin/oprofile/op_model_bf533.c
- * Based on:
- * Author:       Anton Blanchard <anton@au.ibm.com>
- *
- * Created:
- * Description:
- *
- * Modified:
- *               Copyright (C) 2004 Anton Blanchard <anton@au.ibm.com>, IBM
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#include <linux/oprofile.h>
-#include <linux/init.h>
-#include <linux/smp.h>
-#include <linux/interrupt.h>
-#include <linux/ptrace.h>
-#include <linux/irq.h>
-#include <linux/io.h>
-#include <asm/system.h>
-#include <asm/processor.h>
-#include <asm/blackfin.h>
-
-#include "op_blackfin.h"
-
-#define PM_ENABLE 0x01;
-#define PM_CTL1_ENABLE  0x18
-#define PM_CTL0_ENABLE  0xC000
-#define COUNT_EDGE_ONLY 0x3000000
-
-static int oprofile_running;
-
-static unsigned curr_pfctl, curr_count[2];
-
-static int bfin533_reg_setup(struct op_counter_config *ctr)
-{
-	unsigned int pfctl = ctr_read();
-	unsigned int count[2];
-
-	/* set Blackfin perf monitor regs with ctr */
-	if (ctr[0].enabled) {
-		pfctl |= (PM_CTL0_ENABLE | ((char)ctr[0].event << 5));
-		count[0] = 0xFFFFFFFF - ctr[0].count;
-		curr_count[0] = count[0];
-	}
-	if (ctr[1].enabled) {
-		pfctl |= (PM_CTL1_ENABLE | ((char)ctr[1].event << 16));
-		count[1] = 0xFFFFFFFF - ctr[1].count;
-		curr_count[1] = count[1];
-	}
-
-	pr_debug("ctr[0].enabled=%d,ctr[1].enabled=%d,ctr[0].event<<5=0x%x,ctr[1].event<<16=0x%x\n", ctr[0].enabled, ctr[1].enabled, ctr[0].event << 5, ctr[1].event << 16);
-	pfctl |= COUNT_EDGE_ONLY;
-	curr_pfctl = pfctl;
-
-	pr_debug("write 0x%x to pfctl\n", pfctl);
-	ctr_write(pfctl);
-	count_write(count);
-
-	return 0;
-}
-
-static int bfin533_start(struct op_counter_config *ctr)
-{
-	unsigned int pfctl = ctr_read();
-
-	pfctl |= PM_ENABLE;
-	curr_pfctl = pfctl;
-
-	ctr_write(pfctl);
-
-	oprofile_running = 1;
-	pr_debug("start oprofile counter \n");
-
-	return 0;
-}
-
-static void bfin533_stop(void)
-{
-	int pfctl;
-
-	pfctl = ctr_read();
-	pfctl &= ~PM_ENABLE;
-	/* freeze counters */
-	ctr_write(pfctl);
-
-	oprofile_running = 0;
-	pr_debug("stop oprofile counter \n");
-}
-
-static int get_kernel(void)
-{
-	int ipend, is_kernel;
-
-	ipend = bfin_read_IPEND();
-
-	/* test bit 15 */
-	is_kernel = ((ipend & 0x8000) != 0);
-
-	return is_kernel;
-}
-
-int pm_overflow_handler(int irq, struct pt_regs *regs)
-{
-	int is_kernel;
-	int i, cpu;
-	unsigned int pc, pfctl;
-	unsigned int count[2];
-
-	pr_debug("get interrupt in %s\n", __func__);
-	if (oprofile_running == 0) {
-		pr_debug("error: entering interrupt when oprofile is stopped.\n\r");
-		return -1;
-	}
-
-	is_kernel = get_kernel();
-	cpu = smp_processor_id();
-	pc = regs->pc;
-	pfctl = ctr_read();
-
-	/* read the two event counter regs */
-	count_read(count);
-
-	/* if the counter overflows, add sample to oprofile buffer */
-	for (i = 0; i < 2; ++i) {
-		if (oprofile_running) {
-			oprofile_add_sample(regs, i);
-		}
-	}
-
-	/* reset the perfmon counter */
-	ctr_write(curr_pfctl);
-	count_write(curr_count);
-	return 0;
-}
-
-struct op_bfin533_model op_model_bfin533 = {
-	.reg_setup = bfin533_reg_setup,
-	.start = bfin533_start,
-	.stop = bfin533_stop,
-	.num_counters = 2,
-	.name = "blackfin/bf533"
-};
diff --git a/arch/blackfin/oprofile/timer_int.c b/arch/blackfin/oprofile/timer_int.c
deleted file mode 100644
index 6c6f8606af4c..000000000000
--- a/arch/blackfin/oprofile/timer_int.c
+++ /dev/null
@@ -1,73 +0,0 @@
-/*
- * File:         arch/blackfin/oprofile/timer_int.c
- * Based on:
- * Author:       Michael Kang
- *
- * Created:
- * Description:
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#include <linux/init.h>
-#include <linux/smp.h>
-#include <linux/irq.h>
-#include <linux/oprofile.h>
-#include <linux/ptrace.h>
-
-static void enable_sys_timer0()
-{
-}
-static void disable_sys_timer0()
-{
-}
-
-static irqreturn_t sys_timer0_int_handler(int irq, void *dev_id,
-					  struct pt_regs *regs)
-{
-	oprofile_add_sample(regs, 0);
-	return IRQ_HANDLED;
-}
-
-static int sys_timer0_start(void)
-{
-	enable_sys_timer0();
-	return request_irq(IVG11, sys_timer0_int_handler, 0, "sys_timer0", NULL);
-}
-
-static void sys_timer0_stop(void)
-{
-	disable_sys_timer();
-}
-
-int __init sys_timer0_init(struct oprofile_operations *ops)
-{
-	extern int nmi_active;
-
-	if (nmi_active <= 0)
-		return -ENODEV;
-
-	ops->start = timer_start;
-	ops->stop = timer_stop;
-	ops->cpu_type = "timer";
-	printk(KERN_INFO "oprofile: using NMI timer interrupt.\n");
-	return 0;
-}
diff --git a/arch/cris/Kconfig b/arch/cris/Kconfig
index b17aeea8d620..3462245fe9fb 100644
--- a/arch/cris/Kconfig
+++ b/arch/cris/Kconfig
@@ -681,6 +681,8 @@ source "drivers/usb/Kconfig"
 
 source "drivers/uwb/Kconfig"
 
+source "drivers/staging/Kconfig"
+
 source "arch/cris/Kconfig.debug"
 
 source "security/Kconfig"
diff --git a/arch/cris/arch-v32/drivers/iop_fw_load.c b/arch/cris/arch-v32/drivers/iop_fw_load.c
index 3b3857ec1f15..2f8ea0f7a63c 100644
--- a/arch/cris/arch-v32/drivers/iop_fw_load.c
+++ b/arch/cris/arch-v32/drivers/iop_fw_load.c
@@ -24,12 +24,12 @@
 #error "Please contact <greg@kroah.com> for details on how to fix it properly."
 
 static struct device iop_spu_device[2] = {
-	{ .bus_id =     "iop-spu0", },
-	{ .bus_id =     "iop-spu1", },
+	{ .init_name =     "iop-spu0", },
+	{ .init_name =     "iop-spu1", },
 };
 
 static struct device iop_mpu_device = {
-	.bus_id =       "iop-mpu",
+	.init_name =       "iop-mpu",
 };
 
 static int wait_mpu_idle(void)
diff --git a/arch/cris/arch-v32/kernel/signal.c b/arch/cris/arch-v32/kernel/signal.c
index da7d2be000ba..372d0ca6efbc 100644
--- a/arch/cris/arch-v32/kernel/signal.c
+++ b/arch/cris/arch-v32/kernel/signal.c
@@ -456,7 +456,7 @@ give_sigsegv:
 	return -EFAULT;
 }
 
-/* Invoke a singal handler to, well, handle the signal. */
+/* Invoke a signal handler to, well, handle the signal. */
 static inline int
 handle_signal(int canrestart, unsigned long sig,
 	      siginfo_t *info, struct k_sigaction *ka,
diff --git a/arch/cris/include/asm/atomic.h b/arch/cris/include/asm/atomic.h
index f71ea686a2ea..5718dd8902a1 100644
--- a/arch/cris/include/asm/atomic.h
+++ b/arch/cris/include/asm/atomic.h
@@ -4,7 +4,7 @@
 #define __ASM_CRIS_ATOMIC__
 
 #include <linux/compiler.h>
-
+#include <linux/types.h>
 #include <asm/system.h>
 #include <arch/atomic.h>
 
@@ -13,8 +13,6 @@
  * resource counting etc..
  */
 
-typedef struct { volatile int counter; } atomic_t;
-
 #define ATOMIC_INIT(i)  { (i) }
 
 #define atomic_read(v) ((v)->counter)
diff --git a/arch/h8300/Kconfig b/arch/h8300/Kconfig
index 28f06fd9b7b7..9420648352b8 100644
--- a/arch/h8300/Kconfig
+++ b/arch/h8300/Kconfig
@@ -220,6 +220,8 @@ source "drivers/uwb/Kconfig"
 
 endmenu
 
+source "drivers/staging/Kconfig"
+
 source "fs/Kconfig"
 
 source "arch/h8300/Kconfig.debug"
diff --git a/arch/h8300/include/asm/Kbuild b/arch/h8300/include/asm/Kbuild
index c68e1680da01..27b108a86b39 100644
--- a/arch/h8300/include/asm/Kbuild
+++ b/arch/h8300/include/asm/Kbuild
@@ -1 +1,2 @@
 include include/asm-generic/Kbuild.asm
+unifdef-y += swab.h
diff --git a/arch/h8300/include/asm/atomic.h b/arch/h8300/include/asm/atomic.h
index b4cf0ea97ede..833186c8dc3b 100644
--- a/arch/h8300/include/asm/atomic.h
+++ b/arch/h8300/include/asm/atomic.h
@@ -1,12 +1,13 @@
 #ifndef __ARCH_H8300_ATOMIC__
 #define __ARCH_H8300_ATOMIC__
 
+#include <linux/types.h>
+
 /*
  * Atomic operations that C can't guarantee us.  Useful for
  * resource counting etc..
  */
 
-typedef struct { int counter; } atomic_t;
 #define ATOMIC_INIT(i)	{ (i) }
 
 #define atomic_read(v)		((v)->counter)
diff --git a/arch/h8300/include/asm/byteorder.h b/arch/h8300/include/asm/byteorder.h
index 36e597d61619..c36b80a3dd84 100644
--- a/arch/h8300/include/asm/byteorder.h
+++ b/arch/h8300/include/asm/byteorder.h
@@ -1,13 +1,7 @@
 #ifndef _H8300_BYTEORDER_H
 #define _H8300_BYTEORDER_H
 
-#include <asm/types.h>
-
-#if defined(__GNUC__) && !defined(__STRICT_ANSI__) || defined(__KERNEL__)
-#  define __BYTEORDER_HAS_U64__
-#  define __SWAB_64_THRU_32__
-#endif
-
+#include <asm/swab.h>
 #include <linux/byteorder/big_endian.h>
 
 #endif /* _H8300_BYTEORDER_H */
diff --git a/arch/h8300/include/asm/swab.h b/arch/h8300/include/asm/swab.h
new file mode 100644
index 000000000000..c108f39b8bc4
--- /dev/null
+++ b/arch/h8300/include/asm/swab.h
@@ -0,0 +1,10 @@
+#ifndef _H8300_SWAB_H
+#define _H8300_SWAB_H
+
+#include <asm/types.h>
+
+#if defined(__GNUC__) && !defined(__STRICT_ANSI__) || defined(__KERNEL__)
+#  define __SWAB_64_THRU_32__
+#endif
+
+#endif /* _H8300_SWAB_H */
diff --git a/arch/ia64/include/asm/Kbuild b/arch/ia64/include/asm/Kbuild
index ccbe8ae47a61..3b25bd9dca91 100644
--- a/arch/ia64/include/asm/Kbuild
+++ b/arch/ia64/include/asm/Kbuild
@@ -14,3 +14,4 @@ unifdef-y += gcc_intrin.h
 unifdef-y += intrinsics.h
 unifdef-y += perfmon.h
 unifdef-y += ustack.h
+unifdef-y += swab.h
diff --git a/arch/ia64/include/asm/atomic.h b/arch/ia64/include/asm/atomic.h
index 50c2b83fd5a0..d37292bd9875 100644
--- a/arch/ia64/include/asm/atomic.h
+++ b/arch/ia64/include/asm/atomic.h
@@ -17,12 +17,6 @@
 #include <asm/intrinsics.h>
 #include <asm/system.h>
 
-/*
- * On IA-64, counter must always be volatile to ensure that that the
- * memory accesses are ordered.
- */
-typedef struct { volatile __s32 counter; } atomic_t;
-typedef struct { volatile __s64 counter; } atomic64_t;
 
 #define ATOMIC_INIT(i)		((atomic_t) { (i) })
 #define ATOMIC64_INIT(i)	((atomic64_t) { (i) })
diff --git a/arch/ia64/include/asm/byteorder.h b/arch/ia64/include/asm/byteorder.h
index 69bd41d7c26e..0f84c5cb703d 100644
--- a/arch/ia64/include/asm/byteorder.h
+++ b/arch/ia64/include/asm/byteorder.h
@@ -1,42 +1,7 @@
 #ifndef _ASM_IA64_BYTEORDER_H
 #define _ASM_IA64_BYTEORDER_H
 
-/*
- * Modified 1998, 1999
- *	David Mosberger-Tang <davidm@hpl.hp.com>, Hewlett-Packard Co.
- */
-
-#include <asm/types.h>
-#include <asm/intrinsics.h>
-#include <linux/compiler.h>
-
-static __inline__ __attribute_const__ __u64
-__ia64_swab64 (__u64 x)
-{
-	__u64 result;
-
-	result = ia64_mux1(x, ia64_mux1_rev);
-	return result;
-}
-
-static __inline__ __attribute_const__ __u32
-__ia64_swab32 (__u32 x)
-{
-	return __ia64_swab64(x) >> 32;
-}
-
-static __inline__ __attribute_const__ __u16
-__ia64_swab16(__u16 x)
-{
-	return __ia64_swab64(x) >> 48;
-}
-
-#define __arch__swab64(x) __ia64_swab64(x)
-#define __arch__swab32(x) __ia64_swab32(x)
-#define __arch__swab16(x) __ia64_swab16(x)
-
-#define __BYTEORDER_HAS_U64__
-
+#include <asm/swab.h>
 #include <linux/byteorder/little_endian.h>
 
 #endif /* _ASM_IA64_BYTEORDER_H */
diff --git a/arch/ia64/include/asm/swab.h b/arch/ia64/include/asm/swab.h
new file mode 100644
index 000000000000..6aa58b699eea
--- /dev/null
+++ b/arch/ia64/include/asm/swab.h
@@ -0,0 +1,34 @@
+#ifndef _ASM_IA64_SWAB_H
+#define _ASM_IA64_SWAB_H
+
+/*
+ * Modified 1998, 1999
+ *	David Mosberger-Tang <davidm@hpl.hp.com>, Hewlett-Packard Co.
+ */
+
+#include <asm/types.h>
+#include <asm/intrinsics.h>
+#include <linux/compiler.h>
+
+static __inline__ __attribute_const__ __u64 __arch_swab64(__u64 x)
+{
+	__u64 result;
+
+	result = ia64_mux1(x, ia64_mux1_rev);
+	return result;
+}
+#define __arch_swab64 __arch_swab64
+
+static __inline__ __attribute_const__ __u32 __arch_swab32(__u32 x)
+{
+	return __arch_swab64(x) >> 32;
+}
+#define __arch_swab32 __arch_swab32
+
+static __inline__ __attribute_const__ __u16 __arch_swab16(__u16 x)
+{
+	return __arch_swab64(x) >> 48;
+}
+#define __arch_swab16 __arch_swab16
+
+#endif /* _ASM_IA64_SWAB_H */
diff --git a/arch/ia64/include/asm/swiotlb.h b/arch/ia64/include/asm/swiotlb.h
index fb79423834d0..dcbaea7ce128 100644
--- a/arch/ia64/include/asm/swiotlb.h
+++ b/arch/ia64/include/asm/swiotlb.h
@@ -2,44 +2,7 @@
 #define ASM_IA64__SWIOTLB_H
 
 #include <linux/dma-mapping.h>
-
-/* SWIOTLB interface */
-
-extern dma_addr_t swiotlb_map_single(struct device *hwdev, void *ptr,
-				     size_t size, int dir);
-extern void *swiotlb_alloc_coherent(struct device *hwdev, size_t size,
-				    dma_addr_t *dma_handle, gfp_t flags);
-extern void swiotlb_unmap_single(struct device *hwdev, dma_addr_t dev_addr,
-				 size_t size, int dir);
-extern void swiotlb_sync_single_for_cpu(struct device *hwdev,
-					dma_addr_t dev_addr,
-					size_t size, int dir);
-extern void swiotlb_sync_single_for_device(struct device *hwdev,
-					   dma_addr_t dev_addr,
-					   size_t size, int dir);
-extern void swiotlb_sync_single_range_for_cpu(struct device *hwdev,
-					      dma_addr_t dev_addr,
-					      unsigned long offset,
-					      size_t size, int dir);
-extern void swiotlb_sync_single_range_for_device(struct device *hwdev,
-						 dma_addr_t dev_addr,
-						 unsigned long offset,
-						 size_t size, int dir);
-extern void swiotlb_sync_sg_for_cpu(struct device *hwdev,
-				    struct scatterlist *sg, int nelems,
-				    int dir);
-extern void swiotlb_sync_sg_for_device(struct device *hwdev,
-				       struct scatterlist *sg, int nelems,
-				       int dir);
-extern int swiotlb_map_sg(struct device *hwdev, struct scatterlist *sg,
-			  int nents, int direction);
-extern void swiotlb_unmap_sg(struct device *hwdev, struct scatterlist *sg,
-			     int nents, int direction);
-extern int swiotlb_dma_mapping_error(struct device *hwdev, dma_addr_t dma_addr);
-extern void swiotlb_free_coherent(struct device *hwdev, size_t size,
-				  void *vaddr, dma_addr_t dma_handle);
-extern int swiotlb_dma_supported(struct device *hwdev, u64 mask);
-extern void swiotlb_init(void);
+#include <linux/swiotlb.h>
 
 extern int swiotlb_force;
 
diff --git a/arch/ia64/kernel/kprobes.c b/arch/ia64/kernel/kprobes.c
index f07688da947c..f90be51b1123 100644
--- a/arch/ia64/kernel/kprobes.c
+++ b/arch/ia64/kernel/kprobes.c
@@ -434,7 +434,7 @@ int __kprobes trampoline_probe_handler(struct kprobe *p, struct pt_regs *regs)
 	/*
 	 * It is possible to have multiple instances associated with a given
 	 * task either because an multiple functions in the call path
-	 * have a return probe installed on them, and/or more then one return
+	 * have a return probe installed on them, and/or more than one return
 	 * return probe was registered for a target function.
 	 *
 	 * We can handle this because:
@@ -670,9 +670,11 @@ void __kprobes arch_disarm_kprobe(struct kprobe *p)
 
 void __kprobes arch_remove_kprobe(struct kprobe *p)
 {
-	mutex_lock(&kprobe_mutex);
-	free_insn_slot(p->ainsn.insn, p->ainsn.inst_flag & INST_FLAG_BOOSTABLE);
-	mutex_unlock(&kprobe_mutex);
+	if (p->ainsn.insn) {
+		free_insn_slot(p->ainsn.insn,
+			       p->ainsn.inst_flag & INST_FLAG_BOOSTABLE);
+		p->ainsn.insn = NULL;
+	}
 }
 /*
  * We are resuming execution after a single step fault, so the pt_regs
diff --git a/arch/ia64/kernel/pci-dma.c b/arch/ia64/kernel/pci-dma.c
index 2a92f637431d..d0ada067a4af 100644
--- a/arch/ia64/kernel/pci-dma.c
+++ b/arch/ia64/kernel/pci-dma.c
@@ -39,7 +39,7 @@ int iommu_detected __read_mostly;
    be probably a smaller DMA mask, but this is bug-to-bug compatible
    to i386. */
 struct device fallback_dev = {
-	.bus_id = "fallback device",
+	.init_name = "fallback device",
 	.coherent_dma_mask = DMA_32BIT_MASK,
 	.dma_mask = &fallback_dev.coherent_dma_mask,
 };
diff --git a/arch/ia64/mm/init.c b/arch/ia64/mm/init.c
index 054bcd9439aa..56e12903973c 100644
--- a/arch/ia64/mm/init.c
+++ b/arch/ia64/mm/init.c
@@ -692,7 +692,7 @@ int arch_add_memory(int nid, u64 start, u64 size)
 	pgdat = NODE_DATA(nid);
 
 	zone = pgdat->node_zones + ZONE_NORMAL;
-	ret = __add_pages(zone, start_pfn, nr_pages);
+	ret = __add_pages(nid, zone, start_pfn, nr_pages);
 
 	if (ret)
 		printk("%s: Problem encountered in __add_pages() as ret=%d\n",
diff --git a/arch/ia64/sn/kernel/tiocx.c b/arch/ia64/sn/kernel/tiocx.c
index a88eba3314d7..3f864238566d 100644
--- a/arch/ia64/sn/kernel/tiocx.c
+++ b/arch/ia64/sn/kernel/tiocx.c
@@ -206,8 +206,7 @@ cx_device_register(nasid_t nasid, int part_num, int mfg_num,
 	cx_dev->dev.parent = NULL;
 	cx_dev->dev.bus = &tiocx_bus_type;
 	cx_dev->dev.release = tiocx_bus_release;
-	snprintf(cx_dev->dev.bus_id, BUS_ID_SIZE, "%d",
-		 cx_dev->cx_id.nasid);
+	dev_set_name(&cx_dev->dev, "%d", cx_dev->cx_id.nasid);
 	device_register(&cx_dev->dev);
 	get_device(&cx_dev->dev);
 
diff --git a/arch/m68k/Kconfig b/arch/m68k/Kconfig
index c825bde17cb3..fb87c08c6b57 100644
--- a/arch/m68k/Kconfig
+++ b/arch/m68k/Kconfig
@@ -303,7 +303,7 @@ config M68KFPU_EMU_EXTRAPREC
 	  correct rounding, the emulator can (often) do the same but this
 	  extra calculation can cost quite some time, so you can disable
 	  it here. The emulator will then "only" calculate with a 64 bit
-	  mantissa and round slightly incorrect, what is more then enough
+	  mantissa and round slightly incorrect, what is more than enough
 	  for normal usage.
 
 config M68KFPU_EMU_ONLY
diff --git a/arch/m68k/kernel/traps.c b/arch/m68k/kernel/traps.c
index 6d813de2baf1..184acc90808d 100644
--- a/arch/m68k/kernel/traps.c
+++ b/arch/m68k/kernel/traps.c
@@ -401,7 +401,7 @@ static inline void do_040writebacks(struct frame *fp)
  * called from sigreturn(), must ensure userspace code didn't
  * manipulate exception frame to circumvent protection, then complete
  * pending writebacks
- * we just clear TM2 to turn it into an userspace access
+ * we just clear TM2 to turn it into a userspace access
  */
 asmlinkage void berr_040cleanup(struct frame *fp)
 {
diff --git a/arch/m68knommu/include/asm/Kbuild b/arch/m68knommu/include/asm/Kbuild
index c68e1680da01..58c02a454130 100644
--- a/arch/m68knommu/include/asm/Kbuild
+++ b/arch/m68knommu/include/asm/Kbuild
@@ -1 +1,3 @@
 include include/asm-generic/Kbuild.asm
+
+unifdef-y += swab.h
diff --git a/arch/m68knommu/include/asm/atomic.h b/arch/m68knommu/include/asm/atomic.h
index d5632a305dae..6bb674855a3f 100644
--- a/arch/m68knommu/include/asm/atomic.h
+++ b/arch/m68knommu/include/asm/atomic.h
@@ -1,6 +1,7 @@
 #ifndef __ARCH_M68KNOMMU_ATOMIC__
 #define __ARCH_M68KNOMMU_ATOMIC__
 
+#include <linux/types.h>
 #include <asm/system.h>
 
 /*
@@ -12,7 +13,6 @@
  * We do not have SMP m68k systems, so we don't have to deal with that.
  */
 
-typedef struct { int counter; } atomic_t;
 #define ATOMIC_INIT(i)	{ (i) }
 
 #define atomic_read(v)		((v)->counter)
diff --git a/arch/m68knommu/include/asm/byteorder.h b/arch/m68knommu/include/asm/byteorder.h
index 20bb4426b610..a6f0b8f7f622 100644
--- a/arch/m68knommu/include/asm/byteorder.h
+++ b/arch/m68knommu/include/asm/byteorder.h
@@ -1,27 +1,7 @@
 #ifndef _M68KNOMMU_BYTEORDER_H
 #define _M68KNOMMU_BYTEORDER_H
 
-#include <linux/types.h>
-
-#if defined(__GNUC__) && !defined(__STRICT_ANSI__) || defined(__KERNEL__)
-#  define __BYTEORDER_HAS_U64__
-#  define __SWAB_64_THRU_32__
-#endif
-
-#if defined (__mcfisaaplus__) || defined (__mcfisac__)
-static inline __attribute_const__ __u32 ___arch__swab32(__u32 val)
-{
-	asm(
-			"byterev %0"
-			: "=d" (val)
-			: "0" (val)
-	   );
-	return val;
-}
-
-#define __arch__swab32(x) ___arch__swab32(x)
-#endif
-
+#include <asm/swab.h>
 #include <linux/byteorder/big_endian.h>
 
 #endif /* _M68KNOMMU_BYTEORDER_H */
diff --git a/arch/m68knommu/include/asm/swab.h b/arch/m68knommu/include/asm/swab.h
new file mode 100644
index 000000000000..e582257db300
--- /dev/null
+++ b/arch/m68knommu/include/asm/swab.h
@@ -0,0 +1,24 @@
+#ifndef _M68KNOMMU_SWAB_H
+#define _M68KNOMMU_SWAB_H
+
+#include <linux/types.h>
+
+#if defined(__GNUC__) && !defined(__STRICT_ANSI__) || defined(__KERNEL__)
+#  define __SWAB_64_THRU_32__
+#endif
+
+#if defined (__mcfisaaplus__) || defined (__mcfisac__)
+static inline __attribute_const__ __u32 __arch_swab32(__u32 val)
+{
+	asm(
+			"byterev %0"
+			: "=d" (val)
+			: "0" (val)
+	   );
+	return val;
+}
+
+#define __arch_swab32 __arch_swab32
+#endif
+
+#endif /* _M68KNOMMU_SWAB_H */
diff --git a/arch/mips/include/asm/Kbuild b/arch/mips/include/asm/Kbuild
index 7897f05e3165..023866c0c102 100644
--- a/arch/mips/include/asm/Kbuild
+++ b/arch/mips/include/asm/Kbuild
@@ -1,3 +1,4 @@
 include include/asm-generic/Kbuild.asm
 
 header-y += cachectl.h sgidefs.h sysmips.h
+header-y += swab.h
diff --git a/arch/mips/include/asm/atomic.h b/arch/mips/include/asm/atomic.h
index 1232be3885b0..c996c3b4d074 100644
--- a/arch/mips/include/asm/atomic.h
+++ b/arch/mips/include/asm/atomic.h
@@ -15,13 +15,12 @@
 #define _ASM_ATOMIC_H
 
 #include <linux/irqflags.h>
+#include <linux/types.h>
 #include <asm/barrier.h>
 #include <asm/cpu-features.h>
 #include <asm/war.h>
 #include <asm/system.h>
 
-typedef struct { volatile int counter; } atomic_t;
-
 #define ATOMIC_INIT(i)    { (i) }
 
 /*
@@ -404,8 +403,6 @@ static __inline__ int atomic_add_unless(atomic_t *v, int a, int u)
 
 #ifdef CONFIG_64BIT
 
-typedef struct { volatile long counter; } atomic64_t;
-
 #define ATOMIC64_INIT(i)    { (i) }
 
 /*
diff --git a/arch/mips/include/asm/byteorder.h b/arch/mips/include/asm/byteorder.h
index 33790b9e0cc0..607b71830707 100644
--- a/arch/mips/include/asm/byteorder.h
+++ b/arch/mips/include/asm/byteorder.h
@@ -8,60 +8,14 @@
 #ifndef _ASM_BYTEORDER_H
 #define _ASM_BYTEORDER_H
 
-#include <linux/compiler.h>
-#include <asm/types.h>
+#include <asm/swab.h>
 
 #if defined(__MIPSEB__)
-# define __BIG_ENDIAN
+#include <linux/byteorder/big_endian.h>
 #elif defined(__MIPSEL__)
-# define __LITTLE_ENDIAN
+#include <linux/byteorder/little_endian.h>
 #else
 # error "MIPS, but neither __MIPSEB__, nor __MIPSEL__???"
 #endif
 
-#define __SWAB_64_THRU_32__
-
-#ifdef CONFIG_CPU_MIPSR2
-
-static inline __attribute_const__ __u16 __arch_swab16(__u16 x)
-{
-	__asm__(
-	"	wsbh	%0, %1			\n"
-	: "=r" (x)
-	: "r" (x));
-
-	return x;
-}
-#define __arch_swab16 __arch_swab16
-
-static inline __attribute_const__ __u32 __arch_swab32(__u32 x)
-{
-	__asm__(
-	"	wsbh	%0, %1			\n"
-	"	rotr	%0, %0, 16		\n"
-	: "=r" (x)
-	: "r" (x));
-
-	return x;
-}
-#define __arch_swab32 __arch_swab32
-
-#ifdef CONFIG_CPU_MIPS64_R2
-static inline __attribute_const__ __u64 __arch_swab64(__u64 x)
-{
-	__asm__(
-	"	dsbh	%0, %1\n"
-	"	dshd	%0, %0"
-	: "=r" (x)
-	: "r" (x));
-
-	return x;
-}
-#define __arch_swab64 __arch_swab64
-#endif /* CONFIG_CPU_MIPS64_R2 */
-
-#endif /* CONFIG_CPU_MIPSR2 */
-
-#include <linux/byteorder.h>
-
 #endif /* _ASM_BYTEORDER_H */
diff --git a/arch/mips/include/asm/swab.h b/arch/mips/include/asm/swab.h
new file mode 100644
index 000000000000..88f1f7d555cb
--- /dev/null
+++ b/arch/mips/include/asm/swab.h
@@ -0,0 +1,55 @@
+/*
+ * 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) 1996, 99, 2003 by Ralf Baechle
+ */
+#ifndef _ASM_SWAB_H
+#define _ASM_SWAB_H
+
+#include <linux/compiler.h>
+#include <asm/types.h>
+
+#define __SWAB_64_THRU_32__
+
+#ifdef CONFIG_CPU_MIPSR2
+
+static inline __attribute_const__ __u16 __arch_swab16(__u16 x)
+{
+	__asm__(
+	"	wsbh	%0, %1			\n"
+	: "=r" (x)
+	: "r" (x));
+
+	return x;
+}
+#define __arch_swab16 __arch_swab16
+
+static inline __attribute_const__ __u32 __arch_swab32(__u32 x)
+{
+	__asm__(
+	"	wsbh	%0, %1			\n"
+	"	rotr	%0, %0, 16		\n"
+	: "=r" (x)
+	: "r" (x));
+
+	return x;
+}
+#define __arch_swab32 __arch_swab32
+
+#ifdef CONFIG_CPU_MIPS64_R2
+static inline __attribute_const__ __u64 __arch_swab64(__u64 x)
+{
+	__asm__(
+	"	dsbh	%0, %1\n"
+	"	dshd	%0, %0"
+	: "=r" (x)
+	: "r" (x));
+
+	return x;
+}
+#define __arch_swab64 __arch_swab64
+#endif /* CONFIG_CPU_MIPS64_R2 */
+#endif /* CONFIG_CPU_MIPSR2 */
+#endif /* _ASM_SWAB_H */
diff --git a/arch/mips/kernel/stacktrace.c b/arch/mips/kernel/stacktrace.c
index 0632e2a849c0..58f5cd76c8c3 100644
--- a/arch/mips/kernel/stacktrace.c
+++ b/arch/mips/kernel/stacktrace.c
@@ -32,7 +32,8 @@ static void save_raw_context_stack(struct stack_trace *trace,
 	}
 }
 
-static void save_context_stack(struct stack_trace *trace, struct pt_regs *regs)
+static void save_context_stack(struct stack_trace *trace,
+	struct task_struct *tsk, struct pt_regs *regs)
 {
 	unsigned long sp = regs->regs[29];
 #ifdef CONFIG_KALLSYMS
@@ -41,7 +42,7 @@ static void save_context_stack(struct stack_trace *trace, struct pt_regs *regs)
 
 	if (raw_show_trace || !__kernel_text_address(pc)) {
 		unsigned long stack_page =
-			(unsigned long)task_stack_page(current);
+			(unsigned long)task_stack_page(tsk);
 		if (stack_page && sp >= stack_page &&
 		    sp <= stack_page + THREAD_SIZE - 32)
 			save_raw_context_stack(trace, sp);
@@ -54,7 +55,7 @@ static void save_context_stack(struct stack_trace *trace, struct pt_regs *regs)
 			trace->entries[trace->nr_entries++] = pc;
 		if (trace->nr_entries >= trace->max_entries)
 			break;
-		pc = unwind_stack(current, &sp, pc, &ra);
+		pc = unwind_stack(tsk, &sp, pc, &ra);
 	} while (pc);
 #else
 	save_raw_context_stack(trace, sp);
@@ -66,12 +67,23 @@ static void save_context_stack(struct stack_trace *trace, struct pt_regs *regs)
  */
 void save_stack_trace(struct stack_trace *trace)
 {
+	save_stack_trace_tsk(current, trace);
+}
+EXPORT_SYMBOL_GPL(save_stack_trace);
+
+void save_stack_trace_tsk(struct task_struct *tsk, struct stack_trace *trace)
+{
 	struct pt_regs dummyregs;
 	struct pt_regs *regs = &dummyregs;
 
 	WARN_ON(trace->nr_entries || !trace->max_entries);
 
-	prepare_frametrace(regs);
-	save_context_stack(trace, regs);
+	if (tsk != current) {
+		regs->regs[29] = tsk->thread.reg29;
+		regs->regs[31] = 0;
+		regs->cp0_epc = tsk->thread.reg31;
+	} else
+		prepare_frametrace(regs);
+	save_context_stack(trace, tsk, regs);
 }
-EXPORT_SYMBOL_GPL(save_stack_trace);
+EXPORT_SYMBOL_GPL(save_stack_trace_tsk);
diff --git a/arch/mips/kernel/vpe.c b/arch/mips/kernel/vpe.c
index 010b27e01f7b..3ca5f42e819d 100644
--- a/arch/mips/kernel/vpe.c
+++ b/arch/mips/kernel/vpe.c
@@ -1454,7 +1454,7 @@ static int __init vpe_module_init(void)
 	device_initialize(&vpe_device);
 	vpe_device.class	= &vpe_class,
 	vpe_device.parent	= NULL,
-	strlcpy(vpe_device.bus_id, "vpe1", BUS_ID_SIZE);
+	dev_set_name(&vpe_device, "vpe1");
 	vpe_device.devt = MKDEV(major, minor);
 	err = device_add(&vpe_device);
 	if (err) {
diff --git a/arch/mips/pci/pci-ip27.c b/arch/mips/pci/pci-ip27.c
index f97ab1461012..dda6f2058665 100644
--- a/arch/mips/pci/pci-ip27.c
+++ b/arch/mips/pci/pci-ip27.c
@@ -146,12 +146,6 @@ int __devinit pcibios_map_irq(const struct pci_dev *dev, u8 slot, u8 pin)
 	return 0;
 }
 
-/* Most MIPS systems have straight-forward swizzling needs.  */
-static inline u8 bridge_swizzle(u8 pin, u8 slot)
-{
-	return (((pin - 1) + slot) % 4) + 1;
-}
-
 static inline struct pci_dev *bridge_root_dev(struct pci_dev *dev)
 {
 	while (dev->bus->parent) {
diff --git a/arch/mips/pci/pci.c b/arch/mips/pci/pci.c
index 62cae740e250..b0eb9e75c682 100644
--- a/arch/mips/pci/pci.c
+++ b/arch/mips/pci/pci.c
@@ -149,28 +149,6 @@ out:
 	       "Skipping PCI bus scan due to resource conflict\n");
 }
 
-/* Most MIPS systems have straight-forward swizzling needs.  */
-
-static inline u8 bridge_swizzle(u8 pin, u8 slot)
-{
-	return (((pin - 1) + slot) % 4) + 1;
-}
-
-static u8 __init common_swizzle(struct pci_dev *dev, u8 *pinp)
-{
-	u8 pin = *pinp;
-
-	while (dev->bus->parent) {
-		pin = bridge_swizzle(pin, PCI_SLOT(dev->devfn));
-		/* Move up the chain of bridges. */
-		dev = dev->bus->self;
-        }
-	*pinp = pin;
-
-	/* The slot is the slot of the last bridge. */
-	return PCI_SLOT(dev->devfn);
-}
-
 static int __init pcibios_init(void)
 {
 	struct pci_controller *hose;
@@ -179,7 +157,7 @@ static int __init pcibios_init(void)
 	for (hose = hose_head; hose; hose = hose->next)
 		pcibios_scanbus(hose);
 
-	pci_fixup_irqs(common_swizzle, pcibios_map_irq);
+	pci_fixup_irqs(pci_common_swizzle, pcibios_map_irq);
 
 	pci_initialized = 1;
 
diff --git a/arch/mips/pmc-sierra/yosemite/atmel_read_eeprom.c b/arch/mips/pmc-sierra/yosemite/atmel_read_eeprom.c
index 97862f45496d..caf5e9a0acc7 100644
--- a/arch/mips/pmc-sierra/yosemite/atmel_read_eeprom.c
+++ b/arch/mips/pmc-sierra/yosemite/atmel_read_eeprom.c
@@ -148,7 +148,7 @@ int read_eeprom(char *buffer, int eeprom_size, int size)
 	send_byte(W_HEADER);
 	recv_ack();
 
-	/* EEPROM with size of more then 2K need two byte addressing */
+	/* EEPROM with size of more than 2K need two byte addressing */
 	if (eeprom_size > 2048) {
 		send_byte(0x00);
 		recv_ack();
diff --git a/arch/parisc/include/asm/atomic.h b/arch/parisc/include/asm/atomic.h
index 57fcc4a5ebb4..edbfe25c5fc1 100644
--- a/arch/parisc/include/asm/atomic.h
+++ b/arch/parisc/include/asm/atomic.h
@@ -155,14 +155,11 @@ static inline unsigned long __cmpxchg_local(volatile void *ptr,
 #define cmpxchg64_local(ptr, o, n) __cmpxchg64_local_generic((ptr), (o), (n))
 #endif
 
-/* Note that we need not lock read accesses - aligned word writes/reads
- * are atomic, so a reader never sees unconsistent values.
- *
- * Cache-line alignment would conflict with, for example, linux/module.h
+/*
+ * Note that we need not lock read accesses - aligned word writes/reads
+ * are atomic, so a reader never sees inconsistent values.
  */
 
-typedef struct { volatile int counter; } atomic_t;
-
 /* It's possible to reduce all atomic operations to either
  * __atomic_add_return, atomic_set and atomic_read (the latter
  * is there only for consistency).
@@ -260,8 +257,6 @@ static __inline__ int atomic_add_unless(atomic_t *v, int a, int u)
 
 #ifdef CONFIG_64BIT
 
-typedef struct { volatile s64 counter; } atomic64_t;
-
 #define ATOMIC64_INIT(i) ((atomic64_t) { (i) })
 
 static __inline__ int
diff --git a/arch/powerpc/boot/Makefile b/arch/powerpc/boot/Makefile
index f32829937aad..ab6dda372438 100644
--- a/arch/powerpc/boot/Makefile
+++ b/arch/powerpc/boot/Makefile
@@ -208,7 +208,7 @@ image-$(CONFIG_DEFAULT_UIMAGE)		+= uImage
 #
 # Theses are default targets to build images which embed device tree blobs.
 # They are only required on boards which do not have FDT support in firmware.
-# Boards with newish u-boot firmare can use the uImage target above
+# Boards with newish u-boot firmware can use the uImage target above
 #
 
 # Board ports in arch/powerpc/platform/40x/Kconfig
diff --git a/arch/powerpc/boot/dts/sequoia.dts b/arch/powerpc/boot/dts/sequoia.dts
index 3b295e8df53f..43cc68bd3192 100644
--- a/arch/powerpc/boot/dts/sequoia.dts
+++ b/arch/powerpc/boot/dts/sequoia.dts
@@ -134,7 +134,7 @@
 		};
 
 		USB1: usb@e0000400 {
-			compatible = "ohci-be";
+			compatible = "ibm,usb-ohci-440epx", "ohci-be";
 			reg = <0x00000000 0xe0000400 0x00000060>;
 			interrupt-parent = <&UIC0>;
 			interrupts = <0x15 0x8>;
diff --git a/arch/powerpc/include/asm/Kbuild b/arch/powerpc/include/asm/Kbuild
index 5ab7d7fe198c..9268602de5d0 100644
--- a/arch/powerpc/include/asm/Kbuild
+++ b/arch/powerpc/include/asm/Kbuild
@@ -35,3 +35,4 @@ unifdef-y += spu_info.h
 unifdef-y += termios.h
 unifdef-y += types.h
 unifdef-y += unistd.h
+unifdef-y += swab.h
diff --git a/arch/powerpc/include/asm/atomic.h b/arch/powerpc/include/asm/atomic.h
index 499be5bdd6fa..b401950f5259 100644
--- a/arch/powerpc/include/asm/atomic.h
+++ b/arch/powerpc/include/asm/atomic.h
@@ -5,7 +5,7 @@
  * PowerPC atomic operations
  */
 
-typedef struct { int counter; } atomic_t;
+#include <linux/types.h>
 
 #ifdef __KERNEL__
 #include <linux/compiler.h>
@@ -251,8 +251,6 @@ static __inline__ int atomic_dec_if_positive(atomic_t *v)
 
 #ifdef __powerpc64__
 
-typedef struct { long counter; } atomic64_t;
-
 #define ATOMIC64_INIT(i)	{ (i) }
 
 static __inline__ long atomic64_read(const atomic64_t *v)
diff --git a/arch/powerpc/include/asm/byteorder.h b/arch/powerpc/include/asm/byteorder.h
index d5de325472e9..5cca27a41532 100644
--- a/arch/powerpc/include/asm/byteorder.h
+++ b/arch/powerpc/include/asm/byteorder.h
@@ -8,86 +8,7 @@
  * 2 of the License, or (at your option) any later version.
  */
 
-#include <asm/types.h>
-#include <linux/compiler.h>
-
-#define __BIG_ENDIAN
-
-#ifdef __GNUC__
-#ifdef __KERNEL__
-
-static __inline__ __u16 ld_le16(const volatile __u16 *addr)
-{
-	__u16 val;
-
-	__asm__ __volatile__ ("lhbrx %0,0,%1" : "=r" (val) : "r" (addr), "m" (*addr));
-	return val;
-}
-#define __arch_swab16p ld_le16
-
-static __inline__ void st_le16(volatile __u16 *addr, const __u16 val)
-{
-	__asm__ __volatile__ ("sthbrx %1,0,%2" : "=m" (*addr) : "r" (val), "r" (addr));
-}
-
-static inline void __arch_swab16s(__u16 *addr)
-{
-	st_le16(addr, *addr);
-}
-#define __arch_swab16s __arch_swab16s
-
-static __inline__ __u32 ld_le32(const volatile __u32 *addr)
-{
-	__u32 val;
-
-	__asm__ __volatile__ ("lwbrx %0,0,%1" : "=r" (val) : "r" (addr), "m" (*addr));
-	return val;
-}
-#define __arch_swab32p ld_le32
-
-static __inline__ void st_le32(volatile __u32 *addr, const __u32 val)
-{
-	__asm__ __volatile__ ("stwbrx %1,0,%2" : "=m" (*addr) : "r" (val), "r" (addr));
-}
-
-static inline void __arch_swab32s(__u32 *addr)
-{
-	st_le32(addr, *addr);
-}
-#define __arch_swab32s __arch_swab32s
-
-static inline __attribute_const__ __u16 __arch_swab16(__u16 value)
-{
-	__u16 result;
-
-	__asm__("rlwimi %0,%1,8,16,23"
-	    : "=r" (result)
-	    : "r" (value), "0" (value >> 8));
-	return result;
-}
-#define __arch_swab16 __arch_swab16
-
-static inline __attribute_const__ __u32 __arch_swab32(__u32 value)
-{
-	__u32 result;
-
-	__asm__("rlwimi %0,%1,24,16,23\n\t"
-	    "rlwimi %0,%1,8,8,15\n\t"
-	    "rlwimi %0,%1,24,0,7"
-	    : "=r" (result)
-	    : "r" (value), "0" (value >> 24));
-	return result;
-}
-#define __arch_swab32 __arch_swab32
-
-#endif /* __KERNEL__ */
-
-#ifndef __powerpc64__
-#define __SWAB_64_THRU_32__
-#endif /* __powerpc64__ */
-
-#endif /* __GNUC__ */
-
-#include <linux/byteorder.h>
+#include <asm/swab.h>
+#include <linux/byteorder/big_endian.h>
 
 #endif /* _ASM_POWERPC_BYTEORDER_H */
diff --git a/arch/powerpc/include/asm/hugetlb.h b/arch/powerpc/include/asm/hugetlb.h
index 26f0d0ab27a5..b1dafb6a9743 100644
--- a/arch/powerpc/include/asm/hugetlb.h
+++ b/arch/powerpc/include/asm/hugetlb.h
@@ -18,6 +18,12 @@ pte_t huge_ptep_get_and_clear(struct mm_struct *mm, unsigned long addr,
 			      pte_t *ptep);
 
 /*
+ * The version of vma_mmu_pagesize() in arch/powerpc/mm/hugetlbpage.c needs
+ * to override the version in mm/hugetlb.c
+ */
+#define vma_mmu_pagesize vma_mmu_pagesize
+
+/*
  * If the arch doesn't supply something else, assume that hugepage
  * size aligned regions are ok without further preparation.
  */
diff --git a/arch/powerpc/include/asm/swab.h b/arch/powerpc/include/asm/swab.h
new file mode 100644
index 000000000000..ef824ae4b79c
--- /dev/null
+++ b/arch/powerpc/include/asm/swab.h
@@ -0,0 +1,90 @@
+#ifndef _ASM_POWERPC_SWAB_H
+#define _ASM_POWERPC_SWAB_H
+
+/*
+ * 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/types.h>
+#include <linux/compiler.h>
+
+#ifdef __GNUC__
+
+#ifndef __powerpc64__
+#define __SWAB_64_THRU_32__
+#endif /* __powerpc64__ */
+
+#ifdef __KERNEL__
+
+static __inline__ __u16 ld_le16(const volatile __u16 *addr)
+{
+	__u16 val;
+
+	__asm__ __volatile__ ("lhbrx %0,0,%1" : "=r" (val) : "r" (addr), "m" (*addr));
+	return val;
+}
+#define __arch_swab16p ld_le16
+
+static __inline__ void st_le16(volatile __u16 *addr, const __u16 val)
+{
+	__asm__ __volatile__ ("sthbrx %1,0,%2" : "=m" (*addr) : "r" (val), "r" (addr));
+}
+
+static inline void __arch_swab16s(__u16 *addr)
+{
+	st_le16(addr, *addr);
+}
+#define __arch_swab16s __arch_swab16s
+
+static __inline__ __u32 ld_le32(const volatile __u32 *addr)
+{
+	__u32 val;
+
+	__asm__ __volatile__ ("lwbrx %0,0,%1" : "=r" (val) : "r" (addr), "m" (*addr));
+	return val;
+}
+#define __arch_swab32p ld_le32
+
+static __inline__ void st_le32(volatile __u32 *addr, const __u32 val)
+{
+	__asm__ __volatile__ ("stwbrx %1,0,%2" : "=m" (*addr) : "r" (val), "r" (addr));
+}
+
+static inline void __arch_swab32s(__u32 *addr)
+{
+	st_le32(addr, *addr);
+}
+#define __arch_swab32s __arch_swab32s
+
+static inline __attribute_const__ __u16 __arch_swab16(__u16 value)
+{
+	__u16 result;
+
+	__asm__("rlwimi %0,%1,8,16,23"
+	    : "=r" (result)
+	    : "r" (value), "0" (value >> 8));
+	return result;
+}
+#define __arch_swab16 __arch_swab16
+
+static inline __attribute_const__ __u32 __arch_swab32(__u32 value)
+{
+	__u32 result;
+
+	__asm__("rlwimi %0,%1,24,16,23\n\t"
+	    "rlwimi %0,%1,8,8,15\n\t"
+	    "rlwimi %0,%1,24,0,7"
+	    : "=r" (result)
+	    : "r" (value), "0" (value >> 24));
+	return result;
+}
+#define __arch_swab32 __arch_swab32
+
+#endif /* __KERNEL__ */
+
+#endif /* __GNUC__ */
+
+#endif /* _ASM_POWERPC_SWAB_H */
diff --git a/arch/powerpc/kernel/kprobes.c b/arch/powerpc/kernel/kprobes.c
index de79915452c8..c9329786073b 100644
--- a/arch/powerpc/kernel/kprobes.c
+++ b/arch/powerpc/kernel/kprobes.c
@@ -96,9 +96,10 @@ void __kprobes arch_disarm_kprobe(struct kprobe *p)
 
 void __kprobes arch_remove_kprobe(struct kprobe *p)
 {
-	mutex_lock(&kprobe_mutex);
-	free_insn_slot(p->ainsn.insn, 0);
-	mutex_unlock(&kprobe_mutex);
+	if (p->ainsn.insn) {
+		free_insn_slot(p->ainsn.insn, 0);
+		p->ainsn.insn = NULL;
+	}
 }
 
 static void __kprobes prepare_singlestep(struct kprobe *p, struct pt_regs *regs)
@@ -316,7 +317,7 @@ static int __kprobes trampoline_probe_handler(struct kprobe *p,
 	/*
 	 * It is possible to have multiple instances associated with a given
 	 * task either because an multiple functions in the call path
-	 * have a return probe installed on them, and/or more then one return
+	 * have a return probe installed on them, and/or more than one return
 	 * return probe was registered for a target function.
 	 *
 	 * We can handle this because:
diff --git a/arch/powerpc/kernel/prom_parse.c b/arch/powerpc/kernel/prom_parse.c
index 8c1335566089..8f0856f312da 100644
--- a/arch/powerpc/kernel/prom_parse.c
+++ b/arch/powerpc/kernel/prom_parse.c
@@ -232,11 +232,6 @@ int of_pci_address_to_resource(struct device_node *dev, int bar,
 }
 EXPORT_SYMBOL_GPL(of_pci_address_to_resource);
 
-static u8 of_irq_pci_swizzle(u8 slot, u8 pin)
-{
-	return (((pin - 1) + slot) % 4) + 1;
-}
-
 int of_irq_map_pci(struct pci_dev *pdev, struct of_irq *out_irq)
 {
 	struct device_node *dn, *ppnode;
@@ -306,7 +301,7 @@ int of_irq_map_pci(struct pci_dev *pdev, struct of_irq *out_irq)
 		/* We can only get here if we hit a P2P bridge with no node,
 		 * let's do standard swizzling and try again
 		 */
-		lspec = of_irq_pci_swizzle(PCI_SLOT(pdev->devfn), lspec);
+		lspec = pci_swizzle_interrupt_pin(pdev, lspec);
 		pdev = ppdev;
 	}
 
diff --git a/arch/powerpc/mm/hugetlbpage.c b/arch/powerpc/mm/hugetlbpage.c
index 201c7a5486cb..9920d6a7cf29 100644
--- a/arch/powerpc/mm/hugetlbpage.c
+++ b/arch/powerpc/mm/hugetlbpage.c
@@ -512,6 +512,13 @@ unsigned long hugetlb_get_unmapped_area(struct file *file, unsigned long addr,
 	return slice_get_unmapped_area(addr, len, flags, mmu_psize, 1, 0);
 }
 
+unsigned long vma_mmu_pagesize(struct vm_area_struct *vma)
+{
+	unsigned int psize = get_slice_psize(vma->vm_mm, vma->vm_start);
+
+	return 1UL << mmu_psize_to_shift(psize);
+}
+
 /*
  * Called by asm hashtable.S for doing lazy icache flush
  */
diff --git a/arch/powerpc/mm/mem.c b/arch/powerpc/mm/mem.c
index 53b06ebb3f2f..f00f09a77f12 100644
--- a/arch/powerpc/mm/mem.c
+++ b/arch/powerpc/mm/mem.c
@@ -132,7 +132,7 @@ int arch_add_memory(int nid, u64 start, u64 size)
 	/* this should work for most non-highmem platforms */
 	zone = pgdata->node_zones;
 
-	return __add_pages(zone, start_pfn, nr_pages);
+	return __add_pages(nid, zone, start_pfn, nr_pages);
 }
 #endif /* CONFIG_MEMORY_HOTPLUG */
 
diff --git a/arch/powerpc/oprofile/cell/spu_profiler.c b/arch/powerpc/oprofile/cell/spu_profiler.c
index dd499c3e9da7..83faa958b9d4 100644
--- a/arch/powerpc/oprofile/cell/spu_profiler.c
+++ b/arch/powerpc/oprofile/cell/spu_profiler.c
@@ -49,7 +49,7 @@ void set_spu_profiling_frequency(unsigned int freq_khz, unsigned int cycles_rese
 	 * of precision.  This is close enough for the purpose at hand.
 	 *
 	 * The value of the timeout should be small enough that the hw
-	 * trace buffer will not get more then about 1/3 full for the
+	 * trace buffer will not get more than about 1/3 full for the
 	 * maximum user specified (the LFSR value) hw sampling frequency.
 	 * This is to ensure the trace buffer will never fill even if the
 	 * kernel thread scheduling varies under a heavy system load.
diff --git a/arch/s390/Kconfig b/arch/s390/Kconfig
index 19577aeffd7b..a94a3c3ae932 100644
--- a/arch/s390/Kconfig
+++ b/arch/s390/Kconfig
@@ -299,7 +299,7 @@ config WARN_STACK
 	  This option enables the compiler options -mwarn-framesize and
 	  -mwarn-dynamicstack. If the compiler supports these options it
 	  will generate warnings for function which either use alloca or
-	  create a stack frame bigger then CONFIG_WARN_STACK_SIZE.
+	  create a stack frame bigger than CONFIG_WARN_STACK_SIZE.
 
 	  Say N if you are unsure.
 
diff --git a/arch/s390/include/asm/Kbuild b/arch/s390/include/asm/Kbuild
index 63a23415fba6..f2af4167bd5f 100644
--- a/arch/s390/include/asm/Kbuild
+++ b/arch/s390/include/asm/Kbuild
@@ -13,3 +13,4 @@ unifdef-y += cmb.h
 unifdef-y += debug.h
 unifdef-y += chpid.h
 unifdef-y += schid.h
+unifdef-y += swab.h
diff --git a/arch/s390/include/asm/atomic.h b/arch/s390/include/asm/atomic.h
index 2d184655bc5d..de432f2de2d2 100644
--- a/arch/s390/include/asm/atomic.h
+++ b/arch/s390/include/asm/atomic.h
@@ -2,6 +2,7 @@
 #define __ARCH_S390_ATOMIC__
 
 #include <linux/compiler.h>
+#include <linux/types.h>
 
 /*
  *  include/asm-s390/atomic.h
@@ -23,9 +24,6 @@
  * S390 uses 'Compare And Swap' for atomicity in SMP enviroment
  */
 
-typedef struct {
-	int counter;
-} __attribute__ ((aligned (4))) atomic_t;
 #define ATOMIC_INIT(i)  { (i) }
 
 #ifdef __KERNEL__
@@ -149,9 +147,6 @@ static __inline__ int atomic_add_unless(atomic_t *v, int a, int u)
 #undef __CS_LOOP
 
 #ifdef __s390x__
-typedef struct {
-	long long counter;
-} __attribute__ ((aligned (8))) atomic64_t;
 #define ATOMIC64_INIT(i)  { (i) }
 
 #if __GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ > 2)
diff --git a/arch/s390/include/asm/byteorder.h b/arch/s390/include/asm/byteorder.h
index 8bcf277c8468..b95a2b2933fb 100644
--- a/arch/s390/include/asm/byteorder.h
+++ b/arch/s390/include/asm/byteorder.h
@@ -1,95 +1,7 @@
 #ifndef _S390_BYTEORDER_H
 #define _S390_BYTEORDER_H
 
-/*
- *  include/asm-s390/byteorder.h
- *
- *  S390 version
- *    Copyright (C) 1999 IBM Deutschland Entwicklung GmbH, IBM Corporation
- *    Author(s): Martin Schwidefsky (schwidefsky@de.ibm.com)
- */
-
-#include <asm/types.h>
-
-#define __BIG_ENDIAN
-
-#ifndef __s390x__
-# define __SWAB_64_THRU_32__
-#endif
-
-#ifdef __s390x__
-static inline __u64 __arch_swab64p(const __u64 *x)
-{
-	__u64 result;
-
-	asm volatile("lrvg %0,%1" : "=d" (result) : "m" (*x));
-	return result;
-}
-#define __arch_swab64p __arch_swab64p
-
-static inline __u64 __arch_swab64(__u64 x)
-{
-	__u64 result;
-
-	asm volatile("lrvgr %0,%1" : "=d" (result) : "d" (x));
-	return result;
-}
-#define __arch_swab64 __arch_swab64
-
-static inline void __arch_swab64s(__u64 *x)
-{
-	*x = __arch_swab64p(x);
-}
-#define __arch_swab64s __arch_swab64s
-#endif /* __s390x__ */
-
-static inline __u32 __arch_swab32p(const __u32 *x)
-{
-	__u32 result;
-	
-	asm volatile(
-#ifndef __s390x__
-		"	icm	%0,8,3(%1)\n"
-		"	icm	%0,4,2(%1)\n"
-		"	icm	%0,2,1(%1)\n"
-		"	ic	%0,0(%1)"
-		: "=&d" (result) : "a" (x), "m" (*x) : "cc");
-#else /* __s390x__ */
-		"	lrv	%0,%1"
-		: "=d" (result) : "m" (*x));
-#endif /* __s390x__ */
-	return result;
-}
-#define __arch_swab32p __arch_swab32p
-
-#ifdef __s390x__
-static inline __u32 __arch_swab32(__u32 x)
-{
-	__u32 result;
-	
-	asm volatile("lrvr  %0,%1" : "=d" (result) : "d" (x));
-	return result;
-}
-#define __arch_swab32 __arch_swab32
-#endif /* __s390x__ */
-
-static inline __u16 __arch_swab16p(const __u16 *x)
-{
-	__u16 result;
-	
-	asm volatile(
-#ifndef __s390x__
-		"	icm	%0,2,1(%1)\n"
-		"	ic	%0,0(%1)\n"
-		: "=&d" (result) : "a" (x), "m" (*x) : "cc");
-#else /* __s390x__ */
-		"	lrvh	%0,%1"
-		: "=d" (result) : "m" (*x));
-#endif /* __s390x__ */
-	return result;
-}
-#define __arch_swab16p __arch_swab16p
-
-#include <linux/byteorder.h>
+#include <asm/swab.h>
+#include <linux/byteorder/big_endian.h>
 
 #endif /* _S390_BYTEORDER_H */
diff --git a/arch/s390/include/asm/s390_rdev.h b/arch/s390/include/asm/s390_rdev.h
deleted file mode 100644
index 6fa20442a48c..000000000000
--- a/arch/s390/include/asm/s390_rdev.h
+++ /dev/null
@@ -1,15 +0,0 @@
-/*
- *  include/asm-s390/ccwdev.h
- *
- *    Copyright (C) 2002,2005 IBM Deutschland Entwicklung GmbH, IBM Corporation
- *    Author(s): Cornelia Huck <cornelia.huck@de.ibm.com>
- *               Carsten Otte  <cotte@de.ibm.com>
- *
- *  Interface for s390 root device
- */
-
-#ifndef _S390_RDEV_H_
-#define _S390_RDEV_H_
-extern struct device *s390_root_dev_register(const char *);
-extern void s390_root_dev_unregister(struct device *);
-#endif /* _S390_RDEV_H_ */
diff --git a/arch/s390/include/asm/swab.h b/arch/s390/include/asm/swab.h
new file mode 100644
index 000000000000..bd9321aa55a9
--- /dev/null
+++ b/arch/s390/include/asm/swab.h
@@ -0,0 +1,91 @@
+#ifndef _S390_SWAB_H
+#define _S390_SWAB_H
+
+/*
+ *  include/asm-s390/swab.h
+ *
+ *  S390 version
+ *    Copyright (C) 1999 IBM Deutschland Entwicklung GmbH, IBM Corporation
+ *    Author(s): Martin Schwidefsky (schwidefsky@de.ibm.com)
+ */
+
+#include <asm/types.h>
+
+#ifndef __s390x__
+# define __SWAB_64_THRU_32__
+#endif
+
+#ifdef __s390x__
+static inline __u64 __arch_swab64p(const __u64 *x)
+{
+	__u64 result;
+
+	asm volatile("lrvg %0,%1" : "=d" (result) : "m" (*x));
+	return result;
+}
+#define __arch_swab64p __arch_swab64p
+
+static inline __u64 __arch_swab64(__u64 x)
+{
+	__u64 result;
+
+	asm volatile("lrvgr %0,%1" : "=d" (result) : "d" (x));
+	return result;
+}
+#define __arch_swab64 __arch_swab64
+
+static inline void __arch_swab64s(__u64 *x)
+{
+	*x = __arch_swab64p(x);
+}
+#define __arch_swab64s __arch_swab64s
+#endif /* __s390x__ */
+
+static inline __u32 __arch_swab32p(const __u32 *x)
+{
+	__u32 result;
+	
+	asm volatile(
+#ifndef __s390x__
+		"	icm	%0,8,3(%1)\n"
+		"	icm	%0,4,2(%1)\n"
+		"	icm	%0,2,1(%1)\n"
+		"	ic	%0,0(%1)"
+		: "=&d" (result) : "a" (x), "m" (*x) : "cc");
+#else /* __s390x__ */
+		"	lrv	%0,%1"
+		: "=d" (result) : "m" (*x));
+#endif /* __s390x__ */
+	return result;
+}
+#define __arch_swab32p __arch_swab32p
+
+#ifdef __s390x__
+static inline __u32 __arch_swab32(__u32 x)
+{
+	__u32 result;
+	
+	asm volatile("lrvr  %0,%1" : "=d" (result) : "d" (x));
+	return result;
+}
+#define __arch_swab32 __arch_swab32
+#endif /* __s390x__ */
+
+static inline __u16 __arch_swab16p(const __u16 *x)
+{
+	__u16 result;
+	
+	asm volatile(
+#ifndef __s390x__
+		"	icm	%0,2,1(%1)\n"
+		"	ic	%0,0(%1)\n"
+		: "=&d" (result) : "a" (x), "m" (*x) : "cc");
+#else /* __s390x__ */
+		"	lrvh	%0,%1"
+		: "=d" (result) : "m" (*x));
+#endif /* __s390x__ */
+	return result;
+}
+#define __arch_swab16p __arch_swab16p
+
+#endif /* _S390_SWAB_H */
diff --git a/arch/s390/kernel/kprobes.c b/arch/s390/kernel/kprobes.c
index 569079ec4ff0..a01cf0284db2 100644
--- a/arch/s390/kernel/kprobes.c
+++ b/arch/s390/kernel/kprobes.c
@@ -218,9 +218,10 @@ void __kprobes arch_disarm_kprobe(struct kprobe *p)
 
 void __kprobes arch_remove_kprobe(struct kprobe *p)
 {
-	mutex_lock(&kprobe_mutex);
-	free_insn_slot(p->ainsn.insn, 0);
-	mutex_unlock(&kprobe_mutex);
+	if (p->ainsn.insn) {
+		free_insn_slot(p->ainsn.insn, 0);
+		p->ainsn.insn = NULL;
+	}
 }
 
 static void __kprobes prepare_singlestep(struct kprobe *p, struct pt_regs *regs)
@@ -381,7 +382,7 @@ static int __kprobes trampoline_probe_handler(struct kprobe *p,
 	/*
 	 * It is possible to have multiple instances associated with a given
 	 * task either because an multiple functions in the call path
-	 * have a return probe installed on them, and/or more then one return
+	 * have a return probe installed on them, and/or more than one return
 	 * return probe was registered for a target function.
 	 *
 	 * We can handle this because:
diff --git a/arch/s390/mm/init.c b/arch/s390/mm/init.c
index 158b0d6d7046..f0258ca3b17e 100644
--- a/arch/s390/mm/init.c
+++ b/arch/s390/mm/init.c
@@ -183,7 +183,7 @@ int arch_add_memory(int nid, u64 start, u64 size)
 	rc = vmem_add_mapping(start, size);
 	if (rc)
 		return rc;
-	rc = __add_pages(zone, PFN_DOWN(start), PFN_DOWN(size));
+	rc = __add_pages(nid, zone, PFN_DOWN(start), PFN_DOWN(size));
 	if (rc)
 		vmem_remove_mapping(start, size);
 	return rc;
diff --git a/arch/sh/drivers/pci/ops-cayman.c b/arch/sh/drivers/pci/ops-cayman.c
index 5ccf9ea3a9de..38ef76207af6 100644
--- a/arch/sh/drivers/pci/ops-cayman.c
+++ b/arch/sh/drivers/pci/ops-cayman.c
@@ -5,11 +5,6 @@
 #include <cpu/irq.h>
 #include "pci-sh5.h"
 
-static inline u8 bridge_swizzle(u8 pin, u8 slot)
-{
-	return (((pin - 1) + slot) % 4) + 1;
-}
-
 int __init pcibios_map_platform_irq(struct pci_dev *dev, u8 slot, u8 pin)
 {
 	int result = -1;
@@ -42,7 +37,7 @@ int __init pcibios_map_platform_irq(struct pci_dev *dev, u8 slot, u8 pin)
 	while (dev->bus->number > 0) {
 
 		slot = path[i].slot = PCI_SLOT(dev->devfn);
-		pin = path[i].pin = bridge_swizzle(pin, slot);
+		pin = path[i].pin = pci_swizzle_interrupt_pin(dev, pin);
 		dev = dev->bus->self;
 		i++;
 		if (i > 3) panic("PCI path to root bus too long!\n");
@@ -56,7 +51,7 @@ int __init pcibios_map_platform_irq(struct pci_dev *dev, u8 slot, u8 pin)
 	if ((slot < 3) || (i == 0)) {
 		/* Bus 0 (incl. PCI-PCI bridge itself) : perform the final
 		   swizzle now. */
-		result = IRQ_INTA + bridge_swizzle(pin, slot) - 1;
+		result = IRQ_INTA + pci_swizzle_interrupt_pin(dev, pin) - 1;
 	} else {
 		i--;
 		slot = path[i].slot;
diff --git a/arch/sh/drivers/pci/pci.c b/arch/sh/drivers/pci/pci.c
index d3839e609aac..e36c7b870861 100644
--- a/arch/sh/drivers/pci/pci.c
+++ b/arch/sh/drivers/pci/pci.c
@@ -21,26 +21,6 @@
 #include <linux/init.h>
 #include <asm/io.h>
 
-static inline u8 bridge_swizzle(u8 pin, u8 slot)
-{
-	return (((pin - 1) + slot) % 4) + 1;
-}
-
-static u8 __init simple_swizzle(struct pci_dev *dev, u8 *pinp)
-{
-	u8 pin = *pinp;
-
-	while (dev->bus->parent) {
-		pin = bridge_swizzle(pin, PCI_SLOT(dev->devfn));
-		/* Move up the chain of bridges. */
-		dev = dev->bus->self;
-	}
-	*pinp = pin;
-
-	/* The slot is the slot of the last bridge. */
-	return PCI_SLOT(dev->devfn);
-}
-
 static int __init pcibios_init(void)
 {
 	struct pci_channel *p;
@@ -61,7 +41,7 @@ static int __init pcibios_init(void)
 		busno = bus->subordinate + 1;
 	}
 
-	pci_fixup_irqs(simple_swizzle, pcibios_map_platform_irq);
+	pci_fixup_irqs(pci_common_swizzle, pcibios_map_platform_irq);
 
 	return 0;
 }
diff --git a/arch/sh/include/asm/Kbuild b/arch/sh/include/asm/Kbuild
index 43910cdf78a5..f1a2a0d1c79c 100644
--- a/arch/sh/include/asm/Kbuild
+++ b/arch/sh/include/asm/Kbuild
@@ -6,3 +6,4 @@ unifdef-y += unistd_32.h
 unifdef-y += unistd_64.h
 unifdef-y += posix_types_32.h
 unifdef-y += posix_types_64.h
+unifdef-y += swab.h
diff --git a/arch/sh/include/asm/atomic.h b/arch/sh/include/asm/atomic.h
index c043ef003028..6327ffbb1992 100644
--- a/arch/sh/include/asm/atomic.h
+++ b/arch/sh/include/asm/atomic.h
@@ -7,16 +7,15 @@
  *
  */
 
-typedef struct { volatile int counter; } atomic_t;
+#include <linux/compiler.h>
+#include <linux/types.h>
+#include <asm/system.h>
 
 #define ATOMIC_INIT(i)	( (atomic_t) { (i) } )
 
 #define atomic_read(v)		((v)->counter)
 #define atomic_set(v,i)		((v)->counter = (i))
 
-#include <linux/compiler.h>
-#include <asm/system.h>
-
 #if defined(CONFIG_GUSA_RB)
 #include <asm/atomic-grb.h>
 #elif defined(CONFIG_CPU_SH4A)
diff --git a/arch/sh/include/asm/byteorder.h b/arch/sh/include/asm/byteorder.h
index f5fa0653ebc6..e95c41a5c8cc 100644
--- a/arch/sh/include/asm/byteorder.h
+++ b/arch/sh/include/asm/byteorder.h
@@ -1,68 +1,12 @@
 #ifndef __ASM_SH_BYTEORDER_H
 #define __ASM_SH_BYTEORDER_H
 
-/*
- * Copyright (C) 1999  Niibe Yutaka
- * Copyright (C) 2000, 2001  Paolo Alberelli
- */
-#include <linux/compiler.h>
-#include <linux/types.h>
+#include <asm/swab.h>
 
 #ifdef __LITTLE_ENDIAN__
-# define __LITTLE_ENDIAN
+#include <linux/byteorder/little_endian.h>
 #else
-# define __BIG_ENDIAN
+#include <linux/byteorder/big_endian.h>
 #endif
 
-#define __SWAB_64_THRU_32__
-
-static inline __attribute_const__ __u32 __arch_swab32(__u32 x)
-{
-	__asm__(
-#ifdef __SH5__
-		"byterev	%0, %0\n\t"
-		"shari		%0, 32, %0"
-#else
-		"swap.b		%0, %0\n\t"
-		"swap.w		%0, %0\n\t"
-		"swap.b		%0, %0"
-#endif
-		: "=r" (x)
-		: "0" (x));
-
-	return x;
-}
-#define __arch_swab32 __arch_swab32
-
-static inline __attribute_const__ __u16 __arch_swab16(__u16 x)
-{
-	__asm__(
-#ifdef __SH5__
-		"byterev	%0, %0\n\t"
-		"shari		%0, 32, %0"
-#else
-		"swap.b		%0, %0"
-#endif
-		: "=r" (x)
-		:  "0" (x));
-
-	return x;
-}
-#define __arch_swab16 __arch_swab16
-
-static inline __u64 __arch_swab64(__u64 val)
-{
-	union {
-		struct { __u32 a,b; } s;
-		__u64 u;
-	} v, w;
-	v.u = val;
-	w.s.b = __arch_swab32(v.s.a);
-	w.s.a = __arch_swab32(v.s.b);
-	return w.u;
-}
-#define __arch_swab64 __arch_swab64
-
-#include <linux/byteorder.h>
-
 #endif /* __ASM_SH_BYTEORDER_H */
diff --git a/arch/sh/include/asm/swab.h b/arch/sh/include/asm/swab.h
new file mode 100644
index 000000000000..e69315935107
--- /dev/null
+++ b/arch/sh/include/asm/swab.h
@@ -0,0 +1,60 @@
+#ifndef __ASM_SH_SWAB_H
+#define __ASM_SH_SWAB_H
+
+/*
+ * Copyright (C) 1999  Niibe Yutaka
+ * Copyright (C) 2000, 2001  Paolo Alberelli
+ */
+#include <linux/compiler.h>
+#include <linux/types.h>
+
+#define __SWAB_64_THRU_32__
+
+static inline __attribute_const__ __u32 __arch_swab32(__u32 x)
+{
+	__asm__(
+#ifdef __SH5__
+		"byterev	%0, %0\n\t"
+		"shari		%0, 32, %0"
+#else
+		"swap.b		%0, %0\n\t"
+		"swap.w		%0, %0\n\t"
+		"swap.b		%0, %0"
+#endif
+		: "=r" (x)
+		: "0" (x));
+
+	return x;
+}
+#define __arch_swab32 __arch_swab32
+
+static inline __attribute_const__ __u16 __arch_swab16(__u16 x)
+{
+	__asm__(
+#ifdef __SH5__
+		"byterev	%0, %0\n\t"
+		"shari		%0, 32, %0"
+#else
+		"swap.b		%0, %0"
+#endif
+		: "=r" (x)
+		:  "0" (x));
+
+	return x;
+}
+#define __arch_swab16 __arch_swab16
+
+static inline __u64 __arch_swab64(__u64 val)
+{
+	union {
+		struct { __u32 a,b; } s;
+		__u64 u;
+	} v, w;
+	v.u = val;
+	w.s.b = __arch_swab32(v.s.a);
+	w.s.a = __arch_swab32(v.s.b);
+	return w.u;
+}
+#define __arch_swab64 __arch_swab64
+
+#endif /* __ASM_SH_SWAB_H */
diff --git a/arch/sh/kernel/traps_32.c b/arch/sh/kernel/traps_32.c
index 88807a2aacc3..c0aa3d83ec0e 100644
--- a/arch/sh/kernel/traps_32.c
+++ b/arch/sh/kernel/traps_32.c
@@ -13,6 +13,7 @@
  */
 #include <linux/kernel.h>
 #include <linux/ptrace.h>
+#include <linux/hardirq.h>
 #include <linux/init.h>
 #include <linux/spinlock.h>
 #include <linux/module.h>
diff --git a/arch/sh/mm/init.c b/arch/sh/mm/init.c
index 6cbef8caeb56..3edf297c829b 100644
--- a/arch/sh/mm/init.c
+++ b/arch/sh/mm/init.c
@@ -311,7 +311,8 @@ int arch_add_memory(int nid, u64 start, u64 size)
 	pgdat = NODE_DATA(nid);
 
 	/* We only have ZONE_NORMAL, so this is easy.. */
-	ret = __add_pages(pgdat->node_zones + ZONE_NORMAL, start_pfn, nr_pages);
+	ret = __add_pages(nid, pgdat->node_zones + ZONE_NORMAL,
+				start_pfn, nr_pages);
 	if (unlikely(ret))
 		printk("%s: Failed, __add_pages() == %d\n", __func__, ret);
 
diff --git a/arch/sparc/include/asm/Kbuild b/arch/sparc/include/asm/Kbuild
index deeb0fba8029..95e38a43dff0 100644
--- a/arch/sparc/include/asm/Kbuild
+++ b/arch/sparc/include/asm/Kbuild
@@ -17,3 +17,4 @@ header-y += traps.h
 header-y += uctx.h
 header-y += utrap.h
 header-y += watchdog.h
+header-y += swab.h
diff --git a/arch/sparc/include/asm/atomic_32.h b/arch/sparc/include/asm/atomic_32.h
index 5c944b5a8040..ce465975a6a5 100644
--- a/arch/sparc/include/asm/atomic_32.h
+++ b/arch/sparc/include/asm/atomic_32.h
@@ -13,8 +13,6 @@
 
 #include <linux/types.h>
 
-typedef struct { volatile int counter; } atomic_t;
-
 #ifdef __KERNEL__
 
 #define ATOMIC_INIT(i)  { (i) }
diff --git a/arch/sparc/include/asm/atomic_64.h b/arch/sparc/include/asm/atomic_64.h
index 5982c5ae7f07..a0a706492696 100644
--- a/arch/sparc/include/asm/atomic_64.h
+++ b/arch/sparc/include/asm/atomic_64.h
@@ -10,9 +10,6 @@
 #include <linux/types.h>
 #include <asm/system.h>
 
-typedef struct { volatile int counter; } atomic_t;
-typedef struct { volatile __s64 counter; } atomic64_t;
-
 #define ATOMIC_INIT(i)		{ (i) }
 #define ATOMIC64_INIT(i)	{ (i) }
 
diff --git a/arch/sparc/include/asm/byteorder.h b/arch/sparc/include/asm/byteorder.h
index 738414b26558..48a047cd6fa9 100644
--- a/arch/sparc/include/asm/byteorder.h
+++ b/arch/sparc/include/asm/byteorder.h
@@ -1,49 +1,7 @@
 #ifndef _SPARC_BYTEORDER_H
 #define _SPARC_BYTEORDER_H
 
-#include <linux/types.h>
-#include <asm/asi.h>
-
-#define __BIG_ENDIAN
-
-#if defined(__sparc__) && defined(__arch64__)
-static inline __u16 __arch_swab16p(const __u16 *addr)
-{
-	__u16 ret;
-
-	__asm__ __volatile__ ("lduha [%1] %2, %0"
-			      : "=r" (ret)
-			      : "r" (addr), "i" (ASI_PL));
-	return ret;
-}
-#define __arch_swab16p __arch_swab16p
-
-static inline __u32 __arch_swab32p(const __u32 *addr)
-{
-	__u32 ret;
-
-	__asm__ __volatile__ ("lduwa [%1] %2, %0"
-			      : "=r" (ret)
-			      : "r" (addr), "i" (ASI_PL));
-	return ret;
-}
-#define __arch_swab32p __arch_swab32p
-
-static inline __u64 __arch_swab64p(const __u64 *addr)
-{
-	__u64 ret;
-
-	__asm__ __volatile__ ("ldxa [%1] %2, %0"
-			      : "=r" (ret)
-			      : "r" (addr), "i" (ASI_PL));
-	return ret;
-}
-#define __arch_swab64p __arch_swab64p
-
-#else
-#define __SWAB_64_THRU_32__
-#endif /* defined(__sparc__) && defined(__arch64__) */
-
-#include <linux/byteorder.h>
+#include <asm/swab.h>
+#include <linux/byteorder/big_endian.h>
 
 #endif /* _SPARC_BYTEORDER_H */
diff --git a/arch/sparc/include/asm/swab.h b/arch/sparc/include/asm/swab.h
new file mode 100644
index 000000000000..a34ad079487e
--- /dev/null
+++ b/arch/sparc/include/asm/swab.h
@@ -0,0 +1,45 @@
+#ifndef _SPARC_SWAB_H
+#define _SPARC_SWAB_H
+
+#include <linux/types.h>
+#include <asm/asi.h>
+
+#if defined(__sparc__) && defined(__arch64__)
+static inline __u16 __arch_swab16p(const __u16 *addr)
+{
+	__u16 ret;
+
+	__asm__ __volatile__ ("lduha [%1] %2, %0"
+			      : "=r" (ret)
+			      : "r" (addr), "i" (ASI_PL));
+	return ret;
+}
+#define __arch_swab16p __arch_swab16p
+
+static inline __u32 __arch_swab32p(const __u32 *addr)
+{
+	__u32 ret;
+
+	__asm__ __volatile__ ("lduwa [%1] %2, %0"
+			      : "=r" (ret)
+			      : "r" (addr), "i" (ASI_PL));
+	return ret;
+}
+#define __arch_swab32p __arch_swab32p
+
+static inline __u64 __arch_swab64p(const __u64 *addr)
+{
+	__u64 ret;
+
+	__asm__ __volatile__ ("ldxa [%1] %2, %0"
+			      : "=r" (ret)
+			      : "r" (addr), "i" (ASI_PL));
+	return ret;
+}
+#define __arch_swab64p __arch_swab64p
+
+#else
+#define __SWAB_64_THRU_32__
+#endif /* defined(__sparc__) && defined(__arch64__) */
+
+#endif /* _SPARC_SWAB_H */
diff --git a/arch/sparc/kernel/kprobes.c b/arch/sparc/kernel/kprobes.c
index 201a6e547e4a..3bc6527c95af 100644
--- a/arch/sparc/kernel/kprobes.c
+++ b/arch/sparc/kernel/kprobes.c
@@ -517,7 +517,7 @@ int __kprobes trampoline_probe_handler(struct kprobe *p, struct pt_regs *regs)
 	/*
 	 * It is possible to have multiple instances associated with a given
 	 * task either because an multiple functions in the call path
-	 * have a return probe installed on them, and/or more then one return
+	 * have a return probe installed on them, and/or more than one return
 	 * return probe was registered for a target function.
 	 *
 	 * We can handle this because:
diff --git a/arch/um/kernel/trap.c b/arch/um/kernel/trap.c
index 44e490419495..7384d8accfe7 100644
--- a/arch/um/kernel/trap.c
+++ b/arch/um/kernel/trap.c
@@ -64,11 +64,10 @@ good_area:
 
 	do {
 		int fault;
-survive:
+
 		fault = handle_mm_fault(mm, vma, address, is_write);
 		if (unlikely(fault & VM_FAULT_ERROR)) {
 			if (fault & VM_FAULT_OOM) {
-				err = -ENOMEM;
 				goto out_of_memory;
 			} else if (fault & VM_FAULT_SIGBUS) {
 				err = -EACCES;
@@ -104,18 +103,14 @@ out:
 out_nosemaphore:
 	return err;
 
-/*
- * We ran out of memory, or some other thing happened to us that made
- * us unable to handle the page fault gracefully.
- */
 out_of_memory:
-	if (is_global_init(current)) {
-		up_read(&mm->mmap_sem);
-		yield();
-		down_read(&mm->mmap_sem);
-		goto survive;
-	}
-	goto out;
+	/*
+	 * We ran out of memory, call the OOM killer, and return the userspace
+	 * (which will retry the fault, or kill us if we got oom-killed).
+	 */
+	up_read(&mm->mmap_sem);
+	pagefault_out_of_memory();
+	return 0;
 }
 
 static void bad_segv(struct faultinfo fi, unsigned long ip)
@@ -214,9 +209,6 @@ unsigned long segv(struct faultinfo fi, unsigned long ip, int is_user,
 		si.si_addr = (void __user *)address;
 		current->thread.arch.faultinfo = fi;
 		force_sig_info(SIGBUS, &si, current);
-	} else if (err == -ENOMEM) {
-		printk(KERN_INFO "VM: killing process %s\n", current->comm);
-		do_exit(SIGKILL);
 	} else {
 		BUG_ON(err != -EFAULT);
 		si.si_signo = SIGSEGV;
diff --git a/arch/x86/include/asm/Kbuild b/arch/x86/include/asm/Kbuild
index 4a8e80cdcfa5..a9f8a814a1f7 100644
--- a/arch/x86/include/asm/Kbuild
+++ b/arch/x86/include/asm/Kbuild
@@ -22,3 +22,4 @@ unifdef-y += unistd_32.h
 unifdef-y += unistd_64.h
 unifdef-y += vm86.h
 unifdef-y += vsyscall.h
+unifdef-y += swab.h
diff --git a/arch/x86/include/asm/atomic_32.h b/arch/x86/include/asm/atomic_32.h
index ad5b9f6ecddf..85b46fba4229 100644
--- a/arch/x86/include/asm/atomic_32.h
+++ b/arch/x86/include/asm/atomic_32.h
@@ -2,6 +2,7 @@
 #define _ASM_X86_ATOMIC_32_H
 
 #include <linux/compiler.h>
+#include <linux/types.h>
 #include <asm/processor.h>
 #include <asm/cmpxchg.h>
 
@@ -10,15 +11,6 @@
  * resource counting etc..
  */
 
-/*
- * Make sure gcc doesn't try to be clever and move things around
- * on us. We need to use _exactly_ the address the user gave us,
- * not some alias that contains the same information.
- */
-typedef struct {
-	int counter;
-} atomic_t;
-
 #define ATOMIC_INIT(i)	{ (i) }
 
 /**
diff --git a/arch/x86/include/asm/atomic_64.h b/arch/x86/include/asm/atomic_64.h
index 279d2a731f3f..8c21731984da 100644
--- a/arch/x86/include/asm/atomic_64.h
+++ b/arch/x86/include/asm/atomic_64.h
@@ -1,25 +1,15 @@
 #ifndef _ASM_X86_ATOMIC_64_H
 #define _ASM_X86_ATOMIC_64_H
 
+#include <linux/types.h>
 #include <asm/alternative.h>
 #include <asm/cmpxchg.h>
 
-/* atomic_t should be 32 bit signed type */
-
 /*
  * Atomic operations that C can't guarantee us.  Useful for
  * resource counting etc..
  */
 
-/*
- * Make sure gcc doesn't try to be clever and move things around
- * on us. We need to use _exactly_ the address the user gave us,
- * not some alias that contains the same information.
- */
-typedef struct {
-	int counter;
-} atomic_t;
-
 #define ATOMIC_INIT(i)	{ (i) }
 
 /**
@@ -191,11 +181,7 @@ static inline int atomic_sub_return(int i, atomic_t *v)
 #define atomic_inc_return(v)  (atomic_add_return(1, v))
 #define atomic_dec_return(v)  (atomic_sub_return(1, v))
 
-/* An 64bit atomic type */
-
-typedef struct {
-	long counter;
-} atomic64_t;
+/* The 64-bit atomic type */
 
 #define ATOMIC64_INIT(i)	{ (i) }
 
diff --git a/arch/x86/include/asm/byteorder.h b/arch/x86/include/asm/byteorder.h
index f110ad417df3..7c49917e3d9d 100644
--- a/arch/x86/include/asm/byteorder.h
+++ b/arch/x86/include/asm/byteorder.h
@@ -1,65 +1,7 @@
 #ifndef _ASM_X86_BYTEORDER_H
 #define _ASM_X86_BYTEORDER_H
 
-#include <asm/types.h>
-#include <linux/compiler.h>
-
-#define __LITTLE_ENDIAN
-
-static inline __attribute_const__ __u32 __arch_swab32(__u32 val)
-{
-#ifdef __i386__
-# ifdef CONFIG_X86_BSWAP
-	asm("bswap %0" : "=r" (val) : "0" (val));
-# else
-	asm("xchgb %b0,%h0\n\t"	/* swap lower bytes	*/
-	    "rorl $16,%0\n\t"	/* swap words		*/
-	    "xchgb %b0,%h0"	/* swap higher bytes	*/
-	    : "=q" (val)
-	    : "0" (val));
-# endif
-
-#else /* __i386__ */
-	asm("bswapl %0"
-	    : "=r" (val)
-	    : "0" (val));
-#endif
-	return val;
-}
-#define __arch_swab32 __arch_swab32
-
-static inline __attribute_const__ __u64 __arch_swab64(__u64 val)
-{
-#ifdef __i386__
-	union {
-		struct {
-			__u32 a;
-			__u32 b;
-		} s;
-		__u64 u;
-	} v;
-	v.u = val;
-# ifdef CONFIG_X86_BSWAP
-	asm("bswapl %0 ; bswapl %1 ; xchgl %0,%1"
-	    : "=r" (v.s.a), "=r" (v.s.b)
-	    : "0" (v.s.a), "1" (v.s.b));
-# else
-	v.s.a = __arch_swab32(v.s.a);
-	v.s.b = __arch_swab32(v.s.b);
-	asm("xchgl %0,%1"
-	    : "=r" (v.s.a), "=r" (v.s.b)
-	    : "0" (v.s.a), "1" (v.s.b));
-# endif
-	return v.u;
-#else /* __i386__ */
-	asm("bswapq %0"
-	    : "=r" (val)
-	    : "0" (val));
-	return val;
-#endif
-}
-#define __arch_swab64 __arch_swab64
-
-#include <linux/byteorder.h>
+#include <asm/swab.h>
+#include <linux/byteorder/little_endian.h>
 
 #endif /* _ASM_X86_BYTEORDER_H */
diff --git a/arch/x86/include/asm/swab.h b/arch/x86/include/asm/swab.h
new file mode 100644
index 000000000000..306d4178ffc9
--- /dev/null
+++ b/arch/x86/include/asm/swab.h
@@ -0,0 +1,61 @@
+#ifndef _ASM_X86_SWAB_H
+#define _ASM_X86_SWAB_H
+
+#include <asm/types.h>
+#include <linux/compiler.h>
+
+static inline __attribute_const__ __u32 __arch_swab32(__u32 val)
+{
+#ifdef __i386__
+# ifdef CONFIG_X86_BSWAP
+	asm("bswap %0" : "=r" (val) : "0" (val));
+# else
+	asm("xchgb %b0,%h0\n\t"	/* swap lower bytes	*/
+	    "rorl $16,%0\n\t"	/* swap words		*/
+	    "xchgb %b0,%h0"	/* swap higher bytes	*/
+	    : "=q" (val)
+	    : "0" (val));
+# endif
+
+#else /* __i386__ */
+	asm("bswapl %0"
+	    : "=r" (val)
+	    : "0" (val));
+#endif
+	return val;
+}
+#define __arch_swab32 __arch_swab32
+
+static inline __attribute_const__ __u64 __arch_swab64(__u64 val)
+{
+#ifdef __i386__
+	union {
+		struct {
+			__u32 a;
+			__u32 b;
+		} s;
+		__u64 u;
+	} v;
+	v.u = val;
+# ifdef CONFIG_X86_BSWAP
+	asm("bswapl %0 ; bswapl %1 ; xchgl %0,%1"
+	    : "=r" (v.s.a), "=r" (v.s.b)
+	    : "0" (v.s.a), "1" (v.s.b));
+# else
+	v.s.a = __arch_swab32(v.s.a);
+	v.s.b = __arch_swab32(v.s.b);
+	asm("xchgl %0,%1"
+	    : "=r" (v.s.a), "=r" (v.s.b)
+	    : "0" (v.s.a), "1" (v.s.b));
+# endif
+	return v.u;
+#else /* __i386__ */
+	asm("bswapq %0"
+	    : "=r" (val)
+	    : "0" (val));
+	return val;
+#endif
+}
+#define __arch_swab64 __arch_swab64
+
+#endif /* _ASM_X86_SWAB_H */
diff --git a/arch/x86/include/asm/swiotlb.h b/arch/x86/include/asm/swiotlb.h
index 51fb2c76ad74..b9e4e20174fb 100644
--- a/arch/x86/include/asm/swiotlb.h
+++ b/arch/x86/include/asm/swiotlb.h
@@ -1,46 +1,10 @@
 #ifndef _ASM_X86_SWIOTLB_H
 #define _ASM_X86_SWIOTLB_H
 
-#include <asm/dma-mapping.h>
+#include <linux/swiotlb.h>
 
 /* SWIOTLB interface */
 
-extern dma_addr_t swiotlb_map_single(struct device *hwdev, void *ptr,
-				     size_t size, int dir);
-extern void *swiotlb_alloc_coherent(struct device *hwdev, size_t size,
-				    dma_addr_t *dma_handle, gfp_t flags);
-extern void swiotlb_unmap_single(struct device *hwdev, dma_addr_t dev_addr,
-				 size_t size, int dir);
-extern void swiotlb_sync_single_for_cpu(struct device *hwdev,
-					dma_addr_t dev_addr,
-					size_t size, int dir);
-extern void swiotlb_sync_single_for_device(struct device *hwdev,
-					   dma_addr_t dev_addr,
-					   size_t size, int dir);
-extern void swiotlb_sync_single_range_for_cpu(struct device *hwdev,
-					      dma_addr_t dev_addr,
-					      unsigned long offset,
-					      size_t size, int dir);
-extern void swiotlb_sync_single_range_for_device(struct device *hwdev,
-						 dma_addr_t dev_addr,
-						 unsigned long offset,
-						 size_t size, int dir);
-extern void swiotlb_sync_sg_for_cpu(struct device *hwdev,
-				    struct scatterlist *sg, int nelems,
-				    int dir);
-extern void swiotlb_sync_sg_for_device(struct device *hwdev,
-				       struct scatterlist *sg, int nelems,
-				       int dir);
-extern int swiotlb_map_sg(struct device *hwdev, struct scatterlist *sg,
-			  int nents, int direction);
-extern void swiotlb_unmap_sg(struct device *hwdev, struct scatterlist *sg,
-			     int nents, int direction);
-extern int swiotlb_dma_mapping_error(struct device *hwdev, dma_addr_t dma_addr);
-extern void swiotlb_free_coherent(struct device *hwdev, size_t size,
-				  void *vaddr, dma_addr_t dma_handle);
-extern int swiotlb_dma_supported(struct device *hwdev, u64 mask);
-extern void swiotlb_init(void);
-
 extern int swiotlb_force;
 
 #ifdef CONFIG_SWIOTLB
diff --git a/arch/x86/include/asm/unwind.h b/arch/x86/include/asm/unwind.h
deleted file mode 100644
index 8b064bd9c553..000000000000
--- a/arch/x86/include/asm/unwind.h
+++ /dev/null
@@ -1,13 +0,0 @@
-#ifndef _ASM_X86_UNWIND_H
-#define _ASM_X86_UNWIND_H
-
-#define UNW_PC(frame) ((void)(frame), 0UL)
-#define UNW_SP(frame) ((void)(frame), 0UL)
-#define UNW_FP(frame) ((void)(frame), 0UL)
-
-static inline int arch_unw_user_mode(const void *info)
-{
-	return 0;
-}
-
-#endif /* _ASM_X86_UNWIND_H */
diff --git a/arch/x86/kernel/kprobes.c b/arch/x86/kernel/kprobes.c
index 6c27679ec6aa..884d985b8b82 100644
--- a/arch/x86/kernel/kprobes.c
+++ b/arch/x86/kernel/kprobes.c
@@ -376,9 +376,10 @@ void __kprobes arch_disarm_kprobe(struct kprobe *p)
 
 void __kprobes arch_remove_kprobe(struct kprobe *p)
 {
-	mutex_lock(&kprobe_mutex);
-	free_insn_slot(p->ainsn.insn, (p->ainsn.boostable == 1));
-	mutex_unlock(&kprobe_mutex);
+	if (p->ainsn.insn) {
+		free_insn_slot(p->ainsn.insn, (p->ainsn.boostable == 1));
+		p->ainsn.insn = NULL;
+	}
 }
 
 static void __kprobes save_previous_kprobe(struct kprobe_ctlblk *kcb)
@@ -694,7 +695,7 @@ static __used __kprobes void *trampoline_handler(struct pt_regs *regs)
 	/*
 	 * It is possible to have multiple instances associated with a given
 	 * task either because multiple functions in the call path have
-	 * return probes installed on them, and/or more then one
+	 * return probes installed on them, and/or more than one
 	 * return probe was registered for a target function.
 	 *
 	 * We can handle this because:
diff --git a/arch/x86/kernel/mfgpt_32.c b/arch/x86/kernel/mfgpt_32.c
index c12314c9e86f..8815f3c7fec7 100644
--- a/arch/x86/kernel/mfgpt_32.c
+++ b/arch/x86/kernel/mfgpt_32.c
@@ -252,7 +252,7 @@ EXPORT_SYMBOL_GPL(geode_mfgpt_alloc_timer);
 /*
  * The MFPGT timers on the CS5536 provide us with suitable timers to use
  * as clock event sources - not as good as a HPET or APIC, but certainly
- * better then the PIT.  This isn't a general purpose MFGPT driver, but
+ * better than the PIT.  This isn't a general purpose MFGPT driver, but
  * a simplified one designed specifically to act as a clock event source.
  * For full details about the MFGPT, please consult the CS5536 data sheet.
  */
diff --git a/arch/x86/kernel/pci-dma.c b/arch/x86/kernel/pci-dma.c
index 19a1044a0cd9..b25428533141 100644
--- a/arch/x86/kernel/pci-dma.c
+++ b/arch/x86/kernel/pci-dma.c
@@ -38,7 +38,7 @@ EXPORT_SYMBOL(bad_dma_address);
    be probably a smaller DMA mask, but this is bug-to-bug compatible
    to older i386. */
 struct device x86_dma_fallback_dev = {
-	.bus_id = "fallback device",
+	.init_name = "fallback device",
 	.coherent_dma_mask = DMA_32BIT_MASK,
 	.dma_mask = &x86_dma_fallback_dev.coherent_dma_mask,
 };
diff --git a/arch/x86/kernel/pci-swiotlb_64.c b/arch/x86/kernel/pci-swiotlb_64.c
index 8cba3749a511..d59c91747665 100644
--- a/arch/x86/kernel/pci-swiotlb_64.c
+++ b/arch/x86/kernel/pci-swiotlb_64.c
@@ -23,7 +23,7 @@ void *swiotlb_alloc(unsigned order, unsigned long nslabs)
 	return (void *)__get_free_pages(GFP_DMA | __GFP_NOWARN, order);
 }
 
-dma_addr_t swiotlb_phys_to_bus(phys_addr_t paddr)
+dma_addr_t swiotlb_phys_to_bus(struct device *hwdev, phys_addr_t paddr)
 {
 	return paddr;
 }
diff --git a/arch/x86/kernel/traps.c b/arch/x86/kernel/traps.c
index ce6650eb64e9..c9a666cdd3db 100644
--- a/arch/x86/kernel/traps.c
+++ b/arch/x86/kernel/traps.c
@@ -20,7 +20,6 @@
 #include <linux/module.h>
 #include <linux/ptrace.h>
 #include <linux/string.h>
-#include <linux/unwind.h>
 #include <linux/delay.h>
 #include <linux/errno.h>
 #include <linux/kexec.h>
@@ -51,7 +50,6 @@
 #include <asm/debugreg.h>
 #include <asm/atomic.h>
 #include <asm/system.h>
-#include <asm/unwind.h>
 #include <asm/traps.h>
 #include <asm/desc.h>
 #include <asm/i387.h>
diff --git a/arch/x86/mm/fault.c b/arch/x86/mm/fault.c
index 57ec8c86a877..9e268b6b204e 100644
--- a/arch/x86/mm/fault.c
+++ b/arch/x86/mm/fault.c
@@ -667,7 +667,6 @@ void __kprobes do_page_fault(struct pt_regs *regs, unsigned long error_code)
 	if (unlikely(in_atomic() || !mm))
 		goto bad_area_nosemaphore;
 
-again:
 	/*
 	 * When running in the kernel we expect faults to occur only to
 	 * addresses in user space.  All other faults represent errors in the
@@ -859,25 +858,14 @@ no_context:
 	oops_end(flags, regs, sig);
 #endif
 
-/*
- * We ran out of memory, or some other thing happened to us that made
- * us unable to handle the page fault gracefully.
- */
 out_of_memory:
+	/*
+	 * We ran out of memory, call the OOM killer, and return the userspace
+	 * (which will retry the fault, or kill us if we got oom-killed).
+	 */
 	up_read(&mm->mmap_sem);
-	if (is_global_init(tsk)) {
-		yield();
-		/*
-		 * Re-lookup the vma - in theory the vma tree might
-		 * have changed:
-		 */
-		goto again;
-	}
-
-	printk("VM: killing process %s\n", tsk->comm);
-	if (error_code & PF_USER)
-		do_group_exit(SIGKILL);
-	goto no_context;
+	pagefault_out_of_memory();
+	return;
 
 do_sigbus:
 	up_read(&mm->mmap_sem);
diff --git a/arch/x86/mm/init_32.c b/arch/x86/mm/init_32.c
index f99a6c6c432e..88f1b10de3be 100644
--- a/arch/x86/mm/init_32.c
+++ b/arch/x86/mm/init_32.c
@@ -328,6 +328,8 @@ int devmem_is_allowed(unsigned long pagenr)
 {
 	if (pagenr <= 256)
 		return 1;
+	if (iomem_is_exclusive(pagenr << PAGE_SHIFT))
+		return 0;
 	if (!page_is_ram(pagenr))
 		return 1;
 	return 0;
@@ -1079,7 +1081,7 @@ int arch_add_memory(int nid, u64 start, u64 size)
 	unsigned long start_pfn = start >> PAGE_SHIFT;
 	unsigned long nr_pages = size >> PAGE_SHIFT;
 
-	return __add_pages(zone, start_pfn, nr_pages);
+	return __add_pages(nid, zone, start_pfn, nr_pages);
 }
 #endif
 
diff --git a/arch/x86/mm/init_64.c b/arch/x86/mm/init_64.c
index 9f7a0d24d42a..23f68e77ad1f 100644
--- a/arch/x86/mm/init_64.c
+++ b/arch/x86/mm/init_64.c
@@ -857,7 +857,7 @@ int arch_add_memory(int nid, u64 start, u64 size)
 	if (last_mapped_pfn > max_pfn_mapped)
 		max_pfn_mapped = last_mapped_pfn;
 
-	ret = __add_pages(zone, start_pfn, nr_pages);
+	ret = __add_pages(nid, zone, start_pfn, nr_pages);
 	WARN_ON_ONCE(ret);
 
 	return ret;
@@ -888,6 +888,8 @@ int devmem_is_allowed(unsigned long pagenr)
 {
 	if (pagenr <= 256)
 		return 1;
+	if (iomem_is_exclusive(pagenr << PAGE_SHIFT))
+		return 0;
 	if (!page_is_ram(pagenr))
 		return 1;
 	return 0;
diff --git a/arch/x86/mm/numa_32.c b/arch/x86/mm/numa_32.c
index 8518c678d83f..d1f7439d173c 100644
--- a/arch/x86/mm/numa_32.c
+++ b/arch/x86/mm/numa_32.c
@@ -239,7 +239,7 @@ void resume_map_numa_kva(pgd_t *pgd_base)
 		start_pfn = node_remap_start_pfn[node];
 		size = node_remap_size[node];
 
-		printk(KERN_DEBUG "%s: node %d\n", __FUNCTION__, node);
+		printk(KERN_DEBUG "%s: node %d\n", __func__, node);
 
 		for (pfn = 0; pfn < size; pfn += PTRS_PER_PTE) {
 			unsigned long vaddr = start_va + (pfn << PAGE_SHIFT);
@@ -251,7 +251,7 @@ void resume_map_numa_kva(pgd_t *pgd_base)
 						PAGE_KERNEL_LARGE_EXEC));
 
 			printk(KERN_DEBUG "%s: %08lx -> pfn %08lx\n",
-				__FUNCTION__, vaddr, start_pfn + pfn);
+				__func__, vaddr, start_pfn + pfn);
 		}
 	}
 }
diff --git a/arch/x86/pci/acpi.c b/arch/x86/pci/acpi.c
index 9e5752fe4d15..c0ecf250fe51 100644
--- a/arch/x86/pci/acpi.c
+++ b/arch/x86/pci/acpi.c
@@ -210,11 +210,10 @@ struct pci_bus * __devinit pci_acpi_scan_root(struct acpi_device *device, int do
 	if (bus && node != -1) {
 #ifdef CONFIG_ACPI_NUMA
 		if (pxm >= 0)
-			printk(KERN_DEBUG "bus %02x -> pxm %d -> node %d\n",
-				busnum, pxm, node);
+			dev_printk(KERN_DEBUG, &bus->dev,
+				   "on NUMA node %d (pxm %d)\n", node, pxm);
 #else
-		printk(KERN_DEBUG "bus %02x -> node %d\n",
-			busnum, node);
+		dev_printk(KERN_DEBUG, &bus->dev, "on NUMA node %d\n", node);
 #endif
 	}
 
diff --git a/arch/x86/pci/common.c b/arch/x86/pci/common.c
index 62ddb73e09ed..82d22fc601ae 100644
--- a/arch/x86/pci/common.c
+++ b/arch/x86/pci/common.c
@@ -551,17 +551,25 @@ int pcibios_enable_device(struct pci_dev *dev, int mask)
 	if ((err = pci_enable_resources(dev, mask)) < 0)
 		return err;
 
-	if (!dev->msi_enabled)
+	if (!pci_dev_msi_enabled(dev))
 		return pcibios_enable_irq(dev);
 	return 0;
 }
 
 void pcibios_disable_device (struct pci_dev *dev)
 {
-	if (!dev->msi_enabled && pcibios_disable_irq)
+	if (!pci_dev_msi_enabled(dev) && pcibios_disable_irq)
 		pcibios_disable_irq(dev);
 }
 
+int pci_ext_cfg_avail(struct pci_dev *dev)
+{
+	if (raw_pci_ext_ops)
+		return 1;
+	else
+		return 0;
+}
+
 struct pci_bus * __devinit pci_scan_bus_on_node(int busno, struct pci_ops *ops, int node)
 {
 	struct pci_bus *bus = NULL;
diff --git a/arch/x86/pci/i386.c b/arch/x86/pci/i386.c
index e51bf2cda4b0..f884740da318 100644
--- a/arch/x86/pci/i386.c
+++ b/arch/x86/pci/i386.c
@@ -129,7 +129,7 @@ static void __init pcibios_allocate_bus_resources(struct list_head *bus_list)
 				pr = pci_find_parent_resource(dev, r);
 				if (!r->start || !pr ||
 				    request_resource(pr, r) < 0) {
-					dev_err(&dev->dev, "BAR %d: can't allocate resource\n", idx);
+					dev_info(&dev->dev, "BAR %d: can't allocate resource\n", idx);
 					/*
 					 * Something is wrong with the region.
 					 * Invalidate the resource to prevent
@@ -170,7 +170,7 @@ static void __init pcibios_allocate_resources(int pass)
 					r->flags, disabled, pass);
 				pr = pci_find_parent_resource(dev, r);
 				if (!pr || request_resource(pr, r) < 0) {
-					dev_err(&dev->dev, "BAR %d: can't allocate resource\n", idx);
+					dev_info(&dev->dev, "BAR %d: can't allocate resource\n", idx);
 					/* We'll assign a new address later */
 					r->end -= r->start;
 					r->start = 0;
diff --git a/arch/x86/pci/init.c b/arch/x86/pci/init.c
index bec3b048e72b..25a1f8efed4a 100644
--- a/arch/x86/pci/init.c
+++ b/arch/x86/pci/init.c
@@ -12,7 +12,8 @@ static __init int pci_arch_init(void)
 	type = pci_direct_probe();
 #endif
 
-	pci_mmcfg_early_init();
+	if (!(pci_probe & PCI_PROBE_NOEARLY))
+		pci_mmcfg_early_init();
 
 #ifdef CONFIG_PCI_OLPC
 	if (!pci_olpc_init())
diff --git a/arch/x86/pci/irq.c b/arch/x86/pci/irq.c
index 373b9afe6d44..4064345cf144 100644
--- a/arch/x86/pci/irq.c
+++ b/arch/x86/pci/irq.c
@@ -533,7 +533,7 @@ static int pirq_bios_set(struct pci_dev *router, struct pci_dev *dev, int pirq,
 {
 	struct pci_dev *bridge;
 	int pin = pci_get_interrupt_pin(dev, &bridge);
-	return pcibios_set_irq_routing(bridge, pin, irq);
+	return pcibios_set_irq_routing(bridge, pin - 1, irq);
 }
 
 #endif
@@ -887,7 +887,6 @@ static int pcibios_lookup_irq(struct pci_dev *dev, int assign)
 		dev_dbg(&dev->dev, "no interrupt pin\n");
 		return 0;
 	}
-	pin = pin - 1;
 
 	/* Find IRQ routing entry */
 
@@ -897,17 +896,17 @@ static int pcibios_lookup_irq(struct pci_dev *dev, int assign)
 	info = pirq_get_info(dev);
 	if (!info) {
 		dev_dbg(&dev->dev, "PCI INT %c not found in routing table\n",
-			'A' + pin);
+			'A' + pin - 1);
 		return 0;
 	}
-	pirq = info->irq[pin].link;
-	mask = info->irq[pin].bitmap;
+	pirq = info->irq[pin - 1].link;
+	mask = info->irq[pin - 1].bitmap;
 	if (!pirq) {
-		dev_dbg(&dev->dev, "PCI INT %c not routed\n", 'A' + pin);
+		dev_dbg(&dev->dev, "PCI INT %c not routed\n", 'A' + pin - 1);
 		return 0;
 	}
 	dev_dbg(&dev->dev, "PCI INT %c -> PIRQ %02x, mask %04x, excl %04x",
-		'A' + pin, pirq, mask, pirq_table->exclusive_irqs);
+		'A' + pin - 1, pirq, mask, pirq_table->exclusive_irqs);
 	mask &= pcibios_irq_mask;
 
 	/* Work around broken HP Pavilion Notebooks which assign USB to
@@ -949,7 +948,7 @@ static int pcibios_lookup_irq(struct pci_dev *dev, int assign)
 				newirq = i;
 		}
 	}
-	dev_dbg(&dev->dev, "PCI INT %c -> newirq %d", 'A' + pin, newirq);
+	dev_dbg(&dev->dev, "PCI INT %c -> newirq %d", 'A' + pin - 1, newirq);
 
 	/* Check if it is hardcoded */
 	if ((pirq & 0xf0) == 0xf0) {
@@ -977,18 +976,18 @@ static int pcibios_lookup_irq(struct pci_dev *dev, int assign)
 			return 0;
 		}
 	}
-	dev_info(&dev->dev, "%s PCI INT %c -> IRQ %d\n", msg, 'A' + pin, irq);
+	dev_info(&dev->dev, "%s PCI INT %c -> IRQ %d\n", msg, 'A' + pin - 1, irq);
 
 	/* Update IRQ for all devices with the same pirq value */
 	while ((dev2 = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, dev2)) != NULL) {
 		pci_read_config_byte(dev2, PCI_INTERRUPT_PIN, &pin);
 		if (!pin)
 			continue;
-		pin--;
+
 		info = pirq_get_info(dev2);
 		if (!info)
 			continue;
-		if (info->irq[pin].link == pirq) {
+		if (info->irq[pin - 1].link == pirq) {
 			/*
 			 * We refuse to override the dev->irq
 			 * information. Give a warning!
@@ -1042,6 +1041,9 @@ static void __init pcibios_fixup_irqs(void)
 	dev = NULL;
 	while ((dev = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, dev)) != NULL) {
 		pci_read_config_byte(dev, PCI_INTERRUPT_PIN, &pin);
+		if (!pin)
+			continue;
+
 #ifdef CONFIG_X86_IO_APIC
 		/*
 		 * Recalculate IRQ numbers if we use the I/O APIC.
@@ -1049,15 +1051,11 @@ static void __init pcibios_fixup_irqs(void)
 		if (io_apic_assign_pci_irqs) {
 			int irq;
 
-			if (!pin)
-				continue;
-
 			/*
 			 * interrupt pins are numbered starting from 1
 			 */
-			pin--;
 			irq = IO_APIC_get_PCI_irq_vector(dev->bus->number,
-				PCI_SLOT(dev->devfn), pin);
+				PCI_SLOT(dev->devfn), pin - 1);
 			/*
 			 * Busses behind bridges are typically not listed in the
 			 * MP-table.  In this case we have to look up the IRQ
@@ -1070,22 +1068,22 @@ static void __init pcibios_fixup_irqs(void)
 				struct pci_dev *bridge = dev->bus->self;
 				int bus;
 
-				pin = (pin + PCI_SLOT(dev->devfn)) % 4;
+				pin = pci_swizzle_interrupt_pin(dev, pin);
 				bus = bridge->bus->number;
 				irq = IO_APIC_get_PCI_irq_vector(bus,
-						PCI_SLOT(bridge->devfn), pin);
+						PCI_SLOT(bridge->devfn), pin - 1);
 				if (irq >= 0)
 					dev_warn(&dev->dev,
 						"using bridge %s INT %c to "
 							"get IRQ %d\n",
 						 pci_name(bridge),
-						 'A' + pin, irq);
+						 'A' + pin - 1, irq);
 			}
 			if (irq >= 0) {
 				dev_info(&dev->dev,
 					"PCI->APIC IRQ transform: INT %c "
 						"-> IRQ %d\n",
-					'A' + pin, irq);
+					'A' + pin - 1, irq);
 				dev->irq = irq;
 			}
 		}
@@ -1093,7 +1091,7 @@ static void __init pcibios_fixup_irqs(void)
 		/*
 		 * Still no IRQ? Try to lookup one...
 		 */
-		if (pin && !dev->irq)
+		if (!dev->irq)
 			pcibios_lookup_irq(dev, 0);
 	}
 }
@@ -1220,12 +1218,10 @@ static int pirq_enable_irq(struct pci_dev *dev)
 	if (pin && !pcibios_lookup_irq(dev, 1) && !dev->irq) {
 		char *msg = "";
 
-		pin--; /* interrupt pins are numbered starting from 1 */
-
 		if (io_apic_assign_pci_irqs) {
 			int irq;
 
-			irq = IO_APIC_get_PCI_irq_vector(dev->bus->number, PCI_SLOT(dev->devfn), pin);
+			irq = IO_APIC_get_PCI_irq_vector(dev->bus->number, PCI_SLOT(dev->devfn), pin - 1);
 			/*
 			 * Busses behind bridges are typically not listed in the MP-table.
 			 * In this case we have to look up the IRQ based on the parent bus,
@@ -1236,20 +1232,20 @@ static int pirq_enable_irq(struct pci_dev *dev)
 			while (irq < 0 && dev->bus->parent) { /* go back to the bridge */
 				struct pci_dev *bridge = dev->bus->self;
 
-				pin = (pin + PCI_SLOT(dev->devfn)) % 4;
+				pin = pci_swizzle_interrupt_pin(dev, pin);
 				irq = IO_APIC_get_PCI_irq_vector(bridge->bus->number,
-						PCI_SLOT(bridge->devfn), pin);
+						PCI_SLOT(bridge->devfn), pin - 1);
 				if (irq >= 0)
 					dev_warn(&dev->dev, "using bridge %s "
 						 "INT %c to get IRQ %d\n",
-						 pci_name(bridge), 'A' + pin,
+						 pci_name(bridge), 'A' + pin - 1,
 						 irq);
 				dev = bridge;
 			}
 			dev = temp_dev;
 			if (irq >= 0) {
 				dev_info(&dev->dev, "PCI->APIC IRQ transform: "
-					 "INT %c -> IRQ %d\n", 'A' + pin, irq);
+					 "INT %c -> IRQ %d\n", 'A' + pin - 1, irq);
 				dev->irq = irq;
 				return 0;
 			} else
@@ -1268,7 +1264,7 @@ static int pirq_enable_irq(struct pci_dev *dev)
 			return 0;
 
 		dev_warn(&dev->dev, "can't find IRQ for PCI INT %c%s\n",
-			 'A' + pin, msg);
+			 'A' + pin - 1, msg);
 	}
 	return 0;
 }
diff --git a/arch/x86/pci/visws.c b/arch/x86/pci/visws.c
index 16d0c0eb0d19..bcead7a46871 100644
--- a/arch/x86/pci/visws.c
+++ b/arch/x86/pci/visws.c
@@ -24,24 +24,6 @@ static void pci_visws_disable_irq(struct pci_dev *dev) { }
 
 unsigned int pci_bus0, pci_bus1;
 
-static inline u8 bridge_swizzle(u8 pin, u8 slot) 
-{
-	return (((pin - 1) + slot) % 4) + 1;
-}
-
-static u8 __init visws_swizzle(struct pci_dev *dev, u8 *pinp)
-{
-	u8 pin = *pinp;
-
-	while (dev->bus->self) {	/* Move up the chain of bridges. */
-		pin = bridge_swizzle(pin, PCI_SLOT(dev->devfn));
-		dev = dev->bus->self;
-	}
-	*pinp = pin;
-
-	return PCI_SLOT(dev->devfn);
-}
-
 static int __init visws_map_irq(struct pci_dev *dev, u8 slot, u8 pin)
 {
 	int irq, bus = dev->bus->number;
@@ -106,7 +88,7 @@ int __init pci_visws_init(void)
 	raw_pci_ops = &pci_direct_conf1;
 	pci_scan_bus_with_sysdata(pci_bus0);
 	pci_scan_bus_with_sysdata(pci_bus1);
-	pci_fixup_irqs(visws_swizzle, visws_map_irq);
+	pci_fixup_irqs(pci_common_swizzle, visws_map_irq);
 	pcibios_resource_survey();
 	return 0;
 }
diff --git a/arch/xtensa/Makefile b/arch/xtensa/Makefile
index 015b6b2a26b9..1da55fe4beff 100644
--- a/arch/xtensa/Makefile
+++ b/arch/xtensa/Makefile
@@ -33,6 +33,15 @@ KBUILD_CFLAGS += -ffreestanding
 
 KBUILD_CFLAGS += -pipe -mlongcalls
 
+vardirs := $(patsubst %,arch/xtensa/variants/%/,$(variant-y))
+plfdirs := $(patsubst %,arch/xtensa/platforms/%/,$(platform-y))
+
+ifeq ($(KBUILD_SRC),)
+KBUILD_CPPFLAGS += $(patsubst %,-I%include,$(vardirs) $(plfdirs))
+else
+KBUILD_CPPFLAGS += $(patsubst %,-I$(srctree)/%include,$(vardirs) $(plfdirs))
+endif
+
 KBUILD_DEFCONFIG := iss_defconfig
 
 # ramdisk/initrd support
@@ -66,21 +75,6 @@ libs-y		+= arch/xtensa/lib/ $(LIBGCC)
 
 boot		:= arch/xtensa/boot
 
-archinc		:= include/asm-xtensa
-
-archprepare: $(archinc)/.platform
-
-# Update processor variant and platform symlinks if something which affects
-# them changed.
-
-$(archinc)/.platform: $(wildcard include/config/arch/*.h) include/config/auto.conf
-	@echo '  SYMLINK $(archinc)/variant -> $(archinc)/variant-$(VARIANT)'
-	$(Q)mkdir -p $(archinc)
-	$(Q)ln -fsn $(srctree)/$(archinc)/variant-$(VARIANT) $(archinc)/variant
-	@echo '  SYMLINK $(archinc)/platform -> $(archinc)/platform-$(PLATFORM)'
-	$(Q)ln -fsn $(srctree)/$(archinc)/platform-$(PLATFORM) $(archinc)/platform
-	@touch $@
-
 
 all: zImage
 
@@ -89,10 +83,6 @@ bzImage : zImage
 zImage zImage.initrd: vmlinux
 	$(Q)$(MAKE) $(build)=$(boot) $@
 
-CLEAN_FILES	+= arch/xtensa/vmlinux.lds                      \
-		   $(archinc)/platform $(archinc)/variant	\
-		   $(archinc)/.platform
-
 define archhelp
   @echo '* zImage      - Compressed kernel image (arch/xtensa/boot/images/zImage.*)'
 endef
diff --git a/arch/xtensa/boot/boot-elf/boot.lds.S b/arch/xtensa/boot/boot-elf/boot.lds.S
index 849dfcafd518..4e53b74dc44b 100644
--- a/arch/xtensa/boot/boot-elf/boot.lds.S
+++ b/arch/xtensa/boot/boot-elf/boot.lds.S
@@ -1,4 +1,4 @@
-#include <asm/variant/core.h>
+#include <variant/core.h>
 OUTPUT_ARCH(xtensa)
 ENTRY(_ResetVector)
 
diff --git a/arch/xtensa/boot/boot-redboot/bootstrap.S b/arch/xtensa/boot/boot-redboot/bootstrap.S
index 84848123e2a8..5582e8cfac8f 100644
--- a/arch/xtensa/boot/boot-redboot/bootstrap.S
+++ b/arch/xtensa/boot/boot-redboot/bootstrap.S
@@ -1,4 +1,4 @@
-#include <asm/variant/core.h>
+#include <variant/core.h>
 #include <asm/regs.h>
 #include <asm/asmmacro.h>
 #include <asm/cacheasm.h>
diff --git a/include/asm-xtensa/Kbuild b/arch/xtensa/include/asm/Kbuild
index c68e1680da01..58c02a454130 100644
--- a/include/asm-xtensa/Kbuild
+++ b/arch/xtensa/include/asm/Kbuild
@@ -1 +1,3 @@
 include include/asm-generic/Kbuild.asm
+
+unifdef-y += swab.h
diff --git a/include/asm-xtensa/asmmacro.h b/arch/xtensa/include/asm/asmmacro.h
index 76915cabad17..755320f6e0bc 100644
--- a/include/asm-xtensa/asmmacro.h
+++ b/arch/xtensa/include/asm/asmmacro.h
@@ -11,7 +11,7 @@
 #ifndef _XTENSA_ASMMACRO_H
 #define _XTENSA_ASMMACRO_H
 
-#include <asm/variant/core.h>
+#include <variant/core.h>
 
 /*
  * Some little helpers for loops. Use zero-overhead-loops
diff --git a/include/asm-xtensa/atomic.h b/arch/xtensa/include/asm/atomic.h
index b3b23540f14d..67ad67bed8c1 100644
--- a/include/asm-xtensa/atomic.h
+++ b/arch/xtensa/include/asm/atomic.h
@@ -14,8 +14,7 @@
 #define _XTENSA_ATOMIC_H
 
 #include <linux/stringify.h>
-
-typedef struct { volatile int counter; } atomic_t;
+#include <linux/types.h>
 
 #ifdef __KERNEL__
 #include <asm/processor.h>
diff --git a/include/asm-xtensa/auxvec.h b/arch/xtensa/include/asm/auxvec.h
index 257dec75c5af..257dec75c5af 100644
--- a/include/asm-xtensa/auxvec.h
+++ b/arch/xtensa/include/asm/auxvec.h
diff --git a/include/asm-xtensa/bitops.h b/arch/xtensa/include/asm/bitops.h
index 6c3930397bd3..6c3930397bd3 100644
--- a/include/asm-xtensa/bitops.h
+++ b/arch/xtensa/include/asm/bitops.h
diff --git a/include/asm-xtensa/bootparam.h b/arch/xtensa/include/asm/bootparam.h
index 9983f2c1b7ee..9983f2c1b7ee 100644
--- a/include/asm-xtensa/bootparam.h
+++ b/arch/xtensa/include/asm/bootparam.h
diff --git a/include/asm-xtensa/bug.h b/arch/xtensa/include/asm/bug.h
index 3e52d72712f1..3e52d72712f1 100644
--- a/include/asm-xtensa/bug.h
+++ b/arch/xtensa/include/asm/bug.h
diff --git a/include/asm-xtensa/bugs.h b/arch/xtensa/include/asm/bugs.h
index 69b29d198249..69b29d198249 100644
--- a/include/asm-xtensa/bugs.h
+++ b/arch/xtensa/include/asm/bugs.h
diff --git a/arch/xtensa/include/asm/byteorder.h b/arch/xtensa/include/asm/byteorder.h
new file mode 100644
index 000000000000..329b94591ca4
--- /dev/null
+++ b/arch/xtensa/include/asm/byteorder.h
@@ -0,0 +1,14 @@
+#ifndef _XTENSA_BYTEORDER_H
+#define _XTENSA_BYTEORDER_H
+
+#include <asm/swab.h>
+
+#ifdef __XTENSA_EL__
+#include <linux/byteorder/little_endian.h>
+#elif defined(__XTENSA_EB__)
+#include <linux/byteorder/big_endian.h>
+#else
+# error processor byte order undefined!
+#endif
+
+#endif /* _XTENSA_BYTEORDER_H */
diff --git a/include/asm-xtensa/cache.h b/arch/xtensa/include/asm/cache.h
index 3bba2a540cf0..f04c9891142f 100644
--- a/include/asm-xtensa/cache.h
+++ b/arch/xtensa/include/asm/cache.h
@@ -11,7 +11,7 @@
 #ifndef _XTENSA_CACHE_H
 #define _XTENSA_CACHE_H
 
-#include <asm/variant/core.h>
+#include <variant/core.h>
 
 #define L1_CACHE_SHIFT	XCHAL_DCACHE_LINEWIDTH
 #define L1_CACHE_BYTES	XCHAL_DCACHE_LINESIZE
diff --git a/include/asm-xtensa/cacheasm.h b/arch/xtensa/include/asm/cacheasm.h
index 2c20a58f94cd..2c20a58f94cd 100644
--- a/include/asm-xtensa/cacheasm.h
+++ b/arch/xtensa/include/asm/cacheasm.h
diff --git a/include/asm-xtensa/cacheflush.h b/arch/xtensa/include/asm/cacheflush.h
index 94c4c53a099e..94c4c53a099e 100644
--- a/include/asm-xtensa/cacheflush.h
+++ b/arch/xtensa/include/asm/cacheflush.h
diff --git a/include/asm-xtensa/checksum.h b/arch/xtensa/include/asm/checksum.h
index 23534c60b3a4..f84d3f00774a 100644
--- a/include/asm-xtensa/checksum.h
+++ b/arch/xtensa/include/asm/checksum.h
@@ -12,7 +12,7 @@
 #define _XTENSA_CHECKSUM_H
 
 #include <linux/in6.h>
-#include <asm/variant/core.h>
+#include <variant/core.h>
 
 /*
  * computes the checksum of a memory block at buff, length len,
diff --git a/include/asm-xtensa/coprocessor.h b/arch/xtensa/include/asm/coprocessor.h
index 1cbcf9001a41..65a285d8d3fb 100644
--- a/include/asm-xtensa/coprocessor.h
+++ b/arch/xtensa/include/asm/coprocessor.h
@@ -13,11 +13,11 @@
 #define _XTENSA_COPROCESSOR_H
 
 #include <linux/stringify.h>
-#include <asm/variant/tie.h>
+#include <variant/tie.h>
 #include <asm/types.h>
 
 #ifdef __ASSEMBLY__
-# include <asm/variant/tie-asm.h>
+# include <variant/tie-asm.h>
 
 .macro	xchal_sa_start  a b
 	.set .Lxchal_pofs_, 0
diff --git a/include/asm-xtensa/cpumask.h b/arch/xtensa/include/asm/cpumask.h
index ebeede397db3..ebeede397db3 100644
--- a/include/asm-xtensa/cpumask.h
+++ b/arch/xtensa/include/asm/cpumask.h
diff --git a/include/asm-xtensa/cputime.h b/arch/xtensa/include/asm/cputime.h
index a7fb864a50ae..a7fb864a50ae 100644
--- a/include/asm-xtensa/cputime.h
+++ b/arch/xtensa/include/asm/cputime.h
diff --git a/include/asm-xtensa/current.h b/arch/xtensa/include/asm/current.h
index 8d1eb5d78649..8d1eb5d78649 100644
--- a/include/asm-xtensa/current.h
+++ b/arch/xtensa/include/asm/current.h
diff --git a/include/asm-xtensa/delay.h b/arch/xtensa/include/asm/delay.h
index e1d8c9e010c1..e1d8c9e010c1 100644
--- a/include/asm-xtensa/delay.h
+++ b/arch/xtensa/include/asm/delay.h
diff --git a/include/asm-xtensa/device.h b/arch/xtensa/include/asm/device.h
index d8f9872b0e2d..d8f9872b0e2d 100644
--- a/include/asm-xtensa/device.h
+++ b/arch/xtensa/include/asm/device.h
diff --git a/include/asm-xtensa/div64.h b/arch/xtensa/include/asm/div64.h
index f35678cb0a9b..f35678cb0a9b 100644
--- a/include/asm-xtensa/div64.h
+++ b/arch/xtensa/include/asm/div64.h
diff --git a/include/asm-xtensa/dma-mapping.h b/arch/xtensa/include/asm/dma-mapping.h
index 51882ae3db4d..51882ae3db4d 100644
--- a/include/asm-xtensa/dma-mapping.h
+++ b/arch/xtensa/include/asm/dma-mapping.h
diff --git a/include/asm-xtensa/dma.h b/arch/xtensa/include/asm/dma.h
index e30f3abf48f0..e30f3abf48f0 100644
--- a/include/asm-xtensa/dma.h
+++ b/arch/xtensa/include/asm/dma.h
diff --git a/include/asm-xtensa/elf.h b/arch/xtensa/include/asm/elf.h
index c3f53e755ca5..c3f53e755ca5 100644
--- a/include/asm-xtensa/elf.h
+++ b/arch/xtensa/include/asm/elf.h
diff --git a/include/asm-xtensa/emergency-restart.h b/arch/xtensa/include/asm/emergency-restart.h
index 108d8c48e42e..108d8c48e42e 100644
--- a/include/asm-xtensa/emergency-restart.h
+++ b/arch/xtensa/include/asm/emergency-restart.h
diff --git a/include/asm-xtensa/errno.h b/arch/xtensa/include/asm/errno.h
index a0f3b96b79b4..a0f3b96b79b4 100644
--- a/include/asm-xtensa/errno.h
+++ b/arch/xtensa/include/asm/errno.h
diff --git a/include/asm-xtensa/fb.h b/arch/xtensa/include/asm/fb.h
index c7df38030992..c7df38030992 100644
--- a/include/asm-xtensa/fb.h
+++ b/arch/xtensa/include/asm/fb.h
diff --git a/include/asm-xtensa/fcntl.h b/arch/xtensa/include/asm/fcntl.h
index 46ab12db5739..46ab12db5739 100644
--- a/include/asm-xtensa/fcntl.h
+++ b/arch/xtensa/include/asm/fcntl.h
diff --git a/include/asm-xtensa/futex.h b/arch/xtensa/include/asm/futex.h
index 0b745828f42b..0b745828f42b 100644
--- a/include/asm-xtensa/futex.h
+++ b/arch/xtensa/include/asm/futex.h
diff --git a/include/asm-xtensa/hardirq.h b/arch/xtensa/include/asm/hardirq.h
index 87cb19d1b10c..87cb19d1b10c 100644
--- a/include/asm-xtensa/hardirq.h
+++ b/arch/xtensa/include/asm/hardirq.h
diff --git a/include/asm-xtensa/highmem.h b/arch/xtensa/include/asm/highmem.h
index 0a046ca5a687..0a046ca5a687 100644
--- a/include/asm-xtensa/highmem.h
+++ b/arch/xtensa/include/asm/highmem.h
diff --git a/include/asm-xtensa/hw_irq.h b/arch/xtensa/include/asm/hw_irq.h
index 3ddbea759b2b..3ddbea759b2b 100644
--- a/include/asm-xtensa/hw_irq.h
+++ b/arch/xtensa/include/asm/hw_irq.h
diff --git a/include/asm-xtensa/io.h b/arch/xtensa/include/asm/io.h
index 07b7299dab20..07b7299dab20 100644
--- a/include/asm-xtensa/io.h
+++ b/arch/xtensa/include/asm/io.h
diff --git a/include/asm-xtensa/ioctl.h b/arch/xtensa/include/asm/ioctl.h
index b279fe06dfe5..b279fe06dfe5 100644
--- a/include/asm-xtensa/ioctl.h
+++ b/arch/xtensa/include/asm/ioctl.h
diff --git a/include/asm-xtensa/ioctls.h b/arch/xtensa/include/asm/ioctls.h
index 0ffa942954b9..0ffa942954b9 100644
--- a/include/asm-xtensa/ioctls.h
+++ b/arch/xtensa/include/asm/ioctls.h
diff --git a/include/asm-xtensa/ipcbuf.h b/arch/xtensa/include/asm/ipcbuf.h
index c33aa6a42145..c33aa6a42145 100644
--- a/include/asm-xtensa/ipcbuf.h
+++ b/arch/xtensa/include/asm/ipcbuf.h
diff --git a/include/asm-xtensa/irq.h b/arch/xtensa/include/asm/irq.h
index fc73b7f11aff..1620d1e0e695 100644
--- a/include/asm-xtensa/irq.h
+++ b/arch/xtensa/include/asm/irq.h
@@ -11,8 +11,8 @@
 #ifndef _XTENSA_IRQ_H
 #define _XTENSA_IRQ_H
 
-#include <asm/platform/hardware.h>
-#include <asm/variant/core.h>
+#include <platform/hardware.h>
+#include <variant/core.h>
 
 #ifndef PLATFORM_NR_IRQS
 # define PLATFORM_NR_IRQS 0
diff --git a/include/asm-xtensa/irq_regs.h b/arch/xtensa/include/asm/irq_regs.h
index 3dd9c0b70270..3dd9c0b70270 100644
--- a/include/asm-xtensa/irq_regs.h
+++ b/arch/xtensa/include/asm/irq_regs.h
diff --git a/include/asm-xtensa/kdebug.h b/arch/xtensa/include/asm/kdebug.h
index 6ece1b037665..6ece1b037665 100644
--- a/include/asm-xtensa/kdebug.h
+++ b/arch/xtensa/include/asm/kdebug.h
diff --git a/include/asm-xtensa/kmap_types.h b/arch/xtensa/include/asm/kmap_types.h
index 9e822d2e3bce..9e822d2e3bce 100644
--- a/include/asm-xtensa/kmap_types.h
+++ b/arch/xtensa/include/asm/kmap_types.h
diff --git a/include/asm-xtensa/linkage.h b/arch/xtensa/include/asm/linkage.h
index bf2128a99d79..bf2128a99d79 100644
--- a/include/asm-xtensa/linkage.h
+++ b/arch/xtensa/include/asm/linkage.h
diff --git a/include/asm-xtensa/local.h b/arch/xtensa/include/asm/local.h
index 48723e550d14..48723e550d14 100644
--- a/include/asm-xtensa/local.h
+++ b/arch/xtensa/include/asm/local.h
diff --git a/include/asm-xtensa/mman.h b/arch/xtensa/include/asm/mman.h
index 9b92620c8a1e..9b92620c8a1e 100644
--- a/include/asm-xtensa/mman.h
+++ b/arch/xtensa/include/asm/mman.h
diff --git a/include/asm-xtensa/mmu.h b/arch/xtensa/include/asm/mmu.h
index 44c5bb04c55c..44c5bb04c55c 100644
--- a/include/asm-xtensa/mmu.h
+++ b/arch/xtensa/include/asm/mmu.h
diff --git a/include/asm-xtensa/mmu_context.h b/arch/xtensa/include/asm/mmu_context.h
index c0fd8e5b4513..c0fd8e5b4513 100644
--- a/include/asm-xtensa/mmu_context.h
+++ b/arch/xtensa/include/asm/mmu_context.h
diff --git a/include/asm-xtensa/module.h b/arch/xtensa/include/asm/module.h
index d9b34bee4d42..d9b34bee4d42 100644
--- a/include/asm-xtensa/module.h
+++ b/arch/xtensa/include/asm/module.h
diff --git a/include/asm-xtensa/msgbuf.h b/arch/xtensa/include/asm/msgbuf.h
index 693c96755280..693c96755280 100644
--- a/include/asm-xtensa/msgbuf.h
+++ b/arch/xtensa/include/asm/msgbuf.h
diff --git a/include/asm-xtensa/mutex.h b/arch/xtensa/include/asm/mutex.h
index 458c1f7fbc18..458c1f7fbc18 100644
--- a/include/asm-xtensa/mutex.h
+++ b/arch/xtensa/include/asm/mutex.h
diff --git a/include/asm-xtensa/page.h b/arch/xtensa/include/asm/page.h
index 11f7dc2dbec7..11f7dc2dbec7 100644
--- a/include/asm-xtensa/page.h
+++ b/arch/xtensa/include/asm/page.h
diff --git a/include/asm-xtensa/param.h b/arch/xtensa/include/asm/param.h
index ba03d5aeab6b..ba03d5aeab6b 100644
--- a/include/asm-xtensa/param.h
+++ b/arch/xtensa/include/asm/param.h
diff --git a/include/asm-xtensa/pci-bridge.h b/arch/xtensa/include/asm/pci-bridge.h
index 00fcbd7c534a..00fcbd7c534a 100644
--- a/include/asm-xtensa/pci-bridge.h
+++ b/arch/xtensa/include/asm/pci-bridge.h
diff --git a/include/asm-xtensa/pci.h b/arch/xtensa/include/asm/pci.h
index 66410acf18b4..66410acf18b4 100644
--- a/include/asm-xtensa/pci.h
+++ b/arch/xtensa/include/asm/pci.h
diff --git a/include/asm-xtensa/percpu.h b/arch/xtensa/include/asm/percpu.h
index 6d2bc2ada9d1..6d2bc2ada9d1 100644
--- a/include/asm-xtensa/percpu.h
+++ b/arch/xtensa/include/asm/percpu.h
diff --git a/include/asm-xtensa/pgalloc.h b/arch/xtensa/include/asm/pgalloc.h
index 4f4a7987eded..4f4a7987eded 100644
--- a/include/asm-xtensa/pgalloc.h
+++ b/arch/xtensa/include/asm/pgalloc.h
diff --git a/include/asm-xtensa/pgtable.h b/arch/xtensa/include/asm/pgtable.h
index 8014d96b21f1..8014d96b21f1 100644
--- a/include/asm-xtensa/pgtable.h
+++ b/arch/xtensa/include/asm/pgtable.h
diff --git a/include/asm-xtensa/platform.h b/arch/xtensa/include/asm/platform.h
index 48135a9718b0..e3d5a48ad495 100644
--- a/include/asm-xtensa/platform.h
+++ b/arch/xtensa/include/asm/platform.h
@@ -1,6 +1,4 @@
 /*
- * include/asm-xtensa/platform.h
- *
  * Platform specific functions
  *
  * This file is subject to the terms and conditions of the GNU General
diff --git a/include/asm-xtensa/poll.h b/arch/xtensa/include/asm/poll.h
index 9d2d5993f068..9d2d5993f068 100644
--- a/include/asm-xtensa/poll.h
+++ b/arch/xtensa/include/asm/poll.h
diff --git a/include/asm-xtensa/posix_types.h b/arch/xtensa/include/asm/posix_types.h
index 43f9dd1126a4..43f9dd1126a4 100644
--- a/include/asm-xtensa/posix_types.h
+++ b/arch/xtensa/include/asm/posix_types.h
diff --git a/include/asm-xtensa/processor.h b/arch/xtensa/include/asm/processor.h
index 4918a4e96d42..07387d3b99f4 100644
--- a/include/asm-xtensa/processor.h
+++ b/arch/xtensa/include/asm/processor.h
@@ -11,7 +11,7 @@
 #ifndef _XTENSA_PROCESSOR_H
 #define _XTENSA_PROCESSOR_H
 
-#include <asm/variant/core.h>
+#include <variant/core.h>
 #include <asm/coprocessor.h>
 
 #include <linux/compiler.h>
diff --git a/include/asm-xtensa/ptrace.h b/arch/xtensa/include/asm/ptrace.h
index 089b0db44816..905e1e619654 100644
--- a/include/asm-xtensa/ptrace.h
+++ b/arch/xtensa/include/asm/ptrace.h
@@ -111,7 +111,7 @@ struct pt_regs {
 	unsigned long areg[16];		/* 128 (64) */
 };
 
-#include <asm/variant/core.h>
+#include <variant/core.h>
 
 # define task_pt_regs(tsk) ((struct pt_regs*) \
   (task_stack_page(tsk) + KERNEL_STACK_SIZE - (XCHAL_NUM_AREGS-16)*4) - 1)
diff --git a/include/asm-xtensa/regs.h b/arch/xtensa/include/asm/regs.h
index d4baed246928..d4baed246928 100644
--- a/include/asm-xtensa/regs.h
+++ b/arch/xtensa/include/asm/regs.h
diff --git a/include/asm-xtensa/resource.h b/arch/xtensa/include/asm/resource.h
index 17b5ab311771..17b5ab311771 100644
--- a/include/asm-xtensa/resource.h
+++ b/arch/xtensa/include/asm/resource.h
diff --git a/include/asm-xtensa/rmap.h b/arch/xtensa/include/asm/rmap.h
index 649588b7e9ad..649588b7e9ad 100644
--- a/include/asm-xtensa/rmap.h
+++ b/arch/xtensa/include/asm/rmap.h
diff --git a/include/asm-xtensa/rwsem.h b/arch/xtensa/include/asm/rwsem.h
index e39edf5c86f2..e39edf5c86f2 100644
--- a/include/asm-xtensa/rwsem.h
+++ b/arch/xtensa/include/asm/rwsem.h
diff --git a/include/asm-xtensa/scatterlist.h b/arch/xtensa/include/asm/scatterlist.h
index 810080bb0a2b..810080bb0a2b 100644
--- a/include/asm-xtensa/scatterlist.h
+++ b/arch/xtensa/include/asm/scatterlist.h
diff --git a/include/asm-xtensa/sections.h b/arch/xtensa/include/asm/sections.h
index 40b5191b55a2..40b5191b55a2 100644
--- a/include/asm-xtensa/sections.h
+++ b/arch/xtensa/include/asm/sections.h
diff --git a/include/asm-xtensa/segment.h b/arch/xtensa/include/asm/segment.h
index a2eb547a1a75..a2eb547a1a75 100644
--- a/include/asm-xtensa/segment.h
+++ b/arch/xtensa/include/asm/segment.h
diff --git a/include/asm-xtensa/sembuf.h b/arch/xtensa/include/asm/sembuf.h
index c15870493b33..c15870493b33 100644
--- a/include/asm-xtensa/sembuf.h
+++ b/arch/xtensa/include/asm/sembuf.h
diff --git a/include/asm-xtensa/serial.h b/arch/xtensa/include/asm/serial.h
index ec04114fcf0b..a8a2493260f6 100644
--- a/include/asm-xtensa/serial.h
+++ b/arch/xtensa/include/asm/serial.h
@@ -13,6 +13,6 @@
 #ifndef _XTENSA_SERIAL_H
 #define _XTENSA_SERIAL_H
 
-#include <asm/platform/serial.h>
+#include <platform/serial.h>
 
 #endif	/* _XTENSA_SERIAL_H */
diff --git a/include/asm-xtensa/setup.h b/arch/xtensa/include/asm/setup.h
index e3636520d8cc..e3636520d8cc 100644
--- a/include/asm-xtensa/setup.h
+++ b/arch/xtensa/include/asm/setup.h
diff --git a/include/asm-xtensa/shmbuf.h b/arch/xtensa/include/asm/shmbuf.h
index ad4b0121782c..ad4b0121782c 100644
--- a/include/asm-xtensa/shmbuf.h
+++ b/arch/xtensa/include/asm/shmbuf.h
diff --git a/include/asm-xtensa/shmparam.h b/arch/xtensa/include/asm/shmparam.h
index c8cc16c3da9e..c8cc16c3da9e 100644
--- a/include/asm-xtensa/shmparam.h
+++ b/arch/xtensa/include/asm/shmparam.h
diff --git a/include/asm-xtensa/sigcontext.h b/arch/xtensa/include/asm/sigcontext.h
index 03383af8c3b7..03383af8c3b7 100644
--- a/include/asm-xtensa/sigcontext.h
+++ b/arch/xtensa/include/asm/sigcontext.h
diff --git a/include/asm-xtensa/siginfo.h b/arch/xtensa/include/asm/siginfo.h
index 6916248295df..6916248295df 100644
--- a/include/asm-xtensa/siginfo.h
+++ b/arch/xtensa/include/asm/siginfo.h
diff --git a/include/asm-xtensa/signal.h b/arch/xtensa/include/asm/signal.h
index 633ba73bc4d2..633ba73bc4d2 100644
--- a/include/asm-xtensa/signal.h
+++ b/arch/xtensa/include/asm/signal.h
diff --git a/include/asm-xtensa/smp.h b/arch/xtensa/include/asm/smp.h
index 83c569e3bdbd..83c569e3bdbd 100644
--- a/include/asm-xtensa/smp.h
+++ b/arch/xtensa/include/asm/smp.h
diff --git a/include/asm-xtensa/socket.h b/arch/xtensa/include/asm/socket.h
index 6100682b1da2..6100682b1da2 100644
--- a/include/asm-xtensa/socket.h
+++ b/arch/xtensa/include/asm/socket.h
diff --git a/include/asm-xtensa/sockios.h b/arch/xtensa/include/asm/sockios.h
index efe0af379f01..efe0af379f01 100644
--- a/include/asm-xtensa/sockios.h
+++ b/arch/xtensa/include/asm/sockios.h
diff --git a/include/asm-xtensa/spinlock.h b/arch/xtensa/include/asm/spinlock.h
index 8ff23649581b..8ff23649581b 100644
--- a/include/asm-xtensa/spinlock.h
+++ b/arch/xtensa/include/asm/spinlock.h
diff --git a/include/asm-xtensa/stat.h b/arch/xtensa/include/asm/stat.h
index c4992038cee0..c4992038cee0 100644
--- a/include/asm-xtensa/stat.h
+++ b/arch/xtensa/include/asm/stat.h
diff --git a/include/asm-xtensa/statfs.h b/arch/xtensa/include/asm/statfs.h
index 9c3d1a213136..9c3d1a213136 100644
--- a/include/asm-xtensa/statfs.h
+++ b/arch/xtensa/include/asm/statfs.h
diff --git a/include/asm-xtensa/string.h b/arch/xtensa/include/asm/string.h
index 5fb8c27cbef5..5fb8c27cbef5 100644
--- a/include/asm-xtensa/string.h
+++ b/arch/xtensa/include/asm/string.h
diff --git a/include/asm-xtensa/byteorder.h b/arch/xtensa/include/asm/swab.h
index 765edf17a9a4..f50b697eb601 100644
--- a/include/asm-xtensa/byteorder.h
+++ b/arch/xtensa/include/asm/swab.h
@@ -1,5 +1,5 @@
 /*
- * include/asm-xtensa/byteorder.h
+ * include/asm-xtensa/swab.h
  *
  * 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
@@ -8,13 +8,15 @@
  * Copyright (C) 2001 - 2005 Tensilica Inc.
  */
 
-#ifndef _XTENSA_BYTEORDER_H
-#define _XTENSA_BYTEORDER_H
+#ifndef _XTENSA_SWAB_H
+#define _XTENSA_SWAB_H
 
 #include <asm/types.h>
 #include <linux/compiler.h>
 
-static __inline__ __attribute_const__ __u32 ___arch__swab32(__u32 x)
+#define __SWAB_64_THRU_32__
+
+static inline __attribute_const__ __u32 __arch_swab32(__u32 x)
 {
     __u32 res;
     /* instruction sequence from Xtensa ISA release 2/2000 */
@@ -28,8 +30,9 @@ static __inline__ __attribute_const__ __u32 ___arch__swab32(__u32 x)
 	    );
     return res;
 }
+#define __arch_swab32 __arch_swab32
 
-static __inline__ __attribute_const__ __u16 ___arch__swab16(__u16 x)
+static inline __attribute_const__ __u16 __arch_swab16(__u16 x)
 {
     /* Given that 'short' values are signed (i.e., can be negative),
      * we cannot assume that the upper 16-bits of the register are
@@ -62,21 +65,6 @@ static __inline__ __attribute_const__ __u16 ___arch__swab16(__u16 x)
 
     return res;
 }
+#define __arch_swab16 __arch_swab16
 
-#define __arch__swab32(x) ___arch__swab32(x)
-#define __arch__swab16(x) ___arch__swab16(x)
-
-#if !defined(__STRICT_ANSI__) || defined(__KERNEL__)
-#  define __BYTEORDER_HAS_U64__
-#  define __SWAB_64_THRU_32__
-#endif
-
-#ifdef __XTENSA_EL__
-# include <linux/byteorder/little_endian.h>
-#elif defined(__XTENSA_EB__)
-# include <linux/byteorder/big_endian.h>
-#else
-# error processor byte order undefined!
-#endif
-
-#endif /* _XTENSA_BYTEORDER_H */
+#endif /* _XTENSA_SWAB_H */
diff --git a/include/asm-xtensa/syscall.h b/arch/xtensa/include/asm/syscall.h
index 05cebf8f62b1..05cebf8f62b1 100644
--- a/include/asm-xtensa/syscall.h
+++ b/arch/xtensa/include/asm/syscall.h
diff --git a/include/asm-xtensa/system.h b/arch/xtensa/include/asm/system.h
index 62b1e8f3c13c..62b1e8f3c13c 100644
--- a/include/asm-xtensa/system.h
+++ b/arch/xtensa/include/asm/system.h
diff --git a/include/asm-xtensa/termbits.h b/arch/xtensa/include/asm/termbits.h
index 85aa6a3c0b6e..85aa6a3c0b6e 100644
--- a/include/asm-xtensa/termbits.h
+++ b/arch/xtensa/include/asm/termbits.h
diff --git a/include/asm-xtensa/termios.h b/arch/xtensa/include/asm/termios.h
index 4673f42f88a7..4673f42f88a7 100644
--- a/include/asm-xtensa/termios.h
+++ b/arch/xtensa/include/asm/termios.h
diff --git a/include/asm-xtensa/thread_info.h b/arch/xtensa/include/asm/thread_info.h
index 0f4fe1faf9ba..0f4fe1faf9ba 100644
--- a/include/asm-xtensa/thread_info.h
+++ b/arch/xtensa/include/asm/thread_info.h
diff --git a/include/asm-xtensa/timex.h b/arch/xtensa/include/asm/timex.h
index b83a8181d448..b83a8181d448 100644
--- a/include/asm-xtensa/timex.h
+++ b/arch/xtensa/include/asm/timex.h
diff --git a/include/asm-xtensa/tlb.h b/arch/xtensa/include/asm/tlb.h
index 31c220faca02..31c220faca02 100644
--- a/include/asm-xtensa/tlb.h
+++ b/arch/xtensa/include/asm/tlb.h
diff --git a/include/asm-xtensa/tlbflush.h b/arch/xtensa/include/asm/tlbflush.h
index 46d240074f74..46d240074f74 100644
--- a/include/asm-xtensa/tlbflush.h
+++ b/arch/xtensa/include/asm/tlbflush.h
diff --git a/include/asm-xtensa/topology.h b/arch/xtensa/include/asm/topology.h
index 7309e38a0ccb..7309e38a0ccb 100644
--- a/include/asm-xtensa/topology.h
+++ b/arch/xtensa/include/asm/topology.h
diff --git a/include/asm-xtensa/types.h b/arch/xtensa/include/asm/types.h
index c89569a8da0c..c89569a8da0c 100644
--- a/include/asm-xtensa/types.h
+++ b/arch/xtensa/include/asm/types.h
diff --git a/include/asm-xtensa/uaccess.h b/arch/xtensa/include/asm/uaccess.h
index b8528426ab1f..b8528426ab1f 100644
--- a/include/asm-xtensa/uaccess.h
+++ b/arch/xtensa/include/asm/uaccess.h
diff --git a/include/asm-xtensa/ucontext.h b/arch/xtensa/include/asm/ucontext.h
index 94c94ed3e00a..94c94ed3e00a 100644
--- a/include/asm-xtensa/ucontext.h
+++ b/arch/xtensa/include/asm/ucontext.h
diff --git a/include/asm-xtensa/unaligned.h b/arch/xtensa/include/asm/unaligned.h
index 8f3424fc5d18..8e7ed046bfed 100644
--- a/include/asm-xtensa/unaligned.h
+++ b/arch/xtensa/include/asm/unaligned.h
@@ -10,20 +10,20 @@
 #ifndef _ASM_XTENSA_UNALIGNED_H
 #define _ASM_XTENSA_UNALIGNED_H
 
-#ifdef __XTENSA_EL__
-# include <linux/unaligned/le_memmove.h>
+#include <asm/byteorder.h>
+
+#ifdef __LITTLE_ENDIAN
+# include <linux/unaligned/le_struct.h>
 # include <linux/unaligned/be_byteshift.h>
 # include <linux/unaligned/generic.h>
 # define get_unaligned	__get_unaligned_le
 # define put_unaligned	__put_unaligned_le
-#elif defined(__XTENSA_EB__)
-# include <linux/unaligned/be_memmove.h>
+#else
+# include <linux/unaligned/be_struct.h>
 # include <linux/unaligned/le_byteshift.h>
 # include <linux/unaligned/generic.h>
 # define get_unaligned	__get_unaligned_be
 # define put_unaligned	__put_unaligned_be
-#else
-# error processor byte order undefined!
 #endif
 
 #endif	/* _ASM_XTENSA_UNALIGNED_H */
diff --git a/include/asm-xtensa/unistd.h b/arch/xtensa/include/asm/unistd.h
index c092c8fbb2cf..c092c8fbb2cf 100644
--- a/include/asm-xtensa/unistd.h
+++ b/arch/xtensa/include/asm/unistd.h
diff --git a/include/asm-xtensa/user.h b/arch/xtensa/include/asm/user.h
index 2c3ed23354a8..2c3ed23354a8 100644
--- a/include/asm-xtensa/user.h
+++ b/arch/xtensa/include/asm/user.h
diff --git a/include/asm-xtensa/vga.h b/arch/xtensa/include/asm/vga.h
index 1fd8cab3a297..1fd8cab3a297 100644
--- a/include/asm-xtensa/vga.h
+++ b/arch/xtensa/include/asm/vga.h
diff --git a/include/asm-xtensa/xor.h b/arch/xtensa/include/asm/xor.h
index e7b1f083991d..e7b1f083991d 100644
--- a/include/asm-xtensa/xor.h
+++ b/arch/xtensa/include/asm/xor.h
diff --git a/arch/xtensa/kernel/entry.S b/arch/xtensa/kernel/entry.S
index dfd35dcc1cb5..a51d36a27389 100644
--- a/arch/xtensa/kernel/entry.S
+++ b/arch/xtensa/kernel/entry.S
@@ -25,7 +25,7 @@
 #include <asm/page.h>
 #include <asm/signal.h>
 #include <asm/tlbflush.h>
-#include <asm/variant/tie-asm.h>
+#include <variant/tie-asm.h>
 
 /* Unimplemented features. */
 
diff --git a/arch/xtensa/kernel/vmlinux.lds.S b/arch/xtensa/kernel/vmlinux.lds.S
index 51f4fb6f16f9..d506774f4b05 100644
--- a/arch/xtensa/kernel/vmlinux.lds.S
+++ b/arch/xtensa/kernel/vmlinux.lds.S
@@ -16,7 +16,7 @@
 
 #include <asm-generic/vmlinux.lds.h>
 
-#include <asm/variant/core.h>
+#include <variant/core.h>
 OUTPUT_ARCH(xtensa)
 ENTRY(_start)
 
diff --git a/arch/xtensa/lib/checksum.S b/arch/xtensa/lib/checksum.S
index 9d9cd990afa6..df397f932d0e 100644
--- a/arch/xtensa/lib/checksum.S
+++ b/arch/xtensa/lib/checksum.S
@@ -16,7 +16,7 @@
 
 #include <asm/errno.h>
 #include <linux/linkage.h>
-#include <asm/variant/core.h>
+#include <variant/core.h>
 
 /*
  * computes a partial checksum, e.g. for TCP/UDP fragments
diff --git a/arch/xtensa/lib/memcopy.S b/arch/xtensa/lib/memcopy.S
index ddda8f4bc862..ea59dcd03866 100644
--- a/arch/xtensa/lib/memcopy.S
+++ b/arch/xtensa/lib/memcopy.S
@@ -9,7 +9,7 @@
  * Copyright (C) 2002 - 2005 Tensilica Inc.
  */
 
-#include <asm/variant/core.h>
+#include <variant/core.h>
 
 	.macro	src_b	r, w0, w1
 #ifdef __XTENSA_EB__
diff --git a/arch/xtensa/lib/memset.S b/arch/xtensa/lib/memset.S
index 56a17495b2db..10b8c400f175 100644
--- a/arch/xtensa/lib/memset.S
+++ b/arch/xtensa/lib/memset.S
@@ -11,7 +11,7 @@
  *  Copyright (C) 2002 Tensilica Inc.
  */
 
-#include <asm/variant/core.h>
+#include <variant/core.h>
 
 /*
  * void *memset(void *dst, int c, size_t length)
diff --git a/arch/xtensa/lib/strncpy_user.S b/arch/xtensa/lib/strncpy_user.S
index b2655d94558d..9f603cdaaa68 100644
--- a/arch/xtensa/lib/strncpy_user.S
+++ b/arch/xtensa/lib/strncpy_user.S
@@ -11,7 +11,7 @@
  *  Copyright (C) 2002 Tensilica Inc.
  */
 
-#include <asm/variant/core.h>
+#include <variant/core.h>
 #include <linux/errno.h>
 
 /* Load or store instructions that may cause exceptions use the EX macro. */
diff --git a/arch/xtensa/lib/strnlen_user.S b/arch/xtensa/lib/strnlen_user.S
index ad3f616322ca..23f2a89816a1 100644
--- a/arch/xtensa/lib/strnlen_user.S
+++ b/arch/xtensa/lib/strnlen_user.S
@@ -11,7 +11,7 @@
  *  Copyright (C) 2002 Tensilica Inc.
  */
 
-#include <asm/variant/core.h>
+#include <variant/core.h>
 
 /* Load or store instructions that may cause exceptions use the EX macro. */
 
diff --git a/arch/xtensa/lib/usercopy.S b/arch/xtensa/lib/usercopy.S
index a8ab1d4fe0ae..46d60314bb16 100644
--- a/arch/xtensa/lib/usercopy.S
+++ b/arch/xtensa/lib/usercopy.S
@@ -53,7 +53,7 @@
  *	a11/ original length
  */
 
-#include <asm/variant/core.h>
+#include <variant/core.h>
 
 #ifdef __XTENSA_EB__
 #define ALIGN(R, W0, W1) src	R, W0, W1
diff --git a/arch/xtensa/platforms/iss/console.c b/arch/xtensa/platforms/iss/console.c
index 9141e3690731..efed8897bef3 100644
--- a/arch/xtensa/platforms/iss/console.c
+++ b/arch/xtensa/platforms/iss/console.c
@@ -1,5 +1,5 @@
 /*
- * arch/xtensa/platform-iss/console.c
+ * arch/xtensa/platforms/iss/console.c
  *
  * 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
@@ -24,7 +24,7 @@
 #include <asm/uaccess.h>
 #include <asm/irq.h>
 
-#include <asm/platform/simcall.h>
+#include <platform/simcall.h>
 
 #include <linux/tty.h>
 #include <linux/tty_flip.h>
diff --git a/include/asm-xtensa/platform-iss/hardware.h b/arch/xtensa/platforms/iss/include/platform/hardware.h
index 6930c12adc16..6930c12adc16 100644
--- a/include/asm-xtensa/platform-iss/hardware.h
+++ b/arch/xtensa/platforms/iss/include/platform/hardware.h
diff --git a/include/asm-xtensa/platform-iss/simcall.h b/arch/xtensa/platforms/iss/include/platform/simcall.h
index b7952c06a2b7..b7952c06a2b7 100644
--- a/include/asm-xtensa/platform-iss/simcall.h
+++ b/arch/xtensa/platforms/iss/include/platform/simcall.h
diff --git a/arch/xtensa/platforms/iss/io.c b/arch/xtensa/platforms/iss/io.c
index 5b161a5cb65f..571d0b24f895 100644
--- a/arch/xtensa/platforms/iss/io.c
+++ b/arch/xtensa/platforms/iss/io.c
@@ -3,7 +3,7 @@
 #if 0
 
 #include <asm/io.h>
-#include <xtensa/simcall.h>
+#include <platform/platform-iss/simcall.h>
 
 extern int __simc ();
 
diff --git a/arch/xtensa/platforms/iss/network.c b/arch/xtensa/platforms/iss/network.c
index 64f057d89e73..edad4156d89a 100644
--- a/arch/xtensa/platforms/iss/network.c
+++ b/arch/xtensa/platforms/iss/network.c
@@ -1,6 +1,6 @@
 /*
  *
- * arch/xtensa/platform-iss/network.c
+ * arch/xtensa/platforms/iss/network.c
  *
  * Platform specific initialization.
  *
@@ -33,7 +33,7 @@
 #include <linux/rtnetlink.h>
 #include <linux/platform_device.h>
 
-#include <asm/platform/simcall.h>
+#include <platform/simcall.h>
 
 #define DRIVER_NAME "iss-netdev"
 #define ETH_MAX_PACKET 1500
diff --git a/arch/xtensa/platforms/xt2000/Makefile b/arch/xtensa/platforms/xt2000/Makefile
new file mode 100644
index 000000000000..54d018e45bfc
--- /dev/null
+++ b/arch/xtensa/platforms/xt2000/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for the Tensilica XT2000 Emulation Board
+#
+
+obj-y 			= setup.o
diff --git a/arch/xtensa/platforms/xt2000/include/platform/hardware.h b/arch/xtensa/platforms/xt2000/include/platform/hardware.h
new file mode 100644
index 000000000000..41459ad07766
--- /dev/null
+++ b/arch/xtensa/platforms/xt2000/include/platform/hardware.h
@@ -0,0 +1,55 @@
+/*
+ * platform/hardware.h
+ *
+ * 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) 2001 Tensilica Inc.
+ */
+
+/*
+ * This file contains the hardware configuration of the XT2000 board.
+ */
+
+#ifndef _XTENSA_XT2000_HARDWARE_H
+#define _XTENSA_XT2000_HARDWARE_H
+
+#include <variant/core.h>
+#include <asm/io.h>
+
+/* 
+ * Memory configuration.
+ */
+
+#define PLATFORM_DEFAULT_MEM_START 0x00000000
+#define PLATFORM_DEFAULT_MEM_SIZE 0x08000000
+
+/*
+ * Number of platform IRQs
+ */
+#define PLATFORM_NR_IRQS 3
+/*
+ * On-board components.
+ */
+
+#define SONIC83934_INTNUM	XCHAL_EXTINT3_NUM
+#define SONIC83934_ADDR		IOADDR(0x0d030000)
+
+/*
+ * V3-PCI
+ */
+
+/* The XT2000 uses the V3 as a cascaded interrupt controller for the PCI bus */
+
+#define IRQ_PCI_A		(XCHAL_NUM_INTERRUPTS + 0)
+#define IRQ_PCI_B		(XCHAL_NUM_INTERRUPTS + 1)
+#define IRQ_PCI_C		(XCHAL_NUM_INTERRUPTS + 2)
+
+/*
+ * Various other components.
+ */
+
+#define XT2000_LED_ADDR		IOADDR(0x0d040000)
+
+#endif /* _XTENSA_XT2000_HARDWARE_H */
diff --git a/arch/xtensa/platforms/xt2000/include/platform/serial.h b/arch/xtensa/platforms/xt2000/include/platform/serial.h
new file mode 100644
index 000000000000..7226cf732b47
--- /dev/null
+++ b/arch/xtensa/platforms/xt2000/include/platform/serial.h
@@ -0,0 +1,28 @@
+/*
+ * platform/serial.h
+ *
+ * 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) 2001 Tensilica Inc.
+ */
+
+#ifndef _XTENSA_XT2000_SERIAL_H
+#define _XTENSA_XT2000_SERIAL_H
+
+#include <variant/core.h>
+#include <asm/io.h>
+
+/*  National-Semi PC16552D DUART:  */
+
+#define DUART16552_1_INTNUM	XCHAL_EXTINT4_NUM
+#define DUART16552_2_INTNUM	XCHAL_EXTINT5_NUM
+
+#define DUART16552_1_ADDR	IOADDR(0x0d050020)	/* channel 1 */
+#define DUART16552_2_ADDR	IOADDR(0x0d050000)	/* channel 2 */
+
+#define DUART16552_XTAL_FREQ	18432000	/* crystal frequency in Hz */
+#define BASE_BAUD ( DUART16552_XTAL_FREQ / 16 )
+
+#endif /* _XTENSA_XT2000_SERIAL_H */
diff --git a/arch/xtensa/platforms/xt2000/setup.c b/arch/xtensa/platforms/xt2000/setup.c
new file mode 100644
index 000000000000..9e83940ac265
--- /dev/null
+++ b/arch/xtensa/platforms/xt2000/setup.c
@@ -0,0 +1,181 @@
+/*
+ * arch/xtensa/platforms/xt2000/setup.c
+ *
+ * Platform specific functions for the XT2000 board.
+ *
+ * Authors:	Chris Zankel <chris@zankel.net>
+ *		Joe Taylor <joe@tensilica.com>
+ *
+ * Copyright 2001 - 2004 Tensilica Inc.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ */
+#include <linux/stddef.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/reboot.h>
+#include <linux/kdev_t.h>
+#include <linux/types.h>
+#include <linux/major.h>
+#include <linux/console.h>
+#include <linux/delay.h>
+#include <linux/stringify.h>
+#include <linux/platform_device.h>
+#include <linux/serial.h>
+#include <linux/serial_8250.h>
+
+#include <asm/processor.h>
+#include <asm/platform.h>
+#include <asm/bootparam.h>
+#include <platform/hardware.h>
+#include <platform/serial.h>
+
+/* Assumes s points to an 8-chr string.  No checking for NULL. */
+
+static void led_print (int f, char *s)
+{
+	unsigned long* led_addr = (unsigned long*) (XT2000_LED_ADDR + 0xE0) + f;
+	int i;
+	for (i = f; i < 8; i++)
+		if ((*led_addr++ = *s++) == 0)
+		    break;
+}
+
+void platform_halt(void)
+{
+	led_print (0, "  HALT  ");
+	local_irq_disable();
+	while (1);
+}
+
+void platform_power_off(void)
+{
+	led_print (0, "POWEROFF");
+	local_irq_disable();
+	while (1);
+}
+
+void platform_restart(void)
+{
+	/* Flush and reset the mmu, simulate a processor reset, and
+	 * jump to the reset vector. */
+
+	__asm__ __volatile__ ("movi	a2, 15\n\t"
+			      "wsr	a2, " __stringify(ICOUNTLEVEL) "\n\t"
+			      "movi	a2, 0\n\t"
+			      "wsr	a2, " __stringify(ICOUNT) "\n\t"
+			      "wsr	a2, " __stringify(IBREAKENABLE) "\n\t"
+			      "wsr	a2, " __stringify(LCOUNT) "\n\t"
+			      "movi	a2, 0x1f\n\t"
+			      "wsr	a2, " __stringify(PS) "\n\t"
+			      "isync\n\t"
+			      "jx	%0\n\t"
+			      :
+			      : "a" (XCHAL_RESET_VECTOR_VADDR)
+			      : "a2"
+			      );
+
+	/* control never gets here */
+}
+
+void __init platform_setup(char** cmdline)
+{
+	led_print (0, "LINUX   ");
+}
+
+/* early initialization */
+
+extern sysmem_info_t __initdata sysmem;
+
+void 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. */
+
+void platform_heartbeat(void)
+{
+	static int i=0, t = 0;
+
+	if (--t < 0)
+	{
+		t = 59;
+		led_print(7, i ? ".": " ");
+		i ^= 1;
+	}
+}
+
+//#define RS_TABLE_SIZE 2
+//#define STD_COM_FLAGS (ASYNC_BOOT_AUTOCONF|ASYNC_SKIP_TEST)
+
+#define _SERIAL_PORT(_base,_irq)					\
+{									\
+	.mapbase	= (_base),					\
+	.membase	= (void*)(_base),				\
+	.irq		= (_irq),					\
+	.uartclk	= DUART16552_XTAL_FREQ,				\
+	.iotype		= UPIO_MEM,					\
+	.flags		= UPF_BOOT_AUTOCONF,				\
+	.regshift	= 2,						\
+}
+
+static struct plat_serial8250_port xt2000_serial_data[] = {
+#if XCHAL_HAVE_BE
+	_SERIAL_PORT(DUART16552_1_ADDR + 3, DUART16552_1_INTNUM),
+	_SERIAL_PORT(DUART16552_2_ADDR + 3, DUART16552_2_INTNUM),
+#else
+	_SERIAL_PORT(DUART16552_1_ADDR, DUART16552_1_INTNUM),
+	_SERIAL_PORT(DUART16552_2_ADDR, DUART16552_2_INTNUM),
+#endif
+	{ }
+};
+
+static struct platform_device xt2000_serial8250_device = {
+	.name		= "serial8250",
+	.id		= PLAT8250_DEV_PLATFORM,
+	.dev		= {
+	    .platform_data = xt2000_serial_data,
+	},
+};
+
+static struct resource xt2000_sonic_res[] = {
+	{
+		.start = SONIC83934_ADDR,
+		.end   = SONIC83934_ADDR + 0xff,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = SONIC83934_INTNUM,
+		.end = SONIC83934_INTNUM,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static struct platform_device xt2000_sonic_device = {
+	.name		= "xtsonic",
+	.num_resources	= ARRAY_SIZE(xt2000_sonic_res),
+	.resource		= xt2000_sonic_res,
+};
+
+static int __init xt2000_setup_devinit(void)
+{
+	platform_device_register(&xt2000_serial8250_device);
+	platform_device_register(&xt2000_sonic_device);
+
+	return 0;
+}
+
+device_initcall(xt2000_setup_devinit);
diff --git a/include/asm-xtensa/variant-dc232b/core.h b/arch/xtensa/variants/dc232b/include/variant/core.h
index 525bd3d90154..525bd3d90154 100644
--- a/include/asm-xtensa/variant-dc232b/core.h
+++ b/arch/xtensa/variants/dc232b/include/variant/core.h
diff --git a/include/asm-xtensa/variant-dc232b/tie-asm.h b/arch/xtensa/variants/dc232b/include/variant/tie-asm.h
index ed4f53f529db..ed4f53f529db 100644
--- a/include/asm-xtensa/variant-dc232b/tie-asm.h
+++ b/arch/xtensa/variants/dc232b/include/variant/tie-asm.h
diff --git a/include/asm-xtensa/variant-dc232b/tie.h b/arch/xtensa/variants/dc232b/include/variant/tie.h
index 018e81af4393..018e81af4393 100644
--- a/include/asm-xtensa/variant-dc232b/tie.h
+++ b/arch/xtensa/variants/dc232b/include/variant/tie.h
diff --git a/include/asm-xtensa/variant-fsf/core.h b/arch/xtensa/variants/fsf/include/variant/core.h
index 2f337605c744..2f337605c744 100644
--- a/include/asm-xtensa/variant-fsf/core.h
+++ b/arch/xtensa/variants/fsf/include/variant/core.h
diff --git a/include/asm-xtensa/variant-fsf/tie-asm.h b/arch/xtensa/variants/fsf/include/variant/tie-asm.h
index 68a73bf4ffc5..68a73bf4ffc5 100644
--- a/include/asm-xtensa/variant-fsf/tie-asm.h
+++ b/arch/xtensa/variants/fsf/include/variant/tie-asm.h
diff --git a/include/asm-xtensa/variant-fsf/tie.h b/arch/xtensa/variants/fsf/include/variant/tie.h
index bf4020116df5..bf4020116df5 100644
--- a/include/asm-xtensa/variant-fsf/tie.h
+++ b/arch/xtensa/variants/fsf/include/variant/tie.h
diff --git a/block/bsg.c b/block/bsg.c
index e73e50daf3d0..d414bb5607e8 100644
--- a/block/bsg.c
+++ b/block/bsg.c
@@ -42,7 +42,7 @@ struct bsg_device {
 	int done_cmds;
 	wait_queue_head_t wq_done;
 	wait_queue_head_t wq_free;
-	char name[BUS_ID_SIZE];
+	char name[20];
 	int max_queue;
 	unsigned long flags;
 };
@@ -781,7 +781,7 @@ static struct bsg_device *bsg_add_device(struct inode *inode,
 	mutex_lock(&bsg_mutex);
 	hlist_add_head(&bd->dev_list, bsg_dev_idx_hash(iminor(inode)));
 
-	strncpy(bd->name, rq->bsg_dev.class_dev->bus_id, sizeof(bd->name) - 1);
+	strncpy(bd->name, dev_name(rq->bsg_dev.class_dev), sizeof(bd->name) - 1);
 	dprintk("bound to <%s>, max queue %d\n",
 		format_dev_t(buf, inode->i_rdev), bd->max_queue);
 
@@ -992,7 +992,7 @@ int bsg_register_queue(struct request_queue *q, struct device *parent,
 	if (name)
 		devname = name;
 	else
-		devname = parent->bus_id;
+		devname = dev_name(parent);
 
 	/*
 	 * we need a proper transport to send commands, not a stacked device
diff --git a/block/genhd.c b/block/genhd.c
index d84a7df1e2a0..397960cf26af 100644
--- a/block/genhd.c
+++ b/block/genhd.c
@@ -1084,7 +1084,7 @@ dev_t blk_lookup_devt(const char *name, int partno)
 		struct gendisk *disk = dev_to_disk(dev);
 		struct hd_struct *part;
 
-		if (strcmp(dev->bus_id, name))
+		if (strcmp(dev_name(dev), name))
 			continue;
 
 		part = disk_get_part(disk, partno);
diff --git a/drivers/Makefile b/drivers/Makefile
index fceb71a741c3..e121b66ef082 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -57,6 +57,7 @@ obj-$(CONFIG_ATA_OVER_ETH)	+= block/aoe/
 obj-$(CONFIG_PARIDE) 		+= block/paride/
 obj-$(CONFIG_TC)		+= tc/
 obj-$(CONFIG_UWB)		+= uwb/
+obj-$(CONFIG_USB_OTG_UTILS)	+= usb/otg/
 obj-$(CONFIG_USB)		+= usb/
 obj-$(CONFIG_USB_MUSB_HDRC)	+= usb/musb/
 obj-$(CONFIG_PCI)		+= usb/
diff --git a/drivers/acpi/acpi_memhotplug.c b/drivers/acpi/acpi_memhotplug.c
index 63a17b55b39b..7a0f4aa4fa1e 100644
--- a/drivers/acpi/acpi_memhotplug.c
+++ b/drivers/acpi/acpi_memhotplug.c
@@ -20,7 +20,7 @@
  *
  *
  * ACPI based HotPlug driver that supports Memory Hotplug
- * This driver fields notifications from firmare for memory add
+ * This driver fields notifications from firmware for memory add
  * and remove operations and alerts the VM of the affected memory
  * ranges.
  */
diff --git a/drivers/acpi/executer/exprep.c b/drivers/acpi/executer/exprep.c
index 5d438c32989d..a7dc87ecee37 100644
--- a/drivers/acpi/executer/exprep.c
+++ b/drivers/acpi/executer/exprep.c
@@ -404,7 +404,7 @@ acpi_ex_prep_common_field_object(union acpi_operand_object *obj_desc,
  *
  * RETURN:      Status
  *
- * DESCRIPTION: Construct an union acpi_operand_object of type def_field and
+ * DESCRIPTION: Construct a union acpi_operand_object of type def_field and
  *              connect it to the parent Node.
  *
  ******************************************************************************/
diff --git a/drivers/acpi/executer/exresolv.c b/drivers/acpi/executer/exresolv.c
index 89571b92a522..60e8c47128e9 100644
--- a/drivers/acpi/executer/exresolv.c
+++ b/drivers/acpi/executer/exresolv.c
@@ -146,7 +146,7 @@ acpi_ex_resolve_object_to_value(union acpi_operand_object **stack_ptr,
 
 	stack_desc = *stack_ptr;
 
-	/* This is an union acpi_operand_object    */
+	/* This is a union acpi_operand_object    */
 
 	switch (ACPI_GET_OBJECT_TYPE(stack_desc)) {
 	case ACPI_TYPE_LOCAL_REFERENCE:
diff --git a/drivers/acpi/executer/exstore.c b/drivers/acpi/executer/exstore.c
index 3318df4cbd98..1c118ba78adb 100644
--- a/drivers/acpi/executer/exstore.c
+++ b/drivers/acpi/executer/exstore.c
@@ -274,7 +274,7 @@ acpi_ex_do_debug_object(union acpi_operand_object *source_desc,
  *
  * PARAMETERS:  *source_desc        - Value to be stored
  *              *dest_desc          - Where to store it.  Must be an NS node
- *                                    or an union acpi_operand_object of type
+ *                                    or a union acpi_operand_object of type
  *                                    Reference;
  *              walk_state          - Current walk state
  *
diff --git a/drivers/acpi/pci_root.c b/drivers/acpi/pci_root.c
index 642554b1b60c..5b38a026d122 100644
--- a/drivers/acpi/pci_root.c
+++ b/drivers/acpi/pci_root.c
@@ -31,6 +31,7 @@
 #include <linux/spinlock.h>
 #include <linux/pm.h>
 #include <linux/pci.h>
+#include <linux/pci-acpi.h>
 #include <linux/acpi.h>
 #include <acpi/acpi_bus.h>
 #include <acpi/acpi_drivers.h>
@@ -193,6 +194,7 @@ static int __devinit acpi_pci_root_add(struct acpi_device *device)
 	unsigned long long value = 0;
 	acpi_handle handle = NULL;
 	struct acpi_device *child;
+	u32 flags, base_flags;
 
 
 	if (!device)
@@ -210,6 +212,13 @@ static int __devinit acpi_pci_root_add(struct acpi_device *device)
 
 	device->ops.bind = acpi_pci_bind;
 
+	/*
+	 * All supported architectures that use ACPI have support for
+	 * PCI domains, so we indicate this in _OSC support capabilities.
+	 */
+	flags = base_flags = OSC_PCI_SEGMENT_GROUPS_SUPPORT;
+	pci_acpi_osc_support(device->handle, flags);
+
 	/* 
 	 * Segment
 	 * -------
@@ -335,6 +344,17 @@ static int __devinit acpi_pci_root_add(struct acpi_device *device)
 	list_for_each_entry(child, &device->children, node)
 		acpi_pci_bridge_scan(child);
 
+	/* Indicate support for various _OSC capabilities. */
+	if (pci_ext_cfg_avail(root->bus->self))
+		flags |= OSC_EXT_PCI_CONFIG_SUPPORT;
+	if (pcie_aspm_enabled())
+		flags |= OSC_ACTIVE_STATE_PWR_SUPPORT |
+			OSC_CLOCK_PWR_CAPABILITY_SUPPORT;
+	if (pci_msi_enabled())
+		flags |= OSC_MSI_SUPPORT;
+	if (flags != base_flags)
+		pci_acpi_osc_support(device->handle, flags);
+
       end:
 	if (result) {
 		if (!list_empty(&root->node))
diff --git a/drivers/acpi/processor_idle.c b/drivers/acpi/processor_idle.c
index 38aca048e951..66a9d8145562 100644
--- a/drivers/acpi/processor_idle.c
+++ b/drivers/acpi/processor_idle.c
@@ -41,6 +41,7 @@
 #include <linux/pm_qos_params.h>
 #include <linux/clockchips.h>
 #include <linux/cpuidle.h>
+#include <linux/irqflags.h>
 
 /*
  * Include the apic definitions for x86 to have the APIC timer related defines
diff --git a/drivers/acpi/resources/rscreate.c b/drivers/acpi/resources/rscreate.c
index c0bbfa2c4193..08b8d73e6ee5 100644
--- a/drivers/acpi/resources/rscreate.c
+++ b/drivers/acpi/resources/rscreate.c
@@ -124,7 +124,7 @@ acpi_rs_create_resource_list(union acpi_operand_object *aml_buffer,
  *
  * FUNCTION:    acpi_rs_create_pci_routing_table
  *
- * PARAMETERS:  package_object          - Pointer to an union acpi_operand_object
+ * PARAMETERS:  package_object          - Pointer to a union acpi_operand_object
  *                                        package
  *              output_buffer           - Pointer to the user's buffer
  *
diff --git a/drivers/acpi/utilities/utobject.c b/drivers/acpi/utilities/utobject.c
index c354e7a42bcd..4bef3cfbaccb 100644
--- a/drivers/acpi/utilities/utobject.c
+++ b/drivers/acpi/utilities/utobject.c
@@ -297,7 +297,7 @@ union acpi_operand_object *acpi_ut_create_string_object(acpi_size string_size)
  *
  * RETURN:      TRUE if object is valid, FALSE otherwise
  *
- * DESCRIPTION: Validate a pointer to be an union acpi_operand_object
+ * DESCRIPTION: Validate a pointer to be a union acpi_operand_object
  *
  ******************************************************************************/
 
@@ -389,7 +389,7 @@ void acpi_ut_delete_object_desc(union acpi_operand_object *object)
 {
 	ACPI_FUNCTION_TRACE_PTR(ut_delete_object_desc, object);
 
-	/* Object must be an union acpi_operand_object    */
+	/* Object must be a union acpi_operand_object    */
 
 	if (ACPI_GET_DESCRIPTOR_TYPE(object) != ACPI_DESC_TYPE_OPERAND) {
 		ACPI_ERROR((AE_INFO,
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index fecca4223f8e..f178a450ec08 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -56,6 +56,7 @@
 #include <linux/workqueue.h>
 #include <linux/scatterlist.h>
 #include <linux/io.h>
+#include <linux/async.h>
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
 #include <scsi/scsi_host.h>
@@ -5909,6 +5910,54 @@ void ata_host_init(struct ata_host *host, struct device *dev,
 	host->ops = ops;
 }
 
+
+static void async_port_probe(void *data, async_cookie_t cookie)
+{
+	int rc;
+	struct ata_port *ap = data;
+	/* probe */
+	if (ap->ops->error_handler) {
+		struct ata_eh_info *ehi = &ap->link.eh_info;
+		unsigned long flags;
+
+		ata_port_probe(ap);
+
+		/* kick EH for boot probing */
+		spin_lock_irqsave(ap->lock, flags);
+
+		ehi->probe_mask |= ATA_ALL_DEVICES;
+		ehi->action |= ATA_EH_RESET | ATA_EH_LPM;
+		ehi->flags |= ATA_EHI_NO_AUTOPSY | ATA_EHI_QUIET;
+
+		ap->pflags &= ~ATA_PFLAG_INITIALIZING;
+		ap->pflags |= ATA_PFLAG_LOADING;
+		ata_port_schedule_eh(ap);
+
+		spin_unlock_irqrestore(ap->lock, flags);
+
+		/* wait for EH to finish */
+		ata_port_wait_eh(ap);
+	} else {
+		DPRINTK("ata%u: bus probe begin\n", ap->print_id);
+		rc = ata_bus_probe(ap);
+		DPRINTK("ata%u: bus probe end\n", ap->print_id);
+
+		if (rc) {
+			/* FIXME: do something useful here?
+			 * Current libata behavior will
+			 * tear down everything when
+			 * the module is removed
+			 * or the h/w is unplugged.
+			 */
+		}
+	}
+
+	/* in order to keep device order, we need to synchronize at this point */
+	async_synchronize_cookie(cookie);
+
+	ata_scsi_scan_host(ap, 1);
+
+}
 /**
  *	ata_host_register - register initialized ATA host
  *	@host: ATA host to register
@@ -5988,52 +6037,9 @@ int ata_host_register(struct ata_host *host, struct scsi_host_template *sht)
 	DPRINTK("probe begin\n");
 	for (i = 0; i < host->n_ports; i++) {
 		struct ata_port *ap = host->ports[i];
-
-		/* probe */
-		if (ap->ops->error_handler) {
-			struct ata_eh_info *ehi = &ap->link.eh_info;
-			unsigned long flags;
-
-			ata_port_probe(ap);
-
-			/* kick EH for boot probing */
-			spin_lock_irqsave(ap->lock, flags);
-
-			ehi->probe_mask |= ATA_ALL_DEVICES;
-			ehi->action |= ATA_EH_RESET | ATA_EH_LPM;
-			ehi->flags |= ATA_EHI_NO_AUTOPSY | ATA_EHI_QUIET;
-
-			ap->pflags &= ~ATA_PFLAG_INITIALIZING;
-			ap->pflags |= ATA_PFLAG_LOADING;
-			ata_port_schedule_eh(ap);
-
-			spin_unlock_irqrestore(ap->lock, flags);
-
-			/* wait for EH to finish */
-			ata_port_wait_eh(ap);
-		} else {
-			DPRINTK("ata%u: bus probe begin\n", ap->print_id);
-			rc = ata_bus_probe(ap);
-			DPRINTK("ata%u: bus probe end\n", ap->print_id);
-
-			if (rc) {
-				/* FIXME: do something useful here?
-				 * Current libata behavior will
-				 * tear down everything when
-				 * the module is removed
-				 * or the h/w is unplugged.
-				 */
-			}
-		}
-	}
-
-	/* probes are done, now scan each port's disk(s) */
-	DPRINTK("host probe begin\n");
-	for (i = 0; i < host->n_ports; i++) {
-		struct ata_port *ap = host->ports[i];
-
-		ata_scsi_scan_host(ap, 1);
+		async_schedule(async_port_probe, ap);
 	}
+	DPRINTK("probe end\n");
 
 	return 0;
 }
diff --git a/drivers/ata/libata-scsi.c b/drivers/ata/libata-scsi.c
index 4040d8b53216..9e92107691f2 100644
--- a/drivers/ata/libata-scsi.c
+++ b/drivers/ata/libata-scsi.c
@@ -3369,7 +3369,7 @@ static void ata_scsi_remove_dev(struct ata_device *dev)
 
 	if (sdev) {
 		ata_dev_printk(dev, KERN_INFO, "detaching (SCSI %s)\n",
-			       sdev->sdev_gendev.bus_id);
+			       dev_name(&sdev->sdev_gendev));
 
 		scsi_remove_device(sdev);
 		scsi_device_put(sdev);
diff --git a/drivers/base/Kconfig b/drivers/base/Kconfig
index d8e8c49c0cbd..8f006f96ff53 100644
--- a/drivers/base/Kconfig
+++ b/drivers/base/Kconfig
@@ -54,7 +54,7 @@ config FIRMWARE_IN_KERNEL
 	  such firmware, and do not wish to use an initrd.
 
 	  This single option controls the inclusion of firmware for
-	  every driver which uses request_firmare() and ships its
+	  every driver which uses request_firmware() and ships its
 	  firmware in the kernel source tree, to avoid a proliferation
 	  of 'Include firmware for xxx device' options.
 
diff --git a/drivers/base/attribute_container.c b/drivers/base/attribute_container.c
index f57652db0a2a..b9cda053d3c0 100644
--- a/drivers/base/attribute_container.c
+++ b/drivers/base/attribute_container.c
@@ -167,7 +167,7 @@ attribute_container_add_device(struct device *dev,
 		ic->classdev.parent = get_device(dev);
 		ic->classdev.class = cont->class;
 		cont->class->dev_release = attribute_container_release;
-		strcpy(ic->classdev.bus_id, dev->bus_id);
+		dev_set_name(&ic->classdev, dev_name(dev));
 		if (fn)
 			fn(cont, dev, &ic->classdev);
 		else
diff --git a/drivers/base/base.h b/drivers/base/base.h
index 0a5f055dffba..b676f8f801f8 100644
--- a/drivers/base/base.h
+++ b/drivers/base/base.h
@@ -63,6 +63,32 @@ struct class_private {
 #define to_class(obj)	\
 	container_of(obj, struct class_private, class_subsys.kobj)
 
+/**
+ * struct device_private - structure to hold the private to the driver core portions of the device structure.
+ *
+ * @klist_children - klist containing all children of this device
+ * @knode_parent - node in sibling list
+ * @knode_driver - node in driver list
+ * @knode_bus - node in bus list
+ * @device - pointer back to the struct class that this structure is
+ * associated with.
+ *
+ * Nothing outside of the driver core should ever touch these fields.
+ */
+struct device_private {
+	struct klist klist_children;
+	struct klist_node knode_parent;
+	struct klist_node knode_driver;
+	struct klist_node knode_bus;
+	struct device *device;
+};
+#define to_device_private_parent(obj)	\
+	container_of(obj, struct device_private, knode_parent)
+#define to_device_private_driver(obj)	\
+	container_of(obj, struct device_private, knode_driver)
+#define to_device_private_bus(obj)	\
+	container_of(obj, struct device_private, knode_bus)
+
 /* initialisation functions */
 extern int devices_init(void);
 extern int buses_init(void);
diff --git a/drivers/base/bus.c b/drivers/base/bus.c
index 5aee1c0169ea..0f0a50444672 100644
--- a/drivers/base/bus.c
+++ b/drivers/base/bus.c
@@ -253,7 +253,14 @@ static ssize_t store_drivers_probe(struct bus_type *bus,
 static struct device *next_device(struct klist_iter *i)
 {
 	struct klist_node *n = klist_next(i);
-	return n ? container_of(n, struct device, knode_bus) : NULL;
+	struct device *dev = NULL;
+	struct device_private *dev_prv;
+
+	if (n) {
+		dev_prv = to_device_private_bus(n);
+		dev = dev_prv->device;
+	}
+	return dev;
 }
 
 /**
@@ -286,7 +293,7 @@ int bus_for_each_dev(struct bus_type *bus, struct device *start,
 		return -EINVAL;
 
 	klist_iter_init_node(&bus->p->klist_devices, &i,
-			     (start ? &start->knode_bus : NULL));
+			     (start ? &start->p->knode_bus : NULL));
 	while ((dev = next_device(&i)) && !error)
 		error = fn(dev, data);
 	klist_iter_exit(&i);
@@ -320,7 +327,7 @@ struct device *bus_find_device(struct bus_type *bus,
 		return NULL;
 
 	klist_iter_init_node(&bus->p->klist_devices, &i,
-			     (start ? &start->knode_bus : NULL));
+			     (start ? &start->p->knode_bus : NULL));
 	while ((dev = next_device(&i)))
 		if (match(dev, data) && get_device(dev))
 			break;
@@ -333,7 +340,7 @@ static int match_name(struct device *dev, void *data)
 {
 	const char *name = data;
 
-	return sysfs_streq(name, dev->bus_id);
+	return sysfs_streq(name, dev_name(dev));
 }
 
 /**
@@ -461,12 +468,12 @@ int bus_add_device(struct device *dev)
 	int error = 0;
 
 	if (bus) {
-		pr_debug("bus: '%s': add device %s\n", bus->name, dev->bus_id);
+		pr_debug("bus: '%s': add device %s\n", bus->name, dev_name(dev));
 		error = device_add_attrs(bus, dev);
 		if (error)
 			goto out_put;
 		error = sysfs_create_link(&bus->p->devices_kset->kobj,
-						&dev->kobj, dev->bus_id);
+						&dev->kobj, dev_name(dev));
 		if (error)
 			goto out_id;
 		error = sysfs_create_link(&dev->kobj,
@@ -482,7 +489,7 @@ int bus_add_device(struct device *dev)
 out_deprecated:
 	sysfs_remove_link(&dev->kobj, "subsystem");
 out_subsys:
-	sysfs_remove_link(&bus->p->devices_kset->kobj, dev->bus_id);
+	sysfs_remove_link(&bus->p->devices_kset->kobj, dev_name(dev));
 out_id:
 	device_remove_attrs(bus, dev);
 out_put:
@@ -507,7 +514,8 @@ void bus_attach_device(struct device *dev)
 			ret = device_attach(dev);
 		WARN_ON(ret < 0);
 		if (ret >= 0)
-			klist_add_tail(&dev->knode_bus, &bus->p->klist_devices);
+			klist_add_tail(&dev->p->knode_bus,
+				       &bus->p->klist_devices);
 	}
 }
 
@@ -526,13 +534,13 @@ void bus_remove_device(struct device *dev)
 		sysfs_remove_link(&dev->kobj, "subsystem");
 		remove_deprecated_bus_links(dev);
 		sysfs_remove_link(&dev->bus->p->devices_kset->kobj,
-				  dev->bus_id);
+				  dev_name(dev));
 		device_remove_attrs(dev->bus, dev);
-		if (klist_node_attached(&dev->knode_bus))
-			klist_del(&dev->knode_bus);
+		if (klist_node_attached(&dev->p->knode_bus))
+			klist_del(&dev->p->knode_bus);
 
 		pr_debug("bus: '%s': remove device %s\n",
-			 dev->bus->name, dev->bus_id);
+			 dev->bus->name, dev_name(dev));
 		device_release_driver(dev);
 		bus_put(dev->bus);
 	}
@@ -831,14 +839,16 @@ static void bus_remove_attrs(struct bus_type *bus)
 
 static void klist_devices_get(struct klist_node *n)
 {
-	struct device *dev = container_of(n, struct device, knode_bus);
+	struct device_private *dev_prv = to_device_private_bus(n);
+	struct device *dev = dev_prv->device;
 
 	get_device(dev);
 }
 
 static void klist_devices_put(struct klist_node *n)
 {
-	struct device *dev = container_of(n, struct device, knode_bus);
+	struct device_private *dev_prv = to_device_private_bus(n);
+	struct device *dev = dev_prv->device;
 
 	put_device(dev);
 }
@@ -993,18 +1003,20 @@ static void device_insertion_sort_klist(struct device *a, struct list_head *list
 {
 	struct list_head *pos;
 	struct klist_node *n;
+	struct device_private *dev_prv;
 	struct device *b;
 
 	list_for_each(pos, list) {
 		n = container_of(pos, struct klist_node, n_node);
-		b = container_of(n, struct device, knode_bus);
+		dev_prv = to_device_private_bus(n);
+		b = dev_prv->device;
 		if (compare(a, b) <= 0) {
-			list_move_tail(&a->knode_bus.n_node,
-				       &b->knode_bus.n_node);
+			list_move_tail(&a->p->knode_bus.n_node,
+				       &b->p->knode_bus.n_node);
 			return;
 		}
 	}
-	list_move_tail(&a->knode_bus.n_node, list);
+	list_move_tail(&a->p->knode_bus.n_node, list);
 }
 
 void bus_sort_breadthfirst(struct bus_type *bus,
@@ -1014,6 +1026,7 @@ void bus_sort_breadthfirst(struct bus_type *bus,
 	LIST_HEAD(sorted_devices);
 	struct list_head *pos, *tmp;
 	struct klist_node *n;
+	struct device_private *dev_prv;
 	struct device *dev;
 	struct klist *device_klist;
 
@@ -1022,7 +1035,8 @@ void bus_sort_breadthfirst(struct bus_type *bus,
 	spin_lock(&device_klist->k_lock);
 	list_for_each_safe(pos, tmp, &device_klist->k_list) {
 		n = container_of(pos, struct klist_node, n_node);
-		dev = container_of(n, struct device, knode_bus);
+		dev_prv = to_device_private_bus(n);
+		dev = dev_prv->device;
 		device_insertion_sort_klist(dev, &sorted_devices, compare);
 	}
 	list_splice(&sorted_devices, &device_klist->k_list);
diff --git a/drivers/base/core.c b/drivers/base/core.c
index 8c2cc2648f5a..61df508fa62b 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -109,6 +109,7 @@ static struct sysfs_ops dev_sysfs_ops = {
 static void device_release(struct kobject *kobj)
 {
 	struct device *dev = to_dev(kobj);
+	struct device_private *p = dev->p;
 
 	if (dev->release)
 		dev->release(dev);
@@ -119,7 +120,8 @@ static void device_release(struct kobject *kobj)
 	else
 		WARN(1, KERN_ERR "Device '%s' does not have a release() "
 			"function, it is broken and must be fixed.\n",
-			dev->bus_id);
+			dev_name(dev));
+	kfree(p);
 }
 
 static struct kobj_type device_ktype = {
@@ -209,7 +211,7 @@ static int dev_uevent(struct kset *kset, struct kobject *kobj,
 		retval = dev->bus->uevent(dev, env);
 		if (retval)
 			pr_debug("device: '%s': %s: bus uevent() returned %d\n",
-				 dev->bus_id, __func__, retval);
+				 dev_name(dev), __func__, retval);
 	}
 
 	/* have the class specific function add its stuff */
@@ -217,7 +219,7 @@ static int dev_uevent(struct kset *kset, struct kobject *kobj,
 		retval = dev->class->dev_uevent(dev, env);
 		if (retval)
 			pr_debug("device: '%s': %s: class uevent() "
-				 "returned %d\n", dev->bus_id,
+				 "returned %d\n", dev_name(dev),
 				 __func__, retval);
 	}
 
@@ -226,7 +228,7 @@ static int dev_uevent(struct kset *kset, struct kobject *kobj,
 		retval = dev->type->uevent(dev, env);
 		if (retval)
 			pr_debug("device: '%s': %s: dev_type uevent() "
-				 "returned %d\n", dev->bus_id,
+				 "returned %d\n", dev_name(dev),
 				 __func__, retval);
 	}
 
@@ -507,14 +509,16 @@ EXPORT_SYMBOL_GPL(device_schedule_callback_owner);
 
 static void klist_children_get(struct klist_node *n)
 {
-	struct device *dev = container_of(n, struct device, knode_parent);
+	struct device_private *p = to_device_private_parent(n);
+	struct device *dev = p->device;
 
 	get_device(dev);
 }
 
 static void klist_children_put(struct klist_node *n)
 {
-	struct device *dev = container_of(n, struct device, knode_parent);
+	struct device_private *p = to_device_private_parent(n);
+	struct device *dev = p->device;
 
 	put_device(dev);
 }
@@ -536,9 +540,15 @@ static void klist_children_put(struct klist_node *n)
  */
 void device_initialize(struct device *dev)
 {
+	dev->p = kzalloc(sizeof(*dev->p), GFP_KERNEL);
+	if (!dev->p) {
+		WARN_ON(1);
+		return;
+	}
+	dev->p->device = dev;
 	dev->kobj.kset = devices_kset;
 	kobject_init(&dev->kobj, &device_ktype);
-	klist_init(&dev->klist_children, klist_children_get,
+	klist_init(&dev->p->klist_children, klist_children_get,
 		   klist_children_put);
 	INIT_LIST_HEAD(&dev->dma_pools);
 	init_MUTEX(&dev->sem);
@@ -672,7 +682,7 @@ static int device_add_class_symlinks(struct device *dev)
 	if (dev->kobj.parent != &dev->class->p->class_subsys.kobj &&
 	    device_is_not_partition(dev)) {
 		error = sysfs_create_link(&dev->class->p->class_subsys.kobj,
-					  &dev->kobj, dev->bus_id);
+					  &dev->kobj, dev_name(dev));
 		if (error)
 			goto out_subsys;
 	}
@@ -712,11 +722,11 @@ out_busid:
 	if (dev->kobj.parent != &dev->class->p->class_subsys.kobj &&
 	    device_is_not_partition(dev))
 		sysfs_remove_link(&dev->class->p->class_subsys.kobj,
-				  dev->bus_id);
+				  dev_name(dev));
 #else
 	/* link in the class directory pointing to the device */
 	error = sysfs_create_link(&dev->class->p->class_subsys.kobj,
-				  &dev->kobj, dev->bus_id);
+				  &dev->kobj, dev_name(dev));
 	if (error)
 		goto out_subsys;
 
@@ -729,7 +739,7 @@ out_busid:
 	return 0;
 
 out_busid:
-	sysfs_remove_link(&dev->class->p->class_subsys.kobj, dev->bus_id);
+	sysfs_remove_link(&dev->class->p->class_subsys.kobj, dev_name(dev));
 #endif
 
 out_subsys:
@@ -758,12 +768,12 @@ static void device_remove_class_symlinks(struct device *dev)
 	if (dev->kobj.parent != &dev->class->p->class_subsys.kobj &&
 	    device_is_not_partition(dev))
 		sysfs_remove_link(&dev->class->p->class_subsys.kobj,
-				  dev->bus_id);
+				  dev_name(dev));
 #else
 	if (dev->parent && device_is_not_partition(dev))
 		sysfs_remove_link(&dev->kobj, "device");
 
-	sysfs_remove_link(&dev->class->p->class_subsys.kobj, dev->bus_id);
+	sysfs_remove_link(&dev->class->p->class_subsys.kobj, dev_name(dev));
 #endif
 
 	sysfs_remove_link(&dev->kobj, "subsystem");
@@ -866,7 +876,7 @@ int device_add(struct device *dev)
 	if (!strlen(dev->bus_id))
 		goto done;
 
-	pr_debug("device: '%s': %s\n", dev->bus_id, __func__);
+	pr_debug("device: '%s': %s\n", dev_name(dev), __func__);
 
 	parent = get_device(dev->parent);
 	setup_parent(dev, parent);
@@ -876,7 +886,7 @@ int device_add(struct device *dev)
 		set_dev_node(dev, dev_to_node(parent));
 
 	/* first, register with generic layer. */
-	error = kobject_add(&dev->kobj, dev->kobj.parent, "%s", dev->bus_id);
+	error = kobject_add(&dev->kobj, dev->kobj.parent, "%s", dev_name(dev));
 	if (error)
 		goto Error;
 
@@ -884,11 +894,6 @@ int device_add(struct device *dev)
 	if (platform_notify)
 		platform_notify(dev);
 
-	/* notify clients of device entry (new way) */
-	if (dev->bus)
-		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
-					     BUS_NOTIFY_ADD_DEVICE, dev);
-
 	error = device_create_file(dev, &uevent_attr);
 	if (error)
 		goto attrError;
@@ -916,10 +921,19 @@ int device_add(struct device *dev)
 	if (error)
 		goto DPMError;
 	device_pm_add(dev);
+
+	/* Notify clients of device addition.  This call must come
+	 * after dpm_sysf_add() and before kobject_uevent().
+	 */
+	if (dev->bus)
+		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
+					     BUS_NOTIFY_ADD_DEVICE, dev);
+
 	kobject_uevent(&dev->kobj, KOBJ_ADD);
 	bus_attach_device(dev);
 	if (parent)
-		klist_add_tail(&dev->knode_parent, &parent->klist_children);
+		klist_add_tail(&dev->p->knode_parent,
+			       &parent->p->klist_children);
 
 	if (dev->class) {
 		mutex_lock(&dev->class->p->class_mutex);
@@ -940,9 +954,6 @@ done:
  DPMError:
 	bus_remove_device(dev);
  BusError:
-	if (dev->bus)
-		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
-					     BUS_NOTIFY_DEL_DEVICE, dev);
 	device_remove_attrs(dev);
  AttrsError:
 	device_remove_class_symlinks(dev);
@@ -1027,10 +1038,16 @@ void device_del(struct device *dev)
 	struct device *parent = dev->parent;
 	struct class_interface *class_intf;
 
+	/* Notify clients of device removal.  This call must come
+	 * before dpm_sysfs_remove().
+	 */
+	if (dev->bus)
+		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
+					     BUS_NOTIFY_DEL_DEVICE, dev);
 	device_pm_remove(dev);
 	dpm_sysfs_remove(dev);
 	if (parent)
-		klist_del(&dev->knode_parent);
+		klist_del(&dev->p->knode_parent);
 	if (MAJOR(dev->devt)) {
 		device_remove_sys_dev_entry(dev);
 		device_remove_file(dev, &devt_attr);
@@ -1064,9 +1081,6 @@ void device_del(struct device *dev)
 	 */
 	if (platform_notify_remove)
 		platform_notify_remove(dev);
-	if (dev->bus)
-		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
-					     BUS_NOTIFY_DEL_DEVICE, dev);
 	kobject_uevent(&dev->kobj, KOBJ_REMOVE);
 	cleanup_device_parent(dev);
 	kobject_del(&dev->kobj);
@@ -1086,7 +1100,7 @@ void device_del(struct device *dev)
  */
 void device_unregister(struct device *dev)
 {
-	pr_debug("device: '%s': %s\n", dev->bus_id, __func__);
+	pr_debug("device: '%s': %s\n", dev_name(dev), __func__);
 	device_del(dev);
 	put_device(dev);
 }
@@ -1094,7 +1108,14 @@ void device_unregister(struct device *dev)
 static struct device *next_device(struct klist_iter *i)
 {
 	struct klist_node *n = klist_next(i);
-	return n ? container_of(n, struct device, knode_parent) : NULL;
+	struct device *dev = NULL;
+	struct device_private *p;
+
+	if (n) {
+		p = to_device_private_parent(n);
+		dev = p->device;
+	}
+	return dev;
 }
 
 /**
@@ -1116,7 +1137,7 @@ int device_for_each_child(struct device *parent, void *data,
 	struct device *child;
 	int error = 0;
 
-	klist_iter_init(&parent->klist_children, &i);
+	klist_iter_init(&parent->p->klist_children, &i);
 	while ((child = next_device(&i)) && !error)
 		error = fn(child, data);
 	klist_iter_exit(&i);
@@ -1147,7 +1168,7 @@ struct device *device_find_child(struct device *parent, void *data,
 	if (!parent)
 		return NULL;
 
-	klist_iter_init(&parent->klist_children, &i);
+	klist_iter_init(&parent->p->klist_children, &i);
 	while ((child = next_device(&i)))
 		if (match(child, data) && get_device(child))
 			break;
@@ -1196,10 +1217,101 @@ EXPORT_SYMBOL_GPL(put_device);
 EXPORT_SYMBOL_GPL(device_create_file);
 EXPORT_SYMBOL_GPL(device_remove_file);
 
+struct root_device
+{
+	struct device dev;
+	struct module *owner;
+};
+
+#define to_root_device(dev) container_of(dev, struct root_device, dev)
+
+static void root_device_release(struct device *dev)
+{
+	kfree(to_root_device(dev));
+}
+
+/**
+ * __root_device_register - allocate and register a root device
+ * @name: root device name
+ * @owner: owner module of the root device, usually THIS_MODULE
+ *
+ * This function allocates a root device and registers it
+ * using device_register(). In order to free the returned
+ * device, use root_device_unregister().
+ *
+ * Root devices are dummy devices which allow other devices
+ * to be grouped under /sys/devices. Use this function to
+ * allocate a root device and then use it as the parent of
+ * any device which should appear under /sys/devices/{name}
+ *
+ * The /sys/devices/{name} directory will also contain a
+ * 'module' symlink which points to the @owner directory
+ * in sysfs.
+ *
+ * Note: You probably want to use root_device_register().
+ */
+struct device *__root_device_register(const char *name, struct module *owner)
+{
+	struct root_device *root;
+	int err = -ENOMEM;
+
+	root = kzalloc(sizeof(struct root_device), GFP_KERNEL);
+	if (!root)
+		return ERR_PTR(err);
+
+	err = dev_set_name(&root->dev, name);
+	if (err) {
+		kfree(root);
+		return ERR_PTR(err);
+	}
+
+	root->dev.release = root_device_release;
+
+	err = device_register(&root->dev);
+	if (err) {
+		put_device(&root->dev);
+		return ERR_PTR(err);
+	}
+
+#ifdef CONFIG_MODULE	/* gotta find a "cleaner" way to do this */
+	if (owner) {
+		struct module_kobject *mk = &owner->mkobj;
+
+		err = sysfs_create_link(&root->dev.kobj, &mk->kobj, "module");
+		if (err) {
+			device_unregister(&root->dev);
+			return ERR_PTR(err);
+		}
+		root->owner = owner;
+	}
+#endif
+
+	return &root->dev;
+}
+EXPORT_SYMBOL_GPL(__root_device_register);
+
+/**
+ * root_device_unregister - unregister and free a root device
+ * @root: device going away.
+ *
+ * This function unregisters and cleans up a device that was created by
+ * root_device_register().
+ */
+void root_device_unregister(struct device *dev)
+{
+	struct root_device *root = to_root_device(dev);
+
+	if (root->owner)
+		sysfs_remove_link(&root->dev.kobj, "module");
+
+	device_unregister(dev);
+}
+EXPORT_SYMBOL_GPL(root_device_unregister);
+
 
 static void device_create_release(struct device *dev)
 {
-	pr_debug("device: '%s': %s\n", dev->bus_id, __func__);
+	pr_debug("device: '%s': %s\n", dev_name(dev), __func__);
 	kfree(dev);
 }
 
@@ -1344,7 +1456,7 @@ int device_rename(struct device *dev, char *new_name)
 	if (!dev)
 		return -EINVAL;
 
-	pr_debug("device: '%s': %s: renaming to '%s'\n", dev->bus_id,
+	pr_debug("device: '%s': %s: renaming to '%s'\n", dev_name(dev),
 		 __func__, new_name);
 
 #ifdef CONFIG_SYSFS_DEPRECATED
@@ -1381,7 +1493,7 @@ int device_rename(struct device *dev, char *new_name)
 #else
 	if (dev->class) {
 		error = sysfs_create_link_nowarn(&dev->class->p->class_subsys.kobj,
-						 &dev->kobj, dev->bus_id);
+						 &dev->kobj, dev_name(dev));
 		if (error)
 			goto out;
 		sysfs_remove_link(&dev->class->p->class_subsys.kobj,
@@ -1459,8 +1571,8 @@ int device_move(struct device *dev, struct device *new_parent)
 	new_parent = get_device(new_parent);
 	new_parent_kobj = get_device_parent(dev, new_parent);
 
-	pr_debug("device: '%s': %s: moving to '%s'\n", dev->bus_id,
-		 __func__, new_parent ? new_parent->bus_id : "<NULL>");
+	pr_debug("device: '%s': %s: moving to '%s'\n", dev_name(dev),
+		 __func__, new_parent ? dev_name(new_parent) : "<NULL>");
 	error = kobject_move(&dev->kobj, new_parent_kobj);
 	if (error) {
 		cleanup_glue_dir(dev, new_parent_kobj);
@@ -1470,9 +1582,10 @@ int device_move(struct device *dev, struct device *new_parent)
 	old_parent = dev->parent;
 	dev->parent = new_parent;
 	if (old_parent)
-		klist_remove(&dev->knode_parent);
+		klist_remove(&dev->p->knode_parent);
 	if (new_parent) {
-		klist_add_tail(&dev->knode_parent, &new_parent->klist_children);
+		klist_add_tail(&dev->p->knode_parent,
+			       &new_parent->p->klist_children);
 		set_dev_node(dev, dev_to_node(new_parent));
 	}
 
@@ -1484,11 +1597,11 @@ int device_move(struct device *dev, struct device *new_parent)
 		device_move_class_links(dev, new_parent, old_parent);
 		if (!kobject_move(&dev->kobj, &old_parent->kobj)) {
 			if (new_parent)
-				klist_remove(&dev->knode_parent);
+				klist_remove(&dev->p->knode_parent);
 			dev->parent = old_parent;
 			if (old_parent) {
-				klist_add_tail(&dev->knode_parent,
-					       &old_parent->klist_children);
+				klist_add_tail(&dev->p->knode_parent,
+					       &old_parent->p->klist_children);
 				set_dev_node(dev, dev_to_node(old_parent));
 			}
 		}
diff --git a/drivers/base/dd.c b/drivers/base/dd.c
index 20febc00a525..6fdaf76f033f 100644
--- a/drivers/base/dd.c
+++ b/drivers/base/dd.c
@@ -28,20 +28,20 @@
 
 static void driver_bound(struct device *dev)
 {
-	if (klist_node_attached(&dev->knode_driver)) {
+	if (klist_node_attached(&dev->p->knode_driver)) {
 		printk(KERN_WARNING "%s: device %s already bound\n",
 			__func__, kobject_name(&dev->kobj));
 		return;
 	}
 
-	pr_debug("driver: '%s': %s: bound to device '%s'\n", dev->bus_id,
+	pr_debug("driver: '%s': %s: bound to device '%s'\n", dev_name(dev),
 		 __func__, dev->driver->name);
 
 	if (dev->bus)
 		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
 					     BUS_NOTIFY_BOUND_DRIVER, dev);
 
-	klist_add_tail(&dev->knode_driver, &dev->driver->p->klist_devices);
+	klist_add_tail(&dev->p->knode_driver, &dev->driver->p->klist_devices);
 }
 
 static int driver_sysfs_add(struct device *dev)
@@ -104,13 +104,13 @@ static int really_probe(struct device *dev, struct device_driver *drv)
 
 	atomic_inc(&probe_count);
 	pr_debug("bus: '%s': %s: probing driver %s with device %s\n",
-		 drv->bus->name, __func__, drv->name, dev->bus_id);
+		 drv->bus->name, __func__, drv->name, dev_name(dev));
 	WARN_ON(!list_empty(&dev->devres_head));
 
 	dev->driver = drv;
 	if (driver_sysfs_add(dev)) {
 		printk(KERN_ERR "%s: driver_sysfs_add(%s) failed\n",
-			__func__, dev->bus_id);
+			__func__, dev_name(dev));
 		goto probe_failed;
 	}
 
@@ -127,7 +127,7 @@ static int really_probe(struct device *dev, struct device_driver *drv)
 	driver_bound(dev);
 	ret = 1;
 	pr_debug("bus: '%s': %s: bound device %s to driver %s\n",
-		 drv->bus->name, __func__, dev->bus_id, drv->name);
+		 drv->bus->name, __func__, dev_name(dev), drv->name);
 	goto done;
 
 probe_failed:
@@ -139,7 +139,7 @@ probe_failed:
 		/* driver matched but the probe failed */
 		printk(KERN_WARNING
 		       "%s: probe of %s failed with error %d\n",
-		       drv->name, dev->bus_id, ret);
+		       drv->name, dev_name(dev), ret);
 	}
 	/*
 	 * Ignore errors returned by ->probe so that the next driver can try
@@ -194,7 +194,7 @@ int driver_probe_device(struct device_driver *drv, struct device *dev)
 		goto done;
 
 	pr_debug("bus: '%s': %s: matched device %s with driver %s\n",
-		 drv->bus->name, __func__, dev->bus_id, drv->name);
+		 drv->bus->name, __func__, dev_name(dev), drv->name);
 
 	ret = really_probe(dev, drv);
 
@@ -298,7 +298,6 @@ static void __device_release_driver(struct device *dev)
 	drv = dev->driver;
 	if (drv) {
 		driver_sysfs_remove(dev);
-		sysfs_remove_link(&dev->kobj, "driver");
 
 		if (dev->bus)
 			blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
@@ -311,7 +310,7 @@ static void __device_release_driver(struct device *dev)
 			drv->remove(dev);
 		devres_release_all(dev);
 		dev->driver = NULL;
-		klist_remove(&dev->knode_driver);
+		klist_remove(&dev->p->knode_driver);
 	}
 }
 
@@ -341,6 +340,7 @@ EXPORT_SYMBOL_GPL(device_release_driver);
  */
 void driver_detach(struct device_driver *drv)
 {
+	struct device_private *dev_prv;
 	struct device *dev;
 
 	for (;;) {
@@ -349,8 +349,10 @@ void driver_detach(struct device_driver *drv)
 			spin_unlock(&drv->p->klist_devices.k_lock);
 			break;
 		}
-		dev = list_entry(drv->p->klist_devices.k_list.prev,
-				struct device, knode_driver.n_node);
+		dev_prv = list_entry(drv->p->klist_devices.k_list.prev,
+				     struct device_private,
+				     knode_driver.n_node);
+		dev = dev_prv->device;
 		get_device(dev);
 		spin_unlock(&drv->p->klist_devices.k_lock);
 
diff --git a/drivers/base/driver.c b/drivers/base/driver.c
index 1e2bda780e48..b76cc69f1106 100644
--- a/drivers/base/driver.c
+++ b/drivers/base/driver.c
@@ -19,7 +19,14 @@
 static struct device *next_device(struct klist_iter *i)
 {
 	struct klist_node *n = klist_next(i);
-	return n ? container_of(n, struct device, knode_driver) : NULL;
+	struct device *dev = NULL;
+	struct device_private *dev_prv;
+
+	if (n) {
+		dev_prv = to_device_private_driver(n);
+		dev = dev_prv->device;
+	}
+	return dev;
 }
 
 /**
@@ -42,7 +49,7 @@ int driver_for_each_device(struct device_driver *drv, struct device *start,
 		return -EINVAL;
 
 	klist_iter_init_node(&drv->p->klist_devices, &i,
-			     start ? &start->knode_driver : NULL);
+			     start ? &start->p->knode_driver : NULL);
 	while ((dev = next_device(&i)) && !error)
 		error = fn(dev, data);
 	klist_iter_exit(&i);
@@ -76,7 +83,7 @@ struct device *driver_find_device(struct device_driver *drv,
 		return NULL;
 
 	klist_iter_init_node(&drv->p->klist_devices, &i,
-			     (start ? &start->knode_driver : NULL));
+			     (start ? &start->p->knode_driver : NULL));
 	while ((dev = next_device(&i)))
 		if (match(dev, data) && get_device(dev))
 			break;
diff --git a/drivers/base/firmware_class.c b/drivers/base/firmware_class.c
index b7e571031ecd..44699d9dd85c 100644
--- a/drivers/base/firmware_class.c
+++ b/drivers/base/firmware_class.c
@@ -291,12 +291,6 @@ firmware_class_timeout(u_long data)
 	fw_load_abort(fw_priv);
 }
 
-static inline void fw_setup_device_id(struct device *f_dev, struct device *dev)
-{
-	/* XXX warning we should watch out for name collisions */
-	strlcpy(f_dev->bus_id, dev->bus_id, BUS_ID_SIZE);
-}
-
 static int fw_register_device(struct device **dev_p, const char *fw_name,
 			      struct device *device)
 {
@@ -321,7 +315,7 @@ static int fw_register_device(struct device **dev_p, const char *fw_name,
 	fw_priv->timeout.data = (u_long) fw_priv;
 	init_timer(&fw_priv->timeout);
 
-	fw_setup_device_id(f_dev, device);
+	dev_set_name(f_dev, dev_name(device));
 	f_dev->parent = device;
 	f_dev->class = &firmware_class;
 	dev_set_drvdata(f_dev, fw_priv);
diff --git a/drivers/base/isa.c b/drivers/base/isa.c
index efd577574948..479694b6cbe3 100644
--- a/drivers/base/isa.c
+++ b/drivers/base/isa.c
@@ -11,7 +11,7 @@
 #include <linux/isa.h>
 
 static struct device isa_bus = {
-	.bus_id		= "isa"
+	.init_name	= "isa"
 };
 
 struct isa_dev {
@@ -135,9 +135,8 @@ int isa_register_driver(struct isa_driver *isa_driver, unsigned int ndev)
 		isa_dev->dev.parent	= &isa_bus;
 		isa_dev->dev.bus	= &isa_bus_type;
 
-		snprintf(isa_dev->dev.bus_id, BUS_ID_SIZE, "%s.%u",
-				isa_driver->driver.name, id);
-
+		dev_set_name(&isa_dev->dev, "%s.%u",
+			     isa_driver->driver.name, id);
 		isa_dev->dev.platform_data	= isa_driver;
 		isa_dev->dev.release		= isa_dev_release;
 		isa_dev->id			= id;
diff --git a/drivers/base/memory.c b/drivers/base/memory.c
index 5260e9e0df48..989429cfed88 100644
--- a/drivers/base/memory.c
+++ b/drivers/base/memory.c
@@ -347,8 +347,9 @@ static inline int memory_probe_init(void)
  * section belongs to...
  */
 
-static int add_memory_block(unsigned long node_id, struct mem_section *section,
-		     unsigned long state, int phys_device)
+static int add_memory_block(int nid, struct mem_section *section,
+			unsigned long state, int phys_device,
+			enum mem_add_context context)
 {
 	struct memory_block *mem = kzalloc(sizeof(*mem), GFP_KERNEL);
 	int ret = 0;
@@ -370,6 +371,10 @@ static int add_memory_block(unsigned long node_id, struct mem_section *section,
 		ret = mem_create_simple_file(mem, phys_device);
 	if (!ret)
 		ret = mem_create_simple_file(mem, removable);
+	if (!ret) {
+		if (context == HOTPLUG)
+			ret = register_mem_sect_under_node(mem, nid);
+	}
 
 	return ret;
 }
@@ -382,7 +387,7 @@ static int add_memory_block(unsigned long node_id, struct mem_section *section,
  *
  * This could be made generic for all sysdev classes.
  */
-static struct memory_block *find_memory_block(struct mem_section *section)
+struct memory_block *find_memory_block(struct mem_section *section)
 {
 	struct kobject *kobj;
 	struct sys_device *sysdev;
@@ -411,6 +416,7 @@ int remove_memory_block(unsigned long node_id, struct mem_section *section,
 	struct memory_block *mem;
 
 	mem = find_memory_block(section);
+	unregister_mem_sect_under_nodes(mem);
 	mem_remove_simple_file(mem, phys_index);
 	mem_remove_simple_file(mem, state);
 	mem_remove_simple_file(mem, phys_device);
@@ -424,9 +430,9 @@ int remove_memory_block(unsigned long node_id, struct mem_section *section,
  * need an interface for the VM to add new memory regions,
  * but without onlining it.
  */
-int register_new_memory(struct mem_section *section)
+int register_new_memory(int nid, struct mem_section *section)
 {
-	return add_memory_block(0, section, MEM_OFFLINE, 0);
+	return add_memory_block(nid, section, MEM_OFFLINE, 0, HOTPLUG);
 }
 
 int unregister_memory_section(struct mem_section *section)
@@ -458,7 +464,8 @@ int __init memory_dev_init(void)
 	for (i = 0; i < NR_MEM_SECTIONS; i++) {
 		if (!present_section_nr(i))
 			continue;
-		err = add_memory_block(0, __nr_to_section(i), MEM_ONLINE, 0);
+		err = add_memory_block(0, __nr_to_section(i), MEM_ONLINE,
+					0, BOOT);
 		if (!ret)
 			ret = err;
 	}
diff --git a/drivers/base/node.c b/drivers/base/node.c
index 91636cd8b6c9..43fa90b837ee 100644
--- a/drivers/base/node.c
+++ b/drivers/base/node.c
@@ -6,6 +6,7 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/mm.h>
+#include <linux/memory.h>
 #include <linux/node.h>
 #include <linux/hugetlb.h>
 #include <linux/cpumask.h>
@@ -248,6 +249,105 @@ int unregister_cpu_under_node(unsigned int cpu, unsigned int nid)
 	return 0;
 }
 
+#ifdef CONFIG_MEMORY_HOTPLUG_SPARSE
+#define page_initialized(page)  (page->lru.next)
+
+static int get_nid_for_pfn(unsigned long pfn)
+{
+	struct page *page;
+
+	if (!pfn_valid_within(pfn))
+		return -1;
+	page = pfn_to_page(pfn);
+	if (!page_initialized(page))
+		return -1;
+	return pfn_to_nid(pfn);
+}
+
+/* register memory section under specified node if it spans that node */
+int register_mem_sect_under_node(struct memory_block *mem_blk, int nid)
+{
+	unsigned long pfn, sect_start_pfn, sect_end_pfn;
+
+	if (!mem_blk)
+		return -EFAULT;
+	if (!node_online(nid))
+		return 0;
+	sect_start_pfn = section_nr_to_pfn(mem_blk->phys_index);
+	sect_end_pfn = sect_start_pfn + PAGES_PER_SECTION - 1;
+	for (pfn = sect_start_pfn; pfn <= sect_end_pfn; pfn++) {
+		int page_nid;
+
+		page_nid = get_nid_for_pfn(pfn);
+		if (page_nid < 0)
+			continue;
+		if (page_nid != nid)
+			continue;
+		return sysfs_create_link_nowarn(&node_devices[nid].sysdev.kobj,
+					&mem_blk->sysdev.kobj,
+					kobject_name(&mem_blk->sysdev.kobj));
+	}
+	/* mem section does not span the specified node */
+	return 0;
+}
+
+/* unregister memory section under all nodes that it spans */
+int unregister_mem_sect_under_nodes(struct memory_block *mem_blk)
+{
+	nodemask_t unlinked_nodes;
+	unsigned long pfn, sect_start_pfn, sect_end_pfn;
+
+	if (!mem_blk)
+		return -EFAULT;
+	nodes_clear(unlinked_nodes);
+	sect_start_pfn = section_nr_to_pfn(mem_blk->phys_index);
+	sect_end_pfn = sect_start_pfn + PAGES_PER_SECTION - 1;
+	for (pfn = sect_start_pfn; pfn <= sect_end_pfn; pfn++) {
+		unsigned int nid;
+
+		nid = get_nid_for_pfn(pfn);
+		if (nid < 0)
+			continue;
+		if (!node_online(nid))
+			continue;
+		if (node_test_and_set(nid, unlinked_nodes))
+			continue;
+		sysfs_remove_link(&node_devices[nid].sysdev.kobj,
+			 kobject_name(&mem_blk->sysdev.kobj));
+	}
+	return 0;
+}
+
+static int link_mem_sections(int nid)
+{
+	unsigned long start_pfn = NODE_DATA(nid)->node_start_pfn;
+	unsigned long end_pfn = start_pfn + NODE_DATA(nid)->node_spanned_pages;
+	unsigned long pfn;
+	int err = 0;
+
+	for (pfn = start_pfn; pfn < end_pfn; pfn += PAGES_PER_SECTION) {
+		unsigned long section_nr = pfn_to_section_nr(pfn);
+		struct mem_section *mem_sect;
+		struct memory_block *mem_blk;
+		int ret;
+
+		if (!present_section_nr(section_nr))
+			continue;
+		mem_sect = __nr_to_section(section_nr);
+		mem_blk = find_memory_block(mem_sect);
+		ret = register_mem_sect_under_node(mem_blk, nid);
+		if (!err)
+			err = ret;
+
+		/* discard ref obtained in find_memory_block() */
+		kobject_put(&mem_blk->sysdev.kobj);
+	}
+	return err;
+}
+#else
+static int link_mem_sections(int nid) { return 0; }
+#endif /* CONFIG_MEMORY_HOTPLUG_SPARSE */
+
 int register_one_node(int nid)
 {
 	int error = 0;
@@ -267,6 +367,9 @@ int register_one_node(int nid)
 			if (cpu_to_node(cpu) == nid)
 				register_cpu_under_node(cpu, nid);
 		}
+
+		/* link memory sections under this node */
+		error = link_mem_sections(nid);
 	}
 
 	return error;
diff --git a/drivers/base/platform.c b/drivers/base/platform.c
index dfcbfe504867..349a1013603f 100644
--- a/drivers/base/platform.c
+++ b/drivers/base/platform.c
@@ -24,7 +24,7 @@
 				 driver))
 
 struct device platform_bus = {
-	.bus_id		= "platform",
+	.init_name	= "platform",
 };
 EXPORT_SYMBOL_GPL(platform_bus);
 
@@ -242,16 +242,15 @@ int platform_device_add(struct platform_device *pdev)
 	pdev->dev.bus = &platform_bus_type;
 
 	if (pdev->id != -1)
-		snprintf(pdev->dev.bus_id, BUS_ID_SIZE, "%s.%d", pdev->name,
-			 pdev->id);
+		dev_set_name(&pdev->dev, "%s.%d", pdev->name,  pdev->id);
 	else
-		strlcpy(pdev->dev.bus_id, pdev->name, BUS_ID_SIZE);
+		dev_set_name(&pdev->dev, pdev->name);
 
 	for (i = 0; i < pdev->num_resources; i++) {
 		struct resource *p, *r = &pdev->resource[i];
 
 		if (r->name == NULL)
-			r->name = pdev->dev.bus_id;
+			r->name = dev_name(&pdev->dev);
 
 		p = r->parent;
 		if (!p) {
@@ -264,14 +263,14 @@ int platform_device_add(struct platform_device *pdev)
 		if (p && insert_resource(p, r)) {
 			printk(KERN_ERR
 			       "%s: failed to claim resource %d\n",
-			       pdev->dev.bus_id, i);
+			       dev_name(&pdev->dev), i);
 			ret = -EBUSY;
 			goto failed;
 		}
 	}
 
 	pr_debug("Registering platform device '%s'. Parent at %s\n",
-		 pdev->dev.bus_id, pdev->dev.parent->bus_id);
+		 dev_name(&pdev->dev), dev_name(pdev->dev.parent));
 
 	ret = device_add(&pdev->dev);
 	if (ret == 0)
@@ -503,8 +502,6 @@ int platform_driver_register(struct platform_driver *drv)
 		drv->driver.suspend = platform_drv_suspend;
 	if (drv->resume)
 		drv->driver.resume = platform_drv_resume;
-	if (drv->pm)
-		drv->driver.pm = &drv->pm->base;
 	return driver_register(&drv->driver);
 }
 EXPORT_SYMBOL_GPL(platform_driver_register);
@@ -609,7 +606,7 @@ static int platform_match(struct device *dev, struct device_driver *drv)
 	struct platform_device *pdev;
 
 	pdev = container_of(dev, struct platform_device, dev);
-	return (strncmp(pdev->name, drv->name, BUS_ID_SIZE) == 0);
+	return (strcmp(pdev->name, drv->name) == 0);
 }
 
 #ifdef CONFIG_PM_SLEEP
@@ -686,7 +683,10 @@ static int platform_pm_suspend(struct device *dev)
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (drv && drv->pm) {
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
 		if (drv->pm->suspend)
 			ret = drv->pm->suspend(dev);
 	} else {
@@ -698,16 +698,15 @@ static int platform_pm_suspend(struct device *dev)
 
 static int platform_pm_suspend_noirq(struct device *dev)
 {
-	struct platform_driver *pdrv;
+	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (!dev->driver)
+	if (!drv)
 		return 0;
 
-	pdrv = to_platform_driver(dev->driver);
-	if (pdrv->pm) {
-		if (pdrv->pm->suspend_noirq)
-			ret = pdrv->pm->suspend_noirq(dev);
+	if (drv->pm) {
+		if (drv->pm->suspend_noirq)
+			ret = drv->pm->suspend_noirq(dev);
 	} else {
 		ret = platform_legacy_suspend_late(dev, PMSG_SUSPEND);
 	}
@@ -720,7 +719,10 @@ static int platform_pm_resume(struct device *dev)
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (drv && drv->pm) {
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
 		if (drv->pm->resume)
 			ret = drv->pm->resume(dev);
 	} else {
@@ -732,16 +734,15 @@ static int platform_pm_resume(struct device *dev)
 
 static int platform_pm_resume_noirq(struct device *dev)
 {
-	struct platform_driver *pdrv;
+	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (!dev->driver)
+	if (!drv)
 		return 0;
 
-	pdrv = to_platform_driver(dev->driver);
-	if (pdrv->pm) {
-		if (pdrv->pm->resume_noirq)
-			ret = pdrv->pm->resume_noirq(dev);
+	if (drv->pm) {
+		if (drv->pm->resume_noirq)
+			ret = drv->pm->resume_noirq(dev);
 	} else {
 		ret = platform_legacy_resume_early(dev);
 	}
@@ -780,16 +781,15 @@ static int platform_pm_freeze(struct device *dev)
 
 static int platform_pm_freeze_noirq(struct device *dev)
 {
-	struct platform_driver *pdrv;
+	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (!dev->driver)
+	if (!drv)
 		return 0;
 
-	pdrv = to_platform_driver(dev->driver);
-	if (pdrv->pm) {
-		if (pdrv->pm->freeze_noirq)
-			ret = pdrv->pm->freeze_noirq(dev);
+	if (drv->pm) {
+		if (drv->pm->freeze_noirq)
+			ret = drv->pm->freeze_noirq(dev);
 	} else {
 		ret = platform_legacy_suspend_late(dev, PMSG_FREEZE);
 	}
@@ -802,7 +802,10 @@ static int platform_pm_thaw(struct device *dev)
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (drv && drv->pm) {
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
 		if (drv->pm->thaw)
 			ret = drv->pm->thaw(dev);
 	} else {
@@ -814,16 +817,15 @@ static int platform_pm_thaw(struct device *dev)
 
 static int platform_pm_thaw_noirq(struct device *dev)
 {
-	struct platform_driver *pdrv;
+	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (!dev->driver)
+	if (!drv)
 		return 0;
 
-	pdrv = to_platform_driver(dev->driver);
-	if (pdrv->pm) {
-		if (pdrv->pm->thaw_noirq)
-			ret = pdrv->pm->thaw_noirq(dev);
+	if (drv->pm) {
+		if (drv->pm->thaw_noirq)
+			ret = drv->pm->thaw_noirq(dev);
 	} else {
 		ret = platform_legacy_resume_early(dev);
 	}
@@ -836,7 +838,10 @@ static int platform_pm_poweroff(struct device *dev)
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (drv && drv->pm) {
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
 		if (drv->pm->poweroff)
 			ret = drv->pm->poweroff(dev);
 	} else {
@@ -848,16 +853,15 @@ static int platform_pm_poweroff(struct device *dev)
 
 static int platform_pm_poweroff_noirq(struct device *dev)
 {
-	struct platform_driver *pdrv;
+	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (!dev->driver)
+	if (!drv)
 		return 0;
 
-	pdrv = to_platform_driver(dev->driver);
-	if (pdrv->pm) {
-		if (pdrv->pm->poweroff_noirq)
-			ret = pdrv->pm->poweroff_noirq(dev);
+	if (drv->pm) {
+		if (drv->pm->poweroff_noirq)
+			ret = drv->pm->poweroff_noirq(dev);
 	} else {
 		ret = platform_legacy_suspend_late(dev, PMSG_HIBERNATE);
 	}
@@ -870,7 +874,10 @@ static int platform_pm_restore(struct device *dev)
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (drv && drv->pm) {
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
 		if (drv->pm->restore)
 			ret = drv->pm->restore(dev);
 	} else {
@@ -882,16 +889,15 @@ static int platform_pm_restore(struct device *dev)
 
 static int platform_pm_restore_noirq(struct device *dev)
 {
-	struct platform_driver *pdrv;
+	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (!dev->driver)
+	if (!drv)
 		return 0;
 
-	pdrv = to_platform_driver(dev->driver);
-	if (pdrv->pm) {
-		if (pdrv->pm->restore_noirq)
-			ret = pdrv->pm->restore_noirq(dev);
+	if (drv->pm) {
+		if (drv->pm->restore_noirq)
+			ret = drv->pm->restore_noirq(dev);
 	} else {
 		ret = platform_legacy_resume_early(dev);
 	}
@@ -912,17 +918,15 @@ static int platform_pm_restore_noirq(struct device *dev)
 
 #endif /* !CONFIG_HIBERNATION */
 
-static struct pm_ext_ops platform_pm_ops = {
-	.base = {
-		.prepare = platform_pm_prepare,
-		.complete = platform_pm_complete,
-		.suspend = platform_pm_suspend,
-		.resume = platform_pm_resume,
-		.freeze = platform_pm_freeze,
-		.thaw = platform_pm_thaw,
-		.poweroff = platform_pm_poweroff,
-		.restore = platform_pm_restore,
-	},
+static struct dev_pm_ops platform_dev_pm_ops = {
+	.prepare = platform_pm_prepare,
+	.complete = platform_pm_complete,
+	.suspend = platform_pm_suspend,
+	.resume = platform_pm_resume,
+	.freeze = platform_pm_freeze,
+	.thaw = platform_pm_thaw,
+	.poweroff = platform_pm_poweroff,
+	.restore = platform_pm_restore,
 	.suspend_noirq = platform_pm_suspend_noirq,
 	.resume_noirq = platform_pm_resume_noirq,
 	.freeze_noirq = platform_pm_freeze_noirq,
@@ -931,7 +935,7 @@ static struct pm_ext_ops platform_pm_ops = {
 	.restore_noirq = platform_pm_restore_noirq,
 };
 
-#define PLATFORM_PM_OPS_PTR	&platform_pm_ops
+#define PLATFORM_PM_OPS_PTR	(&platform_dev_pm_ops)
 
 #else /* !CONFIG_PM_SLEEP */
 
diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index 692c20ba5144..670c9d6c1407 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -76,7 +76,7 @@ void device_pm_add(struct device *dev)
 	if (dev->parent) {
 		if (dev->parent->power.status >= DPM_SUSPENDING)
 			dev_warn(dev, "parent %s should not be sleeping\n",
-				dev->parent->bus_id);
+				 dev_name(dev->parent));
 	} else if (transition_started) {
 		/*
 		 * We refuse to register parentless devices while a PM
@@ -112,7 +112,8 @@ void device_pm_remove(struct device *dev)
  *	@ops:	PM operations to choose from.
  *	@state:	PM transition of the system being carried out.
  */
-static int pm_op(struct device *dev, struct pm_ops *ops, pm_message_t state)
+static int pm_op(struct device *dev, struct dev_pm_ops *ops,
+			pm_message_t state)
 {
 	int error = 0;
 
@@ -174,7 +175,7 @@ static int pm_op(struct device *dev, struct pm_ops *ops, pm_message_t state)
  *	The operation is executed with interrupts disabled by the only remaining
  *	functional CPU in the system.
  */
-static int pm_noirq_op(struct device *dev, struct pm_ext_ops *ops,
+static int pm_noirq_op(struct device *dev, struct dev_pm_ops *ops,
 			pm_message_t state)
 {
 	int error = 0;
@@ -354,7 +355,7 @@ static int resume_device(struct device *dev, pm_message_t state)
 	if (dev->bus) {
 		if (dev->bus->pm) {
 			pm_dev_dbg(dev, state, "");
-			error = pm_op(dev, &dev->bus->pm->base, state);
+			error = pm_op(dev, dev->bus->pm, state);
 		} else if (dev->bus->resume) {
 			pm_dev_dbg(dev, state, "legacy ");
 			error = dev->bus->resume(dev);
@@ -451,9 +452,9 @@ static void complete_device(struct device *dev, pm_message_t state)
 		dev->type->pm->complete(dev);
 	}
 
-	if (dev->bus && dev->bus->pm && dev->bus->pm->base.complete) {
+	if (dev->bus && dev->bus->pm && dev->bus->pm->complete) {
 		pm_dev_dbg(dev, state, "completing ");
-		dev->bus->pm->base.complete(dev);
+		dev->bus->pm->complete(dev);
 	}
 
 	up(&dev->sem);
@@ -624,7 +625,7 @@ static int suspend_device(struct device *dev, pm_message_t state)
 	if (dev->bus) {
 		if (dev->bus->pm) {
 			pm_dev_dbg(dev, state, "");
-			error = pm_op(dev, &dev->bus->pm->base, state);
+			error = pm_op(dev, dev->bus->pm, state);
 		} else if (dev->bus->suspend) {
 			pm_dev_dbg(dev, state, "legacy ");
 			error = dev->bus->suspend(dev, state);
@@ -685,10 +686,10 @@ static int prepare_device(struct device *dev, pm_message_t state)
 
 	down(&dev->sem);
 
-	if (dev->bus && dev->bus->pm && dev->bus->pm->base.prepare) {
+	if (dev->bus && dev->bus->pm && dev->bus->pm->prepare) {
 		pm_dev_dbg(dev, state, "preparing ");
-		error = dev->bus->pm->base.prepare(dev);
-		suspend_report_result(dev->bus->pm->base.prepare, error);
+		error = dev->bus->pm->prepare(dev);
+		suspend_report_result(dev->bus->pm->prepare, error);
 		if (error)
 			goto End;
 	}
diff --git a/drivers/base/power/trace.c b/drivers/base/power/trace.c
index 2aa6e8fc4def..0a1a2c4dbc6e 100644
--- a/drivers/base/power/trace.c
+++ b/drivers/base/power/trace.c
@@ -140,7 +140,7 @@ static unsigned int hash_string(unsigned int seed, const char *data, unsigned in
 
 void set_trace_device(struct device *dev)
 {
-	dev_hash_value = hash_string(DEVSEED, dev->bus_id, DEVHASH);
+	dev_hash_value = hash_string(DEVSEED, dev_name(dev), DEVHASH);
 }
 EXPORT_SYMBOL(set_trace_device);
 
@@ -192,7 +192,7 @@ static int show_dev_hash(unsigned int value)
 
 	while (entry != &dpm_list) {
 		struct device * dev = to_device(entry);
-		unsigned int hash = hash_string(DEVSEED, dev->bus_id, DEVHASH);
+		unsigned int hash = hash_string(DEVSEED, dev_name(dev), DEVHASH);
 		if (hash == value) {
 			dev_info(dev, "hash matches\n");
 			match++;
diff --git a/drivers/base/topology.c b/drivers/base/topology.c
index a8bc1cbcfa7c..a778fb52b11f 100644
--- a/drivers/base/topology.c
+++ b/drivers/base/topology.c
@@ -28,6 +28,7 @@
 #include <linux/mm.h>
 #include <linux/cpu.h>
 #include <linux/module.h>
+#include <linux/hardirq.h>
 #include <linux/topology.h>
 
 #define define_one_ro(_name) 		\
diff --git a/drivers/block/ub.c b/drivers/block/ub.c
index 048d71d244d7..12fb816db7b0 100644
--- a/drivers/block/ub.c
+++ b/drivers/block/ub.c
@@ -1579,7 +1579,7 @@ static void ub_reset_task(struct work_struct *work)
 	struct ub_dev *sc = container_of(work, struct ub_dev, reset_work);
 	unsigned long flags;
 	struct ub_lun *lun;
-	int lkr, rc;
+	int rc;
 
 	if (!sc->reset) {
 		printk(KERN_WARNING "%s: Running reset unrequested\n",
@@ -1597,10 +1597,11 @@ static void ub_reset_task(struct work_struct *work)
 	} else if (sc->dev->actconfig->desc.bNumInterfaces != 1) {
 		;
 	} else {
-		if ((lkr = usb_lock_device_for_reset(sc->dev, sc->intf)) < 0) {
+		rc = usb_lock_device_for_reset(sc->dev, sc->intf);
+		if (rc < 0) {
 			printk(KERN_NOTICE
 			    "%s: usb_lock_device_for_reset failed (%d)\n",
-			    sc->name, lkr);
+			    sc->name, rc);
 		} else {
 			rc = usb_reset_device(sc->dev);
 			if (rc < 0) {
@@ -1608,9 +1609,7 @@ static void ub_reset_task(struct work_struct *work)
 				    "usb_lock_device_for_reset failed (%d)\n",
 				    sc->name, rc);
 			}
-
-			if (lkr)
-				usb_unlock_device(sc->dev);
+			usb_unlock_device(sc->dev);
 		}
 	}
 
diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index 1697043119bd..35914b6e1d2a 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -841,7 +841,7 @@ config JS_RTC
 
 config GEN_RTC
 	tristate "Generic /dev/rtc emulation"
-	depends on RTC!=y && !IA64 && !ARM && !M32R && !MIPS && !SPARC && !FRV && !S390 && !SUPERH && !AVR32
+	depends on RTC!=y && !IA64 && !ARM && !M32R && !MIPS && !SPARC && !FRV && !S390 && !SUPERH && !AVR32 && !BLACKFIN
 	---help---
 	  If you say Y here and create a character special file /dev/rtc with
 	  major number 10 and minor number 135 using mknod ("man mknod"), you
diff --git a/drivers/char/consolemap.c b/drivers/char/consolemap.c
index 4246b8e36cb3..45d3e80156d4 100644
--- a/drivers/char/consolemap.c
+++ b/drivers/char/consolemap.c
@@ -554,7 +554,7 @@ int con_set_unimap(struct vc_data *vc, ushort ct, struct unipair __user *list)
 		__get_user(fontpos, &list->fontpos);
 		if ((err1 = con_insert_unipair(p, unicode,fontpos)) != 0)
 			err = err1;
-			list++;
+		list++;
 	}
 	
 	if (con_unify_unimap(vc, p))
diff --git a/drivers/char/epca.c b/drivers/char/epca.c
index 39ad820b2350..af7c13ca9493 100644
--- a/drivers/char/epca.c
+++ b/drivers/char/epca.c
@@ -769,7 +769,7 @@ static int pc_open(struct tty_struct *tty, struct file *filp)
 	/* Check status of board configured in system.  */
 
 	/*
-	 * I check to see if the epca_setup routine detected an user error. It
+	 * I check to see if the epca_setup routine detected a user error. It
 	 * might be better to put this in pc_init, but for the moment it goes
 	 * here.
 	 */
diff --git a/drivers/char/mem.c b/drivers/char/mem.c
index 6431f6921a67..3586b3b3df3f 100644
--- a/drivers/char/mem.c
+++ b/drivers/char/mem.c
@@ -425,9 +425,6 @@ static ssize_t read_oldmem(struct file *file, char __user *buf,
 }
 #endif
 
-extern long vread(char *buf, char *addr, unsigned long count);
-extern long vwrite(char *buf, char *addr, unsigned long count);
-
 #ifdef CONFIG_DEVKMEM
 /*
  * This function reads the *virtual* memory as seen by the kernel.
diff --git a/drivers/char/mwave/mwavedd.c b/drivers/char/mwave/mwavedd.c
index 4f8d67fed292..94ad2c3bfc4a 100644
--- a/drivers/char/mwave/mwavedd.c
+++ b/drivers/char/mwave/mwavedd.c
@@ -663,7 +663,7 @@ static int __init mwave_init(void)
 #if 0
 	/* sysfs */
 	memset(&mwave_device, 0, sizeof (struct device));
-	snprintf(mwave_device.bus_id, BUS_ID_SIZE, "mwave");
+	dev_set_name(&mwave_device, "mwave");
 
 	if (device_register(&mwave_device))
 		goto cleanup_error;
diff --git a/drivers/char/random.c b/drivers/char/random.c
index c7afc068c28d..7c13581ca9cd 100644
--- a/drivers/char/random.c
+++ b/drivers/char/random.c
@@ -407,7 +407,7 @@ struct entropy_store {
 	/* read-write data: */
 	spinlock_t lock;
 	unsigned add_ptr;
-	int entropy_count;	/* Must at no time exceed ->POOLBITS! */
+	int entropy_count;
 	int input_rotate;
 };
 
@@ -767,11 +767,10 @@ static size_t account(struct entropy_store *r, size_t nbytes, int min,
 {
 	unsigned long flags;
 
-	BUG_ON(r->entropy_count > r->poolinfo->POOLBITS);
-
 	/* Hold lock while accounting */
 	spin_lock_irqsave(&r->lock, flags);
 
+	BUG_ON(r->entropy_count > r->poolinfo->POOLBITS);
 	DEBUG_ENT("trying to extract %d bits from %s\n",
 		  nbytes * 8, r->name);
 
diff --git a/drivers/char/sysrq.c b/drivers/char/sysrq.c
index 94966edfb44d..d41b9f6f7903 100644
--- a/drivers/char/sysrq.c
+++ b/drivers/char/sysrq.c
@@ -82,7 +82,7 @@ static void sysrq_handle_loglevel(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_loglevel_op = {
 	.handler	= sysrq_handle_loglevel,
-	.help_msg	= "loglevel0-8",
+	.help_msg	= "loglevel(0-9)",
 	.action_msg	= "Changing Loglevel",
 	.enable_mask	= SYSRQ_ENABLE_LOG,
 };
@@ -233,7 +233,7 @@ static void sysrq_handle_showallcpus(int key, struct tty_struct *tty)
 
 static struct sysrq_key_op sysrq_showallcpus_op = {
 	.handler	= sysrq_handle_showallcpus,
-	.help_msg	= "aLlcpus",
+	.help_msg	= "show-backtrace-all-active-cpus(L)",
 	.action_msg	= "Show backtrace of all active CPUs",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -247,7 +247,7 @@ static void sysrq_handle_showregs(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_showregs_op = {
 	.handler	= sysrq_handle_showregs,
-	.help_msg	= "showPc",
+	.help_msg	= "show-registers(P)",
 	.action_msg	= "Show Regs",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -258,7 +258,7 @@ static void sysrq_handle_showstate(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_showstate_op = {
 	.handler	= sysrq_handle_showstate,
-	.help_msg	= "showTasks",
+	.help_msg	= "show-task-states(T)",
 	.action_msg	= "Show State",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -269,7 +269,7 @@ static void sysrq_handle_showstate_blocked(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_showstate_blocked_op = {
 	.handler	= sysrq_handle_showstate_blocked,
-	.help_msg	= "shoW-blocked-tasks",
+	.help_msg	= "show-blocked-tasks(W)",
 	.action_msg	= "Show Blocked State",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -297,7 +297,7 @@ static void sysrq_handle_showmem(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_showmem_op = {
 	.handler	= sysrq_handle_showmem,
-	.help_msg	= "showMem",
+	.help_msg	= "show-memory-usage(M)",
 	.action_msg	= "Show Memory",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -323,7 +323,7 @@ static void sysrq_handle_term(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_term_op = {
 	.handler	= sysrq_handle_term,
-	.help_msg	= "tErm",
+	.help_msg	= "terminate-all-tasks(E)",
 	.action_msg	= "Terminate All Tasks",
 	.enable_mask	= SYSRQ_ENABLE_SIGNAL,
 };
@@ -341,7 +341,7 @@ static void sysrq_handle_moom(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_moom_op = {
 	.handler	= sysrq_handle_moom,
-	.help_msg	= "Full",
+	.help_msg	= "memory-full-oom-kill(F)",
 	.action_msg	= "Manual OOM execution",
 	.enable_mask	= SYSRQ_ENABLE_SIGNAL,
 };
@@ -353,7 +353,7 @@ static void sysrq_handle_kill(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_kill_op = {
 	.handler	= sysrq_handle_kill,
-	.help_msg	= "kIll",
+	.help_msg	= "kill-all-tasks(I)",
 	.action_msg	= "Kill All Tasks",
 	.enable_mask	= SYSRQ_ENABLE_SIGNAL,
 };
@@ -364,7 +364,7 @@ static void sysrq_handle_unrt(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_unrt_op = {
 	.handler	= sysrq_handle_unrt,
-	.help_msg	= "Nice",
+	.help_msg	= "nice-all-RT-tasks(N)",
 	.action_msg	= "Nice All RT Tasks",
 	.enable_mask	= SYSRQ_ENABLE_RTNICE,
 };
diff --git a/drivers/cpufreq/Kconfig b/drivers/cpufreq/Kconfig
index 5f076aef74fa..a8c8d9c19d74 100644
--- a/drivers/cpufreq/Kconfig
+++ b/drivers/cpufreq/Kconfig
@@ -83,7 +83,7 @@ config CPU_FREQ_DEFAULT_GOV_USERSPACE
 	select CPU_FREQ_GOV_USERSPACE
 	help
 	  Use the CPUFreq governor 'userspace' as default. This allows
-	  you to set the CPU frequency manually or when an userspace 
+	  you to set the CPU frequency manually or when a userspace 
 	  program shall be able to set the CPU dynamically without having
 	  to enable the userspace governor manually.
 
@@ -138,7 +138,7 @@ config CPU_FREQ_GOV_USERSPACE
 	tristate "'userspace' governor for userspace frequency scaling"
 	help
 	  Enable this cpufreq governor when you either want to set the
-	  CPU frequency manually or when an userspace program shall
+	  CPU frequency manually or when a userspace program shall
 	  be able to set the CPU dynamically, like on LART 
 	  <http://www.lartmaker.nl/>.
 
diff --git a/drivers/edac/Kconfig b/drivers/edac/Kconfig
index e2667a8c2997..eee47fd16d79 100644
--- a/drivers/edac/Kconfig
+++ b/drivers/edac/Kconfig
@@ -109,6 +109,13 @@ config EDAC_X38
 	  Support for error detection and correction on the Intel
 	  X38 server chipsets.
 
+config EDAC_I5400
+	tristate "Intel 5400 (Seaburg) chipsets"
+	depends on EDAC_MM_EDAC && PCI && X86
+	help
+	  Support for error detection and correction the Intel
+	  i5400 MCH chipset (Seaburg).
+
 config EDAC_I82860
 	tristate "Intel 82860"
 	depends on EDAC_MM_EDAC && PCI && X86_32
diff --git a/drivers/edac/Makefile b/drivers/edac/Makefile
index 62c2d9bad8dc..b75196927de3 100644
--- a/drivers/edac/Makefile
+++ b/drivers/edac/Makefile
@@ -20,6 +20,7 @@ endif
 obj-$(CONFIG_EDAC_AMD76X)		+= amd76x_edac.o
 obj-$(CONFIG_EDAC_I5000)		+= i5000_edac.o
 obj-$(CONFIG_EDAC_I5100)		+= i5100_edac.o
+obj-$(CONFIG_EDAC_I5400)		+= i5400_edac.o
 obj-$(CONFIG_EDAC_E7XXX)		+= e7xxx_edac.o
 obj-$(CONFIG_EDAC_E752X)		+= e752x_edac.o
 obj-$(CONFIG_EDAC_I82443BXGX)		+= i82443bxgx_edac.o
diff --git a/drivers/edac/edac_device.c b/drivers/edac/edac_device.c
index 4041e9143283..ca9113e1c106 100644
--- a/drivers/edac/edac_device.c
+++ b/drivers/edac/edac_device.c
@@ -333,7 +333,7 @@ static int add_edac_dev_to_global_list(struct edac_device_ctl_info *edac_dev)
 fail0:
 	edac_printk(KERN_WARNING, EDAC_MC,
 			"%s (%s) %s %s already assigned %d\n",
-			rover->dev->bus_id, edac_dev_name(rover),
+			dev_name(rover->dev), edac_dev_name(rover),
 			rover->mod_name, rover->ctl_name, rover->dev_idx);
 	return 1;
 
diff --git a/drivers/edac/edac_mc.c b/drivers/edac/edac_mc.c
index d110392d48f4..25d66940b4fa 100644
--- a/drivers/edac/edac_mc.c
+++ b/drivers/edac/edac_mc.c
@@ -401,7 +401,7 @@ static int add_mc_to_global_list(struct mem_ctl_info *mci)
 
 fail0:
 	edac_printk(KERN_WARNING, EDAC_MC,
-		"%s (%s) %s %s already assigned %d\n", p->dev->bus_id,
+		"%s (%s) %s %s already assigned %d\n", dev_name(p->dev),
 		edac_dev_name(mci), p->mod_name, p->ctl_name, p->mc_idx);
 	return 1;
 
diff --git a/drivers/edac/edac_pci.c b/drivers/edac/edac_pci.c
index 22ec9d5d4312..5d3c8083a40e 100644
--- a/drivers/edac/edac_pci.c
+++ b/drivers/edac/edac_pci.c
@@ -150,7 +150,7 @@ static int add_edac_pci_to_global_list(struct edac_pci_ctl_info *pci)
 fail0:
 	edac_printk(KERN_WARNING, EDAC_PCI,
 		"%s (%s) %s %s already assigned %d\n",
-		rover->dev->bus_id, edac_dev_name(rover),
+		dev_name(rover->dev), edac_dev_name(rover),
 		rover->mod_name, rover->ctl_name, rover->pci_idx);
 	return 1;
 
diff --git a/drivers/edac/edac_pci_sysfs.c b/drivers/edac/edac_pci_sysfs.c
index 5c153dccc95e..422728cfe994 100644
--- a/drivers/edac/edac_pci_sysfs.c
+++ b/drivers/edac/edac_pci_sysfs.c
@@ -569,7 +569,7 @@ static void edac_pci_dev_parity_test(struct pci_dev *dev)
 
 	local_irq_restore(flags);
 
-	debugf4("PCI STATUS= 0x%04x %s\n", status, dev->dev.bus_id);
+	debugf4("PCI STATUS= 0x%04x %s\n", status, dev_name(&dev->dev));
 
 	/* check the status reg for errors on boards NOT marked as broken
 	 * if broken, we cannot trust any of the status bits
@@ -600,13 +600,13 @@ static void edac_pci_dev_parity_test(struct pci_dev *dev)
 	}
 
 
-	debugf4("PCI HEADER TYPE= 0x%02x %s\n", header_type, dev->dev.bus_id);
+	debugf4("PCI HEADER TYPE= 0x%02x %s\n", header_type, dev_name(&dev->dev));
 
 	if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {
 		/* On bridges, need to examine secondary status register  */
 		status = get_pci_parity_status(dev, 1);
 
-		debugf4("PCI SEC_STATUS= 0x%04x %s\n", status, dev->dev.bus_id);
+		debugf4("PCI SEC_STATUS= 0x%04x %s\n", status, dev_name(&dev->dev));
 
 		/* check the secondary status reg for errors,
 		 * on NOT broken boards
diff --git a/drivers/edac/i5400_edac.c b/drivers/edac/i5400_edac.c
new file mode 100644
index 000000000000..b08b6d8e2dc7
--- /dev/null
+++ b/drivers/edac/i5400_edac.c
@@ -0,0 +1,1476 @@
+/*
+ * Intel 5400 class Memory Controllers kernel module (Seaburg)
+ *
+ * This file may be distributed under the terms of the
+ * GNU General Public License.
+ *
+ * Copyright (c) 2008 by:
+ *	 Ben Woodard <woodard@redhat.com>
+ *	 Mauro Carvalho Chehab <mchehab@redhat.com>
+ *
+ * Red Hat Inc. http://www.redhat.com
+ *
+ * Forked and adapted from the i5000_edac driver which was
+ * written by Douglas Thompson Linux Networx <norsk5@xmission.com>
+ *
+ * This module is based on the following document:
+ *
+ * Intel 5400 Chipset Memory Controller Hub (MCH) - Datasheet
+ * 	http://developer.intel.com/design/chipsets/datashts/313070.htm
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/pci_ids.h>
+#include <linux/slab.h>
+#include <linux/edac.h>
+#include <linux/mmzone.h>
+
+#include "edac_core.h"
+
+/*
+ * Alter this version for the I5400 module when modifications are made
+ */
+#define I5400_REVISION    " Ver: 1.0.0 " __DATE__
+
+#define EDAC_MOD_STR      "i5400_edac"
+
+#define i5400_printk(level, fmt, arg...) \
+	edac_printk(level, "i5400", fmt, ##arg)
+
+#define i5400_mc_printk(mci, level, fmt, arg...) \
+	edac_mc_chipset_printk(mci, level, "i5400", fmt, ##arg)
+
+/* Limits for i5400 */
+#define NUM_MTRS_PER_BRANCH	4
+#define CHANNELS_PER_BRANCH	2
+#define	MAX_CHANNELS		4
+#define MAX_DIMMS		(MAX_CHANNELS * 4)	/* Up to 4 DIMM's per channel */
+#define MAX_CSROWS		(MAX_DIMMS * 2)		/* max possible csrows per channel */
+
+/* Device 16,
+ * Function 0: System Address
+ * Function 1: Memory Branch Map, Control, Errors Register
+ * Function 2: FSB Error Registers
+ *
+ * All 3 functions of Device 16 (0,1,2) share the SAME DID and
+ * uses PCI_DEVICE_ID_INTEL_5400_ERR for device 16 (0,1,2),
+ * PCI_DEVICE_ID_INTEL_5400_FBD0 and PCI_DEVICE_ID_INTEL_5400_FBD1
+ * for device 21 (0,1).
+ */
+
+	/* OFFSETS for Function 0 */
+#define		AMBASE			0x48 /* AMB Mem Mapped Reg Region Base */
+#define		MAXCH			0x56 /* Max Channel Number */
+#define		MAXDIMMPERCH		0x57 /* Max DIMM PER Channel Number */
+
+	/* OFFSETS for Function 1 */
+#define		TOLM			0x6C
+#define		REDMEMB			0x7C
+#define			REC_ECC_LOCATOR_ODD(x)	((x) & 0x3fe00) /* bits [17:9] indicate ODD, [8:0]  indicate EVEN */
+#define		MIR0			0x80
+#define		MIR1			0x84
+#define		AMIR0			0x8c
+#define		AMIR1			0x90
+
+	/* Fatal error registers */
+#define		FERR_FAT_FBD		0x98	/* also called as FERR_FAT_FB_DIMM at datasheet */
+#define			FERR_FAT_FBDCHAN (3<<28)	/* channel index where the highest-order error occurred */
+
+#define		NERR_FAT_FBD		0x9c
+#define		FERR_NF_FBD		0xa0	/* also called as FERR_NFAT_FB_DIMM at datasheet */
+
+	/* Non-fatal error register */
+#define		NERR_NF_FBD		0xa4
+
+	/* Enable error mask */
+#define		EMASK_FBD		0xa8
+
+#define		ERR0_FBD		0xac
+#define		ERR1_FBD		0xb0
+#define		ERR2_FBD		0xb4
+#define		MCERR_FBD		0xb8
+
+	/* No OFFSETS for Device 16 Function 2 */
+
+/*
+ * Device 21,
+ * Function 0: Memory Map Branch 0
+ *
+ * Device 22,
+ * Function 0: Memory Map Branch 1
+ */
+
+	/* OFFSETS for Function 0 */
+#define AMBPRESENT_0	0x64
+#define AMBPRESENT_1	0x66
+#define MTR0		0x80
+#define MTR1		0x82
+#define MTR2		0x84
+#define MTR3		0x86
+
+	/* OFFSETS for Function 1 */
+#define NRECFGLOG		0x74
+#define RECFGLOG		0x78
+#define NRECMEMA		0xbe
+#define NRECMEMB		0xc0
+#define NRECFB_DIMMA		0xc4
+#define NRECFB_DIMMB		0xc8
+#define NRECFB_DIMMC		0xcc
+#define NRECFB_DIMMD		0xd0
+#define NRECFB_DIMME		0xd4
+#define NRECFB_DIMMF		0xd8
+#define REDMEMA			0xdC
+#define RECMEMA			0xf0
+#define RECMEMB			0xf4
+#define RECFB_DIMMA		0xf8
+#define RECFB_DIMMB		0xec
+#define RECFB_DIMMC		0xf0
+#define RECFB_DIMMD		0xf4
+#define RECFB_DIMME		0xf8
+#define RECFB_DIMMF		0xfC
+
+/*
+ * Error indicator bits and masks
+ * Error masks are according with Table 5-17 of i5400 datasheet
+ */
+
+enum error_mask {
+	EMASK_M1  = 1<<0,  /* Memory Write error on non-redundant retry */
+	EMASK_M2  = 1<<1,  /* Memory or FB-DIMM configuration CRC read error */
+	EMASK_M3  = 1<<2,  /* Reserved */
+	EMASK_M4  = 1<<3,  /* Uncorrectable Data ECC on Replay */
+	EMASK_M5  = 1<<4,  /* Aliased Uncorrectable Non-Mirrored Demand Data ECC */
+	EMASK_M6  = 1<<5,  /* Unsupported on i5400 */
+	EMASK_M7  = 1<<6,  /* Aliased Uncorrectable Resilver- or Spare-Copy Data ECC */
+	EMASK_M8  = 1<<7,  /* Aliased Uncorrectable Patrol Data ECC */
+	EMASK_M9  = 1<<8,  /* Non-Aliased Uncorrectable Non-Mirrored Demand Data ECC */
+	EMASK_M10 = 1<<9,  /* Unsupported on i5400 */
+	EMASK_M11 = 1<<10, /* Non-Aliased Uncorrectable Resilver- or Spare-Copy Data ECC  */
+	EMASK_M12 = 1<<11, /* Non-Aliased Uncorrectable Patrol Data ECC */
+	EMASK_M13 = 1<<12, /* Memory Write error on first attempt */
+	EMASK_M14 = 1<<13, /* FB-DIMM Configuration Write error on first attempt */
+	EMASK_M15 = 1<<14, /* Memory or FB-DIMM configuration CRC read error */
+	EMASK_M16 = 1<<15, /* Channel Failed-Over Occurred */
+	EMASK_M17 = 1<<16, /* Correctable Non-Mirrored Demand Data ECC */
+	EMASK_M18 = 1<<17, /* Unsupported on i5400 */
+	EMASK_M19 = 1<<18, /* Correctable Resilver- or Spare-Copy Data ECC */
+	EMASK_M20 = 1<<19, /* Correctable Patrol Data ECC */
+	EMASK_M21 = 1<<20, /* FB-DIMM Northbound parity error on FB-DIMM Sync Status */
+	EMASK_M22 = 1<<21, /* SPD protocol Error */
+	EMASK_M23 = 1<<22, /* Non-Redundant Fast Reset Timeout */
+	EMASK_M24 = 1<<23, /* Refresh error */
+	EMASK_M25 = 1<<24, /* Memory Write error on redundant retry */
+	EMASK_M26 = 1<<25, /* Redundant Fast Reset Timeout */
+	EMASK_M27 = 1<<26, /* Correctable Counter Threshold Exceeded */
+	EMASK_M28 = 1<<27, /* DIMM-Spare Copy Completed */
+	EMASK_M29 = 1<<28, /* DIMM-Isolation Completed */
+};
+
+/*
+ * Names to translate bit error into something useful
+ */
+static const char *error_name[] = {
+	[0]  = "Memory Write error on non-redundant retry",
+	[1]  = "Memory or FB-DIMM configuration CRC read error",
+	/* Reserved */
+	[3]  = "Uncorrectable Data ECC on Replay",
+	[4]  = "Aliased Uncorrectable Non-Mirrored Demand Data ECC",
+	/* M6 Unsupported on i5400 */
+	[6]  = "Aliased Uncorrectable Resilver- or Spare-Copy Data ECC",
+	[7]  = "Aliased Uncorrectable Patrol Data ECC",
+	[8]  = "Non-Aliased Uncorrectable Non-Mirrored Demand Data ECC",
+	/* M10 Unsupported on i5400 */
+	[10] = "Non-Aliased Uncorrectable Resilver- or Spare-Copy Data ECC",
+	[11] = "Non-Aliased Uncorrectable Patrol Data ECC",
+	[12] = "Memory Write error on first attempt",
+	[13] = "FB-DIMM Configuration Write error on first attempt",
+	[14] = "Memory or FB-DIMM configuration CRC read error",
+	[15] = "Channel Failed-Over Occurred",
+	[16] = "Correctable Non-Mirrored Demand Data ECC",
+	/* M18 Unsupported on i5400 */
+	[18] = "Correctable Resilver- or Spare-Copy Data ECC",
+	[19] = "Correctable Patrol Data ECC",
+	[20] = "FB-DIMM Northbound parity error on FB-DIMM Sync Status",
+	[21] = "SPD protocol Error",
+	[22] = "Non-Redundant Fast Reset Timeout",
+	[23] = "Refresh error",
+	[24] = "Memory Write error on redundant retry",
+	[25] = "Redundant Fast Reset Timeout",
+	[26] = "Correctable Counter Threshold Exceeded",
+	[27] = "DIMM-Spare Copy Completed",
+	[28] = "DIMM-Isolation Completed",
+};
+
+/* Fatal errors */
+#define ERROR_FAT_MASK		(EMASK_M1 | \
+				 EMASK_M2 | \
+				 EMASK_M23)
+
+/* Correctable errors */
+#define ERROR_NF_CORRECTABLE	(EMASK_M27 | \
+				 EMASK_M20 | \
+				 EMASK_M19 | \
+				 EMASK_M18 | \
+				 EMASK_M17 | \
+				 EMASK_M16)
+#define ERROR_NF_DIMM_SPARE	(EMASK_M29 | \
+				 EMASK_M28)
+#define ERROR_NF_SPD_PROTOCOL	(EMASK_M22)
+#define ERROR_NF_NORTH_CRC	(EMASK_M21)
+
+/* Recoverable errors */
+#define ERROR_NF_RECOVERABLE	(EMASK_M26 | \
+				 EMASK_M25 | \
+				 EMASK_M24 | \
+				 EMASK_M15 | \
+				 EMASK_M14 | \
+				 EMASK_M13 | \
+				 EMASK_M12 | \
+				 EMASK_M11 | \
+				 EMASK_M9  | \
+				 EMASK_M8  | \
+				 EMASK_M7  | \
+				 EMASK_M5)
+
+/* uncorrectable errors */
+#define ERROR_NF_UNCORRECTABLE	(EMASK_M4)
+
+/* mask to all non-fatal errors */
+#define ERROR_NF_MASK		(ERROR_NF_CORRECTABLE   | \
+				 ERROR_NF_UNCORRECTABLE | \
+				 ERROR_NF_RECOVERABLE   | \
+				 ERROR_NF_DIMM_SPARE    | \
+				 ERROR_NF_SPD_PROTOCOL  | \
+				 ERROR_NF_NORTH_CRC)
+
+/*
+ * Define error masks for the several registers
+ */
+
+/* Enable all fatal and non fatal errors */
+#define ENABLE_EMASK_ALL	(ERROR_FAT_MASK | ERROR_NF_MASK)
+
+/* mask for fatal error registers */
+#define FERR_FAT_MASK ERROR_FAT_MASK
+
+/* masks for non-fatal error register */
+static inline int to_nf_mask(unsigned int mask)
+{
+	return (mask & EMASK_M29) | (mask >> 3);
+};
+
+static inline int from_nf_ferr(unsigned int mask)
+{
+	return (mask & EMASK_M29) |		/* Bit 28 */
+	       (mask & ((1 << 28) - 1) << 3);	/* Bits 0 to 27 */
+};
+
+#define FERR_NF_MASK		to_nf_mask(ERROR_NF_MASK)
+#define FERR_NF_CORRECTABLE	to_nf_mask(ERROR_NF_CORRECTABLE)
+#define FERR_NF_DIMM_SPARE	to_nf_mask(ERROR_NF_DIMM_SPARE)
+#define FERR_NF_SPD_PROTOCOL	to_nf_mask(ERROR_NF_SPD_PROTOCOL)
+#define FERR_NF_NORTH_CRC	to_nf_mask(ERROR_NF_NORTH_CRC)
+#define FERR_NF_RECOVERABLE	to_nf_mask(ERROR_NF_RECOVERABLE)
+#define FERR_NF_UNCORRECTABLE	to_nf_mask(ERROR_NF_UNCORRECTABLE)
+
+/* Defines to extract the vaious fields from the
+ *	MTRx - Memory Technology Registers
+ */
+#define MTR_DIMMS_PRESENT(mtr)		((mtr) & (1 << 10))
+#define MTR_DIMMS_ETHROTTLE(mtr)	((mtr) & (1 << 9))
+#define MTR_DRAM_WIDTH(mtr)		(((mtr) & (1 << 8)) ? 8 : 4)
+#define MTR_DRAM_BANKS(mtr)		(((mtr) & (1 << 6)) ? 8 : 4)
+#define MTR_DRAM_BANKS_ADDR_BITS(mtr)	((MTR_DRAM_BANKS(mtr) == 8) ? 3 : 2)
+#define MTR_DIMM_RANK(mtr)		(((mtr) >> 5) & 0x1)
+#define MTR_DIMM_RANK_ADDR_BITS(mtr)	(MTR_DIMM_RANK(mtr) ? 2 : 1)
+#define MTR_DIMM_ROWS(mtr)		(((mtr) >> 2) & 0x3)
+#define MTR_DIMM_ROWS_ADDR_BITS(mtr)	(MTR_DIMM_ROWS(mtr) + 13)
+#define MTR_DIMM_COLS(mtr)		((mtr) & 0x3)
+#define MTR_DIMM_COLS_ADDR_BITS(mtr)	(MTR_DIMM_COLS(mtr) + 10)
+
+/* This applies to FERR_NF_FB-DIMM as well as FERR_FAT_FB-DIMM */
+static inline int extract_fbdchan_indx(u32 x)
+{
+	return (x>>28) & 0x3;
+}
+
+#ifdef CONFIG_EDAC_DEBUG
+/* MTR NUMROW */
+static const char *numrow_toString[] = {
+	"8,192 - 13 rows",
+	"16,384 - 14 rows",
+	"32,768 - 15 rows",
+	"65,536 - 16 rows"
+};
+
+/* MTR NUMCOL */
+static const char *numcol_toString[] = {
+	"1,024 - 10 columns",
+	"2,048 - 11 columns",
+	"4,096 - 12 columns",
+	"reserved"
+};
+#endif
+
+/* Device name and register DID (Device ID) */
+struct i5400_dev_info {
+	const char *ctl_name;	/* name for this device */
+	u16 fsb_mapping_errors;	/* DID for the branchmap,control */
+};
+
+/* Table of devices attributes supported by this driver */
+static const struct i5400_dev_info i5400_devs[] = {
+	{
+		.ctl_name = "I5400",
+		.fsb_mapping_errors = PCI_DEVICE_ID_INTEL_5400_ERR,
+	},
+};
+
+struct i5400_dimm_info {
+	int megabytes;		/* size, 0 means not present  */
+	int dual_rank;
+};
+
+/* driver private data structure */
+struct i5400_pvt {
+	struct pci_dev *system_address;		/* 16.0 */
+	struct pci_dev *branchmap_werrors;	/* 16.1 */
+	struct pci_dev *fsb_error_regs;		/* 16.2 */
+	struct pci_dev *branch_0;		/* 21.0 */
+	struct pci_dev *branch_1;		/* 22.0 */
+
+	u16 tolm;				/* top of low memory */
+	u64 ambase;				/* AMB BAR */
+
+	u16 mir0, mir1;
+
+	u16 b0_mtr[NUM_MTRS_PER_BRANCH];	/* Memory Technlogy Reg */
+	u16 b0_ambpresent0;			/* Branch 0, Channel 0 */
+	u16 b0_ambpresent1;			/* Brnach 0, Channel 1 */
+
+	u16 b1_mtr[NUM_MTRS_PER_BRANCH];	/* Memory Technlogy Reg */
+	u16 b1_ambpresent0;			/* Branch 1, Channel 8 */
+	u16 b1_ambpresent1;			/* Branch 1, Channel 1 */
+
+	/* DIMM information matrix, allocating architecture maximums */
+	struct i5400_dimm_info dimm_info[MAX_CSROWS][MAX_CHANNELS];
+
+	/* Actual values for this controller */
+	int maxch;				/* Max channels */
+	int maxdimmperch;			/* Max DIMMs per channel */
+};
+
+/* I5400 MCH error information retrieved from Hardware */
+struct i5400_error_info {
+	/* These registers are always read from the MC */
+	u32 ferr_fat_fbd;	/* First Errors Fatal */
+	u32 nerr_fat_fbd;	/* Next Errors Fatal */
+	u32 ferr_nf_fbd;	/* First Errors Non-Fatal */
+	u32 nerr_nf_fbd;	/* Next Errors Non-Fatal */
+
+	/* These registers are input ONLY if there was a Recoverable Error */
+	u32 redmemb;		/* Recoverable Mem Data Error log B */
+	u16 recmema;		/* Recoverable Mem Error log A */
+	u32 recmemb;		/* Recoverable Mem Error log B */
+
+	/* These registers are input ONLY if there was a Non-Rec Error */
+	u16 nrecmema;		/* Non-Recoverable Mem log A */
+	u16 nrecmemb;		/* Non-Recoverable Mem log B */
+
+};
+
+/* note that nrec_rdwr changed from NRECMEMA to NRECMEMB between the 5000 and
+   5400 better to use an inline function than a macro in this case */
+static inline int nrec_bank(struct i5400_error_info *info)
+{
+	return ((info->nrecmema) >> 12) & 0x7;
+}
+static inline int nrec_rank(struct i5400_error_info *info)
+{
+	return ((info->nrecmema) >> 8) & 0xf;
+}
+static inline int nrec_buf_id(struct i5400_error_info *info)
+{
+	return ((info->nrecmema)) & 0xff;
+}
+static inline int nrec_rdwr(struct i5400_error_info *info)
+{
+	return (info->nrecmemb) >> 31;
+}
+/* This applies to both NREC and REC string so it can be used with nrec_rdwr
+   and rec_rdwr */
+static inline const char *rdwr_str(int rdwr)
+{
+	return rdwr ? "Write" : "Read";
+}
+static inline int nrec_cas(struct i5400_error_info *info)
+{
+	return ((info->nrecmemb) >> 16) & 0x1fff;
+}
+static inline int nrec_ras(struct i5400_error_info *info)
+{
+	return (info->nrecmemb) & 0xffff;
+}
+static inline int rec_bank(struct i5400_error_info *info)
+{
+	return ((info->recmema) >> 12) & 0x7;
+}
+static inline int rec_rank(struct i5400_error_info *info)
+{
+	return ((info->recmema) >> 8) & 0xf;
+}
+static inline int rec_rdwr(struct i5400_error_info *info)
+{
+	return (info->recmemb) >> 31;
+}
+static inline int rec_cas(struct i5400_error_info *info)
+{
+	return ((info->recmemb) >> 16) & 0x1fff;
+}
+static inline int rec_ras(struct i5400_error_info *info)
+{
+	return (info->recmemb) & 0xffff;
+}
+
+static struct edac_pci_ctl_info *i5400_pci;
+
+/*
+ *	i5400_get_error_info	Retrieve the hardware error information from
+ *				the hardware and cache it in the 'info'
+ *				structure
+ */
+static void i5400_get_error_info(struct mem_ctl_info *mci,
+				 struct i5400_error_info *info)
+{
+	struct i5400_pvt *pvt;
+	u32 value;
+
+	pvt = mci->pvt_info;
+
+	/* read in the 1st FATAL error register */
+	pci_read_config_dword(pvt->branchmap_werrors, FERR_FAT_FBD, &value);
+
+	/* Mask only the bits that the doc says are valid
+	 */
+	value &= (FERR_FAT_FBDCHAN | FERR_FAT_MASK);
+
+	/* If there is an error, then read in the
+	   NEXT FATAL error register and the Memory Error Log Register A
+	 */
+	if (value & FERR_FAT_MASK) {
+		info->ferr_fat_fbd = value;
+
+		/* harvest the various error data we need */
+		pci_read_config_dword(pvt->branchmap_werrors,
+				NERR_FAT_FBD, &info->nerr_fat_fbd);
+		pci_read_config_word(pvt->branchmap_werrors,
+				NRECMEMA, &info->nrecmema);
+		pci_read_config_word(pvt->branchmap_werrors,
+				NRECMEMB, &info->nrecmemb);
+
+		/* Clear the error bits, by writing them back */
+		pci_write_config_dword(pvt->branchmap_werrors,
+				FERR_FAT_FBD, value);
+	} else {
+		info->ferr_fat_fbd = 0;
+		info->nerr_fat_fbd = 0;
+		info->nrecmema = 0;
+		info->nrecmemb = 0;
+	}
+
+	/* read in the 1st NON-FATAL error register */
+	pci_read_config_dword(pvt->branchmap_werrors, FERR_NF_FBD, &value);
+
+	/* If there is an error, then read in the 1st NON-FATAL error
+	 * register as well */
+	if (value & FERR_NF_MASK) {
+		info->ferr_nf_fbd = value;
+
+		/* harvest the various error data we need */
+		pci_read_config_dword(pvt->branchmap_werrors,
+				NERR_NF_FBD, &info->nerr_nf_fbd);
+		pci_read_config_word(pvt->branchmap_werrors,
+				RECMEMA, &info->recmema);
+		pci_read_config_dword(pvt->branchmap_werrors,
+				RECMEMB, &info->recmemb);
+		pci_read_config_dword(pvt->branchmap_werrors,
+				REDMEMB, &info->redmemb);
+
+		/* Clear the error bits, by writing them back */
+		pci_write_config_dword(pvt->branchmap_werrors,
+				FERR_NF_FBD, value);
+	} else {
+		info->ferr_nf_fbd = 0;
+		info->nerr_nf_fbd = 0;
+		info->recmema = 0;
+		info->recmemb = 0;
+		info->redmemb = 0;
+	}
+}
+
+/*
+ * i5400_proccess_non_recoverable_info(struct mem_ctl_info *mci,
+ * 					struct i5400_error_info *info,
+ * 					int handle_errors);
+ *
+ *	handle the Intel FATAL and unrecoverable errors, if any
+ */
+static void i5400_proccess_non_recoverable_info(struct mem_ctl_info *mci,
+				    struct i5400_error_info *info,
+				    unsigned long allErrors)
+{
+	char msg[EDAC_MC_LABEL_LEN + 1 + 90 + 80];
+	int branch;
+	int channel;
+	int bank;
+	int buf_id;
+	int rank;
+	int rdwr;
+	int ras, cas;
+	int errnum;
+	char *type = NULL;
+
+	if (!allErrors)
+		return;		/* if no error, return now */
+
+	if (allErrors &  ERROR_FAT_MASK)
+		type = "FATAL";
+	else if (allErrors & FERR_NF_UNCORRECTABLE)
+		type = "NON-FATAL uncorrected";
+	else
+		type = "NON-FATAL recoverable";
+
+	/* ONLY ONE of the possible error bits will be set, as per the docs */
+
+	branch = extract_fbdchan_indx(info->ferr_fat_fbd);
+	channel = branch;
+
+	/* Use the NON-Recoverable macros to extract data */
+	bank = nrec_bank(info);
+	rank = nrec_rank(info);
+	buf_id = nrec_buf_id(info);
+	rdwr = nrec_rdwr(info);
+	ras = nrec_ras(info);
+	cas = nrec_cas(info);
+
+	debugf0("\t\tCSROW= %d  Channels= %d,%d  (Branch= %d "
+		"DRAM Bank= %d Buffer ID = %d rdwr= %s ras= %d cas= %d)\n",
+		rank, channel, channel + 1, branch >> 1, bank,
+		buf_id, rdwr_str(rdwr), ras, cas);
+
+	/* Only 1 bit will be on */
+	errnum = find_first_bit(&allErrors, ARRAY_SIZE(error_name));
+
+	/* Form out message */
+	snprintf(msg, sizeof(msg),
+		 "%s (Branch=%d DRAM-Bank=%d Buffer ID = %d RDWR=%s "
+		 "RAS=%d CAS=%d %s Err=0x%lx (%s))",
+		 type, branch >> 1, bank, buf_id, rdwr_str(rdwr), ras, cas,
+		 type, allErrors, error_name[errnum]);
+
+	/* Call the helper to output message */
+	edac_mc_handle_fbd_ue(mci, rank, channel, channel + 1, msg);
+}
+
+/*
+ * i5400_process_fatal_error_info(struct mem_ctl_info *mci,
+ * 				struct i5400_error_info *info,
+ * 				int handle_errors);
+ *
+ *	handle the Intel NON-FATAL errors, if any
+ */
+static void i5400_process_nonfatal_error_info(struct mem_ctl_info *mci,
+					struct i5400_error_info *info)
+{
+	char msg[EDAC_MC_LABEL_LEN + 1 + 90 + 80];
+	unsigned long allErrors;
+	int branch;
+	int channel;
+	int bank;
+	int rank;
+	int rdwr;
+	int ras, cas;
+	int errnum;
+
+	/* mask off the Error bits that are possible */
+	allErrors = from_nf_ferr(info->ferr_nf_fbd & FERR_NF_MASK);
+	if (!allErrors)
+		return;		/* if no error, return now */
+
+	/* ONLY ONE of the possible error bits will be set, as per the docs */
+
+	if (allErrors & (ERROR_NF_UNCORRECTABLE | ERROR_NF_RECOVERABLE)) {
+		i5400_proccess_non_recoverable_info(mci, info, allErrors);
+		return;
+	}
+
+	/* Correctable errors */
+	if (allErrors & ERROR_NF_CORRECTABLE) {
+		debugf0("\tCorrected bits= 0x%lx\n", allErrors);
+
+		branch = extract_fbdchan_indx(info->ferr_nf_fbd);
+
+		channel = 0;
+		if (REC_ECC_LOCATOR_ODD(info->redmemb))
+			channel = 1;
+
+		/* Convert channel to be based from zero, instead of
+		 * from branch base of 0 */
+		channel += branch;
+
+		bank = rec_bank(info);
+		rank = rec_rank(info);
+		rdwr = rec_rdwr(info);
+		ras = rec_ras(info);
+		cas = rec_cas(info);
+
+		/* Only 1 bit will be on */
+		errnum = find_first_bit(&allErrors, ARRAY_SIZE(error_name));
+
+		debugf0("\t\tCSROW= %d Channel= %d  (Branch %d "
+			"DRAM Bank= %d rdwr= %s ras= %d cas= %d)\n",
+			rank, channel, branch >> 1, bank,
+			rdwr_str(rdwr), ras, cas);
+
+		/* Form out message */
+		snprintf(msg, sizeof(msg),
+			 "Corrected error (Branch=%d DRAM-Bank=%d RDWR=%s "
+			 "RAS=%d CAS=%d, CE Err=0x%lx (%s))",
+			 branch >> 1, bank, rdwr_str(rdwr), ras, cas,
+			 allErrors, error_name[errnum]);
+
+		/* Call the helper to output message */
+		edac_mc_handle_fbd_ce(mci, rank, channel, msg);
+
+		return;
+	}
+
+	/* Miscelaneous errors */
+	errnum = find_first_bit(&allErrors, ARRAY_SIZE(error_name));
+
+	branch = extract_fbdchan_indx(info->ferr_nf_fbd);
+
+	i5400_mc_printk(mci, KERN_EMERG,
+			"Non-Fatal misc error (Branch=%d Err=%#lx (%s))",
+			branch >> 1, allErrors, error_name[errnum]);
+}
+
+/*
+ *	i5400_process_error_info	Process the error info that is
+ *	in the 'info' structure, previously retrieved from hardware
+ */
+static void i5400_process_error_info(struct mem_ctl_info *mci,
+				struct i5400_error_info *info)
+{	u32 allErrors;
+
+	/* First handle any fatal errors that occurred */
+	allErrors = (info->ferr_fat_fbd & FERR_FAT_MASK);
+	i5400_proccess_non_recoverable_info(mci, info, allErrors);
+
+	/* now handle any non-fatal errors that occurred */
+	i5400_process_nonfatal_error_info(mci, info);
+}
+
+/*
+ *	i5400_clear_error	Retrieve any error from the hardware
+ *				but do NOT process that error.
+ *				Used for 'clearing' out of previous errors
+ *				Called by the Core module.
+ */
+static void i5400_clear_error(struct mem_ctl_info *mci)
+{
+	struct i5400_error_info info;
+
+	i5400_get_error_info(mci, &info);
+}
+
+/*
+ *	i5400_check_error	Retrieve and process errors reported by the
+ *				hardware. Called by the Core module.
+ */
+static void i5400_check_error(struct mem_ctl_info *mci)
+{
+	struct i5400_error_info info;
+	debugf4("MC%d: " __FILE__ ": %s()\n", mci->mc_idx, __func__);
+	i5400_get_error_info(mci, &info);
+	i5400_process_error_info(mci, &info);
+}
+
+/*
+ *	i5400_put_devices	'put' all the devices that we have
+ *				reserved via 'get'
+ */
+static void i5400_put_devices(struct mem_ctl_info *mci)
+{
+	struct i5400_pvt *pvt;
+
+	pvt = mci->pvt_info;
+
+	/* Decrement usage count for devices */
+	pci_dev_put(pvt->branch_1);
+	pci_dev_put(pvt->branch_0);
+	pci_dev_put(pvt->fsb_error_regs);
+	pci_dev_put(pvt->branchmap_werrors);
+}
+
+/*
+ *	i5400_get_devices	Find and perform 'get' operation on the MCH's
+ *			device/functions we want to reference for this driver
+ *
+ *			Need to 'get' device 16 func 1 and func 2
+ */
+static int i5400_get_devices(struct mem_ctl_info *mci, int dev_idx)
+{
+	struct i5400_pvt *pvt;
+	struct pci_dev *pdev;
+
+	pvt = mci->pvt_info;
+	pvt->branchmap_werrors = NULL;
+	pvt->fsb_error_regs = NULL;
+	pvt->branch_0 = NULL;
+	pvt->branch_1 = NULL;
+
+	/* Attempt to 'get' the MCH register we want */
+	pdev = NULL;
+	while (!pvt->branchmap_werrors || !pvt->fsb_error_regs) {
+		pdev = pci_get_device(PCI_VENDOR_ID_INTEL,
+				      PCI_DEVICE_ID_INTEL_5400_ERR, pdev);
+		if (!pdev) {
+			/* End of list, leave */
+			i5400_printk(KERN_ERR,
+				"'system address,Process Bus' "
+				"device not found:"
+				"vendor 0x%x device 0x%x ERR funcs "
+				"(broken BIOS?)\n",
+				PCI_VENDOR_ID_INTEL,
+				PCI_DEVICE_ID_INTEL_5400_ERR);
+			goto error;
+		}
+
+		/* Store device 16 funcs 1 and 2 */
+		switch (PCI_FUNC(pdev->devfn)) {
+		case 1:
+			pvt->branchmap_werrors = pdev;
+			break;
+		case 2:
+			pvt->fsb_error_regs = pdev;
+			break;
+		}
+	}
+
+	debugf1("System Address, processor bus- PCI Bus ID: %s  %x:%x\n",
+		pci_name(pvt->system_address),
+		pvt->system_address->vendor, pvt->system_address->device);
+	debugf1("Branchmap, control and errors - PCI Bus ID: %s  %x:%x\n",
+		pci_name(pvt->branchmap_werrors),
+		pvt->branchmap_werrors->vendor, pvt->branchmap_werrors->device);
+	debugf1("FSB Error Regs - PCI Bus ID: %s  %x:%x\n",
+		pci_name(pvt->fsb_error_regs),
+		pvt->fsb_error_regs->vendor, pvt->fsb_error_regs->device);
+
+	pvt->branch_0 = pci_get_device(PCI_VENDOR_ID_INTEL,
+				       PCI_DEVICE_ID_INTEL_5400_FBD0, NULL);
+	if (!pvt->branch_0) {
+		i5400_printk(KERN_ERR,
+			"MC: 'BRANCH 0' device not found:"
+			"vendor 0x%x device 0x%x Func 0 (broken BIOS?)\n",
+			PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_5400_FBD0);
+		goto error;
+	}
+
+	/* If this device claims to have more than 2 channels then
+	 * fetch Branch 1's information
+	 */
+	if (pvt->maxch < CHANNELS_PER_BRANCH)
+		return 0;
+
+	pvt->branch_1 = pci_get_device(PCI_VENDOR_ID_INTEL,
+				       PCI_DEVICE_ID_INTEL_5400_FBD1, NULL);
+	if (!pvt->branch_1) {
+		i5400_printk(KERN_ERR,
+			"MC: 'BRANCH 1' device not found:"
+			"vendor 0x%x device 0x%x Func 0 "
+			"(broken BIOS?)\n",
+			PCI_VENDOR_ID_INTEL,
+			PCI_DEVICE_ID_INTEL_5400_FBD1);
+		goto error;
+	}
+
+	return 0;
+
+error:
+	i5400_put_devices(mci);
+	return -ENODEV;
+}
+
+/*
+ *	determine_amb_present
+ *
+ *		the information is contained in NUM_MTRS_PER_BRANCH different
+ *		registers determining which of the NUM_MTRS_PER_BRANCH requires
+ *              knowing which channel is in question
+ *
+ *	2 branches, each with 2 channels
+ *		b0_ambpresent0 for channel '0'
+ *		b0_ambpresent1 for channel '1'
+ *		b1_ambpresent0 for channel '2'
+ *		b1_ambpresent1 for channel '3'
+ */
+static int determine_amb_present_reg(struct i5400_pvt *pvt, int channel)
+{
+	int amb_present;
+
+	if (channel < CHANNELS_PER_BRANCH) {
+		if (channel & 0x1)
+			amb_present = pvt->b0_ambpresent1;
+		else
+			amb_present = pvt->b0_ambpresent0;
+	} else {
+		if (channel & 0x1)
+			amb_present = pvt->b1_ambpresent1;
+		else
+			amb_present = pvt->b1_ambpresent0;
+	}
+
+	return amb_present;
+}
+
+/*
+ * determine_mtr(pvt, csrow, channel)
+ *
+ * return the proper MTR register as determine by the csrow and desired channel
+ */
+static int determine_mtr(struct i5400_pvt *pvt, int csrow, int channel)
+{
+	int mtr;
+	int n;
+
+	/* There is one MTR for each slot pair of FB-DIMMs,
+	   Each slot may have one or two ranks (2 csrows),
+	   Each slot pair may be at branch 0 or branch 1.
+	   So, csrow should be divided by eight
+	 */
+	n = csrow >> 3;
+
+	if (n >= NUM_MTRS_PER_BRANCH) {
+		debugf0("ERROR: trying to access an invalid csrow: %d\n",
+			csrow);
+		return 0;
+	}
+
+	if (channel < CHANNELS_PER_BRANCH)
+		mtr = pvt->b0_mtr[n];
+	else
+		mtr = pvt->b1_mtr[n];
+
+	return mtr;
+}
+
+/*
+ */
+static void decode_mtr(int slot_row, u16 mtr)
+{
+	int ans;
+
+	ans = MTR_DIMMS_PRESENT(mtr);
+
+	debugf2("\tMTR%d=0x%x:  DIMMs are %s\n", slot_row, mtr,
+		ans ? "Present" : "NOT Present");
+	if (!ans)
+		return;
+
+	debugf2("\t\tWIDTH: x%d\n", MTR_DRAM_WIDTH(mtr));
+
+	debugf2("\t\tELECTRICAL THROTTLING is %s\n",
+		MTR_DIMMS_ETHROTTLE(mtr) ? "enabled" : "disabled");
+
+	debugf2("\t\tNUMBANK: %d bank(s)\n", MTR_DRAM_BANKS(mtr));
+	debugf2("\t\tNUMRANK: %s\n", MTR_DIMM_RANK(mtr) ? "double" : "single");
+	debugf2("\t\tNUMROW: %s\n", numrow_toString[MTR_DIMM_ROWS(mtr)]);
+	debugf2("\t\tNUMCOL: %s\n", numcol_toString[MTR_DIMM_COLS(mtr)]);
+}
+
+static void handle_channel(struct i5400_pvt *pvt, int csrow, int channel,
+			struct i5400_dimm_info *dinfo)
+{
+	int mtr;
+	int amb_present_reg;
+	int addrBits;
+
+	mtr = determine_mtr(pvt, csrow, channel);
+	if (MTR_DIMMS_PRESENT(mtr)) {
+		amb_present_reg = determine_amb_present_reg(pvt, channel);
+
+		/* Determine if there is a DIMM present in this DIMM slot */
+		if (amb_present_reg & (1 << (csrow >> 1))) {
+			dinfo->dual_rank = MTR_DIMM_RANK(mtr);
+
+			if (!((dinfo->dual_rank == 0) &&
+				((csrow & 0x1) == 0x1))) {
+				/* Start with the number of bits for a Bank
+				 * on the DRAM */
+				addrBits = MTR_DRAM_BANKS_ADDR_BITS(mtr);
+				/* Add thenumber of ROW bits */
+				addrBits += MTR_DIMM_ROWS_ADDR_BITS(mtr);
+				/* add the number of COLUMN bits */
+				addrBits += MTR_DIMM_COLS_ADDR_BITS(mtr);
+
+				addrBits += 6;	/* add 64 bits per DIMM */
+				addrBits -= 20;	/* divide by 2^^20 */
+				addrBits -= 3;	/* 8 bits per bytes */
+
+				dinfo->megabytes = 1 << addrBits;
+			}
+		}
+	}
+}
+
+/*
+ *	calculate_dimm_size
+ *
+ *	also will output a DIMM matrix map, if debug is enabled, for viewing
+ *	how the DIMMs are populated
+ */
+static void calculate_dimm_size(struct i5400_pvt *pvt)
+{
+	struct i5400_dimm_info *dinfo;
+	int csrow, max_csrows;
+	char *p, *mem_buffer;
+	int space, n;
+	int channel;
+
+	/* ================= Generate some debug output ================= */
+	space = PAGE_SIZE;
+	mem_buffer = p = kmalloc(space, GFP_KERNEL);
+	if (p == NULL) {
+		i5400_printk(KERN_ERR, "MC: %s:%s() kmalloc() failed\n",
+			__FILE__, __func__);
+		return;
+	}
+
+	/* Scan all the actual CSROWS (which is # of DIMMS * 2)
+	 * and calculate the information for each DIMM
+	 * Start with the highest csrow first, to display it first
+	 * and work toward the 0th csrow
+	 */
+	max_csrows = pvt->maxdimmperch * 2;
+	for (csrow = max_csrows - 1; csrow >= 0; csrow--) {
+
+		/* on an odd csrow, first output a 'boundary' marker,
+		 * then reset the message buffer  */
+		if (csrow & 0x1) {
+			n = snprintf(p, space, "---------------------------"
+					"--------------------------------");
+			p += n;
+			space -= n;
+			debugf2("%s\n", mem_buffer);
+			p = mem_buffer;
+			space = PAGE_SIZE;
+		}
+		n = snprintf(p, space, "csrow %2d    ", csrow);
+		p += n;
+		space -= n;
+
+		for (channel = 0; channel < pvt->maxch; channel++) {
+			dinfo = &pvt->dimm_info[csrow][channel];
+			handle_channel(pvt, csrow, channel, dinfo);
+			n = snprintf(p, space, "%4d MB   | ", dinfo->megabytes);
+			p += n;
+			space -= n;
+		}
+		debugf2("%s\n", mem_buffer);
+		p = mem_buffer;
+		space = PAGE_SIZE;
+	}
+
+	/* Output the last bottom 'boundary' marker */
+	n = snprintf(p, space, "---------------------------"
+			"--------------------------------");
+	p += n;
+	space -= n;
+	debugf2("%s\n", mem_buffer);
+	p = mem_buffer;
+	space = PAGE_SIZE;
+
+	/* now output the 'channel' labels */
+	n = snprintf(p, space, "            ");
+	p += n;
+	space -= n;
+	for (channel = 0; channel < pvt->maxch; channel++) {
+		n = snprintf(p, space, "channel %d | ", channel);
+		p += n;
+		space -= n;
+	}
+
+	/* output the last message and free buffer */
+	debugf2("%s\n", mem_buffer);
+	kfree(mem_buffer);
+}
+
+/*
+ *	i5400_get_mc_regs	read in the necessary registers and
+ *				cache locally
+ *
+ *			Fills in the private data members
+ */
+static void i5400_get_mc_regs(struct mem_ctl_info *mci)
+{
+	struct i5400_pvt *pvt;
+	u32 actual_tolm;
+	u16 limit;
+	int slot_row;
+	int maxch;
+	int maxdimmperch;
+	int way0, way1;
+
+	pvt = mci->pvt_info;
+
+	pci_read_config_dword(pvt->system_address, AMBASE,
+			(u32 *) &pvt->ambase);
+	pci_read_config_dword(pvt->system_address, AMBASE + sizeof(u32),
+			((u32 *) &pvt->ambase) + sizeof(u32));
+
+	maxdimmperch = pvt->maxdimmperch;
+	maxch = pvt->maxch;
+
+	debugf2("AMBASE= 0x%lx  MAXCH= %d  MAX-DIMM-Per-CH= %d\n",
+		(long unsigned int)pvt->ambase, pvt->maxch, pvt->maxdimmperch);
+
+	/* Get the Branch Map regs */
+	pci_read_config_word(pvt->branchmap_werrors, TOLM, &pvt->tolm);
+	pvt->tolm >>= 12;
+	debugf2("\nTOLM (number of 256M regions) =%u (0x%x)\n", pvt->tolm,
+		pvt->tolm);
+
+	actual_tolm = (u32) ((1000l * pvt->tolm) >> (30 - 28));
+	debugf2("Actual TOLM byte addr=%u.%03u GB (0x%x)\n",
+		actual_tolm/1000, actual_tolm % 1000, pvt->tolm << 28);
+
+	pci_read_config_word(pvt->branchmap_werrors, MIR0, &pvt->mir0);
+	pci_read_config_word(pvt->branchmap_werrors, MIR1, &pvt->mir1);
+
+	/* Get the MIR[0-1] regs */
+	limit = (pvt->mir0 >> 4) & 0x0fff;
+	way0 = pvt->mir0 & 0x1;
+	way1 = pvt->mir0 & 0x2;
+	debugf2("MIR0: limit= 0x%x  WAY1= %u  WAY0= %x\n", limit, way1, way0);
+	limit = (pvt->mir1 >> 4) & 0xfff;
+	way0 = pvt->mir1 & 0x1;
+	way1 = pvt->mir1 & 0x2;
+	debugf2("MIR1: limit= 0x%x  WAY1= %u  WAY0= %x\n", limit, way1, way0);
+
+	/* Get the set of MTR[0-3] regs by each branch */
+	for (slot_row = 0; slot_row < NUM_MTRS_PER_BRANCH; slot_row++) {
+		int where = MTR0 + (slot_row * sizeof(u32));
+
+		/* Branch 0 set of MTR registers */
+		pci_read_config_word(pvt->branch_0, where,
+				&pvt->b0_mtr[slot_row]);
+
+		debugf2("MTR%d where=0x%x B0 value=0x%x\n", slot_row, where,
+			pvt->b0_mtr[slot_row]);
+
+		if (pvt->maxch < CHANNELS_PER_BRANCH) {
+			pvt->b1_mtr[slot_row] = 0;
+			continue;
+		}
+
+		/* Branch 1 set of MTR registers */
+		pci_read_config_word(pvt->branch_1, where,
+				&pvt->b1_mtr[slot_row]);
+		debugf2("MTR%d where=0x%x B1 value=0x%x\n", slot_row, where,
+			pvt->b1_mtr[slot_row]);
+	}
+
+	/* Read and dump branch 0's MTRs */
+	debugf2("\nMemory Technology Registers:\n");
+	debugf2("   Branch 0:\n");
+	for (slot_row = 0; slot_row < NUM_MTRS_PER_BRANCH; slot_row++)
+		decode_mtr(slot_row, pvt->b0_mtr[slot_row]);
+
+	pci_read_config_word(pvt->branch_0, AMBPRESENT_0,
+			&pvt->b0_ambpresent0);
+	debugf2("\t\tAMB-Branch 0-present0 0x%x:\n", pvt->b0_ambpresent0);
+	pci_read_config_word(pvt->branch_0, AMBPRESENT_1,
+			&pvt->b0_ambpresent1);
+	debugf2("\t\tAMB-Branch 0-present1 0x%x:\n", pvt->b0_ambpresent1);
+
+	/* Only if we have 2 branchs (4 channels) */
+	if (pvt->maxch < CHANNELS_PER_BRANCH) {
+		pvt->b1_ambpresent0 = 0;
+		pvt->b1_ambpresent1 = 0;
+	} else {
+		/* Read and dump  branch 1's MTRs */
+		debugf2("   Branch 1:\n");
+		for (slot_row = 0; slot_row < NUM_MTRS_PER_BRANCH; slot_row++)
+			decode_mtr(slot_row, pvt->b1_mtr[slot_row]);
+
+		pci_read_config_word(pvt->branch_1, AMBPRESENT_0,
+				&pvt->b1_ambpresent0);
+		debugf2("\t\tAMB-Branch 1-present0 0x%x:\n",
+			pvt->b1_ambpresent0);
+		pci_read_config_word(pvt->branch_1, AMBPRESENT_1,
+				&pvt->b1_ambpresent1);
+		debugf2("\t\tAMB-Branch 1-present1 0x%x:\n",
+			pvt->b1_ambpresent1);
+	}
+
+	/* Go and determine the size of each DIMM and place in an
+	 * orderly matrix */
+	calculate_dimm_size(pvt);
+}
+
+/*
+ *	i5400_init_csrows	Initialize the 'csrows' table within
+ *				the mci control	structure with the
+ *				addressing of memory.
+ *
+ *	return:
+ *		0	success
+ *		1	no actual memory found on this MC
+ */
+static int i5400_init_csrows(struct mem_ctl_info *mci)
+{
+	struct i5400_pvt *pvt;
+	struct csrow_info *p_csrow;
+	int empty, channel_count;
+	int max_csrows;
+	int mtr;
+	int csrow_megs;
+	int channel;
+	int csrow;
+
+	pvt = mci->pvt_info;
+
+	channel_count = pvt->maxch;
+	max_csrows = pvt->maxdimmperch * 2;
+
+	empty = 1;		/* Assume NO memory */
+
+	for (csrow = 0; csrow < max_csrows; csrow++) {
+		p_csrow = &mci->csrows[csrow];
+
+		p_csrow->csrow_idx = csrow;
+
+		/* use branch 0 for the basis */
+		mtr = determine_mtr(pvt, csrow, 0);
+
+		/* if no DIMMS on this row, continue */
+		if (!MTR_DIMMS_PRESENT(mtr))
+			continue;
+
+		/* FAKE OUT VALUES, FIXME */
+		p_csrow->first_page = 0 + csrow * 20;
+		p_csrow->last_page = 9 + csrow * 20;
+		p_csrow->page_mask = 0xFFF;
+
+		p_csrow->grain = 8;
+
+		csrow_megs = 0;
+		for (channel = 0; channel < pvt->maxch; channel++)
+			csrow_megs += pvt->dimm_info[csrow][channel].megabytes;
+
+		p_csrow->nr_pages = csrow_megs << 8;
+
+		/* Assume DDR2 for now */
+		p_csrow->mtype = MEM_FB_DDR2;
+
+		/* ask what device type on this row */
+		if (MTR_DRAM_WIDTH(mtr))
+			p_csrow->dtype = DEV_X8;
+		else
+			p_csrow->dtype = DEV_X4;
+
+		p_csrow->edac_mode = EDAC_S8ECD8ED;
+
+		empty = 0;
+	}
+
+	return empty;
+}
+
+/*
+ *	i5400_enable_error_reporting
+ *			Turn on the memory reporting features of the hardware
+ */
+static void i5400_enable_error_reporting(struct mem_ctl_info *mci)
+{
+	struct i5400_pvt *pvt;
+	u32 fbd_error_mask;
+
+	pvt = mci->pvt_info;
+
+	/* Read the FBD Error Mask Register */
+	pci_read_config_dword(pvt->branchmap_werrors, EMASK_FBD,
+			&fbd_error_mask);
+
+	/* Enable with a '0' */
+	fbd_error_mask &= ~(ENABLE_EMASK_ALL);
+
+	pci_write_config_dword(pvt->branchmap_werrors, EMASK_FBD,
+			fbd_error_mask);
+}
+
+/*
+ * i5400_get_dimm_and_channel_counts(pdev, &num_csrows, &num_channels)
+ *
+ *	ask the device how many channels are present and how many CSROWS
+ *	 as well
+ */
+static void i5400_get_dimm_and_channel_counts(struct pci_dev *pdev,
+					int *num_dimms_per_channel,
+					int *num_channels)
+{
+	u8 value;
+
+	/* Need to retrieve just how many channels and dimms per channel are
+	 * supported on this memory controller
+	 */
+	pci_read_config_byte(pdev, MAXDIMMPERCH, &value);
+	*num_dimms_per_channel = (int)value * 2;
+
+	pci_read_config_byte(pdev, MAXCH, &value);
+	*num_channels = (int)value;
+}
+
+/*
+ *	i5400_probe1	Probe for ONE instance of device to see if it is
+ *			present.
+ *	return:
+ *		0 for FOUND a device
+ *		< 0 for error code
+ */
+static int i5400_probe1(struct pci_dev *pdev, int dev_idx)
+{
+	struct mem_ctl_info *mci;
+	struct i5400_pvt *pvt;
+	int num_channels;
+	int num_dimms_per_channel;
+	int num_csrows;
+
+	if (dev_idx >= ARRAY_SIZE(i5400_devs))
+		return -EINVAL;
+
+	debugf0("MC: " __FILE__ ": %s(), pdev bus %u dev=0x%x fn=0x%x\n",
+		__func__,
+		pdev->bus->number,
+		PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
+
+	/* We only are looking for func 0 of the set */
+	if (PCI_FUNC(pdev->devfn) != 0)
+		return -ENODEV;
+
+	/* Ask the devices for the number of CSROWS and CHANNELS so
+	 * that we can calculate the memory resources, etc
+	 *
+	 * The Chipset will report what it can handle which will be greater
+	 * or equal to what the motherboard manufacturer will implement.
+	 *
+	 * As we don't have a motherboard identification routine to determine
+	 * actual number of slots/dimms per channel, we thus utilize the
+	 * resource as specified by the chipset. Thus, we might have
+	 * have more DIMMs per channel than actually on the mobo, but this
+	 * allows the driver to support upto the chipset max, without
+	 * some fancy mobo determination.
+	 */
+	i5400_get_dimm_and_channel_counts(pdev, &num_dimms_per_channel,
+					&num_channels);
+	num_csrows = num_dimms_per_channel * 2;
+
+	debugf0("MC: %s(): Number of - Channels= %d  DIMMS= %d  CSROWS= %d\n",
+		__func__, num_channels, num_dimms_per_channel, num_csrows);
+
+	/* allocate a new MC control structure */
+	mci = edac_mc_alloc(sizeof(*pvt), num_csrows, num_channels, 0);
+
+	if (mci == NULL)
+		return -ENOMEM;
+
+	debugf0("MC: " __FILE__ ": %s(): mci = %p\n", __func__, mci);
+
+	mci->dev = &pdev->dev;	/* record ptr  to the generic device */
+
+	pvt = mci->pvt_info;
+	pvt->system_address = pdev;	/* Record this device in our private */
+	pvt->maxch = num_channels;
+	pvt->maxdimmperch = num_dimms_per_channel;
+
+	/* 'get' the pci devices we want to reserve for our use */
+	if (i5400_get_devices(mci, dev_idx))
+		goto fail0;
+
+	/* Time to get serious */
+	i5400_get_mc_regs(mci);	/* retrieve the hardware registers */
+
+	mci->mc_idx = 0;
+	mci->mtype_cap = MEM_FLAG_FB_DDR2;
+	mci->edac_ctl_cap = EDAC_FLAG_NONE;
+	mci->edac_cap = EDAC_FLAG_NONE;
+	mci->mod_name = "i5400_edac.c";
+	mci->mod_ver = I5400_REVISION;
+	mci->ctl_name = i5400_devs[dev_idx].ctl_name;
+	mci->dev_name = pci_name(pdev);
+	mci->ctl_page_to_phys = NULL;
+
+	/* Set the function pointer to an actual operation function */
+	mci->edac_check = i5400_check_error;
+
+	/* initialize the MC control structure 'csrows' table
+	 * with the mapping and control information */
+	if (i5400_init_csrows(mci)) {
+		debugf0("MC: Setting mci->edac_cap to EDAC_FLAG_NONE\n"
+			"    because i5400_init_csrows() returned nonzero "
+			"value\n");
+		mci->edac_cap = EDAC_FLAG_NONE;	/* no csrows found */
+	} else {
+		debugf1("MC: Enable error reporting now\n");
+		i5400_enable_error_reporting(mci);
+	}
+
+	/* add this new MC control structure to EDAC's list of MCs */
+	if (edac_mc_add_mc(mci)) {
+		debugf0("MC: " __FILE__
+			": %s(): failed edac_mc_add_mc()\n", __func__);
+		/* FIXME: perhaps some code should go here that disables error
+		 * reporting if we just enabled it
+		 */
+		goto fail1;
+	}
+
+	i5400_clear_error(mci);
+
+	/* allocating generic PCI control info */
+	i5400_pci = edac_pci_create_generic_ctl(&pdev->dev, EDAC_MOD_STR);
+	if (!i5400_pci) {
+		printk(KERN_WARNING
+			"%s(): Unable to create PCI control\n",
+			__func__);
+		printk(KERN_WARNING
+			"%s(): PCI error report via EDAC not setup\n",
+			__func__);
+	}
+
+	return 0;
+
+	/* Error exit unwinding stack */
+fail1:
+
+	i5400_put_devices(mci);
+
+fail0:
+	edac_mc_free(mci);
+	return -ENODEV;
+}
+
+/*
+ *	i5400_init_one	constructor for one instance of device
+ *
+ * 	returns:
+ *		negative on error
+ *		count (>= 0)
+ */
+static int __devinit i5400_init_one(struct pci_dev *pdev,
+				const struct pci_device_id *id)
+{
+	int rc;
+
+	debugf0("MC: " __FILE__ ": %s()\n", __func__);
+
+	/* wake up device */
+	rc = pci_enable_device(pdev);
+	if (rc == -EIO)
+		return rc;
+
+	/* now probe and enable the device */
+	return i5400_probe1(pdev, id->driver_data);
+}
+
+/*
+ *	i5400_remove_one	destructor for one instance of device
+ *
+ */
+static void __devexit i5400_remove_one(struct pci_dev *pdev)
+{
+	struct mem_ctl_info *mci;
+
+	debugf0(__FILE__ ": %s()\n", __func__);
+
+	if (i5400_pci)
+		edac_pci_release_generic_ctl(i5400_pci);
+
+	mci = edac_mc_del_mc(&pdev->dev);
+	if (!mci)
+		return;
+
+	/* retrieve references to resources, and free those resources */
+	i5400_put_devices(mci);
+
+	edac_mc_free(mci);
+}
+
+/*
+ *	pci_device_id	table for which devices we are looking for
+ *
+ *	The "E500P" device is the first device supported.
+ */
+static const struct pci_device_id i5400_pci_tbl[] __devinitdata = {
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_5400_ERR)},
+	{0,}			/* 0 terminated list. */
+};
+
+MODULE_DEVICE_TABLE(pci, i5400_pci_tbl);
+
+/*
+ *	i5400_driver	pci_driver structure for this module
+ *
+ */
+static struct pci_driver i5400_driver = {
+	.name = "i5400_edac",
+	.probe = i5400_init_one,
+	.remove = __devexit_p(i5400_remove_one),
+	.id_table = i5400_pci_tbl,
+};
+
+/*
+ *	i5400_init		Module entry function
+ *			Try to initialize this module for its devices
+ */
+static int __init i5400_init(void)
+{
+	int pci_rc;
+
+	debugf2("MC: " __FILE__ ": %s()\n", __func__);
+
+	/* Ensure that the OPSTATE is set correctly for POLL or NMI */
+	opstate_init();
+
+	pci_rc = pci_register_driver(&i5400_driver);
+
+	return (pci_rc < 0) ? pci_rc : 0;
+}
+
+/*
+ *	i5400_exit()	Module exit function
+ *			Unregister the driver
+ */
+static void __exit i5400_exit(void)
+{
+	debugf2("MC: " __FILE__ ": %s()\n", __func__);
+	pci_unregister_driver(&i5400_driver);
+}
+
+module_init(i5400_init);
+module_exit(i5400_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Ben Woodard <woodard@redhat.com>");
+MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
+MODULE_AUTHOR("Red Hat Inc. (http://www.redhat.com)");
+MODULE_DESCRIPTION("MC Driver for Intel I5400 memory controllers - "
+		   I5400_REVISION);
+
+module_param(edac_op_state, int, 0444);
+MODULE_PARM_DESC(edac_op_state, "EDAC Error Reporting state: 0=Poll,1=NMI");
diff --git a/drivers/edac/i82875p_edac.c b/drivers/edac/i82875p_edac.c
index ebb037b78758..b2d83b95033d 100644
--- a/drivers/edac/i82875p_edac.c
+++ b/drivers/edac/i82875p_edac.c
@@ -311,9 +311,7 @@ static int i82875p_setup_overfl_dev(struct pci_dev *pdev,
 	}
 
 	/* cache is irrelevant for PCI bus reads/writes */
-	window = ioremap_nocache(pci_resource_start(dev, 0),
-				 pci_resource_len(dev, 0));
-
+	window = pci_ioremap_bar(dev, 0);
 	if (window == NULL) {
 		i82875p_printk(KERN_ERR, "%s(): Failed to ioremap bar6\n",
 			__func__);
diff --git a/drivers/edac/mpc85xx_edac.c b/drivers/edac/mpc85xx_edac.c
index 0cfcb2d075a0..853ef37ec006 100644
--- a/drivers/edac/mpc85xx_edac.c
+++ b/drivers/edac/mpc85xx_edac.c
@@ -630,27 +630,22 @@ static int mpc85xx_l2_err_remove(struct of_device *op)
 }
 
 static struct of_device_id mpc85xx_l2_err_of_match[] = {
-	{
-	 .compatible = "fsl,8540-l2-cache-controller",
-	 },
-	{
-	 .compatible = "fsl,8541-l2-cache-controller",
-	 },
-	{
-	 .compatible = "fsl,8544-l2-cache-controller",
-	 },
-	{
-	 .compatible = "fsl,8548-l2-cache-controller",
-	 },
-	{
-	 .compatible = "fsl,8555-l2-cache-controller",
-	 },
-	{
-	 .compatible = "fsl,8568-l2-cache-controller",
-	 },
-	{
-	 .compatible = "fsl,mpc8572-l2-cache-controller",
-	 },
+/* deprecate the fsl,85.. forms in the future, 2.6.30? */
+	{ .compatible = "fsl,8540-l2-cache-controller", },
+	{ .compatible = "fsl,8541-l2-cache-controller", },
+	{ .compatible = "fsl,8544-l2-cache-controller", },
+	{ .compatible = "fsl,8548-l2-cache-controller", },
+	{ .compatible = "fsl,8555-l2-cache-controller", },
+	{ .compatible = "fsl,8568-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8536-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8540-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8541-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8544-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8548-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8555-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8560-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8568-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8572-l2-cache-controller", },
 	{},
 };
 
@@ -967,27 +962,22 @@ static int mpc85xx_mc_err_remove(struct of_device *op)
 }
 
 static struct of_device_id mpc85xx_mc_err_of_match[] = {
-	{
-	 .compatible = "fsl,8540-memory-controller",
-	 },
-	{
-	 .compatible = "fsl,8541-memory-controller",
-	 },
-	{
-	 .compatible = "fsl,8544-memory-controller",
-	 },
-	{
-	 .compatible = "fsl,8548-memory-controller",
-	 },
-	{
-	 .compatible = "fsl,8555-memory-controller",
-	 },
-	{
-	 .compatible = "fsl,8568-memory-controller",
-	 },
-	{
-	 .compatible = "fsl,mpc8572-memory-controller",
-	 },
+/* deprecate the fsl,85.. forms in the future, 2.6.30? */
+	{ .compatible = "fsl,8540-memory-controller", },
+	{ .compatible = "fsl,8541-memory-controller", },
+	{ .compatible = "fsl,8544-memory-controller", },
+	{ .compatible = "fsl,8548-memory-controller", },
+	{ .compatible = "fsl,8555-memory-controller", },
+	{ .compatible = "fsl,8568-memory-controller", },
+	{ .compatible = "fsl,mpc8536-memory-controller", },
+	{ .compatible = "fsl,mpc8540-memory-controller", },
+	{ .compatible = "fsl,mpc8541-memory-controller", },
+	{ .compatible = "fsl,mpc8544-memory-controller", },
+	{ .compatible = "fsl,mpc8548-memory-controller", },
+	{ .compatible = "fsl,mpc8555-memory-controller", },
+	{ .compatible = "fsl,mpc8560-memory-controller", },
+	{ .compatible = "fsl,mpc8568-memory-controller", },
+	{ .compatible = "fsl,mpc8572-memory-controller", },
 	{},
 };
 
diff --git a/drivers/firewire/fw-card.c b/drivers/firewire/fw-card.c
index 418c18f07e9d..799f94424c8a 100644
--- a/drivers/firewire/fw-card.c
+++ b/drivers/firewire/fw-card.c
@@ -75,7 +75,7 @@ generate_config_rom(struct fw_card *card, size_t *config_rom_length)
 	 * controller, block reads to the config rom accesses the host
 	 * memory, but quadlet read access the hardware bus info block
 	 * registers.  That's just crack, but it means we should make
-	 * sure the contents of bus info block in host memory mathces
+	 * sure the contents of bus info block in host memory matches
 	 * the version stored in the OHCI registers.
 	 */
 
@@ -189,6 +189,17 @@ static const char gap_count_table[] = {
 	63, 5, 7, 8, 10, 13, 16, 18, 21, 24, 26, 29, 32, 35, 37, 40
 };
 
+void
+fw_schedule_bm_work(struct fw_card *card, unsigned long delay)
+{
+	int scheduled;
+
+	fw_card_get(card);
+	scheduled = schedule_delayed_work(&card->work, delay);
+	if (!scheduled)
+		fw_card_put(card);
+}
+
 static void
 fw_card_bm_work(struct work_struct *work)
 {
@@ -206,7 +217,7 @@ fw_card_bm_work(struct work_struct *work)
 
 	if (local_node == NULL) {
 		spin_unlock_irqrestore(&card->lock, flags);
-		return;
+		goto out_put_card;
 	}
 	fw_node_get(local_node);
 	fw_node_get(root_node);
@@ -280,7 +291,7 @@ fw_card_bm_work(struct work_struct *work)
 		 * this task 100ms from now.
 		 */
 		spin_unlock_irqrestore(&card->lock, flags);
-		schedule_delayed_work(&card->work, DIV_ROUND_UP(HZ, 10));
+		fw_schedule_bm_work(card, DIV_ROUND_UP(HZ, 10));
 		goto out;
 	}
 
@@ -355,6 +366,8 @@ fw_card_bm_work(struct work_struct *work)
 		fw_device_put(root_device);
 	fw_node_put(root_node);
 	fw_node_put(local_node);
+ out_put_card:
+	fw_card_put(card);
 }
 
 static void
@@ -510,7 +523,6 @@ fw_core_remove_card(struct fw_card *card)
 	fw_card_put(card);
 	wait_for_completion(&card->done);
 
-	cancel_delayed_work_sync(&card->work);
 	WARN_ON(!list_empty(&card->transaction_list));
 	del_timer_sync(&card->flush_timer);
 }
diff --git a/drivers/firewire/fw-device.c b/drivers/firewire/fw-device.c
index 6b9be42c7b98..c173be383725 100644
--- a/drivers/firewire/fw-device.c
+++ b/drivers/firewire/fw-device.c
@@ -617,7 +617,7 @@ static int shutdown_unit(struct device *device, void *data)
  */
 DECLARE_RWSEM(fw_device_rwsem);
 
-static DEFINE_IDR(fw_device_idr);
+DEFINE_IDR(fw_device_idr);
 int fw_cdev_major;
 
 struct fw_device *fw_device_get_by_devt(dev_t devt)
@@ -689,7 +689,7 @@ static void fw_device_init(struct work_struct *work)
 			fw_notify("giving up on config rom for node id %x\n",
 				  device->node_id);
 			if (device->node == device->card->root_node)
-				schedule_delayed_work(&device->card->work, 0);
+				fw_schedule_bm_work(device->card, 0);
 			fw_device_release(&device->device);
 		}
 		return;
@@ -758,7 +758,7 @@ static void fw_device_init(struct work_struct *work)
 	 * pretty harmless.
 	 */
 	if (device->node == device->card->root_node)
-		schedule_delayed_work(&device->card->work, 0);
+		fw_schedule_bm_work(device->card, 0);
 
 	return;
 
@@ -892,7 +892,7 @@ static void fw_device_refresh(struct work_struct *work)
 	fw_device_shutdown(work);
  out:
 	if (node_id == card->root_node->node_id)
-		schedule_delayed_work(&card->work, 0);
+		fw_schedule_bm_work(card, 0);
 }
 
 void fw_node_event(struct fw_card *card, struct fw_node *node, int event)
diff --git a/drivers/firewire/fw-device.h b/drivers/firewire/fw-device.h
index 42305bbac72f..df51732608d9 100644
--- a/drivers/firewire/fw-device.h
+++ b/drivers/firewire/fw-device.h
@@ -21,6 +21,7 @@
 
 #include <linux/fs.h>
 #include <linux/cdev.h>
+#include <linux/idr.h>
 #include <linux/rwsem.h>
 #include <asm/atomic.h>
 
@@ -99,6 +100,7 @@ void fw_device_cdev_update(struct fw_device *device);
 void fw_device_cdev_remove(struct fw_device *device);
 
 extern struct rw_semaphore fw_device_rwsem;
+extern struct idr fw_device_idr;
 extern int fw_cdev_major;
 
 /*
diff --git a/drivers/firewire/fw-sbp2.c b/drivers/firewire/fw-sbp2.c
index e54403ee59e7..e88d5067448c 100644
--- a/drivers/firewire/fw-sbp2.c
+++ b/drivers/firewire/fw-sbp2.c
@@ -670,17 +670,6 @@ static void sbp2_agent_reset_no_wait(struct sbp2_logical_unit *lu)
 			&d, sizeof(d), complete_agent_reset_write_no_wait, t);
 }
 
-static void sbp2_set_generation(struct sbp2_logical_unit *lu, int generation)
-{
-	struct fw_card *card = fw_device(lu->tgt->unit->device.parent)->card;
-	unsigned long flags;
-
-	/* serialize with comparisons of lu->generation and card->generation */
-	spin_lock_irqsave(&card->lock, flags);
-	lu->generation = generation;
-	spin_unlock_irqrestore(&card->lock, flags);
-}
-
 static inline void sbp2_allow_block(struct sbp2_logical_unit *lu)
 {
 	/*
@@ -884,7 +873,7 @@ static void sbp2_login(struct work_struct *work)
 		goto out;
 
 	generation    = device->generation;
-	smp_rmb();    /* node_id must not be older than generation */
+	smp_rmb();    /* node IDs must not be older than generation */
 	node_id       = device->node_id;
 	local_node_id = device->card->node_id;
 
@@ -908,7 +897,8 @@ static void sbp2_login(struct work_struct *work)
 
 	tgt->node_id	  = node_id;
 	tgt->address_high = local_node_id << 16;
-	sbp2_set_generation(lu, generation);
+	smp_wmb();	  /* node IDs must not be older than generation */
+	lu->generation	  = generation;
 
 	lu->command_block_agent_address =
 		((u64)(be32_to_cpu(response.command_block_agent.high) & 0xffff)
@@ -1201,7 +1191,7 @@ static void sbp2_reconnect(struct work_struct *work)
 		goto out;
 
 	generation    = device->generation;
-	smp_rmb();    /* node_id must not be older than generation */
+	smp_rmb();    /* node IDs must not be older than generation */
 	node_id       = device->node_id;
 	local_node_id = device->card->node_id;
 
@@ -1228,7 +1218,8 @@ static void sbp2_reconnect(struct work_struct *work)
 
 	tgt->node_id      = node_id;
 	tgt->address_high = local_node_id << 16;
-	sbp2_set_generation(lu, generation);
+	smp_wmb();	  /* node IDs must not be older than generation */
+	lu->generation	  = generation;
 
 	fw_notify("%s: reconnected to LUN %04x (%d retries)\n",
 		  tgt->bus_id, lu->lun, lu->retries);
diff --git a/drivers/firewire/fw-topology.c b/drivers/firewire/fw-topology.c
index 5e204713002d..c9be6e6948c4 100644
--- a/drivers/firewire/fw-topology.c
+++ b/drivers/firewire/fw-topology.c
@@ -355,6 +355,9 @@ report_lost_node(struct fw_card *card,
 {
 	fw_node_event(card, node, FW_NODE_DESTROYED);
 	fw_node_put(node);
+
+	/* Topology has changed - reset bus manager retry counter */
+	card->bm_retries = 0;
 }
 
 static void
@@ -374,6 +377,9 @@ report_found_node(struct fw_card *card,
 	}
 
 	fw_node_event(card, node, FW_NODE_CREATED);
+
+	/* Topology has changed - reset bus manager retry counter */
+	card->bm_retries = 0;
 }
 
 void fw_destroy_nodes(struct fw_card *card)
@@ -514,14 +520,6 @@ fw_core_handle_bus_reset(struct fw_card *card,
 
 	spin_lock_irqsave(&card->lock, flags);
 
-	/*
-	 * If the new topology has a different self_id_count the topology
-	 * changed, either nodes were added or removed. In that case we
-	 * reset the IRM reset counter.
-	 */
-	if (card->self_id_count != self_id_count)
-		card->bm_retries = 0;
-
 	card->node_id = node_id;
 	/*
 	 * Update node_id before generation to prevent anybody from using
@@ -530,7 +528,7 @@ fw_core_handle_bus_reset(struct fw_card *card,
 	smp_wmb();
 	card->generation = generation;
 	card->reset_jiffies = jiffies;
-	schedule_delayed_work(&card->work, 0);
+	fw_schedule_bm_work(card, 0);
 
 	local_node = build_tree(card, self_ids, self_id_count);
 
diff --git a/drivers/firewire/fw-transaction.c b/drivers/firewire/fw-transaction.c
index 2884f876397b..699ac041f39a 100644
--- a/drivers/firewire/fw-transaction.c
+++ b/drivers/firewire/fw-transaction.c
@@ -19,6 +19,7 @@
  */
 
 #include <linux/completion.h>
+#include <linux/idr.h>
 #include <linux/kernel.h>
 #include <linux/kref.h>
 #include <linux/module.h>
@@ -971,6 +972,7 @@ static void __exit fw_core_cleanup(void)
 {
 	unregister_chrdev(fw_cdev_major, "firewire");
 	bus_unregister(&fw_bus_type);
+	idr_destroy(&fw_device_idr);
 }
 
 module_init(fw_core_init);
diff --git a/drivers/firewire/fw-transaction.h b/drivers/firewire/fw-transaction.h
index 839466f0a795..c9ab12a15f6e 100644
--- a/drivers/firewire/fw-transaction.h
+++ b/drivers/firewire/fw-transaction.h
@@ -237,14 +237,6 @@ struct fw_card {
 	int link_speed;
 	int config_rom_generation;
 
-	/*
-	 * We need to store up to 4 self ID for a maximum of 63
-	 * devices plus 3 words for the topology map header.
-	 */
-	int self_id_count;
-	u32 topology_map[252 + 3];
-	u32 broadcast_channel;
-
 	spinlock_t lock; /* Take this lock when handling the lists in
 			  * this struct. */
 	struct fw_node *local_node;
@@ -262,6 +254,9 @@ struct fw_card {
 	struct delayed_work work;
 	int bm_retries;
 	int bm_generation;
+
+	u32 broadcast_channel;
+	u32 topology_map[(CSR_TOPOLOGY_MAP_END - CSR_TOPOLOGY_MAP) / 4];
 };
 
 static inline struct fw_card *fw_card_get(struct fw_card *card)
@@ -278,6 +273,8 @@ static inline void fw_card_put(struct fw_card *card)
 	kref_put(&card->kref, fw_card_release);
 }
 
+extern void fw_schedule_bm_work(struct fw_card *card, unsigned long delay);
+
 /*
  * The iso packet format allows for an immediate header/payload part
  * stored in 'header' immediately after the packet info plus an
diff --git a/drivers/firmware/dmi-id.c b/drivers/firmware/dmi-id.c
index e880d6c8d896..5a76d056b9d0 100644
--- a/drivers/firmware/dmi-id.c
+++ b/drivers/firmware/dmi-id.c
@@ -223,7 +223,7 @@ static int __init dmi_id_init(void)
 	}
 
 	dmi_dev->class = &dmi_class;
-	strcpy(dmi_dev->bus_id, "id");
+	dev_set_name(dmi_dev, "id");
 	dmi_dev->groups = sys_dmi_attribute_groups;
 
 	ret = device_register(dmi_dev);
diff --git a/drivers/firmware/dmi_scan.c b/drivers/firmware/dmi_scan.c
index 78b989d202a3..d76adfea5df7 100644
--- a/drivers/firmware/dmi_scan.c
+++ b/drivers/firmware/dmi_scan.c
@@ -468,8 +468,8 @@ const char *dmi_get_system_info(int field)
 EXPORT_SYMBOL(dmi_get_system_info);
 
 /**
- *	dmi_name_in_serial - 	Check if string is in the DMI product serial
- *				information.
+ * dmi_name_in_serial - Check if string is in the DMI product serial information
+ * @str: string to check for
  */
 int dmi_name_in_serial(const char *str)
 {
@@ -585,6 +585,8 @@ EXPORT_SYMBOL_GPL(dmi_walk);
 
 /**
  * dmi_match - compare a string to the dmi field (if exists)
+ * @f: DMI field identifier
+ * @str: string to compare the DMI field to
  *
  * Returns true if the requested field equals to the str (including NULL).
  */
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index 48f49d93d249..3d2565441b36 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -95,7 +95,7 @@ config GPIO_MAX732X
 	  number for these GPIOs.
 
 config GPIO_PCA953X
-	tristate "PCA953x, PCA955x, and MAX7310 I/O ports"
+	tristate "PCA953x, PCA955x, TCA64xx, and MAX7310 I/O ports"
 	depends on I2C
 	help
 	  Say yes here to provide access to several register-oriented
@@ -104,9 +104,10 @@ config GPIO_PCA953X
 
 	  4 bits:	pca9536, pca9537
 
-	  8 bits:	max7310, pca9534, pca9538, pca9554, pca9557
+	  8 bits:	max7310, pca9534, pca9538, pca9554, pca9557,
+	  		tca6408
 
-	  16 bits:	pca9535, pca9539, pca9555
+	  16 bits:	pca9535, pca9539, pca9555, tca6416
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called pca953x.
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index 82020abc329e..35e7aea4222c 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -1213,7 +1213,7 @@ static int gpiolib_show(struct seq_file *s, void *unused)
 		if (dev)
 			seq_printf(s, ", %s/%s",
 				dev->bus ? dev->bus->name : "no-bus",
-				dev->bus_id);
+				dev_name(dev));
 		if (chip->label)
 			seq_printf(s, ", %s", chip->label);
 		if (chip->can_sleep)
diff --git a/drivers/gpio/pca953x.c b/drivers/gpio/pca953x.c
index 9ceeb89f1325..37f35388a2ae 100644
--- a/drivers/gpio/pca953x.c
+++ b/drivers/gpio/pca953x.c
@@ -33,7 +33,12 @@ static const struct i2c_device_id pca953x_id[] = {
 	{ "pca9554", 8, },
 	{ "pca9555", 16, },
 	{ "pca9557", 8, },
+
 	{ "max7310", 8, },
+	{ "pca6107", 8, },
+	{ "tca6408", 8, },
+	{ "tca6416", 16, },
+	/* NYET:  { "tca6424", 24, }, */
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, pca953x_id);
@@ -47,9 +52,6 @@ struct pca953x_chip {
 	struct gpio_chip gpio_chip;
 };
 
-/* NOTE:  we can't currently rely on fault codes to come from SMBus
- * calls, so we map all errors to EIO here and return zero otherwise.
- */
 static int pca953x_write_reg(struct pca953x_chip *chip, int reg, uint16_t val)
 {
 	int ret;
@@ -61,7 +63,7 @@ static int pca953x_write_reg(struct pca953x_chip *chip, int reg, uint16_t val)
 
 	if (ret < 0) {
 		dev_err(&chip->client->dev, "failed writing register\n");
-		return -EIO;
+		return ret;
 	}
 
 	return 0;
@@ -78,7 +80,7 @@ static int pca953x_read_reg(struct pca953x_chip *chip, int reg, uint16_t *val)
 
 	if (ret < 0) {
 		dev_err(&chip->client->dev, "failed reading register\n");
-		return -EIO;
+		return ret;
 	}
 
 	*val = (uint16_t)ret;
diff --git a/drivers/gpio/twl4030-gpio.c b/drivers/gpio/twl4030-gpio.c
index 37d3eec8730a..afad14792141 100644
--- a/drivers/gpio/twl4030-gpio.c
+++ b/drivers/gpio/twl4030-gpio.c
@@ -202,37 +202,6 @@ static int twl4030_get_gpio_datain(int gpio)
 	return ret;
 }
 
-/*
- * Configure debounce timing value for a GPIO pin on TWL4030
- */
-int twl4030_set_gpio_debounce(int gpio, int enable)
-{
-	u8 d_bnk = gpio >> 3;
-	u8 d_msk = BIT(gpio & 0x7);
-	u8 reg = 0;
-	u8 base = 0;
-	int ret = 0;
-
-	if (unlikely((gpio >= TWL4030_GPIO_MAX)
-		|| !(gpio_usage_count & BIT(gpio))))
-		return -EPERM;
-
-	base = REG_GPIO_DEBEN1 + d_bnk;
-	mutex_lock(&gpio_lock);
-	ret = gpio_twl4030_read(base);
-	if (ret >= 0) {
-		if (enable)
-			reg = ret | d_msk;
-		else
-			reg = ret & ~d_msk;
-
-		ret = gpio_twl4030_write(base, reg);
-	}
-	mutex_unlock(&gpio_lock);
-	return ret;
-}
-EXPORT_SYMBOL(twl4030_set_gpio_debounce);
-
 /*----------------------------------------------------------------------*/
 
 static int twl_request(struct gpio_chip *chip, unsigned offset)
@@ -405,6 +374,23 @@ static int __devinit gpio_twl4030_pulls(u32 ups, u32 downs)
 				REG_GPIOPUPDCTR1, 5);
 }
 
+static int __devinit gpio_twl4030_debounce(u32 debounce, u8 mmc_cd)
+{
+	u8		message[4];
+
+	/* 30 msec of debouncing is always used for MMC card detect,
+	 * and is optional for everything else.
+	 */
+	message[1] = (debounce & 0xff) | (mmc_cd & 0x03);
+	debounce >>= 8;
+	message[2] = (debounce & 0xff);
+	debounce >>= 8;
+	message[3] = (debounce & 0x03);
+
+	return twl4030_i2c_write(TWL4030_MODULE_GPIO, message,
+				REG_GPIO_DEBEN1, 3);
+}
+
 static int gpio_twl4030_remove(struct platform_device *pdev);
 
 static int __devinit gpio_twl4030_probe(struct platform_device *pdev)
@@ -439,6 +425,12 @@ no_irqs:
 				pdata->pullups, pdata->pulldowns,
 				ret);
 
+	ret = gpio_twl4030_debounce(pdata->debounce, pdata->mmc_cd);
+	if (ret)
+		dev_dbg(&pdev->dev, "debounce %.03x %.01x --> %d\n",
+				pdata->debounce, pdata->mmc_cd,
+				ret);
+
 	twl_gpiochip.base = pdata->gpio_base;
 	twl_gpiochip.ngpio = TWL4030_GPIO_MAX;
 	twl_gpiochip.dev = &pdev->dev;
diff --git a/drivers/gpu/drm/drm_crtc.c b/drivers/gpu/drm/drm_crtc.c
index 53c87254be4c..5b2cbb778162 100644
--- a/drivers/gpu/drm/drm_crtc.c
+++ b/drivers/gpu/drm/drm_crtc.c
@@ -210,7 +210,7 @@ static int drm_mode_object_get(struct drm_device *dev,
 	int ret;
 
 	WARN(!mutex_is_locked(&dev->mode_config.mutex),
-	     "%s called w/o mode_config lock\n", __FUNCTION__);
+	     "%s called w/o mode_config lock\n", __func__);
 again:
 	if (idr_pre_get(&dev->mode_config.crtc_idr, GFP_KERNEL) == 0) {
 		DRM_ERROR("Ran out memory getting a mode number\n");
diff --git a/drivers/gpu/drm/drm_drv.c b/drivers/gpu/drm/drm_drv.c
index febb517ee679..5ff88d952226 100644
--- a/drivers/gpu/drm/drm_drv.c
+++ b/drivers/gpu/drm/drm_drv.c
@@ -314,14 +314,14 @@ static void drm_cleanup(struct drm_device * dev)
 		DRM_DEBUG("mtrr_del=%d\n", retval);
 	}
 
+	if (dev->driver->unload)
+		dev->driver->unload(dev);
+
 	if (drm_core_has_AGP(dev) && dev->agp) {
 		drm_free(dev->agp, sizeof(*dev->agp), DRM_MEM_AGPLISTS);
 		dev->agp = NULL;
 	}
 
-	if (dev->driver->unload)
-		dev->driver->unload(dev);
-
 	drm_ht_remove(&dev->map_hash);
 	drm_ctxbitmap_cleanup(dev);
 
diff --git a/drivers/gpu/drm/drm_fops.c b/drivers/gpu/drm/drm_fops.c
index 3733e36d135e..b06a53715853 100644
--- a/drivers/gpu/drm/drm_fops.c
+++ b/drivers/gpu/drm/drm_fops.c
@@ -183,6 +183,10 @@ int drm_stub_open(struct inode *inode, struct file *filp)
 
 	old_fops = filp->f_op;
 	filp->f_op = fops_get(&dev->driver->fops);
+	if (filp->f_op == NULL) {
+		filp->f_op = old_fops;
+		goto out;
+	}
 	if (filp->f_op->open && (err = filp->f_op->open(inode, filp))) {
 		fops_put(filp->f_op);
 		filp->f_op = fops_get(old_fops);
diff --git a/drivers/gpu/drm/drm_sysfs.c b/drivers/gpu/drm/drm_sysfs.c
index 65d72d094c81..5aa6780652aa 100644
--- a/drivers/gpu/drm/drm_sysfs.c
+++ b/drivers/gpu/drm/drm_sysfs.c
@@ -488,7 +488,7 @@ int drm_sysfs_device_add(struct drm_minor *minor)
         else
                 minor_str = "card%d";
 
-	snprintf(minor->kdev.bus_id, BUS_ID_SIZE, minor_str, minor->index);
+	dev_set_name(&minor->kdev, minor_str, minor->index);
 
 	err = device_register(&minor->kdev);
 	if (err) {
diff --git a/drivers/gpu/drm/i915/Makefile b/drivers/gpu/drm/i915/Makefile
index dd57a5bd4572..793cba39d832 100644
--- a/drivers/gpu/drm/i915/Makefile
+++ b/drivers/gpu/drm/i915/Makefile
@@ -13,6 +13,7 @@ i915-y := i915_drv.o i915_dma.o i915_irq.o i915_mem.o \
 	  intel_crt.o \
 	  intel_lvds.o \
 	  intel_bios.o \
+	  intel_hdmi.o \
 	  intel_sdvo.o \
 	  intel_modes.o \
 	  intel_i2c.o \
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index 3d7082af5b72..62a4bf7b49df 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -827,6 +827,7 @@ static int i915_probe_agp(struct drm_device *dev, unsigned long *aperture_size,
 	struct pci_dev *bridge_dev;
 	u16 tmp = 0;
 	unsigned long overhead;
+	unsigned long stolen;
 
 	bridge_dev = pci_get_bus_and_slot(0, PCI_DEVFN(0,0));
 	if (!bridge_dev) {
@@ -866,36 +867,55 @@ static int i915_probe_agp(struct drm_device *dev, unsigned long *aperture_size,
 	else
 		overhead = (*aperture_size / 1024) + 4096;
 
-	switch (tmp & INTEL_855_GMCH_GMS_MASK) {
+	switch (tmp & INTEL_GMCH_GMS_MASK) {
+	case INTEL_855_GMCH_GMS_DISABLED:
+		DRM_ERROR("video memory is disabled\n");
+		return -1;
 	case INTEL_855_GMCH_GMS_STOLEN_1M:
-		break; /* 1M already */
+		stolen = 1 * 1024 * 1024;
+		break;
 	case INTEL_855_GMCH_GMS_STOLEN_4M:
-		*preallocated_size *= 4;
+		stolen = 4 * 1024 * 1024;
 		break;
 	case INTEL_855_GMCH_GMS_STOLEN_8M:
-		*preallocated_size *= 8;
+		stolen = 8 * 1024 * 1024;
 		break;
 	case INTEL_855_GMCH_GMS_STOLEN_16M:
-		*preallocated_size *= 16;
+		stolen = 16 * 1024 * 1024;
 		break;
 	case INTEL_855_GMCH_GMS_STOLEN_32M:
-		*preallocated_size *= 32;
+		stolen = 32 * 1024 * 1024;
 		break;
 	case INTEL_915G_GMCH_GMS_STOLEN_48M:
-		*preallocated_size *= 48;
+		stolen = 48 * 1024 * 1024;
 		break;
 	case INTEL_915G_GMCH_GMS_STOLEN_64M:
-		*preallocated_size *= 64;
+		stolen = 64 * 1024 * 1024;
+		break;
+	case INTEL_GMCH_GMS_STOLEN_128M:
+		stolen = 128 * 1024 * 1024;
+		break;
+	case INTEL_GMCH_GMS_STOLEN_256M:
+		stolen = 256 * 1024 * 1024;
+		break;
+	case INTEL_GMCH_GMS_STOLEN_96M:
+		stolen = 96 * 1024 * 1024;
+		break;
+	case INTEL_GMCH_GMS_STOLEN_160M:
+		stolen = 160 * 1024 * 1024;
+		break;
+	case INTEL_GMCH_GMS_STOLEN_224M:
+		stolen = 224 * 1024 * 1024;
+		break;
+	case INTEL_GMCH_GMS_STOLEN_352M:
+		stolen = 352 * 1024 * 1024;
 		break;
-	case INTEL_855_GMCH_GMS_DISABLED:
-		DRM_ERROR("video memory is disabled\n");
-		return -1;
 	default:
 		DRM_ERROR("unexpected GMCH_GMS value: 0x%02x\n",
-			tmp & INTEL_855_GMCH_GMS_MASK);
+			tmp & INTEL_GMCH_GMS_MASK);
 		return -1;
 	}
-	*preallocated_size -= overhead;
+	*preallocated_size = stolen - overhead;
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 4756e5cd6b5e..563de18063fd 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -664,6 +664,7 @@ extern void intel_modeset_cleanup(struct drm_device *dev);
 				 writel(upper_32_bits(val), dev_priv->regs + \
 					(reg) + 4))
 #endif
+#define POSTING_READ(reg)	(void)I915_READ(reg)
 
 #define I915_VERBOSE 0
 
@@ -760,6 +761,7 @@ extern int i915_wait_ring(struct drm_device * dev, int n, const char *caller);
 			IS_I945GM(dev) || IS_I965GM(dev) || IS_GM45(dev))
 
 #define I915_NEED_GFX_HWS(dev) (IS_G33(dev) || IS_GM45(dev) || IS_G4X(dev))
+#define SUPPORTS_INTEGRATED_HDMI(dev)	(IS_G4X(dev))
 
 #define PRIMARY_RINGBUFFER_SIZE         (128*1024)
 
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index cc2ca5561feb..1384d6686555 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -1445,7 +1445,7 @@ static void i915_write_fence_reg(struct drm_i915_fence_reg *reg)
 
 	if ((obj_priv->gtt_offset & ~I915_FENCE_START_MASK) ||
 	    (obj_priv->gtt_offset & (obj->size - 1))) {
-		WARN(1, "%s: object not 1M or size aligned\n", __FUNCTION__);
+		WARN(1, "%s: object not 1M or size aligned\n", __func__);
 		return;
 	}
 
@@ -1478,7 +1478,7 @@ static void i830_write_fence_reg(struct drm_i915_fence_reg *reg)
 
 	if ((obj_priv->gtt_offset & ~I915_FENCE_START_MASK) ||
 	    (obj_priv->gtt_offset & (obj->size - 1))) {
-		WARN(1, "%s: object not 1M or size aligned\n", __FUNCTION__);
+		WARN(1, "%s: object not 1M or size aligned\n", __func__);
 		return;
 	}
 
@@ -1623,6 +1623,8 @@ i915_gem_object_bind_to_gtt(struct drm_gem_object *obj, unsigned alignment)
 	struct drm_mm_node *free_space;
 	int page_count, ret;
 
+	if (dev_priv->mm.suspended)
+		return -EBUSY;
 	if (alignment == 0)
 		alignment = PAGE_SIZE;
 	if (alignment & (PAGE_SIZE - 1)) {
@@ -2641,7 +2643,7 @@ i915_gem_object_pin(struct drm_gem_object *obj, uint32_t alignment)
 	if (obj_priv->gtt_space == NULL) {
 		ret = i915_gem_object_bind_to_gtt(obj, alignment);
 		if (ret != 0) {
-			if (ret != -ERESTARTSYS)
+			if (ret != -EBUSY && ret != -ERESTARTSYS)
 				DRM_ERROR("Failure to bind: %d", ret);
 			return ret;
 		}
@@ -3219,20 +3221,21 @@ i915_gem_entervt_ioctl(struct drm_device *dev, void *data,
 		dev_priv->mm.wedged = 0;
 	}
 
-	ret = i915_gem_init_ringbuffer(dev);
-	if (ret != 0)
-		return ret;
-
 	dev_priv->mm.gtt_mapping = io_mapping_create_wc(dev->agp->base,
 							dev->agp->agp_info.aper_size
 							* 1024 * 1024);
 
 	mutex_lock(&dev->struct_mutex);
+	dev_priv->mm.suspended = 0;
+
+	ret = i915_gem_init_ringbuffer(dev);
+	if (ret != 0)
+		return ret;
+
 	BUG_ON(!list_empty(&dev_priv->mm.active_list));
 	BUG_ON(!list_empty(&dev_priv->mm.flushing_list));
 	BUG_ON(!list_empty(&dev_priv->mm.inactive_list));
 	BUG_ON(!list_empty(&dev_priv->mm.request_list));
-	dev_priv->mm.suspended = 0;
 	mutex_unlock(&dev->struct_mutex);
 
 	drm_irq_install(dev);
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index 47e6bafeb743..273162579e1b 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -35,7 +35,7 @@
 #define INTEL_GMCH_MEM_64M	0x1
 #define INTEL_GMCH_MEM_128M	0
 
-#define INTEL_855_GMCH_GMS_MASK		(0x7 << 4)
+#define INTEL_GMCH_GMS_MASK		(0xf << 4)
 #define INTEL_855_GMCH_GMS_DISABLED	(0x0 << 4)
 #define INTEL_855_GMCH_GMS_STOLEN_1M	(0x1 << 4)
 #define INTEL_855_GMCH_GMS_STOLEN_4M	(0x2 << 4)
@@ -45,6 +45,12 @@
 
 #define INTEL_915G_GMCH_GMS_STOLEN_48M	(0x6 << 4)
 #define INTEL_915G_GMCH_GMS_STOLEN_64M	(0x7 << 4)
+#define INTEL_GMCH_GMS_STOLEN_128M	(0x8 << 4)
+#define INTEL_GMCH_GMS_STOLEN_256M	(0x9 << 4)
+#define INTEL_GMCH_GMS_STOLEN_96M	(0xa << 4)
+#define INTEL_GMCH_GMS_STOLEN_160M	(0xb << 4)
+#define INTEL_GMCH_GMS_STOLEN_224M	(0xc << 4)
+#define INTEL_GMCH_GMS_STOLEN_352M	(0xd << 4)
 
 /* PCI config space */
 
@@ -549,6 +555,8 @@
 /** GM965 GM45 render standby register */
 #define MCHBAR_RENDER_STANDBY	0x111B8
 
+#define PEG_BAND_GAP_DATA	0x14d68
+
 /*
  * Overlay regs
  */
@@ -612,6 +620,9 @@
 
 /* Hotplug control (945+ only) */
 #define PORT_HOTPLUG_EN		0x61110
+#define   HDMIB_HOTPLUG_INT_EN			(1 << 29)
+#define   HDMIC_HOTPLUG_INT_EN			(1 << 28)
+#define   HDMID_HOTPLUG_INT_EN			(1 << 27)
 #define   SDVOB_HOTPLUG_INT_EN			(1 << 26)
 #define   SDVOC_HOTPLUG_INT_EN			(1 << 25)
 #define   TV_HOTPLUG_INT_EN			(1 << 18)
@@ -619,6 +630,9 @@
 #define   CRT_HOTPLUG_FORCE_DETECT		(1 << 3)
 
 #define PORT_HOTPLUG_STAT	0x61114
+#define   HDMIB_HOTPLUG_INT_STATUS		(1 << 29)
+#define   HDMIC_HOTPLUG_INT_STATUS		(1 << 28)
+#define   HDMID_HOTPLUG_INT_STATUS		(1 << 27)
 #define   CRT_HOTPLUG_INT_STATUS		(1 << 11)
 #define   TV_HOTPLUG_INT_STATUS			(1 << 10)
 #define   CRT_HOTPLUG_MONITOR_MASK		(3 << 8)
@@ -648,7 +662,16 @@
 #define   SDVO_PHASE_SELECT_DEFAULT	(6 << 19)
 #define   SDVO_CLOCK_OUTPUT_INVERT	(1 << 18)
 #define   SDVOC_GANG_MODE		(1 << 16)
+#define   SDVO_ENCODING_SDVO		(0x0 << 10)
+#define   SDVO_ENCODING_HDMI		(0x2 << 10)
+/** Requird for HDMI operation */
+#define   SDVO_NULL_PACKETS_DURING_VSYNC (1 << 9)
 #define   SDVO_BORDER_ENABLE		(1 << 7)
+#define   SDVO_AUDIO_ENABLE		(1 << 6)
+/** New with 965, default is to be set */
+#define   SDVO_VSYNC_ACTIVE_HIGH	(1 << 4)
+/** New with 965, default is to be set */
+#define   SDVO_HSYNC_ACTIVE_HIGH	(1 << 3)
 #define   SDVOB_PCIE_CONCURRENCY	(1 << 3)
 #define   SDVO_DETECTED			(1 << 2)
 /* Bits to be preserved when writing */
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index e5c1c80d1f90..8ccb9c3ab868 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -751,6 +751,7 @@ static void intel_crtc_mode_set(struct drm_crtc *crtc,
 			is_lvds = true;
 			break;
 		case INTEL_OUTPUT_SDVO:
+		case INTEL_OUTPUT_HDMI:
 			is_sdvo = true;
 			break;
 		case INTEL_OUTPUT_DVO:
@@ -986,19 +987,17 @@ static int intel_crtc_cursor_set(struct drm_crtc *crtc,
 	uint32_t base = (pipe == 0) ? CURABASE : CURBBASE;
 	uint32_t temp;
 	size_t addr;
+	int ret;
 
 	DRM_DEBUG("\n");
 
 	/* if we want to turn off the cursor ignore width and height */
 	if (!handle) {
 		DRM_DEBUG("cursor off\n");
-		/* turn of the cursor */
-		temp = 0;
-		temp |= CURSOR_MODE_DISABLE;
-
-		I915_WRITE(control, temp);
-		I915_WRITE(base, 0);
-		return 0;
+		temp = CURSOR_MODE_DISABLE;
+		addr = 0;
+		bo = NULL;
+		goto finish;
 	}
 
 	/* Currently we only support 64x64 cursors */
@@ -1025,15 +1024,30 @@ static int intel_crtc_cursor_set(struct drm_crtc *crtc,
 		addr = obj_priv->gtt_offset;
 	}
 
-	intel_crtc->cursor_addr = addr;
+	ret = i915_gem_object_pin(bo, PAGE_SIZE);
+	if (ret) {
+		DRM_ERROR("failed to pin cursor bo\n");
+		drm_gem_object_unreference(bo);
+		return ret;
+	}
+
 	temp = 0;
 	/* set the pipe for the cursor */
 	temp |= (pipe << 28);
 	temp |= CURSOR_MODE_64_ARGB_AX | MCURSOR_GAMMA_ENABLE;
 
+ finish:
 	I915_WRITE(control, temp);
 	I915_WRITE(base, addr);
 
+	if (intel_crtc->cursor_bo) {
+		i915_gem_object_unpin(intel_crtc->cursor_bo);
+		drm_gem_object_unreference(intel_crtc->cursor_bo);
+	}
+
+	intel_crtc->cursor_addr = addr;
+	intel_crtc->cursor_bo = bo;
+
 	return 0;
 }
 
@@ -1430,12 +1444,19 @@ static void intel_setup_outputs(struct drm_device *dev)
 		intel_lvds_init(dev);
 
 	if (IS_I9XX(dev)) {
-		intel_sdvo_init(dev, SDVOB);
-		intel_sdvo_init(dev, SDVOC);
+		int found;
+
+		found = intel_sdvo_init(dev, SDVOB);
+		if (!found && SUPPORTS_INTEGRATED_HDMI(dev))
+			intel_hdmi_init(dev, SDVOB);
+
+		found = intel_sdvo_init(dev, SDVOC);
+		if (!found && SUPPORTS_INTEGRATED_HDMI(dev))
+			intel_hdmi_init(dev, SDVOC);
 	} else
 		intel_dvo_init(dev);
 
-	if (IS_I9XX(dev) && !IS_I915G(dev))
+	if (IS_I9XX(dev) && IS_MOBILE(dev))
 		intel_tv_init(dev);
 
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
@@ -1445,6 +1466,11 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 		/* valid crtcs */
 		switch(intel_output->type) {
+		case INTEL_OUTPUT_HDMI:
+			crtc_mask = ((1 << 0)|
+				     (1 << 1));
+			clone_mask = ((1 << INTEL_OUTPUT_HDMI));
+			break;
 		case INTEL_OUTPUT_DVO:
 		case INTEL_OUTPUT_SDVO:
 			crtc_mask = ((1 << 0)|
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index 407edd5bf582..8a4cc50c5b4e 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -53,6 +53,7 @@
 #define INTEL_OUTPUT_SDVO 3
 #define INTEL_OUTPUT_LVDS 4
 #define INTEL_OUTPUT_TVOUT 5
+#define INTEL_OUTPUT_HDMI 6
 
 #define INTEL_DVO_CHIP_NONE 0
 #define INTEL_DVO_CHIP_LVDS 1
@@ -88,6 +89,7 @@ struct intel_crtc {
 	struct drm_crtc base;
 	int pipe;
 	int plane;
+	struct drm_gem_object *cursor_bo;
 	uint32_t cursor_addr;
 	u8 lut_r[256], lut_g[256], lut_b[256];
 	int dpms_mode;
@@ -108,7 +110,8 @@ int intel_ddc_get_modes(struct intel_output *intel_output);
 extern bool intel_ddc_probe(struct intel_output *intel_output);
 
 extern void intel_crt_init(struct drm_device *dev);
-extern void intel_sdvo_init(struct drm_device *dev, int output_device);
+extern void intel_hdmi_init(struct drm_device *dev, int sdvox_reg);
+extern bool intel_sdvo_init(struct drm_device *dev, int output_device);
 extern void intel_dvo_init(struct drm_device *dev);
 extern void intel_tv_init(struct drm_device *dev);
 extern void intel_lvds_init(struct drm_device *dev);
diff --git a/drivers/gpu/drm/i915/intel_hdmi.c b/drivers/gpu/drm/i915/intel_hdmi.c
new file mode 100644
index 000000000000..b06a4a3ff08d
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_hdmi.c
@@ -0,0 +1,280 @@
+/*
+ * Copyright 2006 Dave Airlie <airlied@linux.ie>
+ * Copyright © 2006-2009 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * Authors:
+ *	Eric Anholt <eric@anholt.net>
+ *	Jesse Barnes <jesse.barnes@intel.com>
+ */
+
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include "drmP.h"
+#include "drm.h"
+#include "drm_crtc.h"
+#include "intel_drv.h"
+#include "i915_drm.h"
+#include "i915_drv.h"
+
+struct intel_hdmi_priv {
+	u32 sdvox_reg;
+	u32 save_SDVOX;
+	int has_hdmi_sink;
+};
+
+static void intel_hdmi_mode_set(struct drm_encoder *encoder,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc = encoder->crtc;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_output *intel_output = enc_to_intel_output(encoder);
+	struct intel_hdmi_priv *hdmi_priv = intel_output->dev_priv;
+	u32 sdvox;
+
+	sdvox = SDVO_ENCODING_HDMI |
+		SDVO_BORDER_ENABLE |
+		SDVO_VSYNC_ACTIVE_HIGH |
+		SDVO_HSYNC_ACTIVE_HIGH;
+
+	if (hdmi_priv->has_hdmi_sink)
+		sdvox |= SDVO_AUDIO_ENABLE;
+
+	if (intel_crtc->pipe == 1)
+		sdvox |= SDVO_PIPE_B_SELECT;
+
+	I915_WRITE(hdmi_priv->sdvox_reg, sdvox);
+	POSTING_READ(hdmi_priv->sdvox_reg);
+}
+
+static void intel_hdmi_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_output *intel_output = enc_to_intel_output(encoder);
+	struct intel_hdmi_priv *hdmi_priv = intel_output->dev_priv;
+	u32 temp;
+
+	if (mode != DRM_MODE_DPMS_ON) {
+		temp = I915_READ(hdmi_priv->sdvox_reg);
+		I915_WRITE(hdmi_priv->sdvox_reg, temp & ~SDVO_ENABLE);
+	} else {
+		temp = I915_READ(hdmi_priv->sdvox_reg);
+		I915_WRITE(hdmi_priv->sdvox_reg, temp | SDVO_ENABLE);
+	}
+	POSTING_READ(hdmi_priv->sdvox_reg);
+}
+
+static void intel_hdmi_save(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_output *intel_output = to_intel_output(connector);
+	struct intel_hdmi_priv *hdmi_priv = intel_output->dev_priv;
+
+	hdmi_priv->save_SDVOX = I915_READ(hdmi_priv->sdvox_reg);
+}
+
+static void intel_hdmi_restore(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_output *intel_output = to_intel_output(connector);
+	struct intel_hdmi_priv *hdmi_priv = intel_output->dev_priv;
+
+	I915_WRITE(hdmi_priv->sdvox_reg, hdmi_priv->save_SDVOX);
+	POSTING_READ(hdmi_priv->sdvox_reg);
+}
+
+static int intel_hdmi_mode_valid(struct drm_connector *connector,
+				 struct drm_display_mode *mode)
+{
+	if (mode->clock > 165000)
+		return MODE_CLOCK_HIGH;
+	if (mode->clock < 20000)
+		return MODE_CLOCK_HIGH;
+
+	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
+		return MODE_NO_DBLESCAN;
+
+	return MODE_OK;
+}
+
+static bool intel_hdmi_mode_fixup(struct drm_encoder *encoder,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static enum drm_connector_status
+intel_hdmi_detect(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_output *intel_output = to_intel_output(connector);
+	struct intel_hdmi_priv *hdmi_priv = intel_output->dev_priv;
+	u32 temp, bit;
+
+	temp = I915_READ(PORT_HOTPLUG_EN);
+
+	I915_WRITE(PORT_HOTPLUG_EN,
+		   temp |
+		   HDMIB_HOTPLUG_INT_EN |
+		   HDMIC_HOTPLUG_INT_EN |
+		   HDMID_HOTPLUG_INT_EN);
+
+	POSTING_READ(PORT_HOTPLUG_EN);
+
+	switch (hdmi_priv->sdvox_reg) {
+	case SDVOB:
+		bit = HDMIB_HOTPLUG_INT_STATUS;
+		break;
+	case SDVOC:
+		bit = HDMIC_HOTPLUG_INT_STATUS;
+		break;
+	default:
+		return connector_status_unknown;
+	}
+
+	if ((I915_READ(PORT_HOTPLUG_STAT) & bit) != 0)
+		return connector_status_connected;
+	else
+		return connector_status_disconnected;
+}
+
+static int intel_hdmi_get_modes(struct drm_connector *connector)
+{
+	struct intel_output *intel_output = to_intel_output(connector);
+
+	/* We should parse the EDID data and find out if it's an HDMI sink so
+	 * we can send audio to it.
+	 */
+
+	return intel_ddc_get_modes(intel_output);
+}
+
+static void intel_hdmi_destroy(struct drm_connector *connector)
+{
+	struct intel_output *intel_output = to_intel_output(connector);
+
+	if (intel_output->i2c_bus)
+		intel_i2c_destroy(intel_output->i2c_bus);
+	drm_sysfs_connector_remove(connector);
+	drm_connector_cleanup(connector);
+	kfree(intel_output);
+}
+
+static const struct drm_encoder_helper_funcs intel_hdmi_helper_funcs = {
+	.dpms = intel_hdmi_dpms,
+	.mode_fixup = intel_hdmi_mode_fixup,
+	.prepare = intel_encoder_prepare,
+	.mode_set = intel_hdmi_mode_set,
+	.commit = intel_encoder_commit,
+};
+
+static const struct drm_connector_funcs intel_hdmi_connector_funcs = {
+	.save = intel_hdmi_save,
+	.restore = intel_hdmi_restore,
+	.detect = intel_hdmi_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.destroy = intel_hdmi_destroy,
+};
+
+static const struct drm_connector_helper_funcs intel_hdmi_connector_helper_funcs = {
+	.get_modes = intel_hdmi_get_modes,
+	.mode_valid = intel_hdmi_mode_valid,
+	.best_encoder = intel_best_encoder,
+};
+
+static void intel_hdmi_enc_destroy(struct drm_encoder *encoder)
+{
+	drm_encoder_cleanup(encoder);
+}
+
+static const struct drm_encoder_funcs intel_hdmi_enc_funcs = {
+	.destroy = intel_hdmi_enc_destroy,
+};
+
+
+void intel_hdmi_init(struct drm_device *dev, int sdvox_reg)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_connector *connector;
+	struct intel_output *intel_output;
+	struct intel_hdmi_priv *hdmi_priv;
+
+	intel_output = kcalloc(sizeof(struct intel_output) +
+			       sizeof(struct intel_hdmi_priv), 1, GFP_KERNEL);
+	if (!intel_output)
+		return;
+	hdmi_priv = (struct intel_hdmi_priv *)(intel_output + 1);
+
+	connector = &intel_output->base;
+	drm_connector_init(dev, connector, &intel_hdmi_connector_funcs,
+			   DRM_MODE_CONNECTOR_DVID);
+	drm_connector_helper_add(connector, &intel_hdmi_connector_helper_funcs);
+
+	intel_output->type = INTEL_OUTPUT_HDMI;
+
+	connector->interlace_allowed = 0;
+	connector->doublescan_allowed = 0;
+
+	/* Set up the DDC bus. */
+	if (sdvox_reg == SDVOB)
+		intel_output->ddc_bus = intel_i2c_create(dev, GPIOE, "HDMIB");
+	else
+		intel_output->ddc_bus = intel_i2c_create(dev, GPIOD, "HDMIC");
+
+	if (!intel_output->ddc_bus)
+		goto err_connector;
+
+	hdmi_priv->sdvox_reg = sdvox_reg;
+	intel_output->dev_priv = hdmi_priv;
+
+	drm_encoder_init(dev, &intel_output->enc, &intel_hdmi_enc_funcs,
+			 DRM_MODE_ENCODER_TMDS);
+	drm_encoder_helper_add(&intel_output->enc, &intel_hdmi_helper_funcs);
+
+	drm_mode_connector_attach_encoder(&intel_output->base,
+					  &intel_output->enc);
+	drm_sysfs_connector_add(connector);
+
+	/* For G4X desktop chip, PEG_BAND_GAP_DATA 3:0 must first be written
+	 * 0xd.  Failure to do so will result in spurious interrupts being
+	 * generated on the port when a cable is not attached.
+	 */
+	if (IS_G4X(dev) && !IS_GM45(dev)) {
+		u32 temp = I915_READ(PEG_BAND_GAP_DATA);
+		I915_WRITE(PEG_BAND_GAP_DATA, (temp & ~0xf) | 0xd);
+	}
+
+	return;
+
+err_connector:
+	drm_connector_cleanup(connector);
+	kfree(intel_output);
+
+	return;
+}
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index fbbaa4f414a0..407215469102 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -978,7 +978,7 @@ static const struct drm_encoder_funcs intel_sdvo_enc_funcs = {
 };
 
 
-void intel_sdvo_init(struct drm_device *dev, int output_device)
+bool intel_sdvo_init(struct drm_device *dev, int output_device)
 {
 	struct drm_connector *connector;
 	struct intel_output *intel_output;
@@ -991,7 +991,7 @@ void intel_sdvo_init(struct drm_device *dev, int output_device)
 
 	intel_output = kcalloc(sizeof(struct intel_output)+sizeof(struct intel_sdvo_priv), 1, GFP_KERNEL);
 	if (!intel_output) {
-		return;
+		return false;
 	}
 
 	connector = &intel_output->base;
@@ -1116,7 +1116,7 @@ void intel_sdvo_init(struct drm_device *dev, int output_device)
 
 	intel_output->ddc_bus = i2cbus;
 
-	return;
+	return true;
 
 err_i2c:
 	intel_i2c_destroy(intel_output->i2c_bus);
@@ -1124,5 +1124,5 @@ err_connector:
 	drm_connector_cleanup(connector);
 	kfree(intel_output);
 
-	return;
+	return false;
 }
diff --git a/drivers/hid/usbhid/hid-core.c b/drivers/hid/usbhid/hid-core.c
index 03cb494af1c5..f0a0f72238ab 100644
--- a/drivers/hid/usbhid/hid-core.c
+++ b/drivers/hid/usbhid/hid-core.c
@@ -102,7 +102,7 @@ static void hid_reset(struct work_struct *work)
 	struct usbhid_device *usbhid =
 		container_of(work, struct usbhid_device, reset_work);
 	struct hid_device *hid = usbhid->hid;
-	int rc_lock, rc = 0;
+	int rc = 0;
 
 	if (test_bit(HID_CLEAR_HALT, &usbhid->iofl)) {
 		dev_dbg(&usbhid->intf->dev, "clear halt\n");
@@ -113,11 +113,10 @@ static void hid_reset(struct work_struct *work)
 
 	else if (test_bit(HID_RESET_PENDING, &usbhid->iofl)) {
 		dev_dbg(&usbhid->intf->dev, "resetting device\n");
-		rc = rc_lock = usb_lock_device_for_reset(hid_to_usb_dev(hid), usbhid->intf);
-		if (rc_lock >= 0) {
+		rc = usb_lock_device_for_reset(hid_to_usb_dev(hid), usbhid->intf);
+		if (rc == 0) {
 			rc = usb_reset_device(hid_to_usb_dev(hid));
-			if (rc_lock)
-				usb_unlock_device(hid_to_usb_dev(hid));
+			usb_unlock_device(hid_to_usb_dev(hid));
 		}
 		clear_bit(HID_RESET_PENDING, &usbhid->iofl);
 	}
diff --git a/drivers/hid/usbhid/hiddev.c b/drivers/hid/usbhid/hiddev.c
index 6a98f9f572b0..d73eea382ab3 100644
--- a/drivers/hid/usbhid/hiddev.c
+++ b/drivers/hid/usbhid/hiddev.c
@@ -874,12 +874,14 @@ int hiddev_connect(struct hid_device *hid, unsigned int force)
 	INIT_LIST_HEAD(&hiddev->list);
 	spin_lock_init(&hiddev->list_lock);
 	mutex_init(&hiddev->existancelock);
+	hid->hiddev = hiddev;
 	hiddev->hid = hid;
 	hiddev->exist = 1;
 
 	retval = usb_register_dev(usbhid->intf, &hiddev_class);
 	if (retval) {
 		err_hid("Not able to get a minor for this device.");
+		hid->hiddev = NULL;
 		kfree(hiddev);
 		return -1;
 	} else {
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index c709e821f04b..4b33bc82cc24 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -284,11 +284,12 @@ config SENSORS_F71805F
 	  will be called f71805f.
 
 config SENSORS_F71882FG
-	tristate "Fintek F71882FG and F71883FG"
+	tristate "Fintek F71862FG, F71882FG and F8000"
 	depends on EXPERIMENTAL
 	help
 	  If you say yes here you get support for hardware monitoring
-	  features of the Fintek F71882FG and F71883FG Super-I/O chips.
+	  features of the Fintek F71882FG/F71883FG, F71862FG/71863FG
+	  and F8000 Super-I/O chips.
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called f71882fg.
@@ -304,9 +305,13 @@ config SENSORS_F75375S
 	  will be called f75375s.
 
 config SENSORS_FSCHER
-	tristate "FSC Hermes"
+	tristate "FSC Hermes (DEPRECATED)"
 	depends on X86 && I2C
 	help
+	  This driver is DEPRECATED please use the new merged fschmd
+	  ("FSC Poseidon, Scylla, Hermes, Heimdall and Heracles") driver
+	  instead.
+
 	  If you say yes here you get support for Fujitsu Siemens
 	  Computers Hermes sensor chips.
 
@@ -314,9 +319,13 @@ config SENSORS_FSCHER
 	  will be called fscher.
 
 config SENSORS_FSCPOS
-	tristate "FSC Poseidon"
+	tristate "FSC Poseidon (DEPRECATED)"
 	depends on X86 && I2C
 	help
+	  This driver is DEPRECATED please use the new merged fschmd
+	  ("FSC Poseidon, Scylla, Hermes, Heimdall and Heracles") driver
+	  instead.
+
 	  If you say yes here you get support for Fujitsu Siemens
 	  Computers Poseidon sensor chips.
 
@@ -325,14 +334,15 @@ config SENSORS_FSCPOS
 
 config SENSORS_FSCHMD
 	tristate "FSC Poseidon, Scylla, Hermes, Heimdall and Heracles"
-	depends on X86 && I2C && EXPERIMENTAL
+	depends on X86 && I2C
 	help
 	  If you say yes here you get support for various Fujitsu Siemens
-	  Computers sensor chips.
+	  Computers sensor chips, including support for the integrated
+	  watchdog.
 
-	  This is a new merged driver for FSC sensor chips which is intended
-	  as a replacment for the fscpos, fscscy and fscher drivers and adds
-	  support for several other FCS sensor chips.
+	  This is a merged driver for FSC sensor chips replacing the fscpos,
+	  fscscy and fscher drivers and adding support for several other FSC
+	  sensor chips.
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called fschmd.
@@ -399,7 +409,8 @@ config SENSORS_IT87
 	select HWMON_VID
 	help
 	  If you say yes here you get support for ITE IT8705F, IT8712F,
-	  IT8716F, IT8718F and IT8726F sensor chips, and the SiS960 clone.
+	  IT8716F, IT8718F, IT8720F and IT8726F sensor chips, and the
+	  SiS960 clone.
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called it87.
@@ -417,11 +428,12 @@ config SENSORS_LM63
 	  will be called lm63.
 
 config SENSORS_LM70
-	tristate "National Semiconductor LM70"
+	tristate "National Semiconductor LM70 / Texas Instruments TMP121"
 	depends on SPI_MASTER && EXPERIMENTAL
 	help
 	  If you say yes here you get support for the National Semiconductor
-	  LM70 digital temperature sensor chip.
+	  LM70 and Texas Instruments TMP121/TMP123 digital temperature
+	  sensor chips.
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called lm70.
@@ -548,6 +560,17 @@ config SENSORS_LM93
 	  This driver can also be built as a module.  If so, the module
 	  will be called lm93.
 
+config SENSORS_LTC4245
+	tristate "Linear Technology LTC4245"
+	depends on I2C && EXPERIMENTAL
+	default n
+	help
+	  If you say yes here you get support for Linear Technology LTC4245
+	  Multiple Supply Hot Swap Controller I2C interface.
+
+	  This driver can also be built as a module. If so, the module will
+	  be called ltc4245.
+
 config SENSORS_MAX1111
 	tristate "Maxim MAX1111 Multichannel, Serial 8-bit ADC chip"
 	depends on SPI_MASTER
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index 58fc5be5355d..8fd124eff646 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -62,6 +62,7 @@ obj-$(CONFIG_SENSORS_LM87)	+= lm87.o
 obj-$(CONFIG_SENSORS_LM90)	+= lm90.o
 obj-$(CONFIG_SENSORS_LM92)	+= lm92.o
 obj-$(CONFIG_SENSORS_LM93)	+= lm93.o
+obj-$(CONFIG_SENSORS_LTC4245)	+= ltc4245.o
 obj-$(CONFIG_SENSORS_MAX1111)	+= max1111.o
 obj-$(CONFIG_SENSORS_MAX1619)	+= max1619.o
 obj-$(CONFIG_SENSORS_MAX6650)	+= max6650.o
diff --git a/drivers/hwmon/adt7462.c b/drivers/hwmon/adt7462.c
index 66107b4dc12a..1852f27bac51 100644
--- a/drivers/hwmon/adt7462.c
+++ b/drivers/hwmon/adt7462.c
@@ -204,8 +204,6 @@ I2C_CLIENT_INSMOD_1(adt7462);
 #define MASK_AND_SHIFT(value, prefix)	\
 	(((value) & prefix##_MASK) >> prefix##_SHIFT)
 
-#define ROUND_DIV(x, divisor)  (((x) + ((divisor) / 2)) / (divisor))
-
 struct adt7462_data {
 	struct device		*hwmon_dev;
 	struct attribute_group	attrs;
@@ -840,7 +838,7 @@ static ssize_t set_temp_min(struct device *dev,
 	if (strict_strtol(buf, 10, &temp) || !temp_enabled(data, attr->index))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000) + 64;
+	temp = DIV_ROUND_CLOSEST(temp, 1000) + 64;
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
@@ -878,7 +876,7 @@ static ssize_t set_temp_max(struct device *dev,
 	if (strict_strtol(buf, 10, &temp) || !temp_enabled(data, attr->index))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000) + 64;
+	temp = DIV_ROUND_CLOSEST(temp, 1000) + 64;
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
@@ -943,7 +941,7 @@ static ssize_t set_volt_max(struct device *dev,
 		return -EINVAL;
 
 	temp *= 1000; /* convert mV to uV */
-	temp = ROUND_DIV(temp, x);
+	temp = DIV_ROUND_CLOSEST(temp, x);
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
@@ -985,7 +983,7 @@ static ssize_t set_volt_min(struct device *dev,
 		return -EINVAL;
 
 	temp *= 1000; /* convert mV to uV */
-	temp = ROUND_DIV(temp, x);
+	temp = DIV_ROUND_CLOSEST(temp, x);
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
@@ -1250,7 +1248,7 @@ static ssize_t set_pwm_hyst(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = SENSORS_LIMIT(temp, 0, 15);
 
 	/* package things up */
@@ -1337,7 +1335,7 @@ static ssize_t set_pwm_tmin(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000) + 64;
+	temp = DIV_ROUND_CLOSEST(temp, 1000) + 64;
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
diff --git a/drivers/hwmon/adt7470.c b/drivers/hwmon/adt7470.c
index 1311a595147e..633e1a1e9d79 100644
--- a/drivers/hwmon/adt7470.c
+++ b/drivers/hwmon/adt7470.c
@@ -28,6 +28,7 @@
 #include <linux/mutex.h>
 #include <linux/delay.h>
 #include <linux/log2.h>
+#include <linux/kthread.h>
 
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = { 0x2C, 0x2E, 0x2F, I2C_CLIENT_END };
@@ -74,6 +75,7 @@ I2C_CLIENT_INSMOD_1(adt7470);
 #define ADT7470_REG_PWM12_CFG			0x68
 #define		ADT7470_PWM2_AUTO_MASK		0x40
 #define		ADT7470_PWM1_AUTO_MASK		0x80
+#define		ADT7470_PWM_AUTO_MASK		0xC0
 #define ADT7470_REG_PWM34_CFG			0x69
 #define		ADT7470_PWM3_AUTO_MASK		0x40
 #define		ADT7470_PWM4_AUTO_MASK		0x80
@@ -128,8 +130,11 @@ I2C_CLIENT_INSMOD_1(adt7470);
 /* How often do we reread sensor limit values? (In jiffies) */
 #define LIMIT_REFRESH_INTERVAL	(60 * HZ)
 
-/* sleep 1s while gathering temperature data */
-#define TEMP_COLLECTION_TIME	1000
+/* Wait at least 200ms per sensor for 10 sensors */
+#define TEMP_COLLECTION_TIME	2000
+
+/* auto update thing won't fire more than every 2s */
+#define AUTO_UPDATE_INTERVAL	2000
 
 /* datasheet says to divide this number by the fan reading to get fan rpm */
 #define FAN_PERIOD_TO_RPM(x)	((90000 * 60) / (x))
@@ -137,8 +142,6 @@ I2C_CLIENT_INSMOD_1(adt7470);
 #define FAN_PERIOD_INVALID	65535
 #define FAN_DATA_VALID(x)	((x) && (x) != FAN_PERIOD_INVALID)
 
-#define ROUND_DIV(x, divisor)	(((x) + ((divisor) / 2)) / (divisor))
-
 struct adt7470_data {
 	struct device		*hwmon_dev;
 	struct attribute_group	attrs;
@@ -148,6 +151,9 @@ struct adt7470_data {
 	unsigned long		sensors_last_updated;	/* In jiffies */
 	unsigned long		limits_last_updated;	/* In jiffies */
 
+	int			num_temp_sensors;	/* -1 = probe */
+	int			temperatures_probed;
+
 	s8			temp[ADT7470_TEMP_COUNT];
 	s8			temp_min[ADT7470_TEMP_COUNT];
 	s8			temp_max[ADT7470_TEMP_COUNT];
@@ -163,6 +169,10 @@ struct adt7470_data {
 	u8			pwm_min[ADT7470_PWM_COUNT];
 	s8			pwm_tmin[ADT7470_PWM_COUNT];
 	u8			pwm_auto_temp[ADT7470_PWM_COUNT];
+
+	struct task_struct	*auto_update;
+	struct completion	auto_update_stop;
+	unsigned int		auto_update_interval;
 };
 
 static int adt7470_probe(struct i2c_client *client,
@@ -220,40 +230,126 @@ static void adt7470_init_client(struct i2c_client *client)
 	}
 }
 
-static struct adt7470_data *adt7470_update_device(struct device *dev)
+/* Probe for temperature sensors.  Assumes lock is held */
+static int adt7470_read_temperatures(struct i2c_client *client,
+				     struct adt7470_data *data)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct adt7470_data *data = i2c_get_clientdata(client);
-	unsigned long local_jiffies = jiffies;
-	u8 cfg;
+	unsigned long res;
 	int i;
+	u8 cfg, pwm[4], pwm_cfg[2];
 
-	mutex_lock(&data->lock);
-	if (time_before(local_jiffies, data->sensors_last_updated +
-		SENSOR_REFRESH_INTERVAL)
-		&& data->sensors_valid)
-		goto no_sensor_update;
+	/* save pwm[1-4] config register */
+	pwm_cfg[0] = i2c_smbus_read_byte_data(client, ADT7470_REG_PWM_CFG(0));
+	pwm_cfg[1] = i2c_smbus_read_byte_data(client, ADT7470_REG_PWM_CFG(2));
+
+	/* set manual pwm to whatever it is set to now */
+	for (i = 0; i < ADT7470_FAN_COUNT; i++)
+		pwm[i] = i2c_smbus_read_byte_data(client, ADT7470_REG_PWM(i));
+
+	/* put pwm in manual mode */
+	i2c_smbus_write_byte_data(client, ADT7470_REG_PWM_CFG(0),
+		pwm_cfg[0] & ~(ADT7470_PWM_AUTO_MASK));
+	i2c_smbus_write_byte_data(client, ADT7470_REG_PWM_CFG(2),
+		pwm_cfg[1] & ~(ADT7470_PWM_AUTO_MASK));
+
+	/* write pwm control to whatever it was */
+	for (i = 0; i < ADT7470_FAN_COUNT; i++)
+		i2c_smbus_write_byte_data(client, ADT7470_REG_PWM(i), pwm[i]);
 
 	/* start reading temperature sensors */
 	cfg = i2c_smbus_read_byte_data(client, ADT7470_REG_CFG);
 	cfg |= 0x80;
 	i2c_smbus_write_byte_data(client, ADT7470_REG_CFG, cfg);
 
-	/*
-	 * Delay is 200ms * number of tmp05 sensors.  Too bad
-	 * there's no way to figure out how many are connected.
-	 * For now, assume 1s will work.
-	 */
-	msleep(TEMP_COLLECTION_TIME);
+	/* Delay is 200ms * number of temp sensors. */
+	res = msleep_interruptible((data->num_temp_sensors >= 0 ?
+				    data->num_temp_sensors * 200 :
+				    TEMP_COLLECTION_TIME));
 
 	/* done reading temperature sensors */
 	cfg = i2c_smbus_read_byte_data(client, ADT7470_REG_CFG);
 	cfg &= ~0x80;
 	i2c_smbus_write_byte_data(client, ADT7470_REG_CFG, cfg);
 
-	for (i = 0; i < ADT7470_TEMP_COUNT; i++)
+	/* restore pwm[1-4] config registers */
+	i2c_smbus_write_byte_data(client, ADT7470_REG_PWM_CFG(0), pwm_cfg[0]);
+	i2c_smbus_write_byte_data(client, ADT7470_REG_PWM_CFG(2), pwm_cfg[1]);
+
+	if (res) {
+		printk(KERN_ERR "ha ha, interrupted");
+		return -EAGAIN;
+	}
+
+	/* Only count fans if we have to */
+	if (data->num_temp_sensors >= 0)
+		return 0;
+
+	for (i = 0; i < ADT7470_TEMP_COUNT; i++) {
 		data->temp[i] = i2c_smbus_read_byte_data(client,
 						ADT7470_TEMP_REG(i));
+		if (data->temp[i])
+			data->num_temp_sensors = i + 1;
+	}
+	data->temperatures_probed = 1;
+	return 0;
+}
+
+static int adt7470_update_thread(void *p)
+{
+	struct i2c_client *client = p;
+	struct adt7470_data *data = i2c_get_clientdata(client);
+
+	while (!kthread_should_stop()) {
+		mutex_lock(&data->lock);
+		adt7470_read_temperatures(client, data);
+		mutex_unlock(&data->lock);
+		if (kthread_should_stop())
+			break;
+		msleep_interruptible(data->auto_update_interval);
+	}
+
+	complete_all(&data->auto_update_stop);
+	return 0;
+}
+
+static struct adt7470_data *adt7470_update_device(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct adt7470_data *data = i2c_get_clientdata(client);
+	unsigned long local_jiffies = jiffies;
+	u8 cfg;
+	int i;
+	int need_sensors = 1;
+	int need_limits = 1;
+
+	/*
+	 * Figure out if we need to update the shadow registers.
+	 * Lockless means that we may occasionally report out of
+	 * date data.
+	 */
+	if (time_before(local_jiffies, data->sensors_last_updated +
+			SENSOR_REFRESH_INTERVAL) &&
+	    data->sensors_valid)
+		need_sensors = 0;
+
+	if (time_before(local_jiffies, data->limits_last_updated +
+			LIMIT_REFRESH_INTERVAL) &&
+	    data->limits_valid)
+		need_limits = 0;
+
+	if (!need_sensors && !need_limits)
+		return data;
+
+	mutex_lock(&data->lock);
+	if (!need_sensors)
+		goto no_sensor_update;
+
+	if (!data->temperatures_probed)
+		adt7470_read_temperatures(client, data);
+	else
+		for (i = 0; i < ADT7470_TEMP_COUNT; i++)
+			data->temp[i] = i2c_smbus_read_byte_data(client,
+						ADT7470_TEMP_REG(i));
 
 	for (i = 0; i < ADT7470_FAN_COUNT; i++)
 		data->fan[i] = adt7470_read_word_data(client,
@@ -302,9 +398,7 @@ static struct adt7470_data *adt7470_update_device(struct device *dev)
 	data->sensors_valid = 1;
 
 no_sensor_update:
-	if (time_before(local_jiffies, data->limits_last_updated +
-		LIMIT_REFRESH_INTERVAL)
-		&& data->limits_valid)
+	if (!need_limits)
 		goto out;
 
 	for (i = 0; i < ADT7470_TEMP_COUNT; i++) {
@@ -338,6 +432,66 @@ out:
 	return data;
 }
 
+static ssize_t show_auto_update_interval(struct device *dev,
+					 struct device_attribute *devattr,
+					 char *buf)
+{
+	struct adt7470_data *data = adt7470_update_device(dev);
+	return sprintf(buf, "%d\n", data->auto_update_interval);
+}
+
+static ssize_t set_auto_update_interval(struct device *dev,
+					struct device_attribute *devattr,
+					const char *buf,
+					size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct adt7470_data *data = i2c_get_clientdata(client);
+	long temp;
+
+	if (strict_strtol(buf, 10, &temp))
+		return -EINVAL;
+
+	temp = SENSORS_LIMIT(temp, 0, 60000);
+
+	mutex_lock(&data->lock);
+	data->auto_update_interval = temp;
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t show_num_temp_sensors(struct device *dev,
+				     struct device_attribute *devattr,
+				     char *buf)
+{
+	struct adt7470_data *data = adt7470_update_device(dev);
+	return sprintf(buf, "%d\n", data->num_temp_sensors);
+}
+
+static ssize_t set_num_temp_sensors(struct device *dev,
+				    struct device_attribute *devattr,
+				    const char *buf,
+				    size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct adt7470_data *data = i2c_get_clientdata(client);
+	long temp;
+
+	if (strict_strtol(buf, 10, &temp))
+		return -EINVAL;
+
+	temp = SENSORS_LIMIT(temp, -1, 10);
+
+	mutex_lock(&data->lock);
+	data->num_temp_sensors = temp;
+	if (temp < 0)
+		data->temperatures_probed = 0;
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
 static ssize_t show_temp_min(struct device *dev,
 			     struct device_attribute *devattr,
 			     char *buf)
@@ -360,7 +514,7 @@ static ssize_t set_temp_min(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
@@ -394,7 +548,7 @@ static ssize_t set_temp_max(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
@@ -671,7 +825,7 @@ static ssize_t set_pwm_tmin(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
@@ -804,6 +958,10 @@ static ssize_t show_alarm(struct device *dev,
 }
 
 static DEVICE_ATTR(alarm_mask, S_IRUGO, show_alarm_mask, NULL);
+static DEVICE_ATTR(num_temp_sensors, S_IWUSR | S_IRUGO, show_num_temp_sensors,
+		   set_num_temp_sensors);
+static DEVICE_ATTR(auto_update_interval, S_IWUSR | S_IRUGO,
+		   show_auto_update_interval, set_auto_update_interval);
 
 static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp_max,
 		    set_temp_max, 0);
@@ -976,6 +1134,8 @@ static SENSOR_DEVICE_ATTR(pwm4_auto_channels_temp, S_IWUSR | S_IRUGO,
 static struct attribute *adt7470_attr[] =
 {
 	&dev_attr_alarm_mask.attr,
+	&dev_attr_num_temp_sensors.attr,
+	&dev_attr_auto_update_interval.attr,
 	&sensor_dev_attr_temp1_max.dev_attr.attr,
 	&sensor_dev_attr_temp2_max.dev_attr.attr,
 	&sensor_dev_attr_temp3_max.dev_attr.attr,
@@ -1108,6 +1268,9 @@ static int adt7470_probe(struct i2c_client *client,
 		goto exit;
 	}
 
+	data->num_temp_sensors = -1;
+	data->auto_update_interval = AUTO_UPDATE_INTERVAL;
+
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->lock);
 
@@ -1127,8 +1290,16 @@ static int adt7470_probe(struct i2c_client *client,
 		goto exit_remove;
 	}
 
+	init_completion(&data->auto_update_stop);
+	data->auto_update = kthread_run(adt7470_update_thread, client,
+					dev_name(data->hwmon_dev));
+	if (IS_ERR(data->auto_update))
+		goto exit_unregister;
+
 	return 0;
 
+exit_unregister:
+	hwmon_device_unregister(data->hwmon_dev);
 exit_remove:
 	sysfs_remove_group(&client->dev.kobj, &data->attrs);
 exit_free:
@@ -1141,6 +1312,8 @@ static int adt7470_remove(struct i2c_client *client)
 {
 	struct adt7470_data *data = i2c_get_clientdata(client);
 
+	kthread_stop(data->auto_update);
+	wait_for_completion(&data->auto_update_stop);
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &data->attrs);
 	kfree(data);
diff --git a/drivers/hwmon/adt7473.c b/drivers/hwmon/adt7473.c
index 18aa30866a6c..0a6ce2367b42 100644
--- a/drivers/hwmon/adt7473.c
+++ b/drivers/hwmon/adt7473.c
@@ -129,8 +129,6 @@ I2C_CLIENT_INSMOD_1(adt7473);
 #define FAN_PERIOD_INVALID	65535
 #define FAN_DATA_VALID(x)	((x) && (x) != FAN_PERIOD_INVALID)
 
-#define ROUND_DIV(x, divisor)	(((x) + ((divisor) / 2)) / (divisor))
-
 struct adt7473_data {
 	struct device		*hwmon_dev;
 	struct attribute_group	attrs;
@@ -459,7 +457,7 @@ static ssize_t set_temp_min(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = encode_temp(data->temp_twos_complement, temp);
 
 	mutex_lock(&data->lock);
@@ -495,7 +493,7 @@ static ssize_t set_temp_max(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = encode_temp(data->temp_twos_complement, temp);
 
 	mutex_lock(&data->lock);
@@ -720,7 +718,7 @@ static ssize_t set_temp_tmax(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = encode_temp(data->temp_twos_complement, temp);
 
 	mutex_lock(&data->lock);
@@ -756,7 +754,7 @@ static ssize_t set_temp_tmin(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = encode_temp(data->temp_twos_complement, temp);
 
 	mutex_lock(&data->lock);
diff --git a/drivers/hwmon/applesmc.c b/drivers/hwmon/applesmc.c
index 086c2a5cef0b..dca47a591baf 100644
--- a/drivers/hwmon/applesmc.c
+++ b/drivers/hwmon/applesmc.c
@@ -131,6 +131,10 @@ static const char* temperature_sensors_sets[][36] = {
 /* Set 14: iMac 6,1 */
 	{ "TA0P", "TC0D", "TC0H", "TC0P", "TG0D", "TG0H", "TG0P", "TH0P",
 	  "TO0P", "Tp0P", NULL },
+/* Set 15: MacBook Air 2,1 */
+	{ "TB0T", "TB1S", "TB1T", "TB2S", "TB2T", "TC0D", "TN0D", "TTF0",
+	  "TV0P", "TVFP", "TW0P", "Th0P", "Tp0P", "Tp1P", "TpFP", "Ts0P",
+	  "Ts0S", NULL },
 };
 
 /* List of keys used to read/write fan speeds */
@@ -1301,11 +1305,17 @@ static __initdata struct dmi_match_data applesmc_dmi_data[] = {
 	{ .accelerometer = 0, .light = 0, .temperature_set = 13 },
 /* iMac 6: light sensor only, temperature set 14 */
 	{ .accelerometer = 0, .light = 0, .temperature_set = 14 },
+/* MacBook Air 2,1: accelerometer, backlight and temperature set 15 */
+	{ .accelerometer = 1, .light = 1, .temperature_set = 15 },
 };
 
 /* Note that DMI_MATCH(...,"MacBook") will match "MacBookPro1,1".
  * So we need to put "Apple MacBook Pro" before "Apple MacBook". */
 static __initdata struct dmi_system_id applesmc_whitelist[] = {
+	{ applesmc_dmi_match, "Apple MacBook Air 2", {
+	  DMI_MATCH(DMI_BOARD_VENDOR, "Apple"),
+	  DMI_MATCH(DMI_PRODUCT_NAME, "MacBookAir2") },
+		&applesmc_dmi_data[15]},
 	{ applesmc_dmi_match, "Apple MacBook Air", {
 	  DMI_MATCH(DMI_BOARD_VENDOR, "Apple"),
 	  DMI_MATCH(DMI_PRODUCT_NAME, "MacBookAir") },
diff --git a/drivers/hwmon/asb100.c b/drivers/hwmon/asb100.c
index 8a45a2e6ba8a..8acf82977e7b 100644
--- a/drivers/hwmon/asb100.c
+++ b/drivers/hwmon/asb100.c
@@ -53,7 +53,10 @@ static const unsigned short normal_i2c[] = { 0x2d, I2C_CLIENT_END };
 
 /* Insmod parameters */
 I2C_CLIENT_INSMOD_1(asb100);
-I2C_CLIENT_MODULE_PARM(force_subclients, "List of subclient addresses: "
+
+static unsigned short force_subclients[4];
+module_param_array(force_subclients, short, NULL, 0);
+MODULE_PARM_DESC(force_subclients, "List of subclient addresses: "
 	"{bus, clientaddr, subclientaddr1, subclientaddr2}");
 
 /* Voltage IN registers 0-6 */
diff --git a/drivers/hwmon/dme1737.c b/drivers/hwmon/dme1737.c
index 27a5d397f9a1..3df202a9ad72 100644
--- a/drivers/hwmon/dme1737.c
+++ b/drivers/hwmon/dme1737.c
@@ -34,6 +34,7 @@
 #include <linux/hwmon-vid.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
+#include <linux/acpi.h>
 #include <asm/io.h>
 
 /* ISA device, if found */
@@ -2361,6 +2362,10 @@ static int __init dme1737_isa_device_add(unsigned short addr)
 	};
 	int err;
 
+	err = acpi_check_resource_conflict(&res);
+	if (err)
+		goto exit;
+
 	if (!(pdev = platform_device_alloc("dme1737", addr))) {
 		printk(KERN_ERR "dme1737: Failed to allocate device.\n");
 		err = -ENOMEM;
diff --git a/drivers/hwmon/f71805f.c b/drivers/hwmon/f71805f.c
index 7a14a2dbb752..899876579253 100644
--- a/drivers/hwmon/f71805f.c
+++ b/drivers/hwmon/f71805f.c
@@ -39,6 +39,7 @@
 #include <linux/mutex.h>
 #include <linux/sysfs.h>
 #include <linux/ioport.h>
+#include <linux/acpi.h>
 #include <asm/io.h>
 
 static unsigned short force_id;
@@ -1455,6 +1456,10 @@ static int __init f71805f_device_add(unsigned short address,
 	}
 
 	res.name = pdev->name;
+	err = acpi_check_resource_conflict(&res);
+	if (err)
+		goto exit_device_put;
+
 	err = platform_device_add_resources(pdev, &res, 1);
 	if (err) {
 		printk(KERN_ERR DRVNAME ": Device resource addition failed "
diff --git a/drivers/hwmon/f71882fg.c b/drivers/hwmon/f71882fg.c
index 67067e9a323e..609cafff86bc 100644
--- a/drivers/hwmon/f71882fg.c
+++ b/drivers/hwmon/f71882fg.c
@@ -1,6 +1,6 @@
 /***************************************************************************
  *   Copyright (C) 2006 by Hans Edgington <hans@edgington.nl>              *
- *   Copyright (C) 2007 by Hans de Goede  <j.w.r.degoede@hhs.nl>           *
+ *   Copyright (C) 2007,2008 by Hans de Goede <hdegoede@redhat.com>        *
  *                                                                         *
  *   This program is free software; you can redistribute it and/or modify  *
  *   it under the terms of the GNU General Public License as published by  *
@@ -27,11 +27,12 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
-#include <asm/io.h>
+#include <linux/io.h>
+#include <linux/acpi.h>
 
 #define DRVNAME "f71882fg"
 
-#define SIO_F71882FG_LD_HWM	0x04	/* Hardware monitor logical device*/
+#define SIO_F71882FG_LD_HWM	0x04	/* Hardware monitor logical device */
 #define SIO_UNLOCK_KEY		0x87	/* Key to enable Super-I/O */
 #define SIO_LOCK_KEY		0xAA	/* Key to diasble Super-I/O */
 
@@ -43,7 +44,9 @@
 #define SIO_REG_ADDR		0x60	/* Logical device address (2 bytes) */
 
 #define SIO_FINTEK_ID		0x1934	/* Manufacturers ID */
+#define SIO_F71862_ID		0x0601	/* Chipset ID */
 #define SIO_F71882_ID		0x0541	/* Chipset ID */
+#define SIO_F8000_ID		0x0581	/* Chipset ID */
 
 #define REGION_LENGTH		8
 #define ADDR_REG_OFFSET		5
@@ -51,25 +54,36 @@
 
 #define F71882FG_REG_PECI		0x0A
 
-#define F71882FG_REG_IN_STATUS		0x12
-#define F71882FG_REG_IN_BEEP		0x13
+#define F71882FG_REG_IN_STATUS		0x12 /* f71882fg only */
+#define F71882FG_REG_IN_BEEP		0x13 /* f71882fg only */
 #define F71882FG_REG_IN(nr)		(0x20  + (nr))
-#define F71882FG_REG_IN1_HIGH		0x32
+#define F71882FG_REG_IN1_HIGH		0x32 /* f71882fg only */
 
 #define F71882FG_REG_FAN(nr)		(0xA0 + (16 * (nr)))
+#define F71882FG_REG_FAN_TARGET(nr)	(0xA2 + (16 * (nr)))
+#define F71882FG_REG_FAN_FULL_SPEED(nr)	(0xA4 + (16 * (nr)))
 #define F71882FG_REG_FAN_STATUS		0x92
 #define F71882FG_REG_FAN_BEEP		0x93
 
-#define F71882FG_REG_TEMP(nr)		(0x72 + 2 * (nr))
-#define F71882FG_REG_TEMP_OVT(nr)	(0x82 + 2 * (nr))
-#define F71882FG_REG_TEMP_HIGH(nr)	(0x83 + 2 * (nr))
+#define F71882FG_REG_TEMP(nr)		(0x70 + 2 * (nr))
+#define F71882FG_REG_TEMP_OVT(nr)	(0x80 + 2 * (nr))
+#define F71882FG_REG_TEMP_HIGH(nr)	(0x81 + 2 * (nr))
 #define F71882FG_REG_TEMP_STATUS	0x62
 #define F71882FG_REG_TEMP_BEEP		0x63
-#define F71882FG_REG_TEMP_HYST1		0x6C
-#define F71882FG_REG_TEMP_HYST23	0x6D
+#define F71882FG_REG_TEMP_HYST(nr)	(0x6C + (nr))
 #define F71882FG_REG_TEMP_TYPE		0x6B
 #define F71882FG_REG_TEMP_DIODE_OPEN	0x6F
 
+#define F71882FG_REG_PWM(nr)		(0xA3 + (16 * (nr)))
+#define F71882FG_REG_PWM_TYPE		0x94
+#define F71882FG_REG_PWM_ENABLE		0x96
+
+#define F71882FG_REG_FAN_HYST(nr)	(0x98 + (nr))
+
+#define F71882FG_REG_POINT_PWM(pwm, point)	(0xAA + (point) + (16 * (pwm)))
+#define F71882FG_REG_POINT_TEMP(pwm, point)	(0xA6 + (point) + (16 * (pwm)))
+#define F71882FG_REG_POINT_MAPPING(nr)		(0xAF + 16 * (nr))
+
 #define	F71882FG_REG_START		0x01
 
 #define FAN_MIN_DETECT			366 /* Lowest detectable fanspeed */
@@ -78,7 +92,15 @@ static unsigned short force_id;
 module_param(force_id, ushort, 0);
 MODULE_PARM_DESC(force_id, "Override the detected device ID");
 
-static struct platform_device *f71882fg_pdev = NULL;
+enum chips { f71862fg, f71882fg, f8000 };
+
+static const char *f71882fg_names[] = {
+	"f71862fg",
+	"f71882fg",
+	"f8000",
+};
+
+static struct platform_device *f71882fg_pdev;
 
 /* Super-I/O Function prototypes */
 static inline int superio_inb(int base, int reg);
@@ -87,8 +109,13 @@ static inline void superio_enter(int base);
 static inline void superio_select(int base, int ld);
 static inline void superio_exit(int base);
 
+struct f71882fg_sio_data {
+	enum chips type;
+};
+
 struct f71882fg_data {
 	unsigned short addr;
+	enum chips type;
 	struct device *hwmon_dev;
 
 	struct mutex update_lock;
@@ -102,19 +129,30 @@ struct f71882fg_data {
 	u8	in_status;
 	u8	in_beep;
 	u16	fan[4];
+	u16	fan_target[4];
+	u16	fan_full_speed[4];
 	u8	fan_status;
 	u8	fan_beep;
-	u8	temp[3];
-	u8	temp_ovt[3];
-	u8	temp_high[3];
-	u8	temp_hyst[3];
-	u8	temp_type[3];
+	/* Note: all models have only 3 temperature channels, but on some
+	   they are addressed as 0-2 and on others as 1-3, so for coding
+	   convenience we reserve space for 4 channels */
+	u8	temp[4];
+	u8	temp_ovt[4];
+	u8	temp_high[4];
+	u8	temp_hyst[2]; /* 2 hysts stored per reg */
+	u8	temp_type[4];
 	u8	temp_status;
 	u8	temp_beep;
 	u8	temp_diode_open;
+	u8	pwm[4];
+	u8	pwm_enable;
+	u8	pwm_auto_point_hyst[2];
+	u8	pwm_auto_point_mapping[4];
+	u8	pwm_auto_point_pwm[4][5];
+	u8	pwm_auto_point_temp[4][4];
 };
 
-/* Sysfs in*/
+/* Sysfs in */
 static ssize_t show_in(struct device *dev, struct device_attribute *devattr,
 	char *buf);
 static ssize_t show_in_max(struct device *dev, struct device_attribute
@@ -130,6 +168,10 @@ static ssize_t show_in_alarm(struct device *dev, struct device_attribute
 /* Sysfs Fan */
 static ssize_t show_fan(struct device *dev, struct device_attribute *devattr,
 	char *buf);
+static ssize_t show_fan_full_speed(struct device *dev,
+	struct device_attribute *devattr, char *buf);
+static ssize_t store_fan_full_speed(struct device *dev,
+	struct device_attribute *devattr, const char *buf, size_t count);
 static ssize_t show_fan_beep(struct device *dev, struct device_attribute
 	*devattr, char *buf);
 static ssize_t store_fan_beep(struct device *dev, struct device_attribute
@@ -163,16 +205,41 @@ static ssize_t show_temp_alarm(struct device *dev, struct device_attribute
 	*devattr, char *buf);
 static ssize_t show_temp_fault(struct device *dev, struct device_attribute
 	*devattr, char *buf);
+/* PWM and Auto point control */
+static ssize_t show_pwm(struct device *dev, struct device_attribute *devattr,
+	char *buf);
+static ssize_t store_pwm(struct device *dev, struct device_attribute *devattr,
+	const char *buf, size_t count);
+static ssize_t show_pwm_enable(struct device *dev,
+	struct device_attribute *devattr, char *buf);
+static ssize_t store_pwm_enable(struct device *dev,
+	struct device_attribute	*devattr, const char *buf, size_t count);
+static ssize_t show_pwm_interpolate(struct device *dev,
+	struct device_attribute *devattr, char *buf);
+static ssize_t store_pwm_interpolate(struct device *dev,
+	struct device_attribute *devattr, const char *buf, size_t count);
+static ssize_t show_pwm_auto_point_channel(struct device *dev,
+	struct device_attribute *devattr, char *buf);
+static ssize_t store_pwm_auto_point_channel(struct device *dev,
+	struct device_attribute *devattr, const char *buf, size_t count);
+static ssize_t show_pwm_auto_point_temp_hyst(struct device *dev,
+	struct device_attribute *devattr, char *buf);
+static ssize_t store_pwm_auto_point_temp_hyst(struct device *dev,
+	struct device_attribute *devattr, const char *buf, size_t count);
+static ssize_t show_pwm_auto_point_pwm(struct device *dev,
+	struct device_attribute *devattr, char *buf);
+static ssize_t store_pwm_auto_point_pwm(struct device *dev,
+	struct device_attribute *devattr, const char *buf, size_t count);
+static ssize_t show_pwm_auto_point_temp(struct device *dev,
+	struct device_attribute *devattr, char *buf);
+static ssize_t store_pwm_auto_point_temp(struct device *dev,
+	struct device_attribute *devattr, const char *buf, size_t count);
 /* Sysfs misc */
 static ssize_t show_name(struct device *dev, struct device_attribute *devattr,
 	char *buf);
 
 static int __devinit f71882fg_probe(struct platform_device * pdev);
-static int __devexit f71882fg_remove(struct platform_device *pdev);
-static int __init f71882fg_init(void);
-static int __init f71882fg_find(int sioaddr, unsigned short *address);
-static int __init f71882fg_device_add(unsigned short address);
-static void __exit f71882fg_exit(void);
+static int f71882fg_remove(struct platform_device *pdev);
 
 static struct platform_driver f71882fg_driver = {
 	.driver = {
@@ -183,86 +250,531 @@ static struct platform_driver f71882fg_driver = {
 	.remove		= __devexit_p(f71882fg_remove),
 };
 
-static struct device_attribute f71882fg_dev_attr[] =
-{
-	__ATTR( name, S_IRUGO, show_name, NULL ),
+static DEVICE_ATTR(name, S_IRUGO, show_name, NULL);
+
+/* Temp and in attr common to both the f71862fg and f71882fg */
+static struct sensor_device_attribute_2 f718x2fg_in_temp_attr[] = {
+	SENSOR_ATTR_2(in0_input, S_IRUGO, show_in, NULL, 0, 0),
+	SENSOR_ATTR_2(in1_input, S_IRUGO, show_in, NULL, 0, 1),
+	SENSOR_ATTR_2(in2_input, S_IRUGO, show_in, NULL, 0, 2),
+	SENSOR_ATTR_2(in3_input, S_IRUGO, show_in, NULL, 0, 3),
+	SENSOR_ATTR_2(in4_input, S_IRUGO, show_in, NULL, 0, 4),
+	SENSOR_ATTR_2(in5_input, S_IRUGO, show_in, NULL, 0, 5),
+	SENSOR_ATTR_2(in6_input, S_IRUGO, show_in, NULL, 0, 6),
+	SENSOR_ATTR_2(in7_input, S_IRUGO, show_in, NULL, 0, 7),
+	SENSOR_ATTR_2(in8_input, S_IRUGO, show_in, NULL, 0, 8),
+	SENSOR_ATTR_2(temp1_input, S_IRUGO, show_temp, NULL, 0, 1),
+	SENSOR_ATTR_2(temp1_max, S_IRUGO|S_IWUSR, show_temp_max,
+		store_temp_max, 0, 1),
+	SENSOR_ATTR_2(temp1_max_hyst, S_IRUGO|S_IWUSR, show_temp_max_hyst,
+		store_temp_max_hyst, 0, 1),
+	/* Should really be temp1_max_alarm, but older versions did not handle
+	   the max and crit alarms separately and lm_sensors v2 depends on the
+	   presence of temp#_alarm files. The same goes for temp2/3 _alarm. */
+	SENSOR_ATTR_2(temp1_alarm, S_IRUGO, show_temp_alarm, NULL, 0, 1),
+	SENSOR_ATTR_2(temp1_max_beep, S_IRUGO|S_IWUSR, show_temp_beep,
+		store_temp_beep, 0, 1),
+	SENSOR_ATTR_2(temp1_crit, S_IRUGO|S_IWUSR, show_temp_crit,
+		store_temp_crit, 0, 1),
+	SENSOR_ATTR_2(temp1_crit_hyst, S_IRUGO, show_temp_crit_hyst, NULL,
+		0, 1),
+	SENSOR_ATTR_2(temp1_crit_alarm, S_IRUGO, show_temp_alarm, NULL, 0, 5),
+	SENSOR_ATTR_2(temp1_crit_beep, S_IRUGO|S_IWUSR, show_temp_beep,
+		store_temp_beep, 0, 5),
+	SENSOR_ATTR_2(temp1_type, S_IRUGO, show_temp_type, NULL, 0, 1),
+	SENSOR_ATTR_2(temp1_fault, S_IRUGO, show_temp_fault, NULL, 0, 1),
+	SENSOR_ATTR_2(temp2_input, S_IRUGO, show_temp, NULL, 0, 2),
+	SENSOR_ATTR_2(temp2_max, S_IRUGO|S_IWUSR, show_temp_max,
+		store_temp_max, 0, 2),
+	SENSOR_ATTR_2(temp2_max_hyst, S_IRUGO|S_IWUSR, show_temp_max_hyst,
+		store_temp_max_hyst, 0, 2),
+	/* Should be temp2_max_alarm, see temp1_alarm note */
+	SENSOR_ATTR_2(temp2_alarm, S_IRUGO, show_temp_alarm, NULL, 0, 2),
+	SENSOR_ATTR_2(temp2_max_beep, S_IRUGO|S_IWUSR, show_temp_beep,
+		store_temp_beep, 0, 2),
+	SENSOR_ATTR_2(temp2_crit, S_IRUGO|S_IWUSR, show_temp_crit,
+		store_temp_crit, 0, 2),
+	SENSOR_ATTR_2(temp2_crit_hyst, S_IRUGO, show_temp_crit_hyst, NULL,
+		0, 2),
+	SENSOR_ATTR_2(temp2_crit_alarm, S_IRUGO, show_temp_alarm, NULL, 0, 6),
+	SENSOR_ATTR_2(temp2_crit_beep, S_IRUGO|S_IWUSR, show_temp_beep,
+		store_temp_beep, 0, 6),
+	SENSOR_ATTR_2(temp2_type, S_IRUGO, show_temp_type, NULL, 0, 2),
+	SENSOR_ATTR_2(temp2_fault, S_IRUGO, show_temp_fault, NULL, 0, 2),
+	SENSOR_ATTR_2(temp3_input, S_IRUGO, show_temp, NULL, 0, 3),
+	SENSOR_ATTR_2(temp3_max, S_IRUGO|S_IWUSR, show_temp_max,
+		store_temp_max, 0, 3),
+	SENSOR_ATTR_2(temp3_max_hyst, S_IRUGO|S_IWUSR, show_temp_max_hyst,
+		store_temp_max_hyst, 0, 3),
+	/* Should be temp3_max_alarm, see temp1_alarm note */
+	SENSOR_ATTR_2(temp3_alarm, S_IRUGO, show_temp_alarm, NULL, 0, 3),
+	SENSOR_ATTR_2(temp3_max_beep, S_IRUGO|S_IWUSR, show_temp_beep,
+		store_temp_beep, 0, 3),
+	SENSOR_ATTR_2(temp3_crit, S_IRUGO|S_IWUSR, show_temp_crit,
+		store_temp_crit, 0, 3),
+	SENSOR_ATTR_2(temp3_crit_hyst, S_IRUGO, show_temp_crit_hyst, NULL,
+		0, 3),
+	SENSOR_ATTR_2(temp3_crit_alarm, S_IRUGO, show_temp_alarm, NULL, 0, 7),
+	SENSOR_ATTR_2(temp3_crit_beep, S_IRUGO|S_IWUSR, show_temp_beep,
+		store_temp_beep, 0, 7),
+	SENSOR_ATTR_2(temp3_type, S_IRUGO, show_temp_type, NULL, 0, 3),
+	SENSOR_ATTR_2(temp3_fault, S_IRUGO, show_temp_fault, NULL, 0, 3),
+};
+
+/* Temp and in attr found only on the f71882fg */
+static struct sensor_device_attribute_2 f71882fg_in_temp_attr[] = {
+	SENSOR_ATTR_2(in1_max, S_IRUGO|S_IWUSR, show_in_max, store_in_max,
+		0, 1),
+	SENSOR_ATTR_2(in1_beep, S_IRUGO|S_IWUSR, show_in_beep, store_in_beep,
+		0, 1),
+	SENSOR_ATTR_2(in1_alarm, S_IRUGO, show_in_alarm, NULL, 0, 1),
+};
+
+/* Temp and in attr for the f8000
+   Note on the f8000 temp_ovt (crit) is used as max, and temp_high (max)
+   is used as hysteresis value to clear alarms
+ */
+static struct sensor_device_attribute_2 f8000_in_temp_attr[] = {
+	SENSOR_ATTR_2(in0_input, S_IRUGO, show_in, NULL, 0, 0),
+	SENSOR_ATTR_2(in1_input, S_IRUGO, show_in, NULL, 0, 1),
+	SENSOR_ATTR_2(in2_input, S_IRUGO, show_in, NULL, 0, 2),
+	SENSOR_ATTR_2(temp1_input, S_IRUGO, show_temp, NULL, 0, 0),
+	SENSOR_ATTR_2(temp1_max, S_IRUGO|S_IWUSR, show_temp_crit,
+		store_temp_crit, 0, 0),
+	SENSOR_ATTR_2(temp1_max_hyst, S_IRUGO|S_IWUSR, show_temp_max,
+		store_temp_max, 0, 0),
+	SENSOR_ATTR_2(temp1_alarm, S_IRUGO, show_temp_alarm, NULL, 0, 4),
+	SENSOR_ATTR_2(temp2_input, S_IRUGO, show_temp, NULL, 0, 1),
+	SENSOR_ATTR_2(temp2_max, S_IRUGO|S_IWUSR, show_temp_crit,
+		store_temp_crit, 0, 1),
+	SENSOR_ATTR_2(temp2_max_hyst, S_IRUGO|S_IWUSR, show_temp_max,
+		store_temp_max, 0, 1),
+	SENSOR_ATTR_2(temp2_alarm, S_IRUGO, show_temp_alarm, NULL, 0, 5),
+	SENSOR_ATTR_2(temp2_type, S_IRUGO, show_temp_type, NULL, 0, 1),
+	SENSOR_ATTR_2(temp3_input, S_IRUGO, show_temp, NULL, 0, 2),
+	SENSOR_ATTR_2(temp3_max, S_IRUGO|S_IWUSR, show_temp_crit,
+		store_temp_crit, 0, 2),
+	SENSOR_ATTR_2(temp3_max_hyst, S_IRUGO|S_IWUSR, show_temp_max,
+		store_temp_max, 0, 2),
+	SENSOR_ATTR_2(temp3_alarm, S_IRUGO, show_temp_alarm, NULL, 0, 6),
+};
+
+/* Fan / PWM attr common to all models */
+static struct sensor_device_attribute_2 fxxxx_fan_attr[] = {
+	SENSOR_ATTR_2(fan1_input, S_IRUGO, show_fan, NULL, 0, 0),
+	SENSOR_ATTR_2(fan1_full_speed, S_IRUGO|S_IWUSR,
+		      show_fan_full_speed,
+		      store_fan_full_speed, 0, 0),
+	SENSOR_ATTR_2(fan1_alarm, S_IRUGO, show_fan_alarm, NULL, 0, 0),
+	SENSOR_ATTR_2(fan2_input, S_IRUGO, show_fan, NULL, 0, 1),
+	SENSOR_ATTR_2(fan2_full_speed, S_IRUGO|S_IWUSR,
+		      show_fan_full_speed,
+		      store_fan_full_speed, 0, 1),
+	SENSOR_ATTR_2(fan2_alarm, S_IRUGO, show_fan_alarm, NULL, 0, 1),
+	SENSOR_ATTR_2(fan3_input, S_IRUGO, show_fan, NULL, 0, 2),
+	SENSOR_ATTR_2(fan3_full_speed, S_IRUGO|S_IWUSR,
+		      show_fan_full_speed,
+		      store_fan_full_speed, 0, 2),
+	SENSOR_ATTR_2(fan3_alarm, S_IRUGO, show_fan_alarm, NULL, 0, 2),
+
+	SENSOR_ATTR_2(pwm1, S_IRUGO|S_IWUSR, show_pwm, store_pwm, 0, 0),
+	SENSOR_ATTR_2(pwm1_enable, S_IRUGO|S_IWUSR, show_pwm_enable,
+		      store_pwm_enable, 0, 0),
+	SENSOR_ATTR_2(pwm1_interpolate, S_IRUGO|S_IWUSR,
+		      show_pwm_interpolate, store_pwm_interpolate, 0, 0),
+	SENSOR_ATTR_2(pwm1_auto_channels_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_channel,
+		      store_pwm_auto_point_channel, 0, 0),
+
+	SENSOR_ATTR_2(pwm2, S_IRUGO|S_IWUSR, show_pwm, store_pwm, 0, 1),
+	SENSOR_ATTR_2(pwm2_enable, S_IRUGO|S_IWUSR, show_pwm_enable,
+		      store_pwm_enable, 0, 1),
+	SENSOR_ATTR_2(pwm2_interpolate, S_IRUGO|S_IWUSR,
+		      show_pwm_interpolate, store_pwm_interpolate, 0, 1),
+	SENSOR_ATTR_2(pwm2_auto_channels_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_channel,
+		      store_pwm_auto_point_channel, 0, 1),
+
+	SENSOR_ATTR_2(pwm3_interpolate, S_IRUGO|S_IWUSR,
+		      show_pwm_interpolate, store_pwm_interpolate, 0, 2),
+	SENSOR_ATTR_2(pwm3_auto_channels_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_channel,
+		      store_pwm_auto_point_channel, 0, 2),
 };
 
-static struct sensor_device_attribute f71882fg_in_temp_attr[] =
-{
-	SENSOR_ATTR(in0_input, S_IRUGO, show_in, NULL, 0),
-	SENSOR_ATTR(in1_input, S_IRUGO, show_in, NULL, 1),
-	SENSOR_ATTR(in1_max, S_IRUGO|S_IWUSR, show_in_max, store_in_max, 1),
-	SENSOR_ATTR(in1_beep, S_IRUGO|S_IWUSR, show_in_beep, store_in_beep, 1),
-	SENSOR_ATTR(in1_alarm, S_IRUGO, show_in_alarm, NULL, 1),
-	SENSOR_ATTR(in2_input, S_IRUGO, show_in, NULL, 2),
-	SENSOR_ATTR(in3_input, S_IRUGO, show_in, NULL, 3),
-	SENSOR_ATTR(in4_input, S_IRUGO, show_in, NULL, 4),
-	SENSOR_ATTR(in5_input, S_IRUGO, show_in, NULL, 5),
-	SENSOR_ATTR(in6_input, S_IRUGO, show_in, NULL, 6),
-	SENSOR_ATTR(in7_input, S_IRUGO, show_in, NULL, 7),
-	SENSOR_ATTR(in8_input, S_IRUGO, show_in, NULL, 8),
-	SENSOR_ATTR(temp1_input, S_IRUGO, show_temp, NULL, 0),
-	SENSOR_ATTR(temp1_max, S_IRUGO|S_IWUSR, show_temp_max,
-		store_temp_max, 0),
-	SENSOR_ATTR(temp1_max_hyst, S_IRUGO|S_IWUSR, show_temp_max_hyst,
-		store_temp_max_hyst, 0),
-	SENSOR_ATTR(temp1_crit, S_IRUGO|S_IWUSR, show_temp_crit,
-		store_temp_crit, 0),
-	SENSOR_ATTR(temp1_crit_hyst, S_IRUGO, show_temp_crit_hyst, NULL, 0),
-	SENSOR_ATTR(temp1_type, S_IRUGO, show_temp_type, NULL, 0),
-	SENSOR_ATTR(temp1_beep, S_IRUGO|S_IWUSR, show_temp_beep,
-		store_temp_beep, 0),
-	SENSOR_ATTR(temp1_alarm, S_IRUGO, show_temp_alarm, NULL, 0),
-	SENSOR_ATTR(temp1_fault, S_IRUGO, show_temp_fault, NULL, 0),
-	SENSOR_ATTR(temp2_input, S_IRUGO, show_temp, NULL, 1),
-	SENSOR_ATTR(temp2_max, S_IRUGO|S_IWUSR, show_temp_max,
-		store_temp_max, 1),
-	SENSOR_ATTR(temp2_max_hyst, S_IRUGO|S_IWUSR, show_temp_max_hyst,
-		store_temp_max_hyst, 1),
-	SENSOR_ATTR(temp2_crit, S_IRUGO|S_IWUSR, show_temp_crit,
-		store_temp_crit, 1),
-	SENSOR_ATTR(temp2_crit_hyst, S_IRUGO, show_temp_crit_hyst, NULL, 1),
-	SENSOR_ATTR(temp2_type, S_IRUGO, show_temp_type, NULL, 1),
-	SENSOR_ATTR(temp2_beep, S_IRUGO|S_IWUSR, show_temp_beep,
-		store_temp_beep, 1),
-	SENSOR_ATTR(temp2_alarm, S_IRUGO, show_temp_alarm, NULL, 1),
-	SENSOR_ATTR(temp2_fault, S_IRUGO, show_temp_fault, NULL, 1),
-	SENSOR_ATTR(temp3_input, S_IRUGO, show_temp, NULL, 2),
-	SENSOR_ATTR(temp3_max, S_IRUGO|S_IWUSR, show_temp_max,
-		store_temp_max, 2),
-	SENSOR_ATTR(temp3_max_hyst, S_IRUGO|S_IWUSR, show_temp_max_hyst,
-		store_temp_max_hyst, 2),
-	SENSOR_ATTR(temp3_crit, S_IRUGO|S_IWUSR, show_temp_crit,
-		store_temp_crit, 2),
-	SENSOR_ATTR(temp3_crit_hyst, S_IRUGO, show_temp_crit_hyst, NULL, 2),
-	SENSOR_ATTR(temp3_type, S_IRUGO, show_temp_type, NULL, 2),
-	SENSOR_ATTR(temp3_beep, S_IRUGO|S_IWUSR, show_temp_beep,
-		store_temp_beep, 2),
-	SENSOR_ATTR(temp3_alarm, S_IRUGO, show_temp_alarm, NULL, 2),
-	SENSOR_ATTR(temp3_fault, S_IRUGO, show_temp_fault, NULL, 2)
+/* Fan / PWM attr for the f71862fg, less pwms and less zones per pwm than the
+   f71882fg */
+static struct sensor_device_attribute_2 f71862fg_fan_attr[] = {
+	SENSOR_ATTR_2(fan1_beep, S_IRUGO|S_IWUSR, show_fan_beep,
+		store_fan_beep, 0, 0),
+	SENSOR_ATTR_2(fan2_beep, S_IRUGO|S_IWUSR, show_fan_beep,
+		store_fan_beep, 0, 1),
+	SENSOR_ATTR_2(fan3_beep, S_IRUGO|S_IWUSR, show_fan_beep,
+		store_fan_beep, 0, 2),
+
+	SENSOR_ATTR_2(pwm1_auto_point1_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      1, 0),
+	SENSOR_ATTR_2(pwm1_auto_point2_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      4, 0),
+	SENSOR_ATTR_2(pwm1_auto_point1_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      0, 0),
+	SENSOR_ATTR_2(pwm1_auto_point2_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      3, 0),
+	SENSOR_ATTR_2(pwm1_auto_point1_temp_hyst, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp_hyst,
+		      store_pwm_auto_point_temp_hyst,
+		      0, 0),
+	SENSOR_ATTR_2(pwm1_auto_point2_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 3, 0),
+
+	SENSOR_ATTR_2(pwm2_auto_point1_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      1, 1),
+	SENSOR_ATTR_2(pwm2_auto_point2_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      4, 1),
+	SENSOR_ATTR_2(pwm2_auto_point1_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      0, 1),
+	SENSOR_ATTR_2(pwm2_auto_point2_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      3, 1),
+	SENSOR_ATTR_2(pwm2_auto_point1_temp_hyst, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp_hyst,
+		      store_pwm_auto_point_temp_hyst,
+		      0, 1),
+	SENSOR_ATTR_2(pwm2_auto_point2_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 3, 1),
+
+	SENSOR_ATTR_2(pwm3, S_IRUGO|S_IWUSR, show_pwm, store_pwm, 0, 2),
+	SENSOR_ATTR_2(pwm3_enable, S_IRUGO|S_IWUSR, show_pwm_enable,
+		      store_pwm_enable, 0, 2),
+	SENSOR_ATTR_2(pwm3_auto_point1_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      1, 2),
+	SENSOR_ATTR_2(pwm3_auto_point2_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      4, 2),
+	SENSOR_ATTR_2(pwm3_auto_point1_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      0, 2),
+	SENSOR_ATTR_2(pwm3_auto_point2_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      3, 2),
+	SENSOR_ATTR_2(pwm3_auto_point1_temp_hyst, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp_hyst,
+		      store_pwm_auto_point_temp_hyst,
+		      0, 2),
+	SENSOR_ATTR_2(pwm3_auto_point2_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 3, 2),
 };
 
-static struct sensor_device_attribute f71882fg_fan_attr[] =
-{
-	SENSOR_ATTR(fan1_input, S_IRUGO, show_fan, NULL, 0),
-	SENSOR_ATTR(fan1_beep, S_IRUGO|S_IWUSR, show_fan_beep,
-		store_fan_beep, 0),
-	SENSOR_ATTR(fan1_alarm, S_IRUGO, show_fan_alarm, NULL, 0),
-	SENSOR_ATTR(fan2_input, S_IRUGO, show_fan, NULL, 1),
-	SENSOR_ATTR(fan2_beep, S_IRUGO|S_IWUSR, show_fan_beep,
-		store_fan_beep, 1),
-	SENSOR_ATTR(fan2_alarm, S_IRUGO, show_fan_alarm, NULL, 1),
-	SENSOR_ATTR(fan3_input, S_IRUGO, show_fan, NULL, 2),
-	SENSOR_ATTR(fan3_beep, S_IRUGO|S_IWUSR, show_fan_beep,
-		store_fan_beep, 2),
-	SENSOR_ATTR(fan3_alarm, S_IRUGO, show_fan_alarm, NULL, 2),
-	SENSOR_ATTR(fan4_input, S_IRUGO, show_fan, NULL, 3),
-	SENSOR_ATTR(fan4_beep, S_IRUGO|S_IWUSR, show_fan_beep,
-		store_fan_beep, 3),
-	SENSOR_ATTR(fan4_alarm, S_IRUGO, show_fan_alarm, NULL, 3)
+/* Fan / PWM attr for the f71882fg */
+static struct sensor_device_attribute_2 f71882fg_fan_attr[] = {
+	SENSOR_ATTR_2(fan1_beep, S_IRUGO|S_IWUSR, show_fan_beep,
+		store_fan_beep, 0, 0),
+	SENSOR_ATTR_2(fan2_beep, S_IRUGO|S_IWUSR, show_fan_beep,
+		store_fan_beep, 0, 1),
+	SENSOR_ATTR_2(fan3_beep, S_IRUGO|S_IWUSR, show_fan_beep,
+		store_fan_beep, 0, 2),
+	SENSOR_ATTR_2(fan4_input, S_IRUGO, show_fan, NULL, 0, 3),
+	SENSOR_ATTR_2(fan4_full_speed, S_IRUGO|S_IWUSR,
+		      show_fan_full_speed,
+		      store_fan_full_speed, 0, 3),
+	SENSOR_ATTR_2(fan4_beep, S_IRUGO|S_IWUSR, show_fan_beep,
+		store_fan_beep, 0, 3),
+	SENSOR_ATTR_2(fan4_alarm, S_IRUGO, show_fan_alarm, NULL, 0, 3),
+
+	SENSOR_ATTR_2(pwm1_auto_point1_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      0, 0),
+	SENSOR_ATTR_2(pwm1_auto_point2_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      1, 0),
+	SENSOR_ATTR_2(pwm1_auto_point3_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      2, 0),
+	SENSOR_ATTR_2(pwm1_auto_point4_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      3, 0),
+	SENSOR_ATTR_2(pwm1_auto_point5_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      4, 0),
+	SENSOR_ATTR_2(pwm1_auto_point1_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      0, 0),
+	SENSOR_ATTR_2(pwm1_auto_point2_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      1, 0),
+	SENSOR_ATTR_2(pwm1_auto_point3_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      2, 0),
+	SENSOR_ATTR_2(pwm1_auto_point4_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      3, 0),
+	SENSOR_ATTR_2(pwm1_auto_point1_temp_hyst, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp_hyst,
+		      store_pwm_auto_point_temp_hyst,
+		      0, 0),
+	SENSOR_ATTR_2(pwm1_auto_point2_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 1, 0),
+	SENSOR_ATTR_2(pwm1_auto_point3_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 2, 0),
+	SENSOR_ATTR_2(pwm1_auto_point4_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 3, 0),
+
+	SENSOR_ATTR_2(pwm2_auto_point1_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      0, 1),
+	SENSOR_ATTR_2(pwm2_auto_point2_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      1, 1),
+	SENSOR_ATTR_2(pwm2_auto_point3_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      2, 1),
+	SENSOR_ATTR_2(pwm2_auto_point4_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      3, 1),
+	SENSOR_ATTR_2(pwm2_auto_point5_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      4, 1),
+	SENSOR_ATTR_2(pwm2_auto_point1_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      0, 1),
+	SENSOR_ATTR_2(pwm2_auto_point2_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      1, 1),
+	SENSOR_ATTR_2(pwm2_auto_point3_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      2, 1),
+	SENSOR_ATTR_2(pwm2_auto_point4_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      3, 1),
+	SENSOR_ATTR_2(pwm2_auto_point1_temp_hyst, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp_hyst,
+		      store_pwm_auto_point_temp_hyst,
+		      0, 1),
+	SENSOR_ATTR_2(pwm2_auto_point2_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 1, 1),
+	SENSOR_ATTR_2(pwm2_auto_point3_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 2, 1),
+	SENSOR_ATTR_2(pwm2_auto_point4_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 3, 1),
+
+	SENSOR_ATTR_2(pwm3, S_IRUGO|S_IWUSR, show_pwm, store_pwm, 0, 2),
+	SENSOR_ATTR_2(pwm3_enable, S_IRUGO|S_IWUSR, show_pwm_enable,
+		      store_pwm_enable, 0, 2),
+	SENSOR_ATTR_2(pwm3_auto_point1_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      0, 2),
+	SENSOR_ATTR_2(pwm3_auto_point2_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      1, 2),
+	SENSOR_ATTR_2(pwm3_auto_point3_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      2, 2),
+	SENSOR_ATTR_2(pwm3_auto_point4_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      3, 2),
+	SENSOR_ATTR_2(pwm3_auto_point5_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      4, 2),
+	SENSOR_ATTR_2(pwm3_auto_point1_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      0, 2),
+	SENSOR_ATTR_2(pwm3_auto_point2_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      1, 2),
+	SENSOR_ATTR_2(pwm3_auto_point3_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      2, 2),
+	SENSOR_ATTR_2(pwm3_auto_point4_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      3, 2),
+	SENSOR_ATTR_2(pwm3_auto_point1_temp_hyst, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp_hyst,
+		      store_pwm_auto_point_temp_hyst,
+		      0, 2),
+	SENSOR_ATTR_2(pwm3_auto_point2_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 1, 2),
+	SENSOR_ATTR_2(pwm3_auto_point3_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 2, 2),
+	SENSOR_ATTR_2(pwm3_auto_point4_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 3, 2),
+
+	SENSOR_ATTR_2(pwm4, S_IRUGO|S_IWUSR, show_pwm, store_pwm, 0, 3),
+	SENSOR_ATTR_2(pwm4_enable, S_IRUGO|S_IWUSR, show_pwm_enable,
+		      store_pwm_enable, 0, 3),
+	SENSOR_ATTR_2(pwm4_interpolate, S_IRUGO|S_IWUSR,
+		      show_pwm_interpolate, store_pwm_interpolate, 0, 3),
+	SENSOR_ATTR_2(pwm4_auto_channels_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_channel,
+		      store_pwm_auto_point_channel, 0, 3),
+	SENSOR_ATTR_2(pwm4_auto_point1_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      0, 3),
+	SENSOR_ATTR_2(pwm4_auto_point2_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      1, 3),
+	SENSOR_ATTR_2(pwm4_auto_point3_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      2, 3),
+	SENSOR_ATTR_2(pwm4_auto_point4_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      3, 3),
+	SENSOR_ATTR_2(pwm4_auto_point5_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      4, 3),
+	SENSOR_ATTR_2(pwm4_auto_point1_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      0, 3),
+	SENSOR_ATTR_2(pwm4_auto_point2_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      1, 3),
+	SENSOR_ATTR_2(pwm4_auto_point3_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      2, 3),
+	SENSOR_ATTR_2(pwm4_auto_point4_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      3, 3),
+	SENSOR_ATTR_2(pwm4_auto_point1_temp_hyst, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp_hyst,
+		      store_pwm_auto_point_temp_hyst,
+		      0, 3),
+	SENSOR_ATTR_2(pwm4_auto_point2_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 1, 3),
+	SENSOR_ATTR_2(pwm4_auto_point3_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 2, 3),
+	SENSOR_ATTR_2(pwm4_auto_point4_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 3, 3),
 };
 
+/* Fan / PWM attr for the f8000, zones mapped to temp instead of to pwm!
+   Also the register block at offset A0 maps to TEMP1 (so our temp2, as the
+   F8000 starts counting temps at 0), B0 maps the TEMP2 and C0 maps to TEMP0 */
+static struct sensor_device_attribute_2 f8000_fan_attr[] = {
+	SENSOR_ATTR_2(fan4_input, S_IRUGO, show_fan, NULL, 0, 3),
+
+	SENSOR_ATTR_2(pwm3, S_IRUGO, show_pwm, NULL, 0, 2),
+
+	SENSOR_ATTR_2(temp1_auto_point1_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      0, 2),
+	SENSOR_ATTR_2(temp1_auto_point2_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      1, 2),
+	SENSOR_ATTR_2(temp1_auto_point3_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      2, 2),
+	SENSOR_ATTR_2(temp1_auto_point4_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      3, 2),
+	SENSOR_ATTR_2(temp1_auto_point5_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      4, 2),
+	SENSOR_ATTR_2(temp1_auto_point1_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      0, 2),
+	SENSOR_ATTR_2(temp1_auto_point2_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      1, 2),
+	SENSOR_ATTR_2(temp1_auto_point3_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      2, 2),
+	SENSOR_ATTR_2(temp1_auto_point4_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      3, 2),
+	SENSOR_ATTR_2(temp1_auto_point1_temp_hyst, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp_hyst,
+		      store_pwm_auto_point_temp_hyst,
+		      0, 2),
+	SENSOR_ATTR_2(temp1_auto_point2_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 1, 2),
+	SENSOR_ATTR_2(temp1_auto_point3_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 2, 2),
+	SENSOR_ATTR_2(temp1_auto_point4_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 3, 2),
+
+	SENSOR_ATTR_2(temp2_auto_point1_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      0, 0),
+	SENSOR_ATTR_2(temp2_auto_point2_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      1, 0),
+	SENSOR_ATTR_2(temp2_auto_point3_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      2, 0),
+	SENSOR_ATTR_2(temp2_auto_point4_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      3, 0),
+	SENSOR_ATTR_2(temp2_auto_point5_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      4, 0),
+	SENSOR_ATTR_2(temp2_auto_point1_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      0, 0),
+	SENSOR_ATTR_2(temp2_auto_point2_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      1, 0),
+	SENSOR_ATTR_2(temp2_auto_point3_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      2, 0),
+	SENSOR_ATTR_2(temp2_auto_point4_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      3, 0),
+	SENSOR_ATTR_2(temp2_auto_point1_temp_hyst, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp_hyst,
+		      store_pwm_auto_point_temp_hyst,
+		      0, 0),
+	SENSOR_ATTR_2(temp2_auto_point2_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 1, 0),
+	SENSOR_ATTR_2(temp2_auto_point3_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 2, 0),
+	SENSOR_ATTR_2(temp2_auto_point4_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 3, 0),
+
+	SENSOR_ATTR_2(temp3_auto_point1_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      0, 1),
+	SENSOR_ATTR_2(temp3_auto_point2_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      1, 1),
+	SENSOR_ATTR_2(temp3_auto_point3_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      2, 1),
+	SENSOR_ATTR_2(temp3_auto_point4_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      3, 1),
+	SENSOR_ATTR_2(temp3_auto_point5_pwm, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_pwm, store_pwm_auto_point_pwm,
+		      4, 1),
+	SENSOR_ATTR_2(temp3_auto_point1_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      0, 1),
+	SENSOR_ATTR_2(temp3_auto_point2_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      1, 1),
+	SENSOR_ATTR_2(temp3_auto_point3_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      2, 1),
+	SENSOR_ATTR_2(temp3_auto_point4_temp, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp, store_pwm_auto_point_temp,
+		      3, 1),
+	SENSOR_ATTR_2(temp3_auto_point1_temp_hyst, S_IRUGO|S_IWUSR,
+		      show_pwm_auto_point_temp_hyst,
+		      store_pwm_auto_point_temp_hyst,
+		      0, 1),
+	SENSOR_ATTR_2(temp3_auto_point2_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 1, 1),
+	SENSOR_ATTR_2(temp3_auto_point3_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 2, 1),
+	SENSOR_ATTR_2(temp3_auto_point4_temp_hyst, S_IRUGO,
+		      show_pwm_auto_point_temp_hyst, NULL, 3, 1),
+};
 
 /* Super I/O functions */
 static inline int superio_inb(int base, int reg)
@@ -299,11 +811,16 @@ static inline void superio_exit(int base)
 	outb(SIO_LOCK_KEY, base);
 }
 
-static inline u16 fan_from_reg(u16 reg)
+static inline int fan_from_reg(u16 reg)
 {
 	return reg ? (1500000 / reg) : 0;
 }
 
+static inline u16 fan_to_reg(int fan)
+{
+	return fan ? (1500000 / fan) : 0;
+}
+
 static u8 f71882fg_read8(struct f71882fg_data *data, u8 reg)
 {
 	u8 val;
@@ -332,52 +849,111 @@ static void f71882fg_write8(struct f71882fg_data *data, u8 reg, u8 val)
 	outb(val, data->addr + DATA_REG_OFFSET);
 }
 
-static struct f71882fg_data *f71882fg_update_device(struct device * dev)
+static void f71882fg_write16(struct f71882fg_data *data, u8 reg, u16 val)
+{
+	outb(reg++, data->addr + ADDR_REG_OFFSET);
+	outb(val >> 8, data->addr + DATA_REG_OFFSET);
+	outb(reg, data->addr + ADDR_REG_OFFSET);
+	outb(val & 255, data->addr + DATA_REG_OFFSET);
+}
+
+static struct f71882fg_data *f71882fg_update_device(struct device *dev)
 {
 	struct f71882fg_data *data = dev_get_drvdata(dev);
-	int nr, reg, reg2;
+	int nr, reg = 0, reg2;
+	int nr_fans = (data->type == f71882fg) ? 4 : 3;
+	int nr_ins = (data->type == f8000) ? 3 : 9;
+	int temp_start = (data->type == f8000) ? 0 : 1;
 
 	mutex_lock(&data->update_lock);
 
 	/* Update once every 60 seconds */
 	if ( time_after(jiffies, data->last_limits + 60 * HZ ) ||
 			!data->valid) {
-		data->in1_max = f71882fg_read8(data, F71882FG_REG_IN1_HIGH);
-		data->in_beep = f71882fg_read8(data, F71882FG_REG_IN_BEEP);
+		if (data->type == f71882fg) {
+			data->in1_max =
+				f71882fg_read8(data, F71882FG_REG_IN1_HIGH);
+			data->in_beep =
+				f71882fg_read8(data, F71882FG_REG_IN_BEEP);
+		}
 
 		/* Get High & boundary temps*/
-		for (nr = 0; nr < 3; nr++) {
+		for (nr = temp_start; nr < 3 + temp_start; nr++) {
 			data->temp_ovt[nr] = f71882fg_read8(data,
 						F71882FG_REG_TEMP_OVT(nr));
 			data->temp_high[nr] = f71882fg_read8(data,
 						F71882FG_REG_TEMP_HIGH(nr));
 		}
 
-		/* Have to hardcode hyst*/
-		data->temp_hyst[0] = f71882fg_read8(data,
-						F71882FG_REG_TEMP_HYST1) >> 4;
-		/* Hyst temps 2 & 3 stored in same register */
-		reg = f71882fg_read8(data, F71882FG_REG_TEMP_HYST23);
-		data->temp_hyst[1] = reg & 0x0F;
-		data->temp_hyst[2] = reg >> 4;
-
-		/* Have to hardcode type, because temp1 is special */
-		reg  = f71882fg_read8(data, F71882FG_REG_TEMP_TYPE);
+		if (data->type != f8000) {
+			data->fan_beep = f71882fg_read8(data,
+						F71882FG_REG_FAN_BEEP);
+			data->temp_beep = f71882fg_read8(data,
+						F71882FG_REG_TEMP_BEEP);
+			data->temp_hyst[0] = f71882fg_read8(data,
+						F71882FG_REG_TEMP_HYST(0));
+			data->temp_hyst[1] = f71882fg_read8(data,
+						F71882FG_REG_TEMP_HYST(1));
+			/* Have to hardcode type, because temp1 is special */
+			reg  = f71882fg_read8(data, F71882FG_REG_TEMP_TYPE);
+			data->temp_type[2] = (reg & 0x04) ? 2 : 4;
+			data->temp_type[3] = (reg & 0x08) ? 2 : 4;
+		}
 		reg2 = f71882fg_read8(data, F71882FG_REG_PECI);
 		if ((reg2 & 0x03) == 0x01)
-			data->temp_type[0] = 6 /* PECI */;
+			data->temp_type[1] = 6 /* PECI */;
 		else if ((reg2 & 0x03) == 0x02)
-			data->temp_type[0] = 5 /* AMDSI */;
+			data->temp_type[1] = 5 /* AMDSI */;
+		else if (data->type != f8000)
+			data->temp_type[1] = (reg & 0x02) ? 2 : 4;
 		else
-			data->temp_type[0] = (reg & 0x02) ? 2 : 4;
-
-		data->temp_type[1] = (reg & 0x04) ? 2 : 4;
-		data->temp_type[2] = (reg & 0x08) ? 2 : 4;
-
-		data->temp_beep = f71882fg_read8(data, F71882FG_REG_TEMP_BEEP);
-
-		data->fan_beep = f71882fg_read8(data, F71882FG_REG_FAN_BEEP);
-
+			data->temp_type[1] = 2; /* F8000 only supports BJT */
+
+		data->pwm_enable = f71882fg_read8(data,
+						  F71882FG_REG_PWM_ENABLE);
+		data->pwm_auto_point_hyst[0] =
+			f71882fg_read8(data, F71882FG_REG_FAN_HYST(0));
+		data->pwm_auto_point_hyst[1] =
+			f71882fg_read8(data, F71882FG_REG_FAN_HYST(1));
+
+		for (nr = 0; nr < nr_fans; nr++) {
+			data->pwm_auto_point_mapping[nr] =
+			    f71882fg_read8(data,
+					   F71882FG_REG_POINT_MAPPING(nr));
+
+			if (data->type != f71862fg) {
+				int point;
+				for (point = 0; point < 5; point++) {
+					data->pwm_auto_point_pwm[nr][point] =
+						f71882fg_read8(data,
+							F71882FG_REG_POINT_PWM
+							(nr, point));
+				}
+				for (point = 0; point < 4; point++) {
+					data->pwm_auto_point_temp[nr][point] =
+						f71882fg_read8(data,
+							F71882FG_REG_POINT_TEMP
+							(nr, point));
+				}
+			} else {
+				data->pwm_auto_point_pwm[nr][1] =
+					f71882fg_read8(data,
+						F71882FG_REG_POINT_PWM
+						(nr, 1));
+				data->pwm_auto_point_pwm[nr][4] =
+					f71882fg_read8(data,
+						F71882FG_REG_POINT_PWM
+						(nr, 4));
+				data->pwm_auto_point_temp[nr][0] =
+					f71882fg_read8(data,
+						F71882FG_REG_POINT_TEMP
+						(nr, 0));
+				data->pwm_auto_point_temp[nr][3] =
+					f71882fg_read8(data,
+						F71882FG_REG_POINT_TEMP
+						(nr, 3));
+			}
+		}
 		data->last_limits = jiffies;
 	}
 
@@ -387,19 +963,32 @@ static struct f71882fg_data *f71882fg_update_device(struct device * dev)
 						F71882FG_REG_TEMP_STATUS);
 		data->temp_diode_open = f71882fg_read8(data,
 						F71882FG_REG_TEMP_DIODE_OPEN);
-		for (nr = 0; nr < 3; nr++)
+		for (nr = temp_start; nr < 3 + temp_start; nr++)
 			data->temp[nr] = f71882fg_read8(data,
 						F71882FG_REG_TEMP(nr));
 
 		data->fan_status = f71882fg_read8(data,
 						F71882FG_REG_FAN_STATUS);
-		for (nr = 0; nr < 4; nr++)
+		for (nr = 0; nr < nr_fans; nr++) {
 			data->fan[nr] = f71882fg_read16(data,
 						F71882FG_REG_FAN(nr));
+			data->fan_target[nr] =
+			    f71882fg_read16(data, F71882FG_REG_FAN_TARGET(nr));
+			data->fan_full_speed[nr] =
+			    f71882fg_read16(data,
+					    F71882FG_REG_FAN_FULL_SPEED(nr));
+			data->pwm[nr] =
+			    f71882fg_read8(data, F71882FG_REG_PWM(nr));
+		}
 
-		data->in_status = f71882fg_read8(data,
+		/* The f8000 can monitor 1 more fan, but has no pwm for it */
+		if (data->type == f8000)
+			data->fan[3] = f71882fg_read16(data,
+						F71882FG_REG_FAN(3));
+		if (data->type == f71882fg)
+			data->in_status = f71882fg_read8(data,
 						F71882FG_REG_IN_STATUS);
-		for (nr = 0; nr < 9; nr++)
+		for (nr = 0; nr < nr_ins; nr++)
 			data->in[nr] = f71882fg_read8(data,
 						F71882FG_REG_IN(nr));
 
@@ -417,7 +1006,7 @@ static ssize_t show_fan(struct device *dev, struct device_attribute *devattr,
 	char *buf)
 {
 	struct f71882fg_data *data = f71882fg_update_device(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
 	int speed = fan_from_reg(data->fan[nr]);
 
 	if (speed == FAN_MIN_DETECT)
@@ -426,11 +1015,39 @@ static ssize_t show_fan(struct device *dev, struct device_attribute *devattr,
 	return sprintf(buf, "%d\n", speed);
 }
 
+static ssize_t show_fan_full_speed(struct device *dev,
+				   struct device_attribute *devattr, char *buf)
+{
+	struct f71882fg_data *data = f71882fg_update_device(dev);
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	int speed = fan_from_reg(data->fan_full_speed[nr]);
+	return sprintf(buf, "%d\n", speed);
+}
+
+static ssize_t store_fan_full_speed(struct device *dev,
+				    struct device_attribute *devattr,
+				    const char *buf, size_t count)
+{
+	struct f71882fg_data *data = dev_get_drvdata(dev);
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	long val = simple_strtol(buf, NULL, 10);
+
+	val = SENSORS_LIMIT(val, 23, 1500000);
+	val = fan_to_reg(val);
+
+	mutex_lock(&data->update_lock);
+	f71882fg_write16(data, F71882FG_REG_FAN_FULL_SPEED(nr), val);
+	data->fan_full_speed[nr] = val;
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
 static ssize_t show_fan_beep(struct device *dev, struct device_attribute
 	*devattr, char *buf)
 {
 	struct f71882fg_data *data = f71882fg_update_device(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
 
 	if (data->fan_beep & (1 << nr))
 		return sprintf(buf, "1\n");
@@ -442,10 +1059,11 @@ static ssize_t store_fan_beep(struct device *dev, struct device_attribute
 	*devattr, const char *buf, size_t count)
 {
 	struct f71882fg_data *data = dev_get_drvdata(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
-	int val = simple_strtoul(buf, NULL, 10);
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	unsigned long val = simple_strtoul(buf, NULL, 10);
 
 	mutex_lock(&data->update_lock);
+	data->fan_beep = f71882fg_read8(data, F71882FG_REG_FAN_BEEP);
 	if (val)
 		data->fan_beep |= 1 << nr;
 	else
@@ -461,7 +1079,7 @@ static ssize_t show_fan_alarm(struct device *dev, struct device_attribute
 	*devattr, char *buf)
 {
 	struct f71882fg_data *data = f71882fg_update_device(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
 
 	if (data->fan_status & (1 << nr))
 		return sprintf(buf, "1\n");
@@ -473,7 +1091,7 @@ static ssize_t show_in(struct device *dev, struct device_attribute *devattr,
 	char *buf)
 {
 	struct f71882fg_data *data = f71882fg_update_device(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
 
 	return sprintf(buf, "%d\n", data->in[nr] * 8);
 }
@@ -490,10 +1108,8 @@ static ssize_t store_in_max(struct device *dev, struct device_attribute
 	*devattr, const char *buf, size_t count)
 {
 	struct f71882fg_data *data = dev_get_drvdata(dev);
-	int val = simple_strtoul(buf, NULL, 10) / 8;
-
-	if (val > 255)
-		val = 255;
+	long val = simple_strtol(buf, NULL, 10) / 8;
+	val = SENSORS_LIMIT(val, 0, 255);
 
 	mutex_lock(&data->update_lock);
 	f71882fg_write8(data, F71882FG_REG_IN1_HIGH, val);
@@ -507,7 +1123,7 @@ static ssize_t show_in_beep(struct device *dev, struct device_attribute
 	*devattr, char *buf)
 {
 	struct f71882fg_data *data = f71882fg_update_device(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
 
 	if (data->in_beep & (1 << nr))
 		return sprintf(buf, "1\n");
@@ -519,10 +1135,11 @@ static ssize_t store_in_beep(struct device *dev, struct device_attribute
 	*devattr, const char *buf, size_t count)
 {
 	struct f71882fg_data *data = dev_get_drvdata(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
-	int val = simple_strtoul(buf, NULL, 10);
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	unsigned long val = simple_strtoul(buf, NULL, 10);
 
 	mutex_lock(&data->update_lock);
+	data->in_beep = f71882fg_read8(data, F71882FG_REG_IN_BEEP);
 	if (val)
 		data->in_beep |= 1 << nr;
 	else
@@ -538,7 +1155,7 @@ static ssize_t show_in_alarm(struct device *dev, struct device_attribute
 	*devattr, char *buf)
 {
 	struct f71882fg_data *data = f71882fg_update_device(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
 
 	if (data->in_status & (1 << nr))
 		return sprintf(buf, "1\n");
@@ -550,7 +1167,7 @@ static ssize_t show_temp(struct device *dev, struct device_attribute *devattr,
 	char *buf)
 {
 	struct f71882fg_data *data = f71882fg_update_device(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
 
 	return sprintf(buf, "%d\n", data->temp[nr] * 1000);
 }
@@ -559,7 +1176,7 @@ static ssize_t show_temp_max(struct device *dev, struct device_attribute
 	*devattr, char *buf)
 {
 	struct f71882fg_data *data = f71882fg_update_device(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
 
 	return sprintf(buf, "%d\n", data->temp_high[nr] * 1000);
 }
@@ -568,11 +1185,9 @@ static ssize_t store_temp_max(struct device *dev, struct device_attribute
 	*devattr, const char *buf, size_t count)
 {
 	struct f71882fg_data *data = dev_get_drvdata(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
-	int val = simple_strtoul(buf, NULL, 10) / 1000;
-
-	if (val > 255)
-		val = 255;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	long val = simple_strtol(buf, NULL, 10) / 1000;
+	val = SENSORS_LIMIT(val, 0, 255);
 
 	mutex_lock(&data->update_lock);
 	f71882fg_write8(data, F71882FG_REG_TEMP_HIGH(nr), val);
@@ -586,48 +1201,46 @@ static ssize_t show_temp_max_hyst(struct device *dev, struct device_attribute
 	*devattr, char *buf)
 {
 	struct f71882fg_data *data = f71882fg_update_device(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	int temp_max_hyst;
+
+	mutex_lock(&data->update_lock);
+	if (nr & 1)
+		temp_max_hyst = data->temp_hyst[nr / 2] >> 4;
+	else
+		temp_max_hyst = data->temp_hyst[nr / 2] & 0x0f;
+	temp_max_hyst = (data->temp_high[nr] - temp_max_hyst) * 1000;
+	mutex_unlock(&data->update_lock);
 
-	return sprintf(buf, "%d\n",
-		(data->temp_high[nr] - data->temp_hyst[nr]) * 1000);
+	return sprintf(buf, "%d\n", temp_max_hyst);
 }
 
 static ssize_t store_temp_max_hyst(struct device *dev, struct device_attribute
 	*devattr, const char *buf, size_t count)
 {
 	struct f71882fg_data *data = dev_get_drvdata(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
-	int val = simple_strtoul(buf, NULL, 10) / 1000;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	long val = simple_strtol(buf, NULL, 10) / 1000;
 	ssize_t ret = count;
+	u8 reg;
 
 	mutex_lock(&data->update_lock);
 
 	/* convert abs to relative and check */
+	data->temp_high[nr] = f71882fg_read8(data, F71882FG_REG_TEMP_HIGH(nr));
+	val = SENSORS_LIMIT(val, data->temp_high[nr] - 15,
+			    data->temp_high[nr]);
 	val = data->temp_high[nr] - val;
-	if (val < 0 || val > 15) {
-		ret = -EINVAL;
-		goto store_temp_max_hyst_exit;
-	}
-
-	data->temp_hyst[nr] = val;
 
 	/* convert value to register contents */
-	switch (nr) {
-		case 0:
-			val = val << 4;
-			break;
-		case 1:
-			val = val | (data->temp_hyst[2] << 4);
-			break;
-		case 2:
-			val = data->temp_hyst[1] | (val << 4);
-			break;
-	}
-
-	f71882fg_write8(data, nr ? F71882FG_REG_TEMP_HYST23 :
-		F71882FG_REG_TEMP_HYST1, val);
+	reg = f71882fg_read8(data, F71882FG_REG_TEMP_HYST(nr / 2));
+	if (nr & 1)
+		reg = (reg & 0x0f) | (val << 4);
+	else
+		reg = (reg & 0xf0) | val;
+	f71882fg_write8(data, F71882FG_REG_TEMP_HYST(nr / 2), reg);
+	data->temp_hyst[nr / 2] = reg;
 
-store_temp_max_hyst_exit:
 	mutex_unlock(&data->update_lock);
 	return ret;
 }
@@ -636,7 +1249,7 @@ static ssize_t show_temp_crit(struct device *dev, struct device_attribute
 	*devattr, char *buf)
 {
 	struct f71882fg_data *data = f71882fg_update_device(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
 
 	return sprintf(buf, "%d\n", data->temp_ovt[nr] * 1000);
 }
@@ -645,11 +1258,9 @@ static ssize_t store_temp_crit(struct device *dev, struct device_attribute
 	*devattr, const char *buf, size_t count)
 {
 	struct f71882fg_data *data = dev_get_drvdata(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
-	int val = simple_strtoul(buf, NULL, 10) / 1000;
-
-	if (val > 255)
-		val = 255;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	long val = simple_strtol(buf, NULL, 10) / 1000;
+	val = SENSORS_LIMIT(val, 0, 255);
 
 	mutex_lock(&data->update_lock);
 	f71882fg_write8(data, F71882FG_REG_TEMP_OVT(nr), val);
@@ -663,17 +1274,25 @@ static ssize_t show_temp_crit_hyst(struct device *dev, struct device_attribute
 	*devattr, char *buf)
 {
 	struct f71882fg_data *data = f71882fg_update_device(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	int temp_crit_hyst;
+
+	mutex_lock(&data->update_lock);
+	if (nr & 1)
+		temp_crit_hyst = data->temp_hyst[nr / 2] >> 4;
+	else
+		temp_crit_hyst = data->temp_hyst[nr / 2] & 0x0f;
+	temp_crit_hyst = (data->temp_ovt[nr] - temp_crit_hyst) * 1000;
+	mutex_unlock(&data->update_lock);
 
-	return sprintf(buf, "%d\n",
-		(data->temp_ovt[nr] - data->temp_hyst[nr]) * 1000);
+	return sprintf(buf, "%d\n", temp_crit_hyst);
 }
 
 static ssize_t show_temp_type(struct device *dev, struct device_attribute
 	*devattr, char *buf)
 {
 	struct f71882fg_data *data = f71882fg_update_device(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
 
 	return sprintf(buf, "%d\n", data->temp_type[nr]);
 }
@@ -682,9 +1301,9 @@ static ssize_t show_temp_beep(struct device *dev, struct device_attribute
 	*devattr, char *buf)
 {
 	struct f71882fg_data *data = f71882fg_update_device(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
 
-	if (data->temp_beep & (1 << (nr + 1)))
+	if (data->temp_beep & (1 << nr))
 		return sprintf(buf, "1\n");
 	else
 		return sprintf(buf, "0\n");
@@ -694,14 +1313,15 @@ static ssize_t store_temp_beep(struct device *dev, struct device_attribute
 	*devattr, const char *buf, size_t count)
 {
 	struct f71882fg_data *data = dev_get_drvdata(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
-	int val = simple_strtoul(buf, NULL, 10);
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	unsigned long val = simple_strtoul(buf, NULL, 10);
 
 	mutex_lock(&data->update_lock);
+	data->temp_beep = f71882fg_read8(data, F71882FG_REG_TEMP_BEEP);
 	if (val)
-		data->temp_beep |= 1 << (nr + 1);
+		data->temp_beep |= 1 << nr;
 	else
-		data->temp_beep &= ~(1 << (nr + 1));
+		data->temp_beep &= ~(1 << nr);
 
 	f71882fg_write8(data, F71882FG_REG_TEMP_BEEP, data->temp_beep);
 	mutex_unlock(&data->update_lock);
@@ -713,9 +1333,9 @@ static ssize_t show_temp_alarm(struct device *dev, struct device_attribute
 	*devattr, char *buf)
 {
 	struct f71882fg_data *data = f71882fg_update_device(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
 
-	if (data->temp_status & (1 << (nr + 1)))
+	if (data->temp_status & (1 << nr))
 		return sprintf(buf, "1\n");
 	else
 		return sprintf(buf, "0\n");
@@ -725,113 +1345,528 @@ static ssize_t show_temp_fault(struct device *dev, struct device_attribute
 	*devattr, char *buf)
 {
 	struct f71882fg_data *data = f71882fg_update_device(dev);
-	int nr = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->index;
 
-	if (data->temp_diode_open & (1 << (nr + 1)))
+	if (data->temp_diode_open & (1 << nr))
 		return sprintf(buf, "1\n");
 	else
 		return sprintf(buf, "0\n");
 }
 
+static ssize_t show_pwm(struct device *dev,
+			struct device_attribute *devattr, char *buf)
+{
+	struct f71882fg_data *data = f71882fg_update_device(dev);
+	int val, nr = to_sensor_dev_attr_2(devattr)->index;
+	mutex_lock(&data->update_lock);
+	if (data->pwm_enable & (1 << (2 * nr)))
+		/* PWM mode */
+		val = data->pwm[nr];
+	else {
+		/* RPM mode */
+		val = 255 * fan_from_reg(data->fan_target[nr])
+			/ fan_from_reg(data->fan_full_speed[nr]);
+	}
+	mutex_unlock(&data->update_lock);
+	return sprintf(buf, "%d\n", val);
+}
+
+static ssize_t store_pwm(struct device *dev,
+			 struct device_attribute *devattr, const char *buf,
+			 size_t count)
+{
+	struct f71882fg_data *data = dev_get_drvdata(dev);
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	long val = simple_strtol(buf, NULL, 10);
+	val = SENSORS_LIMIT(val, 0, 255);
+
+	mutex_lock(&data->update_lock);
+	data->pwm_enable = f71882fg_read8(data, F71882FG_REG_PWM_ENABLE);
+	if ((data->type == f8000 && ((data->pwm_enable >> 2 * nr) & 3) != 2) ||
+	    (data->type != f8000 && !((data->pwm_enable >> 2 * nr) & 2))) {
+		count = -EROFS;
+		goto leave;
+	}
+	if (data->pwm_enable & (1 << (2 * nr))) {
+		/* PWM mode */
+		f71882fg_write8(data, F71882FG_REG_PWM(nr), val);
+		data->pwm[nr] = val;
+	} else {
+		/* RPM mode */
+		int target, full_speed;
+		full_speed = f71882fg_read16(data,
+					     F71882FG_REG_FAN_FULL_SPEED(nr));
+		target = fan_to_reg(val * fan_from_reg(full_speed) / 255);
+		f71882fg_write16(data, F71882FG_REG_FAN_TARGET(nr), target);
+		data->fan_target[nr] = target;
+		data->fan_full_speed[nr] = full_speed;
+	}
+leave:
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t show_pwm_enable(struct device *dev,
+			       struct device_attribute *devattr, char *buf)
+{
+	int result = 0;
+	struct f71882fg_data *data = f71882fg_update_device(dev);
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+
+	switch ((data->pwm_enable >> 2 * nr) & 3) {
+	case 0:
+	case 1:
+		result = 2; /* Normal auto mode */
+		break;
+	case 2:
+		result = 1; /* Manual mode */
+		break;
+	case 3:
+		if (data->type == f8000)
+			result = 3; /* Thermostat mode */
+		else
+			result = 1; /* Manual mode */
+		break;
+	}
+
+	return sprintf(buf, "%d\n", result);
+}
+
+static ssize_t store_pwm_enable(struct device *dev, struct device_attribute
+				*devattr, const char *buf, size_t count)
+{
+	struct f71882fg_data *data = dev_get_drvdata(dev);
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	long val = simple_strtol(buf, NULL, 10);
+
+	mutex_lock(&data->update_lock);
+	data->pwm_enable = f71882fg_read8(data, F71882FG_REG_PWM_ENABLE);
+	/* Special case for F8000 auto PWM mode / Thermostat mode */
+	if (data->type == f8000 && ((data->pwm_enable >> 2 * nr) & 1)) {
+		switch (val) {
+		case 2:
+			data->pwm_enable &= ~(2 << (2 * nr));
+			break;		/* Normal auto mode */
+		case 3:
+			data->pwm_enable |= 2 << (2 * nr);
+			break;		/* Thermostat mode */
+		default:
+			count = -EINVAL;
+			goto leave;
+		}
+	} else {
+		switch (val) {
+		case 1:
+			data->pwm_enable |= 2 << (2 * nr);
+			break;		/* Manual */
+		case 2:
+			data->pwm_enable &= ~(2 << (2 * nr));
+			break;		/* Normal auto mode */
+		default:
+			count = -EINVAL;
+			goto leave;
+		}
+	}
+	f71882fg_write8(data, F71882FG_REG_PWM_ENABLE, data->pwm_enable);
+leave:
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t show_pwm_auto_point_pwm(struct device *dev,
+				       struct device_attribute *devattr,
+				       char *buf)
+{
+	int result;
+	struct f71882fg_data *data = f71882fg_update_device(dev);
+	int pwm = to_sensor_dev_attr_2(devattr)->index;
+	int point = to_sensor_dev_attr_2(devattr)->nr;
+
+	mutex_lock(&data->update_lock);
+	if (data->pwm_enable & (1 << (2 * pwm))) {
+		/* PWM mode */
+		result = data->pwm_auto_point_pwm[pwm][point];
+	} else {
+		/* RPM mode */
+		result = 32 * 255 / (32 + data->pwm_auto_point_pwm[pwm][point]);
+	}
+	mutex_unlock(&data->update_lock);
+
+	return sprintf(buf, "%d\n", result);
+}
+
+static ssize_t store_pwm_auto_point_pwm(struct device *dev,
+					struct device_attribute *devattr,
+					const char *buf, size_t count)
+{
+	struct f71882fg_data *data = dev_get_drvdata(dev);
+	int pwm = to_sensor_dev_attr_2(devattr)->index;
+	int point = to_sensor_dev_attr_2(devattr)->nr;
+	long val = simple_strtol(buf, NULL, 10);
+	val = SENSORS_LIMIT(val, 0, 255);
+
+	mutex_lock(&data->update_lock);
+	data->pwm_enable = f71882fg_read8(data, F71882FG_REG_PWM_ENABLE);
+	if (data->pwm_enable & (1 << (2 * pwm))) {
+		/* PWM mode */
+	} else {
+		/* RPM mode */
+		if (val < 29)	/* Prevent negative numbers */
+			val = 255;
+		else
+			val = (255 - val) * 32 / val;
+	}
+	f71882fg_write8(data, F71882FG_REG_POINT_PWM(pwm, point), val);
+	data->pwm_auto_point_pwm[pwm][point] = val;
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t show_pwm_auto_point_temp_hyst(struct device *dev,
+					     struct device_attribute *devattr,
+					     char *buf)
+{
+	int result = 0;
+	struct f71882fg_data *data = f71882fg_update_device(dev);
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	int point = to_sensor_dev_attr_2(devattr)->nr;
+
+	mutex_lock(&data->update_lock);
+	if (nr & 1)
+		result = data->pwm_auto_point_hyst[nr / 2] >> 4;
+	else
+		result = data->pwm_auto_point_hyst[nr / 2] & 0x0f;
+	result = 1000 * (data->pwm_auto_point_temp[nr][point] - result);
+	mutex_unlock(&data->update_lock);
+
+	return sprintf(buf, "%d\n", result);
+}
+
+static ssize_t store_pwm_auto_point_temp_hyst(struct device *dev,
+					      struct device_attribute *devattr,
+					      const char *buf, size_t count)
+{
+	struct f71882fg_data *data = dev_get_drvdata(dev);
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	int point = to_sensor_dev_attr_2(devattr)->nr;
+	long val = simple_strtol(buf, NULL, 10) / 1000;
+	u8 reg;
+
+	mutex_lock(&data->update_lock);
+	data->pwm_auto_point_temp[nr][point] =
+		f71882fg_read8(data, F71882FG_REG_POINT_TEMP(nr, point));
+	val = SENSORS_LIMIT(val, data->pwm_auto_point_temp[nr][point] - 15,
+				data->pwm_auto_point_temp[nr][point]);
+	val = data->pwm_auto_point_temp[nr][point] - val;
+
+	reg = f71882fg_read8(data, F71882FG_REG_FAN_HYST(nr / 2));
+	if (nr & 1)
+		reg = (reg & 0x0f) | (val << 4);
+	else
+		reg = (reg & 0xf0) | val;
+
+	f71882fg_write8(data, F71882FG_REG_FAN_HYST(nr / 2), reg);
+	data->pwm_auto_point_hyst[nr / 2] = reg;
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t show_pwm_interpolate(struct device *dev,
+				    struct device_attribute *devattr, char *buf)
+{
+	int result;
+	struct f71882fg_data *data = f71882fg_update_device(dev);
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+
+	result = (data->pwm_auto_point_mapping[nr] >> 4) & 1;
+
+	return sprintf(buf, "%d\n", result);
+}
+
+static ssize_t store_pwm_interpolate(struct device *dev,
+				     struct device_attribute *devattr,
+				     const char *buf, size_t count)
+{
+	struct f71882fg_data *data = dev_get_drvdata(dev);
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	unsigned long val = simple_strtoul(buf, NULL, 10);
+
+	mutex_lock(&data->update_lock);
+	data->pwm_auto_point_mapping[nr] =
+		f71882fg_read8(data, F71882FG_REG_POINT_MAPPING(nr));
+	if (val)
+		val = data->pwm_auto_point_mapping[nr] | (1 << 4);
+	else
+		val = data->pwm_auto_point_mapping[nr] & (~(1 << 4));
+	f71882fg_write8(data, F71882FG_REG_POINT_MAPPING(nr), val);
+	data->pwm_auto_point_mapping[nr] = val;
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t show_pwm_auto_point_channel(struct device *dev,
+					   struct device_attribute *devattr,
+					   char *buf)
+{
+	int result;
+	struct f71882fg_data *data = f71882fg_update_device(dev);
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	int temp_start = (data->type == f8000) ? 0 : 1;
+
+	result = 1 << ((data->pwm_auto_point_mapping[nr] & 3) - temp_start);
+
+	return sprintf(buf, "%d\n", result);
+}
+
+static ssize_t store_pwm_auto_point_channel(struct device *dev,
+					    struct device_attribute *devattr,
+					    const char *buf, size_t count)
+{
+	struct f71882fg_data *data = dev_get_drvdata(dev);
+	int nr = to_sensor_dev_attr_2(devattr)->index;
+	int temp_start = (data->type == f8000) ? 0 : 1;
+	long val = simple_strtol(buf, NULL, 10);
+
+	switch (val) {
+	case 1:
+		val = 0;
+		break;
+	case 2:
+		val = 1;
+		break;
+	case 4:
+		val = 2;
+		break;
+	default:
+		return -EINVAL;
+	}
+	val += temp_start;
+	mutex_lock(&data->update_lock);
+	data->pwm_auto_point_mapping[nr] =
+		f71882fg_read8(data, F71882FG_REG_POINT_MAPPING(nr));
+	val = (data->pwm_auto_point_mapping[nr] & 0xfc) | val;
+	f71882fg_write8(data, F71882FG_REG_POINT_MAPPING(nr), val);
+	data->pwm_auto_point_mapping[nr] = val;
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t show_pwm_auto_point_temp(struct device *dev,
+					struct device_attribute *devattr,
+					char *buf)
+{
+	int result;
+	struct f71882fg_data *data = f71882fg_update_device(dev);
+	int pwm = to_sensor_dev_attr_2(devattr)->index;
+	int point = to_sensor_dev_attr_2(devattr)->nr;
+
+	result = data->pwm_auto_point_temp[pwm][point];
+	return sprintf(buf, "%d\n", 1000 * result);
+}
+
+static ssize_t store_pwm_auto_point_temp(struct device *dev,
+					 struct device_attribute *devattr,
+					 const char *buf, size_t count)
+{
+	struct f71882fg_data *data = dev_get_drvdata(dev);
+	int pwm = to_sensor_dev_attr_2(devattr)->index;
+	int point = to_sensor_dev_attr_2(devattr)->nr;
+	long val = simple_strtol(buf, NULL, 10) / 1000;
+	val = SENSORS_LIMIT(val, 0, 255);
+
+	mutex_lock(&data->update_lock);
+	f71882fg_write8(data, F71882FG_REG_POINT_TEMP(pwm, point), val);
+	data->pwm_auto_point_temp[pwm][point] = val;
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
 static ssize_t show_name(struct device *dev, struct device_attribute *devattr,
 	char *buf)
 {
-	return sprintf(buf, DRVNAME "\n");
+	struct f71882fg_data *data = dev_get_drvdata(dev);
+	return sprintf(buf, "%s\n", f71882fg_names[data->type]);
 }
 
+static int __devinit f71882fg_create_sysfs_files(struct platform_device *pdev,
+	struct sensor_device_attribute_2 *attr, int count)
+{
+	int err, i;
+
+	for (i = 0; i < count; i++) {
+		err = device_create_file(&pdev->dev, &attr[i].dev_attr);
+		if (err)
+			return err;
+	}
+	return 0;
+}
 
-static int __devinit f71882fg_probe(struct platform_device * pdev)
+static int __devinit f71882fg_probe(struct platform_device *pdev)
 {
 	struct f71882fg_data *data;
-	int err, i;
+	struct f71882fg_sio_data *sio_data = pdev->dev.platform_data;
+	int err, i, nr_fans = (sio_data->type == f71882fg) ? 4 : 3;
 	u8 start_reg;
 
-	if (!(data = kzalloc(sizeof(struct f71882fg_data), GFP_KERNEL)))
+	data = kzalloc(sizeof(struct f71882fg_data), GFP_KERNEL);
+	if (!data)
 		return -ENOMEM;
 
 	data->addr = platform_get_resource(pdev, IORESOURCE_IO, 0)->start;
+	data->type = sio_data->type;
 	mutex_init(&data->update_lock);
 	platform_set_drvdata(pdev, data);
 
-	/* Register sysfs interface files */
-	for (i = 0; i < ARRAY_SIZE(f71882fg_dev_attr); i++) {
-		err = device_create_file(&pdev->dev, &f71882fg_dev_attr[i]);
-		if (err)
-			goto exit_unregister_sysfs;
+	start_reg = f71882fg_read8(data, F71882FG_REG_START);
+	if (start_reg & 0x04) {
+		dev_warn(&pdev->dev, "Hardware monitor is powered down\n");
+		err = -ENODEV;
+		goto exit_free;
+	}
+	if (!(start_reg & 0x03)) {
+		dev_warn(&pdev->dev, "Hardware monitoring not activated\n");
+		err = -ENODEV;
+		goto exit_free;
 	}
 
-	start_reg = f71882fg_read8(data, F71882FG_REG_START);
+	data->pwm_enable = f71882fg_read8(data, F71882FG_REG_PWM_ENABLE);
+	/* If it is a 71862 and the fan / pwm part is enabled sanity check
+	   the pwm settings */
+	if (data->type == f71862fg && (start_reg & 0x02)) {
+		if ((data->pwm_enable & 0x15) != 0x15) {
+			dev_err(&pdev->dev,
+				"Invalid (reserved) pwm settings: 0x%02x\n",
+				(unsigned int)data->pwm_enable);
+			err = -ENODEV;
+			goto exit_free;
+		}
+	}
+
+	/* Register sysfs interface files */
+	err = device_create_file(&pdev->dev, &dev_attr_name);
+	if (err)
+		goto exit_unregister_sysfs;
+
 	if (start_reg & 0x01) {
-		for (i = 0; i < ARRAY_SIZE(f71882fg_in_temp_attr); i++) {
-			err = device_create_file(&pdev->dev,
-					&f71882fg_in_temp_attr[i].dev_attr);
+		switch (data->type) {
+		case f71882fg:
+			err = f71882fg_create_sysfs_files(pdev,
+					f71882fg_in_temp_attr,
+					ARRAY_SIZE(f71882fg_in_temp_attr));
 			if (err)
 				goto exit_unregister_sysfs;
+			/* fall through! */
+		case f71862fg:
+			err = f71882fg_create_sysfs_files(pdev,
+					f718x2fg_in_temp_attr,
+					ARRAY_SIZE(f718x2fg_in_temp_attr));
+			break;
+		case f8000:
+			err = f71882fg_create_sysfs_files(pdev,
+					f8000_in_temp_attr,
+					ARRAY_SIZE(f8000_in_temp_attr));
+			break;
 		}
+		if (err)
+			goto exit_unregister_sysfs;
 	}
 
 	if (start_reg & 0x02) {
-		for (i = 0; i < ARRAY_SIZE(f71882fg_fan_attr); i++) {
-			err = device_create_file(&pdev->dev,
-					&f71882fg_fan_attr[i].dev_attr);
-			if (err)
-				goto exit_unregister_sysfs;
+		err = f71882fg_create_sysfs_files(pdev, fxxxx_fan_attr,
+					ARRAY_SIZE(fxxxx_fan_attr));
+		if (err)
+			goto exit_unregister_sysfs;
+
+		switch (data->type) {
+		case f71862fg:
+			err = f71882fg_create_sysfs_files(pdev,
+					f71862fg_fan_attr,
+					ARRAY_SIZE(f71862fg_fan_attr));
+			break;
+		case f71882fg:
+			err = f71882fg_create_sysfs_files(pdev,
+					f71882fg_fan_attr,
+					ARRAY_SIZE(f71882fg_fan_attr));
+			break;
+		case f8000:
+			err = f71882fg_create_sysfs_files(pdev,
+					f8000_fan_attr,
+					ARRAY_SIZE(f8000_fan_attr));
+			break;
 		}
+		if (err)
+			goto exit_unregister_sysfs;
+
+		for (i = 0; i < nr_fans; i++)
+			dev_info(&pdev->dev, "Fan: %d is in %s mode\n", i + 1,
+				 (data->pwm_enable & (1 << 2 * i)) ?
+				 "duty-cycle" : "RPM");
 	}
 
 	data->hwmon_dev = hwmon_device_register(&pdev->dev);
 	if (IS_ERR(data->hwmon_dev)) {
 		err = PTR_ERR(data->hwmon_dev);
+		data->hwmon_dev = NULL;
 		goto exit_unregister_sysfs;
 	}
 
 	return 0;
 
 exit_unregister_sysfs:
-	for (i = 0; i < ARRAY_SIZE(f71882fg_dev_attr); i++)
-		device_remove_file(&pdev->dev, &f71882fg_dev_attr[i]);
-
-	for (i = 0; i < ARRAY_SIZE(f71882fg_in_temp_attr); i++)
-		device_remove_file(&pdev->dev,
-					&f71882fg_in_temp_attr[i].dev_attr);
-
-	for (i = 0; i < ARRAY_SIZE(f71882fg_fan_attr); i++)
-		device_remove_file(&pdev->dev, &f71882fg_fan_attr[i].dev_attr);
-
+	f71882fg_remove(pdev); /* Will unregister the sysfs files for us */
+	return err; /* f71882fg_remove() also frees our data */
+exit_free:
 	kfree(data);
-
 	return err;
 }
 
-static int __devexit f71882fg_remove(struct platform_device *pdev)
+static int f71882fg_remove(struct platform_device *pdev)
 {
 	int i;
 	struct f71882fg_data *data = platform_get_drvdata(pdev);
 
 	platform_set_drvdata(pdev, NULL);
-	hwmon_device_unregister(data->hwmon_dev);
+	if (data->hwmon_dev)
+		hwmon_device_unregister(data->hwmon_dev);
+
+	/* Note we are not looping over all attr arrays we have as the ones
+	   below are supersets of the ones skipped. */
+	device_remove_file(&pdev->dev, &dev_attr_name);
 
-	for (i = 0; i < ARRAY_SIZE(f71882fg_dev_attr); i++)
-		device_remove_file(&pdev->dev, &f71882fg_dev_attr[i]);
+	for (i = 0; i < ARRAY_SIZE(f718x2fg_in_temp_attr); i++)
+		device_remove_file(&pdev->dev,
+					&f718x2fg_in_temp_attr[i].dev_attr);
 
 	for (i = 0; i < ARRAY_SIZE(f71882fg_in_temp_attr); i++)
 		device_remove_file(&pdev->dev,
 					&f71882fg_in_temp_attr[i].dev_attr);
 
+	for (i = 0; i < ARRAY_SIZE(fxxxx_fan_attr); i++)
+		device_remove_file(&pdev->dev, &fxxxx_fan_attr[i].dev_attr);
+
 	for (i = 0; i < ARRAY_SIZE(f71882fg_fan_attr); i++)
 		device_remove_file(&pdev->dev, &f71882fg_fan_attr[i].dev_attr);
 
+	for (i = 0; i < ARRAY_SIZE(f8000_fan_attr); i++)
+		device_remove_file(&pdev->dev, &f8000_fan_attr[i].dev_attr);
+
 	kfree(data);
 
 	return 0;
 }
 
-static int __init f71882fg_find(int sioaddr, unsigned short *address)
+static int __init f71882fg_find(int sioaddr, unsigned short *address,
+	struct f71882fg_sio_data *sio_data)
 {
 	int err = -ENODEV;
 	u16 devid;
-	u8 start_reg;
-	struct f71882fg_data data;
 
 	superio_enter(sioaddr);
 
@@ -842,7 +1877,17 @@ static int __init f71882fg_find(int sioaddr, unsigned short *address)
 	}
 
 	devid = force_id ? force_id : superio_inw(sioaddr, SIO_REG_DEVID);
-	if (devid != SIO_F71882_ID) {
+	switch (devid) {
+	case SIO_F71862_ID:
+		sio_data->type = f71862fg;
+		break;
+	case SIO_F71882_ID:
+		sio_data->type = f71882fg;
+		break;
+	case SIO_F8000_ID:
+		sio_data->type = f8000;
+		break;
+	default:
 		printk(KERN_INFO DRVNAME ": Unsupported Fintek device\n");
 		goto exit;
 	}
@@ -861,24 +1906,17 @@ static int __init f71882fg_find(int sioaddr, unsigned short *address)
 	}
 	*address &= ~(REGION_LENGTH - 1);	/* Ignore 3 LSB */
 
-	data.addr = *address;
-	start_reg = f71882fg_read8(&data, F71882FG_REG_START);
-	if (!(start_reg & 0x03)) {
-		printk(KERN_WARNING DRVNAME
-			": Hardware monitoring not activated\n");
-		goto exit;
-	}
-
 	err = 0;
-	printk(KERN_INFO DRVNAME ": Found F71882FG chip at %#x, revision %d\n",
-		(unsigned int)*address,
+	printk(KERN_INFO DRVNAME ": Found %s chip at %#x, revision %d\n",
+		f71882fg_names[sio_data->type],	(unsigned int)*address,
 		(int)superio_inb(sioaddr, SIO_REG_DEVREV));
 exit:
 	superio_exit(sioaddr);
 	return err;
 }
 
-static int __init f71882fg_device_add(unsigned short address)
+static int __init f71882fg_device_add(unsigned short address,
+	const struct f71882fg_sio_data *sio_data)
 {
 	struct resource res = {
 		.start	= address,
@@ -892,12 +1930,23 @@ static int __init f71882fg_device_add(unsigned short address)
 		return -ENOMEM;
 
 	res.name = f71882fg_pdev->name;
+	err = acpi_check_resource_conflict(&res);
+	if (err)
+		return err;
+
 	err = platform_device_add_resources(f71882fg_pdev, &res, 1);
 	if (err) {
 		printk(KERN_ERR DRVNAME ": Device resource addition failed\n");
 		goto exit_device_put;
 	}
 
+	err = platform_device_add_data(f71882fg_pdev, sio_data,
+				       sizeof(struct f71882fg_sio_data));
+	if (err) {
+		printk(KERN_ERR DRVNAME ": Platform data allocation failed\n");
+		goto exit_device_put;
+	}
+
 	err = platform_device_add(f71882fg_pdev);
 	if (err) {
 		printk(KERN_ERR DRVNAME ": Device addition failed\n");
@@ -916,14 +1965,20 @@ static int __init f71882fg_init(void)
 {
 	int err = -ENODEV;
 	unsigned short address;
+	struct f71882fg_sio_data sio_data;
+
+	memset(&sio_data, 0, sizeof(sio_data));
 
-	if (f71882fg_find(0x2e, &address) && f71882fg_find(0x4e, &address))
+	if (f71882fg_find(0x2e, &address, &sio_data) &&
+	    f71882fg_find(0x4e, &address, &sio_data))
 		goto exit;
 
-	if ((err = platform_driver_register(&f71882fg_driver)))
+	err = platform_driver_register(&f71882fg_driver);
+	if (err)
 		goto exit;
 
-	if ((err = f71882fg_device_add(address)))
+	err = f71882fg_device_add(address, &sio_data);
+	if (err)
 		goto exit_driver;
 
 	return 0;
@@ -941,7 +1996,7 @@ static void __exit f71882fg_exit(void)
 }
 
 MODULE_DESCRIPTION("F71882FG Hardware Monitoring Driver");
-MODULE_AUTHOR("Hans Edgington (hans@edgington.nl)");
+MODULE_AUTHOR("Hans Edgington, Hans de Goede (hdegoede@redhat.com)");
 MODULE_LICENSE("GPL");
 
 module_init(f71882fg_init);
diff --git a/drivers/hwmon/fschmd.c b/drivers/hwmon/fschmd.c
index 967170368933..d07f4ef75092 100644
--- a/drivers/hwmon/fschmd.c
+++ b/drivers/hwmon/fschmd.c
@@ -1,6 +1,6 @@
 /* fschmd.c
  *
- * Copyright (C) 2007 Hans de Goede <j.w.r.degoede@hhs.nl>
+ * Copyright (C) 2007,2008 Hans de Goede <hdegoede@redhat.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -42,11 +42,20 @@
 #include <linux/mutex.h>
 #include <linux/sysfs.h>
 #include <linux/dmi.h>
+#include <linux/fs.h>
+#include <linux/watchdog.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/kref.h>
 
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = { 0x73, I2C_CLIENT_END };
 
 /* Insmod parameters */
+static int nowayout = WATCHDOG_NOWAYOUT;
+module_param(nowayout, int, 0);
+MODULE_PARM_DESC(nowayout, "Watchdog cannot be stopped once started (default="
+	__MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
 I2C_CLIENT_INSMOD_5(fscpos, fscher, fscscy, fschrc, fschmd);
 
 /*
@@ -63,19 +72,26 @@ I2C_CLIENT_INSMOD_5(fscpos, fscher, fscscy, fschrc, fschmd);
 #define FSCHMD_REG_EVENT_STATE		0x04
 #define FSCHMD_REG_CONTROL		0x05
 
-#define FSCHMD_CONTROL_ALERT_LED_MASK	0x01
+#define FSCHMD_CONTROL_ALERT_LED	0x01
 
-/* watchdog (support to be implemented) */
+/* watchdog */
 #define FSCHMD_REG_WDOG_PRESET		0x28
 #define FSCHMD_REG_WDOG_STATE		0x23
 #define FSCHMD_REG_WDOG_CONTROL		0x21
 
+#define FSCHMD_WDOG_CONTROL_TRIGGER	0x10
+#define FSCHMD_WDOG_CONTROL_STARTED	0x10 /* the same as trigger */
+#define FSCHMD_WDOG_CONTROL_STOP	0x20
+#define FSCHMD_WDOG_CONTROL_RESOLUTION	0x40
+
+#define FSCHMD_WDOG_STATE_CARDRESET	0x02
+
 /* voltages, weird order is to keep the same order as the old drivers */
 static const u8 FSCHMD_REG_VOLT[3] = { 0x45, 0x42, 0x48 };
 
 /* minimum pwm at which the fan is driven (pwm can by increased depending on
    the temp. Notice that for the scy some fans share there minimum speed.
-   Also notice that with the scy the sensor order is different then with the
+   Also notice that with the scy the sensor order is different than with the
    other chips, this order was in the 2.4 driver and kept for consistency. */
 static const u8 FSCHMD_REG_FAN_MIN[5][6] = {
 	{ 0x55, 0x65 },					/* pos */
@@ -115,8 +131,8 @@ static const u8 FSCHMD_REG_FAN_RIPPLE[5][6] = {
 static const int FSCHMD_NO_FAN_SENSORS[5] = { 3, 3, 6, 4, 5 };
 
 /* Fan status register bitmasks */
-#define FSCHMD_FAN_ALARM_MASK		0x04 /* called fault by FSC! */
-#define FSCHMD_FAN_NOT_PRESENT_MASK	0x08 /* not documented */
+#define FSCHMD_FAN_ALARM	0x04 /* called fault by FSC! */
+#define FSCHMD_FAN_NOT_PRESENT	0x08 /* not documented */
 
 
 /* actual temperature registers */
@@ -158,14 +174,11 @@ static const u8 FSCHER_REG_TEMP_AUTOP2[] =	{ 0x75, 0x85, 0x95 }; */
 static const int FSCHMD_NO_TEMP_SENSORS[5] = { 3, 3, 4, 3, 5 };
 
 /* temp status register bitmasks */
-#define FSCHMD_TEMP_WORKING_MASK	0x01
-#define FSCHMD_TEMP_ALERT_MASK		0x02
+#define FSCHMD_TEMP_WORKING	0x01
+#define FSCHMD_TEMP_ALERT	0x02
 /* there only really is an alarm if the sensor is working and alert == 1 */
 #define FSCHMD_TEMP_ALARM_MASK \
-	(FSCHMD_TEMP_WORKING_MASK | FSCHMD_TEMP_ALERT_MASK)
-
-/* our driver name */
-#define FSCHMD_NAME "fschmd"
+	(FSCHMD_TEMP_WORKING | FSCHMD_TEMP_ALERT)
 
 /*
  * Functions declarations
@@ -195,7 +208,7 @@ MODULE_DEVICE_TABLE(i2c, fschmd_id);
 static struct i2c_driver fschmd_driver = {
 	.class		= I2C_CLASS_HWMON,
 	.driver = {
-		.name	= FSCHMD_NAME,
+		.name	= "fschmd",
 	},
 	.probe		= fschmd_probe,
 	.remove		= fschmd_remove,
@@ -209,14 +222,26 @@ static struct i2c_driver fschmd_driver = {
  */
 
 struct fschmd_data {
+	struct i2c_client *client;
 	struct device *hwmon_dev;
 	struct mutex update_lock;
+	struct mutex watchdog_lock;
+	struct list_head list; /* member of the watchdog_data_list */
+	struct kref kref;
+	struct miscdevice watchdog_miscdev;
 	int kind;
+	unsigned long watchdog_is_open;
+	char watchdog_expect_close;
+	char watchdog_name[10]; /* must be unique to avoid sysfs conflict */
 	char valid; /* zero until following fields are valid */
 	unsigned long last_updated; /* in jiffies */
 
 	/* register values */
+	u8 revision;            /* chip revision */
 	u8 global_control;	/* global control register */
+	u8 watchdog_control;    /* watchdog control register */
+	u8 watchdog_state;      /* watchdog status register */
+	u8 watchdog_preset;     /* watchdog counter preset on trigger val */
 	u8 volt[3];		/* 12, 5, battery voltage */
 	u8 temp_act[5];		/* temperature */
 	u8 temp_status[5];	/* status of sensor */
@@ -228,11 +253,28 @@ struct fschmd_data {
 };
 
 /* Global variables to hold information read from special DMI tables, which are
-   available on FSC machines with an fscher or later chip. */
+   available on FSC machines with an fscher or later chip. There is no need to
+   protect these with a lock as they are only modified from our attach function
+   which always gets called with the i2c-core lock held and never accessed
+   before the attach function is done with them. */
 static int dmi_mult[3] = { 490, 200, 100 };
 static int dmi_offset[3] = { 0, 0, 0 };
 static int dmi_vref = -1;
 
+/* Somewhat ugly :( global data pointer list with all fschmd devices, so that
+   we can find our device data as when using misc_register there is no other
+   method to get to ones device data from the open fop. */
+static LIST_HEAD(watchdog_data_list);
+/* Note this lock not only protect list access, but also data.kref access */
+static DEFINE_MUTEX(watchdog_data_mutex);
+
+/* Release our data struct when we're detached from the i2c client *and* all
+   references to our watchdog device are released */
+static void fschmd_release_resources(struct kref *ref)
+{
+	struct fschmd_data *data = container_of(ref, struct fschmd_data, kref);
+	kfree(data);
+}
 
 /*
  * Sysfs attr show / store functions
@@ -300,7 +342,7 @@ static ssize_t show_temp_fault(struct device *dev,
 	struct fschmd_data *data = fschmd_update_device(dev);
 
 	/* bit 0 set means sensor working ok, so no fault! */
-	if (data->temp_status[index] & FSCHMD_TEMP_WORKING_MASK)
+	if (data->temp_status[index] & FSCHMD_TEMP_WORKING)
 		return sprintf(buf, "0\n");
 	else
 		return sprintf(buf, "1\n");
@@ -385,7 +427,7 @@ static ssize_t show_fan_alarm(struct device *dev,
 	int index = to_sensor_dev_attr(devattr)->index;
 	struct fschmd_data *data = fschmd_update_device(dev);
 
-	if (data->fan_status[index] & FSCHMD_FAN_ALARM_MASK)
+	if (data->fan_status[index] & FSCHMD_FAN_ALARM)
 		return sprintf(buf, "1\n");
 	else
 		return sprintf(buf, "0\n");
@@ -397,7 +439,7 @@ static ssize_t show_fan_fault(struct device *dev,
 	int index = to_sensor_dev_attr(devattr)->index;
 	struct fschmd_data *data = fschmd_update_device(dev);
 
-	if (data->fan_status[index] & FSCHMD_FAN_NOT_PRESENT_MASK)
+	if (data->fan_status[index] & FSCHMD_FAN_NOT_PRESENT)
 		return sprintf(buf, "1\n");
 	else
 		return sprintf(buf, "0\n");
@@ -449,7 +491,7 @@ static ssize_t show_alert_led(struct device *dev,
 {
 	struct fschmd_data *data = fschmd_update_device(dev);
 
-	if (data->global_control & FSCHMD_CONTROL_ALERT_LED_MASK)
+	if (data->global_control & FSCHMD_CONTROL_ALERT_LED)
 		return sprintf(buf, "1\n");
 	else
 		return sprintf(buf, "0\n");
@@ -467,9 +509,9 @@ static ssize_t store_alert_led(struct device *dev,
 	reg = i2c_smbus_read_byte_data(to_i2c_client(dev), FSCHMD_REG_CONTROL);
 
 	if (v)
-		reg |= FSCHMD_CONTROL_ALERT_LED_MASK;
+		reg |= FSCHMD_CONTROL_ALERT_LED;
 	else
-		reg &= ~FSCHMD_CONTROL_ALERT_LED_MASK;
+		reg &= ~FSCHMD_CONTROL_ALERT_LED;
 
 	i2c_smbus_write_byte_data(to_i2c_client(dev), FSCHMD_REG_CONTROL, reg);
 
@@ -551,7 +593,265 @@ static struct sensor_device_attribute fschmd_fan_attr[] = {
 
 
 /*
- * Real code
+ * Watchdog routines
+ */
+
+static int watchdog_set_timeout(struct fschmd_data *data, int timeout)
+{
+	int ret, resolution;
+	int kind = data->kind + 1; /* 0-x array index -> 1-x module param */
+
+	/* 2 second or 60 second resolution? */
+	if (timeout <= 510 || kind == fscpos || kind == fscscy)
+		resolution = 2;
+	else
+		resolution = 60;
+
+	if (timeout < resolution || timeout > (resolution * 255))
+		return -EINVAL;
+
+	mutex_lock(&data->watchdog_lock);
+	if (!data->client) {
+		ret = -ENODEV;
+		goto leave;
+	}
+
+	if (resolution == 2)
+		data->watchdog_control &= ~FSCHMD_WDOG_CONTROL_RESOLUTION;
+	else
+		data->watchdog_control |= FSCHMD_WDOG_CONTROL_RESOLUTION;
+
+	data->watchdog_preset = DIV_ROUND_UP(timeout, resolution);
+
+	/* Write new timeout value */
+	i2c_smbus_write_byte_data(data->client, FSCHMD_REG_WDOG_PRESET,
+		data->watchdog_preset);
+	/* Write new control register, do not trigger! */
+	i2c_smbus_write_byte_data(data->client, FSCHMD_REG_WDOG_CONTROL,
+		data->watchdog_control & ~FSCHMD_WDOG_CONTROL_TRIGGER);
+
+	ret = data->watchdog_preset * resolution;
+
+leave:
+	mutex_unlock(&data->watchdog_lock);
+	return ret;
+}
+
+static int watchdog_get_timeout(struct fschmd_data *data)
+{
+	int timeout;
+
+	mutex_lock(&data->watchdog_lock);
+	if (data->watchdog_control & FSCHMD_WDOG_CONTROL_RESOLUTION)
+		timeout = data->watchdog_preset * 60;
+	else
+		timeout = data->watchdog_preset * 2;
+	mutex_unlock(&data->watchdog_lock);
+
+	return timeout;
+}
+
+static int watchdog_trigger(struct fschmd_data *data)
+{
+	int ret = 0;
+
+	mutex_lock(&data->watchdog_lock);
+	if (!data->client) {
+		ret = -ENODEV;
+		goto leave;
+	}
+
+	data->watchdog_control |= FSCHMD_WDOG_CONTROL_TRIGGER;
+	i2c_smbus_write_byte_data(data->client, FSCHMD_REG_WDOG_CONTROL,
+					data->watchdog_control);
+leave:
+	mutex_unlock(&data->watchdog_lock);
+	return ret;
+}
+
+static int watchdog_stop(struct fschmd_data *data)
+{
+	int ret = 0;
+
+	mutex_lock(&data->watchdog_lock);
+	if (!data->client) {
+		ret = -ENODEV;
+		goto leave;
+	}
+
+	data->watchdog_control &= ~FSCHMD_WDOG_CONTROL_STARTED;
+	/* Don't store the stop flag in our watchdog control register copy, as
+	   its a write only bit (read always returns 0) */
+	i2c_smbus_write_byte_data(data->client, FSCHMD_REG_WDOG_CONTROL,
+		data->watchdog_control | FSCHMD_WDOG_CONTROL_STOP);
+leave:
+	mutex_unlock(&data->watchdog_lock);
+	return ret;
+}
+
+static int watchdog_open(struct inode *inode, struct file *filp)
+{
+	struct fschmd_data *pos, *data = NULL;
+
+	/* We get called from drivers/char/misc.c with misc_mtx hold, and we
+	   call misc_register() from fschmd_probe() with watchdog_data_mutex
+	   hold, as misc_register() takes the misc_mtx lock, this is a possible
+	   deadlock, so we use mutex_trylock here. */
+	if (!mutex_trylock(&watchdog_data_mutex))
+		return -ERESTARTSYS;
+	list_for_each_entry(pos, &watchdog_data_list, list) {
+		if (pos->watchdog_miscdev.minor == iminor(inode)) {
+			data = pos;
+			break;
+		}
+	}
+	/* Note we can never not have found data, so we don't check for this */
+	kref_get(&data->kref);
+	mutex_unlock(&watchdog_data_mutex);
+
+	if (test_and_set_bit(0, &data->watchdog_is_open))
+		return -EBUSY;
+
+	/* Start the watchdog */
+	watchdog_trigger(data);
+	filp->private_data = data;
+
+	return nonseekable_open(inode, filp);
+}
+
+static int watchdog_release(struct inode *inode, struct file *filp)
+{
+	struct fschmd_data *data = filp->private_data;
+
+	if (data->watchdog_expect_close) {
+		watchdog_stop(data);
+		data->watchdog_expect_close = 0;
+	} else {
+		watchdog_trigger(data);
+		dev_crit(&data->client->dev,
+			"unexpected close, not stopping watchdog!\n");
+	}
+
+	clear_bit(0, &data->watchdog_is_open);
+
+	mutex_lock(&watchdog_data_mutex);
+	kref_put(&data->kref, fschmd_release_resources);
+	mutex_unlock(&watchdog_data_mutex);
+
+	return 0;
+}
+
+static ssize_t watchdog_write(struct file *filp, const char __user *buf,
+	size_t count, loff_t *offset)
+{
+	size_t ret;
+	struct fschmd_data *data = filp->private_data;
+
+	if (count) {
+		if (!nowayout) {
+			size_t i;
+
+			/* Clear it in case it was set with a previous write */
+			data->watchdog_expect_close = 0;
+
+			for (i = 0; i != count; i++) {
+				char c;
+				if (get_user(c, buf + i))
+					return -EFAULT;
+				if (c == 'V')
+					data->watchdog_expect_close = 1;
+			}
+		}
+		ret = watchdog_trigger(data);
+		if (ret < 0)
+			return ret;
+	}
+	return count;
+}
+
+static int watchdog_ioctl(struct inode *inode, struct file *filp,
+	unsigned int cmd, unsigned long arg)
+{
+	static struct watchdog_info ident = {
+		.options = WDIOF_KEEPALIVEPING | WDIOF_SETTIMEOUT |
+				WDIOF_CARDRESET,
+		.identity = "FSC watchdog"
+	};
+	int i, ret = 0;
+	struct fschmd_data *data = filp->private_data;
+
+	switch (cmd) {
+	case WDIOC_GETSUPPORT:
+		ident.firmware_version = data->revision;
+		if (!nowayout)
+			ident.options |= WDIOF_MAGICCLOSE;
+		if (copy_to_user((void __user *)arg, &ident, sizeof(ident)))
+			ret = -EFAULT;
+		break;
+
+	case WDIOC_GETSTATUS:
+		ret = put_user(0, (int __user *)arg);
+		break;
+
+	case WDIOC_GETBOOTSTATUS:
+		if (data->watchdog_state & FSCHMD_WDOG_STATE_CARDRESET)
+			ret = put_user(WDIOF_CARDRESET, (int __user *)arg);
+		else
+			ret = put_user(0, (int __user *)arg);
+		break;
+
+	case WDIOC_KEEPALIVE:
+		ret = watchdog_trigger(data);
+		break;
+
+	case WDIOC_GETTIMEOUT:
+		i = watchdog_get_timeout(data);
+		ret = put_user(i, (int __user *)arg);
+		break;
+
+	case WDIOC_SETTIMEOUT:
+		if (get_user(i, (int __user *)arg)) {
+			ret = -EFAULT;
+			break;
+		}
+		ret = watchdog_set_timeout(data, i);
+		if (ret > 0)
+			ret = put_user(ret, (int __user *)arg);
+		break;
+
+	case WDIOC_SETOPTIONS:
+		if (get_user(i, (int __user *)arg)) {
+			ret = -EFAULT;
+			break;
+		}
+
+		if (i & WDIOS_DISABLECARD)
+			ret = watchdog_stop(data);
+		else if (i & WDIOS_ENABLECARD)
+			ret = watchdog_trigger(data);
+		else
+			ret = -EINVAL;
+
+		break;
+	default:
+		ret = -ENOTTY;
+	}
+
+	return ret;
+}
+
+static struct file_operations watchdog_fops = {
+	.owner = THIS_MODULE,
+	.llseek = no_llseek,
+	.open = watchdog_open,
+	.release = watchdog_release,
+	.write = watchdog_write,
+	.ioctl = watchdog_ioctl,
+};
+
+
+/*
+ * Detect, register, unregister and update device functions
  */
 
 /* DMI decode routine to read voltage scaling factors from special DMI tables,
@@ -661,9 +961,9 @@ static int fschmd_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
 	struct fschmd_data *data;
-	u8 revision;
 	const char * const names[5] = { "Poseidon", "Hermes", "Scylla",
 					"Heracles", "Heimdall" };
+	const int watchdog_minors[] = { WATCHDOG_MINOR, 212, 213, 214, 215 };
 	int i, err;
 	enum chips kind = id->driver_data;
 
@@ -673,6 +973,13 @@ static int fschmd_probe(struct i2c_client *client,
 
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->update_lock);
+	mutex_init(&data->watchdog_lock);
+	INIT_LIST_HEAD(&data->list);
+	kref_init(&data->kref);
+	/* Store client pointer in our data struct for watchdog usage
+	   (where the client is found through a data ptr instead of the
+	   otherway around) */
+	data->client = client;
 
 	if (kind == fscpos) {
 		/* The Poseidon has hardwired temp limits, fill these
@@ -683,16 +990,27 @@ static int fschmd_probe(struct i2c_client *client,
 	}
 
 	/* Read the special DMI table for fscher and newer chips */
-	if (kind == fscher || kind >= fschrc) {
+	if ((kind == fscher || kind >= fschrc) && dmi_vref == -1) {
 		dmi_walk(fschmd_dmi_decode);
 		if (dmi_vref == -1) {
-			printk(KERN_WARNING FSCHMD_NAME
-				": Couldn't get voltage scaling factors from "
+			dev_warn(&client->dev,
+				"Couldn't get voltage scaling factors from "
 				"BIOS DMI table, using builtin defaults\n");
 			dmi_vref = 33;
 		}
 	}
 
+	/* Read in some never changing registers */
+	data->revision = i2c_smbus_read_byte_data(client, FSCHMD_REG_REVISION);
+	data->global_control = i2c_smbus_read_byte_data(client,
+					FSCHMD_REG_CONTROL);
+	data->watchdog_control = i2c_smbus_read_byte_data(client,
+					FSCHMD_REG_WDOG_CONTROL);
+	data->watchdog_state = i2c_smbus_read_byte_data(client,
+					FSCHMD_REG_WDOG_STATE);
+	data->watchdog_preset = i2c_smbus_read_byte_data(client,
+					FSCHMD_REG_WDOG_PRESET);
+
 	/* i2c kind goes from 1-5, we want from 0-4 to address arrays */
 	data->kind = kind - 1;
 
@@ -735,9 +1053,43 @@ static int fschmd_probe(struct i2c_client *client,
 		goto exit_detach;
 	}
 
-	revision = i2c_smbus_read_byte_data(client, FSCHMD_REG_REVISION);
-	printk(KERN_INFO FSCHMD_NAME ": Detected FSC %s chip, revision: %d\n",
-		names[data->kind], (int) revision);
+	/* We take the data_mutex lock early so that watchdog_open() cannot
+	   run when misc_register() has completed, but we've not yet added
+	   our data to the watchdog_data_list (and set the default timeout) */
+	mutex_lock(&watchdog_data_mutex);
+	for (i = 0; i < ARRAY_SIZE(watchdog_minors); i++) {
+		/* Register our watchdog part */
+		snprintf(data->watchdog_name, sizeof(data->watchdog_name),
+			"watchdog%c", (i == 0) ? '\0' : ('0' + i));
+		data->watchdog_miscdev.name = data->watchdog_name;
+		data->watchdog_miscdev.fops = &watchdog_fops;
+		data->watchdog_miscdev.minor = watchdog_minors[i];
+		err = misc_register(&data->watchdog_miscdev);
+		if (err == -EBUSY)
+			continue;
+		if (err) {
+			data->watchdog_miscdev.minor = 0;
+			dev_err(&client->dev,
+				"Registering watchdog chardev: %d\n", err);
+			break;
+		}
+
+		list_add(&data->list, &watchdog_data_list);
+		watchdog_set_timeout(data, 60);
+		dev_info(&client->dev,
+			"Registered watchdog chardev major 10, minor: %d\n",
+			watchdog_minors[i]);
+		break;
+	}
+	if (i == ARRAY_SIZE(watchdog_minors)) {
+		data->watchdog_miscdev.minor = 0;
+		dev_warn(&client->dev, "Couldn't register watchdog chardev "
+			"(due to no free minor)\n");
+	}
+	mutex_unlock(&watchdog_data_mutex);
+
+	dev_info(&client->dev, "Detected FSC %s chip, revision: %d\n",
+		names[data->kind], (int) data->revision);
 
 	return 0;
 
@@ -751,6 +1103,24 @@ static int fschmd_remove(struct i2c_client *client)
 	struct fschmd_data *data = i2c_get_clientdata(client);
 	int i;
 
+	/* Unregister the watchdog (if registered) */
+	if (data->watchdog_miscdev.minor) {
+		misc_deregister(&data->watchdog_miscdev);
+		if (data->watchdog_is_open) {
+			dev_warn(&client->dev,
+				"i2c client detached with watchdog open! "
+				"Stopping watchdog.\n");
+			watchdog_stop(data);
+		}
+		mutex_lock(&watchdog_data_mutex);
+		list_del(&data->list);
+		mutex_unlock(&watchdog_data_mutex);
+		/* Tell the watchdog code the client is gone */
+		mutex_lock(&data->watchdog_lock);
+		data->client = NULL;
+		mutex_unlock(&data->watchdog_lock);
+	}
+
 	/* Check if registered in case we're called from fschmd_detect
 	   to cleanup after an error */
 	if (data->hwmon_dev)
@@ -765,7 +1135,10 @@ static int fschmd_remove(struct i2c_client *client)
 		device_remove_file(&client->dev,
 					&fschmd_fan_attr[i].dev_attr);
 
-	kfree(data);
+	mutex_lock(&watchdog_data_mutex);
+	kref_put(&data->kref, fschmd_release_resources);
+	mutex_unlock(&watchdog_data_mutex);
+
 	return 0;
 }
 
@@ -798,7 +1171,7 @@ static struct fschmd_data *fschmd_update_device(struct device *dev)
 					data->temp_act[i] < data->temp_max[i])
 				i2c_smbus_write_byte_data(client,
 					FSCHMD_REG_TEMP_STATE[data->kind][i],
-					FSCHMD_TEMP_ALERT_MASK);
+					FSCHMD_TEMP_ALERT);
 		}
 
 		for (i = 0; i < FSCHMD_NO_FAN_SENSORS[data->kind]; i++) {
@@ -816,28 +1189,17 @@ static struct fschmd_data *fschmd_update_device(struct device *dev)
 					FSCHMD_REG_FAN_MIN[data->kind][i]);
 
 			/* reset fan status if speed is back to > 0 */
-			if ((data->fan_status[i] & FSCHMD_FAN_ALARM_MASK) &&
+			if ((data->fan_status[i] & FSCHMD_FAN_ALARM) &&
 					data->fan_act[i])
 				i2c_smbus_write_byte_data(client,
 					FSCHMD_REG_FAN_STATE[data->kind][i],
-					FSCHMD_FAN_ALARM_MASK);
+					FSCHMD_FAN_ALARM);
 		}
 
 		for (i = 0; i < 3; i++)
 			data->volt[i] = i2c_smbus_read_byte_data(client,
 						FSCHMD_REG_VOLT[i]);
 
-		data->global_control = i2c_smbus_read_byte_data(client,
-						FSCHMD_REG_CONTROL);
-
-		/* To be implemented in the future
-		data->watchdog[0] = i2c_smbus_read_byte_data(client,
-						FSCHMD_REG_WDOG_PRESET);
-		data->watchdog[1] = i2c_smbus_read_byte_data(client,
-						FSCHMD_REG_WDOG_STATE);
-		data->watchdog[2] = i2c_smbus_read_byte_data(client,
-						FSCHMD_REG_WDOG_CONTROL); */
-
 		data->last_updated = jiffies;
 		data->valid = 1;
 	}
@@ -857,7 +1219,7 @@ static void __exit fschmd_exit(void)
 	i2c_del_driver(&fschmd_driver);
 }
 
-MODULE_AUTHOR("Hans de Goede <j.w.r.degoede@hhs.nl>");
+MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
 MODULE_DESCRIPTION("FSC Poseidon, Hermes, Scylla, Heracles and "
 			"Heimdall driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/hwmon.c b/drivers/hwmon/hwmon.c
index 076a59cdabe9..e15c3e7b07e9 100644
--- a/drivers/hwmon/hwmon.c
+++ b/drivers/hwmon/hwmon.c
@@ -76,7 +76,7 @@ void hwmon_device_unregister(struct device *dev)
 {
 	int id;
 
-	if (likely(sscanf(dev->bus_id, HWMON_ID_FORMAT, &id) == 1)) {
+	if (likely(sscanf(dev_name(dev), HWMON_ID_FORMAT, &id) == 1)) {
 		device_unregister(dev);
 		spin_lock(&idr_lock);
 		idr_remove(&hwmon_idr, id);
diff --git a/drivers/hwmon/i5k_amb.c b/drivers/hwmon/i5k_amb.c
index 2ede9388096b..27d7f72a5f11 100644
--- a/drivers/hwmon/i5k_amb.c
+++ b/drivers/hwmon/i5k_amb.c
@@ -490,6 +490,13 @@ static unsigned long chipset_ids[] = {
 	0
 };
 
+static struct pci_device_id i5k_amb_ids[] __devinitdata = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_5000_ERR) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_5400_ERR) },
+	{ 0, }
+};
+MODULE_DEVICE_TABLE(pci, i5k_amb_ids);
+
 static int __devinit i5k_amb_probe(struct platform_device *pdev)
 {
 	struct i5k_amb_data *data;
diff --git a/drivers/hwmon/ibmpex.c b/drivers/hwmon/ibmpex.c
index 537d9fb2ff88..a36363312f2f 100644
--- a/drivers/hwmon/ibmpex.c
+++ b/drivers/hwmon/ibmpex.c
@@ -40,7 +40,7 @@
 
 static inline u16 extract_value(const char *data, int offset)
 {
-	return be16_to_cpup((u16 *)&data[offset]);
+	return be16_to_cpup((__be16 *)&data[offset]);
 }
 
 #define TEMP_SENSOR		1
diff --git a/drivers/hwmon/it87.c b/drivers/hwmon/it87.c
index b74c95735f95..95a99c590da2 100644
--- a/drivers/hwmon/it87.c
+++ b/drivers/hwmon/it87.c
@@ -14,6 +14,7 @@
               IT8712F  Super I/O chip w/LPC interface
               IT8716F  Super I/O chip w/LPC interface
               IT8718F  Super I/O chip w/LPC interface
+              IT8720F  Super I/O chip w/LPC interface
               IT8726F  Super I/O chip w/LPC interface
               Sis950   A clone of the IT8705F
 
@@ -48,11 +49,12 @@
 #include <linux/sysfs.h>
 #include <linux/string.h>
 #include <linux/dmi.h>
+#include <linux/acpi.h>
 #include <asm/io.h>
 
 #define DRVNAME "it87"
 
-enum chips { it87, it8712, it8716, it8718 };
+enum chips { it87, it8712, it8716, it8718, it8720 };
 
 static unsigned short force_id;
 module_param(force_id, ushort, 0);
@@ -64,7 +66,10 @@ static struct platform_device *pdev;
 #define	DEV	0x07	/* Register: Logical device select */
 #define	VAL	0x2f	/* The value to read/write */
 #define PME	0x04	/* The device with the fan registers in it */
-#define GPIO	0x07	/* The device with the IT8718F VID value in it */
+
+/* The device with the IT8718F/IT8720F VID value in it */
+#define GPIO	0x07
+
 #define	DEVID	0x20	/* Register: Device ID */
 #define	DEVREV	0x22	/* Register: Device Revision */
 
@@ -113,6 +118,7 @@ superio_exit(void)
 #define IT8705F_DEVID 0x8705
 #define IT8716F_DEVID 0x8716
 #define IT8718F_DEVID 0x8718
+#define IT8720F_DEVID 0x8720
 #define IT8726F_DEVID 0x8726
 #define IT87_ACT_REG  0x30
 #define IT87_BASE_REG 0x60
@@ -150,8 +156,8 @@ static int fix_pwm_polarity;
 #define IT87_REG_ALARM2        0x02
 #define IT87_REG_ALARM3        0x03
 
-/* The IT8718F has the VID value in a different register, in Super-I/O
-   configuration space. */
+/* The IT8718F and IT8720F have the VID value in a different register, in
+   Super-I/O configuration space. */
 #define IT87_REG_VID           0x0a
 /* The IT8705F and IT8712F earlier than revision 0x08 use register 0x0b
    for fan divisors. Later IT8712F revisions must use 16-bit tachometer
@@ -282,7 +288,8 @@ static inline int has_16bit_fans(const struct it87_data *data)
 	return (data->type == it87 && data->revision >= 0x03)
 	    || (data->type == it8712 && data->revision >= 0x08)
 	    || data->type == it8716
-	    || data->type == it8718;
+	    || data->type == it8718
+	    || data->type == it8720;
 }
 
 static int it87_probe(struct platform_device *pdev);
@@ -992,6 +999,9 @@ static int __init it87_find(unsigned short *address,
 	case IT8718F_DEVID:
 		sio_data->type = it8718;
 		break;
+	case IT8720F_DEVID:
+		sio_data->type = it8720;
+		break;
 	case 0xffff:	/* No device at all */
 		goto exit;
 	default:
@@ -1022,7 +1032,8 @@ static int __init it87_find(unsigned short *address,
 		int reg;
 
 		superio_select(GPIO);
-		if (chip_type == it8718)
+		if ((chip_type == it8718) ||
+		    (chip_type == it8720))
 			sio_data->vid_value = superio_inb(IT87_SIO_VID_REG);
 
 		reg = superio_inb(IT87_SIO_PINX2_REG);
@@ -1068,6 +1079,7 @@ static int __devinit it87_probe(struct platform_device *pdev)
 		"it8712",
 		"it8716",
 		"it8718",
+		"it8720",
 	};
 
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
@@ -1226,7 +1238,7 @@ static int __devinit it87_probe(struct platform_device *pdev)
 	}
 
 	if (data->type == it8712 || data->type == it8716
-	 || data->type == it8718) {
+	 || data->type == it8718 || data->type == it8720) {
 		data->vrm = vid_which_vrm();
 		/* VID reading from Super-I/O config space if available */
 		data->vid = sio_data->vid_value;
@@ -1374,7 +1386,7 @@ static void __devinit it87_init_device(struct platform_device *pdev)
 			it87_write_value(data, IT87_REG_TEMP_HIGH(i), 127);
 	}
 
-	/* Check if temperature channnels are reset manually or by some reason */
+	/* Check if temperature channels are reset manually or by some reason */
 	tmp = it87_read_value(data, IT87_REG_TEMP_ENABLE);
 	if ((tmp & 0x3f) == 0) {
 		/* Temp1,Temp3=thermistor; Temp2=thermal diode */
@@ -1513,7 +1525,8 @@ static struct it87_data *it87_update_device(struct device *dev)
 
 		data->sensor = it87_read_value(data, IT87_REG_TEMP_ENABLE);
 		/* The 8705 does not have VID capability.
-		   The 8718 does not use IT87_REG_VID for the same purpose. */
+		   The 8718 and the 8720 don't use IT87_REG_VID for the
+		   same purpose. */
 		if (data->type == it8712 || data->type == it8716) {
 			data->vid = it87_read_value(data, IT87_REG_VID);
 			/* The older IT8712F revisions had only 5 VID pins,
@@ -1540,6 +1553,10 @@ static int __init it87_device_add(unsigned short address,
 	};
 	int err;
 
+	err = acpi_check_resource_conflict(&res);
+	if (err)
+		goto exit;
+
 	pdev = platform_device_alloc(DRVNAME, address);
 	if (!pdev) {
 		err = -ENOMEM;
@@ -1608,7 +1625,7 @@ static void __exit sm_it87_exit(void)
 
 MODULE_AUTHOR("Chris Gauthron, "
 	      "Jean Delvare <khali@linux-fr.org>");
-MODULE_DESCRIPTION("IT8705F/8712F/8716F/8718F/8726F, SiS950 driver");
+MODULE_DESCRIPTION("IT8705F/8712F/8716F/8718F/8720F/8726F, SiS950 driver");
 module_param(update_vbat, bool, 0);
 MODULE_PARM_DESC(update_vbat, "Update vbat if set else return powerup value");
 module_param(fix_pwm_polarity, bool, 0);
diff --git a/drivers/hwmon/lm70.c b/drivers/hwmon/lm70.c
index d435f003292d..ae6204f33214 100644
--- a/drivers/hwmon/lm70.c
+++ b/drivers/hwmon/lm70.c
@@ -37,9 +37,13 @@
 
 #define DRVNAME		"lm70"
 
+#define LM70_CHIP_LM70		0	/* original NS LM70 */
+#define LM70_CHIP_TMP121	1	/* TI TMP121/TMP123 */
+
 struct lm70 {
 	struct device *hwmon_dev;
 	struct mutex lock;
+	unsigned int chip;
 };
 
 /* sysfs hook function */
@@ -47,7 +51,7 @@ static ssize_t lm70_sense_temp(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
 	struct spi_device *spi = to_spi_device(dev);
-	int status, val;
+	int status, val = 0;
 	u8 rxbuf[2];
 	s16 raw=0;
 	struct lm70 *p_lm70 = dev_get_drvdata(&spi->dev);
@@ -65,12 +69,12 @@ static ssize_t lm70_sense_temp(struct device *dev,
 		"spi_write_then_read failed with status %d\n", status);
 		goto out;
 	}
-	dev_dbg(dev, "rxbuf[1] : 0x%x rxbuf[0] : 0x%x\n", rxbuf[1], rxbuf[0]);
-
-	raw = (rxbuf[1] << 8) + rxbuf[0];
-	dev_dbg(dev, "raw=0x%x\n", raw);
+	raw = (rxbuf[0] << 8) + rxbuf[1];
+	dev_dbg(dev, "rxbuf[0] : 0x%02x rxbuf[1] : 0x%02x raw=0x%04x\n",
+		rxbuf[0], rxbuf[1], raw);
 
 	/*
+	 * LM70:
 	 * The "raw" temperature read into rxbuf[] is a 16-bit signed 2's
 	 * complement value. Only the MSB 11 bits (1 sign + 10 temperature
 	 * bits) are meaningful; the LSB 5 bits are to be discarded.
@@ -80,8 +84,21 @@ static ssize_t lm70_sense_temp(struct device *dev,
 	 * by 0.25. Also multiply by 1000 to represent in millidegrees
 	 * Celsius.
 	 * So it's equivalent to multiplying by 0.25 * 1000 = 250.
+	 *
+	 * TMP121/TMP123:
+	 * 13 bits of 2's complement data, discard LSB 3 bits,
+	 * resolution 0.0625 degrees celsius.
 	 */
-	val = ((int)raw/32) * 250;
+	switch (p_lm70->chip) {
+	case LM70_CHIP_LM70:
+		val = ((int)raw / 32) * 250;
+		break;
+
+	case LM70_CHIP_TMP121:
+		val = ((int)raw / 8) * 625 / 10;
+		break;
+	}
+
 	status = sprintf(buf, "%d\n", val); /* millidegrees Celsius */
 out:
 	mutex_unlock(&p_lm70->lock);
@@ -93,27 +110,39 @@ 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, "lm70\n");
+	struct lm70 *p_lm70 = dev_get_drvdata(dev);
+	int ret;
+
+	switch (p_lm70->chip) {
+	case LM70_CHIP_LM70:
+		ret = sprintf(buf, "lm70\n");
+		break;
+	case LM70_CHIP_TMP121:
+		ret = sprintf(buf, "tmp121\n");
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	return ret;
 }
 
 static DEVICE_ATTR(name, S_IRUGO, lm70_show_name, NULL);
 
 /*----------------------------------------------------------------------*/
 
-static int __devinit lm70_probe(struct spi_device *spi)
+static int __devinit common_probe(struct spi_device *spi, int chip)
 {
 	struct lm70 *p_lm70;
 	int status;
 
-	/* signaling is SPI_MODE_0 on a 3-wire link (shared SI/SO) */
-	if ((spi->mode & (SPI_CPOL|SPI_CPHA)) || !(spi->mode & SPI_3WIRE))
-		return -EINVAL;
+	/* NOTE:  we assume 8-bit words, and convert to 16 bits manually */
 
 	p_lm70 = kzalloc(sizeof *p_lm70, GFP_KERNEL);
 	if (!p_lm70)
 		return -ENOMEM;
 
 	mutex_init(&p_lm70->lock);
+	p_lm70->chip = chip;
 
 	/* sysfs hook */
 	p_lm70->hwmon_dev = hwmon_device_register(&spi->dev);
@@ -141,6 +170,24 @@ out_dev_reg_failed:
 	return status;
 }
 
+static int __devinit lm70_probe(struct spi_device *spi)
+{
+	/* signaling is SPI_MODE_0 on a 3-wire link (shared SI/SO) */
+	if ((spi->mode & (SPI_CPOL | SPI_CPHA)) || !(spi->mode & SPI_3WIRE))
+		return -EINVAL;
+
+	return common_probe(spi, LM70_CHIP_LM70);
+}
+
+static int __devinit tmp121_probe(struct spi_device *spi)
+{
+	/* signaling is SPI_MODE_0 with only MISO connected */
+	if (spi->mode & (SPI_CPOL | SPI_CPHA))
+		return -EINVAL;
+
+	return common_probe(spi, LM70_CHIP_TMP121);
+}
+
 static int __devexit lm70_remove(struct spi_device *spi)
 {
 	struct lm70 *p_lm70 = dev_get_drvdata(&spi->dev);
@@ -154,6 +201,15 @@ static int __devexit lm70_remove(struct spi_device *spi)
 	return 0;
 }
 
+static struct spi_driver tmp121_driver = {
+	.driver = {
+		.name	= "tmp121",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= tmp121_probe,
+	.remove	= __devexit_p(lm70_remove),
+};
+
 static struct spi_driver lm70_driver = {
 	.driver = {
 		.name	= "lm70",
@@ -165,17 +221,26 @@ static struct spi_driver lm70_driver = {
 
 static int __init init_lm70(void)
 {
-	return spi_register_driver(&lm70_driver);
+	int ret = spi_register_driver(&lm70_driver);
+	if (ret)
+		return ret;
+
+	ret = spi_register_driver(&tmp121_driver);
+	if (ret)
+		spi_unregister_driver(&lm70_driver);
+
+	return ret;
 }
 
 static void __exit cleanup_lm70(void)
 {
 	spi_unregister_driver(&lm70_driver);
+	spi_unregister_driver(&tmp121_driver);
 }
 
 module_init(init_lm70);
 module_exit(cleanup_lm70);
 
 MODULE_AUTHOR("Kaiwan N Billimoria");
-MODULE_DESCRIPTION("National Semiconductor LM70 Linux driver");
+MODULE_DESCRIPTION("NS LM70 / TI TMP121/TMP123 Linux driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/lm75.c b/drivers/hwmon/lm75.c
index 8f9595f2fb53..55bd87c15c9a 100644
--- a/drivers/hwmon/lm75.c
+++ b/drivers/hwmon/lm75.c
@@ -190,7 +190,7 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	}
 
 	dev_info(&client->dev, "%s: sensor '%s'\n",
-		data->hwmon_dev->bus_id, client->name);
+		 dev_name(data->hwmon_dev), client->name);
 
 	return 0;
 
diff --git a/drivers/hwmon/ltc4245.c b/drivers/hwmon/ltc4245.c
new file mode 100644
index 000000000000..034b2c515848
--- /dev/null
+++ b/drivers/hwmon/ltc4245.c
@@ -0,0 +1,567 @@
+/*
+ * Driver for Linear Technology LTC4245 I2C Multiple Supply Hot Swap Controller
+ *
+ * Copyright (C) 2008 Ira W. Snyder <iws@ovro.caltech.edu>
+ *
+ * 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 driver is based on the ds1621 and ina209 drivers.
+ *
+ * Datasheet:
+ * http://www.linear.com/pc/downloadDocument.do?navId=H0,C1,C1003,C1006,C1140,P19392,D13517
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+
+/* Valid addresses are 0x20 - 0x3f
+ *
+ * For now, we do not probe, since some of these addresses
+ * are known to be unfriendly to probing */
+static const unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+/* Insmod parameters */
+I2C_CLIENT_INSMOD_1(ltc4245);
+
+/* Here are names of the chip's registers (a.k.a. commands) */
+enum ltc4245_cmd {
+	LTC4245_STATUS			= 0x00, /* readonly */
+	LTC4245_ALERT			= 0x01,
+	LTC4245_CONTROL			= 0x02,
+	LTC4245_ON			= 0x03,
+	LTC4245_FAULT1			= 0x04,
+	LTC4245_FAULT2			= 0x05,
+	LTC4245_GPIO			= 0x06,
+	LTC4245_ADCADR			= 0x07,
+
+	LTC4245_12VIN			= 0x10,
+	LTC4245_12VSENSE		= 0x11,
+	LTC4245_12VOUT			= 0x12,
+	LTC4245_5VIN			= 0x13,
+	LTC4245_5VSENSE			= 0x14,
+	LTC4245_5VOUT			= 0x15,
+	LTC4245_3VIN			= 0x16,
+	LTC4245_3VSENSE			= 0x17,
+	LTC4245_3VOUT			= 0x18,
+	LTC4245_VEEIN			= 0x19,
+	LTC4245_VEESENSE		= 0x1a,
+	LTC4245_VEEOUT			= 0x1b,
+	LTC4245_GPIOADC1		= 0x1c,
+	LTC4245_GPIOADC2		= 0x1d,
+	LTC4245_GPIOADC3		= 0x1e,
+};
+
+struct ltc4245_data {
+	struct device *hwmon_dev;
+
+	struct mutex update_lock;
+	bool valid;
+	unsigned long last_updated; /* in jiffies */
+
+	/* Control registers */
+	u8 cregs[0x08];
+
+	/* Voltage registers */
+	u8 vregs[0x0f];
+};
+
+static struct ltc4245_data *ltc4245_update_device(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct ltc4245_data *data = i2c_get_clientdata(client);
+	s32 val;
+	int i;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + HZ) || !data->valid) {
+
+		dev_dbg(&client->dev, "Starting ltc4245 update\n");
+
+		/* Read control registers -- 0x00 to 0x07 */
+		for (i = 0; i < ARRAY_SIZE(data->cregs); i++) {
+			val = i2c_smbus_read_byte_data(client, i);
+			if (unlikely(val < 0))
+				data->cregs[i] = 0;
+			else
+				data->cregs[i] = val;
+		}
+
+		/* Read voltage registers -- 0x10 to 0x1f */
+		for (i = 0; i < ARRAY_SIZE(data->vregs); i++) {
+			val = i2c_smbus_read_byte_data(client, i+0x10);
+			if (unlikely(val < 0))
+				data->vregs[i] = 0;
+			else
+				data->vregs[i] = val;
+		}
+
+		data->last_updated = jiffies;
+		data->valid = 1;
+	}
+
+	mutex_unlock(&data->update_lock);
+
+	return data;
+}
+
+/* Return the voltage from the given register in millivolts */
+static int ltc4245_get_voltage(struct device *dev, u8 reg)
+{
+	struct ltc4245_data *data = ltc4245_update_device(dev);
+	const u8 regval = data->vregs[reg - 0x10];
+	u32 voltage = 0;
+
+	switch (reg) {
+	case LTC4245_12VIN:
+	case LTC4245_12VOUT:
+		voltage = regval * 55;
+		break;
+	case LTC4245_5VIN:
+	case LTC4245_5VOUT:
+		voltage = regval * 22;
+		break;
+	case LTC4245_3VIN:
+	case LTC4245_3VOUT:
+		voltage = regval * 15;
+		break;
+	case LTC4245_VEEIN:
+	case LTC4245_VEEOUT:
+		voltage = regval * -55;
+		break;
+	case LTC4245_GPIOADC1:
+	case LTC4245_GPIOADC2:
+	case LTC4245_GPIOADC3:
+		voltage = regval * 10;
+		break;
+	default:
+		/* If we get here, the developer messed up */
+		WARN_ON_ONCE(1);
+		break;
+	}
+
+	return voltage;
+}
+
+/* Return the current in the given sense register in milliAmperes */
+static unsigned int ltc4245_get_current(struct device *dev, u8 reg)
+{
+	struct ltc4245_data *data = ltc4245_update_device(dev);
+	const u8 regval = data->vregs[reg - 0x10];
+	unsigned int voltage;
+	unsigned int curr;
+
+	/* The strange looking conversions that follow are fixed-point
+	 * math, since we cannot do floating point in the kernel.
+	 *
+	 * Step 1: convert sense register to microVolts
+	 * Step 2: convert voltage to milliAmperes
+	 *
+	 * If you play around with the V=IR equation, you come up with
+	 * the following: X uV / Y mOhm == Z mA
+	 *
+	 * With the resistors that are fractions of a milliOhm, we multiply
+	 * the voltage and resistance by 10, to shift the decimal point.
+	 * Now we can use the normal division operator again.
+	 */
+
+	switch (reg) {
+	case LTC4245_12VSENSE:
+		voltage = regval * 250; /* voltage in uV */
+		curr = voltage / 50; /* sense resistor 50 mOhm */
+		break;
+	case LTC4245_5VSENSE:
+		voltage = regval * 125; /* voltage in uV */
+		curr = (voltage * 10) / 35; /* sense resistor 3.5 mOhm */
+		break;
+	case LTC4245_3VSENSE:
+		voltage = regval * 125; /* voltage in uV */
+		curr = (voltage * 10) / 25; /* sense resistor 2.5 mOhm */
+		break;
+	case LTC4245_VEESENSE:
+		voltage = regval * 250; /* voltage in uV */
+		curr = voltage / 100; /* sense resistor 100 mOhm */
+		break;
+	default:
+		/* If we get here, the developer messed up */
+		WARN_ON_ONCE(1);
+		curr = 0;
+		break;
+	}
+
+	return curr;
+}
+
+static ssize_t ltc4245_show_voltage(struct device *dev,
+				    struct device_attribute *da,
+				    char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	const int voltage = ltc4245_get_voltage(dev, attr->index);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", voltage);
+}
+
+static ssize_t ltc4245_show_current(struct device *dev,
+				    struct device_attribute *da,
+				    char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	const unsigned int curr = ltc4245_get_current(dev, attr->index);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", curr);
+}
+
+static ssize_t ltc4245_show_power(struct device *dev,
+				  struct device_attribute *da,
+				  char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	const unsigned int curr = ltc4245_get_current(dev, attr->index);
+	const int output_voltage = ltc4245_get_voltage(dev, attr->index+1);
+
+	/* current in mA * voltage in mV == power in uW */
+	const unsigned int power = abs(output_voltage * curr);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", power);
+}
+
+static ssize_t ltc4245_show_alarm(struct device *dev,
+					  struct device_attribute *da,
+					  char *buf)
+{
+	struct sensor_device_attribute_2 *attr = to_sensor_dev_attr_2(da);
+	struct ltc4245_data *data = ltc4245_update_device(dev);
+	const u8 reg = data->cregs[attr->index];
+	const u32 mask = attr->nr;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", (reg & mask) ? 1 : 0);
+}
+
+/* These macros are used below in constructing device attribute objects
+ * for use with sysfs_create_group() to make a sysfs device file
+ * for each register.
+ */
+
+#define LTC4245_VOLTAGE(name, ltc4245_cmd_idx) \
+	static SENSOR_DEVICE_ATTR(name, S_IRUGO, \
+	ltc4245_show_voltage, NULL, ltc4245_cmd_idx)
+
+#define LTC4245_CURRENT(name, ltc4245_cmd_idx) \
+	static SENSOR_DEVICE_ATTR(name, S_IRUGO, \
+	ltc4245_show_current, NULL, ltc4245_cmd_idx)
+
+#define LTC4245_POWER(name, ltc4245_cmd_idx) \
+	static SENSOR_DEVICE_ATTR(name, S_IRUGO, \
+	ltc4245_show_power, NULL, ltc4245_cmd_idx)
+
+#define LTC4245_ALARM(name, mask, reg) \
+	static SENSOR_DEVICE_ATTR_2(name, S_IRUGO, \
+	ltc4245_show_alarm, NULL, (mask), reg)
+
+/* Construct a sensor_device_attribute structure for each register */
+
+/* Input voltages */
+LTC4245_VOLTAGE(in1_input,			LTC4245_12VIN);
+LTC4245_VOLTAGE(in2_input,			LTC4245_5VIN);
+LTC4245_VOLTAGE(in3_input,			LTC4245_3VIN);
+LTC4245_VOLTAGE(in4_input,			LTC4245_VEEIN);
+
+/* Input undervoltage alarms */
+LTC4245_ALARM(in1_min_alarm,	(1 << 0),	LTC4245_FAULT1);
+LTC4245_ALARM(in2_min_alarm,	(1 << 1),	LTC4245_FAULT1);
+LTC4245_ALARM(in3_min_alarm,	(1 << 2),	LTC4245_FAULT1);
+LTC4245_ALARM(in4_min_alarm,	(1 << 3),	LTC4245_FAULT1);
+
+/* Currents (via sense resistor) */
+LTC4245_CURRENT(curr1_input,			LTC4245_12VSENSE);
+LTC4245_CURRENT(curr2_input,			LTC4245_5VSENSE);
+LTC4245_CURRENT(curr3_input,			LTC4245_3VSENSE);
+LTC4245_CURRENT(curr4_input,			LTC4245_VEESENSE);
+
+/* Overcurrent alarms */
+LTC4245_ALARM(curr1_max_alarm,	(1 << 4),	LTC4245_FAULT1);
+LTC4245_ALARM(curr2_max_alarm,	(1 << 5),	LTC4245_FAULT1);
+LTC4245_ALARM(curr3_max_alarm,	(1 << 6),	LTC4245_FAULT1);
+LTC4245_ALARM(curr4_max_alarm,	(1 << 7),	LTC4245_FAULT1);
+
+/* Output voltages */
+LTC4245_VOLTAGE(in5_input,			LTC4245_12VOUT);
+LTC4245_VOLTAGE(in6_input,			LTC4245_5VOUT);
+LTC4245_VOLTAGE(in7_input,			LTC4245_3VOUT);
+LTC4245_VOLTAGE(in8_input,			LTC4245_VEEOUT);
+
+/* Power Bad alarms */
+LTC4245_ALARM(in5_min_alarm,	(1 << 0),	LTC4245_FAULT2);
+LTC4245_ALARM(in6_min_alarm,	(1 << 1),	LTC4245_FAULT2);
+LTC4245_ALARM(in7_min_alarm,	(1 << 2),	LTC4245_FAULT2);
+LTC4245_ALARM(in8_min_alarm,	(1 << 3),	LTC4245_FAULT2);
+
+/* GPIO voltages */
+LTC4245_VOLTAGE(in9_input,			LTC4245_GPIOADC1);
+LTC4245_VOLTAGE(in10_input,			LTC4245_GPIOADC2);
+LTC4245_VOLTAGE(in11_input,			LTC4245_GPIOADC3);
+
+/* Power Consumption (virtual) */
+LTC4245_POWER(power1_input,			LTC4245_12VSENSE);
+LTC4245_POWER(power2_input,			LTC4245_5VSENSE);
+LTC4245_POWER(power3_input,			LTC4245_3VSENSE);
+LTC4245_POWER(power4_input,			LTC4245_VEESENSE);
+
+/* Finally, construct an array of pointers to members of the above objects,
+ * as required for sysfs_create_group()
+ */
+static struct attribute *ltc4245_attributes[] = {
+	&sensor_dev_attr_in1_input.dev_attr.attr,
+	&sensor_dev_attr_in2_input.dev_attr.attr,
+	&sensor_dev_attr_in3_input.dev_attr.attr,
+	&sensor_dev_attr_in4_input.dev_attr.attr,
+
+	&sensor_dev_attr_in1_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_in2_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_in3_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_in4_min_alarm.dev_attr.attr,
+
+	&sensor_dev_attr_curr1_input.dev_attr.attr,
+	&sensor_dev_attr_curr2_input.dev_attr.attr,
+	&sensor_dev_attr_curr3_input.dev_attr.attr,
+	&sensor_dev_attr_curr4_input.dev_attr.attr,
+
+	&sensor_dev_attr_curr1_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_curr2_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_curr3_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_curr4_max_alarm.dev_attr.attr,
+
+	&sensor_dev_attr_in5_input.dev_attr.attr,
+	&sensor_dev_attr_in6_input.dev_attr.attr,
+	&sensor_dev_attr_in7_input.dev_attr.attr,
+	&sensor_dev_attr_in8_input.dev_attr.attr,
+
+	&sensor_dev_attr_in5_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_in6_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_in7_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_in8_min_alarm.dev_attr.attr,
+
+	&sensor_dev_attr_in9_input.dev_attr.attr,
+	&sensor_dev_attr_in10_input.dev_attr.attr,
+	&sensor_dev_attr_in11_input.dev_attr.attr,
+
+	&sensor_dev_attr_power1_input.dev_attr.attr,
+	&sensor_dev_attr_power2_input.dev_attr.attr,
+	&sensor_dev_attr_power3_input.dev_attr.attr,
+	&sensor_dev_attr_power4_input.dev_attr.attr,
+
+	NULL,
+};
+
+static const struct attribute_group ltc4245_group = {
+	.attrs = ltc4245_attributes,
+};
+
+static int ltc4245_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct ltc4245_data *data;
+	int ret;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		ret = -ENOMEM;
+		goto out_kzalloc;
+	}
+
+	i2c_set_clientdata(client, data);
+	mutex_init(&data->update_lock);
+
+	/* Initialize the LTC4245 chip */
+	/* TODO */
+
+	/* Register sysfs hooks */
+	ret = sysfs_create_group(&client->dev.kobj, &ltc4245_group);
+	if (ret)
+		goto out_sysfs_create_group;
+
+	data->hwmon_dev = hwmon_device_register(&client->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		ret = PTR_ERR(data->hwmon_dev);
+		goto out_hwmon_device_register;
+	}
+
+	return 0;
+
+out_hwmon_device_register:
+	sysfs_remove_group(&client->dev.kobj, &ltc4245_group);
+out_sysfs_create_group:
+	kfree(data);
+out_kzalloc:
+	return ret;
+}
+
+static int ltc4245_remove(struct i2c_client *client)
+{
+	struct ltc4245_data *data = i2c_get_clientdata(client);
+
+	hwmon_device_unregister(data->hwmon_dev);
+	sysfs_remove_group(&client->dev.kobj, &ltc4245_group);
+
+	kfree(data);
+
+	return 0;
+}
+
+/* Check that some bits in a control register appear at all possible
+ * locations without changing value
+ *
+ * @client: the i2c client to use
+ * @reg: the register to read
+ * @bits: the bits to check (0xff checks all bits,
+ *                           0x03 checks only the last two bits)
+ *
+ * return -ERRNO if the register read failed
+ * return -ENODEV if the register value doesn't stay constant at all
+ * possible addresses
+ *
+ * return 0 for success
+ */
+static int ltc4245_check_control_reg(struct i2c_client *client, u8 reg, u8 bits)
+{
+	int i;
+	s32 v, voff1, voff2;
+
+	/* Read register and check for error */
+	v = i2c_smbus_read_byte_data(client, reg);
+	if (v < 0)
+		return v;
+
+	v &= bits;
+
+	for (i = 0x00; i < 0xff; i += 0x20) {
+
+		voff1 = i2c_smbus_read_byte_data(client, reg + i);
+		if (voff1 < 0)
+			return voff1;
+
+		voff2 = i2c_smbus_read_byte_data(client, reg + i + 0x08);
+		if (voff2 < 0)
+			return voff2;
+
+		voff1 &= bits;
+		voff2 &= bits;
+
+		if (v != voff1 || v != voff2)
+			return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int ltc4245_detect(struct i2c_client *client,
+			  int kind,
+			  struct i2c_board_info *info)
+{
+	struct i2c_adapter *adapter = client->adapter;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	if (kind < 0) {		/* probed detection - check the chip type */
+		s32 v;		/* 8 bits from the chip, or -ERRNO */
+
+		/* Chip registers 0x00-0x07 are control registers
+		 * Chip registers 0x10-0x1f are data registers
+		 *
+		 * Address bits b7-b5 are ignored. This makes the chip "repeat"
+		 * in steps of 0x20. Any control registers should appear with
+		 * the same values across all duplicated addresses.
+		 *
+		 * Register 0x02 bit b2 is reserved, expect 0
+		 * Register 0x07 bits b7 to b4 are reserved, expect 0
+		 *
+		 * Registers 0x01, 0x02 are control registers and should not
+		 * change on their own.
+		 *
+		 * Register 0x06 bits b6 and b7 are control bits, and should
+		 * not change on their own.
+		 *
+		 * Register 0x07 bits b3 to b0 are control bits, and should
+		 * not change on their own.
+		 */
+
+		/* read register 0x02 reserved bit, expect 0 */
+		v = i2c_smbus_read_byte_data(client, LTC4245_CONTROL);
+		if (v < 0 || (v & 0x04) != 0)
+			return -ENODEV;
+
+		/* read register 0x07 reserved bits, expect 0 */
+		v = i2c_smbus_read_byte_data(client, LTC4245_ADCADR);
+		if (v < 0 || (v & 0xf0) != 0)
+			return -ENODEV;
+
+		/* check that the alert register appears at all locations */
+		if (ltc4245_check_control_reg(client, LTC4245_ALERT, 0xff))
+			return -ENODEV;
+
+		/* check that the control register appears at all locations */
+		if (ltc4245_check_control_reg(client, LTC4245_CONTROL, 0xff))
+			return -ENODEV;
+
+		/* check that register 0x06 bits b6 and b7 stay constant */
+		if (ltc4245_check_control_reg(client, LTC4245_GPIO, 0xc0))
+			return -ENODEV;
+
+		/* check that register 0x07 bits b3-b0 stay constant */
+		if (ltc4245_check_control_reg(client, LTC4245_ADCADR, 0x0f))
+			return -ENODEV;
+	}
+
+	strlcpy(info->type, "ltc4245", I2C_NAME_SIZE);
+	dev_info(&adapter->dev, "ltc4245 %s at address 0x%02x\n",
+			kind < 0 ? "probed" : "forced",
+			client->addr);
+
+	return 0;
+}
+
+static const struct i2c_device_id ltc4245_id[] = {
+	{ "ltc4245", ltc4245 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ltc4245_id);
+
+/* This is the driver that will be inserted */
+static struct i2c_driver ltc4245_driver = {
+	.class		= I2C_CLASS_HWMON,
+	.driver = {
+		.name	= "ltc4245",
+	},
+	.probe		= ltc4245_probe,
+	.remove		= ltc4245_remove,
+	.id_table	= ltc4245_id,
+	.detect		= ltc4245_detect,
+	.address_data	= &addr_data,
+};
+
+static int __init ltc4245_init(void)
+{
+	return i2c_add_driver(&ltc4245_driver);
+}
+
+static void __exit ltc4245_exit(void)
+{
+	i2c_del_driver(&ltc4245_driver);
+}
+
+MODULE_AUTHOR("Ira W. Snyder <iws@ovro.caltech.edu>");
+MODULE_DESCRIPTION("LTC4245 driver");
+MODULE_LICENSE("GPL");
+
+module_init(ltc4245_init);
+module_exit(ltc4245_exit);
diff --git a/drivers/hwmon/pc87360.c b/drivers/hwmon/pc87360.c
index 5fbfa34c110e..fb052fea3744 100644
--- a/drivers/hwmon/pc87360.c
+++ b/drivers/hwmon/pc87360.c
@@ -43,6 +43,7 @@
 #include <linux/hwmon-vid.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
+#include <linux/acpi.h>
 #include <asm/io.h>
 
 static u8 devid;
@@ -1627,6 +1628,11 @@ static int __init pc87360_device_add(unsigned short address)
 			continue;
 		res.start = extra_isa[i];
 		res.end = extra_isa[i] + PC87360_EXTENT - 1;
+
+		err = acpi_check_resource_conflict(&res);
+		if (err)
+			goto exit_device_put;
+
 		err = platform_device_add_resources(pdev, &res, 1);
 		if (err) {
 			printk(KERN_ERR "pc87360: Device resource[%d] "
diff --git a/drivers/hwmon/pc87427.c b/drivers/hwmon/pc87427.c
index 7265f22ae5cd..3a8a0f7a7736 100644
--- a/drivers/hwmon/pc87427.c
+++ b/drivers/hwmon/pc87427.c
@@ -32,6 +32,7 @@
 #include <linux/mutex.h>
 #include <linux/sysfs.h>
 #include <linux/ioport.h>
+#include <linux/acpi.h>
 #include <asm/io.h>
 
 static unsigned short force_id;
@@ -524,6 +525,10 @@ static int __init pc87427_device_add(unsigned short address)
 	};
 	int err;
 
+	err = acpi_check_resource_conflict(&res);
+	if (err)
+		goto exit;
+
 	pdev = platform_device_alloc(DRVNAME, address);
 	if (!pdev) {
 		err = -ENOMEM;
diff --git a/drivers/hwmon/sis5595.c b/drivers/hwmon/sis5595.c
index a276806f3d53..aa2e8318f167 100644
--- a/drivers/hwmon/sis5595.c
+++ b/drivers/hwmon/sis5595.c
@@ -62,6 +62,7 @@
 #include <linux/jiffies.h>
 #include <linux/mutex.h>
 #include <linux/sysfs.h>
+#include <linux/acpi.h>
 #include <asm/io.h>
 
 
@@ -727,6 +728,10 @@ static int __devinit sis5595_device_add(unsigned short address)
 	};
 	int err;
 
+	err = acpi_check_resource_conflict(&res);
+	if (err)
+		goto exit;
+
 	pdev = platform_device_alloc("sis5595", address);
 	if (!pdev) {
 		err = -ENOMEM;
diff --git a/drivers/hwmon/smsc47b397.c b/drivers/hwmon/smsc47b397.c
index eb03544c731c..6f6d52b4fb64 100644
--- a/drivers/hwmon/smsc47b397.c
+++ b/drivers/hwmon/smsc47b397.c
@@ -36,6 +36,7 @@
 #include <linux/err.h>
 #include <linux/init.h>
 #include <linux/mutex.h>
+#include <linux/acpi.h>
 #include <asm/io.h>
 
 static unsigned short force_id;
@@ -303,6 +304,10 @@ static int __init smsc47b397_device_add(unsigned short address)
 	};
 	int err;
 
+	err = acpi_check_resource_conflict(&res);
+	if (err)
+		goto exit;
+
 	pdev = platform_device_alloc(DRVNAME, address);
 	if (!pdev) {
 		err = -ENOMEM;
diff --git a/drivers/hwmon/smsc47m1.c b/drivers/hwmon/smsc47m1.c
index d1b498548736..a92dbb97ee99 100644
--- a/drivers/hwmon/smsc47m1.c
+++ b/drivers/hwmon/smsc47m1.c
@@ -37,6 +37,7 @@
 #include <linux/init.h>
 #include <linux/mutex.h>
 #include <linux/sysfs.h>
+#include <linux/acpi.h>
 #include <asm/io.h>
 
 static unsigned short force_id;
@@ -705,6 +706,10 @@ static int __init smsc47m1_device_add(unsigned short address,
 	};
 	int err;
 
+	err = acpi_check_resource_conflict(&res);
+	if (err)
+		goto exit;
+
 	pdev = platform_device_alloc(DRVNAME, address);
 	if (!pdev) {
 		err = -ENOMEM;
diff --git a/drivers/hwmon/via686a.c b/drivers/hwmon/via686a.c
index f1ee5e731968..a022aedcaacb 100644
--- a/drivers/hwmon/via686a.c
+++ b/drivers/hwmon/via686a.c
@@ -41,6 +41,7 @@
 #include <linux/init.h>
 #include <linux/mutex.h>
 #include <linux/sysfs.h>
+#include <linux/acpi.h>
 #include <asm/io.h>
 
 
@@ -783,6 +784,10 @@ static int __devinit via686a_device_add(unsigned short address)
 	};
 	int err;
 
+	err = acpi_check_resource_conflict(&res);
+	if (err)
+		goto exit;
+
 	pdev = platform_device_alloc("via686a", address);
 	if (!pdev) {
 		err = -ENOMEM;
diff --git a/drivers/hwmon/vt1211.c b/drivers/hwmon/vt1211.c
index 12b43590fa53..b0ce37852281 100644
--- a/drivers/hwmon/vt1211.c
+++ b/drivers/hwmon/vt1211.c
@@ -32,6 +32,7 @@
 #include <linux/err.h>
 #include <linux/mutex.h>
 #include <linux/ioport.h>
+#include <linux/acpi.h>
 #include <asm/io.h>
 
 static int uch_config = -1;
@@ -1259,6 +1260,10 @@ static int __init vt1211_device_add(unsigned short address)
 	}
 
 	res.name = pdev->name;
+	err = acpi_check_resource_conflict(&res);
+	if (err)
+		goto EXIT;
+
 	err = platform_device_add_resources(pdev, &res, 1);
 	if (err) {
 		printk(KERN_ERR DRVNAME ": Device resource addition failed "
diff --git a/drivers/hwmon/vt8231.c b/drivers/hwmon/vt8231.c
index 5bc57275cae8..9982b45fbb14 100644
--- a/drivers/hwmon/vt8231.c
+++ b/drivers/hwmon/vt8231.c
@@ -35,6 +35,7 @@
 #include <linux/hwmon-vid.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
+#include <linux/acpi.h>
 #include <asm/io.h>
 
 static int force_addr;
@@ -894,6 +895,10 @@ static int __devinit vt8231_device_add(unsigned short address)
 	};
 	int err;
 
+	err = acpi_check_resource_conflict(&res);
+	if (err)
+		goto exit;
+
 	pdev = platform_device_alloc("vt8231", address);
 	if (!pdev) {
 		err = -ENOMEM;
diff --git a/drivers/hwmon/w83627ehf.c b/drivers/hwmon/w83627ehf.c
index 075164dd65a7..cb808d015361 100644
--- a/drivers/hwmon/w83627ehf.c
+++ b/drivers/hwmon/w83627ehf.c
@@ -48,6 +48,7 @@
 #include <linux/hwmon-vid.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
+#include <linux/acpi.h>
 #include <asm/io.h>
 #include "lm75.h"
 
@@ -502,7 +503,7 @@ static struct w83627ehf_data *w83627ehf_update_device(struct device *dev)
 		}
 
 		for (i = 0; i < 4; i++) {
-			/* pwmcfg, tolarance mapped for i=0, i=1 to same reg */
+			/* pwmcfg, tolerance mapped for i=0, i=1 to same reg */
 			if (i != 1) {
 				pwmcfg = w83627ehf_read_value(data,
 						W83627EHF_REG_PWM_ENABLE[i]);
@@ -1544,6 +1545,11 @@ static int __init sensors_w83627ehf_init(void)
 	res.start = address + IOREGION_OFFSET;
 	res.end = address + IOREGION_OFFSET + IOREGION_LENGTH - 1;
 	res.flags = IORESOURCE_IO;
+
+	err = acpi_check_resource_conflict(&res);
+	if (err)
+		goto exit;
+
 	err = platform_device_add_resources(pdev, &res, 1);
 	if (err) {
 		printk(KERN_ERR DRVNAME ": Device resource addition failed "
diff --git a/drivers/hwmon/w83627hf.c b/drivers/hwmon/w83627hf.c
index b30e5796cb26..389150ba30d3 100644
--- a/drivers/hwmon/w83627hf.c
+++ b/drivers/hwmon/w83627hf.c
@@ -50,6 +50,7 @@
 #include <linux/err.h>
 #include <linux/mutex.h>
 #include <linux/ioport.h>
+#include <linux/acpi.h>
 #include <asm/io.h>
 #include "lm75.h"
 
@@ -1793,6 +1794,10 @@ static int __init w83627hf_device_add(unsigned short address,
 	};
 	int err;
 
+	err = acpi_check_resource_conflict(&res);
+	if (err)
+		goto exit;
+
 	pdev = platform_device_alloc(DRVNAME, address);
 	if (!pdev) {
 		err = -ENOMEM;
diff --git a/drivers/hwmon/w83781d.c b/drivers/hwmon/w83781d.c
index fc12bd412e3a..dbfb30c588d8 100644
--- a/drivers/hwmon/w83781d.c
+++ b/drivers/hwmon/w83781d.c
@@ -58,7 +58,10 @@ static const unsigned short normal_i2c[] = { 0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d,
 						0x2e, 0x2f, I2C_CLIENT_END };
 /* Insmod parameters */
 I2C_CLIENT_INSMOD_4(w83781d, w83782d, w83783s, as99127f);
-I2C_CLIENT_MODULE_PARM(force_subclients, "List of subclient addresses: "
+
+static unsigned short force_subclients[4];
+module_param_array(force_subclients, short, NULL, 0);
+MODULE_PARM_DESC(force_subclients, "List of subclient addresses: "
 		    "{bus, clientaddr, subclientaddr1, subclientaddr2}");
 
 static int reset;
diff --git a/drivers/hwmon/w83791d.c b/drivers/hwmon/w83791d.c
index 5768def8a4f2..97851c5ba3a3 100644
--- a/drivers/hwmon/w83791d.c
+++ b/drivers/hwmon/w83791d.c
@@ -53,7 +53,10 @@ static const unsigned short normal_i2c[] = { 0x2c, 0x2d, 0x2e, 0x2f,
 
 /* Insmod parameters */
 I2C_CLIENT_INSMOD_1(w83791d);
-I2C_CLIENT_MODULE_PARM(force_subclients, "List of subclient addresses: "
+
+static unsigned short force_subclients[4];
+module_param_array(force_subclients, short, NULL, 0);
+MODULE_PARM_DESC(force_subclients, "List of subclient addresses: "
 			"{bus, clientaddr, subclientaddr1, subclientaddr2}");
 
 static int reset;
diff --git a/drivers/hwmon/w83792d.c b/drivers/hwmon/w83792d.c
index cf94c5b0c879..2be16194ddf3 100644
--- a/drivers/hwmon/w83792d.c
+++ b/drivers/hwmon/w83792d.c
@@ -51,7 +51,10 @@ static const unsigned short normal_i2c[] = { 0x2c, 0x2d, 0x2e, 0x2f,
 
 /* Insmod parameters */
 I2C_CLIENT_INSMOD_1(w83792d);
-I2C_CLIENT_MODULE_PARM(force_subclients, "List of subclient addresses: "
+
+static unsigned short force_subclients[4];
+module_param_array(force_subclients, short, NULL, 0);
+MODULE_PARM_DESC(force_subclients, "List of subclient addresses: "
 			"{bus, clientaddr, subclientaddr1, subclientaddr2}");
 
 static int init;
diff --git a/drivers/hwmon/w83793.c b/drivers/hwmon/w83793.c
index 0a739f1c69be..47dd398f7258 100644
--- a/drivers/hwmon/w83793.c
+++ b/drivers/hwmon/w83793.c
@@ -42,7 +42,10 @@ static const unsigned short normal_i2c[] = { 0x2c, 0x2d, 0x2e, 0x2f,
 
 /* Insmod parameters */
 I2C_CLIENT_INSMOD_1(w83793);
-I2C_CLIENT_MODULE_PARM(force_subclients, "List of subclient addresses: "
+
+static unsigned short force_subclients[4];
+module_param_array(force_subclients, short, NULL, 0);
+MODULE_PARM_DESC(force_subclients, "List of subclient addresses: "
 		       "{bus, clientaddr, subclientaddr1, subclientaddr2}");
 
 static int reset;
diff --git a/drivers/i2c/busses/i2c-ali1563.c b/drivers/i2c/busses/i2c-ali1563.c
index fc3e5b026423..dd9e796fad69 100644
--- a/drivers/i2c/busses/i2c-ali1563.c
+++ b/drivers/i2c/busses/i2c-ali1563.c
@@ -399,8 +399,8 @@ static int __devinit ali1563_probe(struct pci_dev * dev,
 	if ((error = ali1563_setup(dev)))
 		goto exit;
 	ali1563_adapter.dev.parent = &dev->dev;
-	sprintf(ali1563_adapter.name,"SMBus ALi 1563 Adapter @ %04x",
-		ali1563_smba);
+	snprintf(ali1563_adapter.name, sizeof(ali1563_adapter.name),
+		 "SMBus ALi 1563 Adapter @ %04x", ali1563_smba);
 	if ((error = i2c_add_adapter(&ali1563_adapter)))
 		goto exit_shutdown;
 	return 0;
diff --git a/drivers/i2c/busses/i2c-amd756-s4882.c b/drivers/i2c/busses/i2c-amd756-s4882.c
index 8ba2bcf727d3..378fcb5d5783 100644
--- a/drivers/i2c/busses/i2c-amd756-s4882.c
+++ b/drivers/i2c/busses/i2c-amd756-s4882.c
@@ -197,8 +197,8 @@ static int __init amd756_s4882_init(void)
 	for (i = 1; i < 5; i++) {
 		s4882_algo[i] = *(amd756_smbus.algo);
 		s4882_adapter[i] = amd756_smbus;
-		sprintf(s4882_adapter[i].name,
-			"SMBus 8111 adapter (CPU%d)", i-1);
+		snprintf(s4882_adapter[i].name, sizeof(s4882_adapter[i].name),
+			 "SMBus 8111 adapter (CPU%d)", i-1);
 		s4882_adapter[i].algo = s4882_algo+i;
 		s4882_adapter[i].dev.parent = amd756_smbus.dev.parent;
 	}
diff --git a/drivers/i2c/busses/i2c-amd756.c b/drivers/i2c/busses/i2c-amd756.c
index 424dad6f18d8..36bee5b9c952 100644
--- a/drivers/i2c/busses/i2c-amd756.c
+++ b/drivers/i2c/busses/i2c-amd756.c
@@ -380,8 +380,9 @@ static int __devinit amd756_probe(struct pci_dev *pdev,
 	/* set up the sysfs linkage to our parent device */
 	amd756_smbus.dev.parent = &pdev->dev;
 
-	sprintf(amd756_smbus.name, "SMBus %s adapter at %04x",
-		chipname[id->driver_data], amd756_ioport);
+	snprintf(amd756_smbus.name, sizeof(amd756_smbus.name),
+		 "SMBus %s adapter at %04x", chipname[id->driver_data],
+		 amd756_ioport);
 
 	error = i2c_add_adapter(&amd756_smbus);
 	if (error) {
diff --git a/drivers/i2c/busses/i2c-at91.c b/drivers/i2c/busses/i2c-at91.c
index 9efb02137254..67d9dc5b351b 100644
--- a/drivers/i2c/busses/i2c-at91.c
+++ b/drivers/i2c/busses/i2c-at91.c
@@ -222,7 +222,7 @@ static int __devinit at91_i2c_probe(struct platform_device *pdev)
 		rc = -ENOMEM;
 		goto fail2;
 	}
-	sprintf(adapter->name, "AT91");
+	snprintf(adapter->name, sizeof(adapter->name), "AT91");
 	adapter->algo = &at91_algorithm;
 	adapter->class = I2C_CLASS_HWMON;
 	adapter->dev.parent = &pdev->dev;
diff --git a/drivers/i2c/busses/i2c-bfin-twi.c b/drivers/i2c/busses/i2c-bfin-twi.c
index 3c855ff2992f..3fd2c417c1e0 100644
--- a/drivers/i2c/busses/i2c-bfin-twi.c
+++ b/drivers/i2c/busses/i2c-bfin-twi.c
@@ -656,7 +656,7 @@ static int i2c_bfin_twi_probe(struct platform_device *pdev)
 	strlcpy(p_adap->name, pdev->name, sizeof(p_adap->name));
 	p_adap->algo = &bfin_twi_algorithm;
 	p_adap->algo_data = iface;
-	p_adap->class = I2C_CLASS_ALL;
+	p_adap->class = I2C_CLASS_HWMON | I2C_CLASS_SPD;
 	p_adap->dev.parent = &pdev->dev;
 
 	rc = peripheral_request_list(pin_req[pdev->id], "i2c-bfin-twi");
diff --git a/drivers/i2c/busses/i2c-i801.c b/drivers/i2c/busses/i2c-i801.c
index 5123eb69a971..526625eaa84b 100644
--- a/drivers/i2c/busses/i2c-i801.c
+++ b/drivers/i2c/busses/i2c-i801.c
@@ -64,7 +64,7 @@
 #include <linux/init.h>
 #include <linux/i2c.h>
 #include <linux/acpi.h>
-#include <asm/io.h>
+#include <linux/io.h>
 
 /* I801 SMBus address offsets */
 #define SMBHSTSTS	(0 + i801_smba)
@@ -583,6 +583,40 @@ static struct pci_device_id i801_ids[] = {
 
 MODULE_DEVICE_TABLE (pci, i801_ids);
 
+#if defined CONFIG_INPUT_APANEL || defined CONFIG_INPUT_APANEL_MODULE
+static unsigned char apanel_addr;
+
+/* Scan the system ROM for the signature "FJKEYINF" */
+static __init const void __iomem *bios_signature(const void __iomem *bios)
+{
+	ssize_t offset;
+	const unsigned char signature[] = "FJKEYINF";
+
+	for (offset = 0; offset < 0x10000; offset += 0x10) {
+		if (check_signature(bios + offset, signature,
+				    sizeof(signature)-1))
+			return bios + offset;
+	}
+	return NULL;
+}
+
+static void __init input_apanel_init(void)
+{
+	void __iomem *bios;
+	const void __iomem *p;
+
+	bios = ioremap(0xF0000, 0x10000); /* Can't fail */
+	p = bios_signature(bios);
+	if (p) {
+		/* just use the first address */
+		apanel_addr = readb(p + 8 + 3) >> 1;
+	}
+	iounmap(bios);
+}
+#else
+static void __init input_apanel_init(void) {}
+#endif
+
 static int __devinit i801_probe(struct pci_dev *dev, const struct pci_device_id *id)
 {
 	unsigned char temp;
@@ -667,6 +701,19 @@ static int __devinit i801_probe(struct pci_dev *dev, const struct pci_device_id
 		dev_err(&dev->dev, "Failed to add SMBus adapter\n");
 		goto exit_release;
 	}
+
+	/* Register optional slaves */
+#if defined CONFIG_INPUT_APANEL || defined CONFIG_INPUT_APANEL_MODULE
+	if (apanel_addr) {
+		struct i2c_board_info info;
+
+		memset(&info, 0, sizeof(struct i2c_board_info));
+		info.addr = apanel_addr;
+		strlcpy(info.type, "fujitsu_apanel", I2C_NAME_SIZE);
+		i2c_new_device(&i801_adapter, &info);
+	}
+#endif
+
 	return 0;
 
 exit_release:
@@ -717,6 +764,7 @@ static struct pci_driver i801_driver = {
 
 static int __init i2c_i801_init(void)
 {
+	input_apanel_init();
 	return pci_register_driver(&i801_driver);
 }
 
diff --git a/drivers/i2c/busses/i2c-pxa.c b/drivers/i2c/busses/i2c-pxa.c
index 587f5b2380d4..6af68146c342 100644
--- a/drivers/i2c/busses/i2c-pxa.c
+++ b/drivers/i2c/busses/i2c-pxa.c
@@ -1076,10 +1076,10 @@ static int i2c_pxa_probe(struct platform_device *dev)
 
 #ifdef CONFIG_I2C_PXA_SLAVE
 	printk(KERN_INFO "I2C: %s: PXA I2C adapter, slave address %d\n",
-	       i2c->adap.dev.bus_id, i2c->slave_addr);
+	       dev_name(&i2c->adap.dev), i2c->slave_addr);
 #else
 	printk(KERN_INFO "I2C: %s: PXA I2C adapter\n",
-	       i2c->adap.dev.bus_id);
+	       dev_name(&i2c->adap.dev));
 #endif
 	return 0;
 
diff --git a/drivers/i2c/busses/i2c-s3c2410.c b/drivers/i2c/busses/i2c-s3c2410.c
index f69f91ffb469..5b7f95641ba4 100644
--- a/drivers/i2c/busses/i2c-s3c2410.c
+++ b/drivers/i2c/busses/i2c-s3c2410.c
@@ -906,7 +906,7 @@ static int s3c24xx_i2c_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, i2c);
 
-	dev_info(&pdev->dev, "%s: S3C I2C adapter\n", i2c->adap.dev.bus_id);
+	dev_info(&pdev->dev, "%s: S3C I2C adapter\n", dev_name(&i2c->adap.dev));
 	return 0;
 
  err_cpufreq:
diff --git a/drivers/i2c/busses/i2c-sh7760.c b/drivers/i2c/busses/i2c-sh7760.c
index 5e0e254976de..baa28b73ae42 100644
--- a/drivers/i2c/busses/i2c-sh7760.c
+++ b/drivers/i2c/busses/i2c-sh7760.c
@@ -475,7 +475,7 @@ static int __devinit sh7760_i2c_probe(struct platform_device *pdev)
 
 	id->adap.nr = pdev->id;
 	id->adap.algo = &sh7760_i2c_algo;
-	id->adap.class = I2C_CLASS_ALL;
+	id->adap.class = I2C_CLASS_HWMON | I2C_CLASS_SPD;
 	id->adap.retries = 3;
 	id->adap.algo_data = id;
 	id->adap.dev.parent = &pdev->dev;
diff --git a/drivers/i2c/busses/i2c-sh_mobile.c b/drivers/i2c/busses/i2c-sh_mobile.c
index 6c3d60b939bf..1c01083b01b5 100644
--- a/drivers/i2c/busses/i2c-sh_mobile.c
+++ b/drivers/i2c/busses/i2c-sh_mobile.c
@@ -500,7 +500,7 @@ static int sh_mobile_i2c_hook_irqs(struct platform_device *dev, int hook)
 	while ((res = platform_get_resource(dev, IORESOURCE_IRQ, k))) {
 		for (n = res->start; hook && n <= res->end; n++) {
 			if (request_irq(n, sh_mobile_i2c_isr, IRQF_DISABLED,
-					dev->dev.bus_id, dev))
+					dev_name(&dev->dev), dev))
 				goto rollback;
 		}
 		k++;
diff --git a/drivers/i2c/busses/i2c-sis5595.c b/drivers/i2c/busses/i2c-sis5595.c
index dfc2d5eb6a68..8ce2daff985c 100644
--- a/drivers/i2c/busses/i2c-sis5595.c
+++ b/drivers/i2c/busses/i2c-sis5595.c
@@ -389,8 +389,8 @@ static int __devinit sis5595_probe(struct pci_dev *dev, const struct pci_device_
 	/* set up the sysfs linkage to our parent device */
 	sis5595_adapter.dev.parent = &dev->dev;
 
-	sprintf(sis5595_adapter.name, "SMBus SIS5595 adapter at %04x",
-		sis5595_base + SMB_INDEX);
+	snprintf(sis5595_adapter.name, sizeof(sis5595_adapter.name),
+		 "SMBus SIS5595 adapter at %04x", sis5595_base + SMB_INDEX);
 	err = i2c_add_adapter(&sis5595_adapter);
 	if (err) {
 		release_region(sis5595_base + SMB_INDEX, 2);
diff --git a/drivers/i2c/busses/i2c-sis630.c b/drivers/i2c/busses/i2c-sis630.c
index e7c4b790da54..9c9c016ff2b5 100644
--- a/drivers/i2c/busses/i2c-sis630.c
+++ b/drivers/i2c/busses/i2c-sis630.c
@@ -487,8 +487,8 @@ static int __devinit sis630_probe(struct pci_dev *dev, const struct pci_device_i
 	/* set up the sysfs linkage to our parent device */
 	sis630_adapter.dev.parent = &dev->dev;
 
-	sprintf(sis630_adapter.name, "SMBus SIS630 adapter at %04x",
-		acpi_base + SMB_STS);
+	snprintf(sis630_adapter.name, sizeof(sis630_adapter.name),
+		 "SMBus SIS630 adapter at %04x", acpi_base + SMB_STS);
 
 	return i2c_add_adapter(&sis630_adapter);
 }
diff --git a/drivers/i2c/chips/Kconfig b/drivers/i2c/chips/Kconfig
index 864ac561fdbb..59c3d23f5bdc 100644
--- a/drivers/i2c/chips/Kconfig
+++ b/drivers/i2c/chips/Kconfig
@@ -114,18 +114,6 @@ config SENSORS_PCF8591
 	  These devices are hard to detect and rarely found on mainstream
 	  hardware.  If unsure, say N.
 
-config ISP1301_OMAP
-	tristate "Philips ISP1301 with OMAP OTG"
-	depends on ARCH_OMAP_OTG
-	help
-	  If you say yes here you get support for the Philips ISP1301
-	  USB-On-The-Go transceiver working with the OMAP OTG controller.
-	  The ISP1301 is used in products including H2 and H3 development
-	  boards for Texas Instruments OMAP processors.
-	  
-	  This driver can also be built as a module.  If so, the module
-	  will be called isp1301_omap.
-
 config SENSORS_MAX6875
 	tristate "Maxim MAX6875 Power supply supervisor"
 	depends on EXPERIMENTAL
diff --git a/drivers/i2c/chips/Makefile b/drivers/i2c/chips/Makefile
index 8b95f41a5001..83accaaf8164 100644
--- a/drivers/i2c/chips/Makefile
+++ b/drivers/i2c/chips/Makefile
@@ -18,7 +18,6 @@ obj-$(CONFIG_SENSORS_PCA9539)	+= pca9539.o
 obj-$(CONFIG_SENSORS_PCF8574)	+= pcf8574.o
 obj-$(CONFIG_PCF8575)		+= pcf8575.o
 obj-$(CONFIG_SENSORS_PCF8591)	+= pcf8591.o
-obj-$(CONFIG_ISP1301_OMAP)	+= isp1301_omap.o
 obj-$(CONFIG_SENSORS_TSL2550)	+= tsl2550.o
 obj-$(CONFIG_MCU_MPC8349EMITX)	+= mcu_mpc8349emitx.o
 
diff --git a/drivers/i2c/i2c-core.c b/drivers/i2c/i2c-core.c
index c6a63f46bc15..b1c9abe24c7b 100644
--- a/drivers/i2c/i2c-core.c
+++ b/drivers/i2c/i2c-core.c
@@ -459,7 +459,7 @@ static int i2c_register_adapter(struct i2c_adapter *adap)
 		pr_debug("I2C adapter driver [%s] forgot to specify "
 			 "physical device\n", adap->name);
 	}
-	sprintf(adap->dev.bus_id, "i2c-%d", adap->nr);
+	dev_set_name(&adap->dev, "i2c-%d", adap->nr);
 	adap->dev.release = &i2c_adapter_dev_release;
 	adap->dev.class = &i2c_adapter_class;
 	res = device_register(&adap->dev);
@@ -845,8 +845,8 @@ int i2c_attach_client(struct i2c_client *client)
 	} else
 		client->dev.release = i2c_client_dev_release;
 
-	snprintf(&client->dev.bus_id[0], sizeof(client->dev.bus_id),
-		"%d-%04x", i2c_adapter_id(adapter), client->addr);
+	dev_set_name(&client->dev, "%d-%04x", i2c_adapter_id(adapter),
+		     client->addr);
 	res = device_register(&client->dev);
 	if (res)
 		goto out_err;
@@ -856,7 +856,7 @@ int i2c_attach_client(struct i2c_client *client)
 	mutex_unlock(&adapter->clist_lock);
 
 	dev_dbg(&adapter->dev, "client [%s] registered with bus id %s\n",
-		client->name, client->dev.bus_id);
+		client->name, dev_name(&client->dev));
 
 	if (adapter->client_register)  {
 		if (adapter->client_register(client)) {
diff --git a/drivers/ide/Kconfig b/drivers/ide/Kconfig
index 4ee85fcf9aaf..3f9503867e6b 100644
--- a/drivers/ide/Kconfig
+++ b/drivers/ide/Kconfig
@@ -511,6 +511,13 @@ config BLK_DEV_PIIX
 	  This allows the kernel to change PIO, DMA and UDMA speeds and to
 	  configure the chip to optimum performance.
 
+config BLK_DEV_IT8172
+	tristate "IT8172 IDE support"
+	select BLK_DEV_IDEDMA_PCI
+	help
+	  This driver adds support for the IDE controller on the
+	  IT8172 System Controller.
+
 config BLK_DEV_IT8213
 	tristate "IT8213 IDE support"
 	select BLK_DEV_IDEDMA_PCI
diff --git a/drivers/ide/Makefile b/drivers/ide/Makefile
index 410728992e6a..c2b9c93f0095 100644
--- a/drivers/ide/Makefile
+++ b/drivers/ide/Makefile
@@ -47,6 +47,7 @@ obj-$(CONFIG_BLK_DEV_SC1200)		+= sc1200.o
 obj-$(CONFIG_BLK_DEV_CY82C693)		+= cy82c693.o
 obj-$(CONFIG_BLK_DEV_DELKIN)		+= delkin_cb.o
 obj-$(CONFIG_BLK_DEV_HPT366)		+= hpt366.o
+obj-$(CONFIG_BLK_DEV_IT8172)		+= it8172.o
 obj-$(CONFIG_BLK_DEV_IT8213)		+= it8213.o
 obj-$(CONFIG_BLK_DEV_IT821X)		+= it821x.o
 obj-$(CONFIG_BLK_DEV_JMICRON)		+= jmicron.o
diff --git a/drivers/ide/aec62xx.c b/drivers/ide/aec62xx.c
index 4142c698e0d3..4485b9c6f0e6 100644
--- a/drivers/ide/aec62xx.c
+++ b/drivers/ide/aec62xx.c
@@ -83,7 +83,7 @@ static u8 pci_bus_clock_list_ultra (u8 speed, struct chipset_bus_clock_list_entr
 
 static void aec6210_set_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	struct ide_host *host	= pci_get_drvdata(dev);
 	struct chipset_bus_clock_list_entry *bus_clock = host->host_priv;
@@ -111,7 +111,7 @@ static void aec6210_set_mode(ide_drive_t *drive, const u8 speed)
 
 static void aec6260_set_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	struct ide_host *host	= pci_get_drvdata(dev);
 	struct chipset_bus_clock_list_entry *bus_clock = host->host_priv;
diff --git a/drivers/ide/alim15x3.c b/drivers/ide/alim15x3.c
index 45d2356bb725..66f43083408b 100644
--- a/drivers/ide/alim15x3.c
+++ b/drivers/ide/alim15x3.c
@@ -68,7 +68,7 @@ static struct pci_dev *isa_dev;
 
 static void ali_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
 	struct ide_timing *t = ide_timing_find_mode(XFER_PIO_0 + pio);
 	int s_time = t->setup, a_time = t->active, c_time = t->cycle;
@@ -150,7 +150,7 @@ static u8 ali_udma_filter(ide_drive_t *drive)
 
 static void ali_set_dma_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 speed1		= speed;
 	u8 unit			= drive->dn & 1;
@@ -198,7 +198,7 @@ static void ali_set_dma_mode(ide_drive_t *drive, const u8 speed)
 static int ali15x3_dma_setup(ide_drive_t *drive)
 {
 	if (m5229_revision < 0xC2 && drive->media != ide_disk) {
-		if (rq_data_dir(drive->hwif->hwgroup->rq))
+		if (rq_data_dir(drive->hwif->rq))
 			return 1;	/* try PIO instead of DMA */
 	}
 	return ide_dma_setup(drive);
@@ -490,8 +490,6 @@ static int __devinit init_dma_ali15x3(ide_hwif_t *hwif,
 	if (ide_allocate_dma_engine(hwif))
 		return -1;
 
-	hwif->dma_ops = &sff_dma_ops;
-
 	return 0;
 }
 
@@ -511,6 +509,7 @@ static const struct ide_dma_ops ali_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_port_info ali15x3_chipset __devinitdata = {
@@ -519,6 +518,7 @@ static const struct ide_port_info ali15x3_chipset __devinitdata = {
 	.init_hwif	= init_hwif_ali15x3,
 	.init_dma	= init_dma_ali15x3,
 	.port_ops	= &ali_port_ops,
+	.dma_ops	= &sff_dma_ops,
 	.pio_mask	= ATA_PIO5,
 	.swdma_mask	= ATA_SWDMA2,
 	.mwdma_mask	= ATA_MWDMA2,
diff --git a/drivers/ide/amd74xx.c b/drivers/ide/amd74xx.c
index c6bcd3014a29..69660a431cd9 100644
--- a/drivers/ide/amd74xx.c
+++ b/drivers/ide/amd74xx.c
@@ -82,7 +82,7 @@ static void amd_set_drive(ide_drive_t *drive, const u8 speed)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
-	ide_drive_t *peer = hwif->drives + (~drive->dn & 1);
+	ide_drive_t *peer = ide_get_pair_dev(drive);
 	struct ide_timing t, p;
 	int T, UT;
 	u8 udma_mask = hwif->ultra_mask;
@@ -92,7 +92,7 @@ static void amd_set_drive(ide_drive_t *drive, const u8 speed)
 
 	ide_timing_compute(drive, speed, &t, T, UT);
 
-	if (peer->dev_flags & IDE_DFLAG_PRESENT) {
+	if (peer) {
 		ide_timing_compute(peer, peer->current_speed, &p, T, UT);
 		ide_timing_merge(&p, &t, &t, IDE_TIMING_8BIT);
 	}
diff --git a/drivers/ide/au1xxx-ide.c b/drivers/ide/au1xxx-ide.c
index 0ec8fd1e4dcb..79a2dfed8eb7 100644
--- a/drivers/ide/au1xxx-ide.c
+++ b/drivers/ide/au1xxx-ide.c
@@ -212,8 +212,8 @@ static void auide_set_dma_mode(ide_drive_t *drive, const u8 speed)
 static int auide_build_dmatable(ide_drive_t *drive)
 {
 	int i, iswrite, count = 0;
-	ide_hwif_t *hwif = HWIF(drive);
-	struct request *rq = HWGROUP(drive)->rq;
+	ide_hwif_t *hwif = drive->hwif;
+	struct request *rq = hwif->rq;
 	_auide_hwif *ahwif = &auide_hwif;
 	struct scatterlist *sg;
 
@@ -286,7 +286,7 @@ static int auide_build_dmatable(ide_drive_t *drive)
 
 static int auide_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 
 	if (hwif->sg_nents) {
 		ide_destroy_dmatable(drive);
@@ -309,8 +309,8 @@ static void auide_dma_exec_cmd(ide_drive_t *drive, u8 command)
 }
 
 static int auide_dma_setup(ide_drive_t *drive)
-{       	
-	struct request *rq = HWGROUP(drive)->rq;
+{
+	struct request *rq = drive->hwif->rq;
 
 	if (!auide_build_dmatable(drive)) {
 		ide_map_sg(drive, rq);
@@ -502,7 +502,6 @@ static const struct ide_tp_ops au1xxx_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
diff --git a/drivers/ide/cmd640.c b/drivers/ide/cmd640.c
index e4306647d00d..8890276fef7f 100644
--- a/drivers/ide/cmd640.c
+++ b/drivers/ide/cmd640.c
@@ -467,11 +467,10 @@ static void program_drive_counts(ide_drive_t *drive, unsigned int index)
 	 * so we merge the timings, using the slowest value for each timing.
 	 */
 	if (index > 1) {
-		ide_hwif_t *hwif = drive->hwif;
-		ide_drive_t *peer = &hwif->drives[!(drive->dn & 1)];
+		ide_drive_t *peer = ide_get_pair_dev(drive);
 		unsigned int mate = index ^ 1;
 
-		if (peer->dev_flags & IDE_DFLAG_PRESENT) {
+		if (peer) {
 			if (setup_count < setup_counts[mate])
 				setup_count = setup_counts[mate];
 			if (active_count < active_counts[mate])
diff --git a/drivers/ide/cmd64x.c b/drivers/ide/cmd64x.c
index 3623bf013bcf..2f9688d87ecd 100644
--- a/drivers/ide/cmd64x.c
+++ b/drivers/ide/cmd64x.c
@@ -115,7 +115,7 @@ static void program_cycle_times (ide_drive_t *drive, int cycle_time, int active_
  */
 static void cmd64x_tune_pio(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	struct ide_timing *t	= ide_timing_find_mode(XFER_PIO_0 + pio);
 	unsigned int cycle_time;
@@ -138,10 +138,12 @@ static void cmd64x_tune_pio(ide_drive_t *drive, const u8 pio)
 	 * the slowest address setup timing ourselves.
 	 */
 	if (hwif->channel) {
-		ide_drive_t *drives = hwif->drives;
+		ide_drive_t *pair = ide_get_pair_dev(drive);
 
 		drive->drive_data = setup_count;
-		setup_count = max(drives[0].drive_data, drives[1].drive_data);
+
+		if (pair)
+			setup_count = max_t(u8, setup_count, pair->drive_data);
 	}
 
 	if (setup_count > 5)		/* shouldn't actually happen... */
@@ -180,7 +182,7 @@ static void cmd64x_set_pio_mode(ide_drive_t *drive, const u8 pio)
 
 static void cmd64x_set_dma_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 unit			= drive->dn & 0x01;
 	u8 regU = 0, pciU	= hwif->channel ? UDIDETCR1 : UDIDETCR0;
@@ -226,7 +228,7 @@ static void cmd64x_set_dma_mode(ide_drive_t *drive, const u8 speed)
 
 static int cmd648_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	unsigned long base	= hwif->dma_base - (hwif->channel * 8);
 	int err			= ide_dma_end(drive);
 	u8  irq_mask		= hwif->channel ? MRDMODE_INTR_CH1 :
@@ -242,7 +244,7 @@ static int cmd648_dma_end(ide_drive_t *drive)
 
 static int cmd64x_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	int irq_reg		= hwif->channel ? ARTTIM23 : CFR;
 	u8  irq_mask		= hwif->channel ? ARTTIM23_INTR_CH1 :
@@ -259,7 +261,7 @@ static int cmd64x_dma_end(ide_drive_t *drive)
 
 static int cmd648_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	unsigned long base	= hwif->dma_base - (hwif->channel * 8);
 	u8 irq_mask		= hwif->channel ? MRDMODE_INTR_CH1 :
 						  MRDMODE_INTR_CH0;
@@ -282,7 +284,7 @@ static int cmd648_dma_test_irq(ide_drive_t *drive)
 
 static int cmd64x_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	int irq_reg		= hwif->channel ? ARTTIM23 : CFR;
 	u8  irq_mask		= hwif->channel ? ARTTIM23_INTR_CH1 :
@@ -313,7 +315,7 @@ static int cmd64x_dma_test_irq(ide_drive_t *drive)
 
 static int cmd646_1_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	u8 dma_stat = 0, dma_cmd = 0;
 
 	drive->waiting_for_dma = 0;
@@ -383,6 +385,7 @@ static const struct ide_dma_ops cmd64x_dma_ops = {
 	.dma_test_irq		= cmd64x_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_dma_ops cmd646_rev1_dma_ops = {
@@ -394,6 +397,7 @@ static const struct ide_dma_ops cmd646_rev1_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_dma_ops cmd648_dma_ops = {
@@ -405,6 +409,7 @@ static const struct ide_dma_ops cmd648_dma_ops = {
 	.dma_test_irq		= cmd648_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_port_info cmd64x_chipsets[] __devinitdata = {
diff --git a/drivers/ide/cs5520.c b/drivers/ide/cs5520.c
index 5efb467f8fa0..d003bec56ff9 100644
--- a/drivers/ide/cs5520.c
+++ b/drivers/ide/cs5520.c
@@ -59,7 +59,7 @@ static struct pio_clocks cs5520_pio_clocks[]={
 
 static void cs5520_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *pdev = to_pci_dev(hwif->dev);
 	int controller = drive->dn > 1 ? 1 : 0;
 
diff --git a/drivers/ide/cy82c693.c b/drivers/ide/cy82c693.c
index d37baf8ecc5f..74fc5401f407 100644
--- a/drivers/ide/cy82c693.c
+++ b/drivers/ide/cy82c693.c
@@ -203,7 +203,7 @@ static void cy82c693_set_dma_mode(ide_drive_t *drive, const u8 mode)
 
 static void cy82c693_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
 	pio_clocks_t pclk;
 	unsigned int addrCtrl;
diff --git a/drivers/ide/falconide.c b/drivers/ide/falconide.c
index 39d500d84b07..a5ba820d69bb 100644
--- a/drivers/ide/falconide.c
+++ b/drivers/ide/falconide.c
@@ -70,7 +70,6 @@ static const struct ide_tp_ops falconide_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
diff --git a/drivers/ide/hpt366.c b/drivers/ide/hpt366.c
index b18e10d99d2e..3eb9b5c63a0f 100644
--- a/drivers/ide/hpt366.c
+++ b/drivers/ide/hpt366.c
@@ -626,7 +626,7 @@ static struct hpt_info *hpt3xx_get_info(struct device *dev)
 
 static u8 hpt3xx_udma_filter(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct hpt_info *info	= hpt3xx_get_info(hwif->dev);
 	u8 mask 		= hwif->ultra_mask;
 
@@ -665,7 +665,7 @@ static u8 hpt3xx_udma_filter(ide_drive_t *drive)
 
 static u8 hpt3xx_mdma_filter(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct hpt_info *info	= hpt3xx_get_info(hwif->dev);
 
 	switch (info->chip_type) {
@@ -743,7 +743,7 @@ static void hpt3xx_quirkproc(ide_drive_t *drive)
 
 static void hpt3xx_maskproc(ide_drive_t *drive, int mask)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev	*dev	= to_pci_dev(hwif->dev);
 	struct hpt_info *info	= hpt3xx_get_info(hwif->dev);
 
@@ -788,7 +788,7 @@ static void hpt366_dma_lost_irq(ide_drive_t *drive)
 
 static void hpt370_clear_engine(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
 
 	pci_write_config_byte(dev, hwif->select_data, 0x37);
@@ -797,7 +797,7 @@ static void hpt370_clear_engine(ide_drive_t *drive)
 
 static void hpt370_irq_timeout(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u16 bfifo		= 0;
 	u8  dma_cmd;
@@ -822,7 +822,7 @@ static void hpt370_dma_start(ide_drive_t *drive)
 
 static int hpt370_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	u8  dma_stat		= inb(hwif->dma_base + ATA_DMA_STATUS);
 
 	if (dma_stat & 0x01) {
@@ -844,7 +844,7 @@ static void hpt370_dma_timeout(ide_drive_t *drive)
 /* returns 1 if DMA IRQ issued, 0 otherwise */
 static int hpt374_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u16 bfifo		= 0;
 	u8  dma_stat;
@@ -865,7 +865,7 @@ static int hpt374_dma_test_irq(ide_drive_t *drive)
 
 static int hpt374_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 mcr	= 0, mcr_addr	= hwif->select_data;
 	u8 bwsr = 0, mask	= hwif->channel ? 0x02 : 0x01;
@@ -927,7 +927,7 @@ static void hpt3xxn_set_clock(ide_hwif_t *hwif, u8 mode)
 
 static void hpt3xxn_rw_disk(ide_drive_t *drive, struct request *rq)
 {
-	hpt3xxn_set_clock(HWIF(drive), rq_data_dir(rq) ? 0x23 : 0x21);
+	hpt3xxn_set_clock(drive->hwif, rq_data_dir(rq) ? 0x23 : 0x21);
 }
 
 /**
@@ -1349,8 +1349,6 @@ static int __devinit init_dma_hpt366(ide_hwif_t *hwif,
 	if (ide_allocate_dma_engine(hwif))
 		return -1;
 
-	hwif->dma_ops = &sff_dma_ops;
-
 	return 0;
 }
 
@@ -1426,6 +1424,7 @@ static const struct ide_dma_ops hpt37x_dma_ops = {
 	.dma_test_irq		= hpt374_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_dma_ops hpt370_dma_ops = {
@@ -1437,6 +1436,7 @@ static const struct ide_dma_ops hpt370_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= hpt370_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_dma_ops hpt36x_dma_ops = {
@@ -1448,6 +1448,7 @@ static const struct ide_dma_ops hpt36x_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= hpt366_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_port_info hpt366_chipsets[] __devinitdata = {
diff --git a/drivers/ide/icside.c b/drivers/ide/icside.c
index 81f70caeb40f..97a35c667aee 100644
--- a/drivers/ide/icside.c
+++ b/drivers/ide/icside.c
@@ -166,7 +166,7 @@ static const expansioncard_ops_t icside_ops_arcin_v6 = {
  */
 static void icside_maskproc(ide_drive_t *drive, int mask)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct expansion_card *ec = ECARD_DEV(hwif->dev);
 	struct icside_state *state = ecard_get_drvdata(ec);
 	unsigned long flags;
@@ -284,7 +284,7 @@ static void icside_dma_host_set(ide_drive_t *drive, int on)
 
 static int icside_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct expansion_card *ec = ECARD_DEV(hwif->dev);
 
 	drive->waiting_for_dma = 0;
@@ -299,7 +299,7 @@ static int icside_dma_end(ide_drive_t *drive)
 
 static void icside_dma_start(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct expansion_card *ec = ECARD_DEV(hwif->dev);
 
 	/* We can not enable DMA on both channels simultaneously. */
@@ -309,10 +309,10 @@ static void icside_dma_start(ide_drive_t *drive)
 
 static int icside_dma_setup(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct expansion_card *ec = ECARD_DEV(hwif->dev);
 	struct icside_state *state = ecard_get_drvdata(ec);
-	struct request *rq = hwif->hwgroup->rq;
+	struct request *rq = hwif->rq;
 	unsigned int dma_mode;
 
 	if (rq_data_dir(rq))
@@ -362,7 +362,7 @@ static void icside_dma_exec_cmd(ide_drive_t *drive, u8 cmd)
 
 static int icside_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct expansion_card *ec = ECARD_DEV(hwif->dev);
 	struct icside_state *state = ecard_get_drvdata(ec);
 
diff --git a/drivers/ide/ide-acpi.c b/drivers/ide/ide-acpi.c
index fd4a36433050..2f9e941968d6 100644
--- a/drivers/ide/ide-acpi.c
+++ b/drivers/ide/ide-acpi.c
@@ -218,7 +218,7 @@ static acpi_handle ide_acpi_hwif_get_handle(ide_hwif_t *hwif)
  */
 static acpi_handle ide_acpi_drive_get_handle(ide_drive_t *drive)
 {
-	ide_hwif_t	*hwif = HWIF(drive);
+	ide_hwif_t	*hwif = drive->hwif;
 	int		 port;
 	acpi_handle	 drive_handle;
 
@@ -263,7 +263,7 @@ static int do_drive_get_GTF(ide_drive_t *drive,
 	acpi_status			status;
 	struct acpi_buffer		output;
 	union acpi_object 		*out_obj;
-	ide_hwif_t			*hwif = HWIF(drive);
+	ide_hwif_t			*hwif = drive->hwif;
 	struct device			*dev = hwif->gendev.parent;
 	int				err = -ENODEV;
 	int				port;
@@ -641,7 +641,8 @@ void ide_acpi_push_timing(ide_hwif_t *hwif)
  */
 void ide_acpi_set_state(ide_hwif_t *hwif, int on)
 {
-	int unit;
+	ide_drive_t *drive;
+	int i;
 
 	if (ide_noacpi || ide_noacpi_psx)
 		return;
@@ -655,9 +656,8 @@ void ide_acpi_set_state(ide_hwif_t *hwif, int on)
 	/* channel first and then drives for power on and verse versa for power off */
 	if (on)
 		acpi_bus_set_power(hwif->acpidata->obj_handle, ACPI_STATE_D0);
-	for (unit = 0; unit < MAX_DRIVES; ++unit) {
-		ide_drive_t *drive = &hwif->drives[unit];
 
+	ide_port_for_each_dev(i, drive, hwif) {
 		if (!drive->acpidata->obj_handle)
 			drive->acpidata->obj_handle = ide_acpi_drive_get_handle(drive);
 
@@ -711,15 +711,13 @@ void ide_acpi_port_init_devices(ide_hwif_t *hwif)
 	 * for both drives, regardless whether they are connected
 	 * or not.
 	 */
-	hwif->drives[0].acpidata = &hwif->acpidata->master;
-	hwif->drives[1].acpidata = &hwif->acpidata->slave;
+	hwif->devices[0]->acpidata = &hwif->acpidata->master;
+	hwif->devices[1]->acpidata = &hwif->acpidata->slave;
 
 	/*
 	 * Send IDENTIFY for each drive
 	 */
-	for (i = 0; i < MAX_DRIVES; i++) {
-		drive = &hwif->drives[i];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		memset(drive->acpidata, 0, sizeof(*drive->acpidata));
 
 		if ((drive->dev_flags & IDE_DFLAG_PRESENT) == 0)
@@ -744,9 +742,7 @@ void ide_acpi_port_init_devices(ide_hwif_t *hwif)
 	ide_acpi_get_timing(hwif);
 	ide_acpi_push_timing(hwif);
 
-	for (i = 0; i < MAX_DRIVES; i++) {
-		drive = &hwif->drives[i];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		if (drive->dev_flags & IDE_DFLAG_PRESENT)
 			/* Execute ACPI startup code */
 			ide_acpi_exec_tfs(drive);
diff --git a/drivers/ide/ide-atapi.c b/drivers/ide/ide-atapi.c
index e8688c0f8645..e96c01260598 100644
--- a/drivers/ide/ide-atapi.c
+++ b/drivers/ide/ide-atapi.c
@@ -243,7 +243,7 @@ EXPORT_SYMBOL_GPL(ide_retry_pc);
 
 int ide_cd_expiry(ide_drive_t *drive)
 {
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = drive->hwif->rq;
 	unsigned long wait = 0;
 
 	debug_log("%s: rq->cmd[0]: 0x%x\n", __func__, rq->cmd[0]);
@@ -294,7 +294,7 @@ static ide_startstop_t ide_pc_intr(ide_drive_t *drive)
 {
 	struct ide_atapi_pc *pc = drive->pc;
 	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = hwif->hwgroup->rq;
+	struct request *rq = hwif->rq;
 	const struct ide_tp_ops *tp_ops = hwif->tp_ops;
 	xfer_func_t *xferfunc;
 	unsigned int timeout, temp;
@@ -491,7 +491,7 @@ static ide_startstop_t ide_transfer_pc(ide_drive_t *drive)
 {
 	struct ide_atapi_pc *uninitialized_var(pc);
 	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = hwif->hwgroup->rq;
+	struct request *rq = hwif->rq;
 	ide_expiry_t *expiry;
 	unsigned int timeout;
 	int cmd_len;
@@ -549,7 +549,10 @@ static ide_startstop_t ide_transfer_pc(ide_drive_t *drive)
 	}
 
 	/* Set the interrupt routine */
-	ide_set_handler(drive, ide_pc_intr, timeout, expiry);
+	ide_set_handler(drive,
+			(dev_is_idecd(drive) ? drive->irq_handler
+					     : ide_pc_intr),
+			timeout, expiry);
 
 	/* Begin DMA, if necessary */
 	if (dev_is_idecd(drive)) {
@@ -580,7 +583,7 @@ ide_startstop_t ide_issue_pc(ide_drive_t *drive)
 
 	if (dev_is_idecd(drive)) {
 		tf_flags = IDE_TFLAG_OUT_NSECT | IDE_TFLAG_OUT_LBAL;
-		bcount = ide_cd_get_xferlen(hwif->hwgroup->rq);
+		bcount = ide_cd_get_xferlen(hwif->rq);
 		expiry = ide_cd_expiry;
 		timeout = ATAPI_WAIT_PC;
 
diff --git a/drivers/ide/ide-cd.c b/drivers/ide/ide-cd.c
index 1a7410f88249..cae69372cf45 100644
--- a/drivers/ide/ide-cd.c
+++ b/drivers/ide/ide-cd.c
@@ -239,7 +239,7 @@ static void cdrom_queue_request_sense(ide_drive_t *drive, void *sense,
 
 static void cdrom_end_request(ide_drive_t *drive, int uptodate)
 {
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = drive->hwif->rq;
 	int nsectors = rq->hard_cur_sectors;
 
 	ide_debug_log(IDE_DBG_FUNC, "Call %s, cmd: 0x%x, uptodate: 0x%x, "
@@ -306,8 +306,7 @@ static void ide_dump_status_no_sense(ide_drive_t *drive, const char *msg, u8 st)
 static int cdrom_decode_status(ide_drive_t *drive, int good_stat, int *stat_ret)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
-	struct request *rq = hwgroup->rq;
+	struct request *rq = hwif->rq;
 	int stat, err, sense_key;
 
 	/* check for errors */
@@ -502,7 +501,7 @@ end_request:
 		blkdev_dequeue_request(rq);
 		spin_unlock_irqrestore(q->queue_lock, flags);
 
-		hwgroup->rq = NULL;
+		hwif->rq = NULL;
 
 		cdrom_queue_request_sense(drive, rq->sense, rq);
 	} else
@@ -511,106 +510,6 @@ end_request:
 	return 1;
 }
 
-static ide_startstop_t cdrom_transfer_packet_command(ide_drive_t *);
-static ide_startstop_t cdrom_newpc_intr(ide_drive_t *);
-
-/*
- * Set up the device registers for transferring a packet command on DEV,
- * expecting to later transfer XFERLEN bytes.  HANDLER is the routine
- * which actually transfers the command to the drive.  If this is a
- * drq_interrupt device, this routine will arrange for HANDLER to be
- * called when the interrupt from the drive arrives.  Otherwise, HANDLER
- * will be called immediately after the drive is prepared for the transfer.
- */
-static ide_startstop_t cdrom_start_packet_command(ide_drive_t *drive)
-{
-	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = hwif->hwgroup->rq;
-	int xferlen;
-
-	xferlen = ide_cd_get_xferlen(rq);
-
-	ide_debug_log(IDE_DBG_PC, "Call %s, xferlen: %d\n", __func__, xferlen);
-
-	/* FIXME: for Virtual DMA we must check harder */
-	if (drive->dma)
-		drive->dma = !hwif->dma_ops->dma_setup(drive);
-
-	/* set up the controller registers */
-	ide_pktcmd_tf_load(drive, IDE_TFLAG_OUT_NSECT | IDE_TFLAG_OUT_LBAL,
-			   xferlen, drive->dma);
-
-	if (drive->atapi_flags & IDE_AFLAG_DRQ_INTERRUPT) {
-		/* waiting for CDB interrupt, not DMA yet. */
-		if (drive->dma)
-			drive->waiting_for_dma = 0;
-
-		/* packet command */
-		ide_execute_command(drive, ATA_CMD_PACKET,
-				    cdrom_transfer_packet_command,
-				    ATAPI_WAIT_PC, ide_cd_expiry);
-		return ide_started;
-	} else {
-		ide_execute_pkt_cmd(drive);
-
-		return cdrom_transfer_packet_command(drive);
-	}
-}
-
-/*
- * Send a packet command to DRIVE described by CMD_BUF and CMD_LEN. The device
- * registers must have already been prepared by cdrom_start_packet_command.
- * HANDLER is the interrupt handler to call when the command completes or
- * there's data ready.
- */
-#define ATAPI_MIN_CDB_BYTES 12
-static ide_startstop_t cdrom_transfer_packet_command(ide_drive_t *drive)
-{
-	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = hwif->hwgroup->rq;
-	int cmd_len;
-	ide_startstop_t startstop;
-
-	ide_debug_log(IDE_DBG_PC, "Call %s\n", __func__);
-
-	if (drive->atapi_flags & IDE_AFLAG_DRQ_INTERRUPT) {
-		/*
-		 * Here we should have been called after receiving an interrupt
-		 * from the device.  DRQ should how be set.
-		 */
-
-		/* check for errors */
-		if (cdrom_decode_status(drive, ATA_DRQ, NULL))
-			return ide_stopped;
-
-		/* ok, next interrupt will be DMA interrupt */
-		if (drive->dma)
-			drive->waiting_for_dma = 1;
-	} else {
-		/* otherwise, we must wait for DRQ to get set */
-		if (ide_wait_stat(&startstop, drive, ATA_DRQ,
-				  ATA_BUSY, WAIT_READY))
-			return startstop;
-	}
-
-	/* arm the interrupt handler */
-	ide_set_handler(drive, cdrom_newpc_intr, rq->timeout, ide_cd_expiry);
-
-	/* ATAPI commands get padded out to 12 bytes minimum */
-	cmd_len = COMMAND_SIZE(rq->cmd[0]);
-	if (cmd_len < ATAPI_MIN_CDB_BYTES)
-		cmd_len = ATAPI_MIN_CDB_BYTES;
-
-	/* send the command to the device */
-	hwif->tp_ops->output_data(drive, NULL, rq->cmd, cmd_len);
-
-	/* start the DMA if need be */
-	if (drive->dma)
-		hwif->dma_ops->dma_start(drive);
-
-	return ide_started;
-}
-
 /*
  * Check the contents of the interrupt reason register from the cdrom
  * and attempt to recover if there are problems.  Returns  0 if everything's
@@ -854,8 +753,7 @@ static int cdrom_newpc_intr_dummy_cb(struct request *rq)
 static ide_startstop_t cdrom_newpc_intr(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
-	struct request *rq = hwgroup->rq;
+	struct request *rq = hwif->rq;
 	xfer_func_t *xferfunc;
 	ide_expiry_t *expiry = NULL;
 	int dma_error = 0, dma, stat, thislen, uptodate = 0;
@@ -1061,7 +959,7 @@ end_request:
 		if (blk_end_request(rq, 0, dlen))
 			BUG();
 
-		hwgroup->rq = NULL;
+		hwif->rq = NULL;
 	} else {
 		if (!uptodate)
 			rq->cmd_flags |= REQ_FAILED;
@@ -1183,7 +1081,7 @@ static ide_startstop_t ide_cd_do_request(ide_drive_t *drive, struct request *rq,
 		return ide_stopped;
 	}
 
-	return cdrom_start_packet_command(drive);
+	return ide_issue_pc(drive);
 }
 
 /*
@@ -1916,7 +1814,7 @@ static void ide_cd_release(struct kref *kref)
 
 static int ide_cd_probe(ide_drive_t *);
 
-static ide_driver_t ide_cdrom_driver = {
+static struct ide_driver ide_cdrom_driver = {
 	.gen_driver = {
 		.owner		= THIS_MODULE,
 		.name		= "ide-cdrom",
@@ -1927,7 +1825,6 @@ static ide_driver_t ide_cdrom_driver = {
 	.version		= IDECD_VERSION,
 	.do_request		= ide_cd_do_request,
 	.end_request		= ide_end_request,
-	.error			= __ide_error,
 #ifdef CONFIG_IDE_PROC_FS
 	.proc_entries		= ide_cd_proc_entries,
 	.proc_devsets		= ide_cd_proc_devsets,
@@ -2082,6 +1979,7 @@ static int ide_cd_probe(ide_drive_t *drive)
 	}
 
 	drive->debug_mask = debug_mask;
+	drive->irq_handler = cdrom_newpc_intr;
 
 	info = kzalloc(sizeof(struct cdrom_info), GFP_KERNEL);
 	if (info == NULL) {
diff --git a/drivers/ide/ide-cd.h b/drivers/ide/ide-cd.h
index bf676b262181..ac40d6cb90a2 100644
--- a/drivers/ide/ide-cd.h
+++ b/drivers/ide/ide-cd.h
@@ -33,33 +33,33 @@
 
 /* Structure of a MSF cdrom address. */
 struct atapi_msf {
-	byte reserved;
-	byte minute;
-	byte second;
-	byte frame;
+	u8 reserved;
+	u8 minute;
+	u8 second;
+	u8 frame;
 };
 
 /* Space to hold the disk TOC. */
 #define MAX_TRACKS 99
 struct atapi_toc_header {
 	unsigned short toc_length;
-	byte first_track;
-	byte last_track;
+	u8 first_track;
+	u8 last_track;
 };
 
 struct atapi_toc_entry {
-	byte reserved1;
+	u8 reserved1;
 #if defined(__BIG_ENDIAN_BITFIELD)
-	__u8 adr     : 4;
-	__u8 control : 4;
+	u8 adr     : 4;
+	u8 control : 4;
 #elif defined(__LITTLE_ENDIAN_BITFIELD)
-	__u8 control : 4;
-	__u8 adr     : 4;
+	u8 control : 4;
+	u8 adr     : 4;
 #else
 #error "Please fix <asm/byteorder.h>"
 #endif
-	byte track;
-	byte reserved2;
+	u8 track;
+	u8 reserved2;
 	union {
 		unsigned lba;
 		struct atapi_msf msf;
@@ -77,10 +77,10 @@ struct atapi_toc {
 
 /* Extra per-device info for cdrom drives. */
 struct cdrom_info {
-	ide_drive_t	*drive;
-	ide_driver_t	*driver;
-	struct gendisk	*disk;
-	struct kref	kref;
+	ide_drive_t		*drive;
+	struct ide_driver	*driver;
+	struct gendisk		*disk;
+	struct kref		kref;
 
 	/* Buffer for table of contents.  NULL if we haven't allocated
 	   a TOC buffer for this device yet. */
diff --git a/drivers/ide/ide-disk.c b/drivers/ide/ide-disk.c
index eb9fac4d0f0c..4088a622873e 100644
--- a/drivers/ide/ide-disk.c
+++ b/drivers/ide/ide-disk.c
@@ -89,7 +89,7 @@ static void ide_tf_set_cmd(ide_drive_t *drive, ide_task_t *task, u8 dma)
 static ide_startstop_t __ide_do_rw_disk(ide_drive_t *drive, struct request *rq,
 					sector_t block)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	u16 nsectors		= (u16)rq->nr_sectors;
 	u8 lba48		= !!(drive->dev_flags & IDE_DFLAG_LBA48);
 	u8 dma			= !!(drive->dev_flags & IDE_DFLAG_USING_DMA);
@@ -187,7 +187,7 @@ static ide_startstop_t __ide_do_rw_disk(ide_drive_t *drive, struct request *rq,
 static ide_startstop_t ide_do_rw_disk(ide_drive_t *drive, struct request *rq,
 				      sector_t block)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 
 	BUG_ON(drive->dev_flags & IDE_DFLAG_BLOCKED);
 
diff --git a/drivers/ide/ide-dma-sff.c b/drivers/ide/ide-dma-sff.c
index f6d2d44d8a9a..123d393658af 100644
--- a/drivers/ide/ide-dma-sff.c
+++ b/drivers/ide/ide-dma-sff.c
@@ -50,6 +50,27 @@ int config_drive_for_dma(ide_drive_t *drive)
 	return 0;
 }
 
+u8 ide_dma_sff_read_status(ide_hwif_t *hwif)
+{
+	unsigned long addr = hwif->dma_base + ATA_DMA_STATUS;
+
+	if (hwif->host_flags & IDE_HFLAG_MMIO)
+		return readb((void __iomem *)addr);
+	else
+		return inb(addr);
+}
+EXPORT_SYMBOL_GPL(ide_dma_sff_read_status);
+
+static void ide_dma_sff_write_status(ide_hwif_t *hwif, u8 val)
+{
+	unsigned long addr = hwif->dma_base + ATA_DMA_STATUS;
+
+	if (hwif->host_flags & IDE_HFLAG_MMIO)
+		writeb(val, (void __iomem *)addr);
+	else
+		outb(val, addr);
+}
+
 /**
  *	ide_dma_host_set	-	Enable/disable DMA on a host
  *	@drive: drive to control
@@ -62,18 +83,14 @@ void ide_dma_host_set(ide_drive_t *drive, int on)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	u8 unit = drive->dn & 1;
-	u8 dma_stat = hwif->tp_ops->read_sff_dma_status(hwif);
+	u8 dma_stat = hwif->dma_ops->dma_sff_read_status(hwif);
 
 	if (on)
 		dma_stat |= (1 << (5 + unit));
 	else
 		dma_stat &= ~(1 << (5 + unit));
 
-	if (hwif->host_flags & IDE_HFLAG_MMIO)
-		writeb(dma_stat,
-		       (void __iomem *)(hwif->dma_base + ATA_DMA_STATUS));
-	else
-		outb(dma_stat, hwif->dma_base + ATA_DMA_STATUS);
+	ide_dma_sff_write_status(hwif, dma_stat);
 }
 EXPORT_SYMBOL_GPL(ide_dma_host_set);
 
@@ -175,7 +192,7 @@ EXPORT_SYMBOL_GPL(ide_build_dmatable);
 int ide_dma_setup(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = hwif->hwgroup->rq;
+	struct request *rq = hwif->rq;
 	unsigned int reading = rq_data_dir(rq) ? 0 : ATA_DMA_WR;
 	u8 mmio = (hwif->host_flags & IDE_HFLAG_MMIO) ? 1 : 0;
 	u8 dma_stat;
@@ -187,7 +204,7 @@ int ide_dma_setup(ide_drive_t *drive)
 	}
 
 	/* PRD table */
-	if (hwif->host_flags & IDE_HFLAG_MMIO)
+	if (mmio)
 		writel(hwif->dmatable_dma,
 		       (void __iomem *)(hwif->dma_base + ATA_DMA_TABLE_OFS));
 	else
@@ -200,15 +217,10 @@ int ide_dma_setup(ide_drive_t *drive)
 		outb(reading, hwif->dma_base + ATA_DMA_CMD);
 
 	/* read DMA status for INTR & ERROR flags */
-	dma_stat = hwif->tp_ops->read_sff_dma_status(hwif);
+	dma_stat = hwif->dma_ops->dma_sff_read_status(hwif);
 
 	/* clear INTR & ERROR flags */
-	if (mmio)
-		writeb(dma_stat | ATA_DMA_ERR | ATA_DMA_INTR,
-		       (void __iomem *)(hwif->dma_base + ATA_DMA_STATUS));
-	else
-		outb(dma_stat | ATA_DMA_ERR | ATA_DMA_INTR,
-		     hwif->dma_base + ATA_DMA_STATUS);
+	ide_dma_sff_write_status(hwif, dma_stat | ATA_DMA_ERR | ATA_DMA_INTR);
 
 	drive->waiting_for_dma = 1;
 	return 0;
@@ -232,7 +244,7 @@ EXPORT_SYMBOL_GPL(ide_dma_setup);
 static int dma_timer_expiry(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	u8 dma_stat = hwif->tp_ops->read_sff_dma_status(hwif);
+	u8 dma_stat = hwif->dma_ops->dma_sff_read_status(hwif);
 
 	printk(KERN_WARNING "%s: %s: DMA status (0x%02x)\n",
 		drive->name, __func__, dma_stat);
@@ -240,7 +252,7 @@ static int dma_timer_expiry(ide_drive_t *drive)
 	if ((dma_stat & 0x18) == 0x18)	/* BUSY Stupid Early Timer !! */
 		return WAIT_CMD;
 
-	hwif->hwgroup->expiry = NULL;	/* one free ride for now */
+	hwif->expiry = NULL;	/* one free ride for now */
 
 	if (dma_stat & ATA_DMA_ERR)	/* ERROR */
 		return -1;
@@ -289,13 +301,12 @@ EXPORT_SYMBOL_GPL(ide_dma_start);
 int ide_dma_end(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	u8 mmio = (hwif->host_flags & IDE_HFLAG_MMIO) ? 1 : 0;
 	u8 dma_stat = 0, dma_cmd = 0, mask;
 
 	drive->waiting_for_dma = 0;
 
 	/* stop DMA */
-	if (mmio) {
+	if (hwif->host_flags & IDE_HFLAG_MMIO) {
 		dma_cmd = readb((void __iomem *)(hwif->dma_base + ATA_DMA_CMD));
 		writeb(dma_cmd & ~ATA_DMA_START,
 		       (void __iomem *)(hwif->dma_base + ATA_DMA_CMD));
@@ -305,15 +316,10 @@ int ide_dma_end(ide_drive_t *drive)
 	}
 
 	/* get DMA status */
-	dma_stat = hwif->tp_ops->read_sff_dma_status(hwif);
+	dma_stat = hwif->dma_ops->dma_sff_read_status(hwif);
 
-	if (mmio)
-		/* clear the INTR & ERROR bits */
-		writeb(dma_stat | ATA_DMA_ERR | ATA_DMA_INTR,
-		       (void __iomem *)(hwif->dma_base + ATA_DMA_STATUS));
-	else
-		outb(dma_stat | ATA_DMA_ERR | ATA_DMA_INTR,
-		     hwif->dma_base + ATA_DMA_STATUS);
+	/* clear INTR & ERROR bits */
+	ide_dma_sff_write_status(hwif, dma_stat | ATA_DMA_ERR | ATA_DMA_INTR);
 
 	/* purge DMA mappings */
 	ide_destroy_dmatable(drive);
@@ -331,7 +337,7 @@ EXPORT_SYMBOL_GPL(ide_dma_end);
 int ide_dma_test_irq(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	u8 dma_stat = hwif->tp_ops->read_sff_dma_status(hwif);
+	u8 dma_stat = hwif->dma_ops->dma_sff_read_status(hwif);
 
 	return (dma_stat & ATA_DMA_INTR) ? 1 : 0;
 }
@@ -346,5 +352,6 @@ const struct ide_dma_ops sff_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_timeout		= ide_dma_timeout,
 	.dma_lost_irq		= ide_dma_lost_irq,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 EXPORT_SYMBOL_GPL(sff_dma_ops);
diff --git a/drivers/ide/ide-dma.c b/drivers/ide/ide-dma.c
index fffd11717b2d..72ebab0bc755 100644
--- a/drivers/ide/ide-dma.c
+++ b/drivers/ide/ide-dma.c
@@ -96,7 +96,7 @@ ide_startstop_t ide_dma_intr(ide_drive_t *drive)
 
 	if (OK_STAT(stat, DRIVE_READY, drive->bad_wstat | ATA_DRQ)) {
 		if (!dma_stat) {
-			struct request *rq = hwif->hwgroup->rq;
+			struct request *rq = hwif->rq;
 
 			task_end_request(drive, rq, stat);
 			return ide_stopped;
diff --git a/drivers/ide/ide-floppy.c b/drivers/ide/ide-floppy.c
index 0a48e2dc53a2..3eab1c6c9b31 100644
--- a/drivers/ide/ide-floppy.c
+++ b/drivers/ide/ide-floppy.c
@@ -71,7 +71,7 @@
 static int ide_floppy_end_request(ide_drive_t *drive, int uptodate, int nsecs)
 {
 	struct ide_disk_obj *floppy = drive->driver_data;
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = drive->hwif->rq;
 	int error;
 
 	ide_debug_log(IDE_DBG_FUNC, "Call %s\n", __func__);
diff --git a/drivers/ide/ide-gd.c b/drivers/ide/ide-gd.c
index b8078b3231f7..7857b209c6df 100644
--- a/drivers/ide/ide-gd.c
+++ b/drivers/ide/ide-gd.c
@@ -149,7 +149,7 @@ static int ide_gd_end_request(ide_drive_t *drive, int uptodate, int nrsecs)
 	return drive->disk_ops->end_request(drive, uptodate, nrsecs);
 }
 
-static ide_driver_t ide_gd_driver = {
+static struct ide_driver ide_gd_driver = {
 	.gen_driver = {
 		.owner		= THIS_MODULE,
 		.name		= "ide-gd",
@@ -162,7 +162,6 @@ static ide_driver_t ide_gd_driver = {
 	.version		= IDE_GD_VERSION,
 	.do_request		= ide_gd_do_request,
 	.end_request		= ide_gd_end_request,
-	.error			= __ide_error,
 #ifdef CONFIG_IDE_PROC_FS
 	.proc_entries		= ide_disk_proc_entries,
 	.proc_devsets		= ide_disk_proc_devsets,
diff --git a/drivers/ide/ide-gd.h b/drivers/ide/ide-gd.h
index 7d3d101713e0..a86779f0756b 100644
--- a/drivers/ide/ide-gd.h
+++ b/drivers/ide/ide-gd.h
@@ -14,11 +14,11 @@
 #endif
 
 struct ide_disk_obj {
-	ide_drive_t	*drive;
-	ide_driver_t	*driver;
-	struct gendisk	*disk;
-	struct kref	kref;
-	unsigned int	openers;	/* protected by BKL for now */
+	ide_drive_t		*drive;
+	struct ide_driver	*driver;
+	struct gendisk		*disk;
+	struct kref		kref;
+	unsigned int		openers;	/* protected by BKL for now */
 
 	/* Last failed packet command */
 	struct ide_atapi_pc *failed_pc;
diff --git a/drivers/ide/ide-h8300.c b/drivers/ide/ide-h8300.c
index e2cdd2e9cdec..9270d3255ee0 100644
--- a/drivers/ide/ide-h8300.c
+++ b/drivers/ide/ide-h8300.c
@@ -159,7 +159,6 @@ static const struct ide_tp_ops h8300_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
diff --git a/drivers/ide/ide-io.c b/drivers/ide/ide-io.c
index 1c36a8e83d36..cc163319dfbd 100644
--- a/drivers/ide/ide-io.c
+++ b/drivers/ide/ide-io.c
@@ -88,7 +88,7 @@ static int __ide_end_request(ide_drive_t *drive, struct request *rq,
 		ret = 0;
 
 	if (ret == 0 && dequeue)
-		drive->hwif->hwgroup->rq = NULL;
+		drive->hwif->rq = NULL;
 
 	return ret;
 }
@@ -107,7 +107,7 @@ static int __ide_end_request(ide_drive_t *drive, struct request *rq,
 int ide_end_request (ide_drive_t *drive, int uptodate, int nr_sectors)
 {
 	unsigned int nr_bytes = nr_sectors << 9;
-	struct request *rq = drive->hwif->hwgroup->rq;
+	struct request *rq = drive->hwif->rq;
 
 	if (!nr_bytes) {
 		if (blk_pc_request(rq))
@@ -160,8 +160,8 @@ EXPORT_SYMBOL_GPL(ide_end_dequeued_request);
  
 void ide_end_drive_cmd (ide_drive_t *drive, u8 stat, u8 err)
 {
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
-	struct request *rq = hwgroup->rq;
+	ide_hwif_t *hwif = drive->hwif;
+	struct request *rq = hwif->rq;
 
 	if (rq->cmd_type == REQ_TYPE_ATA_TASKFILE) {
 		ide_task_t *task = (ide_task_t *)rq->special;
@@ -186,7 +186,7 @@ void ide_end_drive_cmd (ide_drive_t *drive, u8 stat, u8 err)
 		return;
 	}
 
-	hwgroup->rq = NULL;
+	hwif->rq = NULL;
 
 	rq->errors = err;
 
@@ -199,9 +199,9 @@ EXPORT_SYMBOL(ide_end_drive_cmd);
 static void ide_kill_rq(ide_drive_t *drive, struct request *rq)
 {
 	if (rq->rq_disk) {
-		ide_driver_t *drv;
+		struct ide_driver *drv;
 
-		drv = *(ide_driver_t **)rq->rq_disk->private_data;
+		drv = *(struct ide_driver **)rq->rq_disk->private_data;
 		drv->end_request(drive, 0, 0);
 	} else
 		ide_end_request(drive, 0, 0);
@@ -291,7 +291,7 @@ static ide_startstop_t ide_atapi_error(ide_drive_t *drive, struct request *rq, u
 	return ide_stopped;
 }
 
-ide_startstop_t
+static ide_startstop_t
 __ide_error(ide_drive_t *drive, struct request *rq, u8 stat, u8 err)
 {
 	if (drive->media == ide_disk)
@@ -299,8 +299,6 @@ __ide_error(ide_drive_t *drive, struct request *rq, u8 stat, u8 err)
 	return ide_atapi_error(drive, rq, stat, err);
 }
 
-EXPORT_SYMBOL_GPL(__ide_error);
-
 /**
  *	ide_error	-	handle an error on the IDE
  *	@drive: drive the error occurred on
@@ -321,7 +319,8 @@ ide_startstop_t ide_error (ide_drive_t *drive, const char *msg, u8 stat)
 
 	err = ide_dump_status(drive, msg, stat);
 
-	if ((rq = HWGROUP(drive)->rq) == NULL)
+	rq = drive->hwif->rq;
+	if (rq == NULL)
 		return ide_stopped;
 
 	/* retry only "normal" I/O: */
@@ -331,15 +330,8 @@ ide_startstop_t ide_error (ide_drive_t *drive, const char *msg, u8 stat)
 		return ide_stopped;
 	}
 
-	if (rq->rq_disk) {
-		ide_driver_t *drv;
-
-		drv = *(ide_driver_t **)rq->rq_disk->private_data;
-		return drv->error(drive, rq, stat, err);
-	} else
-		return __ide_error(drive, rq, stat, err);
+	return __ide_error(drive, rq, stat, err);
 }
-
 EXPORT_SYMBOL_GPL(ide_error);
 
 static void ide_tf_set_specify_cmd(ide_drive_t *drive, struct ide_taskfile *tf)
@@ -462,7 +454,7 @@ EXPORT_SYMBOL_GPL(ide_init_sg_cmd);
 static ide_startstop_t execute_drive_cmd (ide_drive_t *drive,
 		struct request *rq)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	ide_task_t *task = rq->special;
 
 	if (task) {
@@ -586,7 +578,7 @@ static ide_startstop_t start_request (ide_drive_t *drive, struct request *rq)
 
 #ifdef DEBUG
 	printk("%s: start_request: current=0x%08lx\n",
-		HWIF(drive)->name, (unsigned long) rq);
+		drive->hwif->name, (unsigned long) rq);
 #endif
 
 	/* bail early if we've exceeded max_failures */
@@ -605,7 +597,7 @@ static ide_startstop_t start_request (ide_drive_t *drive, struct request *rq)
 		return startstop;
 	}
 	if (!drive->special.all) {
-		ide_driver_t *drv;
+		struct ide_driver *drv;
 
 		/*
 		 * We reset the drive so we need to issue a SETFEATURES.
@@ -638,7 +630,7 @@ static ide_startstop_t start_request (ide_drive_t *drive, struct request *rq)
 			 */
 			return ide_special_rq(drive, rq);
 
-		drv = *(ide_driver_t **)rq->rq_disk->private_data;
+		drv = *(struct ide_driver **)rq->rq_disk->private_data;
 
 		return drv->do_request(drive, rq, rq->sector);
 	}
@@ -654,7 +646,7 @@ kill_rq:
  *	@timeout: time to stall for (jiffies)
  *
  *	ide_stall_queue() can be used by a drive to give excess bandwidth back
- *	to the hwgroup by sleeping for timeout jiffies.
+ *	to the port by sleeping for timeout jiffies.
  */
  
 void ide_stall_queue (ide_drive_t *drive, unsigned long timeout)
@@ -666,45 +658,53 @@ void ide_stall_queue (ide_drive_t *drive, unsigned long timeout)
 }
 EXPORT_SYMBOL(ide_stall_queue);
 
+static inline int ide_lock_port(ide_hwif_t *hwif)
+{
+	if (hwif->busy)
+		return 1;
+
+	hwif->busy = 1;
+
+	return 0;
+}
+
+static inline void ide_unlock_port(ide_hwif_t *hwif)
+{
+	hwif->busy = 0;
+}
+
+static inline int ide_lock_host(struct ide_host *host, ide_hwif_t *hwif)
+{
+	int rc = 0;
+
+	if (host->host_flags & IDE_HFLAG_SERIALIZE) {
+		rc = test_and_set_bit_lock(IDE_HOST_BUSY, &host->host_busy);
+		if (rc == 0) {
+			/* for atari only */
+			ide_get_lock(ide_intr, hwif);
+		}
+	}
+	return rc;
+}
+
+static inline void ide_unlock_host(struct ide_host *host)
+{
+	if (host->host_flags & IDE_HFLAG_SERIALIZE) {
+		/* for atari only */
+		ide_release_lock();
+		clear_bit_unlock(IDE_HOST_BUSY, &host->host_busy);
+	}
+}
+
 /*
- * Issue a new request to a drive from hwgroup
- *
- * A hwgroup is a serialized group of IDE interfaces.  Usually there is
- * exactly one hwif (interface) per hwgroup, but buggy controllers (eg. CMD640)
- * may have both interfaces in a single hwgroup to "serialize" access.
- * Or possibly multiple ISA interfaces can share a common IRQ by being grouped
- * together into one hwgroup for serialized access.
- *
- * Note also that several hwgroups can end up sharing a single IRQ,
- * possibly along with many other devices.  This is especially common in
- * PCI-based systems with off-board IDE controller cards.
- *
- * The IDE driver uses a per-hwgroup lock to protect the hwgroup->busy flag.
- *
- * The first thread into the driver for a particular hwgroup sets the
- * hwgroup->busy flag to indicate that this hwgroup is now active,
- * and then initiates processing of the top request from the request queue.
- *
- * Other threads attempting entry notice the busy setting, and will simply
- * queue their new requests and exit immediately.  Note that hwgroup->busy
- * remains set even when the driver is merely awaiting the next interrupt.
- * Thus, the meaning is "this hwgroup is busy processing a request".
- *
- * When processing of a request completes, the completing thread or IRQ-handler
- * will start the next request from the queue.  If no more work remains,
- * the driver will clear the hwgroup->busy flag and exit.
- *
- * The per-hwgroup spinlock is used to protect all access to the
- * hwgroup->busy flag, but is otherwise not needed for most processing in
- * the driver.  This makes the driver much more friendlier to shared IRQs
- * than previous designs, while remaining 100% (?) SMP safe and capable.
+ * Issue a new request to a device.
  */
 void do_ide_request(struct request_queue *q)
 {
 	ide_drive_t	*drive = q->queuedata;
 	ide_hwif_t	*hwif = drive->hwif;
-	ide_hwgroup_t	*hwgroup = hwif->hwgroup;
-	struct request	*rq;
+	struct ide_host *host = hwif->host;
+	struct request	*rq = NULL;
 	ide_startstop_t	startstop;
 
 	/*
@@ -721,32 +721,40 @@ void do_ide_request(struct request_queue *q)
 		blk_remove_plug(q);
 
 	spin_unlock_irq(q->queue_lock);
-	spin_lock_irq(&hwgroup->lock);
 
-	if (!ide_lock_hwgroup(hwgroup)) {
+	if (ide_lock_host(host, hwif))
+		goto plug_device_2;
+
+	spin_lock_irq(&hwif->lock);
+
+	if (!ide_lock_port(hwif)) {
+		ide_hwif_t *prev_port;
 repeat:
-		hwgroup->rq = NULL;
+		prev_port = hwif->host->cur_port;
+		hwif->rq = NULL;
 
 		if (drive->dev_flags & IDE_DFLAG_SLEEPING) {
 			if (time_before(drive->sleep, jiffies)) {
-				ide_unlock_hwgroup(hwgroup);
+				ide_unlock_port(hwif);
 				goto plug_device;
 			}
 		}
 
-		if (hwif != hwgroup->hwif) {
+		if ((hwif->host->host_flags & IDE_HFLAG_SERIALIZE) &&
+		    hwif != prev_port) {
 			/*
-			 * set nIEN for previous hwif, drives in the
+			 * set nIEN for previous port, drives in the
 			 * quirk_list may not like intr setups/cleanups
 			 */
-			if (drive->quirk_list == 0)
-				hwif->tp_ops->set_irq(hwif, 0);
+			if (prev_port && prev_port->cur_dev->quirk_list == 0)
+				prev_port->tp_ops->set_irq(prev_port, 0);
+
+			hwif->host->cur_port = hwif;
 		}
-		hwgroup->hwif = hwif;
-		hwgroup->drive = drive;
+		hwif->cur_dev = drive;
 		drive->dev_flags &= ~(IDE_DFLAG_SLEEPING | IDE_DFLAG_PARKED);
 
-		spin_unlock_irq(&hwgroup->lock);
+		spin_unlock_irq(&hwif->lock);
 		spin_lock_irq(q->queue_lock);
 		/*
 		 * we know that the queue isn't empty, but this can happen
@@ -754,10 +762,10 @@ repeat:
 		 */
 		rq = elv_next_request(drive->queue);
 		spin_unlock_irq(q->queue_lock);
-		spin_lock_irq(&hwgroup->lock);
+		spin_lock_irq(&hwif->lock);
 
 		if (!rq) {
-			ide_unlock_hwgroup(hwgroup);
+			ide_unlock_port(hwif);
 			goto out;
 		}
 
@@ -778,27 +786,31 @@ repeat:
 		    blk_pm_request(rq) == 0 &&
 		    (rq->cmd_flags & REQ_PREEMPT) == 0) {
 			/* there should be no pending command at this point */
-			ide_unlock_hwgroup(hwgroup);
+			ide_unlock_port(hwif);
 			goto plug_device;
 		}
 
-		hwgroup->rq = rq;
+		hwif->rq = rq;
 
-		spin_unlock_irq(&hwgroup->lock);
+		spin_unlock_irq(&hwif->lock);
 		startstop = start_request(drive, rq);
-		spin_lock_irq(&hwgroup->lock);
+		spin_lock_irq(&hwif->lock);
 
 		if (startstop == ide_stopped)
 			goto repeat;
 	} else
 		goto plug_device;
 out:
-	spin_unlock_irq(&hwgroup->lock);
+	spin_unlock_irq(&hwif->lock);
+	if (rq == NULL)
+		ide_unlock_host(host);
 	spin_lock_irq(q->queue_lock);
 	return;
 
 plug_device:
-	spin_unlock_irq(&hwgroup->lock);
+	spin_unlock_irq(&hwif->lock);
+	ide_unlock_host(host);
+plug_device_2:
 	spin_lock_irq(q->queue_lock);
 
 	if (!elv_queue_empty(q))
@@ -806,13 +818,13 @@ plug_device:
 }
 
 /*
- * un-busy the hwgroup etc, and clear any pending DMA status. we want to
+ * un-busy the port etc, and clear any pending DMA status. we want to
  * retry the current request in pio mode instead of risking tossing it
  * all away
  */
 static ide_startstop_t ide_dma_timeout_retry(ide_drive_t *drive, int error)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct request *rq;
 	ide_startstop_t ret = ide_stopped;
 
@@ -840,15 +852,14 @@ static ide_startstop_t ide_dma_timeout_retry(ide_drive_t *drive, int error)
 	ide_dma_off_quietly(drive);
 
 	/*
-	 * un-busy drive etc (hwgroup->busy is cleared on return) and
-	 * make sure request is sane
+	 * un-busy drive etc and make sure request is sane
 	 */
-	rq = HWGROUP(drive)->rq;
 
+	rq = hwif->rq;
 	if (!rq)
 		goto out;
 
-	HWGROUP(drive)->rq = NULL;
+	hwif->rq = NULL;
 
 	rq->errors = 0;
 
@@ -876,7 +887,7 @@ static void ide_plug_device(ide_drive_t *drive)
 
 /**
  *	ide_timer_expiry	-	handle lack of an IDE interrupt
- *	@data: timer callback magic (hwgroup)
+ *	@data: timer callback magic (hwif)
  *
  *	An IDE command has timed out before the expected drive return
  *	occurred. At this point we attempt to clean up the current
@@ -890,18 +901,18 @@ static void ide_plug_device(ide_drive_t *drive)
  
 void ide_timer_expiry (unsigned long data)
 {
-	ide_hwgroup_t	*hwgroup = (ide_hwgroup_t *) data;
+	ide_hwif_t	*hwif = (ide_hwif_t *)data;
 	ide_drive_t	*uninitialized_var(drive);
 	ide_handler_t	*handler;
-	ide_expiry_t	*expiry;
 	unsigned long	flags;
 	unsigned long	wait = -1;
 	int		plug_device = 0;
 
-	spin_lock_irqsave(&hwgroup->lock, flags);
+	spin_lock_irqsave(&hwif->lock, flags);
 
-	if (((handler = hwgroup->handler) == NULL) ||
-	    (hwgroup->req_gen != hwgroup->req_gen_timer)) {
+	handler = hwif->handler;
+
+	if (handler == NULL || hwif->req_gen != hwif->req_gen_timer) {
 		/*
 		 * Either a marginal timeout occurred
 		 * (got the interrupt just as timer expired),
@@ -909,72 +920,68 @@ void ide_timer_expiry (unsigned long data)
 		 * Either way, we don't really want to complain about anything.
 		 */
 	} else {
-		drive = hwgroup->drive;
-		if (!drive) {
-			printk(KERN_ERR "ide_timer_expiry: hwgroup->drive was NULL\n");
-			hwgroup->handler = NULL;
-		} else {
-			ide_hwif_t *hwif;
-			ide_startstop_t startstop = ide_stopped;
-
-			if ((expiry = hwgroup->expiry) != NULL) {
-				/* continue */
-				if ((wait = expiry(drive)) > 0) {
-					/* reset timer */
-					hwgroup->timer.expires  = jiffies + wait;
-					hwgroup->req_gen_timer = hwgroup->req_gen;
-					add_timer(&hwgroup->timer);
-					spin_unlock_irqrestore(&hwgroup->lock, flags);
-					return;
-				}
-			}
-			hwgroup->handler = NULL;
-			/*
-			 * We need to simulate a real interrupt when invoking
-			 * the handler() function, which means we need to
-			 * globally mask the specific IRQ:
-			 */
-			spin_unlock(&hwgroup->lock);
-			hwif  = HWIF(drive);
-			/* disable_irq_nosync ?? */
-			disable_irq(hwif->irq);
-			/* local CPU only,
-			 * as if we were handling an interrupt */
-			local_irq_disable();
-			if (hwgroup->polling) {
-				startstop = handler(drive);
-			} else if (drive_is_ready(drive)) {
-				if (drive->waiting_for_dma)
-					hwif->dma_ops->dma_lost_irq(drive);
-				(void)ide_ack_intr(hwif);
-				printk(KERN_WARNING "%s: lost interrupt\n", drive->name);
-				startstop = handler(drive);
-			} else {
-				if (drive->waiting_for_dma) {
-					startstop = ide_dma_timeout_retry(drive, wait);
-				} else
-					startstop =
-					ide_error(drive, "irq timeout",
-						  hwif->tp_ops->read_status(hwif));
-			}
-			spin_lock_irq(&hwgroup->lock);
-			enable_irq(hwif->irq);
-			if (startstop == ide_stopped) {
-				ide_unlock_hwgroup(hwgroup);
-				plug_device = 1;
+		ide_expiry_t *expiry = hwif->expiry;
+		ide_startstop_t startstop = ide_stopped;
+
+		drive = hwif->cur_dev;
+
+		if (expiry) {
+			wait = expiry(drive);
+			if (wait > 0) { /* continue */
+				/* reset timer */
+				hwif->timer.expires = jiffies + wait;
+				hwif->req_gen_timer = hwif->req_gen;
+				add_timer(&hwif->timer);
+				spin_unlock_irqrestore(&hwif->lock, flags);
+				return;
 			}
 		}
+		hwif->handler = NULL;
+		/*
+		 * We need to simulate a real interrupt when invoking
+		 * the handler() function, which means we need to
+		 * globally mask the specific IRQ:
+		 */
+		spin_unlock(&hwif->lock);
+		/* disable_irq_nosync ?? */
+		disable_irq(hwif->irq);
+		/* local CPU only, as if we were handling an interrupt */
+		local_irq_disable();
+		if (hwif->polling) {
+			startstop = handler(drive);
+		} else if (drive_is_ready(drive)) {
+			if (drive->waiting_for_dma)
+				hwif->dma_ops->dma_lost_irq(drive);
+			(void)ide_ack_intr(hwif);
+			printk(KERN_WARNING "%s: lost interrupt\n",
+				drive->name);
+			startstop = handler(drive);
+		} else {
+			if (drive->waiting_for_dma)
+				startstop = ide_dma_timeout_retry(drive, wait);
+			else
+				startstop = ide_error(drive, "irq timeout",
+					hwif->tp_ops->read_status(hwif));
+		}
+		spin_lock_irq(&hwif->lock);
+		enable_irq(hwif->irq);
+		if (startstop == ide_stopped) {
+			ide_unlock_port(hwif);
+			plug_device = 1;
+		}
 	}
-	spin_unlock_irqrestore(&hwgroup->lock, flags);
+	spin_unlock_irqrestore(&hwif->lock, flags);
 
-	if (plug_device)
+	if (plug_device) {
+		ide_unlock_host(hwif->host);
 		ide_plug_device(drive);
+	}
 }
 
 /**
  *	unexpected_intr		-	handle an unexpected IDE interrupt
  *	@irq: interrupt line
- *	@hwgroup: hwgroup being processed
+ *	@hwif: port being processed
  *
  *	There's nothing really useful we can do with an unexpected interrupt,
  *	other than reading the status register (to clear it), and logging it.
@@ -998,52 +1005,38 @@ void ide_timer_expiry (unsigned long data)
  *	before completing the issuance of any new drive command, so we will not
  *	be accidentally invoked as a result of any valid command completion
  *	interrupt.
- *
- *	Note that we must walk the entire hwgroup here. We know which hwif
- *	is doing the current command, but we don't know which hwif burped
- *	mysteriously.
  */
- 
-static void unexpected_intr (int irq, ide_hwgroup_t *hwgroup)
-{
-	u8 stat;
-	ide_hwif_t *hwif = hwgroup->hwif;
 
-	/*
-	 * handle the unexpected interrupt
-	 */
-	do {
-		if (hwif->irq == irq) {
-			stat = hwif->tp_ops->read_status(hwif);
-
-			if (!OK_STAT(stat, ATA_DRDY, BAD_STAT)) {
-				/* Try to not flood the console with msgs */
-				static unsigned long last_msgtime, count;
-				++count;
-				if (time_after(jiffies, last_msgtime + HZ)) {
-					last_msgtime = jiffies;
-					printk(KERN_ERR "%s%s: unexpected interrupt, "
-						"status=0x%02x, count=%ld\n",
-						hwif->name,
-						(hwif->next==hwgroup->hwif) ? "" : "(?)", stat, count);
-				}
-			}
+static void unexpected_intr(int irq, ide_hwif_t *hwif)
+{
+	u8 stat = hwif->tp_ops->read_status(hwif);
+
+	if (!OK_STAT(stat, ATA_DRDY, BAD_STAT)) {
+		/* Try to not flood the console with msgs */
+		static unsigned long last_msgtime, count;
+		++count;
+
+		if (time_after(jiffies, last_msgtime + HZ)) {
+			last_msgtime = jiffies;
+			printk(KERN_ERR "%s: unexpected interrupt, "
+				"status=0x%02x, count=%ld\n",
+				hwif->name, stat, count);
 		}
-	} while ((hwif = hwif->next) != hwgroup->hwif);
+	}
 }
 
 /**
  *	ide_intr	-	default IDE interrupt handler
  *	@irq: interrupt number
- *	@dev_id: hwif group
+ *	@dev_id: hwif
  *	@regs: unused weirdness from the kernel irq layer
  *
  *	This is the default IRQ handler for the IDE layer. You should
  *	not need to override it. If you do be aware it is subtle in
  *	places
  *
- *	hwgroup->hwif is the interface in the group currently performing
- *	a command. hwgroup->drive is the drive and hwgroup->handler is
+ *	hwif is the interface in the group currently performing
+ *	a command. hwif->cur_dev is the drive and hwif->handler is
  *	the IRQ handler to call. As we issue a command the handlers
  *	step through multiple states, reassigning the handler to the
  *	next step in the process. Unlike a smart SCSI controller IDE
@@ -1054,26 +1047,32 @@ static void unexpected_intr (int irq, ide_hwgroup_t *hwgroup)
  *
  *	The handler eventually returns ide_stopped to indicate the
  *	request completed. At this point we issue the next request
- *	on the hwgroup and the process begins again.
+ *	on the port and the process begins again.
  */
- 
+
 irqreturn_t ide_intr (int irq, void *dev_id)
 {
-	unsigned long flags;
-	ide_hwgroup_t *hwgroup = (ide_hwgroup_t *)dev_id;
-	ide_hwif_t *hwif = hwgroup->hwif;
+	ide_hwif_t *hwif = (ide_hwif_t *)dev_id;
 	ide_drive_t *uninitialized_var(drive);
 	ide_handler_t *handler;
+	unsigned long flags;
 	ide_startstop_t startstop;
 	irqreturn_t irq_ret = IRQ_NONE;
 	int plug_device = 0;
 
-	spin_lock_irqsave(&hwgroup->lock, flags);
+	if (hwif->host->host_flags & IDE_HFLAG_SERIALIZE) {
+		if (hwif != hwif->host->cur_port)
+			goto out_early;
+	}
+
+	spin_lock_irqsave(&hwif->lock, flags);
 
 	if (!ide_ack_intr(hwif))
 		goto out;
 
-	if ((handler = hwgroup->handler) == NULL || hwgroup->polling) {
+	handler = hwif->handler;
+
+	if (handler == NULL || hwif->polling) {
 		/*
 		 * Not expecting an interrupt from this drive.
 		 * That means this could be:
@@ -1097,7 +1096,7 @@ irqreturn_t ide_intr (int irq, void *dev_id)
 			 * Probably not a shared PCI interrupt,
 			 * so we can safely try to do something about it:
 			 */
-			unexpected_intr(irq, hwgroup);
+			unexpected_intr(irq, hwif);
 #ifdef CONFIG_BLK_DEV_IDEPCI
 		} else {
 			/*
@@ -1110,16 +1109,7 @@ irqreturn_t ide_intr (int irq, void *dev_id)
 		goto out;
 	}
 
-	drive = hwgroup->drive;
-	if (!drive) {
-		/*
-		 * This should NEVER happen, and there isn't much
-		 * we could do about it here.
-		 *
-		 * [Note - this can occur if the drive is hot unplugged]
-		 */
-		goto out_handled;
-	}
+	drive = hwif->cur_dev;
 
 	if (!drive_is_ready(drive))
 		/*
@@ -1131,10 +1121,10 @@ irqreturn_t ide_intr (int irq, void *dev_id)
 		 */
 		goto out;
 
-	hwgroup->handler = NULL;
-	hwgroup->req_gen++;
-	del_timer(&hwgroup->timer);
-	spin_unlock(&hwgroup->lock);
+	hwif->handler = NULL;
+	hwif->req_gen++;
+	del_timer(&hwif->timer);
+	spin_unlock(&hwif->lock);
 
 	if (hwif->port_ops && hwif->port_ops->clear_irq)
 		hwif->port_ops->clear_irq(drive);
@@ -1145,7 +1135,7 @@ irqreturn_t ide_intr (int irq, void *dev_id)
 	/* service this interrupt, may set handler for next interrupt */
 	startstop = handler(drive);
 
-	spin_lock_irq(&hwgroup->lock);
+	spin_lock_irq(&hwif->lock);
 	/*
 	 * Note that handler() may have set things up for another
 	 * interrupt to occur soon, but it cannot happen until
@@ -1154,20 +1144,18 @@ irqreturn_t ide_intr (int irq, void *dev_id)
 	 * won't allow another of the same (on any CPU) until we return.
 	 */
 	if (startstop == ide_stopped) {
-		if (hwgroup->handler == NULL) {	/* paranoia */
-			ide_unlock_hwgroup(hwgroup);
-			plug_device = 1;
-		} else
-			printk(KERN_ERR "%s: %s: huh? expected NULL handler "
-					"on exit\n", __func__, drive->name);
+		BUG_ON(hwif->handler);
+		ide_unlock_port(hwif);
+		plug_device = 1;
 	}
-out_handled:
 	irq_ret = IRQ_HANDLED;
 out:
-	spin_unlock_irqrestore(&hwgroup->lock, flags);
-
-	if (plug_device)
+	spin_unlock_irqrestore(&hwif->lock, flags);
+out_early:
+	if (plug_device) {
+		ide_unlock_host(hwif->host);
 		ide_plug_device(drive);
+	}
 
 	return irq_ret;
 }
@@ -1189,15 +1177,13 @@ out:
 
 void ide_do_drive_cmd(ide_drive_t *drive, struct request *rq)
 {
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
 	struct request_queue *q = drive->queue;
 	unsigned long flags;
 
-	hwgroup->rq = NULL;
+	drive->hwif->rq = NULL;
 
 	spin_lock_irqsave(q->queue_lock, flags);
 	__elv_add_request(q, rq, ELEVATOR_INSERT_FRONT, 0);
-	blk_start_queueing(q);
 	spin_unlock_irqrestore(q->queue_lock, flags);
 }
 EXPORT_SYMBOL(ide_do_drive_cmd);
diff --git a/drivers/ide/ide-iops.c b/drivers/ide/ide-iops.c
index ad8bd6539283..e728cfe7273f 100644
--- a/drivers/ide/ide-iops.c
+++ b/drivers/ide/ide-iops.c
@@ -105,15 +105,6 @@ u8 ide_read_altstatus(ide_hwif_t *hwif)
 }
 EXPORT_SYMBOL_GPL(ide_read_altstatus);
 
-u8 ide_read_sff_dma_status(ide_hwif_t *hwif)
-{
-	if (hwif->host_flags & IDE_HFLAG_MMIO)
-		return readb((void __iomem *)(hwif->dma_base + ATA_DMA_STATUS));
-	else
-		return inb(hwif->dma_base + ATA_DMA_STATUS);
-}
-EXPORT_SYMBOL_GPL(ide_read_sff_dma_status);
-
 void ide_set_irq(ide_hwif_t *hwif, int on)
 {
 	u8 ctl = ATA_DEVCTL_OBS;
@@ -388,7 +379,6 @@ const struct ide_tp_ops default_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
@@ -451,7 +441,7 @@ EXPORT_SYMBOL(ide_fixstring);
  */
 int drive_is_ready (ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	u8 stat			= 0;
 
 	if (drive->waiting_for_dma)
@@ -503,7 +493,8 @@ static int __ide_wait_stat(ide_drive_t *drive, u8 good, u8 bad, unsigned long ti
 	stat = tp_ops->read_status(hwif);
 
 	if (stat & ATA_BUSY) {
-		local_irq_set(flags);
+		local_irq_save(flags);
+		local_irq_enable_in_hardirq();
 		timeout += jiffies;
 		while ((stat = tp_ops->read_status(hwif)) & ATA_BUSY) {
 			if (time_after(jiffies, timeout)) {
@@ -822,25 +813,25 @@ int ide_config_drive_speed(ide_drive_t *drive, u8 speed)
 static void __ide_set_handler (ide_drive_t *drive, ide_handler_t *handler,
 		      unsigned int timeout, ide_expiry_t *expiry)
 {
-	ide_hwgroup_t *hwgroup = HWGROUP(drive);
-
-	BUG_ON(hwgroup->handler);
-	hwgroup->handler	= handler;
-	hwgroup->expiry		= expiry;
-	hwgroup->timer.expires	= jiffies + timeout;
-	hwgroup->req_gen_timer	= hwgroup->req_gen;
-	add_timer(&hwgroup->timer);
+	ide_hwif_t *hwif = drive->hwif;
+
+	BUG_ON(hwif->handler);
+	hwif->handler		= handler;
+	hwif->expiry		= expiry;
+	hwif->timer.expires	= jiffies + timeout;
+	hwif->req_gen_timer	= hwif->req_gen;
+	add_timer(&hwif->timer);
 }
 
 void ide_set_handler (ide_drive_t *drive, ide_handler_t *handler,
 		      unsigned int timeout, ide_expiry_t *expiry)
 {
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
+	ide_hwif_t *hwif = drive->hwif;
 	unsigned long flags;
 
-	spin_lock_irqsave(&hwgroup->lock, flags);
+	spin_lock_irqsave(&hwif->lock, flags);
 	__ide_set_handler(drive, handler, timeout, expiry);
-	spin_unlock_irqrestore(&hwgroup->lock, flags);
+	spin_unlock_irqrestore(&hwif->lock, flags);
 }
 
 EXPORT_SYMBOL(ide_set_handler);
@@ -863,10 +854,9 @@ void ide_execute_command(ide_drive_t *drive, u8 cmd, ide_handler_t *handler,
 			 unsigned timeout, ide_expiry_t *expiry)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
 	unsigned long flags;
 
-	spin_lock_irqsave(&hwgroup->lock, flags);
+	spin_lock_irqsave(&hwif->lock, flags);
 	__ide_set_handler(drive, handler, timeout, expiry);
 	hwif->tp_ops->exec_command(hwif, cmd);
 	/*
@@ -876,26 +866,25 @@ void ide_execute_command(ide_drive_t *drive, u8 cmd, ide_handler_t *handler,
 	 * FIXME: we could skip this delay with care on non shared devices
 	 */
 	ndelay(400);
-	spin_unlock_irqrestore(&hwgroup->lock, flags);
+	spin_unlock_irqrestore(&hwif->lock, flags);
 }
 EXPORT_SYMBOL(ide_execute_command);
 
 void ide_execute_pkt_cmd(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
 	unsigned long flags;
 
-	spin_lock_irqsave(&hwgroup->lock, flags);
+	spin_lock_irqsave(&hwif->lock, flags);
 	hwif->tp_ops->exec_command(hwif, ATA_CMD_PACKET);
 	ndelay(400);
-	spin_unlock_irqrestore(&hwgroup->lock, flags);
+	spin_unlock_irqrestore(&hwif->lock, flags);
 }
 EXPORT_SYMBOL_GPL(ide_execute_pkt_cmd);
 
 static inline void ide_complete_drive_reset(ide_drive_t *drive, int err)
 {
-	struct request *rq = drive->hwif->hwgroup->rq;
+	struct request *rq = drive->hwif->rq;
 
 	if (rq && blk_special_request(rq) && rq->cmd[0] == REQ_DRIVE_RESET)
 		ide_end_request(drive, err ? err : 1, 0);
@@ -913,7 +902,6 @@ static ide_startstop_t do_reset1 (ide_drive_t *, int);
 static ide_startstop_t atapi_reset_pollfunc (ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
 	u8 stat;
 
 	SELECT_DRIVE(drive);
@@ -923,20 +911,20 @@ static ide_startstop_t atapi_reset_pollfunc (ide_drive_t *drive)
 	if (OK_STAT(stat, 0, ATA_BUSY))
 		printk("%s: ATAPI reset complete\n", drive->name);
 	else {
-		if (time_before(jiffies, hwgroup->poll_timeout)) {
+		if (time_before(jiffies, hwif->poll_timeout)) {
 			ide_set_handler(drive, &atapi_reset_pollfunc, HZ/20, NULL);
 			/* continue polling */
 			return ide_started;
 		}
 		/* end of polling */
-		hwgroup->polling = 0;
+		hwif->polling = 0;
 		printk("%s: ATAPI reset timed-out, status=0x%02x\n",
 				drive->name, stat);
 		/* do it the old fashioned way */
 		return do_reset1(drive, 1);
 	}
 	/* done polling */
-	hwgroup->polling = 0;
+	hwif->polling = 0;
 	ide_complete_drive_reset(drive, 0);
 	return ide_stopped;
 }
@@ -968,8 +956,7 @@ static void ide_reset_report_error(ide_hwif_t *hwif, u8 err)
  */
 static ide_startstop_t reset_pollfunc (ide_drive_t *drive)
 {
-	ide_hwgroup_t *hwgroup	= HWGROUP(drive);
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	const struct ide_port_ops *port_ops = hwif->port_ops;
 	u8 tmp;
 	int err = 0;
@@ -986,7 +973,7 @@ static ide_startstop_t reset_pollfunc (ide_drive_t *drive)
 	tmp = hwif->tp_ops->read_status(hwif);
 
 	if (!OK_STAT(tmp, 0, ATA_BUSY)) {
-		if (time_before(jiffies, hwgroup->poll_timeout)) {
+		if (time_before(jiffies, hwif->poll_timeout)) {
 			ide_set_handler(drive, &reset_pollfunc, HZ/20, NULL);
 			/* continue polling */
 			return ide_started;
@@ -1007,7 +994,7 @@ static ide_startstop_t reset_pollfunc (ide_drive_t *drive)
 		}
 	}
 out:
-	hwgroup->polling = 0;	/* done polling */
+	hwif->polling = 0;	/* done polling */
 	ide_complete_drive_reset(drive, err);
 	return ide_stopped;
 }
@@ -1081,18 +1068,18 @@ static void pre_reset(ide_drive_t *drive)
 static ide_startstop_t do_reset1 (ide_drive_t *drive, int do_not_try_atapi)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
 	struct ide_io_ports *io_ports = &hwif->io_ports;
 	const struct ide_tp_ops *tp_ops = hwif->tp_ops;
 	const struct ide_port_ops *port_ops;
+	ide_drive_t *tdrive;
 	unsigned long flags, timeout;
-	unsigned int unit;
+	int i;
 	DEFINE_WAIT(wait);
 
-	spin_lock_irqsave(&hwgroup->lock, flags);
+	spin_lock_irqsave(&hwif->lock, flags);
 
 	/* We must not reset with running handlers */
-	BUG_ON(hwgroup->handler != NULL);
+	BUG_ON(hwif->handler != NULL);
 
 	/* For an ATAPI device, first try an ATAPI SRST. */
 	if (drive->media != ide_disk && !do_not_try_atapi) {
@@ -1101,10 +1088,10 @@ static ide_startstop_t do_reset1 (ide_drive_t *drive, int do_not_try_atapi)
 		udelay (20);
 		tp_ops->exec_command(hwif, ATA_CMD_DEV_RESET);
 		ndelay(400);
-		hwgroup->poll_timeout = jiffies + WAIT_WORSTCASE;
-		hwgroup->polling = 1;
+		hwif->poll_timeout = jiffies + WAIT_WORSTCASE;
+		hwif->polling = 1;
 		__ide_set_handler(drive, &atapi_reset_pollfunc, HZ/20, NULL);
-		spin_unlock_irqrestore(&hwgroup->lock, flags);
+		spin_unlock_irqrestore(&hwif->lock, flags);
 		return ide_started;
 	}
 
@@ -1114,9 +1101,7 @@ static ide_startstop_t do_reset1 (ide_drive_t *drive, int do_not_try_atapi)
 
 		prepare_to_wait(&ide_park_wq, &wait, TASK_UNINTERRUPTIBLE);
 		timeout = jiffies;
-		for (unit = 0; unit < MAX_DRIVES; unit++) {
-			ide_drive_t *tdrive = &hwif->drives[unit];
-
+		ide_port_for_each_dev(i, tdrive, hwif) {
 			if (tdrive->dev_flags & IDE_DFLAG_PRESENT &&
 			    tdrive->dev_flags & IDE_DFLAG_PARKED &&
 			    time_after(tdrive->sleep, timeout))
@@ -1127,9 +1112,9 @@ static ide_startstop_t do_reset1 (ide_drive_t *drive, int do_not_try_atapi)
 		if (time_before_eq(timeout, now))
 			break;
 
-		spin_unlock_irqrestore(&hwgroup->lock, flags);
+		spin_unlock_irqrestore(&hwif->lock, flags);
 		timeout = schedule_timeout_uninterruptible(timeout - now);
-		spin_lock_irqsave(&hwgroup->lock, flags);
+		spin_lock_irqsave(&hwif->lock, flags);
 	} while (timeout);
 	finish_wait(&ide_park_wq, &wait);
 
@@ -1137,11 +1122,11 @@ static ide_startstop_t do_reset1 (ide_drive_t *drive, int do_not_try_atapi)
 	 * First, reset any device state data we were maintaining
 	 * for any of the drives on this interface.
 	 */
-	for (unit = 0; unit < MAX_DRIVES; ++unit)
-		pre_reset(&hwif->drives[unit]);
+	ide_port_for_each_dev(i, tdrive, hwif)
+		pre_reset(tdrive);
 
 	if (io_ports->ctl_addr == 0) {
-		spin_unlock_irqrestore(&hwgroup->lock, flags);
+		spin_unlock_irqrestore(&hwif->lock, flags);
 		ide_complete_drive_reset(drive, -ENXIO);
 		return ide_stopped;
 	}
@@ -1164,8 +1149,8 @@ static ide_startstop_t do_reset1 (ide_drive_t *drive, int do_not_try_atapi)
 	tp_ops->set_irq(hwif, drive->quirk_list == 2);
 	/* more than enough time */
 	udelay(10);
-	hwgroup->poll_timeout = jiffies + WAIT_WORSTCASE;
-	hwgroup->polling = 1;
+	hwif->poll_timeout = jiffies + WAIT_WORSTCASE;
+	hwif->polling = 1;
 	__ide_set_handler(drive, &reset_pollfunc, HZ/20, NULL);
 
 	/*
@@ -1177,7 +1162,7 @@ static ide_startstop_t do_reset1 (ide_drive_t *drive, int do_not_try_atapi)
 	if (port_ops && port_ops->resetproc)
 		port_ops->resetproc(drive);
 
-	spin_unlock_irqrestore(&hwgroup->lock, flags);
+	spin_unlock_irqrestore(&hwif->lock, flags);
 	return ide_started;
 }
 
@@ -1221,6 +1206,3 @@ int ide_wait_not_busy(ide_hwif_t *hwif, unsigned long timeout)
 	}
 	return -EBUSY;
 }
-
-EXPORT_SYMBOL_GPL(ide_wait_not_busy);
-
diff --git a/drivers/ide/ide-lib.c b/drivers/ide/ide-lib.c
index 9f6e33d8a8b2..09526a0de734 100644
--- a/drivers/ide/ide-lib.c
+++ b/drivers/ide/ide-lib.c
@@ -273,7 +273,7 @@ int ide_set_xfer_rate(ide_drive_t *drive, u8 rate)
 
 static void ide_dump_opcode(ide_drive_t *drive)
 {
-	struct request *rq = drive->hwif->hwgroup->rq;
+	struct request *rq = drive->hwif->rq;
 	ide_task_t *task = NULL;
 
 	if (!rq)
@@ -346,10 +346,13 @@ static void ide_dump_ata_error(ide_drive_t *drive, u8 err)
 	printk(KERN_CONT "}");
 	if ((err & (ATA_BBK | ATA_ABORTED)) == ATA_BBK ||
 	    (err & (ATA_UNC | ATA_IDNF | ATA_AMNF))) {
+		struct request *rq = drive->hwif->rq;
+
 		ide_dump_sector(drive);
-		if (HWGROUP(drive) && HWGROUP(drive)->rq)
+
+		if (rq)
 			printk(KERN_CONT ", sector=%llu",
-			       (unsigned long long)HWGROUP(drive)->rq->sector);
+			       (unsigned long long)rq->sector);
 	}
 	printk(KERN_CONT "\n");
 }
diff --git a/drivers/ide/ide-park.c b/drivers/ide/ide-park.c
index 678454ac2483..c875a957596c 100644
--- a/drivers/ide/ide-park.c
+++ b/drivers/ide/ide-park.c
@@ -7,22 +7,22 @@ DECLARE_WAIT_QUEUE_HEAD(ide_park_wq);
 
 static void issue_park_cmd(ide_drive_t *drive, unsigned long timeout)
 {
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
+	ide_hwif_t *hwif = drive->hwif;
 	struct request_queue *q = drive->queue;
 	struct request *rq;
 	int rc;
 
 	timeout += jiffies;
-	spin_lock_irq(&hwgroup->lock);
+	spin_lock_irq(&hwif->lock);
 	if (drive->dev_flags & IDE_DFLAG_PARKED) {
 		int reset_timer = time_before(timeout, drive->sleep);
 		int start_queue = 0;
 
 		drive->sleep = timeout;
 		wake_up_all(&ide_park_wq);
-		if (reset_timer && del_timer(&hwgroup->timer))
+		if (reset_timer && del_timer(&hwif->timer))
 			start_queue = 1;
-		spin_unlock_irq(&hwgroup->lock);
+		spin_unlock_irq(&hwif->lock);
 
 		if (start_queue) {
 			spin_lock_irq(q->queue_lock);
@@ -31,7 +31,7 @@ static void issue_park_cmd(ide_drive_t *drive, unsigned long timeout)
 		}
 		return;
 	}
-	spin_unlock_irq(&hwgroup->lock);
+	spin_unlock_irq(&hwif->lock);
 
 	rq = blk_get_request(q, READ, __GFP_WAIT);
 	rq->cmd[0] = REQ_PARK_HEADS;
@@ -64,21 +64,21 @@ ssize_t ide_park_show(struct device *dev, struct device_attribute *attr,
 		      char *buf)
 {
 	ide_drive_t *drive = to_ide_device(dev);
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
+	ide_hwif_t *hwif = drive->hwif;
 	unsigned long now;
 	unsigned int msecs;
 
 	if (drive->dev_flags & IDE_DFLAG_NO_UNLOAD)
 		return -EOPNOTSUPP;
 
-	spin_lock_irq(&hwgroup->lock);
+	spin_lock_irq(&hwif->lock);
 	now = jiffies;
 	if (drive->dev_flags & IDE_DFLAG_PARKED &&
 	    time_after(drive->sleep, now))
 		msecs = jiffies_to_msecs(drive->sleep - now);
 	else
 		msecs = 0;
-	spin_unlock_irq(&hwgroup->lock);
+	spin_unlock_irq(&hwif->lock);
 
 	return snprintf(buf, 20, "%u\n", msecs);
 }
diff --git a/drivers/ide/ide-pm.c b/drivers/ide/ide-pm.c
index 8282c6086e6a..4b3bf6a06b70 100644
--- a/drivers/ide/ide-pm.c
+++ b/drivers/ide/ide-pm.c
@@ -5,7 +5,7 @@
 int generic_ide_suspend(struct device *dev, pm_message_t mesg)
 {
 	ide_drive_t *drive = dev->driver_data, *pair = ide_get_pair_dev(drive);
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct request *rq;
 	struct request_pm_state rqpm;
 	ide_task_t args;
@@ -39,7 +39,7 @@ int generic_ide_suspend(struct device *dev, pm_message_t mesg)
 int generic_ide_resume(struct device *dev)
 {
 	ide_drive_t *drive = dev->driver_data, *pair = ide_get_pair_dev(drive);
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct request *rq;
 	struct request_pm_state rqpm;
 	ide_task_t args;
@@ -67,7 +67,7 @@ int generic_ide_resume(struct device *dev)
 	blk_put_request(rq);
 
 	if (err == 0 && dev->driver) {
-		ide_driver_t *drv = to_ide_driver(dev->driver);
+		struct ide_driver *drv = to_ide_driver(dev->driver);
 
 		if (drv->resume)
 			drv->resume(drive);
@@ -194,7 +194,7 @@ void ide_complete_pm_request(ide_drive_t *drive, struct request *rq)
 	}
 	spin_unlock_irqrestore(q->queue_lock, flags);
 
-	drive->hwif->hwgroup->rq = NULL;
+	drive->hwif->rq = NULL;
 
 	if (blk_end_request(rq, 0, 0))
 		BUG();
diff --git a/drivers/ide/ide-probe.c b/drivers/ide/ide-probe.c
index c5adb7b9c5b5..0ccbb4459fb9 100644
--- a/drivers/ide/ide-probe.c
+++ b/drivers/ide/ide-probe.c
@@ -189,7 +189,7 @@ static void ide_classify_atapi_dev(ide_drive_t *drive)
 
 static void do_identify(ide_drive_t *drive, u8 cmd)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	u16 *id = drive->id;
 	char *m = (char *)&id[ATA_ID_PROD];
 	unsigned long flags;
@@ -266,7 +266,7 @@ err_misc:
 
 static int actual_try_to_identify (ide_drive_t *drive, u8 cmd)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct ide_io_ports *io_ports = &hwif->io_ports;
 	const struct ide_tp_ops *tp_ops = hwif->tp_ops;
 	int use_altstatus = 0, rc;
@@ -341,7 +341,7 @@ static int actual_try_to_identify (ide_drive_t *drive, u8 cmd)
  
 static int try_to_identify (ide_drive_t *drive, u8 cmd)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	const struct ide_tp_ops *tp_ops = hwif->tp_ops;
 	int retval;
 	int autoprobe = 0;
@@ -438,7 +438,7 @@ static u8 ide_read_device(ide_drive_t *drive)
 
 static int do_probe (ide_drive_t *drive, u8 cmd)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	const struct ide_tp_ops *tp_ops = hwif->tp_ops;
 	int rc;
 	u8 present = !!(drive->dev_flags & IDE_DFLAG_PRESENT), stat;
@@ -463,7 +463,7 @@ static int do_probe (ide_drive_t *drive, u8 cmd)
 	if (ide_read_device(drive) != drive->select && present == 0) {
 		if (drive->dn & 1) {
 			/* exit with drive0 selected */
-			SELECT_DRIVE(&hwif->drives[0]);
+			SELECT_DRIVE(hwif->devices[0]);
 			/* allow ATA_BUSY to assert & clear */
 			msleep(50);
 		}
@@ -509,7 +509,7 @@ static int do_probe (ide_drive_t *drive, u8 cmd)
 	}
 	if (drive->dn & 1) {
 		/* exit with drive0 selected */
-		SELECT_DRIVE(&hwif->drives[0]);
+		SELECT_DRIVE(hwif->devices[0]);
 		msleep(50);
 		/* ensure drive irq is clear */
 		(void)tp_ops->read_status(hwif);
@@ -522,7 +522,7 @@ static int do_probe (ide_drive_t *drive, u8 cmd)
  */
 static void enable_nest (ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	const struct ide_tp_ops *tp_ops = hwif->tp_ops;
 	u8 stat;
 
@@ -697,7 +697,8 @@ out:
 
 static int ide_port_wait_ready(ide_hwif_t *hwif)
 {
-	int unit, rc;
+	ide_drive_t *drive;
+	int i, rc;
 
 	printk(KERN_DEBUG "Probing IDE interface %s...\n", hwif->name);
 
@@ -714,9 +715,7 @@ static int ide_port_wait_ready(ide_hwif_t *hwif)
 		return rc;
 
 	/* Now make sure both master & slave are ready */
-	for (unit = 0; unit < MAX_DRIVES; unit++) {
-		ide_drive_t *drive = &hwif->drives[unit];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		/* Ignore disks that we will not probe for later. */
 		if ((drive->dev_flags & IDE_DFLAG_NOPROBE) == 0 ||
 		    (drive->dev_flags & IDE_DFLAG_PRESENT)) {
@@ -732,8 +731,8 @@ static int ide_port_wait_ready(ide_hwif_t *hwif)
 	}
 out:
 	/* Exit function with master reselected (let's be sane) */
-	if (unit)
-		SELECT_DRIVE(&hwif->drives[0]);
+	if (i)
+		SELECT_DRIVE(hwif->devices[0]);
 
 	return rc;
 }
@@ -749,7 +748,7 @@ out:
 
 void ide_undecoded_slave(ide_drive_t *dev1)
 {
-	ide_drive_t *dev0 = &dev1->hwif->drives[0];
+	ide_drive_t *dev0 = dev1->hwif->devices[0];
 
 	if ((dev1->dn & 1) == 0 || (dev0->dev_flags & IDE_DFLAG_PRESENT) == 0)
 		return;
@@ -778,14 +777,15 @@ EXPORT_SYMBOL_GPL(ide_undecoded_slave);
 
 static int ide_probe_port(ide_hwif_t *hwif)
 {
+	ide_drive_t *drive;
 	unsigned long flags;
 	unsigned int irqd;
-	int unit, rc = -ENODEV;
+	int i, rc = -ENODEV;
 
 	BUG_ON(hwif->present);
 
-	if ((hwif->drives[0].dev_flags & IDE_DFLAG_NOPROBE) &&
-	    (hwif->drives[1].dev_flags & IDE_DFLAG_NOPROBE))
+	if ((hwif->devices[0]->dev_flags & IDE_DFLAG_NOPROBE) &&
+	    (hwif->devices[1]->dev_flags & IDE_DFLAG_NOPROBE))
 		return -EACCES;
 
 	/*
@@ -796,7 +796,8 @@ static int ide_probe_port(ide_hwif_t *hwif)
 	if (irqd)
 		disable_irq(hwif->irq);
 
-	local_irq_set(flags);
+	local_irq_save(flags);
+	local_irq_enable_in_hardirq();
 
 	if (ide_port_wait_ready(hwif) == -EBUSY)
 		printk(KERN_DEBUG "%s: Wait for ready failed before probe !\n", hwif->name);
@@ -805,9 +806,7 @@ static int ide_probe_port(ide_hwif_t *hwif)
 	 * Second drive should only exist if first drive was found,
 	 * but a lot of cdrom drives are configured as single slaves.
 	 */
-	for (unit = 0; unit < MAX_DRIVES; ++unit) {
-		ide_drive_t *drive = &hwif->drives[unit];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		(void) probe_for_drive(drive);
 		if (drive->dev_flags & IDE_DFLAG_PRESENT)
 			rc = 0;
@@ -828,20 +827,17 @@ static int ide_probe_port(ide_hwif_t *hwif)
 static void ide_port_tune_devices(ide_hwif_t *hwif)
 {
 	const struct ide_port_ops *port_ops = hwif->port_ops;
-	int unit;
-
-	for (unit = 0; unit < MAX_DRIVES; unit++) {
-		ide_drive_t *drive = &hwif->drives[unit];
+	ide_drive_t *drive;
+	int i;
 
+	ide_port_for_each_dev(i, drive, hwif) {
 		if (drive->dev_flags & IDE_DFLAG_PRESENT) {
 			if (port_ops && port_ops->quirkproc)
 				port_ops->quirkproc(drive);
 		}
 	}
 
-	for (unit = 0; unit < MAX_DRIVES; ++unit) {
-		ide_drive_t *drive = &hwif->drives[unit];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		if (drive->dev_flags & IDE_DFLAG_PRESENT) {
 			ide_set_max_pio(drive);
 
@@ -852,11 +848,8 @@ static void ide_port_tune_devices(ide_hwif_t *hwif)
 		}
 	}
 
-	for (unit = 0; unit < MAX_DRIVES; ++unit) {
-		ide_drive_t *drive = &hwif->drives[unit];
-
-		if ((hwif->host_flags & IDE_HFLAG_NO_IO_32BIT) ||
-		    drive->id[ATA_ID_DWORD_IO])
+	ide_port_for_each_dev(i, drive, hwif) {
+		if (hwif->host_flags & IDE_HFLAG_NO_IO_32BIT)
 			drive->dev_flags |= IDE_DFLAG_NO_IO_32BIT;
 		else
 			drive->dev_flags &= ~IDE_DFLAG_NO_IO_32BIT;
@@ -869,7 +862,7 @@ static void ide_port_tune_devices(ide_hwif_t *hwif)
 static int ide_init_queue(ide_drive_t *drive)
 {
 	struct request_queue *q;
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	int max_sectors = 256;
 	int max_sg_entries = PRD_ENTRIES;
 
@@ -918,36 +911,19 @@ static int ide_init_queue(ide_drive_t *drive)
 	return 0;
 }
 
-static void ide_add_drive_to_hwgroup(ide_drive_t *drive)
-{
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
-
-	spin_lock_irq(&hwgroup->lock);
-	if (!hwgroup->drive) {
-		/* first drive for hwgroup. */
-		drive->next = drive;
-		hwgroup->drive = drive;
-		hwgroup->hwif = HWIF(hwgroup->drive);
-	} else {
-		drive->next = hwgroup->drive->next;
-		hwgroup->drive->next = drive;
-	}
-	spin_unlock_irq(&hwgroup->lock);
-}
+static DEFINE_MUTEX(ide_cfg_mtx);
 
 /*
  * For any present drive:
  * - allocate the block device queue
- * - link drive into the hwgroup
  */
 static int ide_port_setup_devices(ide_hwif_t *hwif)
 {
+	ide_drive_t *drive;
 	int i, j = 0;
 
 	mutex_lock(&ide_cfg_mtx);
-	for (i = 0; i < MAX_DRIVES; i++) {
-		ide_drive_t *drive = &hwif->drives[i];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		if ((drive->dev_flags & IDE_DFLAG_PRESENT) == 0)
 			continue;
 
@@ -961,139 +937,39 @@ static int ide_port_setup_devices(ide_hwif_t *hwif)
 		}
 
 		j++;
-
-		ide_add_drive_to_hwgroup(drive);
 	}
 	mutex_unlock(&ide_cfg_mtx);
 
 	return j;
 }
 
-static ide_hwif_t *ide_ports[MAX_HWIFS];
-
-void ide_remove_port_from_hwgroup(ide_hwif_t *hwif)
-{
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
-
-	ide_ports[hwif->index] = NULL;
-
-	spin_lock_irq(&hwgroup->lock);
-	/*
-	 * Remove us from the hwgroup, and free
-	 * the hwgroup if we were the only member
-	 */
-	if (hwif->next == hwif) {
-		BUG_ON(hwgroup->hwif != hwif);
-		kfree(hwgroup);
-	} else {
-		/* There is another interface in hwgroup.
-		 * Unlink us, and set hwgroup->drive and ->hwif to
-		 * something sane.
-		 */
-		ide_hwif_t *g = hwgroup->hwif;
-
-		while (g->next != hwif)
-			g = g->next;
-		g->next = hwif->next;
-		if (hwgroup->hwif == hwif) {
-			/* Chose a random hwif for hwgroup->hwif.
-			 * It's guaranteed that there are no drives
-			 * left in the hwgroup.
-			 */
-			BUG_ON(hwgroup->drive != NULL);
-			hwgroup->hwif = g;
-		}
-		BUG_ON(hwgroup->hwif == hwif);
-	}
-	spin_unlock_irq(&hwgroup->lock);
-}
-
 /*
- * This routine sets up the irq for an ide interface, and creates a new
- * hwgroup for the irq/hwif if none was previously assigned.
- *
- * Much of the code is for correctly detecting/handling irq sharing
- * and irq serialization situations.  This is somewhat complex because
- * it handles static as well as dynamic (PCMCIA) IDE interfaces.
+ * This routine sets up the IRQ for an IDE interface.
  */
 static int init_irq (ide_hwif_t *hwif)
 {
 	struct ide_io_ports *io_ports = &hwif->io_ports;
-	unsigned int index;
-	ide_hwgroup_t *hwgroup;
-	ide_hwif_t *match = NULL;
+	int sa = 0;
 
 	mutex_lock(&ide_cfg_mtx);
-	hwif->hwgroup = NULL;
+	spin_lock_init(&hwif->lock);
 
-	for (index = 0; index < MAX_HWIFS; index++) {
-		ide_hwif_t *h = ide_ports[index];
+	init_timer(&hwif->timer);
+	hwif->timer.function = &ide_timer_expiry;
+	hwif->timer.data = (unsigned long)hwif;
 
-		if (h && h->hwgroup) {  /* scan only initialized ports */
-			if (hwif->host->host_flags & IDE_HFLAG_SERIALIZE) {
-				if (hwif->host == h->host)
-					match = h;
-			}
-		}
-	}
-
-	/*
-	 * If we are still without a hwgroup, then form a new one
-	 */
-	if (match) {
-		hwgroup = match->hwgroup;
-		hwif->hwgroup = hwgroup;
-		/*
-		 * Link us into the hwgroup.
-		 * This must be done early, do ensure that unexpected_intr
-		 * can find the hwif and prevent irq storms.
-		 * No drives are attached to the new hwif, choose_drive
-		 * can't do anything stupid (yet).
-		 * Add ourself as the 2nd entry to the hwgroup->hwif
-		 * linked list, the first entry is the hwif that owns
-		 * hwgroup->handler - do not change that.
-		 */
-		spin_lock_irq(&hwgroup->lock);
-		hwif->next = hwgroup->hwif->next;
-		hwgroup->hwif->next = hwif;
-		BUG_ON(hwif->next == hwif);
-		spin_unlock_irq(&hwgroup->lock);
-	} else {
-		hwgroup = kmalloc_node(sizeof(*hwgroup), GFP_KERNEL|__GFP_ZERO,
-				       hwif_to_node(hwif));
-		if (hwgroup == NULL)
-			goto out_up;
-
-		spin_lock_init(&hwgroup->lock);
-
-		hwif->hwgroup = hwgroup;
-		hwgroup->hwif = hwif->next = hwif;
-
-		init_timer(&hwgroup->timer);
-		hwgroup->timer.function = &ide_timer_expiry;
-		hwgroup->timer.data = (unsigned long) hwgroup;
-	}
-
-	ide_ports[hwif->index] = hwif;
-
-	/*
-	 * Allocate the irq, if not already obtained for another hwif
-	 */
-	if (!match || match->irq != hwif->irq) {
-		int sa = 0;
 #if defined(__mc68000__)
-		sa = IRQF_SHARED;
+	sa = IRQF_SHARED;
 #endif /* __mc68000__ */
 
-		if (hwif->chipset == ide_pci)
-			sa = IRQF_SHARED;
+	if (hwif->chipset == ide_pci)
+		sa = IRQF_SHARED;
 
-		if (io_ports->ctl_addr)
-			hwif->tp_ops->set_irq(hwif, 1);
+	if (io_ports->ctl_addr)
+		hwif->tp_ops->set_irq(hwif, 1);
 
-		if (request_irq(hwif->irq,&ide_intr,sa,hwif->name,hwgroup))
-	       		goto out_unlink;
-	}
+	if (request_irq(hwif->irq, &ide_intr, sa, hwif->name, hwif))
+		goto out_up;
 
 	if (!hwif->rqsize) {
 		if ((hwif->host_flags & IDE_HFLAG_NO_LBA48) ||
@@ -1111,14 +987,12 @@ static int init_irq (ide_hwif_t *hwif)
 	printk(KERN_INFO "%s at 0x%08lx on irq %d", hwif->name,
 		io_ports->data_addr, hwif->irq);
 #endif /* __mc68000__ */
-	if (match)
-		printk(KERN_CONT " (serialized with %s)", match->name);
+	if (hwif->host->host_flags & IDE_HFLAG_SERIALIZE)
+		printk(KERN_CONT " (serialized)");
 	printk(KERN_CONT "\n");
 
 	mutex_unlock(&ide_cfg_mtx);
 	return 0;
-out_unlink:
-	ide_remove_port_from_hwgroup(hwif);
 out_up:
 	mutex_unlock(&ide_cfg_mtx);
 	return 1;
@@ -1134,7 +1008,7 @@ static struct kobject *ata_probe(dev_t dev, int *part, void *data)
 {
 	ide_hwif_t *hwif = data;
 	int unit = *part >> PARTN_BITS;
-	ide_drive_t *drive = &hwif->drives[unit];
+	ide_drive_t *drive = hwif->devices[unit];
 
 	if ((drive->dev_flags & IDE_DFLAG_PRESENT) == 0)
 		return NULL;
@@ -1196,47 +1070,23 @@ void ide_init_disk(struct gendisk *disk, ide_drive_t *drive)
 
 EXPORT_SYMBOL_GPL(ide_init_disk);
 
-static void ide_remove_drive_from_hwgroup(ide_drive_t *drive)
-{
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
-
-	if (drive == drive->next) {
-		/* special case: last drive from hwgroup. */
-		BUG_ON(hwgroup->drive != drive);
-		hwgroup->drive = NULL;
-	} else {
-		ide_drive_t *walk;
-
-		walk = hwgroup->drive;
-		while (walk->next != drive)
-			walk = walk->next;
-		walk->next = drive->next;
-		if (hwgroup->drive == drive) {
-			hwgroup->drive = drive->next;
-			hwgroup->hwif = hwgroup->drive->hwif;
-		}
-	}
-	BUG_ON(hwgroup->drive == drive);
-}
-
 static void drive_release_dev (struct device *dev)
 {
 	ide_drive_t *drive = container_of(dev, ide_drive_t, gendev);
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
+	ide_hwif_t *hwif = drive->hwif;
 
 	ide_proc_unregister_device(drive);
 
-	spin_lock_irq(&hwgroup->lock);
-	ide_remove_drive_from_hwgroup(drive);
+	spin_lock_irq(&hwif->lock);
 	kfree(drive->id);
 	drive->id = NULL;
 	drive->dev_flags &= ~IDE_DFLAG_PRESENT;
 	/* Messed up locking ... */
-	spin_unlock_irq(&hwgroup->lock);
+	spin_unlock_irq(&hwif->lock);
 	blk_cleanup_queue(drive->queue);
-	spin_lock_irq(&hwgroup->lock);
+	spin_lock_irq(&hwif->lock);
 	drive->queue = NULL;
-	spin_unlock_irq(&hwgroup->lock);
+	spin_unlock_irq(&hwif->lock);
 
 	complete(&drive->gendev_rel_comp);
 }
@@ -1302,10 +1152,10 @@ out:
 
 static void hwif_register_devices(ide_hwif_t *hwif)
 {
+	ide_drive_t *drive;
 	unsigned int i;
 
-	for (i = 0; i < MAX_DRIVES; i++) {
-		ide_drive_t *drive = &hwif->drives[i];
+	ide_port_for_each_dev(i, drive, hwif) {
 		struct device *dev = &drive->gendev;
 		int ret;
 
@@ -1328,11 +1178,10 @@ static void hwif_register_devices(ide_hwif_t *hwif)
 static void ide_port_init_devices(ide_hwif_t *hwif)
 {
 	const struct ide_port_ops *port_ops = hwif->port_ops;
+	ide_drive_t *drive;
 	int i;
 
-	for (i = 0; i < MAX_DRIVES; i++) {
-		ide_drive_t *drive = &hwif->drives[i];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		drive->dn = i + hwif->channel * 2;
 
 		if (hwif->host_flags & IDE_HFLAG_IO_32BIT)
@@ -1380,6 +1229,8 @@ static void ide_init_port(ide_hwif_t *hwif, unsigned int port,
 	if ((d->host_flags & IDE_HFLAG_NO_DMA) == 0) {
 		int rc;
 
+		hwif->dma_ops = d->dma_ops;
+
 		if (d->init_dma)
 			rc = d->init_dma(hwif, d);
 		else
@@ -1387,12 +1238,13 @@ static void ide_init_port(ide_hwif_t *hwif, unsigned int port,
 
 		if (rc < 0) {
 			printk(KERN_INFO "%s: DMA disabled\n", hwif->name);
+
+			hwif->dma_ops = NULL;
 			hwif->dma_base = 0;
 			hwif->swdma_mask = 0;
 			hwif->mwdma_mask = 0;
 			hwif->ultra_mask = 0;
-		} else if (d->dma_ops)
-			hwif->dma_ops = d->dma_ops;
+		}
 	}
 
 	if ((d->host_flags & IDE_HFLAG_SERIALIZE) ||
@@ -1417,6 +1269,66 @@ static void ide_port_cable_detect(ide_hwif_t *hwif)
 	}
 }
 
+static const u8 ide_hwif_to_major[] =
+	{ IDE0_MAJOR, IDE1_MAJOR, IDE2_MAJOR, IDE3_MAJOR, IDE4_MAJOR,
+	  IDE5_MAJOR, IDE6_MAJOR, IDE7_MAJOR, IDE8_MAJOR, IDE9_MAJOR };
+
+static void ide_port_init_devices_data(ide_hwif_t *hwif)
+{
+	ide_drive_t *drive;
+	int i;
+
+	ide_port_for_each_dev(i, drive, hwif) {
+		u8 j = (hwif->index * MAX_DRIVES) + i;
+
+		memset(drive, 0, sizeof(*drive));
+
+		drive->media			= ide_disk;
+		drive->select			= (i << 4) | ATA_DEVICE_OBS;
+		drive->hwif			= hwif;
+		drive->ready_stat		= ATA_DRDY;
+		drive->bad_wstat		= BAD_W_STAT;
+		drive->special.b.recalibrate	= 1;
+		drive->special.b.set_geometry	= 1;
+		drive->name[0]			= 'h';
+		drive->name[1]			= 'd';
+		drive->name[2]			= 'a' + j;
+		drive->max_failures		= IDE_DEFAULT_MAX_FAILURES;
+
+		INIT_LIST_HEAD(&drive->list);
+		init_completion(&drive->gendev_rel_comp);
+	}
+}
+
+static void ide_init_port_data(ide_hwif_t *hwif, unsigned int index)
+{
+	/* fill in any non-zero initial values */
+	hwif->index	= index;
+	hwif->major	= ide_hwif_to_major[index];
+
+	hwif->name[0]	= 'i';
+	hwif->name[1]	= 'd';
+	hwif->name[2]	= 'e';
+	hwif->name[3]	= '0' + index;
+
+	init_completion(&hwif->gendev_rel_comp);
+
+	hwif->tp_ops = &default_tp_ops;
+
+	ide_port_init_devices_data(hwif);
+}
+
+static void ide_init_port_hw(ide_hwif_t *hwif, hw_regs_t *hw)
+{
+	memcpy(&hwif->io_ports, &hw->io_ports, sizeof(hwif->io_ports));
+	hwif->irq = hw->irq;
+	hwif->chipset = hw->chipset;
+	hwif->dev = hw->dev;
+	hwif->gendev.parent = hw->parent ? hw->parent : hw->dev;
+	hwif->ack_intr = hw->ack_intr;
+	hwif->config_data = hw->config;
+}
+
 static unsigned int ide_indexes;
 
 /**
@@ -1466,12 +1378,43 @@ static void ide_free_port_slot(int idx)
 	mutex_unlock(&ide_cfg_mtx);
 }
 
+static void ide_port_free_devices(ide_hwif_t *hwif)
+{
+	ide_drive_t *drive;
+	int i;
+
+	ide_port_for_each_dev(i, drive, hwif)
+		kfree(drive);
+}
+
+static int ide_port_alloc_devices(ide_hwif_t *hwif, int node)
+{
+	int i;
+
+	for (i = 0; i < MAX_DRIVES; i++) {
+		ide_drive_t *drive;
+
+		drive = kzalloc_node(sizeof(*drive), GFP_KERNEL, node);
+		if (drive == NULL)
+			goto out_nomem;
+
+		hwif->devices[i] = drive;
+	}
+	return 0;
+
+out_nomem:
+	ide_port_free_devices(hwif);
+	return -ENOMEM;
+}
+
 struct ide_host *ide_host_alloc(const struct ide_port_info *d, hw_regs_t **hws)
 {
 	struct ide_host *host;
+	struct device *dev = hws[0] ? hws[0]->dev : NULL;
+	int node = dev ? dev_to_node(dev) : -1;
 	int i;
 
-	host = kzalloc(sizeof(*host), GFP_KERNEL);
+	host = kzalloc_node(sizeof(*host), GFP_KERNEL, node);
 	if (host == NULL)
 		return NULL;
 
@@ -1482,10 +1425,15 @@ struct ide_host *ide_host_alloc(const struct ide_port_info *d, hw_regs_t **hws)
 		if (hws[i] == NULL)
 			continue;
 
-		hwif = kzalloc(sizeof(*hwif), GFP_KERNEL);
+		hwif = kzalloc_node(sizeof(*hwif), GFP_KERNEL, node);
 		if (hwif == NULL)
 			continue;
 
+		if (ide_port_alloc_devices(hwif, node) < 0) {
+			kfree(hwif);
+			continue;
+		}
+
 		idx = ide_find_port_slot(d);
 		if (idx < 0) {
 			printk(KERN_ERR "%s: no free slot for interface\n",
@@ -1507,8 +1455,7 @@ struct ide_host *ide_host_alloc(const struct ide_port_info *d, hw_regs_t **hws)
 		return NULL;
 	}
 
-	if (hws[0])
-		host->dev[0] = hws[0]->dev;
+	host->dev[0] = dev;
 
 	if (d) {
 		host->init_chipset = d->init_chipset;
@@ -1525,9 +1472,7 @@ int ide_host_register(struct ide_host *host, const struct ide_port_info *d,
 	ide_hwif_t *hwif, *mate = NULL;
 	int i, j = 0;
 
-	for (i = 0; i < MAX_HOST_PORTS; i++) {
-		hwif = host->ports[i];
-
+	ide_host_for_each_port(i, hwif, host) {
 		if (hwif == NULL) {
 			mate = NULL;
 			continue;
@@ -1553,9 +1498,7 @@ int ide_host_register(struct ide_host *host, const struct ide_port_info *d,
 		ide_port_init_devices(hwif);
 	}
 
-	for (i = 0; i < MAX_HOST_PORTS; i++) {
-		hwif = host->ports[i];
-
+	ide_host_for_each_port(i, hwif, host) {
 		if (hwif == NULL)
 			continue;
 
@@ -1570,9 +1513,7 @@ int ide_host_register(struct ide_host *host, const struct ide_port_info *d,
 			ide_port_tune_devices(hwif);
 	}
 
-	for (i = 0; i < MAX_HOST_PORTS; i++) {
-		hwif = host->ports[i];
-
+	ide_host_for_each_port(i, hwif, host) {
 		if (hwif == NULL)
 			continue;
 
@@ -1597,9 +1538,7 @@ int ide_host_register(struct ide_host *host, const struct ide_port_info *d,
 			ide_acpi_port_init_devices(hwif);
 	}
 
-	for (i = 0; i < MAX_HOST_PORTS; i++) {
-		hwif = host->ports[i];
-
+	ide_host_for_each_port(i, hwif, host) {
 		if (hwif == NULL)
 			continue;
 
@@ -1607,9 +1546,7 @@ int ide_host_register(struct ide_host *host, const struct ide_port_info *d,
 			hwif_register_devices(hwif);
 	}
 
-	for (i = 0; i < MAX_HOST_PORTS; i++) {
-		hwif = host->ports[i];
-
+	ide_host_for_each_port(i, hwif, host) {
 		if (hwif == NULL)
 			continue;
 
@@ -1647,17 +1584,85 @@ int ide_host_add(const struct ide_port_info *d, hw_regs_t **hws,
 }
 EXPORT_SYMBOL_GPL(ide_host_add);
 
+static void __ide_port_unregister_devices(ide_hwif_t *hwif)
+{
+	ide_drive_t *drive;
+	int i;
+
+	ide_port_for_each_dev(i, drive, hwif) {
+		if (drive->dev_flags & IDE_DFLAG_PRESENT) {
+			device_unregister(&drive->gendev);
+			wait_for_completion(&drive->gendev_rel_comp);
+		}
+	}
+}
+
+void ide_port_unregister_devices(ide_hwif_t *hwif)
+{
+	mutex_lock(&ide_cfg_mtx);
+	__ide_port_unregister_devices(hwif);
+	hwif->present = 0;
+	ide_port_init_devices_data(hwif);
+	mutex_unlock(&ide_cfg_mtx);
+}
+EXPORT_SYMBOL_GPL(ide_port_unregister_devices);
+
+/**
+ *	ide_unregister		-	free an IDE interface
+ *	@hwif: IDE interface
+ *
+ *	Perform the final unregister of an IDE interface.
+ *
+ *	Locking:
+ *	The caller must not hold the IDE locks.
+ *
+ *	It is up to the caller to be sure there is no pending I/O here,
+ *	and that the interface will not be reopened (present/vanishing
+ *	locking isn't yet done BTW).
+ */
+
+static void ide_unregister(ide_hwif_t *hwif)
+{
+	BUG_ON(in_interrupt());
+	BUG_ON(irqs_disabled());
+
+	mutex_lock(&ide_cfg_mtx);
+
+	if (hwif->present) {
+		__ide_port_unregister_devices(hwif);
+		hwif->present = 0;
+	}
+
+	ide_proc_unregister_port(hwif);
+
+	free_irq(hwif->irq, hwif);
+
+	device_unregister(hwif->portdev);
+	device_unregister(&hwif->gendev);
+	wait_for_completion(&hwif->gendev_rel_comp);
+
+	/*
+	 * Remove us from the kernel's knowledge
+	 */
+	blk_unregister_region(MKDEV(hwif->major, 0), MAX_DRIVES<<PARTN_BITS);
+	kfree(hwif->sg_table);
+	unregister_blkdev(hwif->major, hwif->name);
+
+	ide_release_dma_engine(hwif);
+
+	mutex_unlock(&ide_cfg_mtx);
+}
+
 void ide_host_free(struct ide_host *host)
 {
 	ide_hwif_t *hwif;
 	int i;
 
-	for (i = 0; i < MAX_HOST_PORTS; i++) {
-		hwif = host->ports[i];
-
+	ide_host_for_each_port(i, hwif, host) {
 		if (hwif == NULL)
 			continue;
 
+		ide_port_free_devices(hwif);
 		ide_free_port_slot(hwif->index);
 		kfree(hwif);
 	}
@@ -1668,11 +1673,12 @@ EXPORT_SYMBOL_GPL(ide_host_free);
 
 void ide_host_remove(struct ide_host *host)
 {
+	ide_hwif_t *hwif;
 	int i;
 
-	for (i = 0; i < MAX_HOST_PORTS; i++) {
-		if (host->ports[i])
-			ide_unregister(host->ports[i]);
+	ide_host_for_each_port(i, hwif, host) {
+		if (hwif)
+			ide_unregister(hwif);
 	}
 
 	ide_host_free(host);
@@ -1691,8 +1697,8 @@ void ide_port_scan(ide_hwif_t *hwif)
 	hwif->present = 1;
 
 	ide_port_tune_devices(hwif);
-	ide_acpi_port_init_devices(hwif);
 	ide_port_setup_devices(hwif);
+	ide_acpi_port_init_devices(hwif);
 	hwif_register_devices(hwif);
 	ide_proc_port_register_devices(hwif);
 }
diff --git a/drivers/ide/ide-proc.c b/drivers/ide/ide-proc.c
index a14e2938e4f3..1d8978b3314a 100644
--- a/drivers/ide/ide-proc.c
+++ b/drivers/ide/ide-proc.c
@@ -439,13 +439,13 @@ static int proc_ide_read_dmodel
 static int proc_ide_read_driver
 	(char *page, char **start, off_t off, int count, int *eof, void *data)
 {
-	ide_drive_t	*drive = (ide_drive_t *) data;
-	struct device	*dev = &drive->gendev;
-	ide_driver_t	*ide_drv;
-	int		len;
+	ide_drive_t		*drive = (ide_drive_t *)data;
+	struct device		*dev = &drive->gendev;
+	struct ide_driver	*ide_drv;
+	int			len;
 
 	if (dev->driver) {
-		ide_drv = container_of(dev->driver, ide_driver_t, gen_driver);
+		ide_drv = to_ide_driver(dev->driver);
 		len = sprintf(page, "%s version %s\n",
 				dev->driver->name, ide_drv->version);
 	} else
@@ -555,7 +555,7 @@ static void ide_remove_proc_entries(struct proc_dir_entry *dir, ide_proc_entry_t
 	}
 }
 
-void ide_proc_register_driver(ide_drive_t *drive, ide_driver_t *driver)
+void ide_proc_register_driver(ide_drive_t *drive, struct ide_driver *driver)
 {
 	mutex_lock(&ide_setting_mtx);
 	drive->settings = driver->proc_devsets(drive);
@@ -577,7 +577,7 @@ EXPORT_SYMBOL(ide_proc_register_driver);
  *	Takes ide_setting_mtx.
  */
 
-void ide_proc_unregister_driver(ide_drive_t *drive, ide_driver_t *driver)
+void ide_proc_unregister_driver(ide_drive_t *drive, struct ide_driver *driver)
 {
 	ide_remove_proc_entries(drive->proc, driver->proc_entries(drive));
 
@@ -593,14 +593,13 @@ EXPORT_SYMBOL(ide_proc_unregister_driver);
 
 void ide_proc_port_register_devices(ide_hwif_t *hwif)
 {
-	int	d;
 	struct proc_dir_entry *ent;
 	struct proc_dir_entry *parent = hwif->proc;
+	ide_drive_t *drive;
 	char name[64];
+	int i;
 
-	for (d = 0; d < MAX_DRIVES; d++) {
-		ide_drive_t *drive = &hwif->drives[d];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		if ((drive->dev_flags & IDE_DFLAG_PRESENT) == 0 || drive->proc)
 			continue;
 
@@ -653,7 +652,7 @@ void ide_proc_unregister_port(ide_hwif_t *hwif)
 
 static int proc_print_driver(struct device_driver *drv, void *data)
 {
-	ide_driver_t *ide_drv = container_of(drv, ide_driver_t, gen_driver);
+	struct ide_driver *ide_drv = to_ide_driver(drv);
 	struct seq_file *s = data;
 
 	seq_printf(s, "%s version %s\n", drv->name, ide_drv->version);
diff --git a/drivers/ide/ide-tape.c b/drivers/ide/ide-tape.c
index 5d2aa22cd6e4..d7ecd3c79757 100644
--- a/drivers/ide/ide-tape.c
+++ b/drivers/ide/ide-tape.c
@@ -166,10 +166,10 @@ struct idetape_bh {
  * to an interrupt or a timer event is stored in the struct defined below.
  */
 typedef struct ide_tape_obj {
-	ide_drive_t	*drive;
-	ide_driver_t	*driver;
-	struct gendisk	*disk;
-	struct kref	kref;
+	ide_drive_t		*drive;
+	struct ide_driver	*driver;
+	struct gendisk		*disk;
+	struct kref		kref;
 
 	/*
 	 *	failed_pc points to the last failed packet command, or contains
@@ -479,7 +479,7 @@ static void ide_tape_kfree_buffer(idetape_tape_t *tape)
 
 static int idetape_end_request(ide_drive_t *drive, int uptodate, int nr_sects)
 {
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = drive->hwif->rq;
 	idetape_tape_t *tape = drive->driver_data;
 	unsigned long flags;
 	int error;
@@ -531,7 +531,7 @@ static void ide_tape_callback(ide_drive_t *drive, int dsc)
 			printk(KERN_ERR "ide-tape: Error in REQUEST SENSE "
 					"itself - Aborting request!\n");
 	} else if (pc->c[0] == READ_6 || pc->c[0] == WRITE_6) {
-		struct request *rq = drive->hwif->hwgroup->rq;
+		struct request *rq = drive->hwif->rq;
 		int blocks = pc->xferred / tape->blk_size;
 
 		tape->avg_size += blocks * tape->blk_size;
@@ -576,7 +576,7 @@ static void ide_tape_callback(ide_drive_t *drive, int dsc)
 
 /*
  * Postpone the current request so that ide.c will be able to service requests
- * from another device on the same hwgroup while we are polling for DSC.
+ * from another device on the same port while we are polling for DSC.
  */
 static void idetape_postpone_request(ide_drive_t *drive)
 {
@@ -584,7 +584,8 @@ static void idetape_postpone_request(ide_drive_t *drive)
 
 	debug_log(DBG_PROCS, "Enter %s\n", __func__);
 
-	tape->postponed_rq = HWGROUP(drive)->rq;
+	tape->postponed_rq = drive->hwif->rq;
+
 	ide_stall_queue(drive, tape->dsc_poll_freq);
 }
 
@@ -2312,7 +2313,7 @@ static const struct ide_proc_devset *ide_tape_proc_devsets(ide_drive_t *drive)
 
 static int ide_tape_probe(ide_drive_t *);
 
-static ide_driver_t idetape_driver = {
+static struct ide_driver idetape_driver = {
 	.gen_driver = {
 		.owner		= THIS_MODULE,
 		.name		= "ide-tape",
@@ -2323,7 +2324,6 @@ static ide_driver_t idetape_driver = {
 	.version		= IDETAPE_VERSION,
 	.do_request		= idetape_do_request,
 	.end_request		= idetape_end_request,
-	.error			= __ide_error,
 #ifdef CONFIG_IDE_PROC_FS
 	.proc_entries		= ide_tape_proc_entries,
 	.proc_devsets		= ide_tape_proc_devsets,
diff --git a/drivers/ide/ide-taskfile.c b/drivers/ide/ide-taskfile.c
index bf4fb9d8d176..16138bce84a7 100644
--- a/drivers/ide/ide-taskfile.c
+++ b/drivers/ide/ide-taskfile.c
@@ -58,7 +58,7 @@ static ide_startstop_t task_in_intr(ide_drive_t *);
 
 ide_startstop_t do_rw_taskfile (ide_drive_t *drive, ide_task_t *task)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct ide_taskfile *tf = &task->tf;
 	ide_handler_t *handler = NULL;
 	const struct ide_tp_ops *tp_ops = hwif->tp_ops;
@@ -309,9 +309,9 @@ static ide_startstop_t task_error(ide_drive_t *drive, struct request *rq,
 		}
 
 		if (sectors > 0) {
-			ide_driver_t *drv;
+			struct ide_driver *drv;
 
-			drv = *(ide_driver_t **)rq->rq_disk->private_data;
+			drv = *(struct ide_driver **)rq->rq_disk->private_data;
 			drv->end_request(drive, 1, sectors);
 		}
 	}
@@ -328,9 +328,9 @@ void task_end_request(ide_drive_t *drive, struct request *rq, u8 stat)
 	}
 
 	if (rq->rq_disk) {
-		ide_driver_t *drv;
+		struct ide_driver *drv;
 
-		drv = *(ide_driver_t **)rq->rq_disk->private_data;;
+		drv = *(struct ide_driver **)rq->rq_disk->private_data;;
 		drv->end_request(drive, 1, rq->nr_sectors);
 	} else
 		ide_end_request(drive, 1, rq->nr_sectors);
@@ -361,7 +361,7 @@ static ide_startstop_t task_in_unexpected(ide_drive_t *drive, struct request *rq
 static ide_startstop_t task_in_intr(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = hwif->hwgroup->rq;
+	struct request *rq = hwif->rq;
 	u8 stat = hwif->tp_ops->read_status(hwif);
 
 	/* Error? */
@@ -395,7 +395,7 @@ static ide_startstop_t task_in_intr(ide_drive_t *drive)
 static ide_startstop_t task_out_intr (ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = hwif->rq;
 	u8 stat = hwif->tp_ops->read_status(hwif);
 
 	if (!OK_STAT(stat, DRIVE_READY, drive->bad_wstat))
diff --git a/drivers/ide/ide.c b/drivers/ide/ide.c
index 46a2d4ca812b..258805da15c3 100644
--- a/drivers/ide/ide.c
+++ b/drivers/ide/ide.c
@@ -60,179 +60,8 @@
 #include <linux/completion.h>
 #include <linux/device.h>
 
-
-/* default maximum number of failures */
-#define IDE_DEFAULT_MAX_FAILURES 	1
-
 struct class *ide_port_class;
 
-static const u8 ide_hwif_to_major[] = { IDE0_MAJOR, IDE1_MAJOR,
-					IDE2_MAJOR, IDE3_MAJOR,
-					IDE4_MAJOR, IDE5_MAJOR,
-					IDE6_MAJOR, IDE7_MAJOR,
-					IDE8_MAJOR, IDE9_MAJOR };
-
-DEFINE_MUTEX(ide_cfg_mtx);
-
-static void ide_port_init_devices_data(ide_hwif_t *);
-
-/*
- * Do not even *think* about calling this!
- */
-void ide_init_port_data(ide_hwif_t *hwif, unsigned int index)
-{
-	/* bulk initialize hwif & drive info with zeros */
-	memset(hwif, 0, sizeof(ide_hwif_t));
-
-	/* fill in any non-zero initial values */
-	hwif->index	= index;
-	hwif->major	= ide_hwif_to_major[index];
-
-	hwif->name[0]	= 'i';
-	hwif->name[1]	= 'd';
-	hwif->name[2]	= 'e';
-	hwif->name[3]	= '0' + index;
-
-	init_completion(&hwif->gendev_rel_comp);
-
-	hwif->tp_ops = &default_tp_ops;
-
-	ide_port_init_devices_data(hwif);
-}
-
-static void ide_port_init_devices_data(ide_hwif_t *hwif)
-{
-	int unit;
-
-	for (unit = 0; unit < MAX_DRIVES; ++unit) {
-		ide_drive_t *drive = &hwif->drives[unit];
-		u8 j = (hwif->index * MAX_DRIVES) + unit;
-
-		memset(drive, 0, sizeof(*drive));
-
-		drive->media			= ide_disk;
-		drive->select			= (unit << 4) | ATA_DEVICE_OBS;
-		drive->hwif			= hwif;
-		drive->ready_stat		= ATA_DRDY;
-		drive->bad_wstat		= BAD_W_STAT;
-		drive->special.b.recalibrate	= 1;
-		drive->special.b.set_geometry	= 1;
-		drive->name[0]			= 'h';
-		drive->name[1]			= 'd';
-		drive->name[2]			= 'a' + j;
-		drive->max_failures		= IDE_DEFAULT_MAX_FAILURES;
-
-		INIT_LIST_HEAD(&drive->list);
-		init_completion(&drive->gendev_rel_comp);
-	}
-}
-
-static void __ide_port_unregister_devices(ide_hwif_t *hwif)
-{
-	int i;
-
-	for (i = 0; i < MAX_DRIVES; i++) {
-		ide_drive_t *drive = &hwif->drives[i];
-
-		if (drive->dev_flags & IDE_DFLAG_PRESENT) {
-			device_unregister(&drive->gendev);
-			wait_for_completion(&drive->gendev_rel_comp);
-		}
-	}
-}
-
-void ide_port_unregister_devices(ide_hwif_t *hwif)
-{
-	mutex_lock(&ide_cfg_mtx);
-	__ide_port_unregister_devices(hwif);
-	hwif->present = 0;
-	ide_port_init_devices_data(hwif);
-	mutex_unlock(&ide_cfg_mtx);
-}
-EXPORT_SYMBOL_GPL(ide_port_unregister_devices);
-
-/**
- *	ide_unregister		-	free an IDE interface
- *	@hwif: IDE interface
- *
- *	Perform the final unregister of an IDE interface. At the moment
- *	we don't refcount interfaces so this will also get split up.
- *
- *	Locking:
- *	The caller must not hold the IDE locks
- *	The drive present/vanishing is not yet properly locked
- *	Take care with the callbacks. These have been split to avoid
- *	deadlocking the IDE layer. The shutdown callback is called
- *	before we take the lock and free resources. It is up to the
- *	caller to be sure there is no pending I/O here, and that
- *	the interface will not be reopened (present/vanishing locking
- *	isn't yet done BTW). After we commit to the final kill we
- *	call the cleanup callback with the ide locks held.
- *
- *	Unregister restores the hwif structures to the default state.
- *	This is raving bonkers.
- */
-
-void ide_unregister(ide_hwif_t *hwif)
-{
-	ide_hwif_t *g;
-	ide_hwgroup_t *hwgroup;
-	int irq_count = 0;
-
-	BUG_ON(in_interrupt());
-	BUG_ON(irqs_disabled());
-
-	mutex_lock(&ide_cfg_mtx);
-
-	if (hwif->present) {
-		__ide_port_unregister_devices(hwif);
-		hwif->present = 0;
-	}
-
-	ide_proc_unregister_port(hwif);
-
-	hwgroup = hwif->hwgroup;
-	/*
-	 * free the irq if we were the only hwif using it
-	 */
-	g = hwgroup->hwif;
-	do {
-		if (g->irq == hwif->irq)
-			++irq_count;
-		g = g->next;
-	} while (g != hwgroup->hwif);
-	if (irq_count == 1)
-		free_irq(hwif->irq, hwgroup);
-
-	ide_remove_port_from_hwgroup(hwif);
-
-	device_unregister(hwif->portdev);
-	device_unregister(&hwif->gendev);
-	wait_for_completion(&hwif->gendev_rel_comp);
-
-	/*
-	 * Remove us from the kernel's knowledge
-	 */
-	blk_unregister_region(MKDEV(hwif->major, 0), MAX_DRIVES<<PARTN_BITS);
-	kfree(hwif->sg_table);
-	unregister_blkdev(hwif->major, hwif->name);
-
-	ide_release_dma_engine(hwif);
-
-	mutex_unlock(&ide_cfg_mtx);
-}
-
-void ide_init_port_hw(ide_hwif_t *hwif, hw_regs_t *hw)
-{
-	memcpy(&hwif->io_ports, &hw->io_ports, sizeof(hwif->io_ports));
-	hwif->irq = hw->irq;
-	hwif->chipset = hw->chipset;
-	hwif->dev = hw->dev;
-	hwif->gendev.parent = hw->parent ? hw->parent : hw->dev;
-	hwif->ack_intr = hw->ack_intr;
-	hwif->config_data = hw->config;
-}
-
 /*
  *	Locks for IDE setting functionality
  */
@@ -330,7 +159,6 @@ static int set_pio_mode_abuse(ide_hwif_t *hwif, u8 req_pio)
 static int set_pio_mode(ide_drive_t *drive, int arg)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
 	const struct ide_port_ops *port_ops = hwif->port_ops;
 
 	if (arg < 0 || arg > 255)
@@ -345,9 +173,9 @@ static int set_pio_mode(ide_drive_t *drive, int arg)
 			unsigned long flags;
 
 			/* take lock for IDE_DFLAG_[NO_]UNMASK/[NO_]IO_32BIT */
-			spin_lock_irqsave(&hwgroup->lock, flags);
+			spin_lock_irqsave(&hwif->lock, flags);
 			port_ops->set_pio_mode(drive, arg);
-			spin_unlock_irqrestore(&hwgroup->lock, flags);
+			spin_unlock_irqrestore(&hwif->lock, flags);
 		} else
 			port_ops->set_pio_mode(drive, arg);
 	} else {
@@ -453,7 +281,7 @@ static int ide_uevent(struct device *dev, struct kobj_uevent_env *env)
 static int generic_ide_probe(struct device *dev)
 {
 	ide_drive_t *drive = to_ide_device(dev);
-	ide_driver_t *drv = to_ide_driver(dev->driver);
+	struct ide_driver *drv = to_ide_driver(dev->driver);
 
 	return drv->probe ? drv->probe(drive) : -ENODEV;
 }
@@ -461,7 +289,7 @@ static int generic_ide_probe(struct device *dev)
 static int generic_ide_remove(struct device *dev)
 {
 	ide_drive_t *drive = to_ide_device(dev);
-	ide_driver_t *drv = to_ide_driver(dev->driver);
+	struct ide_driver *drv = to_ide_driver(dev->driver);
 
 	if (drv->remove)
 		drv->remove(drive);
@@ -472,7 +300,7 @@ static int generic_ide_remove(struct device *dev)
 static void generic_ide_shutdown(struct device *dev)
 {
 	ide_drive_t *drive = to_ide_device(dev);
-	ide_driver_t *drv = to_ide_driver(dev->driver);
+	struct ide_driver *drv = to_ide_driver(dev->driver);
 
 	if (dev->driver && drv->shutdown)
 		drv->shutdown(drive);
@@ -660,6 +488,7 @@ MODULE_PARM_DESC(ignore_cable, "ignore cable detection");
 
 void ide_port_apply_params(ide_hwif_t *hwif)
 {
+	ide_drive_t *drive;
 	int i;
 
 	if (ide_ignore_cable & (1 << hwif->index)) {
@@ -668,8 +497,8 @@ void ide_port_apply_params(ide_hwif_t *hwif)
 		hwif->cbl = ATA_CBL_PATA40_SHORT;
 	}
 
-	for (i = 0; i < MAX_DRIVES; i++)
-		ide_dev_apply_params(&hwif->drives[i], i);
+	ide_port_for_each_dev(i, drive, hwif)
+		ide_dev_apply_params(drive, i);
 }
 
 /*
diff --git a/drivers/ide/it8172.c b/drivers/ide/it8172.c
new file mode 100644
index 000000000000..e021078cd06b
--- /dev/null
+++ b/drivers/ide/it8172.c
@@ -0,0 +1,166 @@
+/*
+ *
+ * BRIEF MODULE DESCRIPTION
+ *      IT8172 IDE controller support
+ *
+ * Copyright (C) 2000 MontaVista Software Inc.
+ * Copyright (C) 2008 Shane McDonald
+ *
+ *  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  SOFTWARE  IS PROVIDED   ``AS  IS'' AND   ANY  EXPRESS OR IMPLIED
+ *  WARRANTIES,   INCLUDING, BUT NOT  LIMITED  TO, THE IMPLIED WARRANTIES OF
+ *  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN
+ *  NO  EVENT  SHALL   THE AUTHOR  BE    LIABLE FOR ANY   DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ *  NOT LIMITED   TO, PROCUREMENT OF  SUBSTITUTE GOODS  OR SERVICES; LOSS OF
+ *  USE, DATA,  OR PROFITS; OR  BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ *  ANY THEORY OF LIABILITY, WHETHER IN  CONTRACT, STRICT LIABILITY, OR TORT
+ *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ *  THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  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/types.h>
+#include <linux/kernel.h>
+#include <linux/ioport.h>
+#include <linux/pci.h>
+#include <linux/ide.h>
+#include <linux/init.h>
+
+#define DRV_NAME "IT8172"
+
+static void it8172_set_pio_mode(ide_drive_t *drive, const u8 pio)
+{
+	ide_hwif_t *hwif	= drive->hwif;
+	struct pci_dev *dev	= to_pci_dev(hwif->dev);
+	u16 drive_enables;
+	u32 drive_timing;
+
+	/*
+	 * The highest value of DIOR/DIOW pulse width and recovery time
+	 * that can be set in the IT8172 is 8 PCI clock cycles.  As a result,
+	 * it cannot be configured for PIO mode 0.  This table sets these
+	 * parameters to the maximum supported by the IT8172.
+	 */
+	static const u8 timings[] = { 0x3f, 0x3c, 0x1b, 0x12, 0x0a };
+
+	pci_read_config_word(dev, 0x40, &drive_enables);
+	pci_read_config_dword(dev, 0x44, &drive_timing);
+
+	/*
+	 * Enable port 0x44. The IT8172 spec is confused; it calls
+	 * this register the "Slave IDE Timing Register", but in fact,
+	 * it controls timing for both master and slave drives.
+	 */
+	drive_enables |= 0x4000;
+
+	drive_enables &= drive->dn ? 0xc006 : 0xc060;
+	if (drive->media == ide_disk)
+		/* enable prefetch */
+		drive_enables |= 0x0004 << (drive->dn * 4);
+	if (ata_id_has_iordy(drive->id))
+		/* enable IORDY sample-point */
+		drive_enables |= 0x0002 << (drive->dn * 4);
+
+	drive_timing &= drive->dn ? 0x00003f00 : 0x000fc000;
+	drive_timing |= timings[pio] << (drive->dn * 6 + 8);
+
+	pci_write_config_word(dev, 0x40, drive_enables);
+	pci_write_config_dword(dev, 0x44, drive_timing);
+}
+
+static void it8172_set_dma_mode(ide_drive_t *drive, const u8 speed)
+{
+	ide_hwif_t *hwif	= drive->hwif;
+	struct pci_dev *dev	= to_pci_dev(hwif->dev);
+	int a_speed		= 3 << (drive->dn * 4);
+	int u_flag		= 1 << drive->dn;
+	int u_speed		= 0;
+	u8 reg48, reg4a;
+
+	pci_read_config_byte(dev, 0x48, &reg48);
+	pci_read_config_byte(dev, 0x4a, &reg4a);
+
+	if (speed >= XFER_UDMA_0) {
+		u8 udma = speed - XFER_UDMA_0;
+		u_speed = udma << (drive->dn * 4);
+
+		pci_write_config_byte(dev, 0x48, reg48 | u_flag);
+		reg4a &= ~a_speed;
+		pci_write_config_byte(dev, 0x4a, reg4a | u_speed);
+	} else {
+		const u8 mwdma_to_pio[] = { 0, 3, 4 };
+		u8 pio;
+
+		pci_write_config_byte(dev, 0x48, reg48 & ~u_flag);
+		pci_write_config_byte(dev, 0x4a, reg4a & ~a_speed);
+
+		pio = mwdma_to_pio[speed - XFER_MW_DMA_0];
+
+		it8172_set_pio_mode(drive, pio);
+	}
+}
+
+
+static const struct ide_port_ops it8172_port_ops = {
+	.set_pio_mode	= it8172_set_pio_mode,
+	.set_dma_mode	= it8172_set_dma_mode,
+};
+
+static const struct ide_port_info it8172_port_info __devinitdata = {
+	.name		= DRV_NAME,
+	.port_ops	= &it8172_port_ops,
+	.enablebits	= { {0x41, 0x80, 0x80}, {0x00, 0x00, 0x00} },
+	.host_flags	= IDE_HFLAG_SINGLE,
+	.pio_mask	= ATA_PIO4 & ~ATA_PIO0,
+	.mwdma_mask	= ATA_MWDMA2,
+	.udma_mask	= ATA_UDMA2,
+};
+
+static int __devinit it8172_init_one(struct pci_dev *dev,
+					const struct pci_device_id *id)
+{
+	if ((dev->class >> 8) != PCI_CLASS_STORAGE_IDE)
+		return -ENODEV; /* IT8172 is more than an IDE controller */
+	return ide_pci_init_one(dev, &it8172_port_info, NULL);
+}
+
+static struct pci_device_id it8172_pci_tbl[] = {
+	{ PCI_VDEVICE(ITE, PCI_DEVICE_ID_ITE_8172), 0 },
+	{ 0, },
+};
+MODULE_DEVICE_TABLE(pci, it8172_pci_tbl);
+
+static struct pci_driver it8172_pci_driver = {
+	.name		= "IT8172_IDE",
+	.id_table	= it8172_pci_tbl,
+	.probe		= it8172_init_one,
+	.remove		= ide_pci_remove,
+	.suspend	= ide_pci_suspend,
+	.resume		= ide_pci_resume,
+};
+
+static int __init it8172_ide_init(void)
+{
+	return ide_pci_register_driver(&it8172_pci_driver);
+}
+
+static void __exit it8172_ide_exit(void)
+{
+	pci_unregister_driver(&it8172_pci_driver);
+}
+
+module_init(it8172_ide_init);
+module_exit(it8172_ide_exit);
+
+MODULE_AUTHOR("Steve Longerbeam");
+MODULE_DESCRIPTION("PCI driver module for ITE 8172 IDE");
+MODULE_LICENSE("GPL");
diff --git a/drivers/ide/it8213.c b/drivers/ide/it8213.c
index 7c2feeb3c5ec..d7969b6d139e 100644
--- a/drivers/ide/it8213.c
+++ b/drivers/ide/it8213.c
@@ -25,7 +25,7 @@
 
 static void it8213_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	int is_slave		= drive->dn & 1;
 	int master_port		= 0x40;
@@ -82,7 +82,7 @@ static void it8213_set_pio_mode(ide_drive_t *drive, const u8 pio)
 
 static void it8213_set_dma_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 maslave		= 0x40;
 	int a_speed		= 3 << (drive->dn * 4);
diff --git a/drivers/ide/it821x.c b/drivers/ide/it821x.c
index ef004089761b..0be27ac1f077 100644
--- a/drivers/ide/it821x.c
+++ b/drivers/ide/it821x.c
@@ -167,12 +167,10 @@ static void it821x_clock_strategy(ide_drive_t *drive)
 	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
 	struct it821x_dev *itdev = ide_get_hwifdata(hwif);
-	ide_drive_t *pair;
+	ide_drive_t *pair = ide_get_pair_dev(drive);
 	int clock, altclock, sel = 0;
 	u8 unit = drive->dn & 1, v;
 
-	pair = &hwif->drives[1 - unit];
-
 	if(itdev->want[0][0] > itdev->want[1][0]) {
 		clock = itdev->want[0][1];
 		altclock = itdev->want[1][1];
@@ -239,15 +237,13 @@ static void it821x_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	struct it821x_dev *itdev = ide_get_hwifdata(hwif);
-	ide_drive_t *pair;
+	ide_drive_t *pair = ide_get_pair_dev(drive);
 	u8 unit = drive->dn & 1, set_pio = pio;
 
 	/* Spec says 89 ref driver uses 88 */
 	static u16 pio_timings[]= { 0xAA88, 0xA382, 0xA181, 0x3332, 0x3121 };
 	static u8 pio_want[]    = { ATA_66, ATA_66, ATA_66, ATA_66, ATA_ANY };
 
-	pair = &hwif->drives[1 - unit];
-
 	/*
 	 * Compute the best PIO mode we can for a given device. We must
 	 * pick a speed that does not cause problems with the other device
@@ -279,7 +275,7 @@ static void it821x_set_pio_mode(ide_drive_t *drive, const u8 pio)
  *	the shared MWDMA/PIO timing register.
  */
 
-static void it821x_tune_mwdma (ide_drive_t *drive, byte mode_wanted)
+static void it821x_tune_mwdma(ide_drive_t *drive, u8 mode_wanted)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
@@ -316,7 +312,7 @@ static void it821x_tune_mwdma (ide_drive_t *drive, byte mode_wanted)
  *	controller when doing UDMA modes in pass through.
  */
 
-static void it821x_tune_udma (ide_drive_t *drive, byte mode_wanted)
+static void it821x_tune_udma(ide_drive_t *drive, u8 mode_wanted)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
@@ -516,6 +512,7 @@ static struct ide_dma_ops it821x_pass_through_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_timeout		= ide_dma_timeout,
 	.dma_lost_irq		= ide_dma_lost_irq,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 /**
diff --git a/drivers/ide/ns87415.c b/drivers/ide/ns87415.c
index 13789060f407..83643ed9a426 100644
--- a/drivers/ide/ns87415.c
+++ b/drivers/ide/ns87415.c
@@ -56,7 +56,7 @@ static u8 superio_read_status(ide_hwif_t *hwif)
 	return superio_ide_inb(hwif->io_ports.status_addr);
 }
 
-static u8 superio_read_sff_dma_status(ide_hwif_t *hwif)
+static u8 superio_dma_sff_read_status(ide_hwif_t *hwif)
 {
 	return superio_ide_inb(hwif->dma_base + ATA_DMA_STATUS);
 }
@@ -109,7 +109,6 @@ static const struct ide_tp_ops superio_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= superio_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= superio_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
@@ -132,18 +131,20 @@ static void __devinit superio_init_iops(struct hwif_s *hwif)
 	tmp = superio_ide_inb(dma_stat);
 	outb(tmp | 0x66, dma_stat);
 }
+#else
+#define superio_dma_sff_read_status ide_dma_sff_read_status
 #endif
 
 static unsigned int ns87415_count = 0, ns87415_control[MAX_HWIFS] = { 0 };
 
 /*
  * This routine either enables/disables (according to IDE_DFLAG_PRESENT)
- * the IRQ associated with the port (HWIF(drive)),
+ * the IRQ associated with the port,
  * and selects either PIO or DMA handshaking for the next I/O operation.
  */
 static void ns87415_prepare_drive (ide_drive_t *drive, unsigned int use_dma)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
 	unsigned int bit, other, new, *old = (unsigned int *) hwif->select_data;
 	unsigned long flags;
@@ -197,11 +198,11 @@ static void ns87415_selectproc (ide_drive_t *drive)
 
 static int ns87415_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t      *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	u8 dma_stat = 0, dma_cmd = 0;
 
 	drive->waiting_for_dma = 0;
-	dma_stat = hwif->tp_ops->read_sff_dma_status(hwif);
+	dma_stat = hwif->dma_ops->dma_sff_read_status(hwif);
 	/* get DMA command mode */
 	dma_cmd = inb(hwif->dma_base + ATA_DMA_CMD);
 	/* stop DMA */
@@ -308,6 +309,7 @@ static const struct ide_dma_ops ns87415_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= superio_dma_sff_read_status,
 };
 
 static const struct ide_port_info ns87415_chipset __devinitdata = {
diff --git a/drivers/ide/palm_bk3710.c b/drivers/ide/palm_bk3710.c
index 122ed3c072fd..a7ac490c9ae3 100644
--- a/drivers/ide/palm_bk3710.c
+++ b/drivers/ide/palm_bk3710.c
@@ -324,8 +324,6 @@ static int __devinit palm_bk3710_init_dma(ide_hwif_t *hwif,
 
 	hwif->dma_base = hwif->io_ports.data_addr - IDE_PALM_ATA_PRI_REG_OFFSET;
 
-	hwif->dma_ops = &sff_dma_ops;
-
 	return 0;
 }
 
@@ -338,6 +336,7 @@ static const struct ide_port_ops palm_bk3710_ports_ops = {
 static struct ide_port_info __devinitdata palm_bk3710_port_info = {
 	.init_dma		= palm_bk3710_init_dma,
 	.port_ops		= &palm_bk3710_ports_ops,
+	.dma_ops		= &sff_dma_ops,
 	.host_flags		= IDE_HFLAG_MMIO,
 	.pio_mask		= ATA_PIO4,
 	.mwdma_mask		= ATA_MWDMA2,
diff --git a/drivers/ide/pdc202xx_new.c b/drivers/ide/pdc202xx_new.c
index 211ae46e3e0c..f21290c4b447 100644
--- a/drivers/ide/pdc202xx_new.c
+++ b/drivers/ide/pdc202xx_new.c
@@ -143,7 +143,7 @@ static struct udma_timing {
 
 static void pdcnew_set_dma_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 adj			= (drive->dn & 1) ? 0x08 : 0x00;
 
@@ -219,7 +219,7 @@ static void pdcnew_reset(ide_drive_t *drive)
 	 * Deleted this because it is redundant from the caller.
 	 */
 	printk(KERN_WARNING "pdc202xx_new: %s channel reset.\n",
-		HWIF(drive)->channel ? "Secondary" : "Primary");
+		drive->hwif->channel ? "Secondary" : "Primary");
 }
 
 /**
diff --git a/drivers/ide/pdc202xx_old.c b/drivers/ide/pdc202xx_old.c
index 624e62e5cc9a..97193323aebf 100644
--- a/drivers/ide/pdc202xx_old.c
+++ b/drivers/ide/pdc202xx_old.c
@@ -39,7 +39,7 @@ static void pdc_old_disable_66MHz_clock(ide_hwif_t *);
 
 static void pdc202xx_set_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 drive_pci		= 0x60 + (drive->dn << 2);
 
@@ -169,8 +169,8 @@ static void pdc202xx_dma_start(ide_drive_t *drive)
 	if (drive->current_speed > XFER_UDMA_2)
 		pdc_old_enable_66MHz_clock(drive->hwif);
 	if (drive->media != ide_disk || (drive->dev_flags & IDE_DFLAG_LBA48)) {
-		struct request *rq	= HWGROUP(drive)->rq;
-		ide_hwif_t *hwif	= HWIF(drive);
+		ide_hwif_t *hwif	= drive->hwif;
+		struct request *rq	= hwif->rq;
 		unsigned long high_16	= hwif->extra_base - 16;
 		unsigned long atapi_reg	= high_16 + (hwif->channel ? 0x24 : 0x20);
 		u32 word_count	= 0;
@@ -189,7 +189,7 @@ static void pdc202xx_dma_start(ide_drive_t *drive)
 static int pdc202xx_dma_end(ide_drive_t *drive)
 {
 	if (drive->media != ide_disk || (drive->dev_flags & IDE_DFLAG_LBA48)) {
-		ide_hwif_t *hwif	= HWIF(drive);
+		ide_hwif_t *hwif	= drive->hwif;
 		unsigned long high_16	= hwif->extra_base - 16;
 		unsigned long atapi_reg	= high_16 + (hwif->channel ? 0x24 : 0x20);
 		u8 clock		= 0;
@@ -205,7 +205,7 @@ static int pdc202xx_dma_end(ide_drive_t *drive)
 
 static int pdc202xx_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	unsigned long high_16	= hwif->extra_base - 16;
 	u8 dma_stat		= inb(hwif->dma_base + ATA_DMA_STATUS);
 	u8 sc1d			= inb(high_16 + 0x001d);
@@ -243,7 +243,7 @@ static void pdc202xx_reset_host (ide_hwif_t *hwif)
 
 static void pdc202xx_reset (ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	ide_hwif_t *mate	= hwif->mate;
 
 	pdc202xx_reset_host(hwif);
@@ -337,6 +337,7 @@ static const struct ide_dma_ops pdc20246_dma_ops = {
 	.dma_test_irq		= pdc202xx_dma_test_irq,
 	.dma_lost_irq		= pdc202xx_dma_lost_irq,
 	.dma_timeout		= pdc202xx_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_dma_ops pdc2026x_dma_ops = {
@@ -348,6 +349,7 @@ static const struct ide_dma_ops pdc2026x_dma_ops = {
 	.dma_test_irq		= pdc202xx_dma_test_irq,
 	.dma_lost_irq		= pdc202xx_dma_lost_irq,
 	.dma_timeout		= pdc202xx_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 #define DECLARE_PDC2026X_DEV(udma, sectors) \
diff --git a/drivers/ide/piix.c b/drivers/ide/piix.c
index 61d2d920a5cd..f1e2e4ef0d71 100644
--- a/drivers/ide/piix.c
+++ b/drivers/ide/piix.c
@@ -67,7 +67,7 @@ static int no_piix_dma;
 
 static void piix_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	int is_slave		= drive->dn & 1;
 	int master_port		= hwif->channel ? 0x42 : 0x40;
@@ -136,7 +136,7 @@ static void piix_set_pio_mode(ide_drive_t *drive, const u8 pio)
 
 static void piix_set_dma_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 maslave		= hwif->channel ? 0x42 : 0x40;
 	int a_speed		= 3 << (drive->dn * 4);
@@ -224,7 +224,7 @@ static unsigned int init_chipset_ich(struct pci_dev *dev)
  */
 static void ich_clear_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	u8 dma_stat;
 
 	/*
@@ -260,6 +260,8 @@ static const struct ich_laptop ich_laptop[] = {
 	{ 0x27DF, 0x103C, 0x30A1 },	/* ICH7 on HP Compaq nc2400 */
 	{ 0x27DF, 0x1071, 0xD221 },	/* ICH7 on Hercules EC-900 */
 	{ 0x24CA, 0x1025, 0x0061 },	/* ICH4 on Acer Aspire 2023WLMi */
+	{ 0x24CA, 0x1025, 0x003d },	/* ICH4 on ACER TM290 */
+	{ 0x266F, 0x1025, 0x0066 },	/* ICH6 on ACER Aspire 1694WLMi */
 	{ 0x2653, 0x1043, 0x82D8 },	/* ICH6M on Asus Eee 701 */
 	/* end marker */
 	{ 0, }
diff --git a/drivers/ide/pmac.c b/drivers/ide/pmac.c
index 7c481bb56fab..74625e821a43 100644
--- a/drivers/ide/pmac.c
+++ b/drivers/ide/pmac.c
@@ -955,7 +955,6 @@ static const struct ide_tp_ops pmac_tp_ops = {
 	.exec_command		= pmac_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= pmac_set_irq,
 
@@ -1513,10 +1512,10 @@ use_pio_instead:
 static int
 pmac_ide_dma_setup(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	pmac_ide_hwif_t *pmif =
 		(pmac_ide_hwif_t *)dev_get_drvdata(hwif->gendev.parent);
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = hwif->rq;
 	u8 unit = drive->dn & 1, ata4 = (pmif->kind == controller_kl_ata4);
 
 	if (!pmac_ide_build_dmatable(drive, rq)) {
@@ -1637,7 +1636,7 @@ pmac_ide_dma_test_irq (ide_drive_t *drive)
 			break;
 		if (++timeout > 100) {
 			printk(KERN_WARNING "ide%d, ide_dma_test_irq \
-			timeout flushing channel\n", HWIF(drive)->index);
+			timeout flushing channel\n", hwif->index);
 			break;
 		}
 	}	
diff --git a/drivers/ide/q40ide.c b/drivers/ide/q40ide.c
index 4af4a8ce4cdf..9f9c0b3cc3a3 100644
--- a/drivers/ide/q40ide.c
+++ b/drivers/ide/q40ide.c
@@ -99,7 +99,6 @@ static const struct ide_tp_ops q40ide_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
diff --git a/drivers/ide/qd65xx.c b/drivers/ide/qd65xx.c
index bc27c7aba936..5b2e3af43c4b 100644
--- a/drivers/ide/qd65xx.c
+++ b/drivers/ide/qd65xx.c
@@ -202,7 +202,8 @@ static void qd6500_set_pio_mode(ide_drive_t *drive, const u8 pio)
 		recovery_time = drive->id[ATA_ID_EIDE_PIO] - 120;
 	}
 
-	qd_set_timing(drive, qd6500_compute_timing(HWIF(drive), active_time, recovery_time));
+	qd_set_timing(drive, qd6500_compute_timing(drive->hwif,
+				active_time, recovery_time));
 }
 
 static void qd6580_set_pio_mode(ide_drive_t *drive, const u8 pio)
@@ -245,11 +246,11 @@ static void qd6580_set_pio_mode(ide_drive_t *drive, const u8 pio)
 		printk(KERN_INFO "%s: PIO mode%d\n", drive->name,pio);
 	}
 
-	if (!HWIF(drive)->channel && drive->media != ide_disk) {
+	if (!hwif->channel && drive->media != ide_disk) {
 		outb(0x5f, QD_CONTROL_PORT);
 		printk(KERN_WARNING "%s: ATAPI: disabled read-ahead FIFO "
 			"and post-write buffer on %s.\n",
-			drive->name, HWIF(drive)->name);
+			drive->name, hwif->name);
 	}
 
 	qd_set_timing(drive, qd6580_compute_timing(active_time, recovery_time));
diff --git a/drivers/ide/qd65xx.h b/drivers/ide/qd65xx.h
index c83dea85e621..6636f9665d16 100644
--- a/drivers/ide/qd65xx.h
+++ b/drivers/ide/qd65xx.h
@@ -31,8 +31,8 @@
 
 #define QD_CONFIG(hwif)		((hwif)->config_data & 0x00ff)
 
-#define QD_TIMING(drive)	(byte)(((drive)->drive_data) & 0x00ff)
-#define QD_TIMREG(drive)	(byte)((((drive)->drive_data) & 0xff00) >> 8)
+#define QD_TIMING(drive)	(u8)(((drive)->drive_data) & 0x00ff)
+#define QD_TIMREG(drive)	(u8)((((drive)->drive_data) & 0xff00) >> 8)
 
 #define QD6500_DEF_DATA		((QD_TIM1_PORT<<8) | (QD_ID3 ? 0x0c : 0x08))
 #define QD6580_DEF_DATA		((QD_TIM1_PORT<<8) | (QD_ID3 ? 0x0a : 0x00))
diff --git a/drivers/ide/sc1200.c b/drivers/ide/sc1200.c
index ec7f766ef5e4..dbdd2985a0d8 100644
--- a/drivers/ide/sc1200.c
+++ b/drivers/ide/sc1200.c
@@ -125,7 +125,7 @@ out:
 
 static void sc1200_set_dma_mode(ide_drive_t *drive, const u8 mode)
 {
-	ide_hwif_t		*hwif = HWIF(drive);
+	ide_hwif_t		*hwif = drive->hwif;
 	struct pci_dev		*dev = to_pci_dev(hwif->dev);
 	unsigned int		reg, timings;
 	unsigned short		pci_clock;
@@ -170,9 +170,9 @@ static void sc1200_set_dma_mode(ide_drive_t *drive, const u8 mode)
  */
 static int sc1200_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	unsigned long dma_base = hwif->dma_base;
-	byte dma_stat;
+	u8 dma_stat;
 
 	dma_stat = inb(dma_base+2);		/* get DMA status */
 
@@ -199,7 +199,7 @@ static int sc1200_dma_end(ide_drive_t *drive)
 
 static void sc1200_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t	*hwif = HWIF(drive);
+	ide_hwif_t	*hwif = drive->hwif;
 	int		mode = -1;
 
 	/*
@@ -292,6 +292,7 @@ static const struct ide_dma_ops sc1200_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_port_info sc1200_chipset __devinitdata = {
diff --git a/drivers/ide/scc_pata.c b/drivers/ide/scc_pata.c
index 0f48f9dacfa5..8d2314b6327c 100644
--- a/drivers/ide/scc_pata.c
+++ b/drivers/ide/scc_pata.c
@@ -143,7 +143,7 @@ static u8 scc_read_altstatus(ide_hwif_t *hwif)
 	return (u8)in_be32((void *)hwif->io_ports.ctl_addr);
 }
 
-static u8 scc_read_sff_dma_status(ide_hwif_t *hwif)
+static u8 scc_dma_sff_read_status(ide_hwif_t *hwif)
 {
 	return (u8)in_be32((void *)(hwif->dma_base + 4));
 }
@@ -217,7 +217,7 @@ scc_ide_outsl(unsigned long port, void *addr, u32 count)
 
 static void scc_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct scc_ports *ports = ide_get_hwifdata(hwif);
 	unsigned long ctl_base = ports->ctl;
 	unsigned long cckctrl_port = ctl_base + 0xff0;
@@ -249,7 +249,7 @@ static void scc_set_pio_mode(ide_drive_t *drive, const u8 pio)
 
 static void scc_set_dma_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct scc_ports *ports = ide_get_hwifdata(hwif);
 	unsigned long ctl_base = ports->ctl;
 	unsigned long cckctrl_port = ctl_base + 0xff0;
@@ -259,7 +259,7 @@ static void scc_set_dma_mode(ide_drive_t *drive, const u8 speed)
 	unsigned long scrcst_port = ctl_base + 0x014;
 	unsigned long udenvt_port = ctl_base + 0x018;
 	unsigned long tdvhsel_port   = ctl_base + 0x020;
-	int is_slave = (&hwif->drives[1] == drive);
+	int is_slave = drive->dn & 1;
 	int offset, idx;
 	unsigned long reg;
 	unsigned long jcactsel;
@@ -292,7 +292,7 @@ static void scc_dma_host_set(ide_drive_t *drive, int on)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	u8 unit = drive->dn & 1;
-	u8 dma_stat = scc_ide_inb(hwif->dma_base + 4);
+	u8 dma_stat = scc_dma_sff_read_status(hwif);
 
 	if (on)
 		dma_stat |= (1 << (5 + unit));
@@ -316,7 +316,7 @@ static void scc_dma_host_set(ide_drive_t *drive, int on)
 static int scc_dma_setup(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = hwif->rq;
 	unsigned int reading;
 	u8 dma_stat;
 
@@ -338,7 +338,7 @@ static int scc_dma_setup(ide_drive_t *drive)
 	out_be32((void __iomem *)hwif->dma_base, reading);
 
 	/* read DMA status for INTR & ERROR flags */
-	dma_stat = in_be32((void __iomem *)(hwif->dma_base + 4));
+	dma_stat = scc_dma_sff_read_status(hwif);
 
 	/* clear INTR & ERROR flags */
 	out_be32((void __iomem *)(hwif->dma_base + 4), dma_stat | 6);
@@ -367,7 +367,7 @@ static int __scc_dma_end(ide_drive_t *drive)
 	/* stop DMA */
 	scc_ide_outb(dma_cmd & ~1, hwif->dma_base);
 	/* get DMA status */
-	dma_stat = scc_ide_inb(hwif->dma_base + 4);
+	dma_stat = scc_dma_sff_read_status(hwif);
 	/* clear the INTR & ERROR bits */
 	scc_ide_outb(dma_stat | 6, hwif->dma_base + 4);
 	/* purge DMA mappings */
@@ -387,7 +387,7 @@ static int __scc_dma_end(ide_drive_t *drive)
 
 static int scc_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	void __iomem *dma_base = (void __iomem *)hwif->dma_base;
 	unsigned long intsts_port = hwif->dma_base + 0x014;
 	u32 reg;
@@ -405,17 +405,18 @@ static int scc_dma_end(ide_drive_t *drive)
 			       drive->name);
 			data_loss = 1;
 			if (retry++) {
-				struct request *rq = HWGROUP(drive)->rq;
-				int unit;
+				struct request *rq = hwif->rq;
+				ide_drive_t *drive;
+				int i;
+
 				/* ERROR_RESET and drive->crc_count are needed
 				 * to reduce DMA transfer mode in retry process.
 				 */
 				if (rq)
 					rq->errors |= ERROR_RESET;
-				for (unit = 0; unit < MAX_DRIVES; unit++) {
-					ide_drive_t *drive = &hwif->drives[unit];
+
+				ide_port_for_each_dev(i, drive, hwif)
 					drive->crc_count++;
-				}
 			}
 		}
 	}
@@ -496,7 +497,7 @@ static int scc_dma_end(ide_drive_t *drive)
 /* returns 1 if dma irq issued, 0 otherwise */
 static int scc_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	u32 int_stat = in_be32((void __iomem *)hwif->dma_base + 0x014);
 
 	/* SCC errata A252,A308 workaround: Step4 */
@@ -852,7 +853,6 @@ static const struct ide_tp_ops scc_tp_ops = {
 	.exec_command		= scc_exec_command,
 	.read_status		= scc_read_status,
 	.read_altstatus		= scc_read_altstatus,
-	.read_sff_dma_status	= scc_read_sff_dma_status,
 
 	.set_irq		= scc_set_irq,
 
@@ -879,6 +879,7 @@ static const struct ide_dma_ops scc_dma_ops = {
 	.dma_test_irq		= scc_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= scc_dma_sff_read_status,
 };
 
 #define DECLARE_SCC_DEV(name_str)			\
diff --git a/drivers/ide/serverworks.c b/drivers/ide/serverworks.c
index 437bc919dafd..382102ba467b 100644
--- a/drivers/ide/serverworks.c
+++ b/drivers/ide/serverworks.c
@@ -151,7 +151,7 @@ static void svwks_set_dma_mode(ide_drive_t *drive, const u8 speed)
 	static const u8 dma_modes[]		= { 0x77, 0x21, 0x20 };
 	static const u8 drive_pci2[]		= { 0x45, 0x44, 0x47, 0x46 };
 
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 unit			= drive->dn & 1;
 
diff --git a/drivers/ide/setup-pci.c b/drivers/ide/setup-pci.c
index 9f1f9163a136..e85d1ed29c2a 100644
--- a/drivers/ide/setup-pci.c
+++ b/drivers/ide/setup-pci.c
@@ -130,7 +130,7 @@ int ide_pci_check_simplex(ide_hwif_t *hwif, const struct ide_port_info *d)
 	 * we tune the drive then try to grab DMA ownership if we want to be
 	 * the DMA end.  This has to be become dynamic to handle hot-plug.
 	 */
-	dma_stat = hwif->tp_ops->read_sff_dma_status(hwif);
+	dma_stat = hwif->dma_ops->dma_sff_read_status(hwif);
 	if ((dma_stat & 0x80) && hwif->mate && hwif->mate->dma_base) {
 		printk(KERN_INFO "%s %s: simplex device: DMA disabled\n",
 			d->name, pci_name(dev));
@@ -377,6 +377,9 @@ int ide_hwif_setup_dma(ide_hwif_t *hwif, const struct ide_port_info *d)
 
 		hwif->dma_base = base;
 
+		if (hwif->dma_ops == NULL)
+			hwif->dma_ops = &sff_dma_ops;
+
 		if (ide_pci_check_simplex(hwif, d) < 0)
 			return -1;
 
@@ -393,8 +396,6 @@ int ide_hwif_setup_dma(ide_hwif_t *hwif, const struct ide_port_info *d)
 
 		if (ide_allocate_dma_engine(hwif))
 			return -1;
-
-		hwif->dma_ops = &sff_dma_ops;
 	}
 
 	return 0;
@@ -471,7 +472,7 @@ void ide_pci_setup_ports(struct pci_dev *dev, const struct ide_port_info *d,
 	 */
 
 	for (port = 0; port < channels; ++port) {
-		const ide_pci_enablebit_t *e = &(d->enablebits[port]);
+		const struct ide_pci_enablebit *e = &d->enablebits[port];
 
 		if (e->reg && (pci_read_config_byte(dev, e->reg, &tmp) ||
 		    (tmp & e->mask) != e->val)) {
@@ -519,8 +520,7 @@ static int do_ide_setup_pci_device(struct pci_dev *dev,
 	if (ret < 0)
 		goto out;
 
-	/* Is it an "IDE storage" device in non-PCI mode? */
-	if ((dev->class >> 8) == PCI_CLASS_STORAGE_IDE && (dev->class & 5) != 5) {
+	if (ide_pci_is_in_compatibility_mode(dev)) {
 		if (noisy)
 			printk(KERN_INFO "%s %s: not 100%% native mode: will "
 				"probe irqs later\n", d->name, pci_name(dev));
diff --git a/drivers/ide/sgiioc4.c b/drivers/ide/sgiioc4.c
index a687a7dfea6f..fdb9d7037694 100644
--- a/drivers/ide/sgiioc4.c
+++ b/drivers/ide/sgiioc4.c
@@ -123,7 +123,7 @@ static int
 sgiioc4_clearirq(ide_drive_t * drive)
 {
 	u32 intr_reg;
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct ide_io_ports *io_ports = &hwif->io_ports;
 	unsigned long other_ir = io_ports->irq_addr + (IOC4_INTR_REG << 2);
 
@@ -181,7 +181,7 @@ sgiioc4_clearirq(ide_drive_t * drive)
 
 static void sgiioc4_dma_start(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	unsigned long ioc4_dma_addr = hwif->dma_base + IOC4_DMA_CTRL * 4;
 	unsigned int reg = readl((void __iomem *)ioc4_dma_addr);
 	unsigned int temp_reg = reg | IOC4_S_DMA_START;
@@ -209,7 +209,7 @@ sgiioc4_ide_dma_stop(ide_hwif_t *hwif, u64 dma_base)
 static int sgiioc4_dma_end(ide_drive_t *drive)
 {
 	u32 ioc4_dma, bc_dev, bc_mem, num, valid = 0, cnt = 0;
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	unsigned long dma_base = hwif->dma_base;
 	int dma_stat = 0;
 	unsigned long *ending_dma = ide_get_hwifdata(hwif);
@@ -271,7 +271,7 @@ static void sgiioc4_set_dma_mode(ide_drive_t *drive, const u8 speed)
 /* returns 1 if dma irq issued, 0 otherwise */
 static int sgiioc4_dma_test_irq(ide_drive_t *drive)
 {
-	return sgiioc4_checkirq(HWIF(drive));
+	return sgiioc4_checkirq(drive->hwif);
 }
 
 static void sgiioc4_dma_host_set(ide_drive_t *drive, int on)
@@ -367,7 +367,7 @@ static void
 sgiioc4_configure_for_dma(int dma_direction, ide_drive_t * drive)
 {
 	u32 ioc4_dma;
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	unsigned long dma_base = hwif->dma_base;
 	unsigned long ioc4_dma_addr = dma_base + IOC4_DMA_CTRL * 4;
 	u32 dma_addr, ending_dma_addr;
@@ -427,7 +427,7 @@ sgiioc4_configure_for_dma(int dma_direction, ide_drive_t * drive)
 static unsigned int
 sgiioc4_build_dma_table(ide_drive_t * drive, struct request *rq, int ddir)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	unsigned int *table = hwif->dmatable_cpu;
 	unsigned int count = 0, i = 1;
 	struct scatterlist *sg;
@@ -492,7 +492,7 @@ use_pio_instead:
 
 static int sgiioc4_dma_setup(ide_drive_t *drive)
 {
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = drive->hwif->rq;
 	unsigned int count = 0;
 	int ddir;
 
@@ -523,7 +523,6 @@ static const struct ide_tp_ops sgiioc4_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= sgiioc4_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
diff --git a/drivers/ide/siimage.c b/drivers/ide/siimage.c
index 7d622d20bc4c..cb2b352b876b 100644
--- a/drivers/ide/siimage.c
+++ b/drivers/ide/siimage.c
@@ -114,7 +114,7 @@ static unsigned long siimage_selreg(ide_hwif_t *hwif, int r)
 
 static inline unsigned long siimage_seldev(ide_drive_t *drive, int r)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	unsigned long base	= (unsigned long)hwif->hwif_data;
 	u8 unit			= drive->dn & 1;
 
@@ -243,7 +243,7 @@ static void sil_set_pio_mode(ide_drive_t *drive, u8 pio)
 	static const u16 tf_speed[]   = { 0x328a, 0x2283, 0x1281, 0x10c3, 0x10c1 };
 	static const u16 data_speed[] = { 0x328a, 0x2283, 0x1104, 0x10c3, 0x10c1 };
 
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	ide_drive_t *pair	= ide_get_pair_dev(drive);
 	u32 speedt		= 0;
@@ -300,7 +300,7 @@ static void sil_set_dma_mode(ide_drive_t *drive, const u8 speed)
 	static const u8 ultra5[] = { 0x0C, 0x07, 0x05, 0x04, 0x02, 0x01 };
 	static const u16 dma[]	 = { 0x2208, 0x10C2, 0x10C1 };
 
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	unsigned long base	= (unsigned long)hwif->hwif_data;
 	u16 ultra = 0, multi	= 0;
@@ -340,7 +340,7 @@ static void sil_set_dma_mode(ide_drive_t *drive, const u8 speed)
 /* returns 1 if dma irq issued, 0 otherwise */
 static int siimage_io_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 dma_altstat		= 0;
 	unsigned long addr	= siimage_selreg(hwif, 1);
@@ -367,7 +367,7 @@ static int siimage_io_dma_test_irq(ide_drive_t *drive)
 
 static int siimage_mmio_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	unsigned long addr	= siimage_selreg(hwif, 0x1);
 	void __iomem *sata_error_addr
 		= (void __iomem *)hwif->sata_scr[SATA_ERROR_OFFSET];
@@ -717,6 +717,7 @@ static const struct ide_dma_ops sil_dma_ops = {
 	.dma_test_irq		= siimage_dma_test_irq,
 	.dma_timeout		= ide_dma_timeout,
 	.dma_lost_irq		= ide_dma_lost_irq,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 #define DECLARE_SII_DEV(p_ops)				\
diff --git a/drivers/ide/sis5513.c b/drivers/ide/sis5513.c
index ad32e18c5ba3..9ec1a4a4432c 100644
--- a/drivers/ide/sis5513.c
+++ b/drivers/ide/sis5513.c
@@ -274,7 +274,7 @@ static void sis_program_timings(ide_drive_t *drive, const u8 mode)
 
 static void config_drive_art_rwp(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 reg4bh		= 0;
 	u8 rw_prefetch		= 0;
diff --git a/drivers/ide/sl82c105.c b/drivers/ide/sl82c105.c
index 84dc33602ff8..48cc748c5043 100644
--- a/drivers/ide/sl82c105.c
+++ b/drivers/ide/sl82c105.c
@@ -140,7 +140,7 @@ static inline void sl82c105_reset_host(struct pci_dev *dev)
  */
 static void sl82c105_dma_lost_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u32 val, mask		= hwif->channel ? CTRL_IDE_IRQB : CTRL_IDE_IRQA;
 	u8 dma_cmd;
@@ -177,7 +177,7 @@ static void sl82c105_dma_lost_irq(ide_drive_t *drive)
  */
 static void sl82c105_dma_start(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	int reg 		= 0x44 + drive->dn * 4;
 
@@ -299,6 +299,7 @@ static const struct ide_dma_ops sl82c105_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= sl82c105_dma_lost_irq,
 	.dma_timeout		= sl82c105_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_port_info sl82c105_chipset __devinitdata = {
diff --git a/drivers/ide/slc90e66.c b/drivers/ide/slc90e66.c
index 0f759e4ed779..40b4b94a4288 100644
--- a/drivers/ide/slc90e66.c
+++ b/drivers/ide/slc90e66.c
@@ -20,7 +20,7 @@ static DEFINE_SPINLOCK(slc90e66_lock);
 
 static void slc90e66_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	int is_slave		= drive->dn & 1;
 	int master_port		= hwif->channel ? 0x42 : 0x40;
@@ -73,7 +73,7 @@ static void slc90e66_set_pio_mode(ide_drive_t *drive, const u8 pio)
 
 static void slc90e66_set_dma_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 maslave		= hwif->channel ? 0x42 : 0x40;
 	int sitre = 0, a_speed	= 7 << (drive->dn * 4);
diff --git a/drivers/ide/tc86c001.c b/drivers/ide/tc86c001.c
index 93e2cce4b296..84109f5a1632 100644
--- a/drivers/ide/tc86c001.c
+++ b/drivers/ide/tc86c001.c
@@ -15,7 +15,7 @@
 
 static void tc86c001_set_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	unsigned long scr_port	= hwif->config_data + (drive->dn ? 0x02 : 0x00);
 	u16 mode, scr		= inw(scr_port);
 
@@ -62,13 +62,12 @@ static void tc86c001_set_pio_mode(ide_drive_t *drive, const u8 pio)
  */
 static int tc86c001_timer_expiry(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	ide_expiry_t *expiry	= ide_get_hwifdata(hwif);
-	ide_hwgroup_t *hwgroup	= HWGROUP(drive);
 	u8 dma_stat		= inb(hwif->dma_base + ATA_DMA_STATUS);
 
 	/* Restore a higher level driver's expiry handler first. */
-	hwgroup->expiry	= expiry;
+	hwif->expiry = expiry;
 
 	if ((dma_stat & 5) == 1) {	/* DMA active and no interrupt */
 		unsigned long sc_base	= hwif->config_data;
@@ -110,11 +109,10 @@ static int tc86c001_timer_expiry(ide_drive_t *drive)
 
 static void tc86c001_dma_start(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
-	ide_hwgroup_t *hwgroup	= HWGROUP(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	unsigned long sc_base	= hwif->config_data;
 	unsigned long twcr_port	= sc_base + (drive->dn ? 0x06 : 0x04);
-	unsigned long nsectors	= hwgroup->rq->nr_sectors;
+	unsigned long nsectors	= hwif->rq->nr_sectors;
 
 	/*
 	 * We have to manually load the sector count and size into
@@ -125,8 +123,8 @@ static void tc86c001_dma_start(ide_drive_t *drive)
 	outw(SECTOR_SIZE / 2, twcr_port); /* Transfer Word Count 1/2 */
 
 	/* Install our timeout expiry hook, saving the current handler... */
-	ide_set_hwifdata(hwif, hwgroup->expiry);
-	hwgroup->expiry = &tc86c001_timer_expiry;
+	ide_set_hwifdata(hwif, hwif->expiry);
+	hwif->expiry = &tc86c001_timer_expiry;
 
 	ide_dma_start(drive);
 }
@@ -190,6 +188,7 @@ static const struct ide_dma_ops tc86c001_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_port_info tc86c001_chipset __devinitdata = {
diff --git a/drivers/ide/triflex.c b/drivers/ide/triflex.c
index b6ff40336aa9..8773c3ba7462 100644
--- a/drivers/ide/triflex.c
+++ b/drivers/ide/triflex.c
@@ -36,7 +36,7 @@
 
 static void triflex_set_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
 	u32 triflex_timings = 0;
 	u16 timing = 0;
diff --git a/drivers/ide/trm290.c b/drivers/ide/trm290.c
index 2a5ea90cf8b8..b6a1285a4021 100644
--- a/drivers/ide/trm290.c
+++ b/drivers/ide/trm290.c
@@ -144,7 +144,7 @@
 
 static void trm290_prepare_drive (ide_drive_t *drive, unsigned int use_dma)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	u16 reg = 0;
 	unsigned long flags;
 
@@ -184,7 +184,7 @@ static void trm290_dma_exec_cmd(ide_drive_t *drive, u8 command)
 static int trm290_dma_setup(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = hwif->hwgroup->rq;
+	struct request *rq = hwif->rq;
 	unsigned int count, rw;
 
 	if (rq_data_dir(rq)) {
@@ -222,15 +222,15 @@ static int trm290_dma_end(ide_drive_t *drive)
 	drive->waiting_for_dma = 0;
 	/* purge DMA mappings */
 	ide_destroy_dmatable(drive);
-	status = inw(HWIF(drive)->dma_base + 2);
+	status = inw(drive->hwif->dma_base + 2);
+
 	return status != 0x00ff;
 }
 
 static int trm290_dma_test_irq(ide_drive_t *drive)
 {
-	u16 status;
+	u16 status = inw(drive->hwif->dma_base + 2);
 
-	status = inw(HWIF(drive)->dma_base + 2);
 	return status == 0x00ff;
 }
 
diff --git a/drivers/ide/tx4939ide.c b/drivers/ide/tx4939ide.c
index 4a8c5a21bd4c..882f6f07c476 100644
--- a/drivers/ide/tx4939ide.c
+++ b/drivers/ide/tx4939ide.c
@@ -293,7 +293,7 @@ static int tx4939ide_dma_setup(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	void __iomem *base = TX4939IDE_BASE(hwif);
-	struct request *rq = hwif->hwgroup->rq;
+	struct request *rq = hwif->rq;
 	u8 reading;
 	int nent;
 
@@ -397,6 +397,17 @@ static int tx4939ide_dma_test_irq(ide_drive_t *drive)
 	return found;
 }
 
+#ifdef __BIG_ENDIAN
+static u8 tx4939ide_dma_sff_read_status(ide_hwif_t *hwif)
+{
+	void __iomem *base = TX4939IDE_BASE(hwif);
+
+	return tx4939ide_readb(base, TX4939IDE_DMA_Stat);
+}
+#else
+#define tx4939ide_dma_sff_read_status ide_dma_sff_read_status
+#endif
+
 static void tx4939ide_init_hwif(ide_hwif_t *hwif)
 {
 	void __iomem *base = TX4939IDE_BASE(hwif);
@@ -443,13 +454,6 @@ static void tx4939ide_tf_load_fixup(ide_drive_t *drive, ide_task_t *task)
 
 #ifdef __BIG_ENDIAN
 
-static u8 tx4939ide_read_sff_dma_status(ide_hwif_t *hwif)
-{
-	void __iomem *base = TX4939IDE_BASE(hwif);
-
-	return tx4939ide_readb(base, TX4939IDE_DMA_Stat);
-}
-
 /* custom iops (independent from SWAP_IO_SPACE) */
 static u8 tx4939ide_inb(unsigned long port)
 {
@@ -585,7 +589,6 @@ static const struct ide_tp_ops tx4939ide_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= tx4939ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
@@ -609,7 +612,6 @@ static const struct ide_tp_ops tx4939ide_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
@@ -638,6 +640,7 @@ static const struct ide_dma_ops tx4939ide_dma_ops = {
 	.dma_test_irq		= tx4939ide_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= tx4939ide_dma_sff_read_status,
 };
 
 static const struct ide_port_info tx4939ide_port_info __initdata = {
diff --git a/drivers/ide/umc8672.c b/drivers/ide/umc8672.c
index e29978cf6197..0608d41fb6d0 100644
--- a/drivers/ide/umc8672.c
+++ b/drivers/ide/umc8672.c
@@ -106,22 +106,21 @@ static void umc_set_speeds(u8 speeds[])
 
 static void umc_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *mate_hwgroup = hwif->mate ? hwif->mate->hwgroup : NULL;
+	ide_hwif_t *hwif = drive->hwif, *mate = hwif->mate;
 	unsigned long uninitialized_var(flags);
 
 	printk("%s: setting umc8672 to PIO mode%d (speed %d)\n",
 		drive->name, pio, pio_to_umc[pio]);
-	if (mate_hwgroup)
-		spin_lock_irqsave(&mate_hwgroup->lock, flags);
-	if (mate_hwgroup && mate_hwgroup->handler) {
+	if (mate)
+		spin_lock_irqsave(&mate->lock, flags);
+	if (mate && mate->handler) {
 		printk(KERN_ERR "umc8672: other interface is busy: exiting tune_umc()\n");
 	} else {
 		current_speeds[drive->name[2] - 'a'] = pio_to_umc[pio];
 		umc_set_speeds(current_speeds);
 	}
-	if (mate_hwgroup)
-		spin_unlock_irqrestore(&mate_hwgroup->lock, flags);
+	if (mate)
+		spin_unlock_irqrestore(&mate->lock, flags);
 }
 
 static const struct ide_port_ops umc8672_port_ops = {
diff --git a/drivers/ide/via82cxxx.c b/drivers/ide/via82cxxx.c
index 2a812d3207e9..fecc0e03c3fc 100644
--- a/drivers/ide/via82cxxx.c
+++ b/drivers/ide/via82cxxx.c
@@ -178,7 +178,7 @@ static void via_set_drive(ide_drive_t *drive, const u8 speed)
 		ide_timing_merge(&p, &t, &t, IDE_TIMING_8BIT);
 	}
 
-	via_set_speed(HWIF(drive), drive->dn, &t);
+	via_set_speed(hwif, drive->dn, &t);
 }
 
 /**
diff --git a/drivers/idle/i7300_idle.c b/drivers/idle/i7300_idle.c
index fb176f6ef9f8..17e8ddd01334 100644
--- a/drivers/idle/i7300_idle.c
+++ b/drivers/idle/i7300_idle.c
@@ -177,7 +177,7 @@ static int __init i7300_idle_ioat_selftest(u8 *ctl,
 }
 
 static struct device dummy_dma_dev = {
-	.bus_id = "fallback device",
+	.init_name = "fallback device",
 	.coherent_dma_mask = DMA_64BIT_MASK,
 	.dma_mask = &dummy_dma_dev.coherent_dma_mask,
 };
diff --git a/drivers/ieee1394/csr.c b/drivers/ieee1394/csr.c
index c90be4070e40..31400c8ae051 100644
--- a/drivers/ieee1394/csr.c
+++ b/drivers/ieee1394/csr.c
@@ -68,22 +68,22 @@ static struct hpsb_highlevel csr_highlevel = {
 	.host_reset =	host_reset,
 };
 
-static struct hpsb_address_ops map_ops = {
+const static struct hpsb_address_ops map_ops = {
 	.read = read_maps,
 };
 
-static struct hpsb_address_ops fcp_ops = {
+const static struct hpsb_address_ops fcp_ops = {
 	.write = write_fcp,
 };
 
-static struct hpsb_address_ops reg_ops = {
+const static struct hpsb_address_ops reg_ops = {
 	.read = read_regs,
 	.write = write_regs,
 	.lock = lock_regs,
 	.lock64 = lock64_regs,
 };
 
-static struct hpsb_address_ops config_rom_ops = {
+const static struct hpsb_address_ops config_rom_ops = {
 	.read = read_config_rom,
 };
 
@@ -217,7 +217,7 @@ static void add_host(struct hpsb_host *host)
 
 	host->csr.generation = 2;
 
-	bus_info[1] = __constant_cpu_to_be32(0x31333934);
+	bus_info[1] = IEEE1394_BUSID_MAGIC;
 	bus_info[2] = cpu_to_be32((hpsb_disable_irm ? 0 : 1 << CSR_IRMC_SHIFT) |
 				  (1 << CSR_CMC_SHIFT) |
 				  (1 << CSR_ISC_SHIFT) |
@@ -250,7 +250,7 @@ static void remove_host(struct hpsb_host *host)
 {
 	quadlet_t bus_info[CSR_BUS_INFO_SIZE];
 
-	bus_info[1] = __constant_cpu_to_be32(0x31333934);
+	bus_info[1] = IEEE1394_BUSID_MAGIC;
 	bus_info[2] = cpu_to_be32((0 << CSR_IRMC_SHIFT) |
 				  (0 << CSR_CMC_SHIFT) |
 				  (0 << CSR_ISC_SHIFT) |
diff --git a/drivers/ieee1394/csr.h b/drivers/ieee1394/csr.h
index f11546550d84..90fb3f2192c3 100644
--- a/drivers/ieee1394/csr.h
+++ b/drivers/ieee1394/csr.h
@@ -50,11 +50,11 @@
 #define CSR_MAX_ROM_SHIFT		8
 #define CSR_GENERATION_SHIFT		4
 
-#define CSR_SET_BUS_INFO_GENERATION(csr, gen)				\
-	((csr)->bus_info_data[2] =					\
-		cpu_to_be32((be32_to_cpu((csr)->bus_info_data[2]) &	\
-			     ~(0xf << CSR_GENERATION_SHIFT)) |		\
-			    (gen) << CSR_GENERATION_SHIFT))
+static inline void csr_set_bus_info_generation(struct csr1212_csr *csr, u8 gen)
+{
+	csr->bus_info_data[2] &= ~cpu_to_be32(0xf << CSR_GENERATION_SHIFT);
+	csr->bus_info_data[2] |= cpu_to_be32((u32)gen << CSR_GENERATION_SHIFT);
+}
 
 struct csr_control {
 	spinlock_t lock;
diff --git a/drivers/ieee1394/csr1212.c b/drivers/ieee1394/csr1212.c
index 5e38a68b8af2..a6dfeb0b3372 100644
--- a/drivers/ieee1394/csr1212.c
+++ b/drivers/ieee1394/csr1212.c
@@ -1077,15 +1077,10 @@ static int csr1212_parse_bus_info_block(struct csr1212_csr *csr)
 	int i;
 	int ret;
 
-	/* IEEE 1212 says that the entire bus info block should be readable in
-	 * a single transaction regardless of the max_rom value.
-	 * Unfortunately, many IEEE 1394 devices do not abide by that, so the
-	 * bus info block will be read 1 quadlet at a time.  The rest of the
-	 * ConfigROM will be read according to the max_rom field. */
 	for (i = 0; i < csr->bus_info_len; i += sizeof(u32)) {
 		ret = csr->ops->bus_read(csr, CSR1212_CONFIG_ROM_SPACE_BASE + i,
-			sizeof(u32), &csr->cache_head->data[bytes_to_quads(i)],
-			csr->private);
+				&csr->cache_head->data[bytes_to_quads(i)],
+				csr->private);
 		if (ret != CSR1212_SUCCESS)
 			return ret;
 
@@ -1104,8 +1099,8 @@ static int csr1212_parse_bus_info_block(struct csr1212_csr *csr)
 	 * a time. */
 	for (i = csr->bus_info_len; i <= csr->crc_len; i += sizeof(u32)) {
 		ret = csr->ops->bus_read(csr, CSR1212_CONFIG_ROM_SPACE_BASE + i,
-			sizeof(u32), &csr->cache_head->data[bytes_to_quads(i)],
-			csr->private);
+				&csr->cache_head->data[bytes_to_quads(i)],
+				csr->private);
 		if (ret != CSR1212_SUCCESS)
 			return ret;
 	}
@@ -1289,7 +1284,7 @@ csr1212_read_keyval(struct csr1212_csr *csr, struct csr1212_keyval *kv)
 
 		if (csr->ops->bus_read(csr,
 				       CSR1212_REGISTER_SPACE_BASE + kv->offset,
-				       sizeof(u32), &q, csr->private))
+				       &q, csr->private))
 			return -EIO;
 
 		kv->value.leaf.len = be32_to_cpu(q) >> 16;
@@ -1372,17 +1367,8 @@ csr1212_read_keyval(struct csr1212_csr *csr, struct csr1212_keyval *kv)
 		addr = (CSR1212_CSR_ARCH_REG_SPACE_BASE + cache->offset +
 			cr->offset_end) & ~(csr->max_rom - 1);
 
-		if (csr->ops->bus_read(csr, addr, csr->max_rom, cache_ptr,
-				       csr->private)) {
-			if (csr->max_rom == 4)
-				/* We've got problems! */
-				return -EIO;
-
-			/* Apperently the max_rom value was a lie, set it to
-			 * do quadlet reads and try again. */
-			csr->max_rom = 4;
-			continue;
-		}
+		if (csr->ops->bus_read(csr, addr, cache_ptr, csr->private))
+			return -EIO;
 
 		cr->offset_end += csr->max_rom - (cr->offset_end &
 						  (csr->max_rom - 1));
@@ -1433,7 +1419,6 @@ csr1212_get_keyval(struct csr1212_csr *csr, struct csr1212_keyval *kv)
 
 int csr1212_parse_csr(struct csr1212_csr *csr)
 {
-	static const int mr_map[] = { 4, 64, 1024, 0 };
 	struct csr1212_dentry *dentry;
 	int ret;
 
@@ -1443,15 +1428,13 @@ int csr1212_parse_csr(struct csr1212_csr *csr)
 	if (ret != CSR1212_SUCCESS)
 		return ret;
 
-	if (!csr->ops->get_max_rom) {
-		csr->max_rom = mr_map[0];	/* default value */
-	} else {
-		int i = csr->ops->get_max_rom(csr->bus_info_data,
-					      csr->private);
-		if (i & ~0x3)
-			return -EINVAL;
-		csr->max_rom = mr_map[i];
-	}
+	/*
+	 * There has been a buggy firmware with bus_info_block.max_rom > 0
+	 * spotted which actually only supported quadlet read requests to the
+	 * config ROM.  Therefore read everything quadlet by quadlet regardless
+	 * of what the bus info block says.
+	 */
+	csr->max_rom = 4;
 
 	csr->cache_head->layout_head = csr->root_kv;
 	csr->cache_head->layout_tail = csr->root_kv;
diff --git a/drivers/ieee1394/csr1212.h b/drivers/ieee1394/csr1212.h
index 043039fc63ec..a892d922dbc9 100644
--- a/drivers/ieee1394/csr1212.h
+++ b/drivers/ieee1394/csr1212.h
@@ -181,7 +181,7 @@ struct csr1212_csr_rom_cache {
 struct csr1212_csr {
 	size_t bus_info_len;	/* bus info block length in bytes */
 	size_t crc_len;		/* crc length in bytes */
-	u32 *bus_info_data;	/* bus info data incl bus name and EUI */
+	__be32 *bus_info_data;	/* bus info data incl bus name and EUI */
 
 	void *private;		/* private, bus specific data */
 	struct csr1212_bus_ops *ops;
@@ -200,7 +200,7 @@ struct csr1212_bus_ops {
 	 * entries located in the Units Space.  Must return 0 on success
 	 * anything else indicates an error. */
 	int (*bus_read) (struct csr1212_csr *csr, u64 addr,
-			 u16 length, void *buffer, void *private);
+			 void *buffer, void *private);
 
 	/* This function is used by csr1212 to allocate a region in units space
 	 * in the event that Config ROM entries don't all fit in the predefined
@@ -211,11 +211,6 @@ struct csr1212_bus_ops {
 	/* This function is used by csr1212 to release a region in units space
 	 * that is no longer needed. */
 	void (*release_addr) (u64 addr, void *private);
-
-	/* This function is used by csr1212 to determine the max read request
-	 * supported by a remote node when reading the ConfigROM space.  Must
-	 * return 0, 1, or 2 per IEEE 1212.  */
-	int (*get_max_rom) (u32 *bus_info, void *private);
 };
 
 
diff --git a/drivers/ieee1394/dv1394-private.h b/drivers/ieee1394/dv1394-private.h
index 7d1d2845b420..18b92cbf4a9f 100644
--- a/drivers/ieee1394/dv1394-private.h
+++ b/drivers/ieee1394/dv1394-private.h
@@ -77,11 +77,11 @@ static inline void fill_cip_header(struct CIP_header *cip,
    See the Texas Instruments OHCI 1394 chipset documentation.
 */
 
-struct output_more_immediate { u32 q[8]; };
-struct output_more { u32 q[4]; };
-struct output_last { u32 q[4]; };
-struct input_more { u32 q[4]; };
-struct input_last { u32 q[4]; };
+struct output_more_immediate { __le32 q[8]; };
+struct output_more { __le32 q[4]; };
+struct output_last { __le32 q[4]; };
+struct input_more { __le32 q[4]; };
+struct input_last { __le32 q[4]; };
 
 /* outputs */
 
@@ -92,9 +92,9 @@ static inline void fill_output_more_immediate(struct output_more_immediate *omi,
 					      unsigned int  payload_size)
 {
 	omi->q[0] = cpu_to_le32(0x02000000 | 8); /* OUTPUT_MORE_IMMEDIATE; 8 is the size of the IT header */
-	omi->q[1] = 0;
-	omi->q[2] = 0;
-	omi->q[3] = 0;
+	omi->q[1] = cpu_to_le32(0);
+	omi->q[2] = cpu_to_le32(0);
+	omi->q[3] = cpu_to_le32(0);
 
 	/* IT packet header */
 	omi->q[4] = cpu_to_le32(  (0x0 << 16)  /* IEEE1394_SPEED_100 */
@@ -106,8 +106,8 @@ static inline void fill_output_more_immediate(struct output_more_immediate *omi,
 	/* reserved field; mimic behavior of my Sony DSR-40 */
 	omi->q[5] = cpu_to_le32((payload_size << 16) | (0x7F << 8) | 0xA0);
 
-	omi->q[6] = 0;
-	omi->q[7] = 0;
+	omi->q[6] = cpu_to_le32(0);
+	omi->q[7] = cpu_to_le32(0);
 }
 
 static inline void fill_output_more(struct output_more *om,
@@ -116,8 +116,8 @@ static inline void fill_output_more(struct output_more *om,
 {
 	om->q[0] = cpu_to_le32(data_size);
 	om->q[1] = cpu_to_le32(data_phys_addr);
-	om->q[2] = 0;
-	om->q[3] = 0;
+	om->q[2] = cpu_to_le32(0);
+	om->q[3] = cpu_to_le32(0);
 }
 
 static inline void fill_output_last(struct output_last *ol,
@@ -140,8 +140,8 @@ static inline void fill_output_last(struct output_last *ol,
 
 	ol->q[0] = cpu_to_le32(temp);
 	ol->q[1] = cpu_to_le32(data_phys_addr);
-	ol->q[2] = 0;
-	ol->q[3] = 0;
+	ol->q[2] = cpu_to_le32(0);
+	ol->q[3] = cpu_to_le32(0);
 }
 
 /* inputs */
@@ -161,8 +161,8 @@ static inline void fill_input_more(struct input_more *im,
 
 	im->q[0] = cpu_to_le32(temp);
 	im->q[1] = cpu_to_le32(data_phys_addr);
-	im->q[2] = 0; /* branchAddress and Z not use in packet-per-buffer mode */
-	im->q[3] = 0; /* xferStatus & resCount, resCount must be initialize to data_size */
+	im->q[2] = cpu_to_le32(0); /* branchAddress and Z not use in packet-per-buffer mode */
+	im->q[3] = cpu_to_le32(0); /* xferStatus & resCount, resCount must be initialize to data_size */
 }
  
 static inline void fill_input_last(struct input_last *il,
@@ -331,7 +331,7 @@ struct frame {
 
 	/* points to status/timestamp field of first DMA packet */
 	/* (we'll check it later to monitor timestamp accuracy) */
-	u32 *frame_begin_timestamp;
+	__le32 *frame_begin_timestamp;
 
 	/* the timestamp we assigned to the first packet in the frame */
 	u32 assigned_timestamp;
@@ -348,15 +348,15 @@ struct frame {
 	   that can cause interrupts. We'll check these from the
 	   interrupt handler.
 	*/
-	u32 *mid_frame_timestamp;
-	u32 *frame_end_timestamp;
+	__le32 *mid_frame_timestamp;
+	__le32 *frame_end_timestamp;
 
 	/* branch address field of final packet. This is effectively
 	   the "tail" in the chain of DMA descriptor blocks.
 	   We will fill it with the address of the first DMA descriptor
 	   block in the subsequent frame, once it is ready.
 	*/
-	u32 *frame_end_branch;
+	__le32 *frame_end_branch;
 
 	/* the number of descriptors in the first descriptor block
 	   of the frame. Needed to start DMA */
@@ -365,10 +365,10 @@ struct frame {
 
 
 struct packet {
-	u16	timestamp;
+	__le16	timestamp;
 	u16	invalid;
 	u16	iso_header;
-	u16	data_length;
+	__le16	data_length;
 	u32	cip_h1;
 	u32	cip_h2;
 	unsigned char data[480];
diff --git a/drivers/ieee1394/dv1394.c b/drivers/ieee1394/dv1394.c
index c19f23267157..a329e6bd5d2d 100644
--- a/drivers/ieee1394/dv1394.c
+++ b/drivers/ieee1394/dv1394.c
@@ -265,7 +265,7 @@ static void frame_prepare(struct video_card *video, unsigned int this_frame)
 	/* these flags denote packets that need special attention */
 	int empty_packet, first_packet, last_packet, mid_packet;
 
-	u32 *branch_address, *last_branch_address = NULL;
+	__le32 *branch_address, *last_branch_address = NULL;
 	unsigned long data_p;
 	int first_packet_empty = 0;
 	u32 cycleTimer, ct_sec, ct_cyc, ct_off;
@@ -848,7 +848,7 @@ static void receive_packets(struct video_card *video)
 	dma_addr_t block_dma = 0;
 	struct packet *data = NULL;
 	dma_addr_t data_dma = 0;
-	u32 *last_branch_address = NULL;
+	__le32 *last_branch_address = NULL;
 	unsigned long irq_flags;
 	int want_interrupt = 0;
 	struct frame *f = NULL;
@@ -2110,17 +2110,17 @@ static void ir_tasklet_func(unsigned long data)
 			f = video->frames[next_i / MAX_PACKETS];
 			next = &(f->descriptor_pool[next_i % MAX_PACKETS]);
 			next_dma = ((unsigned long) block - (unsigned long) f->descriptor_pool) + f->descriptor_pool_dma;
-			next->u.in.il.q[0] |= 3 << 20; /* enable interrupt */
-			next->u.in.il.q[2] = 0; /* disable branch */
+			next->u.in.il.q[0] |= cpu_to_le32(3 << 20); /* enable interrupt */
+			next->u.in.il.q[2] = cpu_to_le32(0); /* disable branch */
 
 			/* link previous to next */
 			prev_i = (next_i == 0) ? (MAX_PACKETS * video->n_frames - 1) : (next_i - 1);
 			f = video->frames[prev_i / MAX_PACKETS];
 			prev = &(f->descriptor_pool[prev_i % MAX_PACKETS]);
 			if (prev_i % (MAX_PACKETS/2)) {
-				prev->u.in.il.q[0] &= ~(3 << 20); /* no interrupt */
+				prev->u.in.il.q[0] &= ~cpu_to_le32(3 << 20); /* no interrupt */
 			} else {
-				prev->u.in.il.q[0] |= 3 << 20; /* enable interrupt */
+				prev->u.in.il.q[0] |= cpu_to_le32(3 << 20); /* enable interrupt */
 			}
 			prev->u.in.il.q[2] = cpu_to_le32(next_dma | 1); /* set Z=1 */
 			wmb();
diff --git a/drivers/ieee1394/eth1394.c b/drivers/ieee1394/eth1394.c
index 20128692b339..a074bfd5f825 100644
--- a/drivers/ieee1394/eth1394.c
+++ b/drivers/ieee1394/eth1394.c
@@ -92,7 +92,7 @@ struct partial_datagram {
 	struct list_head list;
 	u16 dgl;
 	u16 dg_size;
-	u16 ether_type;
+	__be16 ether_type;
 	struct sk_buff *skb;
 	char *pbuf;
 	struct list_head frag_info;
@@ -181,7 +181,7 @@ static void ether1394_remove_host(struct hpsb_host *host);
 static void ether1394_host_reset(struct hpsb_host *host);
 
 /* Function for incoming 1394 packets */
-static struct hpsb_address_ops addr_ops = {
+const static struct hpsb_address_ops addr_ops = {
 	.write =	ether1394_write,
 };
 
@@ -767,7 +767,7 @@ static int ether1394_header_parse(const struct sk_buff *skb,
 static int ether1394_header_cache(const struct neighbour *neigh,
 				  struct hh_cache *hh)
 {
-	unsigned short type = hh->hh_type;
+	__be16 type = hh->hh_type;
 	struct net_device *dev = neigh->dev;
 	struct eth1394hdr *eth =
 		(struct eth1394hdr *)((u8 *)hh->hh_data + 16 - ETH1394_HLEN);
@@ -795,7 +795,7 @@ static void ether1394_header_cache_update(struct hh_cache *hh,
  ******************************************/
 
 /* Copied from net/ethernet/eth.c */
-static u16 ether1394_type_trans(struct sk_buff *skb, struct net_device *dev)
+static __be16 ether1394_type_trans(struct sk_buff *skb, struct net_device *dev)
 {
 	struct eth1394hdr *eth;
 	unsigned char *rawp;
@@ -829,17 +829,17 @@ static u16 ether1394_type_trans(struct sk_buff *skb, struct net_device *dev)
 
 /* Parse an encapsulated IP1394 header into an ethernet frame packet.
  * We also perform ARP translation here, if need be.  */
-static u16 ether1394_parse_encap(struct sk_buff *skb, struct net_device *dev,
+static __be16 ether1394_parse_encap(struct sk_buff *skb, struct net_device *dev,
 				 nodeid_t srcid, nodeid_t destid,
-				 u16 ether_type)
+				 __be16 ether_type)
 {
 	struct eth1394_priv *priv = netdev_priv(dev);
-	u64 dest_hw;
-	unsigned short ret = 0;
+	__be64 dest_hw;
+	__be16 ret = 0;
 
 	/* Setup our hw addresses. We use these to build the ethernet header. */
 	if (destid == (LOCAL_BUS | ALL_NODES))
-		dest_hw = ~0ULL;  /* broadcast */
+		dest_hw = ~cpu_to_be64(0);  /* broadcast */
 	else
 		dest_hw = cpu_to_be64((u64)priv->host->csr.guid_hi << 32 |
 				      priv->host->csr.guid_lo);
@@ -873,7 +873,7 @@ static u16 ether1394_parse_encap(struct sk_buff *skb, struct net_device *dev,
 		node = eth1394_find_node_guid(&priv->ip_node_list,
 					      be64_to_cpu(guid));
 		if (!node)
-			return 0;
+			return cpu_to_be16(0);
 
 		node_info =
 		    (struct eth1394_node_info *)node->ud->device.driver_data;
@@ -1063,7 +1063,7 @@ static int ether1394_data_handler(struct net_device *dev, int srcid, int destid,
 	unsigned long flags;
 	struct eth1394_priv *priv = netdev_priv(dev);
 	union eth1394_hdr *hdr = (union eth1394_hdr *)buf;
-	u16 ether_type = 0;  /* initialized to clear warning */
+	__be16 ether_type = cpu_to_be16(0);  /* initialized to clear warning */
 	int hdr_len;
 	struct unit_directory *ud = priv->ud_list[NODEID_TO_NODE(srcid)];
 	struct eth1394_node_info *node_info;
@@ -1259,7 +1259,7 @@ static int ether1394_write(struct hpsb_host *host, int srcid, int destid,
 
 static void ether1394_iso(struct hpsb_iso *iso)
 {
-	quadlet_t *data;
+	__be32 *data;
 	char *buf;
 	struct eth1394_host_info *hi;
 	struct net_device *dev;
@@ -1283,7 +1283,7 @@ static void ether1394_iso(struct hpsb_iso *iso)
 	for (i = 0; i < nready; i++) {
 		struct hpsb_iso_packet_info *info =
 			&iso->infos[(iso->first_packet + i) % iso->buf_packets];
-		data = (quadlet_t *)(iso->data_buf.kvirt + info->offset);
+		data = (__be32 *)(iso->data_buf.kvirt + info->offset);
 
 		/* skip over GASP header */
 		buf = (char *)data + 8;
@@ -1614,7 +1614,7 @@ static int ether1394_tx(struct sk_buff *skb, struct net_device *dev)
 		if (max_payload < dg_size + hdr_type_len[ETH1394_HDR_LF_UF])
 			priv->bc_dgl++;
 	} else {
-		__be64 guid = get_unaligned((u64 *)hdr_buf.h_dest);
+		__be64 guid = get_unaligned((__be64 *)hdr_buf.h_dest);
 
 		node = eth1394_find_node_guid(&priv->ip_node_list,
 					      be64_to_cpu(guid));
diff --git a/drivers/ieee1394/eth1394.h b/drivers/ieee1394/eth1394.h
index 4f3e2dd46f00..e1b5ea80f623 100644
--- a/drivers/ieee1394/eth1394.h
+++ b/drivers/ieee1394/eth1394.h
@@ -82,7 +82,7 @@ struct eth1394_priv {
 
 struct eth1394hdr {
 	unsigned char	h_dest[ETH1394_ALEN];	/* destination eth1394 addr	*/
-	unsigned short	h_proto;		/* packet type ID field	*/
+	__be16		h_proto;		/* packet type ID field	*/
 }  __attribute__((packed));
 
 static inline struct eth1394hdr *eth1394_hdr(const struct sk_buff *skb)
@@ -99,13 +99,13 @@ typedef enum {ETH1394_GASP, ETH1394_WRREQ} eth1394_tx_type;
 struct eth1394_uf_hdr {
 	u16 lf:2;
 	u16 res:14;
-	u16 ether_type;		/* Ethernet packet type */
+	__be16 ether_type;		/* Ethernet packet type */
 } __attribute__((packed));
 #elif defined __LITTLE_ENDIAN_BITFIELD
 struct eth1394_uf_hdr {
 	u16 res:14;
 	u16 lf:2;
-	u16 ether_type;
+	__be16 ether_type;
 } __attribute__((packed));
 #else
 #error Unknown bit field type
@@ -117,7 +117,7 @@ struct eth1394_ff_hdr {
 	u16 lf:2;
 	u16 res1:2;
 	u16 dg_size:12;		/* Datagram size */
-	u16 ether_type;		/* Ethernet packet type */
+	__be16 ether_type;		/* Ethernet packet type */
 	u16 dgl;		/* Datagram label */
 	u16 res2;
 } __attribute__((packed));
@@ -126,7 +126,7 @@ struct eth1394_ff_hdr {
 	u16 dg_size:12;
 	u16 res1:2;
 	u16 lf:2;
-	u16 ether_type;
+	__be16 ether_type;
 	u16 dgl;
 	u16 res2;
 } __attribute__((packed));
@@ -207,11 +207,11 @@ struct eth1394_arp {
 	u16 opcode;		/* ARP Opcode	*/
 	/* Above is exactly the same format as struct arphdr */
 
-	u64 s_uniq_id;		/* Sender's 64bit EUI			*/
+	__be64 s_uniq_id;	/* Sender's 64bit EUI		*/
 	u8 max_rec;		/* Sender's max packet size		*/
 	u8 sspd;		/* Sender's max speed			*/
-	u16 fifo_hi;		/* hi 16bits of sender's FIFO addr	*/
-	u32 fifo_lo;		/* lo 32bits of sender's FIFO addr	*/
+	__be16 fifo_hi;		/* hi 16bits of sender's FIFO addr	*/
+	__be32 fifo_lo;		/* lo 32bits of sender's FIFO addr	*/
 	u32 sip;		/* Sender's IP Address			*/
 	u32 tip;		/* IP Address of requested hw addr	*/
 };
diff --git a/drivers/ieee1394/highlevel.c b/drivers/ieee1394/highlevel.c
index 272543a42a43..600e391c8fe7 100644
--- a/drivers/ieee1394/highlevel.c
+++ b/drivers/ieee1394/highlevel.c
@@ -320,7 +320,7 @@ void hpsb_unregister_highlevel(struct hpsb_highlevel *hl)
  */
 u64 hpsb_allocate_and_register_addrspace(struct hpsb_highlevel *hl,
 					 struct hpsb_host *host,
-					 struct hpsb_address_ops *ops,
+					 const struct hpsb_address_ops *ops,
 					 u64 size, u64 alignment,
 					 u64 start, u64 end)
 {
@@ -407,7 +407,8 @@ u64 hpsb_allocate_and_register_addrspace(struct hpsb_highlevel *hl,
  * are automatically deallocated together with the hpsb_highlevel @hl.
  */
 int hpsb_register_addrspace(struct hpsb_highlevel *hl, struct hpsb_host *host,
-			    struct hpsb_address_ops *ops, u64 start, u64 end)
+			    const struct hpsb_address_ops *ops,
+			    u64 start, u64 end)
 {
 	struct hpsb_address_serve *as;
 	struct list_head *lh;
@@ -420,7 +421,7 @@ int hpsb_register_addrspace(struct hpsb_highlevel *hl, struct hpsb_host *host,
 		return 0;
 	}
 
-	as = kmalloc(sizeof(*as), GFP_ATOMIC);
+	as = kmalloc(sizeof(*as), GFP_KERNEL);
 	if (!as)
 		return 0;
 
@@ -477,7 +478,7 @@ int hpsb_unregister_addrspace(struct hpsb_highlevel *hl, struct hpsb_host *host,
 	return retval;
 }
 
-static struct hpsb_address_ops dummy_ops;
+const static struct hpsb_address_ops dummy_ops;
 
 /* dummy address spaces as lower and upper bounds of the host's a.s. list */
 static void init_hpsb_highlevel(struct hpsb_host *host)
diff --git a/drivers/ieee1394/highlevel.h b/drivers/ieee1394/highlevel.h
index bc5d0854c17e..9dba89fc60ad 100644
--- a/drivers/ieee1394/highlevel.h
+++ b/drivers/ieee1394/highlevel.h
@@ -15,7 +15,7 @@ struct hpsb_host;
 struct hpsb_address_serve {
 	struct list_head host_list;	/* per host list */
 	struct list_head hl_list;	/* hpsb_highlevel list */
-	struct hpsb_address_ops *op;
+	const struct hpsb_address_ops *op;
 	struct hpsb_host *host;
 	u64 start;	/* first address handled, quadlet aligned */
 	u64 end;	/* first address behind, quadlet aligned */
@@ -119,11 +119,12 @@ void hpsb_unregister_highlevel(struct hpsb_highlevel *hl);
 
 u64 hpsb_allocate_and_register_addrspace(struct hpsb_highlevel *hl,
 					 struct hpsb_host *host,
-					 struct hpsb_address_ops *ops,
+					 const struct hpsb_address_ops *ops,
 					 u64 size, u64 alignment,
 					 u64 start, u64 end);
 int hpsb_register_addrspace(struct hpsb_highlevel *hl, struct hpsb_host *host,
-			    struct hpsb_address_ops *ops, u64 start, u64 end);
+			    const struct hpsb_address_ops *ops,
+			    u64 start, u64 end);
 int hpsb_unregister_addrspace(struct hpsb_highlevel *hl, struct hpsb_host *host,
 			      u64 start);
 
diff --git a/drivers/ieee1394/hosts.c b/drivers/ieee1394/hosts.c
index 237d0c9d69c6..e947d8ffac85 100644
--- a/drivers/ieee1394/hosts.c
+++ b/drivers/ieee1394/hosts.c
@@ -34,18 +34,18 @@ static void delayed_reset_bus(struct work_struct *work)
 {
 	struct hpsb_host *host =
 		container_of(work, struct hpsb_host, delayed_reset.work);
-	int generation = host->csr.generation + 1;
+	u8 generation = host->csr.generation + 1;
 
 	/* The generation field rolls over to 2 rather than 0 per IEEE
 	 * 1394a-2000. */
 	if (generation > 0xf || generation < 2)
 		generation = 2;
 
-	CSR_SET_BUS_INFO_GENERATION(host->csr.rom, generation);
+	csr_set_bus_info_generation(host->csr.rom, generation);
 	if (csr1212_generate_csr_image(host->csr.rom) != CSR1212_SUCCESS) {
 		/* CSR image creation failed.
 		 * Reset generation field and do not issue a bus reset. */
-		CSR_SET_BUS_INFO_GENERATION(host->csr.rom,
+		csr_set_bus_info_generation(host->csr.rom,
 					    host->csr.generation);
 		return;
 	}
diff --git a/drivers/ieee1394/hosts.h b/drivers/ieee1394/hosts.h
index dd229950acca..49c359022c54 100644
--- a/drivers/ieee1394/hosts.h
+++ b/drivers/ieee1394/hosts.h
@@ -154,7 +154,7 @@ struct hpsb_host_driver {
 	 * to set the hardware ConfigROM if the hardware supports handling
 	 * reads to the ConfigROM on its own. */
 	void (*set_hw_config_rom)(struct hpsb_host *host,
-				  quadlet_t *config_rom);
+				  __be32 *config_rom);
 
 	/* This function shall implement packet transmission based on
 	 * packet->type.  It shall CRC both parts of the packet (unless
diff --git a/drivers/ieee1394/ieee1394.h b/drivers/ieee1394/ieee1394.h
index 40492074c013..e0ae0d3d747f 100644
--- a/drivers/ieee1394/ieee1394.h
+++ b/drivers/ieee1394/ieee1394.h
@@ -121,6 +121,9 @@ extern const char *hpsb_speedto_str[];
 
 #include <asm/byteorder.h>
 
+/* '1' '3' '9' '4' in ASCII */
+#define IEEE1394_BUSID_MAGIC	cpu_to_be32(0x31333934)
+
 #ifdef __BIG_ENDIAN_BITFIELD
 
 struct selfid {
diff --git a/drivers/ieee1394/nodemgr.c b/drivers/ieee1394/nodemgr.c
index 79ef5fd928ae..906c5a98d814 100644
--- a/drivers/ieee1394/nodemgr.c
+++ b/drivers/ieee1394/nodemgr.c
@@ -67,7 +67,7 @@ static int nodemgr_check_speed(struct nodemgr_csr_info *ci, u64 addr,
 	for (i = IEEE1394_SPEED_100; i <= old_speed; i++) {
 		*speed = i;
 		error = hpsb_read(ci->host, ci->nodeid, ci->generation, addr,
-				  &q, sizeof(quadlet_t));
+				  &q, 4);
 		if (error)
 			break;
 		*buffer = q;
@@ -85,7 +85,7 @@ static int nodemgr_check_speed(struct nodemgr_csr_info *ci, u64 addr,
 	return error;
 }
 
-static int nodemgr_bus_read(struct csr1212_csr *csr, u64 addr, u16 length,
+static int nodemgr_bus_read(struct csr1212_csr *csr, u64 addr,
 			    void *buffer, void *__ci)
 {
 	struct nodemgr_csr_info *ci = (struct nodemgr_csr_info*)__ci;
@@ -93,7 +93,7 @@ static int nodemgr_bus_read(struct csr1212_csr *csr, u64 addr, u16 length,
 
 	for (i = 1; ; i++) {
 		error = hpsb_read(ci->host, ci->nodeid, ci->generation, addr,
-				  buffer, length);
+				  buffer, 4);
 		if (!error) {
 			ci->speed_unverified = 0;
 			break;
@@ -104,7 +104,7 @@ static int nodemgr_bus_read(struct csr1212_csr *csr, u64 addr, u16 length,
 
 		/* The ieee1394_core guessed the node's speed capability from
 		 * the self ID.  Check whether a lower speed works. */
-		if (ci->speed_unverified && length == sizeof(quadlet_t)) {
+		if (ci->speed_unverified) {
 			error = nodemgr_check_speed(ci, addr, buffer);
 			if (!error)
 				break;
@@ -115,20 +115,8 @@ static int nodemgr_bus_read(struct csr1212_csr *csr, u64 addr, u16 length,
 	return error;
 }
 
-#define OUI_FREECOM_TECHNOLOGIES_GMBH 0x0001db
-
-static int nodemgr_get_max_rom(quadlet_t *bus_info_data, void *__ci)
-{
-	/* Freecom FireWire Hard Drive firmware bug */
-	if (be32_to_cpu(bus_info_data[3]) >> 8 == OUI_FREECOM_TECHNOLOGIES_GMBH)
-		return 0;
-
-	return (be32_to_cpu(bus_info_data[2]) >> 8) & 0x3;
-}
-
 static struct csr1212_bus_ops nodemgr_csr_ops = {
 	.bus_read =	nodemgr_bus_read,
-	.get_max_rom =	nodemgr_get_max_rom
 };
 
 
diff --git a/drivers/ieee1394/nodemgr.h b/drivers/ieee1394/nodemgr.h
index 4f287a3561ba..15ea09733e84 100644
--- a/drivers/ieee1394/nodemgr.h
+++ b/drivers/ieee1394/nodemgr.h
@@ -31,9 +31,6 @@ struct csr1212_keyval;
 struct hpsb_host;
 struct ieee1394_device_id;
 
-/* '1' '3' '9' '4' in ASCII */
-#define IEEE1394_BUSID_MAGIC	__constant_cpu_to_be32(0x31333934)
-
 /* This is the start of a Node entry structure. It should be a stable API
  * for which to gather info from the Node Manager about devices attached
  * to the bus.  */
diff --git a/drivers/ieee1394/ohci1394.c b/drivers/ieee1394/ohci1394.c
index e509e13cb7a7..65c1429e4129 100644
--- a/drivers/ieee1394/ohci1394.c
+++ b/drivers/ieee1394/ohci1394.c
@@ -2973,7 +2973,7 @@ alloc_dma_trm_ctx(struct ti_ohci *ohci, struct dma_trm_ctx *d,
 	return 0;
 }
 
-static void ohci_set_hw_config_rom(struct hpsb_host *host, quadlet_t *config_rom)
+static void ohci_set_hw_config_rom(struct hpsb_host *host, __be32 *config_rom)
 {
 	struct ti_ohci *ohci = host->hostdata;
 
@@ -3199,15 +3199,16 @@ static int __devinit ohci1394_pci_probe(struct pci_dev *dev,
 	/* Now enable LPS, which we need in order to start accessing
 	 * most of the registers.  In fact, on some cards (ALI M5251),
 	 * accessing registers in the SClk domain without LPS enabled
-	 * will lock up the machine.  Wait 50msec to make sure we have
-	 * full link enabled.  */
+	 * will lock up the machine. */
 	reg_write(ohci, OHCI1394_HCControlSet, OHCI1394_HCControl_LPS);
 
 	/* Disable and clear interrupts */
 	reg_write(ohci, OHCI1394_IntEventClear, 0xffffffff);
 	reg_write(ohci, OHCI1394_IntMaskClear, 0xffffffff);
 
-	mdelay(50);
+	/* Flush MMIO writes and wait to make sure we have full link enabled. */
+	reg_read(ohci, OHCI1394_Version);
+	msleep(50);
 
 	/* Determine the number of available IR and IT contexts. */
 	ohci->nb_iso_rcv_ctx =
@@ -3233,8 +3234,9 @@ static int __devinit ohci1394_pci_probe(struct pci_dev *dev,
 	 * we need to get to that "no event", so enough should be initialized
 	 * by that point.
 	 */
-	if (request_irq(dev->irq, ohci_irq_handler, IRQF_SHARED,
-			 OHCI1394_DRIVER_NAME, ohci)) {
+	err = request_irq(dev->irq, ohci_irq_handler, IRQF_SHARED,
+			  OHCI1394_DRIVER_NAME, ohci);
+	if (err) {
 		PRINT_G(KERN_ERR, "Failed to allocate interrupt %d", dev->irq);
 		goto err;
 	}
@@ -3381,6 +3383,7 @@ static int ohci1394_pci_suspend(struct pci_dev *dev, pm_message_t state)
 	ohci_devctl(ohci->host, RESET_BUS, LONG_RESET_NO_FORCE_ROOT);
 	ohci_soft_reset(ohci);
 
+	free_irq(dev->irq, ohci);
 	err = pci_save_state(dev);
 	if (err) {
 		PRINT(KERN_ERR, "pci_save_state failed with %d", err);
@@ -3420,7 +3423,16 @@ static int ohci1394_pci_resume(struct pci_dev *dev)
 	reg_write(ohci, OHCI1394_HCControlSet, OHCI1394_HCControl_LPS);
 	reg_write(ohci, OHCI1394_IntEventClear, 0xffffffff);
 	reg_write(ohci, OHCI1394_IntMaskClear, 0xffffffff);
-	mdelay(50);
+	reg_read(ohci, OHCI1394_Version);
+	msleep(50);
+
+	err = request_irq(dev->irq, ohci_irq_handler, IRQF_SHARED,
+			  OHCI1394_DRIVER_NAME, ohci);
+	if (err) {
+		PRINT_G(KERN_ERR, "Failed to allocate interrupt %d", dev->irq);
+		return err;
+	}
+
 	ohci_initialize(ohci);
 
 	hpsb_resume_host(ohci->host);
diff --git a/drivers/ieee1394/pcilynx.c b/drivers/ieee1394/pcilynx.c
index 7aee1ac97c80..dc15cadb06ef 100644
--- a/drivers/ieee1394/pcilynx.c
+++ b/drivers/ieee1394/pcilynx.c
@@ -1463,7 +1463,7 @@ static int __devinit add_card(struct pci_dev *dev,
 
                                 /* info_length, crc_length and 1394 magic number to check, if it is really a bus info block */
 				if (((be32_to_cpu(lynx->bus_info_block[0]) & 0xffff0000) == 0x04040000) &&
-				    (lynx->bus_info_block[1] == __constant_cpu_to_be32(0x31333934)))
+				    (lynx->bus_info_block[1] == IEEE1394_BUSID_MAGIC))
                                 {
                                         PRINT(KERN_DEBUG, lynx->id, "read a valid bus info block from");
                                 } else {
diff --git a/drivers/ieee1394/pcilynx.h b/drivers/ieee1394/pcilynx.h
index ec27321f6724..693a169acea3 100644
--- a/drivers/ieee1394/pcilynx.h
+++ b/drivers/ieee1394/pcilynx.h
@@ -52,7 +52,7 @@ struct ti_lynx {
         void __iomem *local_rom;
         void __iomem *local_ram;
         void __iomem *aux_port;
-	quadlet_t bus_info_block[5];
+	__be32 bus_info_block[5];
 
         /*
          * use local RAM of LOCALRAM_SIZE bytes for PCLs, which allows for
diff --git a/drivers/ieee1394/raw1394.c b/drivers/ieee1394/raw1394.c
index bf7e761c12b1..bad66c65b0d6 100644
--- a/drivers/ieee1394/raw1394.c
+++ b/drivers/ieee1394/raw1394.c
@@ -90,7 +90,7 @@ static int arm_lock(struct hpsb_host *host, int nodeid, quadlet_t * store,
 static int arm_lock64(struct hpsb_host *host, int nodeid, octlet_t * store,
 		      u64 addr, octlet_t data, octlet_t arg, int ext_tcode,
 		      u16 flags);
-static struct hpsb_address_ops arm_ops = {
+const static struct hpsb_address_ops arm_ops = {
 	.read = arm_read,
 	.write = arm_write,
 	.lock = arm_lock,
diff --git a/drivers/ieee1394/sbp2.c b/drivers/ieee1394/sbp2.c
index a373c18cf7b8..ab1034ccb7fb 100644
--- a/drivers/ieee1394/sbp2.c
+++ b/drivers/ieee1394/sbp2.c
@@ -265,7 +265,7 @@ static struct hpsb_highlevel sbp2_highlevel = {
 	.host_reset	= sbp2_host_reset,
 };
 
-static struct hpsb_address_ops sbp2_ops = {
+const static struct hpsb_address_ops sbp2_ops = {
 	.write		= sbp2_handle_status_write
 };
 
@@ -275,7 +275,7 @@ static int sbp2_handle_physdma_write(struct hpsb_host *, int, int, quadlet_t *,
 static int sbp2_handle_physdma_read(struct hpsb_host *, int, quadlet_t *, u64,
 				    size_t, u16);
 
-static struct hpsb_address_ops sbp2_physdma_ops = {
+const static struct hpsb_address_ops sbp2_physdma_ops = {
 	.read		= sbp2_handle_physdma_read,
 	.write		= sbp2_handle_physdma_write,
 };
diff --git a/drivers/infiniband/core/sysfs.c b/drivers/infiniband/core/sysfs.c
index 4f4d1bb9f069..b43f7d3682d3 100644
--- a/drivers/infiniband/core/sysfs.c
+++ b/drivers/infiniband/core/sysfs.c
@@ -778,7 +778,7 @@ int ib_device_register_sysfs(struct ib_device *device)
 	class_dev->class      = &ib_class;
 	class_dev->driver_data = device;
 	class_dev->parent     = device->dma_device;
-	strlcpy(class_dev->bus_id, device->name, BUS_ID_SIZE);
+	dev_set_name(class_dev, device->name);
 
 	INIT_LIST_HEAD(&device->port_list);
 
diff --git a/drivers/infiniband/core/ucm.c b/drivers/infiniband/core/ucm.c
index e603736682bf..51bd9669cb1f 100644
--- a/drivers/infiniband/core/ucm.c
+++ b/drivers/infiniband/core/ucm.c
@@ -1266,8 +1266,7 @@ static void ib_ucm_add_one(struct ib_device *device)
 	ucm_dev->dev.parent = device->dma_device;
 	ucm_dev->dev.devt = ucm_dev->cdev.dev;
 	ucm_dev->dev.release = ib_ucm_release_dev;
-	snprintf(ucm_dev->dev.bus_id, BUS_ID_SIZE, "ucm%d",
-		 ucm_dev->devnum);
+	dev_set_name(&ucm_dev->dev, "ucm%d", ucm_dev->devnum);
 	if (device_register(&ucm_dev->dev))
 		goto err_cdev;
 
diff --git a/drivers/infiniband/hw/mlx4/cq.c b/drivers/infiniband/hw/mlx4/cq.c
index a3c5af1d7ec0..de5263beab4a 100644
--- a/drivers/infiniband/hw/mlx4/cq.c
+++ b/drivers/infiniband/hw/mlx4/cq.c
@@ -367,7 +367,7 @@ int mlx4_ib_resize_cq(struct ib_cq *ibcq, int entries, struct ib_udata *udata)
 		if (err)
 			goto out;
 	} else {
-		/* Can't be smaller then the number of outstanding CQEs */
+		/* Can't be smaller than the number of outstanding CQEs */
 		outst_cqe = mlx4_ib_get_outstanding_cqes(cq);
 		if (entries < outst_cqe + 1) {
 			err = 0;
diff --git a/drivers/infiniband/ulp/srp/ib_srp.c b/drivers/infiniband/ulp/srp/ib_srp.c
index 7c13db885bf6..54c8fe25c423 100644
--- a/drivers/infiniband/ulp/srp/ib_srp.c
+++ b/drivers/infiniband/ulp/srp/ib_srp.c
@@ -1949,8 +1949,7 @@ static struct srp_host *srp_add_port(struct srp_device *device, u8 port)
 
 	host->dev.class = &srp_class;
 	host->dev.parent = device->dev->dma_device;
-	snprintf(host->dev.bus_id, BUS_ID_SIZE, "srp-%s-%d",
-		 device->dev->name, port);
+	dev_set_name(&host->dev, "srp-%s-%d", device->dev->name, port);
 
 	if (device_register(&host->dev))
 		goto free_host;
diff --git a/drivers/input/Makefile b/drivers/input/Makefile
index 98c4f9a77876..4c9c745a7020 100644
--- a/drivers/input/Makefile
+++ b/drivers/input/Makefile
@@ -5,7 +5,7 @@
 # Each configuration option enables a list of files.
 
 obj-$(CONFIG_INPUT)		+= input-core.o
-input-core-objs := input.o ff-core.o
+input-core-objs := input.o input-compat.o ff-core.o
 
 obj-$(CONFIG_INPUT_FF_MEMLESS)	+= ff-memless.o
 obj-$(CONFIG_INPUT_POLLDEV)	+= input-polldev.o
diff --git a/drivers/input/evbug.c b/drivers/input/evbug.c
index 0353601ac3b5..f7c5c14ec12a 100644
--- a/drivers/input/evbug.c
+++ b/drivers/input/evbug.c
@@ -39,7 +39,7 @@ MODULE_LICENSE("GPL");
 static void evbug_event(struct input_handle *handle, unsigned int type, unsigned int code, int value)
 {
 	printk(KERN_DEBUG "evbug.c: Event. Dev: %s, Type: %d, Code: %d, Value: %d\n",
-		handle->dev->dev.bus_id, type, code, value);
+		dev_name(&handle->dev->dev), type, code, value);
 }
 
 static int evbug_connect(struct input_handler *handler, struct input_dev *dev,
@@ -65,7 +65,7 @@ static int evbug_connect(struct input_handler *handler, struct input_dev *dev,
 		goto err_unregister_handle;
 
 	printk(KERN_DEBUG "evbug.c: Connected device: %s (%s at %s)\n",
-		dev->dev.bus_id,
+		dev_name(&dev->dev),
 		dev->name ?: "unknown",
 		dev->phys ?: "unknown");
 
@@ -81,7 +81,7 @@ static int evbug_connect(struct input_handler *handler, struct input_dev *dev,
 static void evbug_disconnect(struct input_handle *handle)
 {
 	printk(KERN_DEBUG "evbug.c: Disconnected device: %s\n",
-		handle->dev->dev.bus_id);
+		dev_name(&handle->dev->dev));
 
 	input_close_device(handle);
 	input_unregister_handle(handle);
diff --git a/drivers/input/evdev.c b/drivers/input/evdev.c
index 1070db330d35..ed8baa0aec3c 100644
--- a/drivers/input/evdev.c
+++ b/drivers/input/evdev.c
@@ -19,7 +19,7 @@
 #include <linux/input.h>
 #include <linux/major.h>
 #include <linux/device.h>
-#include <linux/compat.h>
+#include "input-compat.h"
 
 struct evdev {
 	int exist;
@@ -290,187 +290,6 @@ static int evdev_open(struct inode *inode, struct file *file)
 	return error;
 }
 
-#ifdef CONFIG_COMPAT
-
-struct input_event_compat {
-	struct compat_timeval time;
-	__u16 type;
-	__u16 code;
-	__s32 value;
-};
-
-struct ff_periodic_effect_compat {
-	__u16 waveform;
-	__u16 period;
-	__s16 magnitude;
-	__s16 offset;
-	__u16 phase;
-
-	struct ff_envelope envelope;
-
-	__u32 custom_len;
-	compat_uptr_t custom_data;
-};
-
-struct ff_effect_compat {
-	__u16 type;
-	__s16 id;
-	__u16 direction;
-	struct ff_trigger trigger;
-	struct ff_replay replay;
-
-	union {
-		struct ff_constant_effect constant;
-		struct ff_ramp_effect ramp;
-		struct ff_periodic_effect_compat periodic;
-		struct ff_condition_effect condition[2]; /* One for each axis */
-		struct ff_rumble_effect rumble;
-	} u;
-};
-
-/* Note to the author of this code: did it ever occur to
-   you why the ifdefs are needed? Think about it again. -AK */
-#ifdef CONFIG_X86_64
-#  define COMPAT_TEST is_compat_task()
-#elif defined(CONFIG_IA64)
-#  define COMPAT_TEST IS_IA32_PROCESS(task_pt_regs(current))
-#elif defined(CONFIG_S390)
-#  define COMPAT_TEST test_thread_flag(TIF_31BIT)
-#elif defined(CONFIG_MIPS)
-#  define COMPAT_TEST test_thread_flag(TIF_32BIT_ADDR)
-#else
-#  define COMPAT_TEST test_thread_flag(TIF_32BIT)
-#endif
-
-static inline size_t evdev_event_size(void)
-{
-	return COMPAT_TEST ?
-		sizeof(struct input_event_compat) : sizeof(struct input_event);
-}
-
-static int evdev_event_from_user(const char __user *buffer,
-				 struct input_event *event)
-{
-	if (COMPAT_TEST) {
-		struct input_event_compat compat_event;
-
-		if (copy_from_user(&compat_event, buffer,
-				   sizeof(struct input_event_compat)))
-			return -EFAULT;
-
-		event->time.tv_sec = compat_event.time.tv_sec;
-		event->time.tv_usec = compat_event.time.tv_usec;
-		event->type = compat_event.type;
-		event->code = compat_event.code;
-		event->value = compat_event.value;
-
-	} else {
-		if (copy_from_user(event, buffer, sizeof(struct input_event)))
-			return -EFAULT;
-	}
-
-	return 0;
-}
-
-static int evdev_event_to_user(char __user *buffer,
-				const struct input_event *event)
-{
-	if (COMPAT_TEST) {
-		struct input_event_compat compat_event;
-
-		compat_event.time.tv_sec = event->time.tv_sec;
-		compat_event.time.tv_usec = event->time.tv_usec;
-		compat_event.type = event->type;
-		compat_event.code = event->code;
-		compat_event.value = event->value;
-
-		if (copy_to_user(buffer, &compat_event,
-				 sizeof(struct input_event_compat)))
-			return -EFAULT;
-
-	} else {
-		if (copy_to_user(buffer, event, sizeof(struct input_event)))
-			return -EFAULT;
-	}
-
-	return 0;
-}
-
-static int evdev_ff_effect_from_user(const char __user *buffer, size_t size,
-				     struct ff_effect *effect)
-{
-	if (COMPAT_TEST) {
-		struct ff_effect_compat *compat_effect;
-
-		if (size != sizeof(struct ff_effect_compat))
-			return -EINVAL;
-
-		/*
-		 * It so happens that the pointer which needs to be changed
-		 * is the last field in the structure, so we can copy the
-		 * whole thing and replace just the pointer.
-		 */
-
-		compat_effect = (struct ff_effect_compat *)effect;
-
-		if (copy_from_user(compat_effect, buffer,
-				   sizeof(struct ff_effect_compat)))
-			return -EFAULT;
-
-		if (compat_effect->type == FF_PERIODIC &&
-		    compat_effect->u.periodic.waveform == FF_CUSTOM)
-			effect->u.periodic.custom_data =
-				compat_ptr(compat_effect->u.periodic.custom_data);
-	} else {
-		if (size != sizeof(struct ff_effect))
-			return -EINVAL;
-
-		if (copy_from_user(effect, buffer, sizeof(struct ff_effect)))
-			return -EFAULT;
-	}
-
-	return 0;
-}
-
-#else
-
-static inline size_t evdev_event_size(void)
-{
-	return sizeof(struct input_event);
-}
-
-static int evdev_event_from_user(const char __user *buffer,
-				 struct input_event *event)
-{
-	if (copy_from_user(event, buffer, sizeof(struct input_event)))
-		return -EFAULT;
-
-	return 0;
-}
-
-static int evdev_event_to_user(char __user *buffer,
-				const struct input_event *event)
-{
-	if (copy_to_user(buffer, event, sizeof(struct input_event)))
-		return -EFAULT;
-
-	return 0;
-}
-
-static int evdev_ff_effect_from_user(const char __user *buffer, size_t size,
-				     struct ff_effect *effect)
-{
-	if (size != sizeof(struct ff_effect))
-		return -EINVAL;
-
-	if (copy_from_user(effect, buffer, sizeof(struct ff_effect)))
-		return -EFAULT;
-
-	return 0;
-}
-
-#endif /* CONFIG_COMPAT */
-
 static ssize_t evdev_write(struct file *file, const char __user *buffer,
 			   size_t count, loff_t *ppos)
 {
@@ -490,14 +309,14 @@ static ssize_t evdev_write(struct file *file, const char __user *buffer,
 
 	while (retval < count) {
 
-		if (evdev_event_from_user(buffer + retval, &event)) {
+		if (input_event_from_user(buffer + retval, &event)) {
 			retval = -EFAULT;
 			goto out;
 		}
 
 		input_inject_event(&evdev->handle,
 				   event.type, event.code, event.value);
-		retval += evdev_event_size();
+		retval += input_event_size();
 	}
 
  out:
@@ -531,7 +350,7 @@ static ssize_t evdev_read(struct file *file, char __user *buffer,
 	struct input_event event;
 	int retval;
 
-	if (count < evdev_event_size())
+	if (count < input_event_size())
 		return -EINVAL;
 
 	if (client->head == client->tail && evdev->exist &&
@@ -546,13 +365,13 @@ static ssize_t evdev_read(struct file *file, char __user *buffer,
 	if (!evdev->exist)
 		return -ENODEV;
 
-	while (retval + evdev_event_size() <= count &&
+	while (retval + input_event_size() <= count &&
 	       evdev_fetch_next_event(client, &event)) {
 
-		if (evdev_event_to_user(buffer + retval, &event))
+		if (input_event_to_user(buffer + retval, &event))
 			return -EFAULT;
 
-		retval += evdev_event_size();
+		retval += input_event_size();
 	}
 
 	return retval;
@@ -823,7 +642,7 @@ static long evdev_do_ioctl(struct file *file, unsigned int cmd,
 
 			if (_IOC_NR(cmd) == _IOC_NR(EVIOCSFF)) {
 
-				if (evdev_ff_effect_from_user(p, _IOC_SIZE(cmd), &effect))
+				if (input_ff_effect_from_user(p, _IOC_SIZE(cmd), &effect))
 					return -EFAULT;
 
 				error = input_ff_upload(dev, &effect, file);
@@ -1000,7 +819,7 @@ static int evdev_connect(struct input_handler *handler, struct input_dev *dev,
 	evdev->handle.handler = handler;
 	evdev->handle.private = evdev;
 
-	strlcpy(evdev->dev.bus_id, evdev->name, sizeof(evdev->dev.bus_id));
+	dev_set_name(&evdev->dev, evdev->name);
 	evdev->dev.devt = MKDEV(INPUT_MAJOR, EVDEV_MINOR_BASE + minor);
 	evdev->dev.class = &input_class;
 	evdev->dev.parent = &dev->dev;
diff --git a/drivers/input/gameport/gameport.c b/drivers/input/gameport/gameport.c
index 2880eaae157a..ebf4be5b7c4e 100644
--- a/drivers/input/gameport/gameport.c
+++ b/drivers/input/gameport/gameport.c
@@ -530,8 +530,7 @@ static void gameport_init_port(struct gameport *gameport)
 
 	mutex_init(&gameport->drv_mutex);
 	device_initialize(&gameport->dev);
-	snprintf(gameport->dev.bus_id, sizeof(gameport->dev.bus_id),
-		 "gameport%lu", (unsigned long)atomic_inc_return(&gameport_no) - 1);
+	dev_set_name(&gameport->dev, "gameport%lu", (unsigned long)atomic_inc_return(&gameport_no) - 1);
 	gameport->dev.bus = &gameport_bus;
 	gameport->dev.release = gameport_release_port;
 	if (gameport->parent)
diff --git a/drivers/input/gameport/ns558.c b/drivers/input/gameport/ns558.c
index 2b282cde4b89..db556b71ddda 100644
--- a/drivers/input/gameport/ns558.c
+++ b/drivers/input/gameport/ns558.c
@@ -226,7 +226,7 @@ static int ns558_pnp_probe(struct pnp_dev *dev, const struct pnp_device_id *did)
 	ns558->gameport = port;
 
 	gameport_set_name(port, "NS558 PnP Gameport");
-	gameport_set_phys(port, "pnp%s/gameport0", dev->dev.bus_id);
+	gameport_set_phys(port, "pnp%s/gameport0", dev_name(&dev->dev));
 	port->dev.parent = &dev->dev;
 	port->io = ioport;
 
diff --git a/drivers/input/input-compat.c b/drivers/input/input-compat.c
new file mode 100644
index 000000000000..1accb89ae66f
--- /dev/null
+++ b/drivers/input/input-compat.c
@@ -0,0 +1,135 @@
+/*
+ * 32bit compatibility wrappers for the input subsystem.
+ *
+ * Very heavily based on evdev.c - Copyright (c) 1999-2002 Vojtech Pavlik
+ *
+ * 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 <asm/uaccess.h>
+#include "input-compat.h"
+
+#ifdef CONFIG_COMPAT
+
+int input_event_from_user(const char __user *buffer,
+			  struct input_event *event)
+{
+	if (INPUT_COMPAT_TEST) {
+		struct input_event_compat compat_event;
+
+		if (copy_from_user(&compat_event, buffer,
+				   sizeof(struct input_event_compat)))
+			return -EFAULT;
+
+		event->time.tv_sec = compat_event.time.tv_sec;
+		event->time.tv_usec = compat_event.time.tv_usec;
+		event->type = compat_event.type;
+		event->code = compat_event.code;
+		event->value = compat_event.value;
+
+	} else {
+		if (copy_from_user(event, buffer, sizeof(struct input_event)))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+int input_event_to_user(char __user *buffer,
+			const struct input_event *event)
+{
+	if (INPUT_COMPAT_TEST) {
+		struct input_event_compat compat_event;
+
+		compat_event.time.tv_sec = event->time.tv_sec;
+		compat_event.time.tv_usec = event->time.tv_usec;
+		compat_event.type = event->type;
+		compat_event.code = event->code;
+		compat_event.value = event->value;
+
+		if (copy_to_user(buffer, &compat_event,
+				 sizeof(struct input_event_compat)))
+			return -EFAULT;
+
+	} else {
+		if (copy_to_user(buffer, event, sizeof(struct input_event)))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+int input_ff_effect_from_user(const char __user *buffer, size_t size,
+			      struct ff_effect *effect)
+{
+	if (INPUT_COMPAT_TEST) {
+		struct ff_effect_compat *compat_effect;
+
+		if (size != sizeof(struct ff_effect_compat))
+			return -EINVAL;
+
+		/*
+		 * It so happens that the pointer which needs to be changed
+		 * is the last field in the structure, so we can retrieve the
+		 * whole thing and replace just the pointer.
+		 */
+		compat_effect = (struct ff_effect_compat *)effect;
+
+		if (copy_from_user(compat_effect, buffer,
+				   sizeof(struct ff_effect_compat)))
+			return -EFAULT;
+
+		if (compat_effect->type == FF_PERIODIC &&
+		    compat_effect->u.periodic.waveform == FF_CUSTOM)
+			effect->u.periodic.custom_data =
+				compat_ptr(compat_effect->u.periodic.custom_data);
+	} else {
+		if (size != sizeof(struct ff_effect))
+			return -EINVAL;
+
+		if (copy_from_user(effect, buffer, sizeof(struct ff_effect)))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+#else
+
+int input_event_from_user(const char __user *buffer,
+			 struct input_event *event)
+{
+	if (copy_from_user(event, buffer, sizeof(struct input_event)))
+		return -EFAULT;
+
+	return 0;
+}
+
+int input_event_to_user(char __user *buffer,
+			const struct input_event *event)
+{
+	if (copy_to_user(buffer, event, sizeof(struct input_event)))
+		return -EFAULT;
+
+	return 0;
+}
+
+int input_ff_effect_from_user(const char __user *buffer, size_t size,
+			      struct ff_effect *effect)
+{
+	if (size != sizeof(struct ff_effect))
+		return -EINVAL;
+
+	if (copy_from_user(effect, buffer, sizeof(struct ff_effect)))
+		return -EFAULT;
+
+	return 0;
+}
+
+#endif /* CONFIG_COMPAT */
+
+EXPORT_SYMBOL_GPL(input_event_from_user);
+EXPORT_SYMBOL_GPL(input_event_to_user);
+EXPORT_SYMBOL_GPL(input_ff_effect_from_user);
diff --git a/drivers/input/input-compat.h b/drivers/input/input-compat.h
new file mode 100644
index 000000000000..47cd9eaee66a
--- /dev/null
+++ b/drivers/input/input-compat.h
@@ -0,0 +1,94 @@
+#ifndef _INPUT_COMPAT_H
+#define _INPUT_COMPAT_H
+
+/*
+ * 32bit compatibility wrappers for the input subsystem.
+ *
+ * Very heavily based on evdev.c - Copyright (c) 1999-2002 Vojtech Pavlik
+ *
+ * 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/compiler.h>
+#include <linux/compat.h>
+#include <linux/input.h>
+
+#ifdef CONFIG_COMPAT
+
+/* Note to the author of this code: did it ever occur to
+   you why the ifdefs are needed? Think about it again. -AK */
+#ifdef CONFIG_X86_64
+#  define INPUT_COMPAT_TEST is_compat_task()
+#elif defined(CONFIG_IA64)
+#  define INPUT_COMPAT_TEST IS_IA32_PROCESS(task_pt_regs(current))
+#elif defined(CONFIG_S390)
+#  define INPUT_COMPAT_TEST test_thread_flag(TIF_31BIT)
+#elif defined(CONFIG_MIPS)
+#  define INPUT_COMPAT_TEST test_thread_flag(TIF_32BIT_ADDR)
+#else
+#  define INPUT_COMPAT_TEST test_thread_flag(TIF_32BIT)
+#endif
+
+struct input_event_compat {
+	struct compat_timeval time;
+	__u16 type;
+	__u16 code;
+	__s32 value;
+};
+
+struct ff_periodic_effect_compat {
+	__u16 waveform;
+	__u16 period;
+	__s16 magnitude;
+	__s16 offset;
+	__u16 phase;
+
+	struct ff_envelope envelope;
+
+	__u32 custom_len;
+	compat_uptr_t custom_data;
+};
+
+struct ff_effect_compat {
+	__u16 type;
+	__s16 id;
+	__u16 direction;
+	struct ff_trigger trigger;
+	struct ff_replay replay;
+
+	union {
+		struct ff_constant_effect constant;
+		struct ff_ramp_effect ramp;
+		struct ff_periodic_effect_compat periodic;
+		struct ff_condition_effect condition[2]; /* One for each axis */
+		struct ff_rumble_effect rumble;
+	} u;
+};
+
+static inline size_t input_event_size(void)
+{
+	return INPUT_COMPAT_TEST ?
+		sizeof(struct input_event_compat) : sizeof(struct input_event);
+}
+
+#else
+
+static inline size_t input_event_size(void)
+{
+	return sizeof(struct input_event);
+}
+
+#endif /* CONFIG_COMPAT */
+
+int input_event_from_user(const char __user *buffer,
+			 struct input_event *event);
+
+int input_event_to_user(char __user *buffer,
+			const struct input_event *event);
+
+int input_ff_effect_from_user(const char __user *buffer, size_t size,
+			      struct ff_effect *effect);
+
+#endif /* _INPUT_COMPAT_H */
diff --git a/drivers/input/input.c b/drivers/input/input.c
index c13ced3e0d3d..1730d7331a5d 100644
--- a/drivers/input/input.c
+++ b/drivers/input/input.c
@@ -1389,8 +1389,8 @@ int input_register_device(struct input_dev *dev)
 	if (!dev->setkeycode)
 		dev->setkeycode = input_default_setkeycode;
 
-	snprintf(dev->dev.bus_id, sizeof(dev->dev.bus_id),
-		 "input%ld", (unsigned long) atomic_inc_return(&input_no) - 1);
+	dev_set_name(&dev->dev, "input%ld",
+		     (unsigned long) atomic_inc_return(&input_no) - 1);
 
 	error = device_add(&dev->dev);
 	if (error)
diff --git a/drivers/input/joydev.c b/drivers/input/joydev.c
index a85b1485e774..6f2366220a50 100644
--- a/drivers/input/joydev.c
+++ b/drivers/input/joydev.c
@@ -800,7 +800,7 @@ static int joydev_connect(struct input_handler *handler, struct input_dev *dev,
 		}
 	}
 
-	strlcpy(joydev->dev.bus_id, joydev->name, sizeof(joydev->dev.bus_id));
+	dev_set_name(&joydev->dev, joydev->name);
 	joydev->dev.devt = MKDEV(INPUT_MAJOR, JOYDEV_MINOR_BASE + minor);
 	joydev->dev.class = &input_class;
 	joydev->dev.parent = &dev->dev;
diff --git a/drivers/input/joystick/Kconfig b/drivers/input/joystick/Kconfig
index be5c14a5a0a4..b11419590cfe 100644
--- a/drivers/input/joystick/Kconfig
+++ b/drivers/input/joystick/Kconfig
@@ -294,4 +294,28 @@ config JOYSTICK_XPAD_LEDS
 	  This option enables support for the LED which surrounds the Big X on
 	  XBox 360 controller.
 
+config JOYSTICK_WALKERA0701
+	tristate "Walkera WK-0701 RC transmitter"
+	depends on HIGH_RES_TIMERS && PARPORT
+	help
+	  Say Y or M here if you have a Walkera WK-0701 transmitter which is
+	  supplied with a ready to fly Walkera helicopters such as HM36,
+	  HM37, HM60 and want to use it via parport as a joystick. More
+	  information is available: <file:Documentation/input/walkera0701.txt>
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called walkera0701.
+
+config JOYSTICK_MAPLE
+	tristate "Dreamcast control pad"
+	depends on MAPLE
+	help
+	  Say Y here if you have a SEGA Dreamcast and want to use your
+	  controller as a joystick.
+
+	  Most Dreamcast users will say Y.
+
+	  To compile this as a module choose M here: the module will be called
+	  maplecontrol.
+
 endif
diff --git a/drivers/input/joystick/Makefile b/drivers/input/joystick/Makefile
index fdbf8c4c2876..f3a8cbe2abb6 100644
--- a/drivers/input/joystick/Makefile
+++ b/drivers/input/joystick/Makefile
@@ -19,6 +19,7 @@ obj-$(CONFIG_JOYSTICK_IFORCE)		+= iforce/
 obj-$(CONFIG_JOYSTICK_INTERACT)		+= interact.o
 obj-$(CONFIG_JOYSTICK_JOYDUMP)		+= joydump.o
 obj-$(CONFIG_JOYSTICK_MAGELLAN)		+= magellan.o
+obj-$(CONFIG_JOYSTICK_MAPLE)		+= maplecontrol.o
 obj-$(CONFIG_JOYSTICK_SIDEWINDER)	+= sidewinder.o
 obj-$(CONFIG_JOYSTICK_SPACEBALL)	+= spaceball.o
 obj-$(CONFIG_JOYSTICK_SPACEORB)		+= spaceorb.o
@@ -29,4 +30,5 @@ obj-$(CONFIG_JOYSTICK_TWIDJOY)		+= twidjoy.o
 obj-$(CONFIG_JOYSTICK_WARRIOR)		+= warrior.o
 obj-$(CONFIG_JOYSTICK_XPAD)		+= xpad.o
 obj-$(CONFIG_JOYSTICK_ZHENHUA)		+= zhenhua.o
+obj-$(CONFIG_JOYSTICK_WALKERA0701)	+= walkera0701.o
 
diff --git a/drivers/input/joystick/maplecontrol.c b/drivers/input/joystick/maplecontrol.c
new file mode 100644
index 000000000000..e50047bfe938
--- /dev/null
+++ b/drivers/input/joystick/maplecontrol.c
@@ -0,0 +1,193 @@
+/*
+ *	SEGA Dreamcast controller driver
+ *	Based on drivers/usb/iforce.c
+ *
+ *	Copyright Yaegashi Takeshi, 2001
+ *	Adrian McMenamin, 2008
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/timer.h>
+#include <linux/maple.h>
+
+MODULE_AUTHOR("Adrian McMenamin <adrian@mcmen.demon.co.uk>");
+MODULE_DESCRIPTION("SEGA Dreamcast controller driver");
+MODULE_LICENSE("GPL");
+
+struct dc_pad {
+	struct input_dev *dev;
+	struct maple_device *mdev;
+};
+
+static void dc_pad_callback(struct mapleq *mq)
+{
+	unsigned short buttons;
+	struct maple_device *mapledev = mq->dev;
+	struct dc_pad *pad = maple_get_drvdata(mapledev);
+	struct input_dev *dev = pad->dev;
+	unsigned char *res = mq->recvbuf;
+
+	buttons = ~le16_to_cpup((__le16 *)(res + 8));
+
+	input_report_abs(dev, ABS_HAT0Y,
+		(buttons & 0x0010 ? -1 : 0) + (buttons & 0x0020 ? 1 : 0));
+	input_report_abs(dev, ABS_HAT0X,
+		(buttons & 0x0040 ? -1 : 0) + (buttons & 0x0080 ? 1 : 0));
+	input_report_abs(dev, ABS_HAT1Y,
+		(buttons & 0x1000 ? -1 : 0) + (buttons & 0x2000 ? 1 : 0));
+	input_report_abs(dev, ABS_HAT1X,
+		(buttons & 0x4000 ? -1 : 0) + (buttons & 0x8000 ? 1 : 0));
+
+	input_report_key(dev, BTN_C,      buttons & 0x0001);
+	input_report_key(dev, BTN_B,      buttons & 0x0002);
+	input_report_key(dev, BTN_A,      buttons & 0x0004);
+	input_report_key(dev, BTN_START,  buttons & 0x0008);
+	input_report_key(dev, BTN_Z,      buttons & 0x0100);
+	input_report_key(dev, BTN_Y,      buttons & 0x0200);
+	input_report_key(dev, BTN_X,      buttons & 0x0400);
+	input_report_key(dev, BTN_SELECT, buttons & 0x0800);
+
+	input_report_abs(dev, ABS_GAS,    res[10]);
+	input_report_abs(dev, ABS_BRAKE,  res[11]);
+	input_report_abs(dev, ABS_X,      res[12]);
+	input_report_abs(dev, ABS_Y,      res[13]);
+	input_report_abs(dev, ABS_RX,     res[14]);
+	input_report_abs(dev, ABS_RY,     res[15]);
+}
+
+static int dc_pad_open(struct input_dev *dev)
+{
+	struct dc_pad *pad = dev->dev.platform_data;
+
+	maple_getcond_callback(pad->mdev, dc_pad_callback, HZ/20,
+		MAPLE_FUNC_CONTROLLER);
+
+	return 0;
+}
+
+static void dc_pad_close(struct input_dev *dev)
+{
+	struct dc_pad *pad = dev->dev.platform_data;
+
+	maple_getcond_callback(pad->mdev, dc_pad_callback, 0,
+		MAPLE_FUNC_CONTROLLER);
+}
+
+/* allow the controller to be used */
+static int __devinit probe_maple_controller(struct device *dev)
+{
+	static const short btn_bit[32] = {
+		BTN_C, BTN_B, BTN_A, BTN_START, -1, -1, -1, -1,
+		BTN_Z, BTN_Y, BTN_X, BTN_SELECT, -1, -1, -1, -1,
+		-1, -1, -1, -1, -1, -1, -1, -1,
+		-1, -1, -1, -1, -1, -1, -1, -1,
+	};
+
+	static const short abs_bit[32] = {
+		-1, -1, -1, -1, ABS_HAT0Y, ABS_HAT0Y, ABS_HAT0X, ABS_HAT0X,
+		-1, -1, -1, -1, ABS_HAT1Y, ABS_HAT1Y, ABS_HAT1X, ABS_HAT1X,
+		ABS_GAS, ABS_BRAKE, ABS_X, ABS_Y, ABS_RX, ABS_RY, -1, -1,
+		-1, -1, -1, -1, -1, -1, -1, -1,
+	};
+
+	struct maple_device *mdev = to_maple_dev(dev);
+	struct maple_driver *mdrv = to_maple_driver(dev->driver);
+	int i, error;
+	struct dc_pad *pad;
+	struct input_dev *idev;
+	unsigned long data = be32_to_cpu(mdev->devinfo.function_data[0]);
+
+	pad = kzalloc(sizeof(struct dc_pad), GFP_KERNEL);
+	idev = input_allocate_device();
+	if (!pad || !idev) {
+		error = -ENOMEM;
+		goto fail;
+	}
+
+	pad->dev = idev;
+	pad->mdev = mdev;
+
+	idev->open = dc_pad_open;
+	idev->close = dc_pad_close;
+
+	for (i = 0; i < 32; i++) {
+		if (data & (1 << i)) {
+			if (btn_bit[i] >= 0)
+				__set_bit(btn_bit[i], idev->keybit);
+			else if (abs_bit[i] >= 0)
+				__set_bit(abs_bit[i], idev->absbit);
+		}
+	}
+
+	if (idev->keybit[BIT_WORD(BTN_JOYSTICK)])
+		idev->evbit[0] |= BIT_MASK(EV_KEY);
+
+	if (idev->absbit[0])
+		idev->evbit[0] |= BIT_MASK(EV_ABS);
+
+	for (i = ABS_X; i <= ABS_BRAKE; i++)
+		input_set_abs_params(idev, i, 0, 255, 0, 0);
+
+	for (i = ABS_HAT0X; i <= ABS_HAT3Y; i++)
+		input_set_abs_params(idev, i, 1, -1, 0, 0);
+
+	idev->dev.platform_data = pad;
+	idev->dev.parent = &mdev->dev;
+	idev->name = mdev->product_name;
+	idev->id.bustype = BUS_HOST;
+	input_set_drvdata(idev, pad);
+
+	error = input_register_device(idev);
+	if (error)
+		goto fail;
+
+	mdev->driver = mdrv;
+	maple_set_drvdata(mdev, pad);
+
+	return 0;
+
+fail:
+	input_free_device(idev);
+	kfree(pad);
+	maple_set_drvdata(mdev, NULL);
+	return error;
+}
+
+static int __devexit remove_maple_controller(struct device *dev)
+{
+	struct maple_device *mdev = to_maple_dev(dev);
+	struct dc_pad *pad = maple_get_drvdata(mdev);
+
+	mdev->callback = NULL;
+	input_unregister_device(pad->dev);
+	maple_set_drvdata(mdev, NULL);
+	kfree(pad);
+
+	return 0;
+}
+
+static struct maple_driver dc_pad_driver = {
+	.function =	MAPLE_FUNC_CONTROLLER,
+	.drv = {
+		.name	= "Dreamcast_controller",
+		.probe	= probe_maple_controller,
+		.remove	= __devexit_p(remove_maple_controller),
+	},
+};
+
+static int __init dc_pad_init(void)
+{
+	return maple_driver_register(&dc_pad_driver);
+}
+
+static void __exit dc_pad_exit(void)
+{
+	maple_driver_unregister(&dc_pad_driver);
+}
+
+module_init(dc_pad_init);
+module_exit(dc_pad_exit);
diff --git a/drivers/input/joystick/walkera0701.c b/drivers/input/joystick/walkera0701.c
new file mode 100644
index 000000000000..4dfa1eed4b7c
--- /dev/null
+++ b/drivers/input/joystick/walkera0701.c
@@ -0,0 +1,292 @@
+/*
+ *  Parallel port to Walkera WK-0701 TX joystick
+ *
+ *  Copyright (c) 2008 Peter Popovec
+ *
+ *  More about driver:  <file:Documentation/input/walkera0701.txt>
+ */
+
+/*
+ * 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 WK0701_DEBUG */
+
+#define RESERVE 20000
+#define SYNC_PULSE 1306000
+#define BIN0_PULSE 288000
+#define BIN1_PULSE 438000
+
+#define ANALOG_MIN_PULSE 318000
+#define ANALOG_MAX_PULSE 878000
+#define ANALOG_DELTA 80000
+
+#define BIN_SAMPLE ((BIN0_PULSE + BIN1_PULSE) / 2)
+
+#define NO_SYNC 25
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/parport.h>
+#include <linux/input.h>
+#include <linux/hrtimer.h>
+
+MODULE_AUTHOR("Peter Popovec <popovec@fei.tuke.sk>");
+MODULE_DESCRIPTION("Walkera WK-0701 TX as joystick");
+MODULE_LICENSE("GPL");
+
+static unsigned int walkera0701_pp_no;
+module_param_named(port, walkera0701_pp_no, int, 0);
+MODULE_PARM_DESC(port,
+		 "Parallel port adapter for Walkera WK-0701 TX (default is 0)");
+
+/*
+ * For now, only one device is supported, if somebody need more devices, code
+ * can be expanded, one struct walkera_dev per device must be allocated and
+ * set up by walkera0701_connect (release of device by walkera0701_disconnect)
+ */
+
+struct walkera_dev {
+	unsigned char buf[25];
+	u64 irq_time, irq_lasttime;
+	int counter;
+	int ack;
+
+	struct input_dev *input_dev;
+	struct hrtimer timer;
+
+	struct parport *parport;
+	struct pardevice *pardevice;
+};
+
+static struct walkera_dev w_dev;
+
+static inline void walkera0701_parse_frame(struct walkera_dev *w)
+{
+	int i;
+	int val1, val2, val3, val4, val5, val6, val7, val8;
+	int crc1, crc2;
+
+	for (crc1 = crc2 = i = 0; i < 10; i++) {
+		crc1 += w->buf[i] & 7;
+		crc2 += (w->buf[i] & 8) >> 3;
+	}
+	if ((w->buf[10] & 7) != (crc1 & 7))
+		return;
+	if (((w->buf[10] & 8) >> 3) != (((crc1 >> 3) + crc2) & 1))
+		return;
+	for (crc1 = crc2 = 0, i = 11; i < 23; i++) {
+		crc1 += w->buf[i] & 7;
+		crc2 += (w->buf[i] & 8) >> 3;
+	}
+	if ((w->buf[23] & 7) != (crc1 & 7))
+		return;
+	if (((w->buf[23] & 8) >> 3) != (((crc1 >> 3) + crc2) & 1))
+		return;
+	val1 = ((w->buf[0] & 7) * 256 + w->buf[1] * 16 + w->buf[2]) >> 2;
+	val1 *= ((w->buf[0] >> 2) & 2) - 1;	/* sign */
+	val2 = (w->buf[2] & 1) << 8 | (w->buf[3] << 4) | w->buf[4];
+	val2 *= (w->buf[2] & 2) - 1;	/* sign */
+	val3 = ((w->buf[5] & 7) * 256 + w->buf[6] * 16 + w->buf[7]) >> 2;
+	val3 *= ((w->buf[5] >> 2) & 2) - 1;	/* sign */
+	val4 = (w->buf[7] & 1) << 8 | (w->buf[8] << 4) | w->buf[9];
+	val4 *= (w->buf[7] & 2) - 1;	/* sign */
+	val5 = ((w->buf[11] & 7) * 256 + w->buf[12] * 16 + w->buf[13]) >> 2;
+	val5 *= ((w->buf[11] >> 2) & 2) - 1;	/* sign */
+	val6 = (w->buf[13] & 1) << 8 | (w->buf[14] << 4) | w->buf[15];
+	val6 *= (w->buf[13] & 2) - 1;	/* sign */
+	val7 = ((w->buf[16] & 7) * 256 + w->buf[17] * 16 + w->buf[18]) >> 2;
+	val7 *= ((w->buf[16] >> 2) & 2) - 1;	/*sign */
+	val8 = (w->buf[18] & 1) << 8 | (w->buf[19] << 4) | w->buf[20];
+	val8 *= (w->buf[18] & 2) - 1;	/*sign */
+
+#ifdef WK0701_DEBUG
+	{
+		int magic, magic_bit;
+		magic = (w->buf[21] << 4) | w->buf[22];
+		magic_bit = (w->buf[24] & 8) >> 3;
+		printk(KERN_DEBUG
+		       "walkera0701: %4d %4d %4d %4d  %4d %4d %4d %4d (magic %2x %d)\n",
+		       val1, val2, val3, val4, val5, val6, val7, val8, magic,
+		       magic_bit);
+	}
+#endif
+	input_report_abs(w->input_dev, ABS_X, val2);
+	input_report_abs(w->input_dev, ABS_Y, val1);
+	input_report_abs(w->input_dev, ABS_Z, val6);
+	input_report_abs(w->input_dev, ABS_THROTTLE, val3);
+	input_report_abs(w->input_dev, ABS_RUDDER, val4);
+	input_report_abs(w->input_dev, ABS_MISC, val7);
+	input_report_key(w->input_dev, BTN_GEAR_DOWN, val5 > 0);
+}
+
+static inline int read_ack(struct pardevice *p)
+{
+	return parport_read_status(p->port) & 0x40;
+}
+
+/* falling edge, prepare to BIN value calculation */
+static void walkera0701_irq_handler(void *handler_data)
+{
+	u64 pulse_time;
+	struct walkera_dev *w = handler_data;
+
+	w->irq_time = ktime_to_ns(ktime_get());
+	pulse_time = w->irq_time - w->irq_lasttime;
+	w->irq_lasttime = w->irq_time;
+
+	/* cancel timer, if in handler or active do resync */
+	if (unlikely(0 != hrtimer_try_to_cancel(&w->timer))) {
+		w->counter = NO_SYNC;
+		return;
+	}
+
+	if (w->counter < NO_SYNC) {
+		if (w->ack) {
+			pulse_time -= BIN1_PULSE;
+			w->buf[w->counter] = 8;
+		} else {
+			pulse_time -= BIN0_PULSE;
+			w->buf[w->counter] = 0;
+		}
+		if (w->counter == 24) {	/* full frame */
+			walkera0701_parse_frame(w);
+			w->counter = NO_SYNC;
+			if (abs(pulse_time - SYNC_PULSE) < RESERVE)	/* new frame sync */
+				w->counter = 0;
+		} else {
+			if ((pulse_time > (ANALOG_MIN_PULSE - RESERVE)
+			     && (pulse_time < (ANALOG_MAX_PULSE + RESERVE)))) {
+				pulse_time -= (ANALOG_MIN_PULSE - RESERVE);
+				pulse_time = (u32) pulse_time / ANALOG_DELTA;	/* overtiping is safe, pulsetime < s32.. */
+				w->buf[w->counter++] |= (pulse_time & 7);
+			} else
+				w->counter = NO_SYNC;
+		}
+	} else if (abs(pulse_time - SYNC_PULSE - BIN0_PULSE) <
+				RESERVE + BIN1_PULSE - BIN0_PULSE)	/* frame sync .. */
+		w->counter = 0;
+
+	hrtimer_start(&w->timer, ktime_set(0, BIN_SAMPLE), HRTIMER_MODE_REL);
+}
+
+static enum hrtimer_restart timer_handler(struct hrtimer
+					  *handle)
+{
+	struct walkera_dev *w;
+
+	w = container_of(handle, struct walkera_dev, timer);
+	w->ack = read_ack(w->pardevice);
+
+	return HRTIMER_NORESTART;
+}
+
+static int walkera0701_open(struct input_dev *dev)
+{
+	struct walkera_dev *w = input_get_drvdata(dev);
+
+	parport_enable_irq(w->parport);
+	return 0;
+}
+
+static void walkera0701_close(struct input_dev *dev)
+{
+	struct walkera_dev *w = input_get_drvdata(dev);
+
+	parport_disable_irq(w->parport);
+}
+
+static int walkera0701_connect(struct walkera_dev *w, int parport)
+{
+	int err = -ENODEV;
+
+	w->parport = parport_find_number(parport);
+	if (w->parport == NULL)
+		return -ENODEV;
+
+	if (w->parport->irq == -1) {
+		printk(KERN_ERR "walkera0701: parport without interrupt\n");
+		goto init_err;
+	}
+
+	err = -EBUSY;
+	w->pardevice = parport_register_device(w->parport, "walkera0701",
+				    NULL, NULL, walkera0701_irq_handler,
+				    PARPORT_DEV_EXCL, w);
+	if (!w->pardevice)
+		goto init_err;
+
+	if (parport_negotiate(w->pardevice->port, IEEE1284_MODE_COMPAT))
+		goto init_err1;
+
+	if (parport_claim(w->pardevice))
+		goto init_err1;
+
+	w->input_dev = input_allocate_device();
+	if (!w->input_dev)
+		goto init_err2;
+
+	input_set_drvdata(w->input_dev, w);
+	w->input_dev->name = "Walkera WK-0701 TX";
+	w->input_dev->phys = w->parport->name;
+	w->input_dev->id.bustype = BUS_PARPORT;
+
+	/* TODO what id vendor/product/version ? */
+	w->input_dev->id.vendor = 0x0001;
+	w->input_dev->id.product = 0x0001;
+	w->input_dev->id.version = 0x0100;
+	w->input_dev->open = walkera0701_open;
+	w->input_dev->close = walkera0701_close;
+
+	w->input_dev->evbit[0] = BIT(EV_ABS) | BIT_MASK(EV_KEY);
+	w->input_dev->keybit[BIT_WORD(BTN_GEAR_DOWN)] = BIT_MASK(BTN_GEAR_DOWN);
+
+	input_set_abs_params(w->input_dev, ABS_X, -512, 512, 0, 0);
+	input_set_abs_params(w->input_dev, ABS_Y, -512, 512, 0, 0);
+	input_set_abs_params(w->input_dev, ABS_Z, -512, 512, 0, 0);
+	input_set_abs_params(w->input_dev, ABS_THROTTLE, -512, 512, 0, 0);
+	input_set_abs_params(w->input_dev, ABS_RUDDER, -512, 512, 0, 0);
+	input_set_abs_params(w->input_dev, ABS_MISC, -512, 512, 0, 0);
+
+	err = input_register_device(w->input_dev);
+	if (err)
+		goto init_err3;
+
+	hrtimer_init(&w->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	w->timer.function = timer_handler;
+	return 0;
+
+ init_err3:
+	input_free_device(w->input_dev);
+ init_err2:
+	parport_release(w->pardevice);
+ init_err1:
+	parport_unregister_device(w->pardevice);
+ init_err:
+	parport_put_port(w->parport);
+	return err;
+}
+
+static void walkera0701_disconnect(struct walkera_dev *w)
+{
+	hrtimer_cancel(&w->timer);
+	input_unregister_device(w->input_dev);
+	parport_release(w->pardevice);
+	parport_unregister_device(w->pardevice);
+	parport_put_port(w->parport);
+}
+
+static int __init walkera0701_init(void)
+{
+	return walkera0701_connect(&w_dev, walkera0701_pp_no);
+}
+
+static void __exit walkera0701_exit(void)
+{
+	walkera0701_disconnect(&w_dev);
+}
+
+module_init(walkera0701_init);
+module_exit(walkera0701_exit);
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index efd70a974591..35561689ff38 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -268,6 +268,15 @@ config KEYBOARD_PXA27x
 	  To compile this driver as a module, choose M here: the
 	  module will be called pxa27x_keypad.
 
+config KEYBOARD_PXA930_ROTARY
+	tristate "PXA930/PXA935 Enhanced Rotary Controller Support"
+	depends on CPU_PXA930 || CPU_PXA935
+	help
+	  Enable support for PXA930/PXA935 Enhanced Rotary Controller.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called pxa930_rotary.
+
 config KEYBOARD_AAED2000
 	tristate "AAED-2000 keyboard"
 	depends on MACH_AAED2000
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index 0edc8f285d1c..36351e1190f9 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -20,6 +20,7 @@ obj-$(CONFIG_KEYBOARD_HIL)		+= hil_kbd.o
 obj-$(CONFIG_KEYBOARD_HIL_OLD)		+= hilkbd.o
 obj-$(CONFIG_KEYBOARD_OMAP)		+= omap-keypad.o
 obj-$(CONFIG_KEYBOARD_PXA27x)		+= pxa27x_keypad.o
+obj-$(CONFIG_KEYBOARD_PXA930_ROTARY)	+= pxa930_rotary.o
 obj-$(CONFIG_KEYBOARD_AAED2000)		+= aaed2000_kbd.o
 obj-$(CONFIG_KEYBOARD_GPIO)		+= gpio_keys.o
 obj-$(CONFIG_KEYBOARD_HP6XX)		+= jornada680_kbd.o
diff --git a/drivers/input/keyboard/atkbd.c b/drivers/input/keyboard/atkbd.c
index 379b7ff354ec..c3c8b9bc40ae 100644
--- a/drivers/input/keyboard/atkbd.c
+++ b/drivers/input/keyboard/atkbd.c
@@ -65,7 +65,7 @@ MODULE_PARM_DESC(extra, "Enable extra LEDs and keys on IBM RapidAcces, EzKey and
 
 /*
  * Scancode to keycode tables. These are just the default setting, and
- * are loadable via an userland utility.
+ * are loadable via a userland utility.
  */
 
 static const unsigned short atkbd_set2_keycode[512] = {
@@ -884,6 +884,39 @@ static void atkbd_inventec_keymap_fixup(struct atkbd *atkbd)
 }
 
 /*
+ * Perform fixup for HP Pavilion ZV6100 laptop that doesn't generate release
+ * for its volume buttons
+ */
+static void atkbd_hp_zv6100_keymap_fixup(struct atkbd *atkbd)
+{
+	const unsigned int forced_release_keys[] = {
+		0xae, 0xb0,
+	};
+	int i;
+
+	if (atkbd->set == 2)
+		for (i = 0; i < ARRAY_SIZE(forced_release_keys); i++)
+			__set_bit(forced_release_keys[i],
+					atkbd->force_release_mask);
+}
+
+/*
+ * Samsung NC10 with Fn+F? key release not working
+ */
+static void atkbd_samsung_keymap_fixup(struct atkbd *atkbd)
+{
+	const unsigned int forced_release_keys[] = {
+		0x82, 0x83, 0x84, 0x86, 0x88, 0x89, 0xb3, 0xf7, 0xf9,
+	};
+	int i;
+
+	if (atkbd->set == 2)
+		for (i = 0; i < ARRAY_SIZE(forced_release_keys); i++)
+			__set_bit(forced_release_keys[i],
+				  atkbd->force_release_mask);
+}
+
+/*
  * atkbd_set_keycode_table() initializes keyboard's keycode table
  * according to the selected scancode set
  */
@@ -1476,6 +1509,15 @@ static struct dmi_system_id atkbd_dmi_quirk_table[] __initdata = {
 		.driver_data = atkbd_dell_laptop_keymap_fixup,
 	},
 	{
+		.ident = "Dell Laptop",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Dell Computer Corporation"),
+			DMI_MATCH(DMI_CHASSIS_TYPE, "8"), /* Portable */
+		},
+		.callback = atkbd_setup_fixup,
+		.driver_data = atkbd_dell_laptop_keymap_fixup,
+	},
+	{
 		.ident = "HP 2133",
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
@@ -1485,6 +1527,15 @@ static struct dmi_system_id atkbd_dmi_quirk_table[] __initdata = {
 		.driver_data = atkbd_hp_keymap_fixup,
 	},
 	{
+		.ident = "HP Pavilion ZV6100",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Pavilion ZV6100"),
+		},
+		.callback = atkbd_setup_fixup,
+		.driver_data = atkbd_hp_zv6100_keymap_fixup,
+	},
+	{
 		.ident = "Inventec Symphony",
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "INVENTEC"),
@@ -1493,6 +1544,15 @@ static struct dmi_system_id atkbd_dmi_quirk_table[] __initdata = {
 		.callback = atkbd_setup_fixup,
 		.driver_data = atkbd_inventec_keymap_fixup,
 	},
+	{
+		.ident = "Samsung NC10",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "NC10"),
+		},
+		.callback = atkbd_setup_fixup,
+		.driver_data = atkbd_samsung_keymap_fixup,
+	},
 	{ }
 };
 
diff --git a/drivers/input/keyboard/gpio_keys.c b/drivers/input/keyboard/gpio_keys.c
index 05f3f43582c2..ad67d763fdbd 100644
--- a/drivers/input/keyboard/gpio_keys.c
+++ b/drivers/input/keyboard/gpio_keys.c
@@ -98,6 +98,10 @@ static int __devinit gpio_keys_probe(struct platform_device *pdev)
 	input->id.product = 0x0001;
 	input->id.version = 0x0100;
 
+	/* Enable auto repeat feature of Linux input subsystem */
+	if (pdata->rep)
+		__set_bit(EV_REP, input->evbit);
+
 	ddata->input = input;
 
 	for (i = 0; i < pdata->nbuttons; i++) {
diff --git a/drivers/input/keyboard/hil_kbd.c b/drivers/input/keyboard/hil_kbd.c
index 71c1971abf80..6f356705ee3b 100644
--- a/drivers/input/keyboard/hil_kbd.c
+++ b/drivers/input/keyboard/hil_kbd.c
@@ -47,6 +47,7 @@
 MODULE_AUTHOR("Brian S. Julin <bri@calyx.com>");
 MODULE_DESCRIPTION(HIL_GENERIC_NAME " driver");
 MODULE_LICENSE("Dual BSD/GPL");
+MODULE_ALIAS("serio:ty03pr25id00ex*");
 
 #define HIL_KBD_MAX_LENGTH 16
 
diff --git a/drivers/input/keyboard/omap-keypad.c b/drivers/input/keyboard/omap-keypad.c
index db22fd9b4cf2..3f3d1198cdb1 100644
--- a/drivers/input/keyboard/omap-keypad.c
+++ b/drivers/input/keyboard/omap-keypad.c
@@ -122,14 +122,10 @@ static void omap_kp_scan_keypad(struct omap_kp *omap_kp, unsigned char *state)
 
 	/* read the keypad status */
 	if (cpu_is_omap24xx()) {
-		int i;
-		for (i = 0; i < omap_kp->rows; i++)
-			disable_irq(OMAP_GPIO_IRQ(row_gpios[i]));
-
 		/* read the keypad status */
 		for (col = 0; col < omap_kp->cols; col++) {
 			set_col_gpio_val(omap_kp, ~(1 << col));
-			state[col] = ~(get_row_gpio_val(omap_kp)) & 0x3f;
+			state[col] = ~(get_row_gpio_val(omap_kp)) & 0xff;
 		}
 		set_col_gpio_val(omap_kp, 0);
 
diff --git a/drivers/input/keyboard/pxa930_rotary.c b/drivers/input/keyboard/pxa930_rotary.c
new file mode 100644
index 000000000000..95fbba470e65
--- /dev/null
+++ b/drivers/input/keyboard/pxa930_rotary.c
@@ -0,0 +1,212 @@
+/*
+ * Driver for the enhanced rotary controller on pxa930 and pxa935
+ *
+ * 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/module.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/input.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+
+#include <mach/pxa930_rotary.h>
+
+#define SBCR	(0x04)
+#define ERCR	(0x0c)
+
+#define SBCR_ERSB	(1 << 5)
+
+struct pxa930_rotary {
+	struct input_dev	*input_dev;
+	void __iomem		*mmio_base;
+	int			last_ercr;
+
+	struct pxa930_rotary_platform_data *pdata;
+};
+
+static void clear_sbcr(struct pxa930_rotary *r)
+{
+	uint32_t sbcr = __raw_readl(r->mmio_base + SBCR);
+
+	__raw_writel(sbcr | SBCR_ERSB, r->mmio_base + SBCR);
+	__raw_writel(sbcr & ~SBCR_ERSB, r->mmio_base + SBCR);
+}
+
+static irqreturn_t rotary_irq(int irq, void *dev_id)
+{
+	struct pxa930_rotary *r = dev_id;
+	struct pxa930_rotary_platform_data *pdata = r->pdata;
+	int ercr, delta, key;
+
+	ercr = __raw_readl(r->mmio_base + ERCR) & 0xf;
+	clear_sbcr(r);
+
+	delta = ercr - r->last_ercr;
+	if (delta == 0)
+		return IRQ_HANDLED;
+
+	r->last_ercr = ercr;
+
+	if (pdata->up_key && pdata->down_key) {
+		key = (delta > 0) ? pdata->up_key : pdata->down_key;
+		input_report_key(r->input_dev, key, 1);
+		input_sync(r->input_dev);
+		input_report_key(r->input_dev, key, 0);
+	} else
+		input_report_rel(r->input_dev, pdata->rel_code, delta);
+
+	input_sync(r->input_dev);
+
+	return IRQ_HANDLED;
+}
+
+static int pxa930_rotary_open(struct input_dev *dev)
+{
+	struct pxa930_rotary *r = input_get_drvdata(dev);
+
+	clear_sbcr(r);
+
+	return 0;
+}
+
+static void pxa930_rotary_close(struct input_dev *dev)
+{
+	struct pxa930_rotary *r = input_get_drvdata(dev);
+
+	clear_sbcr(r);
+}
+
+static int __devinit pxa930_rotary_probe(struct platform_device *pdev)
+{
+	struct pxa930_rotary_platform_data *pdata = pdev->dev.platform_data;
+	struct pxa930_rotary *r;
+	struct input_dev *input_dev;
+	struct resource *res;
+	int irq;
+	int err;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "no irq for rotary controller\n");
+		return -ENXIO;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "no I/O memory defined\n");
+		return -ENXIO;
+	}
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform data defined\n");
+		return -EINVAL;
+	}
+
+	r = kzalloc(sizeof(struct pxa930_rotary), GFP_KERNEL);
+	if (!r)
+		return -ENOMEM;
+
+	r->mmio_base = ioremap_nocache(res->start, resource_size(res));
+	if (r->mmio_base == NULL) {
+		dev_err(&pdev->dev, "failed to remap IO memory\n");
+		err = -ENXIO;
+		goto failed_free;
+	}
+
+	r->pdata = pdata;
+	platform_set_drvdata(pdev, r);
+
+	/* allocate and register the input device */
+	input_dev = input_allocate_device();
+	if (!input_dev) {
+		dev_err(&pdev->dev, "failed to allocate input device\n");
+		err = -ENOMEM;
+		goto failed_free_io;
+	}
+
+	input_dev->name = pdev->name;
+	input_dev->id.bustype = BUS_HOST;
+	input_dev->open = pxa930_rotary_open;
+	input_dev->close = pxa930_rotary_close;
+	input_dev->dev.parent = &pdev->dev;
+
+	if (pdata->up_key && pdata->down_key) {
+		__set_bit(pdata->up_key, input_dev->keybit);
+		__set_bit(pdata->down_key, input_dev->keybit);
+		__set_bit(EV_KEY, input_dev->evbit);
+	} else {
+		__set_bit(pdata->rel_code, input_dev->relbit);
+		__set_bit(EV_REL, input_dev->evbit);
+	}
+
+	r->input_dev = input_dev;
+	input_set_drvdata(input_dev, r);
+
+	err = request_irq(irq, rotary_irq, IRQF_DISABLED,
+			"enhanced rotary", r);
+	if (err) {
+		dev_err(&pdev->dev, "failed to request IRQ\n");
+		goto failed_free_input;
+	}
+
+	err = input_register_device(input_dev);
+	if (err) {
+		dev_err(&pdev->dev, "failed to register input device\n");
+		goto failed_free_irq;
+	}
+
+	return 0;
+
+failed_free_irq:
+	free_irq(irq, r);
+failed_free_input:
+	input_free_device(input_dev);
+failed_free_io:
+	iounmap(r->mmio_base);
+failed_free:
+	kfree(r);
+	return err;
+}
+
+static int __devexit pxa930_rotary_remove(struct platform_device *pdev)
+{
+	struct pxa930_rotary *r = platform_get_drvdata(pdev);
+
+	free_irq(platform_get_irq(pdev, 0), r);
+	input_unregister_device(r->input_dev);
+	iounmap(r->mmio_base);
+	platform_set_drvdata(pdev, NULL);
+	kfree(r);
+
+	return 0;
+}
+
+static struct platform_driver pxa930_rotary_driver = {
+	.driver		= {
+		.name	= "pxa930-rotary",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= pxa930_rotary_probe,
+	.remove		= __devexit_p(pxa930_rotary_remove),
+};
+
+static int __init pxa930_rotary_init(void)
+{
+	return platform_driver_register(&pxa930_rotary_driver);
+}
+module_init(pxa930_rotary_init);
+
+static void __exit pxa930_rotary_exit(void)
+{
+	platform_driver_unregister(&pxa930_rotary_driver);
+}
+module_exit(pxa930_rotary_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Driver for PXA93x Enhanced Rotary Controller");
+MODULE_AUTHOR("Yao Yong <yaoyong@marvell.com>");
diff --git a/drivers/input/misc/apanel.c b/drivers/input/misc/apanel.c
index d82f7f727f7a..71b82434264d 100644
--- a/drivers/input/misc/apanel.c
+++ b/drivers/input/misc/apanel.c
@@ -57,7 +57,7 @@ static enum apanel_chip device_chip[APANEL_DEV_MAX];
 
 struct apanel {
 	struct input_polled_dev *ipdev;
-	struct i2c_client client;
+	struct i2c_client *client;
 	unsigned short keymap[MAX_PANEL_KEYS];
 	u16    nkeys;
 	u16    led_bits;
@@ -66,16 +66,7 @@ struct apanel {
 };
 
 
-static int apanel_probe(struct i2c_adapter *, int, int);
-
-/* for now, we only support one address */
-static unsigned short normal_i2c[] = {0, I2C_CLIENT_END};
-static unsigned short ignore = I2C_CLIENT_END;
-static struct i2c_client_address_data addr_data = {
-	.normal_i2c	= normal_i2c,
-	.probe		= &ignore,
-	.ignore		= &ignore,
-};
+static int apanel_probe(struct i2c_client *, const struct i2c_device_id *);
 
 static void report_key(struct input_dev *input, unsigned keycode)
 {
@@ -103,12 +94,12 @@ static void apanel_poll(struct input_polled_dev *ipdev)
 	s32 data;
 	int i;
 
-	data = i2c_smbus_read_word_data(&ap->client, cmd);
+	data = i2c_smbus_read_word_data(ap->client, cmd);
 	if (data < 0)
 		return;	/* ignore errors (due to ACPI??) */
 
 	/* write back to clear latch */
-	i2c_smbus_write_word_data(&ap->client, cmd, 0);
+	i2c_smbus_write_word_data(ap->client, cmd, 0);
 
 	if (!data)
 		return;
@@ -124,7 +115,7 @@ static void led_update(struct work_struct *work)
 {
 	struct apanel *ap = container_of(work, struct apanel, led_work);
 
-	i2c_smbus_write_word_data(&ap->client, 0x10, ap->led_bits);
+	i2c_smbus_write_word_data(ap->client, 0x10, ap->led_bits);
 }
 
 static void mail_led_set(struct led_classdev *led,
@@ -140,7 +131,7 @@ static void mail_led_set(struct led_classdev *led,
 	schedule_work(&ap->led_work);
 }
 
-static int apanel_detach_client(struct i2c_client *client)
+static int apanel_remove(struct i2c_client *client)
 {
 	struct apanel *ap = i2c_get_clientdata(client);
 
@@ -148,43 +139,33 @@ static int apanel_detach_client(struct i2c_client *client)
 		led_classdev_unregister(&ap->mail_led);
 
 	input_unregister_polled_device(ap->ipdev);
-	i2c_detach_client(&ap->client);
 	input_free_polled_device(ap->ipdev);
 
 	return 0;
 }
 
-/* Function is invoked for every i2c adapter. */
-static int apanel_attach_adapter(struct i2c_adapter *adap)
-{
-	dev_dbg(&adap->dev, APANEL ": attach adapter id=%d\n", adap->id);
-
-	/* Our device is connected only to i801 on laptop */
-	if (adap->id != I2C_HW_SMBUS_I801)
-		return -ENODEV;
-
-	return i2c_probe(adap, &addr_data, apanel_probe);
-}
-
 static void apanel_shutdown(struct i2c_client *client)
 {
-	apanel_detach_client(client);
+	apanel_remove(client);
 }
 
+static struct i2c_device_id apanel_id[] = {
+	{ "fujitsu_apanel", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, apanel_id);
+
 static struct i2c_driver apanel_driver = {
 	.driver = {
 		.name = APANEL,
 	},
-	.attach_adapter = &apanel_attach_adapter,
-	.detach_client  = &apanel_detach_client,
+	.probe		= &apanel_probe,
+	.remove		= &apanel_remove,
 	.shutdown	= &apanel_shutdown,
+	.id_table	= apanel_id,
 };
 
 static struct apanel apanel = {
-	.client = {
-		.driver = &apanel_driver,
-		.name   = APANEL,
-	},
 	.keymap = {
 		[0] = KEY_MAIL,
 		[1] = KEY_WWW,
@@ -204,7 +185,8 @@ static struct apanel apanel = {
 };
 
 /* NB: Only one panel on the i2c. */
-static int apanel_probe(struct i2c_adapter *bus, int address, int kind)
+static int apanel_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
 {
 	struct apanel *ap;
 	struct input_polled_dev *ipdev;
@@ -212,9 +194,6 @@ static int apanel_probe(struct i2c_adapter *bus, int address, int kind)
 	u8 cmd = device_chip[APANEL_DEV_APPBTN] == CHIP_OZ992C ? 0 : 8;
 	int i, err = -ENOMEM;
 
-	dev_dbg(&bus->dev, APANEL ": probe adapter %p addr %d kind %d\n",
-		bus, address, kind);
-
 	ap = &apanel;
 
 	ipdev = input_allocate_polled_device();
@@ -222,18 +201,13 @@ static int apanel_probe(struct i2c_adapter *bus, int address, int kind)
 		goto out1;
 
 	ap->ipdev = ipdev;
-	ap->client.adapter = bus;
-	ap->client.addr = address;
-
-	i2c_set_clientdata(&ap->client, ap);
+	ap->client = client;
 
-	err = i2c_attach_client(&ap->client);
-	if (err)
-		goto out2;
+	i2c_set_clientdata(client, ap);
 
-	err = i2c_smbus_write_word_data(&ap->client, cmd, 0);
+	err = i2c_smbus_write_word_data(client, cmd, 0);
 	if (err) {
-		dev_warn(&ap->client.dev, APANEL ": smbus write error %d\n",
+		dev_warn(&client->dev, APANEL ": smbus write error %d\n",
 			 err);
 		goto out3;
 	}
@@ -246,7 +220,7 @@ static int apanel_probe(struct i2c_adapter *bus, int address, int kind)
 	idev->name = APANEL_NAME " buttons";
 	idev->phys = "apanel/input0";
 	idev->id.bustype = BUS_HOST;
-	idev->dev.parent = &ap->client.dev;
+	idev->dev.parent = &client->dev;
 
 	set_bit(EV_KEY, idev->evbit);
 
@@ -264,7 +238,7 @@ static int apanel_probe(struct i2c_adapter *bus, int address, int kind)
 
 	INIT_WORK(&ap->led_work, led_update);
 	if (device_chip[APANEL_DEV_LED] != CHIP_NONE) {
-		err = led_classdev_register(&ap->client.dev, &ap->mail_led);
+		err = led_classdev_register(&client->dev, &ap->mail_led);
 		if (err)
 			goto out4;
 	}
@@ -273,8 +247,6 @@ static int apanel_probe(struct i2c_adapter *bus, int address, int kind)
 out4:
 	input_unregister_polled_device(ipdev);
 out3:
-	i2c_detach_client(&ap->client);
-out2:
 	input_free_polled_device(ipdev);
 out1:
 	return err;
@@ -301,6 +273,7 @@ static int __init apanel_init(void)
 	void __iomem *bios;
 	const void __iomem *p;
 	u8 devno;
+	unsigned char i2c_addr;
 	int found = 0;
 
 	bios = ioremap(0xF0000, 0x10000); /* Can't fail */
@@ -313,7 +286,7 @@ static int __init apanel_init(void)
 
 	/* just use the first address */
 	p += 8;
-	normal_i2c[0] = readb(p+3) >> 1;
+	i2c_addr = readb(p + 3) >> 1;
 
 	for ( ; (devno = readb(p)) & 0x7f; p += 4) {
 		unsigned char method, slave, chip;
@@ -322,7 +295,7 @@ static int __init apanel_init(void)
 		chip = readb(p + 2);
 		slave = readb(p + 3) >> 1;
 
-		if (slave != normal_i2c[0]) {
+		if (slave != i2c_addr) {
 			pr_notice(APANEL ": only one SMBus slave "
 				  "address supported, skiping device...\n");
 			continue;
diff --git a/drivers/input/misc/pcspkr.c b/drivers/input/misc/pcspkr.c
index 43aaa5cebd12..d6a30cee7bc7 100644
--- a/drivers/input/misc/pcspkr.c
+++ b/drivers/input/misc/pcspkr.c
@@ -52,13 +52,13 @@ static int pcspkr_event(struct input_dev *dev, unsigned int type, unsigned int c
 	spin_lock_irqsave(&i8253_lock, flags);
 
 	if (count) {
-		/* enable counter 2 */
-		outb_p(inb_p(0x61) | 3, 0x61);
 		/* set command for counter 2, 2 byte write */
 		outb_p(0xB6, 0x43);
 		/* select desired HZ */
 		outb_p(count & 0xff, 0x42);
 		outb((count >> 8) & 0xff, 0x42);
+		/* enable counter 2 */
+		outb_p(inb_p(0x61) | 3, 0x61);
 	} else {
 		/* disable counter 2 */
 		outb(inb_p(0x61) & 0xFC, 0x61);
diff --git a/drivers/input/misc/uinput.c b/drivers/input/misc/uinput.c
index 223d56d5555b..46b7caeb2817 100644
--- a/drivers/input/misc/uinput.c
+++ b/drivers/input/misc/uinput.c
@@ -37,6 +37,7 @@
 #include <linux/fs.h>
 #include <linux/miscdevice.h>
 #include <linux/uinput.h>
+#include "../input-compat.h"
 
 static int uinput_dev_event(struct input_dev *dev, unsigned int type, unsigned int code, int value)
 {
@@ -78,6 +79,7 @@ static struct uinput_request* uinput_request_find(struct uinput_device *udev, in
 	/* Find an input request, by ID. Returns NULL if the ID isn't valid. */
 	if (id >= UINPUT_NUM_REQUESTS || id < 0)
 		return NULL;
+
 	return udev->requests[id];
 }
 
@@ -127,6 +129,17 @@ static int uinput_dev_upload_effect(struct input_dev *dev, struct ff_effect *eff
 	struct uinput_request request;
 	int retval;
 
+	/*
+	 * uinput driver does not currently support periodic effects with
+	 * custom waveform since it does not have a way to pass buffer of
+	 * samples (custom_data) to userspace. If ever there is a device
+	 * supporting custom waveforms we would need to define an additional
+	 * ioctl (UI_UPLOAD_SAMPLES) but for now we just bail out.
+	 */
+	if (effect->type == FF_PERIODIC &&
+			effect->u.periodic.waveform == FF_CUSTOM)
+		return -EINVAL;
+
 	request.id = -1;
 	init_completion(&request.done);
 	request.code = UI_FF_UPLOAD;
@@ -353,15 +366,15 @@ static inline ssize_t uinput_inject_event(struct uinput_device *udev, const char
 {
 	struct input_event ev;
 
-	if (count != sizeof(struct input_event))
+	if (count < input_event_size())
 		return -EINVAL;
 
-	if (copy_from_user(&ev, buffer, sizeof(struct input_event)))
+	if (input_event_from_user(buffer, &ev))
 		return -EFAULT;
 
 	input_event(udev->dev, ev.type, ev.code, ev.value);
 
-	return sizeof(struct input_event);
+	return input_event_size();
 }
 
 static ssize_t uinput_write(struct file *file, const char __user *buffer, size_t count, loff_t *ppos)
@@ -407,13 +420,13 @@ static ssize_t uinput_read(struct file *file, char __user *buffer, size_t count,
 		goto out;
 	}
 
-	while (udev->head != udev->tail && retval + sizeof(struct input_event) <= count) {
-		if (copy_to_user(buffer + retval, &udev->buff[udev->tail], sizeof(struct input_event))) {
+	while (udev->head != udev->tail && retval + input_event_size() <= count) {
+		if (input_event_to_user(buffer + retval, &udev->buff[udev->tail])) {
 			retval = -EFAULT;
 			goto out;
 		}
 		udev->tail = (udev->tail + 1) % UINPUT_BUFFER_SIZE;
-		retval += sizeof(struct input_event);
+		retval += input_event_size();
 	}
 
  out:
@@ -444,6 +457,93 @@ static int uinput_release(struct inode *inode, struct file *file)
 	return 0;
 }
 
+#ifdef CONFIG_COMPAT
+struct uinput_ff_upload_compat {
+	int			request_id;
+	int			retval;
+	struct ff_effect_compat	effect;
+	struct ff_effect_compat	old;
+};
+
+static int uinput_ff_upload_to_user(char __user *buffer,
+				    const struct uinput_ff_upload *ff_up)
+{
+	if (INPUT_COMPAT_TEST) {
+		struct uinput_ff_upload_compat ff_up_compat;
+
+		ff_up_compat.request_id = ff_up->request_id;
+		ff_up_compat.retval = ff_up->retval;
+		/*
+		 * It so happens that the pointer that gives us the trouble
+		 * is the last field in the structure. Since we don't support
+		 * custom waveforms in uinput anyway we can just copy the whole
+		 * thing (to the compat size) and ignore the pointer.
+		 */
+		memcpy(&ff_up_compat.effect, &ff_up->effect,
+			sizeof(struct ff_effect_compat));
+		memcpy(&ff_up_compat.old, &ff_up->old,
+			sizeof(struct ff_effect_compat));
+
+		if (copy_to_user(buffer, &ff_up_compat,
+				 sizeof(struct uinput_ff_upload_compat)))
+			return -EFAULT;
+	} else {
+		if (copy_to_user(buffer, ff_up,
+				 sizeof(struct uinput_ff_upload)))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int uinput_ff_upload_from_user(const char __user *buffer,
+				      struct uinput_ff_upload *ff_up)
+{
+	if (INPUT_COMPAT_TEST) {
+		struct uinput_ff_upload_compat ff_up_compat;
+
+		if (copy_from_user(&ff_up_compat, buffer,
+				   sizeof(struct uinput_ff_upload_compat)))
+			return -EFAULT;
+
+		ff_up->request_id = ff_up_compat.request_id;
+		ff_up->retval = ff_up_compat.retval;
+		memcpy(&ff_up->effect, &ff_up_compat.effect,
+			sizeof(struct ff_effect_compat));
+		memcpy(&ff_up->old, &ff_up_compat.old,
+			sizeof(struct ff_effect_compat));
+
+	} else {
+		if (copy_from_user(ff_up, buffer,
+				   sizeof(struct uinput_ff_upload)))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+#else
+
+static int uinput_ff_upload_to_user(char __user *buffer,
+				    const struct uinput_ff_upload *ff_up)
+{
+	if (copy_to_user(buffer, ff_up, sizeof(struct uinput_ff_upload)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static int uinput_ff_upload_from_user(const char __user *buffer,
+				      struct uinput_ff_upload *ff_up)
+{
+	if (copy_from_user(ff_up, buffer, sizeof(struct uinput_ff_upload)))
+		return -EFAULT;
+
+	return 0;
+}
+
+#endif
+
 #define uinput_set_bit(_arg, _bit, _max)		\
 ({							\
 	int __ret = 0;					\
@@ -455,19 +555,17 @@ static int uinput_release(struct inode *inode, struct file *file)
 	__ret;						\
 })
 
-static long uinput_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+static long uinput_ioctl_handler(struct file *file, unsigned int cmd,
+				 unsigned long arg, void __user *p)
 {
 	int			retval;
-	struct uinput_device	*udev;
-	void __user             *p = (void __user *)arg;
+	struct uinput_device	*udev = file->private_data;
 	struct uinput_ff_upload ff_up;
 	struct uinput_ff_erase  ff_erase;
 	struct uinput_request   *req;
 	int                     length;
 	char			*phys;
 
-	udev = file->private_data;
-
 	retval = mutex_lock_interruptible(&udev->mutex);
 	if (retval)
 		return retval;
@@ -549,26 +647,24 @@ static long uinput_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 			break;
 
 		case UI_BEGIN_FF_UPLOAD:
-			if (copy_from_user(&ff_up, p, sizeof(ff_up))) {
-				retval = -EFAULT;
+			retval = uinput_ff_upload_from_user(p, &ff_up);
+			if (retval)
 				break;
-			}
+
 			req = uinput_request_find(udev, ff_up.request_id);
-			if (!(req && req->code == UI_FF_UPLOAD && req->u.upload.effect)) {
+			if (!req || req->code != UI_FF_UPLOAD || !req->u.upload.effect) {
 				retval = -EINVAL;
 				break;
 			}
+
 			ff_up.retval = 0;
-			memcpy(&ff_up.effect, req->u.upload.effect, sizeof(struct ff_effect));
+			ff_up.effect = *req->u.upload.effect;
 			if (req->u.upload.old)
-				memcpy(&ff_up.old, req->u.upload.old, sizeof(struct ff_effect));
+				ff_up.old = *req->u.upload.old;
 			else
 				memset(&ff_up.old, 0, sizeof(struct ff_effect));
 
-			if (copy_to_user(p, &ff_up, sizeof(ff_up))) {
-				retval = -EFAULT;
-				break;
-			}
+			retval = uinput_ff_upload_to_user(p, &ff_up);
 			break;
 
 		case UI_BEGIN_FF_ERASE:
@@ -576,29 +672,34 @@ static long uinput_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 				retval = -EFAULT;
 				break;
 			}
+
 			req = uinput_request_find(udev, ff_erase.request_id);
-			if (!(req && req->code == UI_FF_ERASE)) {
+			if (!req || req->code != UI_FF_ERASE) {
 				retval = -EINVAL;
 				break;
 			}
+
 			ff_erase.retval = 0;
 			ff_erase.effect_id = req->u.effect_id;
 			if (copy_to_user(p, &ff_erase, sizeof(ff_erase))) {
 				retval = -EFAULT;
 				break;
 			}
+
 			break;
 
 		case UI_END_FF_UPLOAD:
-			if (copy_from_user(&ff_up, p, sizeof(ff_up))) {
-				retval = -EFAULT;
+			retval = uinput_ff_upload_from_user(p, &ff_up);
+			if (retval)
 				break;
-			}
+
 			req = uinput_request_find(udev, ff_up.request_id);
-			if (!(req && req->code == UI_FF_UPLOAD && req->u.upload.effect)) {
+			if (!req || req->code != UI_FF_UPLOAD ||
+			    !req->u.upload.effect) {
 				retval = -EINVAL;
 				break;
 			}
+
 			req->retval = ff_up.retval;
 			uinput_request_done(udev, req);
 			break;
@@ -608,11 +709,13 @@ static long uinput_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 				retval = -EFAULT;
 				break;
 			}
+
 			req = uinput_request_find(udev, ff_erase.request_id);
-			if (!(req && req->code == UI_FF_ERASE)) {
+			if (!req || req->code != UI_FF_ERASE) {
 				retval = -EINVAL;
 				break;
 			}
+
 			req->retval = ff_erase.retval;
 			uinput_request_done(udev, req);
 			break;
@@ -626,6 +729,18 @@ static long uinput_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	return retval;
 }
 
+static long uinput_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	return uinput_ioctl_handler(file, cmd, arg, (void __user *)arg);
+}
+
+#ifdef CONFIG_COMPAT
+static long uinput_compat_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	return uinput_ioctl_handler(file, cmd, arg, compat_ptr(arg));
+}
+#endif
+
 static const struct file_operations uinput_fops = {
 	.owner		= THIS_MODULE,
 	.open		= uinput_open,
@@ -634,6 +749,9 @@ static const struct file_operations uinput_fops = {
 	.write		= uinput_write,
 	.poll		= uinput_poll,
 	.unlocked_ioctl	= uinput_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl	= uinput_compat_ioctl,
+#endif
 };
 
 static struct miscdevice uinput_misc = {
diff --git a/drivers/input/mouse/Kconfig b/drivers/input/mouse/Kconfig
index 4e9934259775..093c8c1bca74 100644
--- a/drivers/input/mouse/Kconfig
+++ b/drivers/input/mouse/Kconfig
@@ -286,4 +286,10 @@ config MOUSE_GPIO
 	  To compile this driver as a module, choose M here: the
 	  module will be called gpio_mouse.
 
+config MOUSE_PXA930_TRKBALL
+	tristate "PXA930 Trackball mouse"
+	depends on CPU_PXA930 || CPU_PXA935
+	help
+	  Say Y here to support PXA930 Trackball mouse.
+
 endif
diff --git a/drivers/input/mouse/Makefile b/drivers/input/mouse/Makefile
index 96f1dd8037f8..8c8a1f236e28 100644
--- a/drivers/input/mouse/Makefile
+++ b/drivers/input/mouse/Makefile
@@ -4,19 +4,20 @@
 
 # Each configuration option enables a list of files.
 
-obj-$(CONFIG_MOUSE_AMIGA)	+= amimouse.o
-obj-$(CONFIG_MOUSE_APPLETOUCH)	+= appletouch.o
-obj-$(CONFIG_MOUSE_BCM5974)	+= bcm5974.o
-obj-$(CONFIG_MOUSE_ATARI)	+= atarimouse.o
-obj-$(CONFIG_MOUSE_RISCPC)	+= rpcmouse.o
-obj-$(CONFIG_MOUSE_INPORT)	+= inport.o
-obj-$(CONFIG_MOUSE_LOGIBM)	+= logibm.o
-obj-$(CONFIG_MOUSE_PC110PAD)	+= pc110pad.o
-obj-$(CONFIG_MOUSE_PS2)		+= psmouse.o
-obj-$(CONFIG_MOUSE_SERIAL)	+= sermouse.o
-obj-$(CONFIG_MOUSE_HIL)		+= hil_ptr.o
-obj-$(CONFIG_MOUSE_VSXXXAA)	+= vsxxxaa.o
-obj-$(CONFIG_MOUSE_GPIO)	+= gpio_mouse.o
+obj-$(CONFIG_MOUSE_AMIGA)		+= amimouse.o
+obj-$(CONFIG_MOUSE_APPLETOUCH)		+= appletouch.o
+obj-$(CONFIG_MOUSE_BCM5974)		+= bcm5974.o
+obj-$(CONFIG_MOUSE_ATARI)		+= atarimouse.o
+obj-$(CONFIG_MOUSE_RISCPC)		+= rpcmouse.o
+obj-$(CONFIG_MOUSE_INPORT)		+= inport.o
+obj-$(CONFIG_MOUSE_LOGIBM)		+= logibm.o
+obj-$(CONFIG_MOUSE_PC110PAD)		+= pc110pad.o
+obj-$(CONFIG_MOUSE_PS2)			+= psmouse.o
+obj-$(CONFIG_MOUSE_PXA930_TRKBALL)	+= pxa930_trkball.o
+obj-$(CONFIG_MOUSE_SERIAL)		+= sermouse.o
+obj-$(CONFIG_MOUSE_HIL)			+= hil_ptr.o
+obj-$(CONFIG_MOUSE_VSXXXAA)		+= vsxxxaa.o
+obj-$(CONFIG_MOUSE_GPIO)		+= gpio_mouse.o
 
 psmouse-objs := psmouse-base.o synaptics.o
 
diff --git a/drivers/input/mouse/appletouch.c b/drivers/input/mouse/appletouch.c
index 079816e6b23b..454b96112f03 100644
--- a/drivers/input/mouse/appletouch.c
+++ b/drivers/input/mouse/appletouch.c
@@ -3,7 +3,7 @@
  *
  * Copyright (C) 2001-2004 Greg Kroah-Hartman (greg@kroah.com)
  * Copyright (C) 2005-2008 Johannes Berg (johannes@sipsolutions.net)
- * Copyright (C) 2005      Stelian Pop (stelian@popies.net)
+ * Copyright (C) 2005-2008 Stelian Pop (stelian@popies.net)
  * Copyright (C) 2005      Frank Arnold (frank@scirocco-5v-turbo.de)
  * Copyright (C) 2005      Peter Osterlund (petero2@telia.com)
  * Copyright (C) 2005      Michael Hanselmann (linux-kernel@hansmi.ch)
@@ -35,16 +35,74 @@
 #include <linux/module.h>
 #include <linux/usb/input.h>
 
-/* Type of touchpad */
-enum atp_touchpad_type {
-	ATP_FOUNTAIN,
-	ATP_GEYSER1,
-	ATP_GEYSER2,
-	ATP_GEYSER3,
-	ATP_GEYSER4
+/*
+ * Note: We try to keep the touchpad aspect ratio while still doing only
+ * simple arithmetics:
+ *	0 <= x <= (xsensors - 1) * xfact
+ *	0 <= y <= (ysensors - 1) * yfact
+ */
+struct atp_info {
+	int xsensors;				/* number of X sensors */
+	int xsensors_17;			/* 17" models have more sensors */
+	int ysensors;				/* number of Y sensors */
+	int xfact;				/* X multiplication factor */
+	int yfact;				/* Y multiplication factor */
+	int datalen;				/* size of USB transfers */
+	void (*callback)(struct urb *);		/* callback function */
+};
+
+static void atp_complete_geyser_1_2(struct urb *urb);
+static void atp_complete_geyser_3_4(struct urb *urb);
+
+static const struct atp_info fountain_info = {
+	.xsensors	= 16,
+	.xsensors_17	= 26,
+	.ysensors	= 16,
+	.xfact		= 64,
+	.yfact		= 43,
+	.datalen	= 81,
+	.callback	= atp_complete_geyser_1_2,
+};
+
+static const struct atp_info geyser1_info = {
+	.xsensors	= 16,
+	.xsensors_17	= 26,
+	.ysensors	= 16,
+	.xfact		= 64,
+	.yfact		= 43,
+	.datalen	= 81,
+	.callback	= atp_complete_geyser_1_2,
+};
+
+static const struct atp_info geyser2_info = {
+	.xsensors	= 15,
+	.xsensors_17	= 20,
+	.ysensors	= 9,
+	.xfact		= 64,
+	.yfact		= 43,
+	.datalen	= 64,
+	.callback	= atp_complete_geyser_1_2,
+};
+
+static const struct atp_info geyser3_info = {
+	.xsensors	= 20,
+	.ysensors	= 10,
+	.xfact		= 64,
+	.yfact		= 64,
+	.datalen	= 64,
+	.callback	= atp_complete_geyser_3_4,
 };
 
-#define ATP_DEVICE(prod, type)					\
+static const struct atp_info geyser4_info = {
+	.xsensors	= 20,
+	.ysensors	= 10,
+	.xfact		= 64,
+	.yfact		= 64,
+	.datalen	= 64,
+	.callback	= atp_complete_geyser_3_4,
+};
+
+#define ATP_DEVICE(prod, info)					\
 {								\
 	.match_flags = USB_DEVICE_ID_MATCH_DEVICE |		\
 		       USB_DEVICE_ID_MATCH_INT_CLASS |		\
@@ -53,7 +111,7 @@ enum atp_touchpad_type {
 	.idProduct = (prod),					\
 	.bInterfaceClass = 0x03,				\
 	.bInterfaceProtocol = 0x02,				\
-	.driver_info = ATP_ ## type,				\
+	.driver_info = (unsigned long) &info,			\
 }
 
 /*
@@ -62,43 +120,39 @@ enum atp_touchpad_type {
  *  According to Info.plist Geyser IV is the same as Geyser III.)
  */
 
-static struct usb_device_id atp_table [] = {
+static struct usb_device_id atp_table[] = {
 	/* PowerBooks Feb 2005, iBooks G4 */
-	ATP_DEVICE(0x020e, FOUNTAIN),	/* FOUNTAIN ANSI */
-	ATP_DEVICE(0x020f, FOUNTAIN),	/* FOUNTAIN ISO */
-	ATP_DEVICE(0x030a, FOUNTAIN),	/* FOUNTAIN TP ONLY */
-	ATP_DEVICE(0x030b, GEYSER1),	/* GEYSER 1 TP ONLY */
+	ATP_DEVICE(0x020e, fountain_info),	/* FOUNTAIN ANSI */
+	ATP_DEVICE(0x020f, fountain_info),	/* FOUNTAIN ISO */
+	ATP_DEVICE(0x030a, fountain_info),	/* FOUNTAIN TP ONLY */
+	ATP_DEVICE(0x030b, geyser1_info),	/* GEYSER 1 TP ONLY */
 
 	/* PowerBooks Oct 2005 */
-	ATP_DEVICE(0x0214, GEYSER2),	/* GEYSER 2 ANSI */
-	ATP_DEVICE(0x0215, GEYSER2),	/* GEYSER 2 ISO */
-	ATP_DEVICE(0x0216, GEYSER2),	/* GEYSER 2 JIS */
+	ATP_DEVICE(0x0214, geyser2_info),	/* GEYSER 2 ANSI */
+	ATP_DEVICE(0x0215, geyser2_info),	/* GEYSER 2 ISO */
+	ATP_DEVICE(0x0216, geyser2_info),	/* GEYSER 2 JIS */
 
 	/* Core Duo MacBook & MacBook Pro */
-	ATP_DEVICE(0x0217, GEYSER3),	/* GEYSER 3 ANSI */
-	ATP_DEVICE(0x0218, GEYSER3),	/* GEYSER 3 ISO */
-	ATP_DEVICE(0x0219, GEYSER3),	/* GEYSER 3 JIS */
+	ATP_DEVICE(0x0217, geyser3_info),	/* GEYSER 3 ANSI */
+	ATP_DEVICE(0x0218, geyser3_info),	/* GEYSER 3 ISO */
+	ATP_DEVICE(0x0219, geyser3_info),	/* GEYSER 3 JIS */
 
 	/* Core2 Duo MacBook & MacBook Pro */
-	ATP_DEVICE(0x021a, GEYSER4),	/* GEYSER 4 ANSI */
-	ATP_DEVICE(0x021b, GEYSER4),	/* GEYSER 4 ISO */
-	ATP_DEVICE(0x021c, GEYSER4),	/* GEYSER 4 JIS */
+	ATP_DEVICE(0x021a, geyser4_info),	/* GEYSER 4 ANSI */
+	ATP_DEVICE(0x021b, geyser4_info),	/* GEYSER 4 ISO */
+	ATP_DEVICE(0x021c, geyser4_info),	/* GEYSER 4 JIS */
 
 	/* Core2 Duo MacBook3,1 */
-	ATP_DEVICE(0x0229, GEYSER4),	/* GEYSER 4 HF ANSI */
-	ATP_DEVICE(0x022a, GEYSER4),	/* GEYSER 4 HF ISO */
-	ATP_DEVICE(0x022b, GEYSER4),	/* GEYSER 4 HF JIS */
+	ATP_DEVICE(0x0229, geyser4_info),	/* GEYSER 4 HF ANSI */
+	ATP_DEVICE(0x022a, geyser4_info),	/* GEYSER 4 HF ISO */
+	ATP_DEVICE(0x022b, geyser4_info),	/* GEYSER 4 HF JIS */
 
 	/* Terminating entry */
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, atp_table);
 
-/*
- * number of sensors. Note that only 16 instead of 26 X (horizontal)
- * sensors exist on 12" and 15" PowerBooks. All models have 16 Y
- * (vertical) sensors.
- */
+/* maximum number of sensors */
 #define ATP_XSENSORS	26
 #define ATP_YSENSORS	16
 
@@ -107,21 +161,6 @@ MODULE_DEVICE_TABLE(usb, atp_table);
 
 /* maximum pressure this driver will report */
 #define ATP_PRESSURE	300
-/*
- * multiplication factor for the X and Y coordinates.
- * We try to keep the touchpad aspect ratio while still doing only simple
- * arithmetics.
- * The factors below give coordinates like:
- *
- *      0 <= x <  960 on 12" and 15" Powerbooks
- *      0 <= x < 1600 on 17" Powerbooks and 17" MacBook Pro
- *      0 <= x < 1216 on MacBooks and 15" MacBook Pro
- *
- *      0 <= y <  646 on all Powerbooks
- *      0 <= y <  774 on all MacBooks
- */
-#define ATP_XFACT	64
-#define ATP_YFACT	43
 
 /*
  * Threshold for the touchpad sensors. Any change less than ATP_THRESHOLD is
@@ -159,7 +198,7 @@ struct atp {
 	struct urb		*urb;		/* usb request block */
 	u8			*data;		/* transferred data */
 	struct input_dev	*input;		/* input dev */
-	enum atp_touchpad_type	type;		/* type of touchpad */
+	const struct atp_info	*info;		/* touchpad model */
 	bool			open;
 	bool			valid;		/* are the samples valid? */
 	bool			size_detect_done;
@@ -169,7 +208,6 @@ struct atp {
 	signed char		xy_cur[ATP_XSENSORS + ATP_YSENSORS];
 	signed char		xy_old[ATP_XSENSORS + ATP_YSENSORS];
 	int			xy_acc[ATP_XSENSORS + ATP_YSENSORS];
-	int			datalen;	/* size of USB transfer */
 	int			idlecount;	/* number of empty packets */
 	struct work_struct	work;
 };
@@ -359,7 +397,7 @@ static int atp_status_check(struct urb *urb)
 		if (!dev->overflow_warned) {
 			printk(KERN_WARNING "appletouch: OVERFLOW with data "
 				"length %d, actual length is %d\n",
-				dev->datalen, dev->urb->actual_length);
+				dev->info->datalen, dev->urb->actual_length);
 			dev->overflow_warned = true;
 		}
 	case -ECONNRESET:
@@ -377,7 +415,7 @@ static int atp_status_check(struct urb *urb)
 	}
 
 	/* drop incomplete datasets */
-	if (dev->urb->actual_length != dev->datalen) {
+	if (dev->urb->actual_length != dev->info->datalen) {
 		dprintk("appletouch: incomplete data package"
 			" (first byte: %d, length: %d).\n",
 			dev->data[0], dev->urb->actual_length);
@@ -387,6 +425,25 @@ static int atp_status_check(struct urb *urb)
 	return ATP_URB_STATUS_SUCCESS;
 }
 
+static void atp_detect_size(struct atp *dev)
+{
+	int i;
+
+	/* 17" Powerbooks have extra X sensors */
+	for (i = dev->info->xsensors; i < ATP_XSENSORS; i++) {
+		if (dev->xy_cur[i]) {
+
+			printk(KERN_INFO "appletouch: 17\" model detected.\n");
+
+			input_set_abs_params(dev->input, ABS_X, 0,
+					     (dev->info->xsensors_17 - 1) *
+							dev->info->xfact - 1,
+					     ATP_FUZZ, 0);
+			break;
+		}
+	}
+}
+
 /*
  * USB interrupt callback functions
  */
@@ -407,7 +464,7 @@ static void atp_complete_geyser_1_2(struct urb *urb)
 		goto exit;
 
 	/* reorder the sensors values */
-	if (dev->type == ATP_GEYSER2) {
+	if (dev->info == &geyser2_info) {
 		memset(dev->xy_cur, 0, sizeof(dev->xy_cur));
 
 		/*
@@ -437,8 +494,8 @@ static void atp_complete_geyser_1_2(struct urb *urb)
 				dev->xy_cur[i + 24] = dev->data[5 * i + 44];
 
 			/* Y values */
-			dev->xy_cur[i + 26] = dev->data[5 * i +  1];
-			dev->xy_cur[i + 34] = dev->data[5 * i +  3];
+			dev->xy_cur[ATP_XSENSORS + i] = dev->data[5 * i +  1];
+			dev->xy_cur[ATP_XSENSORS + i + 8] = dev->data[5 * i + 3];
 		}
 	}
 
@@ -453,32 +510,8 @@ static void atp_complete_geyser_1_2(struct urb *urb)
 		memcpy(dev->xy_old, dev->xy_cur, sizeof(dev->xy_old));
 
 		/* Perform size detection, if not done already */
-		if (!dev->size_detect_done) {
-
-			/* 17" Powerbooks have extra X sensors */
-			for (i = (dev->type == ATP_GEYSER2 ? 15 : 16);
-			     i < ATP_XSENSORS; i++) {
-				if (!dev->xy_cur[i])
-					continue;
-
-				printk(KERN_INFO
-					"appletouch: 17\" model detected.\n");
-
-				if (dev->type == ATP_GEYSER2)
-					input_set_abs_params(dev->input, ABS_X,
-							     0,
-							     (20 - 1) *
-							     ATP_XFACT - 1,
-							     ATP_FUZZ, 0);
-				else
-					input_set_abs_params(dev->input, ABS_X,
-							     0,
-							     (26 - 1) *
-							     ATP_XFACT - 1,
-							     ATP_FUZZ, 0);
-				break;
-			}
-
+		if (unlikely(!dev->size_detect_done)) {
+			atp_detect_size(dev);
 			dev->size_detect_done = 1;
 			goto exit;
 		}
@@ -499,10 +532,10 @@ static void atp_complete_geyser_1_2(struct urb *urb)
 	dbg_dump("accumulator", dev->xy_acc);
 
 	x = atp_calculate_abs(dev->xy_acc, ATP_XSENSORS,
-			      ATP_XFACT, &x_z, &x_f);
+			      dev->info->xfact, &x_z, &x_f);
 	y = atp_calculate_abs(dev->xy_acc + ATP_XSENSORS, ATP_YSENSORS,
-			      ATP_YFACT, &y_z, &y_f);
-	key = dev->data[dev->datalen - 1] & ATP_STATUS_BUTTON;
+			      dev->info->yfact, &y_z, &y_f);
+	key = dev->data[dev->info->datalen - 1] & ATP_STATUS_BUTTON;
 
 	if (x && y) {
 		if (dev->x_old != -1) {
@@ -583,7 +616,7 @@ static void atp_complete_geyser_3_4(struct urb *urb)
 	dbg_dump("sample", dev->xy_cur);
 
 	/* Just update the base values (i.e. touchpad in untouched state) */
-	if (dev->data[dev->datalen - 1] & ATP_STATUS_BASE_UPDATE) {
+	if (dev->data[dev->info->datalen - 1] & ATP_STATUS_BASE_UPDATE) {
 
 		dprintk(KERN_DEBUG "appletouch: updated base values\n");
 
@@ -610,10 +643,10 @@ static void atp_complete_geyser_3_4(struct urb *urb)
 	dbg_dump("accumulator", dev->xy_acc);
 
 	x = atp_calculate_abs(dev->xy_acc, ATP_XSENSORS,
-			      ATP_XFACT, &x_z, &x_f);
+			      dev->info->xfact, &x_z, &x_f);
 	y = atp_calculate_abs(dev->xy_acc + ATP_XSENSORS, ATP_YSENSORS,
-			      ATP_YFACT, &y_z, &y_f);
-	key = dev->data[dev->datalen - 1] & ATP_STATUS_BUTTON;
+			      dev->info->yfact, &y_z, &y_f);
+	key = dev->data[dev->info->datalen - 1] & ATP_STATUS_BUTTON;
 
 	if (x && y) {
 		if (dev->x_old != -1) {
@@ -705,7 +738,7 @@ static int atp_handle_geyser(struct atp *dev)
 {
 	struct usb_device *udev = dev->udev;
 
-	if (dev->type != ATP_FOUNTAIN) {
+	if (dev->info != &fountain_info) {
 		/* switch to raw sensor mode */
 		if (atp_geyser_init(udev))
 			return -EIO;
@@ -726,6 +759,7 @@ static int atp_probe(struct usb_interface *iface,
 	struct usb_endpoint_descriptor *endpoint;
 	int int_in_endpointAddr = 0;
 	int i, error = -ENOMEM;
+	const struct atp_info *info = (const struct atp_info *)id->driver_info;
 
 	/* set up the endpoint information */
 	/* use only the first interrupt-in endpoint */
@@ -753,35 +787,22 @@ static int atp_probe(struct usb_interface *iface,
 
 	dev->udev = udev;
 	dev->input = input_dev;
-	dev->type = id->driver_info;
+	dev->info = info;
 	dev->overflow_warned = false;
-	if (dev->type == ATP_FOUNTAIN || dev->type == ATP_GEYSER1)
-		dev->datalen = 81;
-	else
-		dev->datalen = 64;
 
 	dev->urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!dev->urb)
 		goto err_free_devs;
 
-	dev->data = usb_buffer_alloc(dev->udev, dev->datalen, GFP_KERNEL,
+	dev->data = usb_buffer_alloc(dev->udev, dev->info->datalen, GFP_KERNEL,
 				     &dev->urb->transfer_dma);
 	if (!dev->data)
 		goto err_free_urb;
 
-	/* Select the USB complete (callback) function */
-	if (dev->type == ATP_FOUNTAIN ||
-	    dev->type == ATP_GEYSER1 ||
-	    dev->type == ATP_GEYSER2)
-		usb_fill_int_urb(dev->urb, udev,
-				 usb_rcvintpipe(udev, int_in_endpointAddr),
-				 dev->data, dev->datalen,
-				 atp_complete_geyser_1_2, dev, 1);
-	else
-		usb_fill_int_urb(dev->urb, udev,
-				 usb_rcvintpipe(udev, int_in_endpointAddr),
-				 dev->data, dev->datalen,
-				 atp_complete_geyser_3_4, dev, 1);
+	usb_fill_int_urb(dev->urb, udev,
+			 usb_rcvintpipe(udev, int_in_endpointAddr),
+			 dev->data, dev->info->datalen,
+			 dev->info->callback, dev, 1);
 
 	error = atp_handle_geyser(dev);
 	if (error)
@@ -802,35 +823,12 @@ static int atp_probe(struct usb_interface *iface,
 
 	set_bit(EV_ABS, input_dev->evbit);
 
-	if (dev->type == ATP_GEYSER3 || dev->type == ATP_GEYSER4) {
-		/*
-		 * MacBook have 20 X sensors, 10 Y sensors
-		 */
-		input_set_abs_params(input_dev, ABS_X, 0,
-				     ((20 - 1) * ATP_XFACT) - 1, ATP_FUZZ, 0);
-		input_set_abs_params(input_dev, ABS_Y, 0,
-				     ((10 - 1) * ATP_YFACT) - 1, ATP_FUZZ, 0);
-	} else if (dev->type == ATP_GEYSER2) {
-		/*
-		 * Oct 2005 15" PowerBooks have 15 X sensors, 17" are detected
-		 * later.
-		 */
-		input_set_abs_params(input_dev, ABS_X, 0,
-				     ((15 - 1) * ATP_XFACT) - 1, ATP_FUZZ, 0);
-		input_set_abs_params(input_dev, ABS_Y, 0,
-				     ((9 - 1) * ATP_YFACT) - 1, ATP_FUZZ, 0);
-	} else {
-		/*
-		 * 12" and 15" Powerbooks only have 16 x sensors,
-		 * 17" models are detected later.
-		 */
-		input_set_abs_params(input_dev, ABS_X, 0,
-				     (16 - 1) * ATP_XFACT - 1,
-				     ATP_FUZZ, 0);
-		input_set_abs_params(input_dev, ABS_Y, 0,
-				     (ATP_YSENSORS - 1) * ATP_YFACT - 1,
-				     ATP_FUZZ, 0);
-	}
+	input_set_abs_params(input_dev, ABS_X, 0,
+			     (dev->info->xsensors - 1) * dev->info->xfact - 1,
+			     ATP_FUZZ, 0);
+	input_set_abs_params(input_dev, ABS_Y, 0,
+			     (dev->info->ysensors - 1) * dev->info->yfact - 1,
+			     ATP_FUZZ, 0);
 	input_set_abs_params(input_dev, ABS_PRESSURE, 0, ATP_PRESSURE, 0, 0);
 
 	set_bit(EV_KEY, input_dev->evbit);
@@ -852,7 +850,7 @@ static int atp_probe(struct usb_interface *iface,
 	return 0;
 
  err_free_buffer:
-	usb_buffer_free(dev->udev, dev->datalen,
+	usb_buffer_free(dev->udev, dev->info->datalen,
 			dev->data, dev->urb->transfer_dma);
  err_free_urb:
 	usb_free_urb(dev->urb);
@@ -871,7 +869,7 @@ static void atp_disconnect(struct usb_interface *iface)
 	if (dev) {
 		usb_kill_urb(dev->urb);
 		input_unregister_device(dev->input);
-		usb_buffer_free(dev->udev, dev->datalen,
+		usb_buffer_free(dev->udev, dev->info->datalen,
 				dev->data, dev->urb->transfer_dma);
 		usb_free_urb(dev->urb);
 		kfree(dev);
diff --git a/drivers/input/mouse/gpio_mouse.c b/drivers/input/mouse/gpio_mouse.c
index 72cf5e33790e..0db8d16c5edd 100644
--- a/drivers/input/mouse/gpio_mouse.c
+++ b/drivers/input/mouse/gpio_mouse.c
@@ -173,7 +173,7 @@ static int __devexit gpio_mouse_remove(struct platform_device *pdev)
 /* work with hotplug and coldplug */
 MODULE_ALIAS("platform:gpio_mouse");
 
-struct platform_driver gpio_mouse_device_driver = {
+static struct platform_driver gpio_mouse_device_driver = {
 	.remove		= __devexit_p(gpio_mouse_remove),
 	.driver		= {
 		.name	= "gpio_mouse",
diff --git a/drivers/input/mouse/hgpk.c b/drivers/input/mouse/hgpk.c
index 88f04bf2ad6c..81e6ebf323e9 100644
--- a/drivers/input/mouse/hgpk.c
+++ b/drivers/input/mouse/hgpk.c
@@ -48,6 +48,30 @@ module_param(recalib_delta, int, 0644);
 MODULE_PARM_DESC(recalib_delta,
 	"packets containing a delta this large will cause a recalibration.");
 
+static int jumpy_delay = 1000;
+module_param(jumpy_delay, int, 0644);
+MODULE_PARM_DESC(jumpy_delay,
+	"delay (ms) before recal after jumpiness detected");
+
+static int spew_delay = 1000;
+module_param(spew_delay, int, 0644);
+MODULE_PARM_DESC(spew_delay,
+	"delay (ms) before recal after packet spew detected");
+
+static int recal_guard_time = 2000;
+module_param(recal_guard_time, int, 0644);
+MODULE_PARM_DESC(recal_guard_time,
+	"interval (ms) during which recal will be restarted if packet received");
+
+static int post_interrupt_delay = 1000;
+module_param(post_interrupt_delay, int, 0644);
+MODULE_PARM_DESC(post_interrupt_delay,
+	"delay (ms) before recal after recal interrupt detected");
+
+static int autorecal = 1;
+module_param(autorecal, int, 0644);
+MODULE_PARM_DESC(autorecal, "enable recalibration in the driver");
+
 /*
  * When the touchpad gets ultra-sensitive, one can keep their finger 1/2"
  * above the pad and still have it send packets.  This causes a jump cursor
@@ -66,7 +90,7 @@ static void hgpk_jumpy_hack(struct psmouse *psmouse, int x, int y)
 		/* My car gets forty rods to the hogshead and that's the
 		 * way I likes it! */
 		psmouse_queue_work(psmouse, &priv->recalib_wq,
-				msecs_to_jiffies(1000));
+				msecs_to_jiffies(jumpy_delay));
 	}
 }
 
@@ -103,7 +127,7 @@ static void hgpk_spewing_hack(struct psmouse *psmouse,
 			hgpk_dbg(psmouse, "packet spew detected (%d,%d)\n",
 				 priv->x_tally, priv->y_tally);
 			psmouse_queue_work(psmouse, &priv->recalib_wq,
-					   msecs_to_jiffies(1000));
+					   msecs_to_jiffies(spew_delay));
 		}
 		/* reset every 100 packets */
 		priv->count = 0;
@@ -181,7 +205,7 @@ static psmouse_ret_t hgpk_process_byte(struct psmouse *psmouse)
 				 "packet inside calibration window, "
 				 "queueing another recalibration\n");
 			psmouse_queue_work(psmouse, &priv->recalib_wq,
-					msecs_to_jiffies(1000));
+					msecs_to_jiffies(post_interrupt_delay));
 		}
 		priv->recalib_window = 0;
 	}
@@ -231,7 +255,7 @@ static int hgpk_force_recalibrate(struct psmouse *psmouse)
 	 * If someone's finger *was* on the touchpad, it's probably
 	 * miscalibrated.  So, we should schedule another recalibration
 	 */
-	priv->recalib_window = jiffies +  msecs_to_jiffies(2000);
+	priv->recalib_window = jiffies +  msecs_to_jiffies(recal_guard_time);
 
 	return 0;
 }
diff --git a/drivers/input/mouse/hil_ptr.c b/drivers/input/mouse/hil_ptr.c
index e532c48410ea..3263ce083bf0 100644
--- a/drivers/input/mouse/hil_ptr.c
+++ b/drivers/input/mouse/hil_ptr.c
@@ -46,7 +46,7 @@
 MODULE_AUTHOR("Brian S. Julin <bri@calyx.com>");
 MODULE_DESCRIPTION(HIL_GENERIC_NAME " driver");
 MODULE_LICENSE("Dual BSD/GPL");
-
+MODULE_ALIAS("serio:ty03pr25id0Fex*");
 
 #define TABLET_SIMULATES_MOUSE	/* allow tablet to be used as mouse */
 #undef  TABLET_AUTOADJUST	/* auto-adjust valid tablet ranges */
diff --git a/drivers/input/mouse/pxa930_trkball.c b/drivers/input/mouse/pxa930_trkball.c
new file mode 100644
index 000000000000..a0f45c4fc198
--- /dev/null
+++ b/drivers/input/mouse/pxa930_trkball.c
@@ -0,0 +1,269 @@
+/*
+ * PXA930 track ball mouse driver
+ *
+ * Copyright (C) 2007 Marvell International Ltd.
+ * 2008-02-28: Yong Yao <yaoyong@marvell.com>
+ *             initial version
+ *
+ *  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/input.h>
+#include <linux/version.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+
+#include <mach/hardware.h>
+#include <mach/pxa930_trkball.h>
+
+/* Trackball Controller Register Definitions */
+#define TBCR		(0x000C)
+#define TBCNTR		(0x0010)
+#define TBSBC		(0x0014)
+
+#define TBCR_TBRST	(1 << 1)
+#define TBCR_TBSB	(1 << 10)
+
+#define TBCR_Y_FLT(n)	(((n) & 0xf) << 6)
+#define TBCR_X_FLT(n)	(((n) & 0xf) << 2)
+
+#define TBCNTR_YM(n)	(((n) >> 24) & 0xff)
+#define TBCNTR_YP(n)	(((n) >> 16) & 0xff)
+#define TBCNTR_XM(n)	(((n) >> 8) & 0xff)
+#define TBCNTR_XP(n)	((n) & 0xff)
+
+#define TBSBC_TBSBC	(0x1)
+
+struct pxa930_trkball {
+	struct pxa930_trkball_platform_data *pdata;
+
+	/* Memory Mapped Register */
+	struct resource *mem;
+	void __iomem *mmio_base;
+
+	struct input_dev *input;
+};
+
+static irqreturn_t pxa930_trkball_interrupt(int irq, void *dev_id)
+{
+	struct pxa930_trkball *trkball = dev_id;
+	struct input_dev *input = trkball->input;
+	int tbcntr, x, y;
+
+	/* According to the spec software must read TBCNTR twice:
+	 * if the read value is the same, the reading is valid
+	 */
+	tbcntr = __raw_readl(trkball->mmio_base + TBCNTR);
+
+	if (tbcntr == __raw_readl(trkball->mmio_base + TBCNTR)) {
+		x = (TBCNTR_XP(tbcntr) - TBCNTR_XM(tbcntr)) / 2;
+		y = (TBCNTR_YP(tbcntr) - TBCNTR_YM(tbcntr)) / 2;
+
+		input_report_rel(input, REL_X, x);
+		input_report_rel(input, REL_Y, y);
+		input_sync(input);
+	}
+
+	__raw_writel(TBSBC_TBSBC, trkball->mmio_base + TBSBC);
+	__raw_writel(0, trkball->mmio_base + TBSBC);
+
+	return IRQ_HANDLED;
+}
+
+/* For TBCR, we need to wait for a while to make sure it has been modified. */
+static int write_tbcr(struct pxa930_trkball *trkball, int v)
+{
+	int i = 100;
+
+	__raw_writel(v, trkball->mmio_base + TBCR);
+
+	while (i--) {
+		if (__raw_readl(trkball->mmio_base + TBCR) == v)
+			break;
+		msleep(1);
+	}
+
+	if (i == 0) {
+		pr_err("%s: timed out writing TBCR(%x)!\n", __func__, v);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static void pxa930_trkball_config(struct pxa930_trkball *trkball)
+{
+	uint32_t tbcr;
+
+	/* According to spec, need to write the filters of x,y to 0xf first! */
+	tbcr = __raw_readl(trkball->mmio_base + TBCR);
+	write_tbcr(trkball, tbcr | TBCR_X_FLT(0xf) | TBCR_Y_FLT(0xf));
+	write_tbcr(trkball, TBCR_X_FLT(trkball->pdata->x_filter) |
+			    TBCR_Y_FLT(trkball->pdata->y_filter));
+
+	/* According to spec, set TBCR_TBRST first, before clearing it! */
+	tbcr = __raw_readl(trkball->mmio_base + TBCR);
+	write_tbcr(trkball, tbcr | TBCR_TBRST);
+	write_tbcr(trkball, tbcr & ~TBCR_TBRST);
+
+	__raw_writel(TBSBC_TBSBC, trkball->mmio_base + TBSBC);
+	__raw_writel(0, trkball->mmio_base + TBSBC);
+
+	pr_debug("%s: final TBCR=%x!\n", __func__,
+		 __raw_readl(trkball->mmio_base + TBCR));
+}
+
+static int pxa930_trkball_open(struct input_dev *dev)
+{
+	struct pxa930_trkball *trkball = input_get_drvdata(dev);
+
+	pxa930_trkball_config(trkball);
+
+	return 0;
+}
+
+static void pxa930_trkball_disable(struct pxa930_trkball *trkball)
+{
+	uint32_t tbcr = __raw_readl(trkball->mmio_base + TBCR);
+
+	/* Held in reset, gate the 32-KHz input clock off */
+	write_tbcr(trkball, tbcr | TBCR_TBRST);
+}
+
+static void pxa930_trkball_close(struct input_dev *dev)
+{
+	struct pxa930_trkball *trkball = input_get_drvdata(dev);
+
+	pxa930_trkball_disable(trkball);
+}
+
+static int __devinit pxa930_trkball_probe(struct platform_device *pdev)
+{
+	struct pxa930_trkball *trkball;
+	struct input_dev *input;
+	struct resource *res;
+	int irq, error;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "failed to get trkball irq\n");
+		return -ENXIO;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "failed to get register memory\n");
+		return -ENXIO;
+	}
+
+	trkball = kzalloc(sizeof(struct pxa930_trkball), GFP_KERNEL);
+	if (!trkball)
+		return -ENOMEM;
+
+	trkball->pdata = pdev->dev.platform_data;
+	if (!trkball->pdata) {
+		dev_err(&pdev->dev, "no platform data defined\n");
+		error = -EINVAL;
+		goto failed;
+	}
+
+	trkball->mmio_base = ioremap_nocache(res->start, resource_size(res));
+	if (!trkball->mmio_base) {
+		dev_err(&pdev->dev, "failed to ioremap registers\n");
+		error = -ENXIO;
+		goto failed;
+	}
+
+	/* held the module in reset, will be enabled in open() */
+	pxa930_trkball_disable(trkball);
+
+	error = request_irq(irq, pxa930_trkball_interrupt, IRQF_DISABLED,
+			    pdev->name, trkball);
+	if (error) {
+		dev_err(&pdev->dev, "failed to request irq: %d\n", ret);
+		goto failed_free_io;
+	}
+
+	platform_set_drvdata(pdev, trkball);
+
+	input = input_allocate_device();
+	if (!input) {
+		dev_err(&pdev->dev, "failed to allocate input device\n");
+		error = -ENOMEM;
+		goto failed_free_irq;
+	}
+
+	input->name = pdev->name;
+	input->id.bustype = BUS_HOST;
+	input->open = pxa930_trkball_open;
+	input->close = pxa930_trkball_close;
+	input->dev.parent = &pdev->dev;
+	input_set_drvdata(input, trkball);
+
+	trkball->input = input;
+
+	input_set_capability(input, EV_REL, REL_X);
+	input_set_capability(input, EV_REL, REL_Y);
+
+	error = input_register_device(input);
+	if (error) {
+		dev_err(&pdev->dev, "unable to register input device\n");
+		goto failed_free_input;
+	}
+
+	return 0;
+
+failed_free_input:
+	input_free_device(input);
+failed_free_irq:
+	free_irq(irq, trkball);
+failed_free_io:
+	iounmap(trkball->mmio_base);
+failed:
+	kfree(trkball);
+	return ret;
+}
+
+static int __devexit pxa930_trkball_remove(struct platform_device *pdev)
+{
+	struct pxa930_trkball *trkball = platform_get_drvdata(pdev);
+	int irq = platform_get_irq(pdev, 0);
+
+	input_unregister_device(trkball->input);
+	free_irq(irq, trkball);
+	iounmap(trkball->mmio_base);
+	kfree(trkball);
+
+	return 0;
+}
+
+static struct platform_driver pxa930_trkball_driver = {
+	.driver		= {
+		.name	= "pxa930-trkball",
+	},
+	.probe		= pxa930_trkball_probe,
+	.remove		= __devexit_p(pxa930_trkball_remove),
+};
+
+static int __init pxa930_trkball_init(void)
+{
+	return platform_driver_register(&pxa930_trkball_driver);
+}
+
+static void __exit pxa930_trkball_exit(void)
+{
+	platform_driver_unregister(&pxa930_trkball_driver);
+}
+
+module_init(pxa930_trkball_init);
+module_exit(pxa930_trkball_exit);
+
+MODULE_AUTHOR("Yong Yao <yaoyong@marvell.com>");
+MODULE_DESCRIPTION("PXA930 Trackball Mouse Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/mouse/synaptics.c b/drivers/input/mouse/synaptics.c
index d349c4a5e3e8..865fc69e9bc3 100644
--- a/drivers/input/mouse/synaptics.c
+++ b/drivers/input/mouse/synaptics.c
@@ -445,12 +445,14 @@ static void synaptics_process_packet(struct psmouse *psmouse)
 
 	input_report_abs(dev, ABS_TOOL_WIDTH, finger_width);
 	input_report_key(dev, BTN_TOOL_FINGER, num_fingers == 1);
-	input_report_key(dev, BTN_TOOL_DOUBLETAP, num_fingers == 2);
-	input_report_key(dev, BTN_TOOL_TRIPLETAP, num_fingers == 3);
-
 	input_report_key(dev, BTN_LEFT, hw.left);
 	input_report_key(dev, BTN_RIGHT, hw.right);
 
+	if (SYN_CAP_MULTIFINGER(priv->capabilities)) {
+		input_report_key(dev, BTN_TOOL_DOUBLETAP, num_fingers == 2);
+		input_report_key(dev, BTN_TOOL_TRIPLETAP, num_fingers == 3);
+	}
+
 	if (SYN_CAP_MIDDLE_BUTTON(priv->capabilities))
 		input_report_key(dev, BTN_MIDDLE, hw.middle);
 
@@ -543,12 +545,14 @@ static void set_input_params(struct input_dev *dev, struct synaptics_data *priv)
 	set_bit(EV_KEY, dev->evbit);
 	set_bit(BTN_TOUCH, dev->keybit);
 	set_bit(BTN_TOOL_FINGER, dev->keybit);
-	set_bit(BTN_TOOL_DOUBLETAP, dev->keybit);
-	set_bit(BTN_TOOL_TRIPLETAP, dev->keybit);
-
 	set_bit(BTN_LEFT, dev->keybit);
 	set_bit(BTN_RIGHT, dev->keybit);
 
+	if (SYN_CAP_MULTIFINGER(priv->capabilities)) {
+		set_bit(BTN_TOOL_DOUBLETAP, dev->keybit);
+		set_bit(BTN_TOOL_TRIPLETAP, dev->keybit);
+	}
+
 	if (SYN_CAP_MIDDLE_BUTTON(priv->capabilities))
 		set_bit(BTN_MIDDLE, dev->keybit);
 
diff --git a/drivers/input/mousedev.c b/drivers/input/mousedev.c
index d8c056fe7e98..ef99a7e6d40c 100644
--- a/drivers/input/mousedev.c
+++ b/drivers/input/mousedev.c
@@ -878,8 +878,7 @@ static struct mousedev *mousedev_create(struct input_dev *dev,
 	mousedev->handle.handler = handler;
 	mousedev->handle.private = mousedev;
 
-	strlcpy(mousedev->dev.bus_id, mousedev->name,
-		sizeof(mousedev->dev.bus_id));
+	dev_set_name(&mousedev->dev, mousedev->name);
 	mousedev->dev.class = &input_class;
 	if (dev)
 		mousedev->dev.parent = &dev->dev;
diff --git a/drivers/input/serio/hil_mlc.c b/drivers/input/serio/hil_mlc.c
index 37586a68d345..7ba9f2b2c041 100644
--- a/drivers/input/serio/hil_mlc.c
+++ b/drivers/input/serio/hil_mlc.c
@@ -934,6 +934,7 @@ int hil_mlc_register(hil_mlc *mlc)
 		snprintf(mlc_serio->name, sizeof(mlc_serio->name)-1, "HIL_SERIO%d", i);
 		snprintf(mlc_serio->phys, sizeof(mlc_serio->phys)-1, "HIL%d", i);
 		mlc_serio->id			= hil_mlc_serio_id;
+		mlc_serio->id.id		= i; /* HIL port no. */
 		mlc_serio->write		= hil_mlc_serio_write;
 		mlc_serio->open			= hil_mlc_serio_open;
 		mlc_serio->close		= hil_mlc_serio_close;
diff --git a/drivers/input/serio/i8042-x86ia64io.h b/drivers/input/serio/i8042-x86ia64io.h
index 29e686388a2c..6fa2deff7446 100644
--- a/drivers/input/serio/i8042-x86ia64io.h
+++ b/drivers/input/serio/i8042-x86ia64io.h
@@ -143,6 +143,14 @@ static struct dmi_system_id __initdata i8042_dmi_noloop_table[] = {
 			DMI_MATCH(DMI_PRODUCT_VERSION, "M606"),
 		},
 	},
+	{
+		.ident = "Gigabyte M912",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "GIGABYTE"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "M912"),
+			DMI_MATCH(DMI_PRODUCT_VERSION, "01"),
+		},
+	},
 	{ }
 };
 
@@ -351,6 +359,13 @@ static struct dmi_system_id __initdata i8042_dmi_nomux_table[] = {
 			DMI_MATCH(DMI_PRODUCT_NAME, "HEL80I"),
 		},
 	},
+	{
+		.ident = "Dell Vostro 1510",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Vostro1510"),
+		},
+	},
 	{ }
 };
 
diff --git a/drivers/input/serio/libps2.c b/drivers/input/serio/libps2.c
index 2b304c22c200..67248c31e19a 100644
--- a/drivers/input/serio/libps2.c
+++ b/drivers/input/serio/libps2.c
@@ -262,9 +262,17 @@ int ps2_handle_ack(struct ps2dev *ps2dev, unsigned char data)
 			break;
 
 		case PS2_RET_NAK:
-			ps2dev->nak = 1;
+			ps2dev->flags |= PS2_FLAG_NAK;
+			ps2dev->nak = PS2_RET_NAK;
 			break;
 
+		case PS2_RET_ERR:
+			if (ps2dev->flags & PS2_FLAG_NAK) {
+				ps2dev->flags &= ~PS2_FLAG_NAK;
+				ps2dev->nak = PS2_RET_ERR;
+				break;
+			}
+
 		/*
 		 * Workaround for mice which don't ACK the Get ID command.
 		 * These are valid mouse IDs that we recognize.
@@ -282,8 +290,11 @@ int ps2_handle_ack(struct ps2dev *ps2dev, unsigned char data)
 	}
 
 
-	if (!ps2dev->nak && ps2dev->cmdcnt)
-		ps2dev->flags |= PS2_FLAG_CMD | PS2_FLAG_CMD1;
+	if (!ps2dev->nak) {
+		ps2dev->flags &= ~PS2_FLAG_NAK;
+		if (ps2dev->cmdcnt)
+			ps2dev->flags |= PS2_FLAG_CMD | PS2_FLAG_CMD1;
+	}
 
 	ps2dev->flags &= ~PS2_FLAG_ACK;
 	wake_up(&ps2dev->wait);
@@ -329,6 +340,7 @@ void ps2_cmd_aborted(struct ps2dev *ps2dev)
 	if (ps2dev->flags & (PS2_FLAG_ACK | PS2_FLAG_CMD))
 		wake_up(&ps2dev->wait);
 
-	ps2dev->flags = 0;
+	/* reset all flags except last nack */
+	ps2dev->flags &= PS2_FLAG_NAK;
 }
 EXPORT_SYMBOL(ps2_cmd_aborted);
diff --git a/drivers/input/serio/pcips2.c b/drivers/input/serio/pcips2.c
index 1b404f9e3bff..1dacbe0d9348 100644
--- a/drivers/input/serio/pcips2.c
+++ b/drivers/input/serio/pcips2.c
@@ -153,7 +153,7 @@ static int __devinit pcips2_probe(struct pci_dev *dev, const struct pci_device_i
 	serio->open		= pcips2_open;
 	serio->close		= pcips2_close;
 	strlcpy(serio->name, pci_name(dev), sizeof(serio->name));
-	strlcpy(serio->phys, dev->dev.bus_id, sizeof(serio->phys));
+	strlcpy(serio->phys, dev_name(&dev->dev), sizeof(serio->phys));
 	serio->port_data	= ps2if;
 	serio->dev.parent	= &dev->dev;
 	ps2if->io		= serio;
diff --git a/drivers/input/serio/serio.c b/drivers/input/serio/serio.c
index 2f12d60eee3b..bc033250dfcd 100644
--- a/drivers/input/serio/serio.c
+++ b/drivers/input/serio/serio.c
@@ -546,8 +546,8 @@ static void serio_init_port(struct serio *serio)
 	spin_lock_init(&serio->lock);
 	mutex_init(&serio->drv_mutex);
 	device_initialize(&serio->dev);
-	snprintf(serio->dev.bus_id, sizeof(serio->dev.bus_id),
-		 "serio%ld", (long)atomic_inc_return(&serio_no) - 1);
+	dev_set_name(&serio->dev, "serio%ld",
+			(long)atomic_inc_return(&serio_no) - 1);
 	serio->dev.bus = &serio_bus;
 	serio->dev.release = serio_release_port;
 	if (serio->parent) {
diff --git a/drivers/input/serio/xilinx_ps2.c b/drivers/input/serio/xilinx_ps2.c
index 765007899d9a..ebb22f88c842 100644
--- a/drivers/input/serio/xilinx_ps2.c
+++ b/drivers/input/serio/xilinx_ps2.c
@@ -58,23 +58,20 @@
 
 /* Mask for all the Receive Interrupts */
 #define XPS2_IPIXR_RX_ALL	(XPS2_IPIXR_RX_OVF | XPS2_IPIXR_RX_ERR |  \
-					XPS2_IPIXR_RX_FULL)
+				 XPS2_IPIXR_RX_FULL)
 
 /* Mask for all the Interrupts */
 #define XPS2_IPIXR_ALL		(XPS2_IPIXR_TX_ALL | XPS2_IPIXR_RX_ALL |  \
-					XPS2_IPIXR_WDT_TOUT)
+				 XPS2_IPIXR_WDT_TOUT)
 
 /* Global Interrupt Enable mask */
 #define XPS2_GIER_GIE_MASK	0x80000000
 
 struct xps2data {
 	int irq;
-	u32 phys_addr;
-	u32 remap_size;
 	spinlock_t lock;
-	u8 rxb;				/* Rx buffer */
 	void __iomem *base_address;	/* virt. address of control registers */
-	unsigned int dfl;
+	unsigned int flags;
 	struct serio serio;		/* serio */
 };
 
@@ -82,8 +79,13 @@ struct xps2data {
 /* XPS PS/2 data transmission calls */
 /************************************/
 
-/*
- * xps2_recv() will attempt to receive a byte of data from the PS/2 port.
+/**
+ * xps2_recv() - attempts to receive a byte from the PS/2 port.
+ * @drvdata:	pointer to ps2 device private data structure
+ * @byte:	address where the read data will be copied
+ *
+ * If there is any data available in the PS/2 receiver, this functions reads
+ * the data, otherwise it returns error.
  */
 static int xps2_recv(struct xps2data *drvdata, u8 *byte)
 {
@@ -116,33 +118,27 @@ static irqreturn_t xps2_interrupt(int irq, void *dev_id)
 
 	/* Check which interrupt is active */
 	if (intr_sr & XPS2_IPIXR_RX_OVF)
-		printk(KERN_WARNING "%s: receive overrun error\n",
-			drvdata->serio.name);
+		dev_warn(drvdata->serio.dev.parent, "receive overrun error\n");
 
 	if (intr_sr & XPS2_IPIXR_RX_ERR)
-		drvdata->dfl |= SERIO_PARITY;
+		drvdata->flags |= SERIO_PARITY;
 
 	if (intr_sr & (XPS2_IPIXR_TX_NOACK | XPS2_IPIXR_WDT_TOUT))
-		drvdata->dfl |= SERIO_TIMEOUT;
+		drvdata->flags |= SERIO_TIMEOUT;
 
 	if (intr_sr & XPS2_IPIXR_RX_FULL) {
-		status = xps2_recv(drvdata, &drvdata->rxb);
+		status = xps2_recv(drvdata, &c);
 
 		/* Error, if a byte is not received */
 		if (status) {
-			printk(KERN_ERR
-				"%s: wrong rcvd byte count (%d)\n",
-				drvdata->serio.name, status);
+			dev_err(drvdata->serio.dev.parent,
+				"wrong rcvd byte count (%d)\n", status);
 		} else {
-			c = drvdata->rxb;
-			serio_interrupt(&drvdata->serio, c, drvdata->dfl);
-			drvdata->dfl = 0;
+			serio_interrupt(&drvdata->serio, c, drvdata->flags);
+			drvdata->flags = 0;
 		}
 	}
 
-	if (intr_sr & XPS2_IPIXR_TX_ACK)
-		drvdata->dfl = 0;
-
 	return IRQ_HANDLED;
 }
 
@@ -150,8 +146,15 @@ static irqreturn_t xps2_interrupt(int irq, void *dev_id)
 /* serio callbacks */
 /*******************/
 
-/*
- * sxps2_write() sends a byte out through the PS/2 interface.
+/**
+ * sxps2_write() - sends a byte out through the PS/2 port.
+ * @pserio:	pointer to the serio structure of the PS/2 port
+ * @c:		data that needs to be written to the PS/2 port
+ *
+ * This function checks if the PS/2 transmitter is empty and sends a byte.
+ * Otherwise it returns error. Transmission fails only when nothing is connected
+ * to the PS/2 port. Thats why, we do not try to resend the data in case of a
+ * failure.
  */
 static int sxps2_write(struct serio *pserio, unsigned char c)
 {
@@ -174,33 +177,39 @@ static int sxps2_write(struct serio *pserio, unsigned char c)
 	return status;
 }
 
-/*
- * sxps2_open() is called when a port is open by the higher layer.
+/**
+ * sxps2_open() - called when a port is opened by the higher layer.
+ * @pserio:	pointer to the serio structure of the PS/2 device
+ *
+ * This function requests irq and enables interrupts for the PS/2 device.
  */
 static int sxps2_open(struct serio *pserio)
 {
 	struct xps2data *drvdata = pserio->port_data;
-	int retval;
+	int error;
+	u8 c;
 
-	retval = request_irq(drvdata->irq, &xps2_interrupt, 0,
+	error = request_irq(drvdata->irq, &xps2_interrupt, 0,
 				DRIVER_NAME, drvdata);
-	if (retval) {
-		printk(KERN_ERR
-			"%s: Couldn't allocate interrupt %d\n",
-			drvdata->serio.name, drvdata->irq);
-		return retval;
+	if (error) {
+		dev_err(drvdata->serio.dev.parent,
+			"Couldn't allocate interrupt %d\n", drvdata->irq);
+		return error;
 	}
 
 	/* start reception by enabling the interrupts */
 	out_be32(drvdata->base_address + XPS2_GIER_OFFSET, XPS2_GIER_GIE_MASK);
 	out_be32(drvdata->base_address + XPS2_IPIER_OFFSET, XPS2_IPIXR_RX_ALL);
-	(void)xps2_recv(drvdata, &drvdata->rxb);
+	(void)xps2_recv(drvdata, &c);
 
 	return 0;		/* success */
 }
 
-/*
- * sxps2_close() frees the interrupt.
+/**
+ * sxps2_close() - frees the interrupt.
+ * @pserio:	pointer to the serio structure of the PS/2 device
+ *
+ * This function frees the irq and disables interrupts for the PS/2 device.
  */
 static void sxps2_close(struct serio *pserio)
 {
@@ -212,24 +221,41 @@ static void sxps2_close(struct serio *pserio)
 	free_irq(drvdata->irq, drvdata);
 }
 
-/*********************/
-/* Device setup code */
-/*********************/
-
-static int xps2_setup(struct device *dev, struct resource *regs_res,
-		      struct resource *irq_res)
+/**
+ * xps2_of_probe - probe method for the PS/2 device.
+ * @of_dev:	pointer to OF device structure
+ * @match:	pointer to the stucture used for matching a device
+ *
+ * This function probes the PS/2 device in the device tree.
+ * It initializes the driver data structure and the hardware.
+ * It returns 0, if the driver is bound to the PS/2 device, or a negative
+ * value if there is an error.
+ */
+static int __devinit xps2_of_probe(struct of_device *ofdev,
+				   const struct of_device_id *match)
 {
+	struct resource r_irq; /* Interrupt resources */
+	struct resource r_mem; /* IO mem resources */
 	struct xps2data *drvdata;
 	struct serio *serio;
-	unsigned long remap_size;
-	int retval;
+	struct device *dev = &ofdev->dev;
+	resource_size_t remap_size, phys_addr;
+	int error;
+
+	dev_info(dev, "Device Tree Probing \'%s\'\n",
+			ofdev->node->name);
 
-	if (!dev)
-		return -EINVAL;
+	/* Get iospace for the device */
+	error = of_address_to_resource(ofdev->node, 0, &r_mem);
+	if (error) {
+		dev_err(dev, "invalid address\n");
+		return error;
+	}
 
-	if (!regs_res || !irq_res) {
-		dev_err(dev, "IO resource(s) not found\n");
-		return -EINVAL;
+	/* Get IRQ for the device */
+	if (of_irq_to_resource(ofdev->node, 0, &r_irq) == NO_IRQ) {
+		dev_err(dev, "no IRQ found\n");
+		return -ENODEV;
 	}
 
 	drvdata = kzalloc(sizeof(struct xps2data), GFP_KERNEL);
@@ -241,24 +267,23 @@ static int xps2_setup(struct device *dev, struct resource *regs_res,
 	dev_set_drvdata(dev, drvdata);
 
 	spin_lock_init(&drvdata->lock);
-	drvdata->irq = irq_res->start;
-
-	remap_size = regs_res->end - regs_res->start + 1;
-	if (!request_mem_region(regs_res->start, remap_size, DRIVER_NAME)) {
-		dev_err(dev, "Couldn't lock memory region at 0x%08X\n",
-			(unsigned int)regs_res->start);
-		retval = -EBUSY;
+	drvdata->irq = r_irq.start;
+
+	phys_addr = r_mem.start;
+	remap_size = r_mem.end - r_mem.start + 1;
+	if (!request_mem_region(phys_addr, remap_size, DRIVER_NAME)) {
+		dev_err(dev, "Couldn't lock memory region at 0x%08llX\n",
+			(unsigned long long)phys_addr);
+		error = -EBUSY;
 		goto failed1;
 	}
 
 	/* Fill in configuration data and add them to the list */
-	drvdata->phys_addr = regs_res->start;
-	drvdata->remap_size = remap_size;
-	drvdata->base_address = ioremap(regs_res->start, remap_size);
+	drvdata->base_address = ioremap(phys_addr, remap_size);
 	if (drvdata->base_address == NULL) {
-		dev_err(dev, "Couldn't ioremap memory at 0x%08X\n",
-			(unsigned int)regs_res->start);
-		retval = -EFAULT;
+		dev_err(dev, "Couldn't ioremap memory at 0x%08llX\n",
+			(unsigned long long)phys_addr);
+		error = -EFAULT;
 		goto failed2;
 	}
 
@@ -269,8 +294,9 @@ static int xps2_setup(struct device *dev, struct resource *regs_res,
 	 * we have the PS2 in a good state */
 	out_be32(drvdata->base_address + XPS2_SRST_OFFSET, XPS2_SRST_RESET);
 
-	dev_info(dev, "Xilinx PS2 at 0x%08X mapped to 0x%p, irq=%d\n",
-		drvdata->phys_addr, drvdata->base_address, drvdata->irq);
+	dev_info(dev, "Xilinx PS2 at 0x%08llX mapped to 0x%p, irq=%d\n",
+		 (unsigned long long)phys_addr, drvdata->base_address,
+		 drvdata->irq);
 
 	serio = &drvdata->serio;
 	serio->id.type = SERIO_8042;
@@ -280,71 +306,51 @@ static int xps2_setup(struct device *dev, struct resource *regs_res,
 	serio->port_data = drvdata;
 	serio->dev.parent = dev;
 	snprintf(serio->name, sizeof(serio->name),
-		 "Xilinx XPS PS/2 at %08X", drvdata->phys_addr);
+		 "Xilinx XPS PS/2 at %08llX", (unsigned long long)phys_addr);
 	snprintf(serio->phys, sizeof(serio->phys),
-		 "xilinxps2/serio at %08X", drvdata->phys_addr);
+		 "xilinxps2/serio at %08llX", (unsigned long long)phys_addr);
+
 	serio_register_port(serio);
 
 	return 0;		/* success */
 
 failed2:
-	release_mem_region(regs_res->start, remap_size);
+	release_mem_region(phys_addr, remap_size);
 failed1:
 	kfree(drvdata);
 	dev_set_drvdata(dev, NULL);
 
-	return retval;
-}
-
-/***************************/
-/* OF Platform Bus Support */
-/***************************/
-
-static int __devinit xps2_of_probe(struct of_device *ofdev, const struct
-				   of_device_id * match)
-{
-	struct resource r_irq; /* Interrupt resources */
-	struct resource r_mem; /* IO mem resources */
-	int rc = 0;
-
-	printk(KERN_INFO "Device Tree Probing \'%s\'\n",
-			ofdev->node->name);
-
-	/* Get iospace for the device */
-	rc = of_address_to_resource(ofdev->node, 0, &r_mem);
-	if (rc) {
-		dev_err(&ofdev->dev, "invalid address\n");
-		return rc;
-	}
-
-	/* Get IRQ for the device */
-	rc = of_irq_to_resource(ofdev->node, 0, &r_irq);
-	if (rc == NO_IRQ) {
-		dev_err(&ofdev->dev, "no IRQ found\n");
-		return rc;
-	}
-
-	return xps2_setup(&ofdev->dev, &r_mem, &r_irq);
+	return error;
 }
 
+/**
+ * xps2_of_remove - unbinds the driver from the PS/2 device.
+ * @of_dev:	pointer to OF 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 any resources allocated to
+ * the device.
+ */
 static int __devexit xps2_of_remove(struct of_device *of_dev)
 {
 	struct device *dev = &of_dev->dev;
-	struct xps2data *drvdata;
-
-	if (!dev)
-		return -EINVAL;
-
-	drvdata = dev_get_drvdata(dev);
+	struct xps2data *drvdata = dev_get_drvdata(dev);
+	struct resource r_mem; /* IO mem resources */
 
 	serio_unregister_port(&drvdata->serio);
 	iounmap(drvdata->base_address);
-	release_mem_region(drvdata->phys_addr, drvdata->remap_size);
+
+	/* Get iospace of the device */
+	if (of_address_to_resource(of_dev->node, 0, &r_mem))
+		dev_err(dev, "invalid address\n");
+	else
+		release_mem_region(r_mem.start, r_mem.end - r_mem.start + 1);
+
 	kfree(drvdata);
 
 	dev_set_drvdata(dev, NULL);
 
-	return 0;		/* success */
+	return 0;
 }
 
 /* Match table for of_platform binding */
diff --git a/drivers/input/tablet/gtco.c b/drivers/input/tablet/gtco.c
index 5524e01dbb1a..2e18a1c0c351 100644
--- a/drivers/input/tablet/gtco.c
+++ b/drivers/input/tablet/gtco.c
@@ -877,7 +877,7 @@ static int gtco_probe(struct usb_interface *usbinterface,
 	dbg("num endpoints:     %d", usbinterface->cur_altsetting->desc.bNumEndpoints);
 	dbg("interface class:   %d", usbinterface->cur_altsetting->desc.bInterfaceClass);
 	dbg("endpoint: attribute:0x%x type:0x%x", endpoint->bmAttributes, endpoint->bDescriptorType);
-	if ((endpoint->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) == USB_ENDPOINT_XFER_INT)
+	if (usb_endpoint_xfer_int(endpoint))
 		dbg("endpoint: we have interrupt endpoint\n");
 
 	dbg("endpoint extra len:%d ", usbinterface->altsetting[0].extralen);
diff --git a/drivers/input/tablet/wacom_wac.c b/drivers/input/tablet/wacom_wac.c
index 8dc8d1e59bea..2638811c61ac 100644
--- a/drivers/input/tablet/wacom_wac.c
+++ b/drivers/input/tablet/wacom_wac.c
@@ -535,7 +535,7 @@ static int wacom_intuos_irq(struct wacom_wac *wacom, void *wcombo)
 	return 1;
 }
 
-int wacom_tpc_irq(struct wacom_wac *wacom, void *wcombo)
+static int wacom_tpc_irq(struct wacom_wac *wacom, void *wcombo)
 {
 	char *data = wacom->data;
 	int prox = 0, pressure;
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 3d1ab8fa9acc..bb6486a8c070 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -58,6 +58,14 @@ config TOUCHSCREEN_CORGI
 	  NOTE: this driver is deprecated, try enable SPI and generic
 	  ADS7846-based touchscreen driver.
 
+config TOUCHSCREEN_DA9034
+	tristate "Touchscreen support for Dialog Semiconductor DA9034"
+	depends on PMIC_DA903X
+	default y
+	help
+	  Say Y here to enable the support for the touchscreen found
+	  on Dialog Semiconductor DA9034 PMIC.
+
 config TOUCHSCREEN_FUJITSU
 	tristate "Fujitsu serial touchscreen"
 	select SERIO
@@ -95,6 +103,19 @@ config TOUCHSCREEN_ELO
 	  To compile this driver as a module, choose M here: the
 	  module will be called elo.
 
+config TOUCHSCREEN_WACOM_W8001
+	tristate "Wacom W8001 penabled serial touchscreen"
+	select SERIO
+	help
+	  Say Y here if you have an Wacom W8001 penabled serial touchscreen
+	  connected to your system.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called wacom_w8001.
+
+
 config TOUCHSCREEN_MTOUCH
 	tristate "MicroTouch serial touchscreens"
 	select SERIO
@@ -376,4 +397,15 @@ config TOUCHSCREEN_TOUCHIT213
 	  To compile this driver as a module, choose M here: the
 	  module will be called touchit213.
 
+config TOUCHSCREEN_TSC2007
+	tristate "TSC2007 based touchscreens"
+	depends on I2C
+	help
+	  Say Y here if you have a TSC2007 based touchscreen.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tsc2007.
+
 endif
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index 15cf29079489..d3375aff46fe 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -25,8 +25,11 @@ obj-$(CONFIG_TOUCHSCREEN_PENMOUNT)	+= penmount.o
 obj-$(CONFIG_TOUCHSCREEN_TOUCHIT213)	+= touchit213.o
 obj-$(CONFIG_TOUCHSCREEN_TOUCHRIGHT)	+= touchright.o
 obj-$(CONFIG_TOUCHSCREEN_TOUCHWIN)	+= touchwin.o
+obj-$(CONFIG_TOUCHSCREEN_TSC2007)	+= tsc2007.o
 obj-$(CONFIG_TOUCHSCREEN_UCB1400)	+= ucb1400_ts.o
+obj-$(CONFIG_TOUCHSCREEN_WACOM_W8001)	+= wacom_w8001.o
 obj-$(CONFIG_TOUCHSCREEN_WM97XX)	+= wm97xx-ts.o
+obj-$(CONFIG_TOUCHSCREEN_DA9034)	+= da9034-ts.o
 wm97xx-ts-$(CONFIG_TOUCHSCREEN_WM9705)	+= wm9705.o
 wm97xx-ts-$(CONFIG_TOUCHSCREEN_WM9712)	+= wm9712.o
 wm97xx-ts-$(CONFIG_TOUCHSCREEN_WM9713)	+= wm9713.o
diff --git a/drivers/input/touchscreen/ads7846.c b/drivers/input/touchscreen/ads7846.c
index e1ece89fe922..7c27c8b9b6d0 100644
--- a/drivers/input/touchscreen/ads7846.c
+++ b/drivers/input/touchscreen/ads7846.c
@@ -472,7 +472,7 @@ static ssize_t ads7846_disable_store(struct device *dev,
 				     const char *buf, size_t count)
 {
 	struct ads7846 *ts = dev_get_drvdata(dev);
-	long i;
+	unsigned long i;
 
 	if (strict_strtoul(buf, 10, &i))
 		return -EINVAL;
@@ -559,7 +559,7 @@ static void ads7846_rx(void *ads)
 	if (packet->tc.ignore || Rt > ts->pressure_max) {
 #ifdef VERBOSE
 		pr_debug("%s: ignored %d pressure %d\n",
-			ts->spi->dev.bus_id, packet->tc.ignore, Rt);
+			dev_name(&ts->spi->dev), packet->tc.ignore, Rt);
 #endif
 		hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_PERIOD),
 			      HRTIMER_MODE_REL);
@@ -947,7 +947,7 @@ static int __devinit ads7846_probe(struct spi_device *spi)
 		ts->penirq_recheck_delay_usecs =
 				pdata->penirq_recheck_delay_usecs;
 
-	snprintf(ts->phys, sizeof(ts->phys), "%s/input0", spi->dev.bus_id);
+	snprintf(ts->phys, sizeof(ts->phys), "%s/input0", dev_name(&spi->dev));
 
 	input_dev->name = "ADS784x Touchscreen";
 	input_dev->phys = ts->phys;
diff --git a/drivers/input/touchscreen/da9034-ts.c b/drivers/input/touchscreen/da9034-ts.c
new file mode 100644
index 000000000000..fa67d782c3c3
--- /dev/null
+++ b/drivers/input/touchscreen/da9034-ts.c
@@ -0,0 +1,390 @@
+/*
+ * Touchscreen driver for Dialog Semiconductor DA9034
+ *
+ * Copyright (C) 2006-2008 Marvell International Ltd.
+ *	Fengwei Yin <fengwei.yin@marvell.com>
+ *	Eric Miao <eric.miao@marvell.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/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/mfd/da903x.h>
+
+#define DA9034_MANUAL_CTRL	0x50
+#define DA9034_LDO_ADC_EN	(1 << 4)
+
+#define DA9034_AUTO_CTRL1	0x51
+
+#define DA9034_AUTO_CTRL2	0x52
+#define DA9034_AUTO_TSI_EN	(1 << 3)
+#define DA9034_PEN_DETECT	(1 << 4)
+
+#define DA9034_TSI_CTRL1	0x53
+#define DA9034_TSI_CTRL2	0x54
+#define DA9034_TSI_X_MSB	0x6c
+#define DA9034_TSI_Y_MSB	0x6d
+#define DA9034_TSI_XY_LSB	0x6e
+
+enum {
+	STATE_IDLE,	/* wait for pendown */
+	STATE_BUSY,	/* TSI busy sampling */
+	STATE_STOP,	/* sample available */
+	STATE_WAIT,	/* Wait to start next sample */
+};
+
+enum {
+	EVENT_PEN_DOWN,
+	EVENT_PEN_UP,
+	EVENT_TSI_READY,
+	EVENT_TIMEDOUT,
+};
+
+struct da9034_touch {
+	struct device		*da9034_dev;
+	struct input_dev	*input_dev;
+
+	struct delayed_work	tsi_work;
+	struct notifier_block	notifier;
+
+	int	state;
+
+	int	interval_ms;
+	int	x_inverted;
+	int	y_inverted;
+
+	int	last_x;
+	int	last_y;
+};
+
+static inline int is_pen_down(struct da9034_touch *touch)
+{
+	return da903x_query_status(touch->da9034_dev, DA9034_STATUS_PEN_DOWN);
+}
+
+static inline int detect_pen_down(struct da9034_touch *touch, int on)
+{
+	if (on)
+		return da903x_set_bits(touch->da9034_dev,
+				DA9034_AUTO_CTRL2, DA9034_PEN_DETECT);
+	else
+		return da903x_clr_bits(touch->da9034_dev,
+				DA9034_AUTO_CTRL2, DA9034_PEN_DETECT);
+}
+
+static int read_tsi(struct da9034_touch *touch)
+{
+	uint8_t _x, _y, _v;
+	int ret;
+
+	ret = da903x_read(touch->da9034_dev, DA9034_TSI_X_MSB, &_x);
+	if (ret)
+		return ret;
+
+	ret = da903x_read(touch->da9034_dev, DA9034_TSI_Y_MSB, &_y);
+	if (ret)
+		return ret;
+
+	ret = da903x_read(touch->da9034_dev, DA9034_TSI_XY_LSB, &_v);
+	if (ret)
+		return ret;
+
+	touch->last_x = ((_x << 2) & 0x3fc) | (_v & 0x3);
+	touch->last_y = ((_y << 2) & 0x3fc) | ((_v & 0xc) >> 2);
+
+	return 0;
+}
+
+static inline int start_tsi(struct da9034_touch *touch)
+{
+	return da903x_set_bits(touch->da9034_dev,
+			DA9034_AUTO_CTRL2, DA9034_AUTO_TSI_EN);
+}
+
+static inline int stop_tsi(struct da9034_touch *touch)
+{
+	return da903x_clr_bits(touch->da9034_dev,
+			DA9034_AUTO_CTRL2, DA9034_AUTO_TSI_EN);
+}
+
+static inline void report_pen_down(struct da9034_touch *touch)
+{
+	int x = touch->last_x;
+	int y = touch->last_y;
+
+	x &= 0xfff;
+	if (touch->x_inverted)
+		x = 1024 - x;
+	y &= 0xfff;
+	if (touch->y_inverted)
+		y = 1024 - y;
+
+	input_report_abs(touch->input_dev, ABS_X, x);
+	input_report_abs(touch->input_dev, ABS_Y, y);
+	input_report_key(touch->input_dev, BTN_TOUCH, 1);
+
+	input_sync(touch->input_dev);
+}
+
+static inline void report_pen_up(struct da9034_touch *touch)
+{
+	input_report_key(touch->input_dev, BTN_TOUCH, 0);
+	input_sync(touch->input_dev);
+}
+
+static void da9034_event_handler(struct da9034_touch *touch, int event)
+{
+	int err;
+
+	switch (touch->state) {
+	case STATE_IDLE:
+		if (event != EVENT_PEN_DOWN)
+			break;
+
+		/* Enable auto measurement of the TSI, this will
+		 * automatically disable pen down detection
+		 */
+		err = start_tsi(touch);
+		if (err)
+			goto err_reset;
+
+		touch->state = STATE_BUSY;
+		break;
+
+	case STATE_BUSY:
+		if (event != EVENT_TSI_READY)
+			break;
+
+		err = read_tsi(touch);
+		if (err)
+			goto err_reset;
+
+		/* Disable auto measurement of the TSI, so that
+		 * pen down status will be available
+		 */
+		err = stop_tsi(touch);
+		if (err)
+			goto err_reset;
+
+		touch->state = STATE_STOP;
+		break;
+
+	case STATE_STOP:
+		if (event == EVENT_PEN_DOWN) {
+			report_pen_down(touch);
+			schedule_delayed_work(&touch->tsi_work,
+				msecs_to_jiffies(touch->interval_ms));
+			touch->state = STATE_WAIT;
+		}
+
+		if (event == EVENT_PEN_UP) {
+			report_pen_up(touch);
+			touch->state = STATE_IDLE;
+		}
+
+		input_sync(touch->input_dev);
+		break;
+
+	case STATE_WAIT:
+		if (event != EVENT_TIMEDOUT)
+			break;
+
+		if (is_pen_down(touch)) {
+			start_tsi(touch);
+			touch->state = STATE_BUSY;
+		} else
+			touch->state = STATE_IDLE;
+		break;
+	}
+	return;
+
+err_reset:
+	touch->state = STATE_IDLE;
+	stop_tsi(touch);
+	detect_pen_down(touch, 1);
+}
+
+static void da9034_tsi_work(struct work_struct *work)
+{
+	struct da9034_touch *touch =
+		container_of(work, struct da9034_touch, tsi_work.work);
+
+	da9034_event_handler(touch, EVENT_TIMEDOUT);
+}
+
+static int da9034_touch_notifier(struct notifier_block *nb,
+				 unsigned long event, void *data)
+{
+	struct da9034_touch *touch =
+		container_of(nb, struct da9034_touch, notifier);
+
+	if (event & DA9034_EVENT_PEN_DOWN) {
+		if (is_pen_down(touch))
+			da9034_event_handler(touch, EVENT_PEN_DOWN);
+		else
+			da9034_event_handler(touch, EVENT_PEN_UP);
+	}
+
+	if (event & DA9034_EVENT_TSI_READY)
+		da9034_event_handler(touch, EVENT_TSI_READY);
+
+	return 0;
+}
+
+static int da9034_touch_open(struct input_dev *dev)
+{
+	struct da9034_touch *touch = input_get_drvdata(dev);
+	int ret;
+
+	ret = da903x_register_notifier(touch->da9034_dev, &touch->notifier,
+			DA9034_EVENT_PEN_DOWN | DA9034_EVENT_TSI_READY);
+	if (ret)
+		return -EBUSY;
+
+	/* Enable ADC LDO */
+	ret = da903x_set_bits(touch->da9034_dev,
+			DA9034_MANUAL_CTRL, DA9034_LDO_ADC_EN);
+	if (ret)
+		return ret;
+
+	/* TSI_DELAY: 3 slots, TSI_SKIP: 3 slots */
+	ret = da903x_write(touch->da9034_dev, DA9034_TSI_CTRL1, 0x1b);
+	if (ret)
+		return ret;
+
+	ret = da903x_write(touch->da9034_dev, DA9034_TSI_CTRL2, 0x00);
+	if (ret)
+		return ret;
+
+	touch->state = STATE_IDLE;
+	detect_pen_down(touch, 1);
+
+	return 0;
+}
+
+static void da9034_touch_close(struct input_dev *dev)
+{
+	struct da9034_touch *touch = input_get_drvdata(dev);
+
+	da903x_unregister_notifier(touch->da9034_dev, &touch->notifier,
+			DA9034_EVENT_PEN_DOWN | DA9034_EVENT_TSI_READY);
+
+	cancel_delayed_work_sync(&touch->tsi_work);
+
+	touch->state = STATE_IDLE;
+	stop_tsi(touch);
+	detect_pen_down(touch, 0);
+
+	/* Disable ADC LDO */
+	da903x_clr_bits(touch->da9034_dev,
+			DA9034_MANUAL_CTRL, DA9034_LDO_ADC_EN);
+}
+
+
+static int __devinit da9034_touch_probe(struct platform_device *pdev)
+{
+	struct da9034_touch_pdata *pdata = pdev->dev.platform_data;
+	struct da9034_touch *touch;
+	struct input_dev *input_dev;
+	int ret;
+
+	touch = kzalloc(sizeof(struct da9034_touch), GFP_KERNEL);
+	if (touch == NULL) {
+		dev_err(&pdev->dev, "failed to allocate driver data\n");
+		return -ENOMEM;
+	}
+
+	touch->da9034_dev = pdev->dev.parent;
+
+	if (pdata) {
+		touch->interval_ms	= pdata->interval_ms;
+		touch->x_inverted	= pdata->x_inverted;
+		touch->y_inverted	= pdata->y_inverted;
+	} else
+		/* fallback into default */
+		touch->interval_ms	= 10;
+
+	INIT_DELAYED_WORK(&touch->tsi_work, da9034_tsi_work);
+	touch->notifier.notifier_call = da9034_touch_notifier;
+
+	input_dev = input_allocate_device();
+	if (!input_dev) {
+		dev_err(&pdev->dev, "failed to allocate input device\n");
+		ret = -ENOMEM;
+		goto err_free_touch;
+	}
+
+	input_dev->name		= pdev->name;
+	input_dev->open		= da9034_touch_open;
+	input_dev->close	= da9034_touch_close;
+	input_dev->dev.parent	= &pdev->dev;
+
+	__set_bit(EV_ABS, input_dev->evbit);
+	__set_bit(ABS_X, input_dev->absbit);
+	__set_bit(ABS_Y, input_dev->absbit);
+	input_set_abs_params(input_dev, ABS_X, 0, 1023, 0, 0);
+	input_set_abs_params(input_dev, ABS_Y, 0, 1023, 0, 0);
+
+	__set_bit(EV_KEY, input_dev->evbit);
+	__set_bit(BTN_TOUCH, input_dev->keybit);
+
+	touch->input_dev = input_dev;
+	input_set_drvdata(input_dev, touch);
+
+	ret = input_register_device(input_dev);
+	if (ret)
+		goto err_free_input;
+
+	platform_set_drvdata(pdev, touch);
+	return 0;
+
+err_free_input:
+	input_free_device(input_dev);
+err_free_touch:
+	kfree(touch);
+	return ret;
+}
+
+static int __devexit da9034_touch_remove(struct platform_device *pdev)
+{
+	struct da9034_touch *touch = platform_get_drvdata(pdev);
+
+	input_unregister_device(touch->input_dev);
+	kfree(touch);
+
+	return 0;
+}
+
+static struct platform_driver da9034_touch_driver = {
+	.driver	= {
+		.name	= "da9034-touch",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= da9034_touch_probe,
+	.remove		= __devexit_p(da9034_touch_remove),
+};
+
+static int __init da9034_touch_init(void)
+{
+	return platform_driver_register(&da9034_touch_driver);
+}
+module_init(da9034_touch_init);
+
+static void __exit da9034_touch_exit(void)
+{
+	platform_driver_unregister(&da9034_touch_driver);
+}
+module_exit(da9034_touch_exit);
+
+MODULE_DESCRIPTION("Touchscreen driver for Dialog Semiconductor DA9034");
+MODULE_AUTHOR("Eric Miao <eric.miao@marvell.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:da9034-touch");
diff --git a/drivers/input/touchscreen/tsc2007.c b/drivers/input/touchscreen/tsc2007.c
new file mode 100644
index 000000000000..b75dc2990574
--- /dev/null
+++ b/drivers/input/touchscreen/tsc2007.c
@@ -0,0 +1,381 @@
+/*
+ * drivers/input/touchscreen/tsc2007.c
+ *
+ * Copyright (c) 2008 MtekVision Co., Ltd.
+ *	Kwangwoo Lee <kwlee@mtekvision.com>
+ *
+ * Using code from:
+ *  - ads7846.c
+ *	Copyright (c) 2005 David Brownell
+ *	Copyright (c) 2006 Nokia Corporation
+ *  - corgi_ts.c
+ *	Copyright (C) 2004-2005 Richard Purdie
+ *  - omap_ts.[hc], ads7846.h, ts_osk.c
+ *	Copyright (C) 2002 MontaVista Software
+ *	Copyright (C) 2004 Texas Instruments
+ *	Copyright (C) 2005 Dirk Behme
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/hrtimer.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/i2c/tsc2007.h>
+
+#define TS_POLL_DELAY	(10 * 1000)	/* ns delay before the first sample */
+#define TS_POLL_PERIOD	(5 * 1000)	/* ns delay between samples */
+
+#define TSC2007_MEASURE_TEMP0		(0x0 << 4)
+#define TSC2007_MEASURE_AUX		(0x2 << 4)
+#define TSC2007_MEASURE_TEMP1		(0x4 << 4)
+#define TSC2007_ACTIVATE_XN		(0x8 << 4)
+#define TSC2007_ACTIVATE_YN		(0x9 << 4)
+#define TSC2007_ACTIVATE_YP_XN		(0xa << 4)
+#define TSC2007_SETUP			(0xb << 4)
+#define TSC2007_MEASURE_X		(0xc << 4)
+#define TSC2007_MEASURE_Y		(0xd << 4)
+#define TSC2007_MEASURE_Z1		(0xe << 4)
+#define TSC2007_MEASURE_Z2		(0xf << 4)
+
+#define TSC2007_POWER_OFF_IRQ_EN	(0x0 << 2)
+#define TSC2007_ADC_ON_IRQ_DIS0		(0x1 << 2)
+#define TSC2007_ADC_OFF_IRQ_EN		(0x2 << 2)
+#define TSC2007_ADC_ON_IRQ_DIS1		(0x3 << 2)
+
+#define TSC2007_12BIT			(0x0 << 1)
+#define TSC2007_8BIT			(0x1 << 1)
+
+#define	MAX_12BIT			((1 << 12) - 1)
+
+#define ADC_ON_12BIT	(TSC2007_12BIT | TSC2007_ADC_ON_IRQ_DIS0)
+
+#define READ_Y		(ADC_ON_12BIT | TSC2007_MEASURE_Y)
+#define READ_Z1		(ADC_ON_12BIT | TSC2007_MEASURE_Z1)
+#define READ_Z2		(ADC_ON_12BIT | TSC2007_MEASURE_Z2)
+#define READ_X		(ADC_ON_12BIT | TSC2007_MEASURE_X)
+#define PWRDOWN		(TSC2007_12BIT | TSC2007_POWER_OFF_IRQ_EN)
+
+struct ts_event {
+	u16	x;
+	u16	y;
+	u16	z1, z2;
+};
+
+struct tsc2007 {
+	struct input_dev	*input;
+	char			phys[32];
+	struct hrtimer		timer;
+	struct ts_event		tc;
+
+	struct i2c_client	*client;
+
+	spinlock_t		lock;
+
+	u16			model;
+	u16			x_plate_ohms;
+
+	unsigned		pendown;
+	int			irq;
+
+	int			(*get_pendown_state)(void);
+	void			(*clear_penirq)(void);
+};
+
+static inline int tsc2007_xfer(struct tsc2007 *tsc, u8 cmd)
+{
+	s32 data;
+	u16 val;
+
+	data = i2c_smbus_read_word_data(tsc->client, cmd);
+	if (data < 0) {
+		dev_err(&tsc->client->dev, "i2c io error: %d\n", data);
+		return data;
+	}
+
+	/* The protocol and raw data format from i2c interface:
+	 * S Addr Wr [A] Comm [A] S Addr Rd [A] [DataLow] A [DataHigh] NA P
+	 * Where DataLow has [D11-D4], DataHigh has [D3-D0 << 4 | Dummy 4bit].
+	 */
+	val = swab16(data) >> 4;
+
+	dev_dbg(&tsc->client->dev, "data: 0x%x, val: 0x%x\n", data, val);
+
+	return val;
+}
+
+static void tsc2007_send_event(void *tsc)
+{
+	struct tsc2007	*ts = tsc;
+	u32		rt;
+	u16		x, y, z1, z2;
+
+	x = ts->tc.x;
+	y = ts->tc.y;
+	z1 = ts->tc.z1;
+	z2 = ts->tc.z2;
+
+	/* range filtering */
+	if (x == MAX_12BIT)
+		x = 0;
+
+	if (likely(x && z1)) {
+		/* compute touch pressure resistance using equation #1 */
+		rt = z2;
+		rt -= z1;
+		rt *= x;
+		rt *= ts->x_plate_ohms;
+		rt /= z1;
+		rt = (rt + 2047) >> 12;
+	} else
+		rt = 0;
+
+	/* Sample found inconsistent by debouncing or pressure is beyond
+	 * the maximum. Don't report it to user space, repeat at least
+	 * once more the measurement
+	 */
+	if (rt > MAX_12BIT) {
+		dev_dbg(&ts->client->dev, "ignored pressure %d\n", rt);
+
+		hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_PERIOD),
+			      HRTIMER_MODE_REL);
+		return;
+	}
+
+	/* NOTE: We can't rely on the pressure to determine the pen down
+	 * state, even this controller has a pressure sensor.  The pressure
+	 * value can fluctuate for quite a while after lifting the pen and
+	 * in some cases may not even settle at the expected value.
+	 *
+	 * The only safe way to check for the pen up condition is in the
+	 * timer by reading the pen signal state (it's a GPIO _and_ IRQ).
+	 */
+	if (rt) {
+		struct input_dev *input = ts->input;
+
+		if (!ts->pendown) {
+			dev_dbg(&ts->client->dev, "DOWN\n");
+
+			input_report_key(input, BTN_TOUCH, 1);
+			ts->pendown = 1;
+		}
+
+		input_report_abs(input, ABS_X, x);
+		input_report_abs(input, ABS_Y, y);
+		input_report_abs(input, ABS_PRESSURE, rt);
+
+		input_sync(input);
+
+		dev_dbg(&ts->client->dev, "point(%4d,%4d), pressure (%4u)\n",
+			x, y, rt);
+	}
+
+	hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_PERIOD),
+			HRTIMER_MODE_REL);
+}
+
+static int tsc2007_read_values(struct tsc2007 *tsc)
+{
+	/* y- still on; turn on only y+ (and ADC) */
+	tsc->tc.y = tsc2007_xfer(tsc, READ_Y);
+
+	/* turn y- off, x+ on, then leave in lowpower */
+	tsc->tc.x = tsc2007_xfer(tsc, READ_X);
+
+	/* turn y+ off, x- on; we'll use formula #1 */
+	tsc->tc.z1 = tsc2007_xfer(tsc, READ_Z1);
+	tsc->tc.z2 = tsc2007_xfer(tsc, READ_Z2);
+
+	/* power down */
+	tsc2007_xfer(tsc, PWRDOWN);
+
+	return 0;
+}
+
+static enum hrtimer_restart tsc2007_timer(struct hrtimer *handle)
+{
+	struct tsc2007 *ts = container_of(handle, struct tsc2007, timer);
+
+	spin_lock_irq(&ts->lock);
+
+	if (unlikely(!ts->get_pendown_state() && ts->pendown)) {
+		struct input_dev *input = ts->input;
+
+		dev_dbg(&ts->client->dev, "UP\n");
+
+		input_report_key(input, BTN_TOUCH, 0);
+		input_report_abs(input, ABS_PRESSURE, 0);
+		input_sync(input);
+
+		ts->pendown = 0;
+		enable_irq(ts->irq);
+	} else {
+		/* pen is still down, continue with the measurement */
+		dev_dbg(&ts->client->dev, "pen is still down\n");
+
+		tsc2007_read_values(ts);
+		tsc2007_send_event(ts);
+	}
+
+	spin_unlock_irq(&ts->lock);
+
+	return HRTIMER_NORESTART;
+}
+
+static irqreturn_t tsc2007_irq(int irq, void *handle)
+{
+	struct tsc2007 *ts = handle;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ts->lock, flags);
+
+	if (likely(ts->get_pendown_state())) {
+		disable_irq(ts->irq);
+		hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_DELAY),
+					HRTIMER_MODE_REL);
+	}
+
+	if (ts->clear_penirq)
+		ts->clear_penirq();
+
+	spin_unlock_irqrestore(&ts->lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+static int tsc2007_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct tsc2007 *ts;
+	struct tsc2007_platform_data *pdata = pdata = client->dev.platform_data;
+	struct input_dev *input_dev;
+	int err;
+
+	if (!pdata) {
+		dev_err(&client->dev, "platform data is required!\n");
+		return -EINVAL;
+	}
+
+	if (!i2c_check_functionality(client->adapter,
+				     I2C_FUNC_SMBUS_READ_WORD_DATA))
+		return -EIO;
+
+	ts = kzalloc(sizeof(struct tsc2007), GFP_KERNEL);
+	input_dev = input_allocate_device();
+	if (!ts || !input_dev) {
+		err = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	ts->client = client;
+	i2c_set_clientdata(client, ts);
+
+	ts->input = input_dev;
+
+	hrtimer_init(&ts->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	ts->timer.function = tsc2007_timer;
+
+	spin_lock_init(&ts->lock);
+
+	ts->model             = pdata->model;
+	ts->x_plate_ohms      = pdata->x_plate_ohms;
+	ts->get_pendown_state = pdata->get_pendown_state;
+	ts->clear_penirq      = pdata->clear_penirq;
+
+	pdata->init_platform_hw();
+
+	snprintf(ts->phys, sizeof(ts->phys), "%s/input0", client->dev.bus_id);
+
+	input_dev->name = "TSC2007 Touchscreen";
+	input_dev->phys = ts->phys;
+	input_dev->id.bustype = BUS_I2C;
+
+	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+	input_dev->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
+
+	input_set_abs_params(input_dev, ABS_X, 0, MAX_12BIT, 0, 0);
+	input_set_abs_params(input_dev, ABS_Y, 0, MAX_12BIT, 0, 0);
+	input_set_abs_params(input_dev, ABS_PRESSURE, 0, MAX_12BIT, 0, 0);
+
+	tsc2007_read_values(ts);
+
+	ts->irq = client->irq;
+
+	err = request_irq(ts->irq, tsc2007_irq, 0,
+			client->dev.driver->name, ts);
+	if (err < 0) {
+		dev_err(&client->dev, "irq %d busy?\n", ts->irq);
+		goto err_free_mem;
+	}
+
+	err = input_register_device(input_dev);
+	if (err)
+		goto err_free_irq;
+
+	dev_info(&client->dev, "registered with irq (%d)\n", ts->irq);
+
+	return 0;
+
+ err_free_irq:
+	free_irq(ts->irq, ts);
+	hrtimer_cancel(&ts->timer);
+ err_free_mem:
+	input_free_device(input_dev);
+	kfree(ts);
+	return err;
+}
+
+static int tsc2007_remove(struct i2c_client *client)
+{
+	struct tsc2007	*ts = i2c_get_clientdata(client);
+	struct tsc2007_platform_data *pdata;
+
+	pdata = client->dev.platform_data;
+	pdata->exit_platform_hw();
+
+	free_irq(ts->irq, ts);
+	hrtimer_cancel(&ts->timer);
+	input_unregister_device(ts->input);
+	kfree(ts);
+
+	return 0;
+}
+
+static struct i2c_device_id tsc2007_idtable[] = {
+	{ "tsc2007", 0 },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(i2c, tsc2007_idtable);
+
+static struct i2c_driver tsc2007_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "tsc2007"
+	},
+	.id_table	= tsc2007_idtable,
+	.probe		= tsc2007_probe,
+	.remove		= tsc2007_remove,
+};
+
+static int __init tsc2007_init(void)
+{
+	return i2c_add_driver(&tsc2007_driver);
+}
+
+static void __exit tsc2007_exit(void)
+{
+	i2c_del_driver(&tsc2007_driver);
+}
+
+module_init(tsc2007_init);
+module_exit(tsc2007_exit);
+
+MODULE_AUTHOR("Kwangwoo Lee <kwlee@mtekvision.com>");
+MODULE_DESCRIPTION("TSC2007 TouchScreen Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/usbtouchscreen.c b/drivers/input/touchscreen/usbtouchscreen.c
index fdd645c214a2..5080b26ba160 100644
--- a/drivers/input/touchscreen/usbtouchscreen.c
+++ b/drivers/input/touchscreen/usbtouchscreen.c
@@ -424,7 +424,7 @@ static int dmc_tsc10_init(struct usbtouch_usb *usbtouch)
 	                      0, 0, buf, 2, USB_CTRL_SET_TIMEOUT);
 	if (ret < 0)
 		goto err_out;
-	if (buf[0] != 0x06 || buf[1] != 0x00) {
+	if (buf[0] != 0x06) {
 		ret = -ENODEV;
 		goto err_out;
 	}
@@ -437,8 +437,7 @@ static int dmc_tsc10_init(struct usbtouch_usb *usbtouch)
 	                      TSC10_RATE_150, 0, buf, 2, USB_CTRL_SET_TIMEOUT);
 	if (ret < 0)
 		goto err_out;
-	if ((buf[0] != 0x06 || buf[1] != 0x00) &&
-	    (buf[0] != 0x15 || buf[1] != 0x01)) {
+	if ((buf[0] != 0x06) && (buf[0] != 0x15 || buf[1] != 0x01)) {
 		ret = -ENODEV;
 		goto err_out;
 	}
diff --git a/drivers/input/touchscreen/wacom_w8001.c b/drivers/input/touchscreen/wacom_w8001.c
new file mode 100644
index 000000000000..2f33a0167644
--- /dev/null
+++ b/drivers/input/touchscreen/wacom_w8001.c
@@ -0,0 +1,325 @@
+/*
+ * Wacom W8001 penabled serial touchscreen driver
+ *
+ * Copyright (c) 2008 Jaya Kumar
+ *
+ * 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.
+ *
+ * Layout based on Elo serial touchscreen driver by Vojtech Pavlik
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/serio.h>
+#include <linux/init.h>
+#include <linux/ctype.h>
+
+#define DRIVER_DESC	"Wacom W8001 serial touchscreen driver"
+
+MODULE_AUTHOR("Jaya Kumar <jayakumar.lkml@gmail.com>");
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
+
+/*
+ * Definitions & global arrays.
+ */
+
+#define W8001_MAX_LENGTH	11
+#define W8001_PACKET_LEN	11
+#define W8001_LEAD_MASK 0x80
+#define W8001_LEAD_BYTE 0x80
+#define W8001_TAB_MASK 0x40
+#define W8001_TAB_BYTE 0x40
+
+#define W8001_QUERY_PACKET 0x20
+
+struct w8001_coord {
+	u8 rdy;
+	u8 tsw;
+	u8 f1;
+	u8 f2;
+	u16 x;
+	u16 y;
+	u16 pen_pressure;
+	u8 tilt_x;
+	u8 tilt_y;
+};
+
+/*
+ * Per-touchscreen data.
+ */
+
+struct w8001 {
+	struct input_dev *dev;
+	struct serio *serio;
+	struct mutex cmd_mutex;
+	struct completion cmd_done;
+	int id;
+	int idx;
+	unsigned char expected_packet;
+	unsigned char data[W8001_MAX_LENGTH];
+	unsigned char response[W8001_PACKET_LEN];
+	char phys[32];
+};
+
+static int parse_data(u8 *data, struct w8001_coord *coord)
+{
+	coord->rdy = data[0] & 0x20;
+	coord->tsw = data[0] & 0x01;
+	coord->f1 = data[0] & 0x02;
+	coord->f2 = data[0] & 0x04;
+
+	coord->x = (data[1] & 0x7F) << 9;
+	coord->x |= (data[2] & 0x7F) << 2;
+	coord->x |= (data[6] & 0x60) >> 5;
+
+	coord->y = (data[3] & 0x7F) << 9;
+	coord->y |= (data[4] & 0x7F) << 2;
+	coord->y |= (data[6] & 0x18) >> 3;
+
+	coord->pen_pressure = data[5] & 0x7F;
+	coord->pen_pressure |= (data[6] & 0x07) << 7 ;
+
+	coord->tilt_x = data[7] & 0x7F;
+	coord->tilt_y = data[8] & 0x7F;
+
+	return 0;
+}
+
+static void w8001_process_data(struct w8001 *w8001, unsigned char data)
+{
+	struct input_dev *dev = w8001->dev;
+	u8 tmp;
+	struct w8001_coord coord;
+
+	w8001->data[w8001->idx] = data;
+	switch (w8001->idx++) {
+	case 0:
+		if ((data & W8001_LEAD_MASK) != W8001_LEAD_BYTE) {
+			pr_debug("w8001: unsynchronized data: 0x%02x\n", data);
+			w8001->idx = 0;
+		}
+		break;
+	case 8:
+		tmp = w8001->data[0] & W8001_TAB_MASK;
+		if (unlikely(tmp == W8001_TAB_BYTE))
+			break;
+		w8001->idx = 0;
+		memset(&coord, 0, sizeof(coord));
+		parse_data(w8001->data, &coord);
+		input_report_abs(dev, ABS_X, coord.x);
+		input_report_abs(dev, ABS_Y, coord.y);
+		input_report_abs(dev, ABS_PRESSURE, coord.pen_pressure);
+		input_report_key(dev, BTN_TOUCH, coord.tsw);
+		input_sync(dev);
+		break;
+	case 10:
+		w8001->idx = 0;
+		memcpy(w8001->response, &w8001->data, W8001_PACKET_LEN);
+		w8001->expected_packet = W8001_QUERY_PACKET;
+		complete(&w8001->cmd_done);
+		break;
+	}
+}
+
+
+static irqreturn_t w8001_interrupt(struct serio *serio,
+		unsigned char data, unsigned int flags)
+{
+	struct w8001 *w8001 = serio_get_drvdata(serio);
+
+	w8001_process_data(w8001, data);
+
+	return IRQ_HANDLED;
+}
+
+static int w8001_async_command(struct w8001 *w8001, unsigned char *packet,
+					int len)
+{
+	int rc = -1;
+	int i;
+
+	mutex_lock(&w8001->cmd_mutex);
+
+	for (i = 0; i < len; i++) {
+		if (serio_write(w8001->serio, packet[i]))
+			goto out;
+	}
+	rc = 0;
+
+out:
+	mutex_unlock(&w8001->cmd_mutex);
+	return rc;
+}
+
+static int w8001_command(struct w8001 *w8001, unsigned char *packet, int len)
+{
+	int rc = -1;
+	int i;
+
+	mutex_lock(&w8001->cmd_mutex);
+
+	serio_pause_rx(w8001->serio);
+	init_completion(&w8001->cmd_done);
+	serio_continue_rx(w8001->serio);
+
+	for (i = 0; i < len; i++) {
+		if (serio_write(w8001->serio, packet[i]))
+			goto out;
+	}
+
+	wait_for_completion_timeout(&w8001->cmd_done, HZ);
+
+	if (w8001->expected_packet == W8001_QUERY_PACKET) {
+		/* We are back in reporting mode, the query was ACKed */
+		memcpy(packet, w8001->response, W8001_PACKET_LEN);
+		rc = 0;
+	}
+
+out:
+	mutex_unlock(&w8001->cmd_mutex);
+	return rc;
+}
+
+static int w8001_setup(struct w8001 *w8001)
+{
+	struct w8001_coord coord;
+	struct input_dev *dev = w8001->dev;
+	unsigned char start[1] = { '1' };
+	unsigned char query[11] = { '*' };
+
+	if (w8001_command(w8001, query, 1))
+		return -1;
+
+	memset(&coord, 0, sizeof(coord));
+	parse_data(query, &coord);
+
+	input_set_abs_params(dev, ABS_X, 0, coord.x, 0, 0);
+	input_set_abs_params(dev, ABS_Y, 0, coord.y, 0, 0);
+	input_set_abs_params(dev, ABS_PRESSURE, 0, coord.pen_pressure, 0, 0);
+	input_set_abs_params(dev, ABS_TILT_X, 0, coord.tilt_x, 0, 0);
+	input_set_abs_params(dev, ABS_TILT_Y, 0, coord.tilt_y, 0, 0);
+
+	if (w8001_async_command(w8001, start, 1))
+		return -1;
+
+	return 0;
+}
+
+/*
+ * w8001_disconnect() is the opposite of w8001_connect()
+ */
+
+static void w8001_disconnect(struct serio *serio)
+{
+	struct w8001 *w8001 = serio_get_drvdata(serio);
+
+	input_get_device(w8001->dev);
+	input_unregister_device(w8001->dev);
+	serio_close(serio);
+	serio_set_drvdata(serio, NULL);
+	input_put_device(w8001->dev);
+	kfree(w8001);
+}
+
+/*
+ * w8001_connect() is the routine that is called when someone adds a
+ * new serio device that supports the w8001 protocol and registers it as
+ * an input device.
+ */
+
+static int w8001_connect(struct serio *serio, struct serio_driver *drv)
+{
+	struct w8001 *w8001;
+	struct input_dev *input_dev;
+	int err;
+
+	w8001 = kzalloc(sizeof(struct w8001), GFP_KERNEL);
+	input_dev = input_allocate_device();
+	if (!w8001 || !input_dev) {
+		err = -ENOMEM;
+		goto fail1;
+	}
+
+	w8001->serio = serio;
+	w8001->id = serio->id.id;
+	w8001->dev = input_dev;
+	mutex_init(&w8001->cmd_mutex);
+	init_completion(&w8001->cmd_done);
+	snprintf(w8001->phys, sizeof(w8001->phys), "%s/input0", serio->phys);
+
+	input_dev->name = "Wacom W8001 Penabled Serial TouchScreen";
+	input_dev->phys = w8001->phys;
+	input_dev->id.bustype = BUS_RS232;
+	input_dev->id.vendor = SERIO_W8001;
+	input_dev->id.product = w8001->id;
+	input_dev->id.version = 0x0100;
+	input_dev->dev.parent = &serio->dev;
+
+	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+	input_dev->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
+
+	serio_set_drvdata(serio, w8001);
+	err = serio_open(serio, drv);
+	if (err)
+		goto fail2;
+
+	if (w8001_setup(w8001))
+		goto fail3;
+
+	err = input_register_device(w8001->dev);
+	if (err)
+		goto fail3;
+
+	return 0;
+
+fail3:
+	serio_close(serio);
+fail2:
+	serio_set_drvdata(serio, NULL);
+fail1:
+	input_free_device(input_dev);
+	kfree(w8001);
+	return err;
+}
+
+static struct serio_device_id w8001_serio_ids[] = {
+	{
+		.type	= SERIO_RS232,
+		.proto	= SERIO_W8001,
+		.id	= SERIO_ANY,
+		.extra	= SERIO_ANY,
+	},
+	{ 0 }
+};
+
+MODULE_DEVICE_TABLE(serio, w8001_serio_ids);
+
+static struct serio_driver w8001_drv = {
+	.driver		= {
+		.name	= "w8001",
+	},
+	.description	= DRIVER_DESC,
+	.id_table	= w8001_serio_ids,
+	.interrupt	= w8001_interrupt,
+	.connect	= w8001_connect,
+	.disconnect	= w8001_disconnect,
+};
+
+static int __init w8001_init(void)
+{
+	return serio_register_driver(&w8001_drv);
+}
+
+static void __exit w8001_exit(void)
+{
+	serio_unregister_driver(&w8001_drv);
+}
+
+module_init(w8001_init);
+module_exit(w8001_exit);
diff --git a/drivers/isdn/mISDN/dsp_pipeline.c b/drivers/isdn/mISDN/dsp_pipeline.c
index 5ee6651b45b9..83639be7f7ad 100644
--- a/drivers/isdn/mISDN/dsp_pipeline.c
+++ b/drivers/isdn/mISDN/dsp_pipeline.c
@@ -91,7 +91,7 @@ int mISDN_dsp_element_register(struct mISDN_dsp_element *elem)
 
 	entry->dev.class = elements_class;
 	dev_set_drvdata(&entry->dev, elem);
-	snprintf(entry->dev.bus_id, BUS_ID_SIZE, elem->name);
+	dev_set_name(&entry->dev, elem->name);
 	ret = device_register(&entry->dev);
 	if (ret) {
 		printk(KERN_ERR "%s: failed to register %s\n",
diff --git a/drivers/lguest/lguest_device.c b/drivers/lguest/lguest_device.c
index 915da6b8c924..b4d44e571d76 100644
--- a/drivers/lguest/lguest_device.c
+++ b/drivers/lguest/lguest_device.c
@@ -321,10 +321,7 @@ static struct virtio_config_ops lguest_config_ops = {
 
 /* The root device for the lguest virtio devices.  This makes them appear as
  * /sys/devices/lguest/0,1,2 not /sys/devices/0,1,2. */
-static struct device lguest_root = {
-	.parent = NULL,
-	.bus_id = "lguest",
-};
+static struct device *lguest_root;
 
 /*D:120 This is the core of the lguest bus: actually adding a new device.
  * It's a separate function because it's neater that way, and because an
@@ -351,7 +348,7 @@ static void add_lguest_device(struct lguest_device_desc *d,
 	}
 
 	/* This devices' parent is the lguest/ dir. */
-	ldev->vdev.dev.parent = &lguest_root;
+	ldev->vdev.dev.parent = lguest_root;
 	/* We have a unique device index thanks to the dev_index counter. */
 	ldev->vdev.id.device = d->type;
 	/* We have a simple set of routines for querying the device's
@@ -407,7 +404,8 @@ static int __init lguest_devices_init(void)
 	if (strcmp(pv_info.name, "lguest") != 0)
 		return 0;
 
-	if (device_register(&lguest_root) != 0)
+	lguest_root = root_device_register("lguest");
+	if (IS_ERR(lguest_root))
 		panic("Could not register lguest root");
 
 	/* Devices are in a single page above top of "normal" mem */
diff --git a/drivers/macintosh/Kconfig b/drivers/macintosh/Kconfig
index b52659620d50..173cf55c64d0 100644
--- a/drivers/macintosh/Kconfig
+++ b/drivers/macintosh/Kconfig
@@ -138,7 +138,7 @@ config PMAC_BACKLIGHT
 	  Say Y here to enable Macintosh specific extensions of the generic
 	  backlight code. With this enabled, the brightness keys on older
 	  PowerBooks will be enabled so you can change the screen brightness.
-	  Newer models should use an userspace daemon like pbbuttonsd.
+	  Newer models should use a userspace daemon like pbbuttonsd.
 
 config PMAC_BACKLIGHT_LEGACY
 	bool "Provide legacy ioctl's on /dev/pmu for the backlight"
diff --git a/drivers/macintosh/macio_asic.c b/drivers/macintosh/macio_asic.c
index ec9e5f32f0ae..6e149f4a1fff 100644
--- a/drivers/macintosh/macio_asic.c
+++ b/drivers/macintosh/macio_asic.c
@@ -33,7 +33,7 @@
 
 #undef DEBUG
 
-#define MAX_NODE_NAME_SIZE (BUS_ID_SIZE - 12)
+#define MAX_NODE_NAME_SIZE (20 - 12)
 
 static struct macio_chip      *macio_on_hold;
 
@@ -240,7 +240,7 @@ static void macio_create_fixup_irq(struct macio_dev *dev, int index,
 	if (irq != NO_IRQ) {
 		dev->interrupt[index].start = irq;
 		dev->interrupt[index].flags = IORESOURCE_IRQ;
-		dev->interrupt[index].name = dev->ofdev.dev.bus_id;
+		dev->interrupt[index].name = dev_name(&dev->ofdev.dev);
 	}
 	if (dev->n_interrupts <= index)
 		dev->n_interrupts = index + 1;
@@ -303,7 +303,7 @@ static void macio_setup_interrupts(struct macio_dev *dev)
 			break;
 		res->start = irq;
 		res->flags = IORESOURCE_IRQ;
-		res->name = dev->ofdev.dev.bus_id;
+		res->name = dev_name(&dev->ofdev.dev);
 		if (macio_resource_quirks(np, res, i - 1)) {
 			memset(res, 0, sizeof(struct resource));
 			continue;
@@ -325,7 +325,7 @@ static void macio_setup_resources(struct macio_dev *dev,
 		if (index >= MACIO_DEV_COUNT_RESOURCES)
 			break;
 		*res = r;
-		res->name = dev->ofdev.dev.bus_id;
+		res->name = dev_name(&dev->ofdev.dev);
 
 		if (macio_resource_quirks(np, res, index)) {
 			memset(res, 0, sizeof(struct resource));
@@ -338,7 +338,7 @@ static void macio_setup_resources(struct macio_dev *dev,
 		if (insert_resource(parent_res, res)) {
 			printk(KERN_WARNING "Can't request resource "
 			       "%d for MacIO device %s\n",
-			       index, dev->ofdev.dev.bus_id);
+			       index, dev_name(&dev->ofdev.dev));
 		}
 	}
 	dev->n_resources = index;
@@ -385,8 +385,8 @@ static struct macio_dev * macio_add_one_device(struct macio_chip *chip,
 
 	/* MacIO itself has a different reg, we use it's PCI base */
 	if (np == chip->of_node) {
-		sprintf(dev->ofdev.dev.bus_id, "%1d.%08x:%.*s",
-			chip->lbus.index,
+		dev_set_name(&dev->ofdev.dev, "%1d.%08x:%.*s",
+			     chip->lbus.index,
 #ifdef CONFIG_PCI
 			(unsigned int)pci_resource_start(chip->lbus.pdev, 0),
 #else
@@ -395,9 +395,9 @@ static struct macio_dev * macio_add_one_device(struct macio_chip *chip,
 			MAX_NODE_NAME_SIZE, np->name);
 	} else {
 		reg = of_get_property(np, "reg", NULL);
-		sprintf(dev->ofdev.dev.bus_id, "%1d.%08x:%.*s",
-			chip->lbus.index,
-			reg ? *reg : 0, MAX_NODE_NAME_SIZE, np->name);
+		dev_set_name(&dev->ofdev.dev, "%1d.%08x:%.*s",
+			     chip->lbus.index,
+			     reg ? *reg : 0, MAX_NODE_NAME_SIZE, np->name);
 	}
 
 	/* Setup interrupts & resources */
@@ -408,7 +408,7 @@ static struct macio_dev * macio_add_one_device(struct macio_chip *chip,
 	/* Register with core */
 	if (of_device_register(&dev->ofdev) != 0) {
 		printk(KERN_DEBUG"macio: device registration error for %s!\n",
-		       dev->ofdev.dev.bus_id);
+		       dev_name(&dev->ofdev.dev));
 		kfree(dev);
 		return NULL;
 	}
@@ -558,7 +558,7 @@ err_out:
 		resource_no,
 		macio_resource_len(dev, resource_no),
 		macio_resource_start(dev, resource_no),
-		dev->ofdev.dev.bus_id);
+		dev_name(&dev->ofdev.dev));
 	return -EBUSY;
 }
 
diff --git a/drivers/macintosh/therm_adt746x.c b/drivers/macintosh/therm_adt746x.c
index 22bf981d393b..82607add69a9 100644
--- a/drivers/macintosh/therm_adt746x.c
+++ b/drivers/macintosh/therm_adt746x.c
@@ -554,7 +554,7 @@ thermostat_init(void)
 	const u32 *prop;
 	int i = 0, offset = 0;
 	int err;
-	
+
 	np = of_find_node_by_name(NULL, "fan");
 	if (!np)
 		return -ENODEV;
@@ -613,13 +613,13 @@ thermostat_init(void)
 	}
 
 	of_dev = of_platform_device_create(np, "temperatures", NULL);
-	
+	of_node_put(np);
+
 	if (of_dev == NULL) {
 		printk(KERN_ERR "Can't register temperatures device !\n");
-		of_node_put(np);
 		return -ENODEV;
 	}
-	
+
 	err = device_create_file(&of_dev->dev, &dev_attr_sensor1_temperature);
 	err |= device_create_file(&of_dev->dev, &dev_attr_sensor2_temperature);
 	err |= device_create_file(&of_dev->dev, &dev_attr_sensor1_limit);
diff --git a/drivers/media/common/tuners/tda8290.c b/drivers/media/common/tuners/tda8290.c
index 0ee79fd7c7a9..4b8662edb7cb 100644
--- a/drivers/media/common/tuners/tda8290.c
+++ b/drivers/media/common/tuners/tda8290.c
@@ -150,7 +150,7 @@ static void set_audio(struct dvb_frontend *fe,
 	}
 }
 
-struct {
+static struct {
 	unsigned char seq[2];
 } fm_mode[] = {
 	{ { 0x01, 0x81} },	/* Put device into expert mode */
@@ -207,7 +207,6 @@ static void tda8290_set_params(struct dvb_frontend *fe,
 	msleep(1);
 
 	if (params->mode == V4L2_TUNER_RADIO) {
-		int i;
 		unsigned char deemphasis[]  = { 0x13, 1 };
 
 		/* FIXME: allow using a different deemphasis */
@@ -767,7 +766,8 @@ struct dvb_frontend *tda829x_attach(struct dvb_frontend *fe,
 	fe->ops.analog_ops.info.name = name;
 
 	if (priv->ver & TDA8290) {
-		tda8290_init_tuner(fe);
+		if (priv->ver & (TDA8275 | TDA8275A))
+			tda8290_init_tuner(fe);
 		tda8290_init_if(fe);
 	} else if (priv->ver & TDA8295)
 		tda8295_init_if(fe);
diff --git a/drivers/media/dvb/dm1105/Kconfig b/drivers/media/dvb/dm1105/Kconfig
index 1332301ef3ae..43f4d44edca6 100644
--- a/drivers/media/dvb/dm1105/Kconfig
+++ b/drivers/media/dvb/dm1105/Kconfig
@@ -1,6 +1,7 @@
 config DVB_DM1105
 	tristate "SDMC DM1105 based PCI cards"
 	depends on DVB_CORE && PCI && I2C
+	depends on INPUT
 	select DVB_PLL if !DVB_FE_CUSTOMISE
 	select DVB_STV0299 if !DVB_FE_CUSTOMISE
 	select DVB_STV0288 if !DVB_FE_CUSTOMISE
diff --git a/drivers/media/dvb/dvb-core/dvb_frontend.c b/drivers/media/dvb/dvb-core/dvb_frontend.c
index 171f9ca124f7..843407785083 100644
--- a/drivers/media/dvb/dvb-core/dvb_frontend.c
+++ b/drivers/media/dvb/dvb-core/dvb_frontend.c
@@ -824,7 +824,7 @@ static int dvb_frontend_check_parameters(struct dvb_frontend *fe,
 	return 0;
 }
 
-struct dtv_cmds_h dtv_cmds[] = {
+static struct dtv_cmds_h dtv_cmds[] = {
 	[DTV_TUNE] = {
 		.name	= "DTV_TUNE",
 		.cmd	= DTV_TUNE,
@@ -962,7 +962,7 @@ struct dtv_cmds_h dtv_cmds[] = {
 	},
 };
 
-void dtv_property_dump(struct dtv_property *tvp)
+static void dtv_property_dump(struct dtv_property *tvp)
 {
 	int i;
 
@@ -993,7 +993,7 @@ void dtv_property_dump(struct dtv_property *tvp)
 		dprintk("%s() tvp.u.data = 0x%08x\n", __func__, tvp->u.data);
 }
 
-int is_legacy_delivery_system(fe_delivery_system_t s)
+static int is_legacy_delivery_system(fe_delivery_system_t s)
 {
 	if((s == SYS_UNDEFINED) || (s == SYS_DVBC_ANNEX_AC) ||
 	   (s == SYS_DVBC_ANNEX_B) || (s == SYS_DVBT) || (s == SYS_DVBS) ||
@@ -1007,7 +1007,8 @@ int is_legacy_delivery_system(fe_delivery_system_t s)
  * drivers can use a single set_frontend tuning function, regardless of whether
  * it's being used for the legacy or new API, reducing code and complexity.
  */
-void dtv_property_cache_sync(struct dvb_frontend *fe, struct dvb_frontend_parameters *p)
+static void dtv_property_cache_sync(struct dvb_frontend *fe,
+				    struct dvb_frontend_parameters *p)
 {
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 
@@ -1059,7 +1060,7 @@ void dtv_property_cache_sync(struct dvb_frontend *fe, struct dvb_frontend_parame
 /* Ensure the cached values are set correctly in the frontend
  * legacy tuning structures, for the advanced tuning API.
  */
-void dtv_property_legacy_params_sync(struct dvb_frontend *fe)
+static void dtv_property_legacy_params_sync(struct dvb_frontend *fe)
 {
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct dvb_frontend_private *fepriv = fe->frontend_priv;
@@ -1114,7 +1115,7 @@ void dtv_property_legacy_params_sync(struct dvb_frontend *fe)
 /* Ensure the cached values are set correctly in the frontend
  * legacy tuning structures, for the legacy tuning API.
  */
-void dtv_property_adv_params_sync(struct dvb_frontend *fe)
+static void dtv_property_adv_params_sync(struct dvb_frontend *fe)
 {
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct dvb_frontend_private *fepriv = fe->frontend_priv;
@@ -1149,7 +1150,7 @@ void dtv_property_adv_params_sync(struct dvb_frontend *fe)
 	}
 }
 
-void dtv_property_cache_submit(struct dvb_frontend *fe)
+static void dtv_property_cache_submit(struct dvb_frontend *fe)
 {
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 
@@ -1180,8 +1181,9 @@ static int dvb_frontend_ioctl_legacy(struct inode *inode, struct file *file,
 static int dvb_frontend_ioctl_properties(struct inode *inode, struct file *file,
 			unsigned int cmd, void *parg);
 
-int dtv_property_process_get(struct dvb_frontend *fe, struct dtv_property *tvp,
-	struct inode *inode, struct file *file)
+static int dtv_property_process_get(struct dvb_frontend *fe,
+				    struct dtv_property *tvp,
+				    struct inode *inode, struct file *file)
 {
 	int r = 0;
 
@@ -1253,8 +1255,10 @@ int dtv_property_process_get(struct dvb_frontend *fe, struct dtv_property *tvp,
 	return r;
 }
 
-int dtv_property_process_set(struct dvb_frontend *fe, struct dtv_property *tvp,
-	struct inode *inode, struct file *file)
+static int dtv_property_process_set(struct dvb_frontend *fe,
+				    struct dtv_property *tvp,
+				    struct inode *inode,
+				    struct file *file)
 {
 	int r = 0;
 	struct dvb_frontend_private *fepriv = fe->frontend_priv;
diff --git a/drivers/media/dvb/dvb-core/dvbdev.c b/drivers/media/dvb/dvb-core/dvbdev.c
index 65d69665f1fc..6a32680dbb1b 100644
--- a/drivers/media/dvb/dvb-core/dvbdev.c
+++ b/drivers/media/dvb/dvb-core/dvbdev.c
@@ -79,6 +79,10 @@ static int dvb_device_open(struct inode *inode, struct file *file)
 		file->private_data = dvbdev;
 		old_fops = file->f_op;
 		file->f_op = fops_get(dvbdev->fops);
+		if (file->f_op == NULL) {
+			file->f_op = old_fops;
+			goto fail;
+		}
 		if(file->f_op->open)
 			err = file->f_op->open(inode,file);
 		if (err) {
@@ -90,6 +94,7 @@ static int dvb_device_open(struct inode *inode, struct file *file)
 		unlock_kernel();
 		return err;
 	}
+fail:
 	up_read(&minor_rwsem);
 	unlock_kernel();
 	return -ENODEV;
diff --git a/drivers/media/dvb/dvb-usb/anysee.c b/drivers/media/dvb/dvb-usb/anysee.c
index 5017f08b14a6..c6e7b4215d6b 100644
--- a/drivers/media/dvb/dvb-usb/anysee.c
+++ b/drivers/media/dvb/dvb-usb/anysee.c
@@ -41,7 +41,7 @@
 static int dvb_usb_anysee_debug;
 module_param_named(debug, dvb_usb_anysee_debug, int, 0644);
 MODULE_PARM_DESC(debug, "set debugging level" DVB_USB_DEBUG_STATUS);
-int dvb_usb_anysee_delsys;
+static int dvb_usb_anysee_delsys;
 module_param_named(delsys, dvb_usb_anysee_delsys, int, 0644);
 MODULE_PARM_DESC(delsys, "select delivery mode (0=DVB-C, 1=DVB-T)");
 DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
diff --git a/drivers/media/dvb/frontends/cx24116.c b/drivers/media/dvb/frontends/cx24116.c
index 4f514d39b98f..28ad609e73f4 100644
--- a/drivers/media/dvb/frontends/cx24116.c
+++ b/drivers/media/dvb/frontends/cx24116.c
@@ -369,7 +369,7 @@ static int cx24116_set_inversion(struct cx24116_state *state,
  * Not all S2 mmodulation schemes are support and not all rates with
  * a scheme are support. Especially, no auto detect when in S2 mode.
  */
-struct cx24116_modfec {
+static struct cx24116_modfec {
 	fe_delivery_system_t delivery_system;
 	fe_modulation_t modulation;
 	fe_code_rate_t fec;
diff --git a/drivers/media/dvb/frontends/lgdt3304.c b/drivers/media/dvb/frontends/lgdt3304.c
index 469ace5692c6..3bb0c4394f8a 100644
--- a/drivers/media/dvb/frontends/lgdt3304.c
+++ b/drivers/media/dvb/frontends/lgdt3304.c
@@ -42,7 +42,7 @@ static int i2c_write_demod_bytes (struct dvb_frontend *fe, __u8 *buf, int len)
 
 	for (i=0; i<len-1; i+=3){
 		if((err = i2c_transfer(state->i2c, &i2cmsgs, 1))<0) {
-			printk("%s i2c_transfer error %d\n", __FUNCTION__, err);
+			printk("%s i2c_transfer error %d\n", __func__, err);
 			if (err < 0)
 				return err;
 			else
@@ -73,7 +73,7 @@ static int lgdt3304_i2c_read_reg(struct dvb_frontend *fe, unsigned int reg)
 	i2cmsgs[1].buf = &buf;
 
 	if((ret = i2c_transfer(state->i2c, i2cmsgs, 2))<0) {
-		printk("%s i2c_transfer error %d\n", __FUNCTION__, ret);
+		printk("%s i2c_transfer error %d\n", __func__, ret);
 		return ret;
 	}
 
@@ -94,7 +94,7 @@ static int lgdt3304_i2c_write_reg(struct dvb_frontend *fe, int reg, int val)
 	};
 	ret = i2c_transfer(state->i2c, &i2cmsgs, 1);
 	if (ret != 1) {
-		printk("%s i2c_transfer error %d\n", __FUNCTION__, ret);
+		printk("%s i2c_transfer error %d\n", __func__, ret);
 		return ret;
 	}
 
@@ -238,7 +238,7 @@ static int lgdt3304_set_parameters(struct dvb_frontend *fe, struct dvb_frontend_
 		}
 
 		if (err) {
-			printk("%s error setting modulation\n", __FUNCTION__);
+			printk("%s error setting modulation\n", __func__);
 		} else {
 			state->current_modulation = param->u.vsb.modulation;
 		}
@@ -305,7 +305,7 @@ static int lgdt3304_read_status(struct dvb_frontend *fe, fe_status_t *status)
 		}
 		break;
 	default:
-		printk("%s unhandled modulation\n", __FUNCTION__);
+		printk("%s unhandled modulation\n", __func__);
 	}
 
 
diff --git a/drivers/media/dvb/frontends/s921_module.c b/drivers/media/dvb/frontends/s921_module.c
index 3cbb9cb2cf47..892af8c9ed57 100644
--- a/drivers/media/dvb/frontends/s921_module.c
+++ b/drivers/media/dvb/frontends/s921_module.c
@@ -136,7 +136,7 @@ static int s921_write(void *dev, u8 reg, u8 val) {
 	};
 
 	if((err = i2c_transfer(state->i2c, &i2cmsgs, 1))<0) {
-		printk("%s i2c_transfer error %d\n", __FUNCTION__, err);
+		printk("%s i2c_transfer error %d\n", __func__, err);
 		if (err < 0)
 			return err;
 		else
diff --git a/drivers/media/dvb/frontends/stb0899_algo.c b/drivers/media/dvb/frontends/stb0899_algo.c
index ced9b7ae7d50..83dc7e12d5f0 100644
--- a/drivers/media/dvb/frontends/stb0899_algo.c
+++ b/drivers/media/dvb/frontends/stb0899_algo.c
@@ -54,7 +54,7 @@ static u32 stb0899_calc_srate(u32 master_clk, u8 *sfr)
  * stb0899_get_srate
  * Get the current symbol rate
  */
-u32 stb0899_get_srate(struct stb0899_state *state)
+static u32 stb0899_get_srate(struct stb0899_state *state)
 {
 	struct stb0899_internal *internal = &state->internal;
 	u8 sfr[3];
@@ -763,7 +763,7 @@ static void stb0899_dvbs2_config_csm_auto(struct stb0899_state *state)
 	stb0899_write_s2reg(state, STB0899_S2DEMOD, STB0899_BASE_CSM_CNTRL1, STB0899_OFF0_CSM_CNTRL1, reg);
 }
 
-long Log2Int(int number)
+static long Log2Int(int number)
 {
 	int i;
 
diff --git a/drivers/media/dvb/frontends/stb0899_drv.c b/drivers/media/dvb/frontends/stb0899_drv.c
index bee28f77b93f..10613acf18f5 100644
--- a/drivers/media/dvb/frontends/stb0899_drv.c
+++ b/drivers/media/dvb/frontends/stb0899_drv.c
@@ -134,7 +134,7 @@ static const struct stb0899_tab stb0899_dvbs2rf_tab[] = {
 };
 
 /* DVB-S2 Es/N0 quant in dB/100 vs read value * 100*/
-struct stb0899_tab stb0899_quant_tab[] = {
+static struct stb0899_tab stb0899_quant_tab[] = {
 	{    0,	    0 },
 	{    0,	  100 },
 	{  600,	  200 },
@@ -177,7 +177,7 @@ struct stb0899_tab stb0899_quant_tab[] = {
 };
 
 /* DVB-S2 Es/N0 estimate in dB/100 vs read value */
-struct stb0899_tab stb0899_est_tab[] = {
+static struct stb0899_tab stb0899_est_tab[] = {
 	{    0,	     0 },
 	{    0,	     1 },
 	{  301,	     2 },
@@ -217,7 +217,7 @@ struct stb0899_tab stb0899_est_tab[] = {
 	{ 5721,	526017 },
 };
 
-int _stb0899_read_reg(struct stb0899_state *state, unsigned int reg)
+static int _stb0899_read_reg(struct stb0899_state *state, unsigned int reg)
 {
 	int ret;
 
diff --git a/drivers/media/dvb/ttpci/budget-ci.c b/drivers/media/dvb/ttpci/budget-ci.c
index 3507463fdac9..bcbc5d41a0fe 100644
--- a/drivers/media/dvb/ttpci/budget-ci.c
+++ b/drivers/media/dvb/ttpci/budget-ci.c
@@ -1337,7 +1337,7 @@ static struct stb0899_config tt3200_config = {
 	.tuner_set_rfsiggain	= NULL
 };
 
-struct stb6100_config tt3200_stb6100_config = {
+static struct stb6100_config tt3200_stb6100_config = {
 	.tuner_address	= 0x60,
 	.refclock	= 27000000,
 };
@@ -1450,7 +1450,7 @@ static void frontend_init(struct budget_ci *budget_ci)
 		if (budget_ci->budget.dvb_frontend) {
 			if (dvb_attach(stb6100_attach, budget_ci->budget.dvb_frontend, &tt3200_stb6100_config, &budget_ci->budget.i2c_adap)) {
 				if (!dvb_attach(lnbp21_attach, budget_ci->budget.dvb_frontend, &budget_ci->budget.i2c_adap, 0, 0)) {
-					printk("%s: No LNBP21 found!\n", __FUNCTION__);
+					printk("%s: No LNBP21 found!\n", __func__);
 					dvb_frontend_detach(budget_ci->budget.dvb_frontend);
 					budget_ci->budget.dvb_frontend = NULL;
 				}
diff --git a/drivers/media/video/cafe_ccic.c b/drivers/media/video/cafe_ccic.c
index 34a39d2e4703..46fd573a4f15 100644
--- a/drivers/media/video/cafe_ccic.c
+++ b/drivers/media/video/cafe_ccic.c
@@ -569,7 +569,6 @@ static int cafe_smbus_setup(struct cafe_camera *cam)
 
 	cafe_smbus_enable_irq(cam);
 	adap->id = I2C_HW_SMBUS_CAFE;
-	adap->class = I2C_CLASS_CAM_DIGITAL;
 	adap->owner = THIS_MODULE;
 	adap->client_register = cafe_smbus_attach;
 	adap->client_unregister = cafe_smbus_detach;
diff --git a/drivers/media/video/cx88/Kconfig b/drivers/media/video/cx88/Kconfig
index b0f837588e01..2d250a2a7bc3 100644
--- a/drivers/media/video/cx88/Kconfig
+++ b/drivers/media/video/cx88/Kconfig
@@ -69,6 +69,11 @@ config VIDEO_CX88_DVB
 	  To compile this driver as a module, choose M here: the
 	  module will be called cx88-dvb.
 
+config VIDEO_CX88_MPEG
+	tristate
+	depends on VIDEO_CX88_DVB || VIDEO_CX88_BLACKBIRD
+	default y
+
 config VIDEO_CX88_VP3054
 	tristate "VP-3054 Secondary I2C Bus Support"
 	default m
diff --git a/drivers/media/video/cx88/Makefile b/drivers/media/video/cx88/Makefile
index 6ec30f242578..b06b1275a9ec 100644
--- a/drivers/media/video/cx88/Makefile
+++ b/drivers/media/video/cx88/Makefile
@@ -3,7 +3,8 @@ cx88xx-objs	:= cx88-cards.o cx88-core.o cx88-i2c.o cx88-tvaudio.o \
 cx8800-objs	:= cx88-video.o cx88-vbi.o
 cx8802-objs	:= cx88-mpeg.o
 
-obj-$(CONFIG_VIDEO_CX88) += cx88xx.o cx8800.o cx8802.o
+obj-$(CONFIG_VIDEO_CX88) += cx88xx.o cx8800.o
+obj-$(CONFIG_VIDEO_CX88_MPEG) += cx8802.o
 obj-$(CONFIG_VIDEO_CX88_ALSA) += cx88-alsa.o
 obj-$(CONFIG_VIDEO_CX88_BLACKBIRD) += cx88-blackbird.o
 obj-$(CONFIG_VIDEO_CX88_DVB) += cx88-dvb.o
diff --git a/drivers/media/video/cx88/cx88-dvb.c b/drivers/media/video/cx88/cx88-dvb.c
index da4dd4913d9f..613dfea4ff3e 100644
--- a/drivers/media/video/cx88/cx88-dvb.c
+++ b/drivers/media/video/cx88/cx88-dvb.c
@@ -138,6 +138,28 @@ static int cx88_dvb_bus_ctrl(struct dvb_frontend* fe, int acquire)
 	return ret;
 }
 
+static void cx88_dvb_gate_ctrl(struct cx88_core  *core, int open)
+{
+	struct videobuf_dvb_frontends *f;
+	struct videobuf_dvb_frontend *fe;
+
+	if (!core->dvbdev)
+		return;
+
+	f = &core->dvbdev->frontends;
+
+	if (!f)
+		return;
+
+	if (f->gate <= 1) /* undefined or fe0 */
+		fe = videobuf_dvb_get_frontend(f, 1);
+	else
+		fe = videobuf_dvb_get_frontend(f, f->gate);
+
+	if (fe && fe->dvb.frontend && fe->dvb.frontend->ops.i2c_gate_ctrl)
+		fe->dvb.frontend->ops.i2c_gate_ctrl(fe->dvb.frontend, open);
+}
+
 /* ------------------------------------------------------------------ */
 
 static int dvico_fusionhdtv_demod_init(struct dvb_frontend* fe)
@@ -597,12 +619,30 @@ static int dvb_register(struct cx8802_dev *dev)
 	struct cx88_core *core = dev->core;
 	struct videobuf_dvb_frontend *fe0, *fe1 = NULL;
 	int mfe_shared = 0; /* bus not shared by default */
+	int i;
 
 	if (0 != core->i2c_rc) {
 		printk(KERN_ERR "%s/2: no i2c-bus available, cannot attach dvb drivers\n", core->name);
 		goto frontend_detach;
 	}
 
+	if (!core->board.num_frontends)
+		return -EINVAL;
+
+	mutex_init(&dev->frontends.lock);
+	INIT_LIST_HEAD(&dev->frontends.felist);
+
+	printk(KERN_INFO "%s() allocating %d frontend(s)\n", __func__,
+			 core->board.num_frontends);
+	for (i = 1; i <= core->board.num_frontends; i++) {
+		fe0 = videobuf_dvb_alloc_frontend(&dev->frontends, i);
+		if (!fe0) {
+			printk(KERN_ERR "%s() failed to alloc\n", __func__);
+			videobuf_dvb_dealloc_frontends(&dev->frontends);
+			goto frontend_detach;
+		}
+	}
+
 	/* Get the first frontend */
 	fe0 = videobuf_dvb_get_frontend(&dev->frontends, 1);
 	if (!fe0)
@@ -611,6 +651,9 @@ static int dvb_register(struct cx8802_dev *dev)
 	/* multi-frontend gate control is undefined or defaults to fe0 */
 	dev->frontends.gate = 0;
 
+	/* Sets the gate control callback to be used by i2c command calls */
+	core->gate_ctrl = cx88_dvb_gate_ctrl;
+
 	/* init frontend(s) */
 	switch (core->boardnr) {
 	case CX88_BOARD_HAUPPAUGE_DVB_T1:
@@ -1109,6 +1152,7 @@ static int dvb_register(struct cx8802_dev *dev)
 		&dev->pci->dev, adapter_nr, mfe_shared);
 
 frontend_detach:
+	core->gate_ctrl = NULL;
 	videobuf_dvb_dealloc_frontends(&dev->frontends);
 	return -EINVAL;
 }
@@ -1270,6 +1314,8 @@ static int cx8802_dvb_remove(struct cx8802_driver *drv)
 
 	vp3054_i2c_remove(dev);
 
+	core->gate_ctrl = NULL;
+
 	return 0;
 }
 
diff --git a/drivers/media/video/cx88/cx88-i2c.c b/drivers/media/video/cx88/cx88-i2c.c
index 1ab691d20692..c0ff2305d804 100644
--- a/drivers/media/video/cx88/cx88-i2c.c
+++ b/drivers/media/video/cx88/cx88-i2c.c
@@ -116,30 +116,16 @@ static int detach_inform(struct i2c_client *client)
 
 void cx88_call_i2c_clients(struct cx88_core *core, unsigned int cmd, void *arg)
 {
-#if defined(CONFIG_VIDEO_CX88_DVB) || defined(CONFIG_VIDEO_CX88_DVB_MODULE)
-	struct videobuf_dvb_frontends *f = &core->dvbdev->frontends;
-	struct videobuf_dvb_frontend *fe = NULL;
-#endif
 	if (0 != core->i2c_rc)
 		return;
 
-#if defined(CONFIG_VIDEO_CX88_DVB) || defined(CONFIG_VIDEO_CX88_DVB_MODULE)
-	if (core->dvbdev && f) {
-		if(f->gate <= 1) /* undefined or fe0 */
-			fe = videobuf_dvb_get_frontend(f, 1);
-		else
-			fe = videobuf_dvb_get_frontend(f, f->gate);
+	if (core->gate_ctrl)
+		core->gate_ctrl(core, 1);
 
-		if (fe && fe->dvb.frontend && fe->dvb.frontend->ops.i2c_gate_ctrl)
-			fe->dvb.frontend->ops.i2c_gate_ctrl(fe->dvb.frontend, 1);
+	i2c_clients_command(&core->i2c_adap, cmd, arg);
 
-		i2c_clients_command(&core->i2c_adap, cmd, arg);
-
-		if (fe && fe->dvb.frontend && fe->dvb.frontend->ops.i2c_gate_ctrl)
-			fe->dvb.frontend->ops.i2c_gate_ctrl(fe->dvb.frontend, 0);
-	} else
-#endif
-		i2c_clients_command(&core->i2c_adap, cmd, arg);
+	if (core->gate_ctrl)
+		core->gate_ctrl(core, 0);
 }
 
 static const struct i2c_algo_bit_data cx8800_i2c_algo_template = {
diff --git a/drivers/media/video/cx88/cx88-mpeg.c b/drivers/media/video/cx88/cx88-mpeg.c
index 59164fc94f5f..b295b76737e3 100644
--- a/drivers/media/video/cx88/cx88-mpeg.c
+++ b/drivers/media/video/cx88/cx88-mpeg.c
@@ -787,6 +787,9 @@ static int __devinit cx8802_probe(struct pci_dev *pci_dev,
 	dev->pci = pci_dev;
 	dev->core = core;
 
+	/* Maintain a reference so cx88-video can query the 8802 device. */
+	core->dvbdev = dev;
+
 	err = cx8802_init_common(dev);
 	if (err != 0)
 		goto fail_free;
@@ -794,32 +797,6 @@ static int __devinit cx8802_probe(struct pci_dev *pci_dev,
 	INIT_LIST_HEAD(&dev->drvlist);
 	list_add_tail(&dev->devlist,&cx8802_devlist);
 
-#if defined(CONFIG_VIDEO_CX88_DVB) || defined(CONFIG_VIDEO_CX88_DVB_MODULE)
-	mutex_init(&dev->frontends.lock);
-	INIT_LIST_HEAD(&dev->frontends.felist);
-
-	if (core->board.num_frontends) {
-		struct videobuf_dvb_frontend *fe;
-		int i;
-
-		printk(KERN_INFO "%s() allocating %d frontend(s)\n", __func__,
-			core->board.num_frontends);
-		for (i = 1; i <= core->board.num_frontends; i++) {
-			fe = videobuf_dvb_alloc_frontend(&dev->frontends, i);
-			if(fe == NULL) {
-				printk(KERN_ERR "%s() failed to alloc\n",
-					__func__);
-				videobuf_dvb_dealloc_frontends(&dev->frontends);
-				err = -ENOMEM;
-				goto fail_free;
-			}
-		}
-	}
-#endif
-
-	/* Maintain a reference so cx88-video can query the 8802 device. */
-	core->dvbdev = dev;
-
 	/* now autoload cx88-dvb or cx88-blackbird */
 	request_modules(dev);
 	return 0;
@@ -827,6 +804,7 @@ static int __devinit cx8802_probe(struct pci_dev *pci_dev,
  fail_free:
 	kfree(dev);
  fail_core:
+	core->dvbdev = NULL;
 	cx88_core_put(core,pci_dev);
 	return err;
 }
diff --git a/drivers/media/video/cx88/cx88.h b/drivers/media/video/cx88/cx88.h
index eb9ce30dc5e6..60a8b3187f14 100644
--- a/drivers/media/video/cx88/cx88.h
+++ b/drivers/media/video/cx88/cx88.h
@@ -302,6 +302,7 @@ struct cx88_dmaqueue {
 	struct btcx_riscmem    stopper;
 	u32                    count;
 };
+struct cx88_core;
 
 struct cx88_core {
 	struct list_head           devlist;
@@ -334,7 +335,8 @@ struct cx88_core {
 
 	/* config info -- dvb */
 #if defined(CONFIG_VIDEO_CX88_DVB) || defined(CONFIG_VIDEO_CX88_DVB_MODULE)
-	int 			   (*prev_set_voltage)(struct dvb_frontend* fe, fe_sec_voltage_t voltage);
+	int 			   (*prev_set_voltage)(struct dvb_frontend *fe, fe_sec_voltage_t voltage);
+	void			   (*gate_ctrl)(struct cx88_core  *core, int open);
 #endif
 
 	/* state info */
diff --git a/drivers/media/video/em28xx/em28xx-cards.c b/drivers/media/video/em28xx/em28xx-cards.c
index e776699b62f9..ef9bf008a924 100644
--- a/drivers/media/video/em28xx/em28xx-cards.c
+++ b/drivers/media/video/em28xx/em28xx-cards.c
@@ -1842,7 +1842,7 @@ void em28xx_release_resources(struct em28xx *dev)
  * em28xx_init_dev()
  * allocates and inits the device structs, registers i2c bus and v4l device
  */
-int em28xx_init_dev(struct em28xx **devhandle, struct usb_device *udev,
+static int em28xx_init_dev(struct em28xx **devhandle, struct usb_device *udev,
 			   int minor)
 {
 	struct em28xx *dev = *devhandle;
@@ -1990,8 +1990,7 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 		int check_interface = 1;
 		isoc_pipe = 1;
 		endpoint = &interface->cur_altsetting->endpoint[1].desc;
-		if (usb_endpoint_type(endpoint) !=
-		    USB_ENDPOINT_XFER_ISOC)
+		if (!usb_endpoint_xfer_isoc(endpoint))
 			check_interface = 0;
 
 		if (usb_endpoint_dir_out(endpoint))
diff --git a/drivers/media/video/em28xx/em28xx-core.c b/drivers/media/video/em28xx/em28xx-core.c
index 819cceaa6ef4..eb5fb05fab22 100644
--- a/drivers/media/video/em28xx/em28xx-core.c
+++ b/drivers/media/video/em28xx/em28xx-core.c
@@ -393,7 +393,7 @@ static int em28xx_set_audio_source(struct em28xx *dev)
 	return ret;
 }
 
-struct em28xx_vol_table outputs[] = {
+static const struct em28xx_vol_table outputs[] = {
 	{ EM28XX_AOUT_MASTER, AC97_MASTER_VOL      },
 	{ EM28XX_AOUT_LINE,   AC97_LINE_LEVEL_VOL  },
 	{ EM28XX_AOUT_MONO,   AC97_MASTER_MONO_VOL },
diff --git a/drivers/media/video/em28xx/em28xx-input.c b/drivers/media/video/em28xx/em28xx-input.c
index 42bbaf64aceb..0443afe09ff8 100644
--- a/drivers/media/video/em28xx/em28xx-input.c
+++ b/drivers/media/video/em28xx/em28xx-input.c
@@ -307,7 +307,7 @@ static void em28xx_ir_work(struct work_struct *work)
 	mod_timer(&ir->timer, jiffies + msecs_to_jiffies(ir->polling));
 }
 
-void em28xx_ir_start(struct em28xx_IR *ir)
+static void em28xx_ir_start(struct em28xx_IR *ir)
 {
 	setup_timer(&ir->timer, ir_timer, (unsigned long)ir);
 	INIT_WORK(&ir->work, em28xx_ir_work);
diff --git a/drivers/media/video/gspca/m5602/m5602_s5k83a.c b/drivers/media/video/gspca/m5602/m5602_s5k83a.c
index af3f2dc2c702..ccea4a758464 100644
--- a/drivers/media/video/gspca/m5602/m5602_s5k83a.c
+++ b/drivers/media/video/gspca/m5602/m5602_s5k83a.c
@@ -113,7 +113,7 @@ int s5k83a_power_down(struct sd *sd)
 	return 0;
 }
 
-void s5k83a_dump_registers(struct sd *sd)
+static void s5k83a_dump_registers(struct sd *sd)
 {
 	int address;
 	u8 page, old_page;
diff --git a/drivers/media/video/ov7670.c b/drivers/media/video/ov7670.c
index ca26b0c50cf2..05c14a29375a 100644
--- a/drivers/media/video/ov7670.c
+++ b/drivers/media/video/ov7670.c
@@ -1347,7 +1347,6 @@ static struct i2c_driver ov7670_driver = {
 		.name = "ov7670",
 	},
 	.id 		= I2C_DRIVERID_OV7670,
-	.class 		= I2C_CLASS_CAM_DIGITAL,
 	.attach_adapter = ov7670_attach,
 	.detach_client	= ov7670_detach,
 	.command	= ov7670_command,
diff --git a/drivers/media/video/ovcamchip/ovcamchip_core.c b/drivers/media/video/ovcamchip/ovcamchip_core.c
index 2c4acbf5a4fe..c841f4e4fbe4 100644
--- a/drivers/media/video/ovcamchip/ovcamchip_core.c
+++ b/drivers/media/video/ovcamchip/ovcamchip_core.c
@@ -405,7 +405,6 @@ static struct i2c_driver driver = {
 		.name =		"ovcamchip",
 	},
 	.id =			I2C_DRIVERID_OVCAMCHIP,
-	.class =		I2C_CLASS_CAM_DIGITAL,
 	.attach_adapter =	ovcamchip_attach,
 	.detach_client =	ovcamchip_detach,
 	.command =		ovcamchip_command,
diff --git a/drivers/media/video/pvrusb2/pvrusb2-hdw.c b/drivers/media/video/pvrusb2/pvrusb2-hdw.c
index 8fb92ac78c7b..fa304e5f252a 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-hdw.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-hdw.c
@@ -3655,7 +3655,7 @@ void pvr2_hdw_device_reset(struct pvr2_hdw *hdw)
 	int ret;
 	pvr2_trace(PVR2_TRACE_INIT,"Performing a device reset...");
 	ret = usb_lock_device_for_reset(hdw->usb_dev,NULL);
-	if (ret == 1) {
+	if (ret == 0) {
 		ret = usb_reset_device(hdw->usb_dev);
 		usb_unlock_device(hdw->usb_dev);
 	} else {
diff --git a/drivers/media/video/pxa_camera.c b/drivers/media/video/pxa_camera.c
index 9d33de22cc48..a1d6008efcbb 100644
--- a/drivers/media/video/pxa_camera.c
+++ b/drivers/media/video/pxa_camera.c
@@ -34,12 +34,10 @@
 
 #include <linux/videodev2.h>
 
-#include <asm/dma.h>
+#include <mach/dma.h>
 #include <mach/pxa-regs.h>
 #include <mach/camera.h>
 
-#include "pxa_camera.h"
-
 #define PXA_CAM_VERSION_CODE KERNEL_VERSION(0, 0, 5)
 #define PXA_CAM_DRV_NAME "pxa27x-camera"
 
diff --git a/drivers/media/video/pxa_camera.h b/drivers/media/video/pxa_camera.h
deleted file mode 100644
index 89cbfc9a35c5..000000000000
--- a/drivers/media/video/pxa_camera.h
+++ /dev/null
@@ -1,95 +0,0 @@
-/* Camera Interface */
-#define CICR0		__REG(0x50000000)
-#define CICR1		__REG(0x50000004)
-#define CICR2		__REG(0x50000008)
-#define CICR3		__REG(0x5000000C)
-#define CICR4		__REG(0x50000010)
-#define CISR		__REG(0x50000014)
-#define CIFR		__REG(0x50000018)
-#define CITOR		__REG(0x5000001C)
-#define CIBR0		__REG(0x50000028)
-#define CIBR1		__REG(0x50000030)
-#define CIBR2		__REG(0x50000038)
-
-#define CICR0_DMAEN	(1 << 31)	/* DMA request enable */
-#define CICR0_PAR_EN	(1 << 30)	/* Parity enable */
-#define CICR0_SL_CAP_EN	(1 << 29)	/* Capture enable for slave mode */
-#define CICR0_ENB	(1 << 28)	/* Camera interface enable */
-#define CICR0_DIS	(1 << 27)	/* Camera interface disable */
-#define CICR0_SIM	(0x7 << 24)	/* Sensor interface mode mask */
-#define CICR0_TOM	(1 << 9)	/* Time-out mask */
-#define CICR0_RDAVM	(1 << 8)	/* Receive-data-available mask */
-#define CICR0_FEM	(1 << 7)	/* FIFO-empty mask */
-#define CICR0_EOLM	(1 << 6)	/* End-of-line mask */
-#define CICR0_PERRM	(1 << 5)	/* Parity-error mask */
-#define CICR0_QDM	(1 << 4)	/* Quick-disable mask */
-#define CICR0_CDM	(1 << 3)	/* Disable-done mask */
-#define CICR0_SOFM	(1 << 2)	/* Start-of-frame mask */
-#define CICR0_EOFM	(1 << 1)	/* End-of-frame mask */
-#define CICR0_FOM	(1 << 0)	/* FIFO-overrun mask */
-
-#define CICR1_TBIT	(1 << 31)	/* Transparency bit */
-#define CICR1_RGBT_CONV	(0x3 << 29)	/* RGBT conversion mask */
-#define CICR1_PPL	(0x7ff << 15)	/* Pixels per line mask */
-#define CICR1_RGB_CONV	(0x7 << 12)	/* RGB conversion mask */
-#define CICR1_RGB_F	(1 << 11)	/* RGB format */
-#define CICR1_YCBCR_F	(1 << 10)	/* YCbCr format */
-#define CICR1_RGB_BPP	(0x7 << 7)	/* RGB bis per pixel mask */
-#define CICR1_RAW_BPP	(0x3 << 5)	/* Raw bis per pixel mask */
-#define CICR1_COLOR_SP	(0x3 << 3)	/* Color space mask */
-#define CICR1_DW	(0x7 << 0)	/* Data width mask */
-
-#define CICR2_BLW	(0xff << 24)	/* Beginning-of-line pixel clock
-					   wait count mask */
-#define CICR2_ELW	(0xff << 16)	/* End-of-line pixel clock
-					   wait count mask */
-#define CICR2_HSW	(0x3f << 10)	/* Horizontal sync pulse width mask */
-#define CICR2_BFPW	(0x3f << 3)	/* Beginning-of-frame pixel clock
-					   wait count mask */
-#define CICR2_FSW	(0x7 << 0)	/* Frame stabilization
-					   wait count mask */
-
-#define CICR3_BFW	(0xff << 24)	/* Beginning-of-frame line clock
-					   wait count mask */
-#define CICR3_EFW	(0xff << 16)	/* End-of-frame line clock
-					   wait count mask */
-#define CICR3_VSW	(0x3f << 10)	/* Vertical sync pulse width mask */
-#define CICR3_BFPW	(0x3f << 3)	/* Beginning-of-frame pixel clock
-					   wait count mask */
-#define CICR3_LPF	(0x7ff << 0)	/* Lines per frame mask */
-
-#define CICR4_MCLK_DLY	(0x3 << 24)	/* MCLK Data Capture Delay mask */
-#define CICR4_PCLK_EN	(1 << 23)	/* Pixel clock enable */
-#define CICR4_PCP	(1 << 22)	/* Pixel clock polarity */
-#define CICR4_HSP	(1 << 21)	/* Horizontal sync polarity */
-#define CICR4_VSP	(1 << 20)	/* Vertical sync polarity */
-#define CICR4_MCLK_EN	(1 << 19)	/* MCLK enable */
-#define CICR4_FR_RATE	(0x7 << 8)	/* Frame rate mask */
-#define CICR4_DIV	(0xff << 0)	/* Clock divisor mask */
-
-#define CISR_FTO	(1 << 15)	/* FIFO time-out */
-#define CISR_RDAV_2	(1 << 14)	/* Channel 2 receive data available */
-#define CISR_RDAV_1	(1 << 13)	/* Channel 1 receive data available */
-#define CISR_RDAV_0	(1 << 12)	/* Channel 0 receive data available */
-#define CISR_FEMPTY_2	(1 << 11)	/* Channel 2 FIFO empty */
-#define CISR_FEMPTY_1	(1 << 10)	/* Channel 1 FIFO empty */
-#define CISR_FEMPTY_0	(1 << 9)	/* Channel 0 FIFO empty */
-#define CISR_EOL	(1 << 8)	/* End of line */
-#define CISR_PAR_ERR	(1 << 7)	/* Parity error */
-#define CISR_CQD	(1 << 6)	/* Camera interface quick disable */
-#define CISR_CDD	(1 << 5)	/* Camera interface disable done */
-#define CISR_SOF	(1 << 4)	/* Start of frame */
-#define CISR_EOF	(1 << 3)	/* End of frame */
-#define CISR_IFO_2	(1 << 2)	/* FIFO overrun for Channel 2 */
-#define CISR_IFO_1	(1 << 1)	/* FIFO overrun for Channel 1 */
-#define CISR_IFO_0	(1 << 0)	/* FIFO overrun for Channel 0 */
-
-#define CIFR_FLVL2	(0x7f << 23)	/* FIFO 2 level mask */
-#define CIFR_FLVL1	(0x7f << 16)	/* FIFO 1 level mask */
-#define CIFR_FLVL0	(0xff << 8)	/* FIFO 0 level mask */
-#define CIFR_THL_0	(0x3 << 4)	/* Threshold Level for Channel 0 FIFO */
-#define CIFR_RESET_F	(1 << 3)	/* Reset input FIFOs */
-#define CIFR_FEN2	(1 << 2)	/* FIFO enable for channel 2 */
-#define CIFR_FEN1	(1 << 1)	/* FIFO enable for channel 1 */
-#define CIFR_FEN0	(1 << 0)	/* FIFO enable for channel 0 */
-
diff --git a/drivers/media/video/usbvideo/ibmcam.c b/drivers/media/video/usbvideo/ibmcam.c
index f8d85ddb4804..b08549661781 100644
--- a/drivers/media/video/usbvideo/ibmcam.c
+++ b/drivers/media/video/usbvideo/ibmcam.c
@@ -3779,7 +3779,7 @@ static int ibmcam_probe(struct usb_interface *intf, const struct usb_device_id *
 			err("Alternate settings have different endpoint addresses!");
 			return -ENODEV;
 		}
-		if (usb_endpoint_type(endpoint) != USB_ENDPOINT_XFER_ISOC) {
+		if (!usb_endpoint_xfer_isoc(endpoint)) {
 			err("Interface %d. has non-ISO endpoint!", ifnum);
 			return -ENODEV;
 		}
diff --git a/drivers/media/video/usbvideo/konicawc.c b/drivers/media/video/usbvideo/konicawc.c
index 90f0ce6a26bc..900ec2129ca1 100644
--- a/drivers/media/video/usbvideo/konicawc.c
+++ b/drivers/media/video/usbvideo/konicawc.c
@@ -823,7 +823,7 @@ static int konicawc_probe(struct usb_interface *intf, const struct usb_device_id
 			err("Alternate settings have different endpoint addresses!");
 			return -ENODEV;
 		}
-		if (usb_endpoint_type(endpoint) != USB_ENDPOINT_XFER_ISOC) {
+		if (!usb_endpoint_xfer_isoc(endpoint)) {
 			err("Interface %d. has non-ISO endpoint!",
 			    interface->desc.bInterfaceNumber);
 			return -ENODEV;
diff --git a/drivers/media/video/usbvideo/ultracam.c b/drivers/media/video/usbvideo/ultracam.c
index 839a08240c25..fbd1b6392290 100644
--- a/drivers/media/video/usbvideo/ultracam.c
+++ b/drivers/media/video/usbvideo/ultracam.c
@@ -556,7 +556,7 @@ static int ultracam_probe(struct usb_interface *intf, const struct usb_device_id
 			err("Alternate settings have different endpoint addresses!");
 			return -ENODEV;
 		}
-		if (usb_endpoint_type(endpoint) != USB_ENDPOINT_XFER_ISOC) {
+		if (!usb_endpoint_xfer_isoc(endpoint)) {
 			err("Interface %d. has non-ISO endpoint!",
 			    interface->desc.bInterfaceNumber);
 			return -ENODEV;
diff --git a/drivers/media/video/usbvision/usbvision-video.c b/drivers/media/video/usbvision/usbvision-video.c
index 2be5e47ed081..2622de003a45 100644
--- a/drivers/media/video/usbvision/usbvision-video.c
+++ b/drivers/media/video/usbvision/usbvision-video.c
@@ -1674,8 +1674,7 @@ static int __devinit usbvision_probe(struct usb_interface *intf,
 		interface = &dev->actconfig->interface[ifnum]->altsetting[0];
 	}
 	endpoint = &interface->endpoint[1].desc;
-	if (usb_endpoint_type(endpoint) !=
-	    USB_ENDPOINT_XFER_ISOC) {
+	if (!usb_endpoint_xfer_isoc(endpoint)) {
 		err("%s: interface %d. has non-ISO endpoint!",
 		    __func__, ifnum);
 		err("%s: Endpoint attributes %d",
diff --git a/drivers/media/video/v4l1-compat.c b/drivers/media/video/v4l1-compat.c
index d450cab20be4..b617bf05e2d7 100644
--- a/drivers/media/video/v4l1-compat.c
+++ b/drivers/media/video/v4l1-compat.c
@@ -203,7 +203,6 @@ static int poll_one(struct file *file, struct poll_wqueues *pwq)
 	table = &pwq->pt;
 	for (;;) {
 		int mask;
-		set_current_state(TASK_INTERRUPTIBLE);
 		mask = file->f_op->poll(file, table);
 		if (mask & POLLIN)
 			break;
@@ -212,9 +211,8 @@ static int poll_one(struct file *file, struct poll_wqueues *pwq)
 			retval = -ERESTARTSYS;
 			break;
 		}
-		schedule();
+		poll_schedule(pwq, TASK_INTERRUPTIBLE);
 	}
-	set_current_state(TASK_RUNNING);
 	poll_freewait(pwq);
 	return retval;
 }
diff --git a/drivers/media/video/v4l2-device.c b/drivers/media/video/v4l2-device.c
index 9eefde031597..cf9d4c7f571a 100644
--- a/drivers/media/video/v4l2-device.c
+++ b/drivers/media/video/v4l2-device.c
@@ -29,7 +29,7 @@ int v4l2_device_register(struct device *dev, struct v4l2_device *v4l2_dev)
 	if (dev == NULL || v4l2_dev == NULL)
 		return -EINVAL;
 	/* Warn if we apparently re-register a device */
-	WARN_ON(dev_get_drvdata(dev));
+	WARN_ON(dev_get_drvdata(dev) != NULL);
 	INIT_LIST_HEAD(&v4l2_dev->subdevs);
 	spin_lock_init(&v4l2_dev->lock);
 	v4l2_dev->dev = dev;
@@ -61,7 +61,7 @@ int v4l2_device_register_subdev(struct v4l2_device *dev, struct v4l2_subdev *sd)
 	if (dev == NULL || sd == NULL || !sd->name[0])
 		return -EINVAL;
 	/* Warn if we apparently re-register a subdev */
-	WARN_ON(sd->dev);
+	WARN_ON(sd->dev != NULL);
 	if (!try_module_get(sd->owner))
 		return -ENODEV;
 	sd->dev = dev;
diff --git a/drivers/media/video/videobuf-dma-sg.c b/drivers/media/video/videobuf-dma-sg.c
index bc6d5aba0fe6..da1790e57a86 100644
--- a/drivers/media/video/videobuf-dma-sg.c
+++ b/drivers/media/video/videobuf-dma-sg.c
@@ -388,8 +388,7 @@ videobuf_vm_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 	page = alloc_page(GFP_USER | __GFP_DMA32);
 	if (!page)
 		return VM_FAULT_OOM;
-	clear_user_page(page_address(page), (unsigned long)vmf->virtual_address,
-			page);
+	clear_user_highpage(page, (unsigned long)vmf->virtual_address);
 	vmf->page = page;
 	return 0;
 }
diff --git a/drivers/media/video/w9968cf.c b/drivers/media/video/w9968cf.c
index a3997b7d4366..105a832531f2 100644
--- a/drivers/media/video/w9968cf.c
+++ b/drivers/media/video/w9968cf.c
@@ -1553,7 +1553,6 @@ static int w9968cf_i2c_init(struct w9968cf_device* cam)
 
 	static struct i2c_adapter adap = {
 		.id =                I2C_HW_SMBUS_W9968CF,
-		.class =             I2C_CLASS_CAM_DIGITAL,
 		.owner =             THIS_MODULE,
 		.client_register =   w9968cf_i2c_attach_inform,
 		.client_unregister = w9968cf_i2c_detach_inform,
diff --git a/drivers/memstick/core/memstick.c b/drivers/memstick/core/memstick.c
index cea46906408e..a5b448ea4eab 100644
--- a/drivers/memstick/core/memstick.c
+++ b/drivers/memstick/core/memstick.c
@@ -385,8 +385,7 @@ static struct memstick_dev *memstick_alloc_card(struct memstick_host *host)
 
 	if (card) {
 		card->host = host;
-		snprintf(card->dev.bus_id, sizeof(card->dev.bus_id),
-			 "%s", host->dev.bus_id);
+		dev_set_name(&card->dev, "%s", dev_name(&host->dev));
 		card->dev.parent = &host->dev;
 		card->dev.bus = &memstick_bus_type;
 		card->dev.release = memstick_free_card;
@@ -519,7 +518,7 @@ int memstick_add_host(struct memstick_host *host)
 	if (rc)
 		return rc;
 
-	snprintf(host->dev.bus_id, BUS_ID_SIZE, "memstick%u", host->id);
+	dev_set_name(&host->dev, "memstick%u", host->id);
 
 	rc = device_add(&host->dev);
 	if (rc) {
diff --git a/drivers/memstick/core/mspro_block.c b/drivers/memstick/core/mspro_block.c
index 7911151e56a3..1f1e3982b1aa 100644
--- a/drivers/memstick/core/mspro_block.c
+++ b/drivers/memstick/core/mspro_block.c
@@ -887,14 +887,14 @@ try_again:
 	if (rc) {
 		printk(KERN_WARNING
 		       "%s: could not switch to 4-bit mode, error %d\n",
-		       card->dev.bus_id, rc);
+		       dev_name(&card->dev), rc);
 		return 0;
 	}
 
 	msb->system = MEMSTICK_SYS_PAR4;
 	host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
 	printk(KERN_INFO "%s: switching to 4-bit parallel mode\n",
-	       card->dev.bus_id);
+	       dev_name(&card->dev));
 
 	if (msb->caps & MEMSTICK_CAP_PAR8) {
 		rc = mspro_block_set_interface(card, MEMSTICK_SYS_PAR8);
@@ -905,11 +905,11 @@ try_again:
 					MEMSTICK_PAR8);
 			printk(KERN_INFO
 			       "%s: switching to 8-bit parallel mode\n",
-			       card->dev.bus_id);
+			       dev_name(&card->dev));
 		} else
 			printk(KERN_WARNING
 			       "%s: could not switch to 8-bit mode, error %d\n",
-			       card->dev.bus_id, rc);
+			       dev_name(&card->dev), rc);
 	}
 
 	card->next_request = h_mspro_block_req_init;
@@ -922,7 +922,7 @@ try_again:
 	if (rc) {
 		printk(KERN_WARNING
 		       "%s: interface error, trying to fall back to serial\n",
-		       card->dev.bus_id);
+		       dev_name(&card->dev));
 		msb->system = MEMSTICK_SYS_SERIAL;
 		host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_OFF);
 		msleep(10);
@@ -992,14 +992,14 @@ static int mspro_block_read_attributes(struct memstick_dev *card)
 
 	if (be16_to_cpu(attr->signature) != MSPRO_BLOCK_SIGNATURE) {
 		printk(KERN_ERR "%s: unrecognized device signature %x\n",
-		       card->dev.bus_id, be16_to_cpu(attr->signature));
+		       dev_name(&card->dev), be16_to_cpu(attr->signature));
 		rc = -ENODEV;
 		goto out_free_attr;
 	}
 
 	if (attr->count > MSPRO_BLOCK_MAX_ATTRIBUTES) {
 		printk(KERN_WARNING "%s: way too many attribute entries\n",
-		       card->dev.bus_id);
+		       dev_name(&card->dev));
 		attr_count = MSPRO_BLOCK_MAX_ATTRIBUTES;
 	} else
 		attr_count = attr->count;
diff --git a/drivers/memstick/host/tifm_ms.c b/drivers/memstick/host/tifm_ms.c
index d32d6ad8f3fc..03f71a431c82 100644
--- a/drivers/memstick/host/tifm_ms.c
+++ b/drivers/memstick/host/tifm_ms.c
@@ -546,7 +546,7 @@ static void tifm_ms_abort(unsigned long data)
 	printk(KERN_ERR
 	       "%s : card failed to respond for a long period of time "
 	       "(%x, %x)\n",
-	       host->dev->dev.bus_id, host->req ? host->req->tpc : 0,
+	       dev_name(&host->dev->dev), host->req ? host->req->tpc : 0,
 	       host->cmd_flags);
 
 	tifm_eject(host->dev);
@@ -561,7 +561,7 @@ static int tifm_ms_probe(struct tifm_dev *sock)
 	if (!(TIFM_SOCK_STATE_OCCUPIED
 	      & readl(sock->addr + SOCK_PRESENT_STATE))) {
 		printk(KERN_WARNING "%s : card gone, unexpectedly\n",
-		       sock->dev.bus_id);
+		       dev_name(&sock->dev));
 		return rc;
 	}
 
diff --git a/drivers/message/i2o/device.c b/drivers/message/i2o/device.c
index 54c2e9ae23e5..0ee4264f5db7 100644
--- a/drivers/message/i2o/device.c
+++ b/drivers/message/i2o/device.c
@@ -52,7 +52,6 @@ static inline int i2o_device_issue_claim(struct i2o_device *dev, u32 cmd,
 /**
  *	i2o_device_claim - claim a device for use by an OSM
  *	@dev: I2O device to claim
- *	@drv: I2O driver which wants to claim the device
  *
  *	Do the leg work to assign a device to a given OSM. If the claim succeeds,
  *	the owner is the primary. If the attempt fails a negative errno code
@@ -80,7 +79,6 @@ int i2o_device_claim(struct i2o_device *dev)
 /**
  *	i2o_device_claim_release - release a device that the OSM is using
  *	@dev: device to release
- *	@drv: driver which claimed the device
  *
  *	Drop a claim by an OSM on a given I2O device.
  *
@@ -134,7 +132,7 @@ static void i2o_device_release(struct device *dev)
 {
 	struct i2o_device *i2o_dev = to_i2o_device(dev);
 
-	pr_debug("i2o: device %s released\n", dev->bus_id);
+	pr_debug("i2o: device %s released\n", dev_name(dev));
 
 	kfree(i2o_dev);
 }
@@ -229,8 +227,8 @@ static int i2o_device_add(struct i2o_controller *c, i2o_lct_entry *entry)
 
 	i2o_dev->lct_data = *entry;
 
-	snprintf(i2o_dev->device.bus_id, BUS_ID_SIZE, "%d:%03x", c->unit,
-		 i2o_dev->lct_data.tid);
+	dev_set_name(&i2o_dev->device, "%d:%03x", c->unit,
+		     i2o_dev->lct_data.tid);
 
 	i2o_dev->iop = c;
 	i2o_dev->device.parent = &c->device;
@@ -281,7 +279,7 @@ static int i2o_device_add(struct i2o_controller *c, i2o_lct_entry *entry)
 
 	i2o_driver_notify_device_add_all(i2o_dev);
 
-	pr_debug("i2o: device %s added\n", i2o_dev->device.bus_id);
+	pr_debug("i2o: device %s added\n", dev_name(&i2o_dev->device));
 
 	return 0;
 
diff --git a/drivers/message/i2o/driver.c b/drivers/message/i2o/driver.c
index e0d474b17433..a0421efe04ca 100644
--- a/drivers/message/i2o/driver.c
+++ b/drivers/message/i2o/driver.c
@@ -173,7 +173,6 @@ void i2o_driver_unregister(struct i2o_driver *drv)
  *	i2o_driver_dispatch - dispatch an I2O reply message
  *	@c: I2O controller of the message
  *	@m: I2O message number
- *	@msg: I2O message to be delivered
  *
  *	The reply is delivered to the driver from which the original message
  *	was. This function is only called from interrupt context.
diff --git a/drivers/message/i2o/i2o_proc.c b/drivers/message/i2o/i2o_proc.c
index 54a3016ff45d..9a36b5a7de57 100644
--- a/drivers/message/i2o/i2o_proc.c
+++ b/drivers/message/i2o/i2o_proc.c
@@ -1300,7 +1300,7 @@ static int i2o_seq_show_dev_name(struct seq_file *seq, void *v)
 {
 	struct i2o_device *d = (struct i2o_device *)seq->private;
 
-	seq_printf(seq, "%s\n", d->device.bus_id);
+	seq_printf(seq, "%s\n", dev_name(&d->device));
 
 	return 0;
 }
diff --git a/drivers/message/i2o/i2o_scsi.c b/drivers/message/i2o/i2o_scsi.c
index 1bcdbbb9e7d3..3d45817e6dcd 100644
--- a/drivers/message/i2o/i2o_scsi.c
+++ b/drivers/message/i2o/i2o_scsi.c
@@ -390,7 +390,7 @@ static int i2o_scsi_reply(struct i2o_controller *c, u32 m,
  *	@i2o_dev: the I2O device which was added
  *
  *	If a I2O device is added we catch the notification, because I2O classes
- *	other then SCSI peripheral will not be received through
+ *	other than SCSI peripheral will not be received through
  *	i2o_scsi_probe().
  */
 static void i2o_scsi_notify_device_add(struct i2o_device *i2o_dev)
diff --git a/drivers/message/i2o/iop.c b/drivers/message/i2o/iop.c
index 35c67d1f255e..27cf4af0e13d 100644
--- a/drivers/message/i2o/iop.c
+++ b/drivers/message/i2o/iop.c
@@ -1072,7 +1072,7 @@ struct i2o_controller *i2o_iop_alloc(void)
 
 	c->device.release = &i2o_iop_release;
 
-	snprintf(c->device.bus_id, BUS_ID_SIZE, "iop%d", c->unit);
+	dev_set_name(&c->device, "iop%d", c->unit);
 
 #if BITS_PER_LONG == 64
 	spin_lock_init(&c->context_list_lock);
diff --git a/drivers/misc/ibmasm/event.c b/drivers/misc/ibmasm/event.c
index fda6a4d3bf23..68a0a5b94795 100644
--- a/drivers/misc/ibmasm/event.c
+++ b/drivers/misc/ibmasm/event.c
@@ -50,7 +50,7 @@ static void wake_up_event_readers(struct service_processor *sp)
  * Store the event in the circular event buffer, wake up any sleeping
  * event readers.
  * There is no reader marker in the buffer, therefore readers are
- * responsible for keeping up with the writer, or they will loose events.
+ * responsible for keeping up with the writer, or they will lose events.
  */
 void ibmasm_receive_event(struct service_processor *sp, void *data, unsigned int data_size)
 {
diff --git a/drivers/misc/ibmasm/module.c b/drivers/misc/ibmasm/module.c
index b5f6add34b0b..dc14b0b9cbfa 100644
--- a/drivers/misc/ibmasm/module.c
+++ b/drivers/misc/ibmasm/module.c
@@ -104,8 +104,7 @@ static int __devinit ibmasm_init_one(struct pci_dev *pdev, const struct pci_devi
 	}
 
 	sp->irq = pdev->irq;
-	sp->base_address = ioremap(pci_resource_start(pdev, 0),
-					pci_resource_len(pdev, 0));
+	sp->base_address = pci_ioremap_bar(pdev, 0);
 	if (!sp->base_address) {
 		dev_err(sp->dev, "Failed to ioremap pci memory\n");
 		result =  -ENODEV;
diff --git a/drivers/misc/ioc4.c b/drivers/misc/ioc4.c
index 6f76573e7c8a..60b0b1a4fb3a 100644
--- a/drivers/misc/ioc4.c
+++ b/drivers/misc/ioc4.c
@@ -269,6 +269,16 @@ ioc4_variant(struct ioc4_driver_data *idd)
 	return IOC4_VARIANT_PCI_RT;
 }
 
+static void
+ioc4_load_modules(struct work_struct *work)
+{
+	/* arg just has to be freed */
+
+	request_module("sgiioc4");
+
+	kfree(work);
+}
+
 /* Adds a new instance of an IOC4 card */
 static int
 ioc4_probe(struct pci_dev *pdev, const struct pci_device_id *pci_id)
@@ -378,6 +388,30 @@ ioc4_probe(struct pci_dev *pdev, const struct pci_device_id *pci_id)
 	}
 	mutex_unlock(&ioc4_mutex);
 
+	/* Request sgiioc4 IDE driver on boards that bring that functionality
+	 * off of IOC4.  The root filesystem may be hosted on a drive connected
+	 * to IOC4, so we need to make sure the sgiioc4 driver is loaded as it
+	 * won't be picked up by modprobes due to the ioc4 module owning the
+	 * PCI device.
+	 */
+	if (idd->idd_variant != IOC4_VARIANT_PCI_RT) {
+		struct work_struct *work;
+		work = kzalloc(sizeof(struct work_struct), GFP_KERNEL);
+		if (!work) {
+			printk(KERN_WARNING
+			       "%s: IOC4 unable to allocate memory for "
+			       "load of sub-modules.\n", __func__);
+		} else {
+			/* Request the module from a work procedure as the
+			 * modprobe goes out to a userland helper and that
+			 * will hang if done directly from ioc4_probe().
+			 */
+			printk(KERN_INFO "IOC4 loading sgiioc4 submodule\n");
+			INIT_WORK(work, ioc4_load_modules);
+			schedule_work(work);
+		}
+	}
+
 	return 0;
 
 out_misc_region:
@@ -462,6 +496,8 @@ ioc4_init(void)
 static void __devexit
 ioc4_exit(void)
 {
+	/* Ensure ioc4_load_modules() has completed before exiting */
+	flush_scheduled_work();
 	pci_unregister_driver(&ioc4_driver);
 }
 
diff --git a/drivers/misc/phantom.c b/drivers/misc/phantom.c
index abdebe347383..fa57b67593ae 100644
--- a/drivers/misc/phantom.c
+++ b/drivers/misc/phantom.c
@@ -6,7 +6,7 @@
  *  the Free Software Foundation; either version 2 of the License, or
  *  (at your option) any later version.
  *
- *  You need an userspace library to cooperate with this driver. It (and other
+ *  You need a userspace library to cooperate with this driver. It (and other
  *  info) may be obtained here:
  *  http://www.fi.muni.cz/~xslaby/phantom.html
  *  or alternatively, you might use OpenHaptics provided by Sensable.
diff --git a/drivers/misc/sgi-gru/grumain.c b/drivers/misc/sgi-gru/grumain.c
index e11e1ac50900..3d2fc216bae5 100644
--- a/drivers/misc/sgi-gru/grumain.c
+++ b/drivers/misc/sgi-gru/grumain.c
@@ -29,7 +29,7 @@ static struct device_driver gru_driver = {
 };
 
 static struct device gru_device = {
-	.bus_id = {0},
+	.init_name = "",
 	.driver = &gru_driver,
 };
 
diff --git a/drivers/misc/sgi-xp/xp_main.c b/drivers/misc/sgi-xp/xp_main.c
index 9a2e77172d94..16f8dcab2da4 100644
--- a/drivers/misc/sgi-xp/xp_main.c
+++ b/drivers/misc/sgi-xp/xp_main.c
@@ -25,7 +25,7 @@ struct device_driver xp_dbg_name = {
 };
 
 struct device xp_dbg_subname = {
-	.bus_id = {0},		/* set to "" */
+	.init_name = "",		/* set to "" */
 	.driver = &xp_dbg_name
 };
 
diff --git a/drivers/misc/sgi-xp/xpc_main.c b/drivers/misc/sgi-xp/xpc_main.c
index e8d5cfbd32c2..89218f7cfaa7 100644
--- a/drivers/misc/sgi-xp/xpc_main.c
+++ b/drivers/misc/sgi-xp/xpc_main.c
@@ -59,12 +59,12 @@ struct device_driver xpc_dbg_name = {
 };
 
 struct device xpc_part_dbg_subname = {
-	.bus_id = {0},		/* set to "part" at xpc_init() time */
+	.init_name = "",	/* set to "part" at xpc_init() time */
 	.driver = &xpc_dbg_name
 };
 
 struct device xpc_chan_dbg_subname = {
-	.bus_id = {0},		/* set to "chan" at xpc_init() time */
+	.init_name = "",	/* set to "chan" at xpc_init() time */
 	.driver = &xpc_dbg_name
 };
 
@@ -1258,8 +1258,8 @@ xpc_init(void)
 	int ret;
 	struct task_struct *kthread;
 
-	snprintf(xpc_part->bus_id, BUS_ID_SIZE, "part");
-	snprintf(xpc_chan->bus_id, BUS_ID_SIZE, "chan");
+	dev_set_name(xpc_part, "part");
+	dev_set_name(xpc_chan, "chan");
 
 	if (is_shub()) {
 		/*
diff --git a/drivers/misc/sgi-xp/xpnet.c b/drivers/misc/sgi-xp/xpnet.c
index 8e6aa9508f46..81152b3e360c 100644
--- a/drivers/misc/sgi-xp/xpnet.c
+++ b/drivers/misc/sgi-xp/xpnet.c
@@ -138,7 +138,7 @@ struct device_driver xpnet_dbg_name = {
 };
 
 struct device xpnet_dbg_subname = {
-	.bus_id = {0},		/* set to "" */
+	.init_name = "",	/* set to "" */
 	.driver = &xpnet_dbg_name
 };
 
diff --git a/drivers/misc/tifm_7xx1.c b/drivers/misc/tifm_7xx1.c
index 67503ea71d21..be5672a98702 100644
--- a/drivers/misc/tifm_7xx1.c
+++ b/drivers/misc/tifm_7xx1.c
@@ -164,7 +164,7 @@ static void tifm_7xx1_switch_media(struct work_struct *work)
 		if (sock) {
 			printk(KERN_INFO
 			       "%s : demand removing card from socket %u:%u\n",
-			       fm->dev.bus_id, fm->id, cnt);
+			       dev_name(&fm->dev), fm->id, cnt);
 			fm->sockets[cnt] = NULL;
 			sock_addr = sock->addr;
 			spin_unlock_irqrestore(&fm->lock, flags);
@@ -354,8 +354,7 @@ static int tifm_7xx1_probe(struct pci_dev *dev,
 	fm->has_ms_pif = tifm_7xx1_has_ms_pif;
 	pci_set_drvdata(dev, fm);
 
-	fm->addr = ioremap(pci_resource_start(dev, 0),
-			   pci_resource_len(dev, 0));
+	fm->addr = pci_ioremap_bar(dev, 0);
 	if (!fm->addr)
 		goto err_out_free;
 
diff --git a/drivers/misc/tifm_core.c b/drivers/misc/tifm_core.c
index 82dc72a1484f..98bcba521da2 100644
--- a/drivers/misc/tifm_core.c
+++ b/drivers/misc/tifm_core.c
@@ -203,7 +203,7 @@ int tifm_add_adapter(struct tifm_adapter *fm)
 	if (rc)
 		return rc;
 
-	snprintf(fm->dev.bus_id, BUS_ID_SIZE, "tifm%u", fm->id);
+	dev_set_name(&fm->dev, "tifm%u", fm->id);
 	rc = device_add(&fm->dev);
 	if (rc) {
 		spin_lock(&tifm_adapter_lock);
@@ -266,9 +266,8 @@ struct tifm_dev *tifm_alloc_device(struct tifm_adapter *fm, unsigned int id,
 		sock->dev.dma_mask = fm->dev.parent->dma_mask;
 		sock->dev.release = tifm_free_device;
 
-		snprintf(sock->dev.bus_id, BUS_ID_SIZE,
-			 "tifm_%s%u:%u", tifm_media_type_name(type, 2),
-			 fm->id, id);
+		dev_set_name(&sock->dev, "tifm_%s%u:%u",
+			     tifm_media_type_name(type, 2), fm->id, id);
 		printk(KERN_INFO DRIVER_NAME
 		       ": %s card detected in socket %u:%u\n",
 		       tifm_media_type_name(type, 0), fm->id, id);
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index 7a3f2436b011..1e97916914ad 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -25,8 +25,8 @@
 #include <linux/stat.h>
 
 #include <linux/mmc/host.h>
+#include <linux/atmel-mci.h>
 
-#include <asm/atmel-mci.h>
 #include <asm/io.h>
 #include <asm/unaligned.h>
 
diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index 6659b2275c0c..5733f0643843 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -170,7 +170,7 @@ static int wait_till_ready(struct m25p *flash)
 static int erase_chip(struct m25p *flash)
 {
 	DEBUG(MTD_DEBUG_LEVEL3, "%s: %s %dKiB\n",
-			flash->spi->dev.bus_id, __func__,
+			dev_name(&flash->spi->dev), __func__,
 			flash->mtd.size / 1024);
 
 	/* Wait until finished previous write command. */
@@ -197,7 +197,7 @@ static int erase_chip(struct m25p *flash)
 static int erase_sector(struct m25p *flash, u32 offset)
 {
 	DEBUG(MTD_DEBUG_LEVEL3, "%s: %s %dKiB at 0x%08x\n",
-			flash->spi->dev.bus_id, __func__,
+			dev_name(&flash->spi->dev), __func__,
 			flash->mtd.erasesize / 1024, offset);
 
 	/* Wait until finished previous write command. */
@@ -234,7 +234,7 @@ static int m25p80_erase(struct mtd_info *mtd, struct erase_info *instr)
 	u32 addr,len;
 
 	DEBUG(MTD_DEBUG_LEVEL2, "%s: %s %s 0x%08x, len %d\n",
-			flash->spi->dev.bus_id, __func__, "at",
+			dev_name(&flash->spi->dev), __func__, "at",
 			(u32)instr->addr, instr->len);
 
 	/* sanity checks */
@@ -295,7 +295,7 @@ static int m25p80_read(struct mtd_info *mtd, loff_t from, size_t len,
 	struct spi_message m;
 
 	DEBUG(MTD_DEBUG_LEVEL2, "%s: %s %s 0x%08x, len %zd\n",
-			flash->spi->dev.bus_id, __func__, "from",
+			dev_name(&flash->spi->dev), __func__, "from",
 			(u32)from, len);
 
 	/* sanity checks */
@@ -367,7 +367,7 @@ static int m25p80_write(struct mtd_info *mtd, loff_t to, size_t len,
 	struct spi_message m;
 
 	DEBUG(MTD_DEBUG_LEVEL2, "%s: %s %s 0x%08x, len %zd\n",
-			flash->spi->dev.bus_id, __func__, "to",
+			dev_name(&flash->spi->dev), __func__, "to",
 			(u32)to, len);
 
 	if (retlen)
@@ -563,7 +563,7 @@ static struct flash_info *__devinit jedec_probe(struct spi_device *spi)
 	tmp = spi_write_then_read(spi, &code, 1, id, 5);
 	if (tmp < 0) {
 		DEBUG(MTD_DEBUG_LEVEL0, "%s: error %d reading JEDEC ID\n",
-			spi->dev.bus_id, tmp);
+			dev_name(&spi->dev), tmp);
 		return NULL;
 	}
 	jedec = id[0];
@@ -617,7 +617,7 @@ static int __devinit m25p_probe(struct spi_device *spi)
 		/* unrecognized chip? */
 		if (i == ARRAY_SIZE(m25p_data)) {
 			DEBUG(MTD_DEBUG_LEVEL0, "%s: unrecognized id %s\n",
-					spi->dev.bus_id, data->type);
+					dev_name(&spi->dev), data->type);
 			info = NULL;
 
 		/* recognized; is that chip really what's there? */
@@ -658,7 +658,7 @@ static int __devinit m25p_probe(struct spi_device *spi)
 	if (data && data->name)
 		flash->mtd.name = data->name;
 	else
-		flash->mtd.name = spi->dev.bus_id;
+		flash->mtd.name = dev_name(&spi->dev);
 
 	flash->mtd.type = MTD_NORFLASH;
 	flash->mtd.writesize = 1;
diff --git a/drivers/mtd/devices/mtd_dataflash.c b/drivers/mtd/devices/mtd_dataflash.c
index 6dd9aff8bb2d..65126cd668ff 100644
--- a/drivers/mtd/devices/mtd_dataflash.c
+++ b/drivers/mtd/devices/mtd_dataflash.c
@@ -128,7 +128,7 @@ static int dataflash_waitready(struct spi_device *spi)
 		status = dataflash_status(spi);
 		if (status < 0) {
 			DEBUG(MTD_DEBUG_LEVEL1, "%s: status %d?\n",
-					spi->dev.bus_id, status);
+					dev_name(&spi->dev), status);
 			status = 0;
 		}
 
@@ -154,7 +154,7 @@ static int dataflash_erase(struct mtd_info *mtd, struct erase_info *instr)
 	uint8_t			*command;
 
 	DEBUG(MTD_DEBUG_LEVEL2, "%s: erase addr=0x%x len 0x%x\n",
-			spi->dev.bus_id,
+			dev_name(&spi->dev),
 			instr->addr, instr->len);
 
 	/* Sanity checks */
@@ -197,7 +197,7 @@ static int dataflash_erase(struct mtd_info *mtd, struct erase_info *instr)
 
 		if (status < 0) {
 			printk(KERN_ERR "%s: erase %x, err %d\n",
-				spi->dev.bus_id, pageaddr, status);
+				dev_name(&spi->dev), pageaddr, status);
 			/* REVISIT:  can retry instr->retries times; or
 			 * giveup and instr->fail_addr = instr->addr;
 			 */
@@ -239,7 +239,7 @@ static int dataflash_read(struct mtd_info *mtd, loff_t from, size_t len,
 	int			status;
 
 	DEBUG(MTD_DEBUG_LEVEL2, "%s: read 0x%x..0x%x\n",
-		priv->spi->dev.bus_id, (unsigned)from, (unsigned)(from + len));
+		dev_name(&priv->spi->dev), (unsigned)from, (unsigned)(from + len));
 
 	*retlen = 0;
 
@@ -288,7 +288,7 @@ static int dataflash_read(struct mtd_info *mtd, loff_t from, size_t len,
 		status = 0;
 	} else
 		DEBUG(MTD_DEBUG_LEVEL1, "%s: read %x..%x --> %d\n",
-			priv->spi->dev.bus_id,
+			dev_name(&priv->spi->dev),
 			(unsigned)from, (unsigned)(from + len),
 			status);
 	return status;
@@ -315,7 +315,7 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 	uint8_t			*command;
 
 	DEBUG(MTD_DEBUG_LEVEL2, "%s: write 0x%x..0x%x\n",
-		spi->dev.bus_id, (unsigned)to, (unsigned)(to + len));
+		dev_name(&spi->dev), (unsigned)to, (unsigned)(to + len));
 
 	*retlen = 0;
 
@@ -374,7 +374,7 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 			status = spi_sync(spi, &msg);
 			if (status < 0)
 				DEBUG(MTD_DEBUG_LEVEL1, "%s: xfer %u -> %d \n",
-					spi->dev.bus_id, addr, status);
+					dev_name(&spi->dev), addr, status);
 
 			(void) dataflash_waitready(priv->spi);
 		}
@@ -396,7 +396,7 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 		spi_transfer_del(x + 1);
 		if (status < 0)
 			DEBUG(MTD_DEBUG_LEVEL1, "%s: pgm %u/%u -> %d \n",
-				spi->dev.bus_id, addr, writelen, status);
+				dev_name(&spi->dev), addr, writelen, status);
 
 		(void) dataflash_waitready(priv->spi);
 
@@ -416,14 +416,14 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 		status = spi_sync(spi, &msg);
 		if (status < 0)
 			DEBUG(MTD_DEBUG_LEVEL1, "%s: compare %u -> %d \n",
-				spi->dev.bus_id, addr, status);
+				dev_name(&spi->dev), addr, status);
 
 		status = dataflash_waitready(priv->spi);
 
 		/* Check result of the compare operation */
 		if (status & (1 << 6)) {
 			printk(KERN_ERR "%s: compare page %u, err %d\n",
-				spi->dev.bus_id, pageaddr, status);
+				dev_name(&spi->dev), pageaddr, status);
 			remaining = 0;
 			status = -EIO;
 			break;
@@ -779,7 +779,7 @@ static struct flash_info *__devinit jedec_probe(struct spi_device *spi)
 	tmp = spi_write_then_read(spi, &code, 1, id, 3);
 	if (tmp < 0) {
 		DEBUG(MTD_DEBUG_LEVEL0, "%s: error %d reading JEDEC ID\n",
-			spi->dev.bus_id, tmp);
+			dev_name(&spi->dev), tmp);
 		return ERR_PTR(tmp);
 	}
 	if (id[0] != 0x1f)
@@ -869,7 +869,7 @@ static int __devinit dataflash_probe(struct spi_device *spi)
 	status = dataflash_status(spi);
 	if (status <= 0 || status == 0xff) {
 		DEBUG(MTD_DEBUG_LEVEL1, "%s: status error %d\n",
-				spi->dev.bus_id, status);
+				dev_name(&spi->dev), status);
 		if (status == 0 || status == 0xff)
 			status = -ENODEV;
 		return status;
@@ -905,13 +905,13 @@ static int __devinit dataflash_probe(struct spi_device *spi)
 	/* obsolete AT45DB1282 not (yet?) supported */
 	default:
 		DEBUG(MTD_DEBUG_LEVEL1, "%s: unsupported device (%x)\n",
-				spi->dev.bus_id, status & 0x3c);
+				dev_name(&spi->dev), status & 0x3c);
 		status = -ENODEV;
 	}
 
 	if (status < 0)
 		DEBUG(MTD_DEBUG_LEVEL1, "%s: add_dataflash --> %d\n",
-				spi->dev.bus_id, status);
+				dev_name(&spi->dev), status);
 
 	return status;
 }
@@ -921,7 +921,7 @@ static int __devexit dataflash_remove(struct spi_device *spi)
 	struct dataflash	*flash = dev_get_drvdata(&spi->dev);
 	int			status;
 
-	DEBUG(MTD_DEBUG_LEVEL1, "%s: remove\n", spi->dev.bus_id);
+	DEBUG(MTD_DEBUG_LEVEL1, "%s: remove\n", dev_name(&spi->dev));
 
 	if (mtd_has_partitions() && flash->partitioned)
 		status = del_mtd_partitions(&flash->mtd);
diff --git a/drivers/mtd/devices/pmc551.c b/drivers/mtd/devices/pmc551.c
index d38bca64bb15..d2fd550f7e09 100644
--- a/drivers/mtd/devices/pmc551.c
+++ b/drivers/mtd/devices/pmc551.c
@@ -34,7 +34,7 @@
  *	aperture size, not the dram size, and the V370PDC supplies no
  *	other method for memory size discovery.  This problem is
  *	mostly only relevant when compiled as a module, as the
- *	unloading of the module with an aperture size smaller then
+ *	unloading of the module with an aperture size smaller than
  *	the ram will cause the driver to detect the onboard memory
  *	size to be equal to the aperture size when the module is
  *	reloaded.  Soooo, to help, the module supports an msize
diff --git a/drivers/mtd/maps/integrator-flash.c b/drivers/mtd/maps/integrator-flash.c
index 7100ee3c7b01..d2ec262666c7 100644
--- a/drivers/mtd/maps/integrator-flash.c
+++ b/drivers/mtd/maps/integrator-flash.c
@@ -105,7 +105,7 @@ static int armflash_probe(struct platform_device *dev)
 	info->map.bankwidth	= plat->width;
 	info->map.phys		= res->start;
 	info->map.virt		= base;
-	info->map.name		= dev->dev.bus_id;
+	info->map.name		= dev_name(&dev->dev);
 	info->map.set_vpp	= armflash_set_vpp;
 
 	simple_map_init(&info->map);
diff --git a/drivers/mtd/maps/ixp2000.c b/drivers/mtd/maps/ixp2000.c
index 3ea1de9be720..d4fb9a3ab4df 100644
--- a/drivers/mtd/maps/ixp2000.c
+++ b/drivers/mtd/maps/ixp2000.c
@@ -188,7 +188,7 @@ static int ixp2000_flash_probe(struct platform_device *dev)
  	 */
 	info->map.map_priv_2 = (unsigned long) ixp_data->bank_setup;
 
-	info->map.name = dev->dev.bus_id;
+	info->map.name = dev_name(&dev->dev);
 	info->map.read = ixp2000_flash_read8;
 	info->map.write = ixp2000_flash_write8;
 	info->map.copy_from = ixp2000_flash_copy_from;
@@ -196,7 +196,7 @@ static int ixp2000_flash_probe(struct platform_device *dev)
 
 	info->res = request_mem_region(dev->resource->start,
 			dev->resource->end - dev->resource->start + 1,
-			dev->dev.bus_id);
+			dev_name(&dev->dev));
 	if (!info->res) {
 		dev_err(&dev->dev, "Could not reserve memory region\n");
 		err = -ENOMEM;
diff --git a/drivers/mtd/maps/ixp4xx.c b/drivers/mtd/maps/ixp4xx.c
index 16555cbeaea4..7214b876feba 100644
--- a/drivers/mtd/maps/ixp4xx.c
+++ b/drivers/mtd/maps/ixp4xx.c
@@ -218,7 +218,7 @@ static int ixp4xx_flash_probe(struct platform_device *dev)
 	 * handle that.
 	 */
 	info->map.bankwidth = 2;
-	info->map.name = dev->dev.bus_id;
+	info->map.name = dev_name(&dev->dev);
 	info->map.read = ixp4xx_read16,
 	info->map.write = ixp4xx_probe_write16,
 	info->map.copy_from = ixp4xx_copy_from,
diff --git a/drivers/mtd/maps/omap_nor.c b/drivers/mtd/maps/omap_nor.c
index 05f276af15da..7e50e9b1b781 100644
--- a/drivers/mtd/maps/omap_nor.c
+++ b/drivers/mtd/maps/omap_nor.c
@@ -101,7 +101,7 @@ static int __init omapflash_probe(struct platform_device *pdev)
 		err = -ENOMEM;
 		goto out_release_mem_region;
 	}
-	info->map.name		= pdev->dev.bus_id;
+	info->map.name		= dev_name(&pdev->dev);
 	info->map.phys		= res->start;
 	info->map.size		= size;
 	info->map.bankwidth	= pdata->width;
diff --git a/drivers/mtd/maps/physmap.c b/drivers/mtd/maps/physmap.c
index dfbf3f270cea..1db16e549e38 100644
--- a/drivers/mtd/maps/physmap.c
+++ b/drivers/mtd/maps/physmap.c
@@ -108,13 +108,13 @@ static int physmap_flash_probe(struct platform_device *dev)
 		if (!devm_request_mem_region(&dev->dev,
 			dev->resource[i].start,
 			dev->resource[i].end - dev->resource[i].start + 1,
-			dev->dev.bus_id)) {
+			dev_name(&dev->dev))) {
 			dev_err(&dev->dev, "Could not reserve memory region\n");
 			err = -ENOMEM;
 			goto err_out;
 		}
 
-		info->map[i].name = dev->dev.bus_id;
+		info->map[i].name = dev_name(&dev->dev);
 		info->map[i].phys = dev->resource[i].start;
 		info->map[i].size = dev->resource[i].end - dev->resource[i].start + 1;
 		info->map[i].bankwidth = physmap_data->width;
@@ -150,7 +150,7 @@ static int physmap_flash_probe(struct platform_device *dev)
 		 * We detected multiple devices. Concatenate them together.
 		 */
 #ifdef CONFIG_MTD_CONCAT
-		info->cmtd = mtd_concat_create(info->mtd, devices_found, dev->dev.bus_id);
+		info->cmtd = mtd_concat_create(info->mtd, devices_found, dev_name(&dev->dev));
 		if (info->cmtd == NULL)
 			err = -ENXIO;
 #else
diff --git a/drivers/mtd/maps/physmap_of.c b/drivers/mtd/maps/physmap_of.c
index 5fcfec034a94..fbf0ca939d72 100644
--- a/drivers/mtd/maps/physmap_of.c
+++ b/drivers/mtd/maps/physmap_of.c
@@ -183,7 +183,7 @@ static int __devinit of_flash_probe(struct of_device *dev,
 
 	err = -EBUSY;
 	info->res = request_mem_region(res.start, res.end - res.start + 1,
-				       dev->dev.bus_id);
+				       dev_name(&dev->dev));
 	if (!info->res)
 		goto err_out;
 
@@ -194,7 +194,7 @@ static int __devinit of_flash_probe(struct of_device *dev,
 		goto err_out;
 	}
 
-	info->map.name = dev->dev.bus_id;
+	info->map.name = dev_name(&dev->dev);
 	info->map.phys = res.start;
 	info->map.size = res.end - res.start + 1;
 	info->map.bankwidth = *width;
diff --git a/drivers/mtd/mtdconcat.c b/drivers/mtd/mtdconcat.c
index 789842d0e6f2..1a05cf37851e 100644
--- a/drivers/mtd/mtdconcat.c
+++ b/drivers/mtd/mtdconcat.c
@@ -691,7 +691,7 @@ static int concat_block_markbad(struct mtd_info *mtd, loff_t ofs)
  */
 struct mtd_info *mtd_concat_create(struct mtd_info *subdev[],	/* subdevices to concatenate */
 				   int num_devs,	/* number of subdevices      */
-				   char *name)
+				   const char *name)
 {				/* name for the new device   */
 	int i;
 	size_t size;
diff --git a/drivers/mtd/nand/fsl_upm.c b/drivers/mtd/nand/fsl_upm.c
index a83192f80eba..7815a404a632 100644
--- a/drivers/mtd/nand/fsl_upm.c
+++ b/drivers/mtd/nand/fsl_upm.c
@@ -222,7 +222,7 @@ static int __devinit fun_probe(struct of_device *ofdev,
 
 	fun->rnb_gpio = of_get_gpio(ofdev->node, 0);
 	if (fun->rnb_gpio >= 0) {
-		ret = gpio_request(fun->rnb_gpio, ofdev->dev.bus_id);
+		ret = gpio_request(fun->rnb_gpio, dev_name(&ofdev->dev));
 		if (ret) {
 			dev_err(&ofdev->dev, "can't request RNB gpio\n");
 			goto err2;
diff --git a/drivers/mtd/nand/plat_nand.c b/drivers/mtd/nand/plat_nand.c
index f674c5427b17..75f9f4874ecf 100644
--- a/drivers/mtd/nand/plat_nand.c
+++ b/drivers/mtd/nand/plat_nand.c
@@ -54,7 +54,7 @@ static int __init plat_nand_probe(struct platform_device *pdev)
 	data->chip.priv = &data;
 	data->mtd.priv = &data->chip;
 	data->mtd.owner = THIS_MODULE;
-	data->mtd.name = pdev->dev.bus_id;
+	data->mtd.name = dev_name(&pdev->dev);
 
 	data->chip.IO_ADDR_R = data->io_base;
 	data->chip.IO_ADDR_W = data->io_base;
diff --git a/drivers/mtd/nand/tmio_nand.c b/drivers/mtd/nand/tmio_nand.c
index edb1e322113d..daa6a4c3b8ce 100644
--- a/drivers/mtd/nand/tmio_nand.c
+++ b/drivers/mtd/nand/tmio_nand.c
@@ -433,7 +433,7 @@ static int tmio_probe(struct platform_device *dev)
 	nand_chip->chip_delay = 15;
 
 	retval = request_irq(irq, &tmio_irq,
-				IRQF_DISABLED, dev->dev.bus_id, tmio);
+				IRQF_DISABLED, dev_name(&dev->dev), tmio);
 	if (retval) {
 		dev_err(&dev->dev, "request_irq error %d\n", retval);
 		goto err_irq;
diff --git a/drivers/mtd/onenand/generic.c b/drivers/mtd/onenand/generic.c
index ad81ab8e95e2..5b69e7773c6c 100644
--- a/drivers/mtd/onenand/generic.c
+++ b/drivers/mtd/onenand/generic.c
@@ -63,7 +63,7 @@ static int __devinit generic_onenand_probe(struct device *dev)
 	info->onenand.mmcontrol = pdata->mmcontrol;
 	info->onenand.irq = platform_get_irq(pdev, 0);
 
-	info->mtd.name = pdev->dev.bus_id;
+	info->mtd.name = dev_name(&pdev->dev);
 	info->mtd.priv = &info->onenand;
 	info->mtd.owner = THIS_MODULE;
 
diff --git a/drivers/mtd/onenand/omap2.c b/drivers/mtd/onenand/omap2.c
index d1e0b8e7224b..96ecc1766fa8 100644
--- a/drivers/mtd/onenand/omap2.c
+++ b/drivers/mtd/onenand/omap2.c
@@ -668,7 +668,7 @@ static int __devinit omap2_onenand_probe(struct platform_device *pdev)
 		 c->onenand.base);
 
 	c->pdev = pdev;
-	c->mtd.name = pdev->dev.bus_id;
+	c->mtd.name = dev_name(&pdev->dev);
 	c->mtd.priv = &c->onenand;
 	c->mtd.owner = THIS_MODULE;
 
diff --git a/drivers/mtd/ubi/build.c b/drivers/mtd/ubi/build.c
index ba0bd3d5775b..7caf22cd5ad0 100644
--- a/drivers/mtd/ubi/build.c
+++ b/drivers/mtd/ubi/build.c
@@ -280,7 +280,7 @@ static int ubi_sysfs_init(struct ubi_device *ubi)
 	ubi->dev.release = dev_release;
 	ubi->dev.devt = ubi->cdev.dev;
 	ubi->dev.class = ubi_class;
-	sprintf(&ubi->dev.bus_id[0], UBI_NAME_STR"%d", ubi->ubi_num);
+	dev_set_name(&ubi->dev, UBI_NAME_STR"%d", ubi->ubi_num);
 	err = device_register(&ubi->dev);
 	if (err)
 		return err;
diff --git a/drivers/mtd/ubi/eba.c b/drivers/mtd/ubi/eba.c
index 048a606cebde..25def348e5ba 100644
--- a/drivers/mtd/ubi/eba.c
+++ b/drivers/mtd/ubi/eba.c
@@ -717,7 +717,7 @@ write_error:
  * to the real data size, although the @buf buffer has to contain the
  * alignment. In all other cases, @len has to be aligned.
  *
- * It is prohibited to write more then once to logical eraseblocks of static
+ * It is prohibited to write more than once to logical eraseblocks of static
  * volumes. This function returns zero in case of success and a negative error
  * code in case of failure.
  */
diff --git a/drivers/mtd/ubi/io.c b/drivers/mtd/ubi/io.c
index a74118c05745..fe81039f2a7c 100644
--- a/drivers/mtd/ubi/io.c
+++ b/drivers/mtd/ubi/io.c
@@ -465,7 +465,7 @@ out:
  * This function synchronously erases physical eraseblock @pnum. If @torture
  * flag is not zero, the physical eraseblock is checked by means of writing
  * different patterns to it and reading them back. If the torturing is enabled,
- * the physical eraseblock is erased more then once.
+ * the physical eraseblock is erased more than once.
  *
  * This function returns the number of erasures made in case of success, %-EIO
  * if the erasure failed or the torturing test failed, and other negative error
diff --git a/drivers/mtd/ubi/scan.c b/drivers/mtd/ubi/scan.c
index 41d47e1cf15c..ecde202a5a12 100644
--- a/drivers/mtd/ubi/scan.c
+++ b/drivers/mtd/ubi/scan.c
@@ -478,7 +478,7 @@ int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
 			return 0;
 		} else {
 			/*
-			 * This logical eraseblock is older then the one found
+			 * This logical eraseblock is older than the one found
 			 * previously.
 			 */
 			if (cmp_res & 4)
diff --git a/drivers/mtd/ubi/ubi-media.h b/drivers/mtd/ubi/ubi-media.h
index 2ad940409053..8419fdccc79c 100644
--- a/drivers/mtd/ubi/ubi-media.h
+++ b/drivers/mtd/ubi/ubi-media.h
@@ -135,7 +135,7 @@ enum {
  * The erase counter header takes 64 bytes and has a plenty of unused space for
  * future usage. The unused fields are zeroed. The @version field is used to
  * indicate the version of UBI implementation which is supposed to be able to
- * work with this UBI image. If @version is greater then the current UBI
+ * work with this UBI image. If @version is greater than the current UBI
  * version, the image is rejected. This may be useful in future if something
  * is changed radically. This field is duplicated in the volume identifier
  * header.
@@ -187,7 +187,7 @@ struct ubi_ec_hdr {
  * (sequence number) is used to distinguish between older and newer versions of
  * logical eraseblocks.
  *
- * There are 2 situations when there may be more then one physical eraseblock
+ * There are 2 situations when there may be more than one physical eraseblock
  * corresponding to the same logical eraseblock, i.e., having the same @vol_id
  * and @lnum values in the volume identifier header. Suppose we have a logical
  * eraseblock L and it is mapped to the physical eraseblock P.
diff --git a/drivers/mtd/ubi/vmt.c b/drivers/mtd/ubi/vmt.c
index 3531ca9a1e24..22e1d7398fce 100644
--- a/drivers/mtd/ubi/vmt.c
+++ b/drivers/mtd/ubi/vmt.c
@@ -329,7 +329,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 	vol->dev.devt = dev;
 	vol->dev.class = ubi_class;
 
-	sprintf(&vol->dev.bus_id[0], "%s_%d", ubi->ubi_name, vol->vol_id);
+	dev_set_name(&vol->dev, "%s_%d", ubi->ubi_name, vol->vol_id);
 	err = device_register(&vol->dev);
 	if (err) {
 		ubi_err("cannot register device");
@@ -678,7 +678,7 @@ int ubi_add_volume(struct ubi_device *ubi, struct ubi_volume *vol)
 	vol->dev.parent = &ubi->dev;
 	vol->dev.devt = dev;
 	vol->dev.class = ubi_class;
-	sprintf(&vol->dev.bus_id[0], "%s_%d", ubi->ubi_name, vol->vol_id);
+	dev_set_name(&vol->dev, "%s_%d", ubi->ubi_name, vol->vol_id);
 	err = device_register(&vol->dev);
 	if (err)
 		goto out_gluebi;
diff --git a/drivers/mtd/ubi/vtbl.c b/drivers/mtd/ubi/vtbl.c
index 333c8941552f..1afc61e7455d 100644
--- a/drivers/mtd/ubi/vtbl.c
+++ b/drivers/mtd/ubi/vtbl.c
@@ -577,7 +577,7 @@ static int init_volumes(struct ubi_device *ubi, const struct ubi_scan_info *si,
 		if (vtbl[i].flags & UBI_VTBL_AUTORESIZE_FLG) {
 			/* Auto re-size flag may be set only for one volume */
 			if (ubi->autoresize_vol_id != -1) {
-				ubi_err("more then one auto-resize volume (%d "
+				ubi_err("more than one auto-resize volume (%d "
 					"and %d)", ubi->autoresize_vol_id, i);
 				kfree(vol);
 				return -EINVAL;
diff --git a/drivers/mtd/ubi/wl.c b/drivers/mtd/ubi/wl.c
index 14901cb82c18..891534f8210d 100644
--- a/drivers/mtd/ubi/wl.c
+++ b/drivers/mtd/ubi/wl.c
@@ -128,7 +128,7 @@
  * situation when the picked physical eraseblock is constantly erased after the
  * data is written to it. So, we have a constant which limits the highest erase
  * counter of the free physical eraseblock to pick. Namely, the WL sub-system
- * does not pick eraseblocks with erase counter greater then the lowest erase
+ * does not pick eraseblocks with erase counter greater than the lowest erase
  * counter plus %WL_FREE_MAX_DIFF.
  */
 #define WL_FREE_MAX_DIFF (2*UBI_WL_THRESHOLD)
@@ -917,7 +917,7 @@ static int ensure_wear_leveling(struct ubi_device *ubi)
 		/*
 		 * We schedule wear-leveling only if the difference between the
 		 * lowest erase counter of used physical eraseblocks and a high
-		 * erase counter of free physical eraseblocks is greater then
+		 * erase counter of free physical eraseblocks is greater than
 		 * %UBI_WL_THRESHOLD.
 		 */
 		e1 = rb_entry(rb_first(&ubi->used), struct ubi_wl_entry, u.rb);
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index 9a18270c1081..97ea7c60e002 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -2614,6 +2614,8 @@ source "drivers/net/tokenring/Kconfig"
 
 source "drivers/net/wireless/Kconfig"
 
+source "drivers/net/wimax/Kconfig"
+
 source "drivers/net/usb/Kconfig"
 
 source "drivers/net/pcmcia/Kconfig"
diff --git a/drivers/net/Makefile b/drivers/net/Makefile
index e5c34b464211..a3c5c002f224 100644
--- a/drivers/net/Makefile
+++ b/drivers/net/Makefile
@@ -263,3 +263,4 @@ obj-$(CONFIG_NIU) += niu.o
 obj-$(CONFIG_VIRTIO_NET) += virtio_net.o
 obj-$(CONFIG_SFC) += sfc/
 
+obj-$(CONFIG_WIMAX) += wimax/
diff --git a/drivers/net/bnx2x_link.c b/drivers/net/bnx2x_link.c
index 67de94f1f30e..fefa6ab13064 100644
--- a/drivers/net/bnx2x_link.c
+++ b/drivers/net/bnx2x_link.c
@@ -3359,7 +3359,7 @@ static u8 bnx2x_format_ver(u32 num, u8 *str, u16 len)
 	u8 shift = 8*4;
 	u8 digit;
 	if (len < 10) {
-		/* Need more then 10chars for this format */
+		/* Need more than 10chars for this format */
 		*str_ptr = '\0';
 		return -EINVAL;
 	}
diff --git a/drivers/net/e1000/e1000_hw.c b/drivers/net/e1000/e1000_hw.c
index d04eef53571e..e1a3fc1303ee 100644
--- a/drivers/net/e1000/e1000_hw.c
+++ b/drivers/net/e1000/e1000_hw.c
@@ -6758,7 +6758,7 @@ static s32 e1000_get_cable_length(struct e1000_hw *hw, u16 *min_length,
  * returns: - E1000_ERR_XXX
  *            E1000_SUCCESS
  *
- * For phy's older then IGP, this function simply reads the polarity bit in the
+ * For phy's older than IGP, this function simply reads the polarity bit in the
  * Phy Status register.  For IGP phy's, this bit is valid only if link speed is
  * 10 Mbps.  If the link speed is 100 Mbps there is no polarity so this bit will
  * return 0.  If the link speed is 1000 Mbps the polarity status is in the
@@ -6834,7 +6834,7 @@ static s32 e1000_check_polarity(struct e1000_hw *hw,
  * returns: - E1000_ERR_XXX
  *            E1000_SUCCESS
  *
- * For phy's older then IGP, this function reads the Downshift bit in the Phy
+ * For phy's older than IGP, this function reads the Downshift bit in the Phy
  * Specific Status register.  For IGP phy's, it reads the Downgrade bit in the
  * Link Health register.  In IGP this bit is latched high, so the driver must
  * read it immediately after link is established.
diff --git a/drivers/net/e1000e/netdev.c b/drivers/net/e1000e/netdev.c
index d4639facd1bd..91817d0afcaf 100644
--- a/drivers/net/e1000e/netdev.c
+++ b/drivers/net/e1000e/netdev.c
@@ -4807,7 +4807,7 @@ static int __devinit e1000_probe(struct pci_dev *pdev,
 		}
 	}
 
-	err = pci_request_selected_regions(pdev,
+	err = pci_request_selected_regions_exclusive(pdev,
 	                                  pci_select_bars(pdev, IORESOURCE_MEM),
 	                                  e1000e_driver_name);
 	if (err)
diff --git a/drivers/net/sfc/falcon.c b/drivers/net/sfc/falcon.c
index 6884dc8c1f82..5b9f2d9cc4ed 100644
--- a/drivers/net/sfc/falcon.c
+++ b/drivers/net/sfc/falcon.c
@@ -1403,9 +1403,9 @@ static irqreturn_t falcon_fatal_interrupt(struct efx_nic *efx)
 	}
 
 	/* Disable both devices */
-	pci_disable_device(efx->pci_dev);
+	pci_clear_master(efx->pci_dev);
 	if (FALCON_IS_DUAL_FUNC(efx))
-		pci_disable_device(nic_data->pci_dev2);
+		pci_clear_master(nic_data->pci_dev2);
 	falcon_disable_interrupts(efx);
 
 	if (++n_int_errors < FALCON_MAX_INT_ERRORS) {
diff --git a/drivers/net/slip.h b/drivers/net/slip.h
index 853e0f6ec710..9ea5c11287d2 100644
--- a/drivers/net/slip.h
+++ b/drivers/net/slip.h
@@ -75,7 +75,7 @@ struct slip {
   unsigned long         tx_errors;      /* Planned stuff                */
   unsigned long         rx_dropped;     /* No memory for skb            */
   unsigned long         tx_dropped;     /* When MTU change              */
-  unsigned long         rx_over_errors; /* Frame bigger then SLIP buf.  */
+  unsigned long         rx_over_errors; /* Frame bigger than SLIP buf.  */
 #ifdef SL_INCLUDE_CSLIP
   unsigned long		tx_compressed;
   unsigned long		rx_compressed;
diff --git a/drivers/net/tehuti.c b/drivers/net/tehuti.c
index a10a83a11d9f..a7a4dc4d6313 100644
--- a/drivers/net/tehuti.c
+++ b/drivers/net/tehuti.c
@@ -1004,7 +1004,7 @@ static inline void bdx_rxdb_free_elem(struct rxdb *db, int n)
  * skb for rx. It assumes that Rx is desabled in HW
  * funcs are grouped for better cache usage
  *
- * RxD fifo is smaller then RxF fifo by design. Upon high load, RxD will be
+ * RxD fifo is smaller than RxF fifo by design. Upon high load, RxD will be
  * filled and packets will be dropped by nic without getting into host or
  * cousing interrupt. Anyway, in that condition, host has no chance to proccess
  * all packets, but dropping in nic is cheaper, since it takes 0 cpu cycles
@@ -1826,7 +1826,7 @@ static void bdx_tx_free(struct bdx_priv *priv)
  *
  * Pushes desc to TxD fifo and overlaps it if needed.
  * NOTE: this func does not check for available space. this is responsibility
- *    of the caller. Neither does it check that data size is smaller then
+ *    of the caller. Neither does it check that data size is smaller than
  *    fifo size.
  */
 static void bdx_tx_push_desc(struct bdx_priv *priv, void *data, int size)
diff --git a/drivers/net/tokenring/smctr.c b/drivers/net/tokenring/smctr.c
index a011666342ff..50eb29ce3c87 100644
--- a/drivers/net/tokenring/smctr.c
+++ b/drivers/net/tokenring/smctr.c
@@ -3064,7 +3064,7 @@ static int smctr_load_node_addr(struct net_device *dev)
  * will consequently cause a timeout.
  *
  * NOTE 1: If the monitor_state is MS_BEACON_TEST_STATE, all transmit
- * queues other then the one used for the lobe_media_test should be
+ * queues other than the one used for the lobe_media_test should be
  * disabled.!?
  *
  * NOTE 2: If the monitor_state is MS_BEACON_TEST_STATE and the receive_mask
diff --git a/drivers/net/usb/kaweth.c b/drivers/net/usb/kaweth.c
index 2ee034f70d1c..3073ca25a0b0 100644
--- a/drivers/net/usb/kaweth.c
+++ b/drivers/net/usb/kaweth.c
@@ -283,9 +283,9 @@ static int kaweth_control(struct kaweth_device *kaweth,
 
 	dr->bRequestType= requesttype;
 	dr->bRequest = request;
-	dr->wValue = cpu_to_le16p(&value);
-	dr->wIndex = cpu_to_le16p(&index);
-	dr->wLength = cpu_to_le16p(&size);
+	dr->wValue = cpu_to_le16(value);
+	dr->wIndex = cpu_to_le16(index);
+	dr->wLength = cpu_to_le16(size);
 
 	return kaweth_internal_control_msg(kaweth->dev,
 					pipe,
diff --git a/drivers/net/usb/pegasus.c b/drivers/net/usb/pegasus.c
index 166880c113d6..d9241f1c0803 100644
--- a/drivers/net/usb/pegasus.c
+++ b/drivers/net/usb/pegasus.c
@@ -150,8 +150,8 @@ static int get_registers(pegasus_t * pegasus, __u16 indx, __u16 size,
 	pegasus->dr.bRequestType = PEGASUS_REQT_READ;
 	pegasus->dr.bRequest = PEGASUS_REQ_GET_REGS;
 	pegasus->dr.wValue = cpu_to_le16(0);
-	pegasus->dr.wIndex = cpu_to_le16p(&indx);
-	pegasus->dr.wLength = cpu_to_le16p(&size);
+	pegasus->dr.wIndex = cpu_to_le16(indx);
+	pegasus->dr.wLength = cpu_to_le16(size);
 	pegasus->ctrl_urb->transfer_buffer_length = size;
 
 	usb_fill_control_urb(pegasus->ctrl_urb, pegasus->usb,
@@ -208,8 +208,8 @@ static int set_registers(pegasus_t * pegasus, __u16 indx, __u16 size,
 	pegasus->dr.bRequestType = PEGASUS_REQT_WRITE;
 	pegasus->dr.bRequest = PEGASUS_REQ_SET_REGS;
 	pegasus->dr.wValue = cpu_to_le16(0);
-	pegasus->dr.wIndex = cpu_to_le16p(&indx);
-	pegasus->dr.wLength = cpu_to_le16p(&size);
+	pegasus->dr.wIndex = cpu_to_le16(indx);
+	pegasus->dr.wLength = cpu_to_le16(size);
 	pegasus->ctrl_urb->transfer_buffer_length = size;
 
 	usb_fill_control_urb(pegasus->ctrl_urb, pegasus->usb,
@@ -261,7 +261,7 @@ static int set_register(pegasus_t * pegasus, __u16 indx, __u8 data)
 	pegasus->dr.bRequestType = PEGASUS_REQT_WRITE;
 	pegasus->dr.bRequest = PEGASUS_REQ_SET_REG;
 	pegasus->dr.wValue = cpu_to_le16(data);
-	pegasus->dr.wIndex = cpu_to_le16p(&indx);
+	pegasus->dr.wIndex = cpu_to_le16(indx);
 	pegasus->dr.wLength = cpu_to_le16(1);
 	pegasus->ctrl_urb->transfer_buffer_length = 1;
 
@@ -476,7 +476,7 @@ static inline void get_node_id(pegasus_t * pegasus, __u8 * id)
 
 	for (i = 0; i < 3; i++) {
 		read_eprom_word(pegasus, i, &w16);
-		((__le16 *) id)[i] = cpu_to_le16p(&w16);
+		((__le16 *) id)[i] = cpu_to_le16(w16);
 	}
 }
 
diff --git a/drivers/net/wimax/Kconfig b/drivers/net/wimax/Kconfig
new file mode 100644
index 000000000000..565018ec1e3b
--- /dev/null
+++ b/drivers/net/wimax/Kconfig
@@ -0,0 +1,17 @@
+#
+# WiMAX LAN device drivers configuration
+#
+
+
+comment "Enable WiMAX (Networking options) to see the WiMAX drivers"
+	depends on WIMAX = n
+
+if WIMAX
+
+menu "WiMAX Wireless Broadband devices"
+
+source "drivers/net/wimax/i2400m/Kconfig"
+
+endmenu
+
+endif
diff --git a/drivers/net/wimax/Makefile b/drivers/net/wimax/Makefile
new file mode 100644
index 000000000000..992bc02bc016
--- /dev/null
+++ b/drivers/net/wimax/Makefile
@@ -0,0 +1,5 @@
+
+obj-$(CONFIG_WIMAX_I2400M)	+= i2400m/
+
+# (from Sam Ravnborg) force kbuild to create built-in.o
+obj- := dummy.o
diff --git a/drivers/net/wimax/i2400m/Kconfig b/drivers/net/wimax/i2400m/Kconfig
new file mode 100644
index 000000000000..d623b3d99a4b
--- /dev/null
+++ b/drivers/net/wimax/i2400m/Kconfig
@@ -0,0 +1,49 @@
+
+config WIMAX_I2400M
+	tristate
+	depends on WIMAX
+	select FW_LOADER
+
+comment "Enable USB support to see WiMAX USB drivers"
+	depends on USB = n
+
+comment "Enable MMC support to see WiMAX SDIO drivers"
+	depends on MMC = n
+
+config WIMAX_I2400M_USB
+	tristate "Intel Wireless WiMAX Connection 2400 over USB (including 5x50)"
+	depends on WIMAX && USB
+	select WIMAX_I2400M
+	help
+	  Select if you have a device based on the Intel WiMAX
+	  Connection 2400 over USB (like any of the Intel Wireless
+	  WiMAX/WiFi Link 5x50 series).
+
+	  If unsure, it is safe to select M (module).
+
+config WIMAX_I2400M_SDIO
+	tristate "Intel Wireless WiMAX Connection 2400 over SDIO"
+	depends on WIMAX && MMC
+	select WIMAX_I2400M
+	help
+	  Select if you have a device based on the Intel WiMAX
+	  Connection 2400 over SDIO.
+
+	  If unsure, it is safe to select M (module).
+
+config WIMAX_I2400M_DEBUG_LEVEL
+	int "WiMAX i2400m debug level"
+	depends on WIMAX_I2400M
+	default 8
+	help
+
+	  Select the maximum debug verbosity level to be compiled into
+	  the WiMAX i2400m driver code.
+
+	  By default, this is disabled at runtime and can be
+	  selectively enabled at runtime for different parts of the
+	  code using the sysfs debug-levels file.
+
+	  If set at zero, this will compile out all the debug code.
+
+	  It is recommended that it is left at 8.
diff --git a/drivers/net/wimax/i2400m/Makefile b/drivers/net/wimax/i2400m/Makefile
new file mode 100644
index 000000000000..1696e936cf5a
--- /dev/null
+++ b/drivers/net/wimax/i2400m/Makefile
@@ -0,0 +1,29 @@
+
+obj-$(CONFIG_WIMAX_I2400M) += i2400m.o
+obj-$(CONFIG_WIMAX_I2400M_USB) += i2400m-usb.o
+obj-$(CONFIG_WIMAX_I2400M_SDIO) += i2400m-sdio.o
+
+i2400m-y :=		\
+	control.o	\
+	driver.o	\
+	fw.o		\
+	op-rfkill.o	\
+	netdev.o	\
+	tx.o		\
+	rx.o
+
+i2400m-$(CONFIG_DEBUG_FS) += debugfs.o
+
+i2400m-usb-y :=			\
+	usb-fw.o		\
+	usb-notif.o		\
+	usb-tx.o		\
+	usb-rx.o		\
+	usb.o
+
+
+i2400m-sdio-y := 		\
+	sdio.o      		\
+	sdio-tx.o   		\
+	sdio-fw.o	 	\
+	sdio-rx.o
diff --git a/drivers/net/wimax/i2400m/control.c b/drivers/net/wimax/i2400m/control.c
new file mode 100644
index 000000000000..d3d37fed6893
--- /dev/null
+++ b/drivers/net/wimax/i2400m/control.c
@@ -0,0 +1,1291 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * Miscellaneous control functions for managing the device
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ *  - Initial implementation
+ *
+ * This is a collection of functions used to control the device (plus
+ * a few helpers).
+ *
+ * There are utilities for handling TLV buffers, hooks on the device's
+ * reports to act on device changes of state [i2400m_report_hook()],
+ * on acks to commands [i2400m_msg_ack_hook()], a helper for sending
+ * commands to the device and blocking until a reply arrives
+ * [i2400m_msg_to_dev()], a few high level commands for manipulating
+ * the device state, powersving mode and configuration plus the
+ * routines to setup the device once communication is stablished with
+ * it [i2400m_dev_initialize()].
+ *
+ * ROADMAP
+ *
+ * i2400m_dev_initalize()       Called by i2400m_dev_start()
+ *   i2400m_set_init_config()
+ *   i2400m_firmware_check()
+ *   i2400m_cmd_get_state()
+ * i2400m_dev_shutdown()        Called by i2400m_dev_stop()
+ *   i2400m->bus_reset()
+ *
+ * i2400m_{cmd,get,set}_*()
+ *   i2400m_msg_to_dev()
+ *   i2400m_msg_check_status()
+ *
+ * i2400m_report_hook()         Called on reception of an event
+ *   i2400m_report_state_hook()
+ *     i2400m_tlv_buffer_walk()
+ *     i2400m_tlv_match()
+ *     i2400m_report_tlv_system_state()
+ *     i2400m_report_tlv_rf_switches_status()
+ *     i2400m_report_tlv_media_status()
+ *   i2400m_cmd_enter_powersave()
+ *
+ * i2400m_msg_ack_hook()        Called on reception of a reply to a
+ *                              command, get or set
+ */
+
+#include <stdarg.h>
+#include "i2400m.h"
+#include <linux/kernel.h>
+#include <linux/wimax/i2400m.h>
+
+
+#define D_SUBMODULE control
+#include "debug-levels.h"
+
+
+/*
+ * Return if a TLV is of a give type and size
+ *
+ * @tlv_hdr: pointer to the TLV
+ * @tlv_type: type of the TLV we are looking for
+ * @tlv_size: expected size of the TLV we are looking for (if -1,
+ *            don't check the size). This includes the header
+ * Returns: 0 if the TLV matches
+ *          < 0 if it doesn't match at all
+ *          > 0 total TLV + payload size, if the type matches, but not
+ *              the size
+ */
+static
+ssize_t i2400m_tlv_match(const struct i2400m_tlv_hdr *tlv,
+		     enum i2400m_tlv tlv_type, ssize_t tlv_size)
+{
+	if (le16_to_cpu(tlv->type) != tlv_type)	/* Not our type? skip */
+		return -1;
+	if (tlv_size != -1
+	    && le16_to_cpu(tlv->length) + sizeof(*tlv) != tlv_size) {
+		size_t size = le16_to_cpu(tlv->length) + sizeof(*tlv);
+		printk(KERN_WARNING "W: tlv type 0x%x mismatched because of "
+		       "size (got %zu vs %zu expected)\n",
+		       tlv_type, size, tlv_size);
+		return size;
+	}
+	return 0;
+}
+
+
+/*
+ * Given a buffer of TLVs, iterate over them
+ *
+ * @i2400m: device instance
+ * @tlv_buf: pointer to the beginning of the TLV buffer
+ * @buf_size: buffer size in bytes
+ * @tlv_pos: seek position; this is assumed to be a pointer returned
+ *           by i2400m_tlv_buffer_walk() [and thus, validated]. The
+ *           TLV returned will be the one following this one.
+ *
+ * Usage:
+ *
+ * tlv_itr = NULL;
+ * while (tlv_itr = i2400m_tlv_buffer_walk(i2400m, buf, size, tlv_itr))  {
+ *         ...
+ *         // Do stuff with tlv_itr, DON'T MODIFY IT
+ *         ...
+ * }
+ */
+static
+const struct i2400m_tlv_hdr *i2400m_tlv_buffer_walk(
+	struct i2400m *i2400m,
+	const void *tlv_buf, size_t buf_size,
+	const struct i2400m_tlv_hdr *tlv_pos)
+{
+	struct device *dev = i2400m_dev(i2400m);
+	const struct i2400m_tlv_hdr *tlv_top = tlv_buf + buf_size;
+	size_t offset, length, avail_size;
+	unsigned type;
+
+	if (tlv_pos == NULL)	/* Take the first one? */
+		tlv_pos = tlv_buf;
+	else			/* Nope, the next one */
+		tlv_pos = (void *) tlv_pos
+			+ le16_to_cpu(tlv_pos->length) + sizeof(*tlv_pos);
+	if (tlv_pos == tlv_top) {	/* buffer done */
+		tlv_pos = NULL;
+		goto error_beyond_end;
+	}
+	if (tlv_pos > tlv_top) {
+		tlv_pos = NULL;
+		WARN_ON(1);
+		goto error_beyond_end;
+	}
+	offset = (void *) tlv_pos - (void *) tlv_buf;
+	avail_size = buf_size - offset;
+	if (avail_size < sizeof(*tlv_pos)) {
+		dev_err(dev, "HW BUG? tlv_buf %p [%zu bytes], tlv @%zu: "
+			"short header\n", tlv_buf, buf_size, offset);
+		goto error_short_header;
+	}
+	type = le16_to_cpu(tlv_pos->type);
+	length = le16_to_cpu(tlv_pos->length);
+	if (avail_size < sizeof(*tlv_pos) + length) {
+		dev_err(dev, "HW BUG? tlv_buf %p [%zu bytes], "
+			"tlv type 0x%04x @%zu: "
+			"short data (%zu bytes vs %zu needed)\n",
+			tlv_buf, buf_size, type, offset, avail_size,
+			sizeof(*tlv_pos) + length);
+		goto error_short_header;
+	}
+error_short_header:
+error_beyond_end:
+	return tlv_pos;
+}
+
+
+/*
+ * Find a TLV in a buffer of sequential TLVs
+ *
+ * @i2400m: device descriptor
+ * @tlv_hdr: pointer to the first TLV in the sequence
+ * @size: size of the buffer in bytes; all TLVs are assumed to fit
+ *        fully in the buffer (otherwise we'll complain).
+ * @tlv_type: type of the TLV we are looking for
+ * @tlv_size: expected size of the TLV we are looking for (if -1,
+ *            don't check the size). This includes the header
+ *
+ * Returns: NULL if the TLV is not found, otherwise a pointer to
+ *          it. If the sizes don't match, an error is printed and NULL
+ *          returned.
+ */
+static
+const struct i2400m_tlv_hdr *i2400m_tlv_find(
+	struct i2400m *i2400m,
+	const struct i2400m_tlv_hdr *tlv_hdr, size_t size,
+	enum i2400m_tlv tlv_type, ssize_t tlv_size)
+{
+	ssize_t match;
+	struct device *dev = i2400m_dev(i2400m);
+	const struct i2400m_tlv_hdr *tlv = NULL;
+	while ((tlv = i2400m_tlv_buffer_walk(i2400m, tlv_hdr, size, tlv))) {
+		match = i2400m_tlv_match(tlv, tlv_type, tlv_size);
+		if (match == 0)		/* found it :) */
+			break;
+		if (match > 0)
+			dev_warn(dev, "TLV type 0x%04x found with size "
+				 "mismatch (%zu vs %zu needed)\n",
+				 tlv_type, match, tlv_size);
+	}
+	return tlv;
+}
+
+
+static const struct
+{
+	char *msg;
+	int errno;
+} ms_to_errno[I2400M_MS_MAX] = {
+	[I2400M_MS_DONE_OK] = { "", 0 },
+	[I2400M_MS_DONE_IN_PROGRESS] = { "", 0 },
+	[I2400M_MS_INVALID_OP] = { "invalid opcode", -ENOSYS },
+	[I2400M_MS_BAD_STATE] = { "invalid state", -EILSEQ },
+	[I2400M_MS_ILLEGAL_VALUE] = { "illegal value", -EINVAL },
+	[I2400M_MS_MISSING_PARAMS] = { "missing parameters", -ENOMSG },
+	[I2400M_MS_VERSION_ERROR] = { "bad version", -EIO },
+	[I2400M_MS_ACCESSIBILITY_ERROR] = { "accesibility error", -EIO },
+	[I2400M_MS_BUSY] = { "busy", -EBUSY },
+	[I2400M_MS_CORRUPTED_TLV] = { "corrupted TLV", -EILSEQ },
+	[I2400M_MS_UNINITIALIZED] = { "not unitialized", -EILSEQ },
+	[I2400M_MS_UNKNOWN_ERROR] = { "unknown error", -EIO },
+	[I2400M_MS_PRODUCTION_ERROR] = { "production error", -EIO },
+	[I2400M_MS_NO_RF] = { "no RF", -EIO },
+	[I2400M_MS_NOT_READY_FOR_POWERSAVE] =
+		{ "not ready for powersave", -EACCES },
+	[I2400M_MS_THERMAL_CRITICAL] = { "thermal critical", -EL3HLT },
+};
+
+
+/*
+ * i2400m_msg_check_status - translate a message's status code
+ *
+ * @i2400m: device descriptor
+ * @l3l4_hdr: message header
+ * @strbuf: buffer to place a formatted error message (unless NULL).
+ * @strbuf_size: max amount of available space; larger messages will
+ * be truncated.
+ *
+ * Returns: errno code corresponding to the status code in @l3l4_hdr
+ *          and a message in @strbuf describing the error.
+ */
+int i2400m_msg_check_status(const struct i2400m_l3l4_hdr *l3l4_hdr,
+			    char *strbuf, size_t strbuf_size)
+{
+	int result;
+	enum i2400m_ms status = le16_to_cpu(l3l4_hdr->status);
+	const char *str;
+
+	if (status == 0)
+		return 0;
+	if (status > ARRAY_SIZE(ms_to_errno)) {
+		str = "unknown status code";
+		result = -EBADR;
+	} else {
+		str = ms_to_errno[status].msg;
+		result = ms_to_errno[status].errno;
+	}
+	if (strbuf)
+		snprintf(strbuf, strbuf_size, "%s (%d)", str, status);
+	return result;
+}
+
+
+/*
+ * Act on a TLV System State reported by the device
+ *
+ * @i2400m: device descriptor
+ * @ss: validated System State TLV
+ */
+static
+void i2400m_report_tlv_system_state(struct i2400m *i2400m,
+				    const struct i2400m_tlv_system_state *ss)
+{
+	struct device *dev = i2400m_dev(i2400m);
+	struct wimax_dev *wimax_dev = &i2400m->wimax_dev;
+	enum i2400m_system_state i2400m_state = le32_to_cpu(ss->state);
+
+	d_fnstart(3, dev, "(i2400m %p ss %p [%u])\n", i2400m, ss, i2400m_state);
+
+	if (unlikely(i2400m->ready == 0))	/* act if up */
+		goto out;
+	if (i2400m->state != i2400m_state) {
+		i2400m->state = i2400m_state;
+		wake_up_all(&i2400m->state_wq);
+	}
+	switch (i2400m_state) {
+	case I2400M_SS_UNINITIALIZED:
+	case I2400M_SS_INIT:
+	case I2400M_SS_CONFIG:
+	case I2400M_SS_PRODUCTION:
+		wimax_state_change(wimax_dev, WIMAX_ST_UNINITIALIZED);
+		break;
+
+	case I2400M_SS_RF_OFF:
+	case I2400M_SS_RF_SHUTDOWN:
+		wimax_state_change(wimax_dev, WIMAX_ST_RADIO_OFF);
+		break;
+
+	case I2400M_SS_READY:
+	case I2400M_SS_STANDBY:
+	case I2400M_SS_SLEEPACTIVE:
+		wimax_state_change(wimax_dev, WIMAX_ST_READY);
+		break;
+
+	case I2400M_SS_CONNECTING:
+	case I2400M_SS_WIMAX_CONNECTED:
+		wimax_state_change(wimax_dev, WIMAX_ST_READY);
+		break;
+
+	case I2400M_SS_SCAN:
+	case I2400M_SS_OUT_OF_ZONE:
+		wimax_state_change(wimax_dev, WIMAX_ST_SCANNING);
+		break;
+
+	case I2400M_SS_IDLE:
+		d_printf(1, dev, "entering BS-negotiated idle mode\n");
+	case I2400M_SS_DISCONNECTING:
+	case I2400M_SS_DATA_PATH_CONNECTED:
+		wimax_state_change(wimax_dev, WIMAX_ST_CONNECTED);
+		break;
+
+	default:
+		/* Huh? just in case, shut it down */
+		dev_err(dev, "HW BUG? unknown state %u: shutting down\n",
+			i2400m_state);
+		i2400m->bus_reset(i2400m, I2400M_RT_WARM);
+		break;
+	};
+out:
+	d_fnend(3, dev, "(i2400m %p ss %p [%u]) = void\n",
+		i2400m, ss, i2400m_state);
+}
+
+
+/*
+ * Parse and act on a TLV Media Status sent by the device
+ *
+ * @i2400m: device descriptor
+ * @ms: validated Media Status TLV
+ *
+ * This will set the carrier up on down based on the device's link
+ * report. This is done asides of what the WiMAX stack does based on
+ * the device's state as sometimes we need to do a link-renew (the BS
+ * wants us to renew a DHCP lease, for example).
+ *
+ * In fact, doc says that everytime we get a link-up, we should do a
+ * DHCP negotiation...
+ */
+static
+void i2400m_report_tlv_media_status(struct i2400m *i2400m,
+				    const struct i2400m_tlv_media_status *ms)
+{
+	struct device *dev = i2400m_dev(i2400m);
+	struct wimax_dev *wimax_dev = &i2400m->wimax_dev;
+	struct net_device *net_dev = wimax_dev->net_dev;
+	enum i2400m_media_status status = le32_to_cpu(ms->media_status);
+
+	d_fnstart(3, dev, "(i2400m %p ms %p [%u])\n", i2400m, ms, status);
+
+	if (unlikely(i2400m->ready == 0))	/* act if up */
+		goto out;
+	switch (status) {
+	case I2400M_MEDIA_STATUS_LINK_UP:
+		netif_carrier_on(net_dev);
+		break;
+	case I2400M_MEDIA_STATUS_LINK_DOWN:
+		netif_carrier_off(net_dev);
+		break;
+	/*
+	 * This is the network telling us we need to retrain the DHCP
+	 * lease -- so far, we are trusting the WiMAX Network Service
+	 * in user space to pick this up and poke the DHCP client.
+	 */
+	case I2400M_MEDIA_STATUS_LINK_RENEW:
+		netif_carrier_on(net_dev);
+		break;
+	default:
+		dev_err(dev, "HW BUG? unknown media status %u\n",
+			status);
+	};
+out:
+	d_fnend(3, dev, "(i2400m %p ms %p [%u]) = void\n",
+		i2400m, ms, status);
+}
+
+
+/*
+ * Parse a 'state report' and extract carrier on/off information
+ *
+ * @i2400m: device descriptor
+ * @l3l4_hdr: pointer to message; it has been already validated for
+ *            consistent size.
+ * @size: size of the message (header + payload). The header length
+ *        declaration is assumed to be congruent with @size (as in
+ *        sizeof(*l3l4_hdr) + l3l4_hdr->length == size)
+ *
+ * Extract from the report state the system state TLV and infer from
+ * there if we have a carrier or not. Update our local state and tell
+ * netdev.
+ *
+ * When setting the carrier, it's fine to set OFF twice (for example),
+ * as netif_carrier_off() will not generate two OFF events (just on
+ * the transitions).
+ */
+static
+void i2400m_report_state_hook(struct i2400m *i2400m,
+			      const struct i2400m_l3l4_hdr *l3l4_hdr,
+			      size_t size, const char *tag)
+{
+	struct device *dev = i2400m_dev(i2400m);
+	const struct i2400m_tlv_hdr *tlv;
+	const struct i2400m_tlv_system_state *ss;
+	const struct i2400m_tlv_rf_switches_status *rfss;
+	const struct i2400m_tlv_media_status *ms;
+	size_t tlv_size = le16_to_cpu(l3l4_hdr->length);
+
+	d_fnstart(4, dev, "(i2400m %p, l3l4_hdr %p, size %zu, %s)\n",
+		  i2400m, l3l4_hdr, size, tag);
+	tlv = NULL;
+
+	while ((tlv = i2400m_tlv_buffer_walk(i2400m, &l3l4_hdr->pl,
+					     tlv_size, tlv))) {
+		if (0 == i2400m_tlv_match(tlv, I2400M_TLV_SYSTEM_STATE,
+					  sizeof(*ss))) {
+			ss = container_of(tlv, typeof(*ss), hdr);
+			d_printf(2, dev, "%s: system state TLV "
+				 "found (0x%04x), state 0x%08x\n",
+				 tag, I2400M_TLV_SYSTEM_STATE,
+				 le32_to_cpu(ss->state));
+			i2400m_report_tlv_system_state(i2400m, ss);
+		}
+		if (0 == i2400m_tlv_match(tlv, I2400M_TLV_RF_STATUS,
+					  sizeof(*rfss))) {
+			rfss = container_of(tlv, typeof(*rfss), hdr);
+			d_printf(2, dev, "%s: RF status TLV "
+				 "found (0x%04x), sw 0x%02x hw 0x%02x\n",
+				 tag, I2400M_TLV_RF_STATUS,
+				 le32_to_cpu(rfss->sw_rf_switch),
+				 le32_to_cpu(rfss->hw_rf_switch));
+			i2400m_report_tlv_rf_switches_status(i2400m, rfss);
+		}
+		if (0 == i2400m_tlv_match(tlv, I2400M_TLV_MEDIA_STATUS,
+					  sizeof(*ms))) {
+			ms = container_of(tlv, typeof(*ms), hdr);
+			d_printf(2, dev, "%s: Media Status TLV: %u\n",
+				 tag, le32_to_cpu(ms->media_status));
+			i2400m_report_tlv_media_status(i2400m, ms);
+		}
+	}
+	d_fnend(4, dev, "(i2400m %p, l3l4_hdr %p, size %zu, %s) = void\n",
+		i2400m, l3l4_hdr, size, tag);
+}
+
+
+/*
+ * i2400m_report_hook - (maybe) act on a report
+ *
+ * @i2400m: device descriptor
+ * @l3l4_hdr: pointer to message; it has been already validated for
+ *            consistent size.
+ * @size: size of the message (header + payload). The header length
+ *        declaration is assumed to be congruent with @size (as in
+ *        sizeof(*l3l4_hdr) + l3l4_hdr->length == size)
+ *
+ * Extract information we might need (like carrien on/off) from a
+ * device report.
+ */
+void i2400m_report_hook(struct i2400m *i2400m,
+			const struct i2400m_l3l4_hdr *l3l4_hdr, size_t size)
+{
+	struct device *dev = i2400m_dev(i2400m);
+	unsigned msg_type;
+
+	d_fnstart(3, dev, "(i2400m %p l3l4_hdr %p size %zu)\n",
+		  i2400m, l3l4_hdr, size);
+	/* Chew on the message, we might need some information from
+	 * here */
+	msg_type = le16_to_cpu(l3l4_hdr->type);
+	switch (msg_type) {
+	case I2400M_MT_REPORT_STATE:	/* carrier detection... */
+		i2400m_report_state_hook(i2400m,
+					 l3l4_hdr, size, "REPORT STATE");
+		break;
+	/* If the device is ready for power save, then ask it to do
+	 * it. */
+	case I2400M_MT_REPORT_POWERSAVE_READY:	/* zzzzz */
+		if (l3l4_hdr->status == cpu_to_le16(I2400M_MS_DONE_OK)) {
+			d_printf(1, dev, "ready for powersave, requesting\n");
+			i2400m_cmd_enter_powersave(i2400m);
+		}
+		break;
+	};
+	d_fnend(3, dev, "(i2400m %p l3l4_hdr %p size %zu) = void\n",
+		i2400m, l3l4_hdr, size);
+}
+
+
+/*
+ * i2400m_msg_ack_hook - process cmd/set/get ack for internal status
+ *
+ * @i2400m: device descriptor
+ * @l3l4_hdr: pointer to message; it has been already validated for
+ *            consistent size.
+ * @size: size of the message
+ *
+ * Extract information we might need from acks to commands and act on
+ * it. This is akin to i2400m_report_hook(). Note most of this
+ * processing should be done in the function that calls the
+ * command. This is here for some cases where it can't happen...
+ */
+void i2400m_msg_ack_hook(struct i2400m *i2400m,
+			 const struct i2400m_l3l4_hdr *l3l4_hdr, size_t size)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	unsigned ack_type, ack_status;
+	char strerr[32];
+
+	/* Chew on the message, we might need some information from
+	 * here */
+	ack_type = le16_to_cpu(l3l4_hdr->type);
+	ack_status = le16_to_cpu(l3l4_hdr->status);
+	switch (ack_type) {
+	case I2400M_MT_CMD_ENTER_POWERSAVE:
+		/* This is just left here for the sake of example, as
+		 * the processing is done somewhere else. */
+		if (0) {
+			result = i2400m_msg_check_status(
+				l3l4_hdr, strerr, sizeof(strerr));
+			if (result >= 0)
+				d_printf(1, dev, "ready for power save: %zd\n",
+					 size);
+		}
+		break;
+	};
+	return;
+}
+
+
+/*
+ * i2400m_msg_size_check() - verify message size and header are congruent
+ *
+ * It is ok if the total message size is larger than the expected
+ * size, as there can be padding.
+ */
+int i2400m_msg_size_check(struct i2400m *i2400m,
+			  const struct i2400m_l3l4_hdr *l3l4_hdr,
+			  size_t msg_size)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	size_t expected_size;
+	d_fnstart(4, dev, "(i2400m %p l3l4_hdr %p msg_size %zu)\n",
+		  i2400m, l3l4_hdr, msg_size);
+	if (msg_size < sizeof(*l3l4_hdr)) {
+		dev_err(dev, "bad size for message header "
+			"(expected at least %zu, got %zu)\n",
+			(size_t) sizeof(*l3l4_hdr), msg_size);
+		result = -EIO;
+		goto error_hdr_size;
+	}
+	expected_size = le16_to_cpu(l3l4_hdr->length) + sizeof(*l3l4_hdr);
+	if (msg_size < expected_size) {
+		dev_err(dev, "bad size for message code 0x%04x (expected %zu, "
+			"got %zu)\n", le16_to_cpu(l3l4_hdr->type),
+			expected_size, msg_size);
+		result = -EIO;
+	} else
+		result = 0;
+error_hdr_size:
+	d_fnend(4, dev,
+		"(i2400m %p l3l4_hdr %p msg_size %zu) = %d\n",
+		i2400m, l3l4_hdr, msg_size, result);
+	return result;
+}
+
+
+
+/*
+ * Cancel a wait for a command ACK
+ *
+ * @i2400m: device descriptor
+ * @code: [negative] errno code to cancel with (don't use
+ *     -EINPROGRESS)
+ *
+ * If there is an ack already filled out, free it.
+ */
+void i2400m_msg_to_dev_cancel_wait(struct i2400m *i2400m, int code)
+{
+	struct sk_buff *ack_skb;
+	unsigned long flags;
+
+	spin_lock_irqsave(&i2400m->rx_lock, flags);
+	ack_skb = i2400m->ack_skb;
+	if (ack_skb && !IS_ERR(ack_skb))
+		kfree(ack_skb);
+	i2400m->ack_skb = ERR_PTR(code);
+	spin_unlock_irqrestore(&i2400m->rx_lock, flags);
+}
+
+
+/**
+ * i2400m_msg_to_dev - Send a control message to the device and get a response
+ *
+ * @i2400m: device descriptor
+ *
+ * @msg_skb: an skb  *
+ *
+ * @buf: pointer to the buffer containing the message to be sent; it
+ *           has to start with a &struct i2400M_l3l4_hdr and then
+ *           followed by the payload. Once this function returns, the
+ *           buffer can be reused.
+ *
+ * @buf_len: buffer size
+ *
+ * Returns:
+ *
+ * Pointer to skb containing the ack message. You need to check the
+ * pointer with IS_ERR(), as it might be an error code. Error codes
+ * could happen because:
+ *
+ *  - the message wasn't formatted correctly
+ *  - couldn't send the message
+ *  - failed waiting for a response
+ *  - the ack message wasn't formatted correctly
+ *
+ * The returned skb has been allocated with wimax_msg_to_user_alloc(),
+ * it contains the reponse in a netlink attribute and is ready to be
+ * passed up to user space with wimax_msg_to_user_send(). To access
+ * the payload and its length, use wimax_msg_{data,len}() on the skb.
+ *
+ * The skb has to be freed with kfree_skb() once done.
+ *
+ * Description:
+ *
+ * This function delivers a message/command to the device and waits
+ * for an ack to be received. The format is described in
+ * linux/wimax/i2400m.h. In summary, a command/get/set is followed by an
+ * ack.
+ *
+ * This function will not check the ack status, that's left up to the
+ * caller.  Once done with the ack skb, it has to be kfree_skb()ed.
+ *
+ * The i2400m handles only one message at the same time, thus we need
+ * the mutex to exclude other players.
+ *
+ * We write the message and then wait for an answer to come back. The
+ * RX path intercepts control messages and handles them in
+ * i2400m_rx_ctl(). Reports (notifications) are (maybe) processed
+ * locally and then forwarded (as needed) to user space on the WiMAX
+ * stack message pipe. Acks are saved and passed back to us through an
+ * skb in i2400m->ack_skb which is ready to be given to generic
+ * netlink if need be.
+ */
+struct sk_buff *i2400m_msg_to_dev(struct i2400m *i2400m,
+				  const void *buf, size_t buf_len)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	const struct i2400m_l3l4_hdr *msg_l3l4_hdr;
+	struct sk_buff *ack_skb;
+	const struct i2400m_l3l4_hdr *ack_l3l4_hdr;
+	size_t ack_len;
+	int ack_timeout;
+	unsigned msg_type;
+	unsigned long flags;
+
+	d_fnstart(3, dev, "(i2400m %p buf %p len %zu)\n",
+		  i2400m, buf, buf_len);
+
+	if (i2400m->boot_mode)
+		return ERR_PTR(-ENODEV);
+
+	msg_l3l4_hdr = buf;
+	/* Check msg & payload consistency */
+	result = i2400m_msg_size_check(i2400m, msg_l3l4_hdr, buf_len);
+	if (result < 0)
+		goto error_bad_msg;
+	msg_type = le16_to_cpu(msg_l3l4_hdr->type);
+	d_printf(1, dev, "CMD/GET/SET 0x%04x %zu bytes\n",
+		 msg_type, buf_len);
+	d_dump(2, dev, buf, buf_len);
+
+	/* Setup the completion, ack_skb ("we are waiting") and send
+	 * the message to the device */
+	mutex_lock(&i2400m->msg_mutex);
+	spin_lock_irqsave(&i2400m->rx_lock, flags);
+	i2400m->ack_skb = ERR_PTR(-EINPROGRESS);
+	spin_unlock_irqrestore(&i2400m->rx_lock, flags);
+	init_completion(&i2400m->msg_completion);
+	result = i2400m_tx(i2400m, buf, buf_len, I2400M_PT_CTRL);
+	if (result < 0) {
+		dev_err(dev, "can't send message 0x%04x: %d\n",
+			le16_to_cpu(msg_l3l4_hdr->type), result);
+		goto error_tx;
+	}
+
+	/* Some commands take longer to execute because of crypto ops,
+	 * so we give them some more leeway on timeout */
+	switch (msg_type) {
+	case I2400M_MT_GET_TLS_OPERATION_RESULT:
+	case I2400M_MT_CMD_SEND_EAP_RESPONSE:
+		ack_timeout = 5 * HZ;
+		break;
+	default:
+		ack_timeout = HZ;
+	};
+
+	/* The RX path in rx.c will put any response for this message
+	 * in i2400m->ack_skb and wake us up. If we cancel the wait,
+	 * we need to change the value of i2400m->ack_skb to something
+	 * not -EINPROGRESS so RX knows there is no one waiting. */
+	result = wait_for_completion_interruptible_timeout(
+		&i2400m->msg_completion, ack_timeout);
+	if (result == 0) {
+		dev_err(dev, "timeout waiting for reply to message 0x%04x\n",
+			msg_type);
+		result = -ETIMEDOUT;
+		i2400m_msg_to_dev_cancel_wait(i2400m, result);
+		goto error_wait_for_completion;
+	} else if (result < 0) {
+		dev_err(dev, "error waiting for reply to message 0x%04x: %d\n",
+			msg_type, result);
+		i2400m_msg_to_dev_cancel_wait(i2400m, result);
+		goto error_wait_for_completion;
+	}
+
+	/* Pull out the ack data from i2400m->ack_skb -- see if it is
+	 * an error and act accordingly */
+	spin_lock_irqsave(&i2400m->rx_lock, flags);
+	ack_skb = i2400m->ack_skb;
+	if (IS_ERR(ack_skb))
+		result = PTR_ERR(ack_skb);
+	else
+		result = 0;
+	i2400m->ack_skb = NULL;
+	spin_unlock_irqrestore(&i2400m->rx_lock, flags);
+	if (result < 0)
+		goto error_ack_status;
+	ack_l3l4_hdr = wimax_msg_data_len(ack_skb, &ack_len);
+
+	/* Check the ack and deliver it if it is ok */
+	result = i2400m_msg_size_check(i2400m, ack_l3l4_hdr, ack_len);
+	if (result < 0) {
+		dev_err(dev, "HW BUG? reply to message 0x%04x: %d\n",
+			msg_type, result);
+		goto error_bad_ack_len;
+	}
+	if (msg_type != le16_to_cpu(ack_l3l4_hdr->type)) {
+		dev_err(dev, "HW BUG? bad reply 0x%04x to message 0x%04x\n",
+			le16_to_cpu(ack_l3l4_hdr->type), msg_type);
+		result = -EIO;
+		goto error_bad_ack_type;
+	}
+	i2400m_msg_ack_hook(i2400m, ack_l3l4_hdr, ack_len);
+	mutex_unlock(&i2400m->msg_mutex);
+	d_fnend(3, dev, "(i2400m %p buf %p len %zu) = %p\n",
+		i2400m, buf, buf_len, ack_skb);
+	return ack_skb;
+
+error_bad_ack_type:
+error_bad_ack_len:
+	kfree_skb(ack_skb);
+error_ack_status:
+error_wait_for_completion:
+error_tx:
+	mutex_unlock(&i2400m->msg_mutex);
+error_bad_msg:
+	d_fnend(3, dev, "(i2400m %p buf %p len %zu) = %d\n",
+		i2400m, buf, buf_len, result);
+	return ERR_PTR(result);
+}
+
+
+/*
+ * Definitions for the Enter Power Save command
+ *
+ * The Enter Power Save command requests the device to go into power
+ * saving mode. The device will ack or nak the command depending on it
+ * being ready for it. If it acks, we tell the USB subsystem to
+ *
+ * As well, the device might request to go into power saving mode by
+ * sending a report (REPORT_POWERSAVE_READY), in which case, we issue
+ * this command. The hookups in the RX coder allow
+ */
+enum {
+	I2400M_WAKEUP_ENABLED  = 0x01,
+	I2400M_WAKEUP_DISABLED = 0x02,
+	I2400M_TLV_TYPE_WAKEUP_MODE = 144,
+};
+
+struct i2400m_cmd_enter_power_save {
+	struct i2400m_l3l4_hdr hdr;
+	struct i2400m_tlv_hdr tlv;
+	__le32 val;
+} __attribute__((packed));
+
+
+/*
+ * Request entering power save
+ *
+ * This command is (mainly) executed when the device indicates that it
+ * is ready to go into powersave mode via a REPORT_POWERSAVE_READY.
+ */
+int i2400m_cmd_enter_powersave(struct i2400m *i2400m)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	struct sk_buff *ack_skb;
+	struct i2400m_cmd_enter_power_save *cmd;
+	char strerr[32];
+
+	result = -ENOMEM;
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (cmd == NULL)
+		goto error_alloc;
+	cmd->hdr.type = cpu_to_le16(I2400M_MT_CMD_ENTER_POWERSAVE);
+	cmd->hdr.length = cpu_to_le16(sizeof(*cmd) - sizeof(cmd->hdr));
+	cmd->hdr.version = cpu_to_le16(I2400M_L3L4_VERSION);
+	cmd->tlv.type = cpu_to_le16(I2400M_TLV_TYPE_WAKEUP_MODE);
+	cmd->tlv.length = cpu_to_le16(sizeof(cmd->val));
+	cmd->val = cpu_to_le32(I2400M_WAKEUP_ENABLED);
+
+	ack_skb = i2400m_msg_to_dev(i2400m, cmd, sizeof(*cmd));
+	result = PTR_ERR(ack_skb);
+	if (IS_ERR(ack_skb)) {
+		dev_err(dev, "Failed to issue 'Enter power save' command: %d\n",
+			result);
+		goto error_msg_to_dev;
+	}
+	result = i2400m_msg_check_status(wimax_msg_data(ack_skb),
+					 strerr, sizeof(strerr));
+	if (result == -EACCES)
+		d_printf(1, dev, "Cannot enter power save mode\n");
+	else if (result < 0)
+		dev_err(dev, "'Enter power save' (0x%04x) command failed: "
+			"%d - %s\n", I2400M_MT_CMD_ENTER_POWERSAVE,
+			result, strerr);
+	else
+		d_printf(1, dev, "device ready to power save\n");
+	kfree_skb(ack_skb);
+error_msg_to_dev:
+	kfree(cmd);
+error_alloc:
+	return result;
+}
+EXPORT_SYMBOL_GPL(i2400m_cmd_enter_powersave);
+
+
+/*
+ * Definitions for getting device information
+ */
+enum {
+	I2400M_TLV_DETAILED_DEVICE_INFO = 140
+};
+
+/**
+ * i2400m_get_device_info - Query the device for detailed device information
+ *
+ * @i2400m: device descriptor
+ *
+ * Returns: an skb whose skb->data points to a 'struct
+ *    i2400m_tlv_detailed_device_info'. When done, kfree_skb() it. The
+ *    skb is *guaranteed* to contain the whole TLV data structure.
+ *
+ *    On error, IS_ERR(skb) is true and ERR_PTR(skb) is the error
+ *    code.
+ */
+struct sk_buff *i2400m_get_device_info(struct i2400m *i2400m)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	struct sk_buff *ack_skb;
+	struct i2400m_l3l4_hdr *cmd;
+	const struct i2400m_l3l4_hdr *ack;
+	size_t ack_len;
+	const struct i2400m_tlv_hdr *tlv;
+	const struct i2400m_tlv_detailed_device_info *ddi;
+	char strerr[32];
+
+	ack_skb = ERR_PTR(-ENOMEM);
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (cmd == NULL)
+		goto error_alloc;
+	cmd->type = cpu_to_le16(I2400M_MT_GET_DEVICE_INFO);
+	cmd->length = 0;
+	cmd->version = cpu_to_le16(I2400M_L3L4_VERSION);
+
+	ack_skb = i2400m_msg_to_dev(i2400m, cmd, sizeof(*cmd));
+	if (IS_ERR(ack_skb)) {
+		dev_err(dev, "Failed to issue 'get device info' command: %ld\n",
+			PTR_ERR(ack_skb));
+		goto error_msg_to_dev;
+	}
+	ack = wimax_msg_data_len(ack_skb, &ack_len);
+	result = i2400m_msg_check_status(ack, strerr, sizeof(strerr));
+	if (result < 0) {
+		dev_err(dev, "'get device info' (0x%04x) command failed: "
+			"%d - %s\n", I2400M_MT_GET_DEVICE_INFO, result,
+			strerr);
+		goto error_cmd_failed;
+	}
+	tlv = i2400m_tlv_find(i2400m, ack->pl, ack_len - sizeof(*ack),
+			      I2400M_TLV_DETAILED_DEVICE_INFO, sizeof(*ddi));
+	if (tlv == NULL) {
+		dev_err(dev, "GET DEVICE INFO: "
+			"detailed device info TLV not found (0x%04x)\n",
+			I2400M_TLV_DETAILED_DEVICE_INFO);
+		result = -EIO;
+		goto error_no_tlv;
+	}
+	skb_pull(ack_skb, (void *) tlv - (void *) ack_skb->data);
+error_msg_to_dev:
+	kfree(cmd);
+error_alloc:
+	return ack_skb;
+
+error_no_tlv:
+error_cmd_failed:
+	kfree_skb(ack_skb);
+	kfree(cmd);
+	return ERR_PTR(result);
+}
+
+
+/* Firmware interface versions we support */
+enum {
+	I2400M_HDIv_MAJOR = 9,
+	I2400M_HDIv_MAJOR_2 = 8,
+	I2400M_HDIv_MINOR = 1,
+};
+
+
+/**
+ * i2400m_firmware_check - check firmware versions are compatible with
+ * the driver
+ *
+ * @i2400m: device descriptor
+ *
+ * Returns: 0 if ok, < 0 errno code an error and a message in the
+ *    kernel log.
+ *
+ * Long function, but quite simple; first chunk launches the command
+ * and double checks the reply for the right TLV. Then we process the
+ * TLV (where the meat is).
+ */
+int i2400m_firmware_check(struct i2400m *i2400m)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	struct sk_buff *ack_skb;
+	struct i2400m_l3l4_hdr *cmd;
+	const struct i2400m_l3l4_hdr *ack;
+	size_t ack_len;
+	const struct i2400m_tlv_hdr *tlv;
+	const struct i2400m_tlv_l4_message_versions *l4mv;
+	char strerr[32];
+	unsigned major, minor, branch;
+
+	result = -ENOMEM;
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (cmd == NULL)
+		goto error_alloc;
+	cmd->type = cpu_to_le16(I2400M_MT_GET_LM_VERSION);
+	cmd->length = 0;
+	cmd->version = cpu_to_le16(I2400M_L3L4_VERSION);
+
+	ack_skb = i2400m_msg_to_dev(i2400m, cmd, sizeof(*cmd));
+	if (IS_ERR(ack_skb)) {
+		result = PTR_ERR(ack_skb);
+		dev_err(dev, "Failed to issue 'get lm version' command: %-d\n",
+			result);
+		goto error_msg_to_dev;
+	}
+	ack = wimax_msg_data_len(ack_skb, &ack_len);
+	result = i2400m_msg_check_status(ack, strerr, sizeof(strerr));
+	if (result < 0) {
+		dev_err(dev, "'get lm version' (0x%04x) command failed: "
+			"%d - %s\n", I2400M_MT_GET_LM_VERSION, result,
+			strerr);
+		goto error_cmd_failed;
+	}
+	tlv = i2400m_tlv_find(i2400m, ack->pl, ack_len - sizeof(*ack),
+			      I2400M_TLV_L4_MESSAGE_VERSIONS, sizeof(*l4mv));
+	if (tlv == NULL) {
+		dev_err(dev, "get lm version: TLV not found (0x%04x)\n",
+			I2400M_TLV_L4_MESSAGE_VERSIONS);
+		result = -EIO;
+		goto error_no_tlv;
+	}
+	l4mv = container_of(tlv, typeof(*l4mv), hdr);
+	major = le16_to_cpu(l4mv->major);
+	minor = le16_to_cpu(l4mv->minor);
+	branch = le16_to_cpu(l4mv->branch);
+	result = -EINVAL;
+	if (major != I2400M_HDIv_MAJOR
+	    && major != I2400M_HDIv_MAJOR_2) {
+		dev_err(dev, "unsupported major fw interface version "
+			"%u.%u.%u\n", major, minor, branch);
+		goto error_bad_major;
+	}
+	if (major == I2400M_HDIv_MAJOR_2)
+		dev_err(dev, "deprecated major fw interface version "
+			"%u.%u.%u\n", major, minor, branch);
+	result = 0;
+	if (minor != I2400M_HDIv_MINOR)
+		dev_warn(dev, "untested minor fw firmware version %u.%u.%u\n",
+			 major, minor, branch);
+error_bad_major:
+	dev_info(dev, "firmware interface version %u.%u.%u\n",
+		 major, minor, branch);
+error_no_tlv:
+error_cmd_failed:
+	kfree_skb(ack_skb);
+error_msg_to_dev:
+	kfree(cmd);
+error_alloc:
+	return result;
+}
+
+
+/*
+ * Send an DoExitIdle command to the device to ask it to go out of
+ * basestation-idle mode.
+ *
+ * @i2400m: device descriptor
+ *
+ * This starts a renegotiation with the basestation that might involve
+ * another crypto handshake with user space.
+ *
+ * Returns: 0 if ok, < 0 errno code on error.
+ */
+int i2400m_cmd_exit_idle(struct i2400m *i2400m)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	struct sk_buff *ack_skb;
+	struct i2400m_l3l4_hdr *cmd;
+	char strerr[32];
+
+	result = -ENOMEM;
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (cmd == NULL)
+		goto error_alloc;
+	cmd->type = cpu_to_le16(I2400M_MT_CMD_EXIT_IDLE);
+	cmd->length = 0;
+	cmd->version = cpu_to_le16(I2400M_L3L4_VERSION);
+
+	ack_skb = i2400m_msg_to_dev(i2400m, cmd, sizeof(*cmd));
+	result = PTR_ERR(ack_skb);
+	if (IS_ERR(ack_skb)) {
+		dev_err(dev, "Failed to issue 'exit idle' command: %d\n",
+			result);
+		goto error_msg_to_dev;
+	}
+	result = i2400m_msg_check_status(wimax_msg_data(ack_skb),
+					 strerr, sizeof(strerr));
+	kfree_skb(ack_skb);
+error_msg_to_dev:
+	kfree(cmd);
+error_alloc:
+	return result;
+
+}
+
+
+/*
+ * Query the device for its state, update the WiMAX stack's idea of it
+ *
+ * @i2400m: device descriptor
+ *
+ * Returns: 0 if ok, < 0 errno code on error.
+ *
+ * Executes a 'Get State' command and parses the returned
+ * TLVs.
+ *
+ * Because this is almost identical to a 'Report State', we use
+ * i2400m_report_state_hook() to parse the answer. This will set the
+ * carrier state, as well as the RF Kill switches state.
+ */
+int i2400m_cmd_get_state(struct i2400m *i2400m)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	struct sk_buff *ack_skb;
+	struct i2400m_l3l4_hdr *cmd;
+	const struct i2400m_l3l4_hdr *ack;
+	size_t ack_len;
+	char strerr[32];
+
+	result = -ENOMEM;
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (cmd == NULL)
+		goto error_alloc;
+	cmd->type = cpu_to_le16(I2400M_MT_GET_STATE);
+	cmd->length = 0;
+	cmd->version = cpu_to_le16(I2400M_L3L4_VERSION);
+
+	ack_skb = i2400m_msg_to_dev(i2400m, cmd, sizeof(*cmd));
+	if (IS_ERR(ack_skb)) {
+		dev_err(dev, "Failed to issue 'get state' command: %ld\n",
+			PTR_ERR(ack_skb));
+		result = PTR_ERR(ack_skb);
+		goto error_msg_to_dev;
+	}
+	ack = wimax_msg_data_len(ack_skb, &ack_len);
+	result = i2400m_msg_check_status(ack, strerr, sizeof(strerr));
+	if (result < 0) {
+		dev_err(dev, "'get state' (0x%04x) command failed: "
+			"%d - %s\n", I2400M_MT_GET_STATE, result, strerr);
+		goto error_cmd_failed;
+	}
+	i2400m_report_state_hook(i2400m, ack, ack_len - sizeof(*ack),
+				 "GET STATE");
+	result = 0;
+	kfree_skb(ack_skb);
+error_cmd_failed:
+error_msg_to_dev:
+	kfree(cmd);
+error_alloc:
+	return result;
+}
+EXPORT_SYMBOL_GPL(i2400m_cmd_get_state);
+
+
+/**
+ * Set basic configuration settings
+ *
+ * @i2400m: device descriptor
+ * @args: array of pointers to the TLV headers to send for
+ *     configuration (each followed by its payload).
+ *     TLV headers and payloads must be properly initialized, with the
+ *     right endianess (LE).
+ * @arg_size: number of pointers in the @args array
+ */
+int i2400m_set_init_config(struct i2400m *i2400m,
+			   const struct i2400m_tlv_hdr **arg, size_t args)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	struct sk_buff *ack_skb;
+	struct i2400m_l3l4_hdr *cmd;
+	char strerr[32];
+	unsigned argc, argsize, tlv_size;
+	const struct i2400m_tlv_hdr *tlv_hdr;
+	void *buf, *itr;
+
+	d_fnstart(3, dev, "(i2400m %p arg %p args %zu)\n", i2400m, arg, args);
+	result = 0;
+	if (args == 0)
+		goto none;
+	/* Compute the size of all the TLVs, so we can alloc a
+	 * contiguous command block to copy them. */
+	argsize = 0;
+	for (argc = 0; argc < args; argc++) {
+		tlv_hdr = arg[argc];
+		argsize += sizeof(*tlv_hdr) + le16_to_cpu(tlv_hdr->length);
+	}
+	WARN_ON(argc >= 9);	/* As per hw spec */
+
+	/* Alloc the space for the command and TLVs*/
+	result = -ENOMEM;
+	buf = kzalloc(sizeof(*cmd) + argsize, GFP_KERNEL);
+	if (buf == NULL)
+		goto error_alloc;
+	cmd = buf;
+	cmd->type = cpu_to_le16(I2400M_MT_SET_INIT_CONFIG);
+	cmd->length = cpu_to_le16(argsize);
+	cmd->version = cpu_to_le16(I2400M_L3L4_VERSION);
+
+	/* Copy the TLVs */
+	itr = buf + sizeof(*cmd);
+	for (argc = 0; argc < args; argc++) {
+		tlv_hdr = arg[argc];
+		tlv_size = sizeof(*tlv_hdr) + le16_to_cpu(tlv_hdr->length);
+		memcpy(itr, tlv_hdr, tlv_size);
+		itr += tlv_size;
+	}
+
+	/* Send the message! */
+	ack_skb = i2400m_msg_to_dev(i2400m, buf, sizeof(*cmd) + argsize);
+	result = PTR_ERR(ack_skb);
+	if (IS_ERR(ack_skb)) {
+		dev_err(dev, "Failed to issue 'init config' command: %d\n",
+			result);
+
+		goto error_msg_to_dev;
+	}
+	result = i2400m_msg_check_status(wimax_msg_data(ack_skb),
+					 strerr, sizeof(strerr));
+	if (result < 0)
+		dev_err(dev, "'init config' (0x%04x) command failed: %d - %s\n",
+			I2400M_MT_SET_INIT_CONFIG, result, strerr);
+	kfree_skb(ack_skb);
+error_msg_to_dev:
+	kfree(buf);
+error_alloc:
+none:
+	d_fnend(3, dev, "(i2400m %p arg %p args %zu) = %d\n",
+		i2400m, arg, args, result);
+	return result;
+
+}
+EXPORT_SYMBOL_GPL(i2400m_set_init_config);
+
+
+/**
+ * i2400m_dev_initialize - Initialize the device once communications are ready
+ *
+ * @i2400m: device descriptor
+ *
+ * Returns: 0 if ok, < 0 errno code on error.
+ *
+ * Configures the device to work the way we like it.
+ *
+ * At the point of this call, the device is registered with the WiMAX
+ * and netdev stacks, firmware is uploaded and we can talk to the
+ * device normally.
+ */
+int i2400m_dev_initialize(struct i2400m *i2400m)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	struct i2400m_tlv_config_idle_parameters idle_params;
+	const struct i2400m_tlv_hdr *args[9];
+	unsigned argc = 0;
+
+	d_fnstart(3, dev, "(i2400m %p)\n", i2400m);
+	/* Useless for now...might change */
+	if (i2400m_idle_mode_disabled) {
+		idle_params.hdr.type =
+			cpu_to_le16(I2400M_TLV_CONFIG_IDLE_PARAMETERS);
+		idle_params.hdr.length = cpu_to_le16(
+			sizeof(idle_params) - sizeof(idle_params.hdr));
+		idle_params.idle_timeout = 0;
+		idle_params.idle_paging_interval = 0;
+		args[argc++] = &idle_params.hdr;
+	}
+	result = i2400m_set_init_config(i2400m, args, argc);
+	if (result < 0)
+		goto error;
+	result = i2400m_firmware_check(i2400m);	/* fw versions ok? */
+	if (result < 0)
+		goto error;
+	/*
+	 * Update state: Here it just calls a get state; parsing the
+	 * result (System State TLV and RF Status TLV [done in the rx
+	 * path hooks]) will set the hardware and software RF-Kill
+	 * status.
+	 */
+	result = i2400m_cmd_get_state(i2400m);
+error:
+	d_fnend(3, dev, "(i2400m %p) = %d\n", i2400m, result);
+	return result;
+}
+
+
+/**
+ * i2400m_dev_shutdown - Shutdown a running device
+ *
+ * @i2400m: device descriptor
+ *
+ * Gracefully stops the device, moving it to the lowest power
+ * consumption state possible.
+ */
+void i2400m_dev_shutdown(struct i2400m *i2400m)
+{
+	int result = -ENODEV;
+	struct device *dev = i2400m_dev(i2400m);
+
+	d_fnstart(3, dev, "(i2400m %p)\n", i2400m);
+	result = i2400m->bus_reset(i2400m, I2400M_RT_WARM);
+	d_fnend(3, dev, "(i2400m %p) = void [%d]\n", i2400m, result);
+	return;
+}
diff --git a/drivers/net/wimax/i2400m/debug-levels.h b/drivers/net/wimax/i2400m/debug-levels.h
new file mode 100644
index 000000000000..3183baa16a52
--- /dev/null
+++ b/drivers/net/wimax/i2400m/debug-levels.h
@@ -0,0 +1,45 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * Debug levels control file for the i2400m module
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+#ifndef __debug_levels__h__
+#define __debug_levels__h__
+
+/* Maximum compile and run time debug level for all submodules */
+#define D_MODULENAME i2400m
+#define D_MASTER CONFIG_WIMAX_I2400M_DEBUG_LEVEL
+
+#include <linux/wimax/debug.h>
+
+/* List of all the enabled modules */
+enum d_module {
+	D_SUBMODULE_DECLARE(control),
+	D_SUBMODULE_DECLARE(driver),
+	D_SUBMODULE_DECLARE(debugfs),
+	D_SUBMODULE_DECLARE(fw),
+	D_SUBMODULE_DECLARE(netdev),
+	D_SUBMODULE_DECLARE(rfkill),
+	D_SUBMODULE_DECLARE(rx),
+	D_SUBMODULE_DECLARE(tx),
+};
+
+
+#endif /* #ifndef __debug_levels__h__ */
diff --git a/drivers/net/wimax/i2400m/debugfs.c b/drivers/net/wimax/i2400m/debugfs.c
new file mode 100644
index 000000000000..626632985977
--- /dev/null
+++ b/drivers/net/wimax/i2400m/debugfs.c
@@ -0,0 +1,392 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * Debugfs interfaces to manipulate driver and device information
+ *
+ *
+ * Copyright (C) 2007 Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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/debugfs.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/spinlock.h>
+#include <linux/device.h>
+#include "i2400m.h"
+
+
+#define D_SUBMODULE debugfs
+#include "debug-levels.h"
+
+static
+int debugfs_netdev_queue_stopped_get(void *data, u64 *val)
+{
+	struct i2400m *i2400m = data;
+	*val = netif_queue_stopped(i2400m->wimax_dev.net_dev);
+	return 0;
+}
+DEFINE_SIMPLE_ATTRIBUTE(fops_netdev_queue_stopped,
+			debugfs_netdev_queue_stopped_get,
+			NULL, "%llu\n");
+
+
+static
+struct dentry *debugfs_create_netdev_queue_stopped(
+	const char *name, struct dentry *parent, struct i2400m *i2400m)
+{
+	return debugfs_create_file(name, 0400, parent, i2400m,
+				   &fops_netdev_queue_stopped);
+}
+
+
+/*
+ * inode->i_private has the @data argument to debugfs_create_file()
+ */
+static
+int i2400m_stats_open(struct inode *inode, struct file *filp)
+{
+	filp->private_data = inode->i_private;
+	return 0;
+}
+
+/*
+ * We don't allow partial reads of this file, as then the reader would
+ * get weirdly confused data as it is updated.
+ *
+ * So or you read it all or nothing; if you try to read with an offset
+ * != 0, we consider you are done reading.
+ */
+static
+ssize_t i2400m_rx_stats_read(struct file *filp, char __user *buffer,
+			     size_t count, loff_t *ppos)
+{
+	struct i2400m *i2400m = filp->private_data;
+	char buf[128];
+	unsigned long flags;
+
+	if (*ppos != 0)
+		return 0;
+	if (count < sizeof(buf))
+		return -ENOSPC;
+	spin_lock_irqsave(&i2400m->rx_lock, flags);
+	snprintf(buf, sizeof(buf), "%u %u %u %u %u %u %u\n",
+		 i2400m->rx_pl_num, i2400m->rx_pl_min,
+		 i2400m->rx_pl_max, i2400m->rx_num,
+		 i2400m->rx_size_acc,
+		 i2400m->rx_size_min, i2400m->rx_size_max);
+	spin_unlock_irqrestore(&i2400m->rx_lock, flags);
+	return simple_read_from_buffer(buffer, count, ppos, buf, strlen(buf));
+}
+
+
+/* Any write clears the stats */
+static
+ssize_t i2400m_rx_stats_write(struct file *filp, const char __user *buffer,
+			      size_t count, loff_t *ppos)
+{
+	struct i2400m *i2400m = filp->private_data;
+	unsigned long flags;
+
+	spin_lock_irqsave(&i2400m->rx_lock, flags);
+	i2400m->rx_pl_num = 0;
+	i2400m->rx_pl_max = 0;
+	i2400m->rx_pl_min = UINT_MAX;
+	i2400m->rx_num = 0;
+	i2400m->rx_size_acc = 0;
+	i2400m->rx_size_min = UINT_MAX;
+	i2400m->rx_size_max = 0;
+	spin_unlock_irqrestore(&i2400m->rx_lock, flags);
+	return count;
+}
+
+static
+const struct file_operations i2400m_rx_stats_fops = {
+	.owner =	THIS_MODULE,
+	.open =		i2400m_stats_open,
+	.read =		i2400m_rx_stats_read,
+	.write =	i2400m_rx_stats_write,
+};
+
+
+/* See i2400m_rx_stats_read() */
+static
+ssize_t i2400m_tx_stats_read(struct file *filp, char __user *buffer,
+			     size_t count, loff_t *ppos)
+{
+	struct i2400m *i2400m = filp->private_data;
+	char buf[128];
+	unsigned long flags;
+
+	if (*ppos != 0)
+		return 0;
+	if (count < sizeof(buf))
+		return -ENOSPC;
+	spin_lock_irqsave(&i2400m->tx_lock, flags);
+	snprintf(buf, sizeof(buf), "%u %u %u %u %u %u %u\n",
+		 i2400m->tx_pl_num, i2400m->tx_pl_min,
+		 i2400m->tx_pl_max, i2400m->tx_num,
+		 i2400m->tx_size_acc,
+		 i2400m->tx_size_min, i2400m->tx_size_max);
+	spin_unlock_irqrestore(&i2400m->tx_lock, flags);
+	return simple_read_from_buffer(buffer, count, ppos, buf, strlen(buf));
+}
+
+/* Any write clears the stats */
+static
+ssize_t i2400m_tx_stats_write(struct file *filp, const char __user *buffer,
+			      size_t count, loff_t *ppos)
+{
+	struct i2400m *i2400m = filp->private_data;
+	unsigned long flags;
+
+	spin_lock_irqsave(&i2400m->tx_lock, flags);
+	i2400m->tx_pl_num = 0;
+	i2400m->tx_pl_max = 0;
+	i2400m->tx_pl_min = UINT_MAX;
+	i2400m->tx_num = 0;
+	i2400m->tx_size_acc = 0;
+	i2400m->tx_size_min = UINT_MAX;
+	i2400m->tx_size_max = 0;
+	spin_unlock_irqrestore(&i2400m->tx_lock, flags);
+	return count;
+}
+
+static
+const struct file_operations i2400m_tx_stats_fops = {
+	.owner =	THIS_MODULE,
+	.open =		i2400m_stats_open,
+	.read =		i2400m_tx_stats_read,
+	.write =	i2400m_tx_stats_write,
+};
+
+
+/* Write 1 to ask the device to go into suspend */
+static
+int debugfs_i2400m_suspend_set(void *data, u64 val)
+{
+	int result;
+	struct i2400m *i2400m = data;
+	result = i2400m_cmd_enter_powersave(i2400m);
+	if (result >= 0)
+		result = 0;
+	return result;
+}
+DEFINE_SIMPLE_ATTRIBUTE(fops_i2400m_suspend,
+			NULL, debugfs_i2400m_suspend_set,
+			"%llu\n");
+
+static
+struct dentry *debugfs_create_i2400m_suspend(
+	const char *name, struct dentry *parent, struct i2400m *i2400m)
+{
+	return debugfs_create_file(name, 0200, parent, i2400m,
+				   &fops_i2400m_suspend);
+}
+
+
+/*
+ * Reset the device
+ *
+ * Write 0 to ask the device to soft reset, 1 to cold reset, 2 to bus
+ * reset (as defined by enum i2400m_reset_type).
+ */
+static
+int debugfs_i2400m_reset_set(void *data, u64 val)
+{
+	int result;
+	struct i2400m *i2400m = data;
+	enum i2400m_reset_type rt = val;
+	switch(rt) {
+	case I2400M_RT_WARM:
+	case I2400M_RT_COLD:
+	case I2400M_RT_BUS:
+		result = i2400m->bus_reset(i2400m, rt);
+		if (result >= 0)
+			result = 0;
+	default:
+		result = -EINVAL;
+	}
+	return result;
+}
+DEFINE_SIMPLE_ATTRIBUTE(fops_i2400m_reset,
+			NULL, debugfs_i2400m_reset_set,
+			"%llu\n");
+
+static
+struct dentry *debugfs_create_i2400m_reset(
+	const char *name, struct dentry *parent, struct i2400m *i2400m)
+{
+	return debugfs_create_file(name, 0200, parent, i2400m,
+				   &fops_i2400m_reset);
+}
+
+/*
+ * Debug levels control; see debug.h
+ */
+struct d_level D_LEVEL[] = {
+	D_SUBMODULE_DEFINE(control),
+	D_SUBMODULE_DEFINE(driver),
+	D_SUBMODULE_DEFINE(debugfs),
+	D_SUBMODULE_DEFINE(fw),
+	D_SUBMODULE_DEFINE(netdev),
+	D_SUBMODULE_DEFINE(rfkill),
+	D_SUBMODULE_DEFINE(rx),
+	D_SUBMODULE_DEFINE(tx),
+};
+size_t D_LEVEL_SIZE = ARRAY_SIZE(D_LEVEL);
+
+#define __debugfs_register(prefix, name, parent)			\
+do {									\
+	result = d_level_register_debugfs(prefix, name, parent);	\
+	if (result < 0)							\
+		goto error;						\
+} while (0)
+
+
+int i2400m_debugfs_add(struct i2400m *i2400m)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	struct dentry *dentry = i2400m->wimax_dev.debugfs_dentry;
+	struct dentry *fd;
+
+	dentry = debugfs_create_dir("i2400m", dentry);
+	result = PTR_ERR(dentry);
+	if (IS_ERR(dentry)) {
+		if (result == -ENODEV)
+			result = 0;	/* No debugfs support */
+		goto error;
+	}
+	i2400m->debugfs_dentry = dentry;
+	__debugfs_register("dl_", control, dentry);
+	__debugfs_register("dl_", driver, dentry);
+	__debugfs_register("dl_", debugfs, dentry);
+	__debugfs_register("dl_", fw, dentry);
+	__debugfs_register("dl_", netdev, dentry);
+	__debugfs_register("dl_", rfkill, dentry);
+	__debugfs_register("dl_", rx, dentry);
+	__debugfs_register("dl_", tx, dentry);
+
+	fd = debugfs_create_size_t("tx_in", 0400, dentry,
+				   &i2400m->tx_in);
+	result = PTR_ERR(fd);
+	if (IS_ERR(fd) && result != -ENODEV) {
+		dev_err(dev, "Can't create debugfs entry "
+			"tx_in: %d\n", result);
+		goto error;
+	}
+
+	fd = debugfs_create_size_t("tx_out", 0400, dentry,
+				   &i2400m->tx_out);
+	result = PTR_ERR(fd);
+	if (IS_ERR(fd) && result != -ENODEV) {
+		dev_err(dev, "Can't create debugfs entry "
+			"tx_out: %d\n", result);
+		goto error;
+	}
+
+	fd = debugfs_create_u32("state", 0600, dentry,
+				&i2400m->state);
+	result = PTR_ERR(fd);
+	if (IS_ERR(fd) && result != -ENODEV) {
+		dev_err(dev, "Can't create debugfs entry "
+			"state: %d\n", result);
+		goto error;
+	}
+
+	/*
+	 * Trace received messages from user space
+	 *
+	 * In order to tap the bidirectional message stream in the
+	 * 'msg' pipe, user space can read from the 'msg' pipe;
+	 * however, due to limitations in libnl, we can't know what
+	 * the different applications are sending down to the kernel.
+	 *
+	 * So we have this hack where the driver will echo any message
+	 * received on the msg pipe from user space [through a call to
+	 * wimax_dev->op_msg_from_user() into
+	 * i2400m_op_msg_from_user()] into the 'trace' pipe that this
+	 * driver creates.
+	 *
+	 * So then, reading from both the 'trace' and 'msg' pipes in
+	 * user space will provide a full dump of the traffic.
+	 *
+	 * Write 1 to activate, 0 to clear.
+	 *
+	 * It is not really very atomic, but it is also not too
+	 * critical.
+	 */
+	fd = debugfs_create_u8("trace_msg_from_user", 0600, dentry,
+			       &i2400m->trace_msg_from_user);
+	result = PTR_ERR(fd);
+	if (IS_ERR(fd) && result != -ENODEV) {
+		dev_err(dev, "Can't create debugfs entry "
+			"trace_msg_from_user: %d\n", result);
+		goto error;
+	}
+
+	fd = debugfs_create_netdev_queue_stopped("netdev_queue_stopped",
+						 dentry, i2400m);
+	result = PTR_ERR(fd);
+	if (IS_ERR(fd) && result != -ENODEV) {
+		dev_err(dev, "Can't create debugfs entry "
+			"netdev_queue_stopped: %d\n", result);
+		goto error;
+	}
+
+	fd = debugfs_create_file("rx_stats", 0600, dentry, i2400m,
+				 &i2400m_rx_stats_fops);
+	result = PTR_ERR(fd);
+	if (IS_ERR(fd) && result != -ENODEV) {
+		dev_err(dev, "Can't create debugfs entry "
+			"rx_stats: %d\n", result);
+		goto error;
+	}
+
+	fd = debugfs_create_file("tx_stats", 0600, dentry, i2400m,
+				 &i2400m_tx_stats_fops);
+	result = PTR_ERR(fd);
+	if (IS_ERR(fd) && result != -ENODEV) {
+		dev_err(dev, "Can't create debugfs entry "
+			"tx_stats: %d\n", result);
+		goto error;
+	}
+
+	fd = debugfs_create_i2400m_suspend("suspend", dentry, i2400m);
+	result = PTR_ERR(fd);
+	if (IS_ERR(fd) && result != -ENODEV) {
+		dev_err(dev, "Can't create debugfs entry suspend: %d\n",
+			result);
+		goto error;
+	}
+
+	fd = debugfs_create_i2400m_reset("reset", dentry, i2400m);
+	result = PTR_ERR(fd);
+	if (IS_ERR(fd) && result != -ENODEV) {
+		dev_err(dev, "Can't create debugfs entry reset: %d\n", result);
+		goto error;
+	}
+
+	result = 0;
+error:
+	return result;
+}
+
+void i2400m_debugfs_rm(struct i2400m *i2400m)
+{
+	debugfs_remove_recursive(i2400m->debugfs_dentry);
+}
diff --git a/drivers/net/wimax/i2400m/driver.c b/drivers/net/wimax/i2400m/driver.c
new file mode 100644
index 000000000000..5f98047e18cf
--- /dev/null
+++ b/drivers/net/wimax/i2400m/driver.c
@@ -0,0 +1,728 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * Generic probe/disconnect, reset and message passing
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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.
+ *
+ *
+ * See i2400m.h for driver documentation. This contains helpers for
+ * the driver model glue [_setup()/_release()], handling device resets
+ * [_dev_reset_handle()], and the backends for the WiMAX stack ops
+ * reset [_op_reset()] and message from user [_op_msg_from_user()].
+ *
+ * ROADMAP:
+ *
+ * i2400m_op_msg_from_user()
+ *   i2400m_msg_to_dev()
+ *   wimax_msg_to_user_send()
+ *
+ * i2400m_op_reset()
+ *   i240m->bus_reset()
+ *
+ * i2400m_dev_reset_handle()
+ *   __i2400m_dev_reset_handle()
+ *     __i2400m_dev_stop()
+ *     __i2400m_dev_start()
+ *
+ * i2400m_setup()
+ *   i2400m_bootrom_init()
+ *   register_netdev()
+ *   i2400m_dev_start()
+ *     __i2400m_dev_start()
+ *       i2400m_dev_bootstrap()
+ *       i2400m_tx_setup()
+ *       i2400m->bus_dev_start()
+ *       i2400m_check_mac_addr()
+ *   wimax_dev_add()
+ *
+ * i2400m_release()
+ *   wimax_dev_rm()
+ *   i2400m_dev_stop()
+ *     __i2400m_dev_stop()
+ *       i2400m_dev_shutdown()
+ *       i2400m->bus_dev_stop()
+ *       i2400m_tx_release()
+ *   unregister_netdev()
+ */
+#include "i2400m.h"
+#include <linux/wimax/i2400m.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+
+#define D_SUBMODULE driver
+#include "debug-levels.h"
+
+
+int i2400m_idle_mode_disabled;	/* 0 (idle mode enabled) by default */
+module_param_named(idle_mode_disabled, i2400m_idle_mode_disabled, int, 0644);
+MODULE_PARM_DESC(idle_mode_disabled,
+		 "If true, the device will not enable idle mode negotiation "
+		 "with the base station (when connected) to save power.");
+
+/**
+ * i2400m_queue_work - schedule work on a i2400m's queue
+ *
+ * @i2400m: device descriptor
+ *
+ * @fn: function to run to execute work. It gets passed a 'struct
+ *     work_struct' that is wrapped in a 'struct i2400m_work'. Once
+ *     done, you have to (1) i2400m_put(i2400m_work->i2400m) and then
+ *     (2) kfree(i2400m_work).
+ *
+ * @gfp_flags: GFP flags for memory allocation.
+ *
+ * @pl: pointer to a payload buffer that you want to pass to the _work
+ *     function. Use this to pack (for example) a struct with extra
+ *     arguments.
+ *
+ * @pl_size: size of the payload buffer.
+ *
+ * We do this quite often, so this just saves typing; allocate a
+ * wrapper for a i2400m, get a ref to it, pack arguments and launch
+ * the work.
+ *
+ * A usual workflow is:
+ *
+ * struct my_work_args {
+ *         void *something;
+ *         int whatever;
+ * };
+ * ...
+ *
+ * struct my_work_args my_args = {
+ *         .something = FOO,
+ *         .whaetever = BLAH
+ * };
+ * i2400m_queue_work(i2400m, 1, my_work_function, GFP_KERNEL,
+ *                   &args, sizeof(args))
+ *
+ * And now the work function can unpack the arguments and call the
+ * real function (or do the job itself):
+ *
+ * static
+ * void my_work_fn((struct work_struct *ws)
+ * {
+ *         struct i2400m_work *iw =
+ *	           container_of(ws, struct i2400m_work, ws);
+ *	   struct my_work_args *my_args = (void *) iw->pl;
+ *
+ *	   my_work(iw->i2400m, my_args->something, my_args->whatevert);
+ * }
+ */
+int i2400m_queue_work(struct i2400m *i2400m,
+		      void (*fn)(struct work_struct *), gfp_t gfp_flags,
+		      const void *pl, size_t pl_size)
+{
+	int result;
+	struct i2400m_work *iw;
+
+	BUG_ON(i2400m->work_queue == NULL);
+	result = -ENOMEM;
+	iw = kzalloc(sizeof(*iw) + pl_size, gfp_flags);
+	if (iw == NULL)
+		goto error_kzalloc;
+	iw->i2400m = i2400m_get(i2400m);
+	memcpy(iw->pl, pl, pl_size);
+	INIT_WORK(&iw->ws, fn);
+	result = queue_work(i2400m->work_queue, &iw->ws);
+error_kzalloc:
+	return result;
+}
+EXPORT_SYMBOL_GPL(i2400m_queue_work);
+
+
+/*
+ * Schedule i2400m's specific work on the system's queue.
+ *
+ * Used for a few cases where we really need it; otherwise, identical
+ * to i2400m_queue_work().
+ *
+ * Returns < 0 errno code on error, 1 if ok.
+ *
+ * If it returns zero, something really bad happened, as it means the
+ * works struct was already queued, but we have just allocated it, so
+ * it should not happen.
+ */
+int i2400m_schedule_work(struct i2400m *i2400m,
+			 void (*fn)(struct work_struct *), gfp_t gfp_flags)
+{
+	int result;
+	struct i2400m_work *iw;
+
+	BUG_ON(i2400m->work_queue == NULL);
+	result = -ENOMEM;
+	iw = kzalloc(sizeof(*iw), gfp_flags);
+	if (iw == NULL)
+		goto error_kzalloc;
+	iw->i2400m = i2400m_get(i2400m);
+	INIT_WORK(&iw->ws, fn);
+	result = schedule_work(&iw->ws);
+	if (result == 0)
+		result = -ENXIO;
+error_kzalloc:
+	return result;
+}
+
+
+/*
+ * WiMAX stack operation: relay a message from user space
+ *
+ * @wimax_dev: device descriptor
+ * @pipe_name: named pipe the message is for
+ * @msg_buf: pointer to the message bytes
+ * @msg_len: length of the buffer
+ * @genl_info: passed by the generic netlink layer
+ *
+ * The WiMAX stack will call this function when a message was received
+ * from user space.
+ *
+ * For the i2400m, this is an L3L4 message, as specified in
+ * include/linux/wimax/i2400m.h, and thus prefixed with a 'struct
+ * i2400m_l3l4_hdr'. Driver (and device) expect the messages to be
+ * coded in Little Endian.
+ *
+ * This function just verifies that the header declaration and the
+ * payload are consistent and then deals with it, either forwarding it
+ * to the device or procesing it locally.
+ *
+ * In the i2400m, messages are basically commands that will carry an
+ * ack, so we use i2400m_msg_to_dev() and then deliver the ack back to
+ * user space. The rx.c code might intercept the response and use it
+ * to update the driver's state, but then it will pass it on so it can
+ * be relayed back to user space.
+ *
+ * Note that asynchronous events from the device are processed and
+ * sent to user space in rx.c.
+ */
+static
+int i2400m_op_msg_from_user(struct wimax_dev *wimax_dev,
+			    const char *pipe_name,
+			    const void *msg_buf, size_t msg_len,
+			    const struct genl_info *genl_info)
+{
+	int result;
+	struct i2400m *i2400m = wimax_dev_to_i2400m(wimax_dev);
+	struct device *dev = i2400m_dev(i2400m);
+	struct sk_buff *ack_skb;
+
+	d_fnstart(4, dev, "(wimax_dev %p [i2400m %p] msg_buf %p "
+		  "msg_len %zu genl_info %p)\n", wimax_dev, i2400m,
+		  msg_buf, msg_len, genl_info);
+	ack_skb = i2400m_msg_to_dev(i2400m, msg_buf, msg_len);
+	result = PTR_ERR(ack_skb);
+	if (IS_ERR(ack_skb))
+		goto error_msg_to_dev;
+	if (unlikely(i2400m->trace_msg_from_user))
+		wimax_msg(&i2400m->wimax_dev, "trace",
+			  msg_buf, msg_len, GFP_KERNEL);
+	result = wimax_msg_send(&i2400m->wimax_dev, ack_skb);
+error_msg_to_dev:
+	d_fnend(4, dev, "(wimax_dev %p [i2400m %p] msg_buf %p msg_len %zu "
+		"genl_info %p) = %d\n", wimax_dev, i2400m, msg_buf, msg_len,
+		genl_info, result);
+	return result;
+}
+
+
+/*
+ * Context to wait for a reset to finalize
+ */
+struct i2400m_reset_ctx {
+	struct completion completion;
+	int result;
+};
+
+
+/*
+ * WiMAX stack operation: reset a device
+ *
+ * @wimax_dev: device descriptor
+ *
+ * See the documentation for wimax_reset() and wimax_dev->op_reset for
+ * the requirements of this function. The WiMAX stack guarantees
+ * serialization on calls to this function.
+ *
+ * Do a warm reset on the device; if it fails, resort to a cold reset
+ * and return -ENODEV. On successful warm reset, we need to block
+ * until it is complete.
+ *
+ * The bus-driver implementation of reset takes care of falling back
+ * to cold reset if warm fails.
+ */
+static
+int i2400m_op_reset(struct wimax_dev *wimax_dev)
+{
+	int result;
+	struct i2400m *i2400m = wimax_dev_to_i2400m(wimax_dev);
+	struct device *dev = i2400m_dev(i2400m);
+	struct i2400m_reset_ctx ctx = {
+		.completion = COMPLETION_INITIALIZER_ONSTACK(ctx.completion),
+		.result = 0,
+	};
+
+	d_fnstart(4, dev, "(wimax_dev %p)\n", wimax_dev);
+	mutex_lock(&i2400m->init_mutex);
+	i2400m->reset_ctx = &ctx;
+	mutex_unlock(&i2400m->init_mutex);
+	result = i2400m->bus_reset(i2400m, I2400M_RT_WARM);
+	if (result < 0)
+		goto out;
+	result = wait_for_completion_timeout(&ctx.completion, 4*HZ);
+	if (result == 0)
+		result = -ETIMEDOUT;
+	else if (result > 0)
+		result = ctx.result;
+	/* if result < 0, pass it on */
+	mutex_lock(&i2400m->init_mutex);
+	i2400m->reset_ctx = NULL;
+	mutex_unlock(&i2400m->init_mutex);
+out:
+	d_fnend(4, dev, "(wimax_dev %p) = %d\n", wimax_dev, result);
+	return result;
+}
+
+
+/*
+ * Check the MAC address we got from boot mode is ok
+ *
+ * @i2400m: device descriptor
+ *
+ * Returns: 0 if ok, < 0 errno code on error.
+ */
+static
+int i2400m_check_mac_addr(struct i2400m *i2400m)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	struct sk_buff *skb;
+	const struct i2400m_tlv_detailed_device_info *ddi;
+	struct net_device *net_dev = i2400m->wimax_dev.net_dev;
+	const unsigned char zeromac[ETH_ALEN] = { 0 };
+
+	d_fnstart(3, dev, "(i2400m %p)\n", i2400m);
+	skb = i2400m_get_device_info(i2400m);
+	if (IS_ERR(skb)) {
+		result = PTR_ERR(skb);
+		dev_err(dev, "Cannot verify MAC address, error reading: %d\n",
+			result);
+		goto error;
+	}
+	/* Extract MAC addresss */
+	ddi = (void *) skb->data;
+	BUILD_BUG_ON(ETH_ALEN != sizeof(ddi->mac_address));
+	d_printf(2, dev, "GET DEVICE INFO: mac addr "
+		 "%02x:%02x:%02x:%02x:%02x:%02x\n",
+		 ddi->mac_address[0], ddi->mac_address[1],
+		 ddi->mac_address[2], ddi->mac_address[3],
+		 ddi->mac_address[4], ddi->mac_address[5]);
+	if (!memcmp(net_dev->perm_addr, ddi->mac_address,
+		   sizeof(ddi->mac_address)))
+		goto ok;
+	dev_warn(dev, "warning: device reports a different MAC address "
+		 "to that of boot mode's\n");
+	dev_warn(dev, "device reports     %02x:%02x:%02x:%02x:%02x:%02x\n",
+		 ddi->mac_address[0], ddi->mac_address[1],
+		 ddi->mac_address[2], ddi->mac_address[3],
+		 ddi->mac_address[4], ddi->mac_address[5]);
+	dev_warn(dev, "boot mode reported %02x:%02x:%02x:%02x:%02x:%02x\n",
+		 net_dev->perm_addr[0], net_dev->perm_addr[1],
+		 net_dev->perm_addr[2], net_dev->perm_addr[3],
+		 net_dev->perm_addr[4], net_dev->perm_addr[5]);
+	if (!memcmp(zeromac, ddi->mac_address, sizeof(zeromac)))
+		dev_err(dev, "device reports an invalid MAC address, "
+			"not updating\n");
+	else {
+		dev_warn(dev, "updating MAC address\n");
+		net_dev->addr_len = ETH_ALEN;
+		memcpy(net_dev->perm_addr, ddi->mac_address, ETH_ALEN);
+		memcpy(net_dev->dev_addr, ddi->mac_address, ETH_ALEN);
+	}
+ok:
+	result = 0;
+	kfree_skb(skb);
+error:
+	d_fnend(3, dev, "(i2400m %p) = %d\n", i2400m, result);
+	return result;
+}
+
+
+/**
+ * __i2400m_dev_start - Bring up driver communication with the device
+ *
+ * @i2400m: device descriptor
+ * @flags: boot mode flags
+ *
+ * Returns: 0 if ok, < 0 errno code on error.
+ *
+ * Uploads firmware and brings up all the resources needed to be able
+ * to communicate with the device.
+ *
+ * TX needs to be setup before the bus-specific code (otherwise on
+ * shutdown, the bus-tx code could try to access it).
+ */
+static
+int __i2400m_dev_start(struct i2400m *i2400m, enum i2400m_bri flags)
+{
+	int result;
+	struct wimax_dev *wimax_dev = &i2400m->wimax_dev;
+	struct net_device *net_dev = wimax_dev->net_dev;
+	struct device *dev = i2400m_dev(i2400m);
+	int times = 3;
+
+	d_fnstart(3, dev, "(i2400m %p)\n", i2400m);
+retry:
+	result = i2400m_dev_bootstrap(i2400m, flags);
+	if (result < 0) {
+		dev_err(dev, "cannot bootstrap device: %d\n", result);
+		goto error_bootstrap;
+	}
+	result = i2400m_tx_setup(i2400m);
+	if (result < 0)
+		goto error_tx_setup;
+	result = i2400m->bus_dev_start(i2400m);
+	if (result < 0)
+		goto error_bus_dev_start;
+	i2400m->work_queue = create_singlethread_workqueue(wimax_dev->name);
+	if (i2400m->work_queue == NULL) {
+		result = -ENOMEM;
+		dev_err(dev, "cannot create workqueue\n");
+		goto error_create_workqueue;
+	}
+	/* At this point is ok to send commands to the device */
+	result = i2400m_check_mac_addr(i2400m);
+	if (result < 0)
+		goto error_check_mac_addr;
+	i2400m->ready = 1;
+	wimax_state_change(wimax_dev, WIMAX_ST_UNINITIALIZED);
+	result = i2400m_dev_initialize(i2400m);
+	if (result < 0)
+		goto error_dev_initialize;
+	/* At this point, reports will come for the device and set it
+	 * to the right state if it is different than UNINITIALIZED */
+	d_fnend(3, dev, "(net_dev %p [i2400m %p]) = %d\n",
+		net_dev, i2400m, result);
+	return result;
+
+error_dev_initialize:
+error_check_mac_addr:
+	destroy_workqueue(i2400m->work_queue);
+error_create_workqueue:
+	i2400m->bus_dev_stop(i2400m);
+error_bus_dev_start:
+	i2400m_tx_release(i2400m);
+error_tx_setup:
+error_bootstrap:
+	if (result == -ERESTARTSYS && times-- > 0) {
+		flags = I2400M_BRI_SOFT;
+		goto retry;
+	}
+	d_fnend(3, dev, "(net_dev %p [i2400m %p]) = %d\n",
+		net_dev, i2400m, result);
+	return result;
+}
+
+
+static
+int i2400m_dev_start(struct i2400m *i2400m, enum i2400m_bri bm_flags)
+{
+	int result;
+	mutex_lock(&i2400m->init_mutex);	/* Well, start the device */
+	result = __i2400m_dev_start(i2400m, bm_flags);
+	if (result >= 0)
+		i2400m->updown = 1;
+	mutex_unlock(&i2400m->init_mutex);
+	return result;
+}
+
+
+/**
+ * i2400m_dev_stop - Tear down driver communication with the device
+ *
+ * @i2400m: device descriptor
+ *
+ * Returns: 0 if ok, < 0 errno code on error.
+ *
+ * Releases all the resources allocated to communicate with the device.
+ */
+static
+void __i2400m_dev_stop(struct i2400m *i2400m)
+{
+	struct wimax_dev *wimax_dev = &i2400m->wimax_dev;
+	struct device *dev = i2400m_dev(i2400m);
+
+	d_fnstart(3, dev, "(i2400m %p)\n", i2400m);
+	wimax_state_change(wimax_dev, __WIMAX_ST_QUIESCING);
+	i2400m_dev_shutdown(i2400m);
+	i2400m->ready = 0;
+	destroy_workqueue(i2400m->work_queue);
+	i2400m->bus_dev_stop(i2400m);
+	i2400m_tx_release(i2400m);
+	wimax_state_change(wimax_dev, WIMAX_ST_DOWN);
+	d_fnend(3, dev, "(i2400m %p) = 0\n", i2400m);
+}
+
+
+/*
+ * Watch out -- we only need to stop if there is a need for it. The
+ * device could have reset itself and failed to come up again (see
+ * _i2400m_dev_reset_handle()).
+ */
+static
+void i2400m_dev_stop(struct i2400m *i2400m)
+{
+	mutex_lock(&i2400m->init_mutex);
+	if (i2400m->updown) {
+		__i2400m_dev_stop(i2400m);
+		i2400m->updown = 0;
+	}
+	mutex_unlock(&i2400m->init_mutex);
+}
+
+
+/*
+ * The device has rebooted; fix up the device and the driver
+ *
+ * Tear down the driver communication with the device, reload the
+ * firmware and reinitialize the communication with the device.
+ *
+ * If someone calls a reset when the device's firmware is down, in
+ * theory we won't see it because we are not listening. However, just
+ * in case, leave the code to handle it.
+ *
+ * If there is a reset context, use it; this means someone is waiting
+ * for us to tell him when the reset operation is complete and the
+ * device is ready to rock again.
+ *
+ * NOTE: if we are in the process of bringing up or down the
+ *       communication with the device [running i2400m_dev_start() or
+ *       _stop()], don't do anything, let it fail and handle it.
+ *
+ * This function is ran always in a thread context
+ */
+static
+void __i2400m_dev_reset_handle(struct work_struct *ws)
+{
+	int result;
+	struct i2400m_work *iw = container_of(ws, struct i2400m_work, ws);
+	struct i2400m *i2400m = iw->i2400m;
+	struct device *dev = i2400m_dev(i2400m);
+	enum wimax_st wimax_state;
+	struct i2400m_reset_ctx *ctx = i2400m->reset_ctx;
+
+	d_fnstart(3, dev, "(ws %p i2400m %p)\n", ws, i2400m);
+	result = 0;
+	if (mutex_trylock(&i2400m->init_mutex) == 0) {
+		/* We are still in i2400m_dev_start() [let it fail] or
+		 * i2400m_dev_stop() [we are shutting down anyway, so
+		 * ignore it] or we are resetting somewhere else. */
+		dev_err(dev, "device rebooted\n");
+		i2400m_msg_to_dev_cancel_wait(i2400m, -ERESTARTSYS);
+		complete(&i2400m->msg_completion);
+		goto out;
+	}
+	wimax_state = wimax_state_get(&i2400m->wimax_dev);
+	if (wimax_state < WIMAX_ST_UNINITIALIZED) {
+		dev_info(dev, "device rebooted: it is down, ignoring\n");
+		goto out_unlock;	/* ifconfig up/down wasn't called */
+	}
+	dev_err(dev, "device rebooted: reinitializing driver\n");
+	__i2400m_dev_stop(i2400m);
+	i2400m->updown = 0;
+	result = __i2400m_dev_start(i2400m,
+				    I2400M_BRI_SOFT | I2400M_BRI_MAC_REINIT);
+	if (result < 0) {
+		dev_err(dev, "device reboot: cannot start the device: %d\n",
+			result);
+		result = i2400m->bus_reset(i2400m, I2400M_RT_BUS);
+		if (result >= 0)
+			result = -ENODEV;
+	} else
+		i2400m->updown = 1;
+out_unlock:
+	if (i2400m->reset_ctx) {
+		ctx->result = result;
+		complete(&ctx->completion);
+	}
+	mutex_unlock(&i2400m->init_mutex);
+out:
+	i2400m_put(i2400m);
+	kfree(iw);
+	d_fnend(3, dev, "(ws %p i2400m %p) = void\n", ws, i2400m);
+	return;
+}
+
+
+/**
+ * i2400m_dev_reset_handle - Handle a device's reset in a thread context
+ *
+ * Schedule a device reset handling out on a thread context, so it
+ * is safe to call from atomic context. We can't use the i2400m's
+ * queue as we are going to destroy it and reinitialize it as part of
+ * the driver bringup/bringup process.
+ *
+ * See __i2400m_dev_reset_handle() for details; that takes care of
+ * reinitializing the driver to handle the reset, calling into the
+ * bus-specific functions ops as needed.
+ */
+int i2400m_dev_reset_handle(struct i2400m *i2400m)
+{
+	return i2400m_schedule_work(i2400m, __i2400m_dev_reset_handle,
+				    GFP_ATOMIC);
+}
+EXPORT_SYMBOL_GPL(i2400m_dev_reset_handle);
+
+
+/**
+ * i2400m_setup - bus-generic setup function for the i2400m device
+ *
+ * @i2400m: device descriptor (bus-specific parts have been initialized)
+ *
+ * Returns: 0 if ok, < 0 errno code on error.
+ *
+ * Initializes the bus-generic parts of the i2400m driver; the
+ * bus-specific parts have been initialized, function pointers filled
+ * out by the bus-specific probe function.
+ *
+ * As well, this registers the WiMAX and net device nodes. Once this
+ * function returns, the device is operative and has to be ready to
+ * receive and send network traffic and WiMAX control operations.
+ */
+int i2400m_setup(struct i2400m *i2400m, enum i2400m_bri bm_flags)
+{
+	int result = -ENODEV;
+	struct device *dev = i2400m_dev(i2400m);
+	struct wimax_dev *wimax_dev = &i2400m->wimax_dev;
+	struct net_device *net_dev = i2400m->wimax_dev.net_dev;
+
+	d_fnstart(3, dev, "(i2400m %p)\n", i2400m);
+
+	snprintf(wimax_dev->name, sizeof(wimax_dev->name),
+		 "i2400m-%s:%s", dev->bus->name, dev->bus_id);
+
+	i2400m->bm_cmd_buf = kzalloc(I2400M_BM_CMD_BUF_SIZE, GFP_KERNEL);
+	if (i2400m->bm_cmd_buf == NULL) {
+		dev_err(dev, "cannot allocate USB command buffer\n");
+		goto error_bm_cmd_kzalloc;
+	}
+	i2400m->bm_ack_buf = kzalloc(I2400M_BM_ACK_BUF_SIZE, GFP_KERNEL);
+	if (i2400m->bm_ack_buf == NULL) {
+		dev_err(dev, "cannot allocate USB ack buffer\n");
+		goto error_bm_ack_buf_kzalloc;
+	}
+	result = i2400m_bootrom_init(i2400m, bm_flags);
+	if (result < 0) {
+		dev_err(dev, "read mac addr: bootrom init "
+			"failed: %d\n", result);
+		goto error_bootrom_init;
+	}
+	result = i2400m_read_mac_addr(i2400m);
+	if (result < 0)
+		goto error_read_mac_addr;
+
+	result = register_netdev(net_dev);	/* Okey dokey, bring it up */
+	if (result < 0) {
+		dev_err(dev, "cannot register i2400m network device: %d\n",
+			result);
+		goto error_register_netdev;
+	}
+	netif_carrier_off(net_dev);
+
+	result = i2400m_dev_start(i2400m, bm_flags);
+	if (result < 0)
+		goto error_dev_start;
+
+	i2400m->wimax_dev.op_msg_from_user = i2400m_op_msg_from_user;
+	i2400m->wimax_dev.op_rfkill_sw_toggle = i2400m_op_rfkill_sw_toggle;
+	i2400m->wimax_dev.op_reset = i2400m_op_reset;
+	result = wimax_dev_add(&i2400m->wimax_dev, net_dev);
+	if (result < 0)
+		goto error_wimax_dev_add;
+	/* User space needs to do some init stuff */
+	wimax_state_change(wimax_dev, WIMAX_ST_UNINITIALIZED);
+
+	/* Now setup all that requires a registered net and wimax device. */
+	result = i2400m_debugfs_add(i2400m);
+	if (result < 0) {
+		dev_err(dev, "cannot setup i2400m's debugfs: %d\n", result);
+		goto error_debugfs_setup;
+	}
+	d_fnend(3, dev, "(i2400m %p) = %d\n", i2400m, result);
+	return result;
+
+error_debugfs_setup:
+	wimax_dev_rm(&i2400m->wimax_dev);
+error_wimax_dev_add:
+	i2400m_dev_stop(i2400m);
+error_dev_start:
+	unregister_netdev(net_dev);
+error_register_netdev:
+error_read_mac_addr:
+error_bootrom_init:
+	kfree(i2400m->bm_ack_buf);
+error_bm_ack_buf_kzalloc:
+	kfree(i2400m->bm_cmd_buf);
+error_bm_cmd_kzalloc:
+	d_fnend(3, dev, "(i2400m %p) = %d\n", i2400m, result);
+	return result;
+}
+EXPORT_SYMBOL_GPL(i2400m_setup);
+
+
+/**
+ * i2400m_release - release the bus-generic driver resources
+ *
+ * Sends a disconnect message and undoes any setup done by i2400m_setup()
+ */
+void i2400m_release(struct i2400m *i2400m)
+{
+	struct device *dev = i2400m_dev(i2400m);
+
+	d_fnstart(3, dev, "(i2400m %p)\n", i2400m);
+	netif_stop_queue(i2400m->wimax_dev.net_dev);
+
+	i2400m_debugfs_rm(i2400m);
+	wimax_dev_rm(&i2400m->wimax_dev);
+	i2400m_dev_stop(i2400m);
+	unregister_netdev(i2400m->wimax_dev.net_dev);
+	kfree(i2400m->bm_ack_buf);
+	kfree(i2400m->bm_cmd_buf);
+	d_fnend(3, dev, "(i2400m %p) = void\n", i2400m);
+}
+EXPORT_SYMBOL_GPL(i2400m_release);
+
+
+static
+int __init i2400m_driver_init(void)
+{
+	return 0;
+}
+module_init(i2400m_driver_init);
+
+static
+void __exit i2400m_driver_exit(void)
+{
+	/* for scheds i2400m_dev_reset_handle() */
+	flush_scheduled_work();
+	return;
+}
+module_exit(i2400m_driver_exit);
+
+MODULE_AUTHOR("Intel Corporation <linux-wimax@intel.com>");
+MODULE_DESCRIPTION("Intel 2400M WiMAX networking bus-generic driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/wimax/i2400m/fw.c b/drivers/net/wimax/i2400m/fw.c
new file mode 100644
index 000000000000..1d8271f34c38
--- /dev/null
+++ b/drivers/net/wimax/i2400m/fw.c
@@ -0,0 +1,1095 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * Firmware uploader
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Yanir Lubetkin <yanirx.lubetkin@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ *  - Initial implementation
+ *
+ *
+ * THE PROCEDURE
+ *
+ * (this is decribed for USB, but for SDIO is similar)
+ *
+ * The 2400m works in two modes: boot-mode or normal mode. In boot
+ * mode we can execute only a handful of commands targeted at
+ * uploading the firmware and launching it.
+ *
+ * The 2400m enters boot mode when it is first connected to the
+ * system, when it crashes and when you ask it to reboot. There are
+ * two submodes of the boot mode: signed and non-signed. Signed takes
+ * firmwares signed with a certain private key, non-signed takes any
+ * firmware. Normal hardware takes only signed firmware.
+ *
+ * Upon entrance to boot mode, the device sends a few zero length
+ * packets (ZLPs) on the notification endpoint, then a reboot barker
+ * (4 le32 words with value I2400M_{S,N}BOOT_BARKER). We ack it by
+ * sending the same barker on the bulk out endpoint. The device acks
+ * with a reboot ack barker (4 le32 words with value 0xfeedbabe) and
+ * then the device is fully rebooted. At this point we can upload the
+ * firmware.
+ *
+ * This process is accomplished by the i2400m_bootrom_init()
+ * function. All the device interaction happens through the
+ * i2400m_bm_cmd() [boot mode command]. Special return values will
+ * indicate if the device resets.
+ *
+ * After this, we read the MAC address and then (if needed)
+ * reinitialize the device. We need to read it ahead of time because
+ * in the future, we might not upload the firmware until userspace
+ * 'ifconfig up's the device.
+ *
+ * We can then upload the firmware file. The file is composed of a BCF
+ * header (basic data, keys and signatures) and a list of write
+ * commands and payloads. We first upload the header
+ * [i2400m_dnload_init()] and then pass the commands and payloads
+ * verbatim to the i2400m_bm_cmd() function
+ * [i2400m_dnload_bcf()]. Then we tell the device to jump to the new
+ * firmware [i2400m_dnload_finalize()].
+ *
+ * Once firmware is uploaded, we are good to go :)
+ *
+ * When we don't know in which mode we are, we first try by sending a
+ * warm reset request that will take us to boot-mode. If we time out
+ * waiting for a reboot barker, that means maybe we are already in
+ * boot mode, so we send a reboot barker.
+ *
+ * COMMAND EXECUTION
+ *
+ * This code (and process) is single threaded; for executing commands,
+ * we post a URB to the notification endpoint, post the command, wait
+ * for data on the notification buffer. We don't need to worry about
+ * others as we know we are the only ones in there.
+ *
+ * BACKEND IMPLEMENTATION
+ *
+ * This code is bus-generic; the bus-specific driver provides back end
+ * implementations to send a boot mode command to the device and to
+ * read an acknolwedgement from it (or an asynchronous notification)
+ * from it.
+ *
+ * ROADMAP
+ *
+ * i2400m_dev_bootstrap               Called by __i2400m_dev_start()
+ *   request_firmware
+ *   i2400m_fw_check
+ *   i2400m_fw_dnload
+ *   release_firmware
+ *
+ * i2400m_fw_dnload
+ *   i2400m_bootrom_init
+ *     i2400m_bm_cmd
+ *     i2400m->bus_reset
+ *   i2400m_dnload_init
+ *     i2400m_dnload_init_signed
+ *     i2400m_dnload_init_nonsigned
+ *       i2400m_download_chunk
+ *         i2400m_bm_cmd
+ *   i2400m_dnload_bcf
+ *     i2400m_bm_cmd
+ *   i2400m_dnload_finalize
+ *     i2400m_bm_cmd
+ *
+ * i2400m_bm_cmd
+ *   i2400m->bus_bm_cmd_send()
+ *   i2400m->bus_bm_wait_for_ack
+ *   __i2400m_bm_ack_verify
+ *
+ * i2400m_bm_cmd_prepare              Used by bus-drivers to prep
+ *                                    commands before sending
+ */
+#include <linux/firmware.h>
+#include <linux/sched.h>
+#include <linux/usb.h>
+#include "i2400m.h"
+
+
+#define D_SUBMODULE fw
+#include "debug-levels.h"
+
+
+static const __le32 i2400m_ACK_BARKER[4] = {
+	__constant_cpu_to_le32(I2400M_ACK_BARKER),
+	__constant_cpu_to_le32(I2400M_ACK_BARKER),
+	__constant_cpu_to_le32(I2400M_ACK_BARKER),
+	__constant_cpu_to_le32(I2400M_ACK_BARKER)
+};
+
+
+/**
+ * Prepare a boot-mode command for delivery
+ *
+ * @cmd: pointer to bootrom header to prepare
+ *
+ * Computes checksum if so needed. After calling this function, DO NOT
+ * modify the command or header as the checksum won't work anymore.
+ *
+ * We do it from here because some times we cannot do it in the
+ * original context the command was sent (it is a const), so when we
+ * copy it to our staging buffer, we add the checksum there.
+ */
+void i2400m_bm_cmd_prepare(struct i2400m_bootrom_header *cmd)
+{
+	if (i2400m_brh_get_use_checksum(cmd)) {
+		int i;
+		u32 checksum = 0;
+		const u32 *checksum_ptr = (void *) cmd->payload;
+		for (i = 0; i < cmd->data_size / 4; i++)
+			checksum += cpu_to_le32(*checksum_ptr++);
+		checksum += cmd->command + cmd->target_addr + cmd->data_size;
+		cmd->block_checksum = cpu_to_le32(checksum);
+	}
+}
+EXPORT_SYMBOL_GPL(i2400m_bm_cmd_prepare);
+
+
+/*
+ * Verify the ack data received
+ *
+ * Given a reply to a boot mode command, chew it and verify everything
+ * is ok.
+ *
+ * @opcode: opcode which generated this ack. For error messages.
+ * @ack: pointer to ack data we received
+ * @ack_size: size of that data buffer
+ * @flags: I2400M_BM_CMD_* flags we called the command with.
+ *
+ * Way too long function -- maybe it should be further split
+ */
+static
+ssize_t __i2400m_bm_ack_verify(struct i2400m *i2400m, int opcode,
+			       struct i2400m_bootrom_header *ack,
+			       size_t ack_size, int flags)
+{
+	ssize_t result = -ENOMEM;
+	struct device *dev = i2400m_dev(i2400m);
+
+	d_fnstart(8, dev, "(i2400m %p opcode %d ack %p size %zu)\n",
+		  i2400m, opcode, ack, ack_size);
+	if (ack_size < sizeof(*ack)) {
+		result = -EIO;
+		dev_err(dev, "boot-mode cmd %d: HW BUG? notification didn't "
+			"return enough data (%zu bytes vs %zu expected)\n",
+			opcode, ack_size, sizeof(*ack));
+		goto error_ack_short;
+	}
+	if (ack_size == sizeof(i2400m_NBOOT_BARKER)
+		 && memcmp(ack, i2400m_NBOOT_BARKER, sizeof(*ack)) == 0) {
+		result = -ERESTARTSYS;
+		i2400m->sboot = 0;
+		d_printf(6, dev, "boot-mode cmd %d: "
+			 "HW non-signed boot barker\n", opcode);
+		goto error_reboot;
+	}
+	if (ack_size == sizeof(i2400m_SBOOT_BARKER)
+		 && memcmp(ack, i2400m_SBOOT_BARKER, sizeof(*ack)) == 0) {
+		result = -ERESTARTSYS;
+		i2400m->sboot = 1;
+		d_printf(6, dev, "boot-mode cmd %d: HW signed reboot barker\n",
+			 opcode);
+		goto error_reboot;
+	}
+	if (ack_size == sizeof(i2400m_ACK_BARKER)
+		 && memcmp(ack, i2400m_ACK_BARKER, sizeof(*ack)) == 0) {
+		result = -EISCONN;
+		d_printf(3, dev, "boot-mode cmd %d: HW reboot ack barker\n",
+			 opcode);
+		goto error_reboot_ack;
+	}
+	result = 0;
+	if (flags & I2400M_BM_CMD_RAW)
+		goto out_raw;
+	ack->data_size = le32_to_cpu(ack->data_size);
+	ack->target_addr = le32_to_cpu(ack->target_addr);
+	ack->block_checksum = le32_to_cpu(ack->block_checksum);
+	d_printf(5, dev, "boot-mode cmd %d: notification for opcode %u "
+		 "response %u csum %u rr %u da %u\n",
+		 opcode, i2400m_brh_get_opcode(ack),
+		 i2400m_brh_get_response(ack),
+		 i2400m_brh_get_use_checksum(ack),
+		 i2400m_brh_get_response_required(ack),
+		 i2400m_brh_get_direct_access(ack));
+	result = -EIO;
+	if (i2400m_brh_get_signature(ack) != 0xcbbc) {
+		dev_err(dev, "boot-mode cmd %d: HW BUG? wrong signature "
+			"0x%04x\n", opcode, i2400m_brh_get_signature(ack));
+		goto error_ack_signature;
+	}
+	if (opcode != -1 && opcode != i2400m_brh_get_opcode(ack)) {
+		dev_err(dev, "boot-mode cmd %d: HW BUG? "
+			"received response for opcode %u, expected %u\n",
+			opcode, i2400m_brh_get_opcode(ack), opcode);
+		goto error_ack_opcode;
+	}
+	if (i2400m_brh_get_response(ack) != 0) {	/* failed? */
+		dev_err(dev, "boot-mode cmd %d: error; hw response %u\n",
+			opcode, i2400m_brh_get_response(ack));
+		goto error_ack_failed;
+	}
+	if (ack_size < ack->data_size + sizeof(*ack)) {
+		dev_err(dev, "boot-mode cmd %d: SW BUG "
+			"driver provided only %zu bytes for %zu bytes "
+			"of data\n", opcode, ack_size,
+			(size_t) le32_to_cpu(ack->data_size) + sizeof(*ack));
+		goto error_ack_short_buffer;
+	}
+	result = ack_size;
+	/* Don't you love this stack of empty targets? Well, I don't
+	 * either, but it helps track exactly who comes in here and
+	 * why :) */
+error_ack_short_buffer:
+error_ack_failed:
+error_ack_opcode:
+error_ack_signature:
+out_raw:
+error_reboot_ack:
+error_reboot:
+error_ack_short:
+	d_fnend(8, dev, "(i2400m %p opcode %d ack %p size %zu) = %d\n",
+		i2400m, opcode, ack, ack_size, (int) result);
+	return result;
+}
+
+
+/**
+ * i2400m_bm_cmd - Execute a boot mode command
+ *
+ * @cmd: buffer containing the command data (pointing at the header).
+ *     This data can be ANYWHERE (for USB, we will copy it to an
+ *     specific buffer). Make sure everything is in proper little
+ *     endian.
+ *
+ *     A raw buffer can be also sent, just cast it and set flags to
+ *     I2400M_BM_CMD_RAW.
+ *
+ *     This function will generate a checksum for you if the
+ *     checksum bit in the command is set (unless I2400M_BM_CMD_RAW
+ *     is set).
+ *
+ *     You can use the i2400m->bm_cmd_buf to stage your commands and
+ *     send them.
+ *
+ *     If NULL, no command is sent (we just wait for an ack).
+ *
+ * @cmd_size: size of the command. Will be auto padded to the
+ *     bus-specific drivers padding requirements.
+ *
+ * @ack: buffer where to place the acknowledgement. If it is a regular
+ *     command response, all fields will be returned with the right,
+ *     native endianess.
+ *
+ *     You *cannot* use i2400m->bm_ack_buf for this buffer.
+ *
+ * @ack_size: size of @ack, 16 aligned; you need to provide at least
+ *     sizeof(*ack) bytes and then enough to contain the return data
+ *     from the command
+ *
+ * @flags: see I2400M_BM_CMD_* above.
+ *
+ * @returns: bytes received by the notification; if < 0, an errno code
+ *     denoting an error or:
+ *
+ *     -ERESTARTSYS  The device has rebooted
+ *
+ * Executes a boot-mode command and waits for a response, doing basic
+ * validation on it; if a zero length response is received, it retries
+ * waiting for a response until a non-zero one is received (timing out
+ * after %I2400M_BOOT_RETRIES retries).
+ */
+static
+ssize_t i2400m_bm_cmd(struct i2400m *i2400m,
+		      const struct i2400m_bootrom_header *cmd, size_t cmd_size,
+		      struct i2400m_bootrom_header *ack, size_t ack_size,
+		      int flags)
+{
+	ssize_t result = -ENOMEM, rx_bytes;
+	struct device *dev = i2400m_dev(i2400m);
+	int opcode = cmd == NULL ? -1 : i2400m_brh_get_opcode(cmd);
+
+	d_fnstart(6, dev, "(i2400m %p cmd %p size %zu ack %p size %zu)\n",
+		  i2400m, cmd, cmd_size, ack, ack_size);
+	BUG_ON(ack_size < sizeof(*ack));
+	BUG_ON(i2400m->boot_mode == 0);
+
+	if (cmd != NULL) {		/* send the command */
+		memcpy(i2400m->bm_cmd_buf, cmd, cmd_size);
+		result = i2400m->bus_bm_cmd_send(i2400m, cmd, cmd_size, flags);
+		if (result < 0)
+			goto error_cmd_send;
+		if ((flags & I2400M_BM_CMD_RAW) == 0)
+			d_printf(5, dev,
+				 "boot-mode cmd %d csum %u rr %u da %u: "
+				 "addr 0x%04x size %u block csum 0x%04x\n",
+				 opcode, i2400m_brh_get_use_checksum(cmd),
+				 i2400m_brh_get_response_required(cmd),
+				 i2400m_brh_get_direct_access(cmd),
+				 cmd->target_addr, cmd->data_size,
+				 cmd->block_checksum);
+	}
+	result = i2400m->bus_bm_wait_for_ack(i2400m, ack, ack_size);
+	if (result < 0) {
+		dev_err(dev, "boot-mode cmd %d: error waiting for an ack: %d\n",
+			opcode, (int) result);	/* bah, %zd doesn't work */
+		goto error_wait_for_ack;
+	}
+	rx_bytes = result;
+	/* verify the ack and read more if neccessary [result is the
+	 * final amount of bytes we get in the ack]  */
+	result = __i2400m_bm_ack_verify(i2400m, opcode, ack, ack_size, flags);
+	if (result < 0)
+		goto error_bad_ack;
+	/* Don't you love this stack of empty targets? Well, I don't
+	 * either, but it helps track exactly who comes in here and
+	 * why :) */
+	result = rx_bytes;
+error_bad_ack:
+error_wait_for_ack:
+error_cmd_send:
+	d_fnend(6, dev, "(i2400m %p cmd %p size %zu ack %p size %zu) = %d\n",
+		i2400m, cmd, cmd_size, ack, ack_size, (int) result);
+	return result;
+}
+
+
+/**
+ * i2400m_download_chunk - write a single chunk of data to the device's memory
+ *
+ * @i2400m: device descriptor
+ * @buf: the buffer to write
+ * @buf_len: length of the buffer to write
+ * @addr: address in the device memory space
+ * @direct: bootrom write mode
+ * @do_csum: should a checksum validation be performed
+ */
+static int i2400m_download_chunk(struct i2400m *i2400m, const void *chunk,
+				 size_t __chunk_len, unsigned long addr,
+				 unsigned int direct, unsigned int do_csum)
+{
+	int ret;
+	size_t chunk_len = ALIGN(__chunk_len, I2400M_PL_PAD);
+	struct device *dev = i2400m_dev(i2400m);
+	struct {
+		struct i2400m_bootrom_header cmd;
+		u8 cmd_payload[chunk_len];
+	} __attribute__((packed)) *buf;
+	struct i2400m_bootrom_header ack;
+
+	d_fnstart(5, dev, "(i2400m %p chunk %p __chunk_len %zu addr 0x%08lx "
+		  "direct %u do_csum %u)\n", i2400m, chunk, __chunk_len,
+		  addr, direct, do_csum);
+	buf = i2400m->bm_cmd_buf;
+	memcpy(buf->cmd_payload, chunk, __chunk_len);
+	memset(buf->cmd_payload + __chunk_len, 0xad, chunk_len - __chunk_len);
+
+	buf->cmd.command = i2400m_brh_command(I2400M_BRH_WRITE,
+					      __chunk_len & 0x3 ? 0 : do_csum,
+					      __chunk_len & 0xf ? 0 : direct);
+	buf->cmd.target_addr = cpu_to_le32(addr);
+	buf->cmd.data_size = cpu_to_le32(__chunk_len);
+	ret = i2400m_bm_cmd(i2400m, &buf->cmd, sizeof(buf->cmd) + chunk_len,
+			    &ack, sizeof(ack), 0);
+	if (ret >= 0)
+		ret = 0;
+	d_fnend(5, dev, "(i2400m %p chunk %p __chunk_len %zu addr 0x%08lx "
+		"direct %u do_csum %u) = %d\n", i2400m, chunk, __chunk_len,
+		addr, direct, do_csum, ret);
+	return ret;
+}
+
+
+/*
+ * Download a BCF file's sections to the device
+ *
+ * @i2400m: device descriptor
+ * @bcf: pointer to firmware data (followed by the payloads). Assumed
+ *       verified and consistent.
+ * @bcf_len: length (in bytes) of the @bcf buffer.
+ *
+ * Returns: < 0 errno code on error or the offset to the jump instruction.
+ *
+ * Given a BCF file, downloads each section (a command and a payload)
+ * to the device's address space. Actually, it just executes each
+ * command i the BCF file.
+ *
+ * The section size has to be aligned to 4 bytes AND the padding has
+ * to be taken from the firmware file, as the signature takes it into
+ * account.
+ */
+static
+ssize_t i2400m_dnload_bcf(struct i2400m *i2400m,
+			  const struct i2400m_bcf_hdr *bcf, size_t bcf_len)
+{
+	ssize_t ret;
+	struct device *dev = i2400m_dev(i2400m);
+	size_t offset,		/* iterator offset */
+		data_size,	/* Size of the data payload */
+		section_size,	/* Size of the whole section (cmd + payload) */
+		section = 1;
+	const struct i2400m_bootrom_header *bh;
+	struct i2400m_bootrom_header ack;
+
+	d_fnstart(3, dev, "(i2400m %p bcf %p bcf_len %zu)\n",
+		  i2400m, bcf, bcf_len);
+	/* Iterate over the command blocks in the BCF file that start
+	 * after the header */
+	offset = le32_to_cpu(bcf->header_len) * sizeof(u32);
+	while (1) {	/* start sending the file */
+		bh = (void *) bcf + offset;
+		data_size = le32_to_cpu(bh->data_size);
+		section_size = ALIGN(sizeof(*bh) + data_size, 4);
+		d_printf(7, dev,
+			 "downloading section #%zu (@%zu %zu B) to 0x%08x\n",
+			 section, offset, sizeof(*bh) + data_size,
+			 le32_to_cpu(bh->target_addr));
+		if (i2400m_brh_get_opcode(bh) == I2400M_BRH_SIGNED_JUMP) {
+			/* Secure boot needs to stop here */
+			d_printf(5, dev,  "signed jump found @%zu\n", offset);
+			break;
+		}
+		if (offset + section_size == bcf_len)
+			/* Non-secure boot stops here */
+			break;
+		if (offset + section_size > bcf_len) {
+			dev_err(dev, "fw %s: bad section #%zu, "
+				"end (@%zu) beyond EOF (@%zu)\n",
+				i2400m->bus_fw_name, section,
+				offset + section_size,  bcf_len);
+			ret = -EINVAL;
+			goto error_section_beyond_eof;
+		}
+		__i2400m_msleep(20);
+		ret = i2400m_bm_cmd(i2400m, bh, section_size,
+				    &ack, sizeof(ack), I2400M_BM_CMD_RAW);
+		if (ret < 0) {
+			dev_err(dev, "fw %s: section #%zu (@%zu %zu B) "
+				"failed %d\n", i2400m->bus_fw_name, section,
+				offset, sizeof(*bh) + data_size, (int) ret);
+			goto error_send;
+		}
+		offset += section_size;
+		section++;
+	}
+	ret = offset;
+error_section_beyond_eof:
+error_send:
+	d_fnend(3, dev, "(i2400m %p bcf %p bcf_len %zu) = %d\n",
+		i2400m, bcf, bcf_len, (int) ret);
+	return ret;
+}
+
+
+/*
+ * Do the final steps of uploading firmware
+ *
+ * Depending on the boot mode (signed vs non-signed), different
+ * actions need to be taken.
+ */
+static
+int i2400m_dnload_finalize(struct i2400m *i2400m,
+			   const struct i2400m_bcf_hdr *bcf, size_t offset)
+{
+	int ret = 0;
+	struct device *dev = i2400m_dev(i2400m);
+	struct i2400m_bootrom_header *cmd, ack;
+	struct {
+		struct i2400m_bootrom_header cmd;
+		u8 cmd_pl[0];
+	} __attribute__((packed)) *cmd_buf;
+	size_t signature_block_offset, signature_block_size;
+
+	d_fnstart(3, dev, "offset %zu\n", offset);
+	cmd = (void *) bcf + offset;
+	if (i2400m->sboot == 0) {
+		struct i2400m_bootrom_header jump_ack;
+		d_printf(3, dev, "unsecure boot, jumping to 0x%08x\n",
+			le32_to_cpu(cmd->target_addr));
+		i2400m_brh_set_opcode(cmd, I2400M_BRH_JUMP);
+		cmd->data_size = 0;
+		ret = i2400m_bm_cmd(i2400m, cmd, sizeof(*cmd),
+				    &jump_ack, sizeof(jump_ack), 0);
+	} else {
+		d_printf(3, dev, "secure boot, jumping to 0x%08x\n",
+			 le32_to_cpu(cmd->target_addr));
+		cmd_buf = i2400m->bm_cmd_buf;
+		memcpy(&cmd_buf->cmd, cmd, sizeof(*cmd));
+		signature_block_offset =
+			sizeof(*bcf)
+			+ le32_to_cpu(bcf->key_size) * sizeof(u32)
+			+ le32_to_cpu(bcf->exponent_size) * sizeof(u32);
+		signature_block_size =
+			le32_to_cpu(bcf->modulus_size) * sizeof(u32);
+		memcpy(cmd_buf->cmd_pl, (void *) bcf + signature_block_offset,
+		       signature_block_size);
+		ret = i2400m_bm_cmd(i2400m, &cmd_buf->cmd,
+				    sizeof(cmd_buf->cmd) + signature_block_size,
+				    &ack, sizeof(ack), I2400M_BM_CMD_RAW);
+	}
+	d_fnend(3, dev, "returning %d\n", ret);
+	return ret;
+}
+
+
+/**
+ * i2400m_bootrom_init - Reboots a powered device into boot mode
+ *
+ * @i2400m: device descriptor
+ * @flags:
+ *      I2400M_BRI_SOFT: a reboot notification has been seen
+ *          already, so don't wait for it.
+ *
+ *      I2400M_BRI_NO_REBOOT: Don't send a reboot command, but wait
+ *          for a reboot barker notification. This is a one shot; if
+ *          the state machine needs to send a reboot command it will.
+ *
+ * Returns:
+ *
+ *     < 0 errno code on error, 0 if ok.
+ *
+ *     i2400m->sboot set to 0 for unsecure boot process, 1 for secure
+ *     boot process.
+ *
+ * Description:
+ *
+ * Tries hard enough to put the device in boot-mode. There are two
+ * main phases to this:
+ *
+ * a. (1) send a reboot command and (2) get a reboot barker
+ * b. (1) ack the reboot sending a reboot barker and (2) getting an
+ *        ack barker in return
+ *
+ * We want to skip (a) in some cases [soft]. The state machine is
+ * horrible, but it is basically: on each phase, send what has to be
+ * sent (if any), wait for the answer and act on the answer. We might
+ * have to backtrack and retry, so we keep a max tries counter for
+ * that.
+ *
+ * If we get a timeout after sending a warm reset, we do it again.
+ */
+int i2400m_bootrom_init(struct i2400m *i2400m, enum i2400m_bri flags)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	struct i2400m_bootrom_header *cmd;
+	struct i2400m_bootrom_header ack;
+	int count = I2400M_BOOT_RETRIES;
+	int ack_timeout_cnt = 1;
+
+	BUILD_BUG_ON(sizeof(*cmd) != sizeof(i2400m_NBOOT_BARKER));
+	BUILD_BUG_ON(sizeof(ack) != sizeof(i2400m_ACK_BARKER));
+
+	d_fnstart(4, dev, "(i2400m %p flags 0x%08x)\n", i2400m, flags);
+	result = -ENOMEM;
+	cmd = i2400m->bm_cmd_buf;
+	if (flags & I2400M_BRI_SOFT)
+		goto do_reboot_ack;
+do_reboot:
+	if (--count < 0)
+		goto error_timeout;
+	d_printf(4, dev, "device reboot: reboot command [%d # left]\n",
+		 count);
+	if ((flags & I2400M_BRI_NO_REBOOT) == 0)
+		i2400m->bus_reset(i2400m, I2400M_RT_WARM);
+	result = i2400m_bm_cmd(i2400m, NULL, 0, &ack, sizeof(ack),
+			       I2400M_BM_CMD_RAW);
+	flags &= ~I2400M_BRI_NO_REBOOT;
+	switch (result) {
+	case -ERESTARTSYS:
+		d_printf(4, dev, "device reboot: got reboot barker\n");
+		break;
+	case -EISCONN:	/* we don't know how it got here...but we follow it */
+		d_printf(4, dev, "device reboot: got ack barker - whatever\n");
+		goto do_reboot;
+	case -ETIMEDOUT:	/* device has timed out, we might be in boot
+				 * mode already and expecting an ack, let's try
+				 * that */
+		dev_info(dev, "warm reset timed out, trying an ack\n");
+		goto do_reboot_ack;
+	case -EPROTO:
+	case -ESHUTDOWN:	/* dev is gone */
+	case -EINTR:		/* user cancelled */
+		goto error_dev_gone;
+	default:
+		dev_err(dev, "device reboot: error %d while waiting "
+			"for reboot barker - rebooting\n", result);
+		goto do_reboot;
+	}
+	/* At this point we ack back with 4 REBOOT barkers and expect
+	 * 4 ACK barkers. This is ugly, as we send a raw command --
+	 * hence the cast. _bm_cmd() will catch the reboot ack
+	 * notification and report it as -EISCONN. */
+do_reboot_ack:
+	d_printf(4, dev, "device reboot ack: sending ack [%d # left]\n", count);
+	if (i2400m->sboot == 0)
+		memcpy(cmd, i2400m_NBOOT_BARKER,
+		       sizeof(i2400m_NBOOT_BARKER));
+	else
+		memcpy(cmd, i2400m_SBOOT_BARKER,
+		       sizeof(i2400m_SBOOT_BARKER));
+	result = i2400m_bm_cmd(i2400m, cmd, sizeof(*cmd),
+			       &ack, sizeof(ack), I2400M_BM_CMD_RAW);
+	switch (result) {
+	case -ERESTARTSYS:
+		d_printf(4, dev, "reboot ack: got reboot barker - retrying\n");
+		if (--count < 0)
+			goto error_timeout;
+		goto do_reboot_ack;
+	case -EISCONN:
+		d_printf(4, dev, "reboot ack: got ack barker - good\n");
+		break;
+	case -ETIMEDOUT:	/* no response, maybe it is the other type? */
+		if (ack_timeout_cnt-- >= 0) {
+			d_printf(4, dev, "reboot ack timedout: "
+				 "trying the other type?\n");
+			i2400m->sboot = !i2400m->sboot;
+			goto do_reboot_ack;
+		} else {
+			dev_err(dev, "reboot ack timedout too long: "
+				"trying reboot\n");
+			goto do_reboot;
+		}
+		break;
+	case -EPROTO:
+	case -ESHUTDOWN:	/* dev is gone */
+		goto error_dev_gone;
+	default:
+		dev_err(dev, "device reboot ack: error %d while waiting for "
+			"reboot ack barker - rebooting\n", result);
+		goto do_reboot;
+	}
+	d_printf(2, dev, "device reboot ack: got ack barker - boot done\n");
+	result = 0;
+exit_timeout:
+error_dev_gone:
+	d_fnend(4, dev, "(i2400m %p flags 0x%08x) = %d\n",
+		i2400m, flags, result);
+	return result;
+
+error_timeout:
+	dev_err(dev, "Timed out waiting for reboot ack, resetting\n");
+	i2400m->bus_reset(i2400m, I2400M_RT_BUS);
+	result = -ETIMEDOUT;
+	goto exit_timeout;
+}
+
+
+/*
+ * Read the MAC addr
+ *
+ * The position this function reads is fixed in device memory and
+ * always available, even without firmware.
+ *
+ * Note we specify we want to read only six bytes, but provide space
+ * for 16, as we always get it rounded up.
+ */
+int i2400m_read_mac_addr(struct i2400m *i2400m)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	struct net_device *net_dev = i2400m->wimax_dev.net_dev;
+	struct i2400m_bootrom_header *cmd;
+	struct {
+		struct i2400m_bootrom_header ack;
+		u8 ack_pl[16];
+	} __attribute__((packed)) ack_buf;
+
+	d_fnstart(5, dev, "(i2400m %p)\n", i2400m);
+	cmd = i2400m->bm_cmd_buf;
+	cmd->command = i2400m_brh_command(I2400M_BRH_READ, 0, 1);
+	cmd->target_addr = cpu_to_le32(0x00203fe8);
+	cmd->data_size = cpu_to_le32(6);
+	result = i2400m_bm_cmd(i2400m, cmd, sizeof(*cmd),
+			       &ack_buf.ack, sizeof(ack_buf), 0);
+	if (result < 0) {
+		dev_err(dev, "BM: read mac addr failed: %d\n", result);
+		goto error_read_mac;
+	}
+	d_printf(2, dev,
+		 "mac addr is %02x:%02x:%02x:%02x:%02x:%02x\n",
+		 ack_buf.ack_pl[0], ack_buf.ack_pl[1],
+		 ack_buf.ack_pl[2], ack_buf.ack_pl[3],
+		 ack_buf.ack_pl[4], ack_buf.ack_pl[5]);
+	if (i2400m->bus_bm_mac_addr_impaired == 1) {
+		ack_buf.ack_pl[0] = 0x00;
+		ack_buf.ack_pl[1] = 0x16;
+		ack_buf.ack_pl[2] = 0xd3;
+		get_random_bytes(&ack_buf.ack_pl[3], 3);
+		dev_err(dev, "BM is MAC addr impaired, faking MAC addr to "
+			"mac addr is %02x:%02x:%02x:%02x:%02x:%02x\n",
+			ack_buf.ack_pl[0], ack_buf.ack_pl[1],
+			ack_buf.ack_pl[2], ack_buf.ack_pl[3],
+			ack_buf.ack_pl[4], ack_buf.ack_pl[5]);
+		result = 0;
+	}
+	net_dev->addr_len = ETH_ALEN;
+	memcpy(net_dev->perm_addr, ack_buf.ack_pl, ETH_ALEN);
+	memcpy(net_dev->dev_addr, ack_buf.ack_pl, ETH_ALEN);
+error_read_mac:
+	d_fnend(5, dev, "(i2400m %p) = %d\n", i2400m, result);
+	return result;
+}
+
+
+/*
+ * Initialize a non signed boot
+ *
+ * This implies sending some magic values to the device's memory. Note
+ * we convert the values to little endian in the same array
+ * declaration.
+ */
+static
+int i2400m_dnload_init_nonsigned(struct i2400m *i2400m)
+{
+#define POKE(a, d) {					\
+	.address = __constant_cpu_to_le32(a),		\
+	.data = __constant_cpu_to_le32(d)		\
+}
+	static const struct {
+		__le32 address;
+		__le32 data;
+	} i2400m_pokes[] = {
+		POKE(0x081A58, 0xA7810230),
+		POKE(0x080040, 0x00000000),
+		POKE(0x080048, 0x00000082),
+		POKE(0x08004C, 0x0000081F),
+		POKE(0x080054, 0x00000085),
+		POKE(0x080058, 0x00000180),
+		POKE(0x08005C, 0x00000018),
+		POKE(0x080060, 0x00000010),
+		POKE(0x080574, 0x00000001),
+		POKE(0x080550, 0x00000005),
+		POKE(0xAE0000, 0x00000000),
+	};
+#undef POKE
+	unsigned i;
+	int ret;
+	struct device *dev = i2400m_dev(i2400m);
+
+	dev_warn(dev, "WARNING!!! non-signed boot UNTESTED PATH!\n");
+
+	d_fnstart(5, dev, "(i2400m %p)\n", i2400m);
+	for (i = 0; i < ARRAY_SIZE(i2400m_pokes); i++) {
+		ret = i2400m_download_chunk(i2400m, &i2400m_pokes[i].data,
+					    sizeof(i2400m_pokes[i].data),
+					    i2400m_pokes[i].address, 1, 1);
+		if (ret < 0)
+			break;
+	}
+	d_fnend(5, dev, "(i2400m %p) = %d\n", i2400m, ret);
+	return ret;
+}
+
+
+/*
+ * Initialize the signed boot process
+ *
+ * @i2400m: device descriptor
+ *
+ * @bcf_hdr: pointer to the firmware header; assumes it is fully in
+ *     memory (it has gone through basic validation).
+ *
+ * Returns: 0 if ok, < 0 errno code on error, -ERESTARTSYS if the hw
+ *     rebooted.
+ *
+ * This writes the firmware BCF header to the device using the
+ * HASH_PAYLOAD_ONLY command.
+ */
+static
+int i2400m_dnload_init_signed(struct i2400m *i2400m,
+			      const struct i2400m_bcf_hdr *bcf_hdr)
+{
+	int ret;
+	struct device *dev = i2400m_dev(i2400m);
+	struct {
+		struct i2400m_bootrom_header cmd;
+		struct i2400m_bcf_hdr cmd_pl;
+	} __attribute__((packed)) *cmd_buf;
+	struct i2400m_bootrom_header ack;
+
+	d_fnstart(5, dev, "(i2400m %p bcf_hdr %p)\n", i2400m, bcf_hdr);
+	cmd_buf = i2400m->bm_cmd_buf;
+	cmd_buf->cmd.command =
+		i2400m_brh_command(I2400M_BRH_HASH_PAYLOAD_ONLY, 0, 0);
+	cmd_buf->cmd.target_addr = 0;
+	cmd_buf->cmd.data_size = cpu_to_le32(sizeof(cmd_buf->cmd_pl));
+	memcpy(&cmd_buf->cmd_pl, bcf_hdr, sizeof(*bcf_hdr));
+	ret = i2400m_bm_cmd(i2400m, &cmd_buf->cmd, sizeof(*cmd_buf),
+			    &ack, sizeof(ack), 0);
+	if (ret >= 0)
+		ret = 0;
+	d_fnend(5, dev, "(i2400m %p bcf_hdr %p) = %d\n", i2400m, bcf_hdr, ret);
+	return ret;
+}
+
+
+/*
+ * Initialize the firmware download at the device size
+ *
+ * Multiplex to the one that matters based on the device's mode
+ * (signed or non-signed).
+ */
+static
+int i2400m_dnload_init(struct i2400m *i2400m, const struct i2400m_bcf_hdr *bcf)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	u32 module_id = le32_to_cpu(bcf->module_id);
+
+	if (i2400m->sboot == 0
+	    && (module_id & I2400M_BCF_MOD_ID_POKES) == 0) {
+		/* non-signed boot process without pokes */
+		result = i2400m_dnload_init_nonsigned(i2400m);
+		if (result == -ERESTARTSYS)
+			return result;
+		if (result < 0)
+			dev_err(dev, "fw %s: non-signed download "
+				"initialization failed: %d\n",
+				i2400m->bus_fw_name, result);
+	} else if (i2400m->sboot == 0
+		 && (module_id & I2400M_BCF_MOD_ID_POKES)) {
+		/* non-signed boot process with pokes, nothing to do */
+		result = 0;
+	} else {		 /* signed boot process */
+		result = i2400m_dnload_init_signed(i2400m, bcf);
+		if (result == -ERESTARTSYS)
+			return result;
+		if (result < 0)
+			dev_err(dev, "fw %s: signed boot download "
+				"initialization failed: %d\n",
+				i2400m->bus_fw_name, result);
+	}
+	return result;
+}
+
+
+/*
+ * Run quick consistency tests on the firmware file
+ *
+ * Check for the firmware being made for the i2400m device,
+ * etc...These checks are mostly informative, as the device will make
+ * them too; but the driver's response is more informative on what
+ * went wrong.
+ */
+static
+int i2400m_fw_check(struct i2400m *i2400m,
+		    const struct i2400m_bcf_hdr *bcf,
+		    size_t bcf_size)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	unsigned module_type, header_len, major_version, minor_version,
+		module_id, module_vendor, date, size;
+
+	/* Check hard errors */
+	result = -EINVAL;
+	if (bcf_size < sizeof(*bcf)) {	/* big enough header? */
+		dev_err(dev, "firmware %s too short: "
+			"%zu B vs %zu (at least) expected\n",
+			i2400m->bus_fw_name, bcf_size, sizeof(*bcf));
+		goto error;
+	}
+
+	module_type = bcf->module_type;
+	header_len = sizeof(u32) * le32_to_cpu(bcf->header_len);
+	major_version = le32_to_cpu(bcf->header_version) & 0xffff0000 >> 16;
+	minor_version = le32_to_cpu(bcf->header_version) & 0x0000ffff;
+	module_id = le32_to_cpu(bcf->module_id);
+	module_vendor = le32_to_cpu(bcf->module_vendor);
+	date = le32_to_cpu(bcf->date);
+	size = sizeof(u32) * le32_to_cpu(bcf->size);
+
+	if (bcf_size != size) {		/* annoyingly paranoid */
+		dev_err(dev, "firmware %s: bad size, got "
+			"%zu B vs %u expected\n",
+			i2400m->bus_fw_name, bcf_size, size);
+		goto error;
+	}
+
+	d_printf(2, dev, "type 0x%x id 0x%x vendor 0x%x; header v%u.%u (%zu B) "
+		 "date %08x (%zu B)\n",
+		 module_type, module_id, module_vendor,
+		 major_version, minor_version, (size_t) header_len,
+		 date, (size_t) size);
+
+	if (module_type != 6) {		/* built for the right hardware? */
+		dev_err(dev, "bad fw %s: unexpected module type 0x%x; "
+			"aborting\n", i2400m->bus_fw_name, module_type);
+		goto error;
+	}
+
+	/* Check soft-er errors */
+	result = 0;
+	if (module_vendor != 0x8086)
+		dev_err(dev, "bad fw %s? unexpected vendor 0x%04x\n",
+			i2400m->bus_fw_name, module_vendor);
+	if (date < 0x20080300)
+		dev_err(dev, "bad fw %s? build date too old %08x\n",
+			i2400m->bus_fw_name, date);
+error:
+	return result;
+}
+
+
+/*
+ * Download the firmware to the device
+ *
+ * @i2400m: device descriptor
+ * @bcf: pointer to loaded (and minimally verified for consistency)
+ *    firmware
+ * @bcf_size: size of the @bcf buffer (header plus payloads)
+ *
+ * The process for doing this is described in this file's header.
+ *
+ * Note we only reinitialize boot-mode if the flags say so. Some hw
+ * iterations need it, some don't. In any case, if we loop, we always
+ * need to reinitialize the boot room, hence the flags modification.
+ */
+static
+int i2400m_fw_dnload(struct i2400m *i2400m, const struct i2400m_bcf_hdr *bcf,
+		     size_t bcf_size, enum i2400m_bri flags)
+{
+	int ret = 0;
+	struct device *dev = i2400m_dev(i2400m);
+	int count = I2400M_BOOT_RETRIES;
+
+	d_fnstart(5, dev, "(i2400m %p bcf %p size %zu)\n",
+		  i2400m, bcf, bcf_size);
+	i2400m->boot_mode = 1;
+hw_reboot:
+	if (count-- == 0) {
+		ret = -ERESTARTSYS;
+		dev_err(dev, "device rebooted too many times, aborting\n");
+		goto error_too_many_reboots;
+	}
+	if (flags & I2400M_BRI_MAC_REINIT) {
+		ret = i2400m_bootrom_init(i2400m, flags);
+		if (ret < 0) {
+			dev_err(dev, "bootrom init failed: %d\n", ret);
+			goto error_bootrom_init;
+		}
+	}
+	flags |= I2400M_BRI_MAC_REINIT;
+
+	/*
+	 * Initialize the download, push the bytes to the device and
+	 * then jump to the new firmware. Note @ret is passed with the
+	 * offset of the jump instruction to _dnload_finalize()
+	 */
+	ret = i2400m_dnload_init(i2400m, bcf);	/* Init device's dnload */
+	if (ret == -ERESTARTSYS)
+		goto error_dev_rebooted;
+	if (ret < 0)
+		goto error_dnload_init;
+
+	ret = i2400m_dnload_bcf(i2400m, bcf, bcf_size);
+	if (ret == -ERESTARTSYS)
+		goto error_dev_rebooted;
+	if (ret < 0) {
+		dev_err(dev, "fw %s: download failed: %d\n",
+			i2400m->bus_fw_name, ret);
+		goto error_dnload_bcf;
+	}
+
+	ret = i2400m_dnload_finalize(i2400m, bcf, ret);
+	if (ret == -ERESTARTSYS)
+		goto error_dev_rebooted;
+	if (ret < 0) {
+		dev_err(dev, "fw %s: "
+			"download finalization failed: %d\n",
+			i2400m->bus_fw_name, ret);
+		goto error_dnload_finalize;
+	}
+
+	d_printf(2, dev, "fw %s successfully uploaded\n",
+		 i2400m->bus_fw_name);
+	i2400m->boot_mode = 0;
+error_dnload_finalize:
+error_dnload_bcf:
+error_dnload_init:
+error_bootrom_init:
+error_too_many_reboots:
+	d_fnend(5, dev, "(i2400m %p bcf %p size %zu) = %d\n",
+		i2400m, bcf, bcf_size, ret);
+	return ret;
+
+error_dev_rebooted:
+	dev_err(dev, "device rebooted, %d tries left\n", count);
+	/* we got the notification already, no need to wait for it again */
+	flags |= I2400M_BRI_SOFT;
+	goto hw_reboot;
+}
+
+
+/**
+ * i2400m_dev_bootstrap - Bring the device to a known state and upload firmware
+ *
+ * @i2400m: device descriptor
+ *
+ * Returns: >= 0 if ok, < 0 errno code on error.
+ *
+ * This sets up the firmware upload environment, loads the firmware
+ * file from disk, verifies and then calls the firmware upload process
+ * per se.
+ *
+ * Can be called either from probe, or after a warm reset.  Can not be
+ * called from within an interrupt.  All the flow in this code is
+ * single-threade; all I/Os are synchronous.
+ */
+int i2400m_dev_bootstrap(struct i2400m *i2400m, enum i2400m_bri flags)
+{
+	int ret = 0;
+	struct device *dev = i2400m_dev(i2400m);
+	const struct firmware *fw;
+	const struct i2400m_bcf_hdr *bcf;	/* Firmware data */
+
+	d_fnstart(5, dev, "(i2400m %p)\n", i2400m);
+	/* Load firmware files to memory. */
+	ret = request_firmware(&fw, i2400m->bus_fw_name, dev);
+	if (ret) {
+		dev_err(dev, "fw %s: request failed: %d\n",
+			i2400m->bus_fw_name, ret);
+		goto error_fw_req;
+	}
+	bcf = (void *) fw->data;
+
+	ret = i2400m_fw_check(i2400m, bcf, fw->size);
+	if (ret < 0)
+		goto error_fw_bad;
+	ret = i2400m_fw_dnload(i2400m, bcf, fw->size, flags);
+error_fw_bad:
+	release_firmware(fw);
+error_fw_req:
+	d_fnend(5, dev, "(i2400m %p) = %d\n", i2400m, ret);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(i2400m_dev_bootstrap);
diff --git a/drivers/net/wimax/i2400m/i2400m-sdio.h b/drivers/net/wimax/i2400m/i2400m-sdio.h
new file mode 100644
index 000000000000..08c2fb739234
--- /dev/null
+++ b/drivers/net/wimax/i2400m/i2400m-sdio.h
@@ -0,0 +1,132 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * SDIO-specific i2400m driver definitions
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Brian Bian <brian.bian@intel.com>
+ * Dirk Brandewie <dirk.j.brandewie@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ * Yanir Lubetkin <yanirx.lubetkin@intel.com>
+ *  - Initial implementation
+ *
+ *
+ * This driver implements the bus-specific part of the i2400m for
+ * SDIO. Check i2400m.h for a generic driver description.
+ *
+ * ARCHITECTURE
+ *
+ * This driver sits under the bus-generic i2400m driver, providing the
+ * connection to the device.
+ *
+ * When probed, all the function pointers are setup and then the
+ * bus-generic code called. The generic driver will then use the
+ * provided pointers for uploading firmware (i2400ms_bus_bm*() in
+ * sdio-fw.c) and then setting up the device (i2400ms_dev_*() in
+ * sdio.c).
+ *
+ * Once firmware is uploaded, TX functions (sdio-tx.c) are called when
+ * data is ready for transmission in the TX fifo; then the SDIO IRQ is
+ * fired and data is available (sdio-rx.c), it is sent to the generic
+ * driver for processing with i2400m_rx.
+ */
+
+#ifndef __I2400M_SDIO_H__
+#define __I2400M_SDIO_H__
+
+#include "i2400m.h"
+
+/* Host-Device interface for SDIO */
+enum {
+	I2400MS_BLK_SIZE = 256,
+	I2400MS_PL_SIZE_MAX = 0x3E00,
+
+	I2400MS_DATA_ADDR = 0x0,
+	I2400MS_INTR_STATUS_ADDR = 0x13,
+	I2400MS_INTR_CLEAR_ADDR = 0x13,
+	I2400MS_INTR_ENABLE_ADDR = 0x14,
+	I2400MS_INTR_GET_SIZE_ADDR = 0x2C,
+	/* The number of ticks to wait for the device to signal that
+	 * it is ready */
+	I2400MS_INIT_SLEEP_INTERVAL = 10,
+};
+
+
+/**
+ * struct i2400ms - descriptor for a SDIO connected i2400m
+ *
+ * @i2400m: bus-generic i2400m implementation; has to be first (see
+ *     it's documentation in i2400m.h).
+ *
+ * @func: pointer to our SDIO function
+ *
+ * @tx_worker: workqueue struct used to TX data when the bus-generic
+ *     code signals packets are pending for transmission to the device.
+ *
+ * @tx_workqueue: workqeueue used for data TX; we don't use the
+ *     system's workqueue as that might cause deadlocks with code in
+ *     the bus-generic driver.
+ */
+struct i2400ms {
+	struct i2400m i2400m;		/* FIRST! See doc */
+	struct sdio_func *func;
+
+	struct work_struct tx_worker;
+	struct workqueue_struct *tx_workqueue;
+	char tx_wq_name[32];
+
+	struct dentry *debugfs_dentry;
+};
+
+
+static inline
+void i2400ms_init(struct i2400ms *i2400ms)
+{
+	i2400m_init(&i2400ms->i2400m);
+}
+
+
+extern int i2400ms_rx_setup(struct i2400ms *);
+extern void i2400ms_rx_release(struct i2400ms *);
+extern ssize_t __i2400ms_rx_get_size(struct i2400ms *);
+
+extern int i2400ms_tx_setup(struct i2400ms *);
+extern void i2400ms_tx_release(struct i2400ms *);
+extern void i2400ms_bus_tx_kick(struct i2400m *);
+
+extern ssize_t i2400ms_bus_bm_cmd_send(struct i2400m *,
+				       const struct i2400m_bootrom_header *,
+				       size_t, int);
+extern ssize_t i2400ms_bus_bm_wait_for_ack(struct i2400m *,
+					   struct i2400m_bootrom_header *,
+					   size_t);
+#endif /* #ifndef __I2400M_SDIO_H__ */
diff --git a/drivers/net/wimax/i2400m/i2400m-usb.h b/drivers/net/wimax/i2400m/i2400m-usb.h
new file mode 100644
index 000000000000..6f76558b170f
--- /dev/null
+++ b/drivers/net/wimax/i2400m/i2400m-usb.h
@@ -0,0 +1,264 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * USB-specific i2400m driver definitions
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ * Yanir Lubetkin <yanirx.lubetkin@intel.com>
+ *  - Initial implementation
+ *
+ *
+ * This driver implements the bus-specific part of the i2400m for
+ * USB. Check i2400m.h for a generic driver description.
+ *
+ * ARCHITECTURE
+ *
+ * This driver listens to notifications sent from the notification
+ * endpoint (in usb-notif.c); when data is ready to read, the code in
+ * there schedules a read from the device (usb-rx.c) and then passes
+ * the data to the generic RX code (rx.c).
+ *
+ * When the generic driver needs to send data (network or control), it
+ * queues up in the TX FIFO (tx.c) and that will notify the driver
+ * through the i2400m->bus_tx_kick() callback
+ * (usb-tx.c:i2400mu_bus_tx_kick) which will send the items in the
+ * FIFO queue.
+ *
+ * This driver, as well, implements the USB-specific ops for the generic
+ * driver to be able to setup/teardown communication with the device
+ * [i2400m_bus_dev_start() and i2400m_bus_dev_stop()], reseting the
+ * device [i2400m_bus_reset()] and performing firmware upload
+ * [i2400m_bus_bm_cmd() and i2400_bus_bm_wait_for_ack()].
+ */
+
+#ifndef __I2400M_USB_H__
+#define __I2400M_USB_H__
+
+#include "i2400m.h"
+#include <linux/kthread.h>
+
+
+/*
+ * Error Density Count: cheapo error density (over time) counter
+ *
+ * Originally by Reinette Chatre <reinette.chatre@intel.com>
+ *
+ * Embed an 'struct edc' somewhere. Each time there is a soft or
+ * retryable error, call edc_inc() and check if the error top
+ * watermark has been reached.
+ */
+enum {
+	EDC_MAX_ERRORS = 10,
+	EDC_ERROR_TIMEFRAME = HZ,
+};
+
+/* error density counter */
+struct edc {
+	unsigned long timestart;
+	u16 errorcount;
+};
+
+static inline void edc_init(struct edc *edc)
+{
+	edc->timestart = jiffies;
+}
+
+/**
+ * edc_inc - report a soft error and check if we are over the watermark
+ *
+ * @edc: pointer to error density counter.
+ * @max_err: maximum number of errors we can accept over the timeframe
+ * @timeframe: lenght of the timeframe (in jiffies).
+ *
+ * Returns: !0 1 if maximum acceptable errors per timeframe has been
+ *     exceeded. 0 otherwise.
+ *
+ * This is way to determine if the number of acceptable errors per time
+ * period has been exceeded. It is not accurate as there are cases in which
+ * this scheme will not work, for example if there are periodic occurences
+ * of errors that straddle updates to the start time. This scheme is
+ * sufficient for our usage.
+ *
+ * To use, embed a 'struct edc' somewhere, initialize it with
+ * edc_init() and when an error hits:
+ *
+ * if (do_something_fails_with_a_soft_error) {
+ *        if (edc_inc(&my->edc, MAX_ERRORS, MAX_TIMEFRAME))
+ * 	           Ops, hard error, do something about it
+ *        else
+ *                 Retry or ignore, depending on whatever
+ * }
+ */
+static inline int edc_inc(struct edc *edc, u16 max_err, u16 timeframe)
+{
+	unsigned long now;
+
+	now = jiffies;
+	if (now - edc->timestart > timeframe) {
+		edc->errorcount = 1;
+		edc->timestart = now;
+	} else if (++edc->errorcount > max_err) {
+		edc->errorcount = 0;
+		edc->timestart = now;
+		return 1;
+	}
+	return 0;
+}
+
+/* Host-Device interface for USB */
+enum {
+	I2400MU_MAX_NOTIFICATION_LEN = 256,
+	I2400MU_BLK_SIZE = 16,
+	I2400MU_PL_SIZE_MAX = 0x3EFF,
+
+	/* Endpoints */
+	I2400MU_EP_BULK_OUT = 0,
+	I2400MU_EP_NOTIFICATION,
+	I2400MU_EP_RESET_COLD,
+	I2400MU_EP_BULK_IN,
+};
+
+
+/**
+ * struct i2400mu - descriptor for a USB connected i2400m
+ *
+ * @i2400m: bus-generic i2400m implementation; has to be first (see
+ *     it's documentation in i2400m.h).
+ *
+ * @usb_dev: pointer to our USB device
+ *
+ * @usb_iface: pointer to our USB interface
+ *
+ * @urb_edc: error density counter; used to keep a density-on-time tab
+ *     on how many soft (retryable or ignorable) errors we get. If we
+ *     go over the threshold, we consider the bus transport is failing
+ *     too much and reset.
+ *
+ * @notif_urb: URB for receiving notifications from the device.
+ *
+ * @tx_kthread: thread we use for data TX. We use a thread because in
+ *     order to do deep power saving and put the device to sleep, we
+ *     need to call usb_autopm_*() [blocking functions].
+ *
+ * @tx_wq: waitqueue for the TX kthread to sleep when there is no data
+ *     to be sent; when more data is available, it is woken up by
+ *     i2400mu_bus_tx_kick().
+ *
+ * @rx_kthread: thread we use for data RX. We use a thread because in
+ *     order to do deep power saving and put the device to sleep, we
+ *     need to call usb_autopm_*() [blocking functions].
+ *
+ * @rx_wq: waitqueue for the RX kthread to sleep when there is no data
+ *     to receive. When data is available, it is woken up by
+ *     usb-notif.c:i2400mu_notification_grok().
+ *
+ * @rx_pending_count: number of rx-data-ready notifications that were
+ *     still not handled by the RX kthread.
+ *
+ * @rx_size: current RX buffer size that is being used.
+ *
+ * @rx_size_acc: accumulator of the sizes of the previous read
+ *     transactions.
+ *
+ * @rx_size_cnt: number of read transactions accumulated in
+ *     @rx_size_acc.
+ *
+ * @do_autopm: disable(0)/enable(>0) calling the
+ *     usb_autopm_get/put_interface() barriers when executing
+ *     commands. See doc in i2400mu_suspend() for more information.
+ *
+ * @rx_size_auto_shrink: if true, the rx_size is shrinked
+ *     automatically based on the average size of the received
+ *     transactions. This allows the receive code to allocate smaller
+ *     chunks of memory and thus reduce pressure on the memory
+ *     allocator by not wasting so much space. By default it is
+ *     enabled.
+ *
+ * @debugfs_dentry: hookup for debugfs files.
+ *     These have to be in a separate directory, a child of
+ *     (wimax_dev->debugfs_dentry) so they can be removed when the
+ *     module unloads, as we don't keep each dentry.
+ */
+struct i2400mu {
+	struct i2400m i2400m;		/* FIRST! See doc */
+
+	struct usb_device *usb_dev;
+	struct usb_interface *usb_iface;
+	struct edc urb_edc;		/* Error density counter */
+
+	struct urb *notif_urb;
+	struct task_struct *tx_kthread;
+	wait_queue_head_t tx_wq;
+
+	struct task_struct *rx_kthread;
+	wait_queue_head_t rx_wq;
+	atomic_t rx_pending_count;
+	size_t rx_size, rx_size_acc, rx_size_cnt;
+	atomic_t do_autopm;
+	u8 rx_size_auto_shrink;
+
+	struct dentry *debugfs_dentry;
+};
+
+
+static inline
+void i2400mu_init(struct i2400mu *i2400mu)
+{
+	i2400m_init(&i2400mu->i2400m);
+	edc_init(&i2400mu->urb_edc);
+	init_waitqueue_head(&i2400mu->tx_wq);
+	atomic_set(&i2400mu->rx_pending_count, 0);
+	init_waitqueue_head(&i2400mu->rx_wq);
+	i2400mu->rx_size = PAGE_SIZE - sizeof(struct skb_shared_info);
+	atomic_set(&i2400mu->do_autopm, 1);
+	i2400mu->rx_size_auto_shrink = 1;
+}
+
+extern int i2400mu_notification_setup(struct i2400mu *);
+extern void i2400mu_notification_release(struct i2400mu *);
+
+extern int i2400mu_rx_setup(struct i2400mu *);
+extern void i2400mu_rx_release(struct i2400mu *);
+extern void i2400mu_rx_kick(struct i2400mu *);
+
+extern int i2400mu_tx_setup(struct i2400mu *);
+extern void i2400mu_tx_release(struct i2400mu *);
+extern void i2400mu_bus_tx_kick(struct i2400m *);
+
+extern ssize_t i2400mu_bus_bm_cmd_send(struct i2400m *,
+				       const struct i2400m_bootrom_header *,
+				       size_t, int);
+extern ssize_t i2400mu_bus_bm_wait_for_ack(struct i2400m *,
+					   struct i2400m_bootrom_header *,
+					   size_t);
+#endif /* #ifndef __I2400M_USB_H__ */
diff --git a/drivers/net/wimax/i2400m/i2400m.h b/drivers/net/wimax/i2400m/i2400m.h
new file mode 100644
index 000000000000..067c871cc226
--- /dev/null
+++ b/drivers/net/wimax/i2400m/i2400m.h
@@ -0,0 +1,755 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * Declarations for bus-generic internal APIs
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ * Yanir Lubetkin <yanirx.lubetkin@intel.com>
+ *  - Initial implementation
+ *
+ *
+ * GENERAL DRIVER ARCHITECTURE
+ *
+ * The i2400m driver is split in the following two major parts:
+ *
+ *  - bus specific driver
+ *  - bus generic driver (this part)
+ *
+ * The bus specific driver sets up stuff specific to the bus the
+ * device is connected to (USB, SDIO, PCI, tam-tam...non-authoritative
+ * nor binding list) which is basically the device-model management
+ * (probe/disconnect, etc), moving data from device to kernel and
+ * back, doing the power saving details and reseting the device.
+ *
+ * For details on each bus-specific driver, see it's include file,
+ * i2400m-BUSNAME.h
+ *
+ * The bus-generic functionality break up is:
+ *
+ *  - Firmware upload: fw.c - takes care of uploading firmware to the
+ *        device. bus-specific driver just needs to provides a way to
+ *        execute boot-mode commands and to reset the device.
+ *
+ *  - RX handling: rx.c - receives data from the bus-specific code and
+ *        feeds it to the network or WiMAX stack or uses it to modify
+ *        the driver state. bus-specific driver only has to receive
+ *        frames and pass them to this module.
+ *
+ *  - TX handling: tx.c - manages the TX FIFO queue and provides means
+ *        for the bus-specific TX code to pull data from the FIFO
+ *        queue. bus-specific code just pulls frames from this module
+ *        to sends them to the device.
+ *
+ *  - netdev glue: netdev.c - interface with Linux networking
+ *        stack. Pass around data frames, and configure when the
+ *        device is up and running or shutdown (through ifconfig up /
+ *        down). Bus-generic only.
+ *
+ *  - control ops: control.c - implements various commmands for
+ *        controlling the device. bus-generic only.
+ *
+ *  - device model glue: driver.c - implements helpers for the
+ *        device-model glue done by the bus-specific layer
+ *        (setup/release the driver resources), turning the device on
+ *        and off, handling the device reboots/resets and a few simple
+ *        WiMAX stack ops.
+ *
+ * Code is also broken up in linux-glue / device-glue.
+ *
+ * Linux glue contains functions that deal mostly with gluing with the
+ * rest of the Linux kernel.
+ *
+ * Device-glue are functions that deal mostly with the way the device
+ * does things and talk the device's language.
+ *
+ * device-glue code is licensed BSD so other open source OSes can take
+ * it to implement their drivers.
+ *
+ *
+ * APIs AND HEADER FILES
+ *
+ * This bus generic code exports three APIs:
+ *
+ *  - HDI (host-device interface) definitions common to all busses
+ *    (include/linux/wimax/i2400m.h); these can be also used by user
+ *    space code.
+ *  - internal API for the bus-generic code
+ *  - external API for the bus-specific drivers
+ *
+ *
+ * LIFE CYCLE:
+ *
+ * When the bus-specific driver probes, it allocates a network device
+ * with enough space for it's data structue, that must contain a
+ * &struct i2400m at the top.
+ *
+ * On probe, it needs to fill the i2400m members marked as [fill], as
+ * well as i2400m->wimax_dev.net_dev and call i2400m_setup(). The
+ * i2400m driver will only register with the WiMAX and network stacks;
+ * the only access done to the device is to read the MAC address so we
+ * can register a network device. This calls i2400m_dev_start() to
+ * load firmware, setup communication with the device and configure it
+ * for operation.
+ *
+ * At this point, control and data communications are possible.
+ *
+ * On disconnect/driver unload, the bus-specific disconnect function
+ * calls i2400m_release() to undo i2400m_setup(). i2400m_dev_stop()
+ * shuts the firmware down and releases resources uses to communicate
+ * with the device.
+ *
+ * While the device is up, it might reset. The bus-specific driver has
+ * to catch that situation and call i2400m_dev_reset_handle() to deal
+ * with it (reset the internal driver structures and go back to square
+ * one).
+ */
+
+#ifndef __I2400M_H__
+#define __I2400M_H__
+
+#include <linux/usb.h>
+#include <linux/netdevice.h>
+#include <linux/completion.h>
+#include <linux/rwsem.h>
+#include <asm/atomic.h>
+#include <net/wimax.h>
+#include <linux/wimax/i2400m.h>
+#include <asm/byteorder.h>
+
+/* Misc constants */
+enum {
+	/* Firmware uploading */
+	I2400M_BOOT_RETRIES = 3,
+	/* Size of the Boot Mode Command buffer */
+	I2400M_BM_CMD_BUF_SIZE = 16 * 1024,
+	I2400M_BM_ACK_BUF_SIZE = 256,
+};
+
+
+/* Firmware version we request when pulling the fw image file */
+#define I2400M_FW_VERSION "1.3"
+
+
+/**
+ * i2400m_reset_type - methods to reset a device
+ *
+ * @I2400M_RT_WARM: Reset without device disconnection, device handles
+ *     are kept valid but state is back to power on, with firmware
+ *     re-uploaded.
+ * @I2400M_RT_COLD: Tell the device to disconnect itself from the bus
+ *     and reconnect. Renders all device handles invalid.
+ * @I2400M_RT_BUS: Tells the bus to reset the device; last measure
+ *     used when both types above don't work.
+ */
+enum i2400m_reset_type {
+	I2400M_RT_WARM,	/* first measure */
+	I2400M_RT_COLD,	/* second measure */
+	I2400M_RT_BUS,	/* call in artillery */
+};
+
+struct i2400m_reset_ctx;
+
+/**
+ * struct i2400m - descriptor for an Intel 2400m
+ *
+ * Members marked with [fill] must be filled out/initialized before
+ * calling i2400m_setup().
+ *
+ * @bus_tx_block_size: [fill] SDIO imposes a 256 block size, USB 16,
+ *     so we have a tx_blk_size variable that the bus layer sets to
+ *     tell the engine how much of that we need.
+ *
+ * @bus_pl_size_max: [fill] Maximum payload size.
+ *
+ * @bus_dev_start: [fill] Function called by the bus-generic code
+ *     [i2400m_dev_start()] to setup the bus-specific communications
+ *     to the the device. See LIFE CYCLE above.
+ *
+ *     NOTE: Doesn't need to upload the firmware, as that is taken
+ *     care of by the bus-generic code.
+ *
+ * @bus_dev_stop: [fill] Function called by the bus-generic code
+ *     [i2400m_dev_stop()] to shutdown the bus-specific communications
+ *     to the the device. See LIFE CYCLE above.
+ *
+ *     This function does not need to reset the device, just tear down
+ *     all the host resources created to  handle communication with
+ *     the device.
+ *
+ * @bus_tx_kick: [fill] Function called by the bus-generic code to let
+ *     the bus-specific code know that there is data available in the
+ *     TX FIFO for transmission to the device.
+ *
+ *     This function cannot sleep.
+ *
+ * @bus_reset: [fill] Function called by the bus-generic code to reset
+ *     the device in in various ways. Doesn't need to wait for the
+ *     reset to finish.
+ *
+ *     If warm or cold reset fail, this function is expected to do a
+ *     bus-specific reset (eg: USB reset) to get the device to a
+ *     working state (even if it implies device disconecction).
+ *
+ *     Note the warm reset is used by the firmware uploader to
+ *     reinitialize the device.
+ *
+ *     IMPORTANT: this is called very early in the device setup
+ *     process, so it cannot rely on common infrastructure being laid
+ *     out.
+ *
+ * @bus_bm_cmd_send: [fill] Function called to send a boot-mode
+ *     command. Flags are defined in 'enum i2400m_bm_cmd_flags'. This
+ *     is synchronous and has to return 0 if ok or < 0 errno code in
+ *     any error condition.
+ *
+ * @bus_bm_wait_for_ack: [fill] Function called to wait for a
+ *     boot-mode notification (that can be a response to a previously
+ *     issued command or an asynchronous one). Will read until all the
+ *     indicated size is read or timeout. Reading more or less data
+ *     than asked for is an error condition. Return 0 if ok, < 0 errno
+ *     code on error.
+ *
+ *     The caller to this function will check if the response is a
+ *     barker that indicates the device going into reset mode.
+ *
+ * @bus_fw_name: [fill] name of the firmware image (in most cases,
+ *     they are all the same for a single release, except that they
+ *     have the type of the bus embedded in the name (eg:
+ *     i2400m-fw-X-VERSION.sbcf, where X is the bus name).
+ *
+ * @bus_bm_mac_addr_impaired: [fill] Set to true if the device's MAC
+ *     address provided in boot mode is kind of broken and needs to
+ *     be re-read later on.
+ *
+ *
+ * @wimax_dev: WiMAX generic device for linkage into the kernel WiMAX
+ *     stack. Due to the way a net_device is allocated, we need to
+ *     force this to be the first field so that we can get from
+ *     netdev_priv() the right pointer.
+ *
+ * @state: device's state (as reported by it)
+ *
+ * @state_wq: waitqueue that is woken up whenever the state changes
+ *
+ * @tx_lock: spinlock to protect TX members
+ *
+ * @tx_buf: FIFO buffer for TX; we queue data here
+ *
+ * @tx_in: FIFO index for incoming data. Note this doesn't wrap around
+ *     and it is always greater than @tx_out.
+ *
+ * @tx_out: FIFO index for outgoing data
+ *
+ * @tx_msg: current TX message that is active in the FIFO for
+ *     appending payloads.
+ *
+ * @tx_sequence: current sequence number for TX messages from the
+ *     device to the host.
+ *
+ * @tx_msg_size: size of the current message being transmitted by the
+ *     bus-specific code.
+ *
+ * @tx_pl_num: total number of payloads sent
+ *
+ * @tx_pl_max: maximum number of payloads sent in a TX message
+ *
+ * @tx_pl_min: minimum number of payloads sent in a TX message
+ *
+ * @tx_num: number of TX messages sent
+ *
+ * @tx_size_acc: number of bytes in all TX messages sent
+ *     (this is different to net_dev's statistics as it also counts
+ *     control messages).
+ *
+ * @tx_size_min: smallest TX message sent.
+ *
+ * @tx_size_max: biggest TX message sent.
+ *
+ * @rx_lock: spinlock to protect RX members
+ *
+ * @rx_pl_num: total number of payloads received
+ *
+ * @rx_pl_max: maximum number of payloads received in a RX message
+ *
+ * @rx_pl_min: minimum number of payloads received in a RX message
+ *
+ * @rx_num: number of RX messages received
+ *
+ * @rx_size_acc: number of bytes in all RX messages received
+ *     (this is different to net_dev's statistics as it also counts
+ *     control messages).
+ *
+ * @rx_size_min: smallest RX message received.
+ *
+ * @rx_size_max: buggest RX message received.
+ *
+ * @init_mutex: Mutex used for serializing the device bringup
+ *     sequence; this way if the device reboots in the middle, we
+ *     don't try to do a bringup again while we are tearing down the
+ *     one that failed.
+ *
+ *     Can't reuse @msg_mutex because from within the bringup sequence
+ *     we need to send messages to the device and thus use @msg_mutex.
+ *
+ * @msg_mutex: mutex used to send control commands to the device (we
+ *     only allow one at a time, per host-device interface design).
+ *
+ * @msg_completion: used to wait for an ack to a control command sent
+ *     to the device.
+ *
+ * @ack_skb: used to store the actual ack to a control command if the
+ *     reception of the command was successful. Otherwise, a ERR_PTR()
+ *     errno code that indicates what failed with the ack reception.
+ *
+ *     Only valid after @msg_completion is woken up. Only updateable
+ *     if @msg_completion is armed. Only touched by
+ *     i2400m_msg_to_dev().
+ *
+ *     Protected by @rx_lock. In theory the command execution flow is
+ *     sequential, but in case the device sends an out-of-phase or
+ *     very delayed response, we need to avoid it trampling current
+ *     execution.
+ *
+ * @bm_cmd_buf: boot mode command buffer for composing firmware upload
+ *     commands.
+ *
+ *     USB can't r/w to stack, vmalloc, etc...as well, we end up
+ *     having to alloc/free a lot to compose commands, so we use these
+ *     for stagging and not having to realloc all the time.
+ *
+ *     This assumes the code always runs serialized. Only one thread
+ *     can call i2400m_bm_cmd() at the same time.
+ *
+ * @bm_ack_buf: boot mode acknoledge buffer for staging reception of
+ *     responses to commands.
+ *
+ *     See @bm_cmd_buf.
+ *
+ * @work_queue: work queue for processing device reports. This
+ *     workqueue cannot be used for processing TX or RX to the device,
+ *     as from it we'll process device reports, which might require
+ *     further communication with the device.
+ *
+ * @debugfs_dentry: hookup for debugfs files.
+ *     These have to be in a separate directory, a child of
+ *     (wimax_dev->debugfs_dentry) so they can be removed when the
+ *     module unloads, as we don't keep each dentry.
+ */
+struct i2400m {
+	struct wimax_dev wimax_dev;	/* FIRST! See doc */
+
+	unsigned updown:1;		/* Network device is up or down */
+	unsigned boot_mode:1;		/* is the device in boot mode? */
+	unsigned sboot:1;		/* signed or unsigned fw boot */
+	unsigned ready:1;		/* all probing steps done */
+	u8 trace_msg_from_user;		/* echo rx msgs to 'trace' pipe */
+					/* typed u8 so debugfs/u8 can tweak */
+	enum i2400m_system_state state;
+	wait_queue_head_t state_wq;	/* Woken up when on state updates */
+
+	size_t bus_tx_block_size;
+	size_t bus_pl_size_max;
+	int (*bus_dev_start)(struct i2400m *);
+	void (*bus_dev_stop)(struct i2400m *);
+	void (*bus_tx_kick)(struct i2400m *);
+	int (*bus_reset)(struct i2400m *, enum i2400m_reset_type);
+	ssize_t (*bus_bm_cmd_send)(struct i2400m *,
+				   const struct i2400m_bootrom_header *,
+				   size_t, int flags);
+	ssize_t (*bus_bm_wait_for_ack)(struct i2400m *,
+				       struct i2400m_bootrom_header *, size_t);
+	const char *bus_fw_name;
+	unsigned bus_bm_mac_addr_impaired:1;
+
+	spinlock_t tx_lock;		/* protect TX state */
+	void *tx_buf;
+	size_t tx_in, tx_out;
+	struct i2400m_msg_hdr *tx_msg;
+	size_t tx_sequence, tx_msg_size;
+	/* TX stats */
+	unsigned tx_pl_num, tx_pl_max, tx_pl_min,
+		tx_num, tx_size_acc, tx_size_min, tx_size_max;
+
+	/* RX stats */
+	spinlock_t rx_lock;		/* protect RX state */
+	unsigned rx_pl_num, rx_pl_max, rx_pl_min,
+		rx_num, rx_size_acc, rx_size_min, rx_size_max;
+
+	struct mutex msg_mutex;		/* serialize command execution */
+	struct completion msg_completion;
+	struct sk_buff *ack_skb;	/* protected by rx_lock */
+
+	void *bm_ack_buf;		/* for receiving acks over USB */
+	void *bm_cmd_buf;		/* for issuing commands over USB */
+
+	struct workqueue_struct *work_queue;
+
+	struct mutex init_mutex;	/* protect bringup seq */
+	struct i2400m_reset_ctx *reset_ctx;	/* protected by init_mutex */
+
+	struct work_struct wake_tx_ws;
+	struct sk_buff *wake_tx_skb;
+
+	struct dentry *debugfs_dentry;
+};
+
+
+/*
+ * Initialize a 'struct i2400m' from all zeroes
+ *
+ * This is a bus-generic API call.
+ */
+static inline
+void i2400m_init(struct i2400m *i2400m)
+{
+	wimax_dev_init(&i2400m->wimax_dev);
+
+	i2400m->boot_mode = 1;
+	init_waitqueue_head(&i2400m->state_wq);
+
+	spin_lock_init(&i2400m->tx_lock);
+	i2400m->tx_pl_min = UINT_MAX;
+	i2400m->tx_size_min = UINT_MAX;
+
+	spin_lock_init(&i2400m->rx_lock);
+	i2400m->rx_pl_min = UINT_MAX;
+	i2400m->rx_size_min = UINT_MAX;
+
+	mutex_init(&i2400m->msg_mutex);
+	init_completion(&i2400m->msg_completion);
+
+	mutex_init(&i2400m->init_mutex);
+	/* wake_tx_ws is initialized in i2400m_tx_setup() */
+}
+
+
+/*
+ * Bus-generic internal APIs
+ * -------------------------
+ */
+
+static inline
+struct i2400m *wimax_dev_to_i2400m(struct wimax_dev *wimax_dev)
+{
+	return container_of(wimax_dev, struct i2400m, wimax_dev);
+}
+
+static inline
+struct i2400m *net_dev_to_i2400m(struct net_device *net_dev)
+{
+	return wimax_dev_to_i2400m(netdev_priv(net_dev));
+}
+
+/*
+ * Boot mode support
+ */
+
+/**
+ * i2400m_bm_cmd_flags - flags to i2400m_bm_cmd()
+ *
+ * @I2400M_BM_CMD_RAW: send the command block as-is, without doing any
+ *     extra processing for adding CRC.
+ */
+enum i2400m_bm_cmd_flags {
+	I2400M_BM_CMD_RAW	= 1 << 2,
+};
+
+/**
+ * i2400m_bri - Boot-ROM indicators
+ *
+ * Flags for i2400m_bootrom_init() and i2400m_dev_bootstrap() [which
+ * are passed from things like i2400m_setup()]. Can be combined with
+ * |.
+ *
+ * @I2400M_BRI_SOFT: The device rebooted already and a reboot
+ *     barker received, proceed directly to ack the boot sequence.
+ * @I2400M_BRI_NO_REBOOT: Do not reboot the device and proceed
+ *     directly to wait for a reboot barker from the device.
+ * @I2400M_BRI_MAC_REINIT: We need to reinitialize the boot
+ *     rom after reading the MAC adress. This is quite a dirty hack,
+ *     if you ask me -- the device requires the bootrom to be
+ *     intialized after reading the MAC address.
+ */
+enum i2400m_bri {
+	I2400M_BRI_SOFT       = 1 << 1,
+	I2400M_BRI_NO_REBOOT  = 1 << 2,
+	I2400M_BRI_MAC_REINIT = 1 << 3,
+};
+
+extern void i2400m_bm_cmd_prepare(struct i2400m_bootrom_header *);
+extern int i2400m_dev_bootstrap(struct i2400m *, enum i2400m_bri);
+extern int i2400m_read_mac_addr(struct i2400m *);
+extern int i2400m_bootrom_init(struct i2400m *, enum i2400m_bri);
+
+/* Make/grok boot-rom header commands */
+
+static inline
+__le32 i2400m_brh_command(enum i2400m_brh_opcode opcode, unsigned use_checksum,
+			  unsigned direct_access)
+{
+	return cpu_to_le32(
+		I2400M_BRH_SIGNATURE
+		| (direct_access ? I2400M_BRH_DIRECT_ACCESS : 0)
+		| I2400M_BRH_RESPONSE_REQUIRED /* response always required */
+		| (use_checksum ? I2400M_BRH_USE_CHECKSUM : 0)
+		| (opcode & I2400M_BRH_OPCODE_MASK));
+}
+
+static inline
+void i2400m_brh_set_opcode(struct i2400m_bootrom_header *hdr,
+			   enum i2400m_brh_opcode opcode)
+{
+	hdr->command = cpu_to_le32(
+		(le32_to_cpu(hdr->command) & ~I2400M_BRH_OPCODE_MASK)
+		| (opcode & I2400M_BRH_OPCODE_MASK));
+}
+
+static inline
+unsigned i2400m_brh_get_opcode(const struct i2400m_bootrom_header *hdr)
+{
+	return le32_to_cpu(hdr->command) & I2400M_BRH_OPCODE_MASK;
+}
+
+static inline
+unsigned i2400m_brh_get_response(const struct i2400m_bootrom_header *hdr)
+{
+	return (le32_to_cpu(hdr->command) & I2400M_BRH_RESPONSE_MASK)
+		>> I2400M_BRH_RESPONSE_SHIFT;
+}
+
+static inline
+unsigned i2400m_brh_get_use_checksum(const struct i2400m_bootrom_header *hdr)
+{
+	return le32_to_cpu(hdr->command) & I2400M_BRH_USE_CHECKSUM;
+}
+
+static inline
+unsigned i2400m_brh_get_response_required(
+	const struct i2400m_bootrom_header *hdr)
+{
+	return le32_to_cpu(hdr->command) & I2400M_BRH_RESPONSE_REQUIRED;
+}
+
+static inline
+unsigned i2400m_brh_get_direct_access(const struct i2400m_bootrom_header *hdr)
+{
+	return le32_to_cpu(hdr->command) & I2400M_BRH_DIRECT_ACCESS;
+}
+
+static inline
+unsigned i2400m_brh_get_signature(const struct i2400m_bootrom_header *hdr)
+{
+	return (le32_to_cpu(hdr->command) & I2400M_BRH_SIGNATURE_MASK)
+		>> I2400M_BRH_SIGNATURE_SHIFT;
+}
+
+
+/*
+ * Driver / device setup and internal functions
+ */
+extern void i2400m_netdev_setup(struct net_device *net_dev);
+extern int i2400m_tx_setup(struct i2400m *);
+extern void i2400m_wake_tx_work(struct work_struct *);
+extern void i2400m_tx_release(struct i2400m *);
+
+extern void i2400m_net_rx(struct i2400m *, struct sk_buff *, unsigned,
+			  const void *, int);
+enum i2400m_pt;
+extern int i2400m_tx(struct i2400m *, const void *, size_t, enum i2400m_pt);
+
+#ifdef CONFIG_DEBUG_FS
+extern int i2400m_debugfs_add(struct i2400m *);
+extern void i2400m_debugfs_rm(struct i2400m *);
+#else
+static inline int i2400m_debugfs_add(struct i2400m *i2400m)
+{
+	return 0;
+}
+static inline void i2400m_debugfs_rm(struct i2400m *i2400m) {}
+#endif
+
+/* Called by _dev_start()/_dev_stop() to initialize the device itself */
+extern int i2400m_dev_initialize(struct i2400m *);
+extern void i2400m_dev_shutdown(struct i2400m *);
+
+extern struct attribute_group i2400m_dev_attr_group;
+
+extern int i2400m_schedule_work(struct i2400m *,
+				void (*)(struct work_struct *), gfp_t);
+
+/* HDI message's payload description handling */
+
+static inline
+size_t i2400m_pld_size(const struct i2400m_pld *pld)
+{
+	return I2400M_PLD_SIZE_MASK & le32_to_cpu(pld->val);
+}
+
+static inline
+enum i2400m_pt i2400m_pld_type(const struct i2400m_pld *pld)
+{
+	return (I2400M_PLD_TYPE_MASK & le32_to_cpu(pld->val))
+		>> I2400M_PLD_TYPE_SHIFT;
+}
+
+static inline
+void i2400m_pld_set(struct i2400m_pld *pld, size_t size,
+		    enum i2400m_pt type)
+{
+	pld->val = cpu_to_le32(
+		((type << I2400M_PLD_TYPE_SHIFT) & I2400M_PLD_TYPE_MASK)
+		|  (size & I2400M_PLD_SIZE_MASK));
+}
+
+
+/*
+ * API for the bus-specific drivers
+ * --------------------------------
+ */
+
+static inline
+struct i2400m *i2400m_get(struct i2400m *i2400m)
+{
+	dev_hold(i2400m->wimax_dev.net_dev);
+	return i2400m;
+}
+
+static inline
+void i2400m_put(struct i2400m *i2400m)
+{
+	dev_put(i2400m->wimax_dev.net_dev);
+}
+
+extern int i2400m_dev_reset_handle(struct i2400m *);
+
+/*
+ * _setup()/_release() are called by the probe/disconnect functions of
+ * the bus-specific drivers.
+ */
+extern int i2400m_setup(struct i2400m *, enum i2400m_bri bm_flags);
+extern void i2400m_release(struct i2400m *);
+
+extern int i2400m_rx(struct i2400m *, struct sk_buff *);
+extern struct i2400m_msg_hdr *i2400m_tx_msg_get(struct i2400m *, size_t *);
+extern void i2400m_tx_msg_sent(struct i2400m *);
+
+static const __le32 i2400m_NBOOT_BARKER[4] = {
+	__constant_cpu_to_le32(I2400M_NBOOT_BARKER),
+	__constant_cpu_to_le32(I2400M_NBOOT_BARKER),
+	__constant_cpu_to_le32(I2400M_NBOOT_BARKER),
+	__constant_cpu_to_le32(I2400M_NBOOT_BARKER)
+};
+
+static const __le32 i2400m_SBOOT_BARKER[4] = {
+	__constant_cpu_to_le32(I2400M_SBOOT_BARKER),
+	__constant_cpu_to_le32(I2400M_SBOOT_BARKER),
+	__constant_cpu_to_le32(I2400M_SBOOT_BARKER),
+	__constant_cpu_to_le32(I2400M_SBOOT_BARKER)
+};
+
+
+/*
+ * Utility functions
+ */
+
+static inline
+struct device *i2400m_dev(struct i2400m *i2400m)
+{
+	return i2400m->wimax_dev.net_dev->dev.parent;
+}
+
+/*
+ * Helper for scheduling simple work functions
+ *
+ * This struct can get any kind of payload attached (normally in the
+ * form of a struct where you pack the stuff you want to pass to the
+ * _work function).
+ */
+struct i2400m_work {
+	struct work_struct ws;
+	struct i2400m *i2400m;
+	u8 pl[0];
+};
+extern int i2400m_queue_work(struct i2400m *,
+			     void (*)(struct work_struct *), gfp_t,
+				const void *, size_t);
+
+extern int i2400m_msg_check_status(const struct i2400m_l3l4_hdr *,
+				   char *, size_t);
+extern int i2400m_msg_size_check(struct i2400m *,
+				 const struct i2400m_l3l4_hdr *, size_t);
+extern struct sk_buff *i2400m_msg_to_dev(struct i2400m *, const void *, size_t);
+extern void i2400m_msg_to_dev_cancel_wait(struct i2400m *, int);
+extern void i2400m_msg_ack_hook(struct i2400m *,
+				const struct i2400m_l3l4_hdr *, size_t);
+extern void i2400m_report_hook(struct i2400m *,
+			       const struct i2400m_l3l4_hdr *, size_t);
+extern int i2400m_cmd_enter_powersave(struct i2400m *);
+extern int i2400m_cmd_get_state(struct i2400m *);
+extern int i2400m_cmd_exit_idle(struct i2400m *);
+extern struct sk_buff *i2400m_get_device_info(struct i2400m *);
+extern int i2400m_firmware_check(struct i2400m *);
+extern int i2400m_set_init_config(struct i2400m *,
+				  const struct i2400m_tlv_hdr **, size_t);
+
+static inline
+struct usb_endpoint_descriptor *usb_get_epd(struct usb_interface *iface, int ep)
+{
+	return &iface->cur_altsetting->endpoint[ep].desc;
+}
+
+extern int i2400m_op_rfkill_sw_toggle(struct wimax_dev *,
+				      enum wimax_rf_state);
+extern void i2400m_report_tlv_rf_switches_status(
+	struct i2400m *, const struct i2400m_tlv_rf_switches_status *);
+
+
+/*
+ * Do a millisecond-sleep for allowing wireshark to dump all the data
+ * packets. Used only for debugging.
+ */
+static inline
+void __i2400m_msleep(unsigned ms)
+{
+#if 1
+#else
+	msleep(ms);
+#endif
+}
+
+/* Module parameters */
+
+extern int i2400m_idle_mode_disabled;
+
+
+#endif /* #ifndef __I2400M_H__ */
diff --git a/drivers/net/wimax/i2400m/netdev.c b/drivers/net/wimax/i2400m/netdev.c
new file mode 100644
index 000000000000..63fe708e8a31
--- /dev/null
+++ b/drivers/net/wimax/i2400m/netdev.c
@@ -0,0 +1,524 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * Glue with the networking stack
+ *
+ *
+ * Copyright (C) 2007 Intel Corporation <linux-wimax@intel.com>
+ * Yanir Lubetkin <yanirx.lubetkin@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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.
+ *
+ *
+ * This implements an ethernet device for the i2400m.
+ *
+ * We fake being an ethernet device to simplify the support from user
+ * space and from the other side. The world is (sadly) configured to
+ * take in only Ethernet devices...
+ *
+ * Because of this, currently there is an copy-each-rxed-packet
+ * overhead on the RX path. Each IP packet has to be reallocated to
+ * add an ethernet header (as there is no space in what we get from
+ * the device). This is a known drawback and coming versions of the
+ * device's firmware are being changed to add header space that can be
+ * used to insert the ethernet header without having to reallocate and
+ * copy.
+ *
+ * TX error handling is tricky; because we have to FIFO/queue the
+ * buffers for transmission (as the hardware likes it aggregated), we
+ * just give the skb to the TX subsystem and by the time it is
+ * transmitted, we have long forgotten about it. So we just don't care
+ * too much about it.
+ *
+ * Note that when the device is in idle mode with the basestation, we
+ * need to negotiate coming back up online. That involves negotiation
+ * and possible user space interaction. Thus, we defer to a workqueue
+ * to do all that. By default, we only queue a single packet and drop
+ * the rest, as potentially the time to go back from idle to normal is
+ * long.
+ *
+ * ROADMAP
+ *
+ * i2400m_open         Called on ifconfig up
+ * i2400m_stop         Called on ifconfig down
+ *
+ * i2400m_hard_start_xmit Called by the network stack to send a packet
+ *   i2400m_net_wake_tx	  Wake up device from basestation-IDLE & TX
+ *     i2400m_wake_tx_work
+ *       i2400m_cmd_exit_idle
+ *       i2400m_tx
+ *   i2400m_net_tx        TX a data frame
+ *     i2400m_tx
+ *
+ * i2400m_change_mtu      Called on ifconfig mtu XXX
+ *
+ * i2400m_tx_timeout      Called when the device times out
+ *
+ * i2400m_net_rx          Called by the RX code when a data frame is
+ *                        available.
+ * i2400m_netdev_setup    Called to setup all the netdev stuff from
+ *                        alloc_netdev.
+ */
+#include <linux/if_arp.h>
+#include <linux/netdevice.h>
+#include "i2400m.h"
+
+
+#define D_SUBMODULE netdev
+#include "debug-levels.h"
+
+enum {
+/* netdev interface */
+	/*
+	 * Out of NWG spec (R1_v1.2.2), 3.3.3 ASN Bearer Plane MTU Size
+	 *
+	 * The MTU is 1400 or less
+	 */
+	I2400M_MAX_MTU = 1400,
+	I2400M_TX_TIMEOUT = HZ,
+	I2400M_TX_QLEN = 5,
+};
+
+
+static
+int i2400m_open(struct net_device *net_dev)
+{
+	int result;
+	struct i2400m *i2400m = net_dev_to_i2400m(net_dev);
+	struct device *dev = i2400m_dev(i2400m);
+
+	d_fnstart(3, dev, "(net_dev %p [i2400m %p])\n", net_dev, i2400m);
+	if (i2400m->ready == 0) {
+		dev_err(dev, "Device is still initializing\n");
+		result = -EBUSY;
+	} else
+		result = 0;
+	d_fnend(3, dev, "(net_dev %p [i2400m %p]) = %d\n",
+		net_dev, i2400m, result);
+	return result;
+}
+
+
+/*
+ *
+ * On kernel versions where cancel_work_sync() didn't return anything,
+ * we rely on wake_tx_skb() being non-NULL.
+ */
+static
+int i2400m_stop(struct net_device *net_dev)
+{
+	struct i2400m *i2400m = net_dev_to_i2400m(net_dev);
+	struct device *dev = i2400m_dev(i2400m);
+
+	d_fnstart(3, dev, "(net_dev %p [i2400m %p])\n", net_dev, i2400m);
+	/* See i2400m_hard_start_xmit(), references are taken there
+	 * and here we release them if the work was still
+	 * pending. Note we can't differentiate work not pending vs
+	 * never scheduled, so the NULL check does that. */
+	if (cancel_work_sync(&i2400m->wake_tx_ws) == 0
+	    && i2400m->wake_tx_skb != NULL) {
+		unsigned long flags;
+		struct sk_buff *wake_tx_skb;
+		spin_lock_irqsave(&i2400m->tx_lock, flags);
+		wake_tx_skb = i2400m->wake_tx_skb;	/* compat help */
+		i2400m->wake_tx_skb = NULL;	/* compat help */
+		spin_unlock_irqrestore(&i2400m->tx_lock, flags);
+		i2400m_put(i2400m);
+		kfree_skb(wake_tx_skb);
+	}
+	d_fnend(3, dev, "(net_dev %p [i2400m %p]) = 0\n", net_dev, i2400m);
+	return 0;
+}
+
+
+/*
+ * Wake up the device and transmit a held SKB, then restart the net queue
+ *
+ * When the device goes into basestation-idle mode, we need to tell it
+ * to exit that mode; it will negotiate with the base station, user
+ * space may have to intervene to rehandshake crypto and then tell us
+ * when it is ready to transmit the packet we have "queued". Still we
+ * need to give it sometime after it reports being ok.
+ *
+ * On error, there is not much we can do. If the error was on TX, we
+ * still wake the queue up to see if the next packet will be luckier.
+ *
+ * If _cmd_exit_idle() fails...well, it could be many things; most
+ * commonly it is that something else took the device out of IDLE mode
+ * (for example, the base station). In that case we get an -EILSEQ and
+ * we are just going to ignore that one. If the device is back to
+ * connected, then fine -- if it is someother state, the packet will
+ * be dropped anyway.
+ */
+void i2400m_wake_tx_work(struct work_struct *ws)
+{
+	int result;
+	struct i2400m *i2400m = container_of(ws, struct i2400m, wake_tx_ws);
+	struct device *dev = i2400m_dev(i2400m);
+	struct sk_buff *skb = i2400m->wake_tx_skb;
+	unsigned long flags;
+
+	spin_lock_irqsave(&i2400m->tx_lock, flags);
+	skb = i2400m->wake_tx_skb;
+	i2400m->wake_tx_skb = NULL;
+	spin_unlock_irqrestore(&i2400m->tx_lock, flags);
+
+	d_fnstart(3, dev, "(ws %p i2400m %p skb %p)\n", ws, i2400m, skb);
+	result = -EINVAL;
+	if (skb == NULL) {
+		dev_err(dev, "WAKE&TX: skb dissapeared!\n");
+		goto out_put;
+	}
+	result = i2400m_cmd_exit_idle(i2400m);
+	if (result == -EILSEQ)
+		result = 0;
+	if (result < 0) {
+		dev_err(dev, "WAKE&TX: device didn't get out of idle: "
+			"%d\n", result);
+			goto error;
+	}
+	result = wait_event_timeout(i2400m->state_wq,
+				    i2400m->state != I2400M_SS_IDLE, 5 * HZ);
+	if (result == 0)
+		result = -ETIMEDOUT;
+	if (result < 0) {
+		dev_err(dev, "WAKE&TX: error waiting for device to exit IDLE: "
+			"%d\n", result);
+		goto error;
+	}
+	msleep(20);	/* device still needs some time or it drops it */
+	result = i2400m_tx(i2400m, skb->data, skb->len, I2400M_PT_DATA);
+	netif_wake_queue(i2400m->wimax_dev.net_dev);
+error:
+	kfree_skb(skb);	/* refcount transferred by _hard_start_xmit() */
+out_put:
+	i2400m_put(i2400m);
+	d_fnend(3, dev, "(ws %p i2400m %p skb %p) = void [%d]\n",
+		ws, i2400m, skb, result);
+}
+
+
+/*
+ * Prepare the data payload TX header
+ *
+ * The i2400m expects a 4 byte header in front of a data packet.
+ *
+ * Because we pretend to be an ethernet device, this packet comes with
+ * an ethernet header. Pull it and push our header.
+ */
+static
+void i2400m_tx_prep_header(struct sk_buff *skb)
+{
+	struct i2400m_pl_data_hdr *pl_hdr;
+	skb_pull(skb, ETH_HLEN);
+	pl_hdr = (struct i2400m_pl_data_hdr *) skb_push(skb, sizeof(*pl_hdr));
+	pl_hdr->reserved = 0;
+}
+
+
+/*
+ * TX an skb to an idle device
+ *
+ * When the device is in basestation-idle mode, we need to wake it up
+ * and then TX. So we queue a work_struct for doing so.
+ *
+ * We need to get an extra ref for the skb (so it is not dropped), as
+ * well as be careful not to queue more than one request (won't help
+ * at all). If more than one request comes or there are errors, we
+ * just drop the packets (see i2400m_hard_start_xmit()).
+ */
+static
+int i2400m_net_wake_tx(struct i2400m *i2400m, struct net_device *net_dev,
+		       struct sk_buff *skb)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	unsigned long flags;
+
+	d_fnstart(3, dev, "(skb %p net_dev %p)\n", skb, net_dev);
+	if (net_ratelimit()) {
+		d_printf(3, dev, "WAKE&NETTX: "
+			 "skb %p sending %d bytes to radio\n",
+			 skb, skb->len);
+		d_dump(4, dev, skb->data, skb->len);
+	}
+	/* We hold a ref count for i2400m and skb, so when
+	 * stopping() the device, we need to cancel that work
+	 * and if pending, release those resources. */
+	result = 0;
+	spin_lock_irqsave(&i2400m->tx_lock, flags);
+	if (!work_pending(&i2400m->wake_tx_ws)) {
+		netif_stop_queue(net_dev);
+		i2400m_get(i2400m);
+		i2400m->wake_tx_skb = skb_get(skb);	/* transfer ref count */
+		i2400m_tx_prep_header(skb);
+		result = schedule_work(&i2400m->wake_tx_ws);
+		WARN_ON(result == 0);
+	}
+	spin_unlock_irqrestore(&i2400m->tx_lock, flags);
+	if (result == 0) {
+		/* Yes, this happens even if we stopped the
+		 * queue -- blame the queue disciplines that
+		 * queue without looking -- I guess there is a reason
+		 * for that. */
+		if (net_ratelimit())
+			d_printf(1, dev, "NETTX: device exiting idle, "
+				 "dropping skb %p, queue running %d\n",
+				 skb, netif_queue_stopped(net_dev));
+		result = -EBUSY;
+	}
+	d_fnend(3, dev, "(skb %p net_dev %p) = %d\n", skb, net_dev, result);
+	return result;
+}
+
+
+/*
+ * Transmit a packet to the base station on behalf of the network stack.
+ *
+ * Returns: 0 if ok, < 0 errno code on error.
+ *
+ * We need to pull the ethernet header and add the hardware header,
+ * which is currently set to all zeroes and reserved.
+ */
+static
+int i2400m_net_tx(struct i2400m *i2400m, struct net_device *net_dev,
+		  struct sk_buff *skb)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+
+	d_fnstart(3, dev, "(i2400m %p net_dev %p skb %p)\n",
+		  i2400m, net_dev, skb);
+	/* FIXME: check eth hdr, only IPv4 is routed by the device as of now */
+	net_dev->trans_start = jiffies;
+	i2400m_tx_prep_header(skb);
+	d_printf(3, dev, "NETTX: skb %p sending %d bytes to radio\n",
+		 skb, skb->len);
+	d_dump(4, dev, skb->data, skb->len);
+	result = i2400m_tx(i2400m, skb->data, skb->len, I2400M_PT_DATA);
+	d_fnend(3, dev, "(i2400m %p net_dev %p skb %p) = %d\n",
+		i2400m, net_dev, skb, result);
+	return result;
+}
+
+
+/*
+ * Transmit a packet to the base station on behalf of the network stack
+ *
+ *
+ * Returns: NETDEV_TX_OK (always, even in case of error)
+ *
+ * In case of error, we just drop it. Reasons:
+ *
+ *  - we add a hw header to each skb, and if the network stack
+ *    retries, we have no way to know if that skb has it or not.
+ *
+ *  - network protocols have their own drop-recovery mechanisms
+ *
+ *  - there is not much else we can do
+ *
+ * If the device is idle, we need to wake it up; that is an operation
+ * that will sleep. See i2400m_net_wake_tx() for details.
+ */
+static
+int i2400m_hard_start_xmit(struct sk_buff *skb,
+			   struct net_device *net_dev)
+{
+	int result;
+	struct i2400m *i2400m = net_dev_to_i2400m(net_dev);
+	struct device *dev = i2400m_dev(i2400m);
+
+	d_fnstart(3, dev, "(skb %p net_dev %p)\n", skb, net_dev);
+	if (i2400m->state == I2400M_SS_IDLE)
+		result = i2400m_net_wake_tx(i2400m, net_dev, skb);
+	else
+		result = i2400m_net_tx(i2400m, net_dev, skb);
+	if (result <  0)
+		net_dev->stats.tx_dropped++;
+	else {
+		net_dev->stats.tx_packets++;
+		net_dev->stats.tx_bytes += skb->len;
+	}
+	kfree_skb(skb);
+	result = NETDEV_TX_OK;
+	d_fnend(3, dev, "(skb %p net_dev %p) = %d\n", skb, net_dev, result);
+	return result;
+}
+
+
+static
+int i2400m_change_mtu(struct net_device *net_dev, int new_mtu)
+{
+	int result;
+	struct i2400m *i2400m = net_dev_to_i2400m(net_dev);
+	struct device *dev = i2400m_dev(i2400m);
+
+	if (new_mtu >= I2400M_MAX_MTU) {
+		dev_err(dev, "Cannot change MTU to %d (max is %d)\n",
+			new_mtu, I2400M_MAX_MTU);
+		result = -EINVAL;
+	} else {
+		net_dev->mtu = new_mtu;
+		result = 0;
+	}
+	return result;
+}
+
+
+static
+void i2400m_tx_timeout(struct net_device *net_dev)
+{
+	/*
+	 * We might want to kick the device
+	 *
+	 * There is not much we can do though, as the device requires
+	 * that we send the data aggregated. By the time we receive
+	 * this, there might be data pending to be sent or not...
+	 */
+	net_dev->stats.tx_errors++;
+	return;
+}
+
+
+/*
+ * Create a fake ethernet header
+ *
+ * For emulating an ethernet device, every received IP header has to
+ * be prefixed with an ethernet header.
+ *
+ * What we receive has (potentially) many IP packets concatenated with
+ * no ETH_HLEN bytes prefixed. Thus there is no space for an eth
+ * header.
+ *
+ * We would have to reallocate or do ugly fragment tricks in order to
+ * add it.
+ *
+ * But what we do is use the header space of the RX transaction
+ * (*msg_hdr) as we don't need it anymore; then we'll point all the
+ * data skbs there, as they share the same backing store.
+ *
+ * We only support IPv4 for v3 firmware.
+ */
+static
+void i2400m_rx_fake_eth_header(struct net_device *net_dev,
+			       void *_eth_hdr)
+{
+	struct ethhdr *eth_hdr = _eth_hdr;
+
+	memcpy(eth_hdr->h_dest, net_dev->dev_addr, sizeof(eth_hdr->h_dest));
+	memset(eth_hdr->h_source, 0, sizeof(eth_hdr->h_dest));
+	eth_hdr->h_proto = __constant_cpu_to_be16(ETH_P_IP);
+}
+
+
+/*
+ * i2400m_net_rx - pass a network packet to the stack
+ *
+ * @i2400m: device instance
+ * @skb_rx: the skb where the buffer pointed to by @buf is
+ * @i: 1 if payload is the only one
+ * @buf: pointer to the buffer containing the data
+ * @len: buffer's length
+ *
+ * We just clone the skb and set it up so that it's skb->data pointer
+ * points to "buf" and it's length.
+ *
+ * Note that if the payload is the last (or the only one) in a
+ * multi-payload message, we don't clone the SKB but just reuse it.
+ *
+ * This function is normally run from a thread context. However, we
+ * still use netif_rx() instead of netif_receive_skb() as was
+ * recommended in the mailing list. Reason is in some stress tests
+ * when sending/receiving a lot of data we seem to hit a softlock in
+ * the kernel's TCP implementation [aroudn tcp_delay_timer()]. Using
+ * netif_rx() took care of the issue.
+ *
+ * This is, of course, still open to do more research on why running
+ * with netif_receive_skb() hits this softlock. FIXME.
+ *
+ * FIXME: currently we don't do any efforts at distinguishing if what
+ * we got was an IPv4 or IPv6 header, to setup the protocol field
+ * correctly.
+ */
+void i2400m_net_rx(struct i2400m *i2400m, struct sk_buff *skb_rx,
+		   unsigned i, const void *buf, int buf_len)
+{
+	struct net_device *net_dev = i2400m->wimax_dev.net_dev;
+	struct device *dev = i2400m_dev(i2400m);
+	struct sk_buff *skb;
+
+	d_fnstart(2, dev, "(i2400m %p buf %p buf_len %d)\n",
+		  i2400m, buf, buf_len);
+	if (i) {
+		skb = skb_get(skb_rx);
+		d_printf(2, dev, "RX: reusing first payload skb %p\n", skb);
+		skb_pull(skb, buf - (void *) skb->data);
+		skb_trim(skb, (void *) skb_end_pointer(skb) - buf);
+	} else {
+		/* Yes, this is bad -- a lot of overhead -- see
+		 * comments at the top of the file */
+		skb = __netdev_alloc_skb(net_dev, buf_len, GFP_KERNEL);
+		if (skb == NULL) {
+			dev_err(dev, "NETRX: no memory to realloc skb\n");
+			net_dev->stats.rx_dropped++;
+			goto error_skb_realloc;
+		}
+		memcpy(skb_put(skb, buf_len), buf, buf_len);
+	}
+	i2400m_rx_fake_eth_header(i2400m->wimax_dev.net_dev,
+				  skb->data - ETH_HLEN);
+	skb_set_mac_header(skb, -ETH_HLEN);
+	skb->dev = i2400m->wimax_dev.net_dev;
+	skb->protocol = htons(ETH_P_IP);
+	net_dev->stats.rx_packets++;
+	net_dev->stats.rx_bytes += buf_len;
+	d_printf(3, dev, "NETRX: receiving %d bytes to network stack\n",
+		buf_len);
+	d_dump(4, dev, buf, buf_len);
+	netif_rx_ni(skb);	/* see notes in function header */
+error_skb_realloc:
+	d_fnend(2, dev, "(i2400m %p buf %p buf_len %d) = void\n",
+		i2400m, buf, buf_len);
+}
+
+
+/**
+ * i2400m_netdev_setup - Setup setup @net_dev's i2400m private data
+ *
+ * Called by alloc_netdev()
+ */
+void i2400m_netdev_setup(struct net_device *net_dev)
+{
+	d_fnstart(3, NULL, "(net_dev %p)\n", net_dev);
+	ether_setup(net_dev);
+	net_dev->mtu = I2400M_MAX_MTU;
+	net_dev->tx_queue_len = I2400M_TX_QLEN;
+	net_dev->features =
+		  NETIF_F_VLAN_CHALLENGED
+		| NETIF_F_HIGHDMA;
+	net_dev->flags =
+		IFF_NOARP		/* i2400m is apure IP device */
+		& (~IFF_BROADCAST	/* i2400m is P2P */
+		   & ~IFF_MULTICAST);
+	net_dev->watchdog_timeo = I2400M_TX_TIMEOUT;
+	net_dev->open = i2400m_open;
+	net_dev->stop = i2400m_stop;
+	net_dev->hard_start_xmit = i2400m_hard_start_xmit;
+	net_dev->change_mtu = i2400m_change_mtu;
+	net_dev->tx_timeout = i2400m_tx_timeout;
+	d_fnend(3, NULL, "(net_dev %p) = void\n", net_dev);
+}
+EXPORT_SYMBOL_GPL(i2400m_netdev_setup);
+
diff --git a/drivers/net/wimax/i2400m/op-rfkill.c b/drivers/net/wimax/i2400m/op-rfkill.c
new file mode 100644
index 000000000000..487ec58cea46
--- /dev/null
+++ b/drivers/net/wimax/i2400m/op-rfkill.c
@@ -0,0 +1,207 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * Implement backend for the WiMAX stack rfkill support
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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.
+ *
+ *
+ * The WiMAX kernel stack integrates into RF-Kill and keeps the
+ * switches's status. We just need to:
+ *
+ * - report changes in the HW RF Kill switch [with
+ *   wimax_rfkill_{sw,hw}_report(), which happens when we detect those
+ *   indications coming through hardware reports]. We also do it on
+ *   initialization to let the stack know the intial HW state.
+ *
+ * - implement indications from the stack to change the SW RF Kill
+ *   switch (coming from sysfs, the wimax stack or user space).
+ */
+#include "i2400m.h"
+#include <linux/wimax/i2400m.h>
+
+
+
+#define D_SUBMODULE rfkill
+#include "debug-levels.h"
+
+/*
+ * Return true if the i2400m radio is in the requested wimax_rf_state state
+ *
+ */
+static
+int i2400m_radio_is(struct i2400m *i2400m, enum wimax_rf_state state)
+{
+	if (state == WIMAX_RF_OFF)
+		return i2400m->state == I2400M_SS_RF_OFF
+			|| i2400m->state == I2400M_SS_RF_SHUTDOWN;
+	else if (state == WIMAX_RF_ON)
+		/* state == WIMAX_RF_ON */
+		return i2400m->state != I2400M_SS_RF_OFF
+			&& i2400m->state != I2400M_SS_RF_SHUTDOWN;
+	else
+		BUG();
+}
+
+
+/*
+ * WiMAX stack operation: implement SW RFKill toggling
+ *
+ * @wimax_dev: device descriptor
+ * @skb: skb where the message has been received; skb->data is
+ *       expected to point to the message payload.
+ * @genl_info: passed by the generic netlink layer
+ *
+ * Generic Netlink will call this function when a message is sent from
+ * userspace to change the software RF-Kill switch status.
+ *
+ * This function will set the device's sofware RF-Kill switch state to
+ * match what is requested.
+ *
+ * NOTE: the i2400m has a strict state machine; we can only set the
+ *       RF-Kill switch when it is on, the HW RF-Kill is on and the
+ *       device is initialized. So we ignore errors steaming from not
+ *       being in the right state (-EILSEQ).
+ */
+int i2400m_op_rfkill_sw_toggle(struct wimax_dev *wimax_dev,
+			       enum wimax_rf_state state)
+{
+	int result;
+	struct i2400m *i2400m = wimax_dev_to_i2400m(wimax_dev);
+	struct device *dev = i2400m_dev(i2400m);
+	struct sk_buff *ack_skb;
+	struct {
+		struct i2400m_l3l4_hdr hdr;
+		struct i2400m_tlv_rf_operation sw_rf;
+	} __attribute__((packed)) *cmd;
+	char strerr[32];
+
+	d_fnstart(4, dev, "(wimax_dev %p state %d)\n", wimax_dev, state);
+
+	result = -ENOMEM;
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (cmd == NULL)
+		goto error_alloc;
+	cmd->hdr.type = cpu_to_le16(I2400M_MT_CMD_RF_CONTROL);
+	cmd->hdr.length = sizeof(cmd->sw_rf);
+	cmd->hdr.version = cpu_to_le16(I2400M_L3L4_VERSION);
+	cmd->sw_rf.hdr.type = cpu_to_le16(I2400M_TLV_RF_OPERATION);
+	cmd->sw_rf.hdr.length = cpu_to_le16(sizeof(cmd->sw_rf.status));
+	switch (state) {
+	case WIMAX_RF_OFF:	/* RFKILL ON, radio OFF */
+		cmd->sw_rf.status = cpu_to_le32(2);
+		break;
+	case WIMAX_RF_ON:	/* RFKILL OFF, radio ON */
+		cmd->sw_rf.status = cpu_to_le32(1);
+		break;
+	default:
+		BUG();
+	}
+
+	ack_skb = i2400m_msg_to_dev(i2400m, cmd, sizeof(*cmd));
+	result = PTR_ERR(ack_skb);
+	if (IS_ERR(ack_skb)) {
+		dev_err(dev, "Failed to issue 'RF Control' command: %d\n",
+			result);
+		goto error_msg_to_dev;
+	}
+	result = i2400m_msg_check_status(wimax_msg_data(ack_skb),
+					 strerr, sizeof(strerr));
+	if (result < 0) {
+		dev_err(dev, "'RF Control' (0x%04x) command failed: %d - %s\n",
+			I2400M_MT_CMD_RF_CONTROL, result, strerr);
+		goto error_cmd;
+	}
+
+	/* Now we wait for the state to change to RADIO_OFF or RADIO_ON */
+	result = wait_event_timeout(
+		i2400m->state_wq, i2400m_radio_is(i2400m, state),
+		5 * HZ);
+	if (result == 0)
+		result = -ETIMEDOUT;
+	if (result < 0)
+		dev_err(dev, "Error waiting for device to toggle RF state: "
+			"%d\n", result);
+	result = 0;
+error_cmd:
+	kfree_skb(ack_skb);
+error_msg_to_dev:
+error_alloc:
+	d_fnend(4, dev, "(wimax_dev %p state %d) = %d\n",
+		wimax_dev, state, result);
+	return result;
+}
+
+
+/*
+ * Inform the WiMAX stack of changes in the RF Kill switches reported
+ * by the device
+ *
+ * @i2400m: device descriptor
+ * @rfss: TLV for RF Switches status; already validated
+ *
+ * NOTE: the reports on RF switch status cannot be trusted
+ *       or used until the device is in a state of RADIO_OFF
+ *       or greater.
+ */
+void i2400m_report_tlv_rf_switches_status(
+	struct i2400m *i2400m,
+	const struct i2400m_tlv_rf_switches_status *rfss)
+{
+	struct device *dev = i2400m_dev(i2400m);
+	enum i2400m_rf_switch_status hw, sw;
+	enum wimax_st wimax_state;
+
+	sw = le32_to_cpu(rfss->sw_rf_switch);
+	hw = le32_to_cpu(rfss->hw_rf_switch);
+
+	d_fnstart(3, dev, "(i2400m %p rfss %p [hw %u sw %u])\n",
+		  i2400m, rfss, hw, sw);
+	/* We only process rw switch evens when the device has been
+	 * fully initialized */
+	wimax_state = wimax_state_get(&i2400m->wimax_dev);
+	if (wimax_state < WIMAX_ST_RADIO_OFF) {
+		d_printf(3, dev, "ignoring RF switches report, state %u\n",
+			 wimax_state);
+		goto out;
+	}
+	switch (sw) {
+	case I2400M_RF_SWITCH_ON:	/* RF Kill disabled (radio on) */
+		wimax_report_rfkill_sw(&i2400m->wimax_dev, WIMAX_RF_ON);
+		break;
+	case I2400M_RF_SWITCH_OFF:	/* RF Kill enabled (radio off) */
+		wimax_report_rfkill_sw(&i2400m->wimax_dev, WIMAX_RF_OFF);
+		break;
+	default:
+		dev_err(dev, "HW BUG? Unknown RF SW state 0x%x\n", sw);
+	}
+
+	switch (hw) {
+	case I2400M_RF_SWITCH_ON:	/* RF Kill disabled (radio on) */
+		wimax_report_rfkill_hw(&i2400m->wimax_dev, WIMAX_RF_ON);
+		break;
+	case I2400M_RF_SWITCH_OFF:	/* RF Kill enabled (radio off) */
+		wimax_report_rfkill_hw(&i2400m->wimax_dev, WIMAX_RF_OFF);
+		break;
+	default:
+		dev_err(dev, "HW BUG? Unknown RF HW state 0x%x\n", hw);
+	}
+out:
+	d_fnend(3, dev, "(i2400m %p rfss %p [hw %u sw %u]) = void\n",
+		i2400m, rfss, hw, sw);
+}
diff --git a/drivers/net/wimax/i2400m/rx.c b/drivers/net/wimax/i2400m/rx.c
new file mode 100644
index 000000000000..6922022710ac
--- /dev/null
+++ b/drivers/net/wimax/i2400m/rx.c
@@ -0,0 +1,534 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * Handle incoming traffic and deliver it to the control or data planes
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Yanir Lubetkin <yanirx.lubetkin@intel.com>
+ *  - Initial implementation
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ *  - Use skb_clone(), break up processing in chunks
+ *  - Split transport/device specific
+ *  - Make buffer size dynamic to exert less memory pressure
+ *
+ *
+ * This handles the RX path.
+ *
+ * We receive an RX message from the bus-specific driver, which
+ * contains one or more payloads that have potentially different
+ * destinataries (data or control paths).
+ *
+ * So we just take that payload from the transport specific code in
+ * the form of an skb, break it up in chunks (a cloned skb each in the
+ * case of network packets) and pass it to netdev or to the
+ * command/ack handler (and from there to the WiMAX stack).
+ *
+ * PROTOCOL FORMAT
+ *
+ * The format of the buffer is:
+ *
+ * HEADER                      (struct i2400m_msg_hdr)
+ * PAYLOAD DESCRIPTOR 0        (struct i2400m_pld)
+ * PAYLOAD DESCRIPTOR 1
+ * ...
+ * PAYLOAD DESCRIPTOR N
+ * PAYLOAD 0                   (raw bytes)
+ * PAYLOAD 1
+ * ...
+ * PAYLOAD N
+ *
+ * See tx.c for a deeper description on alignment requirements and
+ * other fun facts of it.
+ *
+ * ROADMAP
+ *
+ * i2400m_rx
+ *   i2400m_rx_msg_hdr_check
+ *   i2400m_rx_pl_descr_check
+ *   i2400m_rx_payload
+ *     i2400m_net_rx
+ *     i2400m_rx_ctl
+ *       i2400m_msg_size_check
+ *       i2400m_report_hook_work    [in a workqueue]
+ *         i2400m_report_hook
+ *       wimax_msg_to_user
+ *       i2400m_rx_ctl_ack
+ *         wimax_msg_to_user_alloc
+ *     i2400m_rx_trace
+ *       i2400m_msg_size_check
+ *       wimax_msg
+ */
+#include <linux/kernel.h>
+#include <linux/if_arp.h>
+#include <linux/netdevice.h>
+#include <linux/workqueue.h>
+#include "i2400m.h"
+
+
+#define D_SUBMODULE rx
+#include "debug-levels.h"
+
+struct i2400m_report_hook_args {
+	struct sk_buff *skb_rx;
+	const struct i2400m_l3l4_hdr *l3l4_hdr;
+	size_t size;
+};
+
+
+/*
+ * Execute i2400m_report_hook in a workqueue
+ *
+ * Unpacks arguments from the deferred call, executes it and then
+ * drops the references.
+ *
+ * Obvious NOTE: References are needed because we are a separate
+ *     thread; otherwise the buffer changes under us because it is
+ *     released by the original caller.
+ */
+static
+void i2400m_report_hook_work(struct work_struct *ws)
+{
+	struct i2400m_work *iw =
+		container_of(ws, struct i2400m_work, ws);
+	struct i2400m_report_hook_args *args = (void *) iw->pl;
+	i2400m_report_hook(iw->i2400m, args->l3l4_hdr, args->size);
+	kfree_skb(args->skb_rx);
+	i2400m_put(iw->i2400m);
+	kfree(iw);
+}
+
+
+/*
+ * Process an ack to a command
+ *
+ * @i2400m: device descriptor
+ * @payload: pointer to message
+ * @size: size of the message
+ *
+ * Pass the acknodledgment (in an skb) to the thread that is waiting
+ * for it in i2400m->msg_completion.
+ *
+ * We need to coordinate properly with the thread waiting for the
+ * ack. Check if it is waiting or if it is gone. We loose the spinlock
+ * to avoid allocating on atomic contexts (yeah, could use GFP_ATOMIC,
+ * but this is not so speed critical).
+ */
+static
+void i2400m_rx_ctl_ack(struct i2400m *i2400m,
+		       const void *payload, size_t size)
+{
+	struct device *dev = i2400m_dev(i2400m);
+	struct wimax_dev *wimax_dev = &i2400m->wimax_dev;
+	unsigned long flags;
+	struct sk_buff *ack_skb;
+
+	/* Anyone waiting for an answer? */
+	spin_lock_irqsave(&i2400m->rx_lock, flags);
+	if (i2400m->ack_skb != ERR_PTR(-EINPROGRESS)) {
+		dev_err(dev, "Huh? reply to command with no waiters\n");
+		goto error_no_waiter;
+	}
+	spin_unlock_irqrestore(&i2400m->rx_lock, flags);
+
+	ack_skb = wimax_msg_alloc(wimax_dev, NULL, payload, size, GFP_KERNEL);
+
+	/* Check waiter didn't time out waiting for the answer... */
+	spin_lock_irqsave(&i2400m->rx_lock, flags);
+	if (i2400m->ack_skb != ERR_PTR(-EINPROGRESS)) {
+		d_printf(1, dev, "Huh? waiter for command reply cancelled\n");
+		goto error_waiter_cancelled;
+	}
+	if (ack_skb == NULL) {
+		dev_err(dev, "CMD/GET/SET ack: cannot allocate SKB\n");
+		i2400m->ack_skb = ERR_PTR(-ENOMEM);
+	} else
+		i2400m->ack_skb = ack_skb;
+	spin_unlock_irqrestore(&i2400m->rx_lock, flags);
+	complete(&i2400m->msg_completion);
+	return;
+
+error_waiter_cancelled:
+	if (ack_skb)
+		kfree_skb(ack_skb);
+error_no_waiter:
+	spin_unlock_irqrestore(&i2400m->rx_lock, flags);
+	return;
+}
+
+
+/*
+ * Receive and process a control payload
+ *
+ * @i2400m: device descriptor
+ * @skb_rx: skb that contains the payload (for reference counting)
+ * @payload: pointer to message
+ * @size: size of the message
+ *
+ * There are two types of control RX messages: reports (asynchronous,
+ * like your every day interrupts) and 'acks' (reponses to a command,
+ * get or set request).
+ *
+ * If it is a report, we run hooks on it (to extract information for
+ * things we need to do in the driver) and then pass it over to the
+ * WiMAX stack to send it to user space.
+ *
+ * NOTE: report processing is done in a workqueue specific to the
+ *     generic driver, to avoid deadlocks in the system.
+ *
+ * If it is not a report, it is an ack to a previously executed
+ * command, set or get, so wake up whoever is waiting for it from
+ * i2400m_msg_to_dev(). i2400m_rx_ctl_ack() takes care of that.
+ *
+ * Note that the sizes we pass to other functions from here are the
+ * sizes of the _l3l4_hdr + payload, not full buffer sizes, as we have
+ * verified in _msg_size_check() that they are congruent.
+ *
+ * For reports: We can't clone the original skb where the data is
+ * because we need to send this up via netlink; netlink has to add
+ * headers and we can't overwrite what's preceeding the payload...as
+ * it is another message. So we just dup them.
+ */
+static
+void i2400m_rx_ctl(struct i2400m *i2400m, struct sk_buff *skb_rx,
+		   const void *payload, size_t size)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	const struct i2400m_l3l4_hdr *l3l4_hdr = payload;
+	unsigned msg_type;
+
+	result = i2400m_msg_size_check(i2400m, l3l4_hdr, size);
+	if (result < 0) {
+		dev_err(dev, "HW BUG? device sent a bad message: %d\n",
+			result);
+		goto error_check;
+	}
+	msg_type = le16_to_cpu(l3l4_hdr->type);
+	d_printf(1, dev, "%s 0x%04x: %zu bytes\n",
+		 msg_type & I2400M_MT_REPORT_MASK ? "REPORT" : "CMD/SET/GET",
+		 msg_type, size);
+	d_dump(2, dev, l3l4_hdr, size);
+	if (msg_type & I2400M_MT_REPORT_MASK) {
+		/* These hooks have to be ran serialized; as well, the
+		 * handling might force the execution of commands, and
+		 * that might cause reentrancy issues with
+		 * bus-specific subdrivers and workqueues. So we run
+		 * it in a separate workqueue. */
+		struct i2400m_report_hook_args args = {
+			.skb_rx = skb_rx,
+			.l3l4_hdr = l3l4_hdr,
+			.size = size
+		};
+		if (unlikely(i2400m->ready == 0))	/* only send if up */
+			return;
+		skb_get(skb_rx);
+		i2400m_queue_work(i2400m, i2400m_report_hook_work,
+				  GFP_KERNEL, &args, sizeof(args));
+		result = wimax_msg(&i2400m->wimax_dev, NULL, l3l4_hdr, size,
+				   GFP_KERNEL);
+		if (result < 0)
+			dev_err(dev, "error sending report to userspace: %d\n",
+				result);
+	} else		/* an ack to a CMD, GET or SET */
+		i2400m_rx_ctl_ack(i2400m, payload, size);
+error_check:
+	return;
+}
+
+
+
+
+/*
+ * Receive and send up a trace
+ *
+ * @i2400m: device descriptor
+ * @skb_rx: skb that contains the trace (for reference counting)
+ * @payload: pointer to trace message inside the skb
+ * @size: size of the message
+ *
+ * THe i2400m might produce trace information (diagnostics) and we
+ * send them through a different kernel-to-user pipe (to avoid
+ * clogging it).
+ *
+ * As in i2400m_rx_ctl(), we can't clone the original skb where the
+ * data is because we need to send this up via netlink; netlink has to
+ * add headers and we can't overwrite what's preceeding the
+ * payload...as it is another message. So we just dup them.
+ */
+static
+void i2400m_rx_trace(struct i2400m *i2400m,
+		     const void *payload, size_t size)
+{
+	int result;
+	struct device *dev = i2400m_dev(i2400m);
+	struct wimax_dev *wimax_dev = &i2400m->wimax_dev;
+	const struct i2400m_l3l4_hdr *l3l4_hdr = payload;
+	unsigned msg_type;
+
+	result = i2400m_msg_size_check(i2400m, l3l4_hdr, size);
+	if (result < 0) {
+		dev_err(dev, "HW BUG? device sent a bad trace message: %d\n",
+			result);
+		goto error_check;
+	}
+	msg_type = le16_to_cpu(l3l4_hdr->type);
+	d_printf(1, dev, "Trace %s 0x%04x: %zu bytes\n",
+		 msg_type & I2400M_MT_REPORT_MASK ? "REPORT" : "CMD/SET/GET",
+		 msg_type, size);
+	d_dump(2, dev, l3l4_hdr, size);
+	if (unlikely(i2400m->ready == 0))	/* only send if up */
+		return;
+	result = wimax_msg(wimax_dev, "trace", l3l4_hdr, size, GFP_KERNEL);
+	if (result < 0)
+		dev_err(dev, "error sending trace to userspace: %d\n",
+			result);
+error_check:
+	return;
+}
+
+
+/*
+ * Act on a received payload
+ *
+ * @i2400m: device instance
+ * @skb_rx: skb where the transaction was received
+ * @single: 1 if there is only one payload, 0 otherwise
+ * @pld: payload descriptor
+ * @payload: payload data
+ *
+ * Upon reception of a payload, look at its guts in the payload
+ * descriptor and decide what to do with it.
+ */
+static
+void i2400m_rx_payload(struct i2400m *i2400m, struct sk_buff *skb_rx,
+		       unsigned single, const struct i2400m_pld *pld,
+		       const void *payload)
+{
+	struct device *dev = i2400m_dev(i2400m);
+	size_t pl_size = i2400m_pld_size(pld);
+	enum i2400m_pt pl_type = i2400m_pld_type(pld);
+
+	switch (pl_type) {
+	case I2400M_PT_DATA:
+		d_printf(3, dev, "RX: data payload %zu bytes\n", pl_size);
+		i2400m_net_rx(i2400m, skb_rx, single, payload, pl_size);
+		break;
+	case I2400M_PT_CTRL:
+		i2400m_rx_ctl(i2400m, skb_rx, payload, pl_size);
+		break;
+	case I2400M_PT_TRACE:
+		i2400m_rx_trace(i2400m, payload, pl_size);
+		break;
+	default:	/* Anything else shouldn't come to the host */
+		if (printk_ratelimit())
+			dev_err(dev, "RX: HW BUG? unexpected payload type %u\n",
+				pl_type);
+	}
+}
+
+
+/*
+ * Check a received transaction's message header
+ *
+ * @i2400m: device descriptor
+ * @msg_hdr: message header
+ * @buf_size: size of the received buffer
+ *
+ * Check that the declarations done by a RX buffer message header are
+ * sane and consistent with the amount of data that was received.
+ */
+static
+int i2400m_rx_msg_hdr_check(struct i2400m *i2400m,
+			    const struct i2400m_msg_hdr *msg_hdr,
+			    size_t buf_size)
+{
+	int result = -EIO;
+	struct device *dev = i2400m_dev(i2400m);
+	if (buf_size < sizeof(*msg_hdr)) {
+		dev_err(dev, "RX: HW BUG? message with short header (%zu "
+			"vs %zu bytes expected)\n", buf_size, sizeof(*msg_hdr));
+		goto error;
+	}
+	if (msg_hdr->barker != cpu_to_le32(I2400M_D2H_MSG_BARKER)) {
+		dev_err(dev, "RX: HW BUG? message received with unknown "
+			"barker 0x%08x (buf_size %zu bytes)\n",
+			le32_to_cpu(msg_hdr->barker), buf_size);
+		goto error;
+	}
+	if (msg_hdr->num_pls == 0) {
+		dev_err(dev, "RX: HW BUG? zero payload packets in message\n");
+		goto error;
+	}
+	if (le16_to_cpu(msg_hdr->num_pls) > I2400M_MAX_PLS_IN_MSG) {
+		dev_err(dev, "RX: HW BUG? message contains more payload "
+			"than maximum; ignoring.\n");
+		goto error;
+	}
+	result = 0;
+error:
+	return result;
+}
+
+
+/*
+ * Check a payload descriptor against the received data
+ *
+ * @i2400m: device descriptor
+ * @pld: payload descriptor
+ * @pl_itr: offset (in bytes) in the received buffer the payload is
+ *          located
+ * @buf_size: size of the received buffer
+ *
+ * Given a payload descriptor (part of a RX buffer), check it is sane
+ * and that the data it declares fits in the buffer.
+ */
+static
+int i2400m_rx_pl_descr_check(struct i2400m *i2400m,
+			      const struct i2400m_pld *pld,
+			      size_t pl_itr, size_t buf_size)
+{
+	int result = -EIO;
+	struct device *dev = i2400m_dev(i2400m);
+	size_t pl_size = i2400m_pld_size(pld);
+	enum i2400m_pt pl_type = i2400m_pld_type(pld);
+
+	if (pl_size > i2400m->bus_pl_size_max) {
+		dev_err(dev, "RX: HW BUG? payload @%zu: size %zu is "
+			"bigger than maximum %zu; ignoring message\n",
+			pl_itr, pl_size, i2400m->bus_pl_size_max);
+		goto error;
+	}
+	if (pl_itr + pl_size > buf_size) {	/* enough? */
+		dev_err(dev, "RX: HW BUG? payload @%zu: size %zu "
+			"goes beyond the received buffer "
+			"size (%zu bytes); ignoring message\n",
+			pl_itr, pl_size, buf_size);
+		goto error;
+	}
+	if (pl_type >= I2400M_PT_ILLEGAL) {
+		dev_err(dev, "RX: HW BUG? illegal payload type %u; "
+			"ignoring message\n", pl_type);
+		goto error;
+	}
+	result = 0;
+error:
+	return result;
+}
+
+
+/**
+ * i2400m_rx - Receive a buffer of data from the device
+ *
+ * @i2400m: device descriptor
+ * @skb: skbuff where the data has been received
+ *
+ * Parse in a buffer of data that contains an RX message sent from the
+ * device. See the file header for the format. Run all checks on the
+ * buffer header, then run over each payload's descriptors, verify
+ * their consistency and act on each payload's contents.  If
+ * everything is succesful, update the device's statistics.
+ *
+ * Note: You need to set the skb to contain only the length of the
+ * received buffer; for that, use skb_trim(skb, RECEIVED_SIZE).
+ *
+ * Returns:
+ *
+ * 0 if ok, < 0 errno on error
+ *
+ * If ok, this function owns now the skb and the caller DOESN'T have
+ * to run kfree_skb() on it. However, on error, the caller still owns
+ * the skb and it is responsible for releasing it.
+ */
+int i2400m_rx(struct i2400m *i2400m, struct sk_buff *skb)
+{
+	int i, result;
+	struct device *dev = i2400m_dev(i2400m);
+	const struct i2400m_msg_hdr *msg_hdr;
+	size_t pl_itr, pl_size, skb_len;
+	unsigned long flags;
+	unsigned num_pls;
+
+	skb_len = skb->len;
+	d_fnstart(4, dev, "(i2400m %p skb %p [size %zu])\n",
+		  i2400m, skb, skb_len);
+	result = -EIO;
+	msg_hdr = (void *) skb->data;
+	result = i2400m_rx_msg_hdr_check(i2400m, msg_hdr, skb->len);
+	if (result < 0)
+		goto error_msg_hdr_check;
+	result = -EIO;
+	num_pls = le16_to_cpu(msg_hdr->num_pls);
+	pl_itr = sizeof(*msg_hdr) +	/* Check payload descriptor(s) */
+		num_pls * sizeof(msg_hdr->pld[0]);
+	pl_itr = ALIGN(pl_itr, I2400M_PL_PAD);
+	if (pl_itr > skb->len) {	/* got all the payload descriptors? */
+		dev_err(dev, "RX: HW BUG? message too short (%u bytes) for "
+			"%u payload descriptors (%zu each, total %zu)\n",
+			skb->len, num_pls, sizeof(msg_hdr->pld[0]), pl_itr);
+		goto error_pl_descr_short;
+	}
+	/* Walk each payload payload--check we really got it */
+	for (i = 0; i < num_pls; i++) {
+		/* work around old gcc warnings */
+		pl_size = i2400m_pld_size(&msg_hdr->pld[i]);
+		result = i2400m_rx_pl_descr_check(i2400m, &msg_hdr->pld[i],
+						  pl_itr, skb->len);
+		if (result < 0)
+			goto error_pl_descr_check;
+		i2400m_rx_payload(i2400m, skb, num_pls == 1, &msg_hdr->pld[i],
+				  skb->data + pl_itr);
+		pl_itr += ALIGN(pl_size, I2400M_PL_PAD);
+		cond_resched();		/* Don't monopolize */
+	}
+	kfree_skb(skb);
+	/* Update device statistics */
+	spin_lock_irqsave(&i2400m->rx_lock, flags);
+	i2400m->rx_pl_num += i;
+	if (i > i2400m->rx_pl_max)
+		i2400m->rx_pl_max = i;
+	if (i < i2400m->rx_pl_min)
+		i2400m->rx_pl_min = i;
+	i2400m->rx_num++;
+	i2400m->rx_size_acc += skb->len;
+	if (skb->len < i2400m->rx_size_min)
+		i2400m->rx_size_min = skb->len;
+	if (skb->len > i2400m->rx_size_max)
+		i2400m->rx_size_max = skb->len;
+	spin_unlock_irqrestore(&i2400m->rx_lock, flags);
+error_pl_descr_check:
+error_pl_descr_short:
+error_msg_hdr_check:
+	d_fnend(4, dev, "(i2400m %p skb %p [size %zu]) = %d\n",
+		i2400m, skb, skb_len, result);
+	return result;
+}
+EXPORT_SYMBOL_GPL(i2400m_rx);
diff --git a/drivers/net/wimax/i2400m/sdio-debug-levels.h b/drivers/net/wimax/i2400m/sdio-debug-levels.h
new file mode 100644
index 000000000000..c51998741301
--- /dev/null
+++ b/drivers/net/wimax/i2400m/sdio-debug-levels.h
@@ -0,0 +1,22 @@
+/*
+ * debug levels control file for the i2400m module's
+ */
+#ifndef __debug_levels__h__
+#define __debug_levels__h__
+
+/* Maximum compile and run time debug level for all submodules */
+#define D_MODULENAME i2400m_sdio
+#define D_MASTER CONFIG_WIMAX_I2400M_DEBUG_LEVEL
+
+#include <linux/wimax/debug.h>
+
+/* List of all the enabled modules */
+enum d_module {
+	D_SUBMODULE_DECLARE(main),
+	D_SUBMODULE_DECLARE(tx),
+	D_SUBMODULE_DECLARE(rx),
+	D_SUBMODULE_DECLARE(fw)
+};
+
+
+#endif /* #ifndef __debug_levels__h__ */
diff --git a/drivers/net/wimax/i2400m/sdio-fw.c b/drivers/net/wimax/i2400m/sdio-fw.c
new file mode 100644
index 000000000000..3487205d8f50
--- /dev/null
+++ b/drivers/net/wimax/i2400m/sdio-fw.c
@@ -0,0 +1,224 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * Firmware uploader's SDIO specifics
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Yanir Lubetkin <yanirx.lubetkin@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ *  - Initial implementation
+ *
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ *  - Bus generic/specific split for USB
+ *
+ * Dirk Brandewie <dirk.j.brandewie@intel.com>
+ *  - Initial implementation for SDIO
+ *
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ *  - SDIO rehash for changes in the bus-driver model
+ *
+ * THE PROCEDURE
+ *
+ * See fw.c for the generic description of this procedure.
+ *
+ * This file implements only the SDIO specifics. It boils down to how
+ * to send a command and waiting for an acknowledgement from the
+ * device. We do polled reads.
+ *
+ * COMMAND EXECUTION
+ *
+ * THe generic firmware upload code will call i2400m_bus_bm_cmd_send()
+ * to send commands.
+ *
+ * The SDIO devices expects things in 256 byte blocks, so it will pad
+ * it, compute the checksum (if needed) and pass it to SDIO.
+ *
+ * ACK RECEPTION
+ *
+ * This works in polling mode -- the fw loader says when to wait for
+ * data and for that it calls i2400ms_bus_bm_wait_for_ack().
+ *
+ * This will poll the device for data until it is received. We need to
+ * receive at least as much bytes as where asked for (although it'll
+ * always be a multiple of 256 bytes).
+ */
+#include <linux/mmc/sdio_func.h>
+#include "i2400m-sdio.h"
+
+
+#define D_SUBMODULE fw
+#include "sdio-debug-levels.h"
+
+/*
+ * Send a boot-mode command to the SDIO function
+ *
+ * We use a bounce buffer (i2400m->bm_cmd_buf) because we need to
+ * touch the header if the RAW flag is not set.
+ *
+ * @flags: pass thru from i2400m_bm_cmd()
+ * @return: cmd_size if ok, < 0 errno code on error.
+ *
+ * Note the command is padded to the SDIO block size for the device.
+ */
+ssize_t i2400ms_bus_bm_cmd_send(struct i2400m *i2400m,
+				const struct i2400m_bootrom_header *_cmd,
+				size_t cmd_size, int flags)
+{
+	ssize_t result;
+	struct device *dev = i2400m_dev(i2400m);
+	struct i2400ms *i2400ms = container_of(i2400m, struct i2400ms, i2400m);
+	int opcode = _cmd == NULL ? -1 : i2400m_brh_get_opcode(_cmd);
+	struct i2400m_bootrom_header *cmd;
+	/* SDIO restriction */
+	size_t cmd_size_a = ALIGN(cmd_size, I2400MS_BLK_SIZE);
+
+	d_fnstart(5, dev, "(i2400m %p cmd %p size %zu)\n",
+		  i2400m, _cmd, cmd_size);
+	result = -E2BIG;
+	if (cmd_size > I2400M_BM_CMD_BUF_SIZE)
+		goto error_too_big;
+
+	memcpy(i2400m->bm_cmd_buf, _cmd, cmd_size);	/* Prep command */
+	cmd = i2400m->bm_cmd_buf;
+	if (cmd_size_a > cmd_size)			/* Zero pad space */
+		memset(i2400m->bm_cmd_buf + cmd_size, 0, cmd_size_a - cmd_size);
+	if ((flags & I2400M_BM_CMD_RAW) == 0) {
+		if (WARN_ON(i2400m_brh_get_response_required(cmd) == 0))
+			dev_warn(dev, "SW BUG: response_required == 0\n");
+		i2400m_bm_cmd_prepare(cmd);
+	}
+	d_printf(4, dev, "BM cmd %d: %zu bytes (%zu padded)\n",
+		 opcode, cmd_size, cmd_size_a);
+	d_dump(5, dev, cmd, cmd_size);
+
+	sdio_claim_host(i2400ms->func);			/* Send & check */
+	result = sdio_memcpy_toio(i2400ms->func, I2400MS_DATA_ADDR,
+				  i2400m->bm_cmd_buf, cmd_size_a);
+	sdio_release_host(i2400ms->func);
+	if (result < 0) {
+		dev_err(dev, "BM cmd %d: cannot send: %ld\n",
+			opcode, (long) result);
+		goto error_cmd_send;
+	}
+	result = cmd_size;
+error_cmd_send:
+error_too_big:
+	d_fnend(5, dev, "(i2400m %p cmd %p size %zu) = %d\n",
+		i2400m, _cmd, cmd_size, (int) result);
+	return result;
+}
+
+
+/*
+ * Read an ack from the device's boot-mode (polling)
+ *
+ * @i2400m:
+ * @_ack: pointer to where to store the read data
+ * @ack_size: how many bytes we should read
+ *
+ * Returns: < 0 errno code on error; otherwise, amount of received bytes.
+ *
+ * The ACK for a BM command is always at least sizeof(*ack) bytes, so
+ * check for that. We don't need to check for device reboots
+ *
+ * NOTE: We do an artificial timeout of 1 sec over the SDIO timeout;
+ *     this way we have control over it...there is no way that I know
+ *     of setting an SDIO transaction timeout.
+ */
+ssize_t i2400ms_bus_bm_wait_for_ack(struct i2400m *i2400m,
+				    struct i2400m_bootrom_header *ack,
+				    size_t ack_size)
+{
+	int result;
+	ssize_t rx_size;
+	u64 timeout;
+	struct i2400ms *i2400ms = container_of(i2400m, struct i2400ms, i2400m);
+	struct sdio_func *func = i2400ms->func;
+	struct device *dev = &func->dev;
+
+	BUG_ON(sizeof(*ack) > ack_size);
+
+	d_fnstart(5, dev, "(i2400m %p ack %p size %zu)\n",
+		  i2400m, ack, ack_size);
+
+	timeout = get_jiffies_64() + 2 * HZ;
+	sdio_claim_host(func);
+	while (1) {
+		if (time_after64(get_jiffies_64(), timeout)) {
+			rx_size = -ETIMEDOUT;
+			dev_err(dev, "timeout waiting for ack data\n");
+			goto error_timedout;
+		}
+
+		/* Find the RX size, check if it fits or not -- it if
+		 * doesn't fit, fail, as we have no way to dispose of
+		 * the extra data. */
+		rx_size = __i2400ms_rx_get_size(i2400ms);
+		if (rx_size < 0)
+			goto error_rx_get_size;
+		result = -ENOSPC;		/* Check it fits */
+		if (rx_size < sizeof(*ack)) {
+			rx_size = -EIO;
+			dev_err(dev, "HW BUG? received is too small (%zu vs "
+				"%zu needed)\n", sizeof(*ack), rx_size);
+			goto error_too_small;
+		}
+		if (rx_size > I2400M_BM_ACK_BUF_SIZE) {
+			dev_err(dev, "SW BUG? BM_ACK_BUF is too small (%u vs "
+				"%zu needed)\n", I2400M_BM_ACK_BUF_SIZE,
+				rx_size);
+			goto error_too_small;
+		}
+
+		/* Read it */
+		result = sdio_memcpy_fromio(func, i2400m->bm_ack_buf,
+					    I2400MS_DATA_ADDR, rx_size);
+		if (result == -ETIMEDOUT || result == -ETIME)
+			continue;
+		if (result < 0) {
+			dev_err(dev, "BM SDIO receive (%zu B) failed: %d\n",
+				rx_size, result);
+			goto error_read;
+		} else
+			break;
+	}
+	rx_size = min((ssize_t)ack_size, rx_size);
+	memcpy(ack, i2400m->bm_ack_buf, rx_size);
+error_read:
+error_too_small:
+error_rx_get_size:
+error_timedout:
+	sdio_release_host(func);
+	d_fnend(5, dev, "(i2400m %p ack %p size %zu) = %ld\n",
+		i2400m, ack, ack_size, (long) rx_size);
+	return rx_size;
+}
diff --git a/drivers/net/wimax/i2400m/sdio-rx.c b/drivers/net/wimax/i2400m/sdio-rx.c
new file mode 100644
index 000000000000..a3008b904f7d
--- /dev/null
+++ b/drivers/net/wimax/i2400m/sdio-rx.c
@@ -0,0 +1,255 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * SDIO RX handling
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Dirk Brandewie <dirk.j.brandewie@intel.com>
+ *  - Initial implementation
+ *
+ *
+ * This handles the RX path on SDIO.
+ *
+ * The SDIO bus driver calls the "irq" routine when data is available.
+ * This is not a traditional interrupt routine since the SDIO bus
+ * driver calls us from its irq thread context.  Because of this
+ * sleeping in the SDIO RX IRQ routine is okay.
+ *
+ * From there on, we obtain the size of the data that is available,
+ * allocate an skb, copy it and then pass it to the generic driver's
+ * RX routine [i2400m_rx()].
+ *
+ * ROADMAP
+ *
+ * i2400ms_irq()
+ *   i2400ms_rx()
+ *     __i2400ms_rx_get_size()
+ *     i2400m_rx()
+ *
+ * i2400ms_rx_setup()
+ *
+ * i2400ms_rx_release()
+ */
+#include <linux/workqueue.h>
+#include <linux/wait.h>
+#include <linux/skbuff.h>
+#include <linux/mmc/sdio.h>
+#include <linux/mmc/sdio_func.h>
+#include "i2400m-sdio.h"
+
+#define D_SUBMODULE rx
+#include "sdio-debug-levels.h"
+
+
+/*
+ * Read and return the amount of bytes available for RX
+ *
+ * The RX size has to be read like this: byte reads of three
+ * sequential locations; then glue'em together.
+ *
+ * sdio_readl() doesn't work.
+ */
+ssize_t __i2400ms_rx_get_size(struct i2400ms *i2400ms)
+{
+	int ret, cnt, val;
+	ssize_t rx_size;
+	unsigned xfer_size_addr;
+	struct sdio_func *func = i2400ms->func;
+	struct device *dev = &i2400ms->func->dev;
+
+	d_fnstart(7, dev, "(i2400ms %p)\n", i2400ms);
+	xfer_size_addr = I2400MS_INTR_GET_SIZE_ADDR;
+	rx_size = 0;
+	for (cnt = 0; cnt < 3; cnt++) {
+		val = sdio_readb(func, xfer_size_addr + cnt, &ret);
+		if (ret < 0) {
+			dev_err(dev, "RX: Can't read byte %d of RX size from "
+				"0x%08x: %d\n", cnt, xfer_size_addr + cnt, ret);
+			rx_size = ret;
+			goto error_read;
+		}
+		rx_size = rx_size << 8 | (val & 0xff);
+	}
+	d_printf(6, dev, "RX: rx_size is %ld\n", (long) rx_size);
+error_read:
+	d_fnend(7, dev, "(i2400ms %p) = %ld\n", i2400ms, (long) rx_size);
+	return rx_size;
+}
+
+
+/*
+ * Read data from the device (when in normal)
+ *
+ * Allocate an SKB of the right size, read the data in and then
+ * deliver it to the generic layer.
+ *
+ * We also check for a reboot barker. That means the device died and
+ * we have to reboot it.
+ */
+static
+void i2400ms_rx(struct i2400ms *i2400ms)
+{
+	int ret;
+	struct sdio_func *func = i2400ms->func;
+	struct device *dev = &func->dev;
+	struct i2400m *i2400m = &i2400ms->i2400m;
+	struct sk_buff *skb;
+	ssize_t rx_size;
+
+	d_fnstart(7, dev, "(i2400ms %p)\n", i2400ms);
+	rx_size = __i2400ms_rx_get_size(i2400ms);
+	if (rx_size < 0) {
+		ret = rx_size;
+		goto error_get_size;
+	}
+	ret = -ENOMEM;
+	skb = alloc_skb(rx_size, GFP_ATOMIC);
+	if (NULL == skb) {
+		dev_err(dev, "RX: unable to alloc skb\n");
+		goto error_alloc_skb;
+	}
+
+	ret = sdio_memcpy_fromio(func, skb->data,
+				 I2400MS_DATA_ADDR, rx_size);
+	if (ret < 0) {
+		dev_err(dev, "RX: SDIO data read failed: %d\n", ret);
+		goto error_memcpy_fromio;
+	}
+	/* Check if device has reset */
+	if (!memcmp(skb->data, i2400m_NBOOT_BARKER,
+		    sizeof(i2400m_NBOOT_BARKER))
+	    || !memcmp(skb->data, i2400m_SBOOT_BARKER,
+		       sizeof(i2400m_SBOOT_BARKER))) {
+		ret = i2400m_dev_reset_handle(i2400m);
+		kfree_skb(skb);
+	} else {
+		skb_put(skb, rx_size);
+		i2400m_rx(i2400m, skb);
+	}
+	d_fnend(7, dev, "(i2400ms %p) = void\n", i2400ms);
+	return;
+
+error_memcpy_fromio:
+	kfree_skb(skb);
+error_alloc_skb:
+error_get_size:
+	d_fnend(7, dev, "(i2400ms %p) = %d\n", i2400ms, ret);
+	return;
+}
+
+
+/*
+ * Process an interrupt from the SDIO card
+ *
+ * FIXME: need to process other events that are not just ready-to-read
+ *
+ * Checks there is data ready and then proceeds to read it.
+ */
+static
+void i2400ms_irq(struct sdio_func *func)
+{
+	int ret;
+	struct i2400ms *i2400ms = sdio_get_drvdata(func);
+	struct i2400m *i2400m = &i2400ms->i2400m;
+	struct device *dev = &func->dev;
+	int val;
+
+	d_fnstart(6, dev, "(i2400ms %p)\n", i2400ms);
+	val = sdio_readb(func, I2400MS_INTR_STATUS_ADDR, &ret);
+	if (ret < 0) {
+		dev_err(dev, "RX: Can't read interrupt status: %d\n", ret);
+		goto error_no_irq;
+	}
+	if (!val) {
+		dev_err(dev, "RX: BUG? got IRQ but no interrupt ready?\n");
+		goto error_no_irq;
+	}
+	sdio_writeb(func, 1, I2400MS_INTR_CLEAR_ADDR, &ret);
+	if (WARN_ON(i2400m->boot_mode != 0))
+		dev_err(dev, "RX: SW BUG? boot mode and IRQ is up?\n");
+	else
+		i2400ms_rx(i2400ms);
+error_no_irq:
+	d_fnend(6, dev, "(i2400ms %p) = void\n", i2400ms);
+	return;
+}
+
+
+/*
+ * Setup SDIO RX
+ *
+ * Hooks up the IRQ handler and then enables IRQs.
+ */
+int i2400ms_rx_setup(struct i2400ms *i2400ms)
+{
+	int result;
+	struct sdio_func *func = i2400ms->func;
+	struct device *dev = &func->dev;
+
+	d_fnstart(5, dev, "(i2400ms %p)\n", i2400ms);
+	sdio_claim_host(func);
+	result = sdio_claim_irq(func, i2400ms_irq);
+	if (result < 0) {
+		dev_err(dev, "Cannot claim IRQ: %d\n", result);
+		goto error_irq_claim;
+	}
+	result = 0;
+	sdio_writeb(func, 1, I2400MS_INTR_ENABLE_ADDR, &result);
+	if (result < 0) {
+		sdio_release_irq(func);
+		dev_err(dev, "Failed to enable interrupts %d\n", result);
+	}
+error_irq_claim:
+	sdio_release_host(func);
+	d_fnend(5, dev, "(i2400ms %p) = %d\n", i2400ms, result);
+	return result;
+}
+
+
+/*
+ * Tear down SDIO RX
+ *
+ * Disables IRQs in the device and removes the IRQ handler.
+ */
+void i2400ms_rx_release(struct i2400ms *i2400ms)
+{
+	int result;
+	struct sdio_func *func = i2400ms->func;
+	struct device *dev = &func->dev;
+
+	d_fnstart(5, dev, "(i2400ms %p)\n", i2400ms);
+	sdio_claim_host(func);
+	sdio_writeb(func, 0, I2400MS_INTR_ENABLE_ADDR, &result);
+	sdio_release_irq(func);
+	sdio_release_host(func);
+	d_fnend(5, dev, "(i2400ms %p) = %d\n", i2400ms, result);
+}
diff --git a/drivers/net/wimax/i2400m/sdio-tx.c b/drivers/net/wimax/i2400m/sdio-tx.c
new file mode 100644
index 000000000000..5105a5ebc44f
--- /dev/null
+++ b/drivers/net/wimax/i2400m/sdio-tx.c
@@ -0,0 +1,153 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * SDIO TX transaction backends
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Dirk Brandewie <dirk.j.brandewie@intel.com>
+ *  - Initial implementation
+ *
+ *
+ * Takes the TX messages in the i2400m's driver TX FIFO and sends them
+ * to the device until there are no more.
+ *
+ * If we fail sending the message, we just drop it. There isn't much
+ * we can do at this point. Most of the traffic is network, which has
+ * recovery methods for dropped packets.
+ *
+ * The SDIO functions are not atomic, so we can't run from the context
+ * where i2400m->bus_tx_kick() [i2400ms_bus_tx_kick()] is being called
+ * (some times atomic). Thus, the actual TX work is deferred to a
+ * workqueue.
+ *
+ * ROADMAP
+ *
+ * i2400ms_bus_tx_kick()
+ *   i2400ms_tx_submit()     [through workqueue]
+ *
+ * i2400m_tx_setup()
+ *
+ * i2400m_tx_release()
+ */
+#include <linux/mmc/sdio_func.h>
+#include "i2400m-sdio.h"
+
+#define D_SUBMODULE tx
+#include "sdio-debug-levels.h"
+
+
+/*
+ * Pull TX transations from the TX FIFO and send them to the device
+ * until there are no more.
+ */
+static
+void i2400ms_tx_submit(struct work_struct *ws)
+{
+	int result;
+	struct i2400ms *i2400ms = container_of(ws, struct i2400ms, tx_worker);
+	struct i2400m *i2400m = &i2400ms->i2400m;
+	struct sdio_func *func = i2400ms->func;
+	struct device *dev = &func->dev;
+	struct i2400m_msg_hdr *tx_msg;
+	size_t tx_msg_size;
+
+	d_fnstart(4, dev, "(i2400ms %p, i2400m %p)\n", i2400ms, i2400ms);
+
+	while (NULL != (tx_msg = i2400m_tx_msg_get(i2400m, &tx_msg_size))) {
+		d_printf(2, dev, "TX: submitting %zu bytes\n", tx_msg_size);
+		d_dump(5, dev, tx_msg, tx_msg_size);
+
+		sdio_claim_host(func);
+		result = sdio_memcpy_toio(func, 0, tx_msg, tx_msg_size);
+		sdio_release_host(func);
+
+		i2400m_tx_msg_sent(i2400m);
+
+		if (result < 0) {
+			dev_err(dev, "TX: cannot submit TX; tx_msg @%zu %zu B:"
+				" %d\n", (void *) tx_msg - i2400m->tx_buf,
+				tx_msg_size, result);
+		}
+
+		d_printf(2, dev, "TX: %zub submitted\n", tx_msg_size);
+	}
+
+	d_fnend(4, dev, "(i2400ms %p) = void\n", i2400ms);
+}
+
+
+/*
+ * The generic driver notifies us that there is data ready for TX
+ *
+ * Schedule a run of i2400ms_tx_submit() to handle it.
+ */
+void i2400ms_bus_tx_kick(struct i2400m *i2400m)
+{
+	struct i2400ms *i2400ms = container_of(i2400m, struct i2400ms, i2400m);
+	struct device *dev = &i2400ms->func->dev;
+
+	d_fnstart(3, dev, "(i2400m %p) = void\n", i2400m);
+
+	/* schedule tx work, this is because tx may block, therefore
+	 * it has to run in a thread context.
+	 */
+	queue_work(i2400ms->tx_workqueue, &i2400ms->tx_worker);
+
+	d_fnend(3, dev, "(i2400m %p) = void\n", i2400m);
+}
+
+int i2400ms_tx_setup(struct i2400ms *i2400ms)
+{
+	int result;
+	struct device *dev = &i2400ms->func->dev;
+	struct i2400m *i2400m = &i2400ms->i2400m;
+
+	d_fnstart(5, dev, "(i2400ms %p)\n", i2400ms);
+
+	INIT_WORK(&i2400ms->tx_worker, i2400ms_tx_submit);
+	snprintf(i2400ms->tx_wq_name, sizeof(i2400ms->tx_wq_name),
+		 "%s-tx", i2400m->wimax_dev.name);
+	i2400ms->tx_workqueue =
+		create_singlethread_workqueue(i2400ms->tx_wq_name);
+	if (NULL == i2400ms->tx_workqueue) {
+		dev_err(dev, "TX: failed to create workqueue\n");
+		result = -ENOMEM;
+	} else
+		result = 0;
+	d_fnend(5, dev, "(i2400ms %p) = %d\n", i2400ms, result);
+	return result;
+}
+
+void i2400ms_tx_release(struct i2400ms *i2400ms)
+{
+	destroy_workqueue(i2400ms->tx_workqueue);
+}
diff --git a/drivers/net/wimax/i2400m/sdio.c b/drivers/net/wimax/i2400m/sdio.c
new file mode 100644
index 000000000000..1bfa283bbd8a
--- /dev/null
+++ b/drivers/net/wimax/i2400m/sdio.c
@@ -0,0 +1,511 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * Linux driver model glue for the SDIO device, reset & fw upload
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation <linux-wimax@intel.com>
+ * Dirk Brandewie <dirk.j.brandewie@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ * Yanir Lubetkin <yanirx.lubetkin@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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.
+ *
+ *
+ * See i2400m-sdio.h for a general description of this driver.
+ *
+ * This file implements driver model glue, and hook ups for the
+ * generic driver to implement the bus-specific functions (device
+ * communication setup/tear down, firmware upload and resetting).
+ *
+ * ROADMAP
+ *
+ * i2400m_probe()
+ *   alloc_netdev()
+ *     i2400ms_netdev_setup()
+ *       i2400ms_init()
+ *       i2400m_netdev_setup()
+ *   i2400ms_enable_function()
+ *   i2400m_setup()
+ *
+ * i2400m_remove()
+ *     i2400m_release()
+ *     free_netdev(net_dev)
+ *
+ * i2400ms_bus_reset()            Called by i2400m->bus_reset
+ *   __i2400ms_reset()
+ *     __i2400ms_send_barker()
+ *
+ * i2400ms_bus_dev_start()        Called by i2400m_dev_start() [who is
+ *   i2400ms_tx_setup()           called by i2400m_setup()]
+ *   i2400ms_rx_setup()
+ *
+ * i2400ms_bus_dev_stop()         Called by i2400m_dev_stop() [who is
+ *   i2400ms_rx_release()         is called by i2400m_release()]
+ *   i2400ms_tx_release()
+ *
+ */
+
+#include <linux/debugfs.h>
+#include <linux/mmc/sdio.h>
+#include <linux/mmc/sdio_func.h>
+#include "i2400m-sdio.h"
+#include <linux/wimax/i2400m.h>
+
+#define D_SUBMODULE main
+#include "sdio-debug-levels.h"
+
+/* IOE WiMAX function timeout in seconds */
+static int ioe_timeout = 2;
+module_param(ioe_timeout, int, 0);
+
+/* Our firmware file name */
+#define I2400MS_FW_FILE_NAME "i2400m-fw-sdio-" I2400M_FW_VERSION ".sbcf"
+
+/*
+ * Enable the SDIO function
+ *
+ * Tries to enable the SDIO function; might fail if it is still not
+ * ready (in some hardware, the SDIO WiMAX function is only enabled
+ * when we ask it to explicitly doing). Tries until a timeout is
+ * reached.
+ *
+ * The reverse of this is...sdio_disable_function()
+ *
+ * Returns: 0 if the SDIO function was enabled, < 0 errno code on
+ *     error (-ENODEV when it was unable to enable the function).
+ */
+static
+int i2400ms_enable_function(struct sdio_func *func)
+{
+	u64 timeout;
+	int err;
+	struct device *dev = &func->dev;
+
+	d_fnstart(3, dev, "(func %p)\n", func);
+	/* Setup timeout (FIXME: This needs to read the CIS table to
+	 * get a real timeout) and then wait for the device to signal
+	 * it is ready */
+	timeout = get_jiffies_64() + ioe_timeout * HZ;
+	err = -ENODEV;
+	while (err != 0 && time_before64(get_jiffies_64(), timeout)) {
+		sdio_claim_host(func);
+		err = sdio_enable_func(func);
+		if (0 == err) {
+			sdio_release_host(func);
+			d_printf(2, dev, "SDIO function enabled\n");
+			goto function_enabled;
+		}
+		d_printf(2, dev, "SDIO function failed to enable: %d\n", err);
+		sdio_disable_func(func);
+		sdio_release_host(func);
+		msleep(I2400MS_INIT_SLEEP_INTERVAL);
+	}
+	/* If timed out, device is not there yet -- get -ENODEV so
+	 * the device driver core will retry later on. */
+	if (err == -ETIME) {
+		dev_err(dev, "Can't enable WiMAX function; "
+			" has the function been enabled?\n");
+		err = -ENODEV;
+	}
+function_enabled:
+	d_fnend(3, dev, "(func %p) = %d\n", func, err);
+	return err;
+}
+
+
+/*
+ * Setup driver resources needed to communicate with the device
+ *
+ * The fw needs some time to settle, and it was just uploaded,
+ * so give it a break first. I'd prefer to just wait for the device to
+ * send something, but seems the poking we do to enable SDIO stuff
+ * interferes with it, so just give it a break before starting...
+ */
+static
+int i2400ms_bus_dev_start(struct i2400m *i2400m)
+{
+	int result;
+	struct i2400ms *i2400ms = container_of(i2400m, struct i2400ms, i2400m);
+	struct sdio_func *func = i2400ms->func;
+	struct device *dev = &func->dev;
+
+	d_fnstart(3, dev, "(i2400m %p)\n", i2400m);
+	msleep(200);
+	result = i2400ms_rx_setup(i2400ms);
+	if (result < 0)
+		goto error_rx_setup;
+	result = i2400ms_tx_setup(i2400ms);
+	if (result < 0)
+		goto error_tx_setup;
+	d_fnend(3, dev, "(i2400m %p) = %d\n", i2400m, result);
+	return result;
+
+	i2400ms_tx_release(i2400ms);
+error_tx_setup:
+	i2400ms_rx_release(i2400ms);
+error_rx_setup:
+	d_fnend(3, dev, "(i2400m %p) = void\n", i2400m);
+	return result;
+}
+
+
+static
+void i2400ms_bus_dev_stop(struct i2400m *i2400m)
+{
+	struct i2400ms *i2400ms = container_of(i2400m, struct i2400ms, i2400m);
+	struct sdio_func *func = i2400ms->func;
+	struct device *dev = &func->dev;
+
+	d_fnstart(3, dev, "(i2400m %p)\n", i2400m);
+	i2400ms_rx_release(i2400ms);
+	i2400ms_tx_release(i2400ms);
+	d_fnend(3, dev, "(i2400m %p) = void\n", i2400m);
+}
+
+
+/*
+ * Sends a barker buffer to the device
+ *
+ * This helper will allocate a kmalloced buffer and use it to transmit
+ * (then free it). Reason for this is that the SDIO host controller
+ * expects alignment (unknown exactly which) which the stack won't
+ * really provide and certain arches/host-controller combinations
+ * cannot use stack/vmalloc/text areas for DMA transfers.
+ */
+static
+int __i2400ms_send_barker(struct i2400ms *i2400ms,
+			  const __le32 *barker, size_t barker_size)
+{
+	int  ret;
+	struct sdio_func *func = i2400ms->func;
+	struct device *dev = &func->dev;
+	void *buffer;
+
+	ret = -ENOMEM;
+	buffer = kmalloc(I2400MS_BLK_SIZE, GFP_KERNEL);
+	if (buffer == NULL)
+		goto error_kzalloc;
+
+	memcpy(buffer, barker, barker_size);
+	sdio_claim_host(func);
+	ret = sdio_memcpy_toio(func, 0, buffer, I2400MS_BLK_SIZE);
+	sdio_release_host(func);
+
+	if (ret < 0)
+		d_printf(0, dev, "E: barker error: %d\n", ret);
+
+	kfree(buffer);
+error_kzalloc:
+	return ret;
+}
+
+
+/*
+ * Reset a device at different levels (warm, cold or bus)
+ *
+ * @i2400ms: device descriptor
+ * @reset_type: soft, warm or bus reset (I2400M_RT_WARM/SOFT/BUS)
+ *
+ * FIXME: not tested -- need to confirm expected effects
+ *
+ * Warm and cold resets get an SDIO reset if they fail (unimplemented)
+ *
+ * Warm reset:
+ *
+ * The device will be fully reset internally, but won't be
+ * disconnected from the USB bus (so no reenumeration will
+ * happen). Firmware upload will be neccessary.
+ *
+ * The device will send a reboot barker in the notification endpoint
+ * that will trigger the driver to reinitialize the state
+ * automatically from notif.c:i2400m_notification_grok() into
+ * i2400m_dev_bootstrap_delayed().
+ *
+ * Cold and bus (USB) reset:
+ *
+ * The device will be fully reset internally, disconnected from the
+ * USB bus an a reenumeration will happen. Firmware upload will be
+ * neccessary. Thus, we don't do any locking or struct
+ * reinitialization, as we are going to be fully disconnected and
+ * reenumerated.
+ *
+ * Note we need to return -ENODEV if a warm reset was requested and we
+ * had to resort to a bus reset. See i2400m_op_reset(), wimax_reset()
+ * and wimax_dev->op_reset.
+ *
+ * WARNING: no driver state saved/fixed
+ */
+static
+int i2400ms_bus_reset(struct i2400m *i2400m, enum i2400m_reset_type rt)
+{
+	int result;
+	struct i2400ms *i2400ms =
+		container_of(i2400m, struct i2400ms, i2400m);
+	struct device *dev = i2400m_dev(i2400m);
+	static const __le32 i2400m_WARM_BOOT_BARKER[4] = {
+		__constant_cpu_to_le32(I2400M_WARM_RESET_BARKER),
+		__constant_cpu_to_le32(I2400M_WARM_RESET_BARKER),
+		__constant_cpu_to_le32(I2400M_WARM_RESET_BARKER),
+		__constant_cpu_to_le32(I2400M_WARM_RESET_BARKER),
+	};
+	static const __le32 i2400m_COLD_BOOT_BARKER[4] = {
+		__constant_cpu_to_le32(I2400M_COLD_RESET_BARKER),
+		__constant_cpu_to_le32(I2400M_COLD_RESET_BARKER),
+		__constant_cpu_to_le32(I2400M_COLD_RESET_BARKER),
+		__constant_cpu_to_le32(I2400M_COLD_RESET_BARKER),
+	};
+
+	if (rt == I2400M_RT_WARM)
+		result = __i2400ms_send_barker(i2400ms, i2400m_WARM_BOOT_BARKER,
+					       sizeof(i2400m_WARM_BOOT_BARKER));
+	else if (rt == I2400M_RT_COLD)
+		result = __i2400ms_send_barker(i2400ms, i2400m_COLD_BOOT_BARKER,
+					       sizeof(i2400m_COLD_BOOT_BARKER));
+	else if (rt == I2400M_RT_BUS) {
+do_bus_reset:
+		dev_err(dev, "FIXME: SDIO bus reset not implemented\n");
+		result = rt == I2400M_RT_WARM ? -ENODEV : -ENOSYS;
+	} else
+		BUG();
+	if (result < 0 && rt != I2400M_RT_BUS) {
+		dev_err(dev, "%s reset failed (%d); trying SDIO reset\n",
+			rt == I2400M_RT_WARM ? "warm" : "cold", result);
+		rt = I2400M_RT_BUS;
+		goto do_bus_reset;
+	}
+	return result;
+}
+
+
+static
+void i2400ms_netdev_setup(struct net_device *net_dev)
+{
+	struct i2400m *i2400m = net_dev_to_i2400m(net_dev);
+	struct i2400ms *i2400ms = container_of(i2400m, struct i2400ms, i2400m);
+	i2400ms_init(i2400ms);
+	i2400m_netdev_setup(net_dev);
+}
+
+
+/*
+ * Debug levels control; see debug.h
+ */
+struct d_level D_LEVEL[] = {
+	D_SUBMODULE_DEFINE(main),
+	D_SUBMODULE_DEFINE(tx),
+	D_SUBMODULE_DEFINE(rx),
+	D_SUBMODULE_DEFINE(fw),
+};
+size_t D_LEVEL_SIZE = ARRAY_SIZE(D_LEVEL);
+
+
+#define __debugfs_register(prefix, name, parent)			\
+do {									\
+	result = d_level_register_debugfs(prefix, name, parent);	\
+	if (result < 0)							\
+		goto error;						\
+} while (0)
+
+
+static
+int i2400ms_debugfs_add(struct i2400ms *i2400ms)
+{
+	int result;
+	struct dentry *dentry = i2400ms->i2400m.wimax_dev.debugfs_dentry;
+
+	dentry = debugfs_create_dir("i2400m-usb", dentry);
+	result = PTR_ERR(dentry);
+	if (IS_ERR(dentry)) {
+		if (result == -ENODEV)
+			result = 0;	/* No debugfs support */
+		goto error;
+	}
+	i2400ms->debugfs_dentry = dentry;
+	__debugfs_register("dl_", main, dentry);
+	__debugfs_register("dl_", tx, dentry);
+	__debugfs_register("dl_", rx, dentry);
+	__debugfs_register("dl_", fw, dentry);
+
+	return 0;
+
+error:
+	debugfs_remove_recursive(i2400ms->debugfs_dentry);
+	return result;
+}
+
+
+/*
+ * Probe a i2400m interface and register it
+ *
+ * @func:    SDIO function
+ * @id:      SDIO device ID
+ * @returns: 0 if ok, < 0 errno code on error.
+ *
+ * Alloc a net device, initialize the bus-specific details and then
+ * calls the bus-generic initialization routine. That will register
+ * the wimax and netdev devices, upload the firmware [using
+ * _bus_bm_*()], call _bus_dev_start() to finalize the setup of the
+ * communication with the device and then will start to talk to it to
+ * finnish setting it up.
+ *
+ * Initialization is tricky; some instances of the hw are packed with
+ * others in a way that requires a third driver that enables the WiMAX
+ * function. In those cases, we can't enable the SDIO function and
+ * we'll return with -ENODEV. When the driver that enables the WiMAX
+ * function does its thing, it has to do a bus_rescan_devices() on the
+ * SDIO bus so this driver is called again to enumerate the WiMAX
+ * function.
+ */
+static
+int i2400ms_probe(struct sdio_func *func,
+		  const struct sdio_device_id *id)
+{
+	int result;
+	struct net_device *net_dev;
+	struct device *dev = &func->dev;
+	struct i2400m *i2400m;
+	struct i2400ms *i2400ms;
+
+	/* Allocate instance [calls i2400m_netdev_setup() on it]. */
+	result = -ENOMEM;
+	net_dev = alloc_netdev(sizeof(*i2400ms), "wmx%d",
+			       i2400ms_netdev_setup);
+	if (net_dev == NULL) {
+		dev_err(dev, "no memory for network device instance\n");
+		goto error_alloc_netdev;
+	}
+	SET_NETDEV_DEV(net_dev, dev);
+	i2400m = net_dev_to_i2400m(net_dev);
+	i2400ms = container_of(i2400m, struct i2400ms, i2400m);
+	i2400m->wimax_dev.net_dev = net_dev;
+	i2400ms->func = func;
+	sdio_set_drvdata(func, i2400ms);
+
+	i2400m->bus_tx_block_size = I2400MS_BLK_SIZE;
+	i2400m->bus_pl_size_max = I2400MS_PL_SIZE_MAX;
+	i2400m->bus_dev_start = i2400ms_bus_dev_start;
+	i2400m->bus_dev_stop = i2400ms_bus_dev_stop;
+	i2400m->bus_tx_kick = i2400ms_bus_tx_kick;
+	i2400m->bus_reset = i2400ms_bus_reset;
+	i2400m->bus_bm_cmd_send = i2400ms_bus_bm_cmd_send;
+	i2400m->bus_bm_wait_for_ack = i2400ms_bus_bm_wait_for_ack;
+	i2400m->bus_fw_name = I2400MS_FW_FILE_NAME;
+	i2400m->bus_bm_mac_addr_impaired = 1;
+
+	result = i2400ms_enable_function(i2400ms->func);
+	if (result < 0) {
+		dev_err(dev, "Cannot enable SDIO function: %d\n", result);
+		goto error_func_enable;
+	}
+
+	sdio_claim_host(func);
+	result = sdio_set_block_size(func, I2400MS_BLK_SIZE);
+	if (result < 0) {
+		dev_err(dev, "Failed to set block size: %d\n", result);
+		goto error_set_blk_size;
+	}
+	sdio_release_host(func);
+
+	result = i2400m_setup(i2400m, I2400M_BRI_NO_REBOOT);
+	if (result < 0) {
+		dev_err(dev, "cannot setup device: %d\n", result);
+		goto error_setup;
+	}
+
+	result = i2400ms_debugfs_add(i2400ms);
+	if (result < 0) {
+		dev_err(dev, "cannot create SDIO debugfs: %d\n",
+			result);
+		goto error_debugfs_add;
+	}
+	return 0;
+
+error_debugfs_add:
+	i2400m_release(i2400m);
+error_setup:
+	sdio_set_drvdata(func, NULL);
+	sdio_claim_host(func);
+error_set_blk_size:
+	sdio_disable_func(func);
+	sdio_release_host(func);
+error_func_enable:
+	free_netdev(net_dev);
+error_alloc_netdev:
+	return result;
+}
+
+
+static
+void i2400ms_remove(struct sdio_func *func)
+{
+	struct device *dev = &func->dev;
+	struct i2400ms *i2400ms = sdio_get_drvdata(func);
+	struct i2400m *i2400m = &i2400ms->i2400m;
+	struct net_device *net_dev = i2400m->wimax_dev.net_dev;
+
+	d_fnstart(3, dev, "SDIO func %p\n", func);
+	debugfs_remove_recursive(i2400ms->debugfs_dentry);
+	i2400m_release(i2400m);
+	sdio_set_drvdata(func, NULL);
+	sdio_claim_host(func);
+	sdio_disable_func(func);
+	sdio_release_host(func);
+	free_netdev(net_dev);
+	d_fnend(3, dev, "SDIO func %p\n", func);
+}
+
+enum {
+	I2400MS_INTEL_VID = 0x89,
+};
+
+static
+const struct sdio_device_id i2400ms_sdio_ids[] = {
+	/* Intel: i2400m WiMAX over SDIO */
+	{ SDIO_DEVICE(I2400MS_INTEL_VID, 0x1402) },
+	{ }, 			/* end: all zeroes */
+};
+MODULE_DEVICE_TABLE(sdio, i2400ms_sdio_ids);
+
+
+static
+struct sdio_driver i2400m_sdio_driver = {
+	.name		= KBUILD_MODNAME,
+	.probe		= i2400ms_probe,
+	.remove		= i2400ms_remove,
+	.id_table	= i2400ms_sdio_ids,
+};
+
+
+static
+int __init i2400ms_driver_init(void)
+{
+	return sdio_register_driver(&i2400m_sdio_driver);
+}
+module_init(i2400ms_driver_init);
+
+
+static
+void __exit i2400ms_driver_exit(void)
+{
+	flush_scheduled_work();	/* for the stuff we schedule */
+	sdio_unregister_driver(&i2400m_sdio_driver);
+}
+module_exit(i2400ms_driver_exit);
+
+
+MODULE_AUTHOR("Intel Corporation <linux-wimax@intel.com>");
+MODULE_DESCRIPTION("Intel 2400M WiMAX networking for SDIO");
+MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(I2400MS_FW_FILE_NAME);
diff --git a/drivers/net/wimax/i2400m/tx.c b/drivers/net/wimax/i2400m/tx.c
new file mode 100644
index 000000000000..613a88ffd651
--- /dev/null
+++ b/drivers/net/wimax/i2400m/tx.c
@@ -0,0 +1,817 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * Generic (non-bus specific) TX handling
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Yanir Lubetkin <yanirx.lubetkin@intel.com>
+ *  - Initial implementation
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ *  - Rewritten to use a single FIFO to lower the memory allocation
+ *    pressure and optimize cache hits when copying to the queue, as
+ *    well as splitting out bus-specific code.
+ *
+ *
+ * Implements data transmission to the device; this is done through a
+ * software FIFO, as data/control frames can be coalesced (while the
+ * device is reading the previous tx transaction, others accumulate).
+ *
+ * A FIFO is used because at the end it is resource-cheaper that trying
+ * to implement scatter/gather over USB. As well, most traffic is going
+ * to be download (vs upload).
+ *
+ * The format for sending/receiving data to/from the i2400m is
+ * described in detail in rx.c:PROTOCOL FORMAT. In here we implement
+ * the transmission of that. This is split between a bus-independent
+ * part that just prepares everything and a bus-specific part that
+ * does the actual transmission over the bus to the device (in the
+ * bus-specific driver).
+ *
+ *
+ * The general format of a device-host transaction is MSG-HDR, PLD1,
+ * PLD2...PLDN, PL1, PL2,...PLN, PADDING.
+ *
+ * Because we need the send payload descriptors and then payloads and
+ * because it is kind of expensive to do scatterlists in USB (one URB
+ * per node), it becomes cheaper to append all the data to a FIFO
+ * (copying to a FIFO potentially in cache is cheaper).
+ *
+ * Then the bus-specific code takes the parts of that FIFO that are
+ * written and passes them to the device.
+ *
+ * So the concepts to keep in mind there are:
+ *
+ * We use a FIFO to queue the data in a linear buffer. We first append
+ * a MSG-HDR, space for I2400M_TX_PLD_MAX payload descriptors and then
+ * go appending payloads until we run out of space or of payload
+ * descriptors. Then we append padding to make the whole transaction a
+ * multiple of i2400m->bus_tx_block_size (as defined by the bus layer).
+ *
+ * - A TX message: a combination of a message header, payload
+ *   descriptors and payloads.
+ *
+ *     Open: it is marked as active (i2400m->tx_msg is valid) and we
+ *       can keep adding payloads to it.
+ *
+ *     Closed: we are not appending more payloads to this TX message
+ *       (exahusted space in the queue, too many payloads or
+ *       whichever).  We have appended padding so the whole message
+ *       length is aligned to i2400m->bus_tx_block_size (as set by the
+ *       bus/transport layer).
+ *
+ * - Most of the time we keep a TX message open to which we append
+ *   payloads.
+ *
+ * - If we are going to append and there is no more space (we are at
+ *   the end of the FIFO), we close the message, mark the rest of the
+ *   FIFO space unusable (skip_tail), create a new message at the
+ *   beginning of the FIFO (if there is space) and append the message
+ *   there.
+ *
+ *   This is because we need to give linear TX messages to the bus
+ *   engine. So we don't write a message to the remaining FIFO space
+ *   until the tail and continue at the head of it.
+ *
+ * - We overload one of the fields in the message header to use it as
+ *   'size' of the TX message, so we can iterate over them. It also
+ *   contains a flag that indicates if we have to skip it or not.
+ *   When we send the buffer, we update that to its real on-the-wire
+ *   value.
+ *
+ * - The MSG-HDR PLD1...PLD2 stuff has to be a size multiple of 16.
+ *
+ *   It follows that if MSG-HDR says we have N messages, the whole
+ *   header + descriptors is 16 + 4*N; for those to be a multiple of
+ *   16, it follows that N can be 4, 8, 12, ... (32, 48, 64, 80...
+ *   bytes).
+ *
+ *   So if we have only 1 payload, we have to submit a header that in
+ *   all truth has space for 4.
+ *
+ *   The implication is that we reserve space for 12 (64 bytes); but
+ *   if we fill up only (eg) 2, our header becomes 32 bytes only. So
+ *   the TX engine has to shift those 32 bytes of msg header and 2
+ *   payloads and padding so that right after it the payloads start
+ *   and the TX engine has to know about that.
+ *
+ *   It is cheaper to move the header up than the whole payloads down.
+ *
+ *   We do this in i2400m_tx_close(). See 'i2400m_msg_hdr->offset'.
+ *
+ * - Each payload has to be size-padded to 16 bytes; before appending
+ *   it, we just do it.
+ *
+ * - The whole message has to be padded to i2400m->bus_tx_block_size;
+ *   we do this at close time. Thus, when reserving space for the
+ *   payload, we always make sure there is also free space for this
+ *   padding that sooner or later will happen.
+ *
+ * When we append a message, we tell the bus specific code to kick in
+ * TXs. It will TX (in parallel) until the buffer is exhausted--hence
+ * the lockin we do. The TX code will only send a TX message at the
+ * time (which remember, might contain more than one payload). Of
+ * course, when the bus-specific driver attempts to TX a message that
+ * is still open, it gets closed first.
+ *
+ * Gee, this is messy; well a picture. In the example below we have a
+ * partially full FIFO, with a closed message ready to be delivered
+ * (with a moved message header to make sure it is size-aligned to
+ * 16), TAIL room that was unusable (and thus is marked with a message
+ * header that says 'skip this') and at the head of the buffer, an
+ * imcomplete message with a couple of payloads.
+ *
+ * N   ___________________________________________________
+ *    |                                                   |
+ *    |     TAIL room                                     |
+ *    |                                                   |
+ *    |  msg_hdr to skip (size |= 0x80000)                |
+ *    |---------------------------------------------------|-------
+ *    |                                                   |  /|\
+ *    |                                                   |   |
+ *    |  TX message padding                               |   |
+ *    |                                                   |   |
+ *    |                                                   |   |
+ *    |- - - - - - - - - - - - - - - - - - - - - - - - - -|   |
+ *    |                                                   |   |
+ *    |  payload 1                                        |   |
+ *    |                                                   | N * tx_block_size
+ *    |                                                   |   |
+ *    |- - - - - - - - - - - - - - - - - - - - - - - - - -|   |
+ *    |                                                   |   |
+ *    |  payload 1                                        |   |
+ *    |                                                   |   |
+ *    |                                                   |   |
+ *    |- - - - - - - - - - - - - - - - - - - - - - - - - -|- -|- - - -
+ *    |  padding 3                  /|\                   |   |   /|\
+ *    |  padding 2                   |                    |   |    |
+ *    |  pld 1                32 bytes (2 * 16)           |   |    |
+ *    |  pld 0                       |                    |   |    |
+ *    |  moved msg_hdr              \|/                   |  \|/   |
+ *    |- - - - - - - - - - - - - - - - - - - - - - - - - -|- - -   |
+ *    |                                                   |    _PLD_SIZE
+ *    |  unused                                           |        |
+ *    |                                                   |        |
+ *    |- - - - - - - - - - - - - - - - - - - - - - - - - -|        |
+ *    |  msg_hdr (size X)       [this message is closed]  |       \|/
+ *    |===================================================|========== <=== OUT
+ *    |                                                   |
+ *    |                                                   |
+ *    |                                                   |
+ *    |          Free rooom                               |
+ *    |                                                   |
+ *    |                                                   |
+ *    |                                                   |
+ *    |                                                   |
+ *    |                                                   |
+ *    |                                                   |
+ *    |                                                   |
+ *    |                                                   |
+ *    |                                                   |
+ *    |===================================================|========== <=== IN
+ *    |                                                   |
+ *    |                                                   |
+ *    |                                                   |
+ *    |                                                   |
+ *    |  payload 1                                        |
+ *    |                                                   |
+ *    |                                                   |
+ *    |- - - - - - - - - - - - - - - - - - - - - - - - - -|
+ *    |                                                   |
+ *    |  payload 0                                        |
+ *    |                                                   |
+ *    |                                                   |
+ *    |- - - - - - - - - - - - - - - - - - - - - - - - - -|
+ *    |  pld 11                     /|\                   |
+ *    |  ...                         |                    |
+ *    |  pld 1                64 bytes (2 * 16)           |
+ *    |  pld 0                       |                    |
+ *    |  msg_hdr (size X)           \|/ [message is open] |
+ * 0   ---------------------------------------------------
+ *
+ *
+ * ROADMAP
+ *
+ * i2400m_tx_setup()           Called by i2400m_setup
+ * i2400m_tx_release()         Called by i2400m_release()
+ *
+ *  i2400m_tx()                 Called to send data or control frames
+ *    i2400m_tx_fifo_push()     Allocates append-space in the FIFO
+ *    i2400m_tx_new()           Opens a new message in the FIFO
+ *    i2400m_tx_fits()          Checks if a new payload fits in the message
+ *    i2400m_tx_close()         Closes an open message in the FIFO
+ *    i2400m_tx_skip_tail()     Marks unusable FIFO tail space
+ *    i2400m->bus_tx_kick()
+ *
+ * Now i2400m->bus_tx_kick() is the the bus-specific driver backend
+ * implementation; that would do:
+ *
+ * i2400m->bus_tx_kick()
+ *   i2400m_tx_msg_get()	Gets first message ready to go
+ *   ...sends it...
+ *   i2400m_tx_msg_sent()       Ack the message is sent; repeat from
+ *                              _tx_msg_get() until it returns NULL
+ *                               (FIFO empty).
+ */
+#include <linux/netdevice.h>
+#include "i2400m.h"
+
+
+#define D_SUBMODULE tx
+#include "debug-levels.h"
+
+enum {
+	/**
+	 * TX Buffer size
+	 *
+	 * Doc says maximum transaction is 16KiB. If we had 16KiB en
+	 * route and 16KiB being queued, it boils down to needing
+	 * 32KiB.
+	 */
+	I2400M_TX_BUF_SIZE = 32768,
+	/**
+	 * Message header and payload descriptors have to be 16
+	 * aligned (16 + 4 * N = 16 * M). If we take that average sent
+	 * packets are MTU size (~1400-~1500) it follows that we could
+	 * fit at most 10-11 payloads in one transaction. To meet the
+	 * alignment requirement, that means we need to leave space
+	 * for 12 (64 bytes). To simplify, we leave space for that. If
+	 * at the end there are less, we pad up to the nearest
+	 * multiple of 16.
+	 */
+	I2400M_TX_PLD_MAX = 12,
+	I2400M_TX_PLD_SIZE = sizeof(struct i2400m_msg_hdr)
+	+ I2400M_TX_PLD_MAX * sizeof(struct i2400m_pld),
+	I2400M_TX_SKIP = 0x80000000,
+};
+
+#define TAIL_FULL ((void *)~(unsigned long)NULL)
+
+/*
+ * Allocate @size bytes in the TX fifo, return a pointer to it
+ *
+ * @i2400m: device descriptor
+ * @size: size of the buffer we need to allocate
+ * @padding: ensure that there is at least this many bytes of free
+ *     contiguous space in the fifo. This is needed because later on
+ *     we might need to add padding.
+ *
+ * Returns:
+ *
+ *     Pointer to the allocated space. NULL if there is no
+ *     space. TAIL_FULL if there is no space at the tail but there is at
+ *     the head (Case B below).
+ *
+ * These are the two basic cases we need to keep an eye for -- it is
+ * much better explained in linux/kernel/kfifo.c, but this code
+ * basically does the same. No rocket science here.
+ *
+ *       Case A               Case B
+ * N  ___________          ___________
+ *   | tail room |        |   data    |
+ *   |           |        |           |
+ *   |<-  IN   ->|        |<-  OUT  ->|
+ *   |           |        |           |
+ *   |   data    |        |   room    |
+ *   |           |        |           |
+ *   |<-  OUT  ->|        |<-  IN   ->|
+ *   |           |        |           |
+ *   | head room |        |   data    |
+ * 0  -----------          -----------
+ *
+ * We allocate only *contiguous* space.
+ *
+ * We can allocate only from 'room'. In Case B, it is simple; in case
+ * A, we only try from the tail room; if it is not enough, we just
+ * fail and return TAIL_FULL and let the caller figure out if we wants to
+ * skip the tail room and try to allocate from the head.
+ *
+ * Note:
+ *
+ *     Assumes i2400m->tx_lock is taken, and we use that as a barrier
+ *
+ *     The indexes keep increasing and we reset them to zero when we
+ *     pop data off the queue
+ */
+static
+void *i2400m_tx_fifo_push(struct i2400m *i2400m, size_t size, size_t padding)
+{
+	struct device *dev = i2400m_dev(i2400m);
+	size_t room, tail_room, needed_size;
+	void *ptr;
+
+	needed_size = size + padding;
+	room = I2400M_TX_BUF_SIZE - (i2400m->tx_in - i2400m->tx_out);
+	if (room < needed_size)	{ /* this takes care of Case B */
+		d_printf(2, dev, "fifo push %zu/%zu: no space\n",
+			 size, padding);
+		return NULL;
+	}
+	/* Is there space at the tail? */
+	tail_room = I2400M_TX_BUF_SIZE - i2400m->tx_in % I2400M_TX_BUF_SIZE;
+	if (tail_room < needed_size) {
+		if (i2400m->tx_out % I2400M_TX_BUF_SIZE
+		    < i2400m->tx_in % I2400M_TX_BUF_SIZE) {
+			d_printf(2, dev, "fifo push %zu/%zu: tail full\n",
+				 size, padding);
+			return TAIL_FULL;	/* There might be head space */
+		} else {
+			d_printf(2, dev, "fifo push %zu/%zu: no head space\n",
+				 size, padding);
+			return NULL;	/* There is no space */
+		}
+	}
+	ptr = i2400m->tx_buf + i2400m->tx_in % I2400M_TX_BUF_SIZE;
+	d_printf(2, dev, "fifo push %zu/%zu: at @%zu\n", size, padding,
+		 i2400m->tx_in % I2400M_TX_BUF_SIZE);
+	i2400m->tx_in += size;
+	return ptr;
+}
+
+
+/*
+ * Mark the tail of the FIFO buffer as 'to-skip'
+ *
+ * We should never hit the BUG_ON() because all the sizes we push to
+ * the FIFO are padded to be a multiple of 16 -- the size of *msg
+ * (I2400M_PL_PAD for the payloads, I2400M_TX_PLD_SIZE for the
+ * header).
+ *
+ * Note:
+ *
+ *     Assumes i2400m->tx_lock is taken, and we use that as a barrier
+ */
+static
+void i2400m_tx_skip_tail(struct i2400m *i2400m)
+{
+	struct device *dev = i2400m_dev(i2400m);
+	size_t tx_in = i2400m->tx_in % I2400M_TX_BUF_SIZE;
+	size_t tail_room = I2400M_TX_BUF_SIZE - tx_in;
+	struct i2400m_msg_hdr *msg = i2400m->tx_buf + tx_in;
+	BUG_ON(tail_room < sizeof(*msg));
+	msg->size = tail_room | I2400M_TX_SKIP;
+	d_printf(2, dev, "skip tail: skipping %zu bytes @%zu\n",
+		 tail_room, tx_in);
+	i2400m->tx_in += tail_room;
+}
+
+
+/*
+ * Check if a skb will fit in the TX queue's current active TX
+ * message (if there are still descriptors left unused).
+ *
+ * Returns:
+ *     0 if the message won't fit, 1 if it will.
+ *
+ * Note:
+ *
+ *     Assumes a TX message is active (i2400m->tx_msg).
+ *
+ *     Assumes i2400m->tx_lock is taken, and we use that as a barrier
+ */
+static
+unsigned i2400m_tx_fits(struct i2400m *i2400m)
+{
+	struct i2400m_msg_hdr *msg_hdr = i2400m->tx_msg;
+	return le16_to_cpu(msg_hdr->num_pls) < I2400M_TX_PLD_MAX;
+
+}
+
+
+/*
+ * Start a new TX message header in the queue.
+ *
+ * Reserve memory from the base FIFO engine and then just initialize
+ * the message header.
+ *
+ * We allocate the biggest TX message header we might need (one that'd
+ * fit I2400M_TX_PLD_MAX payloads) -- when it is closed it will be
+ * 'ironed it out' and the unneeded parts removed.
+ *
+ * NOTE:
+ *
+ *     Assumes that the previous message is CLOSED (eg: either
+ *     there was none or 'i2400m_tx_close()' was called on it).
+ *
+ *     Assumes i2400m->tx_lock is taken, and we use that as a barrier
+ */
+static
+void i2400m_tx_new(struct i2400m *i2400m)
+{
+	struct device *dev = i2400m_dev(i2400m);
+	struct i2400m_msg_hdr *tx_msg;
+	BUG_ON(i2400m->tx_msg != NULL);
+try_head:
+	tx_msg = i2400m_tx_fifo_push(i2400m, I2400M_TX_PLD_SIZE, 0);
+	if (tx_msg == NULL)
+		goto out;
+	else if (tx_msg == TAIL_FULL) {
+		i2400m_tx_skip_tail(i2400m);
+		d_printf(2, dev, "new TX message: tail full, trying head\n");
+		goto try_head;
+	}
+	memset(tx_msg, 0, I2400M_TX_PLD_SIZE);
+	tx_msg->size = I2400M_TX_PLD_SIZE;
+out:
+	i2400m->tx_msg = tx_msg;
+	d_printf(2, dev, "new TX message: %p @%zu\n",
+		 tx_msg, (void *) tx_msg - i2400m->tx_buf);
+}
+
+
+/*
+ * Finalize the current TX message header
+ *
+ * Sets the message header to be at the proper location depending on
+ * how many descriptors we have (check documentation at the file's
+ * header for more info on that).
+ *
+ * Appends padding bytes to make sure the whole TX message (counting
+ * from the 'relocated' message header) is aligned to
+ * tx_block_size. We assume the _append() code has left enough space
+ * in the FIFO for that. If there are no payloads, just pass, as it
+ * won't be transferred.
+ *
+ * The amount of padding bytes depends on how many payloads are in the
+ * TX message, as the "msg header and payload descriptors" will be
+ * shifted up in the buffer.
+ */
+static
+void i2400m_tx_close(struct i2400m *i2400m)
+{
+	struct device *dev = i2400m_dev(i2400m);
+	struct i2400m_msg_hdr *tx_msg = i2400m->tx_msg;
+	struct i2400m_msg_hdr *tx_msg_moved;
+	size_t aligned_size, padding, hdr_size;
+	void *pad_buf;
+
+	if (tx_msg->size & I2400M_TX_SKIP)	/* a skipper? nothing to do */
+		goto out;
+
+	/* Relocate the message header
+	 *
+	 * Find the current header size, align it to 16 and if we need
+	 * to move it so the tail is next to the payloads, move it and
+	 * set the offset.
+	 *
+	 * If it moved, this header is good only for transmission; the
+	 * original one (it is kept if we moved) is still used to
+	 * figure out where the next TX message starts (and where the
+	 * offset to the moved header is).
+	 */
+	hdr_size = sizeof(*tx_msg)
+		+ le16_to_cpu(tx_msg->num_pls) * sizeof(tx_msg->pld[0]);
+	hdr_size = ALIGN(hdr_size, I2400M_PL_PAD);
+	tx_msg->offset = I2400M_TX_PLD_SIZE - hdr_size;
+	tx_msg_moved = (void *) tx_msg + tx_msg->offset;
+	memmove(tx_msg_moved, tx_msg, hdr_size);
+	tx_msg_moved->size -= tx_msg->offset;
+	/*
+	 * Now figure out how much we have to add to the (moved!)
+	 * message so the size is a multiple of i2400m->bus_tx_block_size.
+	 */
+	aligned_size = ALIGN(tx_msg_moved->size, i2400m->bus_tx_block_size);
+	padding = aligned_size - tx_msg_moved->size;
+	if (padding > 0) {
+		pad_buf = i2400m_tx_fifo_push(i2400m, padding, 0);
+		if (unlikely(WARN_ON(pad_buf == NULL
+				     || pad_buf == TAIL_FULL))) {
+			/* This should not happen -- append should verify
+			 * there is always space left at least to append
+			 * tx_block_size */
+			dev_err(dev,
+				"SW BUG! Possible data leakage from memory the "
+				"device should not read for padding - "
+				"size %lu aligned_size %zu tx_buf %p in "
+				"%zu out %zu\n",
+				(unsigned long) tx_msg_moved->size,
+				aligned_size, i2400m->tx_buf, i2400m->tx_in,
+				i2400m->tx_out);
+		} else
+			memset(pad_buf, 0xad, padding);
+	}
+	tx_msg_moved->padding = cpu_to_le16(padding);
+	tx_msg_moved->size += padding;
+	if (tx_msg != tx_msg_moved)
+		tx_msg->size += padding;
+out:
+	i2400m->tx_msg = NULL;
+}
+
+
+/**
+ * i2400m_tx - send the data in a buffer to the device
+ *
+ * @buf: pointer to the buffer to transmit
+ *
+ * @buf_len: buffer size
+ *
+ * @pl_type: type of the payload we are sending.
+ *
+ * Returns:
+ *     0 if ok, < 0 errno code on error (-ENOSPC, if there is no more
+ *     room for the message in the queue).
+ *
+ * Appends the buffer to the TX FIFO and notifies the bus-specific
+ * part of the driver that there is new data ready to transmit.
+ * Once this function returns, the buffer has been copied, so it can
+ * be reused.
+ *
+ * The steps followed to append are explained in detail in the file
+ * header.
+ *
+ * Whenever we write to a message, we increase msg->size, so it
+ * reflects exactly how big the message is. This is needed so that if
+ * we concatenate two messages before they can be sent, the code that
+ * sends the messages can find the boundaries (and it will replace the
+ * size with the real barker before sending).
+ *
+ * Note:
+ *
+ *     Cold and warm reset payloads need to be sent as a single
+ *     payload, so we handle that.
+ */
+int i2400m_tx(struct i2400m *i2400m, const void *buf, size_t buf_len,
+	      enum i2400m_pt pl_type)
+{
+	int result = -ENOSPC;
+	struct device *dev = i2400m_dev(i2400m);
+	unsigned long flags;
+	size_t padded_len;
+	void *ptr;
+	unsigned is_singleton = pl_type == I2400M_PT_RESET_WARM
+		|| pl_type == I2400M_PT_RESET_COLD;
+
+	d_fnstart(3, dev, "(i2400m %p skb %p [%zu bytes] pt %u)\n",
+		  i2400m, buf, buf_len, pl_type);
+	padded_len = ALIGN(buf_len, I2400M_PL_PAD);
+	d_printf(5, dev, "padded_len %zd buf_len %zd\n", padded_len, buf_len);
+	/* If there is no current TX message, create one; if the
+	 * current one is out of payload slots or we have a singleton,
+	 * close it and start a new one */
+	spin_lock_irqsave(&i2400m->tx_lock, flags);
+try_new:
+	if (unlikely(i2400m->tx_msg == NULL))
+		i2400m_tx_new(i2400m);
+	else if (unlikely(!i2400m_tx_fits(i2400m)
+			  || (is_singleton && i2400m->tx_msg->num_pls != 0))) {
+		d_printf(2, dev, "closing TX message (fits %u singleton "
+			 "%u num_pls %u)\n", i2400m_tx_fits(i2400m),
+			 is_singleton, i2400m->tx_msg->num_pls);
+		i2400m_tx_close(i2400m);
+		i2400m_tx_new(i2400m);
+	}
+	if (i2400m->tx_msg->size + padded_len > I2400M_TX_BUF_SIZE / 2) {
+		d_printf(2, dev, "TX: message too big, going new\n");
+		i2400m_tx_close(i2400m);
+		i2400m_tx_new(i2400m);
+	}
+	if (i2400m->tx_msg == NULL)
+		goto error_tx_new;
+	/* So we have a current message header; now append space for
+	 * the message -- if there is not enough, try the head */
+	ptr = i2400m_tx_fifo_push(i2400m, padded_len,
+				  i2400m->bus_tx_block_size);
+	if (ptr == TAIL_FULL) {	/* Tail is full, try head */
+		d_printf(2, dev, "pl append: tail full\n");
+		i2400m_tx_close(i2400m);
+		i2400m_tx_skip_tail(i2400m);
+		goto try_new;
+	} else if (ptr == NULL) {	/* All full */
+		result = -ENOSPC;
+		d_printf(2, dev, "pl append: all full\n");
+	} else {			/* Got space, copy it, set padding */
+		struct i2400m_msg_hdr *tx_msg = i2400m->tx_msg;
+		unsigned num_pls = le16_to_cpu(tx_msg->num_pls);
+		memcpy(ptr, buf, buf_len);
+		memset(ptr + buf_len, 0xad, padded_len - buf_len);
+		i2400m_pld_set(&tx_msg->pld[num_pls], buf_len, pl_type);
+		d_printf(3, dev, "pld 0x%08x (type 0x%1x len 0x%04zx\n",
+			 le32_to_cpu(tx_msg->pld[num_pls].val),
+			 pl_type, buf_len);
+		tx_msg->num_pls = le16_to_cpu(num_pls+1);
+		tx_msg->size += padded_len;
+		d_printf(2, dev, "TX: appended %zu b (up to %u b) pl #%u \n",
+			padded_len, tx_msg->size, num_pls+1);
+		d_printf(2, dev,
+			 "TX: appended hdr @%zu %zu b pl #%u @%zu %zu/%zu b\n",
+			 (void *)tx_msg - i2400m->tx_buf, (size_t)tx_msg->size,
+			 num_pls+1, ptr - i2400m->tx_buf, buf_len, padded_len);
+		result = 0;
+		if (is_singleton)
+			i2400m_tx_close(i2400m);
+	}
+error_tx_new:
+	spin_unlock_irqrestore(&i2400m->tx_lock, flags);
+	i2400m->bus_tx_kick(i2400m);	/* always kick, might free up space */
+	d_fnend(3, dev, "(i2400m %p skb %p [%zu bytes] pt %u) = %d\n",
+		i2400m, buf, buf_len, pl_type, result);
+	return result;
+}
+EXPORT_SYMBOL_GPL(i2400m_tx);
+
+
+/**
+ * i2400m_tx_msg_get - Get the first TX message in the FIFO to start sending it
+ *
+ * @i2400m: device descriptors
+ * @bus_size: where to place the size of the TX message
+ *
+ * Called by the bus-specific driver to get the first TX message at
+ * the FIF that is ready for transmission.
+ *
+ * It sets the state in @i2400m to indicate the bus-specific driver is
+ * transfering that message (i2400m->tx_msg_size).
+ *
+ * Once the transfer is completed, call i2400m_tx_msg_sent().
+ *
+ * Notes:
+ *
+ *     The size of the TX message to be transmitted might be smaller than
+ *     that of the TX message in the FIFO (in case the header was
+ *     shorter). Hence, we copy it in @bus_size, for the bus layer to
+ *     use. We keep the message's size in i2400m->tx_msg_size so that
+ *     when the bus later is done transferring we know how much to
+ *     advance the fifo.
+ *
+ *     We collect statistics here as all the data is available and we
+ *     assume it is going to work [see i2400m_tx_msg_sent()].
+ */
+struct i2400m_msg_hdr *i2400m_tx_msg_get(struct i2400m *i2400m,
+					 size_t *bus_size)
+{
+	struct device *dev = i2400m_dev(i2400m);
+	struct i2400m_msg_hdr *tx_msg, *tx_msg_moved;
+	unsigned long flags, pls;
+
+	d_fnstart(3, dev, "(i2400m %p bus_size %p)\n", i2400m, bus_size);
+	spin_lock_irqsave(&i2400m->tx_lock, flags);
+skip:
+	tx_msg_moved = NULL;
+	if (i2400m->tx_in == i2400m->tx_out) {	/* Empty FIFO? */
+		i2400m->tx_in = 0;
+		i2400m->tx_out = 0;
+		d_printf(2, dev, "TX: FIFO empty: resetting\n");
+		goto out_unlock;
+	}
+	tx_msg = i2400m->tx_buf + i2400m->tx_out % I2400M_TX_BUF_SIZE;
+	if (tx_msg->size & I2400M_TX_SKIP) {	/* skip? */
+		d_printf(2, dev, "TX: skip: msg @%zu (%zu b)\n",
+			 i2400m->tx_out % I2400M_TX_BUF_SIZE,
+			 (size_t) tx_msg->size & ~I2400M_TX_SKIP);
+		i2400m->tx_out += tx_msg->size & ~I2400M_TX_SKIP;
+		goto skip;
+	}
+
+	if (tx_msg->num_pls == 0) {		/* No payloads? */
+		if (tx_msg == i2400m->tx_msg) {	/* open, we are done */
+			d_printf(2, dev,
+				 "TX: FIFO empty: open msg w/o payloads @%zu\n",
+				 (void *) tx_msg - i2400m->tx_buf);
+			tx_msg = NULL;
+			goto out_unlock;
+		} else {			/* closed, skip it */
+			d_printf(2, dev,
+				 "TX: skip msg w/o payloads @%zu (%zu b)\n",
+				 (void *) tx_msg - i2400m->tx_buf,
+				 (size_t) tx_msg->size);
+			i2400m->tx_out += tx_msg->size & ~I2400M_TX_SKIP;
+			goto skip;
+		}
+	}
+	if (tx_msg == i2400m->tx_msg)		/* open msg? */
+		i2400m_tx_close(i2400m);
+
+	/* Now we have a valid TX message (with payloads) to TX */
+	tx_msg_moved = (void *) tx_msg + tx_msg->offset;
+	i2400m->tx_msg_size = tx_msg->size;
+	*bus_size = tx_msg_moved->size;
+	d_printf(2, dev, "TX: pid %d msg hdr at @%zu offset +@%zu "
+		 "size %zu bus_size %zu\n",
+		 current->pid, (void *) tx_msg - i2400m->tx_buf,
+		 (size_t) tx_msg->offset, (size_t) tx_msg->size,
+		 (size_t) tx_msg_moved->size);
+	tx_msg_moved->barker = le32_to_cpu(I2400M_H2D_PREVIEW_BARKER);
+	tx_msg_moved->sequence = le32_to_cpu(i2400m->tx_sequence++);
+
+	pls = le32_to_cpu(tx_msg_moved->num_pls);
+	i2400m->tx_pl_num += pls;		/* Update stats */
+	if (pls > i2400m->tx_pl_max)
+		i2400m->tx_pl_max = pls;
+	if (pls < i2400m->tx_pl_min)
+		i2400m->tx_pl_min = pls;
+	i2400m->tx_num++;
+	i2400m->tx_size_acc += *bus_size;
+	if (*bus_size < i2400m->tx_size_min)
+		i2400m->tx_size_min = *bus_size;
+	if (*bus_size > i2400m->tx_size_max)
+		i2400m->tx_size_max = *bus_size;
+out_unlock:
+	spin_unlock_irqrestore(&i2400m->tx_lock, flags);
+	d_fnstart(3, dev, "(i2400m %p bus_size %p [%zu]) = %p\n",
+		  i2400m, bus_size, *bus_size, tx_msg_moved);
+	return tx_msg_moved;
+}
+EXPORT_SYMBOL_GPL(i2400m_tx_msg_get);
+
+
+/**
+ * i2400m_tx_msg_sent - indicate the transmission of a TX message
+ *
+ * @i2400m: device descriptor
+ *
+ * Called by the bus-specific driver when a message has been sent;
+ * this pops it from the FIFO; and as there is space, start the queue
+ * in case it was stopped.
+ *
+ * Should be called even if the message send failed and we are
+ * dropping this TX message.
+ */
+void i2400m_tx_msg_sent(struct i2400m *i2400m)
+{
+	unsigned n;
+	unsigned long flags;
+	struct device *dev = i2400m_dev(i2400m);
+
+	d_fnstart(3, dev, "(i2400m %p)\n", i2400m);
+	spin_lock_irqsave(&i2400m->tx_lock, flags);
+	i2400m->tx_out += i2400m->tx_msg_size;
+	d_printf(2, dev, "TX: sent %zu b\n", (size_t) i2400m->tx_msg_size);
+	i2400m->tx_msg_size = 0;
+	BUG_ON(i2400m->tx_out > i2400m->tx_in);
+	/* level them FIFO markers off */
+	n = i2400m->tx_out / I2400M_TX_BUF_SIZE;
+	i2400m->tx_out %= I2400M_TX_BUF_SIZE;
+	i2400m->tx_in -= n * I2400M_TX_BUF_SIZE;
+	netif_start_queue(i2400m->wimax_dev.net_dev);
+	spin_unlock_irqrestore(&i2400m->tx_lock, flags);
+	d_fnend(3, dev, "(i2400m %p) = void\n", i2400m);
+}
+EXPORT_SYMBOL_GPL(i2400m_tx_msg_sent);
+
+
+/**
+ * i2400m_tx_setup - Initialize the TX queue and infrastructure
+ *
+ * Make sure we reset the TX sequence to zero, as when this function
+ * is called, the firmware has been just restarted.
+ */
+int i2400m_tx_setup(struct i2400m *i2400m)
+{
+	int result;
+
+	/* Do this here only once -- can't do on
+	 * i2400m_hard_start_xmit() as we'll cause race conditions if
+	 * the WS was scheduled on another CPU */
+	INIT_WORK(&i2400m->wake_tx_ws, i2400m_wake_tx_work);
+
+	i2400m->tx_sequence = 0;
+	i2400m->tx_buf = kmalloc(I2400M_TX_BUF_SIZE, GFP_KERNEL);
+	if (i2400m->tx_buf == NULL)
+		result = -ENOMEM;
+	else
+		result = 0;
+	/* Huh? the bus layer has to define this... */
+	BUG_ON(i2400m->bus_tx_block_size == 0);
+	return result;
+
+}
+
+
+/**
+ * i2400m_tx_release - Tear down the TX queue and infrastructure
+ */
+void i2400m_tx_release(struct i2400m *i2400m)
+{
+	kfree(i2400m->tx_buf);
+}
diff --git a/drivers/net/wimax/i2400m/usb-debug-levels.h b/drivers/net/wimax/i2400m/usb-debug-levels.h
new file mode 100644
index 000000000000..e4358bd880be
--- /dev/null
+++ b/drivers/net/wimax/i2400m/usb-debug-levels.h
@@ -0,0 +1,42 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * Debug levels control file for the i2400m-usb module
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+#ifndef __debug_levels__h__
+#define __debug_levels__h__
+
+/* Maximum compile and run time debug level for all submodules */
+#define D_MODULENAME i2400m_usb
+#define D_MASTER CONFIG_WIMAX_I2400M_DEBUG_LEVEL
+
+#include <linux/wimax/debug.h>
+
+/* List of all the enabled modules */
+enum d_module {
+	D_SUBMODULE_DECLARE(usb),
+	D_SUBMODULE_DECLARE(fw),
+	D_SUBMODULE_DECLARE(notif),
+	D_SUBMODULE_DECLARE(rx),
+	D_SUBMODULE_DECLARE(tx),
+};
+
+
+#endif /* #ifndef __debug_levels__h__ */
diff --git a/drivers/net/wimax/i2400m/usb-fw.c b/drivers/net/wimax/i2400m/usb-fw.c
new file mode 100644
index 000000000000..5ad287c228b8
--- /dev/null
+++ b/drivers/net/wimax/i2400m/usb-fw.c
@@ -0,0 +1,340 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * Firmware uploader's USB specifics
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Yanir Lubetkin <yanirx.lubetkin@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ *  - Initial implementation
+ *
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ *  - bus generic/specific split
+ *
+ * THE PROCEDURE
+ *
+ * See fw.c for the generic description of this procedure.
+ *
+ * This file implements only the USB specifics. It boils down to how
+ * to send a command and waiting for an acknowledgement from the
+ * device.
+ *
+ * This code (and process) is single threaded. It assumes it is the
+ * only thread poking around (guaranteed by fw.c).
+ *
+ * COMMAND EXECUTION
+ *
+ * A write URB is posted with the buffer to the bulk output endpoint.
+ *
+ * ACK RECEPTION
+ *
+ * We just post a URB to the notification endpoint and wait for
+ * data. We repeat until we get all the data we expect (as indicated
+ * by the call from the bus generic code).
+ *
+ * The data is not read from the bulk in endpoint for boot mode.
+ *
+ * ROADMAP
+ *
+ * i2400mu_bus_bm_cmd_send
+ *   i2400m_bm_cmd_prepare...
+ *   i2400mu_tx_bulk_out
+ *
+ * i2400mu_bus_bm_wait_for_ack
+ *   i2400m_notif_submit
+ */
+#include <linux/usb.h>
+#include "i2400m-usb.h"
+
+
+#define D_SUBMODULE fw
+#include "usb-debug-levels.h"
+
+
+/*
+ * Synchronous write to the device
+ *
+ * Takes care of updating EDC counts and thus, handle device errors.
+ */
+static
+ssize_t i2400mu_tx_bulk_out(struct i2400mu *i2400mu, void *buf, size_t buf_size)
+{
+	int result;
+	struct device *dev = &i2400mu->usb_iface->dev;
+	int len;
+	struct usb_endpoint_descriptor *epd;
+	int pipe, do_autopm = 1;
+
+	result = usb_autopm_get_interface(i2400mu->usb_iface);
+	if (result < 0) {
+		dev_err(dev, "BM-CMD: can't get autopm: %d\n", result);
+		do_autopm = 0;
+	}
+	epd = usb_get_epd(i2400mu->usb_iface, I2400MU_EP_BULK_OUT);
+	pipe = usb_sndbulkpipe(i2400mu->usb_dev, epd->bEndpointAddress);
+retry:
+	result = usb_bulk_msg(i2400mu->usb_dev, pipe, buf, buf_size, &len, HZ);
+	switch (result) {
+	case 0:
+		if (len != buf_size) {
+			dev_err(dev, "BM-CMD: short write (%u B vs %zu "
+				"expected)\n", len, buf_size);
+			result = -EIO;
+			break;
+		}
+		result = len;
+		break;
+	case -EINVAL:			/* while removing driver */
+	case -ENODEV:			/* dev disconnect ... */
+	case -ENOENT:			/* just ignore it */
+	case -ESHUTDOWN:		/* and exit */
+	case -ECONNRESET:
+		result = -ESHUTDOWN;
+		break;
+	case -ETIMEDOUT:			/* bah... */
+		break;
+	default:				/* any other? */
+		if (edc_inc(&i2400mu->urb_edc,
+			    EDC_MAX_ERRORS, EDC_ERROR_TIMEFRAME)) {
+				dev_err(dev, "BM-CMD: maximum errors in "
+					"URB exceeded; resetting device\n");
+				usb_queue_reset_device(i2400mu->usb_iface);
+				result = -ENODEV;
+				break;
+		}
+		dev_err(dev, "BM-CMD: URB error %d, retrying\n",
+			result);
+		goto retry;
+	}
+	result = len;
+	if (do_autopm)
+		usb_autopm_put_interface(i2400mu->usb_iface);
+	return result;
+}
+
+
+/*
+ * Send a boot-mode command over the bulk-out pipe
+ *
+ * Command can be a raw command, which requires no preparation (and
+ * which might not even be following the command format). Checks that
+ * the right amount of data was transfered.
+ *
+ * To satisfy USB requirements (no onstack, vmalloc or in data segment
+ * buffers), we copy the command to i2400m->bm_cmd_buf and send it from
+ * there.
+ *
+ * @flags: pass thru from i2400m_bm_cmd()
+ * @return: cmd_size if ok, < 0 errno code on error.
+ */
+ssize_t i2400mu_bus_bm_cmd_send(struct i2400m *i2400m,
+				const struct i2400m_bootrom_header *_cmd,
+				size_t cmd_size, int flags)
+{
+	ssize_t result;
+	struct device *dev = i2400m_dev(i2400m);
+	struct i2400mu *i2400mu = container_of(i2400m, struct i2400mu, i2400m);
+	int opcode = _cmd == NULL ? -1 : i2400m_brh_get_opcode(_cmd);
+	struct i2400m_bootrom_header *cmd;
+	size_t cmd_size_a = ALIGN(cmd_size, 16);	/* USB restriction */
+
+	d_fnstart(8, dev, "(i2400m %p cmd %p size %zu)\n",
+		  i2400m, _cmd, cmd_size);
+	result = -E2BIG;
+	if (cmd_size > I2400M_BM_CMD_BUF_SIZE)
+		goto error_too_big;
+	memcpy(i2400m->bm_cmd_buf, _cmd, cmd_size);
+	cmd = i2400m->bm_cmd_buf;
+	if (cmd_size_a > cmd_size)			/* Zero pad space */
+		memset(i2400m->bm_cmd_buf + cmd_size, 0, cmd_size_a - cmd_size);
+	if ((flags & I2400M_BM_CMD_RAW) == 0) {
+		if (WARN_ON(i2400m_brh_get_response_required(cmd) == 0))
+			dev_warn(dev, "SW BUG: response_required == 0\n");
+		i2400m_bm_cmd_prepare(cmd);
+	}
+	result = i2400mu_tx_bulk_out(i2400mu, i2400m->bm_cmd_buf, cmd_size);
+	if (result < 0) {
+		dev_err(dev, "boot-mode cmd %d: cannot send: %zd\n",
+			opcode, result);
+		goto error_cmd_send;
+	}
+	if (result != cmd_size) {		/* all was transferred? */
+		dev_err(dev, "boot-mode cmd %d: incomplete transfer "
+			"(%zu vs %zu submitted)\n",  opcode, result, cmd_size);
+		result = -EIO;
+		goto error_cmd_size;
+	}
+error_cmd_size:
+error_cmd_send:
+error_too_big:
+	d_fnend(8, dev, "(i2400m %p cmd %p size %zu) = %zd\n",
+		i2400m, _cmd, cmd_size, result);
+	return result;
+}
+
+
+static
+void __i2400mu_bm_notif_cb(struct urb *urb)
+{
+	complete(urb->context);
+}
+
+
+/*
+ * submit a read to the notification endpoint
+ *
+ * @i2400m: device descriptor
+ * @urb: urb to use
+ * @completion: completion varible to complete when done
+ *
+ * Data is always read to i2400m->bm_ack_buf
+ */
+static
+int i2400mu_notif_submit(struct i2400mu *i2400mu, struct urb *urb,
+			 struct completion *completion)
+{
+	struct i2400m *i2400m = &i2400mu->i2400m;
+	struct usb_endpoint_descriptor *epd;
+	int pipe;
+
+	epd = usb_get_epd(i2400mu->usb_iface, I2400MU_EP_NOTIFICATION);
+	pipe = usb_rcvintpipe(i2400mu->usb_dev, epd->bEndpointAddress);
+	usb_fill_int_urb(urb, i2400mu->usb_dev, pipe,
+			 i2400m->bm_ack_buf, I2400M_BM_ACK_BUF_SIZE,
+			 __i2400mu_bm_notif_cb, completion,
+			 epd->bInterval);
+	return usb_submit_urb(urb, GFP_KERNEL);
+}
+
+
+/*
+ * Read an ack from  the notification endpoint
+ *
+ * @i2400m:
+ * @_ack: pointer to where to store the read data
+ * @ack_size: how many bytes we should read
+ *
+ * Returns: < 0 errno code on error; otherwise, amount of received bytes.
+ *
+ * Submits a notification read, appends the read data to the given ack
+ * buffer and then repeats (until @ack_size bytes have been
+ * received).
+ */
+ssize_t i2400mu_bus_bm_wait_for_ack(struct i2400m *i2400m,
+				    struct i2400m_bootrom_header *_ack,
+				    size_t ack_size)
+{
+	ssize_t result = -ENOMEM;
+	struct device *dev = i2400m_dev(i2400m);
+	struct i2400mu *i2400mu = container_of(i2400m, struct i2400mu, i2400m);
+	struct urb notif_urb;
+	void *ack = _ack;
+	size_t offset, len;
+	long val;
+	int do_autopm = 1;
+	DECLARE_COMPLETION_ONSTACK(notif_completion);
+
+	d_fnstart(8, dev, "(i2400m %p ack %p size %zu)\n",
+		  i2400m, ack, ack_size);
+	BUG_ON(_ack == i2400m->bm_ack_buf);
+	result = usb_autopm_get_interface(i2400mu->usb_iface);
+	if (result < 0) {
+		dev_err(dev, "BM-ACK: can't get autopm: %d\n", (int) result);
+		do_autopm = 0;
+	}
+	usb_init_urb(&notif_urb);	/* ready notifications */
+	usb_get_urb(&notif_urb);
+	offset = 0;
+	while (offset < ack_size) {
+		init_completion(&notif_completion);
+		result = i2400mu_notif_submit(i2400mu, &notif_urb,
+					      &notif_completion);
+		if (result < 0)
+			goto error_notif_urb_submit;
+		val = wait_for_completion_interruptible_timeout(
+			&notif_completion, HZ);
+		if (val == 0) {
+			result = -ETIMEDOUT;
+			usb_kill_urb(&notif_urb);	/* Timedout */
+			goto error_notif_wait;
+		}
+		if (val == -ERESTARTSYS) {
+			result = -EINTR;		/* Interrupted */
+			usb_kill_urb(&notif_urb);
+			goto error_notif_wait;
+		}
+		result = notif_urb.status;		/* How was the ack? */
+		switch (result) {
+		case 0:
+			break;
+		case -EINVAL:			/* while removing driver */
+		case -ENODEV:			/* dev disconnect ... */
+		case -ENOENT:			/* just ignore it */
+		case -ESHUTDOWN:		/* and exit */
+		case -ECONNRESET:
+			result = -ESHUTDOWN;
+			goto error_dev_gone;
+		default:				/* any other? */
+			usb_kill_urb(&notif_urb);	/* Timedout */
+			if (edc_inc(&i2400mu->urb_edc,
+				    EDC_MAX_ERRORS, EDC_ERROR_TIMEFRAME))
+				goto error_exceeded;
+			dev_err(dev, "BM-ACK: URB error %d, "
+				"retrying\n", notif_urb.status);
+			continue;	/* retry */
+		}
+		if (notif_urb.actual_length == 0) {
+			d_printf(6, dev, "ZLP received, retrying\n");
+			continue;
+		}
+		/* Got data, append it to the buffer */
+		len = min(ack_size - offset, (size_t) notif_urb.actual_length);
+		memcpy(ack + offset, i2400m->bm_ack_buf, len);
+		offset += len;
+	}
+	result = offset;
+error_notif_urb_submit:
+error_notif_wait:
+error_dev_gone:
+out:
+	if (do_autopm)
+		usb_autopm_put_interface(i2400mu->usb_iface);
+	d_fnend(8, dev, "(i2400m %p ack %p size %zu) = %zd\n",
+		i2400m, ack, ack_size, result);
+	return result;
+
+error_exceeded:
+	dev_err(dev, "bm: maximum errors in notification URB exceeded; "
+		"resetting device\n");
+	usb_queue_reset_device(i2400mu->usb_iface);
+	goto out;
+}
diff --git a/drivers/net/wimax/i2400m/usb-notif.c b/drivers/net/wimax/i2400m/usb-notif.c
new file mode 100644
index 000000000000..9702c22b2497
--- /dev/null
+++ b/drivers/net/wimax/i2400m/usb-notif.c
@@ -0,0 +1,269 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m over USB
+ * Notification handling
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Yanir Lubetkin <yanirx.lubetkin@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ *  - Initial implementation
+ *
+ *
+ * The notification endpoint is active when the device is not in boot
+ * mode; in here we just read and get notifications; based on those,
+ * we act to either reinitialize the device after a reboot or to
+ * submit a RX request.
+ *
+ * ROADMAP
+ *
+ * i2400mu_usb_notification_setup()
+ *
+ * i2400mu_usb_notification_release()
+ *
+ * i2400mu_usb_notification_cb()	Called when a URB is ready
+ *   i2400mu_notif_grok()
+ *     i2400m_dev_reset_handle()
+ *     i2400mu_rx_kick()
+ */
+#include <linux/usb.h>
+#include "i2400m-usb.h"
+
+
+#define D_SUBMODULE notif
+#include "usb-debug-levels.h"
+
+
+static const
+__le32 i2400m_ZERO_BARKER[4] = { 0, 0, 0, 0 };
+
+
+/*
+ * Process a received notification
+ *
+ * In normal operation mode, we can only receive two types of payloads
+ * on the notification endpoint:
+ *
+ *   - a reboot barker, we do a bootstrap (the device has reseted).
+ *
+ *   - a block of zeroes: there is pending data in the IN endpoint
+ */
+static
+int i2400mu_notification_grok(struct i2400mu *i2400mu, const void *buf,
+				 size_t buf_len)
+{
+	int ret;
+	struct device *dev = &i2400mu->usb_iface->dev;
+	struct i2400m *i2400m = &i2400mu->i2400m;
+
+	d_fnstart(4, dev, "(i2400m %p buf %p buf_len %zu)\n",
+		  i2400mu, buf, buf_len);
+	ret = -EIO;
+	if (buf_len < sizeof(i2400m_NBOOT_BARKER))
+		/* Not a bug, just ignore */
+		goto error_bad_size;
+	if (!memcmp(i2400m_NBOOT_BARKER, buf, sizeof(i2400m_NBOOT_BARKER))
+	    || !memcmp(i2400m_SBOOT_BARKER, buf, sizeof(i2400m_SBOOT_BARKER)))
+		ret = i2400m_dev_reset_handle(i2400m);
+	else if (!memcmp(i2400m_ZERO_BARKER, buf, sizeof(i2400m_ZERO_BARKER))) {
+		i2400mu_rx_kick(i2400mu);
+		ret = 0;
+	} else {	/* Unknown or unexpected data in the notif message */
+		char prefix[64];
+		ret = -EIO;
+		dev_err(dev, "HW BUG? Unknown/unexpected data in notification "
+			"message (%zu bytes)\n", buf_len);
+		snprintf(prefix, sizeof(prefix), "%s %s: ",
+			 dev_driver_string(dev) , dev->bus_id);
+		if (buf_len > 64) {
+			print_hex_dump(KERN_ERR, prefix, DUMP_PREFIX_OFFSET,
+				       8, 4, buf, 64, 0);
+			printk(KERN_ERR "%s... (only first 64 bytes "
+			       "dumped)\n", prefix);
+		} else
+			print_hex_dump(KERN_ERR, prefix, DUMP_PREFIX_OFFSET,
+				       8, 4, buf, buf_len, 0);
+	}
+error_bad_size:
+	d_fnend(4, dev, "(i2400m %p buf %p buf_len %zu) = %d\n",
+		i2400mu, buf, buf_len, ret);
+	return ret;
+}
+
+
+/*
+ * URB callback for the notification endpoint
+ *
+ * @urb: the urb received from the notification endpoint
+ *
+ * This function will just process the USB side of the transaction,
+ * checking everything is fine, pass the processing to
+ * i2400m_notification_grok() and resubmit the URB.
+ */
+static
+void i2400mu_notification_cb(struct urb *urb)
+{
+	int ret;
+	struct i2400mu *i2400mu = urb->context;
+	struct device *dev = &i2400mu->usb_iface->dev;
+
+	d_fnstart(4, dev, "(urb %p status %d actual_length %d)\n",
+		  urb, urb->status, urb->actual_length);
+	ret = urb->status;
+	switch (ret) {
+	case 0:
+		ret = i2400mu_notification_grok(i2400mu, urb->transfer_buffer,
+						urb->actual_length);
+		if (ret == -EIO && edc_inc(&i2400mu->urb_edc, EDC_MAX_ERRORS,
+					   EDC_ERROR_TIMEFRAME))
+			goto error_exceeded;
+		if (ret == -ENOMEM)	/* uff...power cycle? shutdown? */
+			goto error_exceeded;
+		break;
+	case -EINVAL:			/* while removing driver */
+	case -ENODEV:			/* dev disconnect ... */
+	case -ENOENT:			/* ditto */
+	case -ESHUTDOWN:		/* URB killed */
+	case -ECONNRESET:		/* disconnection */
+		goto out;		/* Notify around */
+	default:			/* Some error? */
+		if (edc_inc(&i2400mu->urb_edc,
+			    EDC_MAX_ERRORS, EDC_ERROR_TIMEFRAME))
+			goto error_exceeded;
+		dev_err(dev, "notification: URB error %d, retrying\n",
+			urb->status);
+	}
+	usb_mark_last_busy(i2400mu->usb_dev);
+	ret = usb_submit_urb(i2400mu->notif_urb, GFP_ATOMIC);
+	switch (ret) {
+	case 0:
+	case -EINVAL:			/* while removing driver */
+	case -ENODEV:			/* dev disconnect ... */
+	case -ENOENT:			/* ditto */
+	case -ESHUTDOWN:		/* URB killed */
+	case -ECONNRESET:		/* disconnection */
+		break;			/* just ignore */
+	default:			/* Some error? */
+		dev_err(dev, "notification: cannot submit URB: %d\n", ret);
+		goto error_submit;
+	}
+	d_fnend(4, dev, "(urb %p status %d actual_length %d) = void\n",
+		urb, urb->status, urb->actual_length);
+	return;
+
+error_exceeded:
+	dev_err(dev, "maximum errors in notification URB exceeded; "
+		"resetting device\n");
+error_submit:
+	usb_queue_reset_device(i2400mu->usb_iface);
+out:
+	d_fnend(4, dev, "(urb %p status %d actual_length %d) = void\n",
+		urb, urb->status, urb->actual_length);
+	return;
+}
+
+
+/*
+ * setup the notification endpoint
+ *
+ * @i2400m: device descriptor
+ *
+ * This procedure prepares the notification urb and handler for receiving
+ * unsolicited barkers from the device.
+ */
+int i2400mu_notification_setup(struct i2400mu *i2400mu)
+{
+	struct device *dev = &i2400mu->usb_iface->dev;
+	int usb_pipe, ret = 0;
+	struct usb_endpoint_descriptor *epd;
+	char *buf;
+
+	d_fnstart(4, dev, "(i2400m %p)\n", i2400mu);
+	buf = kmalloc(I2400MU_MAX_NOTIFICATION_LEN, GFP_KERNEL | GFP_DMA);
+	if (buf == NULL) {
+		dev_err(dev, "notification: buffer allocation failed\n");
+		ret = -ENOMEM;
+		goto error_buf_alloc;
+	}
+
+	i2400mu->notif_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!i2400mu->notif_urb) {
+		ret = -ENOMEM;
+		dev_err(dev, "notification: cannot allocate URB\n");
+		goto error_alloc_urb;
+	}
+	epd = usb_get_epd(i2400mu->usb_iface, I2400MU_EP_NOTIFICATION);
+	usb_pipe = usb_rcvintpipe(i2400mu->usb_dev, epd->bEndpointAddress);
+	usb_fill_int_urb(i2400mu->notif_urb, i2400mu->usb_dev, usb_pipe,
+			 buf, I2400MU_MAX_NOTIFICATION_LEN,
+			 i2400mu_notification_cb, i2400mu, epd->bInterval);
+	ret = usb_submit_urb(i2400mu->notif_urb, GFP_KERNEL);
+	if (ret != 0) {
+		dev_err(dev, "notification: cannot submit URB: %d\n", ret);
+		goto error_submit;
+	}
+	d_fnend(4, dev, "(i2400m %p) = %d\n", i2400mu, ret);
+	return ret;
+
+error_submit:
+	usb_free_urb(i2400mu->notif_urb);
+error_alloc_urb:
+	kfree(buf);
+error_buf_alloc:
+	d_fnend(4, dev, "(i2400m %p) = %d\n", i2400mu, ret);
+	return ret;
+}
+
+
+/*
+ * Tear down of the notification mechanism
+ *
+ * @i2400m: device descriptor
+ *
+ * Kill the interrupt endpoint urb, free any allocated resources.
+ *
+ * We need to check if we have done it before as for example,
+ * _suspend() call this; if after a suspend() we get a _disconnect()
+ * (as the case is when hibernating), nothing bad happens.
+ */
+void i2400mu_notification_release(struct i2400mu *i2400mu)
+{
+	struct device *dev = &i2400mu->usb_iface->dev;
+
+	d_fnstart(4, dev, "(i2400mu %p)\n", i2400mu);
+	if (i2400mu->notif_urb != NULL) {
+		usb_kill_urb(i2400mu->notif_urb);
+		kfree(i2400mu->notif_urb->transfer_buffer);
+		usb_free_urb(i2400mu->notif_urb);
+		i2400mu->notif_urb = NULL;
+	}
+	d_fnend(4, dev, "(i2400mu %p)\n", i2400mu);
+}
diff --git a/drivers/net/wimax/i2400m/usb-rx.c b/drivers/net/wimax/i2400m/usb-rx.c
new file mode 100644
index 000000000000..074cc1f89853
--- /dev/null
+++ b/drivers/net/wimax/i2400m/usb-rx.c
@@ -0,0 +1,417 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * USB RX handling
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Yanir Lubetkin <yanirx.lubetkin@intel.com>
+ *  - Initial implementation
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ *  - Use skb_clone(), break up processing in chunks
+ *  - Split transport/device specific
+ *  - Make buffer size dynamic to exert less memory pressure
+ *
+ *
+ * This handles the RX path on USB.
+ *
+ * When a notification is received that says 'there is RX data ready',
+ * we call i2400mu_rx_kick(); that wakes up the RX kthread, which
+ * reads a buffer from USB and passes it to i2400m_rx() in the generic
+ * handling code. The RX buffer has an specific format that is
+ * described in rx.c.
+ *
+ * We use a kernel thread in a loop because:
+ *
+ *  - we want to be able to call the USB power management get/put
+ *    functions (blocking) before each transaction.
+ *
+ *  - We might get a lot of notifications and we don't want to submit
+ *    a zillion reads; by serializing, we are throttling.
+ *
+ *  - RX data processing can get heavy enough so that it is not
+ *    appropiate for doing it in the USB callback; thus we run it in a
+ *    process context.
+ *
+ * We provide a read buffer of an arbitrary size (short of a page); if
+ * the callback reports -EOVERFLOW, it means it was too small, so we
+ * just double the size and retry (being careful to append, as
+ * sometimes the device provided some data). Every now and then we
+ * check if the average packet size is smaller than the current packet
+ * size and if so, we halve it. At the end, the size of the
+ * preallocated buffer should be following the average received
+ * transaction size, adapting dynamically to it.
+ *
+ * ROADMAP
+ *
+ * i2400mu_rx_kick()		   Called from notif.c when we get a
+ *   			           'data ready' notification
+ * i2400mu_rxd()                   Kernel RX daemon
+ *   i2400mu_rx()                  Receive USB data
+ *   i2400m_rx()                   Send data to generic i2400m RX handling
+ *
+ * i2400mu_rx_setup()              called from i2400mu_bus_dev_start()
+ *
+ * i2400mu_rx_release()            called from i2400mu_bus_dev_stop()
+ */
+#include <linux/workqueue.h>
+#include <linux/usb.h>
+#include "i2400m-usb.h"
+
+
+#define D_SUBMODULE rx
+#include "usb-debug-levels.h"
+
+/*
+ * Dynamic RX size
+ *
+ * We can't let the rx_size be a multiple of 512 bytes (the RX
+ * endpoint's max packet size). On some USB host controllers (we
+ * haven't been able to fully characterize which), if the device is
+ * about to send (for example) X bytes and we only post a buffer to
+ * receive n*512, it will fail to mark that as babble (so that
+ * i2400mu_rx() [case -EOVERFLOW] can resize the buffer and get the
+ * rest).
+ *
+ * So on growing or shrinking, if it is a multiple of the
+ * maxpacketsize, we remove some (instead of incresing some, so in a
+ * buddy allocator we try to waste less space).
+ *
+ * Note we also need a hook for this on i2400mu_rx() -- when we do the
+ * first read, we are sure we won't hit this spot because
+ * i240mm->rx_size has been set properly. However, if we have to
+ * double because of -EOVERFLOW, when we launch the read to get the
+ * rest of the data, we *have* to make sure that also is not a
+ * multiple of the max_pkt_size.
+ */
+
+static
+size_t i2400mu_rx_size_grow(struct i2400mu *i2400mu)
+{
+	struct device *dev = &i2400mu->usb_iface->dev;
+	size_t rx_size;
+	const size_t max_pkt_size = 512;
+
+	rx_size = 2 * i2400mu->rx_size;
+	if (rx_size % max_pkt_size == 0) {
+		rx_size -= 8;
+		d_printf(1, dev,
+			 "RX: expected size grew to %zu [adjusted -8] "
+			 "from %zu\n",
+			 rx_size, i2400mu->rx_size);
+	} else
+		d_printf(1, dev,
+			 "RX: expected size grew to %zu from %zu\n",
+			 rx_size, i2400mu->rx_size);
+	return rx_size;
+}
+
+
+static
+void i2400mu_rx_size_maybe_shrink(struct i2400mu *i2400mu)
+{
+	const size_t max_pkt_size = 512;
+	struct device *dev = &i2400mu->usb_iface->dev;
+
+	if (unlikely(i2400mu->rx_size_cnt >= 100
+		     && i2400mu->rx_size_auto_shrink)) {
+		size_t avg_rx_size =
+			i2400mu->rx_size_acc / i2400mu->rx_size_cnt;
+		size_t new_rx_size = i2400mu->rx_size / 2;
+		if (avg_rx_size < new_rx_size) {
+			if (new_rx_size % max_pkt_size == 0) {
+				new_rx_size -= 8;
+				d_printf(1, dev,
+					 "RX: expected size shrank to %zu "
+					 "[adjusted -8] from %zu\n",
+					 new_rx_size, i2400mu->rx_size);
+			} else
+				d_printf(1, dev,
+					 "RX: expected size shrank to %zu "
+					 "from %zu\n",
+					 new_rx_size, i2400mu->rx_size);
+			i2400mu->rx_size = new_rx_size;
+			i2400mu->rx_size_cnt = 0;
+			i2400mu->rx_size_acc = i2400mu->rx_size;
+		}
+	}
+}
+
+/*
+ * Receive a message with payloads from the USB bus into an skb
+ *
+ * @i2400mu: USB device descriptor
+ * @rx_skb: skb where to place the received message
+ *
+ * Deals with all the USB-specifics of receiving, dynamically
+ * increasing the buffer size if so needed. Returns the payload in the
+ * skb, ready to process. On a zero-length packet, we retry.
+ *
+ * On soft USB errors, we retry (until they become too frequent and
+ * then are promoted to hard); on hard USB errors, we reset the
+ * device. On other errors (skb realloacation, we just drop it and
+ * hope for the next invocation to solve it).
+ *
+ * Returns: pointer to the skb if ok, ERR_PTR on error.
+ *   NOTE: this function might realloc the skb (if it is too small),
+ *   so always update with the one returned.
+ *   ERR_PTR() is < 0 on error.
+ */
+static
+struct sk_buff *i2400mu_rx(struct i2400mu *i2400mu, struct sk_buff *rx_skb)
+{
+	int result = 0;
+	struct device *dev = &i2400mu->usb_iface->dev;
+	int usb_pipe, read_size, rx_size, do_autopm;
+	struct usb_endpoint_descriptor *epd;
+	const size_t max_pkt_size = 512;
+
+	d_fnstart(4, dev, "(i2400mu %p)\n", i2400mu);
+	do_autopm = atomic_read(&i2400mu->do_autopm);
+	result = do_autopm ?
+		usb_autopm_get_interface(i2400mu->usb_iface) : 0;
+	if (result < 0) {
+		dev_err(dev, "RX: can't get autopm: %d\n", result);
+		do_autopm = 0;
+	}
+	epd = usb_get_epd(i2400mu->usb_iface, I2400MU_EP_BULK_IN);
+	usb_pipe = usb_rcvbulkpipe(i2400mu->usb_dev, epd->bEndpointAddress);
+retry:
+	rx_size = skb_end_pointer(rx_skb) - rx_skb->data - rx_skb->len;
+	if (unlikely(rx_size % max_pkt_size == 0)) {
+		rx_size -= 8;
+		d_printf(1, dev, "RX: rx_size adapted to %d [-8]\n", rx_size);
+	}
+	result = usb_bulk_msg(
+		i2400mu->usb_dev, usb_pipe, rx_skb->data + rx_skb->len,
+		rx_size, &read_size, HZ);
+	usb_mark_last_busy(i2400mu->usb_dev);
+	switch (result) {
+	case 0:
+		if (read_size == 0)
+			goto retry;	/* ZLP, just resubmit */
+		skb_put(rx_skb, read_size);
+		break;
+	case -EINVAL:			/* while removing driver */
+	case -ENODEV:			/* dev disconnect ... */
+	case -ENOENT:			/* just ignore it */
+	case -ESHUTDOWN:
+	case -ECONNRESET:
+		break;
+	case -EOVERFLOW: {		/* too small, reallocate */
+		struct sk_buff *new_skb;
+		rx_size = i2400mu_rx_size_grow(i2400mu);
+		if (rx_size <= (1 << 16))	/* cap it */
+			i2400mu->rx_size = rx_size;
+		else if (printk_ratelimit()) {
+			dev_err(dev, "BUG? rx_size up to %d\n", rx_size);
+			result = -EINVAL;
+			goto out;
+		}
+		skb_put(rx_skb, read_size);
+		new_skb = skb_copy_expand(rx_skb, 0, rx_size - rx_skb->len,
+					  GFP_KERNEL);
+		if (new_skb == NULL) {
+			if (printk_ratelimit())
+				dev_err(dev, "RX: Can't reallocate skb to %d; "
+					"RX dropped\n", rx_size);
+			kfree(rx_skb);
+			result = 0;
+			goto out;	/* drop it...*/
+		}
+		kfree_skb(rx_skb);
+		rx_skb = new_skb;
+		i2400mu->rx_size_cnt = 0;
+		i2400mu->rx_size_acc = i2400mu->rx_size;
+		d_printf(1, dev, "RX: size changed to %d, received %d, "
+			 "copied %d, capacity %ld\n",
+			 rx_size, read_size, rx_skb->len,
+			 (long) (skb_end_pointer(new_skb) - new_skb->head));
+		goto retry;
+	}
+		/* In most cases, it happens due to the hardware scheduling a
+		 * read when there was no data - unfortunately, we have no way
+		 * to tell this timeout from a USB timeout. So we just ignore
+		 * it. */
+	case -ETIMEDOUT:
+		dev_err(dev, "RX: timeout: %d\n", result);
+		result = 0;
+		break;
+	default:			/* Any error */
+		if (edc_inc(&i2400mu->urb_edc,
+			    EDC_MAX_ERRORS, EDC_ERROR_TIMEFRAME))
+			goto error_reset;
+		dev_err(dev, "RX: error receiving URB: %d, retrying\n", result);
+		goto retry;
+	}
+out:
+	if (do_autopm)
+		usb_autopm_put_interface(i2400mu->usb_iface);
+	d_fnend(4, dev, "(i2400mu %p) = %p\n", i2400mu, rx_skb);
+	return rx_skb;
+
+error_reset:
+	dev_err(dev, "RX: maximum errors in URB exceeded; "
+		"resetting device\n");
+	usb_queue_reset_device(i2400mu->usb_iface);
+	rx_skb = ERR_PTR(result);
+	goto out;
+}
+
+
+/*
+ * Kernel thread for USB reception of data
+ *
+ * This thread waits for a kick; once kicked, it will allocate an skb
+ * and receive a single message to it from USB (using
+ * i2400mu_rx()). Once received, it is passed to the generic i2400m RX
+ * code for processing.
+ *
+ * When done processing, it runs some dirty statistics to verify if
+ * the last 100 messages received were smaller than half of the
+ * current RX buffer size. In that case, the RX buffer size is
+ * halved. This will helps lowering the pressure on the memory
+ * allocator.
+ *
+ * Hard errors force the thread to exit.
+ */
+static
+int i2400mu_rxd(void *_i2400mu)
+{
+	int result = 0;
+	struct i2400mu *i2400mu = _i2400mu;
+	struct i2400m *i2400m = &i2400mu->i2400m;
+	struct device *dev = &i2400mu->usb_iface->dev;
+	struct net_device *net_dev = i2400m->wimax_dev.net_dev;
+	size_t pending;
+	int rx_size;
+	struct sk_buff *rx_skb;
+
+	d_fnstart(4, dev, "(i2400mu %p)\n", i2400mu);
+	while (1) {
+		d_printf(2, dev, "TX: waiting for messages\n");
+		pending = 0;
+		wait_event_interruptible(
+			i2400mu->rx_wq,
+			(kthread_should_stop()	/* check this first! */
+			 || (pending = atomic_read(&i2400mu->rx_pending_count)))
+			);
+		if (kthread_should_stop())
+			break;
+		if (pending == 0)
+			continue;
+		rx_size = i2400mu->rx_size;
+		d_printf(2, dev, "RX: reading up to %d bytes\n", rx_size);
+		rx_skb = __netdev_alloc_skb(net_dev, rx_size, GFP_KERNEL);
+		if (rx_skb == NULL) {
+			dev_err(dev, "RX: can't allocate skb [%d bytes]\n",
+				rx_size);
+			msleep(50);	/* give it some time? */
+			continue;
+		}
+
+		/* Receive the message with the payloads */
+		rx_skb = i2400mu_rx(i2400mu, rx_skb);
+		result = PTR_ERR(rx_skb);
+		if (IS_ERR(rx_skb))
+			goto out;
+		atomic_dec(&i2400mu->rx_pending_count);
+		if (rx_skb->len == 0) {	/* some ignorable condition */
+			kfree_skb(rx_skb);
+			continue;
+		}
+
+		/* Deliver the message to the generic i2400m code */
+		i2400mu->rx_size_cnt++;
+		i2400mu->rx_size_acc += rx_skb->len;
+		result = i2400m_rx(i2400m, rx_skb);
+		if (result == -EIO
+		    && edc_inc(&i2400mu->urb_edc,
+			       EDC_MAX_ERRORS, EDC_ERROR_TIMEFRAME)) {
+			goto error_reset;
+		}
+
+		/* Maybe adjust RX buffer size */
+		i2400mu_rx_size_maybe_shrink(i2400mu);
+	}
+	result = 0;
+out:
+	d_fnend(4, dev, "(i2400mu %p) = %d\n", i2400mu, result);
+	return result;
+
+error_reset:
+	dev_err(dev, "RX: maximum errors in received buffer exceeded; "
+		"resetting device\n");
+	usb_queue_reset_device(i2400mu->usb_iface);
+	goto out;
+}
+
+
+/*
+ * Start reading from the device
+ *
+ * @i2400m: device instance
+ *
+ * Notify the RX thread that there is data pending.
+ */
+void i2400mu_rx_kick(struct i2400mu *i2400mu)
+{
+	struct i2400m *i2400m = &i2400mu->i2400m;
+	struct device *dev = &i2400mu->usb_iface->dev;
+
+	d_fnstart(3, dev, "(i2400mu %p)\n", i2400m);
+	atomic_inc(&i2400mu->rx_pending_count);
+	wake_up_all(&i2400mu->rx_wq);
+	d_fnend(3, dev, "(i2400m %p) = void\n", i2400m);
+}
+
+
+int i2400mu_rx_setup(struct i2400mu *i2400mu)
+{
+	int result = 0;
+	struct i2400m *i2400m = &i2400mu->i2400m;
+	struct device *dev = &i2400mu->usb_iface->dev;
+	struct wimax_dev *wimax_dev = &i2400m->wimax_dev;
+
+	i2400mu->rx_kthread = kthread_run(i2400mu_rxd, i2400mu, "%s-rx",
+					  wimax_dev->name);
+	if (IS_ERR(i2400mu->rx_kthread)) {
+		result = PTR_ERR(i2400mu->rx_kthread);
+		dev_err(dev, "RX: cannot start thread: %d\n", result);
+	}
+	return result;
+}
+
+void i2400mu_rx_release(struct i2400mu *i2400mu)
+{
+	kthread_stop(i2400mu->rx_kthread);
+}
+
diff --git a/drivers/net/wimax/i2400m/usb-tx.c b/drivers/net/wimax/i2400m/usb-tx.c
new file mode 100644
index 000000000000..dfd893356f49
--- /dev/null
+++ b/drivers/net/wimax/i2400m/usb-tx.c
@@ -0,0 +1,229 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * USB specific TX handling
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Yanir Lubetkin <yanirx.lubetkin@intel.com>
+ *  - Initial implementation
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ *  - Split transport/device specific
+ *
+ *
+ * Takes the TX messages in the i2400m's driver TX FIFO and sends them
+ * to the device until there are no more.
+ *
+ * If we fail sending the message, we just drop it. There isn't much
+ * we can do at this point. We could also retry, but the USB stack has
+ * already retried and still failed, so there is not much of a
+ * point. As well, most of the traffic is network, which has recovery
+ * methods for dropped packets.
+ *
+ * For sending we just obtain a FIFO buffer to send, send it to the
+ * USB bulk out, tell the TX FIFO code we have sent it; query for
+ * another one, etc... until done.
+ *
+ * We use a thread so we can call usb_autopm_enable() and
+ * usb_autopm_disable() for each transaction; this way when the device
+ * goes idle, it will suspend. It also has less overhead than a
+ * dedicated workqueue, as it is being used for a single task.
+ *
+ * ROADMAP
+ *
+ * i2400mu_tx_setup()
+ * i2400mu_tx_release()
+ *
+ * i2400mu_bus_tx_kick()	- Called by the tx.c code when there
+ *                                is new data in the FIFO.
+ * i2400mu_txd()
+ *   i2400m_tx_msg_get()
+ *   i2400m_tx_msg_sent()
+ */
+#include "i2400m-usb.h"
+
+
+#define D_SUBMODULE tx
+#include "usb-debug-levels.h"
+
+
+/*
+ * Get the next TX message in the TX FIFO and send it to the device
+ *
+ * Note that any iteration consumes a message to be sent, no matter if
+ * it succeeds or fails (we have no real way to retry or complain).
+ *
+ * Return: 0 if ok, < 0 errno code on hard error.
+ */
+static
+int i2400mu_tx(struct i2400mu *i2400mu, struct i2400m_msg_hdr *tx_msg,
+	       size_t tx_msg_size)
+{
+	int result = 0;
+	struct i2400m *i2400m = &i2400mu->i2400m;
+	struct device *dev = &i2400mu->usb_iface->dev;
+	int usb_pipe, sent_size, do_autopm;
+	struct usb_endpoint_descriptor *epd;
+
+	d_fnstart(4, dev, "(i2400mu %p)\n", i2400mu);
+	do_autopm = atomic_read(&i2400mu->do_autopm);
+	result = do_autopm ?
+		usb_autopm_get_interface(i2400mu->usb_iface) : 0;
+	if (result < 0) {
+		dev_err(dev, "TX: can't get autopm: %d\n", result);
+		do_autopm = 0;
+	}
+	epd = usb_get_epd(i2400mu->usb_iface, I2400MU_EP_BULK_OUT);
+	usb_pipe = usb_sndbulkpipe(i2400mu->usb_dev, epd->bEndpointAddress);
+retry:
+	result = usb_bulk_msg(i2400mu->usb_dev, usb_pipe,
+			      tx_msg, tx_msg_size, &sent_size, HZ);
+	usb_mark_last_busy(i2400mu->usb_dev);
+	switch (result) {
+	case 0:
+		if (sent_size != tx_msg_size) {	/* Too short? drop it */
+			dev_err(dev, "TX: short write (%d B vs %zu "
+				"expected)\n", sent_size, tx_msg_size);
+			result = -EIO;
+		}
+		break;
+	case -EINVAL:			/* while removing driver */
+	case -ENODEV:			/* dev disconnect ... */
+	case -ENOENT:			/* just ignore it */
+	case -ESHUTDOWN:		/* and exit */
+	case -ECONNRESET:
+		result = -ESHUTDOWN;
+		break;
+	default:			/* Some error? */
+		if (edc_inc(&i2400mu->urb_edc,
+			    EDC_MAX_ERRORS, EDC_ERROR_TIMEFRAME)) {
+			dev_err(dev, "TX: maximum errors in URB "
+				"exceeded; resetting device\n");
+			usb_queue_reset_device(i2400mu->usb_iface);
+		} else {
+			dev_err(dev, "TX: cannot send URB; retrying. "
+				"tx_msg @%zu %zu B [%d sent]: %d\n",
+				(void *) tx_msg - i2400m->tx_buf,
+				tx_msg_size, sent_size, result);
+			goto retry;
+		}
+	}
+	if (do_autopm)
+		usb_autopm_put_interface(i2400mu->usb_iface);
+	d_fnend(4, dev, "(i2400mu %p) = result\n", i2400mu);
+	return result;
+}
+
+
+/*
+ * Get the next TX message in the TX FIFO and send it to the device
+ *
+ * Note we exit the loop if i2400mu_tx() fails; that funtion only
+ * fails on hard error (failing to tx a buffer not being one of them,
+ * see its doc).
+ *
+ * Return: 0
+ */
+static
+int i2400mu_txd(void *_i2400mu)
+{
+	int result = 0;
+	struct i2400mu *i2400mu = _i2400mu;
+	struct i2400m *i2400m = &i2400mu->i2400m;
+	struct device *dev = &i2400mu->usb_iface->dev;
+	struct i2400m_msg_hdr *tx_msg;
+	size_t tx_msg_size;
+
+	d_fnstart(4, dev, "(i2400mu %p)\n", i2400mu);
+
+	while (1) {
+		d_printf(2, dev, "TX: waiting for messages\n");
+		tx_msg = NULL;
+		wait_event_interruptible(
+			i2400mu->tx_wq,
+			(kthread_should_stop()	/* check this first! */
+			 || (tx_msg = i2400m_tx_msg_get(i2400m, &tx_msg_size)))
+			);
+		if (kthread_should_stop())
+			break;
+		WARN_ON(tx_msg == NULL);	/* should not happen...*/
+		d_printf(2, dev, "TX: submitting %zu bytes\n", tx_msg_size);
+		d_dump(5, dev, tx_msg, tx_msg_size);
+		/* Yeah, we ignore errors ... not much we can do */
+		i2400mu_tx(i2400mu, tx_msg, tx_msg_size);
+		i2400m_tx_msg_sent(i2400m);	/* ack it, advance the FIFO */
+		if (result < 0)
+			break;
+	}
+	d_fnend(4, dev, "(i2400mu %p) = %d\n", i2400mu, result);
+	return result;
+}
+
+
+/*
+ * i2400m TX engine notifies us that there is data in the FIFO ready
+ * for TX
+ *
+ * If there is a URB in flight, don't do anything; when it finishes,
+ * it will see there is data in the FIFO and send it. Else, just
+ * submit a write.
+ */
+void i2400mu_bus_tx_kick(struct i2400m *i2400m)
+{
+	struct i2400mu *i2400mu = container_of(i2400m, struct i2400mu, i2400m);
+	struct device *dev = &i2400mu->usb_iface->dev;
+
+	d_fnstart(3, dev, "(i2400m %p) = void\n", i2400m);
+	wake_up_all(&i2400mu->tx_wq);
+	d_fnend(3, dev, "(i2400m %p) = void\n", i2400m);
+}
+
+
+int i2400mu_tx_setup(struct i2400mu *i2400mu)
+{
+	int result = 0;
+	struct i2400m *i2400m = &i2400mu->i2400m;
+	struct device *dev = &i2400mu->usb_iface->dev;
+	struct wimax_dev *wimax_dev = &i2400m->wimax_dev;
+
+	i2400mu->tx_kthread = kthread_run(i2400mu_txd, i2400mu, "%s-tx",
+					  wimax_dev->name);
+	if (IS_ERR(i2400mu->tx_kthread)) {
+		result = PTR_ERR(i2400mu->tx_kthread);
+		dev_err(dev, "TX: cannot start thread: %d\n", result);
+	}
+	return result;
+}
+
+void i2400mu_tx_release(struct i2400mu *i2400mu)
+{
+	kthread_stop(i2400mu->tx_kthread);
+}
diff --git a/drivers/net/wimax/i2400m/usb.c b/drivers/net/wimax/i2400m/usb.c
new file mode 100644
index 000000000000..6d4b65fd9c17
--- /dev/null
+++ b/drivers/net/wimax/i2400m/usb.c
@@ -0,0 +1,591 @@
+/*
+ * Intel Wireless WiMAX Connection 2400m
+ * Linux driver model glue for USB device, reset & fw upload
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ * Yanir Lubetkin <yanirx.lubetkin@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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.
+ *
+ *
+ * See i2400m-usb.h for a general description of this driver.
+ *
+ * This file implements driver model glue, and hook ups for the
+ * generic driver to implement the bus-specific functions (device
+ * communication setup/tear down, firmware upload and resetting).
+ *
+ * ROADMAP
+ *
+ * i2400mu_probe()
+ *   alloc_netdev()...
+ *     i2400mu_netdev_setup()
+ *       i2400mu_init()
+ *       i2400m_netdev_setup()
+ *   i2400m_setup()...
+ *
+ * i2400mu_disconnect
+ *   i2400m_release()
+ *   free_netdev()
+ *
+ * i2400mu_suspend()
+ *   i2400m_cmd_enter_powersave()
+ *   i2400mu_notification_release()
+ *
+ * i2400mu_resume()
+ *   i2400mu_notification_setup()
+ *
+ * i2400mu_bus_dev_start()        Called by i2400m_dev_start() [who is
+ *   i2400mu_tx_setup()           called by i2400m_setup()]
+ *   i2400mu_rx_setup()
+ *   i2400mu_notification_setup()
+ *
+ * i2400mu_bus_dev_stop()         Called by i2400m_dev_stop() [who is
+ *   i2400mu_notification_release()  called by i2400m_release()]
+ *   i2400mu_rx_release()
+ *   i2400mu_tx_release()
+ *
+ * i2400mu_bus_reset()            Called by i2400m->bus_reset
+ *   __i2400mu_reset()
+ *     __i2400mu_send_barker()
+ *   usb_reset_device()
+ */
+#include "i2400m-usb.h"
+#include <linux/wimax/i2400m.h>
+#include <linux/debugfs.h>
+
+
+#define D_SUBMODULE usb
+#include "usb-debug-levels.h"
+
+
+/* Our firmware file name */
+#define I2400MU_FW_FILE_NAME "i2400m-fw-usb-" I2400M_FW_VERSION ".sbcf"
+
+static
+int i2400mu_bus_dev_start(struct i2400m *i2400m)
+{
+	int result;
+	struct i2400mu *i2400mu = container_of(i2400m, struct i2400mu, i2400m);
+	struct device *dev = &i2400mu->usb_iface->dev;
+
+	d_fnstart(3, dev, "(i2400m %p)\n", i2400m);
+	result = i2400mu_tx_setup(i2400mu);
+	if (result < 0)
+		goto error_usb_tx_setup;
+	result = i2400mu_rx_setup(i2400mu);
+	if (result < 0)
+		goto error_usb_rx_setup;
+	result = i2400mu_notification_setup(i2400mu);
+	if (result < 0)
+		goto error_notif_setup;
+	d_fnend(3, dev, "(i2400m %p) = %d\n", i2400m, result);
+	return result;
+
+error_notif_setup:
+	i2400mu_rx_release(i2400mu);
+error_usb_rx_setup:
+	i2400mu_tx_release(i2400mu);
+error_usb_tx_setup:
+	d_fnend(3, dev, "(i2400m %p) = void\n", i2400m);
+	return result;
+}
+
+
+static
+void i2400mu_bus_dev_stop(struct i2400m *i2400m)
+{
+	struct i2400mu *i2400mu = container_of(i2400m, struct i2400mu, i2400m);
+	struct device *dev = &i2400mu->usb_iface->dev;
+
+	d_fnstart(3, dev, "(i2400m %p)\n", i2400m);
+	i2400mu_notification_release(i2400mu);
+	i2400mu_rx_release(i2400mu);
+	i2400mu_tx_release(i2400mu);
+	d_fnend(3, dev, "(i2400m %p) = void\n", i2400m);
+}
+
+
+/*
+ * Sends a barker buffer to the device
+ *
+ * This helper will allocate a kmalloced buffer and use it to transmit
+ * (then free it). Reason for this is that other arches cannot use
+ * stack/vmalloc/text areas for DMA transfers.
+ *
+ * Error recovery here is simpler: anything is considered a hard error
+ * and will move the reset code to use a last-resort bus-based reset.
+ */
+static
+int __i2400mu_send_barker(struct i2400mu *i2400mu,
+			  const __le32 *barker,
+			  size_t barker_size,
+			  unsigned endpoint)
+{
+	struct usb_endpoint_descriptor *epd = NULL;
+	int pipe, actual_len, ret;
+	struct device *dev = &i2400mu->usb_iface->dev;
+	void *buffer;
+	int do_autopm = 1;
+
+	ret = usb_autopm_get_interface(i2400mu->usb_iface);
+	if (ret < 0) {
+		dev_err(dev, "RESET: can't get autopm: %d\n", ret);
+		do_autopm = 0;
+	}
+	ret = -ENOMEM;
+	buffer = kmalloc(barker_size, GFP_KERNEL);
+	if (buffer == NULL)
+		goto error_kzalloc;
+	epd = usb_get_epd(i2400mu->usb_iface, endpoint);
+	pipe = usb_sndbulkpipe(i2400mu->usb_dev, epd->bEndpointAddress);
+	memcpy(buffer, barker, barker_size);
+	ret = usb_bulk_msg(i2400mu->usb_dev, pipe, buffer, barker_size,
+			   &actual_len, HZ);
+	if (ret < 0) {
+		if (ret != -EINVAL)
+			dev_err(dev, "E: barker error: %d\n", ret);
+	} else if (actual_len != barker_size) {
+		dev_err(dev, "E: only %d bytes transmitted\n", actual_len);
+		ret = -EIO;
+	}
+	kfree(buffer);
+error_kzalloc:
+	if (do_autopm)
+		usb_autopm_put_interface(i2400mu->usb_iface);
+	return ret;
+}
+
+
+/*
+ * Reset a device at different levels (warm, cold or bus)
+ *
+ * @i2400m: device descriptor
+ * @reset_type: soft, warm or bus reset (I2400M_RT_WARM/SOFT/BUS)
+ *
+ * Warm and cold resets get a USB reset if they fail.
+ *
+ * Warm reset:
+ *
+ * The device will be fully reset internally, but won't be
+ * disconnected from the USB bus (so no reenumeration will
+ * happen). Firmware upload will be neccessary.
+ *
+ * The device will send a reboot barker in the notification endpoint
+ * that will trigger the driver to reinitialize the state
+ * automatically from notif.c:i2400m_notification_grok() into
+ * i2400m_dev_bootstrap_delayed().
+ *
+ * Cold and bus (USB) reset:
+ *
+ * The device will be fully reset internally, disconnected from the
+ * USB bus an a reenumeration will happen. Firmware upload will be
+ * neccessary. Thus, we don't do any locking or struct
+ * reinitialization, as we are going to be fully disconnected and
+ * reenumerated.
+ *
+ * Note we need to return -ENODEV if a warm reset was requested and we
+ * had to resort to a bus reset. See i2400m_op_reset(), wimax_reset()
+ * and wimax_dev->op_reset.
+ *
+ * WARNING: no driver state saved/fixed
+ */
+static
+int i2400mu_bus_reset(struct i2400m *i2400m, enum i2400m_reset_type rt)
+{
+	int result;
+	struct i2400mu *i2400mu =
+		container_of(i2400m, struct i2400mu, i2400m);
+	struct device *dev = i2400m_dev(i2400m);
+	static const __le32 i2400m_WARM_BOOT_BARKER[4] = {
+		__constant_cpu_to_le32(I2400M_WARM_RESET_BARKER),
+		__constant_cpu_to_le32(I2400M_WARM_RESET_BARKER),
+		__constant_cpu_to_le32(I2400M_WARM_RESET_BARKER),
+		__constant_cpu_to_le32(I2400M_WARM_RESET_BARKER),
+	};
+	static const __le32 i2400m_COLD_BOOT_BARKER[4] = {
+		__constant_cpu_to_le32(I2400M_COLD_RESET_BARKER),
+		__constant_cpu_to_le32(I2400M_COLD_RESET_BARKER),
+		__constant_cpu_to_le32(I2400M_COLD_RESET_BARKER),
+		__constant_cpu_to_le32(I2400M_COLD_RESET_BARKER),
+	};
+
+	d_fnstart(3, dev, "(i2400m %p rt %u)\n", i2400m, rt);
+	if (rt == I2400M_RT_WARM)
+		result = __i2400mu_send_barker(i2400mu, i2400m_WARM_BOOT_BARKER,
+					       sizeof(i2400m_WARM_BOOT_BARKER),
+					       I2400MU_EP_BULK_OUT);
+	else if (rt == I2400M_RT_COLD)
+		result = __i2400mu_send_barker(i2400mu, i2400m_COLD_BOOT_BARKER,
+					       sizeof(i2400m_COLD_BOOT_BARKER),
+					       I2400MU_EP_RESET_COLD);
+	else if (rt == I2400M_RT_BUS) {
+do_bus_reset:
+		result = usb_reset_device(i2400mu->usb_dev);
+		switch (result) {
+		case 0:
+		case -EINVAL:	/* device is gone */
+		case -ENODEV:
+		case -ENOENT:
+		case -ESHUTDOWN:
+			result = rt == I2400M_RT_WARM ? -ENODEV : 0;
+			break;	/* We assume the device is disconnected */
+		default:
+			dev_err(dev, "USB reset failed (%d), giving up!\n",
+				result);
+		}
+	} else
+		BUG();
+	if (result < 0
+	    && result != -EINVAL	/* device is gone */
+	    && rt != I2400M_RT_BUS) {
+		dev_err(dev, "%s reset failed (%d); trying USB reset\n",
+			rt == I2400M_RT_WARM ? "warm" : "cold", result);
+		rt = I2400M_RT_BUS;
+		goto do_bus_reset;
+	}
+	d_fnend(3, dev, "(i2400m %p rt %u) = %d\n", i2400m, rt, result);
+	return result;
+}
+
+
+static
+void i2400mu_netdev_setup(struct net_device *net_dev)
+{
+	struct i2400m *i2400m = net_dev_to_i2400m(net_dev);
+	struct i2400mu *i2400mu = container_of(i2400m, struct i2400mu, i2400m);
+	i2400mu_init(i2400mu);
+	i2400m_netdev_setup(net_dev);
+}
+
+
+/*
+ * Debug levels control; see debug.h
+ */
+struct d_level D_LEVEL[] = {
+	D_SUBMODULE_DEFINE(usb),
+	D_SUBMODULE_DEFINE(fw),
+	D_SUBMODULE_DEFINE(notif),
+	D_SUBMODULE_DEFINE(rx),
+	D_SUBMODULE_DEFINE(tx),
+};
+size_t D_LEVEL_SIZE = ARRAY_SIZE(D_LEVEL);
+
+
+#define __debugfs_register(prefix, name, parent)			\
+do {									\
+	result = d_level_register_debugfs(prefix, name, parent);	\
+	if (result < 0)							\
+		goto error;						\
+} while (0)
+
+
+static
+int i2400mu_debugfs_add(struct i2400mu *i2400mu)
+{
+	int result;
+	struct device *dev = &i2400mu->usb_iface->dev;
+	struct dentry *dentry = i2400mu->i2400m.wimax_dev.debugfs_dentry;
+	struct dentry *fd;
+
+	dentry = debugfs_create_dir("i2400m-usb", dentry);
+	result = PTR_ERR(dentry);
+	if (IS_ERR(dentry)) {
+		if (result == -ENODEV)
+			result = 0;	/* No debugfs support */
+		goto error;
+	}
+	i2400mu->debugfs_dentry = dentry;
+	__debugfs_register("dl_", usb, dentry);
+	__debugfs_register("dl_", fw, dentry);
+	__debugfs_register("dl_", notif, dentry);
+	__debugfs_register("dl_", rx, dentry);
+	__debugfs_register("dl_", tx, dentry);
+
+	/* Don't touch these if you don't know what you are doing */
+	fd = debugfs_create_u8("rx_size_auto_shrink", 0600, dentry,
+			       &i2400mu->rx_size_auto_shrink);
+	result = PTR_ERR(fd);
+	if (IS_ERR(fd) && result != -ENODEV) {
+		dev_err(dev, "Can't create debugfs entry "
+			"rx_size_auto_shrink: %d\n", result);
+		goto error;
+	}
+
+	fd = debugfs_create_size_t("rx_size", 0600, dentry,
+				   &i2400mu->rx_size);
+	result = PTR_ERR(fd);
+	if (IS_ERR(fd) && result != -ENODEV) {
+		dev_err(dev, "Can't create debugfs entry "
+			"rx_size: %d\n", result);
+		goto error;
+	}
+
+	return 0;
+
+error:
+	debugfs_remove_recursive(i2400mu->debugfs_dentry);
+	return result;
+}
+
+
+/*
+ * Probe a i2400m interface and register it
+ *
+ * @iface:   USB interface to link to
+ * @id:      USB class/subclass/protocol id
+ * @returns: 0 if ok, < 0 errno code on error.
+ *
+ * Alloc a net device, initialize the bus-specific details and then
+ * calls the bus-generic initialization routine. That will register
+ * the wimax and netdev devices, upload the firmware [using
+ * _bus_bm_*()], call _bus_dev_start() to finalize the setup of the
+ * communication with the device and then will start to talk to it to
+ * finnish setting it up.
+ */
+static
+int i2400mu_probe(struct usb_interface *iface,
+		  const struct usb_device_id *id)
+{
+	int result;
+	struct net_device *net_dev;
+	struct device *dev = &iface->dev;
+	struct i2400m *i2400m;
+	struct i2400mu *i2400mu;
+	struct usb_device *usb_dev = interface_to_usbdev(iface);
+
+	if (usb_dev->speed != USB_SPEED_HIGH)
+		dev_err(dev, "device not connected as high speed\n");
+
+	/* Allocate instance [calls i2400m_netdev_setup() on it]. */
+	result = -ENOMEM;
+	net_dev = alloc_netdev(sizeof(*i2400mu), "wmx%d",
+			       i2400mu_netdev_setup);
+	if (net_dev == NULL) {
+		dev_err(dev, "no memory for network device instance\n");
+		goto error_alloc_netdev;
+	}
+	SET_NETDEV_DEV(net_dev, dev);
+	i2400m = net_dev_to_i2400m(net_dev);
+	i2400mu = container_of(i2400m, struct i2400mu, i2400m);
+	i2400m->wimax_dev.net_dev = net_dev;
+	i2400mu->usb_dev = usb_get_dev(usb_dev);
+	i2400mu->usb_iface = iface;
+	usb_set_intfdata(iface, i2400mu);
+
+	i2400m->bus_tx_block_size = I2400MU_BLK_SIZE;
+	i2400m->bus_pl_size_max = I2400MU_PL_SIZE_MAX;
+	i2400m->bus_dev_start = i2400mu_bus_dev_start;
+	i2400m->bus_dev_stop = i2400mu_bus_dev_stop;
+	i2400m->bus_tx_kick = i2400mu_bus_tx_kick;
+	i2400m->bus_reset = i2400mu_bus_reset;
+	i2400m->bus_bm_cmd_send = i2400mu_bus_bm_cmd_send;
+	i2400m->bus_bm_wait_for_ack = i2400mu_bus_bm_wait_for_ack;
+	i2400m->bus_fw_name = I2400MU_FW_FILE_NAME;
+	i2400m->bus_bm_mac_addr_impaired = 0;
+
+	iface->needs_remote_wakeup = 1;		/* autosuspend (15s delay) */
+	device_init_wakeup(dev, 1);
+	usb_autopm_enable(i2400mu->usb_iface);
+	usb_dev->autosuspend_delay = 15 * HZ;
+	usb_dev->autosuspend_disabled = 0;
+
+	result = i2400m_setup(i2400m, I2400M_BRI_MAC_REINIT);
+	if (result < 0) {
+		dev_err(dev, "cannot setup device: %d\n", result);
+		goto error_setup;
+	}
+	result = i2400mu_debugfs_add(i2400mu);
+	if (result < 0) {
+		dev_err(dev, "Can't register i2400mu's debugfs: %d\n", result);
+		goto error_debugfs_add;
+	}
+	return 0;
+
+error_debugfs_add:
+	i2400m_release(i2400m);
+error_setup:
+	usb_set_intfdata(iface, NULL);
+	usb_put_dev(i2400mu->usb_dev);
+	free_netdev(net_dev);
+error_alloc_netdev:
+	return result;
+}
+
+
+/*
+ * Disconect a i2400m from the system.
+ *
+ * i2400m_stop() has been called before, so al the rx and tx contexts
+ * have been taken down already. Make sure the queue is stopped,
+ * unregister netdev and i2400m, free and kill.
+ */
+static
+void i2400mu_disconnect(struct usb_interface *iface)
+{
+	struct i2400mu *i2400mu = usb_get_intfdata(iface);
+	struct i2400m *i2400m = &i2400mu->i2400m;
+	struct net_device *net_dev = i2400m->wimax_dev.net_dev;
+	struct device *dev = &iface->dev;
+
+	d_fnstart(3, dev, "(iface %p i2400m %p)\n", iface, i2400m);
+
+	debugfs_remove_recursive(i2400mu->debugfs_dentry);
+	i2400m_release(i2400m);
+	usb_set_intfdata(iface, NULL);
+	usb_put_dev(i2400mu->usb_dev);
+	free_netdev(net_dev);
+	d_fnend(3, dev, "(iface %p i2400m %p) = void\n", iface, i2400m);
+}
+
+
+/*
+ * Get the device ready for USB port or system standby and hibernation
+ *
+ * USB port and system standby are handled the same.
+ *
+ * When the system hibernates, the USB device is powered down and then
+ * up, so we don't really have to do much here, as it will be seen as
+ * a reconnect. Still for simplicity we consider this case the same as
+ * suspend, so that the device has a chance to do notify the base
+ * station (if connected).
+ *
+ * So at the end, the three cases require common handling.
+ *
+ * If at the time of this call the device's firmware is not loaded,
+ * nothing has to be done.
+ *
+ * If the firmware is loaded, we need to:
+ *
+ *  - tell the device to go into host interface power save mode, wait
+ *    for it to ack
+ *
+ *    This is quite more interesting than it is; we need to execute a
+ *    command, but this time, we don't want the code in usb-{tx,rx}.c
+ *    to call the usb_autopm_get/put_interface() barriers as it'd
+ *    deadlock, so we need to decrement i2400mu->do_autopm, that acts
+ *    as a poor man's semaphore. Ugly, but it works.
+ *
+ *    As well, the device might refuse going to sleep for whichever
+ *    reason. In this case we just fail. For system suspend/hibernate,
+ *    we *can't* fail. We look at usb_dev->auto_pm to see if the
+ *    suspend call comes from the USB stack or from the system and act
+ *    in consequence.
+ *
+ *  - stop the notification endpoint polling
+ */
+static
+int i2400mu_suspend(struct usb_interface *iface, pm_message_t pm_msg)
+{
+	int result = 0;
+	struct device *dev = &iface->dev;
+	struct i2400mu *i2400mu = usb_get_intfdata(iface);
+	struct usb_device *usb_dev = i2400mu->usb_dev;
+	struct i2400m *i2400m = &i2400mu->i2400m;
+
+	d_fnstart(3, dev, "(iface %p pm_msg %u)\n", iface, pm_msg.event);
+	if (i2400m->updown == 0)
+		goto no_firmware;
+	d_printf(1, dev, "fw up, requesting standby\n");
+	atomic_dec(&i2400mu->do_autopm);
+	result = i2400m_cmd_enter_powersave(i2400m);
+	atomic_inc(&i2400mu->do_autopm);
+	if (result < 0 && usb_dev->auto_pm == 0) {
+		/* System suspend, can't fail */
+		dev_err(dev, "failed to suspend, will reset on resume\n");
+		result = 0;
+	}
+	if (result < 0)
+		goto error_enter_powersave;
+	i2400mu_notification_release(i2400mu);
+	d_printf(1, dev, "fw up, got standby\n");
+error_enter_powersave:
+no_firmware:
+	d_fnend(3, dev, "(iface %p pm_msg %u) = %d\n",
+		iface, pm_msg.event, result);
+	return result;
+}
+
+
+static
+int i2400mu_resume(struct usb_interface *iface)
+{
+	int ret = 0;
+	struct device *dev = &iface->dev;
+	struct i2400mu *i2400mu = usb_get_intfdata(iface);
+	struct i2400m *i2400m = &i2400mu->i2400m;
+
+	d_fnstart(3, dev, "(iface %p)\n", iface);
+	if (i2400m->updown == 0) {
+		d_printf(1, dev, "fw was down, no resume neeed\n");
+		goto out;
+	}
+	d_printf(1, dev, "fw was up, resuming\n");
+	i2400mu_notification_setup(i2400mu);
+	/* USB has flow control, so we don't need to give it time to
+	 * come back; otherwise, we'd use something like a get-state
+	 * command... */
+out:
+	d_fnend(3, dev, "(iface %p) = %d\n", iface, ret);
+	return ret;
+}
+
+
+static
+struct usb_device_id i2400mu_id_table[] = {
+	{ USB_DEVICE(0x8086, 0x0181) },
+	{ USB_DEVICE(0x8086, 0x1403) },
+	{ USB_DEVICE(0x8086, 0x1405) },
+	{ USB_DEVICE(0x8086, 0x0180) },
+	{ USB_DEVICE(0x8086, 0x0182) },
+	{ USB_DEVICE(0x8086, 0x1406) },
+	{ USB_DEVICE(0x8086, 0x1403) },
+	{ },
+};
+MODULE_DEVICE_TABLE(usb, i2400mu_id_table);
+
+
+static
+struct usb_driver i2400mu_driver = {
+	.name = KBUILD_MODNAME,
+	.suspend = i2400mu_suspend,
+	.resume = i2400mu_resume,
+	.probe = i2400mu_probe,
+	.disconnect = i2400mu_disconnect,
+	.id_table = i2400mu_id_table,
+	.supports_autosuspend = 1,
+};
+
+static
+int __init i2400mu_driver_init(void)
+{
+	return usb_register(&i2400mu_driver);
+}
+module_init(i2400mu_driver_init);
+
+
+static
+void __exit i2400mu_driver_exit(void)
+{
+	flush_scheduled_work();	/* for the stuff we schedule from sysfs.c */
+	usb_deregister(&i2400mu_driver);
+}
+module_exit(i2400mu_driver_exit);
+
+MODULE_AUTHOR("Intel Corporation <linux-wimax@intel.com>");
+MODULE_DESCRIPTION("Intel 2400M WiMAX networking for USB");
+MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(I2400MU_FW_FILE_NAME);
diff --git a/drivers/net/wireless/atmel.c b/drivers/net/wireless/atmel.c
index 350157fcd080..4223672c4432 100644
--- a/drivers/net/wireless/atmel.c
+++ b/drivers/net/wireless/atmel.c
@@ -3836,7 +3836,7 @@ static int reset_atmel_card(struct net_device *dev)
 	   This routine is also responsible for initialising some
 	   hardware-specific fields in the atmel_private structure,
 	   including a copy of the firmware's hostinfo stucture
-	   which is the route into the rest of the firmare datastructures. */
+	   which is the route into the rest of the firmware datastructures. */
 
 	struct atmel_private *priv = netdev_priv(dev);
 	u8 configuration;
diff --git a/drivers/net/wireless/ipw2x00/ipw2100.c b/drivers/net/wireless/ipw2x00/ipw2100.c
index 1667065b86a7..823c2bf5e31e 100644
--- a/drivers/net/wireless/ipw2x00/ipw2100.c
+++ b/drivers/net/wireless/ipw2x00/ipw2100.c
@@ -1332,7 +1332,7 @@ static int ipw2100_power_cycle_adapter(struct ipw2100_priv *priv)
 		       IPW_AUX_HOST_RESET_REG_STOP_MASTER);
 
 	/* Step 2. Wait for stop Master Assert
-	 *         (not more then 50us, otherwise ret error */
+	 *         (not more than 50us, otherwise ret error */
 	i = 5;
 	do {
 		udelay(IPW_WAIT_RESET_MASTER_ASSERT_COMPLETE_DELAY);
@@ -1830,7 +1830,7 @@ static void ipw2100_down(struct ipw2100_priv *priv)
 		cancel_delayed_work(&priv->rf_kill);
 	}
 
-	/* Kill the firmare hang check timer */
+	/* Kill the firmware hang check timer */
 	if (!priv->stop_hang_check) {
 		priv->stop_hang_check = 1;
 		cancel_delayed_work(&priv->hang_check);
diff --git a/drivers/net/wireless/rt2x00/rt2x00crypto.c b/drivers/net/wireless/rt2x00/rt2x00crypto.c
index 37ad0d2fb64c..aee9cba13eb3 100644
--- a/drivers/net/wireless/rt2x00/rt2x00crypto.c
+++ b/drivers/net/wireless/rt2x00/rt2x00crypto.c
@@ -184,8 +184,8 @@ void rt2x00crypto_rx_insert_iv(struct sk_buff *skb, unsigned int align,
 	 * Make room for new data, note that we increase both
 	 * headsize and tailsize when required. The tailsize is
 	 * only needed when ICV data needs to be inserted and
-	 * the padding is smaller then the ICV data.
-	 * When alignment requirements is greater then the
+	 * the padding is smaller than the ICV data.
+	 * When alignment requirements is greater than the
 	 * ICV data we must trim the skb to the correct size
 	 * because we need to remove the extra bytes.
 	 */
diff --git a/drivers/net/wireless/strip.c b/drivers/net/wireless/strip.c
index dd0de3a9ed4e..7015f2480550 100644
--- a/drivers/net/wireless/strip.c
+++ b/drivers/net/wireless/strip.c
@@ -236,7 +236,7 @@ struct strip {
 	unsigned long tx_errors;	/* Planned stuff                */
 	unsigned long rx_dropped;	/* No memory for skb            */
 	unsigned long tx_dropped;	/* When MTU change              */
-	unsigned long rx_over_errors;	/* Frame bigger then STRIP buf. */
+	unsigned long rx_over_errors;	/* Frame bigger than STRIP buf. */
 
 	unsigned long pps_timer;	/* Timer to determine pps       */
 	unsigned long rx_pps_count;	/* Counter to determine pps     */
diff --git a/drivers/parisc/dino.c b/drivers/parisc/dino.c
index 3bc54b30c3a1..77cc8bfef8c9 100644
--- a/drivers/parisc/dino.c
+++ b/drivers/parisc/dino.c
@@ -547,7 +547,7 @@ dino_card_fixup(struct pci_dev *dev)
 	** The additional "-1" adjusts for skewing the IRQ<->slot.
 	*/
 	dino_cfg_read(dev->bus, dev->devfn, PCI_INTERRUPT_PIN, 1, &irq_pin); 
-	dev->irq = (irq_pin + PCI_SLOT(dev->devfn) - 1) % 4 ;
+	dev->irq = pci_swizzle_interrupt_pin(dev, irq_pin) - 1;
 
 	/* Shouldn't really need to do this but it's in case someone tries
 	** to bypass PCI services and look at the card themselves.
@@ -672,7 +672,7 @@ dino_fixup_bus(struct pci_bus *bus)
 			
 			dino_cfg_read(dev->bus, dev->devfn, 
 				      PCI_INTERRUPT_PIN, 1, &irq_pin);
-			irq_pin = (irq_pin + PCI_SLOT(dev->devfn) - 1) % 4 ;
+			irq_pin = pci_swizzle_interrupt_pin(dev, irq_pin) - 1;
 			printk(KERN_WARNING "Device %s has undefined IRQ, "
 					"setting to %d\n", pci_name(dev), irq_pin);
 			dino_cfg_write(dev->bus, dev->devfn, 
diff --git a/drivers/parisc/iosapic.c b/drivers/parisc/iosapic.c
index 9dedbbd218c3..0797659ee016 100644
--- a/drivers/parisc/iosapic.c
+++ b/drivers/parisc/iosapic.c
@@ -519,8 +519,7 @@ iosapic_xlate_pin(struct iosapic_info *isi, struct pci_dev *pcidev)
 		**
 		** Advantage is it's really easy to implement.
 		*/
-		intr_pin = ((intr_pin-1)+PCI_SLOT(pcidev->devfn)) % 4;
-		intr_pin++;	/* convert back to INTA-D (1-4) */
+		intr_pin = pci_swizzle_interrupt_pin(pcidev, intr_pin);
 #endif /* PCI_BRIDGE_FUNCS */
 
 		/*
diff --git a/drivers/parport/ieee1284.c b/drivers/parport/ieee1284.c
index ac2a805ac7ea..8901ecf6e037 100644
--- a/drivers/parport/ieee1284.c
+++ b/drivers/parport/ieee1284.c
@@ -84,7 +84,7 @@ int parport_wait_event (struct parport *port, signed long timeout)
 
 	add_timer (&timer);
 	ret = down_interruptible (&port->physport->ieee1284.irq);
-	if (!del_timer (&timer) && !ret)
+	if (!del_timer_sync(&timer) && !ret)
 		/* Timed out. */
 		ret = 1;
 
diff --git a/drivers/pci/Kconfig b/drivers/pci/Kconfig
index e1ca42591ac4..2a4501dd2515 100644
--- a/drivers/pci/Kconfig
+++ b/drivers/pci/Kconfig
@@ -42,6 +42,15 @@ config PCI_DEBUG
 
 	  When in doubt, say N.
 
+config PCI_STUB
+	tristate "PCI Stub driver"
+	depends on PCI
+	help
+	  Say Y or M here if you want be able to reserve a PCI device
+	  when it is going to be assigned to a guest operating system.
+
+	  When in doubt, say N.
+
 config HT_IRQ
 	bool "Interrupts on hypertransport devices"
 	default y
diff --git a/drivers/pci/Makefile b/drivers/pci/Makefile
index af3bfe22847b..3d07ce24f6a8 100644
--- a/drivers/pci/Makefile
+++ b/drivers/pci/Makefile
@@ -53,6 +53,8 @@ obj-$(CONFIG_HOTPLUG) += setup-bus.o
 
 obj-$(CONFIG_PCI_SYSCALL) += syscall.o
 
+obj-$(CONFIG_PCI_STUB) += pci-stub.o
+
 ifeq ($(CONFIG_PCI_DEBUG),y)
 EXTRA_CFLAGS += -DDEBUG
 endif
diff --git a/drivers/pci/access.c b/drivers/pci/access.c
index 39bb96b413ef..381444794778 100644
--- a/drivers/pci/access.c
+++ b/drivers/pci/access.c
@@ -66,6 +66,39 @@ EXPORT_SYMBOL(pci_bus_write_config_byte);
 EXPORT_SYMBOL(pci_bus_write_config_word);
 EXPORT_SYMBOL(pci_bus_write_config_dword);
 
+
+/**
+ * pci_read_vpd - Read one entry from Vital Product Data
+ * @dev:	pci device struct
+ * @pos:	offset in vpd space
+ * @count:	number of bytes to read
+ * @buf:	pointer to where to store result
+ *
+ */
+ssize_t pci_read_vpd(struct pci_dev *dev, loff_t pos, size_t count, void *buf)
+{
+	if (!dev->vpd || !dev->vpd->ops)
+		return -ENODEV;
+	return dev->vpd->ops->read(dev, pos, count, buf);
+}
+EXPORT_SYMBOL(pci_read_vpd);
+
+/**
+ * pci_write_vpd - Write entry to Vital Product Data
+ * @dev:	pci device struct
+ * @pos:	offset in vpd space
+ * @count:	number of bytes to read
+ * @val:	value to write
+ *
+ */
+ssize_t pci_write_vpd(struct pci_dev *dev, loff_t pos, size_t count, const void *buf)
+{
+	if (!dev->vpd || !dev->vpd->ops)
+		return -ENODEV;
+	return dev->vpd->ops->write(dev, pos, count, buf);
+}
+EXPORT_SYMBOL(pci_write_vpd);
+
 /*
  * The following routines are to prevent the user from accessing PCI config
  * space when it's unsafe to do so.  Some devices require this during BIST and
@@ -133,125 +166,145 @@ PCI_USER_WRITE_CONFIG(dword, u32)
 
 struct pci_vpd_pci22 {
 	struct pci_vpd base;
-	spinlock_t lock; /* controls access to hardware and the flags */
-	u8	cap;
+	struct mutex lock;
+	u16	flag;
 	bool	busy;
-	bool	flag; /* value of F bit to wait for */
+	u8	cap;
 };
 
-/* Wait for last operation to complete */
+/*
+ * Wait for last operation to complete.
+ * This code has to spin since there is no other notification from the PCI
+ * hardware. Since the VPD is often implemented by serial attachment to an
+ * EEPROM, it may take many milliseconds to complete.
+ */
 static int pci_vpd_pci22_wait(struct pci_dev *dev)
 {
 	struct pci_vpd_pci22 *vpd =
 		container_of(dev->vpd, struct pci_vpd_pci22, base);
-	u16 flag, status;
-	int wait;
+	unsigned long timeout = jiffies + HZ/20 + 2;
+	u16 status;
 	int ret;
 
 	if (!vpd->busy)
 		return 0;
 
-	flag = vpd->flag ? PCI_VPD_ADDR_F : 0;
-	wait = vpd->flag ? 10 : 1000; /* read: 100 us; write: 10 ms */
 	for (;;) {
-		ret = pci_user_read_config_word(dev,
-						vpd->cap + PCI_VPD_ADDR,
+		ret = pci_user_read_config_word(dev, vpd->cap + PCI_VPD_ADDR,
 						&status);
-		if (ret < 0)
+		if (ret)
 			return ret;
-		if ((status & PCI_VPD_ADDR_F) == flag) {
+
+		if ((status & PCI_VPD_ADDR_F) == vpd->flag) {
 			vpd->busy = false;
 			return 0;
 		}
-		if (wait-- == 0)
+
+		if (time_after(jiffies, timeout))
 			return -ETIMEDOUT;
-		udelay(10);
+		if (fatal_signal_pending(current))
+			return -EINTR;
+		if (!cond_resched())
+			udelay(10);
 	}
 }
 
-static int pci_vpd_pci22_read(struct pci_dev *dev, int pos, int size,
-			      char *buf)
+static ssize_t pci_vpd_pci22_read(struct pci_dev *dev, loff_t pos, size_t count,
+				  void *arg)
 {
 	struct pci_vpd_pci22 *vpd =
 		container_of(dev->vpd, struct pci_vpd_pci22, base);
-	u32 val;
 	int ret;
-	int begin, end, i;
+	loff_t end = pos + count;
+	u8 *buf = arg;
 
-	if (pos < 0 || pos > vpd->base.len || size > vpd->base.len  - pos)
+	if (pos < 0 || pos > vpd->base.len || end > vpd->base.len)
 		return -EINVAL;
-	if (size == 0)
-		return 0;
 
-	spin_lock_irq(&vpd->lock);
-	ret = pci_vpd_pci22_wait(dev);
-	if (ret < 0)
-		goto out;
-	ret = pci_user_write_config_word(dev, vpd->cap + PCI_VPD_ADDR,
-					 pos & ~3);
-	if (ret < 0)
-		goto out;
-	vpd->busy = true;
-	vpd->flag = 1;
+	if (mutex_lock_killable(&vpd->lock))
+		return -EINTR;
+
 	ret = pci_vpd_pci22_wait(dev);
 	if (ret < 0)
 		goto out;
-	ret = pci_user_read_config_dword(dev, vpd->cap + PCI_VPD_DATA,
-					 &val);
-out:
-	spin_unlock_irq(&vpd->lock);
-	if (ret < 0)
-		return ret;
-
-	/* Convert to bytes */
-	begin = pos & 3;
-	end = min(4, begin + size);
-	for (i = 0; i < end; ++i) {
-		if (i >= begin)
-			*buf++ = val;
-		val >>= 8;
+
+	while (pos < end) {
+		u32 val;
+		unsigned int i, skip;
+
+		ret = pci_user_write_config_word(dev, vpd->cap + PCI_VPD_ADDR,
+						 pos & ~3);
+		if (ret < 0)
+			break;
+		vpd->busy = true;
+		vpd->flag = PCI_VPD_ADDR_F;
+		ret = pci_vpd_pci22_wait(dev);
+		if (ret < 0)
+			break;
+
+		ret = pci_user_read_config_dword(dev, vpd->cap + PCI_VPD_DATA, &val);
+		if (ret < 0)
+			break;
+
+		skip = pos & 3;
+		for (i = 0;  i < sizeof(u32); i++) {
+			if (i >= skip) {
+				*buf++ = val;
+				if (++pos == end)
+					break;
+			}
+			val >>= 8;
+		}
 	}
-	return end - begin;
+out:
+	mutex_unlock(&vpd->lock);
+	return ret ? ret : count;
 }
 
-static int pci_vpd_pci22_write(struct pci_dev *dev, int pos, int size,
-			       const char *buf)
+static ssize_t pci_vpd_pci22_write(struct pci_dev *dev, loff_t pos, size_t count,
+				   const void *arg)
 {
 	struct pci_vpd_pci22 *vpd =
 		container_of(dev->vpd, struct pci_vpd_pci22, base);
-	u32 val;
-	int ret;
+	const u8 *buf = arg;
+	loff_t end = pos + count;
+	int ret = 0;
 
-	if (pos < 0 || pos > vpd->base.len || pos & 3 ||
-	    size > vpd->base.len - pos || size < 4)
+	if (pos < 0 || (pos & 3) || (count & 3) || end > vpd->base.len)
 		return -EINVAL;
 
-	val = (u8) *buf++;
-	val |= ((u8) *buf++) << 8;
-	val |= ((u8) *buf++) << 16;
-	val |= ((u32)(u8) *buf++) << 24;
+	if (mutex_lock_killable(&vpd->lock))
+		return -EINTR;
 
-	spin_lock_irq(&vpd->lock);
 	ret = pci_vpd_pci22_wait(dev);
 	if (ret < 0)
 		goto out;
-	ret = pci_user_write_config_dword(dev, vpd->cap + PCI_VPD_DATA,
-					  val);
-	if (ret < 0)
-		goto out;
-	ret = pci_user_write_config_word(dev, vpd->cap + PCI_VPD_ADDR,
-					 pos | PCI_VPD_ADDR_F);
-	if (ret < 0)
-		goto out;
-	vpd->busy = true;
-	vpd->flag = 0;
-	ret = pci_vpd_pci22_wait(dev);
-out:
-	spin_unlock_irq(&vpd->lock);
-	if (ret < 0)
-		return ret;
 
-	return 4;
+	while (pos < end) {
+		u32 val;
+
+		val = *buf++;
+		val |= *buf++ << 8;
+		val |= *buf++ << 16;
+		val |= *buf++ << 24;
+
+		ret = pci_user_write_config_dword(dev, vpd->cap + PCI_VPD_DATA, val);
+		if (ret < 0)
+			break;
+		ret = pci_user_write_config_word(dev, vpd->cap + PCI_VPD_ADDR,
+						 pos | PCI_VPD_ADDR_F);
+		if (ret < 0)
+			break;
+
+		vpd->busy = true;
+		vpd->flag = 0;
+		ret = pci_vpd_pci22_wait(dev);
+
+		pos += sizeof(u32);
+	}
+out:
+	mutex_unlock(&vpd->lock);
+	return ret ? ret : count;
 }
 
 static void pci_vpd_pci22_release(struct pci_dev *dev)
@@ -259,7 +312,7 @@ static void pci_vpd_pci22_release(struct pci_dev *dev)
 	kfree(container_of(dev->vpd, struct pci_vpd_pci22, base));
 }
 
-static struct pci_vpd_ops pci_vpd_pci22_ops = {
+static const struct pci_vpd_ops pci_vpd_pci22_ops = {
 	.read = pci_vpd_pci22_read,
 	.write = pci_vpd_pci22_write,
 	.release = pci_vpd_pci22_release,
@@ -279,7 +332,7 @@ int pci_vpd_pci22_init(struct pci_dev *dev)
 
 	vpd->base.len = PCI_VPD_PCI22_SIZE;
 	vpd->base.ops = &pci_vpd_pci22_ops;
-	spin_lock_init(&vpd->lock);
+	mutex_init(&vpd->lock);
 	vpd->cap = cap;
 	vpd->busy = false;
 	dev->vpd = &vpd->base;
@@ -287,6 +340,29 @@ int pci_vpd_pci22_init(struct pci_dev *dev)
 }
 
 /**
+ * pci_vpd_truncate - Set available Vital Product Data size
+ * @dev:	pci device struct
+ * @size:	available memory in bytes
+ *
+ * Adjust size of available VPD area.
+ */
+int pci_vpd_truncate(struct pci_dev *dev, size_t size)
+{
+	if (!dev->vpd)
+		return -EINVAL;
+
+	/* limited by the access method */
+	if (size > dev->vpd->len)
+		return -EINVAL;
+
+	dev->vpd->len = size;
+	dev->vpd->attr->size = size;
+
+	return 0;
+}
+EXPORT_SYMBOL(pci_vpd_truncate);
+
+/**
  * pci_block_user_cfg_access - Block userspace PCI config reads/writes
  * @dev:	pci device struct
  *
diff --git a/drivers/pci/bus.c b/drivers/pci/bus.c
index 999cc4088b59..52b54f053be0 100644
--- a/drivers/pci/bus.c
+++ b/drivers/pci/bus.c
@@ -71,7 +71,7 @@ pci_bus_alloc_resource(struct pci_bus *bus, struct resource *res,
 }
 
 /**
- * add a single device
+ * pci_bus_add_device - add a single device
  * @dev: device to add
  *
  * This adds a single pci device to the global
@@ -91,6 +91,37 @@ int pci_bus_add_device(struct pci_dev *dev)
 }
 
 /**
+ * pci_bus_add_child - add a child bus
+ * @bus: bus to add
+ *
+ * This adds sysfs entries for a single bus
+ */
+int pci_bus_add_child(struct pci_bus *bus)
+{
+	int retval;
+
+	if (bus->bridge)
+		bus->dev.parent = bus->bridge;
+
+	retval = device_register(&bus->dev);
+	if (retval)
+		return retval;
+
+	bus->is_added = 1;
+
+	retval = device_create_file(&bus->dev, &dev_attr_cpuaffinity);
+	if (retval)
+		return retval;
+
+	retval = device_create_file(&bus->dev, &dev_attr_cpulistaffinity);
+
+	/* Create legacy_io and legacy_mem files for this bus */
+	pci_create_legacy_files(bus);
+
+	return retval;
+}
+
+/**
  * pci_bus_add_devices - insert newly discovered PCI devices
  * @bus: bus to check for new devices
  *
@@ -105,7 +136,7 @@ int pci_bus_add_device(struct pci_dev *dev)
 void pci_bus_add_devices(struct pci_bus *bus)
 {
 	struct pci_dev *dev;
-	struct pci_bus *child_bus;
+	struct pci_bus *child;
 	int retval;
 
 	list_for_each_entry(dev, &bus->devices, bus_list) {
@@ -120,45 +151,29 @@ void pci_bus_add_devices(struct pci_bus *bus)
 	list_for_each_entry(dev, &bus->devices, bus_list) {
 		BUG_ON(!dev->is_added);
 
+		child = dev->subordinate;
 		/*
 		 * If there is an unattached subordinate bus, attach
 		 * it and then scan for unattached PCI devices.
 		 */
-		if (dev->subordinate) {
-		       if (list_empty(&dev->subordinate->node)) {
-			       down_write(&pci_bus_sem);
-			       list_add_tail(&dev->subordinate->node,
-					       &dev->bus->children);
-			       up_write(&pci_bus_sem);
-			}
-			pci_bus_add_devices(dev->subordinate);
-
-			/* register the bus with sysfs as the parent is now
-			 * properly registered. */
-			child_bus = dev->subordinate;
-			if (child_bus->is_added)
-				continue;
-			child_bus->dev.parent = child_bus->bridge;
-			retval = device_register(&child_bus->dev);
-			if (retval)
-				dev_err(&dev->dev, "Error registering pci_bus,"
-					" continuing...\n");
-			else {
-				child_bus->is_added = 1;
-				retval = device_create_file(&child_bus->dev,
-							&dev_attr_cpuaffinity);
-			}
-			if (retval)
-				dev_err(&dev->dev, "Error creating cpuaffinity"
-					" file, continuing...\n");
-
-			retval = device_create_file(&child_bus->dev,
-						&dev_attr_cpulistaffinity);
-			if (retval)
-				dev_err(&dev->dev,
-					"Error creating cpulistaffinity"
-					" file, continuing...\n");
+		if (!child)
+			continue;
+		if (list_empty(&child->node)) {
+			down_write(&pci_bus_sem);
+			list_add_tail(&child->node, &dev->bus->children);
+			up_write(&pci_bus_sem);
 		}
+		pci_bus_add_devices(child);
+
+		/*
+		 * register the bus with sysfs as the parent is now
+		 * properly registered.
+		 */
+		if (child->is_added)
+			continue;
+		retval = pci_bus_add_child(child);
+		if (retval)
+			dev_err(&dev->dev, "Error adding bus, continuing\n");
 	}
 }
 
diff --git a/drivers/pci/hotplug/Makefile b/drivers/pci/hotplug/Makefile
index 9bdbe1a6688f..e31fb91652ce 100644
--- a/drivers/pci/hotplug/Makefile
+++ b/drivers/pci/hotplug/Makefile
@@ -55,6 +55,9 @@ pciehp-objs		:=	pciehp_core.o	\
 				pciehp_ctrl.o	\
 				pciehp_pci.o	\
 				pciehp_hpc.o
+ifdef CONFIG_ACPI
+pciehp-objs		+=	pciehp_acpi.o
+endif
 
 shpchp-objs		:=	shpchp_core.o	\
 				shpchp_ctrl.o	\
diff --git a/drivers/pci/hotplug/acpi_pcihp.c b/drivers/pci/hotplug/acpi_pcihp.c
index e17ef54f0efc..c62ab8d240aa 100644
--- a/drivers/pci/hotplug/acpi_pcihp.c
+++ b/drivers/pci/hotplug/acpi_pcihp.c
@@ -501,5 +501,74 @@ int acpi_root_bridge(acpi_handle handle)
 }
 EXPORT_SYMBOL_GPL(acpi_root_bridge);
 
+
+static int is_ejectable(acpi_handle handle)
+{
+	acpi_status status;
+	acpi_handle tmp;
+	unsigned long long removable;
+	status = acpi_get_handle(handle, "_ADR", &tmp);
+	if (ACPI_FAILURE(status))
+		return 0;
+	status = acpi_get_handle(handle, "_EJ0", &tmp);
+	if (ACPI_SUCCESS(status))
+		return 1;
+	status = acpi_evaluate_integer(handle, "_RMV", NULL, &removable);
+	if (ACPI_SUCCESS(status) && removable)
+		return 1;
+	return 0;
+}
+
+/**
+ * acpi_pcihp_check_ejectable - check if handle is ejectable ACPI PCI slot
+ * @pbus: the PCI bus of the PCI slot corresponding to 'handle'
+ * @handle: ACPI handle to check
+ *
+ * Return 1 if handle is ejectable PCI slot, 0 otherwise.
+ */
+int acpi_pci_check_ejectable(struct pci_bus *pbus, acpi_handle handle)
+{
+	acpi_handle bridge_handle, parent_handle;
+
+	if (!(bridge_handle = acpi_pci_get_bridge_handle(pbus)))
+		return 0;
+	if ((ACPI_FAILURE(acpi_get_parent(handle, &parent_handle))))
+		return 0;
+	if (bridge_handle != parent_handle)
+		return 0;
+	return is_ejectable(handle);
+}
+EXPORT_SYMBOL_GPL(acpi_pci_check_ejectable);
+
+static acpi_status
+check_hotplug(acpi_handle handle, u32 lvl, void *context, void **rv)
+{
+	int *found = (int *)context;
+	if (is_ejectable(handle)) {
+		*found = 1;
+		return AE_CTRL_TERMINATE;
+	}
+	return AE_OK;
+}
+
+/**
+ * acpi_pci_detect_ejectable - check if the PCI bus has ejectable slots
+ * @pbus - PCI bus to scan
+ *
+ * Returns 1 if the PCI bus has ACPI based ejectable slots, 0 otherwise.
+ */
+int acpi_pci_detect_ejectable(struct pci_bus *pbus)
+{
+	acpi_handle handle;
+	int found = 0;
+
+	if (!(handle = acpi_pci_get_bridge_handle(pbus)))
+		return 0;
+	acpi_walk_namespace(ACPI_TYPE_DEVICE, handle, (u32)1,
+			    check_hotplug, (void *)&found, NULL);
+	return found;
+}
+EXPORT_SYMBOL_GPL(acpi_pci_detect_ejectable);
+
 module_param(debug_acpi, bool, 0644);
 MODULE_PARM_DESC(debug_acpi, "Debugging mode for ACPI enabled or not");
diff --git a/drivers/pci/hotplug/acpiphp.h b/drivers/pci/hotplug/acpiphp.h
index 9bcb6cbd5aa9..4fc168b70095 100644
--- a/drivers/pci/hotplug/acpiphp.h
+++ b/drivers/pci/hotplug/acpiphp.h
@@ -44,7 +44,7 @@
 	do {							\
 		if (acpiphp_debug)				\
 			printk(KERN_DEBUG "%s: " format,	\
-				MY_NAME , ## arg); 		\
+				MY_NAME , ## arg);		\
 	} while (0)
 #define err(format, arg...) printk(KERN_ERR "%s: " format, MY_NAME , ## arg)
 #define info(format, arg...) printk(KERN_INFO "%s: " format, MY_NAME , ## arg)
diff --git a/drivers/pci/hotplug/acpiphp_glue.c b/drivers/pci/hotplug/acpiphp_glue.c
index 3affc6472e65..f09b1010d477 100644
--- a/drivers/pci/hotplug/acpiphp_glue.c
+++ b/drivers/pci/hotplug/acpiphp_glue.c
@@ -46,6 +46,7 @@
 #include <linux/kernel.h>
 #include <linux/pci.h>
 #include <linux/pci_hotplug.h>
+#include <linux/pci-acpi.h>
 #include <linux/mutex.h>
 
 #include "../pci.h"
@@ -62,61 +63,6 @@ static void acpiphp_sanitize_bus(struct pci_bus *bus);
 static void acpiphp_set_hpp_values(acpi_handle handle, struct pci_bus *bus);
 static void handle_hotplug_event_func(acpi_handle handle, u32 type, void *context);
 
-
-/*
- * initialization & terminatation routines
- */
-
-/**
- * is_ejectable - determine if a slot is ejectable
- * @handle: handle to acpi namespace
- *
- * Ejectable slot should satisfy at least these conditions:
- *
- *  1. has _ADR method
- *  2. has _EJ0 method
- *
- * optionally
- *
- *  1. has _STA method
- *  2. has _PS0 method
- *  3. has _PS3 method
- *  4. ..
- */
-static int is_ejectable(acpi_handle handle)
-{
-	acpi_status status;
-	acpi_handle tmp;
-
-	status = acpi_get_handle(handle, "_ADR", &tmp);
-	if (ACPI_FAILURE(status)) {
-		return 0;
-	}
-
-	status = acpi_get_handle(handle, "_EJ0", &tmp);
-	if (ACPI_FAILURE(status)) {
-		return 0;
-	}
-
-	return 1;
-}
-
-
-/* callback routine to check for the existence of ejectable slots */
-static acpi_status
-is_ejectable_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
-{
-	int *count = (int *)context;
-
-	if (is_ejectable(handle)) {
-		(*count)++;
-		/* only one ejectable slot is enough */
-		return AE_CTRL_TERMINATE;
-	} else {
-		return AE_OK;
-	}
-}
-
 /* callback routine to check for the existence of a pci dock device */
 static acpi_status
 is_pci_dock_device(acpi_handle handle, u32 lvl, void *context, void **rv)
@@ -131,9 +77,6 @@ is_pci_dock_device(acpi_handle handle, u32 lvl, void *context, void **rv)
 	}
 }
 
-
-
-
 /*
  * the _DCK method can do funny things... and sometimes not
  * hah-hah funny.
@@ -160,9 +103,9 @@ static int post_dock_fixups(struct notifier_block *nb, unsigned long val,
 
 	if (((buses >> 8) & 0xff) != bus->secondary) {
 		buses = (buses & 0xff000000)
-	     		| ((unsigned int)(bus->primary)     <<  0)
-	     		| ((unsigned int)(bus->secondary)   <<  8)
-	     		| ((unsigned int)(bus->subordinate) << 16);
+			| ((unsigned int)(bus->primary)     <<  0)
+			| ((unsigned int)(bus->secondary)   <<  8)
+			| ((unsigned int)(bus->subordinate) << 16);
 		pci_write_config_dword(bus->self, PCI_PRIMARY_BUS, buses);
 	}
 	return NOTIFY_OK;
@@ -184,17 +127,12 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 	acpi_status status = AE_OK;
 	unsigned long long adr, sun;
 	int device, function, retval;
+	struct pci_bus *pbus = bridge->pci_bus;
 
-	status = acpi_evaluate_integer(handle, "_ADR", NULL, &adr);
-
-	if (ACPI_FAILURE(status))
-		return AE_OK;
-
-	status = acpi_get_handle(handle, "_EJ0", &tmp);
-
-	if (ACPI_FAILURE(status) && !(is_dock_device(handle)))
+	if (!acpi_pci_check_ejectable(pbus, handle) && !is_dock_device(handle))
 		return AE_OK;
 
+	acpi_evaluate_integer(handle, "_ADR", NULL, &adr);
 	device = (adr >> 16) & 0xffff;
 	function = adr & 0xffff;
 
@@ -205,7 +143,8 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 	INIT_LIST_HEAD(&newfunc->sibling);
 	newfunc->handle = handle;
 	newfunc->function = function;
-	if (ACPI_SUCCESS(status))
+
+	if (ACPI_SUCCESS(acpi_get_handle(handle, "_EJ0", &tmp)))
 		newfunc->flags = FUNC_HAS_EJ0;
 
 	if (ACPI_SUCCESS(acpi_get_handle(handle, "_STA", &tmp)))
@@ -256,8 +195,7 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 		bridge->nr_slots++;
 
 		dbg("found ACPI PCI Hotplug slot %llu at PCI %04x:%02x:%02x\n",
-				slot->sun, pci_domain_nr(bridge->pci_bus),
-				bridge->pci_bus->number, slot->device);
+		    slot->sun, pci_domain_nr(pbus), pbus->number, device);
 		retval = acpiphp_register_hotplug_slot(slot);
 		if (retval) {
 			if (retval == -EBUSY)
@@ -274,8 +212,7 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 	list_add_tail(&newfunc->sibling, &slot->funcs);
 
 	/* associate corresponding pci_dev */
-	newfunc->pci_dev = pci_get_slot(bridge->pci_bus,
-					 PCI_DEVFN(device, function));
+	newfunc->pci_dev = pci_get_slot(pbus, PCI_DEVFN(device, function));
 	if (newfunc->pci_dev) {
 		slot->flags |= (SLOT_ENABLED | SLOT_POWEREDON);
 	}
@@ -324,27 +261,15 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 
 
 /* see if it's worth looking at this bridge */
-static int detect_ejectable_slots(acpi_handle *bridge_handle)
+static int detect_ejectable_slots(struct pci_bus *pbus)
 {
-	acpi_status status;
-	int count;
-
-	count = 0;
-
-	/* only check slots defined directly below bridge object */
-	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, bridge_handle, (u32)1,
-				     is_ejectable_slot, (void *)&count, NULL);
-
-	/*
-	 * we also need to add this bridge if there is a dock bridge or
-	 * other pci device on a dock station (removable)
-	 */
-	if (!count)
-		status = acpi_walk_namespace(ACPI_TYPE_DEVICE, bridge_handle,
-				(u32)1, is_pci_dock_device, (void *)&count,
-				NULL);
-
-	return count;
+	int found = acpi_pci_detect_ejectable(pbus);
+	if (!found) {
+		acpi_handle bridge_handle = acpi_pci_get_bridge_handle(pbus);
+		acpi_walk_namespace(ACPI_TYPE_DEVICE, bridge_handle, (u32)1,
+				    is_pci_dock_device, (void *)&found, NULL);
+	}
+	return found;
 }
 
 
@@ -554,7 +479,7 @@ find_p2p_bridge(acpi_handle handle, u32 lvl, void *context, void **rv)
 		goto out;
 
 	/* check if this bridge has ejectable slots */
-	if ((detect_ejectable_slots(handle) > 0)) {
+	if ((detect_ejectable_slots(dev->subordinate) > 0)) {
 		dbg("found PCI-to-PCI bridge at PCI %s\n", pci_name(dev));
 		add_p2p_bridge(handle, dev);
 	}
@@ -615,7 +540,7 @@ static int add_bridge(acpi_handle handle)
 	}
 
 	/* check if this bridge has ejectable slots */
-	if (detect_ejectable_slots(handle) > 0) {
+	if (detect_ejectable_slots(pci_bus) > 0) {
 		dbg("found PCI host-bus bridge with hot-pluggable slots\n");
 		add_host_bridge(handle, pci_bus);
 	}
diff --git a/drivers/pci/hotplug/acpiphp_ibm.c b/drivers/pci/hotplug/acpiphp_ibm.c
index 881fdd2b7313..5befa7e379b7 100644
--- a/drivers/pci/hotplug/acpiphp_ibm.c
+++ b/drivers/pci/hotplug/acpiphp_ibm.c
@@ -271,7 +271,7 @@ static void ibm_handle_events(acpi_handle handle, u32 event, void *context)
 		dbg("%s: generationg bus event\n", __func__);
 		acpi_bus_generate_proc_event(note->device, note->event, detail);
 		acpi_bus_generate_netlink_event(note->device->pnp.device_class,
-						  note->device->dev.bus_id,
+						  dev_name(&note->device->dev),
 						  note->event, detail);
 	} else
 		note->event = event;
diff --git a/drivers/pci/hotplug/cpqphp_ctrl.c b/drivers/pci/hotplug/cpqphp_ctrl.c
index a60a25290995..cc227a8c4b11 100644
--- a/drivers/pci/hotplug/cpqphp_ctrl.c
+++ b/drivers/pci/hotplug/cpqphp_ctrl.c
@@ -1954,7 +1954,7 @@ void cpqhp_pushbutton_thread(unsigned long slot)
 			return ;
 		}
 
-		if (func != NULL && ctrl != NULL) {
+		if (ctrl != NULL) {
 			if (cpqhp_process_SI(ctrl, func) != 0) {
 				amber_LED_on(ctrl, hp_slot);
 				green_LED_off(ctrl, hp_slot);
@@ -2604,7 +2604,7 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 			for (cloop = 0; cloop < 4; cloop++) {
 				if (irqs.valid_INT & (0x01 << cloop)) {
 					rc = cpqhp_set_irq(func->bus, func->device,
-							   0x0A + cloop, irqs.interrupt[cloop]);
+							   cloop + 1, irqs.interrupt[cloop]);
 					if (rc)
 						goto free_and_out;
 				}
@@ -2945,7 +2945,7 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 		}
 
 		if (!behind_bridge) {
-			rc = cpqhp_set_irq(func->bus, func->device, temp_byte + 0x09, IRQ);
+			rc = cpqhp_set_irq(func->bus, func->device, temp_byte, IRQ);
 			if (rc)
 				return 1;
 		} else {
diff --git a/drivers/pci/hotplug/cpqphp_pci.c b/drivers/pci/hotplug/cpqphp_pci.c
index df146be9d2e9..6c0ed0fcb8ee 100644
--- a/drivers/pci/hotplug/cpqphp_pci.c
+++ b/drivers/pci/hotplug/cpqphp_pci.c
@@ -171,7 +171,7 @@ int cpqhp_set_irq (u8 bus_num, u8 dev_num, u8 int_pin, u8 irq_num)
 		fakebus->number = bus_num;
 		dbg("%s: dev %d, bus %d, pin %d, num %d\n",
 		    __func__, dev_num, bus_num, int_pin, irq_num);
-		rc = pcibios_set_irq_routing(fakedev, int_pin - 0x0a, irq_num);
+		rc = pcibios_set_irq_routing(fakedev, int_pin - 1, irq_num);
 		kfree(fakedev);
 		kfree(fakebus);
 		dbg("%s: rc %d\n", __func__, rc);
diff --git a/drivers/pci/hotplug/fakephp.c b/drivers/pci/hotplug/fakephp.c
index 3a2637a00934..b0e7de9e536d 100644
--- a/drivers/pci/hotplug/fakephp.c
+++ b/drivers/pci/hotplug/fakephp.c
@@ -324,6 +324,7 @@ static int disable_slot(struct hotplug_slot *slot)
 
 		if (test_and_set_bit(0, &dslot->removed)) {
 			dbg("Slot already scheduled for removal\n");
+			pci_dev_put(dev);
 			return -ENODEV;
 		}
 
diff --git a/drivers/pci/hotplug/pciehp.h b/drivers/pci/hotplug/pciehp.h
index b2801a7ee37f..27fd18f019f8 100644
--- a/drivers/pci/hotplug/pciehp.h
+++ b/drivers/pci/hotplug/pciehp.h
@@ -220,11 +220,23 @@ struct hpc_ops {
 #include <acpi/actypes.h>
 #include <linux/pci-acpi.h>
 
+extern void __init pciehp_acpi_slot_detection_init(void);
+extern int pciehp_acpi_slot_detection_check(struct pci_dev *dev);
+
+static inline void pciehp_firmware_init(void)
+{
+	pciehp_acpi_slot_detection_init();
+}
+
 static inline int pciehp_get_hp_hw_control_from_firmware(struct pci_dev *dev)
 {
+	int retval;
 	u32 flags = (OSC_PCI_EXPRESS_NATIVE_HP_CONTROL |
 		     OSC_PCI_EXPRESS_CAP_STRUCTURE_CONTROL);
-	return acpi_get_hp_hw_control_from_firmware(dev, flags);
+	retval = acpi_get_hp_hw_control_from_firmware(dev, flags);
+	if (retval)
+		return retval;
+	return pciehp_acpi_slot_detection_check(dev);
 }
 
 static inline int pciehp_get_hp_params_from_firmware(struct pci_dev *dev,
@@ -235,6 +247,7 @@ static inline int pciehp_get_hp_params_from_firmware(struct pci_dev *dev,
 	return 0;
 }
 #else
+#define pciehp_firmware_init()				do {} while (0)
 #define pciehp_get_hp_hw_control_from_firmware(dev) 	0
 #define pciehp_get_hp_params_from_firmware(dev, hpp)    (-ENODEV)
 #endif 				/* CONFIG_ACPI */
diff --git a/drivers/pci/hotplug/pciehp_acpi.c b/drivers/pci/hotplug/pciehp_acpi.c
new file mode 100644
index 000000000000..438d795f9fe3
--- /dev/null
+++ b/drivers/pci/hotplug/pciehp_acpi.c
@@ -0,0 +1,141 @@
+/*
+ * ACPI related functions for PCI Express Hot Plug driver.
+ *
+ * Copyright (C) 2008 Kenji Kaneshige
+ * Copyright (C) 2008 Fujitsu Limited.
+ *
+ * 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.
+ *
+ * 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/acpi.h>
+#include <linux/pci.h>
+#include <linux/pci_hotplug.h>
+#include "pciehp.h"
+
+#define PCIEHP_DETECT_PCIE	(0)
+#define PCIEHP_DETECT_ACPI	(1)
+#define PCIEHP_DETECT_AUTO	(2)
+#define PCIEHP_DETECT_DEFAULT	PCIEHP_DETECT_AUTO
+
+static int slot_detection_mode;
+static char *pciehp_detect_mode;
+module_param(pciehp_detect_mode, charp, 0444);
+MODULE_PARM_DESC(pciehp_detect_mode,
+	 "Slot detection mode: pcie, acpi, auto\n"
+	 "  pcie          - Use PCIe based slot detection\n"
+	 "  acpi          - Use ACPI for slot detection\n"
+	 "  auto(default) - Auto select mode. Use acpi option if duplicate\n"
+	 "                  slot ids are found. Otherwise, use pcie option\n");
+
+int pciehp_acpi_slot_detection_check(struct pci_dev *dev)
+{
+	if (slot_detection_mode != PCIEHP_DETECT_ACPI)
+		return 0;
+	if (acpi_pci_detect_ejectable(dev->subordinate))
+		return 0;
+	return -ENODEV;
+}
+
+static int __init parse_detect_mode(void)
+{
+	if (!pciehp_detect_mode)
+		return PCIEHP_DETECT_DEFAULT;
+	if (!strcmp(pciehp_detect_mode, "pcie"))
+		return PCIEHP_DETECT_PCIE;
+	if (!strcmp(pciehp_detect_mode, "acpi"))
+		return PCIEHP_DETECT_ACPI;
+	if (!strcmp(pciehp_detect_mode, "auto"))
+		return PCIEHP_DETECT_AUTO;
+	warn("bad specifier '%s' for pciehp_detect_mode. Use default\n",
+	     pciehp_detect_mode);
+	return PCIEHP_DETECT_DEFAULT;
+}
+
+static struct pcie_port_service_id __initdata port_pci_ids[] = {
+	{
+		.vendor = PCI_ANY_ID,
+		.device = PCI_ANY_ID,
+		.port_type = PCIE_ANY_PORT,
+		.service_type = PCIE_PORT_SERVICE_HP,
+		.driver_data =  0,
+        }, { /* end: all zeroes */ }
+};
+
+static int __initdata dup_slot_id;
+static int __initdata acpi_slot_detected;
+static struct list_head __initdata dummy_slots = LIST_HEAD_INIT(dummy_slots);
+
+/* Dummy driver for dumplicate name detection */
+static int __init dummy_probe(struct pcie_device *dev,
+			      const struct pcie_port_service_id *id)
+{
+	int pos;
+	u32 slot_cap;
+	struct slot *slot, *tmp;
+	struct pci_dev *pdev = dev->port;
+	struct pci_bus *pbus = pdev->subordinate;
+	if (!(slot = kzalloc(sizeof(*slot), GFP_KERNEL)))
+		return -ENOMEM;
+	/* Note: pciehp_detect_mode != PCIEHP_DETECT_ACPI here */
+	if (pciehp_get_hp_hw_control_from_firmware(pdev))
+		return -ENODEV;
+	if (!(pos = pci_find_capability(pdev, PCI_CAP_ID_EXP)))
+		return -ENODEV;
+	pci_read_config_dword(pdev, pos + PCI_EXP_SLTCAP, &slot_cap);
+	slot->number = slot_cap >> 19;
+	list_for_each_entry(tmp, &dummy_slots, slot_list) {
+		if (tmp->number == slot->number)
+			dup_slot_id++;
+	}
+	list_add_tail(&slot->slot_list, &dummy_slots);
+	if (!acpi_slot_detected && acpi_pci_detect_ejectable(pbus))
+		acpi_slot_detected = 1;
+	return -ENODEV;         /* dummy driver always returns error */
+}
+
+static struct pcie_port_service_driver __initdata dummy_driver = {
+        .name           = "pciehp_dummy",
+        .id_table       = port_pci_ids,
+        .probe          = dummy_probe,
+};
+
+static int __init select_detection_mode(void)
+{
+	struct slot *slot, *tmp;
+	pcie_port_service_register(&dummy_driver);
+	pcie_port_service_unregister(&dummy_driver);
+	list_for_each_entry_safe(slot, tmp, &dummy_slots, slot_list) {
+		list_del(&slot->slot_list);
+		kfree(slot);
+	}
+	if (acpi_slot_detected && dup_slot_id)
+		return PCIEHP_DETECT_ACPI;
+	return PCIEHP_DETECT_PCIE;
+}
+
+void __init pciehp_acpi_slot_detection_init(void)
+{
+	slot_detection_mode = parse_detect_mode();
+	if (slot_detection_mode != PCIEHP_DETECT_AUTO)
+		goto out;
+	slot_detection_mode = select_detection_mode();
+out:
+	if (slot_detection_mode == PCIEHP_DETECT_ACPI)
+		info("Using ACPI for slot detection.\n");
+}
diff --git a/drivers/pci/hotplug/pciehp_core.c b/drivers/pci/hotplug/pciehp_core.c
index 39cf248d24e3..5482d4ed8256 100644
--- a/drivers/pci/hotplug/pciehp_core.c
+++ b/drivers/pci/hotplug/pciehp_core.c
@@ -522,6 +522,7 @@ static int __init pcied_init(void)
 {
 	int retval = 0;
 
+	pciehp_firmware_init();
 	retval = pcie_port_service_register(&hpdriver_portdrv);
  	dbg("pcie_port_service_register = %d\n", retval);
   	info(DRIVER_DESC " version: " DRIVER_VERSION "\n");
diff --git a/drivers/pci/hotplug/pciehp_ctrl.c b/drivers/pci/hotplug/pciehp_ctrl.c
index fead63c6b49e..ff4034502d24 100644
--- a/drivers/pci/hotplug/pciehp_ctrl.c
+++ b/drivers/pci/hotplug/pciehp_ctrl.c
@@ -178,15 +178,14 @@ static void set_slot_off(struct controller *ctrl, struct slot * pslot)
 				 "Issue of Slot Power Off command failed\n");
 			return;
 		}
+		/*
+		 * After turning power off, we must wait for at least 1 second
+		 * before taking any action that relies on power having been
+		 * removed from the slot/adapter.
+		 */
+		msleep(1000);
 	}
 
-	/*
-	 * After turning power off, we must wait for at least 1 second
-	 * before taking any action that relies on power having been
-	 * removed from the slot/adapter.
-	 */
-	msleep(1000);
-
 	if (PWR_LED(ctrl))
 		pslot->hpc_ops->green_led_off(pslot);
 
@@ -286,15 +285,14 @@ static int remove_board(struct slot *p_slot)
 				 "Issue of Slot Disable command failed\n");
 			return retval;
 		}
+		/*
+		 * After turning power off, we must wait for at least 1 second
+		 * before taking any action that relies on power having been
+		 * removed from the slot/adapter.
+		 */
+		msleep(1000);
 	}
 
-	/*
-	 * After turning power off, we must wait for at least 1 second
-	 * before taking any action that relies on power having been
-	 * removed from the slot/adapter.
-	 */
-	msleep(1000);
-
 	if (PWR_LED(ctrl))
 		/* turn off Green LED */
 		p_slot->hpc_ops->green_led_off(p_slot);
diff --git a/drivers/pci/hotplug/pciehp_hpc.c b/drivers/pci/hotplug/pciehp_hpc.c
index b643ca13e4f1..71a8012886b0 100644
--- a/drivers/pci/hotplug/pciehp_hpc.c
+++ b/drivers/pci/hotplug/pciehp_hpc.c
@@ -42,42 +42,6 @@
 
 static atomic_t pciehp_num_controllers = ATOMIC_INIT(0);
 
-struct ctrl_reg {
-	u8 cap_id;
-	u8 nxt_ptr;
-	u16 cap_reg;
-	u32 dev_cap;
-	u16 dev_ctrl;
-	u16 dev_status;
-	u32 lnk_cap;
-	u16 lnk_ctrl;
-	u16 lnk_status;
-	u32 slot_cap;
-	u16 slot_ctrl;
-	u16 slot_status;
-	u16 root_ctrl;
-	u16 rsvp;
-	u32 root_status;
-} __attribute__ ((packed));
-
-/* offsets to the controller registers based on the above structure layout */
-enum ctrl_offsets {
-	PCIECAPID	=	offsetof(struct ctrl_reg, cap_id),
-	NXTCAPPTR	=	offsetof(struct ctrl_reg, nxt_ptr),
-	CAPREG		=	offsetof(struct ctrl_reg, cap_reg),
-	DEVCAP		=	offsetof(struct ctrl_reg, dev_cap),
-	DEVCTRL		=	offsetof(struct ctrl_reg, dev_ctrl),
-	DEVSTATUS	=	offsetof(struct ctrl_reg, dev_status),
-	LNKCAP		=	offsetof(struct ctrl_reg, lnk_cap),
-	LNKCTRL		=	offsetof(struct ctrl_reg, lnk_ctrl),
-	LNKSTATUS	=	offsetof(struct ctrl_reg, lnk_status),
-	SLOTCAP		=	offsetof(struct ctrl_reg, slot_cap),
-	SLOTCTRL	=	offsetof(struct ctrl_reg, slot_ctrl),
-	SLOTSTATUS	=	offsetof(struct ctrl_reg, slot_status),
-	ROOTCTRL	=	offsetof(struct ctrl_reg, root_ctrl),
-	ROOTSTATUS	=	offsetof(struct ctrl_reg, root_status),
-};
-
 static inline int pciehp_readw(struct controller *ctrl, int reg, u16 *value)
 {
 	struct pci_dev *dev = ctrl->pci_dev;
@@ -102,95 +66,9 @@ static inline int pciehp_writel(struct controller *ctrl, int reg, u32 value)
 	return pci_write_config_dword(dev, ctrl->cap_base + reg, value);
 }
 
-/* Field definitions in PCI Express Capabilities Register */
-#define CAP_VER			0x000F
-#define DEV_PORT_TYPE		0x00F0
-#define SLOT_IMPL		0x0100
-#define MSG_NUM			0x3E00
-
-/* Device or Port Type */
-#define NAT_ENDPT		0x00
-#define LEG_ENDPT		0x01
-#define ROOT_PORT		0x04
-#define UP_STREAM		0x05
-#define	DN_STREAM		0x06
-#define PCIE_PCI_BRDG		0x07
-#define PCI_PCIE_BRDG		0x10
-
-/* Field definitions in Device Capabilities Register */
-#define DATTN_BUTTN_PRSN	0x1000
-#define DATTN_LED_PRSN		0x2000
-#define DPWR_LED_PRSN		0x4000
-
-/* Field definitions in Link Capabilities Register */
-#define MAX_LNK_SPEED		0x000F
-#define MAX_LNK_WIDTH		0x03F0
-#define LINK_ACTIVE_REPORTING	0x00100000
-
-/* Link Width Encoding */
-#define LNK_X1		0x01
-#define LNK_X2		0x02
-#define LNK_X4		0x04
-#define LNK_X8		0x08
-#define LNK_X12		0x0C
-#define LNK_X16		0x10
-#define LNK_X32		0x20
-
-/*Field definitions of Link Status Register */
-#define LNK_SPEED	0x000F
-#define NEG_LINK_WD	0x03F0
-#define LNK_TRN_ERR	0x0400
-#define	LNK_TRN		0x0800
-#define SLOT_CLK_CONF	0x1000
-#define LINK_ACTIVE	0x2000
-
-/* Field definitions in Slot Capabilities Register */
-#define ATTN_BUTTN_PRSN	0x00000001
-#define	PWR_CTRL_PRSN	0x00000002
-#define MRL_SENS_PRSN	0x00000004
-#define ATTN_LED_PRSN	0x00000008
-#define PWR_LED_PRSN	0x00000010
-#define HP_SUPR_RM_SUP	0x00000020
-#define HP_CAP		0x00000040
-#define SLOT_PWR_VALUE	0x000003F8
-#define SLOT_PWR_LIMIT	0x00000C00
-#define PSN		0xFFF80000	/* PSN: Physical Slot Number */
-
-/* Field definitions in Slot Control Register */
-#define ATTN_BUTTN_ENABLE		0x0001
-#define PWR_FAULT_DETECT_ENABLE		0x0002
-#define MRL_DETECT_ENABLE		0x0004
-#define PRSN_DETECT_ENABLE		0x0008
-#define CMD_CMPL_INTR_ENABLE		0x0010
-#define HP_INTR_ENABLE			0x0020
-#define ATTN_LED_CTRL			0x00C0
-#define PWR_LED_CTRL			0x0300
-#define PWR_CTRL			0x0400
-#define EMI_CTRL			0x0800
-
-/* Attention indicator and Power indicator states */
-#define LED_ON		0x01
-#define LED_BLINK	0x10
-#define LED_OFF		0x11
-
 /* Power Control Command */
 #define POWER_ON	0
-#define POWER_OFF	0x0400
-
-/* EMI Status defines */
-#define EMI_DISENGAGED	0
-#define EMI_ENGAGED	1
-
-/* Field definitions in Slot Status Register */
-#define ATTN_BUTTN_PRESSED	0x0001
-#define PWR_FAULT_DETECTED	0x0002
-#define MRL_SENS_CHANGED	0x0004
-#define PRSN_DETECT_CHANGED	0x0008
-#define CMD_COMPLETED		0x0010
-#define MRL_STATE		0x0020
-#define PRSN_STATE		0x0040
-#define EMI_STATE		0x0080
-#define EMI_STATUS_BIT		7
+#define POWER_OFF	PCI_EXP_SLTCTL_PCC
 
 static irqreturn_t pcie_isr(int irq, void *dev_id);
 static void start_int_poll_timer(struct controller *ctrl, int sec);
@@ -253,22 +131,20 @@ static inline void pciehp_free_irq(struct controller *ctrl)
 static int pcie_poll_cmd(struct controller *ctrl)
 {
 	u16 slot_status;
-	int timeout = 1000;
+	int err, timeout = 1000;
 
-	if (!pciehp_readw(ctrl, SLOTSTATUS, &slot_status)) {
-		if (slot_status & CMD_COMPLETED) {
-			pciehp_writew(ctrl, SLOTSTATUS, CMD_COMPLETED);
-			return 1;
-		}
+	err = pciehp_readw(ctrl, PCI_EXP_SLTSTA, &slot_status);
+	if (!err && (slot_status & PCI_EXP_SLTSTA_CC)) {
+		pciehp_writew(ctrl, PCI_EXP_SLTSTA, PCI_EXP_SLTSTA_CC);
+		return 1;
 	}
 	while (timeout > 0) {
 		msleep(10);
 		timeout -= 10;
-		if (!pciehp_readw(ctrl, SLOTSTATUS, &slot_status)) {
-			if (slot_status & CMD_COMPLETED) {
-				pciehp_writew(ctrl, SLOTSTATUS, CMD_COMPLETED);
-				return 1;
-			}
+		err = pciehp_readw(ctrl, PCI_EXP_SLTSTA, &slot_status);
+		if (!err && (slot_status & PCI_EXP_SLTSTA_CC)) {
+			pciehp_writew(ctrl, PCI_EXP_SLTSTA, PCI_EXP_SLTSTA_CC);
+			return 1;
 		}
 	}
 	return 0;	/* timeout */
@@ -302,14 +178,14 @@ static int pcie_write_cmd(struct controller *ctrl, u16 cmd, u16 mask)
 
 	mutex_lock(&ctrl->ctrl_lock);
 
-	retval = pciehp_readw(ctrl, SLOTSTATUS, &slot_status);
+	retval = pciehp_readw(ctrl, PCI_EXP_SLTSTA, &slot_status);
 	if (retval) {
 		ctrl_err(ctrl, "%s: Cannot read SLOTSTATUS register\n",
 			 __func__);
 		goto out;
 	}
 
-	if (slot_status & CMD_COMPLETED) {
+	if (slot_status & PCI_EXP_SLTSTA_CC) {
 		if (!ctrl->no_cmd_complete) {
 			/*
 			 * After 1 sec and CMD_COMPLETED still not set, just
@@ -332,7 +208,7 @@ static int pcie_write_cmd(struct controller *ctrl, u16 cmd, u16 mask)
 		}
 	}
 
-	retval = pciehp_readw(ctrl, SLOTCTRL, &slot_ctrl);
+	retval = pciehp_readw(ctrl, PCI_EXP_SLTCTL, &slot_ctrl);
 	if (retval) {
 		ctrl_err(ctrl, "%s: Cannot read SLOTCTRL register\n", __func__);
 		goto out;
@@ -342,7 +218,7 @@ static int pcie_write_cmd(struct controller *ctrl, u16 cmd, u16 mask)
 	slot_ctrl |= (cmd & mask);
 	ctrl->cmd_busy = 1;
 	smp_mb();
-	retval = pciehp_writew(ctrl, SLOTCTRL, slot_ctrl);
+	retval = pciehp_writew(ctrl, PCI_EXP_SLTCTL, slot_ctrl);
 	if (retval)
 		ctrl_err(ctrl, "Cannot write to SLOTCTRL register\n");
 
@@ -356,8 +232,8 @@ static int pcie_write_cmd(struct controller *ctrl, u16 cmd, u16 mask)
 		 * completed interrupt is not enabled, we need to poll
 		 * command completed event.
 		 */
-		if (!(slot_ctrl & HP_INTR_ENABLE) ||
-		    !(slot_ctrl & CMD_CMPL_INTR_ENABLE))
+		if (!(slot_ctrl & PCI_EXP_SLTCTL_HPIE) ||
+		    !(slot_ctrl & PCI_EXP_SLTCTL_CCIE))
 			poll = 1;
                 pcie_wait_cmd(ctrl, poll);
 	}
@@ -370,9 +246,9 @@ static inline int check_link_active(struct controller *ctrl)
 {
 	u16 link_status;
 
-	if (pciehp_readw(ctrl, LNKSTATUS, &link_status))
+	if (pciehp_readw(ctrl, PCI_EXP_LNKSTA, &link_status))
 		return 0;
-	return !!(link_status & LINK_ACTIVE);
+	return !!(link_status & PCI_EXP_LNKSTA_DLLLA);
 }
 
 static void pcie_wait_link_active(struct controller *ctrl)
@@ -412,15 +288,15 @@ static int hpc_check_lnk_status(struct controller *ctrl)
         } else
                 msleep(1000);
 
-	retval = pciehp_readw(ctrl, LNKSTATUS, &lnk_status);
+	retval = pciehp_readw(ctrl, PCI_EXP_LNKSTA, &lnk_status);
 	if (retval) {
 		ctrl_err(ctrl, "Cannot read LNKSTATUS register\n");
 		return retval;
 	}
 
 	ctrl_dbg(ctrl, "%s: lnk_status = %x\n", __func__, lnk_status);
-	if ( (lnk_status & LNK_TRN) || (lnk_status & LNK_TRN_ERR) ||
-		!(lnk_status & NEG_LINK_WD)) {
+	if ((lnk_status & PCI_EXP_LNKSTA_LT) ||
+	    !(lnk_status & PCI_EXP_LNKSTA_NLW)) {
 		ctrl_err(ctrl, "Link Training Error occurs \n");
 		retval = -1;
 		return retval;
@@ -436,16 +312,16 @@ static int hpc_get_attention_status(struct slot *slot, u8 *status)
 	u8 atten_led_state;
 	int retval = 0;
 
-	retval = pciehp_readw(ctrl, SLOTCTRL, &slot_ctrl);
+	retval = pciehp_readw(ctrl, PCI_EXP_SLTCTL, &slot_ctrl);
 	if (retval) {
 		ctrl_err(ctrl, "%s: Cannot read SLOTCTRL register\n", __func__);
 		return retval;
 	}
 
 	ctrl_dbg(ctrl, "%s: SLOTCTRL %x, value read %x\n",
-		 __func__, ctrl->cap_base + SLOTCTRL, slot_ctrl);
+		 __func__, ctrl->cap_base + PCI_EXP_SLTCTL, slot_ctrl);
 
-	atten_led_state = (slot_ctrl & ATTN_LED_CTRL) >> 6;
+	atten_led_state = (slot_ctrl & PCI_EXP_SLTCTL_AIC) >> 6;
 
 	switch (atten_led_state) {
 	case 0:
@@ -475,15 +351,15 @@ static int hpc_get_power_status(struct slot *slot, u8 *status)
 	u8 pwr_state;
 	int	retval = 0;
 
-	retval = pciehp_readw(ctrl, SLOTCTRL, &slot_ctrl);
+	retval = pciehp_readw(ctrl, PCI_EXP_SLTCTL, &slot_ctrl);
 	if (retval) {
 		ctrl_err(ctrl, "%s: Cannot read SLOTCTRL register\n", __func__);
 		return retval;
 	}
 	ctrl_dbg(ctrl, "%s: SLOTCTRL %x value read %x\n",
-		 __func__, ctrl->cap_base + SLOTCTRL, slot_ctrl);
+		 __func__, ctrl->cap_base + PCI_EXP_SLTCTL, slot_ctrl);
 
-	pwr_state = (slot_ctrl & PWR_CTRL) >> 10;
+	pwr_state = (slot_ctrl & PCI_EXP_SLTCTL_PCC) >> 10;
 
 	switch (pwr_state) {
 	case 0:
@@ -504,17 +380,15 @@ static int hpc_get_latch_status(struct slot *slot, u8 *status)
 {
 	struct controller *ctrl = slot->ctrl;
 	u16 slot_status;
-	int retval = 0;
+	int retval;
 
-	retval = pciehp_readw(ctrl, SLOTSTATUS, &slot_status);
+	retval = pciehp_readw(ctrl, PCI_EXP_SLTSTA, &slot_status);
 	if (retval) {
 		ctrl_err(ctrl, "%s: Cannot read SLOTSTATUS register\n",
 			 __func__);
 		return retval;
 	}
-
-	*status = (((slot_status & MRL_STATE) >> 5) == 0) ? 0 : 1;
-
+	*status = !!(slot_status & PCI_EXP_SLTSTA_MRLSS);
 	return 0;
 }
 
@@ -522,18 +396,15 @@ static int hpc_get_adapter_status(struct slot *slot, u8 *status)
 {
 	struct controller *ctrl = slot->ctrl;
 	u16 slot_status;
-	u8 card_state;
-	int retval = 0;
+	int retval;
 
-	retval = pciehp_readw(ctrl, SLOTSTATUS, &slot_status);
+	retval = pciehp_readw(ctrl, PCI_EXP_SLTSTA, &slot_status);
 	if (retval) {
 		ctrl_err(ctrl, "%s: Cannot read SLOTSTATUS register\n",
 			 __func__);
 		return retval;
 	}
-	card_state = (u8)((slot_status & PRSN_STATE) >> 6);
-	*status = (card_state == 1) ? 1 : 0;
-
+	*status = !!(slot_status & PCI_EXP_SLTSTA_PDS);
 	return 0;
 }
 
@@ -541,32 +412,28 @@ static int hpc_query_power_fault(struct slot *slot)
 {
 	struct controller *ctrl = slot->ctrl;
 	u16 slot_status;
-	u8 pwr_fault;
-	int retval = 0;
+	int retval;
 
-	retval = pciehp_readw(ctrl, SLOTSTATUS, &slot_status);
+	retval = pciehp_readw(ctrl, PCI_EXP_SLTSTA, &slot_status);
 	if (retval) {
 		ctrl_err(ctrl, "Cannot check for power fault\n");
 		return retval;
 	}
-	pwr_fault = (u8)((slot_status & PWR_FAULT_DETECTED) >> 1);
-
-	return pwr_fault;
+	return !!(slot_status & PCI_EXP_SLTSTA_PFD);
 }
 
 static int hpc_get_emi_status(struct slot *slot, u8 *status)
 {
 	struct controller *ctrl = slot->ctrl;
 	u16 slot_status;
-	int retval = 0;
+	int retval;
 
-	retval = pciehp_readw(ctrl, SLOTSTATUS, &slot_status);
+	retval = pciehp_readw(ctrl, PCI_EXP_SLTSTA, &slot_status);
 	if (retval) {
 		ctrl_err(ctrl, "Cannot check EMI status\n");
 		return retval;
 	}
-	*status = (slot_status & EMI_STATE) >> EMI_STATUS_BIT;
-
+	*status = !!(slot_status & PCI_EXP_SLTSTA_EIS);
 	return retval;
 }
 
@@ -576,8 +443,8 @@ static int hpc_toggle_emi(struct slot *slot)
 	u16 cmd_mask;
 	int rc;
 
-	slot_cmd = EMI_CTRL;
-	cmd_mask = EMI_CTRL;
+	slot_cmd = PCI_EXP_SLTCTL_EIC;
+	cmd_mask = PCI_EXP_SLTCTL_EIC;
 	rc = pcie_write_cmd(slot->ctrl, slot_cmd, cmd_mask);
 	slot->last_emi_toggle = get_seconds();
 
@@ -591,7 +458,7 @@ static int hpc_set_attention_status(struct slot *slot, u8 value)
 	u16 cmd_mask;
 	int rc;
 
-	cmd_mask = ATTN_LED_CTRL;
+	cmd_mask = PCI_EXP_SLTCTL_AIC;
 	switch (value) {
 		case 0 :	/* turn off */
 			slot_cmd = 0x00C0;
@@ -607,7 +474,7 @@ static int hpc_set_attention_status(struct slot *slot, u8 value)
 	}
 	rc = pcie_write_cmd(ctrl, slot_cmd, cmd_mask);
 	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n",
-		 __func__, ctrl->cap_base + SLOTCTRL, slot_cmd);
+		 __func__, ctrl->cap_base + PCI_EXP_SLTCTL, slot_cmd);
 
 	return rc;
 }
@@ -619,10 +486,10 @@ static void hpc_set_green_led_on(struct slot *slot)
 	u16 cmd_mask;
 
 	slot_cmd = 0x0100;
-	cmd_mask = PWR_LED_CTRL;
+	cmd_mask = PCI_EXP_SLTCTL_PIC;
 	pcie_write_cmd(ctrl, slot_cmd, cmd_mask);
 	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n",
-		 __func__, ctrl->cap_base + SLOTCTRL, slot_cmd);
+		 __func__, ctrl->cap_base + PCI_EXP_SLTCTL, slot_cmd);
 }
 
 static void hpc_set_green_led_off(struct slot *slot)
@@ -632,10 +499,10 @@ static void hpc_set_green_led_off(struct slot *slot)
 	u16 cmd_mask;
 
 	slot_cmd = 0x0300;
-	cmd_mask = PWR_LED_CTRL;
+	cmd_mask = PCI_EXP_SLTCTL_PIC;
 	pcie_write_cmd(ctrl, slot_cmd, cmd_mask);
 	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n",
-		 __func__, ctrl->cap_base + SLOTCTRL, slot_cmd);
+		 __func__, ctrl->cap_base + PCI_EXP_SLTCTL, slot_cmd);
 }
 
 static void hpc_set_green_led_blink(struct slot *slot)
@@ -645,10 +512,10 @@ static void hpc_set_green_led_blink(struct slot *slot)
 	u16 cmd_mask;
 
 	slot_cmd = 0x0200;
-	cmd_mask = PWR_LED_CTRL;
+	cmd_mask = PCI_EXP_SLTCTL_PIC;
 	pcie_write_cmd(ctrl, slot_cmd, cmd_mask);
 	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n",
-		 __func__, ctrl->cap_base + SLOTCTRL, slot_cmd);
+		 __func__, ctrl->cap_base + PCI_EXP_SLTCTL, slot_cmd);
 }
 
 static int hpc_power_on_slot(struct slot * slot)
@@ -662,15 +529,15 @@ static int hpc_power_on_slot(struct slot * slot)
 	ctrl_dbg(ctrl, "%s: slot->hp_slot %x\n", __func__, slot->hp_slot);
 
 	/* Clear sticky power-fault bit from previous power failures */
-	retval = pciehp_readw(ctrl, SLOTSTATUS, &slot_status);
+	retval = pciehp_readw(ctrl, PCI_EXP_SLTSTA, &slot_status);
 	if (retval) {
 		ctrl_err(ctrl, "%s: Cannot read SLOTSTATUS register\n",
 			 __func__);
 		return retval;
 	}
-	slot_status &= PWR_FAULT_DETECTED;
+	slot_status &= PCI_EXP_SLTSTA_PFD;
 	if (slot_status) {
-		retval = pciehp_writew(ctrl, SLOTSTATUS, slot_status);
+		retval = pciehp_writew(ctrl, PCI_EXP_SLTSTA, slot_status);
 		if (retval) {
 			ctrl_err(ctrl,
 				 "%s: Cannot write to SLOTSTATUS register\n",
@@ -680,13 +547,13 @@ static int hpc_power_on_slot(struct slot * slot)
 	}
 
 	slot_cmd = POWER_ON;
-	cmd_mask = PWR_CTRL;
+	cmd_mask = PCI_EXP_SLTCTL_PCC;
 	/* Enable detection that we turned off at slot power-off time */
 	if (!pciehp_poll_mode) {
-		slot_cmd |= (PWR_FAULT_DETECT_ENABLE | MRL_DETECT_ENABLE |
-			     PRSN_DETECT_ENABLE);
-		cmd_mask |= (PWR_FAULT_DETECT_ENABLE | MRL_DETECT_ENABLE |
-			     PRSN_DETECT_ENABLE);
+		slot_cmd |= (PCI_EXP_SLTCTL_PFDE | PCI_EXP_SLTCTL_MRLSCE |
+			     PCI_EXP_SLTCTL_PDCE);
+		cmd_mask |= (PCI_EXP_SLTCTL_PFDE | PCI_EXP_SLTCTL_MRLSCE |
+			     PCI_EXP_SLTCTL_PDCE);
 	}
 
 	retval = pcie_write_cmd(ctrl, slot_cmd, cmd_mask);
@@ -696,7 +563,7 @@ static int hpc_power_on_slot(struct slot * slot)
 		return -1;
 	}
 	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n",
-		 __func__, ctrl->cap_base + SLOTCTRL, slot_cmd);
+		 __func__, ctrl->cap_base + PCI_EXP_SLTCTL, slot_cmd);
 
 	return retval;
 }
@@ -753,7 +620,7 @@ static int hpc_power_off_slot(struct slot * slot)
 	changed = pcie_mask_bad_dllp(ctrl);
 
 	slot_cmd = POWER_OFF;
-	cmd_mask = PWR_CTRL;
+	cmd_mask = PCI_EXP_SLTCTL_PCC;
 	/*
 	 * If we get MRL or presence detect interrupts now, the isr
 	 * will notice the sticky power-fault bit too and issue power
@@ -762,10 +629,10 @@ static int hpc_power_off_slot(struct slot * slot)
 	 * till the slot is powered on again.
 	 */
 	if (!pciehp_poll_mode) {
-		slot_cmd &= ~(PWR_FAULT_DETECT_ENABLE | MRL_DETECT_ENABLE |
-			      PRSN_DETECT_ENABLE);
-		cmd_mask |= (PWR_FAULT_DETECT_ENABLE | MRL_DETECT_ENABLE |
-			     PRSN_DETECT_ENABLE);
+		slot_cmd &= ~(PCI_EXP_SLTCTL_PFDE | PCI_EXP_SLTCTL_MRLSCE |
+			      PCI_EXP_SLTCTL_PDCE);
+		cmd_mask |= (PCI_EXP_SLTCTL_PFDE | PCI_EXP_SLTCTL_MRLSCE |
+			     PCI_EXP_SLTCTL_PDCE);
 	}
 
 	retval = pcie_write_cmd(ctrl, slot_cmd, cmd_mask);
@@ -775,7 +642,7 @@ static int hpc_power_off_slot(struct slot * slot)
 		goto out;
 	}
 	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n",
-		 __func__, ctrl->cap_base + SLOTCTRL, slot_cmd);
+		 __func__, ctrl->cap_base + PCI_EXP_SLTCTL, slot_cmd);
  out:
 	if (changed)
 		pcie_unmask_bad_dllp(ctrl);
@@ -796,19 +663,19 @@ static irqreturn_t pcie_isr(int irq, void *dev_id)
 	 */
 	intr_loc = 0;
 	do {
-		if (pciehp_readw(ctrl, SLOTSTATUS, &detected)) {
+		if (pciehp_readw(ctrl, PCI_EXP_SLTSTA, &detected)) {
 			ctrl_err(ctrl, "%s: Cannot read SLOTSTATUS\n",
 				 __func__);
 			return IRQ_NONE;
 		}
 
-		detected &= (ATTN_BUTTN_PRESSED | PWR_FAULT_DETECTED |
-			     MRL_SENS_CHANGED | PRSN_DETECT_CHANGED |
-			     CMD_COMPLETED);
+		detected &= (PCI_EXP_SLTSTA_ABP | PCI_EXP_SLTSTA_PFD |
+			     PCI_EXP_SLTSTA_MRLSC | PCI_EXP_SLTSTA_PDC |
+			     PCI_EXP_SLTSTA_CC);
 		intr_loc |= detected;
 		if (!intr_loc)
 			return IRQ_NONE;
-		if (detected && pciehp_writew(ctrl, SLOTSTATUS, detected)) {
+		if (detected && pciehp_writew(ctrl, PCI_EXP_SLTSTA, detected)) {
 			ctrl_err(ctrl, "%s: Cannot write to SLOTSTATUS\n",
 				 __func__);
 			return IRQ_NONE;
@@ -818,31 +685,31 @@ static irqreturn_t pcie_isr(int irq, void *dev_id)
 	ctrl_dbg(ctrl, "%s: intr_loc %x\n", __func__, intr_loc);
 
 	/* Check Command Complete Interrupt Pending */
-	if (intr_loc & CMD_COMPLETED) {
+	if (intr_loc & PCI_EXP_SLTSTA_CC) {
 		ctrl->cmd_busy = 0;
 		smp_mb();
 		wake_up(&ctrl->queue);
 	}
 
-	if (!(intr_loc & ~CMD_COMPLETED))
+	if (!(intr_loc & ~PCI_EXP_SLTSTA_CC))
 		return IRQ_HANDLED;
 
 	p_slot = pciehp_find_slot(ctrl, ctrl->slot_device_offset);
 
 	/* Check MRL Sensor Changed */
-	if (intr_loc & MRL_SENS_CHANGED)
+	if (intr_loc & PCI_EXP_SLTSTA_MRLSC)
 		pciehp_handle_switch_change(p_slot);
 
 	/* Check Attention Button Pressed */
-	if (intr_loc & ATTN_BUTTN_PRESSED)
+	if (intr_loc & PCI_EXP_SLTSTA_ABP)
 		pciehp_handle_attention_button(p_slot);
 
 	/* Check Presence Detect Changed */
-	if (intr_loc & PRSN_DETECT_CHANGED)
+	if (intr_loc & PCI_EXP_SLTSTA_PDC)
 		pciehp_handle_presence_change(p_slot);
 
 	/* Check Power Fault Detected */
-	if (intr_loc & PWR_FAULT_DETECTED)
+	if (intr_loc & PCI_EXP_SLTSTA_PFD)
 		pciehp_handle_power_fault(p_slot);
 
 	return IRQ_HANDLED;
@@ -855,7 +722,7 @@ static int hpc_get_max_lnk_speed(struct slot *slot, enum pci_bus_speed *value)
 	u32	lnk_cap;
 	int retval = 0;
 
-	retval = pciehp_readl(ctrl, LNKCAP, &lnk_cap);
+	retval = pciehp_readl(ctrl, PCI_EXP_LNKCAP, &lnk_cap);
 	if (retval) {
 		ctrl_err(ctrl, "%s: Cannot read LNKCAP register\n", __func__);
 		return retval;
@@ -884,13 +751,13 @@ static int hpc_get_max_lnk_width(struct slot *slot,
 	u32	lnk_cap;
 	int retval = 0;
 
-	retval = pciehp_readl(ctrl, LNKCAP, &lnk_cap);
+	retval = pciehp_readl(ctrl, PCI_EXP_LNKCAP, &lnk_cap);
 	if (retval) {
 		ctrl_err(ctrl, "%s: Cannot read LNKCAP register\n", __func__);
 		return retval;
 	}
 
-	switch ((lnk_cap & 0x03F0) >> 4){
+	switch ((lnk_cap & PCI_EXP_LNKSTA_NLW) >> 4){
 	case 0:
 		lnk_wdth = PCIE_LNK_WIDTH_RESRV;
 		break;
@@ -933,14 +800,14 @@ static int hpc_get_cur_lnk_speed(struct slot *slot, enum pci_bus_speed *value)
 	int retval = 0;
 	u16 lnk_status;
 
-	retval = pciehp_readw(ctrl, LNKSTATUS, &lnk_status);
+	retval = pciehp_readw(ctrl, PCI_EXP_LNKSTA, &lnk_status);
 	if (retval) {
 		ctrl_err(ctrl, "%s: Cannot read LNKSTATUS register\n",
 			 __func__);
 		return retval;
 	}
 
-	switch (lnk_status & 0x0F) {
+	switch (lnk_status & PCI_EXP_LNKSTA_CLS) {
 	case 1:
 		lnk_speed = PCIE_2PT5GB;
 		break;
@@ -963,14 +830,14 @@ static int hpc_get_cur_lnk_width(struct slot *slot,
 	int retval = 0;
 	u16 lnk_status;
 
-	retval = pciehp_readw(ctrl, LNKSTATUS, &lnk_status);
+	retval = pciehp_readw(ctrl, PCI_EXP_LNKSTA, &lnk_status);
 	if (retval) {
 		ctrl_err(ctrl, "%s: Cannot read LNKSTATUS register\n",
 			 __func__);
 		return retval;
 	}
 
-	switch ((lnk_status & 0x03F0) >> 4){
+	switch ((lnk_status & PCI_EXP_LNKSTA_NLW) >> 4){
 	case 0:
 		lnk_wdth = PCIE_LNK_WIDTH_RESRV;
 		break;
@@ -1036,18 +903,19 @@ int pcie_enable_notification(struct controller *ctrl)
 {
 	u16 cmd, mask;
 
-	cmd = PRSN_DETECT_ENABLE;
+	cmd = PCI_EXP_SLTCTL_PDCE;
 	if (ATTN_BUTTN(ctrl))
-		cmd |= ATTN_BUTTN_ENABLE;
+		cmd |= PCI_EXP_SLTCTL_ABPE;
 	if (POWER_CTRL(ctrl))
-		cmd |= PWR_FAULT_DETECT_ENABLE;
+		cmd |= PCI_EXP_SLTCTL_PFDE;
 	if (MRL_SENS(ctrl))
-		cmd |= MRL_DETECT_ENABLE;
+		cmd |= PCI_EXP_SLTCTL_MRLSCE;
 	if (!pciehp_poll_mode)
-		cmd |= HP_INTR_ENABLE | CMD_CMPL_INTR_ENABLE;
+		cmd |= PCI_EXP_SLTCTL_HPIE | PCI_EXP_SLTCTL_CCIE;
 
-	mask = PRSN_DETECT_ENABLE | ATTN_BUTTN_ENABLE | MRL_DETECT_ENABLE |
-	       PWR_FAULT_DETECT_ENABLE | HP_INTR_ENABLE | CMD_CMPL_INTR_ENABLE;
+	mask = (PCI_EXP_SLTCTL_PDCE | PCI_EXP_SLTCTL_ABPE |
+		PCI_EXP_SLTCTL_MRLSCE | PCI_EXP_SLTCTL_PFDE |
+		PCI_EXP_SLTCTL_HPIE | PCI_EXP_SLTCTL_CCIE);
 
 	if (pcie_write_cmd(ctrl, cmd, mask)) {
 		ctrl_err(ctrl, "Cannot enable software notification\n");
@@ -1059,8 +927,9 @@ int pcie_enable_notification(struct controller *ctrl)
 static void pcie_disable_notification(struct controller *ctrl)
 {
 	u16 mask;
-	mask = PRSN_DETECT_ENABLE | ATTN_BUTTN_ENABLE | MRL_DETECT_ENABLE |
-	       PWR_FAULT_DETECT_ENABLE | HP_INTR_ENABLE | CMD_CMPL_INTR_ENABLE;
+	mask = (PCI_EXP_SLTCTL_PDCE | PCI_EXP_SLTCTL_ABPE |
+		PCI_EXP_SLTCTL_MRLSCE | PCI_EXP_SLTCTL_PFDE |
+		PCI_EXP_SLTCTL_HPIE | PCI_EXP_SLTCTL_CCIE);
 	if (pcie_write_cmd(ctrl, 0, mask))
 		ctrl_warn(ctrl, "Cannot disable software notification\n");
 }
@@ -1157,9 +1026,9 @@ static inline void dbg_ctrl(struct controller *ctrl)
 		  EMI(ctrl)        ? "yes" : "no");
 	ctrl_info(ctrl, "  Command Completed    : %3s\n",
 		  NO_CMD_CMPL(ctrl) ? "no" : "yes");
-	pciehp_readw(ctrl, SLOTSTATUS, &reg16);
+	pciehp_readw(ctrl, PCI_EXP_SLTSTA, &reg16);
 	ctrl_info(ctrl, "Slot Status            : 0x%04x\n", reg16);
-	pciehp_readw(ctrl, SLOTCTRL, &reg16);
+	pciehp_readw(ctrl, PCI_EXP_SLTCTL, &reg16);
 	ctrl_info(ctrl, "Slot Control           : 0x%04x\n", reg16);
 }
 
@@ -1183,7 +1052,7 @@ struct controller *pcie_init(struct pcie_device *dev)
 		ctrl_err(ctrl, "Cannot find PCI Express capability\n");
 		goto abort_ctrl;
 	}
-	if (pciehp_readl(ctrl, SLOTCAP, &slot_cap)) {
+	if (pciehp_readl(ctrl, PCI_EXP_SLTCAP, &slot_cap)) {
 		ctrl_err(ctrl, "Cannot read SLOTCAP register\n");
 		goto abort_ctrl;
 	}
@@ -1208,17 +1077,17 @@ struct controller *pcie_init(struct pcie_device *dev)
 	    ctrl->no_cmd_complete = 1;
 
         /* Check if Data Link Layer Link Active Reporting is implemented */
-        if (pciehp_readl(ctrl, LNKCAP, &link_cap)) {
+        if (pciehp_readl(ctrl, PCI_EXP_LNKCAP, &link_cap)) {
                 ctrl_err(ctrl, "%s: Cannot read LNKCAP register\n", __func__);
                 goto abort_ctrl;
         }
-        if (link_cap & LINK_ACTIVE_REPORTING) {
+        if (link_cap & PCI_EXP_LNKCAP_DLLLARC) {
                 ctrl_dbg(ctrl, "Link Active Reporting supported\n");
                 ctrl->link_active_reporting = 1;
         }
 
 	/* Clear all remaining event bits in Slot Status register */
-	if (pciehp_writew(ctrl, SLOTSTATUS, 0x1f))
+	if (pciehp_writew(ctrl, PCI_EXP_SLTSTA, 0x1f))
 		goto abort_ctrl;
 
 	/* Disable sotfware notification */
diff --git a/drivers/pci/irq.c b/drivers/pci/irq.c
index 6441dfa969a3..de01174aff06 100644
--- a/drivers/pci/irq.c
+++ b/drivers/pci/irq.c
@@ -15,7 +15,7 @@ static void pci_note_irq_problem(struct pci_dev *pdev, const char *reason)
 
 	dev_printk(KERN_ERR, &pdev->dev,
 		   "Potentially misrouted IRQ (Bridge %s %04x:%04x)\n",
-		   parent->dev.bus_id, parent->vendor, parent->device);
+		   dev_name(&parent->dev), parent->vendor, parent->device);
 	dev_printk(KERN_ERR, &pdev->dev, "%s\n", reason);
 	dev_printk(KERN_ERR, &pdev->dev, "Please report to linux-kernel@vger.kernel.org\n");
 	WARN_ON(1);
diff --git a/drivers/pci/msi.c b/drivers/pci/msi.c
index 11a51f8ed3b3..b4a90badd0a6 100644
--- a/drivers/pci/msi.c
+++ b/drivers/pci/msi.c
@@ -776,28 +776,19 @@ void pci_no_msi(void)
 	pci_msi_enable = 0;
 }
 
-void pci_msi_init_pci_dev(struct pci_dev *dev)
-{
-	INIT_LIST_HEAD(&dev->msi_list);
-}
-
-#ifdef CONFIG_ACPI
-#include <linux/acpi.h>
-#include <linux/pci-acpi.h>
-static void __devinit msi_acpi_init(void)
+/**
+ * pci_msi_enabled - is MSI enabled?
+ *
+ * Returns true if MSI has not been disabled by the command-line option
+ * pci=nomsi.
+ **/
+int pci_msi_enabled(void)
 {
-	if (acpi_pci_disabled)
-		return;
-	pci_osc_support_set(OSC_MSI_SUPPORT);
-	pcie_osc_support_set(OSC_MSI_SUPPORT);
+	return pci_msi_enable;
 }
-#else
-static inline void msi_acpi_init(void) { }
-#endif /* CONFIG_ACPI */
+EXPORT_SYMBOL(pci_msi_enabled);
 
-void __devinit msi_init(void)
+void pci_msi_init_pci_dev(struct pci_dev *dev)
 {
-	if (!pci_msi_enable)
-		return;
-	msi_acpi_init();
+	INIT_LIST_HEAD(&dev->msi_list);
 }
diff --git a/drivers/pci/pci-acpi.c b/drivers/pci/pci-acpi.c
index ae5ec76dca77..3582512e7226 100644
--- a/drivers/pci/pci-acpi.c
+++ b/drivers/pci/pci-acpi.c
@@ -24,13 +24,14 @@ struct acpi_osc_data {
 	acpi_handle handle;
 	u32 support_set;
 	u32 control_set;
+	u32 control_query;
+	int is_queried;
 	struct list_head sibiling;
 };
 static LIST_HEAD(acpi_osc_data_list);
 
 struct acpi_osc_args {
 	u32 capbuf[3];
-	u32 ctrl_result;
 };
 
 static DEFINE_MUTEX(pci_acpi_lock);
@@ -56,7 +57,7 @@ static u8 OSC_UUID[16] = {0x5B, 0x4D, 0xDB, 0x33, 0xF7, 0x1F, 0x1C, 0x40,
 			  0x96, 0x57, 0x74, 0x41, 0xC0, 0x3D, 0xD7, 0x66};
 
 static acpi_status acpi_run_osc(acpi_handle handle,
-				struct acpi_osc_args *osc_args)
+				struct acpi_osc_args *osc_args, u32 *retval)
 {
 	acpi_status status;
 	struct acpi_object_list input;
@@ -112,8 +113,7 @@ static acpi_status acpi_run_osc(acpi_handle handle,
 		goto out_kfree;
 	}
 out_success:
-	osc_args->ctrl_result =
-		*((u32 *)(out_obj->buffer.pointer + 8));
+	*retval = *((u32 *)(out_obj->buffer.pointer + 8));
 	status = AE_OK;
 
 out_kfree:
@@ -121,11 +121,10 @@ out_kfree:
 	return status;
 }
 
-static acpi_status __acpi_query_osc(u32 flags, struct acpi_osc_data *osc_data,
-				    u32 *result)
+static acpi_status __acpi_query_osc(u32 flags, struct acpi_osc_data *osc_data)
 {
 	acpi_status status;
-	u32 support_set;
+	u32 support_set, result;
 	struct acpi_osc_args osc_args;
 
 	/* do _OSC query for all possible controls */
@@ -134,56 +133,45 @@ static acpi_status __acpi_query_osc(u32 flags, struct acpi_osc_data *osc_data,
 	osc_args.capbuf[OSC_SUPPORT_TYPE] = support_set;
 	osc_args.capbuf[OSC_CONTROL_TYPE] = OSC_CONTROL_MASKS;
 
-	status = acpi_run_osc(osc_data->handle, &osc_args);
+	status = acpi_run_osc(osc_data->handle, &osc_args, &result);
 	if (ACPI_SUCCESS(status)) {
 		osc_data->support_set = support_set;
-		*result = osc_args.ctrl_result;
+		osc_data->control_query = result;
+		osc_data->is_queried = 1;
 	}
 
 	return status;
 }
 
-static acpi_status acpi_query_osc(acpi_handle handle,
-				  u32 level, void *context, void **retval)
+/*
+ * pci_acpi_osc_support: Invoke _OSC indicating support for the given feature
+ * @flags: Bitmask of flags to support
+ *
+ * See the ACPI spec for the definition of the flags
+ */
+int pci_acpi_osc_support(acpi_handle handle, u32 flags)
 {
 	acpi_status status;
-	struct acpi_osc_data *osc_data;
-	u32 flags = (unsigned long)context, dummy;
 	acpi_handle tmp;
+	struct acpi_osc_data *osc_data;
+	int rc = 0;
 
 	status = acpi_get_handle(handle, "_OSC", &tmp);
 	if (ACPI_FAILURE(status))
-		return AE_OK;
+		return -ENOTTY;
 
 	mutex_lock(&pci_acpi_lock);
 	osc_data = acpi_get_osc_data(handle);
 	if (!osc_data) {
 		printk(KERN_ERR "acpi osc data array is full\n");
+		rc = -ENOMEM;
 		goto out;
 	}
 
-	__acpi_query_osc(flags, osc_data, &dummy);
+	__acpi_query_osc(flags, osc_data);
 out:
 	mutex_unlock(&pci_acpi_lock);
-	return AE_OK;
-}
-
-/**
- * __pci_osc_support_set - register OS support to Firmware
- * @flags: OS support bits
- * @hid: hardware ID
- *
- * Update OS support fields and doing a _OSC Query to obtain an update
- * from Firmware on supported control bits.
- **/
-acpi_status __pci_osc_support_set(u32 flags, const char *hid)
-{
-	if (!(flags & OSC_SUPPORT_MASKS))
-		return AE_TYPE;
-
-	acpi_get_devices(hid, acpi_query_osc,
-			 (void *)(unsigned long)flags, NULL);
-	return AE_OK;
+	return rc;
 }
 
 /**
@@ -196,7 +184,7 @@ acpi_status __pci_osc_support_set(u32 flags, const char *hid)
 acpi_status pci_osc_control_set(acpi_handle handle, u32 flags)
 {
 	acpi_status status;
-	u32 ctrlset, control_set, result;
+	u32 control_req, control_set, result;
 	acpi_handle tmp;
 	struct acpi_osc_data *osc_data;
 	struct acpi_osc_args osc_args;
@@ -213,28 +201,34 @@ acpi_status pci_osc_control_set(acpi_handle handle, u32 flags)
 		goto out;
 	}
 
-	ctrlset = (flags & OSC_CONTROL_MASKS);
-	if (!ctrlset) {
+	control_req = (flags & OSC_CONTROL_MASKS);
+	if (!control_req) {
 		status = AE_TYPE;
 		goto out;
 	}
 
-	status = __acpi_query_osc(osc_data->support_set, osc_data, &result);
-	if (ACPI_FAILURE(status))
+	/* No need to evaluate _OSC if the control was already granted. */
+	if ((osc_data->control_set & control_req) == control_req)
 		goto out;
 
-	if ((result & ctrlset) != ctrlset) {
+	if (!osc_data->is_queried) {
+		status = __acpi_query_osc(osc_data->support_set, osc_data);
+		if (ACPI_FAILURE(status))
+			goto out;
+	}
+
+	if ((osc_data->control_query & control_req) != control_req) {
 		status = AE_SUPPORT;
 		goto out;
 	}
 
-	control_set = osc_data->control_set | ctrlset;
+	control_set = osc_data->control_set | control_req;
 	osc_args.capbuf[OSC_QUERY_TYPE] = 0;
 	osc_args.capbuf[OSC_SUPPORT_TYPE] = osc_data->support_set;
 	osc_args.capbuf[OSC_CONTROL_TYPE] = control_set;
-	status = acpi_run_osc(handle, &osc_args);
+	status = acpi_run_osc(handle, &osc_args, &result);
 	if (ACPI_SUCCESS(status))
-		osc_data->control_set = control_set;
+		osc_data->control_set = result;
 out:
 	mutex_unlock(&pci_acpi_lock);
 	return status;
@@ -375,7 +369,7 @@ static int acpi_pci_find_root_bridge(struct device *dev, acpi_handle *handle)
 	 * The string should be the same as root bridge's name
 	 * Please look at 'pci_scan_bus_parented'
 	 */
-	num = sscanf(dev->bus_id, "pci%04x:%02x", &seg, &bus);
+	num = sscanf(dev_name(dev), "pci%04x:%02x", &seg, &bus);
 	if (num != 2)
 		return -ENODEV;
 	*handle = acpi_get_pci_rootbridge_handle(seg, bus);
diff --git a/drivers/pci/pci-driver.c b/drivers/pci/pci-driver.c
index b4cdd690ae71..c697f2680856 100644
--- a/drivers/pci/pci-driver.c
+++ b/drivers/pci/pci-driver.c
@@ -16,6 +16,7 @@
 #include <linux/string.h>
 #include <linux/slab.h>
 #include <linux/sched.h>
+#include <linux/cpu.h>
 #include "pci.h"
 
 /*
@@ -48,7 +49,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,
@@ -58,16 +59,18 @@ store_new_id(struct device_driver *driver, const char *buf, size_t count)
 
 	/* Only accept driver_data values that match an existing id_table
 	   entry */
-	retval = -EINVAL;
-	while (ids->vendor || ids->subvendor || ids->class_mask) {
-		if (driver_data == ids->driver_data) {
-			retval = 0;
-			break;
+	if (ids) {
+		retval = -EINVAL;
+		while (ids->vendor || ids->subvendor || ids->class_mask) {
+			if (driver_data == ids->driver_data) {
+				retval = 0;
+				break;
+			}
+			ids++;
 		}
-		ids++;
+		if (retval)	/* No match */
+			return retval;
 	}
-	if (retval)	/* No match */
-		return retval;
 
 	dynid = kzalloc(sizeof(*dynid), GFP_KERNEL);
 	if (!dynid)
@@ -183,32 +186,43 @@ static const struct pci_device_id *pci_match_device(struct pci_driver *drv,
 	return pci_match_id(drv->id_table, dev);
 }
 
+struct drv_dev_and_id {
+	struct pci_driver *drv;
+	struct pci_dev *dev;
+	const struct pci_device_id *id;
+};
+
+static long local_pci_probe(void *_ddi)
+{
+	struct drv_dev_and_id *ddi = _ddi;
+
+	return ddi->drv->probe(ddi->dev, ddi->id);
+}
+
 static int pci_call_probe(struct pci_driver *drv, struct pci_dev *dev,
 			  const struct pci_device_id *id)
 {
-	int error;
-#ifdef CONFIG_NUMA
-	/* Execute driver initialization on node where the
-	   device's bus is attached to.  This way the driver likely
-	   allocates its local memory on the right node without
-	   any need to change it. */
-	struct mempolicy *oldpol;
-	cpumask_t oldmask = current->cpus_allowed;
-	int node = dev_to_node(&dev->dev);
+	int error, node;
+	struct drv_dev_and_id ddi = { drv, dev, id };
 
+	/* Execute driver initialization on node where the device's
+	   bus is attached to.  This way the driver likely allocates
+	   its local memory on the right node without any need to
+	   change it. */
+	node = dev_to_node(&dev->dev);
 	if (node >= 0) {
+		int cpu;
 		node_to_cpumask_ptr(nodecpumask, node);
-		set_cpus_allowed_ptr(current, nodecpumask);
-	}
-	/* And set default memory allocation policy */
-	oldpol = current->mempolicy;
-	current->mempolicy = NULL;	/* fall back to system default policy */
-#endif
-	error = drv->probe(dev, id);
-#ifdef CONFIG_NUMA
-	set_cpus_allowed_ptr(current, &oldmask);
-	current->mempolicy = oldpol;
-#endif
+
+		get_online_cpus();
+		cpu = cpumask_any_and(nodecpumask, cpu_online_mask);
+		if (cpu < nr_cpu_ids)
+			error = work_on_cpu(cpu, local_pci_probe, &ddi);
+		else
+			error = local_pci_probe(&ddi);
+		put_online_cpus();
+	} else
+		error = local_pci_probe(&ddi);
 	return error;
 }
 
@@ -302,11 +316,10 @@ static void pci_device_shutdown(struct device *dev)
 
 /*
  * Default "suspend" method for devices that have no driver provided suspend,
- * or not even a driver at all.
+ * or not even a driver at all (second part).
  */
-static void pci_default_pm_suspend(struct pci_dev *pci_dev)
+static void pci_pm_set_unknown_state(struct pci_dev *pci_dev)
 {
-	pci_save_state(pci_dev);
 	/*
 	 * mark its power state as "unknown", since we don't know if
 	 * e.g. the BIOS will change its device state when we suspend.
@@ -317,14 +330,12 @@ static void pci_default_pm_suspend(struct pci_dev *pci_dev)
 
 /*
  * Default "resume" method for devices that have no driver provided resume,
- * or not even a driver at all.
+ * or not even a driver at all (second part).
  */
-static int pci_default_pm_resume(struct pci_dev *pci_dev)
+static int pci_pm_reenable_device(struct pci_dev *pci_dev)
 {
-	int retval = 0;
+	int retval;
 
-	/* restore the PCI config space */
-	pci_restore_state(pci_dev);
 	/* if the device was enabled before suspend, reenable */
 	retval = pci_reenable_device(pci_dev);
 	/*
@@ -347,8 +358,16 @@ static int pci_legacy_suspend(struct device *dev, pm_message_t state)
 		i = drv->suspend(pci_dev, state);
 		suspend_report_result(drv->suspend, i);
 	} else {
-		pci_default_pm_suspend(pci_dev);
+		pci_save_state(pci_dev);
+		/*
+		 * This is for compatibility with existing code with legacy PM
+		 * support.
+		 */
+		pci_pm_set_unknown_state(pci_dev);
 	}
+
+	pci_fixup_device(pci_fixup_suspend, pci_dev);
+
 	return i;
 }
 
@@ -365,30 +384,130 @@ static int pci_legacy_suspend_late(struct device *dev, pm_message_t state)
 	return i;
 }
 
+static int pci_legacy_resume_early(struct device *dev)
+{
+	int error = 0;
+	struct pci_dev * pci_dev = to_pci_dev(dev);
+	struct pci_driver * drv = pci_dev->driver;
+
+	pci_fixup_device(pci_fixup_resume_early, pci_dev);
+
+	if (drv && drv->resume_early)
+		error = drv->resume_early(pci_dev);
+	return error;
+}
+
 static int pci_legacy_resume(struct device *dev)
 {
 	int error;
 	struct pci_dev * pci_dev = to_pci_dev(dev);
 	struct pci_driver * drv = pci_dev->driver;
 
-	if (drv && drv->resume)
+	pci_fixup_device(pci_fixup_resume, pci_dev);
+
+	if (drv && drv->resume) {
 		error = drv->resume(pci_dev);
-	else
-		error = pci_default_pm_resume(pci_dev);
+	} else {
+		/* restore the PCI config space */
+		pci_restore_state(pci_dev);
+		error = pci_pm_reenable_device(pci_dev);
+	}
 	return error;
 }
 
-static int pci_legacy_resume_early(struct device *dev)
+/* Auxiliary functions used by the new power management framework */
+
+static int pci_restore_standard_config(struct pci_dev *pci_dev)
 {
+	struct pci_dev *parent = pci_dev->bus->self;
 	int error = 0;
-	struct pci_dev * pci_dev = to_pci_dev(dev);
-	struct pci_driver * drv = pci_dev->driver;
 
-	if (drv && drv->resume_early)
-		error = drv->resume_early(pci_dev);
+	/* Check if the device's bus is operational */
+	if (!parent || parent->current_state == PCI_D0) {
+		pci_restore_state(pci_dev);
+		pci_update_current_state(pci_dev, PCI_D0);
+	} else {
+		dev_warn(&pci_dev->dev, "unable to restore config, "
+			"bridge %s in low power state D%d\n", pci_name(parent),
+			parent->current_state);
+		pci_dev->current_state = PCI_UNKNOWN;
+		error = -EAGAIN;
+	}
+
 	return error;
 }
 
+static bool pci_is_bridge(struct pci_dev *pci_dev)
+{
+	return !!(pci_dev->subordinate);
+}
+
+static void pci_pm_default_resume_noirq(struct pci_dev *pci_dev)
+{
+	if (pci_restore_standard_config(pci_dev))
+		pci_fixup_device(pci_fixup_resume_early, pci_dev);
+}
+
+static int pci_pm_default_resume(struct pci_dev *pci_dev)
+{
+	/*
+	 * pci_restore_standard_config() should have been called once already,
+	 * but it would have failed if the device's parent bridge had not been
+	 * in power state D0 at that time.  Check it and try again if necessary.
+	 */
+	if (pci_dev->current_state == PCI_UNKNOWN) {
+		int error = pci_restore_standard_config(pci_dev);
+		if (error)
+			return error;
+	}
+
+	pci_fixup_device(pci_fixup_resume, pci_dev);
+
+	if (!pci_is_bridge(pci_dev))
+		pci_enable_wake(pci_dev, PCI_D0, false);
+
+	return pci_pm_reenable_device(pci_dev);
+}
+
+static void pci_pm_default_suspend_generic(struct pci_dev *pci_dev)
+{
+	/* If device is enabled at this point, disable it */
+	pci_disable_enabled_device(pci_dev);
+	/*
+	 * Save state with interrupts enabled, because in principle the bus the
+	 * device is on may be put into a low power state after this code runs.
+	 */
+	pci_save_state(pci_dev);
+}
+
+static void pci_pm_default_suspend(struct pci_dev *pci_dev)
+{
+	pci_pm_default_suspend_generic(pci_dev);
+
+	if (!pci_is_bridge(pci_dev))
+		pci_prepare_to_sleep(pci_dev);
+
+	pci_fixup_device(pci_fixup_suspend, pci_dev);
+}
+
+static bool pci_has_legacy_pm_support(struct pci_dev *pci_dev)
+{
+	struct pci_driver *drv = pci_dev->driver;
+	bool ret = drv && (drv->suspend || drv->suspend_late || drv->resume
+		|| drv->resume_early);
+
+	/*
+	 * Legacy PM support is used by default, so warn if the new framework is
+	 * supported as well.  Drivers are supposed to support either the
+	 * former, or the latter, but not both at the same time.
+	 */
+	WARN_ON(ret && drv->driver.pm);
+
+	return ret;
+}
+
+/* New power management framework */
+
 static int pci_pm_prepare(struct device *dev)
 {
 	struct device_driver *drv = dev->driver;
@@ -416,17 +535,16 @@ static int pci_pm_suspend(struct device *dev)
 	struct device_driver *drv = dev->driver;
 	int error = 0;
 
-	if (drv && drv->pm) {
-		if (drv->pm->suspend) {
-			error = drv->pm->suspend(dev);
-			suspend_report_result(drv->pm->suspend, error);
-		} else {
-			pci_default_pm_suspend(pci_dev);
-		}
-	} else {
-		error = pci_legacy_suspend(dev, PMSG_SUSPEND);
+	if (pci_has_legacy_pm_support(pci_dev))
+		return pci_legacy_suspend(dev, PMSG_SUSPEND);
+
+	if (drv && drv->pm && drv->pm->suspend) {
+		error = drv->pm->suspend(dev);
+		suspend_report_result(drv->pm->suspend, error);
 	}
-	pci_fixup_device(pci_fixup_suspend, pci_dev);
+
+	if (!error)
+		pci_pm_default_suspend(pci_dev);
 
 	return error;
 }
@@ -434,53 +552,53 @@ static int pci_pm_suspend(struct device *dev)
 static int pci_pm_suspend_noirq(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
-	struct pci_driver *drv = pci_dev->driver;
+	struct device_driver *drv = dev->driver;
 	int error = 0;
 
-	if (drv && drv->pm) {
-		if (drv->pm->suspend_noirq) {
-			error = drv->pm->suspend_noirq(dev);
-			suspend_report_result(drv->pm->suspend_noirq, error);
-		}
-	} else {
-		error = pci_legacy_suspend_late(dev, PMSG_SUSPEND);
+	if (pci_has_legacy_pm_support(pci_dev))
+		return pci_legacy_suspend_late(dev, PMSG_SUSPEND);
+
+	if (drv && drv->pm && drv->pm->suspend_noirq) {
+		error = drv->pm->suspend_noirq(dev);
+		suspend_report_result(drv->pm->suspend_noirq, error);
 	}
 
+	if (!error)
+		pci_pm_set_unknown_state(pci_dev);
+
 	return error;
 }
 
-static int pci_pm_resume(struct device *dev)
+static int pci_pm_resume_noirq(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
 	struct device_driver *drv = dev->driver;
-	int error;
+	int error = 0;
 
-	pci_fixup_device(pci_fixup_resume, pci_dev);
+	if (pci_has_legacy_pm_support(pci_dev))
+		return pci_legacy_resume_early(dev);
 
-	if (drv && drv->pm) {
-		error = drv->pm->resume ? drv->pm->resume(dev) :
-			pci_default_pm_resume(pci_dev);
-	} else {
-		error = pci_legacy_resume(dev);
-	}
+	pci_pm_default_resume_noirq(pci_dev);
+
+	if (drv && drv->pm && drv->pm->resume_noirq)
+		error = drv->pm->resume_noirq(dev);
 
 	return error;
 }
 
-static int pci_pm_resume_noirq(struct device *dev)
+static int pci_pm_resume(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
-	struct pci_driver *drv = pci_dev->driver;
+	struct device_driver *drv = dev->driver;
 	int error = 0;
 
-	pci_fixup_device(pci_fixup_resume_early, pci_dev);
+	if (pci_has_legacy_pm_support(pci_dev))
+		return pci_legacy_resume(dev);
 
-	if (drv && drv->pm) {
-		if (drv->pm->resume_noirq)
-			error = drv->pm->resume_noirq(dev);
-	} else {
-		error = pci_legacy_resume_early(dev);
-	}
+	error = pci_pm_default_resume(pci_dev);
+
+	if (!error && drv && drv->pm && drv->pm->resume)
+		error = drv->pm->resume(dev);
 
 	return error;
 }
@@ -502,141 +620,140 @@ static int pci_pm_freeze(struct device *dev)
 	struct device_driver *drv = dev->driver;
 	int error = 0;
 
-	if (drv && drv->pm) {
-		if (drv->pm->freeze) {
-			error = drv->pm->freeze(dev);
-			suspend_report_result(drv->pm->freeze, error);
-		} else {
-			pci_default_pm_suspend(pci_dev);
-		}
-	} else {
-		error = pci_legacy_suspend(dev, PMSG_FREEZE);
-		pci_fixup_device(pci_fixup_suspend, pci_dev);
+	if (pci_has_legacy_pm_support(pci_dev))
+		return pci_legacy_suspend(dev, PMSG_FREEZE);
+
+	if (drv && drv->pm && drv->pm->freeze) {
+		error = drv->pm->freeze(dev);
+		suspend_report_result(drv->pm->freeze, error);
 	}
 
+	if (!error)
+		pci_pm_default_suspend_generic(pci_dev);
+
 	return error;
 }
 
 static int pci_pm_freeze_noirq(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
-	struct pci_driver *drv = pci_dev->driver;
+	struct device_driver *drv = dev->driver;
 	int error = 0;
 
-	if (drv && drv->pm) {
-		if (drv->pm->freeze_noirq) {
-			error = drv->pm->freeze_noirq(dev);
-			suspend_report_result(drv->pm->freeze_noirq, error);
-		}
-	} else {
-		error = pci_legacy_suspend_late(dev, PMSG_FREEZE);
+	if (pci_has_legacy_pm_support(pci_dev))
+		return pci_legacy_suspend_late(dev, PMSG_FREEZE);
+
+	if (drv && drv->pm && drv->pm->freeze_noirq) {
+		error = drv->pm->freeze_noirq(dev);
+		suspend_report_result(drv->pm->freeze_noirq, error);
 	}
 
+	if (!error)
+		pci_pm_set_unknown_state(pci_dev);
+
 	return error;
 }
 
-static int pci_pm_thaw(struct device *dev)
+static int pci_pm_thaw_noirq(struct device *dev)
 {
+	struct pci_dev *pci_dev = to_pci_dev(dev);
 	struct device_driver *drv = dev->driver;
 	int error = 0;
 
-	if (drv && drv->pm) {
-		if (drv->pm->thaw)
-			error =  drv->pm->thaw(dev);
-	} else {
-		pci_fixup_device(pci_fixup_resume, to_pci_dev(dev));
-		error = pci_legacy_resume(dev);
-	}
+	if (pci_has_legacy_pm_support(pci_dev))
+		return pci_legacy_resume_early(dev);
+
+	pci_update_current_state(pci_dev, PCI_D0);
+
+	if (drv && drv->pm && drv->pm->thaw_noirq)
+		error = drv->pm->thaw_noirq(dev);
 
 	return error;
 }
 
-static int pci_pm_thaw_noirq(struct device *dev)
+static int pci_pm_thaw(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
-	struct pci_driver *drv = pci_dev->driver;
+	struct device_driver *drv = dev->driver;
 	int error = 0;
 
-	if (drv && drv->pm) {
-		if (drv->pm->thaw_noirq)
-			error = drv->pm->thaw_noirq(dev);
-	} else {
-		pci_fixup_device(pci_fixup_resume_early, pci_dev);
-		error = pci_legacy_resume_early(dev);
-	}
+	if (pci_has_legacy_pm_support(pci_dev))
+		return pci_legacy_resume(dev);
+
+	pci_pm_reenable_device(pci_dev);
+
+	if (drv && drv->pm && drv->pm->thaw)
+		error =  drv->pm->thaw(dev);
 
 	return error;
 }
 
 static int pci_pm_poweroff(struct device *dev)
 {
+	struct pci_dev *pci_dev = to_pci_dev(dev);
 	struct device_driver *drv = dev->driver;
 	int error = 0;
 
-	pci_fixup_device(pci_fixup_suspend, to_pci_dev(dev));
+	if (pci_has_legacy_pm_support(pci_dev))
+		return pci_legacy_suspend(dev, PMSG_HIBERNATE);
 
-	if (drv && drv->pm) {
-		if (drv->pm->poweroff) {
-			error = drv->pm->poweroff(dev);
-			suspend_report_result(drv->pm->poweroff, error);
-		}
-	} else {
-		error = pci_legacy_suspend(dev, PMSG_HIBERNATE);
+	if (drv && drv->pm && drv->pm->poweroff) {
+		error = drv->pm->poweroff(dev);
+		suspend_report_result(drv->pm->poweroff, error);
 	}
 
+	if (!error)
+		pci_pm_default_suspend(pci_dev);
+
 	return error;
 }
 
 static int pci_pm_poweroff_noirq(struct device *dev)
 {
-	struct pci_dev *pci_dev = to_pci_dev(dev);
-	struct pci_driver *drv = pci_dev->driver;
+	struct device_driver *drv = dev->driver;
 	int error = 0;
 
-	if (drv && drv->pm) {
-		if (drv->pm->poweroff_noirq) {
-			error = drv->pm->poweroff_noirq(dev);
-			suspend_report_result(drv->pm->poweroff_noirq, error);
-		}
-	} else {
-		error = pci_legacy_suspend_late(dev, PMSG_HIBERNATE);
+	if (pci_has_legacy_pm_support(to_pci_dev(dev)))
+		return pci_legacy_suspend_late(dev, PMSG_HIBERNATE);
+
+	if (drv && drv->pm && drv->pm->poweroff_noirq) {
+		error = drv->pm->poweroff_noirq(dev);
+		suspend_report_result(drv->pm->poweroff_noirq, error);
 	}
 
 	return error;
 }
 
-static int pci_pm_restore(struct device *dev)
+static int pci_pm_restore_noirq(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
 	struct device_driver *drv = dev->driver;
-	int error;
+	int error = 0;
 
-	if (drv && drv->pm) {
-		error = drv->pm->restore ? drv->pm->restore(dev) :
-			pci_default_pm_resume(pci_dev);
-	} else {
-		error = pci_legacy_resume(dev);
-	}
-	pci_fixup_device(pci_fixup_resume, pci_dev);
+	if (pci_has_legacy_pm_support(pci_dev))
+		return pci_legacy_resume_early(dev);
+
+	pci_pm_default_resume_noirq(pci_dev);
+
+	if (drv && drv->pm && drv->pm->restore_noirq)
+		error = drv->pm->restore_noirq(dev);
 
 	return error;
 }
 
-static int pci_pm_restore_noirq(struct device *dev)
+static int pci_pm_restore(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
-	struct pci_driver *drv = pci_dev->driver;
+	struct device_driver *drv = dev->driver;
 	int error = 0;
 
-	pci_fixup_device(pci_fixup_resume, pci_dev);
+	if (pci_has_legacy_pm_support(pci_dev))
+		return pci_legacy_resume(dev);
 
-	if (drv && drv->pm) {
-		if (drv->pm->restore_noirq)
-			error = drv->pm->restore_noirq(dev);
-	} else {
-		error = pci_legacy_resume_early(dev);
-	}
-	pci_fixup_device(pci_fixup_resume_early, pci_dev);
+	error = pci_pm_default_resume(pci_dev);
+
+	if (!error && drv && drv->pm && drv->pm->restore)
+		error = drv->pm->restore(dev);
 
 	return error;
 }
@@ -654,17 +771,15 @@ static int pci_pm_restore_noirq(struct device *dev)
 
 #endif /* !CONFIG_HIBERNATION */
 
-struct pm_ext_ops pci_pm_ops = {
-	.base = {
-		.prepare = pci_pm_prepare,
-		.complete = pci_pm_complete,
-		.suspend = pci_pm_suspend,
-		.resume = pci_pm_resume,
-		.freeze = pci_pm_freeze,
-		.thaw = pci_pm_thaw,
-		.poweroff = pci_pm_poweroff,
-		.restore = pci_pm_restore,
-	},
+struct dev_pm_ops pci_dev_pm_ops = {
+	.prepare = pci_pm_prepare,
+	.complete = pci_pm_complete,
+	.suspend = pci_pm_suspend,
+	.resume = pci_pm_resume,
+	.freeze = pci_pm_freeze,
+	.thaw = pci_pm_thaw,
+	.poweroff = pci_pm_poweroff,
+	.restore = pci_pm_restore,
 	.suspend_noirq = pci_pm_suspend_noirq,
 	.resume_noirq = pci_pm_resume_noirq,
 	.freeze_noirq = pci_pm_freeze_noirq,
@@ -673,7 +788,7 @@ struct pm_ext_ops pci_pm_ops = {
 	.restore_noirq = pci_pm_restore_noirq,
 };
 
-#define PCI_PM_OPS_PTR	&pci_pm_ops
+#define PCI_PM_OPS_PTR	(&pci_dev_pm_ops)
 
 #else /* !CONFIG_PM_SLEEP */
 
@@ -703,9 +818,6 @@ int __pci_register_driver(struct pci_driver *drv, struct module *owner,
 	drv->driver.owner = owner;
 	drv->driver.mod_name = mod_name;
 
-	if (drv->pm)
-		drv->driver.pm = &drv->pm->base;
-
 	spin_lock_init(&drv->dynids.lock);
 	INIT_LIST_HEAD(&drv->dynids.list);
 
diff --git a/drivers/pci/pci-stub.c b/drivers/pci/pci-stub.c
new file mode 100644
index 000000000000..74fbec0bf6cb
--- /dev/null
+++ b/drivers/pci/pci-stub.c
@@ -0,0 +1,47 @@
+/* pci-stub - simple stub driver to reserve a pci device
+ *
+ * Copyright (C) 2008 Red Hat, Inc.
+ * Author:
+ * 	Chris Wright
+ *
+ * This work is licensed under the terms of the GNU GPL, version 2.
+ *
+ * Usage is simple, allocate a new id to the stub driver and bind the
+ * device to it.  For example:
+ * 
+ * # echo "8086 10f5" > /sys/bus/pci/drivers/pci-stub/new_id
+ * # echo -n 0000:00:19.0 > /sys/bus/pci/drivers/e1000e/unbind
+ * # echo -n 0000:00:19.0 > /sys/bus/pci/drivers/pci-stub/bind
+ * # ls -l /sys/bus/pci/devices/0000:00:19.0/driver
+ * .../0000:00:19.0/driver -> ../../../bus/pci/drivers/pci-stub
+ */
+
+#include <linux/module.h>
+#include <linux/pci.h>
+
+static int pci_stub_probe(struct pci_dev *dev, const struct pci_device_id *id)
+{
+	return 0;
+}
+
+static struct pci_driver stub_driver = {
+	.name		= "pci-stub",
+	.id_table	= NULL,	/* only dynamic id's */
+	.probe		= pci_stub_probe,
+};
+
+static int __init pci_stub_init(void)
+{
+	return pci_register_driver(&stub_driver);
+}
+
+static void __exit pci_stub_exit(void)
+{
+	pci_unregister_driver(&stub_driver);
+}
+
+module_init(pci_stub_init);
+module_exit(pci_stub_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Chris Wright <chrisw@sous-sol.org>");
diff --git a/drivers/pci/pci-sysfs.c b/drivers/pci/pci-sysfs.c
index c88485860a0a..c23619fb6c4b 100644
--- a/drivers/pci/pci-sysfs.c
+++ b/drivers/pci/pci-sysfs.c
@@ -58,13 +58,14 @@ static ssize_t broken_parity_status_store(struct device *dev,
 					  const char *buf, size_t count)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
-	ssize_t consumed = -EINVAL;
+	unsigned long val;
 
-	if ((count > 0) && (*buf == '0' || *buf == '1')) {
-		pdev->broken_parity_status = *buf == '1' ? 1 : 0;
-		consumed = count;
-	}
-	return consumed;
+	if (strict_strtoul(buf, 0, &val) < 0)
+		return -EINVAL;
+
+	pdev->broken_parity_status = !!val;
+
+	return count;
 }
 
 static ssize_t local_cpus_show(struct device *dev,
@@ -101,11 +102,13 @@ resource_show(struct device * dev, struct device_attribute *attr, char * buf)
 	struct pci_dev * pci_dev = to_pci_dev(dev);
 	char * str = buf;
 	int i;
-	int max = 7;
+	int max;
 	resource_size_t start, end;
 
 	if (pci_dev->subordinate)
 		max = DEVICE_COUNT_RESOURCE;
+	else
+		max = PCI_BRIDGE_RESOURCES;
 
 	for (i = 0; i < max; i++) {
 		struct resource *res =  &pci_dev->resource[i];
@@ -133,19 +136,23 @@ static ssize_t is_enabled_store(struct device *dev,
 				struct device_attribute *attr, const char *buf,
 				size_t count)
 {
-	ssize_t result = -EINVAL;
 	struct pci_dev *pdev = to_pci_dev(dev);
+	unsigned long val;
+	ssize_t result = strict_strtoul(buf, 0, &val);
+
+	if (result < 0)
+		return result;
 
 	/* this can crash the machine when done on the "wrong" device */
 	if (!capable(CAP_SYS_ADMIN))
-		return count;
+		return -EPERM;
 
-	if (*buf == '0') {
+	if (!val) {
 		if (atomic_read(&pdev->enable_cnt) != 0)
 			pci_disable_device(pdev);
 		else
 			result = -EIO;
-	} else if (*buf == '1')
+	} else
 		result = pci_enable_device(pdev);
 
 	return result < 0 ? result : count;
@@ -185,25 +192,28 @@ msi_bus_store(struct device *dev, struct device_attribute *attr,
 	      const char *buf, size_t count)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
+	unsigned long val;
+
+	if (strict_strtoul(buf, 0, &val) < 0)
+		return -EINVAL;
 
 	/* bad things may happen if the no_msi flag is changed
 	 * while some drivers are loaded */
 	if (!capable(CAP_SYS_ADMIN))
-		return count;
+		return -EPERM;
 
+	/* Maybe pci devices without subordinate busses shouldn't even have this
+	 * attribute in the first place?  */
 	if (!pdev->subordinate)
 		return count;
 
-	if (*buf == '0') {
-		pdev->subordinate->bus_flags |= PCI_BUS_FLAGS_NO_MSI;
-		dev_warn(&pdev->dev, "forced subordinate bus to not support MSI,"
-			 " bad things could happen.\n");
-	}
+	/* Is the flag going to change, or keep the value it already had? */
+	if (!(pdev->subordinate->bus_flags & PCI_BUS_FLAGS_NO_MSI) ^
+	    !!val) {
+		pdev->subordinate->bus_flags ^= PCI_BUS_FLAGS_NO_MSI;
 
-	if (*buf == '1') {
-		pdev->subordinate->bus_flags &= ~PCI_BUS_FLAGS_NO_MSI;
-		dev_warn(&pdev->dev, "forced subordinate bus to support MSI,"
-			 " bad things could happen.\n");
+		dev_warn(&pdev->dev, "forced subordinate bus to%s support MSI,"
+			 " bad things could happen\n", val ? "" : " not");
 	}
 
 	return count;
@@ -361,55 +371,33 @@ pci_write_config(struct kobject *kobj, struct bin_attribute *bin_attr,
 }
 
 static ssize_t
-pci_read_vpd(struct kobject *kobj, struct bin_attribute *bin_attr,
-	     char *buf, loff_t off, size_t count)
+read_vpd_attr(struct kobject *kobj, struct bin_attribute *bin_attr,
+	      char *buf, loff_t off, size_t count)
 {
 	struct pci_dev *dev =
 		to_pci_dev(container_of(kobj, struct device, kobj));
-	int end;
-	int ret;
 
 	if (off > bin_attr->size)
 		count = 0;
 	else if (count > bin_attr->size - off)
 		count = bin_attr->size - off;
-	end = off + count;
-
-	while (off < end) {
-		ret = dev->vpd->ops->read(dev, off, end - off, buf);
-		if (ret < 0)
-			return ret;
-		buf += ret;
-		off += ret;
-	}
 
-	return count;
+	return pci_read_vpd(dev, off, count, buf);
 }
 
 static ssize_t
-pci_write_vpd(struct kobject *kobj, struct bin_attribute *bin_attr,
-	      char *buf, loff_t off, size_t count)
+write_vpd_attr(struct kobject *kobj, struct bin_attribute *bin_attr,
+	       char *buf, loff_t off, size_t count)
 {
 	struct pci_dev *dev =
 		to_pci_dev(container_of(kobj, struct device, kobj));
-	int end;
-	int ret;
 
 	if (off > bin_attr->size)
 		count = 0;
 	else if (count > bin_attr->size - off)
 		count = bin_attr->size - off;
-	end = off + count;
-
-	while (off < end) {
-		ret = dev->vpd->ops->write(dev, off, end - off, buf);
-		if (ret < 0)
-			return ret;
-		buf += ret;
-		off += ret;
-	}
 
-	return count;
+	return pci_write_vpd(dev, off, count, buf);
 }
 
 #ifdef HAVE_PCI_LEGACY
@@ -569,7 +557,7 @@ void pci_remove_legacy_files(struct pci_bus *b)
 
 #ifdef HAVE_PCI_MMAP
 
-static int pci_mmap_fits(struct pci_dev *pdev, int resno, struct vm_area_struct *vma)
+int pci_mmap_fits(struct pci_dev *pdev, int resno, struct vm_area_struct *vma)
 {
 	unsigned long nr, start, size;
 
@@ -620,6 +608,9 @@ pci_mmap_resource(struct kobject *kobj, struct bin_attribute *attr,
 	vma->vm_pgoff += start >> PAGE_SHIFT;
 	mmap_type = res->flags & IORESOURCE_MEM ? pci_mmap_mem : pci_mmap_io;
 
+	if (res->flags & IORESOURCE_MEM && iomem_is_exclusive(start))
+		return -EINVAL;
+
 	return pci_mmap_page_range(pdev, vma, mmap_type, write_combine);
 }
 
@@ -832,8 +823,8 @@ static int pci_create_capabilities_sysfs(struct pci_dev *dev)
 		attr->size = dev->vpd->len;
 		attr->attr.name = "vpd";
 		attr->attr.mode = S_IRUSR | S_IWUSR;
-		attr->read = pci_read_vpd;
-		attr->write = pci_write_vpd;
+		attr->read = read_vpd_attr;
+		attr->write = write_vpd_attr;
 		retval = sysfs_create_bin_file(&dev->dev.kobj, attr);
 		if (retval) {
 			kfree(dev->vpd->attr);
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 061d1ee0046a..c12f6c790698 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -56,6 +56,22 @@ unsigned char pci_bus_max_busnr(struct pci_bus* bus)
 }
 EXPORT_SYMBOL_GPL(pci_bus_max_busnr);
 
+#ifdef CONFIG_HAS_IOMEM
+void __iomem *pci_ioremap_bar(struct pci_dev *pdev, int bar)
+{
+	/*
+	 * Make sure the BAR is actually a memory resource, not an IO resource
+	 */
+	if (!(pci_resource_flags(pdev, bar) & IORESOURCE_MEM)) {
+		WARN_ON(1);
+		return NULL;
+	}
+	return ioremap_nocache(pci_resource_start(pdev, bar),
+				     pci_resource_len(pdev, bar));
+}
+EXPORT_SYMBOL_GPL(pci_ioremap_bar);
+#endif
+
 #if 0
 /**
  * pci_max_busnr - returns maximum PCI bus number
@@ -360,25 +376,10 @@ pci_find_parent_resource(const struct pci_dev *dev, struct resource *res)
 static void
 pci_restore_bars(struct pci_dev *dev)
 {
-	int i, numres;
-
-	switch (dev->hdr_type) {
-	case PCI_HEADER_TYPE_NORMAL:
-		numres = 6;
-		break;
-	case PCI_HEADER_TYPE_BRIDGE:
-		numres = 2;
-		break;
-	case PCI_HEADER_TYPE_CARDBUS:
-		numres = 1;
-		break;
-	default:
-		/* Should never get here, but just in case... */
-		return;
-	}
+	int i;
 
-	for (i = 0; i < numres; i ++)
-		pci_update_resource(dev, &dev->resource[i], i);
+	for (i = 0; i < PCI_BRIDGE_RESOURCES; i++)
+		pci_update_resource(dev, i);
 }
 
 static struct pci_platform_pm_ops *pci_platform_pm;
@@ -524,14 +525,17 @@ pci_raw_set_power_state(struct pci_dev *dev, pci_power_t state)
  * pci_update_current_state - Read PCI power state of given device from its
  *                            PCI PM registers and cache it
  * @dev: PCI device to handle.
+ * @state: State to cache in case the device doesn't have the PM capability
  */
-static void pci_update_current_state(struct pci_dev *dev)
+void pci_update_current_state(struct pci_dev *dev, pci_power_t state)
 {
 	if (dev->pm_cap) {
 		u16 pmcsr;
 
 		pci_read_config_word(dev, dev->pm_cap + PCI_PM_CTRL, &pmcsr);
 		dev->current_state = (pmcsr & PCI_PM_CTRL_STATE_MASK);
+	} else {
+		dev->current_state = state;
 	}
 }
 
@@ -574,7 +578,7 @@ int pci_set_power_state(struct pci_dev *dev, pci_power_t state)
 		 */
 		int ret = platform_pci_set_power_state(dev, PCI_D0);
 		if (!ret)
-			pci_update_current_state(dev);
+			pci_update_current_state(dev, PCI_D0);
 	}
 	/* This device is quirked not to be put into D3, so
 	   don't put it in D3 */
@@ -587,7 +591,7 @@ int pci_set_power_state(struct pci_dev *dev, pci_power_t state)
 		/* Allow the platform to finalize the transition */
 		int ret = platform_pci_set_power_state(dev, state);
 		if (!ret) {
-			pci_update_current_state(dev);
+			pci_update_current_state(dev, state);
 			error = 0;
 		}
 	}
@@ -640,19 +644,14 @@ static int pci_save_pcie_state(struct pci_dev *dev)
 	int pos, i = 0;
 	struct pci_cap_saved_state *save_state;
 	u16 *cap;
-	int found = 0;
 
 	pos = pci_find_capability(dev, PCI_CAP_ID_EXP);
 	if (pos <= 0)
 		return 0;
 
 	save_state = pci_find_saved_cap(dev, PCI_CAP_ID_EXP);
-	if (!save_state)
-		save_state = kzalloc(sizeof(*save_state) + sizeof(u16) * 4, GFP_KERNEL);
-	else
-		found = 1;
 	if (!save_state) {
-		dev_err(&dev->dev, "out of memory in pci_save_pcie_state\n");
+		dev_err(&dev->dev, "buffer not found in %s\n", __FUNCTION__);
 		return -ENOMEM;
 	}
 	cap = (u16 *)&save_state->data[0];
@@ -661,9 +660,7 @@ static int pci_save_pcie_state(struct pci_dev *dev)
 	pci_read_config_word(dev, pos + PCI_EXP_LNKCTL, &cap[i++]);
 	pci_read_config_word(dev, pos + PCI_EXP_SLTCTL, &cap[i++]);
 	pci_read_config_word(dev, pos + PCI_EXP_RTCTL, &cap[i++]);
-	save_state->cap_nr = PCI_CAP_ID_EXP;
-	if (!found)
-		pci_add_saved_cap(dev, save_state);
+
 	return 0;
 }
 
@@ -688,30 +685,21 @@ static void pci_restore_pcie_state(struct pci_dev *dev)
 
 static int pci_save_pcix_state(struct pci_dev *dev)
 {
-	int pos, i = 0;
+	int pos;
 	struct pci_cap_saved_state *save_state;
-	u16 *cap;
-	int found = 0;
 
 	pos = pci_find_capability(dev, PCI_CAP_ID_PCIX);
 	if (pos <= 0)
 		return 0;
 
 	save_state = pci_find_saved_cap(dev, PCI_CAP_ID_PCIX);
-	if (!save_state)
-		save_state = kzalloc(sizeof(*save_state) + sizeof(u16), GFP_KERNEL);
-	else
-		found = 1;
 	if (!save_state) {
-		dev_err(&dev->dev, "out of memory in pci_save_pcie_state\n");
+		dev_err(&dev->dev, "buffer not found in %s\n", __FUNCTION__);
 		return -ENOMEM;
 	}
-	cap = (u16 *)&save_state->data[0];
 
-	pci_read_config_word(dev, pos + PCI_X_CMD, &cap[i++]);
-	save_state->cap_nr = PCI_CAP_ID_PCIX;
-	if (!found)
-		pci_add_saved_cap(dev, save_state);
+	pci_read_config_word(dev, pos + PCI_X_CMD, (u16 *)save_state->data);
+
 	return 0;
 }
 
@@ -982,6 +970,32 @@ void pcim_pin_device(struct pci_dev *pdev)
  */
 void __attribute__ ((weak)) pcibios_disable_device (struct pci_dev *dev) {}
 
+static void do_pci_disable_device(struct pci_dev *dev)
+{
+	u16 pci_command;
+
+	pci_read_config_word(dev, PCI_COMMAND, &pci_command);
+	if (pci_command & PCI_COMMAND_MASTER) {
+		pci_command &= ~PCI_COMMAND_MASTER;
+		pci_write_config_word(dev, PCI_COMMAND, pci_command);
+	}
+
+	pcibios_disable_device(dev);
+}
+
+/**
+ * pci_disable_enabled_device - Disable device without updating enable_cnt
+ * @dev: PCI device to disable
+ *
+ * NOTE: This function is a backend of PCI power management routines and is
+ * not supposed to be called drivers.
+ */
+void pci_disable_enabled_device(struct pci_dev *dev)
+{
+	if (atomic_read(&dev->enable_cnt))
+		do_pci_disable_device(dev);
+}
+
 /**
  * pci_disable_device - Disable PCI device after use
  * @dev: PCI device to be disabled
@@ -996,7 +1010,6 @@ void
 pci_disable_device(struct pci_dev *dev)
 {
 	struct pci_devres *dr;
-	u16 pci_command;
 
 	dr = find_pci_dr(dev);
 	if (dr)
@@ -1005,14 +1018,9 @@ pci_disable_device(struct pci_dev *dev)
 	if (atomic_sub_return(1, &dev->enable_cnt) != 0)
 		return;
 
-	pci_read_config_word(dev, PCI_COMMAND, &pci_command);
-	if (pci_command & PCI_COMMAND_MASTER) {
-		pci_command &= ~PCI_COMMAND_MASTER;
-		pci_write_config_word(dev, PCI_COMMAND, pci_command);
-	}
-	dev->is_busmaster = 0;
+	do_pci_disable_device(dev);
 
-	pcibios_disable_device(dev);
+	dev->is_busmaster = 0;
 }
 
 /**
@@ -1107,7 +1115,7 @@ int pci_enable_wake(struct pci_dev *dev, pci_power_t state, int enable)
 	int error = 0;
 	bool pme_done = false;
 
-	if (!device_may_wakeup(&dev->dev))
+	if (enable && !device_may_wakeup(&dev->dev))
 		return -EINVAL;
 
 	/*
@@ -1252,14 +1260,15 @@ void pci_pm_init(struct pci_dev *dev)
 	/* find PCI PM capability in list */
 	pm = pci_find_capability(dev, PCI_CAP_ID_PM);
 	if (!pm)
-		return;
+		goto Exit;
+
 	/* Check device's ability to generate PME# */
 	pci_read_config_word(dev, pm + PCI_PM_PMC, &pmc);
 
 	if ((pmc & PCI_PM_CAP_VER_MASK) > 3) {
 		dev_err(&dev->dev, "unsupported PM cap regs version (%u)\n",
 			pmc & PCI_PM_CAP_VER_MASK);
-		return;
+		goto Exit;
 	}
 
 	dev->pm_cap = pm;
@@ -1298,6 +1307,74 @@ void pci_pm_init(struct pci_dev *dev)
 	} else {
 		dev->pme_support = 0;
 	}
+
+ Exit:
+	pci_update_current_state(dev, PCI_D0);
+}
+
+/**
+ * platform_pci_wakeup_init - init platform wakeup if present
+ * @dev: PCI device
+ *
+ * Some devices don't have PCI PM caps but can still generate wakeup
+ * events through platform methods (like ACPI events).  If @dev supports
+ * platform wakeup events, set the device flag to indicate as much.  This
+ * may be redundant if the device also supports PCI PM caps, but double
+ * initialization should be safe in that case.
+ */
+void platform_pci_wakeup_init(struct pci_dev *dev)
+{
+	if (!platform_pci_can_wakeup(dev))
+		return;
+
+	device_set_wakeup_capable(&dev->dev, true);
+	device_set_wakeup_enable(&dev->dev, false);
+	platform_pci_sleep_wake(dev, false);
+}
+
+/**
+ * pci_add_save_buffer - allocate buffer for saving given capability registers
+ * @dev: the PCI device
+ * @cap: the capability to allocate the buffer for
+ * @size: requested size of the buffer
+ */
+static int pci_add_cap_save_buffer(
+	struct pci_dev *dev, char cap, unsigned int size)
+{
+	int pos;
+	struct pci_cap_saved_state *save_state;
+
+	pos = pci_find_capability(dev, cap);
+	if (pos <= 0)
+		return 0;
+
+	save_state = kzalloc(sizeof(*save_state) + size, GFP_KERNEL);
+	if (!save_state)
+		return -ENOMEM;
+
+	save_state->cap_nr = cap;
+	pci_add_saved_cap(dev, save_state);
+
+	return 0;
+}
+
+/**
+ * pci_allocate_cap_save_buffers - allocate buffers for saving capabilities
+ * @dev: the PCI device
+ */
+void pci_allocate_cap_save_buffers(struct pci_dev *dev)
+{
+	int error;
+
+	error = pci_add_cap_save_buffer(dev, PCI_CAP_ID_EXP, 4 * sizeof(u16));
+	if (error)
+		dev_err(&dev->dev,
+			"unable to preallocate PCI Express save buffer\n");
+
+	error = pci_add_cap_save_buffer(dev, PCI_CAP_ID_PCIX, sizeof(u16));
+	if (error)
+		dev_err(&dev->dev,
+			"unable to preallocate PCI-X save buffer\n");
 }
 
 /**
@@ -1337,6 +1414,20 @@ void pci_enable_ari(struct pci_dev *dev)
 	bridge->ari_enabled = 1;
 }
 
+/**
+ * pci_swizzle_interrupt_pin - swizzle INTx for device behind bridge
+ * @dev: the PCI device
+ * @pin: the INTx pin (1=INTA, 2=INTB, 3=INTD, 4=INTD)
+ *
+ * Perform INTx swizzling for a device behind one level of bridge.  This is
+ * required by section 9.1 of the PCI-to-PCI bridge specification for devices
+ * behind bridges on add-in cards.
+ */
+u8 pci_swizzle_interrupt_pin(struct pci_dev *dev, u8 pin)
+{
+	return (((pin - 1) + PCI_SLOT(dev->devfn)) % 4) + 1;
+}
+
 int
 pci_get_interrupt_pin(struct pci_dev *dev, struct pci_dev **bridge)
 {
@@ -1345,9 +1436,9 @@ pci_get_interrupt_pin(struct pci_dev *dev, struct pci_dev **bridge)
 	pin = dev->pin;
 	if (!pin)
 		return -1;
-	pin--;
+
 	while (dev->bus->self) {
-		pin = (pin + PCI_SLOT(dev->devfn)) % 4;
+		pin = pci_swizzle_interrupt_pin(dev, pin);
 		dev = dev->bus->self;
 	}
 	*bridge = dev;
@@ -1355,6 +1446,26 @@ pci_get_interrupt_pin(struct pci_dev *dev, struct pci_dev **bridge)
 }
 
 /**
+ * pci_common_swizzle - swizzle INTx all the way to root bridge
+ * @dev: the PCI device
+ * @pinp: pointer to the INTx pin value (1=INTA, 2=INTB, 3=INTD, 4=INTD)
+ *
+ * Perform INTx swizzling for a device.  This traverses through all PCI-to-PCI
+ * bridges all the way up to a PCI root bus.
+ */
+u8 pci_common_swizzle(struct pci_dev *dev, u8 *pinp)
+{
+	u8 pin = *pinp;
+
+	while (dev->bus->self) {
+		pin = pci_swizzle_interrupt_pin(dev, pin);
+		dev = dev->bus->self;
+	}
+	*pinp = pin;
+	return PCI_SLOT(dev->devfn);
+}
+
+/**
  *	pci_release_region - Release a PCI bar
  *	@pdev: PCI device whose resources were previously reserved by pci_request_region
  *	@bar: BAR to release
@@ -1395,7 +1506,8 @@ void pci_release_region(struct pci_dev *pdev, int bar)
  *	Returns 0 on success, or %EBUSY on error.  A warning
  *	message is also printed on failure.
  */
-int pci_request_region(struct pci_dev *pdev, int bar, const char *res_name)
+static int __pci_request_region(struct pci_dev *pdev, int bar, const char *res_name,
+									int exclusive)
 {
 	struct pci_devres *dr;
 
@@ -1408,8 +1520,9 @@ int pci_request_region(struct pci_dev *pdev, int bar, const char *res_name)
 			goto err_out;
 	}
 	else if (pci_resource_flags(pdev, bar) & IORESOURCE_MEM) {
-		if (!request_mem_region(pci_resource_start(pdev, bar),
-				        pci_resource_len(pdev, bar), res_name))
+		if (!__request_mem_region(pci_resource_start(pdev, bar),
+					pci_resource_len(pdev, bar), res_name,
+					exclusive))
 			goto err_out;
 	}
 
@@ -1428,6 +1541,47 @@ err_out:
 }
 
 /**
+ *	pci_request_region - Reserved PCI I/O and memory resource
+ *	@pdev: PCI device whose resources are to be reserved
+ *	@bar: BAR to be reserved
+ *	@res_name: Name to be associated with resource.
+ *
+ *	Mark the PCI region associated with PCI device @pdev BR @bar as
+ *	being reserved by owner @res_name.  Do not access any
+ *	address inside the PCI regions unless this call returns
+ *	successfully.
+ *
+ *	Returns 0 on success, or %EBUSY on error.  A warning
+ *	message is also printed on failure.
+ */
+int pci_request_region(struct pci_dev *pdev, int bar, const char *res_name)
+{
+	return __pci_request_region(pdev, bar, res_name, 0);
+}
+
+/**
+ *	pci_request_region_exclusive - Reserved PCI I/O and memory resource
+ *	@pdev: PCI device whose resources are to be reserved
+ *	@bar: BAR to be reserved
+ *	@res_name: Name to be associated with resource.
+ *
+ *	Mark the PCI region associated with PCI device @pdev BR @bar as
+ *	being reserved by owner @res_name.  Do not access any
+ *	address inside the PCI regions unless this call returns
+ *	successfully.
+ *
+ *	Returns 0 on success, or %EBUSY on error.  A warning
+ *	message is also printed on failure.
+ *
+ *	The key difference that _exclusive makes it that userspace is
+ *	explicitly not allowed to map the resource via /dev/mem or
+ * 	sysfs.
+ */
+int pci_request_region_exclusive(struct pci_dev *pdev, int bar, const char *res_name)
+{
+	return __pci_request_region(pdev, bar, res_name, IORESOURCE_EXCLUSIVE);
+}
+/**
  * pci_release_selected_regions - Release selected PCI I/O and memory resources
  * @pdev: PCI device whose resources were previously reserved
  * @bars: Bitmask of BARs to be released
@@ -1444,20 +1598,14 @@ void pci_release_selected_regions(struct pci_dev *pdev, int bars)
 			pci_release_region(pdev, i);
 }
 
-/**
- * pci_request_selected_regions - Reserve selected PCI I/O and memory resources
- * @pdev: PCI device whose resources are to be reserved
- * @bars: Bitmask of BARs to be requested
- * @res_name: Name to be associated with resource
- */
-int pci_request_selected_regions(struct pci_dev *pdev, int bars,
-				 const char *res_name)
+int __pci_request_selected_regions(struct pci_dev *pdev, int bars,
+				 const char *res_name, int excl)
 {
 	int i;
 
 	for (i = 0; i < 6; i++)
 		if (bars & (1 << i))
-			if(pci_request_region(pdev, i, res_name))
+			if (__pci_request_region(pdev, i, res_name, excl))
 				goto err_out;
 	return 0;
 
@@ -1469,6 +1617,26 @@ err_out:
 	return -EBUSY;
 }
 
+
+/**
+ * pci_request_selected_regions - Reserve selected PCI I/O and memory resources
+ * @pdev: PCI device whose resources are to be reserved
+ * @bars: Bitmask of BARs to be requested
+ * @res_name: Name to be associated with resource
+ */
+int pci_request_selected_regions(struct pci_dev *pdev, int bars,
+				 const char *res_name)
+{
+	return __pci_request_selected_regions(pdev, bars, res_name, 0);
+}
+
+int pci_request_selected_regions_exclusive(struct pci_dev *pdev,
+				 int bars, const char *res_name)
+{
+	return __pci_request_selected_regions(pdev, bars, res_name,
+			IORESOURCE_EXCLUSIVE);
+}
+
 /**
  *	pci_release_regions - Release reserved PCI I/O and memory resources
  *	@pdev: PCI device whose resources were previously reserved by pci_request_regions
@@ -1502,27 +1670,66 @@ int pci_request_regions(struct pci_dev *pdev, const char *res_name)
 }
 
 /**
+ *	pci_request_regions_exclusive - Reserved PCI I/O and memory resources
+ *	@pdev: PCI device whose resources are to be reserved
+ *	@res_name: Name to be associated with resource.
+ *
+ *	Mark all PCI regions associated with PCI device @pdev as
+ *	being reserved by owner @res_name.  Do not access any
+ *	address inside the PCI regions unless this call returns
+ *	successfully.
+ *
+ *	pci_request_regions_exclusive() will mark the region so that
+ * 	/dev/mem and the sysfs MMIO access will not be allowed.
+ *
+ *	Returns 0 on success, or %EBUSY on error.  A warning
+ *	message is also printed on failure.
+ */
+int pci_request_regions_exclusive(struct pci_dev *pdev, const char *res_name)
+{
+	return pci_request_selected_regions_exclusive(pdev,
+					((1 << 6) - 1), res_name);
+}
+
+static void __pci_set_master(struct pci_dev *dev, bool enable)
+{
+	u16 old_cmd, cmd;
+
+	pci_read_config_word(dev, PCI_COMMAND, &old_cmd);
+	if (enable)
+		cmd = old_cmd | PCI_COMMAND_MASTER;
+	else
+		cmd = old_cmd & ~PCI_COMMAND_MASTER;
+	if (cmd != old_cmd) {
+		dev_dbg(&dev->dev, "%s bus mastering\n",
+			enable ? "enabling" : "disabling");
+		pci_write_config_word(dev, PCI_COMMAND, cmd);
+	}
+	dev->is_busmaster = enable;
+}
+
+/**
  * pci_set_master - enables bus-mastering for device dev
  * @dev: the PCI device to enable
  *
  * Enables bus-mastering on the device and calls pcibios_set_master()
  * to do the needed arch specific settings.
  */
-void
-pci_set_master(struct pci_dev *dev)
+void pci_set_master(struct pci_dev *dev)
 {
-	u16 cmd;
-
-	pci_read_config_word(dev, PCI_COMMAND, &cmd);
-	if (! (cmd & PCI_COMMAND_MASTER)) {
-		dev_dbg(&dev->dev, "enabling bus mastering\n");
-		cmd |= PCI_COMMAND_MASTER;
-		pci_write_config_word(dev, PCI_COMMAND, cmd);
-	}
-	dev->is_busmaster = 1;
+	__pci_set_master(dev, true);
 	pcibios_set_master(dev);
 }
 
+/**
+ * pci_clear_master - disables bus-mastering for device dev
+ * @dev: the PCI device to disable
+ */
+void pci_clear_master(struct pci_dev *dev)
+{
+	__pci_set_master(dev, false);
+}
+
 #ifdef PCI_DISABLE_MWI
 int pci_set_mwi(struct pci_dev *dev)
 {
@@ -1751,24 +1958,7 @@ int pci_set_dma_seg_boundary(struct pci_dev *dev, unsigned long mask)
 EXPORT_SYMBOL(pci_set_dma_seg_boundary);
 #endif
 
-/**
- * pci_execute_reset_function() - Reset a PCI device function
- * @dev: Device function to reset
- *
- * Some devices allow an individual function to be reset without affecting
- * other functions in the same device.  The PCI device must be responsive
- * to PCI config space in order to use this function.
- *
- * The device function is presumed to be unused when this function is called.
- * Resetting the device will make the contents of PCI configuration space
- * random, so any caller of this must be prepared to reinitialise the
- * device including MSI, bus mastering, BARs, decoding IO and memory spaces,
- * etc.
- *
- * Returns 0 if the device function was successfully reset or -ENOTTY if the
- * device doesn't support resetting a single function.
- */
-int pci_execute_reset_function(struct pci_dev *dev)
+static int __pcie_flr(struct pci_dev *dev, int probe)
 {
 	u16 status;
 	u32 cap;
@@ -1780,6 +1970,9 @@ int pci_execute_reset_function(struct pci_dev *dev)
 	if (!(cap & PCI_EXP_DEVCAP_FLR))
 		return -ENOTTY;
 
+	if (probe)
+		return 0;
+
 	pci_block_user_cfg_access(dev);
 
 	/* Wait for Transaction Pending bit clean */
@@ -1802,6 +1995,80 @@ int pci_execute_reset_function(struct pci_dev *dev)
 	pci_unblock_user_cfg_access(dev);
 	return 0;
 }
+
+static int __pci_af_flr(struct pci_dev *dev, int probe)
+{
+	int cappos = pci_find_capability(dev, PCI_CAP_ID_AF);
+	u8 status;
+	u8 cap;
+
+	if (!cappos)
+		return -ENOTTY;
+	pci_read_config_byte(dev, cappos + PCI_AF_CAP, &cap);
+	if (!(cap & PCI_AF_CAP_TP) || !(cap & PCI_AF_CAP_FLR))
+		return -ENOTTY;
+
+	if (probe)
+		return 0;
+
+	pci_block_user_cfg_access(dev);
+
+	/* Wait for Transaction Pending bit clean */
+	msleep(100);
+	pci_read_config_byte(dev, cappos + PCI_AF_STATUS, &status);
+	if (status & PCI_AF_STATUS_TP) {
+		dev_info(&dev->dev, "Busy after 100ms while trying to"
+				" reset; sleeping for 1 second\n");
+		ssleep(1);
+		pci_read_config_byte(dev,
+				cappos + PCI_AF_STATUS, &status);
+		if (status & PCI_AF_STATUS_TP)
+			dev_info(&dev->dev, "Still busy after 1s; "
+					"proceeding with reset anyway\n");
+	}
+	pci_write_config_byte(dev, cappos + PCI_AF_CTRL, PCI_AF_CTRL_FLR);
+	mdelay(100);
+
+	pci_unblock_user_cfg_access(dev);
+	return 0;
+}
+
+static int __pci_reset_function(struct pci_dev *pdev, int probe)
+{
+	int res;
+
+	res = __pcie_flr(pdev, probe);
+	if (res != -ENOTTY)
+		return res;
+
+	res = __pci_af_flr(pdev, probe);
+	if (res != -ENOTTY)
+		return res;
+
+	return res;
+}
+
+/**
+ * pci_execute_reset_function() - Reset a PCI device function
+ * @dev: Device function to reset
+ *
+ * Some devices allow an individual function to be reset without affecting
+ * other functions in the same device.  The PCI device must be responsive
+ * to PCI config space in order to use this function.
+ *
+ * The device function is presumed to be unused when this function is called.
+ * Resetting the device will make the contents of PCI configuration space
+ * random, so any caller of this must be prepared to reinitialise the
+ * device including MSI, bus mastering, BARs, decoding IO and memory spaces,
+ * etc.
+ *
+ * Returns 0 if the device function was successfully reset or -ENOTTY if the
+ * device doesn't support resetting a single function.
+ */
+int pci_execute_reset_function(struct pci_dev *dev)
+{
+	return __pci_reset_function(dev, 0);
+}
 EXPORT_SYMBOL_GPL(pci_execute_reset_function);
 
 /**
@@ -1822,15 +2089,10 @@ EXPORT_SYMBOL_GPL(pci_execute_reset_function);
  */
 int pci_reset_function(struct pci_dev *dev)
 {
-	u32 cap;
-	int exppos = pci_find_capability(dev, PCI_CAP_ID_EXP);
-	int r;
+	int r = __pci_reset_function(dev, 1);
 
-	if (!exppos)
-		return -ENOTTY;
-	pci_read_config_dword(dev, exppos + PCI_EXP_DEVCAP, &cap);
-	if (!(cap & PCI_EXP_DEVCAP_FLR))
-		return -ENOTTY;
+	if (r < 0)
+		return r;
 
 	if (!dev->msi_enabled && !dev->msix_enabled && dev->irq != 0)
 		disable_irq(dev->irq);
@@ -2022,6 +2284,28 @@ int pci_select_bars(struct pci_dev *dev, unsigned long flags)
 	return bars;
 }
 
+/**
+ * pci_resource_bar - get position of the BAR associated with a resource
+ * @dev: the PCI device
+ * @resno: the resource number
+ * @type: the BAR type to be filled in
+ *
+ * Returns BAR position in config space, or 0 if the BAR is invalid.
+ */
+int pci_resource_bar(struct pci_dev *dev, int resno, enum pci_bar_type *type)
+{
+	if (resno < PCI_ROM_RESOURCE) {
+		*type = pci_bar_unknown;
+		return PCI_BASE_ADDRESS_0 + 4 * resno;
+	} else if (resno == PCI_ROM_RESOURCE) {
+		*type = pci_bar_mem32;
+		return dev->rom_base_reg;
+	}
+
+	dev_err(&dev->dev, "BAR: invalid resource #%d\n", resno);
+	return 0;
+}
+
 static void __devinit pci_no_domains(void)
 {
 #ifdef CONFIG_PCI_DOMAINS
@@ -2029,6 +2313,19 @@ static void __devinit pci_no_domains(void)
 #endif
 }
 
+/**
+ * pci_ext_cfg_enabled - can we access extended PCI config space?
+ * @dev: The PCI device of the root bridge.
+ *
+ * Returns 1 if we can access PCI extended config space (offsets
+ * greater than 0xff). This is the default implementation. Architecture
+ * implementations can override this.
+ */
+int __attribute__ ((weak)) pci_ext_cfg_avail(struct pci_dev *dev)
+{
+	return 1;
+}
+
 static int __devinit pci_init(void)
 {
 	struct pci_dev *dev = NULL;
@@ -2037,8 +2334,6 @@ static int __devinit pci_init(void)
 		pci_fixup_device(pci_fixup_final, dev);
 	}
 
-	msi_init();
-
 	return 0;
 }
 
@@ -2083,11 +2378,15 @@ EXPORT_SYMBOL(pci_find_capability);
 EXPORT_SYMBOL(pci_bus_find_capability);
 EXPORT_SYMBOL(pci_release_regions);
 EXPORT_SYMBOL(pci_request_regions);
+EXPORT_SYMBOL(pci_request_regions_exclusive);
 EXPORT_SYMBOL(pci_release_region);
 EXPORT_SYMBOL(pci_request_region);
+EXPORT_SYMBOL(pci_request_region_exclusive);
 EXPORT_SYMBOL(pci_release_selected_regions);
 EXPORT_SYMBOL(pci_request_selected_regions);
+EXPORT_SYMBOL(pci_request_selected_regions_exclusive);
 EXPORT_SYMBOL(pci_set_master);
+EXPORT_SYMBOL(pci_clear_master);
 EXPORT_SYMBOL(pci_set_mwi);
 EXPORT_SYMBOL(pci_try_set_mwi);
 EXPORT_SYMBOL(pci_clear_mwi);
diff --git a/drivers/pci/pci.h b/drivers/pci/pci.h
index 9de87e9f98f5..1351bb4addde 100644
--- a/drivers/pci/pci.h
+++ b/drivers/pci/pci.h
@@ -10,6 +10,10 @@ extern int pci_uevent(struct device *dev, struct kobj_uevent_env *env);
 extern int pci_create_sysfs_dev_files(struct pci_dev *pdev);
 extern void pci_remove_sysfs_dev_files(struct pci_dev *pdev);
 extern void pci_cleanup_rom(struct pci_dev *dev);
+#ifdef HAVE_PCI_MMAP
+extern int pci_mmap_fits(struct pci_dev *pdev, int resno,
+			 struct vm_area_struct *vma);
+#endif
 
 /**
  * Firmware PM callbacks
@@ -40,7 +44,11 @@ struct pci_platform_pm_ops {
 };
 
 extern int pci_set_platform_pm(struct pci_platform_pm_ops *ops);
+extern void pci_update_current_state(struct pci_dev *dev, pci_power_t state);
+extern void pci_disable_enabled_device(struct pci_dev *dev);
 extern void pci_pm_init(struct pci_dev *dev);
+extern void platform_pci_wakeup_init(struct pci_dev *dev);
+extern void pci_allocate_cap_save_buffers(struct pci_dev *dev);
 
 extern int pci_user_read_config_byte(struct pci_dev *dev, int where, u8 *val);
 extern int pci_user_read_config_word(struct pci_dev *dev, int where, u16 *val);
@@ -50,14 +58,14 @@ extern int pci_user_write_config_word(struct pci_dev *dev, int where, u16 val);
 extern int pci_user_write_config_dword(struct pci_dev *dev, int where, u32 val);
 
 struct pci_vpd_ops {
-	int (*read)(struct pci_dev *dev, int pos, int size, char *buf);
-	int (*write)(struct pci_dev *dev, int pos, int size, const char *buf);
+	ssize_t (*read)(struct pci_dev *dev, loff_t pos, size_t count, void *buf);
+	ssize_t (*write)(struct pci_dev *dev, loff_t pos, size_t count, const void *buf);
 	void (*release)(struct pci_dev *dev);
 };
 
 struct pci_vpd {
 	unsigned int len;
-	struct pci_vpd_ops *ops;
+	const struct pci_vpd_ops *ops;
 	struct bin_attribute *attr; /* descriptor for sysfs VPD entry */
 };
 
@@ -98,11 +106,9 @@ extern unsigned int pci_pm_d3_delay;
 #ifdef CONFIG_PCI_MSI
 void pci_no_msi(void);
 extern void pci_msi_init_pci_dev(struct pci_dev *dev);
-extern void __devinit msi_init(void);
 #else
 static inline void pci_no_msi(void) { }
 static inline void pci_msi_init_pci_dev(struct pci_dev *dev) { }
-static inline void msi_init(void) { }
 #endif
 
 #ifdef CONFIG_PCIEAER
@@ -159,16 +165,28 @@ struct pci_slot_attribute {
 };
 #define to_pci_slot_attr(s) container_of(s, struct pci_slot_attribute, attr)
 
+enum pci_bar_type {
+	pci_bar_unknown,	/* Standard PCI BAR probe */
+	pci_bar_io,		/* An io port BAR */
+	pci_bar_mem32,		/* A 32-bit memory BAR */
+	pci_bar_mem64,		/* A 64-bit memory BAR */
+};
+
+extern int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
+				struct resource *res, unsigned int reg);
+extern int pci_resource_bar(struct pci_dev *dev, int resno,
+			    enum pci_bar_type *type);
+extern int pci_bus_add_child(struct pci_bus *bus);
 extern void pci_enable_ari(struct pci_dev *dev);
 /**
  * pci_ari_enabled - query ARI forwarding status
- * @dev: the PCI device
+ * @bus: the PCI bus
  *
  * Returns 1 if ARI forwarding is enabled, or 0 if not enabled;
  */
-static inline int pci_ari_enabled(struct pci_dev *dev)
+static inline int pci_ari_enabled(struct pci_bus *bus)
 {
-	return dev->ari_enabled;
+	return bus->self && bus->self->ari_enabled;
 }
 
 #endif /* DRIVERS_PCI_H */
diff --git a/drivers/pci/pcie/aer/aerdrv_acpi.c b/drivers/pci/pcie/aer/aerdrv_acpi.c
index 6dd7b13e9808..ebce26c37049 100644
--- a/drivers/pci/pcie/aer/aerdrv_acpi.c
+++ b/drivers/pci/pcie/aer/aerdrv_acpi.c
@@ -38,7 +38,6 @@ int aer_osc_setup(struct pcie_device *pciedev)
 
 	handle = acpi_find_root_bridge_handle(pdev);
 	if (handle) {
-		pcie_osc_support_set(OSC_EXT_PCI_CONFIG_SUPPORT);
 		status = pci_osc_control_set(handle,
 					OSC_PCI_EXPRESS_AER_CONTROL |
 					OSC_PCI_EXPRESS_CAP_STRUCTURE_CONTROL);
diff --git a/drivers/pci/pcie/aer/aerdrv_errprint.c b/drivers/pci/pcie/aer/aerdrv_errprint.c
index 3933d4f30e8c..0fc29ae80df8 100644
--- a/drivers/pci/pcie/aer/aerdrv_errprint.c
+++ b/drivers/pci/pcie/aer/aerdrv_errprint.c
@@ -233,7 +233,7 @@ void aer_print_error(struct pci_dev *dev, struct aer_err_info *info)
 
 		if (info->flags & AER_TLP_HEADER_VALID_FLAG) {
 			unsigned char *tlp = (unsigned char *) &info->tlp;
-			printk("%sTLB Header:\n", loglevel);
+			printk("%sTLP Header:\n", loglevel);
 			printk("%s%02x%02x%02x%02x %02x%02x%02x%02x"
 				" %02x%02x%02x%02x %02x%02x%02x%02x\n",
 				loglevel,
diff --git a/drivers/pci/pcie/aspm.c b/drivers/pci/pcie/aspm.c
index 9aad608bcf3f..586b6f75910d 100644
--- a/drivers/pci/pcie/aspm.c
+++ b/drivers/pci/pcie/aspm.c
@@ -17,6 +17,7 @@
 #include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/jiffies.h>
+#include <linux/delay.h>
 #include <linux/pci-aspm.h>
 #include "../pci.h"
 
@@ -33,6 +34,11 @@ struct endpoint_state {
 struct pcie_link_state {
 	struct list_head sibiling;
 	struct pci_dev *pdev;
+	bool downstream_has_switch;
+
+	struct pcie_link_state *parent;
+	struct list_head children;
+	struct list_head link;
 
 	/* ASPM state */
 	unsigned int support_state;
@@ -70,6 +76,8 @@ static const char *policy_str[] = {
 	[POLICY_POWERSAVE] = "powersave"
 };
 
+#define LINK_RETRAIN_TIMEOUT HZ
+
 static int policy_to_aspm_state(struct pci_dev *pdev)
 {
 	struct pcie_link_state *link_state = pdev->link_state;
@@ -125,7 +133,7 @@ static void pcie_set_clock_pm(struct pci_dev *pdev, int enable)
 	link_state->clk_pm_enabled = !!enable;
 }
 
-static void pcie_check_clock_pm(struct pci_dev *pdev)
+static void pcie_check_clock_pm(struct pci_dev *pdev, int blacklist)
 {
 	int pos;
 	u32 reg32;
@@ -149,10 +157,26 @@ static void pcie_check_clock_pm(struct pci_dev *pdev)
 		if (!(reg16 & PCI_EXP_LNKCTL_CLKREQ_EN))
 			enabled = 0;
 	}
-	link_state->clk_pm_capable = capable;
 	link_state->clk_pm_enabled = enabled;
 	link_state->bios_clk_state = enabled;
-	pcie_set_clock_pm(pdev, policy_to_clkpm_state(pdev));
+	if (!blacklist) {
+		link_state->clk_pm_capable = capable;
+		pcie_set_clock_pm(pdev, policy_to_clkpm_state(pdev));
+	} else {
+		link_state->clk_pm_capable = 0;
+		pcie_set_clock_pm(pdev, 0);
+	}
+}
+
+static bool pcie_aspm_downstream_has_switch(struct pci_dev *pdev)
+{
+	struct pci_dev *child_dev;
+
+	list_for_each_entry(child_dev, &pdev->subordinate->devices, bus_list) {
+		if (child_dev->pcie_type == PCI_EXP_TYPE_UPSTREAM)
+			return true;
+	}
+	return false;
 }
 
 /*
@@ -217,16 +241,18 @@ static void pcie_aspm_configure_common_clock(struct pci_dev *pdev)
 	pci_write_config_word(pdev, pos + PCI_EXP_LNKCTL, reg16);
 
 	/* Wait for link training end */
-	/* break out after waiting for 1 second */
+	/* break out after waiting for timeout */
 	start_jiffies = jiffies;
-	while ((jiffies - start_jiffies) < HZ) {
+	for (;;) {
 		pci_read_config_word(pdev, pos + PCI_EXP_LNKSTA, &reg16);
 		if (!(reg16 & PCI_EXP_LNKSTA_LT))
 			break;
-		cpu_relax();
+		if (time_after(jiffies, start_jiffies + LINK_RETRAIN_TIMEOUT))
+			break;
+		msleep(1);
 	}
 	/* training failed -> recover */
-	if ((jiffies - start_jiffies) >= HZ) {
+	if (reg16 & PCI_EXP_LNKSTA_LT) {
 		dev_printk (KERN_ERR, &pdev->dev, "ASPM: Could not configure"
 			    " common clock\n");
 		i = 0;
@@ -419,9 +445,9 @@ static unsigned int pcie_aspm_check_state(struct pci_dev *pdev,
 {
 	struct pci_dev *child_dev;
 
-	/* If no child, disable the link */
+	/* If no child, ignore the link */
 	if (list_empty(&pdev->subordinate->devices))
-		return 0;
+		return state;
 	list_for_each_entry(child_dev, &pdev->subordinate->devices, bus_list) {
 		if (child_dev->pcie_type == PCI_EXP_TYPE_PCI_BRIDGE) {
 			/*
@@ -462,6 +488,9 @@ static void __pcie_aspm_config_link(struct pci_dev *pdev, unsigned int state)
 	int valid = 1;
 	struct pcie_link_state *link_state = pdev->link_state;
 
+	/* If no child, disable the link */
+	if (list_empty(&pdev->subordinate->devices))
+		state = 0;
 	/*
 	 * if the downstream component has pci bridge function, don't do ASPM
 	 * now
@@ -493,20 +522,52 @@ static void __pcie_aspm_config_link(struct pci_dev *pdev, unsigned int state)
 	link_state->enabled_state = state;
 }
 
+static struct pcie_link_state *get_root_port_link(struct pcie_link_state *link)
+{
+	struct pcie_link_state *root_port_link = link;
+	while (root_port_link->parent)
+		root_port_link = root_port_link->parent;
+	return root_port_link;
+}
+
+/* check the whole hierarchy, and configure each link in the hierarchy */
 static void __pcie_aspm_configure_link_state(struct pci_dev *pdev,
 	unsigned int state)
 {
 	struct pcie_link_state *link_state = pdev->link_state;
+	struct pcie_link_state *root_port_link = get_root_port_link(link_state);
+	struct pcie_link_state *leaf;
 
-	if (link_state->support_state == 0)
-		return;
 	state &= PCIE_LINK_STATE_L0S|PCIE_LINK_STATE_L1;
 
-	/* state 0 means disabling aspm */
-	state = pcie_aspm_check_state(pdev, state);
+	/* check all links who have specific root port link */
+	list_for_each_entry(leaf, &link_list, sibiling) {
+		if (!list_empty(&leaf->children) ||
+			get_root_port_link(leaf) != root_port_link)
+			continue;
+		state = pcie_aspm_check_state(leaf->pdev, state);
+	}
+	/* check root port link too in case it hasn't children */
+	state = pcie_aspm_check_state(root_port_link->pdev, state);
+
 	if (link_state->enabled_state == state)
 		return;
-	__pcie_aspm_config_link(pdev, state);
+
+	/*
+	 * we must change the hierarchy. See comments in
+	 * __pcie_aspm_config_link for the order
+	 **/
+	if (state & PCIE_LINK_STATE_L1) {
+		list_for_each_entry(leaf, &link_list, sibiling) {
+			if (get_root_port_link(leaf) == root_port_link)
+				__pcie_aspm_config_link(leaf->pdev, state);
+		}
+	} else {
+		list_for_each_entry_reverse(leaf, &link_list, sibiling) {
+			if (get_root_port_link(leaf) == root_port_link)
+				__pcie_aspm_config_link(leaf->pdev, state);
+		}
+	}
 }
 
 /*
@@ -570,6 +631,7 @@ void pcie_aspm_init_link_state(struct pci_dev *pdev)
 	unsigned int state;
 	struct pcie_link_state *link_state;
 	int error = 0;
+	int blacklist;
 
 	if (aspm_disabled || !pdev->is_pcie || pdev->link_state)
 		return;
@@ -580,29 +642,58 @@ void pcie_aspm_init_link_state(struct pci_dev *pdev)
 	if (list_empty(&pdev->subordinate->devices))
 		goto out;
 
-	if (pcie_aspm_sanity_check(pdev))
-		goto out;
+	blacklist = !!pcie_aspm_sanity_check(pdev);
 
 	mutex_lock(&aspm_lock);
 
 	link_state = kzalloc(sizeof(*link_state), GFP_KERNEL);
 	if (!link_state)
 		goto unlock_out;
-	pdev->link_state = link_state;
 
-	pcie_aspm_configure_common_clock(pdev);
+	link_state->downstream_has_switch = pcie_aspm_downstream_has_switch(pdev);
+	INIT_LIST_HEAD(&link_state->children);
+	INIT_LIST_HEAD(&link_state->link);
+	if (pdev->bus->self) {/* this is a switch */
+		struct pcie_link_state *parent_link_state;
 
-	pcie_aspm_cap_init(pdev);
+		parent_link_state = pdev->bus->parent->self->link_state;
+		if (!parent_link_state) {
+			kfree(link_state);
+			goto unlock_out;
+		}
+		list_add(&link_state->link, &parent_link_state->children);
+		link_state->parent = parent_link_state;
+	}
 
-	/* config link state to avoid BIOS error */
-	state = pcie_aspm_check_state(pdev, policy_to_aspm_state(pdev));
-	__pcie_aspm_config_link(pdev, state);
+	pdev->link_state = link_state;
 
-	pcie_check_clock_pm(pdev);
+	if (!blacklist) {
+		pcie_aspm_configure_common_clock(pdev);
+		pcie_aspm_cap_init(pdev);
+	} else {
+		link_state->enabled_state = PCIE_LINK_STATE_L0S|PCIE_LINK_STATE_L1;
+		link_state->bios_aspm_state = 0;
+		/* Set support state to 0, so we will disable ASPM later */
+		link_state->support_state = 0;
+	}
 
 	link_state->pdev = pdev;
 	list_add(&link_state->sibiling, &link_list);
 
+	if (link_state->downstream_has_switch) {
+		/*
+		 * If link has switch, delay the link config. The leaf link
+		 * initialization will config the whole hierarchy. but we must
+		 * make sure BIOS doesn't set unsupported link state
+		 **/
+		state = pcie_aspm_check_state(pdev, link_state->bios_aspm_state);
+		__pcie_aspm_config_link(pdev, state);
+	} else
+		__pcie_aspm_configure_link_state(pdev,
+			policy_to_aspm_state(pdev));
+
+	pcie_check_clock_pm(pdev, blacklist);
+
 unlock_out:
 	if (error)
 		free_link_state(pdev);
@@ -635,6 +726,7 @@ void pcie_aspm_exit_link_state(struct pci_dev *pdev)
 	/* All functions are removed, so just disable ASPM for the link */
 	__pcie_aspm_config_one_dev(parent, 0);
 	list_del(&link_state->sibiling);
+	list_del(&link_state->link);
 	/* Clock PM is for endpoint device */
 
 	free_link_state(parent);
@@ -857,24 +949,15 @@ void pcie_no_aspm(void)
 		aspm_disabled = 1;
 }
 
-#ifdef CONFIG_ACPI
-#include <acpi/acpi_bus.h>
-#include <linux/pci-acpi.h>
-static void pcie_aspm_platform_init(void)
-{
-	pcie_osc_support_set(OSC_ACTIVE_STATE_PWR_SUPPORT|
-		OSC_CLOCK_PWR_CAPABILITY_SUPPORT);
-}
-#else
-static inline void pcie_aspm_platform_init(void) { }
-#endif
-
-static int __init pcie_aspm_init(void)
+/**
+ * pcie_aspm_enabled - is PCIe ASPM enabled?
+ *
+ * Returns true if ASPM has not been disabled by the command-line option
+ * pcie_aspm=off.
+ **/
+int pcie_aspm_enabled(void)
 {
-	if (aspm_disabled)
-		return 0;
-	pcie_aspm_platform_init();
-	return 0;
+       return !aspm_disabled;
 }
+EXPORT_SYMBOL(pcie_aspm_enabled);
 
-fs_initcall(pcie_aspm_init);
diff --git a/drivers/pci/pcie/portdrv_bus.c b/drivers/pci/pcie/portdrv_bus.c
index 359fe5568df1..eec89b767f9f 100644
--- a/drivers/pci/pcie/portdrv_bus.c
+++ b/drivers/pci/pcie/portdrv_bus.c
@@ -16,14 +16,10 @@
 #include "portdrv.h"
 
 static int pcie_port_bus_match(struct device *dev, struct device_driver *drv);
-static int pcie_port_bus_suspend(struct device *dev, pm_message_t state);
-static int pcie_port_bus_resume(struct device *dev);
 
 struct bus_type pcie_port_bus_type = {
 	.name 		= "pci_express",
 	.match 		= pcie_port_bus_match,
-	.suspend	= pcie_port_bus_suspend,
-	.resume		= pcie_port_bus_resume, 
 };
 EXPORT_SYMBOL_GPL(pcie_port_bus_type);
 
@@ -49,32 +45,12 @@ static int pcie_port_bus_match(struct device *dev, struct device_driver *drv)
 	return 1;
 }
 
-static int pcie_port_bus_suspend(struct device *dev, pm_message_t state)
+int pcie_port_bus_register(void)
 {
-	struct pcie_device *pciedev;
-	struct pcie_port_service_driver *driver;
-
-	if (!dev || !dev->driver)
-		return 0;
-
-	pciedev = to_pcie_device(dev);
- 	driver = to_service_driver(dev->driver);
-	if (driver && driver->suspend)
-		driver->suspend(pciedev, state);
-	return 0;
+	return bus_register(&pcie_port_bus_type);
 }
 
-static int pcie_port_bus_resume(struct device *dev)
+void pcie_port_bus_unregister(void)
 {
-	struct pcie_device *pciedev;
-	struct pcie_port_service_driver *driver;
-
-	if (!dev || !dev->driver)
-		return 0;
-
-	pciedev = to_pcie_device(dev);
- 	driver = to_service_driver(dev->driver);
-	if (driver && driver->resume)
-		driver->resume(pciedev);
-	return 0;
+	bus_unregister(&pcie_port_bus_type);
 }
diff --git a/drivers/pci/pcie/portdrv_core.c b/drivers/pci/pcie/portdrv_core.c
index 2e091e014829..8b3f8c18032f 100644
--- a/drivers/pci/pcie/portdrv_core.c
+++ b/drivers/pci/pcie/portdrv_core.c
@@ -19,91 +19,15 @@
 
 extern int pcie_mch_quirk;	/* MSI-quirk Indicator */
 
-static int pcie_port_probe_service(struct device *dev)
-{
-	struct pcie_device *pciedev;
-	struct pcie_port_service_driver *driver;
-	int status;
-
-	if (!dev || !dev->driver)
-		return -ENODEV;
-
- 	driver = to_service_driver(dev->driver);
-	if (!driver || !driver->probe)
-		return -ENODEV;
-
-	pciedev = to_pcie_device(dev);
-	status = driver->probe(pciedev, driver->id_table);
-	if (!status) {
-		dev_printk(KERN_DEBUG, dev, "service driver %s loaded\n",
-			driver->name);
-		get_device(dev);
-	}
-	return status;
-}
-
-static int pcie_port_remove_service(struct device *dev)
-{
-	struct pcie_device *pciedev;
-	struct pcie_port_service_driver *driver;
-
-	if (!dev || !dev->driver)
-		return 0;
-
-	pciedev = to_pcie_device(dev);
- 	driver = to_service_driver(dev->driver);
-	if (driver && driver->remove) { 
-		dev_printk(KERN_DEBUG, dev, "unloading service driver %s\n",
-			driver->name);
-		driver->remove(pciedev);
-		put_device(dev);
-	}
-	return 0;
-}
-
-static void pcie_port_shutdown_service(struct device *dev) {}
-
-static int pcie_port_suspend_service(struct device *dev, pm_message_t state)
-{
-	struct pcie_device *pciedev;
-	struct pcie_port_service_driver *driver;
-
-	if (!dev || !dev->driver)
-		return 0;
-
-	pciedev = to_pcie_device(dev);
- 	driver = to_service_driver(dev->driver);
-	if (driver && driver->suspend)
-		driver->suspend(pciedev, state);
-	return 0;
-}
-
-static int pcie_port_resume_service(struct device *dev)
-{
-	struct pcie_device *pciedev;
-	struct pcie_port_service_driver *driver;
-
-	if (!dev || !dev->driver)
-		return 0;
-
-	pciedev = to_pcie_device(dev);
- 	driver = to_service_driver(dev->driver);
-
-	if (driver && driver->resume)
-		driver->resume(pciedev);
-	return 0;
-}
-
-/*
- * release_pcie_device
- *	
- *	Being invoked automatically when device is being removed 
- *	in response to device_unregister(dev) call.
- *	Release all resources being claimed.
+/**
+ * release_pcie_device - free PCI Express port service device structure
+ * @dev: Port service device to release
+ *
+ * Invoked automatically when device is being removed in response to
+ * device_unregister(dev).  Release all resources being claimed.
  */
 static void release_pcie_device(struct device *dev)
 {
-	dev_printk(KERN_DEBUG, dev, "free port service\n");
 	kfree(to_pcie_device(dev));			
 }
 
@@ -128,7 +52,16 @@ static int is_msi_quirked(struct pci_dev *dev)
 	}
 	return quirk;
 }
-	
+
+/**
+ * assign_interrupt_mode - choose interrupt mode for PCI Express port services
+ *                         (INTx, MSI-X, MSI) and set up vectors
+ * @dev: PCI Express port to handle
+ * @vectors: Array of interrupt vectors to populate
+ * @mask: Bitmask of port capabilities returned by get_port_device_capability()
+ *
+ * Return value: Interrupt mode associated with the port
+ */
 static int assign_interrupt_mode(struct pci_dev *dev, int *vectors, int mask)
 {
 	int i, pos, nvec, status = -EINVAL;
@@ -150,7 +83,6 @@ static int assign_interrupt_mode(struct pci_dev *dev, int *vectors, int mask)
 	if (pos) {
 		struct msix_entry msix_entries[PCIE_PORT_DEVICE_MAXSERVICES] = 
 			{{0, 0}, {0, 1}, {0, 2}, {0, 3}};
-		dev_info(&dev->dev, "found MSI-X capability\n");
 		status = pci_enable_msix(dev, msix_entries, nvec);
 		if (!status) {
 			int j = 0;
@@ -165,7 +97,6 @@ static int assign_interrupt_mode(struct pci_dev *dev, int *vectors, int mask)
 	if (status) {
 		pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
 		if (pos) {
-			dev_info(&dev->dev, "found MSI capability\n");
 			status = pci_enable_msi(dev);
 			if (!status) {
 				interrupt_mode = PCIE_PORT_MSI_MODE;
@@ -177,6 +108,16 @@ static int assign_interrupt_mode(struct pci_dev *dev, int *vectors, int mask)
 	return interrupt_mode;
 }
 
+/**
+ * get_port_device_capability - discover capabilities of a PCI Express port
+ * @dev: PCI Express port to examine
+ *
+ * The capabilities are read from the port's PCI Express configuration registers
+ * as described in PCI Express Base Specification 1.0a sections 7.8.2, 7.8.9 and
+ * 7.9 - 7.11.
+ *
+ * Return value: Bitmask of discovered port capabilities
+ */
 static int get_port_device_capability(struct pci_dev *dev)
 {
 	int services = 0, pos;
@@ -204,6 +145,15 @@ static int get_port_device_capability(struct pci_dev *dev)
 	return services;
 }
 
+/**
+ * pcie_device_init - initialize PCI Express port service device
+ * @dev: Port service device to initialize
+ * @parent: PCI Express port to associate the service device with
+ * @port_type: Type of the port
+ * @service_type: Type of service to associate with the service device
+ * @irq: Interrupt vector to associate with the service device
+ * @irq_mode: Interrupt mode of the service (INTx, MSI-X, MSI)
+ */
 static void pcie_device_init(struct pci_dev *parent, struct pcie_device *dev, 
 	int port_type, int service_type, int irq, int irq_mode)
 {
@@ -224,11 +174,19 @@ static void pcie_device_init(struct pci_dev *parent, struct pcie_device *dev,
 	device->driver = NULL;
 	device->driver_data = NULL;
 	device->release = release_pcie_device;	/* callback to free pcie dev */
-	snprintf(device->bus_id, sizeof(device->bus_id), "%s:pcie%02x",
+	dev_set_name(device, "%s:pcie%02x",
 		 pci_name(parent), get_descriptor_id(port_type, service_type));
 	device->parent = &parent->dev;
 }
 
+/**
+ * alloc_pcie_device - allocate PCI Express port service device structure
+ * @parent: PCI Express port to associate the service device with
+ * @port_type: Type of the port
+ * @service_type: Type of service to associate with the service device
+ * @irq: Interrupt vector to associate with the service device
+ * @irq_mode: Interrupt mode of the service (INTx, MSI-X, MSI)
+ */
 static struct pcie_device* alloc_pcie_device(struct pci_dev *parent,
 	int port_type, int service_type, int irq, int irq_mode)
 {
@@ -239,10 +197,13 @@ static struct pcie_device* alloc_pcie_device(struct pci_dev *parent,
 		return NULL;
 
 	pcie_device_init(parent, device, port_type, service_type, irq,irq_mode);
-	dev_printk(KERN_DEBUG, &device->device, "allocate port service\n");
 	return device;
 }
 
+/**
+ * pcie_port_device_probe - check if device is a PCI Express port
+ * @dev: Device to check
+ */
 int pcie_port_device_probe(struct pci_dev *dev)
 {
 	int pos, type;
@@ -260,6 +221,13 @@ int pcie_port_device_probe(struct pci_dev *dev)
 	return -ENODEV;
 }
 
+/**
+ * pcie_port_device_register - register PCI Express port
+ * @dev: PCI Express port to register
+ *
+ * Allocate the port extension structure and register services associated with
+ * the port.
+ */
 int pcie_port_device_register(struct pci_dev *dev)
 {
 	struct pcie_port_device_ext *p_ext;
@@ -323,6 +291,11 @@ static int suspend_iter(struct device *dev, void *data)
 	return 0;
 }
 
+/**
+ * pcie_port_device_suspend - suspend port services associated with a PCIe port
+ * @dev: PCI Express port to handle
+ * @state: Representation of system power management transition in progress
+ */
 int pcie_port_device_suspend(struct pci_dev *dev, pm_message_t state)
 {
 	return device_for_each_child(&dev->dev, &state, suspend_iter);
@@ -341,6 +314,10 @@ static int resume_iter(struct device *dev, void *data)
 	return 0;
 }
 
+/**
+ * pcie_port_device_suspend - resume port services associated with a PCIe port
+ * @dev: PCI Express port to handle
+ */
 int pcie_port_device_resume(struct pci_dev *dev)
 {
 	return device_for_each_child(&dev->dev, NULL, resume_iter);
@@ -363,6 +340,13 @@ static int remove_iter(struct device *dev, void *data)
 	return 0;
 }
 
+/**
+ * pcie_port_device_remove - unregister PCI Express port service devices
+ * @dev: PCI Express port the service devices to unregister are associated with
+ *
+ * Remove PCI Express port service devices associated with given port and
+ * disable MSI-X or MSI for the port.
+ */
 void pcie_port_device_remove(struct pci_dev *dev)
 {
 	struct device *device;
@@ -386,16 +370,80 @@ void pcie_port_device_remove(struct pci_dev *dev)
 		pci_disable_msi(dev);
 }
 
-int pcie_port_bus_register(void)
+/**
+ * pcie_port_probe_service - probe driver for given PCI Express port service
+ * @dev: PCI Express port service device to probe against
+ *
+ * If PCI Express port service driver is registered with
+ * pcie_port_service_register(), this function will be called by the driver core
+ * whenever match is found between the driver and a port service device.
+ */
+static int pcie_port_probe_service(struct device *dev)
 {
-	return bus_register(&pcie_port_bus_type);
+	struct pcie_device *pciedev;
+	struct pcie_port_service_driver *driver;
+	int status;
+
+	if (!dev || !dev->driver)
+		return -ENODEV;
+
+	driver = to_service_driver(dev->driver);
+	if (!driver || !driver->probe)
+		return -ENODEV;
+
+	pciedev = to_pcie_device(dev);
+	status = driver->probe(pciedev, driver->id_table);
+	if (!status) {
+		dev_printk(KERN_DEBUG, dev, "service driver %s loaded\n",
+			driver->name);
+		get_device(dev);
+	}
+	return status;
 }
 
-void pcie_port_bus_unregister(void)
+/**
+ * pcie_port_remove_service - detach driver from given PCI Express port service
+ * @dev: PCI Express port service device to handle
+ *
+ * If PCI Express port service driver is registered with
+ * pcie_port_service_register(), this function will be called by the driver core
+ * when device_unregister() is called for the port service device associated
+ * with the driver.
+ */
+static int pcie_port_remove_service(struct device *dev)
 {
-	bus_unregister(&pcie_port_bus_type);
+	struct pcie_device *pciedev;
+	struct pcie_port_service_driver *driver;
+
+	if (!dev || !dev->driver)
+		return 0;
+
+	pciedev = to_pcie_device(dev);
+	driver = to_service_driver(dev->driver);
+	if (driver && driver->remove) {
+		dev_printk(KERN_DEBUG, dev, "unloading service driver %s\n",
+			driver->name);
+		driver->remove(pciedev);
+		put_device(dev);
+	}
+	return 0;
 }
 
+/**
+ * pcie_port_shutdown_service - shut down given PCI Express port service
+ * @dev: PCI Express port service device to handle
+ *
+ * If PCI Express port service driver is registered with
+ * pcie_port_service_register(), this function will be called by the driver core
+ * when device_shutdown() is called for the port service device associated
+ * with the driver.
+ */
+static void pcie_port_shutdown_service(struct device *dev) {}
+
+/**
+ * pcie_port_service_register - register PCI Express port service driver
+ * @new: PCI Express port service driver to register
+ */
 int pcie_port_service_register(struct pcie_port_service_driver *new)
 {
 	new->driver.name = (char *)new->name;
@@ -403,15 +451,17 @@ int pcie_port_service_register(struct pcie_port_service_driver *new)
 	new->driver.probe = pcie_port_probe_service;
 	new->driver.remove = pcie_port_remove_service;
 	new->driver.shutdown = pcie_port_shutdown_service;
-	new->driver.suspend = pcie_port_suspend_service;
-	new->driver.resume = pcie_port_resume_service;
 
 	return driver_register(&new->driver);
 }
 
-void pcie_port_service_unregister(struct pcie_port_service_driver *new)
+/**
+ * pcie_port_service_unregister - unregister PCI Express port service driver
+ * @drv: PCI Express port service driver to unregister
+ */
+void pcie_port_service_unregister(struct pcie_port_service_driver *drv)
 {
-	driver_unregister(&new->driver);
+	driver_unregister(&drv->driver);
 }
 
 EXPORT_SYMBOL(pcie_port_service_register);
diff --git a/drivers/pci/pcie/portdrv_pci.c b/drivers/pci/pcie/portdrv_pci.c
index 584422da8d8b..99a914a027f8 100644
--- a/drivers/pci/pcie/portdrv_pci.c
+++ b/drivers/pci/pcie/portdrv_pci.c
@@ -41,7 +41,6 @@ static int pcie_portdrv_restore_config(struct pci_dev *dev)
 {
 	int retval;
 
-	pci_restore_state(dev);
 	retval = pci_enable_device(dev);
 	if (retval)
 		return retval;
@@ -52,11 +51,18 @@ static int pcie_portdrv_restore_config(struct pci_dev *dev)
 #ifdef CONFIG_PM
 static int pcie_portdrv_suspend(struct pci_dev *dev, pm_message_t state)
 {
-	int ret = pcie_port_device_suspend(dev, state);
+	return pcie_port_device_suspend(dev, state);
 
-	if (!ret)
-		ret = pcie_portdrv_save_config(dev);
-	return ret;
+}
+
+static int pcie_portdrv_suspend_late(struct pci_dev *dev, pm_message_t state)
+{
+	return pci_save_state(dev);
+}
+
+static int pcie_portdrv_resume_early(struct pci_dev *dev)
+{
+	return pci_restore_state(dev);
 }
 
 static int pcie_portdrv_resume(struct pci_dev *dev)
@@ -66,6 +72,8 @@ static int pcie_portdrv_resume(struct pci_dev *dev)
 }
 #else
 #define pcie_portdrv_suspend NULL
+#define pcie_portdrv_suspend_late NULL
+#define pcie_portdrv_resume_early NULL
 #define pcie_portdrv_resume NULL
 #endif
 
@@ -221,6 +229,7 @@ static pci_ers_result_t pcie_portdrv_slot_reset(struct pci_dev *dev)
 
 	/* If fatal, restore cfg space for possible link reset at upstream */
 	if (dev->error_state == pci_channel_io_frozen) {
+		pci_restore_state(dev);
 		pcie_portdrv_restore_config(dev);
 		pci_enable_pcie_error_reporting(dev);
 	}
@@ -283,6 +292,8 @@ static struct pci_driver pcie_portdriver = {
 	.remove		= pcie_portdrv_remove,
 
 	.suspend	= pcie_portdrv_suspend,
+	.suspend_late	= pcie_portdrv_suspend_late,
+	.resume_early	= pcie_portdrv_resume_early,
 	.resume		= pcie_portdrv_resume,
 
 	.err_handler 	= &pcie_portdrv_err_handler,
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index 5b3f5937ecf5..303644614eea 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -135,13 +135,6 @@ static u64 pci_size(u64 base, u64 maxbase, u64 mask)
 	return size;
 }
 
-enum pci_bar_type {
-	pci_bar_unknown,	/* Standard PCI BAR probe */
-	pci_bar_io,		/* An io port BAR */
-	pci_bar_mem32,		/* A 32-bit memory BAR */
-	pci_bar_mem64,		/* A 64-bit memory BAR */
-};
-
 static inline enum pci_bar_type decode_bar(struct resource *res, u32 bar)
 {
 	if ((bar & PCI_BASE_ADDRESS_SPACE) == PCI_BASE_ADDRESS_SPACE_IO) {
@@ -156,11 +149,16 @@ static inline enum pci_bar_type decode_bar(struct resource *res, u32 bar)
 	return pci_bar_mem32;
 }
 
-/*
- * If the type is not unknown, we assume that the lowest bit is 'enable'.
- * Returns 1 if the BAR was 64-bit and 0 if it was 32-bit.
+/**
+ * pci_read_base - read a PCI BAR
+ * @dev: the PCI device
+ * @type: type of the BAR
+ * @res: resource buffer to be filled in
+ * @pos: BAR position in the config space
+ *
+ * Returns 1 if the BAR is 64-bit, or 0 if 32-bit.
  */
-static int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
+int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
 			struct resource *res, unsigned int pos)
 {
 	u32 l, sz, mask;
@@ -400,19 +398,17 @@ static struct pci_bus *pci_alloc_child_bus(struct pci_bus *parent,
 	if (!child)
 		return NULL;
 
-	child->self = bridge;
 	child->parent = parent;
 	child->ops = parent->ops;
 	child->sysdata = parent->sysdata;
 	child->bus_flags = parent->bus_flags;
-	child->bridge = get_device(&bridge->dev);
 
 	/* initialize some portions of the bus device, but don't register it
 	 * now as the parent is not properly set up yet.  This device will get
 	 * registered later in pci_bus_add_devices()
 	 */
 	child->dev.class = &pcibus_class;
-	sprintf(child->dev.bus_id, "%04x:%02x", pci_domain_nr(child), busnr);
+	dev_set_name(&child->dev, "%04x:%02x", pci_domain_nr(child), busnr);
 
 	/*
 	 * Set up the primary, secondary and subordinate
@@ -422,8 +418,14 @@ static struct pci_bus *pci_alloc_child_bus(struct pci_bus *parent,
 	child->primary = parent->secondary;
 	child->subordinate = 0xff;
 
+	if (!bridge)
+		return child;
+
+	child->self = bridge;
+	child->bridge = get_device(&bridge->dev);
+
 	/* Set up default resource pointers and names.. */
-	for (i = 0; i < 4; i++) {
+	for (i = 0; i < PCI_BRIDGE_RESOURCE_NUM; i++) {
 		child->resource[i] = &bridge->resource[PCI_BRIDGE_RESOURCES+i];
 		child->resource[i]->name = child->name;
 	}
@@ -958,8 +960,12 @@ static void pci_init_capabilities(struct pci_dev *dev)
 	/* MSI/MSI-X list */
 	pci_msi_init_pci_dev(dev);
 
+	/* Buffers for saving PCIe and PCI-X capabilities */
+	pci_allocate_cap_save_buffers(dev);
+
 	/* Power Management */
 	pci_pm_init(dev);
+	platform_pci_wakeup_init(dev);
 
 	/* Vital Product Data */
 	pci_vpd_pci22_init(dev);
@@ -1130,7 +1136,7 @@ struct pci_bus * pci_create_bus(struct device *parent,
 	memset(dev, 0, sizeof(*dev));
 	dev->parent = parent;
 	dev->release = pci_release_bus_bridge_dev;
-	sprintf(dev->bus_id, "pci%04x:%02x", pci_domain_nr(b), bus);
+	dev_set_name(dev, "pci%04x:%02x", pci_domain_nr(b), bus);
 	error = device_register(dev);
 	if (error)
 		goto dev_reg_err;
@@ -1141,7 +1147,7 @@ struct pci_bus * pci_create_bus(struct device *parent,
 
 	b->dev.class = &pcibus_class;
 	b->dev.parent = b->bridge;
-	sprintf(b->dev.bus_id, "%04x:%02x", pci_domain_nr(b), bus);
+	dev_set_name(&b->dev, "%04x:%02x", pci_domain_nr(b), bus);
 	error = device_register(&b->dev);
 	if (error)
 		goto class_dev_reg_err;
diff --git a/drivers/pci/proc.c b/drivers/pci/proc.c
index e1098c302c45..593bb844b8db 100644
--- a/drivers/pci/proc.c
+++ b/drivers/pci/proc.c
@@ -252,11 +252,20 @@ static int proc_bus_pci_mmap(struct file *file, struct vm_area_struct *vma)
 	const struct proc_dir_entry *dp = PDE(inode);
 	struct pci_dev *dev = dp->data;
 	struct pci_filp_private *fpriv = file->private_data;
-	int ret;
+	int i, ret;
 
 	if (!capable(CAP_SYS_RAWIO))
 		return -EPERM;
 
+	/* Make sure the caller is mapping a real resource for this device */
+	for (i = 0; i < PCI_ROM_RESOURCE; i++) {
+		if (pci_mmap_fits(dev, i, vma))
+			break;
+	}
+
+	if (i >= PCI_ROM_RESOURCE)
+		return -ENODEV;
+
 	ret = pci_mmap_page_range(dev, vma,
 				  fpriv->mmap_state,
 				  fpriv->write_combine);
@@ -352,15 +361,16 @@ static int show_device(struct seq_file *m, void *v)
 			dev->vendor,
 			dev->device,
 			dev->irq);
-	/* Here should be 7 and not PCI_NUM_RESOURCES as we need to preserve compatibility */
-	for (i=0; i<7; i++) {
+
+	/* only print standard and ROM resources to preserve compatibility */
+	for (i = 0; i <= PCI_ROM_RESOURCE; i++) {
 		resource_size_t start, end;
 		pci_resource_to_user(dev, i, &dev->resource[i], &start, &end);
 		seq_printf(m, "\t%16llx",
 			(unsigned long long)(start |
 			(dev->resource[i].flags & PCI_REGION_FLAG_MASK)));
 	}
-	for (i=0; i<7; i++) {
+	for (i = 0; i <= PCI_ROM_RESOURCE; i++) {
 		resource_size_t start, end;
 		pci_resource_to_user(dev, i, &dev->resource[i], &start, &end);
 		seq_printf(m, "\t%16llx",
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index ce0985615133..baad093aafe3 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -56,7 +56,7 @@ static void quirk_passive_release(struct pci_dev *dev)
 	while ((d = pci_get_device(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_82371SB_0, d))) {
 		pci_read_config_byte(d, 0x82, &dlc);
 		if (!(dlc & 1<<1)) {
-			dev_err(&d->dev, "PIIX3: Enabling Passive Release\n");
+			dev_info(&d->dev, "PIIX3: Enabling Passive Release\n");
 			dlc |= 1<<1;
 			pci_write_config_byte(d, 0x82, dlc);
 		}
@@ -449,7 +449,7 @@ DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,    PCI_DEVICE_ID_INTEL_82801DB_12,
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,    PCI_DEVICE_ID_INTEL_82801EB_0,		quirk_ich4_lpc_acpi);
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,    PCI_DEVICE_ID_INTEL_ESB_1,		quirk_ich4_lpc_acpi);
 
-static void __devinit quirk_ich6_lpc_acpi(struct pci_dev *dev)
+static void __devinit ich6_lpc_acpi_gpio(struct pci_dev *dev)
 {
 	u32 region;
 
@@ -459,20 +459,95 @@ static void __devinit quirk_ich6_lpc_acpi(struct pci_dev *dev)
 	pci_read_config_dword(dev, 0x48, &region);
 	quirk_io_region(dev, region, 64, PCI_BRIDGE_RESOURCES+1, "ICH6 GPIO");
 }
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH6_0, quirk_ich6_lpc_acpi);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH6_1, quirk_ich6_lpc_acpi);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH7_0, quirk_ich6_lpc_acpi);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH7_1, quirk_ich6_lpc_acpi);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH7_31, quirk_ich6_lpc_acpi);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH8_0, quirk_ich6_lpc_acpi);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH8_2, quirk_ich6_lpc_acpi);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH8_3, quirk_ich6_lpc_acpi);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH8_1, quirk_ich6_lpc_acpi);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH8_4, quirk_ich6_lpc_acpi);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH9_2, quirk_ich6_lpc_acpi);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH9_4, quirk_ich6_lpc_acpi);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH9_7, quirk_ich6_lpc_acpi);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH9_8, quirk_ich6_lpc_acpi);
+
+static void __devinit ich6_lpc_generic_decode(struct pci_dev *dev, unsigned reg, const char *name, int dynsize)
+{
+	u32 val;
+	u32 size, base;
+
+	pci_read_config_dword(dev, reg, &val);
+
+	/* Enabled? */
+	if (!(val & 1))
+		return;
+	base = val & 0xfffc;
+	if (dynsize) {
+		/*
+		 * This is not correct. It is 16, 32 or 64 bytes depending on
+		 * register D31:F0:ADh bits 5:4.
+		 *
+		 * But this gets us at least _part_ of it.
+		 */
+		size = 16;
+	} else {
+		size = 128;
+	}
+	base &= ~(size-1);
+
+	/* Just print it out for now. We should reserve it after more debugging */
+	dev_info(&dev->dev, "%s PIO at %04x-%04x\n", name, base, base+size-1);
+}
+
+static void __devinit quirk_ich6_lpc(struct pci_dev *dev)
+{
+	/* Shared ACPI/GPIO decode with all ICH6+ */
+	ich6_lpc_acpi_gpio(dev);
+
+	/* ICH6-specific generic IO decode */
+	ich6_lpc_generic_decode(dev, 0x84, "LPC Generic IO decode 1", 0);
+	ich6_lpc_generic_decode(dev, 0x88, "LPC Generic IO decode 2", 1);
+}
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH6_0, quirk_ich6_lpc);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH6_1, quirk_ich6_lpc);
+
+static void __devinit ich7_lpc_generic_decode(struct pci_dev *dev, unsigned reg, const char *name)
+{
+	u32 val;
+	u32 mask, base;
+
+	pci_read_config_dword(dev, reg, &val);
+
+	/* Enabled? */
+	if (!(val & 1))
+		return;
+
+	/*
+	 * IO base in bits 15:2, mask in bits 23:18, both
+	 * are dword-based
+	 */
+	base = val & 0xfffc;
+	mask = (val >> 16) & 0xfc;
+	mask |= 3;
+
+	/* Just print it out for now. We should reserve it after more debugging */
+	dev_info(&dev->dev, "%s PIO at %04x (mask %04x)\n", name, base, mask);
+}
+
+/* ICH7-10 has the same common LPC generic IO decode registers */
+static void __devinit quirk_ich7_lpc(struct pci_dev *dev)
+{
+	/* We share the common ACPI/DPIO decode with ICH6 */
+	ich6_lpc_acpi_gpio(dev);
+
+	/* And have 4 ICH7+ generic decodes */
+	ich7_lpc_generic_decode(dev, 0x84, "ICH7 LPC Generic IO decode 1");
+	ich7_lpc_generic_decode(dev, 0x88, "ICH7 LPC Generic IO decode 2");
+	ich7_lpc_generic_decode(dev, 0x8c, "ICH7 LPC Generic IO decode 3");
+	ich7_lpc_generic_decode(dev, 0x90, "ICH7 LPC Generic IO decode 4");
+}
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH7_0, quirk_ich7_lpc);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH7_1, quirk_ich7_lpc);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH7_31, quirk_ich7_lpc);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH8_0, quirk_ich7_lpc);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH8_2, quirk_ich7_lpc);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH8_3, quirk_ich7_lpc);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH8_1, quirk_ich7_lpc);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH8_4, quirk_ich7_lpc);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH9_2, quirk_ich7_lpc);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH9_4, quirk_ich7_lpc);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH9_7, quirk_ich7_lpc);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_ICH9_8, quirk_ich7_lpc);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,   PCI_DEVICE_ID_INTEL_ICH10_1, quirk_ich7_lpc);
 
 /*
  * VIA ACPI: One IO region pointed to by longword at
@@ -2074,11 +2149,12 @@ DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATI, 0x4375,
 
 #endif /* CONFIG_PCI_MSI */
 
-static void pci_do_fixups(struct pci_dev *dev, struct pci_fixup *f, struct pci_fixup *end)
+static void pci_do_fixups(struct pci_dev *dev, struct pci_fixup *f,
+			  struct pci_fixup *end)
 {
 	while (f < end) {
 		if ((f->vendor == dev->vendor || f->vendor == (u16) PCI_ANY_ID) &&
- 		    (f->device == dev->device || f->device == (u16) PCI_ANY_ID)) {
+		    (f->device == dev->device || f->device == (u16) PCI_ANY_ID)) {
 			dev_dbg(&dev->dev, "calling %pF\n", f->hook);
 			f->hook(dev);
 		}
diff --git a/drivers/pci/setup-bus.c b/drivers/pci/setup-bus.c
index ea979f2bc6db..704608945780 100644
--- a/drivers/pci/setup-bus.c
+++ b/drivers/pci/setup-bus.c
@@ -536,9 +536,8 @@ static void pci_bus_dump_res(struct pci_bus *bus)
                 if (!res)
                         continue;
 
-		printk(KERN_INFO "bus: %02x index %x %s: %pR\n",
-		       bus->number, i,
-		       (res->flags & IORESOURCE_IO) ? "io port" : "mmio", res);
+		dev_printk(KERN_DEBUG, &bus->dev, "resource %d %s %pR\n", i,
+			   (res->flags & IORESOURCE_IO) ? "io: " : "mem:", res);
         }
 }
 
diff --git a/drivers/pci/setup-res.c b/drivers/pci/setup-res.c
index 2dbd96cce2d8..32e8d88a4619 100644
--- a/drivers/pci/setup-res.c
+++ b/drivers/pci/setup-res.c
@@ -26,11 +26,13 @@
 #include "pci.h"
 
 
-void pci_update_resource(struct pci_dev *dev, struct resource *res, int resno)
+void pci_update_resource(struct pci_dev *dev, int resno)
 {
 	struct pci_bus_region region;
 	u32 new, check, mask;
 	int reg;
+	enum pci_bar_type type;
+	struct resource *res = dev->resource + resno;
 
 	/*
 	 * Ignore resources for unimplemented BARs and unused resource slots
@@ -61,17 +63,13 @@ void pci_update_resource(struct pci_dev *dev, struct resource *res, int resno)
 	else
 		mask = (u32)PCI_BASE_ADDRESS_MEM_MASK;
 
-	if (resno < 6) {
-		reg = PCI_BASE_ADDRESS_0 + 4 * resno;
-	} else if (resno == PCI_ROM_RESOURCE) {
+	reg = pci_resource_bar(dev, resno, &type);
+	if (!reg)
+		return;
+	if (type != pci_bar_unknown) {
 		if (!(res->flags & IORESOURCE_ROM_ENABLE))
 			return;
 		new |= PCI_ROM_ADDRESS_ENABLE;
-		reg = dev->rom_base_reg;
-	} else {
-		/* Hmm, non-standard resource. */
-	
-		return;		/* kill uninitialised var warning */
 	}
 
 	pci_write_config_dword(dev, reg, new);
@@ -134,7 +132,7 @@ int pci_assign_resource(struct pci_dev *dev, int resno)
 
 	align = resource_alignment(res);
 	if (!align) {
-		dev_err(&dev->dev, "BAR %d: can't allocate resource (bogus "
+		dev_info(&dev->dev, "BAR %d: can't allocate resource (bogus "
 			"alignment) %pR flags %#lx\n",
 			resno, res, res->flags);
 		return -EINVAL;
@@ -157,12 +155,12 @@ int pci_assign_resource(struct pci_dev *dev, int resno)
 	}
 
 	if (ret) {
-		dev_err(&dev->dev, "BAR %d: can't allocate %s resource %pR\n",
+		dev_info(&dev->dev, "BAR %d: can't allocate %s resource %pR\n",
 			resno, res->flags & IORESOURCE_IO ? "I/O" : "mem", res);
 	} else {
 		res->flags &= ~IORESOURCE_STARTALIGN;
 		if (resno < PCI_BRIDGE_RESOURCES)
-			pci_update_resource(dev, res, resno);
+			pci_update_resource(dev, resno);
 	}
 
 	return ret;
@@ -197,7 +195,7 @@ int pci_assign_resource_fixed(struct pci_dev *dev, int resno)
 		dev_err(&dev->dev, "BAR %d: can't allocate %s resource %pR\n",
 			resno, res->flags & IORESOURCE_IO ? "I/O" : "mem", res);
 	} else if (resno < PCI_BRIDGE_RESOURCES) {
-		pci_update_resource(dev, res, resno);
+		pci_update_resource(dev, resno);
 	}
 
 	return ret;
diff --git a/drivers/pnp/card.c b/drivers/pnp/card.c
index e75b060daa95..efea128f02da 100644
--- a/drivers/pnp/card.c
+++ b/drivers/pnp/card.c
@@ -165,8 +165,7 @@ struct pnp_card *pnp_alloc_card(struct pnp_protocol *protocol, int id, char *pnp
 	card->number = id;
 
 	card->dev.parent = &card->protocol->dev;
-	sprintf(card->dev.bus_id, "%02x:%02x", card->protocol->number,
-		card->number);
+	dev_set_name(&card->dev, "%02x:%02x", card->protocol->number, card->number);
 
 	card->dev.coherent_dma_mask = DMA_24BIT_MASK;
 	card->dev.dma_mask = &card->dev.coherent_dma_mask;
@@ -295,8 +294,8 @@ int pnp_add_card_device(struct pnp_card *card, struct pnp_dev *dev)
 {
 	dev->dev.parent = &card->dev;
 	dev->card_link = NULL;
-	snprintf(dev->dev.bus_id, BUS_ID_SIZE, "%02x:%02x.%02x",
-		 dev->protocol->number, card->number, dev->number);
+	dev_set_name(&dev->dev, "%02x:%02x.%02x",
+		     dev->protocol->number, card->number, dev->number);
 	spin_lock(&pnp_lock);
 	dev->card = card;
 	list_add_tail(&dev->card_list, &card->devices);
diff --git a/drivers/pnp/core.c b/drivers/pnp/core.c
index 16c01c6fa7c5..14814f231739 100644
--- a/drivers/pnp/core.c
+++ b/drivers/pnp/core.c
@@ -70,7 +70,7 @@ int pnp_register_protocol(struct pnp_protocol *protocol)
 	spin_unlock(&pnp_lock);
 
 	protocol->number = nodenum;
-	sprintf(protocol->dev.bus_id, "pnp%d", nodenum);
+	dev_set_name(&protocol->dev, "pnp%d", nodenum);
 	return device_register(&protocol->dev);
 }
 
@@ -145,8 +145,7 @@ struct pnp_dev *pnp_alloc_dev(struct pnp_protocol *protocol, int id, char *pnpid
 	dev->dev.coherent_dma_mask = dev->dma_mask;
 	dev->dev.release = &pnp_release_device;
 
-	sprintf(dev->dev.bus_id, "%02x:%02x", dev->protocol->number,
-		dev->number);
+	dev_set_name(&dev->dev, "%02x:%02x", dev->protocol->number, dev->number);
 
 	dev_id = pnp_add_id(dev, pnpid);
 	if (!dev_id) {
diff --git a/drivers/pnp/system.c b/drivers/pnp/system.c
index 764f3a310685..59b90922da8c 100644
--- a/drivers/pnp/system.c
+++ b/drivers/pnp/system.c
@@ -26,7 +26,7 @@ static void reserve_range(struct pnp_dev *dev, resource_size_t start,
 			  resource_size_t end, int port)
 {
 	char *regionid;
-	const char *pnpid = dev->dev.bus_id;
+	const char *pnpid = dev_name(&dev->dev);
 	struct resource *res;
 
 	regionid = kmalloc(16, GFP_KERNEL);
diff --git a/drivers/power/ds2760_battery.c b/drivers/power/ds2760_battery.c
index 308ddb201b66..1d768928e0bb 100644
--- a/drivers/power/ds2760_battery.c
+++ b/drivers/power/ds2760_battery.c
@@ -354,7 +354,7 @@ static int ds2760_battery_probe(struct platform_device *pdev)
 	pdata = pdev->dev.platform_data;
 	di->dev		= &pdev->dev;
 	di->w1_dev	     = pdev->dev.parent;
-	di->bat.name	   = pdev->dev.bus_id;
+	di->bat.name	   = dev_name(&pdev->dev);
 	di->bat.type	   = POWER_SUPPLY_TYPE_BATTERY;
 	di->bat.properties     = ds2760_battery_props;
 	di->bat.num_properties = ARRAY_SIZE(ds2760_battery_props);
@@ -371,7 +371,7 @@ static int ds2760_battery_probe(struct platform_device *pdev)
 	}
 
 	INIT_DELAYED_WORK(&di->monitor_work, ds2760_battery_work);
-	di->monitor_wqueue = create_singlethread_workqueue(pdev->dev.bus_id);
+	di->monitor_wqueue = create_singlethread_workqueue(dev_name(&pdev->dev));
 	if (!di->monitor_wqueue) {
 		retval = -ESRCH;
 		goto workqueue_failed;
diff --git a/drivers/rapidio/rio-driver.c b/drivers/rapidio/rio-driver.c
index 956d3e79f6aa..addb87cf44d9 100644
--- a/drivers/rapidio/rio-driver.c
+++ b/drivers/rapidio/rio-driver.c
@@ -79,7 +79,6 @@ void rio_dev_put(struct rio_dev *rdev)
 
 /**
  *  rio_device_probe - Tell if a RIO device structure has a matching RIO device id structure
- *  @id: the RIO device id structure to match against
  *  @dev: the RIO device structure to match against
  *
  * return 0 and set rio_dev->driver when drv claims rio_dev, else error
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index 165a81843357..4ad831de41ad 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -35,8 +35,8 @@ config RTC_HCTOSYS_DEVICE
 	default "rtc0"
 	help
 	  The RTC device that will be used to (re)initialize the system
-	  clock, usually rtc0.  Initialization is done when the system
-	  starts up, and when it resumes from a low power state.  This
+	  clock, usually rtc0. Initialization is done when the system
+	  starts up, and when it resumes from a low power state. This
 	  device should record time in UTC, since the kernel won't do
 	  timezone correction.
 
@@ -44,7 +44,7 @@ config RTC_HCTOSYS_DEVICE
 	  functions run, so it must usually be statically linked.
 
 	  This clock should be battery-backed, so that it reads the correct
-	  time when the system boots from a power-off state.  Otherwise, your
+	  time when the system boots from a power-off state. Otherwise, your
 	  system will need an external clock source (like an NTP server).
 
 	  If the clock you specify here is not battery backed, it may still
@@ -69,8 +69,7 @@ config RTC_INTF_SYSFS
 	  Say yes here if you want to use your RTCs using sysfs interfaces,
 	  /sys/class/rtc/rtc0 through /sys/.../rtcN.
 
-	  This driver can also be built as a module. If so, the module
-	  will be called rtc-sysfs.
+	  If unsure, say Y.
 
 config RTC_INTF_PROC
 	boolean "/proc/driver/rtc (procfs for rtc0)"
@@ -78,11 +77,10 @@ config RTC_INTF_PROC
 	default RTC_CLASS
 	help
 	  Say yes here if you want to use your first RTC through the proc
-	  interface, /proc/driver/rtc.  Other RTCs will not be available
+	  interface, /proc/driver/rtc. Other RTCs will not be available
 	  through that API.
 
-	  This driver can also be built as a module. If so, the module
-	  will be called rtc-proc.
+	  If unsure, say Y.
 
 config RTC_INTF_DEV
 	boolean "/dev/rtcN (character devices)"
@@ -90,12 +88,14 @@ config RTC_INTF_DEV
 	help
 	  Say yes here if you want to use your RTCs using the /dev
 	  interfaces, which "udev" sets up as /dev/rtc0 through
-	  /dev/rtcN.  You may want to set up a symbolic link so one
-	  of these can be accessed as /dev/rtc, which is a name
-	  expected by "hwclock" and some other programs.
+	  /dev/rtcN.
 
-	  This driver can also be built as a module. If so, the module
-	  will be called rtc-dev.
+	  You may want to set up a symbolic link so one of these
+	  can be accessed as /dev/rtc, which is a name
+	  expected by "hwclock" and some other programs. Recent
+	  versions of "udev" are known to set up the symlink for you.
+
+	  If unsure, say Y.
 
 config RTC_INTF_DEV_UIE_EMUL
 	bool "RTC UIE emulation on dev interface"
@@ -132,14 +132,14 @@ config RTC_DRV_DS1307
 	tristate "Dallas/Maxim DS1307/37/38/39/40, ST M41T00"
 	help
 	  If you say yes here you get support for various compatible RTC
-	  chips (often with battery backup) connected with I2C.  This driver
+	  chips (often with battery backup) connected with I2C. This driver
 	  should handle DS1307, DS1337, DS1338, DS1339, DS1340, ST M41T00,
-	  and probably other chips.  In some cases the RTC must already
+	  and probably other chips. In some cases the RTC must already
 	  have been initialized (by manufacturing or a bootloader).
 
 	  The first seven registers on these chips hold an RTC, and other
 	  registers may add features such as NVRAM, a trickle charger for
-	  the RTC/NVRAM backup power, and alarms.  NVRAM is visible in
+	  the RTC/NVRAM backup power, and alarms. NVRAM is visible in
 	  sysfs, but other chip features may not be available.
 
 	  This driver can also be built as a module. If so, the module
@@ -150,10 +150,10 @@ config RTC_DRV_DS1374
 	depends on RTC_CLASS && I2C
 	help
 	  If you say yes here you get support for Dallas Semiconductor
-	  DS1374 real-time clock chips.  If an interrupt is associated
+	  DS1374 real-time clock chips. If an interrupt is associated
 	  with the device, the alarm functionality is supported.
 
-	  This driver can also be built as a module.  If so, the module
+	  This driver can also be built as a module. If so, the module
 	  will be called rtc-ds1374.
 
 config RTC_DRV_DS1672
@@ -247,7 +247,7 @@ config RTC_DRV_TWL92330
 	help
 	  If you say yes here you get support for the RTC on the
 	  TWL92330 "Menelaus" power management chip, used with OMAP2
-	  platforms.  The support is integrated with the rest of
+	  platforms. The support is integrated with the rest of
 	  the Menelaus driver; it's not separate module.
 
 config RTC_DRV_TWL4030
@@ -308,7 +308,7 @@ config RTC_DRV_DS1305
 	tristate "Dallas/Maxim DS1305/DS1306"
 	help
 	  Select this driver to get support for the Dallas/Maxim DS1305
-	  and DS1306 real time clock chips.  These support a trickle
+	  and DS1306 real time clock chips. These support a trickle
 	  charger, alarms, and NVRAM in addition to the clock.
 
 	  This driver can also be built as a module. If so, the module
@@ -317,7 +317,8 @@ config RTC_DRV_DS1305
 config RTC_DRV_DS1390
 	tristate "Dallas/Maxim DS1390/93/94"
 	help
-	  If you say yes here you get support for the DS1390/93/94 chips.
+	  If you say yes here you get support for the
+	  Dallas/Maxim DS1390/93/94 chips.
 
 	  This driver only supports the RTC feature, and not other chip
 	  features such as alarms and trickle charging.
@@ -381,7 +382,7 @@ config RTC_DRV_CMOS
 	  or LPC bus chips, and so on.
 
 	  Your system will need to define the platform device used by
-	  this driver, otherwise it won't be accessible.  This means
+	  this driver, otherwise it won't be accessible. This means
 	  you can safely enable this driver if you don't know whether
 	  or not your board has this kind of hardware.
 
@@ -598,7 +599,7 @@ config RTC_DRV_AT91RM9200
 	depends on ARCH_AT91RM9200 || ARCH_AT91SAM9RL
 	help
 	  Driver for the internal RTC (Realtime Clock) module found on
-	  Atmel AT91RM9200's and AT91SAM9RL chips.  On SAM9RL chips
+	  Atmel AT91RM9200's and AT91SAM9RL chips. On SAM9RL chips
 	  this is powered by the backup power supply.
 
 config RTC_DRV_AT91SAM9
@@ -620,8 +621,8 @@ config RTC_DRV_AT91SAM9_RTT
 	prompt "RTT module Number" if ARCH_AT91SAM9263
 	depends on RTC_DRV_AT91SAM9
 	help
-	  More than one RTT module is available.  You can choose which
-	  one will be used as an RTC.  The default of zero is normally
+	  More than one RTT module is available. You can choose which
+	  one will be used as an RTC. The default of zero is normally
 	  OK to use, though some systems use that for non-RTC purposes.
 
 config RTC_DRV_AT91SAM9_GPBR
@@ -633,10 +634,20 @@ config RTC_DRV_AT91SAM9_GPBR
 	depends on RTC_DRV_AT91SAM9
 	help
 	  The RTC driver needs to use one of the General Purpose Backup
-	  Registers (GPBRs) as well as the RTT.  You can choose which one
-	  will be used.  The default of zero is normally OK to use, but
+	  Registers (GPBRs) as well as the RTT. You can choose which one
+	  will be used. The default of zero is normally OK to use, but
 	  on some systems other software needs to use that register.
 
+config RTC_DRV_AU1XXX
+	tristate "Au1xxx Counter0 RTC support"
+	depends on SOC_AU1X00
+	help
+	  This is a driver for the Au1xxx on-chip Counter0 (Time-Of-Year
+	  counter) to be used as a RTC.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-au1xxx.
+
 config RTC_DRV_BFIN
 	tristate "Blackfin On-Chip RTC"
 	depends on BLACKFIN && !BF561
@@ -669,6 +680,17 @@ config RTC_DRV_PPC
 	 the RTC. This exposes that functionality through the generic RTC
 	 class.
 
+config RTC_DRV_PXA
+       tristate "PXA27x/PXA3xx"
+       depends on ARCH_PXA
+       help
+         If you say Y here you will get access to the real time clock
+         built into your PXA27x or PXA3xx CPU.
+
+         This RTC driver uses PXA RTC registers available since pxa27x
+         series (RDxR, RYxR) instead of legacy RCNR, RTAR.
+
+
 config RTC_DRV_SUN4V
 	bool "SUN4V Hypervisor RTC"
 	depends on SPARC64
@@ -683,4 +705,22 @@ config RTC_DRV_STARFIRE
 	  If you say Y here you will get support for the RTC found on
 	  Starfire systems.
 
+config RTC_DRV_TX4939
+	tristate "TX4939 SoC"
+	depends on SOC_TX4939
+	help
+	  Driver for the internal RTC (Realtime Clock) module found on
+	  Toshiba TX4939 SoC.
+
+config RTC_DRV_MV
+	tristate "Marvell SoC RTC"
+	depends on ARCH_KIRKWOOD
+	help
+	  If you say yes here you will get support for the in-chip RTC
+	  that can be found in some of Marvell's SoC devices, such as
+	  the Kirkwood 88F6281 and 88F6192.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-mv.
+
 endif # RTC_CLASS
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index 6e79c912bf9e..9a4340d48f26 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -20,6 +20,7 @@ rtc-core-$(CONFIG_RTC_INTF_SYSFS) += rtc-sysfs.o
 obj-$(CONFIG_RTC_DRV_AT32AP700X)+= rtc-at32ap700x.o
 obj-$(CONFIG_RTC_DRV_AT91RM9200)+= rtc-at91rm9200.o
 obj-$(CONFIG_RTC_DRV_AT91SAM9)	+= rtc-at91sam9.o
+obj-$(CONFIG_RTC_DRV_AU1XXX)	+= rtc-au1xxx.o
 obj-$(CONFIG_RTC_DRV_BFIN)	+= rtc-bfin.o
 obj-$(CONFIG_RTC_DRV_CMOS)	+= rtc-cmos.o
 obj-$(CONFIG_RTC_DRV_DS1216)	+= rtc-ds1216.o
@@ -47,6 +48,7 @@ obj-$(CONFIG_RTC_DRV_SUN4V)	+= rtc-sun4v.o
 obj-$(CONFIG_RTC_DRV_STARFIRE)	+= rtc-starfire.o
 obj-$(CONFIG_RTC_DRV_MAX6900)	+= rtc-max6900.o
 obj-$(CONFIG_RTC_DRV_MAX6902)	+= rtc-max6902.o
+obj-$(CONFIG_RTC_DRV_MV)	+= rtc-mv.o
 obj-$(CONFIG_RTC_DRV_OMAP)	+= rtc-omap.o
 obj-$(CONFIG_RTC_DRV_PCF8563)	+= rtc-pcf8563.o
 obj-$(CONFIG_RTC_DRV_PCF8583)	+= rtc-pcf8583.o
@@ -54,6 +56,7 @@ obj-$(CONFIG_RTC_DRV_PL030)	+= rtc-pl030.o
 obj-$(CONFIG_RTC_DRV_PL031)	+= rtc-pl031.o
 obj-$(CONFIG_RTC_DRV_PARISC)	+= rtc-parisc.o
 obj-$(CONFIG_RTC_DRV_PPC)	+= rtc-ppc.o
+obj-$(CONFIG_RTC_DRV_PXA)	+= rtc-pxa.o
 obj-$(CONFIG_RTC_DRV_R9701)	+= rtc-r9701.o
 obj-$(CONFIG_RTC_DRV_RS5C313)	+= rtc-rs5c313.o
 obj-$(CONFIG_RTC_DRV_RS5C348)	+= rtc-rs5c348.o
@@ -66,6 +69,7 @@ obj-$(CONFIG_RTC_DRV_SH)	+= rtc-sh.o
 obj-$(CONFIG_RTC_DRV_STK17TA8)	+= rtc-stk17ta8.o
 obj-$(CONFIG_RTC_DRV_TEST)	+= rtc-test.o
 obj-$(CONFIG_RTC_DRV_TWL4030)	+= rtc-twl4030.o
+obj-$(CONFIG_RTC_DRV_TX4939)	+= rtc-tx4939.o
 obj-$(CONFIG_RTC_DRV_V3020)	+= rtc-v3020.o
 obj-$(CONFIG_RTC_DRV_VR41XX)	+= rtc-vr41xx.o
 obj-$(CONFIG_RTC_DRV_WM8350)	+= rtc-wm8350.o
diff --git a/drivers/rtc/class.c b/drivers/rtc/class.c
index 4dfdf019fccc..be5a6b73e601 100644
--- a/drivers/rtc/class.c
+++ b/drivers/rtc/class.c
@@ -48,9 +48,7 @@ static int rtc_suspend(struct device *dev, pm_message_t mesg)
 	struct rtc_time		tm;
 	struct timespec		ts = current_kernel_time();
 
-	if (strncmp(rtc->dev.bus_id,
-				CONFIG_RTC_HCTOSYS_DEVICE,
-				BUS_ID_SIZE) != 0)
+	if (strcmp(dev_name(&rtc->dev), CONFIG_RTC_HCTOSYS_DEVICE) != 0)
 		return 0;
 
 	rtc_read_time(rtc, &tm);
@@ -71,20 +69,18 @@ static int rtc_resume(struct device *dev)
 	time_t			newtime;
 	struct timespec		time;
 
-	if (strncmp(rtc->dev.bus_id,
-				CONFIG_RTC_HCTOSYS_DEVICE,
-				BUS_ID_SIZE) != 0)
+	if (strcmp(dev_name(&rtc->dev), CONFIG_RTC_HCTOSYS_DEVICE) != 0)
 		return 0;
 
 	rtc_read_time(rtc, &tm);
 	if (rtc_valid_tm(&tm) != 0) {
-		pr_debug("%s:  bogus resume time\n", rtc->dev.bus_id);
+		pr_debug("%s:  bogus resume time\n", dev_name(&rtc->dev));
 		return 0;
 	}
 	rtc_tm_to_time(&tm, &newtime);
 	if (newtime <= oldtime) {
 		if (newtime < oldtime)
-			pr_debug("%s:  time travel!\n", rtc->dev.bus_id);
+			pr_debug("%s:  time travel!\n", dev_name(&rtc->dev));
 		return 0;
 	}
 
@@ -156,7 +152,7 @@ struct rtc_device *rtc_device_register(const char *name, struct device *dev,
 	init_waitqueue_head(&rtc->irq_queue);
 
 	strlcpy(rtc->name, name, RTC_DEVICE_NAME_SIZE);
-	snprintf(rtc->dev.bus_id, BUS_ID_SIZE, "rtc%d", id);
+	dev_set_name(&rtc->dev, "rtc%d", id);
 
 	rtc_dev_prepare(rtc);
 
@@ -169,7 +165,7 @@ struct rtc_device *rtc_device_register(const char *name, struct device *dev,
 	rtc_proc_add_device(rtc);
 
 	dev_info(dev, "rtc core: registered %s as %s\n",
-			rtc->name, rtc->dev.bus_id);
+			rtc->name, dev_name(&rtc->dev));
 
 	return rtc;
 
diff --git a/drivers/rtc/interface.c b/drivers/rtc/interface.c
index fd2c652504ff..4348c4b0d453 100644
--- a/drivers/rtc/interface.c
+++ b/drivers/rtc/interface.c
@@ -50,10 +50,15 @@ int rtc_set_time(struct rtc_device *rtc, struct rtc_time *tm)
 
 	if (!rtc->ops)
 		err = -ENODEV;
-	else if (!rtc->ops->set_time)
-		err = -EINVAL;
-	else
+	else if (rtc->ops->set_time)
 		err = rtc->ops->set_time(rtc->dev.parent, tm);
+	else if (rtc->ops->set_mmss) {
+		unsigned long secs;
+		err = rtc_tm_to_time(tm, &secs);
+		if (err == 0)
+			err = rtc->ops->set_mmss(rtc->dev.parent, secs);
+	} else
+		err = -EINVAL;
 
 	mutex_unlock(&rtc->ops_lock);
 	return err;
@@ -389,7 +394,7 @@ static int __rtc_match(struct device *dev, void *data)
 {
 	char *name = (char *)data;
 
-	if (strncmp(dev->bus_id, name, BUS_ID_SIZE) == 0)
+	if (strcmp(dev_name(dev), name) == 0)
 		return 1;
 	return 0;
 }
@@ -504,9 +509,6 @@ int rtc_irq_set_freq(struct rtc_device *rtc, struct rtc_task *task, int freq)
 	if (rtc->ops->irq_set_freq == NULL)
 		return -ENXIO;
 
-	if (!is_power_of_2(freq))
-		return -EINVAL;
-
 	spin_lock_irqsave(&rtc->irq_task_lock, flags);
 	if (rtc->irq_task != NULL && task == NULL)
 		err = -EBUSY;
diff --git a/drivers/rtc/rtc-at32ap700x.c b/drivers/rtc/rtc-at32ap700x.c
index 90b9a6503e15..e1ec33e40e38 100644
--- a/drivers/rtc/rtc-at32ap700x.c
+++ b/drivers/rtc/rtc-at32ap700x.c
@@ -205,7 +205,7 @@ static int __init at32_rtc_probe(struct platform_device *pdev)
 {
 	struct resource	*regs;
 	struct rtc_at32ap700x *rtc;
-	int irq = -1;
+	int irq;
 	int ret;
 
 	rtc = kzalloc(sizeof(struct rtc_at32ap700x), GFP_KERNEL);
@@ -222,7 +222,7 @@ static int __init at32_rtc_probe(struct platform_device *pdev)
 	}
 
 	irq = platform_get_irq(pdev, 0);
-	if (irq < 0) {
+	if (irq <= 0) {
 		dev_dbg(&pdev->dev, "could not get irq\n");
 		ret = -ENXIO;
 		goto out;
diff --git a/drivers/rtc/rtc-au1xxx.c b/drivers/rtc/rtc-au1xxx.c
new file mode 100644
index 000000000000..8906a688e6a6
--- /dev/null
+++ b/drivers/rtc/rtc-au1xxx.c
@@ -0,0 +1,153 @@
+/*
+ * Au1xxx counter0 (aka Time-Of-Year counter) RTC interface driver.
+ *
+ * Copyright (C) 2008 Manuel Lauss <mano@roarinelk.homelinux.net>
+ *
+ * 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.
+ */
+
+/* All current Au1xxx SoCs have 2 counters fed by an external 32.768 kHz
+ * crystal. Counter 0, which keeps counting during sleep/powerdown, is
+ * used to count seconds since the beginning of the unix epoch.
+ *
+ * The counters must be configured and enabled by bootloader/board code;
+ * no checks as to whether they really get a proper 32.768kHz clock are
+ * made as this would take far too long.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/rtc.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <asm/mach-au1x00/au1000.h>
+
+/* 32kHz clock enabled and detected */
+#define CNTR_OK (SYS_CNTRL_E0 | SYS_CNTRL_32S)
+
+static int au1xtoy_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	unsigned long t;
+
+	t = au_readl(SYS_TOYREAD);
+
+	rtc_time_to_tm(t, tm);
+
+	return rtc_valid_tm(tm);
+}
+
+static int au1xtoy_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	unsigned long t;
+
+	rtc_tm_to_time(tm, &t);
+
+	au_writel(t, SYS_TOYWRITE);
+	au_sync();
+
+	/* wait for the pending register write to succeed.  This can
+	 * take up to 6 seconds...
+	 */
+	while (au_readl(SYS_COUNTER_CNTRL) & SYS_CNTRL_C0S)
+		msleep(1);
+
+	return 0;
+}
+
+static struct rtc_class_ops au1xtoy_rtc_ops = {
+	.read_time	= au1xtoy_rtc_read_time,
+	.set_time	= au1xtoy_rtc_set_time,
+};
+
+static int __devinit au1xtoy_rtc_probe(struct platform_device *pdev)
+{
+	struct rtc_device *rtcdev;
+	unsigned long t;
+	int ret;
+
+	t = au_readl(SYS_COUNTER_CNTRL);
+	if (!(t & CNTR_OK)) {
+		dev_err(&pdev->dev, "counters not working; aborting.\n");
+		ret = -ENODEV;
+		goto out_err;
+	}
+
+	ret = -ETIMEDOUT;
+
+	/* set counter0 tickrate to 1Hz if necessary */
+	if (au_readl(SYS_TOYTRIM) != 32767) {
+		/* wait until hardware gives access to TRIM register */
+		t = 0x00100000;
+		while ((au_readl(SYS_COUNTER_CNTRL) & SYS_CNTRL_T0S) && t--)
+			msleep(1);
+
+		if (!t) {
+			/* timed out waiting for register access; assume
+			 * counters are unusable.
+			 */
+			dev_err(&pdev->dev, "timeout waiting for access\n");
+			goto out_err;
+		}
+
+		/* set 1Hz TOY tick rate */
+		au_writel(32767, SYS_TOYTRIM);
+		au_sync();
+	}
+
+	/* wait until the hardware allows writes to the counter reg */
+	while (au_readl(SYS_COUNTER_CNTRL) & SYS_CNTRL_C0S)
+		msleep(1);
+
+	rtcdev = rtc_device_register("rtc-au1xxx", &pdev->dev,
+				     &au1xtoy_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtcdev)) {
+		ret = PTR_ERR(rtcdev);
+		goto out_err;
+	}
+
+	platform_set_drvdata(pdev, rtcdev);
+
+	return 0;
+
+out_err:
+	return ret;
+}
+
+static int __devexit au1xtoy_rtc_remove(struct platform_device *pdev)
+{
+	struct rtc_device *rtcdev = platform_get_drvdata(pdev);
+
+	rtc_device_unregister(rtcdev);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver au1xrtc_driver = {
+	.driver		= {
+		.name	= "rtc-au1xxx",
+		.owner	= THIS_MODULE,
+	},
+	.remove		= __devexit_p(au1xtoy_rtc_remove),
+};
+
+static int __init au1xtoy_rtc_init(void)
+{
+	return platform_driver_probe(&au1xrtc_driver, au1xtoy_rtc_probe);
+}
+
+static void __exit au1xtoy_rtc_exit(void)
+{
+	platform_driver_unregister(&au1xrtc_driver);
+}
+
+module_init(au1xtoy_rtc_init);
+module_exit(au1xtoy_rtc_exit);
+
+MODULE_DESCRIPTION("Au1xxx TOY-counter-based RTC driver");
+MODULE_AUTHOR("Manuel Lauss <manuel.lauss@gmail.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:rtc-au1xxx");
diff --git a/drivers/rtc/rtc-bfin.c b/drivers/rtc/rtc-bfin.c
index 34439ce3967e..aafd3e6ebb0d 100644
--- a/drivers/rtc/rtc-bfin.c
+++ b/drivers/rtc/rtc-bfin.c
@@ -390,7 +390,7 @@ static int __devinit bfin_rtc_probe(struct platform_device *pdev)
 
 	/* Register our RTC with the RTC framework */
 	rtc->rtc_dev = rtc_device_register(pdev->name, dev, &bfin_rtc_ops, THIS_MODULE);
-	if (unlikely(IS_ERR(rtc))) {
+	if (unlikely(IS_ERR(rtc->rtc_dev))) {
 		ret = PTR_ERR(rtc->rtc_dev);
 		goto err_irq;
 	}
diff --git a/drivers/rtc/rtc-cmos.c b/drivers/rtc/rtc-cmos.c
index 6cf8e282338f..b6d35f50e404 100644
--- a/drivers/rtc/rtc-cmos.c
+++ b/drivers/rtc/rtc-cmos.c
@@ -35,6 +35,7 @@
 #include <linux/spinlock.h>
 #include <linux/platform_device.h>
 #include <linux/mod_devicetable.h>
+#include <linux/log2.h>
 
 /* this is for "generic access to PC-style RTC" using CMOS_READ/CMOS_WRITE */
 #include <asm-generic/rtc.h>
@@ -58,7 +59,7 @@ struct cmos_rtc {
 };
 
 /* both platform and pnp busses use negative numbers for invalid irqs */
-#define is_valid_irq(n)		((n) >= 0)
+#define is_valid_irq(n)		((n) > 0)
 
 static const char driver_name[] = "rtc_cmos";
 
@@ -384,6 +385,8 @@ static int cmos_irq_set_freq(struct device *dev, int freq)
 	if (!is_valid_irq(cmos->irq))
 		return -ENXIO;
 
+	if (!is_power_of_2(freq))
+		return -EINVAL;
 	/* 0 = no irqs; 1 = 2^15 Hz ... 15 = 2^0 Hz */
 	f = ffs(freq);
 	if (f-- > 16)
@@ -729,7 +732,7 @@ cmos_do_probe(struct device *dev, struct resource *ports, int rtc_irq)
 
 	cmos_rtc.dev = dev;
 	dev_set_drvdata(dev, &cmos_rtc);
-	rename_region(ports, cmos_rtc.rtc->dev.bus_id);
+	rename_region(ports, dev_name(&cmos_rtc.rtc->dev));
 
 	spin_lock_irq(&rtc_lock);
 
@@ -777,7 +780,7 @@ cmos_do_probe(struct device *dev, struct resource *ports, int rtc_irq)
 			rtc_cmos_int_handler = cmos_interrupt;
 
 		retval = request_irq(rtc_irq, rtc_cmos_int_handler,
-				IRQF_DISABLED, cmos_rtc.rtc->dev.bus_id,
+				IRQF_DISABLED, dev_name(&cmos_rtc.rtc->dev),
 				cmos_rtc.rtc);
 		if (retval < 0) {
 			dev_dbg(dev, "IRQ %d is already in use\n", rtc_irq);
@@ -795,7 +798,7 @@ cmos_do_probe(struct device *dev, struct resource *ports, int rtc_irq)
 	}
 
 	pr_info("%s: alarms up to one %s%s, %zd bytes nvram%s\n",
-			cmos_rtc.rtc->dev.bus_id,
+			dev_name(&cmos_rtc.rtc->dev),
 			is_valid_irq(rtc_irq)
 				?  (cmos_rtc.mon_alrm
 					? "year"
@@ -885,7 +888,7 @@ static int cmos_suspend(struct device *dev, pm_message_t mesg)
 	}
 
 	pr_debug("%s: suspend%s, ctrl %02x\n",
-			cmos_rtc.rtc->dev.bus_id,
+			dev_name(&cmos_rtc.rtc->dev),
 			(tmp & RTC_AIE) ? ", alarm may wake" : "",
 			tmp);
 
@@ -941,7 +944,7 @@ static int cmos_resume(struct device *dev)
 	}
 
 	pr_debug("%s: resume, ctrl %02x\n",
-			cmos_rtc.rtc->dev.bus_id,
+			dev_name(&cmos_rtc.rtc->dev),
 			tmp);
 
 	return 0;
diff --git a/drivers/rtc/rtc-ds1216.c b/drivers/rtc/rtc-ds1216.c
index 9a234a4ec06d..4aedc705518c 100644
--- a/drivers/rtc/rtc-ds1216.c
+++ b/drivers/rtc/rtc-ds1216.c
@@ -10,7 +10,7 @@
 #include <linux/platform_device.h>
 #include <linux/bcd.h>
 
-#define DRV_VERSION "0.1"
+#define DRV_VERSION "0.2"
 
 struct ds1216_regs {
 	u8 tsec;
@@ -101,7 +101,8 @@ static int ds1216_rtc_read_time(struct device *dev, struct rtc_time *tm)
 	tm->tm_year = bcd2bin(regs.year);
 	if (tm->tm_year < 70)
 		tm->tm_year += 100;
-	return 0;
+
+	return rtc_valid_tm(tm);
 }
 
 static int ds1216_rtc_set_time(struct device *dev, struct rtc_time *tm)
@@ -138,9 +139,8 @@ static const struct rtc_class_ops ds1216_rtc_ops = {
 	.set_time	= ds1216_rtc_set_time,
 };
 
-static int __devinit ds1216_rtc_probe(struct platform_device *pdev)
+static int __init ds1216_rtc_probe(struct platform_device *pdev)
 {
-	struct rtc_device *rtc;
 	struct resource *res;
 	struct ds1216_priv *priv;
 	int ret = 0;
@@ -152,7 +152,10 @@ static int __devinit ds1216_rtc_probe(struct platform_device *pdev)
 	priv = kzalloc(sizeof *priv, GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
-	priv->size = res->end - res->start + 1;
+
+	platform_set_drvdata(pdev, priv);
+
+	priv->size = resource_size(res);
 	if (!request_mem_region(res->start, priv->size, pdev->name)) {
 		ret = -EBUSY;
 		goto out;
@@ -163,22 +166,18 @@ static int __devinit ds1216_rtc_probe(struct platform_device *pdev)
 		ret = -ENOMEM;
 		goto out;
 	}
-	rtc = rtc_device_register("ds1216", &pdev->dev,
+	priv->rtc = rtc_device_register("ds1216", &pdev->dev,
 				  &ds1216_rtc_ops, THIS_MODULE);
-	if (IS_ERR(rtc)) {
-		ret = PTR_ERR(rtc);
+	if (IS_ERR(priv->rtc)) {
+		ret = PTR_ERR(priv->rtc);
 		goto out;
 	}
-	priv->rtc = rtc;
-	platform_set_drvdata(pdev, priv);
 
 	/* dummy read to get clock into a known state */
 	ds1216_read(priv->ioaddr, dummy);
 	return 0;
 
 out:
-	if (priv->rtc)
-		rtc_device_unregister(priv->rtc);
 	if (priv->ioaddr)
 		iounmap(priv->ioaddr);
 	if (priv->baseaddr)
@@ -187,7 +186,7 @@ out:
 	return ret;
 }
 
-static int __devexit ds1216_rtc_remove(struct platform_device *pdev)
+static int __exit ds1216_rtc_remove(struct platform_device *pdev)
 {
 	struct ds1216_priv *priv = platform_get_drvdata(pdev);
 
@@ -203,13 +202,12 @@ static struct platform_driver ds1216_rtc_platform_driver = {
 		.name	= "rtc-ds1216",
 		.owner	= THIS_MODULE,
 	},
-	.probe		= ds1216_rtc_probe,
-	.remove		= __devexit_p(ds1216_rtc_remove),
+	.remove		= __exit_p(ds1216_rtc_remove),
 };
 
 static int __init ds1216_rtc_init(void)
 {
-	return platform_driver_register(&ds1216_rtc_platform_driver);
+	return platform_driver_probe(&ds1216_rtc_platform_driver, ds1216_rtc_probe);
 }
 
 static void __exit ds1216_rtc_exit(void)
diff --git a/drivers/rtc/rtc-ds1390.c b/drivers/rtc/rtc-ds1390.c
index 599e976bf014..e54b5c619bdf 100644
--- a/drivers/rtc/rtc-ds1390.c
+++ b/drivers/rtc/rtc-ds1390.c
@@ -1,5 +1,5 @@
 /*
- * rtc-ds1390.c -- driver for DS1390/93/94
+ * rtc-ds1390.c -- driver for the Dallas/Maxim DS1390/93/94 SPI RTC
  *
  * Copyright (C) 2008 Mercury IMC Ltd
  * Written by Mark Jackson <mpfj@mimc.co.uk>
@@ -8,11 +8,13 @@
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  *
- * NOTE : Currently this driver only supports the bare minimum for read
- * and write the RTC.  The extra features provided by the chip family
+ * NOTE: Currently this driver only supports the bare minimum for read
+ * and write the RTC. The extra features provided by the chip family
  * (alarms, trickle charger, different control registers) are unavailable.
  */
 
+#include <linux/init.h>
+#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/rtc.h>
 #include <linux/spi/spi.h>
@@ -42,20 +44,6 @@ struct ds1390 {
 	u8 txrx_buf[9];	/* cmd + 8 registers */
 };
 
-static void ds1390_set_reg(struct device *dev, unsigned char address,
-				unsigned char data)
-{
-	struct spi_device *spi = to_spi_device(dev);
-	struct ds1390 *chip = dev_get_drvdata(dev);
-
-	/* Set MSB to indicate write */
-	chip->txrx_buf[0] = address | 0x80;
-	chip->txrx_buf[1] = data;
-
-	/* do the i/o */
-	spi_write_then_read(spi, chip->txrx_buf, 2, NULL, 0);
-}
-
 static int ds1390_get_reg(struct device *dev, unsigned char address,
 				unsigned char *data)
 {
@@ -78,7 +66,7 @@ static int ds1390_get_reg(struct device *dev, unsigned char address,
 	return 0;
 }
 
-static int ds1390_get_datetime(struct device *dev, struct rtc_time *dt)
+static int ds1390_read_time(struct device *dev, struct rtc_time *dt)
 {
 	struct spi_device *spi = to_spi_device(dev);
 	struct ds1390 *chip = dev_get_drvdata(dev);
@@ -107,7 +95,7 @@ static int ds1390_get_datetime(struct device *dev, struct rtc_time *dt)
 	return rtc_valid_tm(dt);
 }
 
-static int ds1390_set_datetime(struct device *dev, struct rtc_time *dt)
+static int ds1390_set_time(struct device *dev, struct rtc_time *dt)
 {
 	struct spi_device *spi = to_spi_device(dev);
 	struct ds1390 *chip = dev_get_drvdata(dev);
@@ -127,16 +115,6 @@ static int ds1390_set_datetime(struct device *dev, struct rtc_time *dt)
 	return spi_write_then_read(spi, chip->txrx_buf, 8, NULL, 0);
 }
 
-static int ds1390_read_time(struct device *dev, struct rtc_time *tm)
-{
-	return ds1390_get_datetime(dev, tm);
-}
-
-static int ds1390_set_time(struct device *dev, struct rtc_time *tm)
-{
-	return ds1390_set_datetime(dev, tm);
-}
-
 static const struct rtc_class_ops ds1390_rtc_ops = {
 	.read_time	= ds1390_read_time,
 	.set_time	= ds1390_set_time,
@@ -149,46 +127,40 @@ static int __devinit ds1390_probe(struct spi_device *spi)
 	struct ds1390 *chip;
 	int res;
 
-	printk(KERN_DEBUG "DS1390 SPI RTC driver\n");
-
-	rtc = rtc_device_register("ds1390",
-				&spi->dev, &ds1390_rtc_ops, THIS_MODULE);
-	if (IS_ERR(rtc)) {
-		printk(KERN_ALERT "RTC : unable to register device\n");
-		return PTR_ERR(rtc);
-	}
-
 	spi->mode = SPI_MODE_3;
 	spi->bits_per_word = 8;
 	spi_setup(spi);
 
 	chip = kzalloc(sizeof *chip, GFP_KERNEL);
 	if (!chip) {
-		printk(KERN_ALERT "RTC : unable to allocate device memory\n");
-		rtc_device_unregister(rtc);
+		dev_err(&spi->dev, "unable to allocate device memory\n");
 		return -ENOMEM;
 	}
-	chip->rtc = rtc;
 	dev_set_drvdata(&spi->dev, chip);
 
 	res = ds1390_get_reg(&spi->dev, DS1390_REG_SECONDS, &tmp);
-	if (res) {
-		printk(KERN_ALERT "RTC : unable to read device\n");
-		rtc_device_unregister(rtc);
+	if (res != 0) {
+		dev_err(&spi->dev, "unable to read device\n");
+		kfree(chip);
 		return res;
 	}
 
-	return 0;
+	chip->rtc = rtc_device_register("ds1390",
+				&spi->dev, &ds1390_rtc_ops, THIS_MODULE);
+	if (IS_ERR(chip->rtc)) {
+		dev_err(&spi->dev, "unable to register device\n");
+		res = PTR_ERR(chip->rtc);
+		kfree(chip);
+	}
+
+	return res;
 }
 
 static int __devexit ds1390_remove(struct spi_device *spi)
 {
 	struct ds1390 *chip = platform_get_drvdata(spi);
-	struct rtc_device *rtc = chip->rtc;
-
-	if (rtc)
-		rtc_device_unregister(rtc);
 
+	rtc_device_unregister(chip->rtc);
 	kfree(chip);
 
 	return 0;
@@ -215,6 +187,6 @@ static __exit void ds1390_exit(void)
 }
 module_exit(ds1390_exit);
 
-MODULE_DESCRIPTION("DS1390/93/94 SPI RTC driver");
+MODULE_DESCRIPTION("Dallas/Maxim DS1390/93/94 SPI RTC driver");
 MODULE_AUTHOR("Mark Jackson <mpfj@mimc.co.uk>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-ds1511.c b/drivers/rtc/rtc-ds1511.c
index 25caada78398..0b6b7730c716 100644
--- a/drivers/rtc/rtc-ds1511.c
+++ b/drivers/rtc/rtc-ds1511.c
@@ -326,9 +326,9 @@ ds1511_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0) {
+	if (pdata->irq <= 0)
 		return -EINVAL;
-	}
+
 	pdata->alrm_mday = alrm->time.tm_mday;
 	pdata->alrm_hour = alrm->time.tm_hour;
 	pdata->alrm_min = alrm->time.tm_min;
@@ -346,9 +346,9 @@ ds1511_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0) {
+	if (pdata->irq <= 0)
 		return -EINVAL;
-	}
+
 	alrm->time.tm_mday = pdata->alrm_mday < 0 ? 0 : pdata->alrm_mday;
 	alrm->time.tm_hour = pdata->alrm_hour < 0 ? 0 : pdata->alrm_hour;
 	alrm->time.tm_min = pdata->alrm_min < 0 ? 0 : pdata->alrm_min;
@@ -385,7 +385,7 @@ ds1511_rtc_ioctl(struct device *dev, unsigned int cmd, unsigned long arg)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0) {
+	if (pdata->irq <= 0) {
 		return -ENOIOCTLCMD; /* fall back into rtc-dev's emulation */
 	}
 	switch (cmd) {
@@ -503,7 +503,6 @@ ds1511_rtc_probe(struct platform_device *pdev)
 	if (!pdata) {
 		return -ENOMEM;
 	}
-	pdata->irq = -1;
 	pdata->size = res->end - res->start + 1;
 	if (!request_mem_region(res->start, pdata->size, pdev->name)) {
 		ret = -EBUSY;
@@ -545,13 +544,13 @@ ds1511_rtc_probe(struct platform_device *pdev)
 	 * if the platform has an interrupt in mind for this device,
 	 * then by all means, set it
 	 */
-	if (pdata->irq >= 0) {
+	if (pdata->irq > 0) {
 		rtc_read(RTC_CMD1);
 		if (request_irq(pdata->irq, ds1511_interrupt,
 			IRQF_DISABLED | IRQF_SHARED, pdev->name, pdev) < 0) {
 
 			dev_warn(&pdev->dev, "interrupt not available.\n");
-			pdata->irq = -1;
+			pdata->irq = 0;
 		}
 	}
 
@@ -572,7 +571,7 @@ ds1511_rtc_probe(struct platform_device *pdev)
 	if (pdata->rtc) {
 		rtc_device_unregister(pdata->rtc);
 	}
-	if (pdata->irq >= 0) {
+	if (pdata->irq > 0) {
 		free_irq(pdata->irq, pdev);
 	}
 	if (ds1511_base) {
@@ -595,7 +594,7 @@ ds1511_rtc_remove(struct platform_device *pdev)
 	sysfs_remove_bin_file(&pdev->dev.kobj, &ds1511_nvram_attr);
 	rtc_device_unregister(pdata->rtc);
 	pdata->rtc = NULL;
-	if (pdata->irq >= 0) {
+	if (pdata->irq > 0) {
 		/*
 		 * disable the alarm interrupt
 		 */
@@ -631,7 +630,7 @@ ds1511_rtc_init(void)
  static void __exit
 ds1511_rtc_exit(void)
 {
-	return platform_driver_unregister(&ds1511_rtc_driver);
+	platform_driver_unregister(&ds1511_rtc_driver);
 }
 
 module_init(ds1511_rtc_init);
diff --git a/drivers/rtc/rtc-ds1553.c b/drivers/rtc/rtc-ds1553.c
index b9475cd20210..38d472b63406 100644
--- a/drivers/rtc/rtc-ds1553.c
+++ b/drivers/rtc/rtc-ds1553.c
@@ -162,7 +162,7 @@ static int ds1553_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0)
+	if (pdata->irq <= 0)
 		return -EINVAL;
 	pdata->alrm_mday = alrm->time.tm_mday;
 	pdata->alrm_hour = alrm->time.tm_hour;
@@ -179,7 +179,7 @@ static int ds1553_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0)
+	if (pdata->irq <= 0)
 		return -EINVAL;
 	alrm->time.tm_mday = pdata->alrm_mday < 0 ? 0 : pdata->alrm_mday;
 	alrm->time.tm_hour = pdata->alrm_hour < 0 ? 0 : pdata->alrm_hour;
@@ -213,7 +213,7 @@ static int ds1553_rtc_ioctl(struct device *dev, unsigned int cmd,
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0)
+	if (pdata->irq <= 0)
 		return -ENOIOCTLCMD; /* fall back into rtc-dev's emulation */
 	switch (cmd) {
 	case RTC_AIE_OFF:
@@ -301,7 +301,6 @@ static int __devinit ds1553_rtc_probe(struct platform_device *pdev)
 	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
 		return -ENOMEM;
-	pdata->irq = -1;
 	if (!request_mem_region(res->start, RTC_REG_SIZE, pdev->name)) {
 		ret = -EBUSY;
 		goto out;
@@ -327,13 +326,13 @@ static int __devinit ds1553_rtc_probe(struct platform_device *pdev)
 	if (readb(ioaddr + RTC_FLAGS) & RTC_FLAGS_BLF)
 		dev_warn(&pdev->dev, "voltage-low detected.\n");
 
-	if (pdata->irq >= 0) {
+	if (pdata->irq > 0) {
 		writeb(0, ioaddr + RTC_INTERRUPTS);
 		if (request_irq(pdata->irq, ds1553_rtc_interrupt,
 				IRQF_DISABLED | IRQF_SHARED,
 				pdev->name, pdev) < 0) {
 			dev_warn(&pdev->dev, "interrupt not available.\n");
-			pdata->irq = -1;
+			pdata->irq = 0;
 		}
 	}
 
@@ -353,7 +352,7 @@ static int __devinit ds1553_rtc_probe(struct platform_device *pdev)
  out:
 	if (pdata->rtc)
 		rtc_device_unregister(pdata->rtc);
-	if (pdata->irq >= 0)
+	if (pdata->irq > 0)
 		free_irq(pdata->irq, pdev);
 	if (ioaddr)
 		iounmap(ioaddr);
@@ -369,7 +368,7 @@ static int __devexit ds1553_rtc_remove(struct platform_device *pdev)
 
 	sysfs_remove_bin_file(&pdev->dev.kobj, &ds1553_nvram_attr);
 	rtc_device_unregister(pdata->rtc);
-	if (pdata->irq >= 0) {
+	if (pdata->irq > 0) {
 		writeb(0, pdata->ioaddr + RTC_INTERRUPTS);
 		free_irq(pdata->irq, pdev);
 	}
diff --git a/drivers/rtc/rtc-ds1672.c b/drivers/rtc/rtc-ds1672.c
index 4e91419e8911..06dfb54f99b6 100644
--- a/drivers/rtc/rtc-ds1672.c
+++ b/drivers/rtc/rtc-ds1672.c
@@ -83,32 +83,11 @@ static int ds1672_set_mmss(struct i2c_client *client, unsigned long secs)
 	return 0;
 }
 
-static int ds1672_set_datetime(struct i2c_client *client, struct rtc_time *tm)
-{
-	unsigned long secs;
-
-	dev_dbg(&client->dev,
-		"%s: secs=%d, mins=%d, hours=%d, "
-		"mday=%d, mon=%d, year=%d, wday=%d\n",
-		__func__,
-		tm->tm_sec, tm->tm_min, tm->tm_hour,
-		tm->tm_mday, tm->tm_mon, tm->tm_year, tm->tm_wday);
-
-	rtc_tm_to_time(tm, &secs);
-
-	return ds1672_set_mmss(client, secs);
-}
-
 static int ds1672_rtc_read_time(struct device *dev, struct rtc_time *tm)
 {
 	return ds1672_get_datetime(to_i2c_client(dev), tm);
 }
 
-static int ds1672_rtc_set_time(struct device *dev, struct rtc_time *tm)
-{
-	return ds1672_set_datetime(to_i2c_client(dev), tm);
-}
-
 static int ds1672_rtc_set_mmss(struct device *dev, unsigned long secs)
 {
 	return ds1672_set_mmss(to_i2c_client(dev), secs);
@@ -152,7 +131,6 @@ static DEVICE_ATTR(control, S_IRUGO, show_control, NULL);
 
 static const struct rtc_class_ops ds1672_rtc_ops = {
 	.read_time = ds1672_rtc_read_time,
-	.set_time = ds1672_rtc_set_time,
 	.set_mmss = ds1672_rtc_set_mmss,
 };
 
diff --git a/drivers/rtc/rtc-ds3234.c b/drivers/rtc/rtc-ds3234.c
index 45e5b106af73..c51589ede5b7 100644
--- a/drivers/rtc/rtc-ds3234.c
+++ b/drivers/rtc/rtc-ds3234.c
@@ -1,4 +1,4 @@
-/* drivers/rtc/rtc-ds3234.c
+/* rtc-ds3234.c
  *
  * Driver for Dallas Semiconductor (DS3234) SPI RTC with Integrated Crystal
  * and SRAM.
@@ -9,13 +9,10 @@
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  *
- * Changelog:
- *
- * 07-May-2008: Dennis Aberilla <denzzzhome@yahoo.com>
- *		- Created based on the max6902 code. Only implements the
- *		  date/time keeping functions; no SRAM yet.
  */
 
+#include <linux/init.h>
+#include <linux/module.h>
 #include <linux/device.h>
 #include <linux/platform_device.h>
 #include <linux/rtc.h>
@@ -34,16 +31,7 @@
 #define DS3234_REG_CONTROL	0x0E
 #define DS3234_REG_CONT_STAT	0x0F
 
-#undef DS3234_DEBUG
-
-struct ds3234 {
-	struct rtc_device *rtc;
-	u8 buf[8]; /* Burst read: addr + 7 regs */
-	u8 tx_buf[2];
-	u8 rx_buf[2];
-};
-
-static void ds3234_set_reg(struct device *dev, unsigned char address,
+static int ds3234_set_reg(struct device *dev, unsigned char address,
 				unsigned char data)
 {
 	struct spi_device *spi = to_spi_device(dev);
@@ -53,107 +41,45 @@ static void ds3234_set_reg(struct device *dev, unsigned char address,
 	buf[0] = address | 0x80;
 	buf[1] = data;
 
-	spi_write(spi, buf, 2);
+	return spi_write_then_read(spi, buf, 2, NULL, 0);
 }
 
 static int ds3234_get_reg(struct device *dev, unsigned char address,
 				unsigned char *data)
 {
 	struct spi_device *spi = to_spi_device(dev);
-	struct ds3234 *chip = dev_get_drvdata(dev);
-	struct spi_message message;
-	struct spi_transfer xfer;
-	int status;
-
-	if (!data)
-		return -EINVAL;
-
-	/* Build our spi message */
-	spi_message_init(&message);
-	memset(&xfer, 0, sizeof(xfer));
-
-	/* Address + dummy tx byte */
-	xfer.len = 2;
-	xfer.tx_buf = chip->tx_buf;
-	xfer.rx_buf = chip->rx_buf;
-
-	chip->tx_buf[0] = address;
-	chip->tx_buf[1] = 0xff;
 
-	spi_message_add_tail(&xfer, &message);
+	*data = address & 0x7f;
 
-	/* do the i/o */
-	status = spi_sync(spi, &message);
-	if (status == 0)
-		status = message.status;
-	else
-		return status;
-
-	*data = chip->rx_buf[1];
-
-	return status;
+	return spi_write_then_read(spi, data, 1, data, 1);
 }
 
-static int ds3234_get_datetime(struct device *dev, struct rtc_time *dt)
+static int ds3234_read_time(struct device *dev, struct rtc_time *dt)
 {
+	int err;
+	unsigned char buf[8];
 	struct spi_device *spi = to_spi_device(dev);
-	struct ds3234 *chip = dev_get_drvdata(dev);
-	struct spi_message message;
-	struct spi_transfer xfer;
-	int status;
-
-	/* build the message */
-	spi_message_init(&message);
-	memset(&xfer, 0, sizeof(xfer));
-	xfer.len = 1 + 7;	/* Addr + 7 registers */
-	xfer.tx_buf = chip->buf;
-	xfer.rx_buf = chip->buf;
-	chip->buf[0] = 0x00;	/* Start address */
-	spi_message_add_tail(&xfer, &message);
-
-	/* do the i/o */
-	status = spi_sync(spi, &message);
-	if (status == 0)
-		status = message.status;
-	else
-		return status;
 
-	/* Seconds, Minutes, Hours, Day, Date, Month, Year */
-	dt->tm_sec	= bcd2bin(chip->buf[1]);
-	dt->tm_min	= bcd2bin(chip->buf[2]);
-	dt->tm_hour	= bcd2bin(chip->buf[3] & 0x3f);
-	dt->tm_wday	= bcd2bin(chip->buf[4]) - 1; /* 0 = Sun */
-	dt->tm_mday	= bcd2bin(chip->buf[5]);
-	dt->tm_mon	= bcd2bin(chip->buf[6] & 0x1f) - 1; /* 0 = Jan */
-	dt->tm_year 	= bcd2bin(chip->buf[7] & 0xff) + 100; /* Assume 20YY */
-
-#ifdef DS3234_DEBUG
-	dev_dbg(dev, "\n%s : Read RTC values\n", __func__);
-	dev_dbg(dev, "tm_hour: %i\n", dt->tm_hour);
-	dev_dbg(dev, "tm_min : %i\n", dt->tm_min);
-	dev_dbg(dev, "tm_sec : %i\n", dt->tm_sec);
-	dev_dbg(dev, "tm_wday: %i\n", dt->tm_wday);
-	dev_dbg(dev, "tm_mday: %i\n", dt->tm_mday);
-	dev_dbg(dev, "tm_mon : %i\n", dt->tm_mon);
-	dev_dbg(dev, "tm_year: %i\n", dt->tm_year);
-#endif
+	buf[0] = 0x00; /* Start address */
 
-	return 0;
+	err = spi_write_then_read(spi, buf, 1, buf, 8);
+	if (err != 0)
+		return err;
+
+	/* Seconds, Minutes, Hours, Day, Date, Month, Year */
+	dt->tm_sec	= bcd2bin(buf[0]);
+	dt->tm_min	= bcd2bin(buf[1]);
+	dt->tm_hour	= bcd2bin(buf[2] & 0x3f);
+	dt->tm_wday	= bcd2bin(buf[3]) - 1; /* 0 = Sun */
+	dt->tm_mday	= bcd2bin(buf[4]);
+	dt->tm_mon	= bcd2bin(buf[5] & 0x1f) - 1; /* 0 = Jan */
+	dt->tm_year 	= bcd2bin(buf[6] & 0xff) + 100; /* Assume 20YY */
+
+	return rtc_valid_tm(dt);
 }
 
-static int ds3234_set_datetime(struct device *dev, struct rtc_time *dt)
+static int ds3234_set_time(struct device *dev, struct rtc_time *dt)
 {
-#ifdef DS3234_DEBUG
-	dev_dbg(dev, "\n%s : Setting RTC values\n", __func__);
-	dev_dbg(dev, "tm_sec : %i\n", dt->tm_sec);
-	dev_dbg(dev, "tm_min : %i\n", dt->tm_min);
-	dev_dbg(dev, "tm_hour: %i\n", dt->tm_hour);
-	dev_dbg(dev, "tm_wday: %i\n", dt->tm_wday);
-	dev_dbg(dev, "tm_mday: %i\n", dt->tm_mday);
-	dev_dbg(dev, "tm_mon : %i\n", dt->tm_mon);
-	dev_dbg(dev, "tm_year: %i\n", dt->tm_year);
-#endif
-
 	ds3234_set_reg(dev, DS3234_REG_SECONDS, bin2bcd(dt->tm_sec));
 	ds3234_set_reg(dev, DS3234_REG_MINUTES, bin2bcd(dt->tm_min));
 	ds3234_set_reg(dev, DS3234_REG_HOURS, bin2bcd(dt->tm_hour) & 0x3f);
@@ -174,16 +100,6 @@ static int ds3234_set_datetime(struct device *dev, struct rtc_time *dt)
 	return 0;
 }
 
-static int ds3234_read_time(struct device *dev, struct rtc_time *tm)
-{
-	return ds3234_get_datetime(dev, tm);
-}
-
-static int ds3234_set_time(struct device *dev, struct rtc_time *tm)
-{
-	return ds3234_set_datetime(dev, tm);
-}
-
 static const struct rtc_class_ops ds3234_rtc_ops = {
 	.read_time	= ds3234_read_time,
 	.set_time	= ds3234_set_time,
@@ -193,31 +109,15 @@ static int __devinit ds3234_probe(struct spi_device *spi)
 {
 	struct rtc_device *rtc;
 	unsigned char tmp;
-	struct ds3234 *chip;
 	int res;
 
-	rtc = rtc_device_register("ds3234",
-				&spi->dev, &ds3234_rtc_ops, THIS_MODULE);
-	if (IS_ERR(rtc))
-		return PTR_ERR(rtc);
-
 	spi->mode = SPI_MODE_3;
 	spi->bits_per_word = 8;
 	spi_setup(spi);
 
-	chip = kzalloc(sizeof(struct ds3234), GFP_KERNEL);
-	if (!chip) {
-		rtc_device_unregister(rtc);
-		return -ENOMEM;
-	}
-	chip->rtc = rtc;
-	dev_set_drvdata(&spi->dev, chip);
-
 	res = ds3234_get_reg(&spi->dev, DS3234_REG_SECONDS, &tmp);
-	if (res) {
-		rtc_device_unregister(rtc);
+	if (res != 0)
 		return res;
-	}
 
 	/* Control settings
 	 *
@@ -246,26 +146,27 @@ static int __devinit ds3234_probe(struct spi_device *spi)
 	ds3234_get_reg(&spi->dev, DS3234_REG_CONT_STAT, &tmp);
 	dev_info(&spi->dev, "Ctrl/Stat Reg: 0x%02x\n", tmp);
 
+	rtc = rtc_device_register("ds3234",
+				&spi->dev, &ds3234_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtc))
+		return PTR_ERR(rtc);
+
+	dev_set_drvdata(&spi->dev, rtc);
+
 	return 0;
 }
 
 static int __devexit ds3234_remove(struct spi_device *spi)
 {
-	struct ds3234 *chip = platform_get_drvdata(spi);
-	struct rtc_device *rtc = chip->rtc;
-
-	if (rtc)
-		rtc_device_unregister(rtc);
-
-	kfree(chip);
+	struct rtc_device *rtc = platform_get_drvdata(spi);
 
+	rtc_device_unregister(rtc);
 	return 0;
 }
 
 static struct spi_driver ds3234_driver = {
 	.driver = {
 		.name	 = "ds3234",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe	 = ds3234_probe,
@@ -274,7 +175,6 @@ static struct spi_driver ds3234_driver = {
 
 static __init int ds3234_init(void)
 {
-	printk(KERN_INFO "DS3234 SPI RTC Driver\n");
 	return spi_register_driver(&ds3234_driver);
 }
 module_init(ds3234_init);
diff --git a/drivers/rtc/rtc-ep93xx.c b/drivers/rtc/rtc-ep93xx.c
index 36e4ac0bd69c..f7a3283dd029 100644
--- a/drivers/rtc/rtc-ep93xx.c
+++ b/drivers/rtc/rtc-ep93xx.c
@@ -49,18 +49,6 @@ static int ep93xx_rtc_set_mmss(struct device *dev, unsigned long secs)
 	return 0;
 }
 
-static int ep93xx_rtc_set_time(struct device *dev, struct rtc_time *tm)
-{
-	int err;
-	unsigned long secs;
-
-	err = rtc_tm_to_time(tm, &secs);
-	if (err != 0)
-		return err;
-
-	return ep93xx_rtc_set_mmss(dev, secs);
-}
-
 static int ep93xx_rtc_proc(struct device *dev, struct seq_file *seq)
 {
 	unsigned short preload, delete;
@@ -75,7 +63,6 @@ static int ep93xx_rtc_proc(struct device *dev, struct seq_file *seq)
 
 static const struct rtc_class_ops ep93xx_rtc_ops = {
 	.read_time	= ep93xx_rtc_read_time,
-	.set_time	= ep93xx_rtc_set_time,
 	.set_mmss	= ep93xx_rtc_set_mmss,
 	.proc		= ep93xx_rtc_proc,
 };
diff --git a/drivers/rtc/rtc-m48t59.c b/drivers/rtc/rtc-m48t59.c
index 43afb7ab5289..33921a6b1707 100644
--- a/drivers/rtc/rtc-m48t59.c
+++ b/drivers/rtc/rtc-m48t59.c
@@ -450,7 +450,7 @@ static int __devinit m48t59_rtc_probe(struct platform_device *pdev)
 	 * the mode without IRQ.
 	 */
 	m48t59->irq = platform_get_irq(pdev, 0);
-	if (m48t59->irq < 0)
+	if (m48t59->irq <= 0)
 		m48t59->irq = NO_IRQ;
 
 	if (m48t59->irq != NO_IRQ) {
diff --git a/drivers/rtc/rtc-max6902.c b/drivers/rtc/rtc-max6902.c
index 2f6507df7b49..36a8ea9ed8ba 100644
--- a/drivers/rtc/rtc-max6902.c
+++ b/drivers/rtc/rtc-max6902.c
@@ -9,14 +9,6 @@
  *
  * Driver for MAX6902 spi RTC
  *
- * Changelog:
- *
- * 24-May-2006: Raphael Assenat <raph@8d.com>
- *                - Major rework
- *				Converted to rtc_device and uses the SPI layer.
- *
- * ??-???-2005: Someone at Compulab
- *                - Initial driver creation.
  */
 
 #include <linux/module.h>
@@ -26,7 +18,6 @@
 #include <linux/rtc.h>
 #include <linux/spi/spi.h>
 #include <linux/bcd.h>
-#include <linux/delay.h>
 
 #define MAX6902_REG_SECONDS		0x01
 #define MAX6902_REG_MINUTES		0x03
@@ -38,16 +29,7 @@
 #define MAX6902_REG_CONTROL		0x0F
 #define MAX6902_REG_CENTURY		0x13
 
-#undef MAX6902_DEBUG
-
-struct max6902 {
-	struct rtc_device *rtc;
-	u8 buf[9]; /* Burst read cmd + 8 registers */
-	u8 tx_buf[2];
-	u8 rx_buf[2];
-};
-
-static void max6902_set_reg(struct device *dev, unsigned char address,
+static int max6902_set_reg(struct device *dev, unsigned char address,
 				unsigned char data)
 {
 	struct spi_device *spi = to_spi_device(dev);
@@ -57,113 +39,58 @@ static void max6902_set_reg(struct device *dev, unsigned char address,
 	buf[0] = address & 0x7f;
 	buf[1] = data;
 
-	spi_write(spi, buf, 2);
+	return spi_write_then_read(spi, buf, 2, NULL, 0);
 }
 
 static int max6902_get_reg(struct device *dev, unsigned char address,
 				unsigned char *data)
 {
 	struct spi_device *spi = to_spi_device(dev);
-	struct max6902 *chip = dev_get_drvdata(dev);
-	struct spi_message message;
-	struct spi_transfer xfer;
-	int status;
-
-	if (!data)
-		return -EINVAL;
-
-	/* Build our spi message */
-	spi_message_init(&message);
-	memset(&xfer, 0, sizeof(xfer));
-	xfer.len = 2;
-	/* Can tx_buf and rx_buf be equal? The doc in spi.h is not sure... */
-	xfer.tx_buf = chip->tx_buf;
-	xfer.rx_buf = chip->rx_buf;
 
 	/* Set MSB to indicate read */
-	chip->tx_buf[0] = address | 0x80;
-
-	spi_message_add_tail(&xfer, &message);
+	*data = address | 0x80;
 
-	/* do the i/o */
-	status = spi_sync(spi, &message);
-
-	if (status == 0)
-		*data = chip->rx_buf[1];
-	return status;
+	return spi_write_then_read(spi, data, 1, data, 1);
 }
 
-static int max6902_get_datetime(struct device *dev, struct rtc_time *dt)
+static int max6902_read_time(struct device *dev, struct rtc_time *dt)
 {
-	unsigned char tmp;
-	int century;
-	int err;
+	int err, century;
 	struct spi_device *spi = to_spi_device(dev);
-	struct max6902 *chip = dev_get_drvdata(dev);
-	struct spi_message message;
-	struct spi_transfer xfer;
-	int status;
+	unsigned char buf[8];
 
-	err = max6902_get_reg(dev, MAX6902_REG_CENTURY, &tmp);
-	if (err)
-		return err;
-
-	/* build the message */
-	spi_message_init(&message);
-	memset(&xfer, 0, sizeof(xfer));
-	xfer.len = 1 + 7;	/* Burst read command + 7 registers */
-	xfer.tx_buf = chip->buf;
-	xfer.rx_buf = chip->buf;
-	chip->buf[0] = 0xbf;	/* Burst read */
-	spi_message_add_tail(&xfer, &message);
+	buf[0] = 0xbf;	/* Burst read */
 
-	/* do the i/o */
-	status = spi_sync(spi, &message);
-	if (status)
-		return status;
+	err = spi_write_then_read(spi, buf, 1, buf, 8);
+	if (err != 0)
+		return err;
 
 	/* The chip sends data in this order:
 	 * Seconds, Minutes, Hours, Date, Month, Day, Year */
-	dt->tm_sec	= bcd2bin(chip->buf[1]);
-	dt->tm_min	= bcd2bin(chip->buf[2]);
-	dt->tm_hour	= bcd2bin(chip->buf[3]);
-	dt->tm_mday	= bcd2bin(chip->buf[4]);
-	dt->tm_mon	= bcd2bin(chip->buf[5]) - 1;
-	dt->tm_wday	= bcd2bin(chip->buf[6]);
-	dt->tm_year = bcd2bin(chip->buf[7]);
+	dt->tm_sec	= bcd2bin(buf[0]);
+	dt->tm_min	= bcd2bin(buf[1]);
+	dt->tm_hour	= bcd2bin(buf[2]);
+	dt->tm_mday	= bcd2bin(buf[3]);
+	dt->tm_mon	= bcd2bin(buf[4]) - 1;
+	dt->tm_wday	= bcd2bin(buf[5]);
+	dt->tm_year	= bcd2bin(buf[6]);
+
+	/* Read century */
+	err = max6902_get_reg(dev, MAX6902_REG_CENTURY, &buf[0]);
+	if (err != 0)
+		return err;
 
-	century = bcd2bin(tmp) * 100;
+	century = bcd2bin(buf[0]) * 100;
 
 	dt->tm_year += century;
 	dt->tm_year -= 1900;
 
-#ifdef MAX6902_DEBUG
-	printk("\n%s : Read RTC values\n",__func__);
-	printk("tm_hour: %i\n",dt->tm_hour);
-	printk("tm_min : %i\n",dt->tm_min);
-	printk("tm_sec : %i\n",dt->tm_sec);
-	printk("tm_year: %i\n",dt->tm_year);
-	printk("tm_mon : %i\n",dt->tm_mon);
-	printk("tm_mday: %i\n",dt->tm_mday);
-	printk("tm_wday: %i\n",dt->tm_wday);
-#endif
-
-	return 0;
+	return rtc_valid_tm(dt);
 }
 
-static int max6902_set_datetime(struct device *dev, struct rtc_time *dt)
+static int max6902_set_time(struct device *dev, struct rtc_time *dt)
 {
-	dt->tm_year = dt->tm_year+1900;
-
-#ifdef MAX6902_DEBUG
-	printk("\n%s : Setting RTC values\n",__func__);
-	printk("tm_sec : %i\n",dt->tm_sec);
-	printk("tm_min : %i\n",dt->tm_min);
-	printk("tm_hour: %i\n",dt->tm_hour);
-	printk("tm_mday: %i\n",dt->tm_mday);
-	printk("tm_wday: %i\n",dt->tm_wday);
-	printk("tm_year: %i\n",dt->tm_year);
-#endif
+	dt->tm_year = dt->tm_year + 1900;
 
 	/* Remove write protection */
 	max6902_set_reg(dev, 0xF, 0);
@@ -173,10 +100,10 @@ static int max6902_set_datetime(struct device *dev, struct rtc_time *dt)
 	max6902_set_reg(dev, 0x05, bin2bcd(dt->tm_hour));
 
 	max6902_set_reg(dev, 0x07, bin2bcd(dt->tm_mday));
-	max6902_set_reg(dev, 0x09, bin2bcd(dt->tm_mon+1));
+	max6902_set_reg(dev, 0x09, bin2bcd(dt->tm_mon + 1));
 	max6902_set_reg(dev, 0x0B, bin2bcd(dt->tm_wday));
-	max6902_set_reg(dev, 0x0D, bin2bcd(dt->tm_year%100));
-	max6902_set_reg(dev, 0x13, bin2bcd(dt->tm_year/100));
+	max6902_set_reg(dev, 0x0D, bin2bcd(dt->tm_year % 100));
+	max6902_set_reg(dev, 0x13, bin2bcd(dt->tm_year / 100));
 
 	/* Compulab used a delay here. However, the datasheet
 	 * does not mention a delay being required anywhere... */
@@ -188,16 +115,6 @@ static int max6902_set_datetime(struct device *dev, struct rtc_time *dt)
 	return 0;
 }
 
-static int max6902_read_time(struct device *dev, struct rtc_time *tm)
-{
-	return max6902_get_datetime(dev, tm);
-}
-
-static int max6902_set_time(struct device *dev, struct rtc_time *tm)
-{
-	return max6902_set_datetime(dev, tm);
-}
-
 static const struct rtc_class_ops max6902_rtc_ops = {
 	.read_time	= max6902_read_time,
 	.set_time	= max6902_set_time,
@@ -207,45 +124,29 @@ static int __devinit max6902_probe(struct spi_device *spi)
 {
 	struct rtc_device *rtc;
 	unsigned char tmp;
-	struct max6902 *chip;
 	int res;
 
-	rtc = rtc_device_register("max6902",
-				&spi->dev, &max6902_rtc_ops, THIS_MODULE);
-	if (IS_ERR(rtc))
-		return PTR_ERR(rtc);
-
 	spi->mode = SPI_MODE_3;
 	spi->bits_per_word = 8;
 	spi_setup(spi);
 
-	chip = kzalloc(sizeof *chip, GFP_KERNEL);
-	if (!chip) {
-		rtc_device_unregister(rtc);
-		return -ENOMEM;
-	}
-	chip->rtc = rtc;
-	dev_set_drvdata(&spi->dev, chip);
-
 	res = max6902_get_reg(&spi->dev, MAX6902_REG_SECONDS, &tmp);
-	if (res) {
-		rtc_device_unregister(rtc);
+	if (res != 0)
 		return res;
-	}
+
+	rtc = rtc_device_register("max6902",
+				&spi->dev, &max6902_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtc))
+		return PTR_ERR(rtc);
 
 	return 0;
 }
 
 static int __devexit max6902_remove(struct spi_device *spi)
 {
-	struct max6902 *chip = platform_get_drvdata(spi);
-	struct rtc_device *rtc = chip->rtc;
-
-	if (rtc)
-		rtc_device_unregister(rtc);
-
-	kfree(chip);
+	struct rtc_device *rtc = platform_get_drvdata(spi);
 
+	rtc_device_unregister(rtc);
 	return 0;
 }
 
@@ -261,7 +162,6 @@ static struct spi_driver max6902_driver = {
 
 static __init int max6902_init(void)
 {
-	printk("max6902 spi driver\n");
 	return spi_register_driver(&max6902_driver);
 }
 module_init(max6902_init);
diff --git a/drivers/rtc/rtc-mv.c b/drivers/rtc/rtc-mv.c
new file mode 100644
index 000000000000..45f12dcd3716
--- /dev/null
+++ b/drivers/rtc/rtc-mv.c
@@ -0,0 +1,163 @@
+/*
+ * Driver for the RTC in Marvell SoCs.
+ *
+ * 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/kernel.h>
+#include <linux/rtc.h>
+#include <linux/bcd.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+
+
+#define RTC_TIME_REG_OFFS	0
+#define RTC_SECONDS_OFFS	0
+#define RTC_MINUTES_OFFS	8
+#define RTC_HOURS_OFFS		16
+#define RTC_WDAY_OFFS		24
+#define RTC_HOURS_12H_MODE		(1 << 22) /* 12 hours mode */
+
+#define RTC_DATE_REG_OFFS	4
+#define RTC_MDAY_OFFS		0
+#define RTC_MONTH_OFFS		8
+#define RTC_YEAR_OFFS		16
+
+
+struct rtc_plat_data {
+	struct rtc_device *rtc;
+	void __iomem *ioaddr;
+};
+
+static int mv_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct rtc_plat_data *pdata = dev_get_drvdata(dev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	u32	rtc_reg;
+
+	rtc_reg = (bin2bcd(tm->tm_sec) << RTC_SECONDS_OFFS) |
+		(bin2bcd(tm->tm_min) << RTC_MINUTES_OFFS) |
+		(bin2bcd(tm->tm_hour) << RTC_HOURS_OFFS) |
+		(bin2bcd(tm->tm_wday) << RTC_WDAY_OFFS);
+	writel(rtc_reg, ioaddr + RTC_TIME_REG_OFFS);
+
+	rtc_reg = (bin2bcd(tm->tm_mday) << RTC_MDAY_OFFS) |
+		(bin2bcd(tm->tm_mon + 1) << RTC_MONTH_OFFS) |
+		(bin2bcd(tm->tm_year % 100) << RTC_YEAR_OFFS);
+	writel(rtc_reg, ioaddr + RTC_DATE_REG_OFFS);
+
+	return 0;
+}
+
+static int mv_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct rtc_plat_data *pdata = dev_get_drvdata(dev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	u32	rtc_time, rtc_date;
+	unsigned int year, month, day, hour, minute, second, wday;
+
+	rtc_time = readl(ioaddr + RTC_TIME_REG_OFFS);
+	rtc_date = readl(ioaddr + RTC_DATE_REG_OFFS);
+
+	second = rtc_time & 0x7f;
+	minute = (rtc_time >> RTC_MINUTES_OFFS) & 0x7f;
+	hour = (rtc_time >> RTC_HOURS_OFFS) & 0x3f; /* assume 24 hours mode */
+	wday = (rtc_time >> RTC_WDAY_OFFS) & 0x7;
+
+	day = rtc_date & 0x3f;
+	month = (rtc_date >> RTC_MONTH_OFFS) & 0x3f;
+	year = (rtc_date >> RTC_YEAR_OFFS) & 0xff;
+
+	tm->tm_sec = bcd2bin(second);
+	tm->tm_min = bcd2bin(minute);
+	tm->tm_hour = bcd2bin(hour);
+	tm->tm_mday = bcd2bin(day);
+	tm->tm_wday = bcd2bin(wday);
+	tm->tm_mon = bcd2bin(month) - 1;
+	/* hw counts from year 2000, but tm_year is relative to 1900 */
+	tm->tm_year = bcd2bin(year) + 100;
+
+	return rtc_valid_tm(tm);
+}
+
+static const struct rtc_class_ops mv_rtc_ops = {
+	.read_time	= mv_rtc_read_time,
+	.set_time	= mv_rtc_set_time,
+};
+
+static int __init mv_rtc_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct rtc_plat_data *pdata;
+	resource_size_t size;
+	u32 rtc_time;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	size = resource_size(res);
+	if (!devm_request_mem_region(&pdev->dev, res->start, size,
+				     pdev->name))
+		return -EBUSY;
+
+	pdata->ioaddr = devm_ioremap(&pdev->dev, res->start, size);
+	if (!pdata->ioaddr)
+		return -ENOMEM;
+
+	/* make sure the 24 hours mode is enabled */
+	rtc_time = readl(pdata->ioaddr + RTC_TIME_REG_OFFS);
+	if (rtc_time & RTC_HOURS_12H_MODE) {
+		dev_err(&pdev->dev, "24 Hours mode not supported.\n");
+		return -EINVAL;
+	}
+
+	platform_set_drvdata(pdev, pdata);
+	pdata->rtc = rtc_device_register(pdev->name, &pdev->dev,
+					 &mv_rtc_ops, THIS_MODULE);
+	if (IS_ERR(pdata->rtc))
+		return PTR_ERR(pdata->rtc);
+
+	return 0;
+}
+
+static int __exit mv_rtc_remove(struct platform_device *pdev)
+{
+	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
+
+	rtc_device_unregister(pdata->rtc);
+	return 0;
+}
+
+static struct platform_driver mv_rtc_driver = {
+	.remove		= __exit_p(mv_rtc_remove),
+	.driver		= {
+		.name	= "rtc-mv",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static __init int mv_init(void)
+{
+	return platform_driver_probe(&mv_rtc_driver, mv_rtc_probe);
+}
+
+static __exit void mv_exit(void)
+{
+	platform_driver_unregister(&mv_rtc_driver);
+}
+
+module_init(mv_init);
+module_exit(mv_exit);
+
+MODULE_AUTHOR("Saeed Bishara <saeed@marvell.com>");
+MODULE_DESCRIPTION("Marvell RTC driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:rtc-mv");
diff --git a/drivers/rtc/rtc-pxa.c b/drivers/rtc/rtc-pxa.c
new file mode 100644
index 000000000000..cc7eb8767b82
--- /dev/null
+++ b/drivers/rtc/rtc-pxa.c
@@ -0,0 +1,489 @@
+/*
+ * Real Time Clock interface for XScale PXA27x and PXA3xx
+ *
+ * Copyright (C) 2008 Robert Jarzmik
+ *
+ * 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/platform_device.h>
+#include <linux/module.h>
+#include <linux/rtc.h>
+#include <linux/seq_file.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+
+#define TIMER_FREQ		CLOCK_TICK_RATE
+#define RTC_DEF_DIVIDER		(32768 - 1)
+#define RTC_DEF_TRIM		0
+#define MAXFREQ_PERIODIC	1000
+
+/*
+ * PXA Registers and bits definitions
+ */
+#define RTSR_PICE	(1 << 15)	/* Periodic interrupt count enable */
+#define RTSR_PIALE	(1 << 14)	/* Periodic interrupt Alarm enable */
+#define RTSR_PIAL	(1 << 13)	/* Periodic interrupt detected */
+#define RTSR_SWALE2	(1 << 11)	/* RTC stopwatch alarm2 enable */
+#define RTSR_SWAL2	(1 << 10)	/* RTC stopwatch alarm2 detected */
+#define RTSR_SWALE1	(1 << 9)	/* RTC stopwatch alarm1 enable */
+#define RTSR_SWAL1	(1 << 8)	/* RTC stopwatch alarm1 detected */
+#define RTSR_RDALE2	(1 << 7)	/* RTC alarm2 enable */
+#define RTSR_RDAL2	(1 << 6)	/* RTC alarm2 detected */
+#define RTSR_RDALE1	(1 << 5)	/* RTC alarm1 enable */
+#define RTSR_RDAL1	(1 << 4)	/* RTC alarm1 detected */
+#define RTSR_HZE	(1 << 3)	/* HZ interrupt enable */
+#define RTSR_ALE	(1 << 2)	/* RTC alarm interrupt enable */
+#define RTSR_HZ		(1 << 1)	/* HZ rising-edge detected */
+#define RTSR_AL		(1 << 0)	/* RTC alarm detected */
+#define RTSR_TRIG_MASK	(RTSR_AL | RTSR_HZ | RTSR_RDAL1 | RTSR_RDAL2\
+			 | RTSR_SWAL1 | RTSR_SWAL2)
+#define RYxR_YEAR_S	9
+#define RYxR_YEAR_MASK	(0xfff << RYxR_YEAR_S)
+#define RYxR_MONTH_S	5
+#define RYxR_MONTH_MASK	(0xf << RYxR_MONTH_S)
+#define RYxR_DAY_MASK	0x1f
+#define RDxR_HOUR_S	12
+#define RDxR_HOUR_MASK	(0x1f << RDxR_HOUR_S)
+#define RDxR_MIN_S	6
+#define RDxR_MIN_MASK	(0x3f << RDxR_MIN_S)
+#define RDxR_SEC_MASK	0x3f
+
+#define RTSR		0x08
+#define RTTR		0x0c
+#define RDCR		0x10
+#define RYCR		0x14
+#define RDAR1		0x18
+#define RYAR1		0x1c
+#define RTCPICR		0x34
+#define PIAR		0x38
+
+#define rtc_readl(pxa_rtc, reg)	\
+	__raw_readl((pxa_rtc)->base + (reg))
+#define rtc_writel(pxa_rtc, reg, value)	\
+	__raw_writel((value), (pxa_rtc)->base + (reg))
+
+struct pxa_rtc {
+	struct resource	*ress;
+	void __iomem		*base;
+	int			irq_1Hz;
+	int			irq_Alrm;
+	struct rtc_device	*rtc;
+	spinlock_t		lock;		/* Protects this structure */
+	struct rtc_time		rtc_alarm;
+};
+
+static u32 ryxr_calc(struct rtc_time *tm)
+{
+	return ((tm->tm_year + 1900) << RYxR_YEAR_S)
+		| ((tm->tm_mon + 1) << RYxR_MONTH_S)
+		| tm->tm_mday;
+}
+
+static u32 rdxr_calc(struct rtc_time *tm)
+{
+	return (tm->tm_hour << RDxR_HOUR_S) | (tm->tm_min << RDxR_MIN_S)
+		| tm->tm_sec;
+}
+
+static void tm_calc(u32 rycr, u32 rdcr, struct rtc_time *tm)
+{
+	tm->tm_year = ((rycr & RYxR_YEAR_MASK) >> RYxR_YEAR_S) - 1900;
+	tm->tm_mon = (((rycr & RYxR_MONTH_MASK) >> RYxR_MONTH_S)) - 1;
+	tm->tm_mday = (rycr & RYxR_DAY_MASK);
+	tm->tm_hour = (rdcr & RDxR_HOUR_MASK) >> RDxR_HOUR_S;
+	tm->tm_min = (rdcr & RDxR_MIN_MASK) >> RDxR_MIN_S;
+	tm->tm_sec = rdcr & RDxR_SEC_MASK;
+}
+
+static void rtsr_clear_bits(struct pxa_rtc *pxa_rtc, u32 mask)
+{
+	u32 rtsr;
+
+	rtsr = rtc_readl(pxa_rtc, RTSR);
+	rtsr &= ~RTSR_TRIG_MASK;
+	rtsr &= ~mask;
+	rtc_writel(pxa_rtc, RTSR, rtsr);
+}
+
+static void rtsr_set_bits(struct pxa_rtc *pxa_rtc, u32 mask)
+{
+	u32 rtsr;
+
+	rtsr = rtc_readl(pxa_rtc, RTSR);
+	rtsr &= ~RTSR_TRIG_MASK;
+	rtsr |= mask;
+	rtc_writel(pxa_rtc, RTSR, rtsr);
+}
+
+static irqreturn_t pxa_rtc_irq(int irq, void *dev_id)
+{
+	struct platform_device *pdev = to_platform_device(dev_id);
+	struct pxa_rtc *pxa_rtc = platform_get_drvdata(pdev);
+	u32 rtsr;
+	unsigned long events = 0;
+
+	spin_lock(&pxa_rtc->lock);
+
+	/* clear interrupt sources */
+	rtsr = rtc_readl(pxa_rtc, RTSR);
+	rtc_writel(pxa_rtc, RTSR, rtsr);
+
+	/* temporary disable rtc interrupts */
+	rtsr_clear_bits(pxa_rtc, RTSR_RDALE1 | RTSR_PIALE | RTSR_HZE);
+
+	/* clear alarm interrupt if it has occurred */
+	if (rtsr & RTSR_RDAL1)
+		rtsr &= ~RTSR_RDALE1;
+
+	/* update irq data & counter */
+	if (rtsr & RTSR_RDAL1)
+		events |= RTC_AF | RTC_IRQF;
+	if (rtsr & RTSR_HZ)
+		events |= RTC_UF | RTC_IRQF;
+	if (rtsr & RTSR_PIAL)
+		events |= RTC_PF | RTC_IRQF;
+
+	rtc_update_irq(pxa_rtc->rtc, 1, events);
+
+	/* enable back rtc interrupts */
+	rtc_writel(pxa_rtc, RTSR, rtsr & ~RTSR_TRIG_MASK);
+
+	spin_unlock(&pxa_rtc->lock);
+	return IRQ_HANDLED;
+}
+
+static int pxa_rtc_open(struct device *dev)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+	int ret;
+
+	ret = request_irq(pxa_rtc->irq_1Hz, pxa_rtc_irq, IRQF_DISABLED,
+			  "rtc 1Hz", dev);
+	if (ret < 0) {
+		dev_err(dev, "can't get irq %i, err %d\n", pxa_rtc->irq_1Hz,
+			ret);
+		goto err_irq_1Hz;
+	}
+	ret = request_irq(pxa_rtc->irq_Alrm, pxa_rtc_irq, IRQF_DISABLED,
+			  "rtc Alrm", dev);
+	if (ret < 0) {
+		dev_err(dev, "can't get irq %i, err %d\n", pxa_rtc->irq_Alrm,
+			ret);
+		goto err_irq_Alrm;
+	}
+
+	return 0;
+
+err_irq_Alrm:
+	free_irq(pxa_rtc->irq_1Hz, dev);
+err_irq_1Hz:
+	return ret;
+}
+
+static void pxa_rtc_release(struct device *dev)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+
+	spin_lock_irq(&pxa_rtc->lock);
+	rtsr_clear_bits(pxa_rtc, RTSR_PIALE | RTSR_RDALE1 | RTSR_HZE);
+	spin_unlock_irq(&pxa_rtc->lock);
+
+	free_irq(pxa_rtc->irq_Alrm, dev);
+	free_irq(pxa_rtc->irq_1Hz, dev);
+}
+
+static int pxa_periodic_irq_set_freq(struct device *dev, int freq)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+	int period_ms;
+
+	if (freq < 1 || freq > MAXFREQ_PERIODIC)
+		return -EINVAL;
+
+	period_ms = 1000 / freq;
+	rtc_writel(pxa_rtc, PIAR, period_ms);
+
+	return 0;
+}
+
+static int pxa_periodic_irq_set_state(struct device *dev, int enabled)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+
+	if (enabled)
+		rtsr_set_bits(pxa_rtc, RTSR_PIALE | RTSR_PICE);
+	else
+		rtsr_clear_bits(pxa_rtc, RTSR_PIALE | RTSR_PICE);
+
+	return 0;
+}
+
+static int pxa_rtc_ioctl(struct device *dev, unsigned int cmd,
+		unsigned long arg)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+	int ret = 0;
+
+	spin_lock_irq(&pxa_rtc->lock);
+	switch (cmd) {
+	case RTC_AIE_OFF:
+		rtsr_clear_bits(pxa_rtc, RTSR_RDALE1);
+		break;
+	case RTC_AIE_ON:
+		rtsr_set_bits(pxa_rtc, RTSR_RDALE1);
+		break;
+	case RTC_UIE_OFF:
+		rtsr_clear_bits(pxa_rtc, RTSR_HZE);
+		break;
+	case RTC_UIE_ON:
+		rtsr_set_bits(pxa_rtc, RTSR_HZE);
+		break;
+	default:
+		ret = -ENOIOCTLCMD;
+	}
+
+	spin_unlock_irq(&pxa_rtc->lock);
+	return ret;
+}
+
+static int pxa_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+	u32 rycr, rdcr;
+
+	rycr = rtc_readl(pxa_rtc, RYCR);
+	rdcr = rtc_readl(pxa_rtc, RDCR);
+
+	tm_calc(rycr, rdcr, tm);
+	return 0;
+}
+
+static int pxa_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+
+	rtc_writel(pxa_rtc, RYCR, ryxr_calc(tm));
+	rtc_writel(pxa_rtc, RDCR, rdxr_calc(tm));
+
+	return 0;
+}
+
+static int pxa_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+	u32 rtsr, ryar, rdar;
+
+	ryar = rtc_readl(pxa_rtc, RYAR1);
+	rdar = rtc_readl(pxa_rtc, RDAR1);
+	tm_calc(ryar, rdar, &alrm->time);
+
+	rtsr = rtc_readl(pxa_rtc, RTSR);
+	alrm->enabled = (rtsr & RTSR_RDALE1) ? 1 : 0;
+	alrm->pending = (rtsr & RTSR_RDAL1) ? 1 : 0;
+	return 0;
+}
+
+static int pxa_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+	u32 rtsr;
+
+	spin_lock_irq(&pxa_rtc->lock);
+
+	rtc_writel(pxa_rtc, RYAR1, ryxr_calc(&alrm->time));
+	rtc_writel(pxa_rtc, RDAR1, rdxr_calc(&alrm->time));
+
+	rtsr = rtc_readl(pxa_rtc, RTSR);
+	if (alrm->enabled)
+		rtsr |= RTSR_RDALE1;
+	else
+		rtsr &= ~RTSR_RDALE1;
+	rtc_writel(pxa_rtc, RTSR, rtsr);
+
+	spin_unlock_irq(&pxa_rtc->lock);
+
+	return 0;
+}
+
+static int pxa_rtc_proc(struct device *dev, struct seq_file *seq)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+
+	seq_printf(seq, "trim/divider\t: 0x%08x\n", rtc_readl(pxa_rtc, RTTR));
+	seq_printf(seq, "update_IRQ\t: %s\n",
+		   (rtc_readl(pxa_rtc, RTSR) & RTSR_HZE) ? "yes" : "no");
+	seq_printf(seq, "periodic_IRQ\t: %s\n",
+		   (rtc_readl(pxa_rtc, RTSR) & RTSR_PIALE) ? "yes" : "no");
+	seq_printf(seq, "periodic_freq\t: %u\n", rtc_readl(pxa_rtc, PIAR));
+
+	return 0;
+}
+
+static const struct rtc_class_ops pxa_rtc_ops = {
+	.open = pxa_rtc_open,
+	.release = pxa_rtc_release,
+	.ioctl = pxa_rtc_ioctl,
+	.read_time = pxa_rtc_read_time,
+	.set_time = pxa_rtc_set_time,
+	.read_alarm = pxa_rtc_read_alarm,
+	.set_alarm = pxa_rtc_set_alarm,
+	.proc = pxa_rtc_proc,
+	.irq_set_state = pxa_periodic_irq_set_state,
+	.irq_set_freq = pxa_periodic_irq_set_freq,
+};
+
+static int __init pxa_rtc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct pxa_rtc *pxa_rtc;
+	int ret;
+	u32 rttr;
+
+	pxa_rtc = kzalloc(sizeof(struct pxa_rtc), GFP_KERNEL);
+	if (!pxa_rtc)
+		return -ENOMEM;
+
+	spin_lock_init(&pxa_rtc->lock);
+	platform_set_drvdata(pdev, pxa_rtc);
+
+	ret = -ENXIO;
+	pxa_rtc->ress = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!pxa_rtc->ress) {
+		dev_err(dev, "No I/O memory resource defined\n");
+		goto err_ress;
+	}
+
+	pxa_rtc->irq_1Hz = platform_get_irq(pdev, 0);
+	if (pxa_rtc->irq_1Hz < 0) {
+		dev_err(dev, "No 1Hz IRQ resource defined\n");
+		goto err_ress;
+	}
+	pxa_rtc->irq_Alrm = platform_get_irq(pdev, 1);
+	if (pxa_rtc->irq_Alrm < 0) {
+		dev_err(dev, "No alarm IRQ resource defined\n");
+		goto err_ress;
+	}
+
+	ret = -ENOMEM;
+	pxa_rtc->base = ioremap(pxa_rtc->ress->start,
+				resource_size(pxa_rtc->ress));
+	if (!pxa_rtc->base) {
+		dev_err(&pdev->dev, "Unable to map pxa RTC I/O memory\n");
+		goto err_map;
+	}
+
+	/*
+	 * If the clock divider is uninitialized then reset it to the
+	 * default value to get the 1Hz clock.
+	 */
+	if (rtc_readl(pxa_rtc, RTTR) == 0) {
+		rttr = RTC_DEF_DIVIDER + (RTC_DEF_TRIM << 16);
+		rtc_writel(pxa_rtc, RTTR, rttr);
+		dev_warn(dev, "warning: initializing default clock"
+			 " divider/trim value\n");
+	}
+
+	rtsr_clear_bits(pxa_rtc, RTSR_PIALE | RTSR_RDALE1 | RTSR_HZE);
+
+	pxa_rtc->rtc = rtc_device_register("pxa-rtc", &pdev->dev, &pxa_rtc_ops,
+					   THIS_MODULE);
+	ret = PTR_ERR(pxa_rtc->rtc);
+	if (IS_ERR(pxa_rtc->rtc)) {
+		dev_err(dev, "Failed to register RTC device -> %d\n", ret);
+		goto err_rtc_reg;
+	}
+
+	device_init_wakeup(dev, 1);
+
+	return 0;
+
+err_rtc_reg:
+	 iounmap(pxa_rtc->base);
+err_ress:
+err_map:
+	kfree(pxa_rtc);
+	return ret;
+}
+
+static int __exit pxa_rtc_remove(struct platform_device *pdev)
+{
+	struct pxa_rtc *pxa_rtc = platform_get_drvdata(pdev);
+
+	rtc_device_unregister(pxa_rtc->rtc);
+
+	spin_lock_irq(&pxa_rtc->lock);
+	iounmap(pxa_rtc->base);
+	spin_unlock_irq(&pxa_rtc->lock);
+
+	kfree(pxa_rtc);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int pxa_rtc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct pxa_rtc *pxa_rtc = platform_get_drvdata(pdev);
+
+	if (device_may_wakeup(&pdev->dev))
+		enable_irq_wake(pxa_rtc->irq_Alrm);
+	return 0;
+}
+
+static int pxa_rtc_resume(struct platform_device *pdev)
+{
+	struct pxa_rtc *pxa_rtc = platform_get_drvdata(pdev);
+
+	if (device_may_wakeup(&pdev->dev))
+		disable_irq_wake(pxa_rtc->irq_Alrm);
+	return 0;
+}
+#else
+#define pxa_rtc_suspend	NULL
+#define pxa_rtc_resume	NULL
+#endif
+
+static struct platform_driver pxa_rtc_driver = {
+	.remove		= __exit_p(pxa_rtc_remove),
+	.suspend	= pxa_rtc_suspend,
+	.resume		= pxa_rtc_resume,
+	.driver		= {
+		.name		= "pxa-rtc",
+	},
+};
+
+static int __init pxa_rtc_init(void)
+{
+	if (cpu_is_pxa27x() || cpu_is_pxa3xx())
+		return platform_driver_probe(&pxa_rtc_driver, pxa_rtc_probe);
+
+	return -ENODEV;
+}
+
+static void __exit pxa_rtc_exit(void)
+{
+	platform_driver_unregister(&pxa_rtc_driver);
+}
+
+module_init(pxa_rtc_init);
+module_exit(pxa_rtc_exit);
+
+MODULE_AUTHOR("Robert Jarzmik");
+MODULE_DESCRIPTION("PXA27x/PXA3xx Realtime Clock Driver (RTC)");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:pxa-rtc");
diff --git a/drivers/rtc/rtc-s3c.c b/drivers/rtc/rtc-s3c.c
index 7a568beba3f0..e0d7b9991505 100644
--- a/drivers/rtc/rtc-s3c.c
+++ b/drivers/rtc/rtc-s3c.c
@@ -94,6 +94,9 @@ static int s3c_rtc_setfreq(struct device *dev, int freq)
 {
 	unsigned int tmp;
 
+	if (!is_power_of_2(freq))
+		return -EINVAL;
+
 	spin_lock_irq(&s3c_rtc_pie_lock);
 
 	tmp = readb(s3c_rtc_base + S3C2410_TICNT) & S3C2410_TICNT_ENABLE;
diff --git a/drivers/rtc/rtc-sh.c b/drivers/rtc/rtc-sh.c
index aaf9d6a337cc..1c3fc6b428e9 100644
--- a/drivers/rtc/rtc-sh.c
+++ b/drivers/rtc/rtc-sh.c
@@ -24,6 +24,7 @@
 #include <linux/interrupt.h>
 #include <linux/spinlock.h>
 #include <linux/io.h>
+#include <linux/log2.h>
 #include <asm/rtc.h>
 
 #define DRV_NAME	"sh-rtc"
@@ -89,7 +90,9 @@ struct sh_rtc {
 	void __iomem *regbase;
 	unsigned long regsize;
 	struct resource *res;
-	unsigned int alarm_irq, periodic_irq, carry_irq;
+	int alarm_irq;
+	int periodic_irq;
+	int carry_irq;
 	struct rtc_device *rtc_dev;
 	spinlock_t lock;
 	unsigned long capabilities;	/* See asm-sh/rtc.h for cap bits */
@@ -549,6 +552,8 @@ static int sh_rtc_irq_set_state(struct device *dev, int enabled)
 
 static int sh_rtc_irq_set_freq(struct device *dev, int freq)
 {
+	if (!is_power_of_2(freq))
+		return -EINVAL;
 	return sh_rtc_ioctl(dev, RTC_IRQP_SET, freq);
 }
 
@@ -578,7 +583,7 @@ static int __devinit sh_rtc_probe(struct platform_device *pdev)
 
 	/* get periodic/carry/alarm irqs */
 	ret = platform_get_irq(pdev, 0);
-	if (unlikely(ret < 0)) {
+	if (unlikely(ret <= 0)) {
 		ret = -ENOENT;
 		dev_err(&pdev->dev, "No IRQ for period\n");
 		goto err_badres;
@@ -586,7 +591,7 @@ static int __devinit sh_rtc_probe(struct platform_device *pdev)
 	rtc->periodic_irq = ret;
 
 	ret = platform_get_irq(pdev, 1);
-	if (unlikely(ret < 0)) {
+	if (unlikely(ret <= 0)) {
 		ret = -ENOENT;
 		dev_err(&pdev->dev, "No IRQ for carry\n");
 		goto err_badres;
@@ -594,7 +599,7 @@ static int __devinit sh_rtc_probe(struct platform_device *pdev)
 	rtc->carry_irq = ret;
 
 	ret = platform_get_irq(pdev, 2);
-	if (unlikely(ret < 0)) {
+	if (unlikely(ret <= 0)) {
 		ret = -ENOENT;
 		dev_err(&pdev->dev, "No IRQ for alarm\n");
 		goto err_badres;
diff --git a/drivers/rtc/rtc-stk17ta8.c b/drivers/rtc/rtc-stk17ta8.c
index f4cd46e15af9..7d1547b0070e 100644
--- a/drivers/rtc/rtc-stk17ta8.c
+++ b/drivers/rtc/rtc-stk17ta8.c
@@ -170,7 +170,7 @@ static int stk17ta8_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0)
+	if (pdata->irq <= 0)
 		return -EINVAL;
 	pdata->alrm_mday = alrm->time.tm_mday;
 	pdata->alrm_hour = alrm->time.tm_hour;
@@ -187,7 +187,7 @@ static int stk17ta8_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0)
+	if (pdata->irq <= 0)
 		return -EINVAL;
 	alrm->time.tm_mday = pdata->alrm_mday < 0 ? 0 : pdata->alrm_mday;
 	alrm->time.tm_hour = pdata->alrm_hour < 0 ? 0 : pdata->alrm_hour;
@@ -221,7 +221,7 @@ static int stk17ta8_rtc_ioctl(struct device *dev, unsigned int cmd,
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0)
+	if (pdata->irq <= 0)
 		return -ENOIOCTLCMD; /* fall back into rtc-dev's emulation */
 	switch (cmd) {
 	case RTC_AIE_OFF:
@@ -303,7 +303,6 @@ static int __init stk17ta8_rtc_probe(struct platform_device *pdev)
 	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
 		return -ENOMEM;
-	pdata->irq = -1;
 	if (!request_mem_region(res->start, RTC_REG_SIZE, pdev->name)) {
 		ret = -EBUSY;
 		goto out;
@@ -329,13 +328,13 @@ static int __init stk17ta8_rtc_probe(struct platform_device *pdev)
 	if (readb(ioaddr + RTC_FLAGS) & RTC_FLAGS_PF)
 		dev_warn(&pdev->dev, "voltage-low detected.\n");
 
-	if (pdata->irq >= 0) {
+	if (pdata->irq > 0) {
 		writeb(0, ioaddr + RTC_INTERRUPTS);
 		if (request_irq(pdata->irq, stk17ta8_rtc_interrupt,
 				IRQF_DISABLED | IRQF_SHARED,
 				pdev->name, pdev) < 0) {
 			dev_warn(&pdev->dev, "interrupt not available.\n");
-			pdata->irq = -1;
+			pdata->irq = 0;
 		}
 	}
 
@@ -355,7 +354,7 @@ static int __init stk17ta8_rtc_probe(struct platform_device *pdev)
  out:
 	if (pdata->rtc)
 		rtc_device_unregister(pdata->rtc);
-	if (pdata->irq >= 0)
+	if (pdata->irq > 0)
 		free_irq(pdata->irq, pdev);
 	if (ioaddr)
 		iounmap(ioaddr);
@@ -371,7 +370,7 @@ static int __devexit stk17ta8_rtc_remove(struct platform_device *pdev)
 
 	sysfs_remove_bin_file(&pdev->dev.kobj, &stk17ta8_nvram_attr);
 	rtc_device_unregister(pdata->rtc);
-	if (pdata->irq >= 0) {
+	if (pdata->irq > 0) {
 		writeb(0, pdata->ioaddr + RTC_INTERRUPTS);
 		free_irq(pdata->irq, pdev);
 	}
@@ -400,7 +399,7 @@ static __init int stk17ta8_init(void)
 
 static __exit void stk17ta8_exit(void)
 {
-	return platform_driver_unregister(&stk17ta8_rtc_driver);
+	platform_driver_unregister(&stk17ta8_rtc_driver);
 }
 
 module_init(stk17ta8_init);
diff --git a/drivers/rtc/rtc-test.c b/drivers/rtc/rtc-test.c
index bc930022004a..e478280ff628 100644
--- a/drivers/rtc/rtc-test.c
+++ b/drivers/rtc/rtc-test.c
@@ -34,14 +34,9 @@ static int test_rtc_read_time(struct device *dev,
 	return 0;
 }
 
-static int test_rtc_set_time(struct device *dev,
-	struct rtc_time *tm)
-{
-	return 0;
-}
-
 static int test_rtc_set_mmss(struct device *dev, unsigned long secs)
 {
+	dev_info(dev, "%s, secs = %lu\n", __func__, secs);
 	return 0;
 }
 
@@ -78,7 +73,6 @@ static int test_rtc_ioctl(struct device *dev, unsigned int cmd,
 static const struct rtc_class_ops test_rtc_ops = {
 	.proc = test_rtc_proc,
 	.read_time = test_rtc_read_time,
-	.set_time = test_rtc_set_time,
 	.read_alarm = test_rtc_read_alarm,
 	.set_alarm = test_rtc_set_alarm,
 	.set_mmss = test_rtc_set_mmss,
diff --git a/drivers/rtc/rtc-twl4030.c b/drivers/rtc/rtc-twl4030.c
index 01d8da9afdc8..8ce5f74ee45b 100644
--- a/drivers/rtc/rtc-twl4030.c
+++ b/drivers/rtc/rtc-twl4030.c
@@ -19,6 +19,7 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/errno.h>
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/types.h>
@@ -415,8 +416,8 @@ static int __devinit twl4030_rtc_probe(struct platform_device *pdev)
 	int irq = platform_get_irq(pdev, 0);
 	u8 rd_reg;
 
-	if (irq < 0)
-		return irq;
+	if (irq <= 0)
+		return -EINVAL;
 
 	rtc = rtc_device_register(pdev->name,
 				  &pdev->dev, &twl4030_rtc_ops, THIS_MODULE);
diff --git a/drivers/rtc/rtc-tx4939.c b/drivers/rtc/rtc-tx4939.c
new file mode 100644
index 000000000000..4ee4857ff207
--- /dev/null
+++ b/drivers/rtc/rtc-tx4939.c
@@ -0,0 +1,317 @@
+/*
+ * TX4939 internal RTC driver
+ * Based on RBTX49xx patch from CELF patch archive.
+ *
+ * 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.
+ *
+ * (C) Copyright TOSHIBA CORPORATION 2005-2007
+ */
+#include <linux/rtc.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <asm/txx9/tx4939.h>
+
+struct tx4939rtc_plat_data {
+	struct rtc_device *rtc;
+	struct tx4939_rtc_reg __iomem *rtcreg;
+};
+
+static struct tx4939rtc_plat_data *get_tx4939rtc_plat_data(struct device *dev)
+{
+	return platform_get_drvdata(to_platform_device(dev));
+}
+
+static int tx4939_rtc_cmd(struct tx4939_rtc_reg __iomem *rtcreg, int cmd)
+{
+	int i = 0;
+
+	__raw_writel(cmd, &rtcreg->ctl);
+	/* This might take 30us (next 32.768KHz clock) */
+	while (__raw_readl(&rtcreg->ctl) & TX4939_RTCCTL_BUSY) {
+		/* timeout on approx. 100us (@ GBUS200MHz) */
+		if (i++ > 200 * 100)
+			return -EBUSY;
+		cpu_relax();
+	}
+	return 0;
+}
+
+static int tx4939_rtc_set_mmss(struct device *dev, unsigned long secs)
+{
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev);
+	struct tx4939_rtc_reg __iomem *rtcreg = pdata->rtcreg;
+	int i, ret;
+	unsigned char buf[6];
+
+	buf[0] = 0;
+	buf[1] = 0;
+	buf[2] = secs;
+	buf[3] = secs >> 8;
+	buf[4] = secs >> 16;
+	buf[5] = secs >> 24;
+	spin_lock_irq(&pdata->rtc->irq_lock);
+	__raw_writel(0, &rtcreg->adr);
+	for (i = 0; i < 6; i++)
+		__raw_writel(buf[i], &rtcreg->dat);
+	ret = tx4939_rtc_cmd(rtcreg,
+			     TX4939_RTCCTL_COMMAND_SETTIME |
+			     (__raw_readl(&rtcreg->ctl) & TX4939_RTCCTL_ALME));
+	spin_unlock_irq(&pdata->rtc->irq_lock);
+	return ret;
+}
+
+static int tx4939_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev);
+	struct tx4939_rtc_reg __iomem *rtcreg = pdata->rtcreg;
+	int i, ret;
+	unsigned long sec;
+	unsigned char buf[6];
+
+	spin_lock_irq(&pdata->rtc->irq_lock);
+	ret = tx4939_rtc_cmd(rtcreg,
+			     TX4939_RTCCTL_COMMAND_GETTIME |
+			     (__raw_readl(&rtcreg->ctl) & TX4939_RTCCTL_ALME));
+	if (ret) {
+		spin_unlock_irq(&pdata->rtc->irq_lock);
+		return ret;
+	}
+	__raw_writel(2, &rtcreg->adr);
+	for (i = 2; i < 6; i++)
+		buf[i] = __raw_readl(&rtcreg->dat);
+	spin_unlock_irq(&pdata->rtc->irq_lock);
+	sec = (buf[5] << 24) | (buf[4] << 16) | (buf[3] << 8) | buf[2];
+	rtc_time_to_tm(sec, tm);
+	return rtc_valid_tm(tm);
+}
+
+static int tx4939_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev);
+	struct tx4939_rtc_reg __iomem *rtcreg = pdata->rtcreg;
+	int i, ret;
+	unsigned long sec;
+	unsigned char buf[6];
+
+	if (alrm->time.tm_sec < 0 ||
+	    alrm->time.tm_min < 0 ||
+	    alrm->time.tm_hour < 0 ||
+	    alrm->time.tm_mday < 0 ||
+	    alrm->time.tm_mon < 0 ||
+	    alrm->time.tm_year < 0)
+		return -EINVAL;
+	rtc_tm_to_time(&alrm->time, &sec);
+	buf[0] = 0;
+	buf[1] = 0;
+	buf[2] = sec;
+	buf[3] = sec >> 8;
+	buf[4] = sec >> 16;
+	buf[5] = sec >> 24;
+	spin_lock_irq(&pdata->rtc->irq_lock);
+	__raw_writel(0, &rtcreg->adr);
+	for (i = 0; i < 6; i++)
+		__raw_writel(buf[i], &rtcreg->dat);
+	ret = tx4939_rtc_cmd(rtcreg, TX4939_RTCCTL_COMMAND_SETALARM |
+			     (alrm->enabled ? TX4939_RTCCTL_ALME : 0));
+	spin_unlock_irq(&pdata->rtc->irq_lock);
+	return ret;
+}
+
+static int tx4939_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev);
+	struct tx4939_rtc_reg __iomem *rtcreg = pdata->rtcreg;
+	int i, ret;
+	unsigned long sec;
+	unsigned char buf[6];
+	u32 ctl;
+
+	spin_lock_irq(&pdata->rtc->irq_lock);
+	ret = tx4939_rtc_cmd(rtcreg,
+			     TX4939_RTCCTL_COMMAND_GETALARM |
+			     (__raw_readl(&rtcreg->ctl) & TX4939_RTCCTL_ALME));
+	if (ret) {
+		spin_unlock_irq(&pdata->rtc->irq_lock);
+		return ret;
+	}
+	__raw_writel(2, &rtcreg->adr);
+	for (i = 2; i < 6; i++)
+		buf[i] = __raw_readl(&rtcreg->dat);
+	ctl = __raw_readl(&rtcreg->ctl);
+	alrm->enabled = (ctl & TX4939_RTCCTL_ALME) ? 1 : 0;
+	alrm->pending = (ctl & TX4939_RTCCTL_ALMD) ? 1 : 0;
+	spin_unlock_irq(&pdata->rtc->irq_lock);
+	sec = (buf[5] << 24) | (buf[4] << 16) | (buf[3] << 8) | buf[2];
+	rtc_time_to_tm(sec, &alrm->time);
+	return rtc_valid_tm(&alrm->time);
+}
+
+static int tx4939_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
+{
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev);
+
+	spin_lock_irq(&pdata->rtc->irq_lock);
+	tx4939_rtc_cmd(pdata->rtcreg,
+		       TX4939_RTCCTL_COMMAND_NOP |
+		       (enabled ? TX4939_RTCCTL_ALME : 0));
+	spin_unlock_irq(&pdata->rtc->irq_lock);
+	return 0;
+}
+
+static irqreturn_t tx4939_rtc_interrupt(int irq, void *dev_id)
+{
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev_id);
+	struct tx4939_rtc_reg __iomem *rtcreg = pdata->rtcreg;
+	unsigned long events = RTC_IRQF;
+
+	spin_lock(&pdata->rtc->irq_lock);
+	if (__raw_readl(&rtcreg->ctl) & TX4939_RTCCTL_ALMD) {
+		events |= RTC_AF;
+		tx4939_rtc_cmd(rtcreg, TX4939_RTCCTL_COMMAND_NOP);
+	}
+	spin_unlock(&pdata->rtc->irq_lock);
+	rtc_update_irq(pdata->rtc, 1, events);
+	return IRQ_HANDLED;
+}
+
+static const struct rtc_class_ops tx4939_rtc_ops = {
+	.read_time		= tx4939_rtc_read_time,
+	.read_alarm		= tx4939_rtc_read_alarm,
+	.set_alarm		= tx4939_rtc_set_alarm,
+	.set_mmss		= tx4939_rtc_set_mmss,
+	.alarm_irq_enable	= tx4939_rtc_alarm_irq_enable,
+};
+
+static ssize_t tx4939_rtc_nvram_read(struct kobject *kobj,
+				     struct bin_attribute *bin_attr,
+				     char *buf, loff_t pos, size_t size)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev);
+	struct tx4939_rtc_reg __iomem *rtcreg = pdata->rtcreg;
+	ssize_t count;
+
+	spin_lock_irq(&pdata->rtc->irq_lock);
+	for (count = 0; size > 0 && pos < TX4939_RTC_REG_RAMSIZE;
+	     count++, size--) {
+		__raw_writel(pos++, &rtcreg->adr);
+		*buf++ = __raw_readl(&rtcreg->dat);
+	}
+	spin_unlock_irq(&pdata->rtc->irq_lock);
+	return count;
+}
+
+static ssize_t tx4939_rtc_nvram_write(struct kobject *kobj,
+				      struct bin_attribute *bin_attr,
+				      char *buf, loff_t pos, size_t size)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev);
+	struct tx4939_rtc_reg __iomem *rtcreg = pdata->rtcreg;
+	ssize_t count;
+
+	spin_lock_irq(&pdata->rtc->irq_lock);
+	for (count = 0; size > 0 && pos < TX4939_RTC_REG_RAMSIZE;
+	     count++, size--) {
+		__raw_writel(pos++, &rtcreg->adr);
+		__raw_writel(*buf++, &rtcreg->dat);
+	}
+	spin_unlock_irq(&pdata->rtc->irq_lock);
+	return count;
+}
+
+static struct bin_attribute tx4939_rtc_nvram_attr = {
+	.attr = {
+		.name = "nvram",
+		.mode = S_IRUGO | S_IWUSR,
+	},
+	.size = TX4939_RTC_REG_RAMSIZE,
+	.read = tx4939_rtc_nvram_read,
+	.write = tx4939_rtc_nvram_write,
+};
+
+static int __init tx4939_rtc_probe(struct platform_device *pdev)
+{
+	struct rtc_device *rtc;
+	struct tx4939rtc_plat_data *pdata;
+	struct resource *res;
+	int irq, ret;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return -ENODEV;
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, pdata);
+
+	if (!devm_request_mem_region(&pdev->dev, res->start,
+				     resource_size(res), pdev->name))
+		return -EBUSY;
+	pdata->rtcreg = devm_ioremap(&pdev->dev, res->start,
+				     resource_size(res));
+	if (!pdata->rtcreg)
+		return -EBUSY;
+
+	tx4939_rtc_cmd(pdata->rtcreg, TX4939_RTCCTL_COMMAND_NOP);
+	if (devm_request_irq(&pdev->dev, irq, tx4939_rtc_interrupt,
+			     IRQF_DISABLED | IRQF_SHARED,
+			     pdev->name, &pdev->dev) < 0) {
+		return -EBUSY;
+	}
+	rtc = rtc_device_register(pdev->name, &pdev->dev,
+				  &tx4939_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtc))
+		return PTR_ERR(rtc);
+	pdata->rtc = rtc;
+	ret = sysfs_create_bin_file(&pdev->dev.kobj, &tx4939_rtc_nvram_attr);
+	if (ret)
+		rtc_device_unregister(rtc);
+	return ret;
+}
+
+static int __exit tx4939_rtc_remove(struct platform_device *pdev)
+{
+	struct tx4939rtc_plat_data *pdata = platform_get_drvdata(pdev);
+	struct rtc_device *rtc = pdata->rtc;
+
+	spin_lock_irq(&rtc->irq_lock);
+	tx4939_rtc_cmd(pdata->rtcreg, TX4939_RTCCTL_COMMAND_NOP);
+	spin_unlock_irq(&rtc->irq_lock);
+	sysfs_remove_bin_file(&pdev->dev.kobj, &tx4939_rtc_nvram_attr);
+	rtc_device_unregister(rtc);
+	platform_set_drvdata(pdev, NULL);
+	return 0;
+}
+
+static struct platform_driver tx4939_rtc_driver = {
+	.remove		= __exit_p(tx4939_rtc_remove),
+	.driver		= {
+		.name	= "tx4939rtc",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init tx4939rtc_init(void)
+{
+	return platform_driver_probe(&tx4939_rtc_driver, tx4939_rtc_probe);
+}
+
+static void __exit tx4939rtc_exit(void)
+{
+	platform_driver_unregister(&tx4939_rtc_driver);
+}
+
+module_init(tx4939rtc_init);
+module_exit(tx4939rtc_exit);
+
+MODULE_AUTHOR("Atsushi Nemoto <anemo@mba.ocn.ne.jp>");
+MODULE_DESCRIPTION("TX4939 internal RTC driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:tx4939rtc");
diff --git a/drivers/rtc/rtc-vr41xx.c b/drivers/rtc/rtc-vr41xx.c
index 834dcc6d785f..f11297aff854 100644
--- a/drivers/rtc/rtc-vr41xx.c
+++ b/drivers/rtc/rtc-vr41xx.c
@@ -27,6 +27,7 @@
 #include <linux/rtc.h>
 #include <linux/spinlock.h>
 #include <linux/types.h>
+#include <linux/log2.h>
 
 #include <asm/div64.h>
 #include <asm/io.h>
@@ -84,8 +85,8 @@ static DEFINE_SPINLOCK(rtc_lock);
 static char rtc_name[] = "RTC";
 static unsigned long periodic_count;
 static unsigned int alarm_enabled;
-static int aie_irq = -1;
-static int pie_irq = -1;
+static int aie_irq;
+static int pie_irq;
 
 static inline unsigned long read_elapsed_second(void)
 {
@@ -210,6 +211,8 @@ static int vr41xx_rtc_irq_set_freq(struct device *dev, int freq)
 {
 	unsigned long count;
 
+	if (!is_power_of_2(freq))
+		return -EINVAL;
 	count = RTC_FREQUENCY;
 	do_div(count, freq);
 
@@ -360,7 +363,7 @@ static int __devinit rtc_probe(struct platform_device *pdev)
 	spin_unlock_irq(&rtc_lock);
 
 	aie_irq = platform_get_irq(pdev, 0);
-	if (aie_irq < 0 || aie_irq >= nr_irqs) {
+	if (aie_irq <= 0) {
 		retval = -EBUSY;
 		goto err_device_unregister;
 	}
@@ -371,7 +374,7 @@ static int __devinit rtc_probe(struct platform_device *pdev)
 		goto err_device_unregister;
 
 	pie_irq = platform_get_irq(pdev, 1);
-	if (pie_irq < 0 || pie_irq >= nr_irqs)
+	if (pie_irq <= 0)
 		goto err_free_irq;
 
 	retval = request_irq(pie_irq, rtclong1_interrupt, IRQF_DISABLED,
diff --git a/drivers/s390/Makefile b/drivers/s390/Makefile
index 4f4e7cf105d4..d0eae59bc366 100644
--- a/drivers/s390/Makefile
+++ b/drivers/s390/Makefile
@@ -4,7 +4,7 @@
 
 CFLAGS_sysinfo.o += -Iinclude/math-emu -Iarch/s390/math-emu -w
 
-obj-y += s390mach.o sysinfo.o s390_rdev.o
+obj-y += s390mach.o sysinfo.o
 obj-y += cio/ block/ char/ crypto/ net/ scsi/ kvm/
 
 drivers-y += drivers/s390/built-in.o
diff --git a/drivers/s390/block/dasd_eer.c b/drivers/s390/block/dasd_eer.c
index 892e2878d61b..f8e05ce98621 100644
--- a/drivers/s390/block/dasd_eer.c
+++ b/drivers/s390/block/dasd_eer.c
@@ -535,8 +535,8 @@ static int dasd_eer_open(struct inode *inp, struct file *filp)
 	    eerb->buffer_page_count > INT_MAX / PAGE_SIZE) {
 		kfree(eerb);
 		MESSAGE(KERN_WARNING, "can't open device since module "
-			"parameter eer_pages is smaller then 1 or"
-			" bigger then %d", (int)(INT_MAX / PAGE_SIZE));
+			"parameter eer_pages is smaller than 1 or"
+			" bigger than %d", (int)(INT_MAX / PAGE_SIZE));
 		unlock_kernel();
 		return -EINVAL;
 	}
diff --git a/drivers/s390/block/dcssblk.c b/drivers/s390/block/dcssblk.c
index 26ffc6ab441d..cfdcf1aed33c 100644
--- a/drivers/s390/block/dcssblk.c
+++ b/drivers/s390/block/dcssblk.c
@@ -18,7 +18,6 @@
 #include <asm/io.h>
 #include <linux/completion.h>
 #include <linux/interrupt.h>
-#include <asm/s390_rdev.h>
 
 #define DCSSBLK_NAME "dcssblk"
 #define DCSSBLK_MINORS_PER_DISK 1
@@ -946,7 +945,7 @@ dcssblk_check_params(void)
 static void __exit
 dcssblk_exit(void)
 {
-	s390_root_dev_unregister(dcssblk_root_dev);
+	root_device_unregister(dcssblk_root_dev);
 	unregister_blkdev(dcssblk_major, DCSSBLK_NAME);
 }
 
@@ -955,22 +954,22 @@ dcssblk_init(void)
 {
 	int rc;
 
-	dcssblk_root_dev = s390_root_dev_register("dcssblk");
+	dcssblk_root_dev = root_device_register("dcssblk");
 	if (IS_ERR(dcssblk_root_dev))
 		return PTR_ERR(dcssblk_root_dev);
 	rc = device_create_file(dcssblk_root_dev, &dev_attr_add);
 	if (rc) {
-		s390_root_dev_unregister(dcssblk_root_dev);
+		root_device_unregister(dcssblk_root_dev);
 		return rc;
 	}
 	rc = device_create_file(dcssblk_root_dev, &dev_attr_remove);
 	if (rc) {
-		s390_root_dev_unregister(dcssblk_root_dev);
+		root_device_unregister(dcssblk_root_dev);
 		return rc;
 	}
 	rc = register_blkdev(0, DCSSBLK_NAME);
 	if (rc < 0) {
-		s390_root_dev_unregister(dcssblk_root_dev);
+		root_device_unregister(dcssblk_root_dev);
 		return rc;
 	}
 	dcssblk_major = rc;
diff --git a/drivers/s390/char/vmlogrdr.c b/drivers/s390/char/vmlogrdr.c
index aabbeb909cc6..d8a2289fcb69 100644
--- a/drivers/s390/char/vmlogrdr.c
+++ b/drivers/s390/char/vmlogrdr.c
@@ -427,7 +427,7 @@ static int vmlogrdr_receive_data(struct vmlogrdr_priv_t *priv)
 			buffer = priv->buffer + sizeof(int);
 		}
 		/*
-		 * If the record is bigger then our buffer, we receive only
+		 * If the record is bigger than our buffer, we receive only
 		 * a part of it. We can get the rest later.
 		 */
 		if (iucv_data_count > NET_BUFFER_SIZE)
@@ -437,7 +437,7 @@ static int vmlogrdr_receive_data(struct vmlogrdr_priv_t *priv)
 					  0, buffer, iucv_data_count,
 					  &priv->residual_length);
 		spin_unlock_bh(&priv->priv_lock);
-		/* An rc of 5 indicates that the record was bigger then
+		/* An rc of 5 indicates that the record was bigger than
 		 * the buffer, which is OK for us. A 9 indicates that the
 		 * record was purged befor we could receive it.
 		 */
diff --git a/drivers/s390/crypto/ap_bus.c b/drivers/s390/crypto/ap_bus.c
index 1f5f5d2d87d9..9c148406b980 100644
--- a/drivers/s390/crypto/ap_bus.c
+++ b/drivers/s390/crypto/ap_bus.c
@@ -36,7 +36,6 @@
 #include <linux/notifier.h>
 #include <linux/kthread.h>
 #include <linux/mutex.h>
-#include <asm/s390_rdev.h>
 #include <asm/reset.h>
 #include <asm/airq.h>
 #include <asm/atomic.h>
@@ -1522,7 +1521,7 @@ int __init ap_module_init(void)
 	}
 
 	/* Create /sys/devices/ap. */
-	ap_root_device = s390_root_dev_register("ap");
+	ap_root_device = root_device_register("ap");
 	rc = IS_ERR(ap_root_device) ? PTR_ERR(ap_root_device) : 0;
 	if (rc)
 		goto out_bus;
@@ -1565,7 +1564,7 @@ out_work:
 	hrtimer_cancel(&ap_poll_timer);
 	destroy_workqueue(ap_work_queue);
 out_root:
-	s390_root_dev_unregister(ap_root_device);
+	root_device_unregister(ap_root_device);
 out_bus:
 	while (i--)
 		bus_remove_file(&ap_bus_type, ap_bus_attrs[i]);
@@ -1600,7 +1599,7 @@ void ap_module_exit(void)
 	hrtimer_cancel(&ap_poll_timer);
 	destroy_workqueue(ap_work_queue);
 	tasklet_kill(&ap_tasklet);
-	s390_root_dev_unregister(ap_root_device);
+	root_device_unregister(ap_root_device);
 	while ((dev = bus_find_device(&ap_bus_type, NULL, NULL,
 		    __ap_match_all)))
 	{
diff --git a/drivers/s390/kvm/kvm_virtio.c b/drivers/s390/kvm/kvm_virtio.c
index 28c90b89f2b4..cbc8566fab70 100644
--- a/drivers/s390/kvm/kvm_virtio.c
+++ b/drivers/s390/kvm/kvm_virtio.c
@@ -24,7 +24,6 @@
 #include <asm/kvm_virtio.h>
 #include <asm/setup.h>
 #include <asm/s390_ext.h>
-#include <asm/s390_rdev.h>
 
 #define VIRTIO_SUBCODE_64 0x0D00
 
@@ -335,7 +334,7 @@ static int __init kvm_devices_init(void)
 	if (!MACHINE_IS_KVM)
 		return -ENODEV;
 
-	kvm_root = s390_root_dev_register("kvm_s390");
+	kvm_root = root_device_register("kvm_s390");
 	if (IS_ERR(kvm_root)) {
 		rc = PTR_ERR(kvm_root);
 		printk(KERN_ERR "Could not register kvm_s390 root device");
@@ -344,7 +343,7 @@ static int __init kvm_devices_init(void)
 
 	rc = vmem_add_mapping(real_memory_size, PAGE_SIZE);
 	if (rc) {
-		s390_root_dev_unregister(kvm_root);
+		root_device_unregister(kvm_root);
 		return rc;
 	}
 
diff --git a/drivers/s390/net/cu3088.c b/drivers/s390/net/cu3088.c
index f4a32375c037..48383459e99b 100644
--- a/drivers/s390/net/cu3088.c
+++ b/drivers/s390/net/cu3088.c
@@ -25,7 +25,6 @@
 #include <linux/module.h>
 #include <linux/err.h>
 
-#include <asm/s390_rdev.h>
 #include <asm/ccwdev.h>
 #include <asm/ccwgroup.h>
 
@@ -120,12 +119,12 @@ cu3088_init (void)
 {
 	int rc;
 
-	cu3088_root_dev = s390_root_dev_register("cu3088");
+	cu3088_root_dev = root_device_register("cu3088");
 	if (IS_ERR(cu3088_root_dev))
 		return PTR_ERR(cu3088_root_dev);
 	rc = ccw_driver_register(&cu3088_driver);
 	if (rc)
-		s390_root_dev_unregister(cu3088_root_dev);
+		root_device_unregister(cu3088_root_dev);
 
 	return rc;
 }
@@ -134,7 +133,7 @@ static void __exit
 cu3088_exit (void)
 {
 	ccw_driver_unregister(&cu3088_driver);
-	s390_root_dev_unregister(cu3088_root_dev);
+	root_device_unregister(cu3088_root_dev);
 }
 
 MODULE_DEVICE_TABLE(ccw,cu3088_ids);
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index 6811dd529f48..d1b5bebea7fb 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -24,7 +24,6 @@
 
 #include <asm/ebcdic.h>
 #include <asm/io.h>
-#include <asm/s390_rdev.h>
 
 #include "qeth_core.h"
 #include "qeth_core_offl.h"
@@ -4525,7 +4524,7 @@ static int __init qeth_core_init(void)
 				&driver_attr_group);
 	if (rc)
 		goto driver_err;
-	qeth_core_root_dev = s390_root_dev_register("qeth");
+	qeth_core_root_dev = root_device_register("qeth");
 	rc = IS_ERR(qeth_core_root_dev) ? PTR_ERR(qeth_core_root_dev) : 0;
 	if (rc)
 		goto register_err;
@@ -4539,7 +4538,7 @@ static int __init qeth_core_init(void)
 
 	return 0;
 slab_err:
-	s390_root_dev_unregister(qeth_core_root_dev);
+	root_device_unregister(qeth_core_root_dev);
 register_err:
 	driver_remove_file(&qeth_core_ccwgroup_driver.driver,
 			   &driver_attr_group);
@@ -4557,7 +4556,7 @@ out_err:
 
 static void __exit qeth_core_exit(void)
 {
-	s390_root_dev_unregister(qeth_core_root_dev);
+	root_device_unregister(qeth_core_root_dev);
 	driver_remove_file(&qeth_core_ccwgroup_driver.driver,
 			   &driver_attr_group);
 	ccwgroup_driver_unregister(&qeth_core_ccwgroup_driver);
diff --git a/drivers/s390/net/qeth_l2_main.c b/drivers/s390/net/qeth_l2_main.c
index 21627ba3093b..591a2b3ae4cb 100644
--- a/drivers/s390/net/qeth_l2_main.c
+++ b/drivers/s390/net/qeth_l2_main.c
@@ -20,8 +20,6 @@
 #include <linux/mii.h>
 #include <linux/ip.h>
 
-#include <asm/s390_rdev.h>
-
 #include "qeth_core.h"
 #include "qeth_core_offl.h"
 
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index cfda1ecffdf2..4693ee4e7b98 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -26,8 +26,6 @@
 #include <net/ip.h>
 #include <net/arp.h>
 
-#include <asm/s390_rdev.h>
-
 #include "qeth_l3.h"
 #include "qeth_core_offl.h"
 
diff --git a/drivers/s390/s390_rdev.c b/drivers/s390/s390_rdev.c
deleted file mode 100644
index 64371c05a3b3..000000000000
--- a/drivers/s390/s390_rdev.c
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- *  drivers/s390/s390_rdev.c
- *  s390 root device
- *
- *    Copyright (C) 2002, 2005 IBM Deutschland Entwicklung GmbH,
- *			 IBM Corporation
- *    Author(s): Cornelia Huck (cornelia.huck@de.ibm.com)
- *		  Carsten Otte  (cotte@de.ibm.com)
- */
-
-#include <linux/slab.h>
-#include <linux/err.h>
-#include <linux/device.h>
-#include <asm/s390_rdev.h>
-
-static void
-s390_root_dev_release(struct device *dev)
-{
-	kfree(dev);
-}
-
-struct device *
-s390_root_dev_register(const char *name)
-{
-	struct device *dev;
-	int ret;
-
-	if (!strlen(name))
-		return ERR_PTR(-EINVAL);
-	dev = kzalloc(sizeof(struct device), GFP_KERNEL);
-	if (!dev)
-		return ERR_PTR(-ENOMEM);
-	dev_set_name(dev, name);
-	dev->release = s390_root_dev_release;
-	ret = device_register(dev);
-	if (ret) {
-		kfree(dev);
-		return ERR_PTR(ret);
-	}
-	return dev;
-}
-
-void
-s390_root_dev_unregister(struct device *dev)
-{
-	if (dev)
-		device_unregister(dev);
-}
-
-EXPORT_SYMBOL(s390_root_dev_register);
-EXPORT_SYMBOL(s390_root_dev_unregister);
diff --git a/drivers/scsi/a100u2w.c b/drivers/scsi/a100u2w.c
index 3c298c7253ee..964769f66eac 100644
--- a/drivers/scsi/a100u2w.c
+++ b/drivers/scsi/a100u2w.c
@@ -633,7 +633,7 @@ static int orc_device_reset(struct orc_host * host, struct scsi_cmnd *cmd, unsig
 		return FAILED;
 	}
 
-	/* Reset device is handled by the firmare, we fill in an SCB and
+	/* Reset device is handled by the firmware, we fill in an SCB and
 	   fire it at the controller, it does the rest */
 	scb->opcode = ORC_BUSDEVRST;
 	scb->target = target;
diff --git a/drivers/scsi/lpfc/lpfc_hbadisc.c b/drivers/scsi/lpfc/lpfc_hbadisc.c
index 8c64494444bf..311ed6dea726 100644
--- a/drivers/scsi/lpfc/lpfc_hbadisc.c
+++ b/drivers/scsi/lpfc/lpfc_hbadisc.c
@@ -1964,10 +1964,10 @@ lpfc_set_disctmo(struct lpfc_vport *vport)
 	uint32_t tmo;
 
 	if (vport->port_state == LPFC_LOCAL_CFG_LINK) {
-		/* For FAN, timeout should be greater then edtov */
+		/* For FAN, timeout should be greater than edtov */
 		tmo = (((phba->fc_edtov + 999) / 1000) + 1);
 	} else {
-		/* Normal discovery timeout should be > then ELS/CT timeout
+		/* Normal discovery timeout should be > than ELS/CT timeout
 		 * FC spec states we need 3 * ratov for CT requests
 		 */
 		tmo = ((phba->fc_ratov * 3) + 3);
diff --git a/drivers/scsi/lpfc/lpfc_sli.c b/drivers/scsi/lpfc/lpfc_sli.c
index 01dfdc8696f8..a36a120561e2 100644
--- a/drivers/scsi/lpfc/lpfc_sli.c
+++ b/drivers/scsi/lpfc/lpfc_sli.c
@@ -420,7 +420,7 @@ lpfc_sli_next_iocb_slot (struct lpfc_hba *phba, struct lpfc_sli_ring *pring)
 		if (unlikely(pring->local_getidx >= max_cmd_idx)) {
 			lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 					"0315 Ring %d issue: portCmdGet %d "
-					"is bigger then cmd ring %d\n",
+					"is bigger than cmd ring %d\n",
 					pring->ringno,
 					pring->local_getidx, max_cmd_idx);
 
@@ -1628,12 +1628,12 @@ lpfc_sli_rsp_pointers_error(struct lpfc_hba *phba, struct lpfc_sli_ring *pring)
 {
 	struct lpfc_pgp *pgp = &phba->port_gp[pring->ringno];
 	/*
-	 * Ring <ringno> handler: portRspPut <portRspPut> is bigger then
+	 * Ring <ringno> handler: portRspPut <portRspPut> is bigger than
 	 * rsp ring <portRspMax>
 	 */
 	lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 			"0312 Ring %d handler: portRspPut %d "
-			"is bigger then rsp ring %d\n",
+			"is bigger than rsp ring %d\n",
 			pring->ringno, le32_to_cpu(pgp->rspPutInx),
 			pring->numRiocb);
 
@@ -2083,12 +2083,12 @@ lpfc_sli_handle_slow_ring_event(struct lpfc_hba *phba,
 	portRspPut = le32_to_cpu(pgp->rspPutInx);
 	if (portRspPut >= portRspMax) {
 		/*
-		 * Ring <ringno> handler: portRspPut <portRspPut> is bigger then
+		 * Ring <ringno> handler: portRspPut <portRspPut> is bigger than
 		 * rsp ring <portRspMax>
 		 */
 		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 				"0303 Ring %d handler: portRspPut %d "
-				"is bigger then rsp ring %d\n",
+				"is bigger than rsp ring %d\n",
 				pring->ringno, portRspPut, portRspMax);
 
 		phba->link_state = LPFC_HBA_ERROR;
diff --git a/drivers/scsi/megaraid.c b/drivers/scsi/megaraid.c
index 7dc62deb4087..9fdcd60c5493 100644
--- a/drivers/scsi/megaraid.c
+++ b/drivers/scsi/megaraid.c
@@ -1967,8 +1967,8 @@ megaraid_abort_and_reset(adapter_t *adapter, Scsi_Cmnd *cmd, int aor)
 			scb->state |= aor;
 
 			/*
-			 * Check if this command has firmare owenership. If
-			 * yes, we cannot reset this command. Whenever, f/w
+			 * Check if this command has firmware ownership. If
+			 * yes, we cannot reset this command. Whenever f/w
 			 * completes this command, we will return appropriate
 			 * status from ISR.
 			 */
diff --git a/drivers/scsi/qla1280.c b/drivers/scsi/qla1280.c
index 8cb9240596ab..df09820e8916 100644
--- a/drivers/scsi/qla1280.c
+++ b/drivers/scsi/qla1280.c
@@ -128,7 +128,7 @@
 	- Integrate ql12160_set_target_parameters() with 1280 version
 	- Make qla1280_setup() non static
 	- Do not call qla1280_check_for_dead_scsi_bus() on every I/O request
-	  sent to the card - this command pauses the firmare!!!
+	  sent to the card - this command pauses the firmware!!!
     Rev  3.23.15 Beta March 19, 2002, Jes Sorensen
 	- Clean up qla1280.h - remove obsolete QL_DEBUG_LEVEL_x definitions
 	- Remove a pile of pointless and confusing (srb_t **) and
@@ -659,7 +659,7 @@ static int qla1280_read_nvram(struct scsi_qla_host *ha)
 	/* The firmware interface is, um, interesting, in that the
 	 * actual firmware image on the chip is little endian, thus,
 	 * the process of taking that image to the CPU would end up
-	 * little endian.  However, the firmare interface requires it
+	 * little endian.  However, the firmware interface requires it
 	 * to be read a word (two bytes) at a time.
 	 *
 	 * The net result of this would be that the word (and
diff --git a/drivers/scsi/qla4xxx/ql4_mbx.c b/drivers/scsi/qla4xxx/ql4_mbx.c
index c577d79bd7e8..051b0f5e8c8e 100644
--- a/drivers/scsi/qla4xxx/ql4_mbx.c
+++ b/drivers/scsi/qla4xxx/ql4_mbx.c
@@ -392,7 +392,7 @@ int qla4xxx_get_firmware_status(struct scsi_qla_host * ha)
 		ha->iocb_hiwat -= IOCB_HIWAT_CUSHION;
 	else
 		dev_info(&ha->pdev->dev, "WARNING!!!  You have less than %d "
-			   "firmare IOCBs available (%d).\n",
+			   "firmware IOCBs available (%d).\n",
 			   IOCB_HIWAT_CUSHION, ha->iocb_hiwat);
 
 	return QLA_SUCCESS;
diff --git a/drivers/scsi/scsi_error.c b/drivers/scsi/scsi_error.c
index 381838ebd460..d86ebea9350a 100644
--- a/drivers/scsi/scsi_error.c
+++ b/drivers/scsi/scsi_error.c
@@ -1650,7 +1650,7 @@ int scsi_error_handler(void *data)
 	 * We use TASK_INTERRUPTIBLE so that the thread is not
 	 * counted against the load average as a running process.
 	 * We never actually get interrupted because kthread_run
-	 * disables singal delivery for the created thread.
+	 * disables signal delivery for the created thread.
 	 */
 	set_current_state(TASK_INTERRUPTIBLE);
 	while (!kthread_should_stop()) {
diff --git a/drivers/scsi/scsi_scan.c b/drivers/scsi/scsi_scan.c
index 18486b51668d..17914a346f71 100644
--- a/drivers/scsi/scsi_scan.c
+++ b/drivers/scsi/scsi_scan.c
@@ -32,6 +32,7 @@
 #include <linux/delay.h>
 #include <linux/kthread.h>
 #include <linux/spinlock.h>
+#include <linux/async.h>
 
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
@@ -179,6 +180,8 @@ int scsi_complete_async_scans(void)
 	spin_unlock(&async_scan_lock);
 
 	kfree(data);
+	/* Synchronize async operations globally */
+	async_synchronize_full();
 	return 0;
 }
 
diff --git a/drivers/scsi/sd.c b/drivers/scsi/sd.c
index 62b28d58e65e..e035c1114010 100644
--- a/drivers/scsi/sd.c
+++ b/drivers/scsi/sd.c
@@ -48,6 +48,7 @@
 #include <linux/delay.h>
 #include <linux/mutex.h>
 #include <linux/string_helpers.h>
+#include <linux/async.h>
 #include <asm/uaccess.h>
 
 #include <scsi/scsi.h>
@@ -1802,6 +1803,71 @@ static int sd_format_disk_name(char *prefix, int index, char *buf, int buflen)
 	return 0;
 }
 
+/*
+ * The asynchronous part of sd_probe
+ */
+static void sd_probe_async(void *data, async_cookie_t cookie)
+{
+	struct scsi_disk *sdkp = data;
+	struct scsi_device *sdp;
+	struct gendisk *gd;
+	u32 index;
+	struct device *dev;
+
+	sdp = sdkp->device;
+	gd = sdkp->disk;
+	index = sdkp->index;
+	dev = &sdp->sdev_gendev;
+
+	if (!sdp->request_queue->rq_timeout) {
+		if (sdp->type != TYPE_MOD)
+			blk_queue_rq_timeout(sdp->request_queue, SD_TIMEOUT);
+		else
+			blk_queue_rq_timeout(sdp->request_queue,
+					     SD_MOD_TIMEOUT);
+	}
+
+	device_initialize(&sdkp->dev);
+	sdkp->dev.parent = &sdp->sdev_gendev;
+	sdkp->dev.class = &sd_disk_class;
+	strncpy(sdkp->dev.bus_id, sdp->sdev_gendev.bus_id, BUS_ID_SIZE);
+
+	if (device_add(&sdkp->dev))
+		goto out_free_index;
+
+	get_device(&sdp->sdev_gendev);
+
+	if (index < SD_MAX_DISKS) {
+		gd->major = sd_major((index & 0xf0) >> 4);
+		gd->first_minor = ((index & 0xf) << 4) | (index & 0xfff00);
+		gd->minors = SD_MINORS;
+	}
+	gd->fops = &sd_fops;
+	gd->private_data = &sdkp->driver;
+	gd->queue = sdkp->device->request_queue;
+
+	sd_revalidate_disk(gd);
+
+	blk_queue_prep_rq(sdp->request_queue, sd_prep_fn);
+
+	gd->driverfs_dev = &sdp->sdev_gendev;
+	gd->flags = GENHD_FL_EXT_DEVT | GENHD_FL_DRIVERFS;
+	if (sdp->removable)
+		gd->flags |= GENHD_FL_REMOVABLE;
+
+	dev_set_drvdata(dev, sdkp);
+	add_disk(gd);
+	sd_dif_config_host(sdkp);
+
+	sd_printk(KERN_NOTICE, sdkp, "Attached SCSI %sdisk\n",
+		  sdp->removable ? "removable " : "");
+
+	return;
+
+ out_free_index:
+	ida_remove(&sd_index_ida, index);
+}
+
 /**
  *	sd_probe - called during driver initialization and whenever a
  *	new scsi device is attached to the system. It is called once
@@ -1865,48 +1931,7 @@ static int sd_probe(struct device *dev)
 	sdkp->openers = 0;
 	sdkp->previous_state = 1;
 
-	if (!sdp->request_queue->rq_timeout) {
-		if (sdp->type != TYPE_MOD)
-			blk_queue_rq_timeout(sdp->request_queue, SD_TIMEOUT);
-		else
-			blk_queue_rq_timeout(sdp->request_queue,
-					     SD_MOD_TIMEOUT);
-	}
-
-	device_initialize(&sdkp->dev);
-	sdkp->dev.parent = &sdp->sdev_gendev;
-	sdkp->dev.class = &sd_disk_class;
-	strncpy(sdkp->dev.bus_id, sdp->sdev_gendev.bus_id, BUS_ID_SIZE);
-
-	if (device_add(&sdkp->dev))
-		goto out_free_index;
-
-	get_device(&sdp->sdev_gendev);
-
-	if (index < SD_MAX_DISKS) {
-		gd->major = sd_major((index & 0xf0) >> 4);
-		gd->first_minor = ((index & 0xf) << 4) | (index & 0xfff00);
-		gd->minors = SD_MINORS;
-	}
-	gd->fops = &sd_fops;
-	gd->private_data = &sdkp->driver;
-	gd->queue = sdkp->device->request_queue;
-
-	sd_revalidate_disk(gd);
-
-	blk_queue_prep_rq(sdp->request_queue, sd_prep_fn);
-
-	gd->driverfs_dev = &sdp->sdev_gendev;
-	gd->flags = GENHD_FL_EXT_DEVT | GENHD_FL_DRIVERFS;
-	if (sdp->removable)
-		gd->flags |= GENHD_FL_REMOVABLE;
-
-	dev_set_drvdata(dev, sdkp);
-	add_disk(gd);
-	sd_dif_config_host(sdkp);
-
-	sd_printk(KERN_NOTICE, sdkp, "Attached SCSI %sdisk\n",
-		  sdp->removable ? "removable " : "");
+	async_schedule(sd_probe_async, sdkp);
 
 	return 0;
 
diff --git a/drivers/serial/8250.c b/drivers/serial/8250.c
index daa00567bc44..1889a63ebc22 100644
--- a/drivers/serial/8250.c
+++ b/drivers/serial/8250.c
@@ -3123,7 +3123,7 @@ static int __init serial8250_init(void)
 	if (nr_uarts > UART_NR)
 		nr_uarts = UART_NR;
 
-	printk(KERN_INFO "Serial: 8250/16550 driver"
+	printk(KERN_INFO "Serial: 8250/16550 driver, "
 		"%d ports, IRQ sharing %sabled\n", nr_uarts,
 		share_irqs ? "en" : "dis");
 
diff --git a/drivers/serial/crisv10.c b/drivers/serial/crisv10.c
index 8b2c619a09f2..e642c22c80e2 100644
--- a/drivers/serial/crisv10.c
+++ b/drivers/serial/crisv10.c
@@ -1203,7 +1203,7 @@ static void e100_disable_txdma_channel(struct e100_serial *info)
 	unsigned long flags;
 
 	/* Disable output DMA channel for the serial port in question
-	 * ( set to something other then serialX)
+	 * ( set to something other than serialX)
 	 */
 	local_irq_save(flags);
 	DFLOW(DEBUG_LOG(info->line, "disable_txdma_channel %i\n", info->line));
@@ -1266,7 +1266,7 @@ static void e100_disable_rxdma_channel(struct e100_serial *info)
 	unsigned long flags;
 
 	/* Disable input DMA channel for the serial port in question
-	 * ( set to something other then serialX)
+	 * ( set to something other than serialX)
 	 */
 	local_irq_save(flags);
 	if (info->line == 0) {
diff --git a/drivers/serial/serial_core.c b/drivers/serial/serial_core.c
index dc68b7e0c930..42f4e66fccaf 100644
--- a/drivers/serial/serial_core.c
+++ b/drivers/serial/serial_core.c
@@ -2030,7 +2030,7 @@ int uart_suspend_port(struct uart_driver *drv, struct uart_port *port)
 		if (!tries)
 			printk(KERN_ERR "%s%s%s%d: Unable to drain "
 					"transmitter\n",
-			       port->dev ? port->dev->bus_id : "",
+			       port->dev ? dev_name(port->dev) : "",
 			       port->dev ? ": " : "",
 			       drv->dev_name,
 			       drv->tty_driver->name_base + port->line);
@@ -2156,7 +2156,7 @@ uart_report_port(struct uart_driver *drv, struct uart_port *port)
 	}
 
 	printk(KERN_INFO "%s%s%s%d at %s (irq = %d) is a %s\n",
-	       port->dev ? port->dev->bus_id : "",
+	       port->dev ? dev_name(port->dev) : "",
 	       port->dev ? ": " : "",
 	       drv->dev_name,
 	       drv->tty_driver->name_base + port->line,
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index b9d0efb6803f..4a6fe01831a8 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -78,7 +78,7 @@ config SPI_AU1550
 	  will be called au1550_spi.
 
 config SPI_BITBANG
-	tristate "Bitbanging SPI master"
+	tristate "Utilities for Bitbanging SPI masters"
 	help
 	  With a few GPIO pins, your system can bitbang the SPI protocol.
 	  Select this to get SPI support through I/O pins (GPIO, parallel
@@ -100,6 +100,22 @@ config SPI_BUTTERFLY
 	  inexpensive battery powered microcontroller evaluation board.
 	  This same cable can be used to flash new firmware.
 
+config SPI_GPIO
+	tristate "GPIO-based bitbanging SPI Master"
+	depends on GENERIC_GPIO
+	select SPI_BITBANG
+	help
+	  This simple GPIO bitbanging SPI master uses the arch-neutral GPIO
+	  interface to manage MOSI, MISO, SCK, and chipselect signals.  SPI
+	  slaves connected to a bus using this driver are configured as usual,
+	  except that the spi_board_info.controller_data holds the GPIO number
+	  for the chipselect used by this controller driver.
+
+	  Note that this driver often won't achieve even 1 Mbit/sec speeds,
+	  making it unusually slow for SPI.  If your platform can inline
+	  GPIO operations, you should be able to leverage that for better
+	  speed with a custom version of this driver; see the source code.
+
 config SPI_IMX
 	tristate "Freescale iMX SPI controller"
 	depends on ARCH_IMX && EXPERIMENTAL
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index ccf18de34e1e..5e9f521b8844 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -16,6 +16,7 @@ obj-$(CONFIG_SPI_BFIN)			+= spi_bfin5xx.o
 obj-$(CONFIG_SPI_BITBANG)		+= spi_bitbang.o
 obj-$(CONFIG_SPI_AU1550)		+= au1550_spi.o
 obj-$(CONFIG_SPI_BUTTERFLY)		+= spi_butterfly.o
+obj-$(CONFIG_SPI_GPIO)			+= spi_gpio.o
 obj-$(CONFIG_SPI_IMX)			+= spi_imx.o
 obj-$(CONFIG_SPI_LM70_LLP)		+= spi_lm70llp.o
 obj-$(CONFIG_SPI_PXA2XX)		+= pxa2xx_spi.o
diff --git a/drivers/spi/atmel_spi.c b/drivers/spi/atmel_spi.c
index 8abae4ad0fa5..5e39bac9c51b 100644
--- a/drivers/spi/atmel_spi.c
+++ b/drivers/spi/atmel_spi.c
@@ -30,13 +30,6 @@
  * The core SPI transfer engine just talks to a register bank to set up
  * DMA transfers; transfer queue progress is driven by IRQs.  The clock
  * framework provides the base clock, subdivided for each spi_device.
- *
- * Newer controllers, marked with "new_1" flag, have:
- *  - CR.LASTXFER
- *  - SPI_MR.DIV32 may become FDIV or must-be-zero (here: always zero)
- *  - SPI_SR.TXEMPTY, SPI_SR.NSSR (and corresponding irqs)
- *  - SPI_CSRx.CSAAT
- *  - SPI_CSRx.SBCR allows faster clocking
  */
 struct atmel_spi {
 	spinlock_t		lock;
@@ -45,7 +38,6 @@ struct atmel_spi {
 	int			irq;
 	struct clk		*clk;
 	struct platform_device	*pdev;
-	unsigned		new_1:1;
 	struct spi_device	*stay;
 
 	u8			stopping;
@@ -59,10 +51,33 @@ struct atmel_spi {
 	dma_addr_t		buffer_dma;
 };
 
+/* Controller-specific per-slave state */
+struct atmel_spi_device {
+	unsigned int		npcs_pin;
+	u32			csr;
+};
+
 #define BUFFER_SIZE		PAGE_SIZE
 #define INVALID_DMA_ADDRESS	0xffffffff
 
 /*
+ * Version 2 of the SPI controller has
+ *  - CR.LASTXFER
+ *  - SPI_MR.DIV32 may become FDIV or must-be-zero (here: always zero)
+ *  - SPI_SR.TXEMPTY, SPI_SR.NSSR (and corresponding irqs)
+ *  - SPI_CSRx.CSAAT
+ *  - SPI_CSRx.SBCR allows faster clocking
+ *
+ * We can determine the controller version by reading the VERSION
+ * register, but I haven't checked that it exists on all chips, and
+ * this is cheaper anyway.
+ */
+static bool atmel_spi_is_v2(void)
+{
+	return !cpu_is_at91rm9200();
+}
+
+/*
  * Earlier SPI controllers (e.g. on at91rm9200) have a design bug whereby
  * they assume that spi slave device state will not change on deselect, so
  * that automagic deselection is OK.  ("NPCSx rises if no data is to be
@@ -80,39 +95,58 @@ struct atmel_spi {
  * Master on Chip Select 0.")  No workaround exists for that ... so for
  * nCS0 on that chip, we (a) don't use the GPIO, (b) can't support CS_HIGH,
  * and (c) will trigger that first erratum in some cases.
+ *
+ * TODO: Test if the atmel_spi_is_v2() branch below works on
+ * AT91RM9200 if we use some other register than CSR0. However, don't
+ * do this unconditionally since AP7000 has an errata where the BITS
+ * field in CSR0 overrides all other CSRs.
  */
 
 static void cs_activate(struct atmel_spi *as, struct spi_device *spi)
 {
-	unsigned gpio = (unsigned) spi->controller_data;
+	struct atmel_spi_device *asd = spi->controller_state;
 	unsigned active = spi->mode & SPI_CS_HIGH;
 	u32 mr;
-	int i;
-	u32 csr;
-	u32 cpol = (spi->mode & SPI_CPOL) ? SPI_BIT(CPOL) : 0;
-
-	/* Make sure clock polarity is correct */
-	for (i = 0; i < spi->master->num_chipselect; i++) {
-		csr = spi_readl(as, CSR0 + 4 * i);
-		if ((csr ^ cpol) & SPI_BIT(CPOL))
-			spi_writel(as, CSR0 + 4 * i, csr ^ SPI_BIT(CPOL));
-	}
 
-	mr = spi_readl(as, MR);
-	mr = SPI_BFINS(PCS, ~(1 << spi->chip_select), mr);
+	if (atmel_spi_is_v2()) {
+		/*
+		 * Always use CSR0. This ensures that the clock
+		 * switches to the correct idle polarity before we
+		 * toggle the CS.
+		 */
+		spi_writel(as, CSR0, asd->csr);
+		spi_writel(as, MR, SPI_BF(PCS, 0x0e) | SPI_BIT(MODFDIS)
+				| SPI_BIT(MSTR));
+		mr = spi_readl(as, MR);
+		gpio_set_value(asd->npcs_pin, active);
+	} else {
+		u32 cpol = (spi->mode & SPI_CPOL) ? SPI_BIT(CPOL) : 0;
+		int i;
+		u32 csr;
+
+		/* Make sure clock polarity is correct */
+		for (i = 0; i < spi->master->num_chipselect; i++) {
+			csr = spi_readl(as, CSR0 + 4 * i);
+			if ((csr ^ cpol) & SPI_BIT(CPOL))
+				spi_writel(as, CSR0 + 4 * i,
+						csr ^ SPI_BIT(CPOL));
+		}
+
+		mr = spi_readl(as, MR);
+		mr = SPI_BFINS(PCS, ~(1 << spi->chip_select), mr);
+		if (spi->chip_select != 0)
+			gpio_set_value(asd->npcs_pin, active);
+		spi_writel(as, MR, mr);
+	}
 
 	dev_dbg(&spi->dev, "activate %u%s, mr %08x\n",
-			gpio, active ? " (high)" : "",
+			asd->npcs_pin, active ? " (high)" : "",
 			mr);
-
-	if (!(cpu_is_at91rm9200() && spi->chip_select == 0))
-		gpio_set_value(gpio, active);
-	spi_writel(as, MR, mr);
 }
 
 static void cs_deactivate(struct atmel_spi *as, struct spi_device *spi)
 {
-	unsigned gpio = (unsigned) spi->controller_data;
+	struct atmel_spi_device *asd = spi->controller_state;
 	unsigned active = spi->mode & SPI_CS_HIGH;
 	u32 mr;
 
@@ -126,11 +160,11 @@ static void cs_deactivate(struct atmel_spi *as, struct spi_device *spi)
 	}
 
 	dev_dbg(&spi->dev, "DEactivate %u%s, mr %08x\n",
-			gpio, active ? " (low)" : "",
+			asd->npcs_pin, active ? " (low)" : "",
 			mr);
 
-	if (!(cpu_is_at91rm9200() && spi->chip_select == 0))
-		gpio_set_value(gpio, !active);
+	if (atmel_spi_is_v2() || spi->chip_select != 0)
+		gpio_set_value(asd->npcs_pin, !active);
 }
 
 static inline int atmel_spi_xfer_is_last(struct spi_message *msg,
@@ -502,6 +536,7 @@ atmel_spi_interrupt(int irq, void *dev_id)
 static int atmel_spi_setup(struct spi_device *spi)
 {
 	struct atmel_spi	*as;
+	struct atmel_spi_device	*asd;
 	u32			scbr, csr;
 	unsigned int		bits = spi->bits_per_word;
 	unsigned long		bus_hz;
@@ -536,19 +571,16 @@ static int atmel_spi_setup(struct spi_device *spi)
 	}
 
 	/* see notes above re chipselect */
-	if (cpu_is_at91rm9200()
+	if (!atmel_spi_is_v2()
 			&& spi->chip_select == 0
 			&& (spi->mode & SPI_CS_HIGH)) {
 		dev_dbg(&spi->dev, "setup: can't be active-high\n");
 		return -EINVAL;
 	}
 
-	/*
-	 * Pre-new_1 chips start out at half the peripheral
-	 * bus speed.
-	 */
+	/* v1 chips start out at half the peripheral bus speed. */
 	bus_hz = clk_get_rate(as->clk);
-	if (!as->new_1)
+	if (!atmel_spi_is_v2())
 		bus_hz /= 2;
 
 	if (spi->max_speed_hz) {
@@ -589,11 +621,20 @@ static int atmel_spi_setup(struct spi_device *spi)
 
 	/* chipselect must have been muxed as GPIO (e.g. in board setup) */
 	npcs_pin = (unsigned int)spi->controller_data;
-	if (!spi->controller_state) {
+	asd = spi->controller_state;
+	if (!asd) {
+		asd = kzalloc(sizeof(struct atmel_spi_device), GFP_KERNEL);
+		if (!asd)
+			return -ENOMEM;
+
 		ret = gpio_request(npcs_pin, spi->dev.bus_id);
-		if (ret)
+		if (ret) {
+			kfree(asd);
 			return ret;
-		spi->controller_state = (void *)npcs_pin;
+		}
+
+		asd->npcs_pin = npcs_pin;
+		spi->controller_state = asd;
 		gpio_direction_output(npcs_pin, !(spi->mode & SPI_CS_HIGH));
 	} else {
 		unsigned long		flags;
@@ -605,11 +646,14 @@ static int atmel_spi_setup(struct spi_device *spi)
 		spin_unlock_irqrestore(&as->lock, flags);
 	}
 
+	asd->csr = csr;
+
 	dev_dbg(&spi->dev,
 		"setup: %lu Hz bpw %u mode 0x%x -> csr%d %08x\n",
 		bus_hz / scbr, bits, spi->mode, spi->chip_select, csr);
 
-	spi_writel(as, CSR0 + 4 * spi->chip_select, csr);
+	if (!atmel_spi_is_v2())
+		spi_writel(as, CSR0 + 4 * spi->chip_select, csr);
 
 	return 0;
 }
@@ -684,10 +728,11 @@ static int atmel_spi_transfer(struct spi_device *spi, struct spi_message *msg)
 static void atmel_spi_cleanup(struct spi_device *spi)
 {
 	struct atmel_spi	*as = spi_master_get_devdata(spi->master);
+	struct atmel_spi_device	*asd = spi->controller_state;
 	unsigned		gpio = (unsigned) spi->controller_data;
 	unsigned long		flags;
 
-	if (!spi->controller_state)
+	if (!asd)
 		return;
 
 	spin_lock_irqsave(&as->lock, flags);
@@ -697,7 +742,9 @@ static void atmel_spi_cleanup(struct spi_device *spi)
 	}
 	spin_unlock_irqrestore(&as->lock, flags);
 
+	spi->controller_state = NULL;
 	gpio_free(gpio);
+	kfree(asd);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -755,8 +802,6 @@ static int __init atmel_spi_probe(struct platform_device *pdev)
 		goto out_free_buffer;
 	as->irq = irq;
 	as->clk = clk;
-	if (!cpu_is_at91rm9200())
-		as->new_1 = 1;
 
 	ret = request_irq(irq, atmel_spi_interrupt, 0,
 			pdev->dev.bus_id, master);
diff --git a/drivers/spi/pxa2xx_spi.c b/drivers/spi/pxa2xx_spi.c
index 6104f461a3cd..d0fc4ca2f656 100644
--- a/drivers/spi/pxa2xx_spi.c
+++ b/drivers/spi/pxa2xx_spi.c
@@ -1561,11 +1561,12 @@ out_error_master_alloc:
 static int pxa2xx_spi_remove(struct platform_device *pdev)
 {
 	struct driver_data *drv_data = platform_get_drvdata(pdev);
-	struct ssp_device *ssp = drv_data->ssp;
+	struct ssp_device *ssp;
 	int status = 0;
 
 	if (!drv_data)
 		return 0;
+	ssp = drv_data->ssp;
 
 	/* Remove the queue */
 	status = destroy_queue(drv_data);
diff --git a/drivers/spi/spi.c b/drivers/spi/spi.c
index 3734dc9708e1..643908b74bc0 100644
--- a/drivers/spi/spi.c
+++ b/drivers/spi/spi.c
@@ -47,7 +47,7 @@ modalias_show(struct device *dev, struct device_attribute *a, char *buf)
 {
 	const struct spi_device	*spi = to_spi_device(dev);
 
-	return snprintf(buf, BUS_ID_SIZE + 1, "%s\n", spi->modalias);
+	return sprintf(buf, "%s\n", spi->modalias);
 }
 
 static struct device_attribute spi_dev_attrs[] = {
@@ -63,7 +63,7 @@ static int spi_match_device(struct device *dev, struct device_driver *drv)
 {
 	const struct spi_device	*spi = to_spi_device(dev);
 
-	return strncmp(spi->modalias, drv->name, BUS_ID_SIZE) == 0;
+	return strcmp(spi->modalias, drv->name) == 0;
 }
 
 static int spi_uevent(struct device *dev, struct kobj_uevent_env *env)
@@ -243,8 +243,7 @@ int spi_add_device(struct spi_device *spi)
 	}
 
 	/* Set the bus ID string */
-	snprintf(spi->dev.bus_id, sizeof spi->dev.bus_id,
-			"%s.%u", spi->master->dev.bus_id,
+	dev_set_name(&spi->dev, "%s.%u", dev_name(&spi->master->dev),
 			spi->chip_select);
 
 
@@ -254,7 +253,7 @@ int spi_add_device(struct spi_device *spi)
 	 */
 	mutex_lock(&spi_add_lock);
 
-	if (bus_find_device_by_name(&spi_bus_type, NULL, spi->dev.bus_id)
+	if (bus_find_device_by_name(&spi_bus_type, NULL, dev_name(&spi->dev))
 			!= NULL) {
 		dev_err(dev, "chipselect %d already in use\n",
 				spi->chip_select);
@@ -269,7 +268,7 @@ int spi_add_device(struct spi_device *spi)
 	status = spi->master->setup(spi);
 	if (status < 0) {
 		dev_err(dev, "can't %s %s, status %d\n",
-				"setup", spi->dev.bus_id, status);
+				"setup", dev_name(&spi->dev), status);
 		goto done;
 	}
 
@@ -277,9 +276,9 @@ int spi_add_device(struct spi_device *spi)
 	status = device_add(&spi->dev);
 	if (status < 0)
 		dev_err(dev, "can't %s %s, status %d\n",
-				"add", spi->dev.bus_id, status);
+				"add", dev_name(&spi->dev), status);
 	else
-		dev_dbg(dev, "registered child %s\n", spi->dev.bus_id);
+		dev_dbg(dev, "registered child %s\n", dev_name(&spi->dev));
 
 done:
 	mutex_unlock(&spi_add_lock);
@@ -504,12 +503,11 @@ int spi_register_master(struct spi_master *master)
 	/* register the device, then userspace will see it.
 	 * registration fails if the bus ID is in use.
 	 */
-	snprintf(master->dev.bus_id, sizeof master->dev.bus_id,
-		"spi%u", master->bus_num);
+	dev_set_name(&master->dev, "spi%u", master->bus_num);
 	status = device_add(&master->dev);
 	if (status < 0)
 		goto done;
-	dev_dbg(dev, "registered master %s%s\n", master->dev.bus_id,
+	dev_dbg(dev, "registered master %s%s\n", dev_name(&master->dev),
 			dynamic ? " (dynamic)" : "");
 
 	/* populate children from any spi device tables */
diff --git a/drivers/spi/spi_bitbang.c b/drivers/spi/spi_bitbang.c
index 96cc39ecb6e2..85e61f451218 100644
--- a/drivers/spi/spi_bitbang.c
+++ b/drivers/spi/spi_bitbang.c
@@ -475,7 +475,7 @@ int spi_bitbang_start(struct spi_bitbang *bitbang)
 	/* this task is the only thing to touch the SPI bits */
 	bitbang->busy = 0;
 	bitbang->workqueue = create_singlethread_workqueue(
-			bitbang->master->dev.parent->bus_id);
+			dev_name(bitbang->master->dev.parent));
 	if (bitbang->workqueue == NULL) {
 		status = -EBUSY;
 		goto err1;
diff --git a/drivers/spi/spi_butterfly.c b/drivers/spi/spi_butterfly.c
index 0ee2b2090252..c2184866fa9c 100644
--- a/drivers/spi/spi_butterfly.c
+++ b/drivers/spi/spi_butterfly.c
@@ -287,7 +287,7 @@ static void butterfly_attach(struct parport *p)
 	pp->dataflash = spi_new_device(pp->bitbang.master, &pp->info[0]);
 	if (pp->dataflash)
 		pr_debug("%s: dataflash at %s\n", p->name,
-				pp->dataflash->dev.bus_id);
+				dev_name(&pp->dataflash->dev));
 
 	// dev_info(_what?_, ...)
 	pr_info("%s: AVR Butterfly\n", p->name);
diff --git a/drivers/spi/spi_gpio.c b/drivers/spi/spi_gpio.c
new file mode 100644
index 000000000000..49698cabc30d
--- /dev/null
+++ b/drivers/spi/spi_gpio.c
@@ -0,0 +1,360 @@
+/*
+ * spi_gpio.c - SPI master driver using generic bitbanged GPIO
+ *
+ * Copyright (C) 2006,2008 David Brownell
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/gpio.h>
+
+#include <linux/spi/spi.h>
+#include <linux/spi/spi_bitbang.h>
+#include <linux/spi/spi_gpio.h>
+
+
+/*
+ * This bitbanging SPI master driver should help make systems usable
+ * when a native hardware SPI engine is not available, perhaps because
+ * its driver isn't yet working or because the I/O pins it requires
+ * are used for other purposes.
+ *
+ * platform_device->driver_data ... points to spi_gpio
+ *
+ * spi->controller_state ... reserved for bitbang framework code
+ * spi->controller_data ... holds chipselect GPIO
+ *
+ * spi->master->dev.driver_data ... points to spi_gpio->bitbang
+ */
+
+struct spi_gpio {
+	struct spi_bitbang		bitbang;
+	struct spi_gpio_platform_data	pdata;
+	struct platform_device		*pdev;
+};
+
+/*----------------------------------------------------------------------*/
+
+/*
+ * Because the overhead of going through four GPIO procedure calls
+ * per transferred bit can make performance a problem, this code
+ * is set up so that you can use it in either of two ways:
+ *
+ *   - The slow generic way:  set up platform_data to hold the GPIO
+ *     numbers used for MISO/MOSI/SCK, and issue procedure calls for
+ *     each of them.  This driver can handle several such busses.
+ *
+ *   - The quicker inlined way:  only helps with platform GPIO code
+ *     that inlines operations for constant GPIOs.  This can give
+ *     you tight (fast!) inner loops, but each such bus needs a
+ *     new driver.  You'll define a new C file, with Makefile and
+ *     Kconfig support; the C code can be a total of six lines:
+ *
+ *		#define DRIVER_NAME	"myboard_spi2"
+ *		#define	SPI_MISO_GPIO	119
+ *		#define	SPI_MOSI_GPIO	120
+ *		#define	SPI_SCK_GPIO	121
+ *		#define	SPI_N_CHIPSEL	4
+ *		#include "spi_gpio.c"
+ */
+
+#ifndef DRIVER_NAME
+#define DRIVER_NAME	"spi_gpio"
+
+#define GENERIC_BITBANG	/* vs tight inlines */
+
+/* all functions referencing these symbols must define pdata */
+#define SPI_MISO_GPIO	((pdata)->miso)
+#define SPI_MOSI_GPIO	((pdata)->mosi)
+#define SPI_SCK_GPIO	((pdata)->sck)
+
+#define SPI_N_CHIPSEL	((pdata)->num_chipselect)
+
+#endif
+
+/*----------------------------------------------------------------------*/
+
+static inline const struct spi_gpio_platform_data * __pure
+spi_to_pdata(const struct spi_device *spi)
+{
+	const struct spi_bitbang	*bang;
+	const struct spi_gpio		*spi_gpio;
+
+	bang = spi_master_get_devdata(spi->master);
+	spi_gpio = container_of(bang, struct spi_gpio, bitbang);
+	return &spi_gpio->pdata;
+}
+
+/* this is #defined to avoid unused-variable warnings when inlining */
+#define pdata		spi_to_pdata(spi)
+
+static inline void setsck(const struct spi_device *spi, int is_on)
+{
+	gpio_set_value(SPI_SCK_GPIO, is_on);
+}
+
+static inline void setmosi(const struct spi_device *spi, int is_on)
+{
+	gpio_set_value(SPI_MOSI_GPIO, is_on);
+}
+
+static inline int getmiso(const struct spi_device *spi)
+{
+	return gpio_get_value(SPI_MISO_GPIO);
+}
+
+#undef pdata
+
+/*
+ * NOTE:  this clocks "as fast as we can".  It "should" be a function of the
+ * requested device clock.  Software overhead means we usually have trouble
+ * reaching even one Mbit/sec (except when we can inline bitops), so for now
+ * we'll just assume we never need additional per-bit slowdowns.
+ */
+#define spidelay(nsecs)	do {} while (0)
+
+#define	EXPAND_BITBANG_TXRX
+#include <linux/spi/spi_bitbang.h>
+
+/*
+ * These functions can leverage inline expansion of GPIO calls to shrink
+ * costs for a txrx bit, often by factors of around ten (by instruction
+ * count).  That is particularly visible for larger word sizes, but helps
+ * even with default 8-bit words.
+ *
+ * REVISIT overheads calling these functions for each word also have
+ * significant performance costs.  Having txrx_bufs() calls that inline
+ * the txrx_word() logic would help performance, e.g. on larger blocks
+ * used with flash storage or MMC/SD.  There should also be ways to make
+ * GCC be less stupid about reloading registers inside the I/O loops,
+ * even without inlined GPIO calls; __attribute__((hot)) on GCC 4.3?
+ */
+
+static u32 spi_gpio_txrx_word_mode0(struct spi_device *spi,
+		unsigned nsecs, u32 word, u8 bits)
+{
+	return bitbang_txrx_be_cpha0(spi, nsecs, 0, word, bits);
+}
+
+static u32 spi_gpio_txrx_word_mode1(struct spi_device *spi,
+		unsigned nsecs, u32 word, u8 bits)
+{
+	return bitbang_txrx_be_cpha1(spi, nsecs, 0, word, bits);
+}
+
+static u32 spi_gpio_txrx_word_mode2(struct spi_device *spi,
+		unsigned nsecs, u32 word, u8 bits)
+{
+	return bitbang_txrx_be_cpha0(spi, nsecs, 1, word, bits);
+}
+
+static u32 spi_gpio_txrx_word_mode3(struct spi_device *spi,
+		unsigned nsecs, u32 word, u8 bits)
+{
+	return bitbang_txrx_be_cpha1(spi, nsecs, 1, word, bits);
+}
+
+/*----------------------------------------------------------------------*/
+
+static void spi_gpio_chipselect(struct spi_device *spi, int is_active)
+{
+	unsigned long cs = (unsigned long) spi->controller_data;
+
+	/* set initial clock polarity */
+	if (is_active)
+		setsck(spi, spi->mode & SPI_CPOL);
+
+	/* SPI is normally active-low */
+	gpio_set_value(cs, (spi->mode & SPI_CS_HIGH) ? is_active : !is_active);
+}
+
+static int spi_gpio_setup(struct spi_device *spi)
+{
+	unsigned long	cs = (unsigned long) spi->controller_data;
+	int		status = 0;
+
+	if (spi->bits_per_word > 32)
+		return -EINVAL;
+
+	if (!spi->controller_state) {
+		status = gpio_request(cs, spi->dev.bus_id);
+		if (status)
+			return status;
+		status = gpio_direction_output(cs, spi->mode & SPI_CS_HIGH);
+	}
+	if (!status)
+		status = spi_bitbang_setup(spi);
+	if (status) {
+		if (!spi->controller_state)
+			gpio_free(cs);
+	}
+	return status;
+}
+
+static void spi_gpio_cleanup(struct spi_device *spi)
+{
+	unsigned long	cs = (unsigned long) spi->controller_data;
+
+	gpio_free(cs);
+	spi_bitbang_cleanup(spi);
+}
+
+static int __init spi_gpio_alloc(unsigned pin, const char *label, bool is_in)
+{
+	int value;
+
+	value = gpio_request(pin, label);
+	if (value == 0) {
+		if (is_in)
+			value = gpio_direction_input(pin);
+		else
+			value = gpio_direction_output(pin, 0);
+	}
+	return value;
+}
+
+static int __init
+spi_gpio_request(struct spi_gpio_platform_data *pdata, const char *label)
+{
+	int value;
+
+	/* NOTE:  SPI_*_GPIO symbols may reference "pdata" */
+
+	value = spi_gpio_alloc(SPI_MOSI_GPIO, label, false);
+	if (value)
+		goto done;
+
+	value = spi_gpio_alloc(SPI_MISO_GPIO, label, true);
+	if (value)
+		goto free_mosi;
+
+	value = spi_gpio_alloc(SPI_SCK_GPIO, label, false);
+	if (value)
+		goto free_miso;
+
+	goto done;
+
+free_miso:
+	gpio_free(SPI_MISO_GPIO);
+free_mosi:
+	gpio_free(SPI_MOSI_GPIO);
+done:
+	return value;
+}
+
+static int __init spi_gpio_probe(struct platform_device *pdev)
+{
+	int				status;
+	struct spi_master		*master;
+	struct spi_gpio			*spi_gpio;
+	struct spi_gpio_platform_data	*pdata;
+
+	pdata = pdev->dev.platform_data;
+#ifdef GENERIC_BITBANG
+	if (!pdata || !pdata->num_chipselect)
+		return -ENODEV;
+#endif
+
+	status = spi_gpio_request(pdata, dev_name(&pdev->dev));
+	if (status < 0)
+		return status;
+
+	master = spi_alloc_master(&pdev->dev, sizeof *spi_gpio);
+	if (!master) {
+		status = -ENOMEM;
+		goto gpio_free;
+	}
+	spi_gpio = spi_master_get_devdata(master);
+	platform_set_drvdata(pdev, spi_gpio);
+
+	spi_gpio->pdev = pdev;
+	if (pdata)
+		spi_gpio->pdata = *pdata;
+
+	master->bus_num = pdev->id;
+	master->num_chipselect = SPI_N_CHIPSEL;
+	master->setup = spi_gpio_setup;
+	master->cleanup = spi_gpio_cleanup;
+
+	spi_gpio->bitbang.master = spi_master_get(master);
+	spi_gpio->bitbang.chipselect = spi_gpio_chipselect;
+	spi_gpio->bitbang.txrx_word[SPI_MODE_0] = spi_gpio_txrx_word_mode0;
+	spi_gpio->bitbang.txrx_word[SPI_MODE_1] = spi_gpio_txrx_word_mode1;
+	spi_gpio->bitbang.txrx_word[SPI_MODE_2] = spi_gpio_txrx_word_mode2;
+	spi_gpio->bitbang.txrx_word[SPI_MODE_3] = spi_gpio_txrx_word_mode3;
+	spi_gpio->bitbang.setup_transfer = spi_bitbang_setup_transfer;
+	spi_gpio->bitbang.flags = SPI_CS_HIGH;
+
+	status = spi_bitbang_start(&spi_gpio->bitbang);
+	if (status < 0) {
+		spi_master_put(spi_gpio->bitbang.master);
+gpio_free:
+		gpio_free(SPI_MISO_GPIO);
+		gpio_free(SPI_MOSI_GPIO);
+		gpio_free(SPI_SCK_GPIO);
+		spi_master_put(master);
+	}
+
+	return status;
+}
+
+static int __exit spi_gpio_remove(struct platform_device *pdev)
+{
+	struct spi_gpio			*spi_gpio;
+	struct spi_gpio_platform_data	*pdata;
+	int				status;
+
+	spi_gpio = platform_get_drvdata(pdev);
+	pdata = pdev->dev.platform_data;
+
+	/* stop() unregisters child devices too */
+	status = spi_bitbang_stop(&spi_gpio->bitbang);
+	spi_master_put(spi_gpio->bitbang.master);
+
+	platform_set_drvdata(pdev, NULL);
+
+	gpio_free(SPI_MISO_GPIO);
+	gpio_free(SPI_MOSI_GPIO);
+	gpio_free(SPI_SCK_GPIO);
+
+	return status;
+}
+
+MODULE_ALIAS("platform:" DRIVER_NAME);
+
+static struct platform_driver spi_gpio_driver = {
+	.driver.name	= DRIVER_NAME,
+	.driver.owner	= THIS_MODULE,
+	.remove		= __exit_p(spi_gpio_remove),
+};
+
+static int __init spi_gpio_init(void)
+{
+	return platform_driver_probe(&spi_gpio_driver, spi_gpio_probe);
+}
+module_init(spi_gpio_init);
+
+static void __exit spi_gpio_exit(void)
+{
+	platform_driver_unregister(&spi_gpio_driver);
+}
+module_exit(spi_gpio_exit);
+
+
+MODULE_DESCRIPTION("SPI master driver using generic bitbanged GPIO ");
+MODULE_AUTHOR("David Brownell");
+MODULE_LICENSE("GPL");
diff --git a/drivers/spi/spi_lm70llp.c b/drivers/spi/spi_lm70llp.c
index 39d8d8ad65c0..568c781ad91c 100644
--- a/drivers/spi/spi_lm70llp.c
+++ b/drivers/spi/spi_lm70llp.c
@@ -1,5 +1,5 @@
 /*
- * spi_lm70llp.c - driver for lm70llp eval board for the LM70 sensor
+ * spi_lm70llp.c - driver for LM70EVAL-LLP board for the LM70 sensor
  *
  * Copyright (C) 2006 Kaiwan N Billimoria <kaiwan@designergraphix.com>
  *
@@ -40,8 +40,12 @@
  * master controller driver.  The hwmon/lm70 driver is a "SPI protocol
  * driver", layered on top of this one and usable without the lm70llp.
  *
+ * Datasheet and Schematic:
  * The LM70 is a temperature sensor chip from National Semiconductor; its
  * datasheet is available at http://www.national.com/pf/LM/LM70.html
+ * The schematic for this particular board (the LM70EVAL-LLP) is
+ * available (on page 4) here:
+ *  http://www.national.com/appinfo/tempsensors/files/LM70LLPEVALmanual.pdf
  *
  * Also see Documentation/spi/spi-lm70llp.  The SPI<->parport code here is
  * (heavily) based on spi-butterfly by David Brownell.
@@ -64,7 +68,7 @@
  *
  * Note that parport pin 13 actually gets inverted by the transistor
  * arrangement which lets either the parport or the LM70 drive the
- * SI/SO signal.
+ * SI/SO signal (see the schematic for details).
  */
 
 #define DRVNAME		"spi-lm70llp"
@@ -106,12 +110,16 @@ static inline struct spi_lm70llp *spidev_to_pp(struct spi_device *spi)
 static inline void deassertCS(struct spi_lm70llp *pp)
 {
 	u8 data = parport_read_data(pp->port);
+
+	data &= ~0x80;		/* pull D7/SI-out low while de-asserted */
 	parport_write_data(pp->port, data | nCS);
 }
 
 static inline void assertCS(struct spi_lm70llp *pp)
 {
 	u8 data = parport_read_data(pp->port);
+
+	data |= 0x80;		/* pull D7/SI-out high so lm70 drives SO-in */
 	parport_write_data(pp->port, data & ~nCS);
 }
 
@@ -184,22 +192,7 @@ static void lm70_chipselect(struct spi_device *spi, int value)
  */
 static u32 lm70_txrx(struct spi_device *spi, unsigned nsecs, u32 word, u8 bits)
 {
-	static u32 sio=0;
-	static int first_time=1;
-
-	/* First time: perform SPI bitbang and return the LSB of
-	 * the result of the SPI call.
-	 */
-	if (first_time) {
-		sio = bitbang_txrx_be_cpha0(spi, nsecs, 0, word, bits);
-		first_time=0;
-		return (sio & 0x00ff);
-	}
-	/* Return the MSB of the result of the SPI call */
-	else {
-		first_time=1;
-		return (sio >> 8);
-	}
+	return bitbang_txrx_be_cpha0(spi, nsecs, 0, word, bits);
 }
 
 static void spi_lm70llp_attach(struct parport *p)
@@ -287,16 +280,15 @@ static void spi_lm70llp_attach(struct parport *p)
 	pp->spidev_lm70 = spi_new_device(pp->bitbang.master, &pp->info);
 	if (pp->spidev_lm70)
 		dev_dbg(&pp->spidev_lm70->dev, "spidev_lm70 at %s\n",
-				pp->spidev_lm70->dev.bus_id);
+				dev_name(&pp->spidev_lm70->dev));
 	else {
 		printk(KERN_WARNING "%s: spi_new_device failed\n", DRVNAME);
 		status = -ENODEV;
 		goto out_bitbang_stop;
 	}
-	pp->spidev_lm70->bits_per_word = 16;
+	pp->spidev_lm70->bits_per_word = 8;
 
 	lm70llp = pp;
-
 	return;
 
 out_bitbang_stop:
@@ -326,7 +318,6 @@ static void spi_lm70llp_detach(struct parport *p)
 
 	/* power down */
 	parport_write_data(pp->port, 0);
-	msleep(10);
 
 	parport_release(pp->pd);
 	parport_unregister_device(pp->pd);
diff --git a/drivers/spi/spi_s3c24xx.c b/drivers/spi/spi_s3c24xx.c
index 256d18395a23..b3ebc1d0f85f 100644
--- a/drivers/spi/spi_s3c24xx.c
+++ b/drivers/spi/spi_s3c24xx.c
@@ -19,6 +19,7 @@
 #include <linux/err.h>
 #include <linux/clk.h>
 #include <linux/platform_device.h>
+#include <linux/gpio.h>
 
 #include <linux/spi/spi.h>
 #include <linux/spi/spi_bitbang.h>
@@ -27,7 +28,6 @@
 #include <asm/dma.h>
 #include <mach/hardware.h>
 
-#include <mach/regs-gpio.h>
 #include <plat/regs-spi.h>
 #include <mach/spi.h>
 
@@ -66,7 +66,7 @@ static inline struct s3c24xx_spi *to_hw(struct spi_device *sdev)
 
 static void s3c24xx_spi_gpiocs(struct s3c2410_spi_info *spi, int cs, int pol)
 {
-	s3c2410_gpio_setpin(spi->pin_cs, pol);
+	gpio_set_value(spi->pin_cs, pol);
 }
 
 static void s3c24xx_spi_chipsel(struct spi_device *spi, int value)
@@ -248,8 +248,13 @@ static void s3c24xx_spi_initialsetup(struct s3c24xx_spi *hw)
 	writeb(SPPIN_DEFAULT, hw->regs + S3C2410_SPPIN);
 	writeb(SPCON_DEFAULT, hw->regs + S3C2410_SPCON);
 
-	if (hw->pdata && hw->pdata->gpio_setup)
-		hw->pdata->gpio_setup(hw->pdata, 1);
+	if (hw->pdata) {
+		if (hw->set_cs == s3c24xx_spi_gpiocs)
+			gpio_direction_output(hw->pdata->pin_cs, 1);
+
+		if (hw->pdata->gpio_setup)
+			hw->pdata->gpio_setup(hw->pdata, 1);
+	}
 }
 
 static int __init s3c24xx_spi_probe(struct platform_device *pdev)
@@ -343,18 +348,27 @@ static int __init s3c24xx_spi_probe(struct platform_device *pdev)
 		goto err_no_clk;
 	}
 
-	s3c24xx_spi_initialsetup(hw);
-
 	/* setup any gpio we can */
 
 	if (!pdata->set_cs) {
-		hw->set_cs = s3c24xx_spi_gpiocs;
+		if (pdata->pin_cs < 0) {
+			dev_err(&pdev->dev, "No chipselect pin\n");
+			goto err_register;
+		}
 
-		s3c2410_gpio_setpin(pdata->pin_cs, 1);
-		s3c2410_gpio_cfgpin(pdata->pin_cs, S3C2410_GPIO_OUTPUT);
+		err = gpio_request(pdata->pin_cs, dev_name(&pdev->dev));
+		if (err) {
+			dev_err(&pdev->dev, "Failed to get gpio for cs\n");
+			goto err_register;
+		}
+
+		hw->set_cs = s3c24xx_spi_gpiocs;
+		gpio_direction_output(pdata->pin_cs, 1);
 	} else
 		hw->set_cs = pdata->set_cs;
 
+	s3c24xx_spi_initialsetup(hw);
+
 	/* register our spi controller */
 
 	err = spi_bitbang_start(&hw->bitbang);
@@ -366,6 +380,9 @@ static int __init s3c24xx_spi_probe(struct platform_device *pdev)
 	return 0;
 
  err_register:
+	if (hw->set_cs == s3c24xx_spi_gpiocs)
+		gpio_free(pdata->pin_cs);
+
 	clk_disable(hw->clk);
 	clk_put(hw->clk);
 
@@ -401,6 +418,9 @@ static int __exit s3c24xx_spi_remove(struct platform_device *dev)
 	free_irq(hw->irq, hw);
 	iounmap(hw->regs);
 
+	if (hw->set_cs == s3c24xx_spi_gpiocs)
+		gpio_free(hw->pdata->pin_cs);
+
 	release_resource(hw->ioarea);
 	kfree(hw->ioarea);
 
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 5d457c96bd7e..ce6badded47a 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -49,6 +49,8 @@ source "drivers/staging/sxg/Kconfig"
 
 source "drivers/staging/me4000/Kconfig"
 
+source "drivers/staging/meilhaus/Kconfig"
+
 source "drivers/staging/go7007/Kconfig"
 
 source "drivers/staging/usbip/Kconfig"
@@ -63,5 +65,35 @@ source "drivers/staging/at76_usb/Kconfig"
 
 source "drivers/staging/poch/Kconfig"
 
+source "drivers/staging/agnx/Kconfig"
+
+source "drivers/staging/otus/Kconfig"
+
+source "drivers/staging/rt2860/Kconfig"
+
+source "drivers/staging/rt2870/Kconfig"
+
+source "drivers/staging/benet/Kconfig"
+
+source "drivers/staging/comedi/Kconfig"
+
+source "drivers/staging/asus_oled/Kconfig"
+
+source "drivers/staging/panel/Kconfig"
+
+source "drivers/staging/altpciechdma/Kconfig"
+
+source "drivers/staging/rtl8187se/Kconfig"
+
+source "drivers/staging/rspiusb/Kconfig"
+
+source "drivers/staging/mimio/Kconfig"
+
+source "drivers/staging/frontier/Kconfig"
+
+source "drivers/staging/epl/Kconfig"
+
+source "drivers/staging/android/Kconfig"
+
 endif # !STAGING_EXCLUDE_BUILD
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index 71c4d53760b8..9ddcc2bb3365 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -7,6 +7,7 @@ obj-$(CONFIG_ET131X)		+= et131x/
 obj-$(CONFIG_SLICOSS)		+= slicoss/
 obj-$(CONFIG_SXG)		+= sxg/
 obj-$(CONFIG_ME4000)		+= me4000/
+obj-$(CONFIG_MEILHAUS)		+= meilhaus/
 obj-$(CONFIG_VIDEO_GO7007)	+= go7007/
 obj-$(CONFIG_USB_IP_COMMON)	+= usbip/
 obj-$(CONFIG_W35UND)		+= winbond/
@@ -14,3 +15,18 @@ obj-$(CONFIG_PRISM2_USB)	+= wlan-ng/
 obj-$(CONFIG_ECHO)		+= echo/
 obj-$(CONFIG_USB_ATMEL)		+= at76_usb/
 obj-$(CONFIG_POCH)		+= poch/
+obj-$(CONFIG_AGNX)		+= agnx/
+obj-$(CONFIG_OTUS)		+= otus/
+obj-$(CONFIG_RT2860)		+= rt2860/
+obj-$(CONFIG_RT2870)		+= rt2870/
+obj-$(CONFIG_BENET)		+= benet/
+obj-$(CONFIG_COMEDI)		+= comedi/
+obj-$(CONFIG_ASUS_OLED)		+= asus_oled/
+obj-$(CONFIG_PANEL)		+= panel/
+obj-$(CONFIG_ALTERA_PCIE_CHDMA)	+= altpciechdma/
+obj-$(CONFIG_RTL8187SE)		+= rtl8187se/
+obj-$(CONFIG_USB_RSPI)		+= rspiusb/
+obj-$(CONFIG_INPUT_MIMIO)	+= mimio/
+obj-$(CONFIG_TRANZPORT)		+= frontier/
+obj-$(CONFIG_EPL)		+= epl/
+obj-$(CONFIG_ANDROID)		+= android/
diff --git a/drivers/staging/agnx/Kconfig b/drivers/staging/agnx/Kconfig
new file mode 100644
index 000000000000..7f43549e36dd
--- /dev/null
+++ b/drivers/staging/agnx/Kconfig
@@ -0,0 +1,5 @@
+config AGNX
+	tristate "Wireless Airgo AGNX support"
+	depends on WLAN_80211 && MAC80211
+	---help---
+	  This is an experimental driver for Airgo AGNX00 wireless chip.
diff --git a/drivers/staging/agnx/Makefile b/drivers/staging/agnx/Makefile
new file mode 100644
index 000000000000..1216564a312d
--- /dev/null
+++ b/drivers/staging/agnx/Makefile
@@ -0,0 +1,8 @@
+obj-$(CONFIG_AGNX)	+= agnx.o
+
+agnx-objs :=	rf.o	\
+		pci.o	\
+		xmit.o	\
+		table.o	\
+		sta.o	\
+		phy.o
diff --git a/drivers/staging/agnx/TODO b/drivers/staging/agnx/TODO
new file mode 100644
index 000000000000..89bec74318aa
--- /dev/null
+++ b/drivers/staging/agnx/TODO
@@ -0,0 +1,22 @@
+2008 7/18
+
+The RX has can't receive OFDM packet correctly,
+Guess it need be do RX calibrate.
+
+
+before 2008 3/1
+
+1: The RX get too much "CRC failed" pakets, it make the card work very unstable,
+2: After running a while, the card will get infinity "RX Frame" and "Error"
+interrupt, not know the root reason so far, try to fix it
+3: Using two tx queue txd and txm but not only txm.
+4: Set the hdr correctly.
+5: Try to do recalibrate correvtly
+6: To support G mode in future
+7: Fix the mac address can't be readed and set correctly in BE machine.
+8: Fix include and exclude FCS in promisous mode and manage mode
+9: Using sta_notify to notice sta change
+10: Turn on frame reception at the end of start
+11: Guess the card support HW_MULTICAST_FILTER
+12: The tx process should be implment atomic?
+13: Using mac80211 function to control the TX&RX LED.
diff --git a/drivers/staging/agnx/agnx.h b/drivers/staging/agnx/agnx.h
new file mode 100644
index 000000000000..a75b0db3726c
--- /dev/null
+++ b/drivers/staging/agnx/agnx.h
@@ -0,0 +1,154 @@
+#ifndef AGNX_H_
+#define AGNX_H_
+
+#include "xmit.h"
+
+#define PFX				KBUILD_MODNAME ": "
+
+static inline u32 agnx_read32(void __iomem *mem_region, u32 offset)
+{
+	return ioread32(mem_region + offset);
+}
+
+static inline void agnx_write32(void __iomem *mem_region, u32 offset, u32 val)
+{
+	iowrite32(val, mem_region + offset);
+}
+
+/* static const struct ieee80211_rate agnx_rates_80211b[] = { */
+/* 	{ .rate = 10, */
+/* 	  .val = 0xa, */
+/* 	  .flags = IEEE80211_RATE_CCK }, */
+/* 	{ .rate = 20, */
+/* 	  .val = 0x14, */
+/* 	  .hw_value = -0x14, */
+/* 	  .flags = IEEE80211_RATE_CCK_2 }, */
+/* 	{ .rate = 55, */
+/* 	  .val = 0x37, */
+/* 	  .val2 = -0x37, */
+/* 	  .flags = IEEE80211_RATE_CCK_2 }, */
+/* 	{ .rate = 110, */
+/* 	  .val = 0x6e, */
+/* 	  .val2 = -0x6e, */
+/* 	  .flags = IEEE80211_RATE_CCK_2 } */
+/* }; */
+
+
+static const struct ieee80211_rate agnx_rates_80211g[] = {
+/* 	{ .bitrate = 10, .hw_value = 1, .flags = IEEE80211_RATE_SHORT_PREAMBLE }, */
+/* 	{ .bitrate = 20, .hw_value = 2, .flags = IEEE80211_RATE_SHORT_PREAMBLE }, */
+/* 	{ .bitrate = 55, .hw_value = 3, .flags = IEEE80211_RATE_SHORT_PREAMBLE }, */
+/* 	{ .bitrate = 110, .hw_value = 4, .flags = IEEE80211_RATE_SHORT_PREAMBLE }, */
+ 	{ .bitrate = 10, .hw_value = 1, },
+ 	{ .bitrate = 20, .hw_value = 2, },
+ 	{ .bitrate = 55, .hw_value = 3, },
+ 	{ .bitrate = 110, .hw_value = 4,},
+
+	{ .bitrate = 60, .hw_value = 0xB, },
+	{ .bitrate = 90, .hw_value = 0xF, },
+	{ .bitrate = 120, .hw_value = 0xA },
+	{ .bitrate = 180, .hw_value = 0xE, },
+//	{ .bitrate = 240, .hw_value = 0xd, },
+	{ .bitrate = 360, .hw_value = 0xD, },
+	{ .bitrate = 480, .hw_value = 0x8, },
+	{ .bitrate = 540, .hw_value = 0xC, },
+};
+
+static const struct ieee80211_channel agnx_channels[] = {
+	{ .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, },
+};
+
+#define NUM_DRIVE_MODES	2
+/* Agnx operate mode */
+enum {
+	AGNX_MODE_80211A,
+	AGNX_MODE_80211A_OOB,
+	AGNX_MODE_80211A_MIMO,
+	AGNX_MODE_80211B_SHORT,
+	AGNX_MODE_80211B_LONG,
+	AGNX_MODE_80211G,
+	AGNX_MODE_80211G_OOB,
+	AGNX_MODE_80211G_MIMO,
+};
+
+enum {
+	AGNX_UNINIT,
+	AGNX_START,
+	AGNX_STOP,
+};
+
+struct agnx_priv {
+	struct pci_dev *pdev;
+	struct ieee80211_hw *hw;
+
+	spinlock_t lock;
+	struct mutex mutex;
+	unsigned int init_status;
+
+	void __iomem *ctl;	/* pointer to base ram address */
+	void __iomem *data;	/* pointer to mem region #2 */
+
+	struct agnx_ring rx;
+	struct agnx_ring txm;
+	struct agnx_ring txd;
+
+	/* Need volatile? */
+	u32 irq_status;
+
+        struct delayed_work periodic_work; /* Periodic tasks like recalibrate*/
+	struct ieee80211_low_level_stats stats;
+
+//        unsigned int phymode;
+	int mode;
+	int channel;
+	u8 bssid[ETH_ALEN];
+
+	u8 mac_addr[ETH_ALEN];
+	u8 revid;
+
+	struct ieee80211_supported_band band;
+};
+
+
+#define AGNX_CHAINS_MAX	6
+#define AGNX_PERIODIC_DELAY 60000 /* unit: ms */
+#define LOCAL_STAID	0	/* the station entry for the card itself */
+#define BSSID_STAID	1	/* the station entry for the bsssid AP */
+#define	spi_delay()	udelay(40)
+#define eeprom_delay()	udelay(40)
+#define	routing_table_delay()	udelay(50)
+
+/* PDU pool MEM region #2 */
+#define AGNX_PDUPOOL		0x40000	/* PDU pool */
+#define AGNX_PDUPOOL_SIZE	0x8000	/* PDU pool size*/
+#define AGNX_PDU_TX_WQ		0x41000	/* PDU list TX workqueue */
+#define AGNX_PDU_FREE		0x41800	/* Free Pool */
+#define PDU_SIZE		0x80	/* Free Pool node size */
+#define PDU_FREE_CNT		0xd0 /* Free pool node count */
+
+
+/* RF stuffs */
+extern void rf_chips_init(struct agnx_priv *priv);
+extern void spi_rc_write(void __iomem *mem_region, u32 chip_ids, u32 sw);
+extern void calibrate_oscillator(struct agnx_priv *priv);
+extern void do_calibration(struct agnx_priv *priv);
+extern void antenna_calibrate(struct agnx_priv *priv);
+extern void __antenna_calibrate(struct agnx_priv *priv);
+extern void print_offsets(struct agnx_priv *priv);
+extern int agnx_set_channel(struct agnx_priv *priv, unsigned int channel);
+
+
+#endif /* AGNX_H_ */
diff --git a/drivers/staging/agnx/debug.h b/drivers/staging/agnx/debug.h
new file mode 100644
index 000000000000..e3e25dda0ba0
--- /dev/null
+++ b/drivers/staging/agnx/debug.h
@@ -0,0 +1,418 @@
+#ifndef AGNX_DEBUG_H_
+#define AGNX_DEBUG_H_
+
+#include "agnx.h"
+#include "phy.h"
+#include "sta.h"
+#include "xmit.h"
+
+#define AGNX_TRACE              printk(KERN_ERR PFX "function:%s line:%d\n", __func__, __LINE__)
+
+#define PRINTK_LE16(prefix, var)	printk(KERN_DEBUG PFX #prefix ": " #var " 0x%.4x\n", le16_to_cpu(var))
+#define PRINTK_LE32(prefix, var)	printk(KERN_DEBUG PFX #prefix ": " #var " 0x%.8x\n", le32_to_cpu(var))
+#define PRINTK_U8(prefix, var) 		printk(KERN_DEBUG PFX #prefix ": " #var " 0x%.2x\n", var)
+#define PRINTK_BE16(prefix, var)	printk(KERN_DEBUG PFX #prefix ": " #var " 0x%.4x\n", be16_to_cpu(var))
+#define PRINTK_BE32(prefix, var)	printk(KERN_DEBUG PFX #prefix ": " #var " 0x%.8x\n", be32_to_cpu(var))
+#define PRINTK_BITS(prefix, field)    	printk(KERN_DEBUG PFX #prefix ": " #field ": 0x%x\n", (reg & field) >> field##_SHIFT)
+
+static inline void agnx_bug(char *reason)
+{
+	printk(KERN_ERR PFX "%s\n", reason);
+	BUG();
+}
+
+static inline void agnx_print_desc(struct agnx_desc *desc)
+{
+        u32 reg = be32_to_cpu(desc->frag);
+
+	PRINTK_BITS(DESC, PACKET_LEN);
+
+	if (reg & FIRST_FRAG) {
+		PRINTK_BITS(DESC, FIRST_PACKET_MASK);
+		PRINTK_BITS(DESC, FIRST_RESERV2);
+		PRINTK_BITS(DESC, FIRST_TKIP_ERROR);
+		PRINTK_BITS(DESC, FIRST_TKIP_PACKET);
+		PRINTK_BITS(DESC, FIRST_RESERV1);
+		PRINTK_BITS(DESC, FIRST_FRAG_LEN);
+	} else {
+		PRINTK_BITS(DESC, SUB_RESERV2);
+		PRINTK_BITS(DESC, SUB_TKIP_ERROR);
+		PRINTK_BITS(DESC, SUB_TKIP_PACKET);
+		PRINTK_BITS(DESC, SUB_RESERV1);
+		PRINTK_BITS(DESC, SUB_FRAG_LEN);
+	}
+
+	PRINTK_BITS(DESC, FIRST_FRAG);
+	PRINTK_BITS(DESC, LAST_FRAG);
+	PRINTK_BITS(DESC, OWNER);
+}
+
+
+static inline void dump_ieee80211b_phy_hdr(__be32 _11b0, __be32 _11b1)
+{
+
+}
+
+static inline void agnx_print_hdr(struct agnx_hdr *hdr)
+{
+	u32 reg;
+	int i;
+
+	reg = be32_to_cpu(hdr->reg0);
+	PRINTK_BITS(HDR, RTS);
+	PRINTK_BITS(HDR, MULTICAST);
+	PRINTK_BITS(HDR, ACK);
+	PRINTK_BITS(HDR, TM);
+	PRINTK_BITS(HDR, RELAY);
+	PRINTK_BITS(HDR, REVISED_FCS);
+	PRINTK_BITS(HDR, NEXT_BUFFER_ADDR);
+
+	reg = be32_to_cpu(hdr->reg1);
+	PRINTK_BITS(HDR, MAC_HDR_LEN);
+	PRINTK_BITS(HDR, DURATION_OVERIDE);
+	PRINTK_BITS(HDR, PHY_HDR_OVERIDE);
+	PRINTK_BITS(HDR, CRC_FAIL);
+	PRINTK_BITS(HDR, SEQUENCE_NUMBER);
+	PRINTK_BITS(HDR, BUFF_HEAD_ADDR);
+
+	reg = be32_to_cpu(hdr->reg2);
+	PRINTK_BITS(HDR, PDU_COUNT);
+	PRINTK_BITS(HDR, WEP_KEY);
+	PRINTK_BITS(HDR, USES_WEP_KEY);
+	PRINTK_BITS(HDR, KEEP_ALIVE);
+	PRINTK_BITS(HDR, BUFF_TAIL_ADDR);
+
+	reg = be32_to_cpu(hdr->reg3);
+	PRINTK_BITS(HDR, CTS_11G);
+	PRINTK_BITS(HDR, RTS_11G);
+	PRINTK_BITS(HDR, FRAG_SIZE);
+	PRINTK_BITS(HDR, PAYLOAD_LEN);
+	PRINTK_BITS(HDR, FRAG_NUM);
+
+	reg = be32_to_cpu(hdr->reg4);
+	PRINTK_BITS(HDR, RELAY_STAID);
+	PRINTK_BITS(HDR, STATION_ID);
+	PRINTK_BITS(HDR, WORKQUEUE_ID);
+
+	reg = be32_to_cpu(hdr->reg5);
+	/* printf the route flag */
+	PRINTK_BITS(HDR, ROUTE_HOST);
+	PRINTK_BITS(HDR, ROUTE_CARD_CPU);
+	PRINTK_BITS(HDR, ROUTE_ENCRYPTION);
+	PRINTK_BITS(HDR, ROUTE_TX);
+	PRINTK_BITS(HDR, ROUTE_RX1);
+	PRINTK_BITS(HDR, ROUTE_RX2);
+	PRINTK_BITS(HDR, ROUTE_COMPRESSION);
+
+	PRINTK_BE32(HDR, hdr->_11g0);
+	PRINTK_BE32(HDR, hdr->_11g1);
+	PRINTK_BE32(HDR, hdr->_11b0);
+	PRINTK_BE32(HDR, hdr->_11b1);
+
+	dump_ieee80211b_phy_hdr(hdr->_11b0, hdr->_11b1);
+
+	/* Fixme */
+	for (i = 0; i < ARRAY_SIZE(hdr->mac_hdr); i++) {
+		if (i == 0)
+			printk(KERN_DEBUG PFX "IEEE80211 HDR: ");
+		printk("%.2x ", hdr->mac_hdr[i]);
+		if (i + 1 == ARRAY_SIZE(hdr->mac_hdr))
+			printk("\n");
+	}
+
+	PRINTK_BE16(HDR, hdr->rts_duration);
+	PRINTK_BE16(HDR, hdr->last_duration);
+	PRINTK_BE16(HDR, hdr->sec_last_duration);
+	PRINTK_BE16(HDR, hdr->other_duration);
+	PRINTK_BE16(HDR, hdr->tx_other_duration);
+	PRINTK_BE16(HDR, hdr->last_11g_len);
+	PRINTK_BE16(HDR, hdr->other_11g_len);
+	PRINTK_BE16(HDR, hdr->last_11b_len);
+	PRINTK_BE16(HDR, hdr->other_11b_len);
+
+	/* FIXME */
+	reg = be16_to_cpu(hdr->reg6);
+	PRINTK_BITS(HDR, MBF);
+	PRINTK_BITS(HDR, RSVD4);
+
+	PRINTK_BE16(HDR, hdr->rx_frag_stat);
+
+	PRINTK_BE32(HDR, hdr->time_stamp);
+	PRINTK_BE32(HDR, hdr->phy_stats_hi);
+	PRINTK_BE32(HDR, hdr->phy_stats_lo);
+	PRINTK_BE32(HDR, hdr->mic_key0);
+	PRINTK_BE32(HDR, hdr->mic_key1);
+} /* agnx_print_hdr */
+
+
+static inline void agnx_print_rx_hdr(struct agnx_hdr *hdr)
+{
+	agnx_print_hdr(hdr);
+
+	PRINTK_BE16(HDR, hdr->rx.rx_packet_duration);
+	PRINTK_BE16(HDR, hdr->rx.replay_cnt);
+
+	PRINTK_U8(HDR, hdr->rx_channel);
+}
+
+static inline void agnx_print_tx_hdr(struct agnx_hdr *hdr)
+{
+	agnx_print_hdr(hdr);
+
+	PRINTK_U8(HDR, hdr->tx.long_retry_limit);
+	PRINTK_U8(HDR, hdr->tx.short_retry_limit);
+	PRINTK_U8(HDR, hdr->tx.long_retry_cnt);
+	PRINTK_U8(HDR, hdr->tx.short_retry_cnt);
+
+	PRINTK_U8(HDR, hdr->rx_channel);
+}
+
+static inline void
+agnx_print_sta_power(struct agnx_priv *priv, unsigned int sta_idx)
+{
+	struct agnx_sta_power power;
+	u32 reg;
+
+	get_sta_power(priv, &power, sta_idx);
+
+	reg = le32_to_cpu(power.reg);
+	PRINTK_BITS(STA_POWER, SIGNAL);
+	PRINTK_BITS(STA_POWER, RATE);
+	PRINTK_BITS(STA_POWER, TIFS);
+	PRINTK_BITS(STA_POWER, EDCF);
+	PRINTK_BITS(STA_POWER, CHANNEL_BOND);
+	PRINTK_BITS(STA_POWER, PHY_MODE);
+	PRINTK_BITS(STA_POWER, POWER_LEVEL);
+	PRINTK_BITS(STA_POWER, NUM_TRANSMITTERS);
+}
+
+static inline void
+agnx_print_sta_tx_wq(struct agnx_priv *priv, unsigned int sta_idx, unsigned int wq_idx)
+{
+	struct agnx_sta_tx_wq tx_wq;
+	u32 reg;
+
+	get_sta_tx_wq(priv, &tx_wq, sta_idx, wq_idx);
+
+	reg = le32_to_cpu(tx_wq.reg0);
+	PRINTK_BITS(STA_TX_WQ, TAIL_POINTER);
+	PRINTK_BITS(STA_TX_WQ, HEAD_POINTER_LOW);
+
+	reg = le32_to_cpu(tx_wq.reg3);
+	PRINTK_BITS(STA_TX_WQ, HEAD_POINTER_HIGH);
+	PRINTK_BITS(STA_TX_WQ, ACK_POINTER_LOW);
+
+	reg = le32_to_cpu(tx_wq.reg1);
+	PRINTK_BITS(STA_TX_WQ, ACK_POINTER_HIGH);
+	PRINTK_BITS(STA_TX_WQ, HEAD_TIMOUT_TAIL_PACK_CNT);
+	PRINTK_BITS(STA_TX_WQ, ACK_TIMOUT_TAIL_PACK_CNT);
+
+	reg = le32_to_cpu(tx_wq.reg2);
+	PRINTK_BITS(STA_TX_WQ, HEAD_TIMOUT_WIN_LIM_BYTE_CNT);
+	PRINTK_BITS(STA_TX_WQ, HEAD_TIMOUT_WIN_LIM_FRAG_CNT);
+	PRINTK_BITS(STA_TX_WQ, WORK_QUEUE_ACK_TYPE);
+	PRINTK_BITS(STA_TX_WQ, WORK_QUEUE_VALID);
+}
+
+static inline void agnx_print_sta_traffic(struct agnx_sta_traffic *traffic)
+{
+	u32 reg;
+
+	reg = le32_to_cpu(traffic->reg0);
+	PRINTK_BITS(STA_TRAFFIC, ACK_TIMOUT_CNT);
+	PRINTK_BITS(STA_TRAFFIC, TRAFFIC_ACK_TYPE);
+	PRINTK_BITS(STA_TRAFFIC, NEW_PACKET);
+	PRINTK_BITS(STA_TRAFFIC, TRAFFIC_VALID);
+	PRINTK_BITS(STA_TRAFFIC, RX_HDR_DESC_POINTER);
+
+	reg = le32_to_cpu(traffic->reg1);
+	PRINTK_BITS(STA_TRAFFIC, RX_PACKET_TIMESTAMP);
+	PRINTK_BITS(STA_TRAFFIC, TRAFFIC_RESERVED);
+	PRINTK_BITS(STA_TRAFFIC, SV);
+	PRINTK_BITS(STA_TRAFFIC, RX_SEQUENCE_NUM);
+
+	PRINTK_LE32(STA_TRAFFIC, traffic->tx_replay_cnt_low);
+
+	PRINTK_LE16(STA_TRAFFIC, traffic->tx_replay_cnt_high);
+	PRINTK_LE16(STA_TRAFFIC, traffic->rx_replay_cnt_high);
+
+	PRINTK_LE32(STA_TRAFFIC, traffic->rx_replay_cnt_low);
+}
+
+static inline void agnx_print_sta(struct agnx_priv *priv, unsigned int sta_idx)
+{
+	struct agnx_sta station;
+	struct agnx_sta *sta = &station;
+	u32 reg;
+	unsigned int i;
+
+	get_sta(priv, sta, sta_idx);
+
+	for (i = 0; i < 4; i++)
+		PRINTK_LE32(STA, sta->tx_session_keys[i]);
+	for (i = 0; i < 4; i++)
+		PRINTK_LE32(STA, sta->rx_session_keys[i]);
+
+	reg = le32_to_cpu(sta->reg);
+	PRINTK_BITS(STA, ID_1);
+	PRINTK_BITS(STA, ID_0);
+	PRINTK_BITS(STA, ENABLE_CONCATENATION);
+	PRINTK_BITS(STA, ENABLE_DECOMPRESSION);
+	PRINTK_BITS(STA, STA_RESERVED);
+	PRINTK_BITS(STA, EAP);
+	PRINTK_BITS(STA, ED_NULL);
+	PRINTK_BITS(STA, ENCRYPTION_POLICY);
+	PRINTK_BITS(STA, DEFINED_KEY_ID);
+	PRINTK_BITS(STA, FIXED_KEY);
+	PRINTK_BITS(STA, KEY_VALID);
+	PRINTK_BITS(STA, STATION_VALID);
+
+	PRINTK_LE32(STA, sta->tx_aes_blks_unicast);
+	PRINTK_LE32(STA, sta->rx_aes_blks_unicast);
+
+	PRINTK_LE16(STA, sta->aes_format_err_unicast_cnt);
+	PRINTK_LE16(STA, sta->aes_replay_unicast);
+
+	PRINTK_LE16(STA, sta->aes_decrypt_err_unicast);
+	PRINTK_LE16(STA, sta->aes_decrypt_err_default);
+
+	PRINTK_LE16(STA, sta->single_retry_packets);
+	PRINTK_LE16(STA, sta->failed_tx_packets);
+
+	PRINTK_LE16(STA, sta->muti_retry_packets);
+	PRINTK_LE16(STA, sta->ack_timeouts);
+
+	PRINTK_LE16(STA, sta->frag_tx_cnt);
+	PRINTK_LE16(STA, sta->rts_brq_sent);
+
+	PRINTK_LE16(STA, sta->tx_packets);
+	PRINTK_LE16(STA, sta->cts_back_timeout);
+
+	PRINTK_LE32(STA, sta->phy_stats_high);
+	PRINTK_LE32(STA, sta->phy_stats_low);
+
+//	for (i = 0; i < 8; i++)
+	agnx_print_sta_traffic(sta->traffic + 0);
+
+	PRINTK_LE16(STA, sta->traffic_class0_frag_success);
+	PRINTK_LE16(STA, sta->traffic_class1_frag_success);
+	PRINTK_LE16(STA, sta->traffic_class2_frag_success);
+	PRINTK_LE16(STA, sta->traffic_class3_frag_success);
+	PRINTK_LE16(STA, sta->traffic_class4_frag_success);
+	PRINTK_LE16(STA, sta->traffic_class5_frag_success);
+	PRINTK_LE16(STA, sta->traffic_class6_frag_success);
+	PRINTK_LE16(STA, sta->traffic_class7_frag_success);
+
+	PRINTK_LE16(STA, sta->num_frag_non_prime_rates);
+	PRINTK_LE16(STA, sta->ack_timeout_non_prime_rates);
+}
+
+
+static inline void dump_ieee80211_hdr(struct ieee80211_hdr *hdr, char *tag)
+{
+	u16 fctl;
+        int hdrlen;
+	DECLARE_MAC_BUF(mac);
+
+        fctl = le16_to_cpu(hdr->frame_control);
+	switch (fctl & IEEE80211_FCTL_FTYPE) {
+	case IEEE80211_FTYPE_DATA:
+		printk(PFX "%s DATA ", tag);
+		break;
+	case IEEE80211_FTYPE_CTL:
+		printk(PFX "%s CTL ", tag);
+		break;
+	case IEEE80211_FTYPE_MGMT:
+		printk(PFX "%s MGMT ", tag);
+		switch(fctl & IEEE80211_FCTL_STYPE) {
+		case IEEE80211_STYPE_ASSOC_REQ:
+			printk("SubType: ASSOC_REQ ");
+			break;
+		case IEEE80211_STYPE_ASSOC_RESP:
+			printk("SubType: ASSOC_RESP ");
+			break;
+		case IEEE80211_STYPE_REASSOC_REQ:
+			printk("SubType: REASSOC_REQ ");
+			break;
+		case IEEE80211_STYPE_REASSOC_RESP:
+			printk("SubType: REASSOC_RESP ");
+			break;
+		case IEEE80211_STYPE_PROBE_REQ:
+			printk("SubType: PROBE_REQ ");
+			break;
+		case IEEE80211_STYPE_PROBE_RESP:
+			printk("SubType: PROBE_RESP ");
+			break;
+		case IEEE80211_STYPE_BEACON:
+			printk("SubType: BEACON ");
+			break;
+		case IEEE80211_STYPE_ATIM:
+			printk("SubType: ATIM ");
+			break;
+		case IEEE80211_STYPE_DISASSOC:
+			printk("SubType: DISASSOC ");
+			break;
+		case IEEE80211_STYPE_AUTH:
+			printk("SubType: AUTH ");
+			break;
+		case IEEE80211_STYPE_DEAUTH:
+			printk("SubType: DEAUTH ");
+			break;
+		case IEEE80211_STYPE_ACTION:
+			printk("SubType: ACTION ");
+			break;
+		default:
+			printk("SubType: Unknow\n");
+		}
+		break;
+	default:
+		printk(PFX "%s Packet type: Unknow\n", tag);
+	}
+
+        hdrlen = ieee80211_hdrlen(fctl);
+
+	if (hdrlen >= 4)
+		printk("FC=0x%04x DUR=0x%04x",
+		       fctl, le16_to_cpu(hdr->duration_id));
+	if (hdrlen >= 10)
+		printk(" A1=%s", print_mac(mac, hdr->addr1));
+	if (hdrlen >= 16)
+		printk(" A2=%s", print_mac(mac, hdr->addr2));
+	if (hdrlen >= 24)
+		printk(" A3=%s", print_mac(mac, hdr->addr3));
+	if (hdrlen >= 30)
+		printk(" A4=%s", print_mac(mac, hdr->addr4));
+	printk("\n");
+}
+
+static inline void dump_txm_registers(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	int i;
+	for (i = 0; i <=0x1e8; i += 4) {
+		printk(KERN_DEBUG PFX "TXM: %x---> 0x%.8x\n", i, ioread32(ctl + i));
+	}
+}
+static inline void dump_rxm_registers(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	int i;
+	for (i = 0; i <=0x108; i += 4)
+		printk(KERN_DEBUG PFX "RXM: %x---> 0x%.8x\n", i, ioread32(ctl + 0x2000 + i));
+}
+static inline void dump_bm_registers(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	int i;
+	for (i = 0; i <=0x90; i += 4)
+		printk(KERN_DEBUG PFX "BM: %x---> 0x%.8x\n", i, ioread32(ctl + 0x2c00 + i));
+}
+static inline void dump_cir_registers(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	int i;
+	for (i = 0; i <=0xb8; i += 4)
+		printk(KERN_DEBUG PFX "CIR: %x---> 0x%.8x\n", i, ioread32(ctl + 0x3000 + i));
+}
+
+#endif /* AGNX_DEBUG_H_ */
diff --git a/drivers/staging/agnx/pci.c b/drivers/staging/agnx/pci.c
new file mode 100644
index 000000000000..854630cb527e
--- /dev/null
+++ b/drivers/staging/agnx/pci.c
@@ -0,0 +1,644 @@
+/**
+ * Airgo MIMO wireless driver
+ *
+ * Copyright (c) 2007 Li YanBo <dreamfly281@gmail.com>
+
+ * Thanks for Jeff Williams <angelbane@gmail.com> do reverse engineer
+ * works and published the SPECS at http://airgo.wdwconsulting.net/mymoin
+
+ * 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/etherdevice.h>
+#include <linux/pci.h>
+#include <linux/delay.h>
+
+#include "agnx.h"
+#include "debug.h"
+#include "xmit.h"
+#include "phy.h"
+
+MODULE_AUTHOR("Li YanBo <dreamfly281@gmail.com>");
+MODULE_DESCRIPTION("Airgo MIMO PCI wireless driver");
+MODULE_LICENSE("GPL");
+
+static struct pci_device_id agnx_pci_id_tbl[] __devinitdata = {
+	{ PCI_DEVICE(0x17cb, 0x0001) },	/* Beklin F5d8010, Netgear WGM511 etc */
+	{ PCI_DEVICE(0x17cb, 0x0002) },	/* Netgear Wpnt511 */
+	{ 0 }
+};
+
+MODULE_DEVICE_TABLE(pci, agnx_pci_id_tbl);
+
+
+static inline void agnx_interrupt_ack(struct agnx_priv *priv, u32 *reason)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	if ( *reason & AGNX_STAT_RX ) {
+		/* Mark complete RX */
+		reg = ioread32(ctl + AGNX_CIR_RXCTL);
+		reg |= 0x4;
+		iowrite32(reg, ctl + AGNX_CIR_RXCTL);
+		/* disable Rx interrupt */
+	}
+	if ( *reason & AGNX_STAT_TX ) {
+		reg = ioread32(ctl + AGNX_CIR_TXDCTL);
+		if (reg & 0x4) {
+			iowrite32(reg, ctl + AGNX_CIR_TXDCTL);
+			*reason |= AGNX_STAT_TXD;
+		}
+ 		reg = ioread32(ctl + AGNX_CIR_TXMCTL);
+		if (reg & 0x4) {
+			iowrite32(reg, ctl + AGNX_CIR_TXMCTL);
+			*reason |= AGNX_STAT_TXM;
+		}
+	}
+	if ( *reason & AGNX_STAT_X ) {
+/* 		reg = ioread32(ctl + AGNX_INT_STAT); */
+/* 		iowrite32(reg, ctl + AGNX_INT_STAT); */
+/* 		/\* FIXME reinit interrupt mask *\/ */
+/* 		reg = 0xc390bf9 & ~IRQ_TX_BEACON; */
+/* 		reg &= ~IRQ_TX_DISABLE; */
+/* 		iowrite32(reg, ctl + AGNX_INT_MASK); */
+/* 		iowrite32(0x800, ctl + AGNX_CIR_BLKCTL); */
+	}
+} /* agnx_interrupt_ack */
+
+static irqreturn_t agnx_interrupt_handler(int irq, void *dev_id)
+{
+	struct ieee80211_hw *dev = dev_id;
+	struct agnx_priv *priv = dev->priv;
+	void __iomem *ctl = priv->ctl;
+	irqreturn_t ret = IRQ_NONE;
+	u32 irq_reason;
+
+	spin_lock(&priv->lock);
+
+//	printk(KERN_ERR PFX "Get a interrupt %s\n", __func__);
+
+	if (priv->init_status != AGNX_START)
+		goto out;
+
+	/* FiXME  Here has no lock, Is this will lead to race? */
+	irq_reason = ioread32(ctl + AGNX_CIR_BLKCTL);
+	if (!(irq_reason & 0x7))
+		goto out;
+
+	ret = IRQ_HANDLED;
+	priv->irq_status = ioread32(ctl + AGNX_INT_STAT);
+
+//	printk(PFX "Interrupt reason is 0x%x\n", irq_reason);
+	/* Make sure the txm and txd flags don't conflict with other unknown
+	   interrupt flag, maybe is not necessary */
+	irq_reason &= 0xF;
+
+	disable_rx_interrupt(priv);
+	/* TODO Make sure the card finished initialized */
+	agnx_interrupt_ack(priv, &irq_reason);
+
+	if ( irq_reason & AGNX_STAT_RX )
+		handle_rx_irq(priv);
+	if ( irq_reason & AGNX_STAT_TXD )
+		handle_txd_irq(priv);
+	if ( irq_reason & AGNX_STAT_TXM )
+		handle_txm_irq(priv);
+	if ( irq_reason & AGNX_STAT_X )
+		handle_other_irq(priv);
+
+	enable_rx_interrupt(priv);
+out:
+	spin_unlock(&priv->lock);
+	return ret;
+} /* agnx_interrupt_handler */
+
+
+/* FIXME */
+static int agnx_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
+{
+	AGNX_TRACE;
+	return _agnx_tx(dev->priv, skb);
+} /* agnx_tx */
+
+
+static int agnx_get_mac_address(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	/* Attention! directly read the MAC or other date from EEPROM will
+	 lead to cardbus(WGM511) lock up when write to PM PLL register */
+	reg = agnx_read32(ctl, 0x3544);
+	udelay(40);
+	reg = agnx_read32(ctl, 0x354c);
+	udelay(50);
+	/* Get the mac address */
+	reg = agnx_read32(ctl, 0x3544);
+	udelay(40);
+
+	/* HACK */
+	reg = cpu_to_le32(reg);
+	priv->mac_addr[0] = ((u8 *)&reg)[2];
+	priv->mac_addr[1] = ((u8 *)&reg)[3];
+	reg = agnx_read32(ctl, 0x3548);
+	udelay(50);
+	*((u32 *)(priv->mac_addr + 2)) = cpu_to_le32(reg);
+
+	if (!is_valid_ether_addr(priv->mac_addr)) {
+		DECLARE_MAC_BUF(mbuf);
+		printk(KERN_WARNING PFX "read mac %s\n", print_mac(mbuf, priv->mac_addr));
+		printk(KERN_WARNING PFX "Invalid hwaddr! Using random hwaddr\n");
+		random_ether_addr(priv->mac_addr);
+	}
+
+	return 0;
+} /* agnx_get_mac_address */
+
+static int agnx_alloc_rings(struct agnx_priv *priv)
+{
+	unsigned int len;
+	AGNX_TRACE;
+
+	/* Allocate RX/TXM/TXD rings info */
+	priv->rx.size = AGNX_RX_RING_SIZE;
+	priv->txm.size = AGNX_TXM_RING_SIZE;
+	priv->txd.size = AGNX_TXD_RING_SIZE;
+
+	len = priv->rx.size + priv->txm.size + priv->txd.size;
+
+//	priv->rx.info = kzalloc(sizeof(struct agnx_info) * len, GFP_KERNEL);
+	priv->rx.info = kzalloc(sizeof(struct agnx_info) * len, GFP_ATOMIC);
+	if (!priv->rx.info)
+		return -ENOMEM;
+	priv->txm.info = priv->rx.info + priv->rx.size;
+	priv->txd.info = priv->txm.info + priv->txm.size;
+
+	/* Allocate RX/TXM/TXD descriptors */
+	priv->rx.desc = pci_alloc_consistent(priv->pdev, sizeof(struct agnx_desc) * len,
+					     &priv->rx.dma);
+	if (!priv->rx.desc) {
+		kfree(priv->rx.info);
+		return -ENOMEM;
+	}
+
+	priv->txm.desc = priv->rx.desc + priv->rx.size;
+	priv->txm.dma = priv->rx.dma + sizeof(struct agnx_desc) * priv->rx.size;
+	priv->txd.desc = priv->txm.desc + priv->txm.size;
+	priv->txd.dma = priv->txm.dma + sizeof(struct agnx_desc) * priv->txm.size;
+
+	return 0;
+} /* agnx_alloc_rings */
+
+static void rings_free(struct agnx_priv *priv)
+{
+	unsigned int len = priv->rx.size + priv->txm.size + priv->txd.size;
+	unsigned long flags;
+	AGNX_TRACE;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	kfree(priv->rx.info);
+	pci_free_consistent(priv->pdev, sizeof(struct agnx_desc) * len,
+			    priv->rx.desc, priv->rx.dma);
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+#if 0
+static void agnx_periodic_work_handler(struct work_struct *work)
+{
+	struct agnx_priv *priv = container_of(work, struct agnx_priv,
+                                             periodic_work.work);
+//	unsigned long flags;
+	unsigned long delay;
+
+	/* fixme: using mutex?? */
+//	spin_lock_irqsave(&priv->lock, flags);
+
+	/* TODO Recalibrate*/
+//	calibrate_oscillator(priv);
+//	antenna_calibrate(priv);
+//	agnx_send_packet(priv, 997);
+	/* FIXME */
+/* 	if (debug == 3) */
+/*                 delay = msecs_to_jiffies(AGNX_PERIODIC_DELAY); */
+/* 	else */
+	delay = msecs_to_jiffies(AGNX_PERIODIC_DELAY);
+//		delay = round_jiffies(HZ * 15);
+
+	queue_delayed_work(priv->hw->workqueue, &priv->periodic_work, delay);
+
+//	spin_unlock_irqrestore(&priv->lock, flags);
+}
+#endif
+
+static int agnx_start(struct ieee80211_hw *dev)
+{
+	struct agnx_priv *priv = dev->priv;
+	/* unsigned long delay; */
+	int err = 0;
+	AGNX_TRACE;
+
+	err = agnx_alloc_rings(priv);
+	if (err) {
+		printk(KERN_ERR PFX "Can't alloc RX/TXM/TXD rings\n");
+		goto out;
+	}
+	err = request_irq(priv->pdev->irq, &agnx_interrupt_handler,
+			  IRQF_SHARED, "agnx_pci", dev);
+	if (err) {
+		printk(KERN_ERR PFX "Failed to register IRQ handler\n");
+		rings_free(priv);
+		goto out;
+	}
+
+//	mdelay(500);
+
+	might_sleep();
+	agnx_hw_init(priv);
+
+//	mdelay(500);
+	might_sleep();
+
+	priv->init_status = AGNX_START;
+/*         INIT_DELAYED_WORK(&priv->periodic_work, agnx_periodic_work_handler); */
+/* 	delay = msecs_to_jiffies(AGNX_PERIODIC_DELAY); */
+/*         queue_delayed_work(priv->hw->workqueue, &priv->periodic_work, delay); */
+out:
+	return err;
+} /* agnx_start */
+
+static void agnx_stop(struct ieee80211_hw *dev)
+{
+	struct agnx_priv *priv = dev->priv;
+	AGNX_TRACE;
+
+	priv->init_status = AGNX_STOP;
+	/* make sure hardware will not generate irq */
+	agnx_hw_reset(priv);
+	free_irq(priv->pdev->irq, dev);
+        flush_workqueue(priv->hw->workqueue);
+//	cancel_delayed_work_sync(&priv->periodic_work);
+	unfill_rings(priv);
+	rings_free(priv);
+}
+
+static int agnx_config(struct ieee80211_hw *dev,
+		       struct ieee80211_conf *conf)
+{
+	struct agnx_priv *priv = dev->priv;
+	int channel = ieee80211_frequency_to_channel(conf->channel->center_freq);
+	AGNX_TRACE;
+
+	spin_lock(&priv->lock);
+	/* FIXME need priv lock? */
+	if (channel != priv->channel) {
+		priv->channel = channel;
+		agnx_set_channel(priv, priv->channel);
+	}
+
+	spin_unlock(&priv->lock);
+	return 0;
+}
+
+static int agnx_config_interface(struct ieee80211_hw *dev,
+				 struct ieee80211_vif *vif,
+				 struct ieee80211_if_conf *conf)
+{
+	struct agnx_priv *priv = dev->priv;
+	void __iomem *ctl = priv->ctl;
+	AGNX_TRACE;
+
+	spin_lock(&priv->lock);
+
+	if (memcmp(conf->bssid, priv->bssid, ETH_ALEN)) {
+//		u32 reghi, reglo;
+		agnx_set_bssid(priv, conf->bssid);
+		memcpy(priv->bssid, conf->bssid, ETH_ALEN);
+		hash_write(priv, conf->bssid, BSSID_STAID);
+		sta_init(priv, BSSID_STAID);
+		/* FIXME needed? */
+		sta_power_init(priv, BSSID_STAID);
+		agnx_write32(ctl, AGNX_BM_MTSM, 0xff & ~0x1);
+	}
+	spin_unlock(&priv->lock);
+	return 0;
+} /* agnx_config_interface */
+
+
+static void agnx_configure_filter(struct ieee80211_hw *dev,
+				  unsigned int changed_flags,
+				  unsigned int *total_flags,
+				  int mc_count, struct dev_mc_list *mclist)
+{
+	unsigned int new_flags = 0;
+
+	*total_flags = new_flags;
+	/* TODO */
+}
+
+static int agnx_add_interface(struct ieee80211_hw *dev,
+			      struct ieee80211_if_init_conf *conf)
+{
+	struct agnx_priv *priv = dev->priv;
+	AGNX_TRACE;
+
+	spin_lock(&priv->lock);
+	/* FIXME */
+	if (priv->mode != NL80211_IFTYPE_MONITOR)
+		return -EOPNOTSUPP;
+
+	switch (conf->type) {
+	case NL80211_IFTYPE_STATION:
+		priv->mode = conf->type;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	spin_unlock(&priv->lock);
+
+	return 0;
+}
+
+static void agnx_remove_interface(struct ieee80211_hw *dev,
+				  struct ieee80211_if_init_conf *conf)
+{
+	struct agnx_priv *priv = dev->priv;
+	AGNX_TRACE;
+
+	/* TODO */
+	priv->mode = NL80211_IFTYPE_MONITOR;
+}
+
+static int agnx_get_stats(struct ieee80211_hw *dev,
+			  struct ieee80211_low_level_stats *stats)
+{
+	struct agnx_priv *priv = dev->priv;
+	AGNX_TRACE;
+	spin_lock(&priv->lock);
+	/* TODO !! */
+	memcpy(stats, &priv->stats, sizeof(*stats));
+	spin_unlock(&priv->lock);
+
+	return 0;
+}
+
+static u64 agnx_get_tsft(struct ieee80211_hw *dev)
+{
+	void __iomem *ctl = ((struct agnx_priv *)dev->priv)->ctl;
+	u32 tsftl;
+	u64 tsft;
+	AGNX_TRACE;
+
+	/* FIXME */
+	tsftl = ioread32(ctl + AGNX_TXM_TIMESTAMPLO);
+	tsft = ioread32(ctl + AGNX_TXM_TIMESTAMPHI);
+	tsft <<= 32;
+	tsft |= tsftl;
+
+	return tsft;
+}
+
+static int agnx_get_tx_stats(struct ieee80211_hw *dev,
+			     struct ieee80211_tx_queue_stats *stats)
+{
+	struct agnx_priv *priv = dev->priv;
+	AGNX_TRACE;
+
+	/* FIXME now we just using txd queue, but should using txm queue too */
+	stats[0].len = (priv->txd.idx - priv->txd.idx_sent) / 2;
+	stats[0].limit = priv->txd.size - 2;
+	stats[0].count = priv->txd.idx / 2;
+
+	return 0;
+}
+
+static struct ieee80211_ops agnx_ops = {
+	.tx			= agnx_tx,
+	.start			= agnx_start,
+	.stop			= agnx_stop,
+	.add_interface		= agnx_add_interface,
+	.remove_interface	= agnx_remove_interface,
+	.config			= agnx_config,
+	.config_interface	= agnx_config_interface,
+ 	.configure_filter	= agnx_configure_filter,
+	.get_stats		= agnx_get_stats,
+	.get_tx_stats		= agnx_get_tx_stats,
+	.get_tsf		= agnx_get_tsft
+};
+
+static void __devexit agnx_pci_remove(struct pci_dev *pdev)
+{
+	struct ieee80211_hw *dev = pci_get_drvdata(pdev);
+	struct agnx_priv *priv = dev->priv;
+	AGNX_TRACE;
+
+	if (!dev)
+		return;
+	ieee80211_unregister_hw(dev);
+	pci_iounmap(pdev, priv->ctl);
+	pci_iounmap(pdev, priv->data);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+
+	ieee80211_free_hw(dev);
+}
+
+static int __devinit agnx_pci_probe(struct pci_dev *pdev,
+				    const struct pci_device_id *id)
+{
+	struct ieee80211_hw *dev;
+	struct agnx_priv *priv;
+	u32 mem_addr0, mem_len0;
+	u32 mem_addr1, mem_len1;
+	int err;
+	DECLARE_MAC_BUF(mac);
+
+	err = pci_enable_device(pdev);
+	if (err) {
+		printk(KERN_ERR PFX "Can't enable new PCI device\n");
+		return err;
+	}
+
+	/* get pci resource */
+	mem_addr0 = pci_resource_start(pdev, 0);
+	mem_len0 = pci_resource_len(pdev, 0);
+	mem_addr1 = pci_resource_start(pdev, 1);
+	mem_len1 = pci_resource_len(pdev, 1);
+	printk(KERN_DEBUG PFX "Memaddr0 is %x, length is %x\n", mem_addr0, mem_len0);
+	printk(KERN_DEBUG PFX "Memaddr1 is %x, length is %x\n", mem_addr1, mem_len1);
+
+	err = pci_request_regions(pdev, "agnx-pci");
+	if (err) {
+		printk(KERN_ERR PFX "Can't obtain PCI resource\n");
+		return err;
+	}
+
+	if (pci_set_dma_mask(pdev, DMA_32BIT_MASK) ||
+	    pci_set_consistent_dma_mask(pdev, DMA_32BIT_MASK)) {
+		printk(KERN_ERR PFX "No suitable DMA available\n");
+		goto err_free_reg;
+	}
+
+	pci_set_master(pdev);
+	printk(KERN_DEBUG PFX "pdev->irq is %d\n", pdev->irq);
+
+	dev = ieee80211_alloc_hw(sizeof(*priv), &agnx_ops);
+	if (!dev) {
+		printk(KERN_ERR PFX "ieee80211 alloc failed\n");
+		err = -ENOMEM;
+		goto err_free_reg;
+	}
+	/* init priv  */
+	priv = dev->priv;
+	memset(priv, 0, sizeof(*priv));
+	priv->mode = NL80211_IFTYPE_MONITOR;
+	priv->pdev = pdev;
+	priv->hw = dev;
+	spin_lock_init(&priv->lock);
+	priv->init_status = AGNX_UNINIT;
+
+	/* Map mem #1 and #2 */
+	priv->ctl = pci_iomap(pdev, 0, mem_len0);
+//	printk(KERN_DEBUG PFX"MEM1 mapped address is 0x%p\n", priv->ctl);
+	if (!priv->ctl) {
+		printk(KERN_ERR PFX "Can't map device memory\n");
+		goto err_free_dev;
+	}
+	priv->data = pci_iomap(pdev, 1, mem_len1);
+	printk(KERN_DEBUG PFX "MEM2 mapped address is 0x%p\n", priv->data);
+	if (!priv->data) {
+		printk(KERN_ERR PFX "Can't map device memory\n");
+		goto err_iounmap2;
+	}
+
+	pci_read_config_byte(pdev, PCI_REVISION_ID, &priv->revid);
+
+	priv->band.channels   = (struct ieee80211_channel *)agnx_channels;
+	priv->band.n_channels = ARRAY_SIZE(agnx_channels);
+	priv->band.bitrates   = (struct ieee80211_rate *)agnx_rates_80211g;
+	priv->band.n_bitrates = ARRAY_SIZE(agnx_rates_80211g);
+
+	/* Init ieee802.11 dev  */
+	SET_IEEE80211_DEV(dev, &pdev->dev);
+	pci_set_drvdata(pdev, dev);
+	dev->extra_tx_headroom = sizeof(struct agnx_hdr);
+
+	/* FIXME It only include FCS in promious mode but not manage mode */
+/*      dev->flags =  IEEE80211_HW_RX_INCLUDES_FCS; */
+	dev->channel_change_time = 5000;
+	dev->max_signal = 100;
+	/* FIXME */
+	dev->queues = 1;
+
+	agnx_get_mac_address(priv);
+
+	SET_IEEE80211_PERM_ADDR(dev, priv->mac_addr);
+
+/* 	/\* FIXME *\/ */
+/* 	for (i = 1; i < NUM_DRIVE_MODES; i++) { */
+/* 		err = ieee80211_register_hwmode(dev, &priv->modes[i]); */
+/* 		if (err) { */
+/* 			printk(KERN_ERR PFX "Can't register hwmode\n"); */
+/* 			goto  err_iounmap; */
+/* 		} */
+/* 	} */
+
+	priv->channel = 1;
+	dev->wiphy->bands[IEEE80211_BAND_2GHZ] = &priv->band;
+
+	err = ieee80211_register_hw(dev);
+	if (err) {
+		printk(KERN_ERR PFX "Can't register hardware\n");
+		goto err_iounmap;
+	}
+
+	agnx_hw_reset(priv);
+
+
+	printk(PFX "%s: hwaddr %s, Rev 0x%02x\n", wiphy_name(dev->wiphy),
+	       print_mac(mac, dev->wiphy->perm_addr), priv->revid);
+	return 0;
+
+ err_iounmap:
+	pci_iounmap(pdev, priv->data);
+
+ err_iounmap2:
+	pci_iounmap(pdev, priv->ctl);
+
+ err_free_dev:
+	pci_set_drvdata(pdev, NULL);
+	ieee80211_free_hw(dev);
+
+ err_free_reg:
+	pci_release_regions(pdev);
+
+	pci_disable_device(pdev);
+	return err;
+} /* agnx_pci_probe*/
+
+#ifdef CONFIG_PM
+
+static int agnx_pci_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	struct ieee80211_hw *dev = pci_get_drvdata(pdev);
+	AGNX_TRACE;
+
+	ieee80211_stop_queues(dev);
+	agnx_stop(dev);
+
+	pci_save_state(pdev);
+	pci_set_power_state(pdev, pci_choose_state(pdev, state));
+	return 0;
+}
+
+static int agnx_pci_resume(struct pci_dev *pdev)
+{
+	struct ieee80211_hw *dev = pci_get_drvdata(pdev);
+	AGNX_TRACE;
+
+	pci_set_power_state(pdev, PCI_D0);
+	pci_restore_state(pdev);
+
+	agnx_start(dev);
+	ieee80211_wake_queues(dev);
+
+	return 0;
+}
+
+#else
+
+#define agnx_pci_suspend NULL
+#define agnx_pci_resume NULL
+
+#endif /* CONFIG_PM */
+
+
+static struct pci_driver agnx_pci_driver = {
+	.name		= "agnx-pci",
+	.id_table	= agnx_pci_id_tbl,
+	.probe		= agnx_pci_probe,
+	.remove		= __devexit_p(agnx_pci_remove),
+	.suspend	= agnx_pci_suspend,
+	.resume		= agnx_pci_resume,
+};
+
+static int __init agnx_pci_init(void)
+{
+	AGNX_TRACE;
+	return pci_register_driver(&agnx_pci_driver);
+}
+
+static void __exit agnx_pci_exit(void)
+{
+	AGNX_TRACE;
+	pci_unregister_driver(&agnx_pci_driver);
+}
+
+
+module_init(agnx_pci_init);
+module_exit(agnx_pci_exit);
diff --git a/drivers/staging/agnx/phy.c b/drivers/staging/agnx/phy.c
new file mode 100644
index 000000000000..da8f10c08382
--- /dev/null
+++ b/drivers/staging/agnx/phy.c
@@ -0,0 +1,960 @@
+/**
+ * Airgo MIMO wireless driver
+ *
+ * Copyright (c) 2007 Li YanBo <dreamfly281@gmail.com>
+
+ * Thanks for Jeff Williams <angelbane@gmail.com> do reverse engineer
+ * works and published the SPECS at http://airgo.wdwconsulting.net/mymoin
+
+ * 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/etherdevice.h>
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include "agnx.h"
+#include "debug.h"
+#include "phy.h"
+#include "table.h"
+#include "sta.h"
+#include "xmit.h"
+
+u8 read_from_eeprom(struct agnx_priv *priv, u16 address)
+{
+	void __iomem *ctl = priv->ctl;
+	struct agnx_eeprom cmd;
+	u32 reg;
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.cmd = EEPROM_CMD_READ << AGNX_EEPROM_COMMAND_SHIFT;
+	cmd.address = address;
+	/* Verify that the Status bit is clear */
+	/* Read Command and Address are written to the Serial Interface */
+	iowrite32(*(__le32 *)&cmd, ctl + AGNX_CIR_SERIALITF);
+	/* Wait for the Status bit to clear again */
+	eeprom_delay();
+	/* Read from Data */
+	reg = ioread32(ctl + AGNX_CIR_SERIALITF);
+
+	cmd = *(struct agnx_eeprom *)&reg;
+
+	return cmd.data;
+}
+
+static int card_full_reset(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	reg = agnx_read32(ctl, AGNX_CIR_BLKCTL);
+	agnx_write32(ctl, AGNX_CIR_BLKCTL, 0x80);
+	reg = agnx_read32(ctl, AGNX_CIR_BLKCTL);
+	return 0;
+}
+
+inline void enable_power_saving(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg &= ~0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+}
+
+inline void disable_power_saving(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+}
+
+
+void disable_receiver(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	AGNX_TRACE;
+
+	/* FIXME Disable the receiver */
+	agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x0);
+	/* Set gain control reset */
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x1);
+	/* Reset gain control reset */
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x0);
+}
+
+
+/* Fixme this shoule be disable RX, above is enable RX */
+void enable_receiver(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	AGNX_TRACE;
+
+	/* Set adaptive gain control discovery mode */
+	agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x3);
+	/* Set gain control reset */
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x1);
+	/* Clear gain control reset */
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x0);
+}
+
+static void mac_address_set(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u8 *mac_addr = priv->mac_addr;
+	u32 reg;
+
+	/* FIXME */
+	reg = (mac_addr[0] << 24) | (mac_addr[1] << 16) | mac_addr[2] << 8 | mac_addr[3];
+	iowrite32(reg, ctl + AGNX_RXM_MACHI);
+ 	reg = (mac_addr[4] << 8) | mac_addr[5];
+	iowrite32(reg, ctl + AGNX_RXM_MACLO);
+}
+
+static void receiver_bssid_set(struct agnx_priv *priv, u8 *bssid)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	disable_receiver(priv);
+	/* FIXME */
+	reg = bssid[0] << 24 | (bssid[1] << 16) | (bssid[2] << 8) | bssid[3];
+	iowrite32(reg, ctl + AGNX_RXM_BSSIDHI);
+ 	reg = (bssid[4] << 8) | bssid[5];
+	iowrite32(reg, ctl + AGNX_RXM_BSSIDLO);
+
+	/* Enable the receiver */
+	enable_receiver(priv);
+
+	/* Clear the TSF */
+/* 	agnx_write32(ctl, AGNX_TXM_TSFLO, 0x0); */
+/* 	agnx_write32(ctl, AGNX_TXM_TSFHI, 0x0); */
+	/* Clear the TBTT */
+	agnx_write32(ctl, AGNX_TXM_TBTTLO, 0x0);
+	agnx_write32(ctl, AGNX_TXM_TBTTHI, 0x0);
+	disable_receiver(priv);
+} /* receiver_bssid_set */
+
+static void band_management_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	void __iomem *data = priv->data;
+	u32 reg;
+	int i;
+	AGNX_TRACE;
+
+	agnx_write32(ctl, AGNX_BM_TXWADDR, AGNX_PDU_TX_WQ);
+	agnx_write32(ctl, AGNX_CIR_ADDRWIN, 0x0);
+	memset_io(data + AGNX_PDUPOOL, 0x0, AGNX_PDUPOOL_SIZE);
+	agnx_write32(ctl, AGNX_BM_BMCTL, 0x200);
+
+	agnx_write32(ctl, AGNX_BM_CIPDUWCNT, 0x40);
+	agnx_write32(ctl, AGNX_BM_SPPDUWCNT, 0x2);
+	agnx_write32(ctl, AGNX_BM_RFPPDUWCNT, 0x0);
+	agnx_write32(ctl, AGNX_BM_RHPPDUWCNT, 0x22);
+
+	/* FIXME Initialize the Free Pool Linked List */
+	/*    1. Write the Address of the Next Node ((0x41800 + node*size)/size)
+	      to the first word of each node.  */
+	for (i = 0; i < PDU_FREE_CNT; i++) {
+		iowrite32((AGNX_PDU_FREE + (i+1)*PDU_SIZE)/PDU_SIZE,
+			  data + AGNX_PDU_FREE + (PDU_SIZE * i));
+		/* The last node should be set to 0x0 */
+		if ((i + 1) == PDU_FREE_CNT)
+			memset_io(data + AGNX_PDU_FREE + (PDU_SIZE * i),
+				  0x0, PDU_SIZE);
+	}
+
+	/* Head is First Pool address (0x41800) / size (0x80) */
+	agnx_write32(ctl, AGNX_BM_FPLHP, AGNX_PDU_FREE/PDU_SIZE);
+	/* Tail is Last Pool Address (0x47f80) / size (0x80) */
+	agnx_write32(ctl, AGNX_BM_FPLTP, 0x47f80/PDU_SIZE);
+	/* Count is Number of Nodes in the Pool (0xd0) */
+	agnx_write32(ctl, AGNX_BM_FPCNT, PDU_FREE_CNT);
+
+	/* Start all workqueue */
+	agnx_write32(ctl, AGNX_BM_CIWQCTL, 0x80000);
+	agnx_write32(ctl, AGNX_BM_CPULWCTL, 0x80000);
+	agnx_write32(ctl, AGNX_BM_CPUHWCTL, 0x80000);
+	agnx_write32(ctl, AGNX_BM_CPUTXWCTL, 0x80000);
+	agnx_write32(ctl, AGNX_BM_CPURXWCTL, 0x80000);
+	agnx_write32(ctl, AGNX_BM_SPRXWCTL, 0x80000);
+	agnx_write32(ctl, AGNX_BM_SPTXWCTL, 0x80000);
+	agnx_write32(ctl, AGNX_BM_RFPWCTL, 0x80000);
+
+	/* Enable the Band Management */
+	reg = agnx_read32(ctl, AGNX_BM_BMCTL);
+	reg |= 0x1;
+	agnx_write32(ctl, AGNX_BM_BMCTL, reg);
+} /* band_managment_init */
+
+
+static void system_itf_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	agnx_write32(ctl, AGNX_SYSITF_GPIOUT, 0x0);
+	agnx_write32(ctl, AGNX_PM_TESTPHY, 0x11e143a);
+
+	if (priv->revid == 0) {
+		reg = agnx_read32(ctl, AGNX_SYSITF_SYSMODE);
+		reg |= 0x11;
+		agnx_write32(ctl, AGNX_SYSITF_SYSMODE, reg);
+	}
+	/* ??? What is that means? it should difference for differice type
+	 of cards */
+	agnx_write32(ctl, AGNX_CIR_SERIALITF, 0xfff81006);
+
+	agnx_write32(ctl, AGNX_SYSITF_GPIOIN, 0x1f0000);
+	agnx_write32(ctl, AGNX_SYSITF_GPIOUT, 0x5);
+	reg = agnx_read32(ctl, AGNX_SYSITF_GPIOIN);
+}
+
+static void encryption_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	AGNX_TRACE;
+
+	agnx_write32(ctl, AGNX_ENCRY_WEPKEY0, 0x0);
+	agnx_write32(ctl, AGNX_ENCRY_WEPKEY1, 0x0);
+	agnx_write32(ctl, AGNX_ENCRY_WEPKEY2, 0x0);
+	agnx_write32(ctl, AGNX_ENCRY_WEPKEY3, 0x0);
+	agnx_write32(ctl, AGNX_ENCRY_CCMRECTL, 0x8);
+}
+
+static void tx_management_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	void __iomem *data = priv->data;
+	u32 reg;
+	AGNX_TRACE;
+
+	/* Fill out the ComputationalEngineLookupTable
+	 * starting at memory #2 offset 0x800
+	 */
+	tx_engine_lookup_tbl_init(priv);
+	memset_io(data + 0x1000, 0, 0xfe0);
+	/* Enable Transmission Management Functions */
+	agnx_write32(ctl, AGNX_TXM_ETMF, 0x3ff);
+	/* Write 0x3f to Transmission Template */
+	agnx_write32(ctl, AGNX_TXM_TXTEMP, 0x3f);
+
+	if (priv->revid >= 2)
+		agnx_write32(ctl, AGNX_TXM_SIFSPIFS, 0x1e140a0b);
+	else
+		agnx_write32(ctl, AGNX_TXM_SIFSPIFS, 0x1e190a0b);
+
+	reg = agnx_read32(ctl, AGNX_TXM_TIFSEIFS);
+	reg &= 0xff00;
+	reg |= 0xb;
+	agnx_write32(ctl, AGNX_TXM_TIFSEIFS, reg);
+	reg = agnx_read32(ctl, AGNX_TXM_TIFSEIFS);
+	reg &= 0xffff00ff;
+	reg |= 0xa00;
+	agnx_write32(ctl, AGNX_TXM_TIFSEIFS, reg);
+	/* Enable TIFS */
+	agnx_write32(ctl, AGNX_TXM_CTL, 0x40000);
+
+	reg = agnx_read32(ctl, AGNX_TXM_TIFSEIFS);
+	reg &= 0xff00ffff;
+	reg |= 0x510000;
+	agnx_write32(ctl, AGNX_TXM_TIFSEIFS, reg);
+	reg = agnx_read32(ctl, AGNX_TXM_PROBDELAY);
+	reg &= 0xff00ffff;
+	agnx_write32(ctl, AGNX_TXM_PROBDELAY, reg);
+	reg = agnx_read32(ctl, AGNX_TXM_TIFSEIFS);
+	reg &= 0x00ffffff;
+	reg |= 0x1c000000;
+	agnx_write32(ctl, AGNX_TXM_TIFSEIFS, reg);
+	reg = agnx_read32(ctl, AGNX_TXM_PROBDELAY);
+	reg &= 0x00ffffff;
+	reg |= 0x01000000;
+	agnx_write32(ctl, AGNX_TXM_PROBDELAY, reg);
+
+	/* # Set DIF 0-1,2-3,4-5,6-7 to defaults */
+	agnx_write32(ctl, AGNX_TXM_DIF01, 0x321d321d);
+	agnx_write32(ctl, AGNX_TXM_DIF23, 0x321d321d);
+	agnx_write32(ctl, AGNX_TXM_DIF45, 0x321d321d);
+	agnx_write32(ctl, AGNX_TXM_DIF67, 0x321d321d);
+
+	/* Max Ack timeout limit */
+	agnx_write32(ctl, AGNX_TXM_MAXACKTIM, 0x1e19);
+	/* Max RX Data Timeout count, */
+	reg = agnx_read32(ctl, AGNX_TXM_MAXRXTIME);
+	reg &= 0xffff0000;
+	reg |= 0xff;
+	agnx_write32(ctl, AGNX_TXM_MAXRXTIME, reg);
+
+	/* CF poll RX Timeout count */
+	reg = agnx_read32(ctl, AGNX_TXM_CFPOLLRXTIM);
+	reg &= 0xffff;
+	reg |= 0xff0000;
+	agnx_write32(ctl, AGNX_TXM_CFPOLLRXTIM, reg);
+
+	/* Max Timeout Exceeded count, */
+	reg = agnx_read32(ctl, AGNX_TXM_MAXTIMOUT);
+	reg &= 0xff00ffff;
+	reg |= 0x190000;
+	agnx_write32(ctl, AGNX_TXM_MAXTIMOUT, reg);
+
+	/* CF ack timeout limit for 11b */
+	reg = agnx_read32(ctl, AGNX_TXM_CFACKT11B);
+	reg &= 0xff00;
+	reg |= 0x1e;
+	agnx_write32(ctl, AGNX_TXM_CFACKT11B, reg);
+
+	/* Max CF Poll Timeout Count */
+	reg = agnx_read32(ctl, AGNX_TXM_CFPOLLRXTIM);
+	reg &= 0xffff0000;
+	reg |= 0x19;
+	agnx_write32(ctl, AGNX_TXM_CFPOLLRXTIM, reg);
+	/* CF Poll RX Timeout Count */
+	reg = agnx_read32(ctl, AGNX_TXM_CFPOLLRXTIM);
+	reg &= 0xffff0000;
+	reg |= 0x1e;
+	agnx_write32(ctl, AGNX_TXM_CFPOLLRXTIM, reg);
+
+	/* # write default to */
+	/*    1. Schedule Empty Count */
+	agnx_write32(ctl, AGNX_TXM_SCHEMPCNT, 0x5);
+	/*    2. CFP Period Count */
+	agnx_write32(ctl, AGNX_TXM_CFPERCNT, 0x1);
+	/*    3. CFP MDV  */
+	agnx_write32(ctl, AGNX_TXM_CFPMDV, 0x10000);
+
+	/* Probe Delay */
+	reg = agnx_read32(ctl, AGNX_TXM_PROBDELAY);
+	reg &= 0xffff0000;
+	reg |= 0x400;
+	agnx_write32(ctl, AGNX_TXM_PROBDELAY, reg);
+
+	/* Max CCA count Slot */
+	reg = agnx_read32(ctl, AGNX_TXM_MAXCCACNTSLOT);
+	reg &= 0xffff00ff;
+	reg |= 0x900;
+	agnx_write32(ctl, AGNX_TXM_MAXCCACNTSLOT, reg);
+
+	/* Slot limit/1 msec Limit */
+	reg = agnx_read32(ctl, AGNX_TXM_SLOTLIMIT);
+	reg &= 0xff00ffff;
+	reg |= 0x140077;
+	agnx_write32(ctl, AGNX_TXM_SLOTLIMIT, reg);
+
+	/* # Set CW #(0-7) to default */
+	agnx_write32(ctl, AGNX_TXM_CW0, 0xff0007);
+	agnx_write32(ctl, AGNX_TXM_CW1, 0xff0007);
+	agnx_write32(ctl, AGNX_TXM_CW2, 0xff0007);
+	agnx_write32(ctl, AGNX_TXM_CW3, 0xff0007);
+	agnx_write32(ctl, AGNX_TXM_CW4, 0xff0007);
+	agnx_write32(ctl, AGNX_TXM_CW5, 0xff0007);
+	agnx_write32(ctl, AGNX_TXM_CW6, 0xff0007);
+	agnx_write32(ctl, AGNX_TXM_CW7, 0xff0007);
+
+	/* # Set Short/Long limit #(0-7) to default */
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM0,  0xa000a);
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM1,  0xa000a);
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM2,  0xa000a);
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM3,  0xa000a);
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM4,  0xa000a);
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM5,  0xa000a);
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM6,  0xa000a);
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM7,  0xa000a);
+
+	reg = agnx_read32(ctl, AGNX_TXM_CTL);
+	reg |= 0x1400;
+	agnx_write32(ctl, AGNX_TXM_CTL, reg);
+	/* Wait for bit 0 in Control Reg to clear  */
+	udelay(80);
+	reg = agnx_read32(ctl, AGNX_TXM_CTL);
+	/* Or 0x18000 to Control reg */
+	reg = agnx_read32(ctl, AGNX_TXM_CTL);
+	reg |= 0x18000;
+	agnx_write32(ctl, AGNX_TXM_CTL, reg);
+	/* Wait for bit 0 in Control Reg to clear */
+	udelay(80);
+	reg = agnx_read32(ctl, AGNX_TXM_CTL);
+
+	/* Set Listen Interval Count to default */
+	agnx_write32(ctl, AGNX_TXM_LISINTERCNT, 0x1);
+	/* Set DTIM period count to default */
+	agnx_write32(ctl, AGNX_TXM_DTIMPERICNT, 0x2000);
+} /* tx_management_init */
+
+static void rx_management_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	AGNX_TRACE;
+
+	/* Initialize the Routing Table */
+	routing_table_init(priv);
+
+	if (priv->revid >= 3) {
+		agnx_write32(ctl, 0x2074, 0x1f171710);
+		agnx_write32(ctl, 0x2078, 0x10100d0d);
+		agnx_write32(ctl, 0x207c, 0x11111010);
+	}
+	else
+		agnx_write32(ctl, AGNX_RXM_DELAY11, 0x0);
+	agnx_write32(ctl, AGNX_RXM_REQRATE, 0x8195e00);
+}
+
+
+static void agnx_timer_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	AGNX_TRACE;
+
+/* 	/\* Write 0x249f00 (tick duration?) to Timer 1 *\/ */
+/* 	agnx_write32(ctl, AGNX_TIMCTL_TIMER1, 0x249f00); */
+/* 	/\* Write 0xe2 to Timer 1 Control *\/ */
+/* 	agnx_write32(ctl, AGNX_TIMCTL_TIM1CTL, 0xe2); */
+
+	/* Write 0x249f00 (tick duration?) to Timer 1 */
+	agnx_write32(ctl, AGNX_TIMCTL_TIMER1, 0x0);
+	/* Write 0xe2 to Timer 1 Control */
+	agnx_write32(ctl, AGNX_TIMCTL_TIM1CTL, 0x0);
+
+	iowrite32(0xFFFFFFFF, priv->ctl + AGNX_TXM_BEACON_CTL);
+}
+
+static void power_manage_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	agnx_write32(ctl, AGNX_PM_MACMSW, 0x1f);
+	agnx_write32(ctl, AGNX_PM_RFCTL, 0x1f);
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg &= 0xf00f;
+	reg |= 0xa0;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+
+	if (priv->revid >= 3) {
+		reg = agnx_read32(ctl, AGNX_PM_SOFTRST);
+		reg |= 0x18;
+		agnx_write32(ctl, AGNX_PM_SOFTRST, reg);
+	}
+} /* power_manage_init */
+
+
+static void gain_ctlcnt_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	agnx_write32(ctl, AGNX_GCR_TRACNT5, 0x119);
+	agnx_write32(ctl, AGNX_GCR_TRACNT6, 0x118);
+	agnx_write32(ctl, AGNX_GCR_TRACNT7, 0x117);
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg &= ~0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+
+	agnx_write32(ctl, AGNX_CIR_ADDRWIN, 0x0);
+
+	/* FIXME Write the initial Station Descriptor for the card */
+	sta_init(priv, LOCAL_STAID);
+	sta_init(priv, BSSID_STAID);
+
+	/* Enable staion 0 and 1 can do TX */
+	/* It seemed if we set other bit to 1 the bit 0 will
+	   be auto change to 0 */
+	agnx_write32(ctl, AGNX_BM_TXTOPEER, 0x2 | 0x1);
+//	agnx_write32(ctl, AGNX_BM_TXTOPEER, 0x1);
+} /* gain_ctlcnt_init */
+
+
+static void phy_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	void __iomem *data = priv->data;
+	u32 reg;
+	AGNX_TRACE;
+
+	/* Load InitialGainTable */
+	gain_table_init(priv);
+
+  	agnx_write32(ctl, AGNX_CIR_ADDRWIN, 0x2000000);
+
+	/* Clear the following offsets in Memory Range #2: */
+	memset_io(data + 0x5040, 0, 0xa * 4);
+	memset_io(data + 0x5080, 0, 0xa * 4);
+	memset_io(data + 0x50c0, 0, 0xa * 4);
+	memset_io(data + 0x5400, 0, 0x80 * 4);
+	memset_io(data + 0x6000, 0, 0x280 * 4);
+	memset_io(data + 0x7000, 0, 0x280 * 4);
+	memset_io(data + 0x8000, 0, 0x280 * 4);
+
+	/* Initialize the Following Registers According to PCI Revision ID */
+	if (priv->revid == 0) {
+		/* fixme the part hasn't been update but below has been update
+		   based on WGM511 */
+		agnx_write32(ctl, AGNX_ACI_LEN, 0xf);
+		agnx_write32(ctl, AGNX_ACI_TIMER1, 0x1d);
+		agnx_write32(ctl, AGNX_ACI_TIMER2, 0x3);
+		agnx_write32(ctl, AGNX_ACI_AICCHA0OVE, 0x11);
+		agnx_write32(ctl, AGNX_ACI_AICCHA1OVE, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THD0A, 0x64);
+		agnx_write32(ctl, AGNX_GCR_THD0AL, 0x4b);
+		agnx_write32(ctl, AGNX_GCR_THD0B, 0x4b);
+		agnx_write32(ctl, AGNX_GCR_DUNSAT, 0x14);
+		agnx_write32(ctl, AGNX_GCR_DSAT, 0x24);
+		agnx_write32(ctl, AGNX_GCR_DFIRCAL, 0x8);
+		agnx_write32(ctl, AGNX_GCR_DGCTL11A, 0x1a);
+		agnx_write32(ctl, AGNX_GCR_DGCTL11B, 0x3);
+		agnx_write32(ctl, AGNX_GCR_GAININIT, 0xd);
+		agnx_write32(ctl, AGNX_GCR_THNOSIG, 0x1);
+		agnx_write32(ctl, AGNX_GCR_COARSTEP, 0x7);
+		agnx_write32(ctl, AGNX_GCR_SIFST11A, 0x28);
+		agnx_write32(ctl, AGNX_GCR_SIFST11B, 0x28);
+		reg = agnx_read32(ctl, AGNX_GCR_CWDETEC);
+		reg |= 0x1;
+		agnx_write32(ctl, AGNX_GCR_CWDETEC, reg);
+		agnx_write32(ctl, AGNX_GCR_0X38, 0x1e);
+		agnx_write32(ctl, AGNX_GCR_BOACT, 0x26);
+		agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x3);
+		agnx_write32(ctl, AGNX_GCR_NLISTANT, 0x3);
+		agnx_write32(ctl, AGNX_GCR_NACTIANT, 0x3);
+		agnx_write32(ctl, AGNX_GCR_NMEASANT, 0x3);
+		agnx_write32(ctl, AGNX_GCR_NCAPTANT, 0x3);
+		agnx_write32(ctl, AGNX_GCR_THCAP11A, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THCAP11B, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THCAPRX11A, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THCAPRX11B, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THLEVDRO, 0x10);
+		agnx_write32(ctl, AGNX_GCR_MAXRXTIME11A, 0x1);
+		agnx_write32(ctl, AGNX_GCR_MAXRXTIME11B, 0x1);
+		agnx_write32(ctl, AGNX_GCR_CORRTIME, 0x190);
+		agnx_write32(ctl, AGNX_GCR_SIGHTH, 0x78);
+		agnx_write32(ctl, AGNX_GCR_SIGLTH, 0x1c);
+		agnx_write32(ctl, AGNX_GCR_CORRDROP, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THCD, 0x0);
+		agnx_write32(ctl, AGNX_GCR_MAXPOWDIFF, 0x1);
+		agnx_write32(ctl, AGNX_GCR_TESTBUS, 0x0);
+		agnx_write32(ctl, AGNX_GCR_ANTCFG, 0x1f);
+		agnx_write32(ctl, AGNX_GCR_THJUMP, 0x14);
+		agnx_write32(ctl, AGNX_GCR_THPOWER, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THPOWCLIP, 0x30);
+		agnx_write32(ctl, AGNX_GCR_THD0BTFEST, 0x32);
+		agnx_write32(ctl, AGNX_GCR_THRX11BPOWMIN, 0x19);
+		agnx_write32(ctl, AGNX_GCR_0X14c, 0x0);
+		agnx_write32(ctl, AGNX_GCR_0X150, 0x0);
+		agnx_write32(ctl, 0x9400, 0x0);
+		agnx_write32(ctl, 0x940c, 0x6ff);
+		agnx_write32(ctl, 0x9428, 0xa0);
+		agnx_write32(ctl, 0x9434, 0x0);
+		agnx_write32(ctl, 0x9c04, 0x15);
+		agnx_write32(ctl, 0x9c0c, 0x7f);
+		agnx_write32(ctl, 0x9c34, 0x0);
+		agnx_write32(ctl, 0xc000, 0x38d);
+		agnx_write32(ctl, 0x14018, 0x0);
+		agnx_write32(ctl, 0x16000, 0x1);
+		agnx_write32(ctl, 0x11004, 0x0);
+		agnx_write32(ctl, 0xec54, 0xa);
+		agnx_write32(ctl, 0xec1c, 0x5);
+	} else if (priv->revid > 0) {
+		agnx_write32(ctl, AGNX_ACI_LEN, 0xf);
+		agnx_write32(ctl, AGNX_ACI_TIMER1, 0x21);
+		agnx_write32(ctl, AGNX_ACI_TIMER2, 0x27);
+		agnx_write32(ctl, AGNX_ACI_AICCHA0OVE, 0x11);
+		agnx_write32(ctl, AGNX_ACI_AICCHA1OVE, 0x0);
+		agnx_write32(ctl, AGNX_GCR_DUNSAT, 0x14);
+		agnx_write32(ctl, AGNX_GCR_DSAT, 0x24);
+		agnx_write32(ctl, AGNX_GCR_DFIRCAL, 0x8);
+		agnx_write32(ctl, AGNX_GCR_DGCTL11A, 0x1a);
+		agnx_write32(ctl, AGNX_GCR_DGCTL11B, 0x3);
+		agnx_write32(ctl, AGNX_GCR_GAININIT, 0xd);
+		agnx_write32(ctl, AGNX_GCR_THNOSIG, 0x1);
+		agnx_write32(ctl, AGNX_GCR_COARSTEP, 0x7);
+		agnx_write32(ctl, AGNX_GCR_SIFST11A, 0x28);
+		agnx_write32(ctl, AGNX_GCR_SIFST11B, 0x28);
+		agnx_write32(ctl, AGNX_GCR_CWDETEC, 0x0);
+		agnx_write32(ctl, AGNX_GCR_0X38, 0x1e);
+//		agnx_write32(ctl, AGNX_GCR_BOACT, 0x26);
+		agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x3);
+
+		agnx_write32(ctl, AGNX_GCR_THCAP11A, 0x32);
+		agnx_write32(ctl, AGNX_GCR_THCAP11B, 0x32);
+		agnx_write32(ctl, AGNX_GCR_THCAPRX11A, 0x32);
+		agnx_write32(ctl, AGNX_GCR_THCAPRX11B, 0x32);
+		agnx_write32(ctl, AGNX_GCR_THLEVDRO, 0x10);
+		agnx_write32(ctl, AGNX_GCR_MAXRXTIME11A, 0x1ad);
+		agnx_write32(ctl, AGNX_GCR_MAXRXTIME11B, 0xa10);
+		agnx_write32(ctl, AGNX_GCR_CORRTIME, 0x190);
+		agnx_write32(ctl, AGNX_GCR_CORRDROP, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THCD, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THCS, 0x0);
+		agnx_write32(ctl, AGNX_GCR_MAXPOWDIFF, 0x4);
+		agnx_write32(ctl, AGNX_GCR_TESTBUS, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THJUMP, 0x1e);
+		agnx_write32(ctl, AGNX_GCR_THPOWER, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THPOWCLIP, 0x2a);
+		agnx_write32(ctl, AGNX_GCR_THD0BTFEST, 0x3c);
+		agnx_write32(ctl, AGNX_GCR_THRX11BPOWMIN, 0x19);
+		agnx_write32(ctl, AGNX_GCR_0X14c, 0x0);
+		agnx_write32(ctl, AGNX_GCR_0X150, 0x0);
+		agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x0);
+		agnx_write32(ctl, AGNX_GCR_WATCHDOG, 0x37);
+		agnx_write32(ctl, 0x9400, 0x0);
+		agnx_write32(ctl, 0x940c, 0x6ff);
+		agnx_write32(ctl, 0x9428, 0xa0);
+		agnx_write32(ctl, 0x9434, 0x0);
+		agnx_write32(ctl, 0x9c04, 0x15);
+		agnx_write32(ctl, 0x9c0c, 0x7f);
+		agnx_write32(ctl, 0x9c34, 0x0);
+		agnx_write32(ctl, 0xc000, 0x38d);
+		agnx_write32(ctl, 0x14014, 0x1000);
+		agnx_write32(ctl, 0x14018, 0x0);
+		agnx_write32(ctl, 0x16000, 0x1);
+		agnx_write32(ctl, 0x11004, 0x0);
+		agnx_write32(ctl, 0xec54, 0xa);
+		agnx_write32(ctl, 0xec1c, 0x50);
+	} else if (priv->revid > 1) {
+		reg = agnx_read32(ctl, 0xec18);
+		reg |= 0x8;
+		agnx_write32(ctl, 0xec18, reg);
+	}
+
+	/* Write the TX Fir Coefficient Table */
+	tx_fir_table_init(priv);
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg &= ~0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+	reg = agnx_read32(ctl, AGNX_PM_PLLCTL);
+	reg |= 0x1;
+	agnx_write32(ctl, AGNX_PM_PLLCTL, reg);
+
+/* 	reg = agnx_read32(ctl, 0x1a030); */
+/* 	reg &= ~0x4; */
+/* 	agnx_write32(ctl, 0x1a030, reg); */
+
+	agnx_write32(ctl, AGNX_GCR_TRACNT4, 0x113);
+} /* phy_init */
+
+static void chip_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	band_management_init(priv);
+
+	rf_chips_init(priv);
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+
+	/* Initialize the PHY */
+	phy_init(priv);
+
+	encryption_init(priv);
+
+	tx_management_init(priv);
+
+	rx_management_init(priv);
+
+	power_manage_init(priv);
+
+	/* Initialize the Timers */
+	agnx_timer_init(priv);
+
+	/* Write 0xc390bf9 to Interrupt Mask (Disable TX) */
+	reg = 0xc390bf9 & ~IRQ_TX_BEACON;
+	reg &= ~IRQ_TX_DISABLE;
+	agnx_write32(ctl, AGNX_INT_MASK, reg);
+
+	reg = agnx_read32(ctl, AGNX_CIR_BLKCTL);
+	reg |= 0x800;
+	agnx_write32(ctl, AGNX_CIR_BLKCTL, reg);
+
+	/* set it when need get multicast enable? */
+	agnx_write32(ctl, AGNX_BM_MTSM, 0xff);
+} /* chip_init */
+
+
+static inline void set_promis_and_managed(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x10 | 0x2);
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x10 | 0x2);
+}
+static inline void set_learn_mode(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x8);
+}
+static inline void set_scan_mode(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x20);
+}
+static inline void set_promiscuous_mode(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	/* agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x210);*/
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x10);
+}
+static inline void set_managed_mode(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x2);
+}
+static inline void set_adhoc_mode(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x0);
+}
+
+#if 0
+static void unknow_register_write(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x0, 0x3e);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x4, 0xb2);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x8, 0x140);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0xc, 0x1C0);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x10, 0x1FF);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x14, 0x1DD);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x18, 0x15F);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x1c, 0xA1);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x20, 0x3E7);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x24, 0x36B);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x28, 0x348);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x2c, 0x37D);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x30, 0x3DE);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x34, 0x36);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x38, 0x64);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x3c, 0x57);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x40, 0x23);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x44, 0x3ED);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x48, 0x3C9);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x4c, 0x3CA);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x50, 0x3E7);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x54, 0x8);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x58, 0x1F);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x5c, 0x1a);
+}
+#endif
+
+static void card_interface_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u8 bssid[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u32 reg;
+	unsigned int i;
+	AGNX_TRACE;
+
+	might_sleep();
+	/* Clear RX Control and Enable RX queues */
+	agnx_write32(ctl, AGNX_CIR_RXCTL, 0x8);
+
+	might_sleep();
+	/* Do a full reset of the card */
+	card_full_reset(priv);
+	might_sleep();
+
+	/* Check and set Card Endianness */
+	reg = ioread32(priv->ctl + AGNX_CIR_ENDIAN);
+	/* TODO If not 0xB3B2B1B0 set to 0xB3B2B1B0 */
+	printk(KERN_INFO PFX "CIR_ENDIAN is %x\n", reg);
+
+
+	/* Config the eeprom */
+	agnx_write32(ctl, AGNX_CIR_SERIALITF, 0x7000086);
+	udelay(10);
+	reg = agnx_read32(ctl, AGNX_CIR_SERIALITF);
+
+
+	agnx_write32(ctl, AGNX_PM_SOFTRST, 0x80000033);
+	reg = agnx_read32(ctl, 0xec50);
+	reg |= 0xf;
+	agnx_write32(ctl, 0xec50, reg);
+	agnx_write32(ctl, AGNX_PM_SOFTRST, 0x0);
+
+
+	reg = agnx_read32(ctl, AGNX_SYSITF_GPIOIN);
+	udelay(10);
+	reg = agnx_read32(ctl, AGNX_CIR_SERIALITF);
+
+	/* Dump the eeprom */
+	do {
+		char eeprom[0x100000/0x100];
+
+		for (i = 0; i < 0x100000; i += 0x100) {
+			agnx_write32(ctl, AGNX_CIR_SERIALITF, 0x3000000 + i);
+			udelay(13);
+			reg = agnx_read32(ctl, AGNX_CIR_SERIALITF);
+			udelay(70);
+			reg = agnx_read32(ctl, AGNX_CIR_SERIALITF);
+			eeprom[i/0x100] = reg & 0xFF;
+			udelay(10);
+		}
+		print_hex_dump_bytes(PFX "EEPROM: ", DUMP_PREFIX_NONE, eeprom,
+				     ARRAY_SIZE(eeprom));
+	} while(0);
+
+	spi_rc_write(ctl, RF_CHIP0, 0x26);
+        reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+
+	/* Initialize the system interface */
+	system_itf_init(priv);
+
+	might_sleep();
+	/* Chip Initialization (Polaris) */
+	chip_init(priv);
+	might_sleep();
+
+	/* Calibrate the antennae */
+	antenna_calibrate(priv);
+
+	reg = agnx_read32(ctl, 0xec50);
+	reg &= ~0x40;
+	agnx_write32(ctl, 0xec50, reg);
+	agnx_write32(ctl, AGNX_PM_SOFTRST, 0x0);
+	agnx_write32(ctl, AGNX_PM_PLLCTL, 0x1);
+
+	reg = agnx_read32(ctl, AGNX_BM_BMCTL);
+	reg |= 0x8000;
+	agnx_write32(ctl, AGNX_BM_BMCTL, reg);
+	enable_receiver(priv);
+	reg = agnx_read32(ctl, AGNX_SYSITF_SYSMODE);
+	reg |= 0x200;
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, reg);
+	enable_receiver(priv);
+
+	might_sleep();
+	/* Initialize Gain Control Counts */
+	gain_ctlcnt_init(priv);
+
+	/* Write Initial Station Power Template for this station(#0) */
+	sta_power_init(priv, LOCAL_STAID);
+
+	might_sleep();
+	/* Initialize the rx,td,tm rings, for each node in the ring */
+	fill_rings(priv);
+
+	might_sleep();
+
+
+	agnx_write32(ctl, AGNX_PM_SOFTRST, 0x80000033);
+	agnx_write32(ctl, 0xec50, 0xc);
+	agnx_write32(ctl, AGNX_PM_SOFTRST, 0x0);
+
+	/* FIXME Initialize the transmit control register */
+	agnx_write32(ctl, AGNX_TXM_CTL, 0x194c1);
+
+	enable_receiver(priv);
+
+	might_sleep();
+	/* FIXME Set the Receive Control Mac Address to card address */
+	mac_address_set(priv);
+	enable_receiver(priv);
+	might_sleep();
+
+	/* Set the recieve request rate */
+	/* FIXME Enable the request */
+	/* Check packet length */
+	/* Set maximum packet length */
+/* 	agnx_write32(ctl, AGNX_RXM_REQRATE, 0x88195e00); */
+/* 	enable_receiver(priv); */
+
+	/* Set the Receiver BSSID */
+	receiver_bssid_set(priv, bssid);
+
+	/* FIXME Set to managed mode */
+	set_managed_mode(priv);
+//	set_promiscuous_mode(priv);
+/* 	set_scan_mode(priv); */
+/* 	set_learn_mode(priv); */
+// 	set_promis_and_managed(priv);
+// 	set_adhoc_mode(priv);
+
+	/* Set the recieve request rate */
+	/* Check packet length */
+	agnx_write32(ctl, AGNX_RXM_REQRATE, 0x08000000);
+	reg = agnx_read32(ctl, AGNX_RXM_REQRATE);
+	/* Set maximum packet length */
+	reg |= 0x00195e00;
+	agnx_write32(ctl, AGNX_RXM_REQRATE, reg);
+
+	/* Configure the RX and TX interrupt */
+	reg = ENABLE_RX_INTERRUPT | RX_CACHE_LINE | FRAG_LEN_2048 | FRAG_BE;
+	agnx_write32(ctl, AGNX_CIR_RXCFG, reg);
+	/* FIXME */
+	reg = ENABLE_TX_INTERRUPT | TX_CACHE_LINE | FRAG_LEN_2048 | FRAG_BE;
+	agnx_write32(ctl, AGNX_CIR_TXCFG, reg);
+
+	/* Enable RX TX Interrupts */
+	agnx_write32(ctl, AGNX_CIR_RXCTL, 0x80);
+	agnx_write32(ctl, AGNX_CIR_TXMCTL, 0x80);
+	agnx_write32(ctl, AGNX_CIR_TXDCTL, 0x80);
+
+	/* FIXME Set the master control interrupt in block control */
+	agnx_write32(ctl, AGNX_CIR_BLKCTL, 0x800);
+
+	/* Enable RX and TX queues */
+	reg = agnx_read32(ctl, AGNX_CIR_RXCTL);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_CIR_RXCTL, reg);
+	reg = agnx_read32(ctl, AGNX_CIR_TXMCTL);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_CIR_TXMCTL, reg);
+	reg = agnx_read32(ctl, AGNX_CIR_TXDCTL);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_CIR_TXDCTL, reg);
+
+	agnx_write32(ctl, AGNX_SYSITF_GPIOUT, 0x5);
+	/* FIXME */
+	/*  unknow_register_write(priv); */
+	/* Update local card hash entry */
+	hash_write(priv, priv->mac_addr, LOCAL_STAID);
+
+	might_sleep();
+
+	/* FIXME */
+	agnx_set_channel(priv, 1);
+	might_sleep();
+} /* agnx_card_interface_init */
+
+
+void agnx_hw_init(struct agnx_priv *priv)
+{
+	AGNX_TRACE;
+	might_sleep();
+	card_interface_init(priv);
+}
+
+int agnx_hw_reset(struct agnx_priv *priv)
+{
+	return card_full_reset(priv);
+}
+
+int agnx_set_ssid(struct agnx_priv *priv, u8 *ssid, size_t ssid_len)
+{
+	AGNX_TRACE;
+	return 0;
+}
+
+void agnx_set_bssid(struct agnx_priv *priv, u8 *bssid)
+{
+	receiver_bssid_set(priv, bssid);
+}
diff --git a/drivers/staging/agnx/phy.h b/drivers/staging/agnx/phy.h
new file mode 100644
index 000000000000..55e1e222179e
--- /dev/null
+++ b/drivers/staging/agnx/phy.h
@@ -0,0 +1,409 @@
+#ifndef AGNX_PHY_H_
+#define AGNX_PHY_H_
+
+#include "agnx.h"
+
+/* Transmission Managment Registers */
+#define AGNX_TXM_BASE		0x0000
+#define AGNX_TXM_CTL		0x0000	/* control register */
+#define AGNX_TXM_ETMF		0x0004 /* enable transmission management functions */
+#define AGNX_TXM_TXTEMP		0x0008 /* transmission template */
+#define AGNX_TXM_RETRYSTAID	0x000c /* Retry Station ID */
+#define AGNX_TXM_TIMESTAMPLO		0x0010	/* Timestamp Lo */
+#define AGNX_TXM_TIMESTAMPHI		0x0014	/* Timestamp Hi */
+#define AGNX_TXM_TXDELAY	0x0018  /* tx delay */
+#define AGNX_TXM_TBTTLO		0x0020	/* tbtt Lo */
+#define AGNX_TXM_TBTTHI		0x0024	/* tbtt Hi */
+#define AGNX_TXM_BEAINTER	0x0028 /* Beacon Interval */
+#define AGNX_TXM_NAV		0x0030 /* NAV */
+#define AGNX_TXM_CFPMDV		0x0034 /* CFP MDV */
+#define AGNX_TXM_CFPERCNT	0x0038 /* CFP period count */
+#define AGNX_TXM_PROBDELAY	0x003c /* probe delay */
+#define AGNX_TXM_LISINTERCNT	0x0040 /* listen interval count */
+#define AGNX_TXM_DTIMPERICNT	0x004c /* DTIM period count */
+
+#define AGNX_TXM_BEACON_CTL	0x005c /* beacon control */
+
+#define AGNX_TXM_SCHEMPCNT	0x007c /* schedule empty count */
+#define AGNX_TXM_MAXTIMOUT	0x0084 /* max timeout exceed count */
+#define AGNX_TXM_MAXCFPTIM	0x0088 /* max CF poll timeout count */
+#define AGNX_TXM_MAXRXTIME	0x008c /* max RX timeout count */
+#define AGNX_TXM_MAXACKTIM	0x0090	/* max ACK timeout count */
+#define AGNX_TXM_DIF01		0x00a0 /* DIF 0-1 */
+#define AGNX_TXM_DIF23		0x00a4 /* DIF 2-3 */
+#define AGNX_TXM_DIF45		0x00a8 /* DIF 4-5 */
+#define AGNX_TXM_DIF67		0x00ac /* DIF 6-7 */
+#define AGNX_TXM_SIFSPIFS	0x00b0 /* SIFS/PIFS */
+#define AGNX_TXM_TIFSEIFS	0x00b4 /* TIFS/EIFS */
+#define AGNX_TXM_MAXCCACNTSLOT	0x00b8 /* max CCA count slot */
+#define AGNX_TXM_SLOTLIMIT	0x00bc /* slot limit/1 msec limit */
+#define AGNX_TXM_CFPOLLRXTIM	0x00f0 /* CF poll RX timeout count */
+#define AGNX_TXM_CFACKT11B	0x00f4 /* CF ack timeout limit for 11b */
+#define AGNX_TXM_CW0		0x0100 /* CW 0 */
+#define AGNX_TXM_SLBEALIM0	0x0108 /* short/long beacon limit 0 */
+#define AGNX_TXM_CW1		0x0120 /* CW 1 */
+#define AGNX_TXM_SLBEALIM1	0x0128 /* short/long beacon limit 1 */
+#define AGNX_TXM_CW2		0x0140 /* CW 2 */
+#define AGNX_TXM_SLBEALIM2	0x0148 /* short/long beacon limit 2 */
+#define AGNX_TXM_CW3		0x0160 /* CW 3 */
+#define AGNX_TXM_SLBEALIM3	0x0168 /* short/long beacon limit 3 */
+#define AGNX_TXM_CW4		0x0180 /* CW 4 */
+#define AGNX_TXM_SLBEALIM4	0x0188 /* short/long beacon limit 4 */
+#define AGNX_TXM_CW5		0x01a0 /* CW 5 */
+#define AGNX_TXM_SLBEALIM5	0x01a8 /* short/long beacon limit 5 */
+#define AGNX_TXM_CW6		0x01c0 /* CW 6 */
+#define AGNX_TXM_SLBEALIM6	0x01c8 /* short/long beacon limit 6 */
+#define AGNX_TXM_CW7		0x01e0 /* CW 7 */
+#define AGNX_TXM_SLBEALIM7	0x01e8 /* short/long beacon limit 7 */
+#define AGNX_TXM_BEACONTEMP     0x1000	/* beacon template */
+#define AGNX_TXM_STAPOWTEMP	0x1a00 /*  Station Power Template */
+
+/* Receive Management Control Registers */
+#define AGNX_RXM_BASE		0x2000
+#define AGNX_RXM_REQRATE	0x2000	/* requested rate */
+#define AGNX_RXM_MACHI		0x2004	/* first 4 bytes of mac address */
+#define AGNX_RXM_MACLO		0x2008	/* last 2 bytes of mac address */
+#define AGNX_RXM_BSSIDHI	0x200c	/* bssid hi */
+#define AGNX_RXM_BSSIDLO	0x2010	/* bssid lo */
+#define AGNX_RXM_HASH_CMD_FLAG	0x2014	/* Flags for the RX Hash Command Default:0 */
+#define AGNX_RXM_HASH_CMD_HIGH	0x2018	/* The High half of the Hash Command */
+#define AGNX_RXM_HASH_CMD_LOW	0x201c	/* The Low half of the Hash Command */
+#define AGNX_RXM_ROUTAB		0x2020	/* routing table */
+#define		ROUTAB_SUBTYPE_SHIFT	24
+#define		ROUTAB_TYPE_SHIFT	28
+#define		ROUTAB_STATUS_SHIFT	30
+#define		ROUTAB_RW_SHIFT		31
+#define		ROUTAB_ROUTE_DROP	0xf00000 /* Drop */
+#define		ROUTAB_ROUTE_CPU	0x400000 /* CPU */
+#define		ROUTAB_ROUTE_ENCRY	0x500800 /* Encryption */
+#define		ROUTAB_ROUTE_RFP	0x800000 /* RFP */
+
+#define		ROUTAB_TYPE_MANAG	0x0 /* Management */
+#define		ROUTAB_TYPE_CTL		0x1 /* Control */
+#define		ROUTAB_TYPE_DATA	0x2 /* Data */
+
+#define		ROUTAB_SUBTYPE_DATA		0x0
+#define		ROUTAB_SUBTYPE_DATAACK		0x1
+#define		ROUTAB_SUBTYPE_DATAPOLL		0x2
+#define		ROUTAB_SUBTYPE_DATAPOLLACK	0x3
+#define		ROUTAB_SUBTYPE_NULL		0x4 /* NULL */
+#define		ROUTAB_SUBTYPE_NULLACK		0x5
+#define		ROUTAB_SUBTYPE_NULLPOLL		0x6
+#define		ROUTAB_SUBTYPE_NULLPOLLACK	0x7
+#define		ROUTAB_SUBTYPE_QOSDATA		0x8 /* QOS DATA */
+#define		ROUTAB_SUBTYPE_QOSDATAACK	0x9
+#define		ROUTAB_SUBTYPE_QOSDATAPOLL	0xa
+#define		ROUTAB_SUBTYPE_QOSDATAACKPOLL	0xb
+#define		ROUTAB_SUBTYPE_QOSNULL		0xc
+#define		ROUTAB_SUBTYPE_QOSNULLACK	0xd
+#define		ROUTAB_SUBTYPE_QOSNULLPOLL	0xe
+#define		ROUTAB_SUBTYPE_QOSNULLPOLLACK	0xf
+#define AGNX_RXM_DELAY11	   0x2024	/* delay 11(AB) */
+#define AGNX_RXM_SOF_CNT	   0x2028	/* SOF Count */
+#define AGNX_RXM_FRAG_CNT	   0x202c	/* Fragment Count*/
+#define AGNX_RXM_FCS_CNT	   0x2030	/* FCS Count */
+#define AGNX_RXM_BSSID_MISS_CNT	   0x2034	/* BSSID Miss Count */
+#define AGNX_RXM_PDU_ERR_CNT	   0x2038	/* PDU Error Count */
+#define AGNX_RXM_DEST_MISS_CNT	   0x203C	/* Destination Miss Count */
+#define AGNX_RXM_DROP_CNT	   0x2040	/* Drop Count */
+#define AGNX_RXM_ABORT_CNT	   0x2044	/* Abort Count */
+#define AGNX_RXM_RELAY_CNT	   0x2048	/* Relay Count */
+#define AGNX_RXM_HASH_MISS_CNT	   0x204c	/* Hash Miss Count */
+#define AGNX_RXM_SA_HI		   0x2050	/* Address of received packet Hi */
+#define AGNX_RXM_SA_LO		   0x2054	/* Address of received packet Lo */
+#define AGNX_RXM_HASH_DUMP_LST	   0x2100	/* Contains Hash Data */
+#define AGNX_RXM_HASH_DUMP_MST	   0x2104	/* Contains Hash Data */
+#define AGNX_RXM_HASH_DUMP_DATA    0x2108	/* The Station ID to dump */
+
+
+/* Encryption Managment */
+#define AGNX_ENCRY_BASE		0x2400
+#define AGNX_ENCRY_WEPKEY0	0x2440 /* wep key #0 */
+#define AGNX_ENCRY_WEPKEY1	0x2444 /* wep key #1 */
+#define AGNX_ENCRY_WEPKEY2	0x2448 /* wep key #2 */
+#define AGNX_ENCRY_WEPKEY3	0x244c /* wep key #3 */
+#define AGNX_ENCRY_CCMRECTL	0x2460 /* ccm replay control */
+
+
+/* Band Management Registers */
+#define AGNX_BM_BASE		0x2c00
+#define AGNX_BM_BMCTL		0x2c00  /* band management control */
+#define AGNX_BM_TXWADDR		0x2c18  /* tx workqueue address start */
+#define AGNX_BM_TXTOPEER	0x2c24	/* transmit to peers */
+#define AGNX_BM_FPLHP		0x2c2c  /* free pool list head pointer */
+#define AGNX_BM_FPLTP		0x2c30  /* free pool list tail pointer */
+#define AGNX_BM_FPCNT		0x2c34  /* free pool count */
+#define AGNX_BM_CIPDUWCNT	0x2c38  /* card interface pdu workqueue count */
+#define AGNX_BM_SPPDUWCNT	0x2c3c  /* sp pdu workqueue count */
+#define AGNX_BM_RFPPDUWCNT	0x2c40  /* rfp pdu workqueue count */
+#define AGNX_BM_RHPPDUWCNT	0x2c44  /* rhp pdu workqueue count */
+#define AGNX_BM_CIWQCTL		0x2c48 /* Card Interface WorkQueue Control */
+#define AGNX_BM_CPUTXWCTL	0x2c50  /* cpu tx workqueue control */
+#define AGNX_BM_CPURXWCTL	0x2c58  /* cpu rx workqueue control */
+#define AGNX_BM_CPULWCTL	0x2c60 /* cpu low workqueue control */
+#define AGNX_BM_CPUHWCTL	0x2c68 /* cpu high workqueue control */
+#define AGNX_BM_SPTXWCTL	0x2c70 /* sp tx workqueue control */
+#define AGNX_BM_SPRXWCTL	0x2c78 /* sp rx workqueue control */
+#define AGNX_BM_RFPWCTL		0x2c80 /* RFP workqueue control */
+#define AGNX_BM_MTSM		0x2c90 /* Multicast Transmit Station Mask */
+
+/* Card Interface Registers (32bits) */
+#define AGNX_CIR_BASE		0x3000
+#define AGNX_CIR_BLKCTL		0x3000	/* block control*/
+#define		AGNX_STAT_TX	0x1
+#define		AGNX_STAT_RX	0x2
+#define		AGNX_STAT_X	0x4
+/* Below two interrupt flags will be set by our but not CPU or the card */
+#define		AGNX_STAT_TXD	0x10
+#define		AGNX_STAT_TXM	0x20
+
+#define AGNX_CIR_ADDRWIN	0x3004	/* Addressable Windows*/
+#define AGNX_CIR_ENDIAN		0x3008  /* card endianness */
+#define AGNX_CIR_SERIALITF	0x3020	/* serial interface */
+#define AGNX_CIR_RXCFG		0x3040	/* receive config */
+#define		ENABLE_RX_INTERRUPT 0x20
+#define		RX_CACHE_LINE	    0x8
+/* the RX fragment length */
+#define		FRAG_LEN_256	0x0 /* 256B */
+#define		FRAG_LEN_512	0x1
+#define		FRAG_LEN_1024	0x2
+#define		FRAG_LEN_2048	0x3
+#define		FRAG_BE		0x10
+#define AGNX_CIR_RXCTL		0x3050	/* receive control */
+/* memory address, chipside */
+#define AGNX_CIR_RXCMSTART	0x3054	/* receive client memory start */
+#define AGNX_CIR_RXCMEND	0x3058	/* receive client memory end */
+/* memory address, pci */
+#define AGNX_CIR_RXHOSTADDR	0x3060	/* receive hostside address */
+/* memory address, chipside */
+#define AGNX_CIR_RXCLIADDR	0x3064	/* receive clientside address */
+#define AGNX_CIR_RXDMACTL	0x3068	/* receive dma control */
+#define AGNX_CIR_TXCFG		0x3080	/* transmit config */
+#define AGNX_CIR_TXMCTL		0x3090 /* Transmit Management Control */
+#define		ENABLE_TX_INTERRUPT 0x20
+#define		TX_CACHE_LINE	    0x8
+#define AGNX_CIR_TXMSTART	0x3094 /* Transmit Management Start */
+#define AGNX_CIR_TXMEND		0x3098 /* Transmit Management End */
+#define AGNX_CIR_TXDCTL		0x30a0	/* transmit data control */
+/* memeory address, chipset */
+#define AGNX_CIR_TXDSTART	0x30a4	/* transmit data start */
+#define AGNX_CIR_TXDEND		0x30a8	/* transmit data end */
+#define AGNX_CIR_TXMHADDR	0x30b0 /* Transmit Management Hostside Address */
+#define AGNX_CIR_TXMCADDR	0x30b4 /* Transmit Management Clientside Address */
+#define AGNX_CIR_TXDMACTL	0x30b8	/* transmit dma control */
+
+
+/* Power Managment Unit */
+#define AGNX_PM_BASE		0x3c00
+#define AGNX_PM_PMCTL		0x3c00	/* PM Control*/
+#define AGNX_PM_MACMSW		0x3c08 /* MAC Manual Slow Work Enable */
+#define AGNX_PM_RFCTL		0x3c0c /* RF Control */
+#define AGNX_PM_PHYMW		0x3c14	/* Phy Mannal Work */
+#define AGNX_PM_SOFTRST		0x3c18	/* PMU Soft Reset */
+#define AGNX_PM_PLLCTL		0x3c1c	/* PMU PLL control*/
+#define AGNX_PM_TESTPHY		0x3c24 /* PMU Test Phy */
+
+
+/* Interrupt Control interface */
+#define AGNX_INT_BASE		0x4000
+#define AGNX_INT_STAT		0x4000	/* interrupt status */
+#define AGNX_INT_MASK		0x400c	/* interrupt mask */
+/* FIXME */
+#define		IRQ_TX_BEACON	0x1	/* TX Beacon */
+#define		IRQ_TX_RETRY	0x8	/* TX Retry Interrupt */
+#define		IRQ_TX_ACTIVITY	0x10	/* TX Activity */
+#define		IRQ_RX_ACTIVITY	0x20	/* RX Activity */
+/* FIXME I guess that instead RX a none exist staion's packet or
+   the station hasn't been init */
+#define		IRQ_RX_X	0x40
+#define		IRQ_RX_Y	0x80	/* RX ? */
+#define		IRQ_RX_HASHHIT	0x100	/* RX Hash Hit */
+#define		IRQ_RX_FRAME	0x200	/* RX Frame */
+#define		IRQ_ERR_INT	0x400	/* Error Interrupt */
+#define		IRQ_TX_QUE_FULL	0x800	/* TX Workqueue Full */
+#define		IRQ_BANDMAN_ERR	0x10000	/* Bandwidth Management Error */
+#define		IRQ_TX_DISABLE	0x20000	/* TX Disable */
+#define		IRQ_RX_IVASESKEY 0x80000 /* RX Invalid Session Key */
+#define		IRQ_RX_KEYIDMIS	0x100000 /* RX key ID Mismatch */
+#define		IRQ_REP_THHIT	0x200000 /* Replay Threshold Hit */
+#define		IRQ_TIMER1	0x4000000 /* Timer1 */
+#define		IRQ_TIMER_CNT	0x10000000 /* Timer Count */
+#define		IRQ_PHY_FASTINT 0x20000000 /* Phy Fast Interrupt */
+#define		IRQ_PHY_SLOWINT	0x40000000 /* Phy Slow Interrupt */
+#define		IRQ_OTHER	0x80000000 /* Unknow interrupt */
+#define		AGNX_IRQ_ALL   	0xffffffff
+
+/* System Interface */
+#define AGNX_SYSITF_BASE	0x4400
+#define AGNX_SYSITF_SYSMODE	0x4400	/* system mode */
+#define AGNX_SYSITF_GPIOIN	0x4410 /* GPIO In */
+/* PIN lines for leds? */
+#define AGNX_SYSITF_GPIOUT	0x4414	/* GPIO Out */
+
+/* Timer Control */
+#define AGNX_TIMCTL_TIMER1	0x4800 /* Timer 1 */
+#define AGNX_TIMCTL_TIM1CTL	0x4808 /* Timer 1 Control */
+
+
+/* Antenna Calibration Interface */
+#define AGNX_ACI_BASE		0x5000
+#define AGNX_ACI_MODE		0x5000 /* Mode */
+#define AGNX_ACI_MEASURE	0x5004 /* Measure */
+#define AGNX_ACI_SELCHAIN	0x5008 /* Select Chain */
+#define AGNX_ACI_LEN		0x500c /* Length */
+#define AGNX_ACI_TIMER1		0x5018 /* Timer 1 */
+#define AGNX_ACI_TIMER2		0x501c /* Timer 2 */
+#define AGNX_ACI_OFFSET		0x5020 /* Offset */
+#define AGNX_ACI_STATUS		0x5030 /* Status */
+#define		CALI_IDLE	0x0
+#define		CALI_DONE	0x1
+#define		CALI_BUSY	0x2
+#define		CALI_ERR	0x3
+#define AGNX_ACI_AICCHA0OVE	0x5034 /* AIC Channel 0 Override */
+#define AGNX_ACI_AICCHA1OVE	0x5038 /* AIC Channel 1 Override */
+
+/* Gain Control Registers */
+#define AGNX_GCR_BASE		0x9000
+/* threshold of primary antenna */
+#define AGNX_GCR_THD0A		0x9000	/* threshold? D0 A */
+/* low threshold of primary antenna */
+#define AGNX_GCR_THD0AL		0x9004	/* threshold? D0 A low */
+/* threshold of secondary antenna */
+#define AGNX_GCR_THD0B		0x9008	/* threshold? D0_B */
+#define AGNX_GCR_DUNSAT		0x900c /* d unsaturated */
+#define AGNX_GCR_DSAT		0x9010 /* d saturated */
+#define AGNX_GCR_DFIRCAL	0x9014 /* D Fir/Cal */
+#define AGNX_GCR_DGCTL11A	0x9018 /* d gain control 11a */
+#define AGNX_GCR_DGCTL11B	0x901c /* d gain control 11b */
+/* strength of gain */
+#define AGNX_GCR_GAININIT	0x9020	/* gain initialization */
+#define AGNX_GCR_THNOSIG	0x9024 /* threhold no signal */
+#define AGNX_GCR_COARSTEP	0x9028 /* coarse stepping */
+#define AGNX_GCR_SIFST11A	0x902c /* sifx time 11a */
+#define AGNX_GCR_SIFST11B	0x9030 /* sifx time 11b */
+#define AGNX_GCR_CWDETEC	0x9034 /* cw detection */
+#define AGNX_GCR_0X38		0x9038 /* ???? */
+#define AGNX_GCR_BOACT		0x903c	/* BO Active */
+#define AGNX_GCR_BOINACT	0x9040	/* BO Inactive */
+#define AGNX_GCR_BODYNA		0x9044	/* BO dynamic */
+/* 802.11 mode(a,b,g) */
+#define AGNX_GCR_DISCOVMOD	0x9048	/* discovery mode */
+#define AGNX_GCR_NLISTANT	0x904c	/* number of listening antenna */
+#define AGNX_GCR_NACTIANT	0x9050	/* number of active antenna */
+#define AGNX_GCR_NMEASANT	0x9054	/* number of measuring antenna */
+#define AGNX_GCR_NCAPTANT	0x9058	/* number of capture antenna */
+#define AGNX_GCR_THCAP11A	0x905c /* threshold capture 11a */
+#define AGNX_GCR_THCAP11B	0x9060 /* threshold capture 11b */
+#define AGNX_GCR_THCAPRX11A	0x9064 /* threshold capture rx 11a */
+#define AGNX_GCR_THCAPRX11B	0x9068 /* threshold capture rx 11b */
+#define AGNX_GCR_THLEVDRO	0x906c /* threshold level drop */
+#define AGNX_GCR_GAINSET0	0x9070 /* Gainset 0 */
+#define AGNX_GCR_GAINSET1	0x9074 /* Gainset 1 */
+#define AGNX_GCR_GAINSET2	0x9078 /* Gainset 2 */
+#define AGNX_GCR_MAXRXTIME11A	0x907c /* maximum rx time 11a */
+#define AGNX_GCR_MAXRXTIME11B	0x9080 /* maximum rx time 11b */
+#define AGNX_GCR_CORRTIME	0x9084 /* correction time */
+/* reset the subsystem, 0 = disable, 1 = enable */
+#define AGNX_GCR_RSTGCTL	0x9088	/* reset gain control */
+/* channel receiving */
+#define AGNX_GCR_RXCHANEL	0x908c	/* receive channel */
+#define AGNX_GCR_NOISE0		0x9090 /* Noise 0 */
+#define AGNX_GCR_NOISE1		0x9094 /* Noise 1 */
+#define AGNX_GCR_NOISE2		0x9098 /* Noise 2 */
+#define AGNX_GCR_SIGHTH		0x909c	/* Signal High Threshold */
+#define AGNX_GCR_SIGLTH		0x90a0	/* Signal Low Threshold */
+#define AGNX_GCR_CORRDROP	0x90a4 /* correction drop */
+/* threshold of tertiay antenna */
+#define AGNX_GCR_THCD		0x90a8	/* threshold? CD */
+#define AGNX_GCR_THCS		0x90ac	/* threshold? CS */
+#define AGNX_GCR_MAXPOWDIFF	0x90b8 /* maximum power difference */
+#define AGNX_GCR_TRACNT4	0x90ec /* Transition Count 4 */
+#define AGNX_GCR_TRACNT5      	0x90f0	/* transition count 5 */
+#define AGNX_GCR_TRACNT6       	0x90f4	/* transition count 6 */
+#define AGNX_GCR_TRACNT7       	0x90f8	/* transition coutn 7 */
+#define AGNX_GCR_TESTBUS	0x911c /* test bus */
+#define AGNX_GCR_CHAINNUM	0x9120 /* Number of Chains */
+#define AGNX_GCR_ANTCFG		0x9124	/* Antenna Config */
+#define AGNX_GCR_THJUMP		0x912c /* threhold jump */
+#define AGNX_GCR_THPOWER	0x9130 /* threshold power */
+#define AGNX_GCR_THPOWCLIP	0x9134 /* threshold power clip*/
+#define AGNX_GCR_FORCECTLCLK	0x9138 /* Force Gain Control Clock */
+#define AGNX_GCR_GAINSETWRITE	0x913c /* Gainset Write */
+#define AGNX_GCR_THD0BTFEST	0x9140	/* threshold d0 b tf estimate */
+#define AGNX_GCR_THRX11BPOWMIN	0x9144	/* threshold rx 11b power minimum */
+#define AGNX_GCR_0X14c		0x914c /* ?? */
+#define AGNX_GCR_0X150		0x9150 /* ?? */
+#define AGNX_GCR_RXOVERIDE	0x9194	/* recieve override */
+#define AGNX_GCR_WATCHDOG	0x91b0	/* watchdog timeout */
+
+
+/* Spi Interface */
+#define AGNX_SPI_BASE		0xdc00
+#define AGNX_SPI_CFG		0xdc00 /* spi configuration */
+/* Only accept 16 bits */
+#define AGNX_SPI_WMSW		0xdc04	/* write most significant word */
+/* Only accept 16 bits */
+#define AGNX_SPI_WLSW		0xdc08	/* write least significant word */
+#define AGNX_SPI_CTL		0xdc0c	/* spi control */
+#define AGNX_SPI_RMSW		0xdc10 /* read most significant word */
+#define AGNX_SPI_RLSW		0xdc14 /* read least significant word */
+/* SPI Control Mask */
+#define		SPI_READ_CTL		0x4000 /* read control */
+#define		SPI_BUSY_CTL		0x8000 /* busy control */
+/* RF and synth chips in spi */
+#define		RF_CHIP0	0x400
+#define		RF_CHIP1	0x800
+#define		RF_CHIP2	0x1000
+#define		SYNTH_CHIP	0x2000
+
+/* Unknown register */
+#define AGNX_UNKNOWN_BASE	0x7800
+
+/* FIXME MonitorGain */
+#define AGNX_MONGCR_BASE	0x12000
+
+/* Gain Table */
+#define AGNX_GAIN_TABLE		0x12400
+
+/* The initial FIR coefficient table */
+#define AGNX_FIR_BASE		0x19804
+
+#define AGNX_ENGINE_LOOKUP_TBL	0x800
+
+/* eeprom commands */
+#define EEPROM_CMD_NULL		0x0 /* NULL */
+#define EEPROM_CMD_WRITE	0x2 /* write */
+#define EEPROM_CMD_READ		0x3 /* read */
+#define EEPROM_CMD_STATUSREAD	0x5 /* status register read */
+#define EEPROM_CMD_WRITEENABLE	0x6 /* write enable */
+#define EEPROM_CMD_CONFIGURE	0x7 /* configure */
+
+#define EEPROM_DATAFORCOFIGURE	0x6 /* ??? */
+
+/* eeprom address */
+#define EEPROM_ADDR_SUBVID	0x0 /* Sub Vendor ID */
+#define EEPROM_ADDR_SUBSID	0x2 /* Sub System ID */
+#define EEPROM_ADDR_MACADDR	0x146 /* MAC Address */
+#define EEPROM_ADDR_LOTYPE	0x14f /* LO type */
+
+struct agnx_eeprom {
+	u8 data;	/* date */
+	u16 address;	/* address in EEPROM */
+	u8 cmd;		/* command, unknown, status */
+}  __attribute__((__packed__));
+
+#define AGNX_EEPROM_COMMAND_SHIFT	5
+#define AGNX_EEPROM_COMMAND_STAT	0x01
+
+void disable_receiver(struct agnx_priv *priv);
+void enable_receiver(struct agnx_priv *priv);
+u8 read_from_eeprom(struct agnx_priv *priv, u16 address);
+void agnx_hw_init(struct agnx_priv *priv);
+int agnx_hw_reset(struct agnx_priv *priv);
+int agnx_set_ssid(struct agnx_priv *priv, u8 *ssid, size_t ssid_len);
+void agnx_set_bssid(struct agnx_priv *priv, u8 *bssid);
+void enable_power_saving(struct agnx_priv *priv);
+void disable_power_saving(struct agnx_priv *priv);
+void calibrate_antenna_period(unsigned long data);
+
+#endif /* AGNX_PHY_H_ */
diff --git a/drivers/staging/agnx/rf.c b/drivers/staging/agnx/rf.c
new file mode 100644
index 000000000000..8294b6e2eb9d
--- /dev/null
+++ b/drivers/staging/agnx/rf.c
@@ -0,0 +1,894 @@
+/**
+ * Airgo MIMO wireless driver
+ *
+ * Copyright (c) 2007 Li YanBo <dreamfly281@gmail.com>
+
+ * Thanks for Jeff Williams <angelbane@gmail.com> do reverse engineer
+ * works and published the SPECS at http://airgo.wdwconsulting.net/mymoin
+
+ * 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/pci.h>
+#include <linux/delay.h>
+#include "agnx.h"
+#include "debug.h"
+#include "phy.h"
+#include "table.h"
+
+/* FIXME! */
+static inline void spi_write(void __iomem *region, u32 chip_ids, u32 sw,
+		      u16 size, u32 control)
+{
+	u32 reg;
+	u32 lsw = sw & 0xffff;		/* lower 16 bits of sw*/
+	u32 msw = sw >> 16;		/* high 16 bits of sw */
+
+	/* FIXME Write Most Significant Word of the 32bit data to MSW */
+	/* FIXME And Least Significant Word to LSW */
+	iowrite32((lsw), region + AGNX_SPI_WLSW);
+	iowrite32((msw), region + AGNX_SPI_WMSW);
+	reg = chip_ids | size | control;
+	/* Write chip id(s), write size and busy control to Control Register */
+	iowrite32((reg), region + AGNX_SPI_CTL);
+	/* Wait for Busy control to clear */
+	spi_delay();
+}
+
+/*
+ * Write to SPI Synth register
+ */
+static inline void spi_sy_write(void __iomem *region, u32 chip_ids, u32 sw)
+{
+	/* FIXME the size 0x15 is a magic value*/
+	spi_write(region, chip_ids, sw, 0x15, SPI_BUSY_CTL);
+}
+
+/*
+ * Write to SPI RF register
+ */
+static inline void spi_rf_write(void __iomem *region, u32 chip_ids, u32 sw)
+{
+	/* FIXME the size 0xd is a magic value*/
+	spi_write(region, chip_ids, sw, 0xd, SPI_BUSY_CTL);
+} /* spi_rf_write */
+
+/*
+ * Write to SPI with Read Control bit set
+ */
+inline void spi_rc_write(void __iomem *region, u32 chip_ids, u32 sw)
+{
+	/* FIXME the size 0xe5 is a magic value */
+	spi_write(region, chip_ids, sw, 0xe5, SPI_BUSY_CTL|SPI_READ_CTL);
+}
+
+/* Get the active chains's count */
+static int get_active_chains(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	int num = 0;
+	u32 reg;
+	AGNX_TRACE;
+
+	spi_rc_write(ctl, RF_CHIP0, 0x21);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+	if (reg == 1)
+		num++;
+
+	spi_rc_write(ctl, RF_CHIP1, 0x21);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+	if (reg == 1)
+		num++;
+
+	spi_rc_write(ctl, RF_CHIP2, 0x21);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+	if (reg == 1)
+		num++;
+
+	spi_rc_write(ctl, RF_CHIP0, 0x26);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+	if (0x33 != reg)
+		printk(KERN_WARNING PFX "Unmatched rf chips result\n");
+
+	return num;
+} /* get_active_chains */
+
+void rf_chips_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	int num;
+	AGNX_TRACE;
+
+	if (priv->revid == 1) {
+		reg = agnx_read32(ctl, AGNX_SYSITF_GPIOUT);
+		reg |= 0x8;
+		agnx_write32(ctl, AGNX_SYSITF_GPIOUT, reg);
+	}
+
+	/* Set SPI clock speed to 200NS */
+        reg = agnx_read32(ctl, AGNX_SPI_CFG);
+        reg &= ~0xF;
+        reg |= 0x3;
+        agnx_write32(ctl, AGNX_SPI_CFG, reg);
+
+        /* Set SPI clock speed to 50NS */
+	reg = agnx_read32(ctl, AGNX_SPI_CFG);
+	reg &= ~0xF;
+	reg |= 0x1;
+	agnx_write32(ctl, AGNX_SPI_CFG, reg);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1101);
+
+	num = get_active_chains(priv);
+	printk(KERN_INFO PFX "Active chains are %d\n", num);
+
+	reg = agnx_read32(ctl, AGNX_SPI_CFG);
+	reg &= ~0xF;
+	agnx_write32(ctl, AGNX_SPI_CFG, reg);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1908);
+} /* rf_chips_init */
+
+
+static u32 channel_tbl[15][9] = {
+	{0,  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+	{1,  0x00, 0x00, 0x624, 0x00, 0x1a4, 0x28, 0x00, 0x1e},
+	{2,  0x00, 0x00, 0x615, 0x00, 0x1ae, 0x28, 0x00, 0x1e},
+	{3,  0x00, 0x00, 0x61a, 0x00, 0x1ae, 0x28, 0x00, 0x1e},
+	{4,  0x00, 0x00, 0x61f, 0x00, 0x1ae, 0x28, 0x00, 0x1e},
+	{5,  0x00, 0x00, 0x624, 0x00, 0x1ae, 0x28, 0x00, 0x1e},
+	{6,  0x00, 0x00, 0x61f, 0x00, 0x1b3, 0x28, 0x00, 0x1e},
+	{7,  0x00, 0x00, 0x624, 0x00, 0x1b3, 0x28, 0x00, 0x1e},
+	{8,  0x00, 0x00, 0x629, 0x00, 0x1b3, 0x28, 0x00, 0x1e},
+	{9,  0x00, 0x00, 0x624, 0x00, 0x1b8, 0x28, 0x00, 0x1e},
+	{10, 0x00, 0x00, 0x629, 0x00, 0x1b8, 0x28, 0x00, 0x1e},
+	{11, 0x00, 0x00, 0x62e, 0x00, 0x1b8, 0x28, 0x00, 0x1e},
+	{12, 0x00, 0x00, 0x633, 0x00, 0x1b8, 0x28, 0x00, 0x1e},
+	{13, 0x00, 0x00, 0x628, 0x00, 0x1b8, 0x28, 0x00, 0x1e},
+	{14, 0x00, 0x00, 0x644, 0x00, 0x1b8, 0x28, 0x00, 0x1e},
+};
+
+
+static inline void
+channel_tbl_write(struct agnx_priv *priv, unsigned int channel, unsigned int reg_num)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	reg = channel_tbl[channel][reg_num];
+	reg <<= 4;
+	reg |= reg_num;
+	spi_sy_write(ctl, SYNTH_CHIP, reg);
+}
+
+static void synth_freq_set(struct agnx_priv *priv, unsigned int channel)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1201);
+
+	/* Set the Clock bits to 50NS */
+	reg = agnx_read32(ctl, AGNX_SPI_CFG);
+	reg &= ~0xF;
+	reg |= 0x1;
+	agnx_write32(ctl, AGNX_SPI_CFG, reg);
+
+	/* Write 0x00c0 to LSW and 0x3 to MSW of Synth Chip */
+	spi_sy_write(ctl, SYNTH_CHIP, 0x300c0);
+
+	spi_sy_write(ctl, SYNTH_CHIP, 0x32);
+
+	/* # Write to Register 1 on the Synth Chip */
+	channel_tbl_write(priv, channel, 1);
+	/* # Write to Register 3 on the Synth Chip */
+	channel_tbl_write(priv, channel, 3);
+	/* # Write to Register 6 on the Synth Chip */
+	channel_tbl_write(priv, channel, 6);
+	/* # Write to Register 5 on the Synth Chip */
+	channel_tbl_write(priv, channel, 5);
+	/* # Write to register 8 on the Synth Chip */
+	channel_tbl_write(priv, channel, 8);
+
+	/* FIXME Clear the clock bits */
+	reg = agnx_read32(ctl, AGNX_SPI_CFG);
+	reg &= ~0xf;
+	agnx_write32(ctl, AGNX_SPI_CFG, reg);
+} /* synth_chip_init */
+
+
+static void antenna_init(struct agnx_priv *priv, int num_antenna)
+{
+	void __iomem *ctl = priv->ctl;
+
+	switch (num_antenna) {
+	case 1:
+		agnx_write32(ctl, AGNX_GCR_NLISTANT, 1);
+		agnx_write32(ctl, AGNX_GCR_NMEASANT, 1);
+		agnx_write32(ctl, AGNX_GCR_NACTIANT, 1);
+		agnx_write32(ctl, AGNX_GCR_NCAPTANT, 1);
+
+		agnx_write32(ctl, AGNX_GCR_ANTCFG, 7);
+		agnx_write32(ctl, AGNX_GCR_BOACT, 34);
+		agnx_write32(ctl, AGNX_GCR_BOINACT, 34);
+		agnx_write32(ctl, AGNX_GCR_BODYNA, 30);
+
+		agnx_write32(ctl, AGNX_GCR_THD0A, 125);
+		agnx_write32(ctl, AGNX_GCR_THD0AL, 100);
+		agnx_write32(ctl, AGNX_GCR_THD0B, 90);
+
+		agnx_write32(ctl, AGNX_GCR_THD0BTFEST, 80);
+		agnx_write32(ctl, AGNX_GCR_SIGHTH, 100);
+		agnx_write32(ctl, AGNX_GCR_SIGLTH, 16);
+		break;
+	case 2:
+		agnx_write32(ctl, AGNX_GCR_NLISTANT, 2);
+		agnx_write32(ctl, AGNX_GCR_NMEASANT, 2);
+		agnx_write32(ctl, AGNX_GCR_NACTIANT, 2);
+		agnx_write32(ctl, AGNX_GCR_NCAPTANT, 2);
+		agnx_write32(ctl, AGNX_GCR_ANTCFG, 15);
+		agnx_write32(ctl, AGNX_GCR_BOACT, 36);
+		agnx_write32(ctl, AGNX_GCR_BOINACT, 36);
+		agnx_write32(ctl, AGNX_GCR_BODYNA, 32);
+		agnx_write32(ctl, AGNX_GCR_THD0A, 120);
+		agnx_write32(ctl, AGNX_GCR_THD0AL, 100);
+		agnx_write32(ctl, AGNX_GCR_THD0B, 80);
+		agnx_write32(ctl, AGNX_GCR_THD0BTFEST, 70);
+		agnx_write32(ctl, AGNX_GCR_SIGHTH, 100);
+		agnx_write32(ctl, AGNX_GCR_SIGLTH, 32);
+		break;
+	case 3:
+		agnx_write32(ctl, AGNX_GCR_NLISTANT, 3);
+		agnx_write32(ctl, AGNX_GCR_NMEASANT, 3);
+		agnx_write32(ctl, AGNX_GCR_NACTIANT, 3);
+		agnx_write32(ctl, AGNX_GCR_NCAPTANT, 3);
+		agnx_write32(ctl, AGNX_GCR_ANTCFG, 31);
+		agnx_write32(ctl, AGNX_GCR_BOACT, 36);
+		agnx_write32(ctl, AGNX_GCR_BOINACT, 36);
+		agnx_write32(ctl, AGNX_GCR_BODYNA, 32);
+		agnx_write32(ctl, AGNX_GCR_THD0A, 100);
+		agnx_write32(ctl, AGNX_GCR_THD0AL, 100);
+		agnx_write32(ctl, AGNX_GCR_THD0B, 70);
+		agnx_write32(ctl, AGNX_GCR_THD0BTFEST, 70);
+		agnx_write32(ctl, AGNX_GCR_SIGHTH, 100);
+		agnx_write32(ctl, AGNX_GCR_SIGLTH, 48);
+//		agnx_write32(ctl, AGNX_GCR_SIGLTH, 16);
+		break;
+	default:
+		printk(KERN_WARNING PFX "Unknow antenna number\n");
+	}
+} /* antenna_init */
+
+static void chain_update(struct agnx_priv *priv, u32 chain)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	spi_rc_write(ctl, RF_CHIP0, 0x20);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+
+	if (reg == 0x4)
+		spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, reg|0x1000);
+	else if (reg != 0x0)
+     		spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, reg|0x1000);
+        else {
+		if (chain == 3 || chain == 6) {
+			spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, reg|0x1000);
+			agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x0);
+		} else if (chain == 2 || chain == 4) {
+			spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, reg|0x1000);
+			spi_rf_write(ctl, RF_CHIP2, 0x1005);
+			agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x824);
+		} else if (chain == 1) {
+			spi_rf_write(ctl, RF_CHIP0, reg|0x1000);
+			spi_rf_write(ctl, RF_CHIP1|RF_CHIP2, 0x1004);
+			agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0xc36);
+		}
+	}
+
+	spi_rc_write(ctl, RF_CHIP0, 0x22);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+
+	switch (reg) {
+	case 0:
+		spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1005);
+		break;
+	case 1:
+		spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1201);
+		break;
+	case 2:
+		if (chain == 6 || chain == 4) {
+			spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1202);
+			spi_rf_write(ctl, RF_CHIP2, 0x1005);
+		} else if (chain < 3) {
+			spi_rf_write(ctl, RF_CHIP0, 0x1202);
+			spi_rf_write(ctl, RF_CHIP1|RF_CHIP2, 0x1005);
+		}
+		break;
+	default:
+		if (chain == 3) {
+			spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1203);
+			spi_rf_write(ctl, RF_CHIP2, 0x1201);
+		} else if (chain == 2) {
+			spi_rf_write(ctl, RF_CHIP0, 0x1203);
+			spi_rf_write(ctl, RF_CHIP2, 0x1200);
+			spi_rf_write(ctl, RF_CHIP1, 0x1201);
+		} else if (chain == 1) {
+			spi_rf_write(ctl, RF_CHIP0, 0x1203);
+			spi_rf_write(ctl, RF_CHIP1|RF_CHIP2, 0x1200);
+		} else if (chain == 4) {
+			spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1203);
+			spi_rf_write(ctl, RF_CHIP2, 0x1201);
+		} else {
+			spi_rf_write(ctl, RF_CHIP0, 0x1203);
+			spi_rf_write(ctl, RF_CHIP1|RF_CHIP2, 0x1201);
+		}
+	}
+} /* chain_update */
+
+static void antenna_config(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	/* Write 0x0 to the TX Management Control Register Enable bit */
+	reg = agnx_read32(ctl, AGNX_TXM_CTL);
+	reg &= ~0x1;
+	agnx_write32(ctl, AGNX_TXM_CTL, reg);
+
+	/* FIXME */
+	/* Set initial value based on number of Antennae */
+	antenna_init(priv, 3);
+
+	/* FIXME Update Power Templates for current valid Stations */
+	/* sta_power_init(priv, 0);*/
+
+	/* FIXME the number of chains should get from eeprom*/
+	chain_update(priv, AGNX_CHAINS_MAX);
+} /* antenna_config */
+
+void calibrate_oscillator(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	spi_rc_write(ctl, RF_CHIP0|RF_CHIP1, 0x1201);
+	reg = agnx_read32(ctl, AGNX_GCR_GAINSET1);
+	reg |= 0x10;
+	agnx_write32(ctl, AGNX_GCR_GAINSET1, reg);
+
+	agnx_write32(ctl, AGNX_GCR_GAINSETWRITE, 1);
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 1);
+
+	agnx_write32(ctl, AGNX_ACI_LEN, 0x3ff);
+
+	agnx_write32(ctl, AGNX_ACI_TIMER1, 0x27);
+	agnx_write32(ctl, AGNX_ACI_TIMER2, 0x27);
+	/* (Residual DC Calibration) to Calibration Mode */
+	agnx_write32(ctl, AGNX_ACI_MODE, 0x2);
+
+	spi_rc_write(ctl, RF_CHIP0|RF_CHIP1, 0x1004);
+	agnx_write32(ctl, AGNX_ACI_LEN, 0x3ff);
+	/* (TX LO Calibration) to Calibration Mode */
+	agnx_write32(ctl, AGNX_ACI_MODE, 0x4);
+
+	do {
+		u32  reg1, reg2, reg3;
+		/* Enable Power Saving Control */
+		enable_power_saving(priv);
+		/* Save the following registers to restore */
+		reg1 = ioread32(ctl + 0x11000);
+		reg2 = ioread32(ctl + 0xec50);
+		reg3 = ioread32(ctl + 0xec54);
+		wmb();
+
+		agnx_write32(ctl, 0x11000, 0xcfdf);
+		agnx_write32(ctl, 0xec50, 0x70);
+		/* Restore the registers */
+		agnx_write32(ctl, 0x11000, reg1);
+		agnx_write32(ctl, 0xec50, reg2);
+		agnx_write32(ctl, 0xec54, reg3);
+		/* Disable Power Saving Control */
+		disable_power_saving(priv);
+	} while (0);
+
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0);
+} /* calibrate_oscillator */
+
+
+static void radio_channel_set(struct agnx_priv *priv, unsigned int channel)
+{
+	void __iomem *ctl = priv->ctl;
+	unsigned int freq = priv->band.channels[channel - 1].center_freq;
+	u32 reg;
+	AGNX_TRACE;
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1201);
+	/* Set SPI Clock to 50 Ns */
+	reg = agnx_read32(ctl, AGNX_SPI_CFG);
+	reg &= ~0xF;
+	reg |= 0x1;
+	agnx_write32(ctl, AGNX_SPI_CFG, reg);
+
+	/* Clear the Disable Tx interrupt bit in Interrupt Mask */
+/* 	reg = agnx_read32(ctl, AGNX_INT_MASK); */
+/* 	reg &= ~IRQ_TX_DISABLE; */
+/* 	agnx_write32(ctl, AGNX_INT_MASK, reg); */
+
+	/* Band Selection */
+	reg = agnx_read32(ctl, AGNX_SYSITF_GPIOUT);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_SYSITF_GPIOUT, reg);
+
+	/* FIXME Set the SiLabs Chip Frequency */
+	synth_freq_set(priv, channel);
+
+	reg = agnx_read32(ctl, AGNX_PM_SOFTRST);
+	reg |= 0x80100030;
+	agnx_write32(ctl, AGNX_PM_SOFTRST, reg);
+	reg = agnx_read32(ctl, AGNX_PM_PLLCTL);
+	reg |= 0x20009;
+	agnx_write32(ctl, AGNX_PM_PLLCTL, reg);
+
+	agnx_write32(ctl, AGNX_SYSITF_GPIOUT, 0x5);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1100);
+
+	/* Load the MonitorGain Table */
+	monitor_gain_table_init(priv);
+
+	/* Load the TX Fir table */
+	tx_fir_table_init(priv);
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+
+	spi_rc_write(ctl, RF_CHIP0|RF_CHIP1, 0x22);
+	udelay(80);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+
+
+	agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0xff);
+	agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x3);
+
+	reg = agnx_read32(ctl, 0xec50);
+	reg |= 0x4f;
+	agnx_write32(ctl, 0xec50, reg);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1201);
+	agnx_write32(ctl, 0x11008, 0x1);
+	agnx_write32(ctl, 0x1100c, 0x0);
+	agnx_write32(ctl, 0x11008, 0x0);
+	agnx_write32(ctl, 0xec50, 0xc);
+
+	agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x3);
+	agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x0);
+	agnx_write32(ctl, 0x11010, 0x6e);
+	agnx_write32(ctl, 0x11014, 0x6c);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1201);
+
+	/* Calibrate the Antenna */
+	/* antenna_calibrate(priv); */
+	/* Calibrate the TxLocalOscillator */
+	calibrate_oscillator(priv);
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg &= ~0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+	agnx_write32(ctl, AGNX_GCR_GAININIT, 0xa);
+	agnx_write32(ctl, AGNX_GCR_THCD, 0x0);
+
+	agnx_write32(ctl, 0x11018, 0xb);
+	agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x0);
+
+	/* Write Frequency to Gain Control Channel */
+	agnx_write32(ctl, AGNX_GCR_RXCHANEL, freq);
+	/* Write 0x140000/Freq to 0x9c08 */
+	reg = 0x140000/freq;
+	agnx_write32(ctl, 0x9c08, reg);
+
+	reg = agnx_read32(ctl, AGNX_PM_SOFTRST);
+	reg &= ~0x80100030;
+	agnx_write32(ctl, AGNX_PM_SOFTRST, reg);
+
+	reg = agnx_read32(ctl, AGNX_PM_PLLCTL);
+	reg &= ~0x20009;
+	reg |= 0x1;
+	agnx_write32(ctl, AGNX_PM_PLLCTL, reg);
+
+	agnx_write32(ctl, AGNX_ACI_MODE, 0x0);
+
+/* FIXME According to Number of Chains: */
+
+/* 			   1. 1: */
+/*          1. Write 0x1203 to RF Chip 0 */
+/*          2. Write 0x1200 to RF Chips 1 +2  */
+/* 			   2. 2: */
+/*          1. Write 0x1203 to RF Chip 0 */
+/*          2. Write 0x1200 to RF Chip 2 */
+/*          3. Write 0x1201 to RF Chip 1  */
+/* 			   3. 3: */
+/*          1. Write 0x1203 to RF Chip 0 */
+/*          2. Write 0x1201 to RF Chip 1 + 2  */
+/* 			   4. 4: */
+/*          1. Write 0x1203 to RF Chip 0 + 1 */
+/*          2. Write 0x1200 to RF Chip 2  */
+
+/* 			   5. 6: */
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1203);
+	spi_rf_write(ctl, RF_CHIP2, 0x1201);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1000);
+	agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x0);
+
+	/* FIXME Set the Disable Tx interrupt bit in Interrupt Mask
+	   (Or 0x20000 to Interrupt Mask) */
+/* 	reg = agnx_read32(ctl, AGNX_INT_MASK); */
+/* 	reg |= IRQ_TX_DISABLE; */
+/* 	agnx_write32(ctl, AGNX_INT_MASK, reg); */
+
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x1);
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x0);
+
+	/* Configure the Antenna */
+	antenna_config(priv);
+
+	/* Write 0x0 to Discovery Mode Enable detect G, B, A packet? */
+	agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0);
+
+	reg = agnx_read32(ctl, AGNX_RXM_REQRATE);
+	reg |= 0x80000000;
+	agnx_write32(ctl, AGNX_RXM_REQRATE, reg);
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x1);
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x0);
+
+	/* enable radio on and the power LED */
+	reg = agnx_read32(ctl, AGNX_SYSITF_GPIOUT);
+	reg &= ~0x1;
+	reg |= 0x2;
+	agnx_write32(ctl, AGNX_SYSITF_GPIOUT, reg);
+
+	reg = agnx_read32(ctl, AGNX_TXM_CTL);
+	reg |= 0x1;
+	agnx_write32(ctl, AGNX_TXM_CTL, reg);
+} /* radio_channel_set */
+
+static void base_band_filter_calibrate(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1700);
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1001);
+	agnx_write32(ctl, AGNX_GCR_FORCECTLCLK, 0x0);
+	spi_rc_write(ctl, RF_CHIP0, 0x27);
+	spi_rc_write(ctl, RF_CHIP1, 0x27);
+	spi_rc_write(ctl, RF_CHIP2, 0x27);
+	agnx_write32(ctl, AGNX_GCR_FORCECTLCLK, 0x1);
+}
+
+static void print_offset(struct agnx_priv *priv, u32 chain)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 offset;
+
+	iowrite32((chain), ctl + AGNX_ACI_SELCHAIN);
+	udelay(10);
+	offset = (ioread32(ctl + AGNX_ACI_OFFSET));
+	printk(PFX "Chain is 0x%x, Offset is 0x%x\n", chain, offset);
+}
+
+void print_offsets(struct agnx_priv *priv)
+{
+	print_offset(priv, 0);
+	print_offset(priv, 4);
+	print_offset(priv, 1);
+	print_offset(priv, 5);
+	print_offset(priv, 2);
+	print_offset(priv, 6);
+}
+
+
+struct chains {
+	u32 cali;		/* calibrate  value*/
+
+#define  NEED_CALIBRATE		0
+#define  SUCCESS_CALIBRATE	1
+	int status;
+};
+
+static void chain_calibrate(struct agnx_priv *priv, struct chains *chains,
+			    unsigned int num)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 calibra = chains[num].cali;
+
+	if (num < 3)
+		calibra |= 0x1400;
+	else
+		calibra |= 0x1500;
+
+	switch (num) {
+	case 0:
+	case 4:
+		spi_rf_write(ctl, RF_CHIP0, calibra);
+		break;
+	case 1:
+	case 5:
+		spi_rf_write(ctl, RF_CHIP1, calibra);
+		break;
+	case 2:
+	case 6:
+		spi_rf_write(ctl, RF_CHIP2, calibra);
+		break;
+	default:
+		BUG();
+	}
+} /* chain_calibrate */
+
+
+static void inline get_calibrete_value(struct agnx_priv *priv, struct chains *chains,
+				       unsigned int num)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 offset;
+
+	iowrite32((num), ctl + AGNX_ACI_SELCHAIN);
+	/* FIXME */
+	udelay(10);
+	offset = (ioread32(ctl + AGNX_ACI_OFFSET));
+
+	if (offset < 0xf) {
+		chains[num].status = SUCCESS_CALIBRATE;
+		return;
+	}
+
+	if (num == 0 || num == 1 || num == 2) {
+		if ( 0 == chains[num].cali)
+			chains[num].cali = 0xff;
+		else
+			chains[num].cali--;
+	} else
+		chains[num].cali++;
+
+	chains[num].status = NEED_CALIBRATE;
+}
+
+static inline void calibra_delay(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	unsigned int i = 100;
+
+	wmb();
+	while (i--) {
+		reg = (ioread32(ctl + AGNX_ACI_STATUS));
+		if (reg == 0x4000)
+			break;
+		udelay(10);
+	}
+	if (!i)
+		printk(PFX "calibration failed\n");
+}
+
+void do_calibration(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	struct chains chains[7];
+	unsigned int i, j;
+	AGNX_TRACE;
+
+	for (i = 0; i < 7; i++) {
+		if (i == 3)
+			continue;
+
+		chains[i].cali = 0x7f;
+		chains[i].status = NEED_CALIBRATE;
+	}
+
+	/* FIXME 0x300 is a magic number */
+	for (j = 0; j < 0x300; j++) {
+		if (chains[0].status == SUCCESS_CALIBRATE &&
+		    chains[1].status == SUCCESS_CALIBRATE &&
+		    chains[2].status == SUCCESS_CALIBRATE &&
+		    chains[4].status == SUCCESS_CALIBRATE &&
+		    chains[5].status == SUCCESS_CALIBRATE &&
+		    chains[6].status == SUCCESS_CALIBRATE)
+			break;
+
+		/* Attention, there is no chain 3 */
+		for (i = 0; i < 7; i++) {
+			if (i == 3)
+				continue;
+			if (chains[i].status == NEED_CALIBRATE)
+				chain_calibrate(priv, chains, i);
+		}
+		/* Write 0x1 to Calibration Measure */
+		iowrite32((0x1), ctl + AGNX_ACI_MEASURE);
+		calibra_delay(priv);
+
+		for (i = 0; i < 7; i++) {
+			if (i == 3)
+				continue;
+
+			get_calibrete_value(priv, chains, i);
+		}
+	}
+	printk(PFX "Clibrate times is %d\n", j);
+	print_offsets(priv);
+} /* do_calibration */
+
+void antenna_calibrate(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	agnx_write32(ctl, AGNX_GCR_NLISTANT, 0x3);
+	agnx_write32(ctl, AGNX_GCR_NMEASANT, 0x3);
+	agnx_write32(ctl, AGNX_GCR_NACTIANT, 0x3);
+	agnx_write32(ctl, AGNX_GCR_NCAPTANT, 0x3);
+
+	agnx_write32(ctl, AGNX_GCR_ANTCFG, 0x1f);
+	agnx_write32(ctl, AGNX_GCR_BOACT, 0x24);
+	agnx_write32(ctl, AGNX_GCR_BOINACT, 0x24);
+	agnx_write32(ctl, AGNX_GCR_BODYNA, 0x20);
+	agnx_write32(ctl, AGNX_GCR_THD0A, 0x64);
+	agnx_write32(ctl, AGNX_GCR_THD0AL, 0x64);
+	agnx_write32(ctl, AGNX_GCR_THD0B, 0x46);
+	agnx_write32(ctl, AGNX_GCR_THD0BTFEST, 0x3c);
+	agnx_write32(ctl, AGNX_GCR_SIGHTH, 0x64);
+	agnx_write32(ctl, AGNX_GCR_SIGLTH, 0x30);
+
+	spi_rc_write(ctl, RF_CHIP0, 0x20);
+	/* Fixme */
+	udelay(80);
+	/*    1. Should read 0x0  */
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+	if (0x0 != reg)
+		printk(KERN_WARNING PFX "Unmatched rf chips result\n");
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1000);
+
+	agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x0);
+
+	spi_rc_write(ctl, RF_CHIP0, 0x22);
+	udelay(80);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+	if (0x0 != reg)
+		printk(KERN_WARNING PFX "Unmatched rf chips result\n");
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1005);
+
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x1);
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x0);
+
+	reg = agnx_read32(ctl, AGNX_PM_SOFTRST);
+	reg |= 0x1c000032;
+	agnx_write32(ctl, AGNX_PM_SOFTRST, reg);
+	reg = agnx_read32(ctl, AGNX_PM_PLLCTL);
+	reg |= 0x0003f07;
+	agnx_write32(ctl, AGNX_PM_PLLCTL, reg);
+
+	reg = agnx_read32(ctl, 0xec50);
+	reg |= 0x40;
+	agnx_write32(ctl, 0xec50, reg);
+
+	agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0xff8);
+	agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x3);
+
+	agnx_write32(ctl, AGNX_GCR_CHAINNUM, 0x6);
+	agnx_write32(ctl, 0x19874, 0x0);
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1700);
+
+	/* Calibrate the BaseBandFilter */
+	base_band_filter_calibrate(priv);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1002);
+
+	agnx_write32(ctl, AGNX_GCR_GAINSET0, 0x1d);
+	agnx_write32(ctl, AGNX_GCR_GAINSET1, 0x1d);
+	agnx_write32(ctl, AGNX_GCR_GAINSET2, 0x1d);
+	agnx_write32(ctl, AGNX_GCR_GAINSETWRITE, 0x1);
+
+	agnx_write32(ctl, AGNX_ACI_MODE, 0x1);
+	agnx_write32(ctl, AGNX_ACI_LEN, 0x3ff);
+
+	agnx_write32(ctl, AGNX_ACI_TIMER1, 0x27);
+	agnx_write32(ctl, AGNX_ACI_TIMER2, 0x27);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1400);
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1500);
+
+	/* Measure Calibration */
+	agnx_write32(ctl, AGNX_ACI_MEASURE, 0x1);
+	calibra_delay(priv);
+
+	/* do calibration */
+	do_calibration(priv);
+
+	agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x0);
+	agnx_write32(ctl, AGNX_ACI_TIMER1, 0x21);
+	agnx_write32(ctl, AGNX_ACI_TIMER2, 0x27);
+	agnx_write32(ctl, AGNX_ACI_LEN, 0xf);
+
+	reg = agnx_read32(ctl, AGNX_GCR_GAINSET0);
+	reg &= 0xf;
+	agnx_write32(ctl, AGNX_GCR_GAINSET0, reg);
+	reg = agnx_read32(ctl, AGNX_GCR_GAINSET1);
+	reg &= 0xf;
+	agnx_write32(ctl, AGNX_GCR_GAINSET1, reg);
+	reg = agnx_read32(ctl, AGNX_GCR_GAINSET2);
+	reg &= 0xf;
+	agnx_write32(ctl, AGNX_GCR_GAINSET2, reg);
+
+	agnx_write32(ctl, AGNX_GCR_GAINSETWRITE, 0x0);
+	disable_receiver(priv);
+} /* antenna_calibrate */
+
+void __antenna_calibrate(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	/* Calibrate the BaseBandFilter */
+	/* base_band_filter_calibrate(priv); */
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1002);
+
+
+	agnx_write32(ctl, AGNX_GCR_GAINSET0, 0x1d);
+	agnx_write32(ctl, AGNX_GCR_GAINSET1, 0x1d);
+	agnx_write32(ctl, AGNX_GCR_GAINSET2, 0x1d);
+
+	agnx_write32(ctl, AGNX_GCR_GAINSETWRITE, 0x1);
+
+	agnx_write32(ctl, AGNX_ACI_MODE, 0x1);
+	agnx_write32(ctl, AGNX_ACI_LEN, 0x3ff);
+
+
+	agnx_write32(ctl, AGNX_ACI_TIMER1, 0x27);
+	agnx_write32(ctl, AGNX_ACI_TIMER2, 0x27);
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1400);
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1500);
+	/* Measure Calibration */
+	agnx_write32(ctl, AGNX_ACI_MEASURE, 0x1);
+	calibra_delay(priv);
+	do_calibration(priv);
+	agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x0);
+
+	agnx_write32(ctl, AGNX_ACI_TIMER1, 0x21);
+	agnx_write32(ctl, AGNX_ACI_TIMER2, 0x27);
+
+	agnx_write32(ctl, AGNX_ACI_LEN, 0xf);
+
+	reg = agnx_read32(ctl, AGNX_GCR_GAINSET0);
+	reg &= 0xf;
+	agnx_write32(ctl, AGNX_GCR_GAINSET0, reg);
+	reg = agnx_read32(ctl, AGNX_GCR_GAINSET1);
+	reg &= 0xf;
+	agnx_write32(ctl, AGNX_GCR_GAINSET1, reg);
+	reg = agnx_read32(ctl, AGNX_GCR_GAINSET2);
+	reg &= 0xf;
+	agnx_write32(ctl, AGNX_GCR_GAINSET2, reg);
+
+
+	agnx_write32(ctl, AGNX_GCR_GAINSETWRITE, 0x0);
+
+	/* Write 0x3 Gain Control Discovery Mode */
+	enable_receiver(priv);
+}
+
+int agnx_set_channel(struct agnx_priv *priv, unsigned int channel)
+{
+	AGNX_TRACE;
+
+	printk(KERN_ERR PFX "Channel is %d %s\n", channel, __func__);
+	radio_channel_set(priv, channel);
+	return 0;
+}
diff --git a/drivers/staging/agnx/sta.c b/drivers/staging/agnx/sta.c
new file mode 100644
index 000000000000..d3ac675e45bd
--- /dev/null
+++ b/drivers/staging/agnx/sta.c
@@ -0,0 +1,219 @@
+#include <linux/delay.h>
+#include <linux/etherdevice.h>
+#include "phy.h"
+#include "sta.h"
+#include "debug.h"
+
+void hash_read(struct agnx_priv *priv, u32 reghi, u32 reglo, u8 sta_id)
+{
+	void __iomem *ctl = priv->ctl;
+
+	reglo &= 0xFFFF;
+	reglo |= 0x30000000;
+	reglo |= 0x40000000;	/* Set status busy */
+	reglo |= sta_id << 16;
+
+	iowrite32(0, ctl + AGNX_RXM_HASH_CMD_FLAG);
+	iowrite32(reghi, ctl + AGNX_RXM_HASH_CMD_HIGH);
+	iowrite32(reglo, ctl + AGNX_RXM_HASH_CMD_LOW);
+
+	reghi = ioread32(ctl + AGNX_RXM_HASH_CMD_HIGH);
+        reglo = ioread32(ctl + AGNX_RXM_HASH_CMD_LOW);
+	printk(PFX "RX hash cmd are : %.8x%.8x\n", reghi, reglo);
+}
+
+void hash_write(struct agnx_priv *priv, u8 *mac_addr, u8 sta_id)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reghi, reglo;
+
+	if (!is_valid_ether_addr(mac_addr))
+		printk(KERN_WARNING PFX "Update hash table: Invalid hwaddr!\n");
+
+	reghi = mac_addr[0] << 24 | mac_addr[1] << 16 | mac_addr[2] << 8 | mac_addr[3];
+	reglo = mac_addr[4] << 8 | mac_addr[5];
+	reglo |= 0x10000000;	/* Set hash commmand */
+	reglo |= 0x40000000;	/* Set status busy */
+	reglo |= sta_id << 16;
+
+	iowrite32(0, ctl + AGNX_RXM_HASH_CMD_FLAG);
+	iowrite32(reghi, ctl + AGNX_RXM_HASH_CMD_HIGH);
+	iowrite32(reglo, ctl + AGNX_RXM_HASH_CMD_LOW);
+
+        reglo = ioread32(ctl + AGNX_RXM_HASH_CMD_LOW);
+	if (!(reglo & 0x80000000))
+		printk(KERN_WARNING PFX "Update hash table failed\n");
+}
+
+void hash_delete(struct agnx_priv *priv, u32 reghi, u32 reglo, u8 sta_id)
+{
+	void __iomem *ctl = priv->ctl;
+
+	reglo &= 0xFFFF;
+	reglo |= 0x20000000;
+	reglo |= 0x40000000;	/* Set status busy */
+	reglo |= sta_id << 16;
+
+	iowrite32(0, ctl + AGNX_RXM_HASH_CMD_FLAG);
+	iowrite32(reghi, ctl + AGNX_RXM_HASH_CMD_HIGH);
+	iowrite32(reglo, ctl + AGNX_RXM_HASH_CMD_LOW);
+	reghi = ioread32(ctl + AGNX_RXM_HASH_CMD_HIGH);
+
+        reglo = ioread32(ctl + AGNX_RXM_HASH_CMD_LOW);
+	printk(PFX "RX hash cmd are : %.8x%.8x\n", reghi, reglo);
+
+}
+
+void hash_dump(struct agnx_priv *priv, u8 sta_id)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reghi, reglo;
+
+	reglo = 0x0;		/* dump command */
+	reglo|= 0x40000000;  	/* status bit */
+	iowrite32(reglo, ctl + AGNX_RXM_HASH_CMD_LOW);
+	iowrite32(sta_id << 16, ctl + AGNX_RXM_HASH_DUMP_DATA);
+
+	udelay(80);
+
+	reghi = ioread32(ctl + AGNX_RXM_HASH_CMD_HIGH);
+        reglo = ioread32(ctl + AGNX_RXM_HASH_CMD_LOW);
+	printk(PFX "hash cmd are : %.8x%.8x\n", reghi, reglo);
+	reghi = ioread32(ctl + AGNX_RXM_HASH_CMD_FLAG);
+	printk(PFX "hash flag is : %.8x\n", reghi);
+	reghi = ioread32(ctl + AGNX_RXM_HASH_DUMP_MST);
+	reglo = ioread32(ctl + AGNX_RXM_HASH_DUMP_LST);
+	printk(PFX "hash dump mst lst: %.8x%.8x\n", reghi, reglo);
+	reghi = ioread32(ctl + AGNX_RXM_HASH_DUMP_DATA);
+	printk(PFX "hash dump data: %.8x\n", reghi);
+}
+
+void get_sta_power(struct agnx_priv *priv, struct agnx_sta_power *power, unsigned int sta_idx)
+{
+	void __iomem *ctl = priv->ctl;
+        memcpy_fromio(power, ctl + AGNX_TXM_STAPOWTEMP + sizeof(*power) * sta_idx,
+		      sizeof(*power));
+}
+
+inline void
+set_sta_power(struct agnx_priv *priv, struct agnx_sta_power *power, unsigned int sta_idx)
+{
+	void __iomem *ctl = priv->ctl;
+	/* FIXME   2. Write Template to offset + station number  */
+        memcpy_toio(ctl + AGNX_TXM_STAPOWTEMP + sizeof(*power) * sta_idx,
+		    power, sizeof(*power));
+}
+
+
+void get_sta_tx_wq(struct agnx_priv *priv, struct agnx_sta_tx_wq *tx_wq,
+		   unsigned int sta_idx, unsigned int wq_idx)
+{
+	void __iomem *data = priv->data;
+	memcpy_fromio(tx_wq, data + AGNX_PDU_TX_WQ + sizeof(*tx_wq) * STA_TX_WQ_NUM * sta_idx +
+		      sizeof(*tx_wq) * wq_idx,  sizeof(*tx_wq));
+
+}
+
+inline void set_sta_tx_wq(struct agnx_priv *priv, struct agnx_sta_tx_wq *tx_wq,
+		   unsigned int sta_idx, unsigned int wq_idx)
+{
+	void __iomem *data = priv->data;
+	memcpy_toio(data + AGNX_PDU_TX_WQ + sizeof(*tx_wq) * STA_TX_WQ_NUM * sta_idx +
+		    sizeof(*tx_wq) * wq_idx, tx_wq, sizeof(*tx_wq));
+}
+
+
+void get_sta(struct agnx_priv *priv, struct agnx_sta *sta, unsigned int sta_idx)
+{
+	void __iomem *data = priv->data;
+
+	memcpy_fromio(sta, data + AGNX_PDUPOOL + sizeof(*sta) * sta_idx,
+		      sizeof(*sta));
+}
+
+inline void set_sta(struct agnx_priv *priv, struct agnx_sta *sta, unsigned int sta_idx)
+{
+	void __iomem *data = priv->data;
+
+        memcpy_toio(data + AGNX_PDUPOOL + sizeof(*sta) * sta_idx,
+		    sta, sizeof(*sta));
+}
+
+/* FIXME */
+void sta_power_init(struct agnx_priv *priv, unsigned int sta_idx)
+{
+	struct agnx_sta_power power;
+	u32 reg;
+	AGNX_TRACE;
+
+	memset(&power, 0, sizeof(power));
+	reg = agnx_set_bits(EDCF, EDCF_SHIFT, 0x1);
+	power.reg = cpu_to_le32(reg);
+	set_sta_power(priv, &power, sta_idx);
+	udelay(40);
+} /* add_power_template */
+
+
+/* @num: The #number of station that is visible to the card */
+static void sta_tx_workqueue_init(struct agnx_priv *priv, unsigned int sta_idx)
+{
+	struct agnx_sta_tx_wq tx_wq;
+	u32 reg;
+	unsigned int i;
+
+	memset(&tx_wq, 0, sizeof(tx_wq));
+
+	reg = agnx_set_bits(WORK_QUEUE_VALID, WORK_QUEUE_VALID_SHIFT, 1);
+	reg |= agnx_set_bits(WORK_QUEUE_ACK_TYPE, WORK_QUEUE_ACK_TYPE_SHIFT, 1);
+//	reg |= agnx_set_bits(WORK_QUEUE_ACK_TYPE, WORK_QUEUE_ACK_TYPE_SHIFT, 0);
+	tx_wq.reg2 |= cpu_to_le32(reg);
+
+	/* Suppose all 8 traffic class are used */
+	for (i = 0; i < STA_TX_WQ_NUM; i++)
+		set_sta_tx_wq(priv, &tx_wq, sta_idx, i);
+} /* sta_tx_workqueue_init */
+
+
+static void sta_traffic_init(struct agnx_sta_traffic *traffic)
+{
+	u32 reg;
+	memset(traffic, 0, sizeof(*traffic));
+
+	reg = agnx_set_bits(NEW_PACKET, NEW_PACKET_SHIFT, 1);
+	reg |= agnx_set_bits(TRAFFIC_VALID, TRAFFIC_VALID_SHIFT, 1);
+//	reg |= agnx_set_bits(TRAFFIC_ACK_TYPE, TRAFFIC_ACK_TYPE_SHIFT, 1);
+	traffic->reg0 = cpu_to_le32(reg);
+
+	/* 	3. setting RX Sequence Number to 4095 */
+	reg = agnx_set_bits(RX_SEQUENCE_NUM, RX_SEQUENCE_NUM_SHIFT, 4095);
+	traffic->reg1 = cpu_to_le32(reg);
+}
+
+
+/* @num: The #number of station that is visible to the card */
+void sta_init(struct agnx_priv *priv, unsigned int sta_idx)
+{
+	/* FIXME the length of sta is 256 bytes Is that
+	 * dangerous to stack overflow? */
+	struct agnx_sta sta;
+	u32 reg;
+	int i;
+
+	memset(&sta, 0, sizeof(sta));
+	/* Set valid to 1 */
+	reg = agnx_set_bits(STATION_VALID, STATION_VALID_SHIFT, 1);
+	/* Set Enable Concatenation to 0 (?) */
+	reg |= agnx_set_bits(ENABLE_CONCATENATION, ENABLE_CONCATENATION_SHIFT, 0);
+	/* Set Enable Decompression to 0 (?) */
+	reg |= agnx_set_bits(ENABLE_DECOMPRESSION, ENABLE_DECOMPRESSION_SHIFT, 0);
+	sta.reg = cpu_to_le32(reg);
+
+	/* Initialize each of the Traffic Class Structures by: */
+	for (i = 0; i < 8; i++)
+		sta_traffic_init(sta.traffic + i);
+
+	set_sta(priv, &sta, sta_idx);
+	sta_tx_workqueue_init(priv, sta_idx);
+} /* sta_descriptor_init */
+
+
diff --git a/drivers/staging/agnx/sta.h b/drivers/staging/agnx/sta.h
new file mode 100644
index 000000000000..58d0b12900df
--- /dev/null
+++ b/drivers/staging/agnx/sta.h
@@ -0,0 +1,222 @@
+#ifndef AGNX_STA_H_
+#define AGNX_STA_H_
+
+#define STA_TX_WQ_NUM	8	/* The number of TX workqueue one STA has */
+
+struct agnx_hash_cmd {
+	__be32 cmdhi;
+#define MACLO		0xFFFF0000
+#define MACLO_SHIFT	16
+#define STA_ID		0x0000FFF0
+#define STA_ID_SHIFT	4
+#define CMD		0x0000000C
+#define CMD_SHIFT	2
+#define STATUS		0x00000002
+#define STATUS_SHIFT	1
+#define PASS		0x00000001
+#define PASS_SHIFT	1
+	__be32 cmdlo;
+}__attribute__((__packed__));
+
+
+/*
+ * Station Power Template
+ * FIXME Just for agn100 yet
+ */
+struct agnx_sta_power {
+	__le32 reg;
+#define SIGNAL			0x000000FF /* signal */
+#define SIGNAL_SHIFT		0
+#define RATE			0x00000F00
+#define RATE_SHIFT		8
+#define TIFS			0x00001000
+#define TIFS_SHIFT		12
+#define EDCF			0x00002000
+#define EDCF_SHIFT		13
+#define CHANNEL_BOND		0x00004000
+#define CHANNEL_BOND_SHIFT	14
+#define PHY_MODE		0x00038000
+#define PHY_MODE_SHIFT		15
+#define POWER_LEVEL		0x007C0000
+#define POWER_LEVEL_SHIFT	18
+#define NUM_TRANSMITTERS	0x00800000
+#define NUM_TRANSMITTERS_SHIFT	23
+} __attribute__((__packed__));
+
+/*
+ * TX Workqueue Descriptor
+ */
+struct agnx_sta_tx_wq {
+	__le32 reg0;
+#define HEAD_POINTER_LOW	0xFF000000 /* Head pointer low */
+#define HEAD_POINTER_LOW_SHIFT	24
+#define TAIL_POINTER		0x00FFFFFF /* Tail pointer */
+#define TAIL_POINTER_SHIFT	0
+
+	__le32 reg3;
+#define ACK_POINTER_LOW	        0xFFFF0000	/* ACK pointer low */
+#define ACK_POINTER_LOW_SHIFT	16
+#define HEAD_POINTER_HIGH	0x0000FFFF	/* Head pointer high */
+#define HEAD_POINTER_HIGH_SHIFT	0
+
+	__le32 reg1;
+/* ACK timeout tail packet count */
+#define ACK_TIMOUT_TAIL_PACK_CNT	0xFFF00000
+#define ACK_TIMOUT_TAIL_PACK_CNT_SHIFT	20
+/* Head timeout tail packet count */
+#define HEAD_TIMOUT_TAIL_PACK_CNT	0x000FFF00
+#define HEAD_TIMOUT_TAIL_PACK_CNT_SHIFT	8
+#define ACK_POINTER_HIGH	        0x000000FF /* ACK pointer high */
+#define ACK_POINTER_HIGH_SHIFT		0
+
+	__le32 reg2;
+#define WORK_QUEUE_VALID		0x80000000 /* valid */
+#define WORK_QUEUE_VALID_SHIFT		31
+#define WORK_QUEUE_ACK_TYPE		0x40000000 /* ACK type */
+#define WORK_QUEUE_ACK_TYPE_SHIFT	30
+/* Head timeout window limit fragmentation count */
+#define HEAD_TIMOUT_WIN_LIM_FRAG_CNT	0x3FFF0000
+#define HEAD_TIMOUT_WIN_LIM_FRAG_CNT_SHIFT	16
+/* Head timeout window limit byte count */
+#define HEAD_TIMOUT_WIN_LIM_BYTE_CNT	0x0000FFFF
+#define HEAD_TIMOUT_WIN_LIM_BYTE_CNT_SHIFT	 0
+} __attribute__((__packed__));
+
+
+/*
+ * Traffic Class Structure
+ */
+struct agnx_sta_traffic {
+	__le32 reg0;
+#define ACK_TIMOUT_CNT		0xFF800000 /* ACK Timeout Counts */
+#define ACK_TIMOUT_CNT_SHIFT	23
+#define TRAFFIC_ACK_TYPE	0x00600000 /* ACK Type */
+#define TRAFFIC_ACK_TYPE_SHIFT	21
+#define NEW_PACKET		0x00100000 /* New Packet  */
+#define NEW_PACKET_SHIFT	20
+#define TRAFFIC_VALID		0x00080000 /* Valid */
+#define TRAFFIC_VALID_SHIFT	19
+#define RX_HDR_DESC_POINTER	0x0007FFFF /* RX Header Descripter pointer */
+#define RX_HDR_DESC_POINTER_SHIFT	 0
+
+	__le32 reg1;
+#define RX_PACKET_TIMESTAMP	0xFFFF0000 /* RX Packet Timestamp */
+#define RX_PACKET_TIMESTAMP_SHIFT	16
+#define TRAFFIC_RESERVED	0x0000E000 /* Reserved */
+#define TRAFFIC_RESERVED_SHIFT  13
+#define SV			0x00001000 /* sv */
+#define SV_SHIFT		12
+#define RX_SEQUENCE_NUM		0x00000FFF /* RX Sequence Number */
+#define RX_SEQUENCE_NUM_SHIFT	0
+
+	__le32 tx_replay_cnt_low; /* TX Replay Counter Low */
+
+	__le16 tx_replay_cnt_high; /* TX Replay Counter High */
+	__le16 rx_replay_cnt_high; /* RX Replay Counter High */
+
+	__be32 rx_replay_cnt_low; /* RX Replay Counter Low */
+} __attribute__((__packed__));
+
+/*
+ * Station Descriptors
+ */
+struct agnx_sta {
+	__le32 tx_session_keys[4]; /* Transmit Session Key (0-3) */
+	__le32 rx_session_keys[4]; /* Receive Session Key (0-3) */
+
+	__le32 reg;
+#define ID_1			0xC0000000 /* id 1 */
+#define ID_1_SHIFT		30
+#define ID_0			0x30000000 /* id 0 */
+#define ID_0_SHIFT		28
+#define ENABLE_CONCATENATION	0x0FF00000 /* Enable concatenation */
+#define ENABLE_CONCATENATION_SHIFT	20
+#define ENABLE_DECOMPRESSION	0x000FF000 /* Enable decompression */
+#define ENABLE_DECOMPRESSION_SHIFT	12
+#define STA_RESERVED		0x00000C00 /* Reserved */
+#define STA_RESERVED_SHIFT	10
+#define EAP			0x00000200 /* EAP */
+#define EAP_SHIFT		9
+#define ED_NULL			0x00000100 /* ED NULL */
+#define ED_NULL_SHIFT		8
+#define ENCRYPTION_POLICY	0x000000E0 /* Encryption Policy */
+#define ENCRYPTION_POLICY_SHIFT 5
+#define DEFINED_KEY_ID		0x00000018 /* Defined Key ID */
+#define DEFINED_KEY_ID_SHIFT	3
+#define FIXED_KEY		0x00000004 /* Fixed Key */
+#define FIXED_KEY_SHIFT		2
+#define KEY_VALID		0x00000002 /* Key Valid */
+#define KEY_VALID_SHIFT		1
+#define STATION_VALID		0x00000001 /* Station Valid */
+#define STATION_VALID_SHIFT	0
+
+	__le32 tx_aes_blks_unicast; /* TX AES Blks Unicast */
+	__le32 rx_aes_blks_unicast; /* RX AES Blks Unicast */
+
+	__le16 aes_format_err_unicast_cnt; /* AES Format Error Unicast Counts */
+	__le16 aes_replay_unicast; /* AES Replay Unicast */
+
+	__le16 aes_decrypt_err_unicast;	/* AES Decrypt Error Unicast */
+	__le16 aes_decrypt_err_default;	/* AES Decrypt Error default */
+
+	__le16 single_retry_packets; /* Single Retry Packets */
+	__le16 failed_tx_packets; /* Failed Tx Packets */
+
+	__le16 muti_retry_packets; /* Multiple Retry Packets */
+	__le16 ack_timeouts;	/* ACK Timeouts */
+
+	__le16 frag_tx_cnt;	/* Fragment TX Counts */
+	__le16 rts_brq_sent;	/* RTS Brq Sent */
+
+	__le16 tx_packets;	/* TX Packets */
+	__le16 cts_back_timeout; /* CTS Back Timeout */
+
+	__le32 phy_stats_high;	/* PHY Stats High */
+	__le32 phy_stats_low;	/* PHY Stats Low */
+
+	struct agnx_sta_traffic traffic[8];	/* Traffic Class Structure (8) */
+
+	__le16 traffic_class0_frag_success; /* Traffic Class 0 Fragment Success */
+	__le16 traffic_class1_frag_success; /* Traffic Class 1 Fragment Success */
+	__le16 traffic_class2_frag_success; /* Traffic Class 2 Fragment Success */
+	__le16 traffic_class3_frag_success; /* Traffic Class 3 Fragment Success */
+	__le16 traffic_class4_frag_success; /* Traffic Class 4 Fragment Success */
+	__le16 traffic_class5_frag_success; /* Traffic Class 5 Fragment Success */
+	__le16 traffic_class6_frag_success; /* Traffic Class 6 Fragment Success */
+	__le16 traffic_class7_frag_success; /* Traffic Class 7 Fragment Success */
+
+	__le16 num_frag_non_prime_rates; /* number of Fragments for non-prime rates */
+	__le16 ack_timeout_non_prime_rates; /* ACK Timeout for non-prime rates */
+
+} __attribute__((__packed__));
+
+
+struct agnx_beacon_hdr {
+	struct agnx_sta_power power; /* Tx Station Power Template  */
+	u8 phy_hdr[6];		/* PHY Hdr */
+	u8 frame_len_lo;	/* Frame Length Lo */
+	u8 frame_len_hi;	/* Frame Length Hi */
+	u8 mac_hdr[24];		/* MAC Header */
+	/* FIXME */
+	/* 802.11(abg) beacon */
+} __attribute__((__packed__));
+
+void hash_write(struct agnx_priv *priv, u8 *mac_addr, u8 sta_id);
+void hash_dump(struct agnx_priv *priv, u8 sta_id);
+void hash_read(struct agnx_priv *priv, u32 reghi, u32 reglo, u8 sta_id);
+void hash_delete(struct agnx_priv *priv, u32 reghi, u32 reglo, u8 sta_id);
+
+void get_sta_power(struct agnx_priv *priv, struct agnx_sta_power *power, unsigned int sta_idx);
+void set_sta_power(struct agnx_priv *priv, struct agnx_sta_power *power,
+		   unsigned int sta_idx);
+void get_sta_tx_wq(struct agnx_priv *priv, struct agnx_sta_tx_wq *tx_wq,
+		   unsigned int sta_idx, unsigned int wq_idx);
+void set_sta_tx_wq(struct agnx_priv *priv, struct agnx_sta_tx_wq *tx_wq,
+		   unsigned int sta_idx, unsigned int wq_idx);
+void get_sta(struct agnx_priv *priv, struct agnx_sta *sta, unsigned int sta_idx);
+void set_sta(struct agnx_priv *priv, struct agnx_sta *sta, unsigned int sta_idx);
+
+void sta_power_init(struct agnx_priv *priv, unsigned int num);
+void sta_init(struct agnx_priv *priv, unsigned int num);
+
+#endif /* AGNX_STA_H_ */
diff --git a/drivers/staging/agnx/table.c b/drivers/staging/agnx/table.c
new file mode 100644
index 000000000000..c60048487b5a
--- /dev/null
+++ b/drivers/staging/agnx/table.c
@@ -0,0 +1,168 @@
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include "agnx.h"
+#include "debug.h"
+#include "phy.h"
+
+static const u32
+tx_fir_table[] = { 0x19, 0x5d, 0xce, 0x151, 0x1c3, 0x1ff, 0x1ea, 0x17c, 0xcf,
+		   0x19, 0x38e, 0x350, 0x362, 0x3ad, 0x5, 0x44, 0x59, 0x49,
+		   0x21, 0x3f7, 0x3e0, 0x3e3, 0x3f3, 0x0 };
+
+void tx_fir_table_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(tx_fir_table); i++)
+		iowrite32(tx_fir_table[i], ctl + AGNX_FIR_BASE + i*4);
+} /* fir_table_setup */
+
+
+static const u32
+gain_table[] = { 0x8, 0x8, 0xf, 0x13, 0x17, 0x1b, 0x1f, 0x23, 0x27, 0x2b,
+		 0x2f, 0x33, 0x37, 0x3b, 0x3f, 0x43, 0x47, 0x4b, 0x4f,
+		 0x53, 0x57, 0x5b, 0x5f, 0x5f, 0x5f, 0x5f, 0x5f, 0x5f,
+		 0x5f, 0x5f, 0x5f, 0x5f };
+
+void gain_table_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(gain_table); i++) {
+		iowrite32(gain_table[i], ctl + AGNX_GAIN_TABLE + i*4);
+		iowrite32(gain_table[i], ctl + AGNX_GAIN_TABLE + i*4 + 0x80);
+	}
+} /* gain_table_init */
+
+void monitor_gain_table_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	unsigned int i;
+
+	for (i = 0; i < 0x44; i += 4) {
+		iowrite32(0x61, ctl + AGNX_MONGCR_BASE + i);
+		iowrite32(0x61, ctl + AGNX_MONGCR_BASE + 0x200 + i);
+	}
+	for (i = 0x44; i < 0x64; i += 4) {
+		iowrite32(0x6e, ctl + AGNX_MONGCR_BASE + i);
+		iowrite32(0x6e, ctl + AGNX_MONGCR_BASE + 0x200 + i);
+	}
+	for (i = 0x64; i < 0x94; i += 4) {
+		iowrite32(0x7a, ctl + AGNX_MONGCR_BASE + i);
+		iowrite32(0x7a, ctl + AGNX_MONGCR_BASE + 0x200 + i);
+	}
+	for (i = 0x94; i < 0xdc; i += 4) {
+		iowrite32(0x87, ctl + AGNX_MONGCR_BASE + i);
+		iowrite32(0x87, ctl + AGNX_MONGCR_BASE + 0x200 + i);
+	}
+	for (i = 0xdc; i < 0x148; i += 4) {
+		iowrite32(0x95, ctl + AGNX_MONGCR_BASE + i);
+		iowrite32(0x95, ctl + AGNX_MONGCR_BASE + 0x200 + i);
+	}
+	for (i = 0x148; i < 0x1e8; i += 4) {
+		iowrite32(0xa2, ctl + AGNX_MONGCR_BASE + i);
+		iowrite32(0xa2, ctl + AGNX_MONGCR_BASE + 0x200 + i);
+	}
+	for (i = 0x1e8; i <= 0x1fc; i += 4) {
+		iowrite32(0xb0, ctl + AGNX_MONGCR_BASE + i);
+		iowrite32(0xb0, ctl + AGNX_MONGCR_BASE + 0x200 + i);
+	}
+} /* monitor_gain_table_init */
+
+
+void routing_table_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	unsigned int type, subtype;
+	u32 reg;
+
+	disable_receiver(priv);
+
+	for ( type = 0; type < 0x3; type++ ) {
+		for (subtype = 0; subtype < 0x10; subtype++) {
+			/* 1. Set Routing table to R/W and to Return status on Read */
+			reg = (type << ROUTAB_TYPE_SHIFT) |
+				(subtype << ROUTAB_SUBTYPE_SHIFT);
+			reg |= (1 << ROUTAB_RW_SHIFT) | (1 << ROUTAB_STATUS_SHIFT);
+			if (type == ROUTAB_TYPE_DATA) {
+				/* NULL goes to RFP */
+				if (subtype == ROUTAB_SUBTYPE_NULL)
+//					reg |= ROUTAB_ROUTE_RFP;
+					reg |= ROUTAB_ROUTE_CPU;
+				/* QOS NULL goes to CPU */
+				else if (subtype == ROUTAB_SUBTYPE_QOSNULL)
+					reg |= ROUTAB_ROUTE_CPU;
+				/* All Data and QOS data subtypes go to Encryption */
+				else if ((subtype == ROUTAB_SUBTYPE_DATA) ||
+					 (subtype == ROUTAB_SUBTYPE_DATAACK) ||
+					 (subtype == ROUTAB_SUBTYPE_DATAPOLL) ||
+					 (subtype == ROUTAB_SUBTYPE_DATAPOLLACK) ||
+					 (subtype == ROUTAB_SUBTYPE_QOSDATA) ||
+					 (subtype == ROUTAB_SUBTYPE_QOSDATAACK) ||
+					 (subtype == ROUTAB_SUBTYPE_QOSDATAPOLL) ||
+					 (subtype == ROUTAB_SUBTYPE_QOSDATAACKPOLL))
+					reg |= ROUTAB_ROUTE_ENCRY;
+//					reg |= ROUTAB_ROUTE_CPU;
+				/*Drop NULL and QOS NULL ack, poll and poll ack*/
+				else if ((subtype == ROUTAB_SUBTYPE_NULLACK) ||
+					 (subtype == ROUTAB_SUBTYPE_QOSNULLACK) ||
+					 (subtype == ROUTAB_SUBTYPE_NULLPOLL) ||
+					 (subtype == ROUTAB_SUBTYPE_QOSNULLPOLL) ||
+					 (subtype == ROUTAB_SUBTYPE_NULLPOLLACK) ||
+					 (subtype == ROUTAB_SUBTYPE_QOSNULLPOLLACK))
+//					reg |= ROUTAB_ROUTE_DROP;
+					reg |= ROUTAB_ROUTE_CPU;
+			}
+			else
+				reg |= (ROUTAB_ROUTE_CPU);
+			iowrite32(reg, ctl + AGNX_RXM_ROUTAB);
+			/* Check to verify that the status bit cleared */
+			routing_table_delay();
+		}
+	}
+	enable_receiver(priv);
+} /* routing_table_init */
+
+void tx_engine_lookup_tbl_init(struct agnx_priv *priv)
+{
+	void __iomem *data = priv->data;
+	unsigned int i;
+
+	for (i = 0; i <= 28; i += 4)
+		iowrite32(0xb00c, data + AGNX_ENGINE_LOOKUP_TBL + i);
+	for (i = 32; i <= 120; i += 8) {
+		iowrite32(0x1e58, data + AGNX_ENGINE_LOOKUP_TBL + i);
+		iowrite32(0xb00c, data + AGNX_ENGINE_LOOKUP_TBL + i + 4);
+	}
+
+	for (i = 128; i <= 156; i += 4)
+		iowrite32(0x980c, data + AGNX_ENGINE_LOOKUP_TBL + i);
+	for (i = 160; i <= 248; i += 8) {
+		iowrite32(0x1858, data + AGNX_ENGINE_LOOKUP_TBL + i);
+		iowrite32(0x980c, data + AGNX_ENGINE_LOOKUP_TBL + i + 4);
+	}
+
+	for (i = 256; i <= 284; i += 4)
+		iowrite32(0x980c, data + AGNX_ENGINE_LOOKUP_TBL + i);
+	for (i = 288; i <= 376; i += 8) {
+		iowrite32(0x1a58, data + AGNX_ENGINE_LOOKUP_TBL + i);
+		iowrite32(0x1858, data + AGNX_ENGINE_LOOKUP_TBL + i + 4);
+	}
+
+	for (i = 512; i <= 540; i += 4)
+		iowrite32(0xc00c, data + AGNX_ENGINE_LOOKUP_TBL + i);
+	for (i = 544; i <= 632; i += 8) {
+		iowrite32(0x2058, data + AGNX_ENGINE_LOOKUP_TBL + i);
+		iowrite32(0xc00c, data + AGNX_ENGINE_LOOKUP_TBL + i + 4);
+	}
+
+	for (i = 640; i <= 668; i += 4)
+		iowrite32(0xc80c, data + AGNX_ENGINE_LOOKUP_TBL + i);
+	for (i = 672; i <= 764; i += 8) {
+		iowrite32(0x2258, data + AGNX_ENGINE_LOOKUP_TBL + i);
+		iowrite32(0xc80c, data + AGNX_ENGINE_LOOKUP_TBL + i + 4);
+	}
+}
+
diff --git a/drivers/staging/agnx/table.h b/drivers/staging/agnx/table.h
new file mode 100644
index 000000000000..f0626b5ee86b
--- /dev/null
+++ b/drivers/staging/agnx/table.h
@@ -0,0 +1,10 @@
+#ifndef AGNX_TABLE_H_
+#define AGNX_TABLE_H_
+
+void tx_fir_table_init(struct agnx_priv *priv);
+void gain_table_init(struct agnx_priv *priv);
+void monitor_gain_table_init(struct agnx_priv *priv);
+void routing_table_init(struct agnx_priv *priv);
+void tx_engine_lookup_tbl_init(struct agnx_priv *priv);
+
+#endif /* AGNX_TABLE_H_ */
diff --git a/drivers/staging/agnx/xmit.c b/drivers/staging/agnx/xmit.c
new file mode 100644
index 000000000000..7f01528b8a46
--- /dev/null
+++ b/drivers/staging/agnx/xmit.c
@@ -0,0 +1,819 @@
+/**
+ * Airgo MIMO wireless driver
+ *
+ * Copyright (c) 2007 Li YanBo <dreamfly281@gmail.com>
+
+ * Thanks for Jeff Williams <angelbane@gmail.com> do reverse engineer
+ * works and published the SPECS at http://airgo.wdwconsulting.net/mymoin
+
+ * 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/pci.h>
+#include <linux/delay.h>
+#include "agnx.h"
+#include "debug.h"
+#include "phy.h"
+
+unsigned int rx_frame_cnt = 0;
+//unsigned int local_tx_sent_cnt = 0;
+
+static inline void disable_rx_engine(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	iowrite32(0x100, ctl + AGNX_CIR_RXCTL);
+	/* Wait for RX Control to have the Disable Rx Interrupt (0x100) set */
+	ioread32(ctl + AGNX_CIR_RXCTL);
+}
+
+static inline void enable_rx_engine(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	iowrite32(0x80, ctl + AGNX_CIR_RXCTL);
+	ioread32(ctl + AGNX_CIR_RXCTL);
+}
+
+inline void disable_rx_interrupt(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	disable_rx_engine(priv);
+	reg = ioread32(ctl + AGNX_CIR_RXCFG);
+	reg &= ~0x20;
+	iowrite32(reg, ctl + AGNX_CIR_RXCFG);
+	ioread32(ctl + AGNX_CIR_RXCFG);
+}
+
+inline void enable_rx_interrupt(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	reg = ioread32(ctl + AGNX_CIR_RXCFG);
+	reg |= 0x20;
+	iowrite32(reg, ctl + AGNX_CIR_RXCFG);
+	ioread32(ctl + AGNX_CIR_RXCFG);
+	enable_rx_engine(priv);
+}
+
+static inline void rx_desc_init(struct agnx_priv *priv, unsigned int idx)
+{
+	struct agnx_desc *desc = priv->rx.desc + idx;
+	struct agnx_info *info = priv->rx.info + idx;
+
+	memset(info, 0, sizeof(*info));
+
+	info->dma_len = IEEE80211_MAX_RTS_THRESHOLD + sizeof(struct agnx_hdr);
+	info->skb = dev_alloc_skb(info->dma_len);
+	if (info->skb == NULL)
+		agnx_bug("refill err");
+
+	info->mapping = pci_map_single(priv->pdev, skb_tail_pointer(info->skb),
+				       info->dma_len, PCI_DMA_FROMDEVICE);
+	memset(desc, 0, sizeof(*desc));
+	desc->dma_addr = cpu_to_be32(info->mapping);
+	/* Set the owner to the card */
+	desc->frag = cpu_to_be32(be32_to_cpu(desc->frag) | OWNER);
+}
+
+static inline void rx_desc_reinit(struct agnx_priv *priv, unsigned int idx)
+{
+	struct agnx_info *info = priv->rx.info + idx;
+
+	/* Cause ieee80211 will free the skb buffer, so we needn't to free it again?! */
+	pci_unmap_single(priv->pdev, info->mapping, info->dma_len, PCI_DMA_FROMDEVICE);
+	rx_desc_init(priv, idx);
+}
+
+static inline void rx_desc_reusing(struct agnx_priv *priv, unsigned int idx)
+{
+	struct agnx_desc *desc = priv->rx.desc + idx;
+	struct agnx_info *info = priv->rx.info + idx;
+
+	memset(desc, 0, sizeof(*desc));
+	desc->dma_addr = cpu_to_be32(info->mapping);
+	/* Set the owner to the card */
+	desc->frag = cpu_to_be32(be32_to_cpu(desc->frag) | OWNER);
+}
+
+static void rx_desc_free(struct agnx_priv *priv, unsigned int idx)
+{
+	struct agnx_desc *desc = priv->rx.desc + idx;
+	struct agnx_info *info = priv->rx.info + idx;
+
+	BUG_ON(!desc || !info);
+	if (info->mapping)
+		pci_unmap_single(priv->pdev, info->mapping, info->dma_len, PCI_DMA_FROMDEVICE);
+	if (info->skb)
+		dev_kfree_skb(info->skb);
+	memset(info, 0, sizeof(*info));
+	memset(desc, 0, sizeof(*desc));
+}
+
+static inline void __tx_desc_free(struct agnx_priv *priv,
+				  struct agnx_desc *desc, struct agnx_info *info)
+{
+	BUG_ON(!desc || !info);
+	/* TODO make sure mapping, skb and len are consistency */
+	if (info->mapping)
+		pci_unmap_single(priv->pdev, info->mapping,
+				 info->dma_len, PCI_DMA_TODEVICE);
+	if (info->type == PACKET)
+		dev_kfree_skb(info->skb);
+
+	memset(info, 0, sizeof(*info));
+	memset(desc, 0, sizeof(*desc));
+}
+
+static void txm_desc_free(struct agnx_priv *priv, unsigned int idx)
+{
+	struct agnx_desc *desc = priv->txm.desc + idx;
+	struct agnx_info *info = priv->txm.info + idx;
+
+	__tx_desc_free(priv, desc, info);
+}
+
+static void txd_desc_free(struct agnx_priv *priv, unsigned int idx)
+{
+	struct agnx_desc *desc = priv->txd.desc + idx;
+	struct agnx_info *info = priv->txd.info + idx;
+
+	__tx_desc_free(priv, desc, info);
+}
+
+int fill_rings(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	unsigned int i;
+	u32 reg;
+	AGNX_TRACE;
+
+	priv->txd.idx_sent = priv->txm.idx_sent = 0;
+	priv->rx.idx = priv->txm.idx = priv->txd.idx = 0;
+
+	for (i = 0; i < priv->rx.size; i++)
+		rx_desc_init(priv, i);
+	for (i = 0; i < priv->txm.size; i++) {
+		memset(priv->txm.desc + i, 0, sizeof(struct agnx_desc));
+		memset(priv->txm.info + i, 0, sizeof(struct agnx_info));
+	}
+	for (i = 0; i < priv->txd.size; i++) {
+		memset(priv->txd.desc + i, 0, sizeof(struct agnx_desc));
+		memset(priv->txd.info + i, 0, sizeof(struct agnx_info));
+	}
+
+	/* FIXME Set the card RX TXM and TXD address */
+	agnx_write32(ctl, AGNX_CIR_RXCMSTART, priv->rx.dma);
+	agnx_write32(ctl, AGNX_CIR_RXCMEND, priv->txm.dma);
+
+	agnx_write32(ctl, AGNX_CIR_TXMSTART, priv->txm.dma);
+	agnx_write32(ctl, AGNX_CIR_TXMEND, priv->txd.dma);
+
+	agnx_write32(ctl, AGNX_CIR_TXDSTART, priv->txd.dma);
+	agnx_write32(ctl, AGNX_CIR_TXDEND, priv->txd.dma +
+		     sizeof(struct agnx_desc) * priv->txd.size);
+
+	/* FIXME Relinquish control of rings to card */
+	reg = agnx_read32(ctl, AGNX_CIR_BLKCTL);
+	reg &= ~0x800;
+	agnx_write32(ctl, AGNX_CIR_BLKCTL, reg);
+	return 0;
+} /* fill_rings */
+
+void unfill_rings(struct agnx_priv *priv)
+{
+	unsigned long flags;
+	unsigned int i;
+	AGNX_TRACE;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	for (i = 0; i < priv->rx.size; i++)
+		rx_desc_free(priv, i);
+	for (i = 0; i < priv->txm.size; i++)
+		txm_desc_free(priv, i);
+	for (i = 0; i < priv->txd.size; i++)
+		txd_desc_free(priv, i);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+/* Extract the bitrate out of a CCK PLCP header.
+   copy from bcm43xx driver */
+static inline u8 agnx_plcp_get_bitrate_cck(__be32 *phyhdr_11b)
+{
+	/* FIXME */
+	switch (*(u8 *)phyhdr_11b) {
+	case 0x0A:
+		return 0;
+	case 0x14:
+		return 1;
+	case 0x37:
+		return 2;
+	case 0x6E:
+		return 3;
+	}
+	agnx_bug("Wrong plcp rate");
+	return 0;
+}
+
+/* FIXME */
+static inline u8 agnx_plcp_get_bitrate_ofdm(__be32 *phyhdr_11g)
+{
+	u8 rate = *(u8 *)phyhdr_11g & 0xF;
+
+	printk(PFX "G mode rate is 0x%x\n", rate);
+	return rate;
+}
+
+/* FIXME */
+static void get_rx_stats(struct agnx_priv *priv, struct agnx_hdr *hdr,
+			 struct ieee80211_rx_status *stat)
+{
+	void __iomem *ctl = priv->ctl;
+	u8 *rssi;
+	u32 noise;
+	/* FIXME just for test */
+	int snr = 40;		/* signal-to-noise ratio */
+
+	memset(stat, 0, sizeof(*stat));
+	/* RSSI */
+	rssi = (u8 *)&hdr->phy_stats_lo;
+//	stat->ssi = (rssi[0] + rssi[1] + rssi[2]) / 3;
+	/* Noise */
+	noise = ioread32(ctl + AGNX_GCR_NOISE0);
+	noise += ioread32(ctl + AGNX_GCR_NOISE1);
+	noise += ioread32(ctl + AGNX_GCR_NOISE2);
+	stat->noise = noise / 3;
+	/* Signal quality */
+	//snr = stat->ssi - stat->noise;
+	if (snr >=0 && snr < 40)
+		stat->signal = 5 * snr / 2;
+	else if (snr >= 40)
+		stat->signal = 100;
+	else
+		stat->signal = 0;
+
+
+	if (hdr->_11b0 && !hdr->_11g0) {
+		stat->rate_idx = agnx_plcp_get_bitrate_cck(&hdr->_11b0);
+	} else if (!hdr->_11b0 && hdr->_11g0) {
+		printk(PFX "RX: Found G mode packet\n");
+		stat->rate_idx = agnx_plcp_get_bitrate_ofdm(&hdr->_11g0);
+	} else
+		agnx_bug("Unknown packets type");
+
+
+	stat->band = IEEE80211_BAND_2GHZ;
+	stat->freq = agnx_channels[priv->channel - 1].center_freq;
+//	stat->antenna = 3;
+//	stat->mactime = be32_to_cpu(hdr->time_stamp);
+//	stat->channel = priv->channel;
+
+}
+
+static inline void combine_hdr_frag(struct ieee80211_hdr *ieeehdr,
+				    struct sk_buff *skb)
+{
+	u16 fctl;
+	unsigned int hdrlen;
+
+	fctl = le16_to_cpu(ieeehdr->frame_control);
+	hdrlen = ieee80211_hdrlen(fctl);
+	/* FIXME */
+	if (hdrlen < (2+2+6)/*minimum hdr*/ ||
+	    hdrlen > sizeof(struct ieee80211_mgmt)) {
+		printk(KERN_ERR PFX "hdr len is %d\n", hdrlen);
+		agnx_bug("Wrong ieee80211 hdr detected");
+	}
+	skb_push(skb, hdrlen);
+	memcpy(skb->data, ieeehdr, hdrlen);
+} /* combine_hdr_frag */
+
+static inline int agnx_packet_check(struct agnx_priv *priv, struct agnx_hdr *agnxhdr,
+				    unsigned packet_len)
+{
+	if (agnx_get_bits(CRC_FAIL, CRC_FAIL_SHIFT, be32_to_cpu(agnxhdr->reg1)) == 1){
+		printk(PFX "RX: CRC check fail\n");
+		goto drop;
+	}
+	if (packet_len > 2048) {
+		printk(PFX "RX: Too long packet detected\n");
+		goto drop;
+	}
+
+	/* FIXME Just usable for Promious Mode, for Manage mode exclude FCS */
+/* 	if (packet_len - sizeof(*agnxhdr) < FCS_LEN) { */
+/* 		printk(PFX "RX: Too short packet detected\n"); */
+/* 		goto drop; */
+/* 	} */
+	return 0;
+drop:
+	priv->stats.dot11FCSErrorCount++;
+	return -1;
+}
+
+void handle_rx_irq(struct agnx_priv *priv)
+{
+	struct ieee80211_rx_status status;
+	unsigned int len;
+//	AGNX_TRACE;
+
+	do {
+		struct agnx_desc *desc;
+		u32 frag;
+		struct agnx_info *info;
+		struct agnx_hdr *hdr;
+		struct sk_buff *skb;
+		unsigned int i = priv->rx.idx % priv->rx.size;
+
+		desc = priv->rx.desc + i;
+		frag = be32_to_cpu(desc->frag);
+		if (frag & OWNER)
+			break;
+
+		info = priv->rx.info + i;
+		skb = info->skb;
+		hdr = (struct agnx_hdr *)(skb->data);
+
+		len = (frag & PACKET_LEN) >> PACKET_LEN_SHIFT;
+		if (agnx_packet_check(priv, hdr, len) == -1) {
+ 			rx_desc_reusing(priv, i);
+			continue;
+		}
+		skb_put(skb, len);
+
+		do {
+			u16 fctl;
+			fctl = le16_to_cpu(((struct ieee80211_hdr *)hdr->mac_hdr)->frame_control);
+			if ((fctl & IEEE80211_FCTL_STYPE) != IEEE80211_STYPE_BEACON)// && !(fctl & IEEE80211_STYPE_BEACON))
+				dump_ieee80211_hdr((struct ieee80211_hdr *)hdr->mac_hdr, "RX");
+		} while (0);
+
+		if (hdr->_11b0 && !hdr->_11g0) {
+/* 			int j; */
+/* 			u16 fctl = le16_to_cpu(((struct ieee80211_hdr *)hdr->mac_hdr) */
+/* 					       ->frame_control); */
+/* 			if ( (fctl & IEEE80211_FCTL_FTYPE) ==  IEEE80211_FTYPE_DATA) { */
+/* 				agnx_print_rx_hdr(hdr); */
+// 				agnx_print_sta(priv, BSSID_STAID);
+/* 				for (j = 0; j < 8; j++) */
+/* 					agnx_print_sta_tx_wq(priv, BSSID_STAID, j);		 */
+/* 			} */
+
+			get_rx_stats(priv, hdr, &status);
+			skb_pull(skb, sizeof(*hdr));
+			combine_hdr_frag((struct ieee80211_hdr *)hdr->mac_hdr, skb);
+		} else if (!hdr->_11b0 && hdr->_11g0) {
+//			int j;
+			agnx_print_rx_hdr(hdr);
+			agnx_print_sta(priv, BSSID_STAID);
+//			for (j = 0; j < 8; j++)
+			agnx_print_sta_tx_wq(priv, BSSID_STAID, 0);
+
+			print_hex_dump_bytes("agnx: RX_PACKET: ", DUMP_PREFIX_NONE,
+					     skb->data, skb->len + 8);
+
+//			if (agnx_plcp_get_bitrate_ofdm(&hdr->_11g0) == 0)
+			get_rx_stats(priv, hdr, &status);
+			skb_pull(skb, sizeof(*hdr));
+			combine_hdr_frag((struct ieee80211_hdr *)
+					 ((void *)&hdr->mac_hdr), skb);
+//			dump_ieee80211_hdr((struct ieee80211_hdr *)skb->data, "RX G");
+		} else
+			agnx_bug("Unknown packets type");
+		ieee80211_rx_irqsafe(priv->hw, skb, &status);
+		rx_desc_reinit(priv, i);
+
+	} while ( priv->rx.idx++ );
+} /* handle_rx_irq */
+
+static inline void handle_tx_irq(struct agnx_priv *priv, struct agnx_ring *ring)
+{
+	struct agnx_desc *desc;
+	struct agnx_info *info;
+	unsigned int idx;
+
+	for (idx = ring->idx_sent; idx < ring->idx; idx++) {
+		unsigned int i = idx % ring->size;
+		u32  frag;
+
+		desc = ring->desc + i;
+		info = ring->info + i;
+
+		frag = be32_to_cpu(desc->frag);
+		if (frag & OWNER) {
+			if (info->type == HEADER)
+				break;
+			else
+				agnx_bug("TX error");
+		}
+
+		pci_unmap_single(priv->pdev, info->mapping, info->dma_len, PCI_DMA_TODEVICE);
+
+		do {
+//			int j;
+			size_t len;
+			len = info->skb->len - sizeof(struct agnx_hdr) + info->hdr_len;
+			//	if (len == 614) {
+//				agnx_print_desc(desc);
+				if (info->type == PACKET) {
+//					agnx_print_tx_hdr((struct agnx_hdr *)info->skb->data);
+/* 					agnx_print_sta_power(priv, LOCAL_STAID); */
+/* 					agnx_print_sta(priv, LOCAL_STAID); */
+/* //					for (j = 0; j < 8; j++) */
+/* 					agnx_print_sta_tx_wq(priv, LOCAL_STAID, 0); */
+//					agnx_print_sta_power(priv, BSSID_STAID);
+//					agnx_print_sta(priv, BSSID_STAID);
+//					for (j = 0; j < 8; j++)
+//					agnx_print_sta_tx_wq(priv, BSSID_STAID, 0);
+				}
+//			}
+		} while (0);
+
+		if (info->type == PACKET) {
+//			dump_txm_registers(priv);
+//			dump_rxm_registers(priv);
+//			dump_bm_registers(priv);
+//			dump_cir_registers(priv);
+		}
+
+		if (info->type == PACKET) {
+//			struct ieee80211_hdr *hdr;
+			struct ieee80211_tx_info *txi = IEEE80211_SKB_CB(info->skb);
+
+			skb_pull(info->skb, sizeof(struct agnx_hdr));
+			memcpy(skb_push(info->skb, info->hdr_len), &info->hdr, info->hdr_len);
+
+//			dump_ieee80211_hdr((struct ieee80211_hdr *)info->skb->data, "TX_HANDLE");
+/* 			print_hex_dump_bytes("agnx: TX_HANDLE: ", DUMP_PREFIX_NONE, */
+/* 					     info->skb->data, info->skb->len); */
+
+			if (!(txi->flags & IEEE80211_TX_CTL_NO_ACK))
+				txi->flags |= IEEE80211_TX_STAT_ACK;
+
+			ieee80211_tx_status_irqsafe(priv->hw, info->skb);
+
+
+/* 				info->tx_status.queue_number = (ring->size - i) / 2; */
+/* 				ieee80211_tx_status_irqsafe(priv->hw, info->skb, &(info->tx_status)); */
+/* 			} else */
+/* 				dev_kfree_skb_irq(info->skb); */
+ 		}
+		memset(desc, 0, sizeof(*desc));
+		memset(info, 0, sizeof(*info));
+	}
+
+	ring->idx_sent = idx;
+	/* TODO fill the priv->low_level_stats */
+
+	/* ieee80211_wake_queue(priv->hw, 0); */
+}
+
+void handle_txm_irq(struct agnx_priv *priv)
+{
+	handle_tx_irq(priv, &priv->txm);
+}
+
+void handle_txd_irq(struct agnx_priv *priv)
+{
+	handle_tx_irq(priv, &priv->txd);
+}
+
+void handle_other_irq(struct agnx_priv *priv)
+{
+//	void __iomem *ctl = priv->ctl;
+	u32 status = priv->irq_status;
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	if (status & IRQ_TX_BEACON) {
+		iowrite32(IRQ_TX_BEACON, ctl + AGNX_INT_STAT);
+		printk(PFX "IRQ: TX Beacon control is 0X%.8X\n", ioread32(ctl + AGNX_TXM_BEACON_CTL));
+		printk(PFX "IRQ: TX Beacon rx frame num: %d\n", rx_frame_cnt);
+	}
+	if (status & IRQ_TX_RETRY) {
+		reg = ioread32(ctl + AGNX_TXM_RETRYSTAID);
+		printk(PFX "IRQ: TX Retry, RETRY STA ID is %x\n", reg);
+	}
+	if (status & IRQ_TX_ACTIVITY)
+		printk(PFX "IRQ: TX Activity\n");
+	if (status & IRQ_RX_ACTIVITY)
+		printk(PFX "IRQ: RX Activity\n");
+	if (status & IRQ_RX_X)
+		printk(PFX "IRQ: RX X\n");
+	if (status & IRQ_RX_Y) {
+		reg = ioread32(ctl + AGNX_INT_MASK);
+		reg &= ~IRQ_RX_Y;
+		iowrite32(reg, ctl + AGNX_INT_MASK);
+		iowrite32(IRQ_RX_Y, ctl + AGNX_INT_STAT);
+		printk(PFX "IRQ: RX Y\n");
+	}
+	if (status & IRQ_RX_HASHHIT)  {
+		reg = ioread32(ctl + AGNX_INT_MASK);
+		reg &= ~IRQ_RX_HASHHIT;
+		iowrite32(reg, ctl + AGNX_INT_MASK);
+		iowrite32(IRQ_RX_HASHHIT, ctl + AGNX_INT_STAT);
+		printk(PFX "IRQ: RX Hash Hit\n");
+
+	}
+	if (status & IRQ_RX_FRAME) {
+		reg = ioread32(ctl + AGNX_INT_MASK);
+		reg &= ~IRQ_RX_FRAME;
+		iowrite32(reg, ctl + AGNX_INT_MASK);
+		iowrite32(IRQ_RX_FRAME, ctl + AGNX_INT_STAT);
+		printk(PFX "IRQ: RX Frame\n");
+ 		rx_frame_cnt++;
+	}
+	if (status & IRQ_ERR_INT) {
+		iowrite32(IRQ_ERR_INT, ctl + AGNX_INT_STAT);
+//		agnx_hw_reset(priv);
+		printk(PFX "IRQ: Error Interrupt\n");
+	}
+	if (status & IRQ_TX_QUE_FULL)
+		printk(PFX "IRQ: TX Workqueue Full\n");
+	if (status & IRQ_BANDMAN_ERR)
+		printk(PFX "IRQ: Bandwidth Management Error\n");
+	if (status & IRQ_TX_DISABLE)
+		printk(PFX "IRQ: TX Disable\n");
+	if (status & IRQ_RX_IVASESKEY)
+		printk(PFX "IRQ: RX Invalid Session Key\n");
+	if (status & IRQ_REP_THHIT)
+		printk(PFX "IRQ: Replay Threshold Hit\n");
+	if (status & IRQ_TIMER1)
+		printk(PFX "IRQ: Timer1\n");
+	if (status & IRQ_TIMER_CNT)
+		printk(PFX "IRQ: Timer Count\n");
+	if (status & IRQ_PHY_FASTINT)
+		printk(PFX "IRQ: Phy Fast Interrupt\n");
+	if (status & IRQ_PHY_SLOWINT)
+		printk(PFX "IRQ: Phy Slow Interrupt\n");
+	if (status & IRQ_OTHER)
+		printk(PFX "IRQ: 0x80000000\n");
+} /* handle_other_irq */
+
+
+static inline void route_flag_set(struct agnx_hdr *txhdr)
+{
+//	u32 reg = 0;
+
+	/* FIXME */
+/*  	reg = (0x7 << ROUTE_COMPRESSION_SHIFT) & ROUTE_COMPRESSION; */
+/* 	txhdr->reg5 = cpu_to_be32(reg); */
+ 	txhdr->reg5 = (0xa << 0x0) | (0x7 << 0x18);
+// 	txhdr->reg5 = cpu_to_be32((0xa << 0x0) | (0x7 << 0x18));
+// 	txhdr->reg5 = cpu_to_be32(0x7 << 0x0);
+}
+
+/* Return 0 if no match */
+static inline unsigned int get_power_level(unsigned int rate, unsigned int antennas_num)
+{
+	unsigned int power_level;
+
+	switch (rate) {
+	case 10:
+	case 20:
+	case 55:
+	case 60:
+	case 90:
+	case 120: power_level = 22; break;
+	case 180: power_level = 19; break;
+	case 240: power_level = 18; break;
+	case 360: power_level = 16; break;
+	case 480: power_level = 15; break;
+	case 540: power_level = 14; break;
+	default:
+		agnx_bug("Error rate setting\n");
+	}
+
+	if (power_level && (antennas_num == 2))
+		power_level -= 3;
+
+	return power_level;
+}
+
+static inline void fill_agnx_hdr(struct agnx_priv *priv, struct agnx_info *tx_info)
+{
+	struct agnx_hdr *txhdr = (struct agnx_hdr *)tx_info->skb->data;
+	size_t len;
+	u16 fc = le16_to_cpu(*(__le16 *)&tx_info->hdr);
+	u32 reg;
+
+	memset(txhdr, 0, sizeof(*txhdr));
+
+//	reg = agnx_set_bits(STATION_ID, STATION_ID_SHIFT, LOCAL_STAID);
+	reg = agnx_set_bits(STATION_ID, STATION_ID_SHIFT, BSSID_STAID);
+	reg |= agnx_set_bits(WORKQUEUE_ID, WORKQUEUE_ID_SHIFT, 0);
+	txhdr->reg4 = cpu_to_be32(reg);
+
+	/* Set the Hardware Sequence Number to 1? */
+	reg = agnx_set_bits(SEQUENCE_NUMBER, SEQUENCE_NUMBER_SHIFT, 0);
+//	reg = agnx_set_bits(SEQUENCE_NUMBER, SEQUENCE_NUMBER_SHIFT, 1);
+	reg |= agnx_set_bits(MAC_HDR_LEN, MAC_HDR_LEN_SHIFT, tx_info->hdr_len);
+	txhdr->reg1 = cpu_to_be32(reg);
+	/* Set the agnx_hdr's MAC header */
+	memcpy(txhdr->mac_hdr, &tx_info->hdr, tx_info->hdr_len);
+
+	reg = agnx_set_bits(ACK, ACK_SHIFT, 1);
+//	reg = agnx_set_bits(ACK, ACK_SHIFT, 0);
+	reg |= agnx_set_bits(MULTICAST, MULTICAST_SHIFT, 0);
+//	reg |= agnx_set_bits(MULTICAST, MULTICAST_SHIFT, 1);
+	reg |= agnx_set_bits(RELAY, RELAY_SHIFT, 0);
+	reg |= agnx_set_bits(TM, TM_SHIFT, 0);
+	txhdr->reg0 = cpu_to_be32(reg);
+
+	/* Set the long and short retry limits */
+ 	txhdr->tx.short_retry_limit = tx_info->txi->control.rates[0].count;
+ 	txhdr->tx.long_retry_limit = tx_info->txi->control.rates[0].count;
+
+	/* FIXME */
+	len = tx_info->skb->len - sizeof(*txhdr) + tx_info->hdr_len + FCS_LEN;
+	if (fc & IEEE80211_FCTL_PROTECTED)
+		len += 8;
+	len = 2398;
+	reg = agnx_set_bits(FRAG_SIZE, FRAG_SIZE_SHIFT, len);
+	len = tx_info->skb->len - sizeof(*txhdr);
+	reg |= agnx_set_bits(PAYLOAD_LEN, PAYLOAD_LEN_SHIFT, len);
+	txhdr->reg3 = cpu_to_be32(reg);
+
+	route_flag_set(txhdr);
+} /* fill_hdr */
+
+static void txm_power_set(struct agnx_priv *priv,
+			  struct ieee80211_tx_info *txi)
+{
+	struct agnx_sta_power power;
+	u32 reg;
+
+	/* FIXME */
+	if (txi->control.rates[0].idx < 0) {
+		/* For B mode Short Preamble */
+		reg = agnx_set_bits(PHY_MODE, PHY_MODE_SHIFT, AGNX_MODE_80211B_SHORT);
+//		control->tx_rate = -control->tx_rate;
+	} else
+		reg = agnx_set_bits(PHY_MODE, PHY_MODE_SHIFT, AGNX_MODE_80211G);
+//		reg = agnx_set_bits(PHY_MODE, PHY_MODE_SHIFT, AGNX_MODE_80211B_LONG);
+	reg |= agnx_set_bits(SIGNAL, SIGNAL_SHIFT, 0xB);
+	reg |= agnx_set_bits(RATE, RATE_SHIFT, 0xB);
+//	reg |= agnx_set_bits(POWER_LEVEL, POWER_LEVEL_SHIFT, 15);
+	reg |= agnx_set_bits(POWER_LEVEL, POWER_LEVEL_SHIFT, 20);
+	/* if rate < 11M set it to 0 */
+	reg |= agnx_set_bits(NUM_TRANSMITTERS, NUM_TRANSMITTERS_SHIFT, 1);
+//	reg |= agnx_set_bits(EDCF, EDCF_SHIFT, 1);
+//	reg |= agnx_set_bits(TIFS, TIFS_SHIFT, 1);
+
+	power.reg = reg;
+//	power.reg = cpu_to_le32(reg);
+
+//	set_sta_power(priv, &power, LOCAL_STAID);
+	set_sta_power(priv, &power, BSSID_STAID);
+}
+
+static inline int tx_packet_check(struct sk_buff *skb)
+{
+	unsigned int ieee_len = ieee80211_get_hdrlen_from_skb(skb);
+	if (skb->len > 2048) {
+		printk(KERN_ERR PFX "length is %d\n", skb->len);
+		agnx_bug("Too long TX skb");
+		return -1;
+	}
+	/* FIXME */
+	if (skb->len == ieee_len) {
+		printk(PFX "A strange TX packet\n");
+		return -1;
+		/* tx_faile_irqsafe(); */
+	}
+	return 0;
+}
+
+static int __agnx_tx(struct agnx_priv *priv, struct sk_buff *skb,
+		     struct agnx_ring *ring)
+{
+	struct agnx_desc *hdr_desc, *frag_desc;
+	struct agnx_info *hdr_info, *frag_info;
+	struct ieee80211_tx_info *txi = IEEE80211_SKB_CB(skb);
+	unsigned long flags;
+	unsigned int i;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* The RX interrupt need be Disable until this TX packet
+	   is handled in the next tx interrupt */
+	disable_rx_interrupt(priv);
+
+	i = ring->idx;
+	ring->idx += 2;
+/*   	if (priv->txm_idx - priv->txm_idx_sent == AGNX_TXM_RING_SIZE - 2) */
+/* 		ieee80211_stop_queue(priv->hw, 0); */
+
+	/* Set agnx header's info and desc */
+	i %= ring->size;
+	hdr_desc = ring->desc + i;
+	hdr_info = ring->info + i;
+	hdr_info->hdr_len = ieee80211_get_hdrlen_from_skb(skb);
+	memcpy(&hdr_info->hdr, skb->data, hdr_info->hdr_len);
+
+	/* Add the agnx header to the front of the SKB */
+	skb_push(skb, sizeof(struct agnx_hdr) - hdr_info->hdr_len);
+
+	hdr_info->txi = txi;
+	hdr_info->dma_len = sizeof(struct agnx_hdr);
+	hdr_info->skb = skb;
+	hdr_info->type = HEADER;
+	fill_agnx_hdr(priv, hdr_info);
+	hdr_info->mapping = pci_map_single(priv->pdev, skb->data,
+					   hdr_info->dma_len, PCI_DMA_TODEVICE);
+	do {
+		u32 frag = 0;
+		frag |= agnx_set_bits(FIRST_FRAG, FIRST_FRAG_SHIFT, 1);
+		frag |= agnx_set_bits(LAST_FRAG, LAST_FRAG_SHIFT, 0);
+		frag |= agnx_set_bits(PACKET_LEN, PACKET_LEN_SHIFT, skb->len);
+		frag |= agnx_set_bits(FIRST_FRAG_LEN, FIRST_FRAG_LEN_SHIFT, 1);
+		frag |= agnx_set_bits(OWNER, OWNER_SHIFT, 1);
+		hdr_desc->frag = cpu_to_be32(frag);
+	} while (0);
+	hdr_desc->dma_addr = cpu_to_be32(hdr_info->mapping);
+
+
+	/* Set Frag's info and desc */
+	i = (i + 1) % ring->size;
+	frag_desc = ring->desc + i;
+	frag_info = ring->info + i;
+	memcpy(frag_info, hdr_info, sizeof(struct agnx_info));
+	frag_info->type = PACKET;
+	frag_info->dma_len = skb->len - hdr_info->dma_len;
+	frag_info->mapping = pci_map_single(priv->pdev, skb->data + hdr_info->dma_len,
+					    frag_info->dma_len, PCI_DMA_TODEVICE);
+	do {
+		u32 frag = 0;
+		frag |= agnx_set_bits(FIRST_FRAG, FIRST_FRAG_SHIFT, 0);
+		frag |= agnx_set_bits(LAST_FRAG, LAST_FRAG_SHIFT, 1);
+		frag |= agnx_set_bits(PACKET_LEN, PACKET_LEN_SHIFT, skb->len);
+		frag |= agnx_set_bits(SUB_FRAG_LEN, SUB_FRAG_LEN_SHIFT, frag_info->dma_len);
+		frag_desc->frag = cpu_to_be32(frag);
+	} while (0);
+	frag_desc->dma_addr = cpu_to_be32(frag_info->mapping);
+
+	txm_power_set(priv, txi);
+
+/* 	do { */
+/* 		int j; */
+/* 		size_t len; */
+/* 		len = skb->len - hdr_info->dma_len + hdr_info->hdr_len;  */
+/* //		if (len == 614) { */
+/* 			agnx_print_desc(hdr_desc); */
+/* 			agnx_print_desc(frag_desc); */
+/* 			agnx_print_tx_hdr((struct agnx_hdr *)skb->data); */
+/* 			agnx_print_sta_power(priv, LOCAL_STAID); */
+/* 			agnx_print_sta(priv, LOCAL_STAID); */
+/* 			for (j = 0; j < 8; j++) */
+/* 				agnx_print_sta_tx_wq(priv, LOCAL_STAID, j); */
+/* 			agnx_print_sta_power(priv, BSSID_STAID); */
+/* 			agnx_print_sta(priv, BSSID_STAID); */
+/* 			for (j = 0; j < 8; j++) */
+/* 				agnx_print_sta_tx_wq(priv, BSSID_STAID, j); */
+/* 			//	} */
+/* 	} while (0); */
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* FIXME ugly code */
+	/* Trigger TXM */
+	do {
+		u32 reg;
+		reg = (ioread32(priv->ctl + AGNX_CIR_TXMCTL));
+		reg |= 0x8;
+		iowrite32((reg), priv->ctl + AGNX_CIR_TXMCTL);
+	}while (0);
+
+	/* Trigger TXD */
+	do {
+		u32 reg;
+		reg = (ioread32(priv->ctl + AGNX_CIR_TXDCTL));
+		reg |= 0x8;
+		iowrite32((reg), priv->ctl + AGNX_CIR_TXDCTL);
+	}while (0);
+
+	return 0;
+}
+
+int _agnx_tx(struct agnx_priv *priv, struct sk_buff *skb)
+{
+	u16 fctl;
+
+	if (tx_packet_check(skb))
+		return 0;
+
+/* 	print_hex_dump_bytes("agnx: TX_PACKET: ", DUMP_PREFIX_NONE, */
+/* 			     skb->data, skb->len); */
+
+        fctl = le16_to_cpu(*((__le16 *)skb->data));
+
+	if ( (fctl & IEEE80211_FCTL_FTYPE)  == IEEE80211_FTYPE_DATA )
+		return __agnx_tx(priv, skb, &priv->txd);
+	else
+		return __agnx_tx(priv, skb, &priv->txm);
+}
diff --git a/drivers/staging/agnx/xmit.h b/drivers/staging/agnx/xmit.h
new file mode 100644
index 000000000000..93ac4157e697
--- /dev/null
+++ b/drivers/staging/agnx/xmit.h
@@ -0,0 +1,250 @@
+#ifndef AGNX_XMIT_H_
+#define AGNX_XMIT_H_
+
+#include <net/mac80211.h>
+
+struct agnx_priv;
+
+static inline u32 agnx_set_bits(u32 mask, u8 shift, u32 value)
+{
+	return (value << shift) & mask;
+}
+
+static inline u32 agnx_get_bits(u32 mask, u8 shift, u32 value)
+{
+	return (value & mask) >> shift;
+}
+
+
+struct agnx_rx {
+	__be16 rx_packet_duration; /*  RX Packet Duration */
+	__be16 replay_cnt;	/* Replay Count */
+} __attribute__((__packed__));
+
+
+struct agnx_tx {
+	u8 long_retry_limit; /* Long Retry Limit */
+	u8 short_retry_limit; /* Short Retry Limit */
+	u8 long_retry_cnt;	/* Long Retry Count */
+	u8 short_retry_cnt; /* Short Retry Count */
+} __attribute__((__packed__));
+
+
+/* Copy from bcm43xx */
+#define P4D_BYT3S(magic, nr_bytes)      u8 __p4dding##magic[nr_bytes]
+#define P4D_BYTES(line, nr_bytes)       P4D_BYT3S(line, nr_bytes)
+#define PAD_BYTES(nr_bytes)             P4D_BYTES(__LINE__, nr_bytes)
+
+#define P4D_BIT3S(magic, nr_bits)       __be32 __padding##magic:nr_bits
+#define P4D_BITS(line, nr_bits)         P4D_BIT3S(line, nr_bits)
+#define PAD_BITS(nr_bits)	        P4D_BITS(__LINE__, nr_bits)
+
+
+struct agnx_hdr {
+	__be32 reg0;
+#define RTS		        0x80000000 /* RTS */
+#define RTS_SHIFT		31
+#define MULTICAST	        0x40000000 /* multicast */
+#define MULTICAST_SHIFT		30
+#define ACK			0x30000000 /* ACK */
+#define ACK_SHIFT		28
+#define TM			0x08000000 /* TM */
+#define TM_SHIFT		27
+#define RELAY			0x04000000 /* Relay */
+#define RELAY_SHIFT		26
+/* 	PAD_BITS(4); */
+#define REVISED_FCS		0x00380000 /* revised FCS */
+#define REVISED_FCS_SHIFT	19
+#define NEXT_BUFFER_ADDR	0x0007FFFF /* Next Buffer Address */
+#define NEXT_BUFFER_ADDR_SHIFT	0
+
+	__be32 reg1;
+#define MAC_HDR_LEN		0xFC000000 /* MAC Header Length  */
+#define MAC_HDR_LEN_SHIFT	26
+#define DURATION_OVERIDE	0x02000000 /* Duration Override */
+#define DURATION_OVERIDE_SHIFT	25
+#define PHY_HDR_OVERIDE		0x01000000 /* PHY Header Override */
+#define PHY_HDR_OVERIDE_SHIFT	24
+#define CRC_FAIL		0x00800000 /* CRC fail */
+#define CRC_FAIL_SHIFT		23
+/*	PAD_BITS(1); */
+#define SEQUENCE_NUMBER		0x00200000 /* Sequence Number */
+#define SEQUENCE_NUMBER_SHIFT	21
+/*	PAD_BITS(2); */
+#define BUFF_HEAD_ADDR		0x0007FFFF /* Buffer Head Address */
+#define BUFF_HEAD_ADDR_SHIFT	0
+
+	__be32 reg2;
+#define PDU_COUNT		0xFC000000 /* PDU Count */
+#define PDU_COUNT_SHIFT		26
+/* 	PAD_BITS(3); */
+#define WEP_KEY			0x00600000 /* WEP Key # */
+#define WEP_KEY_SHIFT		21
+#define USES_WEP_KEY		0x00100000 /* Uses WEP Key */
+#define USES_WEP_KEY_SHIFT	20
+#define KEEP_ALIVE		0x00080000 /* Keep alive */
+#define KEEP_ALIVE_SHIFT	19
+#define BUFF_TAIL_ADDR		0x0007FFFF /* Buffer Tail Address */
+#define BUFF_TAIL_ADDR_SHIFT	0
+
+	__be32 reg3;
+#define CTS_11G			0x80000000	/* CTS in 11g */
+#define CTS_11G_SHIFT		31
+#define RTS_11G			0x40000000	/* RTS in 11g */
+#define RTS_11G_SHIFT		30
+/* PAD_BITS(2); */
+#define FRAG_SIZE		0x0FFF0000	/* fragment size */
+#define FRAG_SIZE_SHIFT		16
+#define PAYLOAD_LEN		0x0000FFF0	/* payload length */
+#define PAYLOAD_LEN_SHIFT	4
+#define FRAG_NUM		0x0000000F	/* number of frags */
+#define FRAG_NUM_SHIFT		0
+
+	__be32 reg4;
+/* 	PAD_BITS(4); */
+#define RELAY_STAID		0x0FFF0000 /* relayStald */
+#define RELAY_STAID_SHIFT	16
+#define STATION_ID		0x0000FFF0 /* Station ID */
+#define STATION_ID_SHIFT	4
+#define WORKQUEUE_ID		0x0000000F /* Workqueue ID */
+#define WORKQUEUE_ID_SHIFT	0
+
+	/* FIXME this register maybe is LE? */
+	__be32 reg5;
+/* 	PAD_BITS(4); */
+#define ROUTE_HOST		0x0F000000
+#define ROUTE_HOST_SHIFT	24
+#define ROUTE_CARD_CPU		0x00F00000
+#define ROUTE_CARD_CPU_SHIFT	20
+#define ROUTE_ENCRYPTION	0x000F0000
+#define ROUTE_ENCRYPTION_SHIFT	16
+#define ROUTE_TX		0x0000F000
+#define ROUTE_TX_SHIFT		12
+#define ROUTE_RX1		0x00000F00
+#define ROUTE_RX1_SHIFT		8
+#define ROUTE_RX2		0x000000F0
+#define ROUTE_RX2_SHIFT		4
+#define ROUTE_COMPRESSION	0x0000000F
+#define ROUTE_COMPRESSION_SHIFT 0
+
+	__be32 _11g0;			/* 11g */
+	__be32 _11g1;			/* 11g */
+	__be32 _11b0;			/* 11b */
+	__be32 _11b1;			/* 11b */
+	u8 mac_hdr[32];			/* MAC header */
+
+	__be16 rts_duration;		/* RTS duration */
+	__be16 last_duration;		/* Last duration */
+	__be16 sec_last_duration;	/* Second to Last duration */
+	__be16 other_duration;		/* Other duration */
+	__be16 tx_last_duration;	/* TX Last duration */
+	__be16 tx_other_duration;	/* TX Other Duration */
+	__be16 last_11g_len;		/* Length of last 11g */
+	__be16 other_11g_len;		/* Lenght of other 11g */
+
+	__be16 last_11b_len;		/* Length of last 11b */
+	__be16 other_11b_len;		/* Lenght of other 11b */
+
+
+	__be16 reg6;
+#define MBF			0xF000 /* mbf */
+#define MBF_SHIFT		12
+#define RSVD4			0x0FFF /* rsvd4 */
+#define RSVD4_SHIFT		0
+
+	__be16 rx_frag_stat;	/* RX fragmentation status */
+
+	__be32 time_stamp;	/* TimeStamp */
+	__be32 phy_stats_hi;	/* PHY stats hi */
+	__be32 phy_stats_lo;	/* PHY stats lo */
+	__be32 mic_key0;	/* MIC key 0 */
+	__be32 mic_key1;	/* MIC key 1 */
+
+	union {			/* RX/TX Union */
+		struct agnx_rx rx;
+		struct agnx_tx tx;
+	};
+
+	u8 rx_channel;		/* Recieve Channel */
+	PAD_BYTES(3);
+
+	u8 reserved[4];
+} __attribute__((__packed__));
+
+
+struct agnx_desc {
+#define PACKET_LEN		0xFFF00000
+#define PACKET_LEN_SHIFT	20
+/* ------------------------------------------------ */
+#define FIRST_PACKET_MASK	0x00080000
+#define FIRST_PACKET_MASK_SHIFT	19
+#define FIRST_RESERV2		0x00040000
+#define FIRST_RESERV2_SHIFT	18
+#define FIRST_TKIP_ERROR	0x00020000
+#define FIRST_TKIP_ERROR_SHIFT	17
+#define FIRST_TKIP_PACKET	0x00010000
+#define FIRST_TKIP_PACKET_SHIFT	16
+#define FIRST_RESERV1		0x0000F000
+#define FIRST_RESERV1_SHIFT	12
+#define FIRST_FRAG_LEN		0x00000FF8
+#define FIRST_FRAG_LEN_SHIFT	3
+/* ------------------------------------------------ */
+#define SUB_RESERV2		0x000c0000
+#define SUB_RESERV2_SHIFT	18
+#define SUB_TKIP_ERROR		0x00020000
+#define SUB_TKIP_ERROR_SHIFT	17
+#define SUB_TKIP_PACKET		0x00010000
+#define SUB_TKIP_PACKET_SHIFT	16
+#define SUB_RESERV1		0x00008000
+#define SUB_RESERV1_SHIFT	15
+#define SUB_FRAG_LEN		0x00007FF8
+#define SUB_FRAG_LEN_SHIFT	3
+/* ------------------------------------------------ */
+#define FIRST_FRAG		0x00000004
+#define FIRST_FRAG_SHIFT	2
+#define LAST_FRAG		0x00000002
+#define LAST_FRAG_SHIFT		1
+#define OWNER			0x00000001
+#define OWNER_SHIFT		0
+	__be32 frag;
+	__be32 dma_addr;
+} __attribute__((__packed__));
+
+enum {HEADER, PACKET};
+
+struct agnx_info {
+        struct sk_buff *skb;
+        dma_addr_t mapping;
+	u32 dma_len;		/* dma buffer len  */
+	/* Below fields only usful for tx */
+	u32 hdr_len;		/* ieee80211 header length */
+	unsigned int type;
+        struct ieee80211_tx_info *txi;
+        struct ieee80211_hdr hdr;
+};
+
+
+struct agnx_ring {
+	struct agnx_desc *desc;
+	dma_addr_t dma;
+	struct agnx_info *info;
+	/* Will lead to overflow when sent packet number enough? */
+	unsigned int idx;
+	unsigned int idx_sent;		/* only usful for txd and txm */
+	unsigned int size;
+};
+
+#define AGNX_RX_RING_SIZE	128
+#define AGNX_TXD_RING_SIZE	256
+#define AGNX_TXM_RING_SIZE	128
+
+void disable_rx_interrupt(struct agnx_priv *priv);
+void enable_rx_interrupt(struct agnx_priv *priv);
+int fill_rings(struct agnx_priv *priv);
+void unfill_rings(struct agnx_priv *priv);
+void handle_rx_irq(struct agnx_priv *priv);
+void handle_txd_irq(struct agnx_priv *priv);
+void handle_txm_irq(struct agnx_priv *priv);
+void handle_other_irq(struct agnx_priv *priv);
+int _agnx_tx(struct agnx_priv *priv, struct sk_buff *skb);
+#endif /* AGNX_XMIT_H_ */
diff --git a/drivers/staging/altpciechdma/Kconfig b/drivers/staging/altpciechdma/Kconfig
new file mode 100644
index 000000000000..0f4bf92cbbfb
--- /dev/null
+++ b/drivers/staging/altpciechdma/Kconfig
@@ -0,0 +1,10 @@
+config ALTERA_PCIE_CHDMA
+	tristate "Altera PCI Express Chaining DMA driver"
+	depends on PCI
+	default N
+	---help---
+	  A reference driver that exercises the Chaining DMA logic reference
+	  design generated along the Altera FPGA PCI Express soft or hard core,
+	  only if instantiated using the MegaWizard, not the SOPC builder, of
+	  Quartus 8.1.
+
diff --git a/drivers/staging/altpciechdma/Makefile b/drivers/staging/altpciechdma/Makefile
new file mode 100644
index 000000000000..c08c8437f4db
--- /dev/null
+++ b/drivers/staging/altpciechdma/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_ALTERA_PCIE_CHDMA)	+= altpciechdma.o
+
diff --git a/drivers/staging/altpciechdma/TODO b/drivers/staging/altpciechdma/TODO
new file mode 100644
index 000000000000..12c945fd61e1
--- /dev/null
+++ b/drivers/staging/altpciechdma/TODO
@@ -0,0 +1,15 @@
+DONE:
+    - functionality similar to logic testbench
+
+TODO:
+	- checkpatch.pl cleanups.
+	- keep state of DMA engines.
+	- keep data structure that keeps state of each transfer.
+	- interrupt handler should iterate over outstanding descriptor tables.
+	- complete userspace cdev to read/write using the DMA engines.
+	- split off the DMA support functions in a module, re-usable by custom
+	  drivers.
+
+Please coordinate work with, and send patches to
+Leon Woestenberg <leon@sidebranch.com>
+
diff --git a/drivers/staging/altpciechdma/altpciechdma.c b/drivers/staging/altpciechdma/altpciechdma.c
new file mode 100644
index 000000000000..8e2b4ca0651d
--- /dev/null
+++ b/drivers/staging/altpciechdma/altpciechdma.c
@@ -0,0 +1,1184 @@
+/**
+ * Driver for Altera PCIe core chaining DMA reference design.
+ *
+ * Copyright (C) 2008 Leon Woestenberg  <leon.woestenberg@axon.tv>
+ * Copyright (C) 2008 Nickolas Heppermann  <heppermannwdt@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.
+ *
+ * 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.
+ *
+ *
+ * Rationale: This driver exercises the chaining DMA read and write engine
+ * in the reference design. It is meant as a complementary reference
+ * driver that can be used for testing early designs as well as a basis to
+ * write your custom driver.
+ *
+ * Status: Test results from Leon Woestenberg  <leon.woestenberg@axon.tv>:
+ *
+ * Sendero Board w/ Cyclone II EP2C35F672C6N, PX1011A PCIe x1 PHY on a
+ * Dell Precision 370 PC, x86, kernel 2.6.20 from Ubuntu 7.04.
+ *
+ * Sendero Board w/ Cyclone II EP2C35F672C6N, PX1011A PCIe x1 PHY on a
+ * Freescale MPC8313E-RDB board, PowerPC, 2.6.24 w/ Freescale patches.
+ *
+ * Driver tests passed with PCIe Compiler 8.1. With PCIe 8.0 the DMA
+ * loopback test had reproducable compare errors. I assume a change
+ * in the compiler or reference design, but could not find evidence nor
+ * documentation on a change or fix in that direction.
+ *
+ * The reference design does not have readable locations and thus a
+ * dummy read, used to flush PCI posted writes, cannot be performed.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/cdev.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+
+
+/* by default do not build the character device interface */
+/* XXX It is non-functional yet */
+#ifndef ALTPCIECHDMA_CDEV
+#  define ALTPCIECHDMA_CDEV 0
+#endif
+
+/* build the character device interface? */
+#if ALTPCIECHDMA_CDEV
+#  define MAX_CHDMA_SIZE (8 * 1024 * 1024)
+#  include "mapper_user_to_sg.h"
+#endif
+
+/** driver name, mimicks Altera naming of the reference design */
+#define DRV_NAME "altpciechdma"
+/** number of BARs on the device */
+#define APE_BAR_NUM (6)
+/** BAR number where the RCSLAVE memory sits */
+#define APE_BAR_RCSLAVE (0)
+/** BAR number where the Descriptor Header sits */
+#define APE_BAR_HEADER (2)
+
+/** maximum size in bytes of the descriptor table, chdma logic limit */
+#define APE_CHDMA_TABLE_SIZE (4096)
+/* single transfer must not exceed 255 table entries. worst case this can be
+ * achieved by 255 scattered pages, with only a single byte in the head and
+ * tail pages. 253 * PAGE_SIZE is a safe upper bound for the transfer size.
+ */
+#define APE_CHDMA_MAX_TRANSFER_LEN (253 * PAGE_SIZE)
+
+/**
+ * Specifies those BARs to be mapped and the length of each mapping.
+ *
+ * Zero (0) means do not map, otherwise specifies the BAR lengths to be mapped.
+ * If the actual BAR length is less, this is considered an error; then
+ * reconfigure your PCIe core.
+ *
+ * @see ug_pci_express 8.0, table 7-2 at page 7-13.
+ */
+static const unsigned long bar_min_len[APE_BAR_NUM] =
+	{ 32768, 0, 256, 0, 32768, 0 };
+
+/**
+ * Descriptor Header, controls the DMA read engine or write engine.
+ *
+ * The descriptor header is the main data structure for starting DMA transfers.
+ *
+ * It sits in End Point (FPGA) memory BAR[2] for 32-bit or BAR[3:2] for 64-bit.
+ * It references a descriptor table which exists in Root Complex (PC) memory.
+ * Writing the rclast field starts the DMA operation, thus all other structures
+ * and fields must be setup before doing so.
+ *
+ * @see ug_pci_express 8.0, tables 7-3, 7-4 and 7-5 at page 7-14.
+ * @note This header must be written in four 32-bit (PCI DWORD) writes.
+ */
+struct ape_chdma_header {
+	/**
+	 * w0 consists of two 16-bit fields:
+	 * lsb u16 number; number of descriptors in ape_chdma_table
+	 * msb u16 control; global control flags
+	 */
+	u32 w0;
+	/* bus address to ape_chdma_table in Root Complex memory */
+	u32 bdt_addr_h;
+	u32 bdt_addr_l;
+	/**
+	 * w3 consists of two 16-bit fields:
+	 * - lsb u16 rclast; last descriptor number available in Root Complex
+	 *    - zero (0) means the first descriptor is ready,
+	 *    - one (1) means two descriptors are ready, etc.
+	 * - msb u16 reserved;
+	 *
+	 * @note writing to this memory location starts the DMA operation!
+	 */
+	u32 w3;
+} __attribute__ ((packed));
+
+/**
+ * Descriptor Entry, describing a (non-scattered) single memory block transfer.
+ *
+ * There is one descriptor for each memory block involved in the transfer, a
+ * block being a contiguous address range on the bus.
+ *
+ * Multiple descriptors are chained by means of the ape_chdma_table data
+ * structure.
+ *
+ * @see ug_pci_express 8.0, tables 7-6, 7-7 and 7-8 at page 7-14 and page 7-15.
+ */
+struct ape_chdma_desc {
+	/**
+	 * w0 consists of two 16-bit fields:
+	 * number of DWORDS to transfer
+	 * - lsb u16 length;
+	 * global control
+	 * - msb u16 control;
+	 */
+	u32 w0;
+	/* address of memory in the End Point */
+	u32 ep_addr;
+	/* bus address of source or destination memory in the Root Complex */
+	u32 rc_addr_h;
+	u32 rc_addr_l;
+} __attribute__ ((packed));
+
+/**
+ * Descriptor Table, an array of descriptors describing a chained transfer.
+ *
+ * An array of descriptors, preceded by workspace for the End Point.
+ * It exists in Root Complex memory.
+ *
+ * The End Point can update its last completed descriptor number in the
+ * eplast field if requested by setting the EPLAST_ENA bit either
+ * globally in the header's or locally in any descriptor's control field.
+ *
+ * @note this structure may not exceed 4096 bytes. This results in a
+ * maximum of 4096 / (4 * 4) - 1 = 255 descriptors per chained transfer.
+ *
+ * @see ug_pci_express 8.0, tables 7-9, 7-10 and 7-11 at page 7-17 and page 7-18.
+ */
+struct ape_chdma_table {
+	/* workspace 0x00-0x0b, reserved */
+	u32 reserved1[3];
+	/* workspace 0x0c-0x0f, last descriptor handled by End Point */
+	u32 w3;
+	/* the actual array of descriptors
+    * 0x10-0x1f, 0x20-0x2f, ... 0xff0-0xfff (255 entries)
+    */
+	struct ape_chdma_desc desc[255];
+} __attribute__ ((packed));
+
+/**
+ * Altera PCI Express ('ape') board specific book keeping data
+ *
+ * Keeps state of the PCIe core and the Chaining DMA controller
+ * application.
+ */
+struct ape_dev {
+	/** the kernel pci device data structure provided by probe() */
+	struct pci_dev *pci_dev;
+	/**
+	 * kernel virtual address of the mapped BAR memory and IO regions of
+	 * the End Point. Used by map_bars()/unmap_bars().
+	 */
+	void * __iomem bar[APE_BAR_NUM];
+	/** kernel virtual address for Descriptor Table in Root Complex memory */
+	struct ape_chdma_table *table_virt;
+	/**
+	 * bus address for the Descriptor Table in Root Complex memory, in
+	 * CPU-native endianess
+	 */
+	dma_addr_t table_bus;
+	/* if the device regions could not be allocated, assume and remember it
+	 * is in use by another driver; this driver must not disable the device.
+	 */
+	int in_use;
+	/* whether this driver enabled msi for the device */
+	int msi_enabled;
+	/* whether this driver could obtain the regions */
+	int got_regions;
+	/* irq line succesfully requested by this driver, -1 otherwise */
+	int irq_line;
+	/* board revision */
+	u8 revision;
+	/* interrupt count, incremented by the interrupt handler */
+	int irq_count;
+#if ALTPCIECHDMA_CDEV
+	/* character device */
+	dev_t cdevno;
+	struct cdev cdev;
+	/* user space scatter gather mapper */
+	struct sg_mapping_t *sgm;
+#endif
+};
+
+/**
+ * Using the subsystem vendor id and subsystem id, it is possible to
+ * distinguish between different cards bases around the same
+ * (third-party) logic core.
+ *
+ * Default Altera vendor and device ID's, and some (non-reserved)
+ * ID's are now used here that are used amongst the testers/developers.
+ */
+static const struct pci_device_id ids[] = {
+	{ PCI_DEVICE(0x1172, 0xE001), },
+	{ PCI_DEVICE(0x2071, 0x2071), },
+	{ 0, }
+};
+MODULE_DEVICE_TABLE(pci, ids);
+
+#if ALTPCIECHDMA_CDEV
+/* prototypes for character device */
+static int sg_init(struct ape_dev *ape);
+static void sg_exit(struct ape_dev *ape);
+#endif
+
+/**
+ * altpciechdma_isr() - Interrupt handler
+ *
+ */
+static irqreturn_t altpciechdma_isr(int irq, void *dev_id)
+{
+	struct ape_dev *ape = (struct ape_dev *)dev_id;
+	if (!ape)
+		return IRQ_NONE;
+	ape->irq_count++;
+	return IRQ_HANDLED;
+}
+
+static int __devinit scan_bars(struct ape_dev *ape, struct pci_dev *dev)
+{
+	int i;
+	for (i = 0; i < APE_BAR_NUM; i++) {
+		unsigned long bar_start = pci_resource_start(dev, i);
+		if (bar_start) {
+			unsigned long bar_end = pci_resource_end(dev, i);
+			unsigned long bar_flags = pci_resource_flags(dev, i);
+			printk(KERN_DEBUG "BAR%d 0x%08lx-0x%08lx flags 0x%08lx\n",
+			  i, bar_start, bar_end, bar_flags);
+		}
+	}
+	return 0;
+}
+
+/**
+ * Unmap the BAR regions that had been mapped earlier using map_bars()
+ */
+static void unmap_bars(struct ape_dev *ape, struct pci_dev *dev)
+{
+	int i;
+	for (i = 0; i < APE_BAR_NUM; i++) {
+	  /* is this BAR mapped? */
+		if (ape->bar[i]) {
+			/* unmap BAR */
+			pci_iounmap(dev, ape->bar[i]);
+			ape->bar[i] = NULL;
+		}
+	}
+}
+
+/**
+ * Map the device memory regions into kernel virtual address space after
+ * verifying their sizes respect the minimum sizes needed, given by the
+ * bar_min_len[] array.
+ */
+static int __devinit map_bars(struct ape_dev *ape, struct pci_dev *dev)
+{
+	int rc;
+	int i;
+	/* iterate through all the BARs */
+	for (i = 0; i < APE_BAR_NUM; i++) {
+		unsigned long bar_start = pci_resource_start(dev, i);
+		unsigned long bar_end = pci_resource_end(dev, i);
+		unsigned long bar_length = bar_end - bar_start + 1;
+		ape->bar[i] = NULL;
+		/* do not map, and skip, BARs with length 0 */
+		if (!bar_min_len[i])
+			continue;
+		/* do not map BARs with address 0 */
+		if (!bar_start || !bar_end) {
+            printk(KERN_DEBUG "BAR #%d is not present?!\n", i);
+			rc = -1;
+			goto fail;
+		}
+		bar_length = bar_end - bar_start + 1;
+		/* BAR length is less than driver requires? */
+		if (bar_length < bar_min_len[i]) {
+            printk(KERN_DEBUG "BAR #%d length = %lu bytes but driver "
+            "requires at least %lu bytes\n", i, bar_length, bar_min_len[i]);
+			rc = -1;
+			goto fail;
+		}
+		/* map the device memory or IO region into kernel virtual
+		 * address space */
+		ape->bar[i] = pci_iomap(dev, i, bar_min_len[i]);
+		if (!ape->bar[i]) {
+			printk(KERN_DEBUG "Could not map BAR #%d.\n", i);
+			rc = -1;
+			goto fail;
+		}
+        printk(KERN_DEBUG "BAR[%d] mapped at 0x%p with length %lu(/%lu).\n", i,
+			ape->bar[i], bar_min_len[i], bar_length);
+	}
+	/* succesfully mapped all required BAR regions */
+	rc = 0;
+	goto success;
+fail:
+	/* unmap any BARs that we did map */
+	unmap_bars(ape, dev);
+success:
+	return rc;
+}
+
+#if 0 /* not yet implemented fully FIXME add opcode */
+static void __devinit rcslave_test(struct ape_dev *ape, struct pci_dev *dev)
+{
+	u32 *rcslave_mem = (u32 *)ape->bar[APE_BAR_RCSLAVE];
+	u32 result = 0;
+	/** this number is assumed to be different each time this test runs */
+	u32 seed = (u32)jiffies;
+	u32 value = seed;
+	int i;
+
+	/* write loop */
+	value = seed;
+	for (i = 1024; i < 32768 / 4 ; i++) {
+		printk(KERN_DEBUG "Writing 0x%08x to 0x%p.\n",
+			(u32)value, (void *)rcslave_mem + i);
+		iowrite32(value, rcslave_mem + i);
+		value++;
+	}
+	/* read-back loop */
+	value = seed;
+	for (i = 1024; i < 32768 / 4; i++) {
+		result = ioread32(rcslave_mem + i);
+		if (result != value) {
+			printk(KERN_DEBUG "Wrote 0x%08x to 0x%p, but read back 0x%08x.\n",
+				(u32)value, (void *)rcslave_mem + i, (u32)result);
+			break;
+		}
+		value++;
+	}
+}
+#endif
+
+/* obtain the 32 most significant (high) bits of a 32-bit or 64-bit address */
+#define pci_dma_h(addr) ((addr >> 16) >> 16)
+/* obtain the 32 least significant (low) bits of a 32-bit or 64-bit address */
+#define pci_dma_l(addr) (addr & 0xffffffffUL)
+
+/* ape_fill_chdma_desc() - Fill a Altera PCI Express Chaining DMA descriptor
+ *
+ * @desc pointer to descriptor to be filled
+ * @addr root complex address
+ * @ep_addr end point address
+ * @len number of bytes, must be a multiple of 4.
+ */
+static inline void ape_chdma_desc_set(struct ape_chdma_desc *desc, dma_addr_t addr, u32 ep_addr, int len)
+{
+  BUG_ON(len & 3);
+	desc->w0 = cpu_to_le32(len / 4);
+	desc->ep_addr = cpu_to_le32(ep_addr);
+	desc->rc_addr_h = cpu_to_le32(pci_dma_h(addr));
+	desc->rc_addr_l = cpu_to_le32(pci_dma_l(addr));
+}
+
+/*
+ * ape_sg_to_chdma_table() - Create a device descriptor table from a scatterlist.
+ *
+ * The scatterlist must have been mapped by pci_map_sg(sgm->sgl).
+ *
+ * @sgl scatterlist.
+ * @nents Number of entries in the scatterlist.
+ * @first Start index in the scatterlist sgm->sgl.
+ * @ep_addr End Point address for the scatter/gather transfer.
+ * @desc pointer to first descriptor
+ *
+ * Returns Number of entries in the table on success, -1 on error.
+ */
+static int ape_sg_to_chdma_table(struct scatterlist *sgl, int nents, int first, struct ape_chdma_desc *desc, u32 ep_addr)
+{
+	int i = first, j = 0;
+	/* inspect first entry */
+	dma_addr_t addr = sg_dma_address(&sgl[i]);
+	unsigned int len = sg_dma_len(&sgl[i]);
+	/* contiguous block */
+	dma_addr_t cont_addr = addr;
+	unsigned int cont_len = len;
+	/* iterate over remaining entries */
+	for (; j < 25 && i < nents - 1; i++) {
+		/* bus address of next entry i + 1 */
+		dma_addr_t next = sg_dma_address(&sgl[i + 1]);
+		/* length of this entry i */
+		len = sg_dma_len(&sgl[i]);
+		printk(KERN_DEBUG "%04d: addr=0x%08x length=0x%08x\n", i, addr, len);
+		/* entry i + 1 is non-contiguous with entry i? */
+		if (next != addr + len) {
+			/* TODO create entry here (we could overwrite i) */
+			printk(KERN_DEBUG "%4d: cont_addr=0x%08x cont_len=0x%08x\n", j, cont_addr, cont_len);
+			/* set descriptor for contiguous transfer */
+			ape_chdma_desc_set(&desc[j], cont_addr, ep_addr, cont_len);
+			/* next end point memory address */
+			ep_addr += cont_len;
+			/* start new contiguous block */
+			cont_addr = next;
+			cont_len = 0;
+			j++;
+		}
+		/* add entry i + 1 to current contiguous block */
+		cont_len += len;
+		/* goto entry i + 1 */
+		addr = next;
+	}
+	/* TODO create entry here  (we could overwrite i) */
+	printk(KERN_DEBUG "%04d: addr=0x%08x length=0x%08x\n", i, addr, len);
+	printk(KERN_DEBUG "%4d: cont_addr=0x%08x length=0x%08x\n", j, cont_addr, cont_len);
+	j++;
+	return j;
+}
+
+/* compare buffers */
+static inline int compare(u32 *p, u32 *q, int len)
+{
+	int result = -1;
+	int fail = 0;
+	int i;
+	for (i = 0; i < len / 4; i++) {
+		if (*p == *q) {
+			/* every so many u32 words, show equals */
+			if ((i & 255) == 0)
+				printk(KERN_DEBUG "[%p] = 0x%08x    [%p] = 0x%08x\n", p, *p, q, *q);
+		} else {
+			fail++;
+			/* show the first few miscompares */
+			if (fail < 10) {
+                printk(KERN_DEBUG "[%p] = 0x%08x != [%p] = 0x%08x ?!\n", p, *p, q, *q);
+            /* but stop after a while */
+            } else if (fail == 10) {
+                printk(KERN_DEBUG "---more errors follow! not printed---\n");
+		  	} else {
+				/* stop compare after this many errors */
+                break;
+            }
+		}
+		p++;
+		q++;
+	}
+	if (!fail)
+		result = 0;
+	return result;
+}
+
+/* dma_test() - Perform DMA loop back test to end point and back to root complex.
+ *
+ * Allocate a cache-coherent buffer in host memory, consisting of four pages.
+ *
+ * Fill the four memory pages such that each 32-bit word contains its own address.
+ *
+ * Now perform a loop back test, have the end point device copy the first buffer
+ * half to end point memory, then have it copy back into the second half.
+ *
+ *   Create a descriptor table to copy the first buffer half into End Point
+ *   memory. Instruct the End Point to do a DMA read using that table.
+ *
+ *   Create a descriptor table to copy End Point memory to the second buffer
+ *   half. Instruct the End Point to do a DMA write using that table.
+ *
+ * Compare results, fail or pass.
+ *
+ */
+static int __devinit dma_test(struct ape_dev *ape, struct pci_dev *dev)
+{
+	/* test result; guilty until proven innocent */
+	int result = -1;
+	/* the DMA read header sits at address 0x00 of the DMA engine BAR */
+	struct ape_chdma_header *write_header = (struct ape_chdma_header *)ape->bar[APE_BAR_HEADER];
+	/* the write DMA header sits after the read header at address 0x10 */
+	struct ape_chdma_header *read_header = write_header + 1;
+	/* virtual address of the allocated buffer */
+	u8 *buffer_virt = 0;
+	/* bus address of the allocated buffer */
+	dma_addr_t buffer_bus = 0;
+	int i, n = 0, irq_count;
+
+	/* temporary value used to construct 32-bit data words */
+	u32 w;
+
+	printk(KERN_DEBUG "bar_tests(), PAGE_SIZE = 0x%0x\n", (int)PAGE_SIZE);
+	printk(KERN_DEBUG "write_header = 0x%p.\n", write_header);
+	printk(KERN_DEBUG "read_header = 0x%p.\n", read_header);
+	printk(KERN_DEBUG "&write_header->w3 = 0x%p\n", &write_header->w3);
+	printk(KERN_DEBUG "&read_header->w3 = 0x%p\n", &read_header->w3);
+	printk(KERN_DEBUG "ape->table_virt = 0x%p.\n", ape->table_virt);
+
+	if (!write_header || !read_header || !ape->table_virt)
+        goto fail;
+
+	/* allocate and map coherently-cached memory for a DMA-able buffer */
+	/* @see 2.6.26.2/Documentation/DMA-mapping.txt line 318 */
+	buffer_virt = (u8 *)pci_alloc_consistent(dev, PAGE_SIZE * 4, &buffer_bus);
+	if (!buffer_virt) {
+		printk(KERN_DEBUG "Could not allocate coherent DMA buffer.\n");
+		goto fail;
+	}
+	printk(KERN_DEBUG "Allocated cache-coherent DMA buffer (virtual address = 0x%016llx, bus address = 0x%016llx).\n",
+		(u64)buffer_virt, (u64)buffer_bus);
+
+	/* fill first half of buffer with its virtual address as data */
+	for (i = 0; i < 4 * PAGE_SIZE; i += 4)
+#if 0
+		*(u32 *)(buffer_virt + i) = i / PAGE_SIZE + 1;
+#else
+		*(u32 *)(buffer_virt + i) = (buffer_virt + i);
+#endif
+#if 0
+  compare((u32 *)buffer_virt, (u32 *)(buffer_virt + 2 * PAGE_SIZE), 8192);
+#endif
+
+#if 0
+	/* fill second half of buffer with zeroes */
+	for (i = 2 * PAGE_SIZE; i < 4 * PAGE_SIZE; i += 4)
+		*(u32 *)(buffer_virt + i) = 0;
+#endif
+
+	/* invalidate EPLAST, outside 0-255, 0xFADE is from the testbench */
+	ape->table_virt->w3 = cpu_to_le32(0x0000FADE);
+
+	/* fill in first descriptor */
+	n = 0;
+	/* read 8192 bytes from RC buffer to EP address 4096 */
+	ape_chdma_desc_set(&ape->table_virt->desc[n], buffer_bus, 4096, 2 * PAGE_SIZE);
+#if 1
+	for (i = 0; i < 255; i++) {
+		ape_chdma_desc_set(&ape->table_virt->desc[i], buffer_bus, 4096, 2 * PAGE_SIZE);
+	}
+	/* index of last descriptor */
+	n = i - 1;
+#endif
+#if 0
+	/* fill in next descriptor */
+	n++;
+	/* read 1024 bytes from RC buffer to EP address 4096 + 1024 */
+	ape_chdma_desc_set(&ape->table_virt->desc[n], buffer_bus + 1024, 4096 + 1024, 1024);
+#endif
+
+#if 1
+	/* enable MSI after the last descriptor is completed */
+	if (ape->msi_enabled)
+		ape->table_virt->desc[n].w0 |= cpu_to_le32(1UL << 16)/*local MSI*/;
+#endif
+#if 0
+	/* dump descriptor table for debugging */
+	printk(KERN_DEBUG "Descriptor Table (Read, in Root Complex Memory, # = %d)\n", n + 1);
+	for (i = 0; i < 4 + (n + 1) * 4; i += 4) {
+		u32 *p = (u32 *)ape->table_virt;
+		p += i;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (LEN=0x%x)\n", (u32)p, (u32)p & 15, *p, 4 * le32_to_cpu(*p));
+		p++;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (EPA=0x%x)\n", (u32)p, (u32)p & 15, *p, le32_to_cpu(*p));
+		p++;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (RCH=0x%x)\n", (u32)p, (u32)p & 15, *p, le32_to_cpu(*p));
+		p++;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (RCL=0x%x)\n", (u32)p, (u32)p & 15, *p, le32_to_cpu(*p));
+	}
+#endif
+	/* set available number of descriptors in table */
+	w = (u32)(n + 1);
+	w |= (1UL << 18)/*global EPLAST_EN*/;
+#if 0
+	if (ape->msi_enabled)
+		w |= (1UL << 17)/*global MSI*/;
+#endif
+	printk(KERN_DEBUG "writing 0x%08x to 0x%p\n", w, (void *)&read_header->w0);
+	iowrite32(w, &read_header->w0);
+
+	/* write table address (higher 32-bits) */
+	printk(KERN_DEBUG "writing 0x%08x to 0x%p\n", (u32)((ape->table_bus >> 16) >> 16), (void *)&read_header->bdt_addr_h);
+	iowrite32(pci_dma_h(ape->table_bus), &read_header->bdt_addr_h);
+
+	/* write table address (lower 32-bits) */
+	printk(KERN_DEBUG "writing 0x%08x to 0x%p\n", (u32)(ape->table_bus & 0xffffffffUL), (void *)&read_header->bdt_addr_l);
+	iowrite32(pci_dma_l(ape->table_bus), &read_header->bdt_addr_l);
+
+	/* memory write barrier */
+	wmb();
+	printk(KERN_DEBUG "Flush posted writes\n");
+	/** FIXME Add dummy read to flush posted writes but need a readable location! */
+#if 0
+	(void)ioread32();
+#endif
+
+	/* remember IRQ count before the transfer */
+	irq_count = ape->irq_count;
+	/* write number of descriptors - this starts the DMA */
+	printk(KERN_DEBUG "\nStart DMA read\n");
+	printk(KERN_DEBUG "writing 0x%08x to 0x%p\n", (u32)n, (void *)&read_header->w3);
+	iowrite32(n, &read_header->w3);
+	printk(KERN_DEBUG "EPLAST = %lu\n", le32_to_cpu(*(u32 *)&ape->table_virt->w3) & 0xffffUL);
+
+	/** memory write barrier */
+	wmb();
+	/* dummy read to flush posted writes */
+	/* FIXME Need a readable location! */
+#if 0
+	(void)ioread32();
+#endif
+	printk(KERN_DEBUG "POLL FOR READ:\n");
+	/* poll for chain completion, 1000 times 1 millisecond */
+	for (i = 0; i < 100; i++) {
+		volatile u32 *p = &ape->table_virt->w3;
+		u32 eplast = le32_to_cpu(*p) & 0xffffUL;
+		printk(KERN_DEBUG "EPLAST = %u, n = %d\n", eplast, n);
+		if (eplast == n) {
+			printk(KERN_DEBUG "DONE\n");
+            /* print IRQ count before the transfer */
+			printk(KERN_DEBUG "#IRQs during transfer: %d\n", ape->irq_count - irq_count);
+			break;
+		}
+		udelay(100);
+	}
+
+	/* invalidate EPLAST, outside 0-255, 0xFADE is from the testbench */
+	ape->table_virt->w3 = cpu_to_le32(0x0000FADE);
+
+	/* setup first descriptor */
+	n = 0;
+	ape_chdma_desc_set(&ape->table_virt->desc[n], buffer_bus + 8192, 4096, 2 * PAGE_SIZE);
+#if 1
+	for (i = 0; i < 255; i++) {
+		ape_chdma_desc_set(&ape->table_virt->desc[i], buffer_bus + 8192, 4096, 2 * PAGE_SIZE);
+	}
+	/* index of last descriptor */
+	n = i - 1;
+#endif
+#if 1 /* test variable, make a module option later */
+	if (ape->msi_enabled)
+		ape->table_virt->desc[n].w0 |= cpu_to_le32(1UL << 16)/*local MSI*/;
+#endif
+#if 0
+	/* dump descriptor table for debugging */
+	printk(KERN_DEBUG "Descriptor Table (Write, in Root Complex Memory, # = %d)\n", n + 1);
+	for (i = 0; i < 4 + (n + 1) * 4; i += 4) {
+		u32 *p = (u32 *)ape->table_virt;
+		p += i;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (LEN=0x%x)\n", (u32)p, (u32)p & 15, *p, 4 * le32_to_cpu(*p));
+		p++;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (EPA=0x%x)\n", (u32)p, (u32)p & 15, *p, le32_to_cpu(*p));
+		p++;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (RCH=0x%x)\n", (u32)p, (u32)p & 15, *p, le32_to_cpu(*p));
+		p++;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (RCL=0x%x)\n", (u32)p, (u32)p & 15, *p, le32_to_cpu(*p));
+	}
+#endif
+
+	/* set number of available descriptors in the table */
+	w = (u32)(n + 1);
+	/* enable updates of eplast for each descriptor completion */
+	w |= (u32)(1UL << 18)/*global EPLAST_EN*/;
+#if 0 // test variable, make a module option later
+	/* enable MSI for each descriptor completion */
+	if (ape->msi_enabled)
+		w |= (1UL << 17)/*global MSI*/;
+#endif
+	iowrite32(w, &write_header->w0);
+	iowrite32(pci_dma_h(ape->table_bus), &write_header->bdt_addr_h);
+	iowrite32(pci_dma_l(ape->table_bus), &write_header->bdt_addr_l);
+
+	/** memory write barrier and flush posted writes */
+	wmb();
+	/* dummy read to flush posted writes */
+	/* FIXME Need a readable location! */
+#if 0
+	(void)ioread32();
+#endif
+	irq_count = ape->irq_count;
+
+	printk(KERN_DEBUG "\nStart DMA write\n");
+	iowrite32(n, &write_header->w3);
+
+	/** memory write barrier */
+	wmb();
+	/** dummy read to flush posted writes */
+	//(void)ioread32();
+
+	printk(KERN_DEBUG "POLL FOR WRITE:\n");
+	/* poll for completion, 1000 times 1 millisecond */
+	for (i = 0; i < 100; i++) {
+		volatile u32 *p = &ape->table_virt->w3;
+		u32 eplast = le32_to_cpu(*p) & 0xffffUL;
+		printk(KERN_DEBUG "EPLAST = %u, n = %d\n", eplast, n);
+		if (eplast == n) {
+			printk(KERN_DEBUG "DONE\n");
+			/* print IRQ count before the transfer */
+			printk(KERN_DEBUG "#IRQs during transfer: %d\n", ape->irq_count - irq_count);
+			break;
+		}
+		udelay(100);
+	}
+	/* soft-reset DMA write engine */
+	iowrite32(0x0000ffffUL, &write_header->w0);
+	/* soft-reset DMA read engine */
+	iowrite32(0x0000ffffUL, &read_header->w0);
+
+	/** memory write barrier */
+	wmb();
+	/* dummy read to flush posted writes */
+	/* FIXME Need a readable location! */
+#if 0
+	(void)ioread32();
+#endif
+	/* compare first half of buffer with second half, should be identical */
+	result = compare((u32 *)buffer_virt, (u32 *)(buffer_virt + 2 * PAGE_SIZE), 8192);
+	printk(KERN_DEBUG "DMA loop back test %s.\n", result ? "FAILED" : "PASSED");
+
+	pci_free_consistent(dev, 4 * PAGE_SIZE, buffer_virt, buffer_bus);
+fail:
+	printk(KERN_DEBUG "bar_tests() end, result %d\n", result);
+	return result;
+}
+
+/* Called when the PCI sub system thinks we can control the given device.
+ * Inspect if we can support the device and if so take control of it.
+ *
+ * Return 0 when we have taken control of the given device.
+ *
+ * - allocate board specific bookkeeping
+ * - allocate coherently-mapped memory for the descriptor table
+ * - enable the board
+ * - verify board revision
+ * - request regions
+ * - query DMA mask
+ * - obtain and request irq
+ * - map regions into kernel address space
+ */
+static int __devinit probe(struct pci_dev *dev, const struct pci_device_id *id)
+{
+	int rc = 0;
+	struct ape_dev *ape = NULL;
+	u8 irq_pin, irq_line;
+	printk(KERN_DEBUG "probe(dev = 0x%p, pciid = 0x%p)\n", dev, id);
+
+	/* allocate memory for per-board book keeping */
+	ape = kzalloc(sizeof(struct ape_dev), GFP_KERNEL);
+	if (!ape) {
+		printk(KERN_DEBUG "Could not kzalloc()ate memory.\n");
+		goto err_ape;
+	}
+	ape->pci_dev = dev;
+	dev->dev.driver_data = (void *)ape;
+	printk(KERN_DEBUG "probe() ape = 0x%p\n", ape);
+
+	printk(KERN_DEBUG "sizeof(struct ape_chdma_table) = %d.\n",
+		(int)sizeof(struct ape_chdma_table));
+	/* the reference design has a size restriction on the table size */
+	BUG_ON(sizeof(struct ape_chdma_table) > APE_CHDMA_TABLE_SIZE);
+
+	/* allocate and map coherently-cached memory for a descriptor table */
+	/* @see LDD3 page 446 */
+	ape->table_virt = (struct ape_chdma_table *)pci_alloc_consistent(dev,
+		APE_CHDMA_TABLE_SIZE, &ape->table_bus);
+	/* could not allocate table? */
+	if (!ape->table_virt) {
+		printk(KERN_DEBUG "Could not dma_alloc()ate_coherent memory.\n");
+		goto err_table;
+	}
+
+	printk(KERN_DEBUG "table_virt = 0x%16llx, table_bus = 0x%16llx.\n",
+		(u64)ape->table_virt, (u64)ape->table_bus);
+
+	/* enable device */
+	rc = pci_enable_device(dev);
+	if (rc) {
+		printk(KERN_DEBUG "pci_enable_device() failed\n");
+		goto err_enable;
+	}
+
+	/* enable bus master capability on device */
+	pci_set_master(dev);
+	/* enable message signaled interrupts */
+	rc = pci_enable_msi(dev);
+	/* could not use MSI? */
+	if (rc) {
+		/* resort to legacy interrupts */
+		printk(KERN_DEBUG "Could not enable MSI interrupting.\n");
+		ape->msi_enabled = 0;
+	/* MSI enabled, remember for cleanup */
+	} else {
+		printk(KERN_DEBUG "Enabled MSI interrupting.\n");
+		ape->msi_enabled = 1;
+	}
+
+	pci_read_config_byte(dev, PCI_REVISION_ID, &ape->revision);
+#if 0 /* example */
+	/* (for example) this driver does not support revision 0x42 */
+    if (ape->revision == 0x42) {
+		printk(KERN_DEBUG "Revision 0x42 is not supported by this driver.\n");
+		rc = -ENODEV;
+		goto err_rev;
+	}
+#endif
+	/** XXX check for native or legacy PCIe endpoint? */
+
+	rc = pci_request_regions(dev, DRV_NAME);
+	/* could not request all regions? */
+	if (rc) {
+		/* assume device is in use (and do not disable it later!) */
+		ape->in_use = 1;
+		goto err_regions;
+	}
+	ape->got_regions = 1;
+
+#if 1 // @todo For now, disable 64-bit, because I do not understand the implications (DAC!)
+	/* query for DMA transfer */
+	/* @see Documentation/DMA-mapping.txt */
+	if (!pci_set_dma_mask(dev, DMA_64BIT_MASK)) {
+		pci_set_consistent_dma_mask(dev, DMA_64BIT_MASK);
+		/* use 64-bit DMA */
+		printk(KERN_DEBUG "Using a 64-bit DMA mask.\n");
+	} else
+#endif
+	if (!pci_set_dma_mask(dev, DMA_32BIT_MASK)) {
+		printk(KERN_DEBUG "Could not set 64-bit DMA mask.\n");
+		pci_set_consistent_dma_mask(dev, DMA_32BIT_MASK);
+		/* use 32-bit DMA */
+		printk(KERN_DEBUG "Using a 32-bit DMA mask.\n");
+	} else {
+		printk(KERN_DEBUG "No suitable DMA possible.\n");
+		/** @todo Choose proper error return code */
+		rc = -1;
+		goto err_mask;
+	}
+
+	rc = pci_read_config_byte(dev, PCI_INTERRUPT_PIN, &irq_pin);
+	/* could not read? */
+	if (rc)
+		goto err_irq;
+	printk(KERN_DEBUG "IRQ pin #%d (0=none, 1=INTA#...4=INTD#).\n", irq_pin);
+
+	/* @see LDD3, page 318 */
+	rc = pci_read_config_byte(dev, PCI_INTERRUPT_LINE, &irq_line);
+	/* could not read? */
+	if (rc) {
+		printk(KERN_DEBUG "Could not query PCI_INTERRUPT_LINE, error %d\n", rc);
+		goto err_irq;
+	}
+	printk(KERN_DEBUG "IRQ line #%d.\n", irq_line);
+#if 1
+	irq_line = dev->irq;
+	/* @see LDD3, page 259 */
+	rc = request_irq(irq_line, altpciechdma_isr, IRQF_SHARED, DRV_NAME, (void *)ape);
+	if (rc) {
+		printk(KERN_DEBUG "Could not request IRQ #%d, error %d\n", irq_line, rc);
+		ape->irq_line = -1;
+		goto err_irq;
+	}
+	/* remember which irq we allocated */
+	ape->irq_line = (int)irq_line;
+	printk(KERN_DEBUG "Succesfully requested IRQ #%d with dev_id 0x%p\n", irq_line, ape);
+#endif
+	/* show BARs */
+	scan_bars(ape, dev);
+	/* map BARs */
+	rc = map_bars(ape, dev);
+	if (rc)
+		goto err_map;
+#if ALTPCIECHDMA_CDEV
+	/* initialize character device */
+	rc = sg_init(ape);
+	if (rc)
+		goto err_cdev;
+#endif
+	/* perform DMA engines loop back test */
+	rc = dma_test(ape, dev);
+	(void)rc;
+	/* succesfully took the device */
+	rc = 0;
+	printk(KERN_DEBUG "probe() successful.\n");
+	goto end;
+err_cdev:
+	/* unmap the BARs */
+	unmap_bars(ape, dev);
+err_map:
+	/* free allocated irq */
+	if (ape->irq_line >= 0)
+		free_irq(ape->irq_line, (void *)ape);
+err_irq:
+	if (ape->msi_enabled)
+		pci_disable_msi(dev);
+	/* disable the device iff it is not in use */
+	if (!ape->in_use)
+		pci_disable_device(dev);
+	if (ape->got_regions)
+		pci_release_regions(dev);
+err_mask:
+err_regions:
+err_rev:
+/* clean up everything before device enable() */
+err_enable:
+	if (ape->table_virt)
+		pci_free_consistent(dev, APE_CHDMA_TABLE_SIZE, ape->table_virt, ape->table_bus);
+/* clean up everything before allocating descriptor table */
+err_table:
+	if (ape)
+		kfree(ape);
+err_ape:
+end:
+	return rc;
+}
+
+static void __devexit remove(struct pci_dev *dev)
+{
+	struct ape_dev *ape;
+	printk(KERN_DEBUG "remove(0x%p)\n", dev);
+	if ((dev == 0) || (dev->dev.driver_data == 0)) {
+		printk(KERN_DEBUG "remove(dev = 0x%p) dev->dev.driver_data = 0x%p\n", dev, dev->dev.driver_data);
+		return;
+	}
+	ape = (struct ape_dev *)dev->dev.driver_data;
+	printk(KERN_DEBUG "remove(dev = 0x%p) where dev->dev.driver_data = 0x%p\n", dev, ape);
+	if (ape->pci_dev != dev) {
+		printk(KERN_DEBUG "dev->dev.driver_data->pci_dev (0x%08lx) != dev (0x%08lx)\n",
+		(unsigned long)ape->pci_dev, (unsigned long)dev);
+	}
+	/* remove character device */
+#if ALTPCIECHDMA_CDEV
+	sg_exit(ape);
+#endif
+
+	if (ape->table_virt)
+		pci_free_consistent(dev, APE_CHDMA_TABLE_SIZE, ape->table_virt, ape->table_bus);
+
+	/* free IRQ
+	 * @see LDD3 page 279
+	 */
+	if (ape->irq_line >= 0) {
+		printk(KERN_DEBUG "Freeing IRQ #%d for dev_id 0x%08lx.\n",
+		ape->irq_line, (unsigned long)ape);
+		free_irq(ape->irq_line, (void *)ape);
+	}
+	/* MSI was enabled? */
+	if (ape->msi_enabled) {
+		/* Disable MSI @see Documentation/MSI-HOWTO.txt */
+		pci_disable_msi(dev);
+		ape->msi_enabled = 0;
+	}
+	/* unmap the BARs */
+	unmap_bars(ape, dev);
+	if (!ape->in_use)
+		pci_disable_device(dev);
+	if (ape->got_regions)
+		/* to be called after device disable */
+		pci_release_regions(dev);
+}
+
+#if ALTPCIECHDMA_CDEV
+
+/*
+ * Called when the device goes from unused to used.
+ */
+static int sg_open(struct inode *inode, struct file *file)
+{
+	struct ape_dev *ape;
+	printk(KERN_DEBUG DRV_NAME "_open()\n");
+	/* pointer to containing data structure of the character device inode */
+	ape = container_of(inode->i_cdev, struct ape_dev, cdev);
+	/* create a reference to our device state in the opened file */
+	file->private_data = ape;
+	/* create virtual memory mapper */
+	ape->sgm = sg_create_mapper(MAX_CHDMA_SIZE);
+	return 0;
+}
+
+/*
+ * Called when the device goes from used to unused.
+ */
+static int sg_close(struct inode *inode, struct file *file)
+{
+	/* fetch device specific data stored earlier during open */
+	struct ape_dev *ape = (struct ape_dev *)file->private_data;
+	printk(KERN_DEBUG DRV_NAME "_close()\n");
+	/* destroy virtual memory mapper */
+	sg_destroy_mapper(ape->sgm);
+	return 0;
+}
+
+static ssize_t sg_read(struct file *file, char __user *buf, size_t count, loff_t *pos)
+{
+	/* fetch device specific data stored earlier during open */
+	struct ape_dev *ape = (struct ape_dev *)file->private_data;
+	(void)ape;
+	printk(KERN_DEBUG DRV_NAME "_read(buf=0x%p, count=%lld, pos=%llu)\n", buf, (s64)count, (u64)*pos);
+	return count;
+}
+
+/* sg_write() - Write to the device
+ *
+ * @buf userspace buffer
+ * @count number of bytes in the userspace buffer
+ *
+ * Iterate over the userspace buffer, taking at most 255 * PAGE_SIZE bytes for
+ * each DMA transfer.
+ *   For each transfer, get the user pages, build a sglist, map, build a
+ *   descriptor table. submit the transfer. wait for the interrupt handler
+ *   to wake us on completion.
+ */
+static ssize_t sg_write(struct file *file, const char __user *buf, size_t count, loff_t *pos)
+{
+	int hwnents, tents;
+	size_t transfer_len, remaining = count, done = 0;
+	u64 transfer_addr = (u64)buf;
+	/* fetch device specific data stored earlier during open */
+	struct ape_dev *ape = (struct ape_dev *)file->private_data;
+	printk(KERN_DEBUG DRV_NAME "_write(buf=0x%p, count=%lld, pos=%llu)\n",
+		buf, (s64)count, (u64)*pos);
+	/* TODO transfer boundaries at PAGE_SIZE granularity */
+	while (remaining > 0)
+	{
+		/* limit DMA transfer size */
+		transfer_len = (remaining < APE_CHDMA_MAX_TRANSFER_LEN)? remaining:
+			APE_CHDMA_MAX_TRANSFER_LEN;
+		/* get all user space buffer pages and create a scattergather list */
+		sgm_map_user_pages(ape->sgm, transfer_addr, transfer_len, 0/*read from userspace*/);
+		printk(KERN_DEBUG DRV_NAME "mapped_pages=%d\n", ape->sgm->mapped_pages);
+		/* map all entries in the scattergather list */
+		hwnents = pci_map_sg(ape->pci_dev, ape->sgm->sgl, ape->sgm->mapped_pages, DMA_TO_DEVICE);
+		printk(KERN_DEBUG DRV_NAME "hwnents=%d\n", hwnents);
+		/* build device descriptor tables and submit them to the DMA engine */
+		tents = ape_sg_to_chdma_table(ape->sgm->sgl, hwnents, 0, &ape->table_virt->desc[0], 4096);
+		printk(KERN_DEBUG DRV_NAME "tents=%d\n", hwnents);
+#if 0
+		while (tables) {
+			/* TODO build table */
+			/* TODO submit table to the device */
+			/* if engine stopped and unfinished work then start engine */
+		}
+		put ourselves on wait queue
+#endif
+
+		dma_unmap_sg(NULL, ape->sgm->sgl, ape->sgm->mapped_pages, DMA_TO_DEVICE);
+		/* dirty and free the pages */
+		sgm_unmap_user_pages(ape->sgm, 1/*dirtied*/);
+		/* book keeping */
+		transfer_addr += transfer_len;
+		remaining -= transfer_len;
+		done += transfer_len;
+	}
+	return done;
+}
+
+/*
+ * character device file operations
+ */
+static struct file_operations sg_fops = {
+  .owner = THIS_MODULE,
+  .open = sg_open,
+  .release = sg_close,
+  .read = sg_read,
+  .write = sg_write,
+};
+
+/* sg_init() - Initialize character device
+ *
+ * XXX Should ideally be tied to the device, on device probe, not module init.
+ */
+static int sg_init(struct ape_dev *ape)
+{
+	int rc;
+	printk(KERN_DEBUG DRV_NAME " sg_init()\n");
+	/* allocate a dynamically allocated character device node */
+	rc = alloc_chrdev_region(&ape->cdevno, 0/*requested minor*/, 1/*count*/, DRV_NAME);
+	/* allocation failed? */
+	if (rc < 0) {
+		printk("alloc_chrdev_region() = %d\n", rc);
+		goto fail_alloc;
+	}
+	/* couple the device file operations to the character device */
+	cdev_init(&ape->cdev, &sg_fops);
+	ape->cdev.owner = THIS_MODULE;
+	/* bring character device live */
+	rc = cdev_add(&ape->cdev, ape->cdevno, 1/*count*/);
+	if (rc < 0) {
+		printk("cdev_add() = %d\n", rc);
+		goto fail_add;
+	}
+	printk(KERN_DEBUG "altpciechdma = %d:%d\n", MAJOR(ape->cdevno), MINOR(ape->cdevno));
+	return 0;
+fail_add:
+	/* free the dynamically allocated character device node */
+    unregister_chrdev_region(ape->cdevno, 1/*count*/);
+fail_alloc:
+	return -1;
+}
+
+/* sg_exit() - Cleanup character device
+ *
+ * XXX Should ideally be tied to the device, on device remove, not module exit.
+ */
+
+static void sg_exit(struct ape_dev *ape)
+{
+	printk(KERN_DEBUG DRV_NAME " sg_exit()\n");
+	/* remove the character device */
+	cdev_del(&ape->cdev);
+	/* free the dynamically allocated character device node */
+	unregister_chrdev_region(ape->cdevno, 1/*count*/);
+}
+
+#endif /* ALTPCIECHDMA_CDEV */
+
+/* used to register the driver with the PCI kernel sub system
+ * @see LDD3 page 311
+ */
+static struct pci_driver pci_driver = {
+	.name = DRV_NAME,
+	.id_table = ids,
+	.probe = probe,
+	.remove = remove,
+	/* resume, suspend are optional */
+};
+
+/**
+ * alterapciechdma_init() - Module initialization, registers devices.
+ */
+static int __init alterapciechdma_init(void)
+{
+  int rc = 0;
+	printk(KERN_DEBUG DRV_NAME " init(), built at " __DATE__ " " __TIME__ "\n");
+	/* register this driver with the PCI bus driver */
+	rc = pci_register_driver(&pci_driver);
+	if (rc < 0)
+	  return rc;
+	return 0;
+}
+
+/**
+ * alterapciechdma_init() - Module cleanup, unregisters devices.
+ */
+static void __exit alterapciechdma_exit(void)
+{
+	printk(KERN_DEBUG DRV_NAME " exit(), built at " __DATE__ " " __TIME__ "\n");
+	/* unregister this driver from the PCI bus driver */
+	pci_unregister_driver(&pci_driver);
+}
+
+MODULE_LICENSE("GPL");
+
+module_init(alterapciechdma_init);
+module_exit(alterapciechdma_exit);
+
diff --git a/drivers/staging/android/Kconfig b/drivers/staging/android/Kconfig
new file mode 100644
index 000000000000..6b996db0dd6a
--- /dev/null
+++ b/drivers/staging/android/Kconfig
@@ -0,0 +1,86 @@
+menu "Android"
+
+config ANDROID
+	bool "Android Drivers"
+	default N
+	---help---
+	  Enable support for various drivers needed on the Android platform
+
+config ANDROID_BINDER_IPC
+	bool "Android Binder IPC Driver"
+	default n
+
+config ANDROID_LOGGER
+	tristate "Android log driver"
+	default n
+
+config ANDROID_RAM_CONSOLE
+	bool "Android RAM buffer console"
+	default n
+
+config ANDROID_RAM_CONSOLE_ENABLE_VERBOSE
+	bool "Enable verbose console messages on Android RAM console"
+	default y
+	depends on ANDROID_RAM_CONSOLE
+
+menuconfig ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	bool "Android RAM Console Enable error correction"
+	default n
+	depends on ANDROID_RAM_CONSOLE
+	select REED_SOLOMON
+	select REED_SOLOMON_ENC8
+	select REED_SOLOMON_DEC8
+
+if ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_DATA_SIZE
+	int "Android RAM Console Data data size"
+	default 128
+	help
+	  Must be a power of 2.
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_ECC_SIZE
+	int "Android RAM Console ECC size"
+	default 16
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE
+	int "Android RAM Console Symbol size"
+	default 8
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_POLYNOMIAL
+	hex "Android RAM Console Polynomial"
+	default 0x19 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 4)
+	default 0x29 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 5)
+	default 0x61 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 6)
+	default 0x89 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 7)
+	default 0x11d if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 8)
+
+endif # ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+
+config ANDROID_RAM_CONSOLE_EARLY_INIT
+	bool "Start Android RAM console early"
+	default n
+	depends on ANDROID_RAM_CONSOLE
+
+config ANDROID_RAM_CONSOLE_EARLY_ADDR
+	hex "Android RAM console virtual address"
+	default 0
+	depends on ANDROID_RAM_CONSOLE_EARLY_INIT
+
+config ANDROID_RAM_CONSOLE_EARLY_SIZE
+	hex "Android RAM console buffer size"
+	default 0
+	depends on ANDROID_RAM_CONSOLE_EARLY_INIT
+
+config ANDROID_TIMED_GPIO
+	tristate "Android timed gpio driver"
+	depends on GENERIC_GPIO
+	default n
+
+config ANDROID_LOW_MEMORY_KILLER
+	bool "Android Low Memory Killer"
+	default N
+	---help---
+	  Register processes to be killed when memory is low
+
+endmenu
diff --git a/drivers/staging/android/Makefile b/drivers/staging/android/Makefile
new file mode 100644
index 000000000000..95209d6273a1
--- /dev/null
+++ b/drivers/staging/android/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_ANDROID_BINDER_IPC)	+= binder.o
+obj-$(CONFIG_ANDROID_LOGGER)		+= logger.o
+obj-$(CONFIG_ANDROID_RAM_CONSOLE)	+= ram_console.o
+obj-$(CONFIG_ANDROID_TIMED_GPIO)	+= timed_gpio.o
+obj-$(CONFIG_ANDROID_LOW_MEMORY_KILLER)	+= lowmemorykiller.o
diff --git a/drivers/staging/android/TODO b/drivers/staging/android/TODO
new file mode 100644
index 000000000000..e59c5be4be2b
--- /dev/null
+++ b/drivers/staging/android/TODO
@@ -0,0 +1,10 @@
+TODO:
+	- checkpatch.pl cleanups
+	- sparse fixes
+	- rename files to be not so "generic"
+	- make sure things build as modules properly
+	- add proper arch dependancies as needed
+	- audit userspace interfaces to make sure they are sane
+
+Please send patches to Greg Kroah-Hartman <greg@kroah.com> and Cc:
+Brian Swetland <swetland@google.com>
diff --git a/drivers/staging/android/binder.c b/drivers/staging/android/binder.c
new file mode 100644
index 000000000000..6a4ceacb33f5
--- /dev/null
+++ b/drivers/staging/android/binder.c
@@ -0,0 +1,3503 @@
+/* binder.c
+ *
+ * Android IPC Subsystem
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <asm/cacheflush.h>
+#include <linux/fdtable.h>
+#include <linux/file.h>
+#include <linux/fs.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/nsproxy.h>
+#include <linux/poll.h>
+#include <linux/proc_fs.h>
+#include <linux/rbtree.h>
+#include <linux/sched.h>
+#include <linux/uaccess.h>
+#include <linux/vmalloc.h>
+#include "binder.h"
+
+static DEFINE_MUTEX(binder_lock);
+static HLIST_HEAD(binder_procs);
+static struct binder_node *binder_context_mgr_node;
+static uid_t binder_context_mgr_uid = -1;
+static int binder_last_id;
+static struct proc_dir_entry *binder_proc_dir_entry_root;
+static struct proc_dir_entry *binder_proc_dir_entry_proc;
+static struct hlist_head binder_dead_nodes;
+
+static int binder_read_proc_proc(
+	char *page, char **start, off_t off, int count, int *eof, void *data);
+
+/* This is only defined in include/asm-arm/sizes.h */
+#ifndef SZ_1K
+#define SZ_1K                               0x400
+#endif
+
+#ifndef SZ_4M
+#define SZ_4M                               0x400000
+#endif
+
+#ifndef __i386__
+#define FORBIDDEN_MMAP_FLAGS                (VM_WRITE | VM_EXEC)
+#else
+#define FORBIDDEN_MMAP_FLAGS                (VM_WRITE)
+#endif
+
+#define BINDER_SMALL_BUF_SIZE (PAGE_SIZE * 64)
+
+enum {
+	BINDER_DEBUG_USER_ERROR             = 1U << 0,
+	BINDER_DEBUG_FAILED_TRANSACTION     = 1U << 1,
+	BINDER_DEBUG_DEAD_TRANSACTION       = 1U << 2,
+	BINDER_DEBUG_OPEN_CLOSE             = 1U << 3,
+	BINDER_DEBUG_DEAD_BINDER            = 1U << 4,
+	BINDER_DEBUG_DEATH_NOTIFICATION     = 1U << 5,
+	BINDER_DEBUG_READ_WRITE             = 1U << 6,
+	BINDER_DEBUG_USER_REFS              = 1U << 7,
+	BINDER_DEBUG_THREADS                = 1U << 8,
+	BINDER_DEBUG_TRANSACTION            = 1U << 9,
+	BINDER_DEBUG_TRANSACTION_COMPLETE   = 1U << 10,
+	BINDER_DEBUG_FREE_BUFFER            = 1U << 11,
+	BINDER_DEBUG_INTERNAL_REFS          = 1U << 12,
+	BINDER_DEBUG_BUFFER_ALLOC           = 1U << 13,
+	BINDER_DEBUG_PRIORITY_CAP           = 1U << 14,
+	BINDER_DEBUG_BUFFER_ALLOC_ASYNC     = 1U << 15,
+};
+static uint32_t binder_debug_mask = BINDER_DEBUG_USER_ERROR |
+	BINDER_DEBUG_FAILED_TRANSACTION | BINDER_DEBUG_DEAD_TRANSACTION;
+module_param_named(debug_mask, binder_debug_mask, uint, S_IWUSR | S_IRUGO);
+static int binder_debug_no_lock;
+module_param_named(proc_no_lock, binder_debug_no_lock, bool, S_IWUSR | S_IRUGO);
+static DECLARE_WAIT_QUEUE_HEAD(binder_user_error_wait);
+static int binder_stop_on_user_error;
+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);
+	return ret;
+}
+module_param_call(stop_on_user_error, binder_set_stop_on_user_error,
+	param_get_int, &binder_stop_on_user_error, S_IWUSR | S_IRUGO);
+
+#define binder_user_error(x...) \
+	do { \
+		if (binder_debug_mask & BINDER_DEBUG_USER_ERROR) \
+			printk(KERN_INFO x); \
+		if (binder_stop_on_user_error) \
+			binder_stop_on_user_error = 2; \
+	} while (0)
+
+enum {
+	BINDER_STAT_PROC,
+	BINDER_STAT_THREAD,
+	BINDER_STAT_NODE,
+	BINDER_STAT_REF,
+	BINDER_STAT_DEATH,
+	BINDER_STAT_TRANSACTION,
+	BINDER_STAT_TRANSACTION_COMPLETE,
+	BINDER_STAT_COUNT
+};
+
+struct binder_stats {
+	int br[_IOC_NR(BR_FAILED_REPLY) + 1];
+	int bc[_IOC_NR(BC_DEAD_BINDER_DONE) + 1];
+	int obj_created[BINDER_STAT_COUNT];
+	int obj_deleted[BINDER_STAT_COUNT];
+};
+
+static struct binder_stats binder_stats;
+
+struct binder_transaction_log_entry {
+	int debug_id;
+	int call_type;
+	int from_proc;
+	int from_thread;
+	int target_handle;
+	int to_proc;
+	int to_thread;
+	int to_node;
+	int data_size;
+	int offsets_size;
+};
+struct binder_transaction_log {
+	int next;
+	int full;
+	struct binder_transaction_log_entry entry[32];
+};
+struct binder_transaction_log binder_transaction_log;
+struct binder_transaction_log binder_transaction_log_failed;
+
+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++;
+	if (log->next == ARRAY_SIZE(log->entry)) {
+		log->next = 0;
+		log->full = 1;
+	}
+	return e;
+}
+
+struct binder_work {
+	struct list_head entry;
+	enum {
+		BINDER_WORK_TRANSACTION = 1,
+		BINDER_WORK_TRANSACTION_COMPLETE,
+		BINDER_WORK_NODE,
+		BINDER_WORK_DEAD_BINDER,
+		BINDER_WORK_DEAD_BINDER_AND_CLEAR,
+		BINDER_WORK_CLEAR_DEATH_NOTIFICATION,
+	} type;
+};
+
+struct binder_node {
+	int debug_id;
+	struct binder_work work;
+	union {
+		struct rb_node rb_node;
+		struct hlist_node dead_node;
+	};
+	struct binder_proc *proc;
+	struct hlist_head refs;
+	int internal_strong_refs;
+	int local_weak_refs;
+	int local_strong_refs;
+	void __user *ptr;
+	void __user *cookie;
+	unsigned has_strong_ref : 1;
+	unsigned pending_strong_ref : 1;
+	unsigned has_weak_ref : 1;
+	unsigned pending_weak_ref : 1;
+	unsigned has_async_transaction : 1;
+	unsigned accept_fds : 1;
+	int min_priority : 8;
+	struct list_head async_todo;
+};
+
+struct binder_ref_death {
+	struct binder_work work;
+	void __user *cookie;
+};
+
+struct binder_ref {
+	/* Lookups needed: */
+	/*   node + proc => ref (transaction) */
+	/*   desc + proc => ref (transaction, inc/dec ref) */
+	/*   node => refs + procs (proc exit) */
+	int debug_id;
+	struct rb_node rb_node_desc;
+	struct rb_node rb_node_node;
+	struct hlist_node node_entry;
+	struct binder_proc *proc;
+	struct binder_node *node;
+	uint32_t desc;
+	int strong;
+	int weak;
+	struct binder_ref_death *death;
+};
+
+struct binder_buffer {
+	struct list_head entry; /* free and allocated entries by addesss */
+	struct rb_node rb_node; /* free entry by size or allocated entry */
+				/* by address */
+	unsigned free : 1;
+	unsigned allow_user_free : 1;
+	unsigned async_transaction : 1;
+	unsigned debug_id : 29;
+
+	struct binder_transaction *transaction;
+
+	struct binder_node *target_node;
+	size_t data_size;
+	size_t offsets_size;
+	uint8_t data[0];
+};
+
+struct binder_proc {
+	struct hlist_node proc_node;
+	struct rb_root threads;
+	struct rb_root nodes;
+	struct rb_root refs_by_desc;
+	struct rb_root refs_by_node;
+	int pid;
+	struct vm_area_struct *vma;
+	struct task_struct *tsk;
+	void *buffer;
+	size_t user_buffer_offset;
+
+	struct list_head buffers;
+	struct rb_root free_buffers;
+	struct rb_root allocated_buffers;
+	size_t free_async_space;
+
+	struct page **pages;
+	size_t buffer_size;
+	uint32_t buffer_free;
+	struct list_head todo;
+	wait_queue_head_t wait;
+	struct binder_stats stats;
+	struct list_head delivered_death;
+	int max_threads;
+	int requested_threads;
+	int requested_threads_started;
+	int ready_threads;
+	long default_priority;
+};
+
+enum {
+	BINDER_LOOPER_STATE_REGISTERED  = 0x01,
+	BINDER_LOOPER_STATE_ENTERED     = 0x02,
+	BINDER_LOOPER_STATE_EXITED      = 0x04,
+	BINDER_LOOPER_STATE_INVALID     = 0x08,
+	BINDER_LOOPER_STATE_WAITING     = 0x10,
+	BINDER_LOOPER_STATE_NEED_RETURN = 0x20
+};
+
+struct binder_thread {
+	struct binder_proc *proc;
+	struct rb_node rb_node;
+	int pid;
+	int looper;
+	struct binder_transaction *transaction_stack;
+	struct list_head todo;
+	uint32_t return_error; /* Write failed, return error code in read buf */
+	uint32_t return_error2; /* Write failed, return error code in read */
+		/* buffer. Used when sending a reply to a dead process that */
+		/* we are also waiting on */
+	wait_queue_head_t wait;
+	struct binder_stats stats;
+};
+
+struct binder_transaction {
+	int debug_id;
+	struct binder_work work;
+	struct binder_thread *from;
+	struct binder_transaction *from_parent;
+	struct binder_proc *to_proc;
+	struct binder_thread *to_thread;
+	struct binder_transaction *to_parent;
+	unsigned need_reply : 1;
+	/*unsigned is_dead : 1;*/ /* not used at the moment */
+
+	struct binder_buffer *buffer;
+	unsigned int	code;
+	unsigned int	flags;
+	long	priority;
+	long	saved_priority;
+	uid_t	sender_euid;
+};
+
+/*
+ * copied from get_unused_fd_flags
+ */
+int task_get_unused_fd_flags(struct task_struct *tsk, int flags)
+{
+	struct files_struct *files = get_files_struct(tsk);
+	int fd, error;
+	struct fdtable *fdt;
+	unsigned long rlim_cur;
+
+	if (files == NULL)
+		return -ESRCH;
+
+	error = -EMFILE;
+	spin_lock(&files->file_lock);
+
+repeat:
+	fdt = files_fdtable(files);
+	fd = find_next_zero_bit(fdt->open_fds->fds_bits, fdt->max_fds,
+				files->next_fd);
+
+	/*
+	 * N.B. For clone tasks sharing a files structure, this test
+	 * will limit the total number of files that can be opened.
+	 */
+	rcu_read_lock();
+	if (tsk->signal)
+		rlim_cur = tsk->signal->rlim[RLIMIT_NOFILE].rlim_cur;
+	else
+		rlim_cur = 0;
+	rcu_read_unlock();
+	if (fd >= rlim_cur)
+		goto out;
+
+	/* Do we need to expand the fd array or fd set?  */
+	error = expand_files(files, fd);
+	if (error < 0)
+		goto out;
+
+	if (error) {
+		/*
+		 * If we needed to expand the fs array we
+		 * might have blocked - try again.
+		 */
+		error = -EMFILE;
+		goto repeat;
+	}
+
+	FD_SET(fd, fdt->open_fds);
+	if (flags & O_CLOEXEC)
+		FD_SET(fd, fdt->close_on_exec);
+	else
+		FD_CLR(fd, fdt->close_on_exec);
+	files->next_fd = fd + 1;
+#if 1
+	/* Sanity check */
+	if (fdt->fd[fd] != NULL) {
+		printk(KERN_WARNING "get_unused_fd: slot %d not NULL!\n", fd);
+		fdt->fd[fd] = NULL;
+	}
+#endif
+	error = fd;
+
+out:
+	spin_unlock(&files->file_lock);
+	put_files_struct(files);
+	return error;
+}
+
+/*
+ * copied from fd_install
+ */
+static void task_fd_install(
+	struct task_struct *tsk, unsigned int fd, struct file *file)
+{
+	struct files_struct *files = get_files_struct(tsk);
+	struct fdtable *fdt;
+
+	if (files == NULL)
+		return;
+
+	spin_lock(&files->file_lock);
+	fdt = files_fdtable(files);
+	BUG_ON(fdt->fd[fd] != NULL);
+	rcu_assign_pointer(fdt->fd[fd], file);
+	spin_unlock(&files->file_lock);
+	put_files_struct(files);
+}
+
+/*
+ * copied from __put_unused_fd in open.c
+ */
+static void __put_unused_fd(struct files_struct *files, unsigned int fd)
+{
+	struct fdtable *fdt = files_fdtable(files);
+	__FD_CLR(fd, fdt->open_fds);
+	if (fd < files->next_fd)
+		files->next_fd = fd;
+}
+
+/*
+ * copied from sys_close
+ */
+static long task_close_fd(struct task_struct *tsk, unsigned int fd)
+{
+	struct file *filp;
+	struct files_struct *files = get_files_struct(tsk);
+	struct fdtable *fdt;
+	int retval;
+
+	if (files == NULL)
+		return -ESRCH;
+
+	spin_lock(&files->file_lock);
+	fdt = files_fdtable(files);
+	if (fd >= fdt->max_fds)
+		goto out_unlock;
+	filp = fdt->fd[fd];
+	if (!filp)
+		goto out_unlock;
+	rcu_assign_pointer(fdt->fd[fd], NULL);
+	FD_CLR(fd, fdt->close_on_exec);
+	__put_unused_fd(files, fd);
+	spin_unlock(&files->file_lock);
+	retval = filp_close(filp, files);
+
+	/* can't restart close syscall because file table entry was cleared */
+	if (unlikely(retval == -ERESTARTSYS ||
+		     retval == -ERESTARTNOINTR ||
+		     retval == -ERESTARTNOHAND ||
+		     retval == -ERESTART_RESTARTBLOCK))
+		retval = -EINTR;
+
+	put_files_struct(files);
+	return retval;
+
+out_unlock:
+	spin_unlock(&files->file_lock);
+	put_files_struct(files);
+	return -EBADF;
+}
+
+static void binder_set_nice(long nice)
+{
+	long min_nice;
+	if (can_nice(current, nice)) {
+		set_user_nice(current, nice);
+		return;
+	}
+	min_nice = 20 - current->signal->rlim[RLIMIT_NICE].rlim_cur;
+	if (binder_debug_mask & BINDER_DEBUG_PRIORITY_CAP)
+		printk(KERN_INFO "binder: %d: nice value %ld not allowed use "
+		       "%ld instead\n", current->pid, nice, min_nice);
+	set_user_nice(current, min_nice);
+	if (min_nice < 20)
+		return;
+	binder_user_error("binder: %d RLIMIT_NICE not set\n", current->pid);
+}
+
+static size_t binder_buffer_size(
+	struct binder_proc *proc, struct binder_buffer *buffer)
+{
+	if (list_is_last(&buffer->entry, &proc->buffers))
+		return proc->buffer + proc->buffer_size - (void *)buffer->data;
+	else
+		return (size_t)list_entry(buffer->entry.next,
+			struct binder_buffer, entry) - (size_t)buffer->data;
+}
+
+static void binder_insert_free_buffer(
+	struct binder_proc *proc, struct binder_buffer *new_buffer)
+{
+	struct rb_node **p = &proc->free_buffers.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_buffer *buffer;
+	size_t buffer_size;
+	size_t new_buffer_size;
+
+	BUG_ON(!new_buffer->free);
+
+	new_buffer_size = binder_buffer_size(proc, new_buffer);
+
+	if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+		printk(KERN_INFO "binder: %d: add free buffer, size %zd, "
+		       "at %p\n", proc->pid, new_buffer_size, new_buffer);
+
+	while (*p) {
+		parent = *p;
+		buffer = rb_entry(parent, struct binder_buffer, rb_node);
+		BUG_ON(!buffer->free);
+
+		buffer_size = binder_buffer_size(proc, buffer);
+
+		if (new_buffer_size < buffer_size)
+			p = &parent->rb_left;
+		else
+			p = &parent->rb_right;
+	}
+	rb_link_node(&new_buffer->rb_node, parent, p);
+	rb_insert_color(&new_buffer->rb_node, &proc->free_buffers);
+}
+
+static void binder_insert_allocated_buffer(
+	struct binder_proc *proc, struct binder_buffer *new_buffer)
+{
+	struct rb_node **p = &proc->allocated_buffers.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_buffer *buffer;
+
+	BUG_ON(new_buffer->free);
+
+	while (*p) {
+		parent = *p;
+		buffer = rb_entry(parent, struct binder_buffer, rb_node);
+		BUG_ON(buffer->free);
+
+		if (new_buffer < buffer)
+			p = &parent->rb_left;
+		else if (new_buffer > buffer)
+			p = &parent->rb_right;
+		else
+			BUG();
+	}
+	rb_link_node(&new_buffer->rb_node, parent, p);
+	rb_insert_color(&new_buffer->rb_node, &proc->allocated_buffers);
+}
+
+static struct binder_buffer *binder_buffer_lookup(
+	struct binder_proc *proc, void __user *user_ptr)
+{
+	struct rb_node *n = proc->allocated_buffers.rb_node;
+	struct binder_buffer *buffer;
+	struct binder_buffer *kern_ptr;
+
+	kern_ptr = user_ptr - proc->user_buffer_offset
+		- offsetof(struct binder_buffer, data);
+
+	while (n) {
+		buffer = rb_entry(n, struct binder_buffer, rb_node);
+		BUG_ON(buffer->free);
+
+		if (kern_ptr < buffer)
+			n = n->rb_left;
+		else if (kern_ptr > buffer)
+			n = n->rb_right;
+		else
+			return buffer;
+	}
+	return NULL;
+}
+
+static int binder_update_page_range(struct binder_proc *proc, int allocate,
+	void *start, void *end, struct vm_area_struct *vma)
+{
+	void *page_addr;
+	unsigned long user_page_addr;
+	struct vm_struct tmp_area;
+	struct page **page;
+	struct mm_struct *mm;
+
+	if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+		printk(KERN_INFO "binder: %d: %s pages %p-%p\n",
+		       proc->pid, allocate ? "allocate" : "free", start, end);
+
+	if (end <= start)
+		return 0;
+
+	if (vma)
+		mm = NULL;
+	else
+		mm = get_task_mm(proc->tsk);
+
+	if (mm) {
+		down_write(&mm->mmap_sem);
+		vma = proc->vma;
+	}
+
+	if (allocate == 0)
+		goto free_range;
+
+	if (vma == NULL) {
+		printk(KERN_ERR "binder: %d: binder_alloc_buf failed to "
+		       "map pages in userspace, no vma\n", proc->pid);
+		goto err_no_vma;
+	}
+
+	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);
+		*page = alloc_page(GFP_KERNEL | __GFP_ZERO);
+		if (*page == NULL) {
+			printk(KERN_ERR "binder: %d: binder_alloc_buf failed "
+			       "for page at %p\n", proc->pid, page_addr);
+			goto err_alloc_page_failed;
+		}
+		tmp_area.addr = page_addr;
+		tmp_area.size = PAGE_SIZE + PAGE_SIZE /* guard page? */;
+		page_array_ptr = page;
+		ret = map_vm_area(&tmp_area, PAGE_KERNEL, &page_array_ptr);
+		if (ret) {
+			printk(KERN_ERR "binder: %d: binder_alloc_buf failed "
+			       "to map page at %p in kernel\n",
+			       proc->pid, page_addr);
+			goto err_map_kernel_failed;
+		}
+		user_page_addr = (size_t)page_addr + proc->user_buffer_offset;
+		ret = vm_insert_page(vma, user_page_addr, page[0]);
+		if (ret) {
+			printk(KERN_ERR "binder: %d: binder_alloc_buf failed "
+			       "to map page at %lx in userspace\n",
+			       proc->pid, user_page_addr);
+			goto err_vm_insert_page_failed;
+		}
+		/* vm_insert_page does not seem to increment the refcount */
+	}
+	if (mm) {
+		up_write(&mm->mmap_sem);
+		mmput(mm);
+	}
+	return 0;
+
+free_range:
+	for (page_addr = end - PAGE_SIZE; page_addr >= start;
+	     page_addr -= PAGE_SIZE) {
+		page = &proc->pages[(page_addr - proc->buffer) / PAGE_SIZE];
+		if (vma)
+			zap_page_range(vma, (size_t)page_addr +
+				proc->user_buffer_offset, PAGE_SIZE, NULL);
+err_vm_insert_page_failed:
+		unmap_kernel_range((unsigned long)page_addr, PAGE_SIZE);
+err_map_kernel_failed:
+		__free_page(*page);
+		*page = NULL;
+err_alloc_page_failed:
+		;
+	}
+err_no_vma:
+	if (mm) {
+		up_write(&mm->mmap_sem);
+		mmput(mm);
+	}
+	return -ENOMEM;
+}
+
+static struct binder_buffer *binder_alloc_buf(struct binder_proc *proc,
+	size_t data_size, size_t offsets_size, int is_async)
+{
+	struct rb_node *n = proc->free_buffers.rb_node;
+	struct binder_buffer *buffer;
+	size_t buffer_size;
+	struct rb_node *best_fit = NULL;
+	void *has_page_addr;
+	void *end_page_addr;
+	size_t size;
+
+	if (proc->vma == NULL) {
+		printk(KERN_ERR "binder: %d: binder_alloc_buf, no vma\n",
+		       proc->pid);
+		return NULL;
+	}
+
+	size = ALIGN(data_size, sizeof(void *)) +
+		ALIGN(offsets_size, sizeof(void *));
+
+	if (size < data_size || size < offsets_size) {
+		binder_user_error("binder: %d: got transaction with invalid "
+			"size %zd-%zd\n", proc->pid, data_size, offsets_size);
+		return NULL;
+	}
+
+	if (is_async &&
+	    proc->free_async_space < size + sizeof(struct binder_buffer)) {
+		if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+			printk(KERN_ERR "binder: %d: binder_alloc_buf size %zd f"
+			       "ailed, no async space left\n", proc->pid, size);
+		return NULL;
+	}
+
+	while (n) {
+		buffer = rb_entry(n, struct binder_buffer, rb_node);
+		BUG_ON(!buffer->free);
+		buffer_size = binder_buffer_size(proc, buffer);
+
+		if (size < buffer_size) {
+			best_fit = n;
+			n = n->rb_left;
+		} else if (size > buffer_size)
+			n = n->rb_right;
+		else {
+			best_fit = n;
+			break;
+		}
+	}
+	if (best_fit == NULL) {
+		printk(KERN_ERR "binder: %d: binder_alloc_buf size %zd failed, "
+		       "no address space\n", proc->pid, size);
+		return NULL;
+	}
+	if (n == NULL) {
+		buffer = rb_entry(best_fit, struct binder_buffer, rb_node);
+		buffer_size = binder_buffer_size(proc, buffer);
+	}
+	if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+		printk(KERN_INFO "binder: %d: binder_alloc_buf size %zd got buff"
+		       "er %p size %zd\n", proc->pid, size, buffer, buffer_size);
+
+	has_page_addr =
+		(void *)(((size_t)buffer->data + buffer_size) & PAGE_MASK);
+	if (n == NULL) {
+		if (size + sizeof(struct binder_buffer) + 4 >= buffer_size)
+			buffer_size = size; /* no room for other buffers */
+		else
+			buffer_size = size + sizeof(struct binder_buffer);
+	}
+	end_page_addr = (void *)PAGE_ALIGN((size_t)buffer->data + buffer_size);
+	if (end_page_addr > has_page_addr)
+		end_page_addr = has_page_addr;
+	if (binder_update_page_range(proc, 1,
+	    (void *)PAGE_ALIGN((size_t)buffer->data), end_page_addr, NULL))
+		return NULL;
+
+	rb_erase(best_fit, &proc->free_buffers);
+	buffer->free = 0;
+	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);
+	}
+	if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+		printk(KERN_INFO "binder: %d: binder_alloc_buf size %zd got "
+		       "%p\n", proc->pid, size, buffer);
+	buffer->data_size = data_size;
+	buffer->offsets_size = offsets_size;
+	buffer->async_transaction = is_async;
+	if (is_async) {
+		proc->free_async_space -= size + sizeof(struct binder_buffer);
+		if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC_ASYNC)
+			printk(KERN_INFO "binder: %d: binder_alloc_buf size %zd "
+			       "async free %zd\n", proc->pid, size,
+			       proc->free_async_space);
+	}
+
+	return buffer;
+}
+
+static void *buffer_start_page(struct binder_buffer *buffer)
+{
+	return (void *)((size_t)buffer & PAGE_MASK);
+}
+
+static void *buffer_end_page(struct binder_buffer *buffer)
+{
+	return (void *)(((size_t)(buffer + 1) - 1) & PAGE_MASK);
+}
+
+static void binder_delete_free_buffer(
+	struct binder_proc *proc, struct binder_buffer *buffer)
+{
+	struct binder_buffer *prev, *next = NULL;
+	int free_page_end = 1;
+	int free_page_start = 1;
+
+	BUG_ON(proc->buffers.next == &buffer->entry);
+	prev = list_entry(buffer->entry.prev, struct binder_buffer, entry);
+	BUG_ON(!prev->free);
+	if (buffer_end_page(prev) == buffer_start_page(buffer)) {
+		free_page_start = 0;
+		if (buffer_end_page(prev) == buffer_end_page(buffer))
+			free_page_end = 0;
+		if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+			printk(KERN_INFO "binder: %d: merge free, buffer %p "
+			       "share page with %p\n", proc->pid, buffer, prev);
+	}
+
+	if (!list_is_last(&buffer->entry, &proc->buffers)) {
+		next = list_entry(buffer->entry.next,
+				  struct binder_buffer, entry);
+		if (buffer_start_page(next) == buffer_end_page(buffer)) {
+			free_page_end = 0;
+			if (buffer_start_page(next) ==
+			    buffer_start_page(buffer))
+				free_page_start = 0;
+			if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+				printk(KERN_INFO "binder: %d: merge free, "
+				       "buffer %p share page with %p\n",
+				       proc->pid, buffer, prev);
+		}
+	}
+	list_del(&buffer->entry);
+	if (free_page_start || free_page_end) {
+		if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+			printk(KERN_INFO "binder: %d: merge free, buffer %p do "
+			       "not share page%s%s with with %p or %p\n",
+			       proc->pid, buffer, free_page_start ? "" : " end",
+			       free_page_end ? "" : " start", prev, next);
+		binder_update_page_range(proc, 0, free_page_start ?
+			buffer_start_page(buffer) : buffer_end_page(buffer),
+			(free_page_end ? buffer_end_page(buffer) :
+			buffer_start_page(buffer)) + PAGE_SIZE, NULL);
+	}
+}
+
+static void binder_free_buf(
+	struct binder_proc *proc, struct binder_buffer *buffer)
+{
+	size_t size, buffer_size;
+
+	buffer_size = binder_buffer_size(proc, buffer);
+
+	size = ALIGN(buffer->data_size, sizeof(void *)) +
+		ALIGN(buffer->offsets_size, sizeof(void *));
+	if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+		printk(KERN_INFO "binder: %d: binder_free_buf %p size %zd buffer"
+		       "_size %zd\n", proc->pid, buffer, size, buffer_size);
+
+	BUG_ON(buffer->free);
+	BUG_ON(size > buffer_size);
+	BUG_ON(buffer->transaction != NULL);
+	BUG_ON((void *)buffer < proc->buffer);
+	BUG_ON((void *)buffer > proc->buffer + proc->buffer_size);
+
+	if (buffer->async_transaction) {
+		proc->free_async_space += size + sizeof(struct binder_buffer);
+		if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC_ASYNC)
+			printk(KERN_INFO "binder: %d: binder_free_buf size %zd "
+			       "async free %zd\n", proc->pid, size,
+			       proc->free_async_space);
+	}
+
+	binder_update_page_range(proc, 0,
+		(void *)PAGE_ALIGN((size_t)buffer->data),
+		(void *)(((size_t)buffer->data + buffer_size) & PAGE_MASK),
+		NULL);
+	rb_erase(&buffer->rb_node, &proc->allocated_buffers);
+	buffer->free = 1;
+	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);
+		}
+	}
+	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);
+			buffer = prev;
+		}
+	}
+	binder_insert_free_buffer(proc, buffer);
+}
+
+static struct binder_node *
+binder_get_node(struct binder_proc *proc, void __user *ptr)
+{
+	struct rb_node *n = proc->nodes.rb_node;
+	struct binder_node *node;
+
+	while (n) {
+		node = rb_entry(n, struct binder_node, rb_node);
+
+		if (ptr < node->ptr)
+			n = n->rb_left;
+		else if (ptr > node->ptr)
+			n = n->rb_right;
+		else
+			return node;
+	}
+	return NULL;
+}
+
+static struct binder_node *
+binder_new_node(struct binder_proc *proc, void __user *ptr, void __user *cookie)
+{
+	struct rb_node **p = &proc->nodes.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_node *node;
+
+	while (*p) {
+		parent = *p;
+		node = rb_entry(parent, struct binder_node, rb_node);
+
+		if (ptr < node->ptr)
+			p = &(*p)->rb_left;
+		else if (ptr > node->ptr)
+			p = &(*p)->rb_right;
+		else
+			return NULL;
+	}
+
+	node = kzalloc(sizeof(*node), GFP_KERNEL);
+	if (node == NULL)
+		return NULL;
+	binder_stats.obj_created[BINDER_STAT_NODE]++;
+	rb_link_node(&node->rb_node, parent, p);
+	rb_insert_color(&node->rb_node, &proc->nodes);
+	node->debug_id = ++binder_last_id;
+	node->proc = proc;
+	node->ptr = ptr;
+	node->cookie = cookie;
+	node->work.type = BINDER_WORK_NODE;
+	INIT_LIST_HEAD(&node->work.entry);
+	INIT_LIST_HEAD(&node->async_todo);
+	if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+		printk(KERN_INFO "binder: %d:%d node %d u%p c%p created\n",
+		       proc->pid, current->pid, node->debug_id,
+		       node->ptr, node->cookie);
+	return node;
+}
+
+static int
+binder_inc_node(struct binder_node *node, int strong, int internal,
+		struct list_head *target_list)
+{
+	if (strong) {
+		if (internal) {
+			if (target_list == NULL &&
+			    node->internal_strong_refs == 0 &&
+			    !(node == binder_context_mgr_node &&
+			    node->has_strong_ref)) {
+				printk(KERN_ERR "binder: invalid inc strong "
+					"node for %d\n", node->debug_id);
+				return -EINVAL;
+			}
+			node->internal_strong_refs++;
+		} else
+			node->local_strong_refs++;
+		if (!node->has_strong_ref && target_list) {
+			list_del_init(&node->work.entry);
+			list_add_tail(&node->work.entry, target_list);
+		}
+	} else {
+		if (!internal)
+			node->local_weak_refs++;
+		if (!node->has_weak_ref && list_empty(&node->work.entry)) {
+			if (target_list == NULL) {
+				printk(KERN_ERR "binder: invalid inc weak node "
+					"for %d\n", node->debug_id);
+				return -EINVAL;
+			}
+			list_add_tail(&node->work.entry, target_list);
+		}
+	}
+	return 0;
+}
+
+static int
+binder_dec_node(struct binder_node *node, int strong, int internal)
+{
+	if (strong) {
+		if (internal)
+			node->internal_strong_refs--;
+		else
+			node->local_strong_refs--;
+		if (node->local_strong_refs || node->internal_strong_refs)
+			return 0;
+	} else {
+		if (!internal)
+			node->local_weak_refs--;
+		if (node->local_weak_refs || !hlist_empty(&node->refs))
+			return 0;
+	}
+	if (node->proc && (node->has_strong_ref || node->has_weak_ref)) {
+		if (list_empty(&node->work.entry)) {
+			list_add_tail(&node->work.entry, &node->proc->todo);
+			wake_up_interruptible(&node->proc->wait);
+		}
+	} else {
+		if (hlist_empty(&node->refs) && !node->local_strong_refs &&
+		    !node->local_weak_refs) {
+			list_del_init(&node->work.entry);
+			if (node->proc) {
+				rb_erase(&node->rb_node, &node->proc->nodes);
+				if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+					printk(KERN_INFO "binder: refless node %d deleted\n", node->debug_id);
+			} else {
+				hlist_del(&node->dead_node);
+				if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+					printk(KERN_INFO "binder: dead node %d deleted\n", node->debug_id);
+			}
+			kfree(node);
+			binder_stats.obj_deleted[BINDER_STAT_NODE]++;
+		}
+	}
+
+	return 0;
+}
+
+
+static struct binder_ref *
+binder_get_ref(struct binder_proc *proc, uint32_t desc)
+{
+	struct rb_node *n = proc->refs_by_desc.rb_node;
+	struct binder_ref *ref;
+
+	while (n) {
+		ref = rb_entry(n, struct binder_ref, rb_node_desc);
+
+		if (desc < ref->desc)
+			n = n->rb_left;
+		else if (desc > ref->desc)
+			n = n->rb_right;
+		else
+			return ref;
+	}
+	return NULL;
+}
+
+static struct binder_ref *
+binder_get_ref_for_node(struct binder_proc *proc, struct binder_node *node)
+{
+	struct rb_node *n;
+	struct rb_node **p = &proc->refs_by_node.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_ref *ref, *new_ref;
+
+	while (*p) {
+		parent = *p;
+		ref = rb_entry(parent, struct binder_ref, rb_node_node);
+
+		if (node < ref->node)
+			p = &(*p)->rb_left;
+		else if (node > ref->node)
+			p = &(*p)->rb_right;
+		else
+			return ref;
+	}
+	new_ref = kzalloc(sizeof(*ref), GFP_KERNEL);
+	if (new_ref == NULL)
+		return NULL;
+	binder_stats.obj_created[BINDER_STAT_REF]++;
+	new_ref->debug_id = ++binder_last_id;
+	new_ref->proc = proc;
+	new_ref->node = node;
+	rb_link_node(&new_ref->rb_node_node, parent, p);
+	rb_insert_color(&new_ref->rb_node_node, &proc->refs_by_node);
+
+	new_ref->desc = (node == binder_context_mgr_node) ? 0 : 1;
+	for (n = rb_first(&proc->refs_by_desc); n != NULL; n = rb_next(n)) {
+		ref = rb_entry(n, struct binder_ref, rb_node_desc);
+		if (ref->desc > new_ref->desc)
+			break;
+		new_ref->desc = ref->desc + 1;
+	}
+
+	p = &proc->refs_by_desc.rb_node;
+	while (*p) {
+		parent = *p;
+		ref = rb_entry(parent, struct binder_ref, rb_node_desc);
+
+		if (new_ref->desc < ref->desc)
+			p = &(*p)->rb_left;
+		else if (new_ref->desc > ref->desc)
+			p = &(*p)->rb_right;
+		else
+			BUG();
+	}
+	rb_link_node(&new_ref->rb_node_desc, parent, p);
+	rb_insert_color(&new_ref->rb_node_desc, &proc->refs_by_desc);
+	if (node) {
+		hlist_add_head(&new_ref->node_entry, &node->refs);
+		if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+			printk(KERN_INFO "binder: %d new ref %d desc %d for "
+				"node %d\n", proc->pid, new_ref->debug_id,
+				new_ref->desc, node->debug_id);
+	} else {
+		if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+			printk(KERN_INFO "binder: %d new ref %d desc %d for "
+				"dead node\n", proc->pid, new_ref->debug_id,
+				new_ref->desc);
+	}
+	return new_ref;
+}
+
+static void
+binder_delete_ref(struct binder_ref *ref)
+{
+	if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+		printk(KERN_INFO "binder: %d delete ref %d desc %d for "
+			"node %d\n", ref->proc->pid, ref->debug_id,
+			ref->desc, ref->node->debug_id);
+	rb_erase(&ref->rb_node_desc, &ref->proc->refs_by_desc);
+	rb_erase(&ref->rb_node_node, &ref->proc->refs_by_node);
+	if (ref->strong)
+		binder_dec_node(ref->node, 1, 1);
+	hlist_del(&ref->node_entry);
+	binder_dec_node(ref->node, 0, 1);
+	if (ref->death) {
+		if (binder_debug_mask & BINDER_DEBUG_DEAD_BINDER)
+			printk(KERN_INFO "binder: %d delete ref %d desc %d "
+				"has death notification\n", ref->proc->pid,
+				ref->debug_id, ref->desc);
+		list_del(&ref->death->work.entry);
+		kfree(ref->death);
+		binder_stats.obj_deleted[BINDER_STAT_DEATH]++;
+	}
+	kfree(ref);
+	binder_stats.obj_deleted[BINDER_STAT_REF]++;
+}
+
+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);
+			if (ret)
+				return ret;
+		}
+		ref->strong++;
+	} else {
+		if (ref->weak == 0) {
+			ret = binder_inc_node(ref->node, 0, 1, target_list);
+			if (ret)
+				return ret;
+		}
+		ref->weak++;
+	}
+	return 0;
+}
+
+
+static int
+binder_dec_ref(struct binder_ref *ref, int strong)
+{
+	if (strong) {
+		if (ref->strong == 0) {
+			binder_user_error("binder: %d invalid dec strong, "
+					  "ref %d desc %d s %d w %d\n",
+					  ref->proc->pid, ref->debug_id,
+					  ref->desc, ref->strong, ref->weak);
+			return -EINVAL;
+		}
+		ref->strong--;
+		if (ref->strong == 0) {
+			int ret;
+			ret = binder_dec_node(ref->node, strong, 1);
+			if (ret)
+				return ret;
+		}
+	} else {
+		if (ref->weak == 0) {
+			binder_user_error("binder: %d invalid dec weak, "
+					  "ref %d desc %d s %d w %d\n",
+					  ref->proc->pid, ref->debug_id,
+					  ref->desc, ref->strong, ref->weak);
+			return -EINVAL;
+		}
+		ref->weak--;
+	}
+	if (ref->strong == 0 && ref->weak == 0)
+		binder_delete_ref(ref);
+	return 0;
+}
+
+static void
+binder_pop_transaction(
+	struct binder_thread *target_thread, struct binder_transaction *t)
+{
+	if (target_thread) {
+		BUG_ON(target_thread->transaction_stack != t);
+		BUG_ON(target_thread->transaction_stack->from != target_thread);
+		target_thread->transaction_stack =
+			target_thread->transaction_stack->from_parent;
+		t->from = NULL;
+	}
+	t->need_reply = 0;
+	if (t->buffer)
+		t->buffer->transaction = NULL;
+	kfree(t);
+	binder_stats.obj_deleted[BINDER_STAT_TRANSACTION]++;
+}
+
+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;
+		if (target_thread) {
+			if (target_thread->return_error != BR_OK &&
+			   target_thread->return_error2 == BR_OK) {
+				target_thread->return_error2 =
+					target_thread->return_error;
+				target_thread->return_error = BR_OK;
+			}
+			if (target_thread->return_error == BR_OK) {
+				if (binder_debug_mask & BINDER_DEBUG_FAILED_TRANSACTION)
+					printk(KERN_INFO "binder: send failed reply for transaction %d to %d:%d\n",
+					       t->debug_id, target_thread->proc->pid, target_thread->pid);
+
+				binder_pop_transaction(target_thread, t);
+				target_thread->return_error = error_code;
+				wake_up_interruptible(&target_thread->wait);
+			} else {
+				printk(KERN_ERR "binder: reply failed, target "
+					"thread, %d:%d, has error code %d "
+					"already\n", target_thread->proc->pid,
+					target_thread->pid,
+					target_thread->return_error);
+			}
+			return;
+		} else {
+			struct binder_transaction *next = t->from_parent;
+
+			if (binder_debug_mask & BINDER_DEBUG_FAILED_TRANSACTION)
+				printk(KERN_INFO "binder: send failed reply "
+					"for transaction %d, target dead\n",
+					t->debug_id);
+
+			binder_pop_transaction(target_thread, t);
+			if (next == NULL) {
+				if (binder_debug_mask & BINDER_DEBUG_DEAD_BINDER)
+					printk(KERN_INFO "binder: reply failed,"
+						" no target thread at root\n");
+				return;
+			}
+			t = next;
+			if (binder_debug_mask & BINDER_DEBUG_DEAD_BINDER)
+				printk(KERN_INFO "binder: reply failed, no targ"
+					"et thread -- retry %d\n", t->debug_id);
+		}
+	}
+}
+
+static void
+binder_transaction_buffer_release(struct binder_proc *proc,
+			struct binder_buffer *buffer, size_t *failed_at);
+
+static void
+binder_transaction(struct binder_proc *proc, struct binder_thread *thread,
+	struct binder_transaction_data *tr, int reply)
+{
+	struct binder_transaction *t;
+	struct binder_work *tcomplete;
+	size_t *offp, *off_end;
+	struct binder_proc *target_proc;
+	struct binder_thread *target_thread = NULL;
+	struct binder_node *target_node = NULL;
+	struct list_head *target_list;
+	wait_queue_head_t *target_wait;
+	struct binder_transaction *in_reply_to = NULL;
+	struct binder_transaction_log_entry *e;
+	uint32_t return_error;
+
+	e = binder_transaction_log_add(&binder_transaction_log);
+	e->call_type = reply ? 2 : !!(tr->flags & TF_ONE_WAY);
+	e->from_proc = proc->pid;
+	e->from_thread = thread->pid;
+	e->target_handle = tr->target.handle;
+	e->data_size = tr->data_size;
+	e->offsets_size = tr->offsets_size;
+
+	if (reply) {
+		in_reply_to = thread->transaction_stack;
+		if (in_reply_to == NULL) {
+			binder_user_error("binder: %d:%d got reply transaction "
+					  "with no transaction stack\n",
+					  proc->pid, thread->pid);
+			return_error = BR_FAILED_REPLY;
+			goto err_empty_call_stack;
+		}
+		binder_set_nice(in_reply_to->saved_priority);
+		if (in_reply_to->to_thread != thread) {
+			binder_user_error("binder: %d:%d got reply transaction "
+				"with bad transaction stack,"
+				" transaction %d has target %d:%d\n",
+				proc->pid, thread->pid, in_reply_to->debug_id,
+				in_reply_to->to_proc ?
+				in_reply_to->to_proc->pid : 0,
+				in_reply_to->to_thread ?
+				in_reply_to->to_thread->pid : 0);
+			return_error = BR_FAILED_REPLY;
+			in_reply_to = NULL;
+			goto err_bad_call_stack;
+		}
+		thread->transaction_stack = in_reply_to->to_parent;
+		target_thread = in_reply_to->from;
+		if (target_thread == NULL) {
+			return_error = BR_DEAD_REPLY;
+			goto err_dead_binder;
+		}
+		if (target_thread->transaction_stack != in_reply_to) {
+			binder_user_error("binder: %d:%d got reply transaction "
+				"with bad target transaction stack %d, "
+				"expected %d\n",
+				proc->pid, thread->pid,
+				target_thread->transaction_stack ?
+				target_thread->transaction_stack->debug_id : 0,
+				in_reply_to->debug_id);
+			return_error = BR_FAILED_REPLY;
+			in_reply_to = NULL;
+			target_thread = NULL;
+			goto err_dead_binder;
+		}
+		target_proc = target_thread->proc;
+	} else {
+		if (tr->target.handle) {
+			struct binder_ref *ref;
+			ref = binder_get_ref(proc, tr->target.handle);
+			if (ref == NULL) {
+				binder_user_error("binder: %d:%d got "
+					"transaction to invalid handle\n",
+					proc->pid, thread->pid);
+				return_error = BR_FAILED_REPLY;
+				goto err_invalid_target_handle;
+			}
+			target_node = ref->node;
+		} else {
+			target_node = binder_context_mgr_node;
+			if (target_node == NULL) {
+				return_error = BR_DEAD_REPLY;
+				goto err_no_context_mgr_node;
+			}
+		}
+		e->to_node = target_node->debug_id;
+		target_proc = target_node->proc;
+		if (target_proc == NULL) {
+			return_error = BR_DEAD_REPLY;
+			goto err_dead_binder;
+		}
+		if (!(tr->flags & TF_ONE_WAY) && thread->transaction_stack) {
+			struct binder_transaction *tmp;
+			tmp = thread->transaction_stack;
+			while (tmp) {
+				if (tmp->from && tmp->from->proc == target_proc)
+					target_thread = tmp->from;
+				tmp = tmp->from_parent;
+			}
+		}
+	}
+	if (target_thread) {
+		e->to_thread = target_thread->pid;
+		target_list = &target_thread->todo;
+		target_wait = &target_thread->wait;
+	} else {
+		target_list = &target_proc->todo;
+		target_wait = &target_proc->wait;
+	}
+	e->to_proc = target_proc->pid;
+
+	/* TODO: reuse incoming transaction for reply */
+	t = kzalloc(sizeof(*t), GFP_KERNEL);
+	if (t == NULL) {
+		return_error = BR_FAILED_REPLY;
+		goto err_alloc_t_failed;
+	}
+	binder_stats.obj_created[BINDER_STAT_TRANSACTION]++;
+
+	tcomplete = kzalloc(sizeof(*tcomplete), GFP_KERNEL);
+	if (tcomplete == NULL) {
+		return_error = BR_FAILED_REPLY;
+		goto err_alloc_tcomplete_failed;
+	}
+	binder_stats.obj_created[BINDER_STAT_TRANSACTION_COMPLETE]++;
+
+	t->debug_id = ++binder_last_id;
+	e->debug_id = t->debug_id;
+
+	if (binder_debug_mask & BINDER_DEBUG_TRANSACTION) {
+		if (reply)
+			printk(KERN_INFO "binder: %d:%d BC_REPLY %d -> %d:%d, "
+			       "data %p-%p size %zd-%zd\n",
+			       proc->pid, thread->pid, t->debug_id,
+			       target_proc->pid, target_thread->pid,
+			       tr->data.ptr.buffer, tr->data.ptr.offsets,
+			       tr->data_size, tr->offsets_size);
+		else
+			printk(KERN_INFO "binder: %d:%d BC_TRANSACTION %d -> "
+			       "%d - node %d, data %p-%p size %zd-%zd\n",
+			       proc->pid, thread->pid, t->debug_id,
+			       target_proc->pid, target_node->debug_id,
+			       tr->data.ptr.buffer, tr->data.ptr.offsets,
+			       tr->data_size, tr->offsets_size);
+	}
+
+	if (!reply && !(tr->flags & TF_ONE_WAY))
+		t->from = thread;
+	else
+		t->from = NULL;
+	t->sender_euid = proc->tsk->cred->euid;
+	t->to_proc = target_proc;
+	t->to_thread = target_thread;
+	t->code = tr->code;
+	t->flags = tr->flags;
+	t->priority = task_nice(current);
+	t->buffer = binder_alloc_buf(target_proc, tr->data_size,
+		tr->offsets_size, !reply && (t->flags & TF_ONE_WAY));
+	if (t->buffer == NULL) {
+		return_error = BR_FAILED_REPLY;
+		goto err_binder_alloc_buf_failed;
+	}
+	t->buffer->allow_user_free = 0;
+	t->buffer->debug_id = t->debug_id;
+	t->buffer->transaction = t;
+	t->buffer->target_node = target_node;
+	if (target_node)
+		binder_inc_node(target_node, 1, 0, NULL);
+
+	offp = (size_t *)(t->buffer->data + ALIGN(tr->data_size, sizeof(void *)));
+
+	if (copy_from_user(t->buffer->data, tr->data.ptr.buffer, tr->data_size)) {
+		binder_user_error("binder: %d:%d got transaction with invalid "
+			"data ptr\n", proc->pid, thread->pid);
+		return_error = BR_FAILED_REPLY;
+		goto err_copy_data_failed;
+	}
+	if (copy_from_user(offp, tr->data.ptr.offsets, tr->offsets_size)) {
+		binder_user_error("binder: %d:%d got transaction with invalid "
+			"offsets ptr\n", proc->pid, thread->pid);
+		return_error = BR_FAILED_REPLY;
+		goto err_copy_data_failed;
+	}
+	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)) {
+			binder_user_error("binder: %d:%d got transaction with "
+				"invalid offset, %zd\n",
+				proc->pid, thread->pid, *offp);
+			return_error = BR_FAILED_REPLY;
+			goto err_bad_offset;
+		}
+		fp = (struct flat_binder_object *)(t->buffer->data + *offp);
+		switch (fp->type) {
+		case BINDER_TYPE_BINDER:
+		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) {
+					return_error = BR_FAILED_REPLY;
+					goto err_binder_new_node_failed;
+				}
+				node->min_priority = fp->flags & FLAT_BINDER_FLAG_PRIORITY_MASK;
+				node->accept_fds = !!(fp->flags & FLAT_BINDER_FLAG_ACCEPTS_FDS);
+			}
+			if (fp->cookie != node->cookie) {
+				binder_user_error("binder: %d:%d sending u%p "
+					"node %d, cookie mismatch %p != %p\n",
+					proc->pid, thread->pid,
+					fp->binder, node->debug_id,
+					fp->cookie, node->cookie);
+				goto err_binder_get_ref_for_node_failed;
+			}
+			ref = binder_get_ref_for_node(target_proc, node);
+			if (ref == NULL) {
+				return_error = BR_FAILED_REPLY;
+				goto err_binder_get_ref_for_node_failed;
+			}
+			if (fp->type == BINDER_TYPE_BINDER)
+				fp->type = BINDER_TYPE_HANDLE;
+			else
+				fp->type = BINDER_TYPE_WEAK_HANDLE;
+			fp->handle = ref->desc;
+			binder_inc_ref(ref, fp->type == BINDER_TYPE_HANDLE, &thread->todo);
+			if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+				printk(KERN_INFO "        node %d u%p -> ref %d desc %d\n",
+				       node->debug_id, node->ptr, ref->debug_id, ref->desc);
+		} break;
+		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("binder: %d:%d got "
+					"transaction with invalid "
+					"handle, %ld\n", proc->pid,
+					thread->pid, fp->handle);
+				return_error = BR_FAILED_REPLY;
+				goto err_binder_get_ref_failed;
+			}
+			if (ref->node->proc == target_proc) {
+				if (fp->type == BINDER_TYPE_HANDLE)
+					fp->type = BINDER_TYPE_BINDER;
+				else
+					fp->type = BINDER_TYPE_WEAK_BINDER;
+				fp->binder = ref->node->ptr;
+				fp->cookie = ref->node->cookie;
+				binder_inc_node(ref->node, fp->type == BINDER_TYPE_BINDER, 0, NULL);
+				if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+					printk(KERN_INFO "        ref %d desc %d -> node %d u%p\n",
+					       ref->debug_id, ref->desc, ref->node->debug_id, 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;
+					goto err_binder_get_ref_for_node_failed;
+				}
+				fp->handle = new_ref->desc;
+				binder_inc_ref(new_ref, fp->type == BINDER_TYPE_HANDLE, NULL);
+				if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+					printk(KERN_INFO "        ref %d desc %d -> ref %d desc %d (node %d)\n",
+					       ref->debug_id, ref->desc, new_ref->debug_id, new_ref->desc, ref->node->debug_id);
+			}
+		} break;
+
+		case BINDER_TYPE_FD: {
+			int target_fd;
+			struct file *file;
+
+			if (reply) {
+				if (!(in_reply_to->flags & TF_ACCEPT_FDS)) {
+					binder_user_error("binder: %d:%d got reply with fd, %ld, but target does not allow fds\n",
+						proc->pid, thread->pid, fp->handle);
+					return_error = BR_FAILED_REPLY;
+					goto err_fd_not_allowed;
+				}
+			} else if (!target_node->accept_fds) {
+				binder_user_error("binder: %d:%d got transaction with fd, %ld, but target does not allow fds\n",
+					proc->pid, thread->pid, fp->handle);
+				return_error = BR_FAILED_REPLY;
+				goto err_fd_not_allowed;
+			}
+
+			file = fget(fp->handle);
+			if (file == NULL) {
+				binder_user_error("binder: %d:%d got transaction with invalid fd, %ld\n",
+					proc->pid, thread->pid, fp->handle);
+				return_error = BR_FAILED_REPLY;
+				goto err_fget_failed;
+			}
+			target_fd = task_get_unused_fd_flags(target_proc->tsk, O_CLOEXEC);
+			if (target_fd < 0) {
+				fput(file);
+				return_error = BR_FAILED_REPLY;
+				goto err_get_unused_fd_failed;
+			}
+			task_fd_install(target_proc->tsk, target_fd, file);
+			if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+				printk(KERN_INFO "        fd %ld -> %d\n", fp->handle, target_fd);
+			/* TODO: fput? */
+			fp->handle = target_fd;
+		} break;
+
+		default:
+			binder_user_error("binder: %d:%d got transactio"
+				"n with invalid object type, %lx\n",
+				proc->pid, thread->pid, fp->type);
+			return_error = BR_FAILED_REPLY;
+			goto err_bad_object_type;
+		}
+	}
+	if (reply) {
+		BUG_ON(t->buffer->async_transaction != 0);
+		binder_pop_transaction(target_thread, in_reply_to);
+	} else if (!(t->flags & TF_ONE_WAY)) {
+		BUG_ON(t->buffer->async_transaction != 0);
+		t->need_reply = 1;
+		t->from_parent = thread->transaction_stack;
+		thread->transaction_stack = t;
+	} else {
+		BUG_ON(target_node == NULL);
+		BUG_ON(t->buffer->async_transaction != 1);
+		if (target_node->has_async_transaction) {
+			target_list = &target_node->async_todo;
+			target_wait = NULL;
+		} else
+			target_node->has_async_transaction = 1;
+	}
+	t->work.type = BINDER_WORK_TRANSACTION;
+	list_add_tail(&t->work.entry, target_list);
+	tcomplete->type = BINDER_WORK_TRANSACTION_COMPLETE;
+	list_add_tail(&tcomplete->entry, &thread->todo);
+	if (target_wait)
+		wake_up_interruptible(target_wait);
+	return;
+
+err_get_unused_fd_failed:
+err_fget_failed:
+err_fd_not_allowed:
+err_binder_get_ref_for_node_failed:
+err_binder_get_ref_failed:
+err_binder_new_node_failed:
+err_bad_object_type:
+err_bad_offset:
+err_copy_data_failed:
+	binder_transaction_buffer_release(target_proc, t->buffer, offp);
+	t->buffer->transaction = NULL;
+	binder_free_buf(target_proc, t->buffer);
+err_binder_alloc_buf_failed:
+	kfree(tcomplete);
+	binder_stats.obj_deleted[BINDER_STAT_TRANSACTION_COMPLETE]++;
+err_alloc_tcomplete_failed:
+	kfree(t);
+	binder_stats.obj_deleted[BINDER_STAT_TRANSACTION]++;
+err_alloc_t_failed:
+err_bad_call_stack:
+err_empty_call_stack:
+err_dead_binder:
+err_invalid_target_handle:
+err_no_context_mgr_node:
+	if (binder_debug_mask & BINDER_DEBUG_FAILED_TRANSACTION)
+		printk(KERN_INFO "binder: %d:%d transaction failed %d, size"
+				"%zd-%zd\n",
+			   proc->pid, thread->pid, return_error,
+			   tr->data_size, tr->offsets_size);
+
+	{
+		struct binder_transaction_log_entry *fe;
+		fe = binder_transaction_log_add(&binder_transaction_log_failed);
+		*fe = *e;
+	}
+
+	BUG_ON(thread->return_error != BR_OK);
+	if (in_reply_to) {
+		thread->return_error = BR_TRANSACTION_COMPLETE;
+		binder_send_failed_reply(in_reply_to, return_error);
+	} else
+		thread->return_error = return_error;
+}
+
+static void
+binder_transaction_buffer_release(struct binder_proc *proc, struct binder_buffer *buffer, size_t *failed_at)
+{
+	size_t *offp, *off_end;
+	int debug_id = buffer->debug_id;
+
+	if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+		printk(KERN_INFO "binder: %d buffer release %d, size %zd-%zd, failed at %p\n",
+			   proc->pid, buffer->debug_id,
+			   buffer->data_size, buffer->offsets_size, failed_at);
+
+	if (buffer->target_node)
+		binder_dec_node(buffer->target_node, 1, 0);
+
+	offp = (size_t *)(buffer->data + ALIGN(buffer->data_size, sizeof(void *)));
+	if (failed_at)
+		off_end = failed_at;
+	else
+		off_end = (void *)offp + buffer->offsets_size;
+	for (; offp < off_end; offp++) {
+		struct flat_binder_object *fp;
+		if (*offp > buffer->data_size - sizeof(*fp)) {
+			printk(KERN_ERR "binder: transaction release %d bad"
+					"offset %zd, size %zd\n", debug_id, *offp, buffer->data_size);
+			continue;
+		}
+		fp = (struct flat_binder_object *)(buffer->data + *offp);
+		switch (fp->type) {
+		case BINDER_TYPE_BINDER:
+		case BINDER_TYPE_WEAK_BINDER: {
+			struct binder_node *node = binder_get_node(proc, fp->binder);
+			if (node == NULL) {
+				printk(KERN_ERR "binder: transaction release %d bad node %p\n", debug_id, fp->binder);
+				break;
+			}
+			if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+				printk(KERN_INFO "        node %d u%p\n",
+				       node->debug_id, node->ptr);
+			binder_dec_node(node, fp->type == BINDER_TYPE_BINDER, 0);
+		} break;
+		case BINDER_TYPE_HANDLE:
+		case BINDER_TYPE_WEAK_HANDLE: {
+			struct binder_ref *ref = binder_get_ref(proc, fp->handle);
+			if (ref == NULL) {
+				printk(KERN_ERR "binder: transaction release %d bad handle %ld\n", debug_id, fp->handle);
+				break;
+			}
+			if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+				printk(KERN_INFO "        ref %d desc %d (node %d)\n",
+				       ref->debug_id, ref->desc, ref->node->debug_id);
+			binder_dec_ref(ref, fp->type == BINDER_TYPE_HANDLE);
+		} break;
+
+		case BINDER_TYPE_FD:
+			if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+				printk(KERN_INFO "        fd %ld\n", fp->handle);
+			if (failed_at)
+				task_close_fd(proc->tsk, fp->handle);
+			break;
+
+		default:
+			printk(KERN_ERR "binder: transaction release %d bad object type %lx\n", debug_id, fp->type);
+			break;
+		}
+	}
+}
+
+int
+binder_thread_write(struct binder_proc *proc, struct binder_thread *thread,
+		    void __user *buffer, int size, signed long *consumed)
+{
+	uint32_t cmd;
+	void __user *ptr = buffer + *consumed;
+	void __user *end = buffer + size;
+
+	while (ptr < end && thread->return_error == BR_OK) {
+		if (get_user(cmd, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+		if (_IOC_NR(cmd) < ARRAY_SIZE(binder_stats.bc)) {
+			binder_stats.bc[_IOC_NR(cmd)]++;
+			proc->stats.bc[_IOC_NR(cmd)]++;
+			thread->stats.bc[_IOC_NR(cmd)]++;
+		}
+		switch (cmd) {
+		case BC_INCREFS:
+		case BC_ACQUIRE:
+		case BC_RELEASE:
+		case BC_DECREFS: {
+			uint32_t target;
+			struct binder_ref *ref;
+			const char *debug_string;
+
+			if (get_user(target, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (target == 0 && binder_context_mgr_node &&
+			    (cmd == BC_INCREFS || cmd == BC_ACQUIRE)) {
+				ref = binder_get_ref_for_node(proc,
+					       binder_context_mgr_node);
+				if (ref->desc != target) {
+					binder_user_error("binder: %d:"
+						"%d tried to acquire "
+						"reference to desc 0, "
+						"got %d instead\n",
+						proc->pid, thread->pid,
+						ref->desc);
+				}
+			} else
+				ref = binder_get_ref(proc, target);
+			if (ref == NULL) {
+				binder_user_error("binder: %d:%d refcou"
+					"nt change on invalid ref %d\n",
+					proc->pid, thread->pid, target);
+				break;
+			}
+			switch (cmd) {
+			case BC_INCREFS:
+				debug_string = "IncRefs";
+				binder_inc_ref(ref, 0, NULL);
+				break;
+			case BC_ACQUIRE:
+				debug_string = "Acquire";
+				binder_inc_ref(ref, 1, NULL);
+				break;
+			case BC_RELEASE:
+				debug_string = "Release";
+				binder_dec_ref(ref, 1);
+				break;
+			case BC_DECREFS:
+			default:
+				debug_string = "DecRefs";
+				binder_dec_ref(ref, 0);
+				break;
+			}
+			if (binder_debug_mask & BINDER_DEBUG_USER_REFS)
+				printk(KERN_INFO "binder: %d:%d %s ref %d desc %d s %d w %d for node %d\n",
+				       proc->pid, thread->pid, debug_string, ref->debug_id, ref->desc, ref->strong, ref->weak, ref->node->debug_id);
+			break;
+		}
+		case BC_INCREFS_DONE:
+		case BC_ACQUIRE_DONE: {
+			void __user *node_ptr;
+			void *cookie;
+			struct binder_node *node;
+
+			if (get_user(node_ptr, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			if (get_user(cookie, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			node = binder_get_node(proc, node_ptr);
+			if (node == NULL) {
+				binder_user_error("binder: %d:%d "
+					"%s u%p no match\n",
+					proc->pid, thread->pid,
+					cmd == BC_INCREFS_DONE ?
+					"BC_INCREFS_DONE" :
+					"BC_ACQUIRE_DONE",
+					node_ptr);
+				break;
+			}
+			if (cookie != node->cookie) {
+				binder_user_error("binder: %d:%d %s u%p node %d"
+					" cookie mismatch %p != %p\n",
+					proc->pid, thread->pid,
+					cmd == BC_INCREFS_DONE ?
+					"BC_INCREFS_DONE" : "BC_ACQUIRE_DONE",
+					node_ptr, node->debug_id,
+					cookie, node->cookie);
+				break;
+			}
+			if (cmd == BC_ACQUIRE_DONE) {
+				if (node->pending_strong_ref == 0) {
+					binder_user_error("binder: %d:%d "
+						"BC_ACQUIRE_DONE node %d has "
+						"no pending acquire request\n",
+						proc->pid, thread->pid,
+						node->debug_id);
+					break;
+				}
+				node->pending_strong_ref = 0;
+			} else {
+				if (node->pending_weak_ref == 0) {
+					binder_user_error("binder: %d:%d "
+						"BC_INCREFS_DONE node %d has "
+						"no pending increfs request\n",
+						proc->pid, thread->pid,
+						node->debug_id);
+					break;
+				}
+				node->pending_weak_ref = 0;
+			}
+			binder_dec_node(node, cmd == BC_ACQUIRE_DONE, 0);
+			if (binder_debug_mask & BINDER_DEBUG_USER_REFS)
+				printk(KERN_INFO "binder: %d:%d %s node %d ls %d lw %d\n",
+				       proc->pid, thread->pid, cmd == BC_INCREFS_DONE ? "BC_INCREFS_DONE" : "BC_ACQUIRE_DONE", node->debug_id, node->local_strong_refs, node->local_weak_refs);
+			break;
+		}
+		case BC_ATTEMPT_ACQUIRE:
+			printk(KERN_ERR "binder: BC_ATTEMPT_ACQUIRE not supported\n");
+			return -EINVAL;
+		case BC_ACQUIRE_RESULT:
+			printk(KERN_ERR "binder: BC_ACQUIRE_RESULT not supported\n");
+			return -EINVAL;
+
+		case BC_FREE_BUFFER: {
+			void __user *data_ptr;
+			struct binder_buffer *buffer;
+
+			if (get_user(data_ptr, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+
+			buffer = binder_buffer_lookup(proc, data_ptr);
+			if (buffer == NULL) {
+				binder_user_error("binder: %d:%d "
+					"BC_FREE_BUFFER u%p no match\n",
+					proc->pid, thread->pid, data_ptr);
+				break;
+			}
+			if (!buffer->allow_user_free) {
+				binder_user_error("binder: %d:%d "
+					"BC_FREE_BUFFER u%p matched "
+					"unreturned buffer\n",
+					proc->pid, thread->pid, data_ptr);
+				break;
+			}
+			if (binder_debug_mask & BINDER_DEBUG_FREE_BUFFER)
+				printk(KERN_INFO "binder: %d:%d BC_FREE_BUFFER u%p found buffer %d for %s transaction\n",
+				       proc->pid, thread->pid, data_ptr, buffer->debug_id,
+				       buffer->transaction ? "active" : "finished");
+
+			if (buffer->transaction) {
+				buffer->transaction->buffer = NULL;
+				buffer->transaction = NULL;
+			}
+			if (buffer->async_transaction && buffer->target_node) {
+				BUG_ON(!buffer->target_node->has_async_transaction);
+				if (list_empty(&buffer->target_node->async_todo))
+					buffer->target_node->has_async_transaction = 0;
+				else
+					list_move_tail(buffer->target_node->async_todo.next, &thread->todo);
+			}
+			binder_transaction_buffer_release(proc, buffer, NULL);
+			binder_free_buf(proc, buffer);
+			break;
+		}
+
+		case BC_TRANSACTION:
+		case BC_REPLY: {
+			struct binder_transaction_data tr;
+
+			if (copy_from_user(&tr, ptr, sizeof(tr)))
+				return -EFAULT;
+			ptr += sizeof(tr);
+			binder_transaction(proc, thread, &tr, cmd == BC_REPLY);
+			break;
+		}
+
+		case BC_REGISTER_LOOPER:
+			if (binder_debug_mask & BINDER_DEBUG_THREADS)
+				printk(KERN_INFO "binder: %d:%d BC_REGISTER_LOOPER\n",
+				       proc->pid, thread->pid);
+			if (thread->looper & BINDER_LOOPER_STATE_ENTERED) {
+				thread->looper |= BINDER_LOOPER_STATE_INVALID;
+				binder_user_error("binder: %d:%d ERROR:"
+					" BC_REGISTER_LOOPER called "
+					"after BC_ENTER_LOOPER\n",
+					proc->pid, thread->pid);
+			} else if (proc->requested_threads == 0) {
+				thread->looper |= BINDER_LOOPER_STATE_INVALID;
+				binder_user_error("binder: %d:%d ERROR:"
+					" BC_REGISTER_LOOPER called "
+					"without request\n",
+					proc->pid, thread->pid);
+			} else {
+				proc->requested_threads--;
+				proc->requested_threads_started++;
+			}
+			thread->looper |= BINDER_LOOPER_STATE_REGISTERED;
+			break;
+		case BC_ENTER_LOOPER:
+			if (binder_debug_mask & BINDER_DEBUG_THREADS)
+				printk(KERN_INFO "binder: %d:%d BC_ENTER_LOOPER\n",
+				       proc->pid, thread->pid);
+			if (thread->looper & BINDER_LOOPER_STATE_REGISTERED) {
+				thread->looper |= BINDER_LOOPER_STATE_INVALID;
+				binder_user_error("binder: %d:%d ERROR:"
+					" BC_ENTER_LOOPER called after "
+					"BC_REGISTER_LOOPER\n",
+					proc->pid, thread->pid);
+			}
+			thread->looper |= BINDER_LOOPER_STATE_ENTERED;
+			break;
+		case BC_EXIT_LOOPER:
+			if (binder_debug_mask & BINDER_DEBUG_THREADS)
+				printk(KERN_INFO "binder: %d:%d BC_EXIT_LOOPER\n",
+				       proc->pid, thread->pid);
+			thread->looper |= BINDER_LOOPER_STATE_EXITED;
+			break;
+
+		case BC_REQUEST_DEATH_NOTIFICATION:
+		case BC_CLEAR_DEATH_NOTIFICATION: {
+			uint32_t target;
+			void __user *cookie;
+			struct binder_ref *ref;
+			struct binder_ref_death *death;
+
+			if (get_user(target, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (get_user(cookie, (void __user * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			ref = binder_get_ref(proc, target);
+			if (ref == NULL) {
+				binder_user_error("binder: %d:%d %s "
+					"invalid ref %d\n",
+					proc->pid, thread->pid,
+					cmd == BC_REQUEST_DEATH_NOTIFICATION ?
+					"BC_REQUEST_DEATH_NOTIFICATION" :
+					"BC_CLEAR_DEATH_NOTIFICATION",
+					target);
+				break;
+			}
+
+			if (binder_debug_mask & BINDER_DEBUG_DEATH_NOTIFICATION)
+				printk(KERN_INFO "binder: %d:%d %s %p ref %d desc %d s %d w %d for node %d\n",
+				       proc->pid, thread->pid,
+				       cmd == BC_REQUEST_DEATH_NOTIFICATION ?
+				       "BC_REQUEST_DEATH_NOTIFICATION" :
+				       "BC_CLEAR_DEATH_NOTIFICATION",
+				       cookie, ref->debug_id, ref->desc,
+				       ref->strong, ref->weak, ref->node->debug_id);
+
+			if (cmd == BC_REQUEST_DEATH_NOTIFICATION) {
+				if (ref->death) {
+					binder_user_error("binder: %d:%"
+						"d BC_REQUEST_DEATH_NOTI"
+						"FICATION death notific"
+						"ation already set\n",
+						proc->pid, thread->pid);
+					break;
+				}
+				death = kzalloc(sizeof(*death), GFP_KERNEL);
+				if (death == NULL) {
+					thread->return_error = BR_ERROR;
+					if (binder_debug_mask & BINDER_DEBUG_FAILED_TRANSACTION)
+						printk(KERN_INFO "binder: %d:%d "
+							"BC_REQUEST_DEATH_NOTIFICATION failed\n",
+							proc->pid, thread->pid);
+					break;
+				}
+				binder_stats.obj_created[BINDER_STAT_DEATH]++;
+				INIT_LIST_HEAD(&death->work.entry);
+				death->cookie = cookie;
+				ref->death = death;
+				if (ref->node->proc == NULL) {
+					ref->death->work.type = BINDER_WORK_DEAD_BINDER;
+					if (thread->looper & (BINDER_LOOPER_STATE_REGISTERED | BINDER_LOOPER_STATE_ENTERED)) {
+						list_add_tail(&ref->death->work.entry, &thread->todo);
+					} else {
+						list_add_tail(&ref->death->work.entry, &proc->todo);
+						wake_up_interruptible(&proc->wait);
+					}
+				}
+			} else {
+				if (ref->death == NULL) {
+					binder_user_error("binder: %d:%"
+						"d BC_CLEAR_DEATH_NOTIFI"
+						"CATION death notificat"
+						"ion not active\n",
+						proc->pid, thread->pid);
+					break;
+				}
+				death = ref->death;
+				if (death->cookie != cookie) {
+					binder_user_error("binder: %d:%"
+						"d BC_CLEAR_DEATH_NOTIFI"
+						"CATION death notificat"
+						"ion cookie mismatch "
+						"%p != %p\n",
+						proc->pid, thread->pid,
+						death->cookie, cookie);
+					break;
+				}
+				ref->death = NULL;
+				if (list_empty(&death->work.entry)) {
+					death->work.type = BINDER_WORK_CLEAR_DEATH_NOTIFICATION;
+					if (thread->looper & (BINDER_LOOPER_STATE_REGISTERED | BINDER_LOOPER_STATE_ENTERED)) {
+						list_add_tail(&death->work.entry, &thread->todo);
+					} else {
+						list_add_tail(&death->work.entry, &proc->todo);
+						wake_up_interruptible(&proc->wait);
+					}
+				} else {
+					BUG_ON(death->work.type != BINDER_WORK_DEAD_BINDER);
+					death->work.type = BINDER_WORK_DEAD_BINDER_AND_CLEAR;
+				}
+			}
+		} break;
+		case BC_DEAD_BINDER_DONE: {
+			struct binder_work *w;
+			void __user *cookie;
+			struct binder_ref_death *death = NULL;
+			if (get_user(cookie, (void __user * __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;
+				}
+			}
+			if (binder_debug_mask & BINDER_DEBUG_DEAD_BINDER)
+				printk(KERN_INFO "binder: %d:%d BC_DEAD_BINDER_DONE %p found %p\n",
+				       proc->pid, thread->pid, cookie, death);
+			if (death == NULL) {
+				binder_user_error("binder: %d:%d BC_DEAD"
+					"_BINDER_DONE %p not found\n",
+					proc->pid, thread->pid, cookie);
+				break;
+			}
+
+			list_del_init(&death->work.entry);
+			if (death->work.type == BINDER_WORK_DEAD_BINDER_AND_CLEAR) {
+				death->work.type = BINDER_WORK_CLEAR_DEATH_NOTIFICATION;
+				if (thread->looper & (BINDER_LOOPER_STATE_REGISTERED | BINDER_LOOPER_STATE_ENTERED)) {
+					list_add_tail(&death->work.entry, &thread->todo);
+				} else {
+					list_add_tail(&death->work.entry, &proc->todo);
+					wake_up_interruptible(&proc->wait);
+				}
+			}
+		} break;
+
+		default:
+			printk(KERN_ERR "binder: %d:%d unknown command %d\n", proc->pid, thread->pid, cmd);
+			return -EINVAL;
+		}
+		*consumed = ptr - buffer;
+	}
+	return 0;
+}
+
+void
+binder_stat_br(struct binder_proc *proc, struct binder_thread *thread, uint32_t cmd)
+{
+	if (_IOC_NR(cmd) < ARRAY_SIZE(binder_stats.br)) {
+		binder_stats.br[_IOC_NR(cmd)]++;
+		proc->stats.br[_IOC_NR(cmd)]++;
+		thread->stats.br[_IOC_NR(cmd)]++;
+	}
+}
+
+static int
+binder_has_proc_work(struct binder_proc *proc, struct binder_thread *thread)
+{
+	return !list_empty(&proc->todo) || (thread->looper & BINDER_LOOPER_STATE_NEED_RETURN);
+}
+
+static int
+binder_has_thread_work(struct binder_thread *thread)
+{
+	return !list_empty(&thread->todo) || thread->return_error != BR_OK ||
+		(thread->looper & BINDER_LOOPER_STATE_NEED_RETURN);
+}
+
+static int
+binder_thread_read(struct binder_proc *proc, struct binder_thread *thread,
+	void  __user *buffer, int size, signed long *consumed, int non_block)
+{
+	void __user *ptr = buffer + *consumed;
+	void __user *end = buffer + size;
+
+	int ret = 0;
+	int wait_for_proc_work;
+
+	if (*consumed == 0) {
+		if (put_user(BR_NOOP, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+	}
+
+retry:
+	wait_for_proc_work = thread->transaction_stack == NULL && list_empty(&thread->todo);
+
+	if (thread->return_error != BR_OK && ptr < end) {
+		if (thread->return_error2 != BR_OK) {
+			if (put_user(thread->return_error2, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (ptr == end)
+				goto done;
+			thread->return_error2 = BR_OK;
+		}
+		if (put_user(thread->return_error, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+		thread->return_error = BR_OK;
+		goto done;
+	}
+
+
+	thread->looper |= BINDER_LOOPER_STATE_WAITING;
+	if (wait_for_proc_work)
+		proc->ready_threads++;
+	mutex_unlock(&binder_lock);
+	if (wait_for_proc_work) {
+		if (!(thread->looper & (BINDER_LOOPER_STATE_REGISTERED |
+					BINDER_LOOPER_STATE_ENTERED))) {
+			binder_user_error("binder: %d:%d ERROR: Thread waiting "
+				"for process work before calling BC_REGISTER_"
+				"LOOPER or BC_ENTER_LOOPER (state %x)\n",
+				proc->pid, thread->pid, thread->looper);
+			wait_event_interruptible(binder_user_error_wait, binder_stop_on_user_error < 2);
+		}
+		binder_set_nice(proc->default_priority);
+		if (non_block) {
+			if (!binder_has_proc_work(proc, thread))
+				ret = -EAGAIN;
+		} else
+			ret = wait_event_interruptible_exclusive(proc->wait, binder_has_proc_work(proc, thread));
+	} else {
+		if (non_block) {
+			if (!binder_has_thread_work(thread))
+				ret = -EAGAIN;
+		} else
+			ret = wait_event_interruptible(thread->wait, binder_has_thread_work(thread));
+	}
+	mutex_lock(&binder_lock);
+	if (wait_for_proc_work)
+		proc->ready_threads--;
+	thread->looper &= ~BINDER_LOOPER_STATE_WAITING;
+
+	if (ret)
+		return ret;
+
+	while (1) {
+		uint32_t cmd;
+		struct binder_transaction_data tr;
+		struct binder_work *w;
+		struct binder_transaction *t = NULL;
+
+		if (!list_empty(&thread->todo))
+			w = list_first_entry(&thread->todo, struct binder_work, entry);
+		else if (!list_empty(&proc->todo) && wait_for_proc_work)
+			w = list_first_entry(&proc->todo, struct binder_work, entry);
+		else {
+			if (ptr - buffer == 4 && !(thread->looper & BINDER_LOOPER_STATE_NEED_RETURN)) /* no data added */
+				goto retry;
+			break;
+		}
+
+		if (end - ptr < sizeof(tr) + 4)
+			break;
+
+		switch (w->type) {
+		case BINDER_WORK_TRANSACTION: {
+			t = container_of(w, struct binder_transaction, work);
+		} break;
+		case BINDER_WORK_TRANSACTION_COMPLETE: {
+			cmd = BR_TRANSACTION_COMPLETE;
+			if (put_user(cmd, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+
+			binder_stat_br(proc, thread, cmd);
+			if (binder_debug_mask & BINDER_DEBUG_TRANSACTION_COMPLETE)
+				printk(KERN_INFO "binder: %d:%d BR_TRANSACTION_COMPLETE\n",
+				       proc->pid, thread->pid);
+
+			list_del(&w->entry);
+			kfree(w);
+			binder_stats.obj_deleted[BINDER_STAT_TRANSACTION_COMPLETE]++;
+		} break;
+		case BINDER_WORK_NODE: {
+			struct binder_node *node = container_of(w, struct binder_node, work);
+			uint32_t cmd = BR_NOOP;
+			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";
+				node->has_weak_ref = 1;
+				node->pending_weak_ref = 1;
+				node->local_weak_refs++;
+			} else if (strong && !node->has_strong_ref) {
+				cmd = BR_ACQUIRE;
+				cmd_name = "BR_ACQUIRE";
+				node->has_strong_ref = 1;
+				node->pending_strong_ref = 1;
+				node->local_strong_refs++;
+			} else if (!strong && node->has_strong_ref) {
+				cmd = BR_RELEASE;
+				cmd_name = "BR_RELEASE";
+				node->has_strong_ref = 0;
+			} else if (!weak && node->has_weak_ref) {
+				cmd = BR_DECREFS;
+				cmd_name = "BR_DECREFS";
+				node->has_weak_ref = 0;
+			}
+			if (cmd != BR_NOOP) {
+				if (put_user(cmd, (uint32_t __user *)ptr))
+					return -EFAULT;
+				ptr += sizeof(uint32_t);
+				if (put_user(node->ptr, (void * __user *)ptr))
+					return -EFAULT;
+				ptr += sizeof(void *);
+				if (put_user(node->cookie, (void * __user *)ptr))
+					return -EFAULT;
+				ptr += sizeof(void *);
+
+				binder_stat_br(proc, thread, cmd);
+				if (binder_debug_mask & BINDER_DEBUG_USER_REFS)
+					printk(KERN_INFO "binder: %d:%d %s %d u%p c%p\n",
+					       proc->pid, thread->pid, cmd_name, node->debug_id, node->ptr, node->cookie);
+			} else {
+				list_del_init(&w->entry);
+				if (!weak && !strong) {
+					if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+						printk(KERN_INFO "binder: %d:%d node %d u%p c%p deleted\n",
+						       proc->pid, thread->pid, node->debug_id, node->ptr, node->cookie);
+					rb_erase(&node->rb_node, &proc->nodes);
+					kfree(node);
+					binder_stats.obj_deleted[BINDER_STAT_NODE]++;
+				} else {
+					if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+						printk(KERN_INFO "binder: %d:%d node %d u%p c%p state unchanged\n",
+						       proc->pid, thread->pid, node->debug_id, node->ptr, node->cookie);
+				}
+			}
+		} break;
+		case BINDER_WORK_DEAD_BINDER:
+		case BINDER_WORK_DEAD_BINDER_AND_CLEAR:
+		case BINDER_WORK_CLEAR_DEATH_NOTIFICATION: {
+			struct binder_ref_death *death = container_of(w, struct binder_ref_death, work);
+			uint32_t cmd;
+			if (w->type == BINDER_WORK_CLEAR_DEATH_NOTIFICATION)
+				cmd = BR_CLEAR_DEATH_NOTIFICATION_DONE;
+			else
+				cmd = BR_DEAD_BINDER;
+			if (put_user(cmd, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (put_user(death->cookie, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			if (binder_debug_mask & BINDER_DEBUG_DEATH_NOTIFICATION)
+				printk(KERN_INFO "binder: %d:%d %s %p\n",
+				       proc->pid, thread->pid,
+				       cmd == BR_DEAD_BINDER ?
+				       "BR_DEAD_BINDER" :
+				       "BR_CLEAR_DEATH_NOTIFICATION_DONE",
+				       death->cookie);
+
+			if (w->type == BINDER_WORK_CLEAR_DEATH_NOTIFICATION) {
+				list_del(&w->entry);
+				kfree(death);
+				binder_stats.obj_deleted[BINDER_STAT_DEATH]++;
+			} else
+				list_move(&w->entry, &proc->delivered_death);
+			if (cmd == BR_DEAD_BINDER)
+				goto done; /* DEAD_BINDER notifications can cause transactions */
+		} break;
+		}
+
+		if (!t)
+			continue;
+
+		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);
+			if (t->priority < target_node->min_priority &&
+			    !(t->flags & TF_ONE_WAY))
+				binder_set_nice(t->priority);
+			else if (!(t->flags & TF_ONE_WAY) ||
+				 t->saved_priority > target_node->min_priority)
+				binder_set_nice(target_node->min_priority);
+			cmd = BR_TRANSACTION;
+		} else {
+			tr.target.ptr = NULL;
+			tr.cookie = NULL;
+			cmd = BR_REPLY;
+		}
+		tr.code = t->code;
+		tr.flags = t->flags;
+		tr.sender_euid = t->sender_euid;
+
+		if (t->from) {
+			struct task_struct *sender = t->from->proc->tsk;
+			tr.sender_pid = task_tgid_nr_ns(sender, current->nsproxy->pid_ns);
+		} else {
+			tr.sender_pid = 0;
+		}
+
+		tr.data_size = t->buffer->data_size;
+		tr.offsets_size = t->buffer->offsets_size;
+		tr.data.ptr.buffer = (void *)((void *)t->buffer->data + proc->user_buffer_offset);
+		tr.data.ptr.offsets = tr.data.ptr.buffer + ALIGN(t->buffer->data_size, sizeof(void *));
+
+		if (put_user(cmd, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+		if (copy_to_user(ptr, &tr, sizeof(tr)))
+			return -EFAULT;
+		ptr += sizeof(tr);
+
+		binder_stat_br(proc, thread, cmd);
+		if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+			printk(KERN_INFO "binder: %d:%d %s %d %d:%d, cmd %d"
+				"size %zd-%zd ptr %p-%p\n",
+			       proc->pid, thread->pid,
+			       (cmd == BR_TRANSACTION) ? "BR_TRANSACTION" : "BR_REPLY",
+			       t->debug_id, t->from ? t->from->proc->pid : 0,
+			       t->from ? t->from->pid : 0, cmd,
+			       t->buffer->data_size, t->buffer->offsets_size,
+			       tr.data.ptr.buffer, tr.data.ptr.offsets);
+
+		list_del(&t->work.entry);
+		t->buffer->allow_user_free = 1;
+		if (cmd == BR_TRANSACTION && !(t->flags & TF_ONE_WAY)) {
+			t->to_parent = thread->transaction_stack;
+			t->to_thread = thread;
+			thread->transaction_stack = t;
+		} else {
+			t->buffer->transaction = NULL;
+			kfree(t);
+			binder_stats.obj_deleted[BINDER_STAT_TRANSACTION]++;
+		}
+		break;
+	}
+
+done:
+
+	*consumed = ptr - buffer;
+	if (proc->requested_threads + proc->ready_threads == 0 &&
+	    proc->requested_threads_started < proc->max_threads &&
+	    (thread->looper & (BINDER_LOOPER_STATE_REGISTERED |
+	     BINDER_LOOPER_STATE_ENTERED)) /* the user-space code fails to */
+	     /*spawn a new thread if we leave this out */) {
+		proc->requested_threads++;
+		if (binder_debug_mask & BINDER_DEBUG_THREADS)
+			printk(KERN_INFO "binder: %d:%d BR_SPAWN_LOOPER\n",
+			       proc->pid, thread->pid);
+		if (put_user(BR_SPAWN_LOOPER, (uint32_t __user *)buffer))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+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);
+		switch (w->type) {
+		case BINDER_WORK_TRANSACTION: {
+			struct binder_transaction *t = container_of(w, struct binder_transaction, work);
+			if (t->buffer->target_node && !(t->flags & TF_ONE_WAY))
+				binder_send_failed_reply(t, BR_DEAD_REPLY);
+		} break;
+		case BINDER_WORK_TRANSACTION_COMPLETE: {
+			kfree(w);
+			binder_stats.obj_deleted[BINDER_STAT_TRANSACTION_COMPLETE]++;
+		} break;
+		default:
+			break;
+		}
+	}
+
+}
+
+static struct binder_thread *binder_get_thread(struct binder_proc *proc)
+{
+	struct binder_thread *thread = NULL;
+	struct rb_node *parent = NULL;
+	struct rb_node **p = &proc->threads.rb_node;
+
+	while (*p) {
+		parent = *p;
+		thread = rb_entry(parent, struct binder_thread, rb_node);
+
+		if (current->pid < thread->pid)
+			p = &(*p)->rb_left;
+		else if (current->pid > thread->pid)
+			p = &(*p)->rb_right;
+		else
+			break;
+	}
+	if (*p == NULL) {
+		thread = kzalloc(sizeof(*thread), GFP_KERNEL);
+		if (thread == NULL)
+			return NULL;
+		binder_stats.obj_created[BINDER_STAT_THREAD]++;
+		thread->proc = proc;
+		thread->pid = current->pid;
+		init_waitqueue_head(&thread->wait);
+		INIT_LIST_HEAD(&thread->todo);
+		rb_link_node(&thread->rb_node, parent, p);
+		rb_insert_color(&thread->rb_node, &proc->threads);
+		thread->looper |= BINDER_LOOPER_STATE_NEED_RETURN;
+		thread->return_error = BR_OK;
+		thread->return_error2 = BR_OK;
+	}
+	return thread;
+}
+
+static int binder_free_thread(struct binder_proc *proc, struct binder_thread *thread)
+{
+	struct binder_transaction *t;
+	struct binder_transaction *send_reply = NULL;
+	int active_transactions = 0;
+
+	rb_erase(&thread->rb_node, &proc->threads);
+	t = thread->transaction_stack;
+	if (t && t->to_thread == thread)
+		send_reply = t;
+	while (t) {
+		active_transactions++;
+		if (binder_debug_mask & BINDER_DEBUG_DEAD_TRANSACTION)
+			printk(KERN_INFO "binder: release %d:%d transaction %d %s, still active\n",
+			       proc->pid, thread->pid, t->debug_id, (t->to_thread == thread) ? "in" : "out");
+		if (t->to_thread == thread) {
+			t->to_proc = NULL;
+			t->to_thread = NULL;
+			if (t->buffer) {
+				t->buffer->transaction = NULL;
+				t->buffer = NULL;
+			}
+			t = t->to_parent;
+		} else if (t->from == thread) {
+			t->from = NULL;
+			t = t->from_parent;
+		} else
+			BUG();
+	}
+	if (send_reply)
+		binder_send_failed_reply(send_reply, BR_DEAD_REPLY);
+	binder_release_work(&thread->todo);
+	kfree(thread);
+	binder_stats.obj_deleted[BINDER_STAT_THREAD]++;
+	return active_transactions;
+}
+
+static unsigned int binder_poll(struct file *filp, struct poll_table_struct *wait)
+{
+	struct binder_proc *proc = filp->private_data;
+	struct binder_thread *thread = NULL;
+	int wait_for_proc_work;
+
+	mutex_lock(&binder_lock);
+	thread = binder_get_thread(proc);
+
+	wait_for_proc_work = thread->transaction_stack == NULL &&
+		list_empty(&thread->todo) && thread->return_error == BR_OK;
+	mutex_unlock(&binder_lock);
+
+	if (wait_for_proc_work) {
+		if (binder_has_proc_work(proc, thread))
+			return POLLIN;
+		poll_wait(filp, &proc->wait, wait);
+		if (binder_has_proc_work(proc, thread))
+			return POLLIN;
+	} else {
+		if (binder_has_thread_work(thread))
+			return POLLIN;
+		poll_wait(filp, &thread->wait, wait);
+		if (binder_has_thread_work(thread))
+			return POLLIN;
+	}
+	return 0;
+}
+
+static long binder_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	int ret;
+	struct binder_proc *proc = filp->private_data;
+	struct binder_thread *thread;
+	unsigned int size = _IOC_SIZE(cmd);
+	void __user *ubuf = (void __user *)arg;
+
+	/*printk(KERN_INFO "binder_ioctl: %d:%d %x %lx\n", proc->pid, current->pid, cmd, arg);*/
+
+	ret = wait_event_interruptible(binder_user_error_wait, binder_stop_on_user_error < 2);
+	if (ret)
+		return ret;
+
+	mutex_lock(&binder_lock);
+	thread = binder_get_thread(proc);
+	if (thread == NULL) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	switch (cmd) {
+	case BINDER_WRITE_READ: {
+		struct binder_write_read bwr;
+		if (size != sizeof(struct binder_write_read)) {
+			ret = -EINVAL;
+			goto err;
+		}
+		if (copy_from_user(&bwr, ubuf, sizeof(bwr))) {
+			ret = -EFAULT;
+			goto err;
+		}
+		if (binder_debug_mask & BINDER_DEBUG_READ_WRITE)
+			printk(KERN_INFO "binder: %d:%d write %ld at %08lx, read %ld at %08lx\n",
+			       proc->pid, thread->pid, bwr.write_size, bwr.write_buffer, bwr.read_size, bwr.read_buffer);
+		if (bwr.write_size > 0) {
+			ret = binder_thread_write(proc, thread, (void __user *)bwr.write_buffer, bwr.write_size, &bwr.write_consumed);
+			if (ret < 0) {
+				bwr.read_consumed = 0;
+				if (copy_to_user(ubuf, &bwr, sizeof(bwr)))
+					ret = -EFAULT;
+				goto err;
+			}
+		}
+		if (bwr.read_size > 0) {
+			ret = binder_thread_read(proc, thread, (void __user *)bwr.read_buffer, bwr.read_size, &bwr.read_consumed, filp->f_flags & O_NONBLOCK);
+			if (!list_empty(&proc->todo))
+				wake_up_interruptible(&proc->wait);
+			if (ret < 0) {
+				if (copy_to_user(ubuf, &bwr, sizeof(bwr)))
+					ret = -EFAULT;
+				goto err;
+			}
+		}
+		if (binder_debug_mask & BINDER_DEBUG_READ_WRITE)
+			printk(KERN_INFO "binder: %d:%d wrote %ld of %ld, read return %ld of %ld\n",
+			       proc->pid, thread->pid, bwr.write_consumed, bwr.write_size, bwr.read_consumed, bwr.read_size);
+		if (copy_to_user(ubuf, &bwr, sizeof(bwr))) {
+			ret = -EFAULT;
+			goto err;
+		}
+		break;
+	}
+	case BINDER_SET_MAX_THREADS:
+		if (copy_from_user(&proc->max_threads, ubuf, sizeof(proc->max_threads))) {
+			ret = -EINVAL;
+			goto err;
+		}
+		break;
+	case BINDER_SET_CONTEXT_MGR:
+		if (binder_context_mgr_node != NULL) {
+			printk(KERN_ERR "binder: BINDER_SET_CONTEXT_MGR already set\n");
+			ret = -EBUSY;
+			goto err;
+		}
+		if (binder_context_mgr_uid != -1) {
+			if (binder_context_mgr_uid != current->cred->euid) {
+				printk(KERN_ERR "binder: BINDER_SET_"
+				       "CONTEXT_MGR bad uid %d != %d\n",
+				       current->cred->euid,
+				       binder_context_mgr_uid);
+				ret = -EPERM;
+				goto err;
+			}
+		} else
+			binder_context_mgr_uid = current->cred->euid;
+		binder_context_mgr_node = binder_new_node(proc, NULL, NULL);
+		if (binder_context_mgr_node == NULL) {
+			ret = -ENOMEM;
+			goto err;
+		}
+		binder_context_mgr_node->local_weak_refs++;
+		binder_context_mgr_node->local_strong_refs++;
+		binder_context_mgr_node->has_strong_ref = 1;
+		binder_context_mgr_node->has_weak_ref = 1;
+		break;
+	case BINDER_THREAD_EXIT:
+		if (binder_debug_mask & BINDER_DEBUG_THREADS)
+			printk(KERN_INFO "binder: %d:%d exit\n",
+			       proc->pid, thread->pid);
+		binder_free_thread(proc, thread);
+		thread = NULL;
+		break;
+	case BINDER_VERSION:
+		if (size != sizeof(struct binder_version)) {
+			ret = -EINVAL;
+			goto err;
+		}
+		if (put_user(BINDER_CURRENT_PROTOCOL_VERSION, &((struct binder_version *)ubuf)->protocol_version)) {
+			ret = -EINVAL;
+			goto err;
+		}
+		break;
+	default:
+		ret = -EINVAL;
+		goto err;
+	}
+	ret = 0;
+err:
+	if (thread)
+		thread->looper &= ~BINDER_LOOPER_STATE_NEED_RETURN;
+	mutex_unlock(&binder_lock);
+	wait_event_interruptible(binder_user_error_wait, binder_stop_on_user_error < 2);
+	if (ret && ret != -ERESTARTSYS)
+		printk(KERN_INFO "binder: %d:%d ioctl %x %lx returned %d\n", proc->pid, current->pid, cmd, arg, ret);
+	return ret;
+}
+
+static void binder_vma_open(struct vm_area_struct *vma)
+{
+	struct binder_proc *proc = vma->vm_private_data;
+	if (binder_debug_mask & BINDER_DEBUG_OPEN_CLOSE)
+		printk(KERN_INFO "binder: %d open vm area %lx-%lx (%ld K) vma %lx pagep %lx\n", proc->pid, vma->vm_start, vma->vm_end, (vma->vm_end - vma->vm_start) / SZ_1K, vma->vm_flags, vma->vm_page_prot.pgprot);
+	dump_stack();
+}
+static void binder_vma_close(struct vm_area_struct *vma)
+{
+	struct binder_proc *proc = vma->vm_private_data;
+	if (binder_debug_mask & BINDER_DEBUG_OPEN_CLOSE)
+		printk(KERN_INFO "binder: %d close vm area %lx-%lx (%ld K) vma %lx pagep %lx\n", proc->pid, vma->vm_start, vma->vm_end, (vma->vm_end - vma->vm_start) / SZ_1K, vma->vm_flags, vma->vm_page_prot.pgprot);
+	proc->vma = NULL;
+}
+
+static struct vm_operations_struct binder_vm_ops = {
+	.open = binder_vma_open,
+	.close = binder_vma_close,
+};
+
+static int binder_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	int ret;
+	struct vm_struct *area;
+	struct binder_proc *proc = filp->private_data;
+	const char *failure_string;
+	struct binder_buffer *buffer;
+
+	if ((vma->vm_end - vma->vm_start) > SZ_4M)
+		vma->vm_end = vma->vm_start + SZ_4M;
+
+	if (binder_debug_mask & BINDER_DEBUG_OPEN_CLOSE)
+		printk(KERN_INFO "binder_mmap: %d %lx-%lx (%ld K) vma %lx pagep %lx\n", proc->pid, vma->vm_start, vma->vm_end, (vma->vm_end - vma->vm_start) / SZ_1K, vma->vm_flags, vma->vm_page_prot.pgprot);
+
+	if (vma->vm_flags & FORBIDDEN_MMAP_FLAGS) {
+		ret = -EPERM;
+		failure_string = "bad vm_flags";
+		goto err_bad_arg;
+	}
+	vma->vm_flags = (vma->vm_flags | VM_DONTCOPY) & ~VM_MAYWRITE;
+
+	area = get_vm_area(vma->vm_end - vma->vm_start, VM_IOREMAP);
+	if (area == NULL) {
+		ret = -ENOMEM;
+		failure_string = "get_vm_area";
+		goto err_get_vm_area_failed;
+	}
+	proc->buffer = area->addr;
+	proc->user_buffer_offset = vma->vm_start - (size_t)proc->buffer;
+
+#ifdef CONFIG_CPU_CACHE_VIPT
+	if (cache_is_vipt_aliasing()) {
+		while (CACHE_COLOUR((vma->vm_start ^ (uint32_t)proc->buffer))) {
+			printk(KERN_INFO "binder_mmap: %d %lx-%lx maps %p bad alignment\n", proc->pid, vma->vm_start, vma->vm_end, proc->buffer);
+			vma->vm_start += PAGE_SIZE;
+		}
+	}
+#endif
+	proc->pages = kzalloc(sizeof(proc->pages[0]) * ((vma->vm_end - vma->vm_start) / PAGE_SIZE), GFP_KERNEL);
+	if (proc->pages == NULL) {
+		ret = -ENOMEM;
+		failure_string = "alloc page array";
+		goto err_alloc_pages_failed;
+	}
+	proc->buffer_size = vma->vm_end - vma->vm_start;
+
+	vma->vm_ops = &binder_vm_ops;
+	vma->vm_private_data = proc;
+
+	if (binder_update_page_range(proc, 1, proc->buffer, proc->buffer + PAGE_SIZE, vma)) {
+		ret = -ENOMEM;
+		failure_string = "alloc small buf";
+		goto err_alloc_small_buf_failed;
+	}
+	buffer = proc->buffer;
+	INIT_LIST_HEAD(&proc->buffers);
+	list_add(&buffer->entry, &proc->buffers);
+	buffer->free = 1;
+	binder_insert_free_buffer(proc, buffer);
+	proc->free_async_space = proc->buffer_size / 2;
+	barrier();
+	proc->vma = vma;
+
+	/*printk(KERN_INFO "binder_mmap: %d %lx-%lx maps %p\n", proc->pid, vma->vm_start, vma->vm_end, proc->buffer);*/
+	return 0;
+
+err_alloc_small_buf_failed:
+	kfree(proc->pages);
+err_alloc_pages_failed:
+	vfree(proc->buffer);
+err_get_vm_area_failed:
+	mutex_unlock(&binder_lock);
+err_bad_arg:
+	printk(KERN_ERR "binder_mmap: %d %lx-%lx %s failed %d\n", proc->pid, vma->vm_start, vma->vm_end, failure_string, ret);
+	return ret;
+}
+
+static int binder_open(struct inode *nodp, struct file *filp)
+{
+	struct binder_proc *proc;
+
+	if (binder_debug_mask & BINDER_DEBUG_OPEN_CLOSE)
+		printk(KERN_INFO "binder_open: %d:%d\n", current->group_leader->pid, current->pid);
+
+	proc = kzalloc(sizeof(*proc), GFP_KERNEL);
+	if (proc == NULL)
+		return -ENOMEM;
+	get_task_struct(current);
+	proc->tsk = current;
+	INIT_LIST_HEAD(&proc->todo);
+	init_waitqueue_head(&proc->wait);
+	proc->default_priority = task_nice(current);
+	mutex_lock(&binder_lock);
+	binder_stats.obj_created[BINDER_STAT_PROC]++;
+	hlist_add_head(&proc->proc_node, &binder_procs);
+	proc->pid = current->group_leader->pid;
+	INIT_LIST_HEAD(&proc->delivered_death);
+	filp->private_data = proc;
+	mutex_unlock(&binder_lock);
+
+	if (binder_proc_dir_entry_proc) {
+		char strbuf[11];
+		snprintf(strbuf, sizeof(strbuf), "%u", proc->pid);
+		create_proc_read_entry(strbuf, S_IRUGO, binder_proc_dir_entry_proc, binder_read_proc_proc, proc);
+	}
+
+	return 0;
+}
+
+static int binder_flush(struct file *filp, fl_owner_t id)
+{
+	struct rb_node *n;
+	struct binder_proc *proc = filp->private_data;
+	int wake_count = 0;
+
+	mutex_lock(&binder_lock);
+	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);
+			wake_count++;
+		}
+	}
+	wake_up_interruptible_all(&proc->wait);
+	mutex_unlock(&binder_lock);
+
+	if (binder_debug_mask & BINDER_DEBUG_OPEN_CLOSE)
+		printk(KERN_INFO "binder_flush: %d woke %d threads\n", proc->pid, wake_count);
+
+	return 0;
+}
+
+static int binder_release(struct inode *nodp, struct file *filp)
+{
+	struct hlist_node *pos;
+	struct binder_transaction *t;
+	struct rb_node *n;
+	struct binder_proc *proc = filp->private_data;
+	int threads, nodes, incoming_refs, outgoing_refs, buffers, active_transactions, page_count;
+
+	if (binder_proc_dir_entry_proc) {
+		char strbuf[11];
+		snprintf(strbuf, sizeof(strbuf), "%u", proc->pid);
+		remove_proc_entry(strbuf, binder_proc_dir_entry_proc);
+	}
+	mutex_lock(&binder_lock);
+	hlist_del(&proc->proc_node);
+	if (binder_context_mgr_node && binder_context_mgr_node->proc == proc) {
+		if (binder_debug_mask & BINDER_DEBUG_DEAD_BINDER)
+			printk(KERN_INFO "binder_release: %d context_mgr_node gone\n", proc->pid);
+		binder_context_mgr_node = NULL;
+	}
+
+	threads = 0;
+	active_transactions = 0;
+	while ((n = rb_first(&proc->threads))) {
+		struct binder_thread *thread = rb_entry(n, struct binder_thread, rb_node);
+		threads++;
+		active_transactions += binder_free_thread(proc, thread);
+	}
+	nodes = 0;
+	incoming_refs = 0;
+	while ((n = rb_first(&proc->nodes))) {
+		struct binder_node *node = rb_entry(n, struct binder_node, rb_node);
+
+		nodes++;
+		rb_erase(&node->rb_node, &proc->nodes);
+		list_del_init(&node->work.entry);
+		if (hlist_empty(&node->refs)) {
+			kfree(node);
+			binder_stats.obj_deleted[BINDER_STAT_NODE]++;
+		} else {
+			struct binder_ref *ref;
+			int death = 0;
+
+			node->proc = NULL;
+			node->local_strong_refs = 0;
+			node->local_weak_refs = 0;
+			hlist_add_head(&node->dead_node, &binder_dead_nodes);
+
+			hlist_for_each_entry(ref, pos, &node->refs, node_entry) {
+				incoming_refs++;
+				if (ref->death) {
+					death++;
+					if (list_empty(&ref->death->work.entry)) {
+						ref->death->work.type = BINDER_WORK_DEAD_BINDER;
+						list_add_tail(&ref->death->work.entry, &ref->proc->todo);
+						wake_up_interruptible(&ref->proc->wait);
+					} else
+						BUG();
+				}
+			}
+			if (binder_debug_mask & BINDER_DEBUG_DEAD_BINDER)
+				printk(KERN_INFO "binder: node %d now dead, refs %d, death %d\n", node->debug_id, incoming_refs, death);
+		}
+	}
+	outgoing_refs = 0;
+	while ((n = rb_first(&proc->refs_by_desc))) {
+		struct binder_ref *ref = rb_entry(n, struct binder_ref, rb_node_desc);
+		outgoing_refs++;
+		binder_delete_ref(ref);
+	}
+	binder_release_work(&proc->todo);
+	buffers = 0;
+
+	while ((n = rb_first(&proc->allocated_buffers))) {
+		struct binder_buffer *buffer = rb_entry(n, struct binder_buffer, rb_node);
+		t = buffer->transaction;
+		if (t) {
+			t->buffer = NULL;
+			buffer->transaction = NULL;
+			printk(KERN_ERR "binder: release proc %d, transaction %d, not freed\n", proc->pid, t->debug_id);
+			/*BUG();*/
+		}
+		binder_free_buf(proc, buffer);
+		buffers++;
+	}
+
+	binder_stats.obj_deleted[BINDER_STAT_PROC]++;
+	mutex_unlock(&binder_lock);
+
+	page_count = 0;
+	if (proc->pages) {
+		int i;
+		for (i = 0; i < proc->buffer_size / PAGE_SIZE; i++) {
+			if (proc->pages[i]) {
+				if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+					printk(KERN_INFO "binder_release: %d: page %d at %p not freed\n", proc->pid, i, proc->buffer + i * PAGE_SIZE);
+				__free_page(proc->pages[i]);
+				page_count++;
+			}
+		}
+		kfree(proc->pages);
+		vfree(proc->buffer);
+	}
+
+	put_task_struct(proc->tsk);
+
+	if (binder_debug_mask & BINDER_DEBUG_OPEN_CLOSE)
+		printk(KERN_INFO "binder_release: %d threads %d, nodes %d (ref %d), refs %d, active transactions %d, buffers %d, pages %d\n",
+		       proc->pid, threads, nodes, incoming_refs, outgoing_refs, active_transactions, buffers, page_count);
+
+	kfree(proc);
+	return 0;
+}
+
+static char *print_binder_transaction(char *buf, char *end, const char *prefix, struct binder_transaction *t)
+{
+	buf += snprintf(buf, end - buf, "%s %d: %p from %d:%d to %d:%d code %x flags %x pri %ld r%d",
+			prefix, t->debug_id, t, t->from ? t->from->proc->pid : 0,
+			t->from ? t->from->pid : 0,
+			t->to_proc ? t->to_proc->pid : 0,
+			t->to_thread ? t->to_thread->pid : 0,
+			t->code, t->flags, t->priority, t->need_reply);
+	if (buf >= end)
+		return buf;
+	if (t->buffer == NULL) {
+		buf += snprintf(buf, end - buf, " buffer free\n");
+		return buf;
+	}
+	if (t->buffer->target_node) {
+		buf += snprintf(buf, end - buf, " node %d",
+				t->buffer->target_node->debug_id);
+		if (buf >= end)
+			return buf;
+	}
+	buf += snprintf(buf, end - buf, " size %zd:%zd data %p\n",
+			t->buffer->data_size, t->buffer->offsets_size,
+			t->buffer->data);
+	return buf;
+}
+
+static char *print_binder_buffer(char *buf, char *end, const char *prefix, struct binder_buffer *buffer)
+{
+	buf += snprintf(buf, end - buf, "%s %d: %p size %zd:%zd %s\n",
+			prefix, buffer->debug_id, buffer->data,
+			buffer->data_size, buffer->offsets_size,
+			buffer->transaction ? "active" : "delivered");
+	return buf;
+}
+
+static char *print_binder_work(char *buf, char *end, const char *prefix,
+	const char *transaction_prefix, struct binder_work *w)
+{
+	struct binder_node *node;
+	struct binder_transaction *t;
+
+	switch (w->type) {
+	case BINDER_WORK_TRANSACTION:
+		t = container_of(w, struct binder_transaction, work);
+		buf = print_binder_transaction(buf, end, transaction_prefix, t);
+		break;
+	case BINDER_WORK_TRANSACTION_COMPLETE:
+		buf += snprintf(buf, end - buf,
+				"%stransaction complete\n", prefix);
+		break;
+	case BINDER_WORK_NODE:
+		node = container_of(w, struct binder_node, work);
+		buf += snprintf(buf, end - buf, "%snode work %d: u%p c%p\n",
+				prefix, node->debug_id, node->ptr, node->cookie);
+		break;
+	case BINDER_WORK_DEAD_BINDER:
+		buf += snprintf(buf, end - buf, "%shas dead binder\n", prefix);
+		break;
+	case BINDER_WORK_DEAD_BINDER_AND_CLEAR:
+		buf += snprintf(buf, end - buf,
+				"%shas cleared dead binder\n", prefix);
+		break;
+	case BINDER_WORK_CLEAR_DEATH_NOTIFICATION:
+		buf += snprintf(buf, end - buf,
+				"%shas cleared death notification\n", prefix);
+		break;
+	default:
+		buf += snprintf(buf, end - buf, "%sunknown work: type %d\n",
+				prefix, w->type);
+		break;
+	}
+	return buf;
+}
+
+static char *print_binder_thread(char *buf, char *end, struct binder_thread *thread, int print_always)
+{
+	struct binder_transaction *t;
+	struct binder_work *w;
+	char *start_buf = buf;
+	char *header_buf;
+
+	buf += snprintf(buf, end - buf, "  thread %d: l %02x\n", thread->pid, thread->looper);
+	header_buf = buf;
+	t = thread->transaction_stack;
+	while (t) {
+		if (buf >= end)
+			break;
+		if (t->from == thread) {
+			buf = print_binder_transaction(buf, end, "    outgoing transaction", t);
+			t = t->from_parent;
+		} else if (t->to_thread == thread) {
+			buf = print_binder_transaction(buf, end, "    incoming transaction", t);
+			t = t->to_parent;
+		} else {
+			buf = print_binder_transaction(buf, end, "    bad transaction", t);
+			t = NULL;
+		}
+	}
+	list_for_each_entry(w, &thread->todo, entry) {
+		if (buf >= end)
+			break;
+		buf = print_binder_work(buf, end, "    ",
+					"    pending transaction", w);
+	}
+	if (!print_always && buf == header_buf)
+		buf = start_buf;
+	return buf;
+}
+
+static char *print_binder_node(char *buf, char *end, struct binder_node *node)
+{
+	struct binder_ref *ref;
+	struct hlist_node *pos;
+	struct binder_work *w;
+	int count;
+	count = 0;
+	hlist_for_each_entry(ref, pos, &node->refs, node_entry)
+		count++;
+
+	buf += snprintf(buf, end - buf, "  node %d: u%p c%p hs %d hw %d ls %d lw %d is %d iw %d",
+			node->debug_id, node->ptr, node->cookie,
+			node->has_strong_ref, node->has_weak_ref,
+			node->local_strong_refs, node->local_weak_refs,
+			node->internal_strong_refs, count);
+	if (buf >= end)
+		return buf;
+	if (count) {
+		buf += snprintf(buf, end - buf, " proc");
+		if (buf >= end)
+			return buf;
+		hlist_for_each_entry(ref, pos, &node->refs, node_entry) {
+			buf += snprintf(buf, end - buf, " %d", ref->proc->pid);
+			if (buf >= end)
+				return buf;
+		}
+	}
+	buf += snprintf(buf, end - buf, "\n");
+	list_for_each_entry(w, &node->async_todo, entry) {
+		if (buf >= end)
+			break;
+		buf = print_binder_work(buf, end, "    ",
+					"    pending async transaction", w);
+	}
+	return buf;
+}
+
+static char *print_binder_ref(char *buf, char *end, struct binder_ref *ref)
+{
+	buf += snprintf(buf, end - buf, "  ref %d: desc %d %snode %d s %d w %d d %p\n",
+			ref->debug_id, ref->desc, ref->node->proc ? "" : "dead ",
+			ref->node->debug_id, ref->strong, ref->weak, ref->death);
+	return buf;
+}
+
+static char *print_binder_proc(char *buf, char *end, struct binder_proc *proc, int print_all)
+{
+	struct binder_work *w;
+	struct rb_node *n;
+	char *start_buf = buf;
+	char *header_buf;
+
+	buf += snprintf(buf, end - buf, "proc %d\n", proc->pid);
+	header_buf = buf;
+
+	for (n = rb_first(&proc->threads); n != NULL && buf < end; n = rb_next(n))
+		buf = print_binder_thread(buf, end, rb_entry(n, struct binder_thread, rb_node), print_all);
+	for (n = rb_first(&proc->nodes); n != NULL && buf < end; n = rb_next(n)) {
+		struct binder_node *node = rb_entry(n, struct binder_node, rb_node);
+		if (print_all || node->has_async_transaction)
+			buf = print_binder_node(buf, end, node);
+	}
+	if (print_all) {
+		for (n = rb_first(&proc->refs_by_desc); n != NULL && buf < end; n = rb_next(n))
+			buf = print_binder_ref(buf, end, rb_entry(n, struct binder_ref, rb_node_desc));
+	}
+	for (n = rb_first(&proc->allocated_buffers); n != NULL && buf < end; n = rb_next(n))
+		buf = print_binder_buffer(buf, end, "  buffer", rb_entry(n, struct binder_buffer, rb_node));
+	list_for_each_entry(w, &proc->todo, entry) {
+		if (buf >= end)
+			break;
+		buf = print_binder_work(buf, end, "  ",
+					"  pending transaction", w);
+	}
+	list_for_each_entry(w, &proc->delivered_death, entry) {
+		if (buf >= end)
+			break;
+		buf += snprintf(buf, end - buf, "  has delivered dead binder\n");
+		break;
+	}
+	if (!print_all && buf == header_buf)
+		buf = start_buf;
+	return buf;
+}
+
+static const char *binder_return_strings[] = {
+	"BR_ERROR",
+	"BR_OK",
+	"BR_TRANSACTION",
+	"BR_REPLY",
+	"BR_ACQUIRE_RESULT",
+	"BR_DEAD_REPLY",
+	"BR_TRANSACTION_COMPLETE",
+	"BR_INCREFS",
+	"BR_ACQUIRE",
+	"BR_RELEASE",
+	"BR_DECREFS",
+	"BR_ATTEMPT_ACQUIRE",
+	"BR_NOOP",
+	"BR_SPAWN_LOOPER",
+	"BR_FINISHED",
+	"BR_DEAD_BINDER",
+	"BR_CLEAR_DEATH_NOTIFICATION_DONE",
+	"BR_FAILED_REPLY"
+};
+
+static const char *binder_command_strings[] = {
+	"BC_TRANSACTION",
+	"BC_REPLY",
+	"BC_ACQUIRE_RESULT",
+	"BC_FREE_BUFFER",
+	"BC_INCREFS",
+	"BC_ACQUIRE",
+	"BC_RELEASE",
+	"BC_DECREFS",
+	"BC_INCREFS_DONE",
+	"BC_ACQUIRE_DONE",
+	"BC_ATTEMPT_ACQUIRE",
+	"BC_REGISTER_LOOPER",
+	"BC_ENTER_LOOPER",
+	"BC_EXIT_LOOPER",
+	"BC_REQUEST_DEATH_NOTIFICATION",
+	"BC_CLEAR_DEATH_NOTIFICATION",
+	"BC_DEAD_BINDER_DONE"
+};
+
+static const char *binder_objstat_strings[] = {
+	"proc",
+	"thread",
+	"node",
+	"ref",
+	"death",
+	"transaction",
+	"transaction_complete"
+};
+
+static char *print_binder_stats(char *buf, char *end, const char *prefix, struct binder_stats *stats)
+{
+	int i;
+
+	BUILD_BUG_ON(ARRAY_SIZE(stats->bc) != ARRAY_SIZE(binder_command_strings));
+	for (i = 0; i < ARRAY_SIZE(stats->bc); i++) {
+		if (stats->bc[i])
+			buf += snprintf(buf, end - buf, "%s%s: %d\n", prefix,
+					binder_command_strings[i], stats->bc[i]);
+		if (buf >= end)
+			return buf;
+	}
+
+	BUILD_BUG_ON(ARRAY_SIZE(stats->br) != ARRAY_SIZE(binder_return_strings));
+	for (i = 0; i < ARRAY_SIZE(stats->br); i++) {
+		if (stats->br[i])
+			buf += snprintf(buf, end - buf, "%s%s: %d\n", prefix,
+					binder_return_strings[i], stats->br[i]);
+		if (buf >= end)
+			return buf;
+	}
+
+	BUILD_BUG_ON(ARRAY_SIZE(stats->obj_created) != ARRAY_SIZE(binder_objstat_strings));
+	BUILD_BUG_ON(ARRAY_SIZE(stats->obj_created) != ARRAY_SIZE(stats->obj_deleted));
+	for (i = 0; i < ARRAY_SIZE(stats->obj_created); i++) {
+		if (stats->obj_created[i] || stats->obj_deleted[i])
+			buf += snprintf(buf, end - buf, "%s%s: active %d total %d\n", prefix,
+					binder_objstat_strings[i],
+					stats->obj_created[i] - stats->obj_deleted[i],
+					stats->obj_created[i]);
+		if (buf >= end)
+			return buf;
+	}
+	return buf;
+}
+
+static char *print_binder_proc_stats(char *buf, char *end, struct binder_proc *proc)
+{
+	struct binder_work *w;
+	struct rb_node *n;
+	int count, strong, weak;
+
+	buf += snprintf(buf, end - buf, "proc %d\n", proc->pid);
+	if (buf >= end)
+		return buf;
+	count = 0;
+	for (n = rb_first(&proc->threads); n != NULL; n = rb_next(n))
+		count++;
+	buf += snprintf(buf, end - buf, "  threads: %d\n", count);
+	if (buf >= end)
+		return buf;
+	buf += snprintf(buf, end - buf, "  requested threads: %d+%d/%d\n"
+			"  ready threads %d\n"
+			"  free async space %zd\n", proc->requested_threads,
+			proc->requested_threads_started, proc->max_threads,
+			proc->ready_threads, proc->free_async_space);
+	if (buf >= end)
+		return buf;
+	count = 0;
+	for (n = rb_first(&proc->nodes); n != NULL; n = rb_next(n))
+		count++;
+	buf += snprintf(buf, end - buf, "  nodes: %d\n", count);
+	if (buf >= end)
+		return buf;
+	count = 0;
+	strong = 0;
+	weak = 0;
+	for (n = rb_first(&proc->refs_by_desc); n != NULL; n = rb_next(n)) {
+		struct binder_ref *ref = rb_entry(n, struct binder_ref, rb_node_desc);
+		count++;
+		strong += ref->strong;
+		weak += ref->weak;
+	}
+	buf += snprintf(buf, end - buf, "  refs: %d s %d w %d\n", count, strong, weak);
+	if (buf >= end)
+		return buf;
+
+	count = 0;
+	for (n = rb_first(&proc->allocated_buffers); n != NULL; n = rb_next(n))
+		count++;
+	buf += snprintf(buf, end - buf, "  buffers: %d\n", count);
+	if (buf >= end)
+		return buf;
+
+	count = 0;
+	list_for_each_entry(w, &proc->todo, entry) {
+		switch (w->type) {
+		case BINDER_WORK_TRANSACTION:
+			count++;
+			break;
+		default:
+			break;
+		}
+	}
+	buf += snprintf(buf, end - buf, "  pending transactions: %d\n", count);
+	if (buf >= end)
+		return buf;
+
+	buf = print_binder_stats(buf, end, "  ", &proc->stats);
+
+	return buf;
+}
+
+
+static int binder_read_proc_state(
+	char *page, char **start, off_t off, int count, int *eof, void *data)
+{
+	struct binder_proc *proc;
+	struct hlist_node *pos;
+	struct binder_node *node;
+	int len = 0;
+	char *buf = page;
+	char *end = page + PAGE_SIZE;
+	int do_lock = !binder_debug_no_lock;
+
+	if (off)
+		return 0;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+
+	buf += snprintf(buf, end - buf, "binder state:\n");
+
+	if (!hlist_empty(&binder_dead_nodes))
+		buf += snprintf(buf, end - buf, "dead nodes:\n");
+	hlist_for_each_entry(node, pos, &binder_dead_nodes, dead_node) {
+		if (buf >= end)
+			break;
+		buf = print_binder_node(buf, end, node);
+	}
+
+	hlist_for_each_entry(proc, pos, &binder_procs, proc_node) {
+		if (buf >= end)
+			break;
+		buf = print_binder_proc(buf, end, proc, 1);
+	}
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+	if (buf > page + PAGE_SIZE)
+		buf = page + PAGE_SIZE;
+
+	*start = page + off;
+
+	len = buf - page;
+	if (len > off)
+		len -= off;
+	else
+		len = 0;
+
+	return len < count ? len  : count;
+}
+
+static int binder_read_proc_stats(
+	char *page, char **start, off_t off, int count, int *eof, void *data)
+{
+	struct binder_proc *proc;
+	struct hlist_node *pos;
+	int len = 0;
+	char *p = page;
+	int do_lock = !binder_debug_no_lock;
+
+	if (off)
+		return 0;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+
+	p += snprintf(p, PAGE_SIZE, "binder stats:\n");
+
+	p = print_binder_stats(p, page + PAGE_SIZE, "", &binder_stats);
+
+	hlist_for_each_entry(proc, pos, &binder_procs, proc_node) {
+		if (p >= page + PAGE_SIZE)
+			break;
+		p = print_binder_proc_stats(p, page + PAGE_SIZE, proc);
+	}
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+	if (p > page + PAGE_SIZE)
+		p = page + PAGE_SIZE;
+
+	*start = page + off;
+
+	len = p - page;
+	if (len > off)
+		len -= off;
+	else
+		len = 0;
+
+	return len < count ? len  : count;
+}
+
+static int binder_read_proc_transactions(
+	char *page, char **start, off_t off, int count, int *eof, void *data)
+{
+	struct binder_proc *proc;
+	struct hlist_node *pos;
+	int len = 0;
+	char *buf = page;
+	char *end = page + PAGE_SIZE;
+	int do_lock = !binder_debug_no_lock;
+
+	if (off)
+		return 0;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+
+	buf += snprintf(buf, end - buf, "binder transactions:\n");
+	hlist_for_each_entry(proc, pos, &binder_procs, proc_node) {
+		if (buf >= end)
+			break;
+		buf = print_binder_proc(buf, end, proc, 0);
+	}
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+	if (buf > page + PAGE_SIZE)
+		buf = page + PAGE_SIZE;
+
+	*start = page + off;
+
+	len = buf - page;
+	if (len > off)
+		len -= off;
+	else
+		len = 0;
+
+	return len < count ? len  : count;
+}
+
+static int binder_read_proc_proc(
+	char *page, char **start, off_t off, int count, int *eof, void *data)
+{
+	struct binder_proc *proc = data;
+	int len = 0;
+	char *p = page;
+	int do_lock = !binder_debug_no_lock;
+
+	if (off)
+		return 0;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+	p += snprintf(p, PAGE_SIZE, "binder proc state:\n");
+	p = print_binder_proc(p, page + PAGE_SIZE, proc, 1);
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+
+	if (p > page + PAGE_SIZE)
+		p = page + PAGE_SIZE;
+	*start = page + off;
+
+	len = p - page;
+	if (len > off)
+		len -= off;
+	else
+		len = 0;
+
+	return len < count ? len  : count;
+}
+
+static char *print_binder_transaction_log_entry(char *buf, char *end, struct binder_transaction_log_entry *e)
+{
+	buf += snprintf(buf, end - buf, "%d: %s from %d:%d to %d:%d node %d handle %d size %d:%d\n",
+			e->debug_id, (e->call_type == 2) ? "reply" :
+			((e->call_type == 1) ? "async" : "call "), e->from_proc,
+			e->from_thread, e->to_proc, e->to_thread, e->to_node,
+			e->target_handle, e->data_size, e->offsets_size);
+	return buf;
+}
+
+static int binder_read_proc_transaction_log(
+	char *page, char **start, off_t off, int count, int *eof, void *data)
+{
+	struct binder_transaction_log *log = data;
+	int len = 0;
+	int i;
+	char *buf = page;
+	char *end = page + PAGE_SIZE;
+
+	if (off)
+		return 0;
+
+	if (log->full) {
+		for (i = log->next; i < ARRAY_SIZE(log->entry); i++) {
+			if (buf >= end)
+				break;
+			buf = print_binder_transaction_log_entry(buf, end, &log->entry[i]);
+		}
+	}
+	for (i = 0; i < log->next; i++) {
+		if (buf >= end)
+			break;
+		buf = print_binder_transaction_log_entry(buf, end, &log->entry[i]);
+	}
+
+	*start = page + off;
+
+	len = buf - page;
+	if (len > off)
+		len -= off;
+	else
+		len = 0;
+
+	return len < count ? len  : count;
+}
+
+static struct file_operations binder_fops = {
+	.owner = THIS_MODULE,
+	.poll = binder_poll,
+	.unlocked_ioctl = binder_ioctl,
+	.mmap = binder_mmap,
+	.open = binder_open,
+	.flush = binder_flush,
+	.release = binder_release,
+};
+
+static struct miscdevice binder_miscdev = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "binder",
+	.fops = &binder_fops
+};
+
+static int __init binder_init(void)
+{
+	int ret;
+
+	binder_proc_dir_entry_root = proc_mkdir("binder", NULL);
+	if (binder_proc_dir_entry_root)
+		binder_proc_dir_entry_proc = proc_mkdir("proc", binder_proc_dir_entry_root);
+	ret = misc_register(&binder_miscdev);
+	if (binder_proc_dir_entry_root) {
+		create_proc_read_entry("state", S_IRUGO, binder_proc_dir_entry_root, binder_read_proc_state, NULL);
+		create_proc_read_entry("stats", S_IRUGO, binder_proc_dir_entry_root, binder_read_proc_stats, NULL);
+		create_proc_read_entry("transactions", S_IRUGO, binder_proc_dir_entry_root, binder_read_proc_transactions, NULL);
+		create_proc_read_entry("transaction_log", S_IRUGO, binder_proc_dir_entry_root, binder_read_proc_transaction_log, &binder_transaction_log);
+		create_proc_read_entry("failed_transaction_log", S_IRUGO, binder_proc_dir_entry_root, binder_read_proc_transaction_log, &binder_transaction_log_failed);
+	}
+	return ret;
+}
+
+device_initcall(binder_init);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/android/binder.h b/drivers/staging/android/binder.h
new file mode 100644
index 000000000000..863ae1ad5d55
--- /dev/null
+++ b/drivers/staging/android/binder.h
@@ -0,0 +1,330 @@
+/*
+ * Copyright (C) 2008 Google, Inc.
+ *
+ * Based on, but no longer compatible with, the original
+ * OpenBinder.org binder driver interface, which is:
+ *
+ * Copyright (c) 2005 Palmsource, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _LINUX_BINDER_H
+#define _LINUX_BINDER_H
+
+#include <linux/ioctl.h>
+
+#define B_PACK_CHARS(c1, c2, c3, c4) \
+	((((c1)<<24)) | (((c2)<<16)) | (((c3)<<8)) | (c4))
+#define B_TYPE_LARGE 0x85
+
+enum {
+	BINDER_TYPE_BINDER	= B_PACK_CHARS('s', 'b', '*', B_TYPE_LARGE),
+	BINDER_TYPE_WEAK_BINDER	= B_PACK_CHARS('w', 'b', '*', B_TYPE_LARGE),
+	BINDER_TYPE_HANDLE	= B_PACK_CHARS('s', 'h', '*', B_TYPE_LARGE),
+	BINDER_TYPE_WEAK_HANDLE	= B_PACK_CHARS('w', 'h', '*', B_TYPE_LARGE),
+	BINDER_TYPE_FD		= B_PACK_CHARS('f', 'd', '*', B_TYPE_LARGE),
+};
+
+enum {
+	FLAT_BINDER_FLAG_PRIORITY_MASK = 0xff,
+	FLAT_BINDER_FLAG_ACCEPTS_FDS = 0x100,
+};
+
+/*
+ * This is the flattened representation of a Binder object for transfer
+ * between processes.  The 'offsets' supplied as part of a binder transaction
+ * contains offsets into the data where these structures occur.  The Binder
+ * driver takes care of re-writing the structure type and data as it moves
+ * between processes.
+ */
+struct flat_binder_object {
+	/* 8 bytes for large_flat_header. */
+	unsigned long		type;
+	unsigned long		flags;
+
+	/* 8 bytes of data. */
+	union {
+		void		*binder;	/* local object */
+		signed long	handle;		/* remote object */
+	};
+
+	/* extra data associated with local object */
+	void			*cookie;
+};
+
+/*
+ * On 64-bit platforms where user code may run in 32-bits the driver must
+ * translate the buffer (and local binder) addresses apropriately.
+ */
+
+struct binder_write_read {
+	signed long	write_size;	/* bytes to write */
+	signed long	write_consumed;	/* bytes consumed by driver */
+	unsigned long	write_buffer;
+	signed long	read_size;	/* bytes to read */
+	signed long	read_consumed;	/* bytes consumed by driver */
+	unsigned long	read_buffer;
+};
+
+/* Use with BINDER_VERSION, driver fills in fields. */
+struct binder_version {
+	/* driver protocol version -- increment with incompatible change */
+	signed long	protocol_version;
+};
+
+/* This is the current protocol version. */
+#define BINDER_CURRENT_PROTOCOL_VERSION 7
+
+#define BINDER_WRITE_READ   		_IOWR('b', 1, struct binder_write_read)
+#define	BINDER_SET_IDLE_TIMEOUT		_IOW('b', 3, int64_t)
+#define	BINDER_SET_MAX_THREADS		_IOW('b', 5, size_t)
+#define	BINDER_SET_IDLE_PRIORITY	_IOW('b', 6, int)
+#define	BINDER_SET_CONTEXT_MGR		_IOW('b', 7, int)
+#define	BINDER_THREAD_EXIT		_IOW('b', 8, int)
+#define BINDER_VERSION			_IOWR('b', 9, struct binder_version)
+
+/*
+ * NOTE: Two special error codes you should check for when calling
+ * in to the driver are:
+ *
+ * EINTR -- The operation has been interupted.  This should be
+ * handled by retrying the ioctl() until a different error code
+ * is returned.
+ *
+ * ECONNREFUSED -- The driver is no longer accepting operations
+ * from your process.  That is, the process is being destroyed.
+ * You should handle this by exiting from your process.  Note
+ * that once this error code is returned, all further calls to
+ * the driver from any thread will return this same code.
+ */
+
+enum transaction_flags {
+	TF_ONE_WAY	= 0x01,	/* this is a one-way call: async, no return */
+	TF_ROOT_OBJECT	= 0x04,	/* contents are the component's root object */
+	TF_STATUS_CODE	= 0x08,	/* contents are a 32-bit status code */
+	TF_ACCEPT_FDS	= 0x10,	/* allow replies with file descriptors */
+};
+
+struct binder_transaction_data {
+	/* The first two are only used for bcTRANSACTION and brTRANSACTION,
+	 * identifying the target and contents of the transaction.
+	 */
+	union {
+		size_t	handle;	/* target descriptor of command transaction */
+		void	*ptr;	/* target descriptor of return transaction */
+	} target;
+	void		*cookie;	/* target object cookie */
+	unsigned int	code;		/* transaction command */
+
+	/* General information about the transaction. */
+	unsigned int	flags;
+	pid_t		sender_pid;
+	uid_t		sender_euid;
+	size_t		data_size;	/* number of bytes of data */
+	size_t		offsets_size;	/* number of bytes of offsets */
+
+	/* If this transaction is inline, the data immediately
+	 * follows here; otherwise, it ends with a pointer to
+	 * the data buffer.
+	 */
+	union {
+		struct {
+			/* transaction data */
+			const void	*buffer;
+			/* offsets from buffer to flat_binder_object structs */
+			const void	*offsets;
+		} ptr;
+		uint8_t	buf[8];
+	} data;
+};
+
+struct binder_ptr_cookie {
+	void *ptr;
+	void *cookie;
+};
+
+struct binder_pri_desc {
+	int priority;
+	int desc;
+};
+
+struct binder_pri_ptr_cookie {
+	int priority;
+	void *ptr;
+	void *cookie;
+};
+
+enum BinderDriverReturnProtocol {
+	BR_ERROR = _IOR('r', 0, int),
+	/*
+	 * int: error code
+	 */
+
+	BR_OK = _IO('r', 1),
+	/* No parameters! */
+
+	BR_TRANSACTION = _IOR('r', 2, struct binder_transaction_data),
+	BR_REPLY = _IOR('r', 3, struct binder_transaction_data),
+	/*
+	 * binder_transaction_data: the received command.
+	 */
+
+	BR_ACQUIRE_RESULT = _IOR('r', 4, int),
+	/*
+	 * not currently supported
+	 * int: 0 if the last bcATTEMPT_ACQUIRE was not successful.
+	 * Else the remote object has acquired a primary reference.
+	 */
+
+	BR_DEAD_REPLY = _IO('r', 5),
+	/*
+	 * The target of the last transaction (either a bcTRANSACTION or
+	 * a bcATTEMPT_ACQUIRE) is no longer with us.  No parameters.
+	 */
+
+	BR_TRANSACTION_COMPLETE = _IO('r', 6),
+	/*
+	 * No parameters... always refers to the last transaction requested
+	 * (including replies).  Note that this will be sent even for
+	 * asynchronous transactions.
+	 */
+
+	BR_INCREFS = _IOR('r', 7, struct binder_ptr_cookie),
+	BR_ACQUIRE = _IOR('r', 8, struct binder_ptr_cookie),
+	BR_RELEASE = _IOR('r', 9, struct binder_ptr_cookie),
+	BR_DECREFS = _IOR('r', 10, struct binder_ptr_cookie),
+	/*
+	 * void *:	ptr to binder
+	 * void *: cookie for binder
+	 */
+
+	BR_ATTEMPT_ACQUIRE = _IOR('r', 11, struct binder_pri_ptr_cookie),
+	/*
+	 * not currently supported
+	 * int:	priority
+	 * void *: ptr to binder
+	 * void *: cookie for binder
+	 */
+
+	BR_NOOP = _IO('r', 12),
+	/*
+	 * No parameters.  Do nothing and examine the next command.  It exists
+	 * primarily so that we can replace it with a BR_SPAWN_LOOPER command.
+	 */
+
+	BR_SPAWN_LOOPER = _IO('r', 13),
+	/*
+	 * No parameters.  The driver has determined that a process has no
+	 * threads waiting to service incomming transactions.  When a process
+	 * receives this command, it must spawn a new service thread and
+	 * register it via bcENTER_LOOPER.
+	 */
+
+	BR_FINISHED = _IO('r', 14),
+	/*
+	 * not currently supported
+	 * stop threadpool thread
+	 */
+
+	BR_DEAD_BINDER = _IOR('r', 15, void *),
+	/*
+	 * void *: cookie
+	 */
+	BR_CLEAR_DEATH_NOTIFICATION_DONE = _IOR('r', 16, void *),
+	/*
+	 * void *: cookie
+	 */
+
+	BR_FAILED_REPLY = _IO('r', 17),
+	/*
+	 * The the last transaction (either a bcTRANSACTION or
+	 * a bcATTEMPT_ACQUIRE) failed (e.g. out of memory).  No parameters.
+	 */
+};
+
+enum BinderDriverCommandProtocol {
+	BC_TRANSACTION = _IOW('c', 0, struct binder_transaction_data),
+	BC_REPLY = _IOW('c', 1, struct binder_transaction_data),
+	/*
+	 * binder_transaction_data: the sent command.
+	 */
+
+	BC_ACQUIRE_RESULT = _IOW('c', 2, int),
+	/*
+	 * not currently supported
+	 * int:  0 if the last BR_ATTEMPT_ACQUIRE was not successful.
+	 * Else you have acquired a primary reference on the object.
+	 */
+
+	BC_FREE_BUFFER = _IOW('c', 3, int),
+	/*
+	 * void *: ptr to transaction data received on a read
+	 */
+
+	BC_INCREFS = _IOW('c', 4, int),
+	BC_ACQUIRE = _IOW('c', 5, int),
+	BC_RELEASE = _IOW('c', 6, int),
+	BC_DECREFS = _IOW('c', 7, int),
+	/*
+	 * int:	descriptor
+	 */
+
+	BC_INCREFS_DONE = _IOW('c', 8, struct binder_ptr_cookie),
+	BC_ACQUIRE_DONE = _IOW('c', 9, struct binder_ptr_cookie),
+	/*
+	 * void *: ptr to binder
+	 * void *: cookie for binder
+	 */
+
+	BC_ATTEMPT_ACQUIRE = _IOW('c', 10, struct binder_pri_desc),
+	/*
+	 * not currently supported
+	 * int: priority
+	 * int: descriptor
+	 */
+
+	BC_REGISTER_LOOPER = _IO('c', 11),
+	/*
+	 * No parameters.
+	 * Register a spawned looper thread with the device.
+	 */
+
+	BC_ENTER_LOOPER = _IO('c', 12),
+	BC_EXIT_LOOPER = _IO('c', 13),
+	/*
+	 * No parameters.
+	 * These two commands are sent as an application-level thread
+	 * enters and exits the binder loop, respectively.  They are
+	 * used so the binder can have an accurate count of the number
+	 * of looping threads it has available.
+	 */
+
+	BC_REQUEST_DEATH_NOTIFICATION = _IOW('c', 14, struct binder_ptr_cookie),
+	/*
+	 * void *: ptr to binder
+	 * void *: cookie
+	 */
+
+	BC_CLEAR_DEATH_NOTIFICATION = _IOW('c', 15, struct binder_ptr_cookie),
+	/*
+	 * void *: ptr to binder
+	 * void *: cookie
+	 */
+
+	BC_DEAD_BINDER_DONE = _IOW('c', 16, void *),
+	/*
+	 * void *: cookie
+	 */
+};
+
+#endif /* _LINUX_BINDER_H */
+
diff --git a/drivers/staging/android/logger.c b/drivers/staging/android/logger.c
new file mode 100644
index 000000000000..ab32003ecd0b
--- /dev/null
+++ b/drivers/staging/android/logger.c
@@ -0,0 +1,607 @@
+/*
+ * drivers/misc/logger.c
+ *
+ * A Logging Subsystem
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * Robert Love <rlove@google.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/poll.h>
+#include <linux/time.h>
+#include "logger.h"
+
+#include <asm/ioctls.h>
+
+/*
+ * struct logger_log - represents a specific log, such as 'main' or 'radio'
+ *
+ * This structure lives from module insertion until module removal, so it does
+ * not need additional reference counting. The structure is protected by the
+ * mutex 'mutex'.
+ */
+struct logger_log {
+	unsigned char *		buffer;	/* the ring buffer itself */
+	struct miscdevice	misc;	/* misc device representing the log */
+	wait_queue_head_t	wq;	/* wait queue for readers */
+	struct list_head	readers; /* this log's readers */
+	struct mutex		mutex;	/* mutex protecting buffer */
+	size_t			w_off;	/* current write head offset */
+	size_t			head;	/* new readers start here */
+	size_t			size;	/* size of the log */
+};
+
+/*
+ * struct logger_reader - a logging device open for reading
+ *
+ * This object lives from open to release, so we don't need additional
+ * reference counting. The structure is protected by log->mutex.
+ */
+struct logger_reader {
+	struct logger_log *	log;	/* associated log */
+	struct list_head	list;	/* entry in logger_log's list */
+	size_t			r_off;	/* current read head offset */
+};
+
+/* logger_offset - returns index 'n' into the log via (optimized) modulus */
+#define logger_offset(n)	((n) & (log->size - 1))
+
+/*
+ * file_get_log - Given a file structure, return the associated log
+ *
+ * This isn't aesthetic. We have several goals:
+ *
+ * 	1) Need to quickly obtain the associated log during an I/O operation
+ * 	2) Readers need to maintain state (logger_reader)
+ * 	3) Writers need to be very fast (open() should be a near no-op)
+ *
+ * In the reader case, we can trivially go file->logger_reader->logger_log.
+ * For a writer, we don't want to maintain a logger_reader, so we just go
+ * file->logger_log. Thus what file->private_data points at depends on whether
+ * or not the file was opened for reading. This function hides that dirtiness.
+ */
+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;
+}
+
+/*
+ * get_entry_len - Grabs the length of the payload of the next entry starting
+ * from 'off'.
+ *
+ * Caller needs to hold log->mutex.
+ */
+static __u32 get_entry_len(struct logger_log *log, size_t off)
+{
+	__u16 val;
+
+	switch (log->size - off) {
+	case 1:
+		memcpy(&val, log->buffer + off, 1);
+		memcpy(((char *) &val) + 1, log->buffer, 1);
+		break;
+	default:
+		memcpy(&val, log->buffer + off, 2);
+	}
+
+	return sizeof(struct logger_entry) + val;
+}
+
+/*
+ * do_read_log_to_user - reads exactly 'count' bytes from 'log' into the
+ * user-space buffer 'buf'. Returns 'count' on success.
+ *
+ * Caller must hold log->mutex.
+ */
+static ssize_t do_read_log_to_user(struct logger_log *log,
+				   struct logger_reader *reader,
+				   char __user *buf,
+				   size_t count)
+{
+	size_t len;
+
+	/*
+	 * We read from the log in two disjoint operations. First, we read from
+	 * the current read head offset up to 'count' bytes or to the end of
+	 * the log, whichever comes first.
+	 */
+	len = min(count, log->size - reader->r_off);
+	if (copy_to_user(buf, log->buffer + reader->r_off, len))
+		return -EFAULT;
+
+	/*
+	 * Second, we read any remaining bytes, starting back at the head of
+	 * the log.
+	 */
+	if (count != len)
+		if (copy_to_user(buf + len, log->buffer, count - len))
+			return -EFAULT;
+
+	reader->r_off = logger_offset(reader->r_off + count);
+
+	return count;
+}
+
+/*
+ * logger_read - our log's read() method
+ *
+ * Behavior:
+ *
+ * 	- O_NONBLOCK works
+ * 	- If there are no log entries to read, blocks until log is written to
+ * 	- Atomically reads exactly one log entry
+ *
+ * Optimal read size is LOGGER_ENTRY_MAX_LEN. Will set errno to EINVAL if read
+ * buffer is insufficient to hold next entry.
+ */
+static ssize_t logger_read(struct file *file, char __user *buf,
+			   size_t count, loff_t *pos)
+{
+	struct logger_reader *reader = file->private_data;
+	struct logger_log *log = reader->log;
+	ssize_t ret;
+	DEFINE_WAIT(wait);
+
+start:
+	while (1) {
+		prepare_to_wait(&log->wq, &wait, TASK_INTERRUPTIBLE);
+
+		mutex_lock(&log->mutex);
+		ret = (log->w_off == reader->r_off);
+		mutex_unlock(&log->mutex);
+		if (!ret)
+			break;
+
+		if (file->f_flags & O_NONBLOCK) {
+			ret = -EAGAIN;
+			break;
+		}
+
+		if (signal_pending(current)) {
+			ret = -EINTR;
+			break;
+		}
+
+		schedule();
+	}
+
+	finish_wait(&log->wq, &wait);
+	if (ret)
+		return ret;
+
+	mutex_lock(&log->mutex);
+
+	/* is there still something to read or did we race? */
+	if (unlikely(log->w_off == reader->r_off)) {
+		mutex_unlock(&log->mutex);
+		goto start;
+	}
+
+	/* get the size of the next entry */
+	ret = get_entry_len(log, reader->r_off);
+	if (count < ret) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* get exactly one entry from the log */
+	ret = do_read_log_to_user(log, reader, buf, ret);
+
+out:
+	mutex_unlock(&log->mutex);
+
+	return ret;
+}
+
+/*
+ * get_next_entry - return the offset of the first valid entry at least 'len'
+ * bytes after 'off'.
+ *
+ * Caller must hold log->mutex.
+ */
+static size_t get_next_entry(struct logger_log *log, size_t off, size_t len)
+{
+	size_t count = 0;
+
+	do {
+		size_t nr = get_entry_len(log, off);
+		off = logger_offset(off + nr);
+		count += nr;
+	} while (count < len);
+
+	return off;
+}
+
+/*
+ * clock_interval - is a < c < b in mod-space? Put another way, does the line
+ * from a to b cross c?
+ */
+static inline int clock_interval(size_t a, size_t b, size_t c)
+{
+	if (b < a) {
+		if (a < c || b >= c)
+			return 1;
+	} else {
+		if (a < c && b >= c)
+			return 1;
+	}
+
+	return 0;
+}
+
+/*
+ * fix_up_readers - walk the list of all readers and "fix up" any who were
+ * lapped by the writer; also do the same for the default "start head".
+ * We do this by "pulling forward" the readers and start head to the first
+ * entry after the new write head.
+ *
+ * The caller needs to hold log->mutex.
+ */
+static void fix_up_readers(struct logger_log *log, size_t len)
+{
+	size_t old = log->w_off;
+	size_t new = logger_offset(old + len);
+	struct logger_reader *reader;
+
+	if (clock_interval(old, new, log->head))
+		log->head = get_next_entry(log, log->head, len);
+
+	list_for_each_entry(reader, &log->readers, list)
+		if (clock_interval(old, new, reader->r_off))
+			reader->r_off = get_next_entry(log, reader->r_off, len);
+}
+
+/*
+ * do_write_log - writes 'len' bytes from 'buf' to 'log'
+ *
+ * The caller needs to hold log->mutex.
+ */
+static void do_write_log(struct logger_log *log, const void *buf, size_t count)
+{
+	size_t len;
+
+	len = min(count, log->size - log->w_off);
+	memcpy(log->buffer + log->w_off, buf, len);
+
+	if (count != len)
+		memcpy(log->buffer, buf + len, count - len);
+
+	log->w_off = logger_offset(log->w_off + count);
+
+}
+
+/*
+ * do_write_log_user - writes 'len' bytes from the user-space buffer 'buf' to
+ * the log 'log'
+ *
+ * The caller needs to hold log->mutex.
+ *
+ * Returns 'count' on success, negative error code on failure.
+ */
+static ssize_t do_write_log_from_user(struct logger_log *log,
+				      const void __user *buf, size_t count)
+{
+	size_t len;
+
+	len = min(count, log->size - log->w_off);
+	if (len && copy_from_user(log->buffer + log->w_off, buf, len))
+		return -EFAULT;
+
+	if (count != len)
+		if (copy_from_user(log->buffer, buf + len, count - len))
+			return -EFAULT;
+
+	log->w_off = logger_offset(log->w_off + count);
+
+	return count;
+}
+
+/*
+ * logger_aio_write - our write method, implementing support for write(),
+ * writev(), and aio_write(). Writes are our fast path, and we try to optimize
+ * them above all else.
+ */
+ssize_t logger_aio_write(struct kiocb *iocb, const struct iovec *iov,
+			 unsigned long nr_segs, loff_t ppos)
+{
+	struct logger_log *log = file_get_log(iocb->ki_filp);
+	size_t orig = log->w_off;
+	struct logger_entry header;
+	struct timespec now;
+	ssize_t ret = 0;
+
+	now = current_kernel_time();
+
+	header.pid = current->tgid;
+	header.tid = current->pid;
+	header.sec = now.tv_sec;
+	header.nsec = now.tv_nsec;
+	header.len = min_t(size_t, iocb->ki_left, LOGGER_ENTRY_MAX_PAYLOAD);
+
+	/* null writes succeed, return zero */
+	if (unlikely(!header.len))
+		return 0;
+
+	mutex_lock(&log->mutex);
+
+	/*
+	 * Fix up any readers, pulling them forward to the first readable
+	 * entry after (what will be) the new write offset. We do this now
+	 * because if we partially fail, we can end up with clobbered log
+	 * entries that encroach on readable buffer.
+	 */
+	fix_up_readers(log, sizeof(struct logger_entry) + header.len);
+
+	do_write_log(log, &header, sizeof(struct logger_entry));
+
+	while (nr_segs-- > 0) {
+		size_t len;
+		ssize_t nr;
+
+		/* figure out how much of this vector we can keep */
+		len = min_t(size_t, iov->iov_len, header.len - ret);
+
+		/* write out this segment's payload */
+		nr = do_write_log_from_user(log, iov->iov_base, len);
+		if (unlikely(nr < 0)) {
+			log->w_off = orig;
+			mutex_unlock(&log->mutex);
+			return nr;
+		}
+
+		iov++;
+		ret += nr;
+	}
+
+	mutex_unlock(&log->mutex);
+
+	/* wake up any blocked readers */
+	wake_up_interruptible(&log->wq);
+
+	return ret;
+}
+
+static struct logger_log * get_log_from_minor(int);
+
+/*
+ * logger_open - the log's open() file operation
+ *
+ * Note how near a no-op this is in the write-only case. Keep it that way!
+ */
+static int logger_open(struct inode *inode, struct file *file)
+{
+	struct logger_log *log;
+	int ret;
+
+	ret = nonseekable_open(inode, file);
+	if (ret)
+		return ret;
+
+	log = get_log_from_minor(MINOR(inode->i_rdev));
+	if (!log)
+		return -ENODEV;
+
+	if (file->f_mode & FMODE_READ) {
+		struct logger_reader *reader;
+
+		reader = kmalloc(sizeof(struct logger_reader), GFP_KERNEL);
+		if (!reader)
+			return -ENOMEM;
+
+		reader->log = log;
+		INIT_LIST_HEAD(&reader->list);
+
+		mutex_lock(&log->mutex);
+		reader->r_off = log->head;
+		list_add_tail(&reader->list, &log->readers);
+		mutex_unlock(&log->mutex);
+
+		file->private_data = reader;
+	} else
+		file->private_data = log;
+
+	return 0;
+}
+
+/*
+ * logger_release - the log's release file operation
+ *
+ * Note this is a total no-op in the write-only case. Keep it that way!
+ */
+static int logger_release(struct inode *ignored, struct file *file)
+{
+	if (file->f_mode & FMODE_READ) {
+		struct logger_reader *reader = file->private_data;
+		list_del(&reader->list);
+		kfree(reader);
+	}
+
+	return 0;
+}
+
+/*
+ * logger_poll - the log's poll file operation, for poll/select/epoll
+ *
+ * Note we always return POLLOUT, because you can always write() to the log.
+ * Note also that, strictly speaking, a return value of POLLIN does not
+ * guarantee that the log is readable without blocking, as there is a small
+ * chance that the writer can lap the reader in the interim between poll()
+ * returning and the read() request.
+ */
+static unsigned int logger_poll(struct file *file, poll_table *wait)
+{
+	struct logger_reader *reader;
+	struct logger_log *log;
+	unsigned int ret = POLLOUT | POLLWRNORM;
+
+	if (!(file->f_mode & FMODE_READ))
+		return ret;
+
+	reader = file->private_data;
+	log = reader->log;
+
+	poll_wait(file, &log->wq, wait);
+
+	mutex_lock(&log->mutex);
+	if (log->w_off != reader->r_off)
+		ret |= POLLIN | POLLRDNORM;
+	mutex_unlock(&log->mutex);
+
+	return ret;
+}
+
+static long logger_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct logger_log *log = file_get_log(file);
+	struct logger_reader *reader;
+	long ret = -ENOTTY;
+
+	mutex_lock(&log->mutex);
+
+	switch (cmd) {
+	case LOGGER_GET_LOG_BUF_SIZE:
+		ret = log->size;
+		break;
+	case LOGGER_GET_LOG_LEN:
+		if (!(file->f_mode & FMODE_READ)) {
+			ret = -EBADF;
+			break;
+		}
+		reader = file->private_data;
+		if (log->w_off >= reader->r_off)
+			ret = log->w_off - reader->r_off;
+		else
+			ret = (log->size - reader->r_off) + log->w_off;
+		break;
+	case LOGGER_GET_NEXT_ENTRY_LEN:
+		if (!(file->f_mode & FMODE_READ)) {
+			ret = -EBADF;
+			break;
+		}
+		reader = file->private_data;
+		if (log->w_off != reader->r_off)
+			ret = get_entry_len(log, reader->r_off);
+		else
+			ret = 0;
+		break;
+	case LOGGER_FLUSH_LOG:
+		if (!(file->f_mode & FMODE_WRITE)) {
+			ret = -EBADF;
+			break;
+		}
+		list_for_each_entry(reader, &log->readers, list)
+			reader->r_off = log->w_off;
+		log->head = log->w_off;
+		ret = 0;
+		break;
+	}
+
+	mutex_unlock(&log->mutex);
+
+	return ret;
+}
+
+static struct file_operations logger_fops = {
+	.owner = THIS_MODULE,
+	.read = logger_read,
+	.aio_write = logger_aio_write,
+	.poll = logger_poll,
+	.unlocked_ioctl = logger_ioctl,
+	.compat_ioctl = logger_ioctl,
+	.open = logger_open,
+	.release = logger_release,
+};
+
+/*
+ * Defines a log structure with name 'NAME' and a size of 'SIZE' bytes, which
+ * must be a power of two, greater than LOGGER_ENTRY_MAX_LEN, and less than
+ * LONG_MAX minus LOGGER_ENTRY_MAX_LEN.
+ */
+#define DEFINE_LOGGER_DEVICE(VAR, NAME, SIZE) \
+static unsigned char _buf_ ## VAR[SIZE]; \
+static struct logger_log VAR = { \
+	.buffer = _buf_ ## VAR, \
+	.misc = { \
+		.minor = MISC_DYNAMIC_MINOR, \
+		.name = NAME, \
+		.fops = &logger_fops, \
+		.parent = NULL, \
+	}, \
+	.wq = __WAIT_QUEUE_HEAD_INITIALIZER(VAR .wq), \
+	.readers = LIST_HEAD_INIT(VAR .readers), \
+	.mutex = __MUTEX_INITIALIZER(VAR .mutex), \
+	.w_off = 0, \
+	.head = 0, \
+	.size = SIZE, \
+};
+
+DEFINE_LOGGER_DEVICE(log_main, LOGGER_LOG_MAIN, 64*1024)
+DEFINE_LOGGER_DEVICE(log_events, LOGGER_LOG_EVENTS, 256*1024)
+DEFINE_LOGGER_DEVICE(log_radio, LOGGER_LOG_RADIO, 64*1024)
+
+static struct logger_log * get_log_from_minor(int minor)
+{
+	if (log_main.misc.minor == minor)
+		return &log_main;
+	if (log_events.misc.minor == minor)
+		return &log_events;
+	if (log_radio.misc.minor == minor)
+		return &log_radio;
+	return NULL;
+}
+
+static int __init init_log(struct logger_log *log)
+{
+	int ret;
+
+	ret = misc_register(&log->misc);
+	if (unlikely(ret)) {
+		printk(KERN_ERR "logger: failed to register misc "
+		       "device for log '%s'!\n", log->misc.name);
+		return ret;
+	}
+
+	printk(KERN_INFO "logger: created %luK log '%s'\n",
+	       (unsigned long) log->size >> 10, log->misc.name);
+
+	return 0;
+}
+
+static int __init logger_init(void)
+{
+	int ret;
+
+	ret = init_log(&log_main);
+	if (unlikely(ret))
+		goto out;
+
+	ret = init_log(&log_events);
+	if (unlikely(ret))
+		goto out;
+
+	ret = init_log(&log_radio);
+	if (unlikely(ret))
+		goto out;
+
+out:
+	return ret;
+}
+device_initcall(logger_init);
diff --git a/drivers/staging/android/logger.h b/drivers/staging/android/logger.h
new file mode 100644
index 000000000000..a562434d7419
--- /dev/null
+++ b/drivers/staging/android/logger.h
@@ -0,0 +1,48 @@
+/* include/linux/logger.h
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ * Author: Robert Love <rlove@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _LINUX_LOGGER_H
+#define _LINUX_LOGGER_H
+
+#include <linux/types.h>
+#include <linux/ioctl.h>
+
+struct logger_entry {
+	__u16		len;	/* length of the payload */
+	__u16		__pad;	/* no matter what, we get 2 bytes of padding */
+	__s32		pid;	/* generating process's pid */
+	__s32		tid;	/* generating process's tid */
+	__s32		sec;	/* seconds since Epoch */
+	__s32		nsec;	/* nanoseconds */
+	char		msg[0];	/* the entry's payload */
+};
+
+#define LOGGER_LOG_RADIO	"log_radio"	/* radio-related messages */
+#define LOGGER_LOG_EVENTS	"log_events"	/* system/hardware events */
+#define LOGGER_LOG_MAIN		"log_main"	/* everything else */
+
+#define LOGGER_ENTRY_MAX_LEN		(4*1024)
+#define LOGGER_ENTRY_MAX_PAYLOAD	\
+	(LOGGER_ENTRY_MAX_LEN - sizeof(struct logger_entry))
+
+#define __LOGGERIO	0xAE
+
+#define LOGGER_GET_LOG_BUF_SIZE		_IO(__LOGGERIO, 1) /* size of log */
+#define LOGGER_GET_LOG_LEN		_IO(__LOGGERIO, 2) /* used log len */
+#define LOGGER_GET_NEXT_ENTRY_LEN	_IO(__LOGGERIO, 3) /* next entry len */
+#define LOGGER_FLUSH_LOG		_IO(__LOGGERIO, 4) /* flush log */
+
+#endif /* _LINUX_LOGGER_H */
diff --git a/drivers/staging/android/lowmemorykiller.c b/drivers/staging/android/lowmemorykiller.c
new file mode 100644
index 000000000000..3715d56ca96b
--- /dev/null
+++ b/drivers/staging/android/lowmemorykiller.c
@@ -0,0 +1,119 @@
+/* drivers/misc/lowmemorykiller.c
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/oom.h>
+#include <linux/sched.h>
+
+static int lowmem_shrink(int nr_to_scan, gfp_t gfp_mask);
+
+static struct shrinker lowmem_shrinker = {
+	.shrink = lowmem_shrink,
+	.seeks = DEFAULT_SEEKS * 16
+};
+static uint32_t lowmem_debug_level = 2;
+static int lowmem_adj[6] = {
+	0,
+	1,
+	6,
+	12,
+};
+static int lowmem_adj_size = 4;
+static size_t lowmem_minfree[6] = {
+	3*512, // 6MB
+	2*1024, // 8MB
+	4*1024, // 16MB
+	16*1024, // 64MB
+};
+static int lowmem_minfree_size = 4;
+
+#define lowmem_print(level, x...) do { if(lowmem_debug_level >= (level)) printk(x); } while(0)
+
+module_param_named(cost, lowmem_shrinker.seeks, int, S_IRUGO | S_IWUSR);
+module_param_array_named(adj, lowmem_adj, int, &lowmem_adj_size, S_IRUGO | S_IWUSR);
+module_param_array_named(minfree, lowmem_minfree, uint, &lowmem_minfree_size, S_IRUGO | S_IWUSR);
+module_param_named(debug_level, lowmem_debug_level, uint, S_IRUGO | S_IWUSR);
+
+static int lowmem_shrink(int nr_to_scan, gfp_t gfp_mask)
+{
+	struct task_struct *p;
+	struct task_struct *selected = NULL;
+	int rem = 0;
+	int tasksize;
+	int i;
+	int min_adj = OOM_ADJUST_MAX + 1;
+	int selected_tasksize = 0;
+	int array_size = ARRAY_SIZE(lowmem_adj);
+	int other_free = global_page_state(NR_FREE_PAGES) + global_page_state(NR_FILE_PAGES);
+	if(lowmem_adj_size < array_size)
+		array_size = lowmem_adj_size;
+	if(lowmem_minfree_size < array_size)
+		array_size = lowmem_minfree_size;
+	for(i = 0; i < array_size; i++) {
+		if(other_free < lowmem_minfree[i]) {
+			min_adj = lowmem_adj[i];
+			break;
+		}
+	}
+	if(nr_to_scan > 0)
+		lowmem_print(3, "lowmem_shrink %d, %x, ofree %d, ma %d\n", nr_to_scan, gfp_mask, other_free, min_adj);
+	read_lock(&tasklist_lock);
+	for_each_process(p) {
+		if(p->oomkilladj >= 0 && p->mm) {
+			tasksize = get_mm_rss(p->mm);
+			if(nr_to_scan > 0 && tasksize > 0 && p->oomkilladj >= min_adj) {
+				if(selected == NULL ||
+				   p->oomkilladj > selected->oomkilladj ||
+				   (p->oomkilladj == selected->oomkilladj &&
+				    tasksize > selected_tasksize)) {
+					selected = p;
+					selected_tasksize = tasksize;
+					lowmem_print(2, "select %d (%s), adj %d, size %d, to kill\n",
+					             p->pid, p->comm, p->oomkilladj, tasksize);
+				}
+			}
+			rem += tasksize;
+		}
+	}
+	if(selected != NULL) {
+		lowmem_print(1, "send sigkill to %d (%s), adj %d, size %d\n",
+		             selected->pid, selected->comm,
+		             selected->oomkilladj, selected_tasksize);
+		force_sig(SIGKILL, selected);
+		rem -= selected_tasksize;
+	}
+	lowmem_print(4, "lowmem_shrink %d, %x, return %d\n", nr_to_scan, gfp_mask, rem);
+	read_unlock(&tasklist_lock);
+	return rem;
+}
+
+static int __init lowmem_init(void)
+{
+	register_shrinker(&lowmem_shrinker);
+	return 0;
+}
+
+static void __exit lowmem_exit(void)
+{
+	unregister_shrinker(&lowmem_shrinker);
+}
+
+module_init(lowmem_init);
+module_exit(lowmem_exit);
+
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/staging/android/ram_console.c b/drivers/staging/android/ram_console.c
new file mode 100644
index 000000000000..bf006857a87a
--- /dev/null
+++ b/drivers/staging/android/ram_console.c
@@ -0,0 +1,395 @@
+/* drivers/android/ram_console.c
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/console.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/proc_fs.h>
+#include <linux/string.h>
+#include <linux/uaccess.h>
+#include <asm/io.h>
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+#include <linux/rslib.h>
+#endif
+
+struct ram_console_buffer {
+	uint32_t    sig;
+	uint32_t    start;
+	uint32_t    size;
+	uint8_t     data[0];
+};
+
+#define RAM_CONSOLE_SIG (0x43474244) /* DBGC */
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+static char __initdata
+	ram_console_old_log_init_buffer[CONFIG_ANDROID_RAM_CONSOLE_EARLY_SIZE];
+#endif
+static char *ram_console_old_log;
+static size_t ram_console_old_log_size;
+
+static struct ram_console_buffer *ram_console_buffer;
+static size_t ram_console_buffer_size;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+static char *ram_console_par_buffer;
+static struct rs_control *ram_console_rs_decoder;
+static int ram_console_corrected_bytes;
+static int ram_console_bad_blocks;
+#define ECC_BLOCK_SIZE CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_DATA_SIZE
+#define ECC_SIZE CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_ECC_SIZE
+#define ECC_SYMSIZE CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE
+#define ECC_POLY CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_POLYNOMIAL
+#endif
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+static void ram_console_encode_rs8(uint8_t *data, size_t len, uint8_t *ecc)
+{
+	int i;
+	uint16_t par[ECC_SIZE];
+	/* Initialize the parity buffer */
+	memset(par, 0, sizeof(par));
+	encode_rs8(ram_console_rs_decoder, data, len, par, 0);
+	for (i = 0; i < ECC_SIZE; i++)
+		ecc[i] = par[i];
+}
+
+static int ram_console_decode_rs8(void *data, size_t len, uint8_t *ecc)
+{
+	int i;
+	uint16_t par[ECC_SIZE];
+	for (i = 0; i < ECC_SIZE; i++)
+		par[i] = ecc[i];
+	return decode_rs8(ram_console_rs_decoder, data, par, len,
+				NULL, 0, NULL, 0, NULL);
+}
+#endif
+
+static void ram_console_update(const char *s, unsigned int count)
+{
+	struct ram_console_buffer *buffer = ram_console_buffer;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	uint8_t *buffer_end = buffer->data + ram_console_buffer_size;
+	uint8_t *block;
+	uint8_t *par;
+	int size = ECC_BLOCK_SIZE;
+#endif
+	memcpy(buffer->data + buffer->start, s, count);
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	block = buffer->data + (buffer->start & ~(ECC_BLOCK_SIZE - 1));
+	par = ram_console_par_buffer +
+	      (buffer->start / ECC_BLOCK_SIZE) * ECC_SIZE;
+	do {
+		if (block + ECC_BLOCK_SIZE > buffer_end)
+			size = buffer_end - block;
+		ram_console_encode_rs8(block, size, par);
+		block += ECC_BLOCK_SIZE;
+		par += ECC_SIZE;
+	} while (block < buffer->data + buffer->start + count);
+#endif
+}
+
+static void ram_console_update_header(void)
+{
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	struct ram_console_buffer *buffer = ram_console_buffer;
+	uint8_t *par;
+	par = ram_console_par_buffer +
+	      DIV_ROUND_UP(ram_console_buffer_size, ECC_BLOCK_SIZE) * ECC_SIZE;
+	ram_console_encode_rs8((uint8_t *)buffer, sizeof(*buffer), par);
+#endif
+}
+
+static void
+ram_console_write(struct console *console, const char *s, unsigned int count)
+{
+	int rem;
+	struct ram_console_buffer *buffer = ram_console_buffer;
+
+	if (count > ram_console_buffer_size) {
+		s += count - ram_console_buffer_size;
+		count = ram_console_buffer_size;
+	}
+	rem = ram_console_buffer_size - buffer->start;
+	if (rem < count) {
+		ram_console_update(s, rem);
+		s += rem;
+		count -= rem;
+		buffer->start = 0;
+		buffer->size = ram_console_buffer_size;
+	}
+	ram_console_update(s, count);
+
+	buffer->start += count;
+	if (buffer->size < ram_console_buffer_size)
+		buffer->size += count;
+	ram_console_update_header();
+}
+
+static struct console ram_console = {
+	.name	= "ram",
+	.write	= ram_console_write,
+	.flags	= CON_PRINTBUFFER | CON_ENABLED,
+	.index	= -1,
+};
+
+static void __init
+ram_console_save_old(struct ram_console_buffer *buffer, char *dest)
+{
+	size_t old_log_size = buffer->size;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	uint8_t *block;
+	uint8_t *par;
+	char strbuf[80];
+	int strbuf_len;
+
+	block = buffer->data;
+	par = ram_console_par_buffer;
+	while (block < buffer->data + buffer->size) {
+		int numerr;
+		int size = ECC_BLOCK_SIZE;
+		if (block + size > buffer->data + ram_console_buffer_size)
+			size = buffer->data + ram_console_buffer_size - block;
+		numerr = ram_console_decode_rs8(block, size, par);
+		if (numerr > 0) {
+#if 0
+			printk(KERN_INFO "ram_console: error in block %p, %d\n",
+			       block, numerr);
+#endif
+			ram_console_corrected_bytes += numerr;
+		} else if (numerr < 0) {
+#if 0
+			printk(KERN_INFO "ram_console: uncorrectable error in "
+			       "block %p\n", block);
+#endif
+			ram_console_bad_blocks++;
+		}
+		block += ECC_BLOCK_SIZE;
+		par += ECC_SIZE;
+	}
+	if (ram_console_corrected_bytes || ram_console_bad_blocks)
+		strbuf_len = snprintf(strbuf, sizeof(strbuf),
+			"\n%d Corrected bytes, %d unrecoverable blocks\n",
+			ram_console_corrected_bytes, ram_console_bad_blocks);
+	else
+		strbuf_len = snprintf(strbuf, sizeof(strbuf),
+				      "\nNo errors detected\n");
+	if (strbuf_len >= sizeof(strbuf))
+		strbuf_len = sizeof(strbuf) - 1;
+	old_log_size += strbuf_len;
+#endif
+
+	if (dest == NULL) {
+		dest = kmalloc(old_log_size, GFP_KERNEL);
+		if (dest == NULL) {
+			printk(KERN_ERR
+			       "ram_console: failed to allocate buffer\n");
+			return;
+		}
+	}
+
+	ram_console_old_log = dest;
+	ram_console_old_log_size = old_log_size;
+	memcpy(ram_console_old_log,
+	       &buffer->data[buffer->start], buffer->size - buffer->start);
+	memcpy(ram_console_old_log + buffer->size - buffer->start,
+	       &buffer->data[0], buffer->start);
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	memcpy(ram_console_old_log + old_log_size - strbuf_len,
+	       strbuf, strbuf_len);
+#endif
+}
+
+static int __init ram_console_init(struct ram_console_buffer *buffer,
+				   size_t buffer_size, char *old_buf)
+{
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	int numerr;
+	uint8_t *par;
+#endif
+	ram_console_buffer = buffer;
+	ram_console_buffer_size =
+		buffer_size - sizeof(struct ram_console_buffer);
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	ram_console_buffer_size -= (DIV_ROUND_UP(ram_console_buffer_size,
+						ECC_BLOCK_SIZE) + 1) * ECC_SIZE;
+	ram_console_par_buffer = buffer->data + ram_console_buffer_size;
+
+
+	/* first consecutive root is 0
+	 * primitive element to generate roots = 1
+	 */
+	ram_console_rs_decoder = init_rs(ECC_SYMSIZE, ECC_POLY, 0, 1, ECC_SIZE);
+	if (ram_console_rs_decoder == NULL) {
+		printk(KERN_INFO "ram_console: init_rs failed\n");
+		return 0;
+	}
+
+	ram_console_corrected_bytes = 0;
+	ram_console_bad_blocks = 0;
+
+	par = ram_console_par_buffer +
+	      DIV_ROUND_UP(ram_console_buffer_size, ECC_BLOCK_SIZE) * ECC_SIZE;
+
+	numerr = ram_console_decode_rs8(buffer, sizeof(*buffer), par);
+	if (numerr > 0) {
+		printk(KERN_INFO "ram_console: error in header, %d\n", numerr);
+		ram_console_corrected_bytes += numerr;
+	} else if (numerr < 0) {
+		printk(KERN_INFO
+		       "ram_console: uncorrectable error in header\n");
+		ram_console_bad_blocks++;
+	}
+#endif
+
+	if (buffer->sig == RAM_CONSOLE_SIG) {
+		if (buffer->size > ram_console_buffer_size
+		    || buffer->start > buffer->size)
+			printk(KERN_INFO "ram_console: found existing invalid "
+			       "buffer, size %d, start %d\n",
+			       buffer->size, buffer->start);
+		else {
+			printk(KERN_INFO "ram_console: found existing buffer, "
+			       "size %d, start %d\n",
+			       buffer->size, buffer->start);
+			ram_console_save_old(buffer, old_buf);
+		}
+	} else {
+		printk(KERN_INFO "ram_console: no valid data in buffer "
+		       "(sig = 0x%08x)\n", buffer->sig);
+	}
+
+	buffer->sig = RAM_CONSOLE_SIG;
+	buffer->start = 0;
+	buffer->size = 0;
+
+	register_console(&ram_console);
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ENABLE_VERBOSE
+	console_verbose();
+#endif
+	return 0;
+}
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+static int __init ram_console_early_init(void)
+{
+	return ram_console_init((struct ram_console_buffer *)
+		CONFIG_ANDROID_RAM_CONSOLE_EARLY_ADDR,
+		CONFIG_ANDROID_RAM_CONSOLE_EARLY_SIZE,
+		ram_console_old_log_init_buffer);
+}
+#else
+static int ram_console_driver_probe(struct platform_device *pdev)
+{
+	struct resource *res = pdev->resource;
+	size_t start;
+	size_t buffer_size;
+	void *buffer;
+
+	if (res == NULL || pdev->num_resources != 1 ||
+	    !(res->flags & IORESOURCE_MEM)) {
+		printk(KERN_ERR "ram_console: invalid resource, %p %d flags "
+		       "%lx\n", res, pdev->num_resources, res ? res->flags : 0);
+		return -ENXIO;
+	}
+	buffer_size = res->end - res->start + 1;
+	start = res->start;
+	printk(KERN_INFO "ram_console: got buffer at %x, size %x\n",
+	       start, buffer_size);
+	buffer = ioremap(res->start, buffer_size);
+	if (buffer == NULL) {
+		printk(KERN_ERR "ram_console: failed to map memory\n");
+		return -ENOMEM;
+	}
+
+	return ram_console_init(buffer, buffer_size, NULL/* allocate */);
+}
+
+static struct platform_driver ram_console_driver = {
+	.probe = ram_console_driver_probe,
+	.driver		= {
+		.name	= "ram_console",
+	},
+};
+
+static int __init ram_console_module_init(void)
+{
+	int err;
+	err = platform_driver_register(&ram_console_driver);
+	return err;
+}
+#endif
+
+static ssize_t ram_console_read_old(struct file *file, char __user *buf,
+				    size_t len, loff_t *offset)
+{
+	loff_t pos = *offset;
+	ssize_t count;
+
+	if (pos >= ram_console_old_log_size)
+		return 0;
+
+	count = min(len, (size_t)(ram_console_old_log_size - pos));
+	if (copy_to_user(buf, ram_console_old_log + pos, count))
+		return -EFAULT;
+
+	*offset += count;
+	return count;
+}
+
+static struct file_operations ram_console_file_ops = {
+	.owner = THIS_MODULE,
+	.read = ram_console_read_old,
+};
+
+static int __init ram_console_late_init(void)
+{
+	struct proc_dir_entry *entry;
+
+	if (ram_console_old_log == NULL)
+		return 0;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+	ram_console_old_log = kmalloc(ram_console_old_log_size, GFP_KERNEL);
+	if (ram_console_old_log == NULL) {
+		printk(KERN_ERR
+		       "ram_console: failed to allocate buffer for old log\n");
+		ram_console_old_log_size = 0;
+		return 0;
+	}
+	memcpy(ram_console_old_log,
+	       ram_console_old_log_init_buffer, ram_console_old_log_size);
+#endif
+	entry = create_proc_entry("last_kmsg", S_IFREG | S_IRUGO, NULL);
+	if (!entry) {
+		printk(KERN_ERR "ram_console: failed to create proc entry\n");
+		kfree(ram_console_old_log);
+		ram_console_old_log = NULL;
+		return 0;
+	}
+
+	entry->proc_fops = &ram_console_file_ops;
+	entry->size = ram_console_old_log_size;
+	return 0;
+}
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+console_initcall(ram_console_early_init);
+#else
+module_init(ram_console_module_init);
+#endif
+late_initcall(ram_console_late_init);
+
diff --git a/drivers/staging/android/timed_gpio.c b/drivers/staging/android/timed_gpio.c
new file mode 100644
index 000000000000..bea68c9fc942
--- /dev/null
+++ b/drivers/staging/android/timed_gpio.c
@@ -0,0 +1,177 @@
+/* drivers/misc/timed_gpio.c
+ *
+ * Copyright (C) 2008 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/hrtimer.h>
+#include <linux/err.h>
+#include <asm/arch/gpio.h>
+
+#include "timed_gpio.h"
+
+
+static struct class *timed_gpio_class;
+
+struct timed_gpio_data {
+	struct device *dev;
+	struct hrtimer timer;
+	spinlock_t lock;
+	unsigned 	gpio;
+	int 		max_timeout;
+	u8 		active_low;
+};
+
+static enum hrtimer_restart gpio_timer_func(struct hrtimer *timer)
+{
+	struct timed_gpio_data *gpio_data = container_of(timer, struct timed_gpio_data, timer);
+
+	gpio_direction_output(gpio_data->gpio, gpio_data->active_low ? 1 : 0);
+	return HRTIMER_NORESTART;
+}
+
+static ssize_t gpio_enable_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct timed_gpio_data *gpio_data = dev_get_drvdata(dev);
+	int remaining;
+
+	if (hrtimer_active(&gpio_data->timer)) {
+		ktime_t r = hrtimer_get_remaining(&gpio_data->timer);
+		remaining = r.tv.sec * 1000 + r.tv.nsec / 1000000;
+	} else
+		remaining = 0;
+
+	return sprintf(buf, "%d\n", remaining);
+}
+
+static ssize_t gpio_enable_store(
+		struct device *dev, struct device_attribute *attr,
+		const char *buf, size_t size)
+{
+	struct timed_gpio_data *gpio_data = dev_get_drvdata(dev);
+	int value;
+	unsigned long	flags;
+
+	sscanf(buf, "%d", &value);
+
+	spin_lock_irqsave(&gpio_data->lock, flags);
+
+	/* cancel previous timer and set GPIO according to value */
+	hrtimer_cancel(&gpio_data->timer);
+	gpio_direction_output(gpio_data->gpio, gpio_data->active_low ? !value : !!value);
+
+	if (value > 0) {
+		if (value > gpio_data->max_timeout)
+			value = gpio_data->max_timeout;
+
+		hrtimer_start(&gpio_data->timer,
+						ktime_set(value / 1000, (value % 1000) * 1000000),
+						HRTIMER_MODE_REL);
+	}
+
+	spin_unlock_irqrestore(&gpio_data->lock, flags);
+
+	return size;
+}
+
+static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR, gpio_enable_show, gpio_enable_store);
+
+static int timed_gpio_probe(struct platform_device *pdev)
+{
+	struct timed_gpio_platform_data *pdata = pdev->dev.platform_data;
+	struct timed_gpio *cur_gpio;
+	struct timed_gpio_data *gpio_data, *gpio_dat;
+	int i, ret = 0;
+
+	if (!pdata)
+		return -EBUSY;
+
+	gpio_data = kzalloc(sizeof(struct timed_gpio_data) * pdata->num_gpios, GFP_KERNEL);
+	if (!gpio_data)
+		return -ENOMEM;
+
+	for (i = 0; i < pdata->num_gpios; i++) {
+		cur_gpio = &pdata->gpios[i];
+		gpio_dat = &gpio_data[i];
+
+		hrtimer_init(&gpio_dat->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+		gpio_dat->timer.function = gpio_timer_func;
+		spin_lock_init(&gpio_dat->lock);
+
+		gpio_dat->gpio = cur_gpio->gpio;
+		gpio_dat->max_timeout = cur_gpio->max_timeout;
+		gpio_dat->active_low = cur_gpio->active_low;
+		gpio_direction_output(gpio_dat->gpio, gpio_dat->active_low);
+
+		gpio_dat->dev = device_create(timed_gpio_class, &pdev->dev, 0, "%s", cur_gpio->name);
+		if (unlikely(IS_ERR(gpio_dat->dev)))
+			return PTR_ERR(gpio_dat->dev);
+
+		dev_set_drvdata(gpio_dat->dev, gpio_dat);
+		ret = device_create_file(gpio_dat->dev, &dev_attr_enable);
+		if (ret)
+			return ret;
+	}
+
+	platform_set_drvdata(pdev, gpio_data);
+
+	return 0;
+}
+
+static int timed_gpio_remove(struct platform_device *pdev)
+{
+	struct timed_gpio_platform_data *pdata = pdev->dev.platform_data;
+	struct timed_gpio_data *gpio_data = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < pdata->num_gpios; i++) {
+		device_remove_file(gpio_data[i].dev, &dev_attr_enable);
+		device_unregister(gpio_data[i].dev);
+	}
+
+	kfree(gpio_data);
+
+	return 0;
+}
+
+static struct platform_driver timed_gpio_driver = {
+	.probe		= timed_gpio_probe,
+	.remove		= timed_gpio_remove,
+	.driver		= {
+		.name		= "timed-gpio",
+		.owner		= THIS_MODULE,
+	},
+};
+
+static int __init timed_gpio_init(void)
+{
+	timed_gpio_class = class_create(THIS_MODULE, "timed_output");
+	if (IS_ERR(timed_gpio_class))
+		return PTR_ERR(timed_gpio_class);
+	return platform_driver_register(&timed_gpio_driver);
+}
+
+static void __exit timed_gpio_exit(void)
+{
+	class_destroy(timed_gpio_class);
+	platform_driver_unregister(&timed_gpio_driver);
+}
+
+module_init(timed_gpio_init);
+module_exit(timed_gpio_exit);
+
+MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
+MODULE_DESCRIPTION("timed gpio driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/android/timed_gpio.h b/drivers/staging/android/timed_gpio.h
new file mode 100644
index 000000000000..78449b2161cf
--- /dev/null
+++ b/drivers/staging/android/timed_gpio.h
@@ -0,0 +1,31 @@
+/* include/linux/timed_gpio.h
+ *
+ * Copyright (C) 2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+*/
+
+#ifndef _LINUX_TIMED_GPIO_H
+#define _LINUX_TIMED_GPIO_H
+
+struct timed_gpio {
+	const char *name;
+	unsigned 	gpio;
+	int     max_timeout;
+	u8 		active_low;
+};
+
+struct timed_gpio_platform_data {
+	int 		num_gpios;
+	struct timed_gpio *gpios;
+};
+
+#endif
diff --git a/drivers/staging/asus_oled/Kconfig b/drivers/staging/asus_oled/Kconfig
new file mode 100644
index 000000000000..e56dbb25ac5d
--- /dev/null
+++ b/drivers/staging/asus_oled/Kconfig
@@ -0,0 +1,6 @@
+config ASUS_OLED
+	tristate "Asus OLED driver"
+	depends on USB
+	default N
+	---help---
+	  Enable support for the OLED display present in some Asus laptops.
diff --git a/drivers/staging/asus_oled/Makefile b/drivers/staging/asus_oled/Makefile
new file mode 100644
index 000000000000..e71f9aa9e037
--- /dev/null
+++ b/drivers/staging/asus_oled/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_ASUS_OLED)		+= asus_oled.o
diff --git a/drivers/staging/asus_oled/README b/drivers/staging/asus_oled/README
new file mode 100644
index 000000000000..96b9717f168f
--- /dev/null
+++ b/drivers/staging/asus_oled/README
@@ -0,0 +1,156 @@
+
+    Driver for Asus OLED display present in some Asus laptops.
+
+    The code of this driver is based on 'asusoled' program taken from
+    https://launchpad.net/asusoled/. I just wanted to have a simple
+    kernel driver for controlling this device, but I didn't know how
+    to do that. Now I know ;) Also, that program can not be used
+    with usbhid loaded, which means no USB mouse/keyboard while
+    controlling OLED display :(
+
+    It has been tested on Asus G1 and didn't cause any problems,
+    but I don't guarantee that it won't do anything wrong :)
+
+    It can (and probably does) have errors. It is usable
+    in my case, and I hope others will find it useful too!
+
+*******
+
+Building the module
+
+   To build the module you need kernel 2.6 include files and some C compiler.
+
+   Just run:
+   make
+   make install (as a root)
+
+   It will build (hopefully) the module and install it in
+   /lib/modules/'uname -r'/extra/asus_oled.ko.
+
+   To load it just use:
+   modprobe asus_oled
+
+   You can check if it has detected your OLED display by looking into dmesg output.
+   There should be something like this:
+   asus-oled 2-7:1.0: Attached Asus OLED device
+
+   If it doesn't find your display, you can try removing usbhid module.
+   If you add asus_oled into the list of modules loaded during system boot
+   before usbhid, it will work even when usbhid is present.
+
+   If it still doesn't detect your hardware, check lsusb output.
+   There should be similar line:
+   Bus 002 Device 005: ID 0b05:1726 ASUSTek Computer, Inc.
+
+   If you don't see any lines with '0b05:1726' it means that you have different
+   type of hardware that is not detected (it may or may not work, but the driver
+   knows only '0b05:1726' device).
+
+*******
+
+Configuration
+
+   There is only one option: start_off.
+   You can use it by: 'modprobe asus_oled start_off=1', or by adding this
+   line to /etc/modprobe.conf:
+   options asus_oled start_off=1
+
+   With this option provided, asus_oled driver will switch off the display
+   when it is detected and attached. It is nice feature to just switch off the 'ASUS'
+   logo. If you don't use the display, it is probably the good idea to switch it off,
+   to protect OLEDs from "wearing off".
+
+*******
+
+Usage
+
+   This module can be controlled with two special files:
+   /sys/class/asus_oled/oled_N/enabled
+   /sys/class/asus_oled/oled_N/picture
+
+   (N is the device number, the first, and probably the only, has number 1,
+    so it is /sys/class/asus_oled/oled_1/enabled
+    and /sys/class/asus_oled/oled_1/picture)
+
+   'enabled' files is for reading and writing, 'picture' is writeable only.
+
+   You can write 0 or 1 to 'enabled' file, which will switch
+   on and off the display. Reading from this file will tell you the last
+   status set, either 0 or 1. By default it is 1, so if the device was set to 'off',
+   and the computer was rebooted without power-off, this file will contain wrong
+   value - because the device is off, but hasn't been disabled this time and is
+   assumed to be on...
+
+   To 'picture' file you write pictures to be displayed by the OLED device.
+   The format of the file:
+   <M:WxH>
+   00001110010111000
+   00010101010101010
+   ....
+
+   First line is a configuration parameter. Meaning of fields in <M:WxH>:
+   M - picture mode. It can be either 's' for static pictures,
+       'r' for rolling pictures, and 'f' for flashing pictures.
+   W - width of the picture. May be between 1 and 1792
+   H - height of the picture. May be between 1 and 32
+
+   For example <s:128x32> means static picture, 128 pixels long and 32 pixels high.
+
+   The physical size of the display is 128x32 pixels. Static and flashing pictures
+   can't be larger than that (actually they can, but only part of them will be displayed ;) )
+
+   If the picture is smaller than 128x32 it will be centered. Rolling pictures wider than
+   128 pixels will be centered too, unless their width = n*128. Vertically they will be
+   centered just like static pictures, if their height is smaller than 32.
+
+   Flashing pictures will be centered horizontally if their width < 128, but they were
+   centered vertically in a different way. If their height < 16, they will be centered
+   in the upper half of the display (rows 0-15). This is because only the first half
+   of flashing pictures is used for flashing. When the picture with heigh = 32 is
+   displayed in flashing mode, its upper 16 rows will be flashing in the upper half
+   of the display, and the lower half will be empty. After few seconds upper part will
+   stop flashing (but that part of the picture will remain there), and the lower
+   half of the display will start displayin the lower half of the picture
+   in rolling mode, unless it is empty, or the picture was small enough to fit in
+   upper part. It is not mine idea, this is just the way Asus' display work ;)
+   So if you need just flashing, use at most 128x16 picture. If you need flashing and
+   rolling, use whole size of the display.
+
+   Lines following the first, configuration, line are picture data. Each '1' means
+   that the pixel is lit, and '0' means that it is not. You can also use '#' as ON,
+   and ' ' (space) as OFF. Empty lines and all other characters are ignored.
+
+   It is possible to write everything in one line <M:WxH>01010101010101010...,
+   and W*H characters will be used. If there is not enough characters, nothing will be
+   displayed. However, the 'line mode' is easier to read (and write), and it also
+   lets to omit parts of data. Whenever End-Of-Line character is found, but
+   the line is not W characters long, it is assumed that all missing characters
+   are equal to the last character in the line.
+
+   Following line represents '0', '1' and a lots of '0's, dependng on the width of the picture
+   provided in configuration data:
+   010
+
+   So if you need empty line, it is sufficient to write line with only one '0' in it.
+   The same works with '1' (or ' ' and '#').
+
+   If there are too many data in the file, they will be ignored. If you are not sure
+   how many characters you are missing, you can add few lines with one zero in each of them.
+
+   There are some example pictures in .txt format, that can be used as follows:
+   cat foo.txt > /sys/class/asus_oled/oled_1/picture
+
+   If the display is switched off you also need to run:
+   echo 1 > /sys/class/asus_oled/oled_1/enabled
+   To switch it off, just use:
+   echo 0 > /sys/class/asus_oled/oled_1/enabled
+
+
+*******
+
+   For any additional info please have a look at http://lapsus.berlios.de/asus_oled.html
+
+
+
+   Jakub Schmidtke (sjakub@gmail.com)
+
diff --git a/drivers/staging/asus_oled/TODO b/drivers/staging/asus_oled/TODO
new file mode 100644
index 000000000000..2514131670a3
--- /dev/null
+++ b/drivers/staging/asus_oled/TODO
@@ -0,0 +1,10 @@
+TODO:
+	- checkpatch.pl cleanups
+	- sparse fixes
+	- audit the userspace interface
+		- sysfs vs. char?
+	- Documentation/ABI/ needs to be added
+	- put the sample .txt files and README file somewhere.
+
+Please send patches to Greg Kroah-Hartman <greg@kroah.com> and
+Cc: Jakub Schmidtke <sjakub@gmail.com>
diff --git a/drivers/staging/asus_oled/asus_oled.c b/drivers/staging/asus_oled/asus_oled.c
new file mode 100644
index 000000000000..666a186e212b
--- /dev/null
+++ b/drivers/staging/asus_oled/asus_oled.c
@@ -0,0 +1,745 @@
+/*
+ *  Asus OLED USB driver
+ *
+ *  Copyright (C) 2007,2008 Jakub Schmidtke (sjakub@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.
+ *
+ *  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.
+ *
+ *
+ *
+ *  This module is based on usbled and asus-laptop modules.
+ *
+ *
+ *  Asus OLED support is based on asusoled program taken from
+ *  https://launchpad.net/asusoled/.
+ *
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/usb.h>
+#include <linux/platform_device.h>
+#include <linux/ctype.h>
+
+#define ASUS_OLED_VERSION		"0.04-dev"
+#define ASUS_OLED_NAME			"asus-oled"
+#define ASUS_OLED_UNDERSCORE_NAME	"asus_oled"
+
+#define ASUS_OLED_ERROR			"Asus OLED Display Error: "
+
+#define ASUS_OLED_STATIC		's'
+#define ASUS_OLED_ROLL			'r'
+#define ASUS_OLED_FLASH			'f'
+
+#define ASUS_OLED_MAX_WIDTH		1792
+#define ASUS_OLED_DISP_HEIGHT		32
+#define ASUS_OLED_PACKET_BUF_SIZE	256
+
+MODULE_AUTHOR("Jakub Schmidtke, sjakub@gmail.com");
+MODULE_DESCRIPTION("Asus OLED Driver v" ASUS_OLED_VERSION);
+MODULE_LICENSE("GPL");
+
+static struct class *oled_class = 0;
+static int oled_num = 0;
+
+static uint start_off = 0;
+
+module_param(start_off, uint, 0644);
+
+MODULE_PARM_DESC(start_off, "Set to 1 to switch off OLED display after it is attached");
+
+typedef enum {
+	PACK_MODE_G1,
+	PACK_MODE_G50,
+	PACK_MODE_LAST
+} oled_pack_mode_t;
+
+struct oled_dev_desc_str {
+	uint16_t		idVendor;
+	uint16_t		idProduct;
+	uint16_t		devWidth; // width of display
+	oled_pack_mode_t	packMode; // formula to be used while packing the picture
+	const char		*devDesc;
+};
+
+/* table of devices that work with this driver */
+static struct usb_device_id id_table [] = {
+	{ USB_DEVICE(0x0b05, 0x1726) }, // Asus G1/G2 (and variants)
+	{ USB_DEVICE(0x0b05, 0x175b) }, // Asus G50V (and possibly others - G70? G71?)
+	{ },
+};
+
+/* parameters of specific devices */
+static struct oled_dev_desc_str oled_dev_desc_table [] = {
+	{ 0x0b05, 0x1726, 128, PACK_MODE_G1, "G1/G2" },
+	{ 0x0b05, 0x175b, 256, PACK_MODE_G50, "G50" },
+	{ },
+};
+
+MODULE_DEVICE_TABLE (usb, id_table);
+
+#define SETUP_PACKET_HEADER(packet, val1, val2, val3, val4, val5, val6, val7) \
+	do {					\
+		memset(packet, 0, sizeof(struct asus_oled_header));		\
+		packet->header.magic1 = 0x55;		\
+		packet->header.magic2 = 0xaa;		\
+		packet->header.flags = val1;		\
+		packet->header.value3 = val2;		\
+		packet->header.buffer1 = val3;		\
+		packet->header.buffer2 = val4;		\
+		packet->header.value6 = val5;		\
+		packet->header.value7 = val6;		\
+		packet->header.value8 = val7;		\
+	} while(0);
+
+struct asus_oled_header {
+	uint8_t		magic1;
+	uint8_t		magic2;
+	uint8_t		flags;
+	uint8_t		value3;
+	uint8_t		buffer1;
+	uint8_t		buffer2;
+	uint8_t		value6;
+	uint8_t		value7;
+	uint8_t		value8;
+	uint8_t		padding2[7];
+} __attribute((packed));
+
+struct asus_oled_packet {
+	struct asus_oled_header		header;
+	uint8_t				bitmap[ASUS_OLED_PACKET_BUF_SIZE];
+} __attribute((packed));
+
+struct asus_oled_dev {
+	struct usb_device *	udev;
+	uint8_t			pic_mode;
+	uint16_t		dev_width;
+	oled_pack_mode_t	pack_mode;
+	size_t			height;
+	size_t			width;
+	size_t			x_shift;
+	size_t			y_shift;
+	size_t			buf_offs;
+	uint8_t			last_val;
+	size_t			buf_size;
+	char			*buf;
+	uint8_t			enabled;
+	struct device		*dev;
+};
+
+static void enable_oled(struct asus_oled_dev *odev, uint8_t enabl)
+{
+	int a;
+	int retval;
+	int act_len;
+	struct asus_oled_packet * packet;
+
+	packet = kzalloc(sizeof(struct asus_oled_packet), GFP_KERNEL);
+
+	if (!packet) {
+		dev_err(&odev->udev->dev, "out of memory\n");
+		return;
+	}
+
+	SETUP_PACKET_HEADER(packet, 0x20, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00);
+
+	if (enabl) packet->bitmap[0] = 0xaf;
+	else packet->bitmap[0] = 0xae;
+
+	for (a=0; a<1; a++) {
+		retval = usb_bulk_msg(odev->udev,
+			usb_sndbulkpipe(odev->udev, 2),
+			packet,
+			sizeof(struct asus_oled_header) + 1,
+			&act_len,
+			-1);
+
+		if (retval)
+			dev_dbg(&odev->udev->dev, "retval = %d\n", retval);
+	}
+
+	odev->enabled = enabl;
+
+	kfree(packet);
+}
+
+static ssize_t set_enabled(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct usb_interface *intf = to_usb_interface(dev);
+	struct asus_oled_dev *odev = usb_get_intfdata(intf);
+	int temp = simple_strtoul(buf, NULL, 10);
+
+	enable_oled(odev, temp);
+
+	return count;
+}
+
+static ssize_t class_set_enabled(struct device *device, struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct asus_oled_dev *odev = (struct asus_oled_dev *) dev_get_drvdata(device);
+
+	int temp = simple_strtoul(buf, NULL, 10);
+
+	enable_oled(odev, temp);
+
+	return count;
+}
+
+static ssize_t get_enabled(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct usb_interface *intf = to_usb_interface(dev);
+	struct asus_oled_dev *odev = usb_get_intfdata(intf);
+
+	return sprintf(buf, "%d\n", odev->enabled);
+}
+
+static ssize_t class_get_enabled(struct device *device, struct device_attribute *attr, char *buf)
+{
+	struct asus_oled_dev *odev = (struct asus_oled_dev *) dev_get_drvdata(device);
+
+	return sprintf(buf, "%d\n", odev->enabled);
+}
+
+static void send_packets(struct usb_device *udev, struct asus_oled_packet *packet,
+	char *buf, uint8_t p_type, size_t p_num)
+{
+	size_t i;
+	int act_len;
+
+	for (i = 0; i < p_num; i++) {
+		int retval;
+
+		switch (p_type) {
+			case ASUS_OLED_ROLL:
+				SETUP_PACKET_HEADER(packet, 0x40, 0x80, p_num, i + 1, 0x00, 0x01, 0xff);
+			break;
+			case ASUS_OLED_STATIC:
+				SETUP_PACKET_HEADER(packet, 0x10 + i, 0x80, 0x01, 0x01, 0x00, 0x01, 0x00);
+			break;
+			case ASUS_OLED_FLASH:
+				SETUP_PACKET_HEADER(packet, 0x10 + i, 0x80, 0x01, 0x01, 0x00, 0x00, 0xff);
+			break;
+		}
+
+		memcpy(packet->bitmap, buf + (ASUS_OLED_PACKET_BUF_SIZE*i), ASUS_OLED_PACKET_BUF_SIZE);
+
+		retval = usb_bulk_msg(udev,
+			usb_sndctrlpipe(udev, 2),
+			packet,
+			sizeof(struct asus_oled_packet),
+			&act_len,
+			-1);
+
+		if (retval)
+			dev_dbg(&udev->dev, "retval = %d\n", retval);
+	}
+}
+
+static void send_packet(struct usb_device *udev, struct asus_oled_packet *packet, size_t offset, size_t len, char *buf, uint8_t b1, uint8_t b2, uint8_t b3, uint8_t b4, uint8_t b5, uint8_t b6){
+	int retval;
+	int act_len;
+
+	SETUP_PACKET_HEADER(packet, b1, b2, b3, b4, b5, b6, 0x00);
+	memcpy(packet->bitmap, buf + offset, len);
+
+	retval = usb_bulk_msg(udev,
+			usb_sndctrlpipe(udev, 2),
+			packet,
+			sizeof(struct asus_oled_packet),
+			&act_len,
+			-1);
+
+	if (retval)
+		dev_dbg(&udev->dev, "retval = %d\n", retval);
+}
+
+
+static void send_packets_g50(struct usb_device *udev, struct asus_oled_packet *packet, char *buf)
+{
+	send_packet(udev, packet,     0, 0x100, buf, 0x10, 0x00, 0x02, 0x01, 0x00, 0x01);
+	send_packet(udev, packet, 0x100, 0x080, buf, 0x10, 0x00, 0x02, 0x02, 0x80, 0x00);
+
+	send_packet(udev, packet, 0x180, 0x100, buf, 0x11, 0x00, 0x03, 0x01, 0x00, 0x01);
+	send_packet(udev, packet, 0x280, 0x100, buf, 0x11, 0x00, 0x03, 0x02, 0x00, 0x01);
+	send_packet(udev, packet, 0x380, 0x080, buf, 0x11, 0x00, 0x03, 0x03, 0x80, 0x00);
+}
+
+
+static void send_data(struct asus_oled_dev *odev)
+{
+	size_t packet_num = odev->buf_size / ASUS_OLED_PACKET_BUF_SIZE;
+	struct asus_oled_packet * packet;
+
+	packet = kzalloc(sizeof(struct asus_oled_packet), GFP_KERNEL);
+
+	if (!packet) {
+		dev_err(&odev->udev->dev, "out of memory\n");
+		return;
+	}
+
+	if (odev->pack_mode==PACK_MODE_G1){
+		// When sending roll-mode data the display updated only first packet.
+		// I have no idea why, but when static picture is send just before
+		// rolling picture - everything works fine.
+		if (odev->pic_mode == ASUS_OLED_ROLL)
+			send_packets(odev->udev, packet, odev->buf, ASUS_OLED_STATIC, 2);
+
+		// Only ROLL mode can use more than 2 packets.
+		if (odev->pic_mode != ASUS_OLED_ROLL && packet_num > 2)
+			packet_num = 2;
+
+		send_packets(odev->udev, packet, odev->buf, odev->pic_mode, packet_num);
+	}
+	else
+	if (odev->pack_mode==PACK_MODE_G50){
+		send_packets_g50(odev->udev, packet, odev->buf);
+	}
+
+	kfree(packet);
+}
+
+static int append_values(struct asus_oled_dev *odev, uint8_t val, size_t count)
+{
+	while (count-- > 0) {
+		if (val) {
+			size_t x = odev->buf_offs % odev->width;
+			size_t y = odev->buf_offs / odev->width;
+			size_t i;
+
+			x += odev->x_shift;
+			y += odev->y_shift;
+
+			switch(odev->pack_mode)
+			{
+				case PACK_MODE_G1:
+					// i = (x/128)*640 + 127 - x + (y/8)*128;
+					// This one for 128 is the same, but might be better for different widths?
+					i = (x/odev->dev_width)*640 + odev->dev_width - 1 - x + (y/8)*odev->dev_width;
+				break;
+
+				case PACK_MODE_G50:
+					i =  (odev->dev_width - 1 - x)/8 + y*odev->dev_width/8;
+				break;
+
+				default:
+					i = 0;
+					printk(ASUS_OLED_ERROR "Unknown OLED Pack Mode: %d!\n", odev->pack_mode);
+				break;
+			}
+
+			if (i >= odev->buf_size) {
+				printk(ASUS_OLED_ERROR "Buffer overflow! Report a bug in the driver: offs: %d >= %d i: %d (x: %d y: %d)\n",
+					(int) odev->buf_offs, (int) odev->buf_size, (int) i, (int) x, (int) y);
+				return -EIO;
+			}
+
+			switch (odev->pack_mode)
+			{
+				case PACK_MODE_G1:
+					odev->buf[i] &= ~(1<<(y%8));
+				break;
+
+				case PACK_MODE_G50:
+					odev->buf[i] &= ~(1<<(x%8));
+				break;
+
+				default:
+					// cannot get here; stops gcc complaining
+				;
+			}
+		}
+
+		odev->last_val = val;
+		odev->buf_offs++;
+	}
+
+	return 0;
+}
+
+static ssize_t odev_set_picture(struct asus_oled_dev *odev, const char *buf, size_t count)
+{
+	size_t offs = 0, max_offs;
+
+	if (count < 1) return 0;
+
+	if (tolower(buf[0]) == 'b'){
+	    // binary mode, set the entire memory
+
+	    size_t i;
+
+	    odev->buf_size = (odev->dev_width * ASUS_OLED_DISP_HEIGHT) / 8;
+
+	    if (odev->buf) kfree(odev->buf);
+	    odev->buf = kmalloc(odev->buf_size, GFP_KERNEL);
+
+	    memset(odev->buf, 0xff, odev->buf_size);
+
+	    for (i=1; i < count && i<=32*32; i++){
+		odev->buf[i-1] = buf[i];
+		odev->buf_offs = i-1;
+	    }
+
+	    odev->width=odev->dev_width / 8;
+	    odev->height=ASUS_OLED_DISP_HEIGHT;
+	    odev->x_shift=0;
+	    odev->y_shift=0;
+	    odev->last_val=0;
+
+	    send_data(odev);
+
+	    return count;
+	}
+
+	if (buf[0] == '<') {
+		size_t i;
+		size_t w = 0, h = 0;
+		size_t w_mem, h_mem;
+
+		if (count < 10 || buf[2] != ':') {
+			goto error_header;
+		}
+
+		switch(tolower(buf[1])) {
+			case ASUS_OLED_STATIC:
+			case ASUS_OLED_ROLL:
+			case ASUS_OLED_FLASH:
+				odev->pic_mode = buf[1];
+				break;
+			default:
+				printk(ASUS_OLED_ERROR "Wrong picture mode: '%c'.\n", buf[1]);
+				return -EIO;
+				break;
+		}
+
+		for (i = 3; i < count; ++i) {
+			if (buf[i] >= '0' && buf[i] <= '9') {
+				w = 10*w + (buf[i] - '0');
+
+				if (w > ASUS_OLED_MAX_WIDTH) goto error_width;
+			}
+			else if (tolower(buf[i]) == 'x') break;
+			else goto error_width;
+		}
+
+		for (++i; i < count; ++i) {
+			if (buf[i] >= '0' && buf[i] <= '9') {
+				h = 10*h + (buf[i] - '0');
+
+				if (h > ASUS_OLED_DISP_HEIGHT) goto error_height;
+			}
+			else if (tolower(buf[i]) == '>') break;
+			else goto error_height;
+		}
+
+		if (w < 1 || w > ASUS_OLED_MAX_WIDTH) goto error_width;
+
+		if (h < 1 || h > ASUS_OLED_DISP_HEIGHT) goto error_height;
+
+		if (i >= count || buf[i] != '>') goto error_header;
+
+		offs = i+1;
+
+		if (w % (odev->dev_width) != 0)
+			w_mem = (w/(odev->dev_width) + 1)*(odev->dev_width);
+		else
+			w_mem = w;
+
+		if (h < ASUS_OLED_DISP_HEIGHT)
+			h_mem = ASUS_OLED_DISP_HEIGHT;
+		else
+			h_mem = h;
+
+		odev->buf_size = w_mem * h_mem / 8;
+
+		if (odev->buf) kfree(odev->buf);
+		odev->buf = kmalloc(odev->buf_size, GFP_KERNEL);
+
+		if (odev->buf == NULL) {
+			odev->buf_size = 0;
+			printk(ASUS_OLED_ERROR "Out of memory!\n");
+			return -ENOMEM;
+		}
+
+		memset(odev->buf, 0xff, odev->buf_size);
+
+		odev->buf_offs = 0;
+		odev->width = w;
+		odev->height = h;
+		odev->x_shift = 0;
+		odev->y_shift = 0;
+		odev->last_val = 0;
+
+		if (odev->pic_mode == ASUS_OLED_FLASH) {
+			if (h < ASUS_OLED_DISP_HEIGHT/2)
+				odev->y_shift = (ASUS_OLED_DISP_HEIGHT/2 - h)/2;
+		}
+		else {
+			if (h < ASUS_OLED_DISP_HEIGHT)
+				odev->y_shift = (ASUS_OLED_DISP_HEIGHT - h)/2;
+		}
+
+		if (w < (odev->dev_width))
+			odev->x_shift = ((odev->dev_width) - w)/2;
+	}
+
+	max_offs = odev->width * odev->height;
+
+	while (offs < count && odev->buf_offs < max_offs) {
+		int ret;
+
+		if (buf[offs] == '1' || buf[offs] == '#') {
+			if ( (ret = append_values(odev, 1, 1)) < 0) return ret;
+		}
+		else if (buf[offs] == '0' || buf[offs] == ' ') {
+			if ( (ret = append_values(odev, 0, 1)) < 0) return ret;
+		}
+		else if (buf[offs] == '\n') {
+			// New line detected. Lets assume, that all characters till the end of the
+			// line were equal to the last character in this line.
+			if (odev->buf_offs % odev->width != 0)
+				if ( (ret = append_values(odev, odev->last_val,
+				      odev->width - (odev->buf_offs % odev->width))) < 0) return ret;
+		}
+
+		offs++;
+	}
+
+	if (odev->buf_offs >= max_offs) send_data(odev);
+
+	return count;
+
+error_width:
+	printk(ASUS_OLED_ERROR "Wrong picture width specified.\n");
+	return -EIO;
+
+error_height:
+	printk(ASUS_OLED_ERROR "Wrong picture height specified.\n");
+	return -EIO;
+
+error_header:
+	printk(ASUS_OLED_ERROR "Wrong picture header.\n");
+	return -EIO;
+}
+
+static ssize_t set_picture(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct usb_interface *intf = to_usb_interface(dev);
+
+	return odev_set_picture(usb_get_intfdata(intf), buf, count);
+}
+
+static ssize_t class_set_picture(struct device *device, struct device_attribute *attr, const char *buf, size_t count)
+{
+	return odev_set_picture((struct asus_oled_dev *) dev_get_drvdata(device), buf, count);
+}
+
+#define ASUS_OLED_DEVICE_ATTR(_file)		dev_attr_asus_oled_##_file
+
+static DEVICE_ATTR(asus_oled_enabled, S_IWUGO | S_IRUGO, get_enabled, set_enabled);
+static DEVICE_ATTR(asus_oled_picture, S_IWUGO , NULL, set_picture);
+
+static DEVICE_ATTR(enabled, S_IWUGO | S_IRUGO, class_get_enabled, class_set_enabled);
+static DEVICE_ATTR(picture, S_IWUGO, NULL, class_set_picture);
+
+static int asus_oled_probe(struct usb_interface *interface, const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(interface);
+	struct asus_oled_dev *odev = NULL;
+	int retval = -ENOMEM;
+	uint16_t dev_width = 0;
+	oled_pack_mode_t pack_mode = PACK_MODE_LAST;
+	const struct oled_dev_desc_str * dev_desc = oled_dev_desc_table;
+	const char *desc = 0;
+
+	if (id == 0) {
+		// Even possible? Just to make sure...
+		dev_err(&interface->dev, "No usb_device_id provided!\n");
+		return -ENODEV;
+	}
+
+	for (; dev_desc->idVendor; dev_desc++)
+	{
+		if (dev_desc->idVendor == id->idVendor
+			&& dev_desc->idProduct == id->idProduct)
+		{
+			dev_width = dev_desc->devWidth;
+			desc = dev_desc->devDesc;
+			pack_mode = dev_desc->packMode;
+			break;
+		}
+	}
+
+	if ( !desc || dev_width < 1 || pack_mode == PACK_MODE_LAST) {
+		dev_err(&interface->dev, "Missing or incomplete device description!\n");
+		return -ENODEV;
+	}
+
+	odev = kzalloc(sizeof(struct asus_oled_dev), GFP_KERNEL);
+
+	if (odev == NULL) {
+		dev_err(&interface->dev, "Out of memory\n");
+		return -ENOMEM;
+	}
+
+	odev->udev = usb_get_dev(udev);
+	odev->pic_mode = ASUS_OLED_STATIC;
+	odev->dev_width = dev_width;
+	odev->pack_mode = pack_mode;
+	odev->height = 0;
+	odev->width = 0;
+	odev->x_shift = 0;
+	odev->y_shift = 0;
+	odev->buf_offs = 0;
+	odev->buf_size = 0;
+	odev->last_val = 0;
+	odev->buf = NULL;
+	odev->enabled = 1;
+	odev->dev = 0;
+
+	usb_set_intfdata (interface, odev);
+
+	if ((retval = device_create_file(&interface->dev, &ASUS_OLED_DEVICE_ATTR(enabled)))) {
+		goto err_files;
+	}
+
+	if ((retval = device_create_file(&interface->dev, &ASUS_OLED_DEVICE_ATTR(picture)))) {
+		goto err_files;
+	}
+
+	odev->dev = device_create(oled_class, &interface->dev, MKDEV(0,0),
+				NULL,"oled_%d", ++oled_num);
+
+	if (IS_ERR(odev->dev)) {
+		retval = PTR_ERR(odev->dev);
+		goto err_files;
+	}
+
+	dev_set_drvdata(odev->dev, odev);
+
+	if ( (retval = device_create_file(odev->dev, &dev_attr_enabled))) {
+		goto err_class_enabled;
+	}
+
+	if ( (retval = device_create_file(odev->dev, &dev_attr_picture))) {
+		goto err_class_picture;
+	}
+
+	dev_info(&interface->dev, "Attached Asus OLED device: %s [width %u, pack_mode %d]\n", desc, odev->dev_width, odev->pack_mode);
+
+	if (start_off)
+		enable_oled(odev, 0);
+
+	return 0;
+
+err_class_picture:
+	device_remove_file(odev->dev, &dev_attr_picture);
+
+err_class_enabled:
+	device_remove_file(odev->dev, &dev_attr_enabled);
+	device_unregister(odev->dev);
+
+err_files:
+	device_remove_file(&interface->dev, &ASUS_OLED_DEVICE_ATTR(enabled));
+	device_remove_file(&interface->dev, &ASUS_OLED_DEVICE_ATTR(picture));
+
+	usb_set_intfdata (interface, NULL);
+	usb_put_dev(odev->udev);
+	kfree(odev);
+
+	return retval;
+}
+
+static void asus_oled_disconnect(struct usb_interface *interface)
+{
+	struct asus_oled_dev *odev;
+
+	odev = usb_get_intfdata (interface);
+	usb_set_intfdata (interface, NULL);
+
+	device_remove_file(odev->dev, &dev_attr_picture);
+	device_remove_file(odev->dev, &dev_attr_enabled);
+	device_unregister(odev->dev);
+
+	device_remove_file(&interface->dev, & ASUS_OLED_DEVICE_ATTR(picture));
+	device_remove_file(&interface->dev, & ASUS_OLED_DEVICE_ATTR(enabled));
+
+	usb_put_dev(odev->udev);
+
+	if (odev->buf) kfree(odev->buf);
+
+	kfree(odev);
+
+	dev_info(&interface->dev, "Disconnected Asus OLED device\n");
+}
+
+static struct usb_driver oled_driver = {
+	.name =		ASUS_OLED_NAME,
+	.probe =	asus_oled_probe,
+	.disconnect =	asus_oled_disconnect,
+	.id_table =	id_table,
+};
+
+static ssize_t version_show(struct class *dev, char *buf)
+{
+	return sprintf(buf, ASUS_OLED_UNDERSCORE_NAME " %s\n", ASUS_OLED_VERSION);
+}
+
+static CLASS_ATTR(version, S_IRUGO, version_show, NULL);
+
+static int __init asus_oled_init(void)
+{
+	int retval = 0;
+	oled_class = class_create(THIS_MODULE, ASUS_OLED_UNDERSCORE_NAME);
+
+	if (IS_ERR(oled_class)) {
+		err("Error creating " ASUS_OLED_UNDERSCORE_NAME " class");
+		return PTR_ERR(oled_class);
+	}
+
+	if ((retval = class_create_file(oled_class, &class_attr_version))) {
+		err("Error creating class version file");
+		goto error;
+	}
+
+	retval = usb_register(&oled_driver);
+
+	if (retval) {
+		err("usb_register failed. Error number %d", retval);
+		goto error;
+	}
+
+	return retval;
+
+error:
+	class_destroy(oled_class);
+	return retval;
+}
+
+static void __exit asus_oled_exit(void)
+{
+	class_remove_file(oled_class, &class_attr_version);
+	class_destroy(oled_class);
+
+	usb_deregister(&oled_driver);
+}
+
+module_init (asus_oled_init);
+module_exit (asus_oled_exit);
+
diff --git a/drivers/staging/asus_oled/linux.txt b/drivers/staging/asus_oled/linux.txt
new file mode 100644
index 000000000000..dc758b0eb379
--- /dev/null
+++ b/drivers/staging/asus_oled/linux.txt
@@ -0,0 +1,33 @@
+<s:74x32>
+0
+0
+00000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000
+01111111111000000000000000000000000000000000000000000000000000000000000000
+00011111100000000000000111000000000000000000000000000000000000000000000000
+00001111000000000000000111000000000000000000000000000000000000000000000000
+00001111000000000000000111000000000000000000000000000000000000000000000000
+00001111000000000000000000000000000000000000000000000000000000000000000000
+00001111000000000000000000000000000000000000000000000000000000000000000000
+00001111000000000000011100001111111111100000111110011111100011111101111000
+00001111000000000000111110000011111000111000111110000111100001111000110000
+00001111000000000001101110000011111000111000001111000111100000111100100000
+00001111000000000001001110000011110000111100001111000111100000111101100000
+00001111000000000100001110000011110000111100001111000111100000011111000000
+00001111000000000100011110000011110000111100001111000111100000001111000000
+00001111000000000100011110000011110000111100001111000111100000001111000000
+00001111000000000100011100100011110000111100001111000111100000001111100000
+00001111000000001100111100100011110000111100001111000111100000001111110000
+00001111000000001100111101100011110000111100001111000111100000011011110000
+00001111000000011100111101000011110000111100001111000111100000010001111000
+00011111000001111100111011000011110000111100001111001111100000110000111100
+11111111111111111100011110001111111011111110000111110111111011111011111110
+00000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000
+0
+0
+0
+0
diff --git a/drivers/staging/asus_oled/linux_f.txt b/drivers/staging/asus_oled/linux_f.txt
new file mode 100644
index 000000000000..b4bb85cc6ebb
--- /dev/null
+++ b/drivers/staging/asus_oled/linux_f.txt
@@ -0,0 +1,18 @@
+<f:128x16>
+00000000000000000000000000000000001111111100000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000001110000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000001110000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000011000111111111100001111001111100111110111000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000111100001111000110001111000111100011100010000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000001011100001111000111000111100111100001110110000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000011100001110000111000111100111100001111100000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000100011100001110000111000111100111100000111100000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000100011100001110000111000111100111100000111100000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000100111001001110000111000111100111100000111110000000000000000000000000000000000000
+00000000000000000000000000000000000011110000001100111011001110000111000111100111100000111110000000000000000000000000000000000000
+00000000000000000000000000000000000011110000001100111010001110000111000111100111100000100111000000000000000000000000000000000000
+00000000000000000000000000000000000011110000111100110110001110000111000111100111100001000011100000000000000000000000000000000000
+00000000000000000000000000000000001111111111111100111100111111011111100011110111110111101111110000000000000000000000000000000000
+
diff --git a/drivers/staging/asus_oled/linux_fr.txt b/drivers/staging/asus_oled/linux_fr.txt
new file mode 100644
index 000000000000..f88e2b3bdd1b
--- /dev/null
+++ b/drivers/staging/asus_oled/linux_fr.txt
@@ -0,0 +1,33 @@
+<f:128x32>
+00000000000000000000000000000000001111111100000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000001110000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000001110000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000011000111111111100001111001111100111110111000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000111100001111000110001111000111100011100010000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000001011100001111000111000111100111100001110110000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000011100001110000111000111100111100001111100000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000100011100001110000111000111100111100000111100000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000100011100001110000111000111100111100000111100000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000100111001001110000111000111100111100000111110000000000000000000000000000000000000
+00000000000000000000000000000000000011110000001100111011001110000111000111100111100000111110000000000000000000000000000000000000
+00000000000000000000000000000000000011110000001100111010001110000111000111100111100000100111000000000000000000000000000000000000
+00000000000000000000000000000000000011110000111100110110001110000111000111100111100001000011100000000000000000000000000000000000
+00000000000000000000000000000000001111111111111100111100111111011111100011110111110111101111110000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000110000000000000000000000000000000000000000000000000000000000000000110000000000000000000000000000000
+00000000000000000000000000001111000000000000000000000000000000000000000000000000000000000000001111000000000000000000000000000000
+00000000000000000000000000011111100000000000000000000000000000000000000000000000000000000000011111100000000000000000000000000000
+00000000000000000000000000011111100000000000000000000000000000000000000000000000000000000000011111100000000000000000000000000000
+00000000000000000000000000001111000000000000000000000000000000000000000000000000000000000000001111000000000000000000000000000000
+00000000000000000000000000000110000000000000000000000000000000000000000000000000000000000000000110000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
diff --git a/drivers/staging/asus_oled/tux.txt b/drivers/staging/asus_oled/tux.txt
new file mode 100644
index 000000000000..9d2052854b64
--- /dev/null
+++ b/drivers/staging/asus_oled/tux.txt
@@ -0,0 +1,33 @@
+<s:32x32>
+00000000000001111111000000000000
+0000000000001       100000000000
+000000000001         10000000000
+000000000001         10000000000
+000000000001         10000000000
+000000000001 1  111  10000000000
+000000000001    1 1   1000000000
+000000000001  111     1000000000
+000000000001 111111   1000000000
+000000000001 111111   1000000000
+000000000001    1 1    100000000
+00000000001      11    100000000
+00000000001 11111111    10000000
+0000000001  11111111     1000000
+000000001   111111111    1000000
+000000001  1111111111     100000
+00000001   11111111111    100000
+00000001  111111111111     10000
+0000001   111111111111     10000
+0000001   111111111111     10000
+0000001   111111111111     10000
+0000001   111111111111     10000
+000000011 11111111111      10000
+000011 11  11111111111    100000
+0001  1111  111111111111111 1000
+001 1111111  11111111111111 1000
+001 1111111  1111111  111111 100
+001 11111111 111111   1111111 10
+001 11111111          11111  100
+001  1111111          111  11100
+000111   111   11111  11  100000
+000000111   111111111    1000000
diff --git a/drivers/staging/asus_oled/tux_r.txt b/drivers/staging/asus_oled/tux_r.txt
new file mode 100644
index 000000000000..fd81a3e84949
--- /dev/null
+++ b/drivers/staging/asus_oled/tux_r.txt
@@ -0,0 +1,33 @@
+<r:32x32>
+00000000000001111111000000000000
+0000000000001       100000000000
+000000000001         10000000000
+000000000001         10000000000
+000000000001         10000000000
+000000000001 1  111  10000000000
+000000000001    1 1   1000000000
+000000000001  111     1000000000
+000000000001 111111   1000000000
+000000000001 111111   1000000000
+000000000001    1 1    100000000
+00000000001      11    100000000
+00000000001 11111111    10000000
+0000000001  11111111     1000000
+000000001   111111111    1000000
+000000001  1111111111     100000
+00000001   11111111111    100000
+00000001  111111111111     10000
+0000001   111111111111     10000
+0000001   111111111111     10000
+0000001   111111111111     10000
+0000001   111111111111     10000
+000000011 11111111111      10000
+000011 11  11111111111    100000
+0001  1111  111111111111111 1000
+001 1111111  11111111111111 1000
+001 1111111  1111111  111111 100
+001 11111111 111111   1111111 10
+001 11111111          11111  100
+001  1111111          111  11100
+000111   111   11111  11  100000
+000000111   111111111    1000000
diff --git a/drivers/staging/asus_oled/tux_r2.txt b/drivers/staging/asus_oled/tux_r2.txt
new file mode 100644
index 000000000000..e94d84eaab0b
--- /dev/null
+++ b/drivers/staging/asus_oled/tux_r2.txt
@@ -0,0 +1,33 @@
+<r:256x32>
+000000000000000000000000000000000000000000000000000000000000011111110000000000000000000000000000000000000000000000000000000000000
+0000000000000000000000000000000000000000000000000000000000001       1000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001         1000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001         1000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001         1000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001 1  111  1000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001    1 1   100000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001  111     100000000000000000000000000000000000000000000000000000000000000000000000000000000000000011111100000000000000111000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001 111111   100000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000000000111000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001 111111   100000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000000000111000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001    1 1    10000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000001      11    10000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000001 11111111    1000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000000011100001111111111100000111110011111100011111101111000000000000000000000000000000
+0000000000000000000000000000000000000000000000000000000001  11111111     100000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000000111110000011111000111000111110000111100001111000110000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000001   111111111    100000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000001101110000011111000111000001111000111100000111100100000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000001  1111111111     10000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000001001110000011110000111100001111000111100000111101100000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000001   11111111111    10000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000100001110000011110000111100001111000111100000011111000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000001  111111111111     10000000000000000000000000000000000000000000000000000000000000000000000000000000000011110000000001000111100000111100001111000011110001111000000011110000000
+0000000000000000000000000000000000000000000000000000001   111111111111     1000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000100011110000011110000111100001111000111100000001111000000
+0000000000000000000000000000000000000000000000000000001   111111111111     1000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000100011100100011110000111100001111000111100000001111100000000000000000000000000000000
+0000000000000000000000000000000000000000000000000000001   111111111111     1000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000001100111100100011110000111100001111000111100000001111110000000000000000000000000000000
+0000000000000000000000000000000000000000000000000000001   111111111111     1000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000001100111101100011110000111100001111000111100000011011110000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000011 11111111111      1000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000011100111101000011110000111100001111000111100000010001111000000000000000000000000000000
+000000000000000000000000000000000000000000000000000011 11  11111111111    10000000000000000000000000000000000000000000000000000000000000000000000000000000000011111000001111100111011000011110000111100001111001111100000110000111100000000000000000000000000000
+0000000000000000000000000000000000000000000000000001  1111  111111111111111 100000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111100011110001111111011111110000111110111111011111011111110000000000000000000000000000
+000000000000000000000000000000000000000000000000001 1111111  11111111111111 100000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000001 1111111  1111111  111111 10000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000001 11111111 111111   1111111 1000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000001 11111111          11111  1000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000001  1111111          111  111000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000111   111   11111  11  10
+000000000000000000000000000000000000000000000000000000111   111111111    10
diff --git a/drivers/staging/asus_oled/zig.txt b/drivers/staging/asus_oled/zig.txt
new file mode 100644
index 000000000000..31573d8f799a
--- /dev/null
+++ b/drivers/staging/asus_oled/zig.txt
@@ -0,0 +1,33 @@
+<r:128x32>
+10000000000000000000000000000000000000000000000000000000000000011000000000000000000000000000000000000000000000000000000000000001
+01000000000000000000000000000000000000000000000000000000000000100100000000000000000000000000000000000000000000000000000000000010
+00100000000000000000000000000000000000000000000000000000000001000010000000000000000000000000000000000000000000000000000000000100
+00010000000000000000000000000000000000000000000000000000000010000001000000000000000000000000000000000000000000000000000000001000
+00001000000000000000000000000000000000000000000000000000000100000000100000000000000000000000000000000000000000000000000000010000
+00000100000000000000000000000000000000000000000000000000001000000000010000000000000000000000000000000000000000000000000000100000
+00000010000000000000000000000000000000000000000000000000010000000000001000000000000000000000000000000000000000000000000001000000
+00000001000000000000000000000000000000000000000000000000100000000000000100000000000000000000000000000000000000000000000010000000
+00000000100000000000000000000000000000000000000000000001000000000000000010000000000000000000000000000000000000000000000100000000
+00000000010000000000000000000000000000000000000000000010000000000000000001000000000000000000000000000000000000000000001000000000
+00000000001000000000000000000000000000000000000000000100000000000000000000100000000000000000000000000000000000000000010000000000
+00000000000100000000000000000000000000000000000000001000000000000000000000010000000000000000000000000000000000000000100000000000
+00000000000010000000000000000000000000000000000000010000000000000000000000001000000000000000000000000000000000000001000000000000
+00000000000001000000000000000000000000000000000000100000000000000000000000000100000000000000000000000000000000000010000000000000
+00000000000000100000000000000000000000000000000001000000000000000000000000000010000000000000000000000000000000000100000000000000
+00000000000000010000000000000000000000000000000010000000000000000000000000000001000000000000000000000000000000001000000000000000
+00000000000000001000000000000000000000000000000100000000000000000000000000000000100000000000000000000000000000010000000000000000
+00000000000000000100000000000000000000000000001000000000000000000000000000000000010000000000000000000000000000100000000000000000
+00000000000000000010000000000000000000000000010000000000000000000000000000000000001000000000000000000000000001000000000000000000
+00000000000000000001000000000000000000000000100000000000000000000000000000000000000100000000000000000000000010000000000000000000
+00000000000000000000100000000000000000000001000000000000000000000000000000000000000010000000000000000000000100000000000000000000
+00000000000000000000010000000000000000000010000000000000000000000000000000000000000001000000000000000000001000000000000000000000
+00000000000000000000001000000000000000000100000000000000000000000000000000000000000000100000000000000000010000000000000000000000
+00000000000000000000000100000000000000001000000000000000000000000000000000000000000000010000000000000000100000000000000000000000
+00000000000000000000000010000000000000010000000000000000000000000000000000000000000000001000000000000001000000000000000000000000
+00000000000000000000000001000000000000100000000000000000000000000000000000000000000000000100000000000010000000000000000000000000
+00000000000000000000000000100000000001000000000000000000000000000000000000000000000000000010000000000100000000000000000000000000
+00000000000000000000000000010000000010000000000000000000000000000000000000000000000000000001000000001000000000000000000000000000
+00000000000000000000000000001000000100000000000000000000000000000000000000000000000000000000100000010000000000000000000000000000
+00000000000000000000000000000100001000000000000000000000000000000000000000000000000000000000010000100000000000000000000000000000
+00000000000000000000000000000010010000000000000000000000000000000000000000000000000000000000001001000000000000000000000000000000
+00000000000000000000000000000001100000000000000000000000000000000000000000000000000000000000000110000000000000000000000000000000
diff --git a/drivers/staging/at76_usb/Kconfig b/drivers/staging/at76_usb/Kconfig
index 8606f9621624..4c0e55e15448 100644
--- a/drivers/staging/at76_usb/Kconfig
+++ b/drivers/staging/at76_usb/Kconfig
@@ -1,6 +1,6 @@
 config USB_ATMEL
 	tristate "Atmel at76c503/at76c505/at76c505a USB cards"
-	depends on WLAN_80211 && USB
+	depends on MAC80211 && WLAN_80211 && USB
 	default N
 	select FW_LOADER
 	---help---
diff --git a/drivers/staging/at76_usb/at76_usb.c b/drivers/staging/at76_usb/at76_usb.c
index 174e2bec9223..185533e54769 100644
--- a/drivers/staging/at76_usb/at76_usb.c
+++ b/drivers/staging/at76_usb/at76_usb.c
@@ -6,6 +6,7 @@
  * Copyright (c) 2004 Nick Jones
  * Copyright (c) 2004 Balint Seeber <n0_5p4m_p13453@hotmail.com>
  * Copyright (c) 2007 Guido Guenther <agx@sigxcpu.org>
+ * Copyright (c) 2007 Kalle Valo <kalle.valo@iki.fi>
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License as
@@ -16,6 +17,13 @@
  * Atmel AT76C503A/505/505A.
  *
  * Some iw_handler code was taken from airo.c, (C) 1999 Benjamin Reed
+ *
+ * TODO for the mac80211 port:
+ * o adhoc support
+ * o RTS/CTS support
+ * o Power Save Mode support
+ * o support for short/long preambles
+ * o export variables through debugfs/sysfs
  */
 
 #include <linux/init.h>
@@ -36,7 +44,7 @@
 #include <net/ieee80211_radiotap.h>
 #include <linux/firmware.h>
 #include <linux/leds.h>
-#include <net/ieee80211.h>
+#include <net/mac80211.h>
 
 #include "at76_usb.h"
 
@@ -76,31 +84,43 @@
 #define DBG_WE_EVENTS		0x08000000	/* dump wireless events */
 #define DBG_FW			0x10000000	/* firmware download */
 #define DBG_DFU			0x20000000	/* device firmware upgrade */
+#define DBG_CMD			0x40000000
+#define DBG_MAC80211		0x80000000
 
 #define DBG_DEFAULTS		0
 
 /* Use our own dbg macro */
 #define at76_dbg(bits, format, arg...) \
-	do { \
-		if (at76_debug & (bits)) \
+do {	\
+	if (at76_debug & (bits))	\
+		printk(KERN_DEBUG DRIVER_NAME ": " format "\n" , ## arg); \
+} while (0)
+
+#define at76_dbg_dump(bits, buf, len, format, arg...)	\
+do {	\
+	if (at76_debug & (bits)) {	\
 		printk(KERN_DEBUG DRIVER_NAME ": " format "\n" , ## arg); \
-	} while (0)
+		print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, buf, len);	\
+		}							\
+} while (0)
 
 static int at76_debug = DBG_DEFAULTS;
 
+#define FIRMWARE_IS_WPA(ver) ((ver.major == 1) && (ver.minor == 103))
+
 /* Protect against concurrent firmware loading and parsing */
 static struct mutex fw_mutex;
 
 static struct fwentry firmwares[] = {
-	[0] = {""},
-	[BOARD_503_ISL3861] = {"atmel_at76c503-i3861.bin"},
-	[BOARD_503_ISL3863] = {"atmel_at76c503-i3863.bin"},
-	[BOARD_503] = {"atmel_at76c503-rfmd.bin"},
-	[BOARD_503_ACC] = {"atmel_at76c503-rfmd-acc.bin"},
-	[BOARD_505] = {"atmel_at76c505-rfmd.bin"},
-	[BOARD_505_2958] = {"atmel_at76c505-rfmd2958.bin"},
-	[BOARD_505A] = {"atmel_at76c505a-rfmd2958.bin"},
-	[BOARD_505AMX] = {"atmel_at76c505amx-rfmd.bin"},
+	[0] = { "" },
+	[BOARD_503_ISL3861] = { "atmel_at76c503-i3861.bin" },
+	[BOARD_503_ISL3863] = { "atmel_at76c503-i3863.bin" },
+	[BOARD_503] = { "atmel_at76c503-rfmd.bin" },
+	[BOARD_503_ACC] = { "atmel_at76c503-rfmd-acc.bin" },
+	[BOARD_505] = { "atmel_at76c505-rfmd.bin" },
+	[BOARD_505_2958] = { "atmel_at76c505-rfmd2958.bin" },
+	[BOARD_505A] = { "atmel_at76c505a-rfmd2958.bin" },
+	[BOARD_505AMX] = { "atmel_at76c505amx-rfmd.bin" },
 };
 
 #define USB_DEVICE_DATA(__ops)	.driver_info = (kernel_ulong_t)(__ops)
@@ -110,133 +130,133 @@ static struct usb_device_id dev_table[] = {
 	 * at76c503-i3861
 	 */
 	/* Generic AT76C503/3861 device */
-	{USB_DEVICE(0x03eb, 0x7603), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x03eb, 0x7603), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Linksys WUSB11 v2.1/v2.6 */
-	{USB_DEVICE(0x066b, 0x2211), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x066b, 0x2211), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Netgear MA101 rev. A */
-	{USB_DEVICE(0x0864, 0x4100), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x0864, 0x4100), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Tekram U300C / Allnet ALL0193 */
-	{USB_DEVICE(0x0b3b, 0x1612), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x0b3b, 0x1612), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* HP HN210W J7801A */
-	{USB_DEVICE(0x03f0, 0x011c), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x03f0, 0x011c), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Sitecom/Z-Com/Zyxel M4Y-750 */
-	{USB_DEVICE(0x0cde, 0x0001), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x0cde, 0x0001), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Dynalink/Askey WLL013 (intersil) */
-	{USB_DEVICE(0x069a, 0x0320), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x069a, 0x0320), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* EZ connect 11Mpbs Wireless USB Adapter SMC2662W v1 */
-	{USB_DEVICE(0x0d5c, 0xa001), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x0d5c, 0xa001), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* BenQ AWL300 */
-	{USB_DEVICE(0x04a5, 0x9000), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x04a5, 0x9000), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Addtron AWU-120, Compex WLU11 */
-	{USB_DEVICE(0x05dd, 0xff31), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x05dd, 0xff31), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Intel AP310 AnyPoint II USB */
-	{USB_DEVICE(0x8086, 0x0200), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x8086, 0x0200), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Dynalink L11U */
-	{USB_DEVICE(0x0d8e, 0x7100), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x0d8e, 0x7100), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Arescom WL-210, FCC id 07J-GL2411USB */
-	{USB_DEVICE(0x0d8e, 0x7110), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x0d8e, 0x7110), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* I-O DATA WN-B11/USB */
-	{USB_DEVICE(0x04bb, 0x0919), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x04bb, 0x0919), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* BT Voyager 1010 */
-	{USB_DEVICE(0x069a, 0x0821), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x069a, 0x0821), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/*
 	 * at76c503-i3863
 	 */
 	/* Generic AT76C503/3863 device */
-	{USB_DEVICE(0x03eb, 0x7604), USB_DEVICE_DATA(BOARD_503_ISL3863)},
+	{ USB_DEVICE(0x03eb, 0x7604), USB_DEVICE_DATA(BOARD_503_ISL3863) },
 	/* Samsung SWL-2100U */
-	{USB_DEVICE(0x055d, 0xa000), USB_DEVICE_DATA(BOARD_503_ISL3863)},
+	{ USB_DEVICE(0x055d, 0xa000), USB_DEVICE_DATA(BOARD_503_ISL3863) },
 	/*
 	 * at76c503-rfmd
 	 */
 	/* Generic AT76C503/RFMD device */
-	{USB_DEVICE(0x03eb, 0x7605), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x03eb, 0x7605), USB_DEVICE_DATA(BOARD_503) },
 	/* Dynalink/Askey WLL013 (rfmd) */
-	{USB_DEVICE(0x069a, 0x0321), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x069a, 0x0321), USB_DEVICE_DATA(BOARD_503) },
 	/* Linksys WUSB11 v2.6 */
-	{USB_DEVICE(0x077b, 0x2219), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x077b, 0x2219), USB_DEVICE_DATA(BOARD_503) },
 	/* Network Everywhere NWU11B */
-	{USB_DEVICE(0x077b, 0x2227), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x077b, 0x2227), USB_DEVICE_DATA(BOARD_503) },
 	/* Netgear MA101 rev. B */
-	{USB_DEVICE(0x0864, 0x4102), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x0864, 0x4102), USB_DEVICE_DATA(BOARD_503) },
 	/* D-Link DWL-120 rev. E */
-	{USB_DEVICE(0x2001, 0x3200), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x2001, 0x3200), USB_DEVICE_DATA(BOARD_503) },
 	/* Actiontec 802UAT1, HWU01150-01UK */
-	{USB_DEVICE(0x1668, 0x7605), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x1668, 0x7605), USB_DEVICE_DATA(BOARD_503) },
 	/* AirVast W-Buddie WN210 */
-	{USB_DEVICE(0x03eb, 0x4102), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x03eb, 0x4102), USB_DEVICE_DATA(BOARD_503) },
 	/* Dick Smith Electronics XH1153 802.11b USB adapter */
-	{USB_DEVICE(0x1371, 0x5743), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x1371, 0x5743), USB_DEVICE_DATA(BOARD_503) },
 	/* CNet CNUSB611 */
-	{USB_DEVICE(0x1371, 0x0001), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x1371, 0x0001), USB_DEVICE_DATA(BOARD_503) },
 	/* FiberLine FL-WL200U */
-	{USB_DEVICE(0x1371, 0x0002), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x1371, 0x0002), USB_DEVICE_DATA(BOARD_503) },
 	/* BenQ AWL400 USB stick */
-	{USB_DEVICE(0x04a5, 0x9001), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x04a5, 0x9001), USB_DEVICE_DATA(BOARD_503) },
 	/* 3Com 3CRSHEW696 */
-	{USB_DEVICE(0x0506, 0x0a01), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x0506, 0x0a01), USB_DEVICE_DATA(BOARD_503) },
 	/* Siemens Santis ADSL WLAN USB adapter WLL 013 */
-	{USB_DEVICE(0x0681, 0x001b), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x0681, 0x001b), USB_DEVICE_DATA(BOARD_503) },
 	/* Belkin F5D6050, version 2 */
-	{USB_DEVICE(0x050d, 0x0050), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x050d, 0x0050), USB_DEVICE_DATA(BOARD_503) },
 	/* iBlitzz, BWU613 (not *B or *SB) */
-	{USB_DEVICE(0x07b8, 0xb000), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x07b8, 0xb000), USB_DEVICE_DATA(BOARD_503) },
 	/* Gigabyte GN-WLBM101 */
-	{USB_DEVICE(0x1044, 0x8003), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x1044, 0x8003), USB_DEVICE_DATA(BOARD_503) },
 	/* Planex GW-US11S */
-	{USB_DEVICE(0x2019, 0x3220), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x2019, 0x3220), USB_DEVICE_DATA(BOARD_503) },
 	/* Internal WLAN adapter in h5[4,5]xx series iPAQs */
-	{USB_DEVICE(0x049f, 0x0032), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x049f, 0x0032), USB_DEVICE_DATA(BOARD_503) },
 	/* Corega Wireless LAN USB-11 mini */
-	{USB_DEVICE(0x07aa, 0x0011), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x07aa, 0x0011), USB_DEVICE_DATA(BOARD_503) },
 	/* Corega Wireless LAN USB-11 mini2 */
-	{USB_DEVICE(0x07aa, 0x0018), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x07aa, 0x0018), USB_DEVICE_DATA(BOARD_503) },
 	/* Uniden PCW100 */
-	{USB_DEVICE(0x05dd, 0xff35), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x05dd, 0xff35), USB_DEVICE_DATA(BOARD_503) },
 	/*
 	 * at76c503-rfmd-acc
 	 */
 	/* SMC2664W */
-	{USB_DEVICE(0x083a, 0x3501), USB_DEVICE_DATA(BOARD_503_ACC)},
+	{ USB_DEVICE(0x083a, 0x3501), USB_DEVICE_DATA(BOARD_503_ACC) },
 	/* Belkin F5D6050, SMC2662W v2, SMC2662W-AR */
-	{USB_DEVICE(0x0d5c, 0xa002), USB_DEVICE_DATA(BOARD_503_ACC)},
+	{ USB_DEVICE(0x0d5c, 0xa002), USB_DEVICE_DATA(BOARD_503_ACC) },
 	/*
 	 * at76c505-rfmd
 	 */
 	/* Generic AT76C505/RFMD */
-	{USB_DEVICE(0x03eb, 0x7606), USB_DEVICE_DATA(BOARD_505)},
+	{ USB_DEVICE(0x03eb, 0x7606), USB_DEVICE_DATA(BOARD_505) },
 	/*
 	 * at76c505-rfmd2958
 	 */
 	/* Generic AT76C505/RFMD, OvisLink WL-1130USB */
-	{USB_DEVICE(0x03eb, 0x7613), USB_DEVICE_DATA(BOARD_505_2958)},
+	{ USB_DEVICE(0x03eb, 0x7613), USB_DEVICE_DATA(BOARD_505_2958) },
 	/* Fiberline FL-WL240U */
-	{USB_DEVICE(0x1371, 0x0014), USB_DEVICE_DATA(BOARD_505_2958)},
+	{ USB_DEVICE(0x1371, 0x0014), USB_DEVICE_DATA(BOARD_505_2958) },
 	/* CNet CNUSB-611G */
-	{USB_DEVICE(0x1371, 0x0013), USB_DEVICE_DATA(BOARD_505_2958)},
+	{ USB_DEVICE(0x1371, 0x0013), USB_DEVICE_DATA(BOARD_505_2958) },
 	/* Linksys WUSB11 v2.8 */
-	{USB_DEVICE(0x1915, 0x2233), USB_DEVICE_DATA(BOARD_505_2958)},
+	{ USB_DEVICE(0x1915, 0x2233), USB_DEVICE_DATA(BOARD_505_2958) },
 	/* Xterasys XN-2122B, IBlitzz BWU613B/BWU613SB */
-	{USB_DEVICE(0x12fd, 0x1001), USB_DEVICE_DATA(BOARD_505_2958)},
+	{ USB_DEVICE(0x12fd, 0x1001), USB_DEVICE_DATA(BOARD_505_2958) },
 	/* Corega WLAN USB Stick 11 */
-	{USB_DEVICE(0x07aa, 0x7613), USB_DEVICE_DATA(BOARD_505_2958)},
+	{ USB_DEVICE(0x07aa, 0x7613), USB_DEVICE_DATA(BOARD_505_2958) },
 	/* Microstar MSI Box MS6978 */
-	{USB_DEVICE(0x0db0, 0x1020), USB_DEVICE_DATA(BOARD_505_2958)},
+	{ USB_DEVICE(0x0db0, 0x1020), USB_DEVICE_DATA(BOARD_505_2958) },
 	/*
 	 * at76c505a-rfmd2958
 	 */
 	/* Generic AT76C505A device */
-	{USB_DEVICE(0x03eb, 0x7614), USB_DEVICE_DATA(BOARD_505A)},
+	{ USB_DEVICE(0x03eb, 0x7614), USB_DEVICE_DATA(BOARD_505A) },
 	/* Generic AT76C505AS device */
-	{USB_DEVICE(0x03eb, 0x7617), USB_DEVICE_DATA(BOARD_505A)},
+	{ USB_DEVICE(0x03eb, 0x7617), USB_DEVICE_DATA(BOARD_505A) },
 	/* Siemens Gigaset USB WLAN Adapter 11 */
-	{USB_DEVICE(0x1690, 0x0701), USB_DEVICE_DATA(BOARD_505A)},
+	{ USB_DEVICE(0x1690, 0x0701), USB_DEVICE_DATA(BOARD_505A) },
 	/*
 	 * at76c505amx-rfmd
 	 */
 	/* Generic AT76C505AMX device */
-	{USB_DEVICE(0x03eb, 0x7615), USB_DEVICE_DATA(BOARD_505AMX)},
-	{}
+	{ USB_DEVICE(0x03eb, 0x7615), USB_DEVICE_DATA(BOARD_505AMX) },
+	{ }
 };
 
 MODULE_DEVICE_TABLE(usb, dev_table);
@@ -244,26 +264,8 @@ MODULE_DEVICE_TABLE(usb, dev_table);
 /* Supported rates of this hardware, bit 7 marks basic rates */
 static const u8 hw_rates[] = { 0x82, 0x84, 0x0b, 0x16 };
 
-/* Frequency of each channel in MHz */
-static const long channel_frequency[] = {
-	2412, 2417, 2422, 2427, 2432, 2437, 2442,
-	2447, 2452, 2457, 2462, 2467, 2472, 2484
-};
-
-#define NUM_CHANNELS ARRAY_SIZE(channel_frequency)
-
 static const char *const preambles[] = { "long", "short", "auto" };
 
-static const char *const mac_states[] = {
-	[MAC_INIT] = "INIT",
-	[MAC_SCANNING] = "SCANNING",
-	[MAC_AUTH] = "AUTH",
-	[MAC_ASSOC] = "ASSOC",
-	[MAC_JOINING] = "JOINING",
-	[MAC_CONNECTED] = "CONNECTED",
-	[MAC_OWN_IBSS] = "OWN_IBSS"
-};
-
 /* Firmware download */
 /* DFU states */
 #define STATE_IDLE			0x00
@@ -298,17 +300,30 @@ struct dfu_status {
 
 static inline int at76_is_intersil(enum board_type board)
 {
-	return (board == BOARD_503_ISL3861 || board == BOARD_503_ISL3863);
+	if (board == BOARD_503_ISL3861 || board == BOARD_503_ISL3863)
+		return 1;
+	return 0;
 }
 
 static inline int at76_is_503rfmd(enum board_type board)
 {
-	return (board == BOARD_503 || board == BOARD_503_ACC);
+	if (board == BOARD_503 || board == BOARD_503_ACC)
+		return 1;
+	return 0;
+}
+
+static inline int at76_is_505(enum board_type board)
+{
+	if (board == BOARD_505 || board == BOARD_505_2958)
+		return 1;
+	return 0;
 }
 
 static inline int at76_is_505a(enum board_type board)
 {
-	return (board == BOARD_505A || board == BOARD_505AMX);
+	if (board == BOARD_505A || board == BOARD_505AMX)
+		return 1;
+	return 0;
 }
 
 /* Load a block of the first (internal) part of the firmware */
@@ -489,41 +504,6 @@ exit:
 	return ret;
 }
 
-/* Report that the scan results are ready */
-static inline void at76_iwevent_scan_complete(struct net_device *netdev)
-{
-	union iwreq_data wrqu;
-	wrqu.data.length = 0;
-	wrqu.data.flags = 0;
-	wireless_send_event(netdev, SIOCGIWSCAN, &wrqu, NULL);
-	at76_dbg(DBG_WE_EVENTS, "%s: SIOCGIWSCAN sent", netdev->name);
-}
-
-static inline void at76_iwevent_bss_connect(struct net_device *netdev,
-					    u8 *bssid)
-{
-	union iwreq_data wrqu;
-	wrqu.data.length = 0;
-	wrqu.data.flags = 0;
-	memcpy(wrqu.ap_addr.sa_data, bssid, ETH_ALEN);
-	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-	wireless_send_event(netdev, SIOCGIWAP, &wrqu, NULL);
-	at76_dbg(DBG_WE_EVENTS, "%s: %s: SIOCGIWAP sent", netdev->name,
-		 __func__);
-}
-
-static inline void at76_iwevent_bss_disconnect(struct net_device *netdev)
-{
-	union iwreq_data wrqu;
-	wrqu.data.length = 0;
-	wrqu.data.flags = 0;
-	memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
-	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-	wireless_send_event(netdev, SIOCGIWAP, &wrqu, NULL);
-	at76_dbg(DBG_WE_EVENTS, "%s: %s: SIOCGIWAP sent", netdev->name,
-		 __func__);
-}
-
 #define HEX2STR_BUFFERS 4
 #define HEX2STR_MAX_LEN 64
 #define BIN2HEX(x) ((x) < 10 ? '0' + (x) : (x) + 'A' - 10)
@@ -595,37 +575,6 @@ static void at76_ledtrig_tx_activity(void)
 		mod_timer(&ledtrig_tx_timer, jiffies + HZ / 4);
 }
 
-/* Check if the given ssid is hidden */
-static inline int at76_is_hidden_ssid(u8 *ssid, int length)
-{
-	static const u8 zeros[32];
-
-	if (length == 0)
-		return 1;
-
-	if (length == 1 && ssid[0] == ' ')
-		return 1;
-
-	return (memcmp(ssid, zeros, length) == 0);
-}
-
-static inline void at76_free_bss_list(struct at76_priv *priv)
-{
-	struct list_head *next, *ptr;
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->bss_list_spinlock, flags);
-
-	priv->curr_bss = NULL;
-
-	list_for_each_safe(ptr, next, &priv->bss_list) {
-		list_del(ptr);
-		kfree(list_entry(ptr, struct bss_info, list));
-	}
-
-	spin_unlock_irqrestore(&priv->bss_list_spinlock, flags);
-}
-
 static int at76_remap(struct usb_device *udev)
 {
 	int ret;
@@ -641,18 +590,25 @@ static int at76_remap(struct usb_device *udev)
 static int at76_get_op_mode(struct usb_device *udev)
 {
 	int ret;
-	u8 op_mode;
+	u8 saved;
+	u8 *op_mode;
 
+	op_mode = kmalloc(1, GFP_NOIO);
+	if (!op_mode)
+		return -ENOMEM;
 	ret = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x33,
 			      USB_TYPE_VENDOR | USB_DIR_IN |
 			      USB_RECIP_INTERFACE, 0x01, 0, &op_mode, 1,
 			      USB_CTRL_GET_TIMEOUT);
+	saved = *op_mode;
+	kfree(op_mode);
+
 	if (ret < 0)
 		return ret;
 	else if (ret < 1)
 		return -EIO;
 	else
-		return op_mode;
+		return saved;
 }
 
 /* Load a block of the second ("external") part of the firmware */
@@ -720,7 +676,7 @@ exit:
 	kfree(hwcfg);
 	if (ret < 0)
 		printk(KERN_ERR "%s: cannot get HW Config (error %d)\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	return ret;
 }
@@ -729,15 +685,15 @@ static struct reg_domain const *at76_get_reg_domain(u16 code)
 {
 	int i;
 	static struct reg_domain const fd_tab[] = {
-		{0x10, "FCC (USA)", 0x7ff},	/* ch 1-11 */
-		{0x20, "IC (Canada)", 0x7ff},	/* ch 1-11 */
-		{0x30, "ETSI (most of Europe)", 0x1fff},	/* ch 1-13 */
-		{0x31, "Spain", 0x600},	/* ch 10-11 */
-		{0x32, "France", 0x1e00},	/* ch 10-13 */
-		{0x40, "MKK (Japan)", 0x2000},	/* ch 14 */
-		{0x41, "MKK1 (Japan)", 0x3fff},	/* ch 1-14 */
-		{0x50, "Israel", 0x3fc},	/* ch 3-9 */
-		{0x00, "<unknown>", 0xffffffff}	/* ch 1-32 */
+		{ 0x10, "FCC (USA)", 0x7ff },	/* ch 1-11 */
+		{ 0x20, "IC (Canada)", 0x7ff },	/* ch 1-11 */
+		{ 0x30, "ETSI (most of Europe)", 0x1fff },	/* ch 1-13 */
+		{ 0x31, "Spain", 0x600 },	/* ch 10-11 */
+		{ 0x32, "France", 0x1e00 },	/* ch 10-13 */
+		{ 0x40, "MKK (Japan)", 0x2000 },	/* ch 14 */
+		{ 0x41, "MKK1 (Japan)", 0x3fff },	/* ch 1-14 */
+		{ 0x50, "Israel", 0x3fc },	/* ch 3-9 */
+		{ 0x00, "<unknown>", 0xffffffff }	/* ch 1-32 */
 	};
 
 	/* Last entry is fallback for unknown domain code */
@@ -765,20 +721,43 @@ static inline int at76_get_mib(struct usb_device *udev, u16 mib, void *buf,
 /* Return positive number for status, negative for an error */
 static inline int at76_get_cmd_status(struct usb_device *udev, u8 cmd)
 {
-	u8 stat_buf[40];
+	u8 *stat_buf;
 	int ret;
 
+	stat_buf = kmalloc(40, GFP_NOIO);
+	if (!stat_buf)
+		return -ENOMEM;
+
 	ret = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x22,
 			      USB_TYPE_VENDOR | USB_DIR_IN |
 			      USB_RECIP_INTERFACE, cmd, 0, stat_buf,
 			      sizeof(stat_buf), USB_CTRL_GET_TIMEOUT);
-	if (ret < 0)
-		return ret;
+	if (ret >= 0)
+		ret = stat_buf[5];
+	kfree(stat_buf);
 
-	return stat_buf[5];
+	return ret;
 }
 
-static int at76_set_card_command(struct usb_device *udev, int cmd, void *buf,
+#define MAKE_CMD_CASE(c) case (c): return #c
+
+static const char *at76_get_cmd_string(u8 cmd_status)
+{
+	switch (cmd_status) {
+		MAKE_CMD_CASE(CMD_SET_MIB);
+		MAKE_CMD_CASE(CMD_GET_MIB);
+		MAKE_CMD_CASE(CMD_SCAN);
+		MAKE_CMD_CASE(CMD_JOIN);
+		MAKE_CMD_CASE(CMD_START_IBSS);
+		MAKE_CMD_CASE(CMD_RADIO_ON);
+		MAKE_CMD_CASE(CMD_RADIO_OFF);
+		MAKE_CMD_CASE(CMD_STARTUP);
+	}
+
+	return "UNKNOWN";
+}
+
+static int at76_set_card_command(struct usb_device *udev, u8 cmd, void *buf,
 				 int buf_size)
 {
 	int ret;
@@ -793,6 +772,10 @@ static int at76_set_card_command(struct usb_device *udev, int cmd, void *buf,
 	cmd_buf->size = cpu_to_le16(buf_size);
 	memcpy(cmd_buf->data, buf, buf_size);
 
+	at76_dbg_dump(DBG_CMD, cmd_buf, sizeof(struct at76_command) + buf_size,
+		      "issuing command %s (0x%02x)",
+		      at76_get_cmd_string(cmd), cmd);
+
 	ret = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x0e,
 			      USB_TYPE_VENDOR | USB_DIR_OUT | USB_RECIP_DEVICE,
 			      0, 0, cmd_buf,
@@ -830,13 +813,13 @@ static int at76_wait_completion(struct at76_priv *priv, int cmd)
 		status = at76_get_cmd_status(priv->udev, cmd);
 		if (status < 0) {
 			printk(KERN_ERR "%s: at76_get_cmd_status failed: %d\n",
-			       priv->netdev->name, status);
+			       wiphy_name(priv->hw->wiphy), status);
 			break;
 		}
 
 		at76_dbg(DBG_WAIT_COMPLETE,
 			 "%s: Waiting on cmd %d, status = %d (%s)",
-			 priv->netdev->name, cmd, status,
+			 wiphy_name(priv->hw->wiphy), cmd, status,
 			 at76_get_cmd_status_string(status));
 
 		if (status != CMD_STATUS_IN_PROGRESS
@@ -847,7 +830,7 @@ static int at76_wait_completion(struct at76_priv *priv, int cmd)
 		if (time_after(jiffies, timeout)) {
 			printk(KERN_ERR
 			       "%s: completion timeout for command %d\n",
-			       priv->netdev->name, cmd);
+			       wiphy_name(priv->hw->wiphy), cmd);
 			status = -ETIMEDOUT;
 			break;
 		}
@@ -870,7 +853,7 @@ static int at76_set_mib(struct at76_priv *priv, struct set_mib_buffer *buf)
 	if (ret != CMD_STATUS_COMPLETE) {
 		printk(KERN_INFO
 		       "%s: set_mib: at76_wait_completion failed "
-		       "with %d\n", priv->netdev->name, ret);
+		       "with %d\n", wiphy_name(priv->hw->wiphy), ret);
 		ret = -EIO;
 	}
 
@@ -891,7 +874,7 @@ static int at76_set_radio(struct at76_priv *priv, int enable)
 	ret = at76_set_card_command(priv->udev, cmd, NULL, 0);
 	if (ret < 0)
 		printk(KERN_ERR "%s: at76_set_card_command(%d) failed: %d\n",
-		       priv->netdev->name, cmd, ret);
+		       wiphy_name(priv->hw->wiphy), cmd, ret);
 	else
 		ret = 1;
 
@@ -912,44 +895,7 @@ static int at76_set_pm_mode(struct at76_priv *priv)
 	ret = at76_set_mib(priv, &priv->mib_buf);
 	if (ret < 0)
 		printk(KERN_ERR "%s: set_mib (pm_mode) failed: %d\n",
-		       priv->netdev->name, ret);
-
-	return ret;
-}
-
-/* Set the association id for power save mode */
-static int at76_set_associd(struct at76_priv *priv, u16 id)
-{
-	int ret = 0;
-
-	priv->mib_buf.type = MIB_MAC_MGMT;
-	priv->mib_buf.size = 2;
-	priv->mib_buf.index = offsetof(struct mib_mac_mgmt, station_id);
-	priv->mib_buf.data.word = cpu_to_le16(id);
-
-	ret = at76_set_mib(priv, &priv->mib_buf);
-	if (ret < 0)
-		printk(KERN_ERR "%s: set_mib (associd) failed: %d\n",
-		       priv->netdev->name, ret);
-
-	return ret;
-}
-
-/* Set the listen interval for power save mode */
-static int at76_set_listen_interval(struct at76_priv *priv, u16 interval)
-{
-	int ret = 0;
-
-	priv->mib_buf.type = MIB_MAC;
-	priv->mib_buf.size = 2;
-	priv->mib_buf.index = offsetof(struct mib_mac, listen_interval);
-	priv->mib_buf.data.word = cpu_to_le16(interval);
-
-	ret = at76_set_mib(priv, &priv->mib_buf);
-	if (ret < 0)
-		printk(KERN_ERR
-		       "%s: set_mib (listen_interval) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	return ret;
 }
@@ -966,7 +912,7 @@ static int at76_set_preamble(struct at76_priv *priv, u8 type)
 	ret = at76_set_mib(priv, &priv->mib_buf);
 	if (ret < 0)
 		printk(KERN_ERR "%s: set_mib (preamble) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	return ret;
 }
@@ -983,7 +929,7 @@ static int at76_set_frag(struct at76_priv *priv, u16 size)
 	ret = at76_set_mib(priv, &priv->mib_buf);
 	if (ret < 0)
 		printk(KERN_ERR "%s: set_mib (frag threshold) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	return ret;
 }
@@ -1000,7 +946,7 @@ static int at76_set_rts(struct at76_priv *priv, u16 size)
 	ret = at76_set_mib(priv, &priv->mib_buf);
 	if (ret < 0)
 		printk(KERN_ERR "%s: set_mib (rts) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	return ret;
 }
@@ -1017,24 +963,41 @@ static int at76_set_autorate_fallback(struct at76_priv *priv, int onoff)
 	ret = at76_set_mib(priv, &priv->mib_buf);
 	if (ret < 0)
 		printk(KERN_ERR "%s: set_mib (autorate fallback) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	return ret;
 }
 
-static int at76_add_mac_address(struct at76_priv *priv, void *addr)
+static int at76_set_tkip_bssid(struct at76_priv *priv, const void *addr)
 {
 	int ret = 0;
 
-	priv->mib_buf.type = MIB_MAC_ADDR;
+	priv->mib_buf.type = MIB_MAC_ENCRYPTION;
 	priv->mib_buf.size = ETH_ALEN;
-	priv->mib_buf.index = offsetof(struct mib_mac_addr, mac_addr);
+	priv->mib_buf.index = offsetof(struct mib_mac_encryption, tkip_bssid);
 	memcpy(priv->mib_buf.data.addr, addr, ETH_ALEN);
 
 	ret = at76_set_mib(priv, &priv->mib_buf);
 	if (ret < 0)
-		printk(KERN_ERR "%s: set_mib (MAC_ADDR, mac_addr) failed: %d\n",
-		       priv->netdev->name, ret);
+		printk(KERN_ERR "%s: set_mib (MAC_ENCRYPTION, tkip_bssid) failed: %d\n",
+		       wiphy_name(priv->hw->wiphy), ret);
+
+	return ret;
+}
+
+static int at76_reset_rsc(struct at76_priv *priv)
+{
+	int ret = 0;
+
+	priv->mib_buf.type = MIB_MAC_ENCRYPTION;
+	priv->mib_buf.size = 4 * 8;
+	priv->mib_buf.index = offsetof(struct mib_mac_encryption, key_rsc);
+	memset(priv->mib_buf.data.data, 0 , priv->mib_buf.size);
+
+	ret = at76_set_mib(priv, &priv->mib_buf);
+	if (ret < 0)
+		printk(KERN_ERR "%s: set_mib (MAC_ENCRYPTION, key_rsc) failed: %d\n",
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	return ret;
 }
@@ -1053,16 +1016,16 @@ static void at76_dump_mib_mac_addr(struct at76_priv *priv)
 			   sizeof(struct mib_mac_addr));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_get_mib (MAC_ADDR) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		goto exit;
 	}
 
 	at76_dbg(DBG_MIB, "%s: MIB MAC_ADDR: mac_addr %s res 0x%x 0x%x",
-		 priv->netdev->name,
+		 wiphy_name(priv->hw->wiphy),
 		 mac2str(m->mac_addr), m->res[0], m->res[1]);
 	for (i = 0; i < ARRAY_SIZE(m->group_addr); i++)
 		at76_dbg(DBG_MIB, "%s: MIB MAC_ADDR: group addr %d: %s, "
-			 "status %d", priv->netdev->name, i,
+			 "status %d", wiphy_name(priv->hw->wiphy), i,
 			 mac2str(m->group_addr[i]), m->group_addr_status[i]);
 exit:
 	kfree(m);
@@ -1082,13 +1045,13 @@ static void at76_dump_mib_mac_wep(struct at76_priv *priv)
 			   sizeof(struct mib_mac_wep));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_get_mib (MAC_WEP) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		goto exit;
 	}
 
 	at76_dbg(DBG_MIB, "%s: MIB MAC_WEP: priv_invoked %u def_key_id %u "
 		 "key_len %u excl_unencr %u wep_icv_err %u wep_excluded %u "
-		 "encr_level %u key %d", priv->netdev->name,
+		 "encr_level %u key %d", wiphy_name(priv->hw->wiphy),
 		 m->privacy_invoked, m->wep_default_key_id,
 		 m->wep_key_mapping_len, m->exclude_unencrypted,
 		 le32_to_cpu(m->wep_icv_error_count),
@@ -1100,12 +1063,55 @@ static void at76_dump_mib_mac_wep(struct at76_priv *priv)
 
 	for (i = 0; i < WEP_KEYS; i++)
 		at76_dbg(DBG_MIB, "%s: MIB MAC_WEP: key %d: %s",
-			 priv->netdev->name, i,
+			 wiphy_name(priv->hw->wiphy), i,
 			 hex2str(m->wep_default_keyvalue[i], key_len));
 exit:
 	kfree(m);
 }
 
+static void at76_dump_mib_mac_encryption(struct at76_priv *priv)
+{
+	int i;
+	int ret;
+	/*int key_len;*/
+	struct mib_mac_encryption *m;
+
+	m = kmalloc(sizeof(struct mib_mac_encryption), GFP_KERNEL);
+	if (!m)
+		return;
+
+	ret = at76_get_mib(priv->udev, MIB_MAC_ENCRYPTION, m,
+			   sizeof(struct mib_mac_encryption));
+	if (ret < 0) {
+		dev_err(&priv->udev->dev,
+			"%s: at76_get_mib (MAC_ENCRYPTION) failed: %d\n",
+			wiphy_name(priv->hw->wiphy), ret);
+		goto exit;
+	}
+
+	at76_dbg(DBG_MIB,
+		 "%s: MIB MAC_ENCRYPTION: tkip_bssid %s priv_invoked %u "
+		 "ciph_key_id %u grp_key_id %u excl_unencr %u "
+		 "ckip_key_perm %u wep_icv_err %u wep_excluded %u",
+		 wiphy_name(priv->hw->wiphy), mac2str(m->tkip_bssid),
+		 m->privacy_invoked, m->cipher_default_key_id,
+		 m->cipher_default_group_key_id, m->exclude_unencrypted,
+		 m->ckip_key_permutation,
+		 le32_to_cpu(m->wep_icv_error_count),
+		 le32_to_cpu(m->wep_excluded_count));
+
+	/*key_len = (m->encryption_level == 1) ?
+	    WEP_SMALL_KEY_LEN : WEP_LARGE_KEY_LEN;*/
+
+	for (i = 0; i < CIPHER_KEYS; i++)
+		at76_dbg(DBG_MIB, "%s: MIB MAC_ENCRYPTION: key %d: %s",
+			 wiphy_name(priv->hw->wiphy), i,
+			 hex2str(m->cipher_default_keyvalue[i],
+				 CIPHER_KEY_LEN));
+exit:
+	kfree(m);
+}
+
 static void at76_dump_mib_mac_mgmt(struct at76_priv *priv)
 {
 	int ret;
@@ -1119,7 +1125,7 @@ static void at76_dump_mib_mac_mgmt(struct at76_priv *priv)
 			   sizeof(struct mib_mac_mgmt));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_get_mib (MAC_MGMT) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		goto exit;
 	}
 
@@ -1130,7 +1136,7 @@ static void at76_dump_mib_mac_mgmt(struct at76_priv *priv)
 		 "pm_mode %d ibss_change %d res %d "
 		 "multi_domain_capability_implemented %d "
 		 "international_roaming %d country_string %.3s",
-		 priv->netdev->name, le16_to_cpu(m->beacon_period),
+		 wiphy_name(priv->hw->wiphy), le16_to_cpu(m->beacon_period),
 		 le16_to_cpu(m->CFP_max_duration),
 		 le16_to_cpu(m->medium_occupancy_limit),
 		 le16_to_cpu(m->station_id), le16_to_cpu(m->ATIM_window),
@@ -1155,7 +1161,7 @@ static void at76_dump_mib_mac(struct at76_priv *priv)
 	ret = at76_get_mib(priv->udev, MIB_MAC, m, sizeof(struct mib_mac));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_get_mib (MAC) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		goto exit;
 	}
 
@@ -1165,7 +1171,8 @@ static void at76_dump_mib_mac(struct at76_priv *priv)
 		 "scan_type %d scan_channel %d probe_delay %u "
 		 "min_channel_time %d max_channel_time %d listen_int %d "
 		 "desired_ssid %s desired_bssid %s desired_bsstype %d",
-		 priv->netdev->name, le32_to_cpu(m->max_tx_msdu_lifetime),
+		 wiphy_name(priv->hw->wiphy),
+		 le32_to_cpu(m->max_tx_msdu_lifetime),
 		 le32_to_cpu(m->max_rx_lifetime),
 		 le16_to_cpu(m->frag_threshold), le16_to_cpu(m->rts_threshold),
 		 le16_to_cpu(m->cwmin), le16_to_cpu(m->cwmax),
@@ -1191,7 +1198,7 @@ static void at76_dump_mib_phy(struct at76_priv *priv)
 	ret = at76_get_mib(priv->udev, MIB_PHY, m, sizeof(struct mib_phy));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_get_mib (PHY) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		goto exit;
 	}
 
@@ -1200,7 +1207,7 @@ static void at76_dump_mib_phy(struct at76_priv *priv)
 		 "mpdu_max_length %d cca_mode_supported %d operation_rate_set "
 		 "0x%x 0x%x 0x%x 0x%x channel_id %d current_cca_mode %d "
 		 "phy_type %d current_reg_domain %d",
-		 priv->netdev->name, le32_to_cpu(m->ed_threshold),
+		 wiphy_name(priv->hw->wiphy), le32_to_cpu(m->ed_threshold),
 		 le16_to_cpu(m->slot_time), le16_to_cpu(m->sifs_time),
 		 le16_to_cpu(m->preamble_length),
 		 le16_to_cpu(m->plcp_header_length),
@@ -1224,13 +1231,14 @@ static void at76_dump_mib_local(struct at76_priv *priv)
 	ret = at76_get_mib(priv->udev, MIB_LOCAL, m, sizeof(struct mib_local));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_get_mib (LOCAL) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		goto exit;
 	}
 
 	at76_dbg(DBG_MIB, "%s: MIB LOCAL: beacon_enable %d "
 		 "txautorate_fallback %d ssid_size %d promiscuous_mode %d "
-		 "preamble_type %d", priv->netdev->name, m->beacon_enable,
+		 "preamble_type %d", wiphy_name(priv->hw->wiphy),
+		 m->beacon_enable,
 		 m->txautorate_fallback, m->ssid_size, m->promiscuous_mode,
 		 m->preamble_type);
 exit:
@@ -1249,118 +1257,21 @@ static void at76_dump_mib_mdomain(struct at76_priv *priv)
 			   sizeof(struct mib_mdomain));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_get_mib (MDOMAIN) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		goto exit;
 	}
 
 	at76_dbg(DBG_MIB, "%s: MIB MDOMAIN: channel_list %s",
-		 priv->netdev->name,
+		 wiphy_name(priv->hw->wiphy),
 		 hex2str(m->channel_list, sizeof(m->channel_list)));
 
 	at76_dbg(DBG_MIB, "%s: MIB MDOMAIN: tx_powerlevel %s",
-		 priv->netdev->name,
+		 wiphy_name(priv->hw->wiphy),
 		 hex2str(m->tx_powerlevel, sizeof(m->tx_powerlevel)));
 exit:
 	kfree(m);
 }
 
-static int at76_get_current_bssid(struct at76_priv *priv)
-{
-	int ret = 0;
-	struct mib_mac_mgmt *mac_mgmt =
-	    kmalloc(sizeof(struct mib_mac_mgmt), GFP_KERNEL);
-
-	if (!mac_mgmt) {
-		ret = -ENOMEM;
-		goto exit;
-	}
-
-	ret = at76_get_mib(priv->udev, MIB_MAC_MGMT, mac_mgmt,
-			   sizeof(struct mib_mac_mgmt));
-	if (ret < 0) {
-		printk(KERN_ERR "%s: at76_get_mib failed: %d\n",
-		       priv->netdev->name, ret);
-		goto error;
-	}
-	memcpy(priv->bssid, mac_mgmt->current_bssid, ETH_ALEN);
-	printk(KERN_INFO "%s: using BSSID %s\n", priv->netdev->name,
-	       mac2str(priv->bssid));
-error:
-	kfree(mac_mgmt);
-exit:
-	return ret;
-}
-
-static int at76_get_current_channel(struct at76_priv *priv)
-{
-	int ret = 0;
-	struct mib_phy *phy = kmalloc(sizeof(struct mib_phy), GFP_KERNEL);
-
-	if (!phy) {
-		ret = -ENOMEM;
-		goto exit;
-	}
-	ret = at76_get_mib(priv->udev, MIB_PHY, phy, sizeof(struct mib_phy));
-	if (ret < 0) {
-		printk(KERN_ERR "%s: at76_get_mib(MIB_PHY) failed: %d\n",
-		       priv->netdev->name, ret);
-		goto error;
-	}
-	priv->channel = phy->channel_id;
-error:
-	kfree(phy);
-exit:
-	return ret;
-}
-
-/**
- * at76_start_scan - start a scan
- *
- * @use_essid - use the configured ESSID in non passive mode
- */
-static int at76_start_scan(struct at76_priv *priv, int use_essid)
-{
-	struct at76_req_scan scan;
-
-	memset(&scan, 0, sizeof(struct at76_req_scan));
-	memset(scan.bssid, 0xff, ETH_ALEN);
-
-	if (use_essid) {
-		memcpy(scan.essid, priv->essid, IW_ESSID_MAX_SIZE);
-		scan.essid_size = priv->essid_size;
-	} else
-		scan.essid_size = 0;
-
-	/* jal: why should we start at a certain channel? we do scan the whole
-	   range allowed by reg domain. */
-	scan.channel = priv->channel;
-
-	/* atmelwlandriver differs between scan type 0 and 1 (active/passive)
-	   For ad-hoc mode, it uses type 0 only. */
-	scan.scan_type = priv->scan_mode;
-
-	/* INFO: For probe_delay, not multiplying by 1024 as this will be
-	   slightly less than min_channel_time
-	   (per spec: probe delay < min. channel time) */
-	scan.min_channel_time = cpu_to_le16(priv->scan_min_time);
-	scan.max_channel_time = cpu_to_le16(priv->scan_max_time);
-	scan.probe_delay = cpu_to_le16(priv->scan_min_time * 1000);
-	scan.international_scan = 0;
-
-	/* other values are set to 0 for type 0 */
-
-	at76_dbg(DBG_PROGRESS, "%s: start_scan (use_essid = %d, intl = %d, "
-		 "channel = %d, probe_delay = %d, scan_min_time = %d, "
-		 "scan_max_time = %d)",
-		 priv->netdev->name, use_essid,
-		 scan.international_scan, scan.channel,
-		 le16_to_cpu(scan.probe_delay),
-		 le16_to_cpu(scan.min_channel_time),
-		 le16_to_cpu(scan.max_channel_time));
-
-	return at76_set_card_command(priv->udev, CMD_SCAN, &scan, sizeof(scan));
-}
-
 /* Enable monitor mode */
 static int at76_start_monitor(struct at76_priv *priv)
 {
@@ -1381,86 +1292,6 @@ static int at76_start_monitor(struct at76_priv *priv)
 	return ret;
 }
 
-static int at76_start_ibss(struct at76_priv *priv)
-{
-	struct at76_req_ibss bss;
-	int ret;
-
-	WARN_ON(priv->mac_state != MAC_OWN_IBSS);
-	if (priv->mac_state != MAC_OWN_IBSS)
-		return -EBUSY;
-
-	memset(&bss, 0, sizeof(struct at76_req_ibss));
-	memset(bss.bssid, 0xff, ETH_ALEN);
-	memcpy(bss.essid, priv->essid, IW_ESSID_MAX_SIZE);
-	bss.essid_size = priv->essid_size;
-	bss.bss_type = ADHOC_MODE;
-	bss.channel = priv->channel;
-
-	ret = at76_set_card_command(priv->udev, CMD_START_IBSS, &bss,
-				    sizeof(struct at76_req_ibss));
-	if (ret < 0) {
-		printk(KERN_ERR "%s: start_ibss failed: %d\n",
-		       priv->netdev->name, ret);
-		return ret;
-	}
-
-	ret = at76_wait_completion(priv, CMD_START_IBSS);
-	if (ret != CMD_STATUS_COMPLETE) {
-		printk(KERN_ERR "%s: start_ibss failed to complete, %d\n",
-		       priv->netdev->name, ret);
-		return ret;
-	}
-
-	ret = at76_get_current_bssid(priv);
-	if (ret < 0)
-		return ret;
-
-	ret = at76_get_current_channel(priv);
-	if (ret < 0)
-		return ret;
-
-	/* not sure what this is good for ??? */
-	priv->mib_buf.type = MIB_MAC_MGMT;
-	priv->mib_buf.size = 1;
-	priv->mib_buf.index = offsetof(struct mib_mac_mgmt, ibss_change);
-	priv->mib_buf.data.byte = 0;
-
-	ret = at76_set_mib(priv, &priv->mib_buf);
-	if (ret < 0) {
-		printk(KERN_ERR "%s: set_mib (ibss change ok) failed: %d\n",
-		       priv->netdev->name, ret);
-		return ret;
-	}
-
-	netif_carrier_on(priv->netdev);
-	netif_start_queue(priv->netdev);
-	return 0;
-}
-
-/* Request card to join BSS in managed or ad-hoc mode */
-static int at76_join_bss(struct at76_priv *priv, struct bss_info *ptr)
-{
-	struct at76_req_join join;
-
-	BUG_ON(!ptr);
-
-	memset(&join, 0, sizeof(struct at76_req_join));
-	memcpy(join.bssid, ptr->bssid, ETH_ALEN);
-	memcpy(join.essid, ptr->ssid, ptr->ssid_len);
-	join.essid_size = ptr->ssid_len;
-	join.bss_type = (priv->iw_mode == IW_MODE_ADHOC ? 1 : 2);
-	join.channel = ptr->channel;
-	join.timeout = cpu_to_le16(2000);
-
-	at76_dbg(DBG_PROGRESS,
-		 "%s join addr %s ssid %s type %d ch %d timeout %d",
-		 priv->netdev->name, mac2str(join.bssid), join.essid,
-		 join.bss_type, join.channel, le16_to_cpu(join.timeout));
-	return at76_set_card_command(priv->udev, CMD_JOIN, &join,
-				     sizeof(struct at76_req_join));
-}
-
 /* Calculate padding from txbuf->wlength (which excludes the USB TX header),
    likely to compensate a flaw in the AT76C503A USB part ... */
 static inline int at76_calc_padding(int wlen)
@@ -1479,14 +1310,6 @@ static inline int at76_calc_padding(int wlen)
 	return 0;
 }
 
-/* We are doing a lot of things here in an interrupt. Need
-   a bh handler (Watching TV with a TV card is probably
-   a good test: if you see flickers, we are doing too much.
-   Currently I do see flickers... even with our tasklet :-( )
-   Maybe because the bttv driver and usb-uhci use the same interrupt
-*/
-/* Or maybe because our BH handler is preempting bttv's BH handler.. BHs don't
- * solve everything.. (alex) */
 static void at76_rx_callback(struct urb *urb)
 {
 	struct at76_priv *priv = urb->context;
@@ -1496,1758 +1319,6 @@ static void at76_rx_callback(struct urb *urb)
 	return;
 }
 
-static void at76_tx_callback(struct urb *urb)
-{
-	struct at76_priv *priv = urb->context;
-	struct net_device_stats *stats = &priv->stats;
-	unsigned long flags;
-	struct at76_tx_buffer *mgmt_buf;
-	int ret;
-
-	switch (urb->status) {
-	case 0:
-		stats->tx_packets++;
-		break;
-	case -ENOENT:
-	case -ECONNRESET:
-		/* urb has been unlinked */
-		return;
-	default:
-		at76_dbg(DBG_URB, "%s - nonzero tx status received: %d",
-			 __func__, urb->status);
-		stats->tx_errors++;
-		break;
-	}
-
-	spin_lock_irqsave(&priv->mgmt_spinlock, flags);
-	mgmt_buf = priv->next_mgmt_bulk;
-	priv->next_mgmt_bulk = NULL;
-	spin_unlock_irqrestore(&priv->mgmt_spinlock, flags);
-
-	if (!mgmt_buf) {
-		netif_wake_queue(priv->netdev);
-		return;
-	}
-
-	/* we don't copy the padding bytes, but add them
-	   to the length */
-	memcpy(priv->bulk_out_buffer, mgmt_buf,
-	       le16_to_cpu(mgmt_buf->wlength) + AT76_TX_HDRLEN);
-	usb_fill_bulk_urb(priv->tx_urb, priv->udev, priv->tx_pipe,
-			  priv->bulk_out_buffer,
-			  le16_to_cpu(mgmt_buf->wlength) + mgmt_buf->padding +
-			  AT76_TX_HDRLEN, at76_tx_callback, priv);
-	ret = usb_submit_urb(priv->tx_urb, GFP_ATOMIC);
-	if (ret)
-		printk(KERN_ERR "%s: error in tx submit urb: %d\n",
-		       priv->netdev->name, ret);
-
-	kfree(mgmt_buf);
-}
-
-/* Send a management frame on bulk-out.  txbuf->wlength must be set */
-static int at76_tx_mgmt(struct at76_priv *priv, struct at76_tx_buffer *txbuf)
-{
-	unsigned long flags;
-	int ret;
-	int urb_status;
-	void *oldbuf = NULL;
-
-	netif_carrier_off(priv->netdev);	/* stop netdev watchdog */
-	netif_stop_queue(priv->netdev);	/* stop tx data packets */
-
-	spin_lock_irqsave(&priv->mgmt_spinlock, flags);
-
-	urb_status = priv->tx_urb->status;
-	if (urb_status == -EINPROGRESS) {
-		/* cannot transmit now, put in the queue */
-		oldbuf = priv->next_mgmt_bulk;
-		priv->next_mgmt_bulk = txbuf;
-	}
-	spin_unlock_irqrestore(&priv->mgmt_spinlock, flags);
-
-	if (oldbuf) {
-		/* a data/mgmt tx is already pending in the URB -
-		   if this is no error in some situations we must
-		   implement a queue or silently modify the old msg */
-		printk(KERN_ERR "%s: removed pending mgmt buffer %s\n",
-		       priv->netdev->name, hex2str(oldbuf, 64));
-		kfree(oldbuf);
-		return 0;
-	}
-
-	txbuf->tx_rate = TX_RATE_1MBIT;
-	txbuf->padding = at76_calc_padding(le16_to_cpu(txbuf->wlength));
-	memset(txbuf->reserved, 0, sizeof(txbuf->reserved));
-
-	if (priv->next_mgmt_bulk)
-		printk(KERN_ERR "%s: URB status %d, but mgmt is pending\n",
-		       priv->netdev->name, urb_status);
-
-	at76_dbg(DBG_TX_MGMT,
-		 "%s: tx mgmt: wlen %d tx_rate %d pad %d %s",
-		 priv->netdev->name, le16_to_cpu(txbuf->wlength),
-		 txbuf->tx_rate, txbuf->padding,
-		 hex2str(txbuf->packet, le16_to_cpu(txbuf->wlength)));
-
-	/* txbuf was not consumed above -> send mgmt msg immediately */
-	memcpy(priv->bulk_out_buffer, txbuf,
-	       le16_to_cpu(txbuf->wlength) + AT76_TX_HDRLEN);
-	usb_fill_bulk_urb(priv->tx_urb, priv->udev, priv->tx_pipe,
-			  priv->bulk_out_buffer,
-			  le16_to_cpu(txbuf->wlength) + txbuf->padding +
-			  AT76_TX_HDRLEN, at76_tx_callback, priv);
-	ret = usb_submit_urb(priv->tx_urb, GFP_ATOMIC);
-	if (ret)
-		printk(KERN_ERR "%s: error in tx submit urb: %d\n",
-		       priv->netdev->name, ret);
-
-	kfree(txbuf);
-
-	return ret;
-}
-
-/* Go to the next information element */
-static inline void next_ie(struct ieee80211_info_element **ie)
-{
-	*ie = (struct ieee80211_info_element *)(&(*ie)->data[(*ie)->len]);
-}
-
-/* Challenge is the challenge string (in TLV format)
-   we got with seq_nr 2 for shared secret authentication only and
-   send in seq_nr 3 WEP encrypted to prove we have the correct WEP key;
-   otherwise it is NULL */
-static int at76_auth_req(struct at76_priv *priv, struct bss_info *bss,
-			 int seq_nr, struct ieee80211_info_element *challenge)
-{
-	struct at76_tx_buffer *tx_buffer;
-	struct ieee80211_hdr_3addr *mgmt;
-	struct ieee80211_auth *req;
-	int buf_len = (seq_nr != 3 ? AUTH_FRAME_SIZE :
-		       AUTH_FRAME_SIZE + 1 + 1 + challenge->len);
-
-	BUG_ON(!bss);
-	BUG_ON(seq_nr == 3 && !challenge);
-	tx_buffer = kmalloc(buf_len + MAX_PADDING_SIZE, GFP_ATOMIC);
-	if (!tx_buffer)
-		return -ENOMEM;
-
-	req = (struct ieee80211_auth *)tx_buffer->packet;
-	mgmt = &req->header;
-
-	/* make wireless header */
-	/* first auth msg is not encrypted, only the second (seq_nr == 3) */
-	mgmt->frame_ctl =
-	    cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_AUTH |
-			(seq_nr == 3 ? IEEE80211_FCTL_PROTECTED : 0));
-
-	mgmt->duration_id = cpu_to_le16(0x8000);
-	memcpy(mgmt->addr1, bss->bssid, ETH_ALEN);
-	memcpy(mgmt->addr2, priv->netdev->dev_addr, ETH_ALEN);
-	memcpy(mgmt->addr3, bss->bssid, ETH_ALEN);
-	mgmt->seq_ctl = cpu_to_le16(0);
-
-	req->algorithm = cpu_to_le16(priv->auth_mode);
-	req->transaction = cpu_to_le16(seq_nr);
-	req->status = cpu_to_le16(0);
-
-	if (seq_nr == 3)
-		memcpy(req->info_element, challenge, 1 + 1 + challenge->len);
-
-	/* init. at76_priv tx header */
-	tx_buffer->wlength = cpu_to_le16(buf_len - AT76_TX_HDRLEN);
-	at76_dbg(DBG_TX_MGMT, "%s: AuthReq bssid %s alg %d seq_nr %d",
-		 priv->netdev->name, mac2str(mgmt->addr3),
-		 le16_to_cpu(req->algorithm), le16_to_cpu(req->transaction));
-	if (seq_nr == 3)
-		at76_dbg(DBG_TX_MGMT, "%s: AuthReq challenge: %s ...",
-			 priv->netdev->name, hex2str(req->info_element, 18));
-
-	/* either send immediately (if no data tx is pending
-	   or put it in pending list */
-	return at76_tx_mgmt(priv, tx_buffer);
-}
-
-static int at76_assoc_req(struct at76_priv *priv, struct bss_info *bss)
-{
-	struct at76_tx_buffer *tx_buffer;
-	struct ieee80211_hdr_3addr *mgmt;
-	struct ieee80211_assoc_request *req;
-	struct ieee80211_info_element *ie;
-	char *essid;
-	int essid_len;
-	u16 capa;
-
-	BUG_ON(!bss);
-
-	tx_buffer = kmalloc(ASSOCREQ_MAX_SIZE + MAX_PADDING_SIZE, GFP_ATOMIC);
-	if (!tx_buffer)
-		return -ENOMEM;
-
-	req = (struct ieee80211_assoc_request *)tx_buffer->packet;
-	mgmt = &req->header;
-	ie = req->info_element;
-
-	/* make wireless header */
-	mgmt->frame_ctl = cpu_to_le16(IEEE80211_FTYPE_MGMT |
-				      IEEE80211_STYPE_ASSOC_REQ);
-
-	mgmt->duration_id = cpu_to_le16(0x8000);
-	memcpy(mgmt->addr1, bss->bssid, ETH_ALEN);
-	memcpy(mgmt->addr2, priv->netdev->dev_addr, ETH_ALEN);
-	memcpy(mgmt->addr3, bss->bssid, ETH_ALEN);
-	mgmt->seq_ctl = cpu_to_le16(0);
-
-	/* we must set the Privacy bit in the capabilities to assure an
-	   Agere-based AP with optional WEP transmits encrypted frames
-	   to us.  AP only set the Privacy bit in their capabilities
-	   if WEP is mandatory in the BSS! */
-	capa = bss->capa;
-	if (priv->wep_enabled)
-		capa |= WLAN_CAPABILITY_PRIVACY;
-	if (priv->preamble_type != PREAMBLE_TYPE_LONG)
-		capa |= WLAN_CAPABILITY_SHORT_PREAMBLE;
-	req->capability = cpu_to_le16(capa);
-
-	req->listen_interval = cpu_to_le16(2 * bss->beacon_interval);
-
-	/* write TLV data elements */
-
-	ie->id = MFIE_TYPE_SSID;
-	ie->len = bss->ssid_len;
-	memcpy(ie->data, bss->ssid, bss->ssid_len);
-	next_ie(&ie);
-
-	ie->id = MFIE_TYPE_RATES;
-	ie->len = sizeof(hw_rates);
-	memcpy(ie->data, hw_rates, sizeof(hw_rates));
-	next_ie(&ie);		/* ie points behind the supp_rates field */
-
-	/* init. at76_priv tx header */
-	tx_buffer->wlength = cpu_to_le16((u8 *)ie - (u8 *)mgmt);
-
-	ie = req->info_element;
-	essid = ie->data;
-	essid_len = min_t(int, IW_ESSID_MAX_SIZE, ie->len);
-
-	next_ie(&ie);		/* points to IE of rates now */
-	at76_dbg(DBG_TX_MGMT,
-		 "%s: AssocReq bssid %s capa 0x%04x ssid %.*s rates %s",
-		 priv->netdev->name, mac2str(mgmt->addr3),
-		 le16_to_cpu(req->capability), essid_len, essid,
-		 hex2str(ie->data, ie->len));
-
-	/* either send immediately (if no data tx is pending
-	   or put it in pending list */
-	return at76_tx_mgmt(priv, tx_buffer);
-}
-
-/* We got to check the bss_list for old entries */
-static void at76_bss_list_timeout(unsigned long par)
-{
-	struct at76_priv *priv = (struct at76_priv *)par;
-	unsigned long flags;
-	struct list_head *lptr, *nptr;
-	struct bss_info *ptr;
-
-	spin_lock_irqsave(&priv->bss_list_spinlock, flags);
-
-	list_for_each_safe(lptr, nptr, &priv->bss_list) {
-
-		ptr = list_entry(lptr, struct bss_info, list);
-
-		if (ptr != priv->curr_bss
-		    && time_after(jiffies, ptr->last_rx + BSS_LIST_TIMEOUT)) {
-			at76_dbg(DBG_BSS_TABLE_RM,
-				 "%s: bss_list: removing old BSS %s ch %d",
-				 priv->netdev->name, mac2str(ptr->bssid),
-				 ptr->channel);
-			list_del(&ptr->list);
-			kfree(ptr);
-		}
-	}
-	spin_unlock_irqrestore(&priv->bss_list_spinlock, flags);
-	/* restart the timer */
-	mod_timer(&priv->bss_list_timer, jiffies + BSS_LIST_TIMEOUT);
-}
-
-static inline void at76_set_mac_state(struct at76_priv *priv,
-				      enum mac_state mac_state)
-{
-	at76_dbg(DBG_MAC_STATE, "%s state: %s", priv->netdev->name,
-		 mac_states[mac_state]);
-	priv->mac_state = mac_state;
-}
-
-static void at76_dump_bss_table(struct at76_priv *priv)
-{
-	struct bss_info *ptr;
-	unsigned long flags;
-	struct list_head *lptr;
-
-	spin_lock_irqsave(&priv->bss_list_spinlock, flags);
-
-	at76_dbg(DBG_BSS_TABLE, "%s BSS table (curr=%p):", priv->netdev->name,
-		 priv->curr_bss);
-
-	list_for_each(lptr, &priv->bss_list) {
-		ptr = list_entry(lptr, struct bss_info, list);
-		at76_dbg(DBG_BSS_TABLE, "0x%p: bssid %s channel %d ssid %.*s "
-			 "(%s) capa 0x%04x rates %s rssi %d link %d noise %d",
-			 ptr, mac2str(ptr->bssid), ptr->channel, ptr->ssid_len,
-			 ptr->ssid, hex2str(ptr->ssid, ptr->ssid_len),
-			 ptr->capa, hex2str(ptr->rates, ptr->rates_len),
-			 ptr->rssi, ptr->link_qual, ptr->noise_level);
-	}
-	spin_unlock_irqrestore(&priv->bss_list_spinlock, flags);
-}
-
-/* Called upon successful association to mark interface as connected */
-static void at76_work_assoc_done(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      work_assoc_done);
-
-	mutex_lock(&priv->mtx);
-
-	WARN_ON(priv->mac_state != MAC_ASSOC);
-	WARN_ON(!priv->curr_bss);
-	if (priv->mac_state != MAC_ASSOC || !priv->curr_bss)
-		goto exit;
-
-	if (priv->iw_mode == IW_MODE_INFRA) {
-		if (priv->pm_mode != AT76_PM_OFF) {
-			/* calculate the listen interval in units of
-			   beacon intervals of the curr_bss */
-			u32 pm_period_beacon = (priv->pm_period >> 10) /
-			    priv->curr_bss->beacon_interval;
-
-			pm_period_beacon = max(pm_period_beacon, 2u);
-			pm_period_beacon = min(pm_period_beacon, 0xffffu);
-
-			at76_dbg(DBG_PM,
-				 "%s: pm_mode %d assoc id 0x%x listen int %d",
-				 priv->netdev->name, priv->pm_mode,
-				 priv->assoc_id, pm_period_beacon);
-
-			at76_set_associd(priv, priv->assoc_id);
-			at76_set_listen_interval(priv, (u16)pm_period_beacon);
-		}
-		schedule_delayed_work(&priv->dwork_beacon, BEACON_TIMEOUT);
-	}
-	at76_set_pm_mode(priv);
-
-	netif_carrier_on(priv->netdev);
-	netif_wake_queue(priv->netdev);
-	at76_set_mac_state(priv, MAC_CONNECTED);
-	at76_iwevent_bss_connect(priv->netdev, priv->curr_bss->bssid);
-	at76_dbg(DBG_PROGRESS, "%s: connected to BSSID %s",
-		 priv->netdev->name, mac2str(priv->curr_bss->bssid));
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
-/* We only store the new mac address in netdev struct,
-   it gets set when the netdev is opened. */
-static int at76_set_mac_address(struct net_device *netdev, void *addr)
-{
-	struct sockaddr *mac = addr;
-	memcpy(netdev->dev_addr, mac->sa_data, ETH_ALEN);
-	return 1;
-}
-
-static struct net_device_stats *at76_get_stats(struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	return &priv->stats;
-}
-
-static struct iw_statistics *at76_get_wireless_stats(struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_IOCTL, "RETURN qual %d level %d noise %d updated %d",
-		 priv->wstats.qual.qual, priv->wstats.qual.level,
-		 priv->wstats.qual.noise, priv->wstats.qual.updated);
-
-	return &priv->wstats;
-}
-
-static void at76_set_multicast(struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int promisc;
-
-	promisc = ((netdev->flags & IFF_PROMISC) != 0);
-	if (promisc != priv->promisc) {
-		/* This gets called in interrupt, must reschedule */
-		priv->promisc = promisc;
-		schedule_work(&priv->work_set_promisc);
-	}
-}
-
-/* Stop all network activity, flush all pending tasks */
-static void at76_quiesce(struct at76_priv *priv)
-{
-	unsigned long flags;
-
-	netif_stop_queue(priv->netdev);
-	netif_carrier_off(priv->netdev);
-
-	at76_set_mac_state(priv, MAC_INIT);
-
-	cancel_delayed_work(&priv->dwork_get_scan);
-	cancel_delayed_work(&priv->dwork_beacon);
-	cancel_delayed_work(&priv->dwork_auth);
-	cancel_delayed_work(&priv->dwork_assoc);
-	cancel_delayed_work(&priv->dwork_restart);
-
-	spin_lock_irqsave(&priv->mgmt_spinlock, flags);
-	kfree(priv->next_mgmt_bulk);
-	priv->next_mgmt_bulk = NULL;
-	spin_unlock_irqrestore(&priv->mgmt_spinlock, flags);
-}
-
-/*******************************************************************************
- * at76_priv implementations of iw_handler functions:
- */
-static int at76_iw_handler_commit(struct net_device *netdev,
-				  struct iw_request_info *info,
-				  void *null, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_IOCTL, "%s %s: restarting the device", netdev->name,
-		 __func__);
-
-	if (priv->mac_state != MAC_INIT)
-		at76_quiesce(priv);
-
-	/* Wait half second before the restart to process subsequent
-	 * requests from the same iwconfig in a single restart */
-	schedule_delayed_work(&priv->dwork_restart, HZ / 2);
-
-	return 0;
-}
-
-static int at76_iw_handler_get_name(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    char *name, char *extra)
-{
-	strcpy(name, "IEEE 802.11b");
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWNAME - name %s", netdev->name, name);
-	return 0;
-}
-
-static int at76_iw_handler_set_freq(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    struct iw_freq *freq, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int chan = -1;
-	int ret = -EIWCOMMIT;
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWFREQ - freq.m %d freq.e %d",
-		 netdev->name, freq->m, freq->e);
-
-	if ((freq->e == 0) && (freq->m <= 1000))
-		/* Setting by channel number */
-		chan = freq->m;
-	else {
-		/* Setting by frequency - search the table */
-		int mult = 1;
-		int i;
-
-		for (i = 0; i < (6 - freq->e); i++)
-			mult *= 10;
-
-		for (i = 0; i < NUM_CHANNELS; i++) {
-			if (freq->m == (channel_frequency[i] * mult))
-				chan = i + 1;
-		}
-	}
-
-	if (chan < 1 || !priv->domain)
-		/* non-positive channels are invalid
-		 * we need a domain info to set the channel
-		 * either that or an invalid frequency was
-		 * provided by the user */
-		ret = -EINVAL;
-	else if (!(priv->domain->channel_map & (1 << (chan - 1)))) {
-		printk(KERN_INFO "%s: channel %d not allowed for domain %s\n",
-		       priv->netdev->name, chan, priv->domain->name);
-		ret = -EINVAL;
-	}
-
-	if (ret == -EIWCOMMIT) {
-		priv->channel = chan;
-		at76_dbg(DBG_IOCTL, "%s: SIOCSIWFREQ - ch %d", netdev->name,
-			 chan);
-	}
-
-	return ret;
-}
-
-static int at76_iw_handler_get_freq(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    struct iw_freq *freq, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	freq->m = priv->channel;
-	freq->e = 0;
-
-	if (priv->channel)
-		at76_dbg(DBG_IOCTL, "%s: SIOCGIWFREQ - freq %ld x 10e%d",
-			 netdev->name, channel_frequency[priv->channel - 1], 6);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWFREQ - ch %d", netdev->name,
-		 priv->channel);
-
-	return 0;
-}
-
-static int at76_iw_handler_set_mode(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    __u32 *mode, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWMODE - %d", netdev->name, *mode);
-
-	if ((*mode != IW_MODE_ADHOC) && (*mode != IW_MODE_INFRA) &&
-	    (*mode != IW_MODE_MONITOR))
-		return -EINVAL;
-
-	priv->iw_mode = *mode;
-	if (priv->iw_mode != IW_MODE_INFRA)
-		priv->pm_mode = AT76_PM_OFF;
-
-	return -EIWCOMMIT;
-}
-
-static int at76_iw_handler_get_mode(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    __u32 *mode, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	*mode = priv->iw_mode;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWMODE - %d", netdev->name, *mode);
-
-	return 0;
-}
-
-static int at76_iw_handler_get_range(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_point *data, char *extra)
-{
-	/* inspired by atmel.c */
-	struct at76_priv *priv = netdev_priv(netdev);
-	struct iw_range *range = (struct iw_range *)extra;
-	int i;
-
-	data->length = sizeof(struct iw_range);
-	memset(range, 0, sizeof(struct iw_range));
-
-	/* TODO: range->throughput = xxxxxx; */
-
-	range->min_nwid = 0x0000;
-	range->max_nwid = 0x0000;
-
-	/* this driver doesn't maintain sensitivity information */
-	range->sensitivity = 0;
-
-	range->max_qual.qual = 100;
-	range->max_qual.level = 100;
-	range->max_qual.noise = 0;
-	range->max_qual.updated = IW_QUAL_NOISE_INVALID;
-
-	range->avg_qual.qual = 50;
-	range->avg_qual.level = 50;
-	range->avg_qual.noise = 0;
-	range->avg_qual.updated = IW_QUAL_NOISE_INVALID;
-
-	range->bitrate[0] = 1000000;
-	range->bitrate[1] = 2000000;
-	range->bitrate[2] = 5500000;
-	range->bitrate[3] = 11000000;
-	range->num_bitrates = 4;
-
-	range->min_rts = 0;
-	range->max_rts = MAX_RTS_THRESHOLD;
-
-	range->min_frag = MIN_FRAG_THRESHOLD;
-	range->max_frag = MAX_FRAG_THRESHOLD;
-
-	range->pmp_flags = IW_POWER_PERIOD;
-	range->pmt_flags = IW_POWER_ON;
-	range->pm_capa = IW_POWER_PERIOD | IW_POWER_ALL_R;
-
-	range->encoding_size[0] = WEP_SMALL_KEY_LEN;
-	range->encoding_size[1] = WEP_LARGE_KEY_LEN;
-	range->num_encoding_sizes = 2;
-	range->max_encoding_tokens = WEP_KEYS;
-
-	/* both WL-240U and Linksys WUSB11 v2.6 specify 15 dBm as output power
-	   - take this for all (ignore antenna gains) */
-	range->txpower[0] = 15;
-	range->num_txpower = 1;
-	range->txpower_capa = IW_TXPOW_DBM;
-
-	range->we_version_source = WIRELESS_EXT;
-	range->we_version_compiled = WIRELESS_EXT;
-
-	/* same as the values used in atmel.c */
-	range->retry_capa = IW_RETRY_LIMIT;
-	range->retry_flags = IW_RETRY_LIMIT;
-	range->r_time_flags = 0;
-	range->min_retry = 1;
-	range->max_retry = 255;
-
-	range->num_channels = NUM_CHANNELS;
-	range->num_frequency = 0;
-
-	for (i = 0; i < NUM_CHANNELS; i++) {
-		/* test if channel map bit is raised */
-		if (priv->domain->channel_map & (0x1 << i)) {
-			range->num_frequency += 1;
-
-			range->freq[i].i = i + 1;
-			range->freq[i].m = channel_frequency[i] * 100000;
-			range->freq[i].e = 1;	/* freq * 10^1 */
-		}
-	}
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWRANGE", netdev->name);
-
-	return 0;
-}
-
-static int at76_iw_handler_set_spy(struct net_device *netdev,
-				   struct iw_request_info *info,
-				   struct iw_point *data, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = 0;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWSPY - number of addresses %d",
-		 netdev->name, data->length);
-
-	spin_lock_bh(&priv->spy_spinlock);
-	ret = iw_handler_set_spy(priv->netdev, info, (union iwreq_data *)data,
-				 extra);
-	spin_unlock_bh(&priv->spy_spinlock);
-
-	return ret;
-}
-
-static int at76_iw_handler_get_spy(struct net_device *netdev,
-				   struct iw_request_info *info,
-				   struct iw_point *data, char *extra)
-{
-
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = 0;
-
-	spin_lock_bh(&priv->spy_spinlock);
-	ret = iw_handler_get_spy(priv->netdev, info,
-				 (union iwreq_data *)data, extra);
-	spin_unlock_bh(&priv->spy_spinlock);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWSPY - number of addresses %d",
-		 netdev->name, data->length);
-
-	return ret;
-}
-
-static int at76_iw_handler_set_thrspy(struct net_device *netdev,
-				      struct iw_request_info *info,
-				      struct iw_point *data, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWTHRSPY - number of addresses %d)",
-		 netdev->name, data->length);
-
-	spin_lock_bh(&priv->spy_spinlock);
-	ret = iw_handler_set_thrspy(netdev, info, (union iwreq_data *)data,
-				    extra);
-	spin_unlock_bh(&priv->spy_spinlock);
-
-	return ret;
-}
-
-static int at76_iw_handler_get_thrspy(struct net_device *netdev,
-				      struct iw_request_info *info,
-				      struct iw_point *data, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret;
-
-	spin_lock_bh(&priv->spy_spinlock);
-	ret = iw_handler_get_thrspy(netdev, info, (union iwreq_data *)data,
-				    extra);
-	spin_unlock_bh(&priv->spy_spinlock);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWTHRSPY - number of addresses %d)",
-		 netdev->name, data->length);
-
-	return ret;
-}
-
-static int at76_iw_handler_set_wap(struct net_device *netdev,
-				   struct iw_request_info *info,
-				   struct sockaddr *ap_addr, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWAP - wap/bssid %s", netdev->name,
-		 mac2str(ap_addr->sa_data));
-
-	/* if the incoming address == ff:ff:ff:ff:ff:ff, the user has
-	   chosen any or auto AP preference */
-	if (is_broadcast_ether_addr(ap_addr->sa_data)
-	    || is_zero_ether_addr(ap_addr->sa_data))
-		priv->wanted_bssid_valid = 0;
-	else {
-		/* user wants to set a preferred AP address */
-		priv->wanted_bssid_valid = 1;
-		memcpy(priv->wanted_bssid, ap_addr->sa_data, ETH_ALEN);
-	}
-
-	return -EIWCOMMIT;
-}
-
-static int at76_iw_handler_get_wap(struct net_device *netdev,
-				   struct iw_request_info *info,
-				   struct sockaddr *ap_addr, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	ap_addr->sa_family = ARPHRD_ETHER;
-	memcpy(ap_addr->sa_data, priv->bssid, ETH_ALEN);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWAP - wap/bssid %s", netdev->name,
-		 mac2str(ap_addr->sa_data));
-
-	return 0;
-}
-
-static int at76_iw_handler_set_scan(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    union iwreq_data *wrqu, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = 0;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWSCAN", netdev->name);
-
-	if (mutex_lock_interruptible(&priv->mtx))
-		return -EINTR;
-
-	if (!netif_running(netdev)) {
-		ret = -ENETDOWN;
-		goto exit;
-	}
-
-	/* jal: we don't allow "iwlist ethX scan" while we are
-	   in monitor mode */
-	if (priv->iw_mode == IW_MODE_MONITOR) {
-		ret = -EBUSY;
-		goto exit;
-	}
-
-	/* Discard old scan results */
-	if ((jiffies - priv->last_scan) > (20 * HZ))
-		priv->scan_state = SCAN_IDLE;
-	priv->last_scan = jiffies;
-
-	/* Initiate a scan command */
-	if (priv->scan_state == SCAN_IN_PROGRESS) {
-		ret = -EBUSY;
-		goto exit;
-	}
-
-	priv->scan_state = SCAN_IN_PROGRESS;
-
-	at76_quiesce(priv);
-
-	/* Try to do passive or active scan if WE asks as. */
-	if (wrqu->data.length
-	    && wrqu->data.length == sizeof(struct iw_scan_req)) {
-		struct iw_scan_req *req = (struct iw_scan_req *)extra;
-
-		if (req->scan_type == IW_SCAN_TYPE_PASSIVE)
-			priv->scan_mode = SCAN_TYPE_PASSIVE;
-		else if (req->scan_type == IW_SCAN_TYPE_ACTIVE)
-			priv->scan_mode = SCAN_TYPE_ACTIVE;
-
-		/* Sanity check values? */
-		if (req->min_channel_time > 0)
-			priv->scan_min_time = req->min_channel_time;
-
-		if (req->max_channel_time > 0)
-			priv->scan_max_time = req->max_channel_time;
-	}
-
-	/* change to scanning state */
-	at76_set_mac_state(priv, MAC_SCANNING);
-	schedule_work(&priv->work_start_scan);
-
-exit:
-	mutex_unlock(&priv->mtx);
-	return ret;
-}
-
-static int at76_iw_handler_get_scan(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    struct iw_point *data, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	unsigned long flags;
-	struct list_head *lptr, *nptr;
-	struct bss_info *curr_bss;
-	struct iw_event *iwe = kmalloc(sizeof(struct iw_event), GFP_KERNEL);
-	char *curr_val, *curr_pos = extra;
-	int i;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWSCAN", netdev->name);
-
-	if (!iwe)
-		return -ENOMEM;
-
-	if (priv->scan_state != SCAN_COMPLETED) {
-		/* scan not yet finished */
-		kfree(iwe);
-		return -EAGAIN;
-	}
-
-	spin_lock_irqsave(&priv->bss_list_spinlock, flags);
-
-	list_for_each_safe(lptr, nptr, &priv->bss_list) {
-		curr_bss = list_entry(lptr, struct bss_info, list);
-
-		iwe->cmd = SIOCGIWAP;
-		iwe->u.ap_addr.sa_family = ARPHRD_ETHER;
-		memcpy(iwe->u.ap_addr.sa_data, curr_bss->bssid, 6);
-		curr_pos = iwe_stream_add_event(info, curr_pos,
-						extra + IW_SCAN_MAX_DATA, iwe,
-						IW_EV_ADDR_LEN);
-
-		iwe->u.data.length = curr_bss->ssid_len;
-		iwe->cmd = SIOCGIWESSID;
-		iwe->u.data.flags = 1;
-
-		curr_pos = iwe_stream_add_point(info, curr_pos,
-						extra + IW_SCAN_MAX_DATA, iwe,
-						curr_bss->ssid);
-
-		iwe->cmd = SIOCGIWMODE;
-		iwe->u.mode = (curr_bss->capa & WLAN_CAPABILITY_IBSS) ?
-		    IW_MODE_ADHOC :
-		    (curr_bss->capa & WLAN_CAPABILITY_ESS) ?
-		    IW_MODE_MASTER : IW_MODE_AUTO;
-		/* IW_MODE_AUTO = 0 which I thought is
-		 * the most logical value to return in this case */
-		curr_pos = iwe_stream_add_event(info, curr_pos,
-						extra + IW_SCAN_MAX_DATA, iwe,
-						IW_EV_UINT_LEN);
-
-		iwe->cmd = SIOCGIWFREQ;
-		iwe->u.freq.m = curr_bss->channel;
-		iwe->u.freq.e = 0;
-		curr_pos = iwe_stream_add_event(info, curr_pos,
-						extra + IW_SCAN_MAX_DATA, iwe,
-						IW_EV_FREQ_LEN);
-
-		iwe->cmd = SIOCGIWENCODE;
-		if (curr_bss->capa & 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;
-		curr_pos = iwe_stream_add_point(info, curr_pos,
-						extra + IW_SCAN_MAX_DATA, iwe,
-						NULL);
-
-		/* Add quality statistics */
-		iwe->cmd = IWEVQUAL;
-		iwe->u.qual.noise = 0;
-		iwe->u.qual.updated =
-		    IW_QUAL_NOISE_INVALID | IW_QUAL_LEVEL_UPDATED;
-		iwe->u.qual.level = (curr_bss->rssi * 100 / 42);
-		if (iwe->u.qual.level > 100)
-			iwe->u.qual.level = 100;
-		if (at76_is_intersil(priv->board_type))
-			iwe->u.qual.qual = curr_bss->link_qual;
-		else {
-			iwe->u.qual.qual = 0;
-			iwe->u.qual.updated |= IW_QUAL_QUAL_INVALID;
-		}
-		/* Add new value to event */
-		curr_pos = iwe_stream_add_event(info, curr_pos,
-						extra + IW_SCAN_MAX_DATA, iwe,
-						IW_EV_QUAL_LEN);
-
-		/* Rate: stuffing multiple values in a single event requires
-		 * a bit more of magic - Jean II */
-		curr_val = curr_pos + IW_EV_LCP_LEN;
-
-		iwe->cmd = SIOCGIWRATE;
-		/* Those two flags are ignored... */
-		iwe->u.bitrate.fixed = 0;
-		iwe->u.bitrate.disabled = 0;
-		/* Max 8 values */
-		for (i = 0; i < curr_bss->rates_len; i++) {
-			/* Bit rate given in 500 kb/s units (+ 0x80) */
-			iwe->u.bitrate.value =
-			    ((curr_bss->rates[i] & 0x7f) * 500000);
-			/* Add new value to event */
-			curr_val = iwe_stream_add_value(info, curr_pos,
-							curr_val,
-							extra +
-							IW_SCAN_MAX_DATA, iwe,
-							IW_EV_PARAM_LEN);
-		}
-
-		/* Check if we added any event */
-		if ((curr_val - curr_pos) > IW_EV_LCP_LEN)
-			curr_pos = curr_val;
-
-		/* more information may be sent back using IWECUSTOM */
-
-	}
-
-	spin_unlock_irqrestore(&priv->bss_list_spinlock, flags);
-
-	data->length = (curr_pos - extra);
-	data->flags = 0;
-
-	kfree(iwe);
-	return 0;
-}
-
-static int at76_iw_handler_set_essid(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_point *data, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWESSID - %s", netdev->name, extra);
-
-	if (data->flags) {
-		memcpy(priv->essid, extra, data->length);
-		priv->essid_size = data->length;
-	} else
-		priv->essid_size = 0;	/* Use any SSID */
-
-	return -EIWCOMMIT;
-}
-
-static int at76_iw_handler_get_essid(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_point *data, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	if (priv->essid_size) {
-		/* not the ANY ssid in priv->essid */
-		data->flags = 1;
-		data->length = priv->essid_size;
-		memcpy(extra, priv->essid, data->length);
-	} else {
-		/* the ANY ssid was specified */
-		if (priv->mac_state == MAC_CONNECTED && priv->curr_bss) {
-			/* report the SSID we have found */
-			data->flags = 1;
-			data->length = priv->curr_bss->ssid_len;
-			memcpy(extra, priv->curr_bss->ssid, data->length);
-		} else {
-			/* report ANY back */
-			data->flags = 0;
-			data->length = 0;
-		}
-	}
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWESSID - %.*s", netdev->name,
-		 data->length, extra);
-
-	return 0;
-}
-
-static int at76_iw_handler_set_rate(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    struct iw_param *bitrate, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = -EIWCOMMIT;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWRATE - %d", netdev->name,
-		 bitrate->value);
-
-	switch (bitrate->value) {
-	case -1:
-		priv->txrate = TX_RATE_AUTO;
-		break;		/* auto rate */
-	case 1000000:
-		priv->txrate = TX_RATE_1MBIT;
-		break;
-	case 2000000:
-		priv->txrate = TX_RATE_2MBIT;
-		break;
-	case 5500000:
-		priv->txrate = TX_RATE_5_5MBIT;
-		break;
-	case 11000000:
-		priv->txrate = TX_RATE_11MBIT;
-		break;
-	default:
-		ret = -EINVAL;
-	}
-
-	return ret;
-}
-
-static int at76_iw_handler_get_rate(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    struct iw_param *bitrate, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = 0;
-
-	switch (priv->txrate) {
-		/* return max rate if RATE_AUTO */
-	case TX_RATE_AUTO:
-		bitrate->value = 11000000;
-		break;
-	case TX_RATE_1MBIT:
-		bitrate->value = 1000000;
-		break;
-	case TX_RATE_2MBIT:
-		bitrate->value = 2000000;
-		break;
-	case TX_RATE_5_5MBIT:
-		bitrate->value = 5500000;
-		break;
-	case TX_RATE_11MBIT:
-		bitrate->value = 11000000;
-		break;
-	default:
-		ret = -EINVAL;
-	}
-
-	bitrate->fixed = (priv->txrate != TX_RATE_AUTO);
-	bitrate->disabled = 0;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWRATE - %d", netdev->name,
-		 bitrate->value);
-
-	return ret;
-}
-
-static int at76_iw_handler_set_rts(struct net_device *netdev,
-				   struct iw_request_info *info,
-				   struct iw_param *rts, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = -EIWCOMMIT;
-	int rthr = rts->value;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWRTS - value %d disabled %s",
-		 netdev->name, rts->value, (rts->disabled) ? "true" : "false");
-
-	if (rts->disabled)
-		rthr = MAX_RTS_THRESHOLD;
-
-	if ((rthr < 0) || (rthr > MAX_RTS_THRESHOLD))
-		ret = -EINVAL;
-	else
-		priv->rts_threshold = rthr;
-
-	return ret;
-}
-
-static int at76_iw_handler_get_rts(struct net_device *netdev,
-				   struct iw_request_info *info,
-				   struct iw_param *rts, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	rts->value = priv->rts_threshold;
-	rts->disabled = (rts->value >= MAX_RTS_THRESHOLD);
-	rts->fixed = 1;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWRTS - value %d disabled %s",
-		 netdev->name, rts->value, (rts->disabled) ? "true" : "false");
-
-	return 0;
-}
-
-static int at76_iw_handler_set_frag(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    struct iw_param *frag, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = -EIWCOMMIT;
-	int fthr = frag->value;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWFRAG - value %d, disabled %s",
-		 netdev->name, frag->value,
-		 (frag->disabled) ? "true" : "false");
-
-	if (frag->disabled)
-		fthr = MAX_FRAG_THRESHOLD;
-
-	if ((fthr < MIN_FRAG_THRESHOLD) || (fthr > MAX_FRAG_THRESHOLD))
-		ret = -EINVAL;
-	else
-		priv->frag_threshold = fthr & ~0x1;	/* get an even value */
-
-	return ret;
-}
-
-static int at76_iw_handler_get_frag(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    struct iw_param *frag, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	frag->value = priv->frag_threshold;
-	frag->disabled = (frag->value >= MAX_FRAG_THRESHOLD);
-	frag->fixed = 1;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWFRAG - value %d, disabled %s",
-		 netdev->name, frag->value,
-		 (frag->disabled) ? "true" : "false");
-
-	return 0;
-}
-
-static int at76_iw_handler_get_txpow(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_param *power, char *extra)
-{
-	power->value = 15;
-	power->fixed = 1;	/* No power control */
-	power->disabled = 0;
-	power->flags = IW_TXPOW_DBM;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWTXPOW - txpow %d dBm", netdev->name,
-		 power->value);
-
-	return 0;
-}
-
-/* jal: short retry is handled by the firmware (at least 0.90.x),
-   while long retry is not (?) */
-static int at76_iw_handler_set_retry(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_param *retry, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = -EIWCOMMIT;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWRETRY disabled %d flags 0x%x val %d",
-		 netdev->name, retry->disabled, retry->flags, retry->value);
-
-	if (!retry->disabled && (retry->flags & IW_RETRY_LIMIT)) {
-		if ((retry->flags & IW_RETRY_MIN) ||
-		    !(retry->flags & IW_RETRY_MAX))
-			priv->short_retry_limit = retry->value;
-		else
-			ret = -EINVAL;
-	} else
-		ret = -EINVAL;
-
-	return ret;
-}
-
-/* Adapted (ripped) from atmel.c */
-static int at76_iw_handler_get_retry(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_param *retry, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWRETRY", netdev->name);
-
-	retry->disabled = 0;	/* Can't be disabled */
-	retry->flags = IW_RETRY_LIMIT;
-	retry->value = priv->short_retry_limit;
-
-	return 0;
-}
-
-static int at76_iw_handler_set_encode(struct net_device *netdev,
-				      struct iw_request_info *info,
-				      struct iw_point *encoding, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int index = (encoding->flags & IW_ENCODE_INDEX) - 1;
-	int len = encoding->length;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWENCODE - enc.flags %08x "
-		 "pointer %p len %d", netdev->name, encoding->flags,
-		 encoding->pointer, encoding->length);
-	at76_dbg(DBG_IOCTL,
-		 "%s: SIOCSIWENCODE - old wepstate: enabled %s key_id %d "
-		 "auth_mode %s", netdev->name,
-		 (priv->wep_enabled) ? "true" : "false", priv->wep_key_id,
-		 (priv->auth_mode ==
-		  WLAN_AUTH_SHARED_KEY) ? "restricted" : "open");
-
-	/* take the old default key if index is invalid */
-	if ((index < 0) || (index >= WEP_KEYS))
-		index = priv->wep_key_id;
-
-	if (len > 0) {
-		if (len > WEP_LARGE_KEY_LEN)
-			len = WEP_LARGE_KEY_LEN;
-
-		memset(priv->wep_keys[index], 0, WEP_KEY_LEN);
-		memcpy(priv->wep_keys[index], extra, len);
-		priv->wep_keys_len[index] = (len <= WEP_SMALL_KEY_LEN) ?
-		    WEP_SMALL_KEY_LEN : WEP_LARGE_KEY_LEN;
-		priv->wep_enabled = 1;
-	}
-
-	priv->wep_key_id = index;
-	priv->wep_enabled = ((encoding->flags & IW_ENCODE_DISABLED) == 0);
-
-	if (encoding->flags & IW_ENCODE_RESTRICTED)
-		priv->auth_mode = WLAN_AUTH_SHARED_KEY;
-	if (encoding->flags & IW_ENCODE_OPEN)
-		priv->auth_mode = WLAN_AUTH_OPEN;
-
-	at76_dbg(DBG_IOCTL,
-		 "%s: SIOCSIWENCODE - new wepstate: enabled %s key_id %d "
-		 "key_len %d auth_mode %s", netdev->name,
-		 (priv->wep_enabled) ? "true" : "false", priv->wep_key_id + 1,
-		 priv->wep_keys_len[priv->wep_key_id],
-		 (priv->auth_mode ==
-		  WLAN_AUTH_SHARED_KEY) ? "restricted" : "open");
-
-	return -EIWCOMMIT;
-}
-
-static int at76_iw_handler_get_encode(struct net_device *netdev,
-				      struct iw_request_info *info,
-				      struct iw_point *encoding, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int index = (encoding->flags & IW_ENCODE_INDEX) - 1;
-
-	if ((index < 0) || (index >= WEP_KEYS))
-		index = priv->wep_key_id;
-
-	encoding->flags =
-	    (priv->auth_mode == WLAN_AUTH_SHARED_KEY) ?
-	    IW_ENCODE_RESTRICTED : IW_ENCODE_OPEN;
-
-	if (!priv->wep_enabled)
-		encoding->flags |= IW_ENCODE_DISABLED;
-
-	if (encoding->pointer) {
-		encoding->length = priv->wep_keys_len[index];
-
-		memcpy(extra, priv->wep_keys[index], priv->wep_keys_len[index]);
-
-		encoding->flags |= (index + 1);
-	}
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWENCODE - enc.flags %08x "
-		 "pointer %p len %d", netdev->name, encoding->flags,
-		 encoding->pointer, encoding->length);
-	at76_dbg(DBG_IOCTL,
-		 "%s: SIOCGIWENCODE - wepstate: enabled %s key_id %d "
-		 "key_len %d auth_mode %s", netdev->name,
-		 (priv->wep_enabled) ? "true" : "false", priv->wep_key_id + 1,
-		 priv->wep_keys_len[priv->wep_key_id],
-		 (priv->auth_mode ==
-		  WLAN_AUTH_SHARED_KEY) ? "restricted" : "open");
-
-	return 0;
-}
-
-static int at76_iw_handler_set_power(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_param *prq, char *extra)
-{
-	int err = -EIWCOMMIT;
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_IOCTL,
-		 "%s: SIOCSIWPOWER - disabled %s flags 0x%x value 0x%x",
-		 netdev->name, (prq->disabled) ? "true" : "false", prq->flags,
-		 prq->value);
-
-	if (prq->disabled)
-		priv->pm_mode = AT76_PM_OFF;
-	else {
-		switch (prq->flags & IW_POWER_MODE) {
-		case IW_POWER_ALL_R:
-		case IW_POWER_ON:
-			break;
-		default:
-			err = -EINVAL;
-			goto exit;
-		}
-		if (prq->flags & IW_POWER_PERIOD)
-			priv->pm_period = prq->value;
-
-		if (prq->flags & IW_POWER_TIMEOUT) {
-			err = -EINVAL;
-			goto exit;
-		}
-		priv->pm_mode = AT76_PM_ON;
-	}
-exit:
-	return err;
-}
-
-static int at76_iw_handler_get_power(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_param *power, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	power->disabled = (priv->pm_mode == AT76_PM_OFF);
-	if (!power->disabled) {
-		power->flags = IW_POWER_PERIOD | IW_POWER_ALL_R;
-		power->value = priv->pm_period;
-	}
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWPOWER - %s flags 0x%x value 0x%x",
-		 netdev->name, power->disabled ? "disabled" : "enabled",
-		 power->flags, power->value);
-
-	return 0;
-}
-
-/*******************************************************************************
- * Private IOCTLS
- */
-static int at76_iw_set_short_preamble(struct net_device *netdev,
-				      struct iw_request_info *info, char *name,
-				      char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int val = *((int *)name);
-	int ret = -EIWCOMMIT;
-
-	at76_dbg(DBG_IOCTL, "%s: AT76_SET_SHORT_PREAMBLE, %d",
-		 netdev->name, val);
-
-	if (val < PREAMBLE_TYPE_LONG || val > PREAMBLE_TYPE_AUTO)
-		ret = -EINVAL;
-	else
-		priv->preamble_type = val;
-
-	return ret;
-}
-
-static int at76_iw_get_short_preamble(struct net_device *netdev,
-				      struct iw_request_info *info,
-				      union iwreq_data *wrqu, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	snprintf(wrqu->name, sizeof(wrqu->name), "%s (%d)",
-		 preambles[priv->preamble_type], priv->preamble_type);
-	return 0;
-}
-
-static int at76_iw_set_debug(struct net_device *netdev,
-			     struct iw_request_info *info,
-			     struct iw_point *data, char *extra)
-{
-	char *ptr;
-	u32 val;
-
-	if (data->length > 0) {
-		val = simple_strtol(extra, &ptr, 0);
-
-		if (ptr == extra)
-			val = DBG_DEFAULTS;
-
-		at76_dbg(DBG_IOCTL, "%s: AT76_SET_DEBUG input %d: %s -> 0x%x",
-			 netdev->name, data->length, extra, val);
-	} else
-		val = DBG_DEFAULTS;
-
-	at76_dbg(DBG_IOCTL, "%s: AT76_SET_DEBUG, old 0x%x, new 0x%x",
-		 netdev->name, at76_debug, val);
-
-	/* jal: some more output to pin down lockups */
-	at76_dbg(DBG_IOCTL, "%s: netif running %d queue_stopped %d "
-		 "carrier_ok %d", netdev->name, netif_running(netdev),
-		 netif_queue_stopped(netdev), netif_carrier_ok(netdev));
-
-	at76_debug = val;
-
-	return 0;
-}
-
-static int at76_iw_get_debug(struct net_device *netdev,
-			     struct iw_request_info *info,
-			     union iwreq_data *wrqu, char *extra)
-{
-	snprintf(wrqu->name, sizeof(wrqu->name), "0x%08x", at76_debug);
-	return 0;
-}
-
-static int at76_iw_set_powersave_mode(struct net_device *netdev,
-				      struct iw_request_info *info, char *name,
-				      char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int val = *((int *)name);
-	int ret = -EIWCOMMIT;
-
-	at76_dbg(DBG_IOCTL, "%s: AT76_SET_POWERSAVE_MODE, %d (%s)",
-		 netdev->name, val,
-		 val == AT76_PM_OFF ? "active" : val == AT76_PM_ON ? "save" :
-		 val == AT76_PM_SMART ? "smart save" : "<invalid>");
-	if (val < AT76_PM_OFF || val > AT76_PM_SMART)
-		ret = -EINVAL;
-	else
-		priv->pm_mode = val;
-
-	return ret;
-}
-
-static int at76_iw_get_powersave_mode(struct net_device *netdev,
-				      struct iw_request_info *info,
-				      union iwreq_data *wrqu, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int *param = (int *)extra;
-
-	param[0] = priv->pm_mode;
-	return 0;
-}
-
-static int at76_iw_set_scan_times(struct net_device *netdev,
-				  struct iw_request_info *info, char *name,
-				  char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int mint = *((int *)name);
-	int maxt = *((int *)name + 1);
-	int ret = -EIWCOMMIT;
-
-	at76_dbg(DBG_IOCTL, "%s: AT76_SET_SCAN_TIMES - min %d max %d",
-		 netdev->name, mint, maxt);
-	if (mint <= 0 || maxt <= 0 || mint > maxt)
-		ret = -EINVAL;
-	else {
-		priv->scan_min_time = mint;
-		priv->scan_max_time = maxt;
-	}
-
-	return ret;
-}
-
-static int at76_iw_get_scan_times(struct net_device *netdev,
-				  struct iw_request_info *info,
-				  union iwreq_data *wrqu, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int *param = (int *)extra;
-
-	param[0] = priv->scan_min_time;
-	param[1] = priv->scan_max_time;
-	return 0;
-}
-
-static int at76_iw_set_scan_mode(struct net_device *netdev,
-				 struct iw_request_info *info, char *name,
-				 char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int val = *((int *)name);
-	int ret = -EIWCOMMIT;
-
-	at76_dbg(DBG_IOCTL, "%s: AT76_SET_SCAN_MODE - mode %s",
-		 netdev->name, (val = SCAN_TYPE_ACTIVE) ? "active" :
-		 (val = SCAN_TYPE_PASSIVE) ? "passive" : "<invalid>");
-
-	if (val != SCAN_TYPE_ACTIVE && val != SCAN_TYPE_PASSIVE)
-		ret = -EINVAL;
-	else
-		priv->scan_mode = val;
-
-	return ret;
-}
-
-static int at76_iw_get_scan_mode(struct net_device *netdev,
-				 struct iw_request_info *info,
-				 union iwreq_data *wrqu, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int *param = (int *)extra;
-
-	param[0] = priv->scan_mode;
-	return 0;
-}
-
-#define AT76_SET_HANDLER(h, f) [h - SIOCIWFIRST] = (iw_handler) f
-
-/* Standard wireless handlers */
-static const iw_handler at76_handlers[] = {
-	AT76_SET_HANDLER(SIOCSIWCOMMIT, at76_iw_handler_commit),
-	AT76_SET_HANDLER(SIOCGIWNAME, at76_iw_handler_get_name),
-	AT76_SET_HANDLER(SIOCSIWFREQ, at76_iw_handler_set_freq),
-	AT76_SET_HANDLER(SIOCGIWFREQ, at76_iw_handler_get_freq),
-	AT76_SET_HANDLER(SIOCSIWMODE, at76_iw_handler_set_mode),
-	AT76_SET_HANDLER(SIOCGIWMODE, at76_iw_handler_get_mode),
-	AT76_SET_HANDLER(SIOCGIWRANGE, at76_iw_handler_get_range),
-	AT76_SET_HANDLER(SIOCSIWSPY, at76_iw_handler_set_spy),
-	AT76_SET_HANDLER(SIOCGIWSPY, at76_iw_handler_get_spy),
-	AT76_SET_HANDLER(SIOCSIWTHRSPY, at76_iw_handler_set_thrspy),
-	AT76_SET_HANDLER(SIOCGIWTHRSPY, at76_iw_handler_get_thrspy),
-	AT76_SET_HANDLER(SIOCSIWAP, at76_iw_handler_set_wap),
-	AT76_SET_HANDLER(SIOCGIWAP, at76_iw_handler_get_wap),
-	AT76_SET_HANDLER(SIOCSIWSCAN, at76_iw_handler_set_scan),
-	AT76_SET_HANDLER(SIOCGIWSCAN, at76_iw_handler_get_scan),
-	AT76_SET_HANDLER(SIOCSIWESSID, at76_iw_handler_set_essid),
-	AT76_SET_HANDLER(SIOCGIWESSID, at76_iw_handler_get_essid),
-	AT76_SET_HANDLER(SIOCSIWRATE, at76_iw_handler_set_rate),
-	AT76_SET_HANDLER(SIOCGIWRATE, at76_iw_handler_get_rate),
-	AT76_SET_HANDLER(SIOCSIWRTS, at76_iw_handler_set_rts),
-	AT76_SET_HANDLER(SIOCGIWRTS, at76_iw_handler_get_rts),
-	AT76_SET_HANDLER(SIOCSIWFRAG, at76_iw_handler_set_frag),
-	AT76_SET_HANDLER(SIOCGIWFRAG, at76_iw_handler_get_frag),
-	AT76_SET_HANDLER(SIOCGIWTXPOW, at76_iw_handler_get_txpow),
-	AT76_SET_HANDLER(SIOCSIWRETRY, at76_iw_handler_set_retry),
-	AT76_SET_HANDLER(SIOCGIWRETRY, at76_iw_handler_get_retry),
-	AT76_SET_HANDLER(SIOCSIWENCODE, at76_iw_handler_set_encode),
-	AT76_SET_HANDLER(SIOCGIWENCODE, at76_iw_handler_get_encode),
-	AT76_SET_HANDLER(SIOCSIWPOWER, at76_iw_handler_set_power),
-	AT76_SET_HANDLER(SIOCGIWPOWER, at76_iw_handler_get_power)
-};
-
-#define AT76_SET_PRIV(h, f) [h - SIOCIWFIRSTPRIV] = (iw_handler) f
-
-/* Private wireless handlers */
-static const iw_handler at76_priv_handlers[] = {
-	AT76_SET_PRIV(AT76_SET_SHORT_PREAMBLE, at76_iw_set_short_preamble),
-	AT76_SET_PRIV(AT76_GET_SHORT_PREAMBLE, at76_iw_get_short_preamble),
-	AT76_SET_PRIV(AT76_SET_DEBUG, at76_iw_set_debug),
-	AT76_SET_PRIV(AT76_GET_DEBUG, at76_iw_get_debug),
-	AT76_SET_PRIV(AT76_SET_POWERSAVE_MODE, at76_iw_set_powersave_mode),
-	AT76_SET_PRIV(AT76_GET_POWERSAVE_MODE, at76_iw_get_powersave_mode),
-	AT76_SET_PRIV(AT76_SET_SCAN_TIMES, at76_iw_set_scan_times),
-	AT76_SET_PRIV(AT76_GET_SCAN_TIMES, at76_iw_get_scan_times),
-	AT76_SET_PRIV(AT76_SET_SCAN_MODE, at76_iw_set_scan_mode),
-	AT76_SET_PRIV(AT76_GET_SCAN_MODE, at76_iw_get_scan_mode),
-};
-
-/* Names and arguments of private wireless handlers */
-static const struct iw_priv_args at76_priv_args[] = {
-	/* 0 - long, 1 - short */
-	{AT76_SET_SHORT_PREAMBLE,
-	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "set_preamble"},
-
-	{AT76_GET_SHORT_PREAMBLE,
-	 0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 10, "get_preamble"},
-
-	/* we must pass the new debug mask as a string, because iwpriv cannot
-	 * parse hex numbers starting with 0x :-(  */
-	{AT76_SET_DEBUG,
-	 IW_PRIV_TYPE_CHAR | 10, 0, "set_debug"},
-
-	{AT76_GET_DEBUG,
-	 0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 10, "get_debug"},
-
-	/* 1 - active, 2 - power save, 3 - smart power save */
-	{AT76_SET_POWERSAVE_MODE,
-	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "set_powersave"},
-
-	{AT76_GET_POWERSAVE_MODE,
-	 0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "get_powersave"},
-
-	/* min_channel_time, max_channel_time */
-	{AT76_SET_SCAN_TIMES,
-	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 2, 0, "set_scan_times"},
-
-	{AT76_GET_SCAN_TIMES,
-	 0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 2, "get_scan_times"},
-
-	/* 0 - active, 1 - passive scan */
-	{AT76_SET_SCAN_MODE,
-	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "set_scan_mode"},
-
-	{AT76_GET_SCAN_MODE,
-	 0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "get_scan_mode"},
-};
-
-static const struct iw_handler_def at76_handler_def = {
-	.num_standard = ARRAY_SIZE(at76_handlers),
-	.num_private = ARRAY_SIZE(at76_priv_handlers),
-	.num_private_args = ARRAY_SIZE(at76_priv_args),
-	.standard = at76_handlers,
-	.private = at76_priv_handlers,
-	.private_args = at76_priv_args,
-	.get_wireless_stats = at76_get_wireless_stats,
-};
-
-static const u8 snapsig[] = { 0xaa, 0xaa, 0x03 };
-
-/* RFC 1042 encapsulates Ethernet frames in 802.2 SNAP (0xaa, 0xaa, 0x03) with
- * a SNAP OID of 0 (0x00, 0x00, 0x00) */
-static const u8 rfc1042sig[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00 };
-
-static int at76_tx(struct sk_buff *skb, struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	struct net_device_stats *stats = &priv->stats;
-	int ret = 0;
-	int wlen;
-	int submit_len;
-	struct at76_tx_buffer *tx_buffer = priv->bulk_out_buffer;
-	struct ieee80211_hdr_3addr *i802_11_hdr =
-	    (struct ieee80211_hdr_3addr *)tx_buffer->packet;
-	u8 *payload = i802_11_hdr->payload;
-	struct ethhdr *eh = (struct ethhdr *)skb->data;
-
-	if (netif_queue_stopped(netdev)) {
-		printk(KERN_ERR "%s: %s called while netdev is stopped\n",
-		       netdev->name, __func__);
-		/* skip this packet */
-		dev_kfree_skb(skb);
-		return 0;
-	}
-
-	if (priv->tx_urb->status == -EINPROGRESS) {
-		printk(KERN_ERR "%s: %s called while tx urb is pending\n",
-		       netdev->name, __func__);
-		/* skip this packet */
-		dev_kfree_skb(skb);
-		return 0;
-	}
-
-	if (skb->len < ETH_HLEN) {
-		printk(KERN_ERR "%s: %s: skb too short (%d)\n",
-		       netdev->name, __func__, skb->len);
-		dev_kfree_skb(skb);
-		return 0;
-	}
-
-	at76_ledtrig_tx_activity();	/* tell ledtrigger we send a packet */
-
-	/* we can get rid of memcpy if we set netdev->hard_header_len to
-	   reserve enough space, but we would need to keep the skb around */
-
-	if (ntohs(eh->h_proto) <= ETH_DATA_LEN) {
-		/* this is a 802.3 packet */
-		if (skb->len >= ETH_HLEN + sizeof(rfc1042sig)
-		    && skb->data[ETH_HLEN] == rfc1042sig[0]
-		    && skb->data[ETH_HLEN + 1] == rfc1042sig[1]) {
-			/* higher layer delivered SNAP header - keep it */
-			memcpy(payload, skb->data + ETH_HLEN,
-			       skb->len - ETH_HLEN);
-			wlen = IEEE80211_3ADDR_LEN + skb->len - ETH_HLEN;
-		} else {
-			printk(KERN_ERR "%s: dropping non-SNAP 802.2 packet "
-			       "(DSAP 0x%02x SSAP 0x%02x cntrl 0x%02x)\n",
-			       priv->netdev->name, skb->data[ETH_HLEN],
-			       skb->data[ETH_HLEN + 1],
-			       skb->data[ETH_HLEN + 2]);
-			dev_kfree_skb(skb);
-			return 0;
-		}
-	} else {
-		/* add RFC 1042 header in front */
-		memcpy(payload, rfc1042sig, sizeof(rfc1042sig));
-		memcpy(payload + sizeof(rfc1042sig), &eh->h_proto,
-		       skb->len - offsetof(struct ethhdr, h_proto));
-		wlen = IEEE80211_3ADDR_LEN + sizeof(rfc1042sig) + skb->len -
-		    offsetof(struct ethhdr, h_proto);
-	}
-
-	/* make wireless header */
-	i802_11_hdr->frame_ctl =
-	    cpu_to_le16(IEEE80211_FTYPE_DATA |
-			(priv->wep_enabled ? IEEE80211_FCTL_PROTECTED : 0) |
-			(priv->iw_mode ==
-			 IW_MODE_INFRA ? IEEE80211_FCTL_TODS : 0));
-
-	if (priv->iw_mode == IW_MODE_ADHOC) {
-		memcpy(i802_11_hdr->addr1, eh->h_dest, ETH_ALEN);
-		memcpy(i802_11_hdr->addr2, eh->h_source, ETH_ALEN);
-		memcpy(i802_11_hdr->addr3, priv->bssid, ETH_ALEN);
-	} else if (priv->iw_mode == IW_MODE_INFRA) {
-		memcpy(i802_11_hdr->addr1, priv->bssid, ETH_ALEN);
-		memcpy(i802_11_hdr->addr2, eh->h_source, ETH_ALEN);
-		memcpy(i802_11_hdr->addr3, eh->h_dest, ETH_ALEN);
-	}
-
-	i802_11_hdr->duration_id = cpu_to_le16(0);
-	i802_11_hdr->seq_ctl = cpu_to_le16(0);
-
-	/* setup 'Atmel' header */
-	tx_buffer->wlength = cpu_to_le16(wlen);
-	tx_buffer->tx_rate = priv->txrate;
-	/* for broadcast destination addresses, the firmware 0.100.x
-	   seems to choose the highest rate set with CMD_STARTUP in
-	   basic_rate_set replacing this value */
-
-	memset(tx_buffer->reserved, 0, sizeof(tx_buffer->reserved));
-
-	tx_buffer->padding = at76_calc_padding(wlen);
-	submit_len = wlen + AT76_TX_HDRLEN + tx_buffer->padding;
-
-	at76_dbg(DBG_TX_DATA_CONTENT, "%s skb->data %s", priv->netdev->name,
-		 hex2str(skb->data, 32));
-	at76_dbg(DBG_TX_DATA, "%s tx: wlen 0x%x pad 0x%x rate %d hdr %s",
-		 priv->netdev->name,
-		 le16_to_cpu(tx_buffer->wlength),
-		 tx_buffer->padding, tx_buffer->tx_rate,
-		 hex2str(i802_11_hdr, sizeof(*i802_11_hdr)));
-	at76_dbg(DBG_TX_DATA_CONTENT, "%s payload %s", priv->netdev->name,
-		 hex2str(payload, 48));
-
-	/* send stuff */
-	netif_stop_queue(netdev);
-	netdev->trans_start = jiffies;
-
-	usb_fill_bulk_urb(priv->tx_urb, priv->udev, priv->tx_pipe, tx_buffer,
-			  submit_len, at76_tx_callback, priv);
-	ret = usb_submit_urb(priv->tx_urb, GFP_ATOMIC);
-	if (ret) {
-		stats->tx_errors++;
-		printk(KERN_ERR "%s: error in tx submit urb: %d\n",
-		       netdev->name, ret);
-		if (ret == -EINVAL)
-			printk(KERN_ERR
-			       "%s: -EINVAL: tx urb %p hcpriv %p complete %p\n",
-			       priv->netdev->name, priv->tx_urb,
-			       priv->tx_urb->hcpriv, priv->tx_urb->complete);
-	} else {
-		stats->tx_bytes += skb->len;
-		dev_kfree_skb(skb);
-	}
-
-	return ret;
-}
-
-static void at76_tx_timeout(struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	if (!priv)
-		return;
-	dev_warn(&netdev->dev, "tx timeout.");
-
-	usb_unlink_urb(priv->tx_urb);
-	priv->stats.tx_errors++;
-}
-
 static int at76_submit_rx_urb(struct at76_priv *priv)
 {
 	int ret;
@@ -3256,7 +1327,7 @@ static int at76_submit_rx_urb(struct at76_priv *priv)
 
 	if (!priv->rx_urb) {
 		printk(KERN_ERR "%s: %s: priv->rx_urb is NULL\n",
-		       priv->netdev->name, __func__);
+		       wiphy_name(priv->hw->wiphy), __func__);
 		return -EFAULT;
 	}
 
@@ -3264,7 +1335,7 @@ static int at76_submit_rx_urb(struct at76_priv *priv)
 		skb = dev_alloc_skb(sizeof(struct at76_rx_buffer));
 		if (!skb) {
 			printk(KERN_ERR "%s: cannot allocate rx skbuff\n",
-			       priv->netdev->name);
+			       wiphy_name(priv->hw->wiphy));
 			ret = -ENOMEM;
 			goto exit;
 		}
@@ -3284,110 +1355,18 @@ static int at76_submit_rx_urb(struct at76_priv *priv)
 				 "usb_submit_urb returned -ENODEV");
 		else
 			printk(KERN_ERR "%s: rx, usb_submit_urb failed: %d\n",
-			       priv->netdev->name, ret);
+			       wiphy_name(priv->hw->wiphy), ret);
 	}
 
 exit:
 	if (ret < 0 && ret != -ENODEV)
 		printk(KERN_ERR "%s: cannot submit rx urb - please unload the "
 		       "driver and/or power cycle the device\n",
-		       priv->netdev->name);
+		       wiphy_name(priv->hw->wiphy));
 
 	return ret;
 }
 
-static int at76_open(struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = 0;
-
-	at76_dbg(DBG_PROC_ENTRY, "%s(): entry", __func__);
-
-	if (mutex_lock_interruptible(&priv->mtx))
-		return -EINTR;
-
-	/* if netdev->dev_addr != priv->mac_addr we must
-	   set the mac address in the device ! */
-	if (compare_ether_addr(netdev->dev_addr, priv->mac_addr)) {
-		if (at76_add_mac_address(priv, netdev->dev_addr) >= 0)
-			at76_dbg(DBG_PROGRESS, "%s: set new MAC addr %s",
-				 netdev->name, mac2str(netdev->dev_addr));
-	}
-
-	priv->scan_state = SCAN_IDLE;
-	priv->last_scan = jiffies;
-
-	ret = at76_submit_rx_urb(priv);
-	if (ret < 0) {
-		printk(KERN_ERR "%s: open: submit_rx_urb failed: %d\n",
-		       netdev->name, ret);
-		goto error;
-	}
-
-	schedule_delayed_work(&priv->dwork_restart, 0);
-
-	at76_dbg(DBG_PROC_ENTRY, "%s(): end", __func__);
-error:
-	mutex_unlock(&priv->mtx);
-	return ret < 0 ? ret : 0;
-}
-
-static int at76_stop(struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_DEVSTART, "%s: ENTER", __func__);
-
-	if (mutex_lock_interruptible(&priv->mtx))
-		return -EINTR;
-
-	at76_quiesce(priv);
-
-	if (!priv->device_unplugged) {
-		/* We are called by "ifconfig ethX down", not because the
-		 * device is not available anymore. */
-		at76_set_radio(priv, 0);
-
-		/* We unlink rx_urb because at76_open() re-submits it.
-		 * If unplugged, at76_delete_device() takes care of it. */
-		usb_kill_urb(priv->rx_urb);
-	}
-
-	/* free the bss_list */
-	at76_free_bss_list(priv);
-
-	mutex_unlock(&priv->mtx);
-	at76_dbg(DBG_DEVSTART, "%s: EXIT", __func__);
-
-	return 0;
-}
-
-static void at76_ethtool_get_drvinfo(struct net_device *netdev,
-				     struct ethtool_drvinfo *info)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	strncpy(info->driver, DRIVER_NAME, sizeof(info->driver));
-	strncpy(info->version, DRIVER_VERSION, sizeof(info->version));
-
-	usb_make_path(priv->udev, info->bus_info, sizeof(info->bus_info));
-
-	snprintf(info->fw_version, sizeof(info->fw_version), "%d.%d.%d-%d",
-		 priv->fw_version.major, priv->fw_version.minor,
-		 priv->fw_version.patch, priv->fw_version.build);
-}
-
-static u32 at76_ethtool_get_link(struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	return priv->mac_state == MAC_CONNECTED;
-}
-
-static struct ethtool_ops at76_ethtool_ops = {
-	.get_drvinfo = at76_ethtool_get_drvinfo,
-	.get_link = at76_ethtool_get_link,
-};
-
 /* Download external firmware */
 static int at76_load_external_fw(struct usb_device *udev, struct fwentry *fwe)
 {
@@ -3484,406 +1463,6 @@ exit:
 	return ret;
 }
 
-static int at76_match_essid(struct at76_priv *priv, struct bss_info *ptr)
-{
-	/* common criteria for both modi */
-
-	int ret = (priv->essid_size == 0 /* ANY ssid */  ||
-		   (priv->essid_size == ptr->ssid_len &&
-		    !memcmp(priv->essid, ptr->ssid, ptr->ssid_len)));
-	if (!ret)
-		at76_dbg(DBG_BSS_MATCH,
-			 "%s bss table entry %p: essid didn't match",
-			 priv->netdev->name, ptr);
-	return ret;
-}
-
-static inline int at76_match_mode(struct at76_priv *priv, struct bss_info *ptr)
-{
-	int ret;
-
-	if (priv->iw_mode == IW_MODE_ADHOC)
-		ret = ptr->capa & WLAN_CAPABILITY_IBSS;
-	else
-		ret = ptr->capa & WLAN_CAPABILITY_ESS;
-	if (!ret)
-		at76_dbg(DBG_BSS_MATCH,
-			 "%s bss table entry %p: mode didn't match",
-			 priv->netdev->name, ptr);
-	return ret;
-}
-
-static int at76_match_rates(struct at76_priv *priv, struct bss_info *ptr)
-{
-	int i;
-
-	for (i = 0; i < ptr->rates_len; i++) {
-		u8 rate = ptr->rates[i];
-
-		if (!(rate & 0x80))
-			continue;
-
-		/* this is a basic rate we have to support
-		   (see IEEE802.11, ch. 7.3.2.2) */
-		if (rate != (0x80 | hw_rates[0])
-		    && rate != (0x80 | hw_rates[1])
-		    && rate != (0x80 | hw_rates[2])
-		    && rate != (0x80 | hw_rates[3])) {
-			at76_dbg(DBG_BSS_MATCH,
-				 "%s: bss table entry %p: basic rate %02x not "
-				 "supported", priv->netdev->name, ptr, rate);
-			return 0;
-		}
-	}
-
-	/* if we use short preamble, the bss must support it */
-	if (priv->preamble_type == PREAMBLE_TYPE_SHORT &&
-	    !(ptr->capa & WLAN_CAPABILITY_SHORT_PREAMBLE)) {
-		at76_dbg(DBG_BSS_MATCH,
-			 "%s: %p does not support short preamble",
-			 priv->netdev->name, ptr);
-		return 0;
-	} else
-		return 1;
-}
-
-static inline int at76_match_wep(struct at76_priv *priv, struct bss_info *ptr)
-{
-	if (!priv->wep_enabled && ptr->capa & WLAN_CAPABILITY_PRIVACY) {
-		/* we have disabled WEP, but the BSS signals privacy */
-		at76_dbg(DBG_BSS_MATCH,
-			 "%s: bss table entry %p: requires encryption",
-			 priv->netdev->name, ptr);
-		return 0;
-	}
-	/* otherwise if the BSS does not signal privacy it may well
-	   accept encrypted packets from us ... */
-	return 1;
-}
-
-static inline int at76_match_bssid(struct at76_priv *priv, struct bss_info *ptr)
-{
-	if (!priv->wanted_bssid_valid ||
-	    !compare_ether_addr(ptr->bssid, priv->wanted_bssid))
-		return 1;
-
-	at76_dbg(DBG_BSS_MATCH,
-		 "%s: requested bssid - %s does not match",
-		 priv->netdev->name, mac2str(priv->wanted_bssid));
-	at76_dbg(DBG_BSS_MATCH,
-		 "      AP bssid - %s of bss table entry %p",
-		 mac2str(ptr->bssid), ptr);
-	return 0;
-}
-
-/**
- * at76_match_bss - try to find a matching bss in priv->bss
- *
- * last - last bss tried
- *
- * last == NULL signals a new round starting with priv->bss_list.next
- * this function must be called inside an acquired priv->bss_list_spinlock
- * otherwise the timeout on bss may remove the newly chosen entry
- */
-static struct bss_info *at76_match_bss(struct at76_priv *priv,
-				       struct bss_info *last)
-{
-	struct bss_info *ptr = NULL;
-	struct list_head *curr;
-
-	curr = last ? last->list.next : priv->bss_list.next;
-	while (curr != &priv->bss_list) {
-		ptr = list_entry(curr, struct bss_info, list);
-		if (at76_match_essid(priv, ptr) && at76_match_mode(priv, ptr)
-		    && at76_match_wep(priv, ptr) && at76_match_rates(priv, ptr)
-		    && at76_match_bssid(priv, ptr))
-			break;
-		curr = curr->next;
-	}
-
-	if (curr == &priv->bss_list)
-		ptr = NULL;
-	/* otherwise ptr points to the struct bss_info we have chosen */
-
-	at76_dbg(DBG_BSS_TABLE, "%s %s: returned %p", priv->netdev->name,
-		 __func__, ptr);
-	return ptr;
-}
-
-/* Start joining a matching BSS, or create own IBSS */
-static void at76_work_join(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      work_join);
-	int ret;
-	unsigned long flags;
-
-	mutex_lock(&priv->mtx);
-
-	WARN_ON(priv->mac_state != MAC_JOINING);
-	if (priv->mac_state != MAC_JOINING)
-		goto exit;
-
-	/* secure the access to priv->curr_bss ! */
-	spin_lock_irqsave(&priv->bss_list_spinlock, flags);
-	priv->curr_bss = at76_match_bss(priv, priv->curr_bss);
-	spin_unlock_irqrestore(&priv->bss_list_spinlock, flags);
-
-	if (!priv->curr_bss) {
-		/* here we haven't found a matching (i)bss ... */
-		if (priv->iw_mode == IW_MODE_ADHOC) {
-			at76_set_mac_state(priv, MAC_OWN_IBSS);
-			at76_start_ibss(priv);
-			goto exit;
-		}
-		/* haven't found a matching BSS in infra mode - try again */
-		at76_set_mac_state(priv, MAC_SCANNING);
-		schedule_work(&priv->work_start_scan);
-		goto exit;
-	}
-
-	ret = at76_join_bss(priv, priv->curr_bss);
-	if (ret < 0) {
-		printk(KERN_ERR "%s: join_bss failed with %d\n",
-		       priv->netdev->name, ret);
-		goto exit;
-	}
-
-	ret = at76_wait_completion(priv, CMD_JOIN);
-	if (ret != CMD_STATUS_COMPLETE) {
-		if (ret != CMD_STATUS_TIME_OUT)
-			printk(KERN_ERR "%s: join_bss completed with %d\n",
-			       priv->netdev->name, ret);
-		else
-			printk(KERN_INFO "%s: join_bss ssid %s timed out\n",
-			       priv->netdev->name,
-			       mac2str(priv->curr_bss->bssid));
-
-		/* retry next BSS immediately */
-		schedule_work(&priv->work_join);
-		goto exit;
-	}
-
-	/* here we have joined the (I)BSS */
-	if (priv->iw_mode == IW_MODE_ADHOC) {
-		struct bss_info *bptr = priv->curr_bss;
-		at76_set_mac_state(priv, MAC_CONNECTED);
-		/* get ESSID, BSSID and channel for priv->curr_bss */
-		priv->essid_size = bptr->ssid_len;
-		memcpy(priv->essid, bptr->ssid, bptr->ssid_len);
-		memcpy(priv->bssid, bptr->bssid, ETH_ALEN);
-		priv->channel = bptr->channel;
-		at76_iwevent_bss_connect(priv->netdev, bptr->bssid);
-		netif_carrier_on(priv->netdev);
-		netif_start_queue(priv->netdev);
-		/* just to be sure */
-		cancel_delayed_work(&priv->dwork_get_scan);
-		cancel_delayed_work(&priv->dwork_auth);
-		cancel_delayed_work(&priv->dwork_assoc);
-	} else {
-		/* send auth req */
-		priv->retries = AUTH_RETRIES;
-		at76_set_mac_state(priv, MAC_AUTH);
-		at76_auth_req(priv, priv->curr_bss, 1, NULL);
-		at76_dbg(DBG_MGMT_TIMER,
-			 "%s:%d: starting mgmt_timer + HZ", __func__, __LINE__);
-		schedule_delayed_work(&priv->dwork_auth, AUTH_TIMEOUT);
-	}
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
-/* Reap scan results */
-static void at76_dwork_get_scan(struct work_struct *work)
-{
-	int status;
-	int ret;
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      dwork_get_scan.work);
-
-	mutex_lock(&priv->mtx);
-	WARN_ON(priv->mac_state != MAC_SCANNING);
-	if (priv->mac_state != MAC_SCANNING)
-		goto exit;
-
-	status = at76_get_cmd_status(priv->udev, CMD_SCAN);
-	if (status < 0) {
-		printk(KERN_ERR "%s: %s: at76_get_cmd_status failed with %d\n",
-		       priv->netdev->name, __func__, status);
-		status = CMD_STATUS_IN_PROGRESS;
-		/* INFO: Hope it was a one off error - if not, scanning
-		   further down the line and stop this cycle */
-	}
-	at76_dbg(DBG_PROGRESS,
-		 "%s %s: got cmd_status %d (state %s, need_any %d)",
-		 priv->netdev->name, __func__, status,
-		 mac_states[priv->mac_state], priv->scan_need_any);
-
-	if (status != CMD_STATUS_COMPLETE) {
-		if ((status != CMD_STATUS_IN_PROGRESS) &&
-		    (status != CMD_STATUS_IDLE))
-			printk(KERN_ERR "%s: %s: Bad scan status: %s\n",
-			       priv->netdev->name, __func__,
-			       at76_get_cmd_status_string(status));
-
-		/* the first cmd status after scan start is always a IDLE ->
-		   start the timer to poll again until COMPLETED */
-		at76_dbg(DBG_MGMT_TIMER,
-			 "%s:%d: starting mgmt_timer for %d ticks",
-			 __func__, __LINE__, SCAN_POLL_INTERVAL);
-		schedule_delayed_work(&priv->dwork_get_scan,
-				      SCAN_POLL_INTERVAL);
-		goto exit;
-	}
-
-	if (at76_debug & DBG_BSS_TABLE)
-		at76_dump_bss_table(priv);
-
-	if (priv->scan_need_any) {
-		ret = at76_start_scan(priv, 0);
-		if (ret < 0)
-			printk(KERN_ERR
-			       "%s: %s: start_scan (ANY) failed with %d\n",
-			       priv->netdev->name, __func__, ret);
-		at76_dbg(DBG_MGMT_TIMER,
-			 "%s:%d: starting mgmt_timer for %d ticks", __func__,
-			 __LINE__, SCAN_POLL_INTERVAL);
-		schedule_delayed_work(&priv->dwork_get_scan,
-				      SCAN_POLL_INTERVAL);
-		priv->scan_need_any = 0;
-	} else {
-		priv->scan_state = SCAN_COMPLETED;
-		/* report the end of scan to user space */
-		at76_iwevent_scan_complete(priv->netdev);
-		at76_set_mac_state(priv, MAC_JOINING);
-		schedule_work(&priv->work_join);
-	}
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
-/* Handle loss of beacons from the AP */
-static void at76_dwork_beacon(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      dwork_beacon.work);
-
-	mutex_lock(&priv->mtx);
-	if (priv->mac_state != MAC_CONNECTED || priv->iw_mode != IW_MODE_INFRA)
-		goto exit;
-
-	/* We haven't received any beacons from out AP for BEACON_TIMEOUT */
-	printk(KERN_INFO "%s: lost beacon bssid %s\n",
-	       priv->netdev->name, mac2str(priv->curr_bss->bssid));
-
-	netif_carrier_off(priv->netdev);
-	netif_stop_queue(priv->netdev);
-	at76_iwevent_bss_disconnect(priv->netdev);
-	at76_set_mac_state(priv, MAC_SCANNING);
-	schedule_work(&priv->work_start_scan);
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
-/* Handle authentication response timeout */
-static void at76_dwork_auth(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      dwork_auth.work);
-
-	mutex_lock(&priv->mtx);
-	WARN_ON(priv->mac_state != MAC_AUTH);
-	if (priv->mac_state != MAC_AUTH)
-		goto exit;
-
-	at76_dbg(DBG_PROGRESS, "%s: authentication response timeout",
-		 priv->netdev->name);
-
-	if (priv->retries-- >= 0) {
-		at76_auth_req(priv, priv->curr_bss, 1, NULL);
-		at76_dbg(DBG_MGMT_TIMER, "%s:%d: starting mgmt_timer + HZ",
-			 __func__, __LINE__);
-		schedule_delayed_work(&priv->dwork_auth, AUTH_TIMEOUT);
-	} else {
-		/* try to get next matching BSS */
-		at76_set_mac_state(priv, MAC_JOINING);
-		schedule_work(&priv->work_join);
-	}
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
-/* Handle association response timeout */
-static void at76_dwork_assoc(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      dwork_assoc.work);
-
-	mutex_lock(&priv->mtx);
-	WARN_ON(priv->mac_state != MAC_ASSOC);
-	if (priv->mac_state != MAC_ASSOC)
-		goto exit;
-
-	at76_dbg(DBG_PROGRESS, "%s: association response timeout",
-		 priv->netdev->name);
-
-	if (priv->retries-- >= 0) {
-		at76_assoc_req(priv, priv->curr_bss);
-		at76_dbg(DBG_MGMT_TIMER, "%s:%d: starting mgmt_timer + HZ",
-			 __func__, __LINE__);
-		schedule_delayed_work(&priv->dwork_assoc, ASSOC_TIMEOUT);
-	} else {
-		/* try to get next matching BSS */
-		at76_set_mac_state(priv, MAC_JOINING);
-		schedule_work(&priv->work_join);
-	}
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
-/* Read new bssid in ad-hoc mode */
-static void at76_work_new_bss(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      work_new_bss);
-	int ret;
-	struct mib_mac_mgmt mac_mgmt;
-
-	mutex_lock(&priv->mtx);
-
-	ret = at76_get_mib(priv->udev, MIB_MAC_MGMT, &mac_mgmt,
-			   sizeof(struct mib_mac_mgmt));
-	if (ret < 0) {
-		printk(KERN_ERR "%s: at76_get_mib failed: %d\n",
-		       priv->netdev->name, ret);
-		goto exit;
-	}
-
-	at76_dbg(DBG_PROGRESS, "ibss_change = 0x%2x", mac_mgmt.ibss_change);
-	memcpy(priv->bssid, mac_mgmt.current_bssid, ETH_ALEN);
-	at76_dbg(DBG_PROGRESS, "using BSSID %s", mac2str(priv->bssid));
-
-	at76_iwevent_bss_connect(priv->netdev, priv->bssid);
-
-	priv->mib_buf.type = MIB_MAC_MGMT;
-	priv->mib_buf.size = 1;
-	priv->mib_buf.index = offsetof(struct mib_mac_mgmt, ibss_change);
-	priv->mib_buf.data.byte = 0;
-
-	ret = at76_set_mib(priv, &priv->mib_buf);
-	if (ret < 0)
-		printk(KERN_ERR "%s: set_mib (ibss change ok) failed: %d\n",
-		       priv->netdev->name, ret);
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
 static int at76_startup_device(struct at76_priv *priv)
 {
 	struct at76_card_config *ccfg = &priv->card_config;
@@ -3891,14 +1470,14 @@ static int at76_startup_device(struct at76_priv *priv)
 
 	at76_dbg(DBG_PARAMS,
 		 "%s param: ssid %.*s (%s) mode %s ch %d wep %s key %d "
-		 "keylen %d", priv->netdev->name, priv->essid_size, priv->essid,
-		 hex2str(priv->essid, IW_ESSID_MAX_SIZE),
+		 "keylen %d", wiphy_name(priv->hw->wiphy), priv->essid_size,
+		 priv->essid, hex2str(priv->essid, IW_ESSID_MAX_SIZE),
 		 priv->iw_mode == IW_MODE_ADHOC ? "adhoc" : "infra",
 		 priv->channel, priv->wep_enabled ? "enabled" : "disabled",
 		 priv->wep_key_id, priv->wep_keys_len[priv->wep_key_id]);
 	at76_dbg(DBG_PARAMS,
 		 "%s param: preamble %s rts %d retry %d frag %d "
-		 "txrate %s auth_mode %d", priv->netdev->name,
+		 "txrate %s auth_mode %d", wiphy_name(priv->hw->wiphy),
 		 preambles[priv->preamble_type], priv->rts_threshold,
 		 priv->short_retry_limit, priv->frag_threshold,
 		 priv->txrate == TX_RATE_1MBIT ? "1MBit" : priv->txrate ==
@@ -3909,7 +1488,7 @@ static int at76_startup_device(struct at76_priv *priv)
 	at76_dbg(DBG_PARAMS,
 		 "%s param: pm_mode %d pm_period %d auth_mode %s "
 		 "scan_times %d %d scan_mode %s",
-		 priv->netdev->name, priv->pm_mode, priv->pm_period,
+		 wiphy_name(priv->hw->wiphy), priv->pm_mode, priv->pm_period,
 		 priv->auth_mode == WLAN_AUTH_OPEN ? "open" : "shared_secret",
 		 priv->scan_min_time, priv->scan_max_time,
 		 priv->scan_mode == SCAN_TYPE_ACTIVE ? "active" : "passive");
@@ -3943,7 +1522,8 @@ static int at76_startup_device(struct at76_priv *priv)
 	ccfg->ssid_len = priv->essid_size;
 
 	ccfg->wep_default_key_id = priv->wep_key_id;
-	memcpy(ccfg->wep_default_key_value, priv->wep_keys, 4 * WEP_KEY_LEN);
+	memcpy(ccfg->wep_default_key_value, priv->wep_keys,
+	       sizeof(priv->wep_keys));
 
 	ccfg->short_preamble = priv->preamble_type;
 	ccfg->beacon_period = cpu_to_le16(priv->beacon_period);
@@ -3952,7 +1532,7 @@ static int at76_startup_device(struct at76_priv *priv)
 				    sizeof(struct at76_card_config));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_set_card_command failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		return ret;
 	}
 
@@ -3998,69 +1578,6 @@ static int at76_startup_device(struct at76_priv *priv)
 	return 0;
 }
 
-/* Restart the interface */
-static void at76_dwork_restart(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      dwork_restart.work);
-
-	mutex_lock(&priv->mtx);
-
-	netif_carrier_off(priv->netdev);	/* stop netdev watchdog */
-	netif_stop_queue(priv->netdev);	/* stop tx data packets */
-
-	at76_startup_device(priv);
-
-	if (priv->iw_mode != IW_MODE_MONITOR) {
-		priv->netdev->type = ARPHRD_ETHER;
-		at76_set_mac_state(priv, MAC_SCANNING);
-		schedule_work(&priv->work_start_scan);
-	} else {
-		priv->netdev->type = ARPHRD_IEEE80211_RADIOTAP;
-		at76_start_monitor(priv);
-	}
-
-	mutex_unlock(&priv->mtx);
-}
-
-/* Initiate scanning */
-static void at76_work_start_scan(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      work_start_scan);
-	int ret;
-
-	mutex_lock(&priv->mtx);
-
-	WARN_ON(priv->mac_state != MAC_SCANNING);
-	if (priv->mac_state != MAC_SCANNING)
-		goto exit;
-
-	/* only clear the bss list when a scan is actively initiated,
-	 * otherwise simply rely on at76_bss_list_timeout */
-	if (priv->scan_state == SCAN_IN_PROGRESS) {
-		at76_free_bss_list(priv);
-		priv->scan_need_any = 1;
-	} else
-		priv->scan_need_any = 0;
-
-	ret = at76_start_scan(priv, 1);
-
-	if (ret < 0)
-		printk(KERN_ERR "%s: %s: start_scan failed with %d\n",
-		       priv->netdev->name, __func__, ret);
-	else {
-		at76_dbg(DBG_MGMT_TIMER,
-			 "%s:%d: starting mgmt_timer for %d ticks",
-			 __func__, __LINE__, SCAN_POLL_INTERVAL);
-		schedule_delayed_work(&priv->dwork_get_scan,
-				      SCAN_POLL_INTERVAL);
-	}
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
 /* Enable or disable promiscuous mode */
 static void at76_work_set_promisc(struct work_struct *work)
 {
@@ -4078,7 +1595,7 @@ static void at76_work_set_promisc(struct work_struct *work)
 	ret = at76_set_mib(priv, &priv->mib_buf);
 	if (ret < 0)
 		printk(KERN_ERR "%s: set_mib (promiscuous_mode) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	mutex_unlock(&priv->mtx);
 }
@@ -4094,1088 +1611,759 @@ static void at76_work_submit_rx(struct work_struct *work)
 	mutex_unlock(&priv->mtx);
 }
 
-/* We got an association response */
-static void at76_rx_mgmt_assoc(struct at76_priv *priv,
-			       struct at76_rx_buffer *buf)
+static void at76_rx_tasklet(unsigned long param)
 {
-	struct ieee80211_assoc_response *resp =
-	    (struct ieee80211_assoc_response *)buf->packet;
-	u16 assoc_id = le16_to_cpu(resp->aid);
-	u16 status = le16_to_cpu(resp->status);
-
-	at76_dbg(DBG_RX_MGMT, "%s: rx AssocResp bssid %s capa 0x%04x status "
-		 "0x%04x assoc_id 0x%04x rates %s", priv->netdev->name,
-		 mac2str(resp->header.addr3), le16_to_cpu(resp->capability),
-		 status, assoc_id, hex2str(resp->info_element->data,
-					   resp->info_element->len));
-
-	if (priv->mac_state != MAC_ASSOC) {
-		printk(KERN_INFO "%s: AssocResp in state %s ignored\n",
-		       priv->netdev->name, mac_states[priv->mac_state]);
-		return;
-	}
-
-	BUG_ON(!priv->curr_bss);
-
-	cancel_delayed_work(&priv->dwork_assoc);
-	if (status == WLAN_STATUS_SUCCESS) {
-		struct bss_info *ptr = priv->curr_bss;
-		priv->assoc_id = assoc_id & 0x3fff;
-		/* update iwconfig params */
-		memcpy(priv->bssid, ptr->bssid, ETH_ALEN);
-		memcpy(priv->essid, ptr->ssid, ptr->ssid_len);
-		priv->essid_size = ptr->ssid_len;
-		priv->channel = ptr->channel;
-		schedule_work(&priv->work_assoc_done);
-	} else {
-		at76_set_mac_state(priv, MAC_JOINING);
-		schedule_work(&priv->work_join);
-	}
-}
+	struct urb *urb = (struct urb *)param;
+	struct at76_priv *priv = urb->context;
+	struct at76_rx_buffer *buf;
+	struct ieee80211_rx_status rx_status = { 0 };
 
-/* Process disassociation request from the AP */
-static void at76_rx_mgmt_disassoc(struct at76_priv *priv,
-				  struct at76_rx_buffer *buf)
-{
-	struct ieee80211_disassoc *resp =
-	    (struct ieee80211_disassoc *)buf->packet;
-	struct ieee80211_hdr_3addr *mgmt = &resp->header;
-
-	at76_dbg(DBG_RX_MGMT,
-		 "%s: rx DisAssoc bssid %s reason 0x%04x destination %s",
-		 priv->netdev->name, mac2str(mgmt->addr3),
-		 le16_to_cpu(resp->reason), mac2str(mgmt->addr1));
-
-	/* We are not connected, ignore */
-	if (priv->mac_state == MAC_SCANNING || priv->mac_state == MAC_INIT
-	    || !priv->curr_bss)
+	if (priv->device_unplugged) {
+		at76_dbg(DBG_DEVSTART, "device unplugged");
+		if (urb)
+			at76_dbg(DBG_DEVSTART, "urb status %d", urb->status);
 		return;
+	}
 
-	/* Not our BSSID, ignore */
-	if (compare_ether_addr(mgmt->addr3, priv->curr_bss->bssid))
+	if (!priv->rx_skb || !priv->rx_skb->data)
 		return;
 
-	/* Not for our STA and not broadcast, ignore */
-	if (compare_ether_addr(priv->netdev->dev_addr, mgmt->addr1)
-	    && !is_broadcast_ether_addr(mgmt->addr1))
-		return;
+	buf = (struct at76_rx_buffer *)priv->rx_skb->data;
 
-	if (priv->mac_state != MAC_ASSOC && priv->mac_state != MAC_CONNECTED
-	    && priv->mac_state != MAC_JOINING) {
-		printk(KERN_INFO "%s: DisAssoc in state %s ignored\n",
-		       priv->netdev->name, mac_states[priv->mac_state]);
+	if (urb->status != 0) {
+		if (urb->status != -ENOENT && urb->status != -ECONNRESET)
+			at76_dbg(DBG_URB,
+				 "%s %s: - nonzero Rx bulk status received: %d",
+				 __func__, wiphy_name(priv->hw->wiphy),
+				 urb->status);
 		return;
 	}
 
-	if (priv->mac_state == MAC_CONNECTED) {
-		netif_carrier_off(priv->netdev);
-		netif_stop_queue(priv->netdev);
-		at76_iwevent_bss_disconnect(priv->netdev);
-	}
-	cancel_delayed_work(&priv->dwork_get_scan);
-	cancel_delayed_work(&priv->dwork_beacon);
-	cancel_delayed_work(&priv->dwork_auth);
-	cancel_delayed_work(&priv->dwork_assoc);
-	at76_set_mac_state(priv, MAC_JOINING);
-	schedule_work(&priv->work_join);
+	at76_dbg(DBG_RX_ATMEL_HDR,
+		 "%s: rx frame: rate %d rssi %d noise %d link %d",
+		 wiphy_name(priv->hw->wiphy), buf->rx_rate, buf->rssi,
+		 buf->noise_level, buf->link_quality);
+
+	skb_trim(priv->rx_skb, le16_to_cpu(buf->wlength) + AT76_RX_HDRLEN);
+	at76_dbg_dump(DBG_RX_DATA, &priv->rx_skb->data[AT76_RX_HDRLEN],
+		      priv->rx_skb->len, "RX: len=%d",
+		      (int)(priv->rx_skb->len - AT76_RX_HDRLEN));
+
+	rx_status.signal = buf->rssi;
+	/* FIXME: is rate_idx still present in structure? */
+	rx_status.rate_idx = buf->rx_rate;
+	rx_status.flag |= RX_FLAG_DECRYPTED;
+	rx_status.flag |= RX_FLAG_IV_STRIPPED;
+
+	skb_pull(priv->rx_skb, AT76_RX_HDRLEN);
+	at76_dbg(DBG_MAC80211, "calling ieee80211_rx_irqsafe(): %d/%d",
+		 priv->rx_skb->len, priv->rx_skb->data_len);
+	ieee80211_rx_irqsafe(priv->hw, priv->rx_skb, &rx_status);
+
+	/* Use a new skb for the next receive */
+	priv->rx_skb = NULL;
+
+	at76_submit_rx_urb(priv);
 }
 
-static void at76_rx_mgmt_auth(struct at76_priv *priv,
-			      struct at76_rx_buffer *buf)
+/* Load firmware into kernel memory and parse it */
+static struct fwentry *at76_load_firmware(struct usb_device *udev,
+					  enum board_type board_type)
 {
-	struct ieee80211_auth *resp = (struct ieee80211_auth *)buf->packet;
-	struct ieee80211_hdr_3addr *mgmt = &resp->header;
-	int seq_nr = le16_to_cpu(resp->transaction);
-	int alg = le16_to_cpu(resp->algorithm);
-	int status = le16_to_cpu(resp->status);
-
-	at76_dbg(DBG_RX_MGMT,
-		 "%s: rx AuthFrame bssid %s alg %d seq_nr %d status %d "
-		 "destination %s", priv->netdev->name, mac2str(mgmt->addr3),
-		 alg, seq_nr, status, mac2str(mgmt->addr1));
-
-	if (alg == WLAN_AUTH_SHARED_KEY && seq_nr == 2)
-		at76_dbg(DBG_RX_MGMT, "%s: AuthFrame challenge %s ...",
-			 priv->netdev->name, hex2str(resp->info_element, 18));
-
-	if (priv->mac_state != MAC_AUTH) {
-		printk(KERN_INFO "%s: ignored AuthFrame in state %s\n",
-		       priv->netdev->name, mac_states[priv->mac_state]);
-		return;
-	}
-	if (priv->auth_mode != alg) {
-		printk(KERN_INFO "%s: ignored AuthFrame for alg %d\n",
-		       priv->netdev->name, alg);
-		return;
+	int ret;
+	char *str;
+	struct at76_fw_header *fwh;
+	struct fwentry *fwe = &firmwares[board_type];
+
+	mutex_lock(&fw_mutex);
+
+	if (fwe->loaded) {
+		at76_dbg(DBG_FW, "re-using previously loaded fw");
+		goto exit;
 	}
 
-	BUG_ON(!priv->curr_bss);
+	at76_dbg(DBG_FW, "downloading firmware %s", fwe->fwname);
+	ret = request_firmware(&fwe->fw, fwe->fwname, &udev->dev);
+	if (ret < 0) {
+		dev_printk(KERN_ERR, &udev->dev, "firmware %s not found!\n",
+			   fwe->fwname);
+		dev_printk(KERN_ERR, &udev->dev,
+			   "you may need to download the firmware from "
+			   "http://developer.berlios.de/projects/at76c503a/\n");
+		goto exit;
+	}
 
-	/* Not our BSSID or not for our STA, ignore */
-	if (compare_ether_addr(mgmt->addr3, priv->curr_bss->bssid)
-	    || compare_ether_addr(priv->netdev->dev_addr, mgmt->addr1))
-		return;
+	at76_dbg(DBG_FW, "got it.");
+	fwh = (struct at76_fw_header *)(fwe->fw->data);
 
-	cancel_delayed_work(&priv->dwork_auth);
-	if (status != WLAN_STATUS_SUCCESS) {
-		/* try to join next bss */
-		at76_set_mac_state(priv, MAC_JOINING);
-		schedule_work(&priv->work_join);
-		return;
+	if (fwe->fw->size <= sizeof(*fwh)) {
+		dev_printk(KERN_ERR, &udev->dev,
+			   "firmware is too short (0x%zx)\n", fwe->fw->size);
+		goto exit;
 	}
 
-	if (priv->auth_mode == WLAN_AUTH_OPEN || seq_nr == 4) {
-		priv->retries = ASSOC_RETRIES;
-		at76_set_mac_state(priv, MAC_ASSOC);
-		at76_assoc_req(priv, priv->curr_bss);
-		at76_dbg(DBG_MGMT_TIMER,
-			 "%s:%d: starting mgmt_timer + HZ", __func__, __LINE__);
-		schedule_delayed_work(&priv->dwork_assoc, ASSOC_TIMEOUT);
-		return;
+	/* CRC currently not checked */
+	fwe->board_type = le32_to_cpu(fwh->board_type);
+	if (fwe->board_type != board_type) {
+		dev_printk(KERN_ERR, &udev->dev,
+			   "board type mismatch, requested %u, got %u\n",
+			   board_type, fwe->board_type);
+		goto exit;
 	}
 
-	WARN_ON(seq_nr != 2);
-	at76_auth_req(priv, priv->curr_bss, seq_nr + 1, resp->info_element);
-	at76_dbg(DBG_MGMT_TIMER, "%s:%d: starting mgmt_timer + HZ", __func__,
-		 __LINE__);
-	schedule_delayed_work(&priv->dwork_auth, AUTH_TIMEOUT);
-}
+	fwe->fw_version.major = fwh->major;
+	fwe->fw_version.minor = fwh->minor;
+	fwe->fw_version.patch = fwh->patch;
+	fwe->fw_version.build = fwh->build;
 
-static void at76_rx_mgmt_deauth(struct at76_priv *priv,
-				struct at76_rx_buffer *buf)
-{
-	struct ieee80211_disassoc *resp =
-	    (struct ieee80211_disassoc *)buf->packet;
-	struct ieee80211_hdr_3addr *mgmt = &resp->header;
-
-	at76_dbg(DBG_RX_MGMT | DBG_PROGRESS,
-		 "%s: rx DeAuth bssid %s reason 0x%04x destination %s",
-		 priv->netdev->name, mac2str(mgmt->addr3),
-		 le16_to_cpu(resp->reason), mac2str(mgmt->addr1));
-
-	if (priv->mac_state != MAC_AUTH && priv->mac_state != MAC_ASSOC
-	    && priv->mac_state != MAC_CONNECTED) {
-		printk(KERN_INFO "%s: DeAuth in state %s ignored\n",
-		       priv->netdev->name, mac_states[priv->mac_state]);
-		return;
-	}
+	str = (char *)fwh + le32_to_cpu(fwh->str_offset);
+	fwe->intfw = (u8 *)fwh + le32_to_cpu(fwh->int_fw_offset);
+	fwe->intfw_size = le32_to_cpu(fwh->int_fw_len);
+	fwe->extfw = (u8 *)fwh + le32_to_cpu(fwh->ext_fw_offset);
+	fwe->extfw_size = le32_to_cpu(fwh->ext_fw_len);
 
-	BUG_ON(!priv->curr_bss);
+	fwe->loaded = 1;
 
-	/* Not our BSSID, ignore */
-	if (compare_ether_addr(mgmt->addr3, priv->curr_bss->bssid))
-		return;
+	dev_printk(KERN_DEBUG, &udev->dev,
+		   "using firmware %s (version %d.%d.%d-%d)\n",
+		   fwe->fwname, fwh->major, fwh->minor, fwh->patch, fwh->build);
 
-	/* Not for our STA and not broadcast, ignore */
-	if (compare_ether_addr(priv->netdev->dev_addr, mgmt->addr1)
-	    && !is_broadcast_ether_addr(mgmt->addr1))
-		return;
+	at76_dbg(DBG_DEVSTART, "board %u, int %d:%d, ext %d:%d", board_type,
+		 le32_to_cpu(fwh->int_fw_offset), le32_to_cpu(fwh->int_fw_len),
+		 le32_to_cpu(fwh->ext_fw_offset), le32_to_cpu(fwh->ext_fw_len));
+	at76_dbg(DBG_DEVSTART, "firmware id %s", str);
 
-	if (priv->mac_state == MAC_CONNECTED)
-		at76_iwevent_bss_disconnect(priv->netdev);
+exit:
+	mutex_unlock(&fw_mutex);
 
-	at76_set_mac_state(priv, MAC_JOINING);
-	schedule_work(&priv->work_join);
-	cancel_delayed_work(&priv->dwork_get_scan);
-	cancel_delayed_work(&priv->dwork_beacon);
-	cancel_delayed_work(&priv->dwork_auth);
-	cancel_delayed_work(&priv->dwork_assoc);
+	if (fwe->loaded)
+		return fwe;
+	else
+		return NULL;
 }
 
-static void at76_rx_mgmt_beacon(struct at76_priv *priv,
-				struct at76_rx_buffer *buf)
+static void at76_mac80211_tx_callback(struct urb *urb)
 {
-	int varpar_len;
-	/* beacon content */
-	struct ieee80211_beacon *bdata = (struct ieee80211_beacon *)buf->packet;
-	struct ieee80211_hdr_3addr *mgmt = &bdata->header;
-
-	struct list_head *lptr;
-	struct bss_info *match;	/* entry matching addr3 with its bssid */
-	int new_entry = 0;
-	int len;
-	struct ieee80211_info_element *ie;
-	int have_ssid = 0;
-	int have_rates = 0;
-	int have_channel = 0;
-	int keep_going = 1;
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->bss_list_spinlock, flags);
-	if (priv->mac_state == MAC_CONNECTED) {
-		/* in state MAC_CONNECTED we use the mgmt_timer to control
-		   the beacon of the BSS */
-		BUG_ON(!priv->curr_bss);
-
-		if (!compare_ether_addr(priv->curr_bss->bssid, mgmt->addr3)) {
-			/* We got our AP's beacon, defer the timeout handler.
-			   Kill pending work first, as schedule_delayed_work()
-			   won't do it. */
-			cancel_delayed_work(&priv->dwork_beacon);
-			schedule_delayed_work(&priv->dwork_beacon,
-					      BEACON_TIMEOUT);
-			priv->curr_bss->rssi = buf->rssi;
-			priv->beacons_received++;
-			goto exit;
-		}
-	}
+	struct at76_priv *priv = urb->context;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(priv->tx_skb);
 
-	/* look if we have this BSS already in the list */
-	match = NULL;
+	at76_dbg(DBG_MAC80211, "%s()", __func__);
 
-	if (!list_empty(&priv->bss_list)) {
-		list_for_each(lptr, &priv->bss_list) {
-			struct bss_info *bss_ptr =
-			    list_entry(lptr, struct bss_info, list);
-			if (!compare_ether_addr(bss_ptr->bssid, mgmt->addr3)) {
-				match = bss_ptr;
-				break;
-			}
-		}
+	switch (urb->status) {
+	case 0:
+		/* success */
+		/* FIXME:
+		 * is the frame really ACKed when tx_callback is called ? */
+		info->flags |= IEEE80211_TX_STAT_ACK;
+		break;
+	case -ENOENT:
+	case -ECONNRESET:
+		/* fail, urb has been unlinked */
+		/* FIXME: add error message */
+		break;
+	default:
+		at76_dbg(DBG_URB, "%s - nonzero tx status received: %d",
+			 __func__, urb->status);
+		break;
 	}
 
-	if (!match) {
-		/* BSS not in the list - append it */
-		match = kzalloc(sizeof(struct bss_info), GFP_ATOMIC);
-		if (!match) {
-			at76_dbg(DBG_BSS_TABLE,
-				 "%s: cannot kmalloc new bss info (%zd byte)",
-				 priv->netdev->name, sizeof(struct bss_info));
-			goto exit;
-		}
-		new_entry = 1;
-		list_add_tail(&match->list, &priv->bss_list);
-	}
+	memset(&info->status, 0, sizeof(info->status));
 
-	match->capa = le16_to_cpu(bdata->capability);
-	match->beacon_interval = le16_to_cpu(bdata->beacon_interval);
-	match->rssi = buf->rssi;
-	match->link_qual = buf->link_quality;
-	match->noise_level = buf->noise_level;
-	memcpy(match->bssid, mgmt->addr3, ETH_ALEN);
-	at76_dbg(DBG_RX_BEACON, "%s: bssid %s", priv->netdev->name,
-		 mac2str(match->bssid));
-
-	ie = bdata->info_element;
-
-	/* length of var length beacon parameters */
-	varpar_len = min_t(int, le16_to_cpu(buf->wlength) -
-			   sizeof(struct ieee80211_beacon),
-			   BEACON_MAX_DATA_LENGTH);
-
-	/* This routine steps through the bdata->data array to get
-	 * some useful information about the access point.
-	 * Currently, this implementation supports receipt of: SSID,
-	 * supported transfer rates and channel, in any order, with some
-	 * tolerance for intermittent unknown codes (although this
-	 * functionality may not be necessary as the useful information will
-	 * usually arrive in consecutively, but there have been some
-	 * reports of some of the useful information fields arriving in a
-	 * different order).
-	 * It does not support any more IE types although MFIE_TYPE_TIM may
-	 * be supported (on my AP at least).
-	 * The bdata->data array is about 1500 bytes long but only ~36 of those
-	 * bytes are useful, hence the have_ssid etc optimizations. */
-
-	while (keep_going &&
-	       ((&ie->data[ie->len] - (u8 *)bdata->info_element) <=
-		varpar_len)) {
-
-		switch (ie->id) {
-
-		case MFIE_TYPE_SSID:
-			if (have_ssid)
-				break;
+	ieee80211_tx_status_irqsafe(priv->hw, priv->tx_skb);
 
-			len = min_t(int, IW_ESSID_MAX_SIZE, ie->len);
+	priv->tx_skb = NULL;
 
-			/* we copy only if this is a new entry,
-			   or the incoming SSID is not a hidden SSID. This
-			   will protect us from overwriting a real SSID read
-			   in a ProbeResponse with a hidden one from a
-			   following beacon. */
-			if (!new_entry && at76_is_hidden_ssid(ie->data, len)) {
-				have_ssid = 1;
-				break;
-			}
+	ieee80211_wake_queues(priv->hw);
+}
 
-			match->ssid_len = len;
-			memcpy(match->ssid, ie->data, len);
-			at76_dbg(DBG_RX_BEACON, "%s: SSID - %.*s",
-				 priv->netdev->name, len, match->ssid);
-			have_ssid = 1;
-			break;
+static int at76_mac80211_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct at76_priv *priv = hw->priv;
+	struct at76_tx_buffer *tx_buffer = priv->bulk_out_buffer;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	int padding, submit_len, ret;
 
-		case MFIE_TYPE_RATES:
-			if (have_rates)
-				break;
+	at76_dbg(DBG_MAC80211, "%s()", __func__);
 
-			match->rates_len =
-			    min_t(int, sizeof(match->rates), ie->len);
-			memcpy(match->rates, ie->data, match->rates_len);
-			have_rates = 1;
-			at76_dbg(DBG_RX_BEACON, "%s: SUPPORTED RATES %s",
-				 priv->netdev->name,
-				 hex2str(ie->data, ie->len));
-			break;
+	if (priv->tx_urb->status == -EINPROGRESS) {
+		printk(KERN_ERR "%s: %s called while tx urb is pending\n",
+		       wiphy_name(priv->hw->wiphy), __func__);
+		return NETDEV_TX_BUSY;
+	}
 
-		case MFIE_TYPE_DS_SET:
-			if (have_channel)
-				break;
+	ieee80211_stop_queues(hw);
 
-			match->channel = ie->data[0];
-			have_channel = 1;
-			at76_dbg(DBG_RX_BEACON, "%s: CHANNEL - %d",
-				 priv->netdev->name, match->channel);
-			break;
+	at76_ledtrig_tx_activity();	/* tell ledtrigger we send a packet */
 
-		case MFIE_TYPE_CF_SET:
-		case MFIE_TYPE_TIM:
-		case MFIE_TYPE_IBSS_SET:
-		default:
-			at76_dbg(DBG_RX_BEACON, "%s: beacon IE id %d len %d %s",
-				 priv->netdev->name, ie->id, ie->len,
-				 hex2str(ie->data, ie->len));
-			break;
-		}
+	WARN_ON(priv->tx_skb != NULL);
 
-		/* advance to the next informational element */
-		next_ie(&ie);
+	priv->tx_skb = skb;
+	padding = at76_calc_padding(skb->len);
+	submit_len = AT76_TX_HDRLEN + skb->len + padding;
 
-		/* Optimization: after all, the bdata->data array is
-		 * varpar_len bytes long, whereas we get all of the useful
-		 * information after only ~36 bytes, this saves us a lot of
-		 * time (and trouble as the remaining portion of the array
-		 * could be full of junk)
-		 * Comment this out if you want to see what other information
-		 * comes from the AP - although little of it may be useful */
-	}
+	/* setup 'Atmel' header */
+	memset(tx_buffer, 0, sizeof(*tx_buffer));
+	tx_buffer->padding = padding;
+	tx_buffer->wlength = cpu_to_le16(skb->len);
+	tx_buffer->tx_rate = ieee80211_get_tx_rate(hw, info)->hw_value;
+	if (FIRMWARE_IS_WPA(priv->fw_version) && info->control.hw_key) {
+		tx_buffer->key_id = (info->control.hw_key->keyidx);
+		tx_buffer->cipher_type =
+			priv->keys[info->control.hw_key->keyidx].cipher;
+		tx_buffer->cipher_length =
+			priv->keys[info->control.hw_key->keyidx].keylen;
+		tx_buffer->reserved = 0;
+	} else {
+		tx_buffer->key_id = 0;
+		tx_buffer->cipher_type = 0;
+		tx_buffer->cipher_length = 0;
+		tx_buffer->reserved = 0;
+	};
+	/* memset(tx_buffer->reserved, 0, sizeof(tx_buffer->reserved)); */
+	memcpy(tx_buffer->packet, skb->data, skb->len);
 
-	at76_dbg(DBG_RX_BEACON, "%s: Finished processing beacon data",
-		 priv->netdev->name);
+	at76_dbg(DBG_TX_DATA, "%s tx: wlen 0x%x pad 0x%x rate %d hdr",
+		 wiphy_name(priv->hw->wiphy), le16_to_cpu(tx_buffer->wlength),
+		 tx_buffer->padding, tx_buffer->tx_rate);
 
-	match->last_rx = jiffies;	/* record last rx of beacon */
+	/* send stuff */
+	at76_dbg_dump(DBG_TX_DATA_CONTENT, tx_buffer, submit_len,
+		      "%s(): tx_buffer %d bytes:", __func__, submit_len);
+	usb_fill_bulk_urb(priv->tx_urb, priv->udev, priv->tx_pipe, tx_buffer,
+			  submit_len, at76_mac80211_tx_callback, priv);
+	ret = usb_submit_urb(priv->tx_urb, GFP_ATOMIC);
+	if (ret) {
+		printk(KERN_ERR "%s: error in tx submit urb: %d\n",
+		       wiphy_name(priv->hw->wiphy), ret);
+		if (ret == -EINVAL)
+			printk(KERN_ERR
+			       "%s: -EINVAL: tx urb %p hcpriv %p complete %p\n",
+			       wiphy_name(priv->hw->wiphy), priv->tx_urb,
+			       priv->tx_urb->hcpriv, priv->tx_urb->complete);
+	}
 
-exit:
-	spin_unlock_irqrestore(&priv->bss_list_spinlock, flags);
+	return 0;
 }
 
-/* Calculate the link level from a given rx_buffer */
-static void at76_calc_level(struct at76_priv *priv, struct at76_rx_buffer *buf,
-			    struct iw_quality *qual)
+static int at76_mac80211_start(struct ieee80211_hw *hw)
 {
-	/* just a guess for now, might be different for other chips */
-	int max_rssi = 42;
+	struct at76_priv *priv = hw->priv;
+	int ret;
 
-	qual->level = (buf->rssi * 100 / max_rssi);
-	if (qual->level > 100)
-		qual->level = 100;
-	qual->updated |= IW_QUAL_LEVEL_UPDATED;
-}
+	at76_dbg(DBG_MAC80211, "%s()", __func__);
 
-/* Calculate the link quality from a given rx_buffer */
-static void at76_calc_qual(struct at76_priv *priv, struct at76_rx_buffer *buf,
-			   struct iw_quality *qual)
-{
-	if (at76_is_intersil(priv->board_type))
-		qual->qual = buf->link_quality;
-	else {
-		unsigned long elapsed;
+	mutex_lock(&priv->mtx);
 
-		/* Update qual at most once a second */
-		elapsed = jiffies - priv->beacons_last_qual;
-		if (elapsed < 1 * HZ)
-			return;
+	ret = at76_submit_rx_urb(priv);
+	if (ret < 0) {
+		printk(KERN_ERR "%s: open: submit_rx_urb failed: %d\n",
+		       wiphy_name(priv->hw->wiphy), ret);
+		goto error;
+	}
 
-		qual->qual = qual->level * priv->beacons_received *
-		    msecs_to_jiffies(priv->beacon_period) / elapsed;
+	at76_startup_device(priv);
 
-		priv->beacons_last_qual = jiffies;
-		priv->beacons_received = 0;
-	}
-	qual->qual = (qual->qual > 100) ? 100 : qual->qual;
-	qual->updated |= IW_QUAL_QUAL_UPDATED;
-}
+	at76_start_monitor(priv);
 
-/* Calculate the noise quality from a given rx_buffer */
-static void at76_calc_noise(struct at76_priv *priv, struct at76_rx_buffer *buf,
-			    struct iw_quality *qual)
-{
-	qual->noise = 0;
-	qual->updated |= IW_QUAL_NOISE_INVALID;
+error:
+	mutex_unlock(&priv->mtx);
+
+	return 0;
 }
 
-static void at76_update_wstats(struct at76_priv *priv,
-			       struct at76_rx_buffer *buf)
+static void at76_mac80211_stop(struct ieee80211_hw *hw)
 {
-	struct iw_quality *qual = &priv->wstats.qual;
+	struct at76_priv *priv = hw->priv;
 
-	if (buf->rssi && priv->mac_state == MAC_CONNECTED) {
-		qual->updated = 0;
-		at76_calc_level(priv, buf, qual);
-		at76_calc_qual(priv, buf, qual);
-		at76_calc_noise(priv, buf, qual);
-	} else {
-		qual->qual = 0;
-		qual->level = 0;
-		qual->noise = 0;
-		qual->updated = IW_QUAL_ALL_INVALID;
-	}
-}
+	at76_dbg(DBG_MAC80211, "%s()", __func__);
 
-static void at76_rx_mgmt(struct at76_priv *priv, struct at76_rx_buffer *buf)
-{
-	struct ieee80211_hdr_3addr *mgmt =
-	    (struct ieee80211_hdr_3addr *)buf->packet;
-	u16 framectl = le16_to_cpu(mgmt->frame_ctl);
-
-	/* update wstats */
-	if (priv->mac_state != MAC_INIT && priv->mac_state != MAC_SCANNING) {
-		/* jal: this is a dirty hack needed by Tim in ad-hoc mode */
-		/* Data packets always seem to have a 0 link level, so we
-		   only read link quality info from management packets.
-		   Atmel driver actually averages the present, and previous
-		   values, we just present the raw value at the moment - TJS */
-		if (priv->iw_mode == IW_MODE_ADHOC
-		    || (priv->curr_bss
-			&& !compare_ether_addr(mgmt->addr3,
-					       priv->curr_bss->bssid)))
-			at76_update_wstats(priv, buf);
-	}
+	mutex_lock(&priv->mtx);
 
-	at76_dbg(DBG_RX_MGMT_CONTENT, "%s rx mgmt framectl 0x%x %s",
-		 priv->netdev->name, framectl,
-		 hex2str(mgmt, le16_to_cpu(buf->wlength)));
+	if (!priv->device_unplugged) {
+		/* We are called by "ifconfig ethX down", not because the
+		 * device is not available anymore. */
+		if (at76_set_radio(priv, 0) == 1)
+			at76_wait_completion(priv, CMD_RADIO_ON);
 
-	switch (framectl & IEEE80211_FCTL_STYPE) {
-	case IEEE80211_STYPE_BEACON:
-	case IEEE80211_STYPE_PROBE_RESP:
-		at76_rx_mgmt_beacon(priv, buf);
-		break;
+		/* We unlink rx_urb because at76_open() re-submits it.
+		 * If unplugged, at76_delete_device() takes care of it. */
+		usb_kill_urb(priv->rx_urb);
+	}
 
-	case IEEE80211_STYPE_ASSOC_RESP:
-		at76_rx_mgmt_assoc(priv, buf);
-		break;
+	mutex_unlock(&priv->mtx);
+}
 
-	case IEEE80211_STYPE_DISASSOC:
-		at76_rx_mgmt_disassoc(priv, buf);
-		break;
+static int at76_add_interface(struct ieee80211_hw *hw,
+			      struct ieee80211_if_init_conf *conf)
+{
+	struct at76_priv *priv = hw->priv;
+	int ret = 0;
 
-	case IEEE80211_STYPE_AUTH:
-		at76_rx_mgmt_auth(priv, buf);
-		break;
+	at76_dbg(DBG_MAC80211, "%s()", __func__);
 
-	case IEEE80211_STYPE_DEAUTH:
-		at76_rx_mgmt_deauth(priv, buf);
-		break;
+	mutex_lock(&priv->mtx);
 
+	switch (conf->type) {
+	case NL80211_IFTYPE_STATION:
+		priv->iw_mode = IW_MODE_INFRA;
+		break;
 	default:
-		printk(KERN_DEBUG "%s: ignoring frame with framectl 0x%04x\n",
-		       priv->netdev->name, framectl);
+		ret = -EOPNOTSUPP;
+		goto exit;
 	}
 
-	return;
+exit:
+	mutex_unlock(&priv->mtx);
+
+	return ret;
 }
 
-/* Convert the 802.11 header into an ethernet-style header, make skb
- * ready for consumption by netif_rx() */
-static void at76_ieee80211_to_eth(struct sk_buff *skb, int iw_mode)
+static void at76_remove_interface(struct ieee80211_hw *hw,
+				  struct ieee80211_if_init_conf *conf)
 {
-	struct ieee80211_hdr_3addr *i802_11_hdr;
-	struct ethhdr *eth_hdr_p;
-	u8 *src_addr;
-	u8 *dest_addr;
+	at76_dbg(DBG_MAC80211, "%s()", __func__);
+}
 
-	i802_11_hdr = (struct ieee80211_hdr_3addr *)skb->data;
+static int at76_join(struct at76_priv *priv)
+{
+	struct at76_req_join join;
+	int ret;
 
-	/* That would be the ethernet header if the hardware converted
-	 * the frame for us.  Make sure the source and the destination
-	 * match the 802.11 header.  Which hardware does it? */
-	eth_hdr_p = (struct ethhdr *)skb_pull(skb, IEEE80211_3ADDR_LEN);
+	memset(&join, 0, sizeof(struct at76_req_join));
+	memcpy(join.essid, priv->essid, priv->essid_size);
+	join.essid_size = priv->essid_size;
+	memcpy(join.bssid, priv->bssid, ETH_ALEN);
+	join.bss_type = INFRASTRUCTURE_MODE;
+	join.channel = priv->channel;
+	join.timeout = cpu_to_le16(2000);
 
-	dest_addr = i802_11_hdr->addr1;
-	if (iw_mode == IW_MODE_ADHOC)
-		src_addr = i802_11_hdr->addr2;
-	else
-		src_addr = i802_11_hdr->addr3;
+	at76_dbg(DBG_MAC80211, "%s: sending CMD_JOIN", __func__);
+	ret = at76_set_card_command(priv->udev, CMD_JOIN, &join,
+				    sizeof(struct at76_req_join));
 
-	if (!compare_ether_addr(eth_hdr_p->h_source, src_addr) &&
-	    !compare_ether_addr(eth_hdr_p->h_dest, dest_addr))
-		/* Yes, we already have an ethernet header */
-		skb_reset_mac_header(skb);
-	else {
-		u16 len;
-
-		/* Need to build an ethernet header */
-		if (!memcmp(skb->data, snapsig, sizeof(snapsig))) {
-			/* SNAP frame - decapsulate, keep proto */
-			skb_push(skb, offsetof(struct ethhdr, h_proto) -
-				 sizeof(rfc1042sig));
-			len = 0;
-		} else {
-			/* 802.3 frame, proto is length */
-			len = skb->len;
-			skb_push(skb, ETH_HLEN);
-		}
+	if (ret < 0) {
+		printk(KERN_ERR "%s: at76_set_card_command failed: %d\n",
+		       wiphy_name(priv->hw->wiphy), ret);
+		return 0;
+	}
 
-		skb_reset_mac_header(skb);
-		eth_hdr_p = eth_hdr(skb);
-		/* This needs to be done in this order (eth_hdr_p->h_dest may
-		 * overlap src_addr) */
-		memcpy(eth_hdr_p->h_source, src_addr, ETH_ALEN);
-		memcpy(eth_hdr_p->h_dest, dest_addr, ETH_ALEN);
-		if (len)
-			eth_hdr_p->h_proto = htons(len);
+	ret = at76_wait_completion(priv, CMD_JOIN);
+	at76_dbg(DBG_MAC80211, "%s: CMD_JOIN returned: 0x%02x", __func__, ret);
+	if (ret != CMD_STATUS_COMPLETE) {
+		printk(KERN_ERR "%s: at76_wait_completion failed: %d\n",
+		       wiphy_name(priv->hw->wiphy), ret);
+		return 0;
 	}
 
-	skb->protocol = eth_type_trans(skb, skb->dev);
+	at76_set_tkip_bssid(priv, priv->bssid);
+	at76_set_pm_mode(priv);
+
+	return 0;
 }
 
-/* Check for fragmented data in priv->rx_skb. If the packet was no fragment
-   or it was the last of a fragment set a skb containing the whole packet
-   is returned for further processing. Otherwise we get NULL and are
-   done and the packet is either stored inside the fragment buffer
-   or thrown away.  Every returned skb starts with the ieee802_11 header
-   and contains _no_ FCS at the end */
-static struct sk_buff *at76_check_for_rx_frags(struct at76_priv *priv)
+static void at76_dwork_hw_scan(struct work_struct *work)
 {
-	struct sk_buff *skb = priv->rx_skb;
-	struct at76_rx_buffer *buf = (struct at76_rx_buffer *)skb->data;
-	struct ieee80211_hdr_3addr *i802_11_hdr =
-	    (struct ieee80211_hdr_3addr *)buf->packet;
-	/* seq_ctrl, fragment_number, sequence number of new packet */
-	u16 sctl = le16_to_cpu(i802_11_hdr->seq_ctl);
-	u16 fragnr = sctl & 0xf;
-	u16 seqnr = sctl >> 4;
-	u16 frame_ctl = le16_to_cpu(i802_11_hdr->frame_ctl);
-
-	/* Length including the IEEE802.11 header, but without the trailing
-	 * FCS and without the Atmel Rx header */
-	int length = le16_to_cpu(buf->wlength) - IEEE80211_FCS_LEN;
+	struct at76_priv *priv = container_of(work, struct at76_priv,
+					      dwork_hw_scan.work);
+	int ret;
 
-	/* where does the data payload start in skb->data ? */
-	u8 *data = i802_11_hdr->payload;
+	ret = at76_get_cmd_status(priv->udev, CMD_SCAN);
+	at76_dbg(DBG_MAC80211, "%s: CMD_SCAN status 0x%02x", __func__, ret);
 
-	/* length of payload, excl. the trailing FCS */
-	int data_len = length - IEEE80211_3ADDR_LEN;
+	/* FIXME: add maximum time for scan to complete */
 
-	int i;
-	struct rx_data_buf *bptr, *optr;
-	unsigned long oldest = ~0UL;
-
-	at76_dbg(DBG_RX_FRAGS,
-		 "%s: rx data frame_ctl %04x addr2 %s seq/frag %d/%d "
-		 "length %d data %d: %s ...", priv->netdev->name, frame_ctl,
-		 mac2str(i802_11_hdr->addr2), seqnr, fragnr, length, data_len,
-		 hex2str(data, 32));
-
-	at76_dbg(DBG_RX_FRAGS_SKB, "%s: incoming skb: head %p data %p "
-		 "tail %p end %p len %d", priv->netdev->name, skb->head,
-		 skb->data, skb_tail_pointer(skb), skb_end_pointer(skb),
-		 skb->len);
-
-	if (data_len < 0) {
-		/* make sure data starts in the buffer */
-		printk(KERN_INFO "%s: data frame too short\n",
-		       priv->netdev->name);
-		return NULL;
+	if (ret != CMD_STATUS_COMPLETE) {
+		queue_delayed_work(priv->hw->workqueue, &priv->dwork_hw_scan,
+				   SCAN_POLL_INTERVAL);
+		goto exit;
 	}
 
-	WARN_ON(length <= AT76_RX_HDRLEN);
-	if (length <= AT76_RX_HDRLEN)
-		return NULL;
+	ieee80211_scan_completed(priv->hw);
 
-	/* remove the at76_rx_buffer header - we don't need it anymore */
-	/* we need the IEEE802.11 header (for the addresses) if this packet
-	   is the first of a chain */
-	skb_pull(skb, AT76_RX_HDRLEN);
-
-	/* remove FCS at end */
-	skb_trim(skb, length);
-
-	at76_dbg(DBG_RX_FRAGS_SKB, "%s: trimmed skb: head %p data %p tail %p "
-		 "end %p len %d data %p data_len %d", priv->netdev->name,
-		 skb->head, skb->data, skb_tail_pointer(skb),
-		 skb_end_pointer(skb), skb->len, data, data_len);
-
-	if (fragnr == 0 && !(frame_ctl & IEEE80211_FCTL_MOREFRAGS)) {
-		/* unfragmented packet received */
-		/* Use a new skb for the next receive */
-		priv->rx_skb = NULL;
-		at76_dbg(DBG_RX_FRAGS, "%s: unfragmented", priv->netdev->name);
-		return skb;
+	if (is_valid_ether_addr(priv->bssid)) {
+		ieee80211_wake_queues(priv->hw);
+		at76_join(priv);
 	}
 
-	/* look if we've got a chain for the sender address.
-	   afterwards optr points to first free or the oldest entry,
-	   or, if i < NR_RX_DATA_BUF, bptr points to the entry for the
-	   sender address */
-	/* determining the oldest entry doesn't cope with jiffies wrapping
-	   but I don't care to delete a young entry at these rare moments ... */
-
-	bptr = priv->rx_data;
-	optr = NULL;
-	for (i = 0; i < NR_RX_DATA_BUF; i++, bptr++) {
-		if (!bptr->skb) {
-			optr = bptr;
-			oldest = 0UL;
-			continue;
-		}
+	ieee80211_wake_queues(priv->hw);
 
-		if (!compare_ether_addr(i802_11_hdr->addr2, bptr->sender))
-			break;
+exit:
+	return;
+}
 
-		if (!optr) {
-			optr = bptr;
-			oldest = bptr->last_rx;
-		} else if (bptr->last_rx < oldest)
-			optr = bptr;
-	}
+static int at76_hw_scan(struct ieee80211_hw *hw, u8 *ssid, size_t len)
+{
+	struct at76_priv *priv = hw->priv;
+	struct at76_req_scan scan;
+	int ret;
 
-	if (i < NR_RX_DATA_BUF) {
-
-		at76_dbg(DBG_RX_FRAGS, "%s: %d. cacheentry (seq/frag = %d/%d) "
-			 "matched sender addr",
-			 priv->netdev->name, i, bptr->seqnr, bptr->fragnr);
-
-		/* bptr points to an entry for the sender address */
-		if (bptr->seqnr == seqnr) {
-			int left;
-			/* the fragment has the current sequence number */
-			if (((bptr->fragnr + 1) & 0xf) != fragnr) {
-				/* wrong fragment number -> ignore it */
-				/* is & 0xf necessary above ??? */
-				at76_dbg(DBG_RX_FRAGS,
-					 "%s: frag nr mismatch: %d + 1 != %d",
-					 priv->netdev->name, bptr->fragnr,
-					 fragnr);
-				return NULL;
-			}
-			bptr->last_rx = jiffies;
-			/* the next following fragment number ->
-			   add the data at the end */
-
-			/* for test only ??? */
-			left = skb_tailroom(bptr->skb);
-			if (left < data_len)
-				printk(KERN_INFO
-				       "%s: only %d byte free (need %d)\n",
-				       priv->netdev->name, left, data_len);
-			else
-				memcpy(skb_put(bptr->skb, data_len), data,
-				       data_len);
-
-			bptr->fragnr = fragnr;
-			if (frame_ctl & IEEE80211_FCTL_MOREFRAGS)
-				return NULL;
-
-			/* this was the last fragment - send it */
-			skb = bptr->skb;
-			bptr->skb = NULL;	/* free the entry */
-			at76_dbg(DBG_RX_FRAGS, "%s: last frag of seq %d",
-				 priv->netdev->name, seqnr);
-			return skb;
-		}
+	at76_dbg(DBG_MAC80211, "%s():", __func__);
+	at76_dbg_dump(DBG_MAC80211, ssid, len, "ssid %zd bytes:", len);
 
-		/* got another sequence number */
-		if (fragnr == 0) {
-			/* it's the start of a new chain - replace the
-			   old one by this */
-			/* bptr->sender has the correct value already */
-			at76_dbg(DBG_RX_FRAGS,
-				 "%s: start of new seq %d, removing old seq %d",
-				 priv->netdev->name, seqnr, bptr->seqnr);
-			bptr->seqnr = seqnr;
-			bptr->fragnr = 0;
-			bptr->last_rx = jiffies;
-			/* swap bptr->skb and priv->rx_skb */
-			skb = bptr->skb;
-			bptr->skb = priv->rx_skb;
-			priv->rx_skb = skb;
-		} else {
-			/* it from the middle of a new chain ->
-			   delete the old entry and skip the new one */
-			at76_dbg(DBG_RX_FRAGS,
-				 "%s: middle of new seq %d (%d) "
-				 "removing old seq %d",
-				 priv->netdev->name, seqnr, fragnr,
-				 bptr->seqnr);
-			dev_kfree_skb(bptr->skb);
-			bptr->skb = NULL;
-		}
-		return NULL;
-	}
+	mutex_lock(&priv->mtx);
 
-	/* if we didn't find a chain for the sender address, optr
-	   points either to the first free or the oldest entry */
+	ieee80211_stop_queues(hw);
 
-	if (fragnr != 0) {
-		/* this is not the begin of a fragment chain ... */
-		at76_dbg(DBG_RX_FRAGS,
-			 "%s: no chain for non-first fragment (%d)",
-			 priv->netdev->name, fragnr);
-		return NULL;
+	memset(&scan, 0, sizeof(struct at76_req_scan));
+	memset(scan.bssid, 0xFF, ETH_ALEN);
+	scan.scan_type = SCAN_TYPE_ACTIVE;
+	if (priv->essid_size > 0) {
+		memcpy(scan.essid, ssid, len);
+		scan.essid_size = len;
 	}
+	scan.min_channel_time = cpu_to_le16(priv->scan_min_time);
+	scan.max_channel_time = cpu_to_le16(priv->scan_max_time);
+	scan.probe_delay = cpu_to_le16(priv->scan_min_time * 1000);
+	scan.international_scan = 0;
 
-	BUG_ON(!optr);
-	if (optr->skb) {
-		/* swap the skb's */
-		skb = optr->skb;
-		optr->skb = priv->rx_skb;
-		priv->rx_skb = skb;
+	at76_dbg(DBG_MAC80211, "%s: sending CMD_SCAN", __func__);
+	ret = at76_set_card_command(priv->udev, CMD_SCAN, &scan, sizeof(scan));
 
-		at76_dbg(DBG_RX_FRAGS,
-			 "%s: free old contents: sender %s seq/frag %d/%d",
-			 priv->netdev->name, mac2str(optr->sender),
-			 optr->seqnr, optr->fragnr);
+	if (ret < 0) {
+		err("CMD_SCAN failed: %d", ret);
+		goto exit;
+	}
 
-	} else {
-		/* take the skb from priv->rx_skb */
-		optr->skb = priv->rx_skb;
-		/* let at76_submit_rx_urb() allocate a new skb */
-		priv->rx_skb = NULL;
+	queue_delayed_work(priv->hw->workqueue, &priv->dwork_hw_scan,
+			   SCAN_POLL_INTERVAL);
 
-		at76_dbg(DBG_RX_FRAGS, "%s: use a free entry",
-			 priv->netdev->name);
-	}
-	memcpy(optr->sender, i802_11_hdr->addr2, ETH_ALEN);
-	optr->seqnr = seqnr;
-	optr->fragnr = 0;
-	optr->last_rx = jiffies;
+exit:
+	mutex_unlock(&priv->mtx);
 
-	return NULL;
+	return 0;
 }
 
-/* Rx interrupt: we expect the complete data buffer in priv->rx_skb */
-static void at76_rx_data(struct at76_priv *priv)
+static int at76_config(struct ieee80211_hw *hw, u32 changed)
 {
-	struct net_device *netdev = priv->netdev;
-	struct net_device_stats *stats = &priv->stats;
-	struct sk_buff *skb = priv->rx_skb;
-	struct at76_rx_buffer *buf = (struct at76_rx_buffer *)skb->data;
-	struct ieee80211_hdr_3addr *i802_11_hdr;
-	int length = le16_to_cpu(buf->wlength);
-
-	at76_dbg(DBG_RX_DATA, "%s received data packet: %s", netdev->name,
-		 hex2str(skb->data, AT76_RX_HDRLEN));
+	struct at76_priv *priv = hw->priv;
+	struct ieee80211_conf *conf = &hw->conf;
 
-	at76_dbg(DBG_RX_DATA_CONTENT, "rx packet: %s",
-		 hex2str(skb->data + AT76_RX_HDRLEN, length));
+	at76_dbg(DBG_MAC80211, "%s(): channel %d radio %d",
+		 __func__, conf->channel->hw_value, conf->radio_enabled);
+	at76_dbg_dump(DBG_MAC80211, priv->essid, priv->essid_size, "ssid:");
+	at76_dbg_dump(DBG_MAC80211, priv->bssid, ETH_ALEN, "bssid:");
 
-	skb = at76_check_for_rx_frags(priv);
-	if (!skb)
-		return;
-
-	/* Atmel header and the FCS are already removed */
-	i802_11_hdr = (struct ieee80211_hdr_3addr *)skb->data;
-
-	skb->dev = netdev;
-	skb->ip_summed = CHECKSUM_NONE;	/* TODO: should check CRC */
+	mutex_lock(&priv->mtx);
 
-	if (is_broadcast_ether_addr(i802_11_hdr->addr1)) {
-		if (!compare_ether_addr(i802_11_hdr->addr1, netdev->broadcast))
-			skb->pkt_type = PACKET_BROADCAST;
-		else
-			skb->pkt_type = PACKET_MULTICAST;
-	} else if (compare_ether_addr(i802_11_hdr->addr1, netdev->dev_addr))
-		skb->pkt_type = PACKET_OTHERHOST;
+	priv->channel = conf->channel->hw_value;
 
-	at76_ieee80211_to_eth(skb, priv->iw_mode);
+	if (is_valid_ether_addr(priv->bssid)) {
+		at76_join(priv);
+		ieee80211_wake_queues(priv->hw);
+	} else {
+		ieee80211_stop_queues(priv->hw);
+		at76_start_monitor(priv);
+	};
 
-	netdev->last_rx = jiffies;
-	netif_rx(skb);
-	stats->rx_packets++;
-	stats->rx_bytes += length;
+	mutex_unlock(&priv->mtx);
 
-	return;
+	return 0;
 }
 
-static void at76_rx_monitor_mode(struct at76_priv *priv)
+static int at76_config_interface(struct ieee80211_hw *hw,
+				 struct ieee80211_vif *vif,
+				 struct ieee80211_if_conf *conf)
 {
-	struct at76_rx_radiotap *rt;
-	u8 *payload;
-	int skblen;
-	struct net_device *netdev = priv->netdev;
-	struct at76_rx_buffer *buf =
-	    (struct at76_rx_buffer *)priv->rx_skb->data;
-	/* length including the IEEE802.11 header and the trailing FCS,
-	   but not at76_rx_buffer */
-	int length = le16_to_cpu(buf->wlength);
-	struct sk_buff *skb = priv->rx_skb;
-	struct net_device_stats *stats = &priv->stats;
+	struct at76_priv *priv = hw->priv;
 
-	if (length < IEEE80211_FCS_LEN) {
-		/* buffer contains no data */
-		at76_dbg(DBG_MONITOR_MODE,
-			 "%s: MONITOR MODE: rx skb without data",
-			 priv->netdev->name);
-		return;
-	}
+	at76_dbg_dump(DBG_MAC80211, conf->bssid, ETH_ALEN, "bssid:");
 
-	skblen = sizeof(struct at76_rx_radiotap) + length;
+	mutex_lock(&priv->mtx);
 
-	skb = dev_alloc_skb(skblen);
-	if (!skb) {
-		printk(KERN_ERR "%s: MONITOR MODE: dev_alloc_skb for radiotap "
-		       "header returned NULL\n", priv->netdev->name);
-		return;
-	}
+	memcpy(priv->bssid, conf->bssid, ETH_ALEN);
+//	memcpy(priv->essid, conf->ssid, conf->ssid_len);
+//	priv->essid_size = conf->ssid_len;
 
-	skb_put(skb, skblen);
-
-	rt = (struct at76_rx_radiotap *)skb->data;
-	payload = skb->data + sizeof(struct at76_rx_radiotap);
-
-	rt->rt_hdr.it_version = 0;
-	rt->rt_hdr.it_pad = 0;
-	rt->rt_hdr.it_len = cpu_to_le16(sizeof(struct at76_rx_radiotap));
-	rt->rt_hdr.it_present = cpu_to_le32(AT76_RX_RADIOTAP_PRESENT);
-
-	rt->rt_tsft = cpu_to_le64(le32_to_cpu(buf->rx_time));
-	rt->rt_rate = hw_rates[buf->rx_rate] & (~0x80);
-	rt->rt_signal = buf->rssi;
-	rt->rt_noise = buf->noise_level;
-	rt->rt_flags = IEEE80211_RADIOTAP_F_FCS;
-	if (buf->fragmentation)
-		rt->rt_flags |= IEEE80211_RADIOTAP_F_FRAG;
-
-	memcpy(payload, buf->packet, length);
-	skb->dev = netdev;
-	skb->ip_summed = CHECKSUM_NONE;
-	skb_reset_mac_header(skb);
-	skb->pkt_type = PACKET_OTHERHOST;
-	skb->protocol = htons(ETH_P_802_2);
-
-	netdev->last_rx = jiffies;
-	netif_rx(skb);
-	stats->rx_packets++;
-	stats->rx_bytes += length;
+	if (is_valid_ether_addr(priv->bssid)) {
+		/* mac80211 is joining a bss */
+		ieee80211_wake_queues(priv->hw);
+		at76_join(priv);
+	} else
+		ieee80211_stop_queues(priv->hw);
+
+	mutex_unlock(&priv->mtx);
+
+	return 0;
 }
 
-/* Check if we spy on the sender address in buf and update stats */
-static void at76_iwspy_update(struct at76_priv *priv,
-			      struct at76_rx_buffer *buf)
+/* must be atomic */
+static void at76_configure_filter(struct ieee80211_hw *hw,
+				  unsigned int changed_flags,
+				  unsigned int *total_flags, int mc_count,
+				  struct dev_addr_list *mc_list)
 {
-	struct ieee80211_hdr_3addr *hdr =
-	    (struct ieee80211_hdr_3addr *)buf->packet;
-	struct iw_quality qual;
+	struct at76_priv *priv = hw->priv;
+	int flags;
 
-	/* We can only set the level here */
-	qual.updated = IW_QUAL_QUAL_INVALID | IW_QUAL_NOISE_INVALID;
-	qual.level = 0;
-	qual.noise = 0;
-	at76_calc_level(priv, buf, &qual);
+	at76_dbg(DBG_MAC80211, "%s(): changed_flags=0x%08x "
+		 "total_flags=0x%08x mc_count=%d",
+		 __func__, changed_flags, *total_flags, mc_count);
 
-	spin_lock_bh(&priv->spy_spinlock);
+	flags = changed_flags & AT76_SUPPORTED_FILTERS;
+	*total_flags = AT76_SUPPORTED_FILTERS;
 
-	if (priv->spy_data.spy_number > 0)
-		wireless_spy_update(priv->netdev, hdr->addr2, &qual);
+	/* FIXME: access to priv->promisc should be protected with
+	 * priv->mtx, but it's impossible because this function needs to be
+	 * atomic */
 
-	spin_unlock_bh(&priv->spy_spinlock);
+	if (flags && !priv->promisc) {
+		/* mac80211 wants us to enable promiscuous mode */
+		priv->promisc = 1;
+	} else if (!flags && priv->promisc) {
+		/* we need to disable promiscuous mode */
+		priv->promisc = 0;
+	} else
+		return;
+
+	queue_work(hw->workqueue, &priv->work_set_promisc);
 }
 
-static void at76_rx_tasklet(unsigned long param)
+static int at76_set_key_oldfw(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+			const u8 *local_address, const u8 *address,
+			struct ieee80211_key_conf *key)
 {
-	struct urb *urb = (struct urb *)param;
-	struct at76_priv *priv = urb->context;
-	struct net_device *netdev = priv->netdev;
-	struct at76_rx_buffer *buf;
-	struct ieee80211_hdr_3addr *i802_11_hdr;
-	u16 frame_ctl;
+	struct at76_priv *priv = hw->priv;
 
-	if (priv->device_unplugged) {
-		at76_dbg(DBG_DEVSTART, "device unplugged");
-		if (urb)
-			at76_dbg(DBG_DEVSTART, "urb status %d", urb->status);
-		return;
-	}
-
-	if (!priv->rx_skb || !netdev || !priv->rx_skb->data)
-		return;
+	int i;
 
-	buf = (struct at76_rx_buffer *)priv->rx_skb->data;
+	at76_dbg(DBG_MAC80211, "%s(): cmd %d key->alg %d key->keyidx %d "
+		 "key->keylen %d",
+		 __func__, cmd, key->alg, key->keyidx, key->keylen);
 
-	i802_11_hdr = (struct ieee80211_hdr_3addr *)buf->packet;
+	if (key->alg != ALG_WEP)
+		return -EOPNOTSUPP;
 
-	frame_ctl = le16_to_cpu(i802_11_hdr->frame_ctl);
+	key->hw_key_idx = key->keyidx;
 
-	if (urb->status != 0) {
-		if (urb->status != -ENOENT && urb->status != -ECONNRESET)
-			at76_dbg(DBG_URB,
-				 "%s %s: - nonzero Rx bulk status received: %d",
-				 __func__, netdev->name, urb->status);
-		return;
-	}
+	mutex_lock(&priv->mtx);
 
-	at76_dbg(DBG_RX_ATMEL_HDR,
-		 "%s: rx frame: rate %d rssi %d noise %d link %d %s",
-		 priv->netdev->name, buf->rx_rate, buf->rssi, buf->noise_level,
-		 buf->link_quality, hex2str(i802_11_hdr, 48));
-	if (priv->iw_mode == IW_MODE_MONITOR) {
-		at76_rx_monitor_mode(priv);
-		goto exit;
-	}
+	switch (cmd) {
+	case SET_KEY:
+		memcpy(priv->wep_keys[key->keyidx], key->key, key->keylen);
+		priv->wep_keys_len[key->keyidx] = key->keylen;
 
-	/* there is a new bssid around, accept it: */
-	if (buf->newbss && priv->iw_mode == IW_MODE_ADHOC) {
-		at76_dbg(DBG_PROGRESS, "%s: rx newbss", netdev->name);
-		schedule_work(&priv->work_new_bss);
-	}
+		/* FIXME: find out how to do this properly */
+		priv->wep_key_id = key->keyidx;
 
-	switch (frame_ctl & IEEE80211_FCTL_FTYPE) {
-	case IEEE80211_FTYPE_DATA:
-		at76_rx_data(priv);
 		break;
+	case DISABLE_KEY:
+	default:
+		priv->wep_keys_len[key->keyidx] = 0;
+		break;
+	}
 
-	case IEEE80211_FTYPE_MGMT:
-		/* jal: TODO: find out if we can update iwspy also on
-		   other frames than management (might depend on the
-		   radio chip / firmware version !) */
+	priv->wep_enabled = 0;
 
-		at76_iwspy_update(priv, buf);
+	for (i = 0; i < WEP_KEYS; i++) {
+		if (priv->wep_keys_len[i] != 0)
+			priv->wep_enabled = 1;
+	}
 
-		at76_rx_mgmt(priv, buf);
-		break;
+	at76_startup_device(priv);
 
-	case IEEE80211_FTYPE_CTL:
-		at76_dbg(DBG_RX_CTRL, "%s: ignored ctrl frame: %04x",
-			 priv->netdev->name, frame_ctl);
-		break;
+	mutex_unlock(&priv->mtx);
 
-	default:
-		printk(KERN_DEBUG "%s: ignoring frame with framectl 0x%04x\n",
-		       priv->netdev->name, frame_ctl);
-	}
-exit:
-	at76_submit_rx_urb(priv);
+	return 0;
 }
 
-/* Load firmware into kernel memory and parse it */
-static struct fwentry *at76_load_firmware(struct usb_device *udev,
-					  enum board_type board_type)
+static int at76_set_key_newfw(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+			const u8 *local_address, const u8 *address,
+			struct ieee80211_key_conf *key)
 {
-	int ret;
-	char *str;
-	struct at76_fw_header *fwh;
-	struct fwentry *fwe = &firmwares[board_type];
+	struct at76_priv *priv = hw->priv;
+	int ret = -EOPNOTSUPP;
 
-	mutex_lock(&fw_mutex);
+	at76_dbg(DBG_MAC80211, "%s(): cmd %d key->alg %d key->keyidx %d "
+		 "key->keylen %d",
+		 __func__, cmd, key->alg, key->keyidx, key->keylen);
 
-	if (fwe->loaded) {
-		at76_dbg(DBG_FW, "re-using previously loaded fw");
-		goto exit;
-	}
+	mutex_lock(&priv->mtx);
 
-	at76_dbg(DBG_FW, "downloading firmware %s", fwe->fwname);
-	ret = request_firmware(&fwe->fw, fwe->fwname, &udev->dev);
-	if (ret < 0) {
-		dev_printk(KERN_ERR, &udev->dev, "firmware %s not found!\n",
-			   fwe->fwname);
-		dev_printk(KERN_ERR, &udev->dev,
-			   "you may need to download the firmware from "
-			   "http://developer.berlios.de/projects/at76c503a/");
-		goto exit;
-	}
+	priv->mib_buf.type = MIB_MAC_ENCRYPTION;
 
-	at76_dbg(DBG_FW, "got it.");
-	fwh = (struct at76_fw_header *)(fwe->fw->data);
+	if (cmd == DISABLE_KEY) {
+		priv->mib_buf.size = CIPHER_KEY_LEN;
+		priv->mib_buf.index = offsetof(struct mib_mac_encryption,
+				cipher_default_keyvalue[key->keyidx]);
+		memset(priv->mib_buf.data.data, 0, CIPHER_KEY_LEN);
+		if (at76_set_mib(priv, &priv->mib_buf) != CMD_STATUS_COMPLETE)
+			ret = -EOPNOTSUPP; /* -EIO would be probably better */
+		else {
 
-	if (fwe->fw->size <= sizeof(*fwh)) {
-		dev_printk(KERN_ERR, &udev->dev,
-			   "firmware is too short (0x%zx)\n", fwe->fw->size);
-		goto exit;
-	}
+			priv->keys[key->keyidx].cipher = CIPHER_NONE;
+			priv->keys[key->keyidx].keylen = 0;
+		};
+		if (priv->default_group_key == key->keyidx)
+			priv->default_group_key = 0xff;
 
-	/* CRC currently not checked */
-	fwe->board_type = le32_to_cpu(fwh->board_type);
-	if (fwe->board_type != board_type) {
-		dev_printk(KERN_ERR, &udev->dev,
-			   "board type mismatch, requested %u, got %u\n",
-			   board_type, fwe->board_type);
+		if (priv->default_pairwise_key == key->keyidx)
+			priv->default_pairwise_key = 0xff;
+		/* If default pairwise key is removed, fall back to
+		 * group key? */
+		ret = 0;
 		goto exit;
-	}
+	};
 
-	fwe->fw_version.major = fwh->major;
-	fwe->fw_version.minor = fwh->minor;
-	fwe->fw_version.patch = fwh->patch;
-	fwe->fw_version.build = fwh->build;
+	if (cmd == SET_KEY) {
+		/* store key into MIB */
+		priv->mib_buf.size = CIPHER_KEY_LEN;
+		priv->mib_buf.index = offsetof(struct mib_mac_encryption,
+				cipher_default_keyvalue[key->keyidx]);
+		memset(priv->mib_buf.data.data, 0, CIPHER_KEY_LEN);
+		memcpy(priv->mib_buf.data.data, key->key, key->keylen);
+
+		switch (key->alg) {
+		case ALG_WEP:
+			if (key->keylen == 5) {
+				priv->keys[key->keyidx].cipher =
+					CIPHER_WEP64;
+				priv->keys[key->keyidx].keylen = 8;
+			} else if (key->keylen == 13) {
+				priv->keys[key->keyidx].cipher =
+					CIPHER_WEP128;
+				/* Firmware needs this */
+				priv->keys[key->keyidx].keylen = 8;
+			} else {
+				ret = -EOPNOTSUPP;
+				goto exit;
+			};
+			break;
+		case ALG_TKIP:
+			key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
+			priv->keys[key->keyidx].cipher = CIPHER_TKIP;
+			priv->keys[key->keyidx].keylen = 12;
+			break;
 
-	str = (char *)fwh + le32_to_cpu(fwh->str_offset);
-	fwe->intfw = (u8 *)fwh + le32_to_cpu(fwh->int_fw_offset);
-	fwe->intfw_size = le32_to_cpu(fwh->int_fw_len);
-	fwe->extfw = (u8 *)fwh + le32_to_cpu(fwh->ext_fw_offset);
-	fwe->extfw_size = le32_to_cpu(fwh->ext_fw_len);
+		case ALG_CCMP:
+			if (!at76_is_505a(priv->board_type)) {
+				ret = -EOPNOTSUPP;
+				goto exit;
+			};
+			key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
+			priv->keys[key->keyidx].cipher = CIPHER_CCMP;
+			priv->keys[key->keyidx].keylen = 16;
+			break;
 
-	fwe->loaded = 1;
+		default:
+			ret = -EOPNOTSUPP;
+			goto exit;
+		};
+
+		priv->mib_buf.data.data[38] = priv->keys[key->keyidx].cipher;
+		priv->mib_buf.data.data[39] = 1; /* Taken from atmelwlandriver,
+						    not documented */
+
+		if (is_valid_ether_addr(address))
+			/* Pairwise key */
+			priv->mib_buf.data.data[39] |= (KEY_PAIRWISE | KEY_TX);
+		else if (is_broadcast_ether_addr(address))
+			/* Group key */
+			priv->mib_buf.data.data[39] |= (KEY_TX);
+		else	/* Key used only for transmission ??? */
+			priv->mib_buf.data.data[39] |= (KEY_TX);
+
+		if (at76_set_mib(priv, &priv->mib_buf) !=
+				CMD_STATUS_COMPLETE) {
+			ret = -EOPNOTSUPP; /* -EIO would be probably better */
+			goto exit;
+		};
 
-	dev_printk(KERN_DEBUG, &udev->dev,
-		   "using firmware %s (version %d.%d.%d-%d)\n",
-		   fwe->fwname, fwh->major, fwh->minor, fwh->patch, fwh->build);
+		if ((key->alg == ALG_TKIP) || (key->alg == ALG_CCMP))
+			at76_reset_rsc(priv);
 
-	at76_dbg(DBG_DEVSTART, "board %u, int %d:%d, ext %d:%d", board_type,
-		 le32_to_cpu(fwh->int_fw_offset), le32_to_cpu(fwh->int_fw_len),
-		 le32_to_cpu(fwh->ext_fw_offset), le32_to_cpu(fwh->ext_fw_len));
-	at76_dbg(DBG_DEVSTART, "firmware id %s", str);
+		key->hw_key_idx = key->keyidx;
+
+		/* Set up default keys */
+		if (is_broadcast_ether_addr(address))
+			priv->default_group_key = key->keyidx;
+		if (is_valid_ether_addr(address))
+			priv->default_pairwise_key = key->keyidx;
 
+		/* Set up encryption MIBs */
+
+		/* first block of settings */
+		priv->mib_buf.size = 3;
+		priv->mib_buf.index = offsetof(struct mib_mac_encryption,
+				privacy_invoked);
+		priv->mib_buf.data.data[0] = 1;	/* privacy_invoked */
+		priv->mib_buf.data.data[1] = priv->default_pairwise_key;
+		priv->mib_buf.data.data[2] = priv->default_group_key;
+
+		ret = at76_set_mib(priv, &priv->mib_buf);
+		if (ret != CMD_STATUS_COMPLETE)
+			goto exit;
+
+		/* second block of settings */
+		priv->mib_buf.size = 3;
+		priv->mib_buf.index = offsetof(struct mib_mac_encryption,
+				exclude_unencrypted);
+		priv->mib_buf.data.data[0] = 1;	/* exclude_unencrypted */
+		priv->mib_buf.data.data[1] = 0;	/* wep_encryption_type */
+		priv->mib_buf.data.data[2] = 0;	/* ckip_key_permutation */
+
+		ret = at76_set_mib(priv, &priv->mib_buf);
+		if (ret != CMD_STATUS_COMPLETE)
+			goto exit;
+		ret = 0;
+	};
 exit:
-	mutex_unlock(&fw_mutex);
+	at76_dump_mib_mac_encryption(priv);
+	mutex_unlock(&priv->mtx);
+	return ret;
+}
 
-	if (fwe->loaded)
-		return fwe;
+static int at76_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+			const u8 *local_address, const u8 *address,
+			struct ieee80211_key_conf *key)
+{
+	struct at76_priv *priv = hw->priv;
+
+	at76_dbg(DBG_MAC80211, "%s(): cmd %d key->alg %d key->keyidx %d "
+		 "key->keylen %d",
+		 __func__, cmd, key->alg, key->keyidx, key->keylen);
+
+	if (FIRMWARE_IS_WPA(priv->fw_version))
+		return at76_set_key_newfw(hw, cmd, local_address, address, key);
 	else
-		return NULL;
+		return at76_set_key_oldfw(hw, cmd, local_address, address, key);
+
 }
 
+static const struct ieee80211_ops at76_ops = {
+	.tx = at76_mac80211_tx,
+	.add_interface = at76_add_interface,
+	.remove_interface = at76_remove_interface,
+	.config = at76_config,
+	.config_interface = at76_config_interface,
+	.configure_filter = at76_configure_filter,
+	.start = at76_mac80211_start,
+	.stop = at76_mac80211_stop,
+	.hw_scan = at76_hw_scan,
+	.set_key = at76_set_key,
+};
+
 /* Allocate network device and initialize private data */
 static struct at76_priv *at76_alloc_new_device(struct usb_device *udev)
 {
-	struct net_device *netdev;
+	struct ieee80211_hw *hw;
 	struct at76_priv *priv;
-	int i;
 
-	/* allocate memory for our device state and initialize it */
-	netdev = alloc_etherdev(sizeof(struct at76_priv));
-	if (!netdev) {
-		dev_printk(KERN_ERR, &udev->dev, "out of memory\n");
+	hw = ieee80211_alloc_hw(sizeof(struct at76_priv), &at76_ops);
+	if (!hw) {
+		printk(KERN_ERR DRIVER_NAME ": could not register"
+		       " ieee80211_hw\n");
 		return NULL;
 	}
 
-	priv = netdev_priv(netdev);
+	priv = hw->priv;
+	priv->hw = hw;
 
 	priv->udev = udev;
-	priv->netdev = netdev;
 
 	mutex_init(&priv->mtx);
-	INIT_WORK(&priv->work_assoc_done, at76_work_assoc_done);
-	INIT_WORK(&priv->work_join, at76_work_join);
-	INIT_WORK(&priv->work_new_bss, at76_work_new_bss);
-	INIT_WORK(&priv->work_start_scan, at76_work_start_scan);
 	INIT_WORK(&priv->work_set_promisc, at76_work_set_promisc);
 	INIT_WORK(&priv->work_submit_rx, at76_work_submit_rx);
-	INIT_DELAYED_WORK(&priv->dwork_restart, at76_dwork_restart);
-	INIT_DELAYED_WORK(&priv->dwork_get_scan, at76_dwork_get_scan);
-	INIT_DELAYED_WORK(&priv->dwork_beacon, at76_dwork_beacon);
-	INIT_DELAYED_WORK(&priv->dwork_auth, at76_dwork_auth);
-	INIT_DELAYED_WORK(&priv->dwork_assoc, at76_dwork_assoc);
-
-	spin_lock_init(&priv->mgmt_spinlock);
-	priv->next_mgmt_bulk = NULL;
-	priv->mac_state = MAC_INIT;
-
-	/* initialize empty BSS list */
-	priv->curr_bss = NULL;
-	INIT_LIST_HEAD(&priv->bss_list);
-	spin_lock_init(&priv->bss_list_spinlock);
-
-	init_timer(&priv->bss_list_timer);
-	priv->bss_list_timer.data = (unsigned long)priv;
-	priv->bss_list_timer.function = at76_bss_list_timeout;
-
-	spin_lock_init(&priv->spy_spinlock);
-
-	/* mark all rx data entries as unused */
-	for (i = 0; i < NR_RX_DATA_BUF; i++)
-		priv->rx_data[i].skb = NULL;
+	INIT_DELAYED_WORK(&priv->dwork_hw_scan, at76_dwork_hw_scan);
 
 	priv->rx_tasklet.func = at76_rx_tasklet;
 	priv->rx_tasklet.data = 0;
@@ -5183,6 +2371,9 @@ static struct at76_priv *at76_alloc_new_device(struct usb_device *udev)
 	priv->pm_mode = AT76_PM_OFF;
 	priv->pm_period = 0;
 
+	/* unit us */
+	priv->hw->channel_change_time = 100000;
+
 	return priv;
 }
 
@@ -5245,11 +2436,42 @@ static int at76_alloc_urbs(struct at76_priv *priv,
 	return 0;
 }
 
+static struct ieee80211_rate at76_rates[] = {
+	{ .bitrate = 10, .hw_value = TX_RATE_1MBIT, },
+	{ .bitrate = 20, .hw_value = TX_RATE_2MBIT, },
+	{ .bitrate = 55, .hw_value = TX_RATE_5_5MBIT, },
+	{ .bitrate = 110, .hw_value = TX_RATE_11MBIT, },
+};
+
+static struct ieee80211_channel at76_channels[] = {
+	{ .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_supported_band at76_supported_band = {
+	.channels = at76_channels,
+	.n_channels = ARRAY_SIZE(at76_channels),
+	.bitrates = at76_rates,
+	.n_bitrates = ARRAY_SIZE(at76_rates),
+};
+
 /* Register network device and initialize the hardware */
 static int at76_init_new_device(struct at76_priv *priv,
 				struct usb_interface *interface)
 {
-	struct net_device *netdev = priv->netdev;
+	struct device *dev = &interface->dev;
 	int ret;
 
 	/* set up the endpoint information */
@@ -5265,14 +2487,11 @@ static int at76_init_new_device(struct at76_priv *priv,
 	/* MAC address */
 	ret = at76_get_hw_config(priv);
 	if (ret < 0) {
-		dev_printk(KERN_ERR, &interface->dev,
-			   "cannot get MAC address\n");
+		dev_err(dev, "cannot get MAC address\n");
 		goto exit;
 	}
 
 	priv->domain = at76_get_reg_domain(priv->regulatory_domain);
-	/* init. netdev->dev_addr */
-	memcpy(netdev->dev_addr, priv->mac_addr, ETH_ALEN);
 
 	priv->channel = DEF_CHANNEL;
 	priv->iw_mode = IW_MODE_INFRA;
@@ -5282,47 +2501,54 @@ static int at76_init_new_device(struct at76_priv *priv,
 	priv->txrate = TX_RATE_AUTO;
 	priv->preamble_type = PREAMBLE_TYPE_LONG;
 	priv->beacon_period = 100;
-	priv->beacons_last_qual = jiffies;
 	priv->auth_mode = WLAN_AUTH_OPEN;
 	priv->scan_min_time = DEF_SCAN_MIN_TIME;
 	priv->scan_max_time = DEF_SCAN_MAX_TIME;
 	priv->scan_mode = SCAN_TYPE_ACTIVE;
+	priv->default_pairwise_key = 0xff;
+	priv->default_group_key = 0xff;
+
+	/* mac80211 initialisation */
+	priv->hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &at76_supported_band;
 
-	netdev->flags &= ~IFF_MULTICAST;	/* not yet or never */
-	netdev->open = at76_open;
-	netdev->stop = at76_stop;
-	netdev->get_stats = at76_get_stats;
-	netdev->ethtool_ops = &at76_ethtool_ops;
-
-	/* Add pointers to enable iwspy support. */
-	priv->wireless_data.spy_data = &priv->spy_data;
-	netdev->wireless_data = &priv->wireless_data;
-
-	netdev->hard_start_xmit = at76_tx;
-	netdev->tx_timeout = at76_tx_timeout;
-	netdev->watchdog_timeo = 2 * HZ;
-	netdev->wireless_handlers = &at76_handler_def;
-	netdev->set_multicast_list = at76_set_multicast;
-	netdev->set_mac_address = at76_set_mac_address;
-	dev_alloc_name(netdev, "wlan%d");
-
-	ret = register_netdev(priv->netdev);
+	if (FIRMWARE_IS_WPA(priv->fw_version) &&
+		(at76_is_503rfmd(priv->board_type) ||
+		 at76_is_505(priv->board_type)))
+		priv->hw->flags = IEEE80211_HW_SIGNAL_UNSPEC;
+	else
+		priv->hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
+				  IEEE80211_HW_SIGNAL_UNSPEC;
+
+	priv->hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION);
+
+	SET_IEEE80211_DEV(priv->hw, &interface->dev);
+	SET_IEEE80211_PERM_ADDR(priv->hw, priv->mac_addr);
+
+	ret = ieee80211_register_hw(priv->hw);
 	if (ret) {
-		dev_printk(KERN_ERR, &interface->dev,
-			   "cannot register netdevice (status %d)!\n", ret);
+		dev_err(dev, "cannot register mac80211 hw (status %d)!\n", ret);
 		goto exit;
 	}
-	priv->netdev_registered = 1;
 
-	printk(KERN_INFO "%s: USB %s, MAC %s, firmware %d.%d.%d-%d\n",
-	       netdev->name, interface->dev.bus_id, mac2str(priv->mac_addr),
-	       priv->fw_version.major, priv->fw_version.minor,
-	       priv->fw_version.patch, priv->fw_version.build);
-	printk(KERN_INFO "%s: regulatory domain 0x%02x: %s\n", netdev->name,
-	       priv->regulatory_domain, priv->domain->name);
+	priv->mac80211_registered = 1;
 
-	/* we let this timer run the whole time this driver instance lives */
-	mod_timer(&priv->bss_list_timer, jiffies + BSS_LIST_TIMEOUT);
+	dev_info(dev, "%s: USB %s, MAC %s, firmware %d.%d.%d-%d\n",
+		 wiphy_name(priv->hw->wiphy),
+		 dev_name(&interface->dev), mac2str(priv->mac_addr),
+		 priv->fw_version.major, priv->fw_version.minor,
+		 priv->fw_version.patch, priv->fw_version.build);
+	dev_info(dev, "%s: regulatory domain 0x%02x: %s\n",
+		 wiphy_name(priv->hw->wiphy),
+		 priv->regulatory_domain, priv->domain->name);
+	dev_info(dev, "%s: WPA support: ", wiphy_name(priv->hw->wiphy));
+	if (!FIRMWARE_IS_WPA(priv->fw_version))
+		printk("none\n");
+	else {
+		if (!at76_is_505a(priv->board_type))
+			printk("TKIP\n");
+		else
+			printk("TKIP, AES/CCMP\n");
+	};
 
 exit:
 	return ret;
@@ -5330,15 +2556,13 @@ exit:
 
 static void at76_delete_device(struct at76_priv *priv)
 {
-	int i;
-
 	at76_dbg(DBG_PROC_ENTRY, "%s: ENTER", __func__);
 
 	/* The device is gone, don't bother turning it off */
 	priv->device_unplugged = 1;
 
-	if (priv->netdev_registered)
-		unregister_netdev(priv->netdev);
+	if (priv->mac80211_registered)
+		ieee80211_unregister_hw(priv->hw);
 
 	/* assuming we used keventd, it must quiesce too */
 	flush_scheduled_work();
@@ -5359,25 +2583,11 @@ static void at76_delete_device(struct at76_priv *priv)
 	if (priv->rx_skb)
 		kfree_skb(priv->rx_skb);
 
-	at76_free_bss_list(priv);
-	del_timer_sync(&priv->bss_list_timer);
-	cancel_delayed_work(&priv->dwork_get_scan);
-	cancel_delayed_work(&priv->dwork_beacon);
-	cancel_delayed_work(&priv->dwork_auth);
-	cancel_delayed_work(&priv->dwork_assoc);
-
-	if (priv->mac_state == MAC_CONNECTED)
-		at76_iwevent_bss_disconnect(priv->netdev);
-
-	for (i = 0; i < NR_RX_DATA_BUF; i++)
-		if (priv->rx_data[i].skb) {
-			dev_kfree_skb(priv->rx_data[i].skb);
-			priv->rx_data[i].skb = NULL;
-		}
 	usb_put_dev(priv->udev);
 
-	at76_dbg(DBG_PROC_ENTRY, "%s: before freeing priv/netdev", __func__);
-	free_netdev(priv->netdev);	/* priv is in netdev */
+	at76_dbg(DBG_PROC_ENTRY, "%s: before freeing priv/ieee80211_hw",
+		 __func__);
+	ieee80211_free_hw(priv->hw);
 
 	at76_dbg(DBG_PROC_ENTRY, "%s: EXIT", __func__);
 }
@@ -5411,8 +2621,8 @@ static int at76_probe(struct usb_interface *interface,
 	   we get 204 with 2.4.23, Fiberline FL-WL240u (505A+RFMD2958) ??? */
 
 	if (op_mode == OPMODE_HW_CONFIG_MODE) {
-		dev_printk(KERN_ERR, &interface->dev,
-			   "cannot handle a device in HW_CONFIG_MODE\n");
+		dev_err(&interface->dev,
+			"cannot handle a device in HW_CONFIG_MODE\n");
 		ret = -EBUSY;
 		goto error;
 	}
@@ -5420,13 +2630,12 @@ static int at76_probe(struct usb_interface *interface,
 	if (op_mode != OPMODE_NORMAL_NIC_WITH_FLASH
 	    && op_mode != OPMODE_NORMAL_NIC_WITHOUT_FLASH) {
 		/* download internal firmware part */
-		dev_printk(KERN_DEBUG, &interface->dev,
-			   "downloading internal firmware\n");
+		dev_dbg(&interface->dev, "downloading internal firmware\n");
 		ret = at76_load_internal_fw(udev, fwe);
 		if (ret < 0) {
-			dev_printk(KERN_ERR, &interface->dev,
-				   "error %d downloading internal firmware\n",
-				   ret);
+			dev_err(&interface->dev,
+				"error %d downloading internal firmware\n",
+				ret);
 			goto error;
 		}
 		usb_put_dev(udev);
@@ -5451,8 +2660,7 @@ static int at76_probe(struct usb_interface *interface,
 		need_ext_fw = 1;
 
 	if (need_ext_fw) {
-		dev_printk(KERN_DEBUG, &interface->dev,
-			   "downloading external firmware\n");
+		dev_dbg(&interface->dev, "downloading external firmware\n");
 
 		ret = at76_load_external_fw(udev, fwe);
 		if (ret)
@@ -5461,8 +2669,8 @@ static int at76_probe(struct usb_interface *interface,
 		/* Re-check firmware version */
 		ret = at76_get_mib(udev, MIB_FW_VERSION, &fwv, sizeof(fwv));
 		if (ret < 0) {
-			dev_printk(KERN_ERR, &interface->dev,
-				   "error %d getting firmware version\n", ret);
+			dev_err(&interface->dev,
+				"error %d getting firmware version\n", ret);
 			goto error;
 		}
 	}
@@ -5473,7 +2681,6 @@ static int at76_probe(struct usb_interface *interface,
 		goto error;
 	}
 
-	SET_NETDEV_DEV(priv->netdev, &interface->dev);
 	usb_set_intfdata(interface, priv);
 
 	memcpy(&priv->fw_version, &fwv, sizeof(struct mib_fw_version));
@@ -5501,7 +2708,7 @@ static void at76_disconnect(struct usb_interface *interface)
 	if (!priv)
 		return;
 
-	printk(KERN_INFO "%s: disconnecting\n", priv->netdev->name);
+	printk(KERN_INFO "%s: disconnecting\n", wiphy_name(priv->hw->wiphy));
 	at76_delete_device(priv);
 	dev_printk(KERN_INFO, &interface->dev, "disconnected\n");
 }
@@ -5557,5 +2764,8 @@ MODULE_AUTHOR("Alex <alex@foogod.com>");
 MODULE_AUTHOR("Nick Jones");
 MODULE_AUTHOR("Balint Seeber <n0_5p4m_p13453@hotmail.com>");
 MODULE_AUTHOR("Pavel Roskin <proski@gnu.org>");
+MODULE_AUTHOR("Guido Guenther <agx@sigxcpu.org>");
+MODULE_AUTHOR("Kalle Valo <kalle.valo@iki.fi>");
+MODULE_AUTHOR("Milan Plzik <milan.plzik@gmail.com>");
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/at76_usb/at76_usb.h b/drivers/staging/at76_usb/at76_usb.h
index b20be9da1fa1..8bb352f16d45 100644
--- a/drivers/staging/at76_usb/at76_usb.h
+++ b/drivers/staging/at76_usb/at76_usb.h
@@ -34,23 +34,6 @@ enum board_type {
 	BOARD_505AMX = 8
 };
 
-/* our private ioctl's */
-/* preamble length (0 - long, 1 - short, 2 - auto) */
-#define AT76_SET_SHORT_PREAMBLE		(SIOCIWFIRSTPRIV + 0)
-#define AT76_GET_SHORT_PREAMBLE		(SIOCIWFIRSTPRIV + 1)
-/* which debug channels are enabled */
-#define AT76_SET_DEBUG			(SIOCIWFIRSTPRIV + 2)
-#define AT76_GET_DEBUG			(SIOCIWFIRSTPRIV + 3)
-/* power save mode (incl. the Atmel proprietary smart save mode) */
-#define AT76_SET_POWERSAVE_MODE		(SIOCIWFIRSTPRIV + 4)
-#define AT76_GET_POWERSAVE_MODE		(SIOCIWFIRSTPRIV + 5)
-/* min and max channel times for scan */
-#define AT76_SET_SCAN_TIMES		(SIOCIWFIRSTPRIV + 6)
-#define AT76_GET_SCAN_TIMES		(SIOCIWFIRSTPRIV + 7)
-/* scan mode (0 - active, 1 - passive) */
-#define AT76_SET_SCAN_MODE		(SIOCIWFIRSTPRIV + 8)
-#define AT76_GET_SCAN_MODE		(SIOCIWFIRSTPRIV + 9)
-
 #define CMD_STATUS_IDLE				0x00
 #define CMD_STATUS_COMPLETE			0x01
 #define CMD_STATUS_UNKNOWN			0x02
@@ -82,6 +65,7 @@ enum board_type {
 #define MIB_MAC			0x03
 #define MIB_MAC_MGMT		0x05
 #define MIB_MAC_WEP		0x06
+#define MIB_MAC_ENCRYPTION	0x06
 #define MIB_PHY			0x07
 #define MIB_FW_VERSION		0x08
 #define MIB_MDOMAIN		0x09
@@ -106,6 +90,26 @@ enum board_type {
 #define AT76_PM_ON		2
 #define AT76_PM_SMART		3
 
+/* cipher values for encryption keys */
+#define CIPHER_NONE		0	/* this value is only guessed */
+#define CIPHER_WEP64		1
+#define CIPHER_TKIP		2
+#define CIPHER_CCMP		3
+#define CIPHER_CCX		4	/* for consistency sake only */
+#define CIPHER_WEP128		5
+
+/* bit flags key types for encryption keys */
+#define KEY_PAIRWISE		2
+#define KEY_TX			4
+
+#define CIPHER_KEYS		(4)
+#define CIPHER_KEY_LEN		(40)
+
+struct key_config {
+	u8 cipher;
+	u8 keylen;
+};
+
 struct hwcfg_r505 {
 	u8 cr39_values[14];
 	u8 reserved1[14];
@@ -147,6 +151,9 @@ union at76_hwcfg {
 
 #define WEP_SMALL_KEY_LEN	(40 / 8)
 #define WEP_LARGE_KEY_LEN	(104 / 8)
+#define WEP_KEYS		(4)
+
+
 
 struct at76_card_config {
 	u8 exclude_unencrypted;
@@ -161,7 +168,7 @@ struct at76_card_config {
 	u8 privacy_invoked;
 	u8 wep_default_key_id;	/* 0..3 */
 	u8 current_ssid[32];
-	u8 wep_default_key_value[4][WEP_KEY_LEN];
+	u8 wep_default_key_value[4][WEP_LARGE_KEY_LEN];
 	u8 ssid_len;
 	u8 short_preamble;
 	__le16 beacon_period;
@@ -186,7 +193,7 @@ struct at76_rx_buffer {
 	u8 link_quality;
 	u8 noise_level;
 	__le32 rx_time;
-	u8 packet[IEEE80211_FRAME_LEN + IEEE80211_FCS_LEN];
+	u8 packet[IEEE80211_MAX_FRAG_THRESHOLD];
 } __attribute__((packed));
 
 /* Length of Atmel-specific Tx header before 802.11 frame */
@@ -196,8 +203,11 @@ struct at76_tx_buffer {
 	__le16 wlength;
 	u8 tx_rate;
 	u8 padding;
-	u8 reserved[4];
-	u8 packet[IEEE80211_FRAME_LEN + IEEE80211_FCS_LEN];
+	u8 key_id;
+	u8 cipher_type;
+	u8 cipher_length;
+	u8 reserved;
+	u8 packet[IEEE80211_MAX_FRAG_THRESHOLD];
 } __attribute__((packed));
 
 /* defines for scan_type below */
@@ -244,6 +254,7 @@ struct set_mib_buffer {
 		u8 byte;
 		__le16 word;
 		u8 addr[ETH_ALEN];
+		u8 data[256];	/* we need more space for mib_mac_encryption */
 	} data;
 } __attribute__((packed));
 
@@ -317,10 +328,24 @@ struct mib_mac_wep {
 	u8 exclude_unencrypted;
 	__le32 wep_icv_error_count;
 	__le32 wep_excluded_count;
-	u8 wep_default_keyvalue[WEP_KEYS][WEP_KEY_LEN];
+	u8 wep_default_keyvalue[WEP_KEYS][WEP_LARGE_KEY_LEN];
 	u8 encryption_level;	/* 1 for 40bit, 2 for 104bit encryption */
 } __attribute__((packed));
 
+struct mib_mac_encryption {
+	u8 cipher_default_keyvalue[CIPHER_KEYS][CIPHER_KEY_LEN];
+	u8 tkip_bssid[6];
+	u8 privacy_invoked;
+	u8 cipher_default_key_id;
+	u8 cipher_default_group_key_id;
+	u8 exclude_unencrypted;
+	u8 wep_encryption_type;
+	u8 ckip_key_permutation;	/* bool */
+	__le32 wep_icv_error_count;
+	__le32 wep_excluded_count;
+	u8 key_rsc[CIPHER_KEYS][8];
+} __attribute__((packed));
+
 struct mib_phy {
 	__le32 ed_threshold;
 
@@ -364,16 +389,6 @@ struct at76_fw_header {
 	__le32 ext_fw_len;	/* external firmware image length */
 } __attribute__((packed));
 
-enum mac_state {
-	MAC_INIT,
-	MAC_SCANNING,
-	MAC_AUTH,
-	MAC_ASSOC,
-	MAC_JOINING,
-	MAC_CONNECTED,
-	MAC_OWN_IBSS
-};
-
 /* a description of a regulatory domain and the allowed channels */
 struct reg_domain {
 	u16 code;
@@ -381,47 +396,6 @@ struct reg_domain {
 	u32 channel_map;	/* if bit N is set, channel (N+1) is allowed */
 };
 
-/* how long do we keep a (I)BSS in the bss_list in jiffies
-   this should be long enough for the user to retrieve the table
-   (by iwlist ?) after the device started, because all entries from
-   other channels than the one the device locks on get removed, too */
-#define BSS_LIST_TIMEOUT	(120 * HZ)
-/* struct to store BSS info found during scan */
-#define BSS_LIST_MAX_RATE_LEN	32	/* 32 rates should be enough ... */
-
-struct bss_info {
-	struct list_head list;
-
-	u8 bssid[ETH_ALEN];	/* bssid */
-	u8 ssid[IW_ESSID_MAX_SIZE];	/* essid */
-	u8 ssid_len;		/* length of ssid above */
-	u8 channel;
-	u16 capa;		/* BSS capabilities */
-	u16 beacon_interval;	/* beacon interval, Kus (1024 microseconds) */
-	u8 rates[BSS_LIST_MAX_RATE_LEN];	/* supported rates in units of
-						   500 kbps, ORed with 0x80 for
-						   basic rates */
-	u8 rates_len;
-
-	/* quality of received beacon */
-	u8 rssi;
-	u8 link_qual;
-	u8 noise_level;
-
-	unsigned long last_rx;	/* time (jiffies) of last beacon received */
-};
-
-/* a rx data buffer to collect rx fragments */
-struct rx_data_buf {
-	u8 sender[ETH_ALEN];	/* sender address */
-	u16 seqnr;		/* sequence number */
-	u16 fragnr;		/* last fragment received */
-	unsigned long last_rx;	/* jiffies of last rx */
-	struct sk_buff *skb;	/* == NULL if entry is free */
-};
-
-#define NR_RX_DATA_BUF		8
-
 /* Data for one loaded firmware file */
 struct fwentry {
 	const char *const fwname;
@@ -438,11 +412,9 @@ struct fwentry {
 
 struct at76_priv {
 	struct usb_device *udev;	/* USB device pointer */
-	struct net_device *netdev;	/* net device pointer */
-	struct net_device_stats stats;	/* net device stats */
-	struct iw_statistics wstats;	/* wireless stats */
 
 	struct sk_buff *rx_skb;	/* skbuff for receiving data */
+	struct sk_buff *tx_skb;	/* skbuff for transmitting data */
 	void *bulk_out_buffer;	/* buffer for sending data */
 
 	struct urb *tx_urb;	/* URB for sending data */
@@ -454,26 +426,17 @@ struct at76_priv {
 	struct mutex mtx;	/* locks this structure */
 
 	/* work queues */
-	struct work_struct work_assoc_done;
-	struct work_struct work_join;
-	struct work_struct work_new_bss;
-	struct work_struct work_start_scan;
 	struct work_struct work_set_promisc;
 	struct work_struct work_submit_rx;
-	struct delayed_work dwork_restart;
-	struct delayed_work dwork_get_scan;
-	struct delayed_work dwork_beacon;
-	struct delayed_work dwork_auth;
-	struct delayed_work dwork_assoc;
+	struct delayed_work dwork_hw_scan;
 
 	struct tasklet_struct rx_tasklet;
 
 	/* the WEP stuff */
 	int wep_enabled;	/* 1 if WEP is enabled */
 	int wep_key_id;		/* key id to be used */
-	u8 wep_keys[WEP_KEYS][WEP_KEY_LEN];	/* the four WEP keys,
-						   5 or 13 bytes are used */
-	u8 wep_keys_len[WEP_KEYS];	/* the length of the above keys */
+	u8 wep_keys[WEP_KEYS][WEP_LARGE_KEY_LEN];	/* WEP keys */
+	u8 wep_keys_len[WEP_KEYS];	/* length of WEP keys */
 
 	int channel;
 	int iw_mode;
@@ -495,44 +458,13 @@ struct at76_priv {
 	int scan_mode;		/* SCAN_TYPE_ACTIVE, SCAN_TYPE_PASSIVE */
 	int scan_need_any;	/* if set, need to scan for any ESSID */
 
-	/* the list we got from scanning */
-	spinlock_t bss_list_spinlock;	/* protects bss_list operations */
-	struct list_head bss_list;	/* list of BSS we got beacons from */
-	struct timer_list bss_list_timer;	/* timer to purge old entries
-						   from bss_list */
-	struct bss_info *curr_bss;	/* current BSS */
 	u16 assoc_id;		/* current association ID, if associated */
 
-	u8 wanted_bssid[ETH_ALEN];
-	int wanted_bssid_valid;	/* != 0 if wanted_bssid is to be used */
-
-	/* some data for infrastructure mode only */
-	spinlock_t mgmt_spinlock;	/* this spinlock protects access to
-					   next_mgmt_bulk */
-
-	struct at76_tx_buffer *next_mgmt_bulk;	/* pending management msg to
-						   send via bulk out */
-	enum mac_state mac_state;
-	enum {
-		SCAN_IDLE,
-		SCAN_IN_PROGRESS,
-		SCAN_COMPLETED
-	} scan_state;
-	time_t last_scan;
-
-	int retries;		/* remaining retries in case of timeout when
-				 * sending AuthReq or AssocReq */
 	u8 pm_mode;		/* power management mode */
 	u32 pm_period;		/* power management period in microseconds */
 
 	struct reg_domain const *domain;	/* reg domain description */
 
-	/* iwspy support */
-	spinlock_t spy_spinlock;
-	struct iw_spy_data spy_data;
-
-	struct iw_public_data wireless_data;
-
 	/* These fields contain HW config provided by the device (not all of
 	 * these fields are used by all board types) */
 	u8 mac_addr[ETH_ALEN];
@@ -540,9 +472,6 @@ struct at76_priv {
 
 	struct at76_card_config card_config;
 
-	/* store rx fragments until complete */
-	struct rx_data_buf rx_data[NR_RX_DATA_BUF];
-
 	enum board_type board_type;
 	struct mib_fw_version fw_version;
 
@@ -550,58 +479,20 @@ struct at76_priv {
 	unsigned int netdev_registered:1;
 	struct set_mib_buffer mib_buf;	/* global buffer for set_mib calls */
 
-	/* beacon counting */
 	int beacon_period;	/* period of mgmt beacons, Kus */
-	int beacons_received;
-	unsigned long beacons_last_qual;	/* time we restarted counting
-						   beacons */
-};
 
-struct at76_rx_radiotap {
-	struct ieee80211_radiotap_header rt_hdr;
-	__le64 rt_tsft;
-	u8 rt_flags;
-	u8 rt_rate;
-	s8 rt_signal;
-	s8 rt_noise;
-};
-
-#define AT76_RX_RADIOTAP_PRESENT		  \
-	((1 << IEEE80211_RADIOTAP_TSFT)		| \
-	(1 << IEEE80211_RADIOTAP_FLAGS)		| \
-	(1 << IEEE80211_RADIOTAP_RATE)		| \
-	(1 << IEEE80211_RADIOTAP_DB_ANTSIGNAL)	| \
-	(1 << IEEE80211_RADIOTAP_DB_ANTNOISE))
-
-#define BEACON_MAX_DATA_LENGTH	1500
-
-/* the maximum size of an AssocReq packet */
-#define ASSOCREQ_MAX_SIZE \
-  (AT76_TX_HDRLEN + sizeof(struct ieee80211_assoc_request) + \
-   1 + 1 + IW_ESSID_MAX_SIZE + 1 + 1 + 4)
-
-/* for shared secret auth, add the challenge text size */
-#define AUTH_FRAME_SIZE (AT76_TX_HDRLEN + sizeof(struct ieee80211_auth))
+	struct ieee80211_hw *hw;
+	int mac80211_registered;
 
-/* Maximal number of AuthReq retries */
-#define AUTH_RETRIES		3
-
-/* Maximal number of AssocReq retries */
-#define ASSOC_RETRIES		3
-
-/* Beacon timeout in managed mode when we are connected */
-#define BEACON_TIMEOUT		(10 * HZ)
-
-/* Timeout for authentication response */
-#define AUTH_TIMEOUT		(1 * HZ)
+	struct key_config keys[4];	/* installed key types */
+	u8 default_pairwise_key;
+	u8 default_group_key;
+};
 
-/* Timeout for association response */
-#define ASSOC_TIMEOUT		(1 * HZ)
+#define AT76_SUPPORTED_FILTERS FIF_PROMISC_IN_BSS
 
-/* Polling interval when scan is running */
 #define SCAN_POLL_INTERVAL	(HZ / 4)
 
-/* Command completion timeout */
 #define CMD_COMPLETION_TIMEOUT	(5 * HZ)
 
 #define DEF_RTS_THRESHOLD	1536
@@ -611,8 +502,6 @@ struct at76_rx_radiotap {
 #define DEF_SCAN_MIN_TIME	10
 #define DEF_SCAN_MAX_TIME	120
 
-#define MAX_RTS_THRESHOLD	(MAX_FRAG_THRESHOLD + 1)
-
 /* the max padding size for tx in bytes (see calc_padding) */
 #define MAX_PADDING_SIZE	53
 
diff --git a/drivers/staging/benet/Kconfig b/drivers/staging/benet/Kconfig
new file mode 100644
index 000000000000..f6806074f998
--- /dev/null
+++ b/drivers/staging/benet/Kconfig
@@ -0,0 +1,7 @@
+config BENET
+	tristate "ServerEngines 10Gb NIC - BladeEngine"
+	depends on PCI && INET
+	select INET_LRO
+	help
+	  This driver implements the NIC functionality for ServerEngines
+	  10Gb network adapter BladeEngine (EC 3210).
diff --git a/drivers/staging/benet/MAINTAINERS b/drivers/staging/benet/MAINTAINERS
new file mode 100644
index 000000000000..d5ce340218b3
--- /dev/null
+++ b/drivers/staging/benet/MAINTAINERS
@@ -0,0 +1,6 @@
+SERVER ENGINES 10Gbe NIC - BLADE-ENGINE
+P:	Subbu Seetharaman
+M:	subbus@serverengines.com
+L:	netdev@vger.kernel.org
+W:	http://www.serverengines.com
+S:	Supported
diff --git a/drivers/staging/benet/Makefile b/drivers/staging/benet/Makefile
new file mode 100644
index 000000000000..460b923b99bd
--- /dev/null
+++ b/drivers/staging/benet/Makefile
@@ -0,0 +1,14 @@
+#
+# Makefile to build the network driver for ServerEngine's BladeEngine
+#
+obj-$(CONFIG_BENET) += benet.o
+
+benet-y :=  be_init.o \
+			be_int.o \
+			be_netif.o \
+			be_ethtool.o \
+			funcobj.o \
+			cq.o \
+			eq.o \
+			mpu.o \
+			eth.o
diff --git a/drivers/staging/benet/TODO b/drivers/staging/benet/TODO
new file mode 100644
index 000000000000..a51dfb59a62f
--- /dev/null
+++ b/drivers/staging/benet/TODO
@@ -0,0 +1,6 @@
+TODO:
+	- remove wrappers around common iowrite functions
+	- full netdev audit of common problems/issues
+
+Please send all patches and questions to Subbu Seetharaman
+<subbus@serverengines.com> and Greg Kroah-Hartman <greg@kroah.com>
diff --git a/drivers/staging/benet/asyncmesg.h b/drivers/staging/benet/asyncmesg.h
new file mode 100644
index 000000000000..d1e779adb848
--- /dev/null
+++ b/drivers/staging/benet/asyncmesg.h
@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __asyncmesg_amap_h__
+#define __asyncmesg_amap_h__
+#include "fwcmd_common.h"
+
+/* --- ASYNC_EVENT_CODES --- */
+#define ASYNC_EVENT_CODE_LINK_STATE     (1)
+#define ASYNC_EVENT_CODE_ISCSI          (2)
+
+/* --- ASYNC_LINK_STATES --- */
+#define ASYNC_EVENT_LINK_DOWN           (0)	/* Link Down on a port */
+#define ASYNC_EVENT_LINK_UP             (1)	/* Link Up on a port */
+
+/*
+ * The last 4 bytes of the async events have this common format.  It allows
+ * the driver to distinguish [link]MCC_CQ_ENTRY[/link] structs from
+ * asynchronous events.  Both arrive on the same completion queue.  This
+ * structure also contains the common fields used to decode the async event.
+ */
+struct BE_ASYNC_EVENT_TRAILER_AMAP {
+	u8 rsvd0[8];	/* DWORD 0 */
+	u8 event_code[8];	/* DWORD 0 */
+	u8 event_type[8];	/* DWORD 0 */
+	u8 rsvd1[6];	/* DWORD 0 */
+	u8 async_event;	/* DWORD 0 */
+	u8 valid;		/* DWORD 0 */
+} __packed;
+struct ASYNC_EVENT_TRAILER_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * Applicable in Initiator, Target and NIC modes.
+ * A link state async event is seen by all device drivers as soon they
+ * create an MCC ring. Thereafter, anytime the link status changes the
+ * drivers will receive a link state async event. Notifications continue to
+ * be sent until a driver destroys its MCC ring. A link down event is
+ * reported when either port loses link. A link up event is reported
+ * when either port regains link. When BE's failover mechanism is enabled, a
+ * link down on the active port causes traffic to be diverted to the standby
+ * port by the BE's ARM firmware (assuming the standby port has link). In
+ * this case, the standy port assumes the active status. Note: when link is
+ * restored on the failed port, traffic continues on the currently active
+ * port. The ARM firmware does not attempt to 'fail back' traffic to
+ * the restored port.
+ */
+struct BE_ASYNC_EVENT_LINK_STATE_AMAP {
+	u8 port0_link_status[8];
+	u8 port1_link_status[8];
+	u8 active_port[8];
+	u8 rsvd0[8];	/* DWORD 0 */
+	u8 port0_duplex[8];
+	u8 port0_speed[8];
+	u8 port1_duplex[8];
+	u8 port1_speed[8];
+	u8 port0_fault[8];
+	u8 port1_fault[8];
+	u8 rsvd1[2][8];	/* DWORD 2 */
+	struct BE_ASYNC_EVENT_TRAILER_AMAP trailer;
+} __packed;
+struct ASYNC_EVENT_LINK_STATE_AMAP {
+	u32 dw[4];
+};
+#endif /* __asyncmesg_amap_h__ */
diff --git a/drivers/staging/benet/be_cm.h b/drivers/staging/benet/be_cm.h
new file mode 100644
index 000000000000..b7a1dfd20c36
--- /dev/null
+++ b/drivers/staging/benet/be_cm.h
@@ -0,0 +1,134 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __be_cm_amap_h__
+#define __be_cm_amap_h__
+#include "be_common.h"
+#include "etx_context.h"
+#include "mpu_context.h"
+
+/*
+ * --- CEV_WATERMARK_ENUM ---
+ * CQ/EQ Watermark Encodings. Encoded as number of free entries in
+ * Queue when Watermark is reached.
+ */
+#define CEV_WMARK_0        (0)	/* Watermark when Queue full */
+#define CEV_WMARK_16       (1)	/* Watermark at 16 free entries */
+#define CEV_WMARK_32       (2)	/* Watermark at 32 free entries */
+#define CEV_WMARK_48       (3)	/* Watermark at 48 free entries */
+#define CEV_WMARK_64       (4)	/* Watermark at 64 free entries */
+#define CEV_WMARK_80       (5)	/* Watermark at 80 free entries */
+#define CEV_WMARK_96       (6)	/* Watermark at 96 free entries */
+#define CEV_WMARK_112      (7)	/* Watermark at 112 free entries */
+#define CEV_WMARK_128      (8)	/* Watermark at 128 free entries */
+#define CEV_WMARK_144      (9)	/* Watermark at 144 free entries */
+#define CEV_WMARK_160      (10)	/* Watermark at 160 free entries */
+#define CEV_WMARK_176      (11)	/* Watermark at 176 free entries */
+#define CEV_WMARK_192      (12)	/* Watermark at 192 free entries */
+#define CEV_WMARK_208      (13)	/* Watermark at 208 free entries */
+#define CEV_WMARK_224      (14)	/* Watermark at 224 free entries */
+#define CEV_WMARK_240      (15)	/* Watermark at 240 free entries */
+
+/*
+ * --- CQ_CNT_ENUM ---
+ * Completion Queue Count Encodings.
+ */
+#define CEV_CQ_CNT_256                  (0)	/* CQ has 256 entries */
+#define CEV_CQ_CNT_512                  (1)	/* CQ has 512 entries */
+#define CEV_CQ_CNT_1024                 (2)	/* CQ has 1024 entries */
+
+/*
+ * --- EQ_CNT_ENUM ---
+ * Event Queue Count Encodings.
+ */
+#define CEV_EQ_CNT_256     (0)	/* EQ has 256 entries (16-byte EQEs only) */
+#define CEV_EQ_CNT_512     (1)	/* EQ has 512 entries (16-byte EQEs only) */
+#define CEV_EQ_CNT_1024    (2)	/* EQ has 1024 entries (4-byte or */
+				/* 16-byte EQEs only) */
+#define CEV_EQ_CNT_2048    (3)	/* EQ has 2048 entries (4-byte or */
+				/* 16-byte EQEs only) */
+#define CEV_EQ_CNT_4096    (4)	/* EQ has 4096 entries (4-byte EQEs only) */
+
+/*
+ * --- EQ_SIZE_ENUM ---
+ * Event Queue Entry Size Encoding.
+ */
+#define CEV_EQ_SIZE_4                   (0)	/* EQE is 4 bytes */
+#define CEV_EQ_SIZE_16                  (1)	/* EQE is 16 bytes */
+
+/*
+ * Completion Queue Context Table Entry. Contains the state of a CQ.
+ * Located in RAM within the CEV block.
+ */
+struct BE_CQ_CONTEXT_AMAP {
+	u8 Cidx[11];	/* DWORD 0 */
+	u8 Watermark[4];	/* DWORD 0 */
+	u8 NoDelay;		/* DWORD 0 */
+	u8 EPIdx[11];	/* DWORD 0 */
+	u8 Count[2];	/* DWORD 0 */
+	u8 valid;		/* DWORD 0 */
+	u8 SolEvent;	/* DWORD 0 */
+	u8 Eventable;	/* DWORD 0 */
+	u8 Pidx[11];	/* DWORD 1 */
+	u8 PD[10];		/* DWORD 1 */
+	u8 EQID[7];		/* DWORD 1 */
+	u8 Func;		/* DWORD 1 */
+	u8 WME;		/* DWORD 1 */
+	u8 Stalled;		/* DWORD 1 */
+	u8 Armed;		/* DWORD 1 */
+} __packed;
+struct CQ_CONTEXT_AMAP {
+	u32 dw[2];
+};
+
+/*
+ * Event Queue Context Table Entry. Contains the state of an EQ.
+ * Located in RAM in the CEV block.
+ */
+struct BE_EQ_CONTEXT_AMAP {
+	u8 Cidx[13];	/* DWORD 0 */
+	u8 rsvd0[2];	/* DWORD 0 */
+	u8 Func;		/* DWORD 0 */
+	u8 EPIdx[13];	/* DWORD 0 */
+	u8 valid;		/* DWORD 0 */
+	u8 rsvd1;		/* DWORD 0 */
+	u8 Size;		/* DWORD 0 */
+	u8 Pidx[13];	/* DWORD 1 */
+	u8 rsvd2[3];	/* DWORD 1 */
+	u8 PD[10];		/* DWORD 1 */
+	u8 Count[3];	/* DWORD 1 */
+	u8 SolEvent;	/* DWORD 1 */
+	u8 Stalled;		/* DWORD 1 */
+	u8 Armed;		/* DWORD 1 */
+	u8 Watermark[4];	/* DWORD 2 */
+	u8 WME;		/* DWORD 2 */
+	u8 rsvd3[3];	/* DWORD 2 */
+	u8 EventVect[6];	/* DWORD 2 */
+	u8 rsvd4[2];	/* DWORD 2 */
+	u8 Delay[8];	/* DWORD 2 */
+	u8 rsvd5[6];	/* DWORD 2 */
+	u8 TMR;		/* DWORD 2 */
+	u8 rsvd6;		/* DWORD 2 */
+	u8 rsvd7[32];	/* DWORD 3 */
+} __packed;
+struct EQ_CONTEXT_AMAP {
+	u32 dw[4];
+};
+
+#endif /* __be_cm_amap_h__ */
diff --git a/drivers/staging/benet/be_common.h b/drivers/staging/benet/be_common.h
new file mode 100644
index 000000000000..7e63dc5e3348
--- /dev/null
+++ b/drivers/staging/benet/be_common.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __be_common_amap_h__
+#define __be_common_amap_h__
+
+/* Physical Address. */
+struct BE_PHYS_ADDR_AMAP {
+	u8 lo[32];		/* DWORD 0 */
+	u8 hi[32];		/* DWORD 1 */
+} __packed;
+struct PHYS_ADDR_AMAP {
+	u32 dw[2];
+};
+
+/* Virtual Address. */
+struct BE_VIRT_ADDR_AMAP {
+	u8 lo[32];		/* DWORD 0 */
+	u8 hi[32];		/* DWORD 1 */
+} __packed;
+struct VIRT_ADDR_AMAP {
+	u32 dw[2];
+};
+
+/* Scatter gather element. */
+struct BE_SGE_AMAP {
+	u8 addr_hi[32];	/* DWORD 0 */
+	u8 addr_lo[32];	/* DWORD 1 */
+	u8 rsvd0[32];	/* DWORD 2 */
+	u8 len[16];		/* DWORD 3 */
+	u8 rsvd1[16];	/* DWORD 3 */
+} __packed;
+struct SGE_AMAP {
+	u32 dw[4];
+};
+
+#endif /* __be_common_amap_h__ */
diff --git a/drivers/staging/benet/be_ethtool.c b/drivers/staging/benet/be_ethtool.c
new file mode 100644
index 000000000000..027af85707aa
--- /dev/null
+++ b/drivers/staging/benet/be_ethtool.c
@@ -0,0 +1,348 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * be_ethtool.c
+ *
+ * 	This file contains various functions that ethtool can use
+ * 	to talk to the driver and the BE H/W.
+ */
+
+#include "benet.h"
+
+#include <linux/ethtool.h>
+
+static const char benet_gstrings_stats[][ETH_GSTRING_LEN] = {
+/* net_device_stats */
+	"rx_packets",
+	"tx_packets",
+	"rx_bytes",
+	"tx_bytes",
+	"rx_errors",
+	"tx_errors",
+	"rx_dropped",
+	"tx_dropped",
+	"multicast",
+	"collisions",
+	"rx_length_errors",
+	"rx_over_errors",
+	"rx_crc_errors",
+	"rx_frame_errors",
+	"rx_fifo_errors",
+	"rx_missed_errors",
+	"tx_aborted_errors",
+	"tx_carrier_errors",
+	"tx_fifo_errors",
+	"tx_heartbeat_errors",
+	"tx_window_errors",
+	"rx_compressed",
+	"tc_compressed",
+/* BE driver Stats */
+	"bes_tx_reqs",
+	"bes_tx_fails",
+	"bes_fwd_reqs",
+	"bes_tx_wrbs",
+	"bes_interrupts",
+	"bes_events",
+	"bes_tx_events",
+	"bes_rx_events",
+	"bes_tx_compl",
+	"bes_rx_compl",
+	"bes_ethrx_post_fail",
+	"bes_802_3_dropped_frames",
+	"bes_802_3_malformed_frames",
+	"bes_rx_misc_pkts",
+	"bes_eth_tx_rate",
+	"bes_eth_rx_rate",
+	"Num Packets collected",
+	"Num Times Flushed",
+};
+
+#define NET_DEV_STATS_LEN \
+	(sizeof(struct net_device_stats)/sizeof(unsigned long))
+
+#define BENET_STATS_LEN  ARRAY_SIZE(benet_gstrings_stats)
+
+static void
+be_get_drvinfo(struct net_device *netdev, struct ethtool_drvinfo *drvinfo)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+
+	strncpy(drvinfo->driver, be_driver_name, 32);
+	strncpy(drvinfo->version, be_drvr_ver, 32);
+	strncpy(drvinfo->fw_version, be_fw_ver, 32);
+	strcpy(drvinfo->bus_info, pci_name(adapter->pdev));
+	drvinfo->testinfo_len = 0;
+	drvinfo->regdump_len = 0;
+	drvinfo->eedump_len = 0;
+}
+
+static int
+be_get_coalesce(struct net_device *netdev, struct ethtool_coalesce *coalesce)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+
+	coalesce->rx_max_coalesced_frames = adapter->max_rx_coal;
+
+	coalesce->rx_coalesce_usecs = adapter->cur_eqd;
+	coalesce->rx_coalesce_usecs_high = adapter->max_eqd;
+	coalesce->rx_coalesce_usecs_low = adapter->min_eqd;
+
+	coalesce->tx_coalesce_usecs = adapter->cur_eqd;
+	coalesce->tx_coalesce_usecs_high = adapter->max_eqd;
+	coalesce->tx_coalesce_usecs_low = adapter->min_eqd;
+
+	coalesce->use_adaptive_rx_coalesce = adapter->enable_aic;
+	coalesce->use_adaptive_tx_coalesce = adapter->enable_aic;
+
+	return 0;
+}
+
+/*
+ * This routine is used to set interrup coalescing delay *as well as*
+ * the number of pkts to coalesce for LRO.
+ */
+static int
+be_set_coalesce(struct net_device *netdev, struct ethtool_coalesce *coalesce)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+	struct be_eq_object *eq_objectp;
+	u32 max, min, cur;
+	int status;
+
+	adapter->max_rx_coal = coalesce->rx_max_coalesced_frames;
+	if (adapter->max_rx_coal >= BE_LRO_MAX_PKTS)
+		adapter->max_rx_coal = BE_LRO_MAX_PKTS;
+
+	if (adapter->enable_aic == 0 &&
+		coalesce->use_adaptive_rx_coalesce == 1) {
+		/* if AIC is being turned on now, start with an EQD of 0 */
+		adapter->cur_eqd = 0;
+	}
+	adapter->enable_aic = coalesce->use_adaptive_rx_coalesce;
+
+	/* round off to nearest multiple of 8 */
+	max = (((coalesce->rx_coalesce_usecs_high + 4) >> 3) << 3);
+	min = (((coalesce->rx_coalesce_usecs_low + 4) >> 3) << 3);
+	cur = (((coalesce->rx_coalesce_usecs + 4) >> 3) << 3);
+
+	if (adapter->enable_aic) {
+		/* accept low and high if AIC is enabled */
+		if (max > MAX_EQD)
+			max = MAX_EQD;
+		if (min > max)
+			min = max;
+		adapter->max_eqd = max;
+		adapter->min_eqd = min;
+		if (adapter->cur_eqd > max)
+			adapter->cur_eqd = max;
+		if (adapter->cur_eqd < min)
+			adapter->cur_eqd = min;
+	} else {
+		/* accept specified coalesce_usecs only if AIC is disabled */
+		if (cur > MAX_EQD)
+			cur = MAX_EQD;
+		eq_objectp = &pnob->event_q_obj;
+		status =
+		    be_eq_modify_delay(&pnob->fn_obj, 1, &eq_objectp, &cur,
+				       NULL, NULL, NULL);
+		if (status == BE_SUCCESS)
+			adapter->cur_eqd = cur;
+	}
+	return 0;
+}
+
+static u32 be_get_rx_csum(struct net_device *netdev)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+	return adapter->rx_csum;
+}
+
+static int be_set_rx_csum(struct net_device *netdev, uint32_t data)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+
+	if (data)
+		adapter->rx_csum = 1;
+	else
+		adapter->rx_csum = 0;
+
+	return 0;
+}
+
+static void
+be_get_strings(struct net_device *netdev, uint32_t stringset, uint8_t *data)
+{
+	switch (stringset) {
+	case ETH_SS_STATS:
+		memcpy(data, *benet_gstrings_stats,
+		       sizeof(benet_gstrings_stats));
+		break;
+	}
+}
+
+static int be_get_stats_count(struct net_device *netdev)
+{
+	return BENET_STATS_LEN;
+}
+
+static void
+be_get_ethtool_stats(struct net_device *netdev,
+		     struct ethtool_stats *stats, uint64_t *data)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+	int i;
+
+	benet_get_stats(netdev);
+
+	for (i = 0; i <= NET_DEV_STATS_LEN; i++)
+		data[i] = ((unsigned long *)&adapter->benet_stats)[i];
+
+	data[i] = adapter->be_stat.bes_tx_reqs;
+	data[i++] = adapter->be_stat.bes_tx_fails;
+	data[i++] = adapter->be_stat.bes_fwd_reqs;
+	data[i++] = adapter->be_stat.bes_tx_wrbs;
+
+	data[i++] = adapter->be_stat.bes_ints;
+	data[i++] = adapter->be_stat.bes_events;
+	data[i++] = adapter->be_stat.bes_tx_events;
+	data[i++] = adapter->be_stat.bes_rx_events;
+	data[i++] = adapter->be_stat.bes_tx_compl;
+	data[i++] = adapter->be_stat.bes_rx_compl;
+	data[i++] = adapter->be_stat.bes_ethrx_post_fail;
+	data[i++] = adapter->be_stat.bes_802_3_dropped_frames;
+	data[i++] = adapter->be_stat.bes_802_3_malformed_frames;
+	data[i++] = adapter->be_stat.bes_rx_misc_pkts;
+	data[i++] = adapter->be_stat.bes_eth_tx_rate;
+	data[i++] = adapter->be_stat.bes_eth_rx_rate;
+	data[i++] = adapter->be_stat.bes_rx_coal;
+	data[i++] = adapter->be_stat.bes_rx_flush;
+
+}
+
+static int be_get_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
+{
+	ecmd->speed = SPEED_10000;
+	ecmd->duplex = DUPLEX_FULL;
+	ecmd->autoneg = AUTONEG_DISABLE;
+	return 0;
+}
+
+/* Get the Ring parameters from the pnob */
+static void
+be_get_ringparam(struct net_device *netdev, struct ethtool_ringparam *ring)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	/* Pre Set Maxims */
+	ring->rx_max_pending = pnob->rx_q_len;
+	ring->rx_mini_max_pending = ring->rx_mini_max_pending;
+	ring->rx_jumbo_max_pending = ring->rx_jumbo_max_pending;
+	ring->tx_max_pending = pnob->tx_q_len;
+
+	/* Current hardware Settings                */
+	ring->rx_pending = atomic_read(&pnob->rx_q_posted);
+	ring->rx_mini_pending = ring->rx_mini_pending;
+	ring->rx_jumbo_pending = ring->rx_jumbo_pending;
+	ring->tx_pending = atomic_read(&pnob->tx_q_used);
+
+}
+
+static void
+be_get_pauseparam(struct net_device *netdev, struct ethtool_pauseparam *ecmd)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	bool rxfc, txfc;
+	int status;
+
+	status = be_eth_get_flow_control(&pnob->fn_obj, &txfc, &rxfc);
+	if (status != BE_SUCCESS) {
+		dev_info(&netdev->dev, "Unable to get pause frame settings\n");
+		/* return defaults */
+		ecmd->rx_pause = 1;
+		ecmd->tx_pause = 0;
+		ecmd->autoneg = AUTONEG_ENABLE;
+		return;
+	}
+
+	if (txfc == true)
+		ecmd->tx_pause = 1;
+	else
+		ecmd->tx_pause = 0;
+
+	if (rxfc == true)
+		ecmd->rx_pause = 1;
+	else
+		ecmd->rx_pause = 0;
+
+	ecmd->autoneg = AUTONEG_ENABLE;
+}
+
+static int
+be_set_pauseparam(struct net_device *netdev, struct ethtool_pauseparam *ecmd)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	bool txfc, rxfc;
+	int status;
+
+	if (ecmd->autoneg != AUTONEG_ENABLE)
+		return -EINVAL;
+
+	if (ecmd->tx_pause)
+		txfc = true;
+	else
+		txfc = false;
+
+	if (ecmd->rx_pause)
+		rxfc = true;
+	else
+		rxfc = false;
+
+	status = be_eth_set_flow_control(&pnob->fn_obj, txfc, rxfc);
+	if (status != BE_SUCCESS) {
+		dev_info(&netdev->dev, "Unable to set pause frame settings\n");
+		return -1;
+	}
+	return 0;
+}
+
+struct ethtool_ops be_ethtool_ops = {
+	.get_settings = be_get_settings,
+	.get_drvinfo = be_get_drvinfo,
+	.get_link = ethtool_op_get_link,
+	.get_coalesce = be_get_coalesce,
+	.set_coalesce = be_set_coalesce,
+	.get_ringparam = be_get_ringparam,
+	.get_pauseparam = be_get_pauseparam,
+	.set_pauseparam = be_set_pauseparam,
+	.get_rx_csum = be_get_rx_csum,
+	.set_rx_csum = be_set_rx_csum,
+	.get_tx_csum = ethtool_op_get_tx_csum,
+	.set_tx_csum = ethtool_op_set_tx_csum,
+	.get_sg = ethtool_op_get_sg,
+	.set_sg = ethtool_op_set_sg,
+	.get_tso = ethtool_op_get_tso,
+	.set_tso = ethtool_op_set_tso,
+	.get_strings = be_get_strings,
+	.get_stats_count = be_get_stats_count,
+	.get_ethtool_stats = be_get_ethtool_stats,
+};
diff --git a/drivers/staging/benet/be_init.c b/drivers/staging/benet/be_init.c
new file mode 100644
index 000000000000..12a026c3f9e1
--- /dev/null
+++ b/drivers/staging/benet/be_init.c
@@ -0,0 +1,1382 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#include <linux/etherdevice.h>
+#include "benet.h"
+
+#define  DRVR_VERSION  "1.0.728"
+
+static const struct pci_device_id be_device_id_table[] = {
+	{PCI_DEVICE(0x19a2, 0x0201)},
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, be_device_id_table);
+
+MODULE_VERSION(DRVR_VERSION);
+
+#define DRV_DESCRIPTION "ServerEngines BladeEngine Network Driver Version "
+
+MODULE_DESCRIPTION(DRV_DESCRIPTION DRVR_VERSION);
+MODULE_AUTHOR("ServerEngines");
+MODULE_LICENSE("GPL");
+
+static unsigned int msix = 1;
+module_param(msix, uint, S_IRUGO);
+MODULE_PARM_DESC(msix, "Use MSI-x interrupts");
+
+static unsigned int rxbuf_size = 2048;	/* Default RX frag size */
+module_param(rxbuf_size, uint, S_IRUGO);
+MODULE_PARM_DESC(rxbuf_size, "Size of buffers to hold Rx data");
+
+const char be_drvr_ver[] = DRVR_VERSION;
+char be_fw_ver[32];		/* F/W version filled in by be_probe */
+char be_driver_name[] = "benet";
+
+/*
+ * Number of entries in each queue.
+ */
+#define EVENT_Q_LEN		1024
+#define ETH_TXQ_LEN		2048
+#define ETH_TXCQ_LEN		1024
+#define ETH_RXQ_LEN		1024	/* Does not support any other value */
+#define ETH_UC_RXCQ_LEN		1024
+#define ETH_BC_RXCQ_LEN		256
+#define MCC_Q_LEN               64	/* total size not to exceed 8 pages */
+#define MCC_CQ_LEN              256
+
+/* Bit mask describing events of interest to be traced */
+unsigned int trace_level;
+
+static int
+init_pci_be_function(struct be_adapter *adapter, struct pci_dev *pdev)
+{
+	u64 pa;
+
+	/* CSR */
+	pa = pci_resource_start(pdev, 2);
+	adapter->csr_va = ioremap_nocache(pa, pci_resource_len(pdev, 2));
+	if (adapter->csr_va == NULL)
+		return -ENOMEM;
+
+	/* Door Bell */
+	pa = pci_resource_start(pdev, 4);
+	adapter->db_va = ioremap_nocache(pa, (128 * 1024));
+	if (adapter->db_va == NULL) {
+		iounmap(adapter->csr_va);
+		return -ENOMEM;
+	}
+
+	/* PCI */
+	pa = pci_resource_start(pdev, 1);
+	adapter->pci_va = ioremap_nocache(pa, pci_resource_len(pdev, 1));
+	if (adapter->pci_va == NULL) {
+		iounmap(adapter->csr_va);
+		iounmap(adapter->db_va);
+		return -ENOMEM;
+	}
+	return 0;
+}
+
+/*
+   This function enables the interrupt corresponding to the Event
+   queue ID for the given NetObject
+*/
+void be_enable_eq_intr(struct be_net_object *pnob)
+{
+	struct CQ_DB_AMAP cqdb;
+	cqdb.dw[0] = 0;
+	AMAP_SET_BITS_PTR(CQ_DB, event, &cqdb, 1);
+	AMAP_SET_BITS_PTR(CQ_DB, rearm, &cqdb, 1);
+	AMAP_SET_BITS_PTR(CQ_DB, num_popped, &cqdb, 0);
+	AMAP_SET_BITS_PTR(CQ_DB, qid, &cqdb, pnob->event_q_id);
+	PD_WRITE(&pnob->fn_obj, cq_db, cqdb.dw[0]);
+}
+
+/*
+   This function disables the interrupt corresponding to the Event
+   queue ID for the given NetObject
+*/
+void be_disable_eq_intr(struct be_net_object *pnob)
+{
+	struct CQ_DB_AMAP cqdb;
+	cqdb.dw[0] = 0;
+	AMAP_SET_BITS_PTR(CQ_DB, event, &cqdb, 1);
+	AMAP_SET_BITS_PTR(CQ_DB, rearm, &cqdb, 0);
+	AMAP_SET_BITS_PTR(CQ_DB, num_popped, &cqdb, 0);
+	AMAP_SET_BITS_PTR(CQ_DB, qid, &cqdb, pnob->event_q_id);
+	PD_WRITE(&pnob->fn_obj, cq_db, cqdb.dw[0]);
+}
+
+/*
+    This function enables the interrupt from the  network function
+    of the BladeEngine. Use the function be_disable_eq_intr()
+    to enable the interrupt from the event queue of only one specific
+    NetObject
+*/
+void be_enable_intr(struct be_net_object *pnob)
+{
+	struct PCICFG_HOST_TIMER_INT_CTRL_CSR_AMAP ctrl;
+	u32 host_intr;
+
+	ctrl.dw[0] = PCICFG1_READ(&pnob->fn_obj, host_timer_int_ctrl);
+	host_intr = AMAP_GET_BITS_PTR(PCICFG_HOST_TIMER_INT_CTRL_CSR,
+							hostintr, ctrl.dw);
+	if (!host_intr) {
+		AMAP_SET_BITS_PTR(PCICFG_HOST_TIMER_INT_CTRL_CSR,
+			hostintr, ctrl.dw, 1);
+		PCICFG1_WRITE(&pnob->fn_obj, host_timer_int_ctrl,
+			ctrl.dw[0]);
+	}
+}
+
+/*
+   This function disables the interrupt from the network function of
+   the BladeEngine.  Use the function be_disable_eq_intr() to
+   disable the interrupt from the event queue of only one specific NetObject
+*/
+void be_disable_intr(struct be_net_object *pnob)
+{
+
+	struct PCICFG_HOST_TIMER_INT_CTRL_CSR_AMAP ctrl;
+	u32 host_intr;
+	ctrl.dw[0] = PCICFG1_READ(&pnob->fn_obj, host_timer_int_ctrl);
+	host_intr = AMAP_GET_BITS_PTR(PCICFG_HOST_TIMER_INT_CTRL_CSR,
+							hostintr, ctrl.dw);
+	if (host_intr) {
+		AMAP_SET_BITS_PTR(PCICFG_HOST_TIMER_INT_CTRL_CSR, hostintr,
+			ctrl.dw, 0);
+		PCICFG1_WRITE(&pnob->fn_obj, host_timer_int_ctrl,
+			ctrl.dw[0]);
+	}
+}
+
+static int be_enable_msix(struct be_adapter *adapter)
+{
+	int i, ret;
+
+	if (!msix)
+		return -1;
+
+	for (i = 0; i < BE_MAX_REQ_MSIX_VECTORS; i++)
+		adapter->msix_entries[i].entry = i;
+
+	ret = pci_enable_msix(adapter->pdev, adapter->msix_entries,
+		BE_MAX_REQ_MSIX_VECTORS);
+
+	if (ret == 0)
+		adapter->msix_enabled = 1;
+	return ret;
+}
+
+static int be_register_isr(struct be_adapter *adapter,
+		struct be_net_object *pnob)
+{
+	struct net_device *netdev = pnob->netdev;
+	int intx = 0, r;
+
+	netdev->irq = adapter->pdev->irq;
+	r = be_enable_msix(adapter);
+
+	if (r == 0) {
+		r = request_irq(adapter->msix_entries[0].vector,
+				be_int, IRQF_SHARED, netdev->name, netdev);
+		if (r) {
+			printk(KERN_WARNING
+				"MSIX Request IRQ failed - Errno %d\n", r);
+			intx = 1;
+			pci_disable_msix(adapter->pdev);
+			adapter->msix_enabled = 0;
+		}
+	} else {
+		intx = 1;
+	}
+
+	if (intx) {
+		r = request_irq(netdev->irq, be_int, IRQF_SHARED,
+				netdev->name, netdev);
+		if (r) {
+			printk(KERN_WARNING
+				"INTx Request IRQ failed - Errno %d\n", r);
+			return -1;
+		}
+	}
+	adapter->isr_registered = 1;
+	return 0;
+}
+
+static void be_unregister_isr(struct be_adapter *adapter)
+{
+	struct net_device *netdev = adapter->netdevp;
+	if (adapter->isr_registered) {
+		if (adapter->msix_enabled) {
+			free_irq(adapter->msix_entries[0].vector, netdev);
+			pci_disable_msix(adapter->pdev);
+			adapter->msix_enabled = 0;
+		} else {
+			free_irq(netdev->irq, netdev);
+		}
+		adapter->isr_registered = 0;
+	}
+}
+
+/*
+    This function processes the Flush Completions that are issued by the
+    ARM F/W, when a Recv Ring is destroyed.  A flush completion is
+    identified when a Rx COmpl descriptor has the tcpcksum and udpcksum
+    set and the pktsize is 32.  These completions are received on the
+    Rx Completion Queue.
+*/
+static u32 be_process_rx_flush_cmpl(struct be_net_object *pnob)
+{
+	struct ETH_RX_COMPL_AMAP *rxcp;
+	unsigned int i = 0;
+	while ((rxcp = be_get_rx_cmpl(pnob)) != NULL) {
+		be_notify_cmpl(pnob, 1, pnob->rx_cq_id, 1);
+		i++;
+	}
+	return i;
+}
+
+static void be_tx_q_clean(struct be_net_object *pnob)
+{
+	while (atomic_read(&pnob->tx_q_used))
+		process_one_tx_compl(pnob, tx_compl_lastwrb_idx_get(pnob));
+}
+
+static void be_rx_q_clean(struct be_net_object *pnob)
+{
+	if (pnob->rx_ctxt) {
+		int i;
+		struct be_rx_page_info *rx_page_info;
+		for (i = 0; i < pnob->rx_q_len; i++) {
+			rx_page_info = &(pnob->rx_page_info[i]);
+			if (!pnob->rx_pg_shared || rx_page_info->page_offset) {
+				pci_unmap_page(pnob->adapter->pdev,
+				       pci_unmap_addr(rx_page_info, bus),
+					       pnob->rx_buf_size,
+					       PCI_DMA_FROMDEVICE);
+			}
+			if (rx_page_info->page)
+				put_page(rx_page_info->page);
+			memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+		}
+		pnob->rx_pg_info_hd = 0;
+	}
+}
+
+static void be_destroy_netobj(struct be_net_object *pnob)
+{
+	int status;
+
+	if (pnob->tx_q_created) {
+		status = be_eth_sq_destroy(&pnob->tx_q_obj);
+		pnob->tx_q_created = 0;
+	}
+
+	if (pnob->rx_q_created) {
+		status = be_eth_rq_destroy(&pnob->rx_q_obj);
+		if (status != 0) {
+			status = be_eth_rq_destroy_options(&pnob->rx_q_obj, 0,
+						      NULL, NULL);
+			BUG_ON(status);
+		}
+		pnob->rx_q_created = 0;
+	}
+
+	be_process_rx_flush_cmpl(pnob);
+
+	if (pnob->tx_cq_created) {
+		status = be_cq_destroy(&pnob->tx_cq_obj);
+		pnob->tx_cq_created = 0;
+	}
+
+	if (pnob->rx_cq_created) {
+		status = be_cq_destroy(&pnob->rx_cq_obj);
+		pnob->rx_cq_created = 0;
+	}
+
+	if (pnob->mcc_q_created) {
+		status = be_mcc_ring_destroy(&pnob->mcc_q_obj);
+		pnob->mcc_q_created = 0;
+	}
+	if (pnob->mcc_cq_created) {
+		status = be_cq_destroy(&pnob->mcc_cq_obj);
+		pnob->mcc_cq_created = 0;
+	}
+
+	if (pnob->event_q_created) {
+		status = be_eq_destroy(&pnob->event_q_obj);
+		pnob->event_q_created = 0;
+	}
+	be_function_cleanup(&pnob->fn_obj);
+}
+
+/*
+ * free all resources associated with a pnob
+ * Called at the time of module cleanup as well a any error during
+ * module init.  Some resources may be partially allocated in a NetObj.
+ */
+static void netobject_cleanup(struct be_adapter *adapter,
+			struct be_net_object *pnob)
+{
+	struct net_device *netdev = adapter->netdevp;
+
+	if (netif_running(netdev)) {
+		netif_stop_queue(netdev);
+		be_wait_nic_tx_cmplx_cmpl(pnob);
+		be_disable_eq_intr(pnob);
+	}
+
+	be_unregister_isr(adapter);
+
+	if (adapter->tasklet_started) {
+		tasklet_kill(&(adapter->sts_handler));
+		adapter->tasklet_started = 0;
+	}
+	if (pnob->fn_obj_created)
+		be_disable_intr(pnob);
+
+	if (adapter->dev_state != BE_DEV_STATE_NONE)
+		unregister_netdev(netdev);
+
+	if (pnob->fn_obj_created)
+		be_destroy_netobj(pnob);
+
+	adapter->net_obj = NULL;
+	adapter->netdevp = NULL;
+
+	be_rx_q_clean(pnob);
+	if (pnob->rx_ctxt) {
+		kfree(pnob->rx_page_info);
+		kfree(pnob->rx_ctxt);
+	}
+
+	be_tx_q_clean(pnob);
+	kfree(pnob->tx_ctxt);
+
+	if (pnob->mcc_q)
+		pci_free_consistent(adapter->pdev, pnob->mcc_q_size,
+			pnob->mcc_q, pnob->mcc_q_bus);
+
+	if (pnob->mcc_wrb_ctxt)
+		free_pages((unsigned long)pnob->mcc_wrb_ctxt,
+			   get_order(pnob->mcc_wrb_ctxt_size));
+
+	if (pnob->mcc_cq)
+		pci_free_consistent(adapter->pdev, pnob->mcc_cq_size,
+			pnob->mcc_cq, pnob->mcc_cq_bus);
+
+	if (pnob->event_q)
+		pci_free_consistent(adapter->pdev, pnob->event_q_size,
+			pnob->event_q, pnob->event_q_bus);
+
+	if (pnob->tx_cq)
+		pci_free_consistent(adapter->pdev, pnob->tx_cq_size,
+			pnob->tx_cq, pnob->tx_cq_bus);
+
+	if (pnob->tx_q)
+		pci_free_consistent(adapter->pdev, pnob->tx_q_size,
+			pnob->tx_q, pnob->tx_q_bus);
+
+	if (pnob->rx_q)
+		pci_free_consistent(adapter->pdev, pnob->rx_q_size,
+			pnob->rx_q, pnob->rx_q_bus);
+
+	if (pnob->rx_cq)
+		pci_free_consistent(adapter->pdev, pnob->rx_cq_size,
+			pnob->rx_cq, pnob->rx_cq_bus);
+
+
+	if (pnob->mb_ptr)
+		pci_free_consistent(adapter->pdev, pnob->mb_size, pnob->mb_ptr,
+			pnob->mb_bus);
+
+	free_netdev(netdev);
+}
+
+
+static int be_nob_ring_alloc(struct be_adapter *adapter,
+	struct be_net_object *pnob)
+{
+	u32 size;
+
+	/* Mail box rd; mailbox pointer needs to be 16 byte aligned */
+	pnob->mb_size = sizeof(struct MCC_MAILBOX_AMAP) + 16;
+	pnob->mb_ptr = pci_alloc_consistent(adapter->pdev, pnob->mb_size,
+				&pnob->mb_bus);
+	if (!pnob->mb_bus)
+		return -1;
+	memset(pnob->mb_ptr, 0, pnob->mb_size);
+	pnob->mb_rd.va = PTR_ALIGN(pnob->mb_ptr, 16);
+	pnob->mb_rd.pa = PTR_ALIGN(pnob->mb_bus, 16);
+	pnob->mb_rd.length = sizeof(struct MCC_MAILBOX_AMAP);
+	/*
+	 * Event queue
+	 */
+	pnob->event_q_len = EVENT_Q_LEN;
+	pnob->event_q_size = pnob->event_q_len * sizeof(struct EQ_ENTRY_AMAP);
+	pnob->event_q = pci_alloc_consistent(adapter->pdev, pnob->event_q_size,
+				&pnob->event_q_bus);
+	if (!pnob->event_q_bus)
+		return -1;
+	memset(pnob->event_q, 0, pnob->event_q_size);
+	/*
+	 * Eth TX queue
+	 */
+	pnob->tx_q_len = ETH_TXQ_LEN;
+	pnob->tx_q_port = 0;
+	pnob->tx_q_size =  pnob->tx_q_len * sizeof(struct ETH_WRB_AMAP);
+	pnob->tx_q = pci_alloc_consistent(adapter->pdev, pnob->tx_q_size,
+				&pnob->tx_q_bus);
+	if (!pnob->tx_q_bus)
+		return -1;
+	memset(pnob->tx_q, 0, pnob->tx_q_size);
+	/*
+	 * Eth TX Compl queue
+	 */
+	pnob->txcq_len = ETH_TXCQ_LEN;
+	pnob->tx_cq_size = pnob->txcq_len * sizeof(struct ETH_TX_COMPL_AMAP);
+	pnob->tx_cq = pci_alloc_consistent(adapter->pdev, pnob->tx_cq_size,
+				&pnob->tx_cq_bus);
+	if (!pnob->tx_cq_bus)
+		return -1;
+	memset(pnob->tx_cq, 0, pnob->tx_cq_size);
+	/*
+	 * Eth RX queue
+	 */
+	pnob->rx_q_len = ETH_RXQ_LEN;
+	pnob->rx_q_size =  pnob->rx_q_len * sizeof(struct ETH_RX_D_AMAP);
+	pnob->rx_q = pci_alloc_consistent(adapter->pdev, pnob->rx_q_size,
+				&pnob->rx_q_bus);
+	if (!pnob->rx_q_bus)
+		return -1;
+	memset(pnob->rx_q, 0, pnob->rx_q_size);
+	/*
+	 * Eth Unicast RX Compl queue
+	 */
+	pnob->rx_cq_len = ETH_UC_RXCQ_LEN;
+	pnob->rx_cq_size =  pnob->rx_cq_len *
+			sizeof(struct ETH_RX_COMPL_AMAP);
+	pnob->rx_cq = pci_alloc_consistent(adapter->pdev, pnob->rx_cq_size,
+				&pnob->rx_cq_bus);
+	if (!pnob->rx_cq_bus)
+		return -1;
+	memset(pnob->rx_cq, 0, pnob->rx_cq_size);
+
+	/* TX resources */
+	size = pnob->tx_q_len * sizeof(void **);
+	pnob->tx_ctxt = kzalloc(size, GFP_KERNEL);
+	if (pnob->tx_ctxt == NULL)
+		return -1;
+
+	/* RX resources */
+	size = pnob->rx_q_len * sizeof(void *);
+	pnob->rx_ctxt = kzalloc(size, GFP_KERNEL);
+	if (pnob->rx_ctxt == NULL)
+		return -1;
+
+	size = (pnob->rx_q_len * sizeof(struct be_rx_page_info));
+	pnob->rx_page_info = kzalloc(size, GFP_KERNEL);
+	if (pnob->rx_page_info == NULL)
+		return -1;
+
+	adapter->eth_statsp = kzalloc(sizeof(struct FWCMD_ETH_GET_STATISTICS),
+				GFP_KERNEL);
+	if (adapter->eth_statsp == NULL)
+		return -1;
+	pnob->rx_buf_size = rxbuf_size;
+	return 0;
+}
+
+/*
+    This function initializes the be_net_object for subsequent
+    network operations.
+
+    Before calling this function, the driver  must have allocated
+    space for the NetObject structure, initialized the structure,
+    allocated DMAable memory for all the network queues that form
+    part of the NetObject and populated the start address (virtual)
+    and number of entries allocated for each queue in the NetObject structure.
+
+    The driver must also have allocated memory to hold the
+    mailbox structure (MCC_MAILBOX) and post the physical address,
+    virtual addresses and the size of the mailbox memory in the
+    NetObj.mb_rd.  This structure is used by BECLIB for
+    initial communication with the embedded MCC processor. BECLIB
+    uses the mailbox until MCC rings are created for  more  efficient
+    communication with the MCC processor.
+
+    If the driver wants to create multiple network interface for more
+    than one protection domain, it can call be_create_netobj()
+    multiple times  once for each protection domain.  A Maximum of
+    32 protection domains are supported.
+
+*/
+static int
+be_create_netobj(struct be_net_object *pnob, u8 __iomem *csr_va,
+	u8 __iomem *db_va, u8 __iomem *pci_va)
+{
+	int status = 0;
+	bool  eventable = false, tx_no_delay = false, rx_no_delay = false;
+	struct be_eq_object *eq_objectp = NULL;
+	struct be_function_object *pfob = &pnob->fn_obj;
+	struct ring_desc rd;
+	u32 set_rxbuf_size;
+	u32 tx_cmpl_wm = CEV_WMARK_96;	/* 0xffffffff to disable */
+	u32 rx_cmpl_wm = CEV_WMARK_160;	/* 0xffffffff to disable */
+	u32 eq_delay = 0; /* delay in 8usec units. 0xffffffff to disable */
+
+	memset(&rd, 0, sizeof(struct ring_desc));
+
+	status = be_function_object_create(csr_va, db_va, pci_va,
+			BE_FUNCTION_TYPE_NETWORK, &pnob->mb_rd, pfob);
+	if (status != BE_SUCCESS)
+		return status;
+	pnob->fn_obj_created = true;
+
+	if (tx_cmpl_wm == 0xffffffff)
+		tx_no_delay = true;
+	if (rx_cmpl_wm == 0xffffffff)
+		rx_no_delay = true;
+	/*
+	 * now create the necessary rings
+	 * Event Queue first.
+	 */
+	if (pnob->event_q_len) {
+		rd.va = pnob->event_q;
+		rd.pa = pnob->event_q_bus;
+		rd.length = pnob->event_q_size;
+
+		status = be_eq_create(pfob, &rd, 4, pnob->event_q_len,
+				(u32) -1,	/* CEV_WMARK_* or -1 */
+				eq_delay,	/* in 8us units, or -1 */
+				&pnob->event_q_obj);
+		if (status != BE_SUCCESS)
+			goto error_ret;
+		pnob->event_q_id = pnob->event_q_obj.eq_id;
+		pnob->event_q_created = 1;
+		eventable = true;
+		eq_objectp = &pnob->event_q_obj;
+	}
+	/*
+	 * Now Eth Tx Compl. queue.
+	 */
+	if (pnob->txcq_len) {
+		rd.va = pnob->tx_cq;
+		rd.pa = pnob->tx_cq_bus;
+		rd.length = pnob->tx_cq_size;
+
+		status = be_cq_create(pfob, &rd,
+			pnob->txcq_len * sizeof(struct ETH_TX_COMPL_AMAP),
+			false,	/* solicted events,  */
+			tx_no_delay,	/* nodelay  */
+			tx_cmpl_wm,	/* Watermark encodings */
+			eq_objectp, &pnob->tx_cq_obj);
+		if (status != BE_SUCCESS)
+			goto error_ret;
+
+		pnob->tx_cq_id = pnob->tx_cq_obj.cq_id;
+		pnob->tx_cq_created = 1;
+	}
+	/*
+	 * Eth Tx queue
+	 */
+	if (pnob->tx_q_len) {
+		struct be_eth_sq_parameters ex_params = { 0 };
+		u32 type;
+
+		if (pnob->tx_q_port) {
+			/* TXQ to be bound to a specific port */
+			type = BE_ETH_TX_RING_TYPE_BOUND;
+			ex_params.port = pnob->tx_q_port - 1;
+		} else
+			type = BE_ETH_TX_RING_TYPE_STANDARD;
+
+		rd.va = pnob->tx_q;
+		rd.pa = pnob->tx_q_bus;
+		rd.length = pnob->tx_q_size;
+
+		status = be_eth_sq_create_ex(pfob, &rd,
+				pnob->tx_q_len * sizeof(struct ETH_WRB_AMAP),
+				type, 2, &pnob->tx_cq_obj,
+				&ex_params, &pnob->tx_q_obj);
+
+		if (status != BE_SUCCESS)
+			goto error_ret;
+
+		pnob->tx_q_id = pnob->tx_q_obj.bid;
+		pnob->tx_q_created = 1;
+	}
+	/*
+	 * Now Eth Rx compl. queue.  Always needed.
+	 */
+	rd.va = pnob->rx_cq;
+	rd.pa = pnob->rx_cq_bus;
+	rd.length = pnob->rx_cq_size;
+
+	status = be_cq_create(pfob, &rd,
+			pnob->rx_cq_len * sizeof(struct ETH_RX_COMPL_AMAP),
+			false,	/* solicted events,  */
+			rx_no_delay,	/* nodelay  */
+			rx_cmpl_wm,	/* Watermark encodings */
+			eq_objectp, &pnob->rx_cq_obj);
+	if (status != BE_SUCCESS)
+		goto error_ret;
+
+	pnob->rx_cq_id = pnob->rx_cq_obj.cq_id;
+	pnob->rx_cq_created = 1;
+
+	status = be_eth_rq_set_frag_size(pfob, pnob->rx_buf_size,
+			(u32 *) &set_rxbuf_size);
+	if (status != BE_SUCCESS) {
+		be_eth_rq_get_frag_size(pfob, (u32 *) &pnob->rx_buf_size);
+		if ((pnob->rx_buf_size != 2048) && (pnob->rx_buf_size != 4096)
+		    && (pnob->rx_buf_size != 8192))
+			goto error_ret;
+	} else {
+		if (pnob->rx_buf_size != set_rxbuf_size)
+			pnob->rx_buf_size = set_rxbuf_size;
+	}
+	/*
+	 * Eth RX queue. be_eth_rq_create() always assumes 2 pages size
+	 */
+	rd.va = pnob->rx_q;
+	rd.pa = pnob->rx_q_bus;
+	rd.length = pnob->rx_q_size;
+
+	status = be_eth_rq_create(pfob, &rd, &pnob->rx_cq_obj,
+			     &pnob->rx_cq_obj, &pnob->rx_q_obj);
+
+	if (status != BE_SUCCESS)
+		goto error_ret;
+
+	pnob->rx_q_id = pnob->rx_q_obj.rid;
+	pnob->rx_q_created = 1;
+
+	return BE_SUCCESS;	/* All required queues created. */
+
+error_ret:
+	be_destroy_netobj(pnob);
+	return status;
+}
+
+static int be_nob_ring_init(struct be_adapter *adapter,
+				struct be_net_object *pnob)
+{
+	int status;
+
+	pnob->event_q_tl = 0;
+
+	pnob->tx_q_hd = 0;
+	pnob->tx_q_tl = 0;
+
+	pnob->tx_cq_tl = 0;
+
+	pnob->rx_cq_tl = 0;
+
+	memset(pnob->event_q, 0, pnob->event_q_size);
+	memset(pnob->tx_cq, 0, pnob->tx_cq_size);
+	memset(pnob->tx_ctxt, 0, pnob->tx_q_len * sizeof(void **));
+	memset(pnob->rx_ctxt, 0, pnob->rx_q_len * sizeof(void *));
+	pnob->rx_pg_info_hd = 0;
+	pnob->rx_q_hd = 0;
+	atomic_set(&pnob->rx_q_posted, 0);
+
+	status = be_create_netobj(pnob, adapter->csr_va, adapter->db_va,
+				adapter->pci_va);
+	if (status != BE_SUCCESS)
+		return -1;
+
+	be_post_eth_rx_buffs(pnob);
+	return 0;
+}
+
+/* This function handles async callback for link status */
+static void
+be_link_status_async_callback(void *context, u32 event_code, void *event)
+{
+	struct ASYNC_EVENT_LINK_STATE_AMAP *link_status = event;
+	struct be_adapter *adapter = context;
+	bool link_enable = false;
+	struct be_net_object *pnob;
+	struct ASYNC_EVENT_TRAILER_AMAP *async_trailer;
+	struct net_device *netdev;
+	u32 async_event_code, async_event_type, active_port;
+	u32 port0_link_status, port1_link_status, port0_duplex, port1_duplex;
+	u32 port0_speed, port1_speed;
+
+	if (event_code != ASYNC_EVENT_CODE_LINK_STATE) {
+		/* Not our event to handle */
+		return;
+	}
+	async_trailer = (struct ASYNC_EVENT_TRAILER_AMAP *)
+	    ((u8 *) event + sizeof(struct MCC_CQ_ENTRY_AMAP) -
+	     sizeof(struct ASYNC_EVENT_TRAILER_AMAP));
+
+	async_event_code = AMAP_GET_BITS_PTR(ASYNC_EVENT_TRAILER, event_code,
+					     async_trailer);
+	BUG_ON(async_event_code != ASYNC_EVENT_CODE_LINK_STATE);
+
+	pnob = adapter->net_obj;
+	netdev = pnob->netdev;
+
+	/* Determine if this event is a switch VLD or a physical link event */
+	async_event_type = AMAP_GET_BITS_PTR(ASYNC_EVENT_TRAILER, event_type,
+					     async_trailer);
+	active_port = AMAP_GET_BITS_PTR(ASYNC_EVENT_LINK_STATE,
+					active_port, link_status);
+	port0_link_status = AMAP_GET_BITS_PTR(ASYNC_EVENT_LINK_STATE,
+					      port0_link_status, link_status);
+	port1_link_status = AMAP_GET_BITS_PTR(ASYNC_EVENT_LINK_STATE,
+					      port1_link_status, link_status);
+	port0_duplex = AMAP_GET_BITS_PTR(ASYNC_EVENT_LINK_STATE,
+					 port0_duplex, link_status);
+	port1_duplex = AMAP_GET_BITS_PTR(ASYNC_EVENT_LINK_STATE,
+					 port1_duplex, link_status);
+	port0_speed = AMAP_GET_BITS_PTR(ASYNC_EVENT_LINK_STATE,
+					port0_speed, link_status);
+	port1_speed = AMAP_GET_BITS_PTR(ASYNC_EVENT_LINK_STATE,
+					port1_speed, link_status);
+	if (async_event_type == NTWK_LINK_TYPE_VIRTUAL) {
+		adapter->be_stat.bes_link_change_virtual++;
+		if (adapter->be_link_sts->active_port != active_port) {
+			dev_notice(&netdev->dev,
+			       "Active port changed due to VLD on switch\n");
+		} else {
+			dev_notice(&netdev->dev, "Link status update\n");
+		}
+
+	} else {
+		adapter->be_stat.bes_link_change_physical++;
+		if (adapter->be_link_sts->active_port != active_port) {
+			dev_notice(&netdev->dev,
+			       "Active port changed due to port link"
+			       " status change\n");
+		} else {
+			dev_notice(&netdev->dev, "Link status update\n");
+		}
+	}
+
+	memset(adapter->be_link_sts, 0, sizeof(adapter->be_link_sts));
+
+	if ((port0_link_status == ASYNC_EVENT_LINK_UP) ||
+	    (port1_link_status == ASYNC_EVENT_LINK_UP)) {
+		if ((adapter->port0_link_sts == BE_PORT_LINK_DOWN) &&
+		    (adapter->port1_link_sts == BE_PORT_LINK_DOWN)) {
+			/* Earlier both the ports are down So link is up */
+			link_enable = true;
+		}
+
+		if (port0_link_status == ASYNC_EVENT_LINK_UP) {
+			adapter->port0_link_sts = BE_PORT_LINK_UP;
+			adapter->be_link_sts->mac0_duplex = port0_duplex;
+			adapter->be_link_sts->mac0_speed = port0_speed;
+			if (active_port == NTWK_PORT_A)
+				adapter->be_link_sts->active_port = 0;
+		} else
+			adapter->port0_link_sts = BE_PORT_LINK_DOWN;
+
+		if (port1_link_status == ASYNC_EVENT_LINK_UP) {
+			adapter->port1_link_sts = BE_PORT_LINK_UP;
+			adapter->be_link_sts->mac1_duplex = port1_duplex;
+			adapter->be_link_sts->mac1_speed = port1_speed;
+			if (active_port == NTWK_PORT_B)
+				adapter->be_link_sts->active_port = 1;
+		} else
+			adapter->port1_link_sts = BE_PORT_LINK_DOWN;
+
+		printk(KERN_INFO "Link Properties for %s:\n", netdev->name);
+		dev_info(&netdev->dev, "Link Properties:\n");
+		be_print_link_info(adapter->be_link_sts);
+
+		if (!link_enable)
+			return;
+		/*
+		 * Both ports were down previously, but atleast one of
+		 * them has come up if this netdevice's carrier is not up,
+		 * then indicate to stack
+		 */
+		if (!netif_carrier_ok(netdev)) {
+			netif_start_queue(netdev);
+			netif_carrier_on(netdev);
+		}
+		return;
+	}
+
+	/* Now both the ports are down. Tell the stack about it */
+	dev_info(&netdev->dev, "Both ports are down\n");
+	adapter->port0_link_sts = BE_PORT_LINK_DOWN;
+	adapter->port1_link_sts = BE_PORT_LINK_DOWN;
+	if (netif_carrier_ok(netdev)) {
+		netif_carrier_off(netdev);
+		netif_stop_queue(netdev);
+	}
+	return;
+}
+
+static int be_mcc_create(struct be_adapter *adapter)
+{
+	struct be_net_object *pnob;
+
+	pnob = adapter->net_obj;
+	/*
+	 * Create the MCC ring so that all further communication with
+	 * MCC can go thru the ring. we do this at the end since
+	 * we do not want to be dealing with interrupts until the
+	 * initialization is complete.
+	 */
+	pnob->mcc_q_len = MCC_Q_LEN;
+	pnob->mcc_q_size = pnob->mcc_q_len * sizeof(struct MCC_WRB_AMAP);
+	pnob->mcc_q =  pci_alloc_consistent(adapter->pdev, pnob->mcc_q_size,
+				&pnob->mcc_q_bus);
+	if (!pnob->mcc_q_bus)
+		return -1;
+	/*
+	 * space for MCC WRB context
+	 */
+	pnob->mcc_wrb_ctxtLen = MCC_Q_LEN;
+	pnob->mcc_wrb_ctxt_size =  pnob->mcc_wrb_ctxtLen *
+		sizeof(struct be_mcc_wrb_context);
+	pnob->mcc_wrb_ctxt = (void *)__get_free_pages(GFP_KERNEL,
+		get_order(pnob->mcc_wrb_ctxt_size));
+	if (pnob->mcc_wrb_ctxt == NULL)
+		return -1;
+	/*
+	 * Space for MCC compl. ring
+	 */
+	pnob->mcc_cq_len = MCC_CQ_LEN;
+	pnob->mcc_cq_size = pnob->mcc_cq_len * sizeof(struct MCC_CQ_ENTRY_AMAP);
+	pnob->mcc_cq = pci_alloc_consistent(adapter->pdev, pnob->mcc_cq_size,
+				&pnob->mcc_cq_bus);
+	if (!pnob->mcc_cq_bus)
+		return -1;
+	return 0;
+}
+
+/*
+    This function creates the MCC request and completion ring required
+    for communicating with the ARM processor.  The caller must have
+    allocated required amount of memory for the MCC ring and MCC
+    completion ring and posted the virtual address and number of
+    entries in the corresponding members (mcc_q and mcc_cq) in the
+    NetObject struture.
+
+    When this call is completed, all further communication with
+    ARM will switch from mailbox to this ring.
+
+    pnob	- Pointer to the NetObject structure. This NetObject should
+		  have been created using a previous call to be_create_netobj()
+*/
+int be_create_mcc_rings(struct be_net_object *pnob)
+{
+	int status = 0;
+	struct ring_desc rd;
+	struct be_function_object *pfob = &pnob->fn_obj;
+
+	memset(&rd, 0, sizeof(struct ring_desc));
+	if (pnob->mcc_cq_len) {
+		rd.va = pnob->mcc_cq;
+		rd.pa = pnob->mcc_cq_bus;
+		rd.length = pnob->mcc_cq_size;
+
+		status = be_cq_create(pfob, &rd,
+			pnob->mcc_cq_len * sizeof(struct MCC_CQ_ENTRY_AMAP),
+			false,	/* solicted events,  */
+			true,	/* nodelay  */
+			0,	/* 0 Watermark since Nodelay is true */
+			&pnob->event_q_obj,
+			&pnob->mcc_cq_obj);
+
+		if (status != BE_SUCCESS)
+			return status;
+
+		pnob->mcc_cq_id = pnob->mcc_cq_obj.cq_id;
+		pnob->mcc_cq_created = 1;
+	}
+	if (pnob->mcc_q_len) {
+		rd.va = pnob->mcc_q;
+		rd.pa = pnob->mcc_q_bus;
+		rd.length = pnob->mcc_q_size;
+
+		status = be_mcc_ring_create(pfob, &rd,
+				pnob->mcc_q_len * sizeof(struct MCC_WRB_AMAP),
+				pnob->mcc_wrb_ctxt, pnob->mcc_wrb_ctxtLen,
+				&pnob->mcc_cq_obj, &pnob->mcc_q_obj);
+
+		if (status != BE_SUCCESS)
+			return status;
+
+		pnob->mcc_q_created = 1;
+	}
+	return BE_SUCCESS;
+}
+
+static int be_mcc_init(struct be_adapter *adapter)
+{
+	u32 r;
+	struct be_net_object *pnob;
+
+	pnob = adapter->net_obj;
+	memset(pnob->mcc_q, 0, pnob->mcc_q_size);
+	pnob->mcc_q_hd = 0;
+
+	memset(pnob->mcc_wrb_ctxt, 0, pnob->mcc_wrb_ctxt_size);
+
+	memset(pnob->mcc_cq, 0, pnob->mcc_cq_size);
+	pnob->mcc_cq_tl = 0;
+
+	r = be_create_mcc_rings(adapter->net_obj);
+	if (r != BE_SUCCESS)
+		return -1;
+
+	return 0;
+}
+
+static void be_remove(struct pci_dev *pdev)
+{
+	struct be_net_object *pnob;
+	struct be_adapter *adapter;
+
+	adapter = pci_get_drvdata(pdev);
+	if (!adapter)
+		return;
+
+	pci_set_drvdata(pdev, NULL);
+	pnob = (struct be_net_object *)adapter->net_obj;
+
+	flush_scheduled_work();
+
+	if (pnob) {
+		/* Unregister async callback function for link status updates */
+		if (pnob->mcc_q_created)
+			be_mcc_add_async_event_callback(&pnob->mcc_q_obj,
+								NULL, NULL);
+		netobject_cleanup(adapter, pnob);
+	}
+
+	if (adapter->csr_va)
+		iounmap(adapter->csr_va);
+	if (adapter->db_va)
+		iounmap(adapter->db_va);
+	if (adapter->pci_va)
+		iounmap(adapter->pci_va);
+
+	pci_release_regions(adapter->pdev);
+	pci_disable_device(adapter->pdev);
+
+	kfree(adapter->be_link_sts);
+	kfree(adapter->eth_statsp);
+
+	if (adapter->timer_ctxt.get_stats_timer.function)
+		del_timer_sync(&adapter->timer_ctxt.get_stats_timer);
+	kfree(adapter);
+}
+
+/*
+ * This function is called by the PCI sub-system when it finds a PCI
+ * device with dev/vendor IDs that match with one of our devices.
+ * All of the driver initialization is done in this function.
+ */
+static int be_probe(struct pci_dev *pdev, const struct pci_device_id *pdev_id)
+{
+	int status = 0;
+	struct be_adapter *adapter;
+	struct FWCMD_COMMON_GET_FW_VERSION_RESPONSE_PAYLOAD get_fwv;
+	struct be_net_object *pnob;
+	struct net_device *netdev;
+
+	status = pci_enable_device(pdev);
+	if (status)
+		goto error;
+
+	status = pci_request_regions(pdev, be_driver_name);
+	if (status)
+		goto error_pci_req;
+
+	pci_set_master(pdev);
+	adapter = kzalloc(sizeof(struct be_adapter), GFP_KERNEL);
+	if (adapter == NULL) {
+		status = -ENOMEM;
+		goto error_adapter;
+	}
+	adapter->dev_state = BE_DEV_STATE_NONE;
+	adapter->pdev = pdev;
+	pci_set_drvdata(pdev, adapter);
+
+	adapter->enable_aic = 1;
+	adapter->max_eqd = MAX_EQD;
+	adapter->min_eqd = 0;
+	adapter->cur_eqd = 0;
+
+	status = pci_set_dma_mask(pdev, DMA_64BIT_MASK);
+	if (!status) {
+		adapter->dma_64bit_cap = true;
+	} else {
+		adapter->dma_64bit_cap = false;
+		status = pci_set_dma_mask(pdev, DMA_32BIT_MASK);
+		if (status != 0) {
+			printk(KERN_ERR "Could not set PCI DMA Mask\n");
+			goto cleanup;
+		}
+	}
+
+	status = init_pci_be_function(adapter, pdev);
+	if (status != 0) {
+		printk(KERN_ERR "Failed to map PCI BARS\n");
+		status = -ENOMEM;
+		goto cleanup;
+	}
+
+	be_trace_set_level(DL_ALWAYS | DL_ERR);
+
+	adapter->be_link_sts = kmalloc(sizeof(struct BE_LINK_STATUS),
+					GFP_KERNEL);
+	if (adapter->be_link_sts == NULL) {
+		printk(KERN_ERR "Memory allocation for link status "
+		       "buffer failed\n");
+		goto cleanup;
+	}
+	spin_lock_init(&adapter->txq_lock);
+
+	netdev = alloc_etherdev(sizeof(struct be_net_object));
+	if (netdev == NULL) {
+		status = -ENOMEM;
+		goto cleanup;
+	}
+	pnob = netdev_priv(netdev);
+	adapter->net_obj = pnob;
+	adapter->netdevp = netdev;
+	pnob->adapter = adapter;
+	pnob->netdev = netdev;
+
+	status = be_nob_ring_alloc(adapter, pnob);
+	if (status != 0)
+		goto cleanup;
+
+	status = be_nob_ring_init(adapter, pnob);
+	if (status != 0)
+		goto cleanup;
+
+	be_rxf_mac_address_read_write(&pnob->fn_obj, false, false, false,
+		false, false, netdev->dev_addr, NULL, NULL);
+
+	netdev->init = &benet_init;
+	netif_carrier_off(netdev);
+	netif_stop_queue(netdev);
+
+	SET_NETDEV_DEV(netdev, &(adapter->pdev->dev));
+
+	netif_napi_add(netdev, &pnob->napi, be_poll, 64);
+
+	/* if the rx_frag size if 2K, one page is shared as two RX frags */
+	pnob->rx_pg_shared =
+		(pnob->rx_buf_size <= PAGE_SIZE / 2) ? true : false;
+	if (pnob->rx_buf_size != rxbuf_size) {
+		printk(KERN_WARNING
+		       "Could not set Rx buffer size to %d. Using %d\n",
+				       rxbuf_size, pnob->rx_buf_size);
+		rxbuf_size = pnob->rx_buf_size;
+	}
+
+	tasklet_init(&(adapter->sts_handler), be_process_intr,
+		     (unsigned long)adapter);
+	adapter->tasklet_started = 1;
+	spin_lock_init(&(adapter->int_lock));
+
+	status = be_register_isr(adapter, pnob);
+	if (status != 0)
+		goto cleanup;
+
+	adapter->rx_csum = 1;
+	adapter->max_rx_coal = BE_LRO_MAX_PKTS;
+
+	memset(&get_fwv, 0,
+	       sizeof(struct FWCMD_COMMON_GET_FW_VERSION_RESPONSE_PAYLOAD));
+	printk(KERN_INFO "BladeEngine Driver version:%s. "
+	       "Copyright ServerEngines, Corporation 2005 - 2008\n",
+			       be_drvr_ver);
+	status = be_function_get_fw_version(&pnob->fn_obj, &get_fwv, NULL,
+					    NULL);
+	if (status == BE_SUCCESS) {
+		strncpy(be_fw_ver, get_fwv.firmware_version_string, 32);
+		printk(KERN_INFO "BladeEngine Firmware Version:%s\n",
+		       get_fwv.firmware_version_string);
+	} else {
+		printk(KERN_WARNING "Unable to get BE Firmware Version\n");
+	}
+
+	sema_init(&adapter->get_eth_stat_sem, 0);
+	init_timer(&adapter->timer_ctxt.get_stats_timer);
+	atomic_set(&adapter->timer_ctxt.get_stat_flag, 0);
+	adapter->timer_ctxt.get_stats_timer.function =
+	    &be_get_stats_timer_handler;
+
+	status = be_mcc_create(adapter);
+	if (status < 0)
+		goto cleanup;
+	status = be_mcc_init(adapter);
+	if (status < 0)
+		goto cleanup;
+
+
+	status = be_mcc_add_async_event_callback(&adapter->net_obj->mcc_q_obj,
+			 be_link_status_async_callback, (void *)adapter);
+	if (status != BE_SUCCESS) {
+		printk(KERN_WARNING "add_async_event_callback failed");
+		printk(KERN_WARNING
+		       "Link status changes may not be reflected\n");
+	}
+
+	status = register_netdev(netdev);
+	if (status != 0)
+		goto cleanup;
+	be_update_link_status(adapter);
+	adapter->dev_state = BE_DEV_STATE_INIT;
+	return 0;
+
+cleanup:
+	be_remove(pdev);
+	return status;
+error_adapter:
+	pci_release_regions(pdev);
+error_pci_req:
+	pci_disable_device(pdev);
+error:
+	printk(KERN_ERR "BladeEngine initalization failed\n");
+	return status;
+}
+
+/*
+ * Get the current link status and print the status on console
+ */
+void be_update_link_status(struct be_adapter *adapter)
+{
+	int status;
+	struct be_net_object *pnob = adapter->net_obj;
+
+	status = be_rxf_link_status(&pnob->fn_obj, adapter->be_link_sts, NULL,
+			NULL, NULL);
+	if (status == BE_SUCCESS) {
+		if (adapter->be_link_sts->mac0_speed &&
+		    adapter->be_link_sts->mac0_duplex)
+			adapter->port0_link_sts = BE_PORT_LINK_UP;
+		else
+			adapter->port0_link_sts = BE_PORT_LINK_DOWN;
+
+		if (adapter->be_link_sts->mac1_speed &&
+		    adapter->be_link_sts->mac1_duplex)
+			adapter->port1_link_sts = BE_PORT_LINK_UP;
+		else
+			adapter->port1_link_sts = BE_PORT_LINK_DOWN;
+
+		dev_info(&pnob->netdev->dev, "Link Properties:\n");
+		be_print_link_info(adapter->be_link_sts);
+		return;
+	}
+	dev_info(&pnob->netdev->dev, "Could not get link status\n");
+	return;
+}
+
+
+#ifdef CONFIG_PM
+static void
+be_pm_cleanup(struct be_adapter *adapter,
+	      struct be_net_object *pnob, struct net_device *netdev)
+{
+	netif_carrier_off(netdev);
+	netif_stop_queue(netdev);
+
+	be_wait_nic_tx_cmplx_cmpl(pnob);
+	be_disable_eq_intr(pnob);
+
+	if (adapter->tasklet_started) {
+		tasklet_kill(&adapter->sts_handler);
+		adapter->tasklet_started = 0;
+	}
+
+	be_unregister_isr(adapter);
+	be_disable_intr(pnob);
+
+	be_tx_q_clean(pnob);
+	be_rx_q_clean(pnob);
+
+	be_destroy_netobj(pnob);
+}
+
+static int be_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	struct be_adapter *adapter = pci_get_drvdata(pdev);
+	struct net_device *netdev =  adapter->netdevp;
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	adapter->dev_pm_state = adapter->dev_state;
+	adapter->dev_state = BE_DEV_STATE_SUSPEND;
+
+	netif_device_detach(netdev);
+	if (netif_running(netdev))
+		be_pm_cleanup(adapter, pnob, netdev);
+
+	pci_enable_wake(pdev, 3, 1);
+	pci_enable_wake(pdev, 4, 1);	/* D3 Cold = 4 */
+	pci_save_state(pdev);
+	pci_disable_device(pdev);
+	pci_set_power_state(pdev, pci_choose_state(pdev, state));
+	return 0;
+}
+
+static void be_up(struct be_adapter *adapter)
+{
+	struct be_net_object *pnob = adapter->net_obj;
+
+	if (pnob->num_vlans != 0)
+		be_rxf_vlan_config(&pnob->fn_obj, false, pnob->num_vlans,
+			pnob->vlan_tag, NULL, NULL, NULL);
+
+}
+
+static int be_resume(struct pci_dev *pdev)
+{
+	int status = 0;
+	struct be_adapter *adapter = pci_get_drvdata(pdev);
+	struct net_device *netdev =  adapter->netdevp;
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	netif_device_detach(netdev);
+
+	status = pci_enable_device(pdev);
+	if (status)
+		return status;
+
+	pci_set_power_state(pdev, 0);
+	pci_restore_state(pdev);
+	pci_enable_wake(pdev, 3, 0);
+	pci_enable_wake(pdev, 4, 0);	/* 4 is D3 cold */
+
+	netif_carrier_on(netdev);
+	netif_start_queue(netdev);
+
+	if (netif_running(netdev)) {
+		be_rxf_mac_address_read_write(&pnob->fn_obj, false, false,
+			false, true, false, netdev->dev_addr, NULL, NULL);
+
+		status = be_nob_ring_init(adapter, pnob);
+		if (status < 0)
+			return status;
+
+		tasklet_init(&(adapter->sts_handler), be_process_intr,
+			     (unsigned long)adapter);
+		adapter->tasklet_started = 1;
+
+		if (be_register_isr(adapter, pnob) != 0) {
+			printk(KERN_ERR "be_register_isr failed\n");
+			return status;
+		}
+
+
+		status = be_mcc_init(adapter);
+		if (status < 0) {
+			printk(KERN_ERR "be_mcc_init failed\n");
+			return status;
+		}
+		be_update_link_status(adapter);
+		/*
+		 * Register async call back function to handle link
+		 * status updates
+		 */
+		status = be_mcc_add_async_event_callback(
+				&adapter->net_obj->mcc_q_obj,
+				be_link_status_async_callback, (void *)adapter);
+		if (status != BE_SUCCESS) {
+			printk(KERN_WARNING "add_async_event_callback failed");
+			printk(KERN_WARNING
+			       "Link status changes may not be reflected\n");
+		}
+		be_enable_intr(pnob);
+		be_enable_eq_intr(pnob);
+		be_up(adapter);
+	}
+	netif_device_attach(netdev);
+	adapter->dev_state = adapter->dev_pm_state;
+	return 0;
+
+}
+
+#endif
+
+/* Wait until no more pending transmits  */
+void be_wait_nic_tx_cmplx_cmpl(struct be_net_object *pnob)
+{
+	int i;
+
+	/* Wait for 20us * 50000 (= 1s) and no more */
+	i = 0;
+	while ((pnob->tx_q_tl != pnob->tx_q_hd) && (i < 50000)) {
+		++i;
+		udelay(20);
+	}
+
+	/* Check for no more pending transmits */
+	if (i >= 50000) {
+		printk(KERN_WARNING
+		       "Did not receive completions for all TX requests\n");
+	}
+}
+
+static struct pci_driver be_driver = {
+	.name = be_driver_name,
+	.id_table = be_device_id_table,
+	.probe = be_probe,
+#ifdef CONFIG_PM
+	.suspend = be_suspend,
+	.resume = be_resume,
+#endif
+	.remove = be_remove
+};
+
+/*
+ * Module init entry point. Registers our our device and return.
+ * Our probe will be called if the device is found.
+ */
+static int __init be_init_module(void)
+{
+	int ret;
+
+	if (rxbuf_size != 8192 && rxbuf_size != 4096 && rxbuf_size != 2048) {
+		printk(KERN_WARNING
+		       "Unsupported receive buffer size (%d) requested\n",
+		       rxbuf_size);
+		printk(KERN_WARNING
+		       "Must be 2048, 4096 or 8192. Defaulting to 2048\n");
+		rxbuf_size = 2048;
+	}
+
+	ret = pci_register_driver(&be_driver);
+
+	return ret;
+}
+
+module_init(be_init_module);
+
+/*
+ * be_exit_module - Driver Exit Cleanup Routine
+ */
+static void __exit be_exit_module(void)
+{
+	pci_unregister_driver(&be_driver);
+}
+
+module_exit(be_exit_module);
diff --git a/drivers/staging/benet/be_int.c b/drivers/staging/benet/be_int.c
new file mode 100644
index 000000000000..cba95d09a8b6
--- /dev/null
+++ b/drivers/staging/benet/be_int.c
@@ -0,0 +1,863 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#include <linux/if_vlan.h>
+#include <linux/inet_lro.h>
+
+#include "benet.h"
+
+/* number of bytes of RX frame that are copied to skb->data */
+#define BE_HDR_LEN 64
+
+#define NETIF_RX(skb) netif_receive_skb(skb)
+#define VLAN_ACCEL_RX(skb, pnob, vt) \
+		vlan_hwaccel_rx(skb, pnob->vlan_grp, vt)
+
+/*
+    This function notifies BladeEngine of the number of completion
+    entries processed from the specified completion queue by writing
+    the number of popped entries to the door bell.
+
+    pnob	- Pointer to the NetObject structure
+    n		- Number of completion entries processed
+    cq_id	- Queue ID of the completion queue for which notification
+			is being done.
+    re_arm	- 1  - rearm the completion ring to generate an event.
+		- 0  - dont rearm the completion ring to generate an event
+*/
+void be_notify_cmpl(struct be_net_object *pnob, int n, int cq_id, int re_arm)
+{
+	struct CQ_DB_AMAP cqdb;
+
+	cqdb.dw[0] = 0;
+	AMAP_SET_BITS_PTR(CQ_DB, qid, &cqdb, cq_id);
+	AMAP_SET_BITS_PTR(CQ_DB, rearm, &cqdb, re_arm);
+	AMAP_SET_BITS_PTR(CQ_DB, num_popped, &cqdb, n);
+	PD_WRITE(&pnob->fn_obj, cq_db, cqdb.dw[0]);
+}
+
+/*
+ * adds additional receive frags indicated by BE starting from given
+ * frag index (fi) to specified skb's frag list
+ */
+static void
+add_skb_frags(struct be_net_object *pnob, struct sk_buff *skb,
+	      u32 nresid, u32 fi)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	u32 sk_frag_idx, n;
+	struct be_rx_page_info *rx_page_info;
+	u32 frag_sz = pnob->rx_buf_size;
+
+	sk_frag_idx = skb_shinfo(skb)->nr_frags;
+	while (nresid) {
+		index_inc(&fi, pnob->rx_q_len);
+
+		rx_page_info = (struct be_rx_page_info *)pnob->rx_ctxt[fi];
+		pnob->rx_ctxt[fi] = NULL;
+		if ((rx_page_info->page_offset) ||
+		    (pnob->rx_pg_shared == false)) {
+			pci_unmap_page(adapter->pdev,
+				       pci_unmap_addr(rx_page_info, bus),
+				       frag_sz, PCI_DMA_FROMDEVICE);
+		}
+
+		n = min(nresid, frag_sz);
+		skb_shinfo(skb)->frags[sk_frag_idx].page = rx_page_info->page;
+		skb_shinfo(skb)->frags[sk_frag_idx].page_offset
+		    = rx_page_info->page_offset;
+		skb_shinfo(skb)->frags[sk_frag_idx].size = n;
+
+		sk_frag_idx++;
+		skb->len += n;
+		skb->data_len += n;
+		skb_shinfo(skb)->nr_frags++;
+		nresid -= n;
+
+		memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+		atomic_dec(&pnob->rx_q_posted);
+	}
+}
+
+/*
+ * This function processes incoming nic packets over various Rx queues.
+ * This function takes the adapter, the current Rx status descriptor
+ * entry and the Rx completion queue ID as argument.
+ */
+static inline int process_nic_rx_completion(struct be_net_object *pnob,
+					    struct ETH_RX_COMPL_AMAP *rxcp)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	struct sk_buff *skb;
+	int udpcksm, tcpcksm;
+	int n;
+	u32 nresid, fi;
+	u32 frag_sz = pnob->rx_buf_size;
+	u8 *va;
+	struct be_rx_page_info *rx_page_info;
+	u32 numfrags, vtp, vtm, vlan_tag, pktsize;
+
+	fi = AMAP_GET_BITS_PTR(ETH_RX_COMPL, fragndx, rxcp);
+	BUG_ON(fi >= (int)pnob->rx_q_len);
+	BUG_ON(fi < 0);
+
+	rx_page_info = (struct be_rx_page_info *)pnob->rx_ctxt[fi];
+	BUG_ON(!rx_page_info->page);
+	pnob->rx_ctxt[fi] = NULL;
+
+	/*
+	 * If one page is used per fragment or if this is the second half of
+	 *  of the page, unmap the page here
+	 */
+	if ((rx_page_info->page_offset) || (pnob->rx_pg_shared == false)) {
+		pci_unmap_page(adapter->pdev,
+			       pci_unmap_addr(rx_page_info, bus), frag_sz,
+			       PCI_DMA_FROMDEVICE);
+	}
+
+	atomic_dec(&pnob->rx_q_posted);
+	udpcksm = AMAP_GET_BITS_PTR(ETH_RX_COMPL, udpcksm, rxcp);
+	tcpcksm = AMAP_GET_BITS_PTR(ETH_RX_COMPL, tcpcksm, rxcp);
+	pktsize = AMAP_GET_BITS_PTR(ETH_RX_COMPL, pktsize, rxcp);
+	/*
+	 * get rid of RX flush completions first.
+	 */
+	if ((tcpcksm) && (udpcksm) && (pktsize == 32)) {
+		put_page(rx_page_info->page);
+		memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+		return 0;
+	}
+	skb = netdev_alloc_skb(pnob->netdev, BE_HDR_LEN + NET_IP_ALIGN);
+	if (skb == NULL) {
+		dev_info(&pnob->netdev->dev, "alloc_skb() failed\n");
+		put_page(rx_page_info->page);
+		memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+		goto free_frags;
+	}
+	skb_reserve(skb, NET_IP_ALIGN);
+
+	skb->dev = pnob->netdev;
+
+	n = min(pktsize, frag_sz);
+
+	va = page_address(rx_page_info->page) + rx_page_info->page_offset;
+	prefetch(va);
+
+	skb->len = n;
+	skb->data_len = n;
+	if (n <= BE_HDR_LEN) {
+		memcpy(skb->data, va, n);
+		put_page(rx_page_info->page);
+		skb->data_len -= n;
+		skb->tail += n;
+	} else {
+
+		/* Setup the SKB with page buffer information */
+		skb_shinfo(skb)->frags[0].page = rx_page_info->page;
+		skb_shinfo(skb)->nr_frags++;
+
+		/* Copy the header into the skb_data */
+		memcpy(skb->data, va, BE_HDR_LEN);
+		skb_shinfo(skb)->frags[0].page_offset =
+		    rx_page_info->page_offset + BE_HDR_LEN;
+		skb_shinfo(skb)->frags[0].size = n - BE_HDR_LEN;
+		skb->data_len -= BE_HDR_LEN;
+		skb->tail += BE_HDR_LEN;
+	}
+	memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+	nresid = pktsize - n;
+
+	skb->protocol = eth_type_trans(skb, pnob->netdev);
+
+	if ((tcpcksm || udpcksm) && adapter->rx_csum)
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+	else
+		skb->ip_summed = CHECKSUM_NONE;
+	/*
+	 * if we have more bytes left, the frame has been
+	 * given to us in multiple fragments.  This happens
+	 * with Jumbo frames. Add the remaining fragments to
+	 * skb->frags[] array.
+	 */
+	if (nresid)
+		add_skb_frags(pnob, skb, nresid, fi);
+
+	/* update the the true size of the skb. */
+	skb->truesize = skb->len + sizeof(struct sk_buff);
+
+	/*
+	 * If a 802.3 frame or 802.2 LLC frame
+	 * (i.e) contains length field in MAC Hdr
+	 * and frame len is greater than 64 bytes
+	 */
+	if (((skb->protocol == ntohs(ETH_P_802_2)) ||
+	     (skb->protocol == ntohs(ETH_P_802_3)))
+	    && (pktsize > BE_HDR_LEN)) {
+		/*
+		 * If the length given in Mac Hdr is less than frame size
+		 * Erraneous frame, Drop it
+		 */
+		if ((ntohs(*(u16 *) (va + 12)) + ETH_HLEN) < pktsize) {
+			/* Increment Non Ether type II frames dropped */
+			adapter->be_stat.bes_802_3_dropped_frames++;
+
+			kfree_skb(skb);
+			return 0;
+		}
+		/*
+		 * else if the length given in Mac Hdr is greater than
+		 * frame size, should not be seeing this sort of frames
+		 * dump the pkt and pass to stack
+		 */
+		else if ((ntohs(*(u16 *) (va + 12)) + ETH_HLEN) > pktsize) {
+			/* Increment Non Ether type II frames malformed */
+			adapter->be_stat.bes_802_3_malformed_frames++;
+		}
+	}
+
+	vtp = AMAP_GET_BITS_PTR(ETH_RX_COMPL, vtp, rxcp);
+	vtm = AMAP_GET_BITS_PTR(ETH_RX_COMPL, vtm, rxcp);
+	if (vtp && vtm) {
+		/* Vlan tag present in pkt and BE found
+		 * that the tag matched an entry in VLAN table
+		 */
+		if (!pnob->vlan_grp || pnob->num_vlans == 0) {
+			/* But we have no VLANs configured.
+			 * This should never happen.  Drop the packet.
+			 */
+			dev_info(&pnob->netdev->dev,
+			       "BladeEngine: Unexpected vlan tagged packet\n");
+			kfree_skb(skb);
+			return 0;
+		}
+		/* pass the VLAN packet to stack */
+		vlan_tag = AMAP_GET_BITS_PTR(ETH_RX_COMPL, vlan_tag, rxcp);
+		VLAN_ACCEL_RX(skb, pnob, be16_to_cpu(vlan_tag));
+
+	} else {
+		NETIF_RX(skb);
+	}
+	return 0;
+
+free_frags:
+	/* free all frags associated with the current rxcp */
+	numfrags = AMAP_GET_BITS_PTR(ETH_RX_COMPL, numfrags, rxcp);
+	while (numfrags-- > 1) {
+		index_inc(&fi, pnob->rx_q_len);
+
+		rx_page_info = (struct be_rx_page_info *)
+		    pnob->rx_ctxt[fi];
+		pnob->rx_ctxt[fi] = (void *)NULL;
+		if (rx_page_info->page_offset || !pnob->rx_pg_shared) {
+			pci_unmap_page(adapter->pdev,
+				       pci_unmap_addr(rx_page_info, bus),
+				       frag_sz, PCI_DMA_FROMDEVICE);
+		}
+
+		put_page(rx_page_info->page);
+		memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+		atomic_dec(&pnob->rx_q_posted);
+	}
+	return -ENOMEM;
+}
+
+static void process_nic_rx_completion_lro(struct be_net_object *pnob,
+					  struct ETH_RX_COMPL_AMAP *rxcp)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	struct skb_frag_struct rx_frags[BE_MAX_FRAGS_PER_FRAME];
+	unsigned int udpcksm, tcpcksm;
+	u32 numfrags, vlanf, vtm, vlan_tag, nresid;
+	u16 vlant;
+	unsigned int fi, idx, n;
+	struct be_rx_page_info *rx_page_info;
+	u32 frag_sz = pnob->rx_buf_size, pktsize;
+	bool rx_coal = (adapter->max_rx_coal <= 1) ? 0 : 1;
+	u8 err, *va;
+	__wsum csum = 0;
+
+	if (AMAP_GET_BITS_PTR(ETH_RX_COMPL, ipsec, rxcp)) {
+		/*  Drop the pkt and move to the next completion.  */
+		adapter->be_stat.bes_rx_misc_pkts++;
+		return;
+	}
+	err = AMAP_GET_BITS_PTR(ETH_RX_COMPL, err, rxcp);
+	if (err || !rx_coal) {
+		/* We won't coalesce Rx pkts if the err bit set.
+		 * take the path of normal completion processing */
+		process_nic_rx_completion(pnob, rxcp);
+		return;
+	}
+
+	fi = AMAP_GET_BITS_PTR(ETH_RX_COMPL, fragndx, rxcp);
+	BUG_ON(fi >= (int)pnob->rx_q_len);
+	BUG_ON(fi < 0);
+	rx_page_info = (struct be_rx_page_info *)pnob->rx_ctxt[fi];
+	BUG_ON(!rx_page_info->page);
+	pnob->rx_ctxt[fi] = (void *)NULL;
+	/*  If one page is used per fragment or if this is the
+	 * second half of the page, unmap the page here
+	 */
+	if (rx_page_info->page_offset || !pnob->rx_pg_shared) {
+		pci_unmap_page(adapter->pdev,
+			       pci_unmap_addr(rx_page_info, bus),
+			       frag_sz, PCI_DMA_FROMDEVICE);
+	}
+
+	numfrags = AMAP_GET_BITS_PTR(ETH_RX_COMPL, numfrags, rxcp);
+	udpcksm = AMAP_GET_BITS_PTR(ETH_RX_COMPL, udpcksm, rxcp);
+	tcpcksm = AMAP_GET_BITS_PTR(ETH_RX_COMPL, tcpcksm, rxcp);
+	vlan_tag = AMAP_GET_BITS_PTR(ETH_RX_COMPL, vlan_tag, rxcp);
+	vlant = be16_to_cpu(vlan_tag);
+	vlanf = AMAP_GET_BITS_PTR(ETH_RX_COMPL, vtp, rxcp);
+	vtm = AMAP_GET_BITS_PTR(ETH_RX_COMPL, vtm, rxcp);
+	pktsize = AMAP_GET_BITS_PTR(ETH_RX_COMPL, pktsize, rxcp);
+
+	atomic_dec(&pnob->rx_q_posted);
+
+	if (tcpcksm && udpcksm && pktsize == 32) {
+		/* flush completion entries */
+		put_page(rx_page_info->page);
+		memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+		return;
+	}
+	/* Only one of udpcksum and tcpcksum can be set */
+	BUG_ON(udpcksm && tcpcksm);
+
+	/* jumbo frames could come in multiple fragments */
+	BUG_ON(numfrags != ((pktsize + (frag_sz - 1)) / frag_sz));
+	n = min(pktsize, frag_sz);
+	nresid = pktsize - n;	/* will be useful for jumbo pkts */
+	idx = 0;
+
+	va = page_address(rx_page_info->page) + rx_page_info->page_offset;
+	prefetch(va);
+	rx_frags[idx].page = rx_page_info->page;
+	rx_frags[idx].page_offset = (rx_page_info->page_offset);
+	rx_frags[idx].size = n;
+	memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+
+	/* If we got multiple fragments, we have more data. */
+	while (nresid) {
+		idx++;
+		index_inc(&fi, pnob->rx_q_len);
+
+		rx_page_info = (struct be_rx_page_info *)pnob->rx_ctxt[fi];
+		pnob->rx_ctxt[fi] = (void *)NULL;
+		if (rx_page_info->page_offset || !pnob->rx_pg_shared) {
+			pci_unmap_page(adapter->pdev,
+				       pci_unmap_addr(rx_page_info, bus),
+				       frag_sz, PCI_DMA_FROMDEVICE);
+		}
+
+		n = min(nresid, frag_sz);
+		rx_frags[idx].page = rx_page_info->page;
+		rx_frags[idx].page_offset = (rx_page_info->page_offset);
+		rx_frags[idx].size = n;
+
+		nresid -= n;
+		memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+		atomic_dec(&pnob->rx_q_posted);
+	}
+
+	if (likely(!(vlanf && vtm))) {
+		lro_receive_frags(&pnob->lro_mgr, rx_frags,
+				  pktsize, pktsize,
+				  (void *)(unsigned long)csum, csum);
+	} else {
+		/* Vlan tag present in pkt and BE found
+		 * that the tag matched an entry in VLAN table
+		 */
+		if (unlikely(!pnob->vlan_grp || pnob->num_vlans == 0)) {
+			/* But we have no VLANs configured.
+			 * This should never happen.  Drop the packet.
+			 */
+			dev_info(&pnob->netdev->dev,
+			       "BladeEngine: Unexpected vlan tagged packet\n");
+			return;
+		}
+		/* pass the VLAN packet to stack */
+		lro_vlan_hwaccel_receive_frags(&pnob->lro_mgr,
+					       rx_frags, pktsize, pktsize,
+					       pnob->vlan_grp, vlant,
+					       (void *)(unsigned long)csum,
+					       csum);
+	}
+
+	adapter->be_stat.bes_rx_coal++;
+}
+
+struct ETH_RX_COMPL_AMAP *be_get_rx_cmpl(struct be_net_object *pnob)
+{
+	struct ETH_RX_COMPL_AMAP *rxcp = &pnob->rx_cq[pnob->rx_cq_tl];
+	u32 valid, ct;
+
+	valid = AMAP_GET_BITS_PTR(ETH_RX_COMPL, valid, rxcp);
+	if (valid == 0)
+		return NULL;
+
+	ct = AMAP_GET_BITS_PTR(ETH_RX_COMPL, ct, rxcp);
+	if (ct != 0) {
+		/* Invalid chute #. treat as error */
+		AMAP_SET_BITS_PTR(ETH_RX_COMPL, err, rxcp, 1);
+	}
+
+	be_adv_rxcq_tl(pnob);
+	AMAP_SET_BITS_PTR(ETH_RX_COMPL, valid, rxcp, 0);
+	return rxcp;
+}
+
+static void update_rx_rate(struct be_adapter *adapter)
+{
+	/* update the rate once in two seconds */
+	if ((jiffies - adapter->eth_rx_jiffies) > 2 * (HZ)) {
+		u32 r;
+		r = adapter->eth_rx_bytes /
+		    ((jiffies - adapter->eth_rx_jiffies) / (HZ));
+		r = (r / 1000000);	/* MB/Sec */
+
+		/* Mega Bits/Sec */
+		adapter->be_stat.bes_eth_rx_rate = (r * 8);
+		adapter->eth_rx_jiffies = jiffies;
+		adapter->eth_rx_bytes = 0;
+	}
+}
+
+static int process_rx_completions(struct be_net_object *pnob, int max_work)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	struct ETH_RX_COMPL_AMAP *rxcp;
+	u32 nc = 0;
+	unsigned int pktsize;
+
+	while (max_work && (rxcp = be_get_rx_cmpl(pnob))) {
+		prefetch(rxcp);
+		pktsize = AMAP_GET_BITS_PTR(ETH_RX_COMPL, pktsize, rxcp);
+		process_nic_rx_completion_lro(pnob, rxcp);
+		adapter->eth_rx_bytes += pktsize;
+		update_rx_rate(adapter);
+		nc++;
+		max_work--;
+		adapter->be_stat.bes_rx_compl++;
+	}
+	if (likely(adapter->max_rx_coal > 1)) {
+		adapter->be_stat.bes_rx_flush++;
+		lro_flush_all(&pnob->lro_mgr);
+	}
+
+	/* Refill the queue */
+	if (atomic_read(&pnob->rx_q_posted) < 900)
+		be_post_eth_rx_buffs(pnob);
+
+	return nc;
+}
+
+static struct ETH_TX_COMPL_AMAP *be_get_tx_cmpl(struct be_net_object *pnob)
+{
+	struct ETH_TX_COMPL_AMAP *txcp = &pnob->tx_cq[pnob->tx_cq_tl];
+	u32 valid;
+
+	valid = AMAP_GET_BITS_PTR(ETH_TX_COMPL, valid, txcp);
+	if (valid == 0)
+		return NULL;
+
+	AMAP_SET_BITS_PTR(ETH_TX_COMPL, valid, txcp, 0);
+	be_adv_txcq_tl(pnob);
+	return txcp;
+
+}
+
+void process_one_tx_compl(struct be_net_object *pnob, u32 end_idx)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	int cur_index, tx_wrbs_completed = 0;
+	struct sk_buff *skb;
+	u64 busaddr, pa, pa_lo, pa_hi;
+	struct ETH_WRB_AMAP *wrb;
+	u32 frag_len, last_index, j;
+
+	last_index = tx_compl_lastwrb_idx_get(pnob);
+	BUG_ON(last_index != end_idx);
+	pnob->tx_ctxt[pnob->tx_q_tl] = NULL;
+	do {
+		cur_index = pnob->tx_q_tl;
+		wrb = &pnob->tx_q[cur_index];
+		pa_hi = AMAP_GET_BITS_PTR(ETH_WRB, frag_pa_hi, wrb);
+		pa_lo = AMAP_GET_BITS_PTR(ETH_WRB, frag_pa_lo, wrb);
+		frag_len = AMAP_GET_BITS_PTR(ETH_WRB, frag_len, wrb);
+		busaddr = (pa_hi << 32) | pa_lo;
+		if (busaddr != 0) {
+			pa = le64_to_cpu(busaddr);
+			pci_unmap_single(adapter->pdev, pa,
+					 frag_len, PCI_DMA_TODEVICE);
+		}
+		if (cur_index == last_index) {
+			skb = (struct sk_buff *)pnob->tx_ctxt[cur_index];
+			BUG_ON(!skb);
+			for (j = 0; j < skb_shinfo(skb)->nr_frags; j++) {
+				struct skb_frag_struct *frag;
+				frag = &skb_shinfo(skb)->frags[j];
+				pci_unmap_page(adapter->pdev,
+					       (ulong) frag->page, frag->size,
+					       PCI_DMA_TODEVICE);
+			}
+			kfree_skb(skb);
+			pnob->tx_ctxt[cur_index] = NULL;
+		} else {
+			BUG_ON(pnob->tx_ctxt[cur_index]);
+		}
+		tx_wrbs_completed++;
+		be_adv_txq_tl(pnob);
+	} while (cur_index != last_index);
+	atomic_sub(tx_wrbs_completed, &pnob->tx_q_used);
+}
+
+/* there is no need to take an SMP lock here since currently
+ * we have only one instance of the tasklet that does completion
+ * processing.
+ */
+static void process_nic_tx_completions(struct be_net_object *pnob)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	struct ETH_TX_COMPL_AMAP *txcp;
+	struct net_device *netdev = pnob->netdev;
+	u32 end_idx, num_processed = 0;
+
+	adapter->be_stat.bes_tx_events++;
+
+	while ((txcp = be_get_tx_cmpl(pnob))) {
+		end_idx = AMAP_GET_BITS_PTR(ETH_TX_COMPL, wrb_index, txcp);
+		process_one_tx_compl(pnob, end_idx);
+		num_processed++;
+		adapter->be_stat.bes_tx_compl++;
+	}
+	be_notify_cmpl(pnob, num_processed, pnob->tx_cq_id, 1);
+	/*
+	 * We got Tx completions and have usable WRBs.
+	 * If the netdev's queue has been stopped
+	 * because we had run out of WRBs, wake it now.
+	 */
+	spin_lock(&adapter->txq_lock);
+	if (netif_queue_stopped(netdev)
+	    && atomic_read(&pnob->tx_q_used) < pnob->tx_q_len / 2) {
+		netif_wake_queue(netdev);
+	}
+	spin_unlock(&adapter->txq_lock);
+}
+
+static u32 post_rx_buffs(struct be_net_object *pnob, struct list_head *rxbl)
+{
+	u32 nposted = 0;
+	struct ETH_RX_D_AMAP *rxd = NULL;
+	struct be_recv_buffer *rxbp;
+	void **rx_ctxp;
+	struct RQ_DB_AMAP rqdb;
+
+	rx_ctxp = pnob->rx_ctxt;
+
+	while (!list_empty(rxbl) &&
+	       (rx_ctxp[pnob->rx_q_hd] == NULL) && nposted < 255) {
+
+		rxbp = list_first_entry(rxbl, struct be_recv_buffer, rxb_list);
+		list_del(&rxbp->rxb_list);
+		rxd = pnob->rx_q + pnob->rx_q_hd;
+		AMAP_SET_BITS_PTR(ETH_RX_D, fragpa_lo, rxd, rxbp->rxb_pa_lo);
+		AMAP_SET_BITS_PTR(ETH_RX_D, fragpa_hi, rxd, rxbp->rxb_pa_hi);
+
+		rx_ctxp[pnob->rx_q_hd] = rxbp->rxb_ctxt;
+		be_adv_rxq_hd(pnob);
+		nposted++;
+	}
+
+	if (nposted) {
+		/* Now press the door bell to notify BladeEngine. */
+		rqdb.dw[0] = 0;
+		AMAP_SET_BITS_PTR(RQ_DB, numPosted, &rqdb, nposted);
+		AMAP_SET_BITS_PTR(RQ_DB, rq, &rqdb, pnob->rx_q_id);
+		PD_WRITE(&pnob->fn_obj, erx_rq_db, rqdb.dw[0]);
+	}
+	atomic_add(nposted, &pnob->rx_q_posted);
+	return nposted;
+}
+
+void be_post_eth_rx_buffs(struct be_net_object *pnob)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	u32 num_bufs, r;
+	u64 busaddr = 0, tmp_pa;
+	u32 max_bufs, pg_hd;
+	u32 frag_size;
+	struct be_recv_buffer *rxbp;
+	struct list_head rxbl;
+	struct be_rx_page_info *rx_page_info;
+	struct page *page = NULL;
+	u32 page_order = 0;
+	gfp_t alloc_flags = GFP_ATOMIC;
+
+	BUG_ON(!adapter);
+
+	max_bufs = 64;		/* should be even # <= 255. */
+
+	frag_size = pnob->rx_buf_size;
+	page_order = get_order(frag_size);
+
+	if (frag_size == 8192)
+		alloc_flags |= (gfp_t) __GFP_COMP;
+	/*
+	 * Form a linked list of RECV_BUFFFER structure to be be posted.
+	 * We will post even number of buffer so that pages can be
+	 * shared.
+	 */
+	INIT_LIST_HEAD(&rxbl);
+
+	for (num_bufs = 0; num_bufs < max_bufs &&
+		!pnob->rx_page_info[pnob->rx_pg_info_hd].page; ++num_bufs) {
+
+		rxbp = &pnob->eth_rx_bufs[num_bufs];
+		pg_hd = pnob->rx_pg_info_hd;
+		rx_page_info = &pnob->rx_page_info[pg_hd];
+
+		if (!page) {
+			page = alloc_pages(alloc_flags, page_order);
+			if (unlikely(page == NULL)) {
+				adapter->be_stat.bes_ethrx_post_fail++;
+				pnob->rxbuf_post_fail++;
+				break;
+			}
+			pnob->rxbuf_post_fail = 0;
+			busaddr = pci_map_page(adapter->pdev, page, 0,
+					       frag_size, PCI_DMA_FROMDEVICE);
+			rx_page_info->page_offset = 0;
+			rx_page_info->page = page;
+			/*
+			 * If we are sharing a page among two skbs,
+			 * alloc a new one on the next iteration
+			 */
+			if (pnob->rx_pg_shared == false)
+				page = NULL;
+		} else {
+			get_page(page);
+			rx_page_info->page_offset += frag_size;
+			rx_page_info->page = page;
+			/*
+			 * We are finished with the alloced page,
+			 * Alloc a new one on the next iteration
+			 */
+			page = NULL;
+		}
+		rxbp->rxb_ctxt = (void *)rx_page_info;
+		index_inc(&pnob->rx_pg_info_hd, pnob->rx_q_len);
+
+		pci_unmap_addr_set(rx_page_info, bus, busaddr);
+		tmp_pa = busaddr + rx_page_info->page_offset;
+		rxbp->rxb_pa_lo = (tmp_pa & 0xFFFFFFFF);
+		rxbp->rxb_pa_hi = (tmp_pa >> 32);
+		rxbp->rxb_len = frag_size;
+		list_add_tail(&rxbp->rxb_list, &rxbl);
+	}			/* End of for */
+
+	r = post_rx_buffs(pnob, &rxbl);
+	BUG_ON(r != num_bufs);
+	return;
+}
+
+/*
+ * Interrupt service for network function.  We just schedule the
+ * tasklet which does all completion processing.
+ */
+irqreturn_t be_int(int irq, void *dev)
+{
+	struct net_device *netdev = dev;
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+	u32 isr;
+
+	isr = CSR_READ(&pnob->fn_obj, cev.isr1);
+	if (unlikely(!isr))
+		return IRQ_NONE;
+
+	spin_lock(&adapter->int_lock);
+	adapter->isr |= isr;
+	spin_unlock(&adapter->int_lock);
+
+	adapter->be_stat.bes_ints++;
+
+	tasklet_schedule(&adapter->sts_handler);
+	return IRQ_HANDLED;
+}
+
+/*
+ * Poll function called by NAPI with a work budget.
+ * We process as many UC. BC and MC receive completions
+ * as the budget allows and return the actual number of
+ * RX ststutses processed.
+ */
+int be_poll(struct napi_struct *napi, int budget)
+{
+	struct be_net_object *pnob =
+			container_of(napi, struct be_net_object, napi);
+	u32 work_done;
+
+	pnob->adapter->be_stat.bes_polls++;
+	work_done = process_rx_completions(pnob, budget);
+	BUG_ON(work_done > budget);
+
+	/* All consumed */
+	if (work_done < budget) {
+		netif_rx_complete(napi);
+		/* enable intr */
+		be_notify_cmpl(pnob, work_done, pnob->rx_cq_id, 1);
+	} else {
+		/* More to be consumed; continue with interrupts disabled */
+		be_notify_cmpl(pnob, work_done, pnob->rx_cq_id, 0);
+	}
+	return work_done;
+}
+
+static struct EQ_ENTRY_AMAP *get_event(struct be_net_object *pnob)
+{
+	struct EQ_ENTRY_AMAP *eqp = &(pnob->event_q[pnob->event_q_tl]);
+	if (!AMAP_GET_BITS_PTR(EQ_ENTRY, Valid, eqp))
+		return NULL;
+	be_adv_eq_tl(pnob);
+	return eqp;
+}
+
+/*
+ * Processes all valid events in the event ring associated with given
+ * NetObject.  Also, notifies BE the number of events processed.
+ */
+static inline u32 process_events(struct be_net_object *pnob)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	struct EQ_ENTRY_AMAP *eqp;
+	u32 rid, num_events = 0;
+	struct net_device *netdev = pnob->netdev;
+
+	while ((eqp = get_event(pnob)) != NULL) {
+		adapter->be_stat.bes_events++;
+		rid = AMAP_GET_BITS_PTR(EQ_ENTRY, ResourceID, eqp);
+		if (rid == pnob->rx_cq_id) {
+			adapter->be_stat.bes_rx_events++;
+			netif_rx_schedule(&pnob->napi);
+		} else if (rid == pnob->tx_cq_id) {
+			process_nic_tx_completions(pnob);
+		} else if (rid == pnob->mcc_cq_id) {
+			be_mcc_process_cq(&pnob->mcc_q_obj, 1);
+		} else {
+			dev_info(&netdev->dev,
+					"Invalid EQ ResourceID %d\n", rid);
+		}
+		AMAP_SET_BITS_PTR(EQ_ENTRY, Valid, eqp, 0);
+		AMAP_SET_BITS_PTR(EQ_ENTRY, ResourceID, eqp, 0);
+		num_events++;
+	}
+	return num_events;
+}
+
+static void update_eqd(struct be_adapter *adapter, struct be_net_object *pnob)
+{
+	int status;
+	struct be_eq_object *eq_objectp;
+
+	/* update once a second */
+	if ((jiffies - adapter->ips_jiffies) > 1 * (HZ)) {
+		/* One second elapsed since last update  */
+		u32 r, new_eqd = -1;
+		r = adapter->be_stat.bes_ints - adapter->be_stat.bes_prev_ints;
+		r = r / ((jiffies - adapter->ips_jiffies) / (HZ));
+		adapter->be_stat.bes_ips = r;
+		adapter->ips_jiffies = jiffies;
+		adapter->be_stat.bes_prev_ints = adapter->be_stat.bes_ints;
+		if (r > IPS_HI_WM && adapter->cur_eqd < adapter->max_eqd)
+			new_eqd = (adapter->cur_eqd + 8);
+		if (r < IPS_LO_WM && adapter->cur_eqd > adapter->min_eqd)
+			new_eqd = (adapter->cur_eqd - 8);
+		if (adapter->enable_aic && new_eqd != -1) {
+			eq_objectp = &pnob->event_q_obj;
+			status = be_eq_modify_delay(&pnob->fn_obj, 1,
+						    &eq_objectp, &new_eqd, NULL,
+						    NULL, NULL);
+			if (status == BE_SUCCESS)
+				adapter->cur_eqd = new_eqd;
+		}
+	}
+}
+
+/*
+    This function notifies BladeEngine of how many events were processed
+    from the event queue by ringing the corresponding door bell and
+    optionally re-arms the event queue.
+    n		- number of events processed
+    re_arm	- 1 - re-arm the EQ, 0 - do not re-arm the EQ
+
+*/
+static void be_notify_event(struct be_net_object *pnob, int n, int re_arm)
+{
+	struct CQ_DB_AMAP eqdb;
+	eqdb.dw[0] = 0;
+
+	AMAP_SET_BITS_PTR(CQ_DB, qid, &eqdb, pnob->event_q_id);
+	AMAP_SET_BITS_PTR(CQ_DB, rearm, &eqdb, re_arm);
+	AMAP_SET_BITS_PTR(CQ_DB, event, &eqdb, 1);
+	AMAP_SET_BITS_PTR(CQ_DB, num_popped, &eqdb, n);
+	/*
+	 * Under some situations we see an interrupt and no valid
+	 * EQ entry.  To keep going, we need to ring the DB even if
+	 * numPOsted is 0.
+	 */
+	PD_WRITE(&pnob->fn_obj, cq_db, eqdb.dw[0]);
+	return;
+}
+
+/*
+ * Called from the tasklet scheduled by ISR.  All real interrupt processing
+ * is done here.
+ */
+void be_process_intr(unsigned long context)
+{
+	struct be_adapter *adapter = (struct be_adapter *)context;
+	struct be_net_object *pnob = adapter->net_obj;
+	u32 isr, n;
+	ulong flags = 0;
+
+	isr = adapter->isr;
+
+	/*
+	 * we create only one NIC event queue in Linux. Event is
+	 * expected only in the first event queue
+	 */
+	BUG_ON(isr & 0xfffffffe);
+	if ((isr & 1) == 0)
+		return;		/* not our interrupt */
+	n = process_events(pnob);
+	/*
+	 * Clear the event bit. adapter->isr is  set by
+	 * hard interrupt.  Prevent race with lock.
+	 */
+	spin_lock_irqsave(&adapter->int_lock, flags);
+	adapter->isr &= ~1;
+	spin_unlock_irqrestore(&adapter->int_lock, flags);
+	be_notify_event(pnob, n, 1);
+	/*
+	 * If previous allocation attempts had failed and
+	 * BE has used up all posted buffers, post RX buffers here
+	 */
+	if (pnob->rxbuf_post_fail && atomic_read(&pnob->rx_q_posted) == 0)
+		be_post_eth_rx_buffs(pnob);
+	update_eqd(adapter, pnob);
+	return;
+}
diff --git a/drivers/staging/benet/be_netif.c b/drivers/staging/benet/be_netif.c
new file mode 100644
index 000000000000..2b8daf63dc7d
--- /dev/null
+++ b/drivers/staging/benet/be_netif.c
@@ -0,0 +1,705 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * be_netif.c
+ *
+ * This file contains various entry points of drivers seen by tcp/ip stack.
+ */
+
+#include <linux/if_vlan.h>
+#include <linux/in.h>
+#include "benet.h"
+#include <linux/ip.h>
+#include <linux/inet_lro.h>
+
+/* Strings to print Link properties */
+static const char *link_speed[] = {
+	"Invalid link Speed Value",
+	"10 Mbps",
+	"100 Mbps",
+	"1 Gbps",
+	"10 Gbps"
+};
+
+static const char *link_duplex[] = {
+	"Invalid Duplex Value",
+	"Half Duplex",
+	"Full Duplex"
+};
+
+static const char *link_state[] = {
+	"",
+	"(active)"
+};
+
+void be_print_link_info(struct BE_LINK_STATUS *lnk_status)
+{
+	u16 si, di, ai;
+
+	/* Port 0 */
+	if (lnk_status->mac0_speed && lnk_status->mac0_duplex) {
+		/* Port is up and running */
+		si = (lnk_status->mac0_speed < 5) ? lnk_status->mac0_speed : 0;
+		di = (lnk_status->mac0_duplex < 3) ?
+		    lnk_status->mac0_duplex : 0;
+		ai = (lnk_status->active_port == 0) ? 1 : 0;
+		printk(KERN_INFO "PortNo. 0: Speed - %s %s %s\n",
+		       link_speed[si], link_duplex[di], link_state[ai]);
+	} else
+		printk(KERN_INFO "PortNo. 0: Down\n");
+
+	/* Port 1 */
+	if (lnk_status->mac1_speed && lnk_status->mac1_duplex) {
+		/* Port is up and running */
+		si = (lnk_status->mac1_speed < 5) ? lnk_status->mac1_speed : 0;
+		di = (lnk_status->mac1_duplex < 3) ?
+		    lnk_status->mac1_duplex : 0;
+		ai = (lnk_status->active_port == 0) ? 1 : 0;
+		printk(KERN_INFO "PortNo. 1: Speed - %s %s %s\n",
+		       link_speed[si], link_duplex[di], link_state[ai]);
+	} else
+		printk(KERN_INFO "PortNo. 1: Down\n");
+
+	return;
+}
+
+static int
+be_get_frag_header(struct skb_frag_struct *frag, void **mac_hdr,
+		   void **ip_hdr, void **tcpudp_hdr,
+		   u64 *hdr_flags, void *priv)
+{
+	struct ethhdr *eh;
+	struct vlan_ethhdr *veh;
+	struct iphdr *iph;
+	u8 *va = page_address(frag->page) + frag->page_offset;
+	unsigned long ll_hlen;
+
+	/* find the mac header, abort if not IPv4 */
+
+	prefetch(va);
+	eh = (struct ethhdr *)va;
+	*mac_hdr = eh;
+	ll_hlen = ETH_HLEN;
+	if (eh->h_proto != htons(ETH_P_IP)) {
+		if (eh->h_proto == htons(ETH_P_8021Q)) {
+			veh = (struct vlan_ethhdr *)va;
+			if (veh->h_vlan_encapsulated_proto != htons(ETH_P_IP))
+				return -1;
+
+			ll_hlen += VLAN_HLEN;
+
+		} else {
+			return -1;
+		}
+	}
+	*hdr_flags = LRO_IPV4;
+
+	iph = (struct iphdr *)(va + ll_hlen);
+	*ip_hdr = iph;
+	if (iph->protocol != IPPROTO_TCP)
+		return -1;
+	*hdr_flags |= LRO_TCP;
+	*tcpudp_hdr = (u8 *) (*ip_hdr) + (iph->ihl << 2);
+
+	return 0;
+}
+
+static int benet_open(struct net_device *netdev)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+	struct net_lro_mgr *lro_mgr;
+
+	if (adapter->dev_state < BE_DEV_STATE_INIT)
+		return -EAGAIN;
+
+	lro_mgr = &pnob->lro_mgr;
+	lro_mgr->dev = netdev;
+
+	lro_mgr->features = LRO_F_NAPI;
+	lro_mgr->ip_summed = CHECKSUM_UNNECESSARY;
+	lro_mgr->ip_summed_aggr = CHECKSUM_UNNECESSARY;
+	lro_mgr->max_desc = BE_MAX_LRO_DESCRIPTORS;
+	lro_mgr->lro_arr = pnob->lro_desc;
+	lro_mgr->get_frag_header = be_get_frag_header;
+	lro_mgr->max_aggr = adapter->max_rx_coal;
+	lro_mgr->frag_align_pad = 2;
+	if (lro_mgr->max_aggr > MAX_SKB_FRAGS)
+		lro_mgr->max_aggr = MAX_SKB_FRAGS;
+
+	adapter->max_rx_coal = BE_LRO_MAX_PKTS;
+
+	be_update_link_status(adapter);
+
+	/*
+	 * Set carrier on only if Physical Link up
+	 * Either of the port link status up signifies this
+	 */
+	if ((adapter->port0_link_sts == BE_PORT_LINK_UP) ||
+	    (adapter->port1_link_sts == BE_PORT_LINK_UP)) {
+		netif_start_queue(netdev);
+		netif_carrier_on(netdev);
+	}
+
+	adapter->dev_state = BE_DEV_STATE_OPEN;
+	napi_enable(&pnob->napi);
+	be_enable_intr(pnob);
+	be_enable_eq_intr(pnob);
+	/*
+	 * RX completion queue may be in dis-armed state. Arm it.
+	 */
+	be_notify_cmpl(pnob, 0, pnob->rx_cq_id, 1);
+
+	return 0;
+}
+
+static int benet_close(struct net_device *netdev)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+
+	netif_stop_queue(netdev);
+	synchronize_irq(netdev->irq);
+
+	be_wait_nic_tx_cmplx_cmpl(pnob);
+	adapter->dev_state = BE_DEV_STATE_INIT;
+	netif_carrier_off(netdev);
+
+	adapter->port0_link_sts = BE_PORT_LINK_DOWN;
+	adapter->port1_link_sts = BE_PORT_LINK_DOWN;
+	be_disable_intr(pnob);
+	be_disable_eq_intr(pnob);
+	napi_disable(&pnob->napi);
+
+	return 0;
+}
+
+/*
+ * Setting a Mac Address for BE
+ * Takes netdev and a void pointer as arguments.
+ * The pointer holds the new addres to be used.
+ */
+static int benet_set_mac_addr(struct net_device *netdev, void *p)
+{
+	struct sockaddr *addr = p;
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	memcpy(netdev->dev_addr, addr->sa_data, netdev->addr_len);
+	be_rxf_mac_address_read_write(&pnob->fn_obj, 0, 0, false, true, false,
+				netdev->dev_addr, NULL, NULL);
+	/*
+	 * Since we are doing Active-Passive failover, both
+	 * ports should have matching MAC addresses everytime.
+	 */
+	be_rxf_mac_address_read_write(&pnob->fn_obj, 1, 0, false, true, false,
+				      netdev->dev_addr, NULL, NULL);
+
+	return 0;
+}
+
+void be_get_stats_timer_handler(unsigned long context)
+{
+	struct be_timer_ctxt *ctxt = (struct be_timer_ctxt *)context;
+
+	if (atomic_read(&ctxt->get_stat_flag)) {
+		atomic_dec(&ctxt->get_stat_flag);
+		up((void *)ctxt->get_stat_sem_addr);
+	}
+	del_timer(&ctxt->get_stats_timer);
+	return;
+}
+
+void be_get_stat_cb(void *context, int status,
+		    struct MCC_WRB_AMAP *optional_wrb)
+{
+	struct be_timer_ctxt *ctxt = (struct be_timer_ctxt *)context;
+	/*
+	 * just up the semaphore if the get_stat_flag
+	 * reads 1. so that the waiter can continue.
+	 * If it is 0, then it was handled by the timer handler.
+	 */
+	del_timer(&ctxt->get_stats_timer);
+	if (atomic_read(&ctxt->get_stat_flag)) {
+		atomic_dec(&ctxt->get_stat_flag);
+		up((void *)ctxt->get_stat_sem_addr);
+	}
+}
+
+struct net_device_stats *benet_get_stats(struct net_device *dev)
+{
+	struct be_net_object *pnob = netdev_priv(dev);
+	struct be_adapter *adapter = pnob->adapter;
+	u64 pa;
+	struct be_timer_ctxt *ctxt = &adapter->timer_ctxt;
+
+	if (adapter->dev_state != BE_DEV_STATE_OPEN) {
+		/* Return previously read stats */
+		return &(adapter->benet_stats);
+	}
+	/* Get Physical Addr */
+	pa = pci_map_single(adapter->pdev, adapter->eth_statsp,
+			    sizeof(struct FWCMD_ETH_GET_STATISTICS),
+			    PCI_DMA_FROMDEVICE);
+	ctxt->get_stat_sem_addr = (unsigned long)&adapter->get_eth_stat_sem;
+	atomic_inc(&ctxt->get_stat_flag);
+
+	be_rxf_query_eth_statistics(&pnob->fn_obj, adapter->eth_statsp,
+				    cpu_to_le64(pa), be_get_stat_cb, ctxt,
+				    NULL);
+
+	ctxt->get_stats_timer.data = (unsigned long)ctxt;
+	mod_timer(&ctxt->get_stats_timer, (jiffies + (HZ * 2)));
+	down((void *)ctxt->get_stat_sem_addr);	/* callback will unblock us */
+
+	/* Adding port0 and port1 stats. */
+	adapter->benet_stats.rx_packets =
+	    adapter->eth_statsp->params.response.p0recvdtotalframes +
+	    adapter->eth_statsp->params.response.p1recvdtotalframes;
+	adapter->benet_stats.tx_packets =
+	    adapter->eth_statsp->params.response.p0xmitunicastframes +
+	    adapter->eth_statsp->params.response.p1xmitunicastframes;
+	adapter->benet_stats.tx_bytes =
+	    adapter->eth_statsp->params.response.p0xmitbyteslsd +
+	    adapter->eth_statsp->params.response.p1xmitbyteslsd;
+	adapter->benet_stats.rx_errors =
+	    adapter->eth_statsp->params.response.p0crcerrors +
+	    adapter->eth_statsp->params.response.p1crcerrors;
+	adapter->benet_stats.rx_errors +=
+	    adapter->eth_statsp->params.response.p0alignmentsymerrs +
+	    adapter->eth_statsp->params.response.p1alignmentsymerrs;
+	adapter->benet_stats.rx_errors +=
+	    adapter->eth_statsp->params.response.p0inrangelenerrors +
+	    adapter->eth_statsp->params.response.p1inrangelenerrors;
+	adapter->benet_stats.rx_bytes =
+	    adapter->eth_statsp->params.response.p0recvdtotalbytesLSD +
+	    adapter->eth_statsp->params.response.p1recvdtotalbytesLSD;
+	adapter->benet_stats.rx_crc_errors =
+	    adapter->eth_statsp->params.response.p0crcerrors +
+	    adapter->eth_statsp->params.response.p1crcerrors;
+
+	adapter->benet_stats.tx_packets +=
+	    adapter->eth_statsp->params.response.p0xmitmulticastframes +
+	    adapter->eth_statsp->params.response.p1xmitmulticastframes;
+	adapter->benet_stats.tx_packets +=
+	    adapter->eth_statsp->params.response.p0xmitbroadcastframes +
+	    adapter->eth_statsp->params.response.p1xmitbroadcastframes;
+	adapter->benet_stats.tx_errors = 0;
+
+	adapter->benet_stats.multicast =
+	    adapter->eth_statsp->params.response.p0xmitmulticastframes +
+	    adapter->eth_statsp->params.response.p1xmitmulticastframes;
+
+	adapter->benet_stats.rx_fifo_errors =
+	    adapter->eth_statsp->params.response.p0rxfifooverflowdropped +
+	    adapter->eth_statsp->params.response.p1rxfifooverflowdropped;
+	adapter->benet_stats.rx_frame_errors =
+	    adapter->eth_statsp->params.response.p0alignmentsymerrs +
+	    adapter->eth_statsp->params.response.p1alignmentsymerrs;
+	adapter->benet_stats.rx_length_errors =
+	    adapter->eth_statsp->params.response.p0inrangelenerrors +
+	    adapter->eth_statsp->params.response.p1inrangelenerrors;
+	adapter->benet_stats.rx_length_errors +=
+	    adapter->eth_statsp->params.response.p0outrangeerrors +
+	    adapter->eth_statsp->params.response.p1outrangeerrors;
+	adapter->benet_stats.rx_length_errors +=
+	    adapter->eth_statsp->params.response.p0frametoolongerrors +
+	    adapter->eth_statsp->params.response.p1frametoolongerrors;
+
+	pci_unmap_single(adapter->pdev, (ulong) adapter->eth_statsp,
+			 sizeof(struct FWCMD_ETH_GET_STATISTICS),
+			 PCI_DMA_FROMDEVICE);
+	return &(adapter->benet_stats);
+
+}
+
+static void be_start_tx(struct be_net_object *pnob, u32 nposted)
+{
+#define CSR_ETH_MAX_SQPOSTS 255
+	struct SQ_DB_AMAP sqdb;
+
+	sqdb.dw[0] = 0;
+
+	AMAP_SET_BITS_PTR(SQ_DB, cid, &sqdb, pnob->tx_q_id);
+	while (nposted) {
+		if (nposted > CSR_ETH_MAX_SQPOSTS) {
+			AMAP_SET_BITS_PTR(SQ_DB, numPosted, &sqdb,
+					  CSR_ETH_MAX_SQPOSTS);
+			nposted -= CSR_ETH_MAX_SQPOSTS;
+		} else {
+			AMAP_SET_BITS_PTR(SQ_DB, numPosted, &sqdb, nposted);
+			nposted = 0;
+		}
+		PD_WRITE(&pnob->fn_obj, etx_sq_db, sqdb.dw[0]);
+	}
+
+	return;
+}
+
+static void update_tx_rate(struct be_adapter *adapter)
+{
+	/* update the rate once in two seconds */
+	if ((jiffies - adapter->eth_tx_jiffies) > 2 * (HZ)) {
+		u32 r;
+		r = adapter->eth_tx_bytes /
+		    ((jiffies - adapter->eth_tx_jiffies) / (HZ));
+		r = (r / 1000000);	/* M bytes/s */
+		adapter->be_stat.bes_eth_tx_rate = (r * 8); /* M bits/s */
+		adapter->eth_tx_jiffies = jiffies;
+		adapter->eth_tx_bytes = 0;
+	}
+}
+
+static int wrb_cnt_in_skb(struct sk_buff *skb)
+{
+	int cnt = 0;
+	while (skb) {
+		if (skb->len > skb->data_len)
+			cnt++;
+		cnt += skb_shinfo(skb)->nr_frags;
+		skb = skb_shinfo(skb)->frag_list;
+	}
+	BUG_ON(cnt > BE_MAX_TX_FRAG_COUNT);
+	return cnt;
+}
+
+static void wrb_fill(struct ETH_WRB_AMAP *wrb, u64 addr, int len)
+{
+	AMAP_SET_BITS_PTR(ETH_WRB, frag_pa_hi, wrb, addr >> 32);
+	AMAP_SET_BITS_PTR(ETH_WRB, frag_pa_lo, wrb, addr & 0xFFFFFFFF);
+	AMAP_SET_BITS_PTR(ETH_WRB, frag_len, wrb, len);
+}
+
+static void wrb_fill_extra(struct ETH_WRB_AMAP *wrb, struct sk_buff *skb,
+			   struct be_net_object *pnob)
+{
+	wrb->dw[2] = 0;
+	wrb->dw[3] = 0;
+	AMAP_SET_BITS_PTR(ETH_WRB, crc, wrb, 1);
+	if (skb_shinfo(skb)->gso_segs > 1 && skb_shinfo(skb)->gso_size) {
+		AMAP_SET_BITS_PTR(ETH_WRB, lso, wrb, 1);
+		AMAP_SET_BITS_PTR(ETH_WRB, lso_mss, wrb,
+				  skb_shinfo(skb)->gso_size);
+	} else if (skb->ip_summed == CHECKSUM_PARTIAL) {
+		u8 proto = ((struct iphdr *)ip_hdr(skb))->protocol;
+		if (proto == IPPROTO_TCP)
+			AMAP_SET_BITS_PTR(ETH_WRB, tcpcs, wrb, 1);
+		else if (proto == IPPROTO_UDP)
+			AMAP_SET_BITS_PTR(ETH_WRB, udpcs, wrb, 1);
+	}
+	if (pnob->vlan_grp && vlan_tx_tag_present(skb)) {
+		AMAP_SET_BITS_PTR(ETH_WRB, vlan, wrb, 1);
+		AMAP_SET_BITS_PTR(ETH_WRB, vlan_tag, wrb, vlan_tx_tag_get(skb));
+	}
+}
+
+static inline void wrb_copy_extra(struct ETH_WRB_AMAP *to,
+				  struct ETH_WRB_AMAP *from)
+{
+
+	to->dw[2] = from->dw[2];
+	to->dw[3] = from->dw[3];
+}
+
+/* Returns the actual count of wrbs used including a possible dummy */
+static int copy_skb_to_txq(struct be_net_object *pnob, struct sk_buff *skb,
+			   u32 wrb_cnt, u32 *copied)
+{
+	u64 busaddr;
+	struct ETH_WRB_AMAP *wrb = NULL, *first = NULL;
+	u32 i;
+	bool dummy = true;
+	struct pci_dev *pdev = pnob->adapter->pdev;
+
+	if (wrb_cnt & 1)
+		wrb_cnt++;
+	else
+		dummy = false;
+
+	atomic_add(wrb_cnt, &pnob->tx_q_used);
+
+	while (skb) {
+		if (skb->len > skb->data_len) {
+			int len = skb->len - skb->data_len;
+			busaddr = pci_map_single(pdev, skb->data, len,
+						 PCI_DMA_TODEVICE);
+			busaddr = cpu_to_le64(busaddr);
+			wrb = &pnob->tx_q[pnob->tx_q_hd];
+			if (first == NULL) {
+				wrb_fill_extra(wrb, skb, pnob);
+				first = wrb;
+			} else {
+				wrb_copy_extra(wrb, first);
+			}
+			wrb_fill(wrb, busaddr, len);
+			be_adv_txq_hd(pnob);
+			*copied += len;
+		}
+
+		for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
+			struct skb_frag_struct *frag =
+			    &skb_shinfo(skb)->frags[i];
+			busaddr = pci_map_page(pdev, frag->page,
+					       frag->page_offset, frag->size,
+					       PCI_DMA_TODEVICE);
+			busaddr = cpu_to_le64(busaddr);
+			wrb = &pnob->tx_q[pnob->tx_q_hd];
+			if (first == NULL) {
+				wrb_fill_extra(wrb, skb, pnob);
+				first = wrb;
+			} else {
+				wrb_copy_extra(wrb, first);
+			}
+			wrb_fill(wrb, busaddr, frag->size);
+			be_adv_txq_hd(pnob);
+			*copied += frag->size;
+		}
+		skb = skb_shinfo(skb)->frag_list;
+	}
+
+	if (dummy) {
+		wrb = &pnob->tx_q[pnob->tx_q_hd];
+		BUG_ON(first == NULL);
+		wrb_copy_extra(wrb, first);
+		wrb_fill(wrb, 0, 0);
+		be_adv_txq_hd(pnob);
+	}
+	AMAP_SET_BITS_PTR(ETH_WRB, complete, wrb, 1);
+	AMAP_SET_BITS_PTR(ETH_WRB, last, wrb, 1);
+	return wrb_cnt;
+}
+
+/* For each skb transmitted, tx_ctxt stores the num of wrbs in the
+ * start index and skb pointer in the end index
+ */
+static inline void be_tx_wrb_info_remember(struct be_net_object *pnob,
+					   struct sk_buff *skb, int wrb_cnt,
+					   u32 start)
+{
+	*(u32 *) (&pnob->tx_ctxt[start]) = wrb_cnt;
+	index_adv(&start, wrb_cnt - 1, pnob->tx_q_len);
+	pnob->tx_ctxt[start] = skb;
+}
+
+static int benet_xmit(struct sk_buff *skb, struct net_device *netdev)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+	u32 wrb_cnt, copied = 0;
+	u32 start = pnob->tx_q_hd;
+
+	adapter->be_stat.bes_tx_reqs++;
+
+	wrb_cnt = wrb_cnt_in_skb(skb);
+	spin_lock_bh(&adapter->txq_lock);
+	if ((pnob->tx_q_len - 2 - atomic_read(&pnob->tx_q_used)) <= wrb_cnt) {
+		netif_stop_queue(pnob->netdev);
+		spin_unlock_bh(&adapter->txq_lock);
+		adapter->be_stat.bes_tx_fails++;
+		return NETDEV_TX_BUSY;
+	}
+	spin_unlock_bh(&adapter->txq_lock);
+
+	wrb_cnt = copy_skb_to_txq(pnob, skb, wrb_cnt, &copied);
+	be_tx_wrb_info_remember(pnob, skb, wrb_cnt, start);
+
+	be_start_tx(pnob, wrb_cnt);
+
+	adapter->eth_tx_bytes += copied;
+	adapter->be_stat.bes_tx_wrbs += wrb_cnt;
+	update_tx_rate(adapter);
+	netdev->trans_start = jiffies;
+
+	return NETDEV_TX_OK;
+}
+
+/*
+ * This is the driver entry point to change the mtu of the device
+ * Returns 0 for success and errno for failure.
+ */
+static int benet_change_mtu(struct net_device *netdev, int new_mtu)
+{
+	/*
+	 * BE supports jumbo frame size upto 9000 bytes including the link layer
+	 * header. Considering the different variants of frame formats possible
+	 * like VLAN, SNAP/LLC, the maximum possible value for MTU is 8974 bytes
+	 */
+
+	if (new_mtu < (ETH_ZLEN + ETH_FCS_LEN) || (new_mtu > BE_MAX_MTU)) {
+		dev_info(&netdev->dev, "Invalid MTU requested. "
+			       "Must be between %d and %d bytes\n",
+				       (ETH_ZLEN + ETH_FCS_LEN), BE_MAX_MTU);
+		return -EINVAL;
+	}
+	dev_info(&netdev->dev, "MTU changed from %d to %d\n",
+						netdev->mtu, new_mtu);
+	netdev->mtu = new_mtu;
+	return 0;
+}
+
+/*
+ * This is the driver entry point to register a vlan with the device
+ */
+static void benet_vlan_register(struct net_device *netdev,
+				struct vlan_group *grp)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	be_disable_eq_intr(pnob);
+	pnob->vlan_grp = grp;
+	pnob->num_vlans = 0;
+	be_enable_eq_intr(pnob);
+}
+
+/*
+ * This is the driver entry point to add a vlan vlan_id
+ * with the device netdev
+ */
+static void benet_vlan_add_vid(struct net_device *netdev, u16 vlan_id)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	if (pnob->num_vlans == (BE_NUM_VLAN_SUPPORTED - 1)) {
+		/* no  way to return an error */
+		dev_info(&netdev->dev,
+		       "BladeEngine: Cannot configure more than %d Vlans\n",
+			       BE_NUM_VLAN_SUPPORTED);
+		return;
+	}
+	/* The new vlan tag will be in the slot indicated by num_vlans. */
+	pnob->vlan_tag[pnob->num_vlans++] = vlan_id;
+	be_rxf_vlan_config(&pnob->fn_obj, false, pnob->num_vlans,
+			   pnob->vlan_tag, NULL, NULL, NULL);
+}
+
+/*
+ * This is the driver entry point to remove a vlan vlan_id
+ * with the device netdev
+ */
+static void benet_vlan_rem_vid(struct net_device *netdev, u16 vlan_id)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	u32 i, value;
+
+	/*
+	 * In Blade Engine, we support 32 vlan tag filters across both ports.
+	 * To program a vlan tag, the RXF_RTPR_CSR register is used.
+	 * Each 32-bit value of RXF_RTDR_CSR can address 2 vlan tag entries.
+	 * The Vlan table is of depth 16. thus we support 32 tags.
+	 */
+
+	value = vlan_id | VLAN_VALID_BIT;
+	for (i = 0; i < BE_NUM_VLAN_SUPPORTED; i++) {
+		if (pnob->vlan_tag[i] == vlan_id)
+			break;
+	}
+
+	if (i == BE_NUM_VLAN_SUPPORTED)
+		return;
+	/* Now compact the vlan tag array by removing hole created. */
+	while ((i + 1) < BE_NUM_VLAN_SUPPORTED) {
+		pnob->vlan_tag[i] = pnob->vlan_tag[i + 1];
+		i++;
+	}
+	if ((i + 1) == BE_NUM_VLAN_SUPPORTED)
+		pnob->vlan_tag[i] = (u16) 0x0;
+	pnob->num_vlans--;
+	be_rxf_vlan_config(&pnob->fn_obj, false, pnob->num_vlans,
+			   pnob->vlan_tag, NULL, NULL, NULL);
+}
+
+/*
+ * This function is called to program multicast
+ * address in the multicast filter of the ASIC.
+ */
+static void be_set_multicast_filter(struct net_device *netdev)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct dev_mc_list *mc_ptr;
+	u8 mac_addr[32][ETH_ALEN];
+	int i;
+
+	if (netdev->flags & IFF_ALLMULTI) {
+		/* set BE in Multicast promiscuous */
+		be_rxf_multicast_config(&pnob->fn_obj, true, 0, NULL, NULL,
+					NULL, NULL);
+		return;
+	}
+
+	for (mc_ptr = netdev->mc_list, i = 0; mc_ptr;
+	     mc_ptr = mc_ptr->next, i++) {
+		memcpy(&mac_addr[i][0], mc_ptr->dmi_addr, ETH_ALEN);
+	}
+
+	/* reset the promiscuous mode also. */
+	be_rxf_multicast_config(&pnob->fn_obj, false, i,
+				&mac_addr[0][0], NULL, NULL, NULL);
+}
+
+/*
+ * This is the driver entry point to set multicast list
+ * with the device netdev. This function will be used to
+ * set promiscuous mode or multicast promiscuous mode
+ * or multicast mode....
+ */
+static void benet_set_multicast_list(struct net_device *netdev)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	if (netdev->flags & IFF_PROMISC) {
+		be_rxf_promiscuous(&pnob->fn_obj, 1, 1, NULL, NULL, NULL);
+	} else {
+		be_rxf_promiscuous(&pnob->fn_obj, 0, 0, NULL, NULL, NULL);
+		be_set_multicast_filter(netdev);
+	}
+}
+
+int benet_init(struct net_device *netdev)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+
+	ether_setup(netdev);
+
+	netdev->open = &benet_open;
+	netdev->stop = &benet_close;
+	netdev->hard_start_xmit = &benet_xmit;
+
+	netdev->get_stats = &benet_get_stats;
+
+	netdev->set_multicast_list = &benet_set_multicast_list;
+
+	netdev->change_mtu = &benet_change_mtu;
+	netdev->set_mac_address = &benet_set_mac_addr;
+
+	netdev->vlan_rx_register = benet_vlan_register;
+	netdev->vlan_rx_add_vid = benet_vlan_add_vid;
+	netdev->vlan_rx_kill_vid = benet_vlan_rem_vid;
+
+	netdev->features =
+	    NETIF_F_SG | NETIF_F_HIGHDMA | NETIF_F_HW_VLAN_RX | NETIF_F_TSO |
+	    NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_FILTER | NETIF_F_IP_CSUM;
+
+	netdev->flags |= IFF_MULTICAST;
+
+	/* If device is DAC Capable, set the HIGHDMA flag for netdevice. */
+	if (adapter->dma_64bit_cap)
+		netdev->features |= NETIF_F_HIGHDMA;
+
+	SET_ETHTOOL_OPS(netdev, &be_ethtool_ops);
+	return 0;
+}
diff --git a/drivers/staging/benet/benet.h b/drivers/staging/benet/benet.h
new file mode 100644
index 000000000000..09a1f0817722
--- /dev/null
+++ b/drivers/staging/benet/benet.h
@@ -0,0 +1,429 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#ifndef _BENET_H_
+#define _BENET_H_
+
+#include <linux/pci.h>
+#include <linux/netdevice.h>
+#include <linux/inet_lro.h>
+#include "hwlib.h"
+
+#define _SA_MODULE_NAME "net-driver"
+
+#define VLAN_VALID_BIT		0x8000
+#define BE_NUM_VLAN_SUPPORTED	32
+#define BE_PORT_LINK_DOWN       0000
+#define BE_PORT_LINK_UP         0001
+#define	BE_MAX_TX_FRAG_COUNT		(30)
+
+/* Flag bits for send operation */
+#define IPCS            (1 << 0)	/* Enable IP checksum offload */
+#define UDPCS           (1 << 1)	/* Enable UDP checksum offload */
+#define TCPCS           (1 << 2)	/* Enable TCP checksum offload */
+#define LSO             (1 << 3)	/* Enable Large Segment  offload */
+#define ETHVLAN         (1 << 4)	/* Enable VLAN insert */
+#define ETHEVENT        (1 << 5)	/* Generate  event on completion */
+#define ETHCOMPLETE     (1 << 6)	/* Generate completion when done */
+#define IPSEC           (1 << 7)	/* Enable IPSEC */
+#define FORWARD         (1 << 8)	/* Send the packet in forwarding path */
+#define FIN             (1 << 9)	/* Issue FIN segment */
+
+#define BE_MAX_MTU	8974
+
+#define BE_MAX_LRO_DESCRIPTORS			8
+#define BE_LRO_MAX_PKTS				64
+#define BE_MAX_FRAGS_PER_FRAME			6
+
+extern const char be_drvr_ver[];
+extern char be_fw_ver[];
+extern char be_driver_name[];
+
+extern struct ethtool_ops be_ethtool_ops;
+
+#define BE_DEV_STATE_NONE 0
+#define BE_DEV_STATE_INIT 1
+#define BE_DEV_STATE_OPEN 2
+#define BE_DEV_STATE_SUSPEND 3
+
+/* This structure is used to describe physical fragments to use
+ * for DMAing data from NIC.
+ */
+struct be_recv_buffer {
+	struct list_head rxb_list;	/* for maintaining a linked list */
+	void *rxb_va;		/* buffer virtual address */
+	u32 rxb_pa_lo;		/* low part of physical address */
+	u32 rxb_pa_hi;		/* high part of physical address */
+	u32 rxb_len;		/* length of recv buffer */
+	void *rxb_ctxt;		/* context for OSM driver to use */
+};
+
+/*
+ * fragment list to describe scattered data.
+ */
+struct be_tx_frag_list {
+	u32 txb_len;		/* Size of this fragment */
+	u32 txb_pa_lo;		/* Lower 32 bits of 64 bit physical addr */
+	u32 txb_pa_hi;		/* Higher 32 bits of 64 bit physical addr */
+};
+
+struct be_rx_page_info {
+	struct page *page;
+	dma_addr_t bus;
+	u16 page_offset;
+};
+
+/*
+ *  This structure is the main tracking structure for a NIC interface.
+ */
+struct be_net_object {
+	/* MCC Ring - used to send fwcmds to embedded ARM processor */
+	struct MCC_WRB_AMAP *mcc_q;	/* VA of the start of the ring */
+	u32 mcc_q_len;			/* # of WRB entries in this ring */
+	u32 mcc_q_size;
+	u32 mcc_q_hd;			/* MCC ring head */
+	u8 mcc_q_created;		/* flag to help cleanup */
+	struct be_mcc_object mcc_q_obj;	/* BECLIB's MCC ring Object */
+	dma_addr_t mcc_q_bus;		/* DMA'ble bus address */
+
+	/* MCC Completion Ring - FW responses to fwcmds sent from MCC ring */
+	struct MCC_CQ_ENTRY_AMAP *mcc_cq; /* VA of the start of the ring */
+	u32 mcc_cq_len;			/* # of compl. entries in this ring */
+	u32 mcc_cq_size;
+	u32 mcc_cq_tl;			/* compl. ring tail */
+	u8 mcc_cq_created;		/* flag to help cleanup */
+	struct be_cq_object mcc_cq_obj;	/* BECLIB's MCC compl. ring object */
+	u32 mcc_cq_id;			/* MCC ring ID */
+	dma_addr_t mcc_cq_bus;		/* DMA'ble bus address */
+
+	struct ring_desc mb_rd;		/* RD for MCC_MAIL_BOX */
+	void *mb_ptr;			/* mailbox ptr to be freed  */
+	dma_addr_t mb_bus;		/* DMA'ble bus address */
+	u32 mb_size;
+
+	/* BEClib uses an array of context objects to track outstanding
+	 * requests to the MCC.  We need allocate the same number of
+	 * conext entries as the number of entries in the MCC WRB ring
+	 */
+	u32 mcc_wrb_ctxt_size;
+	void *mcc_wrb_ctxt;		/* pointer to the context area */
+	u32 mcc_wrb_ctxtLen;		/* Number of entries in the context */
+	/*
+	 * NIC send request ring - used for xmitting raw ether frames.
+	 */
+	struct ETH_WRB_AMAP *tx_q;	/* VA of the start of the ring */
+	u32 tx_q_len;			/* # if entries in the send ring */
+	u32 tx_q_size;
+	u32 tx_q_hd;			/* Head index. Next req. goes here */
+	u32 tx_q_tl;			/* Tail indx. oldest outstanding req. */
+	u8 tx_q_created;		/* flag to help cleanup */
+	struct be_ethsq_object tx_q_obj;/* BECLIB's send Q handle */
+	dma_addr_t tx_q_bus;		/* DMA'ble bus address */
+	u32 tx_q_id;			/* send queue ring ID */
+	u32 tx_q_port;			/* 0 no binding, 1 port A,  2 port B */
+	atomic_t tx_q_used;		/* # of WRBs used */
+	/* ptr to an array in which we store context info for each send req. */
+	void **tx_ctxt;
+	/*
+	 * NIC Send compl. ring - completion status for all NIC frames xmitted.
+	 */
+	struct ETH_TX_COMPL_AMAP *tx_cq;/* VA of start of the ring */
+	u32 txcq_len;			/* # of entries in the ring */
+	u32 tx_cq_size;
+	/*
+	 * index into compl ring where the host expects next completion entry
+	 */
+	u32 tx_cq_tl;
+	u32 tx_cq_id;			/* completion queue id */
+	u8 tx_cq_created;		/* flag to help cleanup */
+	struct be_cq_object tx_cq_obj;
+	dma_addr_t tx_cq_bus;		/* DMA'ble bus address */
+	/*
+	 * Event Queue - all completion entries post events here.
+	 */
+	struct EQ_ENTRY_AMAP *event_q;	/* VA of start of event queue */
+	u32 event_q_len;		/* # of entries */
+	u32 event_q_size;
+	u32 event_q_tl;			/* Tail of the event queue */
+	u32 event_q_id;			/* Event queue ID */
+	u8 event_q_created;		/* flag to help cleanup */
+	struct be_eq_object event_q_obj; /* Queue handle */
+	dma_addr_t event_q_bus;		/* DMA'ble bus address */
+	/*
+	 * NIC receive queue - Data buffers to be used for receiving unicast,
+	 * broadcast and multi-cast frames  are posted here.
+	 */
+	struct ETH_RX_D_AMAP *rx_q;	/* VA of start of the queue */
+	u32 rx_q_len;			/* # of entries */
+	u32 rx_q_size;
+	u32 rx_q_hd;			/* Head of the queue */
+	atomic_t rx_q_posted;		/* number of posted buffers */
+	u32 rx_q_id;			/* queue ID */
+	u8 rx_q_created;		/* flag to help cleanup */
+	struct be_ethrq_object rx_q_obj;	/* NIC RX queue handle */
+	dma_addr_t rx_q_bus;		/* DMA'ble bus address */
+	/*
+	 * Pointer to an array of opaque context object for use by OSM driver
+	 */
+	void **rx_ctxt;
+	/*
+	 * NIC unicast RX completion queue - all unicast ether frame completion
+	 * statuses from BE come here.
+	 */
+	struct ETH_RX_COMPL_AMAP *rx_cq;	/* VA of start of the queue */
+	u32 rx_cq_len;		/* # of entries */
+	u32 rx_cq_size;
+	u32 rx_cq_tl;			/* Tail of the queue */
+	u32 rx_cq_id;			/* queue ID */
+	u8 rx_cq_created;		/* flag to help cleanup */
+	struct be_cq_object rx_cq_obj;	/* queue handle */
+	dma_addr_t rx_cq_bus;		/* DMA'ble bus address */
+	struct be_function_object fn_obj;	/* function object   */
+	bool	fn_obj_created;
+	u32 rx_buf_size;		/* Size of the RX buffers */
+
+	struct net_device *netdev;
+	struct be_recv_buffer eth_rx_bufs[256];	/* to pass Rx buffer
+							   addresses */
+	struct be_adapter *adapter;	/* Pointer to OSM adapter */
+	u32 devno;		/* OSM, network dev no. */
+	u32 use_port;		/* Current active port */
+	struct be_rx_page_info *rx_page_info;	/* Array of Rx buf pages */
+	u32 rx_pg_info_hd;	/* Head of queue */
+	int rxbuf_post_fail;	/* RxBuff posting fail count */
+	bool rx_pg_shared;	/* Is an allocsted page shared as two frags ? */
+	struct vlan_group *vlan_grp;
+	u32 num_vlans;		/* Number of vlans in BE's filter */
+	u16 vlan_tag[BE_NUM_VLAN_SUPPORTED]; /* vlans currently configured */
+	struct napi_struct napi;
+	struct net_lro_mgr lro_mgr;
+	struct net_lro_desc lro_desc[BE_MAX_LRO_DESCRIPTORS];
+};
+
+#define NET_FH(np)       (&(np)->fn_obj)
+
+/*
+ * BE driver statistics.
+ */
+struct be_drvr_stat {
+	u32 bes_tx_reqs;	/* number of TX requests initiated */
+	u32 bes_tx_fails;	/* number of TX requests that failed */
+	u32 bes_fwd_reqs;	/* number of send reqs through forwarding i/f */
+	u32 bes_tx_wrbs;	/* number of tx WRBs used */
+
+	u32 bes_ints;		/* number of interrupts */
+	u32 bes_polls;		/* number of times NAPI called poll function */
+	u32 bes_events;		/* total evet entries processed */
+	u32 bes_tx_events;	/* number of tx completion events  */
+	u32 bes_rx_events;	/* number of ucast rx completion events  */
+	u32 bes_tx_compl;	/* number of tx completion entries processed */
+	u32 bes_rx_compl;	/* number of rx completion entries
+				   processed */
+	u32 bes_ethrx_post_fail;	/* number of ethrx buffer alloc
+					   failures */
+	/*
+	 * number of non ether type II frames dropped where
+	 * frame len > length field of Mac Hdr
+	 */
+	u32 bes_802_3_dropped_frames;
+	/*
+	 * number of non ether type II frames malformed where
+	 * in frame len < length field of Mac Hdr
+	 */
+	u32 bes_802_3_malformed_frames;
+	u32 bes_ips;		/*  interrupts / sec */
+	u32 bes_prev_ints;	/* bes_ints at last IPS calculation  */
+	u16 bes_eth_tx_rate;	/*  ETH TX rate - Mb/sec */
+	u16 bes_eth_rx_rate;	/*  ETH RX rate - Mb/sec */
+	u32 bes_rx_coal;	/* Num pkts coalasced */
+	u32 bes_rx_flush;	/* Num times coalasced */
+	u32 bes_link_change_physical;	/*Num of times physical link changed */
+	u32 bes_link_change_virtual;	/*Num of times virtual link changed */
+	u32 bes_rx_misc_pkts;	/* Misc pkts received */
+};
+
+/* Maximum interrupt delay (in microseconds) allowed */
+#define MAX_EQD				120
+
+/*
+ * timer to prevent system shutdown hang for ever if h/w stops responding
+ */
+struct be_timer_ctxt {
+	atomic_t get_stat_flag;
+	struct timer_list get_stats_timer;
+	unsigned long get_stat_sem_addr;
+} ;
+
+/* This structure is the main BladeEngine driver context.  */
+struct be_adapter {
+	struct net_device *netdevp;
+	struct be_drvr_stat be_stat;
+	struct net_device_stats benet_stats;
+
+	/* PCI BAR mapped addresses */
+	u8 __iomem *csr_va;	/* CSR */
+	u8 __iomem *db_va;	/* Door  Bell  */
+	u8 __iomem *pci_va;	/* PCI Config */
+
+	struct tasklet_struct sts_handler;
+	struct timer_list cq_timer;
+	spinlock_t int_lock;	/* to protect the isr field in adapter */
+
+	struct FWCMD_ETH_GET_STATISTICS *eth_statsp;
+	/*
+	 * This will enable the use of ethtool to enable or disable
+	 * Checksum on Rx pkts to be obeyed or disobeyed.
+	 * If this is true = 1, then whatever is the checksum on the
+	 * Received pkt as per BE, it will be given to the stack.
+	 * Else the stack will re calculate it.
+	 */
+	bool rx_csum;
+	/*
+	 * This will enable the use of ethtool to enable or disable
+	 * Coalese on Rx pkts to be obeyed or disobeyed.
+	 * If this is grater than 0 and less than 16 then coalascing
+	 * is enabled else it is disabled
+	 */
+	u32 max_rx_coal;
+	struct pci_dev *pdev;	/* Pointer to OS's PCI dvice */
+
+	spinlock_t txq_lock;	/* to stop/wake queue based on tx_q_used */
+
+	u32 isr;		/* copy of Intr status reg. */
+
+	u32 port0_link_sts;	/* Port 0 link status */
+	u32 port1_link_sts;	/* port 1 list status */
+	struct BE_LINK_STATUS *be_link_sts;
+
+	/* pointer to the first netobject of this adapter */
+	struct be_net_object *net_obj;
+
+	/*  Flags to indicate what to clean up */
+	bool tasklet_started;
+	bool isr_registered;
+	/*
+	 * adaptive interrupt coalescing (AIC) related
+	 */
+	bool enable_aic;	/* 1 if AIC is enabled */
+	u16 min_eqd;		/* minimum EQ delay in usec */
+	u16 max_eqd;		/* minimum EQ delay in usec */
+	u16 cur_eqd;		/* current EQ delay in usec */
+	/*
+	 * book keeping for interrupt / sec and TX/RX rate calculation
+	 */
+	ulong ips_jiffies;	/* jiffies at last IPS calc */
+	u32 eth_tx_bytes;
+	ulong eth_tx_jiffies;
+	u32 eth_rx_bytes;
+	ulong eth_rx_jiffies;
+
+	struct semaphore get_eth_stat_sem;
+
+	/* timer ctxt to prevent shutdown hanging due to un-responsive BE */
+	struct be_timer_ctxt timer_ctxt;
+
+#define BE_MAX_MSIX_VECTORS             32
+#define BE_MAX_REQ_MSIX_VECTORS         1 /* only one EQ in Linux driver */
+	struct msix_entry msix_entries[BE_MAX_MSIX_VECTORS];
+	bool msix_enabled;
+	bool dma_64bit_cap;	/* the Device DAC capable  or not */
+	u8 dev_state;	/* The current state of the device */
+	u8 dev_pm_state; /* The State of device before going to suspend */
+};
+
+/*
+ * Every second we look at the ints/sec and adjust eq_delay
+ * between adapter->min_eqd and adapter->max_eqd to keep the ints/sec between
+ * IPS_HI_WM and IPS_LO_WM.
+ */
+#define IPS_HI_WM	18000
+#define IPS_LO_WM	8000
+
+
+static inline void index_adv(u32 *index, u32 val,  u32 limit)
+{
+	BUG_ON(limit & (limit-1));
+	*index = (*index + val) & (limit - 1);
+}
+
+static inline void index_inc(u32 *index, u32 limit)
+{
+	BUG_ON(limit & (limit-1));
+	*index = (*index + 1) & (limit - 1);
+}
+
+static inline void be_adv_eq_tl(struct be_net_object *pnob)
+{
+	index_inc(&pnob->event_q_tl, pnob->event_q_len);
+}
+
+static inline void be_adv_txq_hd(struct be_net_object *pnob)
+{
+	index_inc(&pnob->tx_q_hd, pnob->tx_q_len);
+}
+
+static inline void be_adv_txq_tl(struct be_net_object *pnob)
+{
+	index_inc(&pnob->tx_q_tl, pnob->tx_q_len);
+}
+
+static inline void be_adv_txcq_tl(struct be_net_object *pnob)
+{
+	index_inc(&pnob->tx_cq_tl, pnob->txcq_len);
+}
+
+static inline void be_adv_rxq_hd(struct be_net_object *pnob)
+{
+	index_inc(&pnob->rx_q_hd, pnob->rx_q_len);
+}
+
+static inline void be_adv_rxcq_tl(struct be_net_object *pnob)
+{
+	index_inc(&pnob->rx_cq_tl, pnob->rx_cq_len);
+}
+
+static inline u32 tx_compl_lastwrb_idx_get(struct be_net_object *pnob)
+{
+	return (pnob->tx_q_tl + *(u32 *)&pnob->tx_ctxt[pnob->tx_q_tl] - 1)
+		    & (pnob->tx_q_len - 1);
+}
+
+int benet_init(struct net_device *);
+int be_ethtool_ioctl(struct net_device *, struct ifreq *);
+struct net_device_stats *benet_get_stats(struct net_device *);
+void be_process_intr(unsigned long context);
+irqreturn_t be_int(int irq, void *dev);
+void be_post_eth_rx_buffs(struct be_net_object *);
+void be_get_stat_cb(void *, int, struct MCC_WRB_AMAP *);
+void be_get_stats_timer_handler(unsigned long);
+void be_wait_nic_tx_cmplx_cmpl(struct be_net_object *);
+void be_print_link_info(struct BE_LINK_STATUS *);
+void be_update_link_status(struct be_adapter *);
+void be_init_procfs(struct be_adapter *);
+void be_cleanup_procfs(struct be_adapter *);
+int be_poll(struct napi_struct *, int);
+struct ETH_RX_COMPL_AMAP *be_get_rx_cmpl(struct be_net_object *);
+void be_notify_cmpl(struct be_net_object *, int, int, int);
+void be_enable_intr(struct be_net_object *);
+void be_enable_eq_intr(struct be_net_object *);
+void be_disable_intr(struct be_net_object *);
+void be_disable_eq_intr(struct be_net_object *);
+int be_set_uc_mac_adr(struct be_net_object *, u8, u8, u8,
+		    u8 *, mcc_wrb_cqe_callback, void *);
+int be_get_flow_ctl(struct be_function_object *pFnObj, bool *, bool *);
+void process_one_tx_compl(struct be_net_object *pnob, u32 end_idx);
+
+#endif /* _BENET_H_ */
diff --git a/drivers/staging/benet/bestatus.h b/drivers/staging/benet/bestatus.h
new file mode 100644
index 000000000000..59c7a4b62223
--- /dev/null
+++ b/drivers/staging/benet/bestatus.h
@@ -0,0 +1,103 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#ifndef _BESTATUS_H_
+#define _BESTATUS_H_
+
+#define BE_SUCCESS                      (0x00000000L)
+/*
+ * MessageId: BE_PENDING
+ *  The BladeEngine Driver call succeeded, and pended operation.
+ */
+#define BE_PENDING                       (0x20070001L)
+#define BE_STATUS_PENDING                (BE_PENDING)
+/*
+ * MessageId: BE_NOT_OK
+ *  An error occurred.
+ */
+#define BE_NOT_OK                        (0xE0070002L)
+/*
+ * MessageId: BE_STATUS_SYSTEM_RESOURCES
+ *  Insufficient host system resources exist to complete the API.
+ */
+#define BE_STATUS_SYSTEM_RESOURCES       (0xE0070003L)
+/*
+ * MessageId: BE_STATUS_CHIP_RESOURCES
+ *  Insufficient chip resources exist to complete the API.
+ */
+#define BE_STATUS_CHIP_RESOURCES         (0xE0070004L)
+/*
+ * MessageId: BE_STATUS_NO_RESOURCE
+ *  Insufficient resources to complete request.
+ */
+#define BE_STATUS_NO_RESOURCE            (0xE0070005L)
+/*
+ * MessageId: BE_STATUS_BUSY
+ *  Resource is currently busy.
+ */
+#define BE_STATUS_BUSY                   (0xE0070006L)
+/*
+ * MessageId: BE_STATUS_INVALID_PARAMETER
+ *  Invalid Parameter in request.
+ */
+#define BE_STATUS_INVALID_PARAMETER      (0xE0000007L)
+/*
+ * MessageId: BE_STATUS_NOT_SUPPORTED
+ *  Requested operation is not supported.
+ */
+#define BE_STATUS_NOT_SUPPORTED          (0xE000000DL)
+
+/*
+ * ***************************************************************************
+ *                     E T H E R N E T   S T A T U S
+ * ***************************************************************************
+ */
+
+/*
+ * MessageId: BE_ETH_TX_ERROR
+ *  The Ethernet device driver failed to transmit a packet.
+ */
+#define BE_ETH_TX_ERROR                  (0xE0070101L)
+
+/*
+ * ***************************************************************************
+ *                     S H A R E D   S T A T U S
+ * ***************************************************************************
+ */
+
+/*
+ * MessageId: BE_STATUS_VBD_INVALID_VERSION
+ *  The device driver is not compatible with this version of the VBD.
+ */
+#define BE_STATUS_INVALID_VERSION    (0xE0070402L)
+/*
+ * MessageId: BE_STATUS_DOMAIN_DENIED
+ *  The operation failed to complete due to insufficient access
+ *  rights for the requesting domain.
+ */
+#define BE_STATUS_DOMAIN_DENIED          (0xE0070403L)
+/*
+ * MessageId: BE_STATUS_TCP_NOT_STARTED
+ *  The embedded TCP/IP stack has not been started.
+ */
+#define BE_STATUS_TCP_NOT_STARTED        (0xE0070409L)
+/*
+ * MessageId: BE_STATUS_NO_MCC_WRB
+ *  No free MCC WRB are available for posting the request.
+ */
+#define BE_STATUS_NO_MCC_WRB                 (0xE0070414L)
+
+#endif /* _BESTATUS_ */
diff --git a/drivers/staging/benet/cev.h b/drivers/staging/benet/cev.h
new file mode 100644
index 000000000000..30996920a544
--- /dev/null
+++ b/drivers/staging/benet/cev.h
@@ -0,0 +1,243 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __cev_amap_h__
+#define __cev_amap_h__
+#include "ep.h"
+
+/*
+ * Host Interrupt Status Register 0. The first of four application
+ * interrupt status registers. This register contains the interrupts
+ * for Event Queues EQ0 through EQ31.
+ */
+struct BE_CEV_ISR0_CSR_AMAP {
+	u8 interrupt0;	/* DWORD 0 */
+	u8 interrupt1;	/* DWORD 0 */
+	u8 interrupt2;	/* DWORD 0 */
+	u8 interrupt3;	/* DWORD 0 */
+	u8 interrupt4;	/* DWORD 0 */
+	u8 interrupt5;	/* DWORD 0 */
+	u8 interrupt6;	/* DWORD 0 */
+	u8 interrupt7;	/* DWORD 0 */
+	u8 interrupt8;	/* DWORD 0 */
+	u8 interrupt9;	/* DWORD 0 */
+	u8 interrupt10;	/* DWORD 0 */
+	u8 interrupt11;	/* DWORD 0 */
+	u8 interrupt12;	/* DWORD 0 */
+	u8 interrupt13;	/* DWORD 0 */
+	u8 interrupt14;	/* DWORD 0 */
+	u8 interrupt15;	/* DWORD 0 */
+	u8 interrupt16;	/* DWORD 0 */
+	u8 interrupt17;	/* DWORD 0 */
+	u8 interrupt18;	/* DWORD 0 */
+	u8 interrupt19;	/* DWORD 0 */
+	u8 interrupt20;	/* DWORD 0 */
+	u8 interrupt21;	/* DWORD 0 */
+	u8 interrupt22;	/* DWORD 0 */
+	u8 interrupt23;	/* DWORD 0 */
+	u8 interrupt24;	/* DWORD 0 */
+	u8 interrupt25;	/* DWORD 0 */
+	u8 interrupt26;	/* DWORD 0 */
+	u8 interrupt27;	/* DWORD 0 */
+	u8 interrupt28;	/* DWORD 0 */
+	u8 interrupt29;	/* DWORD 0 */
+	u8 interrupt30;	/* DWORD 0 */
+	u8 interrupt31;	/* DWORD 0 */
+} __packed;
+struct CEV_ISR0_CSR_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * Host Interrupt Status Register 1. The second of four application
+ * interrupt status registers. This register contains the interrupts
+ * for Event Queues EQ32 through EQ63.
+ */
+struct BE_CEV_ISR1_CSR_AMAP {
+	u8 interrupt32;	/* DWORD 0 */
+	u8 interrupt33;	/* DWORD 0 */
+	u8 interrupt34;	/* DWORD 0 */
+	u8 interrupt35;	/* DWORD 0 */
+	u8 interrupt36;	/* DWORD 0 */
+	u8 interrupt37;	/* DWORD 0 */
+	u8 interrupt38;	/* DWORD 0 */
+	u8 interrupt39;	/* DWORD 0 */
+	u8 interrupt40;	/* DWORD 0 */
+	u8 interrupt41;	/* DWORD 0 */
+	u8 interrupt42;	/* DWORD 0 */
+	u8 interrupt43;	/* DWORD 0 */
+	u8 interrupt44;	/* DWORD 0 */
+	u8 interrupt45;	/* DWORD 0 */
+	u8 interrupt46;	/* DWORD 0 */
+	u8 interrupt47;	/* DWORD 0 */
+	u8 interrupt48;	/* DWORD 0 */
+	u8 interrupt49;	/* DWORD 0 */
+	u8 interrupt50;	/* DWORD 0 */
+	u8 interrupt51;	/* DWORD 0 */
+	u8 interrupt52;	/* DWORD 0 */
+	u8 interrupt53;	/* DWORD 0 */
+	u8 interrupt54;	/* DWORD 0 */
+	u8 interrupt55;	/* DWORD 0 */
+	u8 interrupt56;	/* DWORD 0 */
+	u8 interrupt57;	/* DWORD 0 */
+	u8 interrupt58;	/* DWORD 0 */
+	u8 interrupt59;	/* DWORD 0 */
+	u8 interrupt60;	/* DWORD 0 */
+	u8 interrupt61;	/* DWORD 0 */
+	u8 interrupt62;	/* DWORD 0 */
+	u8 interrupt63;	/* DWORD 0 */
+} __packed;
+struct CEV_ISR1_CSR_AMAP {
+	u32 dw[1];
+};
+/*
+ * Host Interrupt Status Register 2. The third of four application
+ * interrupt status registers. This register contains the interrupts
+ * for Event Queues EQ64 through EQ95.
+ */
+struct BE_CEV_ISR2_CSR_AMAP {
+	u8 interrupt64;	/* DWORD 0 */
+	u8 interrupt65;	/* DWORD 0 */
+	u8 interrupt66;	/* DWORD 0 */
+	u8 interrupt67;	/* DWORD 0 */
+	u8 interrupt68;	/* DWORD 0 */
+	u8 interrupt69;	/* DWORD 0 */
+	u8 interrupt70;	/* DWORD 0 */
+	u8 interrupt71;	/* DWORD 0 */
+	u8 interrupt72;	/* DWORD 0 */
+	u8 interrupt73;	/* DWORD 0 */
+	u8 interrupt74;	/* DWORD 0 */
+	u8 interrupt75;	/* DWORD 0 */
+	u8 interrupt76;	/* DWORD 0 */
+	u8 interrupt77;	/* DWORD 0 */
+	u8 interrupt78;	/* DWORD 0 */
+	u8 interrupt79;	/* DWORD 0 */
+	u8 interrupt80;	/* DWORD 0 */
+	u8 interrupt81;	/* DWORD 0 */
+	u8 interrupt82;	/* DWORD 0 */
+	u8 interrupt83;	/* DWORD 0 */
+	u8 interrupt84;	/* DWORD 0 */
+	u8 interrupt85;	/* DWORD 0 */
+	u8 interrupt86;	/* DWORD 0 */
+	u8 interrupt87;	/* DWORD 0 */
+	u8 interrupt88;	/* DWORD 0 */
+	u8 interrupt89;	/* DWORD 0 */
+	u8 interrupt90;	/* DWORD 0 */
+	u8 interrupt91;	/* DWORD 0 */
+	u8 interrupt92;	/* DWORD 0 */
+	u8 interrupt93;	/* DWORD 0 */
+	u8 interrupt94;	/* DWORD 0 */
+	u8 interrupt95;	/* DWORD 0 */
+} __packed;
+struct CEV_ISR2_CSR_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * Host Interrupt Status Register 3. The fourth of four application
+ * interrupt status registers. This register contains the interrupts
+ * for Event Queues EQ96 through EQ127.
+ */
+struct BE_CEV_ISR3_CSR_AMAP {
+	u8 interrupt96;	/* DWORD 0 */
+	u8 interrupt97;	/* DWORD 0 */
+	u8 interrupt98;	/* DWORD 0 */
+	u8 interrupt99;	/* DWORD 0 */
+	u8 interrupt100;	/* DWORD 0 */
+	u8 interrupt101;	/* DWORD 0 */
+	u8 interrupt102;	/* DWORD 0 */
+	u8 interrupt103;	/* DWORD 0 */
+	u8 interrupt104;	/* DWORD 0 */
+	u8 interrupt105;	/* DWORD 0 */
+	u8 interrupt106;	/* DWORD 0 */
+	u8 interrupt107;	/* DWORD 0 */
+	u8 interrupt108;	/* DWORD 0 */
+	u8 interrupt109;	/* DWORD 0 */
+	u8 interrupt110;	/* DWORD 0 */
+	u8 interrupt111;	/* DWORD 0 */
+	u8 interrupt112;	/* DWORD 0 */
+	u8 interrupt113;	/* DWORD 0 */
+	u8 interrupt114;	/* DWORD 0 */
+	u8 interrupt115;	/* DWORD 0 */
+	u8 interrupt116;	/* DWORD 0 */
+	u8 interrupt117;	/* DWORD 0 */
+	u8 interrupt118;	/* DWORD 0 */
+	u8 interrupt119;	/* DWORD 0 */
+	u8 interrupt120;	/* DWORD 0 */
+	u8 interrupt121;	/* DWORD 0 */
+	u8 interrupt122;	/* DWORD 0 */
+	u8 interrupt123;	/* DWORD 0 */
+	u8 interrupt124;	/* DWORD 0 */
+	u8 interrupt125;	/* DWORD 0 */
+	u8 interrupt126;	/* DWORD 0 */
+	u8 interrupt127;	/* DWORD 0 */
+} __packed;
+struct CEV_ISR3_CSR_AMAP {
+	u32 dw[1];
+};
+
+/*  Completions and Events block Registers.  */
+struct BE_CEV_CSRMAP_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[32];	/* DWORD 1 */
+	u8 rsvd2[32];	/* DWORD 2 */
+	u8 rsvd3[32];	/* DWORD 3 */
+	struct BE_CEV_ISR0_CSR_AMAP isr0;
+	struct BE_CEV_ISR1_CSR_AMAP isr1;
+	struct BE_CEV_ISR2_CSR_AMAP isr2;
+	struct BE_CEV_ISR3_CSR_AMAP isr3;
+	u8 rsvd4[32];	/* DWORD 8 */
+	u8 rsvd5[32];	/* DWORD 9 */
+	u8 rsvd6[32];	/* DWORD 10 */
+	u8 rsvd7[32];	/* DWORD 11 */
+	u8 rsvd8[32];	/* DWORD 12 */
+	u8 rsvd9[32];	/* DWORD 13 */
+	u8 rsvd10[32];	/* DWORD 14 */
+	u8 rsvd11[32];	/* DWORD 15 */
+	u8 rsvd12[32];	/* DWORD 16 */
+	u8 rsvd13[32];	/* DWORD 17 */
+	u8 rsvd14[32];	/* DWORD 18 */
+	u8 rsvd15[32];	/* DWORD 19 */
+	u8 rsvd16[32];	/* DWORD 20 */
+	u8 rsvd17[32];	/* DWORD 21 */
+	u8 rsvd18[32];	/* DWORD 22 */
+	u8 rsvd19[32];	/* DWORD 23 */
+	u8 rsvd20[32];	/* DWORD 24 */
+	u8 rsvd21[32];	/* DWORD 25 */
+	u8 rsvd22[32];	/* DWORD 26 */
+	u8 rsvd23[32];	/* DWORD 27 */
+	u8 rsvd24[32];	/* DWORD 28 */
+	u8 rsvd25[32];	/* DWORD 29 */
+	u8 rsvd26[32];	/* DWORD 30 */
+	u8 rsvd27[32];	/* DWORD 31 */
+	u8 rsvd28[32];	/* DWORD 32 */
+	u8 rsvd29[32];	/* DWORD 33 */
+	u8 rsvd30[192];	/* DWORD 34 */
+	u8 rsvd31[192];	/* DWORD 40 */
+	u8 rsvd32[160];	/* DWORD 46 */
+	u8 rsvd33[160];	/* DWORD 51 */
+	u8 rsvd34[160];	/* DWORD 56 */
+	u8 rsvd35[96];	/* DWORD 61 */
+	u8 rsvd36[192][32];	/* DWORD 64 */
+} __packed;
+struct CEV_CSRMAP_AMAP {
+	u32 dw[256];
+};
+
+#endif /* __cev_amap_h__ */
diff --git a/drivers/staging/benet/cq.c b/drivers/staging/benet/cq.c
new file mode 100644
index 000000000000..650458645433
--- /dev/null
+++ b/drivers/staging/benet/cq.c
@@ -0,0 +1,211 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#include "hwlib.h"
+#include "bestatus.h"
+
+/*
+ * Completion Queue Objects
+ */
+/*
+ *============================================================================
+ *                  P U B L I C  R O U T I N E S
+ *============================================================================
+ */
+
+/*
+    This routine creates a completion queue based on the client completion
+    queue configuration information.
+
+
+    FunctionObject      - Handle to a function object
+    CqBaseVa            - Base VA for a the CQ ring
+    NumEntries          - CEV_CQ_CNT_* values
+    solEventEnable      - 0 = All CQEs can generate Events if CQ is eventable
+			1 = only CQEs with solicited bit set are eventable
+    eventable           - Eventable CQ, generates interrupts.
+    nodelay             - 1 = Force interrupt, relevent if CQ eventable.
+			Interrupt is asserted immediately after EQE
+			write is confirmed, regardless of EQ Timer
+			or watermark settings.
+    wme                 - Enable watermark based coalescing
+    wmThresh            - High watermark(CQ fullness at which event
+			or interrupt should be asserted).  These are the
+			CEV_WATERMARK encoded values.
+    EqObject            - EQ Handle to assign to this CQ
+    ppCqObject          - Internal CQ Handle returned.
+
+    Returns BE_SUCCESS if successfull, otherwise a useful error code is
+	returned.
+
+    IRQL < DISPATCH_LEVEL
+
+*/
+int be_cq_create(struct be_function_object *pfob,
+	struct ring_desc *rd, u32 length, bool solicited_eventable,
+	bool no_delay, u32 wm_thresh,
+	struct be_eq_object *eq_object, struct be_cq_object *cq_object)
+{
+	int status = BE_SUCCESS;
+	u32 num_entries_encoding;
+	u32 num_entries = length / sizeof(struct MCC_CQ_ENTRY_AMAP);
+	struct FWCMD_COMMON_CQ_CREATE *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	u32 n;
+	unsigned long irql;
+
+	ASSERT(rd);
+	ASSERT(cq_object);
+	ASSERT(length % sizeof(struct MCC_CQ_ENTRY_AMAP) == 0);
+
+	switch (num_entries) {
+	case 256:
+		num_entries_encoding = CEV_CQ_CNT_256;
+		break;
+	case 512:
+		num_entries_encoding = CEV_CQ_CNT_512;
+		break;
+	case 1024:
+		num_entries_encoding = CEV_CQ_CNT_1024;
+		break;
+	default:
+		ASSERT(0);
+		return BE_STATUS_INVALID_PARAMETER;
+	}
+
+	/*
+	 * All cq entries all the same size.  Use iSCSI version
+	 * as a test for the proper rd length.
+	 */
+	memset(cq_object, 0, sizeof(*cq_object));
+
+	atomic_set(&cq_object->ref_count, 0);
+	cq_object->parent_function = pfob;
+	cq_object->eq_object = eq_object;
+	cq_object->num_entries = num_entries;
+	/* save for MCC cq processing */
+	cq_object->va = rd->va;
+
+	/* map into UT. */
+	length = num_entries * sizeof(struct MCC_CQ_ENTRY_AMAP);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		ASSERT(wrb);
+		TRACE(DL_ERR, "No free MCC WRBs in create EQ.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_CQ_CREATE);
+
+	fwcmd->params.request.num_pages = PAGES_SPANNED(OFFSET_IN_PAGE(rd->va),
+									length);
+
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, valid, &fwcmd->params.request.context, 1);
+	n = pfob->pci_function_number;
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, Func, &fwcmd->params.request.context, n);
+
+	n = (eq_object != NULL);
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, Eventable,
+				&fwcmd->params.request.context, n);
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, Armed, &fwcmd->params.request.context, 1);
+
+	n = eq_object ? eq_object->eq_id : 0;
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, EQID, &fwcmd->params.request.context, n);
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, Count,
+			&fwcmd->params.request.context, num_entries_encoding);
+
+	n = 0; /* Protection Domain is always 0 in  Linux  driver */
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, PD, &fwcmd->params.request.context, n);
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, NoDelay,
+				&fwcmd->params.request.context, no_delay);
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, SolEvent,
+			&fwcmd->params.request.context, solicited_eventable);
+
+	n = (wm_thresh != 0xFFFFFFFF);
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, WME, &fwcmd->params.request.context, n);
+
+	n = (n ? wm_thresh : 0);
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, Watermark,
+				&fwcmd->params.request.context, n);
+	/* Create a page list for the FWCMD. */
+	be_rd_to_pa_list(rd, fwcmd->params.request.pages,
+			  ARRAY_SIZE(fwcmd->params.request.pages));
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+			NULL, NULL, fwcmd, NULL);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "MCC to create CQ failed.");
+		goto Error;
+	}
+	/* Remember the CQ id. */
+	cq_object->cq_id = fwcmd->params.response.cq_id;
+
+	/* insert this cq into eq_object reference */
+	if (eq_object) {
+		atomic_inc(&eq_object->ref_count);
+		list_add_tail(&cq_object->cqlist_for_eq,
+					&eq_object->cq_list_head);
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+
+    Deferences the given object. Once the object's reference count drops to
+    zero, the object is destroyed and all resources that are held by this object
+    are released.  The on-chip context is also destroyed along with the queue
+    ID, and any mappings made into the UT.
+
+    cq_object            - CQ handle returned from cq_object_create.
+
+    returns the current reference count on the object
+
+    IRQL: IRQL < DISPATCH_LEVEL
+*/
+int be_cq_destroy(struct be_cq_object *cq_object)
+{
+	int status = 0;
+
+	/* Nothing should reference this CQ at this point. */
+	ASSERT(atomic_read(&cq_object->ref_count) == 0);
+
+	/* Send fwcmd to destroy the CQ. */
+	status = be_function_ring_destroy(cq_object->parent_function,
+		     cq_object->cq_id, FWCMD_RING_TYPE_CQ,
+					NULL, NULL, NULL, NULL);
+	ASSERT(status == 0);
+
+	/* Remove reference if this is an eventable CQ. */
+	if (cq_object->eq_object) {
+		atomic_dec(&cq_object->eq_object->ref_count);
+		list_del(&cq_object->cqlist_for_eq);
+	}
+	return BE_SUCCESS;
+}
+
diff --git a/drivers/staging/benet/descriptors.h b/drivers/staging/benet/descriptors.h
new file mode 100644
index 000000000000..8da438c407d2
--- /dev/null
+++ b/drivers/staging/benet/descriptors.h
@@ -0,0 +1,71 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __descriptors_amap_h__
+#define __descriptors_amap_h__
+
+/*
+ * --- IPC_NODE_ID_ENUM ---
+ * IPC processor id values
+ */
+#define TPOST_NODE_ID                   (0)	/* TPOST ID */
+#define TPRE_NODE_ID                    (1)	/* TPRE ID */
+#define TXULP0_NODE_ID                  (2)	/* TXULP0 ID */
+#define TXULP1_NODE_ID                  (3)	/* TXULP1 ID */
+#define TXULP2_NODE_ID                  (4)	/* TXULP2 ID */
+#define RXULP0_NODE_ID                  (5)	/* RXULP0 ID */
+#define RXULP1_NODE_ID                  (6)	/* RXULP1 ID */
+#define RXULP2_NODE_ID                  (7)	/* RXULP2 ID */
+#define MPU_NODE_ID                     (15)	/* MPU ID */
+
+/*
+ * --- MAC_ID_ENUM ---
+ * Meaning of the mac_id field in rxpp_eth_d
+ */
+#define PORT0_HOST_MAC0    (0)  /* PD 0, Port 0, host networking, MAC 0. */
+#define PORT0_HOST_MAC1    (1)	/* PD 0, Port 0, host networking, MAC 1. */
+#define PORT0_STORAGE_MAC0 (2)	/* PD 0, Port 0, host storage, MAC 0. */
+#define PORT0_STORAGE_MAC1 (3)	/* PD 0, Port 0, host storage, MAC 1. */
+#define PORT1_HOST_MAC0    (4)	/* PD 0, Port 1 host networking, MAC 0. */
+#define PORT1_HOST_MAC1    (5)	/* PD 0, Port 1 host networking, MAC 1. */
+#define PORT1_STORAGE_MAC0 (6)	/* PD 0, Port 1 host storage, MAC 0. */
+#define PORT1_STORAGE_MAC1 (7)	/* PD 0, Port 1 host storage, MAC 1. */
+#define FIRST_VM_MAC       (8)	/* PD 1 MAC. Protection domains have IDs */
+				/* from 0x8-0x26, one per PD. */
+#define LAST_VM_MAC        (38)	/* PD 31 MAC. */
+#define MGMT_MAC           (39)	/* Management port MAC. */
+#define MARBLE_MAC0        (59)	/* Used for flushing function 0 receive */
+				  /*
+				   * queues before re-using a torn-down
+				   * receive ring. the DA =
+				   * 00-00-00-00-00-00, and the MSB of the
+				   * SA = 00
+				   */
+#define MARBLE_MAC1        (60)	/* Used for flushing function 1 receive */
+				  /*
+				   * queues before re-using a torn-down
+				   * receive ring. the DA =
+				   * 00-00-00-00-00-00, and the MSB of the
+				   * SA != 00
+				   */
+#define NULL_MAC           (61)	/* Promiscuous mode, indicates no match */
+#define MCAST_MAC          (62)	/* Multicast match. */
+#define BCAST_MATCH        (63)	/* Broadcast match. */
+
+#endif /* __descriptors_amap_h__ */
diff --git a/drivers/staging/benet/doorbells.h b/drivers/staging/benet/doorbells.h
new file mode 100644
index 000000000000..550cc4d5d6f7
--- /dev/null
+++ b/drivers/staging/benet/doorbells.h
@@ -0,0 +1,179 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __doorbells_amap_h__
+#define __doorbells_amap_h__
+
+/* The TX/RDMA send queue doorbell. */
+struct BE_SQ_DB_AMAP {
+	u8 cid[11];		/* DWORD 0 */
+	u8 rsvd0[5];	/* DWORD 0 */
+	u8 numPosted[14];	/* DWORD 0 */
+	u8 rsvd1[2];	/* DWORD 0 */
+} __packed;
+struct SQ_DB_AMAP {
+	u32 dw[1];
+};
+
+/* The receive queue doorbell. */
+struct BE_RQ_DB_AMAP {
+	u8 rq[10];		/* DWORD 0 */
+	u8 rsvd0[13];	/* DWORD 0 */
+	u8 Invalidate;	/* DWORD 0 */
+	u8 numPosted[8];	/* DWORD 0 */
+} __packed;
+struct RQ_DB_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * The CQ/EQ doorbell. Software MUST set reserved fields in this
+ * descriptor to zero, otherwise (CEV) hardware will not execute the
+ * doorbell (flagging a bad_db_qid error instead).
+ */
+struct BE_CQ_DB_AMAP {
+	u8 qid[10];		/* DWORD 0 */
+	u8 rsvd0[4];	/* DWORD 0 */
+	u8 rearm;		/* DWORD 0 */
+	u8 event;		/* DWORD 0 */
+	u8 num_popped[13];	/* DWORD 0 */
+	u8 rsvd1[3];	/* DWORD 0 */
+} __packed;
+struct CQ_DB_AMAP {
+	u32 dw[1];
+};
+
+struct BE_TPM_RQ_DB_AMAP {
+	u8 qid[10];		/* DWORD 0 */
+	u8 rsvd0[6];	/* DWORD 0 */
+	u8 numPosted[11];	/* DWORD 0 */
+	u8 mss_cnt[5];	/* DWORD 0 */
+} __packed;
+struct TPM_RQ_DB_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * Post WRB Queue Doorbell Register used by the host Storage stack
+ * to notify the controller of a posted Work Request Block
+ */
+struct BE_WRB_POST_DB_AMAP {
+	u8 wrb_cid[10];	/* DWORD 0 */
+	u8 rsvd0[6];	/* DWORD 0 */
+	u8 wrb_index[8];	/* DWORD 0 */
+	u8 numberPosted[8];	/* DWORD 0 */
+} __packed;
+struct WRB_POST_DB_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * Update Default PDU Queue Doorbell Register used to communicate
+ * to the controller that the driver has stopped processing the queue
+ * and where in the queue it stopped, this is
+ * a CQ Entry Type. Used by storage driver.
+ */
+struct BE_DEFAULT_PDU_DB_AMAP {
+	u8 qid[10];		/* DWORD 0 */
+	u8 rsvd0[4];	/* DWORD 0 */
+	u8 rearm;		/* DWORD 0 */
+	u8 event;		/* DWORD 0 */
+	u8 cqproc[14];	/* DWORD 0 */
+	u8 rsvd1[2];	/* DWORD 0 */
+} __packed;
+struct DEFAULT_PDU_DB_AMAP {
+	u32 dw[1];
+};
+
+/* Management Command and Controller default fragment ring */
+struct BE_MCC_DB_AMAP {
+	u8 rid[11];		/* DWORD 0 */
+	u8 rsvd0[5];	/* DWORD 0 */
+	u8 numPosted[14];	/* DWORD 0 */
+	u8 rsvd1[2];	/* DWORD 0 */
+} __packed;
+struct MCC_DB_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * Used for bootstrapping the Host interface. This register is
+ * used for driver communication with the MPU when no MCC Rings exist.
+ * The software must write this register twice to post any MCC
+ * command. First, it writes the register with hi=1 and the upper bits of
+ * the  physical address for the MCC_MAILBOX structure.  Software must poll
+ * the ready bit until this is acknowledged.  Then, sotware writes the
+ * register with hi=0 with the lower bits in the address.  It must
+ * poll the ready bit until the MCC command is complete.  Upon completion,
+ * the MCC_MAILBOX will contain a valid completion queue  entry.
+ */
+struct BE_MPU_MAILBOX_DB_AMAP {
+	u8 ready;		/* DWORD 0 */
+	u8 hi;		/* DWORD 0 */
+	u8 address[30];	/* DWORD 0 */
+} __packed;
+struct MPU_MAILBOX_DB_AMAP {
+	u32 dw[1];
+};
+
+/*
+ *  This is the protection domain doorbell register map. Note that
+ *  while this map shows doorbells for all Blade Engine supported
+ *  protocols, not all of these may be valid in a given function or
+ *  protection domain. It is the responsibility of the application
+ *  accessing the doorbells to know which are valid. Each doorbell
+ *  occupies 32 bytes of space, but unless otherwise specified,
+ *  only the first 4 bytes should be written.  There are 32 instances
+ *  of these doorbells for the host and 31 virtual machines respectively.
+ *  The host and VMs will only map the doorbell pages belonging to its
+ *  protection domain. It will not be able to touch the doorbells for
+ *  another VM. The doorbells are the only registers directly accessible
+ *  by a virtual machine. Similarly, there are 511 additional
+ *  doorbells for RDMA protection domains. PD 0 for RDMA shares
+ *  the same physical protection domain doorbell page as ETH/iSCSI.
+ *
+ */
+struct BE_PROTECTION_DOMAIN_DBMAP_AMAP {
+	u8 rsvd0[512];	/* DWORD 0 */
+	struct BE_SQ_DB_AMAP rdma_sq_db;
+	u8 rsvd1[7][32];	/* DWORD 17 */
+	struct BE_WRB_POST_DB_AMAP iscsi_wrb_post_db;
+	u8 rsvd2[7][32];	/* DWORD 25 */
+	struct BE_SQ_DB_AMAP etx_sq_db;
+	u8 rsvd3[7][32];	/* DWORD 33 */
+	struct BE_RQ_DB_AMAP rdma_rq_db;
+	u8 rsvd4[7][32];	/* DWORD 41 */
+	struct BE_DEFAULT_PDU_DB_AMAP iscsi_default_pdu_db;
+	u8 rsvd5[7][32];	/* DWORD 49 */
+	struct BE_TPM_RQ_DB_AMAP tpm_rq_db;
+	u8 rsvd6[7][32];	/* DWORD 57 */
+	struct BE_RQ_DB_AMAP erx_rq_db;
+	u8 rsvd7[7][32];	/* DWORD 65 */
+	struct BE_CQ_DB_AMAP cq_db;
+	u8 rsvd8[7][32];	/* DWORD 73 */
+	struct BE_MCC_DB_AMAP mpu_mcc_db;
+	u8 rsvd9[7][32];	/* DWORD 81 */
+	struct BE_MPU_MAILBOX_DB_AMAP mcc_bootstrap_db;
+	u8 rsvd10[935][32];	/* DWORD 89 */
+} __packed;
+struct PROTECTION_DOMAIN_DBMAP_AMAP {
+	u32 dw[1024];
+};
+
+#endif /* __doorbells_amap_h__ */
diff --git a/drivers/staging/benet/ep.h b/drivers/staging/benet/ep.h
new file mode 100644
index 000000000000..72fcf64a9ffb
--- /dev/null
+++ b/drivers/staging/benet/ep.h
@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __ep_amap_h__
+#define __ep_amap_h__
+
+/* General Control and Status Register. */
+struct BE_EP_CONTROL_CSR_AMAP {
+	u8 m0_RxPbuf;	/* DWORD 0 */
+	u8 m1_RxPbuf;	/* DWORD 0 */
+	u8 m2_RxPbuf;	/* DWORD 0 */
+	u8 ff_en;		/* DWORD 0 */
+	u8 rsvd0[27];	/* DWORD 0 */
+	u8 CPU_reset;	/* DWORD 0 */
+} __packed;
+struct EP_CONTROL_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Semaphore Register. */
+struct BE_EP_SEMAPHORE_CSR_AMAP {
+	u8 value[32];	/* DWORD 0 */
+} __packed;
+struct EP_SEMAPHORE_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Embedded Processor Specific Registers. */
+struct BE_EP_CSRMAP_AMAP {
+	struct BE_EP_CONTROL_CSR_AMAP ep_control;
+	u8 rsvd0[32];	/* DWORD 1 */
+	u8 rsvd1[32];	/* DWORD 2 */
+	u8 rsvd2[32];	/* DWORD 3 */
+	u8 rsvd3[32];	/* DWORD 4 */
+	u8 rsvd4[32];	/* DWORD 5 */
+	u8 rsvd5[8][128];	/* DWORD 6 */
+	u8 rsvd6[32];	/* DWORD 38 */
+	u8 rsvd7[32];	/* DWORD 39 */
+	u8 rsvd8[32];	/* DWORD 40 */
+	u8 rsvd9[32];	/* DWORD 41 */
+	u8 rsvd10[32];	/* DWORD 42 */
+	struct BE_EP_SEMAPHORE_CSR_AMAP ep_semaphore;
+	u8 rsvd11[32];	/* DWORD 44 */
+	u8 rsvd12[19][32];	/* DWORD 45 */
+} __packed;
+struct EP_CSRMAP_AMAP {
+	u32 dw[64];
+};
+
+#endif /* __ep_amap_h__ */
diff --git a/drivers/staging/benet/eq.c b/drivers/staging/benet/eq.c
new file mode 100644
index 000000000000..db92ccd8fed8
--- /dev/null
+++ b/drivers/staging/benet/eq.c
@@ -0,0 +1,299 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#include "hwlib.h"
+#include "bestatus.h"
+/*
+    This routine creates an event queue based on the client completion
+    queue configuration information.
+
+    FunctionObject      - Handle to a function object
+    EqBaseVa            - Base VA for a the EQ ring
+    SizeEncoding        - The encoded size for the EQ entries. This value is
+			either CEV_EQ_SIZE_4 or CEV_EQ_SIZE_16
+    NumEntries          - CEV_CQ_CNT_* values.
+    Watermark           - Enables watermark based coalescing.  This parameter
+			must be of the type CEV_WMARK_* if watermarks
+			are enabled.  If watermarks to to be disabled
+			this value should be-1.
+    TimerDelay          - If a timer delay is enabled this value should be the
+			time of the delay in 8 microsecond units.  If
+			delays are not used this parameter should be
+			set to -1.
+    ppEqObject          - Internal EQ Handle returned.
+
+    Returns BE_SUCCESS if successfull,, otherwise a useful error code
+	is returned.
+
+    IRQL < DISPATCH_LEVEL
+*/
+int
+be_eq_create(struct be_function_object *pfob,
+		struct ring_desc *rd, u32 eqe_size, u32 num_entries,
+		u32 watermark,	/* CEV_WMARK_* or -1 */
+		u32 timer_delay,	/* in 8us units, or -1 */
+		struct be_eq_object *eq_object)
+{
+	int status = BE_SUCCESS;
+	u32 num_entries_encoding, eqe_size_encoding, length;
+	struct FWCMD_COMMON_EQ_CREATE *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	u32 n;
+	unsigned long irql;
+
+	ASSERT(rd);
+	ASSERT(eq_object);
+
+	switch (num_entries) {
+	case 256:
+		num_entries_encoding = CEV_EQ_CNT_256;
+		break;
+	case 512:
+		num_entries_encoding = CEV_EQ_CNT_512;
+		break;
+	case 1024:
+		num_entries_encoding = CEV_EQ_CNT_1024;
+		break;
+	case 2048:
+		num_entries_encoding = CEV_EQ_CNT_2048;
+		break;
+	case 4096:
+		num_entries_encoding = CEV_EQ_CNT_4096;
+		break;
+	default:
+		ASSERT(0);
+		return BE_STATUS_INVALID_PARAMETER;
+	}
+
+	switch (eqe_size) {
+	case 4:
+		eqe_size_encoding = CEV_EQ_SIZE_4;
+		break;
+	case 16:
+		eqe_size_encoding = CEV_EQ_SIZE_16;
+		break;
+	default:
+		ASSERT(0);
+		return BE_STATUS_INVALID_PARAMETER;
+	}
+
+	if ((eqe_size == 4 && num_entries < 1024) ||
+	    (eqe_size == 16 && num_entries == 4096)) {
+		TRACE(DL_ERR, "Bad EQ size. eqe_size:%d num_entries:%d",
+		      eqe_size, num_entries);
+		ASSERT(0);
+		return BE_STATUS_INVALID_PARAMETER;
+	}
+
+	memset(eq_object, 0, sizeof(*eq_object));
+
+	atomic_set(&eq_object->ref_count, 0);
+	eq_object->parent_function = pfob;
+	eq_object->eq_id = 0xFFFFFFFF;
+
+	INIT_LIST_HEAD(&eq_object->cq_list_head);
+
+	length = num_entries * eqe_size;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		ASSERT(wrb);
+		TRACE(DL_ERR, "No free MCC WRBs in create EQ.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_EQ_CREATE);
+
+	fwcmd->params.request.num_pages = PAGES_SPANNED(OFFSET_IN_PAGE(rd->va),
+									length);
+	n = pfob->pci_function_number;
+	AMAP_SET_BITS_PTR(EQ_CONTEXT, Func, &fwcmd->params.request.context, n);
+
+	AMAP_SET_BITS_PTR(EQ_CONTEXT, valid, &fwcmd->params.request.context, 1);
+
+	AMAP_SET_BITS_PTR(EQ_CONTEXT, Size,
+			&fwcmd->params.request.context, eqe_size_encoding);
+
+	n = 0; /* Protection Domain is always 0 in  Linux  driver */
+	AMAP_SET_BITS_PTR(EQ_CONTEXT, PD, &fwcmd->params.request.context, n);
+
+	/* Let the caller ARM the EQ with the doorbell. */
+	AMAP_SET_BITS_PTR(EQ_CONTEXT, Armed, &fwcmd->params.request.context, 0);
+
+	AMAP_SET_BITS_PTR(EQ_CONTEXT, Count, &fwcmd->params.request.context,
+					num_entries_encoding);
+
+	n = pfob->pci_function_number * 32;
+	AMAP_SET_BITS_PTR(EQ_CONTEXT, EventVect,
+				&fwcmd->params.request.context, n);
+	if (watermark != -1) {
+		AMAP_SET_BITS_PTR(EQ_CONTEXT, WME,
+				&fwcmd->params.request.context, 1);
+		AMAP_SET_BITS_PTR(EQ_CONTEXT, Watermark,
+				&fwcmd->params.request.context, watermark);
+		ASSERT(watermark <= CEV_WMARK_240);
+	} else
+		AMAP_SET_BITS_PTR(EQ_CONTEXT, WME,
+					&fwcmd->params.request.context, 0);
+	if (timer_delay != -1) {
+		AMAP_SET_BITS_PTR(EQ_CONTEXT, TMR,
+					&fwcmd->params.request.context, 1);
+
+		ASSERT(timer_delay <= 250);	/* max value according to EAS */
+		timer_delay = min(timer_delay, (u32)250);
+
+		AMAP_SET_BITS_PTR(EQ_CONTEXT, Delay,
+				&fwcmd->params.request.context, timer_delay);
+	} else {
+		AMAP_SET_BITS_PTR(EQ_CONTEXT, TMR,
+				&fwcmd->params.request.context, 0);
+	}
+	/* Create a page list for the FWCMD. */
+	be_rd_to_pa_list(rd, fwcmd->params.request.pages,
+			  ARRAY_SIZE(fwcmd->params.request.pages));
+
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+					NULL, NULL, fwcmd, NULL);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "MCC to create EQ failed.");
+		goto Error;
+	}
+	/* Get the EQ id.  The MPU allocates the IDs. */
+	eq_object->eq_id = fwcmd->params.response.eq_id;
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    Deferences the given object. Once the object's reference count drops to
+    zero, the object is destroyed and all resources that are held by this
+    object are released.  The on-chip context is also destroyed along with
+    the queue ID, and any mappings made into the UT.
+
+    eq_object            - EQ handle returned from eq_object_create.
+
+    Returns BE_SUCCESS if successfull, otherwise a useful error code
+	is returned.
+
+    IRQL: IRQL < DISPATCH_LEVEL
+*/
+int be_eq_destroy(struct be_eq_object *eq_object)
+{
+	int status = 0;
+
+	ASSERT(atomic_read(&eq_object->ref_count) == 0);
+	/* no CQs should reference this EQ now */
+	ASSERT(list_empty(&eq_object->cq_list_head));
+
+	/* Send fwcmd to destroy the EQ. */
+	status = be_function_ring_destroy(eq_object->parent_function,
+			     eq_object->eq_id, FWCMD_RING_TYPE_EQ,
+					NULL, NULL, NULL, NULL);
+	ASSERT(status == 0);
+
+	return BE_SUCCESS;
+}
+/*
+ *---------------------------------------------------------------------------
+ * Function: be_eq_modify_delay
+ *   Changes the EQ delay for a group of EQs.
+ * num_eq             - The number of EQs in the eq_array to adjust.
+ * 			This also is the number of delay values in
+ * 			the eq_delay_array.
+ * eq_array           - Array of struct be_eq_object pointers to adjust.
+ * eq_delay_array     - Array of "num_eq" timer delays in units
+ * 			of microseconds. The be_eq_query_delay_range
+ * 			fwcmd returns the resolution and range of
+ *                      legal EQ delays.
+ * cb           -
+ * cb_context   -
+ * q_ctxt             - Optional. Pointer to a previously allocated
+ * 			struct. If the MCC WRB ring is full, this
+ * 			structure is used to queue the operation. It
+ *                      will be posted to the MCC ring when space
+ *                      becomes available. All queued commands will
+ *                      be posted to the ring in the order they are
+ *                      received. It is always valid to pass a pointer to
+ *                      a generic be_generic_q_cntxt. However,
+ *                      the specific context structs
+ *                      are generally smaller than the generic struct.
+ * return pend_status - BE_SUCCESS (0) on success.
+ * 			BE_PENDING (postive value) if the FWCMD
+ *                      completion is pending. Negative error code on failure.
+ *-------------------------------------------------------------------------
+ */
+int
+be_eq_modify_delay(struct be_function_object *pfob,
+		   u32 num_eq, struct be_eq_object **eq_array,
+		   u32 *eq_delay_array, mcc_wrb_cqe_callback cb,
+		   void *cb_context, struct be_eq_modify_delay_q_ctxt *q_ctxt)
+{
+	struct FWCMD_COMMON_MODIFY_EQ_DELAY *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	struct be_generic_q_ctxt *gen_ctxt = NULL;
+	u32 i;
+	unsigned long irql;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		if (q_ctxt && cb) {
+			wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+			gen_ctxt = (struct be_generic_q_ctxt *) q_ctxt;
+			gen_ctxt->context.bytes = sizeof(*q_ctxt);
+		} else {
+			status = BE_STATUS_NO_MCC_WRB;
+			goto Error;
+		}
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_MODIFY_EQ_DELAY);
+
+	ASSERT(num_eq > 0);
+	ASSERT(num_eq <= ARRAY_SIZE(fwcmd->params.request.delay));
+	fwcmd->params.request.num_eq = num_eq;
+	for (i = 0; i < num_eq; i++) {
+		fwcmd->params.request.delay[i].eq_id = eq_array[i]->eq_id;
+		fwcmd->params.request.delay[i].delay_in_microseconds =
+		    eq_delay_array[i];
+	}
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, gen_ctxt,
+			cb, cb_context, NULL, NULL, fwcmd, NULL);
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
diff --git a/drivers/staging/benet/eth.c b/drivers/staging/benet/eth.c
new file mode 100644
index 000000000000..f641b6260d07
--- /dev/null
+++ b/drivers/staging/benet/eth.c
@@ -0,0 +1,1273 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#include <linux/if_ether.h>
+#include "hwlib.h"
+#include "bestatus.h"
+
+/*
+ *---------------------------------------------------------
+ * Function: be_eth_sq_create_ex
+ *   Creates an ethernet send ring - extended version with
+ *   additional parameters.
+ * pfob -
+ * rd             - ring address
+ * length_in_bytes -
+ * type            - The type of ring to create.
+ * ulp             - The requested ULP number for the ring.
+ * 		     This should be zero based, i.e. 0,1,2. This must
+ * 		     be valid NIC ULP based on the firmware config.
+ *                   All doorbells for this ring must be sent to
+ *                   this ULP. The first network ring allocated for
+ *                   each ULP are higher performance than subsequent rings.
+ * cq_object       - cq object for completions
+ * ex_parameters   - Additional parameters (that may increase in
+ * 		     future revisions). These parameters are only used
+ * 		     for certain ring types -- see
+ *                   struct be_eth_sq_parameters for details.
+ * eth_sq          -
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *---------------------------------------------------------
+ */
+int
+be_eth_sq_create_ex(struct be_function_object *pfob, struct ring_desc *rd,
+		u32 length, u32 type, u32 ulp, struct be_cq_object *cq_object,
+		struct be_eth_sq_parameters *ex_parameters,
+		struct be_ethsq_object *eth_sq)
+{
+	struct FWCMD_COMMON_ETH_TX_CREATE *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	u32 n;
+	unsigned long irql;
+
+	ASSERT(rd);
+	ASSERT(eth_sq);
+	ASSERT(ex_parameters);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	memset(eth_sq, 0, sizeof(*eth_sq));
+
+	eth_sq->parent_function = pfob;
+	eth_sq->bid = 0xFFFFFFFF;
+	eth_sq->cq_object = cq_object;
+
+	/* Translate hwlib interface to arm interface. */
+	switch (type) {
+	case BE_ETH_TX_RING_TYPE_FORWARDING:
+		type = ETH_TX_RING_TYPE_FORWARDING;
+		break;
+	case BE_ETH_TX_RING_TYPE_STANDARD:
+		type = ETH_TX_RING_TYPE_STANDARD;
+		break;
+	case BE_ETH_TX_RING_TYPE_BOUND:
+		ASSERT(ex_parameters->port < 2);
+		type = ETH_TX_RING_TYPE_BOUND;
+		break;
+	default:
+		TRACE(DL_ERR, "Invalid eth tx ring type:%d", type);
+		return BE_NOT_OK;
+		break;
+	}
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		ASSERT(wrb);
+		TRACE(DL_ERR, "No free MCC WRBs in create EQ.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+	/* NIC must be supported by the current config. */
+	ASSERT(pfob->fw_config.nic_ulp_mask);
+
+	/*
+	 * The ulp parameter must select a valid NIC ULP
+	 * for the current config.
+	 */
+	ASSERT((1 << ulp) & pfob->fw_config.nic_ulp_mask);
+
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_ETH_TX_CREATE);
+	fwcmd->header.request.port_number = ex_parameters->port;
+
+	AMAP_SET_BITS_PTR(ETX_CONTEXT, pd_id,
+				&fwcmd->params.request.context, 0);
+
+	n = be_ring_length_to_encoding(length, sizeof(struct ETH_WRB_AMAP));
+	AMAP_SET_BITS_PTR(ETX_CONTEXT, tx_ring_size,
+					&fwcmd->params.request.context, n);
+
+	AMAP_SET_BITS_PTR(ETX_CONTEXT, cq_id_send,
+			&fwcmd->params.request.context, cq_object->cq_id);
+
+	n = pfob->pci_function_number;
+	AMAP_SET_BITS_PTR(ETX_CONTEXT, func, &fwcmd->params.request.context, n);
+
+	fwcmd->params.request.type = type;
+	fwcmd->params.request.ulp_num  = (1 << ulp);
+	fwcmd->params.request.num_pages = DIV_ROUND_UP(length, PAGE_SIZE);
+	ASSERT(PAGES_SPANNED(rd->va, rd->length) >=
+				fwcmd->params.request.num_pages);
+
+	/* Create a page list for the FWCMD. */
+	be_rd_to_pa_list(rd, fwcmd->params.request.pages,
+			  ARRAY_SIZE(fwcmd->params.request.pages));
+
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+					NULL, NULL, fwcmd, NULL);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "MCC to create etx queue failed.");
+		goto Error;
+	}
+	/* save the butler ID */
+	eth_sq->bid = fwcmd->params.response.cid;
+
+	/* add a reference to the corresponding CQ */
+	atomic_inc(&cq_object->ref_count);
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+
+/*
+    This routine destroys an ethernet send queue
+
+    EthSq - EthSq Handle returned from EthSqCreate
+
+    This function always return BE_SUCCESS.
+
+    This function frees memory allocated by EthSqCreate for the EthSq Object.
+
+*/
+int be_eth_sq_destroy(struct be_ethsq_object *eth_sq)
+{
+	int status = 0;
+
+	/* Send fwcmd to destroy the queue. */
+	status = be_function_ring_destroy(eth_sq->parent_function, eth_sq->bid,
+		     FWCMD_RING_TYPE_ETH_TX, NULL, NULL, NULL, NULL);
+	ASSERT(status == 0);
+
+	/* Derefence any associated CQs. */
+	atomic_dec(&eth_sq->cq_object->ref_count);
+	return status;
+}
+/*
+    This routine attempts to set the transmit flow control parameters.
+
+    FunctionObject      - Handle to a function object
+
+    txfc_enable         - transmit flow control enable - true for
+			  enable, false for disable
+
+    rxfc_enable         - receive flow control enable - true for
+				enable, false for disable
+
+    Returns BE_SUCCESS if successfull, otherwise a useful int error
+    code is returned.
+
+    IRQL: < DISPATCH_LEVEL
+
+    This function always fails in non-privileged machine context.
+*/
+int
+be_eth_set_flow_control(struct be_function_object *pfob,
+			bool txfc_enable, bool rxfc_enable)
+{
+	struct FWCMD_COMMON_SET_FLOW_CONTROL *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_SET_FLOW_CONTROL);
+
+	fwcmd->params.request.rx_flow_control = rxfc_enable;
+	fwcmd->params.request.tx_flow_control = txfc_enable;
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+					NULL, NULL, fwcmd, NULL);
+
+	if (status != 0) {
+		TRACE(DL_ERR, "set flow control fwcmd failed.");
+		goto error;
+	}
+
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This routine attempts to get the transmit flow control parameters.
+
+    pfob      - Handle to a function object
+
+    txfc_enable         - transmit flow control enable - true for
+			enable, false for disable
+
+    rxfc_enable         - receive flow control enable - true for enable,
+			false for disable
+
+    Returns BE_SUCCESS if successfull, otherwise a useful int error code
+			is returned.
+
+    IRQL: < DISPATCH_LEVEL
+
+    This function always fails in non-privileged machine context.
+*/
+int
+be_eth_get_flow_control(struct be_function_object *pfob,
+			bool *txfc_enable, bool *rxfc_enable)
+{
+	struct FWCMD_COMMON_GET_FLOW_CONTROL *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_GET_FLOW_CONTROL);
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+						NULL, NULL, fwcmd, NULL);
+
+	if (status != 0) {
+		TRACE(DL_ERR, "get flow control fwcmd failed.");
+		goto error;
+	}
+
+	*txfc_enable = fwcmd->params.response.tx_flow_control;
+	*rxfc_enable = fwcmd->params.response.rx_flow_control;
+
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+ *---------------------------------------------------------
+ * Function: be_eth_set_qos
+ *   This function sets the ethernet transmit Quality of Service (QoS)
+ *   characteristics of BladeEngine for the domain. All ethernet
+ *   transmit rings of the domain will evenly share the bandwidth.
+ *   The exeception to sharing is the host primary (super) ethernet
+ *   transmit ring as well as the host ethernet forwarding ring
+ *   for missed offload data.
+ * pfob -
+ * max_bps         - the maximum bits per second in units of
+ * 			10 Mbps (valid 0-100)
+ * max_pps         - the maximum packets per second in units
+ * 			of 1 Kpps (0 indicates no limit)
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *---------------------------------------------------------
+ */
+int
+be_eth_set_qos(struct be_function_object *pfob, u32 max_bps, u32 max_pps)
+{
+	struct FWCMD_COMMON_SET_QOS *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_SET_QOS);
+
+	/* Set fields in fwcmd */
+	fwcmd->params.request.max_bits_per_second_NIC = max_bps;
+	fwcmd->params.request.max_packets_per_second_NIC = max_pps;
+	fwcmd->params.request.valid_flags = QOS_BITS_NIC | QOS_PKTS_NIC;
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+					NULL, NULL, fwcmd, NULL);
+
+	if (status != 0)
+		TRACE(DL_ERR, "network set qos fwcmd failed.");
+
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+ *---------------------------------------------------------
+ * Function: be_eth_get_qos
+ *   This function retrieves the ethernet transmit Quality of Service (QoS)
+ *   characteristics for the domain.
+ * max_bps         - the maximum bits per second in units of
+ * 			10 Mbps (valid 0-100)
+ * max_pps         - the maximum packets per second in units of
+ * 			1 Kpps (0 indicates no limit)
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *---------------------------------------------------------
+ */
+int
+be_eth_get_qos(struct be_function_object *pfob, u32 *max_bps, u32 *max_pps)
+{
+	struct FWCMD_COMMON_GET_QOS *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_GET_QOS);
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+					NULL, NULL, fwcmd, NULL);
+
+	if (status != 0) {
+		TRACE(DL_ERR, "network get qos fwcmd failed.");
+		goto error;
+	}
+
+	*max_bps = fwcmd->params.response.max_bits_per_second_NIC;
+	*max_pps = fwcmd->params.response.max_packets_per_second_NIC;
+
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+ *---------------------------------------------------------
+ * Function: be_eth_set_frame_size
+ *   This function sets the ethernet maximum frame size. The previous
+ *   values are returned.
+ * pfob -
+ * tx_frame_size   - maximum transmit frame size in bytes
+ * rx_frame_size   - maximum receive frame size in bytes
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *---------------------------------------------------------
+ */
+int
+be_eth_set_frame_size(struct be_function_object *pfob,
+		      u32 *tx_frame_size, u32 *rx_frame_size)
+{
+	struct FWCMD_COMMON_SET_FRAME_SIZE *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_SET_FRAME_SIZE);
+	fwcmd->params.request.max_tx_frame_size = *tx_frame_size;
+	fwcmd->params.request.max_rx_frame_size = *rx_frame_size;
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+						NULL, NULL, fwcmd, NULL);
+
+	if (status != 0) {
+		TRACE(DL_ERR, "network set frame size fwcmd failed.");
+		goto error;
+	}
+
+	*tx_frame_size = fwcmd->params.response.chip_max_tx_frame_size;
+	*rx_frame_size = fwcmd->params.response.chip_max_rx_frame_size;
+
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+
+/*
+    This routine creates a Ethernet receive ring.
+
+    pfob      - handle to a function object
+    rq_base_va            - base VA for the default receive ring. this must be
+			exactly 8K in length and continguous physical memory.
+    cq_object            - handle to a previously created CQ to be associated
+			with the RQ.
+    pp_eth_rq             - pointer to an opqaue handle where an eth
+			receive object is returned.
+    Returns BE_SUCCESS if successfull, , otherwise a useful
+    int error code is returned.
+
+    IRQL: < DISPATCH_LEVEL
+    this function allocates a struct be_ethrq_object *object.
+    there must be no more than 1 of these per function object, unless the
+    function object supports RSS (is networking and on the host).
+    the rq_base_va must point to a buffer of exactly 8K.
+    the erx::host_cqid (or host_stor_cqid) register and erx::ring_page registers
+    will be updated as appropriate on return
+*/
+int
+be_eth_rq_create(struct be_function_object *pfob,
+			struct ring_desc *rd, struct be_cq_object *cq_object,
+			struct be_cq_object *bcmc_cq_object,
+			struct be_ethrq_object *eth_rq)
+{
+	int status = 0;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	struct FWCMD_COMMON_ETH_RX_CREATE *fwcmd = NULL;
+	unsigned long irql;
+
+	/* MPU will set the  */
+	ASSERT(rd);
+	ASSERT(eth_rq);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	eth_rq->parent_function = pfob;
+	eth_rq->cq_object = cq_object;
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_ETH_RX_CREATE);
+
+	fwcmd->params.request.num_pages = 2;	/* required length */
+	fwcmd->params.request.cq_id = cq_object->cq_id;
+
+	if (bcmc_cq_object)
+		fwcmd->params.request.bcmc_cq_id = bcmc_cq_object->cq_id;
+	else
+		fwcmd->params.request.bcmc_cq_id = 0xFFFF;
+
+	/* Create a page list for the FWCMD. */
+	be_rd_to_pa_list(rd, fwcmd->params.request.pages,
+			  ARRAY_SIZE(fwcmd->params.request.pages));
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+						NULL, NULL, fwcmd, NULL);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "fwcmd to map eth rxq frags failed.");
+		goto Error;
+	}
+	/* Save the ring ID for cleanup. */
+	eth_rq->rid = fwcmd->params.response.id;
+
+	atomic_inc(&cq_object->ref_count);
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This routine destroys an Ethernet receive queue
+
+    eth_rq - ethernet receive queue handle returned from eth_rq_create
+
+    Returns BE_SUCCESS on success and an appropriate int on failure.
+
+    This function frees resourcs allocated by EthRqCreate.
+    The erx::host_cqid (or host_stor_cqid) register and erx::ring_page
+    registers will be updated as appropriate on return
+    IRQL: < DISPATCH_LEVEL
+*/
+
+static void be_eth_rq_destroy_internal_cb(void *context, int status,
+					 struct MCC_WRB_AMAP *wrb)
+{
+	struct be_ethrq_object *eth_rq = (struct be_ethrq_object *) context;
+
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "Destroy eth rq failed in internal callback.\n");
+	} else {
+		/* Dereference any CQs associated with this queue. */
+		atomic_dec(&eth_rq->cq_object->ref_count);
+	}
+
+	return;
+}
+
+int be_eth_rq_destroy(struct be_ethrq_object *eth_rq)
+{
+	int status = BE_SUCCESS;
+
+	/* Send fwcmd to destroy the RQ. */
+	status = be_function_ring_destroy(eth_rq->parent_function,
+			eth_rq->rid, FWCMD_RING_TYPE_ETH_RX, NULL, NULL,
+			be_eth_rq_destroy_internal_cb, eth_rq);
+
+	return status;
+}
+
+/*
+ *---------------------------------------------------------------------------
+ * Function: be_eth_rq_destroy_options
+ *   Destroys an ethernet receive ring with finer granularity options
+ *   than the standard be_eth_rq_destroy() API function.
+ * eth_rq           -
+ * flush            - Set to 1 to flush the ring, set to 0 to bypass the flush
+ * cb               - Callback function on completion
+ * cb_context       - Callback context
+ * return status    - BE_SUCCESS (0) on success. Negative error code on failure.
+ *----------------------------------------------------------------------------
+ */
+int
+be_eth_rq_destroy_options(struct be_ethrq_object *eth_rq, bool flush,
+		mcc_wrb_cqe_callback cb, void *cb_context)
+{
+	struct FWCMD_COMMON_RING_DESTROY *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = BE_SUCCESS;
+	struct be_function_object *pfob = NULL;
+	unsigned long irql;
+
+	pfob = eth_rq->parent_function;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	TRACE(DL_INFO, "Destroy eth_rq ring id:%d, flush:%d", eth_rq->rid,
+	      flush);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		ASSERT(wrb);
+		TRACE(DL_ERR, "No free MCC WRBs in destroy eth_rq ring.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_RING_DESTROY);
+
+	fwcmd->params.request.id = eth_rq->rid;
+	fwcmd->params.request.ring_type = FWCMD_RING_TYPE_ETH_RX;
+	fwcmd->params.request.bypass_flush = ((0 == flush) ? 1 : 0);
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, cb, cb_context,
+			be_eth_rq_destroy_internal_cb, eth_rq, fwcmd, NULL);
+
+	if (status != BE_SUCCESS && status != BE_PENDING) {
+		TRACE(DL_ERR, "eth_rq ring destroy failed. id:%d, flush:%d",
+		      eth_rq->rid, flush);
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This routine queries the frag size for erx.
+
+    pfob      - handle to a function object
+
+    frag_size_bytes       - erx frag size in bytes that is/was set.
+
+    Returns BE_SUCCESS if successfull, otherwise a useful int error
+    code is returned.
+
+    IRQL: < DISPATCH_LEVEL
+
+*/
+int
+be_eth_rq_get_frag_size(struct be_function_object *pfob, u32 *frag_size_bytes)
+{
+	struct FWCMD_ETH_GET_RX_FRAG_SIZE *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	ASSERT(frag_size_bytes);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		return BE_STATUS_NO_MCC_WRB;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, ETH_GET_RX_FRAG_SIZE);
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+				NULL, NULL, fwcmd, NULL);
+
+	if (status != 0) {
+		TRACE(DL_ERR, "get frag size fwcmd failed.");
+		goto error;
+	}
+
+	*frag_size_bytes = 1 << fwcmd->params.response.actual_fragsize_log2;
+
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This routine attempts to set the frag size for erx.  If the frag size is
+    already set, the attempt fails and the current frag size is returned.
+
+    pfob      - Handle to a function object
+
+    frag_size       - Erx frag size in bytes that is/was set.
+
+    current_frag_size_bytes    - Pointer to location where currrent frag
+				 is to be rturned
+
+    Returns BE_SUCCESS if successfull, otherwise a useful int error
+    code is returned.
+
+    IRQL: < DISPATCH_LEVEL
+
+    This function always fails in non-privileged machine context.
+*/
+int
+be_eth_rq_set_frag_size(struct be_function_object *pfob,
+			u32 frag_size, u32 *frag_size_bytes)
+{
+	struct FWCMD_ETH_SET_RX_FRAG_SIZE *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	ASSERT(frag_size_bytes);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, ETH_SET_RX_FRAG_SIZE);
+
+	ASSERT(frag_size >= 128 && frag_size <= 16 * 1024);
+
+	/* This is the log2 of the fragsize.  This is not the exact
+	 * ERX encoding. */
+	fwcmd->params.request.new_fragsize_log2 = __ilog2_u32(frag_size);
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+				NULL, NULL, fwcmd, NULL);
+
+	if (status != 0) {
+		TRACE(DL_ERR, "set frag size fwcmd failed.");
+		goto error;
+	}
+
+	*frag_size_bytes = 1 << fwcmd->params.response.actual_fragsize_log2;
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+
+/*
+    This routine gets or sets a mac address for a domain
+    given the port and mac.
+
+    FunctionObject  - Function object handle.
+    port1           - Set to TRUE if this function will set/get the Port 1
+			address.  Only the host may set this to TRUE.
+    mac1            - Set to TRUE if this function will set/get the
+			MAC 1 address.  Only the host may set this to TRUE.
+    write           - Set to TRUE if this function should write the mac address.
+    mac_address      - Buffer of the mac address to read or write.
+
+    Returns BE_SUCCESS if successfull, otherwise a useful int is returned.
+
+    IRQL: < DISPATCH_LEVEL
+*/
+int be_rxf_mac_address_read_write(struct be_function_object *pfob,
+		bool port1,	/* VM must always set to false */
+		bool mac1,	/* VM must always set to false */
+		bool mgmt, bool write,
+		bool permanent, u8 *mac_address,
+		mcc_wrb_cqe_callback cb,	/* optional */
+		void *cb_context)	/* optional */
+{
+	int status = BE_SUCCESS;
+	union {
+		struct FWCMD_COMMON_NTWK_MAC_QUERY *query;
+		struct FWCMD_COMMON_NTWK_MAC_SET *set;
+	} fwcmd = {NULL};
+	struct MCC_WRB_AMAP *wrb = NULL;
+	u32 type = 0;
+	unsigned long irql;
+	struct be_mcc_wrb_response_copy rc;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	ASSERT(mac_address);
+
+	ASSERT(port1 == false);
+	ASSERT(mac1 == false);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+
+	if (mgmt) {
+		type = MAC_ADDRESS_TYPE_MANAGEMENT;
+	} else {
+		if (pfob->type == BE_FUNCTION_TYPE_NETWORK)
+			type = MAC_ADDRESS_TYPE_NETWORK;
+		else
+			type = MAC_ADDRESS_TYPE_STORAGE;
+	}
+
+	if (write) {
+		/* Prepares an embedded fwcmd, including
+		 * request/response sizes.
+		 */
+		fwcmd.set = BE_PREPARE_EMBEDDED_FWCMD(pfob,
+					       wrb, COMMON_NTWK_MAC_SET);
+
+		fwcmd.set->params.request.invalidate = 0;
+		fwcmd.set->params.request.mac1 = (mac1 ? 1 : 0);
+		fwcmd.set->params.request.port = (port1 ? 1 : 0);
+		fwcmd.set->params.request.type = type;
+
+		/* Copy the mac address to set. */
+		fwcmd.set->params.request.mac.SizeOfStructure =
+			    sizeof(fwcmd.set->params.request.mac);
+		memcpy(fwcmd.set->params.request.mac.MACAddress,
+			mac_address, ETH_ALEN);
+
+		/* Post the f/w command */
+		status = be_function_post_mcc_wrb(pfob, wrb, NULL,
+				cb, cb_context, NULL, NULL, fwcmd.set, NULL);
+
+	} else {
+
+		/*
+		 * Prepares an embedded fwcmd, including
+		 * request/response sizes.
+		 */
+		fwcmd.query = BE_PREPARE_EMBEDDED_FWCMD(pfob,
+					       wrb, COMMON_NTWK_MAC_QUERY);
+
+		fwcmd.query->params.request.mac1 = (mac1 ? 1 : 0);
+		fwcmd.query->params.request.port = (port1 ? 1 : 0);
+		fwcmd.query->params.request.type = type;
+		fwcmd.query->params.request.permanent = permanent;
+
+		rc.length = FIELD_SIZEOF(struct FWCMD_COMMON_NTWK_MAC_QUERY,
+						params.response.mac.MACAddress);
+		rc.fwcmd_offset = offsetof(struct FWCMD_COMMON_NTWK_MAC_QUERY,
+						params.response.mac.MACAddress);
+		rc.va = mac_address;
+		/* Post the f/w command (with a copy for the response) */
+		status = be_function_post_mcc_wrb(pfob, wrb, NULL, cb,
+				cb_context, NULL, NULL, fwcmd.query, &rc);
+	}
+
+	if (status < 0) {
+		TRACE(DL_ERR, "mac set/query failed.");
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This routine writes data to context memory.
+
+    pfob  - Function object handle.
+    mac_table     - Set to the 128-bit multicast address hash table.
+
+    Returns BE_SUCCESS if successfull, otherwise a useful int is returned.
+
+    IRQL: < DISPATCH_LEVEL
+*/
+
+int be_rxf_multicast_config(struct be_function_object *pfob,
+		bool promiscuous, u32 num, u8 *mac_table,
+		mcc_wrb_cqe_callback cb,	/* optional */
+		void *cb_context,
+		struct be_multicast_q_ctxt *q_ctxt)
+{
+	int status = BE_SUCCESS;
+	struct FWCMD_COMMON_NTWK_MULTICAST_SET *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	struct be_generic_q_ctxt *generic_ctxt = NULL;
+	unsigned long irql;
+
+	ASSERT(num <= ARRAY_SIZE(fwcmd->params.request.mac));
+
+	if (num > ARRAY_SIZE(fwcmd->params.request.mac)) {
+		TRACE(DL_ERR, "Too many multicast addresses. BE supports %d.",
+		      (int) ARRAY_SIZE(fwcmd->params.request.mac));
+		return BE_NOT_OK;
+	}
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		if (q_ctxt && cb) {
+			wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+			generic_ctxt = (struct be_generic_q_ctxt *) q_ctxt;
+			generic_ctxt->context.bytes = sizeof(*q_ctxt);
+		} else {
+			status = BE_STATUS_NO_MCC_WRB;
+			goto Error;
+		}
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_NTWK_MULTICAST_SET);
+
+	fwcmd->params.request.promiscuous = promiscuous;
+	if (!promiscuous) {
+		fwcmd->params.request.num_mac = num;
+		if (num > 0) {
+			ASSERT(mac_table);
+			memcpy(fwcmd->params.request.mac,
+						mac_table, ETH_ALEN * num);
+		}
+	}
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, generic_ctxt,
+			cb, cb_context, NULL, NULL, fwcmd, NULL);
+	if (status < 0) {
+		TRACE(DL_ERR, "multicast fwcmd failed.");
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This routine adds or removes a vlan tag from the rxf table.
+
+    FunctionObject  - Function object handle.
+    VLanTag         - VLan tag to add or remove.
+    Add             - Set to TRUE if this will add a vlan tag
+
+    Returns BE_SUCCESS if successfull, otherwise a useful int is returned.
+
+    IRQL: < DISPATCH_LEVEL
+*/
+int be_rxf_vlan_config(struct be_function_object *pfob,
+		bool promiscuous, u32 num, u16 *vlan_tag_array,
+		mcc_wrb_cqe_callback cb,	/* optional */
+		void *cb_context,
+		struct be_vlan_q_ctxt *q_ctxt)	/* optional */
+{
+	int status = BE_SUCCESS;
+	struct FWCMD_COMMON_NTWK_VLAN_CONFIG *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	struct be_generic_q_ctxt *generic_ctxt = NULL;
+	unsigned long irql;
+
+	if (num > ARRAY_SIZE(fwcmd->params.request.vlan_tag)) {
+		TRACE(DL_ERR, "Too many VLAN tags.");
+		return BE_NOT_OK;
+	}
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		if (q_ctxt && cb) {
+			wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+			generic_ctxt = (struct be_generic_q_ctxt *) q_ctxt;
+			generic_ctxt->context.bytes = sizeof(*q_ctxt);
+		} else {
+			status = BE_STATUS_NO_MCC_WRB;
+			goto Error;
+		}
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_NTWK_VLAN_CONFIG);
+
+	fwcmd->params.request.promiscuous = promiscuous;
+	if (!promiscuous) {
+		fwcmd->params.request.num_vlan = num;
+
+		if (num > 0) {
+			ASSERT(vlan_tag_array);
+			memcpy(fwcmd->params.request.vlan_tag, vlan_tag_array,
+				  num * sizeof(vlan_tag_array[0]));
+		}
+	}
+
+	/* Post the commadn */
+	status = be_function_post_mcc_wrb(pfob, wrb, generic_ctxt,
+			cb, cb_context, NULL, NULL, fwcmd, NULL);
+	if (status < 0) {
+		TRACE(DL_ERR, "vlan fwcmd failed.");
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+
+int be_rxf_link_status(struct be_function_object *pfob,
+		struct BE_LINK_STATUS *link_status,
+		mcc_wrb_cqe_callback cb,
+		void *cb_context,
+		struct be_link_status_q_ctxt *q_ctxt)
+{
+	struct FWCMD_COMMON_NTWK_LINK_STATUS_QUERY *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	struct be_generic_q_ctxt *generic_ctxt = NULL;
+	unsigned long irql;
+	struct be_mcc_wrb_response_copy rc;
+
+	ASSERT(link_status);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+
+	if (!wrb) {
+		if (q_ctxt && cb) {
+			wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+			generic_ctxt = (struct be_generic_q_ctxt *) q_ctxt;
+			generic_ctxt->context.bytes = sizeof(*q_ctxt);
+		} else {
+			status = BE_STATUS_NO_MCC_WRB;
+			goto Error;
+		}
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb,
+					       COMMON_NTWK_LINK_STATUS_QUERY);
+
+	rc.length = FIELD_SIZEOF(struct FWCMD_COMMON_NTWK_LINK_STATUS_QUERY,
+					params.response);
+	rc.fwcmd_offset = offsetof(struct FWCMD_COMMON_NTWK_LINK_STATUS_QUERY,
+					params.response);
+	rc.va = link_status;
+	/* Post or queue the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, generic_ctxt,
+			cb, cb_context, NULL, NULL, fwcmd, &rc);
+
+	if (status < 0) {
+		TRACE(DL_ERR, "link status fwcmd failed.");
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+int
+be_rxf_query_eth_statistics(struct be_function_object *pfob,
+		    struct FWCMD_ETH_GET_STATISTICS *va_for_fwcmd,
+		    u64 pa_for_fwcmd, mcc_wrb_cqe_callback cb,
+		    void *cb_context,
+		    struct be_nonembedded_q_ctxt *q_ctxt)
+{
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	struct be_generic_q_ctxt *generic_ctxt = NULL;
+	unsigned long irql;
+
+	ASSERT(va_for_fwcmd);
+	ASSERT(pa_for_fwcmd);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+
+	if (!wrb) {
+		if (q_ctxt && cb) {
+			wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+			generic_ctxt = (struct be_generic_q_ctxt *) q_ctxt;
+			generic_ctxt->context.bytes = sizeof(*q_ctxt);
+		} else {
+			status = BE_STATUS_NO_MCC_WRB;
+			goto Error;
+		}
+	}
+
+	TRACE(DL_INFO, "Query eth stats. fwcmd va:%p pa:0x%08x_%08x",
+	      va_for_fwcmd, upper_32_bits(pa_for_fwcmd), (u32)pa_for_fwcmd);
+
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	va_for_fwcmd = BE_PREPARE_NONEMBEDDED_FWCMD(pfob, wrb,
+			  va_for_fwcmd, pa_for_fwcmd, ETH_GET_STATISTICS);
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, generic_ctxt,
+		cb, cb_context, NULL, NULL, va_for_fwcmd, NULL);
+	if (status < 0) {
+		TRACE(DL_ERR, "eth stats fwcmd failed.");
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+int
+be_rxf_promiscuous(struct be_function_object *pfob,
+		   bool enable_port0, bool enable_port1,
+		   mcc_wrb_cqe_callback cb, void *cb_context,
+		   struct be_promiscuous_q_ctxt *q_ctxt)
+{
+	struct FWCMD_ETH_PROMISCUOUS *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	struct be_generic_q_ctxt *generic_ctxt = NULL;
+	unsigned long irql;
+
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+
+	if (!wrb) {
+		if (q_ctxt && cb) {
+			wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+			generic_ctxt = (struct be_generic_q_ctxt *) q_ctxt;
+			generic_ctxt->context.bytes = sizeof(*q_ctxt);
+		} else {
+			status = BE_STATUS_NO_MCC_WRB;
+			goto Error;
+		}
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, ETH_PROMISCUOUS);
+
+	fwcmd->params.request.port0_promiscuous = enable_port0;
+	fwcmd->params.request.port1_promiscuous = enable_port1;
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, generic_ctxt,
+			cb, cb_context, NULL, NULL, fwcmd, NULL);
+
+	if (status < 0) {
+		TRACE(DL_ERR, "promiscuous fwcmd failed.");
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+
+/*
+ *-------------------------------------------------------------------------
+ * Function: be_rxf_filter_config
+ *   Configures BladeEngine ethernet receive filter settings.
+ * pfob    -
+ * settings           - Pointer to the requested filter settings.
+ * 			The response from BladeEngine will be placed back
+ * 			in this structure.
+ * cb                 - optional
+ * cb_context         - optional
+ * q_ctxt             - Optional. Pointer to a previously allocated struct.
+ * 			If the MCC WRB ring is full, this structure is
+ * 			used to queue the operation. It will be posted
+ * 			to the MCC ring when space becomes available. All
+ *                      queued commands will be posted to the ring in
+ *                      the order they are received. It is always valid
+ *                      to pass a pointer to a generic
+ *                      be_generic_q_ctxt. However, the specific
+ *                      context structs are generally smaller than
+ *                      the generic struct.
+ * return pend_status - BE_SUCCESS (0) on success.
+ * 			BE_PENDING (postive value) if the FWCMD
+ *                      completion is pending. Negative error code on failure.
+ *---------------------------------------------------------------------------
+ */
+int
+be_rxf_filter_config(struct be_function_object *pfob,
+		     struct NTWK_RX_FILTER_SETTINGS *settings,
+		     mcc_wrb_cqe_callback cb, void *cb_context,
+		     struct be_rxf_filter_q_ctxt *q_ctxt)
+{
+	struct FWCMD_COMMON_NTWK_RX_FILTER *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	struct be_generic_q_ctxt *generic_ctxt = NULL;
+	unsigned long irql;
+	struct be_mcc_wrb_response_copy rc;
+
+	ASSERT(settings);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+
+	if (!wrb) {
+		if (q_ctxt && cb) {
+			wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+			generic_ctxt = (struct be_generic_q_ctxt *) q_ctxt;
+			generic_ctxt->context.bytes = sizeof(*q_ctxt);
+		} else {
+			status = BE_STATUS_NO_MCC_WRB;
+			goto Error;
+		}
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_NTWK_RX_FILTER);
+	memcpy(&fwcmd->params.request, settings, sizeof(*settings));
+
+	rc.length = FIELD_SIZEOF(struct FWCMD_COMMON_NTWK_RX_FILTER,
+					params.response);
+	rc.fwcmd_offset = offsetof(struct FWCMD_COMMON_NTWK_RX_FILTER,
+					params.response);
+	rc.va = settings;
+	/* Post or queue the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, generic_ctxt,
+			cb, cb_context, NULL, NULL, fwcmd, &rc);
+
+	if (status < 0) {
+		TRACE(DL_ERR, "RXF/ERX filter config fwcmd failed.");
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
diff --git a/drivers/staging/benet/etx_context.h b/drivers/staging/benet/etx_context.h
new file mode 100644
index 000000000000..554fbe5d127b
--- /dev/null
+++ b/drivers/staging/benet/etx_context.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __etx_context_amap_h__
+#define __etx_context_amap_h__
+
+/* ETX ring  context structure. */
+struct BE_ETX_CONTEXT_AMAP {
+	u8 tx_cidx[11];	/* DWORD 0 */
+	u8 rsvd0[5];	/* DWORD 0 */
+	u8 rsvd1[16];	/* DWORD 0 */
+	u8 tx_pidx[11];	/* DWORD 1 */
+	u8 rsvd2;		/* DWORD 1 */
+	u8 tx_ring_size[4];	/* DWORD 1 */
+	u8 pd_id[5];	/* DWORD 1 */
+	u8 pd_id_not_valid;	/* DWORD 1 */
+	u8 cq_id_send[10];	/* DWORD 1 */
+	u8 rsvd3[32];	/* DWORD 2 */
+	u8 rsvd4[32];	/* DWORD 3 */
+	u8 cur_bytes[32];	/* DWORD 4 */
+	u8 max_bytes[32];	/* DWORD 5 */
+	u8 time_stamp[32];	/* DWORD 6 */
+	u8 rsvd5[11];	/* DWORD 7 */
+	u8 func;		/* DWORD 7 */
+	u8 rsvd6[20];	/* DWORD 7 */
+	u8 cur_txd_count[32];	/* DWORD 8 */
+	u8 max_txd_count[32];	/* DWORD 9 */
+	u8 rsvd7[32];	/* DWORD 10 */
+	u8 rsvd8[32];	/* DWORD 11 */
+	u8 rsvd9[32];	/* DWORD 12 */
+	u8 rsvd10[32];	/* DWORD 13 */
+	u8 rsvd11[32];	/* DWORD 14 */
+	u8 rsvd12[32];	/* DWORD 15 */
+} __packed;
+struct ETX_CONTEXT_AMAP {
+	u32 dw[16];
+};
+
+#endif /* __etx_context_amap_h__ */
diff --git a/drivers/staging/benet/funcobj.c b/drivers/staging/benet/funcobj.c
new file mode 100644
index 000000000000..0f57eb58daef
--- /dev/null
+++ b/drivers/staging/benet/funcobj.c
@@ -0,0 +1,565 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#include "hwlib.h"
+#include "bestatus.h"
+
+
+int
+be_function_internal_query_firmware_config(struct be_function_object *pfob,
+				   struct BE_FIRMWARE_CONFIG *config)
+{
+	struct FWCMD_COMMON_FIRMWARE_CONFIG *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+	struct be_mcc_wrb_response_copy rc;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_FIRMWARE_CONFIG);
+
+	rc.length = FIELD_SIZEOF(struct FWCMD_COMMON_FIRMWARE_CONFIG,
+					params.response);
+	rc.fwcmd_offset = offsetof(struct FWCMD_COMMON_FIRMWARE_CONFIG,
+					params.response);
+	rc.va = config;
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL,
+					NULL, NULL, NULL, fwcmd, &rc);
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This allocates and initializes a function object based on the information
+    provided by upper layer drivers.
+
+    Returns BE_SUCCESS on success and an appropriate int on failure.
+
+    A function object represents a single BladeEngine (logical) PCI function.
+    That is a function object either represents
+    the networking side of BladeEngine or the iSCSI side of BladeEngine.
+
+    This routine will also detect and create an appropriate PD object for the
+    PCI function as needed.
+*/
+int
+be_function_object_create(u8 __iomem *csr_va, u8 __iomem *db_va,
+		u8 __iomem *pci_va, u32 function_type,
+		struct ring_desc *mailbox, struct be_function_object *pfob)
+{
+	int status;
+
+	ASSERT(pfob);	/* not a magic assert */
+	ASSERT(function_type <= 2);
+
+	TRACE(DL_INFO, "Create function object. type:%s object:0x%p",
+	      (function_type == BE_FUNCTION_TYPE_ISCSI ? "iSCSI" :
+	       (function_type == BE_FUNCTION_TYPE_NETWORK ? "Network" :
+		"Arm")), pfob);
+
+	memset(pfob, 0, sizeof(*pfob));
+
+	pfob->type = function_type;
+	pfob->csr_va = csr_va;
+	pfob->db_va = db_va;
+	pfob->pci_va = pci_va;
+
+	spin_lock_init(&pfob->cq_lock);
+	spin_lock_init(&pfob->post_lock);
+	spin_lock_init(&pfob->mcc_context_lock);
+
+
+	pfob->pci_function_number = 1;
+
+
+	pfob->emulate = false;
+	TRACE(DL_NOTE, "Non-emulation mode");
+	status = be_drive_POST(pfob);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "BladeEngine POST failed.");
+		goto error;
+	}
+
+	/* Initialize the mailbox */
+	status = be_mpu_init_mailbox(pfob, mailbox);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "Failed to initialize mailbox.");
+		goto error;
+	}
+	/*
+	 * Cache the firmware config for ASSERTs in hwclib and later
+	 * driver queries.
+	 */
+	status = be_function_internal_query_firmware_config(pfob,
+					       &pfob->fw_config);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "Failed to query firmware config.");
+		goto error;
+	}
+
+error:
+	if (status != BE_SUCCESS) {
+		/* No cleanup necessary */
+		TRACE(DL_ERR, "Failed to create function.");
+		memset(pfob, 0, sizeof(*pfob));
+	}
+	return status;
+}
+
+/*
+    This routine drops the reference count on a given function object. Once
+    the reference count falls to zero, the function object is destroyed and all
+    resources held are freed.
+
+    FunctionObject      - The function object to drop the reference to.
+*/
+int be_function_object_destroy(struct be_function_object *pfob)
+{
+	TRACE(DL_INFO, "Destroy pfob. Object:0x%p",
+	      pfob);
+
+
+	ASSERT(pfob->mcc == NULL);
+
+	return BE_SUCCESS;
+}
+
+int be_function_cleanup(struct be_function_object *pfob)
+{
+	int status = 0;
+	u32 isr;
+	u32 host_intr;
+	struct PCICFG_HOST_TIMER_INT_CTRL_CSR_AMAP ctrl;
+
+
+	if (pfob->type == BE_FUNCTION_TYPE_NETWORK) {
+		status = be_rxf_multicast_config(pfob, false, 0,
+						NULL, NULL, NULL, NULL);
+		ASSERT(status == BE_SUCCESS);
+	}
+	/* VLAN */
+	status = be_rxf_vlan_config(pfob, false, 0, NULL, NULL, NULL, NULL);
+	ASSERT(status == BE_SUCCESS);
+	/*
+	 * MCC Queue -- Switches to mailbox mode.  May want to destroy
+	 * all but the MCC CQ before this call if polling CQ is much better
+	 * performance than polling mailbox register.
+	 */
+	if (pfob->mcc)
+		status = be_mcc_ring_destroy(pfob->mcc);
+	/*
+	 * If interrupts are disabled, clear any CEV interrupt assertions that
+	 * fired after we stopped processing EQs.
+	 */
+	ctrl.dw[0] = PCICFG1_READ(pfob, host_timer_int_ctrl);
+	host_intr = AMAP_GET_BITS_PTR(PCICFG_HOST_TIMER_INT_CTRL_CSR,
+							hostintr, ctrl.dw);
+	if (!host_intr)
+		if (pfob->type == BE_FUNCTION_TYPE_NETWORK)
+			isr = CSR_READ(pfob, cev.isr1);
+		else
+			isr = CSR_READ(pfob, cev.isr0);
+	else
+		/* This should never happen... */
+		TRACE(DL_ERR, "function_cleanup called with interrupt enabled");
+	/* Function object destroy */
+	status = be_function_object_destroy(pfob);
+	ASSERT(status == BE_SUCCESS);
+
+	return status;
+}
+
+
+void *
+be_function_prepare_embedded_fwcmd(struct be_function_object *pfob,
+	struct MCC_WRB_AMAP *wrb, u32 payld_len, u32 request_length,
+	u32 response_length, u32 opcode, u32 subsystem)
+{
+	struct FWCMD_REQUEST_HEADER *header = NULL;
+	u32 n;
+
+	ASSERT(wrb);
+
+	n = offsetof(struct BE_MCC_WRB_AMAP, payload)/8;
+	AMAP_SET_BITS_PTR(MCC_WRB, embedded, wrb, 1);
+	AMAP_SET_BITS_PTR(MCC_WRB, payload_length, wrb, min(payld_len, n));
+	header = (struct FWCMD_REQUEST_HEADER *)((u8 *)wrb + n);
+
+	header->timeout = 0;
+	header->domain = 0;
+	header->request_length = max(request_length, response_length);
+	header->opcode = opcode;
+	header->subsystem = subsystem;
+
+	return header;
+}
+
+void *
+be_function_prepare_nonembedded_fwcmd(struct be_function_object *pfob,
+	struct MCC_WRB_AMAP *wrb,
+	void *fwcmd_va, u64 fwcmd_pa,
+	u32 payld_len,
+	u32 request_length,
+	u32 response_length,
+	u32 opcode, u32 subsystem)
+{
+	struct FWCMD_REQUEST_HEADER *header = NULL;
+	u32 n;
+	struct MCC_WRB_PAYLOAD_AMAP *plp;
+
+	ASSERT(wrb);
+	ASSERT(fwcmd_va);
+
+	header = (struct FWCMD_REQUEST_HEADER *) fwcmd_va;
+
+	AMAP_SET_BITS_PTR(MCC_WRB, embedded, wrb, 0);
+	AMAP_SET_BITS_PTR(MCC_WRB, payload_length, wrb, payld_len);
+
+	/*
+	 * Assume one fragment. The caller may override the SGL by
+	 * rewriting the 0th length and adding more entries.  They
+	 * will also need to update the sge_count.
+	 */
+	AMAP_SET_BITS_PTR(MCC_WRB, sge_count, wrb, 1);
+
+	n = offsetof(struct BE_MCC_WRB_AMAP, payload)/8;
+	plp = (struct MCC_WRB_PAYLOAD_AMAP *)((u8 *)wrb + n);
+	AMAP_SET_BITS_PTR(MCC_WRB_PAYLOAD, sgl[0].length, plp, payld_len);
+	AMAP_SET_BITS_PTR(MCC_WRB_PAYLOAD, sgl[0].pa_lo, plp, (u32)fwcmd_pa);
+	AMAP_SET_BITS_PTR(MCC_WRB_PAYLOAD, sgl[0].pa_hi, plp,
+					upper_32_bits(fwcmd_pa));
+
+	header->timeout = 0;
+	header->domain = 0;
+	header->request_length = max(request_length, response_length);
+	header->opcode = opcode;
+	header->subsystem = subsystem;
+
+	return header;
+}
+
+struct MCC_WRB_AMAP *
+be_function_peek_mcc_wrb(struct be_function_object *pfob)
+{
+	struct MCC_WRB_AMAP *wrb = NULL;
+	u32 offset;
+
+	if (pfob->mcc)
+		wrb = _be_mpu_peek_ring_wrb(pfob->mcc, false);
+	else {
+		offset = offsetof(struct BE_MCC_MAILBOX_AMAP, wrb)/8;
+		wrb = (struct MCC_WRB_AMAP *) ((u8 *) pfob->mailbox.va +
+				offset);
+	}
+
+	if (wrb)
+		memset(wrb, 0, sizeof(struct MCC_WRB_AMAP));
+
+	return wrb;
+}
+
+#if defined(BE_DEBUG)
+void be_function_debug_print_wrb(struct be_function_object *pfob,
+		struct MCC_WRB_AMAP *wrb, void *optional_fwcmd_va,
+		struct be_mcc_wrb_context *wrb_context)
+{
+
+	struct FWCMD_REQUEST_HEADER *header = NULL;
+	u8 embedded;
+	u32 n;
+
+	embedded = AMAP_GET_BITS_PTR(MCC_WRB, embedded, wrb);
+
+	if (embedded) {
+		n = offsetof(struct BE_MCC_WRB_AMAP, payload)/8;
+		header = (struct FWCMD_REQUEST_HEADER *)((u8 *)wrb + n);
+	} else {
+		header = (struct FWCMD_REQUEST_HEADER *) optional_fwcmd_va;
+	}
+
+	/* Save the completed count before posting for a debug assert. */
+
+	if (header) {
+		wrb_context->opcode = header->opcode;
+		wrb_context->subsystem = header->subsystem;
+
+	} else {
+		wrb_context->opcode = 0;
+		wrb_context->subsystem = 0;
+	}
+}
+#else
+#define be_function_debug_print_wrb(a_, b_, c_, d_)
+#endif
+
+int
+be_function_post_mcc_wrb(struct be_function_object *pfob,
+		struct MCC_WRB_AMAP *wrb,
+		struct be_generic_q_ctxt *q_ctxt,
+		mcc_wrb_cqe_callback cb, void *cb_context,
+		mcc_wrb_cqe_callback internal_cb,
+		void *internal_cb_context, void *optional_fwcmd_va,
+		struct be_mcc_wrb_response_copy *rc)
+{
+	int status;
+	struct be_mcc_wrb_context *wrb_context = NULL;
+	u64 *p;
+
+	if (q_ctxt) {
+		/* Initialize context.         */
+		q_ctxt->context.internal_cb = internal_cb;
+		q_ctxt->context.internal_cb_context = internal_cb_context;
+		q_ctxt->context.cb = cb;
+		q_ctxt->context.cb_context = cb_context;
+		if (rc) {
+			q_ctxt->context.copy.length = rc->length;
+			q_ctxt->context.copy.fwcmd_offset = rc->fwcmd_offset;
+			q_ctxt->context.copy.va = rc->va;
+		} else
+			q_ctxt->context.copy.length = 0;
+
+		q_ctxt->context.optional_fwcmd_va = optional_fwcmd_va;
+
+		/* Queue this request */
+		status = be_function_queue_mcc_wrb(pfob, q_ctxt);
+
+		goto Error;
+	}
+	/*
+	 * Allocate a WRB context struct to hold the callback pointers,
+	 * status, etc.  This is required if commands complete out of order.
+	 */
+	wrb_context = _be_mcc_allocate_wrb_context(pfob);
+	if (!wrb_context) {
+		TRACE(DL_WARN, "Failed to allocate MCC WRB context.");
+		status = BE_STATUS_SYSTEM_RESOURCES;
+		goto Error;
+	}
+	/* Initialize context. */
+	memset(wrb_context, 0, sizeof(*wrb_context));
+	wrb_context->internal_cb = internal_cb;
+	wrb_context->internal_cb_context = internal_cb_context;
+	wrb_context->cb = cb;
+	wrb_context->cb_context = cb_context;
+	if (rc) {
+		wrb_context->copy.length = rc->length;
+		wrb_context->copy.fwcmd_offset = rc->fwcmd_offset;
+		wrb_context->copy.va = rc->va;
+	} else
+		wrb_context->copy.length = 0;
+	wrb_context->wrb = wrb;
+
+	/*
+	 * Copy the context pointer into the WRB opaque tag field.
+	 * Verify assumption of 64-bit tag with a compile time assert.
+	 */
+	p = (u64 *) ((u8 *)wrb + offsetof(struct BE_MCC_WRB_AMAP, tag)/8);
+	*p = (u64)(size_t)wrb_context;
+
+	/* Print info about this FWCMD for debug builds. */
+	be_function_debug_print_wrb(pfob, wrb, optional_fwcmd_va, wrb_context);
+
+	/*
+	 * issue the WRB to the MPU as appropriate
+	 */
+	if (pfob->mcc) {
+		/*
+		 * we're in WRB mode, pass to the mcc layer
+		 */
+		status = _be_mpu_post_wrb_ring(pfob->mcc, wrb, wrb_context);
+	} else {
+		/*
+		 * we're in mailbox mode
+		 */
+		status = _be_mpu_post_wrb_mailbox(pfob, wrb, wrb_context);
+
+		/* mailbox mode always completes synchronously */
+		ASSERT(status != BE_STATUS_PENDING);
+	}
+
+Error:
+
+	return status;
+}
+
+int
+be_function_ring_destroy(struct be_function_object *pfob,
+		u32 id, u32 ring_type, mcc_wrb_cqe_callback cb,
+		void *cb_context, mcc_wrb_cqe_callback internal_cb,
+		void *internal_cb_context)
+{
+
+	struct FWCMD_COMMON_RING_DESTROY *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	TRACE(DL_INFO, "Destroy ring id:%d type:%d", id, ring_type);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		ASSERT(wrb);
+		TRACE(DL_ERR, "No free MCC WRBs in destroy ring.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_RING_DESTROY);
+
+	fwcmd->params.request.id = id;
+	fwcmd->params.request.ring_type = ring_type;
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, cb, cb_context,
+				internal_cb, internal_cb_context, fwcmd, NULL);
+	if (status != BE_SUCCESS && status != BE_PENDING) {
+		TRACE(DL_ERR, "Ring destroy fwcmd failed. id:%d ring_type:%d",
+			id, ring_type);
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+void
+be_rd_to_pa_list(struct ring_desc *rd, struct PHYS_ADDR *pa_list, u32 max_num)
+{
+	u32 num_pages = PAGES_SPANNED(rd->va, rd->length);
+	u32 i = 0;
+	u64 pa = rd->pa;
+	__le64 lepa;
+
+	ASSERT(pa_list);
+	ASSERT(pa);
+
+	for (i = 0; i < min(num_pages, max_num); i++) {
+		lepa = cpu_to_le64(pa);
+		pa_list[i].lo = (u32)lepa;
+		pa_list[i].hi = upper_32_bits(lepa);
+		pa += PAGE_SIZE;
+	}
+}
+
+
+
+/*-----------------------------------------------------------------------------
+ * Function: be_function_get_fw_version
+ *   Retrieves the firmware version on the adpater. If the callback is
+ *   NULL this call executes synchronously. If the callback is not NULL,
+ *   the returned status will be BE_PENDING if the command was issued
+ *   successfully.
+ * pfob    -
+ * fwv         - Pointer to response buffer if callback is NULL.
+ * cb           - Callback function invoked when the FWCMD completes.
+ * cb_context   - Passed to the callback function.
+ * return pend_status - BE_SUCCESS (0) on success.
+ * 			BE_PENDING (postive value) if the FWCMD
+ *                      completion is pending. Negative error code on failure.
+ *---------------------------------------------------------------------------
+ */
+int
+be_function_get_fw_version(struct be_function_object *pfob,
+		struct FWCMD_COMMON_GET_FW_VERSION_RESPONSE_PAYLOAD *fwv,
+		mcc_wrb_cqe_callback cb, void *cb_context)
+{
+	int status = BE_SUCCESS;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	struct FWCMD_COMMON_GET_FW_VERSION *fwcmd = NULL;
+	unsigned long irql;
+	struct be_mcc_wrb_response_copy rc;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+
+	if (!cb && !fwv) {
+		TRACE(DL_ERR, "callback and response buffer NULL!");
+		status = BE_NOT_OK;
+		goto Error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_GET_FW_VERSION);
+
+	rc.length = FIELD_SIZEOF(struct FWCMD_COMMON_GET_FW_VERSION,
+					params.response);
+	rc.fwcmd_offset = offsetof(struct FWCMD_COMMON_GET_FW_VERSION,
+					params.response);
+	rc.va = fwv;
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, cb,
+				cb_context, NULL, NULL, fwcmd, &rc);
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+int
+be_function_queue_mcc_wrb(struct be_function_object *pfob,
+			  struct be_generic_q_ctxt *q_ctxt)
+{
+	int status;
+
+	ASSERT(q_ctxt);
+
+	/*
+	 * issue the WRB to the MPU as appropriate
+	 */
+	if (pfob->mcc) {
+
+		/* We're in ring mode.  Queue this item. */
+		pfob->mcc->backlog_length++;
+		list_add_tail(&q_ctxt->context.list, &pfob->mcc->backlog);
+		status = BE_PENDING;
+	} else {
+		status = BE_NOT_OK;
+	}
+	return status;
+}
+
diff --git a/drivers/staging/benet/fwcmd_common.h b/drivers/staging/benet/fwcmd_common.h
new file mode 100644
index 000000000000..406e0d6fa985
--- /dev/null
+++ b/drivers/staging/benet/fwcmd_common.h
@@ -0,0 +1,222 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __fwcmd_common_amap_h__
+#define __fwcmd_common_amap_h__
+#include "host_struct.h"
+
+/* --- PHY_LINK_DUPLEX_ENUM --- */
+#define PHY_LINK_DUPLEX_NONE            (0)
+#define PHY_LINK_DUPLEX_HALF            (1)
+#define PHY_LINK_DUPLEX_FULL            (2)
+
+/* --- PHY_LINK_SPEED_ENUM --- */
+#define PHY_LINK_SPEED_ZERO             (0)	/* No link. */
+#define PHY_LINK_SPEED_10MBPS           (1)	/* 10 Mbps */
+#define PHY_LINK_SPEED_100MBPS          (2)	/* 100 Mbps */
+#define PHY_LINK_SPEED_1GBPS            (3)	/* 1 Gbps */
+#define PHY_LINK_SPEED_10GBPS           (4)	/* 10 Gbps */
+
+/* --- PHY_LINK_FAULT_ENUM --- */
+#define PHY_LINK_FAULT_NONE             (0)	/* No fault status
+							available or detected */
+#define PHY_LINK_FAULT_LOCAL            (1)	/* Local fault detected */
+#define PHY_LINK_FAULT_REMOTE           (2)	/* Remote fault detected */
+
+/* --- BE_ULP_MASK --- */
+#define BE_ULP0_MASK                    (1)
+#define BE_ULP1_MASK                    (2)
+#define BE_ULP2_MASK                    (4)
+
+/* --- NTWK_ACTIVE_PORT --- */
+#define NTWK_PORT_A                     (0)	/* Port A is currently active */
+#define NTWK_PORT_B                     (1)	/* Port B is currently active */
+#define NTWK_NO_ACTIVE_PORT             (15)	/* Both ports have lost link */
+
+/* --- NTWK_LINK_TYPE --- */
+#define NTWK_LINK_TYPE_PHYSICAL         (0)	/* link up/down event
+						   applies to BladeEngine's
+						   Physical Ports
+						   */
+#define NTWK_LINK_TYPE_VIRTUAL          (1)	/* Virtual link up/down event
+						   reported by BladeExchange.
+						   This applies only when the
+						   VLD feature is enabled
+						   */
+
+/*
+ * --- FWCMD_MAC_TYPE_ENUM ---
+ * This enum defines the types of MAC addresses in the RXF MAC Address Table.
+ */
+#define MAC_ADDRESS_TYPE_STORAGE        (0)	/* Storage MAC Address */
+#define MAC_ADDRESS_TYPE_NETWORK        (1)	/* Network MAC Address */
+#define MAC_ADDRESS_TYPE_PD             (2)	/* Protection Domain MAC Addr */
+#define MAC_ADDRESS_TYPE_MANAGEMENT     (3)	/* Managment MAC Address */
+
+
+/* --- FWCMD_RING_TYPE_ENUM --- */
+#define FWCMD_RING_TYPE_ETH_RX          (1)	/* Ring created with */
+					/* FWCMD_COMMON_ETH_RX_CREATE. */
+#define FWCMD_RING_TYPE_ETH_TX          (2)	/* Ring created with */
+					/* FWCMD_COMMON_ETH_TX_CREATE. */
+#define FWCMD_RING_TYPE_ISCSI_WRBQ      (3)	/* Ring created with */
+					/* FWCMD_COMMON_ISCSI_WRBQ_CREATE. */
+#define FWCMD_RING_TYPE_ISCSI_DEFQ      (4)	/* Ring created with */
+					/* FWCMD_COMMON_ISCSI_DEFQ_CREATE. */
+#define FWCMD_RING_TYPE_TPM_WRBQ        (5)	/* Ring created with */
+					/* FWCMD_COMMON_TPM_WRBQ_CREATE. */
+#define FWCMD_RING_TYPE_TPM_DEFQ        (6)	/* Ring created with */
+					/* FWCMD_COMMONTPM_TDEFQ_CREATE. */
+#define FWCMD_RING_TYPE_TPM_RQ          (7)	/* Ring created with */
+					/* FWCMD_COMMON_TPM_RQ_CREATE. */
+#define FWCMD_RING_TYPE_MCC             (8)	/* Ring created with */
+					/* FWCMD_COMMON_MCC_CREATE. */
+#define FWCMD_RING_TYPE_CQ              (9)	/* Ring created with */
+					/* FWCMD_COMMON_CQ_CREATE. */
+#define FWCMD_RING_TYPE_EQ              (10)	/* Ring created with */
+					/* FWCMD_COMMON_EQ_CREATE. */
+#define FWCMD_RING_TYPE_QP              (11)	/* Ring created with */
+					/* FWCMD_RDMA_QP_CREATE. */
+
+
+/* --- ETH_TX_RING_TYPE_ENUM --- */
+#define ETH_TX_RING_TYPE_FORWARDING     (1)	/* Ethernet ring for
+						   forwarding packets */
+#define ETH_TX_RING_TYPE_STANDARD       (2)	/* Ethernet ring for sending
+						   network packets. */
+#define ETH_TX_RING_TYPE_BOUND          (3)	/* Ethernet ring bound to the
+						   port specified in the command
+						   header.port_number field.
+						   Rings of this type are
+						   NOT subject to the
+						   failover logic implemented
+						   in the BladeEngine.
+						   */
+
+/* --- FWCMD_COMMON_QOS_TYPE_ENUM --- */
+#define QOS_BITS_NIC                    (1)	/* max_bits_per_second_NIC */
+						  /* field is valid.  */
+#define QOS_PKTS_NIC                    (2)	/* max_packets_per_second_NIC */
+						  /* field is valid.  */
+#define QOS_IOPS_ISCSI                  (4)	/* max_ios_per_second_iSCSI */
+						  /*field is valid.  */
+#define QOS_VLAN_TAG                    (8)	/* domain_VLAN_tag field
+						   is valid. */
+#define QOS_FABRIC_ID                   (16)	/* fabric_domain_ID field
+						   is valid. */
+#define QOS_OEM_PARAMS                  (32)	/* qos_params_oem field
+						   is valid. */
+#define QOS_TPUT_ISCSI                  (64)	/* max_bytes_per_second_iSCSI
+						   field  is valid.  */
+
+
+/*
+ * --- FAILOVER_CONFIG_ENUM ---
+ * Failover configuration setting used in FWCMD_COMMON_FORCE_FAILOVER
+ */
+#define FAILOVER_CONFIG_NO_CHANGE       (0)	/* No change to automatic */
+						  /* port failover setting. */
+#define FAILOVER_CONFIG_ON              (1)	/* Automatic port failover
+						   on link down  is enabled. */
+#define FAILOVER_CONFIG_OFF             (2)	/* Automatic port failover
+						   on link down is disabled. */
+
+/*
+ * --- FAILOVER_PORT_ENUM ---
+ * Failover port setting used in FWCMD_COMMON_FORCE_FAILOVER
+ */
+#define FAILOVER_PORT_A                 (0)	/* Selects port A. */
+#define FAILOVER_PORT_B                 (1)	/* Selects port B. */
+#define FAILOVER_PORT_NONE              (15)	/* No port change requested. */
+
+
+/*
+ * --- MGMT_FLASHROM_OPCODE ---
+ * Flash ROM operation code
+ */
+#define MGMT_FLASHROM_OPCODE_FLASH      (1)	/* Commit downloaded data
+						   to Flash ROM */
+#define MGMT_FLASHROM_OPCODE_SAVE       (2)	/* Save downloaded data to
+						   ARM's DDR - do not flash */
+#define MGMT_FLASHROM_OPCODE_CLEAR      (3)	/* Erase specified component
+						   from FlashROM */
+#define MGMT_FLASHROM_OPCODE_REPORT     (4)	/* Read specified component
+						   from Flash ROM */
+#define MGMT_FLASHROM_OPCODE_IMAGE_INFO (5)	/* Returns size of a
+						   component */
+
+/*
+ * --- MGMT_FLASHROM_OPTYPE ---
+ * Flash ROM operation type
+ */
+#define MGMT_FLASHROM_OPTYPE_CODE_FIRMWARE (0)	/* Includes ARM firmware,
+						   IPSec (optional) and EP
+						   firmware  */
+#define MGMT_FLASHROM_OPTYPE_CODE_REDBOOT (1)
+#define MGMT_FLASHROM_OPTYPE_CODE_BIOS  (2)
+#define MGMT_FLASHROM_OPTYPE_CODE_PXE_BIOS (3)
+#define MGMT_FLASHROM_OPTYPE_CODE_CTRLS (4)
+#define MGMT_FLASHROM_OPTYPE_CFG_IPSEC  (5)
+#define MGMT_FLASHROM_OPTYPE_CFG_INI    (6)
+#define MGMT_FLASHROM_OPTYPE_ROM_OFFSET_SPECIFIED (7)
+
+/*
+ * --- FLASHROM_TYPE ---
+ * Flash ROM manufacturers supported in the f/w
+ */
+#define INTEL                           (0)
+#define SPANSION                        (1)
+#define MICRON                          (2)
+
+/* --- DDR_CAS_TYPE --- */
+#define CAS_3                           (0)
+#define CAS_4                           (1)
+#define CAS_5                           (2)
+
+/* --- DDR_SIZE_TYPE --- */
+#define SIZE_256MB                      (0)
+#define SIZE_512MB                      (1)
+
+/* --- DDR_MODE_TYPE --- */
+#define DDR_NO_ECC                      (0)
+#define DDR_ECC                         (1)
+
+/* --- INTERFACE_10GB_TYPE --- */
+#define CX4_TYPE                        (0)
+#define XFP_TYPE                        (1)
+
+/* --- BE_CHIP_MAX_MTU --- */
+#define CHIP_MAX_MTU                    (9000)
+
+/* --- XAUI_STATE_ENUM --- */
+#define XAUI_STATE_ENABLE               (0)	/* This MUST be the default
+						   value for all requests
+						   which set/change
+						   equalization parameter.  */
+#define XAUI_STATE_DISABLE              (255)	/* The XAUI for both ports
+						   may be disabled for EMI
+						   tests. There is no
+						   provision for turning off
+						   individual ports.
+						   */
+/* --- BE_ASIC_REVISION --- */
+#define BE_ASIC_REV_A0                  (1)
+#define BE_ASIC_REV_A1                  (2)
+
+#endif /* __fwcmd_common_amap_h__ */
diff --git a/drivers/staging/benet/fwcmd_common_bmap.h b/drivers/staging/benet/fwcmd_common_bmap.h
new file mode 100644
index 000000000000..a007cf276500
--- /dev/null
+++ b/drivers/staging/benet/fwcmd_common_bmap.h
@@ -0,0 +1,717 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __fwcmd_common_bmap_h__
+#define __fwcmd_common_bmap_h__
+#include "fwcmd_types_bmap.h"
+#include "fwcmd_hdr_bmap.h"
+
+#if defined(__BIG_ENDIAN)
+   /* Physical Address. */
+struct PHYS_ADDR {
+	union {
+		struct {
+			u32 lo;	/* DWORD 0 */
+			u32 hi;	/* DWORD 1 */
+		} __packed;	/* unnamed struct */
+		u32 dw[2];	/* dword union */
+	};			/* unnamed union */
+} __packed ;
+
+
+#else
+   /* Physical Address. */
+struct PHYS_ADDR {
+	union {
+		struct {
+			u32 lo;	/* DWORD 0 */
+			u32 hi;	/* DWORD 1 */
+		} __packed;	/* unnamed struct */
+		u32 dw[2];	/* dword union */
+	};			/* unnamed union */
+} __packed ;
+
+struct BE_LINK_STATUS {
+	u8 mac0_duplex;
+	u8 mac0_speed;
+	u8 mac1_duplex;
+	u8 mac1_speed;
+	u8 mgmt_mac_duplex;
+	u8 mgmt_mac_speed;
+	u8 active_port;
+	u8 rsvd0;
+	u8 mac0_fault;
+	u8 mac1_fault;
+	u16 rsvd1;
+} __packed;
+#endif
+
+struct FWCMD_COMMON_ANON_170_REQUEST {
+	u32 rsvd0;
+} __packed;
+
+union LINK_STATUS_QUERY_PARAMS {
+	struct BE_LINK_STATUS response;
+	struct FWCMD_COMMON_ANON_170_REQUEST request;
+} __packed;
+
+/*
+ *  Queries the the link status for all ports.  The valid values below
+ *  DO NOT indicate that  a particular duplex or speed is supported by
+ *  BladeEngine. These enumerations simply  list all possible duplexes
+ *  and speeds for any port. Consult BladeEngine product  documentation
+ *  for the supported parameters.
+ */
+struct FWCMD_COMMON_NTWK_LINK_STATUS_QUERY {
+	union FWCMD_HEADER header;
+	union LINK_STATUS_QUERY_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_171_REQUEST {
+	u8 type;
+	u8 port;
+	u8 mac1;
+	u8 permanent;
+} __packed;
+
+struct FWCMD_COMMON_ANON_172_RESPONSE {
+	struct MAC_ADDRESS_FORMAT mac;
+} __packed;
+
+union NTWK_MAC_QUERY_PARAMS {
+	struct FWCMD_COMMON_ANON_171_REQUEST request;
+	struct FWCMD_COMMON_ANON_172_RESPONSE response;
+} __packed;
+
+/* Queries one MAC address.  */
+struct FWCMD_COMMON_NTWK_MAC_QUERY {
+	union FWCMD_HEADER header;
+	union NTWK_MAC_QUERY_PARAMS params;
+} __packed;
+
+struct MAC_SET_PARAMS_IN {
+	u8 type;
+	u8 port;
+	u8 mac1;
+	u8 invalidate;
+	struct MAC_ADDRESS_FORMAT mac;
+} __packed;
+
+struct MAC_SET_PARAMS_OUT {
+	u32 rsvd0;
+} __packed;
+
+union MAC_SET_PARAMS {
+	struct MAC_SET_PARAMS_IN request;
+	struct MAC_SET_PARAMS_OUT response;
+} __packed;
+
+/* Sets a MAC address.  */
+struct FWCMD_COMMON_NTWK_MAC_SET {
+	union FWCMD_HEADER header;
+	union MAC_SET_PARAMS params;
+} __packed;
+
+/* MAC address list. */
+struct NTWK_MULTICAST_MAC_LIST {
+	u8 byte[6];
+} __packed;
+
+struct FWCMD_COMMON_NTWK_MULTICAST_SET_REQUEST_PAYLOAD {
+	u16 num_mac;
+	u8 promiscuous;
+	u8 rsvd0;
+	struct NTWK_MULTICAST_MAC_LIST mac[32];
+} __packed;
+
+struct FWCMD_COMMON_ANON_174_RESPONSE {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_COMMON_ANON_173_PARAMS {
+	struct FWCMD_COMMON_NTWK_MULTICAST_SET_REQUEST_PAYLOAD request;
+	struct FWCMD_COMMON_ANON_174_RESPONSE response;
+} __packed;
+
+/*
+ *  Sets multicast address hash. The MPU will merge the MAC address lists
+ *  from all clients,  including the networking and storage functions.
+ *  This command may fail if the final merged  list of MAC addresses exceeds
+ *  32 entries.
+ */
+struct FWCMD_COMMON_NTWK_MULTICAST_SET {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_173_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_NTWK_VLAN_CONFIG_REQUEST_PAYLOAD {
+	u16 num_vlan;
+	u8 promiscuous;
+	u8 rsvd0;
+	u16 vlan_tag[32];
+} __packed;
+
+struct FWCMD_COMMON_ANON_176_RESPONSE {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_COMMON_ANON_175_PARAMS {
+	struct FWCMD_COMMON_NTWK_VLAN_CONFIG_REQUEST_PAYLOAD request;
+	struct FWCMD_COMMON_ANON_176_RESPONSE response;
+} __packed;
+
+/*
+ *  Sets VLAN tag filter. The MPU will merge the VLAN tag list from all
+ *  clients, including  the networking and storage functions. This command
+ *  may fail if the final vlan_tag array  (from all functions) is longer
+ *  than 32 entries.
+ */
+struct FWCMD_COMMON_NTWK_VLAN_CONFIG {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_175_PARAMS params;
+} __packed;
+
+struct RING_DESTROY_REQUEST {
+	u16 ring_type;
+	u16 id;
+	u8 bypass_flush;
+	u8 rsvd0;
+	u16 rsvd1;
+} __packed;
+
+struct FWCMD_COMMON_ANON_190_RESPONSE {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_COMMON_ANON_189_PARAMS {
+	struct RING_DESTROY_REQUEST request;
+	struct FWCMD_COMMON_ANON_190_RESPONSE response;
+} __packed;
+/*
+ *  Command for destroying any ring. The connection(s) using the ring should
+ *  be quiesced  before destroying the ring.
+ */
+struct FWCMD_COMMON_RING_DESTROY {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_189_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_192_REQUEST {
+	u16 num_pages;
+	u16 rsvd0;
+	struct CQ_CONTEXT_AMAP context;
+	struct PHYS_ADDR pages[4];
+} __packed ;
+
+struct FWCMD_COMMON_ANON_193_RESPONSE {
+	u16 cq_id;
+} __packed ;
+
+union FWCMD_COMMON_ANON_191_PARAMS {
+	struct FWCMD_COMMON_ANON_192_REQUEST request;
+	struct FWCMD_COMMON_ANON_193_RESPONSE response;
+} __packed ;
+
+/*
+ *  Command for creating a completion queue. A Completion Queue must span
+ *  at least 1 page and  at most 4 pages. Each completion queue entry
+ *  is 16 bytes regardless of CQ entry format.  Thus the ring must be
+ *  at least 256 entries deep (corresponding to 1 page) and can be at
+ *   most 1024 entries deep (corresponding to 4 pages). The number of
+ *  pages posted must  contain the CQ ring size as encoded in the context.
+ *
+ */
+struct FWCMD_COMMON_CQ_CREATE {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_191_PARAMS params;
+} __packed ;
+
+struct FWCMD_COMMON_ANON_198_REQUEST {
+	u16 num_pages;
+	u16 rsvd0;
+	struct EQ_CONTEXT_AMAP context;
+	struct PHYS_ADDR pages[8];
+} __packed ;
+
+struct FWCMD_COMMON_ANON_199_RESPONSE {
+	u16 eq_id;
+} __packed ;
+
+union FWCMD_COMMON_ANON_197_PARAMS {
+	struct FWCMD_COMMON_ANON_198_REQUEST request;
+	struct FWCMD_COMMON_ANON_199_RESPONSE response;
+} __packed ;
+
+/*
+ *  Command for creating a event queue. An Event Queue must span at least
+ *  1 page and at most  8 pages. The number of pages posted must contain
+ *  the EQ ring. The ring is defined by  the size of the EQ entries (encoded
+ *  in the context) and the number of EQ entries (also  encoded in the
+ *  context).
+ */
+struct FWCMD_COMMON_EQ_CREATE {
+	union  FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_197_PARAMS params;
+} __packed ;
+
+struct FWCMD_COMMON_ANON_201_REQUEST {
+	u16 cq_id;
+	u16 bcmc_cq_id;
+	u16 num_pages;
+	u16 rsvd0;
+	struct PHYS_ADDR pages[2];
+} __packed;
+
+struct FWCMD_COMMON_ANON_202_RESPONSE {
+	u16 id;
+} __packed;
+
+union FWCMD_COMMON_ANON_200_PARAMS {
+	struct FWCMD_COMMON_ANON_201_REQUEST request;
+	struct FWCMD_COMMON_ANON_202_RESPONSE response;
+} __packed;
+
+/*
+ *  Command for creating Ethernet receive ring.  An ERX ring contains ETH_RX_D
+ *  entries (8  bytes each). An ERX ring must be 1024 entries deep
+ *  (corresponding to 2 pages).
+ */
+struct FWCMD_COMMON_ETH_RX_CREATE {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_200_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_204_REQUEST {
+	u16 num_pages;
+	u8 ulp_num;
+	u8 type;
+	struct ETX_CONTEXT_AMAP context;
+	struct PHYS_ADDR pages[8];
+} __packed ;
+
+struct FWCMD_COMMON_ANON_205_RESPONSE {
+	u16 cid;
+	u8 ulp_num;
+	u8 rsvd0;
+} __packed ;
+
+union FWCMD_COMMON_ANON_203_PARAMS {
+	struct FWCMD_COMMON_ANON_204_REQUEST request;
+	struct FWCMD_COMMON_ANON_205_RESPONSE response;
+} __packed ;
+
+/*
+ *  Command for creating an Ethernet transmit ring.  An ETX ring contains
+ *  ETH_WRB entries (16  bytes each). An ETX ring must be at least 256
+ *  entries deep (corresponding to 1 page)  and at most 2k entries deep
+ *  (corresponding to 8 pages).
+ */
+struct FWCMD_COMMON_ETH_TX_CREATE {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_203_PARAMS params;
+} __packed ;
+
+struct FWCMD_COMMON_ANON_222_REQUEST {
+	u16 num_pages;
+	u16 rsvd0;
+	struct MCC_RING_CONTEXT_AMAP context;
+	struct PHYS_ADDR pages[8];
+} __packed ;
+
+struct FWCMD_COMMON_ANON_223_RESPONSE {
+	u16 id;
+} __packed ;
+
+union FWCMD_COMMON_ANON_221_PARAMS {
+	struct FWCMD_COMMON_ANON_222_REQUEST request;
+	struct FWCMD_COMMON_ANON_223_RESPONSE response;
+} __packed ;
+
+/*
+ *  Command for creating the MCC ring. An MCC ring must be at least 16
+ *  entries deep  (corresponding to 1 page) and at most 128 entries deep
+ *  (corresponding to 8 pages).
+ */
+struct FWCMD_COMMON_MCC_CREATE {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_221_PARAMS params;
+} __packed ;
+
+struct GET_QOS_IN {
+	u32 qos_params_rsvd;
+} __packed;
+
+struct GET_QOS_OUT {
+	u32 max_bits_per_second_NIC;
+	u32 max_packets_per_second_NIC;
+	u32 max_ios_per_second_iSCSI;
+	u32 max_bytes_per_second_iSCSI;
+	u16 domain_VLAN_tag;
+	u16 fabric_domain_ID;
+	u32 qos_params_oem[4];
+} __packed;
+
+union GET_QOS_PARAMS {
+	struct GET_QOS_IN request;
+	struct GET_QOS_OUT response;
+} __packed;
+
+/* QOS/Bandwidth settings per domain. Applicable only in VMs.  */
+struct FWCMD_COMMON_GET_QOS {
+	union FWCMD_HEADER header;
+	union GET_QOS_PARAMS params;
+} __packed;
+
+struct SET_QOS_IN {
+	u32 valid_flags;
+	u32 max_bits_per_second_NIC;
+	u32 max_packets_per_second_NIC;
+	u32 max_ios_per_second_iSCSI;
+	u32 max_bytes_per_second_iSCSI;
+	u16 domain_VLAN_tag;
+	u16 fabric_domain_ID;
+	u32 qos_params_oem[4];
+} __packed;
+
+struct SET_QOS_OUT {
+	u32 qos_params_rsvd;
+} __packed;
+
+union SET_QOS_PARAMS {
+	struct SET_QOS_IN request;
+	struct SET_QOS_OUT response;
+} __packed;
+
+/* QOS/Bandwidth settings per domain. Applicable only in VMs.  */
+struct FWCMD_COMMON_SET_QOS {
+	union FWCMD_HEADER header;
+	union SET_QOS_PARAMS params;
+} __packed;
+
+struct SET_FRAME_SIZE_IN {
+	u32 max_tx_frame_size;
+	u32 max_rx_frame_size;
+} __packed;
+
+struct SET_FRAME_SIZE_OUT {
+	u32 chip_max_tx_frame_size;
+	u32 chip_max_rx_frame_size;
+} __packed;
+
+union SET_FRAME_SIZE_PARAMS {
+	struct SET_FRAME_SIZE_IN request;
+	struct SET_FRAME_SIZE_OUT response;
+} __packed;
+
+/* Set frame size command. Only host domain may issue this command.  */
+struct FWCMD_COMMON_SET_FRAME_SIZE {
+	union FWCMD_HEADER header;
+	union SET_FRAME_SIZE_PARAMS params;
+} __packed;
+
+struct FORCE_FAILOVER_IN {
+	u32 move_to_port;
+	u32 failover_config;
+} __packed;
+
+struct FWCMD_COMMON_ANON_231_RESPONSE {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_COMMON_ANON_230_PARAMS {
+	struct FORCE_FAILOVER_IN request;
+	struct FWCMD_COMMON_ANON_231_RESPONSE response;
+} __packed;
+
+/*
+ *  Use this command to control failover in BladeEngine. It may be used
+ *  to failback to a  restored port or to forcibly move traffic from
+ *  one port to another. It may also be used  to enable or disable the
+ *  automatic failover feature. This command can only be issued by  domain
+ *  0.
+ */
+struct FWCMD_COMMON_FORCE_FAILOVER {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_230_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_240_REQUEST {
+	u64 context;
+} __packed;
+
+struct FWCMD_COMMON_ANON_241_RESPONSE {
+	u64 context;
+} __packed;
+
+union FWCMD_COMMON_ANON_239_PARAMS {
+	struct FWCMD_COMMON_ANON_240_REQUEST request;
+	struct FWCMD_COMMON_ANON_241_RESPONSE response;
+} __packed;
+
+/*
+ *  This command can be used by clients as a no-operation request. Typical
+ *  uses for drivers  are as a heartbeat mechanism, or deferred processing
+ *  catalyst. The ARM will always  complete this command with a good completion.
+ *  The 64-bit parameter is not touched by the  ARM processor.
+ */
+struct FWCMD_COMMON_NOP {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_239_PARAMS params;
+} __packed;
+
+struct NTWK_RX_FILTER_SETTINGS {
+	u8 promiscuous;
+	u8 ip_cksum;
+	u8 tcp_cksum;
+	u8 udp_cksum;
+	u8 pass_err;
+	u8 pass_ckerr;
+	u8 strip_crc;
+	u8 mcast_en;
+	u8 bcast_en;
+	u8 mcast_promiscuous_en;
+	u8 unicast_en;
+	u8 vlan_promiscuous;
+} __packed;
+
+union FWCMD_COMMON_ANON_242_PARAMS {
+	struct NTWK_RX_FILTER_SETTINGS request;
+	struct NTWK_RX_FILTER_SETTINGS response;
+} __packed;
+
+/*
+ *  This command is used to modify the ethernet receive filter configuration.
+ *  Only domain 0  network function drivers may issue this command. The
+ *  applied configuration is returned in  the response payload. Note:
+ *  Some receive packet filter settings are global on  BladeEngine and
+ *  can affect both the storage and network function clients that the
+ *   BladeEngine hardware and firmware serve. Additionaly, depending
+ *  on the revision of  BladeEngine, some ethernet receive filter settings
+ *  are dependent on others. If a  dependency exists between settings
+ *  for the BladeEngine revision, and the command request  settings do
+ *  not meet the dependency requirement, the invalid settings will not
+ *  be  applied despite the comand succeeding. For example: a driver may
+ *  request to enable  broadcast packets, but not enable multicast packets.
+ *  On early revisions of BladeEngine,  there may be no distinction between
+ *  broadcast and multicast filters, so broadcast could  not be enabled
+ *  without enabling multicast. In this scenario, the comand would still
+ *   succeed, but the response payload would indicate the previously
+ *  configured broadcast  and multicast setting.
+ */
+struct FWCMD_COMMON_NTWK_RX_FILTER {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_242_PARAMS params;
+} __packed;
+
+
+struct FWCMD_COMMON_ANON_244_REQUEST {
+	u32 rsvd0;
+} __packed;
+
+struct FWCMD_COMMON_GET_FW_VERSION_RESPONSE_PAYLOAD {
+	u8 firmware_version_string[32];
+	u8 fw_on_flash_version_string[32];
+} __packed;
+
+union FWCMD_COMMON_ANON_243_PARAMS {
+	struct FWCMD_COMMON_ANON_244_REQUEST request;
+	struct FWCMD_COMMON_GET_FW_VERSION_RESPONSE_PAYLOAD response;
+} __packed;
+
+/* This comand retrieves the firmware version.  */
+struct FWCMD_COMMON_GET_FW_VERSION {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_243_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_246_REQUEST {
+	u16 tx_flow_control;
+	u16 rx_flow_control;
+} __packed;
+
+struct FWCMD_COMMON_ANON_247_RESPONSE {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_COMMON_ANON_245_PARAMS {
+	struct FWCMD_COMMON_ANON_246_REQUEST request;
+	struct FWCMD_COMMON_ANON_247_RESPONSE response;
+} __packed;
+
+/*
+ *  This comand is used to program BladeEngine flow control behavior.
+ *  Only the host  networking driver is allowed to use this comand.
+ */
+struct FWCMD_COMMON_SET_FLOW_CONTROL {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_245_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_249_REQUEST {
+	u32 rsvd0;
+} __packed;
+
+struct FWCMD_COMMON_ANON_250_RESPONSE {
+	u16 tx_flow_control;
+	u16 rx_flow_control;
+} __packed;
+
+union FWCMD_COMMON_ANON_248_PARAMS {
+	struct FWCMD_COMMON_ANON_249_REQUEST request;
+	struct FWCMD_COMMON_ANON_250_RESPONSE response;
+} __packed;
+
+/* This comand is used to read BladeEngine flow control settings.  */
+struct FWCMD_COMMON_GET_FLOW_CONTROL {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_248_PARAMS params;
+} __packed;
+
+struct EQ_DELAY_PARAMS {
+	u32 eq_id;
+	u32 delay_in_microseconds;
+} __packed;
+
+struct FWCMD_COMMON_ANON_257_REQUEST {
+	u32 num_eq;
+	u32 rsvd0;
+	struct EQ_DELAY_PARAMS delay[16];
+} __packed;
+
+struct FWCMD_COMMON_ANON_258_RESPONSE {
+	u32 delay_resolution_in_microseconds;
+	u32 delay_max_in_microseconds;
+} __packed;
+
+union MODIFY_EQ_DELAY_PARAMS {
+	struct FWCMD_COMMON_ANON_257_REQUEST request;
+	struct FWCMD_COMMON_ANON_258_RESPONSE response;
+} __packed;
+
+/* This comand changes the EQ delay for a given set of EQs.  */
+struct FWCMD_COMMON_MODIFY_EQ_DELAY {
+	union FWCMD_HEADER header;
+	union MODIFY_EQ_DELAY_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_260_REQUEST {
+	u32 rsvd0;
+} __packed;
+
+struct BE_FIRMWARE_CONFIG {
+	u16 be_config_number;
+	u16 asic_revision;
+	u32 nic_ulp_mask;
+	u32 tulp_mask;
+	u32 iscsi_ulp_mask;
+	u32 rdma_ulp_mask;
+	u32 rsvd0[4];
+	u32 eth_tx_id_start;
+	u32 eth_tx_id_count;
+	u32 eth_rx_id_start;
+	u32 eth_rx_id_count;
+	u32 tpm_wrbq_id_start;
+	u32 tpm_wrbq_id_count;
+	u32 tpm_defq_id_start;
+	u32 tpm_defq_id_count;
+	u32 iscsi_wrbq_id_start;
+	u32 iscsi_wrbq_id_count;
+	u32 iscsi_defq_id_start;
+	u32 iscsi_defq_id_count;
+	u32 rdma_qp_id_start;
+	u32 rdma_qp_id_count;
+	u32 rsvd1[8];
+} __packed;
+
+union FWCMD_COMMON_ANON_259_PARAMS {
+	struct FWCMD_COMMON_ANON_260_REQUEST request;
+	struct BE_FIRMWARE_CONFIG response;
+} __packed;
+
+/*
+ *  This comand queries the current firmware configuration parameters.
+ *   The static  configuration type is defined by be_config_number. This
+ *  differentiates different  BladeEngine builds, such as iSCSI Initiator
+ *  versus iSCSI Target.  For a given static  configuration, the Upper
+ *  Layer Protocol (ULP) processors may be reconfigured to support  different
+ *  protocols. Each ULP processor supports one or more protocols. The
+ *  masks  indicate which processors are configured for each protocol.
+ *   For a given static  configuration, the number of TCP connections
+ *  supported for each protocol may vary. The  *_id_start and *_id_count
+ *  variables define a linear range of IDs that are available for  each
+ *  supported protocol. The *_id_count may be used by the driver to allocate
+ *  the  appropriate number of connection resources. The *_id_start may
+ *  be used to map the  arbitrary range of IDs to a zero-based range
+ *  of indices.
+ */
+struct FWCMD_COMMON_FIRMWARE_CONFIG {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_259_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_PORT_EQUALIZATION_PARAMS {
+	u32 emph_lev_sel_port0;
+	u32 emph_lev_sel_port1;
+	u8 xaui_vo_sel;
+	u8 xaui_state;
+	u16 rsvd0;
+	u32 xaui_eq_vector;
+} __packed;
+
+struct FWCMD_COMMON_ANON_262_REQUEST {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_COMMON_ANON_261_PARAMS {
+	struct FWCMD_COMMON_ANON_262_REQUEST request;
+	struct FWCMD_COMMON_PORT_EQUALIZATION_PARAMS response;
+} __packed;
+
+/*
+ *  This comand can be used to read XAUI equalization parameters. The
+ *  ARM firmware applies  default equalization parameters during initialization.
+ *  These parameters may be  customer-specific when derived from the
+ *  SEEPROM. See SEEPROM_DATA for equalization  specific fields.
+ */
+struct FWCMD_COMMON_GET_PORT_EQUALIZATION {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_261_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_264_RESPONSE {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_COMMON_ANON_263_PARAMS {
+	struct FWCMD_COMMON_PORT_EQUALIZATION_PARAMS request;
+	struct FWCMD_COMMON_ANON_264_RESPONSE response;
+} __packed;
+
+/*
+ *  This comand can be used to set XAUI equalization parameters. The ARM
+ *  firmware applies  default equalization parameters during initialization.
+ *  These parameters may be  customer-specific when derived from the
+ *  SEEPROM. See SEEPROM_DATA for equalization  specific fields.
+ */
+struct FWCMD_COMMON_SET_PORT_EQUALIZATION {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_263_PARAMS params;
+} __packed;
+
+#endif /* __fwcmd_common_bmap_h__ */
diff --git a/drivers/staging/benet/fwcmd_eth_bmap.h b/drivers/staging/benet/fwcmd_eth_bmap.h
new file mode 100644
index 000000000000..234b179eace6
--- /dev/null
+++ b/drivers/staging/benet/fwcmd_eth_bmap.h
@@ -0,0 +1,280 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __fwcmd_eth_bmap_h__
+#define __fwcmd_eth_bmap_h__
+#include "fwcmd_hdr_bmap.h"
+#include "fwcmd_types_bmap.h"
+
+struct MIB_ETH_STATISTICS_PARAMS_IN {
+	u32 rsvd0;
+} __packed;
+
+struct BE_RXF_STATS {
+	u32 p0recvdtotalbytesLSD;	/* DWORD 0 */
+	u32 p0recvdtotalbytesMSD;	/* DWORD 1 */
+	u32 p0recvdtotalframes;	/* DWORD 2 */
+	u32 p0recvdunicastframes;	/* DWORD 3 */
+	u32 p0recvdmulticastframes;	/* DWORD 4 */
+	u32 p0recvdbroadcastframes;	/* DWORD 5 */
+	u32 p0crcerrors;	/* DWORD 6 */
+	u32 p0alignmentsymerrs;	/* DWORD 7 */
+	u32 p0pauseframesrecvd;	/* DWORD 8 */
+	u32 p0controlframesrecvd;	/* DWORD 9 */
+	u32 p0inrangelenerrors;	/* DWORD 10 */
+	u32 p0outrangeerrors;	/* DWORD 11 */
+	u32 p0frametoolongerrors;	/* DWORD 12 */
+	u32 p0droppedaddressmatch;	/* DWORD 13 */
+	u32 p0droppedvlanmismatch;	/* DWORD 14 */
+	u32 p0ipdroppedtoosmall;	/* DWORD 15 */
+	u32 p0ipdroppedtooshort;	/* DWORD 16 */
+	u32 p0ipdroppedhdrtoosmall;	/* DWORD 17 */
+	u32 p0tcpdroppedlen;	/* DWORD 18 */
+	u32 p0droppedrunt;	/* DWORD 19 */
+	u32 p0recvd64;		/* DWORD 20 */
+	u32 p0recvd65_127;	/* DWORD 21 */
+	u32 p0recvd128_256;	/* DWORD 22 */
+	u32 p0recvd256_511;	/* DWORD 23 */
+	u32 p0recvd512_1023;	/* DWORD 24 */
+	u32 p0recvd1518_1522;	/* DWORD 25 */
+	u32 p0recvd1522_2047;	/* DWORD 26 */
+	u32 p0recvd2048_4095;	/* DWORD 27 */
+	u32 p0recvd4096_8191;	/* DWORD 28 */
+	u32 p0recvd8192_9216;	/* DWORD 29 */
+	u32 p0rcvdipcksmerrs;	/* DWORD 30 */
+	u32 p0recvdtcpcksmerrs;	/* DWORD 31 */
+	u32 p0recvdudpcksmerrs;	/* DWORD 32 */
+	u32 p0recvdnonrsspackets;	/* DWORD 33 */
+	u32 p0recvdippackets;	/* DWORD 34 */
+	u32 p0recvdchute1packets;	/* DWORD 35 */
+	u32 p0recvdchute2packets;	/* DWORD 36 */
+	u32 p0recvdchute3packets;	/* DWORD 37 */
+	u32 p0recvdipsecpackets;	/* DWORD 38 */
+	u32 p0recvdmanagementpackets;	/* DWORD 39 */
+	u32 p0xmitbyteslsd;	/* DWORD 40 */
+	u32 p0xmitbytesmsd;	/* DWORD 41 */
+	u32 p0xmitunicastframes;	/* DWORD 42 */
+	u32 p0xmitmulticastframes;	/* DWORD 43 */
+	u32 p0xmitbroadcastframes;	/* DWORD 44 */
+	u32 p0xmitpauseframes;	/* DWORD 45 */
+	u32 p0xmitcontrolframes;	/* DWORD 46 */
+	u32 p0xmit64;		/* DWORD 47 */
+	u32 p0xmit65_127;	/* DWORD 48 */
+	u32 p0xmit128_256;	/* DWORD 49 */
+	u32 p0xmit256_511;	/* DWORD 50 */
+	u32 p0xmit512_1023;	/* DWORD 51 */
+	u32 p0xmit1518_1522;	/* DWORD 52 */
+	u32 p0xmit1522_2047;	/* DWORD 53 */
+	u32 p0xmit2048_4095;	/* DWORD 54 */
+	u32 p0xmit4096_8191;	/* DWORD 55 */
+	u32 p0xmit8192_9216;	/* DWORD 56 */
+	u32 p0rxfifooverflowdropped;	/* DWORD 57 */
+	u32 p0ipseclookupfaileddropped;	/* DWORD 58 */
+	u32 p1recvdtotalbytesLSD;	/* DWORD 59 */
+	u32 p1recvdtotalbytesMSD;	/* DWORD 60 */
+	u32 p1recvdtotalframes;	/* DWORD 61 */
+	u32 p1recvdunicastframes;	/* DWORD 62 */
+	u32 p1recvdmulticastframes;	/* DWORD 63 */
+	u32 p1recvdbroadcastframes;	/* DWORD 64 */
+	u32 p1crcerrors;	/* DWORD 65 */
+	u32 p1alignmentsymerrs;	/* DWORD 66 */
+	u32 p1pauseframesrecvd;	/* DWORD 67 */
+	u32 p1controlframesrecvd;	/* DWORD 68 */
+	u32 p1inrangelenerrors;	/* DWORD 69 */
+	u32 p1outrangeerrors;	/* DWORD 70 */
+	u32 p1frametoolongerrors;	/* DWORD 71 */
+	u32 p1droppedaddressmatch;	/* DWORD 72 */
+	u32 p1droppedvlanmismatch;	/* DWORD 73 */
+	u32 p1ipdroppedtoosmall;	/* DWORD 74 */
+	u32 p1ipdroppedtooshort;	/* DWORD 75 */
+	u32 p1ipdroppedhdrtoosmall;	/* DWORD 76 */
+	u32 p1tcpdroppedlen;	/* DWORD 77 */
+	u32 p1droppedrunt;	/* DWORD 78 */
+	u32 p1recvd64;		/* DWORD 79 */
+	u32 p1recvd65_127;	/* DWORD 80 */
+	u32 p1recvd128_256;	/* DWORD 81 */
+	u32 p1recvd256_511;	/* DWORD 82 */
+	u32 p1recvd512_1023;	/* DWORD 83 */
+	u32 p1recvd1518_1522;	/* DWORD 84 */
+	u32 p1recvd1522_2047;	/* DWORD 85 */
+	u32 p1recvd2048_4095;	/* DWORD 86 */
+	u32 p1recvd4096_8191;	/* DWORD 87 */
+	u32 p1recvd8192_9216;	/* DWORD 88 */
+	u32 p1rcvdipcksmerrs;	/* DWORD 89 */
+	u32 p1recvdtcpcksmerrs;	/* DWORD 90 */
+	u32 p1recvdudpcksmerrs;	/* DWORD 91 */
+	u32 p1recvdnonrsspackets;	/* DWORD 92 */
+	u32 p1recvdippackets;	/* DWORD 93 */
+	u32 p1recvdchute1packets;	/* DWORD 94 */
+	u32 p1recvdchute2packets;	/* DWORD 95 */
+	u32 p1recvdchute3packets;	/* DWORD 96 */
+	u32 p1recvdipsecpackets;	/* DWORD 97 */
+	u32 p1recvdmanagementpackets;	/* DWORD 98 */
+	u32 p1xmitbyteslsd;	/* DWORD 99 */
+	u32 p1xmitbytesmsd;	/* DWORD 100 */
+	u32 p1xmitunicastframes;	/* DWORD 101 */
+	u32 p1xmitmulticastframes;	/* DWORD 102 */
+	u32 p1xmitbroadcastframes;	/* DWORD 103 */
+	u32 p1xmitpauseframes;	/* DWORD 104 */
+	u32 p1xmitcontrolframes;	/* DWORD 105 */
+	u32 p1xmit64;		/* DWORD 106 */
+	u32 p1xmit65_127;	/* DWORD 107 */
+	u32 p1xmit128_256;	/* DWORD 108 */
+	u32 p1xmit256_511;	/* DWORD 109 */
+	u32 p1xmit512_1023;	/* DWORD 110 */
+	u32 p1xmit1518_1522;	/* DWORD 111 */
+	u32 p1xmit1522_2047;	/* DWORD 112 */
+	u32 p1xmit2048_4095;	/* DWORD 113 */
+	u32 p1xmit4096_8191;	/* DWORD 114 */
+	u32 p1xmit8192_9216;	/* DWORD 115 */
+	u32 p1rxfifooverflowdropped;	/* DWORD 116 */
+	u32 p1ipseclookupfaileddropped;	/* DWORD 117 */
+	u32 pxdroppednopbuf;	/* DWORD 118 */
+	u32 pxdroppednotxpb;	/* DWORD 119 */
+	u32 pxdroppednoipsecbuf;	/* DWORD 120 */
+	u32 pxdroppednoerxdescr;	/* DWORD 121 */
+	u32 pxdroppednotpredescr;	/* DWORD 122 */
+	u32 pxrecvdmanagementportpackets;	/* DWORD 123 */
+	u32 pxrecvdmanagementportbytes;	/* DWORD 124 */
+	u32 pxrecvdmanagementportpauseframes;	/* DWORD 125 */
+	u32 pxrecvdmanagementporterrors;	/* DWORD 126 */
+	u32 pxxmitmanagementportpackets;	/* DWORD 127 */
+	u32 pxxmitmanagementportbytes;	/* DWORD 128 */
+	u32 pxxmitmanagementportpause;	/* DWORD 129 */
+	u32 pxxmitmanagementportrxfifooverflow;	/* DWORD 130 */
+	u32 pxrecvdipsecipcksmerrs;	/* DWORD 131 */
+	u32 pxrecvdtcpsecipcksmerrs;	/* DWORD 132 */
+	u32 pxrecvdudpsecipcksmerrs;	/* DWORD 133 */
+	u32 pxipsecrunt;	/* DWORD 134 */
+	u32 pxipsecaddressmismatchdropped;	/* DWORD 135 */
+	u32 pxipsecrxfifooverflowdropped;	/* DWORD 136 */
+	u32 pxipsecframestoolong;	/* DWORD 137 */
+	u32 pxipsectotalipframes;	/* DWORD 138 */
+	u32 pxipseciptoosmall;	/* DWORD 139 */
+	u32 pxipseciptooshort;	/* DWORD 140 */
+	u32 pxipseciphdrtoosmall;	/* DWORD 141 */
+	u32 pxipsectcphdrbad;	/* DWORD 142 */
+	u32 pxrecvdipsecchute1;	/* DWORD 143 */
+	u32 pxrecvdipsecchute2;	/* DWORD 144 */
+	u32 pxrecvdipsecchute3;	/* DWORD 145 */
+	u32 pxdropped7frags;	/* DWORD 146 */
+	u32 pxdroppedfrags;	/* DWORD 147 */
+	u32 pxdroppedinvalidfragring;	/* DWORD 148 */
+	u32 pxnumforwardedpackets;	/* DWORD 149 */
+} __packed;
+
+union MIB_ETH_STATISTICS_PARAMS {
+	struct MIB_ETH_STATISTICS_PARAMS_IN request;
+	struct BE_RXF_STATS response;
+} __packed;
+
+/*
+ *  Query ethernet statistics. All domains may issue this command. The
+ *  host domain drivers  may optionally reset internal statistic counters
+ *  with a query.
+ */
+struct FWCMD_ETH_GET_STATISTICS {
+	union FWCMD_HEADER header;
+	union MIB_ETH_STATISTICS_PARAMS params;
+} __packed;
+
+
+struct FWCMD_ETH_ANON_175_REQUEST {
+	u8 port0_promiscuous;
+	u8 port1_promiscuous;
+	u16 rsvd0;
+} __packed;
+
+struct FWCMD_ETH_ANON_176_RESPONSE {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_ETH_ANON_174_PARAMS {
+	struct FWCMD_ETH_ANON_175_REQUEST request;
+	struct FWCMD_ETH_ANON_176_RESPONSE response;
+} __packed;
+
+/* Enables/Disables promiscuous ethernet receive mode.  */
+struct FWCMD_ETH_PROMISCUOUS {
+	union FWCMD_HEADER header;
+	union FWCMD_ETH_ANON_174_PARAMS params;
+} __packed;
+
+struct FWCMD_ETH_ANON_178_REQUEST {
+	u32 new_fragsize_log2;
+} __packed;
+
+struct FWCMD_ETH_ANON_179_RESPONSE {
+	u32 actual_fragsize_log2;
+} __packed;
+
+union FWCMD_ETH_ANON_177_PARAMS {
+	struct FWCMD_ETH_ANON_178_REQUEST request;
+	struct FWCMD_ETH_ANON_179_RESPONSE response;
+} __packed;
+
+/*
+ *  Sets the Ethernet RX fragment size. Only host (domain 0) networking
+ *  drivers may issue  this command.  This call will fail for non-host
+ *  protection domains. In this situation the  MCC CQ status will indicate
+ *  a failure due to insufficient priviledges. The response  should be
+ *  ignored, and the driver should use the FWCMD_ETH_GET_FRAG_SIZE to
+ *  query the  existing ethernet receive fragment size. It must use this
+ *  fragment size for all  fragments in the ethernet receive ring.  If
+ *  the command succeeds, the driver must use the  frag size indicated
+ *  in the command response since the requested frag size may not be  applied
+ *  until the next reboot. When the requested fragsize matches the response
+ *   fragsize, this indicates the request was applied immediately.
+ */
+struct FWCMD_ETH_SET_RX_FRAG_SIZE {
+	union FWCMD_HEADER header;
+	union FWCMD_ETH_ANON_177_PARAMS params;
+} __packed;
+
+struct FWCMD_ETH_ANON_181_REQUEST {
+	u32 rsvd0;
+} __packed;
+
+struct FWCMD_ETH_ANON_182_RESPONSE {
+	u32 actual_fragsize_log2;
+} __packed;
+
+union FWCMD_ETH_ANON_180_PARAMS {
+	struct FWCMD_ETH_ANON_181_REQUEST request;
+	struct FWCMD_ETH_ANON_182_RESPONSE response;
+} __packed;
+
+/*
+ *  Queries the Ethernet RX fragment size. All domains may issue this
+ *  command.  The driver  should call this command to determine the minimum
+ *  required fragment size for the ethernet  RX ring buffers. Drivers
+ *  may choose to use a larger size for each fragment buffer, but  BladeEngine
+ *  will use up to the configured minimum required fragsize in each ethernet
+ *   receive fragment buffer. For example, if the ethernet receive fragment
+ *  size is  configured to 4kB, and a driver uses 8kB fragments, a 6kB
+ *  ethernet packet received by  BladeEngine will be split accross two
+ *  of the driver's receive framgents (4kB in one  fragment buffer, and
+ *  2kB in the subsequent fragment buffer).
+ */
+struct FWCMD_ETH_GET_RX_FRAG_SIZE {
+	union FWCMD_HEADER header;
+	union FWCMD_ETH_ANON_180_PARAMS params;
+} __packed;
+
+#endif /* __fwcmd_eth_bmap_h__ */
diff --git a/drivers/staging/benet/fwcmd_hdr_bmap.h b/drivers/staging/benet/fwcmd_hdr_bmap.h
new file mode 100644
index 000000000000..28b45328fe7b
--- /dev/null
+++ b/drivers/staging/benet/fwcmd_hdr_bmap.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __fwcmd_hdr_bmap_h__
+#define __fwcmd_hdr_bmap_h__
+
+struct FWCMD_REQUEST_HEADER {
+	u8 opcode;
+	u8 subsystem;
+	u8 port_number;
+	u8 domain;
+	u32 timeout;
+	u32 request_length;
+	u32 rsvd0;
+} __packed;
+
+struct FWCMD_RESPONSE_HEADER {
+	u8 opcode;
+	u8 subsystem;
+	u8 rsvd0;
+	u8 domain;
+	u8 status;
+	u8 additional_status;
+	u16 rsvd1;
+	u32 response_length;
+	u32 actual_response_length;
+} __packed;
+
+/*
+ *  The firmware/driver overwrites the input FWCMD_REQUEST_HEADER with
+ *  the output  FWCMD_RESPONSE_HEADER.
+ */
+union FWCMD_HEADER {
+	struct FWCMD_REQUEST_HEADER request;
+	struct FWCMD_RESPONSE_HEADER response;
+} __packed;
+
+#endif /* __fwcmd_hdr_bmap_h__ */
diff --git a/drivers/staging/benet/fwcmd_mcc.h b/drivers/staging/benet/fwcmd_mcc.h
new file mode 100644
index 000000000000..9eeca878c1fb
--- /dev/null
+++ b/drivers/staging/benet/fwcmd_mcc.h
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __fwcmd_mcc_amap_h__
+#define __fwcmd_mcc_amap_h__
+#include "fwcmd_opcodes.h"
+/*
+ * Where applicable, a WRB, may contain a list of Scatter-gather elements.
+ * Each element supports a 64 bit address and a 32bit length field.
+ */
+struct BE_MCC_SGE_AMAP {
+	u8 pa_lo[32];	/* DWORD 0 */
+	u8 pa_hi[32];	/* DWORD 1 */
+	u8 length[32];	/* DWORD 2 */
+} __packed;
+struct MCC_SGE_AMAP {
+	u32 dw[3];
+};
+/*
+ * The design of an MCC_SGE allows up to 19 elements to be embedded
+ * in a WRB, supporting 64KB data transfers (assuming a 4KB page size).
+ */
+struct BE_MCC_WRB_PAYLOAD_AMAP {
+	union {
+		struct BE_MCC_SGE_AMAP sgl[19];
+		u8 embedded[59][32];	/* DWORD 0 */
+	};
+} __packed;
+struct MCC_WRB_PAYLOAD_AMAP {
+	u32 dw[59];
+};
+
+/*
+ * This is the structure of the MCC Command WRB for commands
+ * sent to the Management Processing Unit (MPU). See section
+ * for usage in embedded and non-embedded modes.
+ */
+struct BE_MCC_WRB_AMAP {
+	u8 embedded;	/* DWORD 0 */
+	u8 rsvd0[2];	/* DWORD 0 */
+	u8 sge_count[5];	/* DWORD 0 */
+	u8 rsvd1[16];	/* DWORD 0 */
+	u8 special[8];	/* DWORD 0 */
+	u8 payload_length[32];	/* DWORD 1 */
+	u8 tag[2][32];	/* DWORD 2 */
+	u8 rsvd2[32];	/* DWORD 4 */
+	struct BE_MCC_WRB_PAYLOAD_AMAP payload;
+} __packed;
+struct MCC_WRB_AMAP {
+	u32 dw[64];
+};
+
+/*  This is the structure of the MCC Completion queue entry  */
+struct BE_MCC_CQ_ENTRY_AMAP {
+	u8 completion_status[16];	/* DWORD 0 */
+	u8 extended_status[16];	/* DWORD 0 */
+	u8 mcc_tag[2][32];	/* DWORD 1 */
+	u8 rsvd0[27];	/* DWORD 3 */
+	u8 consumed;	/* DWORD 3 */
+	u8 completed;	/* DWORD 3 */
+	u8 hpi_buffer_completion;	/* DWORD 3 */
+	u8 async_event;	/* DWORD 3 */
+	u8 valid;		/* DWORD 3 */
+} __packed;
+struct MCC_CQ_ENTRY_AMAP {
+	u32 dw[4];
+};
+
+/* Mailbox structures used by the MPU during bootstrap */
+struct BE_MCC_MAILBOX_AMAP {
+	struct BE_MCC_WRB_AMAP wrb;
+	struct BE_MCC_CQ_ENTRY_AMAP cq;
+} __packed;
+struct MCC_MAILBOX_AMAP {
+	u32 dw[68];
+};
+
+#endif /* __fwcmd_mcc_amap_h__ */
diff --git a/drivers/staging/benet/fwcmd_opcodes.h b/drivers/staging/benet/fwcmd_opcodes.h
new file mode 100644
index 000000000000..23d569386b46
--- /dev/null
+++ b/drivers/staging/benet/fwcmd_opcodes.h
@@ -0,0 +1,244 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __fwcmd_opcodes_amap_h__
+#define __fwcmd_opcodes_amap_h__
+
+/*
+ * --- FWCMD_SUBSYSTEMS ---
+ * The commands are grouped into the following subsystems. The subsystem
+ * code along with the opcode uniquely identify a particular fwcmd.
+ */
+#define FWCMD_SUBSYSTEM_RSVD  (0)	/* This subsystem is reserved. It is */
+						  /* never used. */
+#define FWCMD_SUBSYSTEM_COMMON (1)	/* CMDs in this group are common to
+					* all subsystems. See
+					* COMMON_SUBSYSTEM_OPCODES for opcodes
+					* and Common Host Configuration CMDs
+					* for the FWCMD descriptions.
+					*/
+#define FWCMD_SUBSYSTEM_COMMON_ISCSI    (2) /* CMDs in this group are */
+					/*
+					* common to Initiator and Target. See
+					* COMMON_ISCSI_SUBSYSTEM_OPCODES and
+					* Common iSCSI Initiator and Target
+					* CMDs for the command descriptions.
+					*/
+#define FWCMD_SUBSYSTEM_ETH             (3)	/* This subsystem is used to
+						execute  Ethernet commands.  */
+
+#define FWCMD_SUBSYSTEM_TPM             (4)	/* This subsystem is used
+						 to execute TPM  commands.  */
+#define FWCMD_SUBSYSTEM_PXE_UNDI        (5)	/* This subsystem is used
+						* to execute PXE
+						* and UNDI specific commands.
+						*/
+
+#define FWCMD_SUBSYSTEM_ISCSI_INI       (6)	/* This subsystem is used to
+						execute ISCSI Initiator
+						specific commands.
+						*/
+#define FWCMD_SUBSYSTEM_ISCSI_TGT       (7)	/* This subsystem is used
+						to execute iSCSI Target
+						specific commands.between
+						PTL and ARM firmware.
+						*/
+#define FWCMD_SUBSYSTEM_MILI_PTL        (8)	/* This subsystem is used to
+						execute iSCSI Target specific
+						commands.between MILI
+						and PTL.  */
+#define FWCMD_SUBSYSTEM_MILI_TMD        (9)	/* This subsystem is used to
+						execute iSCSI Target specific
+						commands between MILI
+						and TMD.  */
+#define FWCMD_SUBSYSTEM_PROXY           (11)	/* This subsystem is used
+						to execute proxied commands
+						within the host at the
+						explicit request of a
+						non priviledged domain.
+						This 'subsystem' is entirely
+						virtual from the controller
+						and firmware perspective as
+						it is implemented in host
+						drivers.
+						*/
+
+/*
+ * --- COMMON_SUBSYSTEM_OPCODES ---
+ * These opcodes are common to both networking and storage PCI
+ * functions. They are used to reserve resources and configure
+ * BladeEngine. These opcodes all use the FWCMD_SUBSYSTEM_COMMON
+ * subsystem code.
+ */
+#define OPCODE_COMMON_NTWK_MAC_QUERY    (1)
+#define SUBSYSTEM_COMMON_NTWK_MAC_QUERY (1)
+#define SUBSYSTEM_COMMON_NTWK_MAC_SET   (1)
+#define SUBSYSTEM_COMMON_NTWK_MULTICAST_SET (1)
+#define SUBSYSTEM_COMMON_NTWK_VLAN_CONFIG (1)
+#define SUBSYSTEM_COMMON_NTWK_LINK_STATUS_QUERY (1)
+#define SUBSYSTEM_COMMON_READ_FLASHROM  (1)
+#define SUBSYSTEM_COMMON_WRITE_FLASHROM (1)
+#define SUBSYSTEM_COMMON_QUERY_MAX_FWCMD_BUFFER_SIZE (1)
+#define SUBSYSTEM_COMMON_ADD_PAGE_TABLES (1)
+#define SUBSYSTEM_COMMON_REMOVE_PAGE_TABLES (1)
+#define SUBSYSTEM_COMMON_RING_DESTROY   (1)
+#define SUBSYSTEM_COMMON_CQ_CREATE      (1)
+#define SUBSYSTEM_COMMON_EQ_CREATE      (1)
+#define SUBSYSTEM_COMMON_ETH_RX_CREATE  (1)
+#define SUBSYSTEM_COMMON_ETH_TX_CREATE  (1)
+#define SUBSYSTEM_COMMON_ISCSI_DEFQ_CREATE (1)
+#define SUBSYSTEM_COMMON_ISCSI_WRBQ_CREATE (1)
+#define SUBSYSTEM_COMMON_MCC_CREATE     (1)
+#define SUBSYSTEM_COMMON_JELL_CONFIG    (1)
+#define SUBSYSTEM_COMMON_FORCE_FAILOVER (1)
+#define SUBSYSTEM_COMMON_ADD_TEMPLATE_HEADER_BUFFERS (1)
+#define SUBSYSTEM_COMMON_REMOVE_TEMPLATE_HEADER_BUFFERS (1)
+#define SUBSYSTEM_COMMON_POST_ZERO_BUFFER (1)
+#define SUBSYSTEM_COMMON_GET_QOS        (1)
+#define SUBSYSTEM_COMMON_SET_QOS        (1)
+#define SUBSYSTEM_COMMON_TCP_GET_STATISTICS (1)
+#define SUBSYSTEM_COMMON_SEEPROM_READ   (1)
+#define SUBSYSTEM_COMMON_TCP_STATE_QUERY (1)
+#define SUBSYSTEM_COMMON_GET_CNTL_ATTRIBUTES (1)
+#define SUBSYSTEM_COMMON_NOP            (1)
+#define SUBSYSTEM_COMMON_NTWK_RX_FILTER (1)
+#define SUBSYSTEM_COMMON_GET_FW_VERSION (1)
+#define SUBSYSTEM_COMMON_SET_FLOW_CONTROL (1)
+#define SUBSYSTEM_COMMON_GET_FLOW_CONTROL (1)
+#define SUBSYSTEM_COMMON_SET_TCP_PARAMETERS (1)
+#define SUBSYSTEM_COMMON_SET_FRAME_SIZE (1)
+#define SUBSYSTEM_COMMON_GET_FAT        (1)
+#define SUBSYSTEM_COMMON_MODIFY_EQ_DELAY (1)
+#define SUBSYSTEM_COMMON_FIRMWARE_CONFIG (1)
+#define SUBSYSTEM_COMMON_ENABLE_DISABLE_DOMAINS (1)
+#define SUBSYSTEM_COMMON_GET_DOMAIN_CONFIG (1)
+#define SUBSYSTEM_COMMON_SET_VLD_CONFIG (1)
+#define SUBSYSTEM_COMMON_GET_VLD_CONFIG (1)
+#define SUBSYSTEM_COMMON_GET_PORT_EQUALIZATION (1)
+#define SUBSYSTEM_COMMON_SET_PORT_EQUALIZATION (1)
+#define SUBSYSTEM_COMMON_RED_CONFIG     (1)
+#define OPCODE_COMMON_NTWK_MAC_SET      (2)
+#define OPCODE_COMMON_NTWK_MULTICAST_SET (3)
+#define OPCODE_COMMON_NTWK_VLAN_CONFIG  (4)
+#define OPCODE_COMMON_NTWK_LINK_STATUS_QUERY (5)
+#define OPCODE_COMMON_READ_FLASHROM     (6)
+#define OPCODE_COMMON_WRITE_FLASHROM    (7)
+#define OPCODE_COMMON_QUERY_MAX_FWCMD_BUFFER_SIZE (8)
+#define OPCODE_COMMON_ADD_PAGE_TABLES   (9)
+#define OPCODE_COMMON_REMOVE_PAGE_TABLES (10)
+#define OPCODE_COMMON_RING_DESTROY      (11)
+#define OPCODE_COMMON_CQ_CREATE         (12)
+#define OPCODE_COMMON_EQ_CREATE         (13)
+#define OPCODE_COMMON_ETH_RX_CREATE     (14)
+#define OPCODE_COMMON_ETH_TX_CREATE     (15)
+#define OPCODE_COMMON_NET_RESERVED0     (16)	/* Reserved */
+#define OPCODE_COMMON_NET_RESERVED1     (17)	/* Reserved */
+#define OPCODE_COMMON_NET_RESERVED2     (18)	/* Reserved */
+#define OPCODE_COMMON_ISCSI_DEFQ_CREATE (19)
+#define OPCODE_COMMON_ISCSI_WRBQ_CREATE (20)
+#define OPCODE_COMMON_MCC_CREATE        (21)
+#define OPCODE_COMMON_JELL_CONFIG       (22)
+#define OPCODE_COMMON_FORCE_FAILOVER    (23)
+#define OPCODE_COMMON_ADD_TEMPLATE_HEADER_BUFFERS (24)
+#define OPCODE_COMMON_REMOVE_TEMPLATE_HEADER_BUFFERS (25)
+#define OPCODE_COMMON_POST_ZERO_BUFFER  (26)
+#define OPCODE_COMMON_GET_QOS           (27)
+#define OPCODE_COMMON_SET_QOS           (28)
+#define OPCODE_COMMON_TCP_GET_STATISTICS (29)
+#define OPCODE_COMMON_SEEPROM_READ      (30)
+#define OPCODE_COMMON_TCP_STATE_QUERY   (31)
+#define OPCODE_COMMON_GET_CNTL_ATTRIBUTES (32)
+#define OPCODE_COMMON_NOP               (33)
+#define OPCODE_COMMON_NTWK_RX_FILTER    (34)
+#define OPCODE_COMMON_GET_FW_VERSION    (35)
+#define OPCODE_COMMON_SET_FLOW_CONTROL  (36)
+#define OPCODE_COMMON_GET_FLOW_CONTROL  (37)
+#define OPCODE_COMMON_SET_TCP_PARAMETERS (38)
+#define OPCODE_COMMON_SET_FRAME_SIZE    (39)
+#define OPCODE_COMMON_GET_FAT           (40)
+#define OPCODE_COMMON_MODIFY_EQ_DELAY   (41)
+#define OPCODE_COMMON_FIRMWARE_CONFIG   (42)
+#define OPCODE_COMMON_ENABLE_DISABLE_DOMAINS (43)
+#define OPCODE_COMMON_GET_DOMAIN_CONFIG (44)
+#define OPCODE_COMMON_SET_VLD_CONFIG    (45)
+#define OPCODE_COMMON_GET_VLD_CONFIG    (46)
+#define OPCODE_COMMON_GET_PORT_EQUALIZATION (47)
+#define OPCODE_COMMON_SET_PORT_EQUALIZATION (48)
+#define OPCODE_COMMON_RED_CONFIG        (49)
+
+
+
+/*
+ * --- ETH_SUBSYSTEM_OPCODES ---
+ * These opcodes are used for configuring the Ethernet interfaces. These
+ * opcodes all use the FWCMD_SUBSYSTEM_ETH subsystem code.
+ */
+#define OPCODE_ETH_RSS_CONFIG           (1)
+#define OPCODE_ETH_ACPI_CONFIG          (2)
+#define SUBSYSTEM_ETH_RSS_CONFIG        (3)
+#define SUBSYSTEM_ETH_ACPI_CONFIG       (3)
+#define OPCODE_ETH_PROMISCUOUS          (3)
+#define SUBSYSTEM_ETH_PROMISCUOUS       (3)
+#define SUBSYSTEM_ETH_GET_STATISTICS    (3)
+#define SUBSYSTEM_ETH_GET_RX_FRAG_SIZE  (3)
+#define SUBSYSTEM_ETH_SET_RX_FRAG_SIZE  (3)
+#define OPCODE_ETH_GET_STATISTICS       (4)
+#define OPCODE_ETH_GET_RX_FRAG_SIZE     (5)
+#define OPCODE_ETH_SET_RX_FRAG_SIZE     (6)
+
+
+
+
+
+/*
+ * --- MCC_STATUS_CODE ---
+ * These are the global status codes used by all subsystems
+ */
+#define MCC_STATUS_SUCCESS              (0)	/* Indicates a successful
+						completion of  the command */
+#define MCC_STATUS_INSUFFICIENT_PRIVILEGES (1)	/* The client does not have
+						sufficient privileges to
+						execute the command */
+#define MCC_STATUS_INVALID_PARAMETER    (2)	/* A parameter in the command
+						was invalid. The extended
+						status contains the index
+						of the parameter */
+#define MCC_STATUS_INSUFFICIENT_RESOURCES (3)	/* There are insufficient
+						chip resources to execute
+						the command */
+#define MCC_STATUS_QUEUE_FLUSHING       (4)	/* The command is completing
+						because the queue was
+						getting flushed */
+#define MCC_STATUS_DMA_FAILED           (5)	/* The command is completing
+						with a DMA error */
+
+/*
+ * --- MGMT_ERROR_CODES ---
+ * Error Codes returned in the status field of the FWCMD response header
+ */
+#define MGMT_STATUS_SUCCESS             (0)	/* The FWCMD completed
+						without errors */
+#define MGMT_STATUS_FAILED              (1)	/* Error status in the Status
+						field of  the
+						struct FWCMD_RESPONSE_HEADER */
+#define MGMT_STATUS_ILLEGAL_REQUEST     (2)	/* Invalid FWCMD opcode */
+#define MGMT_STATUS_ILLEGAL_FIELD       (3)	/* Invalid parameter in
+						the FWCMD  payload */
+
+#endif /* __fwcmd_opcodes_amap_h__ */
diff --git a/drivers/staging/benet/fwcmd_types_bmap.h b/drivers/staging/benet/fwcmd_types_bmap.h
new file mode 100644
index 000000000000..92217aff3a16
--- /dev/null
+++ b/drivers/staging/benet/fwcmd_types_bmap.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __fwcmd_types_bmap_h__
+#define __fwcmd_types_bmap_h__
+
+/* MAC address format  */
+struct MAC_ADDRESS_FORMAT {
+	u16 SizeOfStructure;
+	u8 MACAddress[6];
+} __packed;
+
+#endif /* __fwcmd_types_bmap_h__ */
diff --git a/drivers/staging/benet/host_struct.h b/drivers/staging/benet/host_struct.h
new file mode 100644
index 000000000000..3de6722b980f
--- /dev/null
+++ b/drivers/staging/benet/host_struct.h
@@ -0,0 +1,182 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __host_struct_amap_h__
+#define __host_struct_amap_h__
+#include "be_cm.h"
+#include "be_common.h"
+#include "descriptors.h"
+
+/* --- EQ_COMPLETION_MAJOR_CODE_ENUM --- */
+#define EQ_MAJOR_CODE_COMPLETION        (0)	/* Completion event on a */
+						  /* qcompletion ueue. */
+#define EQ_MAJOR_CODE_ETH               (1)	/* Affiliated Ethernet Event. */
+#define EQ_MAJOR_CODE_RESERVED          (2)	/* Reserved */
+#define EQ_MAJOR_CODE_RDMA              (3)	/* Affiliated RDMA Event. */
+#define EQ_MAJOR_CODE_ISCSI             (4)	/* Affiliated ISCSI Event */
+#define EQ_MAJOR_CODE_UNAFFILIATED      (5)	/* Unaffiliated Event */
+
+/* --- EQ_COMPLETION_MINOR_CODE_ENUM --- */
+#define EQ_MINOR_CODE_COMPLETION        (0)	/* Completion event on a */
+						  /* completion queue. */
+#define EQ_MINOR_CODE_OTHER             (1)	/* Other Event (TBD). */
+
+/* Queue Entry Definition for all 4 byte event queue types. */
+struct BE_EQ_ENTRY_AMAP {
+	u8 Valid;		/* DWORD 0 */
+	u8 MajorCode[3];	/* DWORD 0 */
+	u8 MinorCode[12];	/* DWORD 0 */
+	u8 ResourceID[16];	/* DWORD 0 */
+} __packed;
+struct EQ_ENTRY_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * --- ETH_EVENT_CODE ---
+ * These codes are returned by the MPU when one of these events has occurred,
+ * and the event is configured to report to an Event Queue when an event
+ * is detected.
+ */
+#define ETH_EQ_LINK_STATUS              (0)	/* Link status change event */
+						  /* detected. */
+#define ETH_EQ_WATERMARK                (1)	/* watermark event detected. */
+#define ETH_EQ_MAGIC_PKT                (2)	/* magic pkt event detected. */
+#define ETH_EQ_ACPI_PKT0                (3)	/* ACPI interesting packet */
+						  /* detected. */
+#define ETH_EQ_ACPI_PKT1                (3)	/* ACPI interesting packet */
+						  /* detected. */
+#define ETH_EQ_ACPI_PKT2                (3)	/* ACPI interesting packet */
+						  /* detected. */
+#define ETH_EQ_ACPI_PKT3                (3)	/* ACPI interesting packet */
+						  /* detected. */
+
+/*
+ * --- ETH_TX_COMPL_STATUS_ENUM ---
+ * Status codes contained in Ethernet TX completion descriptors.
+ */
+#define ETH_COMP_VALID                  (0)
+#define ETH_COMP_ERROR                  (1)
+#define ETH_COMP_INVALID                (15)
+
+/*
+ * --- ETH_TX_COMPL_PORT_ENUM ---
+ * Port indicator contained in Ethernet TX completion descriptors.
+ */
+#define ETH_COMP_PORT0                  (0)
+#define ETH_COMP_PORT1                  (1)
+#define ETH_COMP_MGMT                   (2)
+
+/*
+ * --- ETH_TX_COMPL_CT_ENUM ---
+ * Completion type indicator contained in Ethernet TX completion descriptors.
+ */
+#define ETH_COMP_ETH                    (0)
+
+/*
+ * Work request block that the driver issues to the chip for
+ * Ethernet transmissions. All control fields must be valid in each WRB for
+ * a message. The controller, as specified by the flags, optionally writes
+ * an entry to the Completion Ring and generate an event.
+ */
+struct BE_ETH_WRB_AMAP {
+	u8 frag_pa_hi[32];	/* DWORD 0 */
+	u8 frag_pa_lo[32];	/* DWORD 1 */
+	u8 complete;	/* DWORD 2 */
+	u8 event;		/* DWORD 2 */
+	u8 crc;		/* DWORD 2 */
+	u8 forward;		/* DWORD 2 */
+	u8 ipsec;		/* DWORD 2 */
+	u8 mgmt;		/* DWORD 2 */
+	u8 ipcs;		/* DWORD 2 */
+	u8 udpcs;		/* DWORD 2 */
+	u8 tcpcs;		/* DWORD 2 */
+	u8 lso;		/* DWORD 2 */
+	u8 last;		/* DWORD 2 */
+	u8 vlan;		/* DWORD 2 */
+	u8 dbg[3];		/* DWORD 2 */
+	u8 hash_val[3];	/* DWORD 2 */
+	u8 lso_mss[14];	/* DWORD 2 */
+	u8 frag_len[16];	/* DWORD 3 */
+	u8 vlan_tag[16];	/* DWORD 3 */
+} __packed;
+struct ETH_WRB_AMAP {
+	u32 dw[4];
+};
+
+/* This is an Ethernet transmit completion descriptor */
+struct BE_ETH_TX_COMPL_AMAP {
+	u8 user_bytes[16];	/* DWORD 0 */
+	u8 nwh_bytes[8];	/* DWORD 0 */
+	u8 lso;		/* DWORD 0 */
+	u8 rsvd0[7];	/* DWORD 0 */
+	u8 wrb_index[16];	/* DWORD 1 */
+	u8 ct[2];		/* DWORD 1 */
+	u8 port[2];		/* DWORD 1 */
+	u8 rsvd1[8];	/* DWORD 1 */
+	u8 status[4];	/* DWORD 1 */
+	u8 rsvd2[16];	/* DWORD 2 */
+	u8 ringid[11];	/* DWORD 2 */
+	u8 hash_val[4];	/* DWORD 2 */
+	u8 valid;		/* DWORD 2 */
+	u8 rsvd3[32];	/* DWORD 3 */
+} __packed;
+struct ETH_TX_COMPL_AMAP {
+	u32 dw[4];
+};
+
+/* Ethernet Receive Buffer descriptor */
+struct BE_ETH_RX_D_AMAP {
+	u8 fragpa_hi[32];	/* DWORD 0 */
+	u8 fragpa_lo[32];	/* DWORD 1 */
+} __packed;
+struct ETH_RX_D_AMAP {
+	u32 dw[2];
+};
+
+/* This is an Ethernet Receive Completion Descriptor */
+struct BE_ETH_RX_COMPL_AMAP {
+	u8 vlan_tag[16];	/* DWORD 0 */
+	u8 pktsize[14];	/* DWORD 0 */
+	u8 port;		/* DWORD 0 */
+	u8 rsvd0;		/* DWORD 0 */
+	u8 err;		/* DWORD 1 */
+	u8 rsshp;		/* DWORD 1 */
+	u8 ipf;		/* DWORD 1 */
+	u8 tcpf;		/* DWORD 1 */
+	u8 udpf;		/* DWORD 1 */
+	u8 ipcksm;		/* DWORD 1 */
+	u8 tcpcksm;		/* DWORD 1 */
+	u8 udpcksm;		/* DWORD 1 */
+	u8 macdst[6];	/* DWORD 1 */
+	u8 vtp;		/* DWORD 1 */
+	u8 vtm;		/* DWORD 1 */
+	u8 fragndx[10];	/* DWORD 1 */
+	u8 ct[2];		/* DWORD 1 */
+	u8 ipsec;		/* DWORD 1 */
+	u8 numfrags[3];	/* DWORD 1 */
+	u8 rsvd1[31];	/* DWORD 2 */
+	u8 valid;		/* DWORD 2 */
+	u8 rsshash[32];	/* DWORD 3 */
+} __packed;
+struct ETH_RX_COMPL_AMAP {
+	u32 dw[4];
+};
+
+#endif /* __host_struct_amap_h__ */
diff --git a/drivers/staging/benet/hwlib.h b/drivers/staging/benet/hwlib.h
new file mode 100644
index 000000000000..afedf4dc5903
--- /dev/null
+++ b/drivers/staging/benet/hwlib.h
@@ -0,0 +1,830 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#ifndef __hwlib_h__
+#define __hwlib_h__
+
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+
+#include "regmap.h"		/* srcgen array map output */
+
+#include "asyncmesg.h"
+#include "fwcmd_opcodes.h"
+#include "post_codes.h"
+#include "fwcmd_mcc.h"
+
+#include "fwcmd_types_bmap.h"
+#include "fwcmd_common_bmap.h"
+#include "fwcmd_eth_bmap.h"
+#include "bestatus.h"
+/*
+ *
+ * Macros for reading/writing a protection domain or CSR registers
+ * in BladeEngine.
+ */
+#define PD_READ(fo, field)	ioread32((fo)->db_va + \
+		offsetof(struct BE_PROTECTION_DOMAIN_DBMAP_AMAP, field)/8)
+
+#define PD_WRITE(fo, field, val) iowrite32(val, (fo)->db_va + \
+		offsetof(struct BE_PROTECTION_DOMAIN_DBMAP_AMAP, field)/8)
+
+#define CSR_READ(fo, field)	ioread32((fo)->csr_va + \
+		offsetof(struct BE_BLADE_ENGINE_CSRMAP_AMAP, field)/8)
+
+#define CSR_WRITE(fo, field, val)	iowrite32(val, (fo)->csr_va + \
+		offsetof(struct BE_BLADE_ENGINE_CSRMAP_AMAP, field)/8)
+
+#define PCICFG0_READ(fo, field)	ioread32((fo)->pci_va + \
+		offsetof(struct BE_PCICFG0_CSRMAP_AMAP, field)/8)
+
+#define PCICFG0_WRITE(fo, field, val)	iowrite32(val, (fo)->pci_va + \
+		offsetof(struct BE_PCICFG0_CSRMAP_AMAP, field)/8)
+
+#define PCICFG1_READ(fo, field)	ioread32((fo)->pci_va + \
+		offsetof(struct BE_PCICFG1_CSRMAP_AMAP, field)/8)
+
+#define PCICFG1_WRITE(fo, field, val)	iowrite32(val, (fo)->pci_va + \
+		offsetof(struct BE_PCICFG1_CSRMAP_AMAP, field)/8)
+
+#ifdef BE_DEBUG
+#define ASSERT(c)       BUG_ON(!(c));
+#else
+#define ASSERT(c)
+#endif
+
+/* debug levels */
+enum BE_DEBUG_LEVELS {
+	DL_ALWAYS = 0,		/* cannot be masked */
+	DL_ERR = 0x1,		/* errors that should never happen */
+	DL_WARN = 0x2,		/* something questionable.
+				   recoverable errors */
+	DL_NOTE = 0x4,		/* infrequent, important debug info */
+	DL_INFO = 0x8,		/* debug information */
+	DL_VERBOSE = 0x10,	/* detailed info, such as buffer traces */
+	BE_DL_MIN_VALUE = 0x1,	/* this is the min value used */
+	BE_DL_MAX_VALUE = 0x80	/* this is the higheset value used */
+} ;
+
+extern unsigned int trace_level;
+
+#define TRACE(lm, fmt, args...)  {				\
+		if (trace_level & lm) {				\
+			printk(KERN_NOTICE "BE: %s:%d \n" fmt,	\
+			__FILE__ , __LINE__ , ## args);		\
+		}						\
+	}
+
+static inline unsigned int be_trace_set_level(unsigned int level)
+{
+	unsigned int old_level = trace_level;
+	trace_level = level;
+	return old_level;
+}
+
+#define be_trace_get_level() 	trace_level
+/*
+ * Returns number of pages spanned by the size of data
+ * starting at the given address.
+ */
+#define PAGES_SPANNED(_address, _size) \
+   ((u32)((((size_t)(_address) & (PAGE_SIZE - 1)) + \
+		(_size) + (PAGE_SIZE - 1)) >> PAGE_SHIFT))
+/* Byte offset into the page corresponding to given address */
+#define OFFSET_IN_PAGE(_addr_) ((size_t)(_addr_) & (PAGE_SIZE-1))
+
+/*
+ * circular subtract.
+ * Returns a - b assuming a circular number system, where a and b are
+ * in range (0, maxValue-1). If a==b, zero is returned so the
+ * highest value possible with this subtraction is maxValue-1.
+ */
+static inline u32 be_subc(u32 a, u32 b, u32 max)
+{
+	ASSERT(a <= max && b <= max);
+	ASSERT(max > 0);
+	return a >= b ? (a - b) : (max - b + a);
+}
+
+static inline u32 be_addc(u32 a, u32 b, u32 max)
+{
+	ASSERT(a < max);
+	ASSERT(max > 0);
+	return (max - a > b) ? (a + b) : (b + a - max);
+}
+
+/* descriptor for a physically contiguous memory used for ring */
+struct ring_desc {
+	u32 length;	/* length in bytes */
+	void *va; 	/* virtual address */
+	u64 pa;		/* bus address */
+} ;
+
+/*
+ * This structure stores information about a ring shared between hardware
+ * and software.  Each ring is allocated by the driver in the uncached
+ * extension and mapped into BladeEngine's unified table.
+ */
+struct mp_ring {
+	u32 pages;		/* queue size in pages */
+	u32 id;			/* queue id assigned by beklib */
+	u32 num;		/* number of elements in queue */
+	u32 cidx;		/* consumer index */
+	u32 pidx;		/* producer index -- not used by most rings */
+	u32 itemSize;		/* size in bytes of one object */
+
+	void *va;		/* The virtual address of the ring.
+				   This should be last to allow 32 & 64
+				   bit debugger extensions to work. */
+} ;
+
+/*-----------  amap bit filed get / set macros and functions -----*/
+/*
+ * Structures defined in the map header files (under fw/amap/) with names
+ * in the format BE_<name>_AMAP are pseudo structures with members
+ * of type u8. These structures are templates that are used in
+ * conjuntion with the structures with names in the format
+ * <name>_AMAP to calculate the bit masks and bit offsets to get or set
+ * bit fields in structures. The structures <name>_AMAP are arrays
+ * of 32 bits words and have the correct size.  The following macros
+ * provide convenient ways to get and set the various members
+ * in the structures without using strucctures with bit fields.
+ * Always use the macros AMAP_GET_BITS_PTR and AMAP_SET_BITS_PTR
+ * macros to extract and set various members.
+ */
+
+/*
+ * Returns the a bit mask for the register that is NOT shifted into location.
+ * That means return values always look like: 0x1, 0xFF, 0x7FF, etc...
+ */
+static inline u32 amap_mask(u32 bit_size)
+{
+	return bit_size == 32 ? 0xFFFFFFFF : (1 << bit_size) - 1;
+}
+
+#define AMAP_BIT_MASK(_struct_, field)       \
+	amap_mask(AMAP_BIT_SIZE(_struct_, field))
+
+/*
+ * non-optimized set bits function. First clears the bits and then assigns them.
+ * This does not require knowledge of the particular DWORD you are setting.
+ * e.g. AMAP_SET_BITS_PTR (struct, field1, &contextMemory, 123);
+ */
+static inline void
+amap_set(void *ptr, u32 dw_offset, u32 mask, u32 offset, u32 value)
+{
+	u32 *dw = (u32 *)ptr;
+	*(dw + dw_offset) &= ~(mask << offset);
+	*(dw + dw_offset) |= (mask & value) << offset;
+}
+
+#define AMAP_SET_BITS_PTR(_struct_, field, _structPtr_, val)	\
+	amap_set(_structPtr_, AMAP_WORD_OFFSET(_struct_, field),\
+		AMAP_BIT_MASK(_struct_, field),			\
+		AMAP_BIT_OFFSET(_struct_, field), val)
+/*
+ * Non-optimized routine that gets the bits without knowing the correct DWORD.
+ * e.g. fieldValue = AMAP_GET_BITS_PTR (struct, field1, &contextMemory);
+ */
+static inline u32
+amap_get(void *ptr, u32 dw_offset, u32 mask, u32 offset)
+{
+	u32 *dw = (u32 *)ptr;
+	return mask & (*(dw + dw_offset) >> offset);
+}
+#define AMAP_GET_BITS_PTR(_struct_, field, _structPtr_)			\
+	amap_get(_structPtr_, AMAP_WORD_OFFSET(_struct_, field),	\
+		AMAP_BIT_MASK(_struct_, field),				\
+		AMAP_BIT_OFFSET(_struct_, field))
+
+/* Returns 0-31 representing bit offset within a DWORD of a bitfield. */
+#define AMAP_BIT_OFFSET(_struct_, field)                  \
+	(offsetof(struct BE_ ## _struct_ ## _AMAP, field) % 32)
+
+/* Returns 0-n representing DWORD offset of bitfield within the structure. */
+#define AMAP_WORD_OFFSET(_struct_, field)  \
+		  (offsetof(struct BE_ ## _struct_ ## _AMAP, field)/32)
+
+/* Returns size of bitfield in bits. */
+#define AMAP_BIT_SIZE(_struct_, field) \
+		sizeof(((struct BE_ ## _struct_ ## _AMAP*)0)->field)
+
+struct be_mcc_wrb_response_copy {
+	u16 length;		/* bytes in response */
+	u16 fwcmd_offset;	/* offset within the wrb of the response */
+	void *va;		/* user's va to copy response into */
+
+} ;
+typedef void (*mcc_wrb_cqe_callback) (void *context, int status,
+				struct MCC_WRB_AMAP *optional_wrb);
+struct be_mcc_wrb_context {
+
+	mcc_wrb_cqe_callback internal_cb;	/* Function to call on
+						completion */
+	void *internal_cb_context;	/* Parameter to pass
+						   to completion function */
+
+	mcc_wrb_cqe_callback cb;	/* Function to call on completion */
+	void *cb_context;	/* Parameter to pass to completion function */
+
+	int *users_final_status;	/* pointer to a local
+						variable for synchronous
+						commands */
+	struct MCC_WRB_AMAP *wrb;	/* pointer to original wrb for embedded
+						commands only */
+	struct list_head next;	/* links context structs together in
+				   free list */
+
+	struct be_mcc_wrb_response_copy copy;	/* Optional parameters to copy
+					   embedded response to user's va */
+
+#if defined(BE_DEBUG)
+	u16 subsystem, opcode;	/* Track this FWCMD for debug builds. */
+	struct MCC_WRB_AMAP *ring_wrb;
+	u32 consumed_count;
+#endif
+} ;
+
+/*
+    Represents a function object for network or storage.  This
+    is used to manage per-function resources like MCC CQs, etc.
+*/
+struct be_function_object {
+
+	u32 magic;		/*!< magic for detecting memory corruption. */
+
+	/* PCI BAR mapped addresses */
+	u8 __iomem *csr_va;	/* CSR */
+	u8 __iomem *db_va;	/* Door Bell */
+	u8 __iomem *pci_va;	/* PCI config space */
+	u32 emulate;		/* if set, MPU is not available.
+				  Emulate everything.     */
+	u32 pend_queue_driving;	/* if set, drive the queued WRBs
+				   after releasing the WRB lock */
+
+	spinlock_t post_lock;	/* lock for verifying one thread posting wrbs */
+	spinlock_t cq_lock;	/* lock for verifying one thread
+				   processing cq */
+	spinlock_t mcc_context_lock;	/* lock for protecting mcc
+					   context free list */
+	unsigned long post_irq;
+	unsigned long cq_irq;
+
+	u32 type;
+	u32 pci_function_number;
+
+	struct be_mcc_object *mcc;	/* mcc rings. */
+
+	struct {
+		struct MCC_MAILBOX_AMAP *va;	/* VA to the mailbox */
+		u64 pa;	/* PA to the mailbox */
+		u32 length;	/* byte length of mailbox */
+
+		/* One default context struct used for posting at
+		 * least one MCC_WRB
+		 */
+		struct be_mcc_wrb_context default_context;
+		bool default_context_allocated;
+	} mailbox;
+
+	struct {
+
+		/* Wake on lans configured. */
+		u32 wol_bitmask;	/* bits 0,1,2,3 are set if
+					   corresponding index is enabled */
+	} config;
+
+
+	struct BE_FIRMWARE_CONFIG fw_config;
+} ;
+
+/*
+      Represents an Event Queue
+*/
+struct be_eq_object {
+	u32 magic;
+	atomic_t ref_count;
+
+	struct be_function_object *parent_function;
+
+	struct list_head eq_list;
+	struct list_head cq_list_head;
+
+	u32 eq_id;
+	void *cb_context;
+
+} ;
+
+/*
+    Manages a completion queue
+*/
+struct be_cq_object {
+	u32 magic;
+	atomic_t ref_count;
+
+	struct be_function_object *parent_function;
+	struct be_eq_object *eq_object;
+
+	struct list_head cq_list;
+	struct list_head cqlist_for_eq;
+
+	void *va;
+	u32 num_entries;
+
+	void *cb_context;
+
+	u32 cq_id;
+
+} ;
+
+/*
+    Manages an ethernet send queue
+*/
+struct be_ethsq_object {
+	u32 magic;
+
+	struct list_head list;
+
+	struct be_function_object *parent_function;
+	struct be_cq_object *cq_object;
+	u32 bid;
+
+} ;
+
+/*
+@brief
+    Manages an ethernet receive queue
+*/
+struct be_ethrq_object {
+	u32 magic;
+	struct list_head list;
+	struct be_function_object *parent_function;
+	u32 rid;
+	struct be_cq_object *cq_object;
+	struct be_cq_object *rss_cq_object[4];
+
+} ;
+
+/*
+    Manages an MCC
+*/
+typedef void (*mcc_async_event_callback) (void *context, u32 event_code,
+				void *event);
+struct be_mcc_object {
+	u32 magic;
+
+	struct be_function_object *parent_function;
+	struct list_head mcc_list;
+
+	struct be_cq_object *cq_object;
+
+	/* Async event callback for MCC CQ. */
+	mcc_async_event_callback async_cb;
+	void *async_context;
+
+	struct {
+		struct be_mcc_wrb_context *base;
+		u32 num;
+		struct list_head list_head;
+	} wrb_context;
+
+	struct {
+		struct ring_desc *rd;
+		struct mp_ring ring;
+	} sq;
+
+	struct {
+		struct mp_ring ring;
+	} cq;
+
+	u32 processing;		/* flag indicating that one thread
+				   is processing CQ */
+	u32 rearm;		/* doorbell rearm setting to make
+				   sure the active processing thread */
+	/* rearms the CQ if any of the threads requested it. */
+
+	struct list_head backlog;
+	u32 backlog_length;
+	u32 driving_backlog;
+	u32 consumed_index;
+
+} ;
+
+
+/* Queue context header -- the required software information for
+ * queueing a WRB.
+ */
+struct be_queue_driver_context {
+	mcc_wrb_cqe_callback internal_cb;	/* Function to call on
+						   completion */
+	void *internal_cb_context;	/* Parameter to pass
+						   to completion function */
+
+	mcc_wrb_cqe_callback cb;	/* Function to call on completion */
+	void *cb_context;	/* Parameter to pass to completion function */
+
+	struct be_mcc_wrb_response_copy copy;	/* Optional parameters to copy
+					   embedded response to user's va */
+	void *optional_fwcmd_va;
+	struct list_head list;
+	u32 bytes;
+} ;
+
+/*
+ * Common MCC WRB header that all commands require.
+ */
+struct be_mcc_wrb_header {
+	u8 rsvd[offsetof(struct BE_MCC_WRB_AMAP, payload)/8];
+} ;
+
+/*
+ * All non embedded commands supported by hwlib functions only allow
+ * 1 SGE.  This queue context handles them all.
+ */
+struct be_nonembedded_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct MCC_SGE_AMAP sge[1];
+} ;
+
+/*
+ * ------------------------------------------------------------------------
+ *  This section contains the specific queue struct for each command.
+ *  The user could always provide a be_generic_q_ctxt but this is a
+ *  rather large struct.  By using the specific struct, memory consumption
+ *  can be reduced.
+ * ------------------------------------------------------------------------
+ */
+
+struct be_link_status_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct FWCMD_COMMON_NTWK_LINK_STATUS_QUERY fwcmd;
+} ;
+
+struct be_multicast_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct FWCMD_COMMON_NTWK_MULTICAST_SET fwcmd;
+} ;
+
+
+struct be_vlan_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct FWCMD_COMMON_NTWK_VLAN_CONFIG fwcmd;
+} ;
+
+struct be_promiscuous_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct FWCMD_ETH_PROMISCUOUS fwcmd;
+} ;
+
+struct be_force_failover_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct FWCMD_COMMON_FORCE_FAILOVER fwcmd;
+} ;
+
+
+struct be_rxf_filter_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct FWCMD_COMMON_NTWK_RX_FILTER fwcmd;
+} ;
+
+struct be_eq_modify_delay_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct FWCMD_COMMON_MODIFY_EQ_DELAY fwcmd;
+} ;
+
+/*
+ * The generic context is the largest size that would be required.
+ * It is the software context plus an entire WRB.
+ */
+struct be_generic_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct MCC_WRB_PAYLOAD_AMAP payload;
+} ;
+
+/*
+ * Types for the BE_QUEUE_CONTEXT object.
+ */
+#define BE_QUEUE_INVALID	(0)
+#define BE_QUEUE_LINK_STATUS	(0xA006)
+#define BE_QUEUE_ETH_STATS	(0xA007)
+#define BE_QUEUE_TPM_STATS	(0xA008)
+#define BE_QUEUE_TCP_STATS	(0xA009)
+#define BE_QUEUE_MULTICAST	(0xA00A)
+#define BE_QUEUE_VLAN		(0xA00B)
+#define BE_QUEUE_RSS		(0xA00C)
+#define BE_QUEUE_FORCE_FAILOVER	(0xA00D)
+#define BE_QUEUE_PROMISCUOUS	(0xA00E)
+#define BE_QUEUE_WAKE_ON_LAN	(0xA00F)
+#define BE_QUEUE_NOP		(0xA010)
+
+/* --- BE_FUNCTION_ENUM --- */
+#define BE_FUNCTION_TYPE_ISCSI          (0)
+#define BE_FUNCTION_TYPE_NETWORK        (1)
+#define BE_FUNCTION_TYPE_ARM            (2)
+
+/* --- BE_ETH_TX_RING_TYPE_ENUM --- */
+#define BE_ETH_TX_RING_TYPE_FORWARDING  (1) 	/* Ether ring for forwarding */
+#define BE_ETH_TX_RING_TYPE_STANDARD    (2)	/* Ether ring for sending */
+						/* network packets. */
+#define BE_ETH_TX_RING_TYPE_BOUND       (3)	/* Ethernet ring for sending */
+						/* network packets, bound */
+						/* to a physical port. */
+/*
+ * ----------------------------------------------------------------------
+ *   API MACROS
+ * ----------------------------------------------------------------------
+ */
+#define BE_FWCMD_NAME(_short_name_)     struct FWCMD_##_short_name_
+#define BE_OPCODE_NAME(_short_name_)    OPCODE_##_short_name_
+#define BE_SUBSYSTEM_NAME(_short_name_) SUBSYSTEM_##_short_name_
+
+
+#define BE_PREPARE_EMBEDDED_FWCMD(_pfob_, _wrb_, _short_name_)	\
+	((BE_FWCMD_NAME(_short_name_) *)				\
+	be_function_prepare_embedded_fwcmd(_pfob_, _wrb_,	\
+		sizeof(BE_FWCMD_NAME(_short_name_)),		\
+		FIELD_SIZEOF(BE_FWCMD_NAME(_short_name_), params.request), \
+		FIELD_SIZEOF(BE_FWCMD_NAME(_short_name_), params.response), \
+		BE_OPCODE_NAME(_short_name_),				\
+		BE_SUBSYSTEM_NAME(_short_name_)));
+
+#define BE_PREPARE_NONEMBEDDED_FWCMD(_pfob_, _wrb_, _iva_, _ipa_, _short_name_)\
+	((BE_FWCMD_NAME(_short_name_) *)				\
+	be_function_prepare_nonembedded_fwcmd(_pfob_, _wrb_, (_iva_), (_ipa_), \
+		sizeof(BE_FWCMD_NAME(_short_name_)),		\
+		FIELD_SIZEOF(BE_FWCMD_NAME(_short_name_), params.request), \
+		FIELD_SIZEOF(BE_FWCMD_NAME(_short_name_), params.response), \
+		BE_OPCODE_NAME(_short_name_),				\
+		BE_SUBSYSTEM_NAME(_short_name_)));
+
+int be_function_object_create(u8 __iomem *csr_va, u8 __iomem *db_va,
+	u8 __iomem *pci_va, u32 function_type, struct ring_desc *mailbox_rd,
+	  struct be_function_object *pfob);
+
+int be_function_object_destroy(struct be_function_object *pfob);
+int be_function_cleanup(struct be_function_object *pfob);
+
+
+int be_function_get_fw_version(struct be_function_object *pfob,
+	struct FWCMD_COMMON_GET_FW_VERSION_RESPONSE_PAYLOAD *fw_version,
+	mcc_wrb_cqe_callback cb, void *cb_context);
+
+
+int be_eq_modify_delay(struct be_function_object *pfob,
+		   u32 num_eq, struct be_eq_object **eq_array,
+		   u32 *eq_delay_array, mcc_wrb_cqe_callback cb,
+		   void *cb_context,
+		   struct be_eq_modify_delay_q_ctxt *q_ctxt);
+
+
+
+int be_eq_create(struct be_function_object *pfob,
+	     struct ring_desc *rd, u32 eqe_size, u32 num_entries,
+	     u32 watermark, u32 timer_delay, struct be_eq_object *eq_object);
+
+int be_eq_destroy(struct be_eq_object *eq);
+
+int be_cq_create(struct be_function_object *pfob,
+	struct ring_desc *rd, u32 length,
+	bool solicited_eventable, bool no_delay,
+	u32 wm_thresh, struct be_eq_object *eq_object,
+	struct be_cq_object *cq_object);
+
+int be_cq_destroy(struct be_cq_object *cq);
+
+int be_mcc_ring_create(struct be_function_object *pfob,
+		   struct ring_desc *rd, u32 length,
+		   struct be_mcc_wrb_context *context_array,
+		   u32 num_context_entries,
+		   struct be_cq_object *cq, struct be_mcc_object *mcc);
+int be_mcc_ring_destroy(struct be_mcc_object *mcc_object);
+
+int be_mcc_process_cq(struct be_mcc_object *mcc_object, bool rearm);
+
+int be_mcc_add_async_event_callback(struct be_mcc_object *mcc_object,
+		mcc_async_event_callback cb, void *cb_context);
+
+int be_pci_soft_reset(struct be_function_object *pfob);
+
+
+int be_drive_POST(struct be_function_object *pfob);
+
+
+int be_eth_sq_create(struct be_function_object *pfob,
+		struct ring_desc *rd, u32 length_in_bytes,
+		u32 type, u32 ulp, struct be_cq_object *cq_object,
+		struct be_ethsq_object *eth_sq);
+
+struct be_eth_sq_parameters {
+	u32 port;
+	u32 rsvd0[2];
+} ;
+
+int be_eth_sq_create_ex(struct be_function_object *pfob,
+		    struct ring_desc *rd, u32 length_in_bytes,
+		    u32 type, u32 ulp, struct be_cq_object *cq_object,
+		    struct be_eth_sq_parameters *ex_parameters,
+		    struct be_ethsq_object *eth_sq);
+int be_eth_sq_destroy(struct be_ethsq_object *eth_sq);
+
+int be_eth_set_flow_control(struct be_function_object *pfob,
+			bool txfc_enable, bool rxfc_enable);
+
+int be_eth_get_flow_control(struct be_function_object *pfob,
+			bool *txfc_enable, bool *rxfc_enable);
+int be_eth_set_qos(struct be_function_object *pfob, u32 max_bps, u32 max_pps);
+
+int be_eth_get_qos(struct be_function_object *pfob, u32 *max_bps, u32 *max_pps);
+
+int be_eth_set_frame_size(struct be_function_object *pfob,
+		      u32 *tx_frame_size, u32 *rx_frame_size);
+
+int be_eth_rq_create(struct be_function_object *pfob,
+		 struct ring_desc *rd, struct be_cq_object *cq_object,
+		 struct be_cq_object *bcmc_cq_object,
+		 struct be_ethrq_object *eth_rq);
+
+int be_eth_rq_destroy(struct be_ethrq_object *eth_rq);
+
+int be_eth_rq_destroy_options(struct be_ethrq_object *eth_rq, bool flush,
+		mcc_wrb_cqe_callback cb, void *cb_context);
+int be_eth_rq_set_frag_size(struct be_function_object *pfob,
+		u32 new_frag_size_bytes, u32 *actual_frag_size_bytes);
+int be_eth_rq_get_frag_size(struct be_function_object *pfob,
+						u32 *frag_size_bytes);
+
+void *be_function_prepare_embedded_fwcmd(struct be_function_object *pfob,
+		   struct MCC_WRB_AMAP *wrb,
+		   u32 payload_length, u32 request_length,
+		   u32 response_length, u32 opcode, u32 subsystem);
+void *be_function_prepare_nonembedded_fwcmd(struct be_function_object *pfob,
+	struct MCC_WRB_AMAP *wrb, void *fwcmd_header_va, u64 fwcmd_header_pa,
+	u32 payload_length, u32 request_length, u32 response_length,
+	u32 opcode, u32 subsystem);
+
+
+struct MCC_WRB_AMAP *
+be_function_peek_mcc_wrb(struct be_function_object *pfob);
+
+int be_rxf_mac_address_read_write(struct be_function_object *pfob,
+	      bool port1, bool mac1, bool mgmt,
+	      bool write, bool permanent, u8 *mac_address,
+	      mcc_wrb_cqe_callback cb,
+	      void *cb_context);
+
+int be_rxf_multicast_config(struct be_function_object *pfob,
+			bool promiscuous, u32 num, u8 *mac_table,
+			mcc_wrb_cqe_callback cb,
+			void *cb_context,
+			struct be_multicast_q_ctxt *q_ctxt);
+
+int be_rxf_vlan_config(struct be_function_object *pfob,
+	   bool promiscuous, u32 num, u16 *vlan_tag_array,
+	   mcc_wrb_cqe_callback cb, void *cb_context,
+	   struct be_vlan_q_ctxt *q_ctxt);
+
+
+int be_rxf_link_status(struct be_function_object *pfob,
+		   struct BE_LINK_STATUS *link_status,
+		   mcc_wrb_cqe_callback cb,
+		   void *cb_context,
+		   struct be_link_status_q_ctxt *q_ctxt);
+
+
+int be_rxf_query_eth_statistics(struct be_function_object *pfob,
+		struct FWCMD_ETH_GET_STATISTICS *va_for_fwcmd,
+		u64 pa_for_fwcmd, mcc_wrb_cqe_callback cb,
+		void *cb_context,
+		struct be_nonembedded_q_ctxt *q_ctxt);
+
+int be_rxf_promiscuous(struct be_function_object *pfob,
+		   bool enable_port0, bool enable_port1,
+		   mcc_wrb_cqe_callback cb, void *cb_context,
+		   struct be_promiscuous_q_ctxt *q_ctxt);
+
+
+int be_rxf_filter_config(struct be_function_object *pfob,
+		     struct NTWK_RX_FILTER_SETTINGS *settings,
+		     mcc_wrb_cqe_callback cb,
+		     void *cb_context,
+		     struct be_rxf_filter_q_ctxt *q_ctxt);
+
+/*
+ * ------------------------------------------------------
+ *  internal functions used by hwlib
+ * ------------------------------------------------------
+ */
+
+
+int be_function_ring_destroy(struct be_function_object *pfob,
+		       u32 id, u32 ring_type, mcc_wrb_cqe_callback cb,
+		       void *cb_context,
+		       mcc_wrb_cqe_callback internal_cb,
+		       void *internal_callback_context);
+
+int be_function_post_mcc_wrb(struct be_function_object *pfob,
+		struct MCC_WRB_AMAP *wrb,
+		struct be_generic_q_ctxt *q_ctxt,
+		mcc_wrb_cqe_callback cb, void *cb_context,
+		mcc_wrb_cqe_callback internal_cb,
+		void *internal_cb_context, void *optional_fwcmd_va,
+		struct be_mcc_wrb_response_copy *response_copy);
+
+int be_function_queue_mcc_wrb(struct be_function_object *pfob,
+			  struct be_generic_q_ctxt *q_ctxt);
+
+/*
+ * ------------------------------------------------------
+ *  MCC QUEUE
+ * ------------------------------------------------------
+ */
+
+int be_mpu_init_mailbox(struct be_function_object *pfob, struct ring_desc *rd);
+
+
+struct MCC_WRB_AMAP *
+_be_mpu_peek_ring_wrb(struct be_mcc_object *mcc, bool driving_queue);
+
+struct be_mcc_wrb_context *
+_be_mcc_allocate_wrb_context(struct be_function_object *pfob);
+
+void _be_mcc_free_wrb_context(struct be_function_object *pfob,
+			 struct be_mcc_wrb_context *context);
+
+int _be_mpu_post_wrb_mailbox(struct be_function_object *pfob,
+	 struct MCC_WRB_AMAP *wrb, struct be_mcc_wrb_context *wrb_context);
+
+int _be_mpu_post_wrb_ring(struct be_mcc_object *mcc,
+	struct MCC_WRB_AMAP *wrb, struct be_mcc_wrb_context *wrb_context);
+
+void be_drive_mcc_wrb_queue(struct be_mcc_object *mcc);
+
+
+/*
+ * ------------------------------------------------------
+ *  Ring Sizes
+ * ------------------------------------------------------
+ */
+static inline u32 be_ring_encoding_to_length(u32 encoding, u32 object_size)
+{
+
+	ASSERT(encoding != 1);	/* 1 is rsvd */
+	ASSERT(encoding < 16);
+	ASSERT(object_size > 0);
+
+	if (encoding == 0)	/* 32k deep */
+		encoding = 16;
+
+	return (1 << (encoding - 1)) * object_size;
+}
+
+static inline
+u32 be_ring_length_to_encoding(u32 length_in_bytes, u32 object_size)
+{
+
+	u32 count, encoding;
+
+	ASSERT(object_size > 0);
+	ASSERT(length_in_bytes % object_size == 0);
+
+	count = length_in_bytes / object_size;
+
+	ASSERT(count > 1);
+	ASSERT(count <= 32 * 1024);
+	ASSERT(length_in_bytes <= 8 * PAGE_SIZE); /* max ring size in UT */
+
+	encoding = __ilog2_u32(count) + 1;
+
+	if (encoding == 16)
+		encoding = 0;	/* 32k deep */
+
+	return encoding;
+}
+
+void be_rd_to_pa_list(struct ring_desc *rd, struct PHYS_ADDR *pa_list,
+						u32 max_num);
+#endif /* __hwlib_h__ */
diff --git a/drivers/staging/benet/mpu.c b/drivers/staging/benet/mpu.c
new file mode 100644
index 000000000000..269cc11d3055
--- /dev/null
+++ b/drivers/staging/benet/mpu.c
@@ -0,0 +1,1364 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#include <linux/delay.h>
+#include "hwlib.h"
+#include "bestatus.h"
+
+static
+inline void mp_ring_create(struct mp_ring *ring, u32 num, u32 size, void *va)
+{
+	ASSERT(ring);
+	memset(ring, 0, sizeof(struct mp_ring));
+	ring->num = num;
+	ring->pages = DIV_ROUND_UP(num * size, PAGE_SIZE);
+	ring->itemSize = size;
+	ring->va = va;
+}
+
+/*
+ * -----------------------------------------------------------------------
+ * Interface for 2 index rings. i.e. consumer/producer rings
+ * --------------------------------------------------------------------------
+ */
+
+/* Returns number items pending on ring. */
+static inline u32 mp_ring_num_pending(struct mp_ring *ring)
+{
+	ASSERT(ring);
+	if (ring->num == 0)
+		return 0;
+	return be_subc(ring->pidx, ring->cidx, ring->num);
+}
+
+/* Returns number items free on ring. */
+static inline u32 mp_ring_num_empty(struct mp_ring *ring)
+{
+	ASSERT(ring);
+	return ring->num - 1 - mp_ring_num_pending(ring);
+}
+
+/* Consume 1 item */
+static inline void mp_ring_consume(struct mp_ring *ring)
+{
+	ASSERT(ring);
+	ASSERT(ring->pidx != ring->cidx);
+
+	ring->cidx = be_addc(ring->cidx, 1, ring->num);
+}
+
+/* Produce 1 item */
+static inline void mp_ring_produce(struct mp_ring *ring)
+{
+	ASSERT(ring);
+	ring->pidx = be_addc(ring->pidx, 1, ring->num);
+}
+
+/* Consume count items */
+static inline void mp_ring_consume_multiple(struct mp_ring *ring, u32 count)
+{
+	ASSERT(ring);
+	ASSERT(mp_ring_num_pending(ring) >= count);
+	ring->cidx = be_addc(ring->cidx, count, ring->num);
+}
+
+static inline void *mp_ring_item(struct mp_ring *ring, u32 index)
+{
+	ASSERT(ring);
+	ASSERT(index < ring->num);
+	ASSERT(ring->itemSize > 0);
+	return (u8 *) ring->va + index * ring->itemSize;
+}
+
+/* Ptr to produce item */
+static inline void *mp_ring_producer_ptr(struct mp_ring *ring)
+{
+	ASSERT(ring);
+	return mp_ring_item(ring, ring->pidx);
+}
+
+/*
+ * Returns a pointer to the current location in the ring.
+ * This is used for rings with 1 index.
+ */
+static inline void *mp_ring_current(struct mp_ring *ring)
+{
+	ASSERT(ring);
+	ASSERT(ring->pidx == 0);	/* not used */
+
+	return mp_ring_item(ring, ring->cidx);
+}
+
+/*
+ * Increment index for rings with only 1 index.
+ * This is used for rings with 1 index.
+ */
+static inline void *mp_ring_next(struct mp_ring *ring)
+{
+	ASSERT(ring);
+	ASSERT(ring->num > 0);
+	ASSERT(ring->pidx == 0);	/* not used */
+
+	ring->cidx = be_addc(ring->cidx, 1, ring->num);
+	return mp_ring_current(ring);
+}
+
+/*
+    This routine waits for a previously posted mailbox WRB to be completed.
+    Specifically it waits for the mailbox to say that it's ready to accept
+    more data by setting the LSB of the mailbox pd register to 1.
+
+    pcontroller      - The function object to post this data to
+
+    IRQL < DISPATCH_LEVEL
+*/
+static void be_mcc_mailbox_wait(struct be_function_object *pfob)
+{
+	struct MPU_MAILBOX_DB_AMAP mailbox_db;
+	u32 i = 0;
+	u32 ready;
+
+	if (pfob->emulate) {
+		/* No waiting for mailbox in emulated mode. */
+		return;
+	}
+
+	mailbox_db.dw[0] = PD_READ(pfob, mcc_bootstrap_db);
+	ready = AMAP_GET_BITS_PTR(MPU_MAILBOX_DB, ready, &mailbox_db);
+
+	while (ready == false) {
+		if ((++i & 0x3FFFF) == 0) {
+			TRACE(DL_WARN, "Waiting for mailbox ready - %dk polls",
+								i / 1000);
+		}
+		udelay(1);
+		mailbox_db.dw[0] = PD_READ(pfob, mcc_bootstrap_db);
+		ready = AMAP_GET_BITS_PTR(MPU_MAILBOX_DB, ready, &mailbox_db);
+	}
+}
+
+/*
+    This routine tells the MCC mailbox that there is data to processed
+    in the mailbox. It does this by setting the physical address for the
+    mailbox location and clearing the LSB.  This routine returns immediately
+    and does not wait for the WRB to be processed.
+
+    pcontroller      - The function object to post this data to
+
+    IRQL < DISPATCH_LEVEL
+
+*/
+static void be_mcc_mailbox_notify(struct be_function_object *pfob)
+{
+	struct MPU_MAILBOX_DB_AMAP mailbox_db;
+	u32 pa;
+
+	ASSERT(pfob->mailbox.pa);
+	ASSERT(pfob->mailbox.va);
+
+	/* If emulated, do not ring the mailbox */
+	if (pfob->emulate) {
+		TRACE(DL_WARN, "MPU disabled. Skipping mailbox notify.");
+		return;
+	}
+
+	/* form the higher bits in the address */
+	mailbox_db.dw[0] = 0;	/* init */
+	AMAP_SET_BITS_PTR(MPU_MAILBOX_DB, hi, &mailbox_db, 1);
+	AMAP_SET_BITS_PTR(MPU_MAILBOX_DB, ready, &mailbox_db, 0);
+
+	/* bits 34 to 63 */
+	pa = (u32) (pfob->mailbox.pa >> 34);
+	AMAP_SET_BITS_PTR(MPU_MAILBOX_DB, address, &mailbox_db, pa);
+
+	/* Wait for the MPU to be ready */
+	be_mcc_mailbox_wait(pfob);
+
+	/* Ring doorbell 1st time */
+	PD_WRITE(pfob, mcc_bootstrap_db, mailbox_db.dw[0]);
+
+	/* Wait for 1st write to be acknowledged. */
+	be_mcc_mailbox_wait(pfob);
+
+	/* lower bits 30 bits from 4th bit (bits 4 to 33)*/
+	pa = (u32) (pfob->mailbox.pa >> 4) & 0x3FFFFFFF;
+
+	AMAP_SET_BITS_PTR(MPU_MAILBOX_DB, hi, &mailbox_db, 0);
+	AMAP_SET_BITS_PTR(MPU_MAILBOX_DB, ready, &mailbox_db, 0);
+	AMAP_SET_BITS_PTR(MPU_MAILBOX_DB, address, &mailbox_db, pa);
+
+	/* Ring doorbell 2nd time */
+	PD_WRITE(pfob, mcc_bootstrap_db, mailbox_db.dw[0]);
+}
+
+/*
+    This routine tells the MCC mailbox that there is data to processed
+    in the mailbox. It does this by setting the physical address for the
+    mailbox location and clearing the LSB.  This routine spins until the
+    MPU writes a 1 into the LSB indicating that the data has been received
+    and is ready to be processed.
+
+    pcontroller      - The function object to post this data to
+
+    IRQL < DISPATCH_LEVEL
+*/
+static void
+be_mcc_mailbox_notify_and_wait(struct be_function_object *pfob)
+{
+	/*
+	 * Notify it
+	 */
+	be_mcc_mailbox_notify(pfob);
+	/*
+	 * Now wait for completion of WRB
+	 */
+	be_mcc_mailbox_wait(pfob);
+}
+
+void
+be_mcc_process_cqe(struct be_function_object *pfob,
+				struct MCC_CQ_ENTRY_AMAP *cqe)
+{
+	struct be_mcc_wrb_context *wrb_context = NULL;
+	u32 offset, status;
+	u8 *p;
+
+	ASSERT(cqe);
+	/*
+	 * A command completed.  Commands complete out-of-order.
+	 * Determine which command completed from the TAG.
+	 */
+	offset = offsetof(struct BE_MCC_CQ_ENTRY_AMAP, mcc_tag)/8;
+	p = (u8 *) cqe + offset;
+	wrb_context = (struct be_mcc_wrb_context *)(void *)(size_t)(*(u64 *)p);
+	ASSERT(wrb_context);
+
+	/*
+	 * Perform a response copy if requested.
+	 * Only copy data if the FWCMD is successful.
+	 */
+	status = AMAP_GET_BITS_PTR(MCC_CQ_ENTRY, completion_status, cqe);
+	if (status == MGMT_STATUS_SUCCESS && wrb_context->copy.length > 0) {
+		ASSERT(wrb_context->wrb);
+		ASSERT(wrb_context->copy.va);
+		p = (u8 *)wrb_context->wrb +
+				offsetof(struct BE_MCC_WRB_AMAP, payload)/8;
+		memcpy(wrb_context->copy.va,
+			  (u8 *)p + wrb_context->copy.fwcmd_offset,
+			  wrb_context->copy.length);
+	}
+
+	if (status)
+		status = BE_NOT_OK;
+	/* internal callback */
+	if (wrb_context->internal_cb) {
+		wrb_context->internal_cb(wrb_context->internal_cb_context,
+						status, wrb_context->wrb);
+	}
+
+	/* callback */
+	if (wrb_context->cb) {
+		wrb_context->cb(wrb_context->cb_context,
+					      status, wrb_context->wrb);
+	}
+	/* Free the context structure */
+	_be_mcc_free_wrb_context(pfob, wrb_context);
+}
+
+void be_drive_mcc_wrb_queue(struct be_mcc_object *mcc)
+{
+	struct be_function_object *pfob = NULL;
+	int status = BE_PENDING;
+	struct be_generic_q_ctxt *q_ctxt;
+	struct MCC_WRB_AMAP *wrb;
+	struct MCC_WRB_AMAP *queue_wrb;
+	u32 length, payload_length, sge_count, embedded;
+	unsigned long irql;
+
+	BUILD_BUG_ON((sizeof(struct be_generic_q_ctxt) <
+			  sizeof(struct be_queue_driver_context) +
+					sizeof(struct MCC_WRB_AMAP)));
+	pfob = mcc->parent_function;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	if (mcc->driving_backlog) {
+		spin_unlock_irqrestore(&pfob->post_lock, irql);
+		if (pfob->pend_queue_driving && pfob->mcc) {
+			pfob->pend_queue_driving = 0;
+			be_drive_mcc_wrb_queue(pfob->mcc);
+		}
+		return;
+	}
+	/* Acquire the flag to limit 1 thread to redrive posts. */
+	mcc->driving_backlog = 1;
+
+	while (!list_empty(&mcc->backlog)) {
+		wrb = _be_mpu_peek_ring_wrb(mcc, true);	/* Driving the queue */
+		if (!wrb)
+			break;	/* No space in the ring yet. */
+		/* Get the next queued entry to process. */
+		q_ctxt = list_first_entry(&mcc->backlog,
+				struct be_generic_q_ctxt, context.list);
+		list_del(&q_ctxt->context.list);
+		pfob->mcc->backlog_length--;
+		/*
+		 * Compute the required length of the WRB.
+		 * Since the queue element may be smaller than
+		 * the complete WRB, copy only the required number of bytes.
+		 */
+		queue_wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+		embedded = AMAP_GET_BITS_PTR(MCC_WRB, embedded, queue_wrb);
+		if (embedded) {
+			payload_length = AMAP_GET_BITS_PTR(MCC_WRB,
+						payload_length, queue_wrb);
+			length = sizeof(struct be_mcc_wrb_header) +
+								payload_length;
+		} else {
+			sge_count = AMAP_GET_BITS_PTR(MCC_WRB, sge_count,
+								queue_wrb);
+			ASSERT(sge_count == 1); /* only 1 frag. */
+			length = sizeof(struct be_mcc_wrb_header) +
+			    sge_count * sizeof(struct MCC_SGE_AMAP);
+		}
+
+		/*
+		 * Truncate the length based on the size of the
+		 * queue element.  Some elements that have output parameters
+		 * can be smaller than the payload_length field would
+		 * indicate.  We really only need to copy the request
+		 * parameters, not the response.
+		 */
+		length = min(length, (u32) (q_ctxt->context.bytes -
+			offsetof(struct be_generic_q_ctxt, wrb_header)));
+
+		/* Copy the queue element WRB into the ring. */
+		memcpy(wrb, &q_ctxt->wrb_header, length);
+
+		/* Post the wrb.  This should not fail assuming we have
+		 * enough context structs. */
+		status = be_function_post_mcc_wrb(pfob, wrb, NULL,
+			   q_ctxt->context.cb, q_ctxt->context.cb_context,
+			   q_ctxt->context.internal_cb,
+			   q_ctxt->context.internal_cb_context,
+			   q_ctxt->context.optional_fwcmd_va,
+			   &q_ctxt->context.copy);
+
+		if (status == BE_SUCCESS) {
+			/*
+			 * Synchronous completion. Since it was queued,
+			 * we will invoke the callback.
+			 * To the user, this is an asynchronous request.
+			 */
+			spin_unlock_irqrestore(&pfob->post_lock, irql);
+			if (pfob->pend_queue_driving && pfob->mcc) {
+				pfob->pend_queue_driving = 0;
+				be_drive_mcc_wrb_queue(pfob->mcc);
+			}
+
+			ASSERT(q_ctxt->context.cb);
+
+			q_ctxt->context.cb(
+				q_ctxt->context.cb_context,
+						BE_SUCCESS, NULL);
+
+			spin_lock_irqsave(&pfob->post_lock, irql);
+
+		} else if (status != BE_PENDING) {
+			/*
+			 * Another resource failed.  Should never happen
+			 * if we have sufficient MCC_WRB_CONTEXT structs.
+			 * Return to head of the queue.
+			 */
+			TRACE(DL_WARN, "Failed to post a queued WRB. 0x%x",
+			      status);
+			list_add(&q_ctxt->context.list, &mcc->backlog);
+			pfob->mcc->backlog_length++;
+			break;
+		}
+	}
+
+	/* Free the flag to limit 1 thread to redrive posts. */
+	mcc->driving_backlog = 0;
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+}
+
+/* This function asserts that the WRB was consumed in order. */
+#ifdef BE_DEBUG
+u32 be_mcc_wrb_consumed_in_order(struct be_mcc_object *mcc,
+					struct MCC_CQ_ENTRY_AMAP *cqe)
+{
+	struct be_mcc_wrb_context *wrb_context = NULL;
+	u32 wrb_index;
+	u32 wrb_consumed_in_order;
+	u32 offset;
+	u8 *p;
+
+	ASSERT(cqe);
+	/*
+	 * A command completed.  Commands complete out-of-order.
+	 * Determine which command completed from the TAG.
+	 */
+	offset = offsetof(struct BE_MCC_CQ_ENTRY_AMAP, mcc_tag)/8;
+	p = (u8 *) cqe + offset;
+	wrb_context = (struct be_mcc_wrb_context *)(void *)(size_t)(*(u64 *)p);
+
+	ASSERT(wrb_context);
+
+	wrb_index = (u32) (((u64)(size_t)wrb_context->ring_wrb -
+		(u64)(size_t)mcc->sq.ring.va) / sizeof(struct MCC_WRB_AMAP));
+
+	ASSERT(wrb_index < mcc->sq.ring.num);
+
+	wrb_consumed_in_order = (u32) (wrb_index == mcc->consumed_index);
+	mcc->consumed_index = be_addc(mcc->consumed_index, 1, mcc->sq.ring.num);
+	return wrb_consumed_in_order;
+}
+#endif
+
+int be_mcc_process_cq(struct be_mcc_object *mcc, bool rearm)
+{
+	struct be_function_object *pfob = NULL;
+	struct MCC_CQ_ENTRY_AMAP *cqe;
+	struct CQ_DB_AMAP db;
+	struct mp_ring *cq_ring = &mcc->cq.ring;
+	struct mp_ring *mp_ring = &mcc->sq.ring;
+	u32 num_processed = 0;
+	u32 consumed = 0, valid, completed, cqe_consumed, async_event;
+
+	pfob = mcc->parent_function;
+
+	spin_lock_irqsave(&pfob->cq_lock, pfob->cq_irq);
+
+	/*
+	 * Verify that only one thread is processing the CQ at once.
+	 * We cannot hold the lock while processing the CQ due to
+	 * the callbacks into the OS.  Therefore, this flag is used
+	 * to control it.  If any of the threads want to
+	 * rearm the CQ, we need to honor that.
+	 */
+	if (mcc->processing != 0) {
+		mcc->rearm = mcc->rearm || rearm;
+		goto Error;
+	} else {
+		mcc->processing = 1;	/* lock processing for this thread. */
+		mcc->rearm = rearm;	/* set our rearm setting */
+	}
+
+	spin_unlock_irqrestore(&pfob->cq_lock, pfob->cq_irq);
+
+	cqe = mp_ring_current(cq_ring);
+	valid = AMAP_GET_BITS_PTR(MCC_CQ_ENTRY, valid, cqe);
+	while (valid) {
+
+		if (num_processed >= 8) {
+			/* coalesce doorbells, but free space in cq
+			 * ring while processing. */
+			db.dw[0] = 0;	/* clear */
+			AMAP_SET_BITS_PTR(CQ_DB, qid, &db, cq_ring->id);
+			AMAP_SET_BITS_PTR(CQ_DB, rearm, &db, false);
+			AMAP_SET_BITS_PTR(CQ_DB, event, &db, false);
+			AMAP_SET_BITS_PTR(CQ_DB, num_popped, &db,
+							num_processed);
+			num_processed = 0;
+
+			PD_WRITE(pfob, cq_db, db.dw[0]);
+		}
+
+		async_event = AMAP_GET_BITS_PTR(MCC_CQ_ENTRY, async_event, cqe);
+		if (async_event) {
+			/* This is an asynchronous event. */
+			struct ASYNC_EVENT_TRAILER_AMAP *async_trailer =
+			    (struct ASYNC_EVENT_TRAILER_AMAP *)
+			    ((u8 *) cqe + sizeof(struct MCC_CQ_ENTRY_AMAP) -
+			     sizeof(struct ASYNC_EVENT_TRAILER_AMAP));
+			u32 event_code;
+			async_event = AMAP_GET_BITS_PTR(ASYNC_EVENT_TRAILER,
+						async_event, async_trailer);
+			ASSERT(async_event == 1);
+
+
+			valid = AMAP_GET_BITS_PTR(ASYNC_EVENT_TRAILER,
+						valid, async_trailer);
+			ASSERT(valid == 1);
+
+			/* Call the async event handler if it is installed. */
+			if (mcc->async_cb) {
+				event_code =
+					AMAP_GET_BITS_PTR(ASYNC_EVENT_TRAILER,
+						event_code, async_trailer);
+				mcc->async_cb(mcc->async_context,
+					    (u32) event_code, (void *) cqe);
+			}
+
+		} else {
+			/* This is a completion entry. */
+
+			/* No vm forwarding in this driver. */
+
+			cqe_consumed = AMAP_GET_BITS_PTR(MCC_CQ_ENTRY,
+						consumed, cqe);
+			if (cqe_consumed) {
+				/*
+				 * A command on the MCC ring was consumed.
+				 * Update the consumer index.
+				 * These occur in order.
+				 */
+				ASSERT(be_mcc_wrb_consumed_in_order(mcc, cqe));
+				consumed++;
+			}
+
+			completed = AMAP_GET_BITS_PTR(MCC_CQ_ENTRY,
+					completed, cqe);
+			if (completed) {
+				/* A command completed.  Use tag to
+				 * determine which command.  */
+				be_mcc_process_cqe(pfob, cqe);
+			}
+		}
+
+		/* Reset the CQE */
+		AMAP_SET_BITS_PTR(MCC_CQ_ENTRY, valid, cqe, false);
+		num_processed++;
+
+		/* Update our tracking for the CQ ring. */
+		cqe = mp_ring_next(cq_ring);
+		valid = AMAP_GET_BITS_PTR(MCC_CQ_ENTRY, valid, cqe);
+	}
+
+	TRACE(DL_INFO, "num_processed:0x%x, and consumed:0x%x",
+	      num_processed, consumed);
+	/*
+	 * Grab the CQ lock to synchronize the "rearm" setting for
+	 * the doorbell, and for clearing the "processing" flag.
+	 */
+	spin_lock_irqsave(&pfob->cq_lock, pfob->cq_irq);
+
+	/*
+	 * Rearm the cq.  This is done based on the global mcc->rearm
+	 * flag which combines the rearm parameter from the current
+	 * call to process_cq and any other threads
+	 * that tried to process the CQ while this one was active.
+	 * This handles the situation where a sync. fwcmd was processing
+	 * the CQ while the interrupt/dpc tries to process it.
+	 * The sync process gets to continue -- but it is now
+	 * responsible for the rearming.
+	 */
+	if (num_processed > 0 || mcc->rearm == true) {
+		db.dw[0] = 0;	/* clear */
+		AMAP_SET_BITS_PTR(CQ_DB, qid, &db, cq_ring->id);
+		AMAP_SET_BITS_PTR(CQ_DB, rearm, &db, mcc->rearm);
+		AMAP_SET_BITS_PTR(CQ_DB, event, &db, false);
+		AMAP_SET_BITS_PTR(CQ_DB, num_popped, &db, num_processed);
+
+		PD_WRITE(pfob, cq_db, db.dw[0]);
+	}
+	/*
+	 * Update the consumer index after ringing the CQ doorbell.
+	 * We don't want another thread to post more WRBs before we
+	 * have CQ space available.
+	 */
+	mp_ring_consume_multiple(mp_ring, consumed);
+
+	/* Clear the processing flag. */
+	mcc->processing = 0;
+
+Error:
+	spin_unlock_irqrestore(&pfob->cq_lock, pfob->cq_irq);
+	/*
+	 * Use the local variable to detect if the current thread
+	 * holds the WRB post lock.  If rearm is false, this is
+	 * either a synchronous command, or the upper layer driver is polling
+	 * from a thread.  We do not drive the queue from that
+	 * context since the driver may hold the
+	 * wrb post lock already.
+	 */
+	if (rearm)
+		be_drive_mcc_wrb_queue(mcc);
+	else
+		pfob->pend_queue_driving = 1;
+
+	return BE_SUCCESS;
+}
+
+/*
+ *============================================================================
+ *                  P U B L I C  R O U T I N E S
+ *============================================================================
+ */
+
+/*
+    This routine creates an MCC object.  This object contains an MCC send queue
+    and a CQ private to the MCC.
+
+    pcontroller      - Handle to a function object
+
+    EqObject            - EQ object that will be used to dispatch this MCC
+
+    ppMccObject         - Pointer to an internal Mcc Object returned.
+
+    Returns BE_SUCCESS if successfull,, otherwise a useful error code
+	is returned.
+
+    IRQL < DISPATCH_LEVEL
+
+*/
+int
+be_mcc_ring_create(struct be_function_object *pfob,
+		   struct ring_desc *rd, u32 length,
+		   struct be_mcc_wrb_context *context_array,
+		   u32 num_context_entries,
+		   struct be_cq_object *cq, struct be_mcc_object *mcc)
+{
+	int status = 0;
+
+	struct FWCMD_COMMON_MCC_CREATE *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	u32 num_entries_encoded, n, i;
+	void *va = NULL;
+	unsigned long irql;
+
+	if (length < sizeof(struct MCC_WRB_AMAP) * 2) {
+		TRACE(DL_ERR, "Invalid MCC ring length:%d", length);
+		return BE_NOT_OK;
+	}
+	/*
+	 * Reduce the actual ring size to be less than the number
+	 * of context entries.  This ensures that we run out of
+	 * ring WRBs first so the queuing works correctly.  We never
+	 * queue based on context structs.
+	 */
+	if (num_context_entries + 1 <
+			length / sizeof(struct MCC_WRB_AMAP) - 1) {
+
+		u32 max_length =
+		    (num_context_entries + 2) * sizeof(struct MCC_WRB_AMAP);
+
+		if (is_power_of_2(max_length))
+			length = __roundup_pow_of_two(max_length+1) / 2;
+		else
+			length = __roundup_pow_of_two(max_length) / 2;
+
+		ASSERT(length <= max_length);
+
+		TRACE(DL_WARN,
+			"MCC ring length reduced based on context entries."
+			" length:%d wrbs:%d context_entries:%d", length,
+			(int) (length / sizeof(struct MCC_WRB_AMAP)),
+			num_context_entries);
+	}
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	num_entries_encoded =
+	    be_ring_length_to_encoding(length, sizeof(struct MCC_WRB_AMAP));
+
+	/* Init MCC object. */
+	memset(mcc, 0, sizeof(*mcc));
+	mcc->parent_function = pfob;
+	mcc->cq_object = cq;
+
+	INIT_LIST_HEAD(&mcc->backlog);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		ASSERT(wrb);
+		TRACE(DL_ERR, "No free MCC WRBs in create EQ.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_MCC_CREATE);
+
+	fwcmd->params.request.num_pages = DIV_ROUND_UP(length, PAGE_SIZE);
+	/*
+	 * Program MCC ring context
+	 */
+	AMAP_SET_BITS_PTR(MCC_RING_CONTEXT, pdid,
+			&fwcmd->params.request.context, 0);
+	AMAP_SET_BITS_PTR(MCC_RING_CONTEXT, invalid,
+			&fwcmd->params.request.context, false);
+	AMAP_SET_BITS_PTR(MCC_RING_CONTEXT, ring_size,
+			&fwcmd->params.request.context, num_entries_encoded);
+
+	n = cq->cq_id;
+	AMAP_SET_BITS_PTR(MCC_RING_CONTEXT,
+				cq_id, &fwcmd->params.request.context, n);
+	be_rd_to_pa_list(rd, fwcmd->params.request.pages,
+				ARRAY_SIZE(fwcmd->params.request.pages));
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+						NULL, NULL, fwcmd, NULL);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "MCC to create CQ failed.");
+		goto error;
+	}
+	/*
+	 * Create a linked list of context structures
+	 */
+	mcc->wrb_context.base = context_array;
+	mcc->wrb_context.num = num_context_entries;
+	INIT_LIST_HEAD(&mcc->wrb_context.list_head);
+	memset(context_array, 0,
+		    sizeof(struct be_mcc_wrb_context) * num_context_entries);
+	for (i = 0; i < mcc->wrb_context.num; i++) {
+		list_add_tail(&context_array[i].next,
+					&mcc->wrb_context.list_head);
+	}
+
+	/*
+	 *
+	 * Create an mcc_ring for tracking WRB hw ring
+	 */
+	va = rd->va;
+	ASSERT(va);
+	mp_ring_create(&mcc->sq.ring, length / sizeof(struct MCC_WRB_AMAP),
+				sizeof(struct MCC_WRB_AMAP), va);
+	mcc->sq.ring.id = fwcmd->params.response.id;
+	/*
+	 * Init a mcc_ring for tracking the MCC CQ.
+	 */
+	ASSERT(cq->va);
+	mp_ring_create(&mcc->cq.ring, cq->num_entries,
+		       sizeof(struct MCC_CQ_ENTRY_AMAP), cq->va);
+	mcc->cq.ring.id = cq->cq_id;
+
+	/* Force zeroing of CQ. */
+	memset(cq->va, 0, cq->num_entries * sizeof(struct MCC_CQ_ENTRY_AMAP));
+
+	/* Initialize debug index. */
+	mcc->consumed_index = 0;
+
+	atomic_inc(&cq->ref_count);
+	pfob->mcc = mcc;
+
+	TRACE(DL_INFO, "MCC ring created. id:%d bytes:%d cq_id:%d cq_entries:%d"
+	      " num_context:%d", mcc->sq.ring.id, length,
+	      cq->cq_id, cq->num_entries, num_context_entries);
+
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This routine destroys an MCC send queue
+
+    MccObject         - Internal Mcc Object to be destroyed.
+
+    Returns BE_SUCCESS if successfull, otherwise an error code is returned.
+
+    IRQL < DISPATCH_LEVEL
+
+    The caller of this routine must ensure that no other WRB may be posted
+    until this routine returns.
+
+*/
+int be_mcc_ring_destroy(struct be_mcc_object *mcc)
+{
+	int status = 0;
+	struct be_function_object *pfob = mcc->parent_function;
+
+
+	ASSERT(mcc->processing == 0);
+
+	/*
+	 * Remove the ring from the function object.
+	 * This transitions back to mailbox mode.
+	 */
+	pfob->mcc = NULL;
+
+	/* Send fwcmd to destroy the queue.  (Using the mailbox.) */
+	status = be_function_ring_destroy(mcc->parent_function, mcc->sq.ring.id,
+			     FWCMD_RING_TYPE_MCC, NULL, NULL, NULL, NULL);
+	ASSERT(status == 0);
+
+	/* Release the SQ reference to the CQ */
+	atomic_dec(&mcc->cq_object->ref_count);
+
+	return status;
+}
+
+static void
+mcc_wrb_sync_cb(void *context, int staus, struct MCC_WRB_AMAP *wrb)
+{
+	struct be_mcc_wrb_context *wrb_context =
+				(struct be_mcc_wrb_context *) context;
+	ASSERT(wrb_context);
+	*wrb_context->users_final_status = staus;
+}
+
+/*
+    This routine posts a command to the MCC send queue
+
+    mcc       - Internal Mcc Object to be destroyed.
+
+    wrb             - wrb to post.
+
+    Returns BE_SUCCESS if successfull, otherwise an error code is returned.
+
+    IRQL < DISPATCH_LEVEL if CompletionCallback is not NULL
+    IRQL <=DISPATCH_LEVEL if CompletionCallback is  NULL
+
+    If this routine is called with CompletionCallback != NULL the
+    call is considered to be asynchronous and will return as soon
+    as the WRB is posted to the MCC with BE_PENDING.
+
+    If CompletionCallback is NULL, then this routine will not return until
+    a completion for this MCC command has been processed.
+    If called at DISPATCH_LEVEL the CompletionCallback must be NULL.
+
+    This routine should only be called if the MPU has been boostraped past
+    mailbox mode.
+
+
+*/
+int
+_be_mpu_post_wrb_ring(struct be_mcc_object *mcc, struct MCC_WRB_AMAP *wrb,
+				struct be_mcc_wrb_context *wrb_context)
+{
+
+	struct MCC_WRB_AMAP *ring_wrb = NULL;
+	int status = BE_PENDING;
+	int final_status = BE_PENDING;
+	mcc_wrb_cqe_callback cb = NULL;
+	struct MCC_DB_AMAP mcc_db;
+	u32 embedded;
+
+	ASSERT(mp_ring_num_empty(&mcc->sq.ring) > 0);
+	/*
+	 * Input wrb is most likely the next wrb in the ring, since the client
+	 * can peek at the address.
+	 */
+	ring_wrb = mp_ring_producer_ptr(&mcc->sq.ring);
+	if (wrb != ring_wrb) {
+		/* If not equal, copy it into the ring. */
+		memcpy(ring_wrb, wrb, sizeof(struct MCC_WRB_AMAP));
+	}
+#ifdef BE_DEBUG
+	wrb_context->ring_wrb = ring_wrb;
+#endif
+	embedded = AMAP_GET_BITS_PTR(MCC_WRB, embedded, ring_wrb);
+	if (embedded) {
+		/* embedded commands will have the response within the WRB. */
+		wrb_context->wrb = ring_wrb;
+	} else {
+		/*
+		 * non-embedded commands will not have the response
+		 * within the WRB, and they may complete out-of-order.
+		 * The WRB will not be valid to inspect
+		 * during the completion.
+		 */
+		wrb_context->wrb = NULL;
+	}
+	cb = wrb_context->cb;
+
+	if (cb == NULL) {
+		/* Assign our internal callback if this is a
+		 * synchronous call. */
+		wrb_context->cb = mcc_wrb_sync_cb;
+		wrb_context->cb_context = wrb_context;
+		wrb_context->users_final_status = &final_status;
+	}
+	/* Increment producer index */
+
+	mcc_db.dw[0] = 0;		/* initialize */
+	AMAP_SET_BITS_PTR(MCC_DB, rid, &mcc_db, mcc->sq.ring.id);
+	AMAP_SET_BITS_PTR(MCC_DB, numPosted, &mcc_db, 1);
+
+	mp_ring_produce(&mcc->sq.ring);
+	PD_WRITE(mcc->parent_function, mpu_mcc_db, mcc_db.dw[0]);
+	TRACE(DL_INFO, "pidx: %x and cidx: %x.", mcc->sq.ring.pidx,
+	      mcc->sq.ring.cidx);
+
+	if (cb == NULL) {
+		int polls = 0;	/* At >= 1 us per poll   */
+		/* Wait until this command completes, polling the CQ. */
+		do {
+			TRACE(DL_INFO, "FWCMD submitted in the poll mode.");
+			/* Do not rearm CQ in this context. */
+			be_mcc_process_cq(mcc, false);
+
+			if (final_status == BE_PENDING) {
+				if ((++polls & 0x7FFFF) == 0) {
+					TRACE(DL_WARN,
+					      "Warning : polling MCC CQ for %d"
+					      "ms.", polls / 1000);
+				}
+
+				udelay(1);
+			}
+
+			/* final_status changed when the command completes */
+		} while (final_status == BE_PENDING);
+
+		status = final_status;
+	}
+
+	return status;
+}
+
+struct MCC_WRB_AMAP *
+_be_mpu_peek_ring_wrb(struct be_mcc_object *mcc, bool driving_queue)
+{
+	/* If we have queued items, do not allow a post to bypass the queue. */
+	if (!driving_queue && !list_empty(&mcc->backlog))
+		return NULL;
+
+	if (mp_ring_num_empty(&mcc->sq.ring) <= 0)
+		return NULL;
+	return (struct MCC_WRB_AMAP *) mp_ring_producer_ptr(&mcc->sq.ring);
+}
+
+int
+be_mpu_init_mailbox(struct be_function_object *pfob, struct ring_desc *mailbox)
+{
+	ASSERT(mailbox);
+	pfob->mailbox.va = mailbox->va;
+	pfob->mailbox.pa =  cpu_to_le64(mailbox->pa);
+	pfob->mailbox.length = mailbox->length;
+
+	ASSERT(((u32)(size_t)pfob->mailbox.va & 0xf) == 0);
+	ASSERT(((u32)(size_t)pfob->mailbox.pa & 0xf) == 0);
+	/*
+	 * Issue the WRB to set MPU endianness
+	 */
+	{
+		u64 *endian_check = (u64 *) (pfob->mailbox.va +
+				offsetof(struct BE_MCC_MAILBOX_AMAP, wrb)/8);
+		*endian_check = 0xFF1234FFFF5678FFULL;
+	}
+
+	be_mcc_mailbox_notify_and_wait(pfob);
+
+	return BE_SUCCESS;
+}
+
+
+/*
+    This routine posts a command to the MCC mailbox.
+
+    FuncObj         - Function Object to post the WRB on behalf of.
+    wrb             - wrb to post.
+    CompletionCallback  - Address of a callback routine to invoke once the WRB
+				is completed.
+    CompletionCallbackContext - Opaque context to be passed during the call to
+				the CompletionCallback.
+    Returns BE_SUCCESS if successfull, otherwise an error code is returned.
+
+    IRQL <=DISPATCH_LEVEL if CompletionCallback is  NULL
+
+    This routine will block until a completion for this MCC command has been
+    processed. If called at DISPATCH_LEVEL the CompletionCallback must be NULL.
+
+    This routine should only be called if the MPU has not been boostraped past
+    mailbox mode.
+*/
+int
+_be_mpu_post_wrb_mailbox(struct be_function_object *pfob,
+	 struct MCC_WRB_AMAP *wrb, struct be_mcc_wrb_context *wrb_context)
+{
+	struct MCC_MAILBOX_AMAP *mailbox = NULL;
+	struct MCC_WRB_AMAP *mb_wrb;
+	struct MCC_CQ_ENTRY_AMAP *mb_cq;
+	u32 offset, status;
+
+	ASSERT(pfob->mcc == NULL);
+	mailbox = pfob->mailbox.va;
+	ASSERT(mailbox);
+
+	offset = offsetof(struct BE_MCC_MAILBOX_AMAP, wrb)/8;
+	mb_wrb = (struct MCC_WRB_AMAP *) (u8 *)mailbox + offset;
+	if (mb_wrb != wrb) {
+		memset(mailbox, 0, sizeof(*mailbox));
+		memcpy(mb_wrb, wrb, sizeof(struct MCC_WRB_AMAP));
+	}
+	/* The callback can inspect the final WRB to get output parameters. */
+	wrb_context->wrb = mb_wrb;
+
+	be_mcc_mailbox_notify_and_wait(pfob);
+
+	/* A command completed.  Use tag to determine which command. */
+	offset = offsetof(struct BE_MCC_MAILBOX_AMAP, cq)/8;
+	mb_cq = (struct MCC_CQ_ENTRY_AMAP *) ((u8 *)mailbox + offset);
+	be_mcc_process_cqe(pfob, mb_cq);
+
+	status = AMAP_GET_BITS_PTR(MCC_CQ_ENTRY, completion_status, mb_cq);
+	if (status)
+		status = BE_NOT_OK;
+	return status;
+}
+
+struct be_mcc_wrb_context *
+_be_mcc_allocate_wrb_context(struct be_function_object *pfob)
+{
+	struct be_mcc_wrb_context *context = NULL;
+	unsigned long irq;
+
+	spin_lock_irqsave(&pfob->mcc_context_lock, irq);
+
+	if (!pfob->mailbox.default_context_allocated) {
+		/* Use the single default context that we
+		 * always have allocated. */
+		pfob->mailbox.default_context_allocated = true;
+		context = &pfob->mailbox.default_context;
+	} else if (pfob->mcc) {
+		/* Get a context from the free list. If any are available. */
+		if (!list_empty(&pfob->mcc->wrb_context.list_head)) {
+			context = list_first_entry(
+				&pfob->mcc->wrb_context.list_head,
+					 struct be_mcc_wrb_context, next);
+		}
+	}
+
+	spin_unlock_irqrestore(&pfob->mcc_context_lock, irq);
+
+	return context;
+}
+
+void
+_be_mcc_free_wrb_context(struct be_function_object *pfob,
+			 struct be_mcc_wrb_context *context)
+{
+	unsigned long irq;
+
+	ASSERT(context);
+	/*
+	 * Zero during free to try and catch any bugs where the context
+	 * is accessed after a free.
+	 */
+	memset(context, 0, sizeof(context));
+
+	spin_lock_irqsave(&pfob->mcc_context_lock, irq);
+
+	if (context == &pfob->mailbox.default_context) {
+		/* Free the default context. */
+		ASSERT(pfob->mailbox.default_context_allocated);
+		pfob->mailbox.default_context_allocated = false;
+	} else {
+		/* Add to free list. */
+		ASSERT(pfob->mcc);
+		list_add_tail(&context->next,
+				&pfob->mcc->wrb_context.list_head);
+	}
+
+	spin_unlock_irqrestore(&pfob->mcc_context_lock, irq);
+}
+
+int
+be_mcc_add_async_event_callback(struct be_mcc_object *mcc_object,
+		mcc_async_event_callback cb, void *cb_context)
+{
+	/* Lock against anyone trying to change the callback/context pointers
+	 * while being used. */
+	spin_lock_irqsave(&mcc_object->parent_function->cq_lock,
+		mcc_object->parent_function->cq_irq);
+
+	/* Assign the async callback. */
+	mcc_object->async_context = cb_context;
+	mcc_object->async_cb = cb;
+
+	spin_unlock_irqrestore(&mcc_object->parent_function->cq_lock,
+					mcc_object->parent_function->cq_irq);
+
+	return BE_SUCCESS;
+}
+
+#define MPU_EP_CONTROL 0
+#define MPU_EP_SEMAPHORE 0xac
+
+/*
+ *-------------------------------------------------------------------
+ * Function: be_wait_for_POST_complete
+ *   Waits until the BladeEngine POST completes (either in error or success).
+ * pfob -
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *-------------------------------------------------------------------
+ */
+static int be_wait_for_POST_complete(struct be_function_object *pfob)
+{
+	struct MGMT_HBA_POST_STATUS_STRUCT_AMAP status;
+	int s;
+	u32 post_error, post_stage;
+
+	const u32 us_per_loop = 1000;	/* 1000us */
+	const u32 print_frequency_loops = 1000000 / us_per_loop;
+	const u32 max_loops = 60 * print_frequency_loops;
+	u32 loops = 0;
+
+	/*
+	 * Wait for arm fw indicating it is done or a fatal error happened.
+	 * Note: POST can take some time to complete depending on configuration
+	 * settings (consider ARM attempts to acquire an IP address
+	 * over DHCP!!!).
+	 *
+	 */
+	do {
+		status.dw[0] = ioread32(pfob->csr_va + MPU_EP_SEMAPHORE);
+		post_error = AMAP_GET_BITS_PTR(MGMT_HBA_POST_STATUS_STRUCT,
+						error, &status);
+		post_stage = AMAP_GET_BITS_PTR(MGMT_HBA_POST_STATUS_STRUCT,
+						stage, &status);
+		if (0 == (loops % print_frequency_loops)) {
+			/* Print current status */
+			TRACE(DL_INFO, "POST status = 0x%x (stage = 0x%x)",
+				status.dw[0], post_stage);
+		}
+		udelay(us_per_loop);
+	} while ((post_error != 1) &&
+		 (post_stage != POST_STAGE_ARMFW_READY) &&
+		 (++loops < max_loops));
+
+	if (post_error == 1) {
+		TRACE(DL_ERR, "POST error! Status = 0x%x (stage = 0x%x)",
+		      status.dw[0], post_stage);
+		s = BE_NOT_OK;
+	} else if (post_stage != POST_STAGE_ARMFW_READY) {
+		TRACE(DL_ERR, "POST time-out! Status = 0x%x (stage = 0x%x)",
+		      status.dw[0], post_stage);
+		s = BE_NOT_OK;
+	} else {
+		s = BE_SUCCESS;
+	}
+	return s;
+}
+
+/*
+ *-------------------------------------------------------------------
+ * Function: be_kickoff_and_wait_for_POST
+ *   Interacts with the BladeEngine management processor to initiate POST, and
+ *   subsequently waits until POST completes (either in error or success).
+ *   The caller must acquire the reset semaphore before initiating POST
+ *   to prevent multiple drivers interacting with the management processor.
+ *   Once POST is complete the caller must release the reset semaphore.
+ *   Callers who only want to wait for POST complete may call
+ *   be_wait_for_POST_complete.
+ * pfob -
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *-------------------------------------------------------------------
+ */
+static int
+be_kickoff_and_wait_for_POST(struct be_function_object *pfob)
+{
+	struct MGMT_HBA_POST_STATUS_STRUCT_AMAP status;
+	int s;
+
+	const u32 us_per_loop = 1000;	/* 1000us */
+	const u32 print_frequency_loops = 1000000 / us_per_loop;
+	const u32 max_loops = 5 * print_frequency_loops;
+	u32 loops = 0;
+	u32 post_error, post_stage;
+
+	/* Wait for arm fw awaiting host ready or a fatal error happened. */
+	TRACE(DL_INFO, "Wait for BladeEngine ready to POST");
+	do {
+		status.dw[0] = ioread32(pfob->csr_va + MPU_EP_SEMAPHORE);
+		post_error = AMAP_GET_BITS_PTR(MGMT_HBA_POST_STATUS_STRUCT,
+						error, &status);
+		post_stage = AMAP_GET_BITS_PTR(MGMT_HBA_POST_STATUS_STRUCT,
+						stage, &status);
+		if (0 == (loops % print_frequency_loops)) {
+			/* Print current status */
+			TRACE(DL_INFO, "POST status = 0x%x (stage = 0x%x)",
+			      status.dw[0], post_stage);
+		}
+		udelay(us_per_loop);
+	} while ((post_error != 1) &&
+		 (post_stage < POST_STAGE_AWAITING_HOST_RDY) &&
+		 (++loops < max_loops));
+
+	if (post_error == 1) {
+		TRACE(DL_ERR, "Pre-POST error! Status = 0x%x (stage = 0x%x)",
+		      status.dw[0], post_stage);
+		s = BE_NOT_OK;
+	} else if (post_stage == POST_STAGE_AWAITING_HOST_RDY) {
+		iowrite32(POST_STAGE_HOST_RDY, pfob->csr_va + MPU_EP_SEMAPHORE);
+
+		/* Wait for POST to complete */
+		s = be_wait_for_POST_complete(pfob);
+	} else {
+		/*
+		 * Either a timeout waiting for host ready signal or POST has
+		 * moved ahead without requiring a host ready signal.
+		 * Might as well give POST a chance to complete
+		 * (or timeout again).
+		 */
+		s = be_wait_for_POST_complete(pfob);
+	}
+	return s;
+}
+
+/*
+ *-------------------------------------------------------------------
+ * Function: be_pci_soft_reset
+ *   This function is called to issue a BladeEngine soft reset.
+ *   Callers should acquire the soft reset semaphore before calling this
+ *   function. Additionaly, callers should ensure they cannot be pre-empted
+ *   while the routine executes. Upon completion of this routine, callers
+ *   should release the reset semaphore. This routine implicitly waits
+ *   for BladeEngine POST to complete.
+ * pfob -
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *-------------------------------------------------------------------
+ */
+int be_pci_soft_reset(struct be_function_object *pfob)
+{
+	struct PCICFG_SOFT_RESET_CSR_AMAP soft_reset;
+	struct PCICFG_ONLINE0_CSR_AMAP pciOnline0;
+	struct PCICFG_ONLINE1_CSR_AMAP pciOnline1;
+	struct EP_CONTROL_CSR_AMAP epControlCsr;
+	int status = BE_SUCCESS;
+	u32 i, soft_reset_bit;
+
+	TRACE(DL_NOTE, "PCI reset...");
+
+	/* Issue soft reset #1 to get BladeEngine into a known state. */
+	soft_reset.dw[0] = PCICFG0_READ(pfob, soft_reset);
+	AMAP_SET_BITS_PTR(PCICFG_SOFT_RESET_CSR, softreset, soft_reset.dw, 1);
+	PCICFG0_WRITE(pfob, host_timer_int_ctrl, soft_reset.dw[0]);
+	/*
+	 * wait til soft reset is deasserted - hardware
+	 * deasserts after some time.
+	 */
+	i = 0;
+	do {
+		udelay(50);
+		soft_reset.dw[0] = PCICFG0_READ(pfob, soft_reset);
+		soft_reset_bit = AMAP_GET_BITS_PTR(PCICFG_SOFT_RESET_CSR,
+					softreset, soft_reset.dw);
+	} while (soft_reset_bit  && (i++ < 1024));
+	if (soft_reset_bit != 0) {
+		TRACE(DL_ERR, "Soft-reset #1 did not deassert as expected.");
+		status = BE_NOT_OK;
+		goto Error_label;
+	}
+	/* Mask everything  */
+	PCICFG0_WRITE(pfob, ue_status_low_mask, 0xFFFFFFFF);
+	PCICFG0_WRITE(pfob, ue_status_hi_mask, 0xFFFFFFFF);
+	/*
+	 * Set everything offline except MPU IRAM (it is offline with
+	 * the soft-reset, but soft-reset does not reset the PCICFG registers!)
+	 */
+	pciOnline0.dw[0] = 0;
+	pciOnline1.dw[0] = 0;
+	AMAP_SET_BITS_PTR(PCICFG_ONLINE1_CSR, mpu_iram_online,
+				pciOnline1.dw, 1);
+	PCICFG0_WRITE(pfob, online0, pciOnline0.dw[0]);
+	PCICFG0_WRITE(pfob, online1, pciOnline1.dw[0]);
+
+	udelay(20000);
+
+	/* Issue soft reset #2. */
+	AMAP_SET_BITS_PTR(PCICFG_SOFT_RESET_CSR, softreset, soft_reset.dw, 1);
+	PCICFG0_WRITE(pfob, host_timer_int_ctrl, soft_reset.dw[0]);
+	/*
+	 * wait til soft reset is deasserted - hardware
+	 * deasserts after some time.
+	 */
+	i = 0;
+	do {
+		udelay(50);
+		soft_reset.dw[0] = PCICFG0_READ(pfob, soft_reset);
+		soft_reset_bit = AMAP_GET_BITS_PTR(PCICFG_SOFT_RESET_CSR,
+					softreset, soft_reset.dw);
+	} while (soft_reset_bit  && (i++ < 1024));
+	if (soft_reset_bit != 0) {
+		TRACE(DL_ERR, "Soft-reset #1 did not deassert as expected.");
+		status = BE_NOT_OK;
+		goto Error_label;
+	}
+
+
+	udelay(20000);
+
+	/* Take MPU out of reset. */
+
+	epControlCsr.dw[0] = ioread32(pfob->csr_va + MPU_EP_CONTROL);
+	AMAP_SET_BITS_PTR(EP_CONTROL_CSR, CPU_reset, &epControlCsr, 0);
+	iowrite32((u32)epControlCsr.dw[0], pfob->csr_va + MPU_EP_CONTROL);
+
+	/* Kickoff BE POST and wait for completion */
+	status = be_kickoff_and_wait_for_POST(pfob);
+
+Error_label:
+	return status;
+}
+
+
+/*
+ *-------------------------------------------------------------------
+ * Function: be_pci_reset_required
+ *   This private function is called to detect if a host entity is
+ *   required to issue a PCI soft reset and subsequently drive
+ *   BladeEngine POST. Scenarios where this is required:
+ *   1) BIOS-less configuration
+ *   2) Hot-swap/plug/power-on
+ * pfob -
+ * return   true if a reset is required, false otherwise
+ *-------------------------------------------------------------------
+ */
+static bool be_pci_reset_required(struct be_function_object *pfob)
+{
+	struct MGMT_HBA_POST_STATUS_STRUCT_AMAP status;
+	bool do_reset = false;
+	u32 post_error, post_stage;
+
+	/*
+	 * Read the POST status register
+	 */
+	status.dw[0] = ioread32(pfob->csr_va + MPU_EP_SEMAPHORE);
+	post_error = AMAP_GET_BITS_PTR(MGMT_HBA_POST_STATUS_STRUCT, error,
+								&status);
+	post_stage = AMAP_GET_BITS_PTR(MGMT_HBA_POST_STATUS_STRUCT, stage,
+								&status);
+	if (post_stage <= POST_STAGE_AWAITING_HOST_RDY) {
+		/*
+		 * If BladeEngine is waiting for host ready indication,
+		 * we want to do a PCI reset.
+		 */
+		do_reset = true;
+	}
+
+	return do_reset;
+}
+
+/*
+ *-------------------------------------------------------------------
+ * Function: be_drive_POST
+ *   This function is called to drive BladeEngine POST. The
+ *   caller should ensure they cannot be pre-empted while this routine executes.
+ * pfob -
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *-------------------------------------------------------------------
+ */
+int be_drive_POST(struct be_function_object *pfob)
+{
+	int status;
+
+	if (false != be_pci_reset_required(pfob)) {
+		/* PCI reset is needed (implicitly starts and waits for POST) */
+		status = be_pci_soft_reset(pfob);
+	} else {
+		/* No PCI reset is needed, start POST */
+		status = be_kickoff_and_wait_for_POST(pfob);
+	}
+
+	return status;
+}
diff --git a/drivers/staging/benet/mpu.h b/drivers/staging/benet/mpu.h
new file mode 100644
index 000000000000..41f3f87516e5
--- /dev/null
+++ b/drivers/staging/benet/mpu.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __mpu_amap_h__
+#define __mpu_amap_h__
+#include "ep.h"
+
+/* Provide control parameters for the Managment Processor Unit. */
+struct BE_MPU_CSRMAP_AMAP {
+	struct BE_EP_CSRMAP_AMAP ep;
+	u8 rsvd0[128];	/* DWORD 64 */
+	u8 rsvd1[32];	/* DWORD 68 */
+	u8 rsvd2[192];	/* DWORD 69 */
+	u8 rsvd3[192];	/* DWORD 75 */
+	u8 rsvd4[32];	/* DWORD 81 */
+	u8 rsvd5[32];	/* DWORD 82 */
+	u8 rsvd6[32];	/* DWORD 83 */
+	u8 rsvd7[32];	/* DWORD 84 */
+	u8 rsvd8[32];	/* DWORD 85 */
+	u8 rsvd9[32];	/* DWORD 86 */
+	u8 rsvd10[32];	/* DWORD 87 */
+	u8 rsvd11[32];	/* DWORD 88 */
+	u8 rsvd12[32];	/* DWORD 89 */
+	u8 rsvd13[32];	/* DWORD 90 */
+	u8 rsvd14[32];	/* DWORD 91 */
+	u8 rsvd15[32];	/* DWORD 92 */
+	u8 rsvd16[32];	/* DWORD 93 */
+	u8 rsvd17[32];	/* DWORD 94 */
+	u8 rsvd18[32];	/* DWORD 95 */
+	u8 rsvd19[32];	/* DWORD 96 */
+	u8 rsvd20[32];	/* DWORD 97 */
+	u8 rsvd21[32];	/* DWORD 98 */
+	u8 rsvd22[32];	/* DWORD 99 */
+	u8 rsvd23[32];	/* DWORD 100 */
+	u8 rsvd24[32];	/* DWORD 101 */
+	u8 rsvd25[32];	/* DWORD 102 */
+	u8 rsvd26[32];	/* DWORD 103 */
+	u8 rsvd27[32];	/* DWORD 104 */
+	u8 rsvd28[96];	/* DWORD 105 */
+	u8 rsvd29[32];	/* DWORD 108 */
+	u8 rsvd30[32];	/* DWORD 109 */
+	u8 rsvd31[32];	/* DWORD 110 */
+	u8 rsvd32[32];	/* DWORD 111 */
+	u8 rsvd33[32];	/* DWORD 112 */
+	u8 rsvd34[96];	/* DWORD 113 */
+	u8 rsvd35[32];	/* DWORD 116 */
+	u8 rsvd36[32];	/* DWORD 117 */
+	u8 rsvd37[32];	/* DWORD 118 */
+	u8 rsvd38[32];	/* DWORD 119 */
+	u8 rsvd39[32];	/* DWORD 120 */
+	u8 rsvd40[32];	/* DWORD 121 */
+	u8 rsvd41[134][32];	/* DWORD 122 */
+} __packed;
+struct MPU_CSRMAP_AMAP {
+	u32 dw[256];
+};
+
+#endif /* __mpu_amap_h__ */
diff --git a/drivers/staging/benet/mpu_context.h b/drivers/staging/benet/mpu_context.h
new file mode 100644
index 000000000000..8ce90f9c46c2
--- /dev/null
+++ b/drivers/staging/benet/mpu_context.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __mpu_context_amap_h__
+#define __mpu_context_amap_h__
+
+/*
+ * Management command and control ring context. The MPUs BTLR_CTRL1 CSR
+ * controls the writeback behavior of the producer and consumer index values.
+ */
+struct BE_MCC_RING_CONTEXT_AMAP {
+	u8 con_index[16];	/* DWORD 0 */
+	u8 ring_size[4];	/* DWORD 0 */
+	u8 cq_id[11];	/* DWORD 0 */
+	u8 rsvd0;		/* DWORD 0 */
+	u8 prod_index[16];	/* DWORD 1 */
+	u8 pdid[15];	/* DWORD 1 */
+	u8 invalid;		/* DWORD 1 */
+	u8 cmd_pending_current[7];	/* DWORD 2 */
+	u8 rsvd1[25];	/* DWORD 2 */
+	u8 hpi_port_cq_id[11];	/* DWORD 3 */
+	u8 rsvd2[5];	/* DWORD 3 */
+	u8 cmd_pending_max[7];	/* DWORD 3 */
+	u8 rsvd3[9];	/* DWORD 3 */
+} __packed;
+struct MCC_RING_CONTEXT_AMAP {
+	u32 dw[4];
+};
+
+#endif /* __mpu_context_amap_h__ */
diff --git a/drivers/staging/benet/pcicfg.h b/drivers/staging/benet/pcicfg.h
new file mode 100644
index 000000000000..7c15684adf4a
--- /dev/null
+++ b/drivers/staging/benet/pcicfg.h
@@ -0,0 +1,825 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __pcicfg_amap_h__
+#define __pcicfg_amap_h__
+
+/* Vendor and Device ID Register. */
+struct BE_PCICFG_ID_CSR_AMAP {
+	u8 vendorid[16];	/* DWORD 0 */
+	u8 deviceid[16];	/* DWORD 0 */
+} __packed;
+struct PCICFG_ID_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* IO Bar Register. */
+struct BE_PCICFG_IOBAR_CSR_AMAP {
+	u8 iospace;		/* DWORD 0 */
+	u8 rsvd0[7];	/* DWORD 0 */
+	u8 iobar[24];	/* DWORD 0 */
+} __packed;
+struct PCICFG_IOBAR_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Memory BAR 0 Register. */
+struct BE_PCICFG_MEMBAR0_CSR_AMAP {
+	u8 memspace;	/* DWORD 0 */
+	u8 type[2];		/* DWORD 0 */
+	u8 pf;		/* DWORD 0 */
+	u8 rsvd0[10];	/* DWORD 0 */
+	u8 membar0[18];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MEMBAR0_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Memory BAR 1 - Low Address Register. */
+struct BE_PCICFG_MEMBAR1_LO_CSR_AMAP {
+	u8 memspace;	/* DWORD 0 */
+	u8 type[2];		/* DWORD 0 */
+	u8 pf;		/* DWORD 0 */
+	u8 rsvd0[13];	/* DWORD 0 */
+	u8 membar1lo[15];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MEMBAR1_LO_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Memory BAR 1 - High Address Register. */
+struct BE_PCICFG_MEMBAR1_HI_CSR_AMAP {
+	u8 membar1hi[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MEMBAR1_HI_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Memory BAR 2 - Low Address Register. */
+struct BE_PCICFG_MEMBAR2_LO_CSR_AMAP {
+	u8 memspace;	/* DWORD 0 */
+	u8 type[2];		/* DWORD 0 */
+	u8 pf;		/* DWORD 0 */
+	u8 rsvd0[17];	/* DWORD 0 */
+	u8 membar2lo[11];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MEMBAR2_LO_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Memory BAR 2 - High Address Register. */
+struct BE_PCICFG_MEMBAR2_HI_CSR_AMAP {
+	u8 membar2hi[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MEMBAR2_HI_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Subsystem Vendor and ID (Function 0) Register. */
+struct BE_PCICFG_SUBSYSTEM_ID_F0_CSR_AMAP {
+	u8 subsys_vendor_id[16];	/* DWORD 0 */
+	u8 subsys_id[16];	/* DWORD 0 */
+} __packed;
+struct PCICFG_SUBSYSTEM_ID_F0_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Subsystem Vendor and ID (Function 1) Register. */
+struct BE_PCICFG_SUBSYSTEM_ID_F1_CSR_AMAP {
+	u8 subsys_vendor_id[16];	/* DWORD 0 */
+	u8 subsys_id[16];	/* DWORD 0 */
+} __packed;
+struct PCICFG_SUBSYSTEM_ID_F1_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Semaphore Register. */
+struct BE_PCICFG_SEMAPHORE_CSR_AMAP {
+	u8 locked;		/* DWORD 0 */
+	u8 rsvd0[31];	/* DWORD 0 */
+} __packed;
+struct PCICFG_SEMAPHORE_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Soft Reset Register. */
+struct BE_PCICFG_SOFT_RESET_CSR_AMAP {
+	u8 rsvd0[7];	/* DWORD 0 */
+	u8 softreset;	/* DWORD 0 */
+	u8 rsvd1[16];	/* DWORD 0 */
+	u8 nec_ll_rcvdetect_i[8];	/* DWORD 0 */
+} __packed;
+struct PCICFG_SOFT_RESET_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Unrecoverable Error Status (Low) Register. Each bit corresponds to
+ * an internal Unrecoverable Error.  These are set by hardware and may be
+ * cleared by writing a one to the respective bit(s) to be cleared.  Any
+ * bit being set that is also unmasked will result in Unrecoverable Error
+ * interrupt notification to the host CPU and/or Server Management chip
+ * and the transitioning of BladeEngine to an Offline state.
+ */
+struct BE_PCICFG_UE_STATUS_LOW_CSR_AMAP {
+	u8 cev_ue_status;	/* DWORD 0 */
+	u8 ctx_ue_status;	/* DWORD 0 */
+	u8 dbuf_ue_status;	/* DWORD 0 */
+	u8 erx_ue_status;	/* DWORD 0 */
+	u8 host_ue_status;	/* DWORD 0 */
+	u8 mpu_ue_status;	/* DWORD 0 */
+	u8 ndma_ue_status;	/* DWORD 0 */
+	u8 ptc_ue_status;	/* DWORD 0 */
+	u8 rdma_ue_status;	/* DWORD 0 */
+	u8 rxf_ue_status;	/* DWORD 0 */
+	u8 rxips_ue_status;	/* DWORD 0 */
+	u8 rxulp0_ue_status;	/* DWORD 0 */
+	u8 rxulp1_ue_status;	/* DWORD 0 */
+	u8 rxulp2_ue_status;	/* DWORD 0 */
+	u8 tim_ue_status;	/* DWORD 0 */
+	u8 tpost_ue_status;	/* DWORD 0 */
+	u8 tpre_ue_status;	/* DWORD 0 */
+	u8 txips_ue_status;	/* DWORD 0 */
+	u8 txulp0_ue_status;	/* DWORD 0 */
+	u8 txulp1_ue_status;	/* DWORD 0 */
+	u8 uc_ue_status;	/* DWORD 0 */
+	u8 wdma_ue_status;	/* DWORD 0 */
+	u8 txulp2_ue_status;	/* DWORD 0 */
+	u8 host1_ue_status;	/* DWORD 0 */
+	u8 p0_ob_link_ue_status;	/* DWORD 0 */
+	u8 p1_ob_link_ue_status;	/* DWORD 0 */
+	u8 host_gpio_ue_status;	/* DWORD 0 */
+	u8 mbox_netw_ue_status;	/* DWORD 0 */
+	u8 mbox_stor_ue_status;	/* DWORD 0 */
+	u8 axgmac0_ue_status;	/* DWORD 0 */
+	u8 axgmac1_ue_status;	/* DWORD 0 */
+	u8 mpu_intpend_ue_status;	/* DWORD 0 */
+} __packed;
+struct PCICFG_UE_STATUS_LOW_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Unrecoverable Error Status (High) Register. Each bit corresponds to
+ * an internal Unrecoverable Error.  These are set by hardware and may be
+ * cleared by writing a one to the respective bit(s) to be cleared.  Any
+ * bit being set that is also unmasked will result in Unrecoverable Error
+ * interrupt notification to the host CPU and/or Server Management chip;
+ * and the transitioning of BladeEngine to an Offline state.
+ */
+struct BE_PCICFG_UE_STATUS_HI_CSR_AMAP {
+	u8 jtag_ue_status;	/* DWORD 0 */
+	u8 lpcmemhost_ue_status;	/* DWORD 0 */
+	u8 mgmt_mac_ue_status;	/* DWORD 0 */
+	u8 mpu_iram_ue_status;	/* DWORD 0 */
+	u8 pcs0online_ue_status;	/* DWORD 0 */
+	u8 pcs1online_ue_status;	/* DWORD 0 */
+	u8 pctl0_ue_status;	/* DWORD 0 */
+	u8 pctl1_ue_status;	/* DWORD 0 */
+	u8 pmem_ue_status;	/* DWORD 0 */
+	u8 rr_ue_status;	/* DWORD 0 */
+	u8 rxpp_ue_status;	/* DWORD 0 */
+	u8 txpb_ue_status;	/* DWORD 0 */
+	u8 txp_ue_status;	/* DWORD 0 */
+	u8 xaui_ue_status;	/* DWORD 0 */
+	u8 arm_ue_status;	/* DWORD 0 */
+	u8 ipc_ue_status;	/* DWORD 0 */
+	u8 rsvd0[16];	/* DWORD 0 */
+} __packed;
+struct PCICFG_UE_STATUS_HI_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Unrecoverable Error Mask (Low) Register. Each bit, when set to one,
+ * will mask the associated Unrecoverable  Error status bit from notification
+ * of Unrecoverable Error to the host CPU and/or Server Managment chip and the
+ * transitioning of all BladeEngine units to an Offline state.
+ */
+struct BE_PCICFG_UE_STATUS_LOW_MASK_CSR_AMAP {
+	u8 cev_ue_mask;	/* DWORD 0 */
+	u8 ctx_ue_mask;	/* DWORD 0 */
+	u8 dbuf_ue_mask;	/* DWORD 0 */
+	u8 erx_ue_mask;	/* DWORD 0 */
+	u8 host_ue_mask;	/* DWORD 0 */
+	u8 mpu_ue_mask;	/* DWORD 0 */
+	u8 ndma_ue_mask;	/* DWORD 0 */
+	u8 ptc_ue_mask;	/* DWORD 0 */
+	u8 rdma_ue_mask;	/* DWORD 0 */
+	u8 rxf_ue_mask;	/* DWORD 0 */
+	u8 rxips_ue_mask;	/* DWORD 0 */
+	u8 rxulp0_ue_mask;	/* DWORD 0 */
+	u8 rxulp1_ue_mask;	/* DWORD 0 */
+	u8 rxulp2_ue_mask;	/* DWORD 0 */
+	u8 tim_ue_mask;	/* DWORD 0 */
+	u8 tpost_ue_mask;	/* DWORD 0 */
+	u8 tpre_ue_mask;	/* DWORD 0 */
+	u8 txips_ue_mask;	/* DWORD 0 */
+	u8 txulp0_ue_mask;	/* DWORD 0 */
+	u8 txulp1_ue_mask;	/* DWORD 0 */
+	u8 uc_ue_mask;	/* DWORD 0 */
+	u8 wdma_ue_mask;	/* DWORD 0 */
+	u8 txulp2_ue_mask;	/* DWORD 0 */
+	u8 host1_ue_mask;	/* DWORD 0 */
+	u8 p0_ob_link_ue_mask;	/* DWORD 0 */
+	u8 p1_ob_link_ue_mask;	/* DWORD 0 */
+	u8 host_gpio_ue_mask;	/* DWORD 0 */
+	u8 mbox_netw_ue_mask;	/* DWORD 0 */
+	u8 mbox_stor_ue_mask;	/* DWORD 0 */
+	u8 axgmac0_ue_mask;	/* DWORD 0 */
+	u8 axgmac1_ue_mask;	/* DWORD 0 */
+	u8 mpu_intpend_ue_mask;	/* DWORD 0 */
+} __packed;
+struct PCICFG_UE_STATUS_LOW_MASK_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Unrecoverable Error Mask (High) Register. Each bit, when set to one,
+ * will mask the associated Unrecoverable Error status bit from notification
+ * of Unrecoverable Error to the host CPU and/or Server Managment chip and the
+ * transitioning of all BladeEngine units to an Offline state.
+ */
+struct BE_PCICFG_UE_STATUS_HI_MASK_CSR_AMAP {
+	u8 jtag_ue_mask;	/* DWORD 0 */
+	u8 lpcmemhost_ue_mask;	/* DWORD 0 */
+	u8 mgmt_mac_ue_mask;	/* DWORD 0 */
+	u8 mpu_iram_ue_mask;	/* DWORD 0 */
+	u8 pcs0online_ue_mask;	/* DWORD 0 */
+	u8 pcs1online_ue_mask;	/* DWORD 0 */
+	u8 pctl0_ue_mask;	/* DWORD 0 */
+	u8 pctl1_ue_mask;	/* DWORD 0 */
+	u8 pmem_ue_mask;	/* DWORD 0 */
+	u8 rr_ue_mask;	/* DWORD 0 */
+	u8 rxpp_ue_mask;	/* DWORD 0 */
+	u8 txpb_ue_mask;	/* DWORD 0 */
+	u8 txp_ue_mask;	/* DWORD 0 */
+	u8 xaui_ue_mask;	/* DWORD 0 */
+	u8 arm_ue_mask;	/* DWORD 0 */
+	u8 ipc_ue_mask;	/* DWORD 0 */
+	u8 rsvd0[16];	/* DWORD 0 */
+} __packed;
+struct PCICFG_UE_STATUS_HI_MASK_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Online Control Register 0. This register controls various units within
+ * BladeEngine being in an Online or Offline state.
+ */
+struct BE_PCICFG_ONLINE0_CSR_AMAP {
+	u8 cev_online;	/* DWORD 0 */
+	u8 ctx_online;	/* DWORD 0 */
+	u8 dbuf_online;	/* DWORD 0 */
+	u8 erx_online;	/* DWORD 0 */
+	u8 host_online;	/* DWORD 0 */
+	u8 mpu_online;	/* DWORD 0 */
+	u8 ndma_online;	/* DWORD 0 */
+	u8 ptc_online;	/* DWORD 0 */
+	u8 rdma_online;	/* DWORD 0 */
+	u8 rxf_online;	/* DWORD 0 */
+	u8 rxips_online;	/* DWORD 0 */
+	u8 rxulp0_online;	/* DWORD 0 */
+	u8 rxulp1_online;	/* DWORD 0 */
+	u8 rxulp2_online;	/* DWORD 0 */
+	u8 tim_online;	/* DWORD 0 */
+	u8 tpost_online;	/* DWORD 0 */
+	u8 tpre_online;	/* DWORD 0 */
+	u8 txips_online;	/* DWORD 0 */
+	u8 txulp0_online;	/* DWORD 0 */
+	u8 txulp1_online;	/* DWORD 0 */
+	u8 uc_online;	/* DWORD 0 */
+	u8 wdma_online;	/* DWORD 0 */
+	u8 txulp2_online;	/* DWORD 0 */
+	u8 host1_online;	/* DWORD 0 */
+	u8 p0_ob_link_online;	/* DWORD 0 */
+	u8 p1_ob_link_online;	/* DWORD 0 */
+	u8 host_gpio_online;	/* DWORD 0 */
+	u8 mbox_netw_online;	/* DWORD 0 */
+	u8 mbox_stor_online;	/* DWORD 0 */
+	u8 axgmac0_online;	/* DWORD 0 */
+	u8 axgmac1_online;	/* DWORD 0 */
+	u8 mpu_intpend_online;	/* DWORD 0 */
+} __packed;
+struct PCICFG_ONLINE0_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Online Control Register 1. This register controls various units within
+ * BladeEngine being in an Online or Offline state.
+ */
+struct BE_PCICFG_ONLINE1_CSR_AMAP {
+	u8 jtag_online;	/* DWORD 0 */
+	u8 lpcmemhost_online;	/* DWORD 0 */
+	u8 mgmt_mac_online;	/* DWORD 0 */
+	u8 mpu_iram_online;	/* DWORD 0 */
+	u8 pcs0online_online;	/* DWORD 0 */
+	u8 pcs1online_online;	/* DWORD 0 */
+	u8 pctl0_online;	/* DWORD 0 */
+	u8 pctl1_online;	/* DWORD 0 */
+	u8 pmem_online;	/* DWORD 0 */
+	u8 rr_online;	/* DWORD 0 */
+	u8 rxpp_online;	/* DWORD 0 */
+	u8 txpb_online;	/* DWORD 0 */
+	u8 txp_online;	/* DWORD 0 */
+	u8 xaui_online;	/* DWORD 0 */
+	u8 arm_online;	/* DWORD 0 */
+	u8 ipc_online;	/* DWORD 0 */
+	u8 rsvd0[16];	/* DWORD 0 */
+} __packed;
+struct PCICFG_ONLINE1_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Host Timer Register. */
+struct BE_PCICFG_HOST_TIMER_INT_CTRL_CSR_AMAP {
+	u8 hosttimer[24];	/* DWORD 0 */
+	u8 hostintr;	/* DWORD 0 */
+	u8 rsvd0[7];	/* DWORD 0 */
+} __packed;
+struct PCICFG_HOST_TIMER_INT_CTRL_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Scratchpad Register (for software use). */
+struct BE_PCICFG_SCRATCHPAD_CSR_AMAP {
+	u8 scratchpad[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_SCRATCHPAD_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express Capabilities Register. */
+struct BE_PCICFG_PCIE_CAP_CSR_AMAP {
+	u8 capid[8];	/* DWORD 0 */
+	u8 nextcap[8];	/* DWORD 0 */
+	u8 capver[4];	/* DWORD 0 */
+	u8 devport[4];	/* DWORD 0 */
+	u8 rsvd0[6];	/* DWORD 0 */
+	u8 rsvd1[2];	/* DWORD 0 */
+} __packed;
+struct PCICFG_PCIE_CAP_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express Device Capabilities Register. */
+struct BE_PCICFG_PCIE_DEVCAP_CSR_AMAP {
+	u8 payload[3];	/* DWORD 0 */
+	u8 rsvd0[3];	/* DWORD 0 */
+	u8 lo_lat[3];	/* DWORD 0 */
+	u8 l1_lat[3];	/* DWORD 0 */
+	u8 rsvd1[3];	/* DWORD 0 */
+	u8 rsvd2[3];	/* DWORD 0 */
+	u8 pwr_value[8];	/* DWORD 0 */
+	u8 pwr_scale[2];	/* DWORD 0 */
+	u8 rsvd3[4];	/* DWORD 0 */
+} __packed;
+struct PCICFG_PCIE_DEVCAP_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express Device Control/Status Registers. */
+struct BE_PCICFG_PCIE_CONTROL_STATUS_CSR_AMAP {
+	u8 CorrErrReportEn;	/* DWORD 0 */
+	u8 NonFatalErrReportEn;	/* DWORD 0 */
+	u8 FatalErrReportEn;	/* DWORD 0 */
+	u8 UnsuppReqReportEn;	/* DWORD 0 */
+	u8 EnableRelaxOrder;	/* DWORD 0 */
+	u8 Max_Payload_Size[3];	/* DWORD 0 */
+	u8 ExtendTagFieldEnable;	/* DWORD 0 */
+	u8 PhantomFnEnable;	/* DWORD 0 */
+	u8 AuxPwrPMEnable;	/* DWORD 0 */
+	u8 EnableNoSnoop;	/* DWORD 0 */
+	u8 Max_Read_Req_Size[3];	/* DWORD 0 */
+	u8 rsvd0;		/* DWORD 0 */
+	u8 CorrErrDetect;	/* DWORD 0 */
+	u8 NonFatalErrDetect;	/* DWORD 0 */
+	u8 FatalErrDetect;	/* DWORD 0 */
+	u8 UnsuppReqDetect;	/* DWORD 0 */
+	u8 AuxPwrDetect;	/* DWORD 0 */
+	u8 TransPending;	/* DWORD 0 */
+	u8 rsvd1[10];	/* DWORD 0 */
+} __packed;
+struct PCICFG_PCIE_CONTROL_STATUS_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express Link Capabilities Register. */
+struct BE_PCICFG_PCIE_LINK_CAP_CSR_AMAP {
+	u8 MaxLinkSpeed[4];	/* DWORD 0 */
+	u8 MaxLinkWidth[6];	/* DWORD 0 */
+	u8 ASPMSupport[2];	/* DWORD 0 */
+	u8 L0sExitLat[3];	/* DWORD 0 */
+	u8 L1ExitLat[3];	/* DWORD 0 */
+	u8 rsvd0[6];	/* DWORD 0 */
+	u8 PortNum[8];	/* DWORD 0 */
+} __packed;
+struct PCICFG_PCIE_LINK_CAP_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express Link Status Register. */
+struct BE_PCICFG_PCIE_LINK_STATUS_CSR_AMAP {
+	u8 ASPMCtl[2];	/* DWORD 0 */
+	u8 rsvd0;		/* DWORD 0 */
+	u8 ReadCmplBndry;	/* DWORD 0 */
+	u8 LinkDisable;	/* DWORD 0 */
+	u8 RetrainLink;	/* DWORD 0 */
+	u8 CommonClkConfig;	/* DWORD 0 */
+	u8 ExtendSync;	/* DWORD 0 */
+	u8 rsvd1[8];	/* DWORD 0 */
+	u8 LinkSpeed[4];	/* DWORD 0 */
+	u8 NegLinkWidth[6];	/* DWORD 0 */
+	u8 LinkTrainErr;	/* DWORD 0 */
+	u8 LinkTrain;	/* DWORD 0 */
+	u8 SlotClkConfig;	/* DWORD 0 */
+	u8 rsvd2[3];	/* DWORD 0 */
+} __packed;
+struct PCICFG_PCIE_LINK_STATUS_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express MSI Configuration Register. */
+struct BE_PCICFG_MSI_CSR_AMAP {
+	u8 capid[8];	/* DWORD 0 */
+	u8 nextptr[8];	/* DWORD 0 */
+	u8 tablesize[11];	/* DWORD 0 */
+	u8 rsvd0[3];	/* DWORD 0 */
+	u8 funcmask;	/* DWORD 0 */
+	u8 en;		/* DWORD 0 */
+} __packed;
+struct PCICFG_MSI_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* MSI-X Table Offset Register. */
+struct BE_PCICFG_MSIX_TABLE_CSR_AMAP {
+	u8 tablebir[3];	/* DWORD 0 */
+	u8 offset[29];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MSIX_TABLE_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* MSI-X PBA Offset Register. */
+struct BE_PCICFG_MSIX_PBA_CSR_AMAP {
+	u8 pbabir[3];	/* DWORD 0 */
+	u8 offset[29];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MSIX_PBA_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express MSI-X Message Vector Control Register. */
+struct BE_PCICFG_MSIX_VECTOR_CONTROL_CSR_AMAP {
+	u8 vector_control;	/* DWORD 0 */
+	u8 rsvd0[31];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MSIX_VECTOR_CONTROL_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express MSI-X Message Data Register. */
+struct BE_PCICFG_MSIX_MSG_DATA_CSR_AMAP {
+	u8 data[16];	/* DWORD 0 */
+	u8 rsvd0[16];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MSIX_MSG_DATA_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express MSI-X Message Address Register - High Part. */
+struct BE_PCICFG_MSIX_MSG_ADDR_HI_CSR_AMAP {
+	u8 addr[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MSIX_MSG_ADDR_HI_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express MSI-X Message Address Register - Low Part. */
+struct BE_PCICFG_MSIX_MSG_ADDR_LO_CSR_AMAP {
+	u8 rsvd0[2];	/* DWORD 0 */
+	u8 addr[30];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MSIX_MSG_ADDR_LO_CSR_AMAP {
+	u32 dw[1];
+};
+
+struct BE_PCICFG_ANON_18_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_ANON_18_RSVD_AMAP {
+	u32 dw[1];
+};
+
+struct BE_PCICFG_ANON_19_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_ANON_19_RSVD_AMAP {
+	u32 dw[1];
+};
+
+struct BE_PCICFG_ANON_20_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[25][32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_20_RSVD_AMAP {
+	u32 dw[26];
+};
+
+struct BE_PCICFG_ANON_21_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[1919][32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_21_RSVD_AMAP {
+	u32 dw[1920];
+};
+
+struct BE_PCICFG_ANON_22_MESSAGE_AMAP {
+	struct BE_PCICFG_MSIX_VECTOR_CONTROL_CSR_AMAP vec_ctrl;
+	struct BE_PCICFG_MSIX_MSG_DATA_CSR_AMAP msg_data;
+	struct BE_PCICFG_MSIX_MSG_ADDR_HI_CSR_AMAP addr_hi;
+	struct BE_PCICFG_MSIX_MSG_ADDR_LO_CSR_AMAP addr_low;
+} __packed;
+struct PCICFG_ANON_22_MESSAGE_AMAP {
+	u32 dw[4];
+};
+
+struct BE_PCICFG_ANON_23_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[895][32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_23_RSVD_AMAP {
+	u32 dw[896];
+};
+
+/* These PCI Configuration Space registers are for the Storage  Function of
+ * BladeEngine (Function 0). In the memory map of the registers below their
+ * table,
+ */
+struct BE_PCICFG0_CSRMAP_AMAP {
+	struct BE_PCICFG_ID_CSR_AMAP id;
+	u8 rsvd0[32];	/* DWORD 1 */
+	u8 rsvd1[32];	/* DWORD 2 */
+	u8 rsvd2[32];	/* DWORD 3 */
+	struct BE_PCICFG_IOBAR_CSR_AMAP iobar;
+	struct BE_PCICFG_MEMBAR0_CSR_AMAP membar0;
+	struct BE_PCICFG_MEMBAR1_LO_CSR_AMAP membar1_lo;
+	struct BE_PCICFG_MEMBAR1_HI_CSR_AMAP membar1_hi;
+	struct BE_PCICFG_MEMBAR2_LO_CSR_AMAP membar2_lo;
+	struct BE_PCICFG_MEMBAR2_HI_CSR_AMAP membar2_hi;
+	u8 rsvd3[32];	/* DWORD 10 */
+	struct BE_PCICFG_SUBSYSTEM_ID_F0_CSR_AMAP subsystem_id;
+	u8 rsvd4[32];	/* DWORD 12 */
+	u8 rsvd5[32];	/* DWORD 13 */
+	u8 rsvd6[32];	/* DWORD 14 */
+	u8 rsvd7[32];	/* DWORD 15 */
+	struct BE_PCICFG_SEMAPHORE_CSR_AMAP semaphore[4];
+	struct BE_PCICFG_SOFT_RESET_CSR_AMAP soft_reset;
+	u8 rsvd8[32];	/* DWORD 21 */
+	struct BE_PCICFG_SCRATCHPAD_CSR_AMAP scratchpad;
+	u8 rsvd9[32];	/* DWORD 23 */
+	u8 rsvd10[32];	/* DWORD 24 */
+	u8 rsvd11[32];	/* DWORD 25 */
+	u8 rsvd12[32];	/* DWORD 26 */
+	u8 rsvd13[32];	/* DWORD 27 */
+	u8 rsvd14[2][32];	/* DWORD 28 */
+	u8 rsvd15[32];	/* DWORD 30 */
+	u8 rsvd16[32];	/* DWORD 31 */
+	u8 rsvd17[8][32];	/* DWORD 32 */
+	struct BE_PCICFG_UE_STATUS_LOW_CSR_AMAP ue_status_low;
+	struct BE_PCICFG_UE_STATUS_HI_CSR_AMAP ue_status_hi;
+	struct BE_PCICFG_UE_STATUS_LOW_MASK_CSR_AMAP ue_status_low_mask;
+	struct BE_PCICFG_UE_STATUS_HI_MASK_CSR_AMAP ue_status_hi_mask;
+	struct BE_PCICFG_ONLINE0_CSR_AMAP online0;
+	struct BE_PCICFG_ONLINE1_CSR_AMAP online1;
+	u8 rsvd18[32];	/* DWORD 46 */
+	u8 rsvd19[32];	/* DWORD 47 */
+	u8 rsvd20[32];	/* DWORD 48 */
+	u8 rsvd21[32];	/* DWORD 49 */
+	struct BE_PCICFG_HOST_TIMER_INT_CTRL_CSR_AMAP host_timer_int_ctrl;
+	u8 rsvd22[32];	/* DWORD 51 */
+	struct BE_PCICFG_PCIE_CAP_CSR_AMAP pcie_cap;
+	struct BE_PCICFG_PCIE_DEVCAP_CSR_AMAP pcie_devcap;
+	struct BE_PCICFG_PCIE_CONTROL_STATUS_CSR_AMAP pcie_control_status;
+	struct BE_PCICFG_PCIE_LINK_CAP_CSR_AMAP pcie_link_cap;
+	struct BE_PCICFG_PCIE_LINK_STATUS_CSR_AMAP pcie_link_status;
+	struct BE_PCICFG_MSI_CSR_AMAP msi;
+	struct BE_PCICFG_MSIX_TABLE_CSR_AMAP msix_table_offset;
+	struct BE_PCICFG_MSIX_PBA_CSR_AMAP msix_pba_offset;
+	u8 rsvd23[32];	/* DWORD 60 */
+	u8 rsvd24[32];	/* DWORD 61 */
+	u8 rsvd25[32];	/* DWORD 62 */
+	u8 rsvd26[32];	/* DWORD 63 */
+	u8 rsvd27[32];	/* DWORD 64 */
+	u8 rsvd28[32];	/* DWORD 65 */
+	u8 rsvd29[32];	/* DWORD 66 */
+	u8 rsvd30[32];	/* DWORD 67 */
+	u8 rsvd31[32];	/* DWORD 68 */
+	u8 rsvd32[32];	/* DWORD 69 */
+	u8 rsvd33[32];	/* DWORD 70 */
+	u8 rsvd34[32];	/* DWORD 71 */
+	u8 rsvd35[32];	/* DWORD 72 */
+	u8 rsvd36[32];	/* DWORD 73 */
+	u8 rsvd37[32];	/* DWORD 74 */
+	u8 rsvd38[32];	/* DWORD 75 */
+	u8 rsvd39[32];	/* DWORD 76 */
+	u8 rsvd40[32];	/* DWORD 77 */
+	u8 rsvd41[32];	/* DWORD 78 */
+	u8 rsvd42[32];	/* DWORD 79 */
+	u8 rsvd43[32];	/* DWORD 80 */
+	u8 rsvd44[32];	/* DWORD 81 */
+	u8 rsvd45[32];	/* DWORD 82 */
+	u8 rsvd46[32];	/* DWORD 83 */
+	u8 rsvd47[32];	/* DWORD 84 */
+	u8 rsvd48[32];	/* DWORD 85 */
+	u8 rsvd49[32];	/* DWORD 86 */
+	u8 rsvd50[32];	/* DWORD 87 */
+	u8 rsvd51[32];	/* DWORD 88 */
+	u8 rsvd52[32];	/* DWORD 89 */
+	u8 rsvd53[32];	/* DWORD 90 */
+	u8 rsvd54[32];	/* DWORD 91 */
+	u8 rsvd55[32];	/* DWORD 92 */
+	u8 rsvd56[832];	/* DWORD 93 */
+	u8 rsvd57[32];	/* DWORD 119 */
+	u8 rsvd58[32];	/* DWORD 120 */
+	u8 rsvd59[32];	/* DWORD 121 */
+	u8 rsvd60[32];	/* DWORD 122 */
+	u8 rsvd61[32];	/* DWORD 123 */
+	u8 rsvd62[32];	/* DWORD 124 */
+	u8 rsvd63[32];	/* DWORD 125 */
+	u8 rsvd64[32];	/* DWORD 126 */
+	u8 rsvd65[32];	/* DWORD 127 */
+	u8 rsvd66[61440];	/* DWORD 128 */
+	struct BE_PCICFG_ANON_22_MESSAGE_AMAP message[32];
+	u8 rsvd67[28672];	/* DWORD 2176 */
+	u8 rsvd68[32];	/* DWORD 3072 */
+	u8 rsvd69[1023][32];	/* DWORD 3073 */
+} __packed;
+struct PCICFG0_CSRMAP_AMAP {
+	u32 dw[4096];
+};
+
+struct BE_PCICFG_ANON_24_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_ANON_24_RSVD_AMAP {
+	u32 dw[1];
+};
+
+struct BE_PCICFG_ANON_25_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_ANON_25_RSVD_AMAP {
+	u32 dw[1];
+};
+
+struct BE_PCICFG_ANON_26_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_ANON_26_RSVD_AMAP {
+	u32 dw[1];
+};
+
+struct BE_PCICFG_ANON_27_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_27_RSVD_AMAP {
+	u32 dw[2];
+};
+
+struct BE_PCICFG_ANON_28_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[3][32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_28_RSVD_AMAP {
+	u32 dw[4];
+};
+
+struct BE_PCICFG_ANON_29_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[36][32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_29_RSVD_AMAP {
+	u32 dw[37];
+};
+
+struct BE_PCICFG_ANON_30_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[1930][32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_30_RSVD_AMAP {
+	u32 dw[1931];
+};
+
+struct BE_PCICFG_ANON_31_MESSAGE_AMAP {
+	struct BE_PCICFG_MSIX_VECTOR_CONTROL_CSR_AMAP vec_ctrl;
+	struct BE_PCICFG_MSIX_MSG_DATA_CSR_AMAP msg_data;
+	struct BE_PCICFG_MSIX_MSG_ADDR_HI_CSR_AMAP addr_hi;
+	struct BE_PCICFG_MSIX_MSG_ADDR_LO_CSR_AMAP addr_low;
+} __packed;
+struct PCICFG_ANON_31_MESSAGE_AMAP {
+	u32 dw[4];
+};
+
+struct BE_PCICFG_ANON_32_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[895][32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_32_RSVD_AMAP {
+	u32 dw[896];
+};
+
+/* This PCI configuration space register map is for the  Networking Function of
+ * BladeEngine (Function 1).
+ */
+struct BE_PCICFG1_CSRMAP_AMAP {
+	struct BE_PCICFG_ID_CSR_AMAP id;
+	u8 rsvd0[32];	/* DWORD 1 */
+	u8 rsvd1[32];	/* DWORD 2 */
+	u8 rsvd2[32];	/* DWORD 3 */
+	struct BE_PCICFG_IOBAR_CSR_AMAP iobar;
+	struct BE_PCICFG_MEMBAR0_CSR_AMAP membar0;
+	struct BE_PCICFG_MEMBAR1_LO_CSR_AMAP membar1_lo;
+	struct BE_PCICFG_MEMBAR1_HI_CSR_AMAP membar1_hi;
+	struct BE_PCICFG_MEMBAR2_LO_CSR_AMAP membar2_lo;
+	struct BE_PCICFG_MEMBAR2_HI_CSR_AMAP membar2_hi;
+	u8 rsvd3[32];	/* DWORD 10 */
+	struct BE_PCICFG_SUBSYSTEM_ID_F1_CSR_AMAP subsystem_id;
+	u8 rsvd4[32];	/* DWORD 12 */
+	u8 rsvd5[32];	/* DWORD 13 */
+	u8 rsvd6[32];	/* DWORD 14 */
+	u8 rsvd7[32];	/* DWORD 15 */
+	struct BE_PCICFG_SEMAPHORE_CSR_AMAP semaphore[4];
+	struct BE_PCICFG_SOFT_RESET_CSR_AMAP soft_reset;
+	u8 rsvd8[32];	/* DWORD 21 */
+	struct BE_PCICFG_SCRATCHPAD_CSR_AMAP scratchpad;
+	u8 rsvd9[32];	/* DWORD 23 */
+	u8 rsvd10[32];	/* DWORD 24 */
+	u8 rsvd11[32];	/* DWORD 25 */
+	u8 rsvd12[32];	/* DWORD 26 */
+	u8 rsvd13[32];	/* DWORD 27 */
+	u8 rsvd14[2][32];	/* DWORD 28 */
+	u8 rsvd15[32];	/* DWORD 30 */
+	u8 rsvd16[32];	/* DWORD 31 */
+	u8 rsvd17[8][32];	/* DWORD 32 */
+	struct BE_PCICFG_UE_STATUS_LOW_CSR_AMAP ue_status_low;
+	struct BE_PCICFG_UE_STATUS_HI_CSR_AMAP ue_status_hi;
+	struct BE_PCICFG_UE_STATUS_LOW_MASK_CSR_AMAP ue_status_low_mask;
+	struct BE_PCICFG_UE_STATUS_HI_MASK_CSR_AMAP ue_status_hi_mask;
+	struct BE_PCICFG_ONLINE0_CSR_AMAP online0;
+	struct BE_PCICFG_ONLINE1_CSR_AMAP online1;
+	u8 rsvd18[32];	/* DWORD 46 */
+	u8 rsvd19[32];	/* DWORD 47 */
+	u8 rsvd20[32];	/* DWORD 48 */
+	u8 rsvd21[32];	/* DWORD 49 */
+	struct BE_PCICFG_HOST_TIMER_INT_CTRL_CSR_AMAP host_timer_int_ctrl;
+	u8 rsvd22[32];	/* DWORD 51 */
+	struct BE_PCICFG_PCIE_CAP_CSR_AMAP pcie_cap;
+	struct BE_PCICFG_PCIE_DEVCAP_CSR_AMAP pcie_devcap;
+	struct BE_PCICFG_PCIE_CONTROL_STATUS_CSR_AMAP pcie_control_status;
+	struct BE_PCICFG_PCIE_LINK_CAP_CSR_AMAP pcie_link_cap;
+	struct BE_PCICFG_PCIE_LINK_STATUS_CSR_AMAP pcie_link_status;
+	struct BE_PCICFG_MSI_CSR_AMAP msi;
+	struct BE_PCICFG_MSIX_TABLE_CSR_AMAP msix_table_offset;
+	struct BE_PCICFG_MSIX_PBA_CSR_AMAP msix_pba_offset;
+	u8 rsvd23[64];	/* DWORD 60 */
+	u8 rsvd24[32];	/* DWORD 62 */
+	u8 rsvd25[32];	/* DWORD 63 */
+	u8 rsvd26[32];	/* DWORD 64 */
+	u8 rsvd27[32];	/* DWORD 65 */
+	u8 rsvd28[32];	/* DWORD 66 */
+	u8 rsvd29[32];	/* DWORD 67 */
+	u8 rsvd30[32];	/* DWORD 68 */
+	u8 rsvd31[32];	/* DWORD 69 */
+	u8 rsvd32[32];	/* DWORD 70 */
+	u8 rsvd33[32];	/* DWORD 71 */
+	u8 rsvd34[32];	/* DWORD 72 */
+	u8 rsvd35[32];	/* DWORD 73 */
+	u8 rsvd36[32];	/* DWORD 74 */
+	u8 rsvd37[128];	/* DWORD 75 */
+	u8 rsvd38[32];	/* DWORD 79 */
+	u8 rsvd39[1184];	/* DWORD 80 */
+	u8 rsvd40[61792];	/* DWORD 117 */
+	struct BE_PCICFG_ANON_31_MESSAGE_AMAP message[32];
+	u8 rsvd41[28672];	/* DWORD 2176 */
+	u8 rsvd42[32];	/* DWORD 3072 */
+	u8 rsvd43[1023][32];	/* DWORD 3073 */
+} __packed;
+struct PCICFG1_CSRMAP_AMAP {
+	u32 dw[4096];
+};
+
+#endif /* __pcicfg_amap_h__ */
diff --git a/drivers/staging/benet/post_codes.h b/drivers/staging/benet/post_codes.h
new file mode 100644
index 000000000000..6d1621f5f5fb
--- /dev/null
+++ b/drivers/staging/benet/post_codes.h
@@ -0,0 +1,111 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __post_codes_amap_h__
+#define __post_codes_amap_h__
+
+/* --- MGMT_HBA_POST_STAGE_ENUM --- */
+#define POST_STAGE_POWER_ON_RESET   (0)	/* State after a cold or warm boot. */
+#define POST_STAGE_AWAITING_HOST_RDY (1)	/* ARM boot code awaiting a
+						go-ahed from  the host. */
+#define POST_STAGE_HOST_RDY (2)	/* Host has given go-ahed to ARM. */
+#define POST_STAGE_BE_RESET (3)	/* Host wants to reset chip, this is a  chip
+						workaround  */
+#define POST_STAGE_SEEPROM_CS_START (256)	/* SEEPROM checksum
+						test start. */
+#define POST_STAGE_SEEPROM_CS_DONE  (257)	/* SEEPROM checksum test
+							done. */
+#define POST_STAGE_DDR_CONFIG_START (512)	/* DDR configuration start. */
+#define POST_STAGE_DDR_CONFIG_DONE  (513)	/* DDR configuration done. */
+#define POST_STAGE_DDR_CALIBRATE_START  (768)	/* DDR calibration start. */
+#define POST_STAGE_DDR_CALIBRATE_DONE   (769)	/* DDR calibration done. */
+#define POST_STAGE_DDR_TEST_START   (1024)	/* DDR memory test start. */
+#define POST_STAGE_DDR_TEST_DONE    (1025)	/* DDR memory test done. */
+#define POST_STAGE_REDBOOT_INIT_START   (1536)	/* Redboot starts execution. */
+#define POST_STAGE_REDBOOT_INIT_DONE (1537)	/* Redboot done execution. */
+#define POST_STAGE_FW_IMAGE_LOAD_START (1792)	/* Firmware image load to
+							DDR start. */
+#define POST_STAGE_FW_IMAGE_LOAD_DONE   (1793)	/* Firmware image load
+							to DDR done. */
+#define POST_STAGE_ARMFW_START          (2048)	/* ARMfw runtime code
+						starts execution. */
+#define POST_STAGE_DHCP_QUERY_START     (2304)	/* DHCP server query start. */
+#define POST_STAGE_DHCP_QUERY_DONE      (2305)	/* DHCP server query done. */
+#define POST_STAGE_BOOT_TARGET_DISCOVERY_START (2560)	/* Boot Target
+						Discovery Start. */
+#define POST_STAGE_BOOT_TARGET_DISCOVERY_DONE (2561)	/* Boot Target
+						Discovery Done. */
+#define POST_STAGE_RC_OPTION_SET        (2816)	/* Remote configuration
+						option is set in  SEEPROM  */
+#define POST_STAGE_SWITCH_LINK          (2817)	/* Wait for link up on switch */
+#define POST_STAGE_SEND_ICDS_MESSAGE    (2818)	/* Send the ICDS message
+						to switch */
+#define POST_STAGE_PERFROM_TFTP         (2819)	/* Download xml using TFTP */
+#define POST_STAGE_PARSE_XML            (2820)	/* Parse XML file */
+#define POST_STAGE_DOWNLOAD_IMAGE       (2821)	/* Download IMAGE from
+						TFTP server */
+#define POST_STAGE_FLASH_IMAGE          (2822)	/* Flash the IMAGE */
+#define POST_STAGE_RC_DONE              (2823)	/* Remote configuration
+						complete */
+#define POST_STAGE_REBOOT_SYSTEM        (2824)	/* Upgrade IMAGE done,
+						reboot required */
+#define POST_STAGE_MAC_ADDRESS          (3072)	/* MAC Address Check */
+#define POST_STAGE_ARMFW_READY          (49152)	/* ARMfw is done with POST
+						and ready. */
+#define POST_STAGE_ARMFW_UE             (61440)	/* ARMfw has asserted an
+						unrecoverable error. The
+						lower 3 hex digits of the
+						stage code identify the
+						unique error code.
+						*/
+
+/* This structure defines the format of the MPU semaphore
+ * register when used for POST.
+ */
+struct BE_MGMT_HBA_POST_STATUS_STRUCT_AMAP {
+	u8 stage[16];	/* DWORD 0 */
+	u8 rsvd0[10];	/* DWORD 0 */
+	u8 iscsi_driver_loaded;	/* DWORD 0 */
+	u8 option_rom_installed;	/* DWORD 0 */
+	u8 iscsi_ip_conflict;	/* DWORD 0 */
+	u8 iscsi_no_ip;	/* DWORD 0 */
+	u8 backup_fw;	/* DWORD 0 */
+	u8 error;		/* DWORD 0 */
+} __packed;
+struct MGMT_HBA_POST_STATUS_STRUCT_AMAP {
+	u32 dw[1];
+};
+
+/* --- MGMT_HBA_POST_DUMMY_BITS_ENUM --- */
+#define POST_BIT_ISCSI_LOADED           (26)
+#define POST_BIT_OPTROM_INST            (27)
+#define POST_BIT_BAD_IP_ADDR            (28)
+#define POST_BIT_NO_IP_ADDR             (29)
+#define POST_BIT_BACKUP_FW              (30)
+#define POST_BIT_ERROR                  (31)
+
+/* --- MGMT_HBA_POST_DUMMY_VALUES_ENUM --- */
+#define POST_ISCSI_DRIVER_LOADED        (67108864)
+#define POST_OPTROM_INSTALLED           (134217728)
+#define POST_ISCSI_IP_ADDRESS_CONFLICT  (268435456)
+#define POST_ISCSI_NO_IP_ADDRESS        (536870912)
+#define POST_BACKUP_FW_LOADED           (1073741824)
+#define POST_FATAL_ERROR                (2147483648)
+
+#endif /* __post_codes_amap_h__ */
diff --git a/drivers/staging/benet/regmap.h b/drivers/staging/benet/regmap.h
new file mode 100644
index 000000000000..e816ba210e83
--- /dev/null
+++ b/drivers/staging/benet/regmap.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * 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
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __regmap_amap_h__
+#define __regmap_amap_h__
+#include "pcicfg.h"
+#include "ep.h"
+#include "cev.h"
+#include "mpu.h"
+#include "doorbells.h"
+
+/*
+ * This is the control and status register map for BladeEngine, showing
+ * the relative size and offset of each sub-module. The CSR registers
+ * are identical for the network and storage PCI functions. The
+ * CSR map is shown below, followed by details of each block,
+ * in sub-sections.  The sub-sections begin with a description
+ * of CSRs that are instantiated in multiple blocks.
+ */
+struct BE_BLADE_ENGINE_CSRMAP_AMAP {
+	struct BE_MPU_CSRMAP_AMAP mpu;
+	u8 rsvd0[8192];	/* DWORD 256 */
+	u8 rsvd1[8192];	/* DWORD 512 */
+	struct BE_CEV_CSRMAP_AMAP cev;
+	u8 rsvd2[8192];	/* DWORD 1024 */
+	u8 rsvd3[8192];	/* DWORD 1280 */
+	u8 rsvd4[8192];	/* DWORD 1536 */
+	u8 rsvd5[8192];	/* DWORD 1792 */
+	u8 rsvd6[8192];	/* DWORD 2048 */
+	u8 rsvd7[8192];	/* DWORD 2304 */
+	u8 rsvd8[8192];	/* DWORD 2560 */
+	u8 rsvd9[8192];	/* DWORD 2816 */
+	u8 rsvd10[8192];	/* DWORD 3072 */
+	u8 rsvd11[8192];	/* DWORD 3328 */
+	u8 rsvd12[8192];	/* DWORD 3584 */
+	u8 rsvd13[8192];	/* DWORD 3840 */
+	u8 rsvd14[8192];	/* DWORD 4096 */
+	u8 rsvd15[8192];	/* DWORD 4352 */
+	u8 rsvd16[8192];	/* DWORD 4608 */
+	u8 rsvd17[8192];	/* DWORD 4864 */
+	u8 rsvd18[8192];	/* DWORD 5120 */
+	u8 rsvd19[8192];	/* DWORD 5376 */
+	u8 rsvd20[8192];	/* DWORD 5632 */
+	u8 rsvd21[8192];	/* DWORD 5888 */
+	u8 rsvd22[8192];	/* DWORD 6144 */
+	u8 rsvd23[17152][32];	/* DWORD 6400 */
+} __packed;
+struct BLADE_ENGINE_CSRMAP_AMAP {
+	u32 dw[23552];
+};
+
+#endif /* __regmap_amap_h__ */
diff --git a/drivers/staging/comedi/Kconfig b/drivers/staging/comedi/Kconfig
new file mode 100644
index 000000000000..b501bfb9c754
--- /dev/null
+++ b/drivers/staging/comedi/Kconfig
@@ -0,0 +1,27 @@
+config COMEDI
+	tristate "Data Acquision support (comedi)"
+	default N
+	---help---
+	  Enable support a wide range of data acquision devices
+	  for Linux.
+
+config COMEDI_RT
+	tristate "Comedi Real-time support"
+	depends on COMEDI && RT
+	default N
+	---help---
+	  Enable Real time support for the Comedi core.
+
+config COMEDI_PCI_DRIVERS
+	tristate "Comedi PCI drivers"
+	depends on COMEDI && PCI
+	default N
+	---help---
+	  Enable lots of comedi PCI drivers to be built
+
+config COMEDI_USB_DRIVERS
+	tristate "Comedi USB drivers"
+	depends on COMEDI && USB
+	default N
+	---help---
+	  Enable lots of comedi USB drivers to be built
diff --git a/drivers/staging/comedi/Makefile b/drivers/staging/comedi/Makefile
new file mode 100644
index 000000000000..afd1a19c1b87
--- /dev/null
+++ b/drivers/staging/comedi/Makefile
@@ -0,0 +1,17 @@
+obj-$(CONFIG_COMEDI) += comedi.o
+obj-$(CONFIG_COMEDI_RT) += comedi_rt.o
+
+obj-$(CONFIG_COMEDI)	+= kcomedilib/
+obj-$(CONFIG_COMEDI)	+= drivers/
+
+comedi-objs :=		\
+	comedi_fops.o	\
+	proc.o		\
+	range.o		\
+	drivers.o	\
+	comedi_compat32.o \
+	comedi_ksyms.o	\
+
+comedi_rt-objs :=	\
+	rt_pend_tq.o	\
+	rt.o
diff --git a/drivers/staging/comedi/TODO b/drivers/staging/comedi/TODO
new file mode 100644
index 000000000000..557812958464
--- /dev/null
+++ b/drivers/staging/comedi/TODO
@@ -0,0 +1,14 @@
+TODO:
+	- checkpatch.pl cleanups
+	- Lindent
+	- remove all wrappers
+	- remove typedefs
+	- audit userspace interface
+	- reserve major number
+	- cleanup the individual comedi drivers as well
+
+Please send patches to Greg Kroah-Hartman <greg@kroah.com> and
+copy:
+	Ian Abbott <abbotti@mev.co.uk>
+	Frank Mori Hess <fmhess@users.sourceforge.net>
+	David Schleef <ds@schleef.org>
diff --git a/drivers/staging/comedi/comedi.h b/drivers/staging/comedi/comedi.h
new file mode 100644
index 000000000000..36d2e1b01e78
--- /dev/null
+++ b/drivers/staging/comedi/comedi.h
@@ -0,0 +1,916 @@
+/*
+    include/comedi.h (installed as /usr/include/comedi.h)
+    header file for comedi
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1998-2001 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU Lesser General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _COMEDI_H
+#define _COMEDI_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define COMEDI_MAJORVERSION	0
+#define COMEDI_MINORVERSION	7
+#define COMEDI_MICROVERSION	76
+#define VERSION	"0.7.76"
+
+/* comedi's major device number */
+#define COMEDI_MAJOR 98
+
+/*
+   maximum number of minor devices.  This can be increased, although
+   kernel structures are currently statically allocated, thus you
+   don't want this to be much more than you actually use.
+ */
+#define COMEDI_NDEVICES 16
+
+/* number of config options in the config structure */
+#define COMEDI_NDEVCONFOPTS 32
+/*length of nth chunk of firmware data*/
+#define COMEDI_DEVCONF_AUX_DATA3_LENGTH		25
+#define COMEDI_DEVCONF_AUX_DATA2_LENGTH		26
+#define COMEDI_DEVCONF_AUX_DATA1_LENGTH		27
+#define COMEDI_DEVCONF_AUX_DATA0_LENGTH		28
+#define COMEDI_DEVCONF_AUX_DATA_HI		29	/* most significant 32 bits of pointer address (if needed) */
+#define COMEDI_DEVCONF_AUX_DATA_LO		30	/* least significant 32 bits of pointer address */
+#define COMEDI_DEVCONF_AUX_DATA_LENGTH		31	/* total data length */
+
+/* max length of device and driver names */
+#define COMEDI_NAMELEN 20
+
+	typedef unsigned int lsampl_t;
+	typedef unsigned short sampl_t;
+
+/* packs and unpacks a channel/range number */
+
+#define CR_PACK(chan, rng, aref)		((((aref)&0x3)<<24) | (((rng)&0xff)<<16) | (chan))
+#define CR_PACK_FLAGS(chan, range, aref, flags)	(CR_PACK(chan, range, aref) | ((flags) & CR_FLAGS_MASK))
+
+#define CR_CHAN(a)	((a)&0xffff)
+#define CR_RANGE(a)	(((a)>>16)&0xff)
+#define CR_AREF(a)	(((a)>>24)&0x03)
+
+#define CR_FLAGS_MASK	0xfc000000
+#define CR_ALT_FILTER	(1<<26)
+#define CR_DITHER	CR_ALT_FILTER
+#define CR_DEGLITCH	CR_ALT_FILTER
+#define CR_ALT_SOURCE	(1<<27)
+#define CR_EDGE		(1<<30)
+#define CR_INVERT	(1<<31)
+
+#define AREF_GROUND	0x00	/* analog ref = analog ground */
+#define AREF_COMMON	0x01	/* analog ref = analog common */
+#define AREF_DIFF	0x02	/* analog ref = differential */
+#define AREF_OTHER	0x03	/* analog ref = other (undefined) */
+
+/* counters -- these are arbitrary values */
+#define GPCT_RESET		0x0001
+#define GPCT_SET_SOURCE		0x0002
+#define GPCT_SET_GATE		0x0004
+#define GPCT_SET_DIRECTION	0x0008
+#define GPCT_SET_OPERATION	0x0010
+#define GPCT_ARM		0x0020
+#define GPCT_DISARM		0x0040
+#define GPCT_GET_INT_CLK_FRQ	0x0080
+
+#define GPCT_INT_CLOCK		0x0001
+#define GPCT_EXT_PIN		0x0002
+#define GPCT_NO_GATE		0x0004
+#define GPCT_UP			0x0008
+#define GPCT_DOWN		0x0010
+#define GPCT_HWUD		0x0020
+#define GPCT_SIMPLE_EVENT	0x0040
+#define GPCT_SINGLE_PERIOD	0x0080
+#define GPCT_SINGLE_PW		0x0100
+#define GPCT_CONT_PULSE_OUT	0x0200
+#define GPCT_SINGLE_PULSE_OUT	0x0400
+
+/* instructions */
+
+#define INSN_MASK_WRITE		0x8000000
+#define INSN_MASK_READ		0x4000000
+#define INSN_MASK_SPECIAL	0x2000000
+
+#define INSN_READ		(0 | INSN_MASK_READ)
+#define INSN_WRITE		(1 | INSN_MASK_WRITE)
+#define INSN_BITS		(2 | INSN_MASK_READ|INSN_MASK_WRITE)
+#define INSN_CONFIG		(3 | INSN_MASK_READ|INSN_MASK_WRITE)
+#define INSN_GTOD		(4 | INSN_MASK_READ|INSN_MASK_SPECIAL)
+#define INSN_WAIT		(5 | INSN_MASK_WRITE|INSN_MASK_SPECIAL)
+#define INSN_INTTRIG		(6 | INSN_MASK_WRITE|INSN_MASK_SPECIAL)
+
+/* trigger flags */
+/* These flags are used in comedi_trig structures */
+
+#define TRIG_BOGUS	0x0001	/* do the motions */
+#define TRIG_DITHER	0x0002	/* enable dithering */
+#define TRIG_DEGLITCH	0x0004	/* enable deglitching */
+/*#define TRIG_RT       0x0008 */	/* perform op in real time */
+#define TRIG_CONFIG	0x0010	/* perform configuration, not triggering */
+#define TRIG_WAKE_EOS	0x0020	/* wake up on end-of-scan events */
+/*#define TRIG_WRITE    0x0040*/	/* write to bidirectional devices */
+
+/* command flags */
+/* These flags are used in comedi_cmd structures */
+
+#define CMDF_PRIORITY		0x00000008	/* try to use a real-time interrupt while performing command */
+
+#define TRIG_RT		CMDF_PRIORITY	/* compatibility definition */
+
+#define CMDF_WRITE		0x00000040
+#define TRIG_WRITE	CMDF_WRITE	/* compatibility definition */
+
+#define CMDF_RAWDATA		0x00000080
+
+#define COMEDI_EV_START		0x00040000
+#define COMEDI_EV_SCAN_BEGIN	0x00080000
+#define COMEDI_EV_CONVERT	0x00100000
+#define COMEDI_EV_SCAN_END	0x00200000
+#define COMEDI_EV_STOP		0x00400000
+
+#define TRIG_ROUND_MASK		0x00030000
+#define TRIG_ROUND_NEAREST	0x00000000
+#define TRIG_ROUND_DOWN		0x00010000
+#define TRIG_ROUND_UP		0x00020000
+#define TRIG_ROUND_UP_NEXT	0x00030000
+
+/* trigger sources */
+
+#define TRIG_ANY	0xffffffff
+#define TRIG_INVALID	0x00000000
+
+#define TRIG_NONE	0x00000001	/* never trigger */
+#define TRIG_NOW	0x00000002	/* trigger now + N ns */
+#define TRIG_FOLLOW	0x00000004	/* trigger on next lower level trig */
+#define TRIG_TIME	0x00000008	/* trigger at time N ns */
+#define TRIG_TIMER	0x00000010	/* trigger at rate N ns */
+#define TRIG_COUNT	0x00000020	/* trigger when count reaches N */
+#define TRIG_EXT	0x00000040	/* trigger on external signal N */
+#define TRIG_INT	0x00000080	/* trigger on comedi-internal signal N */
+#define TRIG_OTHER	0x00000100	/* driver defined */
+
+/* subdevice flags */
+
+#define SDF_BUSY	0x0001	/* device is busy */
+#define SDF_BUSY_OWNER	0x0002	/* device is busy with your job */
+#define SDF_LOCKED	0x0004	/* subdevice is locked */
+#define SDF_LOCK_OWNER	0x0008	/* you own lock */
+#define SDF_MAXDATA	0x0010	/* maxdata depends on channel */
+#define SDF_FLAGS	0x0020	/* flags depend on channel */
+#define SDF_RANGETYPE	0x0040	/* range type depends on channel */
+#define SDF_MODE0	0x0080	/* can do mode 0 */
+#define SDF_MODE1	0x0100	/* can do mode 1 */
+#define SDF_MODE2	0x0200	/* can do mode 2 */
+#define SDF_MODE3	0x0400	/* can do mode 3 */
+#define SDF_MODE4	0x0800	/* can do mode 4 */
+#define SDF_CMD		0x1000	/* can do commands (deprecated) */
+#define SDF_SOFT_CALIBRATED	0x2000	/* subdevice uses software calibration */
+#define SDF_CMD_WRITE		0x4000	/* can do output commands */
+#define SDF_CMD_READ		0x8000	/* can do input commands */
+
+#define SDF_READABLE	0x00010000	/* subdevice can be read (e.g. analog input) */
+#define SDF_WRITABLE	0x00020000	/* subdevice can be written (e.g. analog output) */
+#define SDF_WRITEABLE	SDF_WRITABLE	/* spelling error in API */
+#define SDF_INTERNAL	0x00040000	/* subdevice does not have externally visible lines */
+#define SDF_RT		0x00080000	/* DEPRECATED: subdevice is RT capable */
+#define SDF_GROUND	0x00100000	/* can do aref=ground */
+#define SDF_COMMON	0x00200000	/* can do aref=common */
+#define SDF_DIFF	0x00400000	/* can do aref=diff */
+#define SDF_OTHER	0x00800000	/* can do aref=other */
+#define SDF_DITHER	0x01000000	/* can do dithering */
+#define SDF_DEGLITCH	0x02000000	/* can do deglitching */
+#define SDF_MMAP	0x04000000	/* can do mmap() */
+#define SDF_RUNNING	0x08000000	/* subdevice is acquiring data */
+#define SDF_LSAMPL	0x10000000	/* subdevice uses 32-bit samples */
+#define SDF_PACKED	0x20000000	/* subdevice can do packed DIO */
+/* re recyle these flags for PWM */
+#define SDF_PWM_COUNTER SDF_MODE0       /* PWM can automatically switch off */
+#define SDF_PWM_HBRIDGE SDF_MODE1       /* PWM is signed (H-bridge) */
+
+
+
+/* subdevice types */
+
+enum comedi_subdevice_type {
+	COMEDI_SUBD_UNUSED,	/* unused by driver */
+	COMEDI_SUBD_AI,	/* analog input */
+	COMEDI_SUBD_AO,	/* analog output */
+	COMEDI_SUBD_DI,	/* digital input */
+	COMEDI_SUBD_DO,	/* digital output */
+	COMEDI_SUBD_DIO,	/* digital input/output */
+	COMEDI_SUBD_COUNTER,	/* counter */
+	COMEDI_SUBD_TIMER,	/* timer */
+	COMEDI_SUBD_MEMORY,	/* memory, EEPROM, DPRAM */
+	COMEDI_SUBD_CALIB,	/* calibration DACs */
+	COMEDI_SUBD_PROC,	/* processor, DSP */
+	COMEDI_SUBD_SERIAL,	/* serial IO */
+	COMEDI_SUBD_PWM         /* PWM */
+};
+
+/* configuration instructions */
+
+enum configuration_ids {
+	INSN_CONFIG_DIO_INPUT = 0,
+	INSN_CONFIG_DIO_OUTPUT = 1,
+	INSN_CONFIG_DIO_OPENDRAIN = 2,
+	INSN_CONFIG_ANALOG_TRIG = 16,
+/*	INSN_CONFIG_WAVEFORM = 17, */
+/*	INSN_CONFIG_TRIG = 18, */
+/*	INSN_CONFIG_COUNTER = 19, */
+	INSN_CONFIG_ALT_SOURCE = 20,
+	INSN_CONFIG_DIGITAL_TRIG = 21,
+	INSN_CONFIG_BLOCK_SIZE = 22,
+	INSN_CONFIG_TIMER_1 = 23,
+	INSN_CONFIG_FILTER = 24,
+	INSN_CONFIG_CHANGE_NOTIFY = 25,
+
+	 /*ALPHA*/ INSN_CONFIG_SERIAL_CLOCK = 26,
+	INSN_CONFIG_BIDIRECTIONAL_DATA = 27,
+	INSN_CONFIG_DIO_QUERY = 28,
+	INSN_CONFIG_PWM_OUTPUT = 29,
+	INSN_CONFIG_GET_PWM_OUTPUT = 30,
+	INSN_CONFIG_ARM = 31,
+	INSN_CONFIG_DISARM = 32,
+	INSN_CONFIG_GET_COUNTER_STATUS = 33,
+	INSN_CONFIG_RESET = 34,
+	INSN_CONFIG_GPCT_SINGLE_PULSE_GENERATOR = 1001,	/* Use CTR as single pulsegenerator */
+	INSN_CONFIG_GPCT_PULSE_TRAIN_GENERATOR = 1002,	/* Use CTR as pulsetraingenerator */
+	INSN_CONFIG_GPCT_QUADRATURE_ENCODER = 1003,	/* Use the counter as encoder */
+	INSN_CONFIG_SET_GATE_SRC = 2001,	/* Set gate source */
+	INSN_CONFIG_GET_GATE_SRC = 2002,	/* Get gate source */
+	INSN_CONFIG_SET_CLOCK_SRC = 2003,	/* Set master clock source */
+	INSN_CONFIG_GET_CLOCK_SRC = 2004,	/* Get master clock source */
+	INSN_CONFIG_SET_OTHER_SRC = 2005,	/* Set other source */
+/*	INSN_CONFIG_GET_OTHER_SRC = 2006,*/	/* Get other source */
+	INSN_CONFIG_GET_HARDWARE_BUFFER_SIZE,	/* Get size in bytes of
+						  subdevice's on-board fifos
+						  used during streaming
+						  input/output */
+	INSN_CONFIG_SET_COUNTER_MODE = 4097,
+	INSN_CONFIG_8254_SET_MODE = INSN_CONFIG_SET_COUNTER_MODE,	/* deprecated */
+	INSN_CONFIG_8254_READ_STATUS = 4098,
+	INSN_CONFIG_SET_ROUTING = 4099,
+	INSN_CONFIG_GET_ROUTING = 4109,
+/* PWM */
+	INSN_CONFIG_PWM_SET_PERIOD = 5000,   /* sets frequency */
+	INSN_CONFIG_PWM_GET_PERIOD = 5001,   /* gets frequency */
+	INSN_CONFIG_GET_PWM_STATUS = 5002,          /* is it running? */
+	INSN_CONFIG_PWM_SET_H_BRIDGE = 5003, /* sets H bridge: duty cycle and sign bit for a relay  at the same time*/
+	INSN_CONFIG_PWM_GET_H_BRIDGE = 5004  /* gets H bridge data: duty cycle and the sign bit */
+};
+
+enum comedi_io_direction {
+	COMEDI_INPUT = 0,
+	COMEDI_OUTPUT = 1,
+	COMEDI_OPENDRAIN = 2
+};
+
+enum comedi_support_level {
+	COMEDI_UNKNOWN_SUPPORT = 0,
+	COMEDI_SUPPORTED,
+	COMEDI_UNSUPPORTED
+};
+
+/* ioctls */
+
+#define CIO 'd'
+#define COMEDI_DEVCONFIG _IOW(CIO, 0, comedi_devconfig)
+#define COMEDI_DEVINFO _IOR(CIO, 1, comedi_devinfo)
+#define COMEDI_SUBDINFO _IOR(CIO, 2, comedi_subdinfo)
+#define COMEDI_CHANINFO _IOR(CIO, 3, comedi_chaninfo)
+#define COMEDI_TRIG _IOWR(CIO, 4, comedi_trig)
+#define COMEDI_LOCK _IO(CIO, 5)
+#define COMEDI_UNLOCK _IO(CIO, 6)
+#define COMEDI_CANCEL _IO(CIO, 7)
+#define COMEDI_RANGEINFO _IOR(CIO, 8, comedi_rangeinfo)
+#define COMEDI_CMD _IOR(CIO, 9, comedi_cmd)
+#define COMEDI_CMDTEST _IOR(CIO, 10, comedi_cmd)
+#define COMEDI_INSNLIST _IOR(CIO, 11, comedi_insnlist)
+#define COMEDI_INSN _IOR(CIO, 12, comedi_insn)
+#define COMEDI_BUFCONFIG _IOR(CIO, 13, comedi_bufconfig)
+#define COMEDI_BUFINFO _IOWR(CIO, 14, comedi_bufinfo)
+#define COMEDI_POLL _IO(CIO, 15)
+
+/* structures */
+
+typedef struct comedi_trig_struct comedi_trig;
+typedef struct comedi_cmd_struct comedi_cmd;
+typedef struct comedi_insn_struct comedi_insn;
+typedef struct comedi_insnlist_struct comedi_insnlist;
+typedef struct comedi_chaninfo_struct comedi_chaninfo;
+typedef struct comedi_subdinfo_struct comedi_subdinfo;
+typedef struct comedi_devinfo_struct comedi_devinfo;
+typedef struct comedi_devconfig_struct comedi_devconfig;
+typedef struct comedi_rangeinfo_struct comedi_rangeinfo;
+typedef struct comedi_krange_struct comedi_krange;
+typedef struct comedi_bufconfig_struct comedi_bufconfig;
+typedef struct comedi_bufinfo_struct comedi_bufinfo;
+
+struct comedi_trig_struct {
+	unsigned int subdev;	/* subdevice */
+	unsigned int mode;	/* mode */
+	unsigned int flags;
+	unsigned int n_chan;	/* number of channels */
+	unsigned int *chanlist;	/* channel/range list */
+	sampl_t *data;	/* data list, size depends on subd flags */
+	unsigned int n;	/* number of scans */
+	unsigned int trigsrc;
+	unsigned int trigvar;
+	unsigned int trigvar1;
+	unsigned int data_len;
+	unsigned int unused[3];
+};
+
+struct comedi_insn_struct {
+	unsigned int insn;
+	unsigned int n;
+	lsampl_t *data;
+	unsigned int subdev;
+	unsigned int chanspec;
+	unsigned int unused[3];
+};
+
+struct comedi_insnlist_struct {
+	unsigned int n_insns;
+	comedi_insn *insns;
+};
+
+struct comedi_cmd_struct {
+	unsigned int subdev;
+	unsigned int flags;
+
+	unsigned int start_src;
+	unsigned int start_arg;
+
+	unsigned int scan_begin_src;
+	unsigned int scan_begin_arg;
+
+	unsigned int convert_src;
+	unsigned int convert_arg;
+
+	unsigned int scan_end_src;
+	unsigned int scan_end_arg;
+
+	unsigned int stop_src;
+	unsigned int stop_arg;
+
+	unsigned int *chanlist;	/* channel/range list */
+	unsigned int chanlist_len;
+
+	sampl_t *data;	/* data list, size depends on subd flags */
+	unsigned int data_len;
+};
+
+struct comedi_chaninfo_struct {
+	unsigned int subdev;
+	lsampl_t *maxdata_list;
+	unsigned int *flaglist;
+	unsigned int *rangelist;
+	unsigned int unused[4];
+};
+
+struct comedi_rangeinfo_struct {
+	unsigned int range_type;
+	void *range_ptr;
+};
+
+struct comedi_krange_struct {
+	int min;	/* fixed point, multiply by 1e-6 */
+	int max;	/* fixed point, multiply by 1e-6 */
+	unsigned int flags;
+};
+
+
+struct comedi_subdinfo_struct {
+	unsigned int type;
+	unsigned int n_chan;
+	unsigned int subd_flags;
+	unsigned int timer_type;
+	unsigned int len_chanlist;
+	lsampl_t maxdata;
+	unsigned int flags;	/* channel flags */
+	unsigned int range_type;	/* lookup in kernel */
+	unsigned int settling_time_0;
+	unsigned insn_bits_support;	/* see support_level enum for values*/
+	unsigned int unused[8];
+};
+
+struct comedi_devinfo_struct {
+	unsigned int version_code;
+	unsigned int n_subdevs;
+	char driver_name[COMEDI_NAMELEN];
+	char board_name[COMEDI_NAMELEN];
+	int read_subdevice;
+	int write_subdevice;
+	int unused[30];
+};
+
+struct comedi_devconfig_struct {
+	char board_name[COMEDI_NAMELEN];
+	int options[COMEDI_NDEVCONFOPTS];
+};
+
+struct comedi_bufconfig_struct {
+	unsigned int subdevice;
+	unsigned int flags;
+
+	unsigned int maximum_size;
+	unsigned int size;
+
+	unsigned int unused[4];
+};
+
+struct comedi_bufinfo_struct {
+	unsigned int subdevice;
+	unsigned int bytes_read;
+
+	unsigned int buf_write_ptr;
+	unsigned int buf_read_ptr;
+	unsigned int buf_write_count;
+	unsigned int buf_read_count;
+
+	unsigned int bytes_written;
+
+	unsigned int unused[4];
+};
+
+/* range stuff */
+
+#define __RANGE(a, b)	((((a)&0xffff)<<16)|((b)&0xffff))
+
+#define RANGE_OFFSET(a)		(((a)>>16)&0xffff)
+#define RANGE_LENGTH(b)		((b)&0xffff)
+
+#define RF_UNIT(flags)		((flags)&0xff)
+#define RF_EXTERNAL		(1<<8)
+
+#define UNIT_volt		0
+#define UNIT_mA			1
+#define UNIT_none		2
+
+#define COMEDI_MIN_SPEED	((unsigned int)0xffffffff)
+
+/* callback stuff */
+/* only relevant to kernel modules. */
+
+#define COMEDI_CB_EOS		1	/* end of scan */
+#define COMEDI_CB_EOA		2	/* end of acquisition */
+#define COMEDI_CB_BLOCK		4	/* DEPRECATED: convenient block size */
+#define COMEDI_CB_EOBUF		8	/* DEPRECATED: end of buffer */
+#define COMEDI_CB_ERROR		16	/* card error during acquisition */
+#define COMEDI_CB_OVERFLOW	32	/* buffer overflow/underflow */
+
+/**********************************************************/
+/* everything after this line is ALPHA */
+/**********************************************************/
+
+/*
+  8254 specific configuration.
+
+  It supports two config commands:
+
+  0 ID: INSN_CONFIG_SET_COUNTER_MODE
+  1 8254 Mode
+    I8254_MODE0, I8254_MODE1, ..., I8254_MODE5
+    OR'ed with:
+    I8254_BCD, I8254_BINARY
+
+  0 ID: INSN_CONFIG_8254_READ_STATUS
+  1 <-- Status byte returned here.
+    B7 = Output
+    B6 = NULL Count
+    B5 - B0 Current mode.
+
+*/
+
+enum i8254_mode {
+	I8254_MODE0 = (0 << 1),	/* Interrupt on terminal count */
+	I8254_MODE1 = (1 << 1),	/* Hardware retriggerable one-shot */
+	I8254_MODE2 = (2 << 1),	/* Rate generator */
+	I8254_MODE3 = (3 << 1),	/* Square wave mode */
+	I8254_MODE4 = (4 << 1),	/* Software triggered strobe */
+	I8254_MODE5 = (5 << 1),	/* Hardware triggered strobe (retriggerable) */
+	I8254_BCD = 1,	/* use binary-coded decimal instead of binary (pretty useless) */
+	I8254_BINARY = 0
+};
+
+static inline unsigned NI_USUAL_PFI_SELECT(unsigned pfi_channel)
+{
+	if (pfi_channel < 10)
+		return 0x1 + pfi_channel;
+	else
+		return 0xb + pfi_channel;
+}
+static inline unsigned NI_USUAL_RTSI_SELECT(unsigned rtsi_channel)
+{
+	if (rtsi_channel < 7)
+		return 0xb + rtsi_channel;
+	else
+		return 0x1b;
+}
+/* mode bits for NI general-purpose counters, set with
+ * INSN_CONFIG_SET_COUNTER_MODE */
+#define NI_GPCT_COUNTING_MODE_SHIFT 16
+#define NI_GPCT_INDEX_PHASE_BITSHIFT 20
+#define NI_GPCT_COUNTING_DIRECTION_SHIFT 24
+enum ni_gpct_mode_bits {
+	NI_GPCT_GATE_ON_BOTH_EDGES_BIT = 0x4,
+	NI_GPCT_EDGE_GATE_MODE_MASK = 0x18,
+	NI_GPCT_EDGE_GATE_STARTS_STOPS_BITS = 0x0,
+	NI_GPCT_EDGE_GATE_STOPS_STARTS_BITS = 0x8,
+	NI_GPCT_EDGE_GATE_STARTS_BITS = 0x10,
+	NI_GPCT_EDGE_GATE_NO_STARTS_NO_STOPS_BITS = 0x18,
+	NI_GPCT_STOP_MODE_MASK = 0x60,
+	NI_GPCT_STOP_ON_GATE_BITS = 0x00,
+	NI_GPCT_STOP_ON_GATE_OR_TC_BITS = 0x20,
+	NI_GPCT_STOP_ON_GATE_OR_SECOND_TC_BITS = 0x40,
+	NI_GPCT_LOAD_B_SELECT_BIT = 0x80,
+	NI_GPCT_OUTPUT_MODE_MASK = 0x300,
+	NI_GPCT_OUTPUT_TC_PULSE_BITS = 0x100,
+	NI_GPCT_OUTPUT_TC_TOGGLE_BITS = 0x200,
+	NI_GPCT_OUTPUT_TC_OR_GATE_TOGGLE_BITS = 0x300,
+	NI_GPCT_HARDWARE_DISARM_MASK = 0xc00,
+	NI_GPCT_NO_HARDWARE_DISARM_BITS = 0x000,
+	NI_GPCT_DISARM_AT_TC_BITS = 0x400,
+	NI_GPCT_DISARM_AT_GATE_BITS = 0x800,
+	NI_GPCT_DISARM_AT_TC_OR_GATE_BITS = 0xc00,
+	NI_GPCT_LOADING_ON_TC_BIT = 0x1000,
+	NI_GPCT_LOADING_ON_GATE_BIT = 0x4000,
+	NI_GPCT_COUNTING_MODE_MASK = 0x7 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_NORMAL_BITS =
+		0x0 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_QUADRATURE_X1_BITS =
+		0x1 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_QUADRATURE_X2_BITS =
+		0x2 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_QUADRATURE_X4_BITS =
+		0x3 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_TWO_PULSE_BITS =
+		0x4 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_SYNC_SOURCE_BITS =
+		0x6 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_INDEX_PHASE_MASK = 0x3 << NI_GPCT_INDEX_PHASE_BITSHIFT,
+	NI_GPCT_INDEX_PHASE_LOW_A_LOW_B_BITS =
+		0x0 << NI_GPCT_INDEX_PHASE_BITSHIFT,
+	NI_GPCT_INDEX_PHASE_LOW_A_HIGH_B_BITS =
+		0x1 << NI_GPCT_INDEX_PHASE_BITSHIFT,
+	NI_GPCT_INDEX_PHASE_HIGH_A_LOW_B_BITS =
+		0x2 << NI_GPCT_INDEX_PHASE_BITSHIFT,
+	NI_GPCT_INDEX_PHASE_HIGH_A_HIGH_B_BITS =
+		0x3 << NI_GPCT_INDEX_PHASE_BITSHIFT,
+	NI_GPCT_INDEX_ENABLE_BIT = 0x400000,
+	NI_GPCT_COUNTING_DIRECTION_MASK =
+		0x3 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
+	NI_GPCT_COUNTING_DIRECTION_DOWN_BITS =
+		0x00 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
+	NI_GPCT_COUNTING_DIRECTION_UP_BITS =
+		0x1 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
+	NI_GPCT_COUNTING_DIRECTION_HW_UP_DOWN_BITS =
+		0x2 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
+	NI_GPCT_COUNTING_DIRECTION_HW_GATE_BITS =
+		0x3 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
+	NI_GPCT_RELOAD_SOURCE_MASK = 0xc000000,
+	NI_GPCT_RELOAD_SOURCE_FIXED_BITS = 0x0,
+	NI_GPCT_RELOAD_SOURCE_SWITCHING_BITS = 0x4000000,
+	NI_GPCT_RELOAD_SOURCE_GATE_SELECT_BITS = 0x8000000,
+	NI_GPCT_OR_GATE_BIT = 0x10000000,
+	NI_GPCT_INVERT_OUTPUT_BIT = 0x20000000
+};
+
+/* Bits for setting a clock source with
+ * INSN_CONFIG_SET_CLOCK_SRC when using NI general-purpose counters. */
+enum ni_gpct_clock_source_bits {
+	NI_GPCT_CLOCK_SRC_SELECT_MASK = 0x3f,
+	NI_GPCT_TIMEBASE_1_CLOCK_SRC_BITS = 0x0,
+	NI_GPCT_TIMEBASE_2_CLOCK_SRC_BITS = 0x1,
+	NI_GPCT_TIMEBASE_3_CLOCK_SRC_BITS = 0x2,
+	NI_GPCT_LOGIC_LOW_CLOCK_SRC_BITS = 0x3,
+	NI_GPCT_NEXT_GATE_CLOCK_SRC_BITS = 0x4,
+	NI_GPCT_NEXT_TC_CLOCK_SRC_BITS = 0x5,
+	NI_GPCT_SOURCE_PIN_i_CLOCK_SRC_BITS = 0x6,	/* NI 660x-specific */
+	NI_GPCT_PXI10_CLOCK_SRC_BITS = 0x7,
+	NI_GPCT_PXI_STAR_TRIGGER_CLOCK_SRC_BITS = 0x8,
+	NI_GPCT_ANALOG_TRIGGER_OUT_CLOCK_SRC_BITS = 0x9,
+	NI_GPCT_PRESCALE_MODE_CLOCK_SRC_MASK = 0x30000000,
+	NI_GPCT_NO_PRESCALE_CLOCK_SRC_BITS = 0x0,
+	NI_GPCT_PRESCALE_X2_CLOCK_SRC_BITS = 0x10000000, /* divide source by 2 */
+	NI_GPCT_PRESCALE_X8_CLOCK_SRC_BITS = 0x20000000, /* divide source by 8 */
+	NI_GPCT_INVERT_CLOCK_SRC_BIT = 0x80000000
+};
+static inline unsigned NI_GPCT_SOURCE_PIN_CLOCK_SRC_BITS(unsigned n)
+{
+	/* NI 660x-specific */
+	return 0x10 + n;
+}
+static inline unsigned NI_GPCT_RTSI_CLOCK_SRC_BITS(unsigned n)
+{
+	return 0x18 + n;
+}
+static inline unsigned NI_GPCT_PFI_CLOCK_SRC_BITS(unsigned n)
+{
+	/* no pfi on NI 660x */
+	return 0x20 + n;
+}
+
+/* Possibilities for setting a gate source with
+INSN_CONFIG_SET_GATE_SRC when using NI general-purpose counters.
+May be bitwise-or'd with CR_EDGE or CR_INVERT. */
+enum ni_gpct_gate_select {
+	/* m-series gates */
+	NI_GPCT_TIMESTAMP_MUX_GATE_SELECT = 0x0,
+	NI_GPCT_AI_START2_GATE_SELECT = 0x12,
+	NI_GPCT_PXI_STAR_TRIGGER_GATE_SELECT = 0x13,
+	NI_GPCT_NEXT_OUT_GATE_SELECT = 0x14,
+	NI_GPCT_AI_START1_GATE_SELECT = 0x1c,
+	NI_GPCT_NEXT_SOURCE_GATE_SELECT = 0x1d,
+	NI_GPCT_ANALOG_TRIGGER_OUT_GATE_SELECT = 0x1e,
+	NI_GPCT_LOGIC_LOW_GATE_SELECT = 0x1f,
+	/* more gates for 660x */
+	NI_GPCT_SOURCE_PIN_i_GATE_SELECT = 0x100,
+	NI_GPCT_GATE_PIN_i_GATE_SELECT = 0x101,
+	/* more gates for 660x "second gate" */
+	NI_GPCT_UP_DOWN_PIN_i_GATE_SELECT = 0x201,
+	NI_GPCT_SELECTED_GATE_GATE_SELECT = 0x21e,
+	/* m-series "second gate" sources are unknown,
+	   we should add them here with an offset of 0x300 when known. */
+	NI_GPCT_DISABLED_GATE_SELECT = 0x8000,
+};
+static inline unsigned NI_GPCT_GATE_PIN_GATE_SELECT(unsigned n)
+{
+	return 0x102 + n;
+}
+static inline unsigned NI_GPCT_RTSI_GATE_SELECT(unsigned n)
+{
+	return NI_USUAL_RTSI_SELECT(n);
+}
+static inline unsigned NI_GPCT_PFI_GATE_SELECT(unsigned n)
+{
+	return NI_USUAL_PFI_SELECT(n);
+}
+static inline unsigned NI_GPCT_UP_DOWN_PIN_GATE_SELECT(unsigned n)
+{
+	return 0x202 + n;
+}
+
+/* Possibilities for setting a source with
+INSN_CONFIG_SET_OTHER_SRC when using NI general-purpose counters. */
+enum ni_gpct_other_index {
+	NI_GPCT_SOURCE_ENCODER_A,
+	NI_GPCT_SOURCE_ENCODER_B,
+	NI_GPCT_SOURCE_ENCODER_Z
+};
+enum ni_gpct_other_select {
+	/* m-series gates */
+	/* Still unknown, probably only need NI_GPCT_PFI_OTHER_SELECT */
+	NI_GPCT_DISABLED_OTHER_SELECT = 0x8000,
+};
+static inline unsigned NI_GPCT_PFI_OTHER_SELECT(unsigned n)
+{
+	return NI_USUAL_PFI_SELECT(n);
+}
+
+/* start sources for ni general-purpose counters for use with
+INSN_CONFIG_ARM */
+enum ni_gpct_arm_source {
+	NI_GPCT_ARM_IMMEDIATE = 0x0,
+	NI_GPCT_ARM_PAIRED_IMMEDIATE = 0x1,	/* Start both the counter and
+						   the adjacent paired counter
+						   simultaneously */
+	/* NI doesn't document bits for selecting hardware arm triggers.  If
+	 * the NI_GPCT_ARM_UNKNOWN bit is set, we will pass the least
+	 * significant bits (3 bits for 660x or 5 bits for m-series) through to
+	 * the hardware.  This will at least allow someone to figure out what
+	 * the bits do later. */
+	NI_GPCT_ARM_UNKNOWN = 0x1000,
+};
+
+/* digital filtering options for ni 660x for use with INSN_CONFIG_FILTER. */
+enum ni_gpct_filter_select {
+	NI_GPCT_FILTER_OFF = 0x0,
+	NI_GPCT_FILTER_TIMEBASE_3_SYNC = 0x1,
+	NI_GPCT_FILTER_100x_TIMEBASE_1 = 0x2,
+	NI_GPCT_FILTER_20x_TIMEBASE_1 = 0x3,
+	NI_GPCT_FILTER_10x_TIMEBASE_1 = 0x4,
+	NI_GPCT_FILTER_2x_TIMEBASE_1 = 0x5,
+	NI_GPCT_FILTER_2x_TIMEBASE_3 = 0x6
+};
+
+/* PFI digital filtering options for ni m-series for use with
+ * INSN_CONFIG_FILTER. */
+enum ni_pfi_filter_select {
+	NI_PFI_FILTER_OFF = 0x0,
+	NI_PFI_FILTER_125ns = 0x1,
+	NI_PFI_FILTER_6425ns = 0x2,
+	NI_PFI_FILTER_2550us = 0x3
+};
+
+/* master clock sources for ni mio boards and INSN_CONFIG_SET_CLOCK_SRC */
+enum ni_mio_clock_source {
+	NI_MIO_INTERNAL_CLOCK = 0,
+	NI_MIO_RTSI_CLOCK = 1,	/* doesn't work for m-series, use
+				   NI_MIO_PLL_RTSI_CLOCK() */
+	/* the NI_MIO_PLL_* sources are m-series only */
+	NI_MIO_PLL_PXI_STAR_TRIGGER_CLOCK = 2,
+	NI_MIO_PLL_PXI10_CLOCK = 3,
+	NI_MIO_PLL_RTSI0_CLOCK = 4
+};
+static inline unsigned NI_MIO_PLL_RTSI_CLOCK(unsigned rtsi_channel)
+{
+	return NI_MIO_PLL_RTSI0_CLOCK + rtsi_channel;
+}
+
+/* Signals which can be routed to an NI RTSI pin with INSN_CONFIG_SET_ROUTING.
+ The numbers assigned are not arbitrary, they correspond to the bits required
+ to program the board. */
+enum ni_rtsi_routing {
+	NI_RTSI_OUTPUT_ADR_START1 = 0,
+	NI_RTSI_OUTPUT_ADR_START2 = 1,
+	NI_RTSI_OUTPUT_SCLKG = 2,
+	NI_RTSI_OUTPUT_DACUPDN = 3,
+	NI_RTSI_OUTPUT_DA_START1 = 4,
+	NI_RTSI_OUTPUT_G_SRC0 = 5,
+	NI_RTSI_OUTPUT_G_GATE0 = 6,
+	NI_RTSI_OUTPUT_RGOUT0 = 7,
+	NI_RTSI_OUTPUT_RTSI_BRD_0 = 8,
+	NI_RTSI_OUTPUT_RTSI_OSC = 12	/* pre-m-series always have RTSI clock
+					   on line 7 */
+};
+static inline unsigned NI_RTSI_OUTPUT_RTSI_BRD(unsigned n)
+{
+	return NI_RTSI_OUTPUT_RTSI_BRD_0 + n;
+}
+
+/* Signals which can be routed to an NI PFI pin on an m-series board with
+ * INSN_CONFIG_SET_ROUTING.  These numbers are also returned by
+ * INSN_CONFIG_GET_ROUTING on pre-m-series boards, even though their routing
+ * cannot be changed.  The numbers assigned are not arbitrary, they correspond
+ * to the bits required to program the board. */
+enum ni_pfi_routing {
+	NI_PFI_OUTPUT_PFI_DEFAULT = 0,
+	NI_PFI_OUTPUT_AI_START1 = 1,
+	NI_PFI_OUTPUT_AI_START2 = 2,
+	NI_PFI_OUTPUT_AI_CONVERT = 3,
+	NI_PFI_OUTPUT_G_SRC1 = 4,
+	NI_PFI_OUTPUT_G_GATE1 = 5,
+	NI_PFI_OUTPUT_AO_UPDATE_N = 6,
+	NI_PFI_OUTPUT_AO_START1 = 7,
+	NI_PFI_OUTPUT_AI_START_PULSE = 8,
+	NI_PFI_OUTPUT_G_SRC0 = 9,
+	NI_PFI_OUTPUT_G_GATE0 = 10,
+	NI_PFI_OUTPUT_EXT_STROBE = 11,
+	NI_PFI_OUTPUT_AI_EXT_MUX_CLK = 12,
+	NI_PFI_OUTPUT_GOUT0 = 13,
+	NI_PFI_OUTPUT_GOUT1 = 14,
+	NI_PFI_OUTPUT_FREQ_OUT = 15,
+	NI_PFI_OUTPUT_PFI_DO = 16,
+	NI_PFI_OUTPUT_I_ATRIG = 17,
+	NI_PFI_OUTPUT_RTSI0 = 18,
+	NI_PFI_OUTPUT_PXI_STAR_TRIGGER_IN = 26,
+	NI_PFI_OUTPUT_SCXI_TRIG1 = 27,
+	NI_PFI_OUTPUT_DIO_CHANGE_DETECT_RTSI = 28,
+	NI_PFI_OUTPUT_CDI_SAMPLE = 29,
+	NI_PFI_OUTPUT_CDO_UPDATE = 30
+};
+static inline unsigned NI_PFI_OUTPUT_RTSI(unsigned rtsi_channel)
+{
+	return NI_PFI_OUTPUT_RTSI0 + rtsi_channel;
+}
+
+/* Signals which can be routed to output on a NI PFI pin on a 660x board
+ with INSN_CONFIG_SET_ROUTING.  The numbers assigned are
+ not arbitrary, they correspond to the bits required
+ to program the board.  Lines 0 to 7 can only be set to
+ NI_660X_PFI_OUTPUT_DIO.  Lines 32 to 39 can only be set to
+ NI_660X_PFI_OUTPUT_COUNTER. */
+enum ni_660x_pfi_routing {
+	NI_660X_PFI_OUTPUT_COUNTER = 1,	/* counter */
+	NI_660X_PFI_OUTPUT_DIO = 2,	/* static digital output */
+};
+
+/* NI External Trigger lines.  These values are not arbitrary, but are related
+ * to the bits required to program the board (offset by 1 for historical
+ * reasons). */
+static inline unsigned NI_EXT_PFI(unsigned pfi_channel)
+{
+	return NI_USUAL_PFI_SELECT(pfi_channel) - 1;
+}
+static inline unsigned NI_EXT_RTSI(unsigned rtsi_channel)
+{
+	return NI_USUAL_RTSI_SELECT(rtsi_channel) - 1;
+}
+
+/* status bits for INSN_CONFIG_GET_COUNTER_STATUS */
+enum comedi_counter_status_flags {
+	COMEDI_COUNTER_ARMED = 0x1,
+	COMEDI_COUNTER_COUNTING = 0x2,
+	COMEDI_COUNTER_TERMINAL_COUNT = 0x4,
+};
+
+/* Clock sources for CDIO subdevice on NI m-series boards.  Used as the
+ * scan_begin_arg for a comedi_command. These sources may also be bitwise-or'd
+ * with CR_INVERT to change polarity. */
+enum ni_m_series_cdio_scan_begin_src {
+	NI_CDIO_SCAN_BEGIN_SRC_GROUND = 0,
+	NI_CDIO_SCAN_BEGIN_SRC_AI_START = 18,
+	NI_CDIO_SCAN_BEGIN_SRC_AI_CONVERT = 19,
+	NI_CDIO_SCAN_BEGIN_SRC_PXI_STAR_TRIGGER = 20,
+	NI_CDIO_SCAN_BEGIN_SRC_G0_OUT = 28,
+	NI_CDIO_SCAN_BEGIN_SRC_G1_OUT = 29,
+	NI_CDIO_SCAN_BEGIN_SRC_ANALOG_TRIGGER = 30,
+	NI_CDIO_SCAN_BEGIN_SRC_AO_UPDATE = 31,
+	NI_CDIO_SCAN_BEGIN_SRC_FREQ_OUT = 32,
+	NI_CDIO_SCAN_BEGIN_SRC_DIO_CHANGE_DETECT_IRQ = 33
+};
+static inline unsigned NI_CDIO_SCAN_BEGIN_SRC_PFI(unsigned pfi_channel)
+{
+	return NI_USUAL_PFI_SELECT(pfi_channel);
+}
+static inline unsigned NI_CDIO_SCAN_BEGIN_SRC_RTSI(unsigned rtsi_channel)
+{
+	return NI_USUAL_RTSI_SELECT(rtsi_channel);
+}
+
+/* scan_begin_src for scan_begin_arg==TRIG_EXT with analog output command on NI
+ * boards.  These scan begin sources can also be bitwise-or'd with CR_INVERT to
+ * change polarity. */
+static inline unsigned NI_AO_SCAN_BEGIN_SRC_PFI(unsigned pfi_channel)
+{
+	return NI_USUAL_PFI_SELECT(pfi_channel);
+}
+static inline unsigned NI_AO_SCAN_BEGIN_SRC_RTSI(unsigned rtsi_channel)
+{
+	return NI_USUAL_RTSI_SELECT(rtsi_channel);
+}
+
+/* Bits for setting a clock source with
+ * INSN_CONFIG_SET_CLOCK_SRC when using NI frequency output subdevice. */
+enum ni_freq_out_clock_source_bits {
+	NI_FREQ_OUT_TIMEBASE_1_DIV_2_CLOCK_SRC,	/* 10 MHz */
+	NI_FREQ_OUT_TIMEBASE_2_CLOCK_SRC	/* 100 KHz */
+};
+
+/* Values for setting a clock source with INSN_CONFIG_SET_CLOCK_SRC for
+ * 8254 counter subdevices on Amplicon DIO boards (amplc_dio200 driver). */
+	enum amplc_dio_clock_source {
+		AMPLC_DIO_CLK_CLKN,	/* per channel external clock
+					   input/output pin (pin is only an
+					   input when clock source set to this
+					   value, otherwise it is an output) */
+		AMPLC_DIO_CLK_10MHZ,	/* 10 MHz internal clock */
+		AMPLC_DIO_CLK_1MHZ,	/* 1 MHz internal clock */
+		AMPLC_DIO_CLK_100KHZ,	/* 100 kHz internal clock */
+		AMPLC_DIO_CLK_10KHZ,	/* 10 kHz internal clock */
+		AMPLC_DIO_CLK_1KHZ,	/* 1 kHz internal clock */
+		AMPLC_DIO_CLK_OUTNM1,	/* output of preceding counter channel
+					   (for channel 0, preceding counter
+					   channel is channel 2 on preceding
+					   counter subdevice, for first counter
+					   subdevice, preceding counter
+					   subdevice is the last counter
+					   subdevice) */
+		AMPLC_DIO_CLK_EXT	/* per chip external input pin */
+	};
+
+/* Values for setting a gate source with INSN_CONFIG_SET_GATE_SRC for
+ * 8254 counter subdevices on Amplicon DIO boards (amplc_dio200 driver). */
+	enum amplc_dio_gate_source {
+		AMPLC_DIO_GAT_VCC,	/* internal high logic level */
+		AMPLC_DIO_GAT_GND,	/* internal low logic level */
+		AMPLC_DIO_GAT_GATN,	/* per channel external gate input */
+		AMPLC_DIO_GAT_NOUTNM2,	/* negated output of counter channel
+					   minus 2 (for channels 0 or 1,
+					   channel minus 2 is channel 1 or 2 on
+					   the preceding counter subdevice, for
+					   the first counter subdevice the
+					   preceding counter subdevice is the
+					   last counter subdevice) */
+		AMPLC_DIO_GAT_RESERVED4,
+		AMPLC_DIO_GAT_RESERVED5,
+		AMPLC_DIO_GAT_RESERVED6,
+		AMPLC_DIO_GAT_RESERVED7
+	};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _COMEDI_H */
diff --git a/drivers/staging/comedi/comedi_compat32.c b/drivers/staging/comedi/comedi_compat32.c
new file mode 100644
index 000000000000..7d0116bcb9ff
--- /dev/null
+++ b/drivers/staging/comedi/comedi_compat32.c
@@ -0,0 +1,597 @@
+/*
+    comedi/comedi_compat32.c
+    32-bit ioctl compatibility for 64-bit comedi kernel module.
+
+    Author: Ian Abbott, MEV Ltd. <abbotti@mev.co.uk>
+    Copyright (C) 2007 MEV Ltd. <http://www.mev.co.uk/>
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2007 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#define __NO_VERSION__
+#include "comedi.h"
+#include <linux/smp_lock.h>
+#include <asm/uaccess.h>
+
+#include "comedi_compat32.h"
+
+#ifdef CONFIG_COMPAT
+
+#ifndef HAVE_COMPAT_IOCTL
+#include <linux/ioctl32.h>	/* for (un)register_ioctl32_conversion */
+#endif
+
+#define COMEDI32_CHANINFO _IOR(CIO,3,comedi32_chaninfo)
+#define COMEDI32_RANGEINFO _IOR(CIO,8,comedi32_rangeinfo)
+/* N.B. COMEDI32_CMD and COMEDI_CMD ought to use _IOWR, not _IOR.
+ * It's too late to change it now, but it only affects the command number. */
+#define COMEDI32_CMD _IOR(CIO,9,comedi32_cmd)
+/* N.B. COMEDI32_CMDTEST and COMEDI_CMDTEST ought to use _IOWR, not _IOR.
+ * It's too late to change it now, but it only affects the command number. */
+#define COMEDI32_CMDTEST _IOR(CIO,10,comedi32_cmd)
+#define COMEDI32_INSNLIST _IOR(CIO,11,comedi32_insnlist)
+#define COMEDI32_INSN _IOR(CIO,12,comedi32_insn)
+
+typedef struct comedi32_chaninfo_struct {
+	unsigned int subdev;
+	compat_uptr_t maxdata_list;	/* 32-bit 'lsampl_t *' */
+	compat_uptr_t flaglist;		/* 32-bit 'unsigned int *' */
+	compat_uptr_t rangelist;	/* 32-bit 'unsigned int *' */
+	unsigned int unused[4];
+} comedi32_chaninfo;
+
+typedef struct comedi32_rangeinfo_struct {
+	unsigned int range_type;
+	compat_uptr_t range_ptr;	/* 32-bit 'void *' */
+} comedi32_rangeinfo;
+
+typedef struct comedi32_cmd_struct {
+	unsigned int subdev;
+	unsigned int flags;
+	unsigned int start_src;
+	unsigned int start_arg;
+	unsigned int scan_begin_src;
+	unsigned int scan_begin_arg;
+	unsigned int convert_src;
+	unsigned int convert_arg;
+	unsigned int scan_end_src;
+	unsigned int scan_end_arg;
+	unsigned int stop_src;
+	unsigned int stop_arg;
+	compat_uptr_t chanlist;		/* 32-bit 'unsigned int *' */
+	unsigned int chanlist_len;
+	compat_uptr_t data;		/* 32-bit 'sampl_t *' */
+	unsigned int data_len;
+} comedi32_cmd;
+
+typedef struct comedi32_insn_struct {
+	unsigned int insn;
+	unsigned int n;
+	compat_uptr_t data;		/* 32-bit 'lsampl_t *' */
+	unsigned int subdev;
+	unsigned int chanspec;
+	unsigned int unused[3];
+} comedi32_insn;
+
+typedef struct comedi32_insnlist_struct {
+	unsigned int n_insns;
+	compat_uptr_t insns;		/* 32-bit 'comedi_insn *' */
+} comedi32_insnlist;
+
+/* Handle translated ioctl. */
+static int translated_ioctl(struct file *file, unsigned int cmd,
+		unsigned long arg)
+{
+	if (!file->f_op) {
+		return -ENOTTY;
+	}
+#ifdef HAVE_UNLOCKED_IOCTL
+	if (file->f_op->unlocked_ioctl) {
+		int rc = (int)(*file->f_op->unlocked_ioctl)(file, cmd, arg);
+		if (rc == -ENOIOCTLCMD) {
+			rc = -ENOTTY;
+		}
+		return rc;
+	}
+#endif
+	if (file->f_op->ioctl) {
+		int rc;
+		lock_kernel();
+		rc = (*file->f_op->ioctl)(file->f_dentry->d_inode,
+				file, cmd, arg);
+		unlock_kernel();
+		return rc;
+	}
+	return -ENOTTY;
+}
+
+/* Handle 32-bit COMEDI_CHANINFO ioctl. */
+static int compat_chaninfo(struct file *file, unsigned long arg)
+{
+	comedi_chaninfo __user *chaninfo;
+	comedi32_chaninfo __user *chaninfo32;
+	int err;
+	union {
+		unsigned int uint;
+		compat_uptr_t uptr;
+	} temp;
+
+	chaninfo32 = compat_ptr(arg);
+	chaninfo = compat_alloc_user_space(sizeof(*chaninfo));
+
+	/* Copy chaninfo structure.  Ignore unused members. */
+	if (!access_ok(VERIFY_READ, chaninfo32, sizeof(*chaninfo32))
+			|| !access_ok(VERIFY_WRITE, chaninfo,
+				sizeof(*chaninfo))) {
+		return -EFAULT;
+	}
+	err = 0;
+	err |= __get_user(temp.uint, &chaninfo32->subdev);
+	err |= __put_user(temp.uint, &chaninfo->subdev);
+	err |= __get_user(temp.uptr, &chaninfo32->maxdata_list);
+	err |= __put_user(compat_ptr(temp.uptr), &chaninfo->maxdata_list);
+	err |= __get_user(temp.uptr, &chaninfo32->flaglist);
+	err |= __put_user(compat_ptr(temp.uptr), &chaninfo->flaglist);
+	err |= __get_user(temp.uptr, &chaninfo32->rangelist);
+	err |= __put_user(compat_ptr(temp.uptr), &chaninfo->rangelist);
+	if (err) {
+		return -EFAULT;
+	}
+
+	return translated_ioctl(file, COMEDI_CHANINFO, (unsigned long)chaninfo);
+}
+
+/* Handle 32-bit COMEDI_RANGEINFO ioctl. */
+static int compat_rangeinfo(struct file *file, unsigned long arg)
+{
+	comedi_rangeinfo __user *rangeinfo;
+	comedi32_rangeinfo __user *rangeinfo32;
+	int err;
+	union {
+		unsigned int uint;
+		compat_uptr_t uptr;
+	} temp;
+
+	rangeinfo32 = compat_ptr(arg);
+	rangeinfo = compat_alloc_user_space(sizeof(*rangeinfo));
+
+	/* Copy rangeinfo structure. */
+	if (!access_ok(VERIFY_READ, rangeinfo32, sizeof(*rangeinfo32))
+			|| !access_ok(VERIFY_WRITE, rangeinfo,
+				sizeof(*rangeinfo))) {
+		return -EFAULT;
+	}
+	err = 0;
+	err |= __get_user(temp.uint, &rangeinfo32->range_type);
+	err |= __put_user(temp.uint, &rangeinfo->range_type);
+	err |= __get_user(temp.uptr, &rangeinfo32->range_ptr);
+	err |= __put_user(compat_ptr(temp.uptr), &rangeinfo->range_ptr);
+	if (err) {
+		return -EFAULT;
+	}
+
+	return translated_ioctl(file, COMEDI_RANGEINFO,
+			(unsigned long)rangeinfo);
+}
+
+/* Copy 32-bit cmd structure to native cmd structure. */
+static int get_compat_cmd(comedi_cmd __user *cmd,
+		comedi32_cmd __user *cmd32)
+{
+	int err;
+	union {
+		unsigned int uint;
+		compat_uptr_t uptr;
+	} temp;
+
+	/* Copy cmd structure. */
+	if (!access_ok(VERIFY_READ, cmd32, sizeof(*cmd32))
+			|| !access_ok(VERIFY_WRITE, cmd, sizeof(*cmd))) {
+		return -EFAULT;
+	}
+	err = 0;
+	err |= __get_user(temp.uint, &cmd32->subdev);
+	err |= __put_user(temp.uint, &cmd->subdev);
+	err |= __get_user(temp.uint, &cmd32->flags);
+	err |= __put_user(temp.uint, &cmd->flags);
+	err |= __get_user(temp.uint, &cmd32->start_src);
+	err |= __put_user(temp.uint, &cmd->start_src);
+	err |= __get_user(temp.uint, &cmd32->start_arg);
+	err |= __put_user(temp.uint, &cmd->start_arg);
+	err |= __get_user(temp.uint, &cmd32->scan_begin_src);
+	err |= __put_user(temp.uint, &cmd->scan_begin_src);
+	err |= __get_user(temp.uint, &cmd32->scan_begin_arg);
+	err |= __put_user(temp.uint, &cmd->scan_begin_arg);
+	err |= __get_user(temp.uint, &cmd32->convert_src);
+	err |= __put_user(temp.uint, &cmd->convert_src);
+	err |= __get_user(temp.uint, &cmd32->convert_arg);
+	err |= __put_user(temp.uint, &cmd->convert_arg);
+	err |= __get_user(temp.uint, &cmd32->scan_end_src);
+	err |= __put_user(temp.uint, &cmd->scan_end_src);
+	err |= __get_user(temp.uint, &cmd32->scan_end_arg);
+	err |= __put_user(temp.uint, &cmd->scan_end_arg);
+	err |= __get_user(temp.uint, &cmd32->stop_src);
+	err |= __put_user(temp.uint, &cmd->stop_src);
+	err |= __get_user(temp.uint, &cmd32->stop_arg);
+	err |= __put_user(temp.uint, &cmd->stop_arg);
+	err |= __get_user(temp.uptr, &cmd32->chanlist);
+	err |= __put_user(compat_ptr(temp.uptr), &cmd->chanlist);
+	err |= __get_user(temp.uint, &cmd32->chanlist_len);
+	err |= __put_user(temp.uint, &cmd->chanlist_len);
+	err |= __get_user(temp.uptr, &cmd32->data);
+	err |= __put_user(compat_ptr(temp.uptr), &cmd->data);
+	err |= __get_user(temp.uint, &cmd32->data_len);
+	err |= __put_user(temp.uint, &cmd->data_len);
+	return err ? -EFAULT : 0;
+}
+
+/* Copy native cmd structure to 32-bit cmd structure. */
+static int put_compat_cmd(comedi32_cmd __user *cmd32, comedi_cmd __user *cmd)
+{
+	int err;
+	unsigned int temp;
+
+	/* Copy back most of cmd structure. */
+	/* Assume the pointer values are already valid. */
+	/* (Could use ptr_to_compat() to set them, but that wasn't implemented
+	 * until kernel version 2.6.11.) */
+	if (!access_ok(VERIFY_READ, cmd, sizeof(*cmd))
+			|| !access_ok(VERIFY_WRITE, cmd32, sizeof(*cmd32))) {
+		return -EFAULT;
+	}
+	err = 0;
+	err |= __get_user(temp, &cmd->subdev);
+	err |= __put_user(temp, &cmd32->subdev);
+	err |= __get_user(temp, &cmd->flags);
+	err |= __put_user(temp, &cmd32->flags);
+	err |= __get_user(temp, &cmd->start_src);
+	err |= __put_user(temp, &cmd32->start_src);
+	err |= __get_user(temp, &cmd->start_arg);
+	err |= __put_user(temp, &cmd32->start_arg);
+	err |= __get_user(temp, &cmd->scan_begin_src);
+	err |= __put_user(temp, &cmd32->scan_begin_src);
+	err |= __get_user(temp, &cmd->scan_begin_arg);
+	err |= __put_user(temp, &cmd32->scan_begin_arg);
+	err |= __get_user(temp, &cmd->convert_src);
+	err |= __put_user(temp, &cmd32->convert_src);
+	err |= __get_user(temp, &cmd->convert_arg);
+	err |= __put_user(temp, &cmd32->convert_arg);
+	err |= __get_user(temp, &cmd->scan_end_src);
+	err |= __put_user(temp, &cmd32->scan_end_src);
+	err |= __get_user(temp, &cmd->scan_end_arg);
+	err |= __put_user(temp, &cmd32->scan_end_arg);
+	err |= __get_user(temp, &cmd->stop_src);
+	err |= __put_user(temp, &cmd32->stop_src);
+	err |= __get_user(temp, &cmd->stop_arg);
+	err |= __put_user(temp, &cmd32->stop_arg);
+	/* Assume chanlist pointer is unchanged. */
+	err |= __get_user(temp, &cmd->chanlist_len);
+	err |= __put_user(temp, &cmd32->chanlist_len);
+	/* Assume data pointer is unchanged. */
+	err |= __get_user(temp, &cmd->data_len);
+	err |= __put_user(temp, &cmd32->data_len);
+	return err ? -EFAULT : 0;
+}
+
+/* Handle 32-bit COMEDI_CMD ioctl. */
+static int compat_cmd(struct file *file, unsigned long arg)
+{
+	comedi_cmd __user *cmd;
+	comedi32_cmd __user *cmd32;
+	int rc;
+
+	cmd32 = compat_ptr(arg);
+	cmd = compat_alloc_user_space(sizeof(*cmd));
+
+	rc = get_compat_cmd(cmd, cmd32);
+	if (rc) {
+		return rc;
+	}
+
+	return translated_ioctl(file, COMEDI_CMD, (unsigned long)cmd);
+}
+
+/* Handle 32-bit COMEDI_CMDTEST ioctl. */
+static int compat_cmdtest(struct file *file, unsigned long arg)
+{
+	comedi_cmd __user *cmd;
+	comedi32_cmd __user *cmd32;
+	int rc, err;
+
+	cmd32 = compat_ptr(arg);
+	cmd = compat_alloc_user_space(sizeof(*cmd));
+
+	rc = get_compat_cmd(cmd, cmd32);
+	if (rc) {
+		return rc;
+	}
+
+	rc = translated_ioctl(file, COMEDI_CMDTEST, (unsigned long)cmd);
+	if (rc < 0) {
+		return rc;
+	}
+
+	err = put_compat_cmd(cmd32, cmd);
+	if (err) {
+		rc = err;
+	}
+	return rc;
+}
+
+/* Copy 32-bit insn structure to native insn structure. */
+static int get_compat_insn(comedi_insn __user *insn,
+		comedi32_insn __user *insn32)
+{
+	int err;
+	union {
+		unsigned int uint;
+		compat_uptr_t uptr;
+	} temp;
+
+	/* Copy insn structure.  Ignore the unused members. */
+	err = 0;
+	if (!access_ok(VERIFY_READ, insn32, sizeof(*insn32))
+			|| !access_ok(VERIFY_WRITE, insn, sizeof(*insn))) {
+		return -EFAULT;
+	}
+	err |= __get_user(temp.uint, &insn32->insn);
+	err |= __put_user(temp.uint, &insn->insn);
+	err |= __get_user(temp.uint, &insn32->n);
+	err |= __put_user(temp.uint, &insn->n);
+	err |= __get_user(temp.uptr, &insn32->data);
+	err |= __put_user(compat_ptr(temp.uptr), &insn->data);
+	err |= __get_user(temp.uint, &insn32->subdev);
+	err |= __put_user(temp.uint, &insn->subdev);
+	err |= __get_user(temp.uint, &insn32->chanspec);
+	err |= __put_user(temp.uint, &insn->chanspec);
+	return err ? -EFAULT : 0;
+}
+
+/* Handle 32-bit COMEDI_INSNLIST ioctl. */
+static int compat_insnlist(struct file *file, unsigned long arg)
+{
+	struct combined_insnlist {
+		comedi_insnlist insnlist;
+		comedi_insn insn[1];
+	} __user *s;
+	comedi32_insnlist __user *insnlist32;
+	comedi32_insn __user *insn32;
+	compat_uptr_t uptr;
+	unsigned int n_insns, n;
+	int err, rc;
+
+	insnlist32 = compat_ptr(arg);
+
+	/* Get 32-bit insnlist structure.  */
+	if (!access_ok(VERIFY_READ, insnlist32, sizeof(*insnlist32))) {
+		return -EFAULT;
+	}
+	err = 0;
+	err |= __get_user(n_insns, &insnlist32->n_insns);
+	err |= __get_user(uptr, &insnlist32->insns);
+	insn32 = compat_ptr(uptr);
+	if (err) {
+		return -EFAULT;
+	}
+
+	/* Allocate user memory to copy insnlist and insns into. */
+	s = compat_alloc_user_space(offsetof(struct combined_insnlist,
+				insn[n_insns]));
+
+	/* Set native insnlist structure. */
+	if (!access_ok(VERIFY_WRITE, &s->insnlist, sizeof(s->insnlist))) {
+		return -EFAULT;
+	}
+	err |= __put_user(n_insns, &s->insnlist.n_insns);
+	err |= __put_user(&s->insn[0], &s->insnlist.insns);
+	if (err) {
+		return -EFAULT;
+	}
+
+	/* Copy insn structures. */
+	for (n = 0; n < n_insns; n++) {
+		rc = get_compat_insn(&s->insn[n], &insn32[n]);
+		if (rc) {
+			return rc;
+		}
+	}
+
+	return translated_ioctl(file, COMEDI_INSNLIST,
+			(unsigned long)&s->insnlist);
+}
+
+/* Handle 32-bit COMEDI_INSN ioctl. */
+static int compat_insn(struct file *file, unsigned long arg)
+{
+	comedi_insn __user *insn;
+	comedi32_insn __user *insn32;
+	int rc;
+
+	insn32 = compat_ptr(arg);
+	insn = compat_alloc_user_space(sizeof(*insn));
+
+	rc = get_compat_insn(insn, insn32);
+	if (rc) {
+		return rc;
+	}
+
+	return translated_ioctl(file, COMEDI_INSN, (unsigned long)insn);
+}
+
+/* Process untranslated ioctl. */
+/* Returns -ENOIOCTLCMD for unrecognised ioctl codes. */
+static inline int raw_ioctl(struct file *file, unsigned int cmd,
+		unsigned long arg)
+{
+	int rc;
+
+	switch (cmd) {
+	case COMEDI_DEVCONFIG:
+	case COMEDI_DEVINFO:
+	case COMEDI_SUBDINFO:
+	case COMEDI_BUFCONFIG:
+	case COMEDI_BUFINFO:
+		/* Just need to translate the pointer argument. */
+		arg = (unsigned long)compat_ptr(arg);
+		rc = translated_ioctl(file, cmd, arg);
+		break;
+	case COMEDI_LOCK:
+	case COMEDI_UNLOCK:
+	case COMEDI_CANCEL:
+	case COMEDI_POLL:
+		/* No translation needed. */
+		rc = translated_ioctl(file, cmd, arg);
+		break;
+	case COMEDI32_CHANINFO:
+		rc = compat_chaninfo(file, arg);
+		break;
+	case COMEDI32_RANGEINFO:
+		rc = compat_rangeinfo(file, arg);
+		break;
+	case COMEDI32_CMD:
+		rc = compat_cmd(file, arg);
+		break;
+	case COMEDI32_CMDTEST:
+		rc = compat_cmdtest(file, arg);
+		break;
+	case COMEDI32_INSNLIST:
+		rc = compat_insnlist(file, arg);
+		break;
+	case COMEDI32_INSN:
+		rc = compat_insn(file, arg);
+		break;
+	default:
+		rc = -ENOIOCTLCMD;
+		break;
+	}
+	return rc;
+}
+
+#ifdef HAVE_COMPAT_IOCTL	/* defined in <linux/fs.h> 2.6.11 onwards */
+
+/* compat_ioctl file operation. */
+/* Returns -ENOIOCTLCMD for unrecognised ioctl codes. */
+long comedi_compat_ioctl(struct file *file, unsigned int cmd,
+		unsigned long arg)
+{
+	return raw_ioctl(file, cmd, arg);
+}
+
+#else /* HAVE_COMPAT_IOCTL */
+
+/*
+ * Brain-dead ioctl compatibility for 2.6.10 and earlier.
+ *
+ * It's brain-dead because cmd numbers need to be unique system-wide!
+ * The comedi driver could end up attempting to execute ioctls for non-Comedi
+ * devices because it registered the system-wide cmd code first.  Similarly,
+ * another driver could end up attempting to execute ioctls for a Comedi
+ * device because it registered the cmd code first.  Chaos ensues.
+ */
+
+/* Handler for all 32-bit ioctl codes registered by this driver. */
+static int mapped_ioctl(unsigned int fd, unsigned int cmd, unsigned long arg,
+		struct file *file)
+{
+	int rc;
+
+	/* Make sure we are dealing with a Comedi device. */
+	if (imajor(file->f_dentry->d_inode) != COMEDI_MAJOR) {
+		return -ENOTTY;
+	}
+	rc = raw_ioctl(file, cmd, arg);
+	/* Do not return -ENOIOCTLCMD. */
+	if (rc == -ENOIOCTLCMD) {
+		rc = -ENOTTY;
+	}
+	return rc;
+}
+
+struct ioctl32_map {
+	unsigned int cmd;
+	int (*handler)(unsigned int, unsigned int, unsigned long,
+			struct file *);
+	int registered;
+};
+
+static struct ioctl32_map comedi_ioctl32_map[] = {
+	{ COMEDI_DEVCONFIG, mapped_ioctl, 0 },
+	{ COMEDI_DEVINFO, mapped_ioctl, 0 },
+	{ COMEDI_SUBDINFO, mapped_ioctl, 0 },
+	{ COMEDI_BUFCONFIG, mapped_ioctl, 0 },
+	{ COMEDI_BUFINFO, mapped_ioctl, 0 },
+	{ COMEDI_LOCK, mapped_ioctl, 0 },
+	{ COMEDI_UNLOCK, mapped_ioctl, 0 },
+	{ COMEDI_CANCEL, mapped_ioctl, 0 },
+	{ COMEDI_POLL, mapped_ioctl, 0 },
+	{ COMEDI32_CHANINFO, mapped_ioctl, 0 },
+	{ COMEDI32_RANGEINFO, mapped_ioctl, 0 },
+	{ COMEDI32_CMD, mapped_ioctl, 0 },
+	{ COMEDI32_CMDTEST, mapped_ioctl, 0 },
+	{ COMEDI32_INSNLIST, mapped_ioctl, 0 },
+	{ COMEDI32_INSN, mapped_ioctl, 0 },
+};
+
+#define NUM_IOCTL32_MAPS ARRAY_SIZE(comedi_ioctl32_map)
+
+/* Register system-wide 32-bit ioctl handlers. */
+void comedi_register_ioctl32(void)
+{
+	int n, rc;
+
+	for (n = 0; n < NUM_IOCTL32_MAPS; n++) {
+		rc = register_ioctl32_conversion(comedi_ioctl32_map[n].cmd,
+				comedi_ioctl32_map[n].handler);
+		if (rc) {
+			printk(KERN_WARNING
+					"comedi: failed to register 32-bit "
+					"compatible ioctl handler for 0x%X - "
+					"expect bad things to happen!\n",
+					comedi_ioctl32_map[n].cmd);
+		}
+		comedi_ioctl32_map[n].registered = !rc;
+	}
+}
+
+/* Unregister system-wide 32-bit ioctl translations. */
+void comedi_unregister_ioctl32(void)
+{
+	int n, rc;
+
+	for (n = 0; n < NUM_IOCTL32_MAPS; n++) {
+		if (comedi_ioctl32_map[n].registered) {
+			rc = unregister_ioctl32_conversion(
+					comedi_ioctl32_map[n].cmd,
+					comedi_ioctl32_map[n].handler);
+			if (rc) {
+				printk(KERN_ERR
+					"comedi: failed to unregister 32-bit "
+					"compatible ioctl handler for 0x%X - "
+					"expect kernel Oops!\n",
+					comedi_ioctl32_map[n].cmd);
+			} else {
+				comedi_ioctl32_map[n].registered = 0;
+			}
+		}
+	}
+}
+
+#endif	/* HAVE_COMPAT_IOCTL */
+
+#endif	/* CONFIG_COMPAT */
diff --git a/drivers/staging/comedi/comedi_compat32.h b/drivers/staging/comedi/comedi_compat32.h
new file mode 100644
index 000000000000..0ca01642c165
--- /dev/null
+++ b/drivers/staging/comedi/comedi_compat32.h
@@ -0,0 +1,58 @@
+/*
+    comedi/comedi_compat32.h
+    32-bit ioctl compatibility for 64-bit comedi kernel module.
+
+    Author: Ian Abbott, MEV Ltd. <abbotti@mev.co.uk>
+    Copyright (C) 2007 MEV Ltd. <http://www.mev.co.uk/>
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2007 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _COMEDI_COMPAT32_H
+#define _COMEDI_COMPAT32_H
+
+#include <linux/compat.h>
+#include <linux/fs.h>	/* For HAVE_COMPAT_IOCTL and HAVE_UNLOCKED_IOCTL */
+
+#ifdef CONFIG_COMPAT
+
+#ifdef HAVE_COMPAT_IOCTL
+
+extern long comedi_compat_ioctl(struct file *file, unsigned int cmd,
+		unsigned long arg);
+#define comedi_register_ioctl32() do {} while (0)
+#define comedi_unregister_ioctl32() do {} while (0)
+
+#else /* HAVE_COMPAT_IOCTL */
+
+#define comedi_compat_ioctl 0	/* NULL */
+extern void comedi_register_ioctl32(void);
+extern void comedi_unregister_ioctl32(void);
+
+#endif /* HAVE_COMPAT_IOCTL */
+
+#else /* CONFIG_COMPAT */
+
+#define comedi_compat_ioctl 0	/* NULL */
+#define comedi_register_ioctl32() do {} while (0)
+#define comedi_unregister_ioctl32() do {} while (0)
+
+#endif /* CONFIG_COMPAT */
+
+#endif /* _COMEDI_COMPAT32_H */
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
new file mode 100644
index 000000000000..018c964396df
--- /dev/null
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -0,0 +1,2244 @@
+/*
+    comedi/comedi_fops.c
+    comedi kernel module
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#undef DEBUG
+
+#define __NO_VERSION__
+#include "comedi_fops.h"
+#include "comedi_compat32.h"
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/fcntl.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/kmod.h>
+#include <linux/poll.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/vmalloc.h>
+#include <linux/fs.h>
+#include "comedidev.h"
+#include <linux/cdev.h>
+
+#include <linux/io.h>
+#include <linux/uaccess.h>
+
+/* #include "kvmem.h" */
+
+MODULE_AUTHOR("http://www.comedi.org");
+MODULE_DESCRIPTION("Comedi core module");
+MODULE_LICENSE("GPL");
+
+#ifdef CONFIG_COMEDI_DEBUG
+int comedi_debug;
+module_param(comedi_debug, int, 0644);
+#endif
+
+static DEFINE_SPINLOCK(comedi_file_info_table_lock);
+static struct comedi_device_file_info
+    *comedi_file_info_table[COMEDI_NUM_MINORS];
+
+static int do_devconfig_ioctl(comedi_device *dev, comedi_devconfig *arg);
+static int do_bufconfig_ioctl(comedi_device *dev, void *arg);
+static int do_devinfo_ioctl(comedi_device *dev, comedi_devinfo *arg,
+			    struct file *file);
+static int do_subdinfo_ioctl(comedi_device *dev, comedi_subdinfo *arg,
+			     void *file);
+static int do_chaninfo_ioctl(comedi_device *dev, comedi_chaninfo *arg);
+static int do_bufinfo_ioctl(comedi_device *dev, void *arg);
+static int do_cmd_ioctl(comedi_device *dev, void *arg, void *file);
+static int do_lock_ioctl(comedi_device *dev, unsigned int arg, void *file);
+static int do_unlock_ioctl(comedi_device *dev, unsigned int arg, void *file);
+static int do_cancel_ioctl(comedi_device *dev, unsigned int arg, void *file);
+static int do_cmdtest_ioctl(comedi_device *dev, void *arg, void *file);
+static int do_insnlist_ioctl(comedi_device *dev, void *arg, void *file);
+static int do_insn_ioctl(comedi_device *dev, void *arg, void *file);
+static int do_poll_ioctl(comedi_device *dev, unsigned int subd, void *file);
+
+extern void do_become_nonbusy(comedi_device *dev, comedi_subdevice *s);
+static int do_cancel(comedi_device *dev, comedi_subdevice *s);
+
+static int comedi_fasync(int fd, struct file *file, int on);
+
+static int is_device_busy(comedi_device *dev);
+
+#ifdef HAVE_UNLOCKED_IOCTL
+static long comedi_unlocked_ioctl(struct file *file, unsigned int cmd,
+				  unsigned long arg)
+#else
+static int comedi_ioctl(struct inode *inode, struct file *file,
+			unsigned int cmd, unsigned long arg)
+#endif
+{
+	const unsigned minor = iminor(file->f_dentry->d_inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_device *dev = dev_file_info->device;
+	int rc;
+
+	mutex_lock(&dev->mutex);
+
+	/* Device config is special, because it must work on
+	 * an unconfigured device. */
+	if (cmd == COMEDI_DEVCONFIG) {
+		rc = do_devconfig_ioctl(dev, (void *)arg);
+		goto done;
+	}
+
+	if (!dev->attached) {
+		DPRINTK("no driver configured on /dev/comedi%i\n", dev->minor);
+		rc = -ENODEV;
+		goto done;
+	}
+
+	switch (cmd) {
+	case COMEDI_BUFCONFIG:
+		rc = do_bufconfig_ioctl(dev, (void *)arg);
+		break;
+	case COMEDI_DEVINFO:
+		rc = do_devinfo_ioctl(dev, (void *)arg, file);
+		break;
+	case COMEDI_SUBDINFO:
+		rc = do_subdinfo_ioctl(dev, (void *)arg, file);
+		break;
+	case COMEDI_CHANINFO:
+		rc = do_chaninfo_ioctl(dev, (void *)arg);
+		break;
+	case COMEDI_RANGEINFO:
+		rc = do_rangeinfo_ioctl(dev, (void *)arg);
+		break;
+	case COMEDI_BUFINFO:
+		rc = do_bufinfo_ioctl(dev, (void *)arg);
+		break;
+	case COMEDI_LOCK:
+		rc = do_lock_ioctl(dev, arg, file);
+		break;
+	case COMEDI_UNLOCK:
+		rc = do_unlock_ioctl(dev, arg, file);
+		break;
+	case COMEDI_CANCEL:
+		rc = do_cancel_ioctl(dev, arg, file);
+		break;
+	case COMEDI_CMD:
+		rc = do_cmd_ioctl(dev, (void *)arg, file);
+		break;
+	case COMEDI_CMDTEST:
+		rc = do_cmdtest_ioctl(dev, (void *)arg, file);
+		break;
+	case COMEDI_INSNLIST:
+		rc = do_insnlist_ioctl(dev, (void *)arg, file);
+		break;
+	case COMEDI_INSN:
+		rc = do_insn_ioctl(dev, (void *)arg, file);
+		break;
+	case COMEDI_POLL:
+		rc = do_poll_ioctl(dev, arg, file);
+		break;
+	default:
+		rc = -ENOTTY;
+		break;
+	}
+
+done:
+	mutex_unlock(&dev->mutex);
+	return rc;
+}
+
+/*
+	COMEDI_DEVCONFIG
+	device config ioctl
+
+	arg:
+		pointer to devconfig structure
+
+	reads:
+		devconfig structure at arg
+
+	writes:
+		none
+*/
+static int do_devconfig_ioctl(comedi_device *dev, comedi_devconfig *arg)
+{
+	comedi_devconfig it;
+	int ret;
+	unsigned char *aux_data = NULL;
+	int aux_len;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	if (arg == NULL) {
+		if (is_device_busy(dev))
+			return -EBUSY;
+		if (dev->attached) {
+			struct module *driver_module = dev->driver->module;
+			comedi_device_detach(dev);
+			module_put(driver_module);
+		}
+		return 0;
+	}
+
+	if (copy_from_user(&it, arg, sizeof(comedi_devconfig)))
+		return -EFAULT;
+
+	it.board_name[COMEDI_NAMELEN - 1] = 0;
+
+	if (comedi_aux_data(it.options, 0) &&
+	    it.options[COMEDI_DEVCONF_AUX_DATA_LENGTH]) {
+		int bit_shift;
+		aux_len = it.options[COMEDI_DEVCONF_AUX_DATA_LENGTH];
+		if (aux_len < 0)
+			return -EFAULT;
+
+		aux_data = vmalloc(aux_len);
+		if (!aux_data)
+			return -ENOMEM;
+
+		if (copy_from_user(aux_data,
+				   comedi_aux_data(it.options, 0), aux_len)) {
+			vfree(aux_data);
+			return -EFAULT;
+		}
+		it.options[COMEDI_DEVCONF_AUX_DATA_LO] =
+		    (unsigned long)aux_data;
+		if (sizeof(void *) > sizeof(int)) {
+			bit_shift = sizeof(int) * 8;
+			it.options[COMEDI_DEVCONF_AUX_DATA_HI] =
+			    ((unsigned long)aux_data) >> bit_shift;
+		} else
+			it.options[COMEDI_DEVCONF_AUX_DATA_HI] = 0;
+	}
+
+	ret = comedi_device_attach(dev, &it);
+	if (ret == 0) {
+		if (!try_module_get(dev->driver->module)) {
+			comedi_device_detach(dev);
+			return -ENOSYS;
+		}
+	}
+
+	if (aux_data)
+		vfree(aux_data);
+
+	return ret;
+}
+
+/*
+	COMEDI_BUFCONFIG
+	buffer configuration ioctl
+
+	arg:
+		pointer to bufconfig structure
+
+	reads:
+		bufconfig at arg
+
+	writes:
+		modified bufconfig at arg
+
+*/
+static int do_bufconfig_ioctl(comedi_device *dev, void *arg)
+{
+	comedi_bufconfig bc;
+	comedi_async *async;
+	comedi_subdevice *s;
+	int ret = 0;
+
+	if (copy_from_user(&bc, arg, sizeof(comedi_bufconfig)))
+		return -EFAULT;
+
+	if (bc.subdevice >= dev->n_subdevices || bc.subdevice < 0)
+		return -EINVAL;
+
+	s = dev->subdevices + bc.subdevice;
+	async = s->async;
+
+	if (!async) {
+		DPRINTK("subdevice does not have async capability\n");
+		bc.size = 0;
+		bc.maximum_size = 0;
+		goto copyback;
+	}
+
+	if (bc.maximum_size) {
+		if (!capable(CAP_SYS_ADMIN))
+			return -EPERM;
+
+		async->max_bufsize = bc.maximum_size;
+	}
+
+	if (bc.size) {
+		if (bc.size > async->max_bufsize)
+			return -EPERM;
+
+		if (s->busy) {
+			DPRINTK("subdevice is busy, cannot resize buffer\n");
+			return -EBUSY;
+		}
+		if (async->mmap_count) {
+			DPRINTK("subdevice is mmapped, cannot resize buffer\n");
+			return -EBUSY;
+		}
+
+		if (!async->prealloc_buf)
+			return -EINVAL;
+
+		/* make sure buffer is an integral number of pages
+		 * (we round up) */
+		bc.size = (bc.size + PAGE_SIZE - 1) & PAGE_MASK;
+
+		ret = comedi_buf_alloc(dev, s, bc.size);
+		if (ret < 0)
+			return ret;
+
+		if (s->buf_change) {
+			ret = s->buf_change(dev, s, bc.size);
+			if (ret < 0)
+				return ret;
+		}
+
+		DPRINTK("comedi%i subd %d buffer resized to %i bytes\n",
+			dev->minor, bc.subdevice, async->prealloc_bufsz);
+	}
+
+	bc.size = async->prealloc_bufsz;
+	bc.maximum_size = async->max_bufsize;
+
+copyback:
+	if (copy_to_user(arg, &bc, sizeof(comedi_bufconfig)))
+		return -EFAULT;
+
+	return 0;
+}
+
+/*
+	COMEDI_DEVINFO
+	device info ioctl
+
+	arg:
+		pointer to devinfo structure
+
+	reads:
+		none
+
+	writes:
+		devinfo structure
+
+*/
+static int do_devinfo_ioctl(comedi_device *dev, comedi_devinfo *arg,
+			    struct file *file)
+{
+	comedi_devinfo devinfo;
+	const unsigned minor = iminor(file->f_dentry->d_inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_subdevice *read_subdev =
+	    comedi_get_read_subdevice(dev_file_info);
+	comedi_subdevice *write_subdev =
+	    comedi_get_write_subdevice(dev_file_info);
+
+	memset(&devinfo, 0, sizeof(devinfo));
+
+	/* fill devinfo structure */
+	devinfo.version_code = COMEDI_VERSION_CODE;
+	devinfo.n_subdevs = dev->n_subdevices;
+	memcpy(devinfo.driver_name, dev->driver->driver_name, COMEDI_NAMELEN);
+	memcpy(devinfo.board_name, dev->board_name, COMEDI_NAMELEN);
+
+	if (read_subdev)
+		devinfo.read_subdevice = read_subdev - dev->subdevices;
+	else
+		devinfo.read_subdevice = -1;
+
+	if (write_subdev)
+		devinfo.write_subdevice = write_subdev - dev->subdevices;
+	else
+		devinfo.write_subdevice = -1;
+
+	if (copy_to_user(arg, &devinfo, sizeof(comedi_devinfo)))
+		return -EFAULT;
+
+	return 0;
+}
+
+/*
+	COMEDI_SUBDINFO
+	subdevice info ioctl
+
+	arg:
+		pointer to array of subdevice info structures
+
+	reads:
+		none
+
+	writes:
+		array of subdevice info structures at arg
+
+*/
+static int do_subdinfo_ioctl(comedi_device *dev, comedi_subdinfo *arg,
+			     void *file)
+{
+	int ret, i;
+	comedi_subdinfo *tmp, *us;
+	comedi_subdevice *s;
+
+	tmp = kcalloc(dev->n_subdevices, sizeof(comedi_subdinfo), GFP_KERNEL);
+	if (!tmp)
+		return -ENOMEM;
+
+	/* fill subdinfo structs */
+	for (i = 0; i < dev->n_subdevices; i++) {
+		s = dev->subdevices + i;
+		us = tmp + i;
+
+		us->type = s->type;
+		us->n_chan = s->n_chan;
+		us->subd_flags = s->subdev_flags;
+		if (comedi_get_subdevice_runflags(s) & SRF_RUNNING)
+			us->subd_flags |= SDF_RUNNING;
+#define TIMER_nanosec 5		/* backwards compatibility */
+		us->timer_type = TIMER_nanosec;
+		us->len_chanlist = s->len_chanlist;
+		us->maxdata = s->maxdata;
+		if (s->range_table) {
+			us->range_type =
+			    (i << 24) | (0 << 16) | (s->range_table->length);
+		} else {
+			us->range_type = 0;	/* XXX */
+		}
+		us->flags = s->flags;
+
+		if (s->busy)
+			us->subd_flags |= SDF_BUSY;
+		if (s->busy == file)
+			us->subd_flags |= SDF_BUSY_OWNER;
+		if (s->lock)
+			us->subd_flags |= SDF_LOCKED;
+		if (s->lock == file)
+			us->subd_flags |= SDF_LOCK_OWNER;
+		if (!s->maxdata && s->maxdata_list)
+			us->subd_flags |= SDF_MAXDATA;
+		if (s->flaglist)
+			us->subd_flags |= SDF_FLAGS;
+		if (s->range_table_list)
+			us->subd_flags |= SDF_RANGETYPE;
+		if (s->do_cmd)
+			us->subd_flags |= SDF_CMD;
+
+		if (s->insn_bits != &insn_inval)
+			us->insn_bits_support = COMEDI_SUPPORTED;
+		else
+			us->insn_bits_support = COMEDI_UNSUPPORTED;
+
+		us->settling_time_0 = s->settling_time_0;
+	}
+
+	ret = copy_to_user(arg, tmp,
+			   dev->n_subdevices * sizeof(comedi_subdinfo));
+
+	kfree(tmp);
+
+	return ret ? -EFAULT : 0;
+}
+
+/*
+	COMEDI_CHANINFO
+	subdevice info ioctl
+
+	arg:
+		pointer to chaninfo structure
+
+	reads:
+		chaninfo structure at arg
+
+	writes:
+		arrays at elements of chaninfo structure
+
+*/
+static int do_chaninfo_ioctl(comedi_device *dev, comedi_chaninfo *arg)
+{
+	comedi_subdevice *s;
+	comedi_chaninfo it;
+
+	if (copy_from_user(&it, arg, sizeof(comedi_chaninfo)))
+		return -EFAULT;
+
+	if (it.subdev >= dev->n_subdevices)
+		return -EINVAL;
+	s = dev->subdevices + it.subdev;
+
+	if (it.maxdata_list) {
+		if (s->maxdata || !s->maxdata_list)
+			return -EINVAL;
+		if (copy_to_user(it.maxdata_list, s->maxdata_list,
+				 s->n_chan * sizeof(lsampl_t)))
+			return -EFAULT;
+	}
+
+	if (it.flaglist) {
+		if (!s->flaglist)
+			return -EINVAL;
+		if (copy_to_user(it.flaglist, s->flaglist,
+				 s->n_chan * sizeof(unsigned int)))
+			return -EFAULT;
+	}
+
+	if (it.rangelist) {
+		int i;
+
+		if (!s->range_table_list)
+			return -EINVAL;
+		for (i = 0; i < s->n_chan; i++) {
+			int x;
+
+			x = (dev->minor << 28) | (it.subdev << 24) | (i << 16) |
+			    (s->range_table_list[i]->length);
+			put_user(x, it.rangelist + i);
+		}
+#if 0
+		if (copy_to_user(it.rangelist, s->range_type_list,
+				 s->n_chan*sizeof(unsigned int)))
+			return -EFAULT;
+#endif
+	}
+
+	return 0;
+}
+
+ /*
+    COMEDI_BUFINFO
+    buffer information ioctl
+
+    arg:
+    pointer to bufinfo structure
+
+    reads:
+    bufinfo at arg
+
+    writes:
+    modified bufinfo at arg
+
+  */
+static int do_bufinfo_ioctl(comedi_device *dev, void *arg)
+{
+	comedi_bufinfo bi;
+	comedi_subdevice *s;
+	comedi_async *async;
+
+	if (copy_from_user(&bi, arg, sizeof(comedi_bufinfo)))
+		return -EFAULT;
+
+	if (bi.subdevice >= dev->n_subdevices || bi.subdevice < 0)
+		return -EINVAL;
+
+	s = dev->subdevices + bi.subdevice;
+	async = s->async;
+
+	if (!async) {
+		DPRINTK("subdevice does not have async capability\n");
+		bi.buf_write_ptr = 0;
+		bi.buf_read_ptr = 0;
+		bi.buf_write_count = 0;
+		bi.buf_read_count = 0;
+		goto copyback;
+	}
+
+	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);
+
+		if (!(comedi_get_subdevice_runflags(s) & (SRF_ERROR |
+							  SRF_RUNNING))
+		    && async->buf_write_count == async->buf_read_count) {
+			do_become_nonbusy(dev, s);
+		}
+	}
+
+	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);
+	}
+
+	bi.buf_write_count = async->buf_write_count;
+	bi.buf_write_ptr = async->buf_write_ptr;
+	bi.buf_read_count = async->buf_read_count;
+	bi.buf_read_ptr = async->buf_read_ptr;
+
+copyback:
+	if (copy_to_user(arg, &bi, sizeof(comedi_bufinfo)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static int parse_insn(comedi_device *dev, comedi_insn *insn, lsampl_t *data,
+		      void *file);
+/*
+ * 	COMEDI_INSNLIST
+ * 	synchronous instructions
+ *
+ * 	arg:
+ * 		pointer to sync cmd structure
+ *
+ * 	reads:
+ * 		sync cmd struct at arg
+ * 		instruction list
+ * 		data (for writes)
+ *
+ * 	writes:
+ * 		data (for reads)
+ */
+/* arbitrary limits */
+#define MAX_SAMPLES 256
+static int do_insnlist_ioctl(comedi_device *dev, void *arg, void *file)
+{
+	comedi_insnlist insnlist;
+	comedi_insn *insns = NULL;
+	lsampl_t *data = NULL;
+	int i = 0;
+	int ret = 0;
+
+	if (copy_from_user(&insnlist, arg, sizeof(comedi_insnlist)))
+		return -EFAULT;
+
+	data = kmalloc(sizeof(lsampl_t) * MAX_SAMPLES, GFP_KERNEL);
+	if (!data) {
+		DPRINTK("kmalloc failed\n");
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	insns = kmalloc(sizeof(comedi_insn) * insnlist.n_insns, GFP_KERNEL);
+	if (!insns) {
+		DPRINTK("kmalloc failed\n");
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	if (copy_from_user(insns, insnlist.insns,
+			   sizeof(comedi_insn) * insnlist.n_insns)) {
+		DPRINTK("copy_from_user failed\n");
+		ret = -EFAULT;
+		goto error;
+	}
+
+	for (i = 0; i < insnlist.n_insns; i++) {
+		if (insns[i].n > MAX_SAMPLES) {
+			DPRINTK("number of samples too large\n");
+			ret = -EINVAL;
+			goto error;
+		}
+		if (insns[i].insn & INSN_MASK_WRITE) {
+			if (copy_from_user(data, insns[i].data,
+					   insns[i].n * sizeof(lsampl_t))) {
+				DPRINTK("copy_from_user failed\n");
+				ret = -EFAULT;
+				goto error;
+			}
+		}
+		ret = parse_insn(dev, insns + i, data, file);
+		if (ret < 0)
+			goto error;
+		if (insns[i].insn & INSN_MASK_READ) {
+			if (copy_to_user(insns[i].data, data,
+					 insns[i].n * sizeof(lsampl_t))) {
+				DPRINTK("copy_to_user failed\n");
+				ret = -EFAULT;
+				goto error;
+			}
+		}
+		if (need_resched())
+			schedule();
+	}
+
+error:
+	kfree(insns);
+	kfree(data);
+
+	if (ret < 0)
+		return ret;
+	return i;
+}
+
+static int check_insn_config_length(comedi_insn *insn, lsampl_t *data)
+{
+	if (insn->n < 1)
+		return -EINVAL;
+
+	switch (data[0]) {
+	case INSN_CONFIG_DIO_OUTPUT:
+	case INSN_CONFIG_DIO_INPUT:
+	case INSN_CONFIG_DISARM:
+	case INSN_CONFIG_RESET:
+		if (insn->n == 1)
+			return 0;
+		break;
+	case INSN_CONFIG_ARM:
+	case INSN_CONFIG_DIO_QUERY:
+	case INSN_CONFIG_BLOCK_SIZE:
+	case INSN_CONFIG_FILTER:
+	case INSN_CONFIG_SERIAL_CLOCK:
+	case INSN_CONFIG_BIDIRECTIONAL_DATA:
+	case INSN_CONFIG_ALT_SOURCE:
+	case INSN_CONFIG_SET_COUNTER_MODE:
+	case INSN_CONFIG_8254_READ_STATUS:
+	case INSN_CONFIG_SET_ROUTING:
+	case INSN_CONFIG_GET_ROUTING:
+	case INSN_CONFIG_GET_PWM_STATUS:
+	case INSN_CONFIG_PWM_SET_PERIOD:
+	case INSN_CONFIG_PWM_GET_PERIOD:
+		if (insn->n == 2)
+			return 0;
+		break;
+	case INSN_CONFIG_SET_GATE_SRC:
+	case INSN_CONFIG_GET_GATE_SRC:
+	case INSN_CONFIG_SET_CLOCK_SRC:
+	case INSN_CONFIG_GET_CLOCK_SRC:
+	case INSN_CONFIG_SET_OTHER_SRC:
+	case INSN_CONFIG_GET_COUNTER_STATUS:
+	case INSN_CONFIG_PWM_SET_H_BRIDGE:
+	case INSN_CONFIG_PWM_GET_H_BRIDGE:
+	case INSN_CONFIG_GET_HARDWARE_BUFFER_SIZE:
+		if (insn->n == 3)
+			return 0;
+		break;
+	case INSN_CONFIG_PWM_OUTPUT:
+	case INSN_CONFIG_ANALOG_TRIG:
+		if (insn->n == 5)
+			return 0;
+		break;
+	/* by default we allow the insn since we don't have checks for
+	 * all possible cases yet */
+	default:
+		rt_printk("comedi: no check for data length of config insn id "
+			  "%i is implemented.\n"
+			  " Add a check to %s in %s.\n"
+			  " Assuming n=%i is correct.\n", data[0], __func__,
+			  __FILE__, insn->n);
+		return 0;
+		break;
+	}
+	return -EINVAL;
+}
+
+static int parse_insn(comedi_device *dev, comedi_insn *insn, lsampl_t *data,
+		      void *file)
+{
+	comedi_subdevice *s;
+	int ret = 0;
+	int i;
+
+	if (insn->insn & INSN_MASK_SPECIAL) {
+		/* a non-subdevice instruction */
+
+		switch (insn->insn) {
+		case INSN_GTOD:
+			{
+				struct timeval tv;
+
+				if (insn->n != 2) {
+					ret = -EINVAL;
+					break;
+				}
+
+				do_gettimeofday(&tv);
+				data[0] = tv.tv_sec;
+				data[1] = tv.tv_usec;
+				ret = 2;
+
+				break;
+			}
+		case INSN_WAIT:
+			if (insn->n != 1 || data[0] >= 100000) {
+				ret = -EINVAL;
+				break;
+			}
+			udelay(data[0] / 1000);
+			ret = 1;
+			break;
+		case INSN_INTTRIG:
+			if (insn->n != 1) {
+				ret = -EINVAL;
+				break;
+			}
+			if (insn->subdev >= dev->n_subdevices) {
+				DPRINTK("%d not usable subdevice\n",
+					insn->subdev);
+				ret = -EINVAL;
+				break;
+			}
+			s = dev->subdevices + insn->subdev;
+			if (!s->async) {
+				DPRINTK("no async\n");
+				ret = -EINVAL;
+				break;
+			}
+			if (!s->async->inttrig) {
+				DPRINTK("no inttrig\n");
+				ret = -EAGAIN;
+				break;
+			}
+			ret = s->async->inttrig(dev, s, insn->data[0]);
+			if (ret >= 0)
+				ret = 1;
+			break;
+		default:
+			DPRINTK("invalid insn\n");
+			ret = -EINVAL;
+			break;
+		}
+	} else {
+		/* a subdevice instruction */
+		lsampl_t maxdata;
+
+		if (insn->subdev >= dev->n_subdevices) {
+			DPRINTK("subdevice %d out of range\n", insn->subdev);
+			ret = -EINVAL;
+			goto out;
+		}
+		s = dev->subdevices + insn->subdev;
+
+		if (s->type == COMEDI_SUBD_UNUSED) {
+			DPRINTK("%d not usable subdevice\n", insn->subdev);
+			ret = -EIO;
+			goto out;
+		}
+
+		/* are we locked? (ioctl lock) */
+		if (s->lock && s->lock != file) {
+			DPRINTK("device locked\n");
+			ret = -EACCES;
+			goto out;
+		}
+
+		ret = check_chanlist(s, 1, &insn->chanspec);
+		if (ret < 0) {
+			ret = -EINVAL;
+			DPRINTK("bad chanspec\n");
+			goto out;
+		}
+
+		if (s->busy) {
+			ret = -EBUSY;
+			goto out;
+		}
+		/* This looks arbitrary.  It is. */
+		s->busy = &parse_insn;
+		switch (insn->insn) {
+		case INSN_READ:
+			ret = s->insn_read(dev, s, insn, data);
+			break;
+		case INSN_WRITE:
+			maxdata = s->maxdata_list
+			    ? s->maxdata_list[CR_CHAN(insn->chanspec)]
+			    : s->maxdata;
+			for (i = 0; i < insn->n; ++i) {
+				if (data[i] > maxdata) {
+					ret = -EINVAL;
+					DPRINTK("bad data value(s)\n");
+					break;
+				}
+			}
+			if (ret == 0)
+				ret = s->insn_write(dev, s, insn, data);
+			break;
+		case INSN_BITS:
+			if (insn->n != 2) {
+				ret = -EINVAL;
+				break;
+			}
+			ret = s->insn_bits(dev, s, insn, data);
+			break;
+		case INSN_CONFIG:
+			ret = check_insn_config_length(insn, data);
+			if (ret)
+				break;
+			ret = s->insn_config(dev, s, insn, data);
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		s->busy = NULL;
+	}
+
+out:
+	return ret;
+}
+
+/*
+ * 	COMEDI_INSN
+ * 	synchronous instructions
+ *
+ * 	arg:
+ * 		pointer to insn
+ *
+ * 	reads:
+ * 		comedi_insn struct at arg
+ * 		data (for writes)
+ *
+ * 	writes:
+ * 		data (for reads)
+ */
+static int do_insn_ioctl(comedi_device *dev, void *arg, void *file)
+{
+	comedi_insn insn;
+	lsampl_t *data = NULL;
+	int ret = 0;
+
+	data = kmalloc(sizeof(lsampl_t) * MAX_SAMPLES, GFP_KERNEL);
+	if (!data) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	if (copy_from_user(&insn, arg, sizeof(comedi_insn))) {
+		ret = -EFAULT;
+		goto error;
+	}
+
+	/* This is where the behavior of insn and insnlist deviate. */
+	if (insn.n > MAX_SAMPLES)
+		insn.n = MAX_SAMPLES;
+	if (insn.insn & INSN_MASK_WRITE) {
+		if (copy_from_user(data, insn.data, insn.n * sizeof(lsampl_t))) {
+			ret = -EFAULT;
+			goto error;
+		}
+	}
+	ret = parse_insn(dev, &insn, data, file);
+	if (ret < 0)
+		goto error;
+	if (insn.insn & INSN_MASK_READ) {
+		if (copy_to_user(insn.data, data, insn.n * sizeof(lsampl_t))) {
+			ret = -EFAULT;
+			goto error;
+		}
+	}
+	ret = insn.n;
+
+error:
+	kfree(data);
+
+	return ret;
+}
+
+/*
+	COMEDI_CMD
+	command ioctl
+
+	arg:
+		pointer to cmd structure
+
+	reads:
+		cmd structure at arg
+		channel/range list
+
+	writes:
+		modified cmd structure at arg
+
+*/
+static int do_cmd_ioctl(comedi_device *dev, void *arg, void *file)
+{
+	comedi_cmd user_cmd;
+	comedi_subdevice *s;
+	comedi_async *async;
+	int ret = 0;
+	unsigned int *chanlist_saver = NULL;
+
+	if (copy_from_user(&user_cmd, arg, sizeof(comedi_cmd))) {
+		DPRINTK("bad cmd address\n");
+		return -EFAULT;
+	}
+	/* save user's chanlist pointer so it can be restored later */
+	chanlist_saver = user_cmd.chanlist;
+
+	if (user_cmd.subdev >= dev->n_subdevices) {
+		DPRINTK("%d no such subdevice\n", user_cmd.subdev);
+		return -ENODEV;
+	}
+
+	s = dev->subdevices + user_cmd.subdev;
+	async = s->async;
+
+	if (s->type == COMEDI_SUBD_UNUSED) {
+		DPRINTK("%d not valid subdevice\n", user_cmd.subdev);
+		return -EIO;
+	}
+
+	if (!s->do_cmd || !s->do_cmdtest || !s->async) {
+		DPRINTK("subdevice %i does not support commands\n",
+			user_cmd.subdev);
+		return -EIO;
+	}
+
+	/* are we locked? (ioctl lock) */
+	if (s->lock && s->lock != file) {
+		DPRINTK("subdevice locked\n");
+		return -EACCES;
+	}
+
+	/* are we busy? */
+	if (s->busy) {
+		DPRINTK("subdevice busy\n");
+		return -EBUSY;
+	}
+	s->busy = file;
+
+	/* make sure channel/gain list isn't too long */
+	if (user_cmd.chanlist_len > s->len_chanlist) {
+		DPRINTK("channel/gain list too long %u > %d\n",
+			user_cmd.chanlist_len, s->len_chanlist);
+		ret = -EINVAL;
+		goto cleanup;
+	}
+
+	/* make sure channel/gain list isn't too short */
+	if (user_cmd.chanlist_len < 1) {
+		DPRINTK("channel/gain list too short %u < 1\n",
+			user_cmd.chanlist_len);
+		ret = -EINVAL;
+		goto cleanup;
+	}
+
+	kfree(async->cmd.chanlist);
+	async->cmd = user_cmd;
+	async->cmd.data = NULL;
+	/* load channel/gain list */
+	async->cmd.chanlist =
+	    kmalloc(async->cmd.chanlist_len * sizeof(int), GFP_KERNEL);
+	if (!async->cmd.chanlist) {
+		DPRINTK("allocation failed\n");
+		ret = -ENOMEM;
+		goto cleanup;
+	}
+
+	if (copy_from_user(async->cmd.chanlist, user_cmd.chanlist,
+			   async->cmd.chanlist_len * sizeof(int))) {
+		DPRINTK("fault reading chanlist\n");
+		ret = -EFAULT;
+		goto cleanup;
+	}
+
+	/* make sure each element in channel/gain list is valid */
+	ret = check_chanlist(s, async->cmd.chanlist_len, async->cmd.chanlist);
+	if (ret < 0) {
+		DPRINTK("bad chanlist\n");
+		goto cleanup;
+	}
+
+	ret = s->do_cmdtest(dev, s, &async->cmd);
+
+	if (async->cmd.flags & TRIG_BOGUS || ret) {
+		DPRINTK("test returned %d\n", ret);
+		user_cmd = async->cmd;
+		/* restore chanlist pointer before copying back */
+		user_cmd.chanlist = chanlist_saver;
+		user_cmd.data = NULL;
+		if (copy_to_user(arg, &user_cmd, sizeof(comedi_cmd))) {
+			DPRINTK("fault writing cmd\n");
+			ret = -EFAULT;
+			goto cleanup;
+		}
+		ret = -EAGAIN;
+		goto cleanup;
+	}
+
+	if (!async->prealloc_bufsz) {
+		ret = -ENOMEM;
+		DPRINTK("no buffer (?)\n");
+		goto cleanup;
+	}
+
+	comedi_reset_async_buf(async);
+
+	async->cb_mask =
+	    COMEDI_CB_EOA | COMEDI_CB_BLOCK | COMEDI_CB_ERROR |
+	    COMEDI_CB_OVERFLOW;
+	if (async->cmd.flags & TRIG_WAKE_EOS)
+		async->cb_mask |= COMEDI_CB_EOS;
+
+	comedi_set_subdevice_runflags(s, ~0, SRF_USER | SRF_RUNNING);
+
+#ifdef CONFIG_COMEDI_RT
+	if (async->cmd.flags & TRIG_RT) {
+		if (comedi_switch_to_rt(dev) == 0)
+			comedi_set_subdevice_runflags(s, SRF_RT, SRF_RT);
+	}
+#endif
+
+	ret = s->do_cmd(dev, s);
+	if (ret == 0)
+		return 0;
+
+cleanup:
+	do_become_nonbusy(dev, s);
+
+	return ret;
+}
+
+/*
+	COMEDI_CMDTEST
+	command testing ioctl
+
+	arg:
+		pointer to cmd structure
+
+	reads:
+		cmd structure at arg
+		channel/range list
+
+	writes:
+		modified cmd structure at arg
+
+*/
+static int do_cmdtest_ioctl(comedi_device *dev, void *arg, void *file)
+{
+	comedi_cmd user_cmd;
+	comedi_subdevice *s;
+	int ret = 0;
+	unsigned int *chanlist = NULL;
+	unsigned int *chanlist_saver = NULL;
+
+	if (copy_from_user(&user_cmd, arg, sizeof(comedi_cmd))) {
+		DPRINTK("bad cmd address\n");
+		return -EFAULT;
+	}
+	/* save user's chanlist pointer so it can be restored later */
+	chanlist_saver = user_cmd.chanlist;
+
+	if (user_cmd.subdev >= dev->n_subdevices) {
+		DPRINTK("%d no such subdevice\n", user_cmd.subdev);
+		return -ENODEV;
+	}
+
+	s = dev->subdevices + user_cmd.subdev;
+	if (s->type == COMEDI_SUBD_UNUSED) {
+		DPRINTK("%d not valid subdevice\n", user_cmd.subdev);
+		return -EIO;
+	}
+
+	if (!s->do_cmd || !s->do_cmdtest) {
+		DPRINTK("subdevice %i does not support commands\n",
+			user_cmd.subdev);
+		return -EIO;
+	}
+
+	/* make sure channel/gain list isn't too long */
+	if (user_cmd.chanlist_len > s->len_chanlist) {
+		DPRINTK("channel/gain list too long %d > %d\n",
+			user_cmd.chanlist_len, s->len_chanlist);
+		ret = -EINVAL;
+		goto cleanup;
+	}
+
+	/* load channel/gain list */
+	if (user_cmd.chanlist) {
+		chanlist =
+		    kmalloc(user_cmd.chanlist_len * sizeof(int), GFP_KERNEL);
+		if (!chanlist) {
+			DPRINTK("allocation failed\n");
+			ret = -ENOMEM;
+			goto cleanup;
+		}
+
+		if (copy_from_user(chanlist, user_cmd.chanlist,
+				   user_cmd.chanlist_len * sizeof(int))) {
+			DPRINTK("fault reading chanlist\n");
+			ret = -EFAULT;
+			goto cleanup;
+		}
+
+		/* make sure each element in channel/gain list is valid */
+		ret = check_chanlist(s, user_cmd.chanlist_len, chanlist);
+		if (ret < 0) {
+			DPRINTK("bad chanlist\n");
+			goto cleanup;
+		}
+
+		user_cmd.chanlist = chanlist;
+	}
+
+	ret = s->do_cmdtest(dev, s, &user_cmd);
+
+	/* restore chanlist pointer before copying back */
+	user_cmd.chanlist = chanlist_saver;
+
+	if (copy_to_user(arg, &user_cmd, sizeof(comedi_cmd))) {
+		DPRINTK("bad cmd address\n");
+		ret = -EFAULT;
+		goto cleanup;
+	}
+cleanup:
+	kfree(chanlist);
+
+	return ret;
+}
+
+/*
+	COMEDI_LOCK
+	lock subdevice
+
+	arg:
+		subdevice number
+
+	reads:
+		none
+
+	writes:
+		none
+
+*/
+
+static int do_lock_ioctl(comedi_device *dev, unsigned int arg, void *file)
+{
+	int ret = 0;
+	unsigned long flags;
+	comedi_subdevice *s;
+
+	if (arg >= dev->n_subdevices)
+		return -EINVAL;
+	s = dev->subdevices + arg;
+
+	comedi_spin_lock_irqsave(&s->spin_lock, flags);
+	if (s->busy || s->lock)
+		ret = -EBUSY;
+	else
+		s->lock = file;
+	comedi_spin_unlock_irqrestore(&s->spin_lock, flags);
+
+	if (ret < 0)
+		return ret;
+
+#if 0
+	if (s->lock_f)
+		ret = s->lock_f(dev, s);
+#endif
+
+	return ret;
+}
+
+/*
+	COMEDI_UNLOCK
+	unlock subdevice
+
+	arg:
+		subdevice number
+
+	reads:
+		none
+
+	writes:
+		none
+
+	This function isn't protected by the semaphore, since
+	we already own the lock.
+*/
+static int do_unlock_ioctl(comedi_device *dev, unsigned int arg, void *file)
+{
+	comedi_subdevice *s;
+
+	if (arg >= dev->n_subdevices)
+		return -EINVAL;
+	s = dev->subdevices + arg;
+
+	if (s->busy)
+		return -EBUSY;
+
+	if (s->lock && s->lock != file)
+		return -EACCES;
+
+	if (s->lock == file) {
+#if 0
+		if (s->unlock)
+			s->unlock(dev, s);
+#endif
+
+		s->lock = NULL;
+	}
+
+	return 0;
+}
+
+/*
+	COMEDI_CANCEL
+	cancel acquisition ioctl
+
+	arg:
+		subdevice number
+
+	reads:
+		nothing
+
+	writes:
+		nothing
+
+*/
+static int do_cancel_ioctl(comedi_device *dev, unsigned int arg, void *file)
+{
+	comedi_subdevice *s;
+
+	if (arg >= dev->n_subdevices)
+		return -EINVAL;
+	s = dev->subdevices + arg;
+	if (s->async == NULL)
+		return -EINVAL;
+
+	if (s->lock && s->lock != file)
+		return -EACCES;
+
+	if (!s->busy)
+		return 0;
+
+	if (s->busy != file)
+		return -EBUSY;
+
+	return do_cancel(dev, s);
+}
+
+/*
+	COMEDI_POLL ioctl
+	instructs driver to synchronize buffers
+
+	arg:
+		subdevice number
+
+	reads:
+		nothing
+
+	writes:
+		nothing
+
+*/
+static int do_poll_ioctl(comedi_device *dev, unsigned int arg, void *file)
+{
+	comedi_subdevice *s;
+
+	if (arg >= dev->n_subdevices)
+		return -EINVAL;
+	s = dev->subdevices + arg;
+
+	if (s->lock && s->lock != file)
+		return -EACCES;
+
+	if (!s->busy)
+		return 0;
+
+	if (s->busy != file)
+		return -EBUSY;
+
+	if (s->poll)
+		return s->poll(dev, s);
+
+	return -EINVAL;
+}
+
+static int do_cancel(comedi_device *dev, comedi_subdevice *s)
+{
+	int ret = 0;
+
+	if ((comedi_get_subdevice_runflags(s) & SRF_RUNNING) && s->cancel)
+		ret = s->cancel(dev, s);
+
+	do_become_nonbusy(dev, s);
+
+	return ret;
+}
+
+void comedi_unmap(struct vm_area_struct *area)
+{
+	comedi_async *async;
+	comedi_device *dev;
+
+	async = area->vm_private_data;
+	dev = async->subdevice->device;
+
+	mutex_lock(&dev->mutex);
+	async->mmap_count--;
+	mutex_unlock(&dev->mutex);
+}
+
+static struct vm_operations_struct comedi_vm_ops = {
+	.close =	comedi_unmap,
+};
+
+static int comedi_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	const unsigned minor = iminor(file->f_dentry->d_inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_device *dev = dev_file_info->device;
+	comedi_async *async = NULL;
+	unsigned long start = vma->vm_start;
+	unsigned long size;
+	int n_pages;
+	int i;
+	int retval;
+	comedi_subdevice *s;
+
+	mutex_lock(&dev->mutex);
+	if (!dev->attached) {
+		DPRINTK("no driver configured on comedi%i\n", dev->minor);
+		retval = -ENODEV;
+		goto done;
+	}
+	if (vma->vm_flags & VM_WRITE)
+		s = comedi_get_write_subdevice(dev_file_info);
+	else
+		s = comedi_get_read_subdevice(dev_file_info);
+
+	if (s == NULL) {
+		retval = -EINVAL;
+		goto done;
+	}
+	async = s->async;
+	if (async == NULL) {
+		retval = -EINVAL;
+		goto done;
+	}
+
+	if (vma->vm_pgoff != 0) {
+		DPRINTK("comedi: mmap() offset must be 0.\n");
+		retval = -EINVAL;
+		goto done;
+	}
+
+	size = vma->vm_end - vma->vm_start;
+	if (size > async->prealloc_bufsz) {
+		retval = -EFAULT;
+		goto done;
+	}
+	if (size & (~PAGE_MASK)) {
+		retval = -EFAULT;
+		goto done;
+	}
+
+	n_pages = size >> PAGE_SHIFT;
+	for (i = 0; i < n_pages; ++i) {
+		if (remap_pfn_range(vma, start,
+				    page_to_pfn(virt_to_page(async->
+							     buf_page_list[i].
+							     virt_addr)),
+				    PAGE_SIZE, PAGE_SHARED)) {
+			retval = -EAGAIN;
+			goto done;
+		}
+		start += PAGE_SIZE;
+	}
+
+	vma->vm_ops = &comedi_vm_ops;
+	vma->vm_private_data = async;
+
+	async->mmap_count++;
+
+	retval = 0;
+done:
+	mutex_unlock(&dev->mutex);
+	return retval;
+}
+
+static unsigned int comedi_poll(struct file *file, poll_table *wait)
+{
+	unsigned int mask = 0;
+	const unsigned minor = iminor(file->f_dentry->d_inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_device *dev = dev_file_info->device;
+	comedi_subdevice *read_subdev;
+	comedi_subdevice *write_subdev;
+
+	mutex_lock(&dev->mutex);
+	if (!dev->attached) {
+		DPRINTK("no driver configured on comedi%i\n", dev->minor);
+		mutex_unlock(&dev->mutex);
+		return 0;
+	}
+
+	mask = 0;
+	read_subdev = comedi_get_read_subdevice(dev_file_info);
+	if (read_subdev) {
+		poll_wait(file, &read_subdev->async->wait_head, wait);
+		if (!read_subdev->busy
+		    || comedi_buf_read_n_available(read_subdev->async) > 0
+		    || !(comedi_get_subdevice_runflags(read_subdev) &
+			 SRF_RUNNING)) {
+			mask |= POLLIN | POLLRDNORM;
+		}
+	}
+	write_subdev = comedi_get_write_subdevice(dev_file_info);
+	if (write_subdev) {
+		poll_wait(file, &write_subdev->async->wait_head, wait);
+		comedi_buf_write_alloc(write_subdev->async,
+				       write_subdev->async->prealloc_bufsz);
+		if (!write_subdev->busy
+		    || !(comedi_get_subdevice_runflags(write_subdev) &
+			 SRF_RUNNING)
+		    || comedi_buf_write_n_allocated(write_subdev->async) >=
+		    bytes_per_sample(write_subdev->async->subdevice)) {
+			mask |= POLLOUT | POLLWRNORM;
+		}
+	}
+
+	mutex_unlock(&dev->mutex);
+	return mask;
+}
+
+static ssize_t comedi_write(struct file *file, const char *buf, size_t nbytes,
+			    loff_t *offset)
+{
+	comedi_subdevice *s;
+	comedi_async *async;
+	int n, m, count = 0, retval = 0;
+	DECLARE_WAITQUEUE(wait, current);
+	const unsigned minor = iminor(file->f_dentry->d_inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_device *dev = dev_file_info->device;
+
+	if (!dev->attached) {
+		DPRINTK("no driver configured on comedi%i\n", dev->minor);
+		retval = -ENODEV;
+		goto done;
+	}
+
+	s = comedi_get_write_subdevice(dev_file_info);
+	if (s == NULL) {
+		retval = -EIO;
+		goto done;
+	}
+	async = s->async;
+
+	if (!nbytes) {
+		retval = 0;
+		goto done;
+	}
+	if (!s->busy) {
+		retval = 0;
+		goto done;
+	}
+	if (s->busy != file) {
+		retval = -EACCES;
+		goto done;
+	}
+	add_wait_queue(&async->wait_head, &wait);
+	while (nbytes > 0 && !retval) {
+		set_current_state(TASK_INTERRUPTIBLE);
+
+		n = nbytes;
+
+		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);
+		if (m < n)
+			n = m;
+
+		if (n == 0) {
+			if (!(comedi_get_subdevice_runflags(s) & SRF_RUNNING)) {
+				if (comedi_get_subdevice_runflags(s) &
+				    SRF_ERROR) {
+					retval = -EPIPE;
+				} else {
+					retval = 0;
+				}
+				do_become_nonbusy(dev, s);
+				break;
+			}
+			if (file->f_flags & O_NONBLOCK) {
+				retval = -EAGAIN;
+				break;
+			}
+			if (signal_pending(current)) {
+				retval = -ERESTARTSYS;
+				break;
+			}
+			schedule();
+			if (!s->busy)
+				break;
+			if (s->busy != file) {
+				retval = -EACCES;
+				break;
+			}
+			continue;
+		}
+
+		m = copy_from_user(async->prealloc_buf + async->buf_write_ptr,
+				   buf, n);
+		if (m) {
+			n -= m;
+			retval = -EFAULT;
+		}
+		comedi_buf_write_free(async, n);
+
+		count += n;
+		nbytes -= n;
+
+		buf += n;
+		break;		/* makes device work like a pipe */
+	}
+	set_current_state(TASK_RUNNING);
+	remove_wait_queue(&async->wait_head, &wait);
+
+done:
+	return count ? count : retval;
+}
+
+static ssize_t comedi_read(struct file *file, char *buf, size_t nbytes,
+			   loff_t *offset)
+{
+	comedi_subdevice *s;
+	comedi_async *async;
+	int n, m, count = 0, retval = 0;
+	DECLARE_WAITQUEUE(wait, current);
+	const unsigned minor = iminor(file->f_dentry->d_inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_device *dev = dev_file_info->device;
+
+	if (!dev->attached) {
+		DPRINTK("no driver configured on comedi%i\n", dev->minor);
+		retval = -ENODEV;
+		goto done;
+	}
+
+	s = comedi_get_read_subdevice(dev_file_info);
+	if (s == NULL) {
+		retval = -EIO;
+		goto done;
+	}
+	async = s->async;
+	if (!nbytes) {
+		retval = 0;
+		goto done;
+	}
+	if (!s->busy) {
+		retval = 0;
+		goto done;
+	}
+	if (s->busy != file) {
+		retval = -EACCES;
+		goto done;
+	}
+
+	add_wait_queue(&async->wait_head, &wait);
+	while (nbytes > 0 && !retval) {
+		set_current_state(TASK_INTERRUPTIBLE);
+
+		n = nbytes;
+
+		m = comedi_buf_read_n_available(async);
+		/* printk("%d available\n",m); */
+		if (async->buf_read_ptr + m > async->prealloc_bufsz)
+			m = async->prealloc_bufsz - async->buf_read_ptr;
+		/* printk("%d contiguous\n",m); */
+		if (m < n)
+			n = m;
+
+		if (n == 0) {
+			if (!(comedi_get_subdevice_runflags(s) & SRF_RUNNING)) {
+				do_become_nonbusy(dev, s);
+				if (comedi_get_subdevice_runflags(s) &
+				    SRF_ERROR) {
+					retval = -EPIPE;
+				} else {
+					retval = 0;
+				}
+				break;
+			}
+			if (file->f_flags & O_NONBLOCK) {
+				retval = -EAGAIN;
+				break;
+			}
+			if (signal_pending(current)) {
+				retval = -ERESTARTSYS;
+				break;
+			}
+			schedule();
+			if (!s->busy) {
+				retval = 0;
+				break;
+			}
+			if (s->busy != file) {
+				retval = -EACCES;
+				break;
+			}
+			continue;
+		}
+		m = copy_to_user(buf, async->prealloc_buf +
+				 async->buf_read_ptr, n);
+		if (m) {
+			n -= m;
+			retval = -EFAULT;
+		}
+
+		comedi_buf_read_alloc(async, n);
+		comedi_buf_read_free(async, n);
+
+		count += n;
+		nbytes -= n;
+
+		buf += n;
+		break;		/* makes device work like a pipe */
+	}
+	if (!(comedi_get_subdevice_runflags(s) & (SRF_ERROR | SRF_RUNNING)) &&
+	    async->buf_read_count - async->buf_write_count == 0) {
+		do_become_nonbusy(dev, s);
+	}
+	set_current_state(TASK_RUNNING);
+	remove_wait_queue(&async->wait_head, &wait);
+
+done:
+	return count ? count : retval;
+}
+
+/*
+   This function restores a subdevice to an idle state.
+ */
+void do_become_nonbusy(comedi_device *dev, comedi_subdevice *s)
+{
+	comedi_async *async = s->async;
+
+	comedi_set_subdevice_runflags(s, SRF_RUNNING, 0);
+#ifdef CONFIG_COMEDI_RT
+	if (comedi_get_subdevice_runflags(s) & SRF_RT) {
+		comedi_switch_to_non_rt(dev);
+		comedi_set_subdevice_runflags(s, SRF_RT, 0);
+	}
+#endif
+	if (async) {
+		comedi_reset_async_buf(async);
+		async->inttrig = NULL;
+	} else {
+		printk(KERN_ERR
+		       "BUG: (?) do_become_nonbusy called with async=0\n");
+	}
+
+	s->busy = NULL;
+}
+
+static int comedi_open(struct inode *inode, struct file *file)
+{
+	char mod[32];
+	const unsigned minor = iminor(inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_device *dev = dev_file_info->device;
+	if (dev == NULL) {
+		DPRINTK("invalid minor number\n");
+		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=0 -> autoload
+	 * case: user opens device, unattached, in_request_module=1 -> fail
+	 * case: root opens device, attached -> ok
+	 * case: root opens device, unattached, in_request_module=1 -> ok
+	 *   (typically called from modprobe)
+	 * case: root opens device, unattached, in_request_module=0 -> 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_SYS_MODULE) && dev->in_request_module) {
+		DPRINTK("in request module\n");
+		mutex_unlock(&dev->mutex);
+		return -ENODEV;
+	}
+	if (capable(CAP_SYS_MODULE) && dev->in_request_module)
+		goto ok;
+
+	dev->in_request_module = 1;
+
+	sprintf(mod, "char-major-%i-%i", COMEDI_MAJOR, dev->minor);
+#ifdef CONFIG_KMOD
+	mutex_unlock(&dev->mutex);
+	request_module(mod);
+	mutex_lock(&dev->mutex);
+#endif
+
+	dev->in_request_module = 0;
+
+	if (!dev->attached && !capable(CAP_SYS_MODULE)) {
+		DPRINTK("not attached and not CAP_SYS_MODULE\n");
+		mutex_unlock(&dev->mutex);
+		return -ENODEV;
+	}
+ok:
+	__module_get(THIS_MODULE);
+
+	if (dev->attached) {
+		if (!try_module_get(dev->driver->module)) {
+			module_put(THIS_MODULE);
+			mutex_unlock(&dev->mutex);
+			return -ENOSYS;
+		}
+	}
+
+	if (dev->attached && dev->use_count == 0 && dev->open)
+		dev->open(dev);
+
+	dev->use_count++;
+
+	mutex_unlock(&dev->mutex);
+
+	return 0;
+}
+
+static int comedi_close(struct inode *inode, struct file *file)
+{
+	const unsigned minor = iminor(inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_device *dev = dev_file_info->device;
+	comedi_subdevice *s = NULL;
+	int i;
+
+	mutex_lock(&dev->mutex);
+
+	if (dev->subdevices) {
+		for (i = 0; i < dev->n_subdevices; i++) {
+			s = dev->subdevices + i;
+
+			if (s->busy == file)
+				do_cancel(dev, s);
+			if (s->lock == file)
+				s->lock = NULL;
+		}
+	}
+	if (dev->attached && dev->use_count == 1 && dev->close)
+		dev->close(dev);
+
+	module_put(THIS_MODULE);
+	if (dev->attached)
+		module_put(dev->driver->module);
+
+	dev->use_count--;
+
+	mutex_unlock(&dev->mutex);
+
+	if (file->f_flags & FASYNC)
+		comedi_fasync(-1, file, 0);
+
+	return 0;
+}
+
+static int comedi_fasync(int fd, struct file *file, int on)
+{
+	const unsigned minor = iminor(file->f_dentry->d_inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+
+	comedi_device *dev = dev_file_info->device;
+
+	return fasync_helper(fd, file, on, &dev->async_queue);
+}
+
+const struct file_operations comedi_fops = {
+      .owner =		THIS_MODULE,
+#ifdef HAVE_UNLOCKED_IOCTL
+      .unlocked_ioctl =	comedi_unlocked_ioctl,
+#else
+      .ioctl =		comedi_ioctl,
+#endif
+#ifdef HAVE_COMPAT_IOCTL
+      .compat_ioctl =	comedi_compat_ioctl,
+#endif
+      .open =		comedi_open,
+      .release =	comedi_close,
+      .read =		comedi_read,
+      .write =		comedi_write,
+      .mmap =		comedi_mmap,
+      .poll =		comedi_poll,
+      .fasync =		comedi_fasync,
+};
+
+struct class *comedi_class;
+static struct cdev comedi_cdev;
+
+static void comedi_cleanup_legacy_minors(void)
+{
+	unsigned i;
+
+	for (i = 0; i < COMEDI_NUM_LEGACY_MINORS; i++)
+		comedi_free_board_minor(i);
+}
+
+static int __init comedi_init(void)
+{
+	int i;
+	int retval;
+
+	printk(KERN_INFO "comedi: version " COMEDI_RELEASE
+	       " - http://www.comedi.org\n");
+
+	memset(comedi_file_info_table, 0,
+	       sizeof(struct comedi_device_file_info *) * COMEDI_NUM_MINORS);
+
+	retval = register_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
+					COMEDI_NUM_MINORS, "comedi");
+	if (retval)
+		return -EIO;
+	cdev_init(&comedi_cdev, &comedi_fops);
+	comedi_cdev.owner = THIS_MODULE;
+	kobject_set_name(&comedi_cdev.kobj, "comedi");
+	if (cdev_add(&comedi_cdev, MKDEV(COMEDI_MAJOR, 0), COMEDI_NUM_MINORS)) {
+		unregister_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
+					 COMEDI_NUM_MINORS);
+		return -EIO;
+	}
+	comedi_class = class_create(THIS_MODULE, "comedi");
+	if (IS_ERR(comedi_class)) {
+		printk("comedi: failed to create class");
+		cdev_del(&comedi_cdev);
+		unregister_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
+					 COMEDI_NUM_MINORS);
+		return PTR_ERR(comedi_class);
+	}
+
+	/* XXX requires /proc interface */
+	comedi_proc_init();
+
+	/* create devices files for legacy/manual use */
+	for (i = 0; i < COMEDI_NUM_LEGACY_MINORS; i++) {
+		int minor;
+		minor = comedi_alloc_board_minor(NULL);
+		if (minor < 0) {
+			comedi_cleanup_legacy_minors();
+			cdev_del(&comedi_cdev);
+			unregister_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
+						 COMEDI_NUM_MINORS);
+			return minor;
+		}
+	}
+
+	comedi_rt_init();
+
+	comedi_register_ioctl32();
+
+	return 0;
+}
+
+static void __exit comedi_cleanup(void)
+{
+	int i;
+
+	comedi_cleanup_legacy_minors();
+	for (i = 0; i < COMEDI_NUM_MINORS; ++i)
+		BUG_ON(comedi_file_info_table[i]);
+
+
+	class_destroy(comedi_class);
+	cdev_del(&comedi_cdev);
+	unregister_chrdev_region(MKDEV(COMEDI_MAJOR, 0), COMEDI_NUM_MINORS);
+
+	comedi_proc_cleanup();
+
+	comedi_rt_cleanup();
+
+	comedi_unregister_ioctl32();
+}
+
+module_init(comedi_init);
+module_exit(comedi_cleanup);
+
+void comedi_error(const comedi_device *dev, const char *s)
+{
+	rt_printk("comedi%d: %s: %s\n", dev->minor, dev->driver->driver_name,
+		  s);
+}
+
+void comedi_event(comedi_device *dev, comedi_subdevice *s)
+{
+	comedi_async *async = s->async;
+	unsigned runflags = 0;
+	unsigned runflags_mask = 0;
+
+	/* DPRINTK("comedi_event 0x%x\n",mask); */
+
+	if ((comedi_get_subdevice_runflags(s) & SRF_RUNNING) == 0)
+		return;
+
+	if (s->async->
+	    events & (COMEDI_CB_EOA | COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW)) {
+		runflags_mask |= SRF_RUNNING;
+	}
+	/* remember if an error event has occured, so an error
+	 * can be returned the next time the user does a read() */
+	if (s->async->events & (COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW)) {
+		runflags_mask |= SRF_ERROR;
+		runflags |= SRF_ERROR;
+	}
+	if (runflags_mask) {
+		/*sets SRF_ERROR and SRF_RUNNING together atomically */
+		comedi_set_subdevice_runflags(s, runflags_mask, runflags);
+	}
+
+	if (async->cb_mask & s->async->events) {
+		if (comedi_get_subdevice_runflags(s) & SRF_USER) {
+
+			if (dev->rt) {
+#ifdef CONFIG_COMEDI_RT
+				/* pend wake up */
+				comedi_rt_pend_wakeup(&async->wait_head);
+#else
+				printk
+				    ("BUG: comedi_event() code unreachable\n");
+#endif
+			} else {
+				wake_up_interruptible(&async->wait_head);
+				if (s->subdev_flags & SDF_CMD_READ) {
+					kill_fasync(&dev->async_queue, SIGIO,
+						    POLL_IN);
+				}
+				if (s->subdev_flags & SDF_CMD_WRITE) {
+					kill_fasync(&dev->async_queue, SIGIO,
+						    POLL_OUT);
+				}
+			}
+		} else {
+			if (async->cb_func)
+				async->cb_func(s->async->events, async->cb_arg);
+			/* XXX bug here.  If subdevice A is rt, and
+			 * subdevice B tries to callback to a normal
+			 * linux kernel function, it will be at the
+			 * wrong priority.  Since this isn't very
+			 * common, I'm not going to worry about it. */
+		}
+	}
+	s->async->events = 0;
+}
+
+void comedi_set_subdevice_runflags(comedi_subdevice *s, unsigned mask,
+				   unsigned bits)
+{
+	unsigned long flags;
+
+	comedi_spin_lock_irqsave(&s->spin_lock, flags);
+	s->runflags &= ~mask;
+	s->runflags |= (bits & mask);
+	comedi_spin_unlock_irqrestore(&s->spin_lock, flags);
+}
+
+unsigned comedi_get_subdevice_runflags(comedi_subdevice *s)
+{
+	unsigned long flags;
+	unsigned runflags;
+
+	comedi_spin_lock_irqsave(&s->spin_lock, flags);
+	runflags = s->runflags;
+	comedi_spin_unlock_irqrestore(&s->spin_lock, flags);
+	return runflags;
+}
+
+static int is_device_busy(comedi_device *dev)
+{
+	comedi_subdevice *s;
+	int i;
+
+	if (!dev->attached)
+		return 0;
+
+	for (i = 0; i < dev->n_subdevices; i++) {
+		s = dev->subdevices + i;
+		if (s->busy)
+			return 1;
+		if (s->async && s->async->mmap_count)
+			return 1;
+	}
+
+	return 0;
+}
+
+void comedi_device_init(comedi_device *dev)
+{
+	memset(dev, 0, sizeof(comedi_device));
+	spin_lock_init(&dev->spinlock);
+	mutex_init(&dev->mutex);
+	dev->minor = -1;
+}
+
+void comedi_device_cleanup(comedi_device *dev)
+{
+	if (dev == NULL)
+		return;
+	mutex_lock(&dev->mutex);
+	comedi_device_detach(dev);
+	mutex_unlock(&dev->mutex);
+	mutex_destroy(&dev->mutex);
+}
+
+int comedi_alloc_board_minor(struct device *hardware_device)
+{
+	unsigned long flags;
+	struct comedi_device_file_info *info;
+	device_create_result_type *csdev;
+	unsigned i;
+
+	info = kzalloc(sizeof(struct comedi_device_file_info), GFP_KERNEL);
+	if (info == NULL)
+		return -ENOMEM;
+	info->device = kzalloc(sizeof(comedi_device), GFP_KERNEL);
+	if (info->device == NULL) {
+		kfree(info);
+		return -ENOMEM;
+	}
+	comedi_device_init(info->device);
+	comedi_spin_lock_irqsave(&comedi_file_info_table_lock, flags);
+	for (i = 0; i < COMEDI_NUM_BOARD_MINORS; ++i) {
+		if (comedi_file_info_table[i] == NULL) {
+			comedi_file_info_table[i] = info;
+			break;
+		}
+	}
+	comedi_spin_unlock_irqrestore(&comedi_file_info_table_lock, flags);
+	if (i == COMEDI_NUM_BOARD_MINORS) {
+		comedi_device_cleanup(info->device);
+		kfree(info->device);
+		kfree(info);
+		rt_printk
+		    ("comedi: error: ran out of minor numbers for board device files.\n");
+		return -EBUSY;
+	}
+	info->device->minor = i;
+	csdev = COMEDI_DEVICE_CREATE(comedi_class, NULL,
+				     MKDEV(COMEDI_MAJOR, i), NULL,
+				     hardware_device, "comedi%i", i);
+	if (!IS_ERR(csdev))
+		info->device->class_dev = csdev;
+
+	return i;
+}
+
+void comedi_free_board_minor(unsigned minor)
+{
+	unsigned long flags;
+	struct comedi_device_file_info *info;
+
+	BUG_ON(minor >= COMEDI_NUM_BOARD_MINORS);
+	comedi_spin_lock_irqsave(&comedi_file_info_table_lock, flags);
+	info = comedi_file_info_table[minor];
+	comedi_file_info_table[minor] = NULL;
+	comedi_spin_unlock_irqrestore(&comedi_file_info_table_lock, flags);
+
+	if (info) {
+		comedi_device *dev = info->device;
+		if (dev) {
+			if (dev->class_dev) {
+				device_destroy(comedi_class,
+					       MKDEV(COMEDI_MAJOR, dev->minor));
+			}
+			comedi_device_cleanup(dev);
+			kfree(dev);
+		}
+		kfree(info);
+	}
+}
+
+int comedi_alloc_subdevice_minor(comedi_device *dev, comedi_subdevice *s)
+{
+	unsigned long flags;
+	struct comedi_device_file_info *info;
+	device_create_result_type *csdev;
+	unsigned i;
+
+	info = kmalloc(sizeof(struct comedi_device_file_info), GFP_KERNEL);
+	if (info == NULL)
+		return -ENOMEM;
+	info->device = dev;
+	info->read_subdevice = s;
+	info->write_subdevice = s;
+	comedi_spin_lock_irqsave(&comedi_file_info_table_lock, flags);
+	for (i = COMEDI_FIRST_SUBDEVICE_MINOR; i < COMEDI_NUM_BOARD_MINORS; ++i) {
+		if (comedi_file_info_table[i] == NULL) {
+			comedi_file_info_table[i] = info;
+			break;
+		}
+	}
+	comedi_spin_unlock_irqrestore(&comedi_file_info_table_lock, flags);
+	if (i == COMEDI_NUM_MINORS) {
+		kfree(info);
+		rt_printk
+		    ("comedi: error: ran out of minor numbers for board device files.\n");
+		return -EBUSY;
+	}
+	s->minor = i;
+	csdev = COMEDI_DEVICE_CREATE(comedi_class, dev->class_dev,
+				     MKDEV(COMEDI_MAJOR, i), NULL, NULL,
+				     "comedi%i_subd%i", dev->minor,
+				     (int)(s - dev->subdevices));
+	if (!IS_ERR(csdev))
+		s->class_dev = csdev;
+
+	return i;
+}
+
+void comedi_free_subdevice_minor(comedi_subdevice *s)
+{
+	unsigned long flags;
+	struct comedi_device_file_info *info;
+
+	if (s == NULL)
+		return;
+	if (s->minor < 0)
+		return;
+
+	BUG_ON(s->minor >= COMEDI_NUM_MINORS);
+	BUG_ON(s->minor < COMEDI_FIRST_SUBDEVICE_MINOR);
+
+	comedi_spin_lock_irqsave(&comedi_file_info_table_lock, flags);
+	info = comedi_file_info_table[s->minor];
+	comedi_file_info_table[s->minor] = NULL;
+	comedi_spin_unlock_irqrestore(&comedi_file_info_table_lock, flags);
+
+	if (s->class_dev) {
+		device_destroy(comedi_class, MKDEV(COMEDI_MAJOR, s->minor));
+		s->class_dev = NULL;
+	}
+	kfree(info);
+}
+
+struct comedi_device_file_info *comedi_get_device_file_info(unsigned minor)
+{
+	unsigned long flags;
+	struct comedi_device_file_info *info;
+
+	BUG_ON(minor >= COMEDI_NUM_MINORS);
+	comedi_spin_lock_irqsave(&comedi_file_info_table_lock, flags);
+	info = comedi_file_info_table[minor];
+	comedi_spin_unlock_irqrestore(&comedi_file_info_table_lock, flags);
+	return info;
+}
diff --git a/drivers/staging/comedi/comedi_fops.h b/drivers/staging/comedi/comedi_fops.h
new file mode 100644
index 000000000000..63f8df558e85
--- /dev/null
+++ b/drivers/staging/comedi/comedi_fops.h
@@ -0,0 +1,8 @@
+
+#ifndef _COMEDI_FOPS_H
+#define _COMEDI_FOPS_H
+
+extern struct class *comedi_class;
+extern const struct file_operations comedi_fops;
+
+#endif /* _COMEDI_FOPS_H */
diff --git a/drivers/staging/comedi/comedi_ksyms.c b/drivers/staging/comedi/comedi_ksyms.c
new file mode 100644
index 000000000000..90d57282efb8
--- /dev/null
+++ b/drivers/staging/comedi/comedi_ksyms.c
@@ -0,0 +1,77 @@
+/*
+    module/exp_ioctl.c
+    exported comedi functions
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-8 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#define __NO_VERSION__
+#ifndef EXPORT_SYMTAB
+#define EXPORT_SYMTAB
+#endif
+
+#include "comedidev.h"
+
+/* for drivers */
+EXPORT_SYMBOL(comedi_driver_register);
+EXPORT_SYMBOL(comedi_driver_unregister);
+//EXPORT_SYMBOL(comedi_bufcheck);
+//EXPORT_SYMBOL(comedi_done);
+//EXPORT_SYMBOL(comedi_error_done);
+EXPORT_SYMBOL(comedi_error);
+//EXPORT_SYMBOL(comedi_eobuf);
+//EXPORT_SYMBOL(comedi_eos);
+EXPORT_SYMBOL(comedi_event);
+EXPORT_SYMBOL(comedi_get_subdevice_runflags);
+EXPORT_SYMBOL(comedi_set_subdevice_runflags);
+EXPORT_SYMBOL(range_bipolar10);
+EXPORT_SYMBOL(range_bipolar5);
+EXPORT_SYMBOL(range_bipolar2_5);
+EXPORT_SYMBOL(range_unipolar10);
+EXPORT_SYMBOL(range_unipolar5);
+EXPORT_SYMBOL(range_unknown);
+#ifdef CONFIG_COMEDI_RT
+EXPORT_SYMBOL(comedi_free_irq);
+EXPORT_SYMBOL(comedi_request_irq);
+EXPORT_SYMBOL(comedi_switch_to_rt);
+EXPORT_SYMBOL(comedi_switch_to_non_rt);
+EXPORT_SYMBOL(rt_pend_call);
+#endif
+#ifdef CONFIG_COMEDI_DEBUG
+EXPORT_SYMBOL(comedi_debug);
+#endif
+EXPORT_SYMBOL_GPL(comedi_alloc_board_minor);
+EXPORT_SYMBOL_GPL(comedi_free_board_minor);
+EXPORT_SYMBOL_GPL(comedi_pci_auto_config);
+EXPORT_SYMBOL_GPL(comedi_pci_auto_unconfig);
+
+/* for kcomedilib */
+EXPORT_SYMBOL(check_chanlist);
+EXPORT_SYMBOL_GPL(comedi_get_device_file_info);
+
+EXPORT_SYMBOL(comedi_buf_put);
+EXPORT_SYMBOL(comedi_buf_get);
+EXPORT_SYMBOL(comedi_buf_read_n_available);
+EXPORT_SYMBOL(comedi_buf_write_free);
+EXPORT_SYMBOL(comedi_buf_write_alloc);
+EXPORT_SYMBOL(comedi_buf_read_free);
+EXPORT_SYMBOL(comedi_buf_read_alloc);
+EXPORT_SYMBOL(comedi_buf_memcpy_to);
+EXPORT_SYMBOL(comedi_buf_memcpy_from);
+EXPORT_SYMBOL(comedi_reset_async_buf);
diff --git a/drivers/staging/comedi/comedi_rt.h b/drivers/staging/comedi/comedi_rt.h
new file mode 100644
index 000000000000..61852bf5adcc
--- /dev/null
+++ b/drivers/staging/comedi/comedi_rt.h
@@ -0,0 +1,150 @@
+/*
+    module/comedi_rt.h
+    header file for real-time structures, variables, and constants
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _COMEDI_RT_H
+#define _COMEDI_RT_H
+
+#ifndef _COMEDIDEV_H
+#error comedi_rt.h should only be included by comedidev.h
+#endif
+
+#include <linux/version.h>
+#include <linux/kdev_t.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+
+#ifdef CONFIG_COMEDI_RT
+
+#ifdef CONFIG_COMEDI_RTAI
+#include <rtai.h>
+#include <rtai_sched.h>
+#include <rtai_version.h>
+#endif
+#ifdef CONFIG_COMEDI_RTL
+#include <rtl_core.h>
+#include <rtl_time.h>
+/* #ifdef RTLINUX_VERSION_CODE */
+#include <rtl_sync.h>
+/* #endif */
+#define rt_printk rtl_printf
+#endif
+#ifdef CONFIG_COMEDI_FUSION
+#define rt_printk(format, args...) printk(format , ## args)
+#endif /* CONFIG_COMEDI_FUSION */
+#ifdef CONFIG_PRIORITY_IRQ
+#define rt_printk printk
+#endif
+
+int comedi_request_irq(unsigned int irq, irqreturn_t(*handler) (int,
+		void *PT_REGS_ARG), unsigned long flags, const char *device,
+		comedi_device *dev_id);
+void comedi_free_irq(unsigned int irq, comedi_device *dev_id);
+void comedi_rt_init(void);
+void comedi_rt_cleanup(void);
+int comedi_switch_to_rt(comedi_device *dev);
+void comedi_switch_to_non_rt(comedi_device *dev);
+void comedi_rt_pend_wakeup(wait_queue_head_t *q);
+extern int rt_pend_call(void (*func) (int arg1, void *arg2), int arg1,
+	void *arg2);
+
+#else
+
+#define comedi_request_irq(a, b, c, d, e) request_irq(a, b, c, d, e)
+#define comedi_free_irq(a, b) free_irq(a, b)
+#define comedi_rt_init() do {} while (0)
+#define comedi_rt_cleanup() do {} while (0)
+#define comedi_switch_to_rt(a) (-1)
+#define comedi_switch_to_non_rt(a) do {} while (0)
+#define comedi_rt_pend_wakeup(a) do {} while (0)
+
+#define rt_printk(format, args...)	printk(format, ##args)
+
+#endif
+
+/* Define a spin_lock_irqsave function that will work with rt or without.
+ * Use inline functions instead of just macros to enforce some type checking.
+ */
+#define comedi_spin_lock_irqsave(lock_ptr, flags) \
+	(flags = __comedi_spin_lock_irqsave(lock_ptr))
+
+static inline unsigned long __comedi_spin_lock_irqsave(spinlock_t *lock_ptr)
+{
+	unsigned long flags;
+
+#if defined(CONFIG_COMEDI_RTAI)
+	flags = rt_spin_lock_irqsave(lock_ptr);
+
+#elif defined(CONFIG_COMEDI_RTL)
+	rtl_spin_lock_irqsave(lock_ptr, flags);
+
+#elif defined(CONFIG_COMEDI_RTL_V1)
+	rtl_spin_lock_irqsave(lock_ptr, flags);
+
+#elif defined(CONFIG_COMEDI_FUSION)
+	rthal_spin_lock_irqsave(lock_ptr, flags);
+#else
+	spin_lock_irqsave(lock_ptr, flags);
+
+#endif
+
+	return flags;
+}
+
+static inline void comedi_spin_unlock_irqrestore(spinlock_t *lock_ptr,
+	unsigned long flags)
+{
+
+#if defined(CONFIG_COMEDI_RTAI)
+	rt_spin_unlock_irqrestore(flags, lock_ptr);
+
+#elif defined(CONFIG_COMEDI_RTL)
+	rtl_spin_unlock_irqrestore(lock_ptr, flags);
+
+#elif defined(CONFIG_COMEDI_RTL_V1)
+	rtl_spin_unlock_irqrestore(lock_ptr, flags);
+#elif defined(CONFIG_COMEDI_FUSION)
+	rthal_spin_unlock_irqrestore(lock_ptr, flags);
+#else
+	spin_unlock_irqrestore(lock_ptr, flags);
+
+#endif
+
+}
+
+/* define a RT safe udelay */
+static inline void comedi_udelay(unsigned int usec)
+{
+#if defined(CONFIG_COMEDI_RTAI)
+	static const int nanosec_per_usec = 1000;
+	rt_busy_sleep(usec * nanosec_per_usec);
+#elif defined(CONFIG_COMEDI_RTL)
+	static const int nanosec_per_usec = 1000;
+	rtl_delay(usec * nanosec_per_usec);
+#else
+	udelay(usec);
+#endif
+}
+
+#endif
diff --git a/drivers/staging/comedi/comedidev.h b/drivers/staging/comedi/comedidev.h
new file mode 100644
index 000000000000..3735355d3c58
--- /dev/null
+++ b/drivers/staging/comedi/comedidev.h
@@ -0,0 +1,537 @@
+/*
+    include/linux/comedidev.h
+    header file for kernel-only structures, variables, and constants
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _COMEDIDEV_H
+#define _COMEDIDEV_H
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/kdev_t.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/spinlock.h>
+#include <linux/mutex.h>
+#include <linux/wait.h>
+#include <linux/mm.h>
+#include <linux/init.h>
+#include <linux/vmalloc.h>
+#include <linux/mm.h>
+#include "interrupt.h"
+#include <linux/dma-mapping.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+
+#include "comedi.h"
+
+#define DPRINTK(format, args...)	do {		\
+	if (comedi_debug)				\
+		printk(KERN_DEBUG "comedi: " format , ## args);	\
+} while (0)
+
+#define COMEDI_VERSION(a, b, c) (((a) << 16) + ((b) << 8) + (c))
+#define COMEDI_VERSION_CODE COMEDI_VERSION(COMEDI_MAJORVERSION, COMEDI_MINORVERSION, COMEDI_MICROVERSION)
+#define COMEDI_RELEASE VERSION
+
+#define COMEDI_INITCLEANUP_NOMODULE(x)					\
+	static int __init x ## _init_module(void)			\
+		{return comedi_driver_register(&(x));}			\
+	static void __exit x ## _cleanup_module(void)			\
+		{comedi_driver_unregister(&(x));} 			\
+	module_init(x ## _init_module);					\
+	module_exit(x ## _cleanup_module);					\
+
+#define COMEDI_MODULE_MACROS						\
+	MODULE_AUTHOR("Comedi http://www.comedi.org");		\
+	MODULE_DESCRIPTION("Comedi low-level driver");			\
+	MODULE_LICENSE("GPL");						\
+
+#define COMEDI_INITCLEANUP(x)						\
+	COMEDI_MODULE_MACROS		\
+	COMEDI_INITCLEANUP_NOMODULE(x)
+
+#define COMEDI_PCI_INITCLEANUP_NOMODULE(comedi_driver, pci_id_table) \
+	static int __devinit comedi_driver ## _pci_probe(struct pci_dev *dev, \
+		const struct pci_device_id *ent) \
+	{ \
+		return comedi_pci_auto_config(dev, comedi_driver.driver_name); \
+	} \
+	static void __devexit comedi_driver ## _pci_remove(struct pci_dev *dev) \
+	{ \
+		comedi_pci_auto_unconfig(dev); \
+	} \
+	static struct pci_driver comedi_driver ## _pci_driver = \
+	{ \
+		.id_table = pci_id_table, \
+		.probe = &comedi_driver ## _pci_probe, \
+		.remove = __devexit_p(&comedi_driver ## _pci_remove) \
+	}; \
+	static int __init comedi_driver ## _init_module(void) \
+	{ \
+		int retval; \
+		retval = comedi_driver_register(&comedi_driver); \
+		if (retval < 0) \
+			return retval; \
+		comedi_driver ## _pci_driver.name = (char *)comedi_driver.driver_name; \
+		return pci_register_driver(&comedi_driver ## _pci_driver); \
+	} \
+	static void __exit comedi_driver ## _cleanup_module(void) \
+	{ \
+		pci_unregister_driver(&comedi_driver ## _pci_driver); \
+		comedi_driver_unregister(&comedi_driver); \
+	} \
+	module_init(comedi_driver ## _init_module); \
+	module_exit(comedi_driver ## _cleanup_module);
+
+#define COMEDI_PCI_INITCLEANUP(comedi_driver, pci_id_table) \
+	COMEDI_MODULE_MACROS \
+	COMEDI_PCI_INITCLEANUP_NOMODULE(comedi_driver, pci_id_table)
+
+#define PCI_VENDOR_ID_INOVA		0x104c
+#define PCI_VENDOR_ID_NATINST		0x1093
+#define PCI_VENDOR_ID_DATX		0x1116
+#define PCI_VENDOR_ID_COMPUTERBOARDS	0x1307
+#define PCI_VENDOR_ID_ADVANTECH		0x13fe
+#define PCI_VENDOR_ID_RTD		0x1435
+#define PCI_VENDOR_ID_AMPLICON		0x14dc
+#define PCI_VENDOR_ID_ADLINK		0x144a
+#define PCI_VENDOR_ID_ICP		0x104c
+#define PCI_VENDOR_ID_CONTEC		0x1221
+#define PCI_VENDOR_ID_MEILHAUS		0x1402
+
+#define COMEDI_NUM_MINORS 0x100
+#define COMEDI_NUM_LEGACY_MINORS 0x10
+#define COMEDI_NUM_BOARD_MINORS 0x30
+#define COMEDI_FIRST_SUBDEVICE_MINOR COMEDI_NUM_BOARD_MINORS
+
+typedef struct comedi_device_struct comedi_device;
+typedef struct comedi_subdevice_struct comedi_subdevice;
+typedef struct comedi_async_struct comedi_async;
+typedef struct comedi_driver_struct comedi_driver;
+typedef struct comedi_lrange_struct comedi_lrange;
+
+typedef struct device device_create_result_type;
+
+#define COMEDI_DEVICE_CREATE(cs, parent, devt, drvdata, device, fmt...) \
+	device_create(cs, ((parent) ? (parent) : (device)), devt, drvdata, fmt)
+
+struct comedi_subdevice_struct {
+	comedi_device *device;
+	int type;
+	int n_chan;
+	volatile int subdev_flags;
+	int len_chanlist;	/* maximum length of channel/gain list */
+
+	void *private;
+
+	comedi_async *async;
+
+	void *lock;
+	void *busy;
+	unsigned runflags;
+	spinlock_t spin_lock;
+
+	int io_bits;
+
+	lsampl_t maxdata;	/* if maxdata==0, use list */
+	const lsampl_t *maxdata_list;	/* list is channel specific */
+
+	unsigned int flags;
+	const unsigned int *flaglist;
+
+	unsigned int settling_time_0;
+
+	const comedi_lrange *range_table;
+	const comedi_lrange *const *range_table_list;
+
+	unsigned int *chanlist;	/* driver-owned chanlist (not used) */
+
+	int (*insn_read) (comedi_device *, comedi_subdevice *, comedi_insn *,
+		lsampl_t *);
+	int (*insn_write) (comedi_device *, comedi_subdevice *, comedi_insn *,
+		lsampl_t *);
+	int (*insn_bits) (comedi_device *, comedi_subdevice *, comedi_insn *,
+		lsampl_t *);
+	int (*insn_config) (comedi_device *, comedi_subdevice *, comedi_insn *,
+		lsampl_t *);
+
+	int (*do_cmd) (comedi_device *, comedi_subdevice *);
+	int (*do_cmdtest) (comedi_device *, comedi_subdevice *, comedi_cmd *);
+	int (*poll) (comedi_device *, comedi_subdevice *);
+	int (*cancel) (comedi_device *, comedi_subdevice *);
+	/* int (*do_lock)(comedi_device *,comedi_subdevice *); */
+	/* int (*do_unlock)(comedi_device *,comedi_subdevice *); */
+
+	/* called when the buffer changes */
+	int (*buf_change) (comedi_device *dev, comedi_subdevice *s,
+		unsigned long new_size);
+
+	void (*munge) (comedi_device *dev, comedi_subdevice *s, void *data,
+		unsigned int num_bytes, unsigned int start_chan_index);
+	enum dma_data_direction async_dma_dir;
+
+	unsigned int state;
+
+	device_create_result_type *class_dev;
+	int minor;
+};
+
+struct comedi_buf_page {
+	void *virt_addr;
+	dma_addr_t dma_addr;
+};
+
+struct comedi_async_struct {
+	comedi_subdevice *subdevice;
+
+	void *prealloc_buf;	/* pre-allocated buffer */
+	unsigned int prealloc_bufsz;	/* buffer size, in bytes */
+	struct comedi_buf_page *buf_page_list;	/* virtual and dma address of each page */
+	unsigned n_buf_pages;	/* num elements in buf_page_list */
+
+	unsigned int max_bufsize;	/* maximum buffer size, bytes */
+	unsigned int mmap_count;	/* current number of mmaps of prealloc_buf */
+
+	unsigned int buf_write_count;	/* byte count for writer (write completed) */
+	unsigned int buf_write_alloc_count;	/* byte count for writer (allocated for writing) */
+	unsigned int buf_read_count;	/* byte count for reader (read completed) */
+	unsigned int buf_read_alloc_count;	/* byte count for reader (allocated for reading) */
+
+	unsigned int buf_write_ptr;	/* buffer marker for writer */
+	unsigned int buf_read_ptr;	/* buffer marker for reader */
+
+	unsigned int cur_chan;	/* useless channel marker for interrupt */
+	/* number of bytes that have been received for current scan */
+	unsigned int scan_progress;
+	/* keeps track of where we are in chanlist as for munging */
+	unsigned int munge_chan;
+	/* number of bytes that have been munged */
+	unsigned int munge_count;
+	/* buffer marker for munging */
+	unsigned int munge_ptr;
+
+	unsigned int events;	/* events that have occurred */
+
+	comedi_cmd cmd;
+
+	wait_queue_head_t wait_head;
+
+	/* callback stuff */
+	unsigned int cb_mask;
+	int (*cb_func) (unsigned int flags, void *);
+	void *cb_arg;
+
+	int (*inttrig) (comedi_device *dev, comedi_subdevice *s,
+			unsigned int x);
+};
+
+struct comedi_driver_struct {
+	struct comedi_driver_struct *next;
+
+	const char *driver_name;
+	struct module *module;
+	int (*attach) (comedi_device *, comedi_devconfig *);
+	int (*detach) (comedi_device *);
+
+	/* number of elements in board_name and board_id arrays */
+	unsigned int num_names;
+	const char *const *board_name;
+	/* offset in bytes from one board name pointer to the next */
+	int offset;
+};
+
+struct comedi_device_struct {
+	int use_count;
+	comedi_driver *driver;
+	void *private;
+
+	device_create_result_type *class_dev;
+	int minor;
+	/* hw_dev is passed to dma_alloc_coherent when allocating async buffers
+	 * for subdevices that have async_dma_dir set to something other than
+	 * DMA_NONE */
+	struct device *hw_dev;
+
+	const char *board_name;
+	const void *board_ptr;
+	int attached;
+	int rt;
+	spinlock_t spinlock;
+	struct mutex mutex;
+	int in_request_module;
+
+	int n_subdevices;
+	comedi_subdevice *subdevices;
+
+	/* dumb */
+	unsigned long iobase;
+	unsigned int irq;
+
+	comedi_subdevice *read_subdev;
+	comedi_subdevice *write_subdev;
+
+	struct fasync_struct *async_queue;
+
+	void (*open) (comedi_device *dev);
+	void (*close) (comedi_device *dev);
+};
+
+struct comedi_device_file_info {
+	comedi_device *device;
+	comedi_subdevice *read_subdevice;
+	comedi_subdevice *write_subdevice;
+};
+
+#ifdef CONFIG_COMEDI_DEBUG
+extern int comedi_debug;
+#else
+static const int comedi_debug;
+#endif
+
+/*
+ * function prototypes
+ */
+
+void comedi_event(comedi_device *dev, comedi_subdevice *s);
+void comedi_error(const comedi_device *dev, const char *s);
+
+/* we can expand the number of bits used to encode devices/subdevices into
+ the minor number soon, after more distros support > 8 bit minor numbers
+ (like after Debian Etch gets released) */
+enum comedi_minor_bits {
+	COMEDI_DEVICE_MINOR_MASK = 0xf,
+	COMEDI_SUBDEVICE_MINOR_MASK = 0xf0
+};
+static const unsigned COMEDI_SUBDEVICE_MINOR_SHIFT = 4;
+static const unsigned COMEDI_SUBDEVICE_MINOR_OFFSET = 1;
+
+struct comedi_device_file_info *comedi_get_device_file_info(unsigned minor);
+
+static inline comedi_subdevice *comedi_get_read_subdevice(
+				const struct comedi_device_file_info *info)
+{
+	if (info->read_subdevice)
+		return info->read_subdevice;
+	if (info->device == NULL)
+		return NULL;
+	return info->device->read_subdev;
+}
+
+static inline comedi_subdevice *comedi_get_write_subdevice(
+				const struct comedi_device_file_info *info)
+{
+	if (info->write_subdevice)
+		return info->write_subdevice;
+	if (info->device == NULL)
+		return NULL;
+	return info->device->write_subdev;
+}
+
+void comedi_device_detach(comedi_device *dev);
+int comedi_device_attach(comedi_device *dev, comedi_devconfig *it);
+int comedi_driver_register(comedi_driver *);
+int comedi_driver_unregister(comedi_driver *);
+
+void init_polling(void);
+void cleanup_polling(void);
+void start_polling(comedi_device *);
+void stop_polling(comedi_device *);
+
+int comedi_buf_alloc(comedi_device *dev, comedi_subdevice *s, unsigned long
+	new_size);
+
+#ifdef CONFIG_PROC_FS
+void comedi_proc_init(void);
+void comedi_proc_cleanup(void);
+#else
+static inline void comedi_proc_init(void)
+{
+}
+static inline void comedi_proc_cleanup(void)
+{
+}
+#endif
+
+/* subdevice runflags */
+enum subdevice_runflags {
+	SRF_USER = 0x00000001,
+	SRF_RT = 0x00000002,
+	/* indicates an COMEDI_CB_ERROR event has occurred since the last
+	 * command was started */
+	SRF_ERROR = 0x00000004,
+	SRF_RUNNING = 0x08000000
+};
+
+/*
+   various internal comedi functions
+ */
+
+int do_rangeinfo_ioctl(comedi_device *dev, comedi_rangeinfo *arg);
+int check_chanlist(comedi_subdevice *s, int n, unsigned int *chanlist);
+void comedi_set_subdevice_runflags(comedi_subdevice *s, unsigned mask,
+	unsigned bits);
+unsigned comedi_get_subdevice_runflags(comedi_subdevice *s);
+int insn_inval(comedi_device *dev, comedi_subdevice *s,
+	comedi_insn *insn, lsampl_t *data);
+
+/* range stuff */
+
+#define RANGE(a, b)		{(a)*1e6, (b)*1e6, 0}
+#define RANGE_ext(a, b)		{(a)*1e6, (b)*1e6, RF_EXTERNAL}
+#define RANGE_mA(a, b)		{(a)*1e6, (b)*1e6, UNIT_mA}
+#define RANGE_unitless(a, b)	{(a)*1e6, (b)*1e6, 0}	/* XXX */
+#define BIP_RANGE(a)		{-(a)*1e6, (a)*1e6, 0}
+#define UNI_RANGE(a)		{0, (a)*1e6, 0}
+
+extern const comedi_lrange range_bipolar10;
+extern const comedi_lrange range_bipolar5;
+extern const comedi_lrange range_bipolar2_5;
+extern const comedi_lrange range_unipolar10;
+extern const comedi_lrange range_unipolar5;
+extern const comedi_lrange range_unknown;
+
+#define range_digital		range_unipolar5
+
+#if __GNUC__ >= 3
+#define GCC_ZERO_LENGTH_ARRAY
+#else
+#define GCC_ZERO_LENGTH_ARRAY 0
+#endif
+
+struct comedi_lrange_struct {
+	int length;
+	comedi_krange range[GCC_ZERO_LENGTH_ARRAY];
+};
+
+/* some silly little inline functions */
+
+static inline int alloc_subdevices(comedi_device *dev,
+				   unsigned int num_subdevices)
+{
+	unsigned i;
+
+	dev->n_subdevices = num_subdevices;
+	dev->subdevices =
+		kcalloc(num_subdevices, sizeof(comedi_subdevice), GFP_KERNEL);
+	if (!dev->subdevices)
+		return -ENOMEM;
+	for (i = 0; i < num_subdevices; ++i) {
+		dev->subdevices[i].device = dev;
+		dev->subdevices[i].async_dma_dir = DMA_NONE;
+		spin_lock_init(&dev->subdevices[i].spin_lock);
+		dev->subdevices[i].minor = -1;
+	}
+	return 0;
+}
+
+static inline int alloc_private(comedi_device *dev, int size)
+{
+	dev->private = kzalloc(size, GFP_KERNEL);
+	if (!dev->private)
+		return -ENOMEM;
+	return 0;
+}
+
+static inline unsigned int bytes_per_sample(const comedi_subdevice *subd)
+{
+	if (subd->subdev_flags & SDF_LSAMPL)
+		return sizeof(lsampl_t);
+	else
+		return sizeof(sampl_t);
+}
+
+/* must be used in attach to set dev->hw_dev if you wish to dma directly
+into comedi's buffer */
+static inline void comedi_set_hw_dev(comedi_device *dev, struct device *hw_dev)
+{
+	if (dev->hw_dev)
+		put_device(dev->hw_dev);
+
+	dev->hw_dev = hw_dev;
+	if (dev->hw_dev) {
+		dev->hw_dev = get_device(dev->hw_dev);
+		BUG_ON(dev->hw_dev == NULL);
+	}
+}
+
+int comedi_buf_put(comedi_async *async, sampl_t x);
+int comedi_buf_get(comedi_async *async, sampl_t *x);
+
+unsigned int comedi_buf_write_n_available(comedi_async *async);
+unsigned int comedi_buf_write_alloc(comedi_async *async, unsigned int nbytes);
+unsigned int comedi_buf_write_alloc_strict(comedi_async *async,
+	unsigned int nbytes);
+unsigned comedi_buf_write_free(comedi_async *async, unsigned int nbytes);
+unsigned comedi_buf_read_alloc(comedi_async *async, unsigned nbytes);
+unsigned comedi_buf_read_free(comedi_async *async, unsigned int nbytes);
+unsigned int comedi_buf_read_n_available(comedi_async *async);
+void comedi_buf_memcpy_to(comedi_async *async, unsigned int offset,
+	const void *source, unsigned int num_bytes);
+void comedi_buf_memcpy_from(comedi_async *async, unsigned int offset,
+	void *destination, unsigned int num_bytes);
+static inline unsigned comedi_buf_write_n_allocated(comedi_async *async)
+{
+	return async->buf_write_alloc_count - async->buf_write_count;
+}
+static inline unsigned comedi_buf_read_n_allocated(comedi_async *async)
+{
+	return async->buf_read_alloc_count - async->buf_read_count;
+}
+
+void comedi_reset_async_buf(comedi_async *async);
+
+static inline void *comedi_aux_data(int options[], int n)
+{
+	unsigned long address;
+	unsigned long addressLow;
+	int bit_shift;
+	if (sizeof(int) >= sizeof(void *))
+		address = options[COMEDI_DEVCONF_AUX_DATA_LO];
+	else {
+		address = options[COMEDI_DEVCONF_AUX_DATA_HI];
+		bit_shift = sizeof(int) * 8;
+		address <<= bit_shift;
+		addressLow = options[COMEDI_DEVCONF_AUX_DATA_LO];
+		addressLow &= (1UL << bit_shift) - 1;
+		address |= addressLow;
+	}
+	if (n >= 1)
+		address += options[COMEDI_DEVCONF_AUX_DATA0_LENGTH];
+	if (n >= 2)
+		address += options[COMEDI_DEVCONF_AUX_DATA1_LENGTH];
+	if (n >= 3)
+		address += options[COMEDI_DEVCONF_AUX_DATA2_LENGTH];
+	BUG_ON(n > 3);
+	return (void *)address;
+}
+
+int comedi_alloc_board_minor(struct device *hardware_device);
+void comedi_free_board_minor(unsigned minor);
+int comedi_alloc_subdevice_minor(comedi_device *dev, comedi_subdevice *s);
+void comedi_free_subdevice_minor(comedi_subdevice *s);
+int comedi_pci_auto_config(struct pci_dev *pcidev, const char *board_name);
+void comedi_pci_auto_unconfig(struct pci_dev *pcidev);
+
+#include "comedi_rt.h"
+
+#endif /* _COMEDIDEV_H */
diff --git a/drivers/staging/comedi/comedilib.h b/drivers/staging/comedi/comedilib.h
new file mode 100644
index 000000000000..fc5fc015726b
--- /dev/null
+++ b/drivers/staging/comedi/comedilib.h
@@ -0,0 +1,192 @@
+/*
+    linux/include/comedilib.h
+    header file for kcomedilib
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1998-2001 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _LINUX_COMEDILIB_H
+#define _LINUX_COMEDILIB_H
+
+#include "comedi.h"
+
+/* Kernel internal stuff.  Needed by real-time modules and such. */
+
+#ifndef __KERNEL__
+#error linux/comedilib.h should not be included by non-kernel-space code
+#endif
+
+/* exported functions */
+
+#ifndef KCOMEDILIB_DEPRECATED
+
+typedef void comedi_t;
+
+/* these functions may not be called at real-time priority */
+
+comedi_t *comedi_open(const char *path);
+int comedi_close(comedi_t *dev);
+
+/* these functions may be called at any priority, but may fail at
+   real-time priority */
+
+int comedi_lock(comedi_t *dev, unsigned int subdev);
+int comedi_unlock(comedi_t *dev, unsigned int subdev);
+
+/* these functions may be called at any priority, but you must hold
+   the lock for the subdevice */
+
+int comedi_loglevel(int loglevel);
+void comedi_perror(const char *s);
+char *comedi_strerror(int errnum);
+int comedi_errno(void);
+int comedi_fileno(comedi_t *dev);
+
+int comedi_cancel(comedi_t *dev, unsigned int subdev);
+int comedi_register_callback(comedi_t *dev, unsigned int subdev,
+	unsigned int mask, int (*cb) (unsigned int, void *), void *arg);
+
+int comedi_command(comedi_t *dev, comedi_cmd *cmd);
+int comedi_command_test(comedi_t *dev, comedi_cmd *cmd);
+int comedi_trigger(comedi_t *dev, unsigned int subdev, comedi_trig *it);
+int __comedi_trigger(comedi_t *dev, unsigned int subdev, comedi_trig *it);
+int comedi_data_write(comedi_t *dev, unsigned int subdev, unsigned int chan,
+	unsigned int range, unsigned int aref, lsampl_t data);
+int comedi_data_read(comedi_t *dev, unsigned int subdev, unsigned int chan,
+	unsigned int range, unsigned int aref, lsampl_t *data);
+int comedi_data_read_hint(comedi_t *dev, unsigned int subdev,
+	unsigned int chan, unsigned int range, unsigned int aref);
+int comedi_data_read_delayed(comedi_t *dev, unsigned int subdev,
+	unsigned int chan, unsigned int range, unsigned int aref,
+	lsampl_t *data, unsigned int nano_sec);
+int comedi_dio_config(comedi_t *dev, unsigned int subdev, unsigned int chan,
+	unsigned int io);
+int comedi_dio_read(comedi_t *dev, unsigned int subdev, unsigned int chan,
+	unsigned int *val);
+int comedi_dio_write(comedi_t *dev, unsigned int subdev, unsigned int chan,
+	unsigned int val);
+int comedi_dio_bitfield(comedi_t *dev, unsigned int subdev, unsigned int mask,
+	unsigned int *bits);
+int comedi_get_n_subdevices(comedi_t *dev);
+int comedi_get_version_code(comedi_t *dev);
+const char *comedi_get_driver_name(comedi_t *dev);
+const char *comedi_get_board_name(comedi_t *dev);
+int comedi_get_subdevice_type(comedi_t *dev, unsigned int subdevice);
+int comedi_find_subdevice_by_type(comedi_t *dev, int type, unsigned int subd);
+int comedi_get_n_channels(comedi_t *dev, unsigned int subdevice);
+lsampl_t comedi_get_maxdata(comedi_t *dev, unsigned int subdevice, unsigned
+	int chan);
+int comedi_get_n_ranges(comedi_t *dev, unsigned int subdevice, unsigned int
+	chan);
+int comedi_do_insn(comedi_t *dev, comedi_insn *insn);
+int comedi_poll(comedi_t *dev, unsigned int subdev);
+
+/* DEPRECATED functions */
+int comedi_get_rangetype(comedi_t *dev, unsigned int subdevice,
+	unsigned int chan);
+
+/* ALPHA functions */
+unsigned int comedi_get_subdevice_flags(comedi_t *dev, unsigned int subdevice);
+int comedi_get_len_chanlist(comedi_t *dev, unsigned int subdevice);
+int comedi_get_krange(comedi_t *dev, unsigned int subdevice, unsigned int
+	chan, unsigned int range, comedi_krange *krange);
+unsigned int comedi_get_buf_head_pos(comedi_t *dev, unsigned int subdevice);
+int comedi_set_user_int_count(comedi_t *dev, unsigned int subdevice,
+	unsigned int buf_user_count);
+int comedi_map(comedi_t *dev, unsigned int subdev, void *ptr);
+int comedi_unmap(comedi_t *dev, unsigned int subdev);
+int comedi_get_buffer_size(comedi_t *dev, unsigned int subdev);
+int comedi_mark_buffer_read(comedi_t *dev, unsigned int subdevice,
+	unsigned int num_bytes);
+int comedi_mark_buffer_written(comedi_t *d, unsigned int subdevice,
+	unsigned int num_bytes);
+int comedi_get_buffer_contents(comedi_t *dev, unsigned int subdevice);
+int comedi_get_buffer_offset(comedi_t *dev, unsigned int subdevice);
+
+#else
+
+/* these functions may not be called at real-time priority */
+
+int comedi_open(unsigned int minor);
+void comedi_close(unsigned int minor);
+
+/* these functions may be called at any priority, but may fail at
+   real-time priority */
+
+int comedi_lock(unsigned int minor, unsigned int subdev);
+int comedi_unlock(unsigned int minor, unsigned int subdev);
+
+/* these functions may be called at any priority, but you must hold
+   the lock for the subdevice */
+
+int comedi_cancel(unsigned int minor, unsigned int subdev);
+int comedi_register_callback(unsigned int minor, unsigned int subdev,
+	unsigned int mask, int (*cb) (unsigned int, void *), void *arg);
+
+int comedi_command(unsigned int minor, comedi_cmd *cmd);
+int comedi_command_test(unsigned int minor, comedi_cmd *cmd);
+int comedi_trigger(unsigned int minor, unsigned int subdev, comedi_trig *it);
+int __comedi_trigger(unsigned int minor, unsigned int subdev, comedi_trig *it);
+int comedi_data_write(unsigned int dev, unsigned int subdev, unsigned int chan,
+	unsigned int range, unsigned int aref, lsampl_t data);
+int comedi_data_read(unsigned int dev, unsigned int subdev, unsigned int chan,
+	unsigned int range, unsigned int aref, lsampl_t *data);
+int comedi_dio_config(unsigned int dev, unsigned int subdev, unsigned int chan,
+	unsigned int io);
+int comedi_dio_read(unsigned int dev, unsigned int subdev, unsigned int chan,
+	unsigned int *val);
+int comedi_dio_write(unsigned int dev, unsigned int subdev, unsigned int chan,
+	unsigned int val);
+int comedi_dio_bitfield(unsigned int dev, unsigned int subdev,
+	unsigned int mask, unsigned int *bits);
+int comedi_get_n_subdevices(unsigned int dev);
+int comedi_get_version_code(unsigned int dev);
+char *comedi_get_driver_name(unsigned int dev);
+char *comedi_get_board_name(unsigned int minor);
+int comedi_get_subdevice_type(unsigned int minor, unsigned int subdevice);
+int comedi_find_subdevice_by_type(unsigned int minor, int type,
+	unsigned int subd);
+int comedi_get_n_channels(unsigned int minor, unsigned int subdevice);
+lsampl_t comedi_get_maxdata(unsigned int minor, unsigned int subdevice, unsigned
+	int chan);
+int comedi_get_n_ranges(unsigned int minor, unsigned int subdevice, unsigned int
+	chan);
+int comedi_do_insn(unsigned int minor, comedi_insn *insn);
+int comedi_poll(unsigned int minor, unsigned int subdev);
+
+/* DEPRECATED functions */
+int comedi_get_rangetype(unsigned int minor, unsigned int subdevice,
+	unsigned int chan);
+
+/* ALPHA functions */
+unsigned int comedi_get_subdevice_flags(unsigned int minor, unsigned int
+	subdevice);
+int comedi_get_len_chanlist(unsigned int minor, unsigned int subdevice);
+int comedi_get_krange(unsigned int minor, unsigned int subdevice, unsigned int
+	chan, unsigned int range, comedi_krange *krange);
+unsigned int comedi_get_buf_head_pos(unsigned int minor, unsigned int
+	subdevice);
+int comedi_set_user_int_count(unsigned int minor, unsigned int subdevice,
+	unsigned int buf_user_count);
+int comedi_map(unsigned int minor, unsigned int subdev, void **ptr);
+int comedi_unmap(unsigned int minor, unsigned int subdev);
+
+#endif
+
+#endif
diff --git a/drivers/staging/comedi/drivers.c b/drivers/staging/comedi/drivers.c
new file mode 100644
index 000000000000..36a93b95e3f2
--- /dev/null
+++ b/drivers/staging/comedi/drivers.c
@@ -0,0 +1,846 @@
+/*
+    module/drivers.c
+    functions for manipulating drivers
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#define _GNU_SOURCE
+
+#define __NO_VERSION__
+#include "comedi_fops.h"
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/fcntl.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include "comedidev.h"
+#include "wrapper.h"
+#include <linux/highmem.h>	/* for SuSE brokenness */
+#include <linux/vmalloc.h>
+#include <linux/cdev.h>
+#include <linux/dma-mapping.h>
+
+#include <asm/io.h>
+#include <asm/system.h>
+
+static int postconfig(comedi_device * dev);
+static int insn_rw_emulate_bits(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static void *comedi_recognize(comedi_driver * driv, const char *name);
+static void comedi_report_boards(comedi_driver * driv);
+static int poll_invalid(comedi_device * dev, comedi_subdevice * s);
+int comedi_buf_alloc(comedi_device * dev, comedi_subdevice * s,
+	unsigned long new_size);
+
+comedi_driver *comedi_drivers;
+
+int comedi_modprobe(int minor)
+{
+	return -EINVAL;
+}
+
+static void cleanup_device(comedi_device * dev)
+{
+	int i;
+	comedi_subdevice *s;
+
+	if (dev->subdevices) {
+		for (i = 0; i < dev->n_subdevices; i++) {
+			s = dev->subdevices + i;
+			comedi_free_subdevice_minor(s);
+			if (s->async) {
+				comedi_buf_alloc(dev, s, 0);
+				kfree(s->async);
+			}
+		}
+		kfree(dev->subdevices);
+		dev->subdevices = NULL;
+		dev->n_subdevices = 0;
+	}
+	if (dev->private) {
+		kfree(dev->private);
+		dev->private = NULL;
+	}
+	dev->driver = 0;
+	dev->board_name = NULL;
+	dev->board_ptr = NULL;
+	dev->iobase = 0;
+	dev->irq = 0;
+	dev->read_subdev = NULL;
+	dev->write_subdev = NULL;
+	dev->open = NULL;
+	dev->close = NULL;
+	comedi_set_hw_dev(dev, NULL);
+}
+
+static void __comedi_device_detach(comedi_device * dev)
+{
+	dev->attached = 0;
+	if (dev->driver) {
+		dev->driver->detach(dev);
+	} else {
+		printk("BUG: dev->driver=NULL in comedi_device_detach()\n");
+	}
+	cleanup_device(dev);
+}
+
+void comedi_device_detach(comedi_device * dev)
+{
+	if (!dev->attached)
+		return;
+	__comedi_device_detach(dev);
+}
+
+int comedi_device_attach(comedi_device * dev, comedi_devconfig * it)
+{
+	comedi_driver *driv;
+	int ret;
+
+	if (dev->attached)
+		return -EBUSY;
+
+	for (driv = comedi_drivers; driv; driv = driv->next) {
+		if (!try_module_get(driv->module)) {
+			printk("comedi: failed to increment module count, skipping\n");
+			continue;
+		}
+		if (driv->num_names) {
+			dev->board_ptr = comedi_recognize(driv, it->board_name);
+			if (dev->board_ptr == NULL) {
+				module_put(driv->module);
+				continue;
+			}
+		} else {
+			if (strcmp(driv->driver_name, it->board_name)) {
+				module_put(driv->module);
+				continue;
+			}
+		}
+		//initialize dev->driver here so comedi_error() can be called from attach
+		dev->driver = driv;
+		ret = driv->attach(dev, it);
+		if (ret < 0) {
+			module_put(dev->driver->module);
+			__comedi_device_detach(dev);
+			return ret;
+		}
+		goto attached;
+	}
+
+	// recognize has failed if we get here
+	// report valid board names before returning error
+	for (driv = comedi_drivers; driv; driv = driv->next) {
+		if (!try_module_get(driv->module)) {
+			printk("comedi: failed to increment module count\n");
+			continue;
+		}
+		comedi_report_boards(driv);
+		module_put(driv->module);
+	}
+	return -EIO;
+
+attached:
+	/* do a little post-config cleanup */
+	ret = postconfig(dev);
+	module_put(dev->driver->module);
+	if (ret < 0) {
+		__comedi_device_detach(dev);
+		return ret;
+	}
+
+	if (!dev->board_name) {
+		printk("BUG: dev->board_name=<%p>\n", dev->board_name);
+		dev->board_name = "BUG";
+	}
+	smp_wmb();
+	dev->attached = 1;
+
+	return 0;
+}
+
+int comedi_driver_register(comedi_driver * driver)
+{
+	driver->next = comedi_drivers;
+	comedi_drivers = driver;
+
+	return 0;
+}
+
+int comedi_driver_unregister(comedi_driver * driver)
+{
+	comedi_driver *prev;
+	int i;
+
+	/* check for devices using this driver */
+	for (i = 0; i < COMEDI_NUM_BOARD_MINORS; i++) {
+		struct comedi_device_file_info *dev_file_info = comedi_get_device_file_info(i);
+		comedi_device *dev;
+
+		if(dev_file_info == NULL) continue;
+		dev = dev_file_info->device;
+
+		mutex_lock(&dev->mutex);
+		if (dev->attached && dev->driver == driver) {
+			if (dev->use_count)
+				printk("BUG! detaching device with use_count=%d\n", dev->use_count);
+			comedi_device_detach(dev);
+		}
+		mutex_unlock(&dev->mutex);
+	}
+
+	if (comedi_drivers == driver) {
+		comedi_drivers = driver->next;
+		return 0;
+	}
+
+	for (prev = comedi_drivers; prev->next; prev = prev->next) {
+		if (prev->next == driver) {
+			prev->next = driver->next;
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+static int postconfig(comedi_device * dev)
+{
+	int i;
+	comedi_subdevice *s;
+	comedi_async *async = NULL;
+	int ret;
+
+	for (i = 0; i < dev->n_subdevices; i++) {
+		s = dev->subdevices + i;
+
+		if (s->type == COMEDI_SUBD_UNUSED)
+			continue;
+
+		if (s->len_chanlist == 0)
+			s->len_chanlist = 1;
+
+		if (s->do_cmd) {
+			BUG_ON((s->subdev_flags & (SDF_CMD_READ |
+				SDF_CMD_WRITE)) == 0);
+			BUG_ON(!s->do_cmdtest);
+
+			async = kzalloc(sizeof(comedi_async), GFP_KERNEL);
+			if (async == NULL) {
+				printk("failed to allocate async struct\n");
+				return -ENOMEM;
+			}
+			init_waitqueue_head(&async->wait_head);
+			async->subdevice = s;
+			s->async = async;
+
+#define DEFAULT_BUF_MAXSIZE (64*1024)
+#define DEFAULT_BUF_SIZE (64*1024)
+
+			async->max_bufsize = DEFAULT_BUF_MAXSIZE;
+
+			async->prealloc_buf = NULL;
+			async->prealloc_bufsz = 0;
+			if (comedi_buf_alloc(dev, s, DEFAULT_BUF_SIZE) < 0) {
+				printk("Buffer allocation failed\n");
+				return -ENOMEM;
+			}
+			if (s->buf_change) {
+				ret = s->buf_change(dev, s, DEFAULT_BUF_SIZE);
+				if (ret < 0)
+					return ret;
+			}
+			comedi_alloc_subdevice_minor(dev, s);
+		}
+
+		if (!s->range_table && !s->range_table_list)
+			s->range_table = &range_unknown;
+
+		if (!s->insn_read && s->insn_bits)
+			s->insn_read = insn_rw_emulate_bits;
+		if (!s->insn_write && s->insn_bits)
+			s->insn_write = insn_rw_emulate_bits;
+
+		if (!s->insn_read)
+			s->insn_read = insn_inval;
+		if (!s->insn_write)
+			s->insn_write = insn_inval;
+		if (!s->insn_bits)
+			s->insn_bits = insn_inval;
+		if (!s->insn_config)
+			s->insn_config = insn_inval;
+
+		if (!s->poll)
+			s->poll = poll_invalid;
+	}
+
+	return 0;
+}
+
+// generic recognize function for drivers that register their supported board names
+void *comedi_recognize(comedi_driver * driv, const char *name)
+{
+	unsigned i;
+	const char *const *name_ptr = driv->board_name;
+	for (i = 0; i < driv->num_names; i++) {
+		if (strcmp(*name_ptr, name) == 0)
+			return (void *)name_ptr;
+		name_ptr =
+			(const char *const *)((const char *)name_ptr +
+			driv->offset);
+	}
+
+	return NULL;
+}
+
+void comedi_report_boards(comedi_driver * driv)
+{
+	unsigned int i;
+	const char *const *name_ptr;
+
+	printk("comedi: valid board names for %s driver are:\n",
+		driv->driver_name);
+
+	name_ptr = driv->board_name;
+	for (i = 0; i < driv->num_names; i++) {
+		printk(" %s\n", *name_ptr);
+		name_ptr = (const char **)((char *)name_ptr + driv->offset);
+	}
+
+	if (driv->num_names == 0)
+		printk(" %s\n", driv->driver_name);
+}
+
+static int poll_invalid(comedi_device * dev, comedi_subdevice * s)
+{
+	return -EINVAL;
+}
+
+int insn_inval(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	return -EINVAL;
+}
+
+static int insn_rw_emulate_bits(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	comedi_insn new_insn;
+	int ret;
+	static const unsigned channels_per_bitfield = 32;
+
+	unsigned chan = CR_CHAN(insn->chanspec);
+	const unsigned base_bitfield_channel =
+		(chan < channels_per_bitfield) ? 0 : chan;
+	lsampl_t new_data[2];
+	memset(new_data, 0, sizeof(new_data));
+	memset(&new_insn, 0, sizeof(new_insn));
+	new_insn.insn = INSN_BITS;
+	new_insn.chanspec = base_bitfield_channel;
+	new_insn.n = 2;
+	new_insn.data = new_data;
+	new_insn.subdev = insn->subdev;
+
+	if (insn->insn == INSN_WRITE) {
+		if (!(s->subdev_flags & SDF_WRITABLE))
+			return -EINVAL;
+		new_data[0] = 1 << (chan - base_bitfield_channel);	/* mask */
+		new_data[1] = data[0] ? (1 << (chan - base_bitfield_channel)) : 0;	/* bits */
+	}
+
+	ret = s->insn_bits(dev, s, &new_insn, new_data);
+	if (ret < 0)
+		return ret;
+
+	if (insn->insn == INSN_READ) {
+		data[0] = (new_data[1] >> (chan - base_bitfield_channel)) & 1;
+	}
+
+	return 1;
+}
+
+static inline unsigned long uvirt_to_kva(pgd_t * pgd, unsigned long adr)
+{
+	unsigned long ret = 0UL;
+	pmd_t *pmd;
+	pte_t *ptep, pte;
+	pud_t *pud;
+
+	if (!pgd_none(*pgd)) {
+		pud = pud_offset(pgd, adr);
+		pmd = pmd_offset(pud, adr);
+		if (!pmd_none(*pmd)) {
+			ptep = pte_offset_kernel(pmd, adr);
+			pte = *ptep;
+			if (pte_present(pte)) {
+				ret = (unsigned long)
+					page_address(pte_page(pte));
+				ret |= (adr & (PAGE_SIZE - 1));
+			}
+		}
+	}
+	return ret;
+}
+
+static inline unsigned long kvirt_to_kva(unsigned long adr)
+{
+	unsigned long va, kva;
+
+	va = adr;
+	kva = uvirt_to_kva(pgd_offset_k(va), va);
+
+	return kva;
+}
+
+int comedi_buf_alloc(comedi_device * dev, comedi_subdevice * s,
+	unsigned long new_size)
+{
+	comedi_async *async = s->async;
+
+	/* Round up new_size to multiple of PAGE_SIZE */
+	new_size = (new_size + PAGE_SIZE - 1) & PAGE_MASK;
+
+	/* if no change is required, do nothing */
+	if (async->prealloc_buf && async->prealloc_bufsz == new_size) {
+		return 0;
+	}
+	// deallocate old buffer
+	if (async->prealloc_buf) {
+		vunmap(async->prealloc_buf);
+		async->prealloc_buf = NULL;
+		async->prealloc_bufsz = 0;
+	}
+	if (async->buf_page_list) {
+		unsigned i;
+		for (i = 0; i < async->n_buf_pages; ++i) {
+			if (async->buf_page_list[i].virt_addr) {
+				mem_map_unreserve(virt_to_page(async->
+						buf_page_list[i].virt_addr));
+				if (s->async_dma_dir != DMA_NONE) {
+					dma_free_coherent(dev->hw_dev,
+						PAGE_SIZE,
+						async->buf_page_list[i].
+						virt_addr,
+						async->buf_page_list[i].
+						dma_addr);
+				} else {
+					free_page((unsigned long)async->
+						buf_page_list[i].virt_addr);
+				}
+			}
+		}
+		vfree(async->buf_page_list);
+		async->buf_page_list = NULL;
+		async->n_buf_pages = 0;
+	}
+	// allocate new buffer
+	if (new_size) {
+		unsigned i = 0;
+		unsigned n_pages = new_size >> PAGE_SHIFT;
+		struct page **pages = NULL;
+
+		async->buf_page_list =
+			vmalloc(sizeof(struct comedi_buf_page) * n_pages);
+		if (async->buf_page_list) {
+			memset(async->buf_page_list, 0,
+				sizeof(struct comedi_buf_page) * n_pages);
+			pages = vmalloc(sizeof(struct page *) * n_pages);
+		}
+		if (pages) {
+			for (i = 0; i < n_pages; i++) {
+				if (s->async_dma_dir != DMA_NONE) {
+					async->buf_page_list[i].virt_addr =
+						dma_alloc_coherent(dev->hw_dev,
+						PAGE_SIZE,
+						&async->buf_page_list[i].
+						dma_addr,
+						GFP_KERNEL | __GFP_COMP);
+				} else {
+					async->buf_page_list[i].virt_addr =
+						(void *)
+						get_zeroed_page(GFP_KERNEL);
+				}
+				if (async->buf_page_list[i].virt_addr == NULL) {
+					break;
+				}
+				mem_map_reserve(virt_to_page(async->
+						buf_page_list[i].virt_addr));
+				pages[i] =
+					virt_to_page(async->buf_page_list[i].
+					virt_addr);
+			}
+		}
+		if (i == n_pages) {
+			async->prealloc_buf =
+				vmap(pages, n_pages, VM_MAP,
+				PAGE_KERNEL_NOCACHE);
+		}
+		if (pages) {
+			vfree(pages);
+		}
+		if (async->prealloc_buf == NULL) {
+			/* Some allocation failed above. */
+			if (async->buf_page_list) {
+				for (i = 0; i < n_pages; i++) {
+					if (async->buf_page_list[i].virt_addr ==
+						NULL) {
+						break;
+					}
+					mem_map_unreserve(virt_to_page(async->
+							buf_page_list[i].
+							virt_addr));
+					if (s->async_dma_dir != DMA_NONE) {
+						dma_free_coherent(dev->hw_dev,
+							PAGE_SIZE,
+							async->buf_page_list[i].
+							virt_addr,
+							async->buf_page_list[i].
+							dma_addr);
+					} else {
+						free_page((unsigned long)async->
+							buf_page_list[i].
+							virt_addr);
+					}
+				}
+				vfree(async->buf_page_list);
+				async->buf_page_list = NULL;
+			}
+			return -ENOMEM;
+		}
+		async->n_buf_pages = n_pages;
+	}
+	async->prealloc_bufsz = new_size;
+
+	return 0;
+}
+
+/* munging is applied to data by core as it passes between user
+ * and kernel space */
+unsigned int comedi_buf_munge(comedi_async * async, unsigned int num_bytes)
+{
+	comedi_subdevice *s = async->subdevice;
+	unsigned int count = 0;
+	const unsigned num_sample_bytes = bytes_per_sample(s);
+
+	if (s->munge == NULL || (async->cmd.flags & CMDF_RAWDATA)) {
+		async->munge_count += num_bytes;
+		if ((int)(async->munge_count - async->buf_write_count) > 0)
+			BUG();
+		return num_bytes;
+	}
+	/* don't munge partial samples */
+	num_bytes -= num_bytes % num_sample_bytes;
+	while (count < num_bytes) {
+		int block_size;
+
+		block_size = num_bytes - count;
+		if (block_size < 0) {
+			rt_printk("%s: %s: bug! block_size is negative\n",
+				__FILE__, __func__);
+			break;
+		}
+		if ((int)(async->munge_ptr + block_size -
+				async->prealloc_bufsz) > 0)
+			block_size = async->prealloc_bufsz - async->munge_ptr;
+
+		s->munge(s->device, s, async->prealloc_buf + async->munge_ptr,
+			block_size, async->munge_chan);
+
+		smp_wmb();	//barrier insures data is munged in buffer before munge_count is incremented
+
+		async->munge_chan += block_size / num_sample_bytes;
+		async->munge_chan %= async->cmd.chanlist_len;
+		async->munge_count += block_size;
+		async->munge_ptr += block_size;
+		async->munge_ptr %= async->prealloc_bufsz;
+		count += block_size;
+	}
+	if ((int)(async->munge_count - async->buf_write_count) > 0)
+		BUG();
+	return count;
+}
+
+unsigned int comedi_buf_write_n_available(comedi_async * async)
+{
+	unsigned int free_end;
+	unsigned int nbytes;
+
+	if (async == NULL)
+		return 0;
+
+	free_end = async->buf_read_count + async->prealloc_bufsz;
+	nbytes = free_end - async->buf_write_alloc_count;
+	nbytes -= nbytes % bytes_per_sample(async->subdevice);
+	/* barrier insures the read of buf_read_count in this
+	   query occurs before any following writes to the buffer which
+	   might be based on the return value from this query.
+	 */
+	smp_mb();
+	return nbytes;
+}
+
+/* allocates chunk for the writer from free buffer space */
+unsigned int comedi_buf_write_alloc(comedi_async * async, unsigned int nbytes)
+{
+	unsigned int free_end = async->buf_read_count + async->prealloc_bufsz;
+
+	if ((int)(async->buf_write_alloc_count + nbytes - free_end) > 0) {
+		nbytes = free_end - async->buf_write_alloc_count;
+	}
+	async->buf_write_alloc_count += nbytes;
+	/* barrier insures the read of buf_read_count above occurs before
+	   we write data to the write-alloc'ed buffer space */
+	smp_mb();
+	return nbytes;
+}
+
+/* allocates nothing unless it can completely fulfill the request */
+unsigned int comedi_buf_write_alloc_strict(comedi_async * async,
+	unsigned int nbytes)
+{
+	unsigned int free_end = async->buf_read_count + async->prealloc_bufsz;
+
+	if ((int)(async->buf_write_alloc_count + nbytes - free_end) > 0) {
+		nbytes = 0;
+	}
+	async->buf_write_alloc_count += nbytes;
+	/* barrier insures the read of buf_read_count above occurs before
+	   we write data to the write-alloc'ed buffer space */
+	smp_mb();
+	return nbytes;
+}
+
+/* transfers a chunk from writer to filled buffer space */
+unsigned comedi_buf_write_free(comedi_async * async, unsigned int nbytes)
+{
+	if ((int)(async->buf_write_count + nbytes -
+			async->buf_write_alloc_count) > 0) {
+		rt_printk
+			("comedi: attempted to write-free more bytes than have been write-allocated.\n");
+		nbytes = async->buf_write_alloc_count - async->buf_write_count;
+	}
+	async->buf_write_count += nbytes;
+	async->buf_write_ptr += nbytes;
+	comedi_buf_munge(async, async->buf_write_count - async->munge_count);
+	if (async->buf_write_ptr >= async->prealloc_bufsz) {
+		async->buf_write_ptr %= async->prealloc_bufsz;
+	}
+	return nbytes;
+}
+
+/* allocates a chunk for the reader from filled (and munged) buffer space */
+unsigned comedi_buf_read_alloc(comedi_async * async, unsigned nbytes)
+{
+	if ((int)(async->buf_read_alloc_count + nbytes - async->munge_count) >
+		0) {
+		nbytes = async->munge_count - async->buf_read_alloc_count;
+	}
+	async->buf_read_alloc_count += nbytes;
+	/* barrier insures read of munge_count occurs before we actually read
+	   data out of buffer */
+	smp_rmb();
+	return nbytes;
+}
+
+/* transfers control of a chunk from reader to free buffer space */
+unsigned comedi_buf_read_free(comedi_async * async, unsigned int nbytes)
+{
+	// barrier insures data has been read out of buffer before read count is incremented
+	smp_mb();
+	if ((int)(async->buf_read_count + nbytes -
+			async->buf_read_alloc_count) > 0) {
+		rt_printk
+			("comedi: attempted to read-free more bytes than have been read-allocated.\n");
+		nbytes = async->buf_read_alloc_count - async->buf_read_count;
+	}
+	async->buf_read_count += nbytes;
+	async->buf_read_ptr += nbytes;
+	async->buf_read_ptr %= async->prealloc_bufsz;
+	return nbytes;
+}
+
+void comedi_buf_memcpy_to(comedi_async * async, unsigned int offset,
+	const void *data, unsigned int num_bytes)
+{
+	unsigned int write_ptr = async->buf_write_ptr + offset;
+
+	if (write_ptr >= async->prealloc_bufsz)
+		write_ptr %= async->prealloc_bufsz;
+
+	while (num_bytes) {
+		unsigned int block_size;
+
+		if (write_ptr + num_bytes > async->prealloc_bufsz)
+			block_size = async->prealloc_bufsz - write_ptr;
+		else
+			block_size = num_bytes;
+
+		memcpy(async->prealloc_buf + write_ptr, data, block_size);
+
+		data += block_size;
+		num_bytes -= block_size;
+
+		write_ptr = 0;
+	}
+}
+
+void comedi_buf_memcpy_from(comedi_async * async, unsigned int offset,
+	void *dest, unsigned int nbytes)
+{
+	void *src;
+	unsigned int read_ptr = async->buf_read_ptr + offset;
+
+	if (read_ptr >= async->prealloc_bufsz)
+		read_ptr %= async->prealloc_bufsz;
+
+	while (nbytes) {
+		unsigned int block_size;
+
+		src = async->prealloc_buf + read_ptr;
+
+		if (nbytes >= async->prealloc_bufsz - read_ptr)
+			block_size = async->prealloc_bufsz - read_ptr;
+		else
+			block_size = nbytes;
+
+		memcpy(dest, src, block_size);
+		nbytes -= block_size;
+		dest += block_size;
+		read_ptr = 0;
+	}
+}
+
+unsigned int comedi_buf_read_n_available(comedi_async * async)
+{
+	unsigned num_bytes;
+
+	if (async == NULL)
+		return 0;
+	num_bytes = async->munge_count - async->buf_read_count;
+	/* barrier insures the read of munge_count in this
+	   query occurs before any following reads of the buffer which
+	   might be based on the return value from this query.
+	 */
+	smp_rmb();
+	return num_bytes;
+}
+
+int comedi_buf_get(comedi_async * async, sampl_t * x)
+{
+	unsigned int n = comedi_buf_read_n_available(async);
+
+	if (n < sizeof(sampl_t))
+		return 0;
+	comedi_buf_read_alloc(async, sizeof(sampl_t));
+	*x = *(sampl_t *) (async->prealloc_buf + async->buf_read_ptr);
+	comedi_buf_read_free(async, sizeof(sampl_t));
+	return 1;
+}
+
+int comedi_buf_put(comedi_async * async, sampl_t x)
+{
+	unsigned int n = comedi_buf_write_alloc_strict(async, sizeof(sampl_t));
+
+	if (n < sizeof(sampl_t)) {
+		async->events |= COMEDI_CB_ERROR;
+		return 0;
+	}
+	*(sampl_t *) (async->prealloc_buf + async->buf_write_ptr) = x;
+	comedi_buf_write_free(async, sizeof(sampl_t));
+	return 1;
+}
+
+void comedi_reset_async_buf(comedi_async * async)
+{
+	async->buf_write_alloc_count = 0;
+	async->buf_write_count = 0;
+	async->buf_read_alloc_count = 0;
+	async->buf_read_count = 0;
+
+	async->buf_write_ptr = 0;
+	async->buf_read_ptr = 0;
+
+	async->cur_chan = 0;
+	async->scan_progress = 0;
+	async->munge_chan = 0;
+	async->munge_count = 0;
+	async->munge_ptr = 0;
+
+	async->events = 0;
+}
+
+int comedi_auto_config(struct device *hardware_device, const char *board_name, const int *options, unsigned num_options)
+{
+	comedi_devconfig it;
+	int minor;
+	struct comedi_device_file_info *dev_file_info;
+	int retval;
+
+	minor = comedi_alloc_board_minor(hardware_device);
+	if(minor < 0) return minor;
+	dev_set_drvdata(hardware_device, (void*)(unsigned long)minor);
+
+	dev_file_info = comedi_get_device_file_info(minor);
+
+	memset(&it, 0, sizeof(it));
+	strncpy(it.board_name, board_name, COMEDI_NAMELEN);
+	it.board_name[COMEDI_NAMELEN - 1] = '\0';
+	BUG_ON(num_options > COMEDI_NDEVCONFOPTS);
+	memcpy(it.options, options, num_options * sizeof(int));
+
+	mutex_lock(&dev_file_info->device->mutex);
+	retval = comedi_device_attach(dev_file_info->device, &it);
+	mutex_unlock(&dev_file_info->device->mutex);
+	if(retval < 0)
+	{
+		comedi_free_board_minor(minor);
+	}
+	return retval;
+}
+
+void comedi_auto_unconfig(struct device *hardware_device)
+{
+	unsigned long minor = (unsigned long)dev_get_drvdata(hardware_device);
+
+	BUG_ON(minor >= COMEDI_NUM_BOARD_MINORS);
+
+	comedi_free_board_minor(minor);
+}
+
+int comedi_pci_auto_config(struct pci_dev *pcidev, const char *board_name)
+{
+	int options[2];
+
+	// pci bus
+	options[0] = pcidev->bus->number;
+	// pci slot
+	options[1] = PCI_SLOT(pcidev->devfn);
+
+	return comedi_auto_config(&pcidev->dev, board_name, options, sizeof(options) / sizeof(options[0]));
+}
+
+void comedi_pci_auto_unconfig(struct pci_dev *pcidev)
+{
+	comedi_auto_unconfig(&pcidev->dev);
+}
diff --git a/drivers/staging/comedi/drivers/Makefile b/drivers/staging/comedi/drivers/Makefile
new file mode 100644
index 000000000000..eb7a615e0859
--- /dev/null
+++ b/drivers/staging/comedi/drivers/Makefile
@@ -0,0 +1,21 @@
+# Makefile for individual comedi drivers
+#
+
+# Comedi "helper" modules
+obj-$(CONFIG_COMEDI)			+= comedi_fc.o
+obj-$(CONFIG_COMEDI)			+= comedi_bond.o
+obj-$(CONFIG_COMEDI)			+= comedi_test.o
+obj-$(CONFIG_COMEDI)			+= comedi_parport.o
+
+# Comedi PCI drivers
+obj-$(CONFIG_COMEDI_PCI_DRIVERS)	+= mite.o
+obj-$(CONFIG_COMEDI_PCI_DRIVERS)	+= icp_multi.o
+obj-$(CONFIG_COMEDI_PCI_DRIVERS)	+= me_daq.o
+obj-$(CONFIG_COMEDI_PCI_DRIVERS)	+= me4000.o
+obj-$(CONFIG_COMEDI_PCI_DRIVERS)	+= rtd520.o
+obj-$(CONFIG_COMEDI_PCI_DRIVERS)	+= s626.o
+
+# Comedi USB drivers
+obj-$(CONFIG_COMEDI_USB_DRIVERS)	+= usbdux.o
+obj-$(CONFIG_COMEDI_USB_DRIVERS)	+= usbduxfast.o
+obj-$(CONFIG_COMEDI_USB_DRIVERS)	+= dt9812.o
diff --git a/drivers/staging/comedi/drivers/comedi_bond.c b/drivers/staging/comedi/drivers/comedi_bond.c
new file mode 100644
index 000000000000..9e5496f4f1ae
--- /dev/null
+++ b/drivers/staging/comedi/drivers/comedi_bond.c
@@ -0,0 +1,535 @@
+/*
+    comedi/drivers/comedi_bond.c
+    A Comedi driver to 'bond' or merge multiple drivers and devices as one.
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+    Copyright (C) 2005 Calin A. Culianu <calin@ajvar.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+/*
+Driver: comedi_bond
+Description: A driver to 'bond' (merge) multiple subdevices from multiple
+	     devices together as one.
+Devices:
+Author: ds
+Updated: Mon, 10 Oct 00:18:25 -0500
+Status: works
+
+This driver allows you to 'bond' (merge) multiple comedi subdevices
+(coming from possibly difference boards and/or drivers) together.  For
+example, if you had a board with 2 different DIO subdevices, and
+another with 1 DIO subdevice, you could 'bond' them with this driver
+so that they look like one big fat DIO subdevice.  This makes writing
+applications slightly easier as you don't have to worry about managing
+different subdevices in the application -- you just worry about
+indexing one linear array of channel id's.
+
+Right now only DIO subdevices are supported as that's the personal itch
+I am scratching with this driver.  If you want to add support for AI and AO
+subdevs, go right on ahead and do so!
+
+Commands aren't supported -- although it would be cool if they were.
+
+Configuration Options:
+  List of comedi-minors to bond.  All subdevices of the same type
+  within each minor will be concatenated together in the order given here.
+*/
+
+/*
+ * The previous block comment is used to automatically generate
+ * documentation in Comedi and Comedilib.  The fields:
+ *
+ * Driver: the name of the driver
+ * Description: a short phrase describing the driver.  Don't list boards.
+ * Devices: a full list of the boards that attempt to be supported by
+ *   the driver.  Format is "(manufacturer) board name [comedi name]",
+ *   where comedi_name is the name that is used to configure the board.
+ *   See the comment near board_name: in the comedi_driver structure
+ *   below.  If (manufacturer) or [comedi name] is missing, the previous
+ *   value is used.
+ * Author: you
+ * Updated: date when the _documentation_ was last updated.  Use 'date -R'
+ *   to get a value for this.
+ * Status: a one-word description of the status.  Valid values are:
+ *   works - driver works correctly on most boards supported, and
+ *     passes comedi_test.
+ *   unknown - unknown.  Usually put there by ds.
+ *   experimental - may not work in any particular release.  Author
+ *     probably wants assistance testing it.
+ *   bitrotten - driver has not been update in a long time, probably
+ *     doesn't work, and probably is missing support for significant
+ *     Comedi interface features.
+ *   untested - author probably wrote it "blind", and is believed to
+ *     work, but no confirmation.
+ *
+ * These headers should be followed by a blank line, and any comments
+ * you wish to say about the driver.  The comment area is the place
+ * to put any known bugs, limitations, unsupported features, supported
+ * command triggers, whether or not commands are supported on particular
+ * subdevices, etc.
+ *
+ * Somewhere in the comment should be information about configuration
+ * options that are used with comedi_config.
+ */
+
+#include "../comedilib.h"
+#include "../comedidev.h"
+#include <linux/string.h>
+
+/* The maxiumum number of channels per subdevice. */
+#define MAX_CHANS 256
+
+#define MODULE_NAME "comedi_bond"
+#ifdef MODULE_LICENSE
+MODULE_LICENSE("GPL");
+#endif
+#ifndef STR
+#  define STR1(x) #x
+#  define STR(x) STR1(x)
+#endif
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "If true, print extra cryptic debugging output useful"
+		 "only to developers.");
+
+#define LOG_MSG(x...) printk(KERN_INFO MODULE_NAME": "x)
+#define DEBUG(x...)							\
+	do {								\
+		if (debug)						\
+			printk(KERN_DEBUG MODULE_NAME": DEBUG: "x);	\
+	} while (0)
+#define WARNING(x...)  printk(KERN_WARNING MODULE_NAME ": WARNING: "x)
+#define ERROR(x...)  printk(KERN_ERR MODULE_NAME ": INTERNAL ERROR: "x)
+MODULE_AUTHOR("Calin A. Culianu");
+MODULE_DESCRIPTION(MODULE_NAME "A driver for COMEDI to bond multiple COMEDI "
+		   "devices together as one.  In the words of John Lennon: "
+		   "'And the world will live as one...'");
+
+/*
+ * Board descriptions for two imaginary boards.  Describing the
+ * boards in this way is optional, and completely driver-dependent.
+ * Some drivers use arrays such as this, other do not.
+ */
+struct BondingBoard {
+	const char *name;
+};
+
+static const struct BondingBoard bondingBoards[] = {
+	{
+		.name =	MODULE_NAME,
+	},
+};
+
+/*
+ * Useful for shorthand access to the particular board structure
+ */
+#define thisboard ((const struct BondingBoard *)dev->board_ptr)
+
+struct BondedDevice {
+	comedi_t *dev;
+	unsigned minor;
+	unsigned subdev;
+	unsigned subdev_type;
+	unsigned nchans;
+	unsigned chanid_offset;	/* The offset into our unified linear
+				   channel-id's of chanid 0 on this
+				   subdevice. */
+};
+
+/* this structure is for data unique to this hardware driver.  If
+   several hardware drivers keep similar information in this structure,
+   feel free to suggest moving the variable to the comedi_device struct.  */
+struct Private {
+# define MAX_BOARD_NAME 256
+	char name[MAX_BOARD_NAME];
+	struct BondedDevice **devs;
+	unsigned ndevs;
+	struct BondedDevice *chanIdDevMap[MAX_CHANS];
+	unsigned nchans;
+};
+
+/*
+ * most drivers define the following macro to make it easy to
+ * access the private structure.
+ */
+#define devpriv ((struct Private *)dev->private)
+
+/*
+ * The comedi_driver structure tells the Comedi core module
+ * which functions to call to configure/deconfigure (attach/detach)
+ * the board, and also about the kernel module that contains
+ * the device code.
+ */
+static int bonding_attach(comedi_device *dev, comedi_devconfig *it);
+static int bonding_detach(comedi_device *dev);
+/** Build Private array of all devices.. */
+static int doDevConfig(comedi_device *dev, comedi_devconfig *it);
+static void doDevUnconfig(comedi_device *dev);
+/* Ugly implementation of realloc that always copies memory around -- I'm lazy,
+ * what can I say?  I like to do wasteful memcopies.. :) */
+static void *Realloc(const void *ptr, size_t len, size_t old_len);
+
+static comedi_driver driver_bonding = {
+      .driver_name =	MODULE_NAME,
+      .module =		THIS_MODULE,
+      .attach =		bonding_attach,
+      .detach =		bonding_detach,
+	/* It is not necessary to implement the following members if you are
+	 * writing a driver for a ISA PnP or PCI card */
+	/* Most drivers will support multiple types of boards by
+	 * having an array of board structures.  These were defined
+	 * in skel_boards[] above.  Note that the element 'name'
+	 * was first in the structure -- Comedi uses this fact to
+	 * extract the name of the board without knowing any details
+	 * about the structure except for its length.
+	 * When a device is attached (by comedi_config), the name
+	 * of the device is given to Comedi, and Comedi tries to
+	 * match it by going through the list of board names.  If
+	 * there is a match, the address of the pointer is put
+	 * into dev->board_ptr and driver->attach() is called.
+	 *
+	 * Note that these are not necessary if you can determine
+	 * the type of board in software.  ISA PnP, PCI, and PCMCIA
+	 * devices are such boards.
+	 */
+      .board_name =	&bondingBoards[0].name,
+      .offset =		sizeof(struct BondingBoard),
+      .num_names =	sizeof(bondingBoards) / sizeof(struct BondingBoard),
+};
+
+static int bonding_dio_insn_bits(comedi_device *dev, comedi_subdevice *s,
+				 comedi_insn *insn, lsampl_t *data);
+static int bonding_dio_insn_config(comedi_device *dev, comedi_subdevice *s,
+				   comedi_insn *insn, lsampl_t *data);
+
+/*
+ * Attach is called by the Comedi core to configure the driver
+ * for a particular board.  If you specified a board_name array
+ * in the driver structure, dev->board_ptr contains that
+ * address.
+ */
+static int bonding_attach(comedi_device *dev, comedi_devconfig *it)
+{
+	comedi_subdevice *s;
+
+	LOG_MSG("comedi%d\n", dev->minor);
+
+	/*
+	 * Allocate the private structure area.  alloc_private() is a
+	 * convenient macro defined in comedidev.h.
+	 */
+	if (alloc_private(dev, sizeof(struct Private)) < 0)
+		return -ENOMEM;
+
+	/*
+	 * Setup our bonding from config params.. sets up our Private struct..
+	 */
+	if (!doDevConfig(dev, it))
+		return -EINVAL;
+
+	/*
+	 * Initialize dev->board_name.  Note that we can use the "thisboard"
+	 * macro now, since we just initialized it in the last line.
+	 */
+	dev->board_name = devpriv->name;
+
+	/*
+	 * Allocate the subdevice structures.  alloc_subdevice() is a
+	 * convenient macro defined in comedidev.h.
+	 */
+	if (alloc_subdevices(dev, 1) < 0)
+		return -ENOMEM;
+
+	s = dev->subdevices + 0;
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = devpriv->nchans;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = bonding_dio_insn_bits;
+	s->insn_config = bonding_dio_insn_config;
+
+	LOG_MSG("attached with %u DIO channels coming from %u different "
+		"subdevices all bonded together.  "
+		"John Lennon would be proud!\n",
+		devpriv->nchans, devpriv->ndevs);
+
+	return 1;
+}
+
+/*
+ * _detach is called to deconfigure a device.  It should deallocate
+ * resources.
+ * This function is also called when _attach() fails, so it should be
+ * careful not to release resources that were not necessarily
+ * allocated by _attach().  dev->private and dev->subdevices are
+ * deallocated automatically by the core.
+ */
+static int bonding_detach(comedi_device *dev)
+{
+	LOG_MSG("comedi%d: remove\n", dev->minor);
+	doDevUnconfig(dev);
+	return 0;
+}
+
+/* DIO devices are slightly special.  Although it is possible to
+ * implement the insn_read/insn_write interface, it is much more
+ * useful to applications if you implement the insn_bits interface.
+ * This allows packed reading/writing of the DIO channels.  The
+ * comedi core can convert between insn_bits and insn_read/write */
+static int bonding_dio_insn_bits(comedi_device *dev, comedi_subdevice *s,
+				 comedi_insn *insn, lsampl_t *data)
+{
+#define LSAMPL_BITS (sizeof(lsampl_t)*8)
+	unsigned nchans = LSAMPL_BITS, num_done = 0, i;
+	if (insn->n != 2)
+		return -EINVAL;
+
+	if (devpriv->nchans < nchans)
+		nchans = devpriv->nchans;
+
+	/* The insn data is a mask in data[0] and the new data
+	 * in data[1], each channel cooresponding to a bit. */
+	for (i = 0; num_done < nchans && i < devpriv->ndevs; ++i) {
+		struct BondedDevice *bdev = devpriv->devs[i];
+		/* Grab the channel mask and data of only the bits corresponding
+		   to this subdevice.. need to shift them to zero position of
+		   course. */
+		/* Bits corresponding to this subdev. */
+		lsampl_t subdevMask = ((1 << bdev->nchans) - 1);
+		lsampl_t writeMask, dataBits;
+
+		/* Argh, we have >= LSAMPL_BITS chans.. take all bits */
+		if (bdev->nchans >= LSAMPL_BITS)
+			subdevMask = (lsampl_t) (-1);
+
+		writeMask = (data[0] >> num_done) & subdevMask;
+		dataBits = (data[1] >> num_done) & subdevMask;
+
+		/* Read/Write the new digital lines */
+		if (comedi_dio_bitfield(bdev->dev, bdev->subdev, writeMask,
+				&dataBits) != 2)
+			return -EINVAL;
+
+		/* Make room for the new bits in data[1], the return value */
+		data[1] &= ~(subdevMask << num_done);
+		/* Put the bits in the return value */
+		data[1] |= (dataBits & subdevMask) << num_done;
+		/* Save the new bits to the saved state.. */
+		s->state = data[1];
+
+		num_done += bdev->nchans;
+	}
+
+	return insn->n;
+}
+
+static int bonding_dio_insn_config(comedi_device *dev, comedi_subdevice *s,
+				   comedi_insn *insn, lsampl_t *data)
+{
+	int chan = CR_CHAN(insn->chanspec), ret, io_bits = s->io_bits;
+	unsigned int io;
+	struct BondedDevice *bdev;
+
+	if (chan < 0 || chan >= devpriv->nchans)
+		return -EINVAL;
+	bdev = devpriv->chanIdDevMap[chan];
+
+	/* The input or output configuration of each digital line is
+	 * configured by a special insn_config instruction.  chanspec
+	 * contains the channel to be changed, and data[0] contains the
+	 * value COMEDI_INPUT or COMEDI_OUTPUT. */
+	switch (data[0]) {
+	case INSN_CONFIG_DIO_OUTPUT:
+		io = COMEDI_OUTPUT;	/* is this really necessary? */
+		io_bits |= 1 << chan;
+		break;
+	case INSN_CONFIG_DIO_INPUT:
+		io = COMEDI_INPUT;	/* is this really necessary? */
+		io_bits &= ~(1 << chan);
+		break;
+	case INSN_CONFIG_DIO_QUERY:
+		data[1] =
+			(io_bits & (1 << chan)) ? COMEDI_OUTPUT : COMEDI_INPUT;
+		return insn->n;
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+	/* 'real' channel id for this subdev.. */
+	chan -= bdev->chanid_offset;
+	ret = comedi_dio_config(bdev->dev, bdev->subdev, chan, io);
+	if (ret != 1)
+		return -EINVAL;
+	/* Finally, save the new io_bits values since we didn't get
+	   an error above. */
+	s->io_bits = io_bits;
+	return insn->n;
+}
+
+static void *Realloc(const void *oldmem, size_t newlen, size_t oldlen)
+{
+	void *newmem = kmalloc(newlen, GFP_KERNEL);
+
+	if (newmem && oldmem)
+		memcpy(newmem, oldmem, min(oldlen, newlen));
+	kfree(oldmem);
+	return newmem;
+}
+
+static int doDevConfig(comedi_device *dev, comedi_devconfig *it)
+{
+	int i;
+	comedi_t *devs_opened[COMEDI_NUM_BOARD_MINORS];
+
+	memset(devs_opened, 0, sizeof(devs_opened));
+	devpriv->name[0] = 0;;
+	/* Loop through all comedi devices specified on the command-line,
+	   building our device list */
+	for (i = 0; i < COMEDI_NDEVCONFOPTS && (!i || it->options[i]); ++i) {
+		char file[] = "/dev/comediXXXXXX";
+		int minor = it->options[i];
+		comedi_t *d;
+		int sdev = -1, nchans, tmp;
+		struct BondedDevice *bdev = NULL;
+
+		if (minor < 0 || minor > COMEDI_NUM_BOARD_MINORS) {
+			ERROR("Minor %d is invalid!\n", minor);
+			return 0;
+		}
+		if (minor == dev->minor) {
+			ERROR("Cannot bond this driver to itself!\n");
+			return 0;
+		}
+		if (devs_opened[minor]) {
+			ERROR("Minor %d specified more than once!\n", minor);
+			return 0;
+		}
+
+		snprintf(file, sizeof(file), "/dev/comedi%u", minor);
+		file[sizeof(file) - 1] = 0;
+
+		d = devs_opened[minor] = comedi_open(file);
+
+		if (!d) {
+			ERROR("Minor %u could not be opened\n", minor);
+			return 0;
+		}
+
+		/* Do DIO, as that's all we support now.. */
+		while ((sdev = comedi_find_subdevice_by_type(d, COMEDI_SUBD_DIO,
+					sdev + 1)) > -1) {
+			nchans = comedi_get_n_channels(d, sdev);
+			if (nchans <= 0) {
+				ERROR("comedi_get_n_channels() returned %d "
+				      "on minor %u subdev %d!\n",
+				      nchans, minor, sdev);
+				return 0;
+			}
+			bdev = kmalloc(sizeof(*bdev), GFP_KERNEL);
+			if (!bdev) {
+				ERROR("Out of memory.\n");
+				return 0;
+			}
+			bdev->dev = d;
+			bdev->minor = minor;
+			bdev->subdev = sdev;
+			bdev->subdev_type = COMEDI_SUBD_DIO;
+			bdev->nchans = nchans;
+			bdev->chanid_offset = devpriv->nchans;
+
+			/* map channel id's to BondedDevice * pointer.. */
+			while (nchans--)
+				devpriv->chanIdDevMap[devpriv->nchans++] = bdev;
+
+			/* Now put bdev pointer at end of devpriv->devs array
+			 * list.. */
+
+			/* ergh.. ugly.. we need to realloc :(  */
+			tmp = devpriv->ndevs * sizeof(bdev);
+			devpriv->devs =
+				Realloc(devpriv->devs,
+				++devpriv->ndevs * sizeof(bdev), tmp);
+			if (!devpriv->devs) {
+				ERROR("Could not allocate memory. "
+				      "Out of memory?");
+				return 0;
+			}
+
+			devpriv->devs[devpriv->ndevs - 1] = bdev;
+			{
+	/** Append dev:subdev to devpriv->name */
+				char buf[20];
+				int left =
+					MAX_BOARD_NAME - strlen(devpriv->name) -
+					1;
+				snprintf(buf, sizeof(buf), "%d:%d ", dev->minor,
+					bdev->subdev);
+				buf[sizeof(buf) - 1] = 0;
+				strncat(devpriv->name, buf, left);
+			}
+
+		}
+	}
+
+	if (!devpriv->nchans) {
+		ERROR("No channels found!\n");
+		return 0;
+	}
+
+	return 1;
+}
+
+static void doDevUnconfig(comedi_device *dev)
+{
+	unsigned long devs_closed = 0;
+
+	if (devpriv) {
+		while (devpriv->ndevs-- && devpriv->devs) {
+			struct BondedDevice *bdev;
+
+			bdev = devpriv->devs[devpriv->ndevs];
+			if (!bdev)
+				continue;
+			if (!(devs_closed & (0x1 << bdev->minor))) {
+				comedi_close(bdev->dev);
+				devs_closed |= (0x1 << bdev->minor);
+			}
+			kfree(bdev);
+		}
+		kfree(devpriv->devs);
+		devpriv->devs = NULL;
+		kfree(devpriv);
+		dev->private = NULL;
+	}
+}
+
+static int __init init(void)
+{
+	return comedi_driver_register(&driver_bonding);
+}
+
+static void __exit cleanup(void)
+{
+	comedi_driver_unregister(&driver_bonding);
+}
+
+module_init(init);
+module_exit(cleanup);
diff --git a/drivers/staging/comedi/drivers/comedi_fc.c b/drivers/staging/comedi/drivers/comedi_fc.c
new file mode 100644
index 000000000000..cd74dbe5417d
--- /dev/null
+++ b/drivers/staging/comedi/drivers/comedi_fc.c
@@ -0,0 +1,118 @@
+/*
+    comedi/drivers/comedi_fc.c
+
+    This is a place for code driver writers wish to share between
+    two or more drivers.  fc is short
+    for frank-common.
+
+    Author:  Frank Mori Hess <fmhess@users.sourceforge.net>
+    Copyright (C) 2002 Frank Mori Hess
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+************************************************************************/
+
+#include "../comedidev.h"
+
+#include "comedi_fc.h"
+
+static void increment_scan_progress(comedi_subdevice *subd,
+				    unsigned int num_bytes)
+{
+	comedi_async *async = subd->async;
+	unsigned int scan_length = cfc_bytes_per_scan(subd);
+
+	async->scan_progress += num_bytes;
+	if (async->scan_progress >= scan_length) {
+		async->scan_progress %= scan_length;
+		async->events |= COMEDI_CB_EOS;
+	}
+}
+
+/* Writes an array of data points to comedi's buffer */
+unsigned int cfc_write_array_to_buffer(comedi_subdevice *subd, void *data,
+				       unsigned int num_bytes)
+{
+	comedi_async *async = subd->async;
+	unsigned int retval;
+
+	if (num_bytes == 0)
+		return 0;
+
+	retval = comedi_buf_write_alloc(async, num_bytes);
+	if (retval != num_bytes) {
+		rt_printk("comedi: 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);
+	increment_scan_progress(subd, num_bytes);
+	async->events |= COMEDI_CB_BLOCK;
+
+	return num_bytes;
+}
+EXPORT_SYMBOL(cfc_write_array_to_buffer);
+
+unsigned int cfc_read_array_from_buffer(comedi_subdevice *subd, void *data,
+					unsigned int num_bytes)
+{
+	comedi_async *async = subd->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);
+	increment_scan_progress(subd, num_bytes);
+	async->events |= COMEDI_CB_BLOCK;
+
+	return num_bytes;
+}
+EXPORT_SYMBOL(cfc_read_array_from_buffer);
+
+unsigned int cfc_handle_events(comedi_device *dev, comedi_subdevice *subd)
+{
+	unsigned int events = subd->async->events;
+
+	if (events == 0)
+		return events;
+
+	if (events & (COMEDI_CB_EOA | COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW))
+		subd->cancel(dev, subd);
+
+	comedi_event(dev, subd);
+
+	return events;
+}
+EXPORT_SYMBOL(cfc_handle_events);
+
+MODULE_AUTHOR("Frank Mori Hess <fmhess@users.sourceforge.net>");
+MODULE_DESCRIPTION("Shared functions for Comedi low-level drivers");
+MODULE_LICENSE("GPL");
+
+static int __init comedi_fc_init_module(void)
+{
+	return 0;
+}
+
+static void __exit comedi_fc_cleanup_module(void)
+{
+}
+
+module_init(comedi_fc_init_module);
+module_exit(comedi_fc_cleanup_module);
diff --git a/drivers/staging/comedi/drivers/comedi_fc.h b/drivers/staging/comedi/drivers/comedi_fc.h
new file mode 100644
index 000000000000..6952fe20f273
--- /dev/null
+++ b/drivers/staging/comedi/drivers/comedi_fc.h
@@ -0,0 +1,76 @@
+/*
+    comedi_fc.h
+
+    This is a place for code driver writers wish to share between
+    two or more drivers. These functions are meant to be used only
+    by drivers, they are NOT part of the kcomedilib API!
+
+    Author:  Frank Mori Hess <fmhess@users.sourceforge.net>
+    Copyright (C) 2002 Frank Mori Hess
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+************************************************************************/
+
+#ifndef _COMEDI_FC_H
+#define _COMEDI_FC_H
+
+#include "../comedidev.h"
+
+/* Writes an array of data points to comedi's buffer */
+extern unsigned int cfc_write_array_to_buffer(comedi_subdevice *subd,
+					      void *data,
+					      unsigned int num_bytes);
+
+static inline unsigned int cfc_write_to_buffer(comedi_subdevice *subd,
+					       sampl_t data)
+{
+	return cfc_write_array_to_buffer(subd, &data, sizeof(data));
+};
+
+static inline unsigned int cfc_write_long_to_buffer(comedi_subdevice *subd,
+						    lsampl_t data)
+{
+	return cfc_write_array_to_buffer(subd, &data, sizeof(data));
+};
+
+extern unsigned int cfc_read_array_from_buffer(comedi_subdevice *subd,
+					       void *data,
+					       unsigned int num_bytes);
+
+extern unsigned int cfc_handle_events(comedi_device *dev,
+				      comedi_subdevice *subd);
+
+static inline unsigned int cfc_bytes_per_scan(comedi_subdevice *subd)
+{
+	int num_samples;
+	int bits_per_sample;
+
+	switch (subd->type) {
+	case COMEDI_SUBD_DI:
+	case COMEDI_SUBD_DO:
+	case COMEDI_SUBD_DIO:
+		bits_per_sample = 8 * bytes_per_sample(subd);
+		num_samples = (subd->async->cmd.chanlist_len +
+			       bits_per_sample - 1) / bits_per_sample;
+		break;
+	default:
+		num_samples = subd->async->cmd.chanlist_len;
+		break;
+	}
+	return num_samples * bytes_per_sample(subd);
+}
+
+#endif /* _COMEDI_FC_H */
diff --git a/drivers/staging/comedi/drivers/comedi_parport.c b/drivers/staging/comedi/drivers/comedi_parport.c
new file mode 100644
index 000000000000..ba838fffa29e
--- /dev/null
+++ b/drivers/staging/comedi/drivers/comedi_parport.c
@@ -0,0 +1,390 @@
+/*
+    comedi/drivers/comedi_parport.c
+    hardware driver for standard parallel port
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1998,2001 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+/*
+Driver: comedi_parport
+Description: Standard PC parallel port
+Author: ds
+Status: works in immediate mode
+Devices: [standard] parallel port (comedi_parport)
+Updated: Tue, 30 Apr 2002 21:11:45 -0700
+
+A cheap and easy way to get a few more digital I/O lines.  Steal
+additional parallel ports from old computers or your neighbors'
+computers.
+
+Option list:
+ 0: I/O port base for the parallel port.
+ 1: IRQ
+
+Parallel Port Lines:
+
+pin     subdev  chan    aka
+---     ------  ----    ---
+1       2       0       strobe
+2       0       0       data 0
+3       0       1       data 1
+4       0       2       data 2
+5       0       3       data 3
+6       0       4       data 4
+7       0       5       data 5
+8       0       6       data 6
+9       0       7       data 7
+10      1       3       acknowledge
+11      1       4       busy
+12      1       2       output
+13      1       1       printer selected
+14      2       1       auto LF
+15      1       0       error
+16      2       2       init
+17      2       3       select printer
+18-25   ground
+
+Notes:
+
+Subdevices 0 is digital I/O, subdevice 1 is digital input, and
+subdevice 2 is digital output.  Unlike other Comedi devices,
+subdevice 0 defaults to output.
+
+Pins 13 and 14 are inverted once by Comedi and once by the
+hardware, thus cancelling the effect.
+
+Pin 1 is a strobe, thus acts like one.  There's no way in software
+to change this, at least on a standard parallel port.
+
+Subdevice 3 pretends to be a digital input subdevice, but it always
+returns 0 when read.  However, if you run a command with
+scan_begin_src=TRIG_EXT, it uses pin 10 as a external triggering
+pin, which can be used to wake up tasks.
+*/
+/*
+   see http://www.beyondlogic.org/ for information.
+   or http://www.linux-magazin.de/ausgabe/1999/10/IO/io.html
+ */
+
+#include "../comedidev.h"
+#include <linux/ioport.h>
+
+#define PARPORT_SIZE 3
+
+#define PARPORT_A 0
+#define PARPORT_B 1
+#define PARPORT_C 2
+
+static int parport_attach(comedi_device *dev, comedi_devconfig *it);
+static int parport_detach(comedi_device *dev);
+static comedi_driver driver_parport = {
+      .driver_name =	"comedi_parport",
+      .module =		THIS_MODULE,
+      .attach =		parport_attach,
+      .detach =		parport_detach,
+};
+
+COMEDI_INITCLEANUP(driver_parport);
+
+struct parport_private {
+	unsigned int a_data;
+	unsigned int c_data;
+	int enable_irq;
+};
+#define devpriv ((struct parport_private *)(dev->private))
+
+static int parport_insn_a(comedi_device *dev, comedi_subdevice *s,
+			  comedi_insn *insn, lsampl_t *data)
+{
+	if (data[0]) {
+		devpriv->a_data &= ~data[0];
+		devpriv->a_data |= (data[0] & data[1]);
+
+		outb(devpriv->a_data, dev->iobase + PARPORT_A);
+	}
+
+	data[1] = inb(dev->iobase + PARPORT_A);
+
+	return 2;
+}
+
+static int parport_insn_config_a(comedi_device *dev, comedi_subdevice *s,
+				 comedi_insn *insn, lsampl_t *data)
+{
+	if (data[0]) {
+		s->io_bits = 0xff;
+		devpriv->c_data &= ~(1 << 5);
+	} else {
+		s->io_bits = 0;
+		devpriv->c_data |= (1 << 5);
+	}
+	outb(devpriv->c_data, dev->iobase + PARPORT_C);
+
+	return 1;
+}
+
+static int parport_insn_b(comedi_device *dev, comedi_subdevice *s,
+			  comedi_insn *insn, lsampl_t *data)
+{
+	if (data[0]) {
+		/* should writes be ignored? */
+		/* anyone??? */
+	}
+
+	data[1] = (inb(dev->iobase + PARPORT_B) >> 3);
+
+	return 2;
+}
+
+static int parport_insn_c(comedi_device *dev, comedi_subdevice *s,
+			  comedi_insn *insn, lsampl_t *data)
+{
+	data[0] &= 0x0f;
+	if (data[0]) {
+		devpriv->c_data &= ~data[0];
+		devpriv->c_data |= (data[0] & data[1]);
+
+		outb(devpriv->c_data, dev->iobase + PARPORT_C);
+	}
+
+	data[1] = devpriv->c_data & 0xf;
+
+	return 2;
+}
+
+static int parport_intr_insn(comedi_device *dev, comedi_subdevice *s,
+			     comedi_insn *insn, lsampl_t *data)
+{
+	if (insn->n < 1)
+		return -EINVAL;
+
+	data[1] = 0;
+	return 2;
+}
+
+static int parport_intr_cmdtest(comedi_device *dev, comedi_subdevice *s,
+				comedi_cmd *cmd)
+{
+	int err = 0;
+	int tmp;
+
+	/* step 1 */
+
+	tmp = cmd->start_src;
+	cmd->start_src &= TRIG_NOW;
+	if (!cmd->start_src || tmp != cmd->start_src)
+		err++;
+
+	tmp = cmd->scan_begin_src;
+	cmd->scan_begin_src &= TRIG_EXT;
+	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
+		err++;
+
+	tmp = cmd->convert_src;
+	cmd->convert_src &= TRIG_FOLLOW;
+	if (!cmd->convert_src || tmp != cmd->convert_src)
+		err++;
+
+	tmp = cmd->scan_end_src;
+	cmd->scan_end_src &= TRIG_COUNT;
+	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
+		err++;
+
+	tmp = cmd->stop_src;
+	cmd->stop_src &= TRIG_NONE;
+	if (!cmd->stop_src || tmp != cmd->stop_src)
+		err++;
+
+	if (err)
+		return 1;
+
+	/* step 2: ignored */
+
+	if (err)
+		return 2;
+
+	/* step 3: */
+
+	if (cmd->start_arg != 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+	if (cmd->scan_begin_arg != 0) {
+		cmd->scan_begin_arg = 0;
+		err++;
+	}
+	if (cmd->convert_arg != 0) {
+		cmd->convert_arg = 0;
+		err++;
+	}
+	if (cmd->scan_end_arg != 1) {
+		cmd->scan_end_arg = 1;
+		err++;
+	}
+	if (cmd->stop_arg != 0) {
+		cmd->stop_arg = 0;
+		err++;
+	}
+
+	if (err)
+		return 3;
+
+	/* step 4: ignored */
+
+	if (err)
+		return 4;
+
+	return 0;
+}
+
+static int parport_intr_cmd(comedi_device *dev, comedi_subdevice *s)
+{
+	devpriv->c_data |= 0x10;
+	outb(devpriv->c_data, dev->iobase + PARPORT_C);
+
+	devpriv->enable_irq = 1;
+
+	return 0;
+}
+
+static int parport_intr_cancel(comedi_device *dev, comedi_subdevice *s)
+{
+	printk(KERN_DEBUG "parport_intr_cancel()\n");
+
+	devpriv->c_data &= ~0x10;
+	outb(devpriv->c_data, dev->iobase + PARPORT_C);
+
+	devpriv->enable_irq = 0;
+
+	return 0;
+}
+
+static irqreturn_t parport_interrupt(int irq, void *d PT_REGS_ARG)
+{
+	comedi_device *dev = d;
+	comedi_subdevice *s = dev->subdevices + 3;
+
+	if (!devpriv->enable_irq) {
+		printk(KERN_ERR "comedi_parport: bogus irq, ignored\n");
+		return IRQ_NONE;
+	}
+
+	comedi_buf_put(s->async, 0);
+	s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
+
+	comedi_event(dev, s);
+	return IRQ_HANDLED;
+}
+
+static int parport_attach(comedi_device *dev, comedi_devconfig *it)
+{
+	int ret;
+	unsigned int irq;
+	unsigned long iobase;
+	comedi_subdevice *s;
+
+	iobase = it->options[0];
+	printk(KERN_INFO "comedi%d: parport: 0x%04lx ", dev->minor, iobase);
+	if (!request_region(iobase, PARPORT_SIZE, "parport (comedi)")) {
+		printk("I/O port conflict\n");
+		return -EIO;
+	}
+	dev->iobase = iobase;
+
+	irq = it->options[1];
+	if (irq) {
+		printk(" irq=%u", irq);
+		ret = comedi_request_irq(irq, parport_interrupt, 0,
+			"comedi_parport", dev);
+		if (ret < 0) {
+			printk(" irq not available\n");
+			return -EINVAL;
+		}
+		dev->irq = irq;
+	}
+	dev->board_name = "parport";
+
+	ret = alloc_subdevices(dev, 4);
+	if (ret < 0)
+		return ret;
+	ret = alloc_private(dev, sizeof(struct parport_private));
+	if (ret < 0)
+		return ret;
+
+	s = dev->subdevices + 0;
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 8;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = parport_insn_a;
+	s->insn_config = parport_insn_config_a;
+
+	s = dev->subdevices + 1;
+	s->type = COMEDI_SUBD_DI;
+	s->subdev_flags = SDF_READABLE;
+	s->n_chan = 5;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = parport_insn_b;
+
+	s = dev->subdevices + 2;
+	s->type = COMEDI_SUBD_DO;
+	s->subdev_flags = SDF_WRITABLE;
+	s->n_chan = 4;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = parport_insn_c;
+
+	s = dev->subdevices + 3;
+	if (irq) {
+		dev->read_subdev = s;
+		s->type = COMEDI_SUBD_DI;
+		s->subdev_flags = SDF_READABLE | SDF_CMD_READ;
+		s->n_chan = 1;
+		s->maxdata = 1;
+		s->range_table = &range_digital;
+		s->insn_bits = parport_intr_insn;
+		s->do_cmdtest = parport_intr_cmdtest;
+		s->do_cmd = parport_intr_cmd;
+		s->cancel = parport_intr_cancel;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+	devpriv->a_data = 0;
+	outb(devpriv->a_data, dev->iobase + PARPORT_A);
+	devpriv->c_data = 0;
+	outb(devpriv->c_data, dev->iobase + PARPORT_C);
+
+	printk("\n");
+	return 1;
+}
+
+static int parport_detach(comedi_device *dev)
+{
+	printk("comedi%d: parport: remove\n", dev->minor);
+
+	if (dev->iobase)
+		release_region(dev->iobase, PARPORT_SIZE);
+
+	if (dev->irq)
+		comedi_free_irq(dev->irq, dev);
+
+	return 0;
+}
diff --git a/drivers/staging/comedi/drivers/comedi_pci.h b/drivers/staging/comedi/drivers/comedi_pci.h
new file mode 100644
index 000000000000..c14a036a0536
--- /dev/null
+++ b/drivers/staging/comedi/drivers/comedi_pci.h
@@ -0,0 +1,60 @@
+/*
+    comedi/drivers/comedi_pci.h
+    Various PCI functions for drivers.
+
+    Copyright (C) 2007 MEV Ltd. <http://www.mev.co.uk/>
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _COMEDI_PCI_H_
+#define _COMEDI_PCI_H_
+
+#include <linux/pci.h>
+
+/*
+ * Enable the PCI device and request the regions.
+ */
+static inline int comedi_pci_enable(struct pci_dev *pdev, const char *res_name)
+{
+	int rc;
+
+	rc = pci_enable_device(pdev);
+	if (rc < 0)
+		return rc;
+
+	rc = pci_request_regions(pdev, res_name);
+	if (rc < 0)
+		pci_disable_device(pdev);
+
+	return rc;
+}
+
+/*
+ * Release the regions and disable the PCI device.
+ *
+ * This must be matched with a previous successful call to comedi_pci_enable().
+ */
+static inline void comedi_pci_disable(struct pci_dev *pdev)
+{
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+}
+
+#endif
diff --git a/drivers/staging/comedi/drivers/comedi_test.c b/drivers/staging/comedi/drivers/comedi_test.c
new file mode 100644
index 000000000000..4b4c37d07482
--- /dev/null
+++ b/drivers/staging/comedi/drivers/comedi_test.c
@@ -0,0 +1,527 @@
+/*
+    comedi/drivers/comedi_test.c
+
+    Generates fake waveform signals that can be read through
+    the command interface.  It does _not_ read from any board;
+    it just generates deterministic waveforms.
+    Useful for various testing purposes.
+
+    Copyright (C) 2002 Joachim Wuttke <Joachim.Wuttke@icn.siemens.de>
+    Copyright (C) 2002 Frank Mori Hess <fmhess@users.sourceforge.net>
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+************************************************************************/
+/*
+Driver: comedi_test
+Description: generates fake waveforms
+Author: Joachim Wuttke <Joachim.Wuttke@icn.siemens.de>, Frank Mori Hess
+  <fmhess@users.sourceforge.net>, ds
+Devices:
+Status: works
+Updated: Sat, 16 Mar 2002 17:34:48 -0800
+
+This driver is mainly for testing purposes, but can also be used to
+generate sample waveforms on systems that don't have data acquisition
+hardware.
+
+Configuration options:
+  [0] - Amplitude in microvolts for fake waveforms (default 1 volt)
+  [1] - Period in microseconds for fake waveforms (default 0.1 sec)
+
+Generates a sawtooth wave on channel 0, square wave on channel 1, additional
+waveforms could be added to other channels (currently they return flatline
+zero volts).
+
+*/
+
+#include "../comedidev.h"
+
+#include <asm/div64.h>
+
+#include "comedi_fc.h"
+
+/* Board descriptions */
+struct waveform_board {
+	const char *name;
+	int ai_chans;
+	int ai_bits;
+	int have_dio;
+};
+
+#define N_CHANS 8
+
+static const struct waveform_board waveform_boards[] = {
+	{
+		.name =		"comedi_test",
+		.ai_chans =	N_CHANS,
+		.ai_bits =	16,
+		.have_dio =	0,
+	},
+};
+
+#define thisboard ((const struct waveform_board *)dev->board_ptr)
+
+/* Data unique to this driver */
+struct waveform_private {
+	struct timer_list timer;
+	struct timeval last;	/* time at which last timer interrupt occured */
+	unsigned int uvolt_amplitude;	/* waveform amplitude in microvolts */
+	unsigned long usec_period;	/* waveform period in microseconds */
+	unsigned long usec_current; /* current time (modulo waveform period) */
+	unsigned long usec_remainder;	/* usec since last scan; */
+	unsigned long ai_count;	/* number of conversions remaining */
+	unsigned int scan_period;	/* scan period in usec */
+	unsigned int convert_period;	/* conversion period in usec */
+	unsigned timer_running:1;
+	lsampl_t ao_loopbacks[N_CHANS];
+};
+#define devpriv ((struct waveform_private *)dev->private)
+
+static int waveform_attach(comedi_device *dev, comedi_devconfig *it);
+static int waveform_detach(comedi_device *dev);
+static comedi_driver driver_waveform = {
+      .driver_name =	"comedi_test",
+      .module =		THIS_MODULE,
+      .attach =		waveform_attach,
+      .detach =		waveform_detach,
+      .board_name =	&waveform_boards[0].name,
+      .offset =		sizeof(struct waveform_board),
+      .num_names =	sizeof(waveform_boards) / sizeof(struct waveform_board),
+};
+
+COMEDI_INITCLEANUP(driver_waveform);
+
+static int waveform_ai_cmdtest(comedi_device *dev, comedi_subdevice *s,
+			       comedi_cmd *cmd);
+static int waveform_ai_cmd(comedi_device *dev, comedi_subdevice *s);
+static int waveform_ai_cancel(comedi_device *dev, comedi_subdevice *s);
+static int waveform_ai_insn_read(comedi_device *dev, comedi_subdevice *s,
+				 comedi_insn *insn, lsampl_t *data);
+static int waveform_ao_insn_write(comedi_device *dev, comedi_subdevice *s,
+				  comedi_insn *insn, lsampl_t *data);
+static sampl_t fake_sawtooth(comedi_device *dev, unsigned int range,
+			     unsigned long current_time);
+static sampl_t fake_squarewave(comedi_device *dev, unsigned int range,
+			       unsigned long current_time);
+static sampl_t fake_flatline(comedi_device *dev, unsigned int range,
+			     unsigned long current_time);
+static sampl_t fake_waveform(comedi_device *dev, unsigned int channel,
+			     unsigned int range, unsigned long current_time);
+
+/* 1000 nanosec in a microsec */
+static const int nano_per_micro = 1000;
+
+/* fake analog input ranges */
+static const comedi_lrange waveform_ai_ranges = {
+	2,
+	{
+			BIP_RANGE(10),
+			BIP_RANGE(5),
+		}
+};
+
+/*
+   This is the background routine used to generate arbitrary data.
+   It should run in the background; therefore it is scheduled by
+   a timer mechanism.
+*/
+static void waveform_ai_interrupt(unsigned long arg)
+{
+	comedi_device *dev = (comedi_device *) arg;
+	comedi_async *async = dev->read_subdev->async;
+	comedi_cmd *cmd = &async->cmd;
+	unsigned int i, j;
+	/* all times in microsec */
+	unsigned long elapsed_time;
+	unsigned int num_scans;
+	struct timeval now;
+
+	do_gettimeofday(&now);
+
+	elapsed_time =
+		1000000 * (now.tv_sec - devpriv->last.tv_sec) + now.tv_usec -
+		devpriv->last.tv_usec;
+	devpriv->last = now;
+	num_scans =
+		(devpriv->usec_remainder + elapsed_time) / devpriv->scan_period;
+	devpriv->usec_remainder =
+		(devpriv->usec_remainder + elapsed_time) % devpriv->scan_period;
+	async->events = 0;
+
+	for (i = 0; i < num_scans; i++) {
+		for (j = 0; j < cmd->chanlist_len; j++) {
+			cfc_write_to_buffer(dev->read_subdev,
+				fake_waveform(dev, CR_CHAN(cmd->chanlist[j]),
+					CR_RANGE(cmd->chanlist[j]),
+					devpriv->usec_current +
+					i * devpriv->scan_period +
+					j * devpriv->convert_period));
+		}
+		devpriv->ai_count++;
+		if (cmd->stop_src == TRIG_COUNT
+			&& devpriv->ai_count >= cmd->stop_arg) {
+			async->events |= COMEDI_CB_EOA;
+			break;
+		}
+	}
+
+	devpriv->usec_current += elapsed_time;
+	devpriv->usec_current %= devpriv->usec_period;
+
+	if ((async->events & COMEDI_CB_EOA) == 0 && devpriv->timer_running)
+		mod_timer(&devpriv->timer, jiffies + 1);
+	else
+		del_timer(&devpriv->timer);
+
+	comedi_event(dev, dev->read_subdev);
+}
+
+static int waveform_attach(comedi_device *dev, comedi_devconfig *it)
+{
+	comedi_subdevice *s;
+	int amplitude = it->options[0];
+	int period = it->options[1];
+	int i;
+
+	dev->board_name = thisboard->name;
+
+	if (alloc_private(dev, sizeof(struct waveform_private)) < 0)
+		return -ENOMEM;
+
+	/* set default amplitude and period */
+	if (amplitude <= 0)
+		amplitude = 1000000;	/* 1 volt */
+	if (period <= 0)
+		period = 100000;	/* 0.1 sec */
+
+	devpriv->uvolt_amplitude = amplitude;
+	devpriv->usec_period = period;
+
+	dev->n_subdevices = 2;
+	if (alloc_subdevices(dev, dev->n_subdevices) < 0)
+		return -ENOMEM;
+
+	s = dev->subdevices + 0;
+	dev->read_subdev = s;
+	/* analog input subdevice */
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
+	s->n_chan = thisboard->ai_chans;
+	s->maxdata = (1 << thisboard->ai_bits) - 1;
+	s->range_table = &waveform_ai_ranges;
+	s->len_chanlist = s->n_chan * 2;
+	s->insn_read = waveform_ai_insn_read;
+	s->do_cmd = waveform_ai_cmd;
+	s->do_cmdtest = waveform_ai_cmdtest;
+	s->cancel = waveform_ai_cancel;
+
+	s = dev->subdevices + 1;
+	dev->write_subdev = s;
+	/* analog output subdevice (loopback) */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITEABLE | SDF_GROUND;
+	s->n_chan = thisboard->ai_chans;
+	s->maxdata = (1 << thisboard->ai_bits) - 1;
+	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++)
+		devpriv->ao_loopbacks[i] = s->maxdata / 2;
+
+	init_timer(&(devpriv->timer));
+	devpriv->timer.function = waveform_ai_interrupt;
+	devpriv->timer.data = (unsigned long)dev;
+
+	printk(KERN_INFO "comedi%d: comedi_test: "
+		"%i microvolt, %li microsecond waveform attached\n", dev->minor,
+		devpriv->uvolt_amplitude, devpriv->usec_period);
+	return 1;
+}
+
+static int waveform_detach(comedi_device *dev)
+{
+	printk("comedi%d: comedi_test: remove\n", dev->minor);
+
+	if (dev->private)
+		waveform_ai_cancel(dev, dev->read_subdev);
+
+	return 0;
+}
+
+static int waveform_ai_cmdtest(comedi_device *dev, comedi_subdevice *s,
+			       comedi_cmd *cmd)
+{
+	int err = 0;
+	int tmp;
+
+	/* step 1: make sure trigger sources are trivially valid */
+
+	tmp = cmd->start_src;
+	cmd->start_src &= TRIG_NOW;
+	if (!cmd->start_src || tmp != cmd->start_src)
+		err++;
+
+	tmp = cmd->scan_begin_src;
+	cmd->scan_begin_src &= TRIG_TIMER;
+	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
+		err++;
+
+	tmp = cmd->convert_src;
+	cmd->convert_src &= TRIG_NOW | TRIG_TIMER;
+	if (!cmd->convert_src || tmp != cmd->convert_src)
+		err++;
+
+	tmp = cmd->scan_end_src;
+	cmd->scan_end_src &= TRIG_COUNT;
+	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
+		err++;
+
+	tmp = cmd->stop_src;
+	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
+	if (!cmd->stop_src || tmp != cmd->stop_src)
+		err++;
+
+	if (err)
+		return 1;
+
+	/*
+	 * step 2: make sure trigger sources are unique and mutually compatible
+	 */
+
+	if (cmd->convert_src != TRIG_NOW && cmd->convert_src != TRIG_TIMER)
+		err++;
+	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
+		err++;
+
+	if (err)
+		return 2;
+
+	/* step 3: make sure arguments are trivially compatible */
+
+	if (cmd->start_arg != 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+	if (cmd->convert_src == TRIG_NOW) {
+		if (cmd->convert_arg != 0) {
+			cmd->convert_arg = 0;
+			err++;
+		}
+	}
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		if (cmd->scan_begin_arg < nano_per_micro) {
+			cmd->scan_begin_arg = nano_per_micro;
+			err++;
+		}
+		if (cmd->convert_src == TRIG_TIMER &&
+			cmd->scan_begin_arg <
+			cmd->convert_arg * cmd->chanlist_len) {
+			cmd->scan_begin_arg =
+				cmd->convert_arg * cmd->chanlist_len;
+			err++;
+		}
+	}
+	/*
+	 * XXX these checks are generic and should go in core if not there
+	 * already
+	 */
+	if (!cmd->chanlist_len) {
+		cmd->chanlist_len = 1;
+		err++;
+	}
+	if (cmd->scan_end_arg != cmd->chanlist_len) {
+		cmd->scan_end_arg = cmd->chanlist_len;
+		err++;
+	}
+
+	if (cmd->stop_src == TRIG_COUNT) {
+		if (!cmd->stop_arg) {
+			cmd->stop_arg = 1;
+			err++;
+		}
+	} else {		/* TRIG_NONE */
+		if (cmd->stop_arg != 0) {
+			cmd->stop_arg = 0;
+			err++;
+		}
+	}
+
+	if (err)
+		return 3;
+
+	/* step 4: fix up any arguments */
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		tmp = 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++;
+	}
+	if (cmd->convert_src == TRIG_TIMER) {
+		tmp = 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++;
+	}
+
+	if (err)
+		return 4;
+
+	return 0;
+}
+
+static int waveform_ai_cmd(comedi_device *dev, comedi_subdevice *s)
+{
+	comedi_cmd *cmd = &s->async->cmd;
+
+	if (cmd->flags & TRIG_RT) {
+		comedi_error(dev,
+			"commands at RT priority not supported in this driver");
+		return -1;
+	}
+
+	devpriv->timer_running = 1;
+	devpriv->ai_count = 0;
+	devpriv->scan_period = cmd->scan_begin_arg / nano_per_micro;
+
+	if (cmd->convert_src == TRIG_NOW)
+		devpriv->convert_period = 0;
+	else if (cmd->convert_src == TRIG_TIMER)
+		devpriv->convert_period = cmd->convert_arg / nano_per_micro;
+	else {
+		comedi_error(dev, "bug setting conversion period");
+		return -1;
+	}
+
+	do_gettimeofday(&devpriv->last);
+	devpriv->usec_current = devpriv->last.tv_usec % devpriv->usec_period;
+	devpriv->usec_remainder = 0;
+
+	devpriv->timer.expires = jiffies + 1;
+	add_timer(&devpriv->timer);
+	return 0;
+}
+
+static int waveform_ai_cancel(comedi_device *dev, comedi_subdevice *s)
+{
+	devpriv->timer_running = 0;
+	del_timer(&devpriv->timer);
+	return 0;
+}
+
+static sampl_t fake_sawtooth(comedi_device *dev, unsigned int range_index,
+			     unsigned long current_time)
+{
+	comedi_subdevice *s = dev->read_subdev;
+	unsigned int offset = s->maxdata / 2;
+	u64 value;
+	const comedi_krange *krange = &s->range_table->range[range_index];
+	u64 binary_amplitude;
+
+	binary_amplitude = s->maxdata;
+	binary_amplitude *= devpriv->uvolt_amplitude;
+	do_div(binary_amplitude, krange->max - krange->min);
+
+	current_time %= devpriv->usec_period;
+	value = current_time;
+	value *= binary_amplitude * 2;
+	do_div(value, devpriv->usec_period);
+	value -= binary_amplitude;	/* get rid of sawtooth's dc offset */
+
+	return offset + value;
+}
+static sampl_t fake_squarewave(comedi_device *dev, unsigned int range_index,
+			       unsigned long current_time)
+{
+	comedi_subdevice *s = dev->read_subdev;
+	unsigned int offset = s->maxdata / 2;
+	u64 value;
+	const comedi_krange *krange = &s->range_table->range[range_index];
+	current_time %= devpriv->usec_period;
+
+	value = s->maxdata;
+	value *= devpriv->uvolt_amplitude;
+	do_div(value, krange->max - krange->min);
+
+	if (current_time < devpriv->usec_period / 2)
+		value *= -1;
+
+	return offset + value;
+}
+
+static sampl_t fake_flatline(comedi_device *dev, unsigned int range_index,
+			     unsigned long current_time)
+{
+	return dev->read_subdev->maxdata / 2;
+}
+
+/* generates a different waveform depending on what channel is read */
+static sampl_t fake_waveform(comedi_device *dev, unsigned int channel,
+			     unsigned int range, unsigned long current_time)
+{
+	enum {
+		SAWTOOTH_CHAN,
+		SQUARE_CHAN,
+	};
+	switch (channel) {
+	case SAWTOOTH_CHAN:
+		return fake_sawtooth(dev, range, current_time);
+		break;
+	case SQUARE_CHAN:
+		return fake_squarewave(dev, range, current_time);
+		break;
+	default:
+		break;
+	}
+
+	return fake_flatline(dev, range, current_time);
+}
+
+static int waveform_ai_insn_read(comedi_device *dev, comedi_subdevice *s,
+				 comedi_insn *insn, lsampl_t *data)
+{
+	int i, chan = CR_CHAN(insn->chanspec);
+
+	for (i = 0; i < insn->n; i++)
+		data[i] = devpriv->ao_loopbacks[chan];
+
+	return insn->n;
+}
+
+static int waveform_ao_insn_write(comedi_device *dev, comedi_subdevice *s,
+				  comedi_insn *insn, lsampl_t *data)
+{
+	int i, chan = CR_CHAN(insn->chanspec);
+
+	for (i = 0; i < insn->n; i++)
+		devpriv->ao_loopbacks[chan] = data[i];
+
+	return insn->n;
+}
diff --git a/drivers/staging/comedi/drivers/dt9812.c b/drivers/staging/comedi/drivers/dt9812.c
new file mode 100644
index 000000000000..f2d2173d7219
--- /dev/null
+++ b/drivers/staging/comedi/drivers/dt9812.c
@@ -0,0 +1,1162 @@
+/*
+ * comedi/drivers/dt9812.c
+ *   COMEDI driver for DataTranslation DT9812 USB module
+ *
+ * Copyright (C) 2005 Anders Blomdell <anders.blomdell@control.lth.se>
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+
+ *  This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+/*
+Driver: dt9812
+Description: Data Translation DT9812 USB module
+Author: anders.blomdell@control.lth.se (Anders Blomdell)
+Status: in development
+Devices: [Data Translation] DT9812 (dt9812)
+Updated: Sun Nov 20 20:18:34 EST 2005
+
+This driver works, but bulk transfers not implemented. Might be a starting point
+for someone else. I found out too late that USB has too high latencies (>1 ms)
+for my needs.
+*/
+
+/*
+ * Nota Bene:
+ *   1. All writes to command pipe has to be 32 bytes (ISP1181B SHRTP=0 ?)
+ *   2. The DDK source (as of sep 2005) is in error regarding the
+ *      input MUX bits (example code says P4, but firmware schematics
+ *      says P1).
+ */
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/kref.h>
+#include <linux/uaccess.h>
+#include <linux/usb.h>
+
+#include "../comedidev.h"
+
+#define DT9812_DIAGS_BOARD_INFO_ADDR	0xFBFF
+#define DT9812_MAX_WRITE_CMD_PIPE_SIZE	32
+#define DT9812_MAX_READ_CMD_PIPE_SIZE	32
+
+/*
+ * See Silican Laboratories C8051F020/1/2/3 manual
+ */
+#define F020_SFR_P4			0x84
+#define F020_SFR_P1			0x90
+#define F020_SFR_P2			0xa0
+#define F020_SFR_P3			0xb0
+#define F020_SFR_AMX0CF			0xba
+#define F020_SFR_AMX0SL			0xbb
+#define F020_SFR_ADC0CF			0xbc
+#define F020_SFR_ADC0L			0xbe
+#define F020_SFR_ADC0H			0xbf
+#define F020_SFR_DAC0L			0xd2
+#define F020_SFR_DAC0H			0xd3
+#define F020_SFR_DAC0CN			0xd4
+#define F020_SFR_DAC1L			0xd5
+#define F020_SFR_DAC1H			0xd6
+#define F020_SFR_DAC1CN			0xd7
+#define F020_SFR_ADC0CN			0xe8
+
+#define F020_MASK_ADC0CF_AMP0GN0	0x01
+#define F020_MASK_ADC0CF_AMP0GN1	0x02
+#define F020_MASK_ADC0CF_AMP0GN2	0x04
+
+#define F020_MASK_ADC0CN_AD0EN		0x80
+#define F020_MASK_ADC0CN_AD0INT		0x20
+#define F020_MASK_ADC0CN_AD0BUSY	0x10
+
+#define F020_MASK_DACxCN_DACxEN		0x80
+
+enum {
+				/* A/D  D/A  DI  DO  CT */
+	DT9812_DEVID_DT9812_10,	/*  8    2   8   8   1  +/- 10V */
+	DT9812_DEVID_DT9812_2PT5,/* 8    2   8   8   1  0-2.44V */
+#if 0
+	DT9812_DEVID_DT9813,	/*  16   2   4   4   1  +/- 10V */
+	DT9812_DEVID_DT9814	/*  24   2   0   0   1  +/- 10V */
+#endif
+};
+
+enum dt9812_gain {
+	DT9812_GAIN_0PT25 = 1,
+	DT9812_GAIN_0PT5 = 2,
+	DT9812_GAIN_1 = 4,
+	DT9812_GAIN_2 = 8,
+	DT9812_GAIN_4 = 16,
+	DT9812_GAIN_8 = 32,
+	DT9812_GAIN_16 = 64,
+};
+
+enum {
+	DT9812_LEAST_USB_FIRMWARE_CMD_CODE = 0,
+	/* Write Flash memory */
+	DT9812_W_FLASH_DATA = 0,
+	/* Read Flash memory misc config info */
+	DT9812_R_FLASH_DATA = 1,
+
+	/*
+	 * Register read/write commands for processor
+	 */
+
+	/* Read a single byte of USB memory */
+	DT9812_R_SINGLE_BYTE_REG = 2,
+	/* Write a single byte of USB memory */
+	DT9812_W_SINGLE_BYTE_REG = 3,
+	/* Multiple Reads of USB memory */
+	DT9812_R_MULTI_BYTE_REG = 4,
+	/* Multiple Writes of USB memory */
+	DT9812_W_MULTI_BYTE_REG = 5,
+	/* Read, (AND) with mask, OR value, then write (single) */
+	DT9812_RMW_SINGLE_BYTE_REG = 6,
+	/* Read, (AND) with mask, OR value, then write (multiple) */
+	DT9812_RMW_MULTI_BYTE_REG = 7,
+
+	/*
+	 * Register read/write commands for SMBus
+	 */
+
+	/* Read a single byte of SMBus */
+	DT9812_R_SINGLE_BYTE_SMBUS = 8,
+	/* Write a single byte of SMBus */
+	DT9812_W_SINGLE_BYTE_SMBUS = 9,
+	/* Multiple Reads of SMBus */
+	DT9812_R_MULTI_BYTE_SMBUS = 10,
+	/* Multiple Writes of SMBus */
+	DT9812_W_MULTI_BYTE_SMBUS = 11,
+
+	/*
+	 * Register read/write commands for a device
+	 */
+
+	/* Read a single byte of a device */
+	DT9812_R_SINGLE_BYTE_DEV = 12,
+	/* Write a single byte of a device */
+	DT9812_W_SINGLE_BYTE_DEV = 13,
+	/* Multiple Reads of a device */
+	DT9812_R_MULTI_BYTE_DEV = 14,
+	/* Multiple Writes of a device */
+	DT9812_W_MULTI_BYTE_DEV = 15,
+
+	/* Not sure if we'll need this */
+	DT9812_W_DAC_THRESHOLD = 16,
+
+	/* Set interrupt on change mask */
+	DT9812_W_INT_ON_CHANGE_MASK = 17,
+
+	/* Write (or Clear) the CGL for the ADC */
+	DT9812_W_CGL = 18,
+	/* Multiple Reads of USB memory */
+	DT9812_R_MULTI_BYTE_USBMEM = 19,
+	/* Multiple Writes to USB memory */
+	DT9812_W_MULTI_BYTE_USBMEM = 20,
+
+	/* Issue a start command to a given subsystem */
+	DT9812_START_SUBSYSTEM = 21,
+	/* Issue a stop command to a given subsystem */
+	DT9812_STOP_SUBSYSTEM = 22,
+
+	/* calibrate the board using CAL_POT_CMD */
+	DT9812_CALIBRATE_POT = 23,
+	/* set the DAC FIFO size */
+	DT9812_W_DAC_FIFO_SIZE = 24,
+	/* Write or Clear the CGL for the DAC */
+	DT9812_W_CGL_DAC = 25,
+	/* Read a single value from a subsystem */
+	DT9812_R_SINGLE_VALUE_CMD = 26,
+	/* Write a single value to a subsystem */
+	DT9812_W_SINGLE_VALUE_CMD = 27,
+	/* Valid DT9812_USB_FIRMWARE_CMD_CODE's will be less than this number */
+	DT9812_MAX_USB_FIRMWARE_CMD_CODE,
+};
+
+struct dt9812_flash_data {
+	u16 numbytes;
+	u16 address;
+};
+
+#define DT9812_MAX_NUM_MULTI_BYTE_RDS  \
+    ((DT9812_MAX_WRITE_CMD_PIPE_SIZE - 4 - 1) / sizeof(u8))
+
+struct dt9812_read_multi {
+	u8 count;
+	u8 address[DT9812_MAX_NUM_MULTI_BYTE_RDS];
+};
+
+struct dt9812_write_byte {
+	u8 address;
+	u8 value;
+};
+
+#define DT9812_MAX_NUM_MULTI_BYTE_WRTS  \
+    ((DT9812_MAX_WRITE_CMD_PIPE_SIZE - 4 - 1) / \
+      sizeof(struct dt9812_write_byte))
+
+struct dt9812_write_multi {
+	u8 count;
+	struct dt9812_write_byte write[DT9812_MAX_NUM_MULTI_BYTE_WRTS];
+};
+
+struct dt9812_rmw_byte {
+	u8 address;
+	u8 and_mask;
+	u8 or_value;
+};
+
+#define DT9812_MAX_NUM_MULTI_BYTE_RMWS  \
+    ((DT9812_MAX_WRITE_CMD_PIPE_SIZE - 4 - 1) / sizeof(struct dt9812_rmw_byte))
+
+struct dt9812_rmw_multi {
+	u8 count;
+	struct dt9812_rmw_byte rmw[DT9812_MAX_NUM_MULTI_BYTE_RMWS];
+};
+
+struct dt9812_usb_cmd {
+	u32 cmd;
+	union {
+		struct dt9812_flash_data flash_data_info;
+		struct dt9812_read_multi read_multi_info;
+		struct dt9812_write_multi write_multi_info;
+		struct dt9812_rmw_multi rmw_multi_info;
+	} u;
+#if 0
+	WRITE_BYTE_INFO WriteByteInfo;
+	READ_BYTE_INFO ReadByteInfo;
+	WRITE_MULTI_INFO WriteMultiInfo;
+	READ_MULTI_INFO ReadMultiInfo;
+	RMW_BYTE_INFO RMWByteInfo;
+	RMW_MULTI_INFO RMWMultiInfo;
+	DAC_THRESHOLD_INFO DacThresholdInfo;
+	INT_ON_CHANGE_MASK_INFO IntOnChangeMaskInfo;
+	CGL_INFO CglInfo;
+	SUBSYSTEM_INFO SubsystemInfo;
+	CAL_POT_CMD CalPotCmd;
+	WRITE_DEV_BYTE_INFO WriteDevByteInfo;
+	READ_DEV_BYTE_INFO ReadDevByteInfo;
+	WRITE_DEV_MULTI_INFO WriteDevMultiInfo;
+	READ_DEV_MULTI_INFO ReadDevMultiInfo;
+	READ_SINGLE_VALUE_INFO ReadSingleValueInfo;
+	WRITE_SINGLE_VALUE_INFO WriteSingleValueInfo;
+#endif
+};
+
+#define DT9812_NUM_SLOTS	16
+
+static DECLARE_MUTEX(dt9812_mutex);
+
+static struct usb_device_id dt9812_table[] = {
+	{USB_DEVICE(0x0867, 0x9812)},
+	{ }			/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, dt9812_table);
+
+struct usb_dt9812 {
+	struct slot_dt9812 *slot;
+	struct usb_device *udev;
+	struct usb_interface *interface;
+	u16 vendor;
+	u16 product;
+	u16 device;
+	u32 serial;
+	struct {
+		__u8 addr;
+		size_t size;
+	} message_pipe, command_write, command_read, write_stream, read_stream;
+	struct kref kref;
+	u16 analog_out_shadow[2];
+	u8 digital_out_shadow;
+};
+
+struct comedi_dt9812 {
+	struct slot_dt9812 *slot;
+	u32 serial;
+};
+
+struct slot_dt9812 {
+	struct semaphore mutex;
+	u32 serial;
+	struct usb_dt9812 *usb;
+	struct comedi_dt9812 *comedi;
+};
+
+static const comedi_lrange dt9812_10_ain_range = { 1, {
+			BIP_RANGE(10),
+	}
+};
+
+static const comedi_lrange dt9812_2pt5_ain_range = { 1, {
+			UNI_RANGE(2.5),
+	}
+};
+
+static const comedi_lrange dt9812_10_aout_range = { 1, {
+			BIP_RANGE(10),
+	}
+};
+
+static const comedi_lrange dt9812_2pt5_aout_range = { 1, {
+			UNI_RANGE(2.5),
+	}
+};
+
+static struct slot_dt9812 dt9812[DT9812_NUM_SLOTS];
+
+/* Useful shorthand access to private data */
+#define devpriv ((struct comedi_dt9812 *)dev->private)
+
+static inline struct usb_dt9812 *to_dt9812_dev(struct kref *d)
+{
+	return container_of(d, struct usb_dt9812, kref);
+}
+
+static void dt9812_delete(struct kref *kref)
+{
+	struct usb_dt9812 *dev = to_dt9812_dev(kref);
+
+	usb_put_dev(dev->udev);
+	kfree(dev);
+}
+
+static int dt9812_read_info(struct usb_dt9812 *dev, int offset, void *buf,
+			    size_t buf_size)
+{
+	struct dt9812_usb_cmd cmd;
+	int count, retval;
+
+	cmd.cmd = cpu_to_le32(DT9812_R_FLASH_DATA);
+	cmd.u.flash_data_info.address =
+		cpu_to_le16(DT9812_DIAGS_BOARD_INFO_ADDR + offset);
+	cmd.u.flash_data_info.numbytes = cpu_to_le16(buf_size);
+
+	/* DT9812 only responds to 32 byte writes!! */
+	count = 32;
+	retval = usb_bulk_msg(dev->udev,
+			      usb_sndbulkpipe(dev->udev,
+					      dev->command_write.addr),
+			      &cmd, 32, &count, HZ * 1);
+	if (retval)
+		return retval;
+	retval = usb_bulk_msg(dev->udev,
+			      usb_rcvbulkpipe(dev->udev,
+					      dev->command_read.addr),
+			      buf, buf_size, &count, HZ * 1);
+	return retval;
+}
+
+static int dt9812_read_multiple_registers(struct usb_dt9812 *dev, int reg_count,
+					  u8 *address, u8 *value)
+{
+	struct dt9812_usb_cmd cmd;
+	int i, count, retval;
+
+	cmd.cmd = cpu_to_le32(DT9812_R_MULTI_BYTE_REG);
+	cmd.u.read_multi_info.count = reg_count;
+	for (i = 0; i < reg_count; i++)
+		cmd.u.read_multi_info.address[i] = address[i];
+
+	/* DT9812 only responds to 32 byte writes!! */
+	count = 32;
+	retval = usb_bulk_msg(dev->udev,
+			      usb_sndbulkpipe(dev->udev,
+					      dev->command_write.addr),
+			      &cmd, 32, &count, HZ * 1);
+	if (retval)
+		return retval;
+	retval = usb_bulk_msg(dev->udev,
+			      usb_rcvbulkpipe(dev->udev,
+					      dev->command_read.addr),
+			      value, reg_count, &count, HZ * 1);
+	return retval;
+}
+
+static int dt9812_write_multiple_registers(struct usb_dt9812 *dev,
+					   int reg_count, u8 *address,
+					   u8 *value)
+{
+	struct dt9812_usb_cmd cmd;
+	int i, count, retval;
+
+	cmd.cmd = cpu_to_le32(DT9812_W_MULTI_BYTE_REG);
+	cmd.u.read_multi_info.count = reg_count;
+	for (i = 0; i < reg_count; i++) {
+		cmd.u.write_multi_info.write[i].address = address[i];
+		cmd.u.write_multi_info.write[i].value = value[i];
+	}
+	/* DT9812 only responds to 32 byte writes!! */
+	retval = usb_bulk_msg(dev->udev,
+			      usb_sndbulkpipe(dev->udev,
+					      dev->command_write.addr),
+			      &cmd, 32, &count, HZ * 1);
+	return retval;
+}
+
+static int dt9812_rmw_multiple_registers(struct usb_dt9812 *dev, int reg_count,
+					 struct dt9812_rmw_byte *rmw)
+{
+	struct dt9812_usb_cmd cmd;
+	int i, count, retval;
+
+	cmd.cmd = cpu_to_le32(DT9812_RMW_MULTI_BYTE_REG);
+	cmd.u.rmw_multi_info.count = reg_count;
+	for (i = 0; i < reg_count; i++)
+		cmd.u.rmw_multi_info.rmw[i] = rmw[i];
+
+	/* DT9812 only responds to 32 byte writes!! */
+	retval = usb_bulk_msg(dev->udev,
+			      usb_sndbulkpipe(dev->udev,
+					      dev->command_write.addr),
+			      &cmd, 32, &count, HZ * 1);
+	return retval;
+}
+
+static int dt9812_digital_in(struct slot_dt9812 *slot, u8 *bits)
+{
+	int result = -ENODEV;
+
+	down(&slot->mutex);
+	if (slot->usb) {
+		u8 reg[2] = { F020_SFR_P3, F020_SFR_P1 };
+		u8 value[2];
+
+		result = dt9812_read_multiple_registers(slot->usb, 2, reg,
+							value);
+		if (result == 0) {
+			/*
+			 * bits 0-6 in F020_SFR_P3 are bits 0-6 in the digital
+			 * input port bit 3 in F020_SFR_P1 is bit 7 in the
+			 * digital input port
+			 */
+			*bits = (value[0] & 0x7f) | ((value[1] & 0x08) << 4);
+			/* printk("%2.2x, %2.2x -> %2.2x\n",
+				  value[0], value[1], *bits); */
+		}
+	}
+	up(&slot->mutex);
+
+	return result;
+}
+
+static int dt9812_digital_out(struct slot_dt9812 *slot, u8 bits)
+{
+	int result = -ENODEV;
+
+	down(&slot->mutex);
+	if (slot->usb) {
+		u8 reg[1];
+		u8 value[1];
+
+		reg[0] = F020_SFR_P2;
+		value[0] = bits;
+		result = dt9812_write_multiple_registers(slot->usb, 1, reg,
+							 value);
+		slot->usb->digital_out_shadow = bits;
+	}
+	up(&slot->mutex);
+	return result;
+}
+
+static int dt9812_digital_out_shadow(struct slot_dt9812 *slot, u8 *bits)
+{
+	int result = -ENODEV;
+
+	down(&slot->mutex);
+	if (slot->usb) {
+		*bits = slot->usb->digital_out_shadow;
+		result = 0;
+	}
+	up(&slot->mutex);
+	return result;
+}
+
+static void dt9812_configure_mux(struct usb_dt9812 *dev,
+				 struct dt9812_rmw_byte *rmw, int channel)
+{
+	if (dev->device == DT9812_DEVID_DT9812_10) {
+		/* In the DT9812/10V MUX is selected by P1.5-7 */
+		rmw->address = F020_SFR_P1;
+		rmw->and_mask = 0xe0;
+		rmw->or_value = channel << 5;
+	} else {
+		/* In the DT9812/2.5V, internal mux is selected by bits 0:2 */
+		rmw->address = F020_SFR_AMX0SL;
+		rmw->and_mask = 0xff;
+		rmw->or_value = channel & 0x07;
+	}
+}
+
+static void dt9812_configure_gain(struct usb_dt9812 *dev,
+				  struct dt9812_rmw_byte *rmw,
+				  enum dt9812_gain gain)
+{
+	if (dev->device == DT9812_DEVID_DT9812_10) {
+		/* In the DT9812/10V, there is an external gain of 0.5 */
+		gain <<= 1;
+	}
+
+	rmw->address = F020_SFR_ADC0CF;
+	rmw->and_mask = F020_MASK_ADC0CF_AMP0GN2 |
+			F020_MASK_ADC0CF_AMP0GN1 |
+			F020_MASK_ADC0CF_AMP0GN0;
+	switch (gain) {
+		/*
+		 * 000 -> Gain =  1
+		 * 001 -> Gain =  2
+		 * 010 -> Gain =  4
+		 * 011 -> Gain =  8
+		 * 10x -> Gain = 16
+		 * 11x -> Gain =  0.5
+		 */
+	case DT9812_GAIN_0PT5:
+		rmw->or_value = F020_MASK_ADC0CF_AMP0GN2 ||
+				F020_MASK_ADC0CF_AMP0GN1;
+		break;
+	case DT9812_GAIN_1:
+		rmw->or_value = 0x00;
+		break;
+	case DT9812_GAIN_2:
+		rmw->or_value = F020_MASK_ADC0CF_AMP0GN0;
+		break;
+	case DT9812_GAIN_4:
+		rmw->or_value = F020_MASK_ADC0CF_AMP0GN1;
+		break;
+	case DT9812_GAIN_8:
+		rmw->or_value = F020_MASK_ADC0CF_AMP0GN1 ||
+				F020_MASK_ADC0CF_AMP0GN0;
+		break;
+	case DT9812_GAIN_16:
+		rmw->or_value = F020_MASK_ADC0CF_AMP0GN2;
+		break;
+	default:
+		err("Illegal gain %d\n", gain);
+
+	}
+}
+
+static int dt9812_analog_in(struct slot_dt9812 *slot, int channel, u16 *value,
+			    enum dt9812_gain gain)
+{
+	struct dt9812_rmw_byte rmw[3];
+	u8 reg[3] = {
+		F020_SFR_ADC0CN,
+		F020_SFR_ADC0H,
+		F020_SFR_ADC0L
+	};
+	u8 val[3];
+	int result = -ENODEV;
+
+	down(&slot->mutex);
+	if (!slot->usb)
+		goto exit;
+
+	/* 1 select the gain */
+	dt9812_configure_gain(slot->usb, &rmw[0], gain);
+
+	/* 2 set the MUX to select the channel */
+	dt9812_configure_mux(slot->usb, &rmw[1], channel);
+
+	/* 3 start conversion */
+	rmw[2].address = F020_SFR_ADC0CN;
+	rmw[2].and_mask = 0xff;
+	rmw[2].or_value = F020_MASK_ADC0CN_AD0EN | F020_MASK_ADC0CN_AD0BUSY;
+
+	result = dt9812_rmw_multiple_registers(slot->usb, 3, rmw);
+	if (result)
+		goto exit;
+
+	/* read the status and ADC */
+	result = dt9812_read_multiple_registers(slot->usb, 3, reg, val);
+	if (result)
+		goto exit;
+	/*
+	 * An ADC conversion takes 16 SAR clocks cycles, i.e. about 9us.
+	 * Therefore, between the instant that AD0BUSY was set via
+	 * dt9812_rmw_multiple_registers and the read of AD0BUSY via
+	 * dt9812_read_multiple_registers, the conversion should be complete
+	 * since these two operations require two USB transactions each taking
+	 * at least a millisecond to complete.  However, lets make sure that
+	 * conversion is finished.
+	 */
+	if ((val[0] & (F020_MASK_ADC0CN_AD0INT | F020_MASK_ADC0CN_AD0BUSY)) ==
+	    F020_MASK_ADC0CN_AD0INT) {
+		switch (slot->usb->device) {
+		case DT9812_DEVID_DT9812_10:
+			/*
+			 * For DT9812-10V the personality module set the
+			 * encoding to 2's complement. Hence, convert it before
+			 * returning it
+			 */
+			*value = ((val[1] << 8) | val[2]) + 0x800;
+			break;
+		case DT9812_DEVID_DT9812_2PT5:
+			*value = (val[1] << 8) | val[2];
+			break;
+		}
+	}
+
+exit:
+	up(&slot->mutex);
+	return result;
+}
+
+static int dt9812_analog_out_shadow(struct slot_dt9812 *slot, int channel,
+				    u16 *value)
+{
+	int result = -ENODEV;
+
+	down(&slot->mutex);
+	if (slot->usb) {
+		*value = slot->usb->analog_out_shadow[channel];
+		result = 0;
+	}
+	up(&slot->mutex);
+
+	return result;
+}
+
+static int dt9812_analog_out(struct slot_dt9812 *slot, int channel, u16 value)
+{
+	int result = -ENODEV;
+
+	down(&slot->mutex);
+	if (slot->usb) {
+		struct dt9812_rmw_byte rmw[3];
+
+		switch (channel) {
+		case 0:
+			/* 1. Set DAC mode */
+			rmw[0].address = F020_SFR_DAC0CN;
+			rmw[0].and_mask = 0xff;
+			rmw[0].or_value = F020_MASK_DACxCN_DACxEN;
+
+			/* 2 load low byte of DAC value first */
+			rmw[1].address = F020_SFR_DAC0L;
+			rmw[1].and_mask = 0xff;
+			rmw[1].or_value = value & 0xff;
+
+			/* 3 load high byte of DAC value next to latch the
+			   12-bit value */
+			rmw[2].address = F020_SFR_DAC0H;
+			rmw[2].and_mask = 0xff;
+			rmw[2].or_value = (value >> 8) & 0xf;
+			break;
+
+		case 1:
+			/* 1. Set DAC mode */
+			rmw[0].address = F020_SFR_DAC1CN;
+			rmw[0].and_mask = 0xff;
+			rmw[0].or_value = F020_MASK_DACxCN_DACxEN;
+
+			/* 2 load low byte of DAC value first */
+			rmw[1].address = F020_SFR_DAC1L;
+			rmw[1].and_mask = 0xff;
+			rmw[1].or_value = value & 0xff;
+
+			/* 3 load high byte of DAC value next to latch the
+			   12-bit value */
+			rmw[2].address = F020_SFR_DAC1H;
+			rmw[2].and_mask = 0xff;
+			rmw[2].or_value = (value >> 8) & 0xf;
+			break;
+		}
+		result = dt9812_rmw_multiple_registers(slot->usb, 3, rmw);
+		slot->usb->analog_out_shadow[channel] = value;
+	}
+	up(&slot->mutex);
+
+	return result;
+}
+
+/*
+ * USB framework functions
+ */
+
+static int dt9812_probe(struct usb_interface *interface,
+			const struct usb_device_id *id)
+{
+	int retval = -ENOMEM;
+	struct usb_dt9812 *dev = NULL;
+	struct usb_host_interface *iface_desc;
+	struct usb_endpoint_descriptor *endpoint;
+	int i;
+	u8 fw;
+
+	/* allocate memory for our device state and initialize it */
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (dev == NULL) {
+		dev_err(&interface->dev, "Out of memory\n");
+		goto error;
+	}
+	kref_init(&dev->kref);
+
+	dev->udev = usb_get_dev(interface_to_usbdev(interface));
+	dev->interface = interface;
+
+	/* Check endpoints */
+	iface_desc = interface->cur_altsetting;
+
+	if (iface_desc->desc.bNumEndpoints != 5) {
+		err("Wrong number of endpints.");
+		retval = -ENODEV;
+		goto error;
+	}
+
+	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+		int direction = -1;
+		endpoint = &iface_desc->endpoint[i].desc;
+		switch (i) {
+		case 0:
+			direction = USB_DIR_IN;
+			dev->message_pipe.addr = endpoint->bEndpointAddress;
+			dev->message_pipe.size =
+					le16_to_cpu(endpoint->wMaxPacketSize);
+
+			break;
+		case 1:
+			direction = USB_DIR_OUT;
+			dev->command_write.addr = endpoint->bEndpointAddress;
+			dev->command_write.size =
+					le16_to_cpu(endpoint->wMaxPacketSize);
+			break;
+		case 2:
+			direction = USB_DIR_IN;
+			dev->command_read.addr = endpoint->bEndpointAddress;
+			dev->command_read.size =
+					le16_to_cpu(endpoint->wMaxPacketSize);
+			break;
+		case 3:
+			direction = USB_DIR_OUT;
+			dev->write_stream.addr = endpoint->bEndpointAddress;
+			dev->write_stream.size =
+					le16_to_cpu(endpoint->wMaxPacketSize);
+			break;
+		case 4:
+			direction = USB_DIR_IN;
+			dev->read_stream.addr = endpoint->bEndpointAddress;
+			dev->read_stream.size =
+					le16_to_cpu(endpoint->wMaxPacketSize);
+			break;
+		}
+		if ((endpoint->bEndpointAddress & USB_DIR_IN) != direction) {
+			dev_err(&interface->dev,
+				"Endpoint has wrong direction.\n");
+			retval = -ENODEV;
+			goto error;
+		}
+	}
+	if (dt9812_read_info(dev, 0, &fw, sizeof(fw)) != 0) {
+		/*
+		 * Seems like a configuration reset is necessary if driver is
+		 * reloaded while device is attached
+		 */
+		usb_reset_configuration(dev->udev);
+		for (i = 0; i < 10; i++) {
+			retval = dt9812_read_info(dev, 1, &fw, sizeof(fw));
+			if (retval == 0) {
+				dev_info(&interface->dev,
+					 "usb_reset_configuration succeded "
+					 "after %d iterations\n", i);
+				break;
+			}
+		}
+	}
+
+	if (dt9812_read_info(dev, 1, &dev->vendor, sizeof(dev->vendor)) != 0) {
+		err("Failed to read vendor.");
+		retval = -ENODEV;
+		goto error;
+	}
+	if (dt9812_read_info(dev, 3, &dev->product,
+			     sizeof(dev->product)) != 0) {
+		err("Failed to read product.");
+		retval = -ENODEV;
+		goto error;
+	}
+	if (dt9812_read_info(dev, 5, &dev->device, sizeof(dev->device)) != 0) {
+		err("Failed to read device.");
+		retval = -ENODEV;
+		goto error;
+	}
+	if (dt9812_read_info(dev, 7, &dev->serial, sizeof(dev->serial)) != 0) {
+		err("Failed to read serial.");
+		retval = -ENODEV;
+		goto error;
+	}
+
+	dev->vendor = le16_to_cpu(dev->vendor);
+	dev->product = le16_to_cpu(dev->product);
+	dev->device = le16_to_cpu(dev->device);
+	dev->serial = le32_to_cpu(dev->serial);
+	switch (dev->device) {
+	case DT9812_DEVID_DT9812_10:
+		dev->analog_out_shadow[0] = 0x0800;
+		dev->analog_out_shadow[1] = 0x800;
+		break;
+	case DT9812_DEVID_DT9812_2PT5:
+		dev->analog_out_shadow[0] = 0x0000;
+		dev->analog_out_shadow[1] = 0x0000;
+		break;
+	}
+	dev->digital_out_shadow = 0;
+
+	/* save our data pointer in this interface device */
+	usb_set_intfdata(interface, dev);
+
+	/* let the user know what node this device is now attached to */
+	dev_info(&interface->dev, "USB DT9812 (%4.4x.%4.4x.%4.4x) #0x%8.8x\n",
+		 dev->vendor, dev->product, dev->device, dev->serial);
+
+	down(&dt9812_mutex);
+	{
+		/* Find a slot for the USB device */
+		struct slot_dt9812 *first = NULL;
+		struct slot_dt9812 *best = NULL;
+
+		for (i = 0; i < DT9812_NUM_SLOTS; i++) {
+			if (!first && !dt9812[i].usb && dt9812[i].serial == 0)
+				first = &dt9812[i];
+			if (!best && dt9812[i].serial == dev->serial)
+				best = &dt9812[i];
+		}
+
+		if (!best)
+			best = first;
+
+		if (best) {
+			down(&best->mutex);
+			best->usb = dev;
+			dev->slot = best;
+			up(&best->mutex);
+		}
+	}
+	up(&dt9812_mutex);
+
+	return 0;
+
+error:
+	if (dev)
+		kref_put(&dev->kref, dt9812_delete);
+	return retval;
+}
+
+static void dt9812_disconnect(struct usb_interface *interface)
+{
+	struct usb_dt9812 *dev;
+	int minor = interface->minor;
+
+	down(&dt9812_mutex);
+	dev = usb_get_intfdata(interface);
+	if (dev->slot) {
+		down(&dev->slot->mutex);
+		dev->slot->usb = NULL;
+		up(&dev->slot->mutex);
+		dev->slot = NULL;
+	}
+	usb_set_intfdata(interface, NULL);
+	up(&dt9812_mutex);
+
+	/* queue final destruction */
+	kref_put(&dev->kref, dt9812_delete);
+
+	dev_info(&interface->dev, "USB Dt9812 #%d now disconnected\n", minor);
+}
+
+static struct usb_driver dt9812_usb_driver = {
+	.name = "dt9812",
+	.probe = dt9812_probe,
+	.disconnect = dt9812_disconnect,
+	.id_table = dt9812_table,
+};
+
+/*
+ * Comedi functions
+ */
+
+static void dt9812_comedi_open(comedi_device *dev)
+{
+	down(&devpriv->slot->mutex);
+	if (devpriv->slot->usb) {
+		/* We have an attached device, fill in current range info */
+		comedi_subdevice *s;
+
+		s = &dev->subdevices[0];
+		s->n_chan = 8;
+		s->maxdata = 1;
+
+		s = &dev->subdevices[1];
+		s->n_chan = 8;
+		s->maxdata = 1;
+
+		s = &dev->subdevices[2];
+		s->n_chan = 8;
+		switch (devpriv->slot->usb->device) {
+		case 0:{
+				s->maxdata = 4095;
+				s->range_table = &dt9812_10_ain_range;
+			}
+			break;
+		case 1:{
+				s->maxdata = 4095;
+				s->range_table = &dt9812_2pt5_ain_range;
+			}
+			break;
+		}
+
+		s = &dev->subdevices[3];
+		s->n_chan = 2;
+		switch (devpriv->slot->usb->device) {
+		case 0:{
+				s->maxdata = 4095;
+				s->range_table = &dt9812_10_aout_range;
+			}
+			break;
+		case 1:{
+				s->maxdata = 4095;
+				s->range_table = &dt9812_2pt5_aout_range;
+			}
+			break;
+		}
+	}
+	up(&devpriv->slot->mutex);
+}
+
+static int dt9812_di_rinsn(comedi_device *dev, comedi_subdevice *s,
+			   comedi_insn *insn, lsampl_t *data)
+{
+	int n;
+	u8 bits = 0;
+
+	dt9812_digital_in(devpriv->slot, &bits);
+	for (n = 0; n < insn->n; n++)
+		data[n] = ((1 << insn->chanspec) & bits) != 0;
+	return n;
+}
+
+static int dt9812_do_winsn(comedi_device *dev, comedi_subdevice *s,
+			   comedi_insn *insn, lsampl_t *data)
+{
+	int n;
+	u8 bits = 0;
+
+	dt9812_digital_out_shadow(devpriv->slot, &bits);
+	for (n = 0; n < insn->n; n++) {
+		u8 mask = 1 << insn->chanspec;
+
+		bits &= ~mask;
+		if (data[n])
+			bits |= mask;
+	}
+	dt9812_digital_out(devpriv->slot, bits);
+	return n;
+}
+
+static int dt9812_ai_rinsn(comedi_device *dev, comedi_subdevice *s,
+			   comedi_insn *insn, lsampl_t *data)
+{
+	int n;
+
+	for (n = 0; n < insn->n; n++) {
+		u16 value = 0;
+
+		dt9812_analog_in(devpriv->slot, insn->chanspec, &value,
+				 DT9812_GAIN_1);
+		data[n] = value;
+	}
+	return n;
+}
+
+static int dt9812_ao_rinsn(comedi_device *dev, comedi_subdevice *s,
+			   comedi_insn *insn, lsampl_t *data)
+{
+	int n;
+	u16 value;
+
+	for (n = 0; n < insn->n; n++) {
+		value = 0;
+		dt9812_analog_out_shadow(devpriv->slot, insn->chanspec, &value);
+		data[n] = value;
+	}
+	return n;
+}
+
+static int dt9812_ao_winsn(comedi_device *dev, comedi_subdevice *s,
+			   comedi_insn *insn, lsampl_t *data)
+{
+	int n;
+
+	for (n = 0; n < insn->n; n++)
+		dt9812_analog_out(devpriv->slot, insn->chanspec, data[n]);
+	return n;
+}
+
+static int dt9812_attach(comedi_device *dev, comedi_devconfig *it)
+{
+	int i;
+	comedi_subdevice *s;
+
+	dev->board_name = "dt9812";
+
+	if (alloc_private(dev, sizeof(struct comedi_dt9812)) < 0)
+		return -ENOMEM;
+
+	/*
+	 * Special open routine, since USB unit may be unattached at
+	 * comedi_config time, hence range can not be determined
+	 */
+	dev->open = dt9812_comedi_open;
+
+	devpriv->serial = it->options[0];
+
+	/* Allocate subdevices */
+	if (alloc_subdevices(dev, 4) < 0)
+		return -ENOMEM;
+
+	/* digital input subdevice */
+	s = dev->subdevices + 0;
+	s->type = COMEDI_SUBD_DI;
+	s->subdev_flags = SDF_READABLE;
+	s->n_chan = 0;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_read = &dt9812_di_rinsn;
+
+	/* digital output subdevice */
+	s = dev->subdevices + 1;
+	s->type = COMEDI_SUBD_DO;
+	s->subdev_flags = SDF_WRITEABLE;
+	s->n_chan = 0;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_write = &dt9812_do_winsn;
+
+	/* analog input subdevice */
+	s = dev->subdevices + 2;
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_GROUND;
+	s->n_chan = 0;
+	s->maxdata = 1;
+	s->range_table = NULL;
+	s->insn_read = &dt9812_ai_rinsn;
+
+	/* analog output subdevice */
+	s = dev->subdevices + 3;
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITEABLE;
+	s->n_chan = 0;
+	s->maxdata = 1;
+	s->range_table = NULL;
+	s->insn_write = &dt9812_ao_winsn;
+	s->insn_read = &dt9812_ao_rinsn;
+
+	printk(KERN_INFO "comedi%d: successfully attached to dt9812.\n",
+	       dev->minor);
+
+	down(&dt9812_mutex);
+	/* Find a slot for the comedi device */
+	{
+		struct slot_dt9812 *first = NULL;
+		struct slot_dt9812 *best = NULL;
+		for (i = 0; i < DT9812_NUM_SLOTS; i++) {
+			if (!first && !dt9812[i].comedi) {
+				/* First free slot from comedi side */
+				first = &dt9812[i];
+			}
+			if (!best &&
+			    dt9812[i].usb &&
+			    dt9812[i].usb->serial == devpriv->serial) {
+				/* We have an attaced device with matching ID */
+				best = &dt9812[i];
+			}
+		}
+		if (!best)
+			best = first;
+		if (best) {
+			down(&best->mutex);
+			best->comedi = devpriv;
+			best->serial = devpriv->serial;
+			devpriv->slot = best;
+			up(&best->mutex);
+		}
+	}
+	up(&dt9812_mutex);
+
+	return 0;
+}
+
+static int dt9812_detach(comedi_device *dev)
+{
+	return 0;
+}
+
+static comedi_driver dt9812_comedi_driver = {
+	.module = THIS_MODULE,
+	.driver_name = "dt9812",
+	.attach = dt9812_attach,
+	.detach = dt9812_detach,
+};
+
+static int __init usb_dt9812_init(void)
+{
+	int result, i;
+
+	/* Initialize all driver slots */
+	for (i = 0; i < DT9812_NUM_SLOTS; i++) {
+		init_MUTEX(&dt9812[i].mutex);
+		dt9812[i].serial = 0;
+		dt9812[i].usb = NULL;
+		dt9812[i].comedi = NULL;
+	}
+	dt9812[12].serial = 0x0;
+
+	/* register with the USB subsystem */
+	result = usb_register(&dt9812_usb_driver);
+	if (result) {
+		printk(KERN_ERR KBUILD_MODNAME
+		       ": usb_register failed. Error number %d\n", result);
+		return result;
+	}
+	/* register with comedi */
+	result = comedi_driver_register(&dt9812_comedi_driver);
+	if (result) {
+		usb_deregister(&dt9812_usb_driver);
+		err("comedi_driver_register failed. Error number %d", result);
+	}
+
+	return result;
+}
+
+static void __exit usb_dt9812_exit(void)
+{
+	/* unregister with comedi */
+	comedi_driver_unregister(&dt9812_comedi_driver);
+
+	/* deregister this driver with the USB subsystem */
+	usb_deregister(&dt9812_usb_driver);
+}
+
+module_init(usb_dt9812_init);
+module_exit(usb_dt9812_exit);
+
+MODULE_AUTHOR("Anders Blomdell <anders.blomdell@control.lth.se>");
+MODULE_DESCRIPTION("Comedi DT9812 driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/icp_multi.c b/drivers/staging/comedi/drivers/icp_multi.c
new file mode 100644
index 000000000000..59144d7cb0bc
--- /dev/null
+++ b/drivers/staging/comedi/drivers/icp_multi.c
@@ -0,0 +1,1085 @@
+/*
+    comedi/drivers/icp_multi.c
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2002 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+/*
+Driver: icp_multi
+Description: Inova ICP_MULTI
+Author: Anne Smorthit <anne.smorthit@sfwte.ch>
+Devices: [Inova] ICP_MULTI (icp_multi)
+Status: works
+
+The driver works for analog input and output and digital input and output.
+It does not work with interrupts or with the counters.  Currently no support
+for DMA.
+
+It has 16 single-ended or 8 differential Analogue Input channels with 12-bit
+resolution.  Ranges : 5V, 10V, +/-5V, +/-10V, 0..20mA and 4..20mA.  Input
+ranges can be individually programmed for each channel.  Voltage or current
+measurement is selected by jumper.
+
+There are 4 x 12-bit Analogue Outputs.  Ranges : 5V, 10V, +/-5V, +/-10V
+
+16 x Digital Inputs, 24V
+
+8 x Digital Outputs, 24V, 1A
+
+4 x 16-bit counters
+
+Options:
+ [0] - PCI bus number - if bus number and slot number are 0,
+                        then driver search for first unused card
+ [1] - PCI slot number
+*/
+
+#include "../comedidev.h"
+
+#include <linux/delay.h>
+#include <linux/pci.h>
+
+#include "icp_multi.h"
+
+#define DEVICE_ID	0x8000	/* Device ID */
+
+#define ICP_MULTI_EXTDEBUG
+
+// Hardware types of the cards
+#define TYPE_ICP_MULTI	0
+
+#define IORANGE_ICP_MULTI 	32
+
+#define ICP_MULTI_ADC_CSR	0	/* R/W: ADC command/status register */
+#define ICP_MULTI_AI		2	/* R:   Analogue input data */
+#define ICP_MULTI_DAC_CSR	4	/* R/W: DAC command/status register */
+#define ICP_MULTI_AO		6	/* R/W: Analogue output data */
+#define ICP_MULTI_DI		8	/* R/W: Digital inouts */
+#define ICP_MULTI_DO		0x0A	/* R/W: Digital outputs */
+#define ICP_MULTI_INT_EN	0x0C	/* R/W: Interrupt enable register */
+#define ICP_MULTI_INT_STAT	0x0E	/* R/W: Interrupt status register */
+#define ICP_MULTI_CNTR0		0x10	/* R/W: Counter 0 */
+#define ICP_MULTI_CNTR1		0x12	/* R/W: counter 1 */
+#define ICP_MULTI_CNTR2		0x14	/* R/W: Counter 2 */
+#define ICP_MULTI_CNTR3		0x16	/* R/W: Counter 3 */
+
+#define ICP_MULTI_SIZE		0x20	/* 32 bytes */
+
+// Define bits from ADC command/status register
+#define	ADC_ST		0x0001	/* Start ADC */
+#define	ADC_BSY		0x0001	/* ADC busy */
+#define ADC_BI		0x0010	/* Bipolar input range 1 = bipolar */
+#define ADC_RA		0x0020	/* Input range 0 = 5V, 1 = 10V */
+#define	ADC_DI		0x0040	/* Differential input mode 1 = differential */
+
+// Define bits from DAC command/status register
+#define	DAC_ST		0x0001	/* Start DAC */
+#define DAC_BSY		0x0001	/* DAC busy */
+#define	DAC_BI		0x0010	/* Bipolar input range 1 = bipolar */
+#define	DAC_RA		0x0020	/* Input range 0 = 5V, 1 = 10V */
+
+// Define bits from interrupt enable/status registers
+#define	ADC_READY	0x0001	/* A/d conversion ready interrupt */
+#define	DAC_READY	0x0002	/* D/a conversion ready interrupt */
+#define	DOUT_ERROR	0x0004	/* Digital output error interrupt */
+#define	DIN_STATUS	0x0008	/* Digital input status change interrupt */
+#define	CIE0		0x0010	/* Counter 0 overrun interrupt */
+#define	CIE1		0x0020	/* Counter 1 overrun interrupt */
+#define	CIE2		0x0040	/* Counter 2 overrun interrupt */
+#define	CIE3		0x0080	/* Counter 3 overrun interrupt */
+
+// Useful definitions
+#define	Status_IRQ	0x00ff	// All interrupts
+
+// Define analogue range
+static const comedi_lrange range_analog = { 4, {
+			UNI_RANGE(5),
+			UNI_RANGE(10),
+			BIP_RANGE(5),
+			BIP_RANGE(10)
+	}
+};
+
+static const char range_codes_analog[] = { 0x00, 0x20, 0x10, 0x30 };
+
+/*
+==============================================================================
+	Forward declarations
+==============================================================================
+*/
+static int icp_multi_attach(comedi_device * dev, comedi_devconfig * it);
+static int icp_multi_detach(comedi_device * dev);
+
+/*
+==============================================================================
+	Data & Structure declarations
+==============================================================================
+*/
+static unsigned short pci_list_builded = 0;	/*>0 list of card is known */
+
+typedef struct {
+	const char *name;	// driver name
+	int device_id;
+	int iorange;		// I/O range len
+	char have_irq;		// 1=card support IRQ
+	char cardtype;		// 0=ICP Multi
+	int n_aichan;		// num of A/D chans
+	int n_aichand;		// num of A/D chans in diff mode
+	int n_aochan;		// num of D/A chans
+	int n_dichan;		// num of DI chans
+	int n_dochan;		// num of DO chans
+	int n_ctrs;		// num of counters
+	int ai_maxdata;		// resolution of A/D
+	int ao_maxdata;		// resolution of D/A
+	const comedi_lrange *rangelist_ai;	// rangelist for A/D
+	const char *rangecode;	// range codes for programming
+	const comedi_lrange *rangelist_ao;	// rangelist for D/A
+} boardtype;
+
+static const boardtype boardtypes[] = {
+	{"icp_multi",		// Driver name
+			DEVICE_ID,	// PCI device ID
+			IORANGE_ICP_MULTI,	// I/O range length
+			1,	// 1=Card supports interrupts
+			TYPE_ICP_MULTI,	// Card type = ICP MULTI
+			16,	// Num of A/D channels
+			8,	// Num of A/D channels in diff mode
+			4,	// Num of D/A channels
+			16,	// Num of digital inputs
+			8,	// Num of digital outputs
+			4,	// Num of counters
+			0x0fff,	// Resolution of A/D
+			0x0fff,	// Resolution of D/A
+			&range_analog,	// Rangelist for A/D
+			range_codes_analog,	// Range codes for programming
+		&range_analog},	// Rangelist for D/A
+};
+
+#define n_boardtypes (sizeof(boardtypes)/sizeof(boardtype))
+
+static comedi_driver driver_icp_multi = {
+      driver_name:"icp_multi",
+      module:THIS_MODULE,
+      attach:icp_multi_attach,
+      detach:icp_multi_detach,
+      num_names:n_boardtypes,
+      board_name:&boardtypes[0].name,
+      offset:sizeof(boardtype),
+};
+
+COMEDI_INITCLEANUP(driver_icp_multi);
+
+typedef struct {
+	struct pcilst_struct *card;	// pointer to card
+	char valid;		// card is usable
+	void *io_addr;		// Pointer to mapped io address
+	resource_size_t phys_iobase;	// Physical io address
+	unsigned int AdcCmdStatus;	// ADC Command/Status register
+	unsigned int DacCmdStatus;	// DAC Command/Status register
+	unsigned int IntEnable;	// Interrupt Enable register
+	unsigned int IntStatus;	// Interrupt Status register
+	unsigned int act_chanlist[32];	// list of scaned channel
+	unsigned char act_chanlist_len;	// len of scanlist
+	unsigned char act_chanlist_pos;	// actual position in MUX list
+	unsigned int *ai_chanlist;	// actaul chanlist
+	sampl_t *ai_data;	// data buffer
+	sampl_t ao_data[4];	// data output buffer
+	sampl_t di_data;	// Digital input data
+	unsigned int do_data;	// Remember digital output data
+} icp_multi_private;
+
+#define devpriv ((icp_multi_private *)dev->private)
+#define this_board ((const boardtype *)dev->board_ptr)
+
+/*
+==============================================================================
+	More forward declarations
+==============================================================================
+*/
+
+#if 0
+static int check_channel_list(comedi_device * dev, comedi_subdevice * s,
+	unsigned int *chanlist, unsigned int n_chan);
+#endif
+static void setup_channel_list(comedi_device * dev, comedi_subdevice * s,
+	unsigned int *chanlist, unsigned int n_chan);
+static int icp_multi_reset(comedi_device * dev);
+
+/*
+==============================================================================
+	Functions
+==============================================================================
+*/
+
+/*
+==============================================================================
+
+	Name:	icp_multi_insn_read_ai
+
+	Description:
+		This function reads a single analogue input.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		comedi_insn *insn	Pointer to current comedi instruction
+		lsampl_t *data		Pointer to analogue input data
+
+	Returns:int			Nmuber of instructions executed
+
+==============================================================================
+*/
+static int icp_multi_insn_read_ai(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	int n, timeout;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: BGN: icp_multi_insn_read_ai(...)\n");
+#endif
+	// Disable A/D conversion ready interrupt
+	devpriv->IntEnable &= ~ADC_READY;
+	writew(devpriv->IntEnable, devpriv->io_addr + ICP_MULTI_INT_EN);
+
+	// Clear interrupt status
+	devpriv->IntStatus |= ADC_READY;
+	writew(devpriv->IntStatus, devpriv->io_addr + ICP_MULTI_INT_STAT);
+
+	// Set up appropriate channel, mode and range data, for specified channel
+	setup_channel_list(dev, s, &insn->chanspec, 1);
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp_multi A ST=%4x IO=%p\n",
+		readw(devpriv->io_addr + ICP_MULTI_ADC_CSR),
+		devpriv->io_addr + ICP_MULTI_ADC_CSR);
+#endif
+
+	for (n = 0; n < insn->n; n++) {
+		// Set start ADC bit
+		devpriv->AdcCmdStatus |= ADC_ST;
+		writew(devpriv->AdcCmdStatus,
+			devpriv->io_addr + ICP_MULTI_ADC_CSR);
+		devpriv->AdcCmdStatus &= ~ADC_ST;
+
+#ifdef ICP_MULTI_EXTDEBUG
+		printk("icp multi B n=%d ST=%4x\n", n,
+			readw(devpriv->io_addr + ICP_MULTI_ADC_CSR));
+#endif
+
+		comedi_udelay(1);
+
+#ifdef ICP_MULTI_EXTDEBUG
+		printk("icp multi C n=%d ST=%4x\n", n,
+			readw(devpriv->io_addr + ICP_MULTI_ADC_CSR));
+#endif
+
+		// Wait for conversion to complete, or get fed up waiting
+		timeout = 100;
+		while (timeout--) {
+			if (!(readw(devpriv->io_addr +
+						ICP_MULTI_ADC_CSR) & ADC_BSY))
+				goto conv_finish;
+
+#ifdef ICP_MULTI_EXTDEBUG
+			if (!(timeout % 10))
+				printk("icp multi D n=%d tm=%d ST=%4x\n", n,
+					timeout,
+					readw(devpriv->io_addr +
+						ICP_MULTI_ADC_CSR));
+#endif
+
+			comedi_udelay(1);
+		}
+
+		// If we reach here, a timeout has occurred
+		comedi_error(dev, "A/D insn timeout");
+
+		// Disable interrupt
+		devpriv->IntEnable &= ~ADC_READY;
+		writew(devpriv->IntEnable, devpriv->io_addr + ICP_MULTI_INT_EN);
+
+		// Clear interrupt status
+		devpriv->IntStatus |= ADC_READY;
+		writew(devpriv->IntStatus,
+			devpriv->io_addr + ICP_MULTI_INT_STAT);
+
+		// Clear data received
+		data[n] = 0;
+
+#ifdef ICP_MULTI_EXTDEBUG
+		printk("icp multi EDBG: END: icp_multi_insn_read_ai(...) n=%d\n", n);
+#endif
+		return -ETIME;
+
+	      conv_finish:
+		data[n] =
+			(readw(devpriv->io_addr + ICP_MULTI_AI) >> 4) & 0x0fff;
+	}
+
+	// Disable interrupt
+	devpriv->IntEnable &= ~ADC_READY;
+	writew(devpriv->IntEnable, devpriv->io_addr + ICP_MULTI_INT_EN);
+
+	// Clear interrupt status
+	devpriv->IntStatus |= ADC_READY;
+	writew(devpriv->IntStatus, devpriv->io_addr + ICP_MULTI_INT_STAT);
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: END: icp_multi_insn_read_ai(...) n=%d\n", n);
+#endif
+	return n;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_insn_write_ao
+
+	Description:
+		This function writes a single analogue output.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		comedi_insn *insn	Pointer to current comedi instruction
+		lsampl_t *data		Pointer to analogue output data
+
+	Returns:int			Nmuber of instructions executed
+
+==============================================================================
+*/
+static int icp_multi_insn_write_ao(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	int n, chan, range, timeout;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: BGN: icp_multi_insn_write_ao(...)\n");
+#endif
+	// Disable D/A conversion ready interrupt
+	devpriv->IntEnable &= ~DAC_READY;
+	writew(devpriv->IntEnable, devpriv->io_addr + ICP_MULTI_INT_EN);
+
+	// Clear interrupt status
+	devpriv->IntStatus |= DAC_READY;
+	writew(devpriv->IntStatus, devpriv->io_addr + ICP_MULTI_INT_STAT);
+
+	// Get channel number and range
+	chan = CR_CHAN(insn->chanspec);
+	range = CR_RANGE(insn->chanspec);
+
+	// Set up range and channel data
+	// Bit 4 = 1 : Bipolar
+	// Bit 5 = 0 : 5V
+	// Bit 5 = 1 : 10V
+	// Bits 8-9 : Channel number
+	devpriv->DacCmdStatus &= 0xfccf;
+	devpriv->DacCmdStatus |= this_board->rangecode[range];
+	devpriv->DacCmdStatus |= (chan << 8);
+
+	writew(devpriv->DacCmdStatus, devpriv->io_addr + ICP_MULTI_DAC_CSR);
+
+	for (n = 0; n < insn->n; n++) {
+		// Wait for analogue output data register to be ready for new data, or get fed up waiting
+		timeout = 100;
+		while (timeout--) {
+			if (!(readw(devpriv->io_addr +
+						ICP_MULTI_DAC_CSR) & DAC_BSY))
+				goto dac_ready;
+
+#ifdef ICP_MULTI_EXTDEBUG
+			if (!(timeout % 10))
+				printk("icp multi A n=%d tm=%d ST=%4x\n", n,
+					timeout,
+					readw(devpriv->io_addr +
+						ICP_MULTI_DAC_CSR));
+#endif
+
+			comedi_udelay(1);
+		}
+
+		// If we reach here, a timeout has occurred
+		comedi_error(dev, "D/A insn timeout");
+
+		// Disable interrupt
+		devpriv->IntEnable &= ~DAC_READY;
+		writew(devpriv->IntEnable, devpriv->io_addr + ICP_MULTI_INT_EN);
+
+		// Clear interrupt status
+		devpriv->IntStatus |= DAC_READY;
+		writew(devpriv->IntStatus,
+			devpriv->io_addr + ICP_MULTI_INT_STAT);
+
+		// Clear data received
+		devpriv->ao_data[chan] = 0;
+
+#ifdef ICP_MULTI_EXTDEBUG
+		printk("icp multi EDBG: END: icp_multi_insn_write_ao(...) n=%d\n", n);
+#endif
+		return -ETIME;
+
+	      dac_ready:
+		// Write data to analogue output data register
+		writew(data[n], devpriv->io_addr + ICP_MULTI_AO);
+
+		// Set DAC_ST bit to write the data to selected channel
+		devpriv->DacCmdStatus |= DAC_ST;
+		writew(devpriv->DacCmdStatus,
+			devpriv->io_addr + ICP_MULTI_DAC_CSR);
+		devpriv->DacCmdStatus &= ~DAC_ST;
+
+		// Save analogue output data
+		devpriv->ao_data[chan] = data[n];
+	}
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: END: icp_multi_insn_write_ao(...) n=%d\n", n);
+#endif
+	return n;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_insn_read_ao
+
+	Description:
+		This function reads a single analogue output.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		comedi_insn *insn	Pointer to current comedi instruction
+		lsampl_t *data		Pointer to analogue output data
+
+	Returns:int			Nmuber of instructions executed
+
+==============================================================================
+*/
+static int icp_multi_insn_read_ao(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	int n, chan;
+
+	// Get channel number
+	chan = CR_CHAN(insn->chanspec);
+
+	// Read analogue outputs
+	for (n = 0; n < insn->n; n++)
+		data[n] = devpriv->ao_data[chan];
+
+	return n;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_insn_bits_di
+
+	Description:
+		This function reads the digital inputs.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		comedi_insn *insn	Pointer to current comedi instruction
+		lsampl_t *data		Pointer to analogue output data
+
+	Returns:int			Nmuber of instructions executed
+
+==============================================================================
+*/
+static int icp_multi_insn_bits_di(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	data[1] = readw(devpriv->io_addr + ICP_MULTI_DI);
+
+	return 2;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_insn_bits_do
+
+	Description:
+		This function writes the appropriate digital outputs.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		comedi_insn *insn	Pointer to current comedi instruction
+		lsampl_t *data		Pointer to analogue output data
+
+	Returns:int			Nmuber of instructions executed
+
+==============================================================================
+*/
+static int icp_multi_insn_bits_do(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: BGN: icp_multi_insn_bits_do(...)\n");
+#endif
+
+	if (data[0]) {
+		s->state &= ~data[0];
+		s->state |= (data[0] & data[1]);
+
+		printk("Digital outputs = %4x \n", s->state);
+
+		writew(s->state, devpriv->io_addr + ICP_MULTI_DO);
+	}
+
+	data[1] = readw(devpriv->io_addr + ICP_MULTI_DI);
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: END: icp_multi_insn_bits_do(...)\n");
+#endif
+	return 2;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_insn_read_ctr
+
+	Description:
+		This function reads the specified counter.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		comedi_insn *insn	Pointer to current comedi instruction
+		lsampl_t *data		Pointer to counter data
+
+	Returns:int			Nmuber of instructions executed
+
+==============================================================================
+*/
+static int icp_multi_insn_read_ctr(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	return 0;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_insn_write_ctr
+
+	Description:
+		This function write to the specified counter.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		comedi_insn *insn	Pointer to current comedi instruction
+		lsampl_t *data		Pointer to counter data
+
+	Returns:int			Nmuber of instructions executed
+
+==============================================================================
+*/
+static int icp_multi_insn_write_ctr(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	return 0;
+}
+
+/*
+==============================================================================
+
+	Name:	interrupt_service_icp_multi
+
+	Description:
+		This function is the interrupt service routine for all
+		interrupts generated by the icp multi board.
+
+	Parameters:
+		int irq
+		void *d			Pointer to current device
+
+==============================================================================
+*/
+static irqreturn_t interrupt_service_icp_multi(int irq, void *d PT_REGS_ARG)
+{
+	comedi_device *dev = d;
+	int int_no;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: BGN: interrupt_service_icp_multi(%d,...)\n",
+		irq);
+#endif
+
+	// Is this interrupt from our board?
+	int_no = readw(devpriv->io_addr + ICP_MULTI_INT_STAT) & Status_IRQ;
+	if (!int_no)
+		// No, exit
+		return IRQ_NONE;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: interrupt_service_icp_multi() ST: %4x\n",
+		readw(devpriv->io_addr + ICP_MULTI_INT_STAT));
+#endif
+
+	// Determine which interrupt is active & handle it
+	switch (int_no) {
+	case ADC_READY:
+		break;
+	case DAC_READY:
+		break;
+	case DOUT_ERROR:
+		break;
+	case DIN_STATUS:
+		break;
+	case CIE0:
+		break;
+	case CIE1:
+		break;
+	case CIE2:
+		break;
+	case CIE3:
+		break;
+	default:
+		break;
+
+	}
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: END: interrupt_service_icp_multi(...)\n");
+#endif
+	return IRQ_HANDLED;
+}
+
+#if 0
+/*
+==============================================================================
+
+	Name:	check_channel_list
+
+	Description:
+		This function checks if the channel list, provided by user
+		is built correctly
+
+	Parameters:
+		comedi_device *dev	Pointer to current sevice structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		unsigned int *chanlist	Pointer to packed channel list
+		unsigned int n_chan	Number of channels to scan
+
+	Returns:int 0 = failure
+		    1 = success
+
+==============================================================================
+*/
+static int check_channel_list(comedi_device * dev, comedi_subdevice * s,
+	unsigned int *chanlist, unsigned int n_chan)
+{
+	unsigned int i;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG:  check_channel_list(...,%d)\n", n_chan);
+#endif
+	// Check that we at least have one channel to check
+	if (n_chan < 1) {
+		comedi_error(dev, "range/channel list is empty!");
+		return 0;
+	}
+	// Check all channels
+	for (i = 0; i < n_chan; i++) {
+		// Check that channel number is < maximum
+		if (CR_AREF(chanlist[i]) == AREF_DIFF) {
+			if (CR_CHAN(chanlist[i]) > this_board->n_aichand) {
+				comedi_error(dev,
+					"Incorrect differential ai channel number");
+				return 0;
+			}
+		} else {
+			if (CR_CHAN(chanlist[i]) > this_board->n_aichan) {
+				comedi_error(dev,
+					"Incorrect ai channel number");
+				return 0;
+			}
+		}
+	}
+	return 1;
+}
+#endif
+
+/*
+==============================================================================
+
+	Name:	setup_channel_list
+
+	Description:
+		This function sets the appropriate channel selection,
+		differential input mode and range bits in the ADC Command/
+		Status register.
+
+	Parameters:
+		comedi_device *dev	Pointer to current sevice structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		unsigned int *chanlist	Pointer to packed channel list
+		unsigned int n_chan	Number of channels to scan
+
+	Returns:Void
+
+==============================================================================
+*/
+static void setup_channel_list(comedi_device * dev, comedi_subdevice * s,
+	unsigned int *chanlist, unsigned int n_chan)
+{
+	unsigned int i, range, chanprog;
+	unsigned int diff;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG:  setup_channel_list(...,%d)\n", n_chan);
+#endif
+	devpriv->act_chanlist_len = n_chan;
+	devpriv->act_chanlist_pos = 0;
+
+	for (i = 0; i < n_chan; i++) {
+		// Get channel
+		chanprog = CR_CHAN(chanlist[i]);
+
+		// Determine if it is a differential channel (Bit 15  = 1)
+		if (CR_AREF(chanlist[i]) == AREF_DIFF) {
+			diff = 1;
+			chanprog &= 0x0007;
+		} else {
+			diff = 0;
+			chanprog &= 0x000f;
+		}
+
+		// Clear channel, range and input mode bits in A/D command/status register
+		devpriv->AdcCmdStatus &= 0xf00f;
+
+		// Set channel number and differential mode status bit
+		if (diff) {
+			// Set channel number, bits 9-11 & mode, bit 6
+			devpriv->AdcCmdStatus |= (chanprog << 9);
+			devpriv->AdcCmdStatus |= ADC_DI;
+		} else
+			// Set channel number, bits 8-11
+			devpriv->AdcCmdStatus |= (chanprog << 8);
+
+		// Get range for current channel
+		range = this_board->rangecode[CR_RANGE(chanlist[i])];
+		// Set range. bits 4-5
+		devpriv->AdcCmdStatus |= range;
+
+		/* Output channel, range, mode to ICP Multi */
+		writew(devpriv->AdcCmdStatus,
+			devpriv->io_addr + ICP_MULTI_ADC_CSR);
+
+#ifdef ICP_MULTI_EXTDEBUG
+		printk("GS: %2d. [%4x]=%4x %4x\n", i, chanprog, range,
+			devpriv->act_chanlist[i]);
+#endif
+	}
+
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_reset
+
+	Description:
+		This function resets the icp multi device to a 'safe' state
+
+	Parameters:
+		comedi_device *dev	Pointer to current sevice structure
+
+	Returns:int	0 = success
+
+==============================================================================
+*/
+static int icp_multi_reset(comedi_device * dev)
+{
+	unsigned int i;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp_multi EDBG: BGN: icp_multi_reset(...)\n");
+#endif
+	// Clear INT enables and requests
+	writew(0, devpriv->io_addr + ICP_MULTI_INT_EN);
+	writew(0x00ff, devpriv->io_addr + ICP_MULTI_INT_STAT);
+
+	if (this_board->n_aochan)
+		// Set DACs to 0..5V range and 0V output
+		for (i = 0; i < this_board->n_aochan; i++) {
+			devpriv->DacCmdStatus &= 0xfcce;
+
+			// Set channel number
+			devpriv->DacCmdStatus |= (i << 8);
+
+			// Output 0V
+			writew(0, devpriv->io_addr + ICP_MULTI_AO);
+
+			// Set start conversion bit
+			devpriv->DacCmdStatus |= DAC_ST;
+
+			// Output to command / status register
+			writew(devpriv->DacCmdStatus,
+				devpriv->io_addr + ICP_MULTI_DAC_CSR);
+
+			// Delay to allow DAC time to recover
+			comedi_udelay(1);
+		}
+	// Digital outputs to 0
+	writew(0, devpriv->io_addr + ICP_MULTI_DO);
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: END: icp_multi_reset(...)\n");
+#endif
+	return 0;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_attach
+
+	Description:
+		This function sets up all the appropriate data for the current
+		device.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_devconfig *it	Pointer to current device configuration
+
+	Returns:int	0 = success
+
+==============================================================================
+*/
+static int icp_multi_attach(comedi_device * dev, comedi_devconfig * it)
+{
+	comedi_subdevice *s;
+	int ret, subdev, n_subdevices;
+	unsigned int irq;
+	struct pcilst_struct *card = NULL;
+	resource_size_t io_addr[5], iobase;
+	unsigned char pci_bus, pci_slot, pci_func;
+
+	printk("icp_multi EDBG: BGN: icp_multi_attach(...)\n");
+
+	// Alocate private data storage space
+	if ((ret = alloc_private(dev, sizeof(icp_multi_private))) < 0)
+		return ret;
+
+	// Initialise list of PCI cards in system, if not already done so
+	if (pci_list_builded++ == 0) {
+		pci_card_list_init(PCI_VENDOR_ID_ICP,
+#ifdef ICP_MULTI_EXTDEBUG
+			1
+#else
+			0
+#endif
+			);
+	}
+
+	printk("Anne's comedi%d: icp_multi: board=%s", dev->minor,
+		this_board->name);
+
+	if ((card = select_and_alloc_pci_card(PCI_VENDOR_ID_ICP,
+				this_board->device_id, it->options[0],
+				it->options[1])) == NULL)
+		return -EIO;
+
+	devpriv->card = card;
+
+	if ((pci_card_data(card, &pci_bus, &pci_slot, &pci_func, &io_addr[0],
+				&irq)) < 0) {
+		printk(" - Can't get configuration data!\n");
+		return -EIO;
+	}
+
+	iobase = io_addr[2];
+	devpriv->phys_iobase = iobase;
+
+	printk(", b:s:f=%d:%d:%d, io=0x%8llx \n", pci_bus, pci_slot, pci_func,
+		(unsigned long long)iobase);
+
+	devpriv->io_addr = ioremap(iobase, ICP_MULTI_SIZE);
+
+	if (devpriv->io_addr == NULL) {
+		printk("ioremap failed.\n");
+		return -ENOMEM;
+	}
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("0x%08llx mapped to %p, ", (unsigned long long)iobase,
+		devpriv->io_addr);
+#endif
+
+	dev->board_name = this_board->name;
+
+	n_subdevices = 0;
+	if (this_board->n_aichan)
+		n_subdevices++;
+	if (this_board->n_aochan)
+		n_subdevices++;
+	if (this_board->n_dichan)
+		n_subdevices++;
+	if (this_board->n_dochan)
+		n_subdevices++;
+	if (this_board->n_ctrs)
+		n_subdevices++;
+
+	if ((ret = alloc_subdevices(dev, n_subdevices)) < 0) {
+		return ret;
+	}
+
+	icp_multi_reset(dev);
+
+	if (this_board->have_irq) {
+		if (irq) {
+			if (comedi_request_irq(irq, interrupt_service_icp_multi,
+					IRQF_SHARED, "Inova Icp Multi", dev)) {
+				printk(", unable to allocate IRQ %u, DISABLING IT", irq);
+				irq = 0;	/* Can't use IRQ */
+			} else
+				printk(", irq=%u", irq);
+		} else
+			printk(", IRQ disabled");
+	} else
+		irq = 0;
+
+	dev->irq = irq;
+
+	printk(".\n");
+
+	subdev = 0;
+
+	if (this_board->n_aichan) {
+		s = dev->subdevices + subdev;
+		dev->read_subdev = s;
+		s->type = COMEDI_SUBD_AI;
+		s->subdev_flags = SDF_READABLE | SDF_COMMON | SDF_GROUND;
+		if (this_board->n_aichand)
+			s->subdev_flags |= SDF_DIFF;
+		s->n_chan = this_board->n_aichan;
+		s->maxdata = this_board->ai_maxdata;
+		s->len_chanlist = this_board->n_aichan;
+		s->range_table = this_board->rangelist_ai;
+		s->insn_read = icp_multi_insn_read_ai;
+		subdev++;
+	}
+
+	if (this_board->n_aochan) {
+		s = dev->subdevices + subdev;
+		s->type = COMEDI_SUBD_AO;
+		s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
+		s->n_chan = this_board->n_aochan;
+		s->maxdata = this_board->ao_maxdata;
+		s->len_chanlist = this_board->n_aochan;
+		s->range_table = this_board->rangelist_ao;
+		s->insn_write = icp_multi_insn_write_ao;
+		s->insn_read = icp_multi_insn_read_ao;
+		subdev++;
+	}
+
+	if (this_board->n_dichan) {
+		s = dev->subdevices + subdev;
+		s->type = COMEDI_SUBD_DI;
+		s->subdev_flags = SDF_READABLE;
+		s->n_chan = this_board->n_dichan;
+		s->maxdata = 1;
+		s->len_chanlist = this_board->n_dichan;
+		s->range_table = &range_digital;
+		s->io_bits = 0;
+		s->insn_bits = icp_multi_insn_bits_di;
+		subdev++;
+	}
+
+	if (this_board->n_dochan) {
+		s = dev->subdevices + subdev;
+		s->type = COMEDI_SUBD_DO;
+		s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+		s->n_chan = this_board->n_dochan;
+		s->maxdata = 1;
+		s->len_chanlist = this_board->n_dochan;
+		s->range_table = &range_digital;
+		s->io_bits = (1 << this_board->n_dochan) - 1;
+		s->state = 0;
+		s->insn_bits = icp_multi_insn_bits_do;
+		subdev++;
+	}
+
+	if (this_board->n_ctrs) {
+		s = dev->subdevices + subdev;
+		s->type = COMEDI_SUBD_COUNTER;
+		s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
+		s->n_chan = this_board->n_ctrs;
+		s->maxdata = 0xffff;
+		s->len_chanlist = this_board->n_ctrs;
+		s->state = 0;
+		s->insn_read = icp_multi_insn_read_ctr;
+		s->insn_write = icp_multi_insn_write_ctr;
+		subdev++;
+	}
+
+	devpriv->valid = 1;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: END: icp_multi_attach(...)\n");
+#endif
+
+	return 0;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_detach
+
+	Description:
+		This function releases all the resources used by the current
+		device.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+
+	Returns:int	0 = success
+
+==============================================================================
+*/
+static int icp_multi_detach(comedi_device * dev)
+{
+
+	if (dev->private)
+		if (devpriv->valid)
+			icp_multi_reset(dev);
+
+	if (dev->irq)
+		comedi_free_irq(dev->irq, dev);
+
+	if (dev->private && devpriv->io_addr)
+		iounmap(devpriv->io_addr);
+
+	if (dev->private && devpriv->card)
+		pci_card_free(devpriv->card);
+
+	if (--pci_list_builded == 0) {
+		pci_card_list_cleanup(PCI_VENDOR_ID_ICP);
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/comedi/drivers/icp_multi.h b/drivers/staging/comedi/drivers/icp_multi.h
new file mode 100644
index 000000000000..6df4a8d15ff2
--- /dev/null
+++ b/drivers/staging/comedi/drivers/icp_multi.h
@@ -0,0 +1,278 @@
+/*
+    comedi/drivers/icp_multi.h
+
+    Stuff for ICP Multi
+
+    Author: Anne Smorthit <anne.smorthit@sfwte.ch>
+
+*/
+
+#ifndef _ICP_MULTI_H_
+#define _ICP_MULTI_H_
+
+#include "../comedidev.h"
+#include "comedi_pci.h"
+
+/****************************************************************************/
+
+struct pcilst_struct {
+	struct pcilst_struct *next;
+	int used;
+	struct pci_dev *pcidev;
+	unsigned short vendor;
+	unsigned short device;
+	unsigned char pci_bus;
+	unsigned char pci_slot;
+	unsigned char pci_func;
+	resource_size_t io_addr[5];
+	unsigned int irq;
+};
+
+struct pcilst_struct *inova_devices;	// ptr to root list of all Inova devices
+
+/****************************************************************************/
+
+static void pci_card_list_init(unsigned short pci_vendor, char display);
+static void pci_card_list_cleanup(unsigned short pci_vendor);
+static struct pcilst_struct *find_free_pci_card_by_device(unsigned short
+	vendor_id, unsigned short device_id);
+static int find_free_pci_card_by_position(unsigned short vendor_id,
+	unsigned short device_id, unsigned short pci_bus,
+	unsigned short pci_slot, struct pcilst_struct **card);
+static struct pcilst_struct *select_and_alloc_pci_card(unsigned short vendor_id,
+	unsigned short device_id, unsigned short pci_bus,
+	unsigned short pci_slot);
+
+static int pci_card_alloc(struct pcilst_struct *amcc);
+static int pci_card_free(struct pcilst_struct *amcc);
+static void pci_card_list_display(void);
+static int pci_card_data(struct pcilst_struct *amcc,
+	unsigned char *pci_bus, unsigned char *pci_slot,
+	unsigned char *pci_func, resource_size_t * io_addr, unsigned int *irq);
+
+/****************************************************************************/
+
+/* build list of Inova cards in this system */
+static void pci_card_list_init(unsigned short pci_vendor, char display)
+{
+	struct pci_dev *pcidev;
+	struct pcilst_struct *inova, *last;
+	int i;
+
+	inova_devices = NULL;
+	last = NULL;
+
+	for (pcidev = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, NULL);
+		pcidev != NULL;
+		pcidev = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, pcidev)) {
+		if (pcidev->vendor == pci_vendor) {
+			inova = kmalloc(sizeof(*inova), GFP_KERNEL);
+			if (!inova) {
+				printk("icp_multi: pci_card_list_init: allocation failed\n");
+				pci_dev_put(pcidev);
+				break;
+			}
+			memset(inova, 0, sizeof(*inova));
+
+			inova->pcidev = pci_dev_get(pcidev);
+			if (last) {
+				last->next = inova;
+			} else {
+				inova_devices = inova;
+			}
+			last = inova;
+
+			inova->vendor = pcidev->vendor;
+			inova->device = pcidev->device;
+			inova->pci_bus = pcidev->bus->number;
+			inova->pci_slot = PCI_SLOT(pcidev->devfn);
+			inova->pci_func = PCI_FUNC(pcidev->devfn);
+			/* Note: resources may be invalid if PCI device
+			 * not enabled, but they are corrected in
+			 * pci_card_alloc. */
+			for (i = 0; i < 5; i++)
+				inova->io_addr[i] =
+					pci_resource_start(pcidev, i);
+			inova->irq = pcidev->irq;
+		}
+	}
+
+	if (display)
+		pci_card_list_display();
+}
+
+/****************************************************************************/
+/* free up list of amcc cards in this system */
+static void pci_card_list_cleanup(unsigned short pci_vendor)
+{
+	struct pcilst_struct *inova, *next;
+
+	for (inova = inova_devices; inova; inova = next) {
+		next = inova->next;
+		pci_dev_put(inova->pcidev);
+		kfree(inova);
+	}
+
+	inova_devices = NULL;
+}
+
+/****************************************************************************/
+/* find first unused card with this device_id */
+static struct pcilst_struct *find_free_pci_card_by_device(unsigned short
+	vendor_id, unsigned short device_id)
+{
+	struct pcilst_struct *inova, *next;
+
+	for (inova = inova_devices; inova; inova = next) {
+		next = inova->next;
+		if ((!inova->used) && (inova->device == device_id)
+			&& (inova->vendor == vendor_id))
+			return inova;
+
+	}
+
+	return NULL;
+}
+
+/****************************************************************************/
+/* find card on requested position */
+static int find_free_pci_card_by_position(unsigned short vendor_id,
+	unsigned short device_id, unsigned short pci_bus,
+	unsigned short pci_slot, struct pcilst_struct **card)
+{
+	struct pcilst_struct *inova, *next;
+
+	*card = NULL;
+	for (inova = inova_devices; inova; inova = next) {
+		next = inova->next;
+		if ((inova->vendor == vendor_id) && (inova->device == device_id)
+			&& (inova->pci_bus == pci_bus)
+			&& (inova->pci_slot == pci_slot)) {
+			if (!(inova->used)) {
+				*card = inova;
+				return 0;	// ok, card is found
+			} else {
+				return 2;	// card exist but is used
+			}
+		}
+	}
+
+	return 1;		// no card found
+}
+
+/****************************************************************************/
+/* mark card as used */
+static int pci_card_alloc(struct pcilst_struct *inova)
+{
+	int i;
+
+	if (!inova) {
+		rt_printk(" - BUG!! inova is NULL!\n");
+		return -1;
+	}
+
+	if (inova->used)
+		return 1;
+	if (comedi_pci_enable(inova->pcidev, "icp_multi")) {
+		rt_printk(" - Can't enable PCI device and request regions!\n");
+		return -1;
+	}
+	/* Resources will be accurate now. */
+	for (i = 0; i < 5; i++)
+		inova->io_addr[i] = pci_resource_start(inova->pcidev, i);
+	inova->irq = inova->pcidev->irq;
+	inova->used = 1;
+	return 0;
+}
+
+/****************************************************************************/
+/* mark card as free */
+static int pci_card_free(struct pcilst_struct *inova)
+{
+	if (!inova)
+		return -1;
+
+	if (!inova->used)
+		return 1;
+	inova->used = 0;
+	comedi_pci_disable(inova->pcidev);
+	return 0;
+}
+
+/****************************************************************************/
+/* display list of found cards */
+static void pci_card_list_display(void)
+{
+	struct pcilst_struct *inova, *next;
+
+	printk("Anne's List of pci cards\n");
+	printk("bus:slot:func vendor device io_inova io_daq irq used\n");
+
+	for (inova = inova_devices; inova; inova = next) {
+		next = inova->next;
+		printk("%2d   %2d   %2d  0x%4x 0x%4x   0x%8llx 0x%8llx  %2u  %2d\n", inova->pci_bus, inova->pci_slot, inova->pci_func, inova->vendor, inova->device, (unsigned long long)inova->io_addr[0], (unsigned long long)inova->io_addr[2], inova->irq, inova->used);
+
+	}
+}
+
+/****************************************************************************/
+/* return all card information for driver */
+static int pci_card_data(struct pcilst_struct *inova,
+	unsigned char *pci_bus, unsigned char *pci_slot,
+	unsigned char *pci_func, resource_size_t * io_addr, unsigned int *irq)
+{
+	int i;
+
+	if (!inova)
+		return -1;
+	*pci_bus = inova->pci_bus;
+	*pci_slot = inova->pci_slot;
+	*pci_func = inova->pci_func;
+	for (i = 0; i < 5; i++)
+		io_addr[i] = inova->io_addr[i];
+	*irq = inova->irq;
+	return 0;
+}
+
+/****************************************************************************/
+/* select and alloc card */
+static struct pcilst_struct *select_and_alloc_pci_card(unsigned short vendor_id,
+	unsigned short device_id, unsigned short pci_bus,
+	unsigned short pci_slot)
+{
+	struct pcilst_struct *card;
+	int err;
+
+	if ((pci_bus < 1) & (pci_slot < 1)) {	// use autodetection
+		if ((card = find_free_pci_card_by_device(vendor_id,
+					device_id)) == NULL) {
+			rt_printk(" - Unused card not found in system!\n");
+			return NULL;
+		}
+	} else {
+		switch (find_free_pci_card_by_position(vendor_id, device_id,
+				pci_bus, pci_slot, &card)) {
+		case 1:
+			rt_printk
+				(" - Card not found on requested position b:s %d:%d!\n",
+				pci_bus, pci_slot);
+			return NULL;
+		case 2:
+			rt_printk
+				(" - Card on requested position is used b:s %d:%d!\n",
+				pci_bus, pci_slot);
+			return NULL;
+		}
+	}
+
+	if ((err = pci_card_alloc(card)) != 0) {
+		if (err > 0)
+			rt_printk(" - Can't allocate card!\n");
+		/* else: error already printed. */
+		return NULL;
+	}
+
+	return card;
+}
+
+#endif
diff --git a/drivers/staging/comedi/drivers/me4000.c b/drivers/staging/comedi/drivers/me4000.c
new file mode 100644
index 000000000000..b432aa7d7644
--- /dev/null
+++ b/drivers/staging/comedi/drivers/me4000.c
@@ -0,0 +1,2362 @@
+/*
+   comedi/drivers/me4000.c
+   Source code for the Meilhaus ME-4000 board family.
+
+   COMEDI - Linux Control and Measurement Device Interface
+   Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation; either version 2 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program; if not, write to the Free Software
+   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ */
+/*
+Driver: me4000
+Description: Meilhaus ME-4000 series boards
+Devices: [Meilhaus] ME-4650 (me4000), ME-4670i, ME-4680, ME-4680i, ME-4680is
+Author: gg (Guenter Gebhardt <g.gebhardt@meilhaus.com>)
+Updated: Mon, 18 Mar 2002 15:34:01 -0800
+Status: broken (no support for loading firmware)
+
+Supports:
+
+    - Analog Input
+    - Analog Output
+    - Digital I/O
+    - Counter
+
+Configuration Options:
+
+    [0] - PCI bus number (optional)
+    [1] - PCI slot number (optional)
+
+    If bus/slot is not specified, the first available PCI
+    device will be used.
+
+The firmware required by these boards is available in the
+comedi_nonfree_firmware tarball available from
+http://www.comedi.org.  However, the driver's support for
+loading the firmware through comedi_config is currently
+broken.
+
+ */
+
+#include "../comedidev.h"
+
+#include <linux/delay.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+
+#include "comedi_pci.h"
+#include "me4000.h"
+#if 0
+/* file removed due to GPL incompatibility */
+#include "me4000_fw.h"
+#endif
+
+/*=============================================================================
+  PCI device table.
+  This is used by modprobe to translate PCI IDs to drivers.
+  ===========================================================================*/
+
+static DEFINE_PCI_DEVICE_TABLE(me4000_pci_table) = {
+	{PCI_VENDOR_ID_MEILHAUS, 0x4650, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, 0x4660, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4661, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4662, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4663, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, 0x4670, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4671, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4672, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4673, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, 0x4680, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4681, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4682, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4683, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, me4000_pci_table);
+
+static const me4000_board_t me4000_boards[] = {
+	{"ME-4650", 0x4650, {0, 0}, {16, 0, 0, 0}, {4}, {0}},
+
+	{"ME-4660", 0x4660, {0, 0}, {32, 0, 16, 0}, {4}, {3}},
+	{"ME-4660i", 0x4661, {0, 0}, {32, 0, 16, 0}, {4}, {3}},
+	{"ME-4660s", 0x4662, {0, 0}, {32, 8, 16, 0}, {4}, {3}},
+	{"ME-4660is", 0x4663, {0, 0}, {32, 8, 16, 0}, {4}, {3}},
+
+	{"ME-4670", 0x4670, {4, 0}, {32, 0, 16, 1}, {4}, {3}},
+	{"ME-4670i", 0x4671, {4, 0}, {32, 0, 16, 1}, {4}, {3}},
+	{"ME-4670s", 0x4672, {4, 0}, {32, 8, 16, 1}, {4}, {3}},
+	{"ME-4670is", 0x4673, {4, 0}, {32, 8, 16, 1}, {4}, {3}},
+
+	{"ME-4680", 0x4680, {4, 4}, {32, 0, 16, 1}, {4}, {3}},
+	{"ME-4680i", 0x4681, {4, 4}, {32, 0, 16, 1}, {4}, {3}},
+	{"ME-4680s", 0x4682, {4, 4}, {32, 8, 16, 1}, {4}, {3}},
+	{"ME-4680is", 0x4683, {4, 4}, {32, 8, 16, 1}, {4}, {3}},
+
+	{0},
+};
+
+#define ME4000_BOARD_VERSIONS (sizeof(me4000_boards) / sizeof(me4000_board_t) - 1)
+
+/*-----------------------------------------------------------------------------
+  Comedi function prototypes
+  ---------------------------------------------------------------------------*/
+static int me4000_attach(comedi_device * dev, comedi_devconfig * it);
+static int me4000_detach(comedi_device * dev);
+static comedi_driver driver_me4000 = {
+      driver_name:"me4000",
+      module:THIS_MODULE,
+      attach:me4000_attach,
+      detach:me4000_detach,
+};
+
+/*-----------------------------------------------------------------------------
+  Meilhaus function prototypes
+  ---------------------------------------------------------------------------*/
+static int me4000_probe(comedi_device * dev, comedi_devconfig * it);
+static int get_registers(comedi_device * dev, struct pci_dev *pci_dev_p);
+static int init_board_info(comedi_device * dev, struct pci_dev *pci_dev_p);
+static int init_ao_context(comedi_device * dev);
+static int init_ai_context(comedi_device * dev);
+static int init_dio_context(comedi_device * dev);
+static int init_cnt_context(comedi_device * dev);
+static int xilinx_download(comedi_device * dev);
+static int reset_board(comedi_device * dev);
+
+static int me4000_dio_insn_bits(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data);
+
+static int me4000_dio_insn_config(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data);
+
+static int cnt_reset(comedi_device * dev, unsigned int channel);
+
+static int cnt_config(comedi_device * dev,
+	unsigned int channel, unsigned int mode);
+
+static int me4000_cnt_insn_config(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data);
+
+static int me4000_cnt_insn_write(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data);
+
+static int me4000_cnt_insn_read(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data);
+
+static int me4000_ai_insn_read(comedi_device * dev,
+	comedi_subdevice * subdevice, comedi_insn * insn, lsampl_t * data);
+
+static int me4000_ai_cancel(comedi_device * dev, comedi_subdevice * s);
+
+static int ai_check_chanlist(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd);
+
+static int ai_round_cmd_args(comedi_device * dev,
+	comedi_subdevice * s,
+	comedi_cmd * cmd,
+	unsigned int *init_ticks,
+	unsigned int *scan_ticks, unsigned int *chan_ticks);
+
+static int ai_prepare(comedi_device * dev,
+	comedi_subdevice * s,
+	comedi_cmd * cmd,
+	unsigned int init_ticks,
+	unsigned int scan_ticks, unsigned int chan_ticks);
+
+static int ai_write_chanlist(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd);
+
+static irqreturn_t me4000_ai_isr(int irq, void *dev_id PT_REGS_ARG);
+
+static int me4000_ai_do_cmd_test(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd);
+
+static int me4000_ai_do_cmd(comedi_device * dev, comedi_subdevice * s);
+
+static int me4000_ao_insn_write(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data);
+
+static int me4000_ao_insn_read(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data);
+
+/*-----------------------------------------------------------------------------
+  Meilhaus inline functions
+  ---------------------------------------------------------------------------*/
+
+static inline void me4000_outb(comedi_device * dev, unsigned char value,
+	unsigned long port)
+{
+	PORT_PDEBUG("--> 0x%02X port 0x%04lX\n", value, port);
+	outb(value, port);
+}
+
+static inline void me4000_outl(comedi_device * dev, unsigned long value,
+	unsigned long port)
+{
+	PORT_PDEBUG("--> 0x%08lX port 0x%04lX\n", value, port);
+	outl(value, port);
+}
+
+static inline unsigned long me4000_inl(comedi_device * dev, unsigned long port)
+{
+	unsigned long value;
+	value = inl(port);
+	PORT_PDEBUG("<-- 0x%08lX port 0x%04lX\n", value, port);
+	return value;
+}
+
+static inline unsigned char me4000_inb(comedi_device * dev, unsigned long port)
+{
+	unsigned char value;
+	value = inb(port);
+	PORT_PDEBUG("<-- 0x%08X port 0x%04lX\n", value, port);
+	return value;
+}
+
+static const comedi_lrange me4000_ai_range = {
+	4,
+	{
+			UNI_RANGE(2.5),
+			UNI_RANGE(10),
+			BIP_RANGE(2.5),
+			BIP_RANGE(10),
+		}
+};
+
+static const comedi_lrange me4000_ao_range = {
+	1,
+	{
+			BIP_RANGE(10),
+		}
+};
+
+static int me4000_attach(comedi_device * dev, comedi_devconfig * it)
+{
+	comedi_subdevice *s;
+	int result;
+
+	CALL_PDEBUG("In me4000_attach()\n");
+
+	result = me4000_probe(dev, it);
+	if (result)
+		return result;
+
+	/*
+	 * Allocate the subdevice structures.  alloc_subdevice() is a
+	 * convenient macro defined in comedidev.h.  It relies on
+	 * n_subdevices being set correctly.
+	 */
+	if (alloc_subdevices(dev, 4) < 0)
+		return -ENOMEM;
+
+    /*=========================================================================
+      Analog input subdevice
+      ========================================================================*/
+
+	s = dev->subdevices + 0;
+
+	if (thisboard->ai.count) {
+		s->type = COMEDI_SUBD_AI;
+		s->subdev_flags =
+			SDF_READABLE | SDF_COMMON | SDF_GROUND | SDF_DIFF;
+		s->n_chan = thisboard->ai.count;
+		s->maxdata = 0xFFFF;	// 16 bit ADC
+		s->len_chanlist = ME4000_AI_CHANNEL_LIST_COUNT;
+		s->range_table = &me4000_ai_range;
+		s->insn_read = me4000_ai_insn_read;
+
+		if (info->irq > 0) {
+			if (comedi_request_irq(info->irq, me4000_ai_isr,
+					IRQF_SHARED, "ME-4000", dev)) {
+				printk("comedi%d: me4000: me4000_attach(): Unable to allocate irq\n", dev->minor);
+			} else {
+				dev->read_subdev = s;
+				s->subdev_flags |= SDF_CMD_READ;
+				s->cancel = me4000_ai_cancel;
+				s->do_cmdtest = me4000_ai_do_cmd_test;
+				s->do_cmd = me4000_ai_do_cmd;
+			}
+		} else {
+			printk(KERN_WARNING
+				"comedi%d: me4000: me4000_attach(): No interrupt available\n",
+				dev->minor);
+		}
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+    /*=========================================================================
+      Analog output subdevice
+      ========================================================================*/
+
+	s = dev->subdevices + 1;
+
+	if (thisboard->ao.count) {
+		s->type = COMEDI_SUBD_AO;
+		s->subdev_flags = SDF_WRITEABLE | SDF_COMMON | SDF_GROUND;
+		s->n_chan = thisboard->ao.count;
+		s->maxdata = 0xFFFF;	// 16 bit DAC
+		s->range_table = &me4000_ao_range;
+		s->insn_write = me4000_ao_insn_write;
+		s->insn_read = me4000_ao_insn_read;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+    /*=========================================================================
+      Digital I/O subdevice
+      ========================================================================*/
+
+	s = dev->subdevices + 2;
+
+	if (thisboard->dio.count) {
+		s->type = COMEDI_SUBD_DIO;
+		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+		s->n_chan = thisboard->dio.count * 8;
+		s->maxdata = 1;
+		s->range_table = &range_digital;
+		s->insn_bits = me4000_dio_insn_bits;
+		s->insn_config = me4000_dio_insn_config;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+	/*
+	 * Check for optoisolated ME-4000 version. If one the first
+	 * port is a fixed output port and the second is a fixed input port.
+	 */
+	if (!me4000_inl(dev, info->dio_context.dir_reg)) {
+		s->io_bits |= 0xFF;
+		me4000_outl(dev, ME4000_DIO_CTRL_BIT_MODE_0,
+			info->dio_context.dir_reg);
+	}
+
+    /*=========================================================================
+      Counter subdevice
+      ========================================================================*/
+
+	s = dev->subdevices + 3;
+
+	if (thisboard->cnt.count) {
+		s->type = COMEDI_SUBD_COUNTER;
+		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+		s->n_chan = thisboard->cnt.count;
+		s->maxdata = 0xFFFF;	// 16 bit counters
+		s->insn_read = me4000_cnt_insn_read;
+		s->insn_write = me4000_cnt_insn_write;
+		s->insn_config = me4000_cnt_insn_config;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+	return 0;
+}
+
+static int me4000_probe(comedi_device * dev, comedi_devconfig * it)
+{
+	struct pci_dev *pci_device;
+	int result, i;
+	me4000_board_t *board;
+
+	CALL_PDEBUG("In me4000_probe()\n");
+
+	/* Allocate private memory */
+	if (alloc_private(dev, sizeof(me4000_info_t)) < 0) {
+		return -ENOMEM;
+	}
+	/*
+	 * Probe the device to determine what device in the series it is.
+	 */
+	for (pci_device = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, NULL);
+		pci_device != NULL;
+		pci_device =
+		pci_get_device(PCI_ANY_ID, PCI_ANY_ID, pci_device)) {
+		if (pci_device->vendor == PCI_VENDOR_ID_MEILHAUS) {
+			for (i = 0; i < ME4000_BOARD_VERSIONS; i++) {
+				if (me4000_boards[i].device_id ==
+					pci_device->device) {
+					/* Was a particular bus/slot requested? */
+					if ((it->options[0] != 0)
+						|| (it->options[1] != 0)) {
+						/* Are we on the wrong bus/slot? */
+						if (pci_device->bus->number !=
+							it->options[0]
+							|| PCI_SLOT(pci_device->
+								devfn) !=
+							it->options[1]) {
+							continue;
+						}
+					}
+					dev->board_ptr = me4000_boards + i;
+					board = (me4000_board_t *) dev->
+						board_ptr;
+					info->pci_dev_p = pci_device;
+					goto found;
+				}
+			}
+		}
+	}
+
+	printk(KERN_ERR
+		"comedi%d: me4000: me4000_probe(): No supported board found (req. bus/slot : %d/%d)\n",
+		dev->minor, it->options[0], it->options[1]);
+	return -ENODEV;
+
+      found:
+
+	printk(KERN_INFO
+		"comedi%d: me4000: me4000_probe(): Found %s at PCI bus %d, slot %d\n",
+		dev->minor, me4000_boards[i].name, pci_device->bus->number,
+		PCI_SLOT(pci_device->devfn));
+
+	/* Set data in device structure */
+	dev->board_name = board->name;
+
+	/* Enable PCI device and request regions */
+	result = comedi_pci_enable(pci_device, dev->board_name);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Cannot enable PCI device and request I/O regions\n",
+			dev->minor);
+		return result;
+	}
+
+	/* Get the PCI base registers */
+	result = get_registers(dev, pci_device);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Cannot get registers\n",
+			dev->minor);
+		return result;
+	}
+	/* Initialize board info */
+	result = init_board_info(dev, pci_device);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Cannot init baord info\n",
+			dev->minor);
+		return result;
+	}
+
+	/* Init analog output context */
+	result = init_ao_context(dev);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Cannot init ao context\n",
+			dev->minor);
+		return result;
+	}
+
+	/* Init analog input context */
+	result = init_ai_context(dev);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Cannot init ai context\n",
+			dev->minor);
+		return result;
+	}
+
+	/* Init digital I/O context */
+	result = init_dio_context(dev);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Cannot init dio context\n",
+			dev->minor);
+		return result;
+	}
+
+	/* Init counter context */
+	result = init_cnt_context(dev);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Cannot init cnt context\n",
+			dev->minor);
+		return result;
+	}
+
+	/* Download the xilinx firmware */
+	result = xilinx_download(dev);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Can't download firmware\n",
+			dev->minor);
+		return result;
+	}
+
+	/* Make a hardware reset */
+	result = reset_board(dev);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Can't reset board\n",
+			dev->minor);
+		return result;
+	}
+
+	return 0;
+}
+
+static int get_registers(comedi_device * dev, struct pci_dev *pci_dev_p)
+{
+
+	CALL_PDEBUG("In get_registers()\n");
+
+    /*--------------------------- plx regbase ---------------------------------*/
+
+	info->plx_regbase = pci_resource_start(pci_dev_p, 1);
+	if (info->plx_regbase == 0) {
+		printk(KERN_ERR
+			"comedi%d: me4000: get_registers(): PCI base address 1 is not available\n",
+			dev->minor);
+		return -ENODEV;
+	}
+	info->plx_regbase_size = pci_resource_len(pci_dev_p, 1);
+
+    /*--------------------------- me4000 regbase ------------------------------*/
+
+	info->me4000_regbase = pci_resource_start(pci_dev_p, 2);
+	if (info->me4000_regbase == 0) {
+		printk(KERN_ERR
+			"comedi%d: me4000: get_registers(): PCI base address 2 is not available\n",
+			dev->minor);
+		return -ENODEV;
+	}
+	info->me4000_regbase_size = pci_resource_len(pci_dev_p, 2);
+
+    /*--------------------------- timer regbase ------------------------------*/
+
+	info->timer_regbase = pci_resource_start(pci_dev_p, 3);
+	if (info->timer_regbase == 0) {
+		printk(KERN_ERR
+			"comedi%d: me4000: get_registers(): PCI base address 3 is not available\n",
+			dev->minor);
+		return -ENODEV;
+	}
+	info->timer_regbase_size = pci_resource_len(pci_dev_p, 3);
+
+    /*--------------------------- program regbase ------------------------------*/
+
+	info->program_regbase = pci_resource_start(pci_dev_p, 5);
+	if (info->program_regbase == 0) {
+		printk(KERN_ERR
+			"comedi%d: me4000: get_registers(): PCI base address 5 is not available\n",
+			dev->minor);
+		return -ENODEV;
+	}
+	info->program_regbase_size = pci_resource_len(pci_dev_p, 5);
+
+	return 0;
+}
+
+static int init_board_info(comedi_device * dev, struct pci_dev *pci_dev_p)
+{
+	int result;
+
+	CALL_PDEBUG("In init_board_info()\n");
+
+	/* Init spin locks */
+	//spin_lock_init(&info->preload_lock);
+	//spin_lock_init(&info->ai_ctrl_lock);
+
+	/* Get the serial number */
+	result = pci_read_config_dword(pci_dev_p, 0x2C, &info->serial_no);
+	if (result != PCIBIOS_SUCCESSFUL) {
+		return result;
+	}
+
+	/* Get the hardware revision */
+	result = pci_read_config_byte(pci_dev_p, 0x08, &info->hw_revision);
+	if (result != PCIBIOS_SUCCESSFUL) {
+		return result;
+	}
+
+	/* Get the vendor id */
+	info->vendor_id = pci_dev_p->vendor;
+
+	/* Get the device id */
+	info->device_id = pci_dev_p->device;
+
+	/* Get the irq assigned to the board */
+	info->irq = pci_dev_p->irq;
+
+	return 0;
+}
+
+static int init_ao_context(comedi_device * dev)
+{
+	int i;
+
+	CALL_PDEBUG("In init_ao_context()\n");
+
+	for (i = 0; i < thisboard->ao.count; i++) {
+		//spin_lock_init(&info->ao_context[i].use_lock);
+		info->ao_context[i].irq = info->irq;
+
+		switch (i) {
+		case 0:
+			info->ao_context[i].ctrl_reg =
+				info->me4000_regbase + ME4000_AO_00_CTRL_REG;
+			info->ao_context[i].status_reg =
+				info->me4000_regbase + ME4000_AO_00_STATUS_REG;
+			info->ao_context[i].fifo_reg =
+				info->me4000_regbase + ME4000_AO_00_FIFO_REG;
+			info->ao_context[i].single_reg =
+				info->me4000_regbase + ME4000_AO_00_SINGLE_REG;
+			info->ao_context[i].timer_reg =
+				info->me4000_regbase + ME4000_AO_00_TIMER_REG;
+			info->ao_context[i].irq_status_reg =
+				info->me4000_regbase + ME4000_IRQ_STATUS_REG;
+			info->ao_context[i].preload_reg =
+				info->me4000_regbase + ME4000_AO_LOADSETREG_XX;
+			break;
+		case 1:
+			info->ao_context[i].ctrl_reg =
+				info->me4000_regbase + ME4000_AO_01_CTRL_REG;
+			info->ao_context[i].status_reg =
+				info->me4000_regbase + ME4000_AO_01_STATUS_REG;
+			info->ao_context[i].fifo_reg =
+				info->me4000_regbase + ME4000_AO_01_FIFO_REG;
+			info->ao_context[i].single_reg =
+				info->me4000_regbase + ME4000_AO_01_SINGLE_REG;
+			info->ao_context[i].timer_reg =
+				info->me4000_regbase + ME4000_AO_01_TIMER_REG;
+			info->ao_context[i].irq_status_reg =
+				info->me4000_regbase + ME4000_IRQ_STATUS_REG;
+			info->ao_context[i].preload_reg =
+				info->me4000_regbase + ME4000_AO_LOADSETREG_XX;
+			break;
+		case 2:
+			info->ao_context[i].ctrl_reg =
+				info->me4000_regbase + ME4000_AO_02_CTRL_REG;
+			info->ao_context[i].status_reg =
+				info->me4000_regbase + ME4000_AO_02_STATUS_REG;
+			info->ao_context[i].fifo_reg =
+				info->me4000_regbase + ME4000_AO_02_FIFO_REG;
+			info->ao_context[i].single_reg =
+				info->me4000_regbase + ME4000_AO_02_SINGLE_REG;
+			info->ao_context[i].timer_reg =
+				info->me4000_regbase + ME4000_AO_02_TIMER_REG;
+			info->ao_context[i].irq_status_reg =
+				info->me4000_regbase + ME4000_IRQ_STATUS_REG;
+			info->ao_context[i].preload_reg =
+				info->me4000_regbase + ME4000_AO_LOADSETREG_XX;
+			break;
+		case 3:
+			info->ao_context[i].ctrl_reg =
+				info->me4000_regbase + ME4000_AO_03_CTRL_REG;
+			info->ao_context[i].status_reg =
+				info->me4000_regbase + ME4000_AO_03_STATUS_REG;
+			info->ao_context[i].fifo_reg =
+				info->me4000_regbase + ME4000_AO_03_FIFO_REG;
+			info->ao_context[i].single_reg =
+				info->me4000_regbase + ME4000_AO_03_SINGLE_REG;
+			info->ao_context[i].timer_reg =
+				info->me4000_regbase + ME4000_AO_03_TIMER_REG;
+			info->ao_context[i].irq_status_reg =
+				info->me4000_regbase + ME4000_IRQ_STATUS_REG;
+			info->ao_context[i].preload_reg =
+				info->me4000_regbase + ME4000_AO_LOADSETREG_XX;
+			break;
+		default:
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static int init_ai_context(comedi_device * dev)
+{
+
+	CALL_PDEBUG("In init_ai_context()\n");
+
+	info->ai_context.irq = info->irq;
+
+	info->ai_context.ctrl_reg = info->me4000_regbase + ME4000_AI_CTRL_REG;
+	info->ai_context.status_reg =
+		info->me4000_regbase + ME4000_AI_STATUS_REG;
+	info->ai_context.channel_list_reg =
+		info->me4000_regbase + ME4000_AI_CHANNEL_LIST_REG;
+	info->ai_context.data_reg = info->me4000_regbase + ME4000_AI_DATA_REG;
+	info->ai_context.chan_timer_reg =
+		info->me4000_regbase + ME4000_AI_CHAN_TIMER_REG;
+	info->ai_context.chan_pre_timer_reg =
+		info->me4000_regbase + ME4000_AI_CHAN_PRE_TIMER_REG;
+	info->ai_context.scan_timer_low_reg =
+		info->me4000_regbase + ME4000_AI_SCAN_TIMER_LOW_REG;
+	info->ai_context.scan_timer_high_reg =
+		info->me4000_regbase + ME4000_AI_SCAN_TIMER_HIGH_REG;
+	info->ai_context.scan_pre_timer_low_reg =
+		info->me4000_regbase + ME4000_AI_SCAN_PRE_TIMER_LOW_REG;
+	info->ai_context.scan_pre_timer_high_reg =
+		info->me4000_regbase + ME4000_AI_SCAN_PRE_TIMER_HIGH_REG;
+	info->ai_context.start_reg = info->me4000_regbase + ME4000_AI_START_REG;
+	info->ai_context.irq_status_reg =
+		info->me4000_regbase + ME4000_IRQ_STATUS_REG;
+	info->ai_context.sample_counter_reg =
+		info->me4000_regbase + ME4000_AI_SAMPLE_COUNTER_REG;
+
+	return 0;
+}
+
+static int init_dio_context(comedi_device * dev)
+{
+
+	CALL_PDEBUG("In init_dio_context()\n");
+
+	info->dio_context.dir_reg = info->me4000_regbase + ME4000_DIO_DIR_REG;
+	info->dio_context.ctrl_reg = info->me4000_regbase + ME4000_DIO_CTRL_REG;
+	info->dio_context.port_0_reg =
+		info->me4000_regbase + ME4000_DIO_PORT_0_REG;
+	info->dio_context.port_1_reg =
+		info->me4000_regbase + ME4000_DIO_PORT_1_REG;
+	info->dio_context.port_2_reg =
+		info->me4000_regbase + ME4000_DIO_PORT_2_REG;
+	info->dio_context.port_3_reg =
+		info->me4000_regbase + ME4000_DIO_PORT_3_REG;
+
+	return 0;
+}
+
+static int init_cnt_context(comedi_device * dev)
+{
+
+	CALL_PDEBUG("In init_cnt_context()\n");
+
+	info->cnt_context.ctrl_reg = info->timer_regbase + ME4000_CNT_CTRL_REG;
+	info->cnt_context.counter_0_reg =
+		info->timer_regbase + ME4000_CNT_COUNTER_0_REG;
+	info->cnt_context.counter_1_reg =
+		info->timer_regbase + ME4000_CNT_COUNTER_1_REG;
+	info->cnt_context.counter_2_reg =
+		info->timer_regbase + ME4000_CNT_COUNTER_2_REG;
+
+	return 0;
+}
+
+#define FIRMWARE_NOT_AVAILABLE 1
+#if FIRMWARE_NOT_AVAILABLE
+extern unsigned char *xilinx_firm;
+#endif
+
+static int xilinx_download(comedi_device * dev)
+{
+	u32 value = 0;
+	wait_queue_head_t queue;
+	int idx = 0;
+	int size = 0;
+
+	CALL_PDEBUG("In xilinx_download()\n");
+
+	init_waitqueue_head(&queue);
+
+	/*
+	 * Set PLX local interrupt 2 polarity to high.
+	 * Interrupt is thrown by init pin of xilinx.
+	 */
+	outl(0x10, info->plx_regbase + PLX_INTCSR);
+
+	/* Set /CS and /WRITE of the Xilinx */
+	value = inl(info->plx_regbase + PLX_ICR);
+	value |= 0x100;
+	outl(value, info->plx_regbase + PLX_ICR);
+
+	/* Init Xilinx with CS1 */
+	inb(info->program_regbase + 0xC8);
+
+	/* Wait until /INIT pin is set */
+	udelay(20);
+	if (!inl(info->plx_regbase + PLX_INTCSR) & 0x20) {
+		printk(KERN_ERR
+			"comedi%d: me4000: xilinx_download(): Can't init Xilinx\n",
+			dev->minor);
+		return -EIO;
+	}
+
+	/* Reset /CS and /WRITE of the Xilinx */
+	value = inl(info->plx_regbase + PLX_ICR);
+	value &= ~0x100;
+	outl(value, info->plx_regbase + PLX_ICR);
+	if (FIRMWARE_NOT_AVAILABLE) {
+		comedi_error(dev,
+			"xilinx firmware unavailable due to licensing, aborting");
+		return -EIO;
+	} else {
+		/* Download Xilinx firmware */
+		size = (xilinx_firm[0] << 24) + (xilinx_firm[1] << 16) +
+			(xilinx_firm[2] << 8) + xilinx_firm[3];
+		udelay(10);
+
+		for (idx = 0; idx < size; idx++) {
+			outb(xilinx_firm[16 + idx], info->program_regbase);
+			udelay(10);
+
+			/* Check if BUSY flag is low */
+			if (inl(info->plx_regbase + PLX_ICR) & 0x20) {
+				printk(KERN_ERR
+					"comedi%d: me4000: xilinx_download(): Xilinx is still busy (idx = %d)\n",
+					dev->minor, idx);
+				return -EIO;
+			}
+		}
+	}
+
+	/* If done flag is high download was successful */
+	if (inl(info->plx_regbase + PLX_ICR) & 0x4) {
+	} else {
+		printk(KERN_ERR
+			"comedi%d: me4000: xilinx_download(): DONE flag is not set\n",
+			dev->minor);
+		printk(KERN_ERR
+			"comedi%d: me4000: xilinx_download(): Download not succesful\n",
+			dev->minor);
+		return -EIO;
+	}
+
+	/* Set /CS and /WRITE */
+	value = inl(info->plx_regbase + PLX_ICR);
+	value |= 0x100;
+	outl(value, info->plx_regbase + PLX_ICR);
+
+	return 0;
+}
+
+static int reset_board(comedi_device * dev)
+{
+	unsigned long icr;
+
+	CALL_PDEBUG("In reset_board()\n");
+
+	/* Make a hardware reset */
+	icr = me4000_inl(dev, info->plx_regbase + PLX_ICR);
+	icr |= 0x40000000;
+	me4000_outl(dev, icr, info->plx_regbase + PLX_ICR);
+	icr &= ~0x40000000;
+	me4000_outl(dev, icr, info->plx_regbase + PLX_ICR);
+
+	/* 0x8000 to the DACs means an output voltage of 0V */
+	me4000_outl(dev, 0x8000,
+		info->me4000_regbase + ME4000_AO_00_SINGLE_REG);
+	me4000_outl(dev, 0x8000,
+		info->me4000_regbase + ME4000_AO_01_SINGLE_REG);
+	me4000_outl(dev, 0x8000,
+		info->me4000_regbase + ME4000_AO_02_SINGLE_REG);
+	me4000_outl(dev, 0x8000,
+		info->me4000_regbase + ME4000_AO_03_SINGLE_REG);
+
+	/* Set both stop bits in the analog input control register */
+	me4000_outl(dev,
+		ME4000_AI_CTRL_BIT_IMMEDIATE_STOP | ME4000_AI_CTRL_BIT_STOP,
+		info->me4000_regbase + ME4000_AI_CTRL_REG);
+
+	/* Set both stop bits in the analog output control register */
+	me4000_outl(dev,
+		ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
+		info->me4000_regbase + ME4000_AO_00_CTRL_REG);
+	me4000_outl(dev,
+		ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
+		info->me4000_regbase + ME4000_AO_01_CTRL_REG);
+	me4000_outl(dev,
+		ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
+		info->me4000_regbase + ME4000_AO_02_CTRL_REG);
+	me4000_outl(dev,
+		ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
+		info->me4000_regbase + ME4000_AO_03_CTRL_REG);
+
+	/* Enable interrupts on the PLX */
+	me4000_outl(dev, 0x43, info->plx_regbase + PLX_INTCSR);
+
+	/* Set the adustment register for AO demux */
+	me4000_outl(dev, ME4000_AO_DEMUX_ADJUST_VALUE,
+		info->me4000_regbase + ME4000_AO_DEMUX_ADJUST_REG);
+
+	/* Set digital I/O direction for port 0 to output on isolated versions */
+	if (!(me4000_inl(dev, info->me4000_regbase + ME4000_DIO_DIR_REG) & 0x1)) {
+		me4000_outl(dev, 0x1,
+			info->me4000_regbase + ME4000_DIO_CTRL_REG);
+	}
+
+	return 0;
+}
+
+static int me4000_detach(comedi_device * dev)
+{
+	CALL_PDEBUG("In me4000_detach()\n");
+
+	if (info) {
+		if (info->pci_dev_p) {
+			reset_board(dev);
+			if (info->plx_regbase) {
+				comedi_pci_disable(info->pci_dev_p);
+			}
+			pci_dev_put(info->pci_dev_p);
+		}
+	}
+
+	return 0;
+}
+
+/*=============================================================================
+  Analog input section
+  ===========================================================================*/
+
+static int me4000_ai_insn_read(comedi_device * dev,
+	comedi_subdevice * subdevice, comedi_insn * insn, lsampl_t * data)
+{
+
+	int chan = CR_CHAN(insn->chanspec);
+	int rang = CR_RANGE(insn->chanspec);
+	int aref = CR_AREF(insn->chanspec);
+
+	unsigned long entry = 0;
+	unsigned long tmp;
+	long lval;
+
+	CALL_PDEBUG("In me4000_ai_insn_read()\n");
+
+	if (insn->n == 0) {
+		return 0;
+	} else if (insn->n > 1) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_insn_read(): Invalid instruction length %d\n",
+			dev->minor, insn->n);
+		return -EINVAL;
+	}
+
+	switch (rang) {
+	case 0:
+		entry |= ME4000_AI_LIST_RANGE_UNIPOLAR_2_5;
+		break;
+	case 1:
+		entry |= ME4000_AI_LIST_RANGE_UNIPOLAR_10;
+		break;
+	case 2:
+		entry |= ME4000_AI_LIST_RANGE_BIPOLAR_2_5;
+		break;
+	case 3:
+		entry |= ME4000_AI_LIST_RANGE_BIPOLAR_10;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_insn_read(): Invalid range specified\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	switch (aref) {
+	case AREF_GROUND:
+	case AREF_COMMON:
+		if (chan >= thisboard->ai.count) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_insn_read(): Analog input is not available\n",
+				dev->minor);
+			return -EINVAL;
+		}
+		entry |= ME4000_AI_LIST_INPUT_SINGLE_ENDED | chan;
+		break;
+
+	case AREF_DIFF:
+		if (rang == 0 || rang == 1) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_insn_read(): Range must be bipolar when aref = diff\n",
+				dev->minor);
+			return -EINVAL;
+		}
+
+		if (chan >= thisboard->ai.diff_count) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_insn_read(): Analog input is not available\n",
+				dev->minor);
+			return -EINVAL;
+		}
+		entry |= ME4000_AI_LIST_INPUT_DIFFERENTIAL | chan;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_insn_read(): Invalid aref specified\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	entry |= ME4000_AI_LIST_LAST_ENTRY;
+
+	/* Clear channel list, data fifo and both stop bits */
+	tmp = me4000_inl(dev, info->ai_context.ctrl_reg);
+	tmp &= ~(ME4000_AI_CTRL_BIT_CHANNEL_FIFO |
+		ME4000_AI_CTRL_BIT_DATA_FIFO |
+		ME4000_AI_CTRL_BIT_STOP | ME4000_AI_CTRL_BIT_IMMEDIATE_STOP);
+	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+
+	/* Set the acquisition mode to single */
+	tmp &= ~(ME4000_AI_CTRL_BIT_MODE_0 | ME4000_AI_CTRL_BIT_MODE_1 |
+		ME4000_AI_CTRL_BIT_MODE_2);
+	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+
+	/* Enable channel list and data fifo */
+	tmp |= ME4000_AI_CTRL_BIT_CHANNEL_FIFO | ME4000_AI_CTRL_BIT_DATA_FIFO;
+	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+
+	/* Generate channel list entry */
+	me4000_outl(dev, entry, info->ai_context.channel_list_reg);
+
+	/* Set the timer to maximum sample rate */
+	me4000_outl(dev, ME4000_AI_MIN_TICKS, info->ai_context.chan_timer_reg);
+	me4000_outl(dev, ME4000_AI_MIN_TICKS,
+		info->ai_context.chan_pre_timer_reg);
+
+	/* Start conversion by dummy read */
+	me4000_inl(dev, info->ai_context.start_reg);
+
+	/* Wait until ready */
+	udelay(10);
+	if (!(me4000_inl(dev, info->ai_context.
+				status_reg) & ME4000_AI_STATUS_BIT_EF_DATA)) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_insn_read(): Value not available after wait\n",
+			dev->minor);
+		return -EIO;
+	}
+
+	/* Read value from data fifo */
+	lval = me4000_inl(dev, info->ai_context.data_reg) & 0xFFFF;
+	data[0] = lval ^ 0x8000;
+
+	return 1;
+}
+
+static int me4000_ai_cancel(comedi_device * dev, comedi_subdevice * s)
+{
+	unsigned long tmp;
+
+	CALL_PDEBUG("In me4000_ai_cancel()\n");
+
+	/* Stop any running conversion */
+	tmp = me4000_inl(dev, info->ai_context.ctrl_reg);
+	tmp &= ~(ME4000_AI_CTRL_BIT_STOP | ME4000_AI_CTRL_BIT_IMMEDIATE_STOP);
+	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+
+	/* Clear the control register */
+	me4000_outl(dev, 0x0, info->ai_context.ctrl_reg);
+
+	return 0;
+}
+
+static int ai_check_chanlist(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd)
+{
+	int aref;
+	int i;
+
+	CALL_PDEBUG("In ai_check_chanlist()\n");
+
+	/* Check whether a channel list is available */
+	if (!cmd->chanlist_len) {
+		printk(KERN_ERR
+			"comedi%d: me4000: ai_check_chanlist(): No channel list available\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	/* Check the channel list size */
+	if (cmd->chanlist_len > ME4000_AI_CHANNEL_LIST_COUNT) {
+		printk(KERN_ERR
+			"comedi%d: me4000: ai_check_chanlist(): Channel list is to large\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	/* Check the pointer */
+	if (!cmd->chanlist) {
+		printk(KERN_ERR
+			"comedi%d: me4000: ai_check_chanlist(): NULL pointer to channel list\n",
+			dev->minor);
+		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) {
+			printk(KERN_ERR
+				"comedi%d: me4000: ai_check_chanlist(): Mode is not equal for all entries\n",
+				dev->minor);
+			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_count) {
+				printk(KERN_ERR
+					"comedi%d: me4000: ai_check_chanlist(): Channel number to high\n",
+					dev->minor);
+				return -EINVAL;
+			}
+		}
+	} else {
+		for (i = 0; i < cmd->chanlist_len; i++) {
+			if (CR_CHAN(cmd->chanlist[i]) >= thisboard->ai.count) {
+				printk(KERN_ERR
+					"comedi%d: me4000: ai_check_chanlist(): Channel number to high\n",
+					dev->minor);
+				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) {
+				printk(KERN_ERR
+					"comedi%d: me4000: ai_check_chanlist(): Bipolar is not selected in differential mode\n",
+					dev->minor);
+				return -EINVAL;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int ai_round_cmd_args(comedi_device * dev,
+	comedi_subdevice * s,
+	comedi_cmd * cmd,
+	unsigned int *init_ticks,
+	unsigned int *scan_ticks, unsigned int *chan_ticks)
+{
+
+	int rest;
+
+	CALL_PDEBUG("In ai_round_cmd_args()\n");
+
+	*init_ticks = 0;
+	*scan_ticks = 0;
+	*chan_ticks = 0;
+
+	PDEBUG("ai_round_cmd_arg(): start_arg = %d\n", cmd->start_arg);
+	PDEBUG("ai_round_cmd_arg(): scan_begin_arg = %d\n",
+		cmd->scan_begin_arg);
+	PDEBUG("ai_round_cmd_arg(): convert_arg = %d\n", cmd->convert_arg);
+
+	if (cmd->start_arg) {
+		*init_ticks = (cmd->start_arg * 33) / 1000;
+		rest = (cmd->start_arg * 33) % 1000;
+
+		if (cmd->flags & TRIG_ROUND_NEAREST) {
+			if (rest > 33) {
+				(*init_ticks)++;
+			}
+		} else if (cmd->flags & TRIG_ROUND_UP) {
+			if (rest)
+				(*init_ticks)++;
+		}
+	}
+
+	if (cmd->scan_begin_arg) {
+		*scan_ticks = (cmd->scan_begin_arg * 33) / 1000;
+		rest = (cmd->scan_begin_arg * 33) % 1000;
+
+		if (cmd->flags & TRIG_ROUND_NEAREST) {
+			if (rest > 33) {
+				(*scan_ticks)++;
+			}
+		} else if (cmd->flags & TRIG_ROUND_UP) {
+			if (rest)
+				(*scan_ticks)++;
+		}
+	}
+
+	if (cmd->convert_arg) {
+		*chan_ticks = (cmd->convert_arg * 33) / 1000;
+		rest = (cmd->convert_arg * 33) % 1000;
+
+		if (cmd->flags & TRIG_ROUND_NEAREST) {
+			if (rest > 33) {
+				(*chan_ticks)++;
+			}
+		} else if (cmd->flags & TRIG_ROUND_UP) {
+			if (rest)
+				(*chan_ticks)++;
+		}
+	}
+
+	PDEBUG("ai_round_cmd_args(): init_ticks = %d\n", *init_ticks);
+	PDEBUG("ai_round_cmd_args(): scan_ticks = %d\n", *scan_ticks);
+	PDEBUG("ai_round_cmd_args(): chan_ticks = %d\n", *chan_ticks);
+
+	return 0;
+}
+
+static void ai_write_timer(comedi_device * dev,
+	unsigned int init_ticks,
+	unsigned int scan_ticks, unsigned int chan_ticks)
+{
+
+	CALL_PDEBUG("In ai_write_timer()\n");
+
+	me4000_outl(dev, init_ticks - 1,
+		info->ai_context.scan_pre_timer_low_reg);
+	me4000_outl(dev, 0x0, info->ai_context.scan_pre_timer_high_reg);
+
+	if (scan_ticks) {
+		me4000_outl(dev, scan_ticks - 1,
+			info->ai_context.scan_timer_low_reg);
+		me4000_outl(dev, 0x0, info->ai_context.scan_timer_high_reg);
+	}
+
+	me4000_outl(dev, chan_ticks - 1, info->ai_context.chan_pre_timer_reg);
+	me4000_outl(dev, chan_ticks - 1, info->ai_context.chan_timer_reg);
+}
+
+static int ai_prepare(comedi_device * dev,
+	comedi_subdevice * s,
+	comedi_cmd * cmd,
+	unsigned int init_ticks,
+	unsigned int scan_ticks, unsigned int chan_ticks)
+{
+
+	unsigned long tmp = 0;
+
+	CALL_PDEBUG("In ai_prepare()\n");
+
+	/* Write timer arguments */
+	ai_write_timer(dev, init_ticks, scan_ticks, chan_ticks);
+
+	/* Reset control register */
+	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+
+	/* Start sources */
+	if ((cmd->start_src == TRIG_EXT &&
+			cmd->scan_begin_src == TRIG_TIMER &&
+			cmd->convert_src == TRIG_TIMER) ||
+		(cmd->start_src == TRIG_EXT &&
+			cmd->scan_begin_src == TRIG_FOLLOW &&
+			cmd->convert_src == TRIG_TIMER)) {
+		tmp = ME4000_AI_CTRL_BIT_MODE_1 |
+			ME4000_AI_CTRL_BIT_CHANNEL_FIFO |
+			ME4000_AI_CTRL_BIT_DATA_FIFO;
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_EXT &&
+		cmd->convert_src == TRIG_TIMER) {
+		tmp = ME4000_AI_CTRL_BIT_MODE_2 |
+			ME4000_AI_CTRL_BIT_CHANNEL_FIFO |
+			ME4000_AI_CTRL_BIT_DATA_FIFO;
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_EXT &&
+		cmd->convert_src == TRIG_EXT) {
+		tmp = ME4000_AI_CTRL_BIT_MODE_0 |
+			ME4000_AI_CTRL_BIT_MODE_1 |
+			ME4000_AI_CTRL_BIT_CHANNEL_FIFO |
+			ME4000_AI_CTRL_BIT_DATA_FIFO;
+	} else {
+		tmp = ME4000_AI_CTRL_BIT_MODE_0 |
+			ME4000_AI_CTRL_BIT_CHANNEL_FIFO |
+			ME4000_AI_CTRL_BIT_DATA_FIFO;
+	}
+
+	/* Stop triggers */
+	if (cmd->stop_src == TRIG_COUNT) {
+		me4000_outl(dev, cmd->chanlist_len * cmd->stop_arg,
+			info->ai_context.sample_counter_reg);
+		tmp |= ME4000_AI_CTRL_BIT_HF_IRQ | ME4000_AI_CTRL_BIT_SC_IRQ;
+	} else if (cmd->stop_src == TRIG_NONE &&
+		cmd->scan_end_src == TRIG_COUNT) {
+		me4000_outl(dev, cmd->scan_end_arg,
+			info->ai_context.sample_counter_reg);
+		tmp |= ME4000_AI_CTRL_BIT_HF_IRQ | ME4000_AI_CTRL_BIT_SC_IRQ;
+	} else {
+		tmp |= ME4000_AI_CTRL_BIT_HF_IRQ;
+	}
+
+	/* Write the setup to the control register */
+	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+
+	/* Write the channel list */
+	ai_write_chanlist(dev, s, cmd);
+
+	return 0;
+}
+
+static int ai_write_chanlist(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd)
+{
+	unsigned int entry;
+	unsigned int chan;
+	unsigned int rang;
+	unsigned int aref;
+	int i;
+
+	CALL_PDEBUG("In ai_write_chanlist()\n");
+
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		chan = CR_CHAN(cmd->chanlist[i]);
+		rang = CR_RANGE(cmd->chanlist[i]);
+		aref = CR_AREF(cmd->chanlist[i]);
+
+		entry = chan;
+
+		if (rang == 0) {
+			entry |= ME4000_AI_LIST_RANGE_UNIPOLAR_2_5;
+		} else if (rang == 1) {
+			entry |= ME4000_AI_LIST_RANGE_UNIPOLAR_10;
+		} else if (rang == 2) {
+			entry |= ME4000_AI_LIST_RANGE_BIPOLAR_2_5;
+		} else {
+			entry |= ME4000_AI_LIST_RANGE_BIPOLAR_10;
+		}
+
+		if (aref == SDF_DIFF) {
+			entry |= ME4000_AI_LIST_INPUT_DIFFERENTIAL;
+		} else {
+			entry |= ME4000_AI_LIST_INPUT_SINGLE_ENDED;
+		}
+
+		me4000_outl(dev, entry, info->ai_context.channel_list_reg);
+	}
+
+	return 0;
+}
+
+static int me4000_ai_do_cmd(comedi_device * dev, comedi_subdevice * s)
+{
+	int err;
+	unsigned int init_ticks = 0;
+	unsigned int scan_ticks = 0;
+	unsigned int chan_ticks = 0;
+	comedi_cmd *cmd = &s->async->cmd;
+
+	CALL_PDEBUG("In me4000_ai_do_cmd()\n");
+
+	/* Reset the analog input */
+	err = me4000_ai_cancel(dev, s);
+	if (err)
+		return err;
+
+	/* Round the timer arguments */
+	err = ai_round_cmd_args(dev,
+		s, cmd, &init_ticks, &scan_ticks, &chan_ticks);
+	if (err)
+		return err;
+
+	/* Prepare the AI for acquisition */
+	err = ai_prepare(dev, s, cmd, init_ticks, scan_ticks, chan_ticks);
+	if (err)
+		return err;
+
+	/* Start acquistion by dummy read */
+	me4000_inl(dev, info->ai_context.start_reg);
+
+	return 0;
+}
+
+/*
+ * me4000_ai_do_cmd_test():
+ *
+ * The demo cmd.c in ./comedilib/demo specifies 6 return values:
+ * - success
+ * - invalid source
+ * - source conflict
+ * - invalid argument
+ * - argument conflict
+ * - invalid chanlist
+ * So I tried to adopt this scheme.
+ */
+static int me4000_ai_do_cmd_test(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd)
+{
+
+	unsigned int init_ticks;
+	unsigned int chan_ticks;
+	unsigned int scan_ticks;
+	int err = 0;
+
+	CALL_PDEBUG("In me4000_ai_do_cmd_test()\n");
+
+	PDEBUG("me4000_ai_do_cmd_test(): subdev         = %d\n", cmd->subdev);
+	PDEBUG("me4000_ai_do_cmd_test(): flags          = %08X\n", cmd->flags);
+	PDEBUG("me4000_ai_do_cmd_test(): start_src      = %08X\n",
+		cmd->start_src);
+	PDEBUG("me4000_ai_do_cmd_test(): start_arg      = %d\n",
+		cmd->start_arg);
+	PDEBUG("me4000_ai_do_cmd_test(): scan_begin_src = %08X\n",
+		cmd->scan_begin_src);
+	PDEBUG("me4000_ai_do_cmd_test(): scan_begin_arg = %d\n",
+		cmd->scan_begin_arg);
+	PDEBUG("me4000_ai_do_cmd_test(): convert_src    = %08X\n",
+		cmd->convert_src);
+	PDEBUG("me4000_ai_do_cmd_test(): convert_arg    = %d\n",
+		cmd->convert_arg);
+	PDEBUG("me4000_ai_do_cmd_test(): scan_end_src   = %08X\n",
+		cmd->scan_end_src);
+	PDEBUG("me4000_ai_do_cmd_test(): scan_end_arg   = %d\n",
+		cmd->scan_end_arg);
+	PDEBUG("me4000_ai_do_cmd_test(): stop_src       = %08X\n",
+		cmd->stop_src);
+	PDEBUG("me4000_ai_do_cmd_test(): stop_arg       = %d\n", cmd->stop_arg);
+	PDEBUG("me4000_ai_do_cmd_test(): chanlist       = %d\n",
+		(unsigned int)cmd->chanlist);
+	PDEBUG("me4000_ai_do_cmd_test(): chanlist_len   = %d\n",
+		cmd->chanlist_len);
+
+	/* Only rounding flags are implemented */
+	cmd->flags &= TRIG_ROUND_NEAREST | TRIG_ROUND_UP | TRIG_ROUND_DOWN;
+
+	/* Round the timer arguments */
+	ai_round_cmd_args(dev, s, cmd, &init_ticks, &scan_ticks, &chan_ticks);
+
+	/*
+	 * Stage 1. Check if the trigger sources are generally valid.
+	 */
+	switch (cmd->start_src) {
+	case TRIG_NOW:
+	case TRIG_EXT:
+		break;
+	case TRIG_ANY:
+		cmd->start_src &= TRIG_NOW | TRIG_EXT;
+		err++;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start source\n",
+			dev->minor);
+		cmd->start_src = TRIG_NOW;
+		err++;
+	}
+	switch (cmd->scan_begin_src) {
+	case TRIG_FOLLOW:
+	case TRIG_TIMER:
+	case TRIG_EXT:
+		break;
+	case TRIG_ANY:
+		cmd->scan_begin_src &= TRIG_FOLLOW | TRIG_TIMER | TRIG_EXT;
+		err++;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid scan begin source\n",
+			dev->minor);
+		cmd->scan_begin_src = TRIG_FOLLOW;
+		err++;
+	}
+	switch (cmd->convert_src) {
+	case TRIG_TIMER:
+	case TRIG_EXT:
+		break;
+	case TRIG_ANY:
+		cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
+		err++;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid convert source\n",
+			dev->minor);
+		cmd->convert_src = TRIG_TIMER;
+		err++;
+	}
+	switch (cmd->scan_end_src) {
+	case TRIG_NONE:
+	case TRIG_COUNT:
+		break;
+	case TRIG_ANY:
+		cmd->scan_end_src &= TRIG_NONE | TRIG_COUNT;
+		err++;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid scan end source\n",
+			dev->minor);
+		cmd->scan_end_src = TRIG_NONE;
+		err++;
+	}
+	switch (cmd->stop_src) {
+	case TRIG_NONE:
+	case TRIG_COUNT:
+		break;
+	case TRIG_ANY:
+		cmd->stop_src &= TRIG_NONE | TRIG_COUNT;
+		err++;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid stop source\n",
+			dev->minor);
+		cmd->stop_src = TRIG_NONE;
+		err++;
+	}
+	if (err) {
+		return 1;
+	}
+
+	/*
+	 * Stage 2. Check for trigger source conflicts.
+	 */
+	if (cmd->start_src == TRIG_NOW &&
+		cmd->scan_begin_src == TRIG_TIMER &&
+		cmd->convert_src == TRIG_TIMER) {
+	} else if (cmd->start_src == TRIG_NOW &&
+		cmd->scan_begin_src == TRIG_FOLLOW &&
+		cmd->convert_src == TRIG_TIMER) {
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_TIMER &&
+		cmd->convert_src == TRIG_TIMER) {
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_FOLLOW &&
+		cmd->convert_src == TRIG_TIMER) {
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_EXT &&
+		cmd->convert_src == TRIG_TIMER) {
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_EXT &&
+		cmd->convert_src == TRIG_EXT) {
+	} else {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start trigger combination\n",
+			dev->minor);
+		cmd->start_src = TRIG_NOW;
+		cmd->scan_begin_src = TRIG_FOLLOW;
+		cmd->convert_src = TRIG_TIMER;
+		err++;
+	}
+
+	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 {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid stop trigger combination\n",
+			dev->minor);
+		cmd->stop_src = TRIG_NONE;
+		cmd->scan_end_src = TRIG_NONE;
+		err++;
+	}
+	if (err) {
+		return 2;
+	}
+
+	/*
+	 * Stage 3. Check if arguments are generally valid.
+	 */
+	if (cmd->chanlist_len < 1) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): No channel list\n",
+			dev->minor);
+		cmd->chanlist_len = 1;
+		err++;
+	}
+	if (init_ticks < 66) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Start arg to low\n",
+			dev->minor);
+		cmd->start_arg = 2000;
+		err++;
+	}
+	if (scan_ticks && scan_ticks < 67) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Scan begin arg to low\n",
+			dev->minor);
+		cmd->scan_begin_arg = 2031;
+		err++;
+	}
+	if (chan_ticks < 66) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Convert arg to low\n",
+			dev->minor);
+		cmd->convert_arg = 2000;
+		err++;
+	}
+	if (err) {
+		return 3;
+	}
+
+	/*
+	 * Stage 4. Check for argument conflicts.
+	 */
+	if (cmd->start_src == TRIG_NOW &&
+		cmd->scan_begin_src == TRIG_TIMER &&
+		cmd->convert_src == TRIG_TIMER) {
+
+		/* Check timer arguments */
+		if (init_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start arg\n",
+				dev->minor);
+			cmd->start_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+		if (chan_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid convert arg\n",
+				dev->minor);
+			cmd->convert_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+		if (scan_ticks <= cmd->chanlist_len * chan_ticks) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid scan end arg\n",
+				dev->minor);
+			cmd->scan_end_arg = 2000 * cmd->chanlist_len + 31;	// At least one tick more
+			err++;
+		}
+	} else if (cmd->start_src == TRIG_NOW &&
+		cmd->scan_begin_src == TRIG_FOLLOW &&
+		cmd->convert_src == TRIG_TIMER) {
+
+		/* Check timer arguments */
+		if (init_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start arg\n",
+				dev->minor);
+			cmd->start_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+		if (chan_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid convert arg\n",
+				dev->minor);
+			cmd->convert_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_TIMER &&
+		cmd->convert_src == TRIG_TIMER) {
+
+		/* Check timer arguments */
+		if (init_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start arg\n",
+				dev->minor);
+			cmd->start_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+		if (chan_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid convert arg\n",
+				dev->minor);
+			cmd->convert_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+		if (scan_ticks <= cmd->chanlist_len * chan_ticks) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid scan end arg\n",
+				dev->minor);
+			cmd->scan_end_arg = 2000 * cmd->chanlist_len + 31;	// At least one tick more
+			err++;
+		}
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_FOLLOW &&
+		cmd->convert_src == TRIG_TIMER) {
+
+		/* Check timer arguments */
+		if (init_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start arg\n",
+				dev->minor);
+			cmd->start_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+		if (chan_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid convert arg\n",
+				dev->minor);
+			cmd->convert_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_EXT &&
+		cmd->convert_src == TRIG_TIMER) {
+
+		/* Check timer arguments */
+		if (init_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start arg\n",
+				dev->minor);
+			cmd->start_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+		if (chan_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid convert arg\n",
+				dev->minor);
+			cmd->convert_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_EXT &&
+		cmd->convert_src == TRIG_EXT) {
+
+		/* Check timer arguments */
+		if (init_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start arg\n",
+				dev->minor);
+			cmd->start_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+	}
+	if (cmd->stop_src == TRIG_COUNT) {
+		if (cmd->stop_arg == 0) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid stop arg\n",
+				dev->minor);
+			cmd->stop_arg = 1;
+			err++;
+		}
+	}
+	if (cmd->scan_end_src == TRIG_COUNT) {
+		if (cmd->scan_end_arg == 0) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid scan end arg\n",
+				dev->minor);
+			cmd->scan_end_arg = 1;
+			err++;
+		}
+	}
+	if (err) {
+		return 4;
+	}
+
+	/*
+	 * Stage 5. Check the channel list.
+	 */
+	if (ai_check_chanlist(dev, s, cmd))
+		return 5;
+
+	return 0;
+}
+
+static irqreturn_t me4000_ai_isr(int irq, void *dev_id PT_REGS_ARG)
+{
+	unsigned int tmp;
+	comedi_device *dev = dev_id;
+	comedi_subdevice *s = dev->subdevices;
+	me4000_ai_context_t *ai_context = &info->ai_context;
+	int i;
+	int c = 0;
+	long lval;
+
+	ISR_PDEBUG("me4000_ai_isr() is executed\n");
+
+	if (!dev->attached) {
+		ISR_PDEBUG("me4000_ai_isr() premature interrupt\n");
+		return IRQ_NONE;
+	}
+
+	/* Reset all events */
+	s->async->events = 0;
+
+	/* Check if irq number is right */
+	if (irq != ai_context->irq) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_isr(): Incorrect interrupt num: %d\n",
+			dev->minor, irq);
+		return IRQ_HANDLED;
+	}
+
+	if (me4000_inl(dev,
+			ai_context->
+			irq_status_reg) & ME4000_IRQ_STATUS_BIT_AI_HF) {
+		ISR_PDEBUG
+			("me4000_ai_isr(): Fifo half full interrupt occured\n");
+
+		/* Read status register to find out what happened */
+		tmp = me4000_inl(dev, ai_context->ctrl_reg);
+
+		if (!(tmp & ME4000_AI_STATUS_BIT_FF_DATA) &&
+			!(tmp & ME4000_AI_STATUS_BIT_HF_DATA) &&
+			(tmp & ME4000_AI_STATUS_BIT_EF_DATA)) {
+			ISR_PDEBUG("me4000_ai_isr(): Fifo full\n");
+			c = ME4000_AI_FIFO_COUNT;
+
+			/* FIFO overflow, so stop conversion and disable all interrupts */
+			tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
+			tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ |
+				ME4000_AI_CTRL_BIT_SC_IRQ);
+			me4000_outl(dev, tmp, ai_context->ctrl_reg);
+
+			s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
+
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_isr(): FIFO overflow\n",
+				dev->minor);
+		} else if ((tmp & ME4000_AI_STATUS_BIT_FF_DATA)
+			&& !(tmp & ME4000_AI_STATUS_BIT_HF_DATA)
+			&& (tmp & ME4000_AI_STATUS_BIT_EF_DATA)) {
+			ISR_PDEBUG("me4000_ai_isr(): Fifo half full\n");
+
+			s->async->events |= COMEDI_CB_BLOCK;
+
+			c = ME4000_AI_FIFO_COUNT / 2;
+		} else {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_isr(): Can't determine state of fifo\n",
+				dev->minor);
+			c = 0;
+
+			/* Undefined state, so stop conversion and disable all interrupts */
+			tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
+			tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ |
+				ME4000_AI_CTRL_BIT_SC_IRQ);
+			me4000_outl(dev, tmp, ai_context->ctrl_reg);
+
+			s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
+
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_isr(): Undefined FIFO state\n",
+				dev->minor);
+		}
+
+		ISR_PDEBUG("me4000_ai_isr(): Try to read %d values\n", c);
+
+		for (i = 0; i < c; i++) {
+			/* Read value from data fifo */
+			lval = inl(ai_context->data_reg) & 0xFFFF;
+			lval ^= 0x8000;
+
+			if (!comedi_buf_put(s->async, lval)) {
+				/* Buffer overflow, so stop conversion and disable all interrupts */
+				tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
+				tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ |
+					ME4000_AI_CTRL_BIT_SC_IRQ);
+				me4000_outl(dev, tmp, ai_context->ctrl_reg);
+
+				s->async->events |= COMEDI_CB_OVERFLOW;
+
+				printk(KERN_ERR
+					"comedi%d: me4000: me4000_ai_isr(): Buffer overflow\n",
+					dev->minor);
+
+				break;
+			}
+		}
+
+		/* Work is done, so reset the interrupt */
+		ISR_PDEBUG("me4000_ai_isr(): Reset fifo half full interrupt\n");
+		tmp |= ME4000_AI_CTRL_BIT_HF_IRQ_RESET;
+		me4000_outl(dev, tmp, ai_context->ctrl_reg);
+		tmp &= ~ME4000_AI_CTRL_BIT_HF_IRQ_RESET;
+		me4000_outl(dev, tmp, ai_context->ctrl_reg);
+	}
+
+	if (me4000_inl(dev,
+			ai_context->
+			irq_status_reg) & ME4000_IRQ_STATUS_BIT_SC) {
+		ISR_PDEBUG
+			("me4000_ai_isr(): Sample counter interrupt occured\n");
+
+		s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOA;
+
+		/* Acquisition is complete, so stop conversion and disable all interrupts */
+		tmp = me4000_inl(dev, ai_context->ctrl_reg);
+		tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
+		tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ | ME4000_AI_CTRL_BIT_SC_IRQ);
+		me4000_outl(dev, tmp, ai_context->ctrl_reg);
+
+		/* Poll data until fifo empty */
+		while (inl(ai_context->ctrl_reg) & ME4000_AI_STATUS_BIT_EF_DATA) {
+			/* Read value from data fifo */
+			lval = inl(ai_context->data_reg) & 0xFFFF;
+			lval ^= 0x8000;
+
+			if (!comedi_buf_put(s->async, lval)) {
+				printk(KERN_ERR
+					"comedi%d: me4000: me4000_ai_isr(): Buffer overflow\n",
+					dev->minor);
+				s->async->events |= COMEDI_CB_OVERFLOW;
+				break;
+			}
+		}
+
+		/* Work is done, so reset the interrupt */
+		ISR_PDEBUG
+			("me4000_ai_isr(): Reset interrupt from sample counter\n");
+		tmp |= ME4000_AI_CTRL_BIT_SC_IRQ_RESET;
+		me4000_outl(dev, tmp, ai_context->ctrl_reg);
+		tmp &= ~ME4000_AI_CTRL_BIT_SC_IRQ_RESET;
+		me4000_outl(dev, tmp, ai_context->ctrl_reg);
+	}
+
+	ISR_PDEBUG("me4000_ai_isr(): Events = 0x%X\n", s->async->events);
+
+	if (s->async->events)
+		comedi_event(dev, s);
+
+	return IRQ_HANDLED;
+}
+
+/*=============================================================================
+  Analog output section
+  ===========================================================================*/
+
+static int me4000_ao_insn_write(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+
+	int chan = CR_CHAN(insn->chanspec);
+	int rang = CR_RANGE(insn->chanspec);
+	int aref = CR_AREF(insn->chanspec);
+	unsigned long tmp;
+
+	CALL_PDEBUG("In me4000_ao_insn_write()\n");
+
+	if (insn->n == 0) {
+		return 0;
+	} else if (insn->n > 1) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ao_insn_write(): Invalid instruction length %d\n",
+			dev->minor, insn->n);
+		return -EINVAL;
+	}
+
+	if (chan >= thisboard->ao.count) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ao_insn_write(): Invalid channel %d\n",
+			dev->minor, insn->n);
+		return -EINVAL;
+	}
+
+	if (rang != 0) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ao_insn_write(): Invalid range %d\n",
+			dev->minor, insn->n);
+		return -EINVAL;
+	}
+
+	if (aref != AREF_GROUND && aref != AREF_COMMON) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ao_insn_write(): Invalid aref %d\n",
+			dev->minor, insn->n);
+		return -EINVAL;
+	}
+
+	/* Stop any running conversion */
+	tmp = me4000_inl(dev, info->ao_context[chan].ctrl_reg);
+	tmp |= ME4000_AO_CTRL_BIT_IMMEDIATE_STOP;
+	me4000_outl(dev, tmp, info->ao_context[chan].ctrl_reg);
+
+	/* Clear control register and set to single mode */
+	me4000_outl(dev, 0x0, info->ao_context[chan].ctrl_reg);
+
+	/* Write data value */
+	me4000_outl(dev, data[0], info->ao_context[chan].single_reg);
+
+	/* Store in the mirror */
+	info->ao_context[chan].mirror = data[0];
+
+	return 1;
+}
+
+static int me4000_ao_insn_read(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	int chan = CR_CHAN(insn->chanspec);
+
+	if (insn->n == 0) {
+		return 0;
+	} else if (insn->n > 1) {
+		printk("comedi%d: me4000: me4000_ao_insn_read(): Invalid instruction length\n", dev->minor);
+		return -EINVAL;
+	}
+
+	data[0] = info->ao_context[chan].mirror;
+
+	return 1;
+}
+
+/*=============================================================================
+  Digital I/O section
+  ===========================================================================*/
+
+static int me4000_dio_insn_bits(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+
+	CALL_PDEBUG("In me4000_dio_insn_bits()\n");
+
+	/* Length of data must be 2 (mask and new data, see below) */
+	if (insn->n == 0) {
+		return 0;
+	}
+	if (insn->n != 2) {
+		printk("comedi%d: me4000: me4000_dio_insn_bits(): Invalid instruction length\n", dev->minor);
+		return -EINVAL;
+	}
+
+	/*
+	 * The insn data consists of a mask in data[0] and the new data
+	 * in data[1]. The mask defines which bits we are concerning about.
+	 * The new data must be anded with the mask.
+	 * Each channel corresponds to a bit.
+	 */
+	if (data[0]) {
+		/* Check if requested ports are configured for output */
+		if ((s->io_bits & data[0]) != data[0])
+			return -EIO;
+
+		s->state &= ~data[0];
+		s->state |= data[0] & data[1];
+
+		/* Write out the new digital output lines */
+		me4000_outl(dev, (s->state >> 0) & 0xFF,
+			info->dio_context.port_0_reg);
+		me4000_outl(dev, (s->state >> 8) & 0xFF,
+			info->dio_context.port_1_reg);
+		me4000_outl(dev, (s->state >> 16) & 0xFF,
+			info->dio_context.port_2_reg);
+		me4000_outl(dev, (s->state >> 24) & 0xFF,
+			info->dio_context.port_3_reg);
+	}
+
+	/* On return, data[1] contains the value of
+	   the digital input and output lines. */
+	data[1] =
+		((me4000_inl(dev, info->dio_context.port_0_reg) & 0xFF) << 0) |
+		((me4000_inl(dev, info->dio_context.port_1_reg) & 0xFF) << 8) |
+		((me4000_inl(dev, info->dio_context.port_2_reg) & 0xFF) << 16) |
+		((me4000_inl(dev, info->dio_context.port_3_reg) & 0xFF) << 24);
+
+	return 2;
+}
+
+static int me4000_dio_insn_config(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	unsigned long tmp;
+	int chan = CR_CHAN(insn->chanspec);
+
+	CALL_PDEBUG("In me4000_dio_insn_config()\n");
+
+	if (data[0] == INSN_CONFIG_DIO_QUERY) {
+		data[1] =
+			(s->
+			io_bits & (1 << chan)) ? COMEDI_OUTPUT : COMEDI_INPUT;
+		return insn->n;
+	}
+
+	/*
+	 * The input or output configuration of each digital line is
+	 * configured by a special insn_config instruction.  chanspec
+	 * contains the channel to be changed, and data[0] contains the
+	 * value COMEDI_INPUT or COMEDI_OUTPUT.
+	 * On the ME-4000 it is only possible to switch port wise (8 bit)
+	 */
+
+	tmp = me4000_inl(dev, info->dio_context.ctrl_reg);
+
+	if (data[0] == COMEDI_OUTPUT) {
+		if (chan < 8) {
+			s->io_bits |= 0xFF;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_0 |
+				ME4000_DIO_CTRL_BIT_MODE_1);
+			tmp |= ME4000_DIO_CTRL_BIT_MODE_0;
+		} else if (chan < 16) {
+			/*
+			 * Chech for optoisolated ME-4000 version. If one the first
+			 * port is a fixed output port and the second is a fixed input port.
+			 */
+			if (!me4000_inl(dev, info->dio_context.dir_reg))
+				return -ENODEV;
+
+			s->io_bits |= 0xFF00;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_2 |
+				ME4000_DIO_CTRL_BIT_MODE_3);
+			tmp |= ME4000_DIO_CTRL_BIT_MODE_2;
+		} else if (chan < 24) {
+			s->io_bits |= 0xFF0000;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_4 |
+				ME4000_DIO_CTRL_BIT_MODE_5);
+			tmp |= ME4000_DIO_CTRL_BIT_MODE_4;
+		} else if (chan < 32) {
+			s->io_bits |= 0xFF000000;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_6 |
+				ME4000_DIO_CTRL_BIT_MODE_7);
+			tmp |= ME4000_DIO_CTRL_BIT_MODE_6;
+		} else {
+			return -EINVAL;
+		}
+	} else {
+		if (chan < 8) {
+			/*
+			 * Chech for optoisolated ME-4000 version. If one the first
+			 * port is a fixed output port and the second is a fixed input port.
+			 */
+			if (!me4000_inl(dev, info->dio_context.dir_reg))
+				return -ENODEV;
+
+			s->io_bits &= ~0xFF;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_0 |
+				ME4000_DIO_CTRL_BIT_MODE_1);
+		} else if (chan < 16) {
+			s->io_bits &= ~0xFF00;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_2 |
+				ME4000_DIO_CTRL_BIT_MODE_3);
+		} else if (chan < 24) {
+			s->io_bits &= ~0xFF0000;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_4 |
+				ME4000_DIO_CTRL_BIT_MODE_5);
+		} else if (chan < 32) {
+			s->io_bits &= ~0xFF000000;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_6 |
+				ME4000_DIO_CTRL_BIT_MODE_7);
+		} else {
+			return -EINVAL;
+		}
+	}
+
+	me4000_outl(dev, tmp, info->dio_context.ctrl_reg);
+
+	return 1;
+}
+
+/*=============================================================================
+  Counter section
+  ===========================================================================*/
+
+static int cnt_reset(comedi_device * dev, unsigned int channel)
+{
+
+	CALL_PDEBUG("In cnt_reset()\n");
+
+	switch (channel) {
+	case 0:
+		me4000_outb(dev, 0x30, info->cnt_context.ctrl_reg);
+		me4000_outb(dev, 0x00, info->cnt_context.counter_0_reg);
+		me4000_outb(dev, 0x00, info->cnt_context.counter_0_reg);
+		break;
+	case 1:
+		me4000_outb(dev, 0x70, info->cnt_context.ctrl_reg);
+		me4000_outb(dev, 0x00, info->cnt_context.counter_1_reg);
+		me4000_outb(dev, 0x00, info->cnt_context.counter_1_reg);
+		break;
+	case 2:
+		me4000_outb(dev, 0xB0, info->cnt_context.ctrl_reg);
+		me4000_outb(dev, 0x00, info->cnt_context.counter_2_reg);
+		me4000_outb(dev, 0x00, info->cnt_context.counter_2_reg);
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: cnt_reset(): Invalid channel\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int cnt_config(comedi_device * dev, unsigned int channel,
+	unsigned int mode)
+{
+	int tmp = 0;
+
+	CALL_PDEBUG("In cnt_config()\n");
+
+	switch (channel) {
+	case 0:
+		tmp |= ME4000_CNT_COUNTER_0;
+		break;
+	case 1:
+		tmp |= ME4000_CNT_COUNTER_1;
+		break;
+	case 2:
+		tmp |= ME4000_CNT_COUNTER_2;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: cnt_config(): Invalid channel\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	switch (mode) {
+	case 0:
+		tmp |= ME4000_CNT_MODE_0;
+		break;
+	case 1:
+		tmp |= ME4000_CNT_MODE_1;
+		break;
+	case 2:
+		tmp |= ME4000_CNT_MODE_2;
+		break;
+	case 3:
+		tmp |= ME4000_CNT_MODE_3;
+		break;
+	case 4:
+		tmp |= ME4000_CNT_MODE_4;
+		break;
+	case 5:
+		tmp |= ME4000_CNT_MODE_5;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: cnt_config(): Invalid counter mode\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	/* Write the control word */
+	tmp |= 0x30;
+	me4000_outb(dev, tmp, info->cnt_context.ctrl_reg);
+
+	return 0;
+}
+
+static int me4000_cnt_insn_config(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+
+	int err;
+
+	CALL_PDEBUG("In me4000_cnt_insn_config()\n");
+
+	switch (data[0]) {
+	case GPCT_RESET:
+		if (insn->n != 1) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_cnt_insn_config(): Invalid instruction length%d\n",
+				dev->minor, insn->n);
+			return -EINVAL;
+		}
+
+		err = cnt_reset(dev, insn->chanspec);
+		if (err)
+			return err;
+		break;
+	case GPCT_SET_OPERATION:
+		if (insn->n != 2) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_cnt_insn_config(): Invalid instruction length%d\n",
+				dev->minor, insn->n);
+			return -EINVAL;
+		}
+
+		err = cnt_config(dev, insn->chanspec, data[1]);
+		if (err)
+			return err;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_cnt_insn_config(): Invalid instruction\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	return 2;
+}
+
+static int me4000_cnt_insn_read(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+
+	unsigned short tmp;
+
+	CALL_PDEBUG("In me4000_cnt_insn_read()\n");
+
+	if (insn->n == 0) {
+		return 0;
+	}
+	if (insn->n > 1) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_cnt_insn_read(): Invalid instruction length %d\n",
+			dev->minor, insn->n);
+		return -EINVAL;
+	}
+
+	switch (insn->chanspec) {
+	case 0:
+		tmp = me4000_inb(dev, info->cnt_context.counter_0_reg);
+		data[0] = tmp;
+		tmp = me4000_inb(dev, info->cnt_context.counter_0_reg);
+		data[0] |= tmp << 8;
+		break;
+	case 1:
+		tmp = me4000_inb(dev, info->cnt_context.counter_1_reg);
+		data[0] = tmp;
+		tmp = me4000_inb(dev, info->cnt_context.counter_1_reg);
+		data[0] |= tmp << 8;
+		break;
+	case 2:
+		tmp = me4000_inb(dev, info->cnt_context.counter_2_reg);
+		data[0] = tmp;
+		tmp = me4000_inb(dev, info->cnt_context.counter_2_reg);
+		data[0] |= tmp << 8;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_cnt_insn_read(): Invalid channel %d\n",
+			dev->minor, insn->chanspec);
+		return -EINVAL;
+	}
+
+	return 1;
+}
+
+static int me4000_cnt_insn_write(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+
+	unsigned short tmp;
+
+	CALL_PDEBUG("In me4000_cnt_insn_write()\n");
+
+	if (insn->n == 0) {
+		return 0;
+	} else if (insn->n > 1) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_cnt_insn_write(): Invalid instruction length %d\n",
+			dev->minor, insn->n);
+		return -EINVAL;
+	}
+
+	switch (insn->chanspec) {
+	case 0:
+		tmp = data[0] & 0xFF;
+		me4000_outb(dev, tmp, info->cnt_context.counter_0_reg);
+		tmp = (data[0] >> 8) & 0xFF;
+		me4000_outb(dev, tmp, info->cnt_context.counter_0_reg);
+		break;
+	case 1:
+		tmp = data[0] & 0xFF;
+		me4000_outb(dev, tmp, info->cnt_context.counter_1_reg);
+		tmp = (data[0] >> 8) & 0xFF;
+		me4000_outb(dev, tmp, info->cnt_context.counter_1_reg);
+		break;
+	case 2:
+		tmp = data[0] & 0xFF;
+		me4000_outb(dev, tmp, info->cnt_context.counter_2_reg);
+		tmp = (data[0] >> 8) & 0xFF;
+		me4000_outb(dev, tmp, info->cnt_context.counter_2_reg);
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_cnt_insn_write(): Invalid channel %d\n",
+			dev->minor, insn->chanspec);
+		return -EINVAL;
+	}
+
+	return 1;
+}
+
+COMEDI_PCI_INITCLEANUP(driver_me4000, me4000_pci_table);
diff --git a/drivers/staging/comedi/drivers/me4000.h b/drivers/staging/comedi/drivers/me4000.h
new file mode 100644
index 000000000000..f12b8873ec3c
--- /dev/null
+++ b/drivers/staging/comedi/drivers/me4000.h
@@ -0,0 +1,446 @@
+/*
+    me4000.h
+    Register descriptions and defines for the ME-4000 board family
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1998-9 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _ME4000_H_
+#define _ME4000_H_
+
+/*=============================================================================
+  Debug section
+  ===========================================================================*/
+
+#undef ME4000_CALL_DEBUG	// Debug function entry and exit
+#undef ME4000_PORT_DEBUG	// Debug port access
+#undef ME4000_ISR_DEBUG		// Debug the interrupt service routine
+#undef ME4000_DEBUG		// General purpose debug masseges
+
+#ifdef ME4000_CALL_DEBUG
+#undef CALL_PDEBUG
+#define CALL_PDEBUG(fmt, args...) printk(KERN_DEBUG"comedi%d: me4000: " fmt, dev->minor, ##args)
+#else
+# define CALL_PDEBUG(fmt, args...)	// no debugging, do nothing
+#endif
+
+#ifdef ME4000_PORT_DEBUG
+#undef PORT_PDEBUG
+#define PORT_PDEBUG(fmt, args...) printk(KERN_DEBUG"comedi%d: me4000: " fmt, dev->minor,  ##args)
+#else
+#define PORT_PDEBUG(fmt, args...)	// no debugging, do nothing
+#endif
+
+#ifdef ME4000_ISR_DEBUG
+#undef ISR_PDEBUG
+#define ISR_PDEBUG(fmt, args...) printk(KERN_DEBUG"comedi%d: me4000: " fmt, dev->minor,  ##args)
+#else
+#define ISR_PDEBUG(fmt, args...)	// no debugging, do nothing
+#endif
+
+#ifdef ME4000_DEBUG
+#undef PDEBUG
+#define PDEBUG(fmt, args...) printk(KERN_DEBUG"comedi%d: me4000: " fmt, dev->minor,  ##args)
+#else
+#define PDEBUG(fmt, args...)	// no debugging, do nothing
+#endif
+
+/*=============================================================================
+  PCI vendor and device IDs
+  ===========================================================================*/
+
+#define PCI_VENDOR_ID_MEILHAUS 0x1402
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4650	0x4650	// Low Cost version
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4660	0x4660	// Standard version
+#define PCI_DEVICE_ID_MEILHAUS_ME4660I	0x4661	// Isolated version
+#define PCI_DEVICE_ID_MEILHAUS_ME4660S	0x4662	// Standard version with Sample and Hold
+#define PCI_DEVICE_ID_MEILHAUS_ME4660IS	0x4663	// Isolated version with Sample and Hold
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4670	0x4670	// Standard version
+#define PCI_DEVICE_ID_MEILHAUS_ME4670I	0x4671	// Isolated version
+#define PCI_DEVICE_ID_MEILHAUS_ME4670S	0x4672	// Standard version with Sample and Hold
+#define PCI_DEVICE_ID_MEILHAUS_ME4670IS	0x4673	// Isolated version with Sample and Hold
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4680	0x4680	// Standard version
+#define PCI_DEVICE_ID_MEILHAUS_ME4680I	0x4681	// Isolated version
+#define PCI_DEVICE_ID_MEILHAUS_ME4680S	0x4682	// Standard version with Sample and Hold
+#define PCI_DEVICE_ID_MEILHAUS_ME4680IS	0x4683	// Isolated version with Sample and Hold
+
+/*=============================================================================
+  ME-4000 base register offsets
+  ===========================================================================*/
+
+#define ME4000_AO_00_CTRL_REG			0x00	// R/W
+#define ME4000_AO_00_STATUS_REG			0x04	// R/_
+#define ME4000_AO_00_FIFO_REG			0x08	// _/W
+#define ME4000_AO_00_SINGLE_REG			0x0C	// R/W
+#define ME4000_AO_00_TIMER_REG			0x10	// _/W
+
+#define ME4000_AO_01_CTRL_REG			0x18	// R/W
+#define ME4000_AO_01_STATUS_REG			0x1C	// R/_
+#define ME4000_AO_01_FIFO_REG			0x20	// _/W
+#define ME4000_AO_01_SINGLE_REG			0x24	// R/W
+#define ME4000_AO_01_TIMER_REG			0x28	// _/W
+
+#define ME4000_AO_02_CTRL_REG			0x30	// R/W
+#define ME4000_AO_02_STATUS_REG			0x34	// R/_
+#define ME4000_AO_02_FIFO_REG			0x38	// _/W
+#define ME4000_AO_02_SINGLE_REG			0x3C	// R/W
+#define ME4000_AO_02_TIMER_REG			0x40	// _/W
+
+#define ME4000_AO_03_CTRL_REG			0x48	// R/W
+#define ME4000_AO_03_STATUS_REG			0x4C	// R/_
+#define ME4000_AO_03_FIFO_REG			0x50	// _/W
+#define ME4000_AO_03_SINGLE_REG			0x54	// R/W
+#define ME4000_AO_03_TIMER_REG			0x58	// _/W
+
+#define ME4000_AI_CTRL_REG			0x74	// _/W
+#define ME4000_AI_STATUS_REG			0x74	// R/_
+#define ME4000_AI_CHANNEL_LIST_REG		0x78	// _/W
+#define ME4000_AI_DATA_REG			0x7C	// R/_
+#define ME4000_AI_CHAN_TIMER_REG		0x80	// _/W
+#define ME4000_AI_CHAN_PRE_TIMER_REG		0x84	// _/W
+#define ME4000_AI_SCAN_TIMER_LOW_REG		0x88	// _/W
+#define ME4000_AI_SCAN_TIMER_HIGH_REG		0x8C	// _/W
+#define ME4000_AI_SCAN_PRE_TIMER_LOW_REG	0x90	// _/W
+#define ME4000_AI_SCAN_PRE_TIMER_HIGH_REG	0x94	// _/W
+#define ME4000_AI_START_REG			0x98	// R/_
+
+#define ME4000_IRQ_STATUS_REG			0x9C	// R/_
+
+#define ME4000_DIO_PORT_0_REG			0xA0	// R/W
+#define ME4000_DIO_PORT_1_REG			0xA4	// R/W
+#define ME4000_DIO_PORT_2_REG			0xA8	// R/W
+#define ME4000_DIO_PORT_3_REG			0xAC	// R/W
+#define ME4000_DIO_DIR_REG			0xB0	// R/W
+
+#define ME4000_AO_LOADSETREG_XX			0xB4	// R/W
+
+#define ME4000_DIO_CTRL_REG			0xB8	// R/W
+
+#define ME4000_AO_DEMUX_ADJUST_REG		0xBC	// -/W
+
+#define ME4000_AI_SAMPLE_COUNTER_REG		0xC0	// _/W
+
+/*=============================================================================
+  Value to adjust Demux
+  ===========================================================================*/
+
+#define ME4000_AO_DEMUX_ADJUST_VALUE            0x4C
+
+/*=============================================================================
+  Counter base register offsets
+  ===========================================================================*/
+
+#define ME4000_CNT_COUNTER_0_REG		0x00
+#define ME4000_CNT_COUNTER_1_REG		0x01
+#define ME4000_CNT_COUNTER_2_REG		0x02
+#define ME4000_CNT_CTRL_REG			0x03
+
+/*=============================================================================
+  PLX base register offsets
+  ===========================================================================*/
+
+#define PLX_INTCSR	0x4C	// Interrupt control and status register
+#define PLX_ICR		0x50	// Initialization control register
+
+/*=============================================================================
+  Bits for the PLX_ICSR register
+  ===========================================================================*/
+
+#define PLX_INTCSR_LOCAL_INT1_EN             0x01	// If set, local interrupt 1 is enabled (r/w)
+#define PLX_INTCSR_LOCAL_INT1_POL            0x02	// If set, local interrupt 1 polarity is active high (r/w)
+#define PLX_INTCSR_LOCAL_INT1_STATE          0x04	// If set, local interrupt 1 is active (r/_)
+#define PLX_INTCSR_LOCAL_INT2_EN             0x08	// If set, local interrupt 2 is enabled (r/w)
+#define PLX_INTCSR_LOCAL_INT2_POL            0x10	// If set, local interrupt 2 polarity is active high (r/w)
+#define PLX_INTCSR_LOCAL_INT2_STATE          0x20	// If set, local interrupt 2 is active  (r/_)
+#define PLX_INTCSR_PCI_INT_EN                0x40	// If set, PCI interrupt is enabled (r/w)
+#define PLX_INTCSR_SOFT_INT                  0x80	// If set, a software interrupt is generated (r/w)
+
+/*=============================================================================
+  Bits for the PLX_ICR register
+  ===========================================================================*/
+
+#define PLX_ICR_BIT_EEPROM_CLOCK_SET		0x01000000
+#define PLX_ICR_BIT_EEPROM_CHIP_SELECT		0x02000000
+#define PLX_ICR_BIT_EEPROM_WRITE		0x04000000
+#define PLX_ICR_BIT_EEPROM_READ			0x08000000
+#define PLX_ICR_BIT_EEPROM_VALID		0x10000000
+
+#define PLX_ICR_MASK_EEPROM			0x1F000000
+
+#define EEPROM_DELAY				1
+
+/*=============================================================================
+  Bits for the ME4000_AO_CTRL_REG register
+  ===========================================================================*/
+
+#define ME4000_AO_CTRL_BIT_MODE_0		0x001
+#define ME4000_AO_CTRL_BIT_MODE_1		0x002
+#define ME4000_AO_CTRL_MASK_MODE		0x003
+#define ME4000_AO_CTRL_BIT_STOP			0x004
+#define ME4000_AO_CTRL_BIT_ENABLE_FIFO		0x008
+#define ME4000_AO_CTRL_BIT_ENABLE_EX_TRIG	0x010
+#define ME4000_AO_CTRL_BIT_EX_TRIG_EDGE		0x020
+#define ME4000_AO_CTRL_BIT_IMMEDIATE_STOP	0x080
+#define ME4000_AO_CTRL_BIT_ENABLE_DO		0x100
+#define ME4000_AO_CTRL_BIT_ENABLE_IRQ		0x200
+#define ME4000_AO_CTRL_BIT_RESET_IRQ		0x400
+
+/*=============================================================================
+  Bits for the ME4000_AO_STATUS_REG register
+  ===========================================================================*/
+
+#define ME4000_AO_STATUS_BIT_FSM		0x01
+#define ME4000_AO_STATUS_BIT_FF			0x02
+#define ME4000_AO_STATUS_BIT_HF			0x04
+#define ME4000_AO_STATUS_BIT_EF			0x08
+
+/*=============================================================================
+  Bits for the ME4000_AI_CTRL_REG register
+  ===========================================================================*/
+
+#define ME4000_AI_CTRL_BIT_MODE_0		0x00000001
+#define ME4000_AI_CTRL_BIT_MODE_1		0x00000002
+#define ME4000_AI_CTRL_BIT_MODE_2		0x00000004
+#define ME4000_AI_CTRL_BIT_SAMPLE_HOLD		0x00000008
+#define ME4000_AI_CTRL_BIT_IMMEDIATE_STOP	0x00000010
+#define ME4000_AI_CTRL_BIT_STOP			0x00000020
+#define ME4000_AI_CTRL_BIT_CHANNEL_FIFO		0x00000040
+#define ME4000_AI_CTRL_BIT_DATA_FIFO		0x00000080
+#define ME4000_AI_CTRL_BIT_FULLSCALE		0x00000100
+#define ME4000_AI_CTRL_BIT_OFFSET		0x00000200
+#define ME4000_AI_CTRL_BIT_EX_TRIG_ANALOG	0x00000400
+#define ME4000_AI_CTRL_BIT_EX_TRIG		0x00000800
+#define ME4000_AI_CTRL_BIT_EX_TRIG_FALLING	0x00001000
+#define ME4000_AI_CTRL_BIT_EX_IRQ		0x00002000
+#define ME4000_AI_CTRL_BIT_EX_IRQ_RESET		0x00004000
+#define ME4000_AI_CTRL_BIT_LE_IRQ		0x00008000
+#define ME4000_AI_CTRL_BIT_LE_IRQ_RESET		0x00010000
+#define ME4000_AI_CTRL_BIT_HF_IRQ		0x00020000
+#define ME4000_AI_CTRL_BIT_HF_IRQ_RESET		0x00040000
+#define ME4000_AI_CTRL_BIT_SC_IRQ		0x00080000
+#define ME4000_AI_CTRL_BIT_SC_IRQ_RESET		0x00100000
+#define ME4000_AI_CTRL_BIT_SC_RELOAD		0x00200000
+#define ME4000_AI_CTRL_BIT_EX_TRIG_BOTH		0x80000000
+
+/*=============================================================================
+  Bits for the ME4000_AI_STATUS_REG register
+  ===========================================================================*/
+
+#define ME4000_AI_STATUS_BIT_EF_CHANNEL		0x00400000
+#define ME4000_AI_STATUS_BIT_HF_CHANNEL		0x00800000
+#define ME4000_AI_STATUS_BIT_FF_CHANNEL		0x01000000
+#define ME4000_AI_STATUS_BIT_EF_DATA		0x02000000
+#define ME4000_AI_STATUS_BIT_HF_DATA		0x04000000
+#define ME4000_AI_STATUS_BIT_FF_DATA		0x08000000
+#define ME4000_AI_STATUS_BIT_LE			0x10000000
+#define ME4000_AI_STATUS_BIT_FSM		0x20000000
+
+/*=============================================================================
+  Bits for the ME4000_IRQ_STATUS_REG register
+  ===========================================================================*/
+
+#define ME4000_IRQ_STATUS_BIT_EX		0x01
+#define ME4000_IRQ_STATUS_BIT_LE		0x02
+#define ME4000_IRQ_STATUS_BIT_AI_HF		0x04
+#define ME4000_IRQ_STATUS_BIT_AO_0_HF		0x08
+#define ME4000_IRQ_STATUS_BIT_AO_1_HF		0x10
+#define ME4000_IRQ_STATUS_BIT_AO_2_HF		0x20
+#define ME4000_IRQ_STATUS_BIT_AO_3_HF		0x40
+#define ME4000_IRQ_STATUS_BIT_SC		0x80
+
+/*=============================================================================
+  Bits for the ME4000_DIO_CTRL_REG register
+  ===========================================================================*/
+
+#define ME4000_DIO_CTRL_BIT_MODE_0		0x0001
+#define ME4000_DIO_CTRL_BIT_MODE_1		0x0002
+#define ME4000_DIO_CTRL_BIT_MODE_2		0x0004
+#define ME4000_DIO_CTRL_BIT_MODE_3		0x0008
+#define ME4000_DIO_CTRL_BIT_MODE_4		0x0010
+#define ME4000_DIO_CTRL_BIT_MODE_5		0x0020
+#define ME4000_DIO_CTRL_BIT_MODE_6		0x0040
+#define ME4000_DIO_CTRL_BIT_MODE_7		0x0080
+
+#define ME4000_DIO_CTRL_BIT_FUNCTION_0		0x0100
+#define ME4000_DIO_CTRL_BIT_FUNCTION_1		0x0200
+
+#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_0		0x0400
+#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_1		0x0800
+#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_2		0x1000
+#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_3		0x2000
+
+/*=============================================================================
+  Information about the hardware capabilities
+  ===========================================================================*/
+
+typedef struct me4000_ao_info {
+	int count;
+	int fifo_count;
+} me4000_ao_info_t;
+
+typedef struct me4000_ai_info {
+	int count;
+	int sh_count;
+	int diff_count;
+	int ex_trig_analog;
+} me4000_ai_info_t;
+
+typedef struct me4000_dio_info {
+	int count;
+} me4000_dio_info_t;
+
+typedef struct me4000_cnt_info {
+	int count;
+} me4000_cnt_info_t;
+
+typedef struct me4000_board {
+	const char *name;
+	unsigned short device_id;
+	me4000_ao_info_t ao;
+	me4000_ai_info_t ai;
+	me4000_dio_info_t dio;
+	me4000_cnt_info_t cnt;
+} me4000_board_t;
+
+#define thisboard ((const me4000_board_t *)dev->board_ptr)
+
+/*=============================================================================
+  Global board and subdevice information structures
+  ===========================================================================*/
+
+typedef struct me4000_ao_context {
+	int irq;
+
+	unsigned long mirror;	// Store the last written value
+
+	unsigned long ctrl_reg;
+	unsigned long status_reg;
+	unsigned long fifo_reg;
+	unsigned long single_reg;
+	unsigned long timer_reg;
+	unsigned long irq_status_reg;
+	unsigned long preload_reg;
+} me4000_ao_context_t;
+
+typedef struct me4000_ai_context {
+	int irq;
+
+	unsigned long ctrl_reg;
+	unsigned long status_reg;
+	unsigned long channel_list_reg;
+	unsigned long data_reg;
+	unsigned long chan_timer_reg;
+	unsigned long chan_pre_timer_reg;
+	unsigned long scan_timer_low_reg;
+	unsigned long scan_timer_high_reg;
+	unsigned long scan_pre_timer_low_reg;
+	unsigned long scan_pre_timer_high_reg;
+	unsigned long start_reg;
+	unsigned long irq_status_reg;
+	unsigned long sample_counter_reg;
+} me4000_ai_context_t;
+
+typedef struct me4000_dio_context {
+	unsigned long dir_reg;
+	unsigned long ctrl_reg;
+	unsigned long port_0_reg;
+	unsigned long port_1_reg;
+	unsigned long port_2_reg;
+	unsigned long port_3_reg;
+} me4000_dio_context_t;
+
+typedef struct me4000_cnt_context {
+	unsigned long ctrl_reg;
+	unsigned long counter_0_reg;
+	unsigned long counter_1_reg;
+	unsigned long counter_2_reg;
+} me4000_cnt_context_t;
+
+typedef struct me4000_info {
+	unsigned long plx_regbase;	// PLX configuration space base address
+	unsigned long me4000_regbase;	// Base address of the ME4000
+	unsigned long timer_regbase;	// Base address of the timer circuit
+	unsigned long program_regbase;	// Base address to set the program pin for the xilinx
+
+	unsigned long plx_regbase_size;	// PLX register set space
+	unsigned long me4000_regbase_size;	// ME4000 register set space
+	unsigned long timer_regbase_size;	// Timer circuit register set space
+	unsigned long program_regbase_size;	// Size of program base address of the ME4000
+
+	unsigned int serial_no;	// Serial number of the board
+	unsigned char hw_revision;	// Hardware revision of the board
+	unsigned short vendor_id;	// Meilhaus vendor id
+	unsigned short device_id;	// Device id
+
+	struct pci_dev *pci_dev_p;	// General PCI information
+
+	unsigned int irq;	// IRQ assigned from the PCI BIOS
+
+	struct me4000_ai_context ai_context;	// Analog input  specific context
+	struct me4000_ao_context ao_context[4];	// Vector with analog output specific context
+	struct me4000_dio_context dio_context;	// Digital I/O specific context
+	struct me4000_cnt_context cnt_context;	// Counter specific context
+} me4000_info_t;
+
+#define info	((me4000_info_t *)dev->private)
+
+/*-----------------------------------------------------------------------------
+  Defines for analog input
+ ----------------------------------------------------------------------------*/
+
+/* General stuff */
+#define ME4000_AI_FIFO_COUNT			2048
+
+#define ME4000_AI_MIN_TICKS			66
+#define ME4000_AI_MIN_SAMPLE_TIME		2000	// Minimum sample time [ns]
+#define ME4000_AI_BASE_FREQUENCY		(unsigned int) 33E6
+
+/* Channel list defines and masks */
+#define ME4000_AI_CHANNEL_LIST_COUNT		1024
+
+#define ME4000_AI_LIST_INPUT_SINGLE_ENDED	0x000
+#define ME4000_AI_LIST_INPUT_DIFFERENTIAL	0x020
+
+#define ME4000_AI_LIST_RANGE_BIPOLAR_10		0x000
+#define ME4000_AI_LIST_RANGE_BIPOLAR_2_5	0x040
+#define ME4000_AI_LIST_RANGE_UNIPOLAR_10	0x080
+#define ME4000_AI_LIST_RANGE_UNIPOLAR_2_5	0x0C0
+
+#define ME4000_AI_LIST_LAST_ENTRY		0x100
+
+/*-----------------------------------------------------------------------------
+  Defines for counters
+ ----------------------------------------------------------------------------*/
+
+#define ME4000_CNT_COUNTER_0  0x00
+#define ME4000_CNT_COUNTER_1  0x40
+#define ME4000_CNT_COUNTER_2  0x80
+
+#define ME4000_CNT_MODE_0     0x00	// Change state if zero crossing
+#define ME4000_CNT_MODE_1     0x02	// Retriggerable One-Shot
+#define ME4000_CNT_MODE_2     0x04	// Asymmetrical divider
+#define ME4000_CNT_MODE_3     0x06	// Symmetrical divider
+#define ME4000_CNT_MODE_4     0x08	// Counter start by software trigger
+#define ME4000_CNT_MODE_5     0x0A	// Counter start by hardware trigger
+
+#endif
diff --git a/drivers/staging/comedi/drivers/me_daq.c b/drivers/staging/comedi/drivers/me_daq.c
new file mode 100644
index 000000000000..6accec20a0f9
--- /dev/null
+++ b/drivers/staging/comedi/drivers/me_daq.c
@@ -0,0 +1,845 @@
+/*
+
+   comedi/drivers/me_daq.c
+
+   Hardware driver for Meilhaus data acquisition cards:
+
+     ME-2000i, ME-2600i, ME-3000vm1
+
+   Copyright (C) 2002 Michael Hillmann <hillmann@syscongroup.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., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+
+/*
+Driver: me_daq
+Description: Meilhaus PCI data acquisition cards
+Author: Michael Hillmann <hillmann@syscongroup.de>
+Devices: [Meilhaus] ME-2600i (me_daq), ME-2000i
+Status: experimental
+
+Supports:
+
+    Analog Output
+
+Configuration options:
+
+    [0] - PCI bus number (optional)
+    [1] - PCI slot number (optional)
+
+    If bus/slot is not specified, the first available PCI
+    device will be used.
+
+The 2600 requires a firmware upload, which can be accomplished
+using the -i or --init-data option of comedi_config.
+The firmware can be
+found in the comedi_nonfree_firmware tarball available
+from http://www.comedi.org
+
+*/
+
+#include "../comedidev.h"
+
+#include "comedi_pci.h"
+
+/*#include "me2600_fw.h" */
+
+#define ME_DRIVER_NAME		"me_daq"
+
+#define ME2000_DEVICE_ID	0x2000
+#define ME2600_DEVICE_ID	0x2600
+
+#define PLX_INTCSR		0x4C	/* PLX interrupt status register */
+#define XILINX_DOWNLOAD_RESET	0x42	/* Xilinx registers */
+
+#define ME_CONTROL_1			0x0000	/* - | W */
+#define   INTERRUPT_ENABLE		(1<<15)
+#define   COUNTER_B_IRQ			(1<<12)
+#define   COUNTER_A_IRQ			(1<<11)
+#define   CHANLIST_READY_IRQ		(1<<10)
+#define   EXT_IRQ			(1<<9)
+#define   ADFIFO_HALFFULL_IRQ		(1<<8)
+#define   SCAN_COUNT_ENABLE		(1<<5)
+#define   SIMULTANEOUS_ENABLE		(1<<4)
+#define   TRIGGER_FALLING_EDGE		(1<<3)
+#define   CONTINUOUS_MODE		(1<<2)
+#define   DISABLE_ADC			(0<<0)
+#define   SOFTWARE_TRIGGERED_ADC	(1<<0)
+#define   SCAN_TRIGGERED_ADC		(2<<0)
+#define   EXT_TRIGGERED_ADC		(3<<0)
+#define ME_ADC_START			0x0000	/* R | - */
+#define ME_CONTROL_2			0x0002	/* - | W */
+#define   ENABLE_ADFIFO			(1<<10)
+#define   ENABLE_CHANLIST		(1<<9)
+#define   ENABLE_PORT_B			(1<<7)
+#define   ENABLE_PORT_A			(1<<6)
+#define   ENABLE_COUNTER_B		(1<<4)
+#define   ENABLE_COUNTER_A		(1<<3)
+#define   ENABLE_DAC			(1<<1)
+#define   BUFFERED_DAC			(1<<0)
+#define ME_DAC_UPDATE			0x0002	/* R | - */
+#define ME_STATUS			0x0004	/* R | - */
+#define   COUNTER_B_IRQ_PENDING		(1<<12)
+#define   COUNTER_A_IRQ_PENDING		(1<<11)
+#define   CHANLIST_READY_IRQ_PENDING	(1<<10)
+#define   EXT_IRQ_PENDING		(1<<9)
+#define   ADFIFO_HALFFULL_IRQ_PENDING	(1<<8)
+#define   ADFIFO_FULL			(1<<4)
+#define   ADFIFO_HALFFULL		(1<<3)
+#define   ADFIFO_EMPTY			(1<<2)
+#define   CHANLIST_FULL			(1<<1)
+#define   FST_ACTIVE			(1<<0)
+#define ME_RESET_INTERRUPT		0x0004	/* - | W */
+#define ME_DIO_PORT_A			0x0006	/* R | W */
+#define ME_DIO_PORT_B			0x0008	/* R | W */
+#define ME_TIMER_DATA_0			0x000A	/* - | W */
+#define ME_TIMER_DATA_1			0x000C	/* - | W */
+#define ME_TIMER_DATA_2			0x000E	/* - | W */
+#define ME_CHANNEL_LIST			0x0010	/* - | W */
+#define   ADC_UNIPOLAR			(1<<6)
+#define   ADC_GAIN_0			(0<<4)
+#define   ADC_GAIN_1			(1<<4)
+#define   ADC_GAIN_2			(2<<4)
+#define   ADC_GAIN_3			(3<<4)
+#define ME_READ_AD_FIFO			0x0010	/* R | - */
+#define ME_DAC_CONTROL			0x0012	/* - | W */
+#define   DAC_UNIPOLAR_D		(0<<4)
+#define   DAC_BIPOLAR_D			(1<<4)
+#define   DAC_UNIPOLAR_C		(0<<5)
+#define   DAC_BIPOLAR_C			(1<<5)
+#define   DAC_UNIPOLAR_B		(0<<6)
+#define   DAC_BIPOLAR_B			(1<<6)
+#define   DAC_UNIPOLAR_A		(0<<7)
+#define   DAC_BIPOLAR_A			(1<<7)
+#define   DAC_GAIN_0_D			(0<<8)
+#define   DAC_GAIN_1_D			(1<<8)
+#define   DAC_GAIN_0_C			(0<<9)
+#define   DAC_GAIN_1_C			(1<<9)
+#define   DAC_GAIN_0_B			(0<<10)
+#define   DAC_GAIN_1_B			(1<<10)
+#define   DAC_GAIN_0_A			(0<<11)
+#define   DAC_GAIN_1_A			(1<<11)
+#define ME_DAC_CONTROL_UPDATE		0x0012	/* R | - */
+#define ME_DAC_DATA_A			0x0014	/* - | W */
+#define ME_DAC_DATA_B			0x0016	/* - | W */
+#define ME_DAC_DATA_C			0x0018	/* - | W */
+#define ME_DAC_DATA_D			0x001A	/* - | W */
+#define ME_COUNTER_ENDDATA_A		0x001C	/* - | W */
+#define ME_COUNTER_ENDDATA_B		0x001E	/* - | W */
+#define ME_COUNTER_STARTDATA_A		0x0020	/* - | W */
+#define ME_COUNTER_VALUE_A		0x0020	/* R | - */
+#define ME_COUNTER_STARTDATA_B		0x0022	/* - | W */
+#define ME_COUNTER_VALUE_B		0x0022	/* R | - */
+
+/* Function prototypes */
+static int me_attach(comedi_device *dev, comedi_devconfig *it);
+static int me_detach(comedi_device *dev);
+
+static const comedi_lrange me2000_ai_range = {
+	8,
+	{
+		BIP_RANGE(10),
+		BIP_RANGE(5),
+		BIP_RANGE(2.5),
+		BIP_RANGE(1.25),
+		UNI_RANGE(10),
+		UNI_RANGE(5),
+		UNI_RANGE(2.5),
+		UNI_RANGE(1.25)
+	}
+};
+
+static const comedi_lrange me2600_ai_range = {
+	8,
+	{
+			BIP_RANGE(10),
+			BIP_RANGE(5),
+			BIP_RANGE(2.5),
+			BIP_RANGE(1.25),
+			UNI_RANGE(10),
+			UNI_RANGE(5),
+			UNI_RANGE(2.5),
+			UNI_RANGE(1.25)
+		}
+};
+
+static const comedi_lrange me2600_ao_range = {
+	3,
+	{
+			BIP_RANGE(10),
+			BIP_RANGE(5),
+			UNI_RANGE(10)
+		}
+};
+
+static DEFINE_PCI_DEVICE_TABLE(me_pci_table) = {
+	{PCI_VENDOR_ID_MEILHAUS, ME2600_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+		0},
+	{PCI_VENDOR_ID_MEILHAUS, ME2000_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+		0},
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, me_pci_table);
+
+/* Board specification structure */
+struct me_board {
+	const char *name;	/* driver name */
+	int device_id;
+	int ao_channel_nbr;	/* DA config */
+	int ao_resolution;
+	int ao_resolution_mask;
+	const comedi_lrange *ao_range_list;
+	int ai_channel_nbr;	/* AD config */
+	int ai_resolution;
+	int ai_resolution_mask;
+	const comedi_lrange *ai_range_list;
+	int dio_channel_nbr;	/* DIO config */
+};
+
+static const struct me_board me_boards[] = {
+	{
+		/* -- ME-2600i -- */
+		.name = 		ME_DRIVER_NAME,
+		.device_id =		ME2600_DEVICE_ID,
+		/* Analog Output */
+		.ao_channel_nbr =	4,
+		.ao_resolution =	12,
+		.ao_resolution_mask =	0x0fff,
+		.ao_range_list =	&me2600_ao_range,
+		.ai_channel_nbr =	16,
+		/* Analog Input */
+		.ai_resolution =	12,
+		.ai_resolution_mask =	0x0fff,
+		.ai_range_list =	&me2600_ai_range,
+		.dio_channel_nbr =	32,
+		},
+	{
+		/* -- ME-2000i -- */
+		.name =			ME_DRIVER_NAME,
+		.device_id =		ME2000_DEVICE_ID,
+		/* Analog Output */
+		.ao_channel_nbr =	0,
+		.ao_resolution =	0,
+		.ao_resolution_mask =	0,
+		.ao_range_list =	NULL,
+		.ai_channel_nbr =	16,
+		/* Analog Input */
+		.ai_resolution =	12,
+		.ai_resolution_mask =	0x0fff,
+		.ai_range_list =	&me2000_ai_range,
+		.dio_channel_nbr =	32,
+		}
+};
+
+#define me_board_nbr (sizeof(me_boards)/sizeof(struct me_board))
+
+static comedi_driver me_driver = {
+      .driver_name =	ME_DRIVER_NAME,
+      .module =		THIS_MODULE,
+      .attach =		me_attach,
+      .detach =		me_detach,
+};
+COMEDI_PCI_INITCLEANUP(me_driver, me_pci_table);
+
+/* Private data structure */
+struct me_private_data {
+	struct pci_dev *pci_device;
+	void __iomem *plx_regbase;	/* PLX configuration base address */
+	void __iomem *me_regbase;	/* Base address of the Meilhaus card */
+	unsigned long plx_regbase_size;	/* Size of PLX configuration space */
+	unsigned long me_regbase_size;	/* Size of Meilhaus space */
+
+	unsigned short control_1;	/* Mirror of CONTROL_1 register */
+	unsigned short control_2;	/* Mirror of CONTROL_2 register */
+	unsigned short dac_control;	/* Mirror of the DAC_CONTROL register */
+	int ao_readback[4];	/* Mirror of analog output data */
+};
+
+#define dev_private ((struct me_private_data *)dev->private)
+
+/*
+ * ------------------------------------------------------------------
+ *
+ * Helpful functions
+ *
+ * ------------------------------------------------------------------
+ */
+static inline void sleep(unsigned sec)
+{
+	current->state = TASK_INTERRUPTIBLE;
+	schedule_timeout(sec * HZ);
+}
+
+/*
+ * ------------------------------------------------------------------
+ *
+ * DIGITAL INPUT/OUTPUT SECTION
+ *
+ * ------------------------------------------------------------------
+ */
+static int me_dio_insn_config(comedi_device *dev, comedi_subdevice *s,
+			      comedi_insn *insn, lsampl_t *data)
+{
+	int bits;
+	int mask = 1 << CR_CHAN(insn->chanspec);
+
+	/* calculate port */
+	if (mask & 0x0000ffff) {	/* Port A in use */
+		bits = 0x0000ffff;
+
+		/* Enable Port A */
+		dev_private->control_2 |= ENABLE_PORT_A;
+		writew(dev_private->control_2,
+			dev_private->me_regbase + ME_CONTROL_2);
+	} else {		/* Port B in use */
+
+		bits = 0xffff0000;
+
+		/* Enable Port B */
+		dev_private->control_2 |= ENABLE_PORT_B;
+		writew(dev_private->control_2,
+			dev_private->me_regbase + ME_CONTROL_2);
+	}
+
+	if (data[0]) {
+		/* Config port as output */
+		s->io_bits |= bits;
+	} else {
+		/* Config port as input */
+		s->io_bits &= ~bits;
+	}
+
+	return 1;
+}
+
+/* Digital instant input/outputs */
+static int me_dio_insn_bits(comedi_device *dev, comedi_subdevice *s,
+			    comedi_insn *insn, lsampl_t *data)
+{
+	unsigned int mask = data[0];
+	s->state &= ~mask;
+	s->state |= (mask & data[1]);
+
+	mask &= s->io_bits;
+	if (mask & 0x0000ffff) {	/* Port A */
+		writew((s->state & 0xffff),
+			dev_private->me_regbase + ME_DIO_PORT_A);
+	} else {
+		data[1] &= ~0x0000ffff;
+		data[1] |= readw(dev_private->me_regbase + ME_DIO_PORT_A);
+	}
+
+	if (mask & 0xffff0000) {	/* Port B */
+		writew(((s->state >> 16) & 0xffff),
+			dev_private->me_regbase + ME_DIO_PORT_B);
+	} else {
+		data[1] &= ~0xffff0000;
+		data[1] |= readw(dev_private->me_regbase + ME_DIO_PORT_B) << 16;
+	}
+
+	return 2;
+}
+
+/*
+ * ------------------------------------------------------------------
+ *
+ * ANALOG INPUT SECTION
+ *
+ * ------------------------------------------------------------------
+ */
+
+/* Analog instant input */
+static int me_ai_insn_read(comedi_device *dev, comedi_subdevice *subdevice,
+			   comedi_insn *insn, lsampl_t *data)
+{
+	unsigned short value;
+	int chan = CR_CHAN((&insn->chanspec)[0]);
+	int rang = CR_RANGE((&insn->chanspec)[0]);
+	int aref = CR_AREF((&insn->chanspec)[0]);
+	int i;
+
+	/* stop any running conversion */
+	dev_private->control_1 &= 0xFFFC;
+	writew(dev_private->control_1, dev_private->me_regbase + ME_CONTROL_1);
+
+	/* clear chanlist and ad fifo */
+	dev_private->control_2 &= ~(ENABLE_ADFIFO | ENABLE_CHANLIST);
+	writew(dev_private->control_2, dev_private->me_regbase + ME_CONTROL_2);
+
+	/* reset any pending interrupt */
+	writew(0x00, dev_private->me_regbase + ME_RESET_INTERRUPT);
+
+	/* enable the chanlist and ADC fifo */
+	dev_private->control_2 |= (ENABLE_ADFIFO | ENABLE_CHANLIST);
+	writew(dev_private->control_2, dev_private->me_regbase + ME_CONTROL_2);
+
+	/* write to channel list fifo */
+	/* b3:b0 are the channel number */
+	value = chan & 0x0f;
+	/* b5:b4 are the channel gain */
+	value |= (rang & 0x03) << 4;
+	/* b6 channel polarity */
+	value |= (rang & 0x04) << 4;
+	/* b7 single or differential */
+	value |= ((aref & AREF_DIFF) ? 0x80 : 0);
+	writew(value & 0xff, dev_private->me_regbase + ME_CHANNEL_LIST);
+
+	/* set ADC mode to software trigger */
+	dev_private->control_1 |= SOFTWARE_TRIGGERED_ADC;
+	writew(dev_private->control_1, dev_private->me_regbase + ME_CONTROL_1);
+
+	/* start conversion by reading from ADC_START */
+	readw(dev_private->me_regbase + ME_ADC_START);
+
+	/* wait for ADC fifo not empty flag */
+	for (i = 100000; i > 0; i--)
+		if (!(readw(dev_private->me_regbase + ME_STATUS) & 0x0004))
+			break;
+
+	/* get value from ADC fifo */
+	if (i) {
+		data[0] =
+			(readw(dev_private->me_regbase +
+				ME_READ_AD_FIFO) ^ 0x800) & 0x0FFF;
+	} else {
+		printk(KERN_ERR "comedi%d: Cannot get single value\n",
+		       dev->minor);
+		return -EIO;
+	}
+
+	/* stop any running conversion */
+	dev_private->control_1 &= 0xFFFC;
+	writew(dev_private->control_1, dev_private->me_regbase + ME_CONTROL_1);
+
+	return 1;
+}
+
+/*
+ * ------------------------------------------------------------------
+ *
+ * HARDWARE TRIGGERED ANALOG INPUT SECTION
+ *
+ * ------------------------------------------------------------------
+ */
+
+/* Cancel analog input autoscan */
+static int me_ai_cancel(comedi_device *dev, comedi_subdevice *s)
+{
+	/* disable interrupts */
+
+	/* stop any running conversion */
+	dev_private->control_1 &= 0xFFFC;
+	writew(dev_private->control_1, dev_private->me_regbase + ME_CONTROL_1);
+
+	return 0;
+}
+
+/* Test analog input command */
+static int me_ai_do_cmd_test(comedi_device *dev, comedi_subdevice *s,
+			     comedi_cmd *cmd)
+{
+	return 0;
+}
+
+/* Analog input command */
+static int me_ai_do_cmd(comedi_device *dev, comedi_subdevice *subdevice)
+{
+	return 0;
+}
+
+/*
+ * ------------------------------------------------------------------
+ *
+ * ANALOG OUTPUT SECTION
+ *
+ * ------------------------------------------------------------------
+ */
+
+/* Analog instant output */
+static int me_ao_insn_write(comedi_device *dev, comedi_subdevice *s,
+			    comedi_insn *insn, lsampl_t *data)
+{
+	int chan;
+	int rang;
+	int i;
+
+	/* Enable all DAC */
+	dev_private->control_2 |= ENABLE_DAC;
+	writew(dev_private->control_2, dev_private->me_regbase + ME_CONTROL_2);
+
+	/* and set DAC to "buffered" mode */
+	dev_private->control_2 |= BUFFERED_DAC;
+	writew(dev_private->control_2, dev_private->me_regbase + ME_CONTROL_2);
+
+	/* Set dac-control register */
+	for (i = 0; i < insn->n; i++) {
+		chan = CR_CHAN((&insn->chanspec)[i]);
+		rang = CR_RANGE((&insn->chanspec)[i]);
+
+		/* clear bits for this channel */
+		dev_private->dac_control &= ~(0x0880 >> chan);
+		if (rang == 0)
+			dev_private->dac_control |=
+				((DAC_BIPOLAR_A | DAC_GAIN_1_A) >> chan);
+		else if (rang == 1)
+			dev_private->dac_control |=
+				((DAC_BIPOLAR_A | DAC_GAIN_0_A) >> chan);
+	}
+	writew(dev_private->dac_control,
+		dev_private->me_regbase + ME_DAC_CONTROL);
+
+	/* Update dac-control register */
+	readw(dev_private->me_regbase + ME_DAC_CONTROL_UPDATE);
+
+	/* Set data register */
+	for (i = 0; i < insn->n; i++) {
+		chan = CR_CHAN((&insn->chanspec)[i]);
+		writew((data[0] & s->maxdata),
+			dev_private->me_regbase + ME_DAC_DATA_A + (chan << 1));
+		dev_private->ao_readback[chan] = (data[0] & s->maxdata);
+	}
+
+	/* Update dac with data registers */
+	readw(dev_private->me_regbase + ME_DAC_UPDATE);
+
+	return i;
+}
+
+/* Analog output readback */
+static int me_ao_insn_read(comedi_device *dev, comedi_subdevice *s,
+			   comedi_insn *insn, lsampl_t *data)
+{
+	int i;
+
+	for (i = 0; i < insn->n; i++) {
+		data[i] =
+			dev_private->ao_readback[CR_CHAN((&insn->chanspec)[i])];
+	}
+
+	return 1;
+}
+
+/*
+ * ------------------------------------------------------------------
+ *
+ * INITIALISATION SECTION
+ *
+ * ------------------------------------------------------------------
+ */
+
+/* Xilinx firmware download for card: ME-2600i */
+static int me2600_xilinx_download(comedi_device *dev,
+				  unsigned char *me2600_firmware,
+				  unsigned int length)
+{
+	unsigned int value;
+	unsigned int file_length;
+	unsigned int i;
+
+	/* disable irq's on PLX */
+	writel(0x00, dev_private->plx_regbase + PLX_INTCSR);
+
+	/* First, make a dummy read to reset xilinx */
+	value = readw(dev_private->me_regbase + XILINX_DOWNLOAD_RESET);
+
+	/* Wait until reset is over */
+	sleep(1);
+
+	/* Write a dummy value to Xilinx */
+	writeb(0x00, dev_private->me_regbase + 0x0);
+	sleep(1);
+
+	/*
+	 * Format of the firmware
+	 * Build longs from the byte-wise coded header
+	 * Byte 1-3:   length of the array
+	 * Byte 4-7:   version
+	 * Byte 8-11:  date
+	 * Byte 12-15: reserved
+	 */
+	if (length < 16)
+		return -EINVAL;
+	file_length = (((unsigned int)me2600_firmware[0] & 0xff) << 24) +
+		      (((unsigned int)me2600_firmware[1] & 0xff) << 16) +
+		      (((unsigned int)me2600_firmware[2] & 0xff) << 8) +
+		      ((unsigned int)me2600_firmware[3] & 0xff);
+
+	/*
+	 * Loop for writing firmware byte by byte to xilinx
+	 * Firmware data start at offfset 16
+	 */
+	for (i = 0; i < file_length; i++)
+		writeb((me2600_firmware[16 + i] & 0xff),
+			dev_private->me_regbase + 0x0);
+
+	/* Write 5 dummy values to xilinx */
+	for (i = 0; i < 5; i++)
+		writeb(0x00, dev_private->me_regbase + 0x0);
+
+	/* Test if there was an error during download -> INTB was thrown */
+	value = readl(dev_private->plx_regbase + PLX_INTCSR);
+	if (value & 0x20) {
+		/* Disable interrupt */
+		writel(0x00, dev_private->plx_regbase + PLX_INTCSR);
+		printk(KERN_ERR "comedi%d: Xilinx download failed\n",
+		       dev->minor);
+		return -EIO;
+	}
+
+	/* Wait until the Xilinx is ready for real work */
+	sleep(1);
+
+	/* Enable PLX-Interrupts */
+	writel(0x43, dev_private->plx_regbase + PLX_INTCSR);
+
+	return 0;
+}
+
+/* Reset device */
+static int me_reset(comedi_device *dev)
+{
+	/* Reset board */
+	writew(0x00, dev_private->me_regbase + ME_CONTROL_1);
+	writew(0x00, dev_private->me_regbase + ME_CONTROL_2);
+	writew(0x00, dev_private->me_regbase + ME_RESET_INTERRUPT);
+	writew(0x00, dev_private->me_regbase + ME_DAC_CONTROL);
+
+	/* Save values in the board context */
+	dev_private->dac_control = 0;
+	dev_private->control_1 = 0;
+	dev_private->control_2 = 0;
+
+	return 0;
+}
+
+/*
+ * Attach
+ *
+ * - Register PCI device
+ * - Declare device driver capability
+ */
+static int me_attach(comedi_device *dev, comedi_devconfig *it)
+{
+	struct pci_dev *pci_device;
+	comedi_subdevice *subdevice;
+	struct me_board *board;
+	resource_size_t plx_regbase_tmp;
+	unsigned long plx_regbase_size_tmp;
+	resource_size_t me_regbase_tmp;
+	unsigned long me_regbase_size_tmp;
+	resource_size_t swap_regbase_tmp;
+	unsigned long swap_regbase_size_tmp;
+	resource_size_t regbase_tmp;
+	int result, error, i;
+
+	/* Allocate private memory */
+	if (alloc_private(dev, sizeof(struct me_private_data)) < 0)
+		return -ENOMEM;
+
+	/* Probe the device to determine what device in the series it is. */
+	for (pci_device = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, NULL);
+		pci_device != NULL;
+		pci_device =
+		pci_get_device(PCI_ANY_ID, PCI_ANY_ID, pci_device)) {
+		if (pci_device->vendor == PCI_VENDOR_ID_MEILHAUS) {
+			for (i = 0; i < me_board_nbr; i++) {
+				if (me_boards[i].device_id ==
+					pci_device->device) {
+					/*
+					 * was a particular bus/slot requested?
+					 */
+					if ((it->options[0] != 0)
+						|| (it->options[1] != 0)) {
+						/*
+						 * are we on the wrong bus/slot?
+						 */
+						if (pci_device->bus->number !=
+							it->options[0]
+							|| PCI_SLOT(pci_device->
+								devfn) !=
+							it->options[1]) {
+							continue;
+						}
+					}
+
+					dev->board_ptr = me_boards + i;
+					board = (struct me_board *) dev->
+						board_ptr;
+					dev_private->pci_device = pci_device;
+					goto found;
+				}
+			}
+		}
+	}
+
+	printk(KERN_ERR
+	       "comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
+	       dev->minor, it->options[0], it->options[1]);
+	return -EIO;
+
+found:
+	printk(KERN_INFO "comedi%d: found %s at PCI bus %d, slot %d\n",
+		dev->minor, me_boards[i].name,
+		pci_device->bus->number, PCI_SLOT(pci_device->devfn));
+
+	/* Enable PCI device and request PCI regions */
+	if (comedi_pci_enable(pci_device, ME_DRIVER_NAME) < 0) {
+		printk(KERN_ERR "comedi%d: Failed to enable PCI device and "
+		       "request regions\n", dev->minor);
+		return -EIO;
+	}
+
+	/* Set data in device structure */
+	dev->board_name = board->name;
+
+	/* Read PLX register base address [PCI_BASE_ADDRESS #0]. */
+	plx_regbase_tmp = pci_resource_start(pci_device, 0);
+	plx_regbase_size_tmp = pci_resource_len(pci_device, 0);
+	dev_private->plx_regbase =
+		ioremap(plx_regbase_tmp, plx_regbase_size_tmp);
+	dev_private->plx_regbase_size = plx_regbase_size_tmp;
+	if (!dev_private->plx_regbase) {
+		printk("comedi%d: Failed to remap I/O memory\n", dev->minor);
+		return -ENOMEM;
+	}
+
+	/* Read Swap base address [PCI_BASE_ADDRESS #5]. */
+
+	swap_regbase_tmp = pci_resource_start(pci_device, 5);
+	swap_regbase_size_tmp = pci_resource_len(pci_device, 5);
+
+	if (!swap_regbase_tmp)
+		printk(KERN_ERR "comedi%d: Swap not present\n", dev->minor);
+
+	/*---------------------------------------------- Workaround start ---*/
+	if (plx_regbase_tmp & 0x0080) {
+		printk(KERN_ERR "comedi%d: PLX-Bug detected\n", dev->minor);
+
+		if (swap_regbase_tmp) {
+			regbase_tmp = plx_regbase_tmp;
+			plx_regbase_tmp = swap_regbase_tmp;
+			swap_regbase_tmp = regbase_tmp;
+
+			result = pci_write_config_dword(pci_device,
+				PCI_BASE_ADDRESS_0, plx_regbase_tmp);
+			if (result != PCIBIOS_SUCCESSFUL)
+				return -EIO;
+
+			result = pci_write_config_dword(pci_device,
+				PCI_BASE_ADDRESS_5, swap_regbase_tmp);
+			if (result != PCIBIOS_SUCCESSFUL)
+				return -EIO;
+		} else {
+			plx_regbase_tmp -= 0x80;
+			result = pci_write_config_dword(pci_device,
+				PCI_BASE_ADDRESS_0, plx_regbase_tmp);
+			if (result != PCIBIOS_SUCCESSFUL)
+				return -EIO;
+		}
+	}
+	/*--------------------------------------------- Workaround end -----*/
+
+	/* Read Meilhaus register base address [PCI_BASE_ADDRESS #2]. */
+
+	me_regbase_tmp = pci_resource_start(pci_device, 2);
+	me_regbase_size_tmp = pci_resource_len(pci_device, 2);
+	dev_private->me_regbase_size = me_regbase_size_tmp;
+	dev_private->me_regbase = ioremap(me_regbase_tmp, me_regbase_size_tmp);
+	if (!dev_private->me_regbase) {
+		printk(KERN_ERR "comedi%d: Failed to remap I/O memory\n",
+		       dev->minor);
+		return -ENOMEM;
+	}
+	/* Download firmware and reset card */
+	if (board->device_id == ME2600_DEVICE_ID) {
+		unsigned char *aux_data;
+		int aux_len;
+
+		aux_data = comedi_aux_data(it->options, 0);
+		aux_len = it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH];
+
+		if (!aux_data || aux_len < 1) {
+			comedi_error(dev, "You must provide me2600 firmware "
+				     "using the --init-data option of "
+				     "comedi_config");
+			return -EINVAL;
+		}
+		me2600_xilinx_download(dev, aux_data, aux_len);
+	}
+
+	me_reset(dev);
+
+	/* device driver capabilities */
+	error = alloc_subdevices(dev, 3);
+	if (error < 0)
+		return error;
+
+	subdevice = dev->subdevices + 0;
+	subdevice->type = COMEDI_SUBD_AI;
+	subdevice->subdev_flags = SDF_READABLE | SDF_COMMON | SDF_CMD_READ;
+	subdevice->n_chan = board->ai_channel_nbr;
+	subdevice->maxdata = board->ai_resolution_mask;
+	subdevice->len_chanlist = board->ai_channel_nbr;
+	subdevice->range_table = board->ai_range_list;
+	subdevice->cancel = me_ai_cancel;
+	subdevice->insn_read = me_ai_insn_read;
+	subdevice->do_cmdtest = me_ai_do_cmd_test;
+	subdevice->do_cmd = me_ai_do_cmd;
+
+	subdevice = dev->subdevices + 1;
+	subdevice->type = COMEDI_SUBD_AO;
+	subdevice->subdev_flags = SDF_WRITEABLE | SDF_COMMON;
+	subdevice->n_chan = board->ao_channel_nbr;
+	subdevice->maxdata = board->ao_resolution_mask;
+	subdevice->len_chanlist = board->ao_channel_nbr;
+	subdevice->range_table = board->ao_range_list;
+	subdevice->insn_read = me_ao_insn_read;
+	subdevice->insn_write = me_ao_insn_write;
+
+	subdevice = dev->subdevices + 2;
+	subdevice->type = COMEDI_SUBD_DIO;
+	subdevice->subdev_flags = SDF_READABLE | SDF_WRITEABLE;
+	subdevice->n_chan = board->dio_channel_nbr;
+	subdevice->maxdata = 1;
+	subdevice->len_chanlist = board->dio_channel_nbr;
+	subdevice->range_table = &range_digital;
+	subdevice->insn_bits = me_dio_insn_bits;
+	subdevice->insn_config = me_dio_insn_config;
+	subdevice->io_bits = 0;
+
+	printk(KERN_INFO "comedi%d: "ME_DRIVER_NAME" attached.\n", dev->minor);
+	return 0;
+}
+
+/* Detach */
+static int me_detach(comedi_device *dev)
+{
+	if (dev_private) {
+		if (dev_private->me_regbase) {
+			me_reset(dev);
+			iounmap(dev_private->me_regbase);
+		}
+		if (dev_private->plx_regbase)
+			iounmap(dev_private->plx_regbase);
+		if (dev_private->pci_device) {
+			if (dev_private->plx_regbase_size)
+				comedi_pci_disable(dev_private->pci_device);
+
+			pci_dev_put(dev_private->pci_device);
+		}
+	}
+	return 0;
+}
diff --git a/drivers/staging/comedi/drivers/mite.c b/drivers/staging/comedi/drivers/mite.c
new file mode 100644
index 000000000000..9cc527424d04
--- /dev/null
+++ b/drivers/staging/comedi/drivers/mite.c
@@ -0,0 +1,809 @@
+/*
+    comedi/drivers/mite.c
+    Hardware driver for NI Mite PCI interface chip
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2002 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+/*
+	The PCI-MIO E series driver was originally written by
+	Tomasz Motylewski <...>, and ported to comedi by ds.
+
+	References for specifications:
+
+	   321747b.pdf  Register Level Programmer Manual (obsolete)
+	   321747c.pdf  Register Level Programmer Manual (new)
+	   DAQ-STC reference manual
+
+	Other possibly relevant info:
+
+	   320517c.pdf  User manual (obsolete)
+	   320517f.pdf  User manual (new)
+	   320889a.pdf  delete
+	   320906c.pdf  maximum signal ratings
+	   321066a.pdf  about 16x
+	   321791a.pdf  discontinuation of at-mio-16e-10 rev. c
+	   321808a.pdf  about at-mio-16e-10 rev P
+	   321837a.pdf  discontinuation of at-mio-16de-10 rev d
+	   321838a.pdf  about at-mio-16de-10 rev N
+
+	ISSUES:
+
+*/
+
+//#define USE_KMALLOC
+
+#include "mite.h"
+
+#include "comedi_fc.h"
+#include "comedi_pci.h"
+#include "../comedidev.h"
+
+#include <asm/system.h>
+
+#define PCI_MITE_SIZE		4096
+#define PCI_DAQ_SIZE		4096
+#define PCI_DAQ_SIZE_660X       8192
+
+MODULE_LICENSE("GPL");
+
+struct mite_struct *mite_devices = NULL;
+
+#define TOP_OF_PAGE(x) ((x)|(~(PAGE_MASK)))
+
+void mite_init(void)
+{
+	struct pci_dev *pcidev;
+	struct mite_struct *mite;
+
+	for (pcidev = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, NULL);
+		pcidev != NULL;
+		pcidev = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, pcidev)) {
+		if (pcidev->vendor == PCI_VENDOR_ID_NATINST) {
+			unsigned i;
+
+			mite = kzalloc(sizeof(*mite), GFP_KERNEL);
+			if (!mite) {
+				printk("mite: allocation failed\n");
+				pci_dev_put(pcidev);
+				return;
+			}
+			spin_lock_init(&mite->lock);
+			mite->pcidev = pci_dev_get(pcidev);
+			for (i = 0; i < MAX_MITE_DMA_CHANNELS; ++i) {
+				mite->channels[i].mite = mite;
+				mite->channels[i].channel = i;
+				mite->channels[i].done = 1;
+			}
+			mite->next = mite_devices;
+			mite_devices = mite;
+		}
+	}
+}
+
+static void dump_chip_signature(u32 csigr_bits)
+{
+	printk("mite: version = %i, type = %i, mite mode = %i, interface mode = %i\n", mite_csigr_version(csigr_bits), mite_csigr_type(csigr_bits), mite_csigr_mmode(csigr_bits), mite_csigr_imode(csigr_bits));
+	printk("mite: num channels = %i, write post fifo depth = %i, wins = %i, iowins = %i\n", mite_csigr_dmac(csigr_bits), mite_csigr_wpdep(csigr_bits), mite_csigr_wins(csigr_bits), mite_csigr_iowins(csigr_bits));
+}
+
+unsigned mite_fifo_size(struct mite_struct * mite, unsigned channel)
+{
+	unsigned fcr_bits = readl(mite->mite_io_addr +
+		MITE_FCR(channel));
+	unsigned empty_count = (fcr_bits >> 16) & 0xff;
+	unsigned full_count = fcr_bits & 0xff;
+	return empty_count + full_count;
+}
+
+int mite_setup2(struct mite_struct *mite, unsigned use_iodwbsr_1)
+{
+	unsigned long length;
+	resource_size_t addr;
+	int i;
+	u32 csigr_bits;
+	unsigned unknown_dma_burst_bits;
+
+	if (comedi_pci_enable(mite->pcidev, "mite")) {
+		printk("error enabling mite and requesting io regions\n");
+		return -EIO;
+	}
+	pci_set_master(mite->pcidev);
+
+	addr = pci_resource_start(mite->pcidev, 0);
+	mite->mite_phys_addr = addr;
+	mite->mite_io_addr = ioremap(addr, PCI_MITE_SIZE);
+	if (!mite->mite_io_addr) {
+		printk("failed to remap mite io memory address\n");
+		return -ENOMEM;
+	}
+	printk("MITE:0x%08llx mapped to %p ",
+		(unsigned long long)mite->mite_phys_addr, mite->mite_io_addr);
+
+	addr = pci_resource_start(mite->pcidev, 1);
+	mite->daq_phys_addr = addr;
+	length = pci_resource_len(mite->pcidev, 1);
+	// In case of a 660x board, DAQ size is 8k instead of 4k (see as shown by lspci output)
+	mite->daq_io_addr = ioremap(mite->daq_phys_addr, length);
+	if (!mite->daq_io_addr) {
+		printk("failed to remap daq io memory address\n");
+		return -ENOMEM;
+	}
+	printk("DAQ:0x%08llx mapped to %p\n",
+		(unsigned long long)mite->daq_phys_addr, mite->daq_io_addr);
+
+	if (use_iodwbsr_1) {
+		writel(0, mite->mite_io_addr + MITE_IODWBSR);
+		printk("mite: using I/O Window Base Size register 1\n");
+		writel(mite->
+			daq_phys_addr | WENAB |
+			MITE_IODWBSR_1_WSIZE_bits(length),
+			mite->mite_io_addr + MITE_IODWBSR_1);
+		writel(0, mite->mite_io_addr + MITE_IODWCR_1);
+	} else {
+		writel(mite->daq_phys_addr | WENAB,
+			mite->mite_io_addr + MITE_IODWBSR);
+	}
+	/* make sure dma bursts work.  I got this from running a bus analyzer
+	   on a pxi-6281 and a pxi-6713.  6713 powered up with register value
+	   of 0x61f and bursts worked.  6281 powered up with register value of
+	   0x1f and bursts didn't work.  The NI windows driver reads the register,
+	   then does a bitwise-or of 0x600 with it and writes it back.
+	 */
+	unknown_dma_burst_bits =
+		readl(mite->mite_io_addr + MITE_UNKNOWN_DMA_BURST_REG);
+	unknown_dma_burst_bits |= UNKNOWN_DMA_BURST_ENABLE_BITS;
+	writel(unknown_dma_burst_bits,
+		mite->mite_io_addr + MITE_UNKNOWN_DMA_BURST_REG);
+
+	csigr_bits = readl(mite->mite_io_addr + MITE_CSIGR);
+	mite->num_channels = mite_csigr_dmac(csigr_bits);
+	if (mite->num_channels > MAX_MITE_DMA_CHANNELS) {
+		printk("mite: bug? chip claims to have %i dma channels.  Setting to %i.\n", mite->num_channels, MAX_MITE_DMA_CHANNELS);
+		mite->num_channels = MAX_MITE_DMA_CHANNELS;
+	}
+	dump_chip_signature(csigr_bits);
+	for (i = 0; i < mite->num_channels; i++) {
+		writel(CHOR_DMARESET, mite->mite_io_addr + MITE_CHOR(i));
+		/* disable interrupts */
+		writel(CHCR_CLR_DMA_IE | CHCR_CLR_LINKP_IE | CHCR_CLR_SAR_IE |
+			CHCR_CLR_DONE_IE | CHCR_CLR_MRDY_IE | CHCR_CLR_DRDY_IE |
+			CHCR_CLR_LC_IE | CHCR_CLR_CONT_RB_IE,
+			mite->mite_io_addr + MITE_CHCR(i));
+	}
+	mite->fifo_size = mite_fifo_size(mite, 0);
+	printk("mite: fifo size is %i.\n", mite->fifo_size);
+	mite->used = 1;
+
+	return 0;
+}
+
+int mite_setup(struct mite_struct *mite)
+{
+	return mite_setup2(mite, 0);
+}
+
+void mite_cleanup(void)
+{
+	struct mite_struct *mite, *next;
+
+	for (mite = mite_devices; mite; mite = next) {
+		pci_dev_put(mite->pcidev);
+		next = mite->next;
+		kfree(mite);
+	}
+}
+
+void mite_unsetup(struct mite_struct *mite)
+{
+	//unsigned long offset, start, length;
+
+	if (!mite)
+		return;
+
+	if (mite->mite_io_addr) {
+		iounmap(mite->mite_io_addr);
+		mite->mite_io_addr = NULL;
+	}
+	if (mite->daq_io_addr) {
+		iounmap(mite->daq_io_addr);
+		mite->daq_io_addr = NULL;
+	}
+	if (mite->mite_phys_addr) {
+		comedi_pci_disable(mite->pcidev);
+		mite->mite_phys_addr = 0;
+	}
+
+	mite->used = 0;
+}
+
+void mite_list_devices(void)
+{
+	struct mite_struct *mite, *next;
+
+	printk("Available NI device IDs:");
+	if (mite_devices)
+		for (mite = mite_devices; mite; mite = next) {
+			next = mite->next;
+			printk(" 0x%04x", mite_device_id(mite));
+			if (mite->used)
+				printk("(used)");
+		}
+	printk("\n");
+
+}
+
+struct mite_channel *mite_request_channel_in_range(struct mite_struct *mite,
+	struct mite_dma_descriptor_ring *ring, unsigned min_channel,
+	unsigned max_channel)
+{
+	int i;
+	unsigned long flags;
+	struct mite_channel *channel = NULL;
+
+	// spin lock so mite_release_channel can be called safely from interrupts
+	comedi_spin_lock_irqsave(&mite->lock, flags);
+	for (i = min_channel; i <= max_channel; ++i) {
+		if (mite->channel_allocated[i] == 0) {
+			mite->channel_allocated[i] = 1;
+			channel = &mite->channels[i];
+			channel->ring = ring;
+			break;
+		}
+	}
+	comedi_spin_unlock_irqrestore(&mite->lock, flags);
+	return channel;
+}
+
+void mite_release_channel(struct mite_channel *mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	unsigned long flags;
+
+	// spin lock to prevent races with mite_request_channel
+	comedi_spin_lock_irqsave(&mite->lock, flags);
+	if (mite->channel_allocated[mite_chan->channel]) {
+		mite_dma_disarm(mite_chan);
+		mite_dma_reset(mite_chan);
+/* disable all channel's interrupts (do it after disarm/reset so
+MITE_CHCR reg isn't changed while dma is still active!) */
+		writel(CHCR_CLR_DMA_IE | CHCR_CLR_LINKP_IE |
+			CHCR_CLR_SAR_IE | CHCR_CLR_DONE_IE |
+			CHCR_CLR_MRDY_IE | CHCR_CLR_DRDY_IE |
+			CHCR_CLR_LC_IE | CHCR_CLR_CONT_RB_IE,
+			mite->mite_io_addr + MITE_CHCR(mite_chan->channel));
+		mite->channel_allocated[mite_chan->channel] = 0;
+		mite_chan->ring = NULL;
+		mmiowb();
+	}
+	comedi_spin_unlock_irqrestore(&mite->lock, flags);
+}
+
+void mite_dma_arm(struct mite_channel *mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	int chor;
+	unsigned long flags;
+
+	MDPRINTK("mite_dma_arm ch%i\n", channel);
+	/* memory barrier is intended to insure any twiddling with the buffer
+	   is done before writing to the mite to arm dma transfer */
+	smp_mb();
+	/* arm */
+	chor = CHOR_START;
+	comedi_spin_lock_irqsave(&mite->lock, flags);
+	mite_chan->done = 0;
+	writel(chor, mite->mite_io_addr + MITE_CHOR(mite_chan->channel));
+	mmiowb();
+	comedi_spin_unlock_irqrestore(&mite->lock, flags);
+//      mite_dma_tcr(mite, channel);
+}
+
+/**************************************/
+
+int mite_buf_change(struct mite_dma_descriptor_ring *ring, comedi_async * async)
+{
+	unsigned int n_links;
+	int i;
+
+	if (ring->descriptors) {
+		dma_free_coherent(ring->hw_dev,
+			ring->n_links * sizeof(struct mite_dma_descriptor),
+			ring->descriptors, ring->descriptors_dma_addr);
+	}
+	ring->descriptors = NULL;
+	ring->descriptors_dma_addr = 0;
+	ring->n_links = 0;
+
+	if (async->prealloc_bufsz == 0) {
+		return 0;
+	}
+	n_links = async->prealloc_bufsz >> PAGE_SHIFT;
+
+	MDPRINTK("ring->hw_dev=%p, n_links=0x%04x\n", ring->hw_dev, n_links);
+
+	ring->descriptors =
+		dma_alloc_coherent(ring->hw_dev,
+		n_links * sizeof(struct mite_dma_descriptor),
+		&ring->descriptors_dma_addr, GFP_KERNEL);
+	if (!ring->descriptors) {
+		printk("mite: ring buffer allocation failed\n");
+		return -ENOMEM;
+	}
+	ring->n_links = n_links;
+
+	for (i = 0; i < n_links; i++) {
+		ring->descriptors[i].count = cpu_to_le32(PAGE_SIZE);
+		ring->descriptors[i].addr =
+			cpu_to_le32(async->buf_page_list[i].dma_addr);
+		ring->descriptors[i].next =
+			cpu_to_le32(ring->descriptors_dma_addr + (i +
+				1) * sizeof(struct mite_dma_descriptor));
+	}
+	ring->descriptors[n_links - 1].next =
+		cpu_to_le32(ring->descriptors_dma_addr);
+	/* barrier is meant to insure that all the writes to the dma descriptors
+	   have completed before the dma controller is commanded to read them */
+	smp_wmb();
+	return 0;
+}
+
+void mite_prep_dma(struct mite_channel *mite_chan,
+	unsigned int num_device_bits, unsigned int num_memory_bits)
+{
+	unsigned int chor, chcr, mcr, dcr, lkcr;
+	struct mite_struct *mite = mite_chan->mite;
+
+	MDPRINTK("mite_prep_dma ch%i\n", mite_chan->channel);
+
+	/* reset DMA and FIFO */
+	chor = CHOR_DMARESET | CHOR_FRESET;
+	writel(chor, mite->mite_io_addr + MITE_CHOR(mite_chan->channel));
+
+	/* short link chaining mode */
+	chcr = CHCR_SET_DMA_IE | CHCR_LINKSHORT | CHCR_SET_DONE_IE |
+		CHCR_BURSTEN;
+	/*
+	 * Link Complete Interrupt: interrupt every time a link
+	 * in MITE_RING is completed. This can generate a lot of
+	 * extra interrupts, but right now we update the values
+	 * of buf_int_ptr and buf_int_count at each interrupt.  A
+	 * better method is to poll the MITE before each user
+	 * "read()" to calculate the number of bytes available.
+	 */
+	chcr |= CHCR_SET_LC_IE;
+	if (num_memory_bits == 32 && num_device_bits == 16) {
+		/* Doing a combined 32 and 16 bit byteswap gets the 16 bit samples into the fifo in the right order.
+		   Tested doing 32 bit memory to 16 bit device transfers to the analog out of a pxi-6281,
+		   which has mite version = 1, type = 4.  This also works for dma reads from the counters
+		   on e-series boards.  */
+		chcr |= CHCR_BYTE_SWAP_DEVICE | CHCR_BYTE_SWAP_MEMORY;
+	}
+	if (mite_chan->dir == COMEDI_INPUT) {
+		chcr |= CHCR_DEV_TO_MEM;
+	}
+	writel(chcr, mite->mite_io_addr + MITE_CHCR(mite_chan->channel));
+
+	/* to/from memory */
+	mcr = CR_RL(64) | CR_ASEQUP;
+	switch (num_memory_bits) {
+	case 8:
+		mcr |= CR_PSIZE8;
+		break;
+	case 16:
+		mcr |= CR_PSIZE16;
+		break;
+	case 32:
+		mcr |= CR_PSIZE32;
+		break;
+	default:
+		rt_printk
+			("mite: bug! invalid mem bit width for dma transfer\n");
+		break;
+	}
+	writel(mcr, mite->mite_io_addr + MITE_MCR(mite_chan->channel));
+
+	/* from/to device */
+	dcr = CR_RL(64) | CR_ASEQUP;
+	dcr |= CR_PORTIO | CR_AMDEVICE | CR_REQSDRQ(mite_chan->channel);
+	switch (num_device_bits) {
+	case 8:
+		dcr |= CR_PSIZE8;
+		break;
+	case 16:
+		dcr |= CR_PSIZE16;
+		break;
+	case 32:
+		dcr |= CR_PSIZE32;
+		break;
+	default:
+		rt_printk
+			("mite: bug! invalid dev bit width for dma transfer\n");
+		break;
+	}
+	writel(dcr, mite->mite_io_addr + MITE_DCR(mite_chan->channel));
+
+	/* reset the DAR */
+	writel(0, mite->mite_io_addr + MITE_DAR(mite_chan->channel));
+
+	/* the link is 32bits */
+	lkcr = CR_RL(64) | CR_ASEQUP | CR_PSIZE32;
+	writel(lkcr, mite->mite_io_addr + MITE_LKCR(mite_chan->channel));
+
+	/* starting address for link chaining */
+	writel(mite_chan->ring->descriptors_dma_addr,
+		mite->mite_io_addr + MITE_LKAR(mite_chan->channel));
+
+	MDPRINTK("exit mite_prep_dma\n");
+}
+
+u32 mite_device_bytes_transferred(struct mite_channel *mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	return readl(mite->mite_io_addr + MITE_DAR(mite_chan->channel));
+}
+
+u32 mite_bytes_in_transit(struct mite_channel * mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	return readl(mite->mite_io_addr +
+		MITE_FCR(mite_chan->channel)) & 0x000000FF;
+}
+
+// returns lower bound for number of bytes transferred from device to memory
+u32 mite_bytes_written_to_memory_lb(struct mite_channel * mite_chan)
+{
+	u32 device_byte_count;
+
+	device_byte_count = mite_device_bytes_transferred(mite_chan);
+	return device_byte_count - mite_bytes_in_transit(mite_chan);
+}
+
+// returns upper bound for number of bytes transferred from device to memory
+u32 mite_bytes_written_to_memory_ub(struct mite_channel * mite_chan)
+{
+	u32 in_transit_count;
+
+	in_transit_count = mite_bytes_in_transit(mite_chan);
+	return mite_device_bytes_transferred(mite_chan) - in_transit_count;
+}
+
+// returns lower bound for number of bytes read from memory for transfer to device
+u32 mite_bytes_read_from_memory_lb(struct mite_channel * mite_chan)
+{
+	u32 device_byte_count;
+
+	device_byte_count = mite_device_bytes_transferred(mite_chan);
+	return device_byte_count + mite_bytes_in_transit(mite_chan);
+}
+
+// returns upper bound for number of bytes read from memory for transfer to device
+u32 mite_bytes_read_from_memory_ub(struct mite_channel * mite_chan)
+{
+	u32 in_transit_count;
+
+	in_transit_count = mite_bytes_in_transit(mite_chan);
+	return mite_device_bytes_transferred(mite_chan) + in_transit_count;
+}
+
+unsigned mite_dma_tcr(struct mite_channel *mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	int tcr;
+	int lkar;
+
+	lkar = readl(mite->mite_io_addr + MITE_LKAR(mite_chan->channel));
+	tcr = readl(mite->mite_io_addr + MITE_TCR(mite_chan->channel));
+	MDPRINTK("mite_dma_tcr ch%i, lkar=0x%08x tcr=%d\n", mite_chan->channel,
+		lkar, tcr);
+
+	return tcr;
+}
+
+void mite_dma_disarm(struct mite_channel *mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	unsigned chor;
+
+	/* disarm */
+	chor = CHOR_ABORT;
+	writel(chor, mite->mite_io_addr + MITE_CHOR(mite_chan->channel));
+}
+
+int mite_sync_input_dma(struct mite_channel *mite_chan, comedi_async * async)
+{
+	int count;
+	unsigned int nbytes, old_alloc_count;
+	const unsigned bytes_per_scan = cfc_bytes_per_scan(async->subdevice);
+
+	old_alloc_count = async->buf_write_alloc_count;
+	// write alloc as much as we can
+	comedi_buf_write_alloc(async, async->prealloc_bufsz);
+
+	nbytes = mite_bytes_written_to_memory_lb(mite_chan);
+	if ((int)(mite_bytes_written_to_memory_ub(mite_chan) -
+			old_alloc_count) > 0) {
+		rt_printk("mite: DMA overwrite of free area\n");
+		async->events |= COMEDI_CB_OVERFLOW;
+		return -1;
+	}
+
+	count = nbytes - async->buf_write_count;
+	/* it's possible count will be negative due to
+	 * conservative value returned by mite_bytes_written_to_memory_lb */
+	if (count <= 0) {
+		return 0;
+	}
+	comedi_buf_write_free(async, count);
+
+	async->scan_progress += count;
+	if (async->scan_progress >= bytes_per_scan) {
+		async->scan_progress %= bytes_per_scan;
+		async->events |= COMEDI_CB_EOS;
+	}
+	async->events |= COMEDI_CB_BLOCK;
+	return 0;
+}
+
+int mite_sync_output_dma(struct mite_channel *mite_chan, comedi_async * async)
+{
+	int count;
+	u32 nbytes_ub, nbytes_lb;
+	unsigned int old_alloc_count;
+	u32 stop_count =
+		async->cmd.stop_arg * cfc_bytes_per_scan(async->subdevice);
+
+	old_alloc_count = async->buf_read_alloc_count;
+	// read alloc as much as we can
+	comedi_buf_read_alloc(async, 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)
+		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)
+		nbytes_ub = stop_count;
+	if ((int)(nbytes_ub - old_alloc_count) > 0) {
+		rt_printk("mite: DMA underrun\n");
+		async->events |= COMEDI_CB_OVERFLOW;
+		return -1;
+	}
+	count = nbytes_lb - async->buf_read_count;
+	if (count <= 0) {
+		return 0;
+	}
+	if (count) {
+		comedi_buf_read_free(async, count);
+		async->events |= COMEDI_CB_BLOCK;
+	}
+	return 0;
+}
+
+unsigned mite_get_status(struct mite_channel *mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	unsigned status;
+	unsigned long flags;
+
+	comedi_spin_lock_irqsave(&mite->lock, flags);
+	status = readl(mite->mite_io_addr + MITE_CHSR(mite_chan->channel));
+	if (status & CHSR_DONE) {
+		mite_chan->done = 1;
+		writel(CHOR_CLRDONE,
+			mite->mite_io_addr + MITE_CHOR(mite_chan->channel));
+	}
+	mmiowb();
+	comedi_spin_unlock_irqrestore(&mite->lock, flags);
+	return status;
+}
+
+int mite_done(struct mite_channel *mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	unsigned long flags;
+	int done;
+
+	mite_get_status(mite_chan);
+	comedi_spin_lock_irqsave(&mite->lock, flags);
+	done = mite_chan->done;
+	comedi_spin_unlock_irqrestore(&mite->lock, flags);
+	return done;
+}
+
+#ifdef DEBUG_MITE
+
+static void mite_decode(char **bit_str, unsigned int bits);
+
+/* names of bits in mite registers */
+
+static const char *const mite_CHOR_strings[] = {
+	"start", "cont", "stop", "abort",
+	"freset", "clrlc", "clrrb", "clrdone",
+	"clr_lpause", "set_lpause", "clr_send_tc",
+	"set_send_tc", "12", "13", "14",
+	"15", "16", "17", "18",
+	"19", "20", "21", "22",
+	"23", "24", "25", "26",
+	"27", "28", "29", "30",
+	"dmareset",
+};
+
+static const char *const mite_CHCR_strings[] = {
+	"continue", "ringbuff", "2", "3",
+	"4", "5", "6", "7",
+	"8", "9", "10", "11",
+	"12", "13", "bursten", "fifodis",
+	"clr_cont_rb_ie", "set_cont_rb_ie", "clr_lc_ie", "set_lc_ie",
+	"clr_drdy_ie", "set_drdy_ie", "clr_mrdy_ie", "set_mrdy_ie",
+	"clr_done_ie", "set_done_ie", "clr_sar_ie", "set_sar_ie",
+	"clr_linkp_ie", "set_linkp_ie", "clr_dma_ie", "set_dma_ie",
+};
+
+static const char *const mite_MCR_strings[] = {
+	"amdevice", "1", "2", "3",
+	"4", "5", "portio", "portvxi",
+	"psizebyte", "psizehalf (byte & half = word)", "aseqxp1", "11",
+	"12", "13", "blocken", "berhand",
+	"reqsintlim/reqs0", "reqs1", "reqs2", "rd32",
+	"rd512", "rl1", "rl2", "rl8",
+	"24", "25", "26", "27",
+	"28", "29", "30", "stopen",
+};
+
+static const char *const mite_DCR_strings[] = {
+	"amdevice", "1", "2", "3",
+	"4", "5", "portio", "portvxi",
+	"psizebyte", "psizehalf (byte & half = word)", "aseqxp1", "aseqxp2",
+	"aseqxp8", "13", "blocken", "berhand",
+	"reqsintlim", "reqs1", "reqs2", "rd32",
+	"rd512", "rl1", "rl2", "rl8",
+	"23", "24", "25", "27",
+	"28", "wsdevc", "wsdevs", "rwdevpack",
+};
+
+static const char *const mite_LKCR_strings[] = {
+	"amdevice", "1", "2", "3",
+	"4", "5", "portio", "portvxi",
+	"psizebyte", "psizehalf (byte & half = word)", "asequp", "aseqdown",
+	"12", "13", "14", "berhand",
+	"16", "17", "18", "rd32",
+	"rd512", "rl1", "rl2", "rl8",
+	"24", "25", "26", "27",
+	"28", "29", "30", "chngend",
+};
+
+static const char *const mite_CHSR_strings[] = {
+	"d.err0", "d.err1", "m.err0", "m.err1",
+	"l.err0", "l.err1", "drq0", "drq1",
+	"end", "xferr", "operr0", "operr1",
+	"stops", "habort", "sabort", "error",
+	"16", "conts_rb", "18", "linkc",
+	"20", "drdy", "22", "mrdy",
+	"24", "done", "26", "sars",
+	"28", "lpauses", "30", "int",
+};
+
+void mite_dump_regs(struct mite_channel *mite_chan)
+{
+	unsigned long mite_io_addr =
+		(unsigned long)mite_chan->mite->mite_io_addr;
+	unsigned long addr = 0;
+	unsigned long temp = 0;
+
+	printk("mite_dump_regs ch%i\n", mite_chan->channel);
+	printk("mite address is  =0x%08lx\n", mite_io_addr);
+
+	addr = mite_io_addr + MITE_CHOR(channel);
+	printk("mite status[CHOR]at 0x%08lx =0x%08lx\n", addr, temp =
+		readl(addr));
+	mite_decode(mite_CHOR_strings, temp);
+	addr = mite_io_addr + MITE_CHCR(channel);
+	printk("mite status[CHCR]at 0x%08lx =0x%08lx\n", addr, temp =
+		readl(addr));
+	mite_decode(mite_CHCR_strings, temp);
+	addr = mite_io_addr + MITE_TCR(channel);
+	printk("mite status[TCR] at 0x%08lx =0x%08x\n", addr, readl(addr));
+	addr = mite_io_addr + MITE_MCR(channel);
+	printk("mite status[MCR] at 0x%08lx =0x%08lx\n", addr, temp =
+		readl(addr));
+	mite_decode(mite_MCR_strings, temp);
+
+	addr = mite_io_addr + MITE_MAR(channel);
+	printk("mite status[MAR] at 0x%08lx =0x%08x\n", addr, readl(addr));
+	addr = mite_io_addr + MITE_DCR(channel);
+	printk("mite status[DCR] at 0x%08lx =0x%08lx\n", addr, temp =
+		readl(addr));
+	mite_decode(mite_DCR_strings, temp);
+	addr = mite_io_addr + MITE_DAR(channel);
+	printk("mite status[DAR] at 0x%08lx =0x%08x\n", addr, readl(addr));
+	addr = mite_io_addr + MITE_LKCR(channel);
+	printk("mite status[LKCR]at 0x%08lx =0x%08lx\n", addr, temp =
+		readl(addr));
+	mite_decode(mite_LKCR_strings, temp);
+	addr = mite_io_addr + MITE_LKAR(channel);
+	printk("mite status[LKAR]at 0x%08lx =0x%08x\n", addr, readl(addr));
+
+	addr = mite_io_addr + MITE_CHSR(channel);
+	printk("mite status[CHSR]at 0x%08lx =0x%08lx\n", addr, temp =
+		readl(addr));
+	mite_decode(mite_CHSR_strings, temp);
+	addr = mite_io_addr + MITE_FCR(channel);
+	printk("mite status[FCR] at 0x%08lx =0x%08x\n\n", addr, readl(addr));
+}
+
+static void mite_decode(char **bit_str, unsigned int bits)
+{
+	int i;
+
+	for (i = 31; i >= 0; i--) {
+		if (bits & (1 << i)) {
+			printk(" %s", bit_str[i]);
+		}
+	}
+	printk("\n");
+}
+#endif
+
+#ifdef MODULE
+int __init init_module(void)
+{
+	mite_init();
+	mite_list_devices();
+
+	return 0;
+}
+
+void __exit cleanup_module(void)
+{
+	mite_cleanup();
+}
+
+EXPORT_SYMBOL(mite_dma_tcr);
+EXPORT_SYMBOL(mite_dma_arm);
+EXPORT_SYMBOL(mite_dma_disarm);
+EXPORT_SYMBOL(mite_sync_input_dma);
+EXPORT_SYMBOL(mite_sync_output_dma);
+EXPORT_SYMBOL(mite_setup);
+EXPORT_SYMBOL(mite_setup2);
+EXPORT_SYMBOL(mite_unsetup);
+#if 0
+EXPORT_SYMBOL(mite_kvmem_segment_load);
+EXPORT_SYMBOL(mite_ll_from_kvmem);
+EXPORT_SYMBOL(mite_setregs);
+#endif
+EXPORT_SYMBOL(mite_devices);
+EXPORT_SYMBOL(mite_list_devices);
+EXPORT_SYMBOL(mite_request_channel_in_range);
+EXPORT_SYMBOL(mite_release_channel);
+EXPORT_SYMBOL(mite_prep_dma);
+EXPORT_SYMBOL(mite_buf_change);
+EXPORT_SYMBOL(mite_bytes_written_to_memory_lb);
+EXPORT_SYMBOL(mite_bytes_written_to_memory_ub);
+EXPORT_SYMBOL(mite_bytes_read_from_memory_lb);
+EXPORT_SYMBOL(mite_bytes_read_from_memory_ub);
+EXPORT_SYMBOL(mite_bytes_in_transit);
+EXPORT_SYMBOL(mite_get_status);
+EXPORT_SYMBOL(mite_done);
+#ifdef DEBUG_MITE
+EXPORT_SYMBOL(mite_decode);
+EXPORT_SYMBOL(mite_dump_regs);
+#endif
+
+#endif
diff --git a/drivers/staging/comedi/drivers/mite.h b/drivers/staging/comedi/drivers/mite.h
new file mode 100644
index 000000000000..b84eafa6ff2b
--- /dev/null
+++ b/drivers/staging/comedi/drivers/mite.h
@@ -0,0 +1,453 @@
+/*
+    module/mite.h
+    Hardware driver for NI Mite PCI interface chip
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1999 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _MITE_H_
+#define _MITE_H_
+
+#include <linux/pci.h>
+#include "../comedidev.h"
+
+#define PCI_VENDOR_ID_NATINST		0x1093
+
+// #define DEBUG_MITE
+#define PCIMIO_COMPAT
+
+#ifdef DEBUG_MITE
+#define MDPRINTK(format,args...)	printk(format , ## args )
+#else
+#define MDPRINTK(format,args...)
+#endif
+
+#define MAX_MITE_DMA_CHANNELS 8
+
+struct mite_dma_descriptor {
+	u32 count;
+	u32 addr;
+	u32 next;
+	u32 dar;
+};
+
+struct mite_dma_descriptor_ring {
+	struct device *hw_dev;
+	unsigned int n_links;
+	struct mite_dma_descriptor *descriptors;
+	dma_addr_t descriptors_dma_addr;
+};
+
+struct mite_channel {
+	struct mite_struct *mite;
+	unsigned channel;
+	int dir;
+	int done;
+	struct mite_dma_descriptor_ring *ring;
+};
+
+struct mite_struct {
+	struct mite_struct *next;
+	int used;
+
+	struct pci_dev *pcidev;
+	resource_size_t mite_phys_addr;
+	void *mite_io_addr;
+	resource_size_t daq_phys_addr;
+	void *daq_io_addr;
+
+	struct mite_channel channels[MAX_MITE_DMA_CHANNELS];
+	short channel_allocated[MAX_MITE_DMA_CHANNELS];
+	int num_channels;
+	unsigned fifo_size;
+	spinlock_t lock;
+};
+
+static inline struct mite_dma_descriptor_ring *mite_alloc_ring(struct
+	mite_struct *mite)
+{
+	struct mite_dma_descriptor_ring *ring =
+		kmalloc(sizeof(struct mite_dma_descriptor_ring), GFP_KERNEL);
+	if (ring == NULL)
+		return ring;
+	ring->hw_dev = get_device(&mite->pcidev->dev);
+	if (ring->hw_dev == NULL) {
+		kfree(ring);
+		return NULL;
+	}
+	ring->n_links = 0;
+	ring->descriptors = NULL;
+	ring->descriptors_dma_addr = 0;
+	return ring;
+};
+
+static inline void mite_free_ring(struct mite_dma_descriptor_ring *ring)
+{
+	if (ring) {
+		if (ring->descriptors) {
+			dma_free_coherent(ring->hw_dev,
+				ring->n_links *
+				sizeof(struct mite_dma_descriptor),
+				ring->descriptors, ring->descriptors_dma_addr);
+		}
+		put_device(ring->hw_dev);
+		kfree(ring);
+	}
+};
+
+extern struct mite_struct *mite_devices;
+
+static inline unsigned int mite_irq(struct mite_struct *mite)
+{
+	return mite->pcidev->irq;
+};
+static inline unsigned int mite_device_id(struct mite_struct *mite)
+{
+	return mite->pcidev->device;
+};
+
+void mite_init(void);
+void mite_cleanup(void);
+int mite_setup(struct mite_struct *mite);
+int mite_setup2(struct mite_struct *mite, unsigned use_iodwbsr_1);
+void mite_unsetup(struct mite_struct *mite);
+void mite_list_devices(void);
+struct mite_channel *mite_request_channel_in_range(struct mite_struct *mite,
+	struct mite_dma_descriptor_ring *ring, unsigned min_channel,
+	unsigned max_channel);
+static inline struct mite_channel *mite_request_channel(struct mite_struct
+	*mite, struct mite_dma_descriptor_ring *ring)
+{
+	return mite_request_channel_in_range(mite, ring, 0,
+		mite->num_channels - 1);
+}
+void mite_release_channel(struct mite_channel *mite_chan);
+
+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, comedi_async * async);
+int mite_sync_output_dma(struct mite_channel *mite_chan, comedi_async * async);
+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);
+u32 mite_bytes_read_from_memory_ub(struct mite_channel *mite_chan);
+u32 mite_bytes_in_transit(struct mite_channel *mite_chan);
+unsigned mite_get_status(struct mite_channel *mite_chan);
+int mite_done(struct mite_channel *mite_chan);
+
+#if 0
+unsigned long mite_ll_from_kvmem(struct mite_struct *mite, comedi_async * async,
+	int len);
+void mite_setregs(struct mite_struct *mite, unsigned long ll_start, int chan,
+	int dir);
+#endif
+
+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,
+	comedi_async * async);
+
+#ifdef DEBUG_MITE
+void mite_print_chsr(unsigned int chsr);
+void mite_dump_regs(struct mite_channel *mite_chan);
+#endif
+
+static inline int CHAN_OFFSET(int channel)
+{
+	return 0x500 + 0x100 * channel;
+};
+
+enum mite_registers {
+	/* The bits 0x90180700 in MITE_UNKNOWN_DMA_BURST_REG can be
+	   written and read back.  The bits 0x1f always read as 1.
+	   The rest always read as zero. */
+	MITE_UNKNOWN_DMA_BURST_REG = 0x28,
+	MITE_IODWBSR = 0xc0,	//IO Device Window Base Size Register
+	MITE_IODWBSR_1 = 0xc4,	// IO Device Window Base Size Register 1
+	MITE_IODWCR_1 = 0xf4,
+	MITE_PCI_CONFIG_OFFSET = 0x300,
+	MITE_CSIGR = 0x460	//chip signature
+};
+static inline int MITE_CHOR(int channel)	// channel operation
+{
+	return CHAN_OFFSET(channel) + 0x0;
+};
+static inline int MITE_CHCR(int channel)	// channel control
+{
+	return CHAN_OFFSET(channel) + 0x4;
+};
+static inline int MITE_TCR(int channel)	// transfer count
+{
+	return CHAN_OFFSET(channel) + 0x8;
+};
+static inline int MITE_MCR(int channel)	// memory configuration
+{
+	return CHAN_OFFSET(channel) + 0xc;
+};
+static inline int MITE_MAR(int channel)	// memory address
+{
+	return CHAN_OFFSET(channel) + 0x10;
+};
+static inline int MITE_DCR(int channel)	// device configuration
+{
+	return CHAN_OFFSET(channel) + 0x14;
+};
+static inline int MITE_DAR(int channel)	// device address
+{
+	return CHAN_OFFSET(channel) + 0x18;
+};
+static inline int MITE_LKCR(int channel)	// link configuration
+{
+	return CHAN_OFFSET(channel) + 0x1c;
+};
+static inline int MITE_LKAR(int channel)	// link address
+{
+	return CHAN_OFFSET(channel) + 0x20;
+};
+static inline int MITE_LLKAR(int channel)	// see mite section of tnt5002 manual
+{
+	return CHAN_OFFSET(channel) + 0x24;
+};
+static inline int MITE_BAR(int channel)	// base address
+{
+	return CHAN_OFFSET(channel) + 0x28;
+};
+static inline int MITE_BCR(int channel)	// base count
+{
+	return CHAN_OFFSET(channel) + 0x2c;
+};
+static inline int MITE_SAR(int channel)	// ? address
+{
+	return CHAN_OFFSET(channel) + 0x30;
+};
+static inline int MITE_WSCR(int channel)	// ?
+{
+	return CHAN_OFFSET(channel) + 0x34;
+};
+static inline int MITE_WSER(int channel)	// ?
+{
+	return CHAN_OFFSET(channel) + 0x38;
+};
+static inline int MITE_CHSR(int channel)	// channel status
+{
+	return CHAN_OFFSET(channel) + 0x3c;
+};
+static inline int MITE_FCR(int channel)	// fifo count
+{
+	return CHAN_OFFSET(channel) + 0x40;
+};
+
+enum MITE_IODWBSR_bits {
+	WENAB = 0x80,		// window enable
+};
+
+static inline unsigned MITE_IODWBSR_1_WSIZE_bits(unsigned size)
+{
+	unsigned order = 0;
+	while (size >>= 1)
+		++order;
+	BUG_ON(order < 1);
+	return (order - 1) & 0x1f;
+}
+
+enum MITE_UNKNOWN_DMA_BURST_bits {
+	UNKNOWN_DMA_BURST_ENABLE_BITS = 0x600
+};
+
+static inline int mite_csigr_version(u32 csigr_bits)
+{
+	return csigr_bits & 0xf;
+};
+static inline int mite_csigr_type(u32 csigr_bits)
+{				// original mite = 0, minimite = 1
+	return (csigr_bits >> 4) & 0xf;
+};
+static inline int mite_csigr_mmode(u32 csigr_bits)
+{				// mite mode, minimite = 1
+	return (csigr_bits >> 8) & 0x3;
+};
+static inline int mite_csigr_imode(u32 csigr_bits)
+{				// cpu port interface mode, pci = 0x3
+	return (csigr_bits >> 12) & 0x3;
+};
+static inline int mite_csigr_dmac(u32 csigr_bits)
+{				// number of dma channels
+	return (csigr_bits >> 16) & 0xf;
+};
+static inline int mite_csigr_wpdep(u32 csigr_bits)
+{				// write post fifo depth
+	unsigned int wpdep_bits = (csigr_bits >> 20) & 0x7;
+	if (wpdep_bits == 0)
+		return 0;
+	else
+		return 1 << (wpdep_bits - 1);
+};
+static inline int mite_csigr_wins(u32 csigr_bits)
+{
+	return (csigr_bits >> 24) & 0x1f;
+};
+static inline int mite_csigr_iowins(u32 csigr_bits)
+{				// number of io windows
+	return (csigr_bits >> 29) & 0x7;
+};
+
+enum MITE_MCR_bits {
+	MCRPON = 0,
+};
+
+enum MITE_DCR_bits {
+	DCR_NORMAL = (1 << 29),
+	DCRPON = 0,
+};
+
+enum MITE_CHOR_bits {
+	CHOR_DMARESET = (1 << 31),
+	CHOR_SET_SEND_TC = (1 << 11),
+	CHOR_CLR_SEND_TC = (1 << 10),
+	CHOR_SET_LPAUSE = (1 << 9),
+	CHOR_CLR_LPAUSE = (1 << 8),
+	CHOR_CLRDONE = (1 << 7),
+	CHOR_CLRRB = (1 << 6),
+	CHOR_CLRLC = (1 << 5),
+	CHOR_FRESET = (1 << 4),
+	CHOR_ABORT = (1 << 3),	/* stop without emptying fifo */
+	CHOR_STOP = (1 << 2),	/* stop after emptying fifo */
+	CHOR_CONT = (1 << 1),
+	CHOR_START = (1 << 0),
+	CHOR_PON = (CHOR_CLR_SEND_TC | CHOR_CLR_LPAUSE),
+};
+
+enum MITE_CHCR_bits {
+	CHCR_SET_DMA_IE = (1 << 31),
+	CHCR_CLR_DMA_IE = (1 << 30),
+	CHCR_SET_LINKP_IE = (1 << 29),
+	CHCR_CLR_LINKP_IE = (1 << 28),
+	CHCR_SET_SAR_IE = (1 << 27),
+	CHCR_CLR_SAR_IE = (1 << 26),
+	CHCR_SET_DONE_IE = (1 << 25),
+	CHCR_CLR_DONE_IE = (1 << 24),
+	CHCR_SET_MRDY_IE = (1 << 23),
+	CHCR_CLR_MRDY_IE = (1 << 22),
+	CHCR_SET_DRDY_IE = (1 << 21),
+	CHCR_CLR_DRDY_IE = (1 << 20),
+	CHCR_SET_LC_IE = (1 << 19),
+	CHCR_CLR_LC_IE = (1 << 18),
+	CHCR_SET_CONT_RB_IE = (1 << 17),
+	CHCR_CLR_CONT_RB_IE = (1 << 16),
+	CHCR_FIFODIS = (1 << 15),
+	CHCR_FIFO_ON = 0,
+	CHCR_BURSTEN = (1 << 14),
+	CHCR_NO_BURSTEN = 0,
+	CHCR_BYTE_SWAP_DEVICE = (1 << 6),
+	CHCR_BYTE_SWAP_MEMORY = (1 << 4),
+	CHCR_DIR = (1 << 3),
+	CHCR_DEV_TO_MEM = CHCR_DIR,
+	CHCR_MEM_TO_DEV = 0,
+	CHCR_NORMAL = (0 << 0),
+	CHCR_CONTINUE = (1 << 0),
+	CHCR_RINGBUFF = (2 << 0),
+	CHCR_LINKSHORT = (4 << 0),
+	CHCR_LINKLONG = (5 << 0),
+	CHCRPON =
+		(CHCR_CLR_DMA_IE | CHCR_CLR_LINKP_IE | CHCR_CLR_SAR_IE |
+		CHCR_CLR_DONE_IE | CHCR_CLR_MRDY_IE | CHCR_CLR_DRDY_IE |
+		CHCR_CLR_LC_IE | CHCR_CLR_CONT_RB_IE),
+};
+
+enum ConfigRegister_bits {
+	CR_REQS_MASK = 0x7 << 16,
+	CR_ASEQDONT = 0x0 << 10,
+	CR_ASEQUP = 0x1 << 10,
+	CR_ASEQDOWN = 0x2 << 10,
+	CR_ASEQ_MASK = 0x3 << 10,
+	CR_PSIZE8 = (1 << 8),
+	CR_PSIZE16 = (2 << 8),
+	CR_PSIZE32 = (3 << 8),
+	CR_PORTCPU = (0 << 6),
+	CR_PORTIO = (1 << 6),
+	CR_PORTVXI = (2 << 6),
+	CR_PORTMXI = (3 << 6),
+	CR_AMDEVICE = (1 << 0),
+};
+static inline int CR_REQS(int source)
+{
+	return (source & 0x7) << 16;
+};
+static inline int CR_REQSDRQ(unsigned drq_line)
+{
+	/* This also works on m-series when
+	   using channels (drq_line) 4 or 5. */
+	return CR_REQS((drq_line & 0x3) | 0x4);
+}
+static inline int CR_RL(unsigned int retry_limit)
+{
+	int value = 0;
+
+	while (retry_limit) {
+		retry_limit >>= 1;
+		value++;
+	}
+	if (value > 0x7)
+		rt_printk("comedi: bug! retry_limit too large\n");
+	return (value & 0x7) << 21;
+}
+
+enum CHSR_bits {
+	CHSR_INT = (1 << 31),
+	CHSR_LPAUSES = (1 << 29),
+	CHSR_SARS = (1 << 27),
+	CHSR_DONE = (1 << 25),
+	CHSR_MRDY = (1 << 23),
+	CHSR_DRDY = (1 << 21),
+	CHSR_LINKC = (1 << 19),
+	CHSR_CONTS_RB = (1 << 17),
+	CHSR_ERROR = (1 << 15),
+	CHSR_SABORT = (1 << 14),
+	CHSR_HABORT = (1 << 13),
+	CHSR_STOPS = (1 << 12),
+	CHSR_OPERR_mask = (3 << 10),
+	CHSR_OPERR_NOERROR = (0 << 10),
+	CHSR_OPERR_FIFOERROR = (1 << 10),
+	CHSR_OPERR_LINKERROR = (1 << 10),	/* ??? */
+	CHSR_XFERR = (1 << 9),
+	CHSR_END = (1 << 8),
+	CHSR_DRQ1 = (1 << 7),
+	CHSR_DRQ0 = (1 << 6),
+	CHSR_LxERR_mask = (3 << 4),
+	CHSR_LBERR = (1 << 4),
+	CHSR_LRERR = (2 << 4),
+	CHSR_LOERR = (3 << 4),
+	CHSR_MxERR_mask = (3 << 2),
+	CHSR_MBERR = (1 << 2),
+	CHSR_MRERR = (2 << 2),
+	CHSR_MOERR = (3 << 2),
+	CHSR_DxERR_mask = (3 << 0),
+	CHSR_DBERR = (1 << 0),
+	CHSR_DRERR = (2 << 0),
+	CHSR_DOERR = (3 << 0),
+};
+
+static inline void mite_dma_reset(struct mite_channel *mite_chan)
+{
+	writel(CHOR_DMARESET | CHOR_FRESET,
+		mite_chan->mite->mite_io_addr + MITE_CHOR(mite_chan->channel));
+};
+
+#endif
diff --git a/drivers/staging/comedi/drivers/plx9080.h b/drivers/staging/comedi/drivers/plx9080.h
new file mode 100644
index 000000000000..a5a1a6808c5d
--- /dev/null
+++ b/drivers/staging/comedi/drivers/plx9080.h
@@ -0,0 +1,429 @@
+/* plx9080.h
+ *
+ * Copyright (C) 2002,2003 Frank Mori Hess <fmhess@users.sourceforge.net>
+ *
+ * I modified this file from the plx9060.h header for the
+ * wanXL device driver in the linux kernel,
+ * for the register offsets and bit definitions.  Made minor modifications,
+ * added plx9080 registers and
+ * stripped out stuff that was specifically for the wanXL driver.
+ * Note: I've only made sure the definitions are correct as far
+ * as I make use of them.  There are still various plx9060-isms
+ * left in this header file.
+ *
+ ********************************************************************
+ *
+ * Copyright (C) 1999 RG Studio s.c., http://www.rgstudio.com.pl/
+ * Written by Krzysztof Halasa <khc@rgstudio.com.pl>
+ *
+ * Portions (C) SBE Inc., used by permission.
+ *
+ * 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 __COMEDI_PLX9080_H
+#define __COMEDI_PLX9080_H
+
+// descriptor block used for chained dma transfers
+struct plx_dma_desc {
+	volatile uint32_t pci_start_addr;
+	volatile uint32_t local_start_addr;
+	/* transfer_size is in bytes, only first 23 bits of register are used */
+	volatile uint32_t transfer_size;
+	/* address of next descriptor (quad word aligned), plus some
+	 * additional bits (see PLX_DMA0_DESCRIPTOR_REG) */
+	volatile uint32_t next;
+};
+
+/**********************************************************************
+**            Register Offsets and Bit Definitions
+**
+** Note: All offsets zero relative.  IE. Some standard base address
+** must be added to the Register Number to properly access the register.
+**
+**********************************************************************/
+
+#define PLX_LAS0RNG_REG         0x0000	/* L, Local Addr Space 0 Range Register */
+#define PLX_LAS1RNG_REG         0x00f0	/* L, Local Addr Space 1 Range Register */
+#define  LRNG_IO           0x00000001	/* Map to: 1=I/O, 0=Mem */
+#define  LRNG_ANY32        0x00000000	/* Locate anywhere in 32 bit */
+#define  LRNG_LT1MB        0x00000002	/* Locate in 1st meg */
+#define  LRNG_ANY64        0x00000004	/* Locate anywhere in 64 bit */
+#define  LRNG_MEM_MASK     0xfffffff0	// bits that specify range for memory io
+#define  LRNG_IO_MASK     0xfffffffa	// bits that specify range for normal io
+
+#define PLX_LAS0MAP_REG         0x0004	/* L, Local Addr Space 0 Remap Register */
+#define PLX_LAS1MAP_REG         0x00f4	/* L, Local Addr Space 1 Remap Register */
+#define  LMAP_EN           0x00000001	/* Enable slave decode */
+#define  LMAP_MEM_MASK     0xfffffff0	// bits that specify decode for memory io
+#define  LMAP_IO_MASK     0xfffffffa	// bits that specify decode bits for normal io
+
+/* Mode/Arbitration Register.
+*/
+#define PLX_MARB_REG         0x8	/* L, Local Arbitration Register */
+#define PLX_DMAARB_REG      0xac
+enum marb_bits {
+	MARB_LLT_MASK = 0x000000ff,	/* Local Bus Latency Timer */
+	MARB_LPT_MASK = 0x0000ff00,	/* Local Bus Pause Timer */
+	MARB_LTEN = 0x00010000,	/* Latency Timer Enable */
+	MARB_LPEN = 0x00020000,	/* Pause Timer Enable */
+	MARB_BREQ = 0x00040000,	/* Local Bus BREQ Enable */
+	MARB_DMA_PRIORITY_MASK = 0x00180000,
+	MARB_LBDS_GIVE_UP_BUS_MODE = 0x00200000,	/* local bus direct slave give up bus mode */
+	MARB_DS_LLOCK_ENABLE = 0x00400000,	/* direct slave LLOCKo# enable */
+	MARB_PCI_REQUEST_MODE = 0x00800000,
+	MARB_PCIv21_MODE = 0x01000000,	/* pci specification v2.1 mode */
+	MARB_PCI_READ_NO_WRITE_MODE = 0x02000000,
+	MARB_PCI_READ_WITH_WRITE_FLUSH_MODE = 0x04000000,
+	MARB_GATE_TIMER_WITH_BREQ = 0x08000000,	/* gate local bus latency timer with BREQ */
+	MARB_PCI_READ_NO_FLUSH_MODE = 0x10000000,
+	MARB_USE_SUBSYSTEM_IDS = 0x20000000,
+};
+
+#define PLX_BIGEND_REG 0xc
+enum bigend_bits {
+	BIGEND_CONFIG = 0x1,	/* use big endian ordering for configuration register accesses */
+	BIGEND_DIRECT_MASTER = 0x2,
+	BIGEND_DIRECT_SLAVE_LOCAL0 = 0x4,
+	BIGEND_ROM = 0x8,
+	BIGEND_BYTE_LANE = 0x10,	/* use byte lane consisting of most significant bits instead of least significant */
+	BIGEND_DIRECT_SLAVE_LOCAL1 = 0x20,
+	BIGEND_DMA1 = 0x40,
+	BIGEND_DMA0 = 0x80,
+};
+
+/* Note: The Expansion ROM  stuff is only relevant to the PC environment.
+**       This expansion ROM code is executed by the host CPU at boot time.
+**       For this reason no bit definitions are provided here.
+*/
+#define PLX_ROMRNG_REG         0x0010	/* L, Expn ROM Space Range Register */
+#define PLX_ROMMAP_REG         0x0014	/* L, Local Addr Space Range Register */
+
+#define PLX_REGION0_REG         0x0018	/* L, Local Bus Region 0 Descriptor */
+#define  RGN_WIDTH         0x00000002	/* Local bus width bits */
+#define  RGN_8BITS         0x00000000	/* 08 bit Local Bus */
+#define  RGN_16BITS        0x00000001	/* 16 bit Local Bus */
+#define  RGN_32BITS        0x00000002	/* 32 bit Local Bus */
+#define  RGN_MWS           0x0000003C	/* Memory Access Wait States */
+#define  RGN_0MWS          0x00000000
+#define  RGN_1MWS          0x00000004
+#define  RGN_2MWS          0x00000008
+#define  RGN_3MWS          0x0000000C
+#define  RGN_4MWS          0x00000010
+#define  RGN_6MWS          0x00000018
+#define  RGN_8MWS          0x00000020
+#define  RGN_MRE           0x00000040	/* Memory Space Ready Input Enable */
+#define  RGN_MBE           0x00000080	/* Memory Space Bterm Input Enable */
+#define  RGN_READ_PREFETCH_DISABLE 0x00000100
+#define  RGN_ROM_PREFETCH_DISABLE 0x00000200
+#define  RGN_READ_PREFETCH_COUNT_ENABLE 0x00000400
+#define  RGN_RWS           0x003C0000	/* Expn ROM Wait States */
+#define  RGN_RRE           0x00400000	/* ROM Space Ready Input Enable */
+#define  RGN_RBE           0x00800000	/* ROM Space Bterm Input Enable */
+#define  RGN_MBEN          0x01000000	/* Memory Space Burst Enable */
+#define  RGN_RBEN          0x04000000	/* ROM Space Burst Enable */
+#define  RGN_THROT         0x08000000	/* De-assert TRDY when FIFO full */
+#define  RGN_TRD           0xF0000000	/* Target Ready Delay /8 */
+
+#define PLX_REGION1_REG         0x00f8	/* L, Local Bus Region 1 Descriptor */
+
+#define PLX_DMRNG_REG          0x001C	/* L, Direct Master Range Register */
+
+#define PLX_LBAPMEM_REG        0x0020	/* L, Lcl Base Addr for PCI mem space */
+
+#define PLX_LBAPIO_REG         0x0024	/* L, Lcl Base Addr for PCI I/O space */
+
+#define PLX_DMMAP_REG          0x0028	/* L, Direct Master Remap Register */
+#define  DMM_MAE           0x00000001	/* Direct Mstr Memory Acc Enable */
+#define  DMM_IAE           0x00000002	/* Direct Mstr I/O Acc Enable */
+#define  DMM_LCK           0x00000004	/* LOCK Input Enable */
+#define  DMM_PF4           0x00000008	/* Prefetch 4 Mode Enable */
+#define  DMM_THROT         0x00000010	/* Assert IRDY when read FIFO full */
+#define  DMM_PAF0          0x00000000	/* Programmable Almost fill level */
+#define  DMM_PAF1          0x00000020	/* Programmable Almost fill level */
+#define  DMM_PAF2          0x00000040	/* Programmable Almost fill level */
+#define  DMM_PAF3          0x00000060	/* Programmable Almost fill level */
+#define  DMM_PAF4          0x00000080	/* Programmable Almost fill level */
+#define  DMM_PAF5          0x000000A0	/* Programmable Almost fill level */
+#define  DMM_PAF6          0x000000C0	/* Programmable Almost fill level */
+#define  DMM_PAF7          0x000000D0	/* Programmable Almost fill level */
+#define  DMM_MAP           0xFFFF0000	/* Remap Address Bits */
+
+#define PLX_CAR_REG            0x002C	/* L, Configuration Address Register */
+#define  CAR_CT0           0x00000000	/* Config Type 0 */
+#define  CAR_CT1           0x00000001	/* Config Type 1 */
+#define  CAR_REG           0x000000FC	/* Register Number Bits */
+#define  CAR_FUN           0x00000700	/* Function Number Bits */
+#define  CAR_DEV           0x0000F800	/* Device Number Bits */
+#define  CAR_BUS           0x00FF0000	/* Bus Number Bits */
+#define  CAR_CFG           0x80000000	/* Config Spc Access Enable */
+
+#define PLX_DBR_IN_REG         0x0060	/* L, PCI to Local Doorbell Register */
+
+#define PLX_DBR_OUT_REG        0x0064	/* L, Local to PCI Doorbell Register */
+
+#define PLX_INTRCS_REG         0x0068	/* L, Interrupt Control/Status Reg */
+#define  ICS_AERR          0x00000001	/* Assert LSERR on ABORT */
+#define  ICS_PERR          0x00000002	/* Assert LSERR on Parity Error */
+#define  ICS_SERR          0x00000004	/* Generate PCI SERR# */
+#define  ICS_MBIE          0x00000008	// mailbox interrupt enable
+#define  ICS_PIE           0x00000100	/* PCI Interrupt Enable */
+#define  ICS_PDIE          0x00000200	/* PCI Doorbell Interrupt Enable */
+#define  ICS_PAIE          0x00000400	/* PCI Abort Interrupt Enable */
+#define  ICS_PLIE          0x00000800	/* PCI Local Int Enable */
+#define  ICS_RAE           0x00001000	/* Retry Abort Enable */
+#define  ICS_PDIA          0x00002000	/* PCI Doorbell Interrupt Active */
+#define  ICS_PAIA          0x00004000	/* PCI Abort Interrupt Active */
+#define  ICS_LIA           0x00008000	/* Local Interrupt Active */
+#define  ICS_LIE           0x00010000	/* Local Interrupt Enable */
+#define  ICS_LDIE          0x00020000	/* Local Doorbell Int Enable */
+#define  ICS_DMA0_E        0x00040000	/* DMA #0 Interrupt Enable */
+#define  ICS_DMA1_E        0x00080000	/* DMA #1 Interrupt Enable */
+#define  ICS_LDIA          0x00100000	/* Local Doorbell Int Active */
+#define  ICS_DMA0_A        0x00200000	/* DMA #0 Interrupt Active */
+#define  ICS_DMA1_A        0x00400000	/* DMA #1 Interrupt Active */
+#define  ICS_BIA           0x00800000	/* BIST Interrupt Active */
+#define  ICS_TA_DM         0x01000000	/* Target Abort - Direct Master */
+#define  ICS_TA_DMA0       0x02000000	/* Target Abort - DMA #0 */
+#define  ICS_TA_DMA1       0x04000000	/* Target Abort - DMA #1 */
+#define  ICS_TA_RA         0x08000000	/* Target Abort - Retry Timeout */
+#define  ICS_MBIA(x)       (0x10000000 << ((x) & 0x3))	// mailbox x is active
+
+#define PLX_CONTROL_REG        0x006C	/* L, EEPROM Cntl & PCI Cmd Codes */
+#define  CTL_RDMA          0x0000000E	/* DMA Read Command */
+#define  CTL_WDMA          0x00000070	/* DMA Write Command */
+#define  CTL_RMEM          0x00000600	/* Memory Read Command */
+#define  CTL_WMEM          0x00007000	/* Memory Write Command */
+#define  CTL_USERO         0x00010000	/* USERO output pin control bit */
+#define  CTL_USERI         0x00020000	/* USERI input pin bit */
+#define  CTL_EE_CLK        0x01000000	/* EEPROM Clock line */
+#define  CTL_EE_CS         0x02000000	/* EEPROM Chip Select */
+#define  CTL_EE_W          0x04000000	/* EEPROM Write bit */
+#define  CTL_EE_R          0x08000000	/* EEPROM Read bit */
+#define  CTL_EECHK         0x10000000	/* EEPROM Present bit */
+#define  CTL_EERLD         0x20000000	/* EEPROM Reload Register */
+#define  CTL_RESET         0x40000000	/* !! Adapter Reset !! */
+#define  CTL_READY         0x80000000	/* Local Init Done */
+
+#define PLX_ID_REG	0x70	// hard-coded plx vendor and device ids
+
+#define PLX_REVISION_REG	0x74	// silicon revision
+
+#define PLX_DMA0_MODE_REG	0x80	// dma channel 0 mode register
+#define PLX_DMA1_MODE_REG	0x94	// dma channel 0 mode register
+#define  PLX_LOCAL_BUS_16_WIDE_BITS	0x1
+#define  PLX_LOCAL_BUS_32_WIDE_BITS	0x3
+#define  PLX_LOCAL_BUS_WIDTH_MASK	0x3
+#define  PLX_DMA_EN_READYIN_BIT	0x40	// enable ready in input
+#define  PLX_EN_BTERM_BIT	0x80	// enable BTERM# input
+#define  PLX_DMA_LOCAL_BURST_EN_BIT	0x100	// enable local burst mode
+#define  PLX_EN_CHAIN_BIT	0x200	// enables chaining
+#define  PLX_EN_DMA_DONE_INTR_BIT	0x400	// enables interrupt on dma done
+#define  PLX_LOCAL_ADDR_CONST_BIT	0x800	// hold local address constant (don't increment)
+#define  PLX_DEMAND_MODE_BIT	0x1000	// enables demand-mode for dma transfer
+#define  PLX_EOT_ENABLE_BIT	0x4000
+#define  PLX_STOP_MODE_BIT 0x8000
+#define  PLX_DMA_INTR_PCI_BIT	0x20000	// routes dma interrupt to pci bus (instead of local bus)
+
+#define PLX_DMA0_PCI_ADDRESS_REG	0x84	// pci address that dma transfers start at
+#define PLX_DMA1_PCI_ADDRESS_REG	0x98
+
+#define PLX_DMA0_LOCAL_ADDRESS_REG	0x88	// local address that dma transfers start at
+#define PLX_DMA1_LOCAL_ADDRESS_REG	0x9c
+
+#define PLX_DMA0_TRANSFER_SIZE_REG	0x8c	// number of bytes to transfer (first 23 bits)
+#define PLX_DMA1_TRANSFER_SIZE_REG	0xa0
+
+#define PLX_DMA0_DESCRIPTOR_REG	0x90	// descriptor pointer register
+#define PLX_DMA1_DESCRIPTOR_REG	0xa4
+#define  PLX_DESC_IN_PCI_BIT	0x1	// descriptor is located in pci space (not local space)
+#define  PLX_END_OF_CHAIN_BIT	0x2	// end of chain bit
+#define  PLX_INTR_TERM_COUNT	0x4	// interrupt when this descriptor's transfer is finished
+#define  PLX_XFER_LOCAL_TO_PCI 0x8	// transfer from local to pci bus (not pci to local)
+
+#define PLX_DMA0_CS_REG	0xa8	// command status register
+#define PLX_DMA1_CS_REG	0xa9
+#define  PLX_DMA_EN_BIT	0x1	// enable dma channel
+#define  PLX_DMA_START_BIT	0x2	// start dma transfer
+#define  PLX_DMA_ABORT_BIT	0x4	// abort dma transfer
+#define  PLX_CLEAR_DMA_INTR_BIT	0x8	// clear dma interrupt
+#define  PLX_DMA_DONE_BIT	0x10	// transfer done status bit
+
+#define PLX_DMA0_THRESHOLD_REG	0xb0	// command status register
+
+/*
+ * Accesses near the end of memory can cause the PLX chip
+ * to pre-fetch data off of end-of-ram.  Limit the size of
+ * memory so host-side accesses cannot occur.
+ */
+
+#define PLX_PREFETCH   32
+
+/*
+ * The PCI Interface, via the PCI-9060 Chip, has up to eight (8) Mailbox
+ * Registers.  The PUTS (Power-Up Test Suite) handles the board-side
+ * interface/interaction using the first 4 registers.  Specifications for
+ * the use of the full PUTS' command and status interface is contained
+ * within a separate SBE PUTS Manual.  The Host-Side Device Driver only
+ * uses a subset of the full PUTS interface.
+ */
+
+/*****************************************/
+/***    MAILBOX #(-1) - MEM ACCESS STS ***/
+/*****************************************/
+
+#define MBX_STS_VALID      0x57584744	/* 'WXGD' */
+#define MBX_STS_DILAV      0x44475857	/* swapped = 'DGXW' */
+
+/*****************************************/
+/***    MAILBOX #0  -  PUTS STATUS     ***/
+/*****************************************/
+
+#define MBX_STS_MASK       0x000000ff	/* PUTS Status Register bits */
+#define MBX_STS_TMASK      0x0000000f	/* register bits for TEST number */
+
+#define MBX_STS_PCIRESET   0x00000100	/* Host issued PCI reset request */
+#define MBX_STS_BUSY       0x00000080	/* PUTS is in progress */
+#define MBX_STS_ERROR      0x00000040	/* PUTS has failed */
+#define MBX_STS_RESERVED   0x000000c0	/* Undefined -> status in transition.
+					   We are in process of changing
+					   bits; we SET Error bit before
+					   RESET of Busy bit */
+
+#define MBX_RESERVED_5     0x00000020	/* FYI: reserved/unused bit */
+#define MBX_RESERVED_4     0x00000010	/* FYI: reserved/unused bit */
+
+/******************************************/
+/***    MAILBOX #1  -  PUTS COMMANDS    ***/
+/******************************************/
+
+/*
+ * Any attempt to execute an unimplement command results in the PUTS
+ * interface executing a NOOP and continuing as if the offending command
+ * completed normally.  Note: this supplies a simple method to interrogate
+ * mailbox command processing functionality.
+ */
+
+#define MBX_CMD_MASK       0xffff0000	/* PUTS Command Register bits */
+
+#define MBX_CMD_ABORTJ     0x85000000	/* abort and jump */
+#define MBX_CMD_RESETP     0x86000000	/* reset and pause at start */
+#define MBX_CMD_PAUSE      0x87000000	/* pause immediately */
+#define MBX_CMD_PAUSEC     0x88000000	/* pause on completion */
+#define MBX_CMD_RESUME     0x89000000	/* resume operation */
+#define MBX_CMD_STEP       0x8a000000	/* single step tests */
+
+#define MBX_CMD_BSWAP      0x8c000000	/* identify byte swap scheme */
+#define MBX_CMD_BSWAP_0    0x8c000000	/* use scheme 0 */
+#define MBX_CMD_BSWAP_1    0x8c000001	/* use scheme 1 */
+
+#define MBX_CMD_SETHMS     0x8d000000	/* setup host memory access window
+					   size */
+#define MBX_CMD_SETHBA     0x8e000000	/* setup host memory access base
+					   address */
+#define MBX_CMD_MGO        0x8f000000	/* perform memory setup and continue
+					   (IE. Done) */
+#define MBX_CMD_NOOP       0xFF000000	/* dummy, illegal command */
+
+/*****************************************/
+/***    MAILBOX #2  -  MEMORY SIZE     ***/
+/*****************************************/
+
+#define MBX_MEMSZ_MASK     0xffff0000	/* PUTS Memory Size Register bits */
+
+#define MBX_MEMSZ_128KB    0x00020000	/* 128 kilobyte board */
+#define MBX_MEMSZ_256KB    0x00040000	/* 256 kilobyte board */
+#define MBX_MEMSZ_512KB    0x00080000	/* 512 kilobyte board */
+#define MBX_MEMSZ_1MB      0x00100000	/* 1 megabyte board */
+#define MBX_MEMSZ_2MB      0x00200000	/* 2 megabyte board */
+#define MBX_MEMSZ_4MB      0x00400000	/* 4 megabyte board */
+#define MBX_MEMSZ_8MB      0x00800000	/* 8 megabyte board */
+#define MBX_MEMSZ_16MB     0x01000000	/* 16 megabyte board */
+
+/***************************************/
+/***    MAILBOX #2  -  BOARD TYPE    ***/
+/***************************************/
+
+#define MBX_BTYPE_MASK          0x0000ffff	/* PUTS Board Type Register */
+#define MBX_BTYPE_FAMILY_MASK   0x0000ff00	/* PUTS Board Family Register */
+#define MBX_BTYPE_SUBTYPE_MASK  0x000000ff	/* PUTS Board Subtype */
+
+#define MBX_BTYPE_PLX9060       0x00000100	/* PLX family type */
+#define MBX_BTYPE_PLX9080       0x00000300	/* PLX wanXL100s family type */
+
+#define MBX_BTYPE_WANXL_4       0x00000104	/* wanXL400, 4-port */
+#define MBX_BTYPE_WANXL_2       0x00000102	/* wanXL200, 2-port */
+#define MBX_BTYPE_WANXL_1s      0x00000301	/* wanXL100s, 1-port */
+#define MBX_BTYPE_WANXL_1t      0x00000401	/* wanXL100T1, 1-port */
+
+/*****************************************/
+/***    MAILBOX #3  -  SHMQ MAILBOX    ***/
+/*****************************************/
+
+#define MBX_SMBX_MASK           0x000000ff	/* PUTS SHMQ Mailbox bits */
+
+/***************************************/
+/***    GENERIC HOST-SIDE DRIVER     ***/
+/***************************************/
+
+#define MBX_ERR    0
+#define MBX_OK     1
+
+/* mailbox check routine - type of testing */
+#define MBXCHK_STS      0x00	/* check for PUTS status */
+#define MBXCHK_NOWAIT   0x01	/* dont care about PUTS status */
+
+/* system allocates this many bytes for address mapping mailbox space */
+#define MBX_ADDR_SPACE_360 0x80	/* wanXL100s/200/400 */
+#define MBX_ADDR_MASK_360 (MBX_ADDR_SPACE_360-1)
+
+static inline int plx9080_abort_dma(void *iobase, unsigned int channel)
+{
+	void *dma_cs_addr;
+	uint8_t dma_status;
+	const int timeout = 10000;
+	unsigned int i;
+
+	if (channel)
+		dma_cs_addr = iobase + PLX_DMA1_CS_REG;
+	else
+		dma_cs_addr = iobase + PLX_DMA0_CS_REG;
+
+	// abort dma transfer if necessary
+	dma_status = readb(dma_cs_addr);
+	if ((dma_status & PLX_DMA_EN_BIT) == 0) {
+		return 0;
+	}
+	// wait to make sure done bit is zero
+	for (i = 0; (dma_status & PLX_DMA_DONE_BIT) && i < timeout; i++) {
+		comedi_udelay(1);
+		dma_status = readb(dma_cs_addr);
+	}
+	if (i == timeout) {
+		rt_printk
+			("plx9080: cancel() timed out waiting for dma %i done clear\n",
+			channel);
+		return -ETIMEDOUT;
+	}
+	// disable and abort channel
+	writeb(PLX_DMA_ABORT_BIT, dma_cs_addr);
+	// wait for dma done bit
+	dma_status = readb(dma_cs_addr);
+	for (i = 0; (dma_status & PLX_DMA_DONE_BIT) == 0 && i < timeout; i++) {
+		comedi_udelay(1);
+		dma_status = readb(dma_cs_addr);
+	}
+	if (i == timeout) {
+		rt_printk
+			("plx9080: cancel() timed out waiting for dma %i done set\n",
+			channel);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+#endif /* __COMEDI_PLX9080_H */
diff --git a/drivers/staging/comedi/drivers/rtd520.c b/drivers/staging/comedi/drivers/rtd520.c
new file mode 100644
index 000000000000..65d5242a2585
--- /dev/null
+++ b/drivers/staging/comedi/drivers/rtd520.c
@@ -0,0 +1,2283 @@
+/*
+    comedi/drivers/rtd520.c
+    Comedi driver for Real Time Devices (RTD) PCI4520/DM7520
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 2001 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+/*
+Driver: rtd520
+Description: Real Time Devices PCI4520/DM7520
+Author: Dan Christian
+Devices: [Real Time Devices] DM7520HR-1 (rtd520), DM7520HR-8,
+  PCI4520, PCI4520-8
+Status: Works.  Only tested on DM7520-8.  Not SMP safe.
+
+Configuration options:
+  [0] - PCI bus of device (optional)
+          If bus/slot is not specified, the first available PCI
+          device will be used.
+  [1] - PCI slot of device (optional)
+*/
+/*
+    Created by Dan Christian, NASA Ames Research Center.
+
+    The PCI4520 is a PCI card.  The DM7520 is a PC/104-plus card.
+    Both have:
+    8/16 12 bit ADC with FIFO and channel gain table
+    8 bits high speed digital out (for external MUX) (or 8 in or 8 out)
+    8 bits high speed digital in with FIFO and interrupt on change (or 8 IO)
+    2 12 bit DACs with FIFOs
+    2 bits output
+    2 bits input
+    bus mastering DMA
+    timers: ADC sample, pacer, burst, about, delay, DA1, DA2
+    sample counter
+    3 user timer/counters (8254)
+    external interrupt
+
+    The DM7520 has slightly fewer features (fewer gain steps).
+
+    These boards can support external multiplexors and multi-board
+    synchronization, but this driver doesn't support that.
+
+    Board docs: http://www.rtdusa.com/PC104/DM/analog%20IO/dm7520.htm
+    Data sheet: http://www.rtdusa.com/pdf/dm7520.pdf
+    Example source: http://www.rtdusa.com/examples/dm/dm7520.zip
+    Call them and ask for the register level manual.
+    PCI chip: http://www.plxtech.com/products/toolbox/9080.htm
+
+    Notes:
+    This board is memory mapped.  There is some IO stuff, but it isn't needed.
+
+    I use a pretty loose naming style within the driver (rtd_blah).
+    All externally visible names should be rtd520_blah.
+    I use camelCase for structures (and inside them).
+    I may also use upper CamelCase for function names (old habit).
+
+    This board is somewhat related to the RTD PCI4400 board.
+
+    I borrowed heavily from the ni_mio_common, ni_atmio16d, mite, and
+    das1800, since they have the best documented code.  Driver
+    cb_pcidas64.c uses the same DMA controller.
+
+    As far as I can tell, the About interrupt doesnt work if Sample is
+    also enabled.  It turns out that About really isn't needed, since
+    we always count down samples read.
+
+    There was some timer/counter code, but it didn't follow the right API.
+
+*/
+
+/*
+  driver status:
+
+  Analog-In supports instruction and command mode.
+
+  With DMA, you can sample at 1.15Mhz with 70% idle on a 400Mhz K6-2
+  (single channel, 64K read buffer).  I get random system lockups when
+  using DMA with ALI-15xx based systems.  I haven't been able to test
+  any other chipsets.  The lockups happen soon after the start of an
+  acquistion, not in the middle of a long run.
+
+  Without DMA, you can do 620Khz sampling with 20% idle on a 400Mhz K6-2
+  (with a 256K read buffer).
+
+  Digital-IO and Analog-Out only support instruction mode.
+
+*/
+
+#include <linux/delay.h>
+
+#include "../comedidev.h"
+#include "comedi_pci.h"
+
+#define DRV_NAME "rtd520"
+
+/*======================================================================
+  Driver specific stuff (tunable)
+======================================================================*/
+/* Enable this to test the new DMA support. You may get hard lock ups */
+/*#define USE_DMA*/
+
+/* We really only need 2 buffers.  More than that means being much
+   smarter about knowing which ones are full. */
+#define DMA_CHAIN_COUNT 2	/* max DMA segments/buffers in a ring (min 2) */
+
+/* Target period for periodic transfers.  This sets the user read latency. */
+/* Note: There are certain rates where we give this up and transfer 1/2 FIFO */
+/* If this is too low, efficiency is poor */
+#define TRANS_TARGET_PERIOD 10000000	/* 10 ms (in nanoseconds) */
+
+/* Set a practical limit on how long a list to support (affects memory use) */
+/* The board support a channel list up to the FIFO length (1K or 8K) */
+#define RTD_MAX_CHANLIST	128	/* max channel list that we allow */
+
+/* tuning for ai/ao instruction done polling */
+#ifdef FAST_SPIN
+#define WAIT_QUIETLY		/* as nothing, spin on done bit */
+#define RTD_ADC_TIMEOUT	66000	/* 2 msec at 33mhz bus rate */
+#define RTD_DAC_TIMEOUT	66000
+#define RTD_DMA_TIMEOUT	33000	/* 1 msec */
+#else
+/* by delaying, power and electrical noise are reduced somewhat */
+#define WAIT_QUIETLY	comedi_udelay (1)
+#define RTD_ADC_TIMEOUT	2000	/* in usec */
+#define RTD_DAC_TIMEOUT	2000	/* in usec */
+#define RTD_DMA_TIMEOUT	1000	/* in usec */
+#endif
+
+/*======================================================================
+  Board specific stuff
+======================================================================*/
+
+/* registers  */
+#define PCI_VENDOR_ID_RTD	0x1435
+/*
+  The board has three memory windows: las0, las1, and lcfg (the PCI chip)
+  Las1 has the data and can be burst DMAed 32bits at a time.
+*/
+#define LCFG_PCIINDEX	0
+/* PCI region 1 is a 256 byte IO space mapping.  Use??? */
+#define LAS0_PCIINDEX	2	/* PCI memory resources */
+#define LAS1_PCIINDEX	3
+#define LCFG_PCISIZE	0x100
+#define LAS0_PCISIZE	0x200
+#define LAS1_PCISIZE	0x10
+
+#define RTD_CLOCK_RATE	8000000	/* 8Mhz onboard clock */
+#define RTD_CLOCK_BASE	125	/* clock period in ns */
+
+/* Note: these speed are slower than the spec, but fit the counter resolution*/
+#define RTD_MAX_SPEED	1625	/* when sampling, in nanoseconds */
+/* max speed if we don't have to wait for settling */
+#define RTD_MAX_SPEED_1	875	/* if single channel, in nanoseconds */
+
+#define RTD_MIN_SPEED	2097151875	/* (24bit counter) in nanoseconds */
+/* min speed when only 1 channel (no burst counter) */
+#define RTD_MIN_SPEED_1	5000000	/* 200Hz, in nanoseconds */
+
+#include "rtd520.h"
+#include "plx9080.h"
+
+/* Setup continuous ring of 1/2 FIFO transfers.  See RTD manual p91 */
+#define DMA_MODE_BITS (\
+		       PLX_LOCAL_BUS_16_WIDE_BITS \
+		       | PLX_DMA_EN_READYIN_BIT \
+		       | PLX_DMA_LOCAL_BURST_EN_BIT \
+		       | PLX_EN_CHAIN_BIT \
+		       | PLX_DMA_INTR_PCI_BIT \
+		       | PLX_LOCAL_ADDR_CONST_BIT \
+		       | PLX_DEMAND_MODE_BIT)
+
+#define DMA_TRANSFER_BITS (\
+/* descriptors in PCI memory*/ 	PLX_DESC_IN_PCI_BIT \
+/* interrupt at end of block */ | PLX_INTR_TERM_COUNT \
+/* from board to PCI */		| PLX_XFER_LOCAL_TO_PCI)
+
+/*======================================================================
+  Comedi specific stuff
+======================================================================*/
+
+/*
+  The board has 3 input modes and the gains of 1,2,4,...32 (, 64, 128)
+*/
+static const comedi_lrange rtd_ai_7520_range = { 18, {
+			/* +-5V input range gain steps */
+			BIP_RANGE(5.0),
+			BIP_RANGE(5.0 / 2),
+			BIP_RANGE(5.0 / 4),
+			BIP_RANGE(5.0 / 8),
+			BIP_RANGE(5.0 / 16),
+			BIP_RANGE(5.0 / 32),
+			/* +-10V input range gain steps */
+			BIP_RANGE(10.0),
+			BIP_RANGE(10.0 / 2),
+			BIP_RANGE(10.0 / 4),
+			BIP_RANGE(10.0 / 8),
+			BIP_RANGE(10.0 / 16),
+			BIP_RANGE(10.0 / 32),
+			/* +10V input range gain steps */
+			UNI_RANGE(10.0),
+			UNI_RANGE(10.0 / 2),
+			UNI_RANGE(10.0 / 4),
+			UNI_RANGE(10.0 / 8),
+			UNI_RANGE(10.0 / 16),
+			UNI_RANGE(10.0 / 32),
+
+	}
+};
+
+/* PCI4520 has two more gains (6 more entries) */
+static const comedi_lrange rtd_ai_4520_range = { 24, {
+			/* +-5V input range gain steps */
+			BIP_RANGE(5.0),
+			BIP_RANGE(5.0 / 2),
+			BIP_RANGE(5.0 / 4),
+			BIP_RANGE(5.0 / 8),
+			BIP_RANGE(5.0 / 16),
+			BIP_RANGE(5.0 / 32),
+			BIP_RANGE(5.0 / 64),
+			BIP_RANGE(5.0 / 128),
+			/* +-10V input range gain steps */
+			BIP_RANGE(10.0),
+			BIP_RANGE(10.0 / 2),
+			BIP_RANGE(10.0 / 4),
+			BIP_RANGE(10.0 / 8),
+			BIP_RANGE(10.0 / 16),
+			BIP_RANGE(10.0 / 32),
+			BIP_RANGE(10.0 / 64),
+			BIP_RANGE(10.0 / 128),
+			/* +10V input range gain steps */
+			UNI_RANGE(10.0),
+			UNI_RANGE(10.0 / 2),
+			UNI_RANGE(10.0 / 4),
+			UNI_RANGE(10.0 / 8),
+			UNI_RANGE(10.0 / 16),
+			UNI_RANGE(10.0 / 32),
+			UNI_RANGE(10.0 / 64),
+			UNI_RANGE(10.0 / 128),
+	}
+};
+
+/* Table order matches range values */
+static const comedi_lrange rtd_ao_range = { 4, {
+			RANGE(0, 5),
+			RANGE(0, 10),
+			RANGE(-5, 5),
+			RANGE(-10, 10),
+	}
+};
+
+/*
+  Board descriptions
+ */
+typedef struct rtdBoard_struct {
+	const char *name;	/* must be first */
+	int device_id;
+	int aiChans;
+	int aiBits;
+	int aiMaxGain;
+	int range10Start;	/* start of +-10V range */
+	int rangeUniStart;	/* start of +10V range */
+} rtdBoard;
+
+static const rtdBoard rtd520Boards[] = {
+	{
+	      name:	"DM7520",
+	      device_id:0x7520,
+	      aiChans:	16,
+	      aiBits:	12,
+	      aiMaxGain:32,
+	      range10Start:6,
+	      rangeUniStart:12,
+		},
+	{
+	      name:	"PCI4520",
+	      device_id:0x4520,
+	      aiChans:	16,
+	      aiBits:	12,
+	      aiMaxGain:128,
+	      range10Start:8,
+	      rangeUniStart:16,
+		},
+};
+
+static DEFINE_PCI_DEVICE_TABLE(rtd520_pci_table) = {
+	{PCI_VENDOR_ID_RTD, 0x7520, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_RTD, 0x4520, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, rtd520_pci_table);
+
+/*
+ * Useful for shorthand access to the particular board structure
+ */
+#define thisboard ((const rtdBoard *)dev->board_ptr)
+
+/*
+   This structure is for data unique to this hardware driver.
+   This is also unique for each board in the system.
+*/
+typedef struct {
+	/* memory mapped board structures */
+	void *las0;
+	void *las1;
+	void *lcfg;
+
+	unsigned long intCount;	/* interrupt count */
+	long aiCount;		/* total transfer size (samples) */
+	int transCount;		/* # to tranfer data. 0->1/2FIFO */
+	int flags;		/* flag event modes */
+
+	/* PCI device info */
+	struct pci_dev *pci_dev;
+	int got_regions;	/* non-zero if PCI regions owned */
+
+	/* channel list info */
+	/* chanBipolar tracks whether a channel is bipolar (and needs +2048) */
+	unsigned char chanBipolar[RTD_MAX_CHANLIST / 8];	/* bit array */
+
+	/* read back data */
+	lsampl_t aoValue[2];	/* Used for AO read back */
+
+	/* timer gate (when enabled) */
+	u8 utcGate[4];		/* 1 extra allows simple range check */
+
+	/* shadow registers affect other registers, but cant be read back */
+	/* The macros below update these on writes */
+	u16 intMask;		/* interrupt mask */
+	u16 intClearMask;	/* interrupt clear mask */
+	u8 utcCtrl[4];		/* crtl mode for 3 utc + read back */
+	u8 dioStatus;		/* could be read back (dio0Ctrl) */
+#ifdef USE_DMA
+	/* Always DMA 1/2 FIFO.  Buffer (dmaBuff?) is (at least) twice that size.
+	   After transferring, interrupt processes 1/2 FIFO and passes to comedi */
+	s16 dma0Offset;		/* current processing offset (0, 1/2) */
+	uint16_t *dma0Buff[DMA_CHAIN_COUNT];	/* DMA buffers (for ADC) */
+	dma_addr_t dma0BuffPhysAddr[DMA_CHAIN_COUNT];	/* physical addresses */
+	struct plx_dma_desc *dma0Chain;	/* DMA descriptor ring for dmaBuff */
+	dma_addr_t dma0ChainPhysAddr;	/* physical addresses */
+	/* shadow registers */
+	u8 dma0Control;
+	u8 dma1Control;
+#endif				/* USE_DMA */
+	unsigned fifoLen;
+} rtdPrivate;
+
+/* bit defines for "flags" */
+#define SEND_EOS	0x01	/* send End Of Scan events */
+#define DMA0_ACTIVE	0x02	/* DMA0 is active */
+#define DMA1_ACTIVE	0x04	/* DMA1 is active */
+
+/* Macros for accessing channel list bit array */
+#define CHAN_ARRAY_TEST(array,index) \
+	(((array)[(index)/8] >> ((index) & 0x7)) & 0x1)
+#define CHAN_ARRAY_SET(array,index) \
+	(((array)[(index)/8] |= 1 << ((index) & 0x7)))
+#define CHAN_ARRAY_CLEAR(array,index) \
+	(((array)[(index)/8] &= ~(1 << ((index) & 0x7))))
+
+/*
+ * most drivers define the following macro to make it easy to
+ * access the private structure.
+ */
+#define devpriv ((rtdPrivate *)dev->private)
+
+/* Macros to access registers */
+
+/* Reset board */
+#define RtdResetBoard(dev) \
+    writel (0, devpriv->las0+LAS0_BOARD_RESET)
+
+/* Reset channel gain table read pointer */
+#define RtdResetCGT(dev) \
+    writel (0, devpriv->las0+LAS0_CGT_RESET)
+
+/* Reset channel gain table read and write pointers */
+#define RtdClearCGT(dev) \
+    writel (0, devpriv->las0+LAS0_CGT_CLEAR)
+
+/* Reset channel gain table read and write pointers */
+#define RtdEnableCGT(dev,v) \
+    writel ((v > 0) ? 1 : 0, devpriv->las0+LAS0_CGT_ENABLE)
+
+/* Write channel gain table entry */
+#define RtdWriteCGTable(dev,v) \
+    writel (v, devpriv->las0+LAS0_CGT_WRITE)
+
+/* Write Channel Gain Latch */
+#define RtdWriteCGLatch(dev,v) \
+    writel (v, devpriv->las0+LAS0_CGL_WRITE)
+
+/* Reset ADC FIFO */
+#define RtdAdcClearFifo(dev) \
+    writel (0, devpriv->las0+LAS0_ADC_FIFO_CLEAR)
+
+/* Set ADC start conversion source select (write only) */
+#define RtdAdcConversionSource(dev,v) \
+    writel (v, devpriv->las0+LAS0_ADC_CONVERSION)
+
+/* Set burst start source select (write only) */
+#define RtdBurstStartSource(dev,v) \
+    writel (v, devpriv->las0+LAS0_BURST_START)
+
+/* Set Pacer start source select (write only) */
+#define RtdPacerStartSource(dev,v) \
+    writel (v, devpriv->las0+LAS0_PACER_START)
+
+/* Set Pacer stop source select (write only) */
+#define RtdPacerStopSource(dev,v) \
+    writel (v, devpriv->las0+LAS0_PACER_STOP)
+
+/* Set Pacer clock source select (write only) 0=external 1=internal */
+#define RtdPacerClockSource(dev,v) \
+    writel ((v > 0) ? 1 : 0, devpriv->las0+LAS0_PACER_SELECT)
+
+/* Set sample counter source select (write only) */
+#define RtdAdcSampleCounterSource(dev,v) \
+    writel (v, devpriv->las0+LAS0_ADC_SCNT_SRC)
+
+/* Set Pacer trigger mode select (write only) 0=single cycle, 1=repeat */
+#define RtdPacerTriggerMode(dev,v) \
+    writel ((v > 0) ? 1 : 0, devpriv->las0+LAS0_PACER_REPEAT)
+
+/* Set About counter stop enable (write only) */
+#define RtdAboutStopEnable(dev,v) \
+    writel ((v > 0) ? 1 : 0, devpriv->las0+LAS0_ACNT_STOP_ENABLE)
+
+/* Set external trigger polarity (write only) 0=positive edge, 1=negative */
+#define RtdTriggerPolarity(dev,v) \
+    writel ((v > 0) ? 1 : 0, devpriv->las0+LAS0_ETRG_POLARITY)
+
+/* Start single ADC conversion */
+#define RtdAdcStart(dev) \
+    writew (0, devpriv->las0+LAS0_ADC)
+
+/* Read one ADC data value (12bit (with sign extend) as 16bit) */
+/* Note: matches what DMA would get.  Actual value >> 3 */
+#define RtdAdcFifoGet(dev) \
+    readw (devpriv->las1+LAS1_ADC_FIFO)
+
+/* Read two ADC data values (DOESNT WORK) */
+#define RtdAdcFifoGet2(dev) \
+    readl (devpriv->las1+LAS1_ADC_FIFO)
+
+/* FIFO status */
+#define RtdFifoStatus(dev) \
+    readl (devpriv->las0+LAS0_ADC)
+
+/* pacer start/stop read=start, write=stop*/
+#define RtdPacerStart(dev) \
+    readl (devpriv->las0+LAS0_PACER)
+#define RtdPacerStop(dev) \
+    writel (0, devpriv->las0+LAS0_PACER)
+
+/* Interrupt status */
+#define RtdInterruptStatus(dev) \
+    readw (devpriv->las0+LAS0_IT)
+
+/* Interrupt mask */
+#define RtdInterruptMask(dev,v) \
+    writew ((devpriv->intMask = (v)),devpriv->las0+LAS0_IT)
+
+/* Interrupt status clear (only bits set in mask) */
+#define RtdInterruptClear(dev) \
+    readw (devpriv->las0+LAS0_CLEAR)
+
+/* Interrupt clear mask */
+#define RtdInterruptClearMask(dev,v) \
+    writew ((devpriv->intClearMask = (v)), devpriv->las0+LAS0_CLEAR)
+
+/* Interrupt overrun status */
+#define RtdInterruptOverrunStatus(dev) \
+    readl (devpriv->las0+LAS0_OVERRUN)
+
+/* Interrupt overrun clear */
+#define RtdInterruptOverrunClear(dev) \
+    writel (0, devpriv->las0+LAS0_OVERRUN)
+
+/* Pacer counter, 24bit */
+#define RtdPacerCount(dev) \
+    readl (devpriv->las0+LAS0_PCLK)
+#define RtdPacerCounter(dev,v) \
+    writel ((v) & 0xffffff,devpriv->las0+LAS0_PCLK)
+
+/* Burst counter, 10bit */
+#define RtdBurstCount(dev) \
+    readl (devpriv->las0+LAS0_BCLK)
+#define RtdBurstCounter(dev,v) \
+    writel ((v) & 0x3ff,devpriv->las0+LAS0_BCLK)
+
+/* Delay counter, 16bit */
+#define RtdDelayCount(dev) \
+    readl (devpriv->las0+LAS0_DCLK)
+#define RtdDelayCounter(dev,v) \
+    writel ((v) & 0xffff, devpriv->las0+LAS0_DCLK)
+
+/* About counter, 16bit */
+#define RtdAboutCount(dev) \
+    readl (devpriv->las0+LAS0_ACNT)
+#define RtdAboutCounter(dev,v) \
+    writel ((v) & 0xffff, devpriv->las0+LAS0_ACNT)
+
+/* ADC sample counter, 10bit */
+#define RtdAdcSampleCount(dev) \
+    readl (devpriv->las0+LAS0_ADC_SCNT)
+#define RtdAdcSampleCounter(dev,v) \
+    writel ((v) & 0x3ff, devpriv->las0+LAS0_ADC_SCNT)
+
+/* User Timer/Counter (8254) */
+#define RtdUtcCounterGet(dev,n) \
+    readb (devpriv->las0 \
+        + ((n <= 0) ? LAS0_UTC0 : ((1 == n) ? LAS0_UTC1 : LAS0_UTC2)))
+
+#define RtdUtcCounterPut(dev,n,v) \
+    writeb ((v) & 0xff, devpriv->las0 \
+        + ((n <= 0) ? LAS0_UTC0 : ((1 == n) ? LAS0_UTC1 : LAS0_UTC2)))
+
+/* Set UTC (8254) control byte  */
+#define RtdUtcCtrlPut(dev,n,v) \
+    writeb (devpriv->utcCtrl[(n) & 3] = (((n) & 3) << 6) | ((v) & 0x3f), \
+      devpriv->las0 + LAS0_UTC_CTRL)
+
+/* Set UTCn clock source (write only) */
+#define RtdUtcClockSource(dev,n,v) \
+    writew (v, devpriv->las0 \
+        + ((n <= 0) ? LAS0_UTC0_CLOCK : \
+           ((1 == n) ? LAS0_UTC1_CLOCK : LAS0_UTC2_CLOCK)))
+
+/* Set UTCn gate source (write only) */
+#define RtdUtcGateSource(dev,n,v) \
+    writew (v, devpriv->las0 \
+        + ((n <= 0) ? LAS0_UTC0_GATE : \
+           ((1 == n) ? LAS0_UTC1_GATE : LAS0_UTC2_GATE)))
+
+/* User output N source select (write only) */
+#define RtdUsrOutSource(dev,n,v) \
+    writel (v,devpriv->las0+((n <= 0) ? LAS0_UOUT0_SELECT : LAS0_UOUT1_SELECT))
+
+/* Digital IO */
+#define RtdDio0Read(dev) \
+    (readw (devpriv->las0+LAS0_DIO0) & 0xff)
+#define RtdDio0Write(dev,v) \
+    writew ((v) & 0xff, devpriv->las0+LAS0_DIO0)
+
+#define RtdDio1Read(dev) \
+    (readw (devpriv->las0+LAS0_DIO1) & 0xff)
+#define RtdDio1Write(dev,v) \
+    writew ((v) & 0xff, devpriv->las0+LAS0_DIO1)
+
+#define RtdDioStatusRead(dev) \
+    (readw (devpriv->las0+LAS0_DIO_STATUS) & 0xff)
+#define RtdDioStatusWrite(dev,v) \
+    writew ((devpriv->dioStatus = (v)), devpriv->las0+LAS0_DIO_STATUS)
+
+#define RtdDio0CtrlRead(dev) \
+    (readw (devpriv->las0+LAS0_DIO0_CTRL) & 0xff)
+#define RtdDio0CtrlWrite(dev,v) \
+    writew ((v) & 0xff, devpriv->las0+LAS0_DIO0_CTRL)
+
+/* Digital to Analog converter */
+/* Write one data value (sign + 12bit + marker bits) */
+/* Note: matches what DMA would put.  Actual value << 3 */
+#define RtdDacFifoPut(dev,n,v) \
+    writew ((v), devpriv->las1 +(((n) == 0) ? LAS1_DAC1_FIFO : LAS1_DAC2_FIFO))
+
+/* Start single DAC conversion */
+#define RtdDacUpdate(dev,n) \
+    writew (0, devpriv->las0 +(((n) == 0) ? LAS0_DAC1 : LAS0_DAC2))
+
+/* Start single DAC conversion on both DACs */
+#define RtdDacBothUpdate(dev) \
+    writew (0, devpriv->las0+LAS0_DAC)
+
+/* Set DAC output type and range */
+#define RtdDacRange(dev,n,v) \
+    writew ((v) & 7, devpriv->las0 \
+	+(((n) == 0) ? LAS0_DAC1_CTRL : LAS0_DAC2_CTRL))
+
+/* Reset DAC FIFO */
+#define RtdDacClearFifo(dev,n) \
+    writel (0, devpriv->las0+(((n) == 0) ? LAS0_DAC1_RESET : LAS0_DAC2_RESET))
+
+/* Set source for DMA 0 (write only, shadow?) */
+#define RtdDma0Source(dev,n) \
+    writel ((n) & 0xf, devpriv->las0+LAS0_DMA0_SRC)
+
+/* Set source for DMA 1 (write only, shadow?) */
+#define RtdDma1Source(dev,n) \
+    writel ((n) & 0xf, devpriv->las0+LAS0_DMA1_SRC)
+
+/* Reset board state for DMA 0 */
+#define RtdDma0Reset(dev) \
+    writel (0, devpriv->las0+LAS0_DMA0_RESET)
+
+/* Reset board state for DMA 1 */
+#define RtdDma1Reset(dev) \
+    writel (0, devpriv->las0+LAS0_DMA1_SRC)
+
+/* PLX9080 interrupt mask and status */
+#define RtdPlxInterruptRead(dev) \
+    readl (devpriv->lcfg+LCFG_ITCSR)
+#define RtdPlxInterruptWrite(dev,v) \
+    writel (v, devpriv->lcfg+LCFG_ITCSR)
+
+/* Set  mode for DMA 0 */
+#define RtdDma0Mode(dev,m) \
+    writel ((m), devpriv->lcfg+LCFG_DMAMODE0)
+
+/* Set PCI address for DMA 0 */
+#define RtdDma0PciAddr(dev,a) \
+    writel ((a), devpriv->lcfg+LCFG_DMAPADR0)
+
+/* Set local address for DMA 0 */
+#define RtdDma0LocalAddr(dev,a) \
+    writel ((a), devpriv->lcfg+LCFG_DMALADR0)
+
+/* Set byte count for DMA 0 */
+#define RtdDma0Count(dev,c) \
+    writel ((c), devpriv->lcfg+LCFG_DMASIZ0)
+
+/* Set next descriptor for DMA 0 */
+#define RtdDma0Next(dev,a) \
+    writel ((a), devpriv->lcfg+LCFG_DMADPR0)
+
+/* Set  mode for DMA 1 */
+#define RtdDma1Mode(dev,m) \
+    writel ((m), devpriv->lcfg+LCFG_DMAMODE1)
+
+/* Set PCI address for DMA 1 */
+#define RtdDma1PciAddr(dev,a) \
+    writel ((a), devpriv->lcfg+LCFG_DMAADR1)
+
+/* Set local address for DMA 1 */
+#define RtdDma1LocalAddr(dev,a) \
+    writel ((a), devpriv->lcfg+LCFG_DMALADR1)
+
+/* Set byte count for DMA 1 */
+#define RtdDma1Count(dev,c) \
+    writel ((c), devpriv->lcfg+LCFG_DMASIZ1)
+
+/* Set next descriptor for DMA 1 */
+#define RtdDma1Next(dev,a) \
+    writel ((a), devpriv->lcfg+LCFG_DMADPR1)
+
+/* Set control for DMA 0 (write only, shadow?) */
+#define RtdDma0Control(dev,n) \
+    writeb (devpriv->dma0Control = (n), devpriv->lcfg+LCFG_DMACSR0)
+
+/* Get status for DMA 0 */
+#define RtdDma0Status(dev) \
+    readb (devpriv->lcfg+LCFG_DMACSR0)
+
+/* Set control for DMA 1 (write only, shadow?) */
+#define RtdDma1Control(dev,n) \
+    writeb (devpriv->dma1Control = (n), devpriv->lcfg+LCFG_DMACSR1)
+
+/* Get status for DMA 1 */
+#define RtdDma1Status(dev) \
+    readb (devpriv->lcfg+LCFG_DMACSR1)
+
+/*
+ * The comedi_driver structure tells the Comedi core module
+ * which functions to call to configure/deconfigure (attac/detach)
+ * the board, and also about the kernel module that contains
+ * the device code.
+ */
+static int rtd_attach(comedi_device * dev, comedi_devconfig * it);
+static int rtd_detach(comedi_device * dev);
+
+static comedi_driver rtd520Driver = {
+      driver_name: DRV_NAME,
+      module:THIS_MODULE,
+      attach:rtd_attach,
+      detach:rtd_detach,
+};
+
+static int rtd_ai_rinsn(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int rtd_ao_winsn(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int rtd_ao_rinsn(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int rtd_dio_insn_bits(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int rtd_dio_insn_config(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int rtd_ai_cmdtest(comedi_device * dev, comedi_subdevice * s,
+	comedi_cmd * cmd);
+static int rtd_ai_cmd(comedi_device * dev, comedi_subdevice * s);
+static int rtd_ai_cancel(comedi_device * dev, comedi_subdevice * s);
+//static int rtd_ai_poll (comedi_device *dev,comedi_subdevice *s);
+static int rtd_ns_to_timer(unsigned int *ns, int roundMode);
+static irqreturn_t rtd_interrupt(int irq, void *d PT_REGS_ARG);
+static int rtd520_probe_fifo_depth(comedi_device *dev);
+
+/*
+ * Attach is called by the Comedi core to configure the driver
+ * for a particular board.  If you specified a board_name array
+ * in the driver structure, dev->board_ptr contains that
+ * address.
+ */
+static int rtd_attach(comedi_device * dev, comedi_devconfig * it)
+{				/* board name and options flags */
+	comedi_subdevice *s;
+	struct pci_dev *pcidev;
+	int ret;
+	resource_size_t physLas0;	/* configuation */
+	resource_size_t physLas1;	/* data area */
+	resource_size_t physLcfg;	/* PLX9080 */
+#ifdef USE_DMA
+	int index;
+#endif
+
+	printk("comedi%d: rtd520 attaching.\n", dev->minor);
+
+#if defined (CONFIG_COMEDI_DEBUG) && defined (USE_DMA)
+	/* You can set this a load time: modprobe comedi comedi_debug=1 */
+	if (0 == comedi_debug)	/* force DMA debug printks */
+		comedi_debug = 1;
+#endif
+
+	/*
+	 * Allocate the private structure area.  alloc_private() is a
+	 * convenient macro defined in comedidev.h.
+	 */
+	if (alloc_private(dev, sizeof(rtdPrivate)) < 0)
+		return -ENOMEM;
+
+	/*
+	 * Probe the device to determine what device in the series it is.
+	 */
+	for (pcidev = pci_get_device(PCI_VENDOR_ID_RTD, PCI_ANY_ID, NULL);
+		pcidev != NULL;
+		pcidev = pci_get_device(PCI_VENDOR_ID_RTD, PCI_ANY_ID, pcidev)) {
+		int i;
+
+		if (it->options[0] || it->options[1]) {
+			if (pcidev->bus->number != it->options[0]
+				|| PCI_SLOT(pcidev->devfn) !=
+				it->options[1]) {
+				continue;
+			}
+		}
+		for(i = 0; i < sizeof(rtd520Boards) / sizeof(rtd520Boards[0]); ++i)
+		{
+			if(pcidev->device == rtd520Boards[i].device_id)
+			{
+				dev->board_ptr = &rtd520Boards[i];
+				break;
+			}
+		}
+		if(dev->board_ptr) break;	/* found one */
+	}
+	if (!pcidev) {
+		if (it->options[0] && it->options[1]) {
+			printk("No RTD card at bus=%d slot=%d.\n",
+				it->options[0], it->options[1]);
+		} else {
+			printk("No RTD card found.\n");
+		}
+		return -EIO;
+	}
+	devpriv->pci_dev = pcidev;
+	dev->board_name = thisboard->name;
+
+	if ((ret = comedi_pci_enable(pcidev, DRV_NAME)) < 0) {
+		printk("Failed to enable PCI device and request regions.\n");
+		return ret;
+	}
+	devpriv->got_regions = 1;
+
+	/*
+	 * Initialize base addresses
+	 */
+	/* Get the physical address from PCI config */
+	physLas0 = pci_resource_start(devpriv->pci_dev, LAS0_PCIINDEX);
+	physLas1 = pci_resource_start(devpriv->pci_dev, LAS1_PCIINDEX);
+	physLcfg = pci_resource_start(devpriv->pci_dev, LCFG_PCIINDEX);
+	/* Now have the kernel map this into memory */
+	/* ASSUME page aligned */
+	devpriv->las0 = ioremap_nocache(physLas0, LAS0_PCISIZE);
+	devpriv->las1 = ioremap_nocache(physLas1, LAS1_PCISIZE);
+	devpriv->lcfg = ioremap_nocache(physLcfg, LCFG_PCISIZE);
+
+	if (!devpriv->las0 || !devpriv->las1 || !devpriv->lcfg) {
+		return -ENOMEM;
+	}
+
+	DPRINTK("%s: LAS0=%llx, LAS1=%llx, CFG=%llx.\n", dev->board_name,
+		(unsigned long long)physLas0, (unsigned long long)physLas1,
+		(unsigned long long)physLcfg);
+	{			/* The RTD driver does this */
+		unsigned char pci_latency;
+		u16 revision;
+		/*uint32_t epld_version; */
+
+		pci_read_config_word(devpriv->pci_dev, PCI_REVISION_ID,
+			&revision);
+		DPRINTK("%s: PCI revision %d.\n", dev->board_name, revision);
+
+		pci_read_config_byte(devpriv->pci_dev,
+			PCI_LATENCY_TIMER, &pci_latency);
+		if (pci_latency < 32) {
+			printk("%s: PCI latency changed from %d to %d\n",
+				dev->board_name, pci_latency, 32);
+			pci_write_config_byte(devpriv->pci_dev,
+				PCI_LATENCY_TIMER, 32);
+		} else {
+			DPRINTK("rtd520: PCI latency = %d\n", pci_latency);
+		}
+
+		/* Undocumented EPLD version (doesnt match RTD driver results) */
+		/*DPRINTK ("rtd520: Reading epld from %p\n",
+		   devpriv->las0+0);
+		   epld_version = readl (devpriv->las0+0);
+		   if ((epld_version & 0xF0) >> 4 == 0x0F) {
+		   DPRINTK("rtd520: pre-v8 EPLD. (%x)\n", epld_version);
+		   } else {
+		   DPRINTK("rtd520: EPLD version %x.\n", epld_version >> 4);
+		   } */
+	}
+
+	/* Show board configuration */
+	printk("%s:", dev->board_name);
+
+	/*
+	 * Allocate the subdevice structures.  alloc_subdevice() is a
+	 * convenient macro defined in comedidev.h.
+	 */
+	if (alloc_subdevices(dev, 4) < 0) {
+		return -ENOMEM;
+	}
+
+	s = dev->subdevices + 0;
+	dev->read_subdev = s;
+	/* analog input subdevice */
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags =
+		SDF_READABLE | SDF_GROUND | SDF_COMMON | SDF_DIFF |
+		SDF_CMD_READ;
+	s->n_chan = thisboard->aiChans;
+	s->maxdata = (1 << thisboard->aiBits) - 1;
+	if (thisboard->aiMaxGain <= 32) {
+		s->range_table = &rtd_ai_7520_range;
+	} else {
+		s->range_table = &rtd_ai_4520_range;
+	}
+	s->len_chanlist = RTD_MAX_CHANLIST;	/* devpriv->fifoLen */
+	s->insn_read = rtd_ai_rinsn;
+	s->do_cmd = rtd_ai_cmd;
+	s->do_cmdtest = rtd_ai_cmdtest;
+	s->cancel = rtd_ai_cancel;
+	/*s->poll = rtd_ai_poll; *//* not ready yet */
+
+	s = dev->subdevices + 1;
+	/* analog output subdevice */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITABLE;
+	s->n_chan = 2;
+	s->maxdata = (1 << thisboard->aiBits) - 1;
+	s->range_table = &rtd_ao_range;
+	s->insn_write = rtd_ao_winsn;
+	s->insn_read = rtd_ao_rinsn;
+
+	s = dev->subdevices + 2;
+	/* digital i/o subdevice */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	/* we only support port 0 right now.  Ignoring port 1 and user IO */
+	s->n_chan = 8;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = rtd_dio_insn_bits;
+	s->insn_config = rtd_dio_insn_config;
+
+	/* timer/counter subdevices (not currently supported) */
+	s = dev->subdevices + 3;
+	s->type = COMEDI_SUBD_COUNTER;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 3;
+	s->maxdata = 0xffff;
+
+	/* initialize board, per RTD spec */
+	/* also, initialize shadow registers */
+	RtdResetBoard(dev);
+	comedi_udelay(100);	/* needed? */
+	RtdPlxInterruptWrite(dev, 0);
+	RtdInterruptMask(dev, 0);	/* and sets shadow */
+	RtdInterruptClearMask(dev, ~0);	/* and sets shadow */
+	RtdInterruptClear(dev);	/* clears bits set by mask */
+	RtdInterruptOverrunClear(dev);
+	RtdClearCGT(dev);
+	RtdAdcClearFifo(dev);
+	RtdDacClearFifo(dev, 0);
+	RtdDacClearFifo(dev, 1);
+	/* clear digital IO fifo */
+	RtdDioStatusWrite(dev, 0);	/* safe state, set shadow */
+	RtdUtcCtrlPut(dev, 0, 0x30);	/* safe state, set shadow */
+	RtdUtcCtrlPut(dev, 1, 0x30);	/* safe state, set shadow */
+	RtdUtcCtrlPut(dev, 2, 0x30);	/* safe state, set shadow */
+	RtdUtcCtrlPut(dev, 3, 0);	/* safe state, set shadow */
+	/* TODO: set user out source ??? */
+
+	/* check if our interrupt is available and get it */
+	if ((ret = comedi_request_irq(devpriv->pci_dev->irq, rtd_interrupt,
+				IRQF_SHARED, DRV_NAME, dev)) < 0) {
+		printk("Could not get interrupt! (%u)\n",
+			devpriv->pci_dev->irq);
+		return ret;
+	}
+	dev->irq = devpriv->pci_dev->irq;
+	printk("( irq=%u )", dev->irq);
+
+	ret = rtd520_probe_fifo_depth(dev);
+	if(ret < 0) {
+		return ret;
+	}
+	devpriv->fifoLen = ret;
+	printk("( fifoLen=%d )", devpriv->fifoLen);
+
+#ifdef USE_DMA
+	if (dev->irq > 0) {
+		printk("( DMA buff=%d )\n", DMA_CHAIN_COUNT);
+		/* The PLX9080 has 2 DMA controllers, but there could be 4 sources:
+		   ADC, digital, DAC1, and DAC2.  Since only the ADC supports cmd mode
+		   right now, this isn't an issue (yet) */
+		devpriv->dma0Offset = 0;
+
+		for (index = 0; index < DMA_CHAIN_COUNT; index++) {
+			devpriv->dma0Buff[index] =
+				pci_alloc_consistent(devpriv->pci_dev,
+				sizeof(u16) * devpriv->fifoLen / 2,
+				&devpriv->dma0BuffPhysAddr[index]);
+			if (devpriv->dma0Buff[index] == NULL) {
+				ret = -ENOMEM;
+				goto rtd_attach_die_error;
+			}
+			/*DPRINTK ("buff[%d] @ %p virtual, %x PCI\n",
+			   index,
+			   devpriv->dma0Buff[index], devpriv->dma0BuffPhysAddr[index]); */
+		}
+
+		/* setup DMA descriptor ring (use cpu_to_le32 for byte ordering?) */
+		devpriv->dma0Chain =
+			pci_alloc_consistent(devpriv->pci_dev,
+			sizeof(struct plx_dma_desc) * DMA_CHAIN_COUNT,
+			&devpriv->dma0ChainPhysAddr);
+		for (index = 0; index < DMA_CHAIN_COUNT; index++) {
+			devpriv->dma0Chain[index].pci_start_addr =
+				devpriv->dma0BuffPhysAddr[index];
+			devpriv->dma0Chain[index].local_start_addr =
+				DMALADDR_ADC;
+			devpriv->dma0Chain[index].transfer_size =
+				sizeof(u16) * devpriv->fifoLen / 2;
+			devpriv->dma0Chain[index].next =
+				(devpriv->dma0ChainPhysAddr + ((index +
+						1) % (DMA_CHAIN_COUNT))
+				* sizeof(devpriv->dma0Chain[0]))
+				| DMA_TRANSFER_BITS;
+			/*DPRINTK ("ring[%d] @%lx PCI: %x, local: %x, N: 0x%x, next: %x\n",
+			   index,
+			   ((long)devpriv->dma0ChainPhysAddr
+			   + (index * sizeof(devpriv->dma0Chain[0]))),
+			   devpriv->dma0Chain[index].pci_start_addr,
+			   devpriv->dma0Chain[index].local_start_addr,
+			   devpriv->dma0Chain[index].transfer_size,
+			   devpriv->dma0Chain[index].next); */
+		}
+
+		if (devpriv->dma0Chain == NULL) {
+			ret = -ENOMEM;
+			goto rtd_attach_die_error;
+		}
+
+		RtdDma0Mode(dev, DMA_MODE_BITS);
+		RtdDma0Source(dev, DMAS_ADFIFO_HALF_FULL);	/* set DMA trigger source */
+	} else {
+		printk("( no IRQ->no DMA )");
+	}
+#endif /* USE_DMA */
+
+	if (dev->irq) {		/* enable plx9080 interrupts */
+		RtdPlxInterruptWrite(dev, ICS_PIE | ICS_PLIE);
+	}
+
+	printk("\ncomedi%d: rtd520 driver attached.\n", dev->minor);
+
+	return 1;
+
+#if 0
+	/* hit an error, clean up memory and return ret */
+//rtd_attach_die_error:
+#ifdef USE_DMA
+	for (index = 0; index < DMA_CHAIN_COUNT; index++) {
+		if (NULL != devpriv->dma0Buff[index]) {	/* free buffer memory */
+			pci_free_consistent(devpriv->pci_dev,
+				sizeof(u16) * devpriv->fifoLen / 2,
+				devpriv->dma0Buff[index],
+				devpriv->dma0BuffPhysAddr[index]);
+			devpriv->dma0Buff[index] = NULL;
+		}
+	}
+	if (NULL != devpriv->dma0Chain) {
+		pci_free_consistent(devpriv->pci_dev,
+			sizeof(struct plx_dma_desc)
+			* DMA_CHAIN_COUNT,
+			devpriv->dma0Chain, devpriv->dma0ChainPhysAddr);
+		devpriv->dma0Chain = NULL;
+	}
+#endif /* USE_DMA */
+	/* subdevices and priv are freed by the core */
+	if (dev->irq) {
+		/* disable interrupt controller */
+		RtdPlxInterruptWrite(dev, RtdPlxInterruptRead(dev)
+			& ~(ICS_PLIE | ICS_DMA0_E | ICS_DMA1_E));
+		comedi_free_irq(dev->irq, dev);
+	}
+
+	/* release all regions that were allocated */
+	if (devpriv->las0) {
+		iounmap(devpriv->las0);
+	}
+	if (devpriv->las1) {
+		iounmap(devpriv->las1);
+	}
+	if (devpriv->lcfg) {
+		iounmap(devpriv->lcfg);
+	}
+	if (devpriv->pci_dev) {
+		pci_dev_put(devpriv->pci_dev);
+	}
+	return ret;
+#endif
+}
+
+/*
+ * _detach is called to deconfigure a device.  It should deallocate
+ * resources.
+ * This function is also called when _attach() fails, so it should be
+ * careful not to release resources that were not necessarily
+ * allocated by _attach().  dev->private and dev->subdevices are
+ * deallocated automatically by the core.
+ */
+static int rtd_detach(comedi_device * dev)
+{
+#ifdef USE_DMA
+	int index;
+#endif
+
+	DPRINTK("comedi%d: rtd520: removing (%ld ints)\n",
+		dev->minor, (devpriv ? devpriv->intCount : 0L));
+	if (devpriv && devpriv->lcfg) {
+		DPRINTK("(int status 0x%x, overrun status 0x%x, fifo status 0x%x)...\n", 0xffff & RtdInterruptStatus(dev), 0xffff & RtdInterruptOverrunStatus(dev), (0xffff & RtdFifoStatus(dev)) ^ 0x6666);
+	}
+
+	if (devpriv) {
+		/* Shut down any board ops by resetting it */
+#ifdef USE_DMA
+		if (devpriv->lcfg) {
+			RtdDma0Control(dev, 0);	/* disable DMA */
+			RtdDma1Control(dev, 0);	/* disable DMA */
+			RtdPlxInterruptWrite(dev, ICS_PIE | ICS_PLIE);
+		}
+#endif /* USE_DMA */
+		if (devpriv->las0) {
+			RtdResetBoard(dev);
+			RtdInterruptMask(dev, 0);
+			RtdInterruptClearMask(dev, ~0);
+			RtdInterruptClear(dev);	/* clears bits set by mask */
+		}
+#ifdef USE_DMA
+		/* release DMA */
+		for (index = 0; index < DMA_CHAIN_COUNT; index++) {
+			if (NULL != devpriv->dma0Buff[index]) {
+				pci_free_consistent(devpriv->pci_dev,
+					sizeof(u16) * devpriv->fifoLen / 2,
+					devpriv->dma0Buff[index],
+					devpriv->dma0BuffPhysAddr[index]);
+				devpriv->dma0Buff[index] = NULL;
+			}
+		}
+		if (NULL != devpriv->dma0Chain) {
+			pci_free_consistent(devpriv->pci_dev,
+				sizeof(struct plx_dma_desc) * DMA_CHAIN_COUNT,
+				devpriv->dma0Chain, devpriv->dma0ChainPhysAddr);
+			devpriv->dma0Chain = NULL;
+		}
+#endif /* USE_DMA */
+
+		/* release IRQ */
+		if (dev->irq) {
+			/* disable interrupt controller */
+			RtdPlxInterruptWrite(dev, RtdPlxInterruptRead(dev)
+				& ~(ICS_PLIE | ICS_DMA0_E | ICS_DMA1_E));
+			comedi_free_irq(dev->irq, dev);
+		}
+
+		/* release all regions that were allocated */
+		if (devpriv->las0) {
+			iounmap(devpriv->las0);
+		}
+		if (devpriv->las1) {
+			iounmap(devpriv->las1);
+		}
+		if (devpriv->lcfg) {
+			iounmap(devpriv->lcfg);
+		}
+		if (devpriv->pci_dev) {
+			if (devpriv->got_regions) {
+				comedi_pci_disable(devpriv->pci_dev);
+			}
+			pci_dev_put(devpriv->pci_dev);
+		}
+	}
+
+	printk("comedi%d: rtd520: removed.\n", dev->minor);
+
+	return 0;
+}
+
+/*
+  Convert a single comedi channel-gain entry to a RTD520 table entry
+*/
+static unsigned short rtdConvertChanGain(comedi_device * dev,
+	unsigned int comediChan, int chanIndex)
+{				/* index in channel list */
+	unsigned int chan, range, aref;
+	unsigned short r = 0;
+
+	chan = CR_CHAN(comediChan);
+	range = CR_RANGE(comediChan);
+	aref = CR_AREF(comediChan);
+
+	r |= chan & 0xf;
+
+	/* Note: we also setup the channel list bipolar flag array */
+	if (range < thisboard->range10Start) {	/* first batch are +-5 */
+		r |= 0x000;	/* +-5 range */
+		r |= (range & 0x7) << 4;	/* gain */
+		CHAN_ARRAY_SET(devpriv->chanBipolar, chanIndex);
+	} else if (range < thisboard->rangeUniStart) {	/* second batch are +-10 */
+		r |= 0x100;	/* +-10 range */
+		r |= ((range - thisboard->range10Start) & 0x7) << 4;	/* gain */
+		CHAN_ARRAY_SET(devpriv->chanBipolar, chanIndex);
+	} else {		/* last batch is +10 */
+		r |= 0x200;	/* +10 range */
+		r |= ((range - thisboard->rangeUniStart) & 0x7) << 4;	/* gain */
+		CHAN_ARRAY_CLEAR(devpriv->chanBipolar, chanIndex);
+	}
+
+	switch (aref) {
+	case AREF_GROUND:	/* on-board ground */
+		break;
+
+	case AREF_COMMON:
+		r |= 0x80;	/* ref external analog common */
+		break;
+
+	case AREF_DIFF:
+		r |= 0x400;	/* differential inputs */
+		break;
+
+	case AREF_OTHER:	/* ??? */
+		break;
+	}
+	/*printk ("chan=%d r=%d a=%d -> 0x%x\n",
+	   chan, range, aref, r); */
+	return r;
+}
+
+/*
+  Setup the channel-gain table from a comedi list
+*/
+static void rtd_load_channelgain_list(comedi_device * dev,
+	unsigned int n_chan, unsigned int *list)
+{
+	if (n_chan > 1) {	/* setup channel gain table */
+		int ii;
+		RtdClearCGT(dev);
+		RtdEnableCGT(dev, 1);	/* enable table */
+		for (ii = 0; ii < n_chan; ii++) {
+			RtdWriteCGTable(dev, rtdConvertChanGain(dev, list[ii],
+					ii));
+		}
+	} else {		/* just use the channel gain latch */
+		RtdEnableCGT(dev, 0);	/* disable table, enable latch */
+		RtdWriteCGLatch(dev, rtdConvertChanGain(dev, list[0], 0));
+	}
+}
+
+/* determine fifo size by doing adc conversions until the fifo half
+empty status flag clears */
+static int rtd520_probe_fifo_depth(comedi_device *dev)
+{
+	lsampl_t chanspec = CR_PACK(0, 0, AREF_GROUND);
+	unsigned i;
+	static const unsigned limit = 0x2000;
+	unsigned fifo_size = 0;
+
+	RtdAdcClearFifo(dev);
+	rtd_load_channelgain_list(dev, 1, &chanspec);
+	RtdAdcConversionSource(dev, 0);	/* software */
+	/* convert  samples */
+	for (i = 0; i < limit; ++i) {
+		unsigned fifo_status;
+		/* trigger conversion */
+		RtdAdcStart(dev);
+		comedi_udelay(1);
+		fifo_status = RtdFifoStatus(dev);
+		if((fifo_status & FS_ADC_HEMPTY) == 0) {
+			fifo_size = 2 * i;
+			break;
+		}
+	}
+	if(i == limit)
+	{
+		rt_printk("\ncomedi: %s: failed to probe fifo size.\n", DRV_NAME);
+		return -EIO;
+	}
+	RtdAdcClearFifo(dev);
+	if(fifo_size != 0x400 || fifo_size != 0x2000)
+	{
+		rt_printk("\ncomedi: %s: unexpected fifo size of %i, expected 1024 or 8192.\n",
+			DRV_NAME, fifo_size);
+		return -EIO;
+	}
+	return fifo_size;
+}
+
+/*
+  "instructions" read/write data in "one-shot" or "software-triggered"
+  mode (simplest case).
+  This doesnt use interrupts.
+
+  Note, we don't do any settling delays.  Use a instruction list to
+  select, delay, then read.
+ */
+static int rtd_ai_rinsn(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	int n, ii;
+	int stat;
+
+	/* clear any old fifo data */
+	RtdAdcClearFifo(dev);
+
+	/* write channel to multiplexer and clear channel gain table */
+	rtd_load_channelgain_list(dev, 1, &insn->chanspec);
+
+	/* set conversion source */
+	RtdAdcConversionSource(dev, 0);	/* software */
+
+	/* convert n samples */
+	for (n = 0; n < insn->n; n++) {
+		s16 d;
+		/* trigger conversion */
+		RtdAdcStart(dev);
+
+		for (ii = 0; ii < RTD_ADC_TIMEOUT; ++ii) {
+			stat = RtdFifoStatus(dev);
+			if (stat & FS_ADC_NOT_EMPTY)	/* 1 -> not empty */
+				break;
+			WAIT_QUIETLY;
+		}
+		if (ii >= RTD_ADC_TIMEOUT) {
+			DPRINTK("rtd520: Error: ADC never finished! FifoStatus=0x%x\n", stat ^ 0x6666);
+			return -ETIMEDOUT;
+		}
+
+		/* read data */
+		d = RtdAdcFifoGet(dev);	/* get 2s comp value */
+		/*printk ("rtd520: Got 0x%x after %d usec\n", d, ii+1); */
+		d = d >> 3;	/* low 3 bits are marker lines */
+		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, 0)) {
+			data[n] = d + 2048;	/* convert to comedi unsigned data */
+		} else {
+			data[n] = d;
+		}
+	}
+
+	/* return the number of samples read/written */
+	return n;
+}
+
+/*
+  Get what we know is there.... Fast!
+  This uses 1/2 the bus cycles of read_dregs (below).
+
+  The manual claims that we can do a lword read, but it doesn't work here.
+*/
+static int ai_read_n(comedi_device * dev, comedi_subdevice * s, int count)
+{
+	int ii;
+
+	for (ii = 0; ii < count; ii++) {
+		sampl_t sample;
+		s16 d;
+
+		if (0 == devpriv->aiCount) {	/* done */
+			d = RtdAdcFifoGet(dev);	/* Read N and discard */
+			continue;
+		}
+#if 0
+		if (0 == (RtdFifoStatus(dev) & FS_ADC_NOT_EMPTY)) {	/* DEBUG */
+			DPRINTK("comedi: READ OOPS on %d of %d\n", ii + 1,
+				count);
+			break;
+		}
+#endif
+		d = RtdAdcFifoGet(dev);	/* get 2s comp value */
+
+		d = d >> 3;	/* low 3 bits are marker lines */
+		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, s->async->cur_chan)) {
+			sample = d + 2048;	/* convert to comedi unsigned data */
+		} else {
+			sample = d;
+		}
+		if (!comedi_buf_put(s->async, sample))
+			return -1;
+
+		if (devpriv->aiCount > 0)	/* < 0, means read forever */
+			devpriv->aiCount--;
+	}
+	return 0;
+}
+
+/*
+  unknown amout of data is waiting in fifo.
+*/
+static int ai_read_dregs(comedi_device * dev, comedi_subdevice * s)
+{
+	while (RtdFifoStatus(dev) & FS_ADC_NOT_EMPTY) {	/* 1 -> not empty */
+		sampl_t sample;
+		s16 d = RtdAdcFifoGet(dev);	/* get 2s comp value */
+
+		if (0 == devpriv->aiCount) {	/* done */
+			continue;	/* read rest */
+		}
+
+		d = d >> 3;	/* low 3 bits are marker lines */
+		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, s->async->cur_chan)) {
+			sample = d + 2048;	/* convert to comedi unsigned data */
+		} else {
+			sample = d;
+		}
+		if (!comedi_buf_put(s->async, sample))
+			return -1;
+
+		if (devpriv->aiCount > 0)	/* < 0, means read forever */
+			devpriv->aiCount--;
+	}
+	return 0;
+}
+
+#ifdef USE_DMA
+/*
+  Terminate a DMA transfer and wait for everything to quiet down
+*/
+void abort_dma(comedi_device * dev, unsigned int channel)
+{				/* DMA channel 0, 1 */
+	unsigned long dma_cs_addr;	/* the control/status register */
+	uint8_t status;
+	unsigned int ii;
+	//unsigned long flags;
+
+	dma_cs_addr = (unsigned long)devpriv->lcfg
+		+ ((channel == 0) ? LCFG_DMACSR0 : LCFG_DMACSR1);
+
+	// spinlock for plx dma control/status reg
+	//comedi_spin_lock_irqsave( &dev->spinlock, flags );
+
+	// abort dma transfer if necessary
+	status = readb(dma_cs_addr);
+	if ((status & PLX_DMA_EN_BIT) == 0) {	/* not enabled (Error?) */
+		DPRINTK("rtd520: AbortDma on non-active channel %d (0x%x)\n",
+			channel, status);
+		goto abortDmaExit;
+	}
+
+	/* wait to make sure done bit is zero (needed?) */
+	for (ii = 0; (status & PLX_DMA_DONE_BIT) && ii < RTD_DMA_TIMEOUT; ii++) {
+		WAIT_QUIETLY;
+		status = readb(dma_cs_addr);
+	}
+	if (status & PLX_DMA_DONE_BIT) {
+		printk("rtd520: Timeout waiting for dma %i done clear\n",
+			channel);
+		goto abortDmaExit;
+	}
+
+	/* disable channel (required) */
+	writeb(0, dma_cs_addr);
+	comedi_udelay(1);	/* needed?? */
+	/* set abort bit for channel */
+	writeb(PLX_DMA_ABORT_BIT, dma_cs_addr);
+
+	// wait for dma done bit to be set
+	status = readb(dma_cs_addr);
+	for (ii = 0;
+		(status & PLX_DMA_DONE_BIT) == 0 && ii < RTD_DMA_TIMEOUT;
+		ii++) {
+		status = readb(dma_cs_addr);
+		WAIT_QUIETLY;
+	}
+	if ((status & PLX_DMA_DONE_BIT) == 0) {
+		printk("rtd520: Timeout waiting for dma %i done set\n",
+			channel);
+	}
+
+      abortDmaExit:
+	//comedi_spin_unlock_irqrestore( &dev->spinlock, flags );
+}
+
+/*
+  Process what is in the DMA transfer buffer and pass to comedi
+  Note: this is not re-entrant
+*/
+static int ai_process_dma(comedi_device * dev, comedi_subdevice * s)
+{
+	int ii, n;
+	s16 *dp;
+
+	if (devpriv->aiCount == 0)	/* transfer already complete */
+		return 0;
+
+	dp = devpriv->dma0Buff[devpriv->dma0Offset];
+	for (ii = 0; ii < devpriv->fifoLen / 2;) {	/* convert samples */
+		sampl_t sample;
+
+		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, s->async->cur_chan)) {
+			sample = (*dp >> 3) + 2048;	/* convert to comedi unsigned data */
+		} else {
+			sample = *dp >> 3;	/* low 3 bits are marker lines */
+		}
+		*dp++ = sample;	/* put processed value back */
+
+		if (++s->async->cur_chan >= s->async->cmd.chanlist_len)
+			s->async->cur_chan = 0;
+
+		++ii;		/* number ready to transfer */
+		if (devpriv->aiCount > 0) {	/* < 0, means read forever */
+			if (--devpriv->aiCount == 0) {	/* done */
+				/*DPRINTK ("rtd520: Final %d samples\n", ii); */
+				break;
+			}
+		}
+	}
+
+	/* now pass the whole array to the comedi buffer */
+	dp = devpriv->dma0Buff[devpriv->dma0Offset];
+	n = comedi_buf_write_alloc(s->async, ii * sizeof(s16));
+	if (n < (ii * sizeof(s16))) {	/* any residual is an error */
+		DPRINTK("rtd520:ai_process_dma buffer overflow %d samples!\n",
+			ii - (n / sizeof(s16)));
+		s->async->events |= COMEDI_CB_ERROR;
+		return -1;
+	}
+	comedi_buf_memcpy_to(s->async, 0, dp, n);
+	comedi_buf_write_free(s->async, n);
+
+	/* always at least 1 scan -- 1/2 FIFO is larger than our max scan list */
+	s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
+
+	if (++devpriv->dma0Offset >= DMA_CHAIN_COUNT) {	/* next buffer */
+		devpriv->dma0Offset = 0;
+	}
+	return 0;
+}
+#endif /* USE_DMA */
+
+/*
+  Handle all rtd520 interrupts.
+  Runs atomically and is never re-entered.
+  This is a "slow handler";  other interrupts may be active.
+  The data conversion may someday happen in a "bottom half".
+*/
+static irqreturn_t rtd_interrupt(int irq,	/* interrupt number (ignored) */
+	void *d			/* our data */
+	PT_REGS_ARG)
+{				/* cpu context (ignored) */
+	comedi_device *dev = d;	/* must be called "dev" for devpriv */
+	u16 status;
+	u16 fifoStatus;
+	comedi_subdevice *s = dev->subdevices + 0;	/* analog in subdevice */
+
+	if (!dev->attached) {
+		return IRQ_NONE;
+	}
+
+	devpriv->intCount++;	/* DEBUG statistics */
+
+	fifoStatus = RtdFifoStatus(dev);
+	/* check for FIFO full, this automatically halts the ADC! */
+	if (!(fifoStatus & FS_ADC_NOT_FULL)) {	/* 0 -> full */
+		DPRINTK("rtd520: FIFO full! fifo_status=0x%x\n", (fifoStatus ^ 0x6666) & 0x7777);	/* should be all 0s */
+		goto abortTransfer;
+	}
+#ifdef USE_DMA
+	if (devpriv->flags & DMA0_ACTIVE) {	/* Check DMA */
+		u32 istatus = RtdPlxInterruptRead(dev);
+
+		if (istatus & ICS_DMA0_A) {
+			if (ai_process_dma(dev, s) < 0) {
+				DPRINTK("rtd520: comedi read buffer overflow (DMA) with %ld to go!\n", devpriv->aiCount);
+				RtdDma0Control(dev,
+					(devpriv->
+						dma0Control &
+						~PLX_DMA_START_BIT)
+					| PLX_CLEAR_DMA_INTR_BIT);
+				goto abortTransfer;
+			}
+
+			/*DPRINTK ("rtd520: DMA transfer: %ld to go, istatus %x\n",
+			   devpriv->aiCount, istatus); */
+			RtdDma0Control(dev,
+				(devpriv->dma0Control & ~PLX_DMA_START_BIT)
+				| PLX_CLEAR_DMA_INTR_BIT);
+			if (0 == devpriv->aiCount) {	/* counted down */
+				DPRINTK("rtd520: Samples Done (DMA).\n");
+				goto transferDone;
+			}
+			comedi_event(dev, s);
+		} else {
+			/*DPRINTK ("rtd520: No DMA ready: istatus %x\n", istatus); */
+		}
+	}
+	/* Fall through and check for other interrupt sources */
+#endif /* USE_DMA */
+
+	status = RtdInterruptStatus(dev);
+	/* if interrupt was not caused by our board, or handled above */
+	if (0 == status) {
+		return IRQ_HANDLED;
+	}
+
+	if (status & IRQM_ADC_ABOUT_CNT) {	/* sample count -> read FIFO */
+		/* since the priority interrupt controller may have queued a sample
+		   counter interrupt, even though we have already finished,
+		   we must handle the possibility that there is no data here */
+		if (!(fifoStatus & FS_ADC_HEMPTY)) {	/* 0 -> 1/2 full */
+			/*DPRINTK("rtd520: Sample int, reading 1/2FIFO.  fifo_status 0x%x\n",
+			   (fifoStatus ^ 0x6666) & 0x7777); */
+			if (ai_read_n(dev, s, devpriv->fifoLen / 2) < 0) {
+				DPRINTK("rtd520: comedi read buffer overflow (1/2FIFO) with %ld to go!\n", devpriv->aiCount);
+				goto abortTransfer;
+			}
+			if (0 == devpriv->aiCount) {	/* counted down */
+				DPRINTK("rtd520: Samples Done (1/2). fifo_status was 0x%x\n", (fifoStatus ^ 0x6666) & 0x7777);	/* should be all 0s */
+				goto transferDone;
+			}
+			comedi_event(dev, s);
+		} else if (devpriv->transCount > 0) {	/* read often */
+			/*DPRINTK("rtd520: Sample int, reading %d  fifo_status 0x%x\n",
+			   devpriv->transCount, (fifoStatus ^ 0x6666) & 0x7777); */
+			if (fifoStatus & FS_ADC_NOT_EMPTY) {	/* 1 -> not empty */
+				if (ai_read_n(dev, s, devpriv->transCount) < 0) {
+					DPRINTK("rtd520: comedi read buffer overflow (N) with %ld to go!\n", devpriv->aiCount);
+					goto abortTransfer;
+				}
+				if (0 == devpriv->aiCount) {	/* counted down */
+					DPRINTK("rtd520: Samples Done (N). fifo_status was 0x%x\n", (fifoStatus ^ 0x6666) & 0x7777);
+					goto transferDone;
+				}
+				comedi_event(dev, s);
+			}
+		} else {	/* wait for 1/2 FIFO (old) */
+			DPRINTK("rtd520: Sample int.  Wait for 1/2. fifo_status 0x%x\n", (fifoStatus ^ 0x6666) & 0x7777);
+		}
+	} else {
+		DPRINTK("rtd520: unknown interrupt source!\n");
+	}
+
+	if (0xffff & RtdInterruptOverrunStatus(dev)) {	/* interrupt overrun */
+		DPRINTK("rtd520: Interrupt overrun with %ld to go! over_status=0x%x\n", devpriv->aiCount, 0xffff & RtdInterruptOverrunStatus(dev));
+		goto abortTransfer;
+	}
+
+	/* clear the interrupt */
+	RtdInterruptClearMask(dev, status);
+	RtdInterruptClear(dev);
+	return IRQ_HANDLED;
+
+      abortTransfer:
+	RtdAdcClearFifo(dev);	/* clears full flag */
+	s->async->events |= COMEDI_CB_ERROR;
+	devpriv->aiCount = 0;	/* stop and don't transfer any more */
+	/* fall into transferDone */
+
+      transferDone:
+	RtdPacerStopSource(dev, 0);	/* stop on SOFTWARE stop */
+	RtdPacerStop(dev);	/* Stop PACER */
+	RtdAdcConversionSource(dev, 0);	/* software trigger only */
+	RtdInterruptMask(dev, 0);	/* mask out SAMPLE */
+#ifdef USE_DMA
+	if (devpriv->flags & DMA0_ACTIVE) {
+		RtdPlxInterruptWrite(dev,	/* disable any more interrupts */
+			RtdPlxInterruptRead(dev) & ~ICS_DMA0_E);
+		abort_dma(dev, 0);
+		devpriv->flags &= ~DMA0_ACTIVE;
+		/* if Using DMA, then we should have read everything by now */
+		if (devpriv->aiCount > 0) {
+			DPRINTK("rtd520: Lost DMA data! %ld remain\n",
+				devpriv->aiCount);
+		}
+	}
+#endif /* USE_DMA */
+
+	if (devpriv->aiCount > 0) {	/* there shouldn't be anything left */
+		fifoStatus = RtdFifoStatus(dev);
+		DPRINTK("rtd520: Finishing up. %ld remain, fifoStat=%x\n", devpriv->aiCount, (fifoStatus ^ 0x6666) & 0x7777);	/* should read all 0s */
+		ai_read_dregs(dev, s);	/* read anything left in FIFO */
+	}
+
+	s->async->events |= COMEDI_CB_EOA;	/* signal end to comedi */
+	comedi_event(dev, s);
+
+	/* clear the interrupt */
+	status = RtdInterruptStatus(dev);
+	RtdInterruptClearMask(dev, status);
+	RtdInterruptClear(dev);
+
+	fifoStatus = RtdFifoStatus(dev);	/* DEBUG */
+	DPRINTK("rtd520: Acquisition complete. %ld ints, intStat=%x, overStat=%x\n", devpriv->intCount, status, 0xffff & RtdInterruptOverrunStatus(dev));
+
+	return IRQ_HANDLED;
+}
+
+#if 0
+/*
+  return the number of samples available
+*/
+static int rtd_ai_poll(comedi_device * dev, comedi_subdevice * s)
+{
+	/* TODO: This needs to mask interrupts, read_dregs, and then re-enable */
+	/* Not sure what to do if DMA is active */
+	return s->async->buf_write_count - s->async->buf_read_count;
+}
+#endif
+
+/*
+  cmdtest tests a particular command to see if it is valid.
+  Using the cmdtest ioctl, a user can create a valid cmd
+  and then have it executed by the cmd ioctl (asyncronously).
+
+  cmdtest returns 1,2,3,4 or 0, depending on which tests
+  the command passes.
+*/
+
+static int rtd_ai_cmdtest(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd)
+{
+	int err = 0;
+	int tmp;
+
+	/* step 1: make sure trigger sources are trivially valid */
+
+	tmp = cmd->start_src;
+	cmd->start_src &= TRIG_NOW;
+	if (!cmd->start_src || tmp != cmd->start_src) {
+		err++;
+	}
+
+	tmp = cmd->scan_begin_src;
+	cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT;
+	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src) {
+		err++;
+	}
+
+	tmp = cmd->convert_src;
+	cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
+	if (!cmd->convert_src || tmp != cmd->convert_src) {
+		err++;
+	}
+
+	tmp = cmd->scan_end_src;
+	cmd->scan_end_src &= TRIG_COUNT;
+	if (!cmd->scan_end_src || tmp != cmd->scan_end_src) {
+		err++;
+	}
+
+	tmp = cmd->stop_src;
+	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
+	if (!cmd->stop_src || tmp != cmd->stop_src) {
+		err++;
+	}
+
+	if (err)
+		return 1;
+
+	/* step 2: make sure trigger sources are unique
+	   and mutually compatible */
+	/* note that mutual compatiblity is not an issue here */
+	if (cmd->scan_begin_src != TRIG_TIMER &&
+		cmd->scan_begin_src != TRIG_EXT) {
+		err++;
+	}
+	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT) {
+		err++;
+	}
+	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE) {
+		err++;
+	}
+
+	if (err) {
+		return 2;
+	}
+
+	/* step 3: make sure arguments are trivially compatible */
+
+	if (cmd->start_arg != 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		/* Note: these are time periods, not actual rates */
+		if (1 == cmd->chanlist_len) {	/* no scanning */
+			if (cmd->scan_begin_arg < RTD_MAX_SPEED_1) {
+				cmd->scan_begin_arg = RTD_MAX_SPEED_1;
+				rtd_ns_to_timer(&cmd->scan_begin_arg,
+					TRIG_ROUND_UP);
+				err++;
+			}
+			if (cmd->scan_begin_arg > RTD_MIN_SPEED_1) {
+				cmd->scan_begin_arg = RTD_MIN_SPEED_1;
+				rtd_ns_to_timer(&cmd->scan_begin_arg,
+					TRIG_ROUND_DOWN);
+				err++;
+			}
+		} else {
+			if (cmd->scan_begin_arg < RTD_MAX_SPEED) {
+				cmd->scan_begin_arg = RTD_MAX_SPEED;
+				rtd_ns_to_timer(&cmd->scan_begin_arg,
+					TRIG_ROUND_UP);
+				err++;
+			}
+			if (cmd->scan_begin_arg > RTD_MIN_SPEED) {
+				cmd->scan_begin_arg = RTD_MIN_SPEED;
+				rtd_ns_to_timer(&cmd->scan_begin_arg,
+					TRIG_ROUND_DOWN);
+				err++;
+			}
+		}
+	} else {
+		/* external trigger */
+		/* should be level/edge, hi/lo specification here */
+		/* should specify multiple external triggers */
+		if (cmd->scan_begin_arg > 9) {
+			cmd->scan_begin_arg = 9;
+			err++;
+		}
+	}
+	if (cmd->convert_src == TRIG_TIMER) {
+		if (1 == cmd->chanlist_len) {	/* no scanning */
+			if (cmd->convert_arg < RTD_MAX_SPEED_1) {
+				cmd->convert_arg = RTD_MAX_SPEED_1;
+				rtd_ns_to_timer(&cmd->convert_arg,
+					TRIG_ROUND_UP);
+				err++;
+			}
+			if (cmd->convert_arg > RTD_MIN_SPEED_1) {
+				cmd->convert_arg = RTD_MIN_SPEED_1;
+				rtd_ns_to_timer(&cmd->convert_arg,
+					TRIG_ROUND_DOWN);
+				err++;
+			}
+		} else {
+			if (cmd->convert_arg < RTD_MAX_SPEED) {
+				cmd->convert_arg = RTD_MAX_SPEED;
+				rtd_ns_to_timer(&cmd->convert_arg,
+					TRIG_ROUND_UP);
+				err++;
+			}
+			if (cmd->convert_arg > RTD_MIN_SPEED) {
+				cmd->convert_arg = RTD_MIN_SPEED;
+				rtd_ns_to_timer(&cmd->convert_arg,
+					TRIG_ROUND_DOWN);
+				err++;
+			}
+		}
+	} else {
+		/* external trigger */
+		/* see above */
+		if (cmd->convert_arg > 9) {
+			cmd->convert_arg = 9;
+			err++;
+		}
+	}
+
+#if 0
+	if (cmd->scan_end_arg != cmd->chanlist_len) {
+		cmd->scan_end_arg = cmd->chanlist_len;
+		err++;
+	}
+#endif
+	if (cmd->stop_src == TRIG_COUNT) {
+		/* TODO check for rounding error due to counter wrap */
+
+	} else {
+		/* TRIG_NONE */
+		if (cmd->stop_arg != 0) {
+			cmd->stop_arg = 0;
+			err++;
+		}
+	}
+
+	if (err) {
+		return 3;
+	}
+
+	/* 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++;
+		}
+	}
+	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++;
+		}
+	}
+
+	if (err) {
+		return 4;
+	}
+
+	return 0;
+}
+
+/*
+  Execute a analog in command with many possible triggering options.
+  The data get stored in the async structure of the subdevice.
+  This is usually done by an interrupt handler.
+  Userland gets to the data using read calls.
+*/
+static int rtd_ai_cmd(comedi_device * dev, comedi_subdevice * s)
+{
+	comedi_cmd *cmd = &s->async->cmd;
+	int timer;
+
+	/* stop anything currently running */
+	RtdPacerStopSource(dev, 0);	/* stop on SOFTWARE stop */
+	RtdPacerStop(dev);	/* make sure PACER is stopped */
+	RtdAdcConversionSource(dev, 0);	/* software trigger only */
+	RtdInterruptMask(dev, 0);
+#ifdef USE_DMA
+	if (devpriv->flags & DMA0_ACTIVE) {	/* cancel anything running */
+		RtdPlxInterruptWrite(dev,	/* disable any more interrupts */
+			RtdPlxInterruptRead(dev) & ~ICS_DMA0_E);
+		abort_dma(dev, 0);
+		devpriv->flags &= ~DMA0_ACTIVE;
+		if (RtdPlxInterruptRead(dev) & ICS_DMA0_A) {	/*clear pending int */
+			RtdDma0Control(dev, PLX_CLEAR_DMA_INTR_BIT);
+		}
+	}
+	RtdDma0Reset(dev);	/* reset onboard state */
+#endif /* USE_DMA */
+	RtdAdcClearFifo(dev);	/* clear any old data */
+	RtdInterruptOverrunClear(dev);
+	devpriv->intCount = 0;
+
+	if (!dev->irq) {	/* we need interrupts for this */
+		DPRINTK("rtd520: ERROR! No interrupt available!\n");
+		return -ENXIO;
+	}
+
+	/* start configuration */
+	/* load channel list and reset CGT */
+	rtd_load_channelgain_list(dev, cmd->chanlist_len, cmd->chanlist);
+
+	/* setup the common case and override if needed */
+	if (cmd->chanlist_len > 1) {
+		/*DPRINTK ("rtd520: Multi channel setup\n"); */
+		RtdPacerStartSource(dev, 0);	/* software triggers pacer */
+		RtdBurstStartSource(dev, 1);	/* PACER triggers burst */
+		RtdAdcConversionSource(dev, 2);	/* BURST triggers ADC */
+	} else {		/* single channel */
+		/*DPRINTK ("rtd520: single channel setup\n"); */
+		RtdPacerStartSource(dev, 0);	/* software triggers pacer */
+		RtdAdcConversionSource(dev, 1);	/* PACER triggers ADC */
+	}
+	RtdAboutCounter(dev, devpriv->fifoLen / 2 - 1);	/* 1/2 FIFO */
+
+	if (TRIG_TIMER == cmd->scan_begin_src) {
+		/* scan_begin_arg is in nanoseconds */
+		/* find out how many samples to wait before transferring */
+		if (cmd->flags & TRIG_WAKE_EOS) {
+			/* this may generate un-sustainable interrupt rates */
+			/* the application is responsible for doing the right thing */
+			devpriv->transCount = cmd->chanlist_len;
+			devpriv->flags |= SEND_EOS;
+		} else {
+			/* arrange to transfer data periodically */
+			devpriv->transCount
+				=
+				(TRANS_TARGET_PERIOD * cmd->chanlist_len) /
+				cmd->scan_begin_arg;
+			if (devpriv->transCount < cmd->chanlist_len) {
+				/* tranfer after each scan (and avoid 0) */
+				devpriv->transCount = cmd->chanlist_len;
+			} else {	/* make a multiple of scan length */
+				devpriv->transCount =
+					(devpriv->transCount +
+					cmd->chanlist_len - 1)
+					/ cmd->chanlist_len;
+				devpriv->transCount *= cmd->chanlist_len;
+			}
+			devpriv->flags |= SEND_EOS;
+		}
+		if (devpriv->transCount >= (devpriv->fifoLen / 2)) {
+			/* out of counter range, use 1/2 fifo instead */
+			devpriv->transCount = 0;
+			devpriv->flags &= ~SEND_EOS;
+		} else {
+			/* interrupt for each tranfer */
+			RtdAboutCounter(dev, devpriv->transCount - 1);
+		}
+
+		DPRINTK("rtd520: scanLen=%d tranferCount=%d fifoLen=%d\n  scanTime(ns)=%d flags=0x%x\n", cmd->chanlist_len, devpriv->transCount, devpriv->fifoLen, cmd->scan_begin_arg, devpriv->flags);
+	} else {		/* unknown timing, just use 1/2 FIFO */
+		devpriv->transCount = 0;
+		devpriv->flags &= ~SEND_EOS;
+	}
+	RtdPacerClockSource(dev, 1);	/* use INTERNAL 8Mhz clock source */
+	RtdAboutStopEnable(dev, 1);	/* just interrupt, dont stop */
+
+	/* BUG??? these look like enumerated values, but they are bit fields */
+
+	/* First, setup when to stop */
+	switch (cmd->stop_src) {
+	case TRIG_COUNT:	/* stop after N scans */
+		devpriv->aiCount = cmd->stop_arg * cmd->chanlist_len;
+		if ((devpriv->transCount > 0)
+			&& (devpriv->transCount > devpriv->aiCount)) {
+			devpriv->transCount = devpriv->aiCount;
+		}
+		break;
+
+	case TRIG_NONE:	/* stop when cancel is called */
+		devpriv->aiCount = -1;	/* read forever */
+		break;
+
+	default:
+		DPRINTK("rtd520: Warning! ignoring stop_src mode %d\n",
+			cmd->stop_src);
+	}
+
+	/* Scan timing */
+	switch (cmd->scan_begin_src) {
+	case TRIG_TIMER:	/* periodic scanning */
+		timer = rtd_ns_to_timer(&cmd->scan_begin_arg,
+			TRIG_ROUND_NEAREST);
+		/* set PACER clock */
+		/*DPRINTK ("rtd520: loading %d into pacer\n", timer); */
+		RtdPacerCounter(dev, timer);
+
+		break;
+
+	case TRIG_EXT:
+		RtdPacerStartSource(dev, 1);	/* EXTERNALy trigger pacer */
+		break;
+
+	default:
+		DPRINTK("rtd520: Warning! ignoring scan_begin_src mode %d\n",
+			cmd->scan_begin_src);
+	}
+
+	/* Sample timing within a scan */
+	switch (cmd->convert_src) {
+	case TRIG_TIMER:	/* periodic */
+		if (cmd->chanlist_len > 1) {	/* only needed for multi-channel */
+			timer = rtd_ns_to_timer(&cmd->convert_arg,
+				TRIG_ROUND_NEAREST);
+			/* setup BURST clock */
+			/*DPRINTK ("rtd520: loading %d into burst\n", timer); */
+			RtdBurstCounter(dev, timer);
+		}
+
+		break;
+
+	case TRIG_EXT:		/* external */
+		RtdBurstStartSource(dev, 2);	/* EXTERNALy trigger burst */
+		break;
+
+	default:
+		DPRINTK("rtd520: Warning! ignoring convert_src mode %d\n",
+			cmd->convert_src);
+	}
+	/* end configuration */
+
+	/* This doesn't seem to work.  There is no way to clear an interrupt
+	   that the priority controller has queued! */
+	RtdInterruptClearMask(dev, ~0);	/* clear any existing flags */
+	RtdInterruptClear(dev);
+
+	/* TODO: allow multiple interrupt sources */
+	if (devpriv->transCount > 0) {	/* transfer every N samples */
+		RtdInterruptMask(dev, IRQM_ADC_ABOUT_CNT);
+		DPRINTK("rtd520: Transferring every %d\n", devpriv->transCount);
+	} else {		/* 1/2 FIFO transfers */
+#ifdef USE_DMA
+		devpriv->flags |= DMA0_ACTIVE;
+
+		/* point to first transfer in ring */
+		devpriv->dma0Offset = 0;
+		RtdDma0Mode(dev, DMA_MODE_BITS);
+		RtdDma0Next(dev,	/* point to first block */
+			devpriv->dma0Chain[DMA_CHAIN_COUNT - 1].next);
+		RtdDma0Source(dev, DMAS_ADFIFO_HALF_FULL);	/* set DMA trigger source */
+
+		RtdPlxInterruptWrite(dev,	/* enable interrupt */
+			RtdPlxInterruptRead(dev) | ICS_DMA0_E);
+		/* Must be 2 steps.  See PLX app note about "Starting a DMA transfer" */
+		RtdDma0Control(dev, PLX_DMA_EN_BIT);	/* enable DMA (clear INTR?) */
+		RtdDma0Control(dev, PLX_DMA_EN_BIT | PLX_DMA_START_BIT);	/*start DMA */
+		DPRINTK("rtd520: Using DMA0 transfers. plxInt %x RtdInt %x\n",
+			RtdPlxInterruptRead(dev), devpriv->intMask);
+#else /* USE_DMA */
+		RtdInterruptMask(dev, IRQM_ADC_ABOUT_CNT);
+		DPRINTK("rtd520: Transferring every 1/2 FIFO\n");
+#endif /* USE_DMA */
+	}
+
+	/* BUG: start_src is ASSUMED to be TRIG_NOW */
+	/* BUG? it seems like things are running before the "start" */
+	RtdPacerStart(dev);	/* Start PACER */
+	return 0;
+}
+
+/*
+  Stop a running data aquisition.
+*/
+static int rtd_ai_cancel(comedi_device * dev, comedi_subdevice * s)
+{
+	u16 status;
+
+	RtdPacerStopSource(dev, 0);	/* stop on SOFTWARE stop */
+	RtdPacerStop(dev);	/* Stop PACER */
+	RtdAdcConversionSource(dev, 0);	/* software trigger only */
+	RtdInterruptMask(dev, 0);
+	devpriv->aiCount = 0;	/* stop and don't transfer any more */
+#ifdef USE_DMA
+	if (devpriv->flags & DMA0_ACTIVE) {
+		RtdPlxInterruptWrite(dev,	/* disable any more interrupts */
+			RtdPlxInterruptRead(dev) & ~ICS_DMA0_E);
+		abort_dma(dev, 0);
+		devpriv->flags &= ~DMA0_ACTIVE;
+	}
+#endif /* USE_DMA */
+	status = RtdInterruptStatus(dev);
+	DPRINTK("rtd520: Acquisition canceled. %ld ints, intStat=%x, overStat=%x\n", devpriv->intCount, status, 0xffff & RtdInterruptOverrunStatus(dev));
+	return 0;
+}
+
+/*
+  Given a desired period and the clock period (both in ns),
+  return the proper counter value (divider-1).
+  Sets the original period to be the true value.
+  Note: you have to check if the value is larger than the counter range!
+*/
+static int rtd_ns_to_timer_base(unsigned int *nanosec,	/* desired period (in ns) */
+	int round_mode, int base)
+{				/* clock period (in ns) */
+	int divider;
+
+	switch (round_mode) {
+	case TRIG_ROUND_NEAREST:
+	default:
+		divider = (*nanosec + base / 2) / base;
+		break;
+	case TRIG_ROUND_DOWN:
+		divider = (*nanosec) / base;
+		break;
+	case TRIG_ROUND_UP:
+		divider = (*nanosec + base - 1) / base;
+		break;
+	}
+	if (divider < 2)
+		divider = 2;	/* min is divide by 2 */
+
+	/* Note: we don't check for max, because different timers
+	   have different ranges */
+
+	*nanosec = base * divider;
+	return divider - 1;	/* countdown is divisor+1 */
+}
+
+/*
+  Given a desired period (in ns),
+  return the proper counter value (divider-1) for the internal clock.
+  Sets the original period to be the true value.
+*/
+static int rtd_ns_to_timer(unsigned int *ns, int round_mode)
+{
+	return rtd_ns_to_timer_base(ns, round_mode, RTD_CLOCK_BASE);
+}
+
+/*
+  Output one (or more) analog values to a single port as fast as possible.
+*/
+static int rtd_ao_winsn(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	int i;
+	int chan = CR_CHAN(insn->chanspec);
+	int range = CR_RANGE(insn->chanspec);
+
+	/* Configure the output range (table index matches the range values) */
+	RtdDacRange(dev, chan, range);
+
+	/* Writing a list of values to an AO channel is probably not
+	 * very useful, but that's how the interface is defined. */
+	for (i = 0; i < insn->n; ++i) {
+		int val = data[i] << 3;
+		int stat = 0;	/* initialize to avoid bogus warning */
+		int ii;
+
+		/* VERIFY: comedi range and offset conversions */
+
+		if ((range > 1)	/* bipolar */
+			&&(data[i] < 2048)) {
+			/* offset and sign extend */
+			val = (((int)data[i]) - 2048) << 3;
+		} else {	/* unipolor */
+			val = data[i] << 3;
+		}
+
+		DPRINTK("comedi: rtd520 DAC chan=%d range=%d writing %d as 0x%x\n", chan, range, data[i], val);
+
+		/* a typical programming sequence */
+		RtdDacFifoPut(dev, chan, val);	/* put the value in */
+		RtdDacUpdate(dev, chan);	/* trigger the conversion */
+
+		devpriv->aoValue[chan] = data[i];	/* save for read back */
+
+		for (ii = 0; ii < RTD_DAC_TIMEOUT; ++ii) {
+			stat = RtdFifoStatus(dev);
+			/* 1 -> not empty */
+			if (stat & ((0 == chan) ? FS_DAC1_NOT_EMPTY :
+					FS_DAC2_NOT_EMPTY))
+				break;
+			WAIT_QUIETLY;
+		}
+		if (ii >= RTD_DAC_TIMEOUT) {
+			DPRINTK("rtd520: Error: DAC never finished! FifoStatus=0x%x\n", stat ^ 0x6666);
+			return -ETIMEDOUT;
+		}
+	}
+
+	/* return the number of samples read/written */
+	return i;
+}
+
+/* AO subdevices should have a read insn as well as a write insn.
+ * Usually this means copying a value stored in devpriv. */
+static int rtd_ao_rinsn(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	int i;
+	int chan = CR_CHAN(insn->chanspec);
+
+	for (i = 0; i < insn->n; i++) {
+		data[i] = devpriv->aoValue[chan];
+	}
+
+	return i;
+}
+
+/*
+   Write a masked set of bits and the read back the port.
+   We track what the bits should be (i.e. we don't read the port first).
+
+   DIO devices are slightly special.  Although it is possible to
+ * implement the insn_read/insn_write interface, it is much more
+ * useful to applications if you implement the insn_bits interface.
+ * This allows packed reading/writing of the DIO channels.  The
+ * comedi core can convert between insn_bits and insn_read/write
+ */
+static int rtd_dio_insn_bits(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	if (insn->n != 2)
+		return -EINVAL;
+
+	/* The insn data is a mask in data[0] and the new data
+	 * in data[1], each channel cooresponding to a bit. */
+	if (data[0]) {
+		s->state &= ~data[0];
+		s->state |= data[0] & data[1];
+
+		/* Write out the new digital output lines */
+		RtdDio0Write(dev, s->state);
+	}
+	/* on return, data[1] contains the value of the digital
+	 * input lines. */
+	data[1] = RtdDio0Read(dev);
+
+	/*DPRINTK("rtd520:port_0 wrote: 0x%x read: 0x%x\n", s->state, data[1]); */
+
+	return 2;
+}
+
+/*
+  Configure one bit on a IO port as Input or Output (hence the name :-).
+*/
+static int rtd_dio_insn_config(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	int chan = CR_CHAN(insn->chanspec);
+
+	/* The input or output configuration of each digital line is
+	 * configured by a special insn_config instruction.  chanspec
+	 * contains the channel to be changed, and data[0] contains the
+	 * value COMEDI_INPUT or COMEDI_OUTPUT. */
+	switch (data[0]) {
+	case INSN_CONFIG_DIO_OUTPUT:
+		s->io_bits |= 1 << chan;	/* 1 means Out */
+		break;
+	case INSN_CONFIG_DIO_INPUT:
+		s->io_bits &= ~(1 << chan);
+		break;
+	case INSN_CONFIG_DIO_QUERY:
+		data[1] =
+			(s->
+			io_bits & (1 << chan)) ? COMEDI_OUTPUT : COMEDI_INPUT;
+		return insn->n;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	DPRINTK("rtd520: port_0_direction=0x%x (1 means out)\n", s->io_bits);
+	/* TODO support digital match interrupts and strobes */
+	RtdDioStatusWrite(dev, 0x01);	/* make Dio0Ctrl point to direction */
+	RtdDio0CtrlWrite(dev, s->io_bits);	/* set direction 1 means Out */
+	RtdDioStatusWrite(dev, 0);	/* make Dio0Ctrl clear interrupts */
+
+	/* port1 can only be all input or all output */
+
+	/* there are also 2 user input lines and 2 user output lines */
+
+	return 1;
+}
+
+/*
+ * A convenient macro that defines init_module() and cleanup_module(),
+ * as necessary.
+ */
+COMEDI_PCI_INITCLEANUP(rtd520Driver, rtd520_pci_table);
diff --git a/drivers/staging/comedi/drivers/rtd520.h b/drivers/staging/comedi/drivers/rtd520.h
new file mode 100644
index 000000000000..0eb50b8e605e
--- /dev/null
+++ b/drivers/staging/comedi/drivers/rtd520.h
@@ -0,0 +1,412 @@
+/*
+    comedi/drivers/rtd520.h
+    Comedi driver defines for Real Time Devices (RTD) PCI4520/DM7520
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 2001 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+
+/*
+    Created by Dan Christian, NASA Ames Research Center.
+    See board notes in rtd520.c
+*/
+
+/*
+  LAS0 Runtime Area
+  Local Address Space 0 Offset		Read Function	Write Function
+*/
+#define LAS0_SPARE_00    0x0000	// -                               -
+#define LAS0_SPARE_04    0x0004	// -                               -
+#define LAS0_USER_IO     0x0008	// Read User Inputs                Write User Outputs
+#define LAS0_SPARE_0C    0x000C	// -                               -
+#define LAS0_ADC         0x0010	// Read FIFO Status                Software A/D Start
+#define LAS0_DAC1        0x0014	// -                               Software D/A1 Update
+#define LAS0_DAC2        0x0018	// -                               Software D/A2 Update
+#define LAS0_SPARE_1C    0x001C	// -                               -
+#define LAS0_SPARE_20    0x0020	// -                               -
+#define LAS0_DAC         0x0024	// -                               Software Simultaneous D/A1 and D/A2 Update
+#define LAS0_PACER       0x0028	// Software Pacer Start            Software Pacer Stop
+#define LAS0_TIMER       0x002C	// Read Timer Counters Status      HDIN Software Trigger
+#define LAS0_IT          0x0030	// Read Interrupt Status           Write Interrupt Enable Mask Register
+#define LAS0_CLEAR       0x0034	// Clear ITs set by Clear Mask     Set Interrupt Clear Mask
+#define LAS0_OVERRUN     0x0038	// Read pending interrupts         Clear Overrun Register
+#define LAS0_SPARE_3C    0x003C	// -                               -
+
+/*
+  LAS0 Runtime Area Timer/Counter,Dig.IO
+  Name			Local Address			Function
+*/
+#define LAS0_PCLK        0x0040	// Pacer Clock value (24bit)             Pacer Clock load (24bit)
+#define LAS0_BCLK        0x0044	// Burst Clock value (10bit)             Burst Clock load (10bit)
+#define LAS0_ADC_SCNT    0x0048	// A/D Sample counter value (10bit)      A/D Sample counter load (10bit)
+#define LAS0_DAC1_UCNT   0x004C	// D/A1 Update counter value (10 bit)    D/A1 Update counter load (10bit)
+#define LAS0_DAC2_UCNT   0x0050	// D/A2 Update counter value (10 bit)    D/A2 Update counter load (10bit)
+#define LAS0_DCNT        0x0054	// Delay counter value (16 bit)          Delay counter load (16bit)
+#define LAS0_ACNT        0x0058	// About counter value (16 bit)          About counter load (16bit)
+#define LAS0_DAC_CLK     0x005C	// DAC clock value (16bit)               DAC clock load (16bit)
+#define LAS0_UTC0        0x0060	// 8254 TC Counter 0 User TC 0 value     Load count in TC Counter 0
+#define LAS0_UTC1        0x0064	// 8254 TC Counter 1 User TC 1 value     Load count in TC Counter 1
+#define LAS0_UTC2        0x0068	// 8254 TC Counter 2 User TC 2 value     Load count in TC Counter 2
+#define LAS0_UTC_CTRL    0x006C	// 8254 TC Control Word                  Program counter mode for TC
+#define LAS0_DIO0        0x0070	// Digital I/O Port 0 Read Port          Digital I/O Port 0 Write Port
+#define LAS0_DIO1        0x0074	// Digital I/O Port 1 Read Port          Digital I/O Port 1 Write Port
+#define LAS0_DIO0_CTRL   0x0078	// Clear digital IRQ status flag/read    Clear digital chip/program Port 0
+#define LAS0_DIO_STATUS  0x007C	// Read Digital I/O Status word          Program digital control register &
+
+/*
+  LAS0 Setup Area
+  Name			Local Address			Function
+*/
+#define LAS0_BOARD_RESET        0x0100	// Board reset
+#define LAS0_DMA0_SRC           0x0104	// DMA 0 Sources select
+#define LAS0_DMA1_SRC           0x0108	// DMA 1 Sources select
+#define LAS0_ADC_CONVERSION     0x010C	// A/D Conversion Signal select
+#define LAS0_BURST_START        0x0110	// Burst Clock Start Trigger select
+#define LAS0_PACER_START        0x0114	// Pacer Clock Start Trigger select
+#define LAS0_PACER_STOP         0x0118	// Pacer Clock Stop Trigger select
+#define LAS0_ACNT_STOP_ENABLE   0x011C	// About Counter Stop Enable
+#define LAS0_PACER_REPEAT       0x0120	// Pacer Start Trigger Mode select
+#define LAS0_DIN_START          0x0124	// High Speed Digital Input Sampling Signal select
+#define LAS0_DIN_FIFO_CLEAR     0x0128	// Digital Input FIFO Clear
+#define LAS0_ADC_FIFO_CLEAR     0x012C	// A/D FIFO Clear
+#define LAS0_CGT_WRITE          0x0130	// Channel Gain Table Write
+#define LAS0_CGL_WRITE          0x0134	// Channel Gain Latch Write
+#define LAS0_CG_DATA            0x0138	// Digital Table Write
+#define LAS0_CGT_ENABLE		0x013C	// Channel Gain Table Enable
+#define LAS0_CG_ENABLE          0x0140	// Digital Table Enable
+#define LAS0_CGT_PAUSE          0x0144	// Table Pause Enable
+#define LAS0_CGT_RESET          0x0148	// Reset Channel Gain Table
+#define LAS0_CGT_CLEAR          0x014C	// Clear Channel Gain Table
+#define LAS0_DAC1_CTRL          0x0150	// D/A1 output type/range
+#define LAS0_DAC1_SRC           0x0154	// D/A1 update source
+#define LAS0_DAC1_CYCLE         0x0158	// D/A1 cycle mode
+#define LAS0_DAC1_RESET         0x015C	// D/A1 FIFO reset
+#define LAS0_DAC1_FIFO_CLEAR    0x0160	// D/A1 FIFO clear
+#define LAS0_DAC2_CTRL          0x0164	// D/A2 output type/range
+#define LAS0_DAC2_SRC           0x0168	// D/A2 update source
+#define LAS0_DAC2_CYCLE         0x016C	// D/A2 cycle mode
+#define LAS0_DAC2_RESET         0x0170	// D/A2 FIFO reset
+#define LAS0_DAC2_FIFO_CLEAR    0x0174	// D/A2 FIFO clear
+#define LAS0_ADC_SCNT_SRC       0x0178	// A/D Sample Counter Source select
+#define LAS0_PACER_SELECT       0x0180	// Pacer Clock select
+#define LAS0_SBUS0_SRC          0x0184	// SyncBus 0 Source select
+#define LAS0_SBUS0_ENABLE       0x0188	// SyncBus 0 enable
+#define LAS0_SBUS1_SRC          0x018C	// SyncBus 1 Source select
+#define LAS0_SBUS1_ENABLE       0x0190	// SyncBus 1 enable
+#define LAS0_SBUS2_SRC          0x0198	// SyncBus 2 Source select
+#define LAS0_SBUS2_ENABLE       0x019C	// SyncBus 2 enable
+#define LAS0_ETRG_POLARITY      0x01A4	// External Trigger polarity select
+#define LAS0_EINT_POLARITY      0x01A8	// External Interrupt polarity select
+#define LAS0_UTC0_CLOCK         0x01AC	// UTC0 Clock select
+#define LAS0_UTC0_GATE          0x01B0	// UTC0 Gate select
+#define LAS0_UTC1_CLOCK         0x01B4	// UTC1 Clock select
+#define LAS0_UTC1_GATE          0x01B8	// UTC1 Gate select
+#define LAS0_UTC2_CLOCK         0x01BC	// UTC2 Clock select
+#define LAS0_UTC2_GATE          0x01C0	// UTC2 Gate select
+#define LAS0_UOUT0_SELECT       0x01C4	// User Output 0 source select
+#define LAS0_UOUT1_SELECT       0x01C8	// User Output 1 source select
+#define LAS0_DMA0_RESET         0x01CC	// DMA0 Request state machine reset
+#define LAS0_DMA1_RESET         0x01D0	// DMA1 Request state machine reset
+
+/*
+  LAS1
+  Name			Local Address			Function
+*/
+#define LAS1_ADC_FIFO            0x0000	// Read A/D FIFO (16bit) -
+#define LAS1_HDIO_FIFO           0x0004	// Read High Speed Digital Input FIFO (16bit) -
+#define LAS1_DAC1_FIFO           0x0008	// - Write D/A1 FIFO (16bit)
+#define LAS1_DAC2_FIFO           0x000C	// - Write D/A2 FIFO (16bit)
+
+/*
+  LCFG: PLX 9080 local config & runtime registers
+  Name			Local Address			Function
+*/
+#define LCFG_ITCSR              0x0068	// INTCSR, Interrupt Control/Status Register
+#define LCFG_DMAMODE0           0x0080	// DMA Channel 0 Mode Register
+#define LCFG_DMAPADR0           0x0084	// DMA Channel 0 PCI Address Register
+#define LCFG_DMALADR0           0x0088	// DMA Channel 0 Local Address Reg
+#define LCFG_DMASIZ0            0x008C	// DMA Channel 0 Transfer Size (Bytes) Register
+#define LCFG_DMADPR0            0x0090	// DMA Channel 0 Descriptor Pointer Register
+#define LCFG_DMAMODE1           0x0094	// DMA Channel 1 Mode Register
+#define LCFG_DMAPADR1           0x0098	// DMA Channel 1 PCI Address Register
+#define LCFG_DMALADR1           0x009C	// DMA Channel 1 Local Address Register
+#define LCFG_DMASIZ1            0x00A0	// DMA Channel 1 Transfer Size (Bytes) Register
+#define LCFG_DMADPR1            0x00A4	// DMA Channel 1 Descriptor Pointer Register
+#define LCFG_DMACSR0            0x00A8	// DMA Channel 0 Command/Status Register
+#define LCFG_DMACSR1            0x00A9	// DMA Channel 0 Command/Status Register
+#define LCFG_DMAARB             0x00AC	// DMA Arbitration Register
+#define LCFG_DMATHR             0x00B0	// DMA Threshold Register
+
+/*======================================================================
+  Resister bit definitions
+======================================================================*/
+
+// FIFO Status Word Bits (RtdFifoStatus)
+#define FS_DAC1_NOT_EMPTY    0x0001	// D0  - DAC1 FIFO not empty
+#define FS_DAC1_HEMPTY   0x0002	// D1  - DAC1 FIFO half empty
+#define FS_DAC1_NOT_FULL     0x0004	// D2  - DAC1 FIFO not full
+#define FS_DAC2_NOT_EMPTY    0x0010	// D4  - DAC2 FIFO not empty
+#define FS_DAC2_HEMPTY   0x0020	// D5  - DAC2 FIFO half empty
+#define FS_DAC2_NOT_FULL     0x0040	// D6  - DAC2 FIFO not full
+#define FS_ADC_NOT_EMPTY     0x0100	// D8  - ADC FIFO not empty
+#define FS_ADC_HEMPTY    0x0200	// D9  - ADC FIFO half empty
+#define FS_ADC_NOT_FULL      0x0400	// D10 - ADC FIFO not full
+#define FS_DIN_NOT_EMPTY     0x1000	// D12 - DIN FIFO not empty
+#define FS_DIN_HEMPTY    0x2000	// D13 - DIN FIFO half empty
+#define FS_DIN_NOT_FULL      0x4000	// D14 - DIN FIFO not full
+
+// Timer Status Word Bits (GetTimerStatus)
+#define TS_PCLK_GATE   0x0001
+// D0 - Pacer Clock Gate [0 - gated, 1 - enabled]
+#define TS_BCLK_GATE   0x0002
+// D1 - Burst Clock Gate [0 - disabled, 1 - running]
+#define TS_DCNT_GATE   0x0004
+// D2 - Pacer Clock Delayed Start Trigger [0 - delay over, 1 - delay in
+// progress]
+#define TS_ACNT_GATE   0x0008
+// D3 - Pacer Clock About Trigger [0 - completed, 1 - in progress]
+#define TS_PCLK_RUN    0x0010
+// D4 - Pacer Clock Shutdown Flag [0 - Pacer Clock cannot be start
+// triggered only by Software Pacer Start Command, 1 - Pacer Clock can
+// be start triggered]
+
+// External Trigger polarity select
+// External Interrupt polarity select
+#define POL_POSITIVE         0x0	// positive edge
+#define POL_NEGATIVE         0x1	// negative edge
+
+// User Output Signal select (SetUout0Source, SetUout1Source)
+#define UOUT_ADC                0x0	// A/D Conversion Signal
+#define UOUT_DAC1               0x1	// D/A1 Update
+#define UOUT_DAC2               0x2	// D/A2 Update
+#define UOUT_SOFTWARE           0x3	// Software Programmable
+
+// Pacer clock select (SetPacerSource)
+#define PCLK_INTERNAL           1	// Internal Pacer Clock
+#define PCLK_EXTERNAL           0	// External Pacer Clock
+
+// A/D Sample Counter Sources (SetAdcntSource, SetupSampleCounter)
+#define ADC_SCNT_CGT_RESET         0x0	// needs restart with StartPacer
+#define ADC_SCNT_FIFO_WRITE        0x1
+
+// A/D Conversion Signal Select (for SetConversionSelect)
+#define ADC_START_SOFTWARE         0x0	// Software A/D Start
+#define ADC_START_PCLK             0x1	// Pacer Clock (Ext. Int. see Func.509)
+#define ADC_START_BCLK             0x2	// Burst Clock
+#define ADC_START_DIGITAL_IT       0x3	// Digital Interrupt
+#define ADC_START_DAC1_MARKER1     0x4	// D/A 1 Data Marker 1
+#define ADC_START_DAC2_MARKER1     0x5	// D/A 2 Data Marker 1
+#define ADC_START_SBUS0            0x6	// SyncBus 0
+#define ADC_START_SBUS1            0x7	// SyncBus 1
+#define ADC_START_SBUS2            0x8	// SyncBus 2
+
+// Burst Clock start trigger select (SetBurstStart)
+#define BCLK_START_SOFTWARE        0x0	// Software A/D Start (StartBurst)
+#define BCLK_START_PCLK            0x1	// Pacer Clock
+#define BCLK_START_ETRIG           0x2	// External Trigger
+#define BCLK_START_DIGITAL_IT      0x3	// Digital Interrupt
+#define BCLK_START_SBUS0           0x4	// SyncBus 0
+#define BCLK_START_SBUS1           0x5	// SyncBus 1
+#define BCLK_START_SBUS2           0x6	// SyncBus 2
+
+// Pacer Clock start trigger select (SetPacerStart)
+#define PCLK_START_SOFTWARE        0x0	// Software Pacer Start (StartPacer)
+#define PCLK_START_ETRIG           0x1	// External trigger
+#define PCLK_START_DIGITAL_IT      0x2	// Digital interrupt
+#define PCLK_START_UTC2            0x3	// User TC 2 out
+#define PCLK_START_SBUS0           0x4	// SyncBus 0
+#define PCLK_START_SBUS1           0x5	// SyncBus 1
+#define PCLK_START_SBUS2           0x6	// SyncBus 2
+#define PCLK_START_D_SOFTWARE      0x8	// Delayed Software Pacer Start
+#define PCLK_START_D_ETRIG         0x9	// Delayed external trigger
+#define PCLK_START_D_DIGITAL_IT    0xA	// Delayed digital interrupt
+#define PCLK_START_D_UTC2          0xB	// Delayed User TC 2 out
+#define PCLK_START_D_SBUS0         0xC	// Delayed SyncBus 0
+#define PCLK_START_D_SBUS1         0xD	// Delayed SyncBus 1
+#define PCLK_START_D_SBUS2         0xE	// Delayed SyncBus 2
+#define PCLK_START_ETRIG_GATED     0xF	// External Trigger Gated controlled mode
+
+// Pacer Clock Stop Trigger select (SetPacerStop)
+#define PCLK_STOP_SOFTWARE         0x0	// Software Pacer Stop (StopPacer)
+#define PCLK_STOP_ETRIG            0x1	// External Trigger
+#define PCLK_STOP_DIGITAL_IT       0x2	// Digital Interrupt
+#define PCLK_STOP_ACNT             0x3	// About Counter
+#define PCLK_STOP_UTC2             0x4	// User TC2 out
+#define PCLK_STOP_SBUS0            0x5	// SyncBus 0
+#define PCLK_STOP_SBUS1            0x6	// SyncBus 1
+#define PCLK_STOP_SBUS2            0x7	// SyncBus 2
+#define PCLK_STOP_A_SOFTWARE       0x8	// About Software Pacer Stop
+#define PCLK_STOP_A_ETRIG          0x9	// About External Trigger
+#define PCLK_STOP_A_DIGITAL_IT     0xA	// About Digital Interrupt
+#define PCLK_STOP_A_UTC2           0xC	// About User TC2 out
+#define PCLK_STOP_A_SBUS0          0xD	// About SyncBus 0
+#define PCLK_STOP_A_SBUS1          0xE	// About SyncBus 1
+#define PCLK_STOP_A_SBUS2          0xF	// About SyncBus 2
+
+// About Counter Stop Enable
+#define ACNT_STOP                  0x0	// stop enable
+#define ACNT_NO_STOP               0x1	// stop disabled
+
+// DAC update source (SetDAC1Start & SetDAC2Start)
+#define DAC_START_SOFTWARE         0x0	// Software Update
+#define DAC_START_CGT              0x1	// CGT controlled Update
+#define DAC_START_DAC_CLK          0x2	// D/A Clock
+#define DAC_START_EPCLK            0x3	// External Pacer Clock
+#define DAC_START_SBUS0            0x4	// SyncBus 0
+#define DAC_START_SBUS1            0x5	// SyncBus 1
+#define DAC_START_SBUS2            0x6	// SyncBus 2
+
+// DAC Cycle Mode (SetDAC1Cycle, SetDAC2Cycle, SetupDAC)
+#define DAC_CYCLE_SINGLE           0x0	// not cycle
+#define DAC_CYCLE_MULTI            0x1	// cycle
+
+// 8254 Operation Modes (Set8254Mode, SetupTimerCounter)
+#define M8254_EVENT_COUNTER        0	// Event Counter
+#define M8254_HW_ONE_SHOT          1	// Hardware-Retriggerable One-Shot
+#define M8254_RATE_GENERATOR       2	// Rate Generator
+#define M8254_SQUARE_WAVE          3	// Square Wave Mode
+#define M8254_SW_STROBE            4	// Software Triggered Strobe
+#define M8254_HW_STROBE            5	// Hardware Triggered Strobe (Retriggerable)
+
+// User Timer/Counter 0 Clock Select (SetUtc0Clock)
+#define CUTC0_8MHZ                 0x0	// 8MHz
+#define CUTC0_EXT_TC_CLOCK1        0x1	// Ext. TC Clock 1
+#define CUTC0_EXT_TC_CLOCK2        0x2	// Ext. TC Clock 2
+#define CUTC0_EXT_PCLK             0x3	// Ext. Pacer Clock
+
+// User Timer/Counter 1 Clock Select (SetUtc1Clock)
+#define CUTC1_8MHZ                 0x0	// 8MHz
+#define CUTC1_EXT_TC_CLOCK1        0x1	// Ext. TC Clock 1
+#define CUTC1_EXT_TC_CLOCK2        0x2	// Ext. TC Clock 2
+#define CUTC1_EXT_PCLK             0x3	// Ext. Pacer Clock
+#define CUTC1_UTC0_OUT             0x4	// User Timer/Counter 0 out
+#define CUTC1_DIN_SIGNAL           0x5	// High-Speed Digital Input   Sampling signal
+
+// User Timer/Counter 2 Clock Select (SetUtc2Clock)
+#define CUTC2_8MHZ                 0x0	// 8MHz
+#define CUTC2_EXT_TC_CLOCK1        0x1	// Ext. TC Clock 1
+#define CUTC2_EXT_TC_CLOCK2        0x2	// Ext. TC Clock 2
+#define CUTC2_EXT_PCLK             0x3	// Ext. Pacer Clock
+#define CUTC2_UTC1_OUT             0x4	// User Timer/Counter 1 out
+
+// User Timer/Counter 0 Gate Select (SetUtc0Gate)
+#define GUTC0_NOT_GATED            0x0	// Not gated
+#define GUTC0_GATED                0x1	// Gated
+#define GUTC0_EXT_TC_GATE1         0x2	// Ext. TC Gate 1
+#define GUTC0_EXT_TC_GATE2         0x3	// Ext. TC Gate 2
+
+// User Timer/Counter 1 Gate Select (SetUtc1Gate)
+#define GUTC1_NOT_GATED            0x0	// Not gated
+#define GUTC1_GATED                0x1	// Gated
+#define GUTC1_EXT_TC_GATE1         0x2	// Ext. TC Gate 1
+#define GUTC1_EXT_TC_GATE2         0x3	// Ext. TC Gate 2
+#define GUTC1_UTC0_OUT             0x4	// User Timer/Counter 0 out
+
+// User Timer/Counter 2 Gate Select (SetUtc2Gate)
+#define GUTC2_NOT_GATED            0x0	// Not gated
+#define GUTC2_GATED                0x1	// Gated
+#define GUTC2_EXT_TC_GATE1         0x2	// Ext. TC Gate 1
+#define GUTC2_EXT_TC_GATE2         0x3	// Ext. TC Gate 2
+#define GUTC2_UTC1_OUT             0x4	// User Timer/Counter 1 out
+
+// Interrupt Source Masks (SetITMask, ClearITMask, GetITStatus)
+#define IRQM_ADC_FIFO_WRITE        0x0001	// ADC FIFO Write
+#define IRQM_CGT_RESET             0x0002	// Reset CGT
+#define IRQM_CGT_PAUSE             0x0008	// Pause CGT
+#define IRQM_ADC_ABOUT_CNT         0x0010	// About Counter out
+#define IRQM_ADC_DELAY_CNT         0x0020	// Delay Counter out
+#define IRQM_ADC_SAMPLE_CNT	   0x0040	// ADC Sample Counter
+#define IRQM_DAC1_UCNT             0x0080	// DAC1 Update Counter
+#define IRQM_DAC2_UCNT             0x0100	// DAC2 Update Counter
+#define IRQM_UTC1                  0x0200	// User TC1 out
+#define IRQM_UTC1_INV              0x0400	// User TC1 out, inverted
+#define IRQM_UTC2                  0x0800	// User TC2 out
+#define IRQM_DIGITAL_IT            0x1000	// Digital Interrupt
+#define IRQM_EXTERNAL_IT           0x2000	// External Interrupt
+#define IRQM_ETRIG_RISING          0x4000	// External Trigger rising-edge
+#define IRQM_ETRIG_FALLING         0x8000	// External Trigger falling-edge
+
+// DMA Request Sources (LAS0)
+#define DMAS_DISABLED              0x0	// DMA Disabled
+#define DMAS_ADC_SCNT              0x1	// ADC Sample Counter
+#define DMAS_DAC1_UCNT             0x2	// D/A1 Update Counter
+#define DMAS_DAC2_UCNT             0x3	// D/A2 Update Counter
+#define DMAS_UTC1                  0x4	// User TC1 out
+#define DMAS_ADFIFO_HALF_FULL      0x8	// A/D FIFO half full
+#define DMAS_DAC1_FIFO_HALF_EMPTY  0x9	// D/A1 FIFO half empty
+#define DMAS_DAC2_FIFO_HALF_EMPTY  0xA	// D/A2 FIFO half empty
+
+// DMA Local Addresses   (0x40000000+LAS1 offset)
+#define DMALADDR_ADC       0x40000000	// A/D FIFO
+#define DMALADDR_HDIN      0x40000004	// High Speed Digital Input FIFO
+#define DMALADDR_DAC1      0x40000008	// D/A1 FIFO
+#define DMALADDR_DAC2      0x4000000C	// D/A2 FIFO
+
+// Port 0 compare modes (SetDIO0CompareMode)
+#define DIO_MODE_EVENT     0	// Event Mode
+#define DIO_MODE_MATCH     1	// Match Mode
+
+// Digital Table Enable (Port 1 disable)
+#define DTBL_DISABLE       0	// Enable Digital Table
+#define DTBL_ENABLE        1	// Disable Digital Table
+
+// Sampling Signal for High Speed Digital Input (SetHdinStart)
+#define HDIN_SOFTWARE      0x0	// Software Trigger
+#define HDIN_ADC           0x1	// A/D Conversion Signal
+#define HDIN_UTC0          0x2	// User TC out 0
+#define HDIN_UTC1          0x3	// User TC out 1
+#define HDIN_UTC2          0x4	// User TC out 2
+#define HDIN_EPCLK         0x5	// External Pacer Clock
+#define HDIN_ETRG          0x6	// External Trigger
+
+// Channel Gain Table / Channel Gain Latch
+#define CSC_LATCH          0	// Channel Gain Latch mode
+#define CSC_CGT            1	// Channel Gain Table mode
+
+// Channel Gain Table Pause Enable
+#define CGT_PAUSE_DISABLE  0	// Channel Gain Table Pause Disable
+#define CGT_PAUSE_ENABLE   1	// Channel Gain Table Pause Enable
+
+// DAC output type/range (p63)
+#define AOUT_UNIP5         0	// 0..+5 Volt
+#define AOUT_UNIP10        1	// 0..+10 Volt
+#define AOUT_BIP5          2	// -5..+5 Volt
+#define AOUT_BIP10         3	// -10..+10 Volt
+
+// Ghannel Gain Table field definitions (p61)
+// Gain
+#define GAIN1              0
+#define GAIN2              1
+#define GAIN4              2
+#define GAIN8              3
+#define GAIN16             4
+#define GAIN32             5
+#define GAIN64             6
+#define GAIN128            7
+
+// Input range/polarity
+#define AIN_BIP5           0	// -5..+5 Volt
+#define AIN_BIP10          1	// -10..+10 Volt
+#define AIN_UNIP10         2	// 0..+10 Volt
+
+// non referenced single ended select bit
+#define NRSE_AGND          0	// AGND referenced SE input
+#define NRSE_AINS          1	// AIN SENSE referenced SE input
+
+// single ended vs differential
+#define GND_SE		0	// Single-Ended
+#define GND_DIFF	1	// Differential
diff --git a/drivers/staging/comedi/drivers/s626.c b/drivers/staging/comedi/drivers/s626.c
new file mode 100644
index 000000000000..469ee8c474c9
--- /dev/null
+++ b/drivers/staging/comedi/drivers/s626.c
@@ -0,0 +1,3254 @@
+/*
+  comedi/drivers/s626.c
+  Sensoray s626 Comedi driver
+
+  COMEDI - Linux Control and Measurement Device Interface
+  Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+
+  Based on Sensoray Model 626 Linux driver Version 0.2
+  Copyright (C) 2002-2004 Sensoray Co., Inc.
+
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation; either version 2 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; if not, write to the Free Software
+  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+/*
+Driver: s626
+Description: Sensoray 626 driver
+Devices: [Sensoray] 626 (s626)
+Authors: Gianluca Palli <gpalli@deis.unibo.it>,
+Updated: Fri, 15 Feb 2008 10:28:42 +0000
+Status: experimental
+
+Configuration options:
+  [0] - PCI bus of device (optional)
+  [1] - PCI slot of device (optional)
+  If bus/slot is not specified, the first supported
+  PCI device found will be used.
+
+INSN_CONFIG instructions:
+  analog input:
+   none
+
+  analog output:
+   none
+
+  digital channel:
+   s626 has 3 dio subdevices (2,3 and 4) each with 16 i/o channels
+   supported configuration options:
+   INSN_CONFIG_DIO_QUERY
+   COMEDI_INPUT
+   COMEDI_OUTPUT
+
+  encoder:
+   Every channel must be configured before reading.
+
+   Example code
+
+   insn.insn=INSN_CONFIG;   //configuration instruction
+   insn.n=1;                //number of operation (must be 1)
+   insn.data=&initialvalue; //initial value loaded into encoder
+                            //during configuration
+   insn.subdev=5;           //encoder subdevice
+   insn.chanspec=CR_PACK(encoder_channel,0,AREF_OTHER); //encoder_channel
+                                                        //to configure
+
+   comedi_do_insn(cf,&insn); //executing configuration
+*/
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+
+#include "../comedidev.h"
+
+#include "comedi_pci.h"
+
+#include "comedi_fc.h"
+#include "s626.h"
+
+MODULE_AUTHOR("Gianluca Palli <gpalli@deis.unibo.it>");
+MODULE_DESCRIPTION("Sensoray 626 Comedi driver module");
+MODULE_LICENSE("GPL");
+
+typedef struct s626_board_struct {
+	const char *name;
+	int ai_chans;
+	int ai_bits;
+	int ao_chans;
+	int ao_bits;
+	int dio_chans;
+	int dio_banks;
+	int enc_chans;
+} s626_board;
+
+static const s626_board s626_boards[] = {
+	{
+	      name:	"s626",
+	      ai_chans:S626_ADC_CHANNELS,
+	      ai_bits:	14,
+	      ao_chans:S626_DAC_CHANNELS,
+	      ao_bits:	13,
+	      dio_chans:S626_DIO_CHANNELS,
+	      dio_banks:S626_DIO_BANKS,
+	      enc_chans:S626_ENCODER_CHANNELS,
+		}
+};
+
+#define thisboard ((const s626_board *)dev->board_ptr)
+#define PCI_VENDOR_ID_S626 0x1131
+#define PCI_DEVICE_ID_S626 0x7146
+
+static DEFINE_PCI_DEVICE_TABLE(s626_pci_table) = {
+	{PCI_VENDOR_ID_S626, PCI_DEVICE_ID_S626, PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+		0},
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, s626_pci_table);
+
+static int s626_attach(comedi_device * dev, comedi_devconfig * it);
+static int s626_detach(comedi_device * dev);
+
+static comedi_driver driver_s626 = {
+      driver_name:"s626",
+      module:THIS_MODULE,
+      attach:s626_attach,
+      detach:s626_detach,
+};
+
+typedef struct {
+	struct pci_dev *pdev;
+	void *base_addr;
+	int got_regions;
+	short allocatedBuf;
+	uint8_t ai_cmd_running;	// ai_cmd is running
+	uint8_t ai_continous;	// continous aquisition
+	int ai_sample_count;	// number of samples to aquire
+	unsigned int ai_sample_timer;	// time between samples in
+	// units of the timer
+	int ai_convert_count;	// conversion counter
+	unsigned int ai_convert_timer;	// time between conversion in
+	// units of the timer
+	uint16_t CounterIntEnabs;	//Counter interrupt enable
+	//mask for MISC2 register.
+	uint8_t AdcItems;	//Number of items in ADC poll
+	//list.
+	DMABUF RPSBuf;		//DMA buffer used to hold ADC
+	//(RPS1) program.
+	DMABUF ANABuf;		//DMA buffer used to receive
+	//ADC data and hold DAC data.
+	uint32_t *pDacWBuf;	//Pointer to logical adrs of
+	//DMA buffer used to hold DAC
+	//data.
+	uint16_t Dacpol;	//Image of DAC polarity
+	//register.
+	uint8_t TrimSetpoint[12];	//Images of TrimDAC setpoints.
+	//registers.
+	uint16_t ChargeEnabled;	//Image of MISC2 Battery
+	//Charge Enabled (0 or
+	//WRMISC2_CHARGE_ENABLE).
+	uint16_t WDInterval;	//Image of MISC2 watchdog
+	//interval control bits.
+	uint32_t I2CAdrs;	//I2C device address for
+	//onboard EEPROM (board rev
+	//dependent).
+	//  short         I2Cards;
+	lsampl_t ao_readback[S626_DAC_CHANNELS];
+} s626_private;
+
+typedef struct {
+	uint16_t RDDIn;
+	uint16_t WRDOut;
+	uint16_t RDEdgSel;
+	uint16_t WREdgSel;
+	uint16_t RDCapSel;
+	uint16_t WRCapSel;
+	uint16_t RDCapFlg;
+	uint16_t RDIntSel;
+	uint16_t WRIntSel;
+} dio_private;
+
+static dio_private dio_private_A = {
+      RDDIn:LP_RDDINA,
+      WRDOut:LP_WRDOUTA,
+      RDEdgSel:LP_RDEDGSELA,
+      WREdgSel:LP_WREDGSELA,
+      RDCapSel:LP_RDCAPSELA,
+      WRCapSel:LP_WRCAPSELA,
+      RDCapFlg:LP_RDCAPFLGA,
+      RDIntSel:LP_RDINTSELA,
+      WRIntSel:LP_WRINTSELA,
+};
+
+static dio_private dio_private_B = {
+      RDDIn:LP_RDDINB,
+      WRDOut:LP_WRDOUTB,
+      RDEdgSel:LP_RDEDGSELB,
+      WREdgSel:LP_WREDGSELB,
+      RDCapSel:LP_RDCAPSELB,
+      WRCapSel:LP_WRCAPSELB,
+      RDCapFlg:LP_RDCAPFLGB,
+      RDIntSel:LP_RDINTSELB,
+      WRIntSel:LP_WRINTSELB,
+};
+
+static dio_private dio_private_C = {
+      RDDIn:LP_RDDINC,
+      WRDOut:LP_WRDOUTC,
+      RDEdgSel:LP_RDEDGSELC,
+      WREdgSel:LP_WREDGSELC,
+      RDCapSel:LP_RDCAPSELC,
+      WRCapSel:LP_WRCAPSELC,
+      RDCapFlg:LP_RDCAPFLGC,
+      RDIntSel:LP_RDINTSELC,
+      WRIntSel:LP_WRINTSELC,
+};
+
+/* to group dio devices (48 bits mask and data are not allowed ???)
+static dio_private *dio_private_word[]={
+  &dio_private_A,
+  &dio_private_B,
+  &dio_private_C,
+};
+*/
+
+#define devpriv ((s626_private *)dev->private)
+#define diopriv ((dio_private *)s->private)
+
+COMEDI_PCI_INITCLEANUP_NOMODULE(driver_s626, s626_pci_table);
+
+//ioctl routines
+static int s626_ai_insn_config(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+/* static int s626_ai_rinsn(comedi_device *dev,comedi_subdevice *s,comedi_insn *insn,lsampl_t *data); */
+static int s626_ai_insn_read(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_ai_cmd(comedi_device * dev, comedi_subdevice * s);
+static int s626_ai_cmdtest(comedi_device * dev, comedi_subdevice * s,
+	comedi_cmd * cmd);
+static int s626_ai_cancel(comedi_device * dev, comedi_subdevice * s);
+static int s626_ao_winsn(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_ao_rinsn(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_dio_insn_bits(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_dio_insn_config(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_dio_set_irq(comedi_device * dev, unsigned int chan);
+static int s626_dio_reset_irq(comedi_device * dev, unsigned int gruop,
+	unsigned int mask);
+static int s626_dio_clear_irq(comedi_device * dev);
+static int s626_enc_insn_config(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_enc_insn_read(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_enc_insn_write(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_ns_to_timer(int *nanosec, int round_mode);
+static int s626_ai_load_polllist(uint8_t * ppl, comedi_cmd * cmd);
+static int s626_ai_inttrig(comedi_device * dev, comedi_subdevice * s,
+	unsigned int trignum);
+static irqreturn_t s626_irq_handler(int irq, void *d PT_REGS_ARG);
+static lsampl_t s626_ai_reg_to_uint(int data);
+/* static lsampl_t s626_uint_to_reg(comedi_subdevice *s, int data); */
+
+//end ioctl routines
+
+//internal routines
+static void s626_dio_init(comedi_device * dev);
+static void ResetADC(comedi_device * dev, uint8_t * ppl);
+static void LoadTrimDACs(comedi_device * dev);
+static void WriteTrimDAC(comedi_device * dev, uint8_t LogicalChan,
+	uint8_t DacData);
+static uint8_t I2Cread(comedi_device * dev, uint8_t addr);
+static uint32_t I2Chandshake(comedi_device * dev, uint32_t val);
+static void SetDAC(comedi_device * dev, uint16_t chan, short dacdata);
+static void SendDAC(comedi_device * dev, uint32_t val);
+static void WriteMISC2(comedi_device * dev, uint16_t NewImage);
+static void DEBItransfer(comedi_device * dev);
+static uint16_t DEBIread(comedi_device * dev, uint16_t addr);
+static void DEBIwrite(comedi_device * dev, uint16_t addr, uint16_t wdata);
+static void DEBIreplace(comedi_device * dev, uint16_t addr, uint16_t mask,
+	uint16_t wdata);
+static void CloseDMAB(comedi_device * dev, DMABUF * pdma, size_t bsize);
+
+// COUNTER OBJECT ------------------------------------------------
+typedef struct enc_private_struct {
+	// Pointers to functions that differ for A and B counters:
+	uint16_t(*GetEnable) (comedi_device * dev, struct enc_private_struct *);	//Return clock enable.
+	uint16_t(*GetIntSrc) (comedi_device * dev, struct enc_private_struct *);	//Return interrupt source.
+	uint16_t(*GetLoadTrig) (comedi_device * dev, struct enc_private_struct *);	//Return preload trigger source.
+	uint16_t(*GetMode) (comedi_device * dev, struct enc_private_struct *);	//Return standardized operating mode.
+	void (*PulseIndex) (comedi_device * dev, struct enc_private_struct *);	//Generate soft index strobe.
+	void (*SetEnable) (comedi_device * dev, struct enc_private_struct *, uint16_t enab);	//Program clock enable.
+	void (*SetIntSrc) (comedi_device * dev, struct enc_private_struct *, uint16_t IntSource);	//Program interrupt source.
+	void (*SetLoadTrig) (comedi_device * dev, struct enc_private_struct *, uint16_t Trig);	//Program preload trigger source.
+	void (*SetMode) (comedi_device * dev, struct enc_private_struct *, uint16_t Setup, uint16_t DisableIntSrc);	//Program standardized operating mode.
+	void (*ResetCapFlags) (comedi_device * dev, struct enc_private_struct *);	//Reset event capture flags.
+
+	uint16_t MyCRA;		//   Address of CRA register.
+	uint16_t MyCRB;		//   Address of CRB register.
+	uint16_t MyLatchLsw;	//   Address of Latch least-significant-word
+	//   register.
+	uint16_t MyEventBits[4];	//   Bit translations for IntSrc -->RDMISC2.
+} enc_private;			//counter object
+
+#define encpriv ((enc_private *)(dev->subdevices+5)->private)
+
+//counters routines
+static void s626_timer_load(comedi_device * dev, enc_private * k, int tick);
+static uint32_t ReadLatch(comedi_device * dev, enc_private * k);
+static void ResetCapFlags_A(comedi_device * dev, enc_private * k);
+static void ResetCapFlags_B(comedi_device * dev, enc_private * k);
+static uint16_t GetMode_A(comedi_device * dev, enc_private * k);
+static uint16_t GetMode_B(comedi_device * dev, enc_private * k);
+static void SetMode_A(comedi_device * dev, enc_private * k, uint16_t Setup,
+	uint16_t DisableIntSrc);
+static void SetMode_B(comedi_device * dev, enc_private * k, uint16_t Setup,
+	uint16_t DisableIntSrc);
+static void SetEnable_A(comedi_device * dev, enc_private * k, uint16_t enab);
+static void SetEnable_B(comedi_device * dev, enc_private * k, uint16_t enab);
+static uint16_t GetEnable_A(comedi_device * dev, enc_private * k);
+static uint16_t GetEnable_B(comedi_device * dev, enc_private * k);
+static void SetLatchSource(comedi_device * dev, enc_private * k,
+	uint16_t value);
+/* static uint16_t GetLatchSource(comedi_device *dev, enc_private *k ); */
+static void SetLoadTrig_A(comedi_device * dev, enc_private * k, uint16_t Trig);
+static void SetLoadTrig_B(comedi_device * dev, enc_private * k, uint16_t Trig);
+static uint16_t GetLoadTrig_A(comedi_device * dev, enc_private * k);
+static uint16_t GetLoadTrig_B(comedi_device * dev, enc_private * k);
+static void SetIntSrc_B(comedi_device * dev, enc_private * k,
+	uint16_t IntSource);
+static void SetIntSrc_A(comedi_device * dev, enc_private * k,
+	uint16_t IntSource);
+static uint16_t GetIntSrc_A(comedi_device * dev, enc_private * k);
+static uint16_t GetIntSrc_B(comedi_device * dev, enc_private * k);
+/* static void SetClkMult(comedi_device *dev, enc_private *k, uint16_t value ) ; */
+/* static uint16_t GetClkMult(comedi_device *dev, enc_private *k ) ; */
+/* static void SetIndexPol(comedi_device *dev, enc_private *k, uint16_t value ); */
+/* static uint16_t GetClkPol(comedi_device *dev, enc_private *k ) ; */
+/* static void SetIndexSrc( comedi_device *dev,enc_private *k, uint16_t value );  */
+/* static uint16_t GetClkSrc( comedi_device *dev,enc_private *k );  */
+/* static void SetIndexSrc( comedi_device *dev,enc_private *k, uint16_t value );  */
+/* static uint16_t GetIndexSrc( comedi_device *dev,enc_private *k );  */
+static void PulseIndex_A(comedi_device * dev, enc_private * k);
+static void PulseIndex_B(comedi_device * dev, enc_private * k);
+static void Preload(comedi_device * dev, enc_private * k, uint32_t value);
+static void CountersInit(comedi_device * dev);
+//end internal routines
+
+/////////////////////////////////////////////////////////////////////////
+// Counter objects constructor.
+
+// Counter overflow/index event flag masks for RDMISC2.
+#define INDXMASK(C)		( 1 << ( ( (C) > 2 ) ? ( (C) * 2 - 1 ) : ( (C) * 2 +  4 ) ) )
+#define OVERMASK(C)		( 1 << ( ( (C) > 2 ) ? ( (C) * 2 + 5 ) : ( (C) * 2 + 10 ) ) )
+#define EVBITS(C)		{ 0, OVERMASK(C), INDXMASK(C), OVERMASK(C) | INDXMASK(C) }
+
+// Translation table to map IntSrc into equivalent RDMISC2 event flag
+// bits.
+//static const uint16_t EventBits[][4] = { EVBITS(0), EVBITS(1), EVBITS(2), EVBITS(3), EVBITS(4), EVBITS(5) };
+
+/* enc_private; */
+static enc_private enc_private_data[] = {
+	{
+	      GetEnable:GetEnable_A,
+	      GetIntSrc:GetIntSrc_A,
+	      GetLoadTrig:GetLoadTrig_A,
+	      GetMode:	GetMode_A,
+	      PulseIndex:PulseIndex_A,
+	      SetEnable:SetEnable_A,
+	      SetIntSrc:SetIntSrc_A,
+	      SetLoadTrig:SetLoadTrig_A,
+	      SetMode:	SetMode_A,
+	      ResetCapFlags:ResetCapFlags_A,
+	      MyCRA:	LP_CR0A,
+	      MyCRB:	LP_CR0B,
+	      MyLatchLsw:LP_CNTR0ALSW,
+	      MyEventBits:EVBITS(0),
+		},
+	{
+	      GetEnable:GetEnable_A,
+	      GetIntSrc:GetIntSrc_A,
+	      GetLoadTrig:GetLoadTrig_A,
+	      GetMode:	GetMode_A,
+	      PulseIndex:PulseIndex_A,
+	      SetEnable:SetEnable_A,
+	      SetIntSrc:SetIntSrc_A,
+	      SetLoadTrig:SetLoadTrig_A,
+	      SetMode:	SetMode_A,
+	      ResetCapFlags:ResetCapFlags_A,
+	      MyCRA:	LP_CR1A,
+	      MyCRB:	LP_CR1B,
+	      MyLatchLsw:LP_CNTR1ALSW,
+	      MyEventBits:EVBITS(1),
+		},
+	{
+	      GetEnable:GetEnable_A,
+	      GetIntSrc:GetIntSrc_A,
+	      GetLoadTrig:GetLoadTrig_A,
+	      GetMode:	GetMode_A,
+	      PulseIndex:PulseIndex_A,
+	      SetEnable:SetEnable_A,
+	      SetIntSrc:SetIntSrc_A,
+	      SetLoadTrig:SetLoadTrig_A,
+	      SetMode:	SetMode_A,
+	      ResetCapFlags:ResetCapFlags_A,
+	      MyCRA:	LP_CR2A,
+	      MyCRB:	LP_CR2B,
+	      MyLatchLsw:LP_CNTR2ALSW,
+	      MyEventBits:EVBITS(2),
+		},
+	{
+	      GetEnable:GetEnable_B,
+	      GetIntSrc:GetIntSrc_B,
+	      GetLoadTrig:GetLoadTrig_B,
+	      GetMode:	GetMode_B,
+	      PulseIndex:PulseIndex_B,
+	      SetEnable:SetEnable_B,
+	      SetIntSrc:SetIntSrc_B,
+	      SetLoadTrig:SetLoadTrig_B,
+	      SetMode:	SetMode_B,
+	      ResetCapFlags:ResetCapFlags_B,
+	      MyCRA:	LP_CR0A,
+	      MyCRB:	LP_CR0B,
+	      MyLatchLsw:LP_CNTR0BLSW,
+	      MyEventBits:EVBITS(3),
+		},
+	{
+	      GetEnable:GetEnable_B,
+	      GetIntSrc:GetIntSrc_B,
+	      GetLoadTrig:GetLoadTrig_B,
+	      GetMode:	GetMode_B,
+	      PulseIndex:PulseIndex_B,
+	      SetEnable:SetEnable_B,
+	      SetIntSrc:SetIntSrc_B,
+	      SetLoadTrig:SetLoadTrig_B,
+	      SetMode:	SetMode_B,
+	      ResetCapFlags:ResetCapFlags_B,
+	      MyCRA:	LP_CR1A,
+	      MyCRB:	LP_CR1B,
+	      MyLatchLsw:LP_CNTR1BLSW,
+	      MyEventBits:EVBITS(4),
+		},
+	{
+	      GetEnable:GetEnable_B,
+	      GetIntSrc:GetIntSrc_B,
+	      GetLoadTrig:GetLoadTrig_B,
+	      GetMode:	GetMode_B,
+	      PulseIndex:PulseIndex_B,
+	      SetEnable:SetEnable_B,
+	      SetIntSrc:SetIntSrc_B,
+	      SetLoadTrig:SetLoadTrig_B,
+	      SetMode:	SetMode_B,
+	      ResetCapFlags:ResetCapFlags_B,
+	      MyCRA:	LP_CR2A,
+	      MyCRB:	LP_CR2B,
+	      MyLatchLsw:LP_CNTR2BLSW,
+	      MyEventBits:EVBITS(5),
+		},
+};
+
+// enab/disable a function or test status bit(s) that are accessed
+// through Main Control Registers 1 or 2.
+#define MC_ENABLE( REGADRS, CTRLWORD )	writel(  ( (uint32_t)( CTRLWORD ) << 16 ) | (uint32_t)( CTRLWORD ),devpriv->base_addr+( REGADRS ) )
+
+#define MC_DISABLE( REGADRS, CTRLWORD )	writel(  (uint32_t)( CTRLWORD ) << 16 , devpriv->base_addr+( REGADRS ) )
+
+#define MC_TEST( REGADRS, CTRLWORD )	( ( readl(devpriv->base_addr+( REGADRS )) & CTRLWORD ) != 0 )
+
+/* #define WR7146(REGARDS,CTRLWORD)
+    writel(CTRLWORD,(uint32_t)(devpriv->base_addr+(REGARDS))) */
+#define WR7146(REGARDS,CTRLWORD) writel(CTRLWORD,devpriv->base_addr+(REGARDS))
+
+/* #define RR7146(REGARDS)
+    readl((uint32_t)(devpriv->base_addr+(REGARDS))) */
+#define RR7146(REGARDS)		readl(devpriv->base_addr+(REGARDS))
+
+#define BUGFIX_STREG(REGADRS)   ( REGADRS - 4 )
+
+// Write a time slot control record to TSL2.
+#define VECTPORT( VECTNUM )		(P_TSL2 + ( (VECTNUM) << 2 ))
+#define SETVECT( VECTNUM, VECTVAL )	WR7146(VECTPORT( VECTNUM ), (VECTVAL))
+
+// Code macros used for constructing I2C command bytes.
+#define I2C_B2(ATTR,VAL)	( ( (ATTR) << 6 ) | ( (VAL) << 24 ) )
+#define I2C_B1(ATTR,VAL)	( ( (ATTR) << 4 ) | ( (VAL) << 16 ) )
+#define I2C_B0(ATTR,VAL)	( ( (ATTR) << 2 ) | ( (VAL) <<  8 ) )
+
+static const comedi_lrange s626_range_table = { 2, {
+			RANGE(-5, 5),
+			RANGE(-10, 10),
+	}
+};
+
+static int s626_attach(comedi_device * dev, comedi_devconfig * it)
+{
+/*   uint8_t	PollList; */
+/*   uint16_t	AdcData; */
+/*   uint16_t	StartVal; */
+/*   uint16_t	index; */
+/*   unsigned int data[16]; */
+	int result;
+	int i;
+	int ret;
+	resource_size_t resourceStart;
+	dma_addr_t appdma;
+	comedi_subdevice *s;
+	struct pci_dev *pdev;
+
+	if (alloc_private(dev, sizeof(s626_private)) < 0)
+		return -ENOMEM;
+
+	for (pdev = pci_get_device(PCI_VENDOR_ID_S626, PCI_DEVICE_ID_S626,
+			NULL); pdev != NULL;
+		pdev = pci_get_device(PCI_VENDOR_ID_S626,
+			PCI_DEVICE_ID_S626, pdev)) {
+		if (it->options[0] || it->options[1]) {
+			if (pdev->bus->number == it->options[0] &&
+				PCI_SLOT(pdev->devfn) == it->options[1]) {
+				/* matches requested bus/slot */
+				break;
+			}
+		} else {
+			/* no bus/slot specified */
+			break;
+		}
+	}
+	devpriv->pdev = pdev;
+
+	if (pdev == NULL) {
+		printk("s626_attach: Board not present!!!\n");
+		return -ENODEV;
+	}
+
+	if ((result = comedi_pci_enable(pdev, "s626")) < 0) {
+		printk("s626_attach: comedi_pci_enable fails\n");
+		return -ENODEV;
+	}
+	devpriv->got_regions = 1;
+
+	resourceStart = pci_resource_start(devpriv->pdev, 0);
+
+	devpriv->base_addr = ioremap(resourceStart, SIZEOF_ADDRESS_SPACE);
+	if (devpriv->base_addr == NULL) {
+		printk("s626_attach: IOREMAP failed\n");
+		return -ENODEV;
+	}
+
+	if (devpriv->base_addr) {
+		//disable master interrupt
+		writel(0, devpriv->base_addr + P_IER);
+
+		//soft reset
+		writel(MC1_SOFT_RESET, devpriv->base_addr + P_MC1);
+
+		//DMA FIXME DMA//
+		DEBUG("s626_attach: DMA ALLOCATION\n");
+
+		//adc buffer allocation
+		devpriv->allocatedBuf = 0;
+
+		if ((devpriv->ANABuf.LogicalBase =
+				pci_alloc_consistent(devpriv->pdev, DMABUF_SIZE,
+					&appdma)) == NULL) {
+			printk("s626_attach: DMA Memory mapping error\n");
+			return -ENOMEM;
+		}
+
+		devpriv->ANABuf.PhysicalBase = appdma;
+
+		DEBUG("s626_attach: AllocDMAB ADC Logical=%p, bsize=%d, Physical=0x%x\n", devpriv->ANABuf.LogicalBase, DMABUF_SIZE, (uint32_t) devpriv->ANABuf.PhysicalBase);
+
+		devpriv->allocatedBuf++;
+
+		if ((devpriv->RPSBuf.LogicalBase =
+				pci_alloc_consistent(devpriv->pdev, DMABUF_SIZE,
+					&appdma)) == NULL) {
+			printk("s626_attach: DMA Memory mapping error\n");
+			return -ENOMEM;
+		}
+
+		devpriv->RPSBuf.PhysicalBase = appdma;
+
+		DEBUG("s626_attach: AllocDMAB RPS Logical=%p, bsize=%d, Physical=0x%x\n", devpriv->RPSBuf.LogicalBase, DMABUF_SIZE, (uint32_t) devpriv->RPSBuf.PhysicalBase);
+
+		devpriv->allocatedBuf++;
+
+	}
+
+	dev->board_ptr = s626_boards;
+	dev->board_name = thisboard->name;
+
+	if (alloc_subdevices(dev, 6) < 0)
+		return -ENOMEM;
+
+	dev->iobase = (unsigned long)devpriv->base_addr;
+	dev->irq = devpriv->pdev->irq;
+
+	//set up interrupt handler
+	if (dev->irq == 0) {
+		printk(" unknown irq (bad)\n");
+	} else {
+		if ((ret = comedi_request_irq(dev->irq, s626_irq_handler,
+					IRQF_SHARED, "s626", dev)) < 0) {
+			printk(" irq not available\n");
+			dev->irq = 0;
+		}
+	}
+
+	DEBUG("s626_attach: -- it opts  %d,%d -- \n",
+		it->options[0], it->options[1]);
+
+	s = dev->subdevices + 0;
+	/* analog input subdevice */
+	dev->read_subdev = s;
+	/* we support single-ended (ground) and differential */
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_CMD_READ;
+	s->n_chan = thisboard->ai_chans;
+	s->maxdata = (0xffff >> 2);
+	s->range_table = &s626_range_table;
+	s->len_chanlist = thisboard->ai_chans;	/* This is the maximum chanlist
+						   length that the board can
+						   handle */
+	s->insn_config = s626_ai_insn_config;
+	s->insn_read = s626_ai_insn_read;
+	s->do_cmd = s626_ai_cmd;
+	s->do_cmdtest = s626_ai_cmdtest;
+	s->cancel = s626_ai_cancel;
+
+	s = dev->subdevices + 1;
+	/* analog output subdevice */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = thisboard->ao_chans;
+	s->maxdata = (0x3fff);
+	s->range_table = &range_bipolar10;
+	s->insn_write = s626_ao_winsn;
+	s->insn_read = s626_ao_rinsn;
+
+	s = dev->subdevices + 2;
+	/* digital I/O subdevice */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = S626_DIO_CHANNELS;
+	s->maxdata = 1;
+	s->io_bits = 0xffff;
+	s->private = &dio_private_A;
+	s->range_table = &range_digital;
+	s->insn_config = s626_dio_insn_config;
+	s->insn_bits = s626_dio_insn_bits;
+
+	s = dev->subdevices + 3;
+	/* digital I/O subdevice */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = 16;
+	s->maxdata = 1;
+	s->io_bits = 0xffff;
+	s->private = &dio_private_B;
+	s->range_table = &range_digital;
+	s->insn_config = s626_dio_insn_config;
+	s->insn_bits = s626_dio_insn_bits;
+
+	s = dev->subdevices + 4;
+	/* digital I/O subdevice */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = 16;
+	s->maxdata = 1;
+	s->io_bits = 0xffff;
+	s->private = &dio_private_C;
+	s->range_table = &range_digital;
+	s->insn_config = s626_dio_insn_config;
+	s->insn_bits = s626_dio_insn_bits;
+
+	s = dev->subdevices + 5;
+	/* encoder (counter) subdevice */
+	s->type = COMEDI_SUBD_COUNTER;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE | SDF_LSAMPL;
+	s->n_chan = thisboard->enc_chans;
+	s->private = enc_private_data;
+	s->insn_config = s626_enc_insn_config;
+	s->insn_read = s626_enc_insn_read;
+	s->insn_write = s626_enc_insn_write;
+	s->maxdata = 0xffffff;
+	s->range_table = &range_unknown;
+
+	//stop ai_command
+	devpriv->ai_cmd_running = 0;
+
+	if (devpriv->base_addr && (devpriv->allocatedBuf == 2)) {
+		dma_addr_t pPhysBuf;
+		uint16_t chan;
+
+		// enab DEBI and audio pins, enable I2C interface.
+		MC_ENABLE(P_MC1, MC1_DEBI | MC1_AUDIO | MC1_I2C);
+		// Configure DEBI operating mode.
+		WR7146(P_DEBICFG, DEBI_CFG_SLAVE16	// Local bus is 16
+			// bits wide.
+			| (DEBI_TOUT << DEBI_CFG_TOUT_BIT)	// Declare DEBI
+			// transfer timeout
+			// interval.
+			| DEBI_SWAP	// Set up byte lane
+			// steering.
+			| DEBI_CFG_INTEL);	// Intel-compatible
+		// local bus (DEBI
+		// never times out).
+		DEBUG("s626_attach: %d debi init -- %d\n",
+			DEBI_CFG_SLAVE16 | (DEBI_TOUT << DEBI_CFG_TOUT_BIT) |
+			DEBI_SWAP | DEBI_CFG_INTEL,
+			DEBI_CFG_INTEL | DEBI_CFG_TOQ | DEBI_CFG_INCQ |
+			DEBI_CFG_16Q);
+
+		//DEBI INIT S626 WR7146( P_DEBICFG, DEBI_CFG_INTEL | DEBI_CFG_TOQ
+		//| DEBI_CFG_INCQ| DEBI_CFG_16Q); //end
+
+		// Paging is disabled.
+		WR7146(P_DEBIPAGE, DEBI_PAGE_DISABLE);	// Disable MMU paging.
+
+		// Init GPIO so that ADC Start* is negated.
+		WR7146(P_GPIO, GPIO_BASE | GPIO1_HI);
+
+		//IsBoardRevA is a boolean that indicates whether the board is
+		//RevA.
+
+		// VERSION 2.01 CHANGE: REV A & B BOARDS NOW SUPPORTED BY DYNAMIC
+		// EEPROM ADDRESS SELECTION.  Initialize the I2C interface, which
+		// is used to access the onboard serial EEPROM.  The EEPROM's I2C
+		// DeviceAddress is hardwired to a value that is dependent on the
+		// 626 board revision.  On all board revisions, the EEPROM stores
+		// TrimDAC calibration constants for analog I/O.  On RevB and
+		// higher boards, the DeviceAddress is hardwired to 0 to enable
+		// the EEPROM to also store the PCI SubVendorID and SubDeviceID;
+		// this is the address at which the SAA7146 expects a
+		// configuration EEPROM to reside.  On RevA boards, the EEPROM
+		// device address, which is hardwired to 4, prevents the SAA7146
+		// from retrieving PCI sub-IDs, so the SAA7146 uses its built-in
+		// default values, instead.
+
+		//    devpriv->I2Cards= IsBoardRevA ? 0xA8 : 0xA0; // Set I2C EEPROM
+		// DeviceType (0xA0)
+		// and DeviceAddress<<1.
+
+		devpriv->I2CAdrs = 0xA0;	// I2C device address for onboard
+		// eeprom(revb)
+
+		// Issue an I2C ABORT command to halt any I2C operation in
+		//progress and reset BUSY flag.
+		WR7146(P_I2CSTAT, I2C_CLKSEL | I2C_ABORT);	// Write I2C control:
+		// abort any I2C
+		// activity.
+		MC_ENABLE(P_MC2, MC2_UPLD_IIC);	// Invoke command
+		// upload
+		while ((RR7146(P_MC2) & MC2_UPLD_IIC) == 0) ;	// and wait for
+		// upload to
+		// complete.
+
+		// Per SAA7146 data sheet, write to STATUS reg twice to reset all
+		// I2C error flags.
+		for (i = 0; i < 2; i++) {
+			WR7146(P_I2CSTAT, I2C_CLKSEL);	// Write I2C control: reset
+			// error flags.
+			MC_ENABLE(P_MC2, MC2_UPLD_IIC);	// Invoke command upload
+			while (!MC_TEST(P_MC2, MC2_UPLD_IIC)) ;	//   and wait for
+			//   upload to
+			//   complete.
+		}
+
+		// Init audio interface functional attributes: set DAC/ADC serial
+		// clock rates, invert DAC serial clock so that DAC data setup
+		// times are satisfied, enable DAC serial clock out.
+		WR7146(P_ACON2, ACON2_INIT);
+
+		// Set up TSL1 slot list, which is used to control the
+		// accumulation of ADC data: RSD1 = shift data in on SD1.  SIB_A1
+		// = store data uint8_t at next available location in FB BUFFER1
+		// register.
+		WR7146(P_TSL1, RSD1 | SIB_A1);	// Fetch ADC high data
+		// uint8_t.
+		WR7146(P_TSL1 + 4, RSD1 | SIB_A1 | EOS);	// Fetch ADC low data
+		// uint8_t; end of
+		// TSL1.
+
+		// enab TSL1 slot list so that it executes all the time.
+		WR7146(P_ACON1, ACON1_ADCSTART);
+
+		// Initialize RPS registers used for ADC.
+
+		//Physical start of RPS program.
+		WR7146(P_RPSADDR1, (uint32_t) devpriv->RPSBuf.PhysicalBase);
+
+		WR7146(P_RPSPAGE1, 0);	// RPS program performs no
+		// explicit mem writes.
+		WR7146(P_RPS1_TOUT, 0);	// Disable RPS timeouts.
+
+		// SAA7146 BUG WORKAROUND.  Initialize SAA7146 ADC interface to a
+		// known state by invoking ADCs until FB BUFFER 1 register shows
+		// that it is correctly receiving ADC data.  This is necessary
+		// because the SAA7146 ADC interface does not start up in a
+		// defined state after a PCI reset.
+
+/*     PollList = EOPL;			// Create a simple polling */
+/* 					// list for analog input */
+/* 					// channel 0. */
+/*     ResetADC( dev, &PollList ); */
+
+/*     s626_ai_rinsn(dev,dev->subdevices,NULL,data); //( &AdcData ); // */
+/* 						  //Get initial ADC */
+/* 						  //value. */
+
+/*     StartVal = data[0]; */
+
+/*     // VERSION 2.01 CHANGE: TIMEOUT ADDED TO PREVENT HANGED EXECUTION. */
+/*     // Invoke ADCs until the new ADC value differs from the initial */
+/*     // value or a timeout occurs.  The timeout protects against the */
+/*     // possibility that the driver is restarting and the ADC data is a */
+/*     // fixed value resulting from the applied ADC analog input being */
+/*     // unusually quiet or at the rail. */
+
+/*     for ( index = 0; index < 500; index++ ) */
+/*       { */
+/* 	s626_ai_rinsn(dev,dev->subdevices,NULL,data); */
+/* 	AdcData = data[0];	//ReadADC(  &AdcData ); */
+/* 	if ( AdcData != StartVal ) */
+/* 	  break; */
+/*       } */
+
+		// end initADC
+
+		// init the DAC interface
+
+		// Init Audio2's output DMAC attributes: burst length = 1 DWORD,
+		// threshold = 1 DWORD.
+		WR7146(P_PCI_BT_A, 0);
+
+		// Init Audio2's output DMA physical addresses.  The protection
+		// address is set to 1 DWORD past the base address so that a
+		// single DWORD will be transferred each time a DMA transfer is
+		// enabled.
+
+		pPhysBuf =
+			devpriv->ANABuf.PhysicalBase +
+			(DAC_WDMABUF_OS * sizeof(uint32_t));
+
+		WR7146(P_BASEA2_OUT, (uint32_t) pPhysBuf);	// Buffer base adrs.
+		WR7146(P_PROTA2_OUT, (uint32_t) (pPhysBuf + sizeof(uint32_t)));	// Protection address.
+
+		// Cache Audio2's output DMA buffer logical address.  This is
+		// where DAC data is buffered for A2 output DMA transfers.
+		devpriv->pDacWBuf =
+			(uint32_t *) devpriv->ANABuf.LogicalBase +
+			DAC_WDMABUF_OS;
+
+		// Audio2's output channels does not use paging.  The protection
+		// violation handling bit is set so that the DMAC will
+		// automatically halt and its PCI address pointer will be reset
+		// when the protection address is reached.
+		WR7146(P_PAGEA2_OUT, 8);
+
+		// Initialize time slot list 2 (TSL2), which is used to control
+		// the clock generation for and serialization of data to be sent
+		// to the DAC devices.  Slot 0 is a NOP that is used to trap TSL
+		// execution; this permits other slots to be safely modified
+		// without first turning off the TSL sequencer (which is
+		// apparently impossible to do).  Also, SD3 (which is driven by a
+		// pull-up resistor) is shifted in and stored to the MSB of
+		// FB_BUFFER2 to be used as evidence that the slot sequence has
+		// not yet finished executing.
+		SETVECT(0, XSD2 | RSD3 | SIB_A2 | EOS);	// Slot 0: Trap TSL
+		// execution, shift 0xFF
+		// into FB_BUFFER2.
+
+		// Initialize slot 1, which is constant.  Slot 1 causes a DWORD to
+		// be transferred from audio channel 2's output FIFO to the FIFO's
+		// output buffer so that it can be serialized and sent to the DAC
+		// during subsequent slots.  All remaining slots are dynamically
+		// populated as required by the target DAC device.
+		SETVECT(1, LF_A2);	// Slot 1: Fetch DWORD from Audio2's
+		// output FIFO.
+
+		// Start DAC's audio interface (TSL2) running.
+		WR7146(P_ACON1, ACON1_DACSTART);
+
+		////////////////////////////////////////////////////////
+
+		// end init DAC interface
+
+		// Init Trim DACs to calibrated values.  Do it twice because the
+		// SAA7146 audio channel does not always reset properly and
+		// sometimes causes the first few TrimDAC writes to malfunction.
+
+		LoadTrimDACs(dev);
+		LoadTrimDACs(dev);	// Insurance.
+
+		//////////////////////////////////////////////////////////////////
+		// Manually init all gate array hardware in case this is a soft
+		// reset (we have no way of determining whether this is a warm or
+		// cold start).  This is necessary because the gate array will
+		// reset only in response to a PCI hard reset; there is no soft
+		// reset function.
+
+		// Init all DAC outputs to 0V and init all DAC setpoint and
+		// polarity images.
+		for (chan = 0; chan < S626_DAC_CHANNELS; chan++)
+			SetDAC(dev, chan, 0);
+
+		// Init image of WRMISC2 Battery Charger Enabled control bit.
+		// This image is used when the state of the charger control bit,
+		// which has no direct hardware readback mechanism, is queried.
+		devpriv->ChargeEnabled = 0;
+
+		// Init image of watchdog timer interval in WRMISC2.  This image
+		// maintains the value of the control bits of MISC2 are
+		// continuously reset to zero as long as the WD timer is disabled.
+		devpriv->WDInterval = 0;
+
+		// Init Counter Interrupt enab mask for RDMISC2.  This mask is
+		// applied against MISC2 when testing to determine which timer
+		// events are requesting interrupt service.
+		devpriv->CounterIntEnabs = 0;
+
+		// Init counters.
+		CountersInit(dev);
+
+		// Without modifying the state of the Battery Backup enab, disable
+		// the watchdog timer, set DIO channels 0-5 to operate in the
+		// standard DIO (vs. counter overflow) mode, disable the battery
+		// charger, and reset the watchdog interval selector to zero.
+		WriteMISC2(dev, (uint16_t) (DEBIread(dev,
+					LP_RDMISC2) & MISC2_BATT_ENABLE));
+
+		// Initialize the digital I/O subsystem.
+		s626_dio_init(dev);
+
+		//enable interrupt test
+		// writel(IRQ_GPIO3 | IRQ_RPS1,devpriv->base_addr+P_IER);
+	}
+
+	DEBUG("s626_attach: comedi%d s626 attached %04x\n", dev->minor,
+		(uint32_t) devpriv->base_addr);
+
+	return 1;
+}
+
+static lsampl_t s626_ai_reg_to_uint(int data)
+{
+	lsampl_t tempdata;
+
+	tempdata = (data >> 18);
+	if (tempdata & 0x2000)
+		tempdata &= 0x1fff;
+	else
+		tempdata += (1 << 13);
+
+	return tempdata;
+}
+
+/* static lsampl_t s626_uint_to_reg(comedi_subdevice *s, int data){ */
+/*   return 0; */
+/* } */
+
+static irqreturn_t s626_irq_handler(int irq, void *d PT_REGS_ARG)
+{
+	comedi_device *dev = d;
+	comedi_subdevice *s;
+	comedi_cmd *cmd;
+	enc_private *k;
+	unsigned long flags;
+	int32_t *readaddr;
+	uint32_t irqtype, irqstatus;
+	int i = 0;
+	sampl_t tempdata;
+	uint8_t group;
+	uint16_t irqbit;
+
+	DEBUG("s626_irq_handler: interrupt request recieved!!!\n");
+
+	if (dev->attached == 0)
+		return IRQ_NONE;
+	// lock to avoid race with comedi_poll
+	comedi_spin_lock_irqsave(&dev->spinlock, flags);
+
+	//save interrupt enable register state
+	irqstatus = readl(devpriv->base_addr + P_IER);
+
+	//read interrupt type
+	irqtype = readl(devpriv->base_addr + P_ISR);
+
+	//disable master interrupt
+	writel(0, devpriv->base_addr + P_IER);
+
+	//clear interrupt
+	writel(irqtype, devpriv->base_addr + P_ISR);
+
+	//do somethings
+	DEBUG("s626_irq_handler: interrupt type %d\n", irqtype);
+
+	switch (irqtype) {
+	case IRQ_RPS1:		// end_of_scan occurs
+
+		DEBUG("s626_irq_handler: RPS1 irq detected\n");
+
+		// manage ai subdevice
+		s = dev->subdevices;
+		cmd = &(s->async->cmd);
+
+		// Init ptr to DMA buffer that holds new ADC data.  We skip the
+		// first uint16_t in the buffer because it contains junk data from
+		// the final ADC of the previous poll list scan.
+		readaddr = (int32_t *) devpriv->ANABuf.LogicalBase + 1;
+
+		// get the data and hand it over to comedi
+		for (i = 0; i < (s->async->cmd.chanlist_len); i++) {
+			// Convert ADC data to 16-bit integer values and copy to application
+			// buffer.
+			tempdata = s626_ai_reg_to_uint((int)*readaddr);
+			readaddr++;
+
+			//put data into read buffer
+			// comedi_buf_put(s->async, tempdata);
+			if (cfc_write_to_buffer(s, tempdata) == 0)
+				printk("s626_irq_handler: cfc_write_to_buffer error!\n");
+
+			DEBUG("s626_irq_handler: ai channel %d acquired: %d\n",
+				i, tempdata);
+		}
+
+		//end of scan occurs
+		s->async->events |= COMEDI_CB_EOS;
+
+		if (!(devpriv->ai_continous))
+			devpriv->ai_sample_count--;
+		if (devpriv->ai_sample_count <= 0) {
+			devpriv->ai_cmd_running = 0;
+
+			// Stop RPS program.
+			MC_DISABLE(P_MC1, MC1_ERPS1);
+
+			//send end of acquisition
+			s->async->events |= COMEDI_CB_EOA;
+
+			//disable master interrupt
+			irqstatus = 0;
+		}
+
+		if (devpriv->ai_cmd_running && cmd->scan_begin_src == TRIG_EXT) {
+			DEBUG("s626_irq_handler: enable interrupt on dio channel %d\n", cmd->scan_begin_arg);
+
+			s626_dio_set_irq(dev, cmd->scan_begin_arg);
+
+			DEBUG("s626_irq_handler: External trigger is set!!!\n");
+		}
+		// tell comedi that data is there
+		DEBUG("s626_irq_handler: events %d\n", s->async->events);
+		comedi_event(dev, s);
+		break;
+	case IRQ_GPIO3:	//check dio and conter interrupt
+
+		DEBUG("s626_irq_handler: GPIO3 irq detected\n");
+
+		// manage ai subdevice
+		s = dev->subdevices;
+		cmd = &(s->async->cmd);
+
+		//s626_dio_clear_irq(dev);
+
+		for (group = 0; group < S626_DIO_BANKS; group++) {
+			irqbit = 0;
+			//read interrupt type
+			irqbit = DEBIread(dev,
+				((dio_private *) (dev->subdevices + 2 +
+						group)->private)->RDCapFlg);
+
+			//check if interrupt is generated from dio channels
+			if (irqbit) {
+				s626_dio_reset_irq(dev, group, irqbit);
+				DEBUG("s626_irq_handler: check interrupt on dio group %d %d\n", group, i);
+				if (devpriv->ai_cmd_running) {
+					//check if interrupt is an ai acquisition start trigger
+					if ((irqbit >> (cmd->start_arg -
+								(16 * group)))
+						== 1
+						&& cmd->start_src == TRIG_EXT) {
+						DEBUG("s626_irq_handler: Edge capture interrupt recieved from channel %d\n", cmd->start_arg);
+
+						// Start executing the RPS program.
+						MC_ENABLE(P_MC1, MC1_ERPS1);
+
+						DEBUG("s626_irq_handler: aquisition start triggered!!!\n");
+
+						if (cmd->scan_begin_src ==
+							TRIG_EXT) {
+							DEBUG("s626_ai_cmd: enable interrupt on dio channel %d\n", cmd->scan_begin_arg);
+
+							s626_dio_set_irq(dev,
+								cmd->
+								scan_begin_arg);
+
+							DEBUG("s626_irq_handler: External scan trigger is set!!!\n");
+						}
+					}
+					if ((irqbit >> (cmd->scan_begin_arg -
+								(16 * group)))
+						== 1
+						&& cmd->scan_begin_src ==
+						TRIG_EXT) {
+						DEBUG("s626_irq_handler: Edge capture interrupt recieved from channel %d\n", cmd->scan_begin_arg);
+
+						// Trigger ADC scan loop start by setting RPS Signal 0.
+						MC_ENABLE(P_MC2, MC2_ADC_RPS);
+
+						DEBUG("s626_irq_handler: scan triggered!!! %d\n", devpriv->ai_sample_count);
+						if (cmd->convert_src ==
+							TRIG_EXT) {
+
+							DEBUG("s626_ai_cmd: enable interrupt on dio channel %d group %d\n", cmd->convert_arg - (16 * group), group);
+
+							devpriv->
+								ai_convert_count
+								=
+								cmd->
+								chanlist_len;
+
+							s626_dio_set_irq(dev,
+								cmd->
+								convert_arg);
+
+							DEBUG("s626_irq_handler: External convert trigger is set!!!\n");
+						}
+
+						if (cmd->convert_src ==
+							TRIG_TIMER) {
+							k = &encpriv[5];
+							devpriv->
+								ai_convert_count
+								=
+								cmd->
+								chanlist_len;
+							k->SetEnable(dev, k,
+								CLKENAB_ALWAYS);
+						}
+					}
+					if ((irqbit >> (cmd->convert_arg -
+								(16 * group)))
+						== 1
+						&& cmd->convert_src ==
+						TRIG_EXT) {
+						DEBUG("s626_irq_handler: Edge capture interrupt recieved from channel %d\n", cmd->convert_arg);
+
+						// Trigger ADC scan loop start by setting RPS Signal 0.
+						MC_ENABLE(P_MC2, MC2_ADC_RPS);
+
+						DEBUG("s626_irq_handler: adc convert triggered!!!\n");
+
+						devpriv->ai_convert_count--;
+
+						if (devpriv->ai_convert_count >
+							0) {
+
+							DEBUG("s626_ai_cmd: enable interrupt on dio channel %d group %d\n", cmd->convert_arg - (16 * group), group);
+
+							s626_dio_set_irq(dev,
+								cmd->
+								convert_arg);
+
+							DEBUG("s626_irq_handler: External trigger is set!!!\n");
+						}
+					}
+				}
+				break;
+			}
+		}
+
+		//read interrupt type
+		irqbit = DEBIread(dev, LP_RDMISC2);
+
+		//check interrupt on counters
+		DEBUG("s626_irq_handler: check counters interrupt %d\n",
+			irqbit);
+
+		if (irqbit & IRQ_COINT1A) {
+			DEBUG("s626_irq_handler: interrupt on counter 1A overflow\n");
+			k = &encpriv[0];
+
+			//clear interrupt capture flag
+			k->ResetCapFlags(dev, k);
+		}
+		if (irqbit & IRQ_COINT2A) {
+			DEBUG("s626_irq_handler: interrupt on counter 2A overflow\n");
+			k = &encpriv[1];
+
+			//clear interrupt capture flag
+			k->ResetCapFlags(dev, k);
+		}
+		if (irqbit & IRQ_COINT3A) {
+			DEBUG("s626_irq_handler: interrupt on counter 3A overflow\n");
+			k = &encpriv[2];
+
+			//clear interrupt capture flag
+			k->ResetCapFlags(dev, k);
+		}
+		if (irqbit & IRQ_COINT1B) {
+			DEBUG("s626_irq_handler: interrupt on counter 1B overflow\n");
+			k = &encpriv[3];
+
+			//clear interrupt capture flag
+			k->ResetCapFlags(dev, k);
+		}
+		if (irqbit & IRQ_COINT2B) {
+			DEBUG("s626_irq_handler: interrupt on counter 2B overflow\n");
+			k = &encpriv[4];
+
+			//clear interrupt capture flag
+			k->ResetCapFlags(dev, k);
+
+			if (devpriv->ai_convert_count > 0) {
+				devpriv->ai_convert_count--;
+				if (devpriv->ai_convert_count == 0)
+					k->SetEnable(dev, k, CLKENAB_INDEX);
+
+				if (cmd->convert_src == TRIG_TIMER) {
+					DEBUG("s626_irq_handler: conver timer trigger!!! %d\n", devpriv->ai_convert_count);
+
+					// Trigger ADC scan loop start by setting RPS Signal 0.
+					MC_ENABLE(P_MC2, MC2_ADC_RPS);
+				}
+			}
+		}
+		if (irqbit & IRQ_COINT3B) {
+			DEBUG("s626_irq_handler: interrupt on counter 3B overflow\n");
+			k = &encpriv[5];
+
+			//clear interrupt capture flag
+			k->ResetCapFlags(dev, k);
+
+			if (cmd->scan_begin_src == TRIG_TIMER) {
+				DEBUG("s626_irq_handler: scan timer trigger!!!\n");
+
+				// Trigger ADC scan loop start by setting RPS Signal 0.
+				MC_ENABLE(P_MC2, MC2_ADC_RPS);
+			}
+
+			if (cmd->convert_src == TRIG_TIMER) {
+				DEBUG("s626_irq_handler: convert timer trigger is set\n");
+				k = &encpriv[4];
+				devpriv->ai_convert_count = cmd->chanlist_len;
+				k->SetEnable(dev, k, CLKENAB_ALWAYS);
+			}
+		}
+	}
+
+	//enable interrupt
+	writel(irqstatus, devpriv->base_addr + P_IER);
+
+	DEBUG("s626_irq_handler: exit interrupt service routine.\n");
+
+	comedi_spin_unlock_irqrestore(&dev->spinlock, flags);
+	return IRQ_HANDLED;
+}
+
+static int s626_detach(comedi_device * dev)
+{
+	if (devpriv) {
+		//stop ai_command
+		devpriv->ai_cmd_running = 0;
+
+		if (devpriv->base_addr) {
+			//interrupt mask
+			WR7146(P_IER, 0);	// Disable master interrupt.
+			WR7146(P_ISR, IRQ_GPIO3 | IRQ_RPS1);	// Clear board's IRQ status flag.
+
+			// Disable the watchdog timer and battery charger.
+			WriteMISC2(dev, 0);
+
+			// Close all interfaces on 7146 device.
+			WR7146(P_MC1, MC1_SHUTDOWN);
+			WR7146(P_ACON1, ACON1_BASE);
+
+			CloseDMAB(dev, &devpriv->RPSBuf, DMABUF_SIZE);
+			CloseDMAB(dev, &devpriv->ANABuf, DMABUF_SIZE);
+		}
+
+		if (dev->irq) {
+			comedi_free_irq(dev->irq, dev);
+		}
+
+		if (devpriv->base_addr) {
+			iounmap(devpriv->base_addr);
+		}
+
+		if (devpriv->pdev) {
+			if (devpriv->got_regions) {
+				comedi_pci_disable(devpriv->pdev);
+			}
+			pci_dev_put(devpriv->pdev);
+		}
+	}
+
+	DEBUG("s626_detach: S626 detached!\n");
+
+	return 0;
+}
+
+/*
+ * this functions build the RPS program for hardware driven acquistion
+ */
+void ResetADC(comedi_device * dev, uint8_t * ppl)
+{
+	register uint32_t *pRPS;
+	uint32_t JmpAdrs;
+	uint16_t i;
+	uint16_t n;
+	uint32_t LocalPPL;
+	comedi_cmd *cmd = &(dev->subdevices->async->cmd);
+
+	// Stop RPS program in case it is currently running.
+	MC_DISABLE(P_MC1, MC1_ERPS1);
+
+	// Set starting logical address to write RPS commands.
+	pRPS = (uint32_t *) devpriv->RPSBuf.LogicalBase;
+
+	// Initialize RPS instruction pointer.
+	WR7146(P_RPSADDR1, (uint32_t) devpriv->RPSBuf.PhysicalBase);
+
+	// Construct RPS program in RPSBuf DMA buffer
+
+	if (cmd != NULL && cmd->scan_begin_src != TRIG_FOLLOW) {
+		DEBUG("ResetADC: scan_begin pause inserted\n");
+		// Wait for Start trigger.
+		*pRPS++ = RPS_PAUSE | RPS_SIGADC;
+		*pRPS++ = RPS_CLRSIGNAL | RPS_SIGADC;
+	}
+	// SAA7146 BUG WORKAROUND Do a dummy DEBI Write.  This is necessary
+	// because the first RPS DEBI Write following a non-RPS DEBI write
+	// seems to always fail.  If we don't do this dummy write, the ADC
+	// gain might not be set to the value required for the first slot in
+	// the poll list; the ADC gain would instead remain unchanged from
+	// the previously programmed value.
+	*pRPS++ = RPS_LDREG | (P_DEBICMD >> 2);	// Write DEBI Write command
+	// and address to shadow RAM.
+	*pRPS++ = DEBI_CMD_WRWORD | LP_GSEL;
+	*pRPS++ = RPS_LDREG | (P_DEBIAD >> 2);	// Write DEBI immediate data
+	// to shadow RAM:
+	*pRPS++ = GSEL_BIPOLAR5V;	// arbitrary immediate data
+	// value.
+	*pRPS++ = RPS_CLRSIGNAL | RPS_DEBI;	// Reset "shadow RAM
+	// uploaded" flag.
+	*pRPS++ = RPS_UPLOAD | RPS_DEBI;	// Invoke shadow RAM upload.
+	*pRPS++ = RPS_PAUSE | RPS_DEBI;	// Wait for shadow upload to finish.
+
+	// Digitize all slots in the poll list. This is implemented as a
+	// for loop to limit the slot count to 16 in case the application
+	// forgot to set the EOPL flag in the final slot.
+	for (devpriv->AdcItems = 0; devpriv->AdcItems < 16; devpriv->AdcItems++) {
+		// Convert application's poll list item to private board class
+		// format.  Each app poll list item is an uint8_t with form
+		// (EOPL,x,x,RANGE,CHAN<3:0>), where RANGE code indicates 0 =
+		// +-10V, 1 = +-5V, and EOPL = End of Poll List marker.
+		LocalPPL =
+			(*ppl << 8) | (*ppl & 0x10 ? GSEL_BIPOLAR5V :
+			GSEL_BIPOLAR10V);
+
+		// Switch ADC analog gain.
+		*pRPS++ = RPS_LDREG | (P_DEBICMD >> 2);	// Write DEBI command
+		// and address to
+		// shadow RAM.
+		*pRPS++ = DEBI_CMD_WRWORD | LP_GSEL;
+		*pRPS++ = RPS_LDREG | (P_DEBIAD >> 2);	// Write DEBI
+		// immediate data to
+		// shadow RAM.
+		*pRPS++ = LocalPPL;
+		*pRPS++ = RPS_CLRSIGNAL | RPS_DEBI;	// Reset "shadow RAM uploaded"
+		// flag.
+		*pRPS++ = RPS_UPLOAD | RPS_DEBI;	// Invoke shadow RAM upload.
+		*pRPS++ = RPS_PAUSE | RPS_DEBI;	// Wait for shadow upload to
+		// finish.
+
+		// Select ADC analog input channel.
+		*pRPS++ = RPS_LDREG | (P_DEBICMD >> 2);	// Write DEBI command
+		// and address to
+		// shadow RAM.
+		*pRPS++ = DEBI_CMD_WRWORD | LP_ISEL;
+		*pRPS++ = RPS_LDREG | (P_DEBIAD >> 2);	// Write DEBI
+		// immediate data to
+		// shadow RAM.
+		*pRPS++ = LocalPPL;
+		*pRPS++ = RPS_CLRSIGNAL | RPS_DEBI;	// Reset "shadow RAM uploaded"
+		// flag.
+		*pRPS++ = RPS_UPLOAD | RPS_DEBI;	// Invoke shadow RAM upload.
+		*pRPS++ = RPS_PAUSE | RPS_DEBI;	// Wait for shadow upload to
+		// finish.
+
+		// Delay at least 10 microseconds for analog input settling.
+		// Instead of padding with NOPs, we use RPS_JUMP instructions
+		// here; this allows us to produce a longer delay than is
+		// possible with NOPs because each RPS_JUMP flushes the RPS'
+		// instruction prefetch pipeline.
+		JmpAdrs =
+			(uint32_t) devpriv->RPSBuf.PhysicalBase +
+			(uint32_t) ((unsigned long)pRPS -
+			(unsigned long)devpriv->RPSBuf.LogicalBase);
+		for (i = 0; i < (10 * RPSCLK_PER_US / 2); i++) {
+			JmpAdrs += 8;	// Repeat to implement time delay:
+			*pRPS++ = RPS_JUMP;	// Jump to next RPS instruction.
+			*pRPS++ = JmpAdrs;
+		}
+
+		if (cmd != NULL && cmd->convert_src != TRIG_NOW) {
+			DEBUG("ResetADC: convert pause inserted\n");
+			// Wait for Start trigger.
+			*pRPS++ = RPS_PAUSE | RPS_SIGADC;
+			*pRPS++ = RPS_CLRSIGNAL | RPS_SIGADC;
+		}
+		// Start ADC by pulsing GPIO1.
+		*pRPS++ = RPS_LDREG | (P_GPIO >> 2);	// Begin ADC Start pulse.
+		*pRPS++ = GPIO_BASE | GPIO1_LO;
+		*pRPS++ = RPS_NOP;
+		// VERSION 2.03 CHANGE: STRETCH OUT ADC START PULSE.
+		*pRPS++ = RPS_LDREG | (P_GPIO >> 2);	// End ADC Start pulse.
+		*pRPS++ = GPIO_BASE | GPIO1_HI;
+
+		// Wait for ADC to complete (GPIO2 is asserted high when ADC not
+		// busy) and for data from previous conversion to shift into FB
+		// BUFFER 1 register.
+		*pRPS++ = RPS_PAUSE | RPS_GPIO2;	// Wait for ADC done.
+
+		// Transfer ADC data from FB BUFFER 1 register to DMA buffer.
+		*pRPS++ = RPS_STREG | (BUGFIX_STREG(P_FB_BUFFER1) >> 2);
+		*pRPS++ =
+			(uint32_t) devpriv->ANABuf.PhysicalBase +
+			(devpriv->AdcItems << 2);
+
+		// If this slot's EndOfPollList flag is set, all channels have
+		// now been processed.
+		if (*ppl++ & EOPL) {
+			devpriv->AdcItems++;	// Adjust poll list item count.
+			break;	// Exit poll list processing loop.
+		}
+	}
+	DEBUG("ResetADC: ADC items %d \n", devpriv->AdcItems);
+
+	// VERSION 2.01 CHANGE: DELAY CHANGED FROM 250NS to 2US.  Allow the
+	// ADC to stabilize for 2 microseconds before starting the final
+	// (dummy) conversion.  This delay is necessary to allow sufficient
+	// time between last conversion finished and the start of the dummy
+	// conversion.  Without this delay, the last conversion's data value
+	// is sometimes set to the previous conversion's data value.
+	for (n = 0; n < (2 * RPSCLK_PER_US); n++)
+		*pRPS++ = RPS_NOP;
+
+	// Start a dummy conversion to cause the data from the last
+	// conversion of interest to be shifted in.
+	*pRPS++ = RPS_LDREG | (P_GPIO >> 2);	// Begin ADC Start pulse.
+	*pRPS++ = GPIO_BASE | GPIO1_LO;
+	*pRPS++ = RPS_NOP;
+	// VERSION 2.03 CHANGE: STRETCH OUT ADC START PULSE.
+	*pRPS++ = RPS_LDREG | (P_GPIO >> 2);	// End ADC Start pulse.
+	*pRPS++ = GPIO_BASE | GPIO1_HI;
+
+	// Wait for the data from the last conversion of interest to arrive
+	// in FB BUFFER 1 register.
+	*pRPS++ = RPS_PAUSE | RPS_GPIO2;	// Wait for ADC done.
+
+	// Transfer final ADC data from FB BUFFER 1 register to DMA buffer.
+	*pRPS++ = RPS_STREG | (BUGFIX_STREG(P_FB_BUFFER1) >> 2);	//
+	*pRPS++ =
+		(uint32_t) devpriv->ANABuf.PhysicalBase +
+		(devpriv->AdcItems << 2);
+
+	// Indicate ADC scan loop is finished.
+	// *pRPS++= RPS_CLRSIGNAL | RPS_SIGADC ;  // Signal ReadADC() that scan is done.
+
+	//invoke interrupt
+	if (devpriv->ai_cmd_running == 1) {
+		DEBUG("ResetADC: insert irq in ADC RPS task\n");
+		*pRPS++ = RPS_IRQ;
+	}
+	// Restart RPS program at its beginning.
+	*pRPS++ = RPS_JUMP;	// Branch to start of RPS program.
+	*pRPS++ = (uint32_t) devpriv->RPSBuf.PhysicalBase;
+
+	// End of RPS program build
+	// ------------------------------------------------------------
+}
+
+/* TO COMPLETE, IF NECESSARY */
+static int s626_ai_insn_config(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+
+	return -EINVAL;
+}
+
+/* static int s626_ai_rinsn(comedi_device *dev,comedi_subdevice *s,comedi_insn *insn,lsampl_t *data) */
+/* { */
+/*   register uint8_t	i; */
+/*   register int32_t	*readaddr; */
+
+/*   DEBUG("as626_ai_rinsn: ai_rinsn enter \n");  */
+
+/*   // Trigger ADC scan loop start by setting RPS Signal 0. */
+/*   MC_ENABLE( P_MC2, MC2_ADC_RPS ); */
+
+/*   // Wait until ADC scan loop is finished (RPS Signal 0 reset). */
+/*   while ( MC_TEST( P_MC2, MC2_ADC_RPS ) ); */
+
+/*   // Init ptr to DMA buffer that holds new ADC data.  We skip the */
+/*   // first uint16_t in the buffer because it contains junk data from */
+/*   // the final ADC of the previous poll list scan. */
+/*   readaddr = (uint32_t *)devpriv->ANABuf.LogicalBase + 1; */
+
+/*   // Convert ADC data to 16-bit integer values and copy to application */
+/*   // buffer.	 */
+/*   for ( i = 0; i < devpriv->AdcItems; i++ ) { */
+/*     *data = s626_ai_reg_to_uint( *readaddr++ ); */
+/*     DEBUG("s626_ai_rinsn: data %d \n",*data); */
+/*     data++; */
+/*   } */
+
+/*   DEBUG("s626_ai_rinsn: ai_rinsn escape \n"); */
+/*   return i; */
+/* } */
+
+static int s626_ai_insn_read(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	uint16_t chan = CR_CHAN(insn->chanspec);
+	uint16_t range = CR_RANGE(insn->chanspec);
+	uint16_t AdcSpec = 0;
+	uint32_t GpioImage;
+	int n;
+
+/*   //interrupt call test  */
+/*   writel(IRQ_GPIO3,devpriv->base_addr+P_PSR); //Writing a logical 1 */
+/* 					     //into any of the RPS_PSR */
+/* 					     //bits causes the */
+/* 					     //corresponding interrupt */
+/* 					     //to be generated if */
+/* 					     //enabled */
+
+	DEBUG("s626_ai_insn_read: entering\n");
+
+	// Convert application's ADC specification into form
+	// appropriate for register programming.
+	if (range == 0)
+		AdcSpec = (chan << 8) | (GSEL_BIPOLAR5V);
+	else
+		AdcSpec = (chan << 8) | (GSEL_BIPOLAR10V);
+
+	// Switch ADC analog gain.
+	DEBIwrite(dev, LP_GSEL, AdcSpec);	// Set gain.
+
+	// Select ADC analog input channel.
+	DEBIwrite(dev, LP_ISEL, AdcSpec);	// Select channel.
+
+	for (n = 0; n < insn->n; n++) {
+
+		// Delay 10 microseconds for analog input settling.
+		comedi_udelay(10);
+
+		// Start ADC by pulsing GPIO1 low.
+		GpioImage = RR7146(P_GPIO);
+		// Assert ADC Start command
+		WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
+		//   and stretch it out.
+		WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
+		WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
+		// Negate ADC Start command.
+		WR7146(P_GPIO, GpioImage | GPIO1_HI);
+
+		// Wait for ADC to complete (GPIO2 is asserted high when
+		// ADC not busy) and for data from previous conversion to
+		// shift into FB BUFFER 1 register.
+
+		// Wait for ADC done.
+		while (!(RR7146(P_PSR) & PSR_GPIO2)) ;
+
+		// Fetch ADC data.
+		if (n != 0)
+			data[n - 1] = s626_ai_reg_to_uint(RR7146(P_FB_BUFFER1));
+
+		// Allow the ADC to stabilize for 4 microseconds before
+		// starting the next (final) conversion.  This delay is
+		// necessary to allow sufficient time between last
+		// conversion finished and the start of the next
+		// conversion.  Without this delay, the last conversion's
+		// data value is sometimes set to the previous
+		// conversion's data value.
+		comedi_udelay(4);
+	}
+
+	// Start a dummy conversion to cause the data from the
+	// previous conversion to be shifted in.
+	GpioImage = RR7146(P_GPIO);
+
+	//Assert ADC Start command
+	WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
+	//   and stretch it out.
+	WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
+	WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
+	// Negate ADC Start command.
+	WR7146(P_GPIO, GpioImage | GPIO1_HI);
+
+	// Wait for the data to arrive in FB BUFFER 1 register.
+
+	// Wait for ADC done.
+	while (!(RR7146(P_PSR) & PSR_GPIO2)) ;
+
+	// Fetch ADC data from audio interface's input shift
+	// register.
+
+	// Fetch ADC data.
+	if (n != 0)
+		data[n - 1] = s626_ai_reg_to_uint(RR7146(P_FB_BUFFER1));
+
+	DEBUG("s626_ai_insn_read: samples %d, data %d\n", n, data[n - 1]);
+
+	return n;
+}
+
+static int s626_ai_load_polllist(uint8_t * ppl, comedi_cmd * cmd)
+{
+
+	int n;
+
+	for (n = 0; n < cmd->chanlist_len; n++) {
+		if (CR_RANGE((cmd->chanlist)[n]) == 0)
+			ppl[n] = (CR_CHAN((cmd->chanlist)[n])) | (RANGE_5V);
+		else
+			ppl[n] = (CR_CHAN((cmd->chanlist)[n])) | (RANGE_10V);
+	}
+	ppl[n - 1] |= EOPL;
+
+	return n;
+}
+
+static int s626_ai_inttrig(comedi_device * dev, comedi_subdevice * s,
+	unsigned int trignum)
+{
+	if (trignum != 0)
+		return -EINVAL;
+
+	DEBUG("s626_ai_inttrig: trigger adc start...");
+
+	// Start executing the RPS program.
+	MC_ENABLE(P_MC1, MC1_ERPS1);
+
+	s->async->inttrig = NULL;
+
+	DEBUG(" done\n");
+
+	return 1;
+}
+
+/*  TO COMPLETE  */
+static int s626_ai_cmd(comedi_device * dev, comedi_subdevice * s)
+{
+
+	uint8_t ppl[16];
+	comedi_cmd *cmd = &s->async->cmd;
+	enc_private *k;
+	int tick;
+
+	DEBUG("s626_ai_cmd: entering command function\n");
+
+	if (devpriv->ai_cmd_running) {
+		printk("s626_ai_cmd: Another ai_cmd is running %d\n",
+			dev->minor);
+		return -EBUSY;
+	}
+	//disable interrupt
+	writel(0, devpriv->base_addr + P_IER);
+
+	//clear interrupt request
+	writel(IRQ_RPS1 | IRQ_GPIO3, devpriv->base_addr + P_ISR);
+
+	//clear any pending interrupt
+	s626_dio_clear_irq(dev);
+	//  s626_enc_clear_irq(dev);
+
+	//reset ai_cmd_running flag
+	devpriv->ai_cmd_running = 0;
+
+	// test if cmd is valid
+	if (cmd == NULL) {
+		DEBUG("s626_ai_cmd: NULL command\n");
+		return -EINVAL;
+	} else {
+		DEBUG("s626_ai_cmd: command recieved!!!\n");
+	}
+
+	if (dev->irq == 0) {
+		comedi_error(dev,
+			"s626_ai_cmd: cannot run command without an irq");
+		return -EIO;
+	}
+
+	s626_ai_load_polllist(ppl, cmd);
+	devpriv->ai_cmd_running = 1;
+	devpriv->ai_convert_count = 0;
+
+	switch (cmd->scan_begin_src) {
+	case TRIG_FOLLOW:
+		break;
+	case TRIG_TIMER:
+		// set a conter to generate adc trigger at scan_begin_arg interval
+		k = &encpriv[5];
+		tick = s626_ns_to_timer((int *)&cmd->scan_begin_arg,
+			cmd->flags & TRIG_ROUND_MASK);
+
+		//load timer value and enable interrupt
+		s626_timer_load(dev, k, tick);
+		k->SetEnable(dev, k, CLKENAB_ALWAYS);
+
+		DEBUG("s626_ai_cmd: scan trigger timer is set with value %d\n",
+			tick);
+
+		break;
+	case TRIG_EXT:
+		// set the digital line and interrupt for scan trigger
+		if (cmd->start_src != TRIG_EXT)
+			s626_dio_set_irq(dev, cmd->scan_begin_arg);
+
+		DEBUG("s626_ai_cmd: External scan trigger is set!!!\n");
+
+		break;
+	}
+
+	switch (cmd->convert_src) {
+	case TRIG_NOW:
+		break;
+	case TRIG_TIMER:
+		// set a conter to generate adc trigger at convert_arg interval
+		k = &encpriv[4];
+		tick = s626_ns_to_timer((int *)&cmd->convert_arg,
+			cmd->flags & TRIG_ROUND_MASK);
+
+		//load timer value and enable interrupt
+		s626_timer_load(dev, k, tick);
+		k->SetEnable(dev, k, CLKENAB_INDEX);
+
+		DEBUG("s626_ai_cmd: convert trigger timer is set with value %d\n", tick);
+		break;
+	case TRIG_EXT:
+		// set the digital line and interrupt for convert trigger
+		if (cmd->scan_begin_src != TRIG_EXT
+			&& cmd->start_src == TRIG_EXT)
+			s626_dio_set_irq(dev, cmd->convert_arg);
+
+		DEBUG("s626_ai_cmd: External convert trigger is set!!!\n");
+
+		break;
+	}
+
+	switch (cmd->stop_src) {
+	case TRIG_COUNT:
+		// data arrives as one packet
+		devpriv->ai_sample_count = cmd->stop_arg;
+		devpriv->ai_continous = 0;
+		break;
+	case TRIG_NONE:
+		// continous aquisition
+		devpriv->ai_continous = 1;
+		devpriv->ai_sample_count = 0;
+		break;
+	}
+
+	ResetADC(dev, ppl);
+
+	switch (cmd->start_src) {
+	case TRIG_NOW:
+		// Trigger ADC scan loop start by setting RPS Signal 0.
+		// MC_ENABLE( P_MC2, MC2_ADC_RPS );
+
+		// Start executing the RPS program.
+		MC_ENABLE(P_MC1, MC1_ERPS1);
+
+		DEBUG("s626_ai_cmd: ADC triggered\n");
+		s->async->inttrig = NULL;
+		break;
+	case TRIG_EXT:
+		//configure DIO channel for acquisition trigger
+		s626_dio_set_irq(dev, cmd->start_arg);
+
+		DEBUG("s626_ai_cmd: External start trigger is set!!!\n");
+
+		s->async->inttrig = NULL;
+		break;
+	case TRIG_INT:
+		s->async->inttrig = s626_ai_inttrig;
+		break;
+	}
+
+	//enable interrupt
+	writel(IRQ_GPIO3 | IRQ_RPS1, devpriv->base_addr + P_IER);
+
+	DEBUG("s626_ai_cmd: command function terminated\n");
+
+	return 0;
+}
+
+static int s626_ai_cmdtest(comedi_device * dev, comedi_subdevice * s,
+	comedi_cmd * cmd)
+{
+	int err = 0;
+	int tmp;
+
+	/* cmdtest tests a particular command to see if it is valid.  Using
+	 * the cmdtest ioctl, a user can create a valid cmd and then have it
+	 * executes by the cmd ioctl.
+	 *
+	 * cmdtest returns 1,2,3,4 or 0, depending on which tests the
+	 * command passes. */
+
+	/* step 1: make sure trigger sources are trivially valid */
+
+	tmp = cmd->start_src;
+	cmd->start_src &= TRIG_NOW | TRIG_INT | TRIG_EXT;
+	if (!cmd->start_src || tmp != cmd->start_src)
+		err++;
+
+	tmp = cmd->scan_begin_src;
+	cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT | TRIG_FOLLOW;
+	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
+		err++;
+
+	tmp = cmd->convert_src;
+	cmd->convert_src &= TRIG_TIMER | TRIG_EXT | TRIG_NOW;
+	if (!cmd->convert_src || tmp != cmd->convert_src)
+		err++;
+
+	tmp = cmd->scan_end_src;
+	cmd->scan_end_src &= TRIG_COUNT;
+	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
+		err++;
+
+	tmp = cmd->stop_src;
+	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
+	if (!cmd->stop_src || tmp != cmd->stop_src)
+		err++;
+
+	if (err)
+		return 1;
+
+	/* step 2: make sure trigger sources are unique and mutually
+	   compatible */
+
+	/* note that mutual compatiblity is not an issue here */
+	if (cmd->scan_begin_src != TRIG_TIMER &&
+		cmd->scan_begin_src != TRIG_EXT
+		&& cmd->scan_begin_src != TRIG_FOLLOW)
+		err++;
+	if (cmd->convert_src != TRIG_TIMER &&
+		cmd->convert_src != TRIG_EXT && cmd->convert_src != TRIG_NOW)
+		err++;
+	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
+		err++;
+
+	if (err)
+		return 2;
+
+	/* step 3: make sure arguments are trivially compatible */
+
+	if (cmd->start_src != TRIG_EXT && cmd->start_arg != 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+
+	if (cmd->start_src == TRIG_EXT && cmd->start_arg < 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+
+	if (cmd->start_src == TRIG_EXT && cmd->start_arg > 39) {
+		cmd->start_arg = 39;
+		err++;
+	}
+
+	if (cmd->scan_begin_src == TRIG_EXT && cmd->scan_begin_arg < 0) {
+		cmd->scan_begin_arg = 0;
+		err++;
+	}
+
+	if (cmd->scan_begin_src == TRIG_EXT && cmd->scan_begin_arg > 39) {
+		cmd->scan_begin_arg = 39;
+		err++;
+	}
+
+	if (cmd->convert_src == TRIG_EXT && cmd->convert_arg < 0) {
+		cmd->convert_arg = 0;
+		err++;
+	}
+
+	if (cmd->convert_src == TRIG_EXT && cmd->convert_arg > 39) {
+		cmd->convert_arg = 39;
+		err++;
+	}
+#define MAX_SPEED	200000	/* in nanoseconds */
+#define MIN_SPEED	2000000000	/* in nanoseconds */
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		if (cmd->scan_begin_arg < MAX_SPEED) {
+			cmd->scan_begin_arg = MAX_SPEED;
+			err++;
+		}
+		if (cmd->scan_begin_arg > MIN_SPEED) {
+			cmd->scan_begin_arg = MIN_SPEED;
+			err++;
+		}
+	} else {
+		/* external trigger */
+		/* should be level/edge, hi/lo specification here */
+		/* should specify multiple external triggers */
+/*     if(cmd->scan_begin_arg>9){ */
+/*       cmd->scan_begin_arg=9; */
+/*       err++; */
+/*     } */
+	}
+	if (cmd->convert_src == TRIG_TIMER) {
+		if (cmd->convert_arg < MAX_SPEED) {
+			cmd->convert_arg = MAX_SPEED;
+			err++;
+		}
+		if (cmd->convert_arg > MIN_SPEED) {
+			cmd->convert_arg = MIN_SPEED;
+			err++;
+		}
+	} else {
+		/* external trigger */
+		/* see above */
+/*     if(cmd->convert_arg>9){ */
+/*       cmd->convert_arg=9; */
+/*       err++; */
+/*     } */
+	}
+
+	if (cmd->scan_end_arg != cmd->chanlist_len) {
+		cmd->scan_end_arg = cmd->chanlist_len;
+		err++;
+	}
+	if (cmd->stop_src == TRIG_COUNT) {
+		if (cmd->stop_arg > 0x00ffffff) {
+			cmd->stop_arg = 0x00ffffff;
+			err++;
+		}
+	} else {
+		/* TRIG_NONE */
+		if (cmd->stop_arg != 0) {
+			cmd->stop_arg = 0;
+			err++;
+		}
+	}
+
+	if (err)
+		return 3;
+
+	/* 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++;
+	}
+	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++;
+		}
+	}
+
+	if (err)
+		return 4;
+
+	return 0;
+}
+
+static int s626_ai_cancel(comedi_device * dev, comedi_subdevice * s)
+{
+	// Stop RPS program in case it is currently running.
+	MC_DISABLE(P_MC1, MC1_ERPS1);
+
+	//disable master interrupt
+	writel(0, devpriv->base_addr + P_IER);
+
+	devpriv->ai_cmd_running = 0;
+
+	return 0;
+}
+
+/* This function doesn't require a particular form, this is just what
+ * happens to be used in some of the drivers.  It should convert ns
+ * nanoseconds to a counter value suitable for programming the device.
+ * 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)
+{
+	int divider, base;
+
+	base = 500;		//2MHz internal clock
+
+	switch (round_mode) {
+	case TRIG_ROUND_NEAREST:
+	default:
+		divider = (*nanosec + base / 2) / base;
+		break;
+	case TRIG_ROUND_DOWN:
+		divider = (*nanosec) / base;
+		break;
+	case TRIG_ROUND_UP:
+		divider = (*nanosec + base - 1) / base;
+		break;
+	}
+
+	*nanosec = base * divider;
+	return divider - 1;
+}
+
+static int s626_ao_winsn(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+
+	int i;
+	uint16_t chan = CR_CHAN(insn->chanspec);
+	int16_t dacdata;
+
+	for (i = 0; i < insn->n; i++) {
+		dacdata = (int16_t) data[i];
+		devpriv->ao_readback[CR_CHAN(insn->chanspec)] = data[i];
+		dacdata -= (0x1fff);
+
+		SetDAC(dev, chan, dacdata);
+	}
+
+	return i;
+}
+
+static int s626_ao_rinsn(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	int i;
+
+	for (i = 0; i < insn->n; i++) {
+		data[i] = devpriv->ao_readback[CR_CHAN(insn->chanspec)];
+	}
+
+	return i;
+}
+
+/////////////////////////////////////////////////////////////////////
+///////////////  DIGITAL I/O FUNCTIONS  /////////////////////////////
+/////////////////////////////////////////////////////////////////////
+// All DIO functions address a group of DIO channels by means of
+// "group" argument.  group may be 0, 1 or 2, which correspond to DIO
+// ports A, B and C, respectively.
+/////////////////////////////////////////////////////////////////////
+
+static void s626_dio_init(comedi_device * dev)
+{
+	uint16_t group;
+	comedi_subdevice *s;
+
+	// Prepare to treat writes to WRCapSel as capture disables.
+	DEBIwrite(dev, LP_MISC1, MISC1_NOEDCAP);
+
+	// For each group of sixteen channels ...
+	for (group = 0; group < S626_DIO_BANKS; group++) {
+		s = dev->subdevices + 2 + group;
+		DEBIwrite(dev, diopriv->WRIntSel, 0);	// Disable all interrupts.
+		DEBIwrite(dev, diopriv->WRCapSel, 0xFFFF);	// Disable all event
+		// captures.
+		DEBIwrite(dev, diopriv->WREdgSel, 0);	// Init all DIOs to
+		// default edge
+		// polarity.
+		DEBIwrite(dev, diopriv->WRDOut, 0);	// Program all outputs
+		// to inactive state.
+	}
+	DEBUG("s626_dio_init: DIO initialized \n");
+}
+
+/* DIO devices are slightly special.  Although it is possible to
+ * implement the insn_read/insn_write interface, it is much more
+ * useful to applications if you implement the insn_bits interface.
+ * This allows packed reading/writing of the DIO channels.  The comedi
+ * core can convert between insn_bits and insn_read/write */
+
+static int s626_dio_insn_bits(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+
+	/* Length of data must be 2 (mask and new data, see below) */
+	if (insn->n == 0) {
+		return 0;
+	}
+	if (insn->n != 2) {
+		printk("comedi%d: s626: s626_dio_insn_bits(): Invalid instruction length\n", dev->minor);
+		return -EINVAL;
+	}
+
+	/*
+	 * The insn data consists of a mask in data[0] and the new data in
+	 * data[1]. The mask defines which bits we are concerning about.
+	 * The new data must be anded with the mask.  Each channel
+	 * corresponds to a bit.
+	 */
+	if (data[0]) {
+		/* Check if requested ports are configured for output */
+		if ((s->io_bits & data[0]) != data[0])
+			return -EIO;
+
+		s->state &= ~data[0];
+		s->state |= data[0] & data[1];
+
+		/* Write out the new digital output lines */
+
+		DEBIwrite(dev, diopriv->WRDOut, s->state);
+	}
+	data[1] = DEBIread(dev, diopriv->RDDIn);
+
+	return 2;
+}
+
+static int s626_dio_insn_config(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+
+	switch (data[0]) {
+	case INSN_CONFIG_DIO_QUERY:
+		data[1] =
+			(s->io_bits & (1 << CR_CHAN(insn->
+					chanspec))) ? COMEDI_OUTPUT :
+			COMEDI_INPUT;
+		return insn->n;
+		break;
+	case COMEDI_INPUT:
+		s->io_bits &= ~(1 << CR_CHAN(insn->chanspec));
+		break;
+	case COMEDI_OUTPUT:
+		s->io_bits |= 1 << CR_CHAN(insn->chanspec);
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+	DEBIwrite(dev, diopriv->WRDOut, s->io_bits);
+
+	return 1;
+}
+
+static int s626_dio_set_irq(comedi_device * dev, unsigned int chan)
+{
+	unsigned int group;
+	unsigned int bitmask;
+	unsigned int status;
+
+	//select dio bank
+	group = chan / 16;
+	bitmask = 1 << (chan - (16 * group));
+	DEBUG("s626_dio_set_irq: enable interrupt on dio channel %d group %d\n",
+		chan - (16 * group), group);
+
+	//set channel to capture positive edge
+	status = DEBIread(dev,
+		((dio_private *) (dev->subdevices + 2 +
+				group)->private)->RDEdgSel);
+	DEBIwrite(dev,
+		((dio_private *) (dev->subdevices + 2 +
+				group)->private)->WREdgSel, bitmask | status);
+
+	//enable interrupt on selected channel
+	status = DEBIread(dev,
+		((dio_private *) (dev->subdevices + 2 +
+				group)->private)->RDIntSel);
+	DEBIwrite(dev,
+		((dio_private *) (dev->subdevices + 2 +
+				group)->private)->WRIntSel, bitmask | status);
+
+	//enable edge capture write command
+	DEBIwrite(dev, LP_MISC1, MISC1_EDCAP);
+
+	//enable edge capture on selected channel
+	status = DEBIread(dev,
+		((dio_private *) (dev->subdevices + 2 +
+				group)->private)->RDCapSel);
+	DEBIwrite(dev,
+		((dio_private *) (dev->subdevices + 2 +
+				group)->private)->WRCapSel, bitmask | status);
+
+	return 0;
+}
+
+static int s626_dio_reset_irq(comedi_device * dev, unsigned int group,
+	unsigned int mask)
+{
+	DEBUG("s626_dio_reset_irq: disable  interrupt on dio channel %d group %d\n", mask, group);
+
+	//disable edge capture write command
+	DEBIwrite(dev, LP_MISC1, MISC1_NOEDCAP);
+
+	//enable edge capture on selected channel
+	DEBIwrite(dev,
+		((dio_private *) (dev->subdevices + 2 +
+				group)->private)->WRCapSel, mask);
+
+	return 0;
+}
+
+static int s626_dio_clear_irq(comedi_device * dev)
+{
+	unsigned int group;
+
+	//disable edge capture write command
+	DEBIwrite(dev, LP_MISC1, MISC1_NOEDCAP);
+
+	for (group = 0; group < S626_DIO_BANKS; group++) {
+		//clear pending events and interrupt
+		DEBIwrite(dev,
+			((dio_private *) (dev->subdevices + 2 +
+					group)->private)->WRCapSel, 0xffff);
+	}
+
+	return 0;
+}
+
+/* Now this function initializes the value of the counter (data[0])
+   and set the subdevice. To complete with trigger and interrupt
+   configuration */
+static int s626_enc_insn_config(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	uint16_t Setup = (LOADSRC_INDX << BF_LOADSRC) |	// Preload upon
+		// index.
+		(INDXSRC_SOFT << BF_INDXSRC) |	// Disable hardware index.
+		(CLKSRC_COUNTER << BF_CLKSRC) |	// Operating mode is Counter.
+		(CLKPOL_POS << BF_CLKPOL) |	// Active high clock.
+		//( CNTDIR_UP << BF_CLKPOL ) |      // Count direction is Down.
+		(CLKMULT_1X << BF_CLKMULT) |	// Clock multiplier is 1x.
+		(CLKENAB_INDEX << BF_CLKENAB);
+	/*   uint16_t DisableIntSrc=TRUE; */
+	// uint32_t Preloadvalue;              //Counter initial value
+	uint16_t valueSrclatch = LATCHSRC_AB_READ;
+	uint16_t enab = CLKENAB_ALWAYS;
+	enc_private *k = &encpriv[CR_CHAN(insn->chanspec)];
+
+	DEBUG("s626_enc_insn_config: encoder config\n");
+
+	//  (data==NULL) ? (Preloadvalue=0) : (Preloadvalue=data[0]);
+
+	k->SetMode(dev, k, Setup, TRUE);
+	Preload(dev, k, *(insn->data));
+	k->PulseIndex(dev, k);
+	SetLatchSource(dev, k, valueSrclatch);
+	k->SetEnable(dev, k, (uint16_t) (enab != 0));
+
+	return insn->n;
+}
+
+static int s626_enc_insn_read(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+
+	int n;
+	enc_private *k = &encpriv[CR_CHAN(insn->chanspec)];
+
+	DEBUG("s626_enc_insn_read: encoder read channel %d \n",
+		CR_CHAN(insn->chanspec));
+
+	for (n = 0; n < insn->n; n++)
+		data[n] = ReadLatch(dev, k);
+
+	DEBUG("s626_enc_insn_read: encoder sample %d\n", data[n]);
+
+	return n;
+}
+
+static int s626_enc_insn_write(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+
+	enc_private *k = &encpriv[CR_CHAN(insn->chanspec)];
+
+	DEBUG("s626_enc_insn_write: encoder write channel %d \n",
+		CR_CHAN(insn->chanspec));
+
+	// Set the preload register
+	Preload(dev, k, data[0]);
+
+	// Software index pulse forces the preload register to load
+	// into the counter
+	k->SetLoadTrig(dev, k, 0);
+	k->PulseIndex(dev, k);
+	k->SetLoadTrig(dev, k, 2);
+
+	DEBUG("s626_enc_insn_write: End encoder write\n");
+
+	return 1;
+}
+
+static void s626_timer_load(comedi_device * dev, enc_private * k, int tick)
+{
+	uint16_t Setup = (LOADSRC_INDX << BF_LOADSRC) |	// Preload upon
+		// index.
+		(INDXSRC_SOFT << BF_INDXSRC) |	// Disable hardware index.
+		(CLKSRC_TIMER << BF_CLKSRC) |	// Operating mode is Timer.
+		(CLKPOL_POS << BF_CLKPOL) |	// Active high clock.
+		(CNTDIR_DOWN << BF_CLKPOL) |	// Count direction is Down.
+		(CLKMULT_1X << BF_CLKMULT) |	// Clock multiplier is 1x.
+		(CLKENAB_INDEX << BF_CLKENAB);
+	uint16_t valueSrclatch = LATCHSRC_A_INDXA;
+	//  uint16_t enab=CLKENAB_ALWAYS;
+
+	k->SetMode(dev, k, Setup, FALSE);
+
+	// Set the preload register
+	Preload(dev, k, tick);
+
+	// Software index pulse forces the preload register to load
+	// into the counter
+	k->SetLoadTrig(dev, k, 0);
+	k->PulseIndex(dev, k);
+
+	//set reload on counter overflow
+	k->SetLoadTrig(dev, k, 1);
+
+	//set interrupt on overflow
+	k->SetIntSrc(dev, k, INTSRC_OVER);
+
+	SetLatchSource(dev, k, valueSrclatch);
+	//  k->SetEnable(dev,k,(uint16_t)(enab != 0));
+}
+
+///////////////////////////////////////////////////////////////////////
+/////////////////////  DAC FUNCTIONS /////////////////////////////////
+///////////////////////////////////////////////////////////////////////
+
+// Slot 0 base settings.
+#define VECT0	( XSD2 | RSD3 | SIB_A2 )	// Slot 0 always shifts in
+					 // 0xFF and store it to
+					 // FB_BUFFER2.
+
+// TrimDac LogicalChan-to-PhysicalChan mapping table.
+static uint8_t trimchan[] = { 10, 9, 8, 3, 2, 7, 6, 1, 0, 5, 4 };
+
+// TrimDac LogicalChan-to-EepromAdrs mapping table.
+static uint8_t trimadrs[] =
+	{ 0x40, 0x41, 0x42, 0x50, 0x51, 0x52, 0x53, 0x60, 0x61, 0x62, 0x63 };
+
+static void LoadTrimDACs(comedi_device * dev)
+{
+	register uint8_t i;
+
+	// Copy TrimDac setpoint values from EEPROM to TrimDacs.
+	for (i = 0; i < (sizeof(trimchan) / sizeof(trimchan[0])); i++)
+		WriteTrimDAC(dev, i, I2Cread(dev, trimadrs[i]));
+}
+
+static void WriteTrimDAC(comedi_device * dev, uint8_t LogicalChan,
+	uint8_t DacData)
+{
+	uint32_t chan;
+
+	// Save the new setpoint in case the application needs to read it back later.
+	devpriv->TrimSetpoint[LogicalChan] = (uint8_t) DacData;
+
+	// Map logical channel number to physical channel number.
+	chan = (uint32_t) trimchan[LogicalChan];
+
+	// Set up TSL2 records for TrimDac write operation.  All slots shift
+	// 0xFF in from pulled-up SD3 so that the end of the slot sequence
+	// can be detected.
+	SETVECT(2, XSD2 | XFIFO_1 | WS3);	// Slot 2: Send high uint8_t
+	// to target TrimDac.
+	SETVECT(3, XSD2 | XFIFO_0 | WS3);	// Slot 3: Send low uint8_t to
+	// target TrimDac.
+	SETVECT(4, XSD2 | XFIFO_3 | WS1);	// Slot 4: Send NOP high
+	// uint8_t to DAC0 to keep
+	// clock running.
+	SETVECT(5, XSD2 | XFIFO_2 | WS1 | EOS);	// Slot 5: Send NOP low
+	// uint8_t to DAC0.
+
+	// Construct and transmit target DAC's serial packet: ( 0000 AAAA
+	// ),( DDDD DDDD ),( 0x00 ),( 0x00 ) where A<3:0> is the DAC
+	// channel's address, and D<7:0> is the DAC setpoint.  Append a WORD
+	// value (that writes a channel 0 NOP command to a non-existent main
+	// DAC channel) that serves to keep the clock running after the
+	// packet has been sent to the target DAC.
+
+	SendDAC(dev, ((uint32_t) chan << 8)	// Address the DAC channel
+		// within the trimdac device.
+		| (uint32_t) DacData);	// Include DAC setpoint data.
+}
+
+/////////////////////////////////////////////////////////////////////////
+////////////////  EEPROM ACCESS FUNCTIONS  //////////////////////////////
+/////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////
+// Read uint8_t from EEPROM.
+
+static uint8_t I2Cread(comedi_device * dev, uint8_t addr)
+{
+	uint8_t rtnval;
+
+	// Send EEPROM target address.
+	if (I2Chandshake(dev, I2C_B2(I2C_ATTRSTART, I2CW)	// Byte2 = I2C
+			// command:
+			// write to
+			// I2C EEPROM
+			// device.
+			| I2C_B1(I2C_ATTRSTOP, addr)	// Byte1 = EEPROM
+			// internal target
+			// address.
+			| I2C_B0(I2C_ATTRNOP, 0)))	// Byte0 = Not
+		// sent.
+	{
+		// Abort function and declare error if handshake failed.
+		DEBUG("I2Cread: error handshake I2Cread  a\n");
+		return 0;
+	}
+	// Execute EEPROM read.
+	if (I2Chandshake(dev, I2C_B2(I2C_ATTRSTART, I2CR)	// Byte2 = I2C
+			// command: read
+			// from I2C EEPROM
+			// device.
+			| I2C_B1(I2C_ATTRSTOP, 0)	// Byte1 receives
+			// uint8_t from
+			// EEPROM.
+			| I2C_B0(I2C_ATTRNOP, 0)))	// Byte0 = Not
+		// sent.
+	{
+		// Abort function and declare error if handshake failed.
+		DEBUG("I2Cread: error handshake I2Cread b\n");
+		return 0;
+	}
+	// Return copy of EEPROM value.
+	rtnval = (uint8_t) (RR7146(P_I2CCTRL) >> 16);
+	return rtnval;
+}
+
+static uint32_t I2Chandshake(comedi_device * dev, uint32_t val)
+{
+	// Write I2C command to I2C Transfer Control shadow register.
+	WR7146(P_I2CCTRL, val);
+
+	// Upload I2C shadow registers into working registers and wait for
+	// upload confirmation.
+
+	MC_ENABLE(P_MC2, MC2_UPLD_IIC);
+	while (!MC_TEST(P_MC2, MC2_UPLD_IIC)) ;
+
+	// Wait until I2C bus transfer is finished or an error occurs.
+	while ((RR7146(P_I2CCTRL) & (I2C_BUSY | I2C_ERR)) == I2C_BUSY) ;
+
+	// Return non-zero if I2C error occured.
+	return RR7146(P_I2CCTRL) & I2C_ERR;
+
+}
+
+// Private helper function: Write setpoint to an application DAC channel.
+
+static void SetDAC(comedi_device * dev, uint16_t chan, short dacdata)
+{
+	register uint16_t signmask;
+	register uint32_t WSImage;
+
+	// Adjust DAC data polarity and set up Polarity Control Register
+	// image.
+	signmask = 1 << chan;
+	if (dacdata < 0) {
+		dacdata = -dacdata;
+		devpriv->Dacpol |= signmask;
+	} else
+		devpriv->Dacpol &= ~signmask;
+
+	// Limit DAC setpoint value to valid range.
+	if ((uint16_t) dacdata > 0x1FFF)
+		dacdata = 0x1FFF;
+
+	// Set up TSL2 records (aka "vectors") for DAC update.  Vectors V2
+	// and V3 transmit the setpoint to the target DAC.  V4 and V5 send
+	// data to a non-existent TrimDac channel just to keep the clock
+	// running after sending data to the target DAC.  This is necessary
+	// to eliminate the clock glitch that would otherwise occur at the
+	// end of the target DAC's serial data stream.  When the sequence
+	// restarts at V0 (after executing V5), the gate array automatically
+	// disables gating for the DAC clock and all DAC chip selects.
+	WSImage = (chan & 2) ? WS1 : WS2;	// Choose DAC chip select to
+	// be asserted.
+	SETVECT(2, XSD2 | XFIFO_1 | WSImage);	// Slot 2: Transmit high
+	// data byte to target DAC.
+	SETVECT(3, XSD2 | XFIFO_0 | WSImage);	// Slot 3: Transmit low data
+	// byte to target DAC.
+	SETVECT(4, XSD2 | XFIFO_3 | WS3);	// Slot 4: Transmit to
+	// non-existent TrimDac
+	// channel to keep clock
+	SETVECT(5, XSD2 | XFIFO_2 | WS3 | EOS);	// Slot 5: running after
+	// writing target DAC's
+	// low data byte.
+
+	// Construct and transmit target DAC's serial packet: ( A10D DDDD
+	// ),( DDDD DDDD ),( 0x0F ),( 0x00 ) where A is chan<0>, and D<12:0>
+	// is the DAC setpoint.  Append a WORD value (that writes to a
+	// non-existent TrimDac channel) that serves to keep the clock
+	// running after the packet has been sent to the target DAC.
+	SendDAC(dev, 0x0F000000	//Continue clock after target DAC
+		//data (write to non-existent
+		//trimdac).
+		| 0x00004000	// Address the two main dual-DAC
+		// devices (TSL's chip select enables
+		// target device).
+		| ((uint32_t) (chan & 1) << 15)	// Address the DAC
+		// channel within the
+		// device.
+		| (uint32_t) dacdata);	// Include DAC setpoint data.
+
+}
+
+////////////////////////////////////////////////////////
+// Private helper function: Transmit serial data to DAC via Audio
+// channel 2.  Assumes: (1) TSL2 slot records initialized, and (2)
+// Dacpol contains valid target image.
+
+static void SendDAC(comedi_device * dev, uint32_t val)
+{
+
+	// START THE SERIAL CLOCK RUNNING -------------
+
+	// Assert DAC polarity control and enable gating of DAC serial clock
+	// and audio bit stream signals.  At this point in time we must be
+	// assured of being in time slot 0.  If we are not in slot 0, the
+	// serial clock and audio stream signals will be disabled; this is
+	// because the following DEBIwrite statement (which enables signals
+	// to be passed through the gate array) would execute before the
+	// trailing edge of WS1/WS3 (which turns off the signals), thus
+	// causing the signals to be inactive during the DAC write.
+	DEBIwrite(dev, LP_DACPOL, devpriv->Dacpol);
+
+	// TRANSFER OUTPUT DWORD VALUE INTO A2'S OUTPUT FIFO ----------------
+
+	// Copy DAC setpoint value to DAC's output DMA buffer.
+
+	//WR7146( (uint32_t)devpriv->pDacWBuf, val );
+	*devpriv->pDacWBuf = val;
+
+	// enab the output DMA transfer.  This will cause the DMAC to copy
+	// the DAC's data value to A2's output FIFO.  The DMA transfer will
+	// then immediately terminate because the protection address is
+	// reached upon transfer of the first DWORD value.
+	MC_ENABLE(P_MC1, MC1_A2OUT);
+
+	// While the DMA transfer is executing ...
+
+	// Reset Audio2 output FIFO's underflow flag (along with any other
+	// FIFO underflow/overflow flags).  When set, this flag will
+	// indicate that we have emerged from slot 0.
+	WR7146(P_ISR, ISR_AFOU);
+
+	// Wait for the DMA transfer to finish so that there will be data
+	// available in the FIFO when time slot 1 tries to transfer a DWORD
+	// from the FIFO to the output buffer register.  We test for DMA
+	// Done by polling the DMAC enable flag; this flag is automatically
+	// cleared when the transfer has finished.
+	while ((RR7146(P_MC1) & MC1_A2OUT) != 0) ;
+
+	// START THE OUTPUT STREAM TO THE TARGET DAC --------------------
+
+	// FIFO data is now available, so we enable execution of time slots
+	// 1 and higher by clearing the EOS flag in slot 0.  Note that SD3
+	// will be shifted in and stored in FB_BUFFER2 for end-of-slot-list
+	// detection.
+	SETVECT(0, XSD2 | RSD3 | SIB_A2);
+
+	// Wait for slot 1 to execute to ensure that the Packet will be
+	// transmitted.  This is detected by polling the Audio2 output FIFO
+	// underflow flag, which will be set when slot 1 execution has
+	// finished transferring the DAC's data DWORD from the output FIFO
+	// to the output buffer register.
+	while ((RR7146(P_SSR) & SSR_AF2_OUT) == 0) ;
+
+	// Set up to trap execution at slot 0 when the TSL sequencer cycles
+	// back to slot 0 after executing the EOS in slot 5.  Also,
+	// simultaneously shift out and in the 0x00 that is ALWAYS the value
+	// stored in the last byte to be shifted out of the FIFO's DWORD
+	// buffer register.
+	SETVECT(0, XSD2 | XFIFO_2 | RSD2 | SIB_A2 | EOS);
+
+	// WAIT FOR THE TRANSACTION TO FINISH -----------------------
+
+	// Wait for the TSL to finish executing all time slots before
+	// exiting this function.  We must do this so that the next DAC
+	// write doesn't start, thereby enabling clock/chip select signals:
+	// 1. Before the TSL sequence cycles back to slot 0, which disables
+	// the clock/cs signal gating and traps slot // list execution.  If
+	// we have not yet finished slot 5 then the clock/cs signals are
+	// still gated and we have // not finished transmitting the stream.
+	// 2. While slots 2-5 are executing due to a late slot 0 trap.  In
+	// this case, the slot sequence is currently // repeating, but with
+	// clock/cs signals disabled.  We must wait for slot 0 to trap
+	// execution before setting // up the next DAC setpoint DMA transfer
+	// and enabling the clock/cs signals.  To detect the end of slot 5,
+	// we test for the FB_BUFFER2 MSB contents to be equal to 0xFF.  If
+	// the TSL has not yet finished executing slot 5 ...
+	if ((RR7146(P_FB_BUFFER2) & 0xFF000000) != 0) {
+		// The trap was set on time and we are still executing somewhere
+		// in slots 2-5, so we now wait for slot 0 to execute and trap
+		// TSL execution.  This is detected when FB_BUFFER2 MSB changes
+		// from 0xFF to 0x00, which slot 0 causes to happen by shifting
+		// out/in on SD2 the 0x00 that is always referenced by slot 5.
+		while ((RR7146(P_FB_BUFFER2) & 0xFF000000) != 0) ;
+	}
+	// Either (1) we were too late setting the slot 0 trap; the TSL
+	// sequencer restarted slot 0 before we could set the EOS trap flag,
+	// or (2) we were not late and execution is now trapped at slot 0.
+	// In either case, we must now change slot 0 so that it will store
+	// value 0xFF (instead of 0x00) to FB_BUFFER2 next time it executes.
+	// In order to do this, we reprogram slot 0 so that it will shift in
+	// SD3, which is driven only by a pull-up resistor.
+	SETVECT(0, RSD3 | SIB_A2 | EOS);
+
+	// Wait for slot 0 to execute, at which time the TSL is setup for
+	// the next DAC write.  This is detected when FB_BUFFER2 MSB changes
+	// from 0x00 to 0xFF.
+	while ((RR7146(P_FB_BUFFER2) & 0xFF000000) == 0) ;
+}
+
+static void WriteMISC2(comedi_device * dev, uint16_t NewImage)
+{
+	DEBIwrite(dev, LP_MISC1, MISC1_WENABLE);	// enab writes to
+	// MISC2 register.
+	DEBIwrite(dev, LP_WRMISC2, NewImage);	// Write new image to MISC2.
+	DEBIwrite(dev, LP_MISC1, MISC1_WDISABLE);	// Disable writes to MISC2.
+}
+
+/////////////////////////////////////////////////////////////////////
+// Initialize the DEBI interface for all transfers.
+
+static uint16_t DEBIread(comedi_device * dev, uint16_t addr)
+{
+	uint16_t retval;
+
+	// Set up DEBI control register value in shadow RAM.
+	WR7146(P_DEBICMD, DEBI_CMD_RDWORD | addr);
+
+	// Execute the DEBI transfer.
+	DEBItransfer(dev);
+
+	// Fetch target register value.
+	retval = (uint16_t) RR7146(P_DEBIAD);
+
+	// Return register value.
+	return retval;
+}
+
+// Execute a DEBI transfer.  This must be called from within a
+// critical section.
+static void DEBItransfer(comedi_device * dev)
+{
+	// Initiate upload of shadow RAM to DEBI control register.
+	MC_ENABLE(P_MC2, MC2_UPLD_DEBI);
+
+	// Wait for completion of upload from shadow RAM to DEBI control
+	// register.
+	while (!MC_TEST(P_MC2, MC2_UPLD_DEBI)) ;
+
+	// Wait until DEBI transfer is done.
+	while (RR7146(P_PSR) & PSR_DEBI_S) ;
+}
+
+// Write a value to a gate array register.
+static void DEBIwrite(comedi_device * dev, uint16_t addr, uint16_t wdata)
+{
+
+	// Set up DEBI control register value in shadow RAM.
+	WR7146(P_DEBICMD, DEBI_CMD_WRWORD | addr);
+	WR7146(P_DEBIAD, wdata);
+
+	// Execute the DEBI transfer.
+	DEBItransfer(dev);
+}
+
+/////////////////////////////////////////////////////////////////////////////
+// Replace the specified bits in a gate array register.  Imports: mask
+// specifies bits that are to be preserved, wdata is new value to be
+// or'd with the masked original.
+static void DEBIreplace(comedi_device * dev, uint16_t addr, uint16_t mask,
+	uint16_t wdata)
+{
+
+	// Copy target gate array register into P_DEBIAD register.
+	WR7146(P_DEBICMD, DEBI_CMD_RDWORD | addr);	// Set up DEBI control
+	// reg value in shadow
+	// RAM.
+	DEBItransfer(dev);	// Execute the DEBI
+	// Read transfer.
+
+	// Write back the modified image.
+	WR7146(P_DEBICMD, DEBI_CMD_WRWORD | addr);	// Set up DEBI control
+	// reg value in shadow
+	// RAM.
+
+	WR7146(P_DEBIAD, wdata | ((uint16_t) RR7146(P_DEBIAD) & mask));	// Modify the register image.
+	DEBItransfer(dev);	// Execute the DEBI Write transfer.
+}
+
+static void CloseDMAB(comedi_device * dev, DMABUF * pdma, size_t bsize)
+{
+	void *vbptr;
+	dma_addr_t vpptr;
+
+	DEBUG("CloseDMAB: Entering S626DRV_CloseDMAB():\n");
+	if (pdma == NULL)
+		return;
+	//find the matching allocation from the board struct
+
+	vbptr = pdma->LogicalBase;
+	vpptr = pdma->PhysicalBase;
+	if (vbptr) {
+		pci_free_consistent(devpriv->pdev, bsize, vbptr, vpptr);
+		pdma->LogicalBase = 0;
+		pdma->PhysicalBase = 0;
+
+		DEBUG("CloseDMAB(): Logical=%p, bsize=%d, Physical=0x%x\n",
+			vbptr, bsize, (uint32_t) vpptr);
+	}
+}
+
+////////////////////////////////////////////////////////////////////////
+/////////////////  COUNTER FUNCTIONS  //////////////////////////////////
+////////////////////////////////////////////////////////////////////////
+// All counter functions address a specific counter by means of the
+// "Counter" argument, which is a logical counter number.  The Counter
+// argument may have any of the following legal values: 0=0A, 1=1A,
+// 2=2A, 3=0B, 4=1B, 5=2B.
+////////////////////////////////////////////////////////////////////////
+
+// Forward declarations for functions that are common to both A and B
+// counters:
+
+/////////////////////////////////////////////////////////////////////
+//////////////////// PRIVATE COUNTER FUNCTIONS  /////////////////////
+/////////////////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////////////
+// Read a counter's output latch.
+
+static uint32_t ReadLatch(comedi_device * dev, enc_private * k)
+{
+	register uint32_t value;
+	//DEBUG FIXME DEBUG("ReadLatch: Read Latch enter\n");
+
+	// Latch counts and fetch LSW of latched counts value.
+	value = (uint32_t) DEBIread(dev, k->MyLatchLsw);
+
+	// Fetch MSW of latched counts and combine with LSW.
+	value |= ((uint32_t) DEBIread(dev, k->MyLatchLsw + 2) << 16);
+
+	// DEBUG FIXME DEBUG("ReadLatch: Read Latch exit\n");
+
+	// Return latched counts.
+	return value;
+}
+
+///////////////////////////////////////////////////////////////////
+// Reset a counter's index and overflow event capture flags.
+
+static void ResetCapFlags_A(comedi_device * dev, enc_private * k)
+{
+	DEBIreplace(dev, k->MyCRB, (uint16_t) (~CRBMSK_INTCTRL),
+		CRBMSK_INTRESETCMD | CRBMSK_INTRESET_A);
+}
+
+static void ResetCapFlags_B(comedi_device * dev, enc_private * k)
+{
+	DEBIreplace(dev, k->MyCRB, (uint16_t) (~CRBMSK_INTCTRL),
+		CRBMSK_INTRESETCMD | CRBMSK_INTRESET_B);
+}
+
+/////////////////////////////////////////////////////////////////////////
+// Return counter setup in a format (COUNTER_SETUP) that is consistent
+// for both A and B counters.
+
+static uint16_t GetMode_A(comedi_device * dev, enc_private * k)
+{
+	register uint16_t cra;
+	register uint16_t crb;
+	register uint16_t setup;
+
+	// Fetch CRA and CRB register images.
+	cra = DEBIread(dev, k->MyCRA);
+	crb = DEBIread(dev, k->MyCRB);
+
+	// Populate the standardized counter setup bit fields.  Note:
+	// IndexSrc is restricted to ENC_X or IndxPol.
+	setup = ((cra & STDMSK_LOADSRC)	// LoadSrc  = LoadSrcA.
+		| ((crb << (STDBIT_LATCHSRC - CRBBIT_LATCHSRC)) & STDMSK_LATCHSRC)	// LatchSrc = LatchSrcA.
+		| ((cra << (STDBIT_INTSRC - CRABIT_INTSRC_A)) & STDMSK_INTSRC)	// IntSrc   = IntSrcA.
+		| ((cra << (STDBIT_INDXSRC - (CRABIT_INDXSRC_A + 1))) & STDMSK_INDXSRC)	// IndxSrc  = IndxSrcA<1>.
+		| ((cra >> (CRABIT_INDXPOL_A - STDBIT_INDXPOL)) & STDMSK_INDXPOL)	// IndxPol  = IndxPolA.
+		| ((crb >> (CRBBIT_CLKENAB_A - STDBIT_CLKENAB)) & STDMSK_CLKENAB));	// ClkEnab  = ClkEnabA.
+
+	// Adjust mode-dependent parameters.
+	if (cra & (2 << CRABIT_CLKSRC_A))	// If Timer mode (ClkSrcA<1> == 1):
+		setup |= ((CLKSRC_TIMER << STDBIT_CLKSRC)	//   Indicate Timer mode.
+			| ((cra << (STDBIT_CLKPOL - CRABIT_CLKSRC_A)) & STDMSK_CLKPOL)	//   Set ClkPol to indicate count direction (ClkSrcA<0>).
+			| (MULT_X1 << STDBIT_CLKMULT));	//   ClkMult must be 1x in Timer mode.
+
+	else			// If Counter mode (ClkSrcA<1> == 0):
+		setup |= ((CLKSRC_COUNTER << STDBIT_CLKSRC)	//   Indicate Counter mode.
+			| ((cra >> (CRABIT_CLKPOL_A - STDBIT_CLKPOL)) & STDMSK_CLKPOL)	//   Pass through ClkPol.
+			| (((cra & CRAMSK_CLKMULT_A) == (MULT_X0 << CRABIT_CLKMULT_A)) ?	//   Force ClkMult to 1x if not legal, else pass through.
+				(MULT_X1 << STDBIT_CLKMULT) :
+				((cra >> (CRABIT_CLKMULT_A -
+							STDBIT_CLKMULT)) &
+					STDMSK_CLKMULT)));
+
+	// Return adjusted counter setup.
+	return setup;
+}
+
+static uint16_t GetMode_B(comedi_device * dev, enc_private * k)
+{
+	register uint16_t cra;
+	register uint16_t crb;
+	register uint16_t setup;
+
+	// Fetch CRA and CRB register images.
+	cra = DEBIread(dev, k->MyCRA);
+	crb = DEBIread(dev, k->MyCRB);
+
+	// Populate the standardized counter setup bit fields.  Note:
+	// IndexSrc is restricted to ENC_X or IndxPol.
+	setup = (((crb << (STDBIT_INTSRC - CRBBIT_INTSRC_B)) & STDMSK_INTSRC)	// IntSrc   = IntSrcB.
+		| ((crb << (STDBIT_LATCHSRC - CRBBIT_LATCHSRC)) & STDMSK_LATCHSRC)	// LatchSrc = LatchSrcB.
+		| ((crb << (STDBIT_LOADSRC - CRBBIT_LOADSRC_B)) & STDMSK_LOADSRC)	// LoadSrc  = LoadSrcB.
+		| ((crb << (STDBIT_INDXPOL - CRBBIT_INDXPOL_B)) & STDMSK_INDXPOL)	// IndxPol  = IndxPolB.
+		| ((crb >> (CRBBIT_CLKENAB_B - STDBIT_CLKENAB)) & STDMSK_CLKENAB)	// ClkEnab  = ClkEnabB.
+		| ((cra >> ((CRABIT_INDXSRC_B + 1) - STDBIT_INDXSRC)) & STDMSK_INDXSRC));	// IndxSrc  = IndxSrcB<1>.
+
+	// Adjust mode-dependent parameters.
+	if ((crb & CRBMSK_CLKMULT_B) == (MULT_X0 << CRBBIT_CLKMULT_B))	// If Extender mode (ClkMultB == MULT_X0):
+		setup |= ((CLKSRC_EXTENDER << STDBIT_CLKSRC)	//   Indicate Extender mode.
+			| (MULT_X1 << STDBIT_CLKMULT)	//   Indicate multiplier is 1x.
+			| ((cra >> (CRABIT_CLKSRC_B - STDBIT_CLKPOL)) & STDMSK_CLKPOL));	//   Set ClkPol equal to Timer count direction (ClkSrcB<0>).
+
+	else if (cra & (2 << CRABIT_CLKSRC_B))	// If Timer mode (ClkSrcB<1> == 1):
+		setup |= ((CLKSRC_TIMER << STDBIT_CLKSRC)	//   Indicate Timer mode.
+			| (MULT_X1 << STDBIT_CLKMULT)	//   Indicate multiplier is 1x.
+			| ((cra >> (CRABIT_CLKSRC_B - STDBIT_CLKPOL)) & STDMSK_CLKPOL));	//   Set ClkPol equal to Timer count direction (ClkSrcB<0>).
+
+	else			// If Counter mode (ClkSrcB<1> == 0):
+		setup |= ((CLKSRC_COUNTER << STDBIT_CLKSRC)	//   Indicate Timer mode.
+			| ((crb >> (CRBBIT_CLKMULT_B - STDBIT_CLKMULT)) & STDMSK_CLKMULT)	//   Clock multiplier is passed through.
+			| ((crb << (STDBIT_CLKPOL - CRBBIT_CLKPOL_B)) & STDMSK_CLKPOL));	//   Clock polarity is passed through.
+
+	// Return adjusted counter setup.
+	return setup;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+// Set the operating mode for the specified counter.  The setup
+// parameter is treated as a COUNTER_SETUP data type.  The following
+// parameters are programmable (all other parms are ignored): ClkMult,
+// ClkPol, ClkEnab, IndexSrc, IndexPol, LoadSrc.
+
+static void SetMode_A(comedi_device * dev, enc_private * k, uint16_t Setup,
+	uint16_t DisableIntSrc)
+{
+	register uint16_t cra;
+	register uint16_t crb;
+	register uint16_t setup = Setup;	// Cache the Standard Setup.
+
+	// Initialize CRA and CRB images.
+	cra = ((setup & CRAMSK_LOADSRC_A)	// Preload trigger is passed through.
+		| ((setup & STDMSK_INDXSRC) >> (STDBIT_INDXSRC - (CRABIT_INDXSRC_A + 1))));	// IndexSrc is restricted to ENC_X or IndxPol.
+
+	crb = (CRBMSK_INTRESETCMD | CRBMSK_INTRESET_A	// Reset any pending CounterA event captures.
+		| ((setup & STDMSK_CLKENAB) << (CRBBIT_CLKENAB_A - STDBIT_CLKENAB)));	// Clock enable is passed through.
+
+	// Force IntSrc to Disabled if DisableIntSrc is asserted.
+	if (!DisableIntSrc)
+		cra |= ((setup & STDMSK_INTSRC) >> (STDBIT_INTSRC -
+				CRABIT_INTSRC_A));
+
+	// Populate all mode-dependent attributes of CRA & CRB images.
+	switch ((setup & STDMSK_CLKSRC) >> STDBIT_CLKSRC) {
+	case CLKSRC_EXTENDER:	// Extender Mode: Force to Timer mode
+		// (Extender valid only for B counters).
+
+	case CLKSRC_TIMER:	// Timer Mode:
+		cra |= ((2 << CRABIT_CLKSRC_A)	//   ClkSrcA<1> selects system clock
+			| ((setup & STDMSK_CLKPOL) >> (STDBIT_CLKPOL - CRABIT_CLKSRC_A))	//     with count direction (ClkSrcA<0>) obtained from ClkPol.
+			| (1 << CRABIT_CLKPOL_A)	//   ClkPolA behaves as always-on clock enable.
+			| (MULT_X1 << CRABIT_CLKMULT_A));	//   ClkMult must be 1x.
+		break;
+
+	default:		// Counter Mode:
+		cra |= (CLKSRC_COUNTER	//   Select ENC_C and ENC_D as clock/direction inputs.
+			| ((setup & STDMSK_CLKPOL) << (CRABIT_CLKPOL_A - STDBIT_CLKPOL))	//   Clock polarity is passed through.
+			| (((setup & STDMSK_CLKMULT) == (MULT_X0 << STDBIT_CLKMULT)) ?	//   Force multiplier to x1 if not legal, otherwise pass through.
+				(MULT_X1 << CRABIT_CLKMULT_A) :
+				((setup & STDMSK_CLKMULT) << (CRABIT_CLKMULT_A -
+						STDBIT_CLKMULT))));
+	}
+
+	// Force positive index polarity if IndxSrc is software-driven only,
+	// otherwise pass it through.
+	if (~setup & STDMSK_INDXSRC)
+		cra |= ((setup & STDMSK_INDXPOL) << (CRABIT_INDXPOL_A -
+				STDBIT_INDXPOL));
+
+	// If IntSrc has been forced to Disabled, update the MISC2 interrupt
+	// enable mask to indicate the counter interrupt is disabled.
+	if (DisableIntSrc)
+		devpriv->CounterIntEnabs &= ~k->MyEventBits[3];
+
+	// While retaining CounterB and LatchSrc configurations, program the
+	// new counter operating mode.
+	DEBIreplace(dev, k->MyCRA, CRAMSK_INDXSRC_B | CRAMSK_CLKSRC_B, cra);
+	DEBIreplace(dev, k->MyCRB,
+		(uint16_t) (~(CRBMSK_INTCTRL | CRBMSK_CLKENAB_A)), crb);
+}
+
+static void SetMode_B(comedi_device * dev, enc_private * k, uint16_t Setup,
+	uint16_t DisableIntSrc)
+{
+	register uint16_t cra;
+	register uint16_t crb;
+	register uint16_t setup = Setup;	// Cache the Standard Setup.
+
+	// Initialize CRA and CRB images.
+	cra = ((setup & STDMSK_INDXSRC) << ((CRABIT_INDXSRC_B + 1) - STDBIT_INDXSRC));	// IndexSrc field is restricted to ENC_X or IndxPol.
+
+	crb = (CRBMSK_INTRESETCMD | CRBMSK_INTRESET_B	// Reset event captures and disable interrupts.
+		| ((setup & STDMSK_CLKENAB) << (CRBBIT_CLKENAB_B - STDBIT_CLKENAB))	// Clock enable is passed through.
+		| ((setup & STDMSK_LOADSRC) >> (STDBIT_LOADSRC - CRBBIT_LOADSRC_B)));	// Preload trigger source is passed through.
+
+	// Force IntSrc to Disabled if DisableIntSrc is asserted.
+	if (!DisableIntSrc)
+		crb |= ((setup & STDMSK_INTSRC) >> (STDBIT_INTSRC -
+				CRBBIT_INTSRC_B));
+
+	// Populate all mode-dependent attributes of CRA & CRB images.
+	switch ((setup & STDMSK_CLKSRC) >> STDBIT_CLKSRC) {
+	case CLKSRC_TIMER:	// Timer Mode:
+		cra |= ((2 << CRABIT_CLKSRC_B)	//   ClkSrcB<1> selects system clock
+			| ((setup & STDMSK_CLKPOL) << (CRABIT_CLKSRC_B - STDBIT_CLKPOL)));	//     with direction (ClkSrcB<0>) obtained from ClkPol.
+		crb |= ((1 << CRBBIT_CLKPOL_B)	//   ClkPolB behaves as always-on clock enable.
+			| (MULT_X1 << CRBBIT_CLKMULT_B));	//   ClkMultB must be 1x.
+		break;
+
+	case CLKSRC_EXTENDER:	// Extender Mode:
+		cra |= ((2 << CRABIT_CLKSRC_B)	//   ClkSrcB source is OverflowA (same as "timer")
+			| ((setup & STDMSK_CLKPOL) << (CRABIT_CLKSRC_B - STDBIT_CLKPOL)));	//     with direction obtained from ClkPol.
+		crb |= ((1 << CRBBIT_CLKPOL_B)	//   ClkPolB controls IndexB -- always set to active.
+			| (MULT_X0 << CRBBIT_CLKMULT_B));	//   ClkMultB selects OverflowA as the clock source.
+		break;
+
+	default:		// Counter Mode:
+		cra |= (CLKSRC_COUNTER << CRABIT_CLKSRC_B);	//   Select ENC_C and ENC_D as clock/direction inputs.
+		crb |= (((setup & STDMSK_CLKPOL) >> (STDBIT_CLKPOL - CRBBIT_CLKPOL_B))	//   ClkPol is passed through.
+			| (((setup & STDMSK_CLKMULT) == (MULT_X0 << STDBIT_CLKMULT)) ?	//   Force ClkMult to x1 if not legal, otherwise pass through.
+				(MULT_X1 << CRBBIT_CLKMULT_B) :
+				((setup & STDMSK_CLKMULT) << (CRBBIT_CLKMULT_B -
+						STDBIT_CLKMULT))));
+	}
+
+	// Force positive index polarity if IndxSrc is software-driven only,
+	// otherwise pass it through.
+	if (~setup & STDMSK_INDXSRC)
+		crb |= ((setup & STDMSK_INDXPOL) >> (STDBIT_INDXPOL -
+				CRBBIT_INDXPOL_B));
+
+	// If IntSrc has been forced to Disabled, update the MISC2 interrupt
+	// enable mask to indicate the counter interrupt is disabled.
+	if (DisableIntSrc)
+		devpriv->CounterIntEnabs &= ~k->MyEventBits[3];
+
+	// While retaining CounterA and LatchSrc configurations, program the
+	// new counter operating mode.
+	DEBIreplace(dev, k->MyCRA,
+		(uint16_t) (~(CRAMSK_INDXSRC_B | CRAMSK_CLKSRC_B)), cra);
+	DEBIreplace(dev, k->MyCRB, CRBMSK_CLKENAB_A | CRBMSK_LATCHSRC, crb);
+}
+
+////////////////////////////////////////////////////////////////////////
+// Return/set a counter's enable.  enab: 0=always enabled, 1=enabled by index.
+
+static void SetEnable_A(comedi_device * dev, enc_private * k, uint16_t enab)
+{
+	DEBUG("SetEnable_A: SetEnable_A enter 3541\n");
+	DEBIreplace(dev, k->MyCRB,
+		(uint16_t) (~(CRBMSK_INTCTRL | CRBMSK_CLKENAB_A)),
+		(uint16_t) (enab << CRBBIT_CLKENAB_A));
+}
+
+static void SetEnable_B(comedi_device * dev, enc_private * k, uint16_t enab)
+{
+	DEBIreplace(dev, k->MyCRB,
+		(uint16_t) (~(CRBMSK_INTCTRL | CRBMSK_CLKENAB_B)),
+		(uint16_t) (enab << CRBBIT_CLKENAB_B));
+}
+
+static uint16_t GetEnable_A(comedi_device * dev, enc_private * k)
+{
+	return (DEBIread(dev, k->MyCRB) >> CRBBIT_CLKENAB_A) & 1;
+}
+
+static uint16_t GetEnable_B(comedi_device * dev, enc_private * k)
+{
+	return (DEBIread(dev, k->MyCRB) >> CRBBIT_CLKENAB_B) & 1;
+}
+
+////////////////////////////////////////////////////////////////////////
+// Return/set a counter pair's latch trigger source.  0: On read
+// access, 1: A index latches A, 2: B index latches B, 3: A overflow
+// latches B.
+
+static void SetLatchSource(comedi_device * dev, enc_private * k, uint16_t value)
+{
+	DEBUG("SetLatchSource: SetLatchSource enter 3550 \n");
+	DEBIreplace(dev, k->MyCRB,
+		(uint16_t) (~(CRBMSK_INTCTRL | CRBMSK_LATCHSRC)),
+		(uint16_t) (value << CRBBIT_LATCHSRC));
+
+	DEBUG("SetLatchSource: SetLatchSource exit \n");
+}
+
+/* static uint16_t GetLatchSource(comedi_device *dev, enc_private *k ) */
+/* { */
+/*   return ( DEBIread( dev, k->MyCRB) >> CRBBIT_LATCHSRC ) & 3; */
+/* } */
+
+/////////////////////////////////////////////////////////////////////////
+// Return/set the event that will trigger transfer of the preload
+// register into the counter.  0=ThisCntr_Index, 1=ThisCntr_Overflow,
+// 2=OverflowA (B counters only), 3=disabled.
+
+static void SetLoadTrig_A(comedi_device * dev, enc_private * k, uint16_t Trig)
+{
+	DEBIreplace(dev, k->MyCRA, (uint16_t) (~CRAMSK_LOADSRC_A),
+		(uint16_t) (Trig << CRABIT_LOADSRC_A));
+}
+
+static void SetLoadTrig_B(comedi_device * dev, enc_private * k, uint16_t Trig)
+{
+	DEBIreplace(dev, k->MyCRB,
+		(uint16_t) (~(CRBMSK_LOADSRC_B | CRBMSK_INTCTRL)),
+		(uint16_t) (Trig << CRBBIT_LOADSRC_B));
+}
+
+static uint16_t GetLoadTrig_A(comedi_device * dev, enc_private * k)
+{
+	return (DEBIread(dev, k->MyCRA) >> CRABIT_LOADSRC_A) & 3;
+}
+
+static uint16_t GetLoadTrig_B(comedi_device * dev, enc_private * k)
+{
+	return (DEBIread(dev, k->MyCRB) >> CRBBIT_LOADSRC_B) & 3;
+}
+
+////////////////////
+// Return/set counter interrupt source and clear any captured
+// index/overflow events.  IntSource: 0=Disabled, 1=OverflowOnly,
+// 2=IndexOnly, 3=IndexAndOverflow.
+
+static void SetIntSrc_A(comedi_device * dev, enc_private * k,
+	uint16_t IntSource)
+{
+	// Reset any pending counter overflow or index captures.
+	DEBIreplace(dev, k->MyCRB, (uint16_t) (~CRBMSK_INTCTRL),
+		CRBMSK_INTRESETCMD | CRBMSK_INTRESET_A);
+
+	// Program counter interrupt source.
+	DEBIreplace(dev, k->MyCRA, ~CRAMSK_INTSRC_A,
+		(uint16_t) (IntSource << CRABIT_INTSRC_A));
+
+	// Update MISC2 interrupt enable mask.
+	devpriv->CounterIntEnabs =
+		(devpriv->CounterIntEnabs & ~k->MyEventBits[3]) | k->
+		MyEventBits[IntSource];
+}
+
+static void SetIntSrc_B(comedi_device * dev, enc_private * k,
+	uint16_t IntSource)
+{
+	uint16_t crb;
+
+	// Cache writeable CRB register image.
+	crb = DEBIread(dev, k->MyCRB) & ~CRBMSK_INTCTRL;
+
+	// Reset any pending counter overflow or index captures.
+	DEBIwrite(dev, k->MyCRB,
+		(uint16_t) (crb | CRBMSK_INTRESETCMD | CRBMSK_INTRESET_B));
+
+	// Program counter interrupt source.
+	DEBIwrite(dev, k->MyCRB,
+		(uint16_t) ((crb & ~CRBMSK_INTSRC_B) | (IntSource <<
+				CRBBIT_INTSRC_B)));
+
+	// Update MISC2 interrupt enable mask.
+	devpriv->CounterIntEnabs =
+		(devpriv->CounterIntEnabs & ~k->MyEventBits[3]) | k->
+		MyEventBits[IntSource];
+}
+
+static uint16_t GetIntSrc_A(comedi_device * dev, enc_private * k)
+{
+	return (DEBIread(dev, k->MyCRA) >> CRABIT_INTSRC_A) & 3;
+}
+
+static uint16_t GetIntSrc_B(comedi_device * dev, enc_private * k)
+{
+	return (DEBIread(dev, k->MyCRB) >> CRBBIT_INTSRC_B) & 3;
+}
+
+/////////////////////////////////////////////////////////////////////////
+// Return/set the clock multiplier.
+
+/* static void SetClkMult(comedi_device *dev, enc_private *k, uint16_t value )  */
+/* { */
+/*   k->SetMode(dev, k, (uint16_t)( ( k->GetMode(dev, k ) & ~STDMSK_CLKMULT ) | ( value << STDBIT_CLKMULT ) ), FALSE ); */
+/* } */
+
+/* static uint16_t GetClkMult(comedi_device *dev, enc_private *k )  */
+/* { */
+/*   return ( k->GetMode(dev, k ) >> STDBIT_CLKMULT ) & 3; */
+/* } */
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* // Return/set the clock polarity. */
+
+/* static void SetClkPol( comedi_device *dev,enc_private *k, uint16_t value )  */
+/* { */
+/*   k->SetMode(dev, k, (uint16_t)( ( k->GetMode(dev, k ) & ~STDMSK_CLKPOL ) | ( value << STDBIT_CLKPOL ) ), FALSE ); */
+/* } */
+
+/* static uint16_t GetClkPol(comedi_device *dev, enc_private *k )  */
+/* { */
+/*   return ( k->GetMode(dev, k ) >> STDBIT_CLKPOL ) & 1; */
+/* } */
+
+/* /////////////////////////////////////////////////////////////////////// */
+/* // Return/set the clock source. */
+
+/* static void SetClkSrc( comedi_device *dev,enc_private *k, uint16_t value )  */
+/* { */
+/*   k->SetMode(dev, k, (uint16_t)( ( k->GetMode(dev, k ) & ~STDMSK_CLKSRC ) | ( value << STDBIT_CLKSRC ) ), FALSE ); */
+/* } */
+
+/* static uint16_t GetClkSrc( comedi_device *dev,enc_private *k )  */
+/* { */
+/*   return ( k->GetMode(dev, k ) >> STDBIT_CLKSRC ) & 3; */
+/* } */
+
+/* //////////////////////////////////////////////////////////////////////// */
+/* // Return/set the index polarity. */
+
+/* static void SetIndexPol(comedi_device *dev, enc_private *k, uint16_t value )  */
+/* { */
+/*   k->SetMode(dev, k, (uint16_t)( ( k->GetMode(dev, k ) & ~STDMSK_INDXPOL ) | ( (value != 0) << STDBIT_INDXPOL ) ), FALSE ); */
+/* } */
+
+/* static uint16_t GetIndexPol(comedi_device *dev, enc_private *k )  */
+/* { */
+/*   return ( k->GetMode(dev, k ) >> STDBIT_INDXPOL ) & 1; */
+/* } */
+
+/* //////////////////////////////////////////////////////////////////////// */
+/* // Return/set the index source. */
+
+/* static void SetIndexSrc(comedi_device *dev, enc_private *k, uint16_t value )  */
+/* { */
+/*   DEBUG("SetIndexSrc: set index src enter 3700\n"); */
+/*   k->SetMode(dev, k, (uint16_t)( ( k->GetMode(dev, k ) & ~STDMSK_INDXSRC ) | ( (value != 0) << STDBIT_INDXSRC ) ), FALSE ); */
+/* } */
+
+/* static uint16_t GetIndexSrc(comedi_device *dev, enc_private *k )  */
+/* { */
+/*   return ( k->GetMode(dev, k ) >> STDBIT_INDXSRC ) & 1; */
+/* } */
+
+///////////////////////////////////////////////////////////////////
+// Generate an index pulse.
+
+static void PulseIndex_A(comedi_device * dev, enc_private * k)
+{
+	register uint16_t cra;
+
+	DEBUG("PulseIndex_A: pulse index enter\n");
+
+	cra = DEBIread(dev, k->MyCRA);	// Pulse index.
+	DEBIwrite(dev, k->MyCRA, (uint16_t) (cra ^ CRAMSK_INDXPOL_A));
+	DEBUG("PulseIndex_A: pulse index step1\n");
+	DEBIwrite(dev, k->MyCRA, cra);
+}
+
+static void PulseIndex_B(comedi_device * dev, enc_private * k)
+{
+	register uint16_t crb;
+
+	crb = DEBIread(dev, k->MyCRB) & ~CRBMSK_INTCTRL;	// Pulse index.
+	DEBIwrite(dev, k->MyCRB, (uint16_t) (crb ^ CRBMSK_INDXPOL_B));
+	DEBIwrite(dev, k->MyCRB, crb);
+}
+
+/////////////////////////////////////////////////////////
+// Write value into counter preload register.
+
+static void Preload(comedi_device * dev, enc_private * k, uint32_t value)
+{
+	DEBUG("Preload: preload enter\n");
+	DEBIwrite(dev, (uint16_t) (k->MyLatchLsw), (uint16_t) value);	// Write value to preload register.
+	DEBUG("Preload: preload step 1\n");
+	DEBIwrite(dev, (uint16_t) (k->MyLatchLsw + 2),
+		(uint16_t) (value >> 16));
+}
+
+static void CountersInit(comedi_device * dev)
+{
+	int chan;
+	enc_private *k;
+	uint16_t Setup = (LOADSRC_INDX << BF_LOADSRC) |	// Preload upon
+		// index.
+		(INDXSRC_SOFT << BF_INDXSRC) |	// Disable hardware index.
+		(CLKSRC_COUNTER << BF_CLKSRC) |	// Operating mode is counter.
+		(CLKPOL_POS << BF_CLKPOL) |	// Active high clock.
+		(CNTDIR_UP << BF_CLKPOL) |	// Count direction is up.
+		(CLKMULT_1X << BF_CLKMULT) |	// Clock multiplier is 1x.
+		(CLKENAB_INDEX << BF_CLKENAB);	// Enabled by index
+
+	// Disable all counter interrupts and clear any captured counter events.
+	for (chan = 0; chan < S626_ENCODER_CHANNELS; chan++) {
+		k = &encpriv[chan];
+		k->SetMode(dev, k, Setup, TRUE);
+		k->SetIntSrc(dev, k, 0);
+		k->ResetCapFlags(dev, k);
+		k->SetEnable(dev, k, CLKENAB_ALWAYS);
+	}
+	DEBUG("CountersInit: counters initialized \n");
+
+}
diff --git a/drivers/staging/comedi/drivers/s626.h b/drivers/staging/comedi/drivers/s626.h
new file mode 100644
index 000000000000..11d8b1ceb0b8
--- /dev/null
+++ b/drivers/staging/comedi/drivers/s626.h
@@ -0,0 +1,802 @@
+/*
+  comedi/drivers/s626.h
+  Sensoray s626 Comedi driver, header file
+
+  COMEDI - Linux Control and Measurement Device Interface
+  Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+
+  Based on Sensoray Model 626 Linux driver Version 0.2
+  Copyright (C) 2002-2004 Sensoray Co., Inc.
+
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation; either version 2 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; if not, write to the Free Software
+  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+/*
+  Driver: s626.o (s626.ko)
+  Description: Sensoray 626 driver
+  Devices: Sensoray s626
+  Authors: Gianluca Palli <gpalli@deis.unibo.it>,
+  Updated: Thu, 12 Jul 2005
+  Status: experimental
+
+  Configuration Options:
+  analog input:
+   none
+
+  analog output:
+   none
+
+  digital channel:
+   s626 has 3 dio subdevices (2,3 and 4) each with 16 i/o channels
+   supported configuration options:
+   INSN_CONFIG_DIO_QUERY
+   COMEDI_INPUT
+   COMEDI_OUTPUT
+
+  encoder:
+   Every channel must be configured before reading.
+
+   Example code
+
+   insn.insn=INSN_CONFIG;   //configuration instruction
+   insn.n=1;                //number of operation (must be 1)
+   insn.data=&initialvalue; //initial value loaded into encoder
+                            //during configuration
+   insn.subdev=5;           //encoder subdevice
+   insn.chanspec=CR_PACK(encoder_channel,0,AREF_OTHER); //encoder_channel
+                                                        //to configure
+
+   comedi_do_insn(cf,&insn); //executing configuration
+*/
+
+#ifdef _DEBUG_
+#define DEBUG(...);        rt_printk(__VA_ARGS__);
+#else
+#define DEBUG(...)
+#endif
+
+#if !defined(TRUE)
+#define TRUE    (1)
+#endif
+
+#if !defined(FALSE)
+#define FALSE   (0)
+#endif
+
+#if !defined(EXTERN)
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+#else
+#define EXTERN extern
+#endif
+#endif
+
+#if !defined(INLINE)
+#define INLINE static __inline
+#endif
+
+/////////////////////////////////////////////////////
+#include<linux/slab.h>
+
+#define S626_SIZE 0x0200
+#define SIZEOF_ADDRESS_SPACE		0x0200
+#define DMABUF_SIZE			4096	// 4k pages
+
+#define S626_ADC_CHANNELS       16
+#define S626_DAC_CHANNELS       4
+#define S626_ENCODER_CHANNELS   6
+#define S626_DIO_CHANNELS       48
+#define S626_DIO_BANKS		3	// Number of DIO groups.
+#define S626_DIO_EXTCHANS	40	// Number of
+					// extended-capability
+					// DIO channels.
+
+#define NUM_TRIMDACS	12	// Number of valid TrimDAC channels.
+
+// PCI bus interface types.
+#define INTEL				1	// Intel bus type.
+#define MOTOROLA			2	// Motorola bus type.
+
+//////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////
+#define PLATFORM		INTEL	// *** SELECT PLATFORM TYPE ***
+//////////////////////////////////////////////////////////
+
+#define RANGE_5V                0x10	// +/-5V range
+#define RANGE_10V               0x00	// +/-10V range
+
+#define EOPL			0x80	// End of ADC poll list marker.
+#define GSEL_BIPOLAR5V		0x00F0	// LP_GSEL setting for 5V bipolar range.
+#define GSEL_BIPOLAR10V		0x00A0	// LP_GSEL setting for 10V bipolar range.
+
+// Error codes that must be visible to this base class.
+#define ERR_ILLEGAL_PARM	0x00010000	// Illegal function parameter value was specified.
+#define ERR_I2C			0x00020000	// I2C error.
+#define ERR_COUNTERSETUP	0x00200000	// Illegal setup specified for counter channel.
+#define ERR_DEBI_TIMEOUT	0x00400000	// DEBI transfer timed out.
+
+// Organization (physical order) and size (in DWORDs) of logical DMA buffers contained by ANA_DMABUF.
+#define ADC_DMABUF_DWORDS	40	// ADC DMA buffer must hold 16 samples, plus pre/post garbage samples.
+#define DAC_WDMABUF_DWORDS	1	// DAC output DMA buffer holds a single sample.
+
+// All remaining space in 4KB DMA buffer is available for the RPS1 program.
+
+// Address offsets, in DWORDS, from base of DMA buffer.
+#define DAC_WDMABUF_OS		ADC_DMABUF_DWORDS
+
+// Interrupt enab bit in ISR and IER.
+#define IRQ_GPIO3		0x00000040	// IRQ enable for GPIO3.
+#define IRQ_RPS1                0x10000000
+#define ISR_AFOU		0x00000800	// Audio fifo
+						// under/overflow
+						// detected.
+#define IRQ_COINT1A             0x0400	// conter 1A overflow
+						// interrupt mask
+#define IRQ_COINT1B             0x0800	// conter 1B overflow
+						// interrupt mask
+#define IRQ_COINT2A             0x1000	// conter 2A overflow
+						// interrupt mask
+#define IRQ_COINT2B             0x2000	// conter 2B overflow
+						// interrupt mask
+#define IRQ_COINT3A             0x4000	// conter 3A overflow
+						// interrupt mask
+#define IRQ_COINT3B             0x8000	// conter 3B overflow
+						// interrupt mask
+
+// RPS command codes.
+#define RPS_CLRSIGNAL		0x00000000	// CLEAR SIGNAL
+#define RPS_SETSIGNAL		0x10000000	// SET SIGNAL
+#define RPS_NOP			0x00000000	// NOP
+#define RPS_PAUSE		0x20000000	// PAUSE
+#define RPS_UPLOAD		0x40000000	// UPLOAD
+#define RPS_JUMP		0x80000000	// JUMP
+#define RPS_LDREG		0x90000100	// LDREG (1 uint32_t only)
+#define RPS_STREG		0xA0000100	// STREG (1 uint32_t only)
+#define RPS_STOP		0x50000000	// STOP
+#define RPS_IRQ                 0x60000000	// IRQ
+
+#define RPS_LOGICAL_OR		0x08000000	// Logical OR conditionals.
+#define RPS_INVERT		0x04000000	// Test for negated semaphores.
+#define RPS_DEBI		0x00000002	// DEBI done
+
+#define RPS_SIG0		0x00200000	// RPS semaphore 0 (used by ADC).
+#define RPS_SIG1		0x00400000	// RPS semaphore 1 (used by DAC).
+#define RPS_SIG2		0x00800000	// RPS semaphore 2 (not used).
+#define RPS_GPIO2		0x00080000	// RPS GPIO2
+#define RPS_GPIO3		0x00100000	// RPS GPIO3
+
+#define RPS_SIGADC		RPS_SIG0	// Trigger/status for ADC's RPS program.
+#define RPS_SIGDAC		RPS_SIG1	// Trigger/status for DAC's RPS program.
+
+// RPS clock parameters.
+#define RPSCLK_SCALAR		8	// This is apparent ratio of PCI/RPS clks (undocumented!!).
+#define RPSCLK_PER_US		( 33 / RPSCLK_SCALAR )	// Number of RPS clocks in one microsecond.
+
+// Event counter source addresses.
+#define SBA_RPS_A0		0x27	// Time of RPS0 busy, in PCI clocks.
+
+// GPIO constants.
+#define GPIO_BASE		0x10004000	// GPIO 0,2,3 = inputs, GPIO3 = IRQ; GPIO1 = out.
+#define GPIO1_LO		0x00000000	// GPIO1 set to LOW.
+#define GPIO1_HI		0x00001000	// GPIO1 set to HIGH.
+
+// Primary Status Register (PSR) constants.
+#define PSR_DEBI_E		0x00040000	// DEBI event flag.
+#define PSR_DEBI_S		0x00080000	// DEBI status flag.
+#define PSR_A2_IN		0x00008000	// Audio output DMA2 protection address reached.
+#define PSR_AFOU		0x00000800	// Audio FIFO under/overflow detected.
+#define PSR_GPIO2		0x00000020	// GPIO2 input pin: 0=AdcBusy, 1=AdcIdle.
+#define PSR_EC0S		0x00000001	// Event counter 0 threshold reached.
+
+// Secondary Status Register (SSR) constants.
+#define SSR_AF2_OUT		0x00000200	// Audio 2 output FIFO under/overflow detected.
+
+// Master Control Register 1 (MC1) constants.
+#define MC1_SOFT_RESET		0x80000000	// Invoke 7146 soft reset.
+#define MC1_SHUTDOWN		0x3FFF0000	// Shut down all MC1-controlled enables.
+
+#define MC1_ERPS1		0x2000	// enab/disable RPS task 1.
+#define MC1_ERPS0		0x1000	// enab/disable RPS task 0.
+#define MC1_DEBI		0x0800	// enab/disable DEBI pins.
+#define MC1_AUDIO		0x0200	// enab/disable audio port pins.
+#define MC1_I2C			0x0100	// enab/disable I2C interface.
+#define MC1_A2OUT		0x0008	// enab/disable transfer on A2 out.
+#define MC1_A2IN		0x0004	// enab/disable transfer on A2 in.
+#define MC1_A1IN		0x0001	// enab/disable transfer on A1 in.
+
+// Master Control Register 2 (MC2) constants.
+#define MC2_UPLD_DEBIq		0x00020002	// Upload DEBI registers.
+#define MC2_UPLD_IICq		0x00010001	// Upload I2C registers.
+#define MC2_RPSSIG2_ONq		0x20002000	// Assert RPS_SIG2.
+#define MC2_RPSSIG1_ONq		0x10001000	// Assert RPS_SIG1.
+#define MC2_RPSSIG0_ONq		0x08000800	// Assert RPS_SIG0.
+#define MC2_UPLD_DEBI_MASKq	0x00000002	// Upload DEBI mask.
+#define MC2_UPLD_IIC_MASKq	0x00000001	// Upload I2C mask.
+#define MC2_RPSSIG2_MASKq	0x00002000	// RPS_SIG2 bit mask.
+#define MC2_RPSSIG1_MASKq	0x00001000	// RPS_SIG1 bit mask.
+#define MC2_RPSSIG0_MASKq	0x00000800	// RPS_SIG0 bit mask.
+
+#define MC2_DELAYTRIG_4USq	MC2_RPSSIG1_ON
+#define MC2_DELAYBUSY_4USq	MC2_RPSSIG1_MASK
+
+#define	MC2_DELAYTRIG_6USq	MC2_RPSSIG2_ON
+#define MC2_DELAYBUSY_6USq	MC2_RPSSIG2_MASK
+
+#define MC2_UPLD_DEBI		0x0002	// Upload DEBI.
+#define MC2_UPLD_IIC		0x0001	// Upload I2C.
+#define MC2_RPSSIG2		0x2000	// RPS signal 2 (not used).
+#define MC2_RPSSIG1		0x1000	// RPS signal 1 (DAC RPS busy).
+#define MC2_RPSSIG0		0x0800	// RPS signal 0 (ADC RPS busy).
+
+#define MC2_ADC_RPS		MC2_RPSSIG0	// ADC RPS busy.
+#define MC2_DAC_RPS		MC2_RPSSIG1	// DAC RPS busy.
+
+///////////////////oldies///////////
+#define MC2_UPLD_DEBIQ		0x00020002	// Upload DEBI registers.
+#define MC2_UPLD_IICQ		0x00010001	// Upload I2C registers.
+////////////////////////////////////////
+
+// PCI BUS (SAA7146) REGISTER ADDRESS OFFSETS ////////////////////////
+#define P_PCI_BT_A		0x004C	// Audio DMA
+						// burst/threshold
+						// control.
+#define P_DEBICFG               0x007C	// DEBI configuration.
+#define P_DEBICMD               0x0080	// DEBI command.
+#define P_DEBIPAGE              0x0084	// DEBI page.
+#define P_DEBIAD                0x0088	// DEBI target address.
+#define P_I2CCTRL               0x008C	// I2C control.
+#define P_I2CSTAT               0x0090	// I2C status.
+#define P_BASEA2_IN		0x00AC	// Audio input 2 base
+						// physical DMAbuf
+						// address.
+#define P_PROTA2_IN		0x00B0	// Audio input 2
+						// physical DMAbuf
+						// protection address.
+#define P_PAGEA2_IN		0x00B4	// Audio input 2
+						// paging attributes.
+#define P_BASEA2_OUT		0x00B8	// Audio output 2 base
+						// physical DMAbuf
+						// address.
+#define P_PROTA2_OUT		0x00BC	// Audio output 2
+						// physical DMAbuf
+						// protection address.
+#define P_PAGEA2_OUT		0x00C0	// Audio output 2
+						// paging attributes.
+#define P_RPSPAGE0              0x00C4	// RPS0 page.
+#define P_RPSPAGE1              0x00C8	// RPS1 page.
+#define P_RPS0_TOUT		0x00D4	// RPS0 time-out.
+#define P_RPS1_TOUT		0x00D8	// RPS1 time-out.
+#define P_IER                   0x00DC	// Interrupt enable.
+#define P_GPIO                  0x00E0	// General-purpose I/O.
+#define P_EC1SSR		0x00E4	// Event counter set 1
+						// source select.
+#define P_ECT1R			0x00EC	// Event counter
+						// threshold set 1.
+#define P_ACON1                 0x00F4	// Audio control 1.
+#define P_ACON2                 0x00F8	// Audio control 2.
+#define P_MC1                   0x00FC	// Master control 1.
+#define P_MC2                   0x0100	// Master control 2.
+#define P_RPSADDR0              0x0104	// RPS0 instruction pointer.
+#define P_RPSADDR1              0x0108	// RPS1 instruction pointer.
+#define P_ISR                   0x010C	// Interrupt status.
+#define P_PSR                   0x0110	// Primary status.
+#define P_SSR                   0x0114	// Secondary status.
+#define P_EC1R			0x0118	// Event counter set 1.
+#define P_ADP4			0x0138	// Logical audio DMA
+						// pointer of audio
+						// input FIFO A2_IN.
+#define P_FB_BUFFER1            0x0144	// Audio feedback buffer 1.
+#define P_FB_BUFFER2            0x0148	// Audio feedback buffer 2.
+#define P_TSL1                  0x0180	// Audio time slot list 1.
+#define P_TSL2                  0x01C0	// Audio time slot list 2.
+
+// LOCAL BUS (GATE ARRAY) REGISTER ADDRESS OFFSETS /////////////////
+// Analog I/O registers:
+#define LP_DACPOL		0x0082	//  Write DAC polarity.
+#define LP_GSEL			0x0084	//  Write ADC gain.
+#define LP_ISEL			0x0086	//  Write ADC channel select.
+// Digital I/O (write only):
+#define LP_WRINTSELA		0x0042	//  Write A interrupt enable.
+#define LP_WREDGSELA		0x0044	//  Write A edge selection.
+#define LP_WRCAPSELA		0x0046	//  Write A capture enable.
+#define LP_WRDOUTA		0x0048	//  Write A digital output.
+#define LP_WRINTSELB		0x0052	//  Write B interrupt enable.
+#define LP_WREDGSELB		0x0054	//  Write B edge selection.
+#define LP_WRCAPSELB		0x0056	//  Write B capture enable.
+#define LP_WRDOUTB		0x0058	//  Write B digital output.
+#define LP_WRINTSELC		0x0062	//  Write C interrupt enable.
+#define LP_WREDGSELC		0x0064	//  Write C edge selection.
+#define LP_WRCAPSELC		0x0066	//  Write C capture enable.
+#define LP_WRDOUTC		0x0068	//  Write C digital output.
+
+// Digital I/O (read only):
+#define LP_RDDINA		0x0040	//  Read digital input.
+#define LP_RDCAPFLGA		0x0048	//  Read edges captured.
+#define LP_RDINTSELA		0x004A	//  Read interrupt
+						//  enable register.
+#define LP_RDEDGSELA		0x004C	//  Read edge
+						//  selection
+						//  register.
+#define LP_RDCAPSELA		0x004E	//  Read capture
+						//  enable register.
+#define LP_RDDINB		0x0050	//  Read digital input.
+#define LP_RDCAPFLGB		0x0058	//  Read edges captured.
+#define LP_RDINTSELB		0x005A	//  Read interrupt
+						//  enable register.
+#define LP_RDEDGSELB		0x005C	//  Read edge
+						//  selection
+						//  register.
+#define LP_RDCAPSELB		0x005E	//  Read capture
+						//  enable register.
+#define LP_RDDINC		0x0060	//  Read digital input.
+#define LP_RDCAPFLGC		0x0068	//  Read edges captured.
+#define LP_RDINTSELC		0x006A	//  Read interrupt
+						//  enable register.
+#define LP_RDEDGSELC		0x006C	//  Read edge
+						//  selection
+						//  register.
+#define LP_RDCAPSELC		0x006E	//  Read capture
+						//  enable register.
+// Counter Registers (read/write):
+#define LP_CR0A			0x0000	//  0A setup register.
+#define LP_CR0B			0x0002	//  0B setup register.
+#define LP_CR1A			0x0004	//  1A setup register.
+#define LP_CR1B			0x0006	//  1B setup register.
+#define LP_CR2A			0x0008	//  2A setup register.
+#define LP_CR2B			0x000A	//  2B setup register.
+// Counter PreLoad (write) and Latch (read) Registers:
+#define	LP_CNTR0ALSW		0x000C	//  0A lsw.
+#define	LP_CNTR0AMSW		0x000E	//  0A msw.
+#define	LP_CNTR0BLSW		0x0010	//  0B lsw.
+#define	LP_CNTR0BMSW		0x0012	//  0B msw.
+#define	LP_CNTR1ALSW		0x0014	//  1A lsw.
+#define	LP_CNTR1AMSW		0x0016	//  1A msw.
+#define	LP_CNTR1BLSW		0x0018	//  1B lsw.
+#define	LP_CNTR1BMSW		0x001A	//  1B msw.
+#define	LP_CNTR2ALSW		0x001C	//  2A lsw.
+#define	LP_CNTR2AMSW		0x001E	//  2A msw.
+#define	LP_CNTR2BLSW		0x0020	//  2B lsw.
+#define	LP_CNTR2BMSW		0x0022	//  2B msw.
+// Miscellaneous Registers (read/write):
+#define LP_MISC1		0x0088	//  Read/write Misc1.
+#define LP_WRMISC2		0x0090	//  Write Misc2.
+#define LP_RDMISC2		0x0082	//  Read Misc2.
+
+// Bit masks for MISC1 register that are the same for reads and writes.
+#define MISC1_WENABLE		0x8000	// enab writes to
+						// MISC2 (except Clear
+						// Watchdog bit).
+#define MISC1_WDISABLE		0x0000	// Disable writes to MISC2.
+#define MISC1_EDCAP		0x1000	// enab edge capture
+						// on DIO chans
+						// specified by
+						// LP_WRCAPSELx.
+#define MISC1_NOEDCAP		0x0000	// Disable edge
+						// capture on
+						// specified DIO
+						// chans.
+
+// Bit masks for MISC1 register reads.
+#define RDMISC1_WDTIMEOUT	0x4000	// Watchdog timer timed out.
+
+// Bit masks for MISC2 register writes.
+#define WRMISC2_WDCLEAR		0x8000	// Reset watchdog
+						// timer to zero.
+#define WRMISC2_CHARGE_ENABLE	0x4000	// enab battery
+						// trickle charging.
+
+// Bit masks for MISC2 register that are the same for reads and writes.
+#define MISC2_BATT_ENABLE	0x0008	// Backup battery enable.
+#define MISC2_WDENABLE		0x0004	// Watchdog timer enable.
+#define MISC2_WDPERIOD_MASK	0x0003	// Watchdog interval
+						// select mask.
+
+// Bit masks for ACON1 register.
+#define A2_RUN			0x40000000	// Run A2 based on TSL2.
+#define A1_RUN			0x20000000	// Run A1 based on TSL1.
+#define A1_SWAP			0x00200000	// Use big-endian for A1.
+#define A2_SWAP			0x00100000	// Use big-endian for A2.
+#define WS_MODES		0x00019999	// WS0 = TSL1 trigger
+						// input, WS1-WS4 =
+						// CS* outputs.
+
+#if PLATFORM == INTEL		// Base ACON1 config:
+						// always run A1 based
+						// on TSL1.
+#define ACON1_BASE		( WS_MODES | A1_RUN )
+#elif PLATFORM == MOTOROLA
+#define ACON1_BASE		( WS_MODES | A1_RUN | A1_SWAP | A2_SWAP )
+#endif
+
+#define ACON1_ADCSTART		ACON1_BASE	// Start ADC: run A1
+						// based on TSL1.
+#define ACON1_DACSTART		( ACON1_BASE | A2_RUN )	// Start
+							// transmit to
+							// DAC: run A2
+							// based on
+							// TSL2.
+#define ACON1_DACSTOP		ACON1_BASE	// Halt A2.
+
+// Bit masks for ACON2 register.
+#define A1_CLKSRC_BCLK1		0x00000000	// A1 bit rate = BCLK1 (ADC).
+#define A2_CLKSRC_X1		0x00800000	// A2 bit rate = ACLK/1 (DACs).
+#define A2_CLKSRC_X2		0x00C00000	// A2 bit rate = ACLK/2 (DACs).
+#define A2_CLKSRC_X4		0x01400000	// A2 bit rate = ACLK/4 (DACs).
+#define INVERT_BCLK2		0x00100000	// Invert BCLK2 (DACs).
+#define BCLK2_OE		0x00040000	// enab BCLK2 (DACs).
+#define ACON2_XORMASK		0x000C0000	// XOR mask for ACON2
+						// active-low bits.
+
+#define ACON2_INIT		( ACON2_XORMASK ^ ( A1_CLKSRC_BCLK1 | A2_CLKSRC_X2 | INVERT_BCLK2 | BCLK2_OE ) )
+
+// Bit masks for timeslot records.
+#define WS1		     	0x40000000	// WS output to assert.
+#define WS2		     	0x20000000
+#define WS3		     	0x10000000
+#define WS4		     	0x08000000
+#define RSD1			0x01000000	// Shift A1 data in on SD1.
+#define SDW_A1			0x00800000	// Store rcv'd char at
+						// next char slot of
+						// DWORD1 buffer.
+#define SIB_A1			0x00400000	// Store rcv'd char at
+						// next char slot of
+						// FB1 buffer.
+#define SF_A1			0x00200000	// Write unsigned long
+						// buffer to input
+						// FIFO.
+
+//Select parallel-to-serial converter's data source:
+#define XFIFO_0			0x00000000	//   Data fifo byte 0.
+#define XFIFO_1			0x00000010	//   Data fifo byte 1.
+#define XFIFO_2			0x00000020	//   Data fifo byte 2.
+#define XFIFO_3			0x00000030	//   Data fifo byte 3.
+#define XFB0			0x00000040	//   FB_BUFFER byte 0.
+#define XFB1			0x00000050	//   FB_BUFFER byte 1.
+#define XFB2			0x00000060	//   FB_BUFFER byte 2.
+#define XFB3			0x00000070	//   FB_BUFFER byte 3.
+#define SIB_A2			0x00000200	// Store next dword
+						// from A2's input
+						// shifter to FB2
+						// buffer.
+#define SF_A2			0x00000100	// Store next dword
+						// from A2's input
+						// shifter to its
+						// input fifo.
+#define LF_A2			0x00000080	// Load next dword
+						// from A2's output
+						// fifo into its
+						// output dword
+						// buffer.
+#define XSD2			0x00000008	// Shift data out on SD2.
+#define RSD3			0x00001800	// Shift data in on SD3.
+#define RSD2			0x00001000	// Shift data in on SD2.
+#define LOW_A2			0x00000002	// Drive last SD low
+						// for 7 clks, then
+						// tri-state.
+#define EOS		     	0x00000001	// End of superframe.
+
+//////////////////////
+
+// I2C configuration constants.
+#define I2C_CLKSEL		0x0400	// I2C bit rate =
+						// PCIclk/480 = 68.75
+						// KHz.
+#define I2C_BITRATE		68.75	// I2C bus data bit
+						// rate (determined by
+						// I2C_CLKSEL) in KHz.
+#define I2C_WRTIME		15.0	// Worst case time,in
+						// msec, for EEPROM
+						// internal write op.
+
+// I2C manifest constants.
+
+// Max retries to wait for EEPROM write.
+#define I2C_RETRIES		( I2C_WRTIME * I2C_BITRATE / 9.0 )
+#define I2C_ERR			0x0002	// I2C control/status
+						// flag ERROR.
+#define I2C_BUSY		0x0001	// I2C control/status
+						// flag BUSY.
+#define I2C_ABORT		0x0080	// I2C status flag ABORT.
+#define I2C_ATTRSTART		0x3	// I2C attribute START.
+#define I2C_ATTRCONT		0x2	// I2C attribute CONT.
+#define I2C_ATTRSTOP		0x1	// I2C attribute STOP.
+#define I2C_ATTRNOP		0x0	// I2C attribute NOP.
+
+// I2C read command  | EEPROM address.
+#define I2CR			( devpriv->I2CAdrs | 1 )
+
+// I2C write command | EEPROM address.
+#define I2CW			( devpriv->I2CAdrs )
+
+// Code macros used for constructing I2C command bytes.
+#define I2C_B2(ATTR,VAL)	( ( (ATTR) << 6 ) | ( (VAL) << 24 ) )
+#define I2C_B1(ATTR,VAL)	( ( (ATTR) << 4 ) | ( (VAL) << 16 ) )
+#define I2C_B0(ATTR,VAL)	( ( (ATTR) << 2 ) | ( (VAL) <<  8 ) )
+
+////////////////////////////////////////////////////////
+//oldest
+#define P_DEBICFGq              0x007C	// DEBI configuration.
+#define P_DEBICMDq              0x0080	// DEBI command.
+#define P_DEBIPAGEq             0x0084	// DEBI page.
+#define P_DEBIADq               0x0088	// DEBI target address.
+
+#define DEBI_CFG_TOQ		0x03C00000	// timeout (15 PCI cycles)
+#define DEBI_CFG_FASTQ		0x10000000	// fast mode enable
+#define DEBI_CFG_16Q		0x00080000	// 16-bit access enable
+#define DEBI_CFG_INCQ		0x00040000	// enable address increment
+#define DEBI_CFG_TIMEROFFQ	0x00010000	// disable timer
+#define DEBI_CMD_RDQ		0x00050000	// read immediate 2 bytes
+#define DEBI_CMD_WRQ		0x00040000	// write immediate 2 bytes
+#define DEBI_PAGE_DISABLEQ	0x00000000	// paging disable
+
+///////////////////////////////////////////
+// DEBI command constants.
+#define DEBI_CMD_SIZE16		( 2 << 17 )	// Transfer size is
+						// always 2 bytes.
+#define DEBI_CMD_READ		0x00010000	// Read operation.
+#define DEBI_CMD_WRITE		0x00000000	// Write operation.
+
+// Read immediate 2 bytes.
+#define DEBI_CMD_RDWORD		( DEBI_CMD_READ  | DEBI_CMD_SIZE16 )
+
+// Write immediate 2 bytes.
+#define DEBI_CMD_WRWORD		( DEBI_CMD_WRITE | DEBI_CMD_SIZE16 )
+
+// DEBI configuration constants.
+#define DEBI_CFG_XIRQ_EN	0x80000000	// enab external
+						// interrupt on GPIO3.
+#define DEBI_CFG_XRESUME	0x40000000	// Resume block
+						// transfer when XIRQ
+						// deasserted.
+#define DEBI_CFG_FAST		0x10000000	// Fast mode enable.
+
+// 4-bit field that specifies DEBI timeout value in PCI clock cycles:
+#define DEBI_CFG_TOUT_BIT	22	//   Finish DEBI cycle after
+					//   this many clocks.
+
+// 2-bit field that specifies Endian byte lane steering:
+#define DEBI_CFG_SWAP_NONE	0x00000000	//   Straight - don't
+						//   swap any bytes
+						//   (Intel).
+#define DEBI_CFG_SWAP_2		0x00100000	//   2-byte swap (Motorola).
+#define DEBI_CFG_SWAP_4		0x00200000	//   4-byte swap.
+#define DEBI_CFG_16		0x00080000	// Slave is able to
+						// serve 16-bit
+						// cycles.
+
+#define DEBI_CFG_SLAVE16	0x00080000	// Slave is able to
+						// serve 16-bit
+						// cycles.
+#define DEBI_CFG_INC		0x00040000	// enab address
+						// increment for block
+						// transfers.
+#define DEBI_CFG_INTEL		0x00020000	// Intel style local bus.
+#define DEBI_CFG_TIMEROFF	0x00010000	// Disable timer.
+
+#if PLATFORM == INTEL
+
+#define DEBI_TOUT		7	// Wait 7 PCI clocks
+						// (212 ns) before
+						// polling RDY.
+
+// Intel byte lane steering (pass through all byte lanes).
+#define DEBI_SWAP		DEBI_CFG_SWAP_NONE
+
+#elif PLATFORM == MOTOROLA
+
+#define DEBI_TOUT		15	// Wait 15 PCI clocks (454 ns)
+					// maximum before timing out.
+#define DEBI_SWAP		DEBI_CFG_SWAP_2	// Motorola byte lane steering.
+
+#endif
+
+// DEBI page table constants.
+#define DEBI_PAGE_DISABLE	0x00000000	// Paging disable.
+
+///////////////////EXTRA FROM OTHER SANSORAY  * .h////////
+
+// LoadSrc values:
+#define LOADSRC_INDX		0	// Preload core in response to
+					// Index.
+#define LOADSRC_OVER		1	// Preload core in response to
+					// Overflow.
+#define LOADSRCB_OVERA		2	// Preload B core in response
+					// to A Overflow.
+#define LOADSRC_NONE		3	// Never preload core.
+
+// IntSrc values:
+#define INTSRC_NONE 		0	// Interrupts disabled.
+#define INTSRC_OVER 		1	// Interrupt on Overflow.
+#define INTSRC_INDX 		2	// Interrupt on Index.
+#define INTSRC_BOTH 		3	// Interrupt on Index or Overflow.
+
+// LatchSrc values:
+#define LATCHSRC_AB_READ	0	// Latch on read.
+#define LATCHSRC_A_INDXA	1	// Latch A on A Index.
+#define LATCHSRC_B_INDXB	2	// Latch B on B Index.
+#define LATCHSRC_B_OVERA	3	// Latch B on A Overflow.
+
+// IndxSrc values:
+#define INDXSRC_HARD		0	// Hardware or software index.
+#define INDXSRC_SOFT		1	// Software index only.
+
+// IndxPol values:
+#define INDXPOL_POS 		0	// Index input is active high.
+#define INDXPOL_NEG 		1	// Index input is active low.
+
+// ClkSrc values:
+#define CLKSRC_COUNTER		0	// Counter mode.
+#define CLKSRC_TIMER		2	// Timer mode.
+#define CLKSRC_EXTENDER		3	// Extender mode.
+
+// ClkPol values:
+#define CLKPOL_POS		0	// Counter/Extender clock is
+					// active high.
+#define CLKPOL_NEG		1	// Counter/Extender clock is
+					// active low.
+#define CNTDIR_UP		0	// Timer counts up.
+#define CNTDIR_DOWN 		1	// Timer counts down.
+
+// ClkEnab values:
+#define CLKENAB_ALWAYS		0	// Clock always enabled.
+#define CLKENAB_INDEX		1	// Clock is enabled by index.
+
+// ClkMult values:
+#define CLKMULT_4X 		0	// 4x clock multiplier.
+#define CLKMULT_2X 		1	// 2x clock multiplier.
+#define CLKMULT_1X 		2	// 1x clock multiplier.
+
+// Bit Field positions in COUNTER_SETUP structure:
+#define BF_LOADSRC		9	// Preload trigger.
+#define BF_INDXSRC		7	// Index source.
+#define BF_INDXPOL		6	// Index polarity.
+#define BF_CLKSRC		4	// Clock source.
+#define BF_CLKPOL		3	// Clock polarity/count direction.
+#define BF_CLKMULT		1	// Clock multiplier.
+#define BF_CLKENAB		0	// Clock enable.
+
+// Enumerated counter operating modes specified by ClkSrc bit field in
+// a COUNTER_SETUP.
+
+#define CLKSRC_COUNTER		0	// Counter: ENC_C clock, ENC_D
+					// direction.
+#define CLKSRC_TIMER		2	// Timer: SYS_C clock,
+					// direction specified by
+					// ClkPol.
+#define CLKSRC_EXTENDER		3	// Extender: OVR_A clock,
+					// ENC_D direction.
+
+// Enumerated counter clock multipliers.
+
+#define MULT_X0			0x0003	// Supports no multipliers;
+					// fixed physical multiplier =
+					// 3.
+#define MULT_X1			0x0002	// Supports multiplier x1;
+					// fixed physical multiplier =
+					// 2.
+#define MULT_X2			0x0001	// Supports multipliers x1,
+					// x2; physical multipliers =
+					// 1 or 2.
+#define MULT_X4			0x0000	// Supports multipliers x1,
+					// x2, x4; physical
+					// multipliers = 0, 1 or 2.
+
+// Sanity-check limits for parameters.
+
+#define NUM_COUNTERS		6	// Maximum valid counter
+					// logical channel number.
+#define NUM_INTSOURCES		4
+#define NUM_LATCHSOURCES	4
+#define NUM_CLKMULTS		4
+#define NUM_CLKSOURCES		4
+#define NUM_CLKPOLS		2
+#define NUM_INDEXPOLS		2
+#define NUM_INDEXSOURCES	2
+#define NUM_LOADTRIGS		4
+
+// Bit field positions in CRA and CRB counter control registers.
+
+// Bit field positions in CRA:
+#define CRABIT_INDXSRC_B	14	//   B index source.
+#define CRABIT_CLKSRC_B		12	//   B clock source.
+#define CRABIT_INDXPOL_A	11	//   A index polarity.
+#define CRABIT_LOADSRC_A	 9	//   A preload trigger.
+#define CRABIT_CLKMULT_A	 7	//   A clock multiplier.
+#define CRABIT_INTSRC_A		 5	//   A interrupt source.
+#define CRABIT_CLKPOL_A		 4	//   A clock polarity.
+#define CRABIT_INDXSRC_A	 2	//   A index source.
+#define CRABIT_CLKSRC_A		 0	//   A clock source.
+
+// Bit field positions in CRB:
+#define CRBBIT_INTRESETCMD	15	//   Interrupt reset command.
+#define CRBBIT_INTRESET_B	14	//   B interrupt reset enable.
+#define CRBBIT_INTRESET_A	13	//   A interrupt reset enable.
+#define CRBBIT_CLKENAB_A	12	//   A clock enable.
+#define CRBBIT_INTSRC_B		10	//   B interrupt source.
+#define CRBBIT_LATCHSRC		 8	//   A/B latch source.
+#define CRBBIT_LOADSRC_B	 6	//   B preload trigger.
+#define CRBBIT_CLKMULT_B	 3	//   B clock multiplier.
+#define CRBBIT_CLKENAB_B	 2	//   B clock enable.
+#define CRBBIT_INDXPOL_B	 1	//   B index polarity.
+#define CRBBIT_CLKPOL_B		 0	//   B clock polarity.
+
+// Bit field masks for CRA and CRB.
+
+#define CRAMSK_INDXSRC_B	( (uint16_t)( 3 << CRABIT_INDXSRC_B) )
+#define CRAMSK_CLKSRC_B		( (uint16_t)( 3 << CRABIT_CLKSRC_B) )
+#define CRAMSK_INDXPOL_A	( (uint16_t)( 1 << CRABIT_INDXPOL_A) )
+#define CRAMSK_LOADSRC_A	( (uint16_t)( 3 << CRABIT_LOADSRC_A) )
+#define CRAMSK_CLKMULT_A	( (uint16_t)( 3 << CRABIT_CLKMULT_A) )
+#define CRAMSK_INTSRC_A		( (uint16_t)( 3 << CRABIT_INTSRC_A) )
+#define CRAMSK_CLKPOL_A		( (uint16_t)( 3 << CRABIT_CLKPOL_A) )
+#define CRAMSK_INDXSRC_A	( (uint16_t)( 3 << CRABIT_INDXSRC_A) )
+#define CRAMSK_CLKSRC_A		( (uint16_t)( 3 << CRABIT_CLKSRC_A) )
+
+#define CRBMSK_INTRESETCMD	( (uint16_t)( 1 << CRBBIT_INTRESETCMD) )
+#define CRBMSK_INTRESET_B	( (uint16_t)( 1 << CRBBIT_INTRESET_B) )
+#define CRBMSK_INTRESET_A	( (uint16_t)( 1 << CRBBIT_INTRESET_A) )
+#define CRBMSK_CLKENAB_A	( (uint16_t)( 1 << CRBBIT_CLKENAB_A) )
+#define CRBMSK_INTSRC_B		( (uint16_t)( 3 << CRBBIT_INTSRC_B) )
+#define CRBMSK_LATCHSRC		( (uint16_t)( 3 << CRBBIT_LATCHSRC) )
+#define CRBMSK_LOADSRC_B	( (uint16_t)( 3 << CRBBIT_LOADSRC_B) )
+#define CRBMSK_CLKMULT_B	( (uint16_t)( 3 << CRBBIT_CLKMULT_B) )
+#define CRBMSK_CLKENAB_B	( (uint16_t)( 1 << CRBBIT_CLKENAB_B) )
+#define CRBMSK_INDXPOL_B	( (uint16_t)( 1 << CRBBIT_INDXPOL_B) )
+#define CRBMSK_CLKPOL_B		( (uint16_t)( 1 << CRBBIT_CLKPOL_B) )
+
+#define CRBMSK_INTCTRL		( CRBMSK_INTRESETCMD | CRBMSK_INTRESET_A | CRBMSK_INTRESET_B )	// Interrupt reset control bits.
+
+// Bit field positions for standardized SETUP structure.
+
+#define STDBIT_INTSRC		13
+#define STDBIT_LATCHSRC		11
+#define STDBIT_LOADSRC		 9
+#define STDBIT_INDXSRC		 7
+#define STDBIT_INDXPOL		 6
+#define STDBIT_CLKSRC		 4
+#define STDBIT_CLKPOL		 3
+#define STDBIT_CLKMULT		 1
+#define STDBIT_CLKENAB		 0
+
+// Bit field masks for standardized SETUP structure.
+
+#define STDMSK_INTSRC		( (uint16_t)( 3 << STDBIT_INTSRC   ) )
+#define STDMSK_LATCHSRC		( (uint16_t)( 3 << STDBIT_LATCHSRC ) )
+#define STDMSK_LOADSRC		( (uint16_t)( 3 << STDBIT_LOADSRC  ) )
+#define STDMSK_INDXSRC		( (uint16_t)( 1 << STDBIT_INDXSRC  ) )
+#define STDMSK_INDXPOL		( (uint16_t)( 1 << STDBIT_INDXPOL  ) )
+#define STDMSK_CLKSRC		( (uint16_t)( 3 << STDBIT_CLKSRC   ) )
+#define STDMSK_CLKPOL		( (uint16_t)( 1 << STDBIT_CLKPOL   ) )
+#define STDMSK_CLKMULT		( (uint16_t)( 3 << STDBIT_CLKMULT  ) )
+#define STDMSK_CLKENAB		( (uint16_t)( 1 << STDBIT_CLKENAB  ) )
+
+//////////////////////////////////////////////////////////
+
+/* typedef struct indexCounter */
+/* { */
+/*   unsigned int ao; */
+/*   unsigned int ai; */
+/*   unsigned int digout; */
+/*   unsigned int digin; */
+/*   unsigned int enc; */
+/* }CallCounter; */
+
+typedef struct bufferDMA {
+	dma_addr_t PhysicalBase;
+	void *LogicalBase;
+	uint32_t DMAHandle;
+} DMABUF;
diff --git a/drivers/staging/comedi/drivers/usbdux.c b/drivers/staging/comedi/drivers/usbdux.c
new file mode 100644
index 000000000000..35138257be7f
--- /dev/null
+++ b/drivers/staging/comedi/drivers/usbdux.c
@@ -0,0 +1,2932 @@
+#define DRIVER_VERSION "v2.1"
+#define DRIVER_AUTHOR "Bernd Porr, BerndPorr@f2s.com"
+#define DRIVER_DESC "Stirling/ITL USB-DUX -- Bernd.Porr@f2s.com"
+/*
+   comedi/drivers/usbdux.c
+   Copyright (C) 2003-2007 Bernd Porr, Bernd.Porr@f2s.com
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation; either version 2 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program; if not, write to the Free Software
+   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ */
+/*
+Driver: usbdux
+Description: University of Stirling USB DAQ & INCITE Technology Limited
+Devices: [ITL] USB-DUX (usbdux.o)
+Author: Bernd Porr <BerndPorr@f2s.com>
+Updated: 25 Nov 2007
+Status: Testing
+Configuration options:
+  You have to upload firmware with the -i option. The
+  firmware is usually installed under /usr/share/usb or
+  /usr/local/share/usb or /lib/firmware.
+
+Connection scheme for the counter at the digital port:
+  0=/CLK0, 1=UP/DOWN0, 2=RESET0, 4=/CLK1, 5=UP/DOWN1, 6=RESET1.
+  The sampling rate of the counter is approximately 500Hz.
+
+Please note that under USB2.0 the length of the channel list determines
+the max sampling rate. If you sample only one channel you get 8kHz
+sampling rate. If you sample two channels you get 4kHz and so on.
+*/
+/*
+ * I must give credit here to Chris Baugher who
+ * wrote the driver for AT-MIO-16d. I used some parts of this
+ * driver. I also must give credits to David Brownell
+ * who supported me with the USB development.
+ *
+ * Bernd Porr
+ *
+ *
+ * Revision history:
+ * 0.94: D/A output should work now with any channel list combinations
+ * 0.95: .owner commented out for kernel vers below 2.4.19
+ *       sanity checks in ai/ao_cmd
+ * 0.96: trying to get it working with 2.6, moved all memory alloc to comedi's
+ *       attach final USB IDs
+ *       moved memory allocation completely to the corresponding comedi
+ *       functions firmware upload is by fxload and no longer by comedi (due to
+ *       enumeration)
+ * 0.97: USB IDs received, adjusted table
+ * 0.98: SMP, locking, memroy alloc: moved all usb memory alloc
+ *       to the usb subsystem and moved all comedi related memory
+ *       alloc to comedi.
+ *       | kernel | registration | usbdux-usb | usbdux-comedi | comedi |
+ * 0.99: USB 2.0: changed protocol to isochronous transfer
+ *                IRQ transfer is too buggy and too risky in 2.0
+ *                for the high speed ISO transfer is now a working version
+ *                available
+ * 0.99b: Increased the iso transfer buffer for high sp.to 10 buffers. Some VIA
+ *        chipsets miss out IRQs. Deeper buffering is needed.
+ * 1.00: full USB 2.0 support for the A/D converter. Now: max 8kHz sampling
+ *       rate.
+ *       Firmware vers 1.00 is needed for this.
+ *       Two 16 bit up/down/reset counter with a sampling rate of 1kHz
+ *       And loads of cleaning up, in particular streamlining the
+ *       bulk transfers.
+ * 1.1:  moved EP4 transfers to EP1 to make space for a PWM output on EP4
+ * 1.2:  added PWM suport via EP4
+ * 2.0:  PWM seems to be stable and is not interfering with the other functions
+ * 2.1:  changed PWM API
+ *
+ */
+
+/* generates loads of debug info */
+/* #define NOISY_DUX_DEBUGBUG */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/usb.h>
+#include <linux/smp_lock.h>
+#include <linux/fcntl.h>
+#include <linux/compiler.h>
+
+#include "../comedidev.h"
+
+#define BOARDNAME "usbdux"
+
+/* timeout for the USB-transfer */
+#define EZTIMEOUT 30
+
+/* constants for "firmware" upload and download */
+#define USBDUXSUB_FIRMWARE 0xA0
+#define VENDOR_DIR_IN  0xC0
+#define VENDOR_DIR_OUT 0x40
+
+/* internal adresses of the 8051 processor */
+#define USBDUXSUB_CPUCS 0xE600
+
+/*
+ * the minor device number, major is 180 only for debugging purposes and to
+ * upload special firmware (programming the eeprom etc) which is not compatible
+ * with the comedi framwork
+ */
+#define USBDUXSUB_MINOR 32
+
+/* max lenghth of the transfer-buffer for software upload */
+#define TB_LEN 0x2000
+
+/* Input endpoint number: ISO/IRQ */
+#define ISOINEP           6
+
+/* Output endpoint number: ISO/IRQ */
+#define ISOOUTEP          2
+
+/* This EP sends DUX commands to USBDUX */
+#define COMMAND_OUT_EP     1
+
+/* This EP receives the DUX commands from USBDUX */
+#define COMMAND_IN_EP        8
+
+/* Output endpoint for PWM */
+#define PWM_EP         4
+
+/* 300Hz max frequ under PWM */
+#define MIN_PWM_PERIOD  ((long)(1E9/300))
+
+/* Default PWM frequency */
+#define PWM_DEFAULT_PERIOD ((long)(1E9/100))
+
+/* Number of channels */
+#define NUMCHANNELS       8
+
+/* Size of one A/D value */
+#define SIZEADIN          ((sizeof(int16_t)))
+
+/*
+ * Size of the input-buffer IN BYTES
+ * Always multiple of 8 for 8 microframes which is needed in the highspeed mode
+ */
+#define SIZEINBUF         ((8*SIZEADIN))
+
+/* 16 bytes. */
+#define SIZEINSNBUF       16
+
+/* Number of DA channels */
+#define NUMOUTCHANNELS    8
+
+/* size of one value for the D/A converter: channel and value */
+#define SIZEDAOUT          ((sizeof(int8_t)+sizeof(int16_t)))
+
+/*
+ * Size of the output-buffer in bytes
+ * Actually only the first 4 triplets are used but for the
+ * high speed mode we need to pad it to 8 (microframes).
+ */
+#define SIZEOUTBUF         ((8*SIZEDAOUT))
+
+/*
+ * Size of the buffer for the dux commands: just now max size is determined
+ * by the analogue out + command byte + panic bytes...
+ */
+#define SIZEOFDUXBUFFER    ((8*SIZEDAOUT+2))
+
+/* Number of in-URBs which receive the data: min=2 */
+#define NUMOFINBUFFERSFULL     5
+
+/* Number of out-URBs which send the data: min=2 */
+#define NUMOFOUTBUFFERSFULL    5
+
+/* Number of in-URBs which receive the data: min=5 */
+/* must have more buffers due to buggy USB ctr */
+#define NUMOFINBUFFERSHIGH     10
+
+/* Number of out-URBs which send the data: min=5 */
+/* must have more buffers due to buggy USB ctr */
+#define NUMOFOUTBUFFERSHIGH    10
+
+/* Total number of usbdux devices */
+#define NUMUSBDUX             16
+
+/* Analogue in subdevice */
+#define SUBDEV_AD             0
+
+/* Analogue out subdevice */
+#define SUBDEV_DA             1
+
+/* Digital I/O */
+#define SUBDEV_DIO            2
+
+/* counter */
+#define SUBDEV_COUNTER        3
+
+/* timer aka pwm output */
+#define SUBDEV_PWM            4
+
+/* number of retries to get the right dux command */
+#define RETRIES 10
+
+/**************************************************/
+/* comedi constants */
+static const comedi_lrange range_usbdux_ai_range = { 4, {
+			BIP_RANGE(4.096),
+			BIP_RANGE(4.096 / 2),
+			UNI_RANGE(4.096),
+			UNI_RANGE(4.096 / 2)
+	}
+};
+
+static const comedi_lrange range_usbdux_ao_range = { 2, {
+			BIP_RANGE(4.096),
+			UNI_RANGE(4.096),
+	}
+};
+
+/*
+ * private structure of one subdevice
+ */
+
+/*
+ * This is the structure which holds all the data of
+ * this driver one sub device just now: A/D
+ */
+struct usbduxsub {
+	/* attached? */
+	int attached;
+	/* is it associated with a subdevice? */
+	int probed;
+	/* pointer to the usb-device */
+	struct usb_device *usbdev;
+	/* actual number of in-buffers */
+	int numOfInBuffers;
+	/* actual number of out-buffers */
+	int numOfOutBuffers;
+	/* ISO-transfer handling: buffers */
+	struct urb **urbIn;
+	struct urb **urbOut;
+	/* pwm-transfer handling */
+	struct urb *urbPwm;
+	/* PWM period */
+	lsampl_t pwmPeriod;
+	/* PWM internal delay for the GPIF in the FX2 */
+	int8_t pwmDelay;
+	/* size of the PWM buffer which holds the bit pattern */
+	int sizePwmBuf;
+	/* input buffer for the ISO-transfer */
+	int16_t *inBuffer;
+	/* input buffer for single insn */
+	int16_t *insnBuffer;
+	/* output buffer for single DA outputs */
+	int16_t *outBuffer;
+	/* interface number */
+	int ifnum;
+	/* interface structure in 2.6 */
+	struct usb_interface *interface;
+	/* comedi device for the interrupt context */
+	comedi_device *comedidev;
+	/* is it USB_SPEED_HIGH or not? */
+	short int high_speed;
+	/* asynchronous command is running */
+	short int ai_cmd_running;
+	short int ao_cmd_running;
+	/* pwm is running */
+	short int pwm_cmd_running;
+	/* continous aquisition */
+	short int ai_continous;
+	short int ao_continous;
+	/* number of samples to aquire */
+	int ai_sample_count;
+	int ao_sample_count;
+	/* time between samples in units of the timer */
+	unsigned int ai_timer;
+	unsigned int ao_timer;
+	/* counter between aquisitions */
+	unsigned int ai_counter;
+	unsigned int ao_counter;
+	/* interval in frames/uframes */
+	unsigned int ai_interval;
+	/* D/A commands */
+	int8_t *dac_commands;
+	/* commands */
+	int8_t *dux_commands;
+	struct semaphore sem;
+};
+
+/*
+ * The pointer to the private usb-data of the driver is also the private data
+ * for the comedi-device.  This has to be global as the usb subsystem needs
+ * global variables. The other reason is that this structure must be there
+ * _before_ any comedi command is issued. The usb subsystem must be initialised
+ * before comedi can access it.
+ */
+static struct usbduxsub usbduxsub[NUMUSBDUX];
+
+static DECLARE_MUTEX(start_stop_sem);
+
+/*
+ * Stops the data acquision
+ * It should be safe to call this function from any context
+ */
+static int usbduxsub_unlink_InURBs(struct usbduxsub *usbduxsub_tmp)
+{
+	int i = 0;
+	int err = 0;
+
+	if (usbduxsub_tmp && usbduxsub_tmp->urbIn) {
+		for (i = 0; i < usbduxsub_tmp->numOfInBuffers; i++) {
+			if (usbduxsub_tmp->urbIn[i]) {
+				/* We wait here until all transfers have been
+				 * cancelled. */
+				usb_kill_urb(usbduxsub_tmp->urbIn[i]);
+			}
+			dev_dbg(&usbduxsub_tmp->interface->dev,
+				"comedi: usbdux: unlinked InURB %d, err=%d\n",
+				i, err);
+		}
+	}
+	return err;
+}
+
+/*
+ * This will stop a running acquisition operation
+ * Is called from within this driver from both the
+ * interrupt context and from comedi
+ */
+static int usbdux_ai_stop(struct usbduxsub *this_usbduxsub, int do_unlink)
+{
+	int ret = 0;
+
+	if (!this_usbduxsub) {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi?: usbdux_ai_stop: this_usbduxsub=NULL!\n");
+		return -EFAULT;
+	}
+	dev_dbg(&this_usbduxsub->interface->dev, "comedi: usbdux_ai_stop\n");
+
+	if (do_unlink) {
+		/* stop aquistion */
+		ret = usbduxsub_unlink_InURBs(this_usbduxsub);
+	}
+
+	this_usbduxsub->ai_cmd_running = 0;
+
+	return ret;
+}
+
+/*
+ * This will cancel a running acquisition operation.
+ * This is called by comedi but never from inside the driver.
+ */
+static int usbdux_ai_cancel(comedi_device *dev, comedi_subdevice *s)
+{
+	struct usbduxsub *this_usbduxsub;
+	int res = 0;
+
+	/* force unlink of all urbs */
+	this_usbduxsub = dev->private;
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	dev_dbg(&this_usbduxsub->interface->dev, "comedi: usbdux_ai_cancel\n");
+
+	/* prevent other CPUs from submitting new commands just now */
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	/* unlink only if the urb really has been submitted */
+	res = usbdux_ai_stop(this_usbduxsub, this_usbduxsub->ai_cmd_running);
+	up(&this_usbduxsub->sem);
+	return res;
+}
+
+/* analogue IN - interrupt service routine */
+static void usbduxsub_ai_IsocIrq(struct urb *urb)
+{
+	int i, err, n;
+	struct usbduxsub *this_usbduxsub;
+	comedi_device *this_comedidev;
+	comedi_subdevice *s;
+
+	/* the context variable points to the subdevice */
+	this_comedidev = urb->context;
+	/* the private structure of the subdevice is struct usbduxsub */
+	this_usbduxsub = this_comedidev->private;
+	/* subdevice which is the AD converter */
+	s = this_comedidev->subdevices + SUBDEV_AD;
+
+	/* first we test if something unusual has just happened */
+	switch (urb->status) {
+	case 0:
+		/* copy the result in the transfer buffer */
+		memcpy(this_usbduxsub->inBuffer,
+			urb->transfer_buffer, SIZEINBUF);
+		break;
+	case -EILSEQ:
+		/* error in the ISOchronous data */
+		/* we don't copy the data into the transfer buffer */
+		/* and recycle the last data byte */
+		dev_dbg(&urb->dev->dev,
+			"comedi%d: usbdux: CRC error in ISO IN stream.\n",
+			this_usbduxsub->comedidev->minor);
+
+		break;
+
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+	case -ECONNABORTED:
+		/* happens after an unlink command */
+		if (this_usbduxsub->ai_cmd_running) {
+			/* we are still running a command */
+			/* tell this comedi */
+			s->async->events |= COMEDI_CB_EOA;
+			s->async->events |= COMEDI_CB_ERROR;
+			comedi_event(this_usbduxsub->comedidev, s);
+			/* stop the transfer w/o unlink */
+			usbdux_ai_stop(this_usbduxsub, 0);
+		}
+		return;
+
+	default:
+		/* a real error on the bus */
+		/* pass error to comedi if we are really running a command */
+		if (this_usbduxsub->ai_cmd_running) {
+			dev_err(&urb->dev->dev,
+				"Non-zero urb status received in ai intr "
+				"context: %d\n", urb->status);
+			s->async->events |= COMEDI_CB_EOA;
+			s->async->events |= COMEDI_CB_ERROR;
+			comedi_event(this_usbduxsub->comedidev, s);
+			/* don't do an unlink here */
+			usbdux_ai_stop(this_usbduxsub, 0);
+		}
+		return;
+	}
+
+	/*
+	 * at this point we are reasonably sure that nothing dodgy has happened
+	 * are we running a command?
+	 */
+	if (unlikely((!(this_usbduxsub->ai_cmd_running)))) {
+		/*
+		 * not running a command, do not continue execution if no
+		 * asynchronous command is running in particular not resubmit
+		 */
+		return;
+	}
+
+	urb->dev = this_usbduxsub->usbdev;
+
+	/* resubmit the urb */
+	err = usb_submit_urb(urb, GFP_ATOMIC);
+	if (unlikely(err < 0)) {
+		dev_err(&urb->dev->dev,
+			"comedi_: urb resubmit failed in int-context! err=%d\n",
+			err);
+		if (err == -EL2NSYNC)
+			dev_err(&urb->dev->dev,
+				"buggy USB host controller or bug in IRQ "
+				"handler!\n");
+		s->async->events |= COMEDI_CB_EOA;
+		s->async->events |= COMEDI_CB_ERROR;
+		comedi_event(this_usbduxsub->comedidev, s);
+		/* don't do an unlink here */
+		usbdux_ai_stop(this_usbduxsub, 0);
+		return;
+	}
+
+	this_usbduxsub->ai_counter--;
+	if (likely(this_usbduxsub->ai_counter > 0))
+		return;
+
+	/* timer zero, transfer measurements to comedi */
+	this_usbduxsub->ai_counter = this_usbduxsub->ai_timer;
+
+	/* test, if we transmit only a fixed number of samples */
+	if (!(this_usbduxsub->ai_continous)) {
+		/* not continous, fixed number of samples */
+		this_usbduxsub->ai_sample_count--;
+		/* all samples received? */
+		if (this_usbduxsub->ai_sample_count < 0) {
+			/* prevent a resubmit next time */
+			usbdux_ai_stop(this_usbduxsub, 0);
+			/* say comedi that the acquistion is over */
+			s->async->events |= COMEDI_CB_EOA;
+			comedi_event(this_usbduxsub->comedidev, s);
+			return;
+		}
+	}
+	/* 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++) {
+		/* transfer data */
+		if (CR_RANGE(s->async->cmd.chanlist[i]) <= 1) {
+			comedi_buf_put
+				(s->async,
+				le16_to_cpu(this_usbduxsub->
+					inBuffer[i]) ^ 0x800);
+		} else {
+			comedi_buf_put
+				(s->async,
+				le16_to_cpu(this_usbduxsub->inBuffer[i]));
+		}
+	}
+	/* tell comedi that data is there */
+	comedi_event(this_usbduxsub->comedidev, s);
+}
+
+static int usbduxsub_unlink_OutURBs(struct usbduxsub *usbduxsub_tmp)
+{
+	int i = 0;
+	int err = 0;
+
+	if (usbduxsub_tmp && usbduxsub_tmp->urbOut) {
+		for (i = 0; i < usbduxsub_tmp->numOfOutBuffers; i++) {
+			if (usbduxsub_tmp->urbOut[i])
+				usb_kill_urb(usbduxsub_tmp->urbOut[i]);
+
+			dev_dbg(&usbduxsub_tmp->interface->dev,
+				"comedi: usbdux: unlinked OutURB %d: res=%d\n",
+				i, err);
+		}
+	}
+	return err;
+}
+
+/* This will cancel a running acquisition operation
+ * in any context.
+ */
+static int usbdux_ao_stop(struct usbduxsub *this_usbduxsub, int do_unlink)
+{
+	int ret = 0;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+	dev_dbg(&this_usbduxsub->interface->dev, "comedi: usbdux_ao_cancel\n");
+
+	if (do_unlink)
+		ret = usbduxsub_unlink_OutURBs(this_usbduxsub);
+
+	this_usbduxsub->ao_cmd_running = 0;
+
+	return ret;
+}
+
+/* force unlink, is called by comedi */
+static int usbdux_ao_cancel(comedi_device *dev, comedi_subdevice *s)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int res = 0;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	/* prevent other CPUs from submitting a command just now */
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	/* unlink only if it is really running */
+	res = usbdux_ao_stop(this_usbduxsub, this_usbduxsub->ao_cmd_running);
+	up(&this_usbduxsub->sem);
+	return res;
+}
+
+static void usbduxsub_ao_IsocIrq(struct urb *urb)
+{
+	int i, ret;
+	int8_t *datap;
+	struct usbduxsub *this_usbduxsub;
+	comedi_device *this_comedidev;
+	comedi_subdevice *s;
+
+	/* the context variable points to the subdevice */
+	this_comedidev = urb->context;
+	/* the private structure of the subdevice is struct usbduxsub */
+	this_usbduxsub = this_comedidev->private;
+
+	s = this_comedidev->subdevices + SUBDEV_DA;
+
+	switch (urb->status) {
+	case 0:
+		/* success */
+		break;
+
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+	case -ECONNABORTED:
+		/* after an unlink command, unplug, ... etc */
+		/* no unlink needed here. Already shutting down. */
+		if (this_usbduxsub->ao_cmd_running) {
+			s->async->events |= COMEDI_CB_EOA;
+			comedi_event(this_usbduxsub->comedidev, s);
+			usbdux_ao_stop(this_usbduxsub, 0);
+		}
+		return;
+
+	default:
+		/* a real error */
+		if (this_usbduxsub->ao_cmd_running) {
+			dev_err(&urb->dev->dev,
+				"comedi_: Non-zero urb status received in ao "
+				"intr context: %d\n", urb->status);
+			s->async->events |= COMEDI_CB_ERROR;
+			s->async->events |= COMEDI_CB_EOA;
+			comedi_event(this_usbduxsub->comedidev, s);
+			/* we do an unlink if we are in the high speed mode */
+			usbdux_ao_stop(this_usbduxsub, 0);
+		}
+		return;
+	}
+
+	/* are we actually running? */
+	if (!(this_usbduxsub->ao_cmd_running))
+		return;
+
+	/* normal operation: executing a command in this subdevice */
+	this_usbduxsub->ao_counter--;
+	if (this_usbduxsub->ao_counter <= 0) {
+		/* timer zero */
+		this_usbduxsub->ao_counter = this_usbduxsub->ao_timer;
+
+		/* handle non continous aquisition */
+		if (!(this_usbduxsub->ao_continous)) {
+			/* fixed number of samples */
+			this_usbduxsub->ao_sample_count--;
+			if (this_usbduxsub->ao_sample_count < 0) {
+				/* all samples transmitted */
+				usbdux_ao_stop(this_usbduxsub, 0);
+				s->async->events |= COMEDI_CB_EOA;
+				comedi_event(this_usbduxsub->comedidev, s);
+				/* no resubmit of the urb */
+				return;
+			}
+		}
+		/* transmit data to the USB bus */
+		((uint8_t *) (urb->transfer_buffer))[0] =
+			s->async->cmd.chanlist_len;
+		for (i = 0; i < s->async->cmd.chanlist_len; i++) {
+			sampl_t temp;
+			if (i >= NUMOUTCHANNELS)
+				break;
+
+			/* pointer to the DA */
+			datap =
+			       (&(((int8_t *)urb->transfer_buffer)[i * 3 + 1]));
+			/* get the data from comedi */
+			ret = comedi_buf_get(s->async, &temp);
+			datap[0] = temp;
+			datap[1] = temp >> 8;
+			datap[2] = this_usbduxsub->dac_commands[i];
+			/* printk("data[0]=%x, data[1]=%x, data[2]=%x\n", */
+			/* datap[0],datap[1],datap[2]); */
+			if (ret < 0) {
+				dev_err(&urb->dev->dev,
+					"comedi: buffer underflow\n");
+				s->async->events |= COMEDI_CB_EOA;
+				s->async->events |= COMEDI_CB_OVERFLOW;
+			}
+			/* transmit data to comedi */
+			s->async->events |= COMEDI_CB_BLOCK;
+			comedi_event(this_usbduxsub->comedidev, s);
+		}
+	}
+	urb->transfer_buffer_length = SIZEOUTBUF;
+	urb->dev = this_usbduxsub->usbdev;
+	urb->status = 0;
+	if (this_usbduxsub->ao_cmd_running) {
+		if (this_usbduxsub->high_speed) {
+			/* uframes */
+			urb->interval = 8;
+		} else {
+			/* frames */
+			urb->interval = 1;
+		}
+		urb->number_of_packets = 1;
+		urb->iso_frame_desc[0].offset = 0;
+		urb->iso_frame_desc[0].length = SIZEOUTBUF;
+		urb->iso_frame_desc[0].status = 0;
+		ret = usb_submit_urb(urb, GFP_ATOMIC);
+		if (ret < 0) {
+			dev_err(&urb->dev->dev,
+				"comedi_: ao urb resubm failed in int-cont. "
+				"ret=%d", ret);
+			if (ret == EL2NSYNC)
+				dev_err(&urb->dev->dev,
+					"buggy USB host controller or bug in "
+					"IRQ handling!\n");
+
+			s->async->events |= COMEDI_CB_EOA;
+			s->async->events |= COMEDI_CB_ERROR;
+			comedi_event(this_usbduxsub->comedidev, s);
+			/* don't do an unlink here */
+			usbdux_ao_stop(this_usbduxsub, 0);
+		}
+	}
+}
+
+static int usbduxsub_start(struct usbduxsub *usbduxsub)
+{
+	int errcode = 0;
+	uint8_t local_transfer_buffer[16];
+
+	if (usbduxsub->probed) {
+		/* 7f92 to zero */
+		local_transfer_buffer[0] = 0;
+		errcode = usb_control_msg(usbduxsub->usbdev,
+			/* create a pipe for a control transfer */
+			usb_sndctrlpipe(usbduxsub->usbdev, 0),
+			/* bRequest, "Firmware" */
+			USBDUXSUB_FIRMWARE,
+			/* bmRequestType */
+			VENDOR_DIR_OUT,
+			/* Value */
+			USBDUXSUB_CPUCS,
+			/* Index */
+			0x0000,
+			/* address of the transfer buffer */
+			local_transfer_buffer,
+			/* Length */
+			1,
+			/* Timeout */
+			EZTIMEOUT);
+		if (errcode < 0) {
+			dev_err(&usbduxsub->interface->dev,
+				"comedi_: control msg failed (start)\n");
+			return errcode;
+		}
+	}
+	return 0;
+}
+
+static int usbduxsub_stop(struct usbduxsub *usbduxsub)
+{
+	int errcode = 0;
+
+	uint8_t local_transfer_buffer[16];
+	if (usbduxsub->probed) {
+		/* 7f92 to one */
+		local_transfer_buffer[0] = 1;
+		errcode = usb_control_msg(usbduxsub->usbdev,
+			usb_sndctrlpipe(usbduxsub->usbdev, 0),
+			/* bRequest, "Firmware" */
+			USBDUXSUB_FIRMWARE,
+			/* bmRequestType */
+			VENDOR_DIR_OUT,
+			/* Value */
+			USBDUXSUB_CPUCS,
+			/* Index */
+			0x0000, local_transfer_buffer,
+			/* Length */
+			1,
+			/* Timeout */
+			EZTIMEOUT);
+		if (errcode < 0) {
+			dev_err(&usbduxsub->interface->dev,
+				"comedi_: control msg failed (stop)\n");
+			return errcode;
+		}
+	}
+	return 0;
+}
+
+static int usbduxsub_upload(struct usbduxsub *usbduxsub,
+			    uint8_t *local_transfer_buffer,
+			    unsigned int startAddr, unsigned int len)
+{
+	int errcode;
+
+	if (usbduxsub->probed) {
+		dev_dbg(&usbduxsub->interface->dev,
+			"comedi%d: usbdux: uploading %d bytes"
+			" to addr %d, first byte=%d.\n",
+			usbduxsub->comedidev->minor, len,
+			startAddr, local_transfer_buffer[0]);
+		errcode = usb_control_msg(usbduxsub->usbdev,
+			usb_sndctrlpipe(usbduxsub->usbdev, 0),
+			/* brequest, firmware */
+			USBDUXSUB_FIRMWARE,
+			/* bmRequestType */
+			VENDOR_DIR_OUT,
+			/* value */
+			startAddr,
+			/* index */
+			0x0000,
+			/* our local safe buffer */
+			local_transfer_buffer,
+			/* length */
+			len,
+			/* timeout */
+			EZTIMEOUT);
+		dev_dbg(&usbduxsub->interface->dev,
+			"comedi_: result=%d\n", errcode);
+		if (errcode < 0) {
+			dev_err(&usbduxsub->interface->dev,
+				"comedi_: upload failed\n");
+			return errcode;
+		}
+	} else {
+		/* no device on the bus for this index */
+		return -EFAULT;
+	}
+	return 0;
+}
+
+static int firmwareUpload(struct usbduxsub *usbduxsub, uint8_t *firmwareBinary,
+			  int sizeFirmware)
+{
+	int ret;
+
+	if (!firmwareBinary)
+		return 0;
+
+	ret = usbduxsub_stop(usbduxsub);
+	if (ret < 0) {
+		dev_err(&usbduxsub->interface->dev,
+			"comedi_: can not stop firmware\n");
+		return ret;
+	}
+	ret = usbduxsub_upload(usbduxsub, firmwareBinary, 0, sizeFirmware);
+	if (ret < 0) {
+		dev_err(&usbduxsub->interface->dev,
+			"comedi_: firmware upload failed\n");
+		return ret;
+	}
+	ret = usbduxsub_start(usbduxsub);
+	if (ret < 0) {
+		dev_err(&usbduxsub->interface->dev,
+			"comedi_: can not start firmware\n");
+		return ret;
+	}
+	return 0;
+}
+
+static int usbduxsub_submit_InURBs(struct usbduxsub *usbduxsub)
+{
+	int i, errFlag;
+
+	if (!usbduxsub)
+		return -EFAULT;
+
+	/* Submit all URBs and start the transfer on the bus */
+	for (i = 0; i < usbduxsub->numOfInBuffers; i++) {
+		/* in case of a resubmission after an unlink... */
+		usbduxsub->urbIn[i]->interval = usbduxsub->ai_interval;
+		usbduxsub->urbIn[i]->context = usbduxsub->comedidev;
+		usbduxsub->urbIn[i]->dev = usbduxsub->usbdev;
+		usbduxsub->urbIn[i]->status = 0;
+		usbduxsub->urbIn[i]->transfer_flags = URB_ISO_ASAP;
+		dev_dbg(&usbduxsub->interface->dev,
+			"comedi%d: submitting in-urb[%d]: %p,%p intv=%d\n",
+			usbduxsub->comedidev->minor, i,
+			(usbduxsub->urbIn[i]->context),
+			(usbduxsub->urbIn[i]->dev),
+			(usbduxsub->urbIn[i]->interval));
+		errFlag = usb_submit_urb(usbduxsub->urbIn[i], GFP_ATOMIC);
+		if (errFlag) {
+			dev_err(&usbduxsub->interface->dev,
+				"comedi_: ai: usb_submit_urb(%d) error %d\n",
+				i, errFlag);
+			return errFlag;
+		}
+	}
+	return 0;
+}
+
+static int usbduxsub_submit_OutURBs(struct usbduxsub *usbduxsub)
+{
+	int i, errFlag;
+
+	if (!usbduxsub)
+		return -EFAULT;
+
+	for (i = 0; i < usbduxsub->numOfOutBuffers; i++) {
+		dev_dbg(&usbduxsub->interface->dev,
+			"comedi_: submitting out-urb[%d]\n", i);
+		/* in case of a resubmission after an unlink... */
+		usbduxsub->urbOut[i]->context = usbduxsub->comedidev;
+		usbduxsub->urbOut[i]->dev = usbduxsub->usbdev;
+		usbduxsub->urbOut[i]->status = 0;
+		usbduxsub->urbOut[i]->transfer_flags = URB_ISO_ASAP;
+		errFlag = usb_submit_urb(usbduxsub->urbOut[i], GFP_ATOMIC);
+		if (errFlag) {
+			dev_err(&usbduxsub->interface->dev,
+				"comedi_: ao: usb_submit_urb(%d) error %d\n",
+				i, errFlag);
+			return errFlag;
+		}
+	}
+	return 0;
+}
+
+static int usbdux_ai_cmdtest(comedi_device *dev, comedi_subdevice *s,
+			     comedi_cmd *cmd)
+{
+	int err = 0, tmp, i;
+	unsigned int tmpTimer;
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!(this_usbduxsub->probed))
+		return -ENODEV;
+
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi%d: usbdux_ai_cmdtest\n", dev->minor);
+
+	/* make sure triggers are valid */
+	/* Only immediate triggers are allowed */
+	tmp = cmd->start_src;
+	cmd->start_src &= TRIG_NOW | TRIG_INT;
+	if (!cmd->start_src || tmp != cmd->start_src)
+		err++;
+
+	/* trigger should happen timed */
+	tmp = cmd->scan_begin_src;
+	/* start a new _scan_ with a timer */
+	cmd->scan_begin_src &= TRIG_TIMER;
+	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
+		err++;
+
+	/* scanning is continous */
+	tmp = cmd->convert_src;
+	cmd->convert_src &= TRIG_NOW;
+	if (!cmd->convert_src || tmp != cmd->convert_src)
+		err++;
+
+	/* issue a trigger when scan is finished and start a new scan */
+	tmp = cmd->scan_end_src;
+	cmd->scan_end_src &= TRIG_COUNT;
+	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
+		err++;
+
+	/* trigger at the end of count events or not, stop condition or not */
+	tmp = cmd->stop_src;
+	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
+	if (!cmd->stop_src || tmp != cmd->stop_src)
+		err++;
+
+	if (err)
+		return 1;
+
+	/*
+	 * step 2: make sure trigger sources are unique and mutually compatible
+	 * note that mutual compatiblity is not an issue here
+	 */
+	if (cmd->scan_begin_src != TRIG_FOLLOW &&
+		cmd->scan_begin_src != TRIG_EXT &&
+		cmd->scan_begin_src != TRIG_TIMER)
+		err++;
+	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
+		err++;
+
+	if (err)
+		return 2;
+
+	/* step 3: make sure arguments are trivially compatible */
+	if (cmd->start_arg != 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+
+	if (cmd->scan_begin_src == TRIG_FOLLOW) {
+		/* internal trigger */
+		if (cmd->scan_begin_arg != 0) {
+			cmd->scan_begin_arg = 0;
+			err++;
+		}
+	}
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		if (this_usbduxsub->high_speed) {
+			/*
+			 * In high speed mode microframes are possible.
+			 * However, during one microframe we can roughly
+			 * sample one channel. Thus, the more channels
+			 * are in the channel list the more time we need.
+			 */
+			i = 1;
+			/* find a power of 2 for the number of channels */
+			while (i < (cmd->chanlist_len))
+				i = i * 2;
+
+			if (cmd->scan_begin_arg < (1000000 / 8 * i)) {
+				cmd->scan_begin_arg = 1000000 / 8 * i;
+				err++;
+			}
+			/* now calc the real sampling rate with all the
+			 * rounding errors */
+			tmpTimer =
+				((unsigned int)(cmd->scan_begin_arg / 125000)) *
+				125000;
+			if (cmd->scan_begin_arg != tmpTimer) {
+				cmd->scan_begin_arg = tmpTimer;
+				err++;
+			}
+		} else {
+			/* full speed */
+			/* 1kHz scans every USB frame */
+			if (cmd->scan_begin_arg < 1000000) {
+				cmd->scan_begin_arg = 1000000;
+				err++;
+			}
+			/*
+			 * calc the real sampling rate with the rounding errors
+			 */
+			tmpTimer = ((unsigned int)(cmd->scan_begin_arg /
+					1000000)) * 1000000;
+			if (cmd->scan_begin_arg != tmpTimer) {
+				cmd->scan_begin_arg = tmpTimer;
+				err++;
+			}
+		}
+	}
+	/* the same argument */
+	if (cmd->scan_end_arg != cmd->chanlist_len) {
+		cmd->scan_end_arg = cmd->chanlist_len;
+		err++;
+	}
+
+	if (cmd->stop_src == TRIG_COUNT) {
+		/* any count is allowed */
+	} else {
+		/* TRIG_NONE */
+		if (cmd->stop_arg != 0) {
+			cmd->stop_arg = 0;
+			err++;
+		}
+	}
+
+	if (err)
+		return 3;
+
+	return 0;
+}
+
+/*
+ * creates the ADC command for the MAX1271
+ * range is the range value from comedi
+ */
+static int8_t create_adc_command(unsigned int chan, int range)
+{
+	int8_t p = (range <= 1);
+	int8_t r = ((range % 2) == 0);
+	return (chan << 4) | ((p == 1) << 2) | ((r == 1) << 3);
+}
+
+/* bulk transfers to usbdux */
+
+#define SENDADCOMMANDS            0
+#define SENDDACOMMANDS            1
+#define SENDDIOCONFIGCOMMAND      2
+#define SENDDIOBITSCOMMAND        3
+#define SENDSINGLEAD              4
+#define READCOUNTERCOMMAND        5
+#define WRITECOUNTERCOMMAND       6
+#define SENDPWMON                 7
+#define SENDPWMOFF                8
+
+static int send_dux_commands(struct usbduxsub *this_usbduxsub, int cmd_type)
+{
+	int result, nsent;
+
+	this_usbduxsub->dux_commands[0] = cmd_type;
+#ifdef NOISY_DUX_DEBUGBUG
+	printk(KERN_DEBUG "comedi%d: usbdux: dux_commands: ",
+		this_usbduxsub->comedidev->minor);
+	for (result = 0; result < SIZEOFDUXBUFFER; result++)
+		printk(" %02x", this_usbduxsub->dux_commands[result]);
+	printk("\n");
+#endif
+	result = usb_bulk_msg(this_usbduxsub->usbdev,
+			      usb_sndbulkpipe(this_usbduxsub->usbdev,
+					      COMMAND_OUT_EP),
+			      this_usbduxsub->dux_commands, SIZEOFDUXBUFFER,
+			      &nsent, 10);
+	if (result < 0)
+		dev_err(&this_usbduxsub->interface->dev, "comedi%d: "
+			"could not transmit dux_command to the usb-device, "
+			"err=%d\n", this_usbduxsub->comedidev->minor, result);
+
+	return result;
+}
+
+static int receive_dux_commands(struct usbduxsub *this_usbduxsub, int command)
+{
+	int result = (-EFAULT);
+	int nrec;
+	int i;
+
+	for (i = 0; i < RETRIES; i++) {
+		result = usb_bulk_msg(this_usbduxsub->usbdev,
+				      usb_rcvbulkpipe(this_usbduxsub->usbdev,
+						      COMMAND_IN_EP),
+				      this_usbduxsub->insnBuffer, SIZEINSNBUF,
+				      &nrec, 1);
+		if (result < 0) {
+			dev_err(&this_usbduxsub->interface->dev, "comedi%d: "
+				"insn: USB error %d while receiving DUX command"
+				"\n", this_usbduxsub->comedidev->minor, result);
+			return result;
+		}
+		if (le16_to_cpu(this_usbduxsub->insnBuffer[0]) == command)
+			return result;
+	}
+	/* this is only reached if the data has been requested a couple of
+	 * times */
+	dev_err(&this_usbduxsub->interface->dev, "comedi%d: insn: "
+		"wrong data returned from firmware: want cmd %d, got cmd %d.\n",
+		this_usbduxsub->comedidev->minor, command,
+		le16_to_cpu(this_usbduxsub->insnBuffer[0]));
+	return -EFAULT;
+}
+
+static int usbdux_ai_inttrig(comedi_device *dev, comedi_subdevice *s,
+			     unsigned int trignum)
+{
+	int ret;
+	struct usbduxsub *this_usbduxsub = dev->private;
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi%d: usbdux_ai_inttrig\n", dev->minor);
+
+	if (trignum != 0) {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi%d: usbdux_ai_inttrig: invalid trignum\n",
+			dev->minor);
+		up(&this_usbduxsub->sem);
+		return -EINVAL;
+	}
+	if (!(this_usbduxsub->ai_cmd_running)) {
+		this_usbduxsub->ai_cmd_running = 1;
+		ret = usbduxsub_submit_InURBs(this_usbduxsub);
+		if (ret < 0) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi%d: usbdux_ai_inttrig: "
+				"urbSubmit: err=%d\n", dev->minor, ret);
+			this_usbduxsub->ai_cmd_running = 0;
+			up(&this_usbduxsub->sem);
+			return ret;
+		}
+		s->async->inttrig = NULL;
+	} else {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi%d: ai_inttrig but acqu is already running\n",
+			dev->minor);
+	}
+	up(&this_usbduxsub->sem);
+	return 1;
+}
+
+static int usbdux_ai_cmd(comedi_device *dev, comedi_subdevice *s)
+{
+	comedi_cmd *cmd = &s->async->cmd;
+	unsigned int chan, range;
+	int i, ret;
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int result;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi%d: usbdux_ai_cmd\n", dev->minor);
+
+	/* block other CPUs from starting an ai_cmd */
+	down(&this_usbduxsub->sem);
+
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	if (this_usbduxsub->ai_cmd_running) {
+		dev_err(&this_usbduxsub->interface->dev, "comedi%d: "
+			"ai_cmd not possible. Another ai_cmd is running.\n",
+			dev->minor);
+		up(&this_usbduxsub->sem);
+		return -EBUSY;
+	}
+	/* set current channel of the running aquisition to zero */
+	s->async->cur_chan = 0;
+
+	this_usbduxsub->dux_commands[1] = cmd->chanlist_len;
+	for (i = 0; i < cmd->chanlist_len; ++i) {
+		chan = CR_CHAN(cmd->chanlist[i]);
+		range = CR_RANGE(cmd->chanlist[i]);
+		if (i >= NUMCHANNELS) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi%d: channel list too long\n",
+				dev->minor);
+			break;
+		}
+		this_usbduxsub->dux_commands[i + 2] =
+			create_adc_command(chan, range);
+	}
+
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi %d: sending commands to the usb device: size=%u\n",
+		dev->minor, NUMCHANNELS);
+
+	result = send_dux_commands(this_usbduxsub, SENDADCOMMANDS);
+	if (result < 0) {
+		up(&this_usbduxsub->sem);
+		return result;
+	}
+
+	if (this_usbduxsub->high_speed) {
+		/*
+		 * every channel gets a time window of 125us. Thus, if we
+		 * sample all 8 channels we need 1ms. If we sample only one
+		 * channel we need only 125us
+		 */
+		this_usbduxsub->ai_interval = 1;
+		/* find a power of 2 for the interval */
+		while ((this_usbduxsub->ai_interval) < (cmd->chanlist_len)) {
+			this_usbduxsub->ai_interval =
+				(this_usbduxsub->ai_interval) * 2;
+		}
+		this_usbduxsub->ai_timer = cmd->scan_begin_arg / (125000 *
+			(this_usbduxsub->ai_interval));
+	} else {
+		/* interval always 1ms */
+		this_usbduxsub->ai_interval = 1;
+		this_usbduxsub->ai_timer = cmd->scan_begin_arg / 1000000;
+	}
+	if (this_usbduxsub->ai_timer < 1) {
+		dev_err(&this_usbduxsub->interface->dev, "comedi%d: ai_cmd: "
+			"timer=%d, scan_begin_arg=%d. "
+			"Not properly tested by cmdtest?\n", dev->minor,
+			this_usbduxsub->ai_timer, cmd->scan_begin_arg);
+		up(&this_usbduxsub->sem);
+		return -EINVAL;
+	}
+	this_usbduxsub->ai_counter = this_usbduxsub->ai_timer;
+
+	if (cmd->stop_src == TRIG_COUNT) {
+		/* data arrives as one packet */
+		this_usbduxsub->ai_sample_count = cmd->stop_arg;
+		this_usbduxsub->ai_continous = 0;
+	} else {
+		/* continous aquisition */
+		this_usbduxsub->ai_continous = 1;
+		this_usbduxsub->ai_sample_count = 0;
+	}
+
+	if (cmd->start_src == TRIG_NOW) {
+		/* enable this acquisition operation */
+		this_usbduxsub->ai_cmd_running = 1;
+		ret = usbduxsub_submit_InURBs(this_usbduxsub);
+		if (ret < 0) {
+			this_usbduxsub->ai_cmd_running = 0;
+			/* fixme: unlink here?? */
+			up(&this_usbduxsub->sem);
+			return ret;
+		}
+		s->async->inttrig = NULL;
+	} else {
+		/* TRIG_INT */
+		/* don't enable the acquision operation */
+		/* wait for an internal signal */
+		s->async->inttrig = usbdux_ai_inttrig;
+	}
+	up(&this_usbduxsub->sem);
+	return 0;
+}
+
+/* Mode 0 is used to get a single conversion on demand */
+static int usbdux_ai_insn_read(comedi_device *dev, comedi_subdevice *s,
+			       comedi_insn *insn, lsampl_t *data)
+{
+	int i;
+	lsampl_t one = 0;
+	int chan, range;
+	int err;
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!this_usbduxsub)
+		return 0;
+
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi%d: ai_insn_read, insn->n=%d, insn->subdev=%d\n",
+		dev->minor, insn->n, insn->subdev);
+
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	if (this_usbduxsub->ai_cmd_running) {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi%d: ai_insn_read not possible. "
+			"Async Command is running.\n", dev->minor);
+		up(&this_usbduxsub->sem);
+		return 0;
+	}
+
+	/* sample one channel */
+	chan = CR_CHAN(insn->chanspec);
+	range = CR_RANGE(insn->chanspec);
+	/* set command for the first channel */
+	this_usbduxsub->dux_commands[1] = create_adc_command(chan, range);
+
+	/* adc commands */
+	err = send_dux_commands(this_usbduxsub, SENDSINGLEAD);
+	if (err < 0) {
+		up(&this_usbduxsub->sem);
+		return err;
+	}
+
+	for (i = 0; i < insn->n; i++) {
+		err = receive_dux_commands(this_usbduxsub, SENDSINGLEAD);
+		if (err < 0) {
+			up(&this_usbduxsub->sem);
+			return 0;
+		}
+		one = le16_to_cpu(this_usbduxsub->insnBuffer[1]);
+		if (CR_RANGE(insn->chanspec) <= 1)
+			one = one ^ 0x800;
+
+		data[i] = one;
+	}
+	up(&this_usbduxsub->sem);
+	return i;
+}
+
+/************************************/
+/* analog out */
+
+static int usbdux_ao_insn_read(comedi_device *dev, comedi_subdevice *s,
+			       comedi_insn *insn, lsampl_t *data)
+{
+	int i;
+	int chan = CR_CHAN(insn->chanspec);
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	for (i = 0; i < insn->n; i++)
+		data[i] = this_usbduxsub->outBuffer[chan];
+
+	up(&this_usbduxsub->sem);
+	return i;
+}
+
+static int usbdux_ao_insn_write(comedi_device *dev, comedi_subdevice *s,
+				comedi_insn *insn, lsampl_t *data)
+{
+	int i, err;
+	int chan = CR_CHAN(insn->chanspec);
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi%d: ao_insn_write\n", dev->minor);
+
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	if (this_usbduxsub->ao_cmd_running) {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi%d: ao_insn_write: "
+			"ERROR: asynchronous ao_cmd is running\n", dev->minor);
+		up(&this_usbduxsub->sem);
+		return 0;
+	}
+
+	for (i = 0; i < insn->n; i++) {
+		dev_dbg(&this_usbduxsub->interface->dev,
+			"comedi%d: ao_insn_write: data[chan=%d,i=%d]=%d\n",
+			dev->minor, chan, i, data[i]);
+
+		/* number of channels: 1 */
+		this_usbduxsub->dux_commands[1] = 1;
+		/* one 16 bit value */
+		*((int16_t *) (this_usbduxsub->dux_commands + 2)) =
+			cpu_to_le16(data[i]);
+		this_usbduxsub->outBuffer[chan] = data[i];
+		/* channel number */
+		this_usbduxsub->dux_commands[4] = (chan << 6);
+		err = send_dux_commands(this_usbduxsub, SENDDACOMMANDS);
+		if (err < 0) {
+			up(&this_usbduxsub->sem);
+			return err;
+		}
+	}
+	up(&this_usbduxsub->sem);
+
+	return i;
+}
+
+static int usbdux_ao_inttrig(comedi_device *dev, comedi_subdevice *s,
+			     unsigned int trignum)
+{
+	int ret;
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	if (trignum != 0) {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi%d: usbdux_ao_inttrig: invalid trignum\n",
+			dev->minor);
+		return -EINVAL;
+	}
+	if (!(this_usbduxsub->ao_cmd_running)) {
+		this_usbduxsub->ao_cmd_running = 1;
+		ret = usbduxsub_submit_OutURBs(this_usbduxsub);
+		if (ret < 0) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi%d: usbdux_ao_inttrig: submitURB: "
+				"err=%d\n", dev->minor, ret);
+			this_usbduxsub->ao_cmd_running = 0;
+			up(&this_usbduxsub->sem);
+			return ret;
+		}
+		s->async->inttrig = NULL;
+	} else {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi%d: ao_inttrig but acqu is already running.\n",
+			dev->minor);
+	}
+	up(&this_usbduxsub->sem);
+	return 1;
+}
+
+static int usbdux_ao_cmdtest(comedi_device *dev, comedi_subdevice *s,
+			     comedi_cmd *cmd)
+{
+	int err = 0, tmp;
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	if (!(this_usbduxsub->probed))
+		return -ENODEV;
+
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi%d: usbdux_ao_cmdtest\n", dev->minor);
+
+	/* make sure triggers are valid */
+	/* Only immediate triggers are allowed */
+	tmp = cmd->start_src;
+	cmd->start_src &= TRIG_NOW | TRIG_INT;
+	if (!cmd->start_src || tmp != cmd->start_src)
+		err++;
+
+	/* trigger should happen timed */
+	tmp = cmd->scan_begin_src;
+	/* just now we scan also in the high speed mode every frame */
+	/* this is due to ehci driver limitations */
+	if (0) {		/* (this_usbduxsub->high_speed) */
+		/* start immidiately a new scan */
+		/* the sampling rate is set by the coversion rate */
+		cmd->scan_begin_src &= TRIG_FOLLOW;
+	} else {
+		/* start a new scan (output at once) with a timer */
+		cmd->scan_begin_src &= TRIG_TIMER;
+	}
+	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
+		err++;
+
+	/* scanning is continous */
+	tmp = cmd->convert_src;
+	/* we always output at 1kHz just now all channels at once */
+	if (0) {		/* (this_usbduxsub->high_speed) */
+		/*
+		 * in usb-2.0 only one conversion it tranmitted but with 8kHz/n
+		 */
+		cmd->convert_src &= TRIG_TIMER;
+	} else {
+		/* all conversion events happen simultaneously with a rate of
+		 * 1kHz/n */
+		cmd->convert_src &= TRIG_NOW;
+	}
+	if (!cmd->convert_src || tmp != cmd->convert_src)
+		err++;
+
+	/* issue a trigger when scan is finished and start a new scan */
+	tmp = cmd->scan_end_src;
+	cmd->scan_end_src &= TRIG_COUNT;
+	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
+		err++;
+
+	/* trigger at the end of count events or not, stop condition or not */
+	tmp = cmd->stop_src;
+	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
+	if (!cmd->stop_src || tmp != cmd->stop_src)
+		err++;
+
+	if (err)
+		return 1;
+
+	/*
+	 * step 2: make sure trigger sources are unique and mutually compatible
+	 * note that mutual compatiblity is not an issue here
+	 */
+	if (cmd->scan_begin_src != TRIG_FOLLOW &&
+		cmd->scan_begin_src != TRIG_EXT &&
+		cmd->scan_begin_src != TRIG_TIMER)
+		err++;
+	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
+		err++;
+
+	if (err)
+		return 2;
+
+	/* step 3: make sure arguments are trivially compatible */
+
+	if (cmd->start_arg != 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+
+	if (cmd->scan_begin_src == TRIG_FOLLOW) {
+		/* internal trigger */
+		if (cmd->scan_begin_arg != 0) {
+			cmd->scan_begin_arg = 0;
+			err++;
+		}
+	}
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		/* timer */
+		if (cmd->scan_begin_arg < 1000000) {
+			cmd->scan_begin_arg = 1000000;
+			err++;
+		}
+	}
+	/* not used now, is for later use */
+	if (cmd->convert_src == TRIG_TIMER) {
+		if (cmd->convert_arg < 125000) {
+			cmd->convert_arg = 125000;
+			err++;
+		}
+	}
+
+	/* the same argument */
+	if (cmd->scan_end_arg != cmd->chanlist_len) {
+		cmd->scan_end_arg = cmd->chanlist_len;
+		err++;
+	}
+
+	if (cmd->stop_src == TRIG_COUNT) {
+		/* any count is allowed */
+	} else {
+		/* TRIG_NONE */
+		if (cmd->stop_arg != 0) {
+			cmd->stop_arg = 0;
+			err++;
+		}
+	}
+
+	dev_dbg(&this_usbduxsub->interface->dev, "comedi%d: err=%d, "
+		"scan_begin_src=%d, scan_begin_arg=%d, convert_src=%d, "
+		"convert_arg=%d\n", dev->minor, err, cmd->scan_begin_src,
+		cmd->scan_begin_arg, cmd->convert_src, cmd->convert_arg);
+
+	if (err)
+		return 3;
+
+	return 0;
+}
+
+static int usbdux_ao_cmd(comedi_device *dev, comedi_subdevice *s)
+{
+	comedi_cmd *cmd = &s->async->cmd;
+	unsigned int chan, gain;
+	int i, ret;
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi%d: %s\n", dev->minor, __func__);
+
+	/* set current channel of the running aquisition to zero */
+	s->async->cur_chan = 0;
+	for (i = 0; i < cmd->chanlist_len; ++i) {
+		chan = CR_CHAN(cmd->chanlist[i]);
+		gain = CR_RANGE(cmd->chanlist[i]);
+		if (i >= NUMOUTCHANNELS) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi%d: %s: channel list too long\n",
+				dev->minor, __func__);
+			break;
+		}
+		this_usbduxsub->dac_commands[i] = (chan << 6);
+		dev_dbg(&this_usbduxsub->interface->dev,
+			"comedi%d: dac command for ch %d is %x\n",
+			dev->minor, i, this_usbduxsub->dac_commands[i]);
+	}
+
+	/* we count in steps of 1ms (125us) */
+	/* 125us mode not used yet */
+	if (0) {		/* (this_usbduxsub->high_speed) */
+		/* 125us */
+		/* timing of the conversion itself: every 125 us */
+		this_usbduxsub->ao_timer = cmd->convert_arg / 125000;
+	} else {
+		/* 1ms */
+		/* timing of the scan: we get all channels at once */
+		this_usbduxsub->ao_timer = cmd->scan_begin_arg / 1000000;
+		dev_dbg(&this_usbduxsub->interface->dev,
+			"comedi%d: scan_begin_src=%d, scan_begin_arg=%d, "
+			"convert_src=%d, convert_arg=%d\n", dev->minor,
+			cmd->scan_begin_src, cmd->scan_begin_arg,
+			cmd->convert_src, cmd->convert_arg);
+		dev_dbg(&this_usbduxsub->interface->dev,
+			"comedi%d: ao_timer=%d (ms)\n",
+			dev->minor, this_usbduxsub->ao_timer);
+		if (this_usbduxsub->ao_timer < 1) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi%d: usbdux: ao_timer=%d, "
+				"scan_begin_arg=%d. "
+				"Not properly tested by cmdtest?\n",
+				dev->minor, this_usbduxsub->ao_timer,
+				cmd->scan_begin_arg);
+			up(&this_usbduxsub->sem);
+			return -EINVAL;
+		}
+	}
+	this_usbduxsub->ao_counter = this_usbduxsub->ao_timer;
+
+	if (cmd->stop_src == TRIG_COUNT) {
+		/* not continous */
+		/* counter */
+		/* high speed also scans everything at once */
+		if (0) {	/* (this_usbduxsub->high_speed) */
+			this_usbduxsub->ao_sample_count =
+				(cmd->stop_arg) * (cmd->scan_end_arg);
+		} else {
+			/* there's no scan as the scan has been */
+			/* perf inside the FX2 */
+			/* data arrives as one packet */
+			this_usbduxsub->ao_sample_count = cmd->stop_arg;
+		}
+		this_usbduxsub->ao_continous = 0;
+	} else {
+		/* continous aquisition */
+		this_usbduxsub->ao_continous = 1;
+		this_usbduxsub->ao_sample_count = 0;
+	}
+
+	if (cmd->start_src == TRIG_NOW) {
+		/* enable this acquisition operation */
+		this_usbduxsub->ao_cmd_running = 1;
+		ret = usbduxsub_submit_OutURBs(this_usbduxsub);
+		if (ret < 0) {
+			this_usbduxsub->ao_cmd_running = 0;
+			/* fixme: unlink here?? */
+			up(&this_usbduxsub->sem);
+			return ret;
+		}
+		s->async->inttrig = NULL;
+	} else {
+		/* TRIG_INT */
+		/* submit the urbs later */
+		/* wait for an internal signal */
+		s->async->inttrig = usbdux_ao_inttrig;
+	}
+
+	up(&this_usbduxsub->sem);
+	return 0;
+}
+
+static int usbdux_dio_insn_config(comedi_device *dev, comedi_subdevice *s,
+				  comedi_insn *insn, lsampl_t *data)
+{
+	int chan = CR_CHAN(insn->chanspec);
+
+	/* The input or output configuration of each digital line is
+	 * configured by a special insn_config instruction.  chanspec
+	 * contains the channel to be changed, and data[0] contains the
+	 * value COMEDI_INPUT or COMEDI_OUTPUT. */
+
+	switch (data[0]) {
+	case INSN_CONFIG_DIO_OUTPUT:
+		s->io_bits |= 1 << chan;	/* 1 means Out */
+		break;
+	case INSN_CONFIG_DIO_INPUT:
+		s->io_bits &= ~(1 << chan);
+		break;
+	case INSN_CONFIG_DIO_QUERY:
+		data[1] =
+			(s->
+			io_bits & (1 << chan)) ? COMEDI_OUTPUT : COMEDI_INPUT;
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+	/* we don't tell the firmware here as it would take 8 frames */
+	/* to submit the information. We do it in the insn_bits. */
+	return insn->n;
+}
+
+static int usbdux_dio_insn_bits(comedi_device *dev, comedi_subdevice *s,
+				comedi_insn *insn, lsampl_t *data)
+{
+
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int err;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+
+	if (insn->n != 2)
+		return -EINVAL;
+
+	down(&this_usbduxsub->sem);
+
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+
+	/* The insn data is a mask in data[0] and the new data
+	 * in data[1], each channel cooresponding to a bit. */
+	s->state &= ~data[0];
+	s->state |= data[0] & data[1];
+	this_usbduxsub->dux_commands[1] = s->io_bits;
+	this_usbduxsub->dux_commands[2] = s->state;
+
+	/* This command also tells the firmware to return */
+	/* the digital input lines */
+	err = send_dux_commands(this_usbduxsub, SENDDIOBITSCOMMAND);
+	if (err < 0) {
+		up(&this_usbduxsub->sem);
+		return err;
+	}
+	err = receive_dux_commands(this_usbduxsub, SENDDIOBITSCOMMAND);
+	if (err < 0) {
+		up(&this_usbduxsub->sem);
+		return err;
+	}
+
+	data[1] = le16_to_cpu(this_usbduxsub->insnBuffer[1]);
+	up(&this_usbduxsub->sem);
+	return 2;
+}
+
+/* reads the 4 counters, only two are used just now */
+static int usbdux_counter_read(comedi_device *dev, comedi_subdevice *s,
+			       comedi_insn *insn, lsampl_t *data)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int chan = insn->chanspec;
+	int err;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	down(&this_usbduxsub->sem);
+
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+
+	err = send_dux_commands(this_usbduxsub, READCOUNTERCOMMAND);
+	if (err < 0) {
+		up(&this_usbduxsub->sem);
+		return err;
+	}
+
+	err = receive_dux_commands(this_usbduxsub, READCOUNTERCOMMAND);
+	if (err < 0) {
+		up(&this_usbduxsub->sem);
+		return err;
+	}
+
+	data[0] = le16_to_cpu(this_usbduxsub->insnBuffer[chan + 1]);
+	up(&this_usbduxsub->sem);
+	return 1;
+}
+
+static int usbdux_counter_write(comedi_device *dev, comedi_subdevice *s,
+				comedi_insn *insn, lsampl_t *data)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int err;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	down(&this_usbduxsub->sem);
+
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+
+	this_usbduxsub->dux_commands[1] = insn->chanspec;
+	*((int16_t *) (this_usbduxsub->dux_commands + 2)) = cpu_to_le16(*data);
+
+	err = send_dux_commands(this_usbduxsub, WRITECOUNTERCOMMAND);
+	if (err < 0) {
+		up(&this_usbduxsub->sem);
+		return err;
+	}
+
+	up(&this_usbduxsub->sem);
+
+	return 1;
+}
+
+static int usbdux_counter_config(comedi_device *dev, comedi_subdevice *s,
+				 comedi_insn *insn, lsampl_t *data)
+{
+	/* nothing to do so far */
+	return 2;
+}
+
+/***********************************/
+/* PWM */
+
+static int usbduxsub_unlink_PwmURBs(struct usbduxsub *usbduxsub_tmp)
+{
+	int err = 0;
+
+	if (usbduxsub_tmp && usbduxsub_tmp->urbPwm) {
+		if (usbduxsub_tmp->urbPwm)
+			usb_kill_urb(usbduxsub_tmp->urbPwm);
+		dev_dbg(&usbduxsub_tmp->interface->dev,
+			"comedi: unlinked PwmURB: res=%d\n", err);
+	}
+	return err;
+}
+
+/* This cancels a running acquisition operation
+ * in any context.
+ */
+static int usbdux_pwm_stop(struct usbduxsub *this_usbduxsub, int do_unlink)
+{
+	int ret = 0;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	dev_dbg(&this_usbduxsub->interface->dev, "comedi: %s\n", __func__);
+	if (do_unlink)
+		ret = usbduxsub_unlink_PwmURBs(this_usbduxsub);
+
+
+	this_usbduxsub->pwm_cmd_running = 0;
+
+	return ret;
+}
+
+/* force unlink - is called by comedi */
+static int usbdux_pwm_cancel(comedi_device *dev, comedi_subdevice *s)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int res = 0;
+
+	/* unlink only if it is really running */
+	res = usbdux_pwm_stop(this_usbduxsub, this_usbduxsub->pwm_cmd_running);
+
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi %d: sending pwm off command to the usb device.\n",
+		dev->minor);
+	res = send_dux_commands(this_usbduxsub, SENDPWMOFF);
+	if (res < 0)
+		return res;
+
+	return res;
+}
+
+static void usbduxsub_pwm_irq(struct urb *urb)
+{
+	int ret;
+	struct usbduxsub *this_usbduxsub;
+	comedi_device *this_comedidev;
+	comedi_subdevice *s;
+
+	/* printk(KERN_DEBUG "PWM: IRQ\n"); */
+
+	/* the context variable points to the subdevice */
+	this_comedidev = urb->context;
+	/* the private structure of the subdevice is struct usbduxsub */
+	this_usbduxsub = this_comedidev->private;
+
+	s = this_comedidev->subdevices + SUBDEV_DA;
+
+	switch (urb->status) {
+	case 0:
+		/* success */
+		break;
+
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+	case -ECONNABORTED:
+		/*
+		 * after an unlink command, unplug, ... etc
+		 * no unlink needed here. Already shutting down.
+		 */
+		if (this_usbduxsub->pwm_cmd_running)
+			usbdux_pwm_stop(this_usbduxsub, 0);
+
+		return;
+
+	default:
+		/* a real error */
+		if (this_usbduxsub->pwm_cmd_running) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi_: Non-zero urb status received in "
+				"pwm intr context: %d\n", urb->status);
+			usbdux_pwm_stop(this_usbduxsub, 0);
+		}
+		return;
+	}
+
+	/* are we actually running? */
+	if (!(this_usbduxsub->pwm_cmd_running))
+		return;
+
+	urb->transfer_buffer_length = this_usbduxsub->sizePwmBuf;
+	urb->dev = this_usbduxsub->usbdev;
+	urb->status = 0;
+	if (this_usbduxsub->pwm_cmd_running) {
+		ret = usb_submit_urb(urb, GFP_ATOMIC);
+		if (ret < 0) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi_: pwm urb resubm failed in int-cont. "
+				"ret=%d", ret);
+			if (ret == EL2NSYNC)
+				dev_err(&this_usbduxsub->interface->dev,
+					"buggy USB host controller or bug in "
+					"IRQ handling!\n");
+
+			/* don't do an unlink here */
+			usbdux_pwm_stop(this_usbduxsub, 0);
+		}
+	}
+}
+
+static int usbduxsub_submit_PwmURBs(struct usbduxsub *usbduxsub)
+{
+	int errFlag;
+
+	if (!usbduxsub)
+		return -EFAULT;
+
+	dev_dbg(&usbduxsub->interface->dev, "comedi_: submitting pwm-urb\n");
+
+	/* in case of a resubmission after an unlink... */
+	usb_fill_bulk_urb(usbduxsub->urbPwm,
+		usbduxsub->usbdev,
+		usb_sndbulkpipe(usbduxsub->usbdev, PWM_EP),
+		usbduxsub->urbPwm->transfer_buffer,
+		usbduxsub->sizePwmBuf, usbduxsub_pwm_irq, usbduxsub->comedidev);
+
+	errFlag = usb_submit_urb(usbduxsub->urbPwm, GFP_ATOMIC);
+	if (errFlag) {
+		dev_err(&usbduxsub->interface->dev,
+			"comedi_: usbdux: pwm: usb_submit_urb error %d\n",
+			errFlag);
+		return errFlag;
+	}
+	return 0;
+}
+
+static int usbdux_pwm_period(comedi_device *dev, comedi_subdevice *s,
+			     lsampl_t period)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int fx2delay = 255;
+
+	if (period < MIN_PWM_PERIOD) {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi%d: illegal period setting for pwm.\n",
+			dev->minor);
+		return -EAGAIN;
+	} else {
+		fx2delay = period / ((int)(6*512*(1.0/0.033))) - 6;
+		if (fx2delay > 255) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi%d: period %d for pwm is too low.\n",
+			       dev->minor, period);
+			return -EAGAIN;
+		}
+	}
+	this_usbduxsub->pwmDelay = fx2delay;
+	this_usbduxsub->pwmPeriod = period;
+	dev_dbg(&this_usbduxsub->interface->dev, "%s: frequ=%d, period=%d\n",
+		__func__, period, fx2delay);
+	return 0;
+}
+
+/* is called from insn so there's no need to do all the sanity checks */
+static int usbdux_pwm_start(comedi_device *dev, comedi_subdevice *s)
+{
+	int ret, i;
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	dev_dbg(&this_usbduxsub->interface->dev, "comedi%d: %s\n",
+		dev->minor, __func__);
+
+	if (this_usbduxsub->pwm_cmd_running) {
+		/* already running */
+		return 0;
+	}
+
+	this_usbduxsub->dux_commands[1] = ((int8_t) this_usbduxsub->pwmDelay);
+	ret = send_dux_commands(this_usbduxsub, SENDPWMON);
+	if (ret < 0)
+		return ret;
+
+	/* initalise the buffer */
+	for (i = 0; i < this_usbduxsub->sizePwmBuf; i++)
+		((char *)(this_usbduxsub->urbPwm->transfer_buffer))[i] = 0;
+
+	this_usbduxsub->pwm_cmd_running = 1;
+	ret = usbduxsub_submit_PwmURBs(this_usbduxsub);
+	if (ret < 0) {
+		this_usbduxsub->pwm_cmd_running = 0;
+		return ret;
+	}
+	return 0;
+}
+
+/* generates the bit pattern for PWM with the optional sign bit */
+static int usbdux_pwm_pattern(comedi_device *dev, comedi_subdevice *s,
+			      int channel, lsampl_t value, lsampl_t sign)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int i, szbuf;
+	char *pBuf;
+	char pwm_mask;
+	char sgn_mask;
+	char c;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	/* this is the DIO bit which carries the PWM data */
+	pwm_mask = (1 << channel);
+	/* this is the DIO bit which carries the optional direction bit */
+	sgn_mask = (16 << channel);
+	/* this is the buffer which will be filled with the with bit */
+	/* pattern for one period */
+	szbuf = this_usbduxsub->sizePwmBuf;
+	pBuf = (char *)(this_usbduxsub->urbPwm->transfer_buffer);
+	for (i = 0; i < szbuf; i++) {
+		c = *pBuf;
+		/* reset bits */
+		c = c & (~pwm_mask);
+		/* set the bit as long as the index is lower than the value */
+		if (i < value)
+			c = c | pwm_mask;
+		/* set the optional sign bit for a relay */
+		if (!sign) {
+			/* positive value */
+			c = c & (~sgn_mask);
+		} else {
+			/* negative value */
+			c = c | sgn_mask;
+		}
+		*(pBuf++) = c;
+	}
+	return 1;
+}
+
+static int usbdux_pwm_write(comedi_device *dev, comedi_subdevice *s,
+			    comedi_insn *insn, lsampl_t *data)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	if ((insn->n) != 1) {
+		/*
+		 * doesn't make sense to have more than one value here because
+		 * it would just overwrite the PWM buffer a couple of times
+		 */
+		return -EINVAL;
+	}
+
+	/*
+	 * the sign is set via a special INSN only, this gives us 8 bits for
+	 * normal operation
+	 * relay sign 0 by default
+	 */
+	return usbdux_pwm_pattern(dev, s, CR_CHAN(insn->chanspec),
+				  data[0], 0);
+}
+
+static int usbdux_pwm_read(comedi_device *x1, comedi_subdevice *x2,
+			   comedi_insn *x3, lsampl_t *x4)
+{
+	/* not needed */
+	return -EINVAL;
+};
+
+/* switches on/off PWM */
+static int usbdux_pwm_config(comedi_device *dev, comedi_subdevice *s,
+			     comedi_insn *insn, lsampl_t *data)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+	switch (data[0]) {
+	case INSN_CONFIG_ARM:
+		/* switch it on */
+		dev_dbg(&this_usbduxsub->interface->dev,
+			"comedi%d: %s: pwm on\n", dev->minor, __func__);
+		/*
+		 * if not zero the PWM is limited to a certain time which is
+		 * not supported here
+		 */
+		if (data[1] != 0)
+			return -EINVAL;
+		return usbdux_pwm_start(dev, s);
+	case INSN_CONFIG_DISARM:
+		dev_dbg(&this_usbduxsub->interface->dev,
+			"comedi%d: %s: pwm off\n", dev->minor, __func__);
+		return usbdux_pwm_cancel(dev, s);
+	case INSN_CONFIG_GET_PWM_STATUS:
+		/*
+		 * to check if the USB transmission has failed or in case PWM
+		 * was limited to n cycles to check if it has terminated
+		 */
+		data[1] = this_usbduxsub->pwm_cmd_running;
+		return 0;
+	case INSN_CONFIG_PWM_SET_PERIOD:
+		dev_dbg(&this_usbduxsub->interface->dev,
+			"comedi%d: %s: setting period\n", dev->minor, __func__);
+		return usbdux_pwm_period(dev, s, data[1]);
+	case INSN_CONFIG_PWM_GET_PERIOD:
+		data[1] = this_usbduxsub->pwmPeriod;
+		return 0;
+	case INSN_CONFIG_PWM_SET_H_BRIDGE:
+		/* value in the first byte and the sign in the second for a
+		   relay */
+		return usbdux_pwm_pattern(dev, s,
+					  /* the channel number */
+					  CR_CHAN(insn->chanspec),
+					  /* actual PWM data */
+					  data[1],
+					  /* just a sign */
+					  (data[2] != 0));
+	case INSN_CONFIG_PWM_GET_H_BRIDGE:
+		/* values are not kept in this driver, nothing to return here */
+		return -EINVAL;
+	}
+	return -EINVAL;
+}
+
+/* end of PWM */
+/*****************************************************************/
+
+static void tidy_up(struct usbduxsub *usbduxsub_tmp)
+{
+	int i;
+
+	if (!usbduxsub_tmp)
+		return;
+	dev_dbg(&usbduxsub_tmp->interface->dev, "comedi_: tiding up\n");
+
+	/* shows the usb subsystem that the driver is down */
+	if (usbduxsub_tmp->interface)
+		usb_set_intfdata(usbduxsub_tmp->interface, NULL);
+
+	usbduxsub_tmp->probed = 0;
+
+	if (usbduxsub_tmp->urbIn) {
+		if (usbduxsub_tmp->ai_cmd_running) {
+			usbduxsub_tmp->ai_cmd_running = 0;
+			usbduxsub_unlink_InURBs(usbduxsub_tmp);
+		}
+		for (i = 0; i < usbduxsub_tmp->numOfInBuffers; i++) {
+			kfree(usbduxsub_tmp->urbIn[i]->transfer_buffer);
+			usbduxsub_tmp->urbIn[i]->transfer_buffer = NULL;
+			usb_kill_urb(usbduxsub_tmp->urbIn[i]);
+			usb_free_urb(usbduxsub_tmp->urbIn[i]);
+			usbduxsub_tmp->urbIn[i] = NULL;
+		}
+		kfree(usbduxsub_tmp->urbIn);
+		usbduxsub_tmp->urbIn = NULL;
+	}
+	if (usbduxsub_tmp->urbOut) {
+		if (usbduxsub_tmp->ao_cmd_running) {
+			usbduxsub_tmp->ao_cmd_running = 0;
+			usbduxsub_unlink_OutURBs(usbduxsub_tmp);
+		}
+		for (i = 0; i < usbduxsub_tmp->numOfOutBuffers; i++) {
+			if (usbduxsub_tmp->urbOut[i]->transfer_buffer) {
+				kfree(usbduxsub_tmp->urbOut[i]->
+					transfer_buffer);
+				usbduxsub_tmp->urbOut[i]->transfer_buffer =
+					NULL;
+			}
+			if (usbduxsub_tmp->urbOut[i]) {
+				usb_kill_urb(usbduxsub_tmp->urbOut[i]);
+				usb_free_urb(usbduxsub_tmp->urbOut[i]);
+				usbduxsub_tmp->urbOut[i] = NULL;
+			}
+		}
+		kfree(usbduxsub_tmp->urbOut);
+		usbduxsub_tmp->urbOut = NULL;
+	}
+	if (usbduxsub_tmp->urbPwm) {
+		if (usbduxsub_tmp->pwm_cmd_running) {
+			usbduxsub_tmp->pwm_cmd_running = 0;
+			usbduxsub_unlink_PwmURBs(usbduxsub_tmp);
+		}
+		kfree(usbduxsub_tmp->urbPwm->transfer_buffer);
+		usbduxsub_tmp->urbPwm->transfer_buffer = NULL;
+		usb_kill_urb(usbduxsub_tmp->urbPwm);
+		usb_free_urb(usbduxsub_tmp->urbPwm);
+		usbduxsub_tmp->urbPwm = NULL;
+	}
+	kfree(usbduxsub_tmp->inBuffer);
+	usbduxsub_tmp->inBuffer = NULL;
+	kfree(usbduxsub_tmp->insnBuffer);
+	usbduxsub_tmp->insnBuffer = NULL;
+	kfree(usbduxsub_tmp->inBuffer);
+	usbduxsub_tmp->inBuffer = NULL;
+	kfree(usbduxsub_tmp->dac_commands);
+	usbduxsub_tmp->dac_commands = NULL;
+	kfree(usbduxsub_tmp->dux_commands);
+	usbduxsub_tmp->dux_commands = NULL;
+	usbduxsub_tmp->ai_cmd_running = 0;
+	usbduxsub_tmp->ao_cmd_running = 0;
+	usbduxsub_tmp->pwm_cmd_running = 0;
+}
+
+static unsigned hex2unsigned(char *h)
+{
+	unsigned hi, lo;
+
+	if (h[0] > '9')
+		hi = h[0] - 'A' + 0x0a;
+	else
+		hi = h[0] - '0';
+
+	if (h[1] > '9')
+		lo = h[1] - 'A' + 0x0a;
+	else
+		lo = h[1] - '0';
+
+	return hi * 0x10 + lo;
+}
+
+/* for FX2 */
+#define FIRMWARE_MAX_LEN 0x2000
+
+/* taken from David Brownell's fxload and adjusted for this driver */
+static int read_firmware(struct usbduxsub *usbduxsub, void *firmwarePtr,
+			 long size)
+{
+	struct device *dev = &usbduxsub->interface->dev;
+	int i = 0;
+	unsigned char *fp = (char *)firmwarePtr;
+	unsigned char *firmwareBinary = NULL;
+	int res = 0;
+	int maxAddr = 0;
+
+	firmwareBinary = kzalloc(FIRMWARE_MAX_LEN, GFP_KERNEL);
+	if (!firmwareBinary) {
+		dev_err(dev, "comedi_: mem alloc for firmware failed\n");
+		return -ENOMEM;
+	}
+
+	for (;;) {
+		char buf[256], *cp;
+		char type;
+		int len;
+		int idx, off;
+		int j = 0;
+
+		/* get one line */
+		while ((i < size) && (fp[i] != 13) && (fp[i] != 10)) {
+			buf[j] = fp[i];
+			i++;
+			j++;
+			if (j >= sizeof(buf)) {
+				dev_err(dev, "comedi_: bogus firmware file!\n");
+				return -1;
+			}
+		}
+		/* get rid of LF/CR/... */
+		while ((i < size) && ((fp[i] == 13) || (fp[i] == 10)
+				|| (fp[i] == 0))) {
+			i++;
+		}
+
+		buf[j] = 0;
+		/* dev_dbg(dev, "comedi_: buf=%s\n", buf); */
+
+		/*
+		 * EXTENSION:
+		 * "# comment-till-end-of-line", for copyrights etc
+		 */
+		if (buf[0] == '#')
+			continue;
+
+		if (buf[0] != ':') {
+			dev_err(dev, "comedi_: upload: not an ihex record: %s",
+				buf);
+			return -EFAULT;
+		}
+
+		/* Read the length field (up to 16 bytes) */
+		len = hex2unsigned(buf + 1);
+
+		/* Read the target offset */
+		off = (hex2unsigned(buf + 3) * 0x0100) + hex2unsigned(buf + 5);
+
+		if ((off + len) > maxAddr)
+			maxAddr = off + len;
+
+
+		if (maxAddr >= FIRMWARE_MAX_LEN) {
+			dev_err(dev, "comedi_: firmware upload goes "
+				"beyond FX2 RAM boundaries.\n");
+			return -EFAULT;
+		}
+		/* dev_dbg(dev, "comedi_: off=%x, len=%x:\n", off, len); */
+
+		/* Read the record type */
+		type = hex2unsigned(buf + 7);
+
+		/* If this is an EOF record, then make it so. */
+		if (type == 1)
+			break;
+
+
+		if (type != 0) {
+			dev_err(dev, "comedi_: unsupported record type: %u\n",
+				type);
+			return -EFAULT;
+		}
+
+		for (idx = 0, cp = buf + 9; idx < len; idx += 1, cp += 2) {
+			firmwareBinary[idx + off] = hex2unsigned(cp);
+			/*printk("%02x ",firmwareBinary[idx+off]); */
+		}
+		/*printk("\n"); */
+
+		if (i >= size) {
+			dev_err(dev, "comedi_: unexpected end of hex file\n");
+			break;
+		}
+
+	}
+	res = firmwareUpload(usbduxsub, firmwareBinary, maxAddr + 1);
+	kfree(firmwareBinary);
+	return res;
+}
+
+/* allocate memory for the urbs and initialise them */
+static int usbduxsub_probe(struct usb_interface *uinterf,
+			   const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(uinterf);
+	struct device *dev = &uinterf->dev;
+	int i;
+	int index;
+
+	dev_dbg(dev, "comedi_: usbdux_: "
+		"finding a free structure for the usb-device\n");
+
+	down(&start_stop_sem);
+	/* look for a free place in the usbdux array */
+	index = -1;
+	for (i = 0; i < NUMUSBDUX; i++) {
+		if (!(usbduxsub[i].probed)) {
+			index = i;
+			break;
+		}
+	}
+
+	/* no more space */
+	if (index == -1) {
+		dev_err(dev, "Too many usbdux-devices connected.\n");
+		up(&start_stop_sem);
+		return -EMFILE;
+	}
+	dev_dbg(dev, "comedi_: usbdux: "
+		"usbduxsub[%d] is ready to connect to comedi.\n", index);
+
+	init_MUTEX(&(usbduxsub[index].sem));
+	/* save a pointer to the usb device */
+	usbduxsub[index].usbdev = udev;
+
+	/* 2.6: save the interface itself */
+	usbduxsub[index].interface = uinterf;
+	/* get the interface number from the interface */
+	usbduxsub[index].ifnum = uinterf->altsetting->desc.bInterfaceNumber;
+	/* hand the private data over to the usb subsystem */
+	/* will be needed for disconnect */
+	usb_set_intfdata(uinterf, &(usbduxsub[index]));
+
+	dev_dbg(dev, "comedi_: usbdux: ifnum=%d\n", usbduxsub[index].ifnum);
+
+	/* test if it is high speed (USB 2.0) */
+	usbduxsub[index].high_speed =
+		(usbduxsub[index].usbdev->speed == USB_SPEED_HIGH);
+
+	/* create space for the commands of the DA converter */
+	usbduxsub[index].dac_commands = kzalloc(NUMOUTCHANNELS, GFP_KERNEL);
+	if (!usbduxsub[index].dac_commands) {
+		dev_err(dev, "comedi_: usbdux: "
+			"error alloc space for dac commands\n");
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	/* create space for the commands going to the usb device */
+	usbduxsub[index].dux_commands = kzalloc(SIZEOFDUXBUFFER, GFP_KERNEL);
+	if (!usbduxsub[index].dux_commands) {
+		dev_err(dev, "comedi_: usbdux: "
+			"error alloc space for dac commands\n");
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	/* create space for the in buffer and set it to zero */
+	usbduxsub[index].inBuffer = kzalloc(SIZEINBUF, GFP_KERNEL);
+	if (!(usbduxsub[index].inBuffer)) {
+		dev_err(dev, "comedi_: usbdux: "
+			"could not alloc space for inBuffer\n");
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	/* create space of the instruction buffer */
+	usbduxsub[index].insnBuffer = kzalloc(SIZEINSNBUF, GFP_KERNEL);
+	if (!(usbduxsub[index].insnBuffer)) {
+		dev_err(dev, "comedi_: usbdux: "
+			"could not alloc space for insnBuffer\n");
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	/* create space for the outbuffer */
+	usbduxsub[index].outBuffer = kzalloc(SIZEOUTBUF, GFP_KERNEL);
+	if (!(usbduxsub[index].outBuffer)) {
+		dev_err(dev, "comedi_: usbdux: "
+			"could not alloc space for outBuffer\n");
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	/* setting to alternate setting 3: enabling iso ep and bulk ep. */
+	i = usb_set_interface(usbduxsub[index].usbdev,
+			      usbduxsub[index].ifnum, 3);
+	if (i < 0) {
+		dev_err(dev, "comedi_: usbdux%d: "
+			"could not set alternate setting 3 in high speed.\n",
+			index);
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENODEV;
+	}
+	if (usbduxsub[index].high_speed)
+		usbduxsub[index].numOfInBuffers = NUMOFINBUFFERSHIGH;
+	else
+		usbduxsub[index].numOfInBuffers = NUMOFINBUFFERSFULL;
+
+	usbduxsub[index].urbIn =
+		kzalloc(sizeof(struct urb *) * usbduxsub[index].numOfInBuffers,
+		GFP_KERNEL);
+	if (!(usbduxsub[index].urbIn)) {
+		dev_err(dev, "comedi_: usbdux: Could not alloc. urbIn array\n");
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	for (i = 0; i < usbduxsub[index].numOfInBuffers; i++) {
+		/* one frame: 1ms */
+		usbduxsub[index].urbIn[i] = usb_alloc_urb(1, GFP_KERNEL);
+		if (usbduxsub[index].urbIn[i] == NULL) {
+			dev_err(dev, "comedi_: usbdux%d: "
+				"Could not alloc. urb(%d)\n", index, i);
+			tidy_up(&(usbduxsub[index]));
+			up(&start_stop_sem);
+			return -ENOMEM;
+		}
+		usbduxsub[index].urbIn[i]->dev = usbduxsub[index].usbdev;
+		/* will be filled later with a pointer to the comedi-device */
+		/* and ONLY then the urb should be submitted */
+		usbduxsub[index].urbIn[i]->context = NULL;
+		usbduxsub[index].urbIn[i]->pipe =
+			usb_rcvisocpipe(usbduxsub[index].usbdev, ISOINEP);
+		usbduxsub[index].urbIn[i]->transfer_flags = URB_ISO_ASAP;
+		usbduxsub[index].urbIn[i]->transfer_buffer =
+			kzalloc(SIZEINBUF, GFP_KERNEL);
+		if (!(usbduxsub[index].urbIn[i]->transfer_buffer)) {
+			dev_err(dev, "comedi_: usbdux%d: "
+				"could not alloc. transb.\n", index);
+			tidy_up(&(usbduxsub[index]));
+			up(&start_stop_sem);
+			return -ENOMEM;
+		}
+		usbduxsub[index].urbIn[i]->complete = usbduxsub_ai_IsocIrq;
+		usbduxsub[index].urbIn[i]->number_of_packets = 1;
+		usbduxsub[index].urbIn[i]->transfer_buffer_length = SIZEINBUF;
+		usbduxsub[index].urbIn[i]->iso_frame_desc[0].offset = 0;
+		usbduxsub[index].urbIn[i]->iso_frame_desc[0].length = SIZEINBUF;
+	}
+
+	/* out */
+	if (usbduxsub[index].high_speed)
+		usbduxsub[index].numOfOutBuffers = NUMOFOUTBUFFERSHIGH;
+	else
+		usbduxsub[index].numOfOutBuffers = NUMOFOUTBUFFERSFULL;
+
+	usbduxsub[index].urbOut =
+		kzalloc(sizeof(struct urb *) * usbduxsub[index].numOfOutBuffers,
+		GFP_KERNEL);
+	if (!(usbduxsub[index].urbOut)) {
+		dev_err(dev, "comedi_: usbdux: "
+			"Could not alloc. urbOut array\n");
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	for (i = 0; i < usbduxsub[index].numOfOutBuffers; i++) {
+		/* one frame: 1ms */
+		usbduxsub[index].urbOut[i] = usb_alloc_urb(1, GFP_KERNEL);
+		if (usbduxsub[index].urbOut[i] == NULL) {
+			dev_err(dev, "comedi_: usbdux%d: "
+				"Could not alloc. urb(%d)\n", index, i);
+			tidy_up(&(usbduxsub[index]));
+			up(&start_stop_sem);
+			return -ENOMEM;
+		}
+		usbduxsub[index].urbOut[i]->dev = usbduxsub[index].usbdev;
+		/* will be filled later with a pointer to the comedi-device */
+		/* and ONLY then the urb should be submitted */
+		usbduxsub[index].urbOut[i]->context = NULL;
+		usbduxsub[index].urbOut[i]->pipe =
+			usb_sndisocpipe(usbduxsub[index].usbdev, ISOOUTEP);
+		usbduxsub[index].urbOut[i]->transfer_flags = URB_ISO_ASAP;
+		usbduxsub[index].urbOut[i]->transfer_buffer =
+			kzalloc(SIZEOUTBUF, GFP_KERNEL);
+		if (!(usbduxsub[index].urbOut[i]->transfer_buffer)) {
+			dev_err(dev, "comedi_: usbdux%d: "
+				"could not alloc. transb.\n", index);
+			tidy_up(&(usbduxsub[index]));
+			up(&start_stop_sem);
+			return -ENOMEM;
+		}
+		usbduxsub[index].urbOut[i]->complete = usbduxsub_ao_IsocIrq;
+		usbduxsub[index].urbOut[i]->number_of_packets = 1;
+		usbduxsub[index].urbOut[i]->transfer_buffer_length = SIZEOUTBUF;
+		usbduxsub[index].urbOut[i]->iso_frame_desc[0].offset = 0;
+		usbduxsub[index].urbOut[i]->iso_frame_desc[0].length =
+			SIZEOUTBUF;
+		if (usbduxsub[index].high_speed) {
+			/* uframes */
+			usbduxsub[index].urbOut[i]->interval = 8;
+		} else {
+			/* frames */
+			usbduxsub[index].urbOut[i]->interval = 1;
+		}
+	}
+
+	/* pwm */
+	if (usbduxsub[index].high_speed) {
+		/* max bulk ep size in high speed */
+		usbduxsub[index].sizePwmBuf = 512;
+		usbduxsub[index].urbPwm = usb_alloc_urb(0, GFP_KERNEL);
+		if (usbduxsub[index].urbPwm == NULL) {
+			dev_err(dev, "comedi_: usbdux%d: "
+				"Could not alloc. pwm urb\n", index);
+			tidy_up(&(usbduxsub[index]));
+			up(&start_stop_sem);
+			return -ENOMEM;
+		}
+		usbduxsub[index].urbPwm->transfer_buffer =
+			kzalloc(usbduxsub[index].sizePwmBuf, GFP_KERNEL);
+		if (!(usbduxsub[index].urbPwm->transfer_buffer)) {
+			dev_err(dev, "comedi_: usbdux%d: "
+				"could not alloc. transb. for pwm\n", index);
+			tidy_up(&(usbduxsub[index]));
+			up(&start_stop_sem);
+			return -ENOMEM;
+		}
+	} else {
+		usbduxsub[index].urbPwm = NULL;
+		usbduxsub[index].sizePwmBuf = 0;
+	}
+
+	usbduxsub[index].ai_cmd_running = 0;
+	usbduxsub[index].ao_cmd_running = 0;
+	usbduxsub[index].pwm_cmd_running = 0;
+
+	/* we've reached the bottom of the function */
+	usbduxsub[index].probed = 1;
+	up(&start_stop_sem);
+	dev_info(dev, "comedi_: usbdux%d "
+		 "has been successfully initialised.\n", index);
+	/* success */
+	return 0;
+}
+
+static void usbduxsub_disconnect(struct usb_interface *intf)
+{
+	struct usbduxsub *usbduxsub_tmp = usb_get_intfdata(intf);
+	struct usb_device *udev = interface_to_usbdev(intf);
+
+	if (!usbduxsub_tmp) {
+		dev_err(&intf->dev,
+			"comedi_: disconnect called with null pointer.\n");
+		return;
+	}
+	if (usbduxsub_tmp->usbdev != udev) {
+		dev_err(&intf->dev,
+			"comedi_: BUG! called with wrong ptr!!!\n");
+		return;
+	}
+	down(&start_stop_sem);
+	down(&usbduxsub_tmp->sem);
+	tidy_up(usbduxsub_tmp);
+	up(&usbduxsub_tmp->sem);
+	up(&start_stop_sem);
+	dev_dbg(&intf->dev, "comedi_: disconnected from the usb\n");
+}
+
+/* is called when comedi-config is called */
+static int usbdux_attach(comedi_device *dev, comedi_devconfig *it)
+{
+	int ret;
+	int index;
+	int i;
+	struct usbduxsub *udev;
+
+	comedi_subdevice *s = NULL;
+	dev->private = NULL;
+
+	down(&start_stop_sem);
+	/* find a valid device which has been detected by the probe function of
+	 * the usb */
+	index = -1;
+	for (i = 0; i < NUMUSBDUX; i++) {
+		if ((usbduxsub[i].probed) && (!usbduxsub[i].attached)) {
+			index = i;
+			break;
+		}
+	}
+
+	if (index < 0) {
+		printk(KERN_ERR "comedi%d: usbdux: error: attach failed, no "
+		       "usbdux devs connected to the usb bus.\n", dev->minor);
+		up(&start_stop_sem);
+		return -ENODEV;
+	}
+
+	udev = &usbduxsub[index];
+	down(&udev->sem);
+	/* pointer back to the corresponding comedi device */
+	udev->comedidev = dev;
+
+	/* trying to upload the firmware into the chip */
+	if (comedi_aux_data(it->options, 0) &&
+		it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH]) {
+		read_firmware(udev, comedi_aux_data(it->options, 0),
+			      it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH]);
+	}
+
+	dev->board_name = BOARDNAME;
+
+	/* set number of subdevices */
+	if (udev->high_speed) {
+		/* with pwm */
+		dev->n_subdevices = 5;
+	} else {
+		/* without pwm */
+		dev->n_subdevices = 4;
+	}
+
+	/* allocate space for the subdevices */
+	ret = alloc_subdevices(dev, dev->n_subdevices);
+	if (ret < 0) {
+		dev_err(&udev->interface->dev,
+			"comedi%d: error alloc space for subdev\n", dev->minor);
+		up(&start_stop_sem);
+		return ret;
+	}
+
+	dev_info(&udev->interface->dev,
+		"comedi%d: usb-device %d is attached to comedi.\n",
+		dev->minor, index);
+	/* private structure is also simply the usb-structure */
+	dev->private = udev;
+
+	/* the first subdevice is the A/D converter */
+	s = dev->subdevices + SUBDEV_AD;
+	/* the URBs get the comedi subdevice */
+	/* which is responsible for reading */
+	/* this is the subdevice which reads data */
+	dev->read_subdev = s;
+	/* the subdevice receives as private structure the */
+	/* usb-structure */
+	s->private = NULL;
+	/* analog input */
+	s->type = COMEDI_SUBD_AI;
+	/* readable and ref is to ground */
+	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
+	/* 8 channels */
+	s->n_chan = 8;
+	/* length of the channellist */
+	s->len_chanlist = 8;
+	/* callback functions */
+	s->insn_read = usbdux_ai_insn_read;
+	s->do_cmdtest = usbdux_ai_cmdtest;
+	s->do_cmd = usbdux_ai_cmd;
+	s->cancel = usbdux_ai_cancel;
+	/* max value from the A/D converter (12bit) */
+	s->maxdata = 0xfff;
+	/* range table to convert to physical units */
+	s->range_table = (&range_usbdux_ai_range);
+
+	/* analog out */
+	s = dev->subdevices + SUBDEV_DA;
+	/* analog out */
+	s->type = COMEDI_SUBD_AO;
+	/* backward pointer */
+	dev->write_subdev = s;
+	/* the subdevice receives as private structure the */
+	/* usb-structure */
+	s->private = NULL;
+	/* are writable */
+	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_CMD_WRITE;
+	/* 4 channels */
+	s->n_chan = 4;
+	/* length of the channellist */
+	s->len_chanlist = 4;
+	/* 12 bit resolution */
+	s->maxdata = 0x0fff;
+	/* bipolar range */
+	s->range_table = (&range_usbdux_ao_range);
+	/* callback */
+	s->do_cmdtest = usbdux_ao_cmdtest;
+	s->do_cmd = usbdux_ao_cmd;
+	s->cancel = usbdux_ao_cancel;
+	s->insn_read = usbdux_ao_insn_read;
+	s->insn_write = usbdux_ao_insn_write;
+
+	/* digital I/O */
+	s = dev->subdevices + SUBDEV_DIO;
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 8;
+	s->maxdata = 1;
+	s->range_table = (&range_digital);
+	s->insn_bits = usbdux_dio_insn_bits;
+	s->insn_config = usbdux_dio_insn_config;
+	/* we don't use it */
+	s->private = NULL;
+
+	/* counter */
+	s = dev->subdevices + SUBDEV_COUNTER;
+	s->type = COMEDI_SUBD_COUNTER;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = 4;
+	s->maxdata = 0xFFFF;
+	s->insn_read = usbdux_counter_read;
+	s->insn_write = usbdux_counter_write;
+	s->insn_config = usbdux_counter_config;
+
+	if (udev->high_speed) {
+		/* timer / pwm */
+		s = dev->subdevices + SUBDEV_PWM;
+		s->type = COMEDI_SUBD_PWM;
+		s->subdev_flags = SDF_WRITABLE | SDF_PWM_HBRIDGE;
+		s->n_chan = 8;
+		/* this defines the max duty cycle resolution */
+		s->maxdata = udev->sizePwmBuf;
+		s->insn_write = usbdux_pwm_write;
+		s->insn_read = usbdux_pwm_read;
+		s->insn_config = usbdux_pwm_config;
+		usbdux_pwm_period(dev, s, PWM_DEFAULT_PERIOD);
+	}
+	/* finally decide that it's attached */
+	udev->attached = 1;
+
+	up(&udev->sem);
+
+	up(&start_stop_sem);
+
+	dev_info(&udev->interface->dev, "comedi%d: attached to usbdux.\n",
+		 dev->minor);
+
+	return 0;
+}
+
+static int usbdux_detach(comedi_device *dev)
+{
+	struct usbduxsub *usbduxsub_tmp;
+
+	if (!dev) {
+		printk(KERN_ERR
+			"comedi?: usbdux: detach without dev variable...\n");
+		return -EFAULT;
+	}
+
+	usbduxsub_tmp = dev->private;
+	if (!usbduxsub_tmp) {
+		printk(KERN_ERR
+			"comedi?: usbdux: detach without ptr to usbduxsub[]\n");
+		return -EFAULT;
+	}
+
+	dev_dbg(&usbduxsub_tmp->interface->dev, "comedi%d: detach usb device\n",
+		dev->minor);
+
+	down(&usbduxsub_tmp->sem);
+	/* Don't allow detach to free the private structure */
+	/* It's one entry of of usbduxsub[] */
+	dev->private = NULL;
+	usbduxsub_tmp->attached = 0;
+	usbduxsub_tmp->comedidev = NULL;
+	dev_dbg(&usbduxsub_tmp->interface->dev,
+		"comedi%d: detach: successfully removed\n", dev->minor);
+	up(&usbduxsub_tmp->sem);
+	return 0;
+}
+
+/* main driver struct */
+static comedi_driver driver_usbdux = {
+      .driver_name =	"usbdux",
+      .module =		THIS_MODULE,
+      .attach =		usbdux_attach,
+      .detach =		usbdux_detach,
+};
+
+static void init_usb_devices(void)
+{
+	int index;
+
+	/* all devices entries are invalid to begin with */
+	/* they will become valid by the probe function */
+	/* and then finally by the attach-function */
+	for (index = 0; index < NUMUSBDUX; index++) {
+		memset(&(usbduxsub[index]), 0x00, sizeof(usbduxsub[index]));
+		init_MUTEX(&(usbduxsub[index].sem));
+	}
+}
+
+/* Table with the USB-devices: just now only testing IDs */
+static struct usb_device_id usbduxsub_table[] = {
+	{USB_DEVICE(0x13d8, 0x0001) },
+	{USB_DEVICE(0x13d8, 0x0002) },
+	{}			/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, usbduxsub_table);
+
+/* The usbduxsub-driver */
+static struct usb_driver usbduxsub_driver = {
+      .name =		BOARDNAME,
+      .probe =		usbduxsub_probe,
+      .disconnect =	usbduxsub_disconnect,
+      .id_table =	usbduxsub_table,
+};
+
+/* Can't use the nice macro as I have also to initialise the USB */
+/* subsystem: */
+/* registering the usb-system _and_ the comedi-driver */
+static int init_usbdux(void)
+{
+	printk(KERN_INFO KBUILD_MODNAME ": "
+	       DRIVER_VERSION ":" DRIVER_DESC "\n");
+	init_usb_devices();
+	usb_register(&usbduxsub_driver);
+	comedi_driver_register(&driver_usbdux);
+	return 0;
+}
+
+/* deregistering the comedi driver and the usb-subsystem */
+static void exit_usbdux(void)
+{
+	comedi_driver_unregister(&driver_usbdux);
+	usb_deregister(&usbduxsub_driver);
+}
+
+module_init(init_usbdux);
+module_exit(exit_usbdux);
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/usbduxfast.c b/drivers/staging/comedi/drivers/usbduxfast.c
new file mode 100644
index 000000000000..3a00ff0cfc5a
--- /dev/null
+++ b/drivers/staging/comedi/drivers/usbduxfast.c
@@ -0,0 +1,1778 @@
+#define DRIVER_VERSION "v0.99a"
+#define DRIVER_AUTHOR "Bernd Porr, BerndPorr@f2s.com"
+#define DRIVER_DESC "USB-DUXfast, BerndPorr@f2s.com"
+/*
+   comedi/drivers/usbduxfast.c
+   Copyright (C) 2004 Bernd Porr, Bernd.Porr@f2s.com
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation; either version 2 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program; if not, write to the Free Software
+   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+
+/*
+Driver: usbduxfast
+Description: ITL USB-DUXfast
+Devices: [ITL] USB-DUX (usbduxfast.o)
+Author: Bernd Porr <BerndPorr@f2s.com>
+Updated: 04 Dec 2006
+Status: testing
+*/
+
+/*
+ * I must give credit here to Chris Baugher who
+ * wrote the driver for AT-MIO-16d. I used some parts of this
+ * driver. I also must give credits to David Brownell
+ * who supported me with the USB development.
+ *
+ * Bernd Porr
+ *
+ *
+ * Revision history:
+ * 0.9: Dropping the first data packet which seems to be from the last transfer.
+ *      Buffer overflows in the FX2 are handed over to comedi.
+ * 0.92: Dropping now 4 packets. The quad buffer has to be emptied.
+ *       Added insn command basically for testing. Sample rate is 1MHz/16ch=62.5kHz
+ * 0.99: Ian Abbott pointed out a bug which has been corrected. Thanks!
+ * 0.99a: added external trigger.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/usb.h>
+#include <linux/smp_lock.h>
+#include <linux/fcntl.h>
+#include <linux/compiler.h>
+#include "comedi_fc.h"
+#include "../comedidev.h"
+
+// (un)comment this if you want to have debug info.
+//#define CONFIG_COMEDI_DEBUG
+#undef  CONFIG_COMEDI_DEBUG
+
+#define BOARDNAME "usbduxfast"
+
+// timeout for the USB-transfer
+#define EZTIMEOUT 30
+
+// constants for "firmware" upload and download
+#define USBDUXFASTSUB_FIRMWARE 0xA0
+#define VENDOR_DIR_IN  0xC0
+#define VENDOR_DIR_OUT 0x40
+
+// internal adresses of the 8051 processor
+#define USBDUXFASTSUB_CPUCS 0xE600
+
+// max lenghth of the transfer-buffer for software upload
+#define TB_LEN 0x2000
+
+// Input endpoint number
+#define BULKINEP           6
+
+// Endpoint for the A/D channellist: bulk OUT
+#define CHANNELLISTEP     4
+
+// Number of channels
+#define NUMCHANNELS       32
+
+// size of the waveform descriptor
+#define WAVESIZE          0x20
+
+// Size of one A/D value
+#define SIZEADIN          ((sizeof(int16_t)))
+
+// Size of the input-buffer IN BYTES
+#define SIZEINBUF         512
+
+// 16 bytes.
+#define SIZEINSNBUF       512
+
+// Size of the buffer for the dux commands
+#define SIZEOFDUXBUFFER    256	// bytes
+
+// Number of in-URBs which receive the data: min=5
+#define NUMOFINBUFFERSHIGH     10
+
+// Total number of usbduxfast devices
+#define NUMUSBDUXFAST             16
+
+// Number of subdevices
+#define N_SUBDEVICES          1
+
+// Analogue in subdevice
+#define SUBDEV_AD             0
+
+// min delay steps for more than one channel
+// basically when the mux gives up. ;-)
+#define MIN_SAMPLING_PERIOD 9	// steps at 30MHz in the FX2
+
+// Max number of 1/30MHz delay steps:
+#define MAX_SAMPLING_PERIOD 500
+
+// Number of received packets to ignore before we start handing data over to comedi.
+// It's quad buffering and we have to ignore 4 packets.
+#define PACKETS_TO_IGNORE 4
+
+/////////////////////////////////////////////
+// comedi constants
+static const comedi_lrange range_usbduxfast_ai_range = { 2, {
+			BIP_RANGE(0.75),
+			BIP_RANGE(0.5),
+	}
+};
+
+/*
+ * private structure of one subdevice
+ */
+
+// This is the structure which holds all the data of this driver
+// one sub device just now: A/D
+typedef struct {
+	// attached?
+	int attached;
+	// is it associated with a subdevice?
+	int probed;
+	// pointer to the usb-device
+	struct usb_device *usbdev;
+	// BULK-transfer handling: urb
+	struct urb *urbIn;
+	int8_t *transfer_buffer;
+	// input buffer for single insn
+	int16_t *insnBuffer;
+	// interface number
+	int ifnum;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
+	// interface structure in 2.6
+	struct usb_interface *interface;
+#endif
+	// comedi device for the interrupt context
+	comedi_device *comedidev;
+	// asynchronous command is running
+	short int ai_cmd_running;
+	// continous aquisition
+	short int ai_continous;
+	// number of samples to aquire
+	long int ai_sample_count;
+	// commands
+	uint8_t *dux_commands;
+	// counter which ignores the first buffers
+	int ignore;
+	struct semaphore sem;
+} usbduxfastsub_t;
+
+// The pointer to the private usb-data of the driver
+// is also the private data for the comedi-device.
+// This has to be global as the usb subsystem needs
+// global variables. The other reason is that this
+// structure must be there _before_ any comedi
+// command is issued. The usb subsystem must be
+// initialised before comedi can access it.
+static usbduxfastsub_t usbduxfastsub[NUMUSBDUXFAST];
+
+static DECLARE_MUTEX(start_stop_sem);
+
+// bulk transfers to usbduxfast
+
+#define SENDADCOMMANDS            0
+#define SENDINITEP6               1
+
+static int send_dux_commands(usbduxfastsub_t * this_usbduxfastsub, int cmd_type)
+{
+	int result, nsent;
+	this_usbduxfastsub->dux_commands[0] = cmd_type;
+#ifdef CONFIG_COMEDI_DEBUG
+	int i;
+	printk("comedi%d: usbduxfast: dux_commands: ",
+		this_usbduxfastsub->comedidev->minor);
+	for (i = 0; i < SIZEOFDUXBUFFER; i++) {
+		printk(" %02x", this_usbduxfastsub->dux_commands[i]);
+	}
+	printk("\n");
+#endif
+	result = usb_bulk_msg(this_usbduxfastsub->usbdev,
+			      usb_sndbulkpipe(this_usbduxfastsub->usbdev,
+					      CHANNELLISTEP),
+			      this_usbduxfastsub->dux_commands, SIZEOFDUXBUFFER,
+			      &nsent, 10000);
+	if (result < 0) {
+		printk("comedi%d: could not transmit dux_commands to the usb-device, err=%d\n", this_usbduxfastsub->comedidev->minor, result);
+	}
+	return result;
+}
+
+// Stops the data acquision
+// It should be safe to call this function from any context
+static int usbduxfastsub_unlink_InURBs(usbduxfastsub_t * usbduxfastsub_tmp)
+{
+	int j = 0;
+	int err = 0;
+
+	if (usbduxfastsub_tmp && usbduxfastsub_tmp->urbIn) {
+		usbduxfastsub_tmp->ai_cmd_running = 0;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,8)
+		j = usb_unlink_urb(usbduxfastsub_tmp->urbIn);
+		if (j < 0) {
+			err = j;
+		}
+#else
+		// waits until a running transfer is over
+		usb_kill_urb(usbduxfastsub_tmp->urbIn);
+		j = 0;
+#endif
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi: usbduxfast: unlinked InURB: res=%d\n", j);
+#endif
+	return err;
+}
+
+/* This will stop a running acquisition operation */
+// Is called from within this driver from both the
+// interrupt context and from comedi
+static int usbduxfast_ai_stop(usbduxfastsub_t * this_usbduxfastsub,
+	int do_unlink)
+{
+	int ret = 0;
+
+	if (!this_usbduxfastsub) {
+		printk("comedi?: usbduxfast_ai_stop: this_usbduxfastsub=NULL!\n");
+		return -EFAULT;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi: usbduxfast_ai_stop\n");
+#endif
+
+	this_usbduxfastsub->ai_cmd_running = 0;
+
+	if (do_unlink) {
+		// stop aquistion
+		ret = usbduxfastsub_unlink_InURBs(this_usbduxfastsub);
+	}
+
+	return ret;
+}
+
+// This will cancel a running acquisition operation.
+// This is called by comedi but never from inside the
+// driver.
+static int usbduxfast_ai_cancel(comedi_device * dev, comedi_subdevice * s)
+{
+	usbduxfastsub_t *this_usbduxfastsub;
+	int res = 0;
+
+	// force unlink of all urbs
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi: usbduxfast_ai_cancel\n");
+#endif
+	this_usbduxfastsub = dev->private;
+	if (!this_usbduxfastsub) {
+		printk("comedi: usbduxfast_ai_cancel: this_usbduxfastsub=NULL\n");
+		return -EFAULT;
+	}
+	down(&this_usbduxfastsub->sem);
+	if (!(this_usbduxfastsub->probed)) {
+		up(&this_usbduxfastsub->sem);
+		return -ENODEV;
+	}
+	// unlink
+	res = usbduxfast_ai_stop(this_usbduxfastsub, 1);
+	up(&this_usbduxfastsub->sem);
+
+	return res;
+}
+
+// analogue IN
+// interrupt service routine
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
+static void usbduxfastsub_ai_Irq(struct urb *urb)
+#else
+static void usbduxfastsub_ai_Irq(struct urb *urb PT_REGS_ARG)
+#endif
+{
+	int n, err;
+	usbduxfastsub_t *this_usbduxfastsub;
+	comedi_device *this_comedidev;
+	comedi_subdevice *s;
+	uint16_t *p;
+
+	// sanity checks
+	// is the urb there?
+	if (!urb) {
+		printk("comedi_: usbduxfast_: ao int-handler called with urb=NULL!\n");
+		return;
+	}
+	// the context variable points to the subdevice
+	this_comedidev = urb->context;
+	if (!this_comedidev) {
+		printk("comedi_: usbduxfast_: urb context is a NULL pointer!\n");
+		return;
+	}
+	// the private structure of the subdevice is usbduxfastsub_t
+	this_usbduxfastsub = this_comedidev->private;
+	if (!this_usbduxfastsub) {
+		printk("comedi_: usbduxfast_: private of comedi subdev is a NULL pointer!\n");
+		return;
+	}
+	// are we running a command?
+	if (unlikely(!(this_usbduxfastsub->ai_cmd_running))) {
+		// not running a command
+		// do not continue execution if no asynchronous command is running
+		// in particular not resubmit
+		return;
+	}
+
+	if (unlikely(!(this_usbduxfastsub->attached))) {
+		// no comedi device there
+		return;
+	}
+	// subdevice which is the AD converter
+	s = this_comedidev->subdevices + SUBDEV_AD;
+
+	// first we test if something unusual has just happened
+	switch (urb->status) {
+	case 0:
+		break;
+
+		// happens after an unlink command or when the device is plugged out
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+	case -ECONNABORTED:
+		// tell this comedi
+		s->async->events |= COMEDI_CB_EOA;
+		s->async->events |= COMEDI_CB_ERROR;
+		comedi_event(this_usbduxfastsub->comedidev, s);
+		// stop the transfer w/o unlink
+		usbduxfast_ai_stop(this_usbduxfastsub, 0);
+		return;
+
+	default:
+		printk("comedi%d: usbduxfast: non-zero urb status received in ai intr context: %d\n", this_usbduxfastsub->comedidev->minor, urb->status);
+		s->async->events |= COMEDI_CB_EOA;
+		s->async->events |= COMEDI_CB_ERROR;
+		comedi_event(this_usbduxfastsub->comedidev, s);
+		usbduxfast_ai_stop(this_usbduxfastsub, 0);
+		return;
+	}
+
+	p = urb->transfer_buffer;
+	if (!this_usbduxfastsub->ignore) {
+		if (!(this_usbduxfastsub->ai_continous)) {
+			// not continous, fixed number of samples
+			n = urb->actual_length / sizeof(uint16_t);
+			if (unlikely(this_usbduxfastsub->ai_sample_count < n)) {
+				// we have send only a fraction of the bytes received
+				cfc_write_array_to_buffer(s,
+					urb->transfer_buffer,
+					this_usbduxfastsub->ai_sample_count *
+					sizeof(uint16_t));
+				usbduxfast_ai_stop(this_usbduxfastsub, 0);
+				// say comedi that the acquistion is over
+				s->async->events |= COMEDI_CB_EOA;
+				comedi_event(this_usbduxfastsub->comedidev, s);
+				return;
+			}
+			this_usbduxfastsub->ai_sample_count -= n;
+		}
+		// write the full buffer to comedi
+		cfc_write_array_to_buffer(s,
+			urb->transfer_buffer, urb->actual_length);
+
+		// tell comedi that data is there
+		comedi_event(this_usbduxfastsub->comedidev, s);
+
+	} else {
+		// ignore this packet
+		this_usbduxfastsub->ignore--;
+	}
+
+	// command is still running
+	// resubmit urb for BULK transfer
+	urb->dev = this_usbduxfastsub->usbdev;
+	urb->status = 0;
+	err = usb_submit_urb(urb, GFP_ATOMIC);
+	if (err < 0) {
+		printk("comedi%d: usbduxfast: urb resubm failed: %d",
+			this_usbduxfastsub->comedidev->minor, err);
+		s->async->events |= COMEDI_CB_EOA;
+		s->async->events |= COMEDI_CB_ERROR;
+		comedi_event(this_usbduxfastsub->comedidev, s);
+		usbduxfast_ai_stop(this_usbduxfastsub, 0);
+	}
+}
+
+static int usbduxfastsub_start(usbduxfastsub_t * usbduxfastsub)
+{
+	int errcode = 0;
+	unsigned char local_transfer_buffer[16];
+
+	if (usbduxfastsub->probed) {
+		// 7f92 to zero
+		local_transfer_buffer[0] = 0;
+		errcode = usb_control_msg(usbduxfastsub->usbdev,
+			// create a pipe for a control transfer
+			usb_sndctrlpipe(usbduxfastsub->usbdev, 0),
+			// bRequest, "Firmware"
+			USBDUXFASTSUB_FIRMWARE,
+			// bmRequestType
+			VENDOR_DIR_OUT,
+			// Value
+			USBDUXFASTSUB_CPUCS,
+			// Index
+			0x0000,
+			// address of the transfer buffer
+			local_transfer_buffer,
+			// Length
+			1,
+			// Timeout
+			EZTIMEOUT);
+		if (errcode < 0) {
+			printk("comedi_: usbduxfast_: control msg failed (start)\n");
+			return errcode;
+		}
+	}
+	return 0;
+}
+
+static int usbduxfastsub_stop(usbduxfastsub_t * usbduxfastsub)
+{
+	int errcode = 0;
+
+	unsigned char local_transfer_buffer[16];
+	if (usbduxfastsub->probed) {
+		// 7f92 to one
+		local_transfer_buffer[0] = 1;
+		errcode = usb_control_msg(usbduxfastsub->usbdev,
+			usb_sndctrlpipe(usbduxfastsub->usbdev, 0),
+			// bRequest, "Firmware"
+			USBDUXFASTSUB_FIRMWARE,
+			// bmRequestType
+			VENDOR_DIR_OUT,
+			// Value
+			USBDUXFASTSUB_CPUCS,
+			// Index
+			0x0000, local_transfer_buffer,
+			// Length
+			1,
+			// Timeout
+			EZTIMEOUT);
+		if (errcode < 0) {
+			printk("comedi_: usbduxfast: control msg failed (stop)\n");
+			return errcode;
+		}
+	}
+	return 0;
+}
+
+static int usbduxfastsub_upload(usbduxfastsub_t * usbduxfastsub,
+	unsigned char *local_transfer_buffer,
+	unsigned int startAddr, unsigned int len)
+{
+	int errcode;
+
+	if (usbduxfastsub->probed) {
+#ifdef CONFIG_COMEDI_DEBUG
+		printk("comedi%d: usbduxfast: uploading %d bytes",
+			usbduxfastsub->comedidev->minor, len);
+		printk(" to addr %d, first byte=%d.\n",
+			startAddr, local_transfer_buffer[0]);
+#endif
+		errcode = usb_control_msg(usbduxfastsub->usbdev,
+			usb_sndctrlpipe(usbduxfastsub->usbdev, 0),
+			// brequest, firmware
+			USBDUXFASTSUB_FIRMWARE,
+			// bmRequestType
+			VENDOR_DIR_OUT,
+			// value
+			startAddr,
+			// index
+			0x0000,
+			// our local safe buffer
+			local_transfer_buffer,
+			// length
+			len,
+			// timeout
+			EZTIMEOUT);
+#ifdef CONFIG_COMEDI_DEBUG
+		printk("comedi_: usbduxfast: result=%d\n", errcode);
+#endif
+		if (errcode < 0) {
+			printk("comedi_: usbduxfast: uppload failed\n");
+			return errcode;
+		}
+	} else {
+		// no device on the bus for this index
+		return -EFAULT;
+	}
+	return 0;
+}
+
+int firmwareUpload(usbduxfastsub_t * usbduxfastsub,
+	unsigned char *firmwareBinary, int sizeFirmware)
+{
+	int ret;
+
+	if (!firmwareBinary) {
+		return 0;
+	}
+	ret = usbduxfastsub_stop(usbduxfastsub);
+	if (ret < 0) {
+		printk("comedi_: usbduxfast: can not stop firmware\n");
+		return ret;
+	}
+	ret = usbduxfastsub_upload(usbduxfastsub,
+		firmwareBinary, 0, sizeFirmware);
+	if (ret < 0) {
+		printk("comedi_: usbduxfast: firmware upload failed\n");
+		return ret;
+	}
+	ret = usbduxfastsub_start(usbduxfastsub);
+	if (ret < 0) {
+		printk("comedi_: usbduxfast: can not start firmware\n");
+		return ret;
+	}
+	return 0;
+}
+
+int usbduxfastsub_submit_InURBs(usbduxfastsub_t * usbduxfastsub)
+{
+	int errFlag;
+
+	if (!usbduxfastsub) {
+		return -EFAULT;
+	}
+	usb_fill_bulk_urb(usbduxfastsub->urbIn,
+		usbduxfastsub->usbdev,
+		usb_rcvbulkpipe(usbduxfastsub->usbdev, BULKINEP),
+		usbduxfastsub->transfer_buffer,
+		SIZEINBUF, usbduxfastsub_ai_Irq, usbduxfastsub->comedidev);
+
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast: submitting in-urb: %x,%x\n",
+		usbduxfastsub->comedidev->minor,
+		(int)(usbduxfastsub->urbIn->context),
+		(int)(usbduxfastsub->urbIn->dev));
+#endif
+	errFlag = usb_submit_urb(usbduxfastsub->urbIn, GFP_ATOMIC);
+	if (errFlag) {
+		printk("comedi_: usbduxfast: ai: usb_submit_urb error %d\n",
+			errFlag);
+		return errFlag;
+	}
+	return 0;
+}
+
+static int usbduxfast_ai_cmdtest(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd)
+{
+	int err = 0, stop_mask = 0;
+	long int steps, tmp = 0;
+	int minSamplPer;
+	usbduxfastsub_t *this_usbduxfastsub = dev->private;
+	if (!(this_usbduxfastsub->probed)) {
+		return -ENODEV;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast_ai_cmdtest\n", dev->minor);
+	printk("comedi%d: usbduxfast: convert_arg=%u scan_begin_arg=%u\n",
+		dev->minor, cmd->convert_arg, cmd->scan_begin_arg);
+#endif
+	/* step 1: make sure trigger sources are trivially valid */
+
+	tmp = cmd->start_src;
+	cmd->start_src &= TRIG_NOW | TRIG_EXT | TRIG_INT;
+	if (!cmd->start_src || tmp != cmd->start_src)
+		err++;
+
+	tmp = cmd->scan_begin_src;
+	cmd->scan_begin_src &= TRIG_TIMER | TRIG_FOLLOW | TRIG_EXT;
+	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
+		err++;
+
+	tmp = cmd->convert_src;
+	cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
+	if (!cmd->convert_src || tmp != cmd->convert_src)
+		err++;
+
+	tmp = cmd->scan_end_src;
+	cmd->scan_end_src &= TRIG_COUNT;
+	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
+		err++;
+
+	tmp = cmd->stop_src;
+	stop_mask = TRIG_COUNT | TRIG_NONE;
+	cmd->stop_src &= stop_mask;
+	if (!cmd->stop_src || tmp != cmd->stop_src)
+		err++;
+
+	if (err)
+		return 1;
+
+	/* step 2: make sure trigger sources are unique and mutually compatible */
+
+	if (cmd->start_src != TRIG_NOW &&
+		cmd->start_src != TRIG_EXT && cmd->start_src != TRIG_INT)
+		err++;
+	if (cmd->scan_begin_src != TRIG_TIMER &&
+		cmd->scan_begin_src != TRIG_FOLLOW &&
+		cmd->scan_begin_src != TRIG_EXT)
+		err++;
+	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT)
+		err++;
+	if (cmd->stop_src != TRIG_COUNT &&
+		cmd->stop_src != TRIG_EXT && cmd->stop_src != TRIG_NONE)
+		err++;
+
+	// can't have external stop and start triggers at once
+	if (cmd->start_src == TRIG_EXT && cmd->stop_src == TRIG_EXT)
+		err++;
+
+	if (err)
+		return 2;
+
+	/* step 3: make sure arguments are trivially compatible */
+
+	if (cmd->start_src == TRIG_NOW && cmd->start_arg != 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+
+	if (!cmd->chanlist_len) {
+		err++;
+	}
+	if (cmd->scan_end_arg != cmd->chanlist_len) {
+		cmd->scan_end_arg = cmd->chanlist_len;
+		err++;
+	}
+
+	if (cmd->chanlist_len == 1) {
+		minSamplPer = 1;
+	} else {
+		minSamplPer = MIN_SAMPLING_PERIOD;
+	}
+
+	if (cmd->convert_src == TRIG_TIMER) {
+		steps = cmd->convert_arg * 30;
+		if (steps < (minSamplPer * 1000)) {
+			steps = minSamplPer * 1000;
+		}
+		if (steps > (MAX_SAMPLING_PERIOD * 1000)) {
+			steps = MAX_SAMPLING_PERIOD * 1000;
+		}
+		// calc arg again
+		tmp = steps / 30;
+		if (cmd->convert_arg != tmp) {
+			cmd->convert_arg = tmp;
+			err++;
+		}
+	}
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		err++;
+	}
+	// stop source
+	switch (cmd->stop_src) {
+	case TRIG_COUNT:
+		if (!cmd->stop_arg) {
+			cmd->stop_arg = 1;
+			err++;
+		}
+		break;
+	case TRIG_NONE:
+		if (cmd->stop_arg != 0) {
+			cmd->stop_arg = 0;
+			err++;
+		}
+		break;
+		// TRIG_EXT doesn't care since it doesn't trigger off a numbered channel
+	default:
+		break;
+	}
+
+	if (err)
+		return 3;
+
+	/* step 4: fix up any arguments */
+
+	return 0;
+
+}
+
+static int usbduxfast_ai_inttrig(comedi_device * dev,
+	comedi_subdevice * s, unsigned int trignum)
+{
+	int ret;
+	usbduxfastsub_t *this_usbduxfastsub = dev->private;
+	if (!this_usbduxfastsub) {
+		return -EFAULT;
+	}
+	down(&this_usbduxfastsub->sem);
+	if (!(this_usbduxfastsub->probed)) {
+		up(&this_usbduxfastsub->sem);
+		return -ENODEV;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast_ai_inttrig\n", dev->minor);
+#endif
+
+	if (trignum != 0) {
+		printk("comedi%d: usbduxfast_ai_inttrig: invalid trignum\n",
+			dev->minor);
+		up(&this_usbduxfastsub->sem);
+		return -EINVAL;
+	}
+	if (!(this_usbduxfastsub->ai_cmd_running)) {
+		this_usbduxfastsub->ai_cmd_running = 1;
+		ret = usbduxfastsub_submit_InURBs(this_usbduxfastsub);
+		if (ret < 0) {
+			printk("comedi%d: usbduxfast_ai_inttrig: urbSubmit: err=%d\n", dev->minor, ret);
+			this_usbduxfastsub->ai_cmd_running = 0;
+			up(&this_usbduxfastsub->sem);
+			return ret;
+		}
+		s->async->inttrig = NULL;
+	} else {
+		printk("comedi%d: ai_inttrig but acqu is already running\n",
+			dev->minor);
+	}
+	up(&this_usbduxfastsub->sem);
+	return 1;
+}
+
+// offsets for the GPIF bytes
+// the first byte is the command byte
+#define LENBASE 1+0x00
+#define OPBASE  1+0x08
+#define OUTBASE 1+0x10
+#define LOGBASE 1+0x18
+
+static int usbduxfast_ai_cmd(comedi_device * dev, comedi_subdevice * s)
+{
+	comedi_cmd *cmd = &s->async->cmd;
+	unsigned int chan, gain, rngmask = 0xff;
+	int i, j, ret;
+	usbduxfastsub_t *this_usbduxfastsub = dev->private;
+	int result;
+	long steps, steps_tmp;
+
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast_ai_cmd\n", dev->minor);
+#endif
+	if (!this_usbduxfastsub) {
+		return -EFAULT;
+	}
+	down(&this_usbduxfastsub->sem);
+	if (!(this_usbduxfastsub->probed)) {
+		up(&this_usbduxfastsub->sem);
+		return -ENODEV;
+	}
+	if (this_usbduxfastsub->ai_cmd_running) {
+		printk("comedi%d: ai_cmd not possible. Another ai_cmd is running.\n", dev->minor);
+		up(&this_usbduxfastsub->sem);
+		return -EBUSY;
+	}
+	// set current channel of the running aquisition to zero
+	s->async->cur_chan = 0;
+
+	// ignore the first buffers from the device if there is an error condition
+	this_usbduxfastsub->ignore = PACKETS_TO_IGNORE;
+
+	if (cmd->chanlist_len > 0) {
+		gain = CR_RANGE(cmd->chanlist[0]);
+		for (i = 0; i < cmd->chanlist_len; ++i) {
+			chan = CR_CHAN(cmd->chanlist[i]);
+			if (chan != i) {
+				printk("comedi%d: cmd is accepting only consecutive channels.\n", dev->minor);
+				up(&this_usbduxfastsub->sem);
+				return -EINVAL;
+			}
+			if ((gain != CR_RANGE(cmd->chanlist[i]))
+				&& (cmd->chanlist_len > 3)) {
+				printk("comedi%d: the gain must be the same for all channels.\n", dev->minor);
+				up(&this_usbduxfastsub->sem);
+				return -EINVAL;
+			}
+			if (i >= NUMCHANNELS) {
+				printk("comedi%d: channel list too long\n",
+					dev->minor);
+				break;
+			}
+		}
+	}
+	steps = 0;
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		printk("comedi%d: usbduxfast: scan_begin_src==TRIG_TIMER not valid.\n", dev->minor);
+		up(&this_usbduxfastsub->sem);
+		return -EINVAL;
+	}
+	if (cmd->convert_src == TRIG_TIMER) {
+		steps = (cmd->convert_arg * 30) / 1000;
+	}
+	if ((steps < MIN_SAMPLING_PERIOD) && (cmd->chanlist_len != 1)) {
+		printk("comedi%d: usbduxfast: ai_cmd: steps=%ld, scan_begin_arg=%d. Not properly tested by cmdtest?\n", dev->minor, steps, cmd->scan_begin_arg);
+		up(&this_usbduxfastsub->sem);
+		return -EINVAL;
+	}
+	if (steps > MAX_SAMPLING_PERIOD) {
+		printk("comedi%d: usbduxfast: ai_cmd: sampling rate too low.\n",
+			dev->minor);
+		up(&this_usbduxfastsub->sem);
+		return -EINVAL;
+	}
+	if ((cmd->start_src == TRIG_EXT) && (cmd->chanlist_len != 1)
+		&& (cmd->chanlist_len != 16)) {
+		printk("comedi%d: usbduxfast: ai_cmd: TRIG_EXT only with 1 or 16 channels possible.\n", dev->minor);
+		up(&this_usbduxfastsub->sem);
+		return -EINVAL;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast: steps=%ld, convert_arg=%u, ai_timer=%u\n",
+		dev->minor,
+		steps, cmd->convert_arg, this_usbduxfastsub->ai_timer);
+#endif
+
+	switch (cmd->chanlist_len) {
+		// one channel
+	case 1:
+		if (CR_RANGE(cmd->chanlist[0]) > 0)
+			rngmask = 0xff - 0x04;
+		else
+			rngmask = 0xff;
+
+		// for external trigger: looping in this state until the RDY0 pin
+		// becomes zero
+		if (cmd->start_src == TRIG_EXT) {	// we loop here until ready has been set
+			this_usbduxfastsub->dux_commands[LENBASE + 0] = 0x01;	// branch back to state 0
+			this_usbduxfastsub->dux_commands[OPBASE + 0] = 0x01;	// deceision state w/o data
+			this_usbduxfastsub->dux_commands[OUTBASE + 0] =
+				0xFF & rngmask;
+			this_usbduxfastsub->dux_commands[LOGBASE + 0] = 0x00;	// RDY0 = 0
+		} else {	// we just proceed to state 1
+			this_usbduxfastsub->dux_commands[LENBASE + 0] = 1;
+			this_usbduxfastsub->dux_commands[OPBASE + 0] = 0;
+			this_usbduxfastsub->dux_commands[OUTBASE + 0] =
+				0xFF & rngmask;
+			this_usbduxfastsub->dux_commands[LOGBASE + 0] = 0;
+		}
+
+		if (steps < MIN_SAMPLING_PERIOD) {
+			// for fast single channel aqu without mux
+			if (steps <= 1) {
+				// we just stay here at state 1 and rexecute the same state
+				// this gives us 30MHz sampling rate
+				this_usbduxfastsub->dux_commands[LENBASE + 1] = 0x89;	// branch back to state 1
+				this_usbduxfastsub->dux_commands[OPBASE + 1] = 0x03;	// deceision state with data
+				this_usbduxfastsub->dux_commands[OUTBASE + 1] =
+					0xFF & rngmask;
+				this_usbduxfastsub->dux_commands[LOGBASE + 1] = 0xFF;	// doesn't matter
+			} else {
+				// we loop through two states: data and delay: max rate is 15Mhz
+				this_usbduxfastsub->dux_commands[LENBASE + 1] =
+					steps - 1;
+				this_usbduxfastsub->dux_commands[OPBASE + 1] = 0x02;	// data
+				this_usbduxfastsub->dux_commands[OUTBASE + 1] =
+					0xFF & rngmask;
+				this_usbduxfastsub->dux_commands[LOGBASE + 1] = 0;	// doesn't matter
+
+				this_usbduxfastsub->dux_commands[LENBASE + 2] = 0x09;	// branch back to state 1
+				this_usbduxfastsub->dux_commands[OPBASE + 2] = 0x01;	// deceision state w/o data
+				this_usbduxfastsub->dux_commands[OUTBASE + 2] =
+					0xFF & rngmask;
+				this_usbduxfastsub->dux_commands[LOGBASE + 2] = 0xFF;	// doesn't matter
+			}
+		} else {
+			// we loop through 3 states: 2x delay and 1x data. This gives a min
+			// sampling rate of 60kHz.
+
+			// we have 1 state with duration 1
+			steps = steps - 1;
+
+			// do the first part of the delay
+			this_usbduxfastsub->dux_commands[LENBASE + 1] =
+				steps / 2;
+			this_usbduxfastsub->dux_commands[OPBASE + 1] = 0;
+			this_usbduxfastsub->dux_commands[OUTBASE + 1] =
+				0xFF & rngmask;
+			this_usbduxfastsub->dux_commands[LOGBASE + 1] = 0;
+
+			// and the second part
+			this_usbduxfastsub->dux_commands[LENBASE + 2] =
+				steps - steps / 2;
+			this_usbduxfastsub->dux_commands[OPBASE + 2] = 0;
+			this_usbduxfastsub->dux_commands[OUTBASE + 2] =
+				0xFF & rngmask;
+			this_usbduxfastsub->dux_commands[LOGBASE + 2] = 0;
+
+			// get the data and branch back
+			this_usbduxfastsub->dux_commands[LENBASE + 3] = 0x09;	// branch back to state 1
+			this_usbduxfastsub->dux_commands[OPBASE + 3] = 0x03;	// deceision state w data
+			this_usbduxfastsub->dux_commands[OUTBASE + 3] =
+				0xFF & rngmask;
+			this_usbduxfastsub->dux_commands[LOGBASE + 3] = 0xFF;	// doesn't matter
+		}
+		break;
+
+	case 2:
+		// two channels
+		// commit data to the FIFO
+		if (CR_RANGE(cmd->chanlist[0]) > 0)
+			rngmask = 0xff - 0x04;
+		else
+			rngmask = 0xff;
+		this_usbduxfastsub->dux_commands[LENBASE + 0] = 1;
+		this_usbduxfastsub->dux_commands[OPBASE + 0] = 0x02;	// data
+		this_usbduxfastsub->dux_commands[OUTBASE + 0] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 0] = 0;
+
+		// we have 1 state with duration 1: state 0
+		steps_tmp = steps - 1;
+
+		if (CR_RANGE(cmd->chanlist[1]) > 0)
+			rngmask = 0xff - 0x04;
+		else
+			rngmask = 0xff;
+		// do the first part of the delay
+		this_usbduxfastsub->dux_commands[LENBASE + 1] = steps_tmp / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 1] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 1] = 0xFE & rngmask;	//count
+		this_usbduxfastsub->dux_commands[LOGBASE + 1] = 0;
+
+		// and the second part
+		this_usbduxfastsub->dux_commands[LENBASE + 2] =
+			steps_tmp - steps_tmp / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 2] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 2] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 2] = 0;
+
+		this_usbduxfastsub->dux_commands[LENBASE + 3] = 1;
+		this_usbduxfastsub->dux_commands[OPBASE + 3] = 0x02;	// data
+		this_usbduxfastsub->dux_commands[OUTBASE + 3] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 3] = 0;
+
+		// we have 2 states with duration 1: step 6 and the IDLE state
+		steps_tmp = steps - 2;
+
+		if (CR_RANGE(cmd->chanlist[0]) > 0)
+			rngmask = 0xff - 0x04;
+		else
+			rngmask = 0xff;
+		// do the first part of the delay
+		this_usbduxfastsub->dux_commands[LENBASE + 4] = steps_tmp / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 4] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 4] = (0xFF - 0x02) & rngmask;	//reset
+		this_usbduxfastsub->dux_commands[LOGBASE + 4] = 0;
+
+		// and the second part
+		this_usbduxfastsub->dux_commands[LENBASE + 5] =
+			steps_tmp - steps_tmp / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 5] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 5] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 5] = 0;
+
+		this_usbduxfastsub->dux_commands[LENBASE + 6] = 1;
+		this_usbduxfastsub->dux_commands[OPBASE + 6] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 6] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 6] = 0;
+		break;
+
+	case 3:
+		// three channels
+		for (j = 0; j < 1; j++) {
+			if (CR_RANGE(cmd->chanlist[j]) > 0)
+				rngmask = 0xff - 0x04;
+			else
+				rngmask = 0xff;
+			// commit data to the FIFO and do the first part of the delay
+			this_usbduxfastsub->dux_commands[LENBASE + j * 2] =
+				steps / 2;
+			this_usbduxfastsub->dux_commands[OPBASE + j * 2] = 0x02;	// data
+			this_usbduxfastsub->dux_commands[OUTBASE + j * 2] = 0xFF & rngmask;	// no change
+			this_usbduxfastsub->dux_commands[LOGBASE + j * 2] = 0;
+
+			if (CR_RANGE(cmd->chanlist[j + 1]) > 0)
+				rngmask = 0xff - 0x04;
+			else
+				rngmask = 0xff;
+			// do the second part of the delay
+			this_usbduxfastsub->dux_commands[LENBASE + j * 2 + 1] =
+				steps - steps / 2;
+			this_usbduxfastsub->dux_commands[OPBASE + j * 2 + 1] = 0;	// no data
+			this_usbduxfastsub->dux_commands[OUTBASE + j * 2 + 1] = 0xFE & rngmask;	//count
+			this_usbduxfastsub->dux_commands[LOGBASE + j * 2 + 1] =
+				0;
+		}
+
+		// 2 steps with duration 1: the idele step and step 6:
+		steps_tmp = steps - 2;
+		// commit data to the FIFO and do the first part of the delay
+		this_usbduxfastsub->dux_commands[LENBASE + 4] = steps_tmp / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 4] = 0x02;	// data
+		this_usbduxfastsub->dux_commands[OUTBASE + 4] = 0xFF & rngmask;	// no change
+		this_usbduxfastsub->dux_commands[LOGBASE + 4] = 0;
+
+		if (CR_RANGE(cmd->chanlist[0]) > 0)
+			rngmask = 0xff - 0x04;
+		else
+			rngmask = 0xff;
+		// do the second part of the delay
+		this_usbduxfastsub->dux_commands[LENBASE + 5] =
+			steps_tmp - steps_tmp / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 5] = 0;	// no data
+		this_usbduxfastsub->dux_commands[OUTBASE + 5] = (0xFF - 0x02) & rngmask;	// reset
+		this_usbduxfastsub->dux_commands[LOGBASE + 5] = 0;
+
+		this_usbduxfastsub->dux_commands[LENBASE + 6] = 1;
+		this_usbduxfastsub->dux_commands[OPBASE + 6] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 6] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 6] = 0;
+
+	case 16:
+		if (CR_RANGE(cmd->chanlist[0]) > 0)
+			rngmask = 0xff - 0x04;
+		else
+			rngmask = 0xff;
+		if (cmd->start_src == TRIG_EXT) {	// we loop here until ready has been set
+			this_usbduxfastsub->dux_commands[LENBASE + 0] = 0x01;	// branch back to state 0
+			this_usbduxfastsub->dux_commands[OPBASE + 0] = 0x01;	// deceision state w/o data
+			this_usbduxfastsub->dux_commands[OUTBASE + 0] = (0xFF - 0x02) & rngmask;	// reset
+			this_usbduxfastsub->dux_commands[LOGBASE + 0] = 0x00;	// RDY0 = 0
+		} else {	// we just proceed to state 1
+			this_usbduxfastsub->dux_commands[LENBASE + 0] = 255;	// 30us reset pulse
+			this_usbduxfastsub->dux_commands[OPBASE + 0] = 0;
+			this_usbduxfastsub->dux_commands[OUTBASE + 0] = (0xFF - 0x02) & rngmask;	// reset
+			this_usbduxfastsub->dux_commands[LOGBASE + 0] = 0;
+		}
+
+		// commit data to the FIFO
+		this_usbduxfastsub->dux_commands[LENBASE + 1] = 1;
+		this_usbduxfastsub->dux_commands[OPBASE + 1] = 0x02;	// data
+		this_usbduxfastsub->dux_commands[OUTBASE + 1] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 1] = 0;
+
+		// we have 2 states with duration 1
+		steps = steps - 2;
+
+		// do the first part of the delay
+		this_usbduxfastsub->dux_commands[LENBASE + 2] = steps / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 2] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 2] = 0xFE & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 2] = 0;
+
+		// and the second part
+		this_usbduxfastsub->dux_commands[LENBASE + 3] =
+			steps - steps / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 3] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 3] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 3] = 0;
+
+		this_usbduxfastsub->dux_commands[LENBASE + 4] = 0x09;	// branch back to state 1
+		this_usbduxfastsub->dux_commands[OPBASE + 4] = 0x01;	// deceision state w/o data
+		this_usbduxfastsub->dux_commands[OUTBASE + 4] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 4] = 0xFF;	// doesn't matter
+
+		break;
+
+	default:
+		printk("comedi %d: unsupported combination of channels\n",
+			dev->minor);
+		up(&this_usbduxfastsub->sem);
+		return -EFAULT;
+	}
+
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi %d: sending commands to the usb device\n", dev->minor);
+#endif
+	// 0 means that the AD commands are sent
+	result = send_dux_commands(this_usbduxfastsub, SENDADCOMMANDS);
+	if (result < 0) {
+		printk("comedi%d: adc command could not be submitted. Aborting...\n", dev->minor);
+		up(&this_usbduxfastsub->sem);
+		return result;
+	}
+	if (cmd->stop_src == TRIG_COUNT) {
+		this_usbduxfastsub->ai_sample_count =
+			(cmd->stop_arg) * (cmd->scan_end_arg);
+		if (usbduxfastsub->ai_sample_count < 1) {
+			printk("comedi%d: (cmd->stop_arg)*(cmd->scan_end_arg)<1, aborting.\n", dev->minor);
+			up(&this_usbduxfastsub->sem);
+			return -EFAULT;
+		}
+		this_usbduxfastsub->ai_continous = 0;
+	} else {
+		// continous aquisition
+		this_usbduxfastsub->ai_continous = 1;
+		this_usbduxfastsub->ai_sample_count = 0;
+	}
+
+	if ((cmd->start_src == TRIG_NOW) || (cmd->start_src == TRIG_EXT)) {
+		// enable this acquisition operation
+		this_usbduxfastsub->ai_cmd_running = 1;
+		ret = usbduxfastsub_submit_InURBs(this_usbduxfastsub);
+		if (ret < 0) {
+			this_usbduxfastsub->ai_cmd_running = 0;
+			// fixme: unlink here??
+			up(&this_usbduxfastsub->sem);
+			return ret;
+		}
+		s->async->inttrig = NULL;
+	} else {
+		/* TRIG_INT */
+		// don't enable the acquision operation
+		// wait for an internal signal
+		s->async->inttrig = usbduxfast_ai_inttrig;
+	}
+	up(&this_usbduxfastsub->sem);
+
+	return 0;
+}
+
+/* Mode 0 is used to get a single conversion on demand */
+static int usbduxfast_ai_insn_read(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	int i, j, n, actual_length;
+	int chan, range, rngmask;
+	int err;
+	usbduxfastsub_t *usbduxfastsub = dev->private;
+
+	if (!usbduxfastsub) {
+		printk("comedi%d: ai_insn_read: no usb dev.\n", dev->minor);
+		return -ENODEV;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: ai_insn_read, insn->n=%d, insn->subdev=%d\n",
+		dev->minor, insn->n, insn->subdev);
+#endif
+	down(&usbduxfastsub->sem);
+	if (!(usbduxfastsub->probed)) {
+		up(&usbduxfastsub->sem);
+		return -ENODEV;
+	}
+	if (usbduxfastsub->ai_cmd_running) {
+		printk("comedi%d: ai_insn_read not possible. Async Command is running.\n", dev->minor);
+		up(&usbduxfastsub->sem);
+		return -EBUSY;
+	}
+	// sample one channel
+	chan = CR_CHAN(insn->chanspec);
+	range = CR_RANGE(insn->chanspec);
+	// set command for the first channel
+
+	if (range > 0)
+		rngmask = 0xff - 0x04;
+	else
+		rngmask = 0xff;
+	// commit data to the FIFO
+	usbduxfastsub->dux_commands[LENBASE + 0] = 1;
+	usbduxfastsub->dux_commands[OPBASE + 0] = 0x02;	// data
+	usbduxfastsub->dux_commands[OUTBASE + 0] = 0xFF & rngmask;
+	usbduxfastsub->dux_commands[LOGBASE + 0] = 0;
+
+	// do the first part of the delay
+	usbduxfastsub->dux_commands[LENBASE + 1] = 12;
+	usbduxfastsub->dux_commands[OPBASE + 1] = 0;
+	usbduxfastsub->dux_commands[OUTBASE + 1] = 0xFE & rngmask;
+	usbduxfastsub->dux_commands[LOGBASE + 1] = 0;
+
+	usbduxfastsub->dux_commands[LENBASE + 2] = 1;
+	usbduxfastsub->dux_commands[OPBASE + 2] = 0;
+	usbduxfastsub->dux_commands[OUTBASE + 2] = 0xFE & rngmask;
+	usbduxfastsub->dux_commands[LOGBASE + 2] = 0;
+
+	usbduxfastsub->dux_commands[LENBASE + 3] = 1;
+	usbduxfastsub->dux_commands[OPBASE + 3] = 0;
+	usbduxfastsub->dux_commands[OUTBASE + 3] = 0xFE & rngmask;
+	usbduxfastsub->dux_commands[LOGBASE + 3] = 0;
+
+	usbduxfastsub->dux_commands[LENBASE + 4] = 1;
+	usbduxfastsub->dux_commands[OPBASE + 4] = 0;
+	usbduxfastsub->dux_commands[OUTBASE + 4] = 0xFE & rngmask;
+	usbduxfastsub->dux_commands[LOGBASE + 4] = 0;
+
+	// second part
+	usbduxfastsub->dux_commands[LENBASE + 5] = 12;
+	usbduxfastsub->dux_commands[OPBASE + 5] = 0;
+	usbduxfastsub->dux_commands[OUTBASE + 5] = 0xFF & rngmask;
+	usbduxfastsub->dux_commands[LOGBASE + 5] = 0;
+
+	usbduxfastsub->dux_commands[LENBASE + 6] = 1;
+	usbduxfastsub->dux_commands[OPBASE + 6] = 0;
+	usbduxfastsub->dux_commands[OUTBASE + 6] = 0xFF & rngmask;
+	usbduxfastsub->dux_commands[LOGBASE + 0] = 0;
+
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi %d: sending commands to the usb device\n", dev->minor);
+#endif
+	// 0 means that the AD commands are sent
+	err = send_dux_commands(usbduxfastsub, SENDADCOMMANDS);
+	if (err < 0) {
+		printk("comedi%d: adc command could not be submitted. Aborting...\n", dev->minor);
+		up(&usbduxfastsub->sem);
+		return err;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast: submitting in-urb: %x,%x\n",
+		usbduxfastsub->comedidev->minor,
+		(int)(usbduxfastsub->urbIn->context),
+		(int)(usbduxfastsub->urbIn->dev));
+#endif
+	for (i = 0; i < PACKETS_TO_IGNORE; i++) {
+		err = usb_bulk_msg(usbduxfastsub->usbdev,
+				   usb_rcvbulkpipe(usbduxfastsub->usbdev,
+						   BULKINEP),
+				   usbduxfastsub->transfer_buffer, SIZEINBUF,
+				   &actual_length, 10000);
+		if (err < 0) {
+			printk("comedi%d: insn timeout. No data.\n",
+				dev->minor);
+			up(&usbduxfastsub->sem);
+			return err;
+		}
+	}
+	// data points
+	for (i = 0; i < insn->n;) {
+		err = usb_bulk_msg(usbduxfastsub->usbdev,
+				   usb_rcvbulkpipe(usbduxfastsub->usbdev,
+						   BULKINEP),
+				   usbduxfastsub->transfer_buffer, SIZEINBUF,
+				   &actual_length, 10000);
+		if (err < 0) {
+			printk("comedi%d: insn data error: %d\n",
+				dev->minor, err);
+			up(&usbduxfastsub->sem);
+			return err;
+		}
+		n = actual_length / sizeof(uint16_t);
+		if ((n % 16) != 0) {
+			printk("comedi%d: insn data packet corrupted.\n",
+				dev->minor);
+			up(&usbduxfastsub->sem);
+			return -EINVAL;
+		}
+		for (j = chan; (j < n) && (i < insn->n); j = j + 16) {
+			data[i] =
+				((uint16_t *) (usbduxfastsub->
+					transfer_buffer))[j];
+			i++;
+		}
+	}
+	up(&usbduxfastsub->sem);
+	return i;
+}
+
+static unsigned hex2unsigned(char *h)
+{
+	unsigned hi, lo;
+	if (h[0] > '9') {
+		hi = h[0] - 'A' + 0x0a;
+	} else {
+		hi = h[0] - '0';
+	}
+	if (h[1] > '9') {
+		lo = h[1] - 'A' + 0x0a;
+	} else {
+		lo = h[1] - '0';
+	}
+	return hi * 0x10 + lo;
+}
+
+// for FX2
+#define FIRMWARE_MAX_LEN 0x2000
+
+// taken from David Brownell's fxload and adjusted for this driver
+static int read_firmware(usbduxfastsub_t * usbduxfastsub, void *firmwarePtr,
+	long size)
+{
+	int i = 0;
+	unsigned char *fp = (char *)firmwarePtr;
+	unsigned char *firmwareBinary = NULL;
+	int res = 0;
+	int maxAddr = 0;
+
+	firmwareBinary = kmalloc(FIRMWARE_MAX_LEN, GFP_KERNEL);
+	if (!firmwareBinary) {
+		printk("comedi_: usbduxfast: mem alloc for firmware failed\n");
+		return -ENOMEM;
+	}
+
+	for (;;) {
+		char buf[256], *cp;
+		char type;
+		int len;
+		int idx, off;
+		int j = 0;
+
+		// get one line
+		while ((i < size) && (fp[i] != 13) && (fp[i] != 10)) {
+			buf[j] = fp[i];
+			i++;
+			j++;
+			if (j >= sizeof(buf)) {
+				printk("comedi_: usbduxfast: bogus firmware file!\n");
+				return -1;
+			}
+		}
+		// get rid of LF/CR/...
+		while ((i < size) && ((fp[i] == 13) || (fp[i] == 10)
+				|| (fp[i] == 0))) {
+			i++;
+		}
+
+		buf[j] = 0;
+		//printk("comedi_: buf=%s\n",buf);
+
+		/* EXTENSION: "# comment-till-end-of-line", for copyrights etc */
+		if (buf[0] == '#')
+			continue;
+
+		if (buf[0] != ':') {
+			printk("comedi_: usbduxfast: upload: not an ihex record: %s", buf);
+			return -EFAULT;
+		}
+
+		/* Read the length field (up to 16 bytes) */
+		len = hex2unsigned(buf + 1);
+
+		/* Read the target offset */
+		off = (hex2unsigned(buf + 3) * 0x0100) + hex2unsigned(buf + 5);
+
+		if ((off + len) > maxAddr) {
+			maxAddr = off + len;
+		}
+
+		if (maxAddr >= FIRMWARE_MAX_LEN) {
+			printk("comedi_: usbduxfast: firmware upload goes beyond FX2 RAM boundaries.");
+			return -EFAULT;
+		}
+		//printk("comedi_: usbduxfast: off=%x, len=%x:",off,len);
+
+		/* Read the record type */
+		type = hex2unsigned(buf + 7);
+
+		/* If this is an EOF record, then make it so. */
+		if (type == 1) {
+			break;
+		}
+
+		if (type != 0) {
+			printk("comedi_: usbduxfast: unsupported record type: %u\n", type);
+			return -EFAULT;
+		}
+
+		for (idx = 0, cp = buf + 9; idx < len; idx += 1, cp += 2) {
+			firmwareBinary[idx + off] = hex2unsigned(cp);
+			//printk("%02x ",firmwareBinary[idx+off]);
+		}
+		//printk("\n");
+
+		if (i >= size) {
+			printk("comedi_: usbduxfast: unexpected end of hex file\n");
+			break;
+		}
+
+	}
+	res = firmwareUpload(usbduxfastsub, firmwareBinary, maxAddr + 1);
+	kfree(firmwareBinary);
+	return res;
+}
+
+static void tidy_up(usbduxfastsub_t * usbduxfastsub_tmp)
+{
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi_: usbduxfast: tiding up\n");
+#endif
+	if (!usbduxfastsub_tmp) {
+		return;
+	}
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
+	// shows the usb subsystem that the driver is down
+	if (usbduxfastsub_tmp->interface) {
+		usb_set_intfdata(usbduxfastsub_tmp->interface, NULL);
+	}
+#endif
+
+	usbduxfastsub_tmp->probed = 0;
+
+	if (usbduxfastsub_tmp->urbIn) {
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,8)
+		// waits until a running transfer is over
+		// thus, under 2.4 hotplugging while a command
+		// is running is not safe
+		usb_kill_urb(usbduxfastsub_tmp->urbIn);
+#endif
+		if (usbduxfastsub_tmp->transfer_buffer) {
+			kfree(usbduxfastsub_tmp->transfer_buffer);
+			usbduxfastsub_tmp->transfer_buffer = NULL;
+		}
+		usb_free_urb(usbduxfastsub_tmp->urbIn);
+		usbduxfastsub_tmp->urbIn = NULL;
+	}
+	if (usbduxfastsub_tmp->insnBuffer) {
+		kfree(usbduxfastsub_tmp->insnBuffer);
+		usbduxfastsub_tmp->insnBuffer = NULL;
+	}
+	if (usbduxfastsub_tmp->dux_commands) {
+		kfree(usbduxfastsub_tmp->dux_commands);
+		usbduxfastsub_tmp->dux_commands = NULL;
+	}
+	usbduxfastsub_tmp->ai_cmd_running = 0;
+}
+
+// allocate memory for the urbs and initialise them
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
+static void *usbduxfastsub_probe(struct usb_device *udev,
+	unsigned int interfnum, const struct usb_device_id *id)
+{
+#else
+static int usbduxfastsub_probe(struct usb_interface *uinterf,
+	const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(uinterf);
+#endif
+	int i;
+	int index;
+
+	if (udev->speed != USB_SPEED_HIGH) {
+		printk("comedi_: usbduxfast_: This driver needs USB 2.0 to operate. Aborting...\n");
+		return -ENODEV;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi_: usbduxfast_: finding a free structure for the usb-device\n");
+#endif
+	down(&start_stop_sem);
+	// look for a free place in the usbduxfast array
+	index = -1;
+	for (i = 0; i < NUMUSBDUXFAST; i++) {
+		if (!(usbduxfastsub[i].probed)) {
+			index = i;
+			break;
+		}
+	}
+
+	// no more space
+	if (index == -1) {
+		printk("Too many usbduxfast-devices connected.\n");
+		up(&start_stop_sem);
+		return -EMFILE;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi_: usbduxfast: usbduxfastsub[%d] is ready to connect to comedi.\n", index);
+#endif
+
+	init_MUTEX(&(usbduxfastsub[index].sem));
+	// save a pointer to the usb device
+	usbduxfastsub[index].usbdev = udev;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
+	// save the interface number
+	usbduxfastsub[index].ifnum = interfnum;
+#else
+	// 2.6: save the interface itself
+	usbduxfastsub[index].interface = uinterf;
+	// get the interface number from the interface
+	usbduxfastsub[index].ifnum = uinterf->altsetting->desc.bInterfaceNumber;
+	// hand the private data over to the usb subsystem
+	// will be needed for disconnect
+	usb_set_intfdata(uinterf, &(usbduxfastsub[index]));
+#endif
+
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi_: usbduxfast: ifnum=%d\n", usbduxfastsub[index].ifnum);
+#endif
+	// create space for the commands going to the usb device
+	usbduxfastsub[index].dux_commands = kmalloc(SIZEOFDUXBUFFER,
+		GFP_KERNEL);
+	if (!usbduxfastsub[index].dux_commands) {
+		printk("comedi_: usbduxfast: error alloc space for dac commands\n");
+		tidy_up(&(usbduxfastsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	// create space of the instruction buffer
+	usbduxfastsub[index].insnBuffer = kmalloc(SIZEINSNBUF, GFP_KERNEL);
+	if (!(usbduxfastsub[index].insnBuffer)) {
+		printk("comedi_: usbduxfast: could not alloc space for insnBuffer\n");
+		tidy_up(&(usbduxfastsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	// setting to alternate setting 1: enabling bulk ep
+	i = usb_set_interface(usbduxfastsub[index].usbdev,
+		usbduxfastsub[index].ifnum, 1);
+	if (i < 0) {
+		printk("comedi_: usbduxfast%d: could not switch to alternate setting 1.\n", index);
+		tidy_up(&(usbduxfastsub[index]));
+		up(&start_stop_sem);
+		return -ENODEV;
+	}
+	usbduxfastsub[index].urbIn = usb_alloc_urb(0, GFP_KERNEL);
+	if (usbduxfastsub[index].urbIn == NULL) {
+		printk("comedi_: usbduxfast%d: Could not alloc. urb\n", index);
+		tidy_up(&(usbduxfastsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	usbduxfastsub[index].transfer_buffer = kmalloc(SIZEINBUF, GFP_KERNEL);
+	if (!(usbduxfastsub[index].transfer_buffer)) {
+		printk("comedi_: usbduxfast%d: could not alloc. transb.\n",
+			index);
+		tidy_up(&(usbduxfastsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	// we've reached the bottom of the function
+	usbduxfastsub[index].probed = 1;
+	up(&start_stop_sem);
+	printk("comedi_: usbduxfast%d has been successfully initialized.\n",
+		index);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
+	return (void *)(&usbduxfastsub[index]);
+#else
+	// success
+	return 0;
+#endif
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
+static void usbduxfastsub_disconnect(struct usb_device *udev, void *ptr)
+{
+	usbduxfastsub_t *usbduxfastsub_tmp = (usbduxfastsub_t *) ptr;
+#else
+static void usbduxfastsub_disconnect(struct usb_interface *intf)
+{
+	usbduxfastsub_t *usbduxfastsub_tmp = usb_get_intfdata(intf);
+	struct usb_device *udev = interface_to_usbdev(intf);
+#endif
+	if (!usbduxfastsub_tmp) {
+		printk("comedi_: usbduxfast: disconnect called with null pointer.\n");
+		return;
+	}
+	if (usbduxfastsub_tmp->usbdev != udev) {
+		printk("comedi_: usbduxfast: BUG! called with wrong ptr!!!\n");
+		return;
+	}
+	down(&start_stop_sem);
+	down(&usbduxfastsub_tmp->sem);
+	tidy_up(usbduxfastsub_tmp);
+	up(&usbduxfastsub_tmp->sem);
+	up(&start_stop_sem);
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi_: usbduxfast: disconnected from the usb\n");
+#endif
+}
+
+// is called when comedi-config is called
+static int usbduxfast_attach(comedi_device * dev, comedi_devconfig * it)
+{
+	int ret;
+	int index;
+	int i;
+	comedi_subdevice *s = NULL;
+	dev->private = NULL;
+
+	down(&start_stop_sem);
+	// find a valid device which has been detected by the probe function of the usb
+	index = -1;
+	for (i = 0; i < NUMUSBDUXFAST; i++) {
+		if ((usbduxfastsub[i].probed) && (!usbduxfastsub[i].attached)) {
+			index = i;
+			break;
+		}
+	}
+
+	if (index < 0) {
+		printk("comedi%d: usbduxfast: error: attach failed, no usbduxfast devs connected to the usb bus.\n", dev->minor);
+		up(&start_stop_sem);
+		return -ENODEV;
+	}
+
+	down(&(usbduxfastsub[index].sem));
+	// pointer back to the corresponding comedi device
+	usbduxfastsub[index].comedidev = dev;
+
+	// trying to upload the firmware into the chip
+	if (comedi_aux_data(it->options, 0) &&
+		it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH]) {
+		read_firmware(usbduxfastsub,
+			comedi_aux_data(it->options, 0),
+			it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH]);
+	}
+
+	dev->board_name = BOARDNAME;
+
+	/* set number of subdevices */
+	dev->n_subdevices = N_SUBDEVICES;
+
+	// allocate space for the subdevices
+	if ((ret = alloc_subdevices(dev, N_SUBDEVICES)) < 0) {
+		printk("comedi%d: usbduxfast: error alloc space for subdev\n",
+			dev->minor);
+		up(&start_stop_sem);
+		return ret;
+	}
+
+	printk("comedi%d: usbduxfast: usb-device %d is attached to comedi.\n",
+		dev->minor, index);
+	// private structure is also simply the usb-structure
+	dev->private = usbduxfastsub + index;
+	// the first subdevice is the A/D converter
+	s = dev->subdevices + SUBDEV_AD;
+	// the URBs get the comedi subdevice
+	// which is responsible for reading
+	// this is the subdevice which reads data
+	dev->read_subdev = s;
+	// the subdevice receives as private structure the
+	// usb-structure
+	s->private = NULL;
+	// analog input
+	s->type = COMEDI_SUBD_AI;
+	// readable and ref is to ground
+	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
+	// 16 channels
+	s->n_chan = 16;
+	// length of the channellist
+	s->len_chanlist = 16;
+	// callback functions
+	s->insn_read = usbduxfast_ai_insn_read;
+	s->do_cmdtest = usbduxfast_ai_cmdtest;
+	s->do_cmd = usbduxfast_ai_cmd;
+	s->cancel = usbduxfast_ai_cancel;
+	// max value from the A/D converter (12bit+1 bit for overflow)
+	s->maxdata = 0x1000;
+	// range table to convert to physical units
+	s->range_table = &range_usbduxfast_ai_range;
+
+	// finally decide that it's attached
+	usbduxfastsub[index].attached = 1;
+
+	up(&(usbduxfastsub[index].sem));
+
+	up(&start_stop_sem);
+
+	printk("comedi%d: successfully attached to usbduxfast.\n", dev->minor);
+
+	return 0;
+}
+
+static int usbduxfast_detach(comedi_device * dev)
+{
+	usbduxfastsub_t *usbduxfastsub_tmp;
+
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast: detach usb device\n", dev->minor);
+#endif
+
+	if (!dev) {
+		printk("comedi?: usbduxfast: detach without dev variable...\n");
+		return -EFAULT;
+	}
+
+	usbduxfastsub_tmp = dev->private;
+	if (!usbduxfastsub_tmp) {
+		printk("comedi?: usbduxfast: detach without ptr to usbduxfastsub[]\n");
+		return -EFAULT;
+	}
+
+	down(&usbduxfastsub_tmp->sem);
+	down(&start_stop_sem);
+	// Don't allow detach to free the private structure
+	// It's one entry of of usbduxfastsub[]
+	dev->private = NULL;
+	usbduxfastsub_tmp->attached = 0;
+	usbduxfastsub_tmp->comedidev = NULL;
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast: detach: successfully removed\n",
+		dev->minor);
+#endif
+	up(&start_stop_sem);
+	up(&usbduxfastsub_tmp->sem);
+	return 0;
+}
+
+/* main driver struct */
+static comedi_driver driver_usbduxfast = {
+      driver_name:"usbduxfast",
+      module:THIS_MODULE,
+      attach:usbduxfast_attach,
+      detach:usbduxfast_detach,
+};
+
+static void init_usb_devices(void)
+{
+	int index;
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi_: usbduxfast: setting all possible devs to invalid\n");
+#endif
+	// all devices entries are invalid to begin with
+	// they will become valid by the probe function
+	// and then finally by the attach-function
+	for (index = 0; index < NUMUSBDUXFAST; index++) {
+		memset(&(usbduxfastsub[index]), 0x00,
+			sizeof(usbduxfastsub[index]));
+		init_MUTEX(&(usbduxfastsub[index].sem));
+	}
+}
+
+// Table with the USB-devices: just now only testing IDs
+static struct usb_device_id usbduxfastsub_table[] = {
+	//        { USB_DEVICE(0x4b4, 0x8613), //testing
+	//        },
+	{USB_DEVICE(0x13d8, 0x0010)	//real ID
+		},
+	{USB_DEVICE(0x13d8, 0x0011)	//real ID
+		},
+	{}			/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, usbduxfastsub_table);
+
+// The usbduxfastsub-driver
+static struct usb_driver usbduxfastsub_driver = {
+#ifdef COMEDI_HAVE_USB_DRIVER_OWNER
+      owner:THIS_MODULE,
+#endif
+      name:BOARDNAME,
+      probe:usbduxfastsub_probe,
+      disconnect:usbduxfastsub_disconnect,
+      id_table:usbduxfastsub_table,
+};
+
+// Can't use the nice macro as I have also to initialise the USB
+// subsystem:
+// registering the usb-system _and_ the comedi-driver
+static int init_usbduxfast(void)
+{
+	printk(KERN_INFO KBUILD_MODNAME ": "
+	       DRIVER_VERSION ":" DRIVER_DESC "\n");
+	init_usb_devices();
+	usb_register(&usbduxfastsub_driver);
+	comedi_driver_register(&driver_usbduxfast);
+	return 0;
+}
+
+// deregistering the comedi driver and the usb-subsystem
+static void exit_usbduxfast(void)
+{
+	comedi_driver_unregister(&driver_usbduxfast);
+	usb_deregister(&usbduxfastsub_driver);
+}
+
+module_init(init_usbduxfast);
+module_exit(exit_usbduxfast);
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/interrupt.h b/drivers/staging/comedi/interrupt.h
new file mode 100644
index 000000000000..3038e461948a
--- /dev/null
+++ b/drivers/staging/comedi/interrupt.h
@@ -0,0 +1,60 @@
+/*
+    linux/interrupt.h compatibility header
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __COMPAT_LINUX_INTERRUPT_H_
+#define __COMPAT_LINUX_INTERRUPT_H_
+
+#include <linux/interrupt.h>
+
+#include <linux/version.h>
+
+#ifndef IRQ_NONE
+typedef void irqreturn_t;
+#define IRQ_NONE
+#define IRQ_HANDLED
+#define IRQ_RETVAL(x) (void)(x)
+#endif
+
+#ifndef IRQF_DISABLED
+#define IRQF_DISABLED           SA_INTERRUPT
+#define IRQF_SAMPLE_RANDOM      SA_SAMPLE_RANDOM
+#define IRQF_SHARED             SA_SHIRQ
+#define IRQF_PROBE_SHARED       SA_PROBEIRQ
+#define IRQF_PERCPU             SA_PERCPU
+#ifdef SA_TRIGGER_MASK
+#define IRQF_TRIGGER_NONE       0
+#define IRQF_TRIGGER_LOW        SA_TRIGGER_LOW
+#define IRQF_TRIGGER_HIGH       SA_TRIGGER_HIGH
+#define IRQF_TRIGGER_FALLING    SA_TRIGGER_FALLING
+#define IRQF_TRIGGER_RISING     SA_TRIGGER_RISING
+#define IRQF_TRIGGER_MASK       SA_TRIGGER_MASK
+#else
+#define IRQF_TRIGGER_NONE       0
+#define IRQF_TRIGGER_LOW        0
+#define IRQF_TRIGGER_HIGH       0
+#define IRQF_TRIGGER_FALLING    0
+#define IRQF_TRIGGER_RISING     0
+#define IRQF_TRIGGER_MASK       0
+#endif
+#endif
+
+#define PT_REGS_ARG
+#define PT_REGS_CALL
+#define PT_REGS_NULL
+
+#endif
diff --git a/drivers/staging/comedi/kcomedilib/Makefile b/drivers/staging/comedi/kcomedilib/Makefile
new file mode 100644
index 000000000000..ffcc9ad32adb
--- /dev/null
+++ b/drivers/staging/comedi/kcomedilib/Makefile
@@ -0,0 +1,8 @@
+obj-$(CONFIG_COMEDI)	+= kcomedilib.o
+
+kcomedilib-objs :=				\
+			data.o			\
+			ksyms.o			\
+			dio.o			\
+			kcomedilib_main.o	\
+			get.o
diff --git a/drivers/staging/comedi/kcomedilib/data.c b/drivers/staging/comedi/kcomedilib/data.c
new file mode 100644
index 000000000000..79aec2041150
--- /dev/null
+++ b/drivers/staging/comedi/kcomedilib/data.c
@@ -0,0 +1,89 @@
+/*
+    kcomedilib/data.c
+    implements comedi_data_*() functions
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#include "../comedi.h"
+#include "../comedilib.h"
+#include "../comedidev.h"	/* for comedi_udelay() */
+
+#include <linux/string.h>
+
+int comedi_data_write(comedi_t * dev, unsigned int subdev, unsigned int chan,
+	unsigned int range, unsigned int aref, lsampl_t data)
+{
+	comedi_insn insn;
+
+	memset(&insn, 0, sizeof(insn));
+	insn.insn = INSN_WRITE;
+	insn.n = 1;
+	insn.data = &data;
+	insn.subdev = subdev;
+	insn.chanspec = CR_PACK(chan, range, aref);
+
+	return comedi_do_insn(dev, &insn);
+}
+
+int comedi_data_read(comedi_t * dev, unsigned int subdev, unsigned int chan,
+	unsigned int range, unsigned int aref, lsampl_t * data)
+{
+	comedi_insn insn;
+
+	memset(&insn, 0, sizeof(insn));
+	insn.insn = INSN_READ;
+	insn.n = 1;
+	insn.data = data;
+	insn.subdev = subdev;
+	insn.chanspec = CR_PACK(chan, range, aref);
+
+	return comedi_do_insn(dev, &insn);
+}
+
+int comedi_data_read_hint(comedi_t * dev, unsigned int subdev,
+	unsigned int chan, unsigned int range, unsigned int aref)
+{
+	comedi_insn insn;
+	lsampl_t dummy_data;
+
+	memset(&insn, 0, sizeof(insn));
+	insn.insn = INSN_READ;
+	insn.n = 0;
+	insn.data = &dummy_data;
+	insn.subdev = subdev;
+	insn.chanspec = CR_PACK(chan, range, aref);
+
+	return comedi_do_insn(dev, &insn);
+}
+
+int comedi_data_read_delayed(comedi_t * dev, unsigned int subdev,
+	unsigned int chan, unsigned int range, unsigned int aref,
+	lsampl_t * data, unsigned int nano_sec)
+{
+	int retval;
+
+	retval = comedi_data_read_hint(dev, subdev, chan, range, aref);
+	if (retval < 0)
+		return retval;
+
+	comedi_udelay((nano_sec + 999) / 1000);
+
+	return comedi_data_read(dev, subdev, chan, range, aref, data);
+}
diff --git a/drivers/staging/comedi/kcomedilib/dio.c b/drivers/staging/comedi/kcomedilib/dio.c
new file mode 100644
index 000000000000..a9f488aed369
--- /dev/null
+++ b/drivers/staging/comedi/kcomedilib/dio.c
@@ -0,0 +1,95 @@
+/*
+    kcomedilib/dio.c
+    implements comedi_dio_*() functions
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#include "../comedi.h"
+#include "../comedilib.h"
+
+#include <linux/string.h>
+
+int comedi_dio_config(comedi_t * dev, unsigned int subdev, unsigned int chan,
+	unsigned int io)
+{
+	comedi_insn insn;
+
+	memset(&insn, 0, sizeof(insn));
+	insn.insn = INSN_CONFIG;
+	insn.n = 1;
+	insn.data = &io;
+	insn.subdev = subdev;
+	insn.chanspec = CR_PACK(chan, 0, 0);
+
+	return comedi_do_insn(dev, &insn);
+}
+
+int comedi_dio_read(comedi_t * dev, unsigned int subdev, unsigned int chan,
+	unsigned int *val)
+{
+	comedi_insn insn;
+
+	memset(&insn, 0, sizeof(insn));
+	insn.insn = INSN_READ;
+	insn.n = 1;
+	insn.data = val;
+	insn.subdev = subdev;
+	insn.chanspec = CR_PACK(chan, 0, 0);
+
+	return comedi_do_insn(dev, &insn);
+}
+
+int comedi_dio_write(comedi_t * dev, unsigned int subdev, unsigned int chan,
+	unsigned int val)
+{
+	comedi_insn insn;
+
+	memset(&insn, 0, sizeof(insn));
+	insn.insn = INSN_WRITE;
+	insn.n = 1;
+	insn.data = &val;
+	insn.subdev = subdev;
+	insn.chanspec = CR_PACK(chan, 0, 0);
+
+	return comedi_do_insn(dev, &insn);
+}
+
+int comedi_dio_bitfield(comedi_t * dev, unsigned int subdev, unsigned int mask,
+	unsigned int *bits)
+{
+	comedi_insn insn;
+	lsampl_t data[2];
+	int ret;
+
+	memset(&insn, 0, sizeof(insn));
+	insn.insn = INSN_BITS;
+	insn.n = 2;
+	insn.data = data;
+	insn.subdev = subdev;
+
+	data[0] = mask;
+	data[1] = *bits;
+
+	ret = comedi_do_insn(dev, &insn);
+
+	*bits = data[1];
+
+	return ret;
+}
diff --git a/drivers/staging/comedi/kcomedilib/get.c b/drivers/staging/comedi/kcomedilib/get.c
new file mode 100644
index 000000000000..2004ad4480c0
--- /dev/null
+++ b/drivers/staging/comedi/kcomedilib/get.c
@@ -0,0 +1,294 @@
+/*
+    kcomedilib/get.c
+    a comedlib interface for kernel modules
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#define __NO_VERSION__
+#include "../comedi.h"
+#include "../comedilib.h"
+#include "../comedidev.h"
+
+int comedi_get_n_subdevices(comedi_t * d)
+{
+	comedi_device *dev = (comedi_device *) d;
+
+	return dev->n_subdevices;
+}
+
+int comedi_get_version_code(comedi_t * d)
+{
+	return COMEDI_VERSION_CODE;
+}
+
+const char *comedi_get_driver_name(comedi_t * d)
+{
+	comedi_device *dev = (comedi_device *) d;
+
+	return dev->driver->driver_name;
+}
+
+const char *comedi_get_board_name(comedi_t * d)
+{
+	comedi_device *dev = (comedi_device *) d;
+
+	return dev->board_name;
+}
+
+int comedi_get_subdevice_type(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+
+	return s->type;
+}
+
+unsigned int comedi_get_subdevice_flags(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+
+	return s->subdev_flags;
+}
+
+int comedi_find_subdevice_by_type(comedi_t * d, int type, unsigned int subd)
+{
+	comedi_device *dev = (comedi_device *) d;
+
+	if (subd > dev->n_subdevices)
+		return -ENODEV;
+
+	for (; subd < dev->n_subdevices; subd++) {
+		if (dev->subdevices[subd].type == type)
+			return subd;
+	}
+	return -1;
+}
+
+int comedi_get_n_channels(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+
+	return s->n_chan;
+}
+
+int comedi_get_len_chanlist(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+
+	return s->len_chanlist;
+}
+
+lsampl_t comedi_get_maxdata(comedi_t * d, unsigned int subdevice,
+	unsigned int chan)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+
+	if (s->maxdata_list)
+		return s->maxdata_list[chan];
+
+	return s->maxdata;
+}
+
+#ifdef KCOMEDILIB_DEPRECATED
+int comedi_get_rangetype(comedi_t * d, unsigned int subdevice,
+	unsigned int chan)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	int ret;
+
+	if (s->range_table_list) {
+		ret = s->range_table_list[chan]->length;
+	} else {
+		ret = s->range_table->length;
+	}
+
+	ret = ret | (dev->minor << 28) | (subdevice << 24) | (chan << 16);
+
+	return ret;
+}
+#endif
+
+int comedi_get_n_ranges(comedi_t * d, unsigned int subdevice, unsigned int chan)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	int ret;
+
+	if (s->range_table_list) {
+		ret = s->range_table_list[chan]->length;
+	} else {
+		ret = s->range_table->length;
+	}
+
+	return ret;
+}
+
+/*
+ * ALPHA (non-portable)
+*/
+int comedi_get_krange(comedi_t * d, unsigned int subdevice, unsigned int chan,
+	unsigned int range, comedi_krange * krange)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	const comedi_lrange *lr;
+
+	if (s->range_table_list) {
+		lr = s->range_table_list[chan];
+	} else {
+		lr = s->range_table;
+	}
+	if (range >= lr->length) {
+		return -EINVAL;
+	}
+	memcpy(krange, lr->range + range, sizeof(comedi_krange));
+
+	return 0;
+}
+
+/*
+ * ALPHA (may be renamed)
+*/
+unsigned int comedi_get_buf_head_pos(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	comedi_async *async;
+
+	async = s->async;
+	if (async == NULL)
+		return 0;
+
+	return async->buf_write_count;
+}
+
+int comedi_get_buffer_contents(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	comedi_async *async;
+	unsigned int num_bytes;
+
+	if (subdevice >= dev->n_subdevices)
+		return -1;
+	async = s->async;
+	if (async == NULL)
+		return 0;
+	num_bytes = comedi_buf_read_n_available(s->async);
+	return num_bytes;
+}
+
+/*
+ * ALPHA
+*/
+int comedi_set_user_int_count(comedi_t * d, unsigned int subdevice,
+	unsigned int buf_user_count)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	comedi_async *async;
+	int num_bytes;
+
+	async = s->async;
+	if (async == NULL)
+		return -1;
+
+	num_bytes = buf_user_count - async->buf_read_count;
+	if (num_bytes < 0)
+		return -1;
+	comedi_buf_read_alloc(async, num_bytes);
+	comedi_buf_read_free(async, num_bytes);
+
+	return 0;
+}
+
+int comedi_mark_buffer_read(comedi_t * d, unsigned int subdevice,
+	unsigned int num_bytes)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	comedi_async *async;
+
+	if (subdevice >= dev->n_subdevices)
+		return -1;
+	async = s->async;
+	if (async == NULL)
+		return -1;
+
+	comedi_buf_read_alloc(async, num_bytes);
+	comedi_buf_read_free(async, num_bytes);
+
+	return 0;
+}
+
+int comedi_mark_buffer_written(comedi_t * d, unsigned int subdevice,
+	unsigned int num_bytes)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	comedi_async *async;
+	int bytes_written;
+
+	if (subdevice >= dev->n_subdevices)
+		return -1;
+	async = s->async;
+	if (async == NULL)
+		return -1;
+	bytes_written = comedi_buf_write_alloc(async, num_bytes);
+	comedi_buf_write_free(async, bytes_written);
+	if (bytes_written != num_bytes)
+		return -1;
+	return 0;
+}
+
+int comedi_get_buffer_size(comedi_t * d, unsigned int subdev)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdev;
+	comedi_async *async;
+
+	if (subdev >= dev->n_subdevices)
+		return -1;
+	async = s->async;
+	if (async == NULL)
+		return 0;
+
+	return async->prealloc_bufsz;
+}
+
+int comedi_get_buffer_offset(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	comedi_async *async;
+
+	if (subdevice >= dev->n_subdevices)
+		return -1;
+	async = s->async;
+	if (async == NULL)
+		return 0;
+
+	return async->buf_read_ptr;
+}
diff --git a/drivers/staging/comedi/kcomedilib/kcomedilib_main.c b/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
new file mode 100644
index 000000000000..510fbdcec493
--- /dev/null
+++ b/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
@@ -0,0 +1,567 @@
+/*
+    kcomedilib/kcomedilib.c
+    a comedlib interface for kernel modules
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#define __NO_VERSION__
+#include <linux/module.h>
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/fcntl.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <asm/io.h>
+
+#include "../comedi.h"
+#include "../comedilib.h"
+#include "../comedidev.h"
+
+MODULE_AUTHOR("David Schleef <ds@schleef.org>");
+MODULE_DESCRIPTION("Comedi kernel library");
+MODULE_LICENSE("GPL");
+
+comedi_t *comedi_open(const char *filename)
+{
+	struct comedi_device_file_info *dev_file_info;
+	comedi_device *dev;
+	unsigned int minor;
+
+	if (strncmp(filename, "/dev/comedi", 11) != 0)
+		return NULL;
+
+	minor = simple_strtoul(filename + 11, NULL, 0);
+
+	if (minor >= COMEDI_NUM_BOARD_MINORS)
+		return NULL;
+
+	dev_file_info = comedi_get_device_file_info(minor);
+	if(dev_file_info == NULL)
+		return NULL;
+	dev = dev_file_info->device;
+
+	if(dev == NULL || !dev->attached)
+		return NULL;
+
+	if (!try_module_get(dev->driver->module))
+		return NULL;
+
+	return (comedi_t *) dev;
+}
+
+comedi_t *comedi_open_old(unsigned int minor)
+{
+	struct comedi_device_file_info *dev_file_info;
+	comedi_device *dev;
+
+	if (minor >= COMEDI_NUM_MINORS)
+		return NULL;
+
+	dev_file_info = comedi_get_device_file_info(minor);
+	if(dev_file_info == NULL)
+		return NULL;
+	dev = dev_file_info->device;
+
+	if(dev == NULL || !dev->attached)
+		return NULL;
+
+	return (comedi_t *) dev;
+}
+
+int comedi_close(comedi_t * d)
+{
+	comedi_device *dev = (comedi_device *) d;
+
+	module_put(dev->driver->module);
+
+	return 0;
+}
+
+int comedi_loglevel(int newlevel)
+{
+	return 0;
+}
+
+void comedi_perror(const char *message)
+{
+	rt_printk("%s: unknown error\n", message);
+}
+
+char *comedi_strerror(int err)
+{
+	return "unknown error";
+}
+
+int comedi_fileno(comedi_t * d)
+{
+	comedi_device *dev = (comedi_device *) d;
+
+	/* return something random */
+	return dev->minor;
+}
+
+int comedi_command(comedi_t * d, comedi_cmd * cmd)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+	comedi_async *async;
+	unsigned runflags;
+
+	if (cmd->subdev >= dev->n_subdevices)
+		return -ENODEV;
+
+	s = dev->subdevices + cmd->subdev;
+	if (s->type == COMEDI_SUBD_UNUSED)
+		return -EIO;
+
+	async = s->async;
+	if (async == NULL)
+		return -ENODEV;
+
+	if (s->busy)
+		return -EBUSY;
+	s->busy = d;
+
+	if (async->cb_mask & COMEDI_CB_EOS)
+		cmd->flags |= TRIG_WAKE_EOS;
+
+	async->cmd = *cmd;
+
+	runflags = SRF_RUNNING;
+
+#ifdef CONFIG_COMEDI_RT
+	if (comedi_switch_to_rt(dev) == 0)
+		runflags |= SRF_RT;
+#endif
+	comedi_set_subdevice_runflags(s, ~0, runflags);
+
+	comedi_reset_async_buf(async);
+
+	return s->do_cmd(dev, s);
+}
+
+int comedi_command_test(comedi_t * d, comedi_cmd * cmd)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+
+	if (cmd->subdev >= dev->n_subdevices)
+		return -ENODEV;
+
+	s = dev->subdevices + cmd->subdev;
+	if (s->type == COMEDI_SUBD_UNUSED)
+		return -EIO;
+
+	if (s->async == NULL)
+		return -ENODEV;
+
+	return s->do_cmdtest(dev, s, cmd);
+}
+
+/*
+ *	COMEDI_INSN
+ *	perform an instruction
+ */
+int comedi_do_insn(comedi_t * d, comedi_insn * insn)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+	int ret = 0;
+
+	if (insn->insn & INSN_MASK_SPECIAL) {
+		switch (insn->insn) {
+		case INSN_GTOD:
+			{
+				struct timeval tv;
+
+				do_gettimeofday(&tv);
+				insn->data[0] = tv.tv_sec;
+				insn->data[1] = tv.tv_usec;
+				ret = 2;
+
+				break;
+			}
+		case INSN_WAIT:
+			/* XXX isn't the value supposed to be nanosecs? */
+			if (insn->n != 1 || insn->data[0] >= 100) {
+				ret = -EINVAL;
+				break;
+			}
+			comedi_udelay(insn->data[0]);
+			ret = 1;
+			break;
+		case INSN_INTTRIG:
+			if (insn->n != 1) {
+				ret = -EINVAL;
+				break;
+			}
+			if (insn->subdev >= dev->n_subdevices) {
+				rt_printk("%d not usable subdevice\n",
+					insn->subdev);
+				ret = -EINVAL;
+				break;
+			}
+			s = dev->subdevices + insn->subdev;
+			if (!s->async) {
+				rt_printk("no async\n");
+				ret = -EINVAL;
+				break;
+			}
+			if (!s->async->inttrig) {
+				rt_printk("no inttrig\n");
+				ret = -EAGAIN;
+				break;
+			}
+			ret = s->async->inttrig(dev, s, insn->data[0]);
+			if (ret >= 0)
+				ret = 1;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+	} else {
+		/* a subdevice instruction */
+		if (insn->subdev >= dev->n_subdevices) {
+			ret = -EINVAL;
+			goto error;
+		}
+		s = dev->subdevices + insn->subdev;
+
+		if (s->type == COMEDI_SUBD_UNUSED) {
+			rt_printk("%d not useable subdevice\n", insn->subdev);
+			ret = -EIO;
+			goto error;
+		}
+
+		/* XXX check lock */
+
+		if ((ret = check_chanlist(s, 1, &insn->chanspec)) < 0) {
+			rt_printk("bad chanspec\n");
+			ret = -EINVAL;
+			goto error;
+		}
+
+		if (s->busy) {
+			ret = -EBUSY;
+			goto error;
+		}
+		s->busy = d;
+
+		switch (insn->insn) {
+		case INSN_READ:
+			ret = s->insn_read(dev, s, insn, insn->data);
+			break;
+		case INSN_WRITE:
+			ret = s->insn_write(dev, s, insn, insn->data);
+			break;
+		case INSN_BITS:
+			ret = s->insn_bits(dev, s, insn, insn->data);
+			break;
+		case INSN_CONFIG:
+			/* XXX should check instruction length */
+			ret = s->insn_config(dev, s, insn, insn->data);
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		s->busy = NULL;
+	}
+	if (ret < 0)
+		goto error;
+#if 0
+	/* XXX do we want this? -- abbotti #if'ed it out for now. */
+	if (ret != insn->n) {
+		rt_printk("BUG: result of insn != insn.n\n");
+		ret = -EINVAL;
+		goto error;
+	}
+#endif
+      error:
+
+	return ret;
+}
+
+/*
+	COMEDI_LOCK
+	lock subdevice
+
+	arg:
+		subdevice number
+
+	reads:
+		none
+
+	writes:
+		none
+
+	necessary locking:
+	- ioctl/rt lock  (this type)
+	- lock while subdevice busy
+	- lock while subdevice being programmed
+
+*/
+int comedi_lock(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+	unsigned long flags;
+	int ret = 0;
+
+	if (subdevice >= dev->n_subdevices) {
+		return -EINVAL;
+	}
+	s = dev->subdevices + subdevice;
+
+	comedi_spin_lock_irqsave(&s->spin_lock, flags);
+
+	if (s->busy) {
+		ret = -EBUSY;
+	} else {
+		if (s->lock) {
+			ret = -EBUSY;
+		} else {
+			s->lock = d;
+		}
+	}
+
+	comedi_spin_unlock_irqrestore(&s->spin_lock, flags);
+
+	return ret;
+}
+
+/*
+	COMEDI_UNLOCK
+	unlock subdevice
+
+	arg:
+		subdevice number
+
+	reads:
+		none
+
+	writes:
+		none
+
+*/
+int comedi_unlock(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+	unsigned long flags;
+	comedi_async *async;
+	int ret;
+
+	if (subdevice >= dev->n_subdevices) {
+		return -EINVAL;
+	}
+	s = dev->subdevices + subdevice;
+
+	async = s->async;
+
+	comedi_spin_lock_irqsave(&s->spin_lock, flags);
+
+	if (s->busy) {
+		ret = -EBUSY;
+	} else if (s->lock && s->lock != (void *)d) {
+		ret = -EACCES;
+	} else {
+		s->lock = NULL;
+
+		if (async) {
+			async->cb_mask = 0;
+			async->cb_func = NULL;
+			async->cb_arg = NULL;
+		}
+
+		ret = 0;
+	}
+
+	comedi_spin_unlock_irqrestore(&s->spin_lock, flags);
+
+	return ret;
+}
+
+/*
+	COMEDI_CANCEL
+	cancel acquisition ioctl
+
+	arg:
+		subdevice number
+
+	reads:
+		nothing
+
+	writes:
+		nothing
+
+*/
+int comedi_cancel(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+	int ret = 0;
+
+	if (subdevice >= dev->n_subdevices) {
+		return -EINVAL;
+	}
+	s = dev->subdevices + subdevice;
+
+	if (s->lock && s->lock != d)
+		return -EACCES;
+
+#if 0
+	if (!s->busy)
+		return 0;
+
+	if (s->busy != d)
+		return -EBUSY;
+#endif
+
+	if (!s->cancel || !s->async)
+		return -EINVAL;
+
+	if ((ret = s->cancel(dev, s)))
+		return ret;
+
+#ifdef CONFIG_COMEDI_RT
+	if (comedi_get_subdevice_runflags(s) & SRF_RT) {
+		comedi_switch_to_non_rt(dev);
+	}
+#endif
+	comedi_set_subdevice_runflags(s, SRF_RUNNING | SRF_RT, 0);
+	s->async->inttrig = NULL;
+	s->busy = NULL;
+
+	return 0;
+}
+
+/*
+   registration of callback functions
+ */
+int comedi_register_callback(comedi_t * d, unsigned int subdevice,
+	unsigned int mask, int (*cb) (unsigned int, void *), void *arg)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+	comedi_async *async;
+
+	if (subdevice >= dev->n_subdevices) {
+		return -EINVAL;
+	}
+	s = dev->subdevices + subdevice;
+
+	async = s->async;
+	if (s->type == COMEDI_SUBD_UNUSED || !async)
+		return -EIO;
+
+	/* are we locked? (ioctl lock) */
+	if (s->lock && s->lock != d)
+		return -EACCES;
+
+	/* are we busy? */
+	if (s->busy)
+		return -EBUSY;
+
+	if (!mask) {
+		async->cb_mask = 0;
+		async->cb_func = NULL;
+		async->cb_arg = NULL;
+	} else {
+		async->cb_mask = mask;
+		async->cb_func = cb;
+		async->cb_arg = arg;
+	}
+
+	return 0;
+}
+
+int comedi_poll(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices;
+	comedi_async *async;
+
+	if (subdevice >= dev->n_subdevices) {
+		return -EINVAL;
+	}
+	s = dev->subdevices + subdevice;
+
+	async = s->async;
+	if (s->type == COMEDI_SUBD_UNUSED || !async)
+		return -EIO;
+
+	/* are we locked? (ioctl lock) */
+	if (s->lock && s->lock != d)
+		return -EACCES;
+
+	/* are we running? XXX wrong? */
+	if (!s->busy)
+		return -EIO;
+
+	return s->poll(dev, s);
+}
+
+/* WARNING: not portable */
+int comedi_map(comedi_t * d, unsigned int subdevice, void *ptr)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+
+	if (subdevice >= dev->n_subdevices) {
+		return -EINVAL;
+	}
+	s = dev->subdevices + subdevice;
+
+	if (!s->async)
+		return -EINVAL;
+
+	if (ptr) {
+		*((void **)ptr) = s->async->prealloc_buf;
+	}
+
+	/* XXX no reference counting */
+
+	return 0;
+}
+
+/* WARNING: not portable */
+int comedi_unmap(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+
+	if (subdevice >= dev->n_subdevices) {
+		return -EINVAL;
+	}
+	s = dev->subdevices + subdevice;
+
+	if (!s->async)
+		return -EINVAL;
+
+	/* XXX no reference counting */
+
+	return 0;
+}
diff --git a/drivers/staging/comedi/kcomedilib/ksyms.c b/drivers/staging/comedi/kcomedilib/ksyms.c
new file mode 100644
index 000000000000..76b45063a9fc
--- /dev/null
+++ b/drivers/staging/comedi/kcomedilib/ksyms.c
@@ -0,0 +1,144 @@
+/*
+    comedi/kcomedilib/ksyms.c
+    a comedlib interface for kernel modules
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2001 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef EXPORT_SYMTAB
+#define EXPORT_SYMTAB
+#endif
+
+#include "../comedi.h"
+#include "../comedilib.h"
+#include "../comedidev.h"
+
+#include <linux/module.h>
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/fcntl.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+
+#if LINUX_VERSION_CODE >= 0x020200
+
+/* functions specific to kcomedilib */
+
+EXPORT_SYMBOL(comedi_register_callback);
+EXPORT_SYMBOL(comedi_get_krange);
+EXPORT_SYMBOL(comedi_get_buf_head_pos);
+EXPORT_SYMBOL(comedi_set_user_int_count);
+EXPORT_SYMBOL(comedi_map);
+EXPORT_SYMBOL(comedi_unmap);
+
+/* This list comes from user-space comedilib, to show which
+ * functions are not ported yet. */
+
+EXPORT_SYMBOL(comedi_open);
+EXPORT_SYMBOL(comedi_close);
+
+/* logging */
+EXPORT_SYMBOL(comedi_loglevel);
+EXPORT_SYMBOL(comedi_perror);
+EXPORT_SYMBOL(comedi_strerror);
+//EXPORT_SYMBOL(comedi_errno);
+EXPORT_SYMBOL(comedi_fileno);
+
+/* device queries */
+EXPORT_SYMBOL(comedi_get_n_subdevices);
+EXPORT_SYMBOL(comedi_get_version_code);
+EXPORT_SYMBOL(comedi_get_driver_name);
+EXPORT_SYMBOL(comedi_get_board_name);
+
+/* subdevice queries */
+EXPORT_SYMBOL(comedi_get_subdevice_type);
+EXPORT_SYMBOL(comedi_find_subdevice_by_type);
+EXPORT_SYMBOL(comedi_get_subdevice_flags);
+EXPORT_SYMBOL(comedi_get_n_channels);
+//EXPORT_SYMBOL(comedi_range_is_chan_specific);
+//EXPORT_SYMBOL(comedi_maxdata_is_chan_specific);
+
+/* channel queries */
+EXPORT_SYMBOL(comedi_get_maxdata);
+#ifdef KCOMEDILIB_DEPRECATED
+EXPORT_SYMBOL(comedi_get_rangetype);
+#endif
+EXPORT_SYMBOL(comedi_get_n_ranges);
+//EXPORT_SYMBOL(comedi_find_range);
+
+/* buffer queries */
+EXPORT_SYMBOL(comedi_get_buffer_size);
+//EXPORT_SYMBOL(comedi_get_max_buffer_size);
+//EXPORT_SYMBOL(comedi_set_buffer_size);
+EXPORT_SYMBOL(comedi_get_buffer_contents);
+EXPORT_SYMBOL(comedi_get_buffer_offset);
+
+/* low-level stuff */
+//EXPORT_SYMBOL(comedi_trigger);
+//EXPORT_SYMBOL(comedi_do_insnlist);
+EXPORT_SYMBOL(comedi_do_insn);
+EXPORT_SYMBOL(comedi_lock);
+EXPORT_SYMBOL(comedi_unlock);
+
+/* physical units */
+//EXPORT_SYMBOL(comedi_to_phys);
+//EXPORT_SYMBOL(comedi_from_phys);
+
+/* synchronous stuff */
+EXPORT_SYMBOL(comedi_data_read);
+EXPORT_SYMBOL(comedi_data_read_hint);
+EXPORT_SYMBOL(comedi_data_read_delayed);
+EXPORT_SYMBOL(comedi_data_write);
+EXPORT_SYMBOL(comedi_dio_config);
+EXPORT_SYMBOL(comedi_dio_read);
+EXPORT_SYMBOL(comedi_dio_write);
+EXPORT_SYMBOL(comedi_dio_bitfield);
+
+/* slowly varying stuff */
+//EXPORT_SYMBOL(comedi_sv_init);
+//EXPORT_SYMBOL(comedi_sv_update);
+//EXPORT_SYMBOL(comedi_sv_measure);
+
+/* commands */
+//EXPORT_SYMBOL(comedi_get_cmd_src_mask);
+//EXPORT_SYMBOL(comedi_get_cmd_generic_timed);
+EXPORT_SYMBOL(comedi_cancel);
+EXPORT_SYMBOL(comedi_command);
+EXPORT_SYMBOL(comedi_command_test);
+EXPORT_SYMBOL(comedi_poll);
+
+/* buffer configuration */
+EXPORT_SYMBOL(comedi_mark_buffer_read);
+EXPORT_SYMBOL(comedi_mark_buffer_written);
+
+//EXPORT_SYMBOL(comedi_get_range);
+EXPORT_SYMBOL(comedi_get_len_chanlist);
+
+/* deprecated */
+//EXPORT_SYMBOL(comedi_get_timer);
+//EXPORT_SYMBOL(comedi_timed_1chan);
+
+/* alpha */
+//EXPORT_SYMBOL(comedi_set_global_oor_behavior);
+
+#endif
diff --git a/drivers/staging/comedi/pci_ids.h b/drivers/staging/comedi/pci_ids.h
new file mode 100644
index 000000000000..c61ba90f9601
--- /dev/null
+++ b/drivers/staging/comedi/pci_ids.h
@@ -0,0 +1,31 @@
+/***************************************************************************
+ *                                                                         *
+ *   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 __COMPAT_LINUX_PCI_IDS_H
+#define __COMPAT_LINUX_PCI_IDS_H
+
+#include <linux/pci_ids.h>
+
+#ifndef PCI_VENDOR_ID_AMCC
+#define PCI_VENDOR_ID_AMCC	0x10e8
+#endif
+
+#ifndef PCI_VENDOR_ID_CBOARDS
+#define PCI_VENDOR_ID_CBOARDS	0x1307
+#endif
+
+#ifndef PCI_VENDOR_ID_QUANCOM
+#define PCI_VENDOR_ID_QUANCOM	0x8008
+#endif
+
+#ifndef PCI_DEVICE_ID_QUANCOM_GPIB
+#define PCI_DEVICE_ID_QUANCOM_GPIB	0x3302
+#endif
+
+#endif // __COMPAT_LINUX_PCI_IDS_H
diff --git a/drivers/staging/comedi/proc.c b/drivers/staging/comedi/proc.c
new file mode 100644
index 000000000000..5a2b72d87572
--- /dev/null
+++ b/drivers/staging/comedi/proc.c
@@ -0,0 +1,102 @@
+/*
+    module/proc.c
+    /proc interface for comedi
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1998 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+/*
+	This is some serious bloatware.
+
+	Taken from Dave A.'s PCL-711 driver, 'cuz I thought it
+	was cool.
+*/
+
+#define __NO_VERSION__
+#include "comedidev.h"
+#include <linux/proc_fs.h>
+//#include <linux/string.h>
+
+int comedi_read_procmem(char *buf, char **start, off_t offset, int len,
+	int *eof, void *data);
+
+extern comedi_driver *comedi_drivers;
+
+int comedi_read_procmem(char *buf, char **start, off_t offset, int len,
+	int *eof, void *data)
+{
+	int i;
+	int devices_q = 0;
+	int l = 0;
+	comedi_driver *driv;
+
+	l += sprintf(buf + l,
+		"comedi version " COMEDI_RELEASE "\n"
+		"format string: %s\n",
+		"\"%2d: %-20s %-20s %4d\",i,driver_name,board_name,n_subdevices");
+
+	for (i = 0; i < COMEDI_NUM_BOARD_MINORS; i++) {
+		struct comedi_device_file_info *dev_file_info = comedi_get_device_file_info(i);
+		comedi_device *dev;
+
+		if(dev_file_info == NULL) continue;
+		dev = dev_file_info->device;
+
+		if (dev->attached) {
+			devices_q = 1;
+			l += sprintf(buf + l, "%2d: %-20s %-20s %4d\n",
+				i,
+				dev->driver->driver_name,
+				dev->board_name, dev->n_subdevices);
+		}
+	}
+	if (!devices_q) {
+		l += sprintf(buf + l, "no devices\n");
+	}
+
+	for (driv = comedi_drivers; driv; driv = driv->next) {
+		l += sprintf(buf + l, "%s:\n", driv->driver_name);
+		for (i = 0; i < driv->num_names; i++) {
+			l += sprintf(buf + l, " %s\n",
+				*(char **)((char *)driv->board_name +
+					i * driv->offset));
+		}
+		if (!driv->num_names) {
+			l += sprintf(buf + l, " %s\n", driv->driver_name);
+		}
+	}
+
+	return l;
+}
+
+#ifdef CONFIG_PROC_FS
+void comedi_proc_init(void)
+{
+	struct proc_dir_entry *comedi_proc;
+
+	comedi_proc = create_proc_entry("comedi", S_IFREG | S_IRUGO, 0);
+	if (comedi_proc)
+		comedi_proc->read_proc = comedi_read_procmem;
+}
+
+void comedi_proc_cleanup(void)
+{
+	remove_proc_entry("comedi", 0);
+}
+#endif
diff --git a/drivers/staging/comedi/range.c b/drivers/staging/comedi/range.c
new file mode 100644
index 000000000000..61dc3cd6a9fd
--- /dev/null
+++ b/drivers/staging/comedi/range.c
@@ -0,0 +1,161 @@
+/*
+    module/range.c
+    comedi routines for voltage ranges
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-8 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#include "comedidev.h"
+#include <asm/uaccess.h>
+
+const comedi_lrange range_bipolar10 = { 1, {BIP_RANGE(10)} };
+const comedi_lrange range_bipolar5 = { 1, {BIP_RANGE(5)} };
+const comedi_lrange range_bipolar2_5 = { 1, {BIP_RANGE(2.5)} };
+const comedi_lrange range_unipolar10 = { 1, {UNI_RANGE(10)} };
+const comedi_lrange range_unipolar5 = { 1, {UNI_RANGE(5)} };
+const comedi_lrange range_unknown = { 1, {{0, 1000000, UNIT_none}} };
+
+/*
+   	COMEDI_RANGEINFO
+	range information ioctl
+
+	arg:
+		pointer to rangeinfo structure
+
+	reads:
+		range info structure
+
+	writes:
+		n comedi_krange structures to rangeinfo->range_ptr
+*/
+int do_rangeinfo_ioctl(comedi_device * dev, comedi_rangeinfo * arg)
+{
+	comedi_rangeinfo it;
+	int subd, chan;
+	const comedi_lrange *lr;
+	comedi_subdevice *s;
+
+	if (copy_from_user(&it, arg, sizeof(comedi_rangeinfo)))
+		return -EFAULT;
+	subd = (it.range_type >> 24) & 0xf;
+	chan = (it.range_type >> 16) & 0xff;
+
+	if (!dev->attached)
+		return -EINVAL;
+	if (subd >= dev->n_subdevices)
+		return -EINVAL;
+	s = dev->subdevices + subd;
+	if (s->range_table) {
+		lr = s->range_table;
+	} else if (s->range_table_list) {
+		if (chan >= s->n_chan)
+			return -EINVAL;
+		lr = s->range_table_list[chan];
+	} else {
+		return -EINVAL;
+	}
+
+	if (RANGE_LENGTH(it.range_type) != lr->length) {
+		DPRINTK("wrong length %d should be %d (0x%08x)\n",
+			RANGE_LENGTH(it.range_type), lr->length, it.range_type);
+		return -EINVAL;
+	}
+
+	if (copy_to_user(it.range_ptr, lr->range,
+			sizeof(comedi_krange) * lr->length))
+		return -EFAULT;
+
+	return 0;
+}
+
+static int aref_invalid(comedi_subdevice * s, unsigned int chanspec)
+{
+	unsigned int aref;
+
+	// disable reporting invalid arefs... maybe someday
+	return 0;
+
+	aref = CR_AREF(chanspec);
+	switch (aref) {
+	case AREF_DIFF:
+		if (s->subdev_flags & SDF_DIFF)
+			return 0;
+		break;
+	case AREF_COMMON:
+		if (s->subdev_flags & SDF_COMMON)
+			return 0;
+		break;
+	case AREF_GROUND:
+		if (s->subdev_flags & SDF_GROUND)
+			return 0;
+		break;
+	case AREF_OTHER:
+		if (s->subdev_flags & SDF_OTHER)
+			return 0;
+		break;
+	default:
+		break;
+	}
+	DPRINTK("subdevice does not support aref %i", aref);
+	return 1;
+}
+
+/*
+   This function checks each element in a channel/gain list to make
+   make sure it is valid.
+*/
+int check_chanlist(comedi_subdevice * s, int n, unsigned int *chanlist)
+{
+	int i;
+	int chan;
+
+	if (s->range_table) {
+		for (i = 0; i < n; i++)
+			if (CR_CHAN(chanlist[i]) >= s->n_chan ||
+				CR_RANGE(chanlist[i]) >= s->range_table->length
+				|| aref_invalid(s, chanlist[i])) {
+				rt_printk
+					("bad chanlist[%d]=0x%08x n_chan=%d range length=%d\n",
+					i, chanlist[i], s->n_chan,
+					s->range_table->length);
+#if 0
+				for (i = 0; i < n; i++) {
+					printk("[%d]=0x%08x\n", i, chanlist[i]);
+				}
+#endif
+				return -EINVAL;
+			}
+	} else if (s->range_table_list) {
+		for (i = 0; i < n; i++) {
+			chan = CR_CHAN(chanlist[i]);
+			if (chan >= s->n_chan ||
+				CR_RANGE(chanlist[i]) >=
+				s->range_table_list[chan]->length
+				|| aref_invalid(s, chanlist[i])) {
+				rt_printk("bad chanlist[%d]=0x%08x\n", i,
+					chanlist[i]);
+				return -EINVAL;
+			}
+		}
+	} else {
+		rt_printk("comedi: (bug) no range type list!\n");
+		return -EINVAL;
+	}
+	return 0;
+}
diff --git a/drivers/staging/comedi/rt.c b/drivers/staging/comedi/rt.c
new file mode 100644
index 000000000000..385b81b94ac5
--- /dev/null
+++ b/drivers/staging/comedi/rt.c
@@ -0,0 +1,412 @@
+/*
+    comedi/rt.c
+    comedi kernel module
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#undef DEBUG
+
+#define __NO_VERSION__
+#include <linux/comedidev.h>
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/fcntl.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <asm/io.h>
+
+#include "rt_pend_tq.h"
+
+#ifdef CONFIG_COMEDI_RTAI
+#include <rtai.h>
+#endif
+
+#ifdef CONFIG_COMEDI_FUSION
+#include <nucleus/asm/hal.h>
+#endif
+
+#ifdef CONFIG_COMEDI_RTL
+#include <rtl_core.h>
+#include <rtl_sync.h>
+#endif
+
+struct comedi_irq_struct {
+	int rt;
+	int irq;
+	 irqreturn_t(*handler) (int irq, void *dev_id PT_REGS_ARG);
+	unsigned long flags;
+	const char *device;
+	comedi_device *dev_id;
+};
+
+static int comedi_rt_get_irq(struct comedi_irq_struct *it);
+static int comedi_rt_release_irq(struct comedi_irq_struct *it);
+
+static struct comedi_irq_struct *comedi_irqs[NR_IRQS];
+
+int comedi_request_irq(unsigned irq, irqreturn_t(*handler) (int,
+		void *PT_REGS_ARG), unsigned long flags, const char *device,
+	comedi_device * dev_id)
+{
+	struct comedi_irq_struct *it;
+	int ret;
+	/* null shared interrupt flag, since rt interrupt handlers do not
+	 * support it, and this version of comedi_request_irq() is only
+	 * called for kernels with rt support */
+	unsigned long unshared_flags = flags & ~IRQF_SHARED;
+
+	ret = request_irq(irq, handler, unshared_flags, device, dev_id);
+	if (ret < 0) {
+		// we failed, so fall back on allowing shared interrupt (which we won't ever make RT)
+		if (flags & IRQF_SHARED) {
+			rt_printk
+				("comedi: cannot get unshared interrupt, will not use RT interrupts.\n");
+			ret = request_irq(irq, handler, flags, device, dev_id);
+		}
+		if (ret < 0) {
+			return ret;
+		}
+	} else {
+		it = kzalloc(sizeof(struct comedi_irq_struct), GFP_KERNEL);
+		if (!it)
+			return -ENOMEM;
+
+		it->handler = handler;
+		it->irq = irq;
+		it->dev_id = dev_id;
+		it->device = device;
+		it->flags = unshared_flags;
+		comedi_irqs[irq] = it;
+	}
+	return 0;
+}
+
+void comedi_free_irq(unsigned int irq, comedi_device * dev_id)
+{
+	struct comedi_irq_struct *it;
+
+	free_irq(irq, dev_id);
+
+	it = comedi_irqs[irq];
+	if (it == NULL)
+		return;
+
+	if (it->rt) {
+		printk("real-time IRQ allocated at board removal (ignore)\n");
+		comedi_rt_release_irq(it);
+	}
+
+	kfree(it);
+	comedi_irqs[irq] = NULL;
+}
+
+int comedi_switch_to_rt(comedi_device * dev)
+{
+	struct comedi_irq_struct *it;
+	unsigned long flags;
+
+	it = comedi_irqs[dev->irq];
+	/* drivers might not be using an interrupt for commands,
+	   or we might not have been able to get an unshared irq */
+	if (it == NULL)
+		return -1;
+
+	comedi_spin_lock_irqsave(&dev->spinlock, flags);
+
+	if (!dev->rt)
+		comedi_rt_get_irq(it);
+
+	dev->rt++;
+	it->rt = 1;
+
+	comedi_spin_unlock_irqrestore(&dev->spinlock, flags);
+
+	return 0;
+}
+
+void comedi_switch_to_non_rt(comedi_device * dev)
+{
+	struct comedi_irq_struct *it;
+	unsigned long flags;
+
+	it = comedi_irqs[dev->irq];
+	if (it == NULL)
+		return;
+
+	comedi_spin_lock_irqsave(&dev->spinlock, flags);
+
+	dev->rt--;
+	if (!dev->rt)
+		comedi_rt_release_irq(it);
+
+	it->rt = 0;
+
+	comedi_spin_unlock_irqrestore(&dev->spinlock, flags);
+}
+
+void wake_up_int_handler(int arg1, void *arg2)
+{
+	wake_up_interruptible((wait_queue_head_t *) arg2);
+}
+
+void comedi_rt_pend_wakeup(wait_queue_head_t * q)
+{
+	rt_pend_call(wake_up_int_handler, 0, q);
+}
+
+/* RTAI section */
+#ifdef CONFIG_COMEDI_RTAI
+
+#ifndef HAVE_RT_REQUEST_IRQ_WITH_ARG
+#define DECLARE_VOID_IRQ(irq) \
+static void handle_void_irq_ ## irq (void){ handle_void_irq(irq);}
+
+static void handle_void_irq(int irq)
+{
+	struct comedi_irq_struct *it;
+
+	it = comedi_irqs[irq];
+	if (it == NULL) {
+		rt_printk("comedi: null irq struct?\n");
+		return;
+	}
+	it->handler(irq, it->dev_id PT_REGS_NULL);
+	rt_enable_irq(irq);	//needed by rtai-adeos, seems like it shouldn't hurt earlier versions
+}
+
+DECLARE_VOID_IRQ(0);
+DECLARE_VOID_IRQ(1);
+DECLARE_VOID_IRQ(2);
+DECLARE_VOID_IRQ(3);
+DECLARE_VOID_IRQ(4);
+DECLARE_VOID_IRQ(5);
+DECLARE_VOID_IRQ(6);
+DECLARE_VOID_IRQ(7);
+DECLARE_VOID_IRQ(8);
+DECLARE_VOID_IRQ(9);
+DECLARE_VOID_IRQ(10);
+DECLARE_VOID_IRQ(11);
+DECLARE_VOID_IRQ(12);
+DECLARE_VOID_IRQ(13);
+DECLARE_VOID_IRQ(14);
+DECLARE_VOID_IRQ(15);
+DECLARE_VOID_IRQ(16);
+DECLARE_VOID_IRQ(17);
+DECLARE_VOID_IRQ(18);
+DECLARE_VOID_IRQ(19);
+DECLARE_VOID_IRQ(20);
+DECLARE_VOID_IRQ(21);
+DECLARE_VOID_IRQ(22);
+DECLARE_VOID_IRQ(23);
+
+typedef void (*V_FP_V) (void);
+static V_FP_V handle_void_irq_ptrs[] = {
+	handle_void_irq_0,
+	handle_void_irq_1,
+	handle_void_irq_2,
+	handle_void_irq_3,
+	handle_void_irq_4,
+	handle_void_irq_5,
+	handle_void_irq_6,
+	handle_void_irq_7,
+	handle_void_irq_8,
+	handle_void_irq_9,
+	handle_void_irq_10,
+	handle_void_irq_11,
+	handle_void_irq_12,
+	handle_void_irq_13,
+	handle_void_irq_14,
+	handle_void_irq_15,
+	handle_void_irq_16,
+	handle_void_irq_17,
+	handle_void_irq_18,
+	handle_void_irq_19,
+	handle_void_irq_20,
+	handle_void_irq_21,
+	handle_void_irq_22,
+	handle_void_irq_23,
+};
+
+static int comedi_rt_get_irq(struct comedi_irq_struct *it)
+{
+	rt_request_global_irq(it->irq, handle_void_irq_ptrs[it->irq]);
+	rt_startup_irq(it->irq);
+
+	return 0;
+}
+
+static int comedi_rt_release_irq(struct comedi_irq_struct *it)
+{
+	rt_shutdown_irq(it->irq);
+	rt_free_global_irq(it->irq);
+	return 0;
+}
+#else
+
+static int comedi_rt_get_irq(struct comedi_irq_struct *it)
+{
+	int ret;
+
+	ret = rt_request_global_irq_arg(it->irq, it->handler, it->flags,
+		it->device, it->dev_id);
+	if (ret < 0) {
+		rt_printk("rt_request_global_irq_arg() returned %d\n", ret);
+		return ret;
+	}
+	rt_startup_irq(it->irq);
+
+	return 0;
+}
+
+static int comedi_rt_release_irq(struct comedi_irq_struct *it)
+{
+	rt_shutdown_irq(it->irq);
+	rt_free_global_irq(it->irq);
+	return 0;
+}
+#endif
+
+void comedi_rt_init(void)
+{
+	rt_mount_rtai();
+	rt_pend_tq_init();
+}
+
+void comedi_rt_cleanup(void)
+{
+	rt_umount_rtai();
+	rt_pend_tq_cleanup();
+}
+
+#endif
+
+/* Fusion section */
+#ifdef CONFIG_COMEDI_FUSION
+
+static void fusion_handle_irq(unsigned int irq, void *cookie)
+{
+	struct comedi_irq_struct *it = cookie;
+
+	it->handler(irq, it->dev_id PT_REGS_NULL);
+	rthal_irq_enable(irq);
+}
+
+static int comedi_rt_get_irq(struct comedi_irq_struct *it)
+{
+	rthal_irq_request(it->irq, fusion_handle_irq, it);
+	rthal_irq_enable(it->irq);
+	return 0;
+}
+
+static int comedi_rt_release_irq(struct comedi_irq_struct *it)
+{
+	rthal_irq_disable(it->irq);
+	rthal_irq_release(it->irq);
+	return 0;
+}
+
+void comedi_rt_init(void)
+{
+	rt_pend_tq_init();
+}
+
+void comedi_rt_cleanup(void)
+{
+	rt_pend_tq_cleanup();
+}
+
+#endif /*CONFIG_COMEDI_FUSION */
+
+/* RTLinux section */
+#ifdef CONFIG_COMEDI_RTL
+
+static unsigned int handle_rtl_irq(unsigned int irq PT_REGS_ARG)
+{
+	struct comedi_irq_struct *it;
+
+	it = comedi_irqs[irq];
+	if (it == NULL)
+		return 0;
+	it->handler(irq, it->dev_id PT_REGS_NULL);
+	rtl_hard_enable_irq(irq);
+	return 0;
+}
+
+static int comedi_rt_get_irq(struct comedi_irq_struct *it)
+{
+	rtl_request_global_irq(it->irq, handle_rtl_irq);
+	return 0;
+}
+
+static int comedi_rt_release_irq(struct comedi_irq_struct *it)
+{
+	rtl_free_global_irq(it->irq);
+	return 0;
+}
+
+void comedi_rt_init(void)
+{
+	rt_pend_tq_init();
+}
+
+void comedi_rt_cleanup(void)
+{
+	rt_pend_tq_cleanup();
+}
+
+#endif
+
+#ifdef CONFIG_COMEDI_PIRQ
+static int comedi_rt_get_irq(struct comedi_irq_struct *it)
+{
+	int ret;
+
+	free_irq(it->irq, it->dev_id);
+	ret = request_irq(it->irq, it->handler, it->flags | SA_PRIORITY,
+		it->device, it->dev_id);
+
+	return ret;
+}
+
+static int comedi_rt_release_irq(struct comedi_irq_struct *it)
+{
+	int ret;
+
+	free_irq(it->irq, it->dev_id);
+	ret = request_irq(it->irq, it->handler, it->flags,
+		it->device, it->dev_id);
+
+	return ret;
+}
+
+void comedi_rt_init(void)
+{
+	//rt_pend_tq_init();
+}
+
+void comedi_rt_cleanup(void)
+{
+	//rt_pend_tq_cleanup();
+}
+#endif
diff --git a/drivers/staging/comedi/rt_pend_tq.c b/drivers/staging/comedi/rt_pend_tq.c
new file mode 100644
index 000000000000..995f076e0af3
--- /dev/null
+++ b/drivers/staging/comedi/rt_pend_tq.c
@@ -0,0 +1,113 @@
+#define __NO_VERSION__
+/* rt_pend_tq.c */
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/sched.h>
+#include "comedidev.h"	// for rt spinlocks
+#include "rt_pend_tq.h"
+#ifdef CONFIG_COMEDI_RTAI
+#include <rtai.h>
+#endif
+#ifdef CONFIG_COMEDI_FUSION
+#include <nucleus/asm/hal.h>
+#endif
+#ifdef CONFIG_COMEDI_RTL
+#include <rtl_core.h>
+#endif
+
+#ifdef standalone
+#include <linux/module.h>
+#define rt_pend_tq_init init_module
+#define rt_pend_tq_cleanup cleanup_module
+#endif
+
+volatile static struct rt_pend_tq rt_pend_tq[RT_PEND_TQ_SIZE];
+volatile static struct rt_pend_tq *volatile rt_pend_head = rt_pend_tq,
+	*volatile rt_pend_tail = rt_pend_tq;
+int rt_pend_tq_irq = 0;
+spinlock_t rt_pend_tq_lock = SPIN_LOCK_UNLOCKED;
+
+// WARNING: following code not checked against race conditions yet.
+#define INC_CIRCULAR_PTR(ptr,begin,size) do {if(++(ptr)>=(begin)+(size)) (ptr)=(begin); } while(0)
+#define DEC_CIRCULAR_PTR(ptr,begin,size) do {if(--(ptr)<(begin)) (ptr)=(begin)+(size)-1; } while(0)
+
+int rt_pend_call(void (*func) (int arg1, void *arg2), int arg1, void *arg2)
+{
+	unsigned long flags;
+
+	if (func == NULL)
+		return -EINVAL;
+	if (rt_pend_tq_irq <= 0)
+		return -ENODEV;
+	comedi_spin_lock_irqsave(&rt_pend_tq_lock, flags);
+	INC_CIRCULAR_PTR(rt_pend_head, rt_pend_tq, RT_PEND_TQ_SIZE);
+	if (rt_pend_head == rt_pend_tail) {
+		// overflow, we just refuse to take this request
+		DEC_CIRCULAR_PTR(rt_pend_head, rt_pend_tq, RT_PEND_TQ_SIZE);
+		comedi_spin_unlock_irqrestore(&rt_pend_tq_lock, flags);
+		return -EAGAIN;
+	}
+	rt_pend_head->func = func;
+	rt_pend_head->arg1 = arg1;
+	rt_pend_head->arg2 = arg2;
+	comedi_spin_unlock_irqrestore(&rt_pend_tq_lock, flags);
+#ifdef CONFIG_COMEDI_RTAI
+	rt_pend_linux_srq(rt_pend_tq_irq);
+#endif
+#ifdef CONFIG_COMEDI_FUSION
+	rthal_apc_schedule(rt_pend_tq_irq);
+#endif
+#ifdef CONFIG_COMEDI_RTL
+	rtl_global_pend_irq(rt_pend_tq_irq);
+
+#endif
+	return 0;
+}
+
+#ifdef CONFIG_COMEDI_RTAI
+void rt_pend_irq_handler(void)
+#elif defined(CONFIG_COMEDI_FUSION)
+void rt_pend_irq_handler(void *cookie)
+#elif defined(CONFIG_COMEDI_RTL)
+void rt_pend_irq_handler(int irq, void *dev PT_REGS_ARG)
+#endif
+{
+	while (rt_pend_head != rt_pend_tail) {
+		INC_CIRCULAR_PTR(rt_pend_tail, rt_pend_tq, RT_PEND_TQ_SIZE);
+		rt_pend_tail->func(rt_pend_tail->arg1, rt_pend_tail->arg2);
+	}
+}
+
+int rt_pend_tq_init(void)
+{
+	rt_pend_head = rt_pend_tail = rt_pend_tq;
+#ifdef CONFIG_COMEDI_RTAI
+	rt_pend_tq_irq = rt_request_srq(0, rt_pend_irq_handler, NULL);
+#endif
+#ifdef CONFIG_COMEDI_FUSION
+	rt_pend_tq_irq =
+		rthal_apc_alloc("comedi APC", rt_pend_irq_handler, NULL);
+#endif
+#ifdef CONFIG_COMEDI_RTL
+	rt_pend_tq_irq = rtl_get_soft_irq(rt_pend_irq_handler, "rt_pend_irq");
+#endif
+	if (rt_pend_tq_irq > 0)
+		printk("rt_pend_tq: RT bottom half scheduler initialized OK\n");
+	else
+		printk("rt_pend_tq: rtl_get_soft_irq failed\n");
+	return 0;
+}
+
+void rt_pend_tq_cleanup(void)
+{
+	printk("rt_pend_tq: unloading\n");
+#ifdef CONFIG_COMEDI_RTAI
+	rt_free_srq(rt_pend_tq_irq);
+#endif
+#ifdef CONFIG_COMEDI_FUSION
+	rthal_apc_free(rt_pend_tq_irq);
+#endif
+#ifdef CONFIG_COMEDI_RTL
+	free_irq(rt_pend_tq_irq, NULL);
+#endif
+}
diff --git a/drivers/staging/comedi/rt_pend_tq.h b/drivers/staging/comedi/rt_pend_tq.h
new file mode 100644
index 000000000000..01ed71bf409d
--- /dev/null
+++ b/drivers/staging/comedi/rt_pend_tq.h
@@ -0,0 +1,10 @@
+#define RT_PEND_TQ_SIZE 16
+struct rt_pend_tq {
+	void (*func) (int arg1, void *arg2);
+	int arg1;
+	void *arg2;
+};
+extern int rt_pend_call(void (*func) (int arg1, void *arg2), int arg1,
+	void *arg2);
+extern int rt_pend_tq_init(void);
+extern void rt_pend_tq_cleanup(void);
diff --git a/drivers/staging/comedi/wrapper.h b/drivers/staging/comedi/wrapper.h
new file mode 100644
index 000000000000..77fc673900e9
--- /dev/null
+++ b/drivers/staging/comedi/wrapper.h
@@ -0,0 +1,25 @@
+/*
+    linux/wrapper.h compatibility header
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __COMPAT_LINUX_WRAPPER_H_
+#define __COMPAT_LINUX_WRAPPER_H_
+
+#define mem_map_reserve(p)      set_bit(PG_reserved, &((p)->flags))
+#define mem_map_unreserve(p)    clear_bit(PG_reserved, &((p)->flags))
+
+#endif /* __COMPAT_LINUX_WRAPPER_H_ */
diff --git a/drivers/staging/echo/TODO b/drivers/staging/echo/TODO
index 1ca09afd603e..f6d8580a0baa 100644
--- a/drivers/staging/echo/TODO
+++ b/drivers/staging/echo/TODO
@@ -1,7 +1,5 @@
 TODO:
 	- checkpatch.pl cleanups
-	- Lindent
-	- typedef removals
 	- handle bit_operations.h (merge in or make part of common code?)
 	- remove proc interface, only use echo.h interface (proc interface is
 	  racy and not correct.)
diff --git a/drivers/staging/echo/echo.h b/drivers/staging/echo/echo.h
index 9fb9543c4f13..34fb816b15c8 100644
--- a/drivers/staging/echo/echo.h
+++ b/drivers/staging/echo/echo.h
@@ -149,8 +149,8 @@ struct oslec_state {
 	int Lbgn, Lbgn_acc, Lbgn_upper, Lbgn_upper_acc;
 
 	/* foreground and background filter states */
-	fir16_state_t fir_state;
-	fir16_state_t fir_state_bg;
+	struct fir16_state_t fir_state;
+	struct fir16_state_t fir_state_bg;
 	int16_t *fir_taps16[2];
 
 	/* DC blocking filter states */
diff --git a/drivers/staging/echo/fir.h b/drivers/staging/echo/fir.h
index 5645cb1b2f90..d35f16805f4b 100644
--- a/drivers/staging/echo/fir.h
+++ b/drivers/staging/echo/fir.h
@@ -72,37 +72,37 @@
     16 bit integer FIR descriptor. This defines the working state for a single
     instance of an FIR filter using 16 bit integer coefficients.
 */
-typedef struct {
+struct fir16_state_t {
 	int taps;
 	int curr_pos;
 	const int16_t *coeffs;
 	int16_t *history;
-} fir16_state_t;
+};
 
 /*!
     32 bit integer FIR descriptor. This defines the working state for a single
     instance of an FIR filter using 32 bit integer coefficients, and filtering
     16 bit integer data.
 */
-typedef struct {
+struct fir32_state_t {
 	int taps;
 	int curr_pos;
 	const int32_t *coeffs;
 	int16_t *history;
-} fir32_state_t;
+};
 
 /*!
     Floating point FIR descriptor. This defines the working state for a single
     instance of an FIR filter using floating point coefficients and data.
 */
-typedef struct {
+struct fir_float_state_t {
 	int taps;
 	int curr_pos;
 	const float *coeffs;
 	float *history;
-} fir_float_state_t;
+};
 
-static __inline__ const int16_t *fir16_create(fir16_state_t * fir,
+static __inline__ const int16_t *fir16_create(struct fir16_state_t *fir,
 					      const int16_t * coeffs, int taps)
 {
 	fir->taps = taps;
@@ -116,7 +116,7 @@ static __inline__ const int16_t *fir16_create(fir16_state_t * fir,
 	return fir->history;
 }
 
-static __inline__ void fir16_flush(fir16_state_t * fir)
+static __inline__ void fir16_flush(struct fir16_state_t *fir)
 {
 #if defined(USE_MMX)  ||  defined(USE_SSE2) || defined(__bfin__)
 	memset(fir->history, 0, 2 * fir->taps * sizeof(int16_t));
@@ -125,7 +125,7 @@ static __inline__ void fir16_flush(fir16_state_t * fir)
 #endif
 }
 
-static __inline__ void fir16_free(fir16_state_t * fir)
+static __inline__ void fir16_free(struct fir16_state_t *fir)
 {
 	kfree(fir->history);
 }
@@ -157,19 +157,19 @@ static inline int32_t dot_asm(short *x, short *y, int len)
 }
 #endif
 
-static __inline__ int16_t fir16(fir16_state_t * fir, int16_t sample)
+static __inline__ int16_t fir16(struct fir16_state_t *fir, int16_t sample)
 {
 	int32_t y;
 #if defined(USE_MMX)
 	int i;
-	mmx_t *mmx_coeffs;
-	mmx_t *mmx_hist;
+	union mmx_t *mmx_coeffs;
+	union mmx_t *mmx_hist;
 
 	fir->history[fir->curr_pos] = sample;
 	fir->history[fir->curr_pos + fir->taps] = sample;
 
-	mmx_coeffs = (mmx_t *) fir->coeffs;
-	mmx_hist = (mmx_t *) & fir->history[fir->curr_pos];
+	mmx_coeffs = (union mmx_t *)fir->coeffs;
+	mmx_hist = (union mmx_t *)&fir->history[fir->curr_pos];
 	i = fir->taps;
 	pxor_r2r(mm4, mm4);
 	/* 8 samples per iteration, so the filter must be a multiple of 8 long. */
@@ -193,14 +193,14 @@ static __inline__ int16_t fir16(fir16_state_t * fir, int16_t sample)
 	emms();
 #elif defined(USE_SSE2)
 	int i;
-	xmm_t *xmm_coeffs;
-	xmm_t *xmm_hist;
+	union xmm_t *xmm_coeffs;
+	union xmm_t *xmm_hist;
 
 	fir->history[fir->curr_pos] = sample;
 	fir->history[fir->curr_pos + fir->taps] = sample;
 
-	xmm_coeffs = (xmm_t *) fir->coeffs;
-	xmm_hist = (xmm_t *) & fir->history[fir->curr_pos];
+	xmm_coeffs = (union xmm_t *)fir->coeffs;
+	xmm_hist = (union xmm_t *)&fir->history[fir->curr_pos];
 	i = fir->taps;
 	pxor_r2r(xmm4, xmm4);
 	/* 16 samples per iteration, so the filter must be a multiple of 16 long. */
@@ -250,7 +250,7 @@ static __inline__ int16_t fir16(fir16_state_t * fir, int16_t sample)
 	return (int16_t) (y >> 15);
 }
 
-static __inline__ const int16_t *fir32_create(fir32_state_t * fir,
+static __inline__ const int16_t *fir32_create(struct fir32_state_t *fir,
 					      const int32_t * coeffs, int taps)
 {
 	fir->taps = taps;
@@ -260,17 +260,17 @@ static __inline__ const int16_t *fir32_create(fir32_state_t * fir,
 	return fir->history;
 }
 
-static __inline__ void fir32_flush(fir32_state_t * fir)
+static __inline__ void fir32_flush(struct fir32_state_t *fir)
 {
 	memset(fir->history, 0, fir->taps * sizeof(int16_t));
 }
 
-static __inline__ void fir32_free(fir32_state_t * fir)
+static __inline__ void fir32_free(struct fir32_state_t *fir)
 {
 	kfree(fir->history);
 }
 
-static __inline__ int16_t fir32(fir32_state_t * fir, int16_t sample)
+static __inline__ int16_t fir32(struct fir32_state_t *fir, int16_t sample)
 {
 	int i;
 	int32_t y;
diff --git a/drivers/staging/echo/mmx.h b/drivers/staging/echo/mmx.h
index 35412efe61ce..44e5cfebc18f 100644
--- a/drivers/staging/echo/mmx.h
+++ b/drivers/staging/echo/mmx.h
@@ -27,7 +27,7 @@
  * values by ULL, lest they be truncated by the compiler)
  */
 
-typedef union {
+union mmx_t {
 	long long q;		/* Quadword (64-bit) value */
 	unsigned long long uq;	/* Unsigned Quadword */
 	int d[2];		/* 2 Doubleword (32-bit) values */
@@ -37,12 +37,12 @@ typedef union {
 	char b[8];		/* 8 Byte (8-bit) values */
 	unsigned char ub[8];	/* 8 Unsigned Byte */
 	float s[2];		/* Single-precision (32-bit) value */
-} mmx_t;			/* On an 8-byte (64-bit) boundary */
+};				/* On an 8-byte (64-bit) boundary */
 
 /* SSE registers */
-typedef union {
+union xmm_t {
 	char b[16];
-} xmm_t;
+};
 
 #define         mmx_i2r(op,imm,reg) \
         __asm__ __volatile__ (#op " %0, %%" #reg \
diff --git a/drivers/staging/epl/Benchmark.h b/drivers/staging/epl/Benchmark.h
new file mode 100644
index 000000000000..62dee3b14373
--- /dev/null
+++ b/drivers/staging/epl/Benchmark.h
@@ -0,0 +1,437 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  header file for benchmarking
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: Benchmark.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    ...
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/08/16 d.k.:  start of implementation
+
+****************************************************************************/
+
+#ifndef _BENCHMARK_H_
+#define _BENCHMARK_H_
+
+#include "global.h"
+
+#if (TARGET_SYSTEM == _NO_OS_) && (DEV_SYSTEM == _DEV_GNU_CF548X_)
+#include "common.h"
+
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+
+//    #include <linux/config.h>
+#include <linux/kernel.h>
+
+#ifdef CONFIG_COLDFIRE
+#include <asm/coldfire.h>
+#include <asm/m5485gpio.h>
+
+#define BENCHMARK_SET(x)    MCF_GPIO_PODR_PCIBG |= (1 << (x))	// (x+1)
+#define BENCHMARK_RESET(x)  MCF_GPIO_PODR_PCIBG &= ~(1 << (x))	// (x+1)
+#define BENCHMARK_TOGGLE(x) MCF_GPIO_PODR_PCIBR ^= (1 << (x - 5))
+#else
+#undef BENCHMARK_MODULES
+#define BENCHMARK_MODULES           0x00000000
+#endif
+
+#else
+    // disable Benchmarking
+#undef BENCHMARK_MODULES
+#define BENCHMARK_MODULES               0x00000000
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef BENCHMARK_MODULES
+#define BENCHMARK_MODULES                   0x00000000
+#endif
+
+#define BENCHMARK_MOD_01                    0x00000001
+#define BENCHMARK_MOD_02                    0x00000002
+#define BENCHMARK_MOD_03                    0x00000004
+#define BENCHMARK_MOD_04                    0x00000008
+#define BENCHMARK_MOD_05                    0x00000010
+#define BENCHMARK_MOD_06                    0x00000020
+#define BENCHMARK_MOD_07                    0x00000040
+#define BENCHMARK_MOD_08                    0x00000080
+#define BENCHMARK_MOD_09                    0x00000100
+#define BENCHMARK_MOD_10                    0x00000200
+#define BENCHMARK_MOD_11                    0x00000400
+#define BENCHMARK_MOD_12                    0x00000800
+#define BENCHMARK_MOD_13                    0x00001000
+#define BENCHMARK_MOD_14                    0x00002000
+#define BENCHMARK_MOD_15                    0x00004000
+#define BENCHMARK_MOD_16                    0x00008000
+#define BENCHMARK_MOD_17                    0x00010000
+#define BENCHMARK_MOD_18                    0x00020000
+#define BENCHMARK_MOD_19                    0x00040000
+#define BENCHMARK_MOD_20                    0x00080000
+#define BENCHMARK_MOD_21                    0x00100000
+#define BENCHMARK_MOD_22                    0x00200000
+#define BENCHMARK_MOD_23                    0x00400000
+#define BENCHMARK_MOD_24                    0x00800000
+#define BENCHMARK_MOD_25                    0x01000000
+#define BENCHMARK_MOD_26                    0x02000000
+#define BENCHMARK_MOD_27                    0x04000000
+#define BENCHMARK_MOD_28                    0x08000000
+#define BENCHMARK_MOD_29                    0x10000000
+#define BENCHMARK_MOD_30                    0x20000000
+#define BENCHMARK_MOD_31                    0x40000000
+#define BENCHMARK_MOD_32                    0x80000000
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_01)
+#define BENCHMARK_MOD_01_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_01_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_01_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_01_SET(x)
+#define BENCHMARK_MOD_01_RESET(x)
+#define BENCHMARK_MOD_01_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_02)
+#define BENCHMARK_MOD_02_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_02_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_02_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_02_SET(x)
+#define BENCHMARK_MOD_02_RESET(x)
+#define BENCHMARK_MOD_02_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_03)
+#define BENCHMARK_MOD_03_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_03_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_03_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_03_SET(x)
+#define BENCHMARK_MOD_03_RESET(x)
+#define BENCHMARK_MOD_03_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_04)
+#define BENCHMARK_MOD_04_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_04_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_04_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_04_SET(x)
+#define BENCHMARK_MOD_04_RESET(x)
+#define BENCHMARK_MOD_04_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_05)
+#define BENCHMARK_MOD_05_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_05_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_05_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_05_SET(x)
+#define BENCHMARK_MOD_05_RESET(x)
+#define BENCHMARK_MOD_05_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_06)
+#define BENCHMARK_MOD_06_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_06_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_06_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_06_SET(x)
+#define BENCHMARK_MOD_06_RESET(x)
+#define BENCHMARK_MOD_06_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_07)
+#define BENCHMARK_MOD_07_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_07_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_07_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_07_SET(x)
+#define BENCHMARK_MOD_07_RESET(x)
+#define BENCHMARK_MOD_07_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_08)
+#define BENCHMARK_MOD_08_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_08_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_08_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_08_SET(x)
+#define BENCHMARK_MOD_08_RESET(x)
+#define BENCHMARK_MOD_08_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_09)
+#define BENCHMARK_MOD_09_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_09_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_09_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_09_SET(x)
+#define BENCHMARK_MOD_09_RESET(x)
+#define BENCHMARK_MOD_09_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_10)
+#define BENCHMARK_MOD_10_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_10_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_10_SET(x)
+#define BENCHMARK_MOD_10_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_11)
+#define BENCHMARK_MOD_11_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_11_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_11_SET(x)
+#define BENCHMARK_MOD_11_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_12)
+#define BENCHMARK_MOD_12_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_12_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_12_SET(x)
+#define BENCHMARK_MOD_12_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_13)
+#define BENCHMARK_MOD_13_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_13_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_13_SET(x)
+#define BENCHMARK_MOD_13_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_14)
+#define BENCHMARK_MOD_14_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_14_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_14_SET(x)
+#define BENCHMARK_MOD_14_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_15)
+#define BENCHMARK_MOD_15_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_15_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_15_SET(x)
+#define BENCHMARK_MOD_15_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_16)
+#define BENCHMARK_MOD_16_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_16_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_16_SET(x)
+#define BENCHMARK_MOD_16_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_17)
+#define BENCHMARK_MOD_17_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_17_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_17_SET(x)
+#define BENCHMARK_MOD_17_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_18)
+#define BENCHMARK_MOD_18_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_18_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_18_SET(x)
+#define BENCHMARK_MOD_18_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_19)
+#define BENCHMARK_MOD_19_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_19_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_19_SET(x)
+#define BENCHMARK_MOD_19_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_20)
+#define BENCHMARK_MOD_20_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_20_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_20_SET(x)
+#define BENCHMARK_MOD_20_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_21)
+#define BENCHMARK_MOD_21_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_21_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_21_SET(x)
+#define BENCHMARK_MOD_21_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_22)
+#define BENCHMARK_MOD_22_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_22_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_22_SET(x)
+#define BENCHMARK_MOD_22_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_23)
+#define BENCHMARK_MOD_23_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_23_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_23_SET(x)
+#define BENCHMARK_MOD_23_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_24)
+#define BENCHMARK_MOD_24_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_24_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_24_SET(x)
+#define BENCHMARK_MOD_24_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_25)
+#define BENCHMARK_MOD_25_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_25_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_25_SET(x)
+#define BENCHMARK_MOD_25_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_26)
+#define BENCHMARK_MOD_26_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_26_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_26_SET(x)
+#define BENCHMARK_MOD_26_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_27)
+#define BENCHMARK_MOD_27_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_27_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_27_SET(x)
+#define BENCHMARK_MOD_27_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_28)
+#define BENCHMARK_MOD_28_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_28_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_28_SET(x)
+#define BENCHMARK_MOD_28_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_29)
+#define BENCHMARK_MOD_29_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_29_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_29_SET(x)
+#define BENCHMARK_MOD_29_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_30)
+#define BENCHMARK_MOD_30_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_30_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_30_SET(x)
+#define BENCHMARK_MOD_30_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_31)
+#define BENCHMARK_MOD_31_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_31_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_31_SET(x)
+#define BENCHMARK_MOD_31_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_32)
+#define BENCHMARK_MOD_32_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_32_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_32_SET(x)
+#define BENCHMARK_MOD_32_RESET(x)
+#endif
+
+//---------------------------------------------------------------------------
+// modul global types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+#endif // _BENCHMARK_H_
diff --git a/drivers/staging/epl/Debug.h b/drivers/staging/epl/Debug.h
new file mode 100644
index 000000000000..05de9d541fd6
--- /dev/null
+++ b/drivers/staging/epl/Debug.h
@@ -0,0 +1,734 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  Debug interface
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: Debug.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    ...
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+****************************************************************************/
+
+#ifndef _DEBUG_H_
+#define _DEBUG_H_
+
+#include "global.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// global const defines
+//---------------------------------------------------------------------------
+
+// These definitions are important for level-debug traces.
+// A macro DEBUG_GLB_LVL() defines the current debug-level using following bis.
+// If the corresponding bit is set then trace message will be printed out
+// (only if NDEBUG is not defined). The upper debug-levels are reserved for
+// the debug-levels ALWAYS, ERROR and ASSERT.
+#define DEBUG_LVL_01                    0x00000001
+#define DEBUG_LVL_02                    0x00000002
+#define DEBUG_LVL_03                    0x00000004
+#define DEBUG_LVL_04                    0x00000008
+#define DEBUG_LVL_05                    0x00000010
+#define DEBUG_LVL_06                    0x00000020
+#define DEBUG_LVL_07                    0x00000040
+#define DEBUG_LVL_08                    0x00000080
+#define DEBUG_LVL_09                    0x00000100
+#define DEBUG_LVL_10                    0x00000200
+#define DEBUG_LVL_11                    0x00000400
+#define DEBUG_LVL_12                    0x00000800
+#define DEBUG_LVL_13                    0x00001000
+#define DEBUG_LVL_14                    0x00002000
+#define DEBUG_LVL_15                    0x00004000
+#define DEBUG_LVL_16                    0x00008000
+#define DEBUG_LVL_17                    0x00010000
+#define DEBUG_LVL_18                    0x00020000
+#define DEBUG_LVL_19                    0x00040000
+#define DEBUG_LVL_20                    0x00080000
+#define DEBUG_LVL_21                    0x00100000
+#define DEBUG_LVL_22                    0x00200000
+#define DEBUG_LVL_23                    0x00400000
+#define DEBUG_LVL_24                    0x00800000
+#define DEBUG_LVL_25                    0x01000000
+#define DEBUG_LVL_26                    0x02000000
+#define DEBUG_LVL_27                    0x04000000
+#define DEBUG_LVL_28                    0x08000000
+#define DEBUG_LVL_29                    0x10000000
+#define DEBUG_LVL_ASSERT                0x20000000
+#define DEBUG_LVL_ERROR                 0x40000000
+#define DEBUG_LVL_ALWAYS                0x80000000
+
+//---------------------------------------------------------------------------
+// global types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// global vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// global function prototypes
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// global macros
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// this macro defines a version string
+
+
+//---------------------------------------------------------------------------
+// this macro defines a build info string (e.g. for using in printf())
+#define DEBUG_MAKE_BUILD_INFO(prefix,product,prodid,descr,verstr,author) "\n" \
+    prefix "***************************************************\n" \
+    prefix "Project:   " product ", " prodid                  "\n" \
+    prefix "Descript.: " descr                                "\n" \
+    prefix "Author:    " author                               "\n" \
+    prefix "Date:      " __DATE__                             "\n" \
+    prefix "Version:   " verstr                               "\n" \
+    prefix "***************************************************\n\n"
+
+//---------------------------------------------------------------------------
+// The default debug-level is: ERROR and ALWAYS.
+// You can define an other debug-level in project settings.
+#ifndef DEF_DEBUG_LVL
+#define DEF_DEBUG_LVL                   (DEBUG_LVL_ALWAYS | DEBUG_LVL_ERROR)
+#endif
+#ifndef DEBUG_GLB_LVL
+#define DEBUG_GLB_LVL()                 (DEF_DEBUG_LVL)
+#endif
+
+//---------------------------------------------------------------------------
+#if (DEV_SYSTEM == _DEV_WIN32_) && defined (TRACE_MSG)
+
+    // For WIN32 the macro DEBUG_TRACE0 can be defined as function call TraceLvl()
+    // or as macro TRACE().
+    //
+    // Here the parameter 'lvl' can be used with more than one
+    // debug-level (using OR).
+    //
+    // Example: DEBUG_TRACE1(DEBUG_LVL_30 | DEBUG_LVL_02, "Hello %d", bCount);
+
+#define DEBUG_TRACE0(lvl,str)               TraceLvl((lvl),str)
+#define DEBUG_TRACE1(lvl,str,p1)            TraceLvl((lvl),str,p1)
+#define DEBUG_TRACE2(lvl,str,p1,p2)         TraceLvl((lvl),str,p1,p2)
+#define DEBUG_TRACE3(lvl,str,p1,p2,p3)      TraceLvl((lvl),str,p1,p2,p3)
+#define DEBUG_TRACE4(lvl,str,p1,p2,p3,p4)   TraceLvl((lvl),str,p1,p2,p3,p4)
+#define DEBUG_GLB_LVL()                     dwDebugLevel_g
+
+#else
+
+    // At microcontrollers we do reduce the memory usage by deleting DEBUG_TRACE-lines
+    // (compiler does delete the lines).
+    //
+    // Here the parameter 'lvl' can only be used with one debug-level.
+    //
+    // Example: DEBUG_TRACE1(DEBUG_LVL_ERROR, "error code %d", dwRet);
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_ALWAYS)
+#define DEBUG_LVL_ALWAYS_TRACE0(str)                TRACE0(str)
+#define DEBUG_LVL_ALWAYS_TRACE1(str,p1)             TRACE1(str,p1)
+#define DEBUG_LVL_ALWAYS_TRACE2(str,p1,p2)          TRACE2(str,p1,p2)
+#define DEBUG_LVL_ALWAYS_TRACE3(str,p1,p2,p3)       TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_ALWAYS_TRACE4(str,p1,p2,p3,p4)    TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_ALWAYS_TRACE0(str)
+#define DEBUG_LVL_ALWAYS_TRACE1(str,p1)
+#define DEBUG_LVL_ALWAYS_TRACE2(str,p1,p2)
+#define DEBUG_LVL_ALWAYS_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_ALWAYS_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_ERROR)
+#define DEBUG_LVL_ERROR_TRACE0(str)                 TRACE0(str)
+#define DEBUG_LVL_ERROR_TRACE1(str,p1)              TRACE1(str,p1)
+#define DEBUG_LVL_ERROR_TRACE2(str,p1,p2)           TRACE2(str,p1,p2)
+#define DEBUG_LVL_ERROR_TRACE3(str,p1,p2,p3)        TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_ERROR_TRACE4(str,p1,p2,p3,p4)     TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_ERROR_TRACE0(str)
+#define DEBUG_LVL_ERROR_TRACE1(str,p1)
+#define DEBUG_LVL_ERROR_TRACE2(str,p1,p2)
+#define DEBUG_LVL_ERROR_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_ERROR_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_ASSERT)
+#define DEBUG_LVL_ASSERT_TRACE0(str)                TRACE0(str)
+#define DEBUG_LVL_ASSERT_TRACE1(str,p1)             TRACE1(str,p1)
+#define DEBUG_LVL_ASSERT_TRACE2(str,p1,p2)          TRACE2(str,p1,p2)
+#define DEBUG_LVL_ASSERT_TRACE3(str,p1,p2,p3)       TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_ASSERT_TRACE4(str,p1,p2,p3,p4)    TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_ASSERT_TRACE0(str)
+#define DEBUG_LVL_ASSERT_TRACE1(str,p1)
+#define DEBUG_LVL_ASSERT_TRACE2(str,p1,p2)
+#define DEBUG_LVL_ASSERT_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_ASSERT_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_29)
+#define DEBUG_LVL_29_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_29_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_29_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_29_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_29_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_29_TRACE0(str)
+#define DEBUG_LVL_29_TRACE1(str,p1)
+#define DEBUG_LVL_29_TRACE2(str,p1,p2)
+#define DEBUG_LVL_29_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_29_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_28)
+#define DEBUG_LVL_28_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_28_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_28_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_28_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_28_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_28_TRACE0(str)
+#define DEBUG_LVL_28_TRACE1(str,p1)
+#define DEBUG_LVL_28_TRACE2(str,p1,p2)
+#define DEBUG_LVL_28_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_28_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_27)
+#define DEBUG_LVL_27_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_27_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_27_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_27_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_27_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_27_TRACE0(str)
+#define DEBUG_LVL_27_TRACE1(str,p1)
+#define DEBUG_LVL_27_TRACE2(str,p1,p2)
+#define DEBUG_LVL_27_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_27_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_26)
+#define DEBUG_LVL_26_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_26_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_26_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_26_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_26_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_26_TRACE0(str)
+#define DEBUG_LVL_26_TRACE1(str,p1)
+#define DEBUG_LVL_26_TRACE2(str,p1,p2)
+#define DEBUG_LVL_26_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_26_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_25)
+#define DEBUG_LVL_25_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_25_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_25_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_25_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_25_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_25_TRACE0(str)
+#define DEBUG_LVL_25_TRACE1(str,p1)
+#define DEBUG_LVL_25_TRACE2(str,p1,p2)
+#define DEBUG_LVL_25_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_25_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_24)
+#define DEBUG_LVL_24_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_24_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_24_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_24_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_24_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_24_TRACE0(str)
+#define DEBUG_LVL_24_TRACE1(str,p1)
+#define DEBUG_LVL_24_TRACE2(str,p1,p2)
+#define DEBUG_LVL_24_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_24_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_23)
+#define DEBUG_LVL_23_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_23_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_23_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_23_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_23_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_23_TRACE0(str)
+#define DEBUG_LVL_23_TRACE1(str,p1)
+#define DEBUG_LVL_23_TRACE2(str,p1,p2)
+#define DEBUG_LVL_23_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_23_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_22)
+#define DEBUG_LVL_22_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_22_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_22_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_22_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_22_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_22_TRACE0(str)
+#define DEBUG_LVL_22_TRACE1(str,p1)
+#define DEBUG_LVL_22_TRACE2(str,p1,p2)
+#define DEBUG_LVL_22_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_22_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_21)
+#define DEBUG_LVL_21_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_21_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_21_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_21_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_21_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_21_TRACE0(str)
+#define DEBUG_LVL_21_TRACE1(str,p1)
+#define DEBUG_LVL_21_TRACE2(str,p1,p2)
+#define DEBUG_LVL_21_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_21_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_20)
+#define DEBUG_LVL_20_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_20_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_20_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_20_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_20_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_20_TRACE0(str)
+#define DEBUG_LVL_20_TRACE1(str,p1)
+#define DEBUG_LVL_20_TRACE2(str,p1,p2)
+#define DEBUG_LVL_20_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_20_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_19)
+#define DEBUG_LVL_19_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_19_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_19_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_19_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_19_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_19_TRACE0(str)
+#define DEBUG_LVL_19_TRACE1(str,p1)
+#define DEBUG_LVL_19_TRACE2(str,p1,p2)
+#define DEBUG_LVL_19_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_19_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_18)
+#define DEBUG_LVL_18_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_18_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_18_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_18_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_18_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_18_TRACE0(str)
+#define DEBUG_LVL_18_TRACE1(str,p1)
+#define DEBUG_LVL_18_TRACE2(str,p1,p2)
+#define DEBUG_LVL_18_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_18_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_17)
+#define DEBUG_LVL_17_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_17_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_17_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_17_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_17_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_17_TRACE0(str)
+#define DEBUG_LVL_17_TRACE1(str,p1)
+#define DEBUG_LVL_17_TRACE2(str,p1,p2)
+#define DEBUG_LVL_17_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_17_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_16)
+#define DEBUG_LVL_16_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_16_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_16_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_16_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_16_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_16_TRACE0(str)
+#define DEBUG_LVL_16_TRACE1(str,p1)
+#define DEBUG_LVL_16_TRACE2(str,p1,p2)
+#define DEBUG_LVL_16_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_16_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_15)
+#define DEBUG_LVL_15_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_15_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_15_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_15_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_15_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_15_TRACE0(str)
+#define DEBUG_LVL_15_TRACE1(str,p1)
+#define DEBUG_LVL_15_TRACE2(str,p1,p2)
+#define DEBUG_LVL_15_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_15_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_14)
+#define DEBUG_LVL_14_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_14_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_14_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_14_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_14_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_14_TRACE0(str)
+#define DEBUG_LVL_14_TRACE1(str,p1)
+#define DEBUG_LVL_14_TRACE2(str,p1,p2)
+#define DEBUG_LVL_14_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_14_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_13)
+#define DEBUG_LVL_13_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_13_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_13_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_13_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_13_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_13_TRACE0(str)
+#define DEBUG_LVL_13_TRACE1(str,p1)
+#define DEBUG_LVL_13_TRACE2(str,p1,p2)
+#define DEBUG_LVL_13_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_13_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_12)
+#define DEBUG_LVL_12_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_12_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_12_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_12_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_12_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_12_TRACE0(str)
+#define DEBUG_LVL_12_TRACE1(str,p1)
+#define DEBUG_LVL_12_TRACE2(str,p1,p2)
+#define DEBUG_LVL_12_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_12_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_11)
+#define DEBUG_LVL_11_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_11_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_11_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_11_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_11_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_11_TRACE0(str)
+#define DEBUG_LVL_11_TRACE1(str,p1)
+#define DEBUG_LVL_11_TRACE2(str,p1,p2)
+#define DEBUG_LVL_11_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_11_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_10)
+#define DEBUG_LVL_10_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_10_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_10_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_10_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_10_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_10_TRACE0(str)
+#define DEBUG_LVL_10_TRACE1(str,p1)
+#define DEBUG_LVL_10_TRACE2(str,p1,p2)
+#define DEBUG_LVL_10_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_10_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_09)
+#define DEBUG_LVL_09_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_09_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_09_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_09_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_09_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_09_TRACE0(str)
+#define DEBUG_LVL_09_TRACE1(str,p1)
+#define DEBUG_LVL_09_TRACE2(str,p1,p2)
+#define DEBUG_LVL_09_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_09_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_08)
+#define DEBUG_LVL_08_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_08_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_08_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_08_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_08_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_08_TRACE0(str)
+#define DEBUG_LVL_08_TRACE1(str,p1)
+#define DEBUG_LVL_08_TRACE2(str,p1,p2)
+#define DEBUG_LVL_08_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_08_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_07)
+#define DEBUG_LVL_07_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_07_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_07_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_07_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_07_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_07_TRACE0(str)
+#define DEBUG_LVL_07_TRACE1(str,p1)
+#define DEBUG_LVL_07_TRACE2(str,p1,p2)
+#define DEBUG_LVL_07_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_07_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_06)
+#define DEBUG_LVL_06_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_06_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_06_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_06_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_06_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_06_TRACE0(str)
+#define DEBUG_LVL_06_TRACE1(str,p1)
+#define DEBUG_LVL_06_TRACE2(str,p1,p2)
+#define DEBUG_LVL_06_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_06_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_05)
+#define DEBUG_LVL_05_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_05_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_05_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_05_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_05_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_05_TRACE0(str)
+#define DEBUG_LVL_05_TRACE1(str,p1)
+#define DEBUG_LVL_05_TRACE2(str,p1,p2)
+#define DEBUG_LVL_05_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_05_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_04)
+#define DEBUG_LVL_04_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_04_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_04_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_04_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_04_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_04_TRACE0(str)
+#define DEBUG_LVL_04_TRACE1(str,p1)
+#define DEBUG_LVL_04_TRACE2(str,p1,p2)
+#define DEBUG_LVL_04_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_04_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_03)
+#define DEBUG_LVL_03_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_03_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_03_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_03_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_03_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_03_TRACE0(str)
+#define DEBUG_LVL_03_TRACE1(str,p1)
+#define DEBUG_LVL_03_TRACE2(str,p1,p2)
+#define DEBUG_LVL_03_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_03_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_02)
+#define DEBUG_LVL_02_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_02_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_02_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_02_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_02_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_02_TRACE0(str)
+#define DEBUG_LVL_02_TRACE1(str,p1)
+#define DEBUG_LVL_02_TRACE2(str,p1,p2)
+#define DEBUG_LVL_02_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_02_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_01)
+#define DEBUG_LVL_01_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_01_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_01_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_01_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_01_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_01_TRACE0(str)
+#define DEBUG_LVL_01_TRACE1(str,p1)
+#define DEBUG_LVL_01_TRACE2(str,p1,p2)
+#define DEBUG_LVL_01_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_01_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#define DEBUG_TRACE0(lvl,str)                           lvl##_TRACE0(str)
+#define DEBUG_TRACE1(lvl,str,p1)                        lvl##_TRACE1(str,p1)
+#define DEBUG_TRACE2(lvl,str,p1,p2)                     lvl##_TRACE2(str,p1,p2)
+#define DEBUG_TRACE3(lvl,str,p1,p2,p3)                  lvl##_TRACE3(str,p1,p2,p3)
+#define DEBUG_TRACE4(lvl,str,p1,p2,p3,p4)               lvl##_TRACE4(str,p1,p2,p3,p4)
+
+#endif
+
+//---------------------------------------------------------------------------
+// The macro DEBUG_DUMP_DATA() can be used with the same debug-levels to dump
+// out data bytes. Function DumpData() has to be included.
+// NOTE: DUMP_DATA has to be defined in project settings.
+#if (!defined (NDEBUG) && defined (DUMP_DATA)) || (DEV_SYSTEM == _DEV_WIN32_)
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+	void DumpData(char *szStr_p, BYTE MEM * pbData_p, WORD wSize_p);
+
+#ifdef __cplusplus
+}				// von extern "C"
+#endif
+#define DEBUG_DUMP_DATA(lvl,str,ptr,siz)    if ((DEBUG_GLB_LVL() & (lvl))==(lvl)) \
+                                                    DumpData (str, (BYTE MEM*) (ptr), (WORD) (siz));
+#else
+
+#define DEBUG_DUMP_DATA(lvl,str,ptr,siz)
+
+#endif
+
+//---------------------------------------------------------------------------
+// The macro DEBUG_ASSERT() can be used to print out an error string if the
+// parametered expresion does not result TRUE.
+// NOTE: If DEBUG_KEEP_ASSERT is defined, then DEBUG_ASSERT-line will not be
+//       deleted from compiler (in release version too).
+#if !defined (NDEBUG) || defined (DEBUG_KEEP_ASSERT)
+
+#if (DEV_SYSTEM == _DEV_WIN32_)
+
+	// For WIN32 process will be killed after closing message box.
+
+#define DEBUG_ASSERT0(expr,str)         if (!(expr ) && ((DEBUG_GLB_LVL() & DEBUG_LVL_ASSERT)!=0)) { \
+                                                    MessageBox (NULL, \
+                                                        "Assertion failed: line " __LINE__ " file " __FILE__ \
+                                                        "\n    -> " str "\n"); \
+                                                    ExitProcess (-1); }
+
+#define DEBUG_ASSERT1(expr,str,p1)      if (!(expr ) && ((DEBUG_GLB_LVL() & DEBUG_LVL_ASSERT)!=0)) { \
+                                                    MessageBox (NULL, \
+                                                        "Assertion failed: line " __LINE__ " file " __FILE__ \
+                                                        "\n    -> " str "\n"); \
+                                                    ExitProcess (-1); }
+
+#else
+
+	// For microcontrollers process will be stopped using endless loop.
+
+#define DEBUG_ASSERT0(expr,str)         if (!(expr )) { \
+                                                    DEBUG_LVL_ASSERT_TRACE3 ( \
+                                                        "Assertion failed: line %d file '%s'\n" \
+                                                        "    -> '%s'\n", __LINE__, __FILE__, str); \
+                                                    while (1); }
+
+#define DEBUG_ASSERT1(expr,str,p1)      if (!(expr )) { \
+                                                    DEBUG_LVL_ASSERT_TRACE4 ( \
+                                                        "Assertion failed: line %d file '%s'\n" \
+                                                        "    -> '%s'\n" \
+                                                        "    -> 0x%08lX\n", __LINE__, __FILE__, str, (DWORD) p1); \
+                                                    while (1); }
+
+#endif
+
+#else
+
+#define DEBUG_ASSERT0(expr,str)
+#define DEBUG_ASSERT1(expr,str,p1)
+
+#endif
+
+//---------------------------------------------------------------------------
+// The macro DEBUG_ONLY() implements code, if NDEBUG is not defined.
+#if !defined (DEBUG_ONLY)
+#if !defined (NDEBUG)
+
+#define DEBUG_ONLY(expr)    expr
+
+#else
+
+#define DEBUG_ONLY(expr)
+
+#endif
+#endif
+
+#endif // _DEBUG_H_
diff --git a/drivers/staging/epl/Edrv8139.c b/drivers/staging/epl/Edrv8139.c
new file mode 100644
index 000000000000..296354aaa9c7
--- /dev/null
+++ b/drivers/staging/epl/Edrv8139.c
@@ -0,0 +1,1252 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  Ethernet driver for Realtek RTL8139 chips
+                except the RTL8139C+, because it has a different
+                Tx descriptor handling.
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: Edrv8139.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.10 $  $Date: 2008/11/21 09:00:38 $
+
+                $State: Exp $
+
+                Build Environment:
+                Dev C++ and GNU-Compiler for m68k
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2008/02/05 d.k.:   start of implementation
+
+****************************************************************************/
+
+#include "global.h"
+#include "EplInc.h"
+#include "edrv.h"
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/pci.h>
+#include <linux/interrupt.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/major.h>
+#include <linux/version.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <asm/atomic.h>
+#include <asm/irq.h>
+#include <linux/sched.h>
+#include <linux/delay.h>
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+// Buffer handling:
+// All buffers are created statically (i.e. at compile time resp. at
+// initialisation via kmalloc() ) and not dynamically on request (i.e. via
+// EdrvAllocTxMsgBuffer().
+// EdrvAllocTxMsgBuffer() searches for an unused buffer which is large enough.
+// EdrvInit() may allocate some buffers with sizes less than maximum frame
+// size (i.e. 1514 bytes), e.g. for SoC, SoA, StatusResponse, IdentResponse,
+// NMT requests / commands. The less the size of the buffer the less the
+// number of the buffer.
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef EDRV_MAX_TX_BUFFERS
+#define EDRV_MAX_TX_BUFFERS     20
+#endif
+
+#define EDRV_MAX_FRAME_SIZE     0x600
+
+#define EDRV_RX_BUFFER_SIZE     0x8610	// 32 kB + 16 Byte + 1,5 kB (WRAP is enabled)
+#define EDRV_RX_BUFFER_LENGTH   (EDRV_RX_BUFFER_SIZE & 0xF800)	// buffer size cut down to 2 kB alignment
+
+#define EDRV_TX_BUFFER_SIZE     (EDRV_MAX_TX_BUFFERS * EDRV_MAX_FRAME_SIZE)	// n * (MTU + 14 + 4)
+
+#define DRV_NAME                "epl"
+
+#define EDRV_REGW_INT_MASK      0x3C	// interrupt mask register
+#define EDRV_REGW_INT_STATUS    0x3E	// interrupt status register
+#define EDRV_REGW_INT_ROK       0x0001	// Receive OK interrupt
+#define EDRV_REGW_INT_RER       0x0002	// Receive error interrupt
+#define EDRV_REGW_INT_TOK       0x0004	// Transmit OK interrupt
+#define EDRV_REGW_INT_TER       0x0008	// Transmit error interrupt
+#define EDRV_REGW_INT_RXOVW     0x0010	// Rx buffer overflow interrupt
+#define EDRV_REGW_INT_PUN       0x0020	// Packet underrun/ link change interrupt
+#define EDRV_REGW_INT_FOVW      0x0040	// Rx FIFO overflow interrupt
+#define EDRV_REGW_INT_LENCHG    0x2000	// Cable length change interrupt
+#define EDRV_REGW_INT_TIMEOUT   0x4000	// Time out interrupt
+#define EDRV_REGW_INT_SERR      0x8000	// System error interrupt
+#define EDRV_REGW_INT_MASK_DEF  (EDRV_REGW_INT_ROK \
+                                 | EDRV_REGW_INT_RER \
+                                 | EDRV_REGW_INT_TOK \
+                                 | EDRV_REGW_INT_TER \
+                                 | EDRV_REGW_INT_RXOVW \
+                                 | EDRV_REGW_INT_FOVW \
+                                 | EDRV_REGW_INT_PUN \
+                                 | EDRV_REGW_INT_TIMEOUT \
+                                 | EDRV_REGW_INT_SERR)	// default interrupt mask
+
+#define EDRV_REGB_COMMAND       0x37	// command register
+#define EDRV_REGB_COMMAND_RST   0x10
+#define EDRV_REGB_COMMAND_RE    0x08
+#define EDRV_REGB_COMMAND_TE    0x04
+#define EDRV_REGB_COMMAND_BUFE  0x01
+
+#define EDRV_REGB_CMD9346       0x50	// 93C46 command register
+#define EDRV_REGB_CMD9346_LOCK  0x00	// lock configuration registers
+#define EDRV_REGB_CMD9346_UNLOCK 0xC0	// unlock configuration registers
+
+#define EDRV_REGDW_RCR          0x44	// Rx configuration register
+#define EDRV_REGDW_RCR_NO_FTH   0x0000E000	// no receive FIFO threshold
+#define EDRV_REGDW_RCR_RBLEN32K 0x00001000	// 32 kB receive buffer
+#define EDRV_REGDW_RCR_MXDMAUNL 0x00000700	// unlimited maximum DMA burst size
+#define EDRV_REGDW_RCR_NOWRAP   0x00000080	// do not wrap frame at end of buffer
+#define EDRV_REGDW_RCR_AER      0x00000020	// accept error frames (CRC, alignment, collided)
+#define EDRV_REGDW_RCR_AR       0x00000010	// accept runt
+#define EDRV_REGDW_RCR_AB       0x00000008	// accept broadcast frames
+#define EDRV_REGDW_RCR_AM       0x00000004	// accept multicast frames
+#define EDRV_REGDW_RCR_APM      0x00000002	// accept physical match frames
+#define EDRV_REGDW_RCR_AAP      0x00000001	// accept all frames
+#define EDRV_REGDW_RCR_DEF      (EDRV_REGDW_RCR_NO_FTH \
+                                 | EDRV_REGDW_RCR_RBLEN32K \
+                                 | EDRV_REGDW_RCR_MXDMAUNL \
+                                 | EDRV_REGDW_RCR_NOWRAP \
+                                 | EDRV_REGDW_RCR_AB \
+                                 | EDRV_REGDW_RCR_AM \
+                                 | EDRV_REGDW_RCR_APM)	// default value
+
+#define EDRV_REGDW_TCR          0x40	// Tx configuration register
+#define EDRV_REGDW_TCR_VER_MASK 0x7CC00000	// mask for hardware version
+#define EDRV_REGDW_TCR_VER_C    0x74000000	// RTL8139C
+#define EDRV_REGDW_TCR_VER_D    0x74400000	// RTL8139D
+#define EDRV_REGDW_TCR_IFG96    0x03000000	// default interframe gap (960 ns)
+#define EDRV_REGDW_TCR_CRC      0x00010000	// disable appending of CRC by the controller
+#define EDRV_REGDW_TCR_MXDMAUNL 0x00000700	// maximum DMA burst size of 2048 b
+#define EDRV_REGDW_TCR_TXRETRY  0x00000000	// 16 retries
+#define EDRV_REGDW_TCR_DEF      (EDRV_REGDW_TCR_IFG96 \
+                                 | EDRV_REGDW_TCR_MXDMAUNL \
+                                 | EDRV_REGDW_TCR_TXRETRY)
+
+#define EDRV_REGW_MULINT        0x5C	// multiple interrupt select register
+
+#define EDRV_REGDW_MPC          0x4C	// missed packet counter register
+
+#define EDRV_REGDW_TSAD0        0x20	// Transmit start address of descriptor 0
+#define EDRV_REGDW_TSAD1        0x24	// Transmit start address of descriptor 1
+#define EDRV_REGDW_TSAD2        0x28	// Transmit start address of descriptor 2
+#define EDRV_REGDW_TSAD3        0x2C	// Transmit start address of descriptor 3
+#define EDRV_REGDW_TSD0         0x10	// Transmit status of descriptor 0
+#define EDRV_REGDW_TSD_CRS      0x80000000	// Carrier sense lost
+#define EDRV_REGDW_TSD_TABT     0x40000000	// Transmit Abort
+#define EDRV_REGDW_TSD_OWC      0x20000000	// Out of window collision
+#define EDRV_REGDW_TSD_TXTH_DEF 0x00020000	// Transmit FIFO threshold of 64 bytes
+#define EDRV_REGDW_TSD_TOK      0x00008000	// Transmit OK
+#define EDRV_REGDW_TSD_TUN      0x00004000	// Transmit FIFO underrun
+#define EDRV_REGDW_TSD_OWN      0x00002000	// Owner
+
+#define EDRV_REGDW_RBSTART      0x30	// Receive buffer start address
+
+#define EDRV_REGW_CAPR          0x38	// Current address of packet read
+
+#define EDRV_REGDW_IDR0         0x00	// ID register 0
+#define EDRV_REGDW_IDR4         0x04	// ID register 4
+
+#define EDRV_REGDW_MAR0         0x08	// Multicast address register 0
+#define EDRV_REGDW_MAR4         0x0C	// Multicast address register 4
+
+// defines for the status word in the receive buffer
+#define EDRV_RXSTAT_MAR         0x8000	// Multicast address received
+#define EDRV_RXSTAT_PAM         0x4000	// Physical address matched
+#define EDRV_RXSTAT_BAR         0x2000	// Broadcast address received
+#define EDRV_RXSTAT_ISE         0x0020	// Invalid symbol error
+#define EDRV_RXSTAT_RUNT        0x0010	// Runt packet received
+#define EDRV_RXSTAT_LONG        0x0008	// Long packet
+#define EDRV_RXSTAT_CRC         0x0004	// CRC error
+#define EDRV_RXSTAT_FAE         0x0002	// Frame alignment error
+#define EDRV_RXSTAT_ROK         0x0001	// Receive OK
+
+#define EDRV_REGDW_WRITE(dwReg, dwVal)  writel(dwVal, EdrvInstance_l.m_pIoAddr + dwReg)
+#define EDRV_REGW_WRITE(dwReg, wVal)    writew(wVal, EdrvInstance_l.m_pIoAddr + dwReg)
+#define EDRV_REGB_WRITE(dwReg, bVal)    writeb(bVal, EdrvInstance_l.m_pIoAddr + dwReg)
+#define EDRV_REGDW_READ(dwReg)          readl(EdrvInstance_l.m_pIoAddr + dwReg)
+#define EDRV_REGW_READ(dwReg)           readw(EdrvInstance_l.m_pIoAddr + dwReg)
+#define EDRV_REGB_READ(dwReg)           readb(EdrvInstance_l.m_pIoAddr + dwReg)
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)     TgtDbgPostTraceValue(v)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)
+#endif
+
+#define EDRV_COUNT_SEND                 TGT_DBG_SIGNAL_TRACE_POINT(2)
+#define EDRV_COUNT_TIMEOUT              TGT_DBG_SIGNAL_TRACE_POINT(3)
+#define EDRV_COUNT_PCI_ERR              TGT_DBG_SIGNAL_TRACE_POINT(4)
+#define EDRV_COUNT_TX                   TGT_DBG_SIGNAL_TRACE_POINT(5)
+#define EDRV_COUNT_RX                   TGT_DBG_SIGNAL_TRACE_POINT(6)
+#define EDRV_COUNT_LATECOLLISION        TGT_DBG_SIGNAL_TRACE_POINT(10)
+#define EDRV_COUNT_TX_COL_RL            TGT_DBG_SIGNAL_TRACE_POINT(11)
+#define EDRV_COUNT_TX_FUN               TGT_DBG_SIGNAL_TRACE_POINT(12)
+#define EDRV_COUNT_TX_ERR               TGT_DBG_SIGNAL_TRACE_POINT(13)
+#define EDRV_COUNT_RX_CRC               TGT_DBG_SIGNAL_TRACE_POINT(14)
+#define EDRV_COUNT_RX_ERR               TGT_DBG_SIGNAL_TRACE_POINT(15)
+#define EDRV_COUNT_RX_FOVW              TGT_DBG_SIGNAL_TRACE_POINT(16)
+#define EDRV_COUNT_RX_PUN               TGT_DBG_SIGNAL_TRACE_POINT(17)
+#define EDRV_COUNT_RX_FAE               TGT_DBG_SIGNAL_TRACE_POINT(18)
+#define EDRV_COUNT_RX_OVW               TGT_DBG_SIGNAL_TRACE_POINT(19)
+
+#define EDRV_TRACE_CAPR(x)              TGT_DBG_POST_TRACE_VALUE(((x) & 0xFFFF) | 0x06000000)
+#define EDRV_TRACE_RX_CRC(x)            TGT_DBG_POST_TRACE_VALUE(((x) & 0xFFFF) | 0x0E000000)
+#define EDRV_TRACE_RX_ERR(x)            TGT_DBG_POST_TRACE_VALUE(((x) & 0xFFFF) | 0x0F000000)
+#define EDRV_TRACE_RX_PUN(x)            TGT_DBG_POST_TRACE_VALUE(((x) & 0xFFFF) | 0x11000000)
+#define EDRV_TRACE(x)                   TGT_DBG_POST_TRACE_VALUE(((x) & 0xFFFF0000) | 0x0000FEC0)
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+/*
+typedef struct
+{
+    BOOL            m_fUsed;
+    unsigned int    m_uiSize;
+    MCD_bufDescFec *m_pBufDescr;
+
+} tEdrvTxBufferIntern;
+*/
+
+// Private structure
+typedef struct {
+	struct pci_dev *m_pPciDev;	// pointer to PCI device structure
+	void *m_pIoAddr;	// pointer to register space of Ethernet controller
+	BYTE *m_pbRxBuf;	// pointer to Rx buffer
+	dma_addr_t m_pRxBufDma;
+	BYTE *m_pbTxBuf;	// pointer to Tx buffer
+	dma_addr_t m_pTxBufDma;
+	BOOL m_afTxBufUsed[EDRV_MAX_TX_BUFFERS];
+	unsigned int m_uiCurTxDesc;
+
+	tEdrvInitParam m_InitParam;
+	tEdrvTxBuffer *m_pLastTransmittedTxBuffer;
+
+} tEdrvInstance;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static int EdrvInitOne(struct pci_dev *pPciDev,
+		       const struct pci_device_id *pId);
+
+static void EdrvRemoveOne(struct pci_dev *pPciDev);
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+// buffers and buffer descriptors and pointers
+
+static struct pci_device_id aEdrvPciTbl[] = {
+	{0x10ec, 0x8139, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{0,}
+};
+
+MODULE_DEVICE_TABLE(pci, aEdrvPciTbl);
+
+static tEdrvInstance EdrvInstance_l;
+
+static struct pci_driver EdrvDriver = {
+	.name = DRV_NAME,
+	.id_table = aEdrvPciTbl,
+	.probe = EdrvInitOne,
+	.remove = EdrvRemoveOne,
+};
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <edrv>                                              */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static BYTE EdrvCalcHash(BYTE * pbMAC_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvInit
+//
+// Description: function for init of the Ethernet controller
+//
+// Parameters:  pEdrvInitParam_p    = pointer to struct including the init-parameters
+//
+// Returns:     Errorcode           = kEplSuccessful
+//                                  = kEplNoResource
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvInit(tEdrvInitParam * pEdrvInitParam_p)
+{
+	tEplKernel Ret;
+	int iResult;
+
+	Ret = kEplSuccessful;
+
+	// clear instance structure
+	EPL_MEMSET(&EdrvInstance_l, 0, sizeof(EdrvInstance_l));
+
+	// save the init data
+	EdrvInstance_l.m_InitParam = *pEdrvInitParam_p;
+
+	// register PCI driver
+	iResult = pci_register_driver(&EdrvDriver);
+	if (iResult != 0) {
+		printk("%s pci_register_driver failed with %d\n", __func__,
+		       iResult);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+	if (EdrvInstance_l.m_pPciDev == NULL) {
+		printk("%s m_pPciDev=NULL\n", __func__);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+	// read MAC address from controller
+	printk("%s local MAC = ", __func__);
+	for (iResult = 0; iResult < 6; iResult++) {
+		pEdrvInitParam_p->m_abMyMacAddr[iResult] =
+		    EDRV_REGB_READ((EDRV_REGDW_IDR0 + iResult));
+		printk("%02X ",
+		       (unsigned int)pEdrvInitParam_p->m_abMyMacAddr[iResult]);
+	}
+	printk("\n");
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvShutdown
+//
+// Description: Shutdown the Ethernet controller
+//
+// Parameters:  void
+//
+// Returns:     Errorcode   = kEplSuccessful
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvShutdown(void)
+{
+
+	// unregister PCI driver
+	printk("%s calling pci_unregister_driver()\n", __func__);
+	pci_unregister_driver(&EdrvDriver);
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvDefineRxMacAddrEntry
+//
+// Description: Set a multicast entry into the Ethernet controller
+//
+// Parameters:  pbMacAddr_p     = pointer to multicast entry to set
+//
+// Returns:     Errorcode       = kEplSuccessful
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvDefineRxMacAddrEntry(BYTE * pbMacAddr_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	DWORD dwData;
+	BYTE bHash;
+
+	bHash = EdrvCalcHash(pbMacAddr_p);
+/*
+    dwData = ether_crc(6, pbMacAddr_p);
+
+    printk("EdrvDefineRxMacAddrEntry('%02X:%02X:%02X:%02X:%02X:%02X') hash = %u / %u  ether_crc = 0x%08lX\n",
+        (WORD) pbMacAddr_p[0], (WORD) pbMacAddr_p[1], (WORD) pbMacAddr_p[2],
+        (WORD) pbMacAddr_p[3], (WORD) pbMacAddr_p[4], (WORD) pbMacAddr_p[5],
+        (WORD) bHash, (WORD) (dwData >> 26), dwData);
+*/
+	if (bHash > 31) {
+		dwData = EDRV_REGDW_READ(EDRV_REGDW_MAR4);
+		dwData |= 1 << (bHash - 32);
+		EDRV_REGDW_WRITE(EDRV_REGDW_MAR4, dwData);
+	} else {
+		dwData = EDRV_REGDW_READ(EDRV_REGDW_MAR0);
+		dwData |= 1 << bHash;
+		EDRV_REGDW_WRITE(EDRV_REGDW_MAR0, dwData);
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvUndefineRxMacAddrEntry
+//
+// Description: Reset a multicast entry in the Ethernet controller
+//
+// Parameters:  pbMacAddr_p     = pointer to multicast entry to reset
+//
+// Returns:     Errorcode       = kEplSuccessful
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvUndefineRxMacAddrEntry(BYTE * pbMacAddr_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	DWORD dwData;
+	BYTE bHash;
+
+	bHash = EdrvCalcHash(pbMacAddr_p);
+
+	if (bHash > 31) {
+		dwData = EDRV_REGDW_READ(EDRV_REGDW_MAR4);
+		dwData &= ~(1 << (bHash - 32));
+		EDRV_REGDW_WRITE(EDRV_REGDW_MAR4, dwData);
+	} else {
+		dwData = EDRV_REGDW_READ(EDRV_REGDW_MAR0);
+		dwData &= ~(1 << bHash);
+		EDRV_REGDW_WRITE(EDRV_REGDW_MAR0, dwData);
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvAllocTxMsgBuffer
+//
+// Description: Register a Tx-Buffer
+//
+// Parameters:  pBuffer_p   = pointer to Buffer structure
+//
+// Returns:     Errorcode   = kEplSuccessful
+//                          = kEplEdrvNoFreeBufEntry
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvAllocTxMsgBuffer(tEdrvTxBuffer * pBuffer_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	DWORD i;
+
+	if (pBuffer_p->m_uiMaxBufferLen > EDRV_MAX_FRAME_SIZE) {
+		Ret = kEplEdrvNoFreeBufEntry;
+		goto Exit;
+	}
+	// search a free Tx buffer with appropriate size
+	for (i = 0; i < EDRV_MAX_TX_BUFFERS; i++) {
+		if (EdrvInstance_l.m_afTxBufUsed[i] == FALSE) {
+			// free channel found
+			EdrvInstance_l.m_afTxBufUsed[i] = TRUE;
+			pBuffer_p->m_uiBufferNumber = i;
+			pBuffer_p->m_pbBuffer =
+			    EdrvInstance_l.m_pbTxBuf +
+			    (i * EDRV_MAX_FRAME_SIZE);
+			pBuffer_p->m_uiMaxBufferLen = EDRV_MAX_FRAME_SIZE;
+			break;
+		}
+	}
+	if (i >= EDRV_MAX_TX_BUFFERS) {
+		Ret = kEplEdrvNoFreeBufEntry;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvReleaseTxMsgBuffer
+//
+// Description: Register a Tx-Buffer
+//
+// Parameters:  pBuffer_p   = pointer to Buffer structure
+//
+// Returns:     Errorcode   = kEplSuccessful
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvReleaseTxMsgBuffer(tEdrvTxBuffer * pBuffer_p)
+{
+	unsigned int uiBufferNumber;
+
+	uiBufferNumber = pBuffer_p->m_uiBufferNumber;
+
+	if (uiBufferNumber < EDRV_MAX_TX_BUFFERS) {
+		EdrvInstance_l.m_afTxBufUsed[uiBufferNumber] = FALSE;
+	}
+
+	return kEplSuccessful;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvSendTxMsg
+//
+// Description: immediately starts the transmission of the buffer
+//
+// Parameters:  pBuffer_p   = buffer descriptor to transmit
+//
+// Returns:     Errorcode   = kEplSuccessful
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvSendTxMsg(tEdrvTxBuffer * pBuffer_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiBufferNumber;
+	DWORD dwTemp;
+
+	uiBufferNumber = pBuffer_p->m_uiBufferNumber;
+
+	if ((uiBufferNumber >= EDRV_MAX_TX_BUFFERS)
+	    || (EdrvInstance_l.m_afTxBufUsed[uiBufferNumber] == FALSE)) {
+		Ret = kEplEdrvBufNotExisting;
+		goto Exit;
+	}
+
+	if (EdrvInstance_l.m_pLastTransmittedTxBuffer != NULL) {	// transmission is already active
+		Ret = kEplInvalidOperation;
+		dwTemp =
+		    EDRV_REGDW_READ((EDRV_REGDW_TSD0 +
+				     (EdrvInstance_l.m_uiCurTxDesc *
+				      sizeof(DWORD))));
+		printk("%s InvOp TSD%u = 0x%08lX", __func__,
+		       EdrvInstance_l.m_uiCurTxDesc, dwTemp);
+		printk("  Cmd = 0x%02X\n",
+		       (WORD) EDRV_REGB_READ(EDRV_REGB_COMMAND));
+		goto Exit;
+	}
+	// save pointer to buffer structure for TxHandler
+	EdrvInstance_l.m_pLastTransmittedTxBuffer = pBuffer_p;
+
+	EDRV_COUNT_SEND;
+
+	// pad with zeros if necessary, because controller does not do it
+	if (pBuffer_p->m_uiTxMsgLen < MIN_ETH_SIZE) {
+		EPL_MEMSET(pBuffer_p->m_pbBuffer + pBuffer_p->m_uiTxMsgLen, 0,
+			   MIN_ETH_SIZE - pBuffer_p->m_uiTxMsgLen);
+		pBuffer_p->m_uiTxMsgLen = MIN_ETH_SIZE;
+	}
+	// set DMA address of buffer
+	EDRV_REGDW_WRITE((EDRV_REGDW_TSAD0 +
+			  (EdrvInstance_l.m_uiCurTxDesc * sizeof(DWORD))),
+			 (EdrvInstance_l.m_pTxBufDma +
+			  (uiBufferNumber * EDRV_MAX_FRAME_SIZE)));
+	dwTemp =
+	    EDRV_REGDW_READ((EDRV_REGDW_TSAD0 +
+			     (EdrvInstance_l.m_uiCurTxDesc * sizeof(DWORD))));
+//    printk("%s TSAD%u = 0x%08lX", __func__, EdrvInstance_l.m_uiCurTxDesc, dwTemp);
+
+	// start transmission
+	EDRV_REGDW_WRITE((EDRV_REGDW_TSD0 +
+			  (EdrvInstance_l.m_uiCurTxDesc * sizeof(DWORD))),
+			 (EDRV_REGDW_TSD_TXTH_DEF | pBuffer_p->m_uiTxMsgLen));
+	dwTemp =
+	    EDRV_REGDW_READ((EDRV_REGDW_TSD0 +
+			     (EdrvInstance_l.m_uiCurTxDesc * sizeof(DWORD))));
+//    printk(" TSD%u = 0x%08lX / 0x%08lX\n", EdrvInstance_l.m_uiCurTxDesc, dwTemp, (DWORD)(EDRV_REGDW_TSD_TXTH_DEF | pBuffer_p->m_uiTxMsgLen));
+
+      Exit:
+	return Ret;
+}
+
+#if 0
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvTxMsgReady
+//
+// Description: starts copying the buffer to the ethernet controller's FIFO
+//
+// Parameters:  pbBuffer_p - bufferdescriptor to transmit
+//
+// Returns:     Errorcode - kEplSuccessful
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvTxMsgReady(tEdrvTxBuffer * pBuffer_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiBufferNumber;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvTxMsgStart
+//
+// Description: starts transmission of the ethernet controller's FIFO
+//
+// Parameters:  pbBuffer_p - bufferdescriptor to transmit
+//
+// Returns:     Errorcode - kEplSuccessful
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvTxMsgStart(tEdrvTxBuffer * pBuffer_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	return Ret;
+}
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvReinitRx
+//
+// Description: reinitialize the Rx process, because of error
+//
+// Parameters:  void
+//
+// Returns:     void
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static void EdrvReinitRx(void)
+{
+	BYTE bCmd;
+
+	// simply switch off and on the receiver
+	// this will reset the CAPR register
+	bCmd = EDRV_REGB_READ(EDRV_REGB_COMMAND);
+	EDRV_REGB_WRITE(EDRV_REGB_COMMAND, (bCmd & ~EDRV_REGB_COMMAND_RE));
+	EDRV_REGB_WRITE(EDRV_REGB_COMMAND, bCmd);
+
+	// set receive configuration register
+	EDRV_REGDW_WRITE(EDRV_REGDW_RCR, EDRV_REGDW_RCR_DEF);
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:     EdrvInterruptHandler
+//
+// Description:  interrupt handler
+//
+// Parameters:   void
+//
+// Returns:      void
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if 0
+void EdrvInterruptHandler(void)
+{
+}
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,19)
+static int TgtEthIsr(int nIrqNum_p, void *ppDevInstData_p)
+#else
+static int TgtEthIsr(int nIrqNum_p, void *ppDevInstData_p,
+		     struct pt_regs *ptRegs_p)
+#endif
+{
+//    EdrvInterruptHandler();
+	tEdrvRxBuffer RxBuffer;
+	tEdrvTxBuffer *pTxBuffer;
+	WORD wStatus;
+	DWORD dwTxStatus;
+	DWORD dwRxStatus;
+	WORD wCurRx;
+	BYTE *pbRxBuf;
+	unsigned int uiLength;
+	int iHandled = IRQ_HANDLED;
+
+//    printk("¤");
+
+	// read the interrupt status
+	wStatus = EDRV_REGW_READ(EDRV_REGW_INT_STATUS);
+
+	// acknowledge the interrupts
+	EDRV_REGW_WRITE(EDRV_REGW_INT_STATUS, wStatus);
+
+	if (wStatus == 0) {
+		iHandled = IRQ_NONE;
+		goto Exit;
+	}
+	// process tasks
+	if ((wStatus & (EDRV_REGW_INT_TER | EDRV_REGW_INT_TOK)) != 0) {	// transmit interrupt
+
+		if (EdrvInstance_l.m_pbTxBuf == NULL) {
+			printk("%s Tx buffers currently not allocated\n",
+			       __func__);
+			goto Exit;
+		}
+		// read transmit status
+		dwTxStatus =
+		    EDRV_REGDW_READ((EDRV_REGDW_TSD0 +
+				     (EdrvInstance_l.m_uiCurTxDesc *
+				      sizeof(DWORD))));
+		if ((dwTxStatus & (EDRV_REGDW_TSD_TOK | EDRV_REGDW_TSD_TABT | EDRV_REGDW_TSD_TUN)) != 0) {	// transmit finished
+			EdrvInstance_l.m_uiCurTxDesc =
+			    (EdrvInstance_l.m_uiCurTxDesc + 1) & 0x03;
+			pTxBuffer = EdrvInstance_l.m_pLastTransmittedTxBuffer;
+			EdrvInstance_l.m_pLastTransmittedTxBuffer = NULL;
+
+			if ((dwTxStatus & EDRV_REGDW_TSD_TOK) != 0) {
+				EDRV_COUNT_TX;
+			} else if ((dwTxStatus & EDRV_REGDW_TSD_TUN) != 0) {
+				EDRV_COUNT_TX_FUN;
+			} else {	// assume EDRV_REGDW_TSD_TABT
+				EDRV_COUNT_TX_COL_RL;
+			}
+
+//            printk("T");
+			if (pTxBuffer != NULL) {
+				// call Tx handler of Data link layer
+				EdrvInstance_l.m_InitParam.
+				    m_pfnTxHandler(pTxBuffer);
+			}
+		} else {
+			EDRV_COUNT_TX_ERR;
+		}
+	}
+
+	if ((wStatus & (EDRV_REGW_INT_RER | EDRV_REGW_INT_FOVW | EDRV_REGW_INT_RXOVW | EDRV_REGW_INT_PUN)) != 0) {	// receive error interrupt
+
+		if ((wStatus & EDRV_REGW_INT_FOVW) != 0) {
+			EDRV_COUNT_RX_FOVW;
+		} else if ((wStatus & EDRV_REGW_INT_RXOVW) != 0) {
+			EDRV_COUNT_RX_OVW;
+		} else if ((wStatus & EDRV_REGW_INT_PUN) != 0) {	// Packet underrun
+			EDRV_TRACE_RX_PUN(wStatus);
+			EDRV_COUNT_RX_PUN;
+		} else {	/*if ((wStatus & EDRV_REGW_INT_RER) != 0) */
+
+			EDRV_TRACE_RX_ERR(wStatus);
+			EDRV_COUNT_RX_ERR;
+		}
+
+		// reinitialize Rx process
+		EdrvReinitRx();
+	}
+
+	if ((wStatus & EDRV_REGW_INT_ROK) != 0) {	// receive interrupt
+
+		if (EdrvInstance_l.m_pbRxBuf == NULL) {
+			printk("%s Rx buffers currently not allocated\n",
+			       __func__);
+			goto Exit;
+		}
+		// read current offset in receive buffer
+		wCurRx =
+		    (EDRV_REGW_READ(EDRV_REGW_CAPR) +
+		     0x10) % EDRV_RX_BUFFER_LENGTH;
+
+		while ((EDRV_REGB_READ(EDRV_REGB_COMMAND) & EDRV_REGB_COMMAND_BUFE) == 0) {	// frame available
+
+			// calculate pointer to current frame in receive buffer
+			pbRxBuf = EdrvInstance_l.m_pbRxBuf + wCurRx;
+
+			// read receive status DWORD
+			dwRxStatus = le32_to_cpu(*((DWORD *) pbRxBuf));
+
+			// calculate length of received frame
+			uiLength = dwRxStatus >> 16;
+
+			if (uiLength == 0xFFF0) {	// frame is unfinished (maybe early Rx interrupt is active)
+				break;
+			}
+
+			if ((dwRxStatus & EDRV_RXSTAT_ROK) == 0) {	// error occured while receiving this frame
+				// ignore it
+				if ((dwRxStatus & EDRV_RXSTAT_FAE) != 0) {
+					EDRV_COUNT_RX_FAE;
+				} else if ((dwRxStatus & EDRV_RXSTAT_CRC) != 0) {
+					EDRV_TRACE_RX_CRC(dwRxStatus);
+					EDRV_COUNT_RX_CRC;
+				} else {
+					EDRV_TRACE_RX_ERR(dwRxStatus);
+					EDRV_COUNT_RX_ERR;
+				}
+
+				// reinitialize Rx process
+				EdrvReinitRx();
+
+				break;
+			} else {	// frame is OK
+				RxBuffer.m_BufferInFrame =
+				    kEdrvBufferLastInFrame;
+				RxBuffer.m_uiRxMsgLen = uiLength - ETH_CRC_SIZE;
+				RxBuffer.m_pbBuffer =
+				    pbRxBuf + sizeof(dwRxStatus);
+
+//                printk("R");
+				EDRV_COUNT_RX;
+
+				// call Rx handler of Data link layer
+				EdrvInstance_l.m_InitParam.
+				    m_pfnRxHandler(&RxBuffer);
+			}
+
+			// calulate new offset (DWORD aligned)
+			wCurRx =
+			    (WORD) ((wCurRx + uiLength + sizeof(dwRxStatus) +
+				     3) & ~0x3);
+			EDRV_TRACE_CAPR(wCurRx - 0x10);
+			EDRV_REGW_WRITE(EDRV_REGW_CAPR, wCurRx - 0x10);
+
+			// reread current offset in receive buffer
+			wCurRx =
+			    (EDRV_REGW_READ(EDRV_REGW_CAPR) +
+			     0x10) % EDRV_RX_BUFFER_LENGTH;
+
+		}
+	}
+
+	if ((wStatus & EDRV_REGW_INT_SERR) != 0) {	// PCI error
+		EDRV_COUNT_PCI_ERR;
+	}
+
+	if ((wStatus & EDRV_REGW_INT_TIMEOUT) != 0) {	// Timeout
+		EDRV_COUNT_TIMEOUT;
+	}
+
+      Exit:
+	return iHandled;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvInitOne
+//
+// Description: initializes one PCI device
+//
+// Parameters:  pPciDev             = pointer to corresponding PCI device structure
+//              pId                 = PCI device ID
+//
+// Returns:     (int)               = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static int EdrvInitOne(struct pci_dev *pPciDev, const struct pci_device_id *pId)
+{
+	int iResult = 0;
+	DWORD dwTemp;
+
+	if (EdrvInstance_l.m_pPciDev != NULL) {	// Edrv is already connected to a PCI device
+		printk("%s device %s discarded\n", __func__,
+		       pci_name(pPciDev));
+		iResult = -ENODEV;
+		goto Exit;
+	}
+
+	if (pPciDev->revision >= 0x20) {
+		printk
+		    ("%s device %s is an enhanced 8139C+ version, which is not supported\n",
+		     __func__, pci_name(pPciDev));
+		iResult = -ENODEV;
+		goto Exit;
+	}
+
+	EdrvInstance_l.m_pPciDev = pPciDev;
+
+	// enable device
+	printk("%s enable device\n", __func__);
+	iResult = pci_enable_device(pPciDev);
+	if (iResult != 0) {
+		goto Exit;
+	}
+
+	if ((pci_resource_flags(pPciDev, 1) & IORESOURCE_MEM) == 0) {
+		iResult = -ENODEV;
+		goto Exit;
+	}
+
+	printk("%s request regions\n", __func__);
+	iResult = pci_request_regions(pPciDev, DRV_NAME);
+	if (iResult != 0) {
+		goto Exit;
+	}
+
+	printk("%s ioremap\n", __func__);
+	EdrvInstance_l.m_pIoAddr =
+	    ioremap(pci_resource_start(pPciDev, 1),
+		    pci_resource_len(pPciDev, 1));
+	if (EdrvInstance_l.m_pIoAddr == NULL) {	// remap of controller's register space failed
+		iResult = -EIO;
+		goto Exit;
+	}
+	// enable PCI busmaster
+	printk("%s enable busmaster\n", __func__);
+	pci_set_master(pPciDev);
+
+	// reset controller
+	printk("%s reset controller\n", __func__);
+	EDRV_REGB_WRITE(EDRV_REGB_COMMAND, EDRV_REGB_COMMAND_RST);
+
+	// wait until reset has finished
+	for (iResult = 500; iResult > 0; iResult--) {
+		if ((EDRV_REGB_READ(EDRV_REGB_COMMAND) & EDRV_REGB_COMMAND_RST)
+		    == 0) {
+			break;
+		}
+
+		schedule_timeout(10);
+	}
+
+	// check hardware version, i.e. chip ID
+	dwTemp = EDRV_REGDW_READ(EDRV_REGDW_TCR);
+	if (((dwTemp & EDRV_REGDW_TCR_VER_MASK) != EDRV_REGDW_TCR_VER_C)
+	    && ((dwTemp & EDRV_REGDW_TCR_VER_MASK) != EDRV_REGDW_TCR_VER_D)) {	// unsupported chip
+		printk("%s Unsupported chip! TCR = 0x%08lX\n", __func__,
+		       dwTemp);
+		iResult = -ENODEV;
+		goto Exit;
+	}
+	// disable interrupts
+	printk("%s disable interrupts\n", __func__);
+	EDRV_REGW_WRITE(EDRV_REGW_INT_MASK, 0);
+	// acknowledge all pending interrupts
+	EDRV_REGW_WRITE(EDRV_REGW_INT_STATUS,
+			EDRV_REGW_READ(EDRV_REGW_INT_STATUS));
+
+	// install interrupt handler
+	printk("%s install interrupt handler\n", __func__);
+	iResult =
+	    request_irq(pPciDev->irq, TgtEthIsr, IRQF_SHARED,
+			DRV_NAME /*pPciDev->dev.name */ , pPciDev);
+	if (iResult != 0) {
+		goto Exit;
+	}
+
+/*
+    // unlock configuration registers
+    printk("%s unlock configuration registers\n", __func__);
+    EDRV_REGB_WRITE(EDRV_REGB_CMD9346, EDRV_REGB_CMD9346_UNLOCK);
+
+    // check if user specified a MAC address
+    printk("%s check specified MAC address\n", __func__);
+    for (iResult = 0; iResult < 6; iResult++)
+    {
+        if (EdrvInstance_l.m_InitParam.m_abMyMacAddr[iResult] != 0)
+        {
+            printk("%s set local MAC address\n", __func__);
+            // write this MAC address to controller
+            EDRV_REGDW_WRITE(EDRV_REGDW_IDR0,
+                le32_to_cpu(*((DWORD*)&EdrvInstance_l.m_InitParam.m_abMyMacAddr[0])));
+            dwTemp = EDRV_REGDW_READ(EDRV_REGDW_IDR0);
+
+            EDRV_REGDW_WRITE(EDRV_REGDW_IDR4,
+                le32_to_cpu(*((DWORD*)&EdrvInstance_l.m_InitParam.m_abMyMacAddr[4])));
+            dwTemp = EDRV_REGDW_READ(EDRV_REGDW_IDR4);
+            break;
+        }
+    }
+    iResult = 0;
+
+    // lock configuration registers
+    EDRV_REGB_WRITE(EDRV_REGB_CMD9346, EDRV_REGB_CMD9346_LOCK);
+*/
+
+	// allocate buffers
+	printk("%s allocate buffers\n", __func__);
+	EdrvInstance_l.m_pbTxBuf =
+	    pci_alloc_consistent(pPciDev, EDRV_TX_BUFFER_SIZE,
+				 &EdrvInstance_l.m_pTxBufDma);
+	if (EdrvInstance_l.m_pbTxBuf == NULL) {
+		iResult = -ENOMEM;
+		goto Exit;
+	}
+
+	EdrvInstance_l.m_pbRxBuf =
+	    pci_alloc_consistent(pPciDev, EDRV_RX_BUFFER_SIZE,
+				 &EdrvInstance_l.m_pRxBufDma);
+	if (EdrvInstance_l.m_pbRxBuf == NULL) {
+		iResult = -ENOMEM;
+		goto Exit;
+	}
+	// reset pointers for Tx buffers
+	printk("%s reset pointers fo Tx buffers\n", __func__);
+	EDRV_REGDW_WRITE(EDRV_REGDW_TSAD0, 0);
+	dwTemp = EDRV_REGDW_READ(EDRV_REGDW_TSAD0);
+	EDRV_REGDW_WRITE(EDRV_REGDW_TSAD1, 0);
+	dwTemp = EDRV_REGDW_READ(EDRV_REGDW_TSAD1);
+	EDRV_REGDW_WRITE(EDRV_REGDW_TSAD2, 0);
+	dwTemp = EDRV_REGDW_READ(EDRV_REGDW_TSAD2);
+	EDRV_REGDW_WRITE(EDRV_REGDW_TSAD3, 0);
+	dwTemp = EDRV_REGDW_READ(EDRV_REGDW_TSAD3);
+
+	printk("    Command = 0x%02X\n",
+	       (WORD) EDRV_REGB_READ(EDRV_REGB_COMMAND));
+
+	// set pointer for receive buffer in controller
+	printk("%s set pointer to Rx buffer\n", __func__);
+	EDRV_REGDW_WRITE(EDRV_REGDW_RBSTART, EdrvInstance_l.m_pRxBufDma);
+
+	// enable transmitter and receiver
+	printk("%s enable Tx and Rx", __func__);
+	EDRV_REGB_WRITE(EDRV_REGB_COMMAND,
+			(EDRV_REGB_COMMAND_RE | EDRV_REGB_COMMAND_TE));
+	printk("  Command = 0x%02X\n",
+	       (WORD) EDRV_REGB_READ(EDRV_REGB_COMMAND));
+
+	// clear missed packet counter to enable Rx/Tx process
+	EDRV_REGDW_WRITE(EDRV_REGDW_MPC, 0);
+
+	// set transmit configuration register
+	printk("%s set Tx conf register", __func__);
+	EDRV_REGDW_WRITE(EDRV_REGDW_TCR, EDRV_REGDW_TCR_DEF);
+	printk(" = 0x%08X\n", EDRV_REGDW_READ(EDRV_REGDW_TCR));
+
+	// set receive configuration register
+	printk("%s set Rx conf register", __func__);
+	EDRV_REGDW_WRITE(EDRV_REGDW_RCR, EDRV_REGDW_RCR_DEF);
+	printk(" = 0x%08X\n", EDRV_REGDW_READ(EDRV_REGDW_RCR));
+
+	// reset multicast MAC address filter
+	EDRV_REGDW_WRITE(EDRV_REGDW_MAR0, 0);
+	dwTemp = EDRV_REGDW_READ(EDRV_REGDW_MAR0);
+	EDRV_REGDW_WRITE(EDRV_REGDW_MAR4, 0);
+	dwTemp = EDRV_REGDW_READ(EDRV_REGDW_MAR4);
+
+/*
+    // enable transmitter and receiver
+    printk("%s enable Tx and Rx", __func__);
+    EDRV_REGB_WRITE(EDRV_REGB_COMMAND, (EDRV_REGB_COMMAND_RE | EDRV_REGB_COMMAND_TE));
+    printk("  Command = 0x%02X\n", (WORD) EDRV_REGB_READ(EDRV_REGB_COMMAND));
+*/
+	// disable early interrupts
+	EDRV_REGW_WRITE(EDRV_REGW_MULINT, 0);
+
+	// enable interrupts
+	printk("%s enable interrupts\n", __func__);
+	EDRV_REGW_WRITE(EDRV_REGW_INT_MASK, EDRV_REGW_INT_MASK_DEF);
+
+      Exit:
+	printk("%s finished with %d\n", __func__, iResult);
+	return iResult;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvRemoveOne
+//
+// Description: shuts down one PCI device
+//
+// Parameters:  pPciDev             = pointer to corresponding PCI device structure
+//
+// Returns:     (void)
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static void EdrvRemoveOne(struct pci_dev *pPciDev)
+{
+
+	if (EdrvInstance_l.m_pPciDev != pPciDev) {	// trying to remove unknown device
+		BUG_ON(EdrvInstance_l.m_pPciDev != pPciDev);
+		goto Exit;
+	}
+	// disable transmitter and receiver
+	EDRV_REGB_WRITE(EDRV_REGB_COMMAND, 0);
+
+	// disable interrupts
+	EDRV_REGW_WRITE(EDRV_REGW_INT_MASK, 0);
+
+	// remove interrupt handler
+	free_irq(pPciDev->irq, pPciDev);
+
+	// free buffers
+	if (EdrvInstance_l.m_pbTxBuf != NULL) {
+		pci_free_consistent(pPciDev, EDRV_TX_BUFFER_SIZE,
+				    EdrvInstance_l.m_pbTxBuf,
+				    EdrvInstance_l.m_pTxBufDma);
+		EdrvInstance_l.m_pbTxBuf = NULL;
+	}
+
+	if (EdrvInstance_l.m_pbRxBuf != NULL) {
+		pci_free_consistent(pPciDev, EDRV_RX_BUFFER_SIZE,
+				    EdrvInstance_l.m_pbRxBuf,
+				    EdrvInstance_l.m_pRxBufDma);
+		EdrvInstance_l.m_pbRxBuf = NULL;
+	}
+	// unmap controller's register space
+	if (EdrvInstance_l.m_pIoAddr != NULL) {
+		iounmap(EdrvInstance_l.m_pIoAddr);
+	}
+	// disable the PCI device
+	pci_disable_device(pPciDev);
+
+	// release memory regions
+	pci_release_regions(pPciDev);
+
+	EdrvInstance_l.m_pPciDev = NULL;
+
+      Exit:;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvCalcHash
+//
+// Description: function calculates the entry for the hash-table from MAC
+//              address
+//
+// Parameters:  pbMAC_p - pointer to MAC address
+//
+// Returns:     hash value
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#define HASH_BITS              6	// used bits in hash
+#define CRC32_POLY    0x04C11DB6	//
+//#define CRC32_POLY    0xEDB88320  //
+// G(x) = x32 + x26 + x23 + x22 + x16 + x12 + x11 + x10 + x8 + x7 + x5 + x4 + x2 + x + 1
+
+static BYTE EdrvCalcHash(BYTE * pbMAC_p)
+{
+	DWORD dwByteCounter;
+	DWORD dwBitCounter;
+	DWORD dwData;
+	DWORD dwCrc;
+	DWORD dwCarry;
+	BYTE *pbData;
+	BYTE bHash;
+
+	pbData = pbMAC_p;
+
+	// calculate crc32 value of mac address
+	dwCrc = 0xFFFFFFFF;
+
+	for (dwByteCounter = 0; dwByteCounter < 6; dwByteCounter++) {
+		dwData = *pbData;
+		pbData++;
+		for (dwBitCounter = 0; dwBitCounter < 8;
+		     dwBitCounter++, dwData >>= 1) {
+			dwCarry = (((dwCrc >> 31) ^ dwData) & 1);
+			dwCrc = dwCrc << 1;
+			if (dwCarry != 0) {
+				dwCrc = (dwCrc ^ CRC32_POLY) | dwCarry;
+			}
+		}
+	}
+
+//    printk("MyCRC = 0x%08lX\n", dwCrc);
+	// only upper 6 bits (HASH_BITS) are used
+	// which point to specific bit in the hash registers
+	bHash = (BYTE) ((dwCrc >> (32 - HASH_BITS)) & 0x3f);
+
+	return bHash;
+}
diff --git a/drivers/staging/epl/EdrvFec.h b/drivers/staging/epl/EdrvFec.h
new file mode 100644
index 000000000000..5f252fbed744
--- /dev/null
+++ b/drivers/staging/epl/EdrvFec.h
@@ -0,0 +1,114 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  interface for ethernetdriver
+                "fast ethernet controller" (FEC)
+                freescale coldfire MCF528x and compatible FEC
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EdrvFec.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                Dev C++ and GNU-Compiler for m68k
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2005/08/01 m.b.:   start of implementation
+
+****************************************************************************/
+
+#ifndef _EDRVFEC_H_
+#define _EDRVFEC_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+// do this in config header
+#define TARGET_HARDWARE TGTHW_SPLC_CF54
+
+// base addresses
+#if ((TARGET_HARDWARE & TGT_CPU_MASK_) == TGT_CPU_5282)
+
+#elif ((TARGET_HARDWARE & TGT_CPU_MASK_) == TGT_CPU_5485)
+
+#else
+
+#error 'ERROR: Target was never implemented!'
+
+#endif
+
+//---------------------------------------------------------------------------
+// types
+//---------------------------------------------------------------------------
+
+// Rx and Tx buffer descriptor format
+typedef struct {
+	WORD m_wStatus;		// control / status  ---  used by edrv, do not change in application
+	WORD m_wLength;		// transfer length
+	BYTE *m_pbData;		// buffer address
+} tBufferDescr;
+
+#if ((TARGET_HARDWARE & TGT_CPU_MASK_) == TGT_CPU_5282)
+
+#elif ((TARGET_HARDWARE & TGT_CPU_MASK_) == TGT_CPU_5485)
+
+#endif
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EDRV_FEC_H_
diff --git a/drivers/staging/epl/EdrvFec5282.h b/drivers/staging/epl/EdrvFec5282.h
new file mode 100644
index 000000000000..a16bb1d4da79
--- /dev/null
+++ b/drivers/staging/epl/EdrvFec5282.h
@@ -0,0 +1,340 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  interface for ethernetdriver
+                "fast ethernet controller" (FEC)
+                freescale coldfire MCF528x and compatible FEC
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EdrvFec5282.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                Dev C++ and GNU-Compiler for m68k
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2005/08/01 m.b.:   start of implementation
+
+****************************************************************************/
+
+#ifndef _EDRVFEC_H_
+#define _EDRVFEC_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+// base addresses
+#define FEC0_ADDR 0x0000
+#define FEC1_ADDR 0x0000	//tbd
+
+// control / status registers
+#define FEC_EIR                 0x1004	// interrupt event register
+#define FEC_EIMR                0x1008	// interrupt mask register
+#define FEC_RDAR                0x1010	// receive descriptor active register
+#define FEC_TDAR                0x1014	// transmit descriptor active register
+#define FEC_ECR                 0x1024	// ethernet control register
+#define FEC_MMFR                0x1040	// MII data register
+#define FEC_MSCR                0x1044	// MII speed register
+#define FEC_MIBC                0x1064	// MIB control/status register
+#define FEC_RCR                 0x1084	// receive control register
+#define FEC_TCR                 0x10C4	// transmit control register
+#define FEC_PALR                0x10E4	// physical address low register
+#define FEC_PAUR                0x10E8	// physical address high + type register
+#define FEC_OPD                 0x10EC	// opcode + pause register
+#define FEC_IAUR                0x1118	// upper 32 bit of individual hash table
+#define FEC_IALR                0x111C	// lower 32 bit of individual hash table
+#define FEC_GAUR                0x1120	// upper 32 bit of group hash table
+#define FEC_GALR                0x1124	// lower 32 bit of group hash table
+#define FEC_TFWR                0x1144	// transmit FIFO watermark
+#define FEC_FRBR                0x114C	// FIFO receive bound register
+#define FEC_FRSR                0x1150	// FIFO receive FIFO start register
+#define FEC_ERDSR               0x1180	// pointer to receive descriptor ring
+#define FEC_ETDSR               0x1184	// pointer to transmit descriptor ring
+#define FEC_EMRBR               0x1188	// maximum receive buffer size
+
+// mib block counters memory map
+#define FEC_RMON_T_DROP         0x1200	// count of frames not counted correctly
+#define FEC_RMON_T_PACKETS      0x1204	// RMON tx packet count
+#define FEC_RMON_T_BC_PKT       0x1208	// RMON tx broadcast packets
+#define FEC_RMON_T_MC_PKT       0x120C	// RMON tx multicast packets
+#define FEC_RMON_T_CRC_ALIGN    0x1210	// RMON tx packets w CRC/align error
+#define FEC_RMON_T_UNDERSIZE    0x1214	// RMON tx packets < 64 bytes, good CRC
+#define FEC_RMON_T_OVERSIZE     0x1218	// RMON tx packets > MAX_FL bytes, good CRC
+#define FEC_RMON_T_FRAG         0x121C	// RMON tx packets < 64 bytes, bad CRC
+#define FEC_RMON_T_JAB          0x1220	// RMON tx packets > MAX_FL bytes, bad CRC
+#define FEC_RMON_T_COL          0x1224	// RMON tx collision count
+#define FEC_RMON_T_P64          0x1228	// RMON tx           64 byte packets
+#define FEC_RMON_T_P65TO127     0x122C	// RMON tx   65 to  127 byte packets
+#define FEC_RMON_T_P128TO255    0x1230	// RMON tx  128 to  255 byte packets
+#define FEC_RMON_T_P256TO511    0x1234	// RMON tx  256 to  511 byte packets
+#define FEC_RMON_T_P512TO1023   0x1238	// RMON tx  512 to 1023 byte packets
+#define FEC_RMON_T_P1024TO2047  0x123C	// RMON tx 1024 to 2047 byte packets
+#define FEC_RMON_T_P_GTE2048    0x1240	// RMON tx w > 2048 bytes
+#define FEC_RMON_T_OCTETS       0x1244	// RMON tx octets
+#define FEC_IEEE_T_DROP         0x1248	// count of frames not counted correctly
+#define FEC_IEEE_T_FRAME_OK     0x124C	// frames transmitted OK
+#define FEC_IEEE_T_1COL         0x1250	// frames transmitted with single collision
+#define FEC_IEEE_T_MCOL         0x1254	// frames transmitted with multiple collisions
+#define FEC_IEEE_T_DEF          0x1258	// frames transmitted after deferral delay
+#define FEC_IEEE_T_LCOL         0x125C	// frames transmitted with late collisions
+#define FEC_IEEE_T_EXCOL        0x1260	// frames transmitted with excessive collisions
+#define FEC_IEEE_T_MACERR       0x1264	// frames transmitted with tx-FIFO underrun
+#define FEC_IEEE_T_CSERR        0x1268	// frames transmitted with carrier sense error
+#define FEC_IEEE_T_SQE          0x126C	// frames transmitted with SQE error
+#define FEC_IEEE_T_FDXFC        0x1270	// flow control pause frames transmitted
+#define FEC_IEEE_T_OCTETS_OK    0x1274	// octet count for frames transmitted w/o error
+#define FEC_RMON_R_PACKETS      0x1284	// RMON rx packet count
+#define FEC_RMON_R_BC_PKT       0x1288	// RMON rx broadcast packets
+#define FEC_RMON_R_MC_PKT       0x128C	// RMON rx multicast packets
+#define FEC_RMON_R_CRC_ALIGN    0x1290	// RMON rx packets w CRC/align error
+#define FEC_RMON_R_UNDERSIZE    0x1294	// RMON rx packets < 64 bytes, good CRC
+#define FEC_RMON_R_OVERSIZE     0x1298	// RMON rx packets > MAX_FL bytes, good CRC
+#define FEC_RMON_R_FRAG         0x129C	// RMON rx packets < 64 bytes, bad CRC
+#define FEC_RMON_R_JAB          0x12A0	// RMON rx packets > MAX_FL bytes, bad CRC
+#define FEC_RMON_R_RESVD_0      0x12A4	//
+#define FEC_RMON_R_P64          0x12A8	// RMON rx           64 byte packets
+#define FEC_RMON_R_P65T0127     0x12AC	// RMON rx   65 to  127 byte packets
+#define FEC_RMON_R_P128TO255    0x12B0	// RMON rx  128 to  255 byte packets
+#define FEC_RMON_R_P256TO511    0x12B4	// RMON rx  256 to  511 byte packets
+#define FEC_RMON_R_P512TO1023   0x12B8	// RMON rx  512 to 1023 byte packets
+#define FEC_RMON_R_P1024TO2047  0x12BC	// RMON rx 1024 to 2047 byte packets
+#define FEC_RMON_R_GTE2048      0x12C0	// RMON rx w > 2048 bytes
+#define FEC_RMON_R_OCTETS       0x12C4	// RMON rx octets
+#define FEC_IEEE_R_DROP         0x12C8	// count of frames not counted correctly
+#define FEC_IEEE_R_FRAME_OK     0x12CC	// frames received OK
+#define FEC_IEEE_R_CRC          0x12D0	// frames received with CRC error
+#define FEC_IEEE_R_ALIGN        0x12D4	// frames received with alignment error
+#define FEC_IEEE_R_MACERR       0x12D8	// receive FIFO overflow count
+#define FEC_IEEE_R_FDXFC        0x12DC	// flow control pause frames received
+#define FEC_IEEE_R_OCTETS_OK    0x12E0	// octet count for frames rcvd w/o error
+
+// register bit definitions and macros
+#define FEC_EIR_UN              (0x00080000)
+#define FEC_EIR_RL              (0x00100000)
+#define FEC_EIR_LC              (0x00200000)
+#define FEC_EIR_EBERR           (0x00400000)
+#define FEC_EIR_MII             (0x00800000)
+#define FEC_EIR_RXB             (0x01000000)
+#define FEC_EIR_RXF             (0x02000000)
+#define FEC_EIR_TXB             (0x04000000)
+#define FEC_EIR_TXF             (0x08000000)
+#define FEC_EIR_GRA             (0x10000000)
+#define FEC_EIR_BABT            (0x20000000)
+#define FEC_EIR_BABR            (0x40000000)
+#define FEC_EIR_HBERR           (0x80000000)
+
+#define FEC_EIMR_UN             (0x00080000)
+#define FEC_EIMR_RL             (0x00100000)
+#define FEC_EIMR_LC             (0x00200000)
+#define FEC_EIMR_EBERR          (0x00400000)
+#define FEC_EIMR_MII            (0x00800000)
+#define FEC_EIMR_RXB            (0x01000000)
+#define FEC_EIMR_RXF            (0x02000000)
+#define FEC_EIMR_TXB            (0x04000000)
+#define FEC_EIMR_TXF            (0x08000000)
+#define FEC_EIMR_GRA            (0x10000000)
+#define FEC_EIMR_BABT           (0x20000000)
+#define FEC_EIMR_BABR           (0x40000000)
+#define FEC_EIMR_HBERR          (0x80000000)
+
+#define FEC_RDAR_R_DES_ACTIVE   (0x01000000)
+
+#define FEC_TDAR_X_DES_ACTIVE   (0x01000000)
+
+#define FEC_ECR_RESET           (0x00000001)
+#define FEC_ECR_ETHER_EN        (0x00000002)
+
+#define FEC_MMFR_DATA(x)        (((x) & 0xFFFF))
+#define FEC_MMFR_TA             (0x00020000)
+#define FEC_MMFR_RA(x)          (((x) & 0x1F) << 18)
+#define FEC_MMFR_PA(x)          (((x) & 0x1F) << 23)
+#define FEC_MMFR_OP_WR          (0x10000000)
+#define FEC_MMFR_OP_RD          (0x20000000)
+#define FEC_MMFR_ST             (0x40000000)
+
+#define FEC_MSCR_MII_SPEED(x)   (((x) & 0x1F) << 1)
+#define FEC_MSCR_DIS_PREAMBLE   (0x00000008)
+
+#define FEC_MIBC_MIB_IDLE       (0x40000000)
+#define FEC_MIBC_MIB_DISABLE    (0x80000000)
+
+#define FEC_RCR_LOOP            (0x00000001)
+#define FEC_RCR_DRT             (0x00000002)
+#define FEC_RCR_MII_MODE        (0x00000004)
+#define FEC_RCR_PROM            (0x00000008)
+#define FEC_RCR_BC_REJ          (0x00000010)
+#define FEC_RCR_FCE             (0x00000020)
+#define FEC_RCR_MAX_FL(x)       (((x) & 0x07FF) << 16)
+
+#define FEC_TCR_GTS             (0x00000001)
+#define FEC_TCR_HBC             (0x00000002)
+#define FEC_TCR_FDEN            (0x00000004)
+#define FEC_TCR_TFC_PAUSE       (0x00000008)
+#define FEC_TCR_RFC_PAUSE       (0x00000010)
+
+#define FEC_PALR_BYTE3(x)       (((x) & 0xFF) <<  0)
+#define FEC_PALR_BYTE2(x)       (((x) & 0xFF) <<  8)
+#define FEC_PALR_BYTE1(x)       (((x) & 0xFF) << 16)
+#define FEC_PALR_BYTE0(x)       (((x) & 0xFF) << 24)
+
+//#define FEC_PAUR_TYPE(x)        (((x) & 0xFFFF) <<  0)
+#define FEC_PAUR_BYTE5(x)       (((x) &   0xFF) << 16)
+#define FEC_PAUR_BYTE4(x)       (((x) &   0xFF) << 24)
+
+#define FEC_OPD_PAUSE_DUR(x)    (((x) & 0xFFFF))
+//#define FEC_OPD_OPCODE(x)       (((x) & 0xFFFF) << 16)
+
+//m.b.
+#define FEC_IAUR_BYTE7(x)       (((x) & 0xFF) <<  0)
+#define FEC_IAUR_BYTE6(x)       (((x) & 0xFF) <<  8)
+#define FEC_IAUR_BYTE5(x)       (((x) & 0xFF) << 16)
+#define FEC_IAUR_BYTE4(x)       (((x) & 0xFF) << 24)
+
+#define FEC_IALR_BYTE3(x)       (((x) & 0xFF) <<  0)
+#define FEC_IALR_BYTE2(x)       (((x) & 0xFF) <<  8)
+#define FEC_IALR_BYTE1(x)       (((x) & 0xFF) << 16)
+#define FEC_IALR_BYTE0(x)       (((x) & 0xFF) << 24)
+
+#define FEC_GAUR_BYTE7(x)       (((x) & 0xFF) <<  0)
+#define FEC_GAUR_BYTE6(x)       (((x) & 0xFF) <<  8)
+#define FEC_GAUR_BYTE5(x)       (((x) & 0xFF) << 16)
+#define FEC_GAUR_BYTE4(x)       (((x) & 0xFF) << 24)
+
+#define FEC_GALR_BYTE3(x)       (((x) & 0xFF) <<  0)
+#define FEC_GALR_BYTE2(x)       (((x) & 0xFF) <<  8)
+#define FEC_GALR_BYTE1(x)       (((x) & 0xFF) << 16)
+#define FEC_GALR_BYTE0(x)       (((x) & 0xFF) << 24)
+// ^^^^
+
+#define FEC_TFWR_X_WMRK_64      (0x00000001)
+#define FEC_TFWR_X_WMRK_128     (0x00000002)
+#define FEC_TFWR_X_WMRK_192     (0x00000003)
+
+//m.b.
+#define FEC_FRBR_R_BOUND(x)     (((x) & 0xFF) << 2)
+
+//m.b.
+#define FEC_FRSR_R_FSTART(x)    (((x) & 0xFF) << 2)
+
+//m.b.
+#define FEC_ERDSR_R_DES_START(x)  (((x) & 0x3FFFFFFF) << 2)
+
+//m.b.
+#define FEC_ETSDR_X_DES_START(x)  (((x) & 0x3FFFFFFF) << 2)
+
+#define FEC_EMRBR_R_BUF_SIZE(x) (((x) & 0x7F) <<  4)
+
+#define FEC_RxBD_TR             0x0001
+#define FEC_RxBD_OV             0x0002
+#define FEC_RxBD_CR             0x0004
+#define FEC_RxBD_NO             0x0010
+#define FEC_RxBD_LG             0x0020
+#define FEC_RxBD_MC             0x0040
+#define FEC_RxBD_BC             0x0080
+#define FEC_RxBD_M              0x0100
+#define FEC_RxBD_L              0x0800
+#define FEC_RxBD_R02            0x1000
+#define FEC_RxBD_W              0x2000
+#define FEC_RxBD_R01            0x4000
+#define FEC_RxBD_INUSE          0x4000
+#define FEC_RxBD_E              0x8000
+
+//m.b.
+//#define FEC_TxBD_CSL            0x0001
+//#define FEC_TxBD_UN             0x0002
+//#define FEC_TxBD_RL             0x0040
+//#define FEC_TxBD_LC             0x0080
+//#define FEC_TxBD_HB             0x0100
+//#define FEC_TxBD_DEF            0x0200
+#define FEC_TxBD_ABC            0x0200
+// ^^^^
+#define FEC_TxBD_TC             0x0400
+#define FEC_TxBD_L              0x0800
+#define FEC_TxBD_TO2            0x1000
+#define FEC_TxBD_W              0x2000
+#define FEC_TxBD_TO1            0x4000
+#define FEC_TxBD_INUSE          0x4000
+#define FEC_TxBD_R              0x8000
+
+//---------------------------------------------------------------------------
+// types
+//---------------------------------------------------------------------------
+
+// Rx and Tx buffer descriptor format
+typedef struct {
+	WORD m_wStatus;		// control / status  ---  used by edrv, do not change in application
+	WORD m_wLength;		// transfer length
+	BYTE *m_pbData;		// buffer address
+} tBufferDescr;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if (NO_OF_INSTANCES > 1)
+#define ECI_WRITE_DW_REG(off,val)       (*(DWORD *)(void *)(&__IPSBAR[off]) = val)
+#define ECI_READ_DW_REG(off)            (*(DWORD *)(void *)(&__IPSBAR[off]))
+#else
+#if (EDRV_USED_ETH_CTRL == 0)
+#define ECI_WRITE_DW_REG(off,val)       (*(DWORD *)(void *)(&__IPSBAR[FEC0_ADDR+off]) = val)
+#define ECI_READ_DW_REG(off)            (*(DWORD *)(void *)(&__IPSBAR[FEC0_ADDR+off]))
+#else
+#define ECI_WRITE_DW_REG(off,val)       (*(DWORD *)(void *)(&__IPSBAR[FEC1_ADDR+off]) = val)
+#define ECI_READ_DW_REG(off)            (*(DWORD *)(void *)(&__IPSBAR[FEC1_ADDR+off]))
+#endif
+#endif
+
+#endif // #ifndef _EDRV_FEC_H_
diff --git a/drivers/staging/epl/EdrvSim.h b/drivers/staging/epl/EdrvSim.h
new file mode 100644
index 000000000000..39300e321d6c
--- /dev/null
+++ b/drivers/staging/epl/EdrvSim.h
@@ -0,0 +1,89 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  interface for ethernet driver simulation
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EdrvSim.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                Dev C++ and GNU-Compiler for m68k
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/15 d.k.:   start of implementation
+
+****************************************************************************/
+
+#ifndef _EDRVSIM_H_
+#define _EDRVSIM_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+void EdrvRxInterruptHandler(BYTE bBufferInFrame_p, BYTE * pbEthernetData_p,
+			    WORD wDataLen_p);
+
+#endif // #ifndef _EDRVSIM_H_
diff --git a/drivers/staging/epl/Epl.h b/drivers/staging/epl/Epl.h
new file mode 100644
index 000000000000..be60f771b63a
--- /dev/null
+++ b/drivers/staging/epl/Epl.h
@@ -0,0 +1,273 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for EPL API layer
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: Epl.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/11/17 16:40:39 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_API_H_
+#define _EPL_API_H_
+
+#include "EplInc.h"
+#include "EplSdo.h"
+#include "EplObd.h"
+#include "EplLed.h"
+#include "EplEvent.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef struct {
+	unsigned int m_uiNodeId;
+	tEplNmtState m_NmtState;
+	tEplNmtNodeEvent m_NodeEvent;
+	WORD m_wErrorCode;	// EPL error code if m_NodeEvent == kEplNmtNodeEventError
+	BOOL m_fMandatory;
+
+} tEplApiEventNode;
+
+typedef struct {
+	tEplNmtState m_NmtState;	// local NMT state
+	tEplNmtBootEvent m_BootEvent;
+	WORD m_wErrorCode;	// EPL error code if m_BootEvent == kEplNmtBootEventError
+
+} tEplApiEventBoot;
+
+typedef struct {
+	tEplLedType m_LedType;	// type of the LED (e.g. Status or Error)
+	BOOL m_fOn;		// state of the LED (e.g. on or off)
+
+} tEplApiEventLed;
+
+typedef enum {
+	kEplApiEventNmtStateChange = 0x10,	// m_NmtStateChange
+//    kEplApiEventRequestNmt     = 0x11,    // m_bNmtCmd
+	kEplApiEventCriticalError = 0x12,	// m_InternalError, Stack halted
+	kEplApiEventWarning = 0x13,	// m_InternalError, Stack running
+	kEplApiEventNode = 0x20,	// m_Node
+	kEplApiEventBoot = 0x21,	// m_Boot
+	kEplApiEventSdo = 0x62,	// m_Sdo
+	kEplApiEventObdAccess = 0x69,	// m_ObdCbParam
+	kEplApiEventLed = 0x70,	// m_Led
+
+} tEplApiEventType;
+
+typedef union {
+	tEplEventNmtStateChange m_NmtStateChange;
+	tEplEventError m_InternalError;
+	tEplSdoComFinished m_Sdo;
+	tEplObdCbParam m_ObdCbParam;
+	tEplApiEventNode m_Node;
+	tEplApiEventBoot m_Boot;
+	tEplApiEventLed m_Led;
+
+} tEplApiEventArg;
+
+typedef tEplKernel(PUBLIC ROM * tEplApiCbEvent) (tEplApiEventType EventType_p,	// IN: event type (enum)
+						 tEplApiEventArg * pEventArg_p,	// IN: event argument (union)
+						 void GENERIC * pUserArg_p);
+
+typedef struct {
+	unsigned int m_uiSizeOfStruct;
+	BOOL m_fAsyncOnly;	// do not need to register PRes
+	unsigned int m_uiNodeId;	// local node ID
+	BYTE m_abMacAddress[6];	// local MAC address
+
+	// 0x1F82: NMT_FeatureFlags_U32
+	DWORD m_dwFeatureFlags;
+	// Cycle Length (0x1006: NMT_CycleLen_U32) in [us]
+	DWORD m_dwCycleLen;	// required for error detection
+	// 0x1F98: NMT_CycleTiming_REC
+	// 0x1F98.1: IsochrTxMaxPayload_U16
+	unsigned int m_uiIsochrTxMaxPayload;	// const
+	// 0x1F98.2: IsochrRxMaxPayload_U16
+	unsigned int m_uiIsochrRxMaxPayload;	// const
+	// 0x1F98.3: PResMaxLatency_U32
+	DWORD m_dwPresMaxLatency;	// const in [ns], only required for IdentRes
+	// 0x1F98.4: PReqActPayloadLimit_U16
+	unsigned int m_uiPreqActPayloadLimit;	// required for initialisation (+28 bytes)
+	// 0x1F98.5: PResActPayloadLimit_U16
+	unsigned int m_uiPresActPayloadLimit;	// required for initialisation of Pres frame (+28 bytes)
+	// 0x1F98.6: ASndMaxLatency_U32
+	DWORD m_dwAsndMaxLatency;	// const in [ns], only required for IdentRes
+	// 0x1F98.7: MultiplCycleCnt_U8
+	unsigned int m_uiMultiplCycleCnt;	// required for error detection
+	// 0x1F98.8: AsyncMTU_U16
+	unsigned int m_uiAsyncMtu;	// required to set up max frame size
+	// 0x1F98.9: Prescaler_U16
+	unsigned int m_uiPrescaler;	// required for sync
+	// $$$ Multiplexed Slot
+
+	// 0x1C14: DLL_LossOfFrameTolerance_U32 in [ns]
+	DWORD m_dwLossOfFrameTolerance;
+
+	// 0x1F8A: NMT_MNCycleTiming_REC
+	// 0x1F8A.1: WaitSoCPReq_U32 in [ns]
+	DWORD m_dwWaitSocPreq;
+
+	// 0x1F8A.2: AsyncSlotTimeout_U32 in [ns]
+	DWORD m_dwAsyncSlotTimeout;
+
+	DWORD m_dwDeviceType;	// NMT_DeviceType_U32
+	DWORD m_dwVendorId;	// NMT_IdentityObject_REC.VendorId_U32
+	DWORD m_dwProductCode;	// NMT_IdentityObject_REC.ProductCode_U32
+	DWORD m_dwRevisionNumber;	// NMT_IdentityObject_REC.RevisionNo_U32
+	DWORD m_dwSerialNumber;	// NMT_IdentityObject_REC.SerialNo_U32
+	QWORD m_qwVendorSpecificExt1;
+	DWORD m_dwVerifyConfigurationDate;	// CFM_VerifyConfiguration_REC.ConfDate_U32
+	DWORD m_dwVerifyConfigurationTime;	// CFM_VerifyConfiguration_REC.ConfTime_U32
+	DWORD m_dwApplicationSwDate;	// PDL_LocVerApplSw_REC.ApplSwDate_U32 on programmable device or date portion of NMT_ManufactSwVers_VS on non-programmable device
+	DWORD m_dwApplicationSwTime;	// PDL_LocVerApplSw_REC.ApplSwTime_U32 on programmable device or time portion of NMT_ManufactSwVers_VS on non-programmable device
+	DWORD m_dwIpAddress;
+	DWORD m_dwSubnetMask;
+	DWORD m_dwDefaultGateway;
+	BYTE m_sHostname[32];
+	BYTE m_abVendorSpecificExt2[48];
+
+	char *m_pszDevName;	// NMT_ManufactDevName_VS (0x1008/0 local OD)
+	char *m_pszHwVersion;	// NMT_ManufactHwVers_VS  (0x1009/0 local OD)
+	char *m_pszSwVersion;	// NMT_ManufactSwVers_VS  (0x100A/0 local OD)
+
+	tEplApiCbEvent m_pfnCbEvent;
+	void *m_pEventUserArg;
+	tEplSyncCb m_pfnCbSync;
+
+} tEplApiInitParam;
+
+typedef struct {
+	void *m_pImage;
+	unsigned int m_uiSize;
+
+} tEplApiProcessImage;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiInitialize(tEplApiInitParam * pInitParam_p);
+
+tEplKernel PUBLIC EplApiShutdown(void);
+
+tEplKernel PUBLIC EplApiReadObject(tEplSdoComConHdl * pSdoComConHdl_p,
+				   unsigned int uiNodeId_p,
+				   unsigned int uiIndex_p,
+				   unsigned int uiSubindex_p,
+				   void *pDstData_le_p,
+				   unsigned int *puiSize_p,
+				   tEplSdoType SdoType_p, void *pUserArg_p);
+
+tEplKernel PUBLIC EplApiWriteObject(tEplSdoComConHdl * pSdoComConHdl_p,
+				    unsigned int uiNodeId_p,
+				    unsigned int uiIndex_p,
+				    unsigned int uiSubindex_p,
+				    void *pSrcData_le_p,
+				    unsigned int uiSize_p,
+				    tEplSdoType SdoType_p, void *pUserArg_p);
+
+tEplKernel PUBLIC EplApiFreeSdoChannel(tEplSdoComConHdl SdoComConHdl_p);
+
+tEplKernel PUBLIC EplApiReadLocalObject(unsigned int uiIndex_p,
+					unsigned int uiSubindex_p,
+					void *pDstData_p,
+					unsigned int *puiSize_p);
+
+tEplKernel PUBLIC EplApiWriteLocalObject(unsigned int uiIndex_p,
+					 unsigned int uiSubindex_p,
+					 void *pSrcData_p,
+					 unsigned int uiSize_p);
+
+tEplKernel PUBLIC EplApiCbObdAccess(tEplObdCbParam MEM * pParam_p);
+
+tEplKernel PUBLIC EplApiLinkObject(unsigned int uiObjIndex_p,
+				   void *pVar_p,
+				   unsigned int *puiVarEntries_p,
+				   tEplObdSize * pEntrySize_p,
+				   unsigned int uiFirstSubindex_p);
+
+tEplKernel PUBLIC EplApiExecNmtCommand(tEplNmtEvent NmtEvent_p);
+
+tEplKernel PUBLIC EplApiProcess(void);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+tEplKernel PUBLIC EplApiMnTriggerStateChange(unsigned int uiNodeId_p,
+					     tEplNmtNodeCommand NodeCommand_p);
+#endif
+
+tEplKernel PUBLIC EplApiGetIdentResponse(unsigned int uiNodeId_p,
+					 tEplIdentResponse **
+					 ppIdentResponse_p);
+
+// functions for process image will be implemented in separate file
+tEplKernel PUBLIC EplApiProcessImageSetup(void);
+tEplKernel PUBLIC EplApiProcessImageExchangeIn(tEplApiProcessImage * pPI_p);
+tEplKernel PUBLIC EplApiProcessImageExchangeOut(tEplApiProcessImage * pPI_p);
+
+#endif // #ifndef _EPL_API_H_
diff --git a/drivers/staging/epl/EplAmi.h b/drivers/staging/epl/EplAmi.h
new file mode 100644
index 000000000000..6fa04a4f0a13
--- /dev/null
+++ b/drivers/staging/epl/EplAmi.h
@@ -0,0 +1,362 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  Definitions for Abstract Memory Interface
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplAmi.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.2 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                GCC
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+   06.03.2000  -rs
+               Implementation
+
+   16.09.2002  -as
+               To save code space the functions AmiSetByte and AmiGetByte
+               are replaced by macros. For targets which assign BYTE by
+               an 16Bit type, the definition of macros must changed to
+               functions.
+
+   23.02.2005  r.d.:
+               Functions included for extended data types such as UNSIGNED24,
+               UNSIGNED40, ...
+
+   13.06.2006  d.k.:
+               Extended the interface for EPL with the different functions
+               for little endian and big endian
+
+****************************************************************************/
+
+#ifndef _EPLAMI_H_
+#define _EPLAMI_H_
+
+#if ((DEV_SYSTEM & _DEV_64BIT_SUPPORT_) == 0)
+//    #ifdef USE_VAR64
+#error 'ERROR: development system does not support 64 bit operations!'
+//    #endif
+#endif
+
+//---------------------------------------------------------------------------
+//  types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Prototypen
+//---------------------------------------------------------------------------
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if (TARGET_SYSTEM == _WIN32_)
+#if defined(INLINE_FUNCTION_DEF)
+#undef  INLINE_FUNCTION
+#define INLINE_FUNCTION     INLINE_FUNCTION_DEF
+#define INLINE_ENABLED      TRUE
+#define EPL_AMI_INLINED
+#include "../EplStack/amix86.c"
+#endif
+
+#elif (TARGET_SYSTEM == _LINUX_)
+#if defined(__m68k__)		// it is an big endian machine
+#if defined(INLINE_FUNCTION_DEF)
+#undef  INLINE_FUNCTION
+#define INLINE_FUNCTION     INLINE_FUNCTION_DEF
+#define INLINE_ENABLED      TRUE
+#define EPL_AMI_INLINED
+#include "../EplStack/amibe.c"
+#endif
+#endif
+#endif
+
+//---------------------------------------------------------------------------
+//
+// write functions
+//
+// To save code space the function AmiSetByte is replaced by
+// an macro.
+// void   PUBLIC  AmiSetByte  (void FAR* pAddr_p, BYTE bByteVal_p);
+
+#define AmiSetByteToBe(pAddr_p, bByteVal_p)  {*(BYTE FAR*)(pAddr_p) = (bByteVal_p);}
+#define AmiSetByteToLe(pAddr_p, bByteVal_p)  {*(BYTE FAR*)(pAddr_p) = (bByteVal_p);}
+
+#if !defined(INLINE_ENABLED)
+	void PUBLIC AmiSetWordToBe(void FAR * pAddr_p, WORD wWordVal_p);
+	void PUBLIC AmiSetDwordToBe(void FAR * pAddr_p, DWORD dwDwordVal_p);
+	void PUBLIC AmiSetWordToLe(void FAR * pAddr_p, WORD wWordVal_p);
+	void PUBLIC AmiSetDwordToLe(void FAR * pAddr_p, DWORD dwDwordVal_p);
+#endif
+
+//---------------------------------------------------------------------------
+//
+// read functions
+//
+// To save code space the function AmiGetByte is replaced by
+// an macro.
+// BYTE   PUBLIC  AmiGetByte  (void FAR* pAddr_p);
+
+#define AmiGetByteFromBe(pAddr_p)  (*(BYTE FAR*)(pAddr_p))
+#define AmiGetByteFromLe(pAddr_p)  (*(BYTE FAR*)(pAddr_p))
+
+#if !defined(INLINE_ENABLED)
+
+	WORD PUBLIC AmiGetWordFromBe(void FAR * pAddr_p);
+	DWORD PUBLIC AmiGetDwordFromBe(void FAR * pAddr_p);
+	WORD PUBLIC AmiGetWordFromLe(void FAR * pAddr_p);
+	DWORD PUBLIC AmiGetDwordFromLe(void FAR * pAddr_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetDword24()
+//
+// Description: sets a 24 bit value to a buffer
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              dwDwordVal_p    = value to set
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	void PUBLIC AmiSetDword24ToBe(void FAR * pAddr_p, DWORD dwDwordVal_p);
+	void PUBLIC AmiSetDword24ToLe(void FAR * pAddr_p, DWORD dwDwordVal_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetDword24()
+//
+// Description: reads a 24 bit value from a buffer
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      DWORD           = read value
+//
+//---------------------------------------------------------------------------
+
+	DWORD PUBLIC AmiGetDword24FromBe(void FAR * pAddr_p);
+	DWORD PUBLIC AmiGetDword24FromLe(void FAR * pAddr_p);
+
+//#ifdef USE_VAR64
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword40()
+//
+// Description: sets a 40 bit value to a buffer
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	void PUBLIC AmiSetQword40ToBe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+	void PUBLIC AmiSetQword40ToLe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword40()
+//
+// Description: reads a 40 bit value from a buffer
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+//---------------------------------------------------------------------------
+
+	QWORD PUBLIC AmiGetQword40FromBe(void FAR * pAddr_p);
+	QWORD PUBLIC AmiGetQword40FromLe(void FAR * pAddr_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword48()
+//
+// Description: sets a 48 bit value to a buffer
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	void PUBLIC AmiSetQword48ToBe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+	void PUBLIC AmiSetQword48ToLe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword48()
+//
+// Description: reads a 48 bit value from a buffer
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+//---------------------------------------------------------------------------
+
+	QWORD PUBLIC AmiGetQword48FromBe(void FAR * pAddr_p);
+	QWORD PUBLIC AmiGetQword48FromLe(void FAR * pAddr_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword56()
+//
+// Description: sets a 56 bit value to a buffer
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	void PUBLIC AmiSetQword56ToBe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+	void PUBLIC AmiSetQword56ToLe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword56()
+//
+// Description: reads a 56 bit value from a buffer
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+//---------------------------------------------------------------------------
+
+	QWORD PUBLIC AmiGetQword56FromBe(void FAR * pAddr_p);
+	QWORD PUBLIC AmiGetQword56FromLe(void FAR * pAddr_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword64()
+//
+// Description: sets a 64 bit value to a buffer
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	void PUBLIC AmiSetQword64ToBe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+	void PUBLIC AmiSetQword64ToLe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword64()
+//
+// Description: reads a 64 bit value from a buffer
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	QWORD PUBLIC AmiGetQword64FromBe(void FAR * pAddr_p);
+	QWORD PUBLIC AmiGetQword64FromLe(void FAR * pAddr_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetTimeOfDay()
+//
+// Description: sets a TIME_OF_DAY (CANopen) value to a buffer
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              pTimeOfDay_p    = pointer to struct TIME_OF_DAY
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	void PUBLIC AmiSetTimeOfDay(void FAR * pAddr_p,
+				    tTimeOfDay FAR * pTimeOfDay_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetTimeOfDay()
+//
+// Description: reads a TIME_OF_DAY (CANopen) value from a buffer
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//              pTimeOfDay_p    = pointer to struct TIME_OF_DAY
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	void PUBLIC AmiGetTimeOfDay(void FAR * pAddr_p,
+				    tTimeOfDay FAR * pTimeOfDay_p);
+
+#endif
+
+#undef  INLINE_ENABLED		// disable actual inlining of functions
+#define EPL_AMI_INCLUDED
+
+#ifdef __cplusplus
+}
+#endif
+#endif				// ifndef _EPLAMI_H_
+// Die letzte Zeile muß unbedingt eine leere Zeile sein, weil manche Compiler// damit ein Problem haben, wenn das nicht so ist (z.B. GNU oder Borland C++ Builder).
diff --git a/drivers/staging/epl/EplApiGeneric.c b/drivers/staging/epl/EplApiGeneric.c
new file mode 100644
index 000000000000..ae19e34cd7b3
--- /dev/null
+++ b/drivers/staging/epl/EplApiGeneric.c
@@ -0,0 +1,2060 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for generic EPL API module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplApiGeneric.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.21 $  $Date: 2008/11/21 09:00:38 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/09/05 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "Epl.h"
+#include "kernel/EplDllk.h"
+#include "kernel/EplErrorHandlerk.h"
+#include "kernel/EplEventk.h"
+#include "kernel/EplNmtk.h"
+#include "kernel/EplObdk.h"
+#include "kernel/EplTimerk.h"
+#include "kernel/EplDllkCal.h"
+#include "kernel/EplPdokCal.h"
+#include "user/EplDlluCal.h"
+#include "user/EplLedu.h"
+#include "user/EplNmtCnu.h"
+#include "user/EplNmtMnu.h"
+#include "user/EplSdoComu.h"
+#include "user/EplIdentu.h"
+#include "user/EplStatusu.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+#include "kernel/EplPdok.h"
+#endif
+
+#include "SharedBuff.h"
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) == 0)
+#error "EPL API layer needs EPL module OBDK!"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplApi                                              */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	tEplApiInitParam m_InitParam;
+
+} tEplApiInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+static tEplApiInstance EplApiInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+// NMT state change event callback function
+static tEplKernel PUBLIC EplApiCbNmtStateChange(tEplEventNmtStateChange
+						NmtStateChange_p);
+
+// update DLL configuration from OD
+static tEplKernel PUBLIC EplApiUpdateDllConfig(BOOL fUpdateIdentity_p);
+
+// update OD from init param
+static tEplKernel PUBLIC EplApiUpdateObd(void);
+
+// process events from user event queue
+static tEplKernel PUBLIC EplApiProcessEvent(tEplEvent * pEplEvent_p);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+// callback function of SDO module
+static tEplKernel PUBLIC EplApiCbSdoCon(tEplSdoComFinished * pSdoComFinished_p);
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+// callback functions of NmtMnu module
+static tEplKernel PUBLIC EplApiCbNodeEvent(unsigned int uiNodeId_p,
+					   tEplNmtNodeEvent NodeEvent_p,
+					   tEplNmtState NmtState_p,
+					   WORD wErrorCode_p,
+					   BOOL fMandatory_p);
+
+static tEplKernel PUBLIC EplApiCbBootEvent(tEplNmtBootEvent BootEvent_p,
+					   tEplNmtState NmtState_p,
+					   WORD wErrorCode_p);
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+// callback function of Ledu module
+static tEplKernel PUBLIC EplApiCbLedStateChange(tEplLedType LedType_p,
+						BOOL fOn_p);
+#endif
+
+// OD initialization function (implemented in Objdict.c)
+tEplKernel PUBLIC EplObdInitRam(tEplObdInitParam MEM * pInitParam_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiInitialize()
+//
+// Description: add and initialize new instance of EPL stack.
+//              After return from this function the application must start
+//              the NMT state machine via
+//              EplApiExecNmtCommand(kEplNmtEventSwReset)
+//              and thereby the whole EPL stack :-)
+//
+// Parameters:  pInitParam_p            = initialisation parameters
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiInitialize(tEplApiInitParam * pInitParam_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplObdInitParam ObdInitParam;
+	tEplDllkInitParam DllkInitParam;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+#endif
+
+	// reset instance structure
+	EPL_MEMSET(&EplApiInstance_g, 0, sizeof(EplApiInstance_g));
+
+	EPL_MEMCPY(&EplApiInstance_g.m_InitParam, pInitParam_p,
+		   min(sizeof(tEplApiInitParam),
+		       pInitParam_p->m_uiSizeOfStruct));
+
+	// check event callback function pointer
+	if (EplApiInstance_g.m_InitParam.m_pfnCbEvent == NULL) {	// application must always have an event callback function
+		Ret = kEplApiInvalidParam;
+		goto Exit;
+	}
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	// init OD
+// FIXME
+//    Ret = EplObdInitRam(&ObdInitParam);
+//    if (Ret != kEplSuccessful)
+//    {
+//        goto Exit;
+//    }
+
+	// initialize EplObd module
+	Ret = EplObdInit(&ObdInitParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+#ifndef EPL_NO_FIFO
+	ShbError = ShbInit();
+	if (ShbError != kShbOk) {
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+#endif
+
+	// initialize EplEventk module
+	Ret = EplEventkInit(EplApiInstance_g.m_InitParam.m_pfnCbSync);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// initialize EplEventu module
+	Ret = EplEventuInit(EplApiProcessEvent);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// init EplTimerk module
+	Ret = EplTimerkInit();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// initialize EplNmtk module before DLL
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+	Ret = EplNmtkInit();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// initialize EplDllk module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+	EPL_MEMCPY(DllkInitParam.m_be_abSrcMac,
+		   EplApiInstance_g.m_InitParam.m_abMacAddress, 6);
+	Ret = EplDllkAddInstance(&DllkInitParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// initialize EplErrorHandlerk module
+	Ret = EplErrorHandlerkInit();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// initialize EplDllkCal module
+	Ret = EplDllkCalAddInstance();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// initialize EplDlluCal module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	Ret = EplDlluCalAddInstance();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// initialize EplPdok module
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+	Ret = EplPdokAddInstance();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret = EplPdokCalAddInstance();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// initialize EplNmtCnu module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_CN)) != 0)
+	Ret = EplNmtCnuAddInstance(EplApiInstance_g.m_InitParam.m_uiNodeId);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// initialize EplNmtu module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+	Ret = EplNmtuInit();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// register NMT event callback function
+	Ret = EplNmtuRegisterStateChangeCb(EplApiCbNmtStateChange);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	// initialize EplNmtMnu module
+	Ret = EplNmtMnuInit(EplApiCbNodeEvent, EplApiCbBootEvent);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// initialize EplIdentu module
+	Ret = EplIdentuInit();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// initialize EplStatusu module
+	Ret = EplStatusuInit();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// initialize EplLedu module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+	Ret = EplLeduInit(EplApiCbLedStateChange);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// init SDO module
+#if ((((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0) || \
+     (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0))
+	// init sdo command layer
+	Ret = EplSdoComInit();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// the application must start NMT state machine
+	// via EplApiExecNmtCommand(kEplNmtEventSwReset)
+	// and thereby the whole EPL stack
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiShutdown()
+//
+// Description: deletes an instance of EPL stack
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiShutdown(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// $$$ d.k.: check if NMT state is NMT_GS_OFF
+
+	// $$$ d.k.: maybe delete event queues at first, but this implies that
+	//           no other module must not use the event queues for communication
+	//           during shutdown.
+
+	// delete instance for all modules
+
+	// deinitialize EplSdoCom module
+#if ((((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0) || \
+     (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0))
+	Ret = EplSdoComDelInstance();
+//    PRINTF1("EplSdoComDelInstance():  0x%X\n", Ret);
+#endif
+
+	// deinitialize EplLedu module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+	Ret = EplLeduDelInstance();
+//    PRINTF1("EplLeduDelInstance():    0x%X\n", Ret);
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	// deinitialize EplNmtMnu module
+	Ret = EplNmtMnuDelInstance();
+//    PRINTF1("EplNmtMnuDelInstance():  0x%X\n", Ret);
+
+	// deinitialize EplIdentu module
+	Ret = EplIdentuDelInstance();
+//    PRINTF1("EplIdentuDelInstance():  0x%X\n", Ret);
+
+	// deinitialize EplStatusu module
+	Ret = EplStatusuDelInstance();
+//    PRINTF1("EplStatusuDelInstance():  0x%X\n", Ret);
+#endif
+
+	// deinitialize EplNmtCnu module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_CN)) != 0)
+	Ret = EplNmtCnuDelInstance();
+//    PRINTF1("EplNmtCnuDelInstance():  0x%X\n", Ret);
+#endif
+
+	// deinitialize EplNmtu module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+	Ret = EplNmtuDelInstance();
+//    PRINTF1("EplNmtuDelInstance():    0x%X\n", Ret);
+#endif
+
+	// deinitialize EplDlluCal module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	Ret = EplDlluCalDelInstance();
+//    PRINTF1("EplDlluCalDelInstance(): 0x%X\n", Ret);
+
+#endif
+
+	// deinitialize EplEventu module
+	Ret = EplEventuDelInstance();
+//    PRINTF1("EplEventuDelInstance():  0x%X\n", Ret);
+
+	// deinitialize EplNmtk module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+	Ret = EplNmtkDelInstance();
+//    PRINTF1("EplNmtkDelInstance():    0x%X\n", Ret);
+#endif
+
+	// deinitialize EplDllk module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+	Ret = EplDllkDelInstance();
+//    PRINTF1("EplDllkDelInstance():    0x%X\n", Ret);
+
+	// deinitialize EplDllkCal module
+	Ret = EplDllkCalDelInstance();
+//    PRINTF1("EplDllkCalDelInstance(): 0x%X\n", Ret);
+#endif
+
+	// deinitialize EplEventk module
+	Ret = EplEventkDelInstance();
+//    PRINTF1("EplEventkDelInstance():  0x%X\n", Ret);
+
+	// deinitialize EplTimerk module
+	Ret = EplTimerkDelInstance();
+//    PRINTF1("EplTimerkDelInstance():  0x%X\n", Ret);
+
+#ifndef EPL_NO_FIFO
+	ShbExit();
+#endif
+
+	return Ret;
+}
+
+//----------------------------------------------------------------------------
+// Function:    EplApiExecNmtCommand()
+//
+// Description: executes a NMT command, i.e. post the NMT command/event to the
+//              NMTk module. NMT commands which are not appropriate in the current
+//              NMT state are silently ignored. Please keep in mind that the
+//              NMT state may change until the NMT command is actually executed.
+//
+// Parameters:  NmtEvent_p              = NMT command/event
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiExecNmtCommand(tEplNmtEvent NmtEvent_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+	Ret = EplNmtuNmtEvent(NmtEvent_p);
+#endif
+
+	return Ret;
+}
+
+//----------------------------------------------------------------------------
+// Function:    EplApiLinkObject()
+//
+// Description: Function maps array of application variables onto specified object in OD
+//
+// Parameters:  uiObjIndex_p            = Function maps variables for this object index
+//              pVar_p                  = Pointer to data memory area for the specified object
+//              puiVarEntries_p         = IN: pointer to number of entries to map
+//                                        OUT: pointer to number of actually used entries
+//              pEntrySize_p            = IN: pointer to size of one entry;
+//                                            if size is zero, the actual size will be read from OD
+//                                        OUT: pointer to entire size of all entries mapped
+//              uiFirstSubindex_p       = This is the first subindex to be mapped.
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiLinkObject(unsigned int uiObjIndex_p,
+				   void *pVar_p,
+				   unsigned int *puiVarEntries_p,
+				   tEplObdSize * pEntrySize_p,
+				   unsigned int uiFirstSubindex_p)
+{
+	BYTE bVarEntries;
+	BYTE bIndexEntries;
+	BYTE MEM *pbData;
+	unsigned int uiSubindex;
+	tEplVarParam VarParam;
+	tEplObdSize EntrySize;
+	tEplObdSize UsedSize;
+
+	tEplKernel RetCode = kEplSuccessful;
+
+	if ((pVar_p == NULL)
+	    || (puiVarEntries_p == NULL)
+	    || (*puiVarEntries_p == 0)
+	    || (pEntrySize_p == NULL)) {
+		RetCode = kEplApiInvalidParam;
+		goto Exit;
+	}
+
+	pbData = (BYTE MEM *) pVar_p;
+	bVarEntries = (BYTE) * puiVarEntries_p;
+	UsedSize = 0;
+
+	// init VarParam structure with default values
+	VarParam.m_uiIndex = uiObjIndex_p;
+	VarParam.m_ValidFlag = kVarValidAll;
+
+	if (uiFirstSubindex_p != 0) {	// check if object exists by reading subindex 0x00,
+		// because user wants to link a variable to a subindex unequal 0x00
+		// read number of entries
+		EntrySize = (tEplObdSize) sizeof(bIndexEntries);
+		RetCode = EplObdReadEntry(uiObjIndex_p,
+					  0x00,
+					  (void GENERIC *)&bIndexEntries,
+					  &EntrySize);
+
+		if ((RetCode != kEplSuccessful) || (bIndexEntries == 0x00)) {
+			// Object doesn't exist or invalid entry number
+			RetCode = kEplObdIndexNotExist;
+			goto Exit;
+		}
+	} else {		// user wants to link a variable to subindex 0x00
+		// that's OK
+		bIndexEntries = 0;
+	}
+
+	// Correct number of entries if number read from OD is greater
+	// than the specified number.
+	// This is done, so that we do not set more entries than subindexes the
+	// object actually has.
+	if ((bIndexEntries > (bVarEntries + uiFirstSubindex_p - 1)) &&
+	    (bVarEntries != 0x00)) {
+		bIndexEntries = (BYTE) (bVarEntries + uiFirstSubindex_p - 1);
+	}
+	// map entries
+	for (uiSubindex = uiFirstSubindex_p; uiSubindex <= bIndexEntries;
+	     uiSubindex++) {
+		// if passed entry size is 0, then get size from OD
+		if (*pEntrySize_p == 0x00) {
+			// read entry size
+			EntrySize = EplObdGetDataSize(uiObjIndex_p, uiSubindex);
+
+			if (EntrySize == 0x00) {
+				// invalid entry size (maybe object doesn't exist or entry of type DOMAIN is empty)
+				RetCode = kEplObdSubindexNotExist;
+				break;
+			}
+		} else {	// use passed entry size
+			EntrySize = *pEntrySize_p;
+		}
+
+		VarParam.m_uiSubindex = uiSubindex;
+
+		// set pointer to user var
+		VarParam.m_Size = EntrySize;
+		VarParam.m_pData = pbData;
+
+		UsedSize += EntrySize;
+		pbData += EntrySize;
+
+		RetCode = EplObdDefineVar(&VarParam);
+		if (RetCode != kEplSuccessful) {
+			break;
+		}
+	}
+
+	// set number of mapped entries and entry size
+	*puiVarEntries_p = ((bIndexEntries - uiFirstSubindex_p) + 1);
+	*pEntrySize_p = UsedSize;
+
+      Exit:
+
+	return (RetCode);
+
+}
+
+// ----------------------------------------------------------------------------
+//
+// Function:    EplApiReadObject()
+//
+// Description: reads the specified entry from the OD of the specified node.
+//              If this node is a remote node, it performs a SDO transfer, which
+//              means this function returns kEplApiTaskDeferred and the application
+//              is informed via the event callback function when the task is completed.
+//
+// Parameters:  pSdoComConHdl_p         = INOUT: pointer to SDO connection handle (may be NULL in case of local OD access)
+//              uiNodeId_p              = IN: node ID (0 = itself)
+//              uiIndex_p               = IN: index of object in OD
+//              uiSubindex_p            = IN: sub-index of object in OD
+//              pDstData_le_p           = OUT: pointer to data in little endian
+//              puiSize_p               = INOUT: pointer to size of data
+//              SdoType_p               = IN: type of SDO transfer
+//              pUserArg_p              = IN: user-definable argument pointer,
+//                                            which will be passed to the event callback function
+//
+// Return:      tEplKernel              = error code
+//
+// ----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiReadObject(tEplSdoComConHdl * pSdoComConHdl_p,
+				   unsigned int uiNodeId_p,
+				   unsigned int uiIndex_p,
+				   unsigned int uiSubindex_p,
+				   void *pDstData_le_p,
+				   unsigned int *puiSize_p,
+				   tEplSdoType SdoType_p, void *pUserArg_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if ((uiIndex_p == 0) || (pDstData_le_p == NULL) || (puiSize_p == NULL)
+	    || (*puiSize_p == 0)) {
+		Ret = kEplApiInvalidParam;
+		goto Exit;
+	}
+
+	if (uiNodeId_p == 0 || uiNodeId_p == EplObdGetNodeId()) {	// local OD access can be performed
+		tEplObdSize ObdSize;
+
+		ObdSize = (tEplObdSize) * puiSize_p;
+		Ret =
+		    EplObdReadEntryToLe(uiIndex_p, uiSubindex_p, pDstData_le_p,
+					&ObdSize);
+		*puiSize_p = (unsigned int)ObdSize;
+	} else {		// perform SDO transfer
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+		tEplSdoComTransParamByIndex TransParamByIndex;
+//    tEplSdoComConHdl            SdoComConHdl;
+
+		// check if application provides space for handle
+		if (pSdoComConHdl_p == NULL) {
+			Ret = kEplApiInvalidParam;
+			goto Exit;
+//            pSdoComConHdl_p = &SdoComConHdl;
+		}
+		// init command layer connection
+		Ret = EplSdoComDefineCon(pSdoComConHdl_p, uiNodeId_p,	// target node id
+					 SdoType_p);	// SDO type
+		if ((Ret != kEplSuccessful) && (Ret != kEplSdoComHandleExists)) {
+			goto Exit;
+		}
+		TransParamByIndex.m_pData = pDstData_le_p;
+		TransParamByIndex.m_SdoAccessType = kEplSdoAccessTypeRead;
+		TransParamByIndex.m_SdoComConHdl = *pSdoComConHdl_p;
+		TransParamByIndex.m_uiDataSize = *puiSize_p;
+		TransParamByIndex.m_uiIndex = uiIndex_p;
+		TransParamByIndex.m_uiSubindex = uiSubindex_p;
+		TransParamByIndex.m_pfnSdoFinishedCb = EplApiCbSdoCon;
+		TransParamByIndex.m_pUserArg = pUserArg_p;
+
+		Ret = EplSdoComInitTransferByIndex(&TransParamByIndex);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		Ret = kEplApiTaskDeferred;
+
+#else
+		Ret = kEplApiInvalidParam;
+#endif
+	}
+
+      Exit:
+	return Ret;
+}
+
+// ----------------------------------------------------------------------------
+//
+// Function:    EplApiWriteObject()
+//
+// Description: writes the specified entry to the OD of the specified node.
+//              If this node is a remote node, it performs a SDO transfer, which
+//              means this function returns kEplApiTaskDeferred and the application
+//              is informed via the event callback function when the task is completed.
+//
+// Parameters:  pSdoComConHdl_p         = INOUT: pointer to SDO connection handle (may be NULL in case of local OD access)
+//              uiNodeId_p              = IN: node ID (0 = itself)
+//              uiIndex_p               = IN: index of object in OD
+//              uiSubindex_p            = IN: sub-index of object in OD
+//              pSrcData_le_p           = IN: pointer to data in little endian
+//              uiSize_p                = IN: size of data in bytes
+//              SdoType_p               = IN: type of SDO transfer
+//              pUserArg_p              = IN: user-definable argument pointer,
+//                                            which will be passed to the event callback function
+//
+// Return:      tEplKernel              = error code
+//
+// ----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiWriteObject(tEplSdoComConHdl * pSdoComConHdl_p,
+				    unsigned int uiNodeId_p,
+				    unsigned int uiIndex_p,
+				    unsigned int uiSubindex_p,
+				    void *pSrcData_le_p,
+				    unsigned int uiSize_p,
+				    tEplSdoType SdoType_p, void *pUserArg_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if ((uiIndex_p == 0) || (pSrcData_le_p == NULL) || (uiSize_p == 0)) {
+		Ret = kEplApiInvalidParam;
+		goto Exit;
+	}
+
+	if (uiNodeId_p == 0 || uiNodeId_p == EplObdGetNodeId()) {	// local OD access can be performed
+
+		Ret =
+		    EplObdWriteEntryFromLe(uiIndex_p, uiSubindex_p,
+					   pSrcData_le_p, uiSize_p);
+	} else {		// perform SDO transfer
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+		tEplSdoComTransParamByIndex TransParamByIndex;
+//    tEplSdoComConHdl            SdoComConHdl;
+
+		// check if application provides space for handle
+		if (pSdoComConHdl_p == NULL) {
+			Ret = kEplApiInvalidParam;
+			goto Exit;
+//            pSdoComConHdl_p = &SdoComConHdl;
+		}
+		// d.k.: How to recycle command layer connection?
+		//       Try to redefine it, which will return kEplSdoComHandleExists
+		//       and the existing command layer handle.
+		//       If the returned handle is busy, EplSdoComInitTransferByIndex()
+		//       will return with error.
+		// $$$ d.k.: Collisions may occur with Configuration Manager, if both the application and
+		//           Configuration Manager, are trying to communicate with the very same node.
+		//     possible solution: disallow communication by application if Configuration Manager is busy
+
+		// init command layer connection
+		Ret = EplSdoComDefineCon(pSdoComConHdl_p, uiNodeId_p,	// target node id
+					 SdoType_p);	// SDO type
+		if ((Ret != kEplSuccessful) && (Ret != kEplSdoComHandleExists)) {
+			goto Exit;
+		}
+		TransParamByIndex.m_pData = pSrcData_le_p;
+		TransParamByIndex.m_SdoAccessType = kEplSdoAccessTypeWrite;
+		TransParamByIndex.m_SdoComConHdl = *pSdoComConHdl_p;
+		TransParamByIndex.m_uiDataSize = uiSize_p;
+		TransParamByIndex.m_uiIndex = uiIndex_p;
+		TransParamByIndex.m_uiSubindex = uiSubindex_p;
+		TransParamByIndex.m_pfnSdoFinishedCb = EplApiCbSdoCon;
+		TransParamByIndex.m_pUserArg = pUserArg_p;
+
+		Ret = EplSdoComInitTransferByIndex(&TransParamByIndex);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		Ret = kEplApiTaskDeferred;
+
+#else
+		Ret = kEplApiInvalidParam;
+#endif
+	}
+
+      Exit:
+	return Ret;
+}
+
+// ----------------------------------------------------------------------------
+//
+// Function:    EplApiFreeSdoChannel()
+//
+// Description: frees the specified SDO channel.
+//              This function must be called after each call to EplApiReadObject()/EplApiWriteObject()
+//              which returns kEplApiTaskDeferred and the application
+//              is informed via the event callback function when the task is completed.
+//
+// Parameters:  SdoComConHdl_p          = IN: SDO connection handle
+//
+// Return:      tEplKernel              = error code
+//
+// ----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiFreeSdoChannel(tEplSdoComConHdl SdoComConHdl_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+
+	// init command layer connection
+	Ret = EplSdoComUndefineCon(SdoComConHdl_p);
+
+#else
+	Ret = kEplApiInvalidParam;
+#endif
+
+	return Ret;
+}
+
+// ----------------------------------------------------------------------------
+//
+// Function:    EplApiReadLocalObject()
+//
+// Description: reads the specified entry from the local OD.
+//
+// Parameters:  uiIndex_p               = IN: index of object in OD
+//              uiSubindex_p            = IN: sub-index of object in OD
+//              pDstData_p              = OUT: pointer to data in platform byte order
+//              puiSize_p               = INOUT: pointer to size of data
+//
+// Return:      tEplKernel              = error code
+//
+// ----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiReadLocalObject(unsigned int uiIndex_p,
+					unsigned int uiSubindex_p,
+					void *pDstData_p,
+					unsigned int *puiSize_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplObdSize ObdSize;
+
+	ObdSize = (tEplObdSize) * puiSize_p;
+	Ret = EplObdReadEntry(uiIndex_p, uiSubindex_p, pDstData_p, &ObdSize);
+	*puiSize_p = (unsigned int)ObdSize;
+
+	return Ret;
+}
+
+// ----------------------------------------------------------------------------
+//
+// Function:    EplApiWriteLocalObject()
+//
+// Description: writes the specified entry to the local OD.
+//
+// Parameters:  uiIndex_p               = IN: index of object in OD
+//              uiSubindex_p            = IN: sub-index of object in OD
+//              pSrcData_p              = IN: pointer to data in platform byte order
+//              uiSize_p                = IN: size of data in bytes
+//
+// Return:      tEplKernel              = error code
+//
+// ----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiWriteLocalObject(unsigned int uiIndex_p,
+					 unsigned int uiSubindex_p,
+					 void *pSrcData_p,
+					 unsigned int uiSize_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	Ret =
+	    EplObdWriteEntry(uiIndex_p, uiSubindex_p, pSrcData_p,
+			     (tEplObdSize) uiSize_p);
+
+	return Ret;
+}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+// ----------------------------------------------------------------------------
+//
+// Function:    EplApiMnTriggerStateChange()
+//
+// Description: triggers the specified node command for the specified node.
+//
+// Parameters:  uiNodeId_p              = node ID for which the node command will be executed
+//              NodeCommand_p           = node command
+//
+// Return:      tEplKernel              = error code
+//
+// ----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiMnTriggerStateChange(unsigned int uiNodeId_p,
+					     tEplNmtNodeCommand NodeCommand_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	Ret = EplNmtMnuTriggerStateChange(uiNodeId_p, NodeCommand_p);
+
+	return Ret;
+}
+
+#endif // (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiCbObdAccess
+//
+// Description: callback function for OD accesses
+//
+// Parameters:  pParam_p                = OBD parameter
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiCbObdAccess(tEplObdCbParam MEM * pParam_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+#if (EPL_API_OBD_FORWARD_EVENT != FALSE)
+	tEplApiEventArg EventArg;
+
+	// call user callback
+	// must be disabled for EplApiLinuxKernel.c, because of reentrancy problem
+	// for local OD access. This is not so bad as user callback function in
+	// application does not use OD callbacks at the moment.
+	EventArg.m_ObdCbParam = *pParam_p;
+	Ret = EplApiInstance_g.m_InitParam.m_pfnCbEvent(kEplApiEventObdAccess,
+							&EventArg,
+							EplApiInstance_g.
+							m_InitParam.
+							m_pEventUserArg);
+#endif
+
+	switch (pParam_p->m_uiIndex) {
+		//case 0x1006:    // NMT_CycleLen_U32 (valid on reset)
+	case 0x1C14:		// DLL_LossOfFrameTolerance_U32
+		//case 0x1F98:    // NMT_CycleTiming_REC (valid on reset)
+		{
+			if (pParam_p->m_ObdEvent == kEplObdEvPostWrite) {
+				// update DLL configuration
+				Ret = EplApiUpdateDllConfig(FALSE);
+			}
+			break;
+		}
+
+	case 0x1020:		// CFM_VerifyConfiguration_REC.ConfId_U32 != 0
+		{
+			if ((pParam_p->m_ObdEvent == kEplObdEvPostWrite)
+			    && (pParam_p->m_uiSubIndex == 3)
+			    && (*((DWORD *) pParam_p->m_pArg) != 0)) {
+				DWORD dwVerifyConfInvalid = 0;
+				// set CFM_VerifyConfiguration_REC.VerifyConfInvalid_U32 to 0
+				Ret =
+				    EplObdWriteEntry(0x1020, 4,
+						     &dwVerifyConfInvalid, 4);
+				// ignore any error because this objekt is optional
+				Ret = kEplSuccessful;
+			}
+			break;
+		}
+
+	case 0x1F9E:		// NMT_ResetCmd_U8
+		{
+			if (pParam_p->m_ObdEvent == kEplObdEvPreWrite) {
+				BYTE bNmtCommand;
+
+				bNmtCommand = *((BYTE *) pParam_p->m_pArg);
+				// check value range
+				switch ((tEplNmtCommand) bNmtCommand) {
+				case kEplNmtCmdResetNode:
+				case kEplNmtCmdResetCommunication:
+				case kEplNmtCmdResetConfiguration:
+				case kEplNmtCmdSwReset:
+				case kEplNmtCmdInvalidService:
+					// valid command identifier specified
+					break;
+
+				default:
+					pParam_p->m_dwAbortCode =
+					    EPL_SDOAC_VALUE_RANGE_EXCEEDED;
+					Ret = kEplObdAccessViolation;
+					break;
+				}
+			} else if (pParam_p->m_ObdEvent == kEplObdEvPostWrite) {
+				BYTE bNmtCommand;
+
+				bNmtCommand = *((BYTE *) pParam_p->m_pArg);
+				// check value range
+				switch ((tEplNmtCommand) bNmtCommand) {
+				case kEplNmtCmdResetNode:
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventResetNode);
+#endif
+					break;
+
+				case kEplNmtCmdResetCommunication:
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventResetCom);
+#endif
+					break;
+
+				case kEplNmtCmdResetConfiguration:
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventResetConfig);
+#endif
+					break;
+
+				case kEplNmtCmdSwReset:
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventSwReset);
+#endif
+					break;
+
+				case kEplNmtCmdInvalidService:
+					break;
+
+				default:
+					pParam_p->m_dwAbortCode =
+					    EPL_SDOAC_VALUE_RANGE_EXCEEDED;
+					Ret = kEplObdAccessViolation;
+					break;
+				}
+			}
+			break;
+		}
+
+	default:
+		break;
+	}
+
+//Exit:
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiProcessEvent
+//
+// Description: processes events from event queue and forwards these to
+//              the application's event callback function
+//
+// Parameters:  pEplEvent_p =   pointer to event
+//
+// Returns:     tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplApiProcessEvent(tEplEvent * pEplEvent_p)
+{
+	tEplKernel Ret;
+	tEplEventError *pEventError;
+	tEplApiEventType EventType;
+
+	Ret = kEplSuccessful;
+
+	// process event
+	switch (pEplEvent_p->m_EventType) {
+		// error event
+	case kEplEventTypeError:
+		{
+			pEventError = (tEplEventError *) pEplEvent_p->m_pArg;
+			switch (pEventError->m_EventSource) {
+				// treat the errors from the following sources as critical
+			case kEplEventSourceEventk:
+			case kEplEventSourceEventu:
+			case kEplEventSourceDllk:
+				{
+					EventType = kEplApiEventCriticalError;
+					// halt the stack by entering NMT state Off
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventCriticalError);
+					break;
+				}
+
+				// the other errors are just warnings
+			default:
+				{
+					EventType = kEplApiEventWarning;
+					break;
+				}
+			}
+
+			// call user callback
+			Ret =
+			    EplApiInstance_g.m_InitParam.m_pfnCbEvent(EventType,
+								      (tEplApiEventArg
+								       *)
+								      pEventError,
+								      EplApiInstance_g.
+								      m_InitParam.
+								      m_pEventUserArg);
+			// discard error from callback function, because this could generate an endless loop
+			Ret = kEplSuccessful;
+			break;
+		}
+
+		// at present, there are no other events for this module
+	default:
+		break;
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiCbNmtStateChange
+//
+// Description: callback function for NMT state changes
+//
+// Parameters:  NmtStateChange_p        = NMT state change event
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplApiCbNmtStateChange(tEplEventNmtStateChange
+						NmtStateChange_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	BYTE bNmtState;
+	tEplApiEventArg EventArg;
+
+	// save NMT state in OD
+	bNmtState = (BYTE) NmtStateChange_p.m_NewNmtState;
+	Ret = EplObdWriteEntry(0x1F8C, 0, &bNmtState, 1);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// do work which must be done in that state
+	switch (NmtStateChange_p.m_NewNmtState) {
+		// EPL stack is not running
+	case kEplNmtGsOff:
+		break;
+
+		// first init of the hardware
+	case kEplNmtGsInitialising:
+#if 0
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+		// configure SDO via UDP (i.e. bind it to the EPL ethernet interface)
+		Ret =
+		    EplSdoUdpuConfig(EplApiInstance_g.m_InitParam.m_dwIpAddress,
+				     EPL_C_SDO_EPL_PORT);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+#endif
+#endif
+
+		break;
+
+		// init of the manufacturer-specific profile area and the
+		// standardised device profile area
+	case kEplNmtGsResetApplication:
+		{
+			// reset application part of OD
+			Ret = EplObdAccessOdPart(kEplObdPartApp,
+						 kEplObdDirLoad);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+
+			break;
+		}
+
+		// init of the communication profile area
+	case kEplNmtGsResetCommunication:
+		{
+			// reset communication part of OD
+			Ret = EplObdAccessOdPart(kEplObdPartGen,
+						 kEplObdDirLoad);
+
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+			// $$$ d.k.: update OD only if OD was not loaded from non-volatile memory
+			Ret = EplApiUpdateObd();
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+
+			break;
+		}
+
+		// build the configuration with infos from OD
+	case kEplNmtGsResetConfiguration:
+		{
+
+			Ret = EplApiUpdateDllConfig(TRUE);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+
+			break;
+		}
+
+		//-----------------------------------------------------------
+		// CN part of the state machine
+
+		// node liste for EPL-Frames and check timeout
+	case kEplNmtCsNotActive:
+		{
+			// indicate completion of reset in NMT_ResetCmd_U8
+			bNmtState = (BYTE) kEplNmtCmdInvalidService;
+			Ret = EplObdWriteEntry(0x1F9E, 0, &bNmtState, 1);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+
+			break;
+		}
+
+		// node process only async frames
+	case kEplNmtCsPreOperational1:
+		{
+			break;
+		}
+
+		// node process isochronus and asynchronus frames
+	case kEplNmtCsPreOperational2:
+		{
+			break;
+		}
+
+		// node should be configured und application is ready
+	case kEplNmtCsReadyToOperate:
+		{
+			break;
+		}
+
+		// normal work state
+	case kEplNmtCsOperational:
+		{
+			break;
+		}
+
+		// node stopped by MN
+		// -> only process asynchronus frames
+	case kEplNmtCsStopped:
+		{
+			break;
+		}
+
+		// no EPL cycle
+		// -> normal ethernet communication
+	case kEplNmtCsBasicEthernet:
+		{
+			break;
+		}
+
+		//-----------------------------------------------------------
+		// MN part of the state machine
+
+		// node listens for EPL-Frames and check timeout
+	case kEplNmtMsNotActive:
+		{
+			break;
+		}
+
+		// node processes only async frames
+	case kEplNmtMsPreOperational1:
+		{
+			break;
+		}
+
+		// node processes isochronous and asynchronous frames
+	case kEplNmtMsPreOperational2:
+		{
+			break;
+		}
+
+		// node should be configured und application is ready
+	case kEplNmtMsReadyToOperate:
+		{
+			break;
+		}
+
+		// normal work state
+	case kEplNmtMsOperational:
+		{
+			break;
+		}
+
+		// no EPL cycle
+		// -> normal ethernet communication
+	case kEplNmtMsBasicEthernet:
+		{
+			break;
+		}
+
+	default:
+		{
+			TRACE0
+			    ("EplApiCbNmtStateChange(): unhandled NMT state\n");
+		}
+	}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+	// forward event to Led module
+	Ret = EplLeduCbNmtStateChange(NmtStateChange_p);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	// forward event to NmtMn module
+	Ret = EplNmtMnuCbNmtStateChange(NmtStateChange_p);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// call user callback
+	EventArg.m_NmtStateChange = NmtStateChange_p;
+	Ret =
+	    EplApiInstance_g.m_InitParam.
+	    m_pfnCbEvent(kEplApiEventNmtStateChange, &EventArg,
+			 EplApiInstance_g.m_InitParam.m_pEventUserArg);
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiUpdateDllConfig
+//
+// Description: update configuration of DLL
+//
+// Parameters:  fUpdateIdentity_p       = TRUE, if identity must be updated
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplApiUpdateDllConfig(BOOL fUpdateIdentity_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplDllConfigParam DllConfigParam;
+	tEplDllIdentParam DllIdentParam;
+	tEplObdSize ObdSize;
+	WORD wTemp;
+	BYTE bTemp;
+
+	// configure Dll
+	EPL_MEMSET(&DllConfigParam, 0, sizeof(DllConfigParam));
+	DllConfigParam.m_uiNodeId = EplObdGetNodeId();
+
+	// Cycle Length (0x1006: NMT_CycleLen_U32) in [us]
+	ObdSize = 4;
+	Ret =
+	    EplObdReadEntry(0x1006, 0, &DllConfigParam.m_dwCycleLen, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// 0x1F82: NMT_FeatureFlags_U32
+	ObdSize = 4;
+	Ret =
+	    EplObdReadEntry(0x1F82, 0, &DllConfigParam.m_dwFeatureFlags,
+			    &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// d.k. There is no dependance between FeatureFlags and async-only CN
+	DllConfigParam.m_fAsyncOnly = EplApiInstance_g.m_InitParam.m_fAsyncOnly;
+
+	// 0x1C14: DLL_LossOfFrameTolerance_U32 in [ns]
+	ObdSize = 4;
+	Ret =
+	    EplObdReadEntry(0x1C14, 0, &DllConfigParam.m_dwLossOfFrameTolerance,
+			    &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// 0x1F98: NMT_CycleTiming_REC
+	// 0x1F98.1: IsochrTxMaxPayload_U16
+	ObdSize = 2;
+	Ret = EplObdReadEntry(0x1F98, 1, &wTemp, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	DllConfigParam.m_uiIsochrTxMaxPayload = wTemp;
+
+	// 0x1F98.2: IsochrRxMaxPayload_U16
+	ObdSize = 2;
+	Ret = EplObdReadEntry(0x1F98, 2, &wTemp, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	DllConfigParam.m_uiIsochrRxMaxPayload = wTemp;
+
+	// 0x1F98.3: PResMaxLatency_U32
+	ObdSize = 4;
+	Ret =
+	    EplObdReadEntry(0x1F98, 3, &DllConfigParam.m_dwPresMaxLatency,
+			    &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// 0x1F98.4: PReqActPayloadLimit_U16
+	ObdSize = 2;
+	Ret = EplObdReadEntry(0x1F98, 4, &wTemp, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	DllConfigParam.m_uiPreqActPayloadLimit = wTemp;
+
+	// 0x1F98.5: PResActPayloadLimit_U16
+	ObdSize = 2;
+	Ret = EplObdReadEntry(0x1F98, 5, &wTemp, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	DllConfigParam.m_uiPresActPayloadLimit = wTemp;
+
+	// 0x1F98.6: ASndMaxLatency_U32
+	ObdSize = 4;
+	Ret =
+	    EplObdReadEntry(0x1F98, 6, &DllConfigParam.m_dwAsndMaxLatency,
+			    &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// 0x1F98.7: MultiplCycleCnt_U8
+	ObdSize = 1;
+	Ret = EplObdReadEntry(0x1F98, 7, &bTemp, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	DllConfigParam.m_uiMultiplCycleCnt = bTemp;
+
+	// 0x1F98.8: AsyncMTU_U16
+	ObdSize = 2;
+	Ret = EplObdReadEntry(0x1F98, 8, &wTemp, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	DllConfigParam.m_uiAsyncMtu = wTemp;
+
+	// $$$ Prescaler
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	// 0x1F8A.1: WaitSoCPReq_U32 in [ns]
+	ObdSize = 4;
+	Ret =
+	    EplObdReadEntry(0x1F8A, 1, &DllConfigParam.m_dwWaitSocPreq,
+			    &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// 0x1F8A.2: AsyncSlotTimeout_U32 in [ns] (optional)
+	ObdSize = 4;
+	Ret =
+	    EplObdReadEntry(0x1F8A, 2, &DllConfigParam.m_dwAsyncSlotTimeout,
+			    &ObdSize);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+#endif
+
+	DllConfigParam.m_uiSizeOfStruct = sizeof(DllConfigParam);
+	Ret = EplDllkConfig(&DllConfigParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	if (fUpdateIdentity_p != FALSE) {
+		// configure Identity
+		EPL_MEMSET(&DllIdentParam, 0, sizeof(DllIdentParam));
+		ObdSize = 4;
+		Ret =
+		    EplObdReadEntry(0x1000, 0, &DllIdentParam.m_dwDeviceType,
+				    &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		ObdSize = 4;
+		Ret =
+		    EplObdReadEntry(0x1018, 1, &DllIdentParam.m_dwVendorId,
+				    &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		ObdSize = 4;
+		Ret =
+		    EplObdReadEntry(0x1018, 2, &DllIdentParam.m_dwProductCode,
+				    &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		ObdSize = 4;
+		Ret =
+		    EplObdReadEntry(0x1018, 3,
+				    &DllIdentParam.m_dwRevisionNumber,
+				    &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		ObdSize = 4;
+		Ret =
+		    EplObdReadEntry(0x1018, 4, &DllIdentParam.m_dwSerialNumber,
+				    &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		DllIdentParam.m_dwIpAddress =
+		    EplApiInstance_g.m_InitParam.m_dwIpAddress;
+		DllIdentParam.m_dwSubnetMask =
+		    EplApiInstance_g.m_InitParam.m_dwSubnetMask;
+		EPL_MEMCPY(DllIdentParam.m_sHostname,
+			   EplApiInstance_g.m_InitParam.m_sHostname,
+			   sizeof(DllIdentParam.m_sHostname));
+
+		ObdSize = 4;
+		Ret =
+		    EplObdReadEntry(0x1020, 1,
+				    &DllIdentParam.m_dwVerifyConfigurationDate,
+				    &ObdSize);
+		// ignore any error, because this object is optional
+
+		ObdSize = 4;
+		Ret =
+		    EplObdReadEntry(0x1020, 2,
+				    &DllIdentParam.m_dwVerifyConfigurationTime,
+				    &ObdSize);
+		// ignore any error, because this object is optional
+
+		// $$$ d.k.: fill rest of ident structure
+
+		DllIdentParam.m_uiSizeOfStruct = sizeof(DllIdentParam);
+		Ret = EplDllkSetIdentity(&DllIdentParam);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiUpdateObd
+//
+// Description: update OD from init param
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplApiUpdateObd(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+	WORD wTemp;
+	BYTE bTemp;
+
+	// set node id in OD
+	Ret = EplObdSetNodeId(EplApiInstance_g.m_InitParam.m_uiNodeId,	// node id
+			      kEplObdNodeIdHardware);	// set by hardware
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_dwCycleLen != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1006, 0,
+				     &EplApiInstance_g.m_InitParam.m_dwCycleLen,
+				     4);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_dwLossOfFrameTolerance != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1C14, 0,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwLossOfFrameTolerance, 4);
+		/*        if(Ret != kEplSuccessful)
+		   {
+		   goto Exit;
+		   } */
+	}
+	// d.k. There is no dependance between FeatureFlags and async-only CN.
+	if (EplApiInstance_g.m_InitParam.m_dwFeatureFlags != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1F82, 0,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwFeatureFlags, 4);
+		/*    if(Ret != kEplSuccessful)
+		   {
+		   goto Exit;
+		   } */
+	}
+
+	wTemp = (WORD) EplApiInstance_g.m_InitParam.m_uiIsochrTxMaxPayload;
+	Ret = EplObdWriteEntry(0x1F98, 1, &wTemp, 2);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+
+	wTemp = (WORD) EplApiInstance_g.m_InitParam.m_uiIsochrRxMaxPayload;
+	Ret = EplObdWriteEntry(0x1F98, 2, &wTemp, 2);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+
+	Ret =
+	    EplObdWriteEntry(0x1F98, 3,
+			     &EplApiInstance_g.m_InitParam.m_dwPresMaxLatency,
+			     4);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+
+	if (EplApiInstance_g.m_InitParam.m_uiPreqActPayloadLimit <=
+	    EPL_C_DLL_ISOCHR_MAX_PAYL) {
+		wTemp =
+		    (WORD) EplApiInstance_g.m_InitParam.m_uiPreqActPayloadLimit;
+		Ret = EplObdWriteEntry(0x1F98, 4, &wTemp, 2);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_uiPresActPayloadLimit <=
+	    EPL_C_DLL_ISOCHR_MAX_PAYL) {
+		wTemp =
+		    (WORD) EplApiInstance_g.m_InitParam.m_uiPresActPayloadLimit;
+		Ret = EplObdWriteEntry(0x1F98, 5, &wTemp, 2);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+	}
+
+	Ret =
+	    EplObdWriteEntry(0x1F98, 6,
+			     &EplApiInstance_g.m_InitParam.m_dwAsndMaxLatency,
+			     4);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+
+	if (EplApiInstance_g.m_InitParam.m_uiMultiplCycleCnt <= 0xFF) {
+		bTemp = (BYTE) EplApiInstance_g.m_InitParam.m_uiMultiplCycleCnt;
+		Ret = EplObdWriteEntry(0x1F98, 7, &bTemp, 1);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_uiAsyncMtu <=
+	    EPL_C_DLL_MAX_ASYNC_MTU) {
+		wTemp = (WORD) EplApiInstance_g.m_InitParam.m_uiAsyncMtu;
+		Ret = EplObdWriteEntry(0x1F98, 8, &wTemp, 2);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_uiPrescaler <= 1000) {
+		wTemp = (WORD) EplApiInstance_g.m_InitParam.m_uiPrescaler;
+		Ret = EplObdWriteEntry(0x1F98, 9, &wTemp, 2);
+		// ignore return code
+		Ret = kEplSuccessful;
+	}
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	if (EplApiInstance_g.m_InitParam.m_dwWaitSocPreq != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1F8A, 1,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwWaitSocPreq, 4);
+		/*        if(Ret != kEplSuccessful)
+		   {
+		   goto Exit;
+		   } */
+	}
+
+	if ((EplApiInstance_g.m_InitParam.m_dwAsyncSlotTimeout != 0)
+	    && (EplApiInstance_g.m_InitParam.m_dwAsyncSlotTimeout != -1)) {
+		Ret =
+		    EplObdWriteEntry(0x1F8A, 2,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwAsyncSlotTimeout, 4);
+		/*        if(Ret != kEplSuccessful)
+		   {
+		   goto Exit;
+		   } */
+	}
+#endif
+
+	// configure Identity
+	if (EplApiInstance_g.m_InitParam.m_dwDeviceType != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1000, 0,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwDeviceType, 4);
+/*        if(Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_dwVendorId != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1018, 1,
+				     &EplApiInstance_g.m_InitParam.m_dwVendorId,
+				     4);
+/*        if(Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_dwProductCode != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1018, 2,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwProductCode, 4);
+/*        if(Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_dwRevisionNumber != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1018, 3,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwRevisionNumber, 4);
+/*        if(Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_dwSerialNumber != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1018, 4,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwSerialNumber, 4);
+/*        if(Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_pszDevName != NULL) {
+		// write Device Name (0x1008)
+		Ret =
+		    EplObdWriteEntry(0x1008, 0,
+				     (void GENERIC *)EplApiInstance_g.
+				     m_InitParam.m_pszDevName,
+				     (tEplObdSize) strlen(EplApiInstance_g.
+							  m_InitParam.
+							  m_pszDevName));
+/*        if (Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_pszHwVersion != NULL) {
+		// write Hardware version (0x1009)
+		Ret =
+		    EplObdWriteEntry(0x1009, 0,
+				     (void GENERIC *)EplApiInstance_g.
+				     m_InitParam.m_pszHwVersion,
+				     (tEplObdSize) strlen(EplApiInstance_g.
+							  m_InitParam.
+							  m_pszHwVersion));
+/*        if (Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_pszSwVersion != NULL) {
+		// write Software version (0x100A)
+		Ret =
+		    EplObdWriteEntry(0x100A, 0,
+				     (void GENERIC *)EplApiInstance_g.
+				     m_InitParam.m_pszSwVersion,
+				     (tEplObdSize) strlen(EplApiInstance_g.
+							  m_InitParam.
+							  m_pszSwVersion));
+/*        if (Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiCbSdoCon
+//
+// Description: callback function for SDO transfers
+//
+// Parameters:  pSdoComFinished_p       = SDO parameter
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+static tEplKernel PUBLIC EplApiCbSdoCon(tEplSdoComFinished * pSdoComFinished_p)
+{
+	tEplKernel Ret;
+	tEplApiEventArg EventArg;
+
+	Ret = kEplSuccessful;
+
+	// call user callback
+	EventArg.m_Sdo = *pSdoComFinished_p;
+	Ret = EplApiInstance_g.m_InitParam.m_pfnCbEvent(kEplApiEventSdo,
+							&EventArg,
+							EplApiInstance_g.
+							m_InitParam.
+							m_pEventUserArg);
+
+	return Ret;
+
+}
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiCbNodeEvent
+//
+// Description: callback function for node events
+//
+// Parameters:  uiNodeId_p              = node ID of the CN
+//              NodeEvent_p             = event from the specified CN
+//              NmtState_p              = current NMT state of the CN
+//              wErrorCode_p            = EPL error code if NodeEvent_p==kEplNmtNodeEventError
+//              fMandatory_p            = flag if CN is mandatory
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplApiCbNodeEvent(unsigned int uiNodeId_p,
+					   tEplNmtNodeEvent NodeEvent_p,
+					   tEplNmtState NmtState_p,
+					   WORD wErrorCode_p, BOOL fMandatory_p)
+{
+	tEplKernel Ret;
+	tEplApiEventArg EventArg;
+
+	Ret = kEplSuccessful;
+
+	// call user callback
+	EventArg.m_Node.m_uiNodeId = uiNodeId_p;
+	EventArg.m_Node.m_NodeEvent = NodeEvent_p;
+	EventArg.m_Node.m_NmtState = NmtState_p;
+	EventArg.m_Node.m_wErrorCode = wErrorCode_p;
+	EventArg.m_Node.m_fMandatory = fMandatory_p;
+
+	Ret = EplApiInstance_g.m_InitParam.m_pfnCbEvent(kEplApiEventNode,
+							&EventArg,
+							EplApiInstance_g.
+							m_InitParam.
+							m_pEventUserArg);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiCbBootEvent
+//
+// Description: callback function for boot events
+//
+// Parameters:  BootEvent_p             = event from the boot-up process
+//              NmtState_p              = current local NMT state
+//              wErrorCode_p            = EPL error code if BootEvent_p==kEplNmtBootEventError
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplApiCbBootEvent(tEplNmtBootEvent BootEvent_p,
+					   tEplNmtState NmtState_p,
+					   WORD wErrorCode_p)
+{
+	tEplKernel Ret;
+	tEplApiEventArg EventArg;
+
+	Ret = kEplSuccessful;
+
+	// call user callback
+	EventArg.m_Boot.m_BootEvent = BootEvent_p;
+	EventArg.m_Boot.m_NmtState = NmtState_p;
+	EventArg.m_Boot.m_wErrorCode = wErrorCode_p;
+
+	Ret = EplApiInstance_g.m_InitParam.m_pfnCbEvent(kEplApiEventBoot,
+							&EventArg,
+							EplApiInstance_g.
+							m_InitParam.
+							m_pEventUserArg);
+
+	return Ret;
+
+}
+
+#endif // (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiCbLedStateChange
+//
+// Description: callback function for LED change events.
+//
+// Parameters:  LedType_p       = type of LED
+//              fOn_p           = state of LED
+//
+// Returns:     tEplKernel      = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplApiCbLedStateChange(tEplLedType LedType_p,
+						BOOL fOn_p)
+{
+	tEplKernel Ret;
+	tEplApiEventArg EventArg;
+
+	Ret = kEplSuccessful;
+
+	// call user callback
+	EventArg.m_Led.m_LedType = LedType_p;
+	EventArg.m_Led.m_fOn = fOn_p;
+
+	Ret = EplApiInstance_g.m_InitParam.m_pfnCbEvent(kEplApiEventLed,
+							&EventArg,
+							EplApiInstance_g.
+							m_InitParam.
+							m_pEventUserArg);
+
+	return Ret;
+
+}
+
+#endif
+
+// EOF
diff --git a/drivers/staging/epl/EplApiLinux.h b/drivers/staging/epl/EplApiLinux.h
new file mode 100644
index 000000000000..92cd12532a62
--- /dev/null
+++ b/drivers/staging/epl/EplApiLinux.h
@@ -0,0 +1,141 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for EPL API layer for Linux (kernel and user space)
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplApiLinux.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/08/25 12:17:41 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/10/11 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_API_LINUX_H_
+#define _EPL_API_LINUX_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define EPLLIN_DEV_NAME     "epl"	// used for "/dev" and "/proc" entry
+
+//---------------------------------------------------------------------------
+//  Commands for <ioctl>
+//---------------------------------------------------------------------------
+
+#define EPLLIN_CMD_INITIALIZE               0	// ulArg_p ~ tEplApiInitParam*
+#define EPLLIN_CMD_PI_IN                    1	// ulArg_p ~ tEplApiProcessImage*
+#define EPLLIN_CMD_PI_OUT                   2	// ulArg_p ~ tEplApiProcessImage*
+#define EPLLIN_CMD_WRITE_OBJECT             3	// ulArg_p ~ tEplLinSdoObject*
+#define EPLLIN_CMD_READ_OBJECT              4	// ulArg_p ~ tEplLinSdoObject*
+#define EPLLIN_CMD_WRITE_LOCAL_OBJECT       5	// ulArg_p ~ tEplLinLocalObject*
+#define EPLLIN_CMD_READ_LOCAL_OBJECT        6	// ulArg_p ~ tEplLinLocalObject*
+#define EPLLIN_CMD_FREE_SDO_CHANNEL         7	// ulArg_p ~ tEplSdoComConHdl
+#define EPLLIN_CMD_NMT_COMMAND              8	// ulArg_p ~ tEplNmtEvent
+#define EPLLIN_CMD_GET_EVENT                9	// ulArg_p ~ tEplLinEvent*
+#define EPLLIN_CMD_MN_TRIGGER_STATE_CHANGE 10	// ulArg_p ~ tEplLinNodeCmdObject*
+#define EPLLIN_CMD_PI_SETUP                11	// ulArg_p ~ 0
+#define EPLLIN_CMD_SHUTDOWN                12	// ulArg_p ~ 0
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef struct {
+	unsigned int m_uiEventArgSize;
+	tEplApiEventArg *m_pEventArg;
+	tEplApiEventType *m_pEventType;
+	tEplKernel m_RetCbEvent;
+
+} tEplLinEvent;
+
+typedef struct {
+	tEplSdoComConHdl m_SdoComConHdl;
+	BOOL m_fValidSdoComConHdl;
+	unsigned int m_uiNodeId;
+	unsigned int m_uiIndex;
+	unsigned int m_uiSubindex;
+	void *m_le_pData;
+	unsigned int m_uiSize;
+	tEplSdoType m_SdoType;
+	void *m_pUserArg;
+
+} tEplLinSdoObject;
+
+typedef struct {
+	unsigned int m_uiIndex;
+	unsigned int m_uiSubindex;
+	void *m_pData;
+	unsigned int m_uiSize;
+
+} tEplLinLocalObject;
+
+typedef struct {
+	unsigned int m_uiNodeId;
+	tEplNmtNodeCommand m_NodeCommand;
+
+} tEplLinNodeCmdObject;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_API_LINUX_H_
diff --git a/drivers/staging/epl/EplApiLinuxKernel.c b/drivers/staging/epl/EplApiLinuxKernel.c
new file mode 100644
index 000000000000..05ca0628d6ae
--- /dev/null
+++ b/drivers/staging/epl/EplApiLinuxKernel.c
@@ -0,0 +1,1260 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  Linux kernel module as wrapper of EPL API layer,
+                i.e. counterpart to a Linux application
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplApiLinuxKernel.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.9 $  $Date: 2008/11/21 09:00:38 $
+
+                $State: Exp $
+
+                Build Environment:
+                GNU-Compiler for m68k
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/10/11 d.k.:  Initial Version
+  2008/04/10 m.u.:  Changed to new char driver init
+
+****************************************************************************/
+
+// kernel modul and driver
+
+//#include <linux/version.h>
+//#include <linux/config.h>
+
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/types.h>
+
+//#include <linux/module.h>
+//#include <linux/kernel.h>
+//#include <linux/init.h>
+//#include <linux/errno.h>
+
+// scheduling
+#include <linux/sched.h>
+
+// memory access
+#include <asm/uaccess.h>
+#include <linux/vmalloc.h>
+
+#ifdef CONFIG_DEVFS_FS
+#include <linux/major.h>
+#include <linux/devfs_fs_kernel.h>
+#endif
+
+#include "Epl.h"
+#include "EplApiLinux.h"
+//#include "kernel/EplPdokCal.h"
+#include "proc_fs.h"
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
+    // remove ("make invisible") obsolete symbols for kernel versions 2.6
+    // and higher
+#define MOD_INC_USE_COUNT
+#define MOD_DEC_USE_COUNT
+#define EXPORT_NO_SYMBOLS
+#else
+#error "This driver needs a 2.6.x kernel or higher"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+// Metainformation
+MODULE_LICENSE("Dual BSD/GPL");
+#ifdef MODULE_AUTHOR
+MODULE_AUTHOR("Daniel.Krueger@SYSTEC-electronic.com");
+MODULE_DESCRIPTION("EPL API driver");
+#endif
+
+//---------------------------------------------------------------------------
+//  Configuration
+//---------------------------------------------------------------------------
+
+#define EPLLIN_DRV_NAME     "systec_epl"	// used for <register_chrdev>
+
+//---------------------------------------------------------------------------
+//  Constant definitions
+//---------------------------------------------------------------------------
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#endif
+
+#define EVENT_STATE_INIT        0
+#define EVENT_STATE_IOCTL       1	// ioctl entered and ready to receive EPL event
+#define EVENT_STATE_READY       2	// EPL event can be forwarded to user application
+#define EVENT_STATE_TERM        3	// terminate processing
+
+#define EPL_STATE_NOTOPEN       0
+#define EPL_STATE_NOTINIT       1
+#define EPL_STATE_RUNNING       2
+#define EPL_STATE_SHUTDOWN      3
+
+//---------------------------------------------------------------------------
+//  Global variables
+//---------------------------------------------------------------------------
+
+#ifdef CONFIG_DEVFS_FS
+
+    // driver major number
+static int nDrvMajorNumber_g;
+
+#else
+
+    // device number (major and minor)
+static dev_t nDevNum_g;
+static struct cdev *pEpl_cdev_g;
+
+#endif
+
+static volatile unsigned int uiEplState_g = EPL_STATE_NOTOPEN;
+
+static struct semaphore SemaphoreCbEvent_g;	// semaphore for EplLinCbEvent
+static wait_queue_head_t WaitQueueCbEvent_g;	// wait queue EplLinCbEvent
+static wait_queue_head_t WaitQueueProcess_g;	// wait queue for EplApiProcess (user process)
+static wait_queue_head_t WaitQueueRelease_g;	// wait queue for EplLinRelease
+static atomic_t AtomicEventState_g = ATOMIC_INIT(EVENT_STATE_INIT);
+static tEplApiEventType EventType_g;	// event type (enum)
+static tEplApiEventArg *pEventArg_g;	// event argument (union)
+static tEplKernel RetCbEvent_g;	// return code from event callback function
+static wait_queue_head_t WaitQueueCbSync_g;	// wait queue EplLinCbSync
+static wait_queue_head_t WaitQueuePI_In_g;	// wait queue for EplApiProcessImageExchangeIn (user process)
+static atomic_t AtomicSyncState_g = ATOMIC_INIT(EVENT_STATE_INIT);
+
+//---------------------------------------------------------------------------
+//  Local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	void *m_pUserArg;
+	void *m_pData;
+
+} tEplLinSdoBufHeader;
+
+//---------------------------------------------------------------------------
+//  Local variables
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Prototypes of internal functions
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplLinCbEvent(tEplApiEventType EventType_p,	// IN: event type (enum)
+				tEplApiEventArg * pEventArg_p,	// IN: event argument (union)
+				void GENERIC * pUserArg_p);
+
+tEplKernel PUBLIC EplLinCbSync(void);
+
+static int __init EplLinInit(void);
+static void __exit EplLinExit(void);
+
+static int EplLinOpen(struct inode *pDeviceFile_p, struct file *pInstance_p);
+static int EplLinRelease(struct inode *pDeviceFile_p, struct file *pInstance_p);
+static ssize_t EplLinRead(struct file *pInstance_p, char *pDstBuff_p,
+			  size_t BuffSize_p, loff_t * pFileOffs_p);
+static ssize_t EplLinWrite(struct file *pInstance_p, const char *pSrcBuff_p,
+			   size_t BuffSize_p, loff_t * pFileOffs_p);
+static int EplLinIoctl(struct inode *pDeviceFile_p, struct file *pInstance_p,
+		       unsigned int uiIoctlCmd_p, unsigned long ulArg_p);
+
+//---------------------------------------------------------------------------
+//  Kernel Module specific Data Structures
+//---------------------------------------------------------------------------
+
+EXPORT_NO_SYMBOLS;
+
+module_init(EplLinInit);
+module_exit(EplLinExit);
+
+static struct file_operations EplLinFileOps_g = {
+	.owner = THIS_MODULE,
+	.open = EplLinOpen,
+	.release = EplLinRelease,
+	.read = EplLinRead,
+	.write = EplLinWrite,
+	.ioctl = EplLinIoctl,
+
+};
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//  Initailize Driver
+//---------------------------------------------------------------------------
+//  -> insmod driver
+//---------------------------------------------------------------------------
+
+static int __init EplLinInit(void)
+{
+
+	tEplKernel EplRet;
+	int iErr;
+	int iRet;
+#ifdef CONFIG_DEVFS_FS
+	int nMinorNumber;
+#endif
+
+	TRACE0("EPL: + EplLinInit...\n");
+	TRACE2("EPL:   Driver build: %s / %s\n", __DATE__, __TIME__);
+
+	iRet = 0;
+
+	// initialize global variables
+	atomic_set(&AtomicEventState_g, EVENT_STATE_INIT);
+	sema_init(&SemaphoreCbEvent_g, 1);
+	init_waitqueue_head(&WaitQueueCbEvent_g);
+	init_waitqueue_head(&WaitQueueProcess_g);
+	init_waitqueue_head(&WaitQueueRelease_g);
+
+#ifdef CONFIG_DEVFS_FS
+
+	// register character device handler
+	TRACE2("EPL:   Installing Driver '%s', Version %s...\n",
+	       EPLLIN_DRV_NAME, EPL_PRODUCT_VERSION);
+	TRACE0("EPL:   (using dynamic major number assignment)\n");
+	nDrvMajorNumber_g =
+	    register_chrdev(0, EPLLIN_DRV_NAME, &EplLinFileOps_g);
+	if (nDrvMajorNumber_g != 0) {
+		TRACE2
+		    ("EPL:   Driver '%s' installed successful, assigned MajorNumber=%d\n",
+		     EPLLIN_DRV_NAME, nDrvMajorNumber_g);
+	} else {
+		TRACE1
+		    ("EPL:   ERROR: Driver '%s' is unable to get a free MajorNumber!\n",
+		     EPLLIN_DRV_NAME);
+		iRet = -EIO;
+		goto Exit;
+	}
+
+	// create device node in DEVFS
+	nMinorNumber = 0;
+	TRACE1("EPL:   Creating device node '/dev/%s'...\n", EPLLIN_DEV_NAME);
+	iErr =
+	    devfs_mk_cdev(MKDEV(nDrvMajorNumber_g, nMinorNumber),
+			  S_IFCHR | S_IRUGO | S_IWUGO, EPLLIN_DEV_NAME);
+	if (iErr == 0) {
+		TRACE1("EPL:   Device node '/dev/%s' created successful.\n",
+		       EPLLIN_DEV_NAME);
+	} else {
+		TRACE1("EPL:   ERROR: unable to create device node '/dev/%s'\n",
+		       EPLLIN_DEV_NAME);
+		iRet = -EIO;
+		goto Exit;
+	}
+
+#else
+
+	// register character device handler
+	// only one Minor required
+	TRACE2("EPL:   Installing Driver '%s', Version %s...\n",
+	       EPLLIN_DRV_NAME, EPL_PRODUCT_VERSION);
+	iRet = alloc_chrdev_region(&nDevNum_g, 0, 1, EPLLIN_DRV_NAME);
+	if (iRet == 0) {
+		TRACE2
+		    ("EPL:   Driver '%s' installed successful, assigned MajorNumber=%d\n",
+		     EPLLIN_DRV_NAME, MAJOR(nDevNum_g));
+	} else {
+		TRACE1
+		    ("EPL:   ERROR: Driver '%s' is unable to get a free MajorNumber!\n",
+		     EPLLIN_DRV_NAME);
+		iRet = -EIO;
+		goto Exit;
+	}
+
+	// register cdev structure
+	pEpl_cdev_g = cdev_alloc();
+	pEpl_cdev_g->ops = &EplLinFileOps_g;
+	pEpl_cdev_g->owner = THIS_MODULE;
+	iErr = cdev_add(pEpl_cdev_g, nDevNum_g, 1);
+	if (iErr) {
+		TRACE2("EPL:   ERROR %d: Driver '%s' could not be added!\n",
+		       iErr, EPLLIN_DRV_NAME);
+		iRet = -EIO;
+		goto Exit;
+	}
+#endif
+
+	// create device node in PROCFS
+	EplRet = EplLinProcInit();
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+
+      Exit:
+
+	TRACE1("EPL: - EplLinInit (iRet=%d)\n", iRet);
+	return (iRet);
+
+}
+
+//---------------------------------------------------------------------------
+//  Remove Driver
+//---------------------------------------------------------------------------
+//  -> rmmod driver
+//---------------------------------------------------------------------------
+
+static void __exit EplLinExit(void)
+{
+
+	tEplKernel EplRet;
+
+	// delete instance for all modules
+//    EplRet = EplApiShutdown();
+//    printk("EplApiShutdown():  0x%X\n", EplRet);
+
+	// deinitialize proc fs
+	EplRet = EplLinProcFree();
+	printk("EplLinProcFree():        0x%X\n", EplRet);
+
+	TRACE0("EPL: + EplLinExit...\n");
+
+#ifdef CONFIG_DEVFS_FS
+
+	// remove device node from DEVFS
+	devfs_remove(EPLLIN_DEV_NAME);
+	TRACE1("EPL:   Device node '/dev/%s' removed.\n", EPLLIN_DEV_NAME);
+
+	// unregister character device handler
+	unregister_chrdev(nDrvMajorNumber_g, EPLLIN_DRV_NAME);
+
+#else
+
+	// remove cdev structure
+	cdev_del(pEpl_cdev_g);
+
+	// unregister character device handler
+	unregister_chrdev_region(nDevNum_g, 1);
+
+#endif
+
+	TRACE1("EPL:   Driver '%s' removed.\n", EPLLIN_DRV_NAME);
+
+	TRACE0("EPL: - EplLinExit\n");
+
+}
+
+//---------------------------------------------------------------------------
+//  Open Driver
+//---------------------------------------------------------------------------
+//  -> open("/dev/driver", O_RDWR)...
+//---------------------------------------------------------------------------
+
+static int EplLinOpen(struct inode *pDeviceFile_p,	// information about the device to open
+		      struct file *pInstance_p)	// information about driver instance
+{
+
+	int iRet;
+
+	TRACE0("EPL: + EplLinOpen...\n");
+
+	MOD_INC_USE_COUNT;
+
+	if (uiEplState_g != EPL_STATE_NOTOPEN) {	// stack already initialized
+		iRet = -EALREADY;
+	} else {
+		atomic_set(&AtomicEventState_g, EVENT_STATE_INIT);
+		sema_init(&SemaphoreCbEvent_g, 1);
+		init_waitqueue_head(&WaitQueueCbEvent_g);
+		init_waitqueue_head(&WaitQueueProcess_g);
+		init_waitqueue_head(&WaitQueueRelease_g);
+		atomic_set(&AtomicSyncState_g, EVENT_STATE_INIT);
+		init_waitqueue_head(&WaitQueueCbSync_g);
+		init_waitqueue_head(&WaitQueuePI_In_g);
+
+		uiEplState_g = EPL_STATE_NOTINIT;
+		iRet = 0;
+	}
+
+	TRACE1("EPL: - EplLinOpen (iRet=%d)\n", iRet);
+	return (iRet);
+
+}
+
+//---------------------------------------------------------------------------
+//  Close Driver
+//---------------------------------------------------------------------------
+//  -> close(device)...
+//---------------------------------------------------------------------------
+
+static int EplLinRelease(struct inode *pDeviceFile_p,	// information about the device to open
+			 struct file *pInstance_p)	// information about driver instance
+{
+
+	tEplKernel EplRet = kEplSuccessful;
+	int iRet;
+
+	TRACE0("EPL: + EplLinRelease...\n");
+
+	if (uiEplState_g != EPL_STATE_NOTINIT) {
+		// pass control to sync kernel thread, but signal termination
+		atomic_set(&AtomicSyncState_g, EVENT_STATE_TERM);
+		wake_up_interruptible(&WaitQueueCbSync_g);
+		wake_up_interruptible(&WaitQueuePI_In_g);
+
+		// pass control to event queue kernel thread
+		atomic_set(&AtomicEventState_g, EVENT_STATE_TERM);
+		wake_up_interruptible(&WaitQueueCbEvent_g);
+
+		if (uiEplState_g == EPL_STATE_RUNNING) {	// post NmtEventSwitchOff
+			EplRet = EplApiExecNmtCommand(kEplNmtEventSwitchOff);
+
+		}
+
+		if (EplRet == kEplSuccessful) {
+			TRACE0("EPL:   waiting for NMT_GS_OFF\n");
+			wait_event_interruptible(WaitQueueRelease_g,
+						 (uiEplState_g ==
+						  EPL_STATE_SHUTDOWN));
+		} else {	// post NmtEventSwitchOff failed
+			TRACE0("EPL:   event post failed\n");
+		}
+
+		// $$$ d.k.: What if waiting was interrupted by signal?
+
+		TRACE0("EPL:   call EplApiShutdown()\n");
+		// EPL stack can be safely shut down
+		// delete instance for all EPL modules
+		EplRet = EplApiShutdown();
+		printk("EplApiShutdown():  0x%X\n", EplRet);
+	}
+
+	uiEplState_g = EPL_STATE_NOTOPEN;
+	iRet = 0;
+
+	MOD_DEC_USE_COUNT;
+
+	TRACE1("EPL: - EplLinRelease (iRet=%d)\n", iRet);
+	return (iRet);
+
+}
+
+//---------------------------------------------------------------------------
+//  Read Data from Driver
+//---------------------------------------------------------------------------
+//  -> read(...)
+//---------------------------------------------------------------------------
+
+static ssize_t EplLinRead(struct file *pInstance_p,	// information about driver instance
+			  char *pDstBuff_p,	// address of buffer to fill with data
+			  size_t BuffSize_p,	// length of the buffer
+			  loff_t * pFileOffs_p)	// offset in the file
+{
+
+	int iRet;
+
+	TRACE0("EPL: + EplLinRead...\n");
+
+	TRACE0("EPL:   Sorry, this operation isn't supported.\n");
+	iRet = -EINVAL;
+
+	TRACE1("EPL: - EplLinRead (iRet=%d)\n", iRet);
+	return (iRet);
+
+}
+
+//---------------------------------------------------------------------------
+//  Write Data to Driver
+//---------------------------------------------------------------------------
+//  -> write(...)
+//---------------------------------------------------------------------------
+
+static ssize_t EplLinWrite(struct file *pInstance_p,	// information about driver instance
+			   const char *pSrcBuff_p,	// address of buffer to get data from
+			   size_t BuffSize_p,	// length of the buffer
+			   loff_t * pFileOffs_p)	// offset in the file
+{
+
+	int iRet;
+
+	TRACE0("EPL: + EplLinWrite...\n");
+
+	TRACE0("EPL:   Sorry, this operation isn't supported.\n");
+	iRet = -EINVAL;
+
+	TRACE1("EPL: - EplLinWrite (iRet=%d)\n", iRet);
+	return (iRet);
+
+}
+
+//---------------------------------------------------------------------------
+//  Generic Access to Driver
+//---------------------------------------------------------------------------
+//  -> ioctl(...)
+//---------------------------------------------------------------------------
+
+static int EplLinIoctl(struct inode *pDeviceFile_p,	// information about the device to open
+		       struct file *pInstance_p,	// information about driver instance
+		       unsigned int uiIoctlCmd_p,	// Ioctl command to execute
+		       unsigned long ulArg_p)	// Ioctl command specific argument/parameter
+{
+
+	tEplKernel EplRet;
+	int iErr;
+	int iRet;
+
+//    TRACE1("EPL: + EplLinIoctl (uiIoctlCmd_p=%d)...\n", uiIoctlCmd_p);
+
+	iRet = -EINVAL;
+
+	switch (uiIoctlCmd_p) {
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_INITIALIZE:
+		{
+			tEplApiInitParam EplApiInitParam;
+
+			iErr =
+			    copy_from_user(&EplApiInitParam,
+					   (const void *)ulArg_p,
+					   sizeof(EplApiInitParam));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			EplApiInitParam.m_pfnCbEvent = EplLinCbEvent;
+			EplApiInitParam.m_pfnCbSync = EplLinCbSync;
+
+			EplRet = EplApiInitialize(&EplApiInitParam);
+
+			uiEplState_g = EPL_STATE_RUNNING;
+
+			iRet = (int)EplRet;
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_SHUTDOWN:
+		{		// shutdown the threads
+
+			// pass control to sync kernel thread, but signal termination
+			atomic_set(&AtomicSyncState_g, EVENT_STATE_TERM);
+			wake_up_interruptible(&WaitQueueCbSync_g);
+			wake_up_interruptible(&WaitQueuePI_In_g);
+
+			// pass control to event queue kernel thread
+			atomic_set(&AtomicEventState_g, EVENT_STATE_TERM);
+			wake_up_interruptible(&WaitQueueCbEvent_g);
+
+			if (uiEplState_g == EPL_STATE_RUNNING) {	// post NmtEventSwitchOff
+				EplRet =
+				    EplApiExecNmtCommand(kEplNmtEventSwitchOff);
+
+			}
+
+			iRet = 0;
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_READ_LOCAL_OBJECT:
+		{
+			tEplLinLocalObject LocalObject;
+			void *pData;
+
+			iErr =
+			    copy_from_user(&LocalObject, (const void *)ulArg_p,
+					   sizeof(LocalObject));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if ((LocalObject.m_pData == NULL)
+			    || (LocalObject.m_uiSize == 0)) {
+				iRet = (int)kEplApiInvalidParam;
+				goto Exit;
+			}
+
+			pData = vmalloc(LocalObject.m_uiSize);
+			if (pData == NULL) {	// no memory available
+				iRet = -ENOMEM;
+				goto Exit;
+			}
+
+			EplRet =
+			    EplApiReadLocalObject(LocalObject.m_uiIndex,
+						  LocalObject.m_uiSubindex,
+						  pData, &LocalObject.m_uiSize);
+
+			if (EplRet == kEplSuccessful) {
+				iErr =
+				    copy_to_user(LocalObject.m_pData, pData,
+						 LocalObject.m_uiSize);
+
+				vfree(pData);
+
+				if (iErr != 0) {
+					iRet = -EIO;
+					goto Exit;
+				}
+				// return actual size (LocalObject.m_uiSize)
+				iErr = put_user(LocalObject.m_uiSize,
+						(unsigned int *)(ulArg_p +
+								 (unsigned long)
+								 &LocalObject.
+								 m_uiSize -
+								 (unsigned long)
+								 &LocalObject));
+				if (iErr != 0) {
+					iRet = -EIO;
+					goto Exit;
+				}
+
+			} else {
+				vfree(pData);
+			}
+
+			iRet = (int)EplRet;
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_WRITE_LOCAL_OBJECT:
+		{
+			tEplLinLocalObject LocalObject;
+			void *pData;
+
+			iErr =
+			    copy_from_user(&LocalObject, (const void *)ulArg_p,
+					   sizeof(LocalObject));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if ((LocalObject.m_pData == NULL)
+			    || (LocalObject.m_uiSize == 0)) {
+				iRet = (int)kEplApiInvalidParam;
+				goto Exit;
+			}
+
+			pData = vmalloc(LocalObject.m_uiSize);
+			if (pData == NULL) {	// no memory available
+				iRet = -ENOMEM;
+				goto Exit;
+			}
+			iErr =
+			    copy_from_user(pData, LocalObject.m_pData,
+					   LocalObject.m_uiSize);
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			EplRet =
+			    EplApiWriteLocalObject(LocalObject.m_uiIndex,
+						   LocalObject.m_uiSubindex,
+						   pData, LocalObject.m_uiSize);
+
+			vfree(pData);
+
+			iRet = (int)EplRet;
+			break;
+		}
+
+	case EPLLIN_CMD_READ_OBJECT:
+		{
+			tEplLinSdoObject SdoObject;
+			void *pData;
+			tEplLinSdoBufHeader *pBufHeader;
+			tEplSdoComConHdl *pSdoComConHdl;
+
+			iErr =
+			    copy_from_user(&SdoObject, (const void *)ulArg_p,
+					   sizeof(SdoObject));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if ((SdoObject.m_le_pData == NULL)
+			    || (SdoObject.m_uiSize == 0)) {
+				iRet = (int)kEplApiInvalidParam;
+				goto Exit;
+			}
+
+			pBufHeader =
+			    (tEplLinSdoBufHeader *)
+			    vmalloc(sizeof(tEplLinSdoBufHeader) +
+				    SdoObject.m_uiSize);
+			if (pBufHeader == NULL) {	// no memory available
+				iRet = -ENOMEM;
+				goto Exit;
+			}
+			// initiate temporary buffer
+			pBufHeader->m_pUserArg = SdoObject.m_pUserArg;	// original user argument pointer
+			pBufHeader->m_pData = SdoObject.m_le_pData;	// original data pointer from app
+			pData = pBufHeader + sizeof(tEplLinSdoBufHeader);
+
+			if (SdoObject.m_fValidSdoComConHdl != FALSE) {
+				pSdoComConHdl = &SdoObject.m_SdoComConHdl;
+			} else {
+				pSdoComConHdl = NULL;
+			}
+
+			EplRet =
+			    EplApiReadObject(pSdoComConHdl,
+					     SdoObject.m_uiNodeId,
+					     SdoObject.m_uiIndex,
+					     SdoObject.m_uiSubindex, pData,
+					     &SdoObject.m_uiSize,
+					     SdoObject.m_SdoType, pBufHeader);
+
+			// return actual SDO handle (SdoObject.m_SdoComConHdl)
+			iErr = put_user(SdoObject.m_SdoComConHdl,
+					(unsigned int *)(ulArg_p +
+							 (unsigned long)
+							 &SdoObject.
+							 m_SdoComConHdl -
+							 (unsigned long)
+							 &SdoObject));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if (EplRet == kEplSuccessful) {
+				iErr =
+				    copy_to_user(SdoObject.m_le_pData, pData,
+						 SdoObject.m_uiSize);
+
+				vfree(pBufHeader);
+
+				if (iErr != 0) {
+					iRet = -EIO;
+					goto Exit;
+				}
+				// return actual size (SdoObject.m_uiSize)
+				iErr = put_user(SdoObject.m_uiSize,
+						(unsigned int *)(ulArg_p +
+								 (unsigned long)
+								 &SdoObject.
+								 m_uiSize -
+								 (unsigned long)
+								 &SdoObject));
+				if (iErr != 0) {
+					iRet = -EIO;
+					goto Exit;
+				}
+			} else if (EplRet != kEplApiTaskDeferred) {	// error ocurred
+				vfree(pBufHeader);
+				if (iErr != 0) {
+					iRet = -EIO;
+					goto Exit;
+				}
+			}
+
+			iRet = (int)EplRet;
+			break;
+		}
+
+	case EPLLIN_CMD_WRITE_OBJECT:
+		{
+			tEplLinSdoObject SdoObject;
+			void *pData;
+			tEplLinSdoBufHeader *pBufHeader;
+			tEplSdoComConHdl *pSdoComConHdl;
+
+			iErr =
+			    copy_from_user(&SdoObject, (const void *)ulArg_p,
+					   sizeof(SdoObject));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if ((SdoObject.m_le_pData == NULL)
+			    || (SdoObject.m_uiSize == 0)) {
+				iRet = (int)kEplApiInvalidParam;
+				goto Exit;
+			}
+
+			pBufHeader =
+			    (tEplLinSdoBufHeader *)
+			    vmalloc(sizeof(tEplLinSdoBufHeader) +
+				    SdoObject.m_uiSize);
+			if (pBufHeader == NULL) {	// no memory available
+				iRet = -ENOMEM;
+				goto Exit;
+			}
+			// initiate temporary buffer
+			pBufHeader->m_pUserArg = SdoObject.m_pUserArg;	// original user argument pointer
+			pBufHeader->m_pData = SdoObject.m_le_pData;	// original data pointer from app
+			pData = pBufHeader + sizeof(tEplLinSdoBufHeader);
+
+			iErr =
+			    copy_from_user(pData, SdoObject.m_le_pData,
+					   SdoObject.m_uiSize);
+
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if (SdoObject.m_fValidSdoComConHdl != FALSE) {
+				pSdoComConHdl = &SdoObject.m_SdoComConHdl;
+			} else {
+				pSdoComConHdl = NULL;
+			}
+
+			EplRet =
+			    EplApiWriteObject(pSdoComConHdl,
+					      SdoObject.m_uiNodeId,
+					      SdoObject.m_uiIndex,
+					      SdoObject.m_uiSubindex, pData,
+					      SdoObject.m_uiSize,
+					      SdoObject.m_SdoType, pBufHeader);
+
+			// return actual SDO handle (SdoObject.m_SdoComConHdl)
+			iErr = put_user(SdoObject.m_SdoComConHdl,
+					(unsigned int *)(ulArg_p +
+							 (unsigned long)
+							 &SdoObject.
+							 m_SdoComConHdl -
+							 (unsigned long)
+							 &SdoObject));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if (EplRet != kEplApiTaskDeferred) {	// succeeded or error ocurred, but task not deferred
+				vfree(pBufHeader);
+			}
+
+			iRet = (int)EplRet;
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_FREE_SDO_CHANNEL:
+		{
+			// forward SDO handle to EPL stack
+			EplRet =
+			    EplApiFreeSdoChannel((tEplSdoComConHdl) ulArg_p);
+
+			iRet = (int)EplRet;
+			break;
+		}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_MN_TRIGGER_STATE_CHANGE:
+		{
+			tEplLinNodeCmdObject NodeCmdObject;
+
+			iErr =
+			    copy_from_user(&NodeCmdObject,
+					   (const void *)ulArg_p,
+					   sizeof(NodeCmdObject));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			EplRet =
+			    EplApiMnTriggerStateChange(NodeCmdObject.m_uiNodeId,
+						       NodeCmdObject.
+						       m_NodeCommand);
+			iRet = (int)EplRet;
+			break;
+		}
+#endif
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_GET_EVENT:
+		{
+			tEplLinEvent Event;
+
+			// save event structure
+			iErr =
+			    copy_from_user(&Event, (const void *)ulArg_p,
+					   sizeof(Event));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+			// save return code from application's event callback function
+			RetCbEvent_g = Event.m_RetCbEvent;
+
+			if (RetCbEvent_g == kEplShutdown) {
+				// pass control to event queue kernel thread, but signal termination
+				atomic_set(&AtomicEventState_g,
+					   EVENT_STATE_TERM);
+				wake_up_interruptible(&WaitQueueCbEvent_g);
+				// exit with error -> EplApiProcess() will leave the infinite loop
+				iRet = 1;
+				goto Exit;
+			}
+			// pass control to event queue kernel thread
+			atomic_set(&AtomicEventState_g, EVENT_STATE_IOCTL);
+			wake_up_interruptible(&WaitQueueCbEvent_g);
+
+			// fall asleep itself in own wait queue
+			iErr = wait_event_interruptible(WaitQueueProcess_g,
+							(atomic_read
+							 (&AtomicEventState_g)
+							 == EVENT_STATE_READY)
+							||
+							(atomic_read
+							 (&AtomicEventState_g)
+							 == EVENT_STATE_TERM));
+			if (iErr != 0) {	// waiting was interrupted by signal
+				// pass control to event queue kernel thread, but signal termination
+				atomic_set(&AtomicEventState_g,
+					   EVENT_STATE_TERM);
+				wake_up_interruptible(&WaitQueueCbEvent_g);
+				// exit with this error -> EplApiProcess() will leave the infinite loop
+				iRet = iErr;
+				goto Exit;
+			} else if (atomic_read(&AtomicEventState_g) == EVENT_STATE_TERM) {	// termination in progress
+				// pass control to event queue kernel thread, but signal termination
+				wake_up_interruptible(&WaitQueueCbEvent_g);
+				// exit with this error -> EplApiProcess() will leave the infinite loop
+				iRet = 1;
+				goto Exit;
+			}
+			// copy event to user space
+			iErr =
+			    copy_to_user(Event.m_pEventType, &EventType_g,
+					 sizeof(EventType_g));
+			if (iErr != 0) {	// not all data could be copied
+				iRet = -EIO;
+				goto Exit;
+			}
+			// $$$ d.k. perform SDO event processing
+			if (EventType_g == kEplApiEventSdo) {
+				void *pData;
+				tEplLinSdoBufHeader *pBufHeader;
+
+				pBufHeader =
+				    (tEplLinSdoBufHeader *) pEventArg_g->m_Sdo.
+				    m_pUserArg;
+				pData =
+				    pBufHeader + sizeof(tEplLinSdoBufHeader);
+
+				if (pEventArg_g->m_Sdo.m_SdoAccessType ==
+				    kEplSdoAccessTypeRead) {
+					// copy read data to user space
+					iErr =
+					    copy_to_user(pBufHeader->m_pData,
+							 pData,
+							 pEventArg_g->m_Sdo.
+							 m_uiTransferredByte);
+					if (iErr != 0) {	// not all data could be copied
+						iRet = -EIO;
+						goto Exit;
+					}
+				}
+				pEventArg_g->m_Sdo.m_pUserArg =
+				    pBufHeader->m_pUserArg;
+				vfree(pBufHeader);
+			}
+
+			iErr =
+			    copy_to_user(Event.m_pEventArg, pEventArg_g,
+					 min(sizeof(tEplApiEventArg),
+					     Event.m_uiEventArgSize));
+			if (iErr != 0) {	// not all data could be copied
+				iRet = -EIO;
+				goto Exit;
+			}
+			// return to EplApiProcess(), which will call the application's event callback function
+			iRet = 0;
+
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_PI_SETUP:
+		{
+			EplRet = EplApiProcessImageSetup();
+			iRet = (int)EplRet;
+
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_PI_IN:
+		{
+			tEplApiProcessImage ProcessImageIn;
+
+			// save process image structure
+			iErr =
+			    copy_from_user(&ProcessImageIn,
+					   (const void *)ulArg_p,
+					   sizeof(ProcessImageIn));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+			// pass control to event queue kernel thread
+			atomic_set(&AtomicSyncState_g, EVENT_STATE_IOCTL);
+
+			// fall asleep itself in own wait queue
+			iErr = wait_event_interruptible(WaitQueuePI_In_g,
+							(atomic_read
+							 (&AtomicSyncState_g) ==
+							 EVENT_STATE_READY)
+							||
+							(atomic_read
+							 (&AtomicSyncState_g) ==
+							 EVENT_STATE_TERM));
+			if (iErr != 0) {	// waiting was interrupted by signal
+				// pass control to sync kernel thread, but signal termination
+				atomic_set(&AtomicSyncState_g,
+					   EVENT_STATE_TERM);
+				wake_up_interruptible(&WaitQueueCbSync_g);
+				// exit with this error -> application will leave the infinite loop
+				iRet = iErr;
+				goto Exit;
+			} else if (atomic_read(&AtomicSyncState_g) == EVENT_STATE_TERM) {	// termination in progress
+				// pass control to sync kernel thread, but signal termination
+				wake_up_interruptible(&WaitQueueCbSync_g);
+				// exit with this error -> application will leave the infinite loop
+				iRet = 1;
+				goto Exit;
+			}
+			// exchange process image
+			EplRet = EplApiProcessImageExchangeIn(&ProcessImageIn);
+
+			// return to EplApiProcessImageExchangeIn()
+			iRet = (int)EplRet;
+
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_PI_OUT:
+		{
+			tEplApiProcessImage ProcessImageOut;
+
+			// save process image structure
+			iErr =
+			    copy_from_user(&ProcessImageOut,
+					   (const void *)ulArg_p,
+					   sizeof(ProcessImageOut));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if (atomic_read(&AtomicSyncState_g) !=
+			    EVENT_STATE_READY) {
+				iRet = (int)kEplInvalidOperation;
+				goto Exit;
+			}
+			// exchange process image
+			EplRet =
+			    EplApiProcessImageExchangeOut(&ProcessImageOut);
+
+			// pass control to sync kernel thread
+			atomic_set(&AtomicSyncState_g, EVENT_STATE_TERM);
+			wake_up_interruptible(&WaitQueueCbSync_g);
+
+			// return to EplApiProcessImageExchangeout()
+			iRet = (int)EplRet;
+
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_NMT_COMMAND:
+		{
+			// forward NMT command to EPL stack
+			EplRet = EplApiExecNmtCommand((tEplNmtEvent) ulArg_p);
+
+			iRet = (int)EplRet;
+
+			break;
+		}
+
+		// ----------------------------------------------------------
+	default:
+		{
+			break;
+		}
+	}
+
+      Exit:
+
+//    TRACE1("EPL: - EplLinIoctl (iRet=%d)\n", iRet);
+	return (iRet);
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+tEplKernel PUBLIC EplLinCbEvent(tEplApiEventType EventType_p,	// IN: event type (enum)
+				tEplApiEventArg * pEventArg_p,	// IN: event argument (union)
+				void GENERIC * pUserArg_p)
+{
+	tEplKernel EplRet = kEplSuccessful;
+	int iErr;
+
+	// block any further call to this function, i.e. enter critical section
+	iErr = down_interruptible(&SemaphoreCbEvent_g);
+	if (iErr != 0) {	// waiting was interrupted by signal
+		EplRet = kEplShutdown;
+		goto Exit;
+	}
+	// wait for EplApiProcess() to call ioctl
+	// normally it should be waiting already for us to pass a new event
+	iErr = wait_event_interruptible(WaitQueueCbEvent_g,
+					(atomic_read(&AtomicEventState_g) ==
+					 EVENT_STATE_IOCTL)
+					|| (atomic_read(&AtomicEventState_g) ==
+					    EVENT_STATE_TERM));
+	if ((iErr != 0) || (atomic_read(&AtomicEventState_g) == EVENT_STATE_TERM)) {	// waiting was interrupted by signal
+		EplRet = kEplShutdown;
+		goto LeaveCriticalSection;
+	}
+	// save event information for ioctl
+	EventType_g = EventType_p;
+	pEventArg_g = pEventArg_p;
+
+	// pass control to application's event callback function, i.e. EplApiProcess()
+	atomic_set(&AtomicEventState_g, EVENT_STATE_READY);
+	wake_up_interruptible(&WaitQueueProcess_g);
+
+	// now, the application's event callback function processes the event
+
+	// wait for completion of application's event callback function, i.e. EplApiProcess() calls ioctl again
+	iErr = wait_event_interruptible(WaitQueueCbEvent_g,
+					(atomic_read(&AtomicEventState_g) ==
+					 EVENT_STATE_IOCTL)
+					|| (atomic_read(&AtomicEventState_g) ==
+					    EVENT_STATE_TERM));
+	if ((iErr != 0) || (atomic_read(&AtomicEventState_g) == EVENT_STATE_TERM)) {	// waiting was interrupted by signal
+		EplRet = kEplShutdown;
+		goto LeaveCriticalSection;
+	}
+	// read return code from application's event callback function
+	EplRet = RetCbEvent_g;
+
+      LeaveCriticalSection:
+	up(&SemaphoreCbEvent_g);
+
+      Exit:
+	// check if NMT_GS_OFF is reached
+	if (EventType_p == kEplApiEventNmtStateChange) {
+		if (pEventArg_p->m_NmtStateChange.m_NewNmtState == kEplNmtGsOff) {	// NMT state machine was shut down
+			TRACE0("EPL:   EplLinCbEvent(NMT_GS_OFF)\n");
+			uiEplState_g = EPL_STATE_SHUTDOWN;
+			atomic_set(&AtomicEventState_g, EVENT_STATE_TERM);
+			wake_up(&WaitQueueRelease_g);
+		} else {	// NMT state machine is running
+			uiEplState_g = EPL_STATE_RUNNING;
+		}
+	}
+
+	return EplRet;
+}
+
+tEplKernel PUBLIC EplLinCbSync(void)
+{
+	tEplKernel EplRet = kEplSuccessful;
+	int iErr;
+
+	// check if user process waits for sync
+	if (atomic_read(&AtomicSyncState_g) == EVENT_STATE_IOCTL) {
+		// pass control to application, i.e. EplApiProcessImageExchangeIn()
+		atomic_set(&AtomicSyncState_g, EVENT_STATE_READY);
+		wake_up_interruptible(&WaitQueuePI_In_g);
+
+		// now, the application processes the sync event
+
+		// wait for call of EplApiProcessImageExchangeOut()
+		iErr = wait_event_interruptible(WaitQueueCbSync_g,
+						(atomic_read(&AtomicSyncState_g)
+						 == EVENT_STATE_IOCTL)
+						||
+						(atomic_read(&AtomicSyncState_g)
+						 == EVENT_STATE_TERM));
+		if ((iErr != 0) || (atomic_read(&AtomicEventState_g) == EVENT_STATE_IOCTL)) {	// waiting was interrupted by signal or application called wrong function
+			EplRet = kEplShutdown;
+		}
+	} else {		// application is currently not waiting for sync
+		// continue without interruption
+		// TPDO are set valid by caller (i.e. EplEventkProcess())
+	}
+
+	TGT_DBG_SIGNAL_TRACE_POINT(1);
+
+	return EplRet;
+}
+
+// EOF
diff --git a/drivers/staging/epl/EplApiProcessImage.c b/drivers/staging/epl/EplApiProcessImage.c
new file mode 100644
index 000000000000..2b2fdf229a75
--- /dev/null
+++ b/drivers/staging/epl/EplApiProcessImage.c
@@ -0,0 +1,347 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for EPL API module (process image)
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplApiProcessImage.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.7 $  $Date: 2008/11/13 17:13:09 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/10/10 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "Epl.h"
+//#include "kernel/EplPdokCal.h"
+
+#if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+#include <asm/uaccess.h>
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplApi                                              */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+#if ((EPL_API_PROCESS_IMAGE_SIZE_IN > 0) || (EPL_API_PROCESS_IMAGE_SIZE_OUT > 0))
+typedef struct {
+#if EPL_API_PROCESS_IMAGE_SIZE_IN > 0
+	BYTE m_abProcessImageInput[EPL_API_PROCESS_IMAGE_SIZE_IN];
+#endif
+#if EPL_API_PROCESS_IMAGE_SIZE_OUT > 0
+	BYTE m_abProcessImageOutput[EPL_API_PROCESS_IMAGE_SIZE_OUT];
+#endif
+
+} tEplApiProcessImageInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+static tEplApiProcessImageInstance EplApiProcessImageInstance_g;
+#endif
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiProcessImageSetup()
+//
+// Description: sets up static process image
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiProcessImageSetup(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+#if ((EPL_API_PROCESS_IMAGE_SIZE_IN > 0) || (EPL_API_PROCESS_IMAGE_SIZE_OUT > 0))
+	unsigned int uiVarEntries;
+	tEplObdSize ObdSize;
+#endif
+
+#if EPL_API_PROCESS_IMAGE_SIZE_IN > 0
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_IN;
+	ObdSize = 1;
+	Ret = EplApiLinkObject(0x2000,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageInput, &uiVarEntries, &ObdSize,
+			       1);
+
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_IN;
+	ObdSize = 1;
+	Ret = EplApiLinkObject(0x2001,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageInput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 2;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_IN / ObdSize;
+	Ret = EplApiLinkObject(0x2010,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageInput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 2;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_IN / ObdSize;
+	Ret = EplApiLinkObject(0x2011,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageInput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 4;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_IN / ObdSize;
+	Ret = EplApiLinkObject(0x2020,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageInput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 4;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_IN / ObdSize;
+	Ret = EplApiLinkObject(0x2021,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageInput, &uiVarEntries, &ObdSize,
+			       1);
+#endif
+
+#if EPL_API_PROCESS_IMAGE_SIZE_OUT > 0
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_OUT;
+	ObdSize = 1;
+	Ret = EplApiLinkObject(0x2030,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageOutput, &uiVarEntries, &ObdSize,
+			       1);
+
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_OUT;
+	ObdSize = 1;
+	Ret = EplApiLinkObject(0x2031,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageOutput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 2;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_OUT / ObdSize;
+	Ret = EplApiLinkObject(0x2040,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageOutput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 2;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_OUT / ObdSize;
+	Ret = EplApiLinkObject(0x2041,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageOutput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 4;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_OUT / ObdSize;
+	Ret = EplApiLinkObject(0x2050,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageOutput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 4;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_OUT / ObdSize;
+	Ret = EplApiLinkObject(0x2051,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageOutput, &uiVarEntries, &ObdSize,
+			       1);
+#endif
+
+	return Ret;
+}
+
+//----------------------------------------------------------------------------
+// Function:    EplApiProcessImageExchangeIn()
+//
+// Description: replaces passed input process image with the one of EPL stack
+//
+// Parameters:  pPI_p                   = input process image
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiProcessImageExchangeIn(tEplApiProcessImage * pPI_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+#if EPL_API_PROCESS_IMAGE_SIZE_IN > 0
+#if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+	copy_to_user(pPI_p->m_pImage,
+		     EplApiProcessImageInstance_g.m_abProcessImageInput,
+		     min(pPI_p->m_uiSize,
+			 sizeof(EplApiProcessImageInstance_g.
+				m_abProcessImageInput)));
+#else
+	EPL_MEMCPY(pPI_p->m_pImage,
+		   EplApiProcessImageInstance_g.m_abProcessImageInput,
+		   min(pPI_p->m_uiSize,
+		       sizeof(EplApiProcessImageInstance_g.
+			      m_abProcessImageInput)));
+#endif
+#endif
+
+	return Ret;
+}
+
+//----------------------------------------------------------------------------
+// Function:    EplApiProcessImageExchangeOut()
+//
+// Description: copies passed output process image to EPL stack.
+//
+// Parameters:  pPI_p                   = output process image
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiProcessImageExchangeOut(tEplApiProcessImage * pPI_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+#if EPL_API_PROCESS_IMAGE_SIZE_OUT > 0
+#if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+	copy_from_user(EplApiProcessImageInstance_g.m_abProcessImageOutput,
+		       pPI_p->m_pImage,
+		       min(pPI_p->m_uiSize,
+			   sizeof(EplApiProcessImageInstance_g.
+				  m_abProcessImageOutput)));
+#else
+	EPL_MEMCPY(EplApiProcessImageInstance_g.m_abProcessImageOutput,
+		   pPI_p->m_pImage,
+		   min(pPI_p->m_uiSize,
+		       sizeof(EplApiProcessImageInstance_g.
+			      m_abProcessImageOutput)));
+#endif
+#endif
+
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+// EOF
diff --git a/drivers/staging/epl/EplCfg.h b/drivers/staging/epl/EplCfg.h
new file mode 100644
index 000000000000..38e958a042a8
--- /dev/null
+++ b/drivers/staging/epl/EplCfg.h
@@ -0,0 +1,196 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  configuration file
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplCfg.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    ...
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/06    k.t.: Start of Implementation
+
+****************************************************************************/
+
+#ifndef _EPLCFG_H_
+#define _EPLCFG_H_
+
+// =========================================================================
+// generic defines which for whole EPL Stack
+// =========================================================================
+#define EPL_USE_DELETEINST_FUNC TRUE
+
+// needed to support datatypes over 32 bit by global.h
+#define USE_VAR64
+
+// EPL_MAX_INSTANCES specifies count of instances of all EPL modules.
+// If it is greater than 1 the first parameter of all
+// functions is the instance number.
+#define EPL_MAX_INSTANCES               1
+
+// This defines the target hardware. Here is encoded wich CPU and wich external
+// peripherals are connected. For possible values refere to target.h. If
+// necessary value is not available EPL stack has to
+// be adapted and tested.
+#define TARGET_HARDWARE                 TGTHW_PC_WRAPP
+
+// use no FIFOs, make direct calls
+//#define EPL_NO_FIFO
+
+// use no IPC between user- and kernelspace modules, make direct calls
+#define EPL_NO_USER_KERNEL
+
+#ifndef BENCHMARK_MODULES
+#define BENCHMARK_MODULES       0	//0xEE800042L
+#endif
+
+// Default defug level:
+// Only debug traces of these modules will be compiled which flags are set in define DEF_DEBUG_LVL.
+#ifndef DEF_DEBUG_LVL
+#define DEF_DEBUG_LVL           0xEC000000L
+#endif
+//   EPL_DBGLVL_OBD         =   0x00000004L
+// * EPL_DBGLVL_ASSERT      =   0x20000000L
+// * EPL_DBGLVL_ERROR       =   0x40000000L
+// * EPL_DBGLVL_ALWAYS      =   0x80000000L
+
+// EPL_MODULE_INTEGRATION defines all modules which are included in
+// EPL application. Please add or delete modules for your application.
+#define EPL_MODULE_INTEGRATION EPL_MODULE_OBDK \
+                               | EPL_MODULE_PDOK \
+                               | EPL_MODULE_NMT_MN \
+                               | EPL_MODULE_SDOS \
+                               | EPL_MODULE_SDOC \
+                               | EPL_MODULE_SDO_ASND \
+                               | EPL_MODULE_SDO_UDP \
+                               | EPL_MODULE_NMT_CN \
+                               | EPL_MODULE_NMTU \
+                               | EPL_MODULE_NMTK \
+                               | EPL_MODULE_DLLK \
+                               | EPL_MODULE_DLLU \
+                               | EPL_MODULE_VETH
+//                               | EPL_MODULE_OBDU
+
+// =========================================================================
+// EPL ethernet driver (Edrv) specific defines
+// =========================================================================
+
+// switch this define to TRUE if Edrv supports fast tx frames
+#define EDRV_FAST_TXFRAMES              FALSE
+//#define EDRV_FAST_TXFRAMES              TRUE
+
+// switch this define to TRUE if Edrv supports early receive interrupts
+#define EDRV_EARLY_RX_INT               FALSE
+//#define EDRV_EARLY_RX_INT               TRUE
+
+// enables setting of several port pins for benchmarking purposes
+#define EDRV_BENCHMARK                  FALSE
+//#define EDRV_BENCHMARK                  TRUE // MCF_GPIO_PODR_PCIBR
+
+// Call Tx handler (i.e. EplDllCbFrameTransmitted()) already if DMA has finished,
+// otherwise call the Tx handler if frame was actually transmitted over ethernet.
+#define EDRV_DMA_TX_HANDLER             FALSE
+//#define EDRV_DMA_TX_HANDLER             TRUE
+
+// number of used ethernet controller
+//#define EDRV_USED_ETH_CTRL              1
+
+// =========================================================================
+// Data Link Layer (DLL) specific defines
+// =========================================================================
+
+// switch this define to TRUE if Edrv supports fast tx frames
+// and DLL shall pass PRes as ready to Edrv after SoC
+#define EPL_DLL_PRES_READY_AFTER_SOC    FALSE
+//#define EPL_DLL_PRES_READY_AFTER_SOC    TRUE
+
+// switch this define to TRUE if Edrv supports fast tx frames
+// and DLL shall pass PRes as ready to Edrv after SoA
+#define EPL_DLL_PRES_READY_AFTER_SOA    FALSE
+//#define EPL_DLL_PRES_READY_AFTER_SOA    TRUE
+
+// =========================================================================
+// OBD specific defines
+// =========================================================================
+
+// switch this define to TRUE if Epl should compare object range
+// automaticly
+#define EPL_OBD_CHECK_OBJECT_RANGE          FALSE
+//#define EPL_OBD_CHECK_OBJECT_RANGE          TRUE
+
+// set this define to TRUE if there are strings or domains in OD, which
+// may be changed in object size and/or object data pointer by its object
+// callback function (called event kObdEvWrStringDomain)
+//#define EPL_OBD_USE_STRING_DOMAIN_IN_RAM    FALSE
+#define EPL_OBD_USE_STRING_DOMAIN_IN_RAM    TRUE
+
+#define EPL_OBD_USE_VARIABLE_SUBINDEX_TAB TRUE
+
+// =========================================================================
+// Timer module specific defines
+// =========================================================================
+
+// if TRUE it uses the Timer module implementation of EPL user also in EPL kernel
+#define EPL_TIMER_USE_USER              TRUE
+
+// if TRUE the high resolution timer module will be used
+#define EPL_TIMER_USE_HIGHRES              TRUE
+//#define EPL_TIMER_USE_HIGHRES              FALSE
+
+#endif //_EPLCFG_H_
diff --git a/drivers/staging/epl/EplDef.h b/drivers/staging/epl/EplDef.h
new file mode 100644
index 000000000000..1dc8108449c5
--- /dev/null
+++ b/drivers/staging/epl/EplDef.h
@@ -0,0 +1,355 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for EPL default constants
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDef.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.15 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_DEF_H_
+#define _EPL_DEF_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define EPL_C_ADR_BROADCAST         0xFF	// EPL broadcast address
+#define EPL_C_ADR_DIAG_DEF_NODE_ID  0xFD	// EPL default address of dignostic device
+#define EPL_C_ADR_DUMMY_NODE_ID     0xFC	// EPL dummy node address
+#define EPL_C_ADR_INVALID           0x00	// invalid EPL address
+#define EPL_C_ADR_MN_DEF_NODE_ID    0xF0	// EPL default address of MN
+#define EPL_C_ADR_RT1_DEF_NODE_ID   0xFE	// EPL default address of router type 1
+#define EPL_C_DLL_ASND_PRIO_NMTRQST 7	// increased ASnd request priority to be used by NMT Requests
+#define EPL_C_DLL_ASND_PRIO_STD     0	// standard ASnd request priority
+#define EPL_C_DLL_ETHERTYPE_EPL     0x88AB
+#define EPL_C_DLL_ISOCHR_MAX_PAYL   1490	// Byte: maximum size of PReq and PRes payload data, requires C_IP_MAX_MTU
+#define EPL_C_DLL_MAX_ASYNC_MTU     1500	// Byte: maximum asynchronous payload in bytes
+#define EPL_C_DLL_MAX_PAYL_OFFSET   1499	// Byte: maximum offset of Ethernet frame payload, requires C_IP_MAX_MTU
+#define EPL_C_DLL_MAX_RS            7
+#define EPL_C_DLL_MIN_ASYNC_MTU     282	// Byte: minimum asynchronous payload in bytes.
+#define EPL_C_DLL_MIN_PAYL_OFFSET   45	// Byte: minimum offset of Ethernet frame payload
+#define EPL_C_DLL_MULTICAST_ASND    0x01111E000004LL	// EPL ASnd multicast MAC address, canonical form
+#define EPL_C_DLL_MULTICAST_PRES    0x01111E000002LL	// EPL PRes multicast MAC address, canonical form
+#define EPL_C_DLL_MULTICAST_SOA     0x01111E000003LL	// EPL SoA multicast MAC address, canonical form
+#define EPL_C_DLL_MULTICAST_SOC     0x01111E000001LL	// EPL Soc multicast MAC address, canonical form
+#define EPL_C_DLL_PREOP1_START_CYCLES 10	// number of unassigning SoA frames at start of NMT_MS_PRE_OPERATIONAL_1
+#define EPL_C_DLL_T_BITTIME         10	// ns: Transmission time per bit on 100 Mbit/s network
+#define EPL_C_DLL_T_EPL_PDO_HEADER  10	// Byte: size of PReq and PRes EPL PDO message header
+#define EPL_C_DLL_T_ETH2_WRAPPER    18	// Byte: size of Ethernet type II wrapper consisting of header and checksum
+#define EPL_C_DLL_T_IFG             640	// ns: Ethernet Interframe Gap
+#define EPL_C_DLL_T_MIN_FRAME       5120	// ns: Size of minimum Ethernet frame (without preamble)
+#define EPL_C_DLL_T_PREAMBLE        960	// ns: Size of Ethernet frame preamble
+
+#define EPL_C_DLL_MINSIZE_SOC       36	// minimum size of SoC without padding and CRC
+#define EPL_C_DLL_MINSIZE_PREQ      60	// minimum size of PRec without CRC
+#define EPL_C_DLL_MINSIZE_PRES      60	// minimum size of PRes without CRC
+#define EPL_C_DLL_MINSIZE_SOA       24	// minimum size of SoA without padding and CRC
+#define EPL_C_DLL_MINSIZE_IDENTRES  176	// minimum size of IdentResponse without CRC
+#define EPL_C_DLL_MINSIZE_STATUSRES 72	// minimum size of StatusResponse without CRC
+#define EPL_C_DLL_MINSIZE_NMTCMD    20	// minimum size of NmtCommand without CommandData, padding and CRC
+#define EPL_C_DLL_MINSIZE_NMTCMDEXT 52	// minimum size of NmtCommand without padding and CRC
+#define EPL_C_DLL_MINSIZE_NMTREQ    20	// minimum size of NmtRequest without CommandData, padding and CRC
+#define EPL_C_DLL_MINSIZE_NMTREQEXT 52	// minimum size of NmtRequest without padding and CRC
+
+#define EPL_C_ERR_MONITOR_DELAY     10	// Error monitoring start delay (not used in DS 1.0.0)
+#define EPL_C_IP_ADR_INVALID        0x00000000L	// invalid IP address (0.0.0.0) used to indicate no change
+#define EPL_C_IP_INVALID_MTU        0	// Byte: invalid MTU size used to indicate no change
+#define EPL_C_IP_MAX_MTU            1518	// Byte: maximum size in bytes of the IP stack which must be processed.
+#define EPL_C_IP_MIN_MTU            300	// Byte: minimum size in bytes of the IP stack which must be processed.
+#define EPL_C_NMT_STATE_TOLERANCE   5	// Cycles: maximum reaction time to NMT state commands
+#define EPL_C_NMT_STATREQ_CYCLE     5	// sec: StatusRequest cycle time to be applied to AsyncOnly CNs
+#define EPL_C_SDO_EPL_PORT          3819
+
+#define EPL_C_DLL_MAX_ASND_SERVICE_IDS  5	// see tEplDllAsndServiceId in EplDll.h
+
+// Default configuration
+// ======================
+
+#ifndef EPL_D_PDO_Granularity_U8
+#define EPL_D_PDO_Granularity_U8    8	// minimum size of objects to be mapped in bits UNSIGNED8 O O 1 1
+#endif
+
+#ifndef EPL_NMT_MAX_NODE_ID
+#define EPL_NMT_MAX_NODE_ID         254	// maximum node-ID
+#endif
+
+#ifndef EPL_D_NMT_MaxCNNumber_U8
+#define EPL_D_NMT_MaxCNNumber_U8    239	// maximum number of supported regular CNs in the Node ID range 1 .. 239 UNSIGNED8 O O 239 239
+#endif
+
+// defines for EPL API layer static process image
+#ifndef EPL_API_PROCESS_IMAGE_SIZE_IN
+#define EPL_API_PROCESS_IMAGE_SIZE_IN   0
+#endif
+
+#ifndef EPL_API_PROCESS_IMAGE_SIZE_OUT
+#define EPL_API_PROCESS_IMAGE_SIZE_OUT  0
+#endif
+
+// configure whether OD access events shall be forwarded
+// to user callback function.
+// Because of reentrancy for local OD accesses, this has to be disabled
+// when application resides in other address space as the stack (e.g. if
+// EplApiLinuxUser.c and EplApiLinuxKernel.c are used)
+#ifndef EPL_API_OBD_FORWARD_EVENT
+#define EPL_API_OBD_FORWARD_EVENT       TRUE
+#endif
+
+#ifndef EPL_OBD_MAX_STRING_SIZE
+#define EPL_OBD_MAX_STRING_SIZE        32	// is used for objects 0x1008/0x1009/0x100A
+#endif
+
+#ifndef EPL_OBD_USE_STORE_RESTORE
+#define EPL_OBD_USE_STORE_RESTORE       FALSE
+#endif
+
+#ifndef EPL_OBD_CHECK_OBJECT_RANGE
+#define EPL_OBD_CHECK_OBJECT_RANGE      TRUE
+#endif
+
+#ifndef EPL_OBD_USE_STRING_DOMAIN_IN_RAM
+#define EPL_OBD_USE_STRING_DOMAIN_IN_RAM    TRUE
+#endif
+
+#ifndef EPL_OBD_USE_VARIABLE_SUBINDEX_TAB
+#define EPL_OBD_USE_VARIABLE_SUBINDEX_TAB   TRUE
+#endif
+
+#ifndef EPL_OBD_USE_KERNEL
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) == 0)
+#define EPL_OBD_USE_KERNEL                  TRUE
+#else
+#define EPL_OBD_USE_KERNEL                  FALSE
+#endif
+#endif
+
+#ifndef EPL_OBD_INCLUDE_A000_TO_DEVICE_PART
+#define EPL_OBD_INCLUDE_A000_TO_DEVICE_PART FALSE
+#endif
+
+#ifndef EPL_VETH_NAME
+#define EPL_VETH_NAME       "epl"	// name of net device in Linux
+#endif
+
+/*
+#define EPL_D_CFG_ConfigManager_BOOL // Ability of a MN node to perform Configuration Manager functions BOOLEAN O - N -
+#define EPL_D_CFM_VerifyConf_BOOL   // Support of objects CFM_VerifyConfiguration_REC, CFM_ExpConfDateList_AU32, CFM_ExpConfTimeList_AU32 BOOLEAN O O N N
+#define EPL_D_CFM_VerifyConfId_BOOL // Support of objects CFM_VerifyConfiguration_REC.ConfId_U32 and CFM_ExpConfIdList_AU32 BOOLEAN O O N N
+#define EPL_D_DLL_CNFeatureIsochr_BOOL // CN’s ability to perform isochronous functions BOOLEAN - O - Y
+#define EPL_D_DLL_CNFeatureMultiplex_BOOL // node’s ability to perform control of multiplexed isochronous communication BOOLEAN - O - N
+#define EPL_D_DLL_FeatureCN_BOOL // node’s ability to perform CN functions BOOLEAN O O Y Y
+#define EPL_D_DLL_FeatureMN_BOOL // node’s ability to perform MN functions BOOLEAN M O - N
+#define EPL_D_DLL_MNFeatureMultiplex_BOOL // MN’s ability to perform control of multiplexed isochronous communication BOOLEAN O - Y -
+#define EPL_D_DLL_MNFeaturePResTx_BOOL // MN’s ability to transmit PRes BOOLEAN O - Y -
+#define EPL_D_NMT_ASndRxMaxPayload_U16 // size of ASnd frame receive buffer UNSIGNED16 M M - -
+#define EPL_D_NMT_ASndTxMaxPayload_U16 // size of ASnd frame transmit buffer UNSIGNED16 M M - -
+#define EPL_D_NMT_CNASnd2SoC_U32 // minimum delay between end of reception of ASnd and start of reception of SoC UNSIGNED32 - M - -
+#define EPL_D_NMT_CNASndMaxLatency_U32 // delay between end of SoA reception and start of ASnd transmission UNSIGNED32 - M - -
+#define EPL_D_NMT_CNPResMaxLatency_U32 // delay between end of PReq reception and start of PRes transmission UNSIGNED32 - M - -
+#define EPL_D_NMT_CNSoC2PReq_U32 // CN SoC handling maximum time, a subsequent PReq won’t be handled before SoC handling was finished UNSIGNED32 - M - -
+#define EPL_D_NMT_DeviceType_U32 // Device Type ID UNSIGNED32 M M - -
+#define EPL_D_NMT_EPLVers_U8 EPL // Version implemented by the device UNSIGNED8 M M - -
+#define EPL_D_NMT_ExtStateCmd_BOOL // abitilty to support Extended NMT State Commands BOOLEAN O O Y Y
+#define EPL_D_NMT_InfoSvc_BOOL // ability to support NMT Info Services BOOLEAN O - Y -
+#define EPL_D_NMT_InterfaceAddr_Xh_OSTR // Physical Address of Interface No. Xh OCTET_STRING M M - -
+#define EPL_D_NMT_InterfaceDescr_Xh_VSTR // Description text of Interface No. Xh VISIBLE_STRINGM M - -
+#define EPL_D_NMT_InterfaceMtu_Xh_U32 // MTU of Interface No. Xh UNSIGNED32 M M - -
+#define EPL_D_NMT_InterfaceType_Xh_U8 // Type of Interface No. Xh UNSIGNED8 M M - -
+#define EPL_D_NMT_IsochrRxMaxPayload_U16 // size of isochronous frame receive buffer UNSIGNED16 M M - -
+#define EPL_D_NMT_IsochrTxMaxPayload_U16 // size of isochronous frame transmit buffer UNSIGNED16 M M - -
+#define EPL_D_NMT_ManufactDevName_VS // Manufacturer Device Name VISIBLE_STRING O O - -
+#define EPL_D_NMT_ManufactHwVers_VS // Manufacturer HW version VISIBLE_STRING O O - -
+#define EPL_D_NMT_ManufactSwVers_VS // Manufacturer SW version VISIBLE_STRING O O - -
+#define EPL_D_NMT_MaxCNNodeID_U8 // maximum Node ID available for regular CNs the entry provides an upper limit to the NodeID available for cross traffic PDO reception from a regular CN UNSIGNED8 O O 239 239
+#define EPL_D_NMT_MaxCNNumber_U8 // maximum number of supported regular CNs in the Node ID range 1 .. 239 UNSIGNED8 O O 239 239
+#define EPL_D_NMT_MaxHeartbeats_U8 // number of guard channels UNSIGNED8 O O 254 254
+#define EPL_D_NMT_MNASnd2SoC_U32 // minimum delay between end of reception of ASnd and start of transmission of SoC UNSIGNED32 M - - -
+#define EPL_D_NMT_MNMultiplCycMax_U8 // maximum number of EPL cycles per multiplexed cycle UNSIGNED8 O - 0 -
+#define EPL_D_NMT_MNPRes2PReq_U32 // delay between end of PRes reception and start of PReq transmission UNSIGNED32 M - - -
+#define EPL_D_NMT_MNPRes2PRes_U32 // delay between end of reception of PRes from CNn and start of transmission of PRes by MN UNSIGNED32 M - - -
+#define EPL_D_NMT_MNPResRx2SoA_U32 // delay between end of reception of PRes from CNn and start of transmission of SoA by MN UNSIGNED32 M - - -
+#define EPL_D_NMT_MNPResTx2SoA_U32 // delay between end of PRes transmission by MN and start of transmission of SoA by MN UNSIGNED32 M - - -
+#define EPL_D_NMT_MNSoA2ASndTx_U32 // delay between end of transmission of SoA and start of transmission of ASnd by MN UNSIGNED32 M - - -
+#define EPL_D_NMT_MNSoC2PReq_U32 // MN minimum delay between end of SoC transmission and start of PReq transmission UNSIGNED32 M - - -
+#define EPL_D_NMT_NMTSvcViaUDPIP_BOOL // Ability of a node to perform NMT services via UDP/IP BOOLEAN O - Y -
+#define EPL_D_NMT_NodeIDByHW_BOOL // Ability of a node to support NodeID setup by HW BOOLEAN O O Y Y
+#define EPL_D_NMT_NodeIDBySW_BOOL // Ability of a node to support NodeID setup by SW BOOLEAN O O N N
+#define EPL_D_NMT_ProductCode_U32 // Identity Object Product Code UNSIGNED32 M M - -
+#define EPL_D_NMT_RevisionNo_U32 // Identity Object Revision Number UNSIGNED32 M M - -
+#define EPL_D_NMT_SerialNo_U32 // Identity Object Serial Number UNSIGNED32 M M - -
+#define EPL_D_NMT_SimpleBoot_BOOL // Ability of a MN node to perform Simple Boot Process, if not set Indivual Boot Process shall be proviced BOOLEAN M - - -
+#define EPL_D_NMT_VendorID_U32 // Identity Object Vendor ID UNSIGNED32 M M - -
+#define EPL_D_NWL_Forward_BOOL // Ability of node to forward datagrams BOOLEAN O O N N
+#define EPL_D_NWL_IPSupport_BOOL // Ability of the node cummunicate via IP BOOLEAN - - Y Y
+#define EPL_D_PDO_DynamicMapping_BOOL // Ability of a node to perform dynamic PDO mapping BOOLEAN O O Y Y
+#define EPL_D_PDO_MaxDescrMem_U32 // maximum cumulative memory consumption of TPDO and RPDO describing objects in byte UNSIGNED32 O O MAX_U32 MAX_U32
+#define EPL_D_PDO_RPDOChannels_U8 // number of supported RPDO channels UNSIGNED8 O O 256 256
+#define EPL_D_PDO_RPDOMaxMem_U32 // Maximum memory available for RPDO data per EPL cycle in byte UNSIGNED32 O O MAX_U32 MAX_U32
+#define EPL_D_PDO_RPDOObjects_U8 // Number of supported mapped objects per RPDO channel UNSIGNED8 O O 254 254
+#define EPL_D_PDO_TPDOChannels_U8 // number of supported TPDO channels UNSIGNED8 O - 256 -
+#define EPL_D_PDO_TPDOMaxMem_U32 // Maximum memory available for TPDO data per EPL cycle in byte UNSIGNED32 O O MAX_U32 MAX_U32
+#define EPL_D_PDO_TPDOObjects_U8 // Number of supported mapped objects per TPDO channel UNSIGNED8 O O 254 254
+#define EPL_D_SDO_ViaASnd_BOOL // Ability of a CN to perform SDO transfer by EPL ASnd BOOLEAN - M - -
+#define EPL_D_SDO_ViaPDO_BOOL // Ability of a node to perform SDO transfer by PDO BOOLEAN O O N N
+#define EPL_D_SDO_ViaUDPIP_BOOL // Ability of a CN to perform SDO transfer by UDP/IP BOOLEAN - M - -
+#define EPL_D_SYN_OptimizedSync_BOOL // Ability of node to perform optimized synchronisation BOOLEAN O O N N
+*/
+
+// Emergency error codes
+// ======================
+#define EPL_E_NO_ERROR                  0x0000
+// 0xFxxx manufacturer specific error codes
+#define EPL_E_NMT_NO_IDENT_RES          0xF001
+#define EPL_E_NMT_NO_STATUS_RES         0xF002
+
+// 0x816x HW errors
+#define EPL_E_DLL_BAD_PHYS_MODE         0x8161
+#define EPL_E_DLL_COLLISION             0x8162
+#define EPL_E_DLL_COLLISION_TH          0x8163
+#define EPL_E_DLL_CRC_TH                0x8164
+#define EPL_E_DLL_LOSS_OF_LINK          0x8165
+#define EPL_E_DLL_MAC_BUFFER            0x8166
+// 0x82xx Protocol errors
+#define EPL_E_DLL_ADDRESS_CONFLICT      0x8201
+#define EPL_E_DLL_MULTIPLE_MN           0x8202
+// 0x821x Frame size errors
+#define EPL_E_PDO_SHORT_RX              0x8210
+#define EPL_E_PDO_MAP_VERS              0x8211
+#define EPL_E_NMT_ASND_MTU_DIF          0x8212
+#define EPL_E_NMT_ASND_MTU_LIM          0x8213
+#define EPL_E_NMT_ASND_TX_LIM           0x8214
+// 0x823x Timing errors
+#define EPL_E_NMT_CYCLE_LEN             0x8231
+#define EPL_E_DLL_CYCLE_EXCEED          0x8232
+#define EPL_E_DLL_CYCLE_EXCEED_TH       0x8233
+#define EPL_E_NMT_IDLE_LIM              0x8234
+#define EPL_E_DLL_JITTER_TH             0x8235
+#define EPL_E_DLL_LATE_PRES_TH          0x8236
+#define EPL_E_NMT_PREQ_CN               0x8237
+#define EPL_E_NMT_PREQ_LIM              0x8238
+#define EPL_E_NMT_PRES_CN               0x8239
+#define EPL_E_NMT_PRES_RX_LIM           0x823A
+#define EPL_E_NMT_PRES_TX_LIM           0x823B
+// 0x824x Frame errors
+#define EPL_E_DLL_INVALID_FORMAT        0x8241
+#define EPL_E_DLL_LOSS_PREQ_TH          0x8242
+#define EPL_E_DLL_LOSS_PRES_TH          0x8243
+#define EPL_E_DLL_LOSS_SOA_TH           0x8244
+#define EPL_E_DLL_LOSS_SOC_TH           0x8245
+// 0x84xx BootUp Errors
+#define EPL_E_NMT_BA1                   0x8410	// other MN in MsNotActive active
+#define EPL_E_NMT_BA1_NO_MN_SUPPORT     0x8411	// MN is not supported
+#define EPL_E_NMT_BPO1                  0x8420	// mandatory CN was not found or failed in BootStep1
+#define EPL_E_NMT_BPO1_GET_IDENT        0x8421	// IdentRes was not received
+#define EPL_E_NMT_BPO1_DEVICE_TYPE      0x8422	// wrong device type
+#define EPL_E_NMT_BPO1_VENDOR_ID        0x8423	// wrong vendor ID
+#define EPL_E_NMT_BPO1_PRODUCT_CODE     0x8424	// wrong product code
+#define EPL_E_NMT_BPO1_REVISION_NO      0x8425	// wrong revision number
+#define EPL_E_NMT_BPO1_SERIAL_NO        0x8426	// wrong serial number
+#define EPL_E_NMT_BPO1_CF_VERIFY        0x8428	// verification of configuration failed
+#define EPL_E_NMT_BPO2                  0x8430	// mandatory CN failed in BootStep2
+#define EPL_E_NMT_BRO                   0x8440	// CheckCommunication failed for mandatory CN
+#define EPL_E_NMT_WRONG_STATE           0x8480	// mandatory CN has wrong NMT state
+
+// Defines for object 0x1F80 NMT_StartUp_U32
+// ==========================================
+#define EPL_NMTST_STARTALLNODES         0x00000002L	// Bit 1
+#define EPL_NMTST_NO_AUTOSTART          0x00000004L	// Bit 2
+#define EPL_NMTST_NO_STARTNODE          0x00000008L	// Bit 3
+#define EPL_NMTST_RESETALL_MAND_CN      0x00000010L	// Bit 4
+#define EPL_NMTST_STOPALL_MAND_CN       0x00000040L	// Bit 6
+#define EPL_NMTST_NO_AUTOPREOP2         0x00000080L	// Bit 7
+#define EPL_NMTST_NO_AUTOREADYTOOP      0x00000100L	// Bit 8
+#define EPL_NMTST_EXT_CNIDENTCHECK      0x00000200L	// Bit 9
+#define EPL_NMTST_SWVERSIONCHECK        0x00000400L	// Bit 10
+#define EPL_NMTST_CONFCHECK             0x00000800L	// Bit 11
+#define EPL_NMTST_NO_RETURN_PREOP1      0x00001000L	// Bit 12
+#define EPL_NMTST_BASICETHERNET         0x00002000L	// Bit 13
+
+// Defines for object 0x1F81 NMT_NodeAssignment_AU32
+// ==================================================
+#define EPL_NODEASSIGN_NODE_EXISTS      0x00000001L	// Bit 0
+#define EPL_NODEASSIGN_NODE_IS_CN       0x00000002L	// Bit 1
+#define EPL_NODEASSIGN_START_CN         0x00000004L	// Bit 2
+#define EPL_NODEASSIGN_MANDATORY_CN     0x00000008L	// Bit 3
+#define EPL_NODEASSIGN_KEEPALIVE        0x00000010L	//currently not used in EPL V2 standard
+#define EPL_NODEASSIGN_SWVERSIONCHECK   0x00000020L	// Bit 5
+#define EPL_NODEASSIGN_SWUPDATE         0x00000040L	// Bit 6
+#define EPL_NODEASSIGN_ASYNCONLY_NODE   0x00000100L	// Bit 8
+#define EPL_NODEASSIGN_MULTIPLEXED_CN   0x00000200L	// Bit 9
+#define EPL_NODEASSIGN_RT1              0x00000400L	// Bit 10
+#define EPL_NODEASSIGN_RT2              0x00000800L	// Bit 11
+#define EPL_NODEASSIGN_MN_PRES          0x00001000L	// Bit 12
+#define EPL_NODEASSIGN_VALID            0x80000000L	// Bit 31
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_DEF_H_
diff --git a/drivers/staging/epl/EplDll.h b/drivers/staging/epl/EplDll.h
new file mode 100644
index 000000000000..36657f2daf8b
--- /dev/null
+++ b/drivers/staging/epl/EplDll.h
@@ -0,0 +1,205 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for DLL module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDll.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/08 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_DLL_H_
+#define _EPL_DLL_H_
+
+#include "EplInc.h"
+#include "EplFrame.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef EPL_DLL_MAX_ASND_SERVICE_ID
+#define EPL_DLL_MAX_ASND_SERVICE_ID (EPL_C_DLL_MAX_ASND_SERVICE_IDS + 1)	// last is kEplDllAsndSdo == 5
+#endif
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef enum {
+	kEplDllAsndNotDefined = 0x00,
+	kEplDllAsndIdentResponse = 0x01,
+	kEplDllAsndStatusResponse = 0x02,
+	kEplDllAsndNmtRequest = 0x03,
+	kEplDllAsndNmtCommand = 0x04,
+	kEplDllAsndSdo = 0x05
+} tEplDllAsndServiceId;
+
+typedef enum {
+	kEplDllAsndFilterNone = 0x00,
+	kEplDllAsndFilterLocal = 0x01,	// receive only ASnd frames with local or broadcast node ID
+	kEplDllAsndFilterAny = 0x02,	// receive any ASnd frame
+} tEplDllAsndFilter;
+
+typedef enum {
+	kEplDllReqServiceNo = 0x00,
+	kEplDllReqServiceIdent = 0x01,
+	kEplDllReqServiceStatus = 0x02,
+	kEplDllReqServiceNmtRequest = 0x03,
+	kEplDllReqServiceUnspecified = 0xFF,
+
+} tEplDllReqServiceId;
+
+typedef enum {
+	kEplDllAsyncReqPrioNmt = 0x07,	// PRIO_NMT_REQUEST
+	kEplDllAsyncReqPrio6 = 0x06,
+	kEplDllAsyncReqPrio5 = 0x05,
+	kEplDllAsyncReqPrio4 = 0x04,
+	kEplDllAsyncReqPrioGeneric = 0x03,	// PRIO_GENERIC_REQUEST
+	kEplDllAsyncReqPrio2 = 0x02,	// till WSP 0.1.3: PRIO_ABOVE_GENERIC
+	kEplDllAsyncReqPrio1 = 0x01,	// till WSP 0.1.3: PRIO_BELOW_GENERIC
+	kEplDllAsyncReqPrio0 = 0x00,	// till WSP 0.1.3: PRIO_GENERIC_REQUEST
+
+} tEplDllAsyncReqPriority;
+
+typedef struct {
+	unsigned int m_uiFrameSize;
+	tEplFrame *m_pFrame;
+	tEplNetTime m_NetTime;
+
+} tEplFrameInfo;
+
+typedef struct {
+	unsigned int m_uiSizeOfStruct;
+	BOOL m_fAsyncOnly;	// do not need to register PRes-Frame
+	unsigned int m_uiNodeId;	// local node ID
+
+	// 0x1F82: NMT_FeatureFlags_U32
+	DWORD m_dwFeatureFlags;
+	// Cycle Length (0x1006: NMT_CycleLen_U32) in [us]
+	DWORD m_dwCycleLen;	// required for error detection
+	// 0x1F98: NMT_CycleTiming_REC
+	// 0x1F98.1: IsochrTxMaxPayload_U16
+	unsigned int m_uiIsochrTxMaxPayload;	// const
+	// 0x1F98.2: IsochrRxMaxPayload_U16
+	unsigned int m_uiIsochrRxMaxPayload;	// const
+	// 0x1F98.3: PResMaxLatency_U32
+	DWORD m_dwPresMaxLatency;	// const in [ns], only required for IdentRes
+	// 0x1F98.4: PReqActPayloadLimit_U16
+	unsigned int m_uiPreqActPayloadLimit;	// required for initialisation (+24 bytes)
+	// 0x1F98.5: PResActPayloadLimit_U16
+	unsigned int m_uiPresActPayloadLimit;	// required for initialisation of Pres frame (+24 bytes)
+	// 0x1F98.6: ASndMaxLatency_U32
+	DWORD m_dwAsndMaxLatency;	// const in [ns], only required for IdentRes
+	// 0x1F98.7: MultiplCycleCnt_U8
+	unsigned int m_uiMultiplCycleCnt;	// required for error detection
+	// 0x1F98.8: AsyncMTU_U16
+	unsigned int m_uiAsyncMtu;	// required to set up max frame size
+	// $$$ 0x1F98.9: Prescaler_U16
+	// $$$ Multiplexed Slot
+
+	// 0x1C14: DLL_LossOfFrameTolerance_U32 in [ns]
+	DWORD m_dwLossOfFrameTolerance;
+
+	// 0x1F8A: NMT_MNCycleTiming_REC
+	// 0x1F8A.1: WaitSoCPReq_U32 in [ns]
+	DWORD m_dwWaitSocPreq;
+
+	// 0x1F8A.2: AsyncSlotTimeout_U32 in [ns]
+	DWORD m_dwAsyncSlotTimeout;
+
+} tEplDllConfigParam;
+
+typedef struct {
+	unsigned int m_uiSizeOfStruct;
+	DWORD m_dwDeviceType;	// NMT_DeviceType_U32
+	DWORD m_dwVendorId;	// NMT_IdentityObject_REC.VendorId_U32
+	DWORD m_dwProductCode;	// NMT_IdentityObject_REC.ProductCode_U32
+	DWORD m_dwRevisionNumber;	// NMT_IdentityObject_REC.RevisionNo_U32
+	DWORD m_dwSerialNumber;	// NMT_IdentityObject_REC.SerialNo_U32
+	QWORD m_qwVendorSpecificExt1;
+	DWORD m_dwVerifyConfigurationDate;	// CFM_VerifyConfiguration_REC.ConfDate_U32
+	DWORD m_dwVerifyConfigurationTime;	// CFM_VerifyConfiguration_REC.ConfTime_U32
+	DWORD m_dwApplicationSwDate;	// PDL_LocVerApplSw_REC.ApplSwDate_U32 on programmable device or date portion of NMT_ManufactSwVers_VS on non-programmable device
+	DWORD m_dwApplicationSwTime;	// PDL_LocVerApplSw_REC.ApplSwTime_U32 on programmable device or time portion of NMT_ManufactSwVers_VS on non-programmable device
+	DWORD m_dwIpAddress;
+	DWORD m_dwSubnetMask;
+	DWORD m_dwDefaultGateway;
+	BYTE m_sHostname[32];
+	BYTE m_abVendorSpecificExt2[48];
+
+} tEplDllIdentParam;
+
+typedef struct {
+	unsigned int m_uiNodeId;
+	WORD m_wPreqPayloadLimit;	// object 0x1F8B: NMT_MNPReqPayloadLimitList_AU16
+	WORD m_wPresPayloadLimit;	// object 0x1F8D: NMT_PResPayloadLimitList_AU16
+	DWORD m_dwPresTimeout;	// object 0x1F92: NMT_MNCNPResTimeout_AU32
+
+} tEplDllNodeInfo;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_DLL_H_
diff --git a/drivers/staging/epl/EplDllCal.h b/drivers/staging/epl/EplDllCal.h
new file mode 100644
index 000000000000..24460087e987
--- /dev/null
+++ b/drivers/staging/epl/EplDllCal.h
@@ -0,0 +1,123 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for DLL Communication Abstraction Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDllCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/20 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_DLLCAL_H_
+#define _EPL_DLLCAL_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+/*#ifndef EPL_DLLCAL_BUFFER_ID_RX
+#define EPL_DLLCAL_BUFFER_ID_RX    "EplSblDllCalRx"
+#endif
+
+#ifndef EPL_DLLCAL_BUFFER_SIZE_RX
+#define EPL_DLLCAL_BUFFER_SIZE_RX  32767
+#endif
+*/
+#ifndef EPL_DLLCAL_BUFFER_ID_TX_NMT
+#define EPL_DLLCAL_BUFFER_ID_TX_NMT     "EplSblDllCalTxNmt"
+#endif
+
+#ifndef EPL_DLLCAL_BUFFER_SIZE_TX_NMT
+#define EPL_DLLCAL_BUFFER_SIZE_TX_NMT   32767
+#endif
+
+#ifndef EPL_DLLCAL_BUFFER_ID_TX_GEN
+#define EPL_DLLCAL_BUFFER_ID_TX_GEN     "EplSblDllCalTxGen"
+#endif
+
+#ifndef EPL_DLLCAL_BUFFER_SIZE_TX_GEN
+#define EPL_DLLCAL_BUFFER_SIZE_TX_GEN   32767
+#endif
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef struct {
+	tEplDllAsndServiceId m_ServiceId;
+	tEplDllAsndFilter m_Filter;
+
+} tEplDllCalAsndServiceIdFilter;
+
+typedef struct {
+	tEplDllReqServiceId m_Service;
+	unsigned int m_uiNodeId;
+	BYTE m_bSoaFlag1;
+
+} tEplDllCalIssueRequest;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_DLLKCAL_H_
diff --git a/drivers/staging/epl/EplDllk.c b/drivers/staging/epl/EplDllk.c
new file mode 100644
index 000000000000..9e22641055c6
--- /dev/null
+++ b/drivers/staging/epl/EplDllk.c
@@ -0,0 +1,4054 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for kernel DLL module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDllk.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.21 $  $Date: 2008/11/13 17:13:09 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/12 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "kernel/EplDllk.h"
+#include "kernel/EplDllkCal.h"
+#include "kernel/EplEventk.h"
+#include "kernel/EplNmtk.h"
+#include "edrv.h"
+#include "Benchmark.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+#include "kernel/EplPdok.h"
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_VETH)) != 0)
+#include "kernel/VirtualEthernet.h"
+#endif
+
+//#if EPL_TIMER_USE_HIGHRES != FALSE
+#include "kernel/EplTimerHighResk.h"
+//#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) == 0)
+#error "EPL module DLLK needs EPL module NMTK!"
+#endif
+
+#if (EPL_DLL_PRES_READY_AFTER_SOA != FALSE) && (EPL_DLL_PRES_READY_AFTER_SOC != FALSE)
+#error "EPL module DLLK: select only one of EPL_DLL_PRES_READY_AFTER_SOA and EPL_DLL_PRES_READY_AFTER_SOC."
+#endif
+
+#if ((EPL_DLL_PRES_READY_AFTER_SOA != FALSE) || (EPL_DLL_PRES_READY_AFTER_SOC != FALSE)) \
+    && (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) == 0)
+#error "EPL module DLLK: currently, EPL_DLL_PRES_READY_AFTER_* is not supported if EPL_MODULE_NMT_MN is enabled."
+#endif
+
+#if (EDRV_FAST_TXFRAMES == FALSE) && \
+    ((EPL_DLL_PRES_READY_AFTER_SOA != FALSE) || (EPL_DLL_PRES_READY_AFTER_SOC != FALSE))
+#error "EPL module DLLK: EPL_DLL_PRES_READY_AFTER_* is enabled, but not EDRV_FAST_TXFRAMES."
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)     TgtDbgPostTraceValue(v)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)
+#endif
+#define EPL_DLLK_DBG_POST_TRACE_VALUE(Event_p, uiNodeId_p, wErrorCode_p) \
+    TGT_DBG_POST_TRACE_VALUE((kEplEventSinkDllk << 28) | (Event_p << 24) \
+                             | (uiNodeId_p << 16) | wErrorCode_p)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplDllk                                             */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// defines for indexes of tEplDllInstance.m_pTxFrameInfo
+#define EPL_DLLK_TXFRAME_IDENTRES   0	// IdentResponse on CN / MN
+#define EPL_DLLK_TXFRAME_STATUSRES  1	// StatusResponse on CN / MN
+#define EPL_DLLK_TXFRAME_NMTREQ     2	// NMT Request from FIFO on CN / MN
+#define EPL_DLLK_TXFRAME_NONEPL     3	// non-EPL frame from FIFO on CN / MN
+#define EPL_DLLK_TXFRAME_PRES       4	// PRes on CN / MN
+#define EPL_DLLK_TXFRAME_SOC        5	// SoC on MN
+#define EPL_DLLK_TXFRAME_SOA        6	// SoA on MN
+#define EPL_DLLK_TXFRAME_PREQ       7	// PReq on MN
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+#define EPL_DLLK_TXFRAME_COUNT      (7 + EPL_D_NMT_MaxCNNumber_U8 + 2)	// on MN: 7 + MaxPReq of regular CNs + 1 Diag + 1 Router
+#else
+#define EPL_DLLK_TXFRAME_COUNT      5	// on CN: 5
+#endif
+
+#define EPL_DLLK_BUFLEN_EMPTY       0	// buffer is empty
+#define EPL_DLLK_BUFLEN_FILLING     1	// just the buffer is being filled
+#define EPL_DLLK_BUFLEN_MIN         60	// minimum ethernet frame length
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef enum {
+	kEplDllGsInit = 0x00,	// MN/CN: initialisation (< PreOp2)
+	kEplDllCsWaitPreq = 0x01,	// CN: wait for PReq frame
+	kEplDllCsWaitSoc = 0x02,	// CN: wait for SoC frame
+	kEplDllCsWaitSoa = 0x03,	// CN: wait for SoA frame
+	kEplDllMsNonCyclic = 0x04,	// MN: reduced EPL cycle (PreOp1)
+	kEplDllMsWaitSocTrig = 0x05,	// MN: wait for SoC trigger (cycle timer)
+	kEplDllMsWaitPreqTrig = 0x06,	// MN: wait for (first) PReq trigger (WaitSoCPReq_U32)
+	kEplDllMsWaitPres = 0x07,	// MN: wait for PRes frame from CN
+	kEplDllMsWaitSoaTrig = 0x08,	// MN: wait for SoA trigger (PRes transmitted)
+	kEplDllMsWaitAsndTrig = 0x09,	// MN: wait for ASnd trigger (SoA transmitted)
+	kEplDllMsWaitAsnd = 0x0A,	// MN: wait for ASnd frame if SoA contained invitation
+
+} tEplDllState;
+
+typedef struct {
+	BYTE m_be_abSrcMac[6];
+	tEdrvTxBuffer *m_pTxBuffer;	// Buffers for Tx-Frames
+	unsigned int m_uiMaxTxFrames;
+	BYTE m_bFlag1;		// Flag 1 with EN, EC for PRes, StatusRes
+	BYTE m_bMnFlag1;	// Flag 1 with EA, ER from PReq, SoA of MN
+	BYTE m_bFlag2;		// Flag 2 with PR and RS for PRes, StatusRes, IdentRes
+	tEplDllConfigParam m_DllConfigParam;
+	tEplDllIdentParam m_DllIdentParam;
+	tEplDllState m_DllState;
+	tEplDllkCbAsync m_pfnCbAsync;
+	tEplDllAsndFilter m_aAsndFilter[EPL_DLL_MAX_ASND_SERVICE_ID];
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	tEplDllkNodeInfo *m_pFirstNodeInfo;
+	tEplDllkNodeInfo *m_pCurNodeInfo;
+	tEplDllkNodeInfo m_aNodeInfo[EPL_NMT_MAX_NODE_ID];
+	tEplDllReqServiceId m_LastReqServiceId;
+	unsigned int m_uiLastTargetNodeId;
+#endif
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+	tEplTimerHdl m_TimerHdlCycle;	// used for EPL cycle monitoring on CN and generation on MN
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	tEplTimerHdl m_TimerHdlResponse;	// used for CN response monitoring
+#endif				//(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+#endif
+
+	unsigned int m_uiCycleCount;	// cycle counter (needed for multiplexed cycle support)
+	unsigned long long m_ullFrameTimeout;	// frame timeout (cycle length + loss of frame tolerance)
+
+} tEplDllkInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+// if no dynamic memory allocation shall be used
+// define structures statically
+static tEplDllkInstance EplDllkInstance_g;
+
+static tEdrvTxBuffer aEplDllkTxBuffer_l[EPL_DLLK_TXFRAME_COUNT];
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+// change DLL state on event
+static tEplKernel EplDllkChangeState(tEplNmtEvent NmtEvent_p,
+				     tEplNmtState NmtState_p);
+
+// called from EdrvInterruptHandler()
+static void EplDllkCbFrameReceived(tEdrvRxBuffer * pRxBuffer_p);
+
+// called from EdrvInterruptHandler()
+static void EplDllkCbFrameTransmitted(tEdrvTxBuffer * pTxBuffer_p);
+
+// check frame and set missing information
+static tEplKernel EplDllkCheckFrame(tEplFrame * pFrame_p,
+				    unsigned int uiFrameSize_p);
+
+// called by high resolution timer module to monitor EPL cycle as CN
+#if EPL_TIMER_USE_HIGHRES != FALSE
+static tEplKernel PUBLIC EplDllkCbCnTimer(tEplTimerEventArg * pEventArg_p);
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+// MN: returns internal node info structure
+static tEplDllkNodeInfo *EplDllkGetNodeInfo(unsigned int uiNodeId_p);
+
+// transmit SoA
+static tEplKernel EplDllkMnSendSoa(tEplNmtState NmtState_p,
+				   tEplDllState * pDllStateProposed_p,
+				   BOOL fEnableInvitation_p);
+
+static tEplKernel EplDllkMnSendSoc(void);
+
+static tEplKernel EplDllkMnSendPreq(tEplNmtState NmtState_p,
+				    tEplDllState * pDllStateProposed_p);
+
+static tEplKernel EplDllkAsyncFrameNotReceived(tEplDllReqServiceId
+					       ReqServiceId_p,
+					       unsigned int uiNodeId_p);
+
+static tEplKernel PUBLIC EplDllkCbMnTimerCycle(tEplTimerEventArg * pEventArg_p);
+
+static tEplKernel PUBLIC EplDllkCbMnTimerResponse(tEplTimerEventArg *
+						  pEventArg_p);
+
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkAddInstance()
+//
+// Description: add and initialize new instance of EPL stack
+//
+// Parameters:  pInitParam_p            = initialisation parameters like MAC address
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkAddInstance(tEplDllkInitParam * pInitParam_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiIndex;
+	tEdrvInitParam EdrvInitParam;
+
+	// reset instance structure
+	EPL_MEMSET(&EplDllkInstance_g, 0, sizeof(EplDllkInstance_g));
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+	Ret = EplTimerHighReskInit();
+	if (Ret != kEplSuccessful) {	// error occured while initializing high resolution timer module
+		goto Exit;
+	}
+#endif
+
+	// if dynamic memory allocation available
+	// allocate instance structure
+	// allocate TPDO and RPDO table with default size
+
+	// initialize and link pointers in instance structure to frame tables
+	EplDllkInstance_g.m_pTxBuffer = aEplDllkTxBuffer_l;
+	EplDllkInstance_g.m_uiMaxTxFrames =
+	    sizeof(aEplDllkTxBuffer_l) / sizeof(tEdrvTxBuffer);
+
+	// initialize state
+	EplDllkInstance_g.m_DllState = kEplDllGsInit;
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	// set up node info structure
+	for (uiIndex = 0; uiIndex < tabentries(EplDllkInstance_g.m_aNodeInfo);
+	     uiIndex++) {
+		EplDllkInstance_g.m_aNodeInfo[uiIndex].m_uiNodeId = uiIndex + 1;
+		EplDllkInstance_g.m_aNodeInfo[uiIndex].m_wPresPayloadLimit =
+		    0xFFFF;
+	}
+#endif
+
+	// initialize Edrv
+	EPL_MEMCPY(EdrvInitParam.m_abMyMacAddr, pInitParam_p->m_be_abSrcMac, 6);
+	EdrvInitParam.m_pfnRxHandler = EplDllkCbFrameReceived;
+	EdrvInitParam.m_pfnTxHandler = EplDllkCbFrameTransmitted;
+	Ret = EdrvInit(&EdrvInitParam);
+	if (Ret != kEplSuccessful) {	// error occured while initializing ethernet driver
+		goto Exit;
+	}
+	// copy local MAC address from Ethernet driver back to local instance structure
+	// because Ethernet driver may have read it from controller EEPROM
+	EPL_MEMCPY(EplDllkInstance_g.m_be_abSrcMac, EdrvInitParam.m_abMyMacAddr,
+		   6);
+	EPL_MEMCPY(pInitParam_p->m_be_abSrcMac, EdrvInitParam.m_abMyMacAddr, 6);
+
+	// initialize TxBuffer array
+	for (uiIndex = 0; uiIndex < EplDllkInstance_g.m_uiMaxTxFrames;
+	     uiIndex++) {
+		EplDllkInstance_g.m_pTxBuffer[uiIndex].m_pbBuffer = NULL;
+	}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_VETH)) != 0)
+	Ret = VEthAddInstance(pInitParam_p);
+#endif
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkDelInstance()
+//
+// Description: deletes an instance of EPL stack
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkDelInstance(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// reset state
+	EplDllkInstance_g.m_DllState = kEplDllGsInit;
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+	Ret = EplTimerHighReskDelInstance();
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_VETH)) != 0)
+	Ret = VEthDelInstance();
+#endif
+
+	Ret = EdrvShutdown();
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCreateTxFrame
+//
+// Description: creates the buffer for a Tx frame and registers it to the
+//              ethernet driver
+//
+// Parameters:  puiHandle_p             = OUT: handle to frame buffer
+//              ppFrame_p               = OUT: pointer to pointer of EPL frame
+//              puiFrameSize_p          = IN/OUT: pointer to size of frame
+//                                        returned size is always equal or larger than
+//                                        requested size, if that is not possible
+//                                        an error will be returned
+//              MsgType_p               = EPL message type
+//              ServiceId_p             = Service ID in case of ASnd frame, otherwise
+//                                        kEplDllAsndNotDefined
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCreateTxFrame(unsigned int *puiHandle_p,
+				tEplFrame ** ppFrame_p,
+				unsigned int *puiFrameSize_p,
+				tEplMsgType MsgType_p,
+				tEplDllAsndServiceId ServiceId_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplFrame *pTxFrame;
+	unsigned int uiHandle = EplDllkInstance_g.m_uiMaxTxFrames;
+	tEdrvTxBuffer *pTxBuffer = NULL;
+
+	if (MsgType_p == kEplMsgTypeAsnd) {
+		// search for fixed Tx buffers
+		if (ServiceId_p == kEplDllAsndIdentResponse) {
+			uiHandle = EPL_DLLK_TXFRAME_IDENTRES;
+		} else if (ServiceId_p == kEplDllAsndStatusResponse) {
+			uiHandle = EPL_DLLK_TXFRAME_STATUSRES;
+		} else if ((ServiceId_p == kEplDllAsndNmtRequest)
+			   || (ServiceId_p == kEplDllAsndNmtCommand)) {
+			uiHandle = EPL_DLLK_TXFRAME_NMTREQ;
+		}
+
+		if (uiHandle >= EplDllkInstance_g.m_uiMaxTxFrames) {	// look for free entry
+			uiHandle = EPL_DLLK_TXFRAME_PREQ;
+			pTxBuffer = &EplDllkInstance_g.m_pTxBuffer[uiHandle];
+			for (; uiHandle < EplDllkInstance_g.m_uiMaxTxFrames;
+			     uiHandle++, pTxBuffer++) {
+				if (pTxBuffer->m_pbBuffer == NULL) {	// free entry found
+					break;
+				}
+			}
+		}
+	} else if (MsgType_p == kEplMsgTypeNonEpl) {
+		uiHandle = EPL_DLLK_TXFRAME_NONEPL;
+	} else if (MsgType_p == kEplMsgTypePres) {
+		uiHandle = EPL_DLLK_TXFRAME_PRES;
+	} else if (MsgType_p == kEplMsgTypeSoc) {
+		uiHandle = EPL_DLLK_TXFRAME_SOC;
+	} else if (MsgType_p == kEplMsgTypeSoa) {
+		uiHandle = EPL_DLLK_TXFRAME_SOA;
+	} else {		// look for free entry
+		uiHandle = EPL_DLLK_TXFRAME_PREQ;
+		pTxBuffer = &EplDllkInstance_g.m_pTxBuffer[uiHandle];
+		for (; uiHandle < EplDllkInstance_g.m_uiMaxTxFrames;
+		     uiHandle++, pTxBuffer++) {
+			if (pTxBuffer->m_pbBuffer == NULL) {	// free entry found
+				break;
+			}
+		}
+		if (pTxBuffer->m_pbBuffer != NULL) {
+			Ret = kEplEdrvNoFreeBufEntry;
+			goto Exit;
+		}
+	}
+
+	// test if requested entry is free
+	pTxBuffer = &EplDllkInstance_g.m_pTxBuffer[uiHandle];
+	if (pTxBuffer->m_pbBuffer != NULL) {	// entry is not free
+		Ret = kEplEdrvNoFreeBufEntry;
+		goto Exit;
+	}
+	// setup Tx buffer
+	pTxBuffer->m_EplMsgType = MsgType_p;
+	pTxBuffer->m_uiMaxBufferLen = *puiFrameSize_p;
+
+	Ret = EdrvAllocTxMsgBuffer(pTxBuffer);
+	if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+		goto Exit;
+	}
+	// because buffer size may be larger than requested
+	// memorize real length of frame
+	pTxBuffer->m_uiTxMsgLen = *puiFrameSize_p;
+
+	// fill whole frame with 0
+	EPL_MEMSET(pTxBuffer->m_pbBuffer, 0, pTxBuffer->m_uiMaxBufferLen);
+
+	pTxFrame = (tEplFrame *) pTxBuffer->m_pbBuffer;
+
+	if (MsgType_p != kEplMsgTypeNonEpl) {	// fill out Frame only if it is an EPL frame
+		// ethertype
+		AmiSetWordToBe(&pTxFrame->m_be_wEtherType,
+			       EPL_C_DLL_ETHERTYPE_EPL);
+		// source node ID
+		AmiSetByteToLe(&pTxFrame->m_le_bSrcNodeId,
+			       (BYTE) EplDllkInstance_g.m_DllConfigParam.
+			       m_uiNodeId);
+		// source MAC address
+		EPL_MEMCPY(&pTxFrame->m_be_abSrcMac[0],
+			   &EplDllkInstance_g.m_be_abSrcMac[0], 6);
+		switch (MsgType_p) {
+		case kEplMsgTypeAsnd:
+			// destination MAC address
+			AmiSetQword48ToBe(&pTxFrame->m_be_abDstMac[0],
+					  EPL_C_DLL_MULTICAST_ASND);
+			// destination node ID
+			switch (ServiceId_p) {
+			case kEplDllAsndIdentResponse:
+			case kEplDllAsndStatusResponse:
+				{	// IdentResponses and StatusResponses are Broadcast
+					AmiSetByteToLe(&pTxFrame->
+						       m_le_bDstNodeId,
+						       (BYTE)
+						       EPL_C_ADR_BROADCAST);
+					break;
+				}
+
+			default:
+				break;
+			}
+			// ASnd Service ID
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Asnd.m_le_bServiceId,
+				       ServiceId_p);
+			break;
+
+		case kEplMsgTypeSoc:
+			// destination MAC address
+			AmiSetQword48ToBe(&pTxFrame->m_be_abDstMac[0],
+					  EPL_C_DLL_MULTICAST_SOC);
+			// destination node ID
+			AmiSetByteToLe(&pTxFrame->m_le_bDstNodeId,
+				       (BYTE) EPL_C_ADR_BROADCAST);
+			// reset Flags
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Soc.m_le_bFlag1, (BYTE) 0);
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Soc.m_le_bFlag2, (BYTE) 0);
+			break;
+
+		case kEplMsgTypeSoa:
+			// destination MAC address
+			AmiSetQword48ToBe(&pTxFrame->m_be_abDstMac[0],
+					  EPL_C_DLL_MULTICAST_SOA);
+			// destination node ID
+			AmiSetByteToLe(&pTxFrame->m_le_bDstNodeId,
+				       (BYTE) EPL_C_ADR_BROADCAST);
+			// reset Flags
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.m_le_bFlag1, (BYTE) 0);
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.m_le_bFlag2, (BYTE) 0);
+			// EPL profile version
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.m_le_bEplVersion,
+				       (BYTE) EPL_SPEC_VERSION);
+			break;
+
+		case kEplMsgTypePres:
+			// destination MAC address
+			AmiSetQword48ToBe(&pTxFrame->m_be_abDstMac[0],
+					  EPL_C_DLL_MULTICAST_PRES);
+			// destination node ID
+			AmiSetByteToLe(&pTxFrame->m_le_bDstNodeId,
+				       (BYTE) EPL_C_ADR_BROADCAST);
+			// reset Flags
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.m_le_bFlag1, (BYTE) 0);
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.m_le_bFlag2, (BYTE) 0);
+			// PDO size
+			//AmiSetWordToLe(&pTxFrame->m_Data.m_Pres.m_le_wSize, 0);
+			break;
+
+		case kEplMsgTypePreq:
+			// reset Flags
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Preq.m_le_bFlag1, (BYTE) 0);
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Preq.m_le_bFlag2, (BYTE) 0);
+			// PDO size
+			//AmiSetWordToLe(&pTxFrame->m_Data.m_Preq.m_le_wSize, 0);
+			break;
+
+		default:
+			break;
+		}
+		// EPL message type
+		AmiSetByteToLe(&pTxFrame->m_le_bMessageType, (BYTE) MsgType_p);
+	}
+
+	*ppFrame_p = pTxFrame;
+	*puiFrameSize_p = pTxBuffer->m_uiMaxBufferLen;
+	*puiHandle_p = uiHandle;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkDeleteTxFrame
+//
+// Description: deletes the buffer for a Tx frame and frees it in the
+//              ethernet driver
+//
+// Parameters:  uiHandle_p              = IN: handle to frame buffer
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkDeleteTxFrame(unsigned int uiHandle_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEdrvTxBuffer *pTxBuffer = NULL;
+
+	if (uiHandle_p >= EplDllkInstance_g.m_uiMaxTxFrames) {	// handle is not valid
+		Ret = kEplDllIllegalHdl;
+		goto Exit;
+	}
+
+	pTxBuffer = &EplDllkInstance_g.m_pTxBuffer[uiHandle_p];
+
+	// mark buffer as free so that frame will not be send in future anymore
+	// $$$ d.k. What's up with running transmissions?
+	pTxBuffer->m_uiTxMsgLen = EPL_DLLK_BUFLEN_EMPTY;
+	pTxBuffer->m_pbBuffer = NULL;
+
+	// delete Tx buffer
+	Ret = EdrvReleaseTxMsgBuffer(pTxBuffer);
+	if (Ret != kEplSuccessful) {	// error occured while releasing Tx frame
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkProcess
+//
+// Description: process the passed event
+//
+// Parameters:  pEvent_p                = event to be processed
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkProcess(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplFrame *pTxFrame;
+	tEdrvTxBuffer *pTxBuffer;
+	unsigned int uiHandle;
+	unsigned int uiFrameSize;
+	BYTE abMulticastMac[6];
+	tEplDllAsyncReqPriority AsyncReqPriority;
+	unsigned int uiFrameCount;
+	tEplNmtState NmtState;
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+	tEplFrameInfo FrameInfo;
+#endif
+
+	switch (pEvent_p->m_EventType) {
+	case kEplEventTypeDllkCreate:
+		{
+			// $$$ reset ethernet driver
+
+			NmtState = *((tEplNmtState *) pEvent_p->m_pArg);
+
+			// initialize flags for PRes and StatusRes
+			EplDllkInstance_g.m_bFlag1 = EPL_FRAME_FLAG1_EC;
+			EplDllkInstance_g.m_bMnFlag1 = 0;
+			EplDllkInstance_g.m_bFlag2 = 0;
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			// initialize linked node list
+			EplDllkInstance_g.m_pFirstNodeInfo = NULL;
+#endif
+
+			// register TxFrames in Edrv
+
+			// IdentResponse
+			uiFrameSize = EPL_C_DLL_MINSIZE_IDENTRES;
+			Ret =
+			    EplDllkCreateTxFrame(&uiHandle, &pTxFrame,
+						 &uiFrameSize, kEplMsgTypeAsnd,
+						 kEplDllAsndIdentResponse);
+			if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+				goto Exit;
+			}
+			// EPL profile version
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+				       m_IdentResponse.m_le_bEplProfileVersion,
+				       (BYTE) EPL_SPEC_VERSION);
+			// FeatureFlags
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwFeatureFlags,
+					EplDllkInstance_g.m_DllConfigParam.
+					m_dwFeatureFlags);
+			// MTU
+			AmiSetWordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+				       m_IdentResponse.m_le_wMtu,
+				       (WORD) EplDllkInstance_g.
+				       m_DllConfigParam.m_uiAsyncMtu);
+			// PollInSize
+			AmiSetWordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+				       m_IdentResponse.m_le_wPollInSize,
+				       (WORD) EplDllkInstance_g.
+				       m_DllConfigParam.
+				       m_uiPreqActPayloadLimit);
+			// PollOutSize
+			AmiSetWordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+				       m_IdentResponse.m_le_wPollOutSize,
+				       (WORD) EplDllkInstance_g.
+				       m_DllConfigParam.
+				       m_uiPresActPayloadLimit);
+			// ResponseTime / PresMaxLatency
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwResponseTime,
+					EplDllkInstance_g.m_DllConfigParam.
+					m_dwPresMaxLatency);
+			// DeviceType
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwDeviceType,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwDeviceType);
+			// VendorId
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwVendorId,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwVendorId);
+			// ProductCode
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwProductCode,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwProductCode);
+			// RevisionNumber
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwRevisionNumber,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwRevisionNumber);
+			// SerialNumber
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwSerialNumber,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwSerialNumber);
+			// VendorSpecificExt1
+			AmiSetQword64ToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					  m_IdentResponse.
+					  m_le_qwVendorSpecificExt1,
+					  EplDllkInstance_g.m_DllIdentParam.
+					  m_qwVendorSpecificExt1);
+			// VerifyConfigurationDate
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.
+					m_le_dwVerifyConfigurationDate,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwVerifyConfigurationDate);
+			// VerifyConfigurationTime
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.
+					m_le_dwVerifyConfigurationTime,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwVerifyConfigurationTime);
+			// ApplicationSwDate
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.
+					m_le_dwApplicationSwDate,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwApplicationSwDate);
+			// ApplicationSwTime
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.
+					m_le_dwApplicationSwTime,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwApplicationSwTime);
+			// IPAddress
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwIpAddress,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwIpAddress);
+			// SubnetMask
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwSubnetMask,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwSubnetMask);
+			// DefaultGateway
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwDefaultGateway,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwDefaultGateway);
+			// HostName
+			EPL_MEMCPY(&pTxFrame->m_Data.m_Asnd.m_Payload.
+				   m_IdentResponse.m_le_sHostname[0],
+				   &EplDllkInstance_g.m_DllIdentParam.
+				   m_sHostname[0],
+				   sizeof(EplDllkInstance_g.m_DllIdentParam.
+					  m_sHostname));
+			// VendorSpecificExt2
+			EPL_MEMCPY(&pTxFrame->m_Data.m_Asnd.m_Payload.
+				   m_IdentResponse.m_le_abVendorSpecificExt2[0],
+				   &EplDllkInstance_g.m_DllIdentParam.
+				   m_abVendorSpecificExt2[0],
+				   sizeof(EplDllkInstance_g.m_DllIdentParam.
+					  m_abVendorSpecificExt2));
+
+			// StatusResponse
+			uiFrameSize = EPL_C_DLL_MINSIZE_STATUSRES;
+			Ret =
+			    EplDllkCreateTxFrame(&uiHandle, &pTxFrame,
+						 &uiFrameSize, kEplMsgTypeAsnd,
+						 kEplDllAsndStatusResponse);
+			if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+				goto Exit;
+			}
+			// PRes $$$ maybe move this to PDO module
+			if ((EplDllkInstance_g.m_DllConfigParam.m_fAsyncOnly ==
+			     FALSE)
+			    && (EplDllkInstance_g.m_DllConfigParam.m_uiPresActPayloadLimit >= 36)) {	// it is not configured as async-only CN,
+				// so take part in isochronous phase and register PRes frame
+				uiFrameSize =
+				    EplDllkInstance_g.m_DllConfigParam.
+				    m_uiPresActPayloadLimit + 24;
+				Ret =
+				    EplDllkCreateTxFrame(&uiHandle, &pTxFrame,
+							 &uiFrameSize,
+							 kEplMsgTypePres,
+							 kEplDllAsndNotDefined);
+				if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+					goto Exit;
+				}
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+				// initially encode TPDO -> inform PDO module
+				FrameInfo.m_pFrame = pTxFrame;
+				FrameInfo.m_uiFrameSize = uiFrameSize;
+				Ret = EplPdokCbPdoTransmitted(&FrameInfo);
+#endif
+				// reset cycle counter
+				EplDllkInstance_g.m_uiCycleCount = 0;
+			} else {	// it is an async-only CN
+				// fool EplDllkChangeState() to think that PRes was not expected
+				EplDllkInstance_g.m_uiCycleCount = 1;
+			}
+
+			// NMT request
+			uiFrameSize = EPL_C_IP_MAX_MTU;
+			Ret =
+			    EplDllkCreateTxFrame(&uiHandle, &pTxFrame,
+						 &uiFrameSize, kEplMsgTypeAsnd,
+						 kEplDllAsndNmtRequest);
+			if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+				goto Exit;
+			}
+			// mark Tx buffer as empty
+			EplDllkInstance_g.m_pTxBuffer[uiHandle].m_uiTxMsgLen =
+			    EPL_DLLK_BUFLEN_EMPTY;
+
+			// non-EPL frame
+			uiFrameSize = EPL_C_IP_MAX_MTU;
+			Ret =
+			    EplDllkCreateTxFrame(&uiHandle, &pTxFrame,
+						 &uiFrameSize,
+						 kEplMsgTypeNonEpl,
+						 kEplDllAsndNotDefined);
+			if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+				goto Exit;
+			}
+			// mark Tx buffer as empty
+			EplDllkInstance_g.m_pTxBuffer[uiHandle].m_uiTxMsgLen =
+			    EPL_DLLK_BUFLEN_EMPTY;
+
+			// register multicast MACs in ethernet driver
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_SOC);
+			Ret = EdrvDefineRxMacAddrEntry(abMulticastMac);
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_SOA);
+			Ret = EdrvDefineRxMacAddrEntry(abMulticastMac);
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_PRES);
+			Ret = EdrvDefineRxMacAddrEntry(abMulticastMac);
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_ASND);
+			Ret = EdrvDefineRxMacAddrEntry(abMulticastMac);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			if (NmtState >= kEplNmtMsNotActive) {	// local node is MN
+				unsigned int uiIndex;
+
+				// SoC
+				uiFrameSize = EPL_C_DLL_MINSIZE_SOC;
+				Ret =
+				    EplDllkCreateTxFrame(&uiHandle, &pTxFrame,
+							 &uiFrameSize,
+							 kEplMsgTypeSoc,
+							 kEplDllAsndNotDefined);
+				if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+					goto Exit;
+				}
+				// SoA
+				uiFrameSize = EPL_C_DLL_MINSIZE_SOA;
+				Ret =
+				    EplDllkCreateTxFrame(&uiHandle, &pTxFrame,
+							 &uiFrameSize,
+							 kEplMsgTypeSoa,
+							 kEplDllAsndNotDefined);
+				if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+					goto Exit;
+				}
+
+				for (uiIndex = 0;
+				     uiIndex <
+				     tabentries(EplDllkInstance_g.m_aNodeInfo);
+				     uiIndex++) {
+//                    EplDllkInstance_g.m_aNodeInfo[uiIndex].m_uiNodeId = uiIndex + 1;
+					EplDllkInstance_g.m_aNodeInfo[uiIndex].
+					    m_wPresPayloadLimit =
+					    (WORD) EplDllkInstance_g.
+					    m_DllConfigParam.
+					    m_uiIsochrRxMaxPayload;
+				}
+
+				// calculate cycle length
+				EplDllkInstance_g.m_ullFrameTimeout = 1000LL
+				    *
+				    ((unsigned long long)EplDllkInstance_g.
+				     m_DllConfigParam.m_dwCycleLen);
+			}
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+			Ret = EplDllkCalAsyncClearBuffer();
+
+			break;
+		}
+
+	case kEplEventTypeDllkDestroy:
+		{
+			// destroy all data structures
+
+			NmtState = *((tEplNmtState *) pEvent_p->m_pArg);
+
+			// delete Tx frames
+			Ret = EplDllkDeleteTxFrame(EPL_DLLK_TXFRAME_IDENTRES);
+			if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+				goto Exit;
+			}
+
+			Ret = EplDllkDeleteTxFrame(EPL_DLLK_TXFRAME_STATUSRES);
+			if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+				goto Exit;
+			}
+
+			Ret = EplDllkDeleteTxFrame(EPL_DLLK_TXFRAME_PRES);
+			if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+				goto Exit;
+			}
+
+			Ret = EplDllkDeleteTxFrame(EPL_DLLK_TXFRAME_NMTREQ);
+			if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+				goto Exit;
+			}
+
+			Ret = EplDllkDeleteTxFrame(EPL_DLLK_TXFRAME_NONEPL);
+			if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+				goto Exit;
+			}
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			if (NmtState >= kEplNmtMsNotActive) {	// local node was MN
+				unsigned int uiIndex;
+
+				Ret =
+				    EplDllkDeleteTxFrame(EPL_DLLK_TXFRAME_SOC);
+				if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+					goto Exit;
+				}
+
+				Ret =
+				    EplDllkDeleteTxFrame(EPL_DLLK_TXFRAME_SOA);
+				if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+					goto Exit;
+				}
+
+				for (uiIndex = 0;
+				     uiIndex <
+				     tabentries(EplDllkInstance_g.m_aNodeInfo);
+				     uiIndex++) {
+					if (EplDllkInstance_g.
+					    m_aNodeInfo[uiIndex].
+					    m_pPreqTxBuffer != NULL) {
+						uiHandle =
+						    EplDllkInstance_g.
+						    m_aNodeInfo[uiIndex].
+						    m_pPreqTxBuffer -
+						    EplDllkInstance_g.
+						    m_pTxBuffer;
+						EplDllkInstance_g.
+						    m_aNodeInfo[uiIndex].
+						    m_pPreqTxBuffer = NULL;
+						Ret =
+						    EplDllkDeleteTxFrame
+						    (uiHandle);
+						if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+							goto Exit;
+						}
+
+					}
+					EplDllkInstance_g.m_aNodeInfo[uiIndex].
+					    m_wPresPayloadLimit = 0xFFFF;
+				}
+			}
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+			// deregister multicast MACs in ethernet driver
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_SOC);
+			Ret = EdrvUndefineRxMacAddrEntry(abMulticastMac);
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_SOA);
+			Ret = EdrvUndefineRxMacAddrEntry(abMulticastMac);
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_PRES);
+			Ret = EdrvUndefineRxMacAddrEntry(abMulticastMac);
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_ASND);
+			Ret = EdrvUndefineRxMacAddrEntry(abMulticastMac);
+
+			// delete timer
+#if EPL_TIMER_USE_HIGHRES != FALSE
+			Ret =
+			    EplTimerHighReskDeleteTimer(&EplDllkInstance_g.
+							m_TimerHdlCycle);
+#endif
+
+			break;
+		}
+
+	case kEplEventTypeDllkFillTx:
+		{
+			// fill TxBuffer of specified priority with new frame if empty
+
+			pTxFrame = NULL;
+			AsyncReqPriority =
+			    *((tEplDllAsyncReqPriority *) pEvent_p->m_pArg);
+			switch (AsyncReqPriority) {
+			case kEplDllAsyncReqPrioNmt:	// NMT request priority
+				{
+					pTxBuffer =
+					    &EplDllkInstance_g.
+					    m_pTxBuffer
+					    [EPL_DLLK_TXFRAME_NMTREQ];
+					if (pTxBuffer->m_pbBuffer != NULL) {	// NmtRequest does exist
+						// check if frame is empty and not being filled
+						if (pTxBuffer->m_uiTxMsgLen ==
+						    EPL_DLLK_BUFLEN_EMPTY) {
+							// mark Tx buffer as filling is in process
+							pTxBuffer->
+							    m_uiTxMsgLen =
+							    EPL_DLLK_BUFLEN_FILLING;
+							// set max buffer size as input parameter
+							uiFrameSize =
+							    pTxBuffer->
+							    m_uiMaxBufferLen;
+							// copy frame from shared loop buffer to Tx buffer
+							Ret =
+							    EplDllkCalAsyncGetTxFrame
+							    (pTxBuffer->
+							     m_pbBuffer,
+							     &uiFrameSize,
+							     AsyncReqPriority);
+							if (Ret ==
+							    kEplSuccessful) {
+								pTxFrame =
+								    (tEplFrame
+								     *)
+								    pTxBuffer->
+								    m_pbBuffer;
+								Ret =
+								    EplDllkCheckFrame
+								    (pTxFrame,
+								     uiFrameSize);
+
+								// set buffer valid
+								pTxBuffer->
+								    m_uiTxMsgLen
+								    =
+								    uiFrameSize;
+							} else if (Ret == kEplDllAsyncTxBufferEmpty) {	// empty Tx buffer is not a real problem
+								// so just ignore it
+								Ret =
+								    kEplSuccessful;
+								// mark Tx buffer as empty
+								pTxBuffer->
+								    m_uiTxMsgLen
+								    =
+								    EPL_DLLK_BUFLEN_EMPTY;
+							}
+						}
+					}
+					break;
+				}
+
+			default:	// generic priority
+				{
+					pTxBuffer =
+					    &EplDllkInstance_g.
+					    m_pTxBuffer
+					    [EPL_DLLK_TXFRAME_NONEPL];
+					if (pTxBuffer->m_pbBuffer != NULL) {	// non-EPL frame does exist
+						// check if frame is empty and not being filled
+						if (pTxBuffer->m_uiTxMsgLen ==
+						    EPL_DLLK_BUFLEN_EMPTY) {
+							// mark Tx buffer as filling is in process
+							pTxBuffer->
+							    m_uiTxMsgLen =
+							    EPL_DLLK_BUFLEN_FILLING;
+							// set max buffer size as input parameter
+							uiFrameSize =
+							    pTxBuffer->
+							    m_uiMaxBufferLen;
+							// copy frame from shared loop buffer to Tx buffer
+							Ret =
+							    EplDllkCalAsyncGetTxFrame
+							    (pTxBuffer->
+							     m_pbBuffer,
+							     &uiFrameSize,
+							     AsyncReqPriority);
+							if (Ret ==
+							    kEplSuccessful) {
+								pTxFrame =
+								    (tEplFrame
+								     *)
+								    pTxBuffer->
+								    m_pbBuffer;
+								Ret =
+								    EplDllkCheckFrame
+								    (pTxFrame,
+								     uiFrameSize);
+
+								// set buffer valid
+								pTxBuffer->
+								    m_uiTxMsgLen
+								    =
+								    uiFrameSize;
+							} else if (Ret == kEplDllAsyncTxBufferEmpty) {	// empty Tx buffer is not a real problem
+								// so just ignore it
+								Ret =
+								    kEplSuccessful;
+								// mark Tx buffer as empty
+								pTxBuffer->
+								    m_uiTxMsgLen
+								    =
+								    EPL_DLLK_BUFLEN_EMPTY;
+							}
+						}
+					}
+					break;
+				}
+			}
+
+			NmtState = EplNmtkGetNmtState();
+
+			if ((NmtState == kEplNmtCsBasicEthernet) || (NmtState == kEplNmtMsBasicEthernet)) {	// send frame immediately
+				if (pTxFrame != NULL) {	// frame is present
+					// padding is done by Edrv or ethernet controller
+					Ret = EdrvSendTxMsg(pTxBuffer);
+				} else {	// no frame moved to TxBuffer
+					// check if TxBuffers contain unsent frames
+					if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_uiTxMsgLen > EPL_DLLK_BUFLEN_EMPTY) {	// NMT request Tx buffer contains a frame
+						Ret =
+						    EdrvSendTxMsg
+						    (&EplDllkInstance_g.
+						     m_pTxBuffer
+						     [EPL_DLLK_TXFRAME_NMTREQ]);
+					} else if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NONEPL].m_uiTxMsgLen > EPL_DLLK_BUFLEN_EMPTY) {	// non-EPL Tx buffer contains a frame
+						Ret =
+						    EdrvSendTxMsg
+						    (&EplDllkInstance_g.
+						     m_pTxBuffer
+						     [EPL_DLLK_TXFRAME_NONEPL]);
+					}
+					if (Ret == kEplInvalidOperation) {	// ignore error if caused by already active transmission
+						Ret = kEplSuccessful;
+					}
+				}
+				// reset PRes flag 2
+				EplDllkInstance_g.m_bFlag2 = 0;
+			} else {
+				// update Flag 2 (PR, RS)
+				Ret =
+				    EplDllkCalAsyncGetTxCount(&AsyncReqPriority,
+							      &uiFrameCount);
+				if (AsyncReqPriority == kEplDllAsyncReqPrioNmt) {	// non-empty FIFO with hightest priority is for NMT requests
+					if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_uiTxMsgLen > EPL_DLLK_BUFLEN_EMPTY) {	// NMT request Tx buffer contains a frame
+						// add one more frame
+						uiFrameCount++;
+					}
+				} else {	// non-empty FIFO with highest priority is for generic frames
+					if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_uiTxMsgLen > EPL_DLLK_BUFLEN_EMPTY) {	// NMT request Tx buffer contains a frame
+						// use NMT request FIFO, because of higher priority
+						uiFrameCount = 1;
+						AsyncReqPriority =
+						    kEplDllAsyncReqPrioNmt;
+					} else if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NONEPL].m_uiTxMsgLen > EPL_DLLK_BUFLEN_EMPTY) {	// non-EPL Tx buffer contains a frame
+						// use NMT request FIFO, because of higher priority
+						// add one more frame
+						uiFrameCount++;
+					}
+				}
+
+				if (uiFrameCount > 7) {	// limit frame request to send counter to 7
+					uiFrameCount = 7;
+				}
+				if (uiFrameCount > 0) {
+					EplDllkInstance_g.m_bFlag2 =
+					    (BYTE) (((AsyncReqPriority <<
+						      EPL_FRAME_FLAG2_PR_SHIFT)
+						     & EPL_FRAME_FLAG2_PR)
+						    | (uiFrameCount &
+						       EPL_FRAME_FLAG2_RS));
+				} else {
+					EplDllkInstance_g.m_bFlag2 = 0;
+				}
+			}
+
+			break;
+		}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	case kEplEventTypeDllkStartReducedCycle:
+		{
+			// start the reduced cycle by programming the cycle timer
+			// it is issued by NMT MN module, when PreOp1 is entered
+
+			// clear the asynchronous queues
+			Ret = EplDllkCalAsyncClearQueues();
+
+			// reset cycle counter (everytime a SoA is triggerd in PreOp1 the counter is incremented
+			// and when it reaches EPL_C_DLL_PREOP1_START_CYCLES the SoA may contain invitations)
+			EplDllkInstance_g.m_uiCycleCount = 0;
+
+			// remove any CN from isochronous phase
+			while (EplDllkInstance_g.m_pFirstNodeInfo != NULL) {
+				EplDllkDeleteNode(EplDllkInstance_g.
+						  m_pFirstNodeInfo->m_uiNodeId);
+			}
+
+			// change state to NonCyclic,
+			// hence EplDllkChangeState() will not ignore the next call
+			EplDllkInstance_g.m_DllState = kEplDllMsNonCyclic;
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+			if (EplDllkInstance_g.m_DllConfigParam.
+			    m_dwAsyncSlotTimeout != 0) {
+				Ret =
+				    EplTimerHighReskModifyTimerNs
+				    (&EplDllkInstance_g.m_TimerHdlCycle,
+				     EplDllkInstance_g.m_DllConfigParam.
+				     m_dwAsyncSlotTimeout,
+				     EplDllkCbMnTimerCycle, 0L, FALSE);
+			}
+#endif
+
+			break;
+		}
+#endif
+
+#if EPL_DLL_PRES_READY_AFTER_SOA != FALSE
+	case kEplEventTypeDllkPresReady:
+		{
+			// post PRes to transmit FIFO
+
+			NmtState = EplNmtkGetNmtState();
+
+			if (NmtState != kEplNmtCsBasicEthernet) {
+				// Does PRes exist?
+				if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_PRES].m_pbBuffer != NULL) {	// PRes does exist
+					pTxFrame =
+					    (tEplFrame *) EplDllkInstance_g.
+					    m_pTxBuffer[EPL_DLLK_TXFRAME_PRES].
+					    m_pbBuffer;
+					// update frame (NMT state, RD, RS, PR, MS, EN flags)
+					if (NmtState < kEplNmtCsPreOperational2) {	// NMT state is not PreOp2, ReadyToOp or Op
+						// fake NMT state PreOp2, because PRes will be sent only in PreOp2 or greater
+						NmtState =
+						    kEplNmtCsPreOperational2;
+					}
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+						       m_le_bNmtStatus,
+						       (BYTE) NmtState);
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+						       m_le_bFlag2,
+						       EplDllkInstance_g.
+						       m_bFlag2);
+					if (NmtState != kEplNmtCsOperational) {	// mark PDO as invalid in NMT state Op
+						// $$$ reset only RD flag; set other flags appropriately
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Pres.
+							       m_le_bFlag1, 0);
+					}
+					// $$$ make function that updates Pres, StatusRes
+					// mark PRes frame as ready for transmission
+					Ret =
+					    EdrvTxMsgReady(&EplDllkInstance_g.
+							   m_pTxBuffer
+							   [EPL_DLLK_TXFRAME_PRES]);
+				}
+			}
+
+			break;
+		}
+#endif
+	default:
+		{
+			ASSERTMSG(FALSE,
+				  "EplDllkProcess(): unhandled event type!\n");
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkConfig
+//
+// Description: configure parameters of DLL
+//
+// Parameters:  pDllConfigParam_p       = configuration parameters
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkConfig(tEplDllConfigParam * pDllConfigParam_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+// d.k. check of NMT state disabled, because CycleLen is programmed at run time by MN without reset of CN
+/*tEplNmtState    NmtState;
+
+    NmtState = EplNmtkGetNmtState();
+
+    if (NmtState > kEplNmtGsResetConfiguration)
+    {   // only allowed in state DLL_GS_INIT
+        Ret = kEplInvalidOperation;
+        goto Exit;
+    }
+*/
+	EPL_MEMCPY(&EplDllkInstance_g.m_DllConfigParam, pDllConfigParam_p,
+		   (pDllConfigParam_p->m_uiSizeOfStruct <
+		    sizeof(tEplDllConfigParam) ? pDllConfigParam_p->
+		    m_uiSizeOfStruct : sizeof(tEplDllConfigParam)));
+
+	if ((EplDllkInstance_g.m_DllConfigParam.m_dwCycleLen != 0)
+	    && (EplDllkInstance_g.m_DllConfigParam.m_dwLossOfFrameTolerance != 0)) {	// monitor EPL cycle, calculate frame timeout
+		EplDllkInstance_g.m_ullFrameTimeout = (1000LL
+						       *
+						       ((unsigned long long)
+							EplDllkInstance_g.
+							m_DllConfigParam.
+							m_dwCycleLen))
+		    +
+		    ((unsigned long long)EplDllkInstance_g.m_DllConfigParam.
+		     m_dwLossOfFrameTolerance);
+	} else {
+		EplDllkInstance_g.m_ullFrameTimeout = 0LL;
+	}
+
+	if (EplDllkInstance_g.m_DllConfigParam.m_fAsyncOnly != FALSE) {	// it is configured as async-only CN
+		// disable multiplexed cycle, that m_uiCycleCount will not be incremented spuriously on SoC
+		EplDllkInstance_g.m_DllConfigParam.m_uiMultiplCycleCnt = 0;
+	}
+//Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkSetIdentity
+//
+// Description: configure identity of local node for IdentResponse
+//
+// Parameters:  pDllIdentParam_p        = identity
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkSetIdentity(tEplDllIdentParam * pDllIdentParam_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	EPL_MEMCPY(&EplDllkInstance_g.m_DllIdentParam, pDllIdentParam_p,
+		   (pDllIdentParam_p->m_uiSizeOfStruct <
+		    sizeof(tEplDllIdentParam) ? pDllIdentParam_p->
+		    m_uiSizeOfStruct : sizeof(tEplDllIdentParam)));
+
+	// $$$ if IdentResponse frame exists update it
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkRegAsyncHandler
+//
+// Description: registers handler for non-EPL frames
+//
+// Parameters:  pfnDllkCbAsync_p        = pointer to callback function
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkRegAsyncHandler(tEplDllkCbAsync pfnDllkCbAsync_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (EplDllkInstance_g.m_pfnCbAsync == NULL) {	// no handler registered yet
+		EplDllkInstance_g.m_pfnCbAsync = pfnDllkCbAsync_p;
+	} else {		// handler already registered
+		Ret = kEplDllCbAsyncRegistered;
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkDeregAsyncHandler
+//
+// Description: deregisters handler for non-EPL frames
+//
+// Parameters:  pfnDllkCbAsync_p        = pointer to callback function
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkDeregAsyncHandler(tEplDllkCbAsync pfnDllkCbAsync_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (EplDllkInstance_g.m_pfnCbAsync == pfnDllkCbAsync_p) {	// same handler is registered
+		// deregister it
+		EplDllkInstance_g.m_pfnCbAsync = NULL;
+	} else {		// wrong handler or no handler registered
+		Ret = kEplDllCbAsyncRegistered;
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkSetAsndServiceIdFilter()
+//
+// Description: sets the specified node ID filter for the specified
+//              AsndServiceId. It registers C_DLL_MULTICAST_ASND in ethernet
+//              driver if any AsndServiceId is open.
+//
+// Parameters:  ServiceId_p             = ASnd Service ID
+//              Filter_p                = node ID filter
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkSetAsndServiceIdFilter(tEplDllAsndServiceId ServiceId_p,
+					 tEplDllAsndFilter Filter_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (ServiceId_p < tabentries(EplDllkInstance_g.m_aAsndFilter)) {
+		EplDllkInstance_g.m_aAsndFilter[ServiceId_p] = Filter_p;
+	}
+
+	return Ret;
+}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkSetFlag1OfNode()
+//
+// Description: sets Flag1 (for PReq and SoA) of the specified node ID.
+//
+// Parameters:  uiNodeId_p              = node ID
+//              bSoaFlag1_p             = flag1
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkSetFlag1OfNode(unsigned int uiNodeId_p, BYTE bSoaFlag1_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplDllkNodeInfo *pNodeInfo;
+
+	pNodeInfo = EplDllkGetNodeInfo(uiNodeId_p);
+	if (pNodeInfo == NULL) {	// no node info structure available
+		Ret = kEplDllNoNodeInfo;
+		goto Exit;
+	}
+	// store flag1 in internal node info structure
+	pNodeInfo->m_bSoaFlag1 = bSoaFlag1_p;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkGetFirstNodeInfo()
+//
+// Description: returns first info structure of first node in isochronous phase.
+//              It is only useful for ErrorHandlerk module.
+//
+// Parameters:  ppNodeInfo_p            = pointer to pointer of internal node info structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkGetFirstNodeInfo(tEplDllkNodeInfo ** ppNodeInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	*ppNodeInfo_p = EplDllkInstance_g.m_pFirstNodeInfo;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkAddNode()
+//
+// Description: adds the specified node to the isochronous phase.
+//
+// Parameters:  pNodeInfo_p             = pointer of node info structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkAddNode(tEplDllNodeInfo * pNodeInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplDllkNodeInfo *pIntNodeInfo;
+	tEplDllkNodeInfo **ppIntNodeInfo;
+	unsigned int uiHandle;
+	tEplFrame *pFrame;
+	unsigned int uiFrameSize;
+
+	pIntNodeInfo = EplDllkGetNodeInfo(pNodeInfo_p->m_uiNodeId);
+	if (pIntNodeInfo == NULL) {	// no node info structure available
+		Ret = kEplDllNoNodeInfo;
+		goto Exit;
+	}
+
+	EPL_DLLK_DBG_POST_TRACE_VALUE(kEplEventTypeDllkAddNode,
+				      pNodeInfo_p->m_uiNodeId, 0);
+
+	// copy node configuration
+	pIntNodeInfo->m_dwPresTimeout = pNodeInfo_p->m_dwPresTimeout;
+	pIntNodeInfo->m_wPresPayloadLimit = pNodeInfo_p->m_wPresPayloadLimit;
+
+	// $$$ d.k.: actually add node only if MN. On CN it is sufficient to update the node configuration
+	if (pNodeInfo_p->m_uiNodeId == EplDllkInstance_g.m_DllConfigParam.m_uiNodeId) {	// we shall send PRes ourself
+		// insert our node at the end of the list
+		ppIntNodeInfo = &EplDllkInstance_g.m_pFirstNodeInfo;
+		while ((*ppIntNodeInfo != NULL)
+		       && ((*ppIntNodeInfo)->m_pNextNodeInfo != NULL)) {
+			ppIntNodeInfo = &(*ppIntNodeInfo)->m_pNextNodeInfo;
+		}
+		if (*ppIntNodeInfo != NULL) {
+			if ((*ppIntNodeInfo)->m_uiNodeId == pNodeInfo_p->m_uiNodeId) {	// node was already added to list
+				// $$$ d.k. maybe this should be an error
+				goto Exit;
+			} else {	// add our node at the end of the list
+				ppIntNodeInfo =
+				    &(*ppIntNodeInfo)->m_pNextNodeInfo;
+			}
+		}
+		// set "PReq"-TxBuffer to PRes-TxBuffer
+		pIntNodeInfo->m_pPreqTxBuffer =
+		    &EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_PRES];
+	} else {		// normal CN shall be added to isochronous phase
+		// insert node into list in ascending order
+		ppIntNodeInfo = &EplDllkInstance_g.m_pFirstNodeInfo;
+		while ((*ppIntNodeInfo != NULL)
+		       && ((*ppIntNodeInfo)->m_uiNodeId <
+			   pNodeInfo_p->m_uiNodeId)
+		       && ((*ppIntNodeInfo)->m_uiNodeId !=
+			   EplDllkInstance_g.m_DllConfigParam.m_uiNodeId)) {
+			ppIntNodeInfo = &(*ppIntNodeInfo)->m_pNextNodeInfo;
+		}
+		if ((*ppIntNodeInfo != NULL) && ((*ppIntNodeInfo)->m_uiNodeId == pNodeInfo_p->m_uiNodeId)) {	// node was already added to list
+			// $$$ d.k. maybe this should be an error
+			goto Exit;
+		}
+	}
+
+	// initialize elements of internal node info structure
+	pIntNodeInfo->m_bSoaFlag1 = 0;
+	pIntNodeInfo->m_fSoftDelete = FALSE;
+	pIntNodeInfo->m_NmtState = kEplNmtCsNotActive;
+	if (pIntNodeInfo->m_pPreqTxBuffer == NULL) {	// create TxBuffer entry
+		uiFrameSize = pNodeInfo_p->m_wPreqPayloadLimit + 24;
+		Ret =
+		    EplDllkCreateTxFrame(&uiHandle, &pFrame, &uiFrameSize,
+					 kEplMsgTypePreq,
+					 kEplDllAsndNotDefined);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		pIntNodeInfo->m_pPreqTxBuffer =
+		    &EplDllkInstance_g.m_pTxBuffer[uiHandle];
+		AmiSetByteToLe(&pFrame->m_le_bDstNodeId,
+			       (BYTE) pNodeInfo_p->m_uiNodeId);
+
+		// set up destination MAC address
+		EPL_MEMCPY(pFrame->m_be_abDstMac, pIntNodeInfo->m_be_abMacAddr,
+			   6);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+		{
+			tEplFrameInfo FrameInfo;
+
+			// initially encode TPDO -> inform PDO module
+			FrameInfo.m_pFrame = pFrame;
+			FrameInfo.m_uiFrameSize = uiFrameSize;
+			Ret = EplPdokCbPdoTransmitted(&FrameInfo);
+		}
+#endif
+	}
+	pIntNodeInfo->m_ulDllErrorEvents = 0L;
+	// add node to list
+	pIntNodeInfo->m_pNextNodeInfo = *ppIntNodeInfo;
+	*ppIntNodeInfo = pIntNodeInfo;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkDeleteNode()
+//
+// Description: removes the specified node from the isochronous phase.
+//
+// Parameters:  uiNodeId_p              = node ID
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkDeleteNode(unsigned int uiNodeId_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplDllkNodeInfo *pIntNodeInfo;
+	tEplDllkNodeInfo **ppIntNodeInfo;
+	unsigned int uiHandle;
+
+	pIntNodeInfo = EplDllkGetNodeInfo(uiNodeId_p);
+	if (pIntNodeInfo == NULL) {	// no node info structure available
+		Ret = kEplDllNoNodeInfo;
+		goto Exit;
+	}
+
+	EPL_DLLK_DBG_POST_TRACE_VALUE(kEplEventTypeDllkDelNode, uiNodeId_p, 0);
+
+	// search node in whole list
+	ppIntNodeInfo = &EplDllkInstance_g.m_pFirstNodeInfo;
+	while ((*ppIntNodeInfo != NULL)
+	       && ((*ppIntNodeInfo)->m_uiNodeId != uiNodeId_p)) {
+		ppIntNodeInfo = &(*ppIntNodeInfo)->m_pNextNodeInfo;
+	}
+	if ((*ppIntNodeInfo == NULL) || ((*ppIntNodeInfo)->m_uiNodeId != uiNodeId_p)) {	// node was not found in list
+		// $$$ d.k. maybe this should be an error
+		goto Exit;
+	}
+	// remove node from list
+	*ppIntNodeInfo = pIntNodeInfo->m_pNextNodeInfo;
+
+	if ((pIntNodeInfo->m_pPreqTxBuffer != NULL)
+	    && (pIntNodeInfo->m_pPreqTxBuffer != &EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_PRES])) {	// delete TxBuffer entry
+		uiHandle =
+		    pIntNodeInfo->m_pPreqTxBuffer -
+		    EplDllkInstance_g.m_pTxBuffer;
+		pIntNodeInfo->m_pPreqTxBuffer = NULL;
+		Ret = EplDllkDeleteTxFrame(uiHandle);
+/*        if (Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkSoftDeleteNode()
+//
+// Description: removes the specified node not immediately from the isochronous phase.
+//              Instead the will be removed after error (late/loss PRes) without
+//              charging the error.
+//
+// Parameters:  uiNodeId_p              = node ID
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkSoftDeleteNode(unsigned int uiNodeId_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplDllkNodeInfo *pIntNodeInfo;
+
+	pIntNodeInfo = EplDllkGetNodeInfo(uiNodeId_p);
+	if (pIntNodeInfo == NULL) {	// no node info structure available
+		Ret = kEplDllNoNodeInfo;
+		goto Exit;
+	}
+
+	EPL_DLLK_DBG_POST_TRACE_VALUE(kEplEventTypeDllkSoftDelNode,
+				      uiNodeId_p, 0);
+
+	pIntNodeInfo->m_fSoftDelete = TRUE;
+
+      Exit:
+	return Ret;
+}
+
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkChangeState
+//
+// Description: change DLL state on event and diagnose some communication errors
+//
+// Parameters:  NmtEvent_p              = DLL event (wrapped in NMT event)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDllkChangeState(tEplNmtEvent NmtEvent_p,
+				     tEplNmtState NmtState_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+	tEplErrorHandlerkEvent DllEvent;
+
+	DllEvent.m_ulDllErrorEvents = 0;
+	DllEvent.m_uiNodeId = 0;
+	DllEvent.m_NmtState = NmtState_p;
+
+	switch (NmtState_p) {
+	case kEplNmtGsOff:
+	case kEplNmtGsInitialising:
+	case kEplNmtGsResetApplication:
+	case kEplNmtGsResetCommunication:
+	case kEplNmtGsResetConfiguration:
+	case kEplNmtCsBasicEthernet:
+		// enter DLL_GS_INIT
+		EplDllkInstance_g.m_DllState = kEplDllGsInit;
+		break;
+
+	case kEplNmtCsNotActive:
+	case kEplNmtCsPreOperational1:
+		// reduced EPL cycle is active
+		if (NmtEvent_p == kEplNmtEventDllCeSoc) {	// SoC received
+			// enter DLL_CS_WAIT_PREQ
+			EplDllkInstance_g.m_DllState = kEplDllCsWaitPreq;
+		} else {
+			// enter DLL_GS_INIT
+			EplDllkInstance_g.m_DllState = kEplDllGsInit;
+		}
+		break;
+
+	case kEplNmtCsPreOperational2:
+	case kEplNmtCsReadyToOperate:
+	case kEplNmtCsOperational:
+		// full EPL cycle is active
+
+		switch (EplDllkInstance_g.m_DllState) {
+		case kEplDllCsWaitPreq:
+			switch (NmtEvent_p) {
+				// DLL_CT2
+			case kEplNmtEventDllCePreq:
+				// enter DLL_CS_WAIT_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_RECVD_PREQ;
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoa;
+				break;
+
+				// DLL_CT8
+			case kEplNmtEventDllCeFrameTimeout:
+				if (NmtState_p == kEplNmtCsPreOperational2) {	// ignore frame timeout in PreOp2,
+					// because the previously configured cycle len
+					// may be wrong.
+					// 2008/10/15 d.k. If it would not be ignored,
+					// we would go cyclically to PreOp1 and on next
+					// SoC back to PreOp2.
+					break;
+				}
+				// report DLL_CEV_LOSS_SOC and DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA |
+				    EPL_DLL_ERR_CN_LOSS_SOC;
+
+				// enter DLL_CS_WAIT_SOC
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoc;
+				break;
+
+			case kEplNmtEventDllCeSoa:
+				// check if multiplexed and PReq should have been received in this cycle
+				// and if >= NMT_CS_READY_TO_OPERATE
+				if ((EplDllkInstance_g.m_uiCycleCount == 0)
+				    && (NmtState_p >= kEplNmtCsReadyToOperate)) {	// report DLL_CEV_LOSS_OF_PREQ
+					DllEvent.m_ulDllErrorEvents |=
+					    EPL_DLL_ERR_CN_LOSS_PREQ;
+				}
+				// enter DLL_CS_WAIT_SOC
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoc;
+				break;
+
+				// DLL_CT7
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeAsnd:
+				// report DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA;
+
+			case kEplNmtEventDllCePres:
+			default:
+				// remain in this state
+				break;
+			}
+			break;
+
+		case kEplDllCsWaitSoc:
+			switch (NmtEvent_p) {
+				// DLL_CT1
+			case kEplNmtEventDllCeSoc:
+				// start of cycle and isochronous phase
+				// enter DLL_CS_WAIT_PREQ
+				EplDllkInstance_g.m_DllState =
+				    kEplDllCsWaitPreq;
+				break;
+
+				// DLL_CT4
+//                        case kEplNmtEventDllCePres:
+			case kEplNmtEventDllCeFrameTimeout:
+				if (NmtState_p == kEplNmtCsPreOperational2) {	// ignore frame timeout in PreOp2,
+					// because the previously configured cycle len
+					// may be wrong.
+					// 2008/10/15 d.k. If it would not be ignored,
+					// we would go cyclically to PreOp1 and on next
+					// SoC back to PreOp2.
+					break;
+				}
+				// fall through
+
+			case kEplNmtEventDllCePreq:
+			case kEplNmtEventDllCeSoa:
+				// report DLL_CEV_LOSS_SOC
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOC;
+
+			case kEplNmtEventDllCeAsnd:
+			default:
+				// remain in this state
+				break;
+			}
+			break;
+
+		case kEplDllCsWaitSoa:
+			switch (NmtEvent_p) {
+			case kEplNmtEventDllCeFrameTimeout:
+				// DLL_CT3
+				if (NmtState_p == kEplNmtCsPreOperational2) {	// ignore frame timeout in PreOp2,
+					// because the previously configured cycle len
+					// may be wrong.
+					// 2008/10/15 d.k. If it would not be ignored,
+					// we would go cyclically to PreOp1 and on next
+					// SoC back to PreOp2.
+					break;
+				}
+				// fall through
+
+			case kEplNmtEventDllCePreq:
+				// report DLL_CEV_LOSS_SOC and DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA |
+				    EPL_DLL_ERR_CN_LOSS_SOC;
+
+			case kEplNmtEventDllCeSoa:
+				// enter DLL_CS_WAIT_SOC
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoc;
+				break;
+
+				// DLL_CT9
+			case kEplNmtEventDllCeSoc:
+				// report DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA;
+
+				// enter DLL_CS_WAIT_PREQ
+				EplDllkInstance_g.m_DllState =
+				    kEplDllCsWaitPreq;
+				break;
+
+				// DLL_CT10
+			case kEplNmtEventDllCeAsnd:
+				// report DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA;
+
+			case kEplNmtEventDllCePres:
+			default:
+				// remain in this state
+				break;
+			}
+			break;
+
+		case kEplDllGsInit:
+			// enter DLL_CS_WAIT_PREQ
+			EplDllkInstance_g.m_DllState = kEplDllCsWaitPreq;
+			break;
+
+		default:
+			break;
+		}
+		break;
+
+	case kEplNmtCsStopped:
+		// full EPL cycle is active, but without PReq/PRes
+
+		switch (EplDllkInstance_g.m_DllState) {
+		case kEplDllCsWaitPreq:
+			switch (NmtEvent_p) {
+				// DLL_CT2
+			case kEplNmtEventDllCePreq:
+				// enter DLL_CS_WAIT_SOA
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoa;
+				break;
+
+				// DLL_CT8
+			case kEplNmtEventDllCeFrameTimeout:
+				// report DLL_CEV_LOSS_SOC and DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA |
+				    EPL_DLL_ERR_CN_LOSS_SOC;
+
+			case kEplNmtEventDllCeSoa:
+				// NMT_CS_STOPPED active
+				// it is Ok if no PReq was received
+
+				// enter DLL_CS_WAIT_SOC
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoc;
+				break;
+
+				// DLL_CT7
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeAsnd:
+				// report DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA;
+
+			case kEplNmtEventDllCePres:
+			default:
+				// remain in this state
+				break;
+			}
+			break;
+
+		case kEplDllCsWaitSoc:
+			switch (NmtEvent_p) {
+				// DLL_CT1
+			case kEplNmtEventDllCeSoc:
+				// start of cycle and isochronous phase
+				// enter DLL_CS_WAIT_SOA
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoa;
+				break;
+
+				// DLL_CT4
+//                        case kEplNmtEventDllCePres:
+			case kEplNmtEventDllCePreq:
+			case kEplNmtEventDllCeSoa:
+			case kEplNmtEventDllCeFrameTimeout:
+				// report DLL_CEV_LOSS_SOC
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOC;
+
+			case kEplNmtEventDllCeAsnd:
+			default:
+				// remain in this state
+				break;
+			}
+			break;
+
+		case kEplDllCsWaitSoa:
+			switch (NmtEvent_p) {
+				// DLL_CT3
+			case kEplNmtEventDllCeFrameTimeout:
+				// report DLL_CEV_LOSS_SOC and DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA |
+				    EPL_DLL_ERR_CN_LOSS_SOC;
+
+			case kEplNmtEventDllCeSoa:
+				// enter DLL_CS_WAIT_SOC
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoc;
+				break;
+
+				// DLL_CT9
+			case kEplNmtEventDllCeSoc:
+				// report DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA;
+				// remain in DLL_CS_WAIT_SOA
+				break;
+
+				// DLL_CT10
+			case kEplNmtEventDllCeAsnd:
+				// report DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA;
+
+			case kEplNmtEventDllCePreq:
+				// NMT_CS_STOPPED active and we do not expect any PReq
+				// so just ignore it
+			case kEplNmtEventDllCePres:
+			default:
+				// remain in this state
+				break;
+			}
+			break;
+
+		case kEplDllGsInit:
+		default:
+			// enter DLL_CS_WAIT_PREQ
+			EplDllkInstance_g.m_DllState = kEplDllCsWaitSoa;
+			break;
+		}
+		break;
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	case kEplNmtMsNotActive:
+	case kEplNmtMsBasicEthernet:
+		break;
+
+	case kEplNmtMsPreOperational1:
+		// reduced EPL cycle is active
+		if (EplDllkInstance_g.m_DllState != kEplDllMsNonCyclic) {	// stop cycle timer
+#if EPL_TIMER_USE_HIGHRES != FALSE
+			Ret =
+			    EplTimerHighReskDeleteTimer(&EplDllkInstance_g.
+							m_TimerHdlCycle);
+#endif
+			EplDllkInstance_g.m_DllState = kEplDllMsNonCyclic;
+
+			// stop further processing,
+			// because it will be restarted by NMT MN module
+			break;
+		}
+
+		switch (NmtEvent_p) {
+		case kEplNmtEventDllMeSocTrig:
+		case kEplNmtEventDllCeAsnd:
+			{	// because of reduced EPL cycle SoA shall be triggered, not SoC
+				tEplDllState DummyDllState;
+
+				Ret =
+				    EplDllkAsyncFrameNotReceived
+				    (EplDllkInstance_g.m_LastReqServiceId,
+				     EplDllkInstance_g.m_uiLastTargetNodeId);
+
+				// go ahead and send SoA
+				Ret = EplDllkMnSendSoa(NmtState_p,
+						       &DummyDllState,
+						       (EplDllkInstance_g.
+							m_uiCycleCount >=
+							EPL_C_DLL_PREOP1_START_CYCLES));
+				// increment cycle counter to detect if EPL_C_DLL_PREOP1_START_CYCLES empty cycles are elapsed
+				EplDllkInstance_g.m_uiCycleCount++;
+
+				// reprogram timer
+#if EPL_TIMER_USE_HIGHRES != FALSE
+				if (EplDllkInstance_g.m_DllConfigParam.
+				    m_dwAsyncSlotTimeout != 0) {
+					Ret =
+					    EplTimerHighReskModifyTimerNs
+					    (&EplDllkInstance_g.m_TimerHdlCycle,
+					     EplDllkInstance_g.m_DllConfigParam.
+					     m_dwAsyncSlotTimeout,
+					     EplDllkCbMnTimerCycle, 0L, FALSE);
+				}
+#endif
+				break;
+			}
+
+		default:
+			break;
+		}
+		break;
+
+	case kEplNmtMsPreOperational2:
+	case kEplNmtMsReadyToOperate:
+	case kEplNmtMsOperational:
+		// full EPL cycle is active
+		switch (NmtEvent_p) {
+		case kEplNmtEventDllMeSocTrig:
+			{
+				// update cycle counter
+				if (EplDllkInstance_g.m_DllConfigParam.m_uiMultiplCycleCnt > 0) {	// multiplexed cycle active
+					EplDllkInstance_g.m_uiCycleCount =
+					    (EplDllkInstance_g.m_uiCycleCount +
+					     1) %
+					    EplDllkInstance_g.m_DllConfigParam.
+					    m_uiMultiplCycleCnt;
+					// $$$ check multiplexed cycle restart
+					//     -> toggle MC flag
+					//     -> change node linked list
+				} else {	// non-multiplexed cycle active
+					// start with first node in isochronous phase
+					EplDllkInstance_g.m_pCurNodeInfo = NULL;
+				}
+
+				switch (EplDllkInstance_g.m_DllState) {
+				case kEplDllMsNonCyclic:
+					{	// start continuous cycle timer
+#if EPL_TIMER_USE_HIGHRES != FALSE
+						Ret =
+						    EplTimerHighReskModifyTimerNs
+						    (&EplDllkInstance_g.
+						     m_TimerHdlCycle,
+						     EplDllkInstance_g.
+						     m_ullFrameTimeout,
+						     EplDllkCbMnTimerCycle, 0L,
+						     TRUE);
+#endif
+						// continue with sending SoC
+					}
+
+				case kEplDllMsWaitAsnd:
+				case kEplDllMsWaitSocTrig:
+					{	// if m_LastReqServiceId is still valid,
+						// SoA was not correctly answered
+						// and user part has to be informed
+						Ret =
+						    EplDllkAsyncFrameNotReceived
+						    (EplDllkInstance_g.
+						     m_LastReqServiceId,
+						     EplDllkInstance_g.
+						     m_uiLastTargetNodeId);
+
+						// send SoC
+						Ret = EplDllkMnSendSoc();
+
+						// new DLL state
+						EplDllkInstance_g.m_DllState =
+						    kEplDllMsWaitPreqTrig;
+
+						// start WaitSoCPReq Timer
+#if EPL_TIMER_USE_HIGHRES != FALSE
+						Ret =
+						    EplTimerHighReskModifyTimerNs
+						    (&EplDllkInstance_g.
+						     m_TimerHdlResponse,
+						     EplDllkInstance_g.
+						     m_DllConfigParam.
+						     m_dwWaitSocPreq,
+						     EplDllkCbMnTimerResponse,
+						     0L, FALSE);
+#endif
+						break;
+					}
+
+				default:
+					{	// wrong DLL state / cycle time exceeded
+						DllEvent.m_ulDllErrorEvents |=
+						    EPL_DLL_ERR_MN_CYCTIMEEXCEED;
+						EplDllkInstance_g.m_DllState =
+						    kEplDllMsWaitSocTrig;
+						break;
+					}
+				}
+
+				break;
+			}
+
+		case kEplNmtEventDllMePresTimeout:
+			{
+
+				switch (EplDllkInstance_g.m_DllState) {
+				case kEplDllMsWaitPres:
+					{	// PRes not received
+
+						if (EplDllkInstance_g.m_pCurNodeInfo->m_fSoftDelete == FALSE) {	// normal isochronous CN
+							DllEvent.
+							    m_ulDllErrorEvents
+							    |=
+							    EPL_DLL_ERR_MN_CN_LOSS_PRES;
+							DllEvent.m_uiNodeId =
+							    EplDllkInstance_g.
+							    m_pCurNodeInfo->
+							    m_uiNodeId;
+						} else {	// CN shall be deleted softly
+							Event.m_EventSink =
+							    kEplEventSinkDllkCal;
+							Event.m_EventType =
+							    kEplEventTypeDllkSoftDelNode;
+							// $$$ d.k. set Event.m_NetTime to current time
+							Event.m_uiSize =
+							    sizeof(unsigned
+								   int);
+							Event.m_pArg =
+							    &EplDllkInstance_g.
+							    m_pCurNodeInfo->
+							    m_uiNodeId;
+							Ret =
+							    EplEventkPost
+							    (&Event);
+						}
+
+						// continue with sending next PReq
+					}
+
+				case kEplDllMsWaitPreqTrig:
+					{
+						// send next PReq
+						Ret =
+						    EplDllkMnSendPreq
+						    (NmtState_p,
+						     &EplDllkInstance_g.
+						     m_DllState);
+
+						break;
+					}
+
+				default:
+					{	// wrong DLL state
+						break;
+					}
+				}
+
+				break;
+			}
+
+		case kEplNmtEventDllCePres:
+			{
+
+				switch (EplDllkInstance_g.m_DllState) {
+				case kEplDllMsWaitPres:
+					{	// PRes received
+						// send next PReq
+						Ret =
+						    EplDllkMnSendPreq
+						    (NmtState_p,
+						     &EplDllkInstance_g.
+						     m_DllState);
+
+						break;
+					}
+
+				default:
+					{	// wrong DLL state
+						break;
+					}
+				}
+
+				break;
+			}
+
+		case kEplNmtEventDllMeSoaTrig:
+			{
+
+				switch (EplDllkInstance_g.m_DllState) {
+				case kEplDllMsWaitSoaTrig:
+					{	// MN PRes sent
+						// send SoA
+						Ret =
+						    EplDllkMnSendSoa(NmtState_p,
+								     &EplDllkInstance_g.
+								     m_DllState,
+								     TRUE);
+
+						break;
+					}
+
+				default:
+					{	// wrong DLL state
+						break;
+					}
+				}
+
+				break;
+			}
+
+		case kEplNmtEventDllCeAsnd:
+			{	// ASnd has been received, but it may be not the requested one
+/*
+                    // report if SoA was correctly answered
+                    Ret = EplDllkAsyncFrameNotReceived(EplDllkInstance_g.m_LastReqServiceId,
+                                                       EplDllkInstance_g.m_uiLastTargetNodeId);
+*/
+				if (EplDllkInstance_g.m_DllState ==
+				    kEplDllMsWaitAsnd) {
+					EplDllkInstance_g.m_DllState =
+					    kEplDllMsWaitSocTrig;
+				}
+				break;
+			}
+
+		default:
+			break;
+		}
+		break;
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+	default:
+		break;
+	}
+
+	if (DllEvent.m_ulDllErrorEvents != 0) {	// error event set -> post it to error handler
+		Event.m_EventSink = kEplEventSinkErrk;
+		Event.m_EventType = kEplEventTypeDllError;
+		// $$$ d.k. set Event.m_NetTime to current time
+		Event.m_uiSize = sizeof(DllEvent);
+		Event.m_pArg = &DllEvent;
+		Ret = EplEventkPost(&Event);
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCbFrameReceived()
+//
+// Description: called from EdrvInterruptHandler()
+//
+// Parameters:  pRxBuffer_p             = receive buffer structure
+//
+// Returns:     (none)
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static void EplDllkCbFrameReceived(tEdrvRxBuffer * pRxBuffer_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplNmtState NmtState;
+	tEplNmtEvent NmtEvent = kEplNmtEventNoEvent;
+	tEplEvent Event;
+	tEplFrame *pFrame;
+	tEplFrame *pTxFrame;
+	tEdrvTxBuffer *pTxBuffer = NULL;
+	tEplFrameInfo FrameInfo;
+	tEplMsgType MsgType;
+	tEplDllReqServiceId ReqServiceId;
+	unsigned int uiAsndServiceId;
+	unsigned int uiNodeId;
+	BYTE bFlag1;
+
+	BENCHMARK_MOD_02_SET(3);
+	NmtState = EplNmtkGetNmtState();
+
+	if (NmtState <= kEplNmtGsResetConfiguration) {
+		goto Exit;
+	}
+
+	pFrame = (tEplFrame *) pRxBuffer_p->m_pbBuffer;
+
+#if EDRV_EARLY_RX_INT != FALSE
+	switch (pRxBuffer_p->m_BufferInFrame) {
+	case kEdrvBufferFirstInFrame:
+		{
+			MsgType =
+			    (tEplMsgType) AmiGetByteFromLe(&pFrame->
+							   m_le_bMessageType);
+			if (MsgType == kEplMsgTypePreq) {
+				if (EplDllkInstance_g.m_DllState == kEplDllCsWaitPreq) {	// PReq expected and actually received
+					// d.k.: The condition above is sufficent, because EPL cycle is active
+					//       and no non-EPL frame shall be received in isochronous phase.
+					// start transmission PRes
+					// $$$ What if Tx buffer is invalid?
+					pTxBuffer =
+					    &EplDllkInstance_g.
+					    m_pTxBuffer[EPL_DLLK_TXFRAME_PRES];
+#if (EPL_DLL_PRES_READY_AFTER_SOA != FALSE) || (EPL_DLL_PRES_READY_AFTER_SOC != FALSE)
+					Ret = EdrvTxMsgStart(pTxBuffer);
+#else
+					pTxFrame =
+					    (tEplFrame *) pTxBuffer->m_pbBuffer;
+					// update frame (NMT state, RD, RS, PR, MS, EN flags)
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+						       m_le_bNmtStatus,
+						       (BYTE) NmtState);
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+						       m_le_bFlag2,
+						       EplDllkInstance_g.
+						       m_bFlag2);
+					if (NmtState != kEplNmtCsOperational) {	// mark PDO as invalid in NMT state Op
+						// $$$ reset only RD flag; set other flags appropriately
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Pres.
+							       m_le_bFlag1, 0);
+					}
+					// $$$ make function that updates Pres, StatusRes
+					// send PRes frame
+					Ret = EdrvSendTxMsg(pTxBuffer);
+#endif
+				}
+			}
+			goto Exit;
+		}
+
+	case kEdrvBufferMiddleInFrame:
+		{
+			goto Exit;
+		}
+
+	case kEdrvBufferLastInFrame:
+		{
+			break;
+		}
+	}
+#endif
+
+	FrameInfo.m_pFrame = pFrame;
+	FrameInfo.m_uiFrameSize = pRxBuffer_p->m_uiRxMsgLen;
+	FrameInfo.m_NetTime.m_dwNanoSec = pRxBuffer_p->m_NetTime.m_dwNanoSec;
+	FrameInfo.m_NetTime.m_dwSec = pRxBuffer_p->m_NetTime.m_dwSec;
+
+	if (AmiGetWordFromBe(&pFrame->m_be_wEtherType) != EPL_C_DLL_ETHERTYPE_EPL) {	// non-EPL frame
+		//TRACE2("EplDllkCbFrameReceived: pfnCbAsync=0x%p SrcMAC=0x%llx\n", EplDllkInstance_g.m_pfnCbAsync, AmiGetQword48FromBe(pFrame->m_be_abSrcMac));
+		if (EplDllkInstance_g.m_pfnCbAsync != NULL) {	// handler for async frames is registered
+			EplDllkInstance_g.m_pfnCbAsync(&FrameInfo);
+		}
+
+		goto Exit;
+	}
+
+	MsgType = (tEplMsgType) AmiGetByteFromLe(&pFrame->m_le_bMessageType);
+	switch (MsgType) {
+	case kEplMsgTypePreq:
+		{
+			// PReq frame
+			// d.k.: (we assume that this PReq frame is intended for us and don't check DstNodeId)
+			if (AmiGetByteFromLe(&pFrame->m_le_bDstNodeId) != EplDllkInstance_g.m_DllConfigParam.m_uiNodeId) {	// this PReq is not intended for us
+				goto Exit;
+			}
+			NmtEvent = kEplNmtEventDllCePreq;
+
+			if (NmtState >= kEplNmtMsNotActive) {	// MN is active -> wrong msg type
+				break;
+			}
+#if EDRV_EARLY_RX_INT == FALSE
+			if (NmtState >= kEplNmtCsPreOperational2) {	// respond to and process PReq frames only in PreOp2, ReadyToOp and Op
+				// Does PRes exist?
+				pTxBuffer =
+				    &EplDllkInstance_g.
+				    m_pTxBuffer[EPL_DLLK_TXFRAME_PRES];
+				if (pTxBuffer->m_pbBuffer != NULL) {	// PRes does exist
+#if (EPL_DLL_PRES_READY_AFTER_SOA != FALSE) || (EPL_DLL_PRES_READY_AFTER_SOC != FALSE)
+					EdrvTxMsgStart(pTxBuffer);
+#else
+					pTxFrame =
+					    (tEplFrame *) pTxBuffer->m_pbBuffer;
+					// update frame (NMT state, RD, RS, PR, MS, EN flags)
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+						       m_le_bNmtStatus,
+						       (BYTE) NmtState);
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+						       m_le_bFlag2,
+						       EplDllkInstance_g.
+						       m_bFlag2);
+					bFlag1 =
+					    AmiGetByteFromLe(&pFrame->m_Data.
+							     m_Preq.
+							     m_le_bFlag1);
+					// save EA flag
+					EplDllkInstance_g.m_bMnFlag1 =
+					    (EplDllkInstance_g.
+					     m_bMnFlag1 & ~EPL_FRAME_FLAG1_EA)
+					    | (bFlag1 & EPL_FRAME_FLAG1_EA);
+					// preserve MS flag
+					bFlag1 &= EPL_FRAME_FLAG1_MS;
+					// add EN flag from Error signaling module
+					bFlag1 |=
+					    EplDllkInstance_g.
+					    m_bFlag1 & EPL_FRAME_FLAG1_EN;
+					if (NmtState != kEplNmtCsOperational) {	// mark PDO as invalid in NMT state Op
+						// reset only RD flag
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Pres.
+							       m_le_bFlag1,
+							       bFlag1);
+					} else {	// leave RD flag untouched
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Pres.
+							       m_le_bFlag1,
+							       (AmiGetByteFromLe
+								(&pTxFrame->
+								 m_Data.m_Pres.
+								 m_le_bFlag1) &
+								EPL_FRAME_FLAG1_RD)
+							       | bFlag1);
+					}
+					// $$$ update EPL_DLL_PRES_READY_AFTER_* code
+					// send PRes frame
+					Ret = EdrvSendTxMsg(pTxBuffer);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+#endif
+				}
+#endif
+				// inform PDO module
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+				if (NmtState >= kEplNmtCsReadyToOperate) {	// inform PDO module only in ReadyToOp and Op
+					if (NmtState != kEplNmtCsOperational) {
+						// reset RD flag and all other flags, but that does not matter, because they were processed above
+						AmiSetByteToLe(&pFrame->m_Data.
+							       m_Preq.
+							       m_le_bFlag1, 0);
+					}
+					// compares real frame size and PDO size
+					if ((unsigned
+					     int)(AmiGetWordFromLe(&pFrame->
+								   m_Data.
+								   m_Preq.
+								   m_le_wSize) +
+						  24)
+					    > FrameInfo.m_uiFrameSize) {	// format error
+						tEplErrorHandlerkEvent DllEvent;
+
+						DllEvent.m_ulDllErrorEvents =
+						    EPL_DLL_ERR_INVALID_FORMAT;
+						DllEvent.m_uiNodeId =
+						    AmiGetByteFromLe(&pFrame->
+								     m_le_bSrcNodeId);
+						DllEvent.m_NmtState = NmtState;
+						Event.m_EventSink =
+						    kEplEventSinkErrk;
+						Event.m_EventType =
+						    kEplEventTypeDllError;
+						Event.m_NetTime =
+						    FrameInfo.m_NetTime;
+						Event.m_uiSize =
+						    sizeof(DllEvent);
+						Event.m_pArg = &DllEvent;
+						Ret = EplEventkPost(&Event);
+						break;
+					}
+					// forward PReq frame as RPDO to PDO module
+					Ret = EplPdokCbPdoReceived(&FrameInfo);
+
+				}
+#if (EPL_DLL_PRES_READY_AFTER_SOC != FALSE)
+				if (pTxBuffer->m_pbBuffer != NULL) {	// PRes does exist
+					// inform PDO module about PRes after PReq
+					FrameInfo.m_pFrame =
+					    (tEplFrame *) pTxBuffer->m_pbBuffer;
+					FrameInfo.m_uiFrameSize =
+					    pTxBuffer->m_uiMaxBufferLen;
+					Ret =
+					    EplPdokCbPdoTransmitted(&FrameInfo);
+				}
+#endif
+#endif
+
+#if EDRV_EARLY_RX_INT == FALSE
+				// $$$ inform emergency protocol handling (error signaling module) about flags
+			}
+#endif
+
+			// reset cycle counter
+			EplDllkInstance_g.m_uiCycleCount = 0;
+
+			break;
+		}
+
+	case kEplMsgTypePres:
+		{
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			tEplDllkNodeInfo *pIntNodeInfo;
+			tEplHeartbeatEvent HeartbeatEvent;
+#endif
+
+			// PRes frame
+			NmtEvent = kEplNmtEventDllCePres;
+
+			uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bSrcNodeId);
+
+			if ((NmtState >= kEplNmtCsPreOperational2)
+			    && (NmtState <= kEplNmtCsOperational)) {	// process PRes frames only in PreOp2, ReadyToOp and Op of CN
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+				pIntNodeInfo = EplDllkGetNodeInfo(uiNodeId);
+				if (pIntNodeInfo == NULL) {	// no node info structure available
+					Ret = kEplDllNoNodeInfo;
+					goto Exit;
+				}
+			} else if (EplDllkInstance_g.m_DllState == kEplDllMsWaitPres) {	// or process PRes frames in MsWaitPres
+
+				pIntNodeInfo = EplDllkInstance_g.m_pCurNodeInfo;
+				if ((pIntNodeInfo == NULL) || (pIntNodeInfo->m_uiNodeId != uiNodeId)) {	// ignore PRes, because it is from wrong CN
+					// $$$ maybe post event to NmtMn module
+					goto Exit;
+				}
+				// forward Flag2 to asynchronous scheduler
+				bFlag1 =
+				    AmiGetByteFromLe(&pFrame->m_Data.m_Asnd.
+						     m_Payload.m_StatusResponse.
+						     m_le_bFlag2);
+				Ret =
+				    EplDllkCalAsyncSetPendingRequests(uiNodeId,
+								      ((tEplDllAsyncReqPriority) ((bFlag1 & EPL_FRAME_FLAG2_PR) >> EPL_FRAME_FLAG2_PR_SHIFT)), (bFlag1 & EPL_FRAME_FLAG2_RS));
+
+#endif
+			} else {	// ignore PRes, because it was received in wrong NMT state
+				// but execute EplDllkChangeState() and post event to NMT module
+				break;
+			}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			{	// check NMT state of CN
+				HeartbeatEvent.m_wErrorCode = EPL_E_NO_ERROR;
+				HeartbeatEvent.m_NmtState =
+				    (tEplNmtState) (AmiGetByteFromLe
+						    (&pFrame->m_Data.m_Pres.
+						     m_le_bNmtStatus) |
+						    EPL_NMT_TYPE_CS);
+				if (pIntNodeInfo->m_NmtState != HeartbeatEvent.m_NmtState) {	// NMT state of CN has changed -> post event to NmtMnu module
+					if (pIntNodeInfo->m_fSoftDelete == FALSE) {	// normal isochronous CN
+						HeartbeatEvent.m_uiNodeId =
+						    uiNodeId;
+						Event.m_EventSink =
+						    kEplEventSinkNmtMnu;
+						Event.m_EventType =
+						    kEplEventTypeHeartbeat;
+						Event.m_uiSize =
+						    sizeof(HeartbeatEvent);
+						Event.m_pArg = &HeartbeatEvent;
+					} else {	// CN shall be deleted softly
+						Event.m_EventSink =
+						    kEplEventSinkDllkCal;
+						Event.m_EventType =
+						    kEplEventTypeDllkSoftDelNode;
+						Event.m_uiSize =
+						    sizeof(unsigned int);
+						Event.m_pArg =
+						    &pIntNodeInfo->m_uiNodeId;
+					}
+					Event.m_NetTime = FrameInfo.m_NetTime;
+					Ret = EplEventkPost(&Event);
+
+					// save current NMT state of CN in internal node structure
+					pIntNodeInfo->m_NmtState =
+					    HeartbeatEvent.m_NmtState;
+				}
+			}
+#endif
+
+			// inform PDO module
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+			if ((NmtState != kEplNmtCsPreOperational2)
+			    && (NmtState != kEplNmtMsPreOperational2)) {	// inform PDO module only in ReadyToOp and Op
+				// compare real frame size and PDO size?
+				if (((unsigned
+				      int)(AmiGetWordFromLe(&pFrame->m_Data.
+							    m_Pres.m_le_wSize) +
+					   24)
+				     > FrameInfo.m_uiFrameSize)
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+				    ||
+				    (AmiGetWordFromLe
+				     (&pFrame->m_Data.m_Pres.m_le_wSize) >
+				     pIntNodeInfo->m_wPresPayloadLimit)
+#endif
+				    ) {	// format error
+					tEplErrorHandlerkEvent DllEvent;
+
+					DllEvent.m_ulDllErrorEvents =
+					    EPL_DLL_ERR_INVALID_FORMAT;
+					DllEvent.m_uiNodeId = uiNodeId;
+					DllEvent.m_NmtState = NmtState;
+					Event.m_EventSink = kEplEventSinkErrk;
+					Event.m_EventType =
+					    kEplEventTypeDllError;
+					Event.m_NetTime = FrameInfo.m_NetTime;
+					Event.m_uiSize = sizeof(DllEvent);
+					Event.m_pArg = &DllEvent;
+					Ret = EplEventkPost(&Event);
+					break;
+				}
+				if ((NmtState != kEplNmtCsOperational)
+				    && (NmtState != kEplNmtMsOperational)) {
+					// reset RD flag and all other flags, but that does not matter, because they were processed above
+					AmiSetByteToLe(&pFrame->m_Data.m_Pres.
+						       m_le_bFlag1, 0);
+				}
+				Ret = EplPdokCbPdoReceived(&FrameInfo);
+			}
+#endif
+
+			break;
+		}
+
+	case kEplMsgTypeSoc:
+		{
+			// SoC frame
+			NmtEvent = kEplNmtEventDllCeSoc;
+
+			if (NmtState >= kEplNmtMsNotActive) {	// MN is active -> wrong msg type
+				break;
+			}
+#if EPL_DLL_PRES_READY_AFTER_SOC != FALSE
+			// post PRes to transmit FIFO of the ethernet controller, but don't start
+			// transmission over bus
+			pTxBuffer =
+			    &EplDllkInstance_g.
+			    m_pTxBuffer[EPL_DLLK_TXFRAME_PRES];
+			// Does PRes exist?
+			if (pTxBuffer->m_pbBuffer != NULL) {	// PRes does exist
+				pTxFrame = (tEplFrame *) pTxBuffer->m_pbBuffer;
+				// update frame (NMT state, RD, RS, PR, MS, EN flags)
+				if (NmtState < kEplNmtCsPreOperational2) {	// NMT state is not PreOp2, ReadyToOp or Op
+					// fake NMT state PreOp2, because PRes will be sent only in PreOp2 or greater
+					NmtState = kEplNmtCsPreOperational2;
+				}
+				AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+					       m_le_bNmtStatus,
+					       (BYTE) NmtState);
+				AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+					       m_le_bFlag2,
+					       EplDllkInstance_g.m_bFlag2);
+				if (NmtState != kEplNmtCsOperational) {	// mark PDO as invalid in NMT state Op
+					// $$$ reset only RD flag; set other flags appropriately
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+						       m_le_bFlag1, 0);
+				}
+				// $$$ make function that updates Pres, StatusRes
+				// mark PRes frame as ready for transmission
+				Ret = EdrvTxMsgReady(pTxBuffer);
+			}
+#endif
+
+			if (NmtState >= kEplNmtCsPreOperational2) {	// SoC frames only in PreOp2, ReadyToOp and Op
+				// trigger synchronous task
+				Event.m_EventSink = kEplEventSinkSync;
+				Event.m_EventType = kEplEventTypeSync;
+				Event.m_uiSize = 0;
+				Ret = EplEventkPost(&Event);
+
+				// update cycle counter
+				if (EplDllkInstance_g.m_DllConfigParam.m_uiMultiplCycleCnt > 0) {	// multiplexed cycle active
+					EplDllkInstance_g.m_uiCycleCount =
+					    (EplDllkInstance_g.m_uiCycleCount +
+					     1) %
+					    EplDllkInstance_g.m_DllConfigParam.
+					    m_uiMultiplCycleCnt;
+				}
+			}
+			// reprogram timer
+#if EPL_TIMER_USE_HIGHRES != FALSE
+			if (EplDllkInstance_g.m_ullFrameTimeout != 0) {
+				Ret =
+				    EplTimerHighReskModifyTimerNs
+				    (&EplDllkInstance_g.m_TimerHdlCycle,
+				     EplDllkInstance_g.m_ullFrameTimeout,
+				     EplDllkCbCnTimer, 0L, FALSE);
+			}
+#endif
+
+			break;
+		}
+
+	case kEplMsgTypeSoa:
+		{
+			// SoA frame
+			NmtEvent = kEplNmtEventDllCeSoa;
+
+			if (NmtState >= kEplNmtMsNotActive) {	// MN is active -> wrong msg type
+				break;
+			}
+
+			pTxFrame = NULL;
+
+			if ((NmtState & EPL_NMT_SUPERSTATE_MASK) != EPL_NMT_CS_EPLMODE) {	// do not respond, if NMT state is < PreOp1 (i.e. not EPL_MODE)
+				break;
+			}
+			// check TargetNodeId
+			uiNodeId =
+			    AmiGetByteFromLe(&pFrame->m_Data.m_Soa.
+					     m_le_bReqServiceTarget);
+			if (uiNodeId == EplDllkInstance_g.m_DllConfigParam.m_uiNodeId) {	// local node is the target of the current request
+
+				// check ServiceId
+				ReqServiceId =
+				    (tEplDllReqServiceId)
+				    AmiGetByteFromLe(&pFrame->m_Data.m_Soa.
+						     m_le_bReqServiceId);
+				if (ReqServiceId == kEplDllReqServiceStatus) {	// StatusRequest
+					if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_STATUSRES].m_pbBuffer != NULL) {	// StatusRes does exist
+
+						pTxFrame =
+						    (tEplFrame *)
+						    EplDllkInstance_g.
+						    m_pTxBuffer
+						    [EPL_DLLK_TXFRAME_STATUSRES].
+						    m_pbBuffer;
+						// update StatusRes frame (NMT state, EN, EC, RS, PR flags)
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Asnd.
+							       m_Payload.
+							       m_StatusResponse.
+							       m_le_bNmtStatus,
+							       (BYTE) NmtState);
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Asnd.
+							       m_Payload.
+							       m_StatusResponse.
+							       m_le_bFlag1,
+							       EplDllkInstance_g.
+							       m_bFlag1);
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Asnd.
+							       m_Payload.
+							       m_StatusResponse.
+							       m_le_bFlag2,
+							       EplDllkInstance_g.
+							       m_bFlag2);
+						// send StatusRes
+						Ret =
+						    EdrvSendTxMsg
+						    (&EplDllkInstance_g.
+						     m_pTxBuffer
+						     [EPL_DLLK_TXFRAME_STATUSRES]);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						TGT_DBG_SIGNAL_TRACE_POINT(8);
+
+						// update error signaling
+						bFlag1 =
+						    AmiGetByteFromLe(&pFrame->
+								     m_Data.
+								     m_Soa.
+								     m_le_bFlag1);
+						if (((bFlag1 ^ EplDllkInstance_g.m_bMnFlag1) & EPL_FRAME_FLAG1_ER) != 0) {	// exception reset flag was changed by MN
+							// assume same state for EC in next cycle (clear all other bits)
+							if ((bFlag1 &
+							     EPL_FRAME_FLAG1_ER)
+							    != 0) {
+								// set EC and reset rest
+								EplDllkInstance_g.
+								    m_bFlag1 =
+								    EPL_FRAME_FLAG1_EC;
+							} else {
+								// reset complete flag 1 (including EC and EN)
+								EplDllkInstance_g.
+								    m_bFlag1 =
+								    0;
+							}
+						}
+						// save flag 1 from MN for Status request response cycle
+						EplDllkInstance_g.m_bMnFlag1 =
+						    bFlag1;
+					}
+				} else if (ReqServiceId == kEplDllReqServiceIdent) {	// IdentRequest
+					if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_IDENTRES].m_pbBuffer != NULL) {	// IdentRes does exist
+						pTxFrame =
+						    (tEplFrame *)
+						    EplDllkInstance_g.
+						    m_pTxBuffer
+						    [EPL_DLLK_TXFRAME_IDENTRES].
+						    m_pbBuffer;
+						// update IdentRes frame (NMT state, RS, PR flags)
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Asnd.
+							       m_Payload.
+							       m_IdentResponse.
+							       m_le_bNmtStatus,
+							       (BYTE) NmtState);
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Asnd.
+							       m_Payload.
+							       m_IdentResponse.
+							       m_le_bFlag2,
+							       EplDllkInstance_g.
+							       m_bFlag2);
+						// send IdentRes
+						Ret =
+						    EdrvSendTxMsg
+						    (&EplDllkInstance_g.
+						     m_pTxBuffer
+						     [EPL_DLLK_TXFRAME_IDENTRES]);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						TGT_DBG_SIGNAL_TRACE_POINT(7);
+					}
+				} else if (ReqServiceId == kEplDllReqServiceNmtRequest) {	// NmtRequest
+					if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_pbBuffer != NULL) {	// NmtRequest does exist
+						// check if frame is not empty and not being filled
+						if (EplDllkInstance_g.
+						    m_pTxBuffer
+						    [EPL_DLLK_TXFRAME_NMTREQ].
+						    m_uiTxMsgLen >
+						    EPL_DLLK_BUFLEN_FILLING) {
+							/*if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_uiTxMsgLen < EPL_DLLK_BUFLEN_MIN)
+							   {   // pad frame
+							   EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_uiTxMsgLen = EPL_DLLK_BUFLEN_MIN;
+							   } */
+							// memorize transmission
+							pTxFrame =
+							    (tEplFrame *) 1;
+							// send NmtRequest
+							Ret =
+							    EdrvSendTxMsg
+							    (&EplDllkInstance_g.
+							     m_pTxBuffer
+							     [EPL_DLLK_TXFRAME_NMTREQ]);
+							if (Ret !=
+							    kEplSuccessful) {
+								goto Exit;
+							}
+
+						}
+					}
+
+				} else if (ReqServiceId == kEplDllReqServiceUnspecified) {	// unspecified invite
+					if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NONEPL].m_pbBuffer != NULL) {	// non-EPL frame does exist
+						// check if frame is not empty and not being filled
+						if (EplDllkInstance_g.
+						    m_pTxBuffer
+						    [EPL_DLLK_TXFRAME_NONEPL].
+						    m_uiTxMsgLen >
+						    EPL_DLLK_BUFLEN_FILLING) {
+							/*if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_uiTxMsgLen < EPL_DLLK_BUFLEN_MIN)
+							   {   // pad frame
+							   EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_uiTxMsgLen = EPL_DLLK_BUFLEN_MIN;
+							   } */
+							// memorize transmission
+							pTxFrame =
+							    (tEplFrame *) 1;
+							// send non-EPL frame
+							Ret =
+							    EdrvSendTxMsg
+							    (&EplDllkInstance_g.
+							     m_pTxBuffer
+							     [EPL_DLLK_TXFRAME_NONEPL]);
+							if (Ret !=
+							    kEplSuccessful) {
+								goto Exit;
+							}
+
+						}
+					}
+
+				} else if (ReqServiceId == kEplDllReqServiceNo) {	// no async service requested -> do nothing
+				}
+			}
+#if EPL_DLL_PRES_READY_AFTER_SOA != FALSE
+			if (pTxFrame == NULL) {	// signal process function readiness of PRes frame
+				Event.m_EventSink = kEplEventSinkDllk;
+				Event.m_EventType = kEplEventTypeDllkPresReady;
+				Event.m_uiSize = 0;
+				Event.m_pArg = NULL;
+				Ret = EplEventkPost(&Event);
+			}
+#endif
+
+			// inform PDO module
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+//            Ret = EplPdokCbSoa(&FrameInfo);
+#endif
+
+			// $$$ put SrcNodeId, NMT state and NetTime as HeartbeatEvent into eventqueue
+
+			// $$$ inform emergency protocol handling about flags
+			break;
+		}
+
+	case kEplMsgTypeAsnd:
+		{
+			// ASnd frame
+			NmtEvent = kEplNmtEventDllCeAsnd;
+
+			// ASnd service registered?
+			uiAsndServiceId =
+			    (unsigned int)AmiGetByteFromLe(&pFrame->m_Data.
+							   m_Asnd.
+							   m_le_bServiceId);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			if ((EplDllkInstance_g.m_DllState >= kEplDllMsNonCyclic)
+			    &&
+			    ((((tEplDllAsndServiceId) uiAsndServiceId) ==
+			      kEplDllAsndStatusResponse)
+			     || (((tEplDllAsndServiceId) uiAsndServiceId) == kEplDllAsndIdentResponse))) {	// StatusRes or IdentRes received
+				uiNodeId =
+				    AmiGetByteFromLe(&pFrame->m_le_bSrcNodeId);
+				if ((EplDllkInstance_g.m_LastReqServiceId ==
+				     ((tEplDllReqServiceId) uiAsndServiceId))
+				    && (uiNodeId == EplDllkInstance_g.m_uiLastTargetNodeId)) {	// mark request as responded
+					EplDllkInstance_g.m_LastReqServiceId =
+					    kEplDllReqServiceNo;
+				}
+				if (((tEplDllAsndServiceId) uiAsndServiceId) == kEplDllAsndIdentResponse) {	// memorize MAC address of CN for PReq
+					tEplDllkNodeInfo *pIntNodeInfo;
+
+					pIntNodeInfo =
+					    EplDllkGetNodeInfo(uiNodeId);
+					if (pIntNodeInfo == NULL) {	// no node info structure available
+						Ret = kEplDllNoNodeInfo;
+					} else {
+						EPL_MEMCPY(pIntNodeInfo->
+							   m_be_abMacAddr,
+							   pFrame->
+							   m_be_abSrcMac, 6);
+					}
+				}
+				// forward Flag2 to asynchronous scheduler
+				bFlag1 =
+				    AmiGetByteFromLe(&pFrame->m_Data.m_Asnd.
+						     m_Payload.m_StatusResponse.
+						     m_le_bFlag2);
+				Ret =
+				    EplDllkCalAsyncSetPendingRequests(uiNodeId,
+								      ((tEplDllAsyncReqPriority) ((bFlag1 & EPL_FRAME_FLAG2_PR) >> EPL_FRAME_FLAG2_PR_SHIFT)), (bFlag1 & EPL_FRAME_FLAG2_RS));
+			}
+#endif
+
+			if (uiAsndServiceId < EPL_DLL_MAX_ASND_SERVICE_ID) {	// ASnd service ID is valid
+				if (EplDllkInstance_g.m_aAsndFilter[uiAsndServiceId] == kEplDllAsndFilterAny) {	// ASnd service ID is registered
+					// forward frame via async receive FIFO to userspace
+					Ret =
+					    EplDllkCalAsyncFrameReceived
+					    (&FrameInfo);
+				} else if (EplDllkInstance_g.m_aAsndFilter[uiAsndServiceId] == kEplDllAsndFilterLocal) {	// ASnd service ID is registered, but only local node ID or broadcasts
+					// shall be forwarded
+					uiNodeId =
+					    AmiGetByteFromLe(&pFrame->
+							     m_le_bDstNodeId);
+					if ((uiNodeId ==
+					     EplDllkInstance_g.m_DllConfigParam.
+					     m_uiNodeId)
+					    || (uiNodeId == EPL_C_ADR_BROADCAST)) {	// ASnd frame is intended for us
+						// forward frame via async receive FIFO to userspace
+						Ret =
+						    EplDllkCalAsyncFrameReceived
+						    (&FrameInfo);
+					}
+				}
+			}
+			break;
+		}
+
+	default:
+		{
+			break;
+		}
+	}
+
+	if (NmtEvent != kEplNmtEventNoEvent) {	// event for DLL and NMT state machine generated
+		Ret = EplDllkChangeState(NmtEvent, NmtState);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		if ((NmtEvent != kEplNmtEventDllCeAsnd)
+		    && ((NmtState <= kEplNmtCsPreOperational1) || (NmtEvent != kEplNmtEventDllCePres))) {	// NMT state machine is not interested in ASnd frames and PRes frames when not CsNotActive or CsPreOp1
+			// inform NMT module
+			Event.m_EventSink = kEplEventSinkNmtk;
+			Event.m_EventType = kEplEventTypeNmtEvent;
+			Event.m_uiSize = sizeof(NmtEvent);
+			Event.m_pArg = &NmtEvent;
+			Ret = EplEventkPost(&Event);
+		}
+	}
+
+      Exit:
+	if (Ret != kEplSuccessful) {
+		DWORD dwArg;
+
+		BENCHMARK_MOD_02_TOGGLE(9);
+
+		dwArg = EplDllkInstance_g.m_DllState | (NmtEvent << 8);
+
+		// Error event for API layer
+		Ret = EplEventkPostError(kEplEventSourceDllk,
+					 Ret, sizeof(dwArg), &dwArg);
+	}
+	BENCHMARK_MOD_02_RESET(3);
+	return;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCbFrameTransmitted()
+//
+// Description: called from EdrvInterruptHandler().
+//              It signals
+//
+// Parameters:  pRxBuffer_p             = receive buffer structure
+//
+// Returns:     (none)
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static void EplDllkCbFrameTransmitted(tEdrvTxBuffer * pTxBuffer_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+	tEplDllAsyncReqPriority Priority;
+	tEplNmtState NmtState;
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0) \
+    && (EPL_DLL_PRES_READY_AFTER_SOC == FALSE)
+	tEplFrameInfo FrameInfo;
+#endif
+
+	NmtState = EplNmtkGetNmtState();
+
+	if (NmtState <= kEplNmtGsResetConfiguration) {
+		goto Exit;
+	}
+
+	if ((pTxBuffer_p - EplDllkInstance_g.m_pTxBuffer) == EPL_DLLK_TXFRAME_NMTREQ) {	// frame from NMT request FIFO sent
+		// mark Tx-buffer as empty
+		pTxBuffer_p->m_uiTxMsgLen = EPL_DLLK_BUFLEN_EMPTY;
+
+		// post event to DLL
+		Priority = kEplDllAsyncReqPrioNmt;
+		Event.m_EventSink = kEplEventSinkDllk;
+		Event.m_EventType = kEplEventTypeDllkFillTx;
+		EPL_MEMSET(&Event.m_NetTime, 0x00, sizeof(Event.m_NetTime));
+		Event.m_pArg = &Priority;
+		Event.m_uiSize = sizeof(Priority);
+		Ret = EplEventkPost(&Event);
+	} else if ((pTxBuffer_p - EplDllkInstance_g.m_pTxBuffer) == EPL_DLLK_TXFRAME_NONEPL) {	// frame from generic priority FIFO sent
+		// mark Tx-buffer as empty
+		pTxBuffer_p->m_uiTxMsgLen = EPL_DLLK_BUFLEN_EMPTY;
+
+		// post event to DLL
+		Priority = kEplDllAsyncReqPrioGeneric;
+		Event.m_EventSink = kEplEventSinkDllk;
+		Event.m_EventType = kEplEventTypeDllkFillTx;
+		EPL_MEMSET(&Event.m_NetTime, 0x00, sizeof(Event.m_NetTime));
+		Event.m_pArg = &Priority;
+		Event.m_uiSize = sizeof(Priority);
+		Ret = EplEventkPost(&Event);
+	}
+#if ((((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0) \
+    && (EPL_DLL_PRES_READY_AFTER_SOC == FALSE)) \
+    || (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	else if ((pTxBuffer_p->m_EplMsgType == kEplMsgTypePreq)
+		 || (pTxBuffer_p->m_EplMsgType == kEplMsgTypePres)) {	// PRes resp. PReq frame sent
+
+#if ((((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0) \
+            && (EPL_DLL_PRES_READY_AFTER_SOC == FALSE))
+		{
+			// inform PDO module
+			FrameInfo.m_pFrame =
+			    (tEplFrame *) pTxBuffer_p->m_pbBuffer;
+			FrameInfo.m_uiFrameSize = pTxBuffer_p->m_uiMaxBufferLen;
+			Ret = EplPdokCbPdoTransmitted(&FrameInfo);
+		}
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+		{
+			// if own Pres on MN, trigger SoA
+			if ((NmtState >= kEplNmtMsPreOperational2)
+			    && (pTxBuffer_p ==
+				&EplDllkInstance_g.
+				m_pTxBuffer[EPL_DLLK_TXFRAME_PRES])) {
+				Ret =
+				    EplDllkChangeState(kEplNmtEventDllMeSoaTrig,
+						       NmtState);
+			}
+		}
+#endif
+
+#if EPL_DLL_PRES_READY_AFTER_SOA != FALSE
+		goto Exit;
+#endif
+	}
+#endif
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	else if (pTxBuffer_p->m_EplMsgType == kEplMsgTypeSoa) {	// SoA frame sent
+		tEplNmtEvent NmtEvent = kEplNmtEventDllMeSoaSent;
+
+		// check if we are invited
+		if (EplDllkInstance_g.m_uiLastTargetNodeId ==
+		    EplDllkInstance_g.m_DllConfigParam.m_uiNodeId) {
+			tEplFrame *pTxFrame;
+
+			if (EplDllkInstance_g.m_LastReqServiceId == kEplDllReqServiceStatus) {	// StatusRequest
+				if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_STATUSRES].m_pbBuffer != NULL) {	// StatusRes does exist
+
+					pTxFrame =
+					    (tEplFrame *) EplDllkInstance_g.
+					    m_pTxBuffer
+					    [EPL_DLLK_TXFRAME_STATUSRES].
+					    m_pbBuffer;
+					// update StatusRes frame (NMT state, EN, EC, RS, PR flags)
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Asnd.
+						       m_Payload.
+						       m_StatusResponse.
+						       m_le_bNmtStatus,
+						       (BYTE) NmtState);
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Asnd.
+						       m_Payload.
+						       m_StatusResponse.
+						       m_le_bFlag1,
+						       EplDllkInstance_g.
+						       m_bFlag1);
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Asnd.
+						       m_Payload.
+						       m_StatusResponse.
+						       m_le_bFlag2,
+						       EplDllkInstance_g.
+						       m_bFlag2);
+					// send StatusRes
+					Ret =
+					    EdrvSendTxMsg(&EplDllkInstance_g.
+							  m_pTxBuffer
+							  [EPL_DLLK_TXFRAME_STATUSRES]);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+					TGT_DBG_SIGNAL_TRACE_POINT(8);
+
+				}
+			} else if (EplDllkInstance_g.m_LastReqServiceId == kEplDllReqServiceIdent) {	// IdentRequest
+				if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_IDENTRES].m_pbBuffer != NULL) {	// IdentRes does exist
+					pTxFrame =
+					    (tEplFrame *) EplDllkInstance_g.
+					    m_pTxBuffer
+					    [EPL_DLLK_TXFRAME_IDENTRES].
+					    m_pbBuffer;
+					// update IdentRes frame (NMT state, RS, PR flags)
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Asnd.
+						       m_Payload.
+						       m_IdentResponse.
+						       m_le_bNmtStatus,
+						       (BYTE) NmtState);
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Asnd.
+						       m_Payload.
+						       m_IdentResponse.
+						       m_le_bFlag2,
+						       EplDllkInstance_g.
+						       m_bFlag2);
+					// send IdentRes
+					Ret =
+					    EdrvSendTxMsg(&EplDllkInstance_g.
+							  m_pTxBuffer
+							  [EPL_DLLK_TXFRAME_IDENTRES]);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+					TGT_DBG_SIGNAL_TRACE_POINT(7);
+				}
+			} else if (EplDllkInstance_g.m_LastReqServiceId == kEplDllReqServiceNmtRequest) {	// NmtRequest
+				if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_pbBuffer != NULL) {	// NmtRequest does exist
+					// check if frame is not empty and not being filled
+					if (EplDllkInstance_g.
+					    m_pTxBuffer
+					    [EPL_DLLK_TXFRAME_NMTREQ].
+					    m_uiTxMsgLen >
+					    EPL_DLLK_BUFLEN_FILLING) {
+						// check if this frame is a NMT command,
+						// then forward this frame back to NmtMnu module,
+						// because it needs the time, when this frame is
+						// actually sent, to start the timer for monitoring
+						// the NMT state change.
+
+						pTxFrame =
+						    (tEplFrame *)
+						    EplDllkInstance_g.
+						    m_pTxBuffer
+						    [EPL_DLLK_TXFRAME_NMTREQ].
+						    m_pbBuffer;
+						if ((AmiGetByteFromLe
+						     (&pTxFrame->
+						      m_le_bMessageType)
+						     == (BYTE) kEplMsgTypeAsnd)
+						    &&
+						    (AmiGetByteFromLe
+						     (&pTxFrame->m_Data.m_Asnd.
+						      m_le_bServiceId)
+						     == (BYTE) kEplDllAsndNmtCommand)) {	// post event directly to NmtMnu module
+							Event.m_EventSink =
+							    kEplEventSinkNmtMnu;
+							Event.m_EventType =
+							    kEplEventTypeNmtMnuNmtCmdSent;
+							Event.m_uiSize =
+							    EplDllkInstance_g.
+							    m_pTxBuffer
+							    [EPL_DLLK_TXFRAME_NMTREQ].
+							    m_uiTxMsgLen;
+							Event.m_pArg = pTxFrame;
+							Ret =
+							    EplEventkPost
+							    (&Event);
+
+						}
+						// send NmtRequest
+						Ret =
+						    EdrvSendTxMsg
+						    (&EplDllkInstance_g.
+						     m_pTxBuffer
+						     [EPL_DLLK_TXFRAME_NMTREQ]);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+
+					}
+				}
+
+			} else if (EplDllkInstance_g.m_LastReqServiceId == kEplDllReqServiceUnspecified) {	// unspecified invite
+				if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NONEPL].m_pbBuffer != NULL) {	// non-EPL frame does exist
+					// check if frame is not empty and not being filled
+					if (EplDllkInstance_g.
+					    m_pTxBuffer
+					    [EPL_DLLK_TXFRAME_NONEPL].
+					    m_uiTxMsgLen >
+					    EPL_DLLK_BUFLEN_FILLING) {
+						// send non-EPL frame
+						Ret =
+						    EdrvSendTxMsg
+						    (&EplDllkInstance_g.
+						     m_pTxBuffer
+						     [EPL_DLLK_TXFRAME_NONEPL]);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+
+					}
+				}
+			}
+			// ASnd frame was sent, remove the request
+			EplDllkInstance_g.m_LastReqServiceId =
+			    kEplDllReqServiceNo;
+		}
+		// forward event to ErrorHandler and PDO module
+		Event.m_EventSink = kEplEventSinkNmtk;
+		Event.m_EventType = kEplEventTypeNmtEvent;
+		Event.m_uiSize = sizeof(NmtEvent);
+		Event.m_pArg = &NmtEvent;
+		Ret = EplEventkPost(&Event);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+#endif
+
+#if EPL_DLL_PRES_READY_AFTER_SOA != FALSE
+	else {			// d.k.: Why that else? on CN it is entered on IdentRes and StatusRes
+		goto Exit;
+	}
+
+	// signal process function readiness of PRes frame
+	Event.m_EventSink = kEplEventSinkDllk;
+	Event.m_EventType = kEplEventTypeDllkPresReady;
+	Event.m_uiSize = 0;
+	Event.m_pArg = NULL;
+	Ret = EplEventkPost(&Event);
+
+#endif
+
+      Exit:
+	if (Ret != kEplSuccessful) {
+		DWORD dwArg;
+
+		BENCHMARK_MOD_02_TOGGLE(9);
+
+		dwArg =
+		    EplDllkInstance_g.m_DllState | (pTxBuffer_p->
+						    m_EplMsgType << 16);
+
+		// Error event for API layer
+		Ret = EplEventkPostError(kEplEventSourceDllk,
+					 Ret, sizeof(dwArg), &dwArg);
+	}
+
+	return;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCheckFrame()
+//
+// Description: check frame and set missing information
+//
+// Parameters:  pFrame_p                = ethernet frame
+//              uiFrameSize_p           = size of frame
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDllkCheckFrame(tEplFrame * pFrame_p,
+				    unsigned int uiFrameSize_p)
+{
+	tEplMsgType MsgType;
+	WORD wEtherType;
+
+	// check frame
+	if (pFrame_p != NULL) {
+		// check SrcMAC
+		if (AmiGetQword48FromBe(pFrame_p->m_be_abSrcMac) == 0) {
+			// source MAC address
+			EPL_MEMCPY(&pFrame_p->m_be_abSrcMac[0],
+				   &EplDllkInstance_g.m_be_abSrcMac[0], 6);
+		}
+		// check ethertype
+		wEtherType = AmiGetWordFromBe(&pFrame_p->m_be_wEtherType);
+		if (wEtherType == 0) {
+			// assume EPL frame
+			wEtherType = EPL_C_DLL_ETHERTYPE_EPL;
+			AmiSetWordToBe(&pFrame_p->m_be_wEtherType, wEtherType);
+		}
+
+		if (wEtherType == EPL_C_DLL_ETHERTYPE_EPL) {
+			// source node ID
+			AmiSetByteToLe(&pFrame_p->m_le_bSrcNodeId,
+				       (BYTE) EplDllkInstance_g.
+				       m_DllConfigParam.m_uiNodeId);
+
+			// check message type
+			MsgType =
+			    AmiGetByteFromLe(&pFrame_p->m_le_bMessageType);
+			if (MsgType == 0) {
+				MsgType = kEplMsgTypeAsnd;
+				AmiSetByteToLe(&pFrame_p->m_le_bMessageType,
+					       (BYTE) MsgType);
+			}
+
+			if (MsgType == kEplMsgTypeAsnd) {
+				// destination MAC address
+				AmiSetQword48ToBe(&pFrame_p->m_be_abDstMac[0],
+						  EPL_C_DLL_MULTICAST_ASND);
+			}
+
+		}
+	}
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCbCnTimer()
+//
+// Description: called by timer module. It monitors the EPL cycle when it is a CN.
+//
+// Parameters:  pEventArg_p             = timer event argument
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+static tEplKernel PUBLIC EplDllkCbCnTimer(tEplTimerEventArg * pEventArg_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplNmtState NmtState;
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+	if (pEventArg_p->m_TimerHdl != EplDllkInstance_g.m_TimerHdlCycle) {	// zombie callback
+		// just exit
+		goto Exit;
+	}
+#endif
+
+	NmtState = EplNmtkGetNmtState();
+
+	if (NmtState <= kEplNmtGsResetConfiguration) {
+		goto Exit;
+	}
+
+	Ret = EplDllkChangeState(kEplNmtEventDllCeFrameTimeout, NmtState);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// 2008/10/15 d.k. reprogramming of timer not necessary,
+	// because it will be programmed, when SoC is received.
+/*
+    // reprogram timer
+#if EPL_TIMER_USE_HIGHRES != FALSE
+    if ((NmtState > kEplNmtCsPreOperational1)
+        && (EplDllkInstance_g.m_ullFrameTimeout != 0))
+    {
+        Ret = EplTimerHighReskModifyTimerNs(&EplDllkInstance_g.m_TimerHdlCycle, EplDllkInstance_g.m_ullFrameTimeout, EplDllkCbCnTimer, 0L, FALSE);
+    }
+#endif
+*/
+
+      Exit:
+	if (Ret != kEplSuccessful) {
+		DWORD dwArg;
+
+		BENCHMARK_MOD_02_TOGGLE(9);
+
+		dwArg =
+		    EplDllkInstance_g.
+		    m_DllState | (kEplNmtEventDllCeFrameTimeout << 8);
+
+		// Error event for API layer
+		Ret = EplEventkPostError(kEplEventSourceDllk,
+					 Ret, sizeof(dwArg), &dwArg);
+	}
+
+	return Ret;
+}
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCbMnTimerCycle()
+//
+// Description: called by timer module. It triggers the SoC when it is a MN.
+//
+// Parameters:  pEventArg_p             = timer event argument
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplDllkCbMnTimerCycle(tEplTimerEventArg * pEventArg_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplNmtState NmtState;
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+	if (pEventArg_p->m_TimerHdl != EplDllkInstance_g.m_TimerHdlCycle) {	// zombie callback
+		// just exit
+		goto Exit;
+	}
+#endif
+
+	NmtState = EplNmtkGetNmtState();
+
+	if (NmtState <= kEplNmtGsResetConfiguration) {
+		goto Exit;
+	}
+
+	Ret = EplDllkChangeState(kEplNmtEventDllMeSocTrig, NmtState);
+
+      Exit:
+	if (Ret != kEplSuccessful) {
+		DWORD dwArg;
+
+		BENCHMARK_MOD_02_TOGGLE(9);
+
+		dwArg =
+		    EplDllkInstance_g.
+		    m_DllState | (kEplNmtEventDllMeSocTrig << 8);
+
+		// Error event for API layer
+		Ret = EplEventkPostError(kEplEventSourceDllk,
+					 Ret, sizeof(dwArg), &dwArg);
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCbMnTimerResponse()
+//
+// Description: called by timer module. It monitors the PRes timeout.
+//
+// Parameters:  pEventArg_p             = timer event argument
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplDllkCbMnTimerResponse(tEplTimerEventArg *
+						  pEventArg_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplNmtState NmtState;
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+	if (pEventArg_p->m_TimerHdl != EplDllkInstance_g.m_TimerHdlResponse) {	// zombie callback
+		// just exit
+		goto Exit;
+	}
+#endif
+
+	NmtState = EplNmtkGetNmtState();
+
+	if (NmtState <= kEplNmtGsResetConfiguration) {
+		goto Exit;
+	}
+
+	Ret = EplDllkChangeState(kEplNmtEventDllMePresTimeout, NmtState);
+
+      Exit:
+	if (Ret != kEplSuccessful) {
+		DWORD dwArg;
+
+		BENCHMARK_MOD_02_TOGGLE(9);
+
+		dwArg =
+		    EplDllkInstance_g.
+		    m_DllState | (kEplNmtEventDllMePresTimeout << 8);
+
+		// Error event for API layer
+		Ret = EplEventkPostError(kEplEventSourceDllk,
+					 Ret, sizeof(dwArg), &dwArg);
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkGetNodeInfo()
+//
+// Description: returns node info structure of the specified node.
+//
+// Parameters:  uiNodeId_p              = node ID
+//
+// Returns:     tEplDllkNodeInfo*       = pointer to internal node info structure
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplDllkNodeInfo *EplDllkGetNodeInfo(unsigned int uiNodeId_p)
+{
+	// $$$ d.k.: use hash algorithm to retrieve the appropriate node info structure
+	//           if size of array is less than 254.
+	uiNodeId_p--;		// node ID starts at 1 but array at 0
+	if (uiNodeId_p >= tabentries(EplDllkInstance_g.m_aNodeInfo)) {
+		return NULL;
+	} else {
+		return &EplDllkInstance_g.m_aNodeInfo[uiNodeId_p];
+	}
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkMnSendSoa()
+//
+// Description: it updates and transmits the SoA.
+//
+// Parameters:  NmtState_p              = current NMT state
+//              pDllStateProposed_p     = proposed DLL state
+//              fEnableInvitation_p     = enable invitation for asynchronous phase
+//                                        it will be disabled for EPL_C_DLL_PREOP1_START_CYCLES SoAs
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDllkMnSendSoa(tEplNmtState NmtState_p,
+				   tEplDllState * pDllStateProposed_p,
+				   BOOL fEnableInvitation_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEdrvTxBuffer *pTxBuffer = NULL;
+	tEplFrame *pTxFrame;
+	tEplDllkNodeInfo *pNodeInfo;
+
+	*pDllStateProposed_p = kEplDllMsNonCyclic;
+
+	pTxBuffer = &EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_SOA];
+	if (pTxBuffer->m_pbBuffer != NULL) {	// SoA does exist
+		pTxFrame = (tEplFrame *) pTxBuffer->m_pbBuffer;
+
+		if (fEnableInvitation_p != FALSE) {	// fetch target of asynchronous phase
+			if (EplDllkInstance_g.m_bFlag2 == 0) {	// own queues are empty
+				EplDllkInstance_g.m_LastReqServiceId =
+				    kEplDllReqServiceNo;
+			} else if (((tEplDllAsyncReqPriority) (EplDllkInstance_g.m_bFlag2 >> EPL_FRAME_FLAG2_PR_SHIFT)) == kEplDllAsyncReqPrioNmt) {	// frames in own NMT request queue available
+				EplDllkInstance_g.m_LastReqServiceId =
+				    kEplDllReqServiceNmtRequest;
+			} else {
+				EplDllkInstance_g.m_LastReqServiceId =
+				    kEplDllReqServiceUnspecified;
+			}
+			Ret =
+			    EplDllkCalAsyncGetSoaRequest(&EplDllkInstance_g.
+							 m_LastReqServiceId,
+							 &EplDllkInstance_g.
+							 m_uiLastTargetNodeId);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+			if (EplDllkInstance_g.m_LastReqServiceId != kEplDllReqServiceNo) {	// asynchronous phase will be assigned to one node
+				if (EplDllkInstance_g.m_uiLastTargetNodeId == EPL_C_ADR_INVALID) {	// exchange invalid node ID with local node ID
+					EplDllkInstance_g.m_uiLastTargetNodeId =
+					    EplDllkInstance_g.m_DllConfigParam.
+					    m_uiNodeId;
+					// d.k. DLL state WaitAsndTrig is not helpful;
+					//      so just step over to WaitSocTrig,
+					//      because own ASnd is sent automatically in CbFrameTransmitted() after SoA.
+					//*pDllStateProposed_p = kEplDllMsWaitAsndTrig;
+					*pDllStateProposed_p =
+					    kEplDllMsWaitSocTrig;
+				} else {	// assignment to CN
+					*pDllStateProposed_p =
+					    kEplDllMsWaitAsnd;
+				}
+
+				pNodeInfo =
+				    EplDllkGetNodeInfo(EplDllkInstance_g.
+						       m_uiLastTargetNodeId);
+				if (pNodeInfo == NULL) {	// no node info structure available
+					Ret = kEplDllNoNodeInfo;
+					goto Exit;
+				}
+				// update frame (EA, ER flags)
+				AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.
+					       m_le_bFlag1,
+					       pNodeInfo->
+					       m_bSoaFlag1 & (EPL_FRAME_FLAG1_EA
+							      |
+							      EPL_FRAME_FLAG1_ER));
+			} else {	// no assignment of asynchronous phase
+				*pDllStateProposed_p = kEplDllMsWaitSocTrig;
+				EplDllkInstance_g.m_uiLastTargetNodeId =
+				    EPL_C_ADR_INVALID;
+			}
+
+			// update frame (target)
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.
+				       m_le_bReqServiceId,
+				       (BYTE) EplDllkInstance_g.
+				       m_LastReqServiceId);
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.
+				       m_le_bReqServiceTarget,
+				       (BYTE) EplDllkInstance_g.
+				       m_uiLastTargetNodeId);
+
+		} else {	// invite nobody
+			// update frame (target)
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.
+				       m_le_bReqServiceId, (BYTE) 0);
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.
+				       m_le_bReqServiceTarget, (BYTE) 0);
+		}
+
+		// update frame (NMT state)
+		AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.m_le_bNmtStatus,
+			       (BYTE) NmtState_p);
+
+		// send SoA frame
+		Ret = EdrvSendTxMsg(pTxBuffer);
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkMnSendSoc()
+//
+// Description: it updates and transmits the SoA.
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDllkMnSendSoc(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEdrvTxBuffer *pTxBuffer = NULL;
+	tEplFrame *pTxFrame;
+	tEplEvent Event;
+
+	pTxBuffer = &EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_SOC];
+	if (pTxBuffer->m_pbBuffer != NULL) {	// SoC does exist
+		pTxFrame = (tEplFrame *) pTxBuffer->m_pbBuffer;
+
+		// $$$ update NetTime
+
+		// send SoC frame
+		Ret = EdrvSendTxMsg(pTxBuffer);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		// trigger synchronous task
+		Event.m_EventSink = kEplEventSinkSync;
+		Event.m_EventType = kEplEventTypeSync;
+		Event.m_uiSize = 0;
+		Ret = EplEventkPost(&Event);
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkMnSendPreq()
+//
+// Description: it updates and transmits the PReq for the next isochronous CN
+//              or own PRes if enabled.
+//
+// Parameters:  NmtState_p              = current NMT state
+//              pDllStateProposed_p     = proposed DLL state
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDllkMnSendPreq(tEplNmtState NmtState_p,
+				    tEplDllState * pDllStateProposed_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEdrvTxBuffer *pTxBuffer = NULL;
+	tEplFrame *pTxFrame;
+	BYTE bFlag1 = 0;
+
+	if (EplDllkInstance_g.m_pCurNodeInfo == NULL) {	// start with first isochronous CN
+		EplDllkInstance_g.m_pCurNodeInfo =
+		    EplDllkInstance_g.m_pFirstNodeInfo;
+	} else {		// iterate to next isochronous CN
+		EplDllkInstance_g.m_pCurNodeInfo =
+		    EplDllkInstance_g.m_pCurNodeInfo->m_pNextNodeInfo;
+	}
+
+	if (EplDllkInstance_g.m_pCurNodeInfo == NULL) {	// last isochronous CN reached
+		Ret = EplDllkMnSendSoa(NmtState_p, pDllStateProposed_p, TRUE);
+		goto Exit;
+	} else {
+		pTxBuffer = EplDllkInstance_g.m_pCurNodeInfo->m_pPreqTxBuffer;
+		bFlag1 =
+		    EplDllkInstance_g.m_pCurNodeInfo->
+		    m_bSoaFlag1 & EPL_FRAME_FLAG1_EA;
+		*pDllStateProposed_p = kEplDllMsWaitPres;
+
+		// start PRes Timer
+		// $$$ d.k.: maybe move this call to CbFrameTransmitted(), because the time should run from there
+#if EPL_TIMER_USE_HIGHRES != FALSE
+		Ret =
+		    EplTimerHighReskModifyTimerNs(&EplDllkInstance_g.
+						  m_TimerHdlResponse,
+						  EplDllkInstance_g.
+						  m_pCurNodeInfo->
+						  m_dwPresTimeout,
+						  EplDllkCbMnTimerResponse, 0L,
+						  FALSE);
+#endif
+	}
+
+	if (pTxBuffer == NULL) {	// PReq does not exist
+		Ret = kEplDllTxBufNotReady;
+		goto Exit;
+	}
+
+	pTxFrame = (tEplFrame *) pTxBuffer->m_pbBuffer;
+
+	if (pTxFrame != NULL) {	// PReq does exist
+		if (NmtState_p == kEplNmtMsOperational) {	// leave RD flag untouched
+			bFlag1 |=
+			    AmiGetByteFromLe(&pTxFrame->m_Data.m_Preq.
+					     m_le_bFlag1) & EPL_FRAME_FLAG1_RD;
+		}
+
+		if (pTxBuffer == &EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_PRES]) {	// PRes of MN will be sent
+			// update NMT state
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.m_le_bNmtStatus,
+				       (BYTE) NmtState_p);
+			*pDllStateProposed_p = kEplDllMsWaitSoaTrig;
+		}
+		// $$$ d.k. set EPL_FRAME_FLAG1_MS if necessary
+		// update frame (Flag1)
+		AmiSetByteToLe(&pTxFrame->m_Data.m_Preq.m_le_bFlag1, bFlag1);
+
+		// calculate frame size from payload size
+		pTxBuffer->m_uiTxMsgLen =
+		    AmiGetWordFromLe(&pTxFrame->m_Data.m_Preq.m_le_wSize) + 24;
+
+		// send PReq frame
+		Ret = EdrvSendTxMsg(pTxBuffer);
+	} else {
+		Ret = kEplDllTxFrameInvalid;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkAsyncFrameNotReceived()
+//
+// Description: passes empty ASnd frame to receive FIFO.
+//              It will be called only for frames with registered AsndServiceIds
+//              (only kEplDllAsndFilterAny).
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDllkAsyncFrameNotReceived(tEplDllReqServiceId
+					       ReqServiceId_p,
+					       unsigned int uiNodeId_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	BYTE abBuffer[18];
+	tEplFrame *pFrame = (tEplFrame *) abBuffer;
+	tEplFrameInfo FrameInfo;
+
+	// check if previous SoA invitation was not answered
+	switch (ReqServiceId_p) {
+	case kEplDllReqServiceIdent:
+	case kEplDllReqServiceStatus:
+		// ASnd service registered?
+		if (EplDllkInstance_g.m_aAsndFilter[ReqServiceId_p] == kEplDllAsndFilterAny) {	// ASnd service ID is registered
+			AmiSetByteToLe(&pFrame->m_le_bSrcNodeId,
+				       (BYTE) uiNodeId_p);
+			// EPL MsgType ASnd
+			AmiSetByteToLe(&pFrame->m_le_bMessageType,
+				       (BYTE) kEplMsgTypeAsnd);
+			// ASnd Service ID
+			AmiSetByteToLe(&pFrame->m_Data.m_Asnd.m_le_bServiceId,
+				       (BYTE) ReqServiceId_p);
+			// create frame info structure
+			FrameInfo.m_pFrame = pFrame;
+			FrameInfo.m_uiFrameSize = 18;	// empty non existing ASnd frame
+			// forward frame via async receive FIFO to userspace
+			Ret = EplDllkCalAsyncFrameReceived(&FrameInfo);
+		}
+		break;
+	default:
+		// no invitation issued or it was successfully answered or it is uninteresting
+		break;
+	}
+
+	return Ret;
+}
+
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+// EOF
diff --git a/drivers/staging/epl/EplDllkCal.c b/drivers/staging/epl/EplDllkCal.c
new file mode 100644
index 000000000000..359083ebe12d
--- /dev/null
+++ b/drivers/staging/epl/EplDllkCal.c
@@ -0,0 +1,1260 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for kernel DLL Communication Abstraction Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDllkCal.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.7 $  $Date: 2008/11/13 17:13:09 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/15 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "kernel/EplDllkCal.h"
+#include "kernel/EplDllk.h"
+#include "kernel/EplEventk.h"
+
+#include "EplDllCal.h"
+#ifndef EPL_NO_FIFO
+#include "SharedBuff.h"
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef min
+#define min(a,b)            (((a) < (b)) ? (a) : (b))
+#endif
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplDllkCal                                          */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define EPL_DLLKCAL_MAX_QUEUES  5	// CnGenReq, CnNmtReq, {MnGenReq, MnNmtReq}, MnIdentReq, MnStatusReq
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+#ifndef EPL_NO_FIFO
+//    tShbInstance    m_ShbInstanceRx;      // FIFO for Rx ASnd frames
+	tShbInstance m_ShbInstanceTxNmt;	// FIFO for Tx frames with NMT request priority
+	tShbInstance m_ShbInstanceTxGen;	// FIFO for Tx frames with generic priority
+#else
+	unsigned int m_uiFrameSizeNmt;
+	BYTE m_abFrameNmt[1500];
+	unsigned int m_uiFrameSizeGen;
+	BYTE m_abFrameGen[1500];
+#endif
+
+	tEplDllkCalStatistics m_Statistics;
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	// IdentRequest queue with CN node IDs
+	unsigned int m_auiQueueIdentReq[EPL_D_NMT_MaxCNNumber_U8 + 1];	// 1 entry is reserved to distinguish between full and empty
+	unsigned int m_uiWriteIdentReq;
+	unsigned int m_uiReadIdentReq;
+
+	// StatusRequest queue with CN node IDs
+	unsigned int m_auiQueueStatusReq[EPL_D_NMT_MaxCNNumber_U8 + 1];	// 1 entry is reserved to distinguish between full and empty
+	unsigned int m_uiWriteStatusReq;
+	unsigned int m_uiReadStatusReq;
+
+	unsigned int m_auiQueueCnRequests[254 * 2];
+	// first 254 entries represent the generic requests of the corresponding node
+	// second 254 entries represent the NMT requests of the corresponding node
+	unsigned int m_uiNextQueueCnRequest;
+	unsigned int m_uiNextRequestQueue;
+#endif
+
+} tEplDllkCalInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+// if no dynamic memory allocation shall be used
+// define structures statically
+static tEplDllkCalInstance EplDllkCalInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAddInstance()
+//
+// Description: add and initialize new instance of DLL CAL module
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAddInstance()
+{
+	tEplKernel Ret = kEplSuccessful;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+	unsigned int fShbNewCreated;
+
+/*    ShbError = ShbCirAllocBuffer (EPL_DLLCAL_BUFFER_SIZE_RX, EPL_DLLCAL_BUFFER_ID_RX,
+        &EplDllkCalInstance_g.m_ShbInstanceRx, &fShbNewCreated);
+    // returns kShbOk, kShbOpenMismatch, kShbOutOfMem or kShbInvalidArg
+
+    if (ShbError != kShbOk)
+    {
+        Ret = kEplNoResource;
+    }
+*/
+	ShbError =
+	    ShbCirAllocBuffer(EPL_DLLCAL_BUFFER_SIZE_TX_NMT,
+			      EPL_DLLCAL_BUFFER_ID_TX_NMT,
+			      &EplDllkCalInstance_g.m_ShbInstanceTxNmt,
+			      &fShbNewCreated);
+	// returns kShbOk, kShbOpenMismatch, kShbOutOfMem or kShbInvalidArg
+
+	if (ShbError != kShbOk) {
+		Ret = kEplNoResource;
+	}
+
+/*    ShbError = ShbCirSetSignalHandlerNewData (EplDllkCalInstance_g.m_ShbInstanceTxNmt, EplDllkCalTxNmtSignalHandler, kShbPriorityNormal);
+    // returns kShbOk, kShbAlreadySignaling or kShbInvalidArg
+
+    if (ShbError != kShbOk)
+    {
+        Ret = kEplNoResource;
+    }
+*/
+	ShbError =
+	    ShbCirAllocBuffer(EPL_DLLCAL_BUFFER_SIZE_TX_GEN,
+			      EPL_DLLCAL_BUFFER_ID_TX_GEN,
+			      &EplDllkCalInstance_g.m_ShbInstanceTxGen,
+			      &fShbNewCreated);
+	// returns kShbOk, kShbOpenMismatch, kShbOutOfMem or kShbInvalidArg
+
+	if (ShbError != kShbOk) {
+		Ret = kEplNoResource;
+	}
+
+/*    ShbError = ShbCirSetSignalHandlerNewData (EplDllkCalInstance_g.m_ShbInstanceTxGen, EplDllkCalTxGenSignalHandler, kShbPriorityNormal);
+    // returns kShbOk, kShbAlreadySignaling or kShbInvalidArg
+
+    if (ShbError != kShbOk)
+    {
+        Ret = kEplNoResource;
+    }
+*/
+#else
+	EplDllkCalInstance_g.m_uiFrameSizeNmt = 0;
+	EplDllkCalInstance_g.m_uiFrameSizeGen = 0;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalDelInstance()
+//
+// Description: deletes instance of DLL CAL module
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalDelInstance()
+{
+	tEplKernel Ret = kEplSuccessful;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+
+/*    ShbError = ShbCirReleaseBuffer (EplDllkCalInstance_g.m_ShbInstanceRx);
+    if (ShbError != kShbOk)
+    {
+        Ret = kEplNoResource;
+    }
+    EplDllkCalInstance_g.m_ShbInstanceRx = NULL;
+*/
+	ShbError = ShbCirReleaseBuffer(EplDllkCalInstance_g.m_ShbInstanceTxNmt);
+	if (ShbError != kShbOk) {
+		Ret = kEplNoResource;
+	}
+	EplDllkCalInstance_g.m_ShbInstanceTxNmt = NULL;
+
+	ShbError = ShbCirReleaseBuffer(EplDllkCalInstance_g.m_ShbInstanceTxGen);
+	if (ShbError != kShbOk) {
+		Ret = kEplNoResource;
+	}
+	EplDllkCalInstance_g.m_ShbInstanceTxGen = NULL;
+
+#else
+	EplDllkCalInstance_g.m_uiFrameSizeNmt = 0;
+	EplDllkCalInstance_g.m_uiFrameSizeGen = 0;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalProcess
+//
+// Description: process the passed configuration
+//
+// Parameters:  pEvent_p                = event containing configuration options
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalProcess(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	switch (pEvent_p->m_EventType) {
+	case kEplEventTypeDllkServFilter:
+		{
+			tEplDllCalAsndServiceIdFilter *pServFilter;
+
+			pServFilter =
+			    (tEplDllCalAsndServiceIdFilter *) pEvent_p->m_pArg;
+			Ret =
+			    EplDllkSetAsndServiceIdFilter(pServFilter->
+							  m_ServiceId,
+							  pServFilter->
+							  m_Filter);
+			break;
+		}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	case kEplEventTypeDllkIssueReq:
+		{
+			tEplDllCalIssueRequest *pIssueReq;
+
+			pIssueReq = (tEplDllCalIssueRequest *) pEvent_p->m_pArg;
+			Ret =
+			    EplDllkCalIssueRequest(pIssueReq->m_Service,
+						   pIssueReq->m_uiNodeId,
+						   pIssueReq->m_bSoaFlag1);
+			break;
+		}
+
+	case kEplEventTypeDllkAddNode:
+		{
+			tEplDllNodeInfo *pNodeInfo;
+
+			pNodeInfo = (tEplDllNodeInfo *) pEvent_p->m_pArg;
+			Ret = EplDllkAddNode(pNodeInfo);
+			break;
+		}
+
+	case kEplEventTypeDllkDelNode:
+		{
+			unsigned int *puiNodeId;
+
+			puiNodeId = (unsigned int *)pEvent_p->m_pArg;
+			Ret = EplDllkDeleteNode(*puiNodeId);
+			break;
+		}
+
+	case kEplEventTypeDllkSoftDelNode:
+		{
+			unsigned int *puiNodeId;
+
+			puiNodeId = (unsigned int *)pEvent_p->m_pArg;
+			Ret = EplDllkSoftDeleteNode(*puiNodeId);
+			break;
+		}
+#endif
+
+	case kEplEventTypeDllkIdentity:
+		{
+			tEplDllIdentParam *pIdentParam;
+
+			pIdentParam = (tEplDllIdentParam *) pEvent_p->m_pArg;
+			if (pIdentParam->m_uiSizeOfStruct > pEvent_p->m_uiSize) {
+				pIdentParam->m_uiSizeOfStruct =
+				    pEvent_p->m_uiSize;
+			}
+			Ret = EplDllkSetIdentity(pIdentParam);
+			break;
+		}
+
+	case kEplEventTypeDllkConfig:
+		{
+			tEplDllConfigParam *pConfigParam;
+
+			pConfigParam = (tEplDllConfigParam *) pEvent_p->m_pArg;
+			if (pConfigParam->m_uiSizeOfStruct > pEvent_p->m_uiSize) {
+				pConfigParam->m_uiSizeOfStruct =
+				    pEvent_p->m_uiSize;
+			}
+			Ret = EplDllkConfig(pConfigParam);
+			break;
+		}
+
+	default:
+		break;
+	}
+
+//Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncGetTxCount()
+//
+// Description: returns count of Tx frames of FIFO with highest priority
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAsyncGetTxCount(tEplDllAsyncReqPriority * pPriority_p,
+				     unsigned int *puiCount_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+	unsigned long ulFrameCount;
+
+	// get frame count of Tx FIFO with NMT request priority
+	ShbError =
+	    ShbCirGetReadBlockCount(EplDllkCalInstance_g.m_ShbInstanceTxNmt,
+				    &ulFrameCount);
+	// returns kShbOk, kShbInvalidArg
+
+	// error handling
+	if (ShbError != kShbOk) {
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+	if (ulFrameCount >
+	    EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountNmt) {
+		EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountNmt =
+		    ulFrameCount;
+	}
+
+	if (ulFrameCount != 0) {	// NMT requests are in queue
+		*pPriority_p = kEplDllAsyncReqPrioNmt;
+		*puiCount_p = (unsigned int)ulFrameCount;
+		goto Exit;
+	}
+	// get frame count of Tx FIFO with generic priority
+	ShbError =
+	    ShbCirGetReadBlockCount(EplDllkCalInstance_g.m_ShbInstanceTxGen,
+				    &ulFrameCount);
+	// returns kShbOk, kShbInvalidArg
+
+	// error handling
+	if (ShbError != kShbOk) {
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+	if (ulFrameCount >
+	    EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountGen) {
+		EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountGen =
+		    ulFrameCount;
+	}
+
+	*pPriority_p = kEplDllAsyncReqPrioGeneric;
+	*puiCount_p = (unsigned int)ulFrameCount;
+
+      Exit:
+#else
+	if (EplDllkCalInstance_g.m_uiFrameSizeNmt > 0) {
+		*pPriority_p = kEplDllAsyncReqPrioNmt;
+		*puiCount_p = 1;
+	} else if (EplDllkCalInstance_g.m_uiFrameSizeGen > 0) {
+		*pPriority_p = kEplDllAsyncReqPrioGeneric;
+		*puiCount_p = 1;
+	} else {
+		*pPriority_p = kEplDllAsyncReqPrioGeneric;
+		*puiCount_p = 0;
+	}
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncGetTxFrame()
+//
+// Description: returns Tx frames from FIFO with specified priority
+//
+// Parameters:  pFrame_p                = IN: pointer to buffer
+//              puiFrameSize_p          = IN: max size of buffer
+//                                        OUT: actual size of frame
+//              Priority_p              = IN: priority
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAsyncGetTxFrame(void *pFrame_p,
+				     unsigned int *puiFrameSize_p,
+				     tEplDllAsyncReqPriority Priority_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+	unsigned long ulFrameSize;
+
+	switch (Priority_p) {
+	case kEplDllAsyncReqPrioNmt:	// NMT request priority
+		ShbError =
+		    ShbCirReadDataBlock(EplDllkCalInstance_g.m_ShbInstanceTxNmt,
+					(BYTE *) pFrame_p, *puiFrameSize_p,
+					&ulFrameSize);
+		// returns kShbOk, kShbDataTruncated, kShbInvalidArg, kShbNoReadableData
+		break;
+
+	default:		// generic priority
+		ShbError =
+		    ShbCirReadDataBlock(EplDllkCalInstance_g.m_ShbInstanceTxGen,
+					(BYTE *) pFrame_p, *puiFrameSize_p,
+					&ulFrameSize);
+		// returns kShbOk, kShbDataTruncated, kShbInvalidArg, kShbNoReadableData
+		break;
+
+	}
+
+	// error handling
+	if (ShbError != kShbOk) {
+		if (ShbError == kShbNoReadableData) {
+			Ret = kEplDllAsyncTxBufferEmpty;
+		} else {	// other error
+			Ret = kEplNoResource;
+		}
+		goto Exit;
+	}
+
+	*puiFrameSize_p = (unsigned int)ulFrameSize;
+
+      Exit:
+#else
+	switch (Priority_p) {
+	case kEplDllAsyncReqPrioNmt:	// NMT request priority
+		*puiFrameSize_p =
+		    min(*puiFrameSize_p, EplDllkCalInstance_g.m_uiFrameSizeNmt);
+		EPL_MEMCPY(pFrame_p, EplDllkCalInstance_g.m_abFrameNmt,
+			   *puiFrameSize_p);
+		EplDllkCalInstance_g.m_uiFrameSizeNmt = 0;
+		break;
+
+	default:		// generic priority
+		*puiFrameSize_p =
+		    min(*puiFrameSize_p, EplDllkCalInstance_g.m_uiFrameSizeGen);
+		EPL_MEMCPY(pFrame_p, EplDllkCalInstance_g.m_abFrameGen,
+			   *puiFrameSize_p);
+		EplDllkCalInstance_g.m_uiFrameSizeGen = 0;
+		break;
+	}
+
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncFrameReceived()
+//
+// Description: passes ASnd frame to receive FIFO.
+//              It will be called only for frames with registered AsndServiceIds.
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAsyncFrameReceived(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+
+	Event.m_EventSink = kEplEventSinkDlluCal;
+	Event.m_EventType = kEplEventTypeAsndRx;
+	Event.m_pArg = pFrameInfo_p->m_pFrame;
+	Event.m_uiSize = pFrameInfo_p->m_uiFrameSize;
+	// pass NetTime of frame to userspace
+	Event.m_NetTime = pFrameInfo_p->m_NetTime;
+
+	Ret = EplEventkPost(&Event);
+	if (Ret != kEplSuccessful) {
+		EplDllkCalInstance_g.m_Statistics.m_ulCurRxFrameCount++;
+	} else {
+		EplDllkCalInstance_g.m_Statistics.m_ulMaxRxFrameCount++;
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncSend()
+//
+// Description: puts the given frame into the transmit FIFO with the specified
+//              priority.
+//
+// Parameters:  pFrameInfo_p            = frame info structure
+//              Priority_p              = priority
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAsyncSend(tEplFrameInfo * pFrameInfo_p,
+			       tEplDllAsyncReqPriority Priority_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+
+	switch (Priority_p) {
+	case kEplDllAsyncReqPrioNmt:	// NMT request priority
+		ShbError =
+		    ShbCirWriteDataBlock(EplDllkCalInstance_g.
+					 m_ShbInstanceTxNmt,
+					 pFrameInfo_p->m_pFrame,
+					 pFrameInfo_p->m_uiFrameSize);
+		// returns kShbOk, kShbExceedDataSizeLimit, kShbBufferFull, kShbInvalidArg
+		break;
+
+	default:		// generic priority
+		ShbError =
+		    ShbCirWriteDataBlock(EplDllkCalInstance_g.
+					 m_ShbInstanceTxGen,
+					 pFrameInfo_p->m_pFrame,
+					 pFrameInfo_p->m_uiFrameSize);
+		// returns kShbOk, kShbExceedDataSizeLimit, kShbBufferFull, kShbInvalidArg
+		break;
+
+	}
+
+	// error handling
+	switch (ShbError) {
+	case kShbOk:
+		break;
+
+	case kShbExceedDataSizeLimit:
+		Ret = kEplDllAsyncTxBufferFull;
+		break;
+
+	case kShbBufferFull:
+		Ret = kEplDllAsyncTxBufferFull;
+		break;
+
+	case kShbInvalidArg:
+	default:
+		Ret = kEplNoResource;
+		break;
+	}
+
+#else
+
+	switch (Priority_p) {
+	case kEplDllAsyncReqPrioNmt:	// NMT request priority
+		if (EplDllkCalInstance_g.m_uiFrameSizeNmt == 0) {
+			EPL_MEMCPY(EplDllkCalInstance_g.m_abFrameNmt,
+				   pFrameInfo_p->m_pFrame,
+				   pFrameInfo_p->m_uiFrameSize);
+			EplDllkCalInstance_g.m_uiFrameSizeNmt =
+			    pFrameInfo_p->m_uiFrameSize;
+		} else {
+			Ret = kEplDllAsyncTxBufferFull;
+			goto Exit;
+		}
+		break;
+
+	default:		// generic priority
+		if (EplDllkCalInstance_g.m_uiFrameSizeGen == 0) {
+			EPL_MEMCPY(EplDllkCalInstance_g.m_abFrameGen,
+				   pFrameInfo_p->m_pFrame,
+				   pFrameInfo_p->m_uiFrameSize);
+			EplDllkCalInstance_g.m_uiFrameSizeGen =
+			    pFrameInfo_p->m_uiFrameSize;
+		} else {
+			Ret = kEplDllAsyncTxBufferFull;
+			goto Exit;
+		}
+		break;
+	}
+
+#endif
+
+	// post event to DLL
+	Event.m_EventSink = kEplEventSinkDllk;
+	Event.m_EventType = kEplEventTypeDllkFillTx;
+	EPL_MEMSET(&Event.m_NetTime, 0x00, sizeof(Event.m_NetTime));
+	Event.m_pArg = &Priority_p;
+	Event.m_uiSize = sizeof(Priority_p);
+	Ret = EplEventkPost(&Event);
+
+#ifdef EPL_NO_FIFO
+      Exit:
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncClearBuffer()
+//
+// Description: clears the transmit buffer
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAsyncClearBuffer(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+
+	ShbError =
+	    ShbCirResetBuffer(EplDllkCalInstance_g.m_ShbInstanceTxNmt, 1000,
+			      NULL);
+	ShbError =
+	    ShbCirResetBuffer(EplDllkCalInstance_g.m_ShbInstanceTxGen, 1000,
+			      NULL);
+
+#else
+	EplDllkCalInstance_g.m_uiFrameSizeNmt = 0;
+	EplDllkCalInstance_g.m_uiFrameSizeGen = 0;
+#endif
+
+//    EPL_MEMSET(&EplDllkCalInstance_g.m_Statistics, 0, sizeof (tEplDllkCalStatistics));
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncClearQueues()
+//
+// Description: clears the transmit buffer
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+tEplKernel EplDllkCalAsyncClearQueues(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// clear MN asynchronous queues
+	EplDllkCalInstance_g.m_uiNextQueueCnRequest = 0;
+	EplDllkCalInstance_g.m_uiNextRequestQueue = 0;
+	EplDllkCalInstance_g.m_uiReadIdentReq = 0;
+	EplDllkCalInstance_g.m_uiWriteIdentReq = 0;
+	EplDllkCalInstance_g.m_uiReadStatusReq = 0;
+	EplDllkCalInstance_g.m_uiWriteStatusReq = 0;
+
+	return Ret;
+}
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalGetStatistics()
+//
+// Description: returns statistics of the asynchronous queues.
+//
+// Parameters:  ppStatistics            = statistics structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalGetStatistics(tEplDllkCalStatistics ** ppStatistics)
+{
+	tEplKernel Ret = kEplSuccessful;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+
+	ShbError =
+	    ShbCirGetReadBlockCount(EplDllkCalInstance_g.m_ShbInstanceTxNmt,
+				    &EplDllkCalInstance_g.m_Statistics.
+				    m_ulCurTxFrameCountNmt);
+	ShbError =
+	    ShbCirGetReadBlockCount(EplDllkCalInstance_g.m_ShbInstanceTxGen,
+				    &EplDllkCalInstance_g.m_Statistics.
+				    m_ulCurTxFrameCountGen);
+//    ShbError = ShbCirGetReadBlockCount (EplDllkCalInstance_g.m_ShbInstanceRx, &EplDllkCalInstance_g.m_Statistics.m_ulCurRxFrameCount);
+
+#else
+	if (EplDllkCalInstance_g.m_uiFrameSizeNmt > 0) {
+		EplDllkCalInstance_g.m_Statistics.m_ulCurTxFrameCountNmt = 1;
+	} else {
+		EplDllkCalInstance_g.m_Statistics.m_ulCurTxFrameCountNmt = 0;
+	}
+	if (EplDllkCalInstance_g.m_uiFrameSizeGen > 0) {
+		EplDllkCalInstance_g.m_Statistics.m_ulCurTxFrameCountGen = 1;
+	} else {
+		EplDllkCalInstance_g.m_Statistics.m_ulCurTxFrameCountGen = 0;
+	}
+#endif
+
+	*ppStatistics = &EplDllkCalInstance_g.m_Statistics;
+	return Ret;
+}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalIssueRequest()
+//
+// Description: issues a StatusRequest or a IdentRequest to the specified node.
+//
+// Parameters:  Service_p               = request service ID
+//              uiNodeId_p              = node ID
+//              bSoaFlag1_p             = flag1 for this node (transmit in SoA and PReq)
+//                                        If 0xFF this flag is ignored.
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalIssueRequest(tEplDllReqServiceId Service_p,
+				  unsigned int uiNodeId_p, BYTE bSoaFlag1_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (bSoaFlag1_p != 0xFF) {
+		Ret = EplDllkSetFlag1OfNode(uiNodeId_p, bSoaFlag1_p);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+	// add node to appropriate request queue
+	switch (Service_p) {
+	case kEplDllReqServiceIdent:
+		{
+			if (((EplDllkCalInstance_g.m_uiWriteIdentReq +
+			      1) %
+			     tabentries(EplDllkCalInstance_g.
+					m_auiQueueIdentReq))
+			    == EplDllkCalInstance_g.m_uiReadIdentReq) {	// queue is full
+				Ret = kEplDllAsyncTxBufferFull;
+				goto Exit;
+			}
+			EplDllkCalInstance_g.
+			    m_auiQueueIdentReq[EplDllkCalInstance_g.
+					       m_uiWriteIdentReq] = uiNodeId_p;
+			EplDllkCalInstance_g.m_uiWriteIdentReq =
+			    (EplDllkCalInstance_g.m_uiWriteIdentReq +
+			     1) %
+			    tabentries(EplDllkCalInstance_g.m_auiQueueIdentReq);
+			break;
+		}
+
+	case kEplDllReqServiceStatus:
+		{
+			if (((EplDllkCalInstance_g.m_uiWriteStatusReq +
+			      1) %
+			     tabentries(EplDllkCalInstance_g.
+					m_auiQueueStatusReq))
+			    == EplDllkCalInstance_g.m_uiReadStatusReq) {	// queue is full
+				Ret = kEplDllAsyncTxBufferFull;
+				goto Exit;
+			}
+			EplDllkCalInstance_g.
+			    m_auiQueueStatusReq[EplDllkCalInstance_g.
+						m_uiWriteStatusReq] =
+			    uiNodeId_p;
+			EplDllkCalInstance_g.m_uiWriteStatusReq =
+			    (EplDllkCalInstance_g.m_uiWriteStatusReq +
+			     1) %
+			    tabentries(EplDllkCalInstance_g.
+				       m_auiQueueStatusReq);
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplDllInvalidParam;
+			goto Exit;
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncGetSoaRequest()
+//
+// Description: returns next request for SoA. This function is called by DLLk module.
+//
+// Parameters:  pReqServiceId_p         = pointer to request service ID
+//                                        IN: available request for MN NMT or generic request queue (Flag2.PR)
+//                                            or kEplDllReqServiceNo if queues are empty
+//                                        OUT: next request
+//              puiNodeId_p             = OUT: pointer to node ID of next request
+//                                             = EPL_C_ADR_INVALID, if request is self addressed
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAsyncGetSoaRequest(tEplDllReqServiceId * pReqServiceId_p,
+					unsigned int *puiNodeId_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiCount;
+
+//    *pReqServiceId_p = kEplDllReqServiceNo;
+
+	for (uiCount = EPL_DLLKCAL_MAX_QUEUES; uiCount > 0; uiCount--) {
+		switch (EplDllkCalInstance_g.m_uiNextRequestQueue) {
+		case 0:
+			{	// CnGenReq
+				for (;
+				     EplDllkCalInstance_g.
+				     m_uiNextQueueCnRequest <
+				     (tabentries
+				      (EplDllkCalInstance_g.
+				       m_auiQueueCnRequests) / 2);
+				     EplDllkCalInstance_g.
+				     m_uiNextQueueCnRequest++) {
+					if (EplDllkCalInstance_g.m_auiQueueCnRequests[EplDllkCalInstance_g.m_uiNextQueueCnRequest] > 0) {	// non empty queue found
+						// remove one request from queue
+						EplDllkCalInstance_g.
+						    m_auiQueueCnRequests
+						    [EplDllkCalInstance_g.
+						     m_uiNextQueueCnRequest]--;
+						*puiNodeId_p =
+						    EplDllkCalInstance_g.
+						    m_uiNextQueueCnRequest + 1;
+						*pReqServiceId_p =
+						    kEplDllReqServiceUnspecified;
+						EplDllkCalInstance_g.
+						    m_uiNextQueueCnRequest++;
+						if (EplDllkCalInstance_g.m_uiNextQueueCnRequest >= (tabentries(EplDllkCalInstance_g.m_auiQueueCnRequests) / 2)) {	// last node reached
+							// continue with CnNmtReq queue at next SoA
+							EplDllkCalInstance_g.
+							    m_uiNextRequestQueue
+							    = 1;
+						}
+						goto Exit;
+					}
+				}
+				// all CnGenReq queues are empty -> continue with CnNmtReq queue
+				EplDllkCalInstance_g.m_uiNextRequestQueue = 1;
+				break;
+			}
+
+		case 1:
+			{	// CnNmtReq
+				for (;
+				     EplDllkCalInstance_g.
+				     m_uiNextQueueCnRequest <
+				     tabentries(EplDllkCalInstance_g.
+						m_auiQueueCnRequests);
+				     EplDllkCalInstance_g.
+				     m_uiNextQueueCnRequest++) {
+					if (EplDllkCalInstance_g.m_auiQueueCnRequests[EplDllkCalInstance_g.m_uiNextQueueCnRequest] > 0) {	// non empty queue found
+						// remove one request from queue
+						EplDllkCalInstance_g.
+						    m_auiQueueCnRequests
+						    [EplDllkCalInstance_g.
+						     m_uiNextQueueCnRequest]--;
+						*puiNodeId_p =
+						    EplDllkCalInstance_g.
+						    m_uiNextQueueCnRequest + 1 -
+						    (tabentries
+						     (EplDllkCalInstance_g.
+						      m_auiQueueCnRequests) /
+						     2);
+						*pReqServiceId_p =
+						    kEplDllReqServiceNmtRequest;
+						EplDllkCalInstance_g.
+						    m_uiNextQueueCnRequest++;
+						if (EplDllkCalInstance_g.m_uiNextQueueCnRequest > tabentries(EplDllkCalInstance_g.m_auiQueueCnRequests)) {	// last node reached
+							// restart CnGenReq queue
+							EplDllkCalInstance_g.
+							    m_uiNextQueueCnRequest
+							    = 0;
+							// continue with MnGenReq queue at next SoA
+							EplDllkCalInstance_g.
+							    m_uiNextRequestQueue
+							    = 2;
+						}
+						goto Exit;
+					}
+				}
+				// restart CnGenReq queue
+				EplDllkCalInstance_g.m_uiNextQueueCnRequest = 0;
+				// all CnNmtReq queues are empty -> continue with MnGenReq queue
+				EplDllkCalInstance_g.m_uiNextRequestQueue = 2;
+				break;
+			}
+
+		case 2:
+			{	// MnNmtReq and MnGenReq
+				// next queue will be MnIdentReq queue
+				EplDllkCalInstance_g.m_uiNextRequestQueue = 3;
+				if (*pReqServiceId_p != kEplDllReqServiceNo) {
+					*puiNodeId_p = EPL_C_ADR_INVALID;	// DLLk must exchange this with the actual node ID
+					goto Exit;
+				}
+				break;
+			}
+
+		case 3:
+			{	// MnIdentReq
+				// next queue will be MnStatusReq queue
+				EplDllkCalInstance_g.m_uiNextRequestQueue = 4;
+				if (EplDllkCalInstance_g.m_uiReadIdentReq != EplDllkCalInstance_g.m_uiWriteIdentReq) {	// queue is not empty
+					*puiNodeId_p =
+					    EplDllkCalInstance_g.
+					    m_auiQueueIdentReq
+					    [EplDllkCalInstance_g.
+					     m_uiReadIdentReq];
+					EplDllkCalInstance_g.m_uiReadIdentReq =
+					    (EplDllkCalInstance_g.
+					     m_uiReadIdentReq +
+					     1) %
+					    tabentries(EplDllkCalInstance_g.
+						       m_auiQueueIdentReq);
+					*pReqServiceId_p =
+					    kEplDllReqServiceIdent;
+					goto Exit;
+				}
+				break;
+			}
+
+		case 4:
+			{	// MnStatusReq
+				// next queue will be CnGenReq queue
+				EplDllkCalInstance_g.m_uiNextRequestQueue = 0;
+				if (EplDllkCalInstance_g.m_uiReadStatusReq != EplDllkCalInstance_g.m_uiWriteStatusReq) {	// queue is not empty
+					*puiNodeId_p =
+					    EplDllkCalInstance_g.
+					    m_auiQueueStatusReq
+					    [EplDllkCalInstance_g.
+					     m_uiReadStatusReq];
+					EplDllkCalInstance_g.m_uiReadStatusReq =
+					    (EplDllkCalInstance_g.
+					     m_uiReadStatusReq +
+					     1) %
+					    tabentries(EplDllkCalInstance_g.
+						       m_auiQueueStatusReq);
+					*pReqServiceId_p =
+					    kEplDllReqServiceStatus;
+					goto Exit;
+				}
+				break;
+			}
+
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncSetPendingRequests()
+//
+// Description: sets the pending asynchronous frame requests of the specified node.
+//              This will add the node to the asynchronous request scheduler.
+//
+// Parameters:  uiNodeId_p              = node ID
+//              AsyncReqPrio_p          = asynchronous request priority
+//              uiCount_p               = count of asynchronous frames
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAsyncSetPendingRequests(unsigned int uiNodeId_p,
+					     tEplDllAsyncReqPriority
+					     AsyncReqPrio_p,
+					     unsigned int uiCount_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// add node to appropriate request queue
+	switch (AsyncReqPrio_p) {
+	case kEplDllAsyncReqPrioNmt:
+		{
+			uiNodeId_p--;
+			if (uiNodeId_p >=
+			    (tabentries
+			     (EplDllkCalInstance_g.m_auiQueueCnRequests) / 2)) {
+				Ret = kEplDllInvalidParam;
+				goto Exit;
+			}
+			uiNodeId_p +=
+			    tabentries(EplDllkCalInstance_g.
+				       m_auiQueueCnRequests) / 2;
+			EplDllkCalInstance_g.m_auiQueueCnRequests[uiNodeId_p] =
+			    uiCount_p;
+			break;
+		}
+
+	default:
+		{
+			uiNodeId_p--;
+			if (uiNodeId_p >=
+			    (tabentries
+			     (EplDllkCalInstance_g.m_auiQueueCnRequests) / 2)) {
+				Ret = kEplDllInvalidParam;
+				goto Exit;
+			}
+			EplDllkCalInstance_g.m_auiQueueCnRequests[uiNodeId_p] =
+			    uiCount_p;
+			break;
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//  Callback handler for new data signaling
+//---------------------------------------------------------------------------
+
+#ifndef EPL_NO_FIFO
+/*static void  EplDllkCalTxNmtSignalHandler (
+    tShbInstance pShbRxInstance_p,
+    unsigned long ulDataSize_p)
+{
+tEplKernel      Ret = kEplSuccessful;
+tEplEvent       Event;
+tEplDllAsyncReqPriority Priority;
+#ifndef EPL_NO_FIFO
+tShbError   ShbError;
+unsigned long   ulBlockCount;
+
+    ShbError = ShbCirGetReadBlockCount (EplDllkCalInstance_g.m_ShbInstanceTxNmt, &ulBlockCount);
+    if (ulBlockCount > EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountNmt)
+    {
+        EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountNmt = ulBlockCount;
+    }
+
+#endif
+
+    // post event to DLL
+    Priority = kEplDllAsyncReqPrioNmt;
+    Event.m_EventSink = kEplEventSinkDllk;
+    Event.m_EventType = kEplEventTypeDllkFillTx;
+    EPL_MEMSET(&Event.m_NetTime, 0x00, sizeof(Event.m_NetTime));
+    Event.m_pArg = &Priority;
+    Event.m_uiSize = sizeof(Priority);
+    Ret = EplEventkPost(&Event);
+
+}
+
+static void  EplDllkCalTxGenSignalHandler (
+    tShbInstance pShbRxInstance_p,
+    unsigned long ulDataSize_p)
+{
+tEplKernel      Ret = kEplSuccessful;
+tEplEvent       Event;
+tEplDllAsyncReqPriority Priority;
+#ifndef EPL_NO_FIFO
+tShbError   ShbError;
+unsigned long   ulBlockCount;
+
+    ShbError = ShbCirGetReadBlockCount (EplDllkCalInstance_g.m_ShbInstanceTxGen, &ulBlockCount);
+    if (ulBlockCount > EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountGen)
+    {
+        EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountGen = ulBlockCount;
+    }
+
+#endif
+
+    // post event to DLL
+    Priority = kEplDllAsyncReqPrioGeneric;
+    Event.m_EventSink = kEplEventSinkDllk;
+    Event.m_EventType = kEplEventTypeDllkFillTx;
+    EPL_MEMSET(&Event.m_NetTime, 0x00, sizeof(Event.m_NetTime));
+    Event.m_pArg = &Priority;
+    Event.m_uiSize = sizeof(Priority);
+    Ret = EplEventkPost(&Event);
+
+}
+*/
+#endif
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplDlluCal.c b/drivers/staging/epl/EplDlluCal.c
new file mode 100644
index 000000000000..7f4a17450237
--- /dev/null
+++ b/drivers/staging/epl/EplDlluCal.c
@@ -0,0 +1,529 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for DLL Communication Abstraction Layer module in EPL user part
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDlluCal.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.7 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/20 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "user/EplDlluCal.h"
+#include "user/EplEventu.h"
+
+#include "EplDllCal.h"
+
+// include only if direct call between user- and kernelspace is enabled
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+#include "kernel/EplDllkCal.h"
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplDlluCal                                          */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	tEplDlluCbAsnd m_apfnDlluCbAsnd[EPL_DLL_MAX_ASND_SERVICE_ID];
+
+} tEplDlluCalInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+// if no dynamic memory allocation shall be used
+// define structures statically
+static tEplDlluCalInstance EplDlluCalInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDlluCalSetAsndServiceIdFilter(tEplDllAsndServiceId
+						   ServiceId_p,
+						   tEplDllAsndFilter Filter_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalAddInstance()
+//
+// Description: add and initialize new instance of DLL CAL module
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalAddInstance()
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// reset instance structure
+	EPL_MEMSET(&EplDlluCalInstance_g, 0, sizeof(EplDlluCalInstance_g));
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalDelInstance()
+//
+// Description: deletes an instance of DLL CAL module
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalDelInstance()
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// reset instance structure
+	EPL_MEMSET(&EplDlluCalInstance_g, 0, sizeof(EplDlluCalInstance_g));
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalProcess
+//
+// Description: process the passed asynch frame
+//
+// Parameters:  pEvent_p                = event containing frame to be processed
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalProcess(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplMsgType MsgType;
+	unsigned int uiAsndServiceId;
+	tEplFrameInfo FrameInfo;
+
+	if (pEvent_p->m_EventType == kEplEventTypeAsndRx) {
+		FrameInfo.m_pFrame = (tEplFrame *) pEvent_p->m_pArg;
+		FrameInfo.m_uiFrameSize = pEvent_p->m_uiSize;
+		// extract NetTime
+		FrameInfo.m_NetTime = pEvent_p->m_NetTime;
+
+		MsgType =
+		    (tEplMsgType) AmiGetByteFromLe(&FrameInfo.m_pFrame->
+						   m_le_bMessageType);
+		if (MsgType != kEplMsgTypeAsnd) {
+			Ret = kEplInvalidOperation;
+			goto Exit;
+		}
+
+		uiAsndServiceId =
+		    (unsigned int)AmiGetByteFromLe(&FrameInfo.m_pFrame->m_Data.
+						   m_Asnd.m_le_bServiceId);
+		if (uiAsndServiceId < EPL_DLL_MAX_ASND_SERVICE_ID) {	// ASnd service ID is valid
+			if (EplDlluCalInstance_g.m_apfnDlluCbAsnd[uiAsndServiceId] != NULL) {	// handler was registered
+				Ret =
+				    EplDlluCalInstance_g.
+				    m_apfnDlluCbAsnd[uiAsndServiceId]
+				    (&FrameInfo);
+			}
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalRegAsndService()
+//
+// Description: registers the specified handler for the specified
+//              AsndServiceId with the specified node ID filter.
+//
+// Parameters:  ServiceId_p             = ASnd Service ID
+//              pfnDlluCbAsnd_p         = callback function
+//              Filter_p                = node ID filter
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalRegAsndService(tEplDllAsndServiceId ServiceId_p,
+				    tEplDlluCbAsnd pfnDlluCbAsnd_p,
+				    tEplDllAsndFilter Filter_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (ServiceId_p < tabentries(EplDlluCalInstance_g.m_apfnDlluCbAsnd)) {
+		// memorize function pointer
+		EplDlluCalInstance_g.m_apfnDlluCbAsnd[ServiceId_p] =
+		    pfnDlluCbAsnd_p;
+
+		if (pfnDlluCbAsnd_p == NULL) {	// close filter
+			Filter_p = kEplDllAsndFilterNone;
+		}
+		// set filter in DLL module in kernel part
+		Ret = EplDlluCalSetAsndServiceIdFilter(ServiceId_p, Filter_p);
+
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalAsyncSend()
+//
+// Description: sends the frame with the specified priority.
+//
+// Parameters:  pFrameInfo_p            = frame
+//                                        m_uiFrameSize does not include the
+//                                        ethernet header (14 bytes)
+//              Priority_p              = priority
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalAsyncSend(tEplFrameInfo * pFrameInfo_p,
+			       tEplDllAsyncReqPriority Priority_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+	pFrameInfo_p->m_uiFrameSize += 14;	// add size of ethernet header
+	Ret = EplDllkCalAsyncSend(pFrameInfo_p, Priority_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+}
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalIssueRequest()
+//
+// Description: issues a StatusRequest or a IdentRequest to the specified node.
+//
+// Parameters:  Service_p               = request service ID
+//              uiNodeId_p              = node ID
+//              bSoaFlag1_p             = flag1 for this node (transmit in SoA and PReq)
+//                                        If 0xFF this flag is ignored.
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalIssueRequest(tEplDllReqServiceId Service_p,
+				  unsigned int uiNodeId_p, BYTE bSoaFlag1_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// add node to appropriate request queue
+	switch (Service_p) {
+	case kEplDllReqServiceIdent:
+	case kEplDllReqServiceStatus:
+		{
+			tEplEvent Event;
+			tEplDllCalIssueRequest IssueReq;
+
+			Event.m_EventSink = kEplEventSinkDllkCal;
+			Event.m_EventType = kEplEventTypeDllkIssueReq;
+			IssueReq.m_Service = Service_p;
+			IssueReq.m_uiNodeId = uiNodeId_p;
+			IssueReq.m_bSoaFlag1 = bSoaFlag1_p;
+			Event.m_pArg = &IssueReq;
+			Event.m_uiSize = sizeof(IssueReq);
+
+			Ret = EplEventuPost(&Event);
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplDllInvalidParam;
+			goto Exit;
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalAddNode()
+//
+// Description: adds the specified node to the isochronous phase.
+//
+// Parameters:  pNodeInfo_p             = pointer of node info structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalAddNode(tEplDllNodeInfo * pNodeInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+
+	Event.m_EventSink = kEplEventSinkDllkCal;
+	Event.m_EventType = kEplEventTypeDllkAddNode;
+	Event.m_pArg = pNodeInfo_p;
+	Event.m_uiSize = sizeof(tEplDllNodeInfo);
+
+	Ret = EplEventuPost(&Event);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalDeleteNode()
+//
+// Description: removes the specified node from the isochronous phase.
+//
+// Parameters:  uiNodeId_p              = node ID
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalDeleteNode(unsigned int uiNodeId_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+
+	Event.m_EventSink = kEplEventSinkDllkCal;
+	Event.m_EventType = kEplEventTypeDllkDelNode;
+	Event.m_pArg = &uiNodeId_p;
+	Event.m_uiSize = sizeof(uiNodeId_p);
+
+	Ret = EplEventuPost(&Event);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalSoftDeleteNode()
+//
+// Description: removes the specified node softly from the isochronous phase.
+//
+// Parameters:  uiNodeId_p              = node ID
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalSoftDeleteNode(unsigned int uiNodeId_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+
+	Event.m_EventSink = kEplEventSinkDllkCal;
+	Event.m_EventType = kEplEventTypeDllkSoftDelNode;
+	Event.m_pArg = &uiNodeId_p;
+	Event.m_uiSize = sizeof(uiNodeId_p);
+
+	Ret = EplEventuPost(&Event);
+
+	return Ret;
+}
+
+#endif // (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalSetAsndServiceIdFilter()
+//
+// Description: forwards call to EplDllkSetAsndServiceIdFilter() in kernel part
+//
+// Parameters:  ServiceId_p             = ASnd Service ID
+//              Filter_p                = node ID filter
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDlluCalSetAsndServiceIdFilter(tEplDllAsndServiceId
+						   ServiceId_p,
+						   tEplDllAsndFilter Filter_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+	tEplDllCalAsndServiceIdFilter ServFilter;
+
+	Event.m_EventSink = kEplEventSinkDllkCal;
+	Event.m_EventType = kEplEventTypeDllkServFilter;
+	ServFilter.m_ServiceId = ServiceId_p;
+	ServFilter.m_Filter = Filter_p;
+	Event.m_pArg = &ServFilter;
+	Event.m_uiSize = sizeof(ServFilter);
+
+	Ret = EplEventuPost(&Event);
+
+	return Ret;
+}
+
+#endif // (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplErrDef.h b/drivers/staging/epl/EplErrDef.h
new file mode 100644
index 000000000000..2aee12cfd28c
--- /dev/null
+++ b/drivers/staging/epl/EplErrDef.h
@@ -0,0 +1,294 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  definitions for all EPL-function return codes
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplErrDef.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.9 $  $Date: 2008/06/23 14:56:33 $
+
+                $State: Exp $
+
+                Build Environment:
+                    all
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2005/12/05 -as:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_ERRORDEF_H_
+#define _EPL_ERRORDEF_H_
+
+//---------------------------------------------------------------------------
+// return codes
+//---------------------------------------------------------------------------
+
+typedef enum {
+	// area for generic errors 0x0000 - 0x000F
+	kEplSuccessful = 0x0000,	// no error/successful run
+	kEplIllegalInstance = 0x0001,	// the called Instanz does not exist
+	kEplInvalidInstanceParam = 0x0002,	//
+	kEplNoFreeInstance = 0x0003,	// XxxAddInstance was called but no free instance is available
+	kEplWrongSignature = 0x0004,	// wrong signature while writing to object 0x1010 or 0x1011
+	kEplInvalidOperation = 0x0005,	// operation not allowed in this situation
+	kEplInvalidNodeId = 0x0007,	// invalid NodeId was specified
+	kEplNoResource = 0x0008,	// resource could not be created (Windows, PxROS, ...)
+	kEplShutdown = 0x0009,	// stack is shutting down
+	kEplReject = 0x000A,	// reject the subsequent command
+
+	// area for EDRV module 0x0010 - 0x001F
+//    kEplEdrvNoFrame             = 0x0010,       // no CAN message was received
+//    kEplEdrvMsgHigh             = 0x0011,       // CAN message with high priority was received
+//    kEplEdrvMsgLow              = 0x0012,       // CAN message with low priority was received
+	kEplEdrvInitError = 0x0013,	// initialisation error
+	kEplEdrvNoFreeBufEntry = 0x0014,	// no free entry in internal buffer table for Tx frames
+	kEplEdrvBufNotExisting = 0x0015,	// specified Tx buffer does not exist
+//    kEplEdrvNoFreeChannel       = 0x0014,       // CAN controller has not a free channel
+//    kEplEdrvTxBuffHighOverrun   = 0x0015,       // buffer for high priority CAN transmit messages has overrun
+//    kEplEdrvTxBuffLowOverrun    = 0x0016,       // buffer for low priority CAN transmit messages has overrun
+//    kEplEdrvIllegalBdi          = 0x0017,       // unsupported baudrate within baudrate table
+//    kEplEdrvBusy                = 0x0018,       // remote frame can not be updated because no bus contact or CAN
+	// transmission is activ
+//    kEplEdrvInvalidDriverType   = 0x0019,       // (PC: Windows or Linux) invalid driver type
+//    kEplEdrvDriverNotFound      = 0x001A,       // (PC: Windows or Linux) driver (DLL) could not be found
+//    kEplEdrvInvalidBaseAddress  = 0x001B,       // (PC: Windows or Linux) driver could not found the CAN controller
+//    kEplEdrvInvalidParam        = 0x001C,       // invalid param in function call
+
+	// area for COB module 0x0020 - 0x002F
+/*    kEplCobNoFreeEntry          = 0x0020,       // no free entry in RX- or TX-COB table
+    kEplCobAlreadyExist         = 0x0021,       // COB-ID already exists in RX- resp. TX-COB table
+    */
+	kEplDllIllegalHdl = 0x0022,	// illegal handle for a TxFrame was passed
+	kEplDllCbAsyncRegistered = 0x0023,	// handler for non-EPL frames was already registered before
+//    kEplDllAsyncRxBufferFull    = 0x0024,       // receive buffer for asynchronous frames is full
+	kEplDllAsyncTxBufferEmpty = 0x0025,	// transmit buffer for asynchronous frames is empty
+	kEplDllAsyncTxBufferFull = 0x0026,	// transmit buffer for asynchronous frames is full
+	kEplDllNoNodeInfo = 0x0027,	// MN: too less space in the internal node info structure
+	kEplDllInvalidParam = 0x0028,	// invalid parameters passed to function
+	kEplDllTxBufNotReady = 0x002E,	// TxBuffer (e.g. for PReq) is not ready yet
+	kEplDllTxFrameInvalid = 0x002F,	// TxFrame (e.g. for PReq) is invalid or does not exist
+/*    kEplCobIllegalCanId         = 0x0023,       // COB-ID is not allowed (like 0x000 is reserved for NMT, ...)
+    kEplCobInvalidCanId         = 0x0024,       // COB-ID is switched off
+    kEplCobCdrvStateSet         = 0x0025,       // at least one bit of CAN driver state is set
+    kEplCobNoFreeEntryHighBuf   = 0x0026,       // no free entry in high priotity RX- or TX-COB table
+    kEplCobOwnId                = 0x0027,       // COB-ID already exists in own module which calls CobDefine() or CobCheck()
+*/
+	// area for OBD module 0x0030 - 0x003F
+	kEplObdIllegalPart = 0x0030,	// unknown OD part
+	kEplObdIndexNotExist = 0x0031,	// object index does not exist in OD
+	kEplObdSubindexNotExist = 0x0032,	// subindex does not exist in object index
+	kEplObdReadViolation = 0x0033,	// read access to a write-only object
+	kEplObdWriteViolation = 0x0034,	// write access to a read-only object
+	kEplObdAccessViolation = 0x0035,	// access not allowed
+	kEplObdUnknownObjectType = 0x0036,	// object type not defined/known
+	kEplObdVarEntryNotExist = 0x0037,	// object does not contain VarEntry structure
+	kEplObdValueTooLow = 0x0038,	// value to write to an object is too low
+	kEplObdValueTooHigh = 0x0039,	// value to write to an object is too high
+	kEplObdValueLengthError = 0x003A,	// value to write is to long or to short
+//    kEplObdIllegalFloat         = 0x003B,       // illegal float variable
+//    kEplObdWrongOdBuilderKey    = 0x003F,       // OD was generated with demo version of tool ODBuilder
+
+	// area for NMT module 0x0040 - 0x004F
+	kEplNmtUnknownCommand = 0x0040,	// unknown NMT command
+	kEplNmtInvalidFramePointer = 0x0041,	// pointer to the frame is not valid
+	kEplNmtInvalidEvent = 0x0042,	// invalid event send to NMT-modul
+	kEplNmtInvalidState = 0x0043,	// unknown state in NMT-State-Maschine
+	kEplNmtInvalidParam = 0x0044,	// invalid parameters specified
+
+	// area for SDO/UDP module 0x0050 - 0x005F
+	kEplSdoUdpMissCb = 0x0050,	// missing callback-function pointer during inti of
+	// module
+	kEplSdoUdpNoSocket = 0x0051,	// error during init of socket
+	kEplSdoUdpSocketError = 0x0052,	// error during usage of socket
+	kEplSdoUdpThreadError = 0x0053,	// error during start of listen thread
+	kEplSdoUdpNoFreeHandle = 0x0054,	// no free connection handle for Udp
+	kEplSdoUdpSendError = 0x0055,	// Error during send of frame
+	kEplSdoUdpInvalidHdl = 0x0056,	// the connection handle is invalid
+
+	// area for SDO Sequence layer module 0x0060 - 0x006F
+	kEplSdoSeqMissCb = 0x0060,	// no callback-function assign
+	kEplSdoSeqNoFreeHandle = 0x0061,	// no free handle for connection
+	kEplSdoSeqInvalidHdl = 0x0062,	// invalid handle in SDO sequence layer
+	kEplSdoSeqUnsupportedProt = 0x0063,	// unsupported Protocol selected
+	kEplSdoSeqNoFreeHistory = 0x0064,	// no free entry in history
+	kEplSdoSeqFrameSizeError = 0x0065,	// the size of the frames is not correct
+	kEplSdoSeqRequestAckNeeded = 0x0066,	// indeicates that the history buffer is full
+	// and a ack request is needed
+	kEplSdoSeqInvalidFrame = 0x0067,	// frame not valid
+	kEplSdoSeqConnectionBusy = 0x0068,	// connection is busy -> retry later
+	kEplSdoSeqInvalidEvent = 0x0069,	// invalid event received
+
+	// area for SDO Command Layer Module 0x0070 - 0x007F
+	kEplSdoComUnsupportedProt = 0x0070,	// unsupported Protocol selected
+	kEplSdoComNoFreeHandle = 0x0071,	// no free handle for connection
+	kEplSdoComInvalidServiceType = 0x0072,	// invalid SDO service type specified
+	kEplSdoComInvalidHandle = 0x0073,	// handle invalid
+	kEplSdoComInvalidSendType = 0x0074,	// the stated to of frame to send is
+	// not possible
+	kEplSdoComNotResponsible = 0x0075,	// internal error: command layer handle is
+	// not responsible for this event from sequence layer
+	kEplSdoComHandleExists = 0x0076,	// handle to same node already exists
+	kEplSdoComHandleBusy = 0x0077,	// transfer via this handle is already running
+	kEplSdoComInvalidParam = 0x0078,	// invalid parameters passed to function
+
+	// area for EPL Event-Modul 0x0080 - 0x008F
+	kEplEventUnknownSink = 0x0080,	// unknown sink for event
+	kEplEventPostError = 0x0081,	// error during post of event
+
+	// area for EPL Timer Modul 0x0090 - 0x009F
+	kEplTimerInvalidHandle = 0x0090,	// invalid handle for timer
+	kEplTimerNoTimerCreated = 0x0091,	// no timer was created caused by
+	// an error
+
+	// area for EPL SDO/Asnd Module 0x00A0 - 0x0AF
+	kEplSdoAsndInvalidNodeId = 0x00A0,	//0 node id is invalid
+	kEplSdoAsndNoFreeHandle = 0x00A1,	// no free handle for connection
+	kEplSdoAsndInvalidHandle = 0x00A2,	// handle for connection is invalid
+
+	// area for PDO module 0x00B0 - 0x00BF
+	kEplPdoNotExist = 0x00B0,	// selected PDO does not exist
+	kEplPdoLengthExceeded = 0x00B1,	// length of PDO mapping exceedes 64 bis
+	kEplPdoGranularityMismatch = 0x00B2,	// configured PDO granularity is not equal to supported granularity
+	kEplPdoInitError = 0x00B3,	// error during initialisation of PDO module
+	kEplPdoErrorPdoEncode = 0x00B4,	// error during encoding a PDO
+	kEplPdoErrorPdoDecode = 0x00B5,	// error during decoding a PDO
+	kEplPdoErrorSend = 0x00B6,	// error during sending a PDO
+	kEplPdoErrorSyncWin = 0x00B7,	// the SYNC window runs out during sending SYNC-PDOs
+	kEplPdoErrorMapp = 0x00B8,	// invalid PDO mapping
+	kEplPdoVarNotFound = 0x00B9,	// variable was not found in function PdoSignalVar()
+	kEplPdoErrorEmcyPdoLen = 0x00BA,	// the length of a received PDO is unequal to the expected value
+	kEplPdoWriteConstObject = 0x00BB,	// constant object can not be written
+	// (only TxType, Inhibit-, Event Time for CANopen Kit)
+
+	// area for LSS slave module
+/*    kEplLsssResetNode           = 0x0080,       // NMT command "reset node" has to be processed after LSS configuration
+                                                // new of NodeId
+    kEplLsssInvalidNodeId       = 0x0081,       // no valid NodeId is configured -> wait until it is configured with
+                                                // LSS service before calling CcmConnectToNet()
+*/
+	// area for emergency consumer module 0x0090 - 0x009F
+/*    kEplEmccNoFreeProducerEntry = 0x0090,       // no free entry to add a Emergency Producer
+    kEplEmccNodeIdNotExist      = 0x0091,       // selected NodeId was never added
+    kEplEmccNodeIdInvalid       = 0x0092,       // selected NodeId is outside of range (0x01 until 0x7F)
+    kEplEmccNodeIdExist         = 0x0093,       // selected NodeId already exist
+*/
+	// area for dynamic OD 0x00A0 - 0x00AF
+/*    kEplDynNoMemory             = 0x00A0,       // no memory available
+    kEplDynInvalidConfig        = 0x00A1,       // invalid configuration in segment container
+*/
+	// area for hertbeat consumer module 0x00B0 - 0x00BF
+/*    kEplHbcEntryNotExist        = 0x00B0,       // Heartbeat Producer node not configured
+    kEplHbcEntryAlreadyExist    = 0x00B1,       // NodeId was already defined in heartbeat consumer table (object 0x1016)
+*/
+	// Configuration manager module 0x00C0 - 0x00CF
+	kEplCfgMaConfigError = 0x00C0,	// error in configuration manager
+	kEplCfgMaSdocTimeOutError = 0x00C1,	// error in configuration manager, Sdo timeout
+	kEplCfgMaInvalidDcf = 0x00C2,	// configration file not valid
+	kEplCfgMaUnsupportedDcf = 0x00C3,	// unsupported Dcf format
+	kEplCfgMaConfigWithErrors = 0x00C4,	// configuration finished with errors
+	kEplCfgMaNoFreeConfig = 0x00C5,	// no free configuration entry
+	kEplCfgMaNoConfigData = 0x00C6,	// no configuration data present
+	kEplCfgMaUnsuppDatatypeDcf = 0x00C7,	// unsupported datatype found in dcf
+	// -> this entry was not configured
+
+	// area for LSS master module 0x00D0 - 0x00DF
+/*    kEplLssmIllegalMode         = 0x00D0,       // illegal LSS mode (operation / configuration)
+    kEplLssmIllegalState        = 0x00D1,       // function was called in illegal state of LSS master
+    kEplLssmBusy                = 0x00D2,       // LSS process is busy with an previous service
+    kEplLssmIllegalCmd          = 0x00D3,       // illegal command code was set for function LssmInquireIdentity()
+    kEplLssmTimeout             = 0x00D4,       // LSS slave did not answer a LSS service
+    kEplLssmErrorInConfirm      = 0x00D5,       // LSS slave replied an error code for a LSS service
+*/
+	// area for CCM modules 0x00E0 - 0xEF
+/*    kEplCcmStoreUnvalidState    = 0x00E0,       // memory device not available due device state
+    kEplCcmStoreHwError         = 0x00E1,       // hw error due device access
+*/
+	// area for SRDO module 0x0100 - 0x011F
+/*    kEplSrdoNotExist            = 0x0100,       // selected SRDO does not exist
+    kEplSrdoGranularityMismatch = 0x0101,       // configured SRDO granularity is not equal to supported granularity
+    kEplSrdoCfgTimingError      = 0x0102,       // configuration is not ok (Timing)
+    kEplSrdoCfgIdError          = 0x0103,       // configuration is not ok (CobIds)
+    kEplSrdoCfgCrcError         = 0x0104,       // configuration is not ok (CRC)
+    kEplSrdoNmtError            = 0x0105,       // an action was tried in a wrong NMT state
+    kEplSrdoInvalidCfg          = 0x0106,       // an action was tried with an invald SRDO configuration
+    kEplSrdoInvalid             = 0x0107,       // an action was tried with an invald SRDO
+    kEplSrdoRxTxConflict        = 0x0108,       // an transmission was tried with an receive SRDO (or the other way)
+    kEplSrdoIllegalCanId        = 0x0109,       // the CanId is invalid
+    kEplSrdoCanIdAlreadyInUse   = 0x010A,       // the CanId is already in use
+    kEplSrdoNotInOrder          = 0x010B,       // the two messages of a SRDO are not in order
+    kEplSrdoSctTimeout          = 0x010C,       // timeout of SCT
+    kEplSrdoSrvtTimeout         = 0x010D,       // timeout of SRVT
+    kEplSrdoCanIdNotValid       = 0x010E,       // one of received CAN-IDs are not equal to configured one
+    kEplSrdoDlcNotValid         = 0x010F,       // one of received CAN-DLC are not equal to configured one
+    kEplSrdoErrorMapp           = 0x0110,       // wrong values in mapping found
+    kEplSrdoDataError           = 0x0111,       // data of CAN messages are not invers
+    kEplSrdoLengthExceeded      = 0x0112,       // length of SRDO mapping exceedes 64 bit per CAN-message
+    kEplSrdoNotHandledInApp     = 0x0113,       // the SRDO error was not handled in AppSrdoError()
+    kEplSrdoOverrun             = 0x0114        // a RxSRDO was received but the pevious one was not else processed
+*/
+
+	kEplApiTaskDeferred = 0x0140,	// EPL performs task in background and informs the application (or vice-versa), when it is finished
+	kEplApiInvalidParam = 0x0142,	// passed invalid parameters to a function (e.g. invalid node id)
+
+	// area untill 0x07FF is reserved
+	// area for user application from 0x0800 to 0x7FFF
+
+} tEplKernel;
+
+#endif
+//EOF
+
+// Die letzte Zeile muß unbedingt eine leere Zeile sein, weil manche Compiler
+// damit ein Problem haben, wenn das nicht so ist (z.B. GNU oder Borland C++ Builder).
diff --git a/drivers/staging/epl/EplErrorHandlerk.c b/drivers/staging/epl/EplErrorHandlerk.c
new file mode 100644
index 000000000000..d12521fc99a4
--- /dev/null
+++ b/drivers/staging/epl/EplErrorHandlerk.c
@@ -0,0 +1,810 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for error handler module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplErrorHandlerk.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.9 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/10/02 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "kernel/EplErrorHandlerk.h"
+#include "EplNmt.h"
+#include "kernel/EplEventk.h"
+#include "kernel/EplObdk.h"	// function prototyps of the EplOBD-Modul
+#include "kernel/EplDllk.h"
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) == 0)
+#error "EPL ErrorHandler module needs EPL module OBDK!"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	DWORD m_dwCumulativeCnt;	// subindex 1
+	DWORD m_dwThresholdCnt;	// subindex 2
+	DWORD m_dwThreshold;	// subindex 3
+
+} tEplErrorHandlerkErrorCounter;
+
+typedef struct {
+	tEplErrorHandlerkErrorCounter m_CnLossSoc;	// object 0x1C0B
+	tEplErrorHandlerkErrorCounter m_CnLossPreq;	// object 0x1C0D
+	tEplErrorHandlerkErrorCounter m_CnCrcErr;	// object 0x1C0F
+	unsigned long m_ulDllErrorEvents;
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	tEplErrorHandlerkErrorCounter m_MnCrcErr;	// object 0x1C00
+	tEplErrorHandlerkErrorCounter m_MnCycTimeExceed;	// object 0x1C02
+	DWORD m_adwMnCnLossPresCumCnt[254];	// object 0x1C07
+	DWORD m_adwMnCnLossPresThrCnt[254];	// object 0x1C08
+	DWORD m_adwMnCnLossPresThreshold[254];	// object 0x1C09
+	BOOL m_afMnCnLossPresEvent[254];
+#endif
+
+} tEplErrorHandlerkInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+static tEplErrorHandlerkInstance EplErrorHandlerkInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplKernel EplErrorHandlerkLinkErrorCounter(tEplErrorHandlerkErrorCounter
+						   * pErrorCounter_p,
+						   unsigned int uiIndex_p);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+static tEplKernel EplErrorHandlerkLinkArray(DWORD * pdwValue_p,
+					    unsigned int uiValueCount_p,
+					    unsigned int uiIndex_p);
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <Epl-Kernelspace-Error-Handler>                     */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplErrorHandlerkInit
+//
+// Description: function initialize the first instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplErrorHandlerkInit(void)
+{
+	tEplKernel Ret;
+
+	Ret = EplErrorHandlerkAddInstance();
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplErrorHandlerkAddInstance
+//
+// Description: function add one more instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplErrorHandlerkAddInstance(void)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// reset only event variable,
+	// all other instance members are reset by OD or may keep their current value
+	// d.k.: this is necessary for the cumulative counters, which shall not be reset
+	EplErrorHandlerkInstance_g.m_ulDllErrorEvents = 0;
+
+	// link counters to OD
+	// $$$ d.k. if OD resides in userspace, fetch pointer to shared memory,
+	//          which shall have the same structure as the instance (needs to be declared globally).
+	//          Other idea: error counter shall belong to the process image
+	//          (reset of counters by SDO write are a little bit tricky).
+
+	Ret =
+	    EplErrorHandlerkLinkErrorCounter(&EplErrorHandlerkInstance_g.
+					     m_CnLossSoc, 0x1C0B);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret =
+	    EplErrorHandlerkLinkErrorCounter(&EplErrorHandlerkInstance_g.
+					     m_CnLossPreq, 0x1C0D);
+	// ignore return code, because object 0x1C0D is conditional
+
+	Ret =
+	    EplErrorHandlerkLinkErrorCounter(&EplErrorHandlerkInstance_g.
+					     m_CnCrcErr, 0x1C0F);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	Ret =
+	    EplErrorHandlerkLinkErrorCounter(&EplErrorHandlerkInstance_g.
+					     m_MnCrcErr, 0x1C00);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret =
+	    EplErrorHandlerkLinkErrorCounter(&EplErrorHandlerkInstance_g.
+					     m_MnCycTimeExceed, 0x1C02);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret =
+	    EplErrorHandlerkLinkArray(EplErrorHandlerkInstance_g.
+				      m_adwMnCnLossPresCumCnt,
+				      tabentries(EplErrorHandlerkInstance_g.
+						 m_adwMnCnLossPresCumCnt),
+				      0x1C07);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret =
+	    EplErrorHandlerkLinkArray(EplErrorHandlerkInstance_g.
+				      m_adwMnCnLossPresThrCnt,
+				      tabentries(EplErrorHandlerkInstance_g.
+						 m_adwMnCnLossPresThrCnt),
+				      0x1C08);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret =
+	    EplErrorHandlerkLinkArray(EplErrorHandlerkInstance_g.
+				      m_adwMnCnLossPresThreshold,
+				      tabentries(EplErrorHandlerkInstance_g.
+						 m_adwMnCnLossPresThreshold),
+				      0x1C09);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplErrorHandlerkDelInstance
+//
+// Description: function delete instance an free the bufferstructure
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplErrorHandlerkDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplErrorHandlerkProcess
+//
+// Description: processes error events from DLL
+//
+//
+//
+// Parameters:  pEvent_p = pointer to event-structur from buffer
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplErrorHandlerkProcess(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+	unsigned long ulDllErrorEvents;
+	tEplEvent Event;
+	tEplNmtEvent NmtEvent;
+
+	Ret = kEplSuccessful;
+
+	// check m_EventType
+	switch (pEvent_p->m_EventType) {
+	case kEplEventTypeDllError:
+		{
+			tEplErrorHandlerkEvent *pErrHandlerEvent =
+			    (tEplErrorHandlerkEvent *) pEvent_p->m_pArg;
+
+			ulDllErrorEvents = pErrHandlerEvent->m_ulDllErrorEvents;
+
+			// check the several error events
+			if ((EplErrorHandlerkInstance_g.m_CnLossSoc.
+			     m_dwThreshold > 0)
+			    && ((ulDllErrorEvents & EPL_DLL_ERR_CN_LOSS_SOC) != 0)) {	// loss of SoC event occured
+				// increment cumulative counter by 1
+				EplErrorHandlerkInstance_g.m_CnLossSoc.
+				    m_dwCumulativeCnt++;
+				// increment threshold counter by 8
+				EplErrorHandlerkInstance_g.m_CnLossSoc.
+				    m_dwThresholdCnt += 8;
+				if (EplErrorHandlerkInstance_g.m_CnLossSoc.m_dwThresholdCnt >= EplErrorHandlerkInstance_g.m_CnLossSoc.m_dwThreshold) {	// threshold is reached
+					// $$$ d.k.: generate error history entry E_DLL_LOSS_SOC_TH
+
+					// post event to NMT state machine
+					NmtEvent = kEplNmtEventNmtCycleError;
+					Event.m_EventSink = kEplEventSinkNmtk;
+					Event.m_EventType =
+					    kEplEventTypeNmtEvent;
+					Event.m_pArg = &NmtEvent;
+					Event.m_uiSize = sizeof(NmtEvent);
+					Ret = EplEventkPost(&Event);
+				}
+				EplErrorHandlerkInstance_g.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOC;
+			}
+
+			if ((EplErrorHandlerkInstance_g.m_CnLossPreq.
+			     m_dwThreshold > 0)
+			    && ((ulDllErrorEvents & EPL_DLL_ERR_CN_LOSS_PREQ) != 0)) {	// loss of PReq event occured
+				// increment cumulative counter by 1
+				EplErrorHandlerkInstance_g.m_CnLossPreq.
+				    m_dwCumulativeCnt++;
+				// increment threshold counter by 8
+				EplErrorHandlerkInstance_g.m_CnLossPreq.
+				    m_dwThresholdCnt += 8;
+				if (EplErrorHandlerkInstance_g.m_CnLossPreq.m_dwThresholdCnt >= EplErrorHandlerkInstance_g.m_CnLossPreq.m_dwThreshold) {	// threshold is reached
+					// $$$ d.k.: generate error history entry E_DLL_LOSS_PREQ_TH
+
+					// post event to NMT state machine
+					NmtEvent = kEplNmtEventNmtCycleError;
+					Event.m_EventSink = kEplEventSinkNmtk;
+					Event.m_EventType =
+					    kEplEventTypeNmtEvent;
+					Event.m_pArg = &NmtEvent;
+					Event.m_uiSize = sizeof(NmtEvent);
+					Ret = EplEventkPost(&Event);
+				}
+			}
+
+			if ((EplErrorHandlerkInstance_g.m_CnLossPreq.
+			     m_dwThresholdCnt > 0)
+			    && ((ulDllErrorEvents & EPL_DLL_ERR_CN_RECVD_PREQ) != 0)) {	// PReq correctly received
+				// decrement threshold counter by 1
+				EplErrorHandlerkInstance_g.m_CnLossPreq.
+				    m_dwThresholdCnt--;
+			}
+
+			if ((EplErrorHandlerkInstance_g.m_CnCrcErr.
+			     m_dwThreshold > 0)
+			    && ((ulDllErrorEvents & EPL_DLL_ERR_CN_CRC) != 0)) {	// CRC error event occured
+				// increment cumulative counter by 1
+				EplErrorHandlerkInstance_g.m_CnCrcErr.
+				    m_dwCumulativeCnt++;
+				// increment threshold counter by 8
+				EplErrorHandlerkInstance_g.m_CnCrcErr.
+				    m_dwThresholdCnt += 8;
+				if (EplErrorHandlerkInstance_g.m_CnCrcErr.m_dwThresholdCnt >= EplErrorHandlerkInstance_g.m_CnCrcErr.m_dwThreshold) {	// threshold is reached
+					// $$$ d.k.: generate error history entry E_DLL_CRC_TH
+
+					// post event to NMT state machine
+					NmtEvent = kEplNmtEventNmtCycleError;
+					Event.m_EventSink = kEplEventSinkNmtk;
+					Event.m_EventType =
+					    kEplEventTypeNmtEvent;
+					Event.m_pArg = &NmtEvent;
+					Event.m_uiSize = sizeof(NmtEvent);
+					Ret = EplEventkPost(&Event);
+				}
+				EplErrorHandlerkInstance_g.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_CRC;
+			}
+
+			if ((ulDllErrorEvents & EPL_DLL_ERR_INVALID_FORMAT) != 0) {	// invalid format error occured (only direct reaction)
+				// $$$ d.k.: generate error history entry E_DLL_INVALID_FORMAT
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+				if (pErrHandlerEvent->m_NmtState >= kEplNmtMsNotActive) {	// MN is active
+					if (pErrHandlerEvent->m_uiNodeId != 0) {
+						tEplHeartbeatEvent
+						    HeartbeatEvent;
+
+						// remove node from isochronous phase
+						Ret =
+						    EplDllkDeleteNode
+						    (pErrHandlerEvent->
+						     m_uiNodeId);
+
+						// inform NmtMnu module about state change, which shall send NMT command ResetNode to this CN
+						HeartbeatEvent.m_uiNodeId =
+						    pErrHandlerEvent->
+						    m_uiNodeId;
+						HeartbeatEvent.m_NmtState =
+						    kEplNmtCsNotActive;
+						HeartbeatEvent.m_wErrorCode =
+						    EPL_E_DLL_INVALID_FORMAT;
+						Event.m_EventSink =
+						    kEplEventSinkNmtMnu;
+						Event.m_EventType =
+						    kEplEventTypeHeartbeat;
+						Event.m_uiSize =
+						    sizeof(HeartbeatEvent);
+						Event.m_pArg = &HeartbeatEvent;
+						Ret = EplEventkPost(&Event);
+					}
+					// $$$ and else should lead to InternComError
+				} else
+#endif
+				{	// CN is active
+					// post event to NMT state machine
+					NmtEvent = kEplNmtEventInternComError;
+					Event.m_EventSink = kEplEventSinkNmtk;
+					Event.m_EventType =
+					    kEplEventTypeNmtEvent;
+					Event.m_pArg = &NmtEvent;
+					Event.m_uiSize = sizeof(NmtEvent);
+					Ret = EplEventkPost(&Event);
+				}
+			}
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			if ((EplErrorHandlerkInstance_g.m_MnCrcErr.
+			     m_dwThreshold > 0)
+			    && ((ulDllErrorEvents & EPL_DLL_ERR_MN_CRC) != 0)) {	// CRC error event occured
+				// increment cumulative counter by 1
+				EplErrorHandlerkInstance_g.m_MnCrcErr.
+				    m_dwCumulativeCnt++;
+				// increment threshold counter by 8
+				EplErrorHandlerkInstance_g.m_MnCrcErr.
+				    m_dwThresholdCnt += 8;
+				if (EplErrorHandlerkInstance_g.m_MnCrcErr.m_dwThresholdCnt >= EplErrorHandlerkInstance_g.m_MnCrcErr.m_dwThreshold) {	// threshold is reached
+					// $$$ d.k.: generate error history entry E_DLL_CRC_TH
+
+					// post event to NMT state machine
+					NmtEvent = kEplNmtEventNmtCycleError;
+					Event.m_EventSink = kEplEventSinkNmtk;
+					Event.m_EventType =
+					    kEplEventTypeNmtEvent;
+					Event.m_pArg = &NmtEvent;
+					Event.m_uiSize = sizeof(NmtEvent);
+					Ret = EplEventkPost(&Event);
+				}
+				EplErrorHandlerkInstance_g.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_MN_CRC;
+			}
+
+			if ((EplErrorHandlerkInstance_g.m_MnCycTimeExceed.
+			     m_dwThreshold > 0)
+			    && ((ulDllErrorEvents & EPL_DLL_ERR_MN_CYCTIMEEXCEED) != 0)) {	// cycle time exceeded event occured
+				// increment cumulative counter by 1
+				EplErrorHandlerkInstance_g.m_MnCycTimeExceed.
+				    m_dwCumulativeCnt++;
+				// increment threshold counter by 8
+				EplErrorHandlerkInstance_g.m_MnCycTimeExceed.
+				    m_dwThresholdCnt += 8;
+				if (EplErrorHandlerkInstance_g.m_MnCycTimeExceed.m_dwThresholdCnt >= EplErrorHandlerkInstance_g.m_MnCycTimeExceed.m_dwThreshold) {	// threshold is reached
+					// $$$ d.k.: generate error history entry E_DLL_CYCLE_EXCEED_TH
+
+					// post event to NMT state machine
+					NmtEvent = kEplNmtEventNmtCycleError;
+					Event.m_EventSink = kEplEventSinkNmtk;
+					Event.m_EventType =
+					    kEplEventTypeNmtEvent;
+					Event.m_pArg = &NmtEvent;
+					Event.m_uiSize = sizeof(NmtEvent);
+					Ret = EplEventkPost(&Event);
+				}
+				// $$$ d.k.: else generate error history entry E_DLL_CYCLE_EXCEED
+				EplErrorHandlerkInstance_g.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_MN_CYCTIMEEXCEED;
+			}
+
+			if ((ulDllErrorEvents & EPL_DLL_ERR_MN_CN_LOSS_PRES) != 0) {	// CN loss PRes event occured
+				unsigned int uiNodeId;
+
+				uiNodeId = pErrHandlerEvent->m_uiNodeId - 1;
+				if ((uiNodeId <
+				     tabentries(EplErrorHandlerkInstance_g.
+						m_adwMnCnLossPresCumCnt))
+				    && (EplErrorHandlerkInstance_g.
+					m_adwMnCnLossPresThreshold[uiNodeId] >
+					0)) {
+					// increment cumulative counter by 1
+					EplErrorHandlerkInstance_g.
+					    m_adwMnCnLossPresCumCnt[uiNodeId]++;
+					// increment threshold counter by 8
+					EplErrorHandlerkInstance_g.
+					    m_adwMnCnLossPresThrCnt[uiNodeId] +=
+					    8;
+					if (EplErrorHandlerkInstance_g.
+					    m_adwMnCnLossPresThrCnt[uiNodeId]
+					    >= EplErrorHandlerkInstance_g.m_adwMnCnLossPresThreshold[uiNodeId]) {	// threshold is reached
+						tEplHeartbeatEvent
+						    HeartbeatEvent;
+
+						// $$$ d.k.: generate error history entry E_DLL_LOSS_PRES_TH
+
+						// remove node from isochronous phase
+						Ret =
+						    EplDllkDeleteNode
+						    (pErrHandlerEvent->
+						     m_uiNodeId);
+
+						// inform NmtMnu module about state change, which shall send NMT command ResetNode to this CN
+						HeartbeatEvent.m_uiNodeId =
+						    pErrHandlerEvent->
+						    m_uiNodeId;
+						HeartbeatEvent.m_NmtState =
+						    kEplNmtCsNotActive;
+						HeartbeatEvent.m_wErrorCode =
+						    EPL_E_DLL_LOSS_PRES_TH;
+						Event.m_EventSink =
+						    kEplEventSinkNmtMnu;
+						Event.m_EventType =
+						    kEplEventTypeHeartbeat;
+						Event.m_uiSize =
+						    sizeof(HeartbeatEvent);
+						Event.m_pArg = &HeartbeatEvent;
+						Ret = EplEventkPost(&Event);
+					}
+					EplErrorHandlerkInstance_g.
+					    m_afMnCnLossPresEvent[uiNodeId] =
+					    TRUE;
+				}
+			}
+#endif
+
+			break;
+		}
+
+		// NMT event
+	case kEplEventTypeNmtEvent:
+		{
+			if ((*(tEplNmtEvent *) pEvent_p->m_pArg) == kEplNmtEventDllCeSoa) {	// SoA event of CN -> decrement threshold counters
+
+				if ((EplErrorHandlerkInstance_g.m_ulDllErrorEvents & EPL_DLL_ERR_CN_LOSS_SOC) == 0) {	// decrement loss of SoC threshold counter, because it didn't occur last cycle
+					if (EplErrorHandlerkInstance_g.
+					    m_CnLossSoc.m_dwThresholdCnt > 0) {
+						EplErrorHandlerkInstance_g.
+						    m_CnLossSoc.
+						    m_dwThresholdCnt--;
+					}
+				}
+
+				if ((EplErrorHandlerkInstance_g.m_ulDllErrorEvents & EPL_DLL_ERR_CN_CRC) == 0) {	// decrement CRC threshold counter, because it didn't occur last cycle
+					if (EplErrorHandlerkInstance_g.
+					    m_CnCrcErr.m_dwThresholdCnt > 0) {
+						EplErrorHandlerkInstance_g.
+						    m_CnCrcErr.
+						    m_dwThresholdCnt--;
+					}
+				}
+			}
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			else if ((*(tEplNmtEvent *) pEvent_p->m_pArg) == kEplNmtEventDllMeSoaSent) {	// SoA event of MN -> decrement threshold counters
+				tEplDllkNodeInfo *pIntNodeInfo;
+				unsigned int uiNodeId;
+
+				Ret = EplDllkGetFirstNodeInfo(&pIntNodeInfo);
+				if (Ret != kEplSuccessful) {
+					break;
+				}
+				// iterate through node info structure list
+				while (pIntNodeInfo != NULL) {
+					uiNodeId = pIntNodeInfo->m_uiNodeId - 1;
+					if (uiNodeId <
+					    tabentries
+					    (EplErrorHandlerkInstance_g.
+					     m_adwMnCnLossPresCumCnt)) {
+						if (EplErrorHandlerkInstance_g.
+						    m_afMnCnLossPresEvent
+						    [uiNodeId] == FALSE) {
+							if (EplErrorHandlerkInstance_g.m_adwMnCnLossPresThrCnt[uiNodeId] > 0) {
+								EplErrorHandlerkInstance_g.
+								    m_adwMnCnLossPresThrCnt
+								    [uiNodeId]--;
+							}
+						} else {
+							EplErrorHandlerkInstance_g.
+							    m_afMnCnLossPresEvent
+							    [uiNodeId] = FALSE;
+						}
+					}
+					pIntNodeInfo =
+					    pIntNodeInfo->m_pNextNodeInfo;
+				}
+
+				if ((EplErrorHandlerkInstance_g.m_ulDllErrorEvents & EPL_DLL_ERR_MN_CRC) == 0) {	// decrement CRC threshold counter, because it didn't occur last cycle
+					if (EplErrorHandlerkInstance_g.
+					    m_MnCrcErr.m_dwThresholdCnt > 0) {
+						EplErrorHandlerkInstance_g.
+						    m_MnCrcErr.
+						    m_dwThresholdCnt--;
+					}
+				}
+
+				if ((EplErrorHandlerkInstance_g.m_ulDllErrorEvents & EPL_DLL_ERR_MN_CYCTIMEEXCEED) == 0) {	// decrement cycle exceed threshold counter, because it didn't occur last cycle
+					if (EplErrorHandlerkInstance_g.
+					    m_MnCycTimeExceed.m_dwThresholdCnt >
+					    0) {
+						EplErrorHandlerkInstance_g.
+						    m_MnCycTimeExceed.
+						    m_dwThresholdCnt--;
+					}
+				}
+			}
+#endif
+
+			// reset error events
+			EplErrorHandlerkInstance_g.m_ulDllErrorEvents = 0L;
+
+			break;
+		}
+
+		// unknown type
+	default:
+		{
+		}
+
+	}			// end of switch(pEvent_p->m_EventType)
+
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplErrorHandlerkLinkErrorCounter
+//
+// Description: link specified error counter structure to OD entry
+//
+// Parameters:  pErrorCounter_p         = pointer to error counter structure
+//              uiIndex_p               = OD index
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplErrorHandlerkLinkErrorCounter(tEplErrorHandlerkErrorCounter
+						   * pErrorCounter_p,
+						   unsigned int uiIndex_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplVarParam VarParam;
+
+	VarParam.m_pData = &pErrorCounter_p->m_dwCumulativeCnt;
+	VarParam.m_Size = sizeof(DWORD);
+	VarParam.m_uiIndex = uiIndex_p;
+	VarParam.m_uiSubindex = 0x01;
+	VarParam.m_ValidFlag = kVarValidAll;
+	Ret = EplObdDefineVar(&VarParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	VarParam.m_pData = &pErrorCounter_p->m_dwThresholdCnt;
+	VarParam.m_Size = sizeof(DWORD);
+	VarParam.m_uiIndex = uiIndex_p;
+	VarParam.m_uiSubindex = 0x02;
+	VarParam.m_ValidFlag = kVarValidAll;
+	Ret = EplObdDefineVar(&VarParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	VarParam.m_pData = &pErrorCounter_p->m_dwThreshold;
+	VarParam.m_Size = sizeof(DWORD);
+	VarParam.m_uiIndex = uiIndex_p;
+	VarParam.m_uiSubindex = 0x03;
+	VarParam.m_ValidFlag = kVarValidAll;
+	Ret = EplObdDefineVar(&VarParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplErrorHandlerkLinkErrorCounter
+//
+// Description: link specified error counter structure to OD entry
+//
+// Parameters:  pErrorCounter_p         = pointer to error counter structure
+//              uiIndex_p               = OD index
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+static tEplKernel EplErrorHandlerkLinkArray(DWORD * pdwValue_p,
+					    unsigned int uiValueCount_p,
+					    unsigned int uiIndex_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplVarParam VarParam;
+	tEplObdSize EntrySize;
+	BYTE bIndexEntries;
+
+	EntrySize = (tEplObdSize) sizeof(bIndexEntries);
+	Ret = EplObdReadEntry(uiIndex_p,
+			      0x00, (void GENERIC *)&bIndexEntries, &EntrySize);
+
+	if ((Ret != kEplSuccessful) || (bIndexEntries == 0x00)) {
+		// Object doesn't exist or invalid entry number
+		Ret = kEplObdIndexNotExist;
+		goto Exit;
+	}
+
+	if (bIndexEntries < uiValueCount_p) {
+		uiValueCount_p = bIndexEntries;
+	}
+
+	VarParam.m_Size = sizeof(DWORD);
+	VarParam.m_uiIndex = uiIndex_p;
+	VarParam.m_ValidFlag = kVarValidAll;
+
+	for (VarParam.m_uiSubindex = 0x01;
+	     VarParam.m_uiSubindex <= uiValueCount_p; VarParam.m_uiSubindex++) {
+		VarParam.m_pData = pdwValue_p;
+		Ret = EplObdDefineVar(&VarParam);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		pdwValue_p++;
+	}
+
+      Exit:
+	return Ret;
+}
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplEvent.h b/drivers/staging/epl/EplEvent.h
new file mode 100644
index 000000000000..b6dc1b9b2ab1
--- /dev/null
+++ b/drivers/staging/epl/EplEvent.h
@@ -0,0 +1,279 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for event module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplEvent.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/11/17 16:40:39 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/12 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_EVENT_H_
+#define _EPL_EVENT_H_
+
+#include "EplInc.h"
+#include "EplNmt.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// name and size of event queues
+#define EPL_EVENT_NAME_SHB_KERNEL_TO_USER   "ShbKernelToUser"
+#ifndef EPL_EVENT_SIZE_SHB_KERNEL_TO_USER
+#define EPL_EVENT_SIZE_SHB_KERNEL_TO_USER   32768	// 32 kByte
+#endif
+
+#define EPL_EVENT_NAME_SHB_USER_TO_KERNEL   "ShbUserToKernel"
+#ifndef EPL_EVENT_SIZE_SHB_USER_TO_KERNEL
+#define EPL_EVENT_SIZE_SHB_USER_TO_KERNEL   32768	// 32 kByte
+#endif
+
+// max size of event argument
+#ifndef EPL_MAX_EVENT_ARG_SIZE
+#define EPL_MAX_EVENT_ARG_SIZE      256	// because of PDO
+#endif
+
+#define EPL_DLL_ERR_MN_CRC           0x00000001L	// object 0x1C00
+#define EPL_DLL_ERR_MN_COLLISION     0x00000002L	// object 0x1C01
+#define EPL_DLL_ERR_MN_CYCTIMEEXCEED 0x00000004L	// object 0x1C02
+#define EPL_DLL_ERR_MN_LOSS_LINK     0x00000008L	// object 0x1C03
+#define EPL_DLL_ERR_MN_CN_LATE_PRES  0x00000010L	// objects 0x1C04-0x1C06
+#define EPL_DLL_ERR_MN_CN_LOSS_PRES  0x00000080L	// objects 0x1C07-0x1C09
+#define EPL_DLL_ERR_CN_COLLISION     0x00000400L	// object 0x1C0A
+#define EPL_DLL_ERR_CN_LOSS_SOC      0x00000800L	// object 0x1C0B
+#define EPL_DLL_ERR_CN_LOSS_SOA      0x00001000L	// object 0x1C0C
+#define EPL_DLL_ERR_CN_LOSS_PREQ     0x00002000L	// object 0x1C0D
+#define EPL_DLL_ERR_CN_RECVD_PREQ    0x00004000L	// decrement object 0x1C0D/2
+#define EPL_DLL_ERR_CN_SOC_JITTER    0x00008000L	// object 0x1C0E
+#define EPL_DLL_ERR_CN_CRC           0x00010000L	// object 0x1C0F
+#define EPL_DLL_ERR_CN_LOSS_LINK     0x00020000L	// object 0x1C10
+#define EPL_DLL_ERR_MN_LOSS_STATRES  0x00040000L	// objects 0x1C15-0x1C17 (should be operated by NmtMnu module)
+#define EPL_DLL_ERR_BAD_PHYS_MODE    0x00080000L	// no object
+#define EPL_DLL_ERR_MAC_BUFFER       0x00100000L	// no object (NMT_GT6)
+#define EPL_DLL_ERR_INVALID_FORMAT   0x00200000L	// no object (NMT_GT6)
+#define EPL_DLL_ERR_ADDRESS_CONFLICT 0x00400000L	// no object (remove CN from configuration)
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+// EventType determines the argument of the event
+typedef enum {
+	kEplEventTypeNmtEvent = 0x01,	// NMT event
+	// arg is pointer to tEplNmtEvent
+	kEplEventTypePdoRx = 0x02,	// PDO frame received event (PRes/PReq)
+	// arg is pointer to tEplFrame
+	kEplEventTypePdoTx = 0x03,	// PDO frame transmitted event (PRes/PReq)
+	// arg is pointer to tEplFrameInfo
+	kEplEventTypePdoSoa = 0x04,	// SoA frame received event (isochronous phase completed)
+	// arg is pointer to nothing
+	kEplEventTypeSync = 0x05,	// Sync event (e.g. SoC or anticipated SoC)
+	// arg is pointer to nothing
+	kEplEventTypeTimer = 0x06,	// Timer event
+	// arg is pointer to tEplTimerEventArg
+	kEplEventTypeHeartbeat = 0x07,	// Heartbeat event
+	// arg is pointer to tEplHeartbeatEvent
+	kEplEventTypeDllkCreate = 0x08,	// DLL kernel create event
+	// arg is pointer to the new tEplNmtState
+	kEplEventTypeDllkDestroy = 0x09,	// DLL kernel destroy event
+	// arg is pointer to the old tEplNmtState
+	kEplEventTypeDllkFillTx = 0x0A,	// DLL kernel fill TxBuffer event
+	// arg is pointer to tEplDllAsyncReqPriority
+	kEplEventTypeDllkPresReady = 0x0B,	// DLL kernel PRes ready event
+	// arg is pointer to nothing
+	kEplEventTypeError = 0x0C,	// Error event for API layer
+	// arg is pointer to tEplEventError
+	kEplEventTypeNmtStateChange = 0x0D,	// indicate change of NMT-State
+	// arg is pointer to tEplEventNmtStateChange
+	kEplEventTypeDllError = 0x0E,	// DLL error event for Error handler
+	// arg is pointer to tEplErrorHandlerkEvent
+	kEplEventTypeAsndRx = 0x0F,	// received ASnd frame for DLL user module
+	// arg is pointer to tEplFrame
+	kEplEventTypeDllkServFilter = 0x10,	// configure ServiceIdFilter
+	// arg is pointer to tEplDllCalServiceIdFilter
+	kEplEventTypeDllkIdentity = 0x11,	// configure Identity
+	// arg is pointer to tEplDllIdentParam
+	kEplEventTypeDllkConfig = 0x12,	// configure ConfigParam
+	// arg is pointer to tEplDllConfigParam
+	kEplEventTypeDllkIssueReq = 0x13,	// issue Ident/Status request
+	// arg is pointer to tEplDllCalIssueRequest
+	kEplEventTypeDllkAddNode = 0x14,	// add node to isochronous phase
+	// arg is pointer to tEplDllNodeInfo
+	kEplEventTypeDllkDelNode = 0x15,	// remove node from isochronous phase
+	// arg is pointer to unsigned int
+	kEplEventTypeDllkSoftDelNode = 0x16,	// remove node softly from isochronous phase
+	// arg is pointer to unsigned int
+	kEplEventTypeDllkStartReducedCycle = 0x17,	// start reduced EPL cycle on MN
+	// arg is pointer to nothing
+	kEplEventTypeNmtMnuNmtCmdSent = 0x18,	// NMT command was actually sent
+	// arg is pointer to tEplFrame
+
+} tEplEventType;
+
+// EventSink determines the consumer of the event
+typedef enum {
+	kEplEventSinkSync = 0x00,	// Sync event for application or kernel EPL module
+	kEplEventSinkNmtk = 0x01,	// events for Nmtk module
+	kEplEventSinkDllk = 0x02,	// events for Dllk module
+	kEplEventSinkDlluCal = 0x03,	// events for DlluCal module
+	kEplEventSinkDllkCal = 0x04,	// events for DllkCal module
+	kEplEventSinkPdok = 0x05,	// events for Pdok module
+	kEplEventSinkNmtu = 0x06,	// events for Nmtu module
+	kEplEventSinkErrk = 0x07,	// events for Error handler module
+	kEplEventSinkErru = 0x08,	// events for Error signaling module
+	kEplEventSinkSdoAsySeq = 0x09,	// events for asyncronous SDO Sequence Layer module
+	kEplEventSinkNmtMnu = 0x0A,	// events for NmtMnu module
+	kEplEventSinkLedu = 0x0B,	// events for Ledu module
+	kEplEventSinkApi = 0x0F,	// events for API module
+
+} tEplEventSink;
+
+// EventSource determines the source of an errorevent
+typedef enum {
+	// kernelspace modules
+	kEplEventSourceDllk = 0x01,	// Dllk module
+	kEplEventSourceNmtk = 0x02,	// Nmtk module
+	kEplEventSourceObdk = 0x03,	// Obdk module
+	kEplEventSourcePdok = 0x04,	// Pdok module
+	kEplEventSourceTimerk = 0x05,	// Timerk module
+	kEplEventSourceEventk = 0x06,	// Eventk module
+	kEplEventSourceSyncCb = 0x07,	// sync-Cb
+	kEplEventSourceErrk = 0x08,	// Error handler module
+
+	// userspace modules
+	kEplEventSourceDllu = 0x10,	// Dllu module
+	kEplEventSourceNmtu = 0x11,	// Nmtu module
+	kEplEventSourceNmtCnu = 0x12,	// NmtCnu module
+	kEplEventSourceNmtMnu = 0x13,	// NmtMnu module
+	kEplEventSourceObdu = 0x14,	// Obdu module
+	kEplEventSourceSdoUdp = 0x15,	// Sdo/Udp module
+	kEplEventSourceSdoAsnd = 0x16,	// Sdo/Asnd module
+	kEplEventSourceSdoAsySeq = 0x17,	// Sdo asynchronus Sequence Layer module
+	kEplEventSourceSdoCom = 0x18,	// Sdo command layer module
+	kEplEventSourceTimeru = 0x19,	// Timeru module
+	kEplEventSourceCfgMau = 0x1A,	// CfgMau module
+	kEplEventSourceEventu = 0x1B,	// Eventu module
+	kEplEventSourceEplApi = 0x1C,	// Api module
+	kEplEventSourceLedu = 0x1D,	// Ledu module
+
+} tEplEventSource;
+
+// structure of EPL event (element order must not be changed!)
+typedef struct {
+	tEplEventType m_EventType /*:28 */ ;	// event type
+	tEplEventSink m_EventSink /*:4 */ ;	// event sink
+	tEplNetTime m_NetTime;	// timestamp
+	unsigned int m_uiSize;	// size of argument
+	void *m_pArg;		// argument of event
+
+} tEplEvent;
+
+// short structure of EPL event without argument and its size (element order must not be changed!)
+typedef struct {
+	tEplEventType m_EventType /*:28 */ ;	// event type
+	tEplEventSink m_EventSink /*:4 */ ;	// event sink
+	tEplNetTime m_NetTime;	// timestamp
+
+} tEplEventShort;
+
+typedef struct {
+	unsigned int m_uiIndex;
+	unsigned int m_uiSubIndex;
+
+} tEplEventObdError;
+
+// structure for kEplEventTypeError
+typedef struct {
+	tEplEventSource m_EventSource;	// module which posted this error event
+	tEplKernel m_EplError;	// EPL error which occured
+	union {
+		BYTE m_bArg;
+		DWORD m_dwArg;
+		tEplEventSource m_EventSource;	// from Eventk/u module (originating error source)
+		tEplEventObdError m_ObdError;	// from Obd module
+//        tEplErrHistoryEntry     m_HistoryEntry; // from Nmtk/u module
+
+	} m_Arg;
+
+} tEplEventError;
+
+// structure for kEplEventTypeDllError
+typedef struct {
+	unsigned long m_ulDllErrorEvents;	// EPL_DLL_ERR_*
+	unsigned int m_uiNodeId;
+	tEplNmtState m_NmtState;
+
+} tEplErrorHandlerkEvent;
+
+// callback function to get informed about sync event
+typedef tEplKernel(PUBLIC * tEplSyncCb) (void);
+
+// callback function for generic events
+typedef tEplKernel(PUBLIC * tEplProcessEventCb) (tEplEvent * pEplEvent_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_EVENT_H_
diff --git a/drivers/staging/epl/EplEventk.c b/drivers/staging/epl/EplEventk.c
new file mode 100644
index 000000000000..8068a6c5a0d0
--- /dev/null
+++ b/drivers/staging/epl/EplEventk.c
@@ -0,0 +1,853 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for Epl-Kernelspace-Event-Modul
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplEventk.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.9 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/20 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "kernel/EplEventk.h"
+#include "kernel/EplNmtk.h"
+#include "kernel/EplDllk.h"
+#include "kernel/EplDllkCal.h"
+#include "kernel/EplErrorHandlerk.h"
+#include "Benchmark.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+#include "kernel/EplPdok.h"
+#include "kernel/EplPdokCal.h"
+#endif
+
+#ifdef EPL_NO_FIFO
+#include "user/EplEventu.h"
+#else
+#include "SharedBuff.h"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)     TgtDbgPostTraceValue(v)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)
+#endif
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+#ifndef EPL_NO_FIFO
+	tShbInstance m_pShbKernelToUserInstance;
+	tShbInstance m_pShbUserToKernelInstance;
+#else
+
+#endif
+	tEplSyncCb m_pfnCbSync;
+	unsigned int m_uiUserToKernelFullCount;
+
+} tEplEventkInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+static tEplEventkInstance EplEventkInstance_g;
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+// callback function for incoming events
+#ifndef EPL_NO_FIFO
+static void EplEventkRxSignalHandlerCb(tShbInstance pShbRxInstance_p,
+				       unsigned long ulDataSize_p);
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <Epl-Kernelspace-Event>                             */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventkInit
+//
+// Description: function initializes the first instance
+//
+// Parameters:  pfnCbSync_p = callback-function for sync event
+//
+// Returns:     tEpKernel   = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplEventkInit(tEplSyncCb pfnCbSync_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplEventkAddInstance(pfnCbSync_p);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventkAddInstance
+//
+// Description: function adds one more instance
+//
+// Parameters:  pfnCbSync_p = callback-function for sync event
+//
+// Returns:     tEpKernel   = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplEventkAddInstance(tEplSyncCb pfnCbSync_p)
+{
+	tEplKernel Ret;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+	unsigned int fShbNewCreated;
+#endif
+
+	Ret = kEplSuccessful;
+
+	// init instance structure
+	EplEventkInstance_g.m_uiUserToKernelFullCount = 0;
+
+	// save cb-function
+	EplEventkInstance_g.m_pfnCbSync = pfnCbSync_p;
+
+#ifndef EPL_NO_FIFO
+	// init shared loop buffer
+	// kernel -> user
+	ShbError = ShbCirAllocBuffer(EPL_EVENT_SIZE_SHB_KERNEL_TO_USER,
+				     EPL_EVENT_NAME_SHB_KERNEL_TO_USER,
+				     &EplEventkInstance_g.
+				     m_pShbKernelToUserInstance,
+				     &fShbNewCreated);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventkAddInstance(): ShbCirAllocBuffer(K2U) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+	// user -> kernel
+	ShbError = ShbCirAllocBuffer(EPL_EVENT_SIZE_SHB_USER_TO_KERNEL,
+				     EPL_EVENT_NAME_SHB_USER_TO_KERNEL,
+				     &EplEventkInstance_g.
+				     m_pShbUserToKernelInstance,
+				     &fShbNewCreated);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventkAddInstance(): ShbCirAllocBuffer(U2K) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+	// register eventhandler
+	ShbError =
+	    ShbCirSetSignalHandlerNewData(EplEventkInstance_g.
+					  m_pShbUserToKernelInstance,
+					  EplEventkRxSignalHandlerCb,
+					  kshbPriorityHigh);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventkAddInstance(): ShbCirSetSignalHandlerNewData(U2K) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+      Exit:
+#endif
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventkDelInstance
+//
+// Description: function deletes instance and frees the buffers
+//
+// Parameters:  void
+//
+// Returns:     tEpKernel   = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplEventkDelInstance()
+{
+	tEplKernel Ret;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+#endif
+
+	Ret = kEplSuccessful;
+
+#ifndef EPL_NO_FIFO
+	// set eventhandler to NULL
+	ShbError =
+	    ShbCirSetSignalHandlerNewData(EplEventkInstance_g.
+					  m_pShbUserToKernelInstance, NULL,
+					  kShbPriorityNormal);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventkDelInstance(): ShbCirSetSignalHandlerNewData(U2K) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+	}
+	// free buffer User -> Kernel
+	ShbError =
+	    ShbCirReleaseBuffer(EplEventkInstance_g.m_pShbUserToKernelInstance);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventkDelInstance(): ShbCirReleaseBuffer(U2K) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+	} else {
+		EplEventkInstance_g.m_pShbUserToKernelInstance = NULL;
+	}
+
+	// free buffer  Kernel -> User
+	ShbError =
+	    ShbCirReleaseBuffer(EplEventkInstance_g.m_pShbKernelToUserInstance);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventkDelInstance(): ShbCirReleaseBuffer(K2U) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+	} else {
+		EplEventkInstance_g.m_pShbKernelToUserInstance = NULL;
+	}
+#endif
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventkProcess
+//
+// Description: Kernelthread that dispatches events in kernel part
+//
+// Parameters:  pEvent_p    = pointer to event-structure from buffer
+//
+// Returns:     tEpKernel   = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplEventkProcess(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+	tEplEventSource EventSource;
+
+	Ret = kEplSuccessful;
+
+	// error handling if event queue is full
+	if (EplEventkInstance_g.m_uiUserToKernelFullCount > 0) {	// UserToKernel event queue has run out of space -> kEplNmtEventInternComError
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+		tEplEvent Event;
+		tEplNmtEvent NmtEvent;
+#endif
+#ifndef EPL_NO_FIFO
+		tShbError ShbError;
+#endif
+
+		// directly call NMTk process function, because event queue is full
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+		NmtEvent = kEplNmtEventInternComError;
+		Event.m_EventSink = kEplEventSinkNmtk;
+		Event.m_NetTime.m_dwNanoSec = 0;
+		Event.m_NetTime.m_dwSec = 0;
+		Event.m_EventType = kEplEventTypeNmtEvent;
+		Event.m_pArg = &NmtEvent;
+		Event.m_uiSize = sizeof(NmtEvent);
+		Ret = EplNmtkProcess(&Event);
+#endif
+
+		// NMT state machine changed to reset (i.e. NMT_GS_RESET_COMMUNICATION)
+		// now, it is safe to reset the counter and empty the event queue
+#ifndef EPL_NO_FIFO
+		ShbError =
+		    ShbCirResetBuffer(EplEventkInstance_g.
+				      m_pShbUserToKernelInstance, 1000, NULL);
+#endif
+
+		EplEventkInstance_g.m_uiUserToKernelFullCount = 0;
+		TGT_DBG_SIGNAL_TRACE_POINT(22);
+
+		// also discard the current event (it doesn't matter if we lose another event)
+		goto Exit;
+	}
+	// check m_EventSink
+	switch (pEvent_p->m_EventSink) {
+	case kEplEventSinkSync:
+		{
+			if (EplEventkInstance_g.m_pfnCbSync != NULL) {
+				Ret = EplEventkInstance_g.m_pfnCbSync();
+				if (Ret == kEplSuccessful) {
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+					// mark TPDOs as valid
+					Ret = EplPdokCalSetTpdosValid(TRUE);
+#endif
+				} else if ((Ret != kEplReject)
+					   && (Ret != kEplShutdown)) {
+					EventSource = kEplEventSourceSyncCb;
+
+					// Error event for API layer
+					EplEventkPostError
+					    (kEplEventSourceEventk, Ret,
+					     sizeof(EventSource), &EventSource);
+				}
+			}
+			break;
+		}
+
+		// NMT-Kernel-Modul
+	case kEplEventSinkNmtk:
+		{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+			Ret = EplNmtkProcess(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceNmtk;
+
+				// Error event for API layer
+				EplEventkPostError(kEplEventSourceEventk,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+#endif
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+			if ((pEvent_p->m_EventType == kEplEventTypeNmtEvent)
+			    &&
+			    ((*((tEplNmtEvent *) pEvent_p->m_pArg) ==
+			      kEplNmtEventDllCeSoa)
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			     || (*((tEplNmtEvent *) pEvent_p->m_pArg) ==
+				 kEplNmtEventDllMeSoaSent)
+#endif
+			    )) {	// forward SoA event to error handler
+				Ret = EplErrorHandlerkProcess(pEvent_p);
+				if ((Ret != kEplSuccessful)
+				    && (Ret != kEplShutdown)) {
+					EventSource = kEplEventSourceErrk;
+
+					// Error event for API layer
+					EplEventkPostError
+					    (kEplEventSourceEventk, Ret,
+					     sizeof(EventSource), &EventSource);
+				}
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+				// forward SoA event to PDO module
+				pEvent_p->m_EventType = kEplEventTypePdoSoa;
+				Ret = EplPdokProcess(pEvent_p);
+				if ((Ret != kEplSuccessful)
+				    && (Ret != kEplShutdown)) {
+					EventSource = kEplEventSourcePdok;
+
+					// Error event for API layer
+					EplEventkPostError
+					    (kEplEventSourceEventk, Ret,
+					     sizeof(EventSource), &EventSource);
+				}
+#endif
+
+			}
+			break;
+#endif
+		}
+
+		// events for Dllk module
+	case kEplEventSinkDllk:
+		{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+			Ret = EplDllkProcess(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceDllk;
+
+				// Error event for API layer
+				EplEventkPostError(kEplEventSourceEventk,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+#endif
+			break;
+		}
+
+		// events for DllkCal module
+	case kEplEventSinkDllkCal:
+		{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+			Ret = EplDllkCalProcess(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceDllk;
+
+				// Error event for API layer
+				EplEventkPostError(kEplEventSourceEventk,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+#endif
+			break;
+		}
+
+		//
+	case kEplEventSinkPdok:
+		{
+			// PDO-Module
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+			Ret = EplPdokProcess(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourcePdok;
+
+				// Error event for API layer
+				EplEventkPostError(kEplEventSourceEventk,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+#endif
+			break;
+		}
+
+		// events for Error handler module
+	case kEplEventSinkErrk:
+		{
+			// only call error handler if DLL is present
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+			Ret = EplErrorHandlerkProcess(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceErrk;
+
+				// Error event for API layer
+				EplEventkPostError(kEplEventSourceEventk,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+			break;
+#endif
+		}
+
+		// unknown sink
+	default:
+		{
+			Ret = kEplEventUnknownSink;
+		}
+
+	}			// end of switch(pEvent_p->m_EventSink)
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventkPost
+//
+// Description: post events from kernel part
+//
+// Parameters:  pEvent_p    = pointer to event-structure from buffer
+//
+// Returns:     tEpKernel   = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplEventkPost(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+	tShbCirChunk ShbCirChunk;
+	unsigned long ulDataSize;
+	unsigned int fBufferCompleted;
+#endif
+
+	Ret = kEplSuccessful;
+
+	// the event must be posted by using the abBuffer
+	// it is neede because the Argument must by copied
+	// to the buffer too and not only the pointer
+
+#ifndef EPL_NO_FIFO
+	// 2006/08/03 d.k.: Event and argument are posted as separate chunks to the event queue.
+	ulDataSize =
+	    sizeof(tEplEvent) +
+	    ((pEvent_p->m_pArg != NULL) ? pEvent_p->m_uiSize : 0);
+#endif
+
+	// decide in which buffer the event have to write
+	switch (pEvent_p->m_EventSink) {
+		// kernelspace modules
+	case kEplEventSinkSync:
+	case kEplEventSinkNmtk:
+	case kEplEventSinkDllk:
+	case kEplEventSinkDllkCal:
+	case kEplEventSinkPdok:
+	case kEplEventSinkErrk:
+		{
+#ifndef EPL_NO_FIFO
+			// post message
+			BENCHMARK_MOD_27_SET(2);
+			ShbError =
+			    ShbCirAllocDataBlock(EplEventkInstance_g.
+						 m_pShbUserToKernelInstance,
+						 &ShbCirChunk, ulDataSize);
+			switch (ShbError) {
+			case kShbOk:
+				break;
+
+			case kShbBufferFull:
+				{
+					EplEventkInstance_g.
+					    m_uiUserToKernelFullCount++;
+					Ret = kEplEventPostError;
+					goto Exit;
+				}
+
+			default:
+				{
+					EPL_DBGLVL_EVENTK_TRACE1
+					    ("EplEventkPost(): ShbCirAllocDataBlock(U2K) -> 0x%X\n",
+					     ShbError);
+					Ret = kEplEventPostError;
+					goto Exit;
+				}
+			}
+			ShbError =
+			    ShbCirWriteDataChunk(EplEventkInstance_g.
+						 m_pShbUserToKernelInstance,
+						 &ShbCirChunk, pEvent_p,
+						 sizeof(tEplEvent),
+						 &fBufferCompleted);
+			if (ShbError != kShbOk) {
+				EPL_DBGLVL_EVENTK_TRACE1
+				    ("EplEventkPost(): ShbCirWriteDataChunk(U2K) -> 0x%X\n",
+				     ShbError);
+				Ret = kEplEventPostError;
+				goto Exit;
+			}
+			if (fBufferCompleted == FALSE) {
+				ShbError =
+				    ShbCirWriteDataChunk(EplEventkInstance_g.
+							 m_pShbUserToKernelInstance,
+							 &ShbCirChunk,
+							 pEvent_p->m_pArg,
+							 (unsigned long)
+							 pEvent_p->m_uiSize,
+							 &fBufferCompleted);
+				if ((ShbError != kShbOk)
+				    || (fBufferCompleted == FALSE)) {
+					EPL_DBGLVL_EVENTK_TRACE1
+					    ("EplEventkPost(): ShbCirWriteDataChunk2(U2K) -> 0x%X\n",
+					     ShbError);
+					Ret = kEplEventPostError;
+					goto Exit;
+				}
+			}
+			BENCHMARK_MOD_27_RESET(2);
+
+#else
+			Ret = EplEventkProcess(pEvent_p);
+#endif
+
+			break;
+		}
+
+		// userspace modules
+	case kEplEventSinkNmtu:
+	case kEplEventSinkNmtMnu:
+	case kEplEventSinkSdoAsySeq:
+	case kEplEventSinkApi:
+	case kEplEventSinkDlluCal:
+	case kEplEventSinkErru:
+		{
+#ifndef EPL_NO_FIFO
+			// post message
+//            BENCHMARK_MOD_27_SET(3);    // 74 µs until reset
+			ShbError =
+			    ShbCirAllocDataBlock(EplEventkInstance_g.
+						 m_pShbKernelToUserInstance,
+						 &ShbCirChunk, ulDataSize);
+			if (ShbError != kShbOk) {
+				EPL_DBGLVL_EVENTK_TRACE1
+				    ("EplEventkPost(): ShbCirAllocDataBlock(K2U) -> 0x%X\n",
+				     ShbError);
+				Ret = kEplEventPostError;
+				goto Exit;
+			}
+			ShbError =
+			    ShbCirWriteDataChunk(EplEventkInstance_g.
+						 m_pShbKernelToUserInstance,
+						 &ShbCirChunk, pEvent_p,
+						 sizeof(tEplEvent),
+						 &fBufferCompleted);
+			if (ShbError != kShbOk) {
+				EPL_DBGLVL_EVENTK_TRACE1
+				    ("EplEventkPost(): ShbCirWriteDataChunk(K2U) -> 0x%X\n",
+				     ShbError);
+				Ret = kEplEventPostError;
+				goto Exit;
+			}
+			if (fBufferCompleted == FALSE) {
+				ShbError =
+				    ShbCirWriteDataChunk(EplEventkInstance_g.
+							 m_pShbKernelToUserInstance,
+							 &ShbCirChunk,
+							 pEvent_p->m_pArg,
+							 (unsigned long)
+							 pEvent_p->m_uiSize,
+							 &fBufferCompleted);
+				if ((ShbError != kShbOk)
+				    || (fBufferCompleted == FALSE)) {
+					EPL_DBGLVL_EVENTK_TRACE1
+					    ("EplEventkPost(): ShbCirWriteDataChunk2(K2U) -> 0x%X\n",
+					     ShbError);
+					Ret = kEplEventPostError;
+					goto Exit;
+				}
+			}
+//            BENCHMARK_MOD_27_RESET(3);  // 82 µs until ShbCirGetReadDataSize() in EplEventu
+
+#else
+			Ret = EplEventuProcess(pEvent_p);
+#endif
+
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplEventUnknownSink;
+		}
+
+	}			// end of switch(pEvent_p->m_EventSink)
+
+#ifndef EPL_NO_FIFO
+      Exit:
+#endif
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventkPostError
+//
+// Description: post error event from kernel part to API layer
+//
+// Parameters:  EventSource_p   = source-module of the error event
+//              EplError_p      = code of occured error
+//              ArgSize_p       = size of the argument
+//              pArg_p          = pointer to the argument
+//
+// Returns:     tEpKernel       = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplEventkPostError(tEplEventSource EventSource_p,
+				     tEplKernel EplError_p,
+				     unsigned int uiArgSize_p, void *pArg_p)
+{
+	tEplKernel Ret;
+	BYTE abBuffer[EPL_MAX_EVENT_ARG_SIZE];
+	tEplEventError *pEventError = (tEplEventError *) abBuffer;
+	tEplEvent EplEvent;
+
+	Ret = kEplSuccessful;
+
+	// create argument
+	pEventError->m_EventSource = EventSource_p;
+	pEventError->m_EplError = EplError_p;
+	EPL_MEMCPY(&pEventError->m_Arg, pArg_p, uiArgSize_p);
+
+	// create event
+	EplEvent.m_EventType = kEplEventTypeError;
+	EplEvent.m_EventSink = kEplEventSinkApi;
+	EPL_MEMSET(&EplEvent.m_NetTime, 0x00, sizeof(EplEvent.m_NetTime));
+	EplEvent.m_uiSize =
+	    (sizeof(EventSource_p) + sizeof(EplError_p) + uiArgSize_p);
+	EplEvent.m_pArg = &abBuffer[0];
+
+	// post errorevent
+	Ret = EplEventkPost(&EplEvent);
+
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventkRxSignalHandlerCb()
+//
+// Description: Callback-function for events from user and kernel part
+//
+// Parameters:  pShbRxInstance_p    = Instance-pointer of buffer
+//              ulDataSize_p        = size of data
+//
+// Returns: void
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#ifndef EPL_NO_FIFO
+static void EplEventkRxSignalHandlerCb(tShbInstance pShbRxInstance_p,
+				       unsigned long ulDataSize_p)
+{
+	tEplEvent *pEplEvent;
+	tShbError ShbError;
+//unsigned long   ulBlockCount;
+//unsigned long   ulDataSize;
+	BYTE abDataBuffer[sizeof(tEplEvent) + EPL_MAX_EVENT_ARG_SIZE];
+	// d.k.: abDataBuffer contains the complete tEplEvent structure
+	//       and behind this the argument
+
+	TGT_DBG_SIGNAL_TRACE_POINT(20);
+
+	BENCHMARK_MOD_27_RESET(0);
+	// copy data from event queue
+	ShbError = ShbCirReadDataBlock(pShbRxInstance_p,
+				       &abDataBuffer[0],
+				       sizeof(abDataBuffer), &ulDataSize_p);
+	if (ShbError != kShbOk) {
+		// error goto exit
+		goto Exit;
+	}
+	// resolve the pointer to the event structure
+	pEplEvent = (tEplEvent *) abDataBuffer;
+	// set Datasize
+	pEplEvent->m_uiSize = (ulDataSize_p - sizeof(tEplEvent));
+	if (pEplEvent->m_uiSize > 0) {
+		// set pointer to argument
+		pEplEvent->m_pArg = &abDataBuffer[sizeof(tEplEvent)];
+	} else {
+		//set pointer to NULL
+		pEplEvent->m_pArg = NULL;
+	}
+
+	BENCHMARK_MOD_27_SET(0);
+	// call processfunction
+	EplEventkProcess(pEplEvent);
+
+      Exit:
+	return;
+}
+#endif
+
+// EOF
diff --git a/drivers/staging/epl/EplEventu.c b/drivers/staging/epl/EplEventu.c
new file mode 100644
index 000000000000..815f9a87abf8
--- /dev/null
+++ b/drivers/staging/epl/EplEventu.c
@@ -0,0 +1,814 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for Epl-Userspace-Event-Modul
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplEventu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/11/17 16:40:39 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/20 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplEventu.h"
+#include "user/EplNmtu.h"
+#include "user/EplNmtMnu.h"
+#include "user/EplSdoAsySequ.h"
+#include "user/EplDlluCal.h"
+#include "user/EplLedu.h"
+#include "Benchmark.h"
+
+#ifdef EPL_NO_FIFO
+#include "kernel/EplEventk.h"
+#else
+#include "SharedBuff.h"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)     TgtDbgPostTraceValue(v)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)
+#endif
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+#ifndef EPL_NO_FIFO
+	tShbInstance m_pShbKernelToUserInstance;
+	tShbInstance m_pShbUserToKernelInstance;
+#endif
+	tEplProcessEventCb m_pfnApiProcessEventCb;
+
+} tEplEventuInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//#ifndef EPL_NO_FIFO
+static tEplEventuInstance EplEventuInstance_g;
+//#endif
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+#ifndef EPL_NO_FIFO
+// callback function for incomming events
+static void EplEventuRxSignalHandlerCb(tShbInstance pShbRxInstance_p,
+				       unsigned long ulDataSize_p);
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <Epl-User-Event>                                    */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventuInit
+//
+// Description: function initialize the first instance
+//
+//
+//
+// Parameters:  pfnApiProcessEventCb_p  = function pointer for API event callback
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplEventuInit(tEplProcessEventCb pfnApiProcessEventCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplEventuAddInstance(pfnApiProcessEventCb_p);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventuAddInstance
+//
+// Description: function add one more instance
+//
+//
+//
+// Parameters:  pfnApiProcessEventCb_p  = function pointer for API event callback
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplEventuAddInstance(tEplProcessEventCb
+				       pfnApiProcessEventCb_p)
+{
+	tEplKernel Ret;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+	unsigned int fShbNewCreated;
+#endif
+
+	Ret = kEplSuccessful;
+
+	// init instance variables
+	EplEventuInstance_g.m_pfnApiProcessEventCb = pfnApiProcessEventCb_p;
+
+#ifndef EPL_NO_FIFO
+	// init shared loop buffer
+	// kernel -> user
+	ShbError = ShbCirAllocBuffer(EPL_EVENT_SIZE_SHB_KERNEL_TO_USER,
+				     EPL_EVENT_NAME_SHB_KERNEL_TO_USER,
+				     &EplEventuInstance_g.
+				     m_pShbKernelToUserInstance,
+				     &fShbNewCreated);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventuAddInstance(): ShbCirAllocBuffer(K2U) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+	// user -> kernel
+	ShbError = ShbCirAllocBuffer(EPL_EVENT_SIZE_SHB_USER_TO_KERNEL,
+				     EPL_EVENT_NAME_SHB_USER_TO_KERNEL,
+				     &EplEventuInstance_g.
+				     m_pShbUserToKernelInstance,
+				     &fShbNewCreated);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventuAddInstance(): ShbCirAllocBuffer(U2K) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+	// register eventhandler
+	ShbError =
+	    ShbCirSetSignalHandlerNewData(EplEventuInstance_g.
+					  m_pShbKernelToUserInstance,
+					  EplEventuRxSignalHandlerCb,
+					  kShbPriorityNormal);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventuAddInstance(): ShbCirSetSignalHandlerNewData(K2U) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+      Exit:
+#endif
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventuDelInstance
+//
+// Description: function delete instance an free the bufferstructure
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplEventuDelInstance()
+{
+	tEplKernel Ret;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+#endif
+
+	Ret = kEplSuccessful;
+
+#ifndef EPL_NO_FIFO
+	// set eventhandler to NULL
+	ShbError =
+	    ShbCirSetSignalHandlerNewData(EplEventuInstance_g.
+					  m_pShbKernelToUserInstance, NULL,
+					  kShbPriorityNormal);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventuDelInstance(): ShbCirSetSignalHandlerNewData(K2U) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+	}
+	// free buffer User -> Kernel
+	ShbError =
+	    ShbCirReleaseBuffer(EplEventuInstance_g.m_pShbUserToKernelInstance);
+	if ((ShbError != kShbOk) && (ShbError != kShbMemUsedByOtherProcs)) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventuDelInstance(): ShbCirReleaseBuffer(U2K) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+	} else {
+		EplEventuInstance_g.m_pShbUserToKernelInstance = NULL;
+	}
+
+	// free buffer  Kernel -> User
+	ShbError =
+	    ShbCirReleaseBuffer(EplEventuInstance_g.m_pShbKernelToUserInstance);
+	if ((ShbError != kShbOk) && (ShbError != kShbMemUsedByOtherProcs)) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventuDelInstance(): ShbCirReleaseBuffer(K2U) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+	} else {
+		EplEventuInstance_g.m_pShbKernelToUserInstance = NULL;
+	}
+
+#endif
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventuProcess
+//
+// Description: Kernelthread that dispatches events in kernelspace
+//
+//
+//
+// Parameters:  pEvent_p = pointer to event-structur from buffer
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplEventuProcess(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+	tEplEventSource EventSource;
+
+	Ret = kEplSuccessful;
+
+	// check m_EventSink
+	switch (pEvent_p->m_EventSink) {
+		// NMT-User-Module
+	case kEplEventSinkNmtu:
+		{
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+			Ret = EplNmtuProcessEvent(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceNmtu;
+
+				// Error event for API layer
+				EplEventuPostError(kEplEventSourceEventu,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+#endif
+			break;
+		}
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+		// NMT-MN-User-Module
+	case kEplEventSinkNmtMnu:
+		{
+			Ret = EplNmtMnuProcessEvent(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceNmtMnu;
+
+				// Error event for API layer
+				EplEventuPostError(kEplEventSourceEventu,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+			break;
+		}
+#endif
+
+#if ((((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)   \
+     || (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0))
+		// events for asynchronus SDO Sequence Layer
+	case kEplEventSinkSdoAsySeq:
+		{
+			Ret = EplSdoAsySeqProcessEvent(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceSdoAsySeq;
+
+				// Error event for API layer
+				EplEventuPostError(kEplEventSourceEventu,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+			break;
+		}
+#endif
+
+		// LED user part module
+	case kEplEventSinkLedu:
+		{
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+			Ret = EplLeduProcessEvent(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceLedu;
+
+				// Error event for API layer
+				EplEventuPostError(kEplEventSourceEventu,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+#endif
+			break;
+		}
+
+		// event for EPL api
+	case kEplEventSinkApi:
+		{
+			if (EplEventuInstance_g.m_pfnApiProcessEventCb != NULL) {
+				Ret =
+				    EplEventuInstance_g.
+				    m_pfnApiProcessEventCb(pEvent_p);
+				if ((Ret != kEplSuccessful)
+				    && (Ret != kEplShutdown)) {
+					EventSource = kEplEventSourceEplApi;
+
+					// Error event for API layer
+					EplEventuPostError
+					    (kEplEventSourceEventu, Ret,
+					     sizeof(EventSource), &EventSource);
+				}
+			}
+			break;
+
+		}
+
+	case kEplEventSinkDlluCal:
+		{
+			Ret = EplDlluCalProcess(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceDllu;
+
+				// Error event for API layer
+				EplEventuPostError(kEplEventSourceEventu,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+			break;
+
+		}
+
+	case kEplEventSinkErru:
+		{
+			/*
+			   Ret = EplErruProcess(pEvent_p);
+			   if ((Ret != kEplSuccessful) && (Ret != kEplShutdown))
+			   {
+			   EventSource = kEplEventSourceErru;
+
+			   // Error event for API layer
+			   EplEventuPostError(kEplEventSourceEventu,
+			   Ret,
+			   sizeof(EventSource),
+			   &EventSource);
+			   }
+			 */
+			break;
+
+		}
+
+		// unknown sink
+	default:
+		{
+			Ret = kEplEventUnknownSink;
+		}
+
+	}			// end of switch(pEvent_p->m_EventSink)
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventuPost
+//
+// Description: post events from userspace
+//
+//
+//
+// Parameters:  pEvent_p = pointer to event-structur from buffer
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplEventuPost(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+	tShbCirChunk ShbCirChunk;
+	unsigned long ulDataSize;
+	unsigned int fBufferCompleted;
+#endif
+
+	Ret = kEplSuccessful;
+
+#ifndef EPL_NO_FIFO
+	// 2006/08/03 d.k.: Event and argument are posted as separate chunks to the event queue.
+	ulDataSize =
+	    sizeof(tEplEvent) +
+	    ((pEvent_p->m_pArg != NULL) ? pEvent_p->m_uiSize : 0);
+#endif
+
+	// decide in which buffer the event have to write
+	switch (pEvent_p->m_EventSink) {
+		// kernelspace modules
+	case kEplEventSinkSync:
+	case kEplEventSinkNmtk:
+	case kEplEventSinkDllk:
+	case kEplEventSinkDllkCal:
+	case kEplEventSinkPdok:
+	case kEplEventSinkErrk:
+		{
+#ifndef EPL_NO_FIFO
+			// post message
+			ShbError =
+			    ShbCirAllocDataBlock(EplEventuInstance_g.
+						 m_pShbUserToKernelInstance,
+						 &ShbCirChunk, ulDataSize);
+			if (ShbError != kShbOk) {
+				EPL_DBGLVL_EVENTK_TRACE1
+				    ("EplEventuPost(): ShbCirAllocDataBlock(U2K) -> 0x%X\n",
+				     ShbError);
+				Ret = kEplEventPostError;
+				goto Exit;
+			}
+			ShbError =
+			    ShbCirWriteDataChunk(EplEventuInstance_g.
+						 m_pShbUserToKernelInstance,
+						 &ShbCirChunk, pEvent_p,
+						 sizeof(tEplEvent),
+						 &fBufferCompleted);
+			if (ShbError != kShbOk) {
+				EPL_DBGLVL_EVENTK_TRACE1
+				    ("EplEventuPost(): ShbCirWriteDataChunk(U2K) -> 0x%X\n",
+				     ShbError);
+				Ret = kEplEventPostError;
+				goto Exit;
+			}
+			if (fBufferCompleted == FALSE) {
+				ShbError =
+				    ShbCirWriteDataChunk(EplEventuInstance_g.
+							 m_pShbUserToKernelInstance,
+							 &ShbCirChunk,
+							 pEvent_p->m_pArg,
+							 (unsigned long)
+							 pEvent_p->m_uiSize,
+							 &fBufferCompleted);
+				if ((ShbError != kShbOk)
+				    || (fBufferCompleted == FALSE)) {
+					EPL_DBGLVL_EVENTK_TRACE1
+					    ("EplEventuPost(): ShbCirWriteDataChunk2(U2K) -> 0x%X\n",
+					     ShbError);
+					Ret = kEplEventPostError;
+					goto Exit;
+				}
+			}
+#else
+			Ret = EplEventkProcess(pEvent_p);
+#endif
+
+			break;
+		}
+
+		// userspace modules
+	case kEplEventSinkNmtMnu:
+	case kEplEventSinkNmtu:
+	case kEplEventSinkSdoAsySeq:
+	case kEplEventSinkApi:
+	case kEplEventSinkDlluCal:
+	case kEplEventSinkErru:
+	case kEplEventSinkLedu:
+		{
+#ifndef EPL_NO_FIFO
+			// post message
+			ShbError =
+			    ShbCirAllocDataBlock(EplEventuInstance_g.
+						 m_pShbKernelToUserInstance,
+						 &ShbCirChunk, ulDataSize);
+			if (ShbError != kShbOk) {
+				EPL_DBGLVL_EVENTK_TRACE1
+				    ("EplEventuPost(): ShbCirAllocDataBlock(K2U) -> 0x%X\n",
+				     ShbError);
+				Ret = kEplEventPostError;
+				goto Exit;
+			}
+			ShbError =
+			    ShbCirWriteDataChunk(EplEventuInstance_g.
+						 m_pShbKernelToUserInstance,
+						 &ShbCirChunk, pEvent_p,
+						 sizeof(tEplEvent),
+						 &fBufferCompleted);
+			if (ShbError != kShbOk) {
+				EPL_DBGLVL_EVENTK_TRACE1
+				    ("EplEventuPost(): ShbCirWriteDataChunk(K2U) -> 0x%X\n",
+				     ShbError);
+				Ret = kEplEventPostError;
+				goto Exit;
+			}
+			if (fBufferCompleted == FALSE) {
+				ShbError =
+				    ShbCirWriteDataChunk(EplEventuInstance_g.
+							 m_pShbKernelToUserInstance,
+							 &ShbCirChunk,
+							 pEvent_p->m_pArg,
+							 (unsigned long)
+							 pEvent_p->m_uiSize,
+							 &fBufferCompleted);
+				if ((ShbError != kShbOk)
+				    || (fBufferCompleted == FALSE)) {
+					EPL_DBGLVL_EVENTK_TRACE1
+					    ("EplEventuPost(): ShbCirWriteDataChunk2(K2U) -> 0x%X\n",
+					     ShbError);
+					Ret = kEplEventPostError;
+					goto Exit;
+				}
+			}
+#else
+			Ret = EplEventuProcess(pEvent_p);
+#endif
+
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplEventUnknownSink;
+		}
+
+	}			// end of switch(pEvent_p->m_EventSink)
+
+#ifndef EPL_NO_FIFO
+      Exit:
+#endif
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventuPostError
+//
+// Description: post errorevent from userspace
+//
+//
+//
+// Parameters:  EventSource_p   = source-module of the errorevent
+//              EplError_p     = code of occured error
+//              uiArgSize_p     = size of the argument
+//              pArg_p          = pointer to the argument
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplEventuPostError(tEplEventSource EventSource_p,
+				     tEplKernel EplError_p,
+				     unsigned int uiArgSize_p, void *pArg_p)
+{
+	tEplKernel Ret;
+	BYTE abBuffer[EPL_MAX_EVENT_ARG_SIZE];
+	tEplEventError *pEventError = (tEplEventError *) abBuffer;
+	tEplEvent EplEvent;
+
+	Ret = kEplSuccessful;
+
+	// create argument
+	pEventError->m_EventSource = EventSource_p;
+	pEventError->m_EplError = EplError_p;
+	EPL_MEMCPY(&pEventError->m_Arg, pArg_p, uiArgSize_p);
+
+	// create event
+	EplEvent.m_EventType = kEplEventTypeError;
+	EplEvent.m_EventSink = kEplEventSinkApi;
+	EPL_MEMSET(&EplEvent.m_NetTime, 0x00, sizeof(EplEvent.m_NetTime));
+	EplEvent.m_uiSize =
+	    (sizeof(EventSource_p) + sizeof(EplError_p) + uiArgSize_p);
+	EplEvent.m_pArg = &abBuffer[0];
+
+	// post errorevent
+	Ret = EplEventuPost(&EplEvent);
+
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventuRxSignalHandlerCb()
+//
+// Description: Callback-function for evets from kernelspace
+//
+//
+//
+// Parameters:  pShbRxInstance_p    = Instance-pointer for buffer
+//              ulDataSize_p        = size of data
+//
+//
+// Returns: void
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#ifndef EPL_NO_FIFO
+static void EplEventuRxSignalHandlerCb(tShbInstance pShbRxInstance_p,
+				       unsigned long ulDataSize_p)
+{
+	tEplEvent *pEplEvent;
+	tShbError ShbError;
+//unsigned long   ulBlockCount;
+//unsigned long   ulDataSize;
+	BYTE abDataBuffer[sizeof(tEplEvent) + EPL_MAX_EVENT_ARG_SIZE];
+	// d.k.: abDataBuffer contains the complete tEplEvent structure
+	//       and behind this the argument
+
+	TGT_DBG_SIGNAL_TRACE_POINT(21);
+
+// d.k. not needed because it is already done in SharedBuff
+/*    do
+    {
+        BENCHMARK_MOD_28_SET(1);    // 4 µs until reset
+        // get messagesize
+        ShbError = ShbCirGetReadDataSize (pShbRxInstance_p, &ulDataSize);
+        if(ShbError != kShbOk)
+        {
+            // error goto exit
+            goto Exit;
+        }
+
+        BENCHMARK_MOD_28_RESET(1);  // 14 µs until set
+*/
+	// copy data from event queue
+	ShbError = ShbCirReadDataBlock(pShbRxInstance_p,
+				       &abDataBuffer[0],
+				       sizeof(abDataBuffer), &ulDataSize_p);
+	if (ShbError != kShbOk) {
+		// error goto exit
+		goto Exit;
+	}
+	// resolve the pointer to the event structure
+	pEplEvent = (tEplEvent *) abDataBuffer;
+	// set Datasize
+	pEplEvent->m_uiSize = (ulDataSize_p - sizeof(tEplEvent));
+	if (pEplEvent->m_uiSize > 0) {
+		// set pointer to argument
+		pEplEvent->m_pArg = &abDataBuffer[sizeof(tEplEvent)];
+	} else {
+		//set pointer to NULL
+		pEplEvent->m_pArg = NULL;
+	}
+
+	BENCHMARK_MOD_28_SET(1);
+	// call processfunction
+	EplEventuProcess(pEplEvent);
+
+	BENCHMARK_MOD_28_RESET(1);
+	// read number of left messages to process
+// d.k. not needed because it is already done in SharedBuff
+/*        ShbError = ShbCirGetReadBlockCount (pShbRxInstance_p, &ulBlockCount);
+        if (ShbError != kShbOk)
+        {
+            // error goto exit
+            goto Exit;
+        }
+    } while (ulBlockCount > 0);
+*/
+      Exit:
+	return;
+}
+#endif
+
+// EOF
diff --git a/drivers/staging/epl/EplFrame.h b/drivers/staging/epl/EplFrame.h
new file mode 100644
index 000000000000..9a7f8b9f594e
--- /dev/null
+++ b/drivers/staging/epl/EplFrame.h
@@ -0,0 +1,344 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for EPL frames
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplFrame.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/06/23 14:56:33 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_FRAME_H_
+#define _EPL_FRAME_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// defines for EplFrame.m_wFlag
+#define EPL_FRAME_FLAG1_RD          0x01	// ready                                    (PReq, PRes)
+#define EPL_FRAME_FLAG1_ER          0x02	// exception reset (error signalling)       (SoA)
+#define EPL_FRAME_FLAG1_EA          0x04	// exception acknowledge (error signalling) (PReq, SoA)
+#define EPL_FRAME_FLAG1_EC          0x08	// exception clear (error signalling)       (StatusRes)
+#define EPL_FRAME_FLAG1_EN          0x10	// exception new (error signalling)         (PRes, StatusRes)
+#define EPL_FRAME_FLAG1_MS          0x20	// multiplexed slot                         (PReq)
+#define EPL_FRAME_FLAG1_PS          0x40	// prescaled slot                           (SoC)
+#define EPL_FRAME_FLAG1_MC          0x80	// multiplexed cycle completed              (SoC)
+#define EPL_FRAME_FLAG2_RS          0x07	// number of pending requests to send       (PRes, StatusRes, IdentRes)
+#define EPL_FRAME_FLAG2_PR          0x38	// priority of requested asynch. frame      (PRes, StatusRes, IdentRes)
+#define EPL_FRAME_FLAG2_PR_SHIFT    3	// shift of priority of requested asynch. frame
+
+// error history/status entry types
+#define EPL_ERR_ENTRYTYPE_STATUS        0x8000
+#define EPL_ERR_ENTRYTYPE_HISTORY       0x0000
+#define EPL_ERR_ENTRYTYPE_EMCY          0x4000
+#define EPL_ERR_ENTRYTYPE_MODE_ACTIVE   0x1000
+#define EPL_ERR_ENTRYTYPE_MODE_CLEARED  0x2000
+#define EPL_ERR_ENTRYTYPE_MODE_OCCURRED 0x3000
+#define EPL_ERR_ENTRYTYPE_MODE_MASK     0x3000
+#define EPL_ERR_ENTRYTYPE_PROF_VENDOR   0x0001
+#define EPL_ERR_ENTRYTYPE_PROF_EPL      0x0002
+#define EPL_ERR_ENTRYTYPE_PROF_MASK     0x0FFF
+
+// defines for EPL version / PDO version
+#define EPL_VERSION_SUB             0x0F	// sub version
+#define EPL_VERSION_MAIN            0xF0	// main version
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+// $$$ d.k.: move this definition to global.h
+// byte-align structures
+#ifdef _MSC_VER
+#    pragma pack( push, packing )
+#    pragma pack( 1 )
+#    define PACK_STRUCT
+#elif defined( __GNUC__ )
+#    define PACK_STRUCT    __attribute__((packed))
+#else
+#    error you must byte-align these structures with the appropriate compiler directives
+#endif
+
+typedef struct {
+	// Offset 17
+	BYTE m_le_bRes1;	// reserved
+	// Offset 18
+	BYTE m_le_bFlag1;	// Flags: MC, PS
+	// Offset 19
+	BYTE m_le_bFlag2;	// Flags: res
+	// Offset 20
+	tEplNetTime m_le_NetTime;	// supported if D_NMT_NetTimeIsRealTime_BOOL is set
+	// Offset 28
+	QWORD m_le_RelativeTime;	// in us (supported if D_NMT_RelativeTime_BOOL is set)
+
+} PACK_STRUCT tEplSocFrame;
+
+typedef struct {
+	// Offset 17
+	BYTE m_le_bRes1;	// reserved
+	// Offset 18
+	BYTE m_le_bFlag1;	// Flags: MS, EA, RD
+	// Offset 19
+	BYTE m_le_bFlag2;	// Flags: res
+	// Offset 20
+	BYTE m_le_bPdoVersion;
+	// Offset 21
+	BYTE m_le_bRes2;	// reserved
+	// Offset 22
+	WORD m_le_wSize;
+	// Offset 24
+	BYTE m_le_abPayload[256 /*D_NMT_IsochrRxMaxPayload_U16 */ ];
+
+} PACK_STRUCT tEplPreqFrame;
+
+typedef struct {
+	// Offset 17
+	BYTE m_le_bNmtStatus;	// NMT state
+	// Offset 18
+	BYTE m_le_bFlag1;	// Flags: MS, EN, RD
+	// Offset 19
+	BYTE m_le_bFlag2;	// Flags: PR, RS
+	// Offset 20
+	BYTE m_le_bPdoVersion;
+	// Offset 21
+	BYTE m_le_bRes2;	// reserved
+	// Offset 22
+	WORD m_le_wSize;
+	// Offset 24
+	BYTE m_le_abPayload[256	/*D_NMT_IsochrRxMaxPayload_U16
+				   / D_NMT_IsochrTxMaxPayload_U16 */ ];
+
+} PACK_STRUCT tEplPresFrame;
+
+typedef struct {
+	// Offset 17
+	BYTE m_le_bNmtStatus;	// NMT state
+	// Offset 18
+	BYTE m_le_bFlag1;	// Flags: EA, ER
+	// Offset 19
+	BYTE m_le_bFlag2;	// Flags: res
+	// Offset 20
+	BYTE m_le_bReqServiceId;
+	// Offset 21
+	BYTE m_le_bReqServiceTarget;
+	// Offset 22
+	BYTE m_le_bEplVersion;
+
+} PACK_STRUCT tEplSoaFrame;
+
+typedef struct {
+	WORD m_wEntryType;
+	WORD m_wErrorCode;
+	tEplNetTime m_TimeStamp;
+	BYTE m_abAddInfo[8];
+
+} PACK_STRUCT tEplErrHistoryEntry;
+
+typedef struct {
+	// Offset 18
+	BYTE m_le_bFlag1;	// Flags: EN, EC
+	BYTE m_le_bFlag2;	// Flags: PR, RS
+	BYTE m_le_bNmtStatus;	// NMT state
+	BYTE m_le_bRes1[3];
+	QWORD m_le_qwStaticError;	// static error bit field
+	tEplErrHistoryEntry m_le_aErrHistoryEntry[14];
+
+} PACK_STRUCT tEplStatusResponse;
+
+typedef struct {
+	// Offset 18
+	BYTE m_le_bFlag1;	// Flags: res
+	BYTE m_le_bFlag2;	// Flags: PR, RS
+	BYTE m_le_bNmtStatus;	// NMT state
+	BYTE m_le_bIdentRespFlags;	// Flags: FW
+	BYTE m_le_bEplProfileVersion;
+	BYTE m_le_bRes1;
+	DWORD m_le_dwFeatureFlags;	// NMT_FeatureFlags_U32
+	WORD m_le_wMtu;		// NMT_CycleTiming_REC.AsyncMTU_U16: C_IP_MIN_MTU - C_IP_MAX_MTU
+	WORD m_le_wPollInSize;	// NMT_CycleTiming_REC.PReqActPayload_U16
+	WORD m_le_wPollOutSize;	// NMT_CycleTiming_REC.PResActPayload_U16
+	DWORD m_le_dwResponseTime;	// NMT_CycleTiming_REC.PResMaxLatency_U32
+	WORD m_le_wRes2;
+	DWORD m_le_dwDeviceType;	// NMT_DeviceType_U32
+	DWORD m_le_dwVendorId;	// NMT_IdentityObject_REC.VendorId_U32
+	DWORD m_le_dwProductCode;	// NMT_IdentityObject_REC.ProductCode_U32
+	DWORD m_le_dwRevisionNumber;	// NMT_IdentityObject_REC.RevisionNo_U32
+	DWORD m_le_dwSerialNumber;	// NMT_IdentityObject_REC.SerialNo_U32
+	QWORD m_le_qwVendorSpecificExt1;
+	DWORD m_le_dwVerifyConfigurationDate;	// CFM_VerifyConfiguration_REC.ConfDate_U32
+	DWORD m_le_dwVerifyConfigurationTime;	// CFM_VerifyConfiguration_REC.ConfTime_U32
+	DWORD m_le_dwApplicationSwDate;	// PDL_LocVerApplSw_REC.ApplSwDate_U32 on programmable device or date portion of NMT_ManufactSwVers_VS on non-programmable device
+	DWORD m_le_dwApplicationSwTime;	// PDL_LocVerApplSw_REC.ApplSwTime_U32 on programmable device or time portion of NMT_ManufactSwVers_VS on non-programmable device
+	DWORD m_le_dwIpAddress;
+	DWORD m_le_dwSubnetMask;
+	DWORD m_le_dwDefaultGateway;
+	BYTE m_le_sHostname[32];
+	BYTE m_le_abVendorSpecificExt2[48];
+
+} PACK_STRUCT tEplIdentResponse;
+
+typedef struct {
+	// Offset 18
+	BYTE m_le_bNmtCommandId;
+	BYTE m_le_bRes1;
+	BYTE m_le_abNmtCommandData[32];
+
+} PACK_STRUCT tEplNmtCommandService;
+
+typedef struct {
+	BYTE m_le_bReserved;
+	BYTE m_le_bTransactionId;
+	BYTE m_le_bFlags;
+	BYTE m_le_bCommandId;
+	WORD m_le_wSegmentSize;
+	WORD m_le_wReserved;
+	BYTE m_le_abCommandData[8];	// just reserve a minimum number of bytes as a placeholder
+
+} PACK_STRUCT tEplAsySdoCom;
+
+// asynchronous SDO Sequence Header
+typedef struct {
+	BYTE m_le_bRecSeqNumCon;
+	BYTE m_le_bSendSeqNumCon;
+	BYTE m_le_abReserved[2];
+	tEplAsySdoCom m_le_abSdoSeqPayload;
+
+} PACK_STRUCT tEplAsySdoSeq;
+
+typedef struct {
+	// Offset 18
+	BYTE m_le_bNmtCommandId;
+	BYTE m_le_bTargetNodeId;
+	BYTE m_le_abNmtCommandData[32];
+
+} PACK_STRUCT tEplNmtRequestService;
+
+typedef union {
+	// Offset 18
+	tEplStatusResponse m_StatusResponse;
+	tEplIdentResponse m_IdentResponse;
+	tEplNmtCommandService m_NmtCommandService;
+	tEplNmtRequestService m_NmtRequestService;
+	tEplAsySdoSeq m_SdoSequenceFrame;
+	BYTE m_le_abPayload[256	/*D_NMT_ASndTxMaxPayload_U16
+				   / D_NMT_ASndRxMaxPayload_U16 */ ];
+
+} tEplAsndPayload;
+
+typedef struct {
+	// Offset 17
+	BYTE m_le_bServiceId;
+	// Offset 18
+	tEplAsndPayload m_Payload;
+
+} PACK_STRUCT tEplAsndFrame;
+
+typedef union {
+	// Offset 17
+	tEplSocFrame m_Soc;
+	tEplPreqFrame m_Preq;
+	tEplPresFrame m_Pres;
+	tEplSoaFrame m_Soa;
+	tEplAsndFrame m_Asnd;
+
+} tEplFrameData;
+
+typedef struct {
+	// Offset 0
+	BYTE m_be_abDstMac[6];	// MAC address of the addressed nodes
+	// Offset 6
+	BYTE m_be_abSrcMac[6];	// MAC address of the transmitting node
+	// Offset 12
+	WORD m_be_wEtherType;	// Ethernet message type (big endian)
+	// Offset 14
+	BYTE m_le_bMessageType;	// EPL message type
+	// Offset 15
+	BYTE m_le_bDstNodeId;	// EPL node ID of the addressed nodes
+	// Offset 16
+	BYTE m_le_bSrcNodeId;	// EPL node ID of the transmitting node
+	// Offset 17
+	tEplFrameData m_Data;
+
+} PACK_STRUCT tEplFrame;
+
+// un-byte-align structures
+#ifdef _MSC_VER
+#    pragma pack( pop, packing )
+#endif
+
+typedef enum {
+	kEplMsgTypeNonEpl = 0x00,
+	kEplMsgTypeSoc = 0x01,
+	kEplMsgTypePreq = 0x03,
+	kEplMsgTypePres = 0x04,
+	kEplMsgTypeSoa = 0x05,
+	kEplMsgTypeAsnd = 0x06,
+
+} tEplMsgType;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_FRAME_H_
diff --git a/drivers/staging/epl/EplIdentu.c b/drivers/staging/epl/EplIdentu.c
new file mode 100644
index 000000000000..ce59ef09ccdf
--- /dev/null
+++ b/drivers/staging/epl/EplIdentu.c
@@ -0,0 +1,488 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for Identu-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplIdentu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/11/21 09:00:38 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/11/15 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplIdentu.h"
+#include "user/EplDlluCal.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <xxxxx>                                             */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	tEplIdentResponse *m_apIdentResponse[254];	// the IdentResponse are managed dynamically
+	tEplIdentuCbResponse m_apfnCbResponse[254];
+
+} tEplIdentuInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+static tEplIdentuInstance EplIdentuInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplIdentuCbIdentResponse(tEplFrameInfo * pFrameInfo_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuInit
+//
+// Description: init first instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplIdentuInit()
+{
+	tEplKernel Ret;
+
+	Ret = EplIdentuAddInstance();
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuAddInstance
+//
+// Description: init other instances of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplIdentuAddInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// reset instance structure
+	EPL_MEMSET(&EplIdentuInstance_g, 0, sizeof(EplIdentuInstance_g));
+
+	// register IdentResponse callback function
+	Ret =
+	    EplDlluCalRegAsndService(kEplDllAsndIdentResponse,
+				     EplIdentuCbIdentResponse,
+				     kEplDllAsndFilterAny);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuDelInstance
+//
+// Description: delete instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplIdentuDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// deregister IdentResponse callback function
+	Ret =
+	    EplDlluCalRegAsndService(kEplDllAsndIdentResponse, NULL,
+				     kEplDllAsndFilterNone);
+
+	Ret = EplIdentuReset();
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuReset
+//
+// Description: resets this instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplIdentuReset()
+{
+	tEplKernel Ret;
+	int iIndex;
+
+	Ret = kEplSuccessful;
+
+	for (iIndex = 0;
+	     iIndex < tabentries(EplIdentuInstance_g.m_apIdentResponse);
+	     iIndex++) {
+		if (EplIdentuInstance_g.m_apIdentResponse[iIndex] != NULL) {	// free memory
+			EPL_FREE(EplIdentuInstance_g.m_apIdentResponse[iIndex]);
+		}
+	}
+
+	EPL_MEMSET(&EplIdentuInstance_g, 0, sizeof(EplIdentuInstance_g));
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuGetIdentResponse
+//
+// Description: returns the IdentResponse for the specified node.
+//
+// Parameters:  uiNodeId_p                  = IN: node ID
+//              ppIdentResponse_p           = OUT: pointer to pointer of IdentResponse
+//                                            equals NULL, if no IdentResponse available
+//
+// Return:      tEplKernel                  = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplIdentuGetIdentResponse(unsigned int uiNodeId_p,
+					    tEplIdentResponse **
+					    ppIdentResponse_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// decrement node ID, because array is zero based
+	uiNodeId_p--;
+	if (uiNodeId_p < tabentries(EplIdentuInstance_g.m_apIdentResponse)) {
+		*ppIdentResponse_p =
+		    EplIdentuInstance_g.m_apIdentResponse[uiNodeId_p];
+	} else {		// invalid node ID specified
+		*ppIdentResponse_p = NULL;
+		Ret = kEplInvalidNodeId;
+	}
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuRequestIdentResponse
+//
+// Description: returns the IdentResponse for the specified node.
+//
+// Parameters:  uiNodeId_p                  = IN: node ID
+//              pfnCbResponse_p             = IN: function pointer to callback function
+//                                            which will be called if IdentResponse is received
+//
+// Return:      tEplKernel                  = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplIdentuRequestIdentResponse(unsigned int uiNodeId_p,
+						tEplIdentuCbResponse
+						pfnCbResponse_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// decrement node ID, because array is zero based
+	uiNodeId_p--;
+	if (uiNodeId_p < tabentries(EplIdentuInstance_g.m_apfnCbResponse)) {
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+		if (EplIdentuInstance_g.m_apfnCbResponse[uiNodeId_p] != NULL) {	// request already issued (maybe by someone else)
+			Ret = kEplInvalidOperation;
+		} else {
+			EplIdentuInstance_g.m_apfnCbResponse[uiNodeId_p] =
+			    pfnCbResponse_p;
+			Ret =
+			    EplDlluCalIssueRequest(kEplDllReqServiceIdent,
+						   (uiNodeId_p + 1), 0xFF);
+		}
+#else
+		Ret = kEplInvalidOperation;
+#endif
+	} else {		// invalid node ID specified
+		Ret = kEplInvalidNodeId;
+	}
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuGetRunningRequests
+//
+// Description: returns a bit field with the running requests for node-ID 1-32
+//              just for debugging purposes
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT DWORD PUBLIC EplIdentuGetRunningRequests(void)
+{
+	DWORD dwReqs = 0;
+	unsigned int uiIndex;
+
+	for (uiIndex = 0; uiIndex < 32; uiIndex++) {
+		if (EplIdentuInstance_g.m_apfnCbResponse[uiIndex] != NULL) {
+			dwReqs |= (1 << uiIndex);
+		}
+	}
+
+	return dwReqs;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuCbIdentResponse
+//
+// Description: callback funktion for IdentResponse
+//
+//
+//
+// Parameters:  pFrameInfo_p            = Frame with the IdentResponse
+//
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplIdentuCbIdentResponse(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiNodeId;
+	unsigned int uiIndex;
+	tEplIdentuCbResponse pfnCbResponse;
+
+	uiNodeId = AmiGetByteFromLe(&pFrameInfo_p->m_pFrame->m_le_bSrcNodeId);
+
+	uiIndex = uiNodeId - 1;
+
+	if (uiIndex < tabentries(EplIdentuInstance_g.m_apfnCbResponse)) {
+		// memorize pointer to callback function
+		pfnCbResponse = EplIdentuInstance_g.m_apfnCbResponse[uiIndex];
+		// reset callback function pointer so that caller may issue next request immediately
+		EplIdentuInstance_g.m_apfnCbResponse[uiIndex] = NULL;
+
+		if (pFrameInfo_p->m_uiFrameSize < EPL_C_DLL_MINSIZE_IDENTRES) {	// IdentResponse not received or it has invalid size
+			if (pfnCbResponse == NULL) {	// response was not requested
+				goto Exit;
+			}
+			Ret = pfnCbResponse(uiNodeId, NULL);
+		} else {	// IdentResponse received
+			if (EplIdentuInstance_g.m_apIdentResponse[uiIndex] == NULL) {	// memory for IdentResponse must be allocated
+				EplIdentuInstance_g.m_apIdentResponse[uiIndex] =
+				    EPL_MALLOC(sizeof(tEplIdentResponse));
+				if (EplIdentuInstance_g.m_apIdentResponse[uiIndex] == NULL) {	// malloc failed
+					if (pfnCbResponse == NULL) {	// response was not requested
+						goto Exit;
+					}
+					Ret =
+					    pfnCbResponse(uiNodeId,
+							  &pFrameInfo_p->
+							  m_pFrame->m_Data.
+							  m_Asnd.m_Payload.
+							  m_IdentResponse);
+					goto Exit;
+				}
+			}
+			// copy IdentResponse to instance structure
+			EPL_MEMCPY(EplIdentuInstance_g.
+				   m_apIdentResponse[uiIndex],
+				   &pFrameInfo_p->m_pFrame->m_Data.m_Asnd.
+				   m_Payload.m_IdentResponse,
+				   sizeof(tEplIdentResponse));
+			if (pfnCbResponse == NULL) {	// response was not requested
+				goto Exit;
+			}
+			Ret =
+			    pfnCbResponse(uiNodeId,
+					  EplIdentuInstance_g.
+					  m_apIdentResponse[uiIndex]);
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+// EOF
diff --git a/drivers/staging/epl/EplInc.h b/drivers/staging/epl/EplInc.h
new file mode 100644
index 000000000000..77f93d144166
--- /dev/null
+++ b/drivers/staging/epl/EplInc.h
@@ -0,0 +1,385 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  basic include file for internal EPL stack modules
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplInc.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/11/17 16:40:39 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_INC_H_
+#define _EPL_INC_H_
+
+// ============================================================================
+// include files
+// ============================================================================
+#if defined(WIN32) || defined(_WIN32)
+
+#ifdef UNDER_RTSS
+	// RTX header
+#include <windows.h>
+#include <process.h>
+#include <rtapi.h>
+
+#elif __BORLANDC__
+	// borland C header
+#include <windows.h>
+#include <process.h>
+
+#elif WINCE
+#include <windows.h>
+
+#else
+	// MSVC needs to include windows.h at first
+	// the following defines ar necessary for function prototypes for waitable timers
+#define _WIN32_WINDOWS 0x0401
+#define _WIN32_WINNT   0x0400
+#include <windows.h>
+#include <process.h>
+#endif
+
+#endif
+
+// defines for module integration
+// possible other include file needed
+// These constants defines modules which can be included in the Epl application.
+// Use this constants for define EPL_MODULE_INTEGRATION in file EplCfg.h.
+#define EPL_MODULE_OBDK        0x00000001L	// OBD kernel part module
+#define EPL_MODULE_PDOK        0x00000002L	// PDO kernel part module
+#define EPL_MODULE_NMT_MN      0x00000004L	// NMT MN module
+#define EPL_MODULE_SDOS        0x00000008L	// SDO Server module
+#define EPL_MODULE_SDOC        0x00000010L	// SDO Client module
+#define EPL_MODULE_SDO_ASND    0x00000020L	// SDO over Asnd module
+#define EPL_MODULE_SDO_UDP     0x00000040L	// SDO over UDP module
+#define EPL_MODULE_SDO_PDO     0x00000080L	// SDO in PDO module
+#define EPL_MODULE_NMT_CN      0x00000100L	// NMT CN module
+#define EPL_MODULE_NMTU        0x00000200L	// NMT user part module
+#define EPL_MODULE_NMTK        0x00000400L	// NMT kernel part module
+#define EPL_MODULE_DLLK        0x00000800L	// DLL kernel part module
+#define EPL_MODULE_DLLU        0x00001000L	// DLL user part module
+#define EPL_MODULE_OBDU        0x00002000L	// OBD user part module
+#define EPL_MODULE_CFGMA       0x00004000L	// Configuartioan Manager module
+#define EPL_MODULE_VETH        0x00008000L	// virtual ethernet driver module
+#define EPL_MODULE_PDOU        0x00010000L	// PDO user part module
+#define EPL_MODULE_LEDU        0x00020000L	// LED user part module
+
+#include "EplCfg.h"		// EPL configuration file (configuration from application)
+
+#include "global.h"		// global definitions
+
+#include "EplDef.h"		// EPL configuration file (default configuration)
+#include "EplInstDef.h"		// defines macros for instance types and table
+#include "Debug.h"		// debug definitions
+
+#include "EplErrDef.h"		// EPL error codes for API funtions
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+// IEEE 1588 conformant net time structure
+typedef struct {
+	DWORD m_dwSec;
+	DWORD m_dwNanoSec;
+
+} tEplNetTime;
+
+#include "EplTarget.h"		// target specific functions and definitions
+
+#include "EplAmi.h"
+
+// -------------------------------------------------------------------------
+// macros
+// -------------------------------------------------------------------------
+
+#define EPL_SPEC_VERSION                    0x20	// ETHERNET Powerlink V. 2.0
+#define EPL_STACK_VERSION(ver,rev,rel)      ((((DWORD)(ver)) & 0xFF)|((((DWORD)(rev))&0xFF)<<8)|(((DWORD)(rel))<<16))
+#define EPL_OBJ1018_VERSION(ver,rev,rel)    ((((DWORD)(ver))<<16) |(((DWORD)(rev))&0xFFFF))
+#define EPL_STRING_VERSION(ver,rev,rel)     "V" #ver "." #rev " r" #rel
+
+#include "EplVersion.h"
+
+// defines for EPL FeatureFlags
+#define EPL_FEATURE_ISOCHR          0x00000001
+#define EPL_FEATURE_SDO_UDP         0x00000002
+#define EPL_FEATURE_SDO_ASND        0x00000004
+#define EPL_FEATURE_SDO_PDO         0x00000008
+#define EPL_FEATURE_NMT_INFO        0x00000010
+#define EPL_FEATURE_NMT_EXT         0x00000020
+#define EPL_FEATURE_PDO_DYN         0x00000040
+#define EPL_FEATURE_NMT_UDP         0x00000080
+#define EPL_FEATURE_CFGMA           0x00000100
+#define EPL_FEATURE_DLL_MULTIPLEX   0x00000200
+#define EPL_FEATURE_NODEID_SW       0x00000400
+#define EPL_FEATURE_NMT_BASICETH    0x00000800
+#define EPL_FEATURE_RT1             0x00001000
+#define EPL_FEATURE_RT2             0x00002000
+
+// generate EPL NMT_FeatureFlags_U32
+#ifndef EPL_DEF_FEATURE_ISOCHR
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+#define EPL_DEF_FEATURE_ISOCHR          (EPL_FEATURE_ISOCHR)
+#else
+#define EPL_DEF_FEATURE_ISOCHR          0
+#endif
+#endif
+
+#ifndef EPL_DEF_FEATURE_SDO_ASND
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+#define EPL_DEF_FEATURE_SDO_ASND        (EPL_FEATURE_SDO_ASND)
+#else
+#define EPL_DEF_FEATURE_SDO_ASND        0
+#endif
+#endif
+
+#ifndef EPL_DEF_FEATURE_SDO_UDP
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+#define EPL_DEF_FEATURE_SDO_UDP         (EPL_FEATURE_SDO_UDP)
+#else
+#define EPL_DEF_FEATURE_SDO_UDP         0
+#endif
+#endif
+
+#ifndef EPL_DEF_FEATURE_SDO_PDO
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_PDO)) != 0)
+#define EPL_DEF_FEATURE_SDO_PDO         (EPL_FEATURE_SDO_PDO)
+#else
+#define EPL_DEF_FEATURE_SDO_PDO         0
+#endif
+#endif
+
+#ifndef EPL_DEF_FEATURE_PDO_DYN
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+#define EPL_DEF_FEATURE_PDO_DYN         (EPL_FEATURE_PDO_DYN)
+#else
+#define EPL_DEF_FEATURE_PDO_DYN         0
+#endif
+#endif
+
+#ifndef EPL_DEF_FEATURE_CFGMA
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_CFGMA)) != 0)
+#define EPL_DEF_FEATURE_CFGMA           (EPL_FEATURE_CFGMA)
+#else
+#define EPL_DEF_FEATURE_CFGMA           0
+#endif
+#endif
+
+#define EPL_DEF_FEATURE_FLAGS                   (EPL_DEF_FEATURE_ISOCHR \
+                                                | EPL_DEF_FEATURE_SDO_ASND \
+                                                | EPL_DEF_FEATURE_SDO_UDP \
+                                                | EPL_DEF_FEATURE_SDO_PDO \
+                                                | EPL_DEF_FEATURE_PDO_DYN \
+                                                | EPL_DEF_FEATURE_CFGMA)
+
+#ifndef tabentries
+#define tabentries(a)   (sizeof(a)/sizeof(*(a)))
+#endif
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// definitions for DLL export
+#if ((DEV_SYSTEM == _DEV_WIN32_) || (DEV_SYSTEM == _DEV_WIN_CE_)) && defined (COP_LIB)
+
+#define EPLDLLEXPORT    __declspec (dllexport)
+
+#else
+
+#define EPLDLLEXPORT
+
+#endif
+
+// ============================================================================
+// common debug macros
+// ============================================================================
+// for using macro DEBUG_TRACEx()
+//
+// Example:
+//      DEBUG_TRACE1 (EPL_DBGLVL_OBD, "Value is %d\n" , wObjectIndex);
+//
+// This message only will be printed if:
+//      - NDEBUG is not defined     AND !!!
+//      - flag 0x00000004L is set in DEF_DEBUG_LVL (can be defined in copcfg.h)
+//
+// default level is defined in copdef.h
+
+// debug-level and TRACE-macros         // standard-level   // flags for DEF_DEBUG_LVL
+#define EPL_DBGLVL_EDRV                 DEBUG_LVL_01	// 0x00000001L
+#define EPL_DBGLVL_EDRV_TRACE0          DEBUG_LVL_01_TRACE0
+#define EPL_DBGLVL_EDRV_TRACE1          DEBUG_LVL_01_TRACE1
+#define EPL_DBGLVL_EDRV_TRACE2          DEBUG_LVL_01_TRACE2
+#define EPL_DBGLVL_EDRV_TRACE3          DEBUG_LVL_01_TRACE3
+#define EPL_DBGLVL_EDRV_TRACE4          DEBUG_LVL_01_TRACE4
+
+#define EPL_DBGLVL_DLL                  DEBUG_LVL_02	// 0x00000002L
+#define EPL_DBGLVL_DLL_TRACE0           DEBUG_LVL_02_TRACE0
+#define EPL_DBGLVL_DLL_TRACE1           DEBUG_LVL_02_TRACE1
+#define EPL_DBGLVL_DLL_TRACE2           DEBUG_LVL_02_TRACE2
+#define EPL_DBGLVL_DLL_TRACE3           DEBUG_LVL_02_TRACE3
+#define EPL_DBGLVL_DLL_TRACE4           DEBUG_LVL_02_TRACE4
+
+#define EPL_DBGLVL_OBD                  DEBUG_LVL_03	// 0x00000004L
+#define EPL_DBGLVL_OBD_TRACE0           DEBUG_LVL_03_TRACE0
+#define EPL_DBGLVL_OBD_TRACE1           DEBUG_LVL_03_TRACE1
+#define EPL_DBGLVL_OBD_TRACE2           DEBUG_LVL_03_TRACE2
+#define EPL_DBGLVL_OBD_TRACE3           DEBUG_LVL_03_TRACE3
+#define EPL_DBGLVL_OBD_TRACE4           DEBUG_LVL_03_TRACE4
+
+#define EPL_DBGLVL_NMTK                 DEBUG_LVL_04	// 0x00000008L
+#define EPL_DBGLVL_NMTK_TRACE0          DEBUG_LVL_04_TRACE0
+#define EPL_DBGLVL_NMTK_TRACE1          DEBUG_LVL_04_TRACE1
+#define EPL_DBGLVL_NMTK_TRACE2          DEBUG_LVL_04_TRACE2
+#define EPL_DBGLVL_NMTK_TRACE3          DEBUG_LVL_04_TRACE3
+#define EPL_DBGLVL_NMTK_TRACE4          DEBUG_LVL_04_TRACE4
+
+#define EPL_DBGLVL_NMTCN                DEBUG_LVL_05	// 0x00000010L
+#define EPL_DBGLVL_NMTCN_TRACE0         DEBUG_LVL_05_TRACE0
+#define EPL_DBGLVL_NMTCN_TRACE1         DEBUG_LVL_05_TRACE1
+#define EPL_DBGLVL_NMTCN_TRACE2         DEBUG_LVL_05_TRACE2
+#define EPL_DBGLVL_NMTCN_TRACE3         DEBUG_LVL_05_TRACE3
+#define EPL_DBGLVL_NMTCN_TRACE4         DEBUG_LVL_05_TRACE4
+
+#define EPL_DBGLVL_NMTU                 DEBUG_LVL_06	// 0x00000020L
+#define EPL_DBGLVL_NMTU_TRACE0          DEBUG_LVL_06_TRACE0
+#define EPL_DBGLVL_NMTU_TRACE1          DEBUG_LVL_06_TRACE1
+#define EPL_DBGLVL_NMTU_TRACE2          DEBUG_LVL_06_TRACE2
+#define EPL_DBGLVL_NMTU_TRACE3          DEBUG_LVL_06_TRACE3
+#define EPL_DBGLVL_NMTU_TRACE4          DEBUG_LVL_06_TRACE4
+
+#define EPL_DBGLVL_NMTMN                DEBUG_LVL_07	// 0x00000040L
+#define EPL_DBGLVL_NMTMN_TRACE0         DEBUG_LVL_07_TRACE0
+#define EPL_DBGLVL_NMTMN_TRACE1         DEBUG_LVL_07_TRACE1
+#define EPL_DBGLVL_NMTMN_TRACE2         DEBUG_LVL_07_TRACE2
+#define EPL_DBGLVL_NMTMN_TRACE3         DEBUG_LVL_07_TRACE3
+#define EPL_DBGLVL_NMTMN_TRACE4         DEBUG_LVL_07_TRACE4
+
+//...
+
+#define EPL_DBGLVL_SDO                  DEBUG_LVL_25	// 0x01000000
+#define EPL_DBGLVL_SDO_TRACE0           DEBUG_LVL_25_TRACE0
+#define EPL_DBGLVL_SDO_TRACE1           DEBUG_LVL_25_TRACE1
+#define EPL_DBGLVL_SDO_TRACE2           DEBUG_LVL_25_TRACE2
+#define EPL_DBGLVL_SDO_TRACE3           DEBUG_LVL_25_TRACE3
+#define EPL_DBGLVL_SDO_TRACE4           DEBUG_LVL_25_TRACE4
+
+#define EPL_DBGLVL_VETH                 DEBUG_LVL_26	// 0x02000000
+#define EPL_DBGLVL_VETH_TRACE0          DEBUG_LVL_26_TRACE0
+#define EPL_DBGLVL_VETH_TRACE1          DEBUG_LVL_26_TRACE1
+#define EPL_DBGLVL_VETH_TRACE2          DEBUG_LVL_26_TRACE2
+#define EPL_DBGLVL_VETH_TRACE3          DEBUG_LVL_26_TRACE3
+#define EPL_DBGLVL_VETH_TRACE4          DEBUG_LVL_26_TRACE4
+
+#define EPL_DBGLVL_EVENTK               DEBUG_LVL_27	// 0x04000000
+#define EPL_DBGLVL_EVENTK_TRACE0        DEBUG_LVL_27_TRACE0
+#define EPL_DBGLVL_EVENTK_TRACE1        DEBUG_LVL_27_TRACE1
+#define EPL_DBGLVL_EVENTK_TRACE2        DEBUG_LVL_27_TRACE2
+#define EPL_DBGLVL_EVENTK_TRACE3        DEBUG_LVL_27_TRACE3
+#define EPL_DBGLVL_EVENTK_TRACE4        DEBUG_LVL_27_TRACE4
+
+#define EPL_DBGLVL_EVENTU               DEBUG_LVL_28	// 0x08000000
+#define EPL_DBGLVL_EVENTU_TRACE0        DEBUG_LVL_28_TRACE0
+#define EPL_DBGLVL_EVENTU_TRACE1        DEBUG_LVL_28_TRACE1
+#define EPL_DBGLVL_EVENTU_TRACE2        DEBUG_LVL_28_TRACE2
+#define EPL_DBGLVL_EVENTU_TRACE3        DEBUG_LVL_28_TRACE3
+#define EPL_DBGLVL_EVENTU_TRACE4        DEBUG_LVL_28_TRACE4
+
+// SharedBuff
+#define EPL_DBGLVL_SHB                  DEBUG_LVL_29	// 0x10000000
+#define EPL_DBGLVL_SHB_TRACE0           DEBUG_LVL_29_TRACE0
+#define EPL_DBGLVL_SHB_TRACE1           DEBUG_LVL_29_TRACE1
+#define EPL_DBGLVL_SHB_TRACE2           DEBUG_LVL_29_TRACE2
+#define EPL_DBGLVL_SHB_TRACE3           DEBUG_LVL_29_TRACE3
+#define EPL_DBGLVL_SHB_TRACE4           DEBUG_LVL_29_TRACE4
+
+#define EPL_DBGLVL_ASSERT               DEBUG_LVL_ASSERT	// 0x20000000L
+#define EPL_DBGLVL_ASSERT_TRACE0        DEBUG_LVL_ASSERT_TRACE0
+#define EPL_DBGLVL_ASSERT_TRACE1        DEBUG_LVL_ASSERT_TRACE1
+#define EPL_DBGLVL_ASSERT_TRACE2        DEBUG_LVL_ASSERT_TRACE2
+#define EPL_DBGLVL_ASSERT_TRACE3        DEBUG_LVL_ASSERT_TRACE3
+#define EPL_DBGLVL_ASSERT_TRACE4        DEBUG_LVL_ASSERT_TRACE4
+
+#define EPL_DBGLVL_ERROR                DEBUG_LVL_ERROR	// 0x40000000L
+#define EPL_DBGLVL_ERROR_TRACE0         DEBUG_LVL_ERROR_TRACE0
+#define EPL_DBGLVL_ERROR_TRACE1         DEBUG_LVL_ERROR_TRACE1
+#define EPL_DBGLVL_ERROR_TRACE2         DEBUG_LVL_ERROR_TRACE2
+#define EPL_DBGLVL_ERROR_TRACE3         DEBUG_LVL_ERROR_TRACE3
+#define EPL_DBGLVL_ERROR_TRACE4         DEBUG_LVL_ERROR_TRACE4
+
+#define EPL_DBGLVL_ALWAYS               DEBUG_LVL_ALWAYS	// 0x80000000L
+#define EPL_DBGLVL_ALWAYS_TRACE0        DEBUG_LVL_ALWAYS_TRACE0
+#define EPL_DBGLVL_ALWAYS_TRACE1        DEBUG_LVL_ALWAYS_TRACE1
+#define EPL_DBGLVL_ALWAYS_TRACE2        DEBUG_LVL_ALWAYS_TRACE2
+#define EPL_DBGLVL_ALWAYS_TRACE3        DEBUG_LVL_ALWAYS_TRACE3
+#define EPL_DBGLVL_ALWAYS_TRACE4        DEBUG_LVL_ALWAYS_TRACE4
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_INC_H_
diff --git a/drivers/staging/epl/EplInstDef.h b/drivers/staging/epl/EplInstDef.h
new file mode 100644
index 000000000000..89efbf278264
--- /dev/null
+++ b/drivers/staging/epl/EplInstDef.h
@@ -0,0 +1,377 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  definitions for generating instances
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplInstDef.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    ...
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  r.d.: first implementation
+
+****************************************************************************/
+
+#ifndef _EPLINSTDEF_H_
+#define _EPLINSTDEF_H_
+
+// =========================================================================
+// types and macros for generating instances
+// =========================================================================
+
+typedef enum {
+	kStateUnused = 0,
+	kStateDeleted = 1,
+	kStateUsed = 0xFF
+} tInstState;
+
+//------------------------------------------------------------------------------------------
+
+typedef void MEM *tEplPtrInstance;
+typedef BYTE tEplInstanceHdl;
+
+// define const for illegale values
+#define CCM_ILLINSTANCE      NULL
+#define CCM_ILLINSTANCE_HDL  0xFF
+
+//------------------------------------------------------------------------------------------
+// if more than one instance then use this macros
+#if (EPL_MAX_INSTANCES > 1)
+
+    //--------------------------------------------------------------------------------------
+    // macro definition for instance table definition
+    //--------------------------------------------------------------------------------------
+
+    // memory attributes for instance table
+#define INST_NEAR		// faster access to variables
+#define INST_FAR		// variables wich have to located in xdata
+#define STATIC			// prevent warnings for variables with same name
+
+#define INSTANCE_TYPE_BEGIN     typedef struct {
+#define INSTANCE_TYPE_END       } tEplInstanceInfo;
+
+    //--------------------------------------------------------------------------------------
+    // macro definition for API interface
+    //--------------------------------------------------------------------------------------
+
+    // declaration:
+
+    // macros for declaration within function header or prototype of API functions
+#define CCM_DECL_INSTANCE_HDL                   tEplInstanceHdl InstanceHandle
+#define CCM_DECL_INSTANCE_HDL_                  tEplInstanceHdl InstanceHandle,
+
+    // macros for declaration of pointer to instance handle within function header or prototype of API functions
+#define CCM_DECL_PTR_INSTANCE_HDL               tEplInstanceHdl MEM* pInstanceHandle
+#define CCM_DECL_PTR_INSTANCE_HDL_              tEplInstanceHdl MEM* pInstanceHandle,
+
+    // macros for declaration instance as lokacl variable within functions
+#define CCM_DECL_INSTANCE_PTR_LOCAL             tCcmInstanceInfo MEM* pInstance;
+#define CCM_DECL_PTR_INSTANCE_HDL_LOCAL         tEplInstanceHdl  MEM* pInstanceHandle;
+
+    // reference:
+
+    // macros for reference of instance handle for function parameters
+#define CCM_INSTANCE_HDL                        InstanceHandle
+#define CCM_INSTANCE_HDL_                       InstanceHandle,
+
+    // macros for reference of instance parameter for function parameters
+#define CCM_INSTANCE_PARAM(par)                 par
+#define CCM_INSTANCE_PARAM_(par)                par,
+
+    // macros for reference of instance parameter for writing or reading values
+#define CCM_INST_ENTRY                          (*((tEplPtrInstance)pInstance))
+
+    // processing:
+
+    // macros for process instance handle
+#define CCM_CHECK_INSTANCE_HDL()                if (InstanceHandle >= EPL_MAX_INSTANCES) \
+                                                        {return (kEplIllegalInstance);}
+
+    // macros for process pointer to instance handle
+#define CCM_CHECK_PTR_INSTANCE_HDL()            if (pInstanceHandle == NULL) \
+                                                        {return (kEplInvalidInstanceParam);}
+
+    // This macro returned the handle and pointer to next free instance.
+#define CCM_GET_FREE_INSTANCE_AND_HDL()         pInstance = CcmGetFreeInstanceAndHandle (pInstanceHandle); \
+                                                    ASSERT (*pInstanceHandle != CCM_ILLINSTANCE_HDL);
+
+#define CCM_CHECK_INSTANCE_PTR()                if (pInstance == CCM_ILLINSTANCE) \
+                                                        {return (kEplNoFreeInstance);}
+
+#define CCM_GET_INSTANCE_PTR()                  pInstance = CcmGetInstancePtr (InstanceHandle);
+#define CCM_GET_FREE_INSTANCE_PTR()             pInstance = GetFreeInstance (); \
+                                                    ASSERT (pInstance != CCM_ILLINSTANCE);
+
+    //--------------------------------------------------------------------------------------
+    // macro definition for stack interface
+    //--------------------------------------------------------------------------------------
+
+    // macros for declaration within the function header, prototype or local var list
+    // Declaration of pointers within function paramater list must defined as void MEM*
+    // pointer.
+#define EPL_MCO_DECL_INSTANCE_PTR                   void MEM* pInstance
+#define EPL_MCO_DECL_INSTANCE_PTR_                  void MEM* pInstance,
+#define EPL_MCO_DECL_INSTANCE_PTR_LOCAL             tEplPtrInstance  pInstance;
+
+    // macros for reference of pointer to instance
+    // These macros are used for parameter passing to called function.
+#define EPL_MCO_INSTANCE_PTR                        pInstance
+#define EPL_MCO_INSTANCE_PTR_                       pInstance,
+#define EPL_MCO_ADDR_INSTANCE_PTR_                  &pInstance,
+
+    // macro for access of struct members of one instance
+    // An access to a member of instance table must be casted by the local
+    // defined type of instance table.
+#define EPL_MCO_INST_ENTRY                          (*(tEplPtrInstance)pInstance)
+#define EPL_MCO_GLB_VAR(var)                        (((tEplPtrInstance)pInstance)->var)
+
+    // macros for process pointer to instance
+#define EPL_MCO_GET_INSTANCE_PTR()                  pInstance = (tEplPtrInstance) GetInstancePtr (InstanceHandle);
+#define EPL_MCO_GET_FREE_INSTANCE_PTR()             pInstance = (tEplPtrInstance) GetFreeInstance (); \
+                                                    ASSERT (pInstance != CCM_ILLINSTANCE);
+
+    // This macro should be used to check the passed pointer to an public function
+#define EPL_MCO_CHECK_INSTANCE_STATE()              ASSERT (pInstance != NULL); \
+                                                    ASSERT (((tEplPtrInstance)pInstance)->m_InstState == kStateUsed);
+
+    // macros for declaration of pointer to instance pointer
+#define EPL_MCO_DECL_PTR_INSTANCE_PTR               void MEM*  MEM* pInstancePtr
+#define EPL_MCO_DECL_PTR_INSTANCE_PTR_              void MEM*  MEM* pInstancePtr,
+
+    // macros for reference of pointer to instance pointer
+    // These macros are used for parameter passing to called function.
+#define EPL_MCO_PTR_INSTANCE_PTR                    pInstancePtr
+#define EPL_MCO_PTR_INSTANCE_PTR_                   pInstancePtr,
+
+    // macros for process pointer to instance pointer
+#define EPL_MCO_CHECK_PTR_INSTANCE_PTR()            ASSERT (pInstancePtr != NULL);
+#define EPL_MCO_SET_PTR_INSTANCE_PTR()              (*pInstancePtr = pInstance);
+
+#define EPL_MCO_INSTANCE_PARAM(a)                   (a)
+#define EPL_MCO_INSTANCE_PARAM_(a)                  (a),
+#define EPL_MCO_INSTANCE_PARAM_IDX_()               EPL_MCO_INSTANCE_PARAM_ (EPL_MCO_GLB_VAR (m_bInstIndex))
+#define EPL_MCO_INSTANCE_PARAM_IDX()                EPL_MCO_INSTANCE_PARAM (EPL_MCO_GLB_VAR (m_bInstIndex))
+#define EPL_MCO_WRITE_INSTANCE_STATE(a)             EPL_MCO_GLB_VAR (m_InstState) = a;
+
+    // this macro deletes all instance entries as unused
+#define EPL_MCO_DELETE_INSTANCE_TABLE()                                    \
+    {                                                                      \
+        tEplInstanceInfo MEM*   pInstance       = &aEplInstanceTable_g[0]; \
+        tFastByte               InstNumber      = 0;                       \
+        tFastByte               i               = EPL_MAX_INSTANCES;       \
+        do {                                                               \
+            pInstance->m_InstState = (BYTE) kStateUnused;                  \
+            pInstance->m_bInstIndex = (BYTE) InstNumber;                   \
+            pInstance++; InstNumber++; i--;                                \
+        } while (i != 0);                                                  \
+    }
+
+    // definition of functions which has to be defined in each module of CANopen stack
+#define EPL_MCO_DEFINE_INSTANCE_FCT() \
+        static tEplPtrInstance GetInstancePtr (tEplInstanceHdl InstHandle_p);  \
+        static tEplPtrInstance GetFreeInstance (void);
+#define EPL_MCO_DECL_INSTANCE_FCT()                                            \
+        static tEplPtrInstance GetInstancePtr (tEplInstanceHdl InstHandle_p) { \
+            return &aEplInstanceTable_g[InstHandle_p]; }                       \
+        static tEplPtrInstance GetFreeInstance (void) {                        \
+            tEplInstanceInfo MEM*   pInstance   = &aEplInstanceTable_g[0];     \
+            tFastByte               i           = EPL_MAX_INSTANCES;           \
+            do { if (pInstance->m_InstState != kStateUsed) {                   \
+                    return (tEplPtrInstance) pInstance; }                      \
+                pInstance++; i--; }                                            \
+            while (i != 0);                                                    \
+            return CCM_ILLINSTANCE; }
+
+    // this macro defines the instance table. Each entry is reserved for an instance of CANopen.
+#define EPL_MCO_DECL_INSTANCE_VAR() \
+        static tEplInstanceInfo MEM aEplInstanceTable_g [EPL_MAX_INSTANCES];
+
+    // this macro defines member variables in instance table which are needed in
+    // all modules of Epl stack
+#define EPL_MCO_DECL_INSTANCE_MEMBER() \
+        STATIC  BYTE                            m_InstState; \
+        STATIC  BYTE                            m_bInstIndex;
+
+#define EPL_MCO_INSTANCE_PARAM_IDX_()           EPL_MCO_INSTANCE_PARAM_ (EPL_MCO_GLB_VAR (m_bInstIndex))
+#define EPL_MCO_INSTANCE_PARAM_IDX()            EPL_MCO_INSTANCE_PARAM (EPL_MCO_GLB_VAR (m_bInstIndex))
+
+#else // only one instance is used
+
+    // Memory attributes for instance table.
+#define INST_NEAR   NEAR	// faster access to variables
+#define INST_FAR    MEM		// variables wich have to located in xdata
+#define STATIC      static	// prevent warnings for variables with same name
+
+#define INSTANCE_TYPE_BEGIN
+#define INSTANCE_TYPE_END
+
+// macros for declaration, initializing and member access for instance handle
+// This class of macros are used by API function to inform CCM-modul which
+// instance is to be used.
+
+    // macros for reference of instance handle
+    // These macros are used for parameter passing to CANopen API function.
+#define CCM_INSTANCE_HDL
+#define CCM_INSTANCE_HDL_
+
+#define CCM_DECL_INSTANCE_PTR_LOCAL
+
+    // macros for declaration within the function header or prototype
+#define CCM_DECL_INSTANCE_HDL                   void
+#define CCM_DECL_INSTANCE_HDL_
+
+    // macros for process instance handle
+#define CCM_CHECK_INSTANCE_HDL()
+
+    // macros for declaration of pointer to instance handle
+#define CCM_DECL_PTR_INSTANCE_HDL               void
+#define CCM_DECL_PTR_INSTANCE_HDL_
+
+    // macros for process pointer to instance handle
+#define CCM_CHECK_PTR_INSTANCE_HDL()
+
+    // This macro returned the handle and pointer to next free instance.
+#define CCM_GET_FREE_INSTANCE_AND_HDL()
+
+#define CCM_CHECK_INSTANCE_PTR()
+
+#define CCM_GET_INSTANCE_PTR()
+#define CCM_GET_FREE_INSTANCE_PTR()
+
+#define CCM_INSTANCE_PARAM(par)
+#define CCM_INSTANCE_PARAM_(par)
+
+#define CCM_INST_ENTRY                          aCcmInstanceTable_g[0]
+
+// macros for declaration, initializing and member access for instance pointer
+// This class of macros are used by CANopen internal function to point to one instance.
+
+    // macros for declaration within the function header, prototype or local var list
+#define EPL_MCO_DECL_INSTANCE_PTR                   void
+#define EPL_MCO_DECL_INSTANCE_PTR_
+#define EPL_MCO_DECL_INSTANCE_PTR_LOCAL
+
+    // macros for reference of pointer to instance
+    // These macros are used for parameter passing to called function.
+#define EPL_MCO_INSTANCE_PTR
+#define EPL_MCO_INSTANCE_PTR_
+#define EPL_MCO_ADDR_INSTANCE_PTR_
+
+    // macros for process pointer to instance
+#define EPL_MCO_GET_INSTANCE_PTR()
+#define EPL_MCO_GET_FREE_INSTANCE_PTR()
+
+    // This macro should be used to check the passed pointer to an public function
+#define EPL_MCO_CHECK_INSTANCE_STATE()
+
+    // macros for declaration of pointer to instance pointer
+#define EPL_MCO_DECL_PTR_INSTANCE_PTR               void
+#define EPL_MCO_DECL_PTR_INSTANCE_PTR_
+
+    // macros for reference of pointer to instance pointer
+    // These macros are used for parameter passing to called function.
+#define EPL_MCO_PTR_INSTANCE_PTR
+#define EPL_MCO_PTR_INSTANCE_PTR_
+
+    // macros for process pointer to instance pointer
+#define EPL_MCO_CHECK_PTR_INSTANCE_PTR()
+#define EPL_MCO_SET_PTR_INSTANCE_PTR()
+
+#define EPL_MCO_INSTANCE_PARAM(a)
+#define EPL_MCO_INSTANCE_PARAM_(a)
+#define EPL_MCO_INSTANCE_PARAM_IDX_()
+#define EPL_MCO_INSTANCE_PARAM_IDX()
+
+    // macro for access of struct members of one instance
+#define EPL_MCO_INST_ENTRY                          aEplInstanceTable_g[0]
+#define EPL_MCO_GLB_VAR(var)                        (var)
+#define EPL_MCO_WRITE_INSTANCE_STATE(a)
+
+    // this macro deletes all instance entries as unused
+#define EPL_MCO_DELETE_INSTANCE_TABLE()
+
+    // definition of functions which has to be defined in each module of CANopen stack
+#define EPL_MCO_DEFINE_INSTANCE_FCT()
+#define EPL_MCO_DECL_INSTANCE_FCT()
+
+    // this macro defines the instance table. Each entry is reserved for an instance of CANopen.
+#define EPL_MCO_DECL_INSTANCE_VAR()
+
+    // this macro defines member variables in instance table which are needed in
+    // all modules of CANopen stack
+#define EPL_MCO_DECL_INSTANCE_MEMBER()
+
+#endif
+
+/*
+#if (CDRV_MAX_INSTANCES > 1)
+
+    #define CDRV_REENTRANT                          REENTRANT
+
+#else
+
+    #define CDRV_REENTRANT
+
+#endif
+*/
+
+#endif // _EPLINSTDEF_H_
+
+// Die letzte Zeile muß unbedingt eine leere Zeile sein, weil manche Compiler
+// damit ein Problem haben, wenn das nicht so ist (z.B. GNU oder Borland C++ Builder).
diff --git a/drivers/staging/epl/EplLed.h b/drivers/staging/epl/EplLed.h
new file mode 100644
index 000000000000..6a29aed303a2
--- /dev/null
+++ b/drivers/staging/epl/EplLed.h
@@ -0,0 +1,92 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for status and error LED
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplLed.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.1 $  $Date: 2008/11/17 16:40:39 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2008/11/17 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#ifndef _EPLLED_H_
+#define _EPLLED_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef enum {
+	kEplLedTypeStatus = 0x00,
+	kEplLedTypeError = 0x01,
+
+} tEplLedType;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPLLED_H_
diff --git a/drivers/staging/epl/EplNmt.h b/drivers/staging/epl/EplNmt.h
new file mode 100644
index 000000000000..4c11e5bc74aa
--- /dev/null
+++ b/drivers/staging/epl/EplNmt.h
@@ -0,0 +1,230 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  global include file for EPL-NMT-Modules
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmt.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/11/17 16:40:39 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#ifndef _EPLNMT_H_
+#define _EPLNMT_H_
+
+#include "EplInc.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// define super-states and masks to identify a super-state
+#define EPL_NMT_GS_POWERED          0x0008	// super state
+#define EPL_NMT_GS_INITIALISATION   0x0009	// super state
+#define EPL_NMT_GS_COMMUNICATING    0x000C	// super state
+#define EPL_NMT_CS_EPLMODE          0x000D	// super state
+#define EPL_NMT_MS_EPLMODE          0x000D	// super state
+
+#define EPL_NMT_SUPERSTATE_MASK     0x000F	// mask to select state
+
+#define EPL_NMT_TYPE_UNDEFINED      0x0000	// type of NMT state is still undefined
+#define EPL_NMT_TYPE_CS             0x0100	// CS type of NMT state
+#define EPL_NMT_TYPE_MS             0x0200	// MS type of NMT state
+#define EPL_NMT_TYPE_MASK           0x0300	// mask to select type of NMT state (i.e. CS or MS)
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+// the lower Byte of the NMT-State is encoded
+// like the values in the EPL-Standard
+// the higher byte is used to encode MN
+// (Bit 1 of the higher byte = 1) or CN (Bit 0 of the
+// higher byte  = 1)
+// the super-states are not mentioned in this
+// enum because they are no real states
+// --> there are masks defined to indentify the
+// super-states
+
+typedef enum {
+	kEplNmtGsOff = 0x0000,
+	kEplNmtGsInitialising = 0x0019,
+	kEplNmtGsResetApplication = 0x0029,
+	kEplNmtGsResetCommunication = 0x0039,
+	kEplNmtGsResetConfiguration = 0x0079,
+	kEplNmtCsNotActive = 0x011C,
+	kEplNmtCsPreOperational1 = 0x011D,
+	kEplNmtCsStopped = 0x014D,
+	kEplNmtCsPreOperational2 = 0x015D,
+	kEplNmtCsReadyToOperate = 0x016D,
+	kEplNmtCsOperational = 0x01FD,
+	kEplNmtCsBasicEthernet = 0x011E,
+	kEplNmtMsNotActive = 0x021C,
+	kEplNmtMsPreOperational1 = 0x021D,
+	kEplNmtMsPreOperational2 = 0x025D,
+	kEplNmtMsReadyToOperate = 0x026D,
+	kEplNmtMsOperational = 0x02FD,
+	kEplNmtMsBasicEthernet = 0x021E
+} tEplNmtState;
+
+// NMT-events
+typedef enum {
+	// Events from DLL
+	// Events defined by EPL V2 specification
+	kEplNmtEventNoEvent = 0x00,
+//    kEplNmtEventDllMePres           =   0x01,
+	kEplNmtEventDllMePresTimeout = 0x02,
+//    kEplNmtEventDllMeAsnd           =   0x03,
+//    kEplNmtEventDllMeAsndTimeout    =   0x04,
+	kEplNmtEventDllMeSoaSent = 0x04,
+	kEplNmtEventDllMeSocTrig = 0x05,
+	kEplNmtEventDllMeSoaTrig = 0x06,
+	kEplNmtEventDllCeSoc = 0x07,
+	kEplNmtEventDllCePreq = 0x08,
+	kEplNmtEventDllCePres = 0x09,
+	kEplNmtEventDllCeSoa = 0x0A,
+	kEplNmtEventDllCeAsnd = 0x0B,
+	kEplNmtEventDllCeFrameTimeout = 0x0C,
+
+	// Events triggered by NMT-Commands
+	kEplNmtEventSwReset = 0x10,	// NMT_GT1, NMT_GT2, NMT_GT8
+	kEplNmtEventResetNode = 0x11,
+	kEplNmtEventResetCom = 0x12,
+	kEplNmtEventResetConfig = 0x13,
+	kEplNmtEventEnterPreOperational2 = 0x14,
+	kEplNmtEventEnableReadyToOperate = 0x15,
+	kEplNmtEventStartNode = 0x16,	// NMT_CT7
+	kEplNmtEventStopNode = 0x17,
+
+	// Events triggered by higher layer
+	kEplNmtEventEnterResetApp = 0x20,
+	kEplNmtEventEnterResetCom = 0x21,
+	kEplNmtEventInternComError = 0x22,	// NMT_GT6, internal communication error -> enter ResetCommunication
+	kEplNmtEventEnterResetConfig = 0x23,
+	kEplNmtEventEnterCsNotActive = 0x24,
+	kEplNmtEventEnterMsNotActive = 0x25,
+	kEplNmtEventTimerBasicEthernet = 0x26,	// NMT_CT3; timer triggered state change (NotActive -> BasicEth)
+	kEplNmtEventTimerMsPreOp1 = 0x27,	// enter PreOp1 on MN (NotActive -> MsPreOp1)
+	kEplNmtEventNmtCycleError = 0x28,	// NMT_CT11, NMT_MT6; error during cycle -> enter PreOp1
+	kEplNmtEventTimerMsPreOp2 = 0x29,	// enter PreOp2 on MN (MsPreOp1 -> MsPreOp2 if kEplNmtEventAllMandatoryCNIdent)
+	kEplNmtEventAllMandatoryCNIdent = 0x2A,	// enter PreOp2 on MN if kEplNmtEventTimerMsPreOp2
+	kEplNmtEventEnterReadyToOperate = 0x2B,	// application ready for the state ReadyToOp
+	kEplNmtEventEnterMsOperational = 0x2C,	// enter Operational on MN
+	kEplNmtEventSwitchOff = 0x2D,	// enter state Off
+	kEplNmtEventCriticalError = 0x2E,	// enter state Off because of critical error
+
+} tEplNmtEvent;
+
+// type for argument of event kEplEventTypeNmtStateChange
+typedef struct {
+	tEplNmtState m_NewNmtState;
+	tEplNmtEvent m_NmtEvent;
+
+} tEplEventNmtStateChange;
+
+// structure for kEplEventTypeHeartbeat
+typedef struct {
+	unsigned int m_uiNodeId;	// NodeId
+	tEplNmtState m_NmtState;	// NMT state (remember distinguish between MN / CN)
+	WORD m_wErrorCode;	// EPL error code in case of NMT state NotActive
+
+} tEplHeartbeatEvent;
+
+typedef enum {
+	kEplNmtNodeEventFound = 0x00,
+	kEplNmtNodeEventUpdateSw = 0x01,	// application shall update software on CN
+	kEplNmtNodeEventCheckConf = 0x02,	// application / Configuration Manager shall check and update configuration on CN
+	kEplNmtNodeEventUpdateConf = 0x03,	// application / Configuration Manager shall update configuration on CN (check was done by NmtMn module)
+	kEplNmtNodeEventVerifyConf = 0x04,	// application / Configuration Manager shall verify configuration of CN
+	kEplNmtNodeEventReadyToStart = 0x05,	// issued if EPL_NMTST_NO_STARTNODE set
+	// application must call EplNmtMnuSendNmtCommand(kEplNmtCmdStartNode) manually
+	kEplNmtNodeEventNmtState = 0x06,
+	kEplNmtNodeEventError = 0x07,	// NMT error of CN
+
+} tEplNmtNodeEvent;
+
+typedef enum {
+	kEplNmtNodeCommandBoot = 0x01,	// if EPL_NODEASSIGN_START_CN not set it must be issued after kEplNmtNodeEventFound
+	kEplNmtNodeCommandSwOk = 0x02,	// application updated software on CN successfully
+	kEplNmtNodeCommandSwUpdated = 0x03,	// application updated software on CN successfully
+	kEplNmtNodeCommandConfOk = 0x04,	// application / Configuration Manager has updated configuration on CN successfully
+	kEplNmtNodeCommandConfReset = 0x05,	// application / Configuration Manager has updated configuration on CN successfully
+	// and CN needs ResetConf so that the configuration gets actived
+	kEplNmtNodeCommandConfErr = 0x06,	// application / Configuration Manager failed on updating configuration on CN
+	kEplNmtNodeCommandStart = 0x07,	// if EPL_NMTST_NO_STARTNODE set it must be issued after kEplNmtNodeEventReadyToStart
+
+} tEplNmtNodeCommand;
+
+typedef enum {
+	kEplNmtBootEventBootStep1Finish = 0x00,	// PreOp2 is possible
+	kEplNmtBootEventBootStep2Finish = 0x01,	// ReadyToOp is possible
+	kEplNmtBootEventCheckComFinish = 0x02,	// Operational is possible
+	kEplNmtBootEventOperational = 0x03,	// all mandatory CNs are Operational
+	kEplNmtBootEventError = 0x04,	// boot process halted because of an error
+
+} tEplNmtBootEvent;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPLNMT_H_
diff --git a/drivers/staging/epl/EplNmtCnu.c b/drivers/staging/epl/EplNmtCnu.c
new file mode 100644
index 000000000000..f2f46da08c7d
--- /dev/null
+++ b/drivers/staging/epl/EplNmtCnu.c
@@ -0,0 +1,704 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for NMT-CN-Userspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtCnu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "user/EplNmtCnu.h"
+#include "user/EplDlluCal.h"
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_CN)) != 0)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	unsigned int m_uiNodeId;
+	tEplNmtuCheckEventCallback m_pfnCheckEventCb;
+
+} tEplNmtCnuInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+static tEplNmtCnuInstance EplNmtCnuInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplNmtCommand EplNmtCnuGetNmtCommand(tEplFrameInfo * pFrameInfo_p);
+
+static BOOL EplNmtCnuNodeIdList(BYTE * pbNmtCommandDate_p);
+
+static tEplKernel PUBLIC EplNmtCnuCommandCb(tEplFrameInfo * pFrameInfo_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuInit
+//
+// Description: init the first instance of the module
+//
+//
+//
+// Parameters:      uiNodeId_p = NodeId of the local node
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuInit(unsigned int uiNodeId_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplNmtCnuAddInstance(uiNodeId_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuAddInstance
+//
+// Description: init the add new instance of the module
+//
+//
+//
+// Parameters:      uiNodeId_p = NodeId of the local node
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuAddInstance(unsigned int uiNodeId_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// reset instance structure
+	EPL_MEMSET(&EplNmtCnuInstance_g, 0, sizeof(EplNmtCnuInstance_g));
+
+	// save nodeid
+	EplNmtCnuInstance_g.m_uiNodeId = uiNodeId_p;
+
+	// register callback-function for NMT-commands
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	Ret = EplDlluCalRegAsndService(kEplDllAsndNmtCommand,
+				       EplNmtCnuCommandCb,
+				       kEplDllAsndFilterLocal);
+#endif
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuDelInstance
+//
+// Description: delte instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	// deregister callback function from DLL
+	Ret = EplDlluCalRegAsndService(kEplDllAsndNmtCommand,
+				       NULL, kEplDllAsndFilterNone);
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuSendNmtRequest
+//
+// Description: Send an NMT-Request to the MN
+//
+//
+//
+// Parameters:      uiNodeId_p = NodeId of the local node
+//                  NmtCommand_p = requested NMT-Command
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuSendNmtRequest(unsigned int uiNodeId_p,
+						       tEplNmtCommand
+						       NmtCommand_p)
+{
+	tEplKernel Ret;
+	tEplFrameInfo NmtRequestFrameInfo;
+	tEplFrame NmtRequestFrame;
+
+	Ret = kEplSuccessful;
+
+	// build frame
+	EPL_MEMSET(&NmtRequestFrame.m_be_abDstMac[0], 0x00, sizeof(NmtRequestFrame.m_be_abDstMac));	// set by DLL
+	EPL_MEMSET(&NmtRequestFrame.m_be_abSrcMac[0], 0x00, sizeof(NmtRequestFrame.m_be_abSrcMac));	// set by DLL
+	AmiSetWordToBe(&NmtRequestFrame.m_be_wEtherType,
+		       EPL_C_DLL_ETHERTYPE_EPL);
+	AmiSetByteToLe(&NmtRequestFrame.m_le_bDstNodeId, (BYTE) EPL_C_ADR_MN_DEF_NODE_ID);	// node id of the MN
+	AmiSetByteToLe(&NmtRequestFrame.m_le_bMessageType,
+		       (BYTE) kEplMsgTypeAsnd);
+	AmiSetByteToLe(&NmtRequestFrame.m_Data.m_Asnd.m_le_bServiceId,
+		       (BYTE) kEplDllAsndNmtRequest);
+	AmiSetByteToLe(&NmtRequestFrame.m_Data.m_Asnd.m_Payload.
+		       m_NmtRequestService.m_le_bNmtCommandId,
+		       (BYTE) NmtCommand_p);
+	AmiSetByteToLe(&NmtRequestFrame.m_Data.m_Asnd.m_Payload.m_NmtRequestService.m_le_bTargetNodeId, (BYTE) uiNodeId_p);	// target for the nmt command
+	EPL_MEMSET(&NmtRequestFrame.m_Data.m_Asnd.m_Payload.m_NmtRequestService.
+		   m_le_abNmtCommandData[0], 0x00,
+		   sizeof(NmtRequestFrame.m_Data.m_Asnd.m_Payload.
+			  m_NmtRequestService.m_le_abNmtCommandData));
+
+	// build info-structure
+	NmtRequestFrameInfo.m_NetTime.m_dwNanoSec = 0;
+	NmtRequestFrameInfo.m_NetTime.m_dwSec = 0;
+	NmtRequestFrameInfo.m_pFrame = &NmtRequestFrame;
+	NmtRequestFrameInfo.m_uiFrameSize = EPL_C_DLL_MINSIZE_NMTREQ;	// sizeof(NmtRequestFrame);
+
+	// send NMT-Request
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	Ret = EplDlluCalAsyncSend(&NmtRequestFrameInfo,	// pointer to frameinfo
+				  kEplDllAsyncReqPrioNmt);	// priority
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuRegisterStateChangeCb
+//
+// Description: register Callback-function go get informed about a
+//              NMT-Change-State-Event
+//
+//
+//
+// Parameters:  pfnEplNmtStateChangeCb_p = functionpointer
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC
+EplNmtCnuRegisterCheckEventCb(tEplNmtuCheckEventCallback
+			      pfnEplNmtCheckEventCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// save callback-function in modul global var
+	EplNmtCnuInstance_g.m_pfnCheckEventCb = pfnEplNmtCheckEventCb_p;
+
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuCommandCb
+//
+// Description: callback funktion for NMT-Commands
+//
+//
+//
+// Parameters:      pFrameInfo_p = Frame with the NMT-Commando
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel PUBLIC EplNmtCnuCommandCb(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplNmtCommand NmtCommand;
+	BOOL fNodeIdInList;
+	tEplNmtEvent NmtEvent = kEplNmtEventNoEvent;
+
+	if (pFrameInfo_p == NULL) {
+		Ret = kEplNmtInvalidFramePointer;
+		goto Exit;
+	}
+
+	NmtCommand = EplNmtCnuGetNmtCommand(pFrameInfo_p);
+
+	// check NMT-Command
+	switch (NmtCommand) {
+
+		//------------------------------------------------------------------------
+		// plain NMT state commands
+	case kEplNmtCmdStartNode:
+		{		// send NMT-Event to state maschine kEplNmtEventStartNode
+			NmtEvent = kEplNmtEventStartNode;
+			break;
+		}
+
+	case kEplNmtCmdStopNode:
+		{		// send NMT-Event to state maschine kEplNmtEventStopNode
+			NmtEvent = kEplNmtEventStopNode;
+			break;
+		}
+
+	case kEplNmtCmdEnterPreOperational2:
+		{		// send NMT-Event to state maschine kEplNmtEventEnterPreOperational2
+			NmtEvent = kEplNmtEventEnterPreOperational2;
+			break;
+		}
+
+	case kEplNmtCmdEnableReadyToOperate:
+		{		// send NMT-Event to state maschine kEplNmtEventEnableReadyToOperate
+			NmtEvent = kEplNmtEventEnableReadyToOperate;
+			break;
+		}
+
+	case kEplNmtCmdResetNode:
+		{		// send NMT-Event to state maschine kEplNmtEventResetNode
+			NmtEvent = kEplNmtEventResetNode;
+			break;
+		}
+
+	case kEplNmtCmdResetCommunication:
+		{		// send NMT-Event to state maschine kEplNmtEventResetCom
+			NmtEvent = kEplNmtEventResetCom;
+			break;
+		}
+
+	case kEplNmtCmdResetConfiguration:
+		{		// send NMT-Event to state maschine kEplNmtEventResetConfig
+			NmtEvent = kEplNmtEventResetConfig;
+			break;
+		}
+
+	case kEplNmtCmdSwReset:
+		{		// send NMT-Event to state maschine kEplNmtEventSwReset
+			NmtEvent = kEplNmtEventSwReset;
+			break;
+		}
+
+		//------------------------------------------------------------------------
+		// extended NMT state commands
+
+	case kEplNmtCmdStartNodeEx:
+		{
+			// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&
+						(pFrameInfo_p->m_pFrame->m_Data.
+						 m_Asnd.m_Payload.
+						 m_NmtCommandService.
+						 m_le_abNmtCommandData[0]));
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventStartNode;
+			}
+			break;
+		}
+
+	case kEplNmtCmdStopNodeEx:
+		{		// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&pFrameInfo_p->m_pFrame->m_Data.
+						m_Asnd.m_Payload.
+						m_NmtCommandService.
+						m_le_abNmtCommandData[0]);
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventStopNode;
+			}
+			break;
+		}
+
+	case kEplNmtCmdEnterPreOperational2Ex:
+		{		// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&pFrameInfo_p->m_pFrame->m_Data.
+						m_Asnd.m_Payload.
+						m_NmtCommandService.
+						m_le_abNmtCommandData[0]);
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventEnterPreOperational2;
+			}
+			break;
+		}
+
+	case kEplNmtCmdEnableReadyToOperateEx:
+		{		// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&pFrameInfo_p->m_pFrame->m_Data.
+						m_Asnd.m_Payload.
+						m_NmtCommandService.
+						m_le_abNmtCommandData[0]);
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventEnableReadyToOperate;
+			}
+			break;
+		}
+
+	case kEplNmtCmdResetNodeEx:
+		{		// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&pFrameInfo_p->m_pFrame->m_Data.
+						m_Asnd.m_Payload.
+						m_NmtCommandService.
+						m_le_abNmtCommandData[0]);
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventResetNode;
+			}
+			break;
+		}
+
+	case kEplNmtCmdResetCommunicationEx:
+		{		// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&pFrameInfo_p->m_pFrame->m_Data.
+						m_Asnd.m_Payload.
+						m_NmtCommandService.
+						m_le_abNmtCommandData[0]);
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventResetCom;
+			}
+			break;
+		}
+
+	case kEplNmtCmdResetConfigurationEx:
+		{		// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&pFrameInfo_p->m_pFrame->m_Data.
+						m_Asnd.m_Payload.
+						m_NmtCommandService.
+						m_le_abNmtCommandData[0]);
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventResetConfig;
+			}
+			break;
+		}
+
+	case kEplNmtCmdSwResetEx:
+		{		// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&pFrameInfo_p->m_pFrame->m_Data.
+						m_Asnd.m_Payload.
+						m_NmtCommandService.
+						m_le_abNmtCommandData[0]);
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventSwReset;
+			}
+			break;
+		}
+
+		//------------------------------------------------------------------------
+		// NMT managing commands
+
+		// TODO: add functions to process managing command (optional)
+
+	case kEplNmtCmdNetHostNameSet:
+		{
+			break;
+		}
+
+	case kEplNmtCmdFlushArpEntry:
+		{
+			break;
+		}
+
+		//------------------------------------------------------------------------
+		// NMT info services
+
+		// TODO: forward event with infos to the application (optional)
+
+	case kEplNmtCmdPublishConfiguredCN:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishActiveCN:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishPreOperational1:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishPreOperational2:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishReadyToOperate:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishOperational:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishStopped:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishEmergencyNew:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishTime:
+		{
+			break;
+		}
+
+		//-----------------------------------------------------------------------
+		// error from MN
+		// -> requested command not supported by MN
+	case kEplNmtCmdInvalidService:
+		{
+
+			// TODO: errorevent to application
+			break;
+		}
+
+		//------------------------------------------------------------------------
+		// default
+	default:
+		{
+			Ret = kEplNmtUnknownCommand;
+			goto Exit;
+		}
+
+	}			// end of switch(NmtCommand)
+
+	if (NmtEvent != kEplNmtEventNoEvent) {
+		if (EplNmtCnuInstance_g.m_pfnCheckEventCb != NULL) {
+			Ret = EplNmtCnuInstance_g.m_pfnCheckEventCb(NmtEvent);
+			if (Ret == kEplReject) {
+				Ret = kEplSuccessful;
+				goto Exit;
+			} else if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+		}
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+		Ret = EplNmtuNmtEvent(NmtEvent);
+#endif
+	}
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuGetNmtCommand()
+//
+// Description: returns the NMT-Command from the frame
+//
+//
+//
+// Parameters:      pFrameInfo_p = pointer to the Frame
+//                                 with the NMT-Command
+//
+//
+// Returns:         tEplNmtCommand = NMT-Command
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplNmtCommand EplNmtCnuGetNmtCommand(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplNmtCommand NmtCommand;
+	tEplNmtCommandService *pNmtCommandService;
+
+	pNmtCommandService =
+	    &pFrameInfo_p->m_pFrame->m_Data.m_Asnd.m_Payload.
+	    m_NmtCommandService;
+
+	NmtCommand =
+	    (tEplNmtCommand) AmiGetByteFromLe(&pNmtCommandService->
+					      m_le_bNmtCommandId);
+
+	return NmtCommand;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuNodeIdList()
+//
+// Description: check if the own nodeid is set in EPL Node List
+//
+//
+//
+// Parameters:      pbNmtCommandDate_p = pointer to the data of the NMT Command
+//
+//
+// Returns:         BOOL = TRUE if nodeid is set in EPL Node List
+//                         FALSE if nodeid not set in EPL Node List
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static BOOL EplNmtCnuNodeIdList(BYTE * pbNmtCommandDate_p)
+{
+	BOOL fNodeIdInList;
+	unsigned int uiByteOffset;
+	BYTE bBitOffset;
+	BYTE bNodeListByte;
+
+	// get byte-offset of the own nodeid in NodeIdList
+	// devide though 8
+	uiByteOffset = (unsigned int)(EplNmtCnuInstance_g.m_uiNodeId >> 3);
+	// get bitoffset
+	bBitOffset = (BYTE) EplNmtCnuInstance_g.m_uiNodeId % 8;
+
+	bNodeListByte = AmiGetByteFromLe(&pbNmtCommandDate_p[uiByteOffset]);
+	if ((bNodeListByte & bBitOffset) == 0) {
+		fNodeIdInList = FALSE;
+	} else {
+		fNodeIdInList = TRUE;
+	}
+
+	return fNodeIdInList;
+}
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_CN)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplNmtMnu.c b/drivers/staging/epl/EplNmtMnu.c
new file mode 100644
index 000000000000..4ed0b6ce487c
--- /dev/null
+++ b/drivers/staging/epl/EplNmtMnu.c
@@ -0,0 +1,2835 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for NMT-MN-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtMnu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.18 $  $Date: 2008/11/19 09:52:24 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplNmtMnu.h"
+#include "user/EplTimeru.h"
+#include "user/EplIdentu.h"
+#include "user/EplStatusu.h"
+#include "user/EplObdu.h"
+#include "user/EplDlluCal.h"
+#include "Benchmark.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) == 0) && (EPL_OBD_USE_KERNEL == FALSE)
+#error "EPL NmtMnu module needs EPL module OBDU or OBDK!"
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)     TgtDbgPostTraceValue(v)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)
+#endif
+#define EPL_NMTMNU_DBG_POST_TRACE_VALUE(Event_p, uiNodeId_p, wErrorCode_p) \
+    TGT_DBG_POST_TRACE_VALUE((kEplEventSinkNmtMnu << 28) | (Event_p << 24) \
+                             | (uiNodeId_p << 16) | wErrorCode_p)
+
+// defines for flags in node info structure
+#define EPL_NMTMNU_NODE_FLAG_ISOCHRON       0x0001	// CN is being accessed isochronously
+#define EPL_NMTMNU_NODE_FLAG_NOT_SCANNED    0x0002	// CN was not scanned once -> decrement SignalCounter and reset flag
+#define EPL_NMTMNU_NODE_FLAG_HALTED         0x0004	// boot process for this CN is halted
+#define EPL_NMTMNU_NODE_FLAG_NMT_CMD_ISSUED 0x0008	// NMT command was just issued, wrong NMT states will be tolerated
+#define EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ  0x0300	// counter for StatusRequest timer handle
+#define EPL_NMTMNU_NODE_FLAG_COUNT_LONGER   0x0C00	// counter for longer timeouts timer handle
+#define EPL_NMTMNU_NODE_FLAG_INC_STATREQ    0x0100	// increment for StatusRequest timer handle
+#define EPL_NMTMNU_NODE_FLAG_INC_LONGER     0x0400	// increment for longer timeouts timer handle
+		    // These counters will be incremented at every timer start
+		    // and copied to timerarg. When the timer event occures
+		    // both will be compared and if unequal the timer event
+		    // will be discarded, because it is an old one.
+
+// defines for timer arguments to draw a distinction between serveral events
+#define EPL_NMTMNU_TIMERARG_NODE_MASK   0x000000FFL	// mask that contains the node-ID
+#define EPL_NMTMNU_TIMERARG_IDENTREQ    0x00010000L	// timer event is for IdentRequest
+#define EPL_NMTMNU_TIMERARG_STATREQ     0x00020000L	// timer event is for StatusRequest
+#define EPL_NMTMNU_TIMERARG_LONGER      0x00040000L	// timer event is for longer timeouts
+#define EPL_NMTMNU_TIMERARG_STATE_MON   0x00080000L	// timer event for StatusRequest to monitor execution of NMT state changes
+#define EPL_NMTMNU_TIMERARG_COUNT_SR    0x00000300L	// counter for StatusRequest
+#define EPL_NMTMNU_TIMERARG_COUNT_LO    0x00000C00L	// counter for longer timeouts
+		    // The counters must have the same position as in the node flags above.
+
+#define EPL_NMTMNU_SET_FLAGS_TIMERARG_STATREQ(pNodeInfo_p, uiNodeId_p, TimerArg_p) \
+    pNodeInfo_p->m_wFlags = \
+        ((pNodeInfo_p->m_wFlags + EPL_NMTMNU_NODE_FLAG_INC_STATREQ) \
+         & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) \
+        | (pNodeInfo_p->m_wFlags & ~EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ); \
+    TimerArg_p.m_ulArg = EPL_NMTMNU_TIMERARG_STATREQ | uiNodeId_p | \
+        (pNodeInfo_p->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ); \
+    TimerArg_p.m_EventSink = kEplEventSinkNmtMnu;
+
+#define EPL_NMTMNU_SET_FLAGS_TIMERARG_IDENTREQ(pNodeInfo_p, uiNodeId_p, TimerArg_p) \
+    pNodeInfo_p->m_wFlags = \
+        ((pNodeInfo_p->m_wFlags + EPL_NMTMNU_NODE_FLAG_INC_STATREQ) \
+         & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) \
+        | (pNodeInfo_p->m_wFlags & ~EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ); \
+    TimerArg_p.m_ulArg = EPL_NMTMNU_TIMERARG_IDENTREQ | uiNodeId_p | \
+        (pNodeInfo_p->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ); \
+    TimerArg_p.m_EventSink = kEplEventSinkNmtMnu;
+
+#define EPL_NMTMNU_SET_FLAGS_TIMERARG_LONGER(pNodeInfo_p, uiNodeId_p, TimerArg_p) \
+    pNodeInfo_p->m_wFlags = \
+        ((pNodeInfo_p->m_wFlags + EPL_NMTMNU_NODE_FLAG_INC_LONGER) \
+         & EPL_NMTMNU_NODE_FLAG_COUNT_LONGER) \
+        | (pNodeInfo_p->m_wFlags & ~EPL_NMTMNU_NODE_FLAG_COUNT_LONGER); \
+    TimerArg_p.m_ulArg = EPL_NMTMNU_TIMERARG_LONGER | uiNodeId_p | \
+        (pNodeInfo_p->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_LONGER); \
+    TimerArg_p.m_EventSink = kEplEventSinkNmtMnu;
+
+#define EPL_NMTMNU_SET_FLAGS_TIMERARG_STATE_MON(pNodeInfo_p, uiNodeId_p, TimerArg_p) \
+    pNodeInfo_p->m_wFlags = \
+        ((pNodeInfo_p->m_wFlags + EPL_NMTMNU_NODE_FLAG_INC_STATREQ) \
+         & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) \
+        | (pNodeInfo_p->m_wFlags & ~EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ); \
+    TimerArg_p.m_ulArg = EPL_NMTMNU_TIMERARG_STATE_MON | uiNodeId_p | \
+        (pNodeInfo_p->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ); \
+    TimerArg_p.m_EventSink = kEplEventSinkNmtMnu;
+
+// defines for global flags
+#define EPL_NMTMNU_FLAG_HALTED          0x0001	// boot process is halted
+#define EPL_NMTMNU_FLAG_APP_INFORMED    0x0002	// application was informed about possible NMT state change
+
+// return pointer to node info structure for specified node ID
+// d.k. may be replaced by special (hash) function if node ID array is smaller than 254
+#define EPL_NMTMNU_GET_NODEINFO(uiNodeId_p) (&EplNmtMnuInstance_g.m_aNodeInfo[uiNodeId_p - 1])
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef enum {
+	kEplNmtMnuIntNodeEventNoIdentResponse = 0x00,
+	kEplNmtMnuIntNodeEventIdentResponse = 0x01,
+	kEplNmtMnuIntNodeEventBoot = 0x02,
+	kEplNmtMnuIntNodeEventExecReset = 0x03,
+	kEplNmtMnuIntNodeEventConfigured = 0x04,
+	kEplNmtMnuIntNodeEventNoStatusResponse = 0x05,
+	kEplNmtMnuIntNodeEventStatusResponse = 0x06,
+	kEplNmtMnuIntNodeEventHeartbeat = 0x07,
+	kEplNmtMnuIntNodeEventNmtCmdSent = 0x08,
+	kEplNmtMnuIntNodeEventTimerIdentReq = 0x09,
+	kEplNmtMnuIntNodeEventTimerStatReq = 0x0A,
+	kEplNmtMnuIntNodeEventTimerStateMon = 0x0B,
+	kEplNmtMnuIntNodeEventTimerLonger = 0x0C,
+	kEplNmtMnuIntNodeEventError = 0x0D,
+
+} tEplNmtMnuIntNodeEvent;
+
+typedef enum {
+	kEplNmtMnuNodeStateUnknown = 0x00,
+	kEplNmtMnuNodeStateIdentified = 0x01,
+	kEplNmtMnuNodeStateResetConf = 0x02,	// CN reset after configuration update
+	kEplNmtMnuNodeStateConfigured = 0x03,	// BootStep1 completed
+	kEplNmtMnuNodeStateReadyToOp = 0x04,	// BootStep2 completed
+	kEplNmtMnuNodeStateComChecked = 0x05,	// Communication checked successfully
+	kEplNmtMnuNodeStateOperational = 0x06,	// CN is in NMT state OPERATIONAL
+
+} tEplNmtMnuNodeState;
+
+typedef struct {
+	tEplTimerHdl m_TimerHdlStatReq;	// timer to delay StatusRequests and IdentRequests
+	tEplTimerHdl m_TimerHdlLonger;	// 2nd timer for NMT command EnableReadyToOp and CheckCommunication
+	tEplNmtMnuNodeState m_NodeState;	// internal node state (kind of sub state of NMT state)
+	DWORD m_dwNodeCfg;	// subindex from 0x1F81
+	WORD m_wFlags;		// flags: CN is being accessed isochronously
+
+} tEplNmtMnuNodeInfo;
+
+typedef struct {
+	tEplNmtMnuNodeInfo m_aNodeInfo[EPL_NMT_MAX_NODE_ID];
+	tEplTimerHdl m_TimerHdlNmtState;	// timeout for stay in NMT state
+	unsigned int m_uiMandatorySlaveCount;
+	unsigned int m_uiSignalSlaveCount;
+	unsigned long m_ulStatusRequestDelay;	// in [ms] (object 0x1006 * EPL_C_NMT_STATREQ_CYCLE)
+	unsigned long m_ulTimeoutReadyToOp;	// in [ms] (object 0x1F89/5)
+	unsigned long m_ulTimeoutCheckCom;	// in [ms] (object 0x1006 * MultiplexedCycleCount)
+	WORD m_wFlags;		// global flags
+	DWORD m_dwNmtStartup;	// object 0x1F80 NMT_StartUp_U32
+	tEplNmtMnuCbNodeEvent m_pfnCbNodeEvent;
+	tEplNmtMnuCbBootEvent m_pfnCbBootEvent;
+
+} tEplNmtMnuInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+static tEplNmtMnuInstance EplNmtMnuInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplNmtMnuCbNmtRequest(tEplFrameInfo * pFrameInfo_p);
+
+static tEplKernel PUBLIC EplNmtMnuCbIdentResponse(unsigned int uiNodeId_p,
+						  tEplIdentResponse *
+						  pIdentResponse_p);
+
+static tEplKernel PUBLIC EplNmtMnuCbStatusResponse(unsigned int uiNodeId_p,
+						   tEplStatusResponse *
+						   pStatusResponse_p);
+
+static tEplKernel EplNmtMnuCheckNmtState(unsigned int uiNodeId_p,
+					 tEplNmtMnuNodeInfo * pNodeInfo_p,
+					 tEplNmtState NodeNmtState_p,
+					 WORD wErrorCode_p,
+					 tEplNmtState LocalNmtState_p);
+
+static tEplKernel EplNmtMnuStartBootStep1(void);
+
+static tEplKernel EplNmtMnuStartBootStep2(void);
+
+static tEplKernel EplNmtMnuStartCheckCom(void);
+
+static tEplKernel EplNmtMnuNodeBootStep2(unsigned int uiNodeId_p,
+					 tEplNmtMnuNodeInfo * pNodeInfo_p);
+
+static tEplKernel EplNmtMnuNodeCheckCom(unsigned int uiNodeId_p,
+					tEplNmtMnuNodeInfo * pNodeInfo_p);
+
+static tEplKernel EplNmtMnuStartNodes(void);
+
+static tEplKernel EplNmtMnuProcessInternalEvent(unsigned int uiNodeId_p,
+						tEplNmtState NodeNmtState_p,
+						WORD wErrorCode_p,
+						tEplNmtMnuIntNodeEvent
+						NodeEvent_p);
+
+static tEplKernel EplNmtMnuReset(void);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuInit
+//
+// Description: init first instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplNmtMnuInit(tEplNmtMnuCbNodeEvent pfnCbNodeEvent_p,
+			 tEplNmtMnuCbBootEvent pfnCbBootEvent_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplNmtMnuAddInstance(pfnCbNodeEvent_p, pfnCbBootEvent_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuAddInstance
+//
+// Description: init other instances of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplNmtMnuAddInstance(tEplNmtMnuCbNodeEvent pfnCbNodeEvent_p,
+				tEplNmtMnuCbBootEvent pfnCbBootEvent_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// reset instance structure
+	EPL_MEMSET(&EplNmtMnuInstance_g, 0, sizeof(EplNmtMnuInstance_g));
+
+	if ((pfnCbNodeEvent_p == NULL) || (pfnCbBootEvent_p == NULL)) {
+		Ret = kEplNmtInvalidParam;
+		goto Exit;
+	}
+	EplNmtMnuInstance_g.m_pfnCbNodeEvent = pfnCbNodeEvent_p;
+	EplNmtMnuInstance_g.m_pfnCbBootEvent = pfnCbBootEvent_p;
+
+	// initialize StatusRequest delay
+	EplNmtMnuInstance_g.m_ulStatusRequestDelay = 5000L;
+
+	// register NmtMnResponse callback function
+	Ret =
+	    EplDlluCalRegAsndService(kEplDllAsndNmtRequest,
+				     EplNmtMnuCbNmtRequest,
+				     kEplDllAsndFilterLocal);
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuDelInstance
+//
+// Description: delete instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplNmtMnuDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// deregister NmtMnResponse callback function
+	Ret =
+	    EplDlluCalRegAsndService(kEplDllAsndNmtRequest, NULL,
+				     kEplDllAsndFilterNone);
+
+	Ret = EplNmtMnuReset();
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuSendNmtCommandEx
+//
+// Description: sends the specified NMT command to the specified node.
+//
+// Parameters:  uiNodeId_p              = node ID to which the NMT command will be sent
+//              NmtCommand_p            = NMT command
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplNmtMnuSendNmtCommandEx(unsigned int uiNodeId_p,
+				     tEplNmtCommand NmtCommand_p,
+				     void *pNmtCommandData_p,
+				     unsigned int uiDataSize_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplFrameInfo FrameInfo;
+	BYTE abBuffer[EPL_C_DLL_MINSIZE_NMTCMDEXT];
+	tEplFrame *pFrame = (tEplFrame *) abBuffer;
+	BOOL fSoftDeleteNode = FALSE;
+
+	if ((uiNodeId_p == 0) || (uiNodeId_p > EPL_C_ADR_BROADCAST)) {	// invalid node ID specified
+		Ret = kEplInvalidNodeId;
+		goto Exit;
+	}
+
+	if ((pNmtCommandData_p != NULL)
+	    && (uiDataSize_p >
+		(EPL_C_DLL_MINSIZE_NMTCMDEXT - EPL_C_DLL_MINSIZE_NMTCMD))) {
+		Ret = kEplNmtInvalidParam;
+		goto Exit;
+	}
+	// $$$ d.k. may be check in future versions if the caller wants to perform prohibited state transitions
+	//     the CN should not perform these transitions, but the expected NMT state will be changed and never fullfilled.
+
+	// build frame
+	EPL_MEMSET(pFrame, 0x00, sizeof(abBuffer));
+	AmiSetByteToLe(&pFrame->m_le_bDstNodeId, (BYTE) uiNodeId_p);
+	AmiSetByteToLe(&pFrame->m_Data.m_Asnd.m_le_bServiceId,
+		       (BYTE) kEplDllAsndNmtCommand);
+	AmiSetByteToLe(&pFrame->m_Data.m_Asnd.m_Payload.m_NmtCommandService.
+		       m_le_bNmtCommandId, (BYTE) NmtCommand_p);
+	if ((pNmtCommandData_p != NULL) && (uiDataSize_p > 0)) {	// copy command data to frame
+		EPL_MEMCPY(&pFrame->m_Data.m_Asnd.m_Payload.m_NmtCommandService.
+			   m_le_abNmtCommandData[0], pNmtCommandData_p,
+			   uiDataSize_p);
+	}
+	// build info structure
+	FrameInfo.m_NetTime.m_dwNanoSec = 0;
+	FrameInfo.m_NetTime.m_dwSec = 0;
+	FrameInfo.m_pFrame = pFrame;
+	FrameInfo.m_uiFrameSize = sizeof(abBuffer);
+
+	// send NMT-Request
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	Ret = EplDlluCalAsyncSend(&FrameInfo,	// pointer to frameinfo
+				  kEplDllAsyncReqPrioNmt);	// priority
+#endif
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	EPL_DBGLVL_NMTMN_TRACE2("NMTCmd(%02X->%02X)\n", NmtCommand_p,
+				uiNodeId_p);
+
+	switch (NmtCommand_p) {
+	case kEplNmtCmdStartNode:
+	case kEplNmtCmdEnterPreOperational2:
+	case kEplNmtCmdEnableReadyToOperate:
+		{
+			// nothing left to do,
+			// because any further processing is done
+			// when the NMT command is actually sent
+			goto Exit;
+		}
+
+	case kEplNmtCmdStopNode:
+		{
+			fSoftDeleteNode = TRUE;
+			break;
+		}
+
+	case kEplNmtCmdResetNode:
+	case kEplNmtCmdResetCommunication:
+	case kEplNmtCmdResetConfiguration:
+	case kEplNmtCmdSwReset:
+		{
+			break;
+		}
+
+	default:
+		goto Exit;
+	}
+
+	// remove CN from isochronous phase;
+	// This must be done here and not when NMT command is actually sent
+	// because it will be too late and may cause unwanted errors
+	if (uiNodeId_p != EPL_C_ADR_BROADCAST) {
+		if (fSoftDeleteNode == FALSE) {	// remove CN immediately from isochronous phase
+			Ret = EplDlluCalDeleteNode(uiNodeId_p);
+		} else {	// remove CN from isochronous phase softly
+			Ret = EplDlluCalSoftDeleteNode(uiNodeId_p);
+		}
+	} else {		// do it for all active CNs
+		for (uiNodeId_p = 1;
+		     uiNodeId_p <= tabentries(EplNmtMnuInstance_g.m_aNodeInfo);
+		     uiNodeId_p++) {
+			if ((EPL_NMTMNU_GET_NODEINFO(uiNodeId_p)->
+			     m_dwNodeCfg & (EPL_NODEASSIGN_NODE_IS_CN |
+					    EPL_NODEASSIGN_NODE_EXISTS)) != 0) {
+				if (fSoftDeleteNode == FALSE) {	// remove CN immediately from isochronous phase
+					Ret = EplDlluCalDeleteNode(uiNodeId_p);
+				} else {	// remove CN from isochronous phase softly
+					Ret =
+					    EplDlluCalSoftDeleteNode
+					    (uiNodeId_p);
+				}
+			}
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuSendNmtCommand
+//
+// Description: sends the specified NMT command to the specified node.
+//
+// Parameters:  uiNodeId_p              = node ID to which the NMT command will be sent
+//              NmtCommand_p            = NMT command
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplNmtMnuSendNmtCommand(unsigned int uiNodeId_p,
+				   tEplNmtCommand NmtCommand_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	Ret = EplNmtMnuSendNmtCommandEx(uiNodeId_p, NmtCommand_p, NULL, 0);
+
+//Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuTriggerStateChange
+//
+// Description: triggers the specified node command for the specified node.
+//
+// Parameters:  uiNodeId_p              = node ID for which the node command will be executed
+//              NodeCommand_p           = node command
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplNmtMnuTriggerStateChange(unsigned int uiNodeId_p,
+				       tEplNmtNodeCommand NodeCommand_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplNmtMnuIntNodeEvent NodeEvent;
+	tEplObdSize ObdSize;
+	BYTE bNmtState;
+	WORD wErrorCode = EPL_E_NO_ERROR;
+
+	if ((uiNodeId_p == 0) || (uiNodeId_p >= EPL_C_ADR_BROADCAST)) {
+		Ret = kEplInvalidNodeId;
+		goto Exit;
+	}
+
+	switch (NodeCommand_p) {
+	case kEplNmtNodeCommandBoot:
+		{
+			NodeEvent = kEplNmtMnuIntNodeEventBoot;
+			break;
+		}
+
+	case kEplNmtNodeCommandConfOk:
+		{
+			NodeEvent = kEplNmtMnuIntNodeEventConfigured;
+			break;
+		}
+
+	case kEplNmtNodeCommandConfErr:
+		{
+			NodeEvent = kEplNmtMnuIntNodeEventError;
+			wErrorCode = EPL_E_NMT_BPO1_CF_VERIFY;
+			break;
+		}
+
+	case kEplNmtNodeCommandConfReset:
+		{
+			NodeEvent = kEplNmtMnuIntNodeEventExecReset;
+			break;
+		}
+
+	default:
+		{		// invalid node command
+			goto Exit;
+		}
+	}
+
+	// fetch current NMT state
+	ObdSize = 1;
+	Ret = EplObduReadEntry(0x1F8E, uiNodeId_p, &bNmtState, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret = EplNmtMnuProcessInternalEvent(uiNodeId_p,
+					    (tEplNmtState) (bNmtState |
+							    EPL_NMT_TYPE_CS),
+					    wErrorCode, NodeEvent);
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuCbNmtStateChange
+//
+// Description: callback function for NMT state changes
+//
+// Parameters:  NmtStateChange_p        = NMT state change event
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplNmtMnuCbNmtStateChange(tEplEventNmtStateChange
+					    NmtStateChange_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// do work which must be done in that state
+	switch (NmtStateChange_p.m_NewNmtState) {
+		// EPL stack is not running
+/*        case kEplNmtGsOff:
+            break;
+
+        // first init of the hardware
+        case kEplNmtGsInitialising:
+            break;
+
+        // init of the manufacturer-specific profile area and the
+        // standardised device profile area
+        case kEplNmtGsResetApplication:
+        {
+            break;
+        }
+
+        // init of the communication profile area
+        case kEplNmtGsResetCommunication:
+        {
+            break;
+        }
+*/
+		// build the configuration with infos from OD
+	case kEplNmtGsResetConfiguration:
+		{
+			DWORD dwTimeout;
+			tEplObdSize ObdSize;
+
+			// read object 0x1F80 NMT_StartUp_U32
+			ObdSize = 4;
+			Ret =
+			    EplObduReadEntry(0x1F80, 0,
+					     &EplNmtMnuInstance_g.
+					     m_dwNmtStartup, &ObdSize);
+			if (Ret != kEplSuccessful) {
+				break;
+			}
+			// compute StatusReqDelay = object 0x1006 * EPL_C_NMT_STATREQ_CYCLE
+			ObdSize = sizeof(dwTimeout);
+			Ret = EplObduReadEntry(0x1006, 0, &dwTimeout, &ObdSize);
+			if (Ret != kEplSuccessful) {
+				break;
+			}
+			if (dwTimeout != 0L) {
+				EplNmtMnuInstance_g.m_ulStatusRequestDelay =
+				    dwTimeout * EPL_C_NMT_STATREQ_CYCLE / 1000L;
+				if (EplNmtMnuInstance_g.
+				    m_ulStatusRequestDelay == 0L) {
+					EplNmtMnuInstance_g.m_ulStatusRequestDelay = 1L;	// at least 1 ms
+				}
+				// $$$ fetch and use MultiplexedCycleCount from OD
+				EplNmtMnuInstance_g.m_ulTimeoutCheckCom =
+				    dwTimeout * EPL_C_NMT_STATREQ_CYCLE / 1000L;
+				if (EplNmtMnuInstance_g.m_ulTimeoutCheckCom ==
+				    0L) {
+					EplNmtMnuInstance_g.m_ulTimeoutCheckCom = 1L;	// at least 1 ms
+				}
+			}
+			// fetch ReadyToOp Timeout from OD
+			ObdSize = sizeof(dwTimeout);
+			Ret = EplObduReadEntry(0x1F89, 5, &dwTimeout, &ObdSize);
+			if (Ret != kEplSuccessful) {
+				break;
+			}
+			if (dwTimeout != 0L) {
+				// convert [us] to [ms]
+				dwTimeout /= 1000L;
+				if (dwTimeout == 0L) {
+					dwTimeout = 1L;	// at least 1 ms
+				}
+				EplNmtMnuInstance_g.m_ulTimeoutReadyToOp =
+				    dwTimeout;
+			} else {
+				EplNmtMnuInstance_g.m_ulTimeoutReadyToOp = 0L;
+			}
+			break;
+		}
+/*
+        //-----------------------------------------------------------
+        // CN part of the state machine
+
+        // node liste for EPL-Frames and check timeout
+        case kEplNmtCsNotActive:
+        {
+            break;
+        }
+
+        // node process only async frames
+        case kEplNmtCsPreOperational1:
+        {
+            break;
+        }
+
+        // node process isochronus and asynchronus frames
+        case kEplNmtCsPreOperational2:
+        {
+            break;
+        }
+
+        // node should be configured und application is ready
+        case kEplNmtCsReadyToOperate:
+        {
+            break;
+        }
+
+        // normal work state
+        case kEplNmtCsOperational:
+        {
+            break;
+        }
+
+        // node stopped by MN
+        // -> only process asynchronus frames
+        case kEplNmtCsStopped:
+        {
+            break;
+        }
+
+        // no EPL cycle
+        // -> normal ethernet communication
+        case kEplNmtCsBasicEthernet:
+        {
+            break;
+        }
+*/
+		//-----------------------------------------------------------
+		// MN part of the state machine
+
+		// node listens for EPL-Frames and check timeout
+	case kEplNmtMsNotActive:
+		{
+			break;
+		}
+
+		// node processes only async frames
+	case kEplNmtMsPreOperational1:
+		{
+			DWORD dwTimeout;
+			tEplTimerArg TimerArg;
+			tEplObdSize ObdSize;
+			tEplEvent Event;
+
+			// clear global flags, e.g. reenable boot process
+			EplNmtMnuInstance_g.m_wFlags = 0;
+
+			// reset IdentResponses and running IdentRequests and StatusRequests
+			Ret = EplIdentuReset();
+			Ret = EplStatusuReset();
+
+			// reset timers
+			Ret = EplNmtMnuReset();
+
+			// 2008/11/18 d.k. reset internal node info is not necessary,
+			//                 because timer flags are important and other
+			//                 things are reset by EplNmtMnuStartBootStep1().
+/*
+            EPL_MEMSET(EplNmtMnuInstance_g.m_aNodeInfo,
+                       0,
+                       sizeof (EplNmtMnuInstance_g.m_aNodeInfo));
+*/
+
+			// inform DLL about NMT state change,
+			// so that it can clear the asynchonous queues and start the reduced cycle
+			Event.m_EventSink = kEplEventSinkDllk;
+			Event.m_EventType = kEplEventTypeDllkStartReducedCycle;
+			EPL_MEMSET(&Event.m_NetTime, 0x00,
+				   sizeof(Event.m_NetTime));
+			Event.m_pArg = NULL;
+			Event.m_uiSize = 0;
+			Ret = EplEventuPost(&Event);
+			if (Ret != kEplSuccessful) {
+				break;
+			}
+			// reset all nodes
+			// d.k.: skip this step if was just done before, e.g. because of a ResetNode command from a diagnostic node
+			if (NmtStateChange_p.m_NmtEvent ==
+			    kEplNmtEventTimerMsPreOp1) {
+				BENCHMARK_MOD_07_TOGGLE(9);
+
+				EPL_NMTMNU_DBG_POST_TRACE_VALUE(0,
+								EPL_C_ADR_BROADCAST,
+								kEplNmtCmdResetNode);
+
+				Ret =
+				    EplNmtMnuSendNmtCommand(EPL_C_ADR_BROADCAST,
+							    kEplNmtCmdResetNode);
+				if (Ret != kEplSuccessful) {
+					break;
+				}
+			}
+			// start network scan
+			Ret = EplNmtMnuStartBootStep1();
+
+			// start timer for 0x1F89/2 MNTimeoutPreOp1_U32
+			ObdSize = sizeof(dwTimeout);
+			Ret = EplObduReadEntry(0x1F89, 2, &dwTimeout, &ObdSize);
+			if (Ret != kEplSuccessful) {
+				break;
+			}
+			if (dwTimeout != 0L) {
+				dwTimeout /= 1000L;
+				if (dwTimeout == 0L) {
+					dwTimeout = 1L;	// at least 1 ms
+				}
+				TimerArg.m_EventSink = kEplEventSinkNmtMnu;
+				TimerArg.m_ulArg = 0;
+				Ret =
+				    EplTimeruModifyTimerMs(&EplNmtMnuInstance_g.
+							   m_TimerHdlNmtState,
+							   dwTimeout, TimerArg);
+			}
+			break;
+		}
+
+		// node processes isochronous and asynchronous frames
+	case kEplNmtMsPreOperational2:
+		{
+			// add identified CNs to isochronous phase
+			// send EnableReadyToOp to all identified CNs
+			Ret = EplNmtMnuStartBootStep2();
+
+			// wait for NMT state change of CNs
+			break;
+		}
+
+		// node should be configured und application is ready
+	case kEplNmtMsReadyToOperate:
+		{
+			// check if PRes of CNs are OK
+			// d.k. that means wait CycleLength * MultiplexCycleCount (i.e. start timer)
+			//      because Dllk checks PRes of CNs automatically in ReadyToOp
+			Ret = EplNmtMnuStartCheckCom();
+			break;
+		}
+
+		// normal work state
+	case kEplNmtMsOperational:
+		{
+			// send StartNode to CNs
+			// wait for NMT state change of CNs
+			Ret = EplNmtMnuStartNodes();
+			break;
+		}
+
+		// no EPL cycle
+		// -> normal ethernet communication
+	case kEplNmtMsBasicEthernet:
+		{
+			break;
+		}
+
+	default:
+		{
+//            TRACE0("EplNmtMnuCbNmtStateChange(): unhandled NMT state\n");
+		}
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuCbCheckEvent
+//
+// Description: callback funktion for NMT events before they are actually executed.
+//              The EPL API layer must forward NMT events from NmtCnu module.
+//              This module will reject some NMT commands while MN.
+//
+// Parameters:  NmtEvent_p              = outstanding NMT event for approval
+//
+// Returns:     tEplKernel              = error code
+//                      kEplReject      = reject the NMT event
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplNmtMnuCbCheckEvent(tEplNmtEvent NmtEvent_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuProcessEvent
+//
+// Description: processes events from event queue
+//
+// Parameters:  pEvent_p        = pointer to event
+//
+// Returns:     tEplKernel      = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtMnuProcessEvent(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// process event
+	switch (pEvent_p->m_EventType) {
+		// timer event
+	case kEplEventTypeTimer:
+		{
+			tEplTimerEventArg *pTimerEventArg =
+			    (tEplTimerEventArg *) pEvent_p->m_pArg;
+			unsigned int uiNodeId;
+
+			uiNodeId =
+			    (unsigned int)(pTimerEventArg->
+					   m_ulArg &
+					   EPL_NMTMNU_TIMERARG_NODE_MASK);
+			if (uiNodeId != 0) {
+				tEplObdSize ObdSize;
+				BYTE bNmtState;
+				tEplNmtMnuNodeInfo *pNodeInfo;
+
+				pNodeInfo = EPL_NMTMNU_GET_NODEINFO(uiNodeId);
+
+				ObdSize = 1;
+				Ret =
+				    EplObduReadEntry(0x1F8E, uiNodeId,
+						     &bNmtState, &ObdSize);
+				if (Ret != kEplSuccessful) {
+					break;
+				}
+
+				if ((pTimerEventArg->
+				     m_ulArg & EPL_NMTMNU_TIMERARG_IDENTREQ) !=
+				    0L) {
+					if ((pNodeInfo->
+					     m_wFlags &
+					     EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ)
+					    != (pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR)) {	// this is an old (already deleted or modified) timer
+						// but not the current timer
+						// so discard it
+						EPL_NMTMNU_DBG_POST_TRACE_VALUE
+						    (kEplNmtMnuIntNodeEventTimerIdentReq,
+						     uiNodeId,
+						     ((pNodeInfo->
+						       m_NodeState << 8)
+						      | 0xFF));
+
+						break;
+					}
+/*
+                    EPL_NMTMNU_DBG_POST_TRACE_VALUE(kEplNmtMnuIntNodeEventTimerIdentReq,
+                                                    uiNodeId,
+                                                    ((pNodeInfo->m_NodeState << 8)
+                                                     | 0x80
+                                                     | ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) >> 6)
+                                                     | ((pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR) >> 8)));
+*/
+					Ret =
+					    EplNmtMnuProcessInternalEvent
+					    (uiNodeId,
+					     (tEplNmtState) (bNmtState |
+							     EPL_NMT_TYPE_CS),
+					     EPL_E_NO_ERROR,
+					     kEplNmtMnuIntNodeEventTimerIdentReq);
+				}
+
+				else if ((pTimerEventArg->
+					  m_ulArg & EPL_NMTMNU_TIMERARG_STATREQ)
+					 != 0L) {
+					if ((pNodeInfo->
+					     m_wFlags &
+					     EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ)
+					    != (pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR)) {	// this is an old (already deleted or modified) timer
+						// but not the current timer
+						// so discard it
+						EPL_NMTMNU_DBG_POST_TRACE_VALUE
+						    (kEplNmtMnuIntNodeEventTimerStatReq,
+						     uiNodeId,
+						     ((pNodeInfo->
+						       m_NodeState << 8)
+						      | 0xFF));
+
+						break;
+					}
+/*
+                    EPL_NMTMNU_DBG_POST_TRACE_VALUE(kEplNmtMnuIntNodeEventTimerStatReq,
+                                                    uiNodeId,
+                                                    ((pNodeInfo->m_NodeState << 8)
+                                                     | 0x80
+                                                     | ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) >> 6)
+                                                     | ((pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR) >> 8)));
+*/
+					Ret =
+					    EplNmtMnuProcessInternalEvent
+					    (uiNodeId,
+					     (tEplNmtState) (bNmtState |
+							     EPL_NMT_TYPE_CS),
+					     EPL_E_NO_ERROR,
+					     kEplNmtMnuIntNodeEventTimerStatReq);
+				}
+
+				else if ((pTimerEventArg->
+					  m_ulArg &
+					  EPL_NMTMNU_TIMERARG_STATE_MON) !=
+					 0L) {
+					if ((pNodeInfo->
+					     m_wFlags &
+					     EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ)
+					    != (pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR)) {	// this is an old (already deleted or modified) timer
+						// but not the current timer
+						// so discard it
+						EPL_NMTMNU_DBG_POST_TRACE_VALUE
+						    (kEplNmtMnuIntNodeEventTimerStateMon,
+						     uiNodeId,
+						     ((pNodeInfo->
+						       m_NodeState << 8)
+						      | 0xFF));
+
+						break;
+					}
+/*
+                    EPL_NMTMNU_DBG_POST_TRACE_VALUE(kEplNmtMnuIntNodeEventTimerStatReq,
+                                                    uiNodeId,
+                                                    ((pNodeInfo->m_NodeState << 8)
+                                                     | 0x80
+                                                     | ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) >> 6)
+                                                     | ((pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR) >> 8)));
+*/
+					Ret =
+					    EplNmtMnuProcessInternalEvent
+					    (uiNodeId,
+					     (tEplNmtState) (bNmtState |
+							     EPL_NMT_TYPE_CS),
+					     EPL_E_NO_ERROR,
+					     kEplNmtMnuIntNodeEventTimerStateMon);
+				}
+
+				else if ((pTimerEventArg->
+					  m_ulArg & EPL_NMTMNU_TIMERARG_LONGER)
+					 != 0L) {
+					if ((pNodeInfo->
+					     m_wFlags &
+					     EPL_NMTMNU_NODE_FLAG_COUNT_LONGER)
+					    != (pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_LO)) {	// this is an old (already deleted or modified) timer
+						// but not the current timer
+						// so discard it
+						EPL_NMTMNU_DBG_POST_TRACE_VALUE
+						    (kEplNmtMnuIntNodeEventTimerLonger,
+						     uiNodeId,
+						     ((pNodeInfo->
+						       m_NodeState << 8)
+						      | 0xFF));
+
+						break;
+					}
+/*
+                    EPL_NMTMNU_DBG_POST_TRACE_VALUE(kEplNmtMnuIntNodeEventTimerLonger,
+                                                    uiNodeId,
+                                                    ((pNodeInfo->m_NodeState << 8)
+                                                     | 0x80
+                                                     | ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_LONGER) >> 6)
+                                                     | ((pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_LO) >> 8)));
+*/
+					Ret =
+					    EplNmtMnuProcessInternalEvent
+					    (uiNodeId,
+					     (tEplNmtState) (bNmtState |
+							     EPL_NMT_TYPE_CS),
+					     EPL_E_NO_ERROR,
+					     kEplNmtMnuIntNodeEventTimerLonger);
+				}
+
+			} else {	// global timer event
+			}
+			break;
+		}
+
+	case kEplEventTypeHeartbeat:
+		{
+			tEplHeartbeatEvent *pHeartbeatEvent =
+			    (tEplHeartbeatEvent *) pEvent_p->m_pArg;
+
+			Ret =
+			    EplNmtMnuProcessInternalEvent(pHeartbeatEvent->
+							  m_uiNodeId,
+							  pHeartbeatEvent->
+							  m_NmtState,
+							  pHeartbeatEvent->
+							  m_wErrorCode,
+							  kEplNmtMnuIntNodeEventHeartbeat);
+			break;
+		}
+
+	case kEplEventTypeNmtMnuNmtCmdSent:
+		{
+			tEplFrame *pFrame = (tEplFrame *) pEvent_p->m_pArg;
+			unsigned int uiNodeId;
+			tEplNmtCommand NmtCommand;
+			BYTE bNmtState;
+
+			uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bDstNodeId);
+			NmtCommand =
+			    (tEplNmtCommand) AmiGetByteFromLe(&pFrame->m_Data.
+							      m_Asnd.m_Payload.
+							      m_NmtCommandService.
+							      m_le_bNmtCommandId);
+
+			switch (NmtCommand) {
+			case kEplNmtCmdStartNode:
+				bNmtState =
+				    (BYTE) (kEplNmtCsOperational & 0xFF);
+				break;
+
+			case kEplNmtCmdStopNode:
+				bNmtState = (BYTE) (kEplNmtCsStopped & 0xFF);
+				break;
+
+			case kEplNmtCmdEnterPreOperational2:
+				bNmtState =
+				    (BYTE) (kEplNmtCsPreOperational2 & 0xFF);
+				break;
+
+			case kEplNmtCmdEnableReadyToOperate:
+				// d.k. do not change expected node state, because of DS 1.0.0 7.3.1.2.1 Plain NMT State Command
+				//      and because node may not change NMT state within EPL_C_NMT_STATE_TOLERANCE
+				bNmtState =
+				    (BYTE) (kEplNmtCsPreOperational2 & 0xFF);
+				break;
+
+			case kEplNmtCmdResetNode:
+			case kEplNmtCmdResetCommunication:
+			case kEplNmtCmdResetConfiguration:
+			case kEplNmtCmdSwReset:
+				bNmtState = (BYTE) (kEplNmtCsNotActive & 0xFF);
+				// EplNmtMnuProcessInternalEvent() sets internal node state to kEplNmtMnuNodeStateUnknown
+				// after next unresponded IdentRequest/StatusRequest
+				break;
+
+			default:
+				goto Exit;
+			}
+
+			// process as internal event which update expected NMT state in OD
+			if (uiNodeId != EPL_C_ADR_BROADCAST) {
+				Ret = EplNmtMnuProcessInternalEvent(uiNodeId,
+								    (tEplNmtState)
+								    (bNmtState |
+								     EPL_NMT_TYPE_CS),
+								    0,
+								    kEplNmtMnuIntNodeEventNmtCmdSent);
+
+			} else {	// process internal event for all active nodes (except myself)
+
+				for (uiNodeId = 1;
+				     uiNodeId <=
+				     tabentries(EplNmtMnuInstance_g.
+						m_aNodeInfo); uiNodeId++) {
+					if ((EPL_NMTMNU_GET_NODEINFO(uiNodeId)->
+					     m_dwNodeCfg &
+					     (EPL_NODEASSIGN_NODE_IS_CN |
+					      EPL_NODEASSIGN_NODE_EXISTS)) !=
+					    0) {
+						Ret =
+						    EplNmtMnuProcessInternalEvent
+						    (uiNodeId,
+						     (tEplNmtState) (bNmtState |
+								     EPL_NMT_TYPE_CS),
+						     0,
+						     kEplNmtMnuIntNodeEventNmtCmdSent);
+
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+					}
+				}
+			}
+
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplNmtInvalidEvent;
+		}
+
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuGetRunningTimerStatReq
+//
+// Description: returns a bit field with running StatReq timers
+//              just for debugging purposes
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplNmtMnuGetDiagnosticInfo(unsigned int
+					     *puiMandatorySlaveCount_p,
+					     unsigned int
+					     *puiSignalSlaveCount_p,
+					     WORD * pwFlags_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if ((puiMandatorySlaveCount_p == NULL)
+	    || (puiSignalSlaveCount_p == NULL)
+	    || (pwFlags_p == NULL)) {
+		Ret = kEplNmtInvalidParam;
+		goto Exit;
+	}
+
+	*puiMandatorySlaveCount_p = EplNmtMnuInstance_g.m_uiMandatorySlaveCount;
+	*puiSignalSlaveCount_p = EplNmtMnuInstance_g.m_uiSignalSlaveCount;
+	*pwFlags_p = EplNmtMnuInstance_g.m_wFlags;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuGetRunningTimerStatReq
+//
+// Description: returns a bit field with running StatReq timers
+//              just for debugging purposes
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+/*
+DWORD EplNmtMnuGetRunningTimerStatReq(void)
+{
+tEplKernel      Ret = kEplSuccessful;
+unsigned int    uiIndex;
+tEplNmtMnuNodeInfo* pNodeInfo;
+
+    pNodeInfo = EplNmtMnuInstance_g.m_aNodeInfo;
+    for (uiIndex = 1; uiIndex <= tabentries(EplNmtMnuInstance_g.m_aNodeInfo); uiIndex++, pNodeInfo++)
+    {
+        if (pNodeInfo->m_NodeState == kEplNmtMnuNodeStateConfigured)
+        {
+            // reset flag "scanned once"
+            pNodeInfo->m_wFlags &= ~EPL_NMTMNU_NODE_FLAG_SCANNED;
+
+            Ret = EplNmtMnuNodeBootStep2(uiIndex, pNodeInfo);
+            if (Ret != kEplSuccessful)
+            {
+                goto Exit;
+            }
+            EplNmtMnuInstance_g.m_uiSignalSlaveCount++;
+            // signal slave counter shall be decremented if StatusRequest was sent once to a CN
+            // mandatory slave counter shall be decremented if mandatory CN is ReadyToOp
+        }
+    }
+
+Exit:
+    return Ret;
+}
+*/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuCbNmtRequest
+//
+// Description: callback funktion for NmtRequest
+//
+// Parameters:  pFrameInfo_p            = Frame with the NmtRequest
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplNmtMnuCbNmtRequest(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// $$$ perform NMTRequest
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuCbIdentResponse
+//
+// Description: callback funktion for IdentResponse
+//
+// Parameters:  uiNodeId_p              = node ID for which IdentReponse was received
+//              pIdentResponse_p        = pointer to IdentResponse
+//                                        is NULL if node did not answer
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplNmtMnuCbIdentResponse(unsigned int uiNodeId_p,
+						  tEplIdentResponse *
+						  pIdentResponse_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (pIdentResponse_p == NULL) {	// node did not answer
+		Ret = EplNmtMnuProcessInternalEvent(uiNodeId_p, kEplNmtCsNotActive, EPL_E_NMT_NO_IDENT_RES,	// was EPL_E_NO_ERROR
+						    kEplNmtMnuIntNodeEventNoIdentResponse);
+	} else {		// node answered IdentRequest
+		tEplObdSize ObdSize;
+		DWORD dwDevType;
+		WORD wErrorCode = EPL_E_NO_ERROR;
+		tEplNmtState NmtState =
+		    (tEplNmtState) (AmiGetByteFromLe
+				    (&pIdentResponse_p->
+				     m_le_bNmtStatus) | EPL_NMT_TYPE_CS);
+
+		// check IdentResponse $$$ move to ProcessIntern, because this function may be called also if CN
+
+		// check DeviceType (0x1F84)
+		ObdSize = 4;
+		Ret =
+		    EplObduReadEntry(0x1F84, uiNodeId_p, &dwDevType, &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		if (dwDevType != 0L) {	// actually compare it with DeviceType from IdentResponse
+			if (AmiGetDwordFromLe(&pIdentResponse_p->m_le_dwDeviceType) != dwDevType) {	// wrong DeviceType
+				NmtState = kEplNmtCsNotActive;
+				wErrorCode = EPL_E_NMT_BPO1_DEVICE_TYPE;
+			}
+		}
+
+		Ret = EplNmtMnuProcessInternalEvent(uiNodeId_p,
+						    NmtState,
+						    wErrorCode,
+						    kEplNmtMnuIntNodeEventIdentResponse);
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuCbStatusResponse
+//
+// Description: callback funktion for StatusResponse
+//
+// Parameters:  uiNodeId_p              = node ID for which IdentReponse was received
+//              pIdentResponse_p        = pointer to IdentResponse
+//                                        is NULL if node did not answer
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplNmtMnuCbStatusResponse(unsigned int uiNodeId_p,
+						   tEplStatusResponse *
+						   pStatusResponse_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (pStatusResponse_p == NULL) {	// node did not answer
+		Ret = EplNmtMnuProcessInternalEvent(uiNodeId_p, kEplNmtCsNotActive, EPL_E_NMT_NO_STATUS_RES,	// was EPL_E_NO_ERROR
+						    kEplNmtMnuIntNodeEventNoStatusResponse);
+	} else {		// node answered StatusRequest
+		Ret = EplNmtMnuProcessInternalEvent(uiNodeId_p,
+						    (tEplNmtState)
+						    (AmiGetByteFromLe
+						     (&pStatusResponse_p->
+						      m_le_bNmtStatus) |
+						     EPL_NMT_TYPE_CS),
+						    EPL_E_NO_ERROR,
+						    kEplNmtMnuIntNodeEventStatusResponse);
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuStartBootStep1
+//
+// Description: starts BootStep1
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuStartBootStep1(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiSubIndex;
+	unsigned int uiLocalNodeId;
+	DWORD dwNodeCfg;
+	tEplObdSize ObdSize;
+
+	// $$$ d.k.: save current time for 0x1F89/2 MNTimeoutPreOp1_U32
+
+	// start network scan
+	EplNmtMnuInstance_g.m_uiMandatorySlaveCount = 0;
+	EplNmtMnuInstance_g.m_uiSignalSlaveCount = 0;
+	// check 0x1F81
+	uiLocalNodeId = EplObduGetNodeId();
+	for (uiSubIndex = 1; uiSubIndex <= 254; uiSubIndex++) {
+		ObdSize = 4;
+		Ret =
+		    EplObduReadEntry(0x1F81, uiSubIndex, &dwNodeCfg, &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		if (uiSubIndex != uiLocalNodeId) {
+			// reset flags "not scanned" and "isochronous"
+			EPL_NMTMNU_GET_NODEINFO(uiSubIndex)->m_wFlags &=
+			    ~(EPL_NMTMNU_NODE_FLAG_ISOCHRON |
+			      EPL_NMTMNU_NODE_FLAG_NOT_SCANNED);
+
+			if (uiSubIndex == EPL_C_ADR_DIAG_DEF_NODE_ID) {	// diagnostic node must be scanned by MN in any case
+				dwNodeCfg |=
+				    (EPL_NODEASSIGN_NODE_IS_CN |
+				     EPL_NODEASSIGN_NODE_EXISTS);
+				// and it must be isochronously accessed
+				dwNodeCfg &= ~EPL_NODEASSIGN_ASYNCONLY_NODE;
+			}
+			// save node config in local node info structure
+			EPL_NMTMNU_GET_NODEINFO(uiSubIndex)->m_dwNodeCfg =
+			    dwNodeCfg;
+			EPL_NMTMNU_GET_NODEINFO(uiSubIndex)->m_NodeState =
+			    kEplNmtMnuNodeStateUnknown;
+
+			if ((dwNodeCfg & (EPL_NODEASSIGN_NODE_IS_CN | EPL_NODEASSIGN_NODE_EXISTS)) != 0) {	// node is configured as CN
+				// identify the node
+				Ret =
+				    EplIdentuRequestIdentResponse(uiSubIndex,
+								  EplNmtMnuCbIdentResponse);
+				if (Ret != kEplSuccessful) {
+					goto Exit;
+				}
+				// set flag "not scanned"
+				EPL_NMTMNU_GET_NODEINFO(uiSubIndex)->m_wFlags |=
+				    EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+				EplNmtMnuInstance_g.m_uiSignalSlaveCount++;
+				// signal slave counter shall be decremented if IdentRequest was sent once to a CN
+
+				if ((dwNodeCfg & EPL_NODEASSIGN_MANDATORY_CN) != 0) {	// node is a mandatory CN
+					EplNmtMnuInstance_g.
+					    m_uiMandatorySlaveCount++;
+					// mandatory slave counter shall be decremented if mandatory CN was configured successfully
+				}
+			}
+		} else {	// subindex of MN
+			if ((dwNodeCfg & (EPL_NODEASSIGN_MN_PRES | EPL_NODEASSIGN_NODE_EXISTS)) != 0) {	// MN shall send PRes
+				tEplDllNodeInfo DllNodeInfo;
+
+				EPL_MEMSET(&DllNodeInfo, 0,
+					   sizeof(DllNodeInfo));
+				DllNodeInfo.m_uiNodeId = uiLocalNodeId;
+
+				Ret = EplDlluCalAddNode(&DllNodeInfo);
+			}
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuStartBootStep2
+//
+// Description: starts BootStep2.
+//              That means add nodes to isochronous phase and send
+//              NMT EnableReadyToOp.
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuStartBootStep2(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiIndex;
+	tEplNmtMnuNodeInfo *pNodeInfo;
+
+	if ((EplNmtMnuInstance_g.m_wFlags & EPL_NMTMNU_FLAG_HALTED) == 0) {	// boot process is not halted
+		// add nodes to isochronous phase and send NMT EnableReadyToOp
+		EplNmtMnuInstance_g.m_uiMandatorySlaveCount = 0;
+		EplNmtMnuInstance_g.m_uiSignalSlaveCount = 0;
+		// reset flag that application was informed about possible state change
+		EplNmtMnuInstance_g.m_wFlags &= ~EPL_NMTMNU_FLAG_APP_INFORMED;
+
+		pNodeInfo = EplNmtMnuInstance_g.m_aNodeInfo;
+		for (uiIndex = 1;
+		     uiIndex <= tabentries(EplNmtMnuInstance_g.m_aNodeInfo);
+		     uiIndex++, pNodeInfo++) {
+			if (pNodeInfo->m_NodeState ==
+			    kEplNmtMnuNodeStateConfigured) {
+				Ret =
+				    EplNmtMnuNodeBootStep2(uiIndex, pNodeInfo);
+				if (Ret != kEplSuccessful) {
+					goto Exit;
+				}
+				// set flag "not scanned"
+				pNodeInfo->m_wFlags |=
+				    EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+
+				EplNmtMnuInstance_g.m_uiSignalSlaveCount++;
+				// signal slave counter shall be decremented if StatusRequest was sent once to a CN
+
+				if ((pNodeInfo->m_dwNodeCfg & EPL_NODEASSIGN_MANDATORY_CN) != 0) {	// node is a mandatory CN
+					EplNmtMnuInstance_g.
+					    m_uiMandatorySlaveCount++;
+				}
+				// mandatory slave counter shall be decremented if mandatory CN is ReadyToOp
+			}
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuNodeBootStep2
+//
+// Description: starts BootStep2 for the specified node.
+//              This means the CN is added to isochronous phase if not
+//              async-only and it gets the NMT command EnableReadyToOp.
+//              The CN must be in node state Configured, when it enters
+//              BootStep2. When BootStep2 finishes, the CN is in node state
+//              ReadyToOp.
+//              If TimeoutReadyToOp in object 0x1F89/5 is configured,
+//              TimerHdlLonger will be started with this timeout.
+//
+// Parameters:  uiNodeId_p              = node ID
+//              pNodeInfo_p             = pointer to internal node info structure
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuNodeBootStep2(unsigned int uiNodeId_p,
+					 tEplNmtMnuNodeInfo * pNodeInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplDllNodeInfo DllNodeInfo;
+	DWORD dwNodeCfg;
+	tEplObdSize ObdSize;
+	tEplTimerArg TimerArg;
+
+	dwNodeCfg = pNodeInfo_p->m_dwNodeCfg;
+	if ((dwNodeCfg & EPL_NODEASSIGN_ASYNCONLY_NODE) == 0) {	// add node to isochronous phase
+		DllNodeInfo.m_uiNodeId = uiNodeId_p;
+		ObdSize = 4;
+		Ret =
+		    EplObduReadEntry(0x1F92, uiNodeId_p,
+				     &DllNodeInfo.m_dwPresTimeout, &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		ObdSize = 2;
+		Ret =
+		    EplObduReadEntry(0x1F8B, uiNodeId_p,
+				     &DllNodeInfo.m_wPreqPayloadLimit,
+				     &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		ObdSize = 2;
+		Ret =
+		    EplObduReadEntry(0x1F8D, uiNodeId_p,
+				     &DllNodeInfo.m_wPresPayloadLimit,
+				     &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		pNodeInfo_p->m_wFlags |= EPL_NMTMNU_NODE_FLAG_ISOCHRON;
+
+		Ret = EplDlluCalAddNode(&DllNodeInfo);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+	}
+
+	EPL_NMTMNU_DBG_POST_TRACE_VALUE(0,
+					uiNodeId_p,
+					kEplNmtCmdEnableReadyToOperate);
+
+	Ret =
+	    EplNmtMnuSendNmtCommand(uiNodeId_p, kEplNmtCmdEnableReadyToOperate);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	if (EplNmtMnuInstance_g.m_ulTimeoutReadyToOp != 0L) {	// start timer
+		// when the timer expires the CN must be ReadyToOp
+		EPL_NMTMNU_SET_FLAGS_TIMERARG_LONGER(pNodeInfo_p, uiNodeId_p,
+						     TimerArg);
+//        TimerArg.m_EventSink = kEplEventSinkNmtMnu;
+//        TimerArg.m_ulArg = EPL_NMTMNU_TIMERARG_LONGER | uiNodeId_p;
+		Ret =
+		    EplTimeruModifyTimerMs(&pNodeInfo_p->m_TimerHdlLonger,
+					   EplNmtMnuInstance_g.
+					   m_ulTimeoutReadyToOp, TimerArg);
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuStartCheckCom
+//
+// Description: starts CheckCommunication
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuStartCheckCom(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiIndex;
+	tEplNmtMnuNodeInfo *pNodeInfo;
+
+	if ((EplNmtMnuInstance_g.m_wFlags & EPL_NMTMNU_FLAG_HALTED) == 0) {	// boot process is not halted
+		// wait some time and check that no communication error occurs
+		EplNmtMnuInstance_g.m_uiMandatorySlaveCount = 0;
+		EplNmtMnuInstance_g.m_uiSignalSlaveCount = 0;
+		// reset flag that application was informed about possible state change
+		EplNmtMnuInstance_g.m_wFlags &= ~EPL_NMTMNU_FLAG_APP_INFORMED;
+
+		pNodeInfo = EplNmtMnuInstance_g.m_aNodeInfo;
+		for (uiIndex = 1;
+		     uiIndex <= tabentries(EplNmtMnuInstance_g.m_aNodeInfo);
+		     uiIndex++, pNodeInfo++) {
+			if (pNodeInfo->m_NodeState ==
+			    kEplNmtMnuNodeStateReadyToOp) {
+				Ret = EplNmtMnuNodeCheckCom(uiIndex, pNodeInfo);
+				if (Ret == kEplReject) {	// timer was started
+					// wait until it expires
+					if ((pNodeInfo->m_dwNodeCfg & EPL_NODEASSIGN_MANDATORY_CN) != 0) {	// node is a mandatory CN
+						EplNmtMnuInstance_g.
+						    m_uiMandatorySlaveCount++;
+					}
+				} else if (Ret != kEplSuccessful) {
+					goto Exit;
+				}
+				// set flag "not scanned"
+				pNodeInfo->m_wFlags |=
+				    EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+
+				EplNmtMnuInstance_g.m_uiSignalSlaveCount++;
+				// signal slave counter shall be decremented if timeout elapsed and regardless of an error
+				// mandatory slave counter shall be decremented if timeout elapsed and no error occured
+			}
+		}
+	}
+
+	Ret = kEplSuccessful;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuNodeCheckCom
+//
+// Description: checks communication of the specified node.
+//              That means wait some time and if no error occured everything
+//              is OK.
+//
+// Parameters:  uiNodeId_p              = node ID
+//              pNodeInfo_p             = pointer to internal node info structure
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuNodeCheckCom(unsigned int uiNodeId_p,
+					tEplNmtMnuNodeInfo * pNodeInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	DWORD dwNodeCfg;
+	tEplTimerArg TimerArg;
+
+	dwNodeCfg = pNodeInfo_p->m_dwNodeCfg;
+	if (((dwNodeCfg & EPL_NODEASSIGN_ASYNCONLY_NODE) == 0)
+	    && (EplNmtMnuInstance_g.m_ulTimeoutCheckCom != 0L)) {	// CN is not async-only and timeout for CheckCom was set
+
+		// check communication,
+		// that means wait some time and if no error occured everything is OK;
+
+		// start timer (when the timer expires the CN must be still ReadyToOp)
+		EPL_NMTMNU_SET_FLAGS_TIMERARG_LONGER(pNodeInfo_p, uiNodeId_p,
+						     TimerArg);
+//        TimerArg.m_EventSink = kEplEventSinkNmtMnu;
+//        TimerArg.m_ulArg = EPL_NMTMNU_TIMERARG_LONGER | uiNodeId_p;
+		Ret =
+		    EplTimeruModifyTimerMs(&pNodeInfo_p->m_TimerHdlLonger,
+					   EplNmtMnuInstance_g.
+					   m_ulTimeoutCheckCom, TimerArg);
+
+		// update mandatory slave counter, because timer was started
+		if (Ret == kEplSuccessful) {
+			Ret = kEplReject;
+		}
+	} else {		// timer was not started
+		// assume everything is OK
+		pNodeInfo_p->m_NodeState = kEplNmtMnuNodeStateComChecked;
+	}
+
+//Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuStartNodes
+//
+// Description: really starts all nodes which are ReadyToOp and CheckCom did not fail
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuStartNodes(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiIndex;
+	tEplNmtMnuNodeInfo *pNodeInfo;
+
+	if ((EplNmtMnuInstance_g.m_wFlags & EPL_NMTMNU_FLAG_HALTED) == 0) {	// boot process is not halted
+		// send NMT command Start Node
+		EplNmtMnuInstance_g.m_uiMandatorySlaveCount = 0;
+		EplNmtMnuInstance_g.m_uiSignalSlaveCount = 0;
+		// reset flag that application was informed about possible state change
+		EplNmtMnuInstance_g.m_wFlags &= ~EPL_NMTMNU_FLAG_APP_INFORMED;
+
+		pNodeInfo = EplNmtMnuInstance_g.m_aNodeInfo;
+		for (uiIndex = 1;
+		     uiIndex <= tabentries(EplNmtMnuInstance_g.m_aNodeInfo);
+		     uiIndex++, pNodeInfo++) {
+			if (pNodeInfo->m_NodeState ==
+			    kEplNmtMnuNodeStateComChecked) {
+				if ((EplNmtMnuInstance_g.
+				     m_dwNmtStartup & EPL_NMTST_STARTALLNODES)
+				    == 0) {
+					EPL_NMTMNU_DBG_POST_TRACE_VALUE(0,
+									uiIndex,
+									kEplNmtCmdStartNode);
+
+					Ret =
+					    EplNmtMnuSendNmtCommand(uiIndex,
+								    kEplNmtCmdStartNode);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+				}
+
+				if ((pNodeInfo->m_dwNodeCfg & EPL_NODEASSIGN_MANDATORY_CN) != 0) {	// node is a mandatory CN
+					EplNmtMnuInstance_g.
+					    m_uiMandatorySlaveCount++;
+				}
+				// set flag "not scanned"
+				pNodeInfo->m_wFlags |=
+				    EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+
+				EplNmtMnuInstance_g.m_uiSignalSlaveCount++;
+				// signal slave counter shall be decremented if StatusRequest was sent once to a CN
+				// mandatory slave counter shall be decremented if mandatory CN is OPERATIONAL
+			}
+		}
+
+		// $$$ inform application if EPL_NMTST_NO_STARTNODE is set
+
+		if ((EplNmtMnuInstance_g.
+		     m_dwNmtStartup & EPL_NMTST_STARTALLNODES) != 0) {
+			EPL_NMTMNU_DBG_POST_TRACE_VALUE(0, EPL_C_ADR_BROADCAST,
+							kEplNmtCmdStartNode);
+
+			Ret =
+			    EplNmtMnuSendNmtCommand(EPL_C_ADR_BROADCAST,
+						    kEplNmtCmdStartNode);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuProcessInternalEvent
+//
+// Description: processes internal node events
+//
+// Parameters:  uiNodeId_p              = node ID
+//              NodeNmtState_p          = NMT state of CN
+//              NodeEvent_p             = occured events
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuProcessInternalEvent(unsigned int uiNodeId_p,
+						tEplNmtState NodeNmtState_p,
+						WORD wErrorCode_p,
+						tEplNmtMnuIntNodeEvent
+						NodeEvent_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplNmtState NmtState;
+	tEplNmtMnuNodeInfo *pNodeInfo;
+	tEplTimerArg TimerArg;
+
+	pNodeInfo = EPL_NMTMNU_GET_NODEINFO(uiNodeId_p);
+	NmtState = EplNmtuGetNmtState();
+	if (NmtState <= kEplNmtMsNotActive) {	// MN is not active
+		goto Exit;
+	}
+
+	switch (NodeEvent_p) {
+	case kEplNmtMnuIntNodeEventIdentResponse:
+		{
+			BYTE bNmtState;
+
+			EPL_NMTMNU_DBG_POST_TRACE_VALUE(NodeEvent_p,
+							uiNodeId_p,
+							pNodeInfo->m_NodeState);
+
+			if (pNodeInfo->m_NodeState !=
+			    kEplNmtMnuNodeStateResetConf) {
+				pNodeInfo->m_NodeState =
+				    kEplNmtMnuNodeStateIdentified;
+			}
+			// reset flags ISOCHRON and NMT_CMD_ISSUED
+			pNodeInfo->m_wFlags &= ~(EPL_NMTMNU_NODE_FLAG_ISOCHRON
+						 |
+						 EPL_NMTMNU_NODE_FLAG_NMT_CMD_ISSUED);
+
+			if ((NmtState == kEplNmtMsPreOperational1)
+			    &&
+			    ((pNodeInfo->
+			      m_wFlags & EPL_NMTMNU_NODE_FLAG_NOT_SCANNED) !=
+			     0)) {
+				// decrement only signal slave count
+				EplNmtMnuInstance_g.m_uiSignalSlaveCount--;
+				pNodeInfo->m_wFlags &=
+				    ~EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+			}
+			// update object 0x1F8F NMT_MNNodeExpState_AU8 to PreOp1 (even if local state >= PreOp2)
+			bNmtState = (BYTE) (kEplNmtCsPreOperational1 & 0xFF);
+			Ret =
+			    EplObduWriteEntry(0x1F8F, uiNodeId_p, &bNmtState,
+					      1);
+
+			// check NMT state of CN
+			Ret =
+			    EplNmtMnuCheckNmtState(uiNodeId_p, pNodeInfo,
+						   NodeNmtState_p, wErrorCode_p,
+						   NmtState);
+			if (Ret != kEplSuccessful) {
+				if (Ret == kEplReject) {
+					Ret = kEplSuccessful;
+				}
+				break;
+			}
+			// request StatusResponse immediately,
+			// because we want a fast boot-up of CNs
+			Ret =
+			    EplStatusuRequestStatusResponse(uiNodeId_p,
+							    EplNmtMnuCbStatusResponse);
+			if (Ret != kEplSuccessful) {
+				EPL_NMTMNU_DBG_POST_TRACE_VALUE(NodeEvent_p,
+								uiNodeId_p,
+								Ret);
+
+				if (Ret == kEplInvalidOperation) {	// the only situation when this should happen is, when
+					// StatusResponse was already requested from within
+					// the StatReq timer event.
+					// so ignore this error.
+					Ret = kEplSuccessful;
+				} else {
+					break;
+				}
+			}
+
+			if (pNodeInfo->m_NodeState !=
+			    kEplNmtMnuNodeStateResetConf) {
+				// inform application
+				Ret =
+				    EplNmtMnuInstance_g.
+				    m_pfnCbNodeEvent(uiNodeId_p,
+						     kEplNmtNodeEventFound,
+						     NodeNmtState_p,
+						     EPL_E_NO_ERROR,
+						     (pNodeInfo->
+						      m_dwNodeCfg &
+						      EPL_NODEASSIGN_MANDATORY_CN)
+						     != 0);
+				if (Ret == kEplReject) {	// interrupt boot process on user request
+					EPL_NMTMNU_DBG_POST_TRACE_VALUE
+					    (NodeEvent_p, uiNodeId_p,
+					     ((pNodeInfo->m_NodeState << 8)
+					      | Ret));
+
+					Ret = kEplSuccessful;
+					break;
+				} else if (Ret != kEplSuccessful) {
+					EPL_NMTMNU_DBG_POST_TRACE_VALUE
+					    (NodeEvent_p, uiNodeId_p,
+					     ((pNodeInfo->m_NodeState << 8)
+					      | Ret));
+
+					break;
+				}
+			}
+			// continue BootStep1
+		}
+
+	case kEplNmtMnuIntNodeEventBoot:
+		{
+
+			// $$$ check identification (vendor ID, product code, revision no, serial no)
+
+			if (pNodeInfo->m_NodeState ==
+			    kEplNmtMnuNodeStateIdentified) {
+				// $$$ check software
+
+				// check/start configuration
+				// inform application
+				Ret =
+				    EplNmtMnuInstance_g.
+				    m_pfnCbNodeEvent(uiNodeId_p,
+						     kEplNmtNodeEventCheckConf,
+						     NodeNmtState_p,
+						     EPL_E_NO_ERROR,
+						     (pNodeInfo->
+						      m_dwNodeCfg &
+						      EPL_NODEASSIGN_MANDATORY_CN)
+						     != 0);
+				if (Ret == kEplReject) {	// interrupt boot process on user request
+					EPL_NMTMNU_DBG_POST_TRACE_VALUE
+					    (kEplNmtMnuIntNodeEventBoot,
+					     uiNodeId_p,
+					     ((pNodeInfo->m_NodeState << 8)
+					      | Ret));
+
+					Ret = kEplSuccessful;
+					break;
+				} else if (Ret != kEplSuccessful) {
+					EPL_NMTMNU_DBG_POST_TRACE_VALUE
+					    (kEplNmtMnuIntNodeEventBoot,
+					     uiNodeId_p,
+					     ((pNodeInfo->m_NodeState << 8)
+					      | Ret));
+
+					break;
+				}
+			} else if (pNodeInfo->m_NodeState != kEplNmtMnuNodeStateResetConf) {	// wrong CN state
+				// ignore event
+				break;
+			}
+			// $$$ d.k.: currently we assume configuration is OK
+
+			// continue BootStep1
+		}
+
+	case kEplNmtMnuIntNodeEventConfigured:
+		{
+			if ((pNodeInfo->m_NodeState !=
+			     kEplNmtMnuNodeStateIdentified)
+			    && (pNodeInfo->m_NodeState != kEplNmtMnuNodeStateResetConf)) {	// wrong CN state
+				// ignore event
+				break;
+			}
+
+			pNodeInfo->m_NodeState = kEplNmtMnuNodeStateConfigured;
+
+			if (NmtState == kEplNmtMsPreOperational1) {
+				if ((pNodeInfo->m_dwNodeCfg & EPL_NODEASSIGN_MANDATORY_CN) != 0) {	// decrement mandatory CN counter
+					EplNmtMnuInstance_g.
+					    m_uiMandatorySlaveCount--;
+				}
+			} else {
+				// put optional node to next step (BootStep2)
+				Ret =
+				    EplNmtMnuNodeBootStep2(uiNodeId_p,
+							   pNodeInfo);
+			}
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventNoIdentResponse:
+		{
+			if ((NmtState == kEplNmtMsPreOperational1)
+			    &&
+			    ((pNodeInfo->
+			      m_wFlags & EPL_NMTMNU_NODE_FLAG_NOT_SCANNED) !=
+			     0)) {
+				// decrement only signal slave count
+				EplNmtMnuInstance_g.m_uiSignalSlaveCount--;
+				pNodeInfo->m_wFlags &=
+				    ~EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+			}
+
+			if (pNodeInfo->m_NodeState !=
+			    kEplNmtMnuNodeStateResetConf) {
+				pNodeInfo->m_NodeState =
+				    kEplNmtMnuNodeStateUnknown;
+			}
+			// $$$ d.k. check start time for 0x1F89/2 MNTimeoutPreOp1_U32
+			// $$$ d.k. check individual timeout 0x1F89/6 MNIdentificationTimeout_U32
+			// if mandatory node and timeout elapsed -> halt boot procedure
+			// trigger IdentRequest again (if >= PreOp2, after delay)
+			if (NmtState >= kEplNmtMsPreOperational2) {	// start timer
+				EPL_NMTMNU_SET_FLAGS_TIMERARG_IDENTREQ
+				    (pNodeInfo, uiNodeId_p, TimerArg);
+//                TimerArg.m_EventSink = kEplEventSinkNmtMnu;
+//                TimerArg.m_ulArg = EPL_NMTMNU_TIMERARG_IDENTREQ | uiNodeId_p;
+/*
+                EPL_NMTMNU_DBG_POST_TRACE_VALUE(kEplNmtMnuIntNodeEventNoIdentResponse,
+                                                uiNodeId_p,
+                                                ((pNodeInfo->m_NodeState << 8)
+                                                 | 0x80
+                                                 | ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) >> 6)
+                                                 | ((TimerArg.m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR) >> 8)));
+*/
+				Ret =
+				    EplTimeruModifyTimerMs(&pNodeInfo->
+							   m_TimerHdlStatReq,
+							   EplNmtMnuInstance_g.
+							   m_ulStatusRequestDelay,
+							   TimerArg);
+			} else {	// trigger IdentRequest immediately
+				Ret =
+				    EplIdentuRequestIdentResponse(uiNodeId_p,
+								  EplNmtMnuCbIdentResponse);
+			}
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventStatusResponse:
+		{
+			if ((NmtState >= kEplNmtMsPreOperational2)
+			    &&
+			    ((pNodeInfo->
+			      m_wFlags & EPL_NMTMNU_NODE_FLAG_NOT_SCANNED) !=
+			     0)) {
+				// decrement only signal slave count if checked once for ReadyToOp, CheckCom, Operational
+				EplNmtMnuInstance_g.m_uiSignalSlaveCount--;
+				pNodeInfo->m_wFlags &=
+				    ~EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+			}
+			// check NMT state of CN
+			Ret =
+			    EplNmtMnuCheckNmtState(uiNodeId_p, pNodeInfo,
+						   NodeNmtState_p, wErrorCode_p,
+						   NmtState);
+			if (Ret != kEplSuccessful) {
+				if (Ret == kEplReject) {
+					Ret = kEplSuccessful;
+				}
+				break;
+			}
+
+			if (NmtState == kEplNmtMsPreOperational1) {
+				// request next StatusResponse immediately
+				Ret =
+				    EplStatusuRequestStatusResponse(uiNodeId_p,
+								    EplNmtMnuCbStatusResponse);
+				if (Ret != kEplSuccessful) {
+					EPL_NMTMNU_DBG_POST_TRACE_VALUE
+					    (NodeEvent_p, uiNodeId_p, Ret);
+				}
+
+			} else if ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_ISOCHRON) == 0) {	// start timer
+				// not isochronously accessed CN (e.g. async-only or stopped CN)
+				EPL_NMTMNU_SET_FLAGS_TIMERARG_STATREQ(pNodeInfo,
+								      uiNodeId_p,
+								      TimerArg);
+//                TimerArg.m_EventSink = kEplEventSinkNmtMnu;
+//                TimerArg.m_ulArg = EPL_NMTMNU_TIMERARG_STATREQ | uiNodeId_p;
+/*
+                EPL_NMTMNU_DBG_POST_TRACE_VALUE(kEplNmtMnuIntNodeEventStatusResponse,
+                                                uiNodeId_p,
+                                                ((pNodeInfo->m_NodeState << 8)
+                                                 | 0x80
+                                                 | ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) >> 6)
+                                                 | ((TimerArg.m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR) >> 8)));
+*/
+				Ret =
+				    EplTimeruModifyTimerMs(&pNodeInfo->
+							   m_TimerHdlStatReq,
+							   EplNmtMnuInstance_g.
+							   m_ulStatusRequestDelay,
+							   TimerArg);
+			}
+
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventNoStatusResponse:
+		{
+			// function CheckNmtState sets node state to unknown if necessary
+/*
+            if ((NmtState >= kEplNmtMsPreOperational2)
+                && ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_NOT_SCANNED) != 0))
+            {
+                // decrement only signal slave count if checked once for ReadyToOp, CheckCom, Operational
+                EplNmtMnuInstance_g.m_uiSignalSlaveCount--;
+                pNodeInfo->m_wFlags &= ~EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+            }
+*/
+			// check NMT state of CN
+			Ret =
+			    EplNmtMnuCheckNmtState(uiNodeId_p, pNodeInfo,
+						   NodeNmtState_p, wErrorCode_p,
+						   NmtState);
+			if (Ret != kEplSuccessful) {
+				if (Ret == kEplReject) {
+					Ret = kEplSuccessful;
+				}
+				break;
+			}
+
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventError:
+		{		// currently only issued on kEplNmtNodeCommandConfErr
+
+			if (pNodeInfo->m_NodeState != kEplNmtMnuNodeStateIdentified) {	// wrong CN state
+				// ignore event
+				break;
+			}
+			// check NMT state of CN
+			Ret =
+			    EplNmtMnuCheckNmtState(uiNodeId_p, pNodeInfo,
+						   kEplNmtCsNotActive,
+						   wErrorCode_p, NmtState);
+			if (Ret != kEplSuccessful) {
+				if (Ret == kEplReject) {
+					Ret = kEplSuccessful;
+				}
+				break;
+			}
+
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventExecReset:
+		{
+			if (pNodeInfo->m_NodeState != kEplNmtMnuNodeStateIdentified) {	// wrong CN state
+				// ignore event
+				break;
+			}
+
+			pNodeInfo->m_NodeState = kEplNmtMnuNodeStateResetConf;
+
+			EPL_NMTMNU_DBG_POST_TRACE_VALUE(NodeEvent_p,
+							uiNodeId_p,
+							(((NodeNmtState_p &
+							   0xFF) << 8)
+							 |
+							 kEplNmtCmdResetConfiguration));
+
+			// send NMT reset configuration to CN for activation of configuration
+			Ret =
+			    EplNmtMnuSendNmtCommand(uiNodeId_p,
+						    kEplNmtCmdResetConfiguration);
+
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventHeartbeat:
+		{
+/*
+            if ((NmtState >= kEplNmtMsPreOperational2)
+                && ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_NOT_SCANNED) != 0))
+            {
+                // decrement only signal slave count if checked once for ReadyToOp, CheckCom, Operational
+                EplNmtMnuInstance_g.m_uiSignalSlaveCount--;
+                pNodeInfo->m_wFlags &= ~EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+            }
+*/
+			// check NMT state of CN
+			Ret =
+			    EplNmtMnuCheckNmtState(uiNodeId_p, pNodeInfo,
+						   NodeNmtState_p, wErrorCode_p,
+						   NmtState);
+			if (Ret != kEplSuccessful) {
+				if (Ret == kEplReject) {
+					Ret = kEplSuccessful;
+				}
+				break;
+			}
+
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventTimerIdentReq:
+		{
+			EPL_DBGLVL_NMTMN_TRACE1
+			    ("TimerStatReq->IdentReq(%02X)\n", uiNodeId_p);
+			// trigger IdentRequest again
+			Ret =
+			    EplIdentuRequestIdentResponse(uiNodeId_p,
+							  EplNmtMnuCbIdentResponse);
+			if (Ret != kEplSuccessful) {
+				EPL_NMTMNU_DBG_POST_TRACE_VALUE(NodeEvent_p,
+								uiNodeId_p,
+								(((NodeNmtState_p & 0xFF) << 8)
+								 | Ret));
+				if (Ret == kEplInvalidOperation) {	// this can happen because of a bug in EplTimeruLinuxKernel.c
+					// so ignore this error.
+					Ret = kEplSuccessful;
+				}
+			}
+
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventTimerStateMon:
+		{
+			// reset NMT state change flag
+			// because from now on the CN must have the correct NMT state
+			pNodeInfo->m_wFlags &=
+			    ~EPL_NMTMNU_NODE_FLAG_NMT_CMD_ISSUED;
+
+			// continue with normal StatReq processing
+		}
+
+	case kEplNmtMnuIntNodeEventTimerStatReq:
+		{
+			EPL_DBGLVL_NMTMN_TRACE1("TimerStatReq->StatReq(%02X)\n",
+						uiNodeId_p);
+			// request next StatusResponse
+			Ret =
+			    EplStatusuRequestStatusResponse(uiNodeId_p,
+							    EplNmtMnuCbStatusResponse);
+			if (Ret != kEplSuccessful) {
+				EPL_NMTMNU_DBG_POST_TRACE_VALUE(NodeEvent_p,
+								uiNodeId_p,
+								(((NodeNmtState_p & 0xFF) << 8)
+								 | Ret));
+				if (Ret == kEplInvalidOperation) {	// the only situation when this should happen is, when
+					// StatusResponse was already requested while processing
+					// event IdentResponse.
+					// so ignore this error.
+					Ret = kEplSuccessful;
+				}
+			}
+
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventTimerLonger:
+		{
+			switch (pNodeInfo->m_NodeState) {
+			case kEplNmtMnuNodeStateConfigured:
+				{	// node should be ReadyToOp but it is not
+
+					// check NMT state which shall be intentionally wrong, so that ERROR_TREATMENT will be started
+					Ret =
+					    EplNmtMnuCheckNmtState(uiNodeId_p,
+								   pNodeInfo,
+								   kEplNmtCsNotActive,
+								   EPL_E_NMT_BPO2,
+								   NmtState);
+					if (Ret != kEplSuccessful) {
+						if (Ret == kEplReject) {
+							Ret = kEplSuccessful;
+						}
+						break;
+					}
+
+					break;
+				}
+
+			case kEplNmtMnuNodeStateReadyToOp:
+				{	// CheckCom finished successfully
+
+					pNodeInfo->m_NodeState =
+					    kEplNmtMnuNodeStateComChecked;
+
+					if ((pNodeInfo->
+					     m_wFlags &
+					     EPL_NMTMNU_NODE_FLAG_NOT_SCANNED)
+					    != 0) {
+						// decrement only signal slave count if checked once for ReadyToOp, CheckCom, Operational
+						EplNmtMnuInstance_g.
+						    m_uiSignalSlaveCount--;
+						pNodeInfo->m_wFlags &=
+						    ~EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+					}
+
+					if ((pNodeInfo->
+					     m_dwNodeCfg &
+					     EPL_NODEASSIGN_MANDATORY_CN) !=
+					    0) {
+						// decrement mandatory slave counter
+						EplNmtMnuInstance_g.
+						    m_uiMandatorySlaveCount--;
+					}
+					if (NmtState != kEplNmtMsReadyToOperate) {
+						EPL_NMTMNU_DBG_POST_TRACE_VALUE
+						    (NodeEvent_p, uiNodeId_p,
+						     (((NodeNmtState_p & 0xFF)
+						       << 8)
+						      | kEplNmtCmdStartNode));
+
+						// start optional CN
+						Ret =
+						    EplNmtMnuSendNmtCommand
+						    (uiNodeId_p,
+						     kEplNmtCmdStartNode);
+					}
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+			}
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventNmtCmdSent:
+		{
+			BYTE bNmtState;
+
+			// update expected NMT state with the one that results
+			// from the sent NMT command
+			bNmtState = (BYTE) (NodeNmtState_p & 0xFF);
+
+			// write object 0x1F8F NMT_MNNodeExpState_AU8
+			Ret =
+			    EplObduWriteEntry(0x1F8F, uiNodeId_p, &bNmtState,
+					      1);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+
+			if (NodeNmtState_p == kEplNmtCsNotActive) {	// restart processing with IdentRequest
+				EPL_NMTMNU_SET_FLAGS_TIMERARG_IDENTREQ
+				    (pNodeInfo, uiNodeId_p, TimerArg);
+			} else {	// monitor NMT state change with StatusRequest after
+				// the corresponding delay;
+				// until then wrong NMT states will be ignored
+				EPL_NMTMNU_SET_FLAGS_TIMERARG_STATE_MON
+				    (pNodeInfo, uiNodeId_p, TimerArg);
+
+				// set NMT state change flag
+				pNodeInfo->m_wFlags |=
+				    EPL_NMTMNU_NODE_FLAG_NMT_CMD_ISSUED;
+			}
+
+			Ret =
+			    EplTimeruModifyTimerMs(&pNodeInfo->
+						   m_TimerHdlStatReq,
+						   EplNmtMnuInstance_g.
+						   m_ulStatusRequestDelay,
+						   TimerArg);
+
+			// finish processing, because NmtState_p is the expected and not the current state
+			goto Exit;
+		}
+
+	default:
+		{
+			break;
+		}
+	}
+
+	// check if network is ready to change local NMT state and this was not done before
+	if ((EplNmtMnuInstance_g.m_wFlags & (EPL_NMTMNU_FLAG_HALTED | EPL_NMTMNU_FLAG_APP_INFORMED)) == 0) {	// boot process is not halted
+		switch (NmtState) {
+		case kEplNmtMsPreOperational1:
+			{
+				if ((EplNmtMnuInstance_g.m_uiSignalSlaveCount ==
+				     0)
+				    && (EplNmtMnuInstance_g.m_uiMandatorySlaveCount == 0)) {	// all optional CNs scanned once and all mandatory CNs configured successfully
+					EplNmtMnuInstance_g.m_wFlags |=
+					    EPL_NMTMNU_FLAG_APP_INFORMED;
+					// inform application
+					Ret =
+					    EplNmtMnuInstance_g.
+					    m_pfnCbBootEvent
+					    (kEplNmtBootEventBootStep1Finish,
+					     NmtState, EPL_E_NO_ERROR);
+					if (Ret != kEplSuccessful) {
+						if (Ret == kEplReject) {
+							// wait for application
+							Ret = kEplSuccessful;
+						}
+						break;
+					}
+					// enter PreOp2
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventAllMandatoryCNIdent);
+				}
+				break;
+			}
+
+		case kEplNmtMsPreOperational2:
+			{
+				if ((EplNmtMnuInstance_g.m_uiSignalSlaveCount ==
+				     0)
+				    && (EplNmtMnuInstance_g.m_uiMandatorySlaveCount == 0)) {	// all optional CNs checked once for ReadyToOp and all mandatory CNs are ReadyToOp
+					EplNmtMnuInstance_g.m_wFlags |=
+					    EPL_NMTMNU_FLAG_APP_INFORMED;
+					// inform application
+					Ret =
+					    EplNmtMnuInstance_g.
+					    m_pfnCbBootEvent
+					    (kEplNmtBootEventBootStep2Finish,
+					     NmtState, EPL_E_NO_ERROR);
+					if (Ret != kEplSuccessful) {
+						if (Ret == kEplReject) {
+							// wait for application
+							Ret = kEplSuccessful;
+						}
+						break;
+					}
+					// enter ReadyToOp
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventEnterReadyToOperate);
+				}
+				break;
+			}
+
+		case kEplNmtMsReadyToOperate:
+			{
+				if ((EplNmtMnuInstance_g.m_uiSignalSlaveCount ==
+				     0)
+				    && (EplNmtMnuInstance_g.m_uiMandatorySlaveCount == 0)) {	// all CNs checked for errorless communication
+					EplNmtMnuInstance_g.m_wFlags |=
+					    EPL_NMTMNU_FLAG_APP_INFORMED;
+					// inform application
+					Ret =
+					    EplNmtMnuInstance_g.
+					    m_pfnCbBootEvent
+					    (kEplNmtBootEventCheckComFinish,
+					     NmtState, EPL_E_NO_ERROR);
+					if (Ret != kEplSuccessful) {
+						if (Ret == kEplReject) {
+							// wait for application
+							Ret = kEplSuccessful;
+						}
+						break;
+					}
+					// enter Operational
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventEnterMsOperational);
+				}
+				break;
+			}
+
+		case kEplNmtMsOperational:
+			{
+				if ((EplNmtMnuInstance_g.m_uiSignalSlaveCount ==
+				     0)
+				    && (EplNmtMnuInstance_g.m_uiMandatorySlaveCount == 0)) {	// all optional CNs scanned once and all mandatory CNs are OPERATIONAL
+					EplNmtMnuInstance_g.m_wFlags |=
+					    EPL_NMTMNU_FLAG_APP_INFORMED;
+					// inform application
+					Ret =
+					    EplNmtMnuInstance_g.
+					    m_pfnCbBootEvent
+					    (kEplNmtBootEventOperational,
+					     NmtState, EPL_E_NO_ERROR);
+					if (Ret != kEplSuccessful) {
+						if (Ret == kEplReject) {
+							// ignore error code
+							Ret = kEplSuccessful;
+						}
+						break;
+					}
+				}
+				break;
+			}
+
+		default:
+			{
+				break;
+			}
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuCheckNmtState
+//
+// Description: checks the NMT state, i.e. evaluates it with object 0x1F8F
+//              NMT_MNNodeExpState_AU8 and updates object 0x1F8E
+//              NMT_MNNodeCurrState_AU8.
+//              It manipulates m_NodeState in internal node info structure.
+//
+// Parameters:  uiNodeId_p              = node ID
+//              NodeNmtState_p          = NMT state of CN
+//
+// Returns:     tEplKernel              = error code
+//                  kEplReject          = CN was in wrong state and has been reset
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuCheckNmtState(unsigned int uiNodeId_p,
+					 tEplNmtMnuNodeInfo * pNodeInfo_p,
+					 tEplNmtState NodeNmtState_p,
+					 WORD wErrorCode_p,
+					 tEplNmtState LocalNmtState_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplObdSize ObdSize;
+	BYTE bNmtState;
+	BYTE bNmtStatePrev;
+	tEplNmtState ExpNmtState;
+
+	ObdSize = 1;
+	// read object 0x1F8F NMT_MNNodeExpState_AU8
+	Ret = EplObduReadEntry(0x1F8F, uiNodeId_p, &bNmtState, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// compute expected NMT state
+	ExpNmtState = (tEplNmtState) (bNmtState | EPL_NMT_TYPE_CS);
+	// compute BYTE of current NMT state
+	bNmtState = ((BYTE) NodeNmtState_p & 0xFF);
+
+	if (ExpNmtState == kEplNmtCsNotActive) {	// ignore the current state, because the CN shall be not active
+		Ret = kEplReject;
+		goto Exit;
+	} else if ((ExpNmtState == kEplNmtCsPreOperational2)
+		   && (NodeNmtState_p == kEplNmtCsReadyToOperate)) {	// CN switched to ReadyToOp
+		// delete timer for timeout handling
+		Ret = EplTimeruDeleteTimer(&pNodeInfo_p->m_TimerHdlLonger);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		pNodeInfo_p->m_NodeState = kEplNmtMnuNodeStateReadyToOp;
+
+		// update object 0x1F8F NMT_MNNodeExpState_AU8 to ReadyToOp
+		Ret = EplObduWriteEntry(0x1F8F, uiNodeId_p, &bNmtState, 1);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		if ((pNodeInfo_p->m_dwNodeCfg & EPL_NODEASSIGN_MANDATORY_CN) != 0) {	// node is a mandatory CN -> decrement counter
+			EplNmtMnuInstance_g.m_uiMandatorySlaveCount--;
+		}
+		if (LocalNmtState_p >= kEplNmtMsReadyToOperate) {	// start procedure CheckCommunication for this node
+			Ret = EplNmtMnuNodeCheckCom(uiNodeId_p, pNodeInfo_p);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+
+			if ((LocalNmtState_p == kEplNmtMsOperational)
+			    && (pNodeInfo_p->m_NodeState ==
+				kEplNmtMnuNodeStateComChecked)) {
+				EPL_NMTMNU_DBG_POST_TRACE_VALUE(0, uiNodeId_p,
+								(((NodeNmtState_p & 0xFF) << 8)
+								 |
+								 kEplNmtCmdStartNode));
+
+				// immediately start optional CN, because communication is always OK (e.g. async-only CN)
+				Ret =
+				    EplNmtMnuSendNmtCommand(uiNodeId_p,
+							    kEplNmtCmdStartNode);
+				if (Ret != kEplSuccessful) {
+					goto Exit;
+				}
+			}
+		}
+
+	} else if ((ExpNmtState == kEplNmtCsReadyToOperate)
+		   && (NodeNmtState_p == kEplNmtCsOperational)) {	// CN switched to OPERATIONAL
+		pNodeInfo_p->m_NodeState = kEplNmtMnuNodeStateOperational;
+
+		if ((pNodeInfo_p->m_dwNodeCfg & EPL_NODEASSIGN_MANDATORY_CN) != 0) {	// node is a mandatory CN -> decrement counter
+			EplNmtMnuInstance_g.m_uiMandatorySlaveCount--;
+		}
+
+	} else if ((ExpNmtState != NodeNmtState_p)
+		   && !((ExpNmtState == kEplNmtCsPreOperational1)
+			&& (NodeNmtState_p == kEplNmtCsPreOperational2))) {	// CN is not in expected NMT state (without the exceptions above)
+		WORD wbeErrorCode;
+
+		if ((pNodeInfo_p->
+		     m_wFlags & EPL_NMTMNU_NODE_FLAG_NOT_SCANNED) != 0) {
+			// decrement only signal slave count if checked once
+			EplNmtMnuInstance_g.m_uiSignalSlaveCount--;
+			pNodeInfo_p->m_wFlags &=
+			    ~EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+		}
+
+		if (pNodeInfo_p->m_NodeState == kEplNmtMnuNodeStateUnknown) {	// CN is already in state unknown, which means that it got
+			// NMT reset command earlier
+			goto Exit;
+		}
+		// -> CN is in wrong NMT state
+		pNodeInfo_p->m_NodeState = kEplNmtMnuNodeStateUnknown;
+
+		if (wErrorCode_p == 0) {	// assume wrong NMT state error
+			if ((pNodeInfo_p->m_wFlags & EPL_NMTMNU_NODE_FLAG_NMT_CMD_ISSUED) != 0) {	// NMT command has been just issued;
+				// ignore wrong NMT state until timer expires;
+				// other errors like LOSS_PRES_TH are still processed
+				goto Exit;
+			}
+
+			wErrorCode_p = EPL_E_NMT_WRONG_STATE;
+		}
+
+		BENCHMARK_MOD_07_TOGGLE(9);
+
+		// $$$ start ERROR_TREATMENT and inform application
+		Ret = EplNmtMnuInstance_g.m_pfnCbNodeEvent(uiNodeId_p,
+							   kEplNmtNodeEventError,
+							   NodeNmtState_p,
+							   wErrorCode_p,
+							   (pNodeInfo_p->
+							    m_dwNodeCfg &
+							    EPL_NODEASSIGN_MANDATORY_CN)
+							   != 0);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		EPL_NMTMNU_DBG_POST_TRACE_VALUE(0,
+						uiNodeId_p,
+						(((NodeNmtState_p & 0xFF) << 8)
+						 | kEplNmtCmdResetNode));
+
+		// reset CN
+		// store error code in NMT command data for diagnostic purpose
+		AmiSetWordToLe(&wbeErrorCode, wErrorCode_p);
+		Ret =
+		    EplNmtMnuSendNmtCommandEx(uiNodeId_p, kEplNmtCmdResetNode,
+					      &wbeErrorCode,
+					      sizeof(wbeErrorCode));
+		if (Ret == kEplSuccessful) {
+			Ret = kEplReject;
+		}
+
+		goto Exit;
+	}
+	// check if NMT_MNNodeCurrState_AU8 has to be changed
+	ObdSize = 1;
+	Ret = EplObduReadEntry(0x1F8E, uiNodeId_p, &bNmtStatePrev, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	if (bNmtState != bNmtStatePrev) {
+		// update object 0x1F8E NMT_MNNodeCurrState_AU8
+		Ret = EplObduWriteEntry(0x1F8E, uiNodeId_p, &bNmtState, 1);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		Ret = EplNmtMnuInstance_g.m_pfnCbNodeEvent(uiNodeId_p,
+							   kEplNmtNodeEventNmtState,
+							   NodeNmtState_p,
+							   wErrorCode_p,
+							   (pNodeInfo_p->
+							    m_dwNodeCfg &
+							    EPL_NODEASSIGN_MANDATORY_CN)
+							   != 0);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuReset
+//
+// Description: reset internal structures, e.g. timers
+//
+// Parameters:  void
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuReset(void)
+{
+	tEplKernel Ret;
+	int iIndex;
+
+	Ret = EplTimeruDeleteTimer(&EplNmtMnuInstance_g.m_TimerHdlNmtState);
+
+	for (iIndex = 1; iIndex <= tabentries(EplNmtMnuInstance_g.m_aNodeInfo);
+	     iIndex++) {
+		// delete timer handles
+		Ret =
+		    EplTimeruDeleteTimer(&EPL_NMTMNU_GET_NODEINFO(iIndex)->
+					 m_TimerHdlStatReq);
+		Ret =
+		    EplTimeruDeleteTimer(&EPL_NMTMNU_GET_NODEINFO(iIndex)->
+					 m_TimerHdlLonger);
+	}
+
+	return Ret;
+}
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplNmtk.c b/drivers/staging/epl/EplNmtk.c
new file mode 100644
index 000000000000..4e2d0e15e947
--- /dev/null
+++ b/drivers/staging/epl/EplNmtk.c
@@ -0,0 +1,1842 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for NMT-Kernelspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtk.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.12 $  $Date: 2008/11/13 17:13:09 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "kernel/EplNmtk.h"
+#include "kernel/EplTimerk.h"
+
+#include "kernel/EplDllk.h"	// for EplDllkProcess()
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)     TgtDbgPostTraceValue(v)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)
+#endif
+#define EPL_NMTK_DBG_POST_TRACE_VALUE(NmtEvent_p, OldNmtState_p, NewNmtState_p) \
+    TGT_DBG_POST_TRACE_VALUE((kEplEventSinkNmtk << 28) | (NmtEvent_p << 16) \
+                             | ((OldNmtState_p & 0xFF) << 8) \
+                             | (NewNmtState_p & 0xFF))
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+// struct for instance table
+INSTANCE_TYPE_BEGIN EPL_MCO_DECL_INSTANCE_MEMBER()
+
+STATIC volatile tEplNmtState INST_FAR m_NmtState;
+STATIC volatile BOOL INST_FAR m_fEnableReadyToOperate;
+STATIC volatile BOOL INST_FAR m_fAppReadyToOperate;
+STATIC volatile BOOL INST_FAR m_fTimerMsPreOp2;
+STATIC volatile BOOL INST_FAR m_fAllMandatoryCNIdent;
+STATIC volatile BOOL INST_FAR m_fFrozen;
+
+INSTANCE_TYPE_END
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+// This macro replace the unspecific pointer to an instance through
+// the modul specific type for the local instance table. This macro
+// must defined in each modul.
+//#define tEplPtrInstance             tEplInstanceInfo MEM*
+EPL_MCO_DECL_INSTANCE_VAR()
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+EPL_MCO_DEFINE_INSTANCE_FCT()
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <NMT_Kernel-Module>                                 */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: This module realize the NMT-State-Machine of the EPL-Stack
+//
+//
+/***************************************************************************/
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+//---------------------------------------------------------------------------
+//
+// Function:        EplNmtkInit
+//
+// Description: initializes the first instance
+//
+//
+//
+// Parameters:  EPL_MCO_DECL_PTR_INSTANCE_PTR = Instance pointer
+//              uiNodeId_p = Node Id of the lokal node
+//
+//
+// Returns:     tEplKernel  =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtkInit(EPL_MCO_DECL_PTR_INSTANCE_PTR)
+{
+	tEplKernel Ret;
+
+	Ret = EplNmtkAddInstance(EPL_MCO_PTR_INSTANCE_PTR);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplNmtkAddInstance
+//
+// Description: adds a new instance
+//
+//
+//
+// Parameters:  EPL_MCO_DECL_PTR_INSTANCE_PTR = Instance pointer
+//
+//
+// Returns:     tEplKernel  =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtkAddInstance(EPL_MCO_DECL_PTR_INSTANCE_PTR)
+{
+	EPL_MCO_DECL_INSTANCE_PTR_LOCAL tEplKernel Ret;
+//tEplEvent               Event;
+//tEplEventNmtStateChange NmtStateChange;
+
+	// check if pointer to instance pointer valid
+	// get free instance and set the globale instance pointer
+	// set also the instance addr to parameterlist
+	EPL_MCO_CHECK_PTR_INSTANCE_PTR();
+	EPL_MCO_GET_FREE_INSTANCE_PTR();
+	EPL_MCO_SET_PTR_INSTANCE_PTR();
+
+	// sign instance as used
+	EPL_MCO_WRITE_INSTANCE_STATE(kStateUsed);
+
+	Ret = kEplSuccessful;
+
+	// initialize intern vaiables
+	// 2006/07/31 d.k.: set NMT-State to kEplNmtGsOff
+	EPL_MCO_GLB_VAR(m_NmtState) = kEplNmtGsOff;
+	// set NMT-State to kEplNmtGsInitialising
+	//EPL_MCO_GLB_VAR(m_NmtState) = kEplNmtGsInitialising;
+
+	// set flags to FALSE
+	EPL_MCO_GLB_VAR(m_fEnableReadyToOperate) = FALSE;
+	EPL_MCO_GLB_VAR(m_fAppReadyToOperate) = FALSE;
+	EPL_MCO_GLB_VAR(m_fTimerMsPreOp2) = FALSE;
+	EPL_MCO_GLB_VAR(m_fAllMandatoryCNIdent) = FALSE;
+	EPL_MCO_GLB_VAR(m_fFrozen) = FALSE;
+
+//    EPL_MCO_GLB_VAR(m_TimerHdl) = 0;
+
+	// inform higher layer about state change
+	// 2006/07/31 d.k.: The EPL API layer/application has to start NMT state
+	//                  machine via NmtEventSwReset after initialisation of
+	//                  all modules has been completed. DLL has to be initialised
+	//                  after NMTk because NMT state shall not be uninitialised
+	//                  at that time.
+/*    NmtStateChange.m_NewNmtState = EPL_MCO_GLB_VAR(m_NmtState);
+    NmtStateChange.m_NmtEvent = kEplNmtEventNoEvent;
+    Event.m_EventSink = kEplEventSinkNmtu;
+    Event.m_EventType = kEplEventTypeNmtStateChange;
+    EPL_MEMSET(&Event.m_NetTime, 0x00, sizeof(Event.m_NetTime));
+    Event.m_pArg = &NmtStateChange;
+    Event.m_uiSize = sizeof(NmtStateChange);
+    Ret = EplEventkPost(&Event);
+*/
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplNmtkDelInstance
+//
+// Description: delete instance
+//
+//
+//
+// Parameters:  EPL_MCO_DECL_PTR_INSTANCE_PTR = Instance pointer
+//
+//
+// Returns:     tEplKernel  =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if (EPL_USE_DELETEINST_FUNC != FALSE)
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtkDelInstance(EPL_MCO_DECL_PTR_INSTANCE_PTR)
+{
+	tEplKernel Ret = kEplSuccessful;
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	// set NMT-State to kEplNmtGsOff
+	EPL_MCO_GLB_VAR(m_NmtState) = kEplNmtGsOff;
+
+	// sign instance as unused
+	EPL_MCO_WRITE_INSTANCE_STATE(kStateUnused);
+
+	// delete timer
+//    Ret = EplTimerkDeleteTimer(&EPL_MCO_GLB_VAR(m_TimerHdl));
+
+	return Ret;
+}
+#endif // (EPL_USE_DELETEINST_FUNC != FALSE)
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplNmtkProcess
+//
+// Description: main process function
+//              -> process NMT-State-Maschine und read NMT-Events from Queue
+//
+//
+//
+// Parameters:  EPL_MCO_DECL_PTR_INSTANCE_PTR_ = Instance pointer
+//              pEvent_p    =   Epl-Event with NMT-event to process
+//
+//
+// Returns:     tEplKernel  =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtkProcess(EPL_MCO_DECL_PTR_INSTANCE_PTR_
+					      tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+	tEplNmtState OldNmtState;
+	tEplNmtEvent NmtEvent;
+	tEplEvent Event;
+	tEplEventNmtStateChange NmtStateChange;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	Ret = kEplSuccessful;
+
+	switch (pEvent_p->m_EventType) {
+	case kEplEventTypeNmtEvent:
+		{
+			NmtEvent = *((tEplNmtEvent *) pEvent_p->m_pArg);
+			break;
+		}
+
+	case kEplEventTypeTimer:
+		{
+			NmtEvent =
+			    (tEplNmtEvent) ((tEplTimerEventArg *) pEvent_p->
+					    m_pArg)->m_ulArg;
+			break;
+		}
+	default:
+		{
+			Ret = kEplNmtInvalidEvent;
+			goto Exit;
+		}
+	}
+
+	// save NMT-State
+	// needed for later comparison to
+	// inform hgher layer about state change
+	OldNmtState = EPL_MCO_GLB_VAR(m_NmtState);
+
+	// NMT-State-Maschine
+	switch (EPL_MCO_GLB_VAR(m_NmtState)) {
+		//-----------------------------------------------------------
+		// general part of the statemaschine
+
+		// first init of the hardware
+	case kEplNmtGsOff:
+		{
+			// leave this state only if higher layer says so
+			if (NmtEvent == kEplNmtEventSwReset) {	// new state kEplNmtGsInitialising
+				EPL_MCO_GLB_VAR(m_NmtState) =
+				    kEplNmtGsInitialising;
+			}
+			break;
+		}
+
+		// first init of the hardware
+	case kEplNmtGsInitialising:
+		{
+			// leave this state only if higher layer says so
+
+			// check events
+			switch (NmtEvent) {
+				// 2006/07/31 d.k.: react also on NMT reset commands in ResetApp state
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// new state kEplNmtGsResetApplication
+			case kEplNmtEventEnterResetApp:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+			}
+			break;
+		}
+
+		// init of the manufacturer-specific profile area and the
+		// standardised device profile area
+	case kEplNmtGsResetApplication:
+		{
+			// check events
+			switch (NmtEvent) {
+				// 2006/07/31 d.k.: react also on NMT reset commands in ResetApp state
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// leave this state only if higher layer
+				// say so
+			case kEplNmtEventEnterResetCom:
+				{
+					// new state kEplNmtGsResetCommunication
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+			}
+			break;
+		}
+
+		// init of the communication profile area
+	case kEplNmtGsResetCommunication:
+		{
+			// check events
+			switch (NmtEvent) {
+				// 2006/07/31 d.k.: react also on NMT reset commands in ResetComm state
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// leave this state only if higher layer
+				// say so
+			case kEplNmtEventEnterResetConfig:
+				{
+					// new state kEplNmtGsResetCommunication
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+			}
+			break;
+		}
+
+		// build the configuration with infos from OD
+	case kEplNmtGsResetConfiguration:
+		{
+			// reset flags
+			EPL_MCO_GLB_VAR(m_fEnableReadyToOperate) = FALSE;
+			EPL_MCO_GLB_VAR(m_fAppReadyToOperate) = FALSE;
+			EPL_MCO_GLB_VAR(m_fFrozen) = FALSE;
+
+			// check events
+			switch (NmtEvent) {
+				// 2006/07/31 d.k.: react also on NMT reset commands in ResetConf state
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+			case kEplNmtEventResetCom:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// leave this state only if higher layer says so
+			case kEplNmtEventEnterCsNotActive:
+				{	// Node should be CN
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsNotActive;
+					break;
+
+				}
+
+			case kEplNmtEventEnterMsNotActive:
+				{	// Node should be CN
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) == 0)
+					// no MN functionality
+					// TODO: -create error E_NMT_BA1_NO_MN_SUPPORT
+					EPL_MCO_GLB_VAR(m_fFrozen) = TRUE;
+#else
+
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtMsNotActive;
+#endif
+					break;
+
+				}
+
+			default:
+				{
+					break;
+				}
+			}
+			break;
+		}
+
+		//-----------------------------------------------------------
+		// CN part of the statemaschine
+
+		// node liste for EPL-Frames and check timeout
+	case kEplNmtCsNotActive:
+		{
+
+			// check events
+			switch (NmtEvent) {
+				// 2006/07/31 d.k.: react also on NMT reset commands in NotActive state
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+//                    Ret = EplTimerkDeleteTimer(&EPL_MCO_GLB_VAR(m_TimerHdl));
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+//                    Ret = EplTimerkDeleteTimer(&EPL_MCO_GLB_VAR(m_TimerHdl));
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+//                    Ret = EplTimerkDeleteTimer(&EPL_MCO_GLB_VAR(m_TimerHdl));
+					break;
+				}
+
+				// NMT Command Reset Configuration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+//                    Ret = EplTimerkDeleteTimer(&EPL_MCO_GLB_VAR(m_TimerHdl));
+					break;
+				}
+
+				// see if SoA or SoC received
+				// k.t. 20.07.2006: only SoA forces change of state
+				// see EPL V2 DS 1.0.0 p.267
+				// case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeSoa:
+				{	// new state PRE_OPERATIONAL1
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational1;
+//                    Ret = EplTimerkDeleteTimer(&EPL_MCO_GLB_VAR(m_TimerHdl));
+					break;
+				}
+				// timeout for SoA and Soc
+			case kEplNmtEventTimerBasicEthernet:
+				{
+					// new state BASIC_ETHERNET
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsBasicEthernet;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+			}	// end of switch(NmtEvent)
+
+			break;
+		}
+
+		// node processes only async frames
+	case kEplNmtCsPreOperational1:
+		{
+
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command Reset Configuration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// NMT Command StopNode
+			case kEplNmtEventStopNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsStopped;
+					break;
+				}
+
+				// check if SoC received
+			case kEplNmtEventDllCeSoc:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational2;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+
+			break;
+		}
+
+		// node processes isochronous and asynchronous frames
+	case kEplNmtCsPreOperational2:
+		{
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command Reset Configuration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// NMT Command StopNode
+			case kEplNmtEventStopNode:
+				{
+					// reset flags
+					EPL_MCO_GLB_VAR(m_fEnableReadyToOperate)
+					    = FALSE;
+					EPL_MCO_GLB_VAR(m_fAppReadyToOperate) =
+					    FALSE;
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsStopped;
+					break;
+				}
+
+				// error occured
+			case kEplNmtEventNmtCycleError:
+				{
+					// reset flags
+					EPL_MCO_GLB_VAR(m_fEnableReadyToOperate)
+					    = FALSE;
+					EPL_MCO_GLB_VAR(m_fAppReadyToOperate) =
+					    FALSE;
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational1;
+					break;
+				}
+
+				// check if application is ready to operate
+			case kEplNmtEventEnterReadyToOperate:
+				{
+					// check if command NMTEnableReadyToOperate from MN was received
+					if (EPL_MCO_GLB_VAR(m_fEnableReadyToOperate) == TRUE) {	// reset flags
+						EPL_MCO_GLB_VAR
+						    (m_fEnableReadyToOperate) =
+						    FALSE;
+						EPL_MCO_GLB_VAR
+						    (m_fAppReadyToOperate) =
+						    FALSE;
+						// change state
+						EPL_MCO_GLB_VAR(m_NmtState) =
+						    kEplNmtCsReadyToOperate;
+					} else {	// set Flag
+						EPL_MCO_GLB_VAR
+						    (m_fAppReadyToOperate) =
+						    TRUE;
+					}
+					break;
+				}
+
+				// NMT Commando EnableReadyToOperate
+			case kEplNmtEventEnableReadyToOperate:
+				{
+					// check if application is ready
+					if (EPL_MCO_GLB_VAR(m_fAppReadyToOperate) == TRUE) {	// reset flags
+						EPL_MCO_GLB_VAR
+						    (m_fEnableReadyToOperate) =
+						    FALSE;
+						EPL_MCO_GLB_VAR
+						    (m_fAppReadyToOperate) =
+						    FALSE;
+						// change state
+						EPL_MCO_GLB_VAR(m_NmtState) =
+						    kEplNmtCsReadyToOperate;
+					} else {	// set Flag
+						EPL_MCO_GLB_VAR
+						    (m_fEnableReadyToOperate) =
+						    TRUE;
+					}
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+			break;
+		}
+
+		// node should be configured und application is ready
+	case kEplNmtCsReadyToOperate:
+		{
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// NMT Command StopNode
+			case kEplNmtEventStopNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsStopped;
+					break;
+				}
+
+				// error occured
+			case kEplNmtEventNmtCycleError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational1;
+					break;
+				}
+
+				// NMT Command StartNode
+			case kEplNmtEventStartNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsOperational;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+			break;
+		}
+
+		// normal work state
+	case kEplNmtCsOperational:
+		{
+
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// NMT Command StopNode
+			case kEplNmtEventStopNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsStopped;
+					break;
+				}
+
+				// NMT Command EnterPreOperational2
+			case kEplNmtEventEnterPreOperational2:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational2;
+					break;
+				}
+
+				// error occured
+			case kEplNmtEventNmtCycleError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational1;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+			break;
+		}
+
+		// node stopped by MN
+		// -> only process asynchronous frames
+	case kEplNmtCsStopped:
+		{
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// NMT Command EnterPreOperational2
+			case kEplNmtEventEnterPreOperational2:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational2;
+					break;
+				}
+
+				// error occured
+			case kEplNmtEventNmtCycleError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational1;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+			break;
+		}
+
+		// no epl cycle
+		// -> normal ethernet communication
+	case kEplNmtCsBasicEthernet:
+		{
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// error occured
+				// d.k.: how does this error occur? on CRC errors
+/*                case kEplNmtEventNmtCycleError:
+                {
+                    EPL_MCO_GLB_VAR(m_NmtState) = kEplNmtCsPreOperational1;
+                    break;
+                }
+*/
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCePreq:
+			case kEplNmtEventDllCePres:
+			case kEplNmtEventDllCeSoa:
+				{	// Epl-Frame on net -> stop any communication
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational1;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+
+			break;
+		}
+
+		//-----------------------------------------------------------
+		// MN part of the statemaschine
+
+		// MN listen to network
+		// -> if no EPL traffic go to next state
+	case kEplNmtMsNotActive:
+		{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) == 0)
+			// no MN functionality
+			// TODO: -create error E_NMT_BA1_NO_MN_SUPPORT
+			EPL_MCO_GLB_VAR(m_fFrozen) = TRUE;
+#else
+
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// EPL frames received
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeSoa:
+				{	// other MN in network
+					// $$$ d.k.: generate error history entry
+					EPL_MCO_GLB_VAR(m_fFrozen) = TRUE;
+					break;
+				}
+
+				// timeout event
+			case kEplNmtEventTimerBasicEthernet:
+				{
+					if (EPL_MCO_GLB_VAR(m_fFrozen) == FALSE) {	// new state BasicEthernet
+						EPL_MCO_GLB_VAR(m_NmtState) =
+						    kEplNmtMsBasicEthernet;
+					}
+					break;
+				}
+
+				// timeout event
+			case kEplNmtEventTimerMsPreOp1:
+				{
+					if (EPL_MCO_GLB_VAR(m_fFrozen) == FALSE) {	// new state PreOp1
+						EPL_MCO_GLB_VAR(m_NmtState) =
+						    kEplNmtMsPreOperational1;
+						EPL_MCO_GLB_VAR
+						    (m_fTimerMsPreOp2) = FALSE;
+						EPL_MCO_GLB_VAR
+						    (m_fAllMandatoryCNIdent) =
+						    FALSE;
+
+					}
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+
+#endif // ((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) == 0)
+
+			break;
+		}
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+		// MN process reduces epl cycle
+	case kEplNmtMsPreOperational1:
+		{
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// EPL frames received
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeSoa:
+				{	// other MN in network
+					// $$$ d.k.: generate error history entry
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// error occured
+				// d.k. MSPreOp1->CSPreOp1: nonsense -> keep state
+				/*
+				   case kEplNmtEventNmtCycleError:
+				   {
+				   EPL_MCO_GLB_VAR(m_NmtState) = kEplNmtCsPreOperational1;
+				   break;
+				   }
+				 */
+
+			case kEplNmtEventAllMandatoryCNIdent:
+				{	// all mandatory CN identified
+					if (EPL_MCO_GLB_VAR(m_fTimerMsPreOp2) !=
+					    FALSE) {
+						EPL_MCO_GLB_VAR(m_NmtState) =
+						    kEplNmtMsPreOperational2;
+					} else {
+						EPL_MCO_GLB_VAR
+						    (m_fAllMandatoryCNIdent) =
+						    TRUE;
+					}
+					break;
+				}
+
+			case kEplNmtEventTimerMsPreOp2:
+				{	// residence time for PreOp1 is elapsed
+					if (EPL_MCO_GLB_VAR
+					    (m_fAllMandatoryCNIdent) != FALSE) {
+						EPL_MCO_GLB_VAR(m_NmtState) =
+						    kEplNmtMsPreOperational2;
+					} else {
+						EPL_MCO_GLB_VAR
+						    (m_fTimerMsPreOp2) = TRUE;
+					}
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+			break;
+		}
+
+		// MN process full epl cycle
+	case kEplNmtMsPreOperational2:
+		{
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// EPL frames received
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeSoa:
+				{	// other MN in network
+					// $$$ d.k.: generate error history entry
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// error occured
+			case kEplNmtEventNmtCycleError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtMsPreOperational1;
+					break;
+				}
+
+			case kEplNmtEventEnterReadyToOperate:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtMsReadyToOperate;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+
+			break;
+		}
+
+		// all madatory nodes ready to operate
+		// -> MN process full epl cycle
+	case kEplNmtMsReadyToOperate:
+		{
+
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// EPL frames received
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeSoa:
+				{	// other MN in network
+					// $$$ d.k.: generate error history entry
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// error occured
+			case kEplNmtEventNmtCycleError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtMsPreOperational1;
+					break;
+				}
+
+			case kEplNmtEventEnterMsOperational:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtMsOperational;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+
+			break;
+		}
+
+		// normal eplcycle processing
+	case kEplNmtMsOperational:
+		{
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// EPL frames received
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeSoa:
+				{	// other MN in network
+					// $$$ d.k.: generate error history entry
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// error occured
+			case kEplNmtEventNmtCycleError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtMsPreOperational1;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+			break;
+		}
+
+		//  normal ethernet traffic
+	case kEplNmtMsBasicEthernet:
+		{
+
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// EPL frames received
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeSoa:
+				{	// other MN in network
+					// $$$ d.k.: generate error history entry
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// error occured
+				// d.k. BE->PreOp1 on cycle error? No
+/*                case kEplNmtEventNmtCycleError:
+                {
+                    EPL_MCO_GLB_VAR(m_NmtState) = kEplNmtCsPreOperational1;
+                    break;
+                }
+*/
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+			break;
+		}
+#endif //#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+	default:
+		{
+			//DEBUG_EPL_DBGLVL_NMTK_TRACE0(EPL_DBGLVL_NMT ,"Error in EplNmtProcess: Unknown NMT-State");
+			//EPL_MCO_GLB_VAR(m_NmtState) = kEplNmtGsResetApplication;
+			Ret = kEplNmtInvalidState;
+			goto Exit;
+		}
+
+	}			// end of switch(NmtEvent)
+
+	// inform higher layer about State-Change if needed
+	if (OldNmtState != EPL_MCO_GLB_VAR(m_NmtState)) {
+		EPL_NMTK_DBG_POST_TRACE_VALUE(NmtEvent, OldNmtState,
+					      EPL_MCO_GLB_VAR(m_NmtState));
+
+		// d.k.: memorize NMT state before posting any events
+		NmtStateChange.m_NewNmtState = EPL_MCO_GLB_VAR(m_NmtState);
+
+		// inform DLL
+		if ((OldNmtState > kEplNmtGsResetConfiguration)
+		    && (EPL_MCO_GLB_VAR(m_NmtState) <=
+			kEplNmtGsResetConfiguration)) {
+			// send DLL DEINIT
+			Event.m_EventSink = kEplEventSinkDllk;
+			Event.m_EventType = kEplEventTypeDllkDestroy;
+			EPL_MEMSET(&Event.m_NetTime, 0x00,
+				   sizeof(Event.m_NetTime));
+			Event.m_pArg = &OldNmtState;
+			Event.m_uiSize = sizeof(OldNmtState);
+			// d.k.: directly call DLLk process function, because
+			//       1. execution of process function is still synchonized and serialized,
+			//       2. it is the same as without event queues (i.e. well tested),
+			//       3. DLLk will get those necessary events even if event queue is full,
+			//       4. event queue is very inefficient
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+			Ret = EplDllkProcess(&Event);
+#else
+			Ret = EplEventkPost(&Event);
+#endif
+		} else if ((OldNmtState <= kEplNmtGsResetConfiguration)
+			   && (EPL_MCO_GLB_VAR(m_NmtState) >
+			       kEplNmtGsResetConfiguration)) {
+			// send DLL INIT
+			Event.m_EventSink = kEplEventSinkDllk;
+			Event.m_EventType = kEplEventTypeDllkCreate;
+			EPL_MEMSET(&Event.m_NetTime, 0x00,
+				   sizeof(Event.m_NetTime));
+			Event.m_pArg = &NmtStateChange.m_NewNmtState;
+			Event.m_uiSize = sizeof(NmtStateChange.m_NewNmtState);
+			// d.k.: directly call DLLk process function, because
+			//       1. execution of process function is still synchonized and serialized,
+			//       2. it is the same as without event queues (i.e. well tested),
+			//       3. DLLk will get those necessary events even if event queue is full
+			//       4. event queue is very inefficient
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+			Ret = EplDllkProcess(&Event);
+#else
+			Ret = EplEventkPost(&Event);
+#endif
+		} else
+		    if ((EPL_MCO_GLB_VAR(m_NmtState) == kEplNmtCsBasicEthernet)
+			|| (EPL_MCO_GLB_VAR(m_NmtState) ==
+			    kEplNmtMsBasicEthernet)) {
+			tEplDllAsyncReqPriority AsyncReqPriority;
+
+			// send DLL Fill Async Tx Buffer, because state BasicEthernet was entered
+			Event.m_EventSink = kEplEventSinkDllk;
+			Event.m_EventType = kEplEventTypeDllkFillTx;
+			EPL_MEMSET(&Event.m_NetTime, 0x00,
+				   sizeof(Event.m_NetTime));
+			AsyncReqPriority = kEplDllAsyncReqPrioGeneric;
+			Event.m_pArg = &AsyncReqPriority;
+			Event.m_uiSize = sizeof(AsyncReqPriority);
+			// d.k.: directly call DLLk process function, because
+			//       1. execution of process function is still synchonized and serialized,
+			//       2. it is the same as without event queues (i.e. well tested),
+			//       3. DLLk will get those necessary events even if event queue is full
+			//       4. event queue is very inefficient
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+			Ret = EplDllkProcess(&Event);
+#else
+			Ret = EplEventkPost(&Event);
+#endif
+		}
+		// inform higher layer about state change
+		NmtStateChange.m_NmtEvent = NmtEvent;
+		Event.m_EventSink = kEplEventSinkNmtu;
+		Event.m_EventType = kEplEventTypeNmtStateChange;
+		EPL_MEMSET(&Event.m_NetTime, 0x00, sizeof(Event.m_NetTime));
+		Event.m_pArg = &NmtStateChange;
+		Event.m_uiSize = sizeof(NmtStateChange);
+		Ret = EplEventkPost(&Event);
+		EPL_DBGLVL_NMTK_TRACE2
+		    ("EplNmtkProcess(NMT-Event = 0x%04X): New NMT-State = 0x%03X\n",
+		     NmtEvent, NmtStateChange.m_NewNmtState);
+
+	}
+
+      Exit:
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtkGetNmtState
+//
+// Description: return the actuell NMT-State and the bits
+//              to for MN- or CN-mode
+//
+//
+//
+// Parameters:  EPL_MCO_DECL_PTR_INSTANCE_PTR_ = Instancepointer
+//
+//
+// Returns:     tEplNmtState = NMT-State
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplNmtState PUBLIC
+EplNmtkGetNmtState(EPL_MCO_DECL_PTR_INSTANCE_PTR)
+{
+	tEplNmtState NmtState;
+
+	NmtState = EPL_MCO_GLB_VAR(m_NmtState);
+
+	return NmtState;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+EPL_MCO_DECL_INSTANCE_FCT()
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+// EOF
diff --git a/drivers/staging/epl/EplNmtkCal.c b/drivers/staging/epl/EplNmtkCal.c
new file mode 100644
index 000000000000..4ad71a71e013
--- /dev/null
+++ b/drivers/staging/epl/EplNmtkCal.c
@@ -0,0 +1,149 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for communication abstraction layer of the
+                NMT-Kernel-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtkCal.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/16 -k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "kernel/EplNmtkCal.h"
+
+// TODO: init function needed to prepare EplNmtkGetNmtState for
+//       io-controll-call from EplNmtuCal-Modul
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+// EOF
diff --git a/drivers/staging/epl/EplNmtu.c b/drivers/staging/epl/EplNmtu.c
new file mode 100644
index 000000000000..3de16a1eff5e
--- /dev/null
+++ b/drivers/staging/epl/EplNmtu.c
@@ -0,0 +1,708 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for NMT-Userspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/11/10 17:17:42 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "user/EplNmtu.h"
+#include "user/EplObdu.h"
+#include "user/EplTimeru.h"
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+#include "kernel/EplNmtk.h"
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	tEplNmtuStateChangeCallback m_pfnNmtChangeCb;
+	tEplTimerHdl m_TimerHdl;
+
+} tEplNmtuInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+static tEplNmtuInstance EplNmtuInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuInit
+//
+// Description: init first instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuInit()
+{
+	tEplKernel Ret;
+
+	Ret = EplNmtuAddInstance();
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuAddInstance
+//
+// Description: init other instances of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuAddInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	EplNmtuInstance_g.m_pfnNmtChangeCb = NULL;
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuDelInstance
+//
+// Description: delete instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	EplNmtuInstance_g.m_pfnNmtChangeCb = NULL;
+
+	// delete timer
+	Ret = EplTimeruDeleteTimer(&EplNmtuInstance_g.m_TimerHdl);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuNmtEvent
+//
+// Description: sends the NMT-Event to the NMT-State-Maschine
+//
+//
+//
+// Parameters:  NmtEvent_p  = NMT-Event to send
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuNmtEvent(tEplNmtEvent NmtEvent_p)
+{
+	tEplKernel Ret;
+	tEplEvent Event;
+
+	Event.m_EventSink = kEplEventSinkNmtk;
+	Event.m_NetTime.m_dwNanoSec = 0;
+	Event.m_NetTime.m_dwSec = 0;
+	Event.m_EventType = kEplEventTypeNmtEvent;
+	Event.m_pArg = &NmtEvent_p;
+	Event.m_uiSize = sizeof(NmtEvent_p);
+
+	Ret = EplEventuPost(&Event);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuGetNmtState
+//
+// Description: returns the actuell NMT-State
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplNmtState  = NMT-State
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplNmtState PUBLIC EplNmtuGetNmtState()
+{
+	tEplNmtState NmtState;
+
+	// $$$ call function of communication abstraction layer
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+	NmtState = EplNmtkGetNmtState();
+#else
+	NmtState = 0;
+#endif
+
+	return NmtState;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuProcessEvent
+//
+// Description: processes events from event queue
+//
+//
+//
+// Parameters:  pEplEvent_p =   pointer to event
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuProcessEvent(tEplEvent * pEplEvent_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// process event
+	switch (pEplEvent_p->m_EventType) {
+		// state change of NMT-Module
+	case kEplEventTypeNmtStateChange:
+		{
+			tEplEventNmtStateChange *pNmtStateChange;
+
+			// delete timer
+			Ret =
+			    EplTimeruDeleteTimer(&EplNmtuInstance_g.m_TimerHdl);
+
+			pNmtStateChange =
+			    (tEplEventNmtStateChange *) pEplEvent_p->m_pArg;
+
+			// call cb-functions to inform higher layer
+			if (EplNmtuInstance_g.m_pfnNmtChangeCb != NULL) {
+				Ret =
+				    EplNmtuInstance_g.
+				    m_pfnNmtChangeCb(*pNmtStateChange);
+			}
+
+			if (Ret == kEplSuccessful) {	// everything is OK, so switch to next state if necessary
+				switch (pNmtStateChange->m_NewNmtState) {
+					// EPL stack is not running
+				case kEplNmtGsOff:
+					break;
+
+					// first init of the hardware
+				case kEplNmtGsInitialising:
+					{
+						Ret =
+						    EplNmtuNmtEvent
+						    (kEplNmtEventEnterResetApp);
+						break;
+					}
+
+					// init of the manufacturer-specific profile area and the
+					// standardised device profile area
+				case kEplNmtGsResetApplication:
+					{
+						Ret =
+						    EplNmtuNmtEvent
+						    (kEplNmtEventEnterResetCom);
+						break;
+					}
+
+					// init of the communication profile area
+				case kEplNmtGsResetCommunication:
+					{
+						Ret =
+						    EplNmtuNmtEvent
+						    (kEplNmtEventEnterResetConfig);
+						break;
+					}
+
+					// build the configuration with infos from OD
+				case kEplNmtGsResetConfiguration:
+					{
+						unsigned int uiNodeId;
+
+						// get node ID from OD
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0) || (EPL_OBD_USE_KERNEL != FALSE)
+						uiNodeId =
+						    EplObduGetNodeId
+						    (EPL_MCO_PTR_INSTANCE_PTR);
+#else
+						uiNodeId = 0;
+#endif
+						//check node ID if not should be master or slave
+						if (uiNodeId == EPL_C_ADR_MN_DEF_NODE_ID) {	// node shall be MN
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+							Ret =
+							    EplNmtuNmtEvent
+							    (kEplNmtEventEnterMsNotActive);
+#else
+							TRACE0
+							    ("EplNmtuProcess(): no MN functionality implemented\n");
+#endif
+						} else {	// node shall be CN
+							Ret =
+							    EplNmtuNmtEvent
+							    (kEplNmtEventEnterCsNotActive);
+						}
+						break;
+					}
+
+					//-----------------------------------------------------------
+					// CN part of the state machine
+
+					// node listens for EPL-Frames and check timeout
+				case kEplNmtCsNotActive:
+					{
+						DWORD dwBuffer;
+						tEplObdSize ObdSize;
+						tEplTimerArg TimerArg;
+
+						// create timer to switch automatically to BasicEthernet if no MN available in network
+
+						// read NMT_CNBasicEthernetTimerout_U32 from OD
+						ObdSize = sizeof(dwBuffer);
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0) || (EPL_OBD_USE_KERNEL != FALSE)
+						Ret =
+						    EplObduReadEntry
+						    (EPL_MCO_PTR_INSTANCE_PTR_
+						     0x1F99, 0x00, &dwBuffer,
+						     &ObdSize);
+#else
+						Ret = kEplObdIndexNotExist;
+#endif
+						if (Ret != kEplSuccessful) {
+							break;
+						}
+						if (dwBuffer != 0) {	// BasicEthernet is enabled
+							// convert us into ms
+							dwBuffer =
+							    dwBuffer / 1000;
+							if (dwBuffer == 0) {	// timer was below one ms
+								// set one ms
+								dwBuffer = 1;
+							}
+							TimerArg.m_EventSink =
+							    kEplEventSinkNmtk;
+							TimerArg.m_ulArg =
+							    (unsigned long)
+							    kEplNmtEventTimerBasicEthernet;
+							Ret =
+							    EplTimeruModifyTimerMs
+							    (&EplNmtuInstance_g.
+							     m_TimerHdl,
+							     (unsigned long)
+							     dwBuffer,
+							     TimerArg);
+							// potential error is forwarded to event queue which generates error event
+						}
+						break;
+					}
+
+					// node processes only async frames
+				case kEplNmtCsPreOperational1:
+					{
+						break;
+					}
+
+					// node processes isochronous and asynchronous frames
+				case kEplNmtCsPreOperational2:
+					{
+						Ret =
+						    EplNmtuNmtEvent
+						    (kEplNmtEventEnterReadyToOperate);
+						break;
+					}
+
+					// node should be configured und application is ready
+				case kEplNmtCsReadyToOperate:
+					{
+						break;
+					}
+
+					// normal work state
+				case kEplNmtCsOperational:
+					{
+						break;
+					}
+
+					// node stopped by MN
+					// -> only process asynchronous frames
+				case kEplNmtCsStopped:
+					{
+						break;
+					}
+
+					// no EPL cycle
+					// -> normal ethernet communication
+				case kEplNmtCsBasicEthernet:
+					{
+						break;
+					}
+
+					//-----------------------------------------------------------
+					// MN part of the state machine
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+					// node listens for EPL-Frames and check timeout
+				case kEplNmtMsNotActive:
+					{
+						DWORD dwBuffer;
+						tEplObdSize ObdSize;
+						tEplTimerArg TimerArg;
+
+						// create timer to switch automatically to BasicEthernet/PreOp1 if no other MN active in network
+
+						// check NMT_StartUp_U32.Bit13
+						// read NMT_StartUp_U32 from OD
+						ObdSize = sizeof(dwBuffer);
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0) || (EPL_OBD_USE_KERNEL != FALSE)
+						Ret =
+						    EplObduReadEntry
+						    (EPL_MCO_PTR_INSTANCE_PTR_
+						     0x1F80, 0x00, &dwBuffer,
+						     &ObdSize);
+#else
+						Ret = kEplObdIndexNotExist;
+#endif
+						if (Ret != kEplSuccessful) {
+							break;
+						}
+
+						if ((dwBuffer & EPL_NMTST_BASICETHERNET) == 0) {	// NMT_StartUp_U32.Bit13 == 0
+							// new state PreOperational1
+							TimerArg.m_ulArg =
+							    (unsigned long)
+							    kEplNmtEventTimerMsPreOp1;
+						} else {	// NMT_StartUp_U32.Bit13 == 1
+							// new state BasicEthernet
+							TimerArg.m_ulArg =
+							    (unsigned long)
+							    kEplNmtEventTimerBasicEthernet;
+						}
+
+						// read NMT_BootTime_REC.MNWaitNotAct_U32 from OD
+						ObdSize = sizeof(dwBuffer);
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0) || (EPL_OBD_USE_KERNEL != FALSE)
+						Ret =
+						    EplObduReadEntry
+						    (EPL_MCO_PTR_INSTANCE_PTR_
+						     0x1F89, 0x01, &dwBuffer,
+						     &ObdSize);
+#else
+						Ret = kEplObdIndexNotExist;
+#endif
+						if (Ret != kEplSuccessful) {
+							break;
+						}
+						// convert us into ms
+						dwBuffer = dwBuffer / 1000;
+						if (dwBuffer == 0) {	// timer was below one ms
+							// set one ms
+							dwBuffer = 1;
+						}
+						TimerArg.m_EventSink =
+						    kEplEventSinkNmtk;
+						Ret =
+						    EplTimeruModifyTimerMs
+						    (&EplNmtuInstance_g.
+						     m_TimerHdl,
+						     (unsigned long)dwBuffer,
+						     TimerArg);
+						// potential error is forwarded to event queue which generates error event
+						break;
+					}
+
+					// node processes only async frames
+				case kEplNmtMsPreOperational1:
+					{
+						DWORD dwBuffer = 0;
+						tEplObdSize ObdSize;
+						tEplTimerArg TimerArg;
+
+						// create timer to switch automatically to PreOp2 if MN identified all mandatory CNs
+
+						// read NMT_BootTime_REC.MNWaitPreOp1_U32 from OD
+						ObdSize = sizeof(dwBuffer);
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0) || (EPL_OBD_USE_KERNEL != FALSE)
+						Ret =
+						    EplObduReadEntry
+						    (EPL_MCO_PTR_INSTANCE_PTR_
+						     0x1F89, 0x03, &dwBuffer,
+						     &ObdSize);
+						if (Ret != kEplSuccessful) {
+							// ignore error, because this timeout is optional
+							dwBuffer = 0;
+						}
+#endif
+						if (dwBuffer == 0) {	// delay is deactivated
+							// immediately post timer event
+							Ret =
+							    EplNmtuNmtEvent
+							    (kEplNmtEventTimerMsPreOp2);
+							break;
+						}
+						// convert us into ms
+						dwBuffer = dwBuffer / 1000;
+						if (dwBuffer == 0) {	// timer was below one ms
+							// set one ms
+							dwBuffer = 1;
+						}
+						TimerArg.m_EventSink =
+						    kEplEventSinkNmtk;
+						TimerArg.m_ulArg =
+						    (unsigned long)
+						    kEplNmtEventTimerMsPreOp2;
+						Ret =
+						    EplTimeruModifyTimerMs
+						    (&EplNmtuInstance_g.
+						     m_TimerHdl,
+						     (unsigned long)dwBuffer,
+						     TimerArg);
+						// potential error is forwarded to event queue which generates error event
+						break;
+					}
+
+					// node processes isochronous and asynchronous frames
+				case kEplNmtMsPreOperational2:
+					{
+						break;
+					}
+
+					// node should be configured und application is ready
+				case kEplNmtMsReadyToOperate:
+					{
+						break;
+					}
+
+					// normal work state
+				case kEplNmtMsOperational:
+					{
+						break;
+					}
+
+					// no EPL cycle
+					// -> normal ethernet communication
+				case kEplNmtMsBasicEthernet:
+					{
+						break;
+					}
+#endif // (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+				default:
+					{
+						TRACE1
+						    ("EplNmtuProcess(): unhandled NMT state 0x%X\n",
+						     pNmtStateChange->
+						     m_NewNmtState);
+					}
+				}
+			} else if (Ret == kEplReject) {	// application wants to change NMT state itself
+				// it's OK
+				Ret = kEplSuccessful;
+			}
+
+			EPL_DBGLVL_NMTU_TRACE0
+			    ("EplNmtuProcessEvent(): NMT-State-Maschine announce change of NMT State\n");
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplNmtInvalidEvent;
+		}
+
+	}
+
+//Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuRegisterStateChangeCb
+//
+// Description: register Callback-function go get informed about a
+//              NMT-Change-State-Event
+//
+//
+//
+// Parameters:  pfnEplNmtStateChangeCb_p = functionpointer
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC
+EplNmtuRegisterStateChangeCb(tEplNmtuStateChangeCallback
+			     pfnEplNmtStateChangeCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// save callback-function in modul global var
+	EplNmtuInstance_g.m_pfnNmtChangeCb = pfnEplNmtStateChangeCb_p;
+
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplNmtuCal.c b/drivers/staging/epl/EplNmtuCal.c
new file mode 100644
index 000000000000..4a29ef58bf9b
--- /dev/null
+++ b/drivers/staging/epl/EplNmtuCal.c
@@ -0,0 +1,158 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for communication abstraction layer of the
+                NMT-Userspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtuCal.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/16 -k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplNmtuCal.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtkCalGetNmtState
+//
+// Description: return current NMT-State
+//              -> encapsulate access to kernelspace
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplNmtState = current NMT-State
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplNmtState PUBLIC EplNmtkCalGetNmtState()
+{
+	tEplNmtState NmtState;
+	// for test direkt call for EplNmtkGetNmtState()
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+	NmtState = EplNmtkGetNmtState();
+#else
+	NmtState = 0;
+#endif
+	return NmtState;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+// EOF
diff --git a/drivers/staging/epl/EplObd.c b/drivers/staging/epl/EplObd.c
new file mode 100644
index 000000000000..efbb1967a5dc
--- /dev/null
+++ b/drivers/staging/epl/EplObd.c
@@ -0,0 +1,3262 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for api function of EplOBD-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObd.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.12 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                Microsoft VC7
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/02 k.t.:   start of the implementation, version 1.00
+		     ->based on CANopen OBD-Modul
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "kernel/EplObdk.h"	// function prototyps of the EplOBD-Modul
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// float definitions and macros
+#define _SHIFTED_EXPONENT_MASK_SP   0xff
+#define _BIAS_SP                    126
+#define T_SP                        23
+#define EXPONENT_DENORM_SP          (-_BIAS_SP)
+#define BASE_TO_THE_T_SP            ((float) 8388608.0)
+#define GET_EXPONENT_SP(x)          ((((x) >> T_SP) & _SHIFTED_EXPONENT_MASK_SP) - _BIAS_SP)
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+// struct for instance table
+INSTANCE_TYPE_BEGIN EPL_MCO_DECL_INSTANCE_MEMBER()
+
+STATIC tEplObdInitParam INST_FAR m_ObdInitParam;
+STATIC tEplObdStoreLoadObjCallback INST_NEAR m_fpStoreLoadObjCallback;
+
+INSTANCE_TYPE_END
+// decomposition of float
+typedef union {
+	tEplObdReal32 m_flRealPart;
+	int m_nIntegerPart;
+
+} tEplObdRealParts;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+// This macro replace the unspecific pointer to an instance through
+// the modul specific type for the local instance table. This macro
+// must defined in each modul.
+//#define tEplPtrInstance             tEplInstanceInfo MEM*
+
+EPL_MCO_DECL_INSTANCE_VAR()
+
+BYTE MEM abEplObdTrashObject_g[8];
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+EPL_MCO_DEFINE_INSTANCE_FCT()
+
+static tEplKernel EplObdCallObjectCallback(EPL_MCO_DECL_INSTANCE_PTR_
+					   tEplObdCallback fpCallback_p,
+					   tEplObdCbParam MEM * pCbParam_p);
+
+static tEplObdSize EplObdGetDataSizeIntern(tEplObdSubEntryPtr pSubIndexEntry_p);
+
+static tEplObdSize EplObdGetStrLen(void *pObjData_p,
+				   tEplObdSize ObjLen_p, tEplObdType ObjType_p);
+
+#if (EPL_OBD_CHECK_OBJECT_RANGE != FALSE)
+static tEplKernel EplObdCheckObjectRange(tEplObdSubEntryPtr pSubindexEntry_p,
+					 void *pData_p);
+#endif
+
+static tEplKernel EplObdGetVarEntry(tEplObdSubEntryPtr pSubindexEntry_p,
+				    tEplObdVarEntry MEM ** ppVarEntry_p);
+
+static tEplKernel EplObdGetEntry(EPL_MCO_DECL_INSTANCE_PTR_
+				 unsigned int uiIndex_p,
+				 unsigned int uiSubindex_p,
+				 tEplObdEntryPtr * ppObdEntry_p,
+				 tEplObdSubEntryPtr * ppObdSubEntry_p);
+
+static tEplObdSize EplObdGetObjectSize(tEplObdSubEntryPtr pSubIndexEntry_p);
+
+static tEplKernel EplObdGetIndexIntern(tEplObdInitParam MEM * pInitParam_p,
+				       unsigned int uiIndex_p,
+				       tEplObdEntryPtr * ppObdEntry_p);
+
+static tEplKernel EplObdGetSubindexIntern(tEplObdEntryPtr pObdEntry_p,
+					  unsigned int uiSubIndex_p,
+					  tEplObdSubEntryPtr * ppObdSubEntry_p);
+
+static tEplKernel EplObdAccessOdPartIntern(EPL_MCO_DECL_INSTANCE_PTR_
+					   tEplObdPart CurrentOdPart_p,
+					   tEplObdEntryPtr pObdEnty_p,
+					   tEplObdDir Direction_p);
+
+static void *EplObdGetObjectDefaultPtr(tEplObdSubEntryPtr pSubIndexEntry_p);
+static void MEM *EplObdGetObjectCurrentPtr(tEplObdSubEntryPtr pSubIndexEntry_p);
+
+#if (EPL_OBD_USE_STORE_RESTORE != FALSE)
+
+static tEplKernel EplObdCallStoreCallback(EPL_MCO_DECL_INSTANCE_PTR_
+					  tEplObdCbStoreParam MEM *
+					  pCbStoreParam_p);
+
+#endif // (EPL_OBD_USE_STORE_RESTORE != FALSE)
+
+static void EplObdCopyObjectData(void MEM * pDstData_p,
+				 void *pSrcData_p,
+				 tEplObdSize ObjSize_p, tEplObdType ObjType_p);
+
+void *EplObdGetObjectDataPtrIntern(tEplObdSubEntryPtr pSubindexEntry_p);
+
+static tEplKernel EplObdIsNumericalIntern(tEplObdSubEntryPtr pObdSubEntry_p,
+					  BOOL * pfEntryNumerical_p);
+
+static tEplKernel PUBLIC EplObdWriteEntryPre(EPL_MCO_DECL_INSTANCE_PTR_
+					     unsigned int uiIndex_p,
+					     unsigned int uiSubIndex_p,
+					     void *pSrcData_p,
+					     void **ppDstData_p,
+					     tEplObdSize Size_p,
+					     tEplObdEntryPtr * ppObdEntry_p,
+					     tEplObdSubEntryPtr * ppSubEntry_p,
+					     tEplObdCbParam MEM * pCbParam_p,
+					     tEplObdSize * pObdSize_p);
+
+static tEplKernel PUBLIC EplObdWriteEntryPost(EPL_MCO_DECL_INSTANCE_PTR_
+					      tEplObdEntryPtr pObdEntry_p,
+					      tEplObdSubEntryPtr pSubEntry_p,
+					      tEplObdCbParam MEM * pCbParam_p,
+					      void *pSrcData_p,
+					      void *pDstData_p,
+					      tEplObdSize ObdSize_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdInit()
+//
+// Description: initializes the first instance
+//
+// Parameters:  pInitParam_p    = init parameter
+//
+// Return:      tEplKernel      =   errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplObdInit(EPL_MCO_DECL_PTR_INSTANCE_PTR_
+					  tEplObdInitParam MEM * pInitParam_p)
+{
+
+	tEplKernel Ret;
+	EPL_MCO_DELETE_INSTANCE_TABLE();
+
+	if (pInitParam_p == NULL) {
+		Ret = kEplSuccessful;
+		goto Exit;
+	}
+
+	Ret = EplObdAddInstance(EPL_MCO_PTR_INSTANCE_PTR_ pInitParam_p);
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdAddInstance()
+//
+// Description: adds a new instance
+//
+// Parameters:  pInitParam_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplObdAddInstance(EPL_MCO_DECL_PTR_INSTANCE_PTR_
+						 tEplObdInitParam MEM *
+						 pInitParam_p)
+{
+
+	EPL_MCO_DECL_INSTANCE_PTR_LOCAL tEplKernel Ret;
+
+	// check if pointer to instance pointer valid
+	// get free instance and set the globale instance pointer
+	// set also the instance addr to parameterlist
+	EPL_MCO_CHECK_PTR_INSTANCE_PTR();
+	EPL_MCO_GET_FREE_INSTANCE_PTR();
+	EPL_MCO_SET_PTR_INSTANCE_PTR();
+
+	// save init parameters
+	EPL_MEMCPY(&EPL_MCO_GLB_VAR(m_ObdInitParam), pInitParam_p,
+		   sizeof(tEplObdInitParam));
+
+	// clear callback function for command LOAD and STORE
+	EPL_MCO_GLB_VAR(m_fpStoreLoadObjCallback) = NULL;
+
+	// sign instance as used
+	EPL_MCO_WRITE_INSTANCE_STATE(kStateUsed);
+
+	// initialize object dictionary
+	// so all all VarEntries will be initialized to trash object and default values will be set to current data
+	Ret = EplObdAccessOdPart(EPL_MCO_INSTANCE_PTR_
+				 kEplObdPartAll, kEplObdDirInit);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdDeleteInstance()
+//
+// Description: delete instance
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if (EPL_USE_DELETEINST_FUNC != FALSE)
+EPLDLLEXPORT tEplKernel PUBLIC EplObdDeleteInstance(EPL_MCO_DECL_INSTANCE_PTR)
+{
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	// sign instance as unused
+	EPL_MCO_WRITE_INSTANCE_STATE(kStateUnused);
+
+	return kEplSuccessful;
+
+}
+#endif // (EPL_USE_DELETEINST_FUNC != FALSE)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdWriteEntry()
+//
+// Description: Function writes data to an OBD entry. Strings
+//              are stored with added '\0' character.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplObdWriteEntry(EPL_MCO_DECL_INSTANCE_PTR_
+						unsigned int uiIndex_p,
+						unsigned int uiSubIndex_p,
+						void *pSrcData_p,
+						tEplObdSize Size_p)
+{
+
+	tEplKernel Ret;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pSubEntry;
+	tEplObdCbParam MEM CbParam;
+	void MEM *pDstData;
+	tEplObdSize ObdSize;
+
+	Ret = EplObdWriteEntryPre(EPL_MCO_INSTANCE_PTR_
+				  uiIndex_p,
+				  uiSubIndex_p,
+				  pSrcData_p,
+				  &pDstData,
+				  Size_p,
+				  &pObdEntry, &pSubEntry, &CbParam, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret = EplObdWriteEntryPost(EPL_MCO_INSTANCE_PTR_
+				   pObdEntry,
+				   pSubEntry,
+				   &CbParam, pSrcData_p, pDstData, ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdReadEntry()
+//
+// Description: The function reads an object entry. The application
+//              can always read the data even if attrib kEplObdAccRead
+//              is not set. The attrib is only checked up for SDO transfer.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       = Index oof the OD entry to read
+//              uiSubIndex_p    = Subindex to read
+//              pDstData_p      = pointer to the buffer for data
+//              Offset_p        = offset in data for read access
+//              pSize_p         = IN: Size of the buffer
+//                                OUT: number of readed Bytes
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplObdReadEntry(EPL_MCO_DECL_INSTANCE_PTR_
+					       unsigned int uiIndex_p,
+					       unsigned int uiSubIndex_p,
+					       void *pDstData_p,
+					       tEplObdSize * pSize_p)
+{
+
+	tEplKernel Ret;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pSubEntry;
+	tEplObdCbParam MEM CbParam;
+	void *pSrcData;
+	tEplObdSize ObdSize;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	ASSERT(pDstData_p != NULL);
+	ASSERT(pSize_p != NULL);
+
+	// get address of index and subindex entry
+	Ret = EplObdGetEntry(EPL_MCO_INSTANCE_PTR_
+			     uiIndex_p, uiSubIndex_p, &pObdEntry, &pSubEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get pointer to object data
+	pSrcData = EplObdGetObjectDataPtrIntern(pSubEntry);
+
+	// check source pointer
+	if (pSrcData == NULL) {
+		Ret = kEplObdReadViolation;
+		goto Exit;
+	}
+	//------------------------------------------------------------------------
+	// address of source data to structure of callback parameters
+	// so callback function can change this data before reading
+	CbParam.m_uiIndex = uiIndex_p;
+	CbParam.m_uiSubIndex = uiSubIndex_p;
+	CbParam.m_pArg = pSrcData;
+	CbParam.m_ObdEvent = kEplObdEvPreRead;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry->m_fpCallback, &CbParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get size of data and check if application has reserved enough memory
+	ObdSize = EplObdGetDataSizeIntern(pSubEntry);
+	// check if offset given and calc correct number of bytes to read
+	if (*pSize_p < ObdSize) {
+		Ret = kEplObdValueLengthError;
+		goto Exit;
+	}
+	// read value from object
+	EPL_MEMCPY(pDstData_p, pSrcData, ObdSize);
+	*pSize_p = ObdSize;
+
+	// write address of destination data to structure of callback parameters
+	// so callback function can change this data after reading
+	CbParam.m_pArg = pDstData_p;
+	CbParam.m_ObdEvent = kEplObdEvPostRead;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry->m_fpCallback, &CbParam);
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdAccessOdPart()
+//
+// Description: restores default values of one part of OD
+//
+// Parameters:  ObdPart_p
+//              Direction_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplObdAccessOdPart(EPL_MCO_DECL_INSTANCE_PTR_
+						  tEplObdPart ObdPart_p,
+						  tEplObdDir Direction_p)
+{
+
+	tEplKernel Ret = kEplSuccessful;
+	BOOL fPartFount;
+	tEplObdEntryPtr pObdEntry;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	//  part always has to be unequal to NULL
+	pObdEntry = EPL_MCO_GLB_VAR(m_ObdInitParam.m_pPart);
+	ASSERTMSG(pObdEntry != NULL,
+		  "EplObdAccessOdPart(): no  OD part is defined!\n");
+
+	// if ObdPart_p is not valid fPartFound keeps FALSE and function returns kEplObdIllegalPart
+	fPartFount = FALSE;
+
+	// access to  part
+	if ((ObdPart_p & kEplObdPartGen) != 0) {
+		fPartFount = TRUE;
+
+		Ret = EplObdAccessOdPartIntern(EPL_MCO_INSTANCE_PTR_
+					       kEplObdPartGen, pObdEntry,
+					       Direction_p);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+	// access to manufacturer part
+	pObdEntry = EPL_MCO_GLB_VAR(m_ObdInitParam.m_pManufacturerPart);
+
+	if (((ObdPart_p & kEplObdPartMan) != 0) && (pObdEntry != NULL)) {
+		fPartFount = TRUE;
+
+		Ret = EplObdAccessOdPartIntern(EPL_MCO_INSTANCE_PTR_
+					       kEplObdPartMan, pObdEntry,
+					       Direction_p);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+	// access to device part
+	pObdEntry = EPL_MCO_GLB_VAR(m_ObdInitParam.m_pDevicePart);
+
+	if (((ObdPart_p & kEplObdPartDev) != 0) && (pObdEntry != NULL)) {
+		fPartFount = TRUE;
+
+		Ret = EplObdAccessOdPartIntern(EPL_MCO_INSTANCE_PTR_
+					       kEplObdPartDev, pObdEntry,
+					       Direction_p);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+	{
+		// access to user part
+		pObdEntry = EPL_MCO_GLB_VAR(m_ObdInitParam.m_pUserPart);
+
+		if (((ObdPart_p & kEplObdPartUsr) != 0) && (pObdEntry != NULL)) {
+			fPartFount = TRUE;
+
+			Ret = EplObdAccessOdPartIntern(EPL_MCO_INSTANCE_PTR_
+						       kEplObdPartUsr,
+						       pObdEntry, Direction_p);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+		}
+	}
+#endif
+
+	// no access to an OD part was done? illegal OD part was specified!
+	if (fPartFount == FALSE) {
+		Ret = kEplObdIllegalPart;
+	}
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdDefineVar()
+//
+// Description: defines a variable in OD
+//
+// Parameters:  pEplVarParam_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplObdDefineVar(EPL_MCO_DECL_INSTANCE_PTR_
+					       tEplVarParam MEM * pVarParam_p)
+{
+
+	tEplKernel Ret;
+	tEplObdVarEntry MEM *pVarEntry;
+	tEplVarParamValid VarValid;
+	tEplObdSubEntryPtr pSubindexEntry;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	ASSERT(pVarParam_p != NULL);	// is not allowed to be NULL
+
+	// get address of subindex entry
+	Ret = EplObdGetEntry(EPL_MCO_INSTANCE_PTR_
+			     pVarParam_p->m_uiIndex,
+			     pVarParam_p->m_uiSubindex, NULL, &pSubindexEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get var entry
+	Ret = EplObdGetVarEntry(pSubindexEntry, &pVarEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	VarValid = pVarParam_p->m_ValidFlag;
+
+	// copy only this values, which valid flag is set
+	if ((VarValid & kVarValidSize) != 0) {
+		if (pSubindexEntry->m_Type != kEplObdTypDomain) {
+			tEplObdSize DataSize;
+
+			// check passed size parameter
+			DataSize = EplObdGetObjectSize(pSubindexEntry);
+			if (DataSize != pVarParam_p->m_Size) {	// size of variable does not match
+				Ret = kEplObdValueLengthError;
+				goto Exit;
+			}
+		} else {	// size can be set only for objects of type DOMAIN
+			pVarEntry->m_Size = pVarParam_p->m_Size;
+		}
+	}
+
+	if ((VarValid & kVarValidData) != 0) {
+		pVarEntry->m_pData = pVarParam_p->m_pData;
+	}
+/*
+    #if (EPL_PDO_USE_STATIC_MAPPING == FALSE)
+    {
+        if ((VarValid & kVarValidCallback) != 0)
+        {
+           pVarEntry->m_fpCallback = pVarParam_p->m_fpCallback;
+        }
+
+        if ((VarValid & kVarValidArg) != 0)
+        {
+           pVarEntry->m_pArg = pVarParam_p->m_pArg;
+        }
+    }
+    #endif
+*/
+	// Ret is already set to kEplSuccessful from ObdGetVarIntern()
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetObjectDataPtr()
+//
+// Description: It returnes the current data pointer. But if object is an
+//              constant object it returnes the default pointer.
+//
+// Parameters:  uiIndex_p    =   Index of the entry
+//              uiSubindex_p =   Subindex of the entry
+//
+// Return:      void *    = pointer to object data
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT void *PUBLIC EplObdGetObjectDataPtr(EPL_MCO_DECL_INSTANCE_PTR_
+						 unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p)
+{
+	tEplKernel Ret;
+	void *pData;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pObdSubEntry;
+
+	// get pointer to index structure
+	Ret = EplObdGetIndexIntern(&EPL_MCO_GLB_VAR(m_ObdInitParam),
+				   uiIndex_p, &pObdEntry);
+	if (Ret != kEplSuccessful) {
+		pData = NULL;
+		goto Exit;
+	}
+	// get pointer to subindex structure
+	Ret = EplObdGetSubindexIntern(pObdEntry, uiSubIndex_p, &pObdSubEntry);
+	if (Ret != kEplSuccessful) {
+		pData = NULL;
+		goto Exit;
+	}
+	// get Datapointer
+	pData = EplObdGetObjectDataPtrIntern(pObdSubEntry);
+
+      Exit:
+	return pData;
+
+}
+
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdRegisterUserOd()
+//
+// Description: function registers the user OD
+//
+// Parameters:  pUserOd_p   =pointer to user ODd
+//
+// Return:     tEplKernel = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdRegisterUserOd(EPL_MCO_DECL_INSTANCE_PTR_
+						    tEplObdEntryPtr pUserOd_p)
+{
+
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	EPL_MCO_GLB_VAR(m_ObdInitParam.m_pUserPart) = pUserOd_p;
+
+	return kEplSuccessful;
+
+}
+
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdInitVarEntry()
+//
+// Description: function to initialize VarEntry dependened on object type
+//
+// Parameters:  pVarEntry_p = pointer to var entry structure
+//              Type_p      = object type
+//              ObdSize_p   = size of object data
+//
+// Returns:     none
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT void PUBLIC EplObdInitVarEntry(EPL_MCO_DECL_INSTANCE_PTR_
+					    tEplObdVarEntry MEM * pVarEntry_p,
+					    tEplObdType Type_p,
+					    tEplObdSize ObdSize_p)
+{
+/*
+    #if (EPL_PDO_USE_STATIC_MAPPING == FALSE)
+    {
+        // reset pointer to VAR callback and argument
+        pVarEntry_p->m_fpCallback  = NULL;
+        pVarEntry_p->m_pArg = NULL;
+    }
+    #endif
+*/
+
+// 10-dec-2004 r.d.: this function will not be used for strings
+	if ((Type_p == kEplObdTypDomain))
+//         (bType_p == kEplObdTypVString) /* ||
+//         (bType_p == kEplObdTypOString) ||
+//         (bType_p == kEplObdTypUString)    */ )
+	{
+		// variables which are defined as DOMAIN or VSTRING should not point to
+		// trash object, because this trash object contains only 8 bytes. DOMAINS or
+		// STRINGS can be longer.
+		pVarEntry_p->m_pData = NULL;
+		pVarEntry_p->m_Size = 0;
+	} else {
+		// set address to variable data to trash object
+		// This prevents an access violation if user forgets to call EplObdDefineVar()
+		// for this variable but mappes it in a PDO.
+		pVarEntry_p->m_pData = &abEplObdTrashObject_g[0];
+		pVarEntry_p->m_Size = ObdSize_p;
+	}
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetDataSize()
+//
+// Description: function to initialize VarEntry dependened on object type
+//
+//              gets the data size of an object
+//              for string objects it returnes the string length
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_ = Instancepointer
+//              uiIndex_p   =   Index
+//              uiSubIndex_p=   Subindex
+//
+// Return:      tEplObdSize
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplObdSize PUBLIC EplObdGetDataSize(EPL_MCO_DECL_INSTANCE_PTR_
+						  unsigned int uiIndex_p,
+						  unsigned int uiSubIndex_p)
+{
+	tEplKernel Ret;
+	tEplObdSize ObdSize;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pObdSubEntry;
+
+	// get pointer to index structure
+	Ret = EplObdGetIndexIntern(&EPL_MCO_GLB_VAR(m_ObdInitParam),
+				   uiIndex_p, &pObdEntry);
+	if (Ret != kEplSuccessful) {
+		ObdSize = 0;
+		goto Exit;
+	}
+	// get pointer to subindex structure
+	Ret = EplObdGetSubindexIntern(pObdEntry, uiSubIndex_p, &pObdSubEntry);
+	if (Ret != kEplSuccessful) {
+		ObdSize = 0;
+		goto Exit;
+	}
+	// get size
+	ObdSize = EplObdGetDataSizeIntern(pObdSubEntry);
+      Exit:
+	return ObdSize;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetNodeId()
+//
+// Description: function returns nodeid from entry 0x1F93
+//
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR = Instancepointer
+//
+// Return:      unsigned int = Node Id
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT unsigned int PUBLIC EplObdGetNodeId(EPL_MCO_DECL_INSTANCE_PTR)
+{
+	tEplKernel Ret;
+	tEplObdSize ObdSize;
+	BYTE bNodeId;
+
+	bNodeId = 0;
+	ObdSize = sizeof(bNodeId);
+	Ret = EplObdReadEntry(EPL_MCO_PTR_INSTANCE_PTR_
+			      EPL_OBD_NODE_ID_INDEX,
+			      EPL_OBD_NODE_ID_SUBINDEX, &bNodeId, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		bNodeId = EPL_C_ADR_INVALID;
+		goto Exit;
+	}
+
+      Exit:
+	return (unsigned int)bNodeId;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdSetNodeId()
+//
+// Description: function sets nodeid in entry 0x1F93
+//
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_ = Instancepointer
+//              uiNodeId_p  =   Node Id to set
+//              NodeIdType_p=   Type on which way the Node Id was set
+//
+// Return:      tEplKernel = Errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdSetNodeId(EPL_MCO_DECL_PTR_INSTANCE_PTR_
+					       unsigned int uiNodeId_p,
+					       tEplObdNodeIdType NodeIdType_p)
+{
+	tEplKernel Ret;
+	tEplObdSize ObdSize;
+	BYTE fHwBool;
+	BYTE bNodeId;
+
+	// check Node Id
+	if (uiNodeId_p == EPL_C_ADR_INVALID) {
+		Ret = kEplInvalidNodeId;
+		goto Exit;
+	}
+	bNodeId = (BYTE) uiNodeId_p;
+	ObdSize = sizeof(BYTE);
+	// write NodeId to OD entry
+	Ret = EplObdWriteEntry(EPL_MCO_PTR_INSTANCE_PTR_
+			       EPL_OBD_NODE_ID_INDEX,
+			       EPL_OBD_NODE_ID_SUBINDEX, &bNodeId, ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// set HWBOOL-Flag in Subindex EPL_OBD_NODE_ID_HWBOOL_SUBINDEX
+	switch (NodeIdType_p) {
+		// type unknown
+	case kEplObdNodeIdUnknown:
+		{
+			fHwBool = OBD_FALSE;
+			break;
+		}
+
+	case kEplObdNodeIdSoftware:
+		{
+			fHwBool = OBD_FALSE;
+			break;
+		}
+
+	case kEplObdNodeIdHardware:
+		{
+			fHwBool = OBD_TRUE;
+			break;
+		}
+
+	default:
+		{
+			fHwBool = OBD_FALSE;
+		}
+
+	}			// end of switch (NodeIdType_p)
+
+	// write flag
+	ObdSize = sizeof(fHwBool);
+	Ret = EplObdWriteEntry(EPL_MCO_PTR_INSTANCE_PTR
+			       EPL_OBD_NODE_ID_INDEX,
+			       EPL_OBD_NODE_ID_HWBOOL_SUBINDEX,
+			       &fHwBool, ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdIsNumerical()
+//
+// Description: function checks if a entry is numerical or not
+//
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_ = Instancepointer
+//              uiIndex_p           = Index
+//              uiSubIndex_p        = Subindex
+//              pfEntryNumerical_p  = pointer to BOOL for returnvalue
+//                                  -> TRUE if entry a numerical value
+//                                  -> FALSE if entry not a numerical value
+//
+// Return:      tEplKernel = Errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdIsNumerical(EPL_MCO_DECL_INSTANCE_PTR_
+						 unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p,
+						 BOOL * pfEntryNumerical_p)
+{
+	tEplKernel Ret;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pObdSubEntry;
+
+	// get pointer to index structure
+	Ret = EplObdGetIndexIntern(&EPL_MCO_GLB_VAR(m_ObdInitParam),
+				   uiIndex_p, &pObdEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get pointer to subindex structure
+	Ret = EplObdGetSubindexIntern(pObdEntry, uiSubIndex_p, &pObdSubEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret = EplObdIsNumericalIntern(pObdSubEntry, pfEntryNumerical_p);
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdReadEntryToLe()
+//
+// Description: The function reads an object entry from the byteoder
+//              of the system to the little endian byteorder for numerical values.
+//              For other types a normal read will be processed. This is usefull for
+//              the PDO and SDO module. The application
+//              can always read the data even if attrib kEplObdAccRead
+//              is not set. The attrib is only checked up for SDO transfer.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       = Index of the OD entry to read
+//              uiSubIndex_p    = Subindex to read
+//              pDstData_p      = pointer to the buffer for data
+//              Offset_p        = offset in data for read access
+//              pSize_p         = IN: Size of the buffer
+//                                OUT: number of readed Bytes
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdReadEntryToLe(EPL_MCO_DECL_INSTANCE_PTR_
+						   unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p,
+						   void *pDstData_p,
+						   tEplObdSize * pSize_p)
+{
+	tEplKernel Ret;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pSubEntry;
+	tEplObdCbParam MEM CbParam;
+	void *pSrcData;
+	tEplObdSize ObdSize;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	ASSERT(pDstData_p != NULL);
+	ASSERT(pSize_p != NULL);
+
+	// get address of index and subindex entry
+	Ret = EplObdGetEntry(EPL_MCO_INSTANCE_PTR_
+			     uiIndex_p, uiSubIndex_p, &pObdEntry, &pSubEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get pointer to object data
+	pSrcData = EplObdGetObjectDataPtrIntern(pSubEntry);
+
+	// check source pointer
+	if (pSrcData == NULL) {
+		Ret = kEplObdReadViolation;
+		goto Exit;
+	}
+	//------------------------------------------------------------------------
+	// address of source data to structure of callback parameters
+	// so callback function can change this data before reading
+	CbParam.m_uiIndex = uiIndex_p;
+	CbParam.m_uiSubIndex = uiSubIndex_p;
+	CbParam.m_pArg = pSrcData;
+	CbParam.m_ObdEvent = kEplObdEvPreRead;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry->m_fpCallback, &CbParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get size of data and check if application has reserved enough memory
+	ObdSize = EplObdGetDataSizeIntern(pSubEntry);
+	// check if offset given and calc correct number of bytes to read
+	if (*pSize_p < ObdSize) {
+		Ret = kEplObdValueLengthError;
+		goto Exit;
+	}
+	// check if numerical type
+	switch (pSubEntry->m_Type) {
+		//-----------------------------------------------
+		// types without ami
+	case kEplObdTypVString:
+	case kEplObdTypOString:
+	case kEplObdTypDomain:
+	default:
+		{
+			// read value from object
+			EPL_MEMCPY(pDstData_p, pSrcData, ObdSize);
+			break;
+		}
+
+		//-----------------------------------------------
+		// numerical type which needs ami-write
+		// 8 bit or smaller values
+	case kEplObdTypBool:
+	case kEplObdTypInt8:
+	case kEplObdTypUInt8:
+		{
+			AmiSetByteToLe(pDstData_p, *((BYTE *) pSrcData));
+			break;
+		}
+
+		// 16 bit values
+	case kEplObdTypInt16:
+	case kEplObdTypUInt16:
+		{
+			AmiSetWordToLe(pDstData_p, *((WORD *) pSrcData));
+			break;
+		}
+
+		// 24 bit values
+	case kEplObdTypInt24:
+	case kEplObdTypUInt24:
+		{
+			AmiSetDword24ToLe(pDstData_p, *((DWORD *) pSrcData));
+			break;
+		}
+
+		// 32 bit values
+	case kEplObdTypInt32:
+	case kEplObdTypUInt32:
+	case kEplObdTypReal32:
+		{
+			AmiSetDwordToLe(pDstData_p, *((DWORD *) pSrcData));
+			break;
+		}
+
+		// 40 bit values
+	case kEplObdTypInt40:
+	case kEplObdTypUInt40:
+		{
+			AmiSetQword40ToLe(pDstData_p, *((QWORD *) pSrcData));
+			break;
+		}
+
+		// 48 bit values
+	case kEplObdTypInt48:
+	case kEplObdTypUInt48:
+		{
+			AmiSetQword48ToLe(pDstData_p, *((QWORD *) pSrcData));
+			break;
+		}
+
+		// 56 bit values
+	case kEplObdTypInt56:
+	case kEplObdTypUInt56:
+		{
+			AmiSetQword56ToLe(pDstData_p, *((QWORD *) pSrcData));
+			break;
+		}
+
+		// 64 bit values
+	case kEplObdTypInt64:
+	case kEplObdTypUInt64:
+	case kEplObdTypReal64:
+		{
+			AmiSetQword64ToLe(pDstData_p, *((QWORD *) pSrcData));
+			break;
+		}
+
+		// time of day
+	case kEplObdTypTimeOfDay:
+	case kEplObdTypTimeDiff:
+		{
+			AmiSetTimeOfDay(pDstData_p, ((tTimeOfDay *) pSrcData));
+			break;
+		}
+
+	}			// end of switch(pSubEntry->m_Type)
+
+	*pSize_p = ObdSize;
+
+	// write address of destination data to structure of callback parameters
+	// so callback function can change this data after reading
+	CbParam.m_pArg = pDstData_p;
+	CbParam.m_ObdEvent = kEplObdEvPostRead;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry->m_fpCallback, &CbParam);
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdWriteEntryFromLe()
+//
+// Description: Function writes data to an OBD entry from a source with
+//              little endian byteorder to the od with system specuific
+//              byteorder. Not numerical values will only by copied. Strings
+//              are stored with added '\0' character.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdWriteEntryFromLe(EPL_MCO_DECL_INSTANCE_PTR_
+						      unsigned int uiIndex_p,
+						      unsigned int uiSubIndex_p,
+						      void *pSrcData_p,
+						      tEplObdSize Size_p)
+{
+	tEplKernel Ret;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pSubEntry;
+	tEplObdCbParam MEM CbParam;
+	void MEM *pDstData;
+	tEplObdSize ObdSize;
+	QWORD qwBuffer;
+	void *pBuffer = &qwBuffer;
+
+	Ret = EplObdWriteEntryPre(EPL_MCO_INSTANCE_PTR_
+				  uiIndex_p,
+				  uiSubIndex_p,
+				  pSrcData_p,
+				  &pDstData,
+				  Size_p,
+				  &pObdEntry, &pSubEntry, &CbParam, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	// check if numerical type
+	switch (pSubEntry->m_Type) {
+		//-----------------------------------------------
+		// types without ami
+	default:
+		{		// do nothing, i.e. use the given source pointer
+			pBuffer = pSrcData_p;
+			break;
+		}
+
+		//-----------------------------------------------
+		// numerical type which needs ami-write
+		// 8 bit or smaller values
+	case kEplObdTypBool:
+	case kEplObdTypInt8:
+	case kEplObdTypUInt8:
+		{
+			*((BYTE *) pBuffer) = AmiGetByteFromLe(pSrcData_p);
+			break;
+		}
+
+		// 16 bit values
+	case kEplObdTypInt16:
+	case kEplObdTypUInt16:
+		{
+			*((WORD *) pBuffer) = AmiGetWordFromLe(pSrcData_p);
+			break;
+		}
+
+		// 24 bit values
+	case kEplObdTypInt24:
+	case kEplObdTypUInt24:
+		{
+			*((DWORD *) pBuffer) = AmiGetDword24FromLe(pSrcData_p);
+			break;
+		}
+
+		// 32 bit values
+	case kEplObdTypInt32:
+	case kEplObdTypUInt32:
+	case kEplObdTypReal32:
+		{
+			*((DWORD *) pBuffer) = AmiGetDwordFromLe(pSrcData_p);
+			break;
+		}
+
+		// 40 bit values
+	case kEplObdTypInt40:
+	case kEplObdTypUInt40:
+		{
+			*((QWORD *) pBuffer) = AmiGetQword40FromLe(pSrcData_p);
+			break;
+		}
+
+		// 48 bit values
+	case kEplObdTypInt48:
+	case kEplObdTypUInt48:
+		{
+			*((QWORD *) pBuffer) = AmiGetQword48FromLe(pSrcData_p);
+			break;
+		}
+
+		// 56 bit values
+	case kEplObdTypInt56:
+	case kEplObdTypUInt56:
+		{
+			*((QWORD *) pBuffer) = AmiGetQword56FromLe(pSrcData_p);
+			break;
+		}
+
+		// 64 bit values
+	case kEplObdTypInt64:
+	case kEplObdTypUInt64:
+	case kEplObdTypReal64:
+		{
+			*((QWORD *) pBuffer) = AmiGetQword64FromLe(pSrcData_p);
+			break;
+		}
+
+		// time of day
+	case kEplObdTypTimeOfDay:
+	case kEplObdTypTimeDiff:
+		{
+			AmiGetTimeOfDay(pBuffer, ((tTimeOfDay *) pSrcData_p));
+			break;
+		}
+
+	}			// end of switch(pSubEntry->m_Type)
+
+	Ret = EplObdWriteEntryPost(EPL_MCO_INSTANCE_PTR_
+				   pObdEntry,
+				   pSubEntry,
+				   &CbParam, pBuffer, pDstData, ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetAccessType()
+//
+// Description: Function returns accesstype of the entry
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pAccessTyp_p    =   pointer to buffer to store accesstype
+//
+// Return:      tEplKernel     =   errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdGetAccessType(EPL_MCO_DECL_INSTANCE_PTR_
+						   unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p,
+						   tEplObdAccess * pAccessTyp_p)
+{
+	tEplKernel Ret;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pObdSubEntry;
+
+	// get pointer to index structure
+	Ret = EplObdGetIndexIntern(&EPL_MCO_GLB_VAR(m_ObdInitParam),
+				   uiIndex_p, &pObdEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get pointer to subindex structure
+	Ret = EplObdGetSubindexIntern(pObdEntry, uiSubIndex_p, &pObdSubEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get accessType
+	*pAccessTyp_p = pObdSubEntry->m_Access;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdSearchVarEntry()
+//
+// Description: gets variable from OD
+//
+// Parameters:  uiIndex_p       =   index of the var entry to search
+//              uiSubindex_p    =   subindex of var entry to search
+//              ppVarEntry_p    =   pointer to the pointer to the varentry
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplObdSearchVarEntry(EPL_MCO_DECL_INSTANCE_PTR_
+				       unsigned int uiIndex_p,
+				       unsigned int uiSubindex_p,
+				       tEplObdVarEntry MEM ** ppVarEntry_p)
+{
+
+	tEplKernel Ret;
+	tEplObdSubEntryPtr pSubindexEntry;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	// get address of subindex entry
+	Ret = EplObdGetEntry(EPL_MCO_INSTANCE_PTR_
+			     uiIndex_p, uiSubindex_p, NULL, &pSubindexEntry);
+	if (Ret == kEplSuccessful) {
+		// get var entry
+		Ret = EplObdGetVarEntry(pSubindexEntry, ppVarEntry_p);
+	}
+
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+EPL_MCO_DECL_INSTANCE_FCT()
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdCallObjectCallback()
+//
+// Description: calls callback function of an object or of a variable
+//
+// Parameters:  fpCallback_p
+//              pCbParam_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplObdCallObjectCallback(EPL_MCO_DECL_INSTANCE_PTR_
+					   tEplObdCallback fpCallback_p,
+					   tEplObdCbParam MEM * pCbParam_p)
+{
+
+	tEplKernel Ret;
+	tEplObdCallback MEM fpCallback;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	ASSERT(pCbParam_p != NULL);
+
+	Ret = kEplSuccessful;
+
+	// check address of callback function before calling it
+	if (fpCallback_p != NULL) {
+		// KEIL C51 V6.01 has a bug.
+		// Therefore the parameter fpCallback_p has to be copied in local variable fpCallback.
+		fpCallback = fpCallback_p;
+
+		// call callback function for this object
+		Ret = fpCallback(EPL_MCO_INSTANCE_PARAM_IDX_()
+				 pCbParam_p);
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetDataSizeIntern()
+//
+// Description: gets the data size of an object
+//              for string objects it returnes the string length
+//
+// Parameters:  pSubIndexEntry_p
+//
+// Return:      tEplObdSize
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplObdSize EplObdGetDataSizeIntern(tEplObdSubEntryPtr pSubIndexEntry_p)
+{
+
+	tEplObdSize DataSize;
+	void MEM *pData;
+
+	// If OD entry is defined by macro EPL_OBD_SUBINDEX_ROM_VSTRING
+	// then the current pointer is always NULL. The function
+	// returns the length of default string.
+	DataSize = EplObdGetObjectSize(pSubIndexEntry_p);
+
+	if (pSubIndexEntry_p->m_Type == kEplObdTypVString) {
+		// The pointer to current value can be received from EplObdGetObjectCurrentPtr()
+		pData =
+		    ((void MEM *)EplObdGetObjectCurrentPtr(pSubIndexEntry_p));
+		if (pData != NULL) {
+			DataSize =
+			    EplObdGetStrLen((void *)pData, DataSize,
+					    pSubIndexEntry_p->m_Type);
+		}
+
+	}
+
+	return DataSize;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetStrLen()
+//
+// Description: The function calculates the length of string. The '\0'
+//              character is included!!
+//
+// Parameters:  pObjData_p          = pointer to string
+//              ObjLen_p            = max. length of objectr entry
+//              bObjType_p          = object type (VSTRING, ...)
+//
+// Returns:     string length + 1
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplObdSize EplObdGetStrLen(void *pObjData_p,
+				   tEplObdSize ObjLen_p, tEplObdType ObjType_p)
+{
+
+	tEplObdSize StrLen = 0;
+	BYTE *pbString;
+
+	if (pObjData_p == NULL) {
+		goto Exit;
+	}
+	//----------------------------------------
+	// Visible String: data format byte
+	if (ObjType_p == kEplObdTypVString) {
+		pbString = pObjData_p;
+
+		for (StrLen = 0; StrLen < ObjLen_p; StrLen++) {
+			if (*pbString == '\0') {
+				StrLen++;
+				break;
+			}
+
+			pbString++;
+		}
+	}
+	//----------------------------------------
+	// other string types ...
+
+      Exit:
+	return (StrLen);
+
+}
+
+#if (EPL_OBD_CHECK_OBJECT_RANGE != FALSE)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdCheckObjectRange()
+//
+// Description: function to check value range of object data
+//
+// NOTICE: The pointer of data (pData_p) must point out to an even address,
+//         if ObjType is unequal to kEplObdTypInt8 or kEplObdTypUInt8! But it is
+//         always realiced because pointer m_pDefault points always to an
+//         array of the SPECIFIED type.
+//
+// Parameters:  pSubindexEntry_p
+//              pData_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplObdCheckObjectRange(tEplObdSubEntryPtr pSubindexEntry_p,
+					 void *pData_p)
+{
+
+	tEplKernel Ret;
+	void *pRangeData;
+
+	ASSERTMSG(pSubindexEntry_p != NULL,
+		  "EplObdCheckObjectRange(): no address to subindex struct!\n");
+
+	Ret = kEplSuccessful;
+
+	// check if data range has to be checked
+	if ((pSubindexEntry_p->m_Access & kEplObdAccRange) == 0) {
+		goto Exit;
+	}
+	// get address of default data
+	pRangeData = pSubindexEntry_p->m_pDefault;
+
+	// jump to called object type
+	switch ((tEplObdType) pSubindexEntry_p->m_Type) {
+		// -----------------------------------------------------------------
+		// ObdType kEplObdTypBool will not be checked because there are only
+		// two possible values 0 or 1.
+
+		// -----------------------------------------------------------------
+		// ObdTypes which has to be check up because numerical values
+	case kEplObdTypInt8:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdInteger8 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdInteger8 *) pData_p) <
+		    *((tEplObdInteger8 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdInteger8 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdInteger8 *) pData_p) >
+		    *((tEplObdInteger8 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+	case kEplObdTypUInt8:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdUnsigned8 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdUnsigned8 *) pData_p) <
+		    *((tEplObdUnsigned8 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdUnsigned8 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdUnsigned8 *) pData_p) >
+		    *((tEplObdUnsigned8 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+	case kEplObdTypInt16:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdInteger16 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdInteger16 *) pData_p) <
+		    *((tEplObdInteger16 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdInteger16 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdInteger16 *) pData_p) >
+		    *((tEplObdInteger16 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+	case kEplObdTypUInt16:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdUnsigned16 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdUnsigned16 *) pData_p) <
+		    *((tEplObdUnsigned16 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdUnsigned16 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdUnsigned16 *) pData_p) >
+		    *((tEplObdUnsigned16 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+	case kEplObdTypInt32:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdInteger32 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdInteger32 *) pData_p) <
+		    *((tEplObdInteger32 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdInteger32 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdInteger32 *) pData_p) >
+		    *((tEplObdInteger32 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+	case kEplObdTypUInt32:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdUnsigned32 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdUnsigned32 *) pData_p) <
+		    *((tEplObdUnsigned32 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdUnsigned32 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdUnsigned32 *) pData_p) >
+		    *((tEplObdUnsigned32 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+	case kEplObdTypReal32:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdReal32 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdReal32 *) pData_p) <
+		    *((tEplObdReal32 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdReal32 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdReal32 *) pData_p) >
+		    *((tEplObdReal32 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt40:
+	case kEplObdTypInt48:
+	case kEplObdTypInt56:
+	case kEplObdTypInt64:
+
+		// switch to lower limit
+		pRangeData = ((signed QWORD *)pRangeData) + 1;
+
+		// check if value is to low
+		if (*((signed QWORD *)pData_p) < *((signed QWORD *)pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((signed QWORD *)pRangeData) + 1;
+
+		// check if value is to high
+		if (*((signed QWORD *)pData_p) > *((signed QWORD *)pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypUInt40:
+	case kEplObdTypUInt48:
+	case kEplObdTypUInt56:
+	case kEplObdTypUInt64:
+
+		// switch to lower limit
+		pRangeData = ((unsigned QWORD *)pRangeData) + 1;
+
+		// check if value is to low
+		if (*((unsigned QWORD *)pData_p) <
+		    *((unsigned QWORD *)pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((unsigned QWORD *)pRangeData) + 1;
+
+		// check if value is to high
+		if (*((unsigned QWORD *)pData_p) >
+		    *((unsigned QWORD *)pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypReal64:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdReal64 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdReal64 *) pData_p) <
+		    *((tEplObdReal64 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdReal64 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdReal64 *) pData_p) >
+		    *((tEplObdReal64 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypTimeOfDay:
+	case kEplObdTypTimeDiff:
+		break;
+
+		// -----------------------------------------------------------------
+		// ObdTypes kEplObdTypXString and kEplObdTypDomain can not be checkt because
+		// they have no numerical value.
+	default:
+
+		Ret = kEplObdUnknownObjectType;
+		break;
+	}
+
+      Exit:
+
+	return Ret;
+
+}
+#endif // (EPL_OBD_CHECK_OBJECT_RANGE != FALSE)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdWriteEntryPre()
+//
+// Description: Function prepares write of data to an OBD entry. Strings
+//              are stored with added '\0' character.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplObdWriteEntryPre(EPL_MCO_DECL_INSTANCE_PTR_
+					     unsigned int uiIndex_p,
+					     unsigned int uiSubIndex_p,
+					     void *pSrcData_p,
+					     void **ppDstData_p,
+					     tEplObdSize Size_p,
+					     tEplObdEntryPtr * ppObdEntry_p,
+					     tEplObdSubEntryPtr * ppSubEntry_p,
+					     tEplObdCbParam MEM * pCbParam_p,
+					     tEplObdSize * pObdSize_p)
+{
+
+	tEplKernel Ret;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pSubEntry;
+	tEplObdAccess Access;
+	void MEM *pDstData;
+	tEplObdSize ObdSize;
+	BOOL fEntryNumerical;
+
+#if (EPL_OBD_USE_STRING_DOMAIN_IN_RAM != FALSE)
+	tEplObdVStringDomain MEM MemVStringDomain;
+	void MEM *pCurrData;
+#endif
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	ASSERT(pSrcData_p != NULL);	// should never be NULL
+
+	//------------------------------------------------------------------------
+	// get address of index and subindex entry
+	Ret = EplObdGetEntry(EPL_MCO_INSTANCE_PTR_
+			     uiIndex_p, uiSubIndex_p, &pObdEntry, &pSubEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get pointer to object data
+	pDstData = (void MEM *)EplObdGetObjectDataPtrIntern(pSubEntry);
+
+	Access = (tEplObdAccess) pSubEntry->m_Access;
+
+	// check access for write
+	// access violation if adress to current value is NULL
+	if (((Access & kEplObdAccConst) != 0) || (pDstData == NULL)) {
+		Ret = kEplObdAccessViolation;
+		goto Exit;
+	}
+	//------------------------------------------------------------------------
+	// get size of object
+	// -as ObdSize = ObdGetObjectSize (pSubEntry);
+
+	//------------------------------------------------------------------------
+	// To use the same callback function for ObdWriteEntry as well as for
+	// an SDO download call at first (kEplObdEvPre...) the callback function
+	// with the argument pointer to object size.
+	pCbParam_p->m_uiIndex = uiIndex_p;
+	pCbParam_p->m_uiSubIndex = uiSubIndex_p;
+
+	// Because object size and object pointer are
+	// adapted by user callback function, re-read
+	// this values.
+	ObdSize = EplObdGetObjectSize(pSubEntry);
+	pDstData = (void MEM *)EplObdGetObjectDataPtrIntern(pSubEntry);
+
+	// 09-dec-2004 r.d.:
+	//      Function EplObdWriteEntry() calls new event kEplObdEvWrStringDomain
+	//      for String or Domain which lets called module directly change
+	//      the data pointer or size. This prevents a recursive call to
+	//      the callback function if it calls EplObdGetEntry().
+#if (EPL_OBD_USE_STRING_DOMAIN_IN_RAM != FALSE)
+	if ((pSubEntry->m_Type == kEplObdTypVString) ||
+	    (pSubEntry->m_Type == kEplObdTypDomain) ||
+	    (pSubEntry->m_Type == kEplObdTypOString)) {
+		if (pSubEntry->m_Type == kEplObdTypVString) {
+			// reserve one byte for 0-termination
+			// -as ObdSize -= 1;
+			Size_p += 1;
+		}
+		// fill out new arg-struct
+		MemVStringDomain.m_DownloadSize = Size_p;
+		MemVStringDomain.m_ObjSize = ObdSize;
+		MemVStringDomain.m_pData = pDstData;
+
+		pCbParam_p->m_ObdEvent = kEplObdEvWrStringDomain;
+		pCbParam_p->m_pArg = &MemVStringDomain;
+		//  call user callback
+		Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+					       pObdEntry->m_fpCallback,
+					       pCbParam_p);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		// write back new settings
+		pCurrData = pSubEntry->m_pCurrent;
+		if ((pSubEntry->m_Type == kEplObdTypVString)
+		    || (pSubEntry->m_Type == kEplObdTypOString)) {
+			((tEplObdVString MEM *) pCurrData)->m_Size =
+			    MemVStringDomain.m_ObjSize;
+			((tEplObdVString MEM *) pCurrData)->m_pString =
+			    MemVStringDomain.m_pData;
+		} else		// if (pSdosTableEntry_p->m_bObjType == kEplObdTypDomain)
+		{
+			((tEplObdVarEntry MEM *) pCurrData)->m_Size =
+			    MemVStringDomain.m_ObjSize;
+			((tEplObdVarEntry MEM *) pCurrData)->m_pData =
+			    (void MEM *)MemVStringDomain.m_pData;
+		}
+
+		// Because object size and object pointer are
+		// adapted by user callback function, re-read
+		// this values.
+		ObdSize = MemVStringDomain.m_ObjSize;
+		pDstData = (void MEM *)MemVStringDomain.m_pData;
+	}
+#endif //#if (OBD_USE_STRING_DOMAIN_IN_RAM != FALSE)
+
+	// 07-dec-2004 r.d.: size from application is needed because callback function can change the object size
+	// -as 16.11.04 CbParam.m_pArg     = &ObdSize;
+	// 09-dec-2004 r.d.: CbParam.m_pArg     = &Size_p;
+	pCbParam_p->m_pArg = &ObdSize;
+	pCbParam_p->m_ObdEvent = kEplObdEvInitWrite;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry->m_fpCallback, pCbParam_p);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	if (Size_p > ObdSize) {
+		Ret = kEplObdValueLengthError;
+		goto Exit;
+	}
+
+	if (pSubEntry->m_Type == kEplObdTypVString) {
+		if (((char MEM *)pSrcData_p)[Size_p - 1] == '\0') {	// last byte of source string contains null character
+
+			// reserve one byte in destination for 0-termination
+			Size_p -= 1;
+		} else if (Size_p >= ObdSize) {	// source string is not 0-terminated
+			// and destination buffer is too short
+			Ret = kEplObdValueLengthError;
+			goto Exit;
+		}
+	}
+
+	Ret = EplObdIsNumericalIntern(pSubEntry, &fEntryNumerical);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	if ((fEntryNumerical != FALSE)
+	    && (Size_p != ObdSize)) {
+		// type is numerical, therefor size has to fit, but it does not.
+		Ret = kEplObdValueLengthError;
+		goto Exit;
+	}
+	// use given size, because non-numerical objects can be written with shorter values
+	ObdSize = Size_p;
+
+	// set output parameters
+	*pObdSize_p = ObdSize;
+	*ppObdEntry_p = pObdEntry;
+	*ppSubEntry_p = pSubEntry;
+	*ppDstData_p = pDstData;
+
+	// all checks are done
+	// the caller may now convert the numerial source value to platform byte order in a temporary buffer
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdWriteEntryPost()
+//
+// Description: Function finishes write of data to an OBD entry. Strings
+//              are stored with added '\0' character.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplObdWriteEntryPost(EPL_MCO_DECL_INSTANCE_PTR_
+					      tEplObdEntryPtr pObdEntry_p,
+					      tEplObdSubEntryPtr pSubEntry_p,
+					      tEplObdCbParam MEM * pCbParam_p,
+					      void *pSrcData_p,
+					      void *pDstData_p,
+					      tEplObdSize ObdSize_p)
+{
+
+	tEplKernel Ret;
+
+	// caller converted the source value to platform byte order
+	// now the range of the value may be checked
+
+#if (EPL_OBD_CHECK_OBJECT_RANGE != FALSE)
+	{
+		// check data range
+		Ret = EplObdCheckObjectRange(pSubEntry_p, pSrcData_p);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+#endif
+
+	// now call user callback function to check value
+	// write address of source data to structure of callback parameters
+	// so callback function can check this data
+	pCbParam_p->m_pArg = pSrcData_p;
+	pCbParam_p->m_ObdEvent = kEplObdEvPreWrite;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry_p->m_fpCallback, pCbParam_p);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// copy object data to OBD
+	EPL_MEMCPY(pDstData_p, pSrcData_p, ObdSize_p);
+
+	// terminate string with 0
+	if (pSubEntry_p->m_Type == kEplObdTypVString) {
+		((char MEM *)pDstData_p)[ObdSize_p] = '\0';
+	}
+	// write address of destination to structure of callback parameters
+	// so callback function can change data subsequently
+	pCbParam_p->m_pArg = pDstData_p;
+	pCbParam_p->m_ObdEvent = kEplObdEvPostWrite;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry_p->m_fpCallback, pCbParam_p);
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetObjectSize()
+//
+// Description: function to get size of object
+//              The function determines if an object type an fixed data type (BYTE, WORD, ...)
+//              or non fixed object (string, domain). This information is used to decide
+//              if download data are stored temporary or not. For objects with fixed data length
+//              and types a value range checking can process.
+//              For strings the function returns the whole object size not the
+//              length of string.
+//
+// Parameters:  pSubIndexEntry_p
+//
+// Return:      tEplObdSize
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplObdSize EplObdGetObjectSize(tEplObdSubEntryPtr pSubIndexEntry_p)
+{
+
+	tEplObdSize DataSize = 0;
+	void *pData;
+
+	switch (pSubIndexEntry_p->m_Type) {
+		// -----------------------------------------------------------------
+	case kEplObdTypBool:
+
+		DataSize = 1;
+		break;
+
+		// -----------------------------------------------------------------
+		// ObdTypes which has to be check because numerical values
+	case kEplObdTypInt8:
+		DataSize = sizeof(tEplObdInteger8);
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypUInt8:
+		DataSize = sizeof(tEplObdUnsigned8);
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt16:
+		DataSize = sizeof(tEplObdInteger16);
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypUInt16:
+		DataSize = sizeof(tEplObdUnsigned16);
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt32:
+		DataSize = sizeof(tEplObdInteger32);
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypUInt32:
+		DataSize = sizeof(tEplObdUnsigned32);
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypReal32:
+		DataSize = sizeof(tEplObdReal32);
+		break;
+
+		// -----------------------------------------------------------------
+		// ObdTypes which has to be not checked because not NUM values
+	case kEplObdTypDomain:
+
+		pData = (void *)pSubIndexEntry_p->m_pCurrent;
+		if ((void MEM *)pData != (void MEM *)NULL) {
+			DataSize = ((tEplObdVarEntry MEM *) pData)->m_Size;
+		}
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypVString:
+		//case kEplObdTypUString:
+
+		// If OD entry is defined by macro EPL_OBD_SUBINDEX_ROM_VSTRING
+		// then the current pointer is always NULL. The function
+		// returns the length of default string.
+		pData = (void *)pSubIndexEntry_p->m_pCurrent;
+		if ((void MEM *)pData != (void MEM *)NULL) {
+			// The max. size of strings defined by STRING-Macro is stored in
+			// tEplObdVString of current value.
+			// (types tEplObdVString, tEplObdOString and tEplObdUString has the same members)
+			DataSize = ((tEplObdVString MEM *) pData)->m_Size;
+		} else {
+			// The current position is not decleared. The string
+			// is located in ROM, therefor use default pointer.
+			pData = (void *)pSubIndexEntry_p->m_pDefault;
+			if ((CONST void ROM *)pData != (CONST void ROM *)NULL) {
+				// The max. size of strings defined by STRING-Macro is stored in
+				// tEplObdVString of default value.
+				DataSize =
+				    ((CONST tEplObdVString ROM *) pData)->
+				    m_Size;
+			}
+		}
+
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypOString:
+
+		pData = (void *)pSubIndexEntry_p->m_pCurrent;
+		if ((void MEM *)pData != (void MEM *)NULL) {
+			// The max. size of strings defined by STRING-Macro is stored in
+			// tEplObdVString of current value.
+			// (types tEplObdVString, tEplObdOString and tEplObdUString has the same members)
+			DataSize = ((tEplObdOString MEM *) pData)->m_Size;
+		} else {
+			// The current position is not decleared. The string
+			// is located in ROM, therefor use default pointer.
+			pData = (void *)pSubIndexEntry_p->m_pDefault;
+			if ((CONST void ROM *)pData != (CONST void ROM *)NULL) {
+				// The max. size of strings defined by STRING-Macro is stored in
+				// tEplObdVString of default value.
+				DataSize =
+				    ((CONST tEplObdOString ROM *) pData)->
+				    m_Size;
+			}
+		}
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt24:
+	case kEplObdTypUInt24:
+
+		DataSize = 3;
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt40:
+	case kEplObdTypUInt40:
+
+		DataSize = 5;
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt48:
+	case kEplObdTypUInt48:
+
+		DataSize = 6;
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt56:
+	case kEplObdTypUInt56:
+
+		DataSize = 7;
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt64:
+	case kEplObdTypUInt64:
+	case kEplObdTypReal64:
+
+		DataSize = 8;
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypTimeOfDay:
+	case kEplObdTypTimeDiff:
+
+		DataSize = 6;
+		break;
+
+		// -----------------------------------------------------------------
+	default:
+		break;
+	}
+
+	return DataSize;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetObjectDefaultPtr()
+//
+// Description: function to get the default pointer (type specific)
+//
+// Parameters:  pSubIndexEntry_p    = pointer to subindex structure
+//
+// Returns:     (void *)   = pointer to default value
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static void *EplObdGetObjectDefaultPtr(tEplObdSubEntryPtr pSubIndexEntry_p)
+{
+
+	void *pDefault;
+	tEplObdType Type;
+
+	ASSERTMSG(pSubIndexEntry_p != NULL,
+		  "EplObdGetObjectDefaultPtr(): pointer to SubEntry not valid!\n");
+
+	// get address to default data from default pointer
+	pDefault = pSubIndexEntry_p->m_pDefault;
+	if (pDefault != NULL) {
+		// there are some special types, whose default pointer always is NULL or has to get from other structure
+		// get type from subindex structure
+		Type = pSubIndexEntry_p->m_Type;
+
+		// check if object type is a string value
+		if ((Type == kEplObdTypVString)	/* ||
+						   (Type == kEplObdTypUString) */ ) {
+
+			// EPL_OBD_SUBINDEX_RAM_VSTRING
+			//    tEplObdSize         m_Size;       --> size of default string
+			//    char *    m_pDefString; --> pointer to  default string
+			//    char *    m_pString;    --> pointer to string in RAM
+			//
+			pDefault =
+			    (void *)((tEplObdVString *) pDefault)->m_pString;
+		} else if (Type == kEplObdTypOString) {
+			pDefault =
+			    (void *)((tEplObdOString *) pDefault)->m_pString;
+		}
+	}
+
+	return pDefault;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetVarEntry()
+//
+// Description: gets a variable entry of an object
+//
+// Parameters:  pSubindexEntry_p
+//              ppVarEntry_p
+//
+// Return:      tCopKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplObdGetVarEntry(tEplObdSubEntryPtr pSubindexEntry_p,
+				    tEplObdVarEntry MEM ** ppVarEntry_p)
+{
+
+	tEplKernel Ret = kEplObdVarEntryNotExist;
+
+	ASSERT(ppVarEntry_p != NULL);	// is not allowed to be NULL
+	ASSERT(pSubindexEntry_p != NULL);
+
+	// check VAR-Flag - only this object points to variables
+	if ((pSubindexEntry_p->m_Access & kEplObdAccVar) != 0) {
+		// check if object is an array
+		if ((pSubindexEntry_p->m_Access & kEplObdAccArray) != 0) {
+			*ppVarEntry_p =
+			    &((tEplObdVarEntry MEM *) pSubindexEntry_p->
+			      m_pCurrent)[pSubindexEntry_p->m_uiSubIndex - 1];
+		} else {
+			*ppVarEntry_p =
+			    (tEplObdVarEntry MEM *) pSubindexEntry_p->
+			    m_pCurrent;
+		}
+
+		Ret = kEplSuccessful;
+	}
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetEntry()
+//
+// Description: gets a index entry from OD
+//
+// Parameters:  uiIndex_p       =   Index number
+//              uiSubindex_p    =   Subindex number
+//              ppObdEntry_p    =   pointer to the pointer to the entry
+//              ppObdSubEntry_p =   pointer to the pointer to the subentry
+//
+// Return:      tEplKernel
+
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplObdGetEntry(EPL_MCO_DECL_INSTANCE_PTR_
+				 unsigned int uiIndex_p,
+				 unsigned int uiSubindex_p,
+				 tEplObdEntryPtr * ppObdEntry_p,
+				 tEplObdSubEntryPtr * ppObdSubEntry_p)
+{
+
+	tEplObdEntryPtr pObdEntry;
+	tEplObdCbParam MEM CbParam;
+	tEplKernel Ret;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	//------------------------------------------------------------------------
+	// get address of entry of index
+	Ret =
+	    EplObdGetIndexIntern(&EPL_MCO_GLB_VAR(m_ObdInitParam), uiIndex_p,
+				 &pObdEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	//------------------------------------------------------------------------
+	// get address of entry of subindex
+	Ret = EplObdGetSubindexIntern(pObdEntry, uiSubindex_p, ppObdSubEntry_p);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	//------------------------------------------------------------------------
+	// call callback function to inform user/stack that an object will be searched
+	// if the called module returnes an error then we abort the searching with kEplObdIndexNotExist
+	CbParam.m_uiIndex = uiIndex_p;
+	CbParam.m_uiSubIndex = uiSubindex_p;
+	CbParam.m_pArg = NULL;
+	CbParam.m_ObdEvent = kEplObdEvCheckExist;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry->m_fpCallback, &CbParam);
+	if (Ret != kEplSuccessful) {
+		Ret = kEplObdIndexNotExist;
+		goto Exit;
+	}
+	//------------------------------------------------------------------------
+	// it is allowed to set ppObdEntry_p to NULL
+	// if so, no address will be written to calling function
+	if (ppObdEntry_p != NULL) {
+		*ppObdEntry_p = pObdEntry;
+	}
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetObjectCurrentPtr()
+//
+// Description: function to get Current pointer (type specific)
+//
+// Parameters:  pSubIndexEntry_p
+//
+// Return:      void MEM*
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static void MEM *EplObdGetObjectCurrentPtr(tEplObdSubEntryPtr pSubIndexEntry_p)
+{
+
+	void MEM *pData;
+	unsigned int uiArrayIndex;
+	tEplObdSize Size;
+
+	pData = pSubIndexEntry_p->m_pCurrent;
+
+	// check if constant object
+	if (pData != NULL) {
+		// check if object is an array
+		if ((pSubIndexEntry_p->m_Access & kEplObdAccArray) != 0) {
+			// calculate correct data pointer
+			uiArrayIndex = pSubIndexEntry_p->m_uiSubIndex - 1;
+			if ((pSubIndexEntry_p->m_Access & kEplObdAccVar) != 0) {
+				Size = sizeof(tEplObdVarEntry);
+			} else {
+				Size = EplObdGetObjectSize(pSubIndexEntry_p);
+			}
+			pData = ((BYTE MEM *) pData) + (Size * uiArrayIndex);
+		}
+		// check if VarEntry
+		if ((pSubIndexEntry_p->m_Access & kEplObdAccVar) != 0) {
+			// The data pointer is stored in VarEntry->pData
+			pData = ((tEplObdVarEntry MEM *) pData)->m_pData;
+		}
+		// the default pointer is stored for strings in tEplObdVString
+		else if ((pSubIndexEntry_p->m_Type == kEplObdTypVString)	/* ||
+										   (pSubIndexEntry_p->m_Type == kEplObdTypUString)    */
+			 ) {
+			pData =
+			    (void MEM *)((tEplObdVString MEM *) pData)->
+			    m_pString;
+		} else if (pSubIndexEntry_p->m_Type == kEplObdTypOString) {
+			pData =
+			    (void MEM *)((tEplObdOString MEM *) pData)->
+			    m_pString;
+		}
+	}
+
+	return pData;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetIndexIntern()
+//
+// Description: gets a index entry from OD
+//
+// Parameters:  pInitParam_p
+//              uiIndex_p
+//              ppObdEntry_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplObdGetIndexIntern(tEplObdInitParam MEM * pInitParam_p,
+				       unsigned int uiIndex_p,
+				       tEplObdEntryPtr * ppObdEntry_p)
+{
+
+	tEplObdEntryPtr pObdEntry;
+	tEplKernel Ret;
+	unsigned int uiIndex;
+
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+
+	unsigned int nLoop;
+
+	// if user OD is used then objekts also has to be searched in user OD
+	// there is less code need if we do this in a loop
+	nLoop = 2;
+
+#endif
+
+	ASSERTMSG(ppObdEntry_p != NULL,
+		  "EplObdGetIndexIntern(): pointer to index entry is NULL!\n");
+
+	Ret = kEplObdIndexNotExist;
+
+	// get start address of OD part
+	// start address depends on object index because
+	// object dictionary is divided in 3 parts
+	if ((uiIndex_p >= 0x1000) && (uiIndex_p < 0x2000)) {
+		pObdEntry = pInitParam_p->m_pPart;
+	} else if ((uiIndex_p >= 0x2000) && (uiIndex_p < 0x6000)) {
+		pObdEntry = pInitParam_p->m_pManufacturerPart;
+	}
+	// index range 0xA000 to 0xFFFF is reserved for DSP-405
+	// DS-301 defines that range 0x6000 to 0x9FFF (!!!) is stored if "store" was written to 0x1010/3.
+	// Therefore default configuration is OBD_INCLUDE_A000_TO_DEVICE_PART = FALSE.
+	// But a CANopen Application which does not implement dynamic OD or user-OD but wants to use static objets 0xA000...
+	// should set OBD_INCLUDE_A000_TO_DEVICE_PART to TRUE.
+
+#if (EPL_OBD_INCLUDE_A000_TO_DEVICE_PART == FALSE)
+	else if ((uiIndex_p >= 0x6000) && (uiIndex_p < 0x9FFF))
+#else
+	else if ((uiIndex_p >= 0x6000) && (uiIndex_p < 0xFFFF))
+#endif
+	{
+		pObdEntry = pInitParam_p->m_pDevicePart;
+	}
+
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+
+	// if index does not match in static OD then index only has to be searched in user OD
+	else {
+		// begin from first entry of user OD part
+		pObdEntry = pInitParam_p->m_pUserPart;
+
+		// no user OD is available
+		if (pObdEntry == NULL) {
+			goto Exit;
+		}
+		// loop must only run once
+		nLoop = 1;
+	}
+
+	do {
+
+#else
+
+	// no user OD is available
+	// so other object can be found in OD
+	else {
+		Ret = kEplObdIllegalPart;
+		goto Exit;
+	}
+
+#endif
+
+	// note:
+	// The end of Index table is marked with m_uiIndex = 0xFFFF.
+	// If this function will be called with wIndex_p = 0xFFFF, entry
+	// should not be found. Therefor it is important to use
+	// while{} instead of do{}while !!!
+
+	// get first index of index table
+	uiIndex = pObdEntry->m_uiIndex;
+
+	// search Index in OD part
+	while (uiIndex != EPL_OBD_TABLE_INDEX_END) {
+		// go to the end of this function if index is found
+		if (uiIndex_p == uiIndex) {
+			// write address of OD entry to calling function
+			*ppObdEntry_p = pObdEntry;
+			Ret = kEplSuccessful;
+			goto Exit;
+		}
+		// objects are sorted in OD
+		// if the current index in OD is greater than the index which is to search then break loop
+		// in this case user OD has to be search too
+		if (uiIndex_p < uiIndex) {
+			break;
+		}
+		// next entry in index table
+		pObdEntry++;
+
+		// get next index of index table
+		uiIndex = pObdEntry->m_uiIndex;
+	}
+
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+
+	// begin from first entry of user OD part
+	pObdEntry = pInitParam_p->m_pUserPart;
+
+	// no user OD is available
+	if (pObdEntry == NULL) {
+		goto Exit;
+	}
+	// switch next loop for user OD
+	nLoop--;
+
+}
+
+while (nLoop > 0) ;
+
+#endif
+
+    // in this line Index was not found
+
+Exit:
+
+return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetSubindexIntern()
+//
+// Description: gets a subindex entry from a index entry
+//
+// Parameters:  pObdEntry_p
+//              bSubIndex_p
+//              ppObdSubEntry_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplObdGetSubindexIntern(tEplObdEntryPtr pObdEntry_p,
+					  unsigned int uiSubIndex_p,
+					  tEplObdSubEntryPtr * ppObdSubEntry_p)
+{
+
+	tEplObdSubEntryPtr pSubEntry;
+	unsigned int nSubIndexCount;
+	tEplKernel Ret;
+
+	ASSERTMSG(pObdEntry_p != NULL,
+		  "EplObdGetSubindexIntern(): pointer to index is NULL!\n");
+	ASSERTMSG(ppObdSubEntry_p != NULL,
+		  "EplObdGetSubindexIntern(): pointer to subindex is NULL!\n");
+
+	Ret = kEplObdSubindexNotExist;
+
+	// get start address of subindex table and count of subindices
+	pSubEntry = pObdEntry_p->m_pSubIndex;
+	nSubIndexCount = pObdEntry_p->m_uiCount;
+	ASSERTMSG((pSubEntry != NULL) && (nSubIndexCount > 0), "ObdGetSubindexIntern(): invalid subindex table within index table!\n");	// should never be NULL
+
+	// search subindex in subindex table
+	while (nSubIndexCount > 0) {
+		// check if array is found
+		if ((pSubEntry->m_Access & kEplObdAccArray) != 0) {
+			// check if subindex is in range
+			if (uiSubIndex_p < pObdEntry_p->m_uiCount) {
+				// update subindex number (subindex entry of an array is always in RAM !!!)
+				pSubEntry->m_uiSubIndex = uiSubIndex_p;
+				*ppObdSubEntry_p = pSubEntry;
+				Ret = kEplSuccessful;
+				goto Exit;
+			}
+		}
+		// go to the end of this function if subindex is found
+		else if (uiSubIndex_p == pSubEntry->m_uiSubIndex) {
+			*ppObdSubEntry_p = pSubEntry;
+			Ret = kEplSuccessful;
+			goto Exit;
+		}
+		// objects are sorted in OD
+		// if the current subindex in OD is greater than the subindex which is to search then break loop
+		// in this case user OD has to be search too
+		if (uiSubIndex_p < pSubEntry->m_uiSubIndex) {
+			break;
+		}
+
+		pSubEntry++;
+		nSubIndexCount--;
+	}
+
+	// in this line SubIndex was not fount
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdSetStoreLoadObjCallback()
+//
+// Description: function set address to callbackfunction for command Store and Load
+//
+// Parameters:  fpCallback_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if (EPL_OBD_USE_STORE_RESTORE != FALSE)
+EPLDLLEXPORT tEplKernel PUBLIC
+EplObdSetStoreLoadObjCallback(EPL_MCO_DECL_INSTANCE_PTR_
+			      tEplObdStoreLoadObjCallback fpCallback_p)
+{
+
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	// set new address of callback function
+	EPL_MCO_GLB_VAR(m_fpStoreLoadObjCallback) = fpCallback_p;
+
+	return kEplSuccessful;
+
+}
+#endif // (EPL_OBD_USE_STORE_RESTORE != FALSE)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdAccessOdPartIntern()
+//
+// Description: runs through OD and executes a job
+//
+// Parameters:  CurrentOdPart_p
+//              pObdEnty_p
+//              Direction_p     = what is to do (load values from flash or EEPROM, store, ...)
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplObdAccessOdPartIntern(EPL_MCO_DECL_INSTANCE_PTR_
+					   tEplObdPart CurrentOdPart_p,
+					   tEplObdEntryPtr pObdEnty_p,
+					   tEplObdDir Direction_p)
+{
+
+	tEplObdSubEntryPtr pSubIndex;
+	unsigned int nSubIndexCount;
+	tEplObdAccess Access;
+	void MEM *pDstData;
+	void *pDefault;
+	tEplObdSize ObjSize;
+	tEplKernel Ret;
+	tEplObdCbStoreParam MEM CbStore;
+	tEplObdVarEntry MEM *pVarEntry;
+
+	ASSERT(pObdEnty_p != NULL);
+
+	Ret = kEplSuccessful;
+
+	// prepare structure for STORE RESTORE callback function
+	CbStore.m_bCurrentOdPart = (BYTE) CurrentOdPart_p;
+	CbStore.m_pData = NULL;
+	CbStore.m_ObjSize = 0;
+
+	// command of first action depends on direction to access
+#if (EPL_OBD_USE_STORE_RESTORE != FALSE)
+	if (Direction_p == kEplObdDirLoad) {
+		CbStore.m_bCommand = (BYTE) kEplObdCommOpenRead;
+
+		// call callback function for previous command
+		Ret = EplObdCallStoreCallback(EPL_MCO_INSTANCE_PTR_ & CbStore);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		// set command for index and subindex loop
+		CbStore.m_bCommand = (BYTE) kEplObdCommReadObj;
+	} else if (Direction_p == kEplObdDirStore) {
+		CbStore.m_bCommand = (BYTE) kEplObdCommOpenWrite;
+
+		// call callback function for previous command
+		Ret = EplObdCallStoreCallback(EPL_MCO_INSTANCE_PTR_ & CbStore);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		// set command for index and subindex loop
+		CbStore.m_bCommand = (BYTE) kEplObdCommWriteObj;
+	}
+#endif // (EPL_OBD_USE_STORE_RESTORE != FALSE)
+
+	// we should not restore the OD values here
+	// the next NMT command "Reset Node" or "Reset Communication" resets the OD data
+	if (Direction_p != kEplObdDirRestore) {
+		// walk through OD part till end is found
+		while (pObdEnty_p->m_uiIndex != EPL_OBD_TABLE_INDEX_END) {
+			// get address to subindex table and count of subindices
+			pSubIndex = pObdEnty_p->m_pSubIndex;
+			nSubIndexCount = pObdEnty_p->m_uiCount;
+			ASSERT((pSubIndex != NULL) && (nSubIndexCount > 0));	// should never be NULL
+
+			// walk through subindex table till all subinices were restored
+			while (nSubIndexCount != 0) {
+				Access = (tEplObdAccess) pSubIndex->m_Access;
+
+				// get pointer to current and default data
+				pDefault = EplObdGetObjectDefaultPtr(pSubIndex);
+				pDstData = EplObdGetObjectCurrentPtr(pSubIndex);
+
+				// NOTE (for kEplObdTypVString):
+				//      The function returnes the max. number of bytes for a
+				//      current string.
+				//      r.d.: For stings the default-size will be read in other lines following (kEplObdDirInit).
+				ObjSize = EplObdGetObjectSize(pSubIndex);
+
+				// switch direction of OD access
+				switch (Direction_p) {
+					// --------------------------------------------------------------------------
+					// VarEntry structures has to be initialized
+				case kEplObdDirInit:
+
+					// If VAR-Flag is set, m_pCurrent means not address of data
+					// but address of tEplObdVarEntry. Address of data has to be get from
+					// this structure.
+					if ((Access & kEplObdAccVar) != 0) {
+						EplObdGetVarEntry(pSubIndex,
+								  &pVarEntry);
+						EplObdInitVarEntry(pVarEntry,
+								   pSubIndex->
+								   m_Type,
+								   ObjSize);
+/*
+                            if ((Access & kEplObdAccArray) == 0)
+                            {
+                                EplObdInitVarEntry (pSubIndex->m_pCurrent, pSubIndex->m_Type, ObjSize);
+                            }
+                            else
+                            {
+                                EplObdInitVarEntry ((tEplObdVarEntry MEM*) (((BYTE MEM*) pSubIndex->m_pCurrent) + (sizeof (tEplObdVarEntry) * pSubIndex->m_uiSubIndex)),
+                                    pSubIndex->m_Type, ObjSize);
+                            }
+*/
+						// at this time no application variable is defined !!!
+						// therefore data can not be copied.
+						break;
+					} else if (pSubIndex->m_Type ==
+						   kEplObdTypVString) {
+						// If pointer m_pCurrent is not equal to NULL then the
+						// string was defined with EPL_OBD_SUBINDEX_RAM_VSTRING. The current
+						// pointer points to struct tEplObdVString located in MEM.
+						// The element size includes the max. number of
+						// bytes. The element m_pString includes the pointer
+						// to string in MEM. The memory location of default string
+						// must be copied to memory location of current string.
+
+						pDstData =
+						    pSubIndex->m_pCurrent;
+						if (pDstData != NULL) {
+							// 08-dec-2004: code optimization !!!
+							//              entries ((tEplObdVStringDef ROM*) pSubIndex->m_pDefault)->m_pString
+							//              and ((tEplObdVStringDef ROM*) pSubIndex->m_pDefault)->m_Size were read
+							//              twice. thats not necessary!
+
+							// For copying data we have to set the destination pointer to the real RAM string. This
+							// pointer to RAM string is located in default string info structure. (translated r.d.)
+							pDstData =
+							    (void MEM
+							     *)((tEplObdVStringDef ROM *) pSubIndex->m_pDefault)->m_pString;
+							ObjSize =
+							    ((tEplObdVStringDef
+							      ROM *) pSubIndex->
+							     m_pDefault)->
+							    m_Size;
+
+							((tEplObdVString MEM *)
+							 pSubIndex->
+							 m_pCurrent)->
+				     m_pString = pDstData;
+							((tEplObdVString MEM *)
+							 pSubIndex->
+							 m_pCurrent)->m_Size =
+				     ObjSize;
+						}
+
+					} else if (pSubIndex->m_Type ==
+						   kEplObdTypOString) {
+						pDstData =
+						    pSubIndex->m_pCurrent;
+						if (pDstData != NULL) {
+							// 08-dec-2004: code optimization !!!
+							//              entries ((tEplObdOStringDef ROM*) pSubIndex->m_pDefault)->m_pString
+							//              and ((tEplObdOStringDef ROM*) pSubIndex->m_pDefault)->m_Size were read
+							//              twice. thats not necessary!
+
+							// For copying data we have to set the destination pointer to the real RAM string. This
+							// pointer to RAM string is located in default string info structure. (translated r.d.)
+							pDstData =
+							    (void MEM
+							     *)((tEplObdOStringDef ROM *) pSubIndex->m_pDefault)->m_pString;
+							ObjSize =
+							    ((tEplObdOStringDef
+							      ROM *) pSubIndex->
+							     m_pDefault)->
+							    m_Size;
+
+							((tEplObdOString MEM *)
+							 pSubIndex->
+							 m_pCurrent)->
+				     m_pString = pDstData;
+							((tEplObdOString MEM *)
+							 pSubIndex->
+							 m_pCurrent)->m_Size =
+				     ObjSize;
+						}
+
+					}
+
+					// no break !! because copy of data has to done too.
+
+					// --------------------------------------------------------------------------
+					// all objects has to be restored with default values
+				case kEplObdDirRestore:
+
+					// 09-dec-2004 r.d.: optimization! the same code for kEplObdDirRestore and kEplObdDirLoad
+					//                   is replaced to function ObdCopyObjectData() with a new parameter.
+
+					// restore object data for init phase
+					EplObdCopyObjectData(pDstData, pDefault,
+							     ObjSize,
+							     pSubIndex->m_Type);
+					break;
+
+					// --------------------------------------------------------------------------
+					// objects with attribute kEplObdAccStore has to be load from EEPROM or from a file
+				case kEplObdDirLoad:
+
+					// restore object data for init phase
+					EplObdCopyObjectData(pDstData, pDefault,
+							     ObjSize,
+							     pSubIndex->m_Type);
+
+					// no break !! because callback function has to be called too.
+
+					// --------------------------------------------------------------------------
+					// objects with attribute kEplObdAccStore has to be stored in EEPROM or in a file
+				case kEplObdDirStore:
+
+					// when attribute kEplObdAccStore is set, then call callback function
+#if (EPL_OBD_USE_STORE_RESTORE != FALSE)
+					if ((Access & kEplObdAccStore) != 0) {
+						// fill out data pointer and size of data
+						CbStore.m_pData = pDstData;
+						CbStore.m_ObjSize = ObjSize;
+
+						// call callback function for read or write object
+						Ret =
+						    ObdCallStoreCallback
+						    (EPL_MCO_INSTANCE_PTR_ &
+						     CbStore);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+					}
+#endif // (EPL_OBD_USE_STORE_RESTORE != FALSE)
+					break;
+
+					// --------------------------------------------------------------------------
+					// if OD Builder key has to be checked no access to subindex and data should be made
+				case kEplObdDirOBKCheck:
+
+					// no break !! because we want to break the second loop too.
+
+					// --------------------------------------------------------------------------
+					// unknown Direction
+				default:
+
+					// so we can break the second loop earler
+					nSubIndexCount = 1;
+					break;
+				}
+
+				nSubIndexCount--;
+
+				// next subindex entry
+				if ((Access & kEplObdAccArray) == 0) {
+					pSubIndex++;
+					if ((nSubIndexCount > 0)
+					    &&
+					    ((pSubIndex->
+					      m_Access & kEplObdAccArray) !=
+					     0)) {
+						// next subindex points to an array
+						// reset subindex number
+						pSubIndex->m_uiSubIndex = 1;
+					}
+				} else {
+					if (nSubIndexCount > 0) {
+						// next subindex points to an array
+						// increment subindex number
+						pSubIndex->m_uiSubIndex++;
+					}
+				}
+			}
+
+			// next index entry
+			pObdEnty_p++;
+		}
+	}
+	// -----------------------------------------------------------------------------------------
+	// command of last action depends on direction to access
+	if (Direction_p == kEplObdDirOBKCheck) {
+
+		goto Exit;
+	}
+#if (EPL_OBD_USE_STORE_RESTORE != FALSE)
+	else {
+		if (Direction_p == kEplObdDirLoad) {
+			CbStore.m_bCommand = (BYTE) kEplObdCommCloseRead;
+		} else if (Direction_p == kEplObdDirStore) {
+			CbStore.m_bCommand = (BYTE) kEplObdCommCloseWrite;
+		} else if (Direction_p == kEplObdDirRestore) {
+			CbStore.m_bCommand = (BYTE) kEplObdCommClear;
+		} else {
+			goto Exit;
+		}
+
+		// call callback function for last command
+		Ret = EplObdCallStoreCallback(EPL_MCO_INSTANCE_PTR_ & CbStore);
+	}
+#endif // (EPL_OBD_USE_STORE_RESTORE != FALSE)
+
+//    goto Exit;
+
+      Exit:
+
+	return Ret;
+
+}
+
+// ----------------------------------------------------------------------------
+// Function:    EplObdCopyObjectData()
+//
+// Description: checks pointers to object data and copy them from source to destination
+//
+// Parameters:  pDstData_p              = destination pointer
+//              pSrcData_p              = source pointer
+//              ObjSize_p               = size of object
+//              ObjType_p               =
+//
+// Returns:     tEplKernel              = error code
+// ----------------------------------------------------------------------------
+
+static void EplObdCopyObjectData(void MEM * pDstData_p,
+				 void *pSrcData_p,
+				 tEplObdSize ObjSize_p, tEplObdType ObjType_p)
+{
+
+	tEplObdSize StrSize = 0;
+
+	// it is allowed to set default and current address to NULL (nothing to copy)
+	if (pDstData_p != NULL) {
+
+		if (ObjType_p == kEplObdTypVString) {
+			// The function calculates the really number of characters of string. The
+			// object entry size can be bigger as string size of default string.
+			// The '\0'-termination is included. A string with no characters has a
+			// size of 1.
+			StrSize =
+			    EplObdGetStrLen((void *)pSrcData_p, ObjSize_p,
+					    kEplObdTypVString);
+
+			// If the string length is greater than or equal to the entry size in OD then only copy
+			// entry size - 1 and always set the '\0'-termination.
+			if (StrSize >= ObjSize_p) {
+				StrSize = ObjSize_p - 1;
+			}
+		}
+
+		if (pSrcData_p != NULL) {
+			// copy data
+			EPL_MEMCPY(pDstData_p, pSrcData_p, ObjSize_p);
+
+			if (ObjType_p == kEplObdTypVString) {
+				((char MEM *)pDstData_p)[StrSize] = '\0';
+			}
+		}
+	}
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdIsNumericalIntern()
+//
+// Description: function checks if a entry is numerical or not
+//
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_ = Instancepointer
+//              uiIndex_p           = Index
+//              uiSubIndex_p        = Subindex
+//              pfEntryNumerical_p  = pointer to BOOL for returnvalue
+//                                  -> TRUE if entry a numerical value
+//                                  -> FALSE if entry not a numerical value
+//
+// Return:      tEplKernel = Errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplObdIsNumericalIntern(tEplObdSubEntryPtr pObdSubEntry_p,
+					  BOOL * pfEntryNumerical_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// get Type
+	if ((pObdSubEntry_p->m_Type == kEplObdTypVString)
+	    || (pObdSubEntry_p->m_Type == kEplObdTypOString)
+	    || (pObdSubEntry_p->m_Type == kEplObdTypDomain)) {	// not numerical types
+		*pfEntryNumerical_p = FALSE;
+	} else {		// numerical types
+		*pfEntryNumerical_p = TRUE;
+	}
+
+	return Ret;
+
+}
+
+// -------------------------------------------------------------------------
+// function to classify object type (fixed/non fixed)
+// -------------------------------------------------------------------------
+
+// ----------------------------------------------------------------------------
+// Function:    EplObdCallStoreCallback()
+//
+// Description: checks address to callback function and calles it when unequal
+//              to NULL
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_ = (instance pointer)
+//              pCbStoreParam_p        = address to callback parameters
+//
+// Returns:     tEplKernel             = error code
+// ----------------------------------------------------------------------------
+#if (EPL_OBD_USE_STORE_RESTORE != FALSE)
+static tEplKernel EplObdCallStoreCallback(EPL_MCO_DECL_INSTANCE_PTR_
+					  tEplObdCbStoreParam MEM *
+					  pCbStoreParam_p)
+{
+
+	tEplKernel Ret = kEplSuccessful;
+
+	ASSERT(pCbStoreParam_p != NULL);
+
+	// check if function pointer is NULL - if so, no callback should be called
+	if (EPL_MCO_GLB_VAR(m_fpStoreLoadObjCallback) != NULL) {
+		Ret =
+		    EPL_MCO_GLB_VAR(m_fpStoreLoadObjCallback)
+		    (EPL_MCO_INSTANCE_PARAM_IDX_()
+		     pCbStoreParam_p);
+	}
+
+	return Ret;
+
+}
+#endif // (EPL_OBD_USE_STORE_RESTORE != FALSE)
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetObjectDataPtrIntern()
+//
+// Description: Function gets the data pointer of an object.
+//              It returnes the current data pointer. But if object is an
+//              constant object it returnes the default pointer.
+//
+// Parameters:  pSubindexEntry_p = pointer to subindex entry
+//
+// Return:      void *    = pointer to object data
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+void *EplObdGetObjectDataPtrIntern(tEplObdSubEntryPtr pSubindexEntry_p)
+{
+
+	void *pData;
+	tEplObdAccess Access;
+
+	ASSERTMSG(pSubindexEntry_p != NULL,
+		  "EplObdGetObjectDataPtrIntern(): pointer to SubEntry not valid!\n");
+
+	// there are are some objects whose data pointer has to get from other structure
+	// get access type for this object
+	Access = pSubindexEntry_p->m_Access;
+
+	// If object has access type = const,
+	// for data only exists default values.
+	if ((Access & kEplObdAccConst) != 0) {
+		// The pointer to defualt value can be received from ObdGetObjectDefaultPtr()
+		pData = ((void *)EplObdGetObjectDefaultPtr(pSubindexEntry_p));
+	} else {
+		// The pointer to current value can be received from ObdGetObjectCurrentPtr()
+		pData = ((void *)EplObdGetObjectCurrentPtr(pSubindexEntry_p));
+	}
+
+	return pData;
+
+}
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+// EOF
diff --git a/drivers/staging/epl/EplObd.h b/drivers/staging/epl/EplObd.h
new file mode 100644
index 000000000000..88cc11e34ccd
--- /dev/null
+++ b/drivers/staging/epl/EplObd.h
@@ -0,0 +1,464 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for api function of EplOBD-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObd.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                Microsoft VC7
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/02 k.t.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "EplInc.h"
+
+#ifndef _EPLOBD_H_
+#define _EPLOBD_H_
+
+// ============================================================================
+// defines
+// ============================================================================
+
+#define EPL_OBD_TABLE_INDEX_END     0xFFFF
+
+// for the usage of BOOLEAN in OD
+#define OBD_TRUE    0x01
+#define OBD_FALSE   0x00
+
+// default OD index for Node id
+#define EPL_OBD_NODE_ID_INDEX               0x1F93
+// default subindex for NodeId in OD
+#define EPL_OBD_NODE_ID_SUBINDEX            0x01
+// default subindex for NodeIDByHW_BOOL
+#define EPL_OBD_NODE_ID_HWBOOL_SUBINDEX     0x02
+
+// ============================================================================
+// enums
+// ============================================================================
+
+// directions for access to object dictionary
+typedef enum {
+	kEplObdDirInit = 0x00,	// initialising after power on
+	kEplObdDirStore = 0x01,	// store all object values to non volatile memory
+	kEplObdDirLoad = 0x02,	// load all object values from non volatile memory
+	kEplObdDirRestore = 0x03,	// deletes non volatile memory (restore)
+	kEplObdDirOBKCheck = 0xFF	// reserved
+} tEplObdDir;
+
+// commands for store
+typedef enum {
+	kEplObdCommNothing = 0x00,
+	kEplObdCommOpenWrite = 0x01,
+	kEplObdCommWriteObj = 0x02,
+	kEplObdCommCloseWrite = 0x03,
+	kEplObdCommOpenRead = 0x04,
+	kEplObdCommReadObj = 0x05,
+	kEplObdCommCloseRead = 0x06,
+	kEplObdCommClear = 0x07,
+	kEplObdCommUnknown = 0xFF
+} tEplObdCommand;
+
+//-----------------------------------------------------------------------------------------------------------
+// events of object callback function
+typedef enum {
+//                                                                                                      m_pArg points to
+//                                                                                                    ---------------------
+	kEplObdEvCheckExist = 0x06,	// checking if object does exist (reading and writing)    NULL
+	kEplObdEvPreRead = 0x00,	// before reading an object                               source data buffer in OD
+	kEplObdEvPostRead = 0x01,	// after reading an object                                destination data buffer from caller
+	kEplObdEvWrStringDomain = 0x07,	// event for changing string/domain data pointer or size  struct tEplObdVStringDomain in RAM
+	kEplObdEvInitWrite = 0x04,	// initializes writing an object (checking object size)   size of object in OD (tEplObdSize)
+	kEplObdEvPreWrite = 0x02,	// before writing an object                               source data buffer from caller
+	kEplObdEvPostWrite = 0x03,	// after writing an object                                destination data buffer in OD
+//    kEplObdEvAbortSdo              = 0x05     // after an abort of an SDO transfer
+
+} tEplObdEvent;
+
+// part of OD (bit oriented)
+typedef unsigned int tEplObdPart;
+
+#define kEplObdPartNo          0x00	// nothing
+#define kEplObdPartGen         0x01	//  part      (0x1000 - 0x1FFF)
+#define kEplObdPartMan         0x02	// manufacturer part (0x2000 - 0x5FFF)
+#define kEplObdPartDev         0x04	// device part       (0x6000 - 0x9FFF)
+#define kEplObdPartUsr         0x08	// dynamic part e.g. for ICE61131-3
+
+// combinations
+#define kEplObdPartApp         (              kEplObdPartMan | kEplObdPartDev | kEplObdPartUsr)	// manufacturer and device part (0x2000 - 0x9FFF) and user OD
+#define kEplObdPartAll         (kEplObdPartGen | kEplObdPartMan | kEplObdPartDev | kEplObdPartUsr)	// whole OD
+
+//-----------------------------------------------------------------------------------------------------------
+// access types for objects
+// must be a difine because bit-flags
+typedef unsigned int tEplObdAccess;
+
+#define kEplObdAccRead         0x01	// object can be read
+#define kEplObdAccWrite        0x02	// object can be written
+#define kEplObdAccConst        0x04	// object contains a constant value
+#define kEplObdAccPdo          0x08	// object can be mapped in a PDO
+#define kEplObdAccArray        0x10	// object contains an array of numerical values
+#define kEplObdAccRange        0x20	// object contains lower and upper limit
+#define kEplObdAccVar          0x40	// object data is placed in application
+#define kEplObdAccStore        0x80	// object data can be stored to non volatile memory
+
+// combinations (not all combinations are required)
+#define kEplObdAccR            (0            | 0          | 0            | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccW            (0            | 0          | 0            | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccRW           (0            | 0          | 0            | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccCR           (0            | 0          | 0            | 0          | kEplObdAccConst | 0            | kEplObdAccRead)
+#define kEplObdAccGR           (0            | 0          | kEplObdAccRange | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccGW           (0            | 0          | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccGRW          (0            | 0          | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccVR           (0            | kEplObdAccVar | 0            | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccVW           (0            | kEplObdAccVar | 0            | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccVRW          (0            | kEplObdAccVar | 0            | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccVPR          (0            | kEplObdAccVar | 0            | kEplObdAccPdo | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccVPW          (0            | kEplObdAccVar | 0            | kEplObdAccPdo | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccVPRW         (0            | kEplObdAccVar | 0            | kEplObdAccPdo | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccVGR          (0            | kEplObdAccVar | kEplObdAccRange | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccVGW          (0            | kEplObdAccVar | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccVGRW         (0            | kEplObdAccVar | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccVGPR         (0            | kEplObdAccVar | kEplObdAccRange | kEplObdAccPdo | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccVGPW         (0            | kEplObdAccVar | kEplObdAccRange | kEplObdAccPdo | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccVGPRW        (0            | kEplObdAccVar | kEplObdAccRange | kEplObdAccPdo | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccSR           (kEplObdAccStore | 0          | 0            | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccSW           (kEplObdAccStore | 0          | 0            | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccSRW          (kEplObdAccStore | 0          | 0            | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccSCR          (kEplObdAccStore | 0          | 0            | 0          | kEplObdAccConst | 0            | kEplObdAccRead)
+#define kEplObdAccSGR          (kEplObdAccStore | 0          | kEplObdAccRange | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccSGW          (kEplObdAccStore | 0          | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccSGRW         (kEplObdAccStore | 0          | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccSVR          (kEplObdAccStore | kEplObdAccVar | 0            | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccSVW          (kEplObdAccStore | kEplObdAccVar | 0            | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccSVRW         (kEplObdAccStore | kEplObdAccVar | 0            | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccSVPR         (kEplObdAccStore | kEplObdAccVar | 0            | kEplObdAccPdo | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccSVPW         (kEplObdAccStore | kEplObdAccVar | 0            | kEplObdAccPdo | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccSVPRW        (kEplObdAccStore | kEplObdAccVar | 0            | kEplObdAccPdo | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccSVGR         (kEplObdAccStore | kEplObdAccVar | kEplObdAccRange | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccSVGW         (kEplObdAccStore | kEplObdAccVar | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccSVGRW        (kEplObdAccStore | kEplObdAccVar | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccSVGPR        (kEplObdAccStore | kEplObdAccVar | kEplObdAccRange | kEplObdAccPdo | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccSVGPW        (kEplObdAccStore | kEplObdAccVar | kEplObdAccRange | kEplObdAccPdo | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccSVGPRW       (kEplObdAccStore | kEplObdAccVar | kEplObdAccRange | kEplObdAccPdo | 0            | kEplObdAccWrite | kEplObdAccRead)
+
+typedef unsigned int tEplObdSize;	// For all objects as objects size are used an unsigned int.
+
+// -------------------------------------------------------------------------
+// types for data types defined in DS301
+// -------------------------------------------------------------------------
+
+// types of objects in object dictionary
+// DS-301 defines these types as WORD
+typedef enum {
+// types which are always supported
+	kEplObdTypBool = 0x0001,
+
+	kEplObdTypInt8 = 0x0002,
+	kEplObdTypInt16 = 0x0003,
+	kEplObdTypInt32 = 0x0004,
+	kEplObdTypUInt8 = 0x0005,
+	kEplObdTypUInt16 = 0x0006,
+	kEplObdTypUInt32 = 0x0007,
+	kEplObdTypReal32 = 0x0008,
+	kEplObdTypVString = 0x0009,
+	kEplObdTypOString = 0x000A,
+	kEplObdTypDomain = 0x000F,
+
+	kEplObdTypInt24 = 0x0010,
+	kEplObdTypUInt24 = 0x0016,
+
+	kEplObdTypReal64 = 0x0011,
+	kEplObdTypInt40 = 0x0012,
+	kEplObdTypInt48 = 0x0013,
+	kEplObdTypInt56 = 0x0014,
+	kEplObdTypInt64 = 0x0015,
+	kEplObdTypUInt40 = 0x0018,
+	kEplObdTypUInt48 = 0x0019,
+	kEplObdTypUInt56 = 0x001A,
+	kEplObdTypUInt64 = 0x001B,
+	kEplObdTypTimeOfDay = 0x000C,
+	kEplObdTypTimeDiff = 0x000D
+} tEplObdType;
+// other types are not supported in this version
+
+// -------------------------------------------------------------------------
+// types for data types defined in DS301
+// -------------------------------------------------------------------------
+
+typedef unsigned char tEplObdBoolean;	// 0001
+typedef signed char tEplObdInteger8;	// 0002
+typedef signed short int tEplObdInteger16;	// 0003
+typedef signed long tEplObdInteger32;	// 0004
+typedef unsigned char tEplObdUnsigned8;	// 0005
+typedef unsigned short int tEplObdUnsigned16;	// 0006
+typedef unsigned long tEplObdUnsigned32;	// 0007
+typedef float tEplObdReal32;	// 0008
+typedef unsigned char tEplObdDomain;	// 000F
+typedef signed long tEplObdInteger24;	// 0010
+typedef unsigned long tEplObdUnsigned24;	// 0016
+
+typedef signed QWORD tEplObdInteger40;	// 0012
+typedef signed QWORD tEplObdInteger48;	// 0013
+typedef signed QWORD tEplObdInteger56;	// 0014
+typedef signed QWORD tEplObdInteger64;	// 0015
+
+typedef unsigned QWORD tEplObdUnsigned40;	// 0018
+typedef unsigned QWORD tEplObdUnsigned48;	// 0019
+typedef unsigned QWORD tEplObdUnsigned56;	// 001A
+typedef unsigned QWORD tEplObdUnsigned64;	// 001B
+
+typedef double tEplObdReal64;	// 0011
+
+typedef tTimeOfDay tEplObdTimeOfDay;	// 000C
+typedef tTimeOfDay tEplObdTimeDifference;	// 000D
+
+// -------------------------------------------------------------------------
+// structur for defining a variable
+// -------------------------------------------------------------------------
+// -------------------------------------------------------------------------
+typedef enum {
+	kVarValidSize = 0x01,
+	kVarValidData = 0x02,
+//    kVarValidCallback       = 0x04,
+//    kVarValidArg            = 0x08,
+
+	kVarValidAll = 0x03	// currently only size and data are implemented and used
+} tEplVarParamValid;
+
+typedef tEplKernel(PUBLIC ROM * tEplVarCallback) (CCM_DECL_INSTANCE_HDL_
+						  void *pParam_p);
+
+typedef struct {
+	tEplVarParamValid m_ValidFlag;
+	unsigned int m_uiIndex;
+	unsigned int m_uiSubindex;
+	tEplObdSize m_Size;
+	void MEM *m_pData;
+//    tEplVarCallback     m_fpCallback;
+//    void *       m_pArg;
+
+} tEplVarParam;
+
+typedef struct {
+	void MEM *m_pData;
+	tEplObdSize m_Size;
+/*
+    #if (EPL_PDO_USE_STATIC_MAPPING == FALSE)
+        tEplVarCallback    m_fpCallback;
+        void *   m_pArg;
+    #endif
+*/
+} tEplObdVarEntry;
+
+typedef struct {
+	tEplObdSize m_Size;
+	BYTE *m_pString;
+
+} tEplObdOString;		// 000C
+
+typedef struct {
+	tEplObdSize m_Size;
+	char *m_pString;
+} tEplObdVString;		// 000D
+
+typedef struct {
+	tEplObdSize m_Size;
+	char *m_pDefString;	// $$$ d.k. it is unused, so we could delete it
+	char *m_pString;
+
+} tEplObdVStringDef;
+
+typedef struct {
+	tEplObdSize m_Size;
+	BYTE *m_pDefString;	// $$$ d.k. it is unused, so we could delete it
+	BYTE *m_pString;
+
+} tEplObdOStringDef;
+
+//r.d. parameter struct for changing object size and/or pointer to data of Strings or Domains
+typedef struct {
+	tEplObdSize m_DownloadSize;	// download size from SDO or APP
+	tEplObdSize m_ObjSize;	// current object size from OD - should be changed from callback function
+	void *m_pData;		// current object ptr  from OD - should be changed from callback function
+
+} tEplObdVStringDomain;		// 000D
+
+// ============================================================================
+// types
+// ============================================================================
+// -------------------------------------------------------------------------
+// subindexstruct
+// -------------------------------------------------------------------------
+
+// Change not the order for this struct!!!
+typedef struct {
+	unsigned int m_uiSubIndex;
+	tEplObdType m_Type;
+	tEplObdAccess m_Access;
+	void *m_pDefault;
+	void MEM *m_pCurrent;	// points always to RAM
+
+} tEplObdSubEntry;
+
+// r.d.: has always to be  because new OBD-Macros for arrays
+typedef tEplObdSubEntry *tEplObdSubEntryPtr;
+
+// -------------------------------------------------------------------------
+// callback function for objdictionary modul
+// -------------------------------------------------------------------------
+
+// parameters for callback function
+typedef struct {
+	tEplObdEvent m_ObdEvent;
+	unsigned int m_uiIndex;
+	unsigned int m_uiSubIndex;
+	void *m_pArg;
+	DWORD m_dwAbortCode;
+
+} tEplObdCbParam;
+
+// define type for callback function: pParam_p points to tEplObdCbParam
+typedef tEplKernel(PUBLIC ROM * tEplObdCallback) (CCM_DECL_INSTANCE_HDL_
+						  tEplObdCbParam MEM *
+						  pParam_p);
+
+// do not change the order for this struct!!!
+
+typedef struct {
+	unsigned int m_uiIndex;
+	tEplObdSubEntryPtr m_pSubIndex;
+	unsigned int m_uiCount;
+	tEplObdCallback m_fpCallback;	// function is called back if object access
+
+} tEplObdEntry;
+
+// allways  pointer
+typedef tEplObdEntry *tEplObdEntryPtr;
+
+// -------------------------------------------------------------------------
+// structur to initialize OBD module
+// -------------------------------------------------------------------------
+
+typedef struct {
+	tEplObdEntryPtr m_pPart;
+	tEplObdEntryPtr m_pManufacturerPart;
+	tEplObdEntryPtr m_pDevicePart;
+
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+
+	tEplObdEntryPtr m_pUserPart;
+
+#endif
+
+} tEplObdInitParam;
+
+// -------------------------------------------------------------------------
+// structur for parameters of STORE RESTORE command
+// -------------------------------------------------------------------------
+
+typedef struct {
+	tEplObdCommand m_bCommand;
+	tEplObdPart m_bCurrentOdPart;
+	void MEM *m_pData;
+	tEplObdSize m_ObjSize;
+
+} tEplObdCbStoreParam;
+
+typedef tEplKernel(PUBLIC ROM * tInitTabEntryCallback) (void MEM * pTabEntry_p,
+							unsigned int
+							uiObjIndex_p);
+
+typedef tEplKernel(PUBLIC ROM *
+		   tEplObdStoreLoadObjCallback) (CCM_DECL_INSTANCE_HDL_
+						 tEplObdCbStoreParam MEM *
+						 pCbStoreParam_p);
+
+// -------------------------------------------------------------------------
+// this stucture is used for parameters for function ObdInitModuleTab()
+// -------------------------------------------------------------------------
+typedef struct {
+	unsigned int m_uiLowerObjIndex;	// lower limit of ObjIndex
+	unsigned int m_uiUpperObjIndex;	// upper limit of ObjIndex
+	tInitTabEntryCallback m_fpInitTabEntry;	// will be called if ObjIndex was found
+	void MEM *m_pTabBase;	// base address of table
+	unsigned int m_uiEntrySize;	// size of table entry      // 25-feb-2005 r.d.: expansion from BYTE to WORD necessary for PDO bit mapping
+	unsigned int m_uiMaxEntries;	// max. tabel entries
+
+} tEplObdModulTabParam;
+
+//-------------------------------------------------------------------
+//  enum for function EplObdSetNodeId
+//-------------------------------------------------------------------
+typedef enum {
+	kEplObdNodeIdUnknown = 0x00,	// unknown how the node id was set
+	kEplObdNodeIdSoftware = 0x01,	// node id set by software
+	kEplObdNodeIdHardware = 0x02	// node id set by hardware
+} tEplObdNodeIdType;
+
+// ============================================================================
+// global variables
+// ============================================================================
+
+// ============================================================================
+// public functions
+// ============================================================================
+
+#endif // #ifndef _EPLOBD_H_
diff --git a/drivers/staging/epl/EplObdMacro.h b/drivers/staging/epl/EplObdMacro.h
new file mode 100644
index 000000000000..23f2ad80dde8
--- /dev/null
+++ b/drivers/staging/epl/EplObdMacro.h
@@ -0,0 +1,354 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for macros of EplOBD-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObdMacro.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/05 k.t.:   start of the implementation
+                    -> based on CANopen ObdMacro.h
+
+****************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#if defined (EPL_OBD_DEFINE_MACRO)
+
+    //-------------------------------------------------------------------------------------------
+#if defined (EPL_OBD_CREATE_ROM_DATA)
+
+//        #pragma message ("EPL_OBD_CREATE_ROM_DATA")
+
+#define EPL_OBD_BEGIN()                                                         static  DWORD  dwObd_OBK_g = 0x0000;
+#define EPL_OBD_END()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_PART_GENERIC()
+#define EPL_OBD_BEGIN_PART_MANUFACTURER()
+#define EPL_OBD_BEGIN_PART_DEVICE()
+#define EPL_OBD_END_PART()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_INDEX_RAM(ind,cnt,call)
+#define EPL_OBD_END_INDEX(ind)
+#define EPL_OBD_RAM_INDEX_RAM_ARRAY(ind,cnt,call,typ,acc,dtyp,name,def)         static  tEplObdUnsigned8    xDef##ind##_0x00_g = (cnt); \
+                                                                                        static  dtyp  xDef##ind##_0x01_g = (def);
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY(ind,cnt,call,typ,acc,dtyp,name,def)      static  tEplObdUnsigned8    xDef##ind##_0x00_g = (cnt); \
+                                                                                        static  dtyp  xDef##ind##_0x01_g = (def);
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT(ind,cnt,call,typ,acc,dtyp,name)   static  tEplObdUnsigned8    xDef##ind##_0x00_g = (cnt);
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_SUBINDEX_RAM_VAR(ind,sub,typ,acc,dtyp,name,val)                 static  dtyp  xDef##ind##_##sub##_g        = val;
+#define EPL_OBD_SUBINDEX_RAM_VAR_RG(ind,sub,typ,acc,dtyp,name,val,low,high)     static  dtyp  xDef##ind##_##sub##_g[3]     = {val,low,high};
+#define EPL_OBD_SUBINDEX_RAM_VAR_NOINIT(ind,sub,typ,acc,dtyp,name)
+#define EPL_OBD_SUBINDEX_RAM_VSTRING(ind,sub,acc,name,size,val)                 static char  MEM szCur##ind##_##sub##_g[size+1]; \
+                                                                                        static  tEplObdVStringDef  xDef##ind##_##sub##_g = {size, val, szCur##ind##_##sub##_g};
+
+#define EPL_OBD_SUBINDEX_RAM_OSTRING(ind,sub,acc,name,size)                     static  BYTE  MEM bCur##ind##_##sub##_g[size]; \
+                                                                                        static  tEplObdOStringDef  xDef##ind##_##sub##_g = {size, ((BYTE*)""), bCur##ind##_##sub##_g};
+#define EPL_OBD_SUBINDEX_RAM_DOMAIN(ind,sub,acc,name)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF(ind,sub,typ,acc,dtyp,name,val)             static  dtyp  xDef##ind##_##sub##_g        = val;
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_RG(ind,sub,typ,acc,dtyp,name,val,low,high) static  dtyp  xDef##ind##_##sub##_g[3]     = {val,low,high};
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT(ind,sub,typ,acc,dtyp,name)
+
+//-------------------------------------------------------------------------------------------
+#elif defined (EPL_OBD_CREATE_RAM_DATA)
+
+//        #pragma message ("EPL_OBD_CREATE_RAM_DATA")
+
+#define EPL_OBD_BEGIN()
+#define EPL_OBD_END()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_PART_GENERIC()
+#define EPL_OBD_BEGIN_PART_MANUFACTURER()
+#define EPL_OBD_BEGIN_PART_DEVICE()
+#define EPL_OBD_END_PART()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_INDEX_RAM(ind,cnt,call)
+#define EPL_OBD_END_INDEX(ind)
+#define EPL_OBD_RAM_INDEX_RAM_ARRAY(ind,cnt,call,typ,acc,dtyp,name,def)         static dtyp         MEM axCur##ind##_g[cnt];
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY(ind,cnt,call,typ,acc,dtyp,name,def)      static tEplObdVarEntry MEM aVarEntry##ind##_g[cnt];
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT(ind,cnt,call,typ,acc,dtyp,name)   static tEplObdVarEntry MEM aVarEntry##ind##_g[cnt];
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_SUBINDEX_RAM_VAR(ind,sub,typ,acc,dtyp,name,val)                 static dtyp         MEM xCur##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_VAR_RG(ind,sub,typ,acc,dtyp,name,val,low,high)     static dtyp         MEM xCur##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_VSTRING(ind,sub,acc,name,size,val)                 static tEplObdVString  MEM xCur##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_OSTRING(ind,sub,acc,name,size)                     static tEplObdOString  MEM xCur##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_VAR_NOINIT(ind,sub,typ,acc,dtyp,name)              static dtyp         MEM xCur##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_DOMAIN(ind,sub,acc,name)                           static tEplObdVarEntry MEM VarEntry##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_USERDEF(ind,sub,typ,acc,dtyp,name,val)             static tEplObdVarEntry MEM VarEntry##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_RG(ind,sub,typ,acc,dtyp,name,val,low,high) static tEplObdVarEntry MEM VarEntry##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT(ind,sub,typ,acc,dtyp,name)          static tEplObdVarEntry MEM VarEntry##ind##_##sub##_g;
+
+    //-------------------------------------------------------------------------------------------
+#elif defined (EPL_OBD_CREATE_SUBINDEX_TAB)
+
+//        #pragma message ("EPL_OBD_CREATE_SUBINDEX_TAB")
+
+#define EPL_OBD_BEGIN()
+#define EPL_OBD_END()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_PART_GENERIC()
+#define EPL_OBD_BEGIN_PART_MANUFACTURER()
+#define EPL_OBD_BEGIN_PART_DEVICE()
+#define EPL_OBD_END_PART()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_INDEX_RAM(ind,cnt,call)                                   static tEplObdSubEntry MEM aObdSubEntry##ind##Ram_g[cnt]= {
+#define EPL_OBD_END_INDEX(ind)                                                  EPL_OBD_END_SUBINDEX()};
+#define EPL_OBD_RAM_INDEX_RAM_ARRAY(ind,cnt,call,typ,acc,dtyp,name,def)         static tEplObdSubEntry MEM aObdSubEntry##ind##Ram_g[]= { \
+                                                                                        {0, kEplObdTypUInt8, kEplObdAccCR,          &xDef##ind##_0x00_g,   NULL}, \
+                                                                                        {1, typ,          (acc)|kEplObdAccArray, &xDef##ind##_0x01_g,   &axCur##ind##_g[0]}, \
+                                                                                        EPL_OBD_END_SUBINDEX()};
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY(ind,cnt,call,typ,acc,dtyp,name,def)      static tEplObdSubEntry MEM aObdSubEntry##ind##Ram_g[]= { \
+                                                                                        {0, kEplObdTypUInt8, kEplObdAccCR,                     &xDef##ind##_0x00_g,   NULL}, \
+                                                                                        {1, typ,          (acc)|kEplObdAccArray|kEplObdAccVar, &xDef##ind##_0x01_g,   &aVarEntry##ind##_g[0]}, \
+                                                                                        EPL_OBD_END_SUBINDEX()};
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT(ind,cnt,call,typ,acc,dtyp,name)   static tEplObdSubEntry MEM aObdSubEntry##ind##Ram_g[]= { \
+                                                                                        {0, kEplObdTypUInt8, kEplObdAccCR,                     &xDef##ind##_0x00_g,   NULL}, \
+                                                                                        {1, typ,          (acc)|kEplObdAccArray|kEplObdAccVar, NULL,                  &aVarEntry##ind##_g[0]}, \
+                                                                                        EPL_OBD_END_SUBINDEX()};
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_SUBINDEX_RAM_VAR(ind,sub,typ,acc,dtyp,name,val)                 {sub,typ,            (acc),                        &xDef##ind##_##sub##_g,   &xCur##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_VAR_RG(ind,sub,typ,acc,dtyp,name,val,low,high)     {sub,typ,            (acc)|kEplObdAccRange,           &xDef##ind##_##sub##_g[0],&xCur##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_VAR_NOINIT(ind,sub,typ,acc,dtyp,name)              {sub,typ,            (acc),                        NULL,   &xCur##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_VSTRING(ind,sub,acc,name,size,val)                 {sub,kEplObdTypVString,(acc)/*|kEplObdAccVar*/,         &xDef##ind##_##sub##_g,   &xCur##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_OSTRING(ind,sub,acc,name,size)                     {sub,kEplObdTypOString,(acc)/*|kEplObdAccVar*/,         &xDef##ind##_##sub##_g,   &xCur##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_DOMAIN(ind,sub,acc,name)                           {sub,kEplObdTypDomain, (acc)|kEplObdAccVar,             NULL,                     &VarEntry##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_USERDEF(ind,sub,typ,acc,dtyp,name,val)             {sub,typ,           (acc)|kEplObdAccVar,             &xDef##ind##_##sub##_g,   &VarEntry##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_RG(ind,sub,typ,acc,dtyp,name,val,low,high) {sub,typ,           (acc)|kEplObdAccVar|kEplObdAccRange,&xDef##ind##_##sub##_g[0],&VarEntry##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT(ind,sub,typ,acc,dtyp,name)          {sub,typ,           (acc)|kEplObdAccVar,             NULL,    &VarEntry##ind##_##sub##_g},
+
+    //-------------------------------------------------------------------------------------------
+#elif defined (EPL_OBD_CREATE_INDEX_TAB)
+
+//        #pragma message ("EPL_OBD_CREATE_INDEX_TAB")
+
+#define EPL_OBD_BEGIN()
+#define EPL_OBD_END()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_PART_GENERIC()                                                   static  tEplObdEntry  aObdTab_g[]      = {
+#define EPL_OBD_BEGIN_PART_MANUFACTURER()                                       static  tEplObdEntry  aObdTabManufacturer_g[] = {
+#define EPL_OBD_BEGIN_PART_DEVICE()                                             static  tEplObdEntry  aObdTabDevice_g[]       = {
+#define EPL_OBD_END_PART()                                                      {EPL_OBD_TABLE_INDEX_END,(tEplObdSubEntryPtr)&dwObd_OBK_g,0,NULL}};
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_INDEX_RAM(ind,cnt,call)                                   {ind,(tEplObdSubEntryPtr)&aObdSubEntry##ind##Ram_g[0],cnt,(tEplObdCallback)call},
+#define EPL_OBD_END_INDEX(ind)
+#define EPL_OBD_RAM_INDEX_RAM_ARRAY(ind,cnt,call,typ,acc,dtyp,name,def)         {ind,(tEplObdSubEntryPtr)&aObdSubEntry##ind##Ram_g[0],(cnt)+1,(tEplObdCallback)call},
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY(ind,cnt,call,typ,acc,dtyp,name,def)      {ind,(tEplObdSubEntryPtr)&aObdSubEntry##ind##Ram_g[0],(cnt)+1,(tEplObdCallback)call},
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT(ind,cnt,call,typ,acc,dtyp,name)   {ind,(tEplObdSubEntryPtr)&aObdSubEntry##ind##Ram_g[0],(cnt)+1,(tEplObdCallback)call},
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_SUBINDEX_RAM_VAR(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_VAR_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_VSTRING(ind,sub,acc,name,size,val)
+#define EPL_OBD_SUBINDEX_RAM_OSTRING(ind,sub,acc,name,size)
+#define EPL_OBD_SUBINDEX_RAM_VAR_NOINIT(ind,sub,typ,acc,dtyp,name)
+#define EPL_OBD_SUBINDEX_RAM_DOMAIN(ind,sub,acc,name)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT(ind,sub,typ,acc,dtyp,name)
+
+	    //-------------------------------------------------------------------------------------------
+#elif defined (EPL_OBD_CREATE_INIT_FUNCTION)
+
+//        #pragma message ("EPL_OBD_CREATE_INIT_FUNCTION")
+
+#define EPL_OBD_BEGIN()
+#define EPL_OBD_END()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_PART_GENERIC()                                                   pInitParam->m_pPart      = (tEplObdEntryPtr) &aObdTab_g[0];
+#define EPL_OBD_BEGIN_PART_MANUFACTURER()                                       pInitParam->m_pManufacturerPart = (tEplObdEntryPtr) &aObdTabManufacturer_g[0];
+#define EPL_OBD_BEGIN_PART_DEVICE()                                             pInitParam->m_pDevicePart       = (tEplObdEntryPtr) &aObdTabDevice_g[0];
+#define EPL_OBD_END_PART()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_INDEX_RAM(ind,cnt,call)
+#define EPL_OBD_END_INDEX(ind)
+#define EPL_OBD_RAM_INDEX_RAM_ARRAY(ind,cnt,call,typ,acc,dtyp,name,def)
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY(ind,cnt,call,typ,acc,dtyp,name,def)
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT(ind,cnt,call,typ,acc,dtyp,name)
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_SUBINDEX_RAM_VAR(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_VAR_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_VSTRING(ind,sub,acc,name,size,val)
+#define EPL_OBD_SUBINDEX_RAM_OSTRING(ind,sub,acc,name,size)
+#define EPL_OBD_SUBINDEX_RAM_VAR_NOINIT(ind,sub,typ,acc,dtyp,name)
+#define EPL_OBD_SUBINDEX_RAM_DOMAIN(ind,sub,acc,name)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT(ind,sub,typ,acc,dtyp,name)
+
+    //-------------------------------------------------------------------------------------------
+#elif defined (EPL_OBD_CREATE_INIT_SUBINDEX)
+
+//        #pragma message ("EPL_OBD_CREATE_INIT_SUBINDEX")
+
+#define EPL_OBD_BEGIN()
+#define EPL_OBD_END()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_PART_GENERIC()
+#define EPL_OBD_BEGIN_PART_MANUFACTURER()
+#define EPL_OBD_BEGIN_PART_DEVICE()
+#define EPL_OBD_END_PART()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_INDEX_RAM(ind,cnt,call)	//CCM_SUBINDEX_RAM_ONLY (EPL_MEMCPY (&aObdSubEntry##ind##Ram_g[0],&aObdSubEntry##ind##Rom_g[0],sizeof(aObdSubEntry##ind##Ram_g)));
+#define EPL_OBD_END_INDEX(ind)
+#define EPL_OBD_RAM_INDEX_RAM_ARRAY(ind,cnt,call,typ,acc,dtyp,name,def)	//EPL_MEMCPY (&aObdSubEntry##ind##Ram_g[0],&aObdSubEntry##ind##Rom_g[0],sizeof(aObdSubEntry##ind##Ram_g));
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY(ind,cnt,call,typ,acc,dtyp,name,def)	//EPL_MEMCPY (&aObdSubEntry##ind##Ram_g[0],&aObdSubEntry##ind##Rom_g[0],sizeof(aObdSubEntry##ind##Ram_g));
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT(ind,cnt,call,typ,acc,dtyp,name)	//EPL_MEMCPY (&aObdSubEntry##ind##Ram_g[0],&aObdSubEntry##ind##Rom_g[0],sizeof(aObdSubEntry##ind##Ram_g));
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_SUBINDEX_RAM_VAR(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_VAR_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_VSTRING(ind,sub,acc,name,size,val)
+#define EPL_OBD_SUBINDEX_RAM_OSTRING(ind,sub,acc,name,size)
+#define EPL_OBD_SUBINDEX_RAM_VAR_NOINIT(ind,sub,typ,acc,dtyp,name)
+#define EPL_OBD_SUBINDEX_RAM_DOMAIN(ind,sub,acc,name)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT(ind,sub,typ,acc,dtyp,name)
+
+    //-------------------------------------------------------------------------------------------
+#else
+
+//        #pragma message ("ELSE OF DEFINE")
+
+#define EPL_OBD_BEGIN()
+#define EPL_OBD_END()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_PART_GENERIC()
+#define EPL_OBD_BEGIN_PART_MANUFACTURER()
+#define EPL_OBD_BEGIN_PART_DEVICE()
+#define EPL_OBD_END_PART()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_INDEX_RAM(ind,cnt,call)
+#define EPL_OBD_END_INDEX(ind)
+#define EPL_OBD_RAM_INDEX_RAM_ARRAY(ind,cnt,call,typ,acc,dtyp,name,def)
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY(ind,cnt,call,typ,acc,dtyp,name,def)
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT(ind,cnt,call,typ,acc,dtyp,name)
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_SUBINDEX_RAM_VAR(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_VAR_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_VSTRING(ind,sub,acc,name,sizes,val)
+#define EPL_OBD_SUBINDEX_RAM_OSTRING(ind,sub,acc,name,size)
+#define EPL_OBD_SUBINDEX_RAM_VAR_NOINIT(ind,sub,typ,acc,dtyp,name)
+#define EPL_OBD_SUBINDEX_RAM_DOMAIN(ind,sub,acc,name)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT(ind,sub,typ,acc,dtyp,name)
+
+#endif
+
+    //-------------------------------------------------------------------------------------------
+#elif defined (EPL_OBD_UNDEFINE_MACRO)
+
+//    #pragma message ("EPL_OBD_UNDEFINE_MACRO")
+
+#undef EPL_OBD_BEGIN
+#undef EPL_OBD_END
+
+    //---------------------------------------------------------------------------------------
+#undef EPL_OBD_BEGIN_PART_GENERIC
+#undef EPL_OBD_BEGIN_PART_MANUFACTURER
+#undef EPL_OBD_BEGIN_PART_DEVICE
+#undef EPL_OBD_END_PART
+
+    //---------------------------------------------------------------------------------------
+#undef EPL_OBD_BEGIN_INDEX_RAM
+#undef EPL_OBD_END_INDEX
+#undef EPL_OBD_RAM_INDEX_RAM_ARRAY
+#undef EPL_OBD_RAM_INDEX_RAM_VARARRAY
+#undef EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT
+
+    //---------------------------------------------------------------------------------------
+#undef EPL_OBD_SUBINDEX_RAM_VAR
+#undef EPL_OBD_SUBINDEX_RAM_VAR_RG
+#undef EPL_OBD_SUBINDEX_RAM_VSTRING
+#undef EPL_OBD_SUBINDEX_RAM_OSTRING
+#undef EPL_OBD_SUBINDEX_RAM_VAR_NOINIT
+#undef EPL_OBD_SUBINDEX_RAM_DOMAIN
+#undef EPL_OBD_SUBINDEX_RAM_USERDEF
+#undef EPL_OBD_SUBINDEX_RAM_USERDEF_RG
+#undef EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT
+
+#else
+
+#error "nothing defined"
+
+#endif
diff --git a/drivers/staging/epl/EplObdkCal.c b/drivers/staging/epl/EplObdkCal.c
new file mode 100644
index 000000000000..4c9af89719ed
--- /dev/null
+++ b/drivers/staging/epl/EplObdkCal.c
@@ -0,0 +1,147 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for communication abstraction layer
+                for the Epl-Obd-Kernelspace-Modul
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObdkCal.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/19 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "kernel/EplObdkCal.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+// EOF
diff --git a/drivers/staging/epl/EplObdu.c b/drivers/staging/epl/EplObdu.c
new file mode 100644
index 000000000000..218d152897cf
--- /dev/null
+++ b/drivers/staging/epl/EplObdu.c
@@ -0,0 +1,517 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for Epl-Obd-Userspace-module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObdu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/19 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "user/EplObdu.h"
+#include "user/EplObduCal.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduWriteEntry()
+//
+// Description: Function writes data to an OBD entry. Strings
+//              are stored with added '\0' character.
+//
+// Parameters:  uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduWriteEntry(unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p,
+						 void *pSrcData_p,
+						 tEplObdSize Size_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplObduCalWriteEntry(uiIndex_p, uiSubIndex_p, pSrcData_p, Size_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduReadEntry()
+//
+// Description: The function reads an object entry. The application
+//              can always read the data even if attrib kEplObdAccRead
+//              is not set. The attrib is only checked up for SDO transfer.
+//
+// Parameters:  uiIndex_p       = Index oof the OD entry to read
+//              uiSubIndex_p    = Subindex to read
+//              pDstData_p      = pointer to the buffer for data
+//              Offset_p        = offset in data for read access
+//              pSize_p         = IN: Size of the buffer
+//                                OUT: number of readed Bytes
+//
+// Return:      tEplKernel      =   errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduReadEntry(unsigned int uiIndex_p,
+						unsigned int uiSubIndex_p,
+						void *pDstData_p,
+						tEplObdSize * pSize_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplObduCalReadEntry(uiIndex_p, uiSubIndex_p, pDstData_p, pSize_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdAccessOdPart()
+//
+// Description: restores default values of one part of OD
+//
+// Parameters:  ObdPart_p       = od-part to reset
+//              Direction_p     = directory flag for
+//
+// Return:      tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduAccessOdPart(tEplObdPart ObdPart_p,
+						   tEplObdDir Direction_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplObduCalAccessOdPart(ObdPart_p, Direction_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduDefineVar()
+//
+// Description: defines a variable in OD
+//
+// Parameters:  pEplVarParam_p = varentry
+//
+// Return:      tEplKernel  =   errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduDefineVar(tEplVarParam MEM * pVarParam_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplObduCalDefineVar(pVarParam_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduGetObjectDataPtr()
+//
+// Description: It returnes the current data pointer. But if object is an
+//              constant object it returnes the default pointer.
+//
+// Parameters:  uiIndex_p    =   Index of the entry
+//              uiSubindex_p =   Subindex of the entry
+//
+// Return:      void *    = pointer to object data
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT void *PUBLIC EplObduGetObjectDataPtr(unsigned int uiIndex_p,
+						  unsigned int uiSubIndex_p)
+{
+	void *pData;
+
+	pData = EplObduCalGetObjectDataPtr(uiIndex_p, uiSubIndex_p);
+
+	return pData;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduRegisterUserOd()
+//
+// Description: function registers the user OD
+//
+// Parameters:  pUserOd_p   =pointer to user ODd
+//
+// Return:     tEplKernel = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+EPLDLLEXPORT tEplKernel PUBLIC EplObduRegisterUserOd(tEplObdEntryPtr pUserOd_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplObduCalRegisterUserOd(pUserOd_p);
+
+	return Ret;
+
+}
+#endif
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduInitVarEntry()
+//
+// Description: function to initialize VarEntry dependened on object type
+//
+// Parameters:  pVarEntry_p = pointer to var entry structure
+//              bType_p     = object type
+//              ObdSize_p   = size of object data
+//
+// Returns:     none
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT void PUBLIC EplObduInitVarEntry(tEplObdVarEntry MEM * pVarEntry_p,
+					     BYTE bType_p,
+					     tEplObdSize ObdSize_p)
+{
+	EplObduCalInitVarEntry(pVarEntry_p, bType_p, ObdSize_p);
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduGetDataSize()
+//
+// Description: function to initialize VarEntry dependened on object type
+//
+//              gets the data size of an object
+//              for string objects it returnes the string length
+//
+// Parameters:  uiIndex_p   =   Index
+//              uiSubIndex_p=   Subindex
+//
+// Return:      tEplObdSize
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplObdSize PUBLIC EplObduGetDataSize(unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p)
+{
+	tEplObdSize Size;
+
+	Size = EplObduCalGetDataSize(uiIndex_p, uiSubIndex_p);
+
+	return Size;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduGetNodeId()
+//
+// Description: function returns nodeid from entry 0x1F93
+//
+//
+// Parameters:
+//
+// Return:      unsigned int = Node Id
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT unsigned int PUBLIC EplObduGetNodeId()
+{
+	unsigned int uiNodeId;
+
+	uiNodeId = EplObduCalGetNodeId();
+
+	return uiNodeId;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduSetNodeId()
+//
+// Description: function sets nodeid in entry 0x1F93
+//
+//
+// Parameters:  uiNodeId_p  =   Node Id to set
+//              NodeIdType_p=   Type on which way the Node Id was set
+//
+// Return:      tEplKernel = Errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduSetNodeId(unsigned int uiNodeId_p,
+						tEplObdNodeIdType NodeIdType_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplObduCalSetNodeId(uiNodeId_p, NodeIdType_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduGetAccessType()
+//
+// Description: Function returns accesstype of the entry
+//
+// Parameters:  uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pAccessTyp_p    =   pointer to buffer to store accesstyp
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduGetAccessType(unsigned int uiIndex_p,
+						    unsigned int uiSubIndex_p,
+						    tEplObdAccess *
+						    pAccessTyp_p)
+{
+	tEplObdAccess AccessType;
+
+	AccessType =
+	    EplObduCalGetAccessType(uiIndex_p, uiSubIndex_p, pAccessTyp_p);
+
+	return AccessType;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdReaduEntryToLe()
+//
+// Description: The function reads an object entry from the byteoder
+//              of the system to the little endian byteorder for numeric values.
+//              For other types a normal read will be processed. This is usefull for
+//              the PDO and SDO module. The application
+//              can always read the data even if attrib kEplObdAccRead
+//              is not set. The attrib is only checked up for SDO transfer.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       = Index of the OD entry to read
+//              uiSubIndex_p    = Subindex to read
+//              pDstData_p      = pointer to the buffer for data
+//              Offset_p        = offset in data for read access
+//              pSize_p         = IN: Size of the buffer
+//                                OUT: number of readed Bytes
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduReadEntryToLe(unsigned int uiIndex_p,
+						    unsigned int uiSubIndex_p,
+						    void *pDstData_p,
+						    tEplObdSize * pSize_p)
+{
+	tEplKernel Ret;
+
+	Ret =
+	    EplObduCalReadEntryToLe(uiIndex_p, uiSubIndex_p, pDstData_p,
+				    pSize_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduWriteEntryFromLe()
+//
+// Description: Function writes data to an OBD entry from a source with
+//              little endian byteorder to the od with system specuific
+//              byteorder. Not numeric values will only by copied. Strings
+//              are stored with added '\0' character.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduWriteEntryFromLe(unsigned int uiIndex_p,
+						       unsigned int
+						       uiSubIndex_p,
+						       void *pSrcData_p,
+						       tEplObdSize Size_p)
+{
+	tEplKernel Ret;
+
+	Ret =
+	    EplObduCalWriteEntryFromLe(uiIndex_p, uiSubIndex_p, pSrcData_p,
+				       Size_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduSearchVarEntry()
+//
+// Description: gets variable from OD
+//
+// Parameters:  uiIndex_p       =   index of the var entry to search
+//              uiSubindex_p    =   subindex of var entry to search
+//              ppVarEntry_p    =   pointer to the pointer to the varentry
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduSearchVarEntry(EPL_MCO_DECL_INSTANCE_PTR_
+						     unsigned int uiIndex_p,
+						     unsigned int uiSubindex_p,
+						     tEplObdVarEntry MEM **
+						     ppVarEntry_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplObduCalSearchVarEntry(uiIndex_p, uiSubindex_p, ppVarEntry_p);
+
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplObduCal.c b/drivers/staging/epl/EplObduCal.c
new file mode 100644
index 000000000000..85b3df0886b1
--- /dev/null
+++ b/drivers/staging/epl/EplObduCal.c
@@ -0,0 +1,558 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for communication abstraction layer
+                for the Epl-Obd-Userspace-Modul
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObduCal.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/19 k.t.:   start of the implementation
+
+****************************************************************************/
+#include "EplInc.h"
+#include "user/EplObduCal.h"
+#include "kernel/EplObdk.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0) && (EPL_OBD_USE_KERNEL != FALSE)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalWriteEntry()
+//
+// Description: Function encapsulate access of function EplObdWriteEntry
+//
+// Parameters:  uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalWriteEntry(unsigned int uiIndex_p,
+						    unsigned int uiSubIndex_p,
+						    void *pSrcData_p,
+						    tEplObdSize Size_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdWriteEntry(uiIndex_p, uiSubIndex_p, pSrcData_p, Size_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalReadEntry()
+//
+// Description: Function encapsulate access of function EplObdReadEntry
+//
+// Parameters:  uiIndex_p       = Index oof the OD entry to read
+//              uiSubIndex_p    = Subindex to read
+//              pDstData_p      = pointer to the buffer for data
+//              Offset_p        = offset in data for read access
+//              pSize_p         = IN: Size of the buffer
+//                                OUT: number of readed Bytes
+//
+// Return:      tEplKernel      =   errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalReadEntry(unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p,
+						   void *pDstData_p,
+						   tEplObdSize * pSize_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdReadEntry(uiIndex_p, uiSubIndex_p, pDstData_p, pSize_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalAccessOdPart()
+//
+// Description: Function encapsulate access of function EplObdAccessOdPart
+//
+// Parameters:  ObdPart_p       = od-part to reset
+//              Direction_p     = directory flag for
+//
+// Return:      tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalAccessOdPart(tEplObdPart ObdPart_p,
+						      tEplObdDir Direction_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdAccessOdPart(ObdPart_p, Direction_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalDefineVar()
+//
+// Description: Function encapsulate access of function EplObdDefineVar
+//
+// Parameters:  pEplVarParam_p = varentry
+//
+// Return:      tEplKernel  =   errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalDefineVar(tEplVarParam MEM *
+						   pVarParam_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdDefineVar(pVarParam_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalGetObjectDataPtr()
+//
+// Description: Function encapsulate access of function EplObdGetObjectDataPtr
+//
+// Parameters:  uiIndex_p    =   Index of the entry
+//              uiSubindex_p =   Subindex of the entry
+//
+// Return:      void *    = pointer to object data
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT void *PUBLIC EplObduCalGetObjectDataPtr(unsigned int uiIndex_p,
+						     unsigned int uiSubIndex_p)
+{
+	void *pData;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	pData = EplObdGetObjectDataPtr(uiIndex_p, uiSubIndex_p);
+#else
+	pData = NULL;
+#endif
+
+	return pData;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalRegisterUserOd()
+//
+// Description: Function encapsulate access of function EplObdRegisterUserOd
+//
+// Parameters:  pUserOd_p   = pointer to user OD
+//
+// Return:     tEplKernel = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalRegisterUserOd(tEplObdEntryPtr
+							pUserOd_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdRegisterUserOd(pUserOd_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+
+}
+#endif
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalInitVarEntry()
+//
+// Description: Function encapsulate access of function EplObdInitVarEntry
+//
+// Parameters:  pVarEntry_p = pointer to var entry structure
+//              bType_p     = object type
+//              ObdSize_p   = size of object data
+//
+// Returns:     none
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT void PUBLIC EplObduCalInitVarEntry(tEplObdVarEntry MEM *
+						pVarEntry_p, BYTE bType_p,
+						tEplObdSize ObdSize_p)
+{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	EplObdInitVarEntry(pVarEntry_p, bType_p, ObdSize_p);
+#endif
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalGetDataSize()
+//
+// Description: Function encapsulate access of function EplObdGetDataSize
+//
+//              gets the data size of an object
+//              for string objects it returnes the string length
+//
+// Parameters:  uiIndex_p   =   Index
+//              uiSubIndex_p=   Subindex
+//
+// Return:      tEplObdSize
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplObdSize PUBLIC EplObduCalGetDataSize(unsigned int uiIndex_p,
+						      unsigned int uiSubIndex_p)
+{
+	tEplObdSize Size;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Size = EplObdGetDataSize(uiIndex_p, uiSubIndex_p);
+#else
+	Size = 0;
+#endif
+
+	return Size;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalGetNodeId()
+//
+// Description: Function encapsulate access of function EplObdGetNodeId
+//
+//
+// Parameters:
+//
+// Return:      unsigned int = Node Id
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT unsigned int PUBLIC EplObduCalGetNodeId()
+{
+	unsigned int uiNodeId;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	uiNodeId = EplObdGetNodeId();
+#else
+	uiNodeId = 0;
+#endif
+
+	return uiNodeId;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalSetNodeId()
+//
+// Description: Function encapsulate access of function EplObdSetNodeId
+//
+//
+// Parameters:  uiNodeId_p  =   Node Id to set
+//              NodeIdType_p=   Type on which way the Node Id was set
+//
+// Return:      tEplKernel = Errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalSetNodeId(unsigned int uiNodeId_p,
+						   tEplObdNodeIdType
+						   NodeIdType_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdSetNodeId(uiNodeId_p, NodeIdType_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalGetAccessType()
+//
+// Description: Function encapsulate access of function EplObdGetAccessType
+//
+// Parameters:  uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pAccessTyp_p    =   pointer to buffer to store accesstype
+//
+// Return:      tEplKernel      =   errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalGetAccessType(unsigned int uiIndex_p,
+						       unsigned int
+						       uiSubIndex_p,
+						       tEplObdAccess *
+						       pAccessTyp_p)
+{
+	tEplObdAccess AccesType;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	AccesType = EplObdGetAccessType(uiIndex_p, uiSubIndex_p, pAccessTyp_p);
+#else
+	AccesType = 0;
+#endif
+
+	return AccesType;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalReadEntryToLe()
+//
+// Description: Function encapsulate access of function EplObdReadEntryToLe
+//
+// Parameters:  uiIndex_p       = Index of the OD entry to read
+//              uiSubIndex_p    = Subindex to read
+//              pDstData_p      = pointer to the buffer for data
+//              Offset_p        = offset in data for read access
+//              pSize_p         = IN: Size of the buffer
+//                                OUT: number of readed Bytes
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalReadEntryToLe(unsigned int uiIndex_p,
+						       unsigned int
+						       uiSubIndex_p,
+						       void *pDstData_p,
+						       tEplObdSize * pSize_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdReadEntryToLe(uiIndex_p, uiSubIndex_p, pDstData_p, pSize_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalWriteEntryFromLe()
+//
+// Description: Function encapsulate access of function EplObdWriteEntryFromLe
+//
+// Parameters:  uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalWriteEntryFromLe(unsigned int
+							  uiIndex_p,
+							  unsigned int
+							  uiSubIndex_p,
+							  void *pSrcData_p,
+							  tEplObdSize Size_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret =
+	    EplObdWriteEntryFromLe(uiIndex_p, uiSubIndex_p, pSrcData_p, Size_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalSearchVarEntry()
+//
+// Description: gets variable from OD
+//
+// Parameters:  uiIndex_p       =   index of the var entry to search
+//              uiSubindex_p    =   subindex of var entry to search
+//              ppVarEntry_p    =   pointer to the pointer to the varentry
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC
+EplObduCalSearchVarEntry(EPL_MCO_DECL_INSTANCE_PTR_ unsigned int uiIndex_p,
+			 unsigned int uiSubindex_p,
+			 tEplObdVarEntry MEM ** ppVarEntry_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdSearchVarEntry(uiIndex_p, uiSubindex_p, ppVarEntry_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplObjDef.h b/drivers/staging/epl/EplObjDef.h
new file mode 100644
index 000000000000..7713125a3e8d
--- /dev/null
+++ b/drivers/staging/epl/EplObjDef.h
@@ -0,0 +1,208 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  defines objdict dictionary
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObjDef.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/06    k.t.:   take ObjDef.h from CANopen and modify for EPL
+
+****************************************************************************/
+
+#ifndef _EPLOBJDEF_H_
+#define _EPLOBJDEF_H_
+
+//---------------------------------------------------------------------------
+// security checks
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// macros to help building OD
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+#if (defined (EPL_OBD_USE_VARIABLE_SUBINDEX_TAB) && (EPL_OBD_USE_VARIABLE_SUBINDEX_TAB != FALSE))
+
+#define CCM_SUBINDEX_RAM_ONLY(a)    a;
+#define CCM_SUBINDEX_RAM_ONEOF(a,b) a
+
+#else
+
+#define CCM_SUBINDEX_RAM_ONLY(a)
+#define CCM_SUBINDEX_RAM_ONEOF(a,b) b
+
+#endif
+
+//---------------------------------------------------------------------------
+// To prevent unused memory in subindex tables we need this macro.
+// But not all compilers support to preset the last struct value followed by a comma.
+// Compilers which does not support a comma after last struct value has to place in a dummy subindex.
+#if ((DEV_SYSTEM & _DEV_COMMA_EXT_) != 0)
+
+#define EPL_OBD_END_SUBINDEX()
+#define EPL_OBD_MAX_ARRAY_SUBENTRIES    2
+
+#else
+
+#define EPL_OBD_END_SUBINDEX()          {0,0,0,NULL,NULL}
+#define EPL_OBD_MAX_ARRAY_SUBENTRIES    3
+
+#endif
+
+//---------------------------------------------------------------------------
+//---------------------------------------------------------------------------
+// globale vars
+//---------------------------------------------------------------------------
+//---------------------------------------------------------------------------
+
+// -------------------------------------------------------------------------
+// creation of data in ROM memory
+// -------------------------------------------------------------------------
+#define EPL_OBD_CREATE_ROM_DATA
+#include "objdict.h"
+#undef EPL_OBD_CREATE_ROM_DATA
+
+// -------------------------------------------------------------------------
+// creation of data in RAM memory
+// -------------------------------------------------------------------------
+
+#define EPL_OBD_CREATE_RAM_DATA
+#include "objdict.h"
+#undef EPL_OBD_CREATE_RAM_DATA
+
+// -------------------------------------------------------------------------
+// creation of subindex tables in ROM and RAM
+// -------------------------------------------------------------------------
+
+#define EPL_OBD_CREATE_SUBINDEX_TAB
+#include "objdict.h"
+#undef EPL_OBD_CREATE_SUBINDEX_TAB
+
+// -------------------------------------------------------------------------
+// creation of index tables for generic, manufacturer and device part
+// -------------------------------------------------------------------------
+
+#define EPL_OBD_CREATE_INDEX_TAB
+#include "objdict.h"
+#undef EPL_OBD_CREATE_INDEX_TAB
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+// ----------------------------------------------------------------------------
+//
+// Function:    EPL_OBD_INIT_RAM_NAME()
+//
+// Description: function to initialize object dictionary
+//
+// Parameters:  pInitParam_p    = pointer to init param struct of Epl
+//
+// Returns:     tEplKernel      = error code
+//
+// State:
+//
+// ----------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EPL_OBD_INIT_RAM_NAME(tEplObdInitParam MEM *
+						     pInitParam_p)
+{
+
+	tEplObdInitParam MEM *pInitParam = pInitParam_p;
+
+	// check if pointer to parameter structure is valid
+	// if not then only copy subindex tables below
+	if (pInitParam != NULL) {
+		// at first delete all parameters (all pointers will be set zu NULL)
+		EPL_MEMSET(pInitParam, 0, sizeof(tEplObdInitParam));
+
+#define EPL_OBD_CREATE_INIT_FUNCTION
+		{
+			// inserts code to init pointer to index tables
+#include "objdict.h"
+		}
+#undef EPL_OBD_CREATE_INIT_FUNCTION
+
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+		{
+			// to begin no user OD is defined
+			pInitParam_p->m_pUserPart = NULL;
+		}
+#endif
+	}
+#define EPL_OBD_CREATE_INIT_SUBINDEX
+	{
+		// inserts code to copy subindex tables
+#include "objdict.h"
+	}
+#undef EPL_OBD_CREATE_INIT_SUBINDEX
+
+	return kEplSuccessful;
+
+}
+
+#endif // _EPLOBJDEF_H_
+
+// Die letzte Zeile muß unbedingt eine leere Zeile sein, weil manche Compiler
+// damit ein Problem haben, wenn das nicht so ist (z.B. GNU oder Borland C++ Builder).
diff --git a/drivers/staging/epl/EplPdo.h b/drivers/staging/epl/EplPdo.h
new file mode 100644
index 000000000000..d22ac86e85b6
--- /dev/null
+++ b/drivers/staging/epl/EplPdo.h
@@ -0,0 +1,117 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for PDO module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplPdo.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_PDO_H_
+#define _EPL_PDO_H_
+
+#include "EplInc.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// invalid PDO-NodeId
+#define EPL_PDO_INVALID_NODE_ID     0xFF
+// NodeId for PReq RPDO
+#define EPL_PDO_PREQ_NODE_ID        0x00
+// NodeId for PRes TPDO
+#define EPL_PDO_PRES_NODE_ID        0x00
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef struct {
+	void *m_pVar;
+	WORD m_wOffset;		// in Bits
+	WORD m_wSize;		// in Bits
+	BOOL m_fNumeric;	// numeric value -> use AMI functions
+
+} tEplPdoMapping;
+
+typedef struct {
+	unsigned int m_uiSizeOfStruct;
+	unsigned int m_uiPdoId;
+	unsigned int m_uiNodeId;
+	// 0xFF=invalid, RPDO: 0x00=PReq, localNodeId=PRes, remoteNodeId=PRes
+	//               TPDO: 0x00=PRes, MN: CnNodeId=PReq
+
+	BOOL m_fTxRx;
+	BYTE m_bMappingVersion;
+	unsigned int m_uiMaxMappingEntries;	// maximum number of mapping entries, i.e. size of m_aPdoMapping
+	tEplPdoMapping m_aPdoMapping[1];
+
+} tEplPdoParam;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_PDO_H_
diff --git a/drivers/staging/epl/EplPdok.c b/drivers/staging/epl/EplPdok.c
new file mode 100644
index 000000000000..15999b4f5750
--- /dev/null
+++ b/drivers/staging/epl/EplPdok.c
@@ -0,0 +1,694 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for kernel PDO module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplPdok.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "kernel/EplPdok.h"
+#include "kernel/EplPdokCal.h"
+#include "kernel/EplEventk.h"
+#include "kernel/EplObdk.h"
+
+#if (DEV_SYSTEM == _DEV_GNU_CF548X_)
+#include "plccore.h"
+#define PDO_LED 0x08
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) == 0)
+
+#error 'ERROR: Missing DLLk-Modul!'
+
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) == 0)
+
+#error 'ERROR: Missing OBDk-Modul!'
+
+#endif
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define EPL_PDOK_OBD_IDX_RX_COMM_PARAM  0x1400
+#define EPL_PDOK_OBD_IDX_RX_MAPP_PARAM  0x1600
+#define EPL_PDOK_OBD_IDX_TX_COMM_PARAM  0x1800
+#define EPL_PDOK_OBD_IDX_TX_MAPP_PARAM  0x1A00
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplPdok                                             */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokAddInstance()
+//
+// Description: add and initialize new instance of EPL stack
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokAddInstance(void)
+{
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokDelInstance()
+//
+// Description: deletes an instance of EPL stack
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokDelInstance(void)
+{
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokCbPdoReceived
+//
+// Description: This function is called by DLL if PRes or PReq frame was
+//              received. It posts the frame to the event queue.
+//              It is called in states NMT_CS_READY_TO_OPERATE and NMT_CS_OPERATIONAL.
+//              The passed PDO needs not to be valid.
+//
+// Parameters:  pFrameInfo_p            = pointer to frame info structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCbPdoReceived(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+
+#if (DEV_SYSTEM == _DEV_GNU_CF548X_)
+	// reset LED
+//    MCF_GPIO_PODR_PCIBG &= ~PDO_LED;  // Level
+#endif
+
+	Event.m_EventSink = kEplEventSinkPdok;
+	Event.m_EventType = kEplEventTypePdoRx;
+	// limit copied data to size of PDO (because from some CNs the frame is larger than necessary)
+	Event.m_uiSize = AmiGetWordFromLe(&pFrameInfo_p->m_pFrame->m_Data.m_Pres.m_le_wSize) + 24;	// pFrameInfo_p->m_uiFrameSize;
+	Event.m_pArg = pFrameInfo_p->m_pFrame;
+	Ret = EplEventkPost(&Event);
+
+#if (DEV_SYSTEM == _DEV_GNU_CF548X_)
+	// set LED
+//    MCF_GPIO_PODR_PCIBG |= PDO_LED;  // Level
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokCbPdoTransmitted
+//
+// Description: This function is called by DLL if PRes or PReq frame was
+//              sent. It posts the pointer to the frame to the event queue.
+//              It is called in NMT_CS_PRE_OPERATIONAL_2,
+//              NMT_CS_READY_TO_OPERATE and NMT_CS_OPERATIONAL.
+//
+// Parameters:  pFrameInfo_p            = pointer to frame info structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCbPdoTransmitted(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+
+#if (DEV_SYSTEM == _DEV_GNU_CF548X_)
+	// reset LED
+	MCF_GPIO_PODR_PCIBG &= ~PDO_LED;	// Level
+#endif
+
+	Event.m_EventSink = kEplEventSinkPdok;
+	Event.m_EventType = kEplEventTypePdoTx;
+	Event.m_uiSize = sizeof(tEplFrameInfo);
+	Event.m_pArg = pFrameInfo_p;
+	Ret = EplEventkPost(&Event);
+
+#if (DEV_SYSTEM == _DEV_GNU_CF548X_)
+	// set LED
+	MCF_GPIO_PODR_PCIBG |= PDO_LED;	// Level
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokCbSoa
+//
+// Description: This function is called by DLL if SoA frame was
+//              received resp. sent. It posts this event to the event queue.
+//
+// Parameters:  pFrameInfo_p            = pointer to frame info structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCbSoa(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+
+	Event.m_EventSink = kEplEventSinkPdok;
+	Event.m_EventType = kEplEventTypePdoSoa;
+	Event.m_uiSize = 0;
+	Event.m_pArg = NULL;
+	Ret = EplEventkPost(&Event);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokProcess
+//
+// Description: This function processes all received and transmitted PDOs.
+//              This function must not be interrupted by any other task
+//              except ISRs (like the ethernet driver ISR, which may call
+//              EplPdokCbFrameReceived() or EplPdokCbFrameTransmitted()).
+//
+// Parameters:  pEvent_p                = pointer to event structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokProcess(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	WORD wPdoSize;
+	WORD wBitOffset;
+	WORD wBitSize;
+	WORD wVarSize;
+	QWORD qwObjectMapping;
+	BYTE bMappSubindex;
+	BYTE bObdSubindex;
+	WORD wObdMappIndex;
+	WORD wObdCommIndex;
+	WORD wPdoId;
+	BYTE bObdData;
+	BYTE bObjectCount;
+	BYTE bFrameData;
+	BOOL fValid;
+	tEplObdSize ObdSize;
+	tEplFrame *pFrame;
+	tEplFrameInfo *pFrameInfo;
+	unsigned int uiNodeId;
+	tEplMsgType MsgType;
+
+	// 0xFF=invalid, RPDO: 0x00=PReq, localNodeId=PRes, remoteNodeId=PRes
+	//               TPDO: 0x00=PRes, MN: CnNodeId=PReq
+
+	switch (pEvent_p->m_EventType) {
+	case kEplEventTypePdoRx:	// RPDO received
+		pFrame = (tEplFrame *) pEvent_p->m_pArg;
+
+		// check if received RPDO is valid
+		bFrameData =
+		    AmiGetByteFromLe(&pFrame->m_Data.m_Pres.m_le_bFlag1);
+		if ((bFrameData & EPL_FRAME_FLAG1_RD) == 0) {	// RPDO invalid
+			goto Exit;
+		}
+		// retrieve EPL message type
+		MsgType = AmiGetByteFromLe(&pFrame->m_le_bMessageType);
+		if (MsgType == kEplMsgTypePreq) {	// RPDO is PReq frame
+			uiNodeId = EPL_PDO_PREQ_NODE_ID;	// 0x00
+		} else {	// RPDO is PRes frame
+			// retrieve node ID
+			uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bSrcNodeId);
+		}
+
+		// search for appropriate valid RPDO in OD
+		wObdMappIndex = EPL_PDOK_OBD_IDX_RX_MAPP_PARAM;
+		for (wObdCommIndex = EPL_PDOK_OBD_IDX_RX_COMM_PARAM;
+		     wObdCommIndex < (EPL_PDOK_OBD_IDX_RX_COMM_PARAM + 0x00FF);
+		     wObdCommIndex++, wObdMappIndex++) {
+			ObdSize = 1;
+			// read node ID from OD
+			Ret =
+			    EplObdReadEntry(wObdCommIndex, 0x01, &bObdData,
+					    &ObdSize);
+			if ((Ret == kEplObdIndexNotExist)
+			    || (Ret == kEplObdSubindexNotExist)
+			    || (Ret == kEplObdIllegalPart)) {	// PDO does not exist; last PDO reached
+				Ret = kEplSuccessful;
+				goto Exit;
+			} else if (Ret != kEplSuccessful) {	// other fatal error occured
+				goto Exit;
+			}
+			// entry read successfully
+			if (bObdData != uiNodeId) {	// node ID does not equal - wrong PDO, try next PDO in OD
+				continue;
+			}
+			ObdSize = 1;
+			// read number of mapped objects from OD; this indicates if the PDO is valid
+			Ret =
+			    EplObdReadEntry(wObdMappIndex, 0x00, &bObjectCount,
+					    &ObdSize);
+			if ((Ret == kEplObdIndexNotExist)
+			    || (Ret == kEplObdSubindexNotExist)
+			    || (Ret == kEplObdIllegalPart)) {	// PDO does not exist; last PDO reached
+				Ret = kEplSuccessful;
+				goto Exit;
+			} else if (Ret != kEplSuccessful) {	// other fatal error occured
+				goto Exit;
+			}
+			// entry read successfully
+			if (bObjectCount == 0) {	// PDO in OD not valid, try next PDO in OD
+				continue;
+			}
+
+			ObdSize = 1;
+			// check PDO mapping version
+			Ret =
+			    EplObdReadEntry(wObdCommIndex, 0x02, &bObdData,
+					    &ObdSize);
+			if (Ret != kEplSuccessful) {	// other fatal error occured
+				goto Exit;
+			}
+			// entry read successfully
+			// retrieve PDO version from frame
+			bFrameData =
+			    AmiGetByteFromLe(&pFrame->m_Data.m_Pres.
+					     m_le_bPdoVersion);
+			if ((bObdData & EPL_VERSION_MAIN) != (bFrameData & EPL_VERSION_MAIN)) {	// PDO versions do not match
+				// $$$ raise PDO error
+				// termiate processing of this RPDO
+				goto Exit;
+			}
+			// valid RPDO found
+
+			// retrieve PDO size
+			wPdoSize =
+			    AmiGetWordFromLe(&pFrame->m_Data.m_Pres.m_le_wSize);
+
+			// process mapping
+			for (bMappSubindex = 1; bMappSubindex <= bObjectCount;
+			     bMappSubindex++) {
+				ObdSize = 8;	// QWORD
+				// read object mapping from OD
+				Ret =
+				    EplObdReadEntry(wObdMappIndex,
+						    bMappSubindex,
+						    &qwObjectMapping, &ObdSize);
+				if (Ret != kEplSuccessful) {	// other fatal error occured
+					goto Exit;
+				}
+				// check if object mapping entry is valid, i.e. unequal zero, because "empty" entries are allowed
+				if (qwObjectMapping == 0) {	// invalid entry, continue with next entry
+					continue;
+				}
+				// decode object mapping
+				wObdCommIndex =
+				    (WORD) (qwObjectMapping &
+					    0x000000000000FFFFLL);
+				bObdSubindex =
+				    (BYTE) ((qwObjectMapping &
+					     0x0000000000FF0000LL) >> 16);
+				wBitOffset =
+				    (WORD) ((qwObjectMapping &
+					     0x0000FFFF00000000LL) >> 32);
+				wBitSize =
+				    (WORD) ((qwObjectMapping &
+					     0xFFFF000000000000LL) >> 48);
+
+				// check if object exceeds PDO size
+				if (((wBitOffset + wBitSize) >> 3) > wPdoSize) {	// wrong object mapping; PDO size is too low
+					// $$$ raise PDO error
+					// terminate processing of this RPDO
+					goto Exit;
+				}
+				// copy object from RPDO to process/OD variable
+				ObdSize = wBitSize >> 3;
+				Ret =
+				    EplObdWriteEntryFromLe(wObdCommIndex,
+							   bObdSubindex,
+							   &pFrame->m_Data.
+							   m_Pres.
+							   m_le_abPayload[(wBitOffset >> 3)], ObdSize);
+				if (Ret != kEplSuccessful) {	// other fatal error occured
+					goto Exit;
+				}
+
+			}
+
+			// processing finished successfully
+			goto Exit;
+		}
+		break;
+
+	case kEplEventTypePdoTx:	// TPDO transmitted
+		pFrameInfo = (tEplFrameInfo *) pEvent_p->m_pArg;
+		pFrame = pFrameInfo->m_pFrame;
+
+		// set TPDO invalid, so that only fully processed TPDOs are sent as valid
+		bFrameData =
+		    AmiGetByteFromLe(&pFrame->m_Data.m_Pres.m_le_bFlag1);
+		AmiSetByteToLe(&pFrame->m_Data.m_Pres.m_le_bFlag1,
+			       (bFrameData & ~EPL_FRAME_FLAG1_RD));
+
+		// retrieve EPL message type
+		MsgType = AmiGetByteFromLe(&pFrame->m_le_bMessageType);
+		if (MsgType == kEplMsgTypePres) {	// TPDO is PRes frame
+			uiNodeId = EPL_PDO_PRES_NODE_ID;	// 0x00
+		} else {	// TPDO is PReq frame
+			// retrieve node ID
+			uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bDstNodeId);
+		}
+
+		// search for appropriate valid TPDO in OD
+		wObdMappIndex = EPL_PDOK_OBD_IDX_TX_MAPP_PARAM;
+		wObdCommIndex = EPL_PDOK_OBD_IDX_TX_COMM_PARAM;
+		for (wPdoId = 0;; wPdoId++, wObdCommIndex++, wObdMappIndex++) {
+			ObdSize = 1;
+			// read node ID from OD
+			Ret =
+			    EplObdReadEntry(wObdCommIndex, 0x01, &bObdData,
+					    &ObdSize);
+			if ((Ret == kEplObdIndexNotExist)
+			    || (Ret == kEplObdSubindexNotExist)
+			    || (Ret == kEplObdIllegalPart)) {	// PDO does not exist; last PDO reached
+				Ret = kEplSuccessful;
+				goto Exit;
+			} else if (Ret != kEplSuccessful) {	// other fatal error occured
+				goto Exit;
+			}
+			// entry read successfully
+			if (bObdData != uiNodeId) {	// node ID does not equal - wrong PDO, try next PDO in OD
+				continue;
+			}
+			ObdSize = 1;
+			// read number of mapped objects from OD; this indicates if the PDO is valid
+			Ret =
+			    EplObdReadEntry(wObdMappIndex, 0x00, &bObjectCount,
+					    &ObdSize);
+			if ((Ret == kEplObdIndexNotExist)
+			    || (Ret == kEplObdSubindexNotExist)
+			    || (Ret == kEplObdIllegalPart)) {	// PDO does not exist; last PDO reached
+				Ret = kEplSuccessful;
+				goto Exit;
+			} else if (Ret != kEplSuccessful) {	// other fatal error occured
+				goto Exit;
+			}
+			// entry read successfully
+			if (bObjectCount == 0) {	// PDO in OD not valid, try next PDO in OD
+				continue;
+			}
+			// valid TPDO found
+
+			ObdSize = 1;
+			// get PDO mapping version from OD
+			Ret =
+			    EplObdReadEntry(wObdCommIndex, 0x02, &bObdData,
+					    &ObdSize);
+			if (Ret != kEplSuccessful) {	// other fatal error occured
+				goto Exit;
+			}
+			// entry read successfully
+			// set PDO version in frame
+			AmiSetByteToLe(&pFrame->m_Data.m_Pres.m_le_bPdoVersion,
+				       bObdData);
+
+			// calculate PDO size
+			wPdoSize = 0;
+
+			// process mapping
+			for (bMappSubindex = 1; bMappSubindex <= bObjectCount;
+			     bMappSubindex++) {
+				ObdSize = 8;	// QWORD
+				// read object mapping from OD
+				Ret =
+				    EplObdReadEntry(wObdMappIndex,
+						    bMappSubindex,
+						    &qwObjectMapping, &ObdSize);
+				if (Ret != kEplSuccessful) {	// other fatal error occured
+					goto Exit;
+				}
+				// check if object mapping entry is valid, i.e. unequal zero, because "empty" entries are allowed
+				if (qwObjectMapping == 0) {	// invalid entry, continue with next entry
+					continue;
+				}
+				// decode object mapping
+				wObdCommIndex =
+				    (WORD) (qwObjectMapping &
+					    0x000000000000FFFFLL);
+				bObdSubindex =
+				    (BYTE) ((qwObjectMapping &
+					     0x0000000000FF0000LL) >> 16);
+				wBitOffset =
+				    (WORD) ((qwObjectMapping &
+					     0x0000FFFF00000000LL) >> 32);
+				wBitSize =
+				    (WORD) ((qwObjectMapping &
+					     0xFFFF000000000000LL) >> 48);
+
+				// calculate max PDO size
+				ObdSize = wBitSize >> 3;
+				wVarSize = (wBitOffset >> 3) + (WORD) ObdSize;
+				if ((unsigned int)(wVarSize + 24) > pFrameInfo->m_uiFrameSize) {	// TPDO is too short
+					// $$$ raise PDO error, set Ret
+					goto Exit;
+				}
+				if (wVarSize > wPdoSize) {	// memorize new PDO size
+					wPdoSize = wVarSize;
+				}
+				// copy object from process/OD variable to TPDO
+				Ret =
+				    EplObdReadEntryToLe(wObdCommIndex,
+							bObdSubindex,
+							&pFrame->m_Data.m_Pres.
+							m_le_abPayload[(wBitOffset >> 3)], &ObdSize);
+				if (Ret != kEplSuccessful) {	// other fatal error occured
+					goto Exit;
+				}
+
+			}
+
+			// set PDO size in frame
+			AmiSetWordToLe(&pFrame->m_Data.m_Pres.m_le_wSize,
+				       wPdoSize);
+
+			Ret = EplPdokCalAreTpdosValid(&fValid);
+			if (fValid != FALSE) {
+				// set TPDO valid
+				bFrameData =
+				    AmiGetByteFromLe(&pFrame->m_Data.m_Pres.
+						     m_le_bFlag1);
+				AmiSetByteToLe(&pFrame->m_Data.m_Pres.
+					       m_le_bFlag1,
+					       (bFrameData |
+						EPL_FRAME_FLAG1_RD));
+			}
+			// processing finished successfully
+
+			goto Exit;
+		}
+		break;
+
+	case kEplEventTypePdoSoa:	// SoA received
+
+		// invalidate TPDOs
+		Ret = EplPdokCalSetTpdosValid(FALSE);
+		break;
+
+	default:
+		{
+			ASSERTMSG(FALSE,
+				  "EplPdokProcess(): unhandled event type!\n");
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplPdokCal.c b/drivers/staging/epl/EplPdokCal.c
new file mode 100644
index 000000000000..f44c47578002
--- /dev/null
+++ b/drivers/staging/epl/EplPdokCal.c
@@ -0,0 +1,266 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for kernel PDO Communication Abstraction Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplPdokCal.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/27 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "kernel/EplPdokCal.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplPdokCal                                          */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	BOOL m_fTpdosValid;
+
+} tEplPdokCalInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+static tEplPdokCalInstance EplPdokCalInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokCalAddInstance()
+//
+// Description: add and initialize new instance of EPL stack
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCalAddInstance(void)
+{
+
+	EPL_MEMSET(&EplPdokCalInstance_g, 0, sizeof(EplPdokCalInstance_g));
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokCalDelInstance()
+//
+// Description: deletes an instance of EPL stack
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCalDelInstance(void)
+{
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokCalSetTpdosValid()
+//
+// Description: This function sets the validity flag for TPDOs to the
+//              specified value.
+//
+// Parameters:  fValid_p                = validity flag
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCalSetTpdosValid(BOOL fValid_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	EplPdokCalInstance_g.m_fTpdosValid = fValid_p;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokCalAreTpdosValid()
+//
+// Description: This function returns the validity flag for TPDOs.
+//
+// Parameters:  pfValid_p               = OUT: validity flag
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCalAreTpdosValid(BOOL * pfValid_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	*pfValid_p = EplPdokCalInstance_g.m_fTpdosValid;
+
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#endif
+
+// EOF
diff --git a/drivers/staging/epl/EplPdou.c b/drivers/staging/epl/EplPdou.c
new file mode 100644
index 000000000000..e7b10653a0ed
--- /dev/null
+++ b/drivers/staging/epl/EplPdou.c
@@ -0,0 +1,565 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for user PDO module
+                Currently, this module just implements a OD callback function
+                to check if the PDO configuration is valid.
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplPdou.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "EplInc.h"
+//#include "user/EplPdouCal.h"
+#include "user/EplObdu.h"
+#include "user/EplPdou.h"
+#include "EplSdoAc.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOU)) != 0)
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) == 0) && (EPL_OBD_USE_KERNEL == FALSE)
+#error "EPL PDOu module needs EPL module OBDU or OBDK!"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define EPL_PDOU_OBD_IDX_RX_COMM_PARAM  0x1400
+#define EPL_PDOU_OBD_IDX_RX_MAPP_PARAM  0x1600
+#define EPL_PDOU_OBD_IDX_TX_COMM_PARAM  0x1800
+#define EPL_PDOU_OBD_IDX_TX_MAPP_PARAM  0x1A00
+#define EPL_PDOU_OBD_IDX_MAPP_PARAM     0x0200
+#define EPL_PDOU_OBD_IDX_MASK           0xFF00
+#define EPL_PDOU_PDO_ID_MASK            0x00FF
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplPdou                                             */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam MEM * pParam_p,
+					  unsigned int uiIndex_p);
+
+static void EplPdouDecodeObjectMapping(QWORD qwObjectMapping_p,
+				       unsigned int *puiIndex_p,
+				       unsigned int *puiSubIndex_p,
+				       unsigned int *puiBitOffset_p,
+				       unsigned int *puiBitSize_p);
+
+static tEplKernel EplPdouCheckObjectMapping(QWORD qwObjectMapping_p,
+					    tEplObdAccess AccessType_p,
+					    DWORD * pdwAbortCode_p,
+					    unsigned int *puiPdoSize_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdouAddInstance()
+//
+// Description: add and initialize new instance of EPL stack
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdouAddInstance(void)
+{
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdouDelInstance()
+//
+// Description: deletes an instance of EPL stack
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdouDelInstance(void)
+{
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdouCbObdAccess
+//
+// Description: callback function for OD accesses
+//
+// Parameters:  pParam_p                = OBD parameter
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplPdouCbObdAccess(tEplObdCbParam MEM * pParam_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiPdoId;
+	unsigned int uiIndexType;
+	tEplObdSize ObdSize;
+	BYTE bObjectCount;
+	QWORD qwObjectMapping;
+	tEplObdAccess AccessType;
+	BYTE bMappSubindex;
+	unsigned int uiCurPdoSize;
+	WORD wMaxPdoSize;
+	unsigned int uiSubIndex;
+
+	// fetch PDO ID
+	uiPdoId = pParam_p->m_uiIndex & EPL_PDOU_PDO_ID_MASK;
+
+	// fetch object index type
+	uiIndexType = pParam_p->m_uiIndex & EPL_PDOU_OBD_IDX_MASK;
+
+	if (pParam_p->m_ObdEvent != kEplObdEvPreWrite) {	// read accesses, post write events etc. are OK
+		pParam_p->m_dwAbortCode = 0;
+		goto Exit;
+	}
+	// check index type
+	switch (uiIndexType) {
+	case EPL_PDOU_OBD_IDX_RX_COMM_PARAM:
+		// RPDO communication parameter accessed
+	case EPL_PDOU_OBD_IDX_TX_COMM_PARAM:
+		{		// TPDO communication parameter accessed
+			Ret = EplPdouCheckPdoValidity(pParam_p,
+						      (EPL_PDOU_OBD_IDX_MAPP_PARAM
+						       | pParam_p->m_uiIndex));
+			if (Ret != kEplSuccessful) {	// PDO is valid or does not exist
+				goto Exit;
+			}
+
+			goto Exit;
+		}
+
+	case EPL_PDOU_OBD_IDX_RX_MAPP_PARAM:
+		{		// RPDO mapping parameter accessed
+
+			AccessType = kEplObdAccWrite;
+			break;
+		}
+
+	case EPL_PDOU_OBD_IDX_TX_MAPP_PARAM:
+		{		// TPDO mapping parameter accessed
+
+			AccessType = kEplObdAccRead;
+			break;
+		}
+
+	default:
+		{		// this callback function is only for
+			// PDO mapping and communication parameters
+			pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR;
+			goto Exit;
+		}
+	}
+
+	// RPDO and TPDO mapping parameter accessed
+
+	if (pParam_p->m_uiSubIndex == 0) {	// object mapping count accessed
+
+		// PDO is enabled or disabled
+		bObjectCount = *((BYTE *) pParam_p->m_pArg);
+
+		if (bObjectCount == 0) {	// PDO shall be disabled
+
+			// that is always possible
+			goto Exit;
+		}
+		// PDO shall be enabled
+		// it should have been disabled for this operation
+		Ret = EplPdouCheckPdoValidity(pParam_p, pParam_p->m_uiIndex);
+		if (Ret != kEplSuccessful) {	// PDO is valid or does not exist
+			goto Exit;
+		}
+
+		if (AccessType == kEplObdAccWrite) {
+			uiSubIndex = 0x04;	// PReqActPayloadLimit_U16
+		} else {
+			uiSubIndex = 0x05;	// PResActPayloadLimit_U16
+		}
+
+		// fetch maximum PDO size from Object 1F98h: NMT_CycleTiming_REC
+		ObdSize = sizeof(wMaxPdoSize);
+		Ret =
+		    EplObduReadEntry(0x1F98, uiSubIndex, &wMaxPdoSize,
+				     &ObdSize);
+		if (Ret != kEplSuccessful) {	// other fatal error occured
+			pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR;
+			goto Exit;
+		}
+		// check all objectmappings
+		for (bMappSubindex = 1; bMappSubindex <= bObjectCount;
+		     bMappSubindex++) {
+			// read object mapping from OD
+			ObdSize = sizeof(qwObjectMapping);	// QWORD
+			Ret = EplObduReadEntry(pParam_p->m_uiIndex,
+					       bMappSubindex, &qwObjectMapping,
+					       &ObdSize);
+			if (Ret != kEplSuccessful) {	// other fatal error occured
+				pParam_p->m_dwAbortCode =
+				    EPL_SDOAC_GENERAL_ERROR;
+				goto Exit;
+			}
+			// check object mapping
+			Ret = EplPdouCheckObjectMapping(qwObjectMapping,
+							AccessType,
+							&pParam_p->
+							m_dwAbortCode,
+							&uiCurPdoSize);
+			if (Ret != kEplSuccessful) {	// illegal object mapping
+				goto Exit;
+			}
+
+			if (uiCurPdoSize > wMaxPdoSize) {	// mapping exceeds object size
+				pParam_p->m_dwAbortCode =
+				    EPL_SDOAC_GENERAL_ERROR;
+				Ret = kEplPdoVarNotFound;
+			}
+
+		}
+
+	} else {		// ObjectMapping
+		Ret = EplPdouCheckPdoValidity(pParam_p, pParam_p->m_uiIndex);
+		if (Ret != kEplSuccessful) {	// PDO is valid or does not exist
+			goto Exit;
+		}
+		// check existence of object and validity of object length
+
+		qwObjectMapping = *((QWORD *) pParam_p->m_pArg);
+
+		Ret = EplPdouCheckObjectMapping(qwObjectMapping,
+						AccessType,
+						&pParam_p->m_dwAbortCode,
+						&uiCurPdoSize);
+
+	}
+
+      Exit:
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdouCheckPdoValidity
+//
+// Description: check if PDO is valid
+//
+// Parameters:  pParam_p                = OBD parameter
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam MEM * pParam_p,
+					  unsigned int uiIndex_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplObdSize ObdSize;
+	BYTE bObjectCount;
+
+	ObdSize = 1;
+	// read number of mapped objects from OD; this indicates if the PDO is valid
+	Ret = EplObduReadEntry(uiIndex_p, 0x00, &bObjectCount, &ObdSize);
+	if (Ret != kEplSuccessful) {	// other fatal error occured
+		pParam_p->m_dwAbortCode =
+		    EPL_SDOAC_GEN_INTERNAL_INCOMPATIBILITY;
+		goto Exit;
+	}
+	// entry read successfully
+	if (bObjectCount != 0) {	// PDO in OD is still valid
+		pParam_p->m_dwAbortCode = EPL_SDOAC_GEN_PARAM_INCOMPATIBILITY;
+		Ret = kEplPdoNotExist;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdouDecodeObjectMapping
+//
+// Description: decodes the given object mapping entry into index, subindex,
+//              bit offset and bit size.
+//
+// Parameters:  qwObjectMapping_p       = object mapping entry
+//              puiIndex_p              = [OUT] pointer to object index
+//              puiSubIndex_p           = [OUT] pointer to subindex
+//              puiBitOffset_p          = [OUT] pointer to bit offset
+//              puiBitSize_p            = [OUT] pointer to bit size
+//
+// Returns:     (void)
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static void EplPdouDecodeObjectMapping(QWORD qwObjectMapping_p,
+				       unsigned int *puiIndex_p,
+				       unsigned int *puiSubIndex_p,
+				       unsigned int *puiBitOffset_p,
+				       unsigned int *puiBitSize_p)
+{
+	*puiIndex_p = (unsigned int)
+	    (qwObjectMapping_p & 0x000000000000FFFFLL);
+
+	*puiSubIndex_p = (unsigned int)
+	    ((qwObjectMapping_p & 0x0000000000FF0000LL) >> 16);
+
+	*puiBitOffset_p = (unsigned int)
+	    ((qwObjectMapping_p & 0x0000FFFF00000000LL) >> 32);
+
+	*puiBitSize_p = (unsigned int)
+	    ((qwObjectMapping_p & 0xFFFF000000000000LL) >> 48);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdouCheckObjectMapping
+//
+// Description: checks the given object mapping entry.
+//
+// Parameters:  qwObjectMapping_p       = object mapping entry
+//              AccessType_p            = access type to mapped object:
+//                                        write = RPDO and read = TPDO
+//              puiPdoSize_p            = [OUT] pointer to covered PDO size
+//                                        (offset + size) in byte;
+//                                        0 if mapping failed
+//              pdwAbortCode_p          = [OUT] pointer to SDO abort code;
+//                                        0 if mapping is possible
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplPdouCheckObjectMapping(QWORD qwObjectMapping_p,
+					    tEplObdAccess AccessType_p,
+					    DWORD * pdwAbortCode_p,
+					    unsigned int *puiPdoSize_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplObdSize ObdSize;
+	unsigned int uiIndex;
+	unsigned int uiSubIndex;
+	unsigned int uiBitOffset;
+	unsigned int uiBitSize;
+	tEplObdAccess AccessType;
+	BOOL fNumerical;
+
+	if (qwObjectMapping_p == 0) {	// discard zero value
+		*puiPdoSize_p = 0;
+		goto Exit;
+	}
+	// decode object mapping
+	EplPdouDecodeObjectMapping(qwObjectMapping_p,
+				   &uiIndex,
+				   &uiSubIndex, &uiBitOffset, &uiBitSize);
+
+	if ((uiBitOffset & 0x7) != 0x0) {	// bit mapping is not supported
+		*pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
+		Ret = kEplPdoGranularityMismatch;
+		goto Exit;
+	}
+
+	if ((uiBitSize & 0x7) != 0x0) {	// bit mapping is not supported
+		*pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
+		Ret = kEplPdoGranularityMismatch;
+		goto Exit;
+	}
+	// check access type
+	Ret = EplObduGetAccessType(uiIndex, uiSubIndex, &AccessType);
+	if (Ret != kEplSuccessful) {	// entry doesn't exist
+		*pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_EXIST;
+		goto Exit;
+	}
+
+	if ((AccessType & kEplObdAccPdo) == 0) {	// object is not mappable
+		*pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_MAPPABLE;
+		Ret = kEplPdoVarNotFound;
+		goto Exit;
+	}
+
+	if ((AccessType & AccessType_p) == 0) {	// object is not writeable (RPDO) or readable (TPDO) respectively
+		*pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_MAPPABLE;
+		Ret = kEplPdoVarNotFound;
+		goto Exit;
+	}
+
+	ObdSize = EplObduGetDataSize(uiIndex, uiSubIndex);
+	if (ObdSize < (uiBitSize >> 3)) {	// object does not exist or has smaller size
+		*pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
+		Ret = kEplPdoVarNotFound;
+	}
+
+	Ret = EplObduIsNumerical(uiIndex, uiSubIndex, &fNumerical);
+	if (Ret != kEplSuccessful) {	// entry doesn't exist
+		*pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_EXIST;
+		goto Exit;
+	}
+
+	if ((fNumerical != FALSE)
+	    && ((uiBitSize >> 3) != ObdSize)) {
+		// object is numerical,
+		// therefor size has to fit, but it does not.
+		*pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
+		Ret = kEplPdoVarNotFound;
+		goto Exit;
+	}
+	// calucaled needed PDO size
+	*puiPdoSize_p = (uiBitOffset >> 3) + (uiBitSize >> 3);
+
+      Exit:
+	return Ret;
+}
+
+#endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOU)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplSdo.h b/drivers/staging/epl/EplSdo.h
new file mode 100644
index 000000000000..1cb3f2de4a3c
--- /dev/null
+++ b/drivers/staging/epl/EplSdo.h
@@ -0,0 +1,245 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for api function of the sdo module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdo.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+ 2006/06/26 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "EplFrame.h"
+#include "EplSdoAc.h"
+
+#ifndef _EPLSDO_H_
+#define _EPLSDO_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+// global defines
+#ifndef EPL_SDO_MAX_PAYLOAD
+#define EPL_SDO_MAX_PAYLOAD     256
+#endif
+
+// handle between Protocol Abstraction Layer and asynchronous SDO Sequence Layer
+#define EPL_SDO_UDP_HANDLE      0x8000
+#define EPL_SDO_ASND_HANDLE     0x4000
+#define EPL_SDO_ASY_HANDLE_MASK 0xC000
+#define EPL_SDO_ASY_INVALID_HDL 0x3FFF
+
+// handle between  SDO Sequence Layer and sdo command layer
+#define EPL_SDO_ASY_HANDLE      0x8000
+#define EPL_SDO_PDO_HANDLE      0x4000
+#define EPL_SDO_SEQ_HANDLE_MASK 0xC000
+#define EPL_SDO_SEQ_INVALID_HDL 0x3FFF
+
+#define EPL_ASND_HEADER_SIZE        4
+//#define EPL_SEQ_HEADER_SIZE         4
+#define EPL_ETHERNET_HEADER_SIZE    14
+
+#define EPL_SEQ_NUM_MASK            0xFC
+
+// size for send buffer and history
+#define EPL_MAX_SDO_FRAME_SIZE      EPL_C_IP_MIN_MTU
+// size for receive frame
+// -> needed because SND-Kit sends up to 1518 Byte
+//    without Sdo-Command: Maximum Segment Size
+#define EPL_MAX_SDO_REC_FRAME_SIZE  EPL_C_IP_MAX_MTU
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+// handle between Protocol Abstraction Layer and asynchronuus SDO Sequence Layer
+typedef unsigned int tEplSdoConHdl;
+
+// callback function pointer for Protocol Abstraction Layer to call
+// asynchronuus SDO Sequence Layer
+typedef tEplKernel(PUBLIC * tEplSequLayerReceiveCb) (tEplSdoConHdl ConHdl_p,
+						     tEplAsySdoSeq *
+						     pSdoSeqData_p,
+						     unsigned int uiDataSize_p);
+
+// handle between asynchronuus SDO Sequence Layer and SDO Command layer
+typedef unsigned int tEplSdoSeqConHdl;
+
+// callback function pointer for asynchronuus SDO Sequence Layer to call
+// SDO Command layer for received data
+typedef tEplKernel(PUBLIC *
+		   tEplSdoComReceiveCb) (tEplSdoSeqConHdl SdoSeqConHdl_p,
+					 tEplAsySdoCom * pAsySdoCom_p,
+					 unsigned int uiDataSize_p);
+
+// status of connection
+typedef enum {
+	kAsySdoConStateConnected = 0x00,
+	kAsySdoConStateInitError = 0x01,
+	kAsySdoConStateConClosed = 0x02,
+	kAsySdoConStateAckReceived = 0x03,
+	kAsySdoConStateFrameSended = 0x04,
+	kAsySdoConStateTimeout = 0x05
+} tEplAsySdoConState;
+
+// callback function pointer for asynchronuus SDO Sequence Layer to call
+// SDO Command layer for connection status
+typedef tEplKernel(PUBLIC * tEplSdoComConCb) (tEplSdoSeqConHdl SdoSeqConHdl_p,
+					      tEplAsySdoConState
+					      AsySdoConState_p);
+
+// handle between  SDO Command layer and application
+typedef unsigned int tEplSdoComConHdl;
+
+// status of connection
+typedef enum {
+	kEplSdoComTransferNotActive = 0x00,
+	kEplSdoComTransferRunning = 0x01,
+	kEplSdoComTransferTxAborted = 0x02,
+	kEplSdoComTransferRxAborted = 0x03,
+	kEplSdoComTransferFinished = 0x04,
+	kEplSdoComTransferLowerLayerAbort = 0x05
+} tEplSdoComConState;
+
+// SDO Services and Command-Ids from DS 1.0.0 p.152
+typedef enum {
+	kEplSdoServiceNIL = 0x00,
+	kEplSdoServiceWriteByIndex = 0x01,
+	kEplSdoServiceReadByIndex = 0x02
+	    //--------------------------------
+	    // the following services are optional and
+	    // not supported now
+/*
+    kEplSdoServiceWriteAllByIndex   = 0x03,
+    kEplSdoServiceReadAllByIndex    = 0x04,
+    kEplSdoServiceWriteByName       = 0x05,
+    kEplSdoServiceReadByName        = 0x06,
+
+    kEplSdoServiceFileWrite         = 0x20,
+    kEplSdoServiceFileRead          = 0x21,
+
+    kEplSdoServiceWriteMultiByIndex = 0x31,
+    kEplSdoServiceReadMultiByIndex  = 0x32,
+
+    kEplSdoServiceMaxSegSize        = 0x70
+
+    // 0x80 - 0xFF manufacturer specific
+
+ */
+} tEplSdoServiceType;
+
+// describes if read or write access
+typedef enum {
+	kEplSdoAccessTypeRead = 0x00,
+	kEplSdoAccessTypeWrite = 0x01
+} tEplSdoAccessType;
+
+typedef enum {
+	kEplSdoTypeAuto = 0x00,
+	kEplSdoTypeUdp = 0x01,
+	kEplSdoTypeAsnd = 0x02,
+	kEplSdoTypePdo = 0x03
+} tEplSdoType;
+
+typedef enum {
+	kEplSdoTransAuto = 0x00,
+	kEplSdoTransExpedited = 0x01,
+	kEplSdoTransSegmented = 0x02
+} tEplSdoTransType;
+
+// structure to inform application about finish of SDO transfer
+typedef struct {
+	tEplSdoComConHdl m_SdoComConHdl;
+	tEplSdoComConState m_SdoComConState;
+	DWORD m_dwAbortCode;
+	tEplSdoAccessType m_SdoAccessType;
+	unsigned int m_uiNodeId;	// NodeId of the target
+	unsigned int m_uiTargetIndex;	// index which was accessed
+	unsigned int m_uiTargetSubIndex;	// subindex which was accessed
+	unsigned int m_uiTransferredByte;	// number of bytes transferred
+	void *m_pUserArg;	// user definable argument pointer
+
+} tEplSdoComFinished;
+
+// callback function pointer to inform application about connection
+typedef tEplKernel(PUBLIC * tEplSdoFinishedCb) (tEplSdoComFinished *
+						pSdoComFinished_p);
+
+// structure to init SDO transfer to Read or Write by Index
+typedef struct {
+	tEplSdoComConHdl m_SdoComConHdl;
+	unsigned int m_uiIndex;
+	unsigned int m_uiSubindex;
+	void *m_pData;
+	unsigned int m_uiDataSize;
+	unsigned int m_uiTimeout;	// not used in this version
+	tEplSdoAccessType m_SdoAccessType;
+	tEplSdoFinishedCb m_pfnSdoFinishedCb;
+	void *m_pUserArg;	// user definable argument pointer
+
+} tEplSdoComTransParamByIndex;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPLSDO_H_
diff --git a/drivers/staging/epl/EplSdoAc.h b/drivers/staging/epl/EplSdoAc.h
new file mode 100644
index 000000000000..400fb38ce3e9
--- /dev/null
+++ b/drivers/staging/epl/EplSdoAc.h
@@ -0,0 +1,111 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  definitions for SDO Abort codes
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoAc.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    ...
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/30 k.t.: first implementation
+
+****************************************************************************/
+
+#ifndef _EPLSDOAC_H_
+#define _EPLSDOAC_H_
+
+// =========================================================================
+// SDO abort codes
+// =========================================================================
+
+#define    EPL_SDOAC_TIME_OUT                            0x05040000L
+#define    EPL_SDOAC_UNKNOWN_COMMAND_SPECIFIER           0x05040001L
+#define    EPL_SDOAC_INVALID_BLOCK_SIZE                  0x05040002L
+#define    EPL_SDOAC_INVALID_SEQUENCE_NUMBER             0x05040003L
+#define    EPL_SDOAC_OUT_OF_MEMORY                       0x05040005L
+#define    EPL_SDOAC_UNSUPPORTED_ACCESS                  0x06010000L
+#define    EPL_SDOAC_READ_TO_WRITE_ONLY_OBJ              0x06010001L
+#define    EPL_SDOAC_WRITE_TO_READ_ONLY_OBJ              0x06010002L
+#define    EPL_SDOAC_OBJECT_NOT_EXIST                    0x06020000L
+#define    EPL_SDOAC_OBJECT_NOT_MAPPABLE                 0x06040041L
+#define    EPL_SDOAC_PDO_LENGTH_EXCEEDED                 0x06040042L
+#define    EPL_SDOAC_GEN_PARAM_INCOMPATIBILITY           0x06040043L
+#define    EPL_SDOAC_INVALID_HEARTBEAT_DEC               0x06040044L
+#define    EPL_SDOAC_GEN_INTERNAL_INCOMPATIBILITY        0x06040047L
+#define    EPL_SDOAC_ACCESS_FAILED_DUE_HW_ERROR          0x06060000L
+#define    EPL_SDOAC_DATA_TYPE_LENGTH_NOT_MATCH          0x06070010L
+#define    EPL_SDOAC_DATA_TYPE_LENGTH_TOO_HIGH           0x06070012L
+#define    EPL_SDOAC_DATA_TYPE_LENGTH_TOO_LOW            0x06070013L
+#define    EPL_SDOAC_SUB_INDEX_NOT_EXIST                 0x06090011L
+#define    EPL_SDOAC_VALUE_RANGE_EXCEEDED                0x06090030L
+#define    EPL_SDOAC_VALUE_RANGE_TOO_HIGH                0x06090031L
+#define    EPL_SDOAC_VALUE_RANGE_TOO_LOW                 0x06090032L
+#define    EPL_SDOAC_MAX_VALUE_LESS_MIN_VALUE            0x06090036L
+#define    EPL_SDOAC_GENERAL_ERROR                       0x08000000L
+#define    EPL_SDOAC_DATA_NOT_TRANSF_OR_STORED           0x08000020L
+#define    EPL_SDOAC_DATA_NOT_TRANSF_DUE_LOCAL_CONTROL   0x08000021L
+#define    EPL_SDOAC_DATA_NOT_TRANSF_DUE_DEVICE_STATE    0x08000022L
+#define    EPL_SDOAC_OBJECT_DICTIONARY_NOT_EXIST         0x08000023L
+#define    EPL_SDOAC_CONFIG_DATA_EMPTY                   0x08000024L
+
+#endif // _EPLSDOAC_H_
+
+// Die letzte Zeile muß unbedingt eine leere Zeile sein, weil manche Compiler
+// damit ein Problem haben, wenn das nicht so ist (z.B. GNU oder Borland C++ Builder).
diff --git a/drivers/staging/epl/EplSdoAsndu.c b/drivers/staging/epl/EplSdoAsndu.c
new file mode 100644
index 000000000000..05a00c9a731e
--- /dev/null
+++ b/drivers/staging/epl/EplSdoAsndu.c
@@ -0,0 +1,483 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for SDO/Asnd-Protocolabstractionlayer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoAsndu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.7 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/07 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplSdoAsndu.h"
+#include "user/EplDlluCal.h"
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef EPL_SDO_MAX_CONNECTION_ASND
+#define EPL_SDO_MAX_CONNECTION_ASND 5
+#endif
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+// instance table
+typedef struct {
+	unsigned int m_auiSdoAsndConnection[EPL_SDO_MAX_CONNECTION_ASND];
+	tEplSequLayerReceiveCb m_fpSdoAsySeqCb;
+
+} tEplSdoAsndInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+static tEplSdoAsndInstance SdoAsndInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplSdoAsnduCb(tEplFrameInfo * pFrameInfo_p);
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <EPL SDO-Asnd Protocolabstraction layer>            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: EPL SDO-Asnd Protocolabstraction layer
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsnduInit
+//
+// Description: init first instance of the module
+//
+//
+//
+// Parameters:  pReceiveCb_p    =   functionpointer to Sdo-Sequence layer
+//                                  callback-function
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsnduInit(tEplSequLayerReceiveCb fpReceiveCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplSdoAsnduAddInstance(fpReceiveCb_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsnduAddInstance
+//
+// Description: init additional instance of the module
+//
+//
+//
+// Parameters:  pReceiveCb_p    =   functionpointer to Sdo-Sequence layer
+//                                  callback-function
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsnduAddInstance(tEplSequLayerReceiveCb fpReceiveCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// init control structure
+	EPL_MEMSET(&SdoAsndInstance_g, 0x00, sizeof(SdoAsndInstance_g));
+
+	// save pointer to callback-function
+	if (fpReceiveCb_p != NULL) {
+		SdoAsndInstance_g.m_fpSdoAsySeqCb = fpReceiveCb_p;
+	} else {
+		Ret = kEplSdoUdpMissCb;
+	}
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	Ret = EplDlluCalRegAsndService(kEplDllAsndSdo,
+				       EplSdoAsnduCb, kEplDllAsndFilterLocal);
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsnduDelInstance
+//
+// Description: del instance of the module
+//              del socket and del Listen-Thread
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsnduDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	// deregister callback function from DLL
+	Ret = EplDlluCalRegAsndService(kEplDllAsndSdo,
+				       NULL, kEplDllAsndFilterNone);
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsnduInitCon
+//
+// Description: init a new connect
+//
+//
+//
+// Parameters:  pSdoConHandle_p = pointer for the new connection handle
+//              uiTargetNodeId_p = NodeId of the target node
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsnduInitCon(tEplSdoConHdl * pSdoConHandle_p,
+				     unsigned int uiTargetNodeId_p)
+{
+	tEplKernel Ret;
+	unsigned int uiCount;
+	unsigned int uiFreeCon;
+	unsigned int *puiConnection;
+
+	Ret = kEplSuccessful;
+
+	if ((uiTargetNodeId_p == EPL_C_ADR_INVALID)
+	    || (uiTargetNodeId_p >= EPL_C_ADR_BROADCAST)) {
+		Ret = kEplSdoAsndInvalidNodeId;
+		goto Exit;
+	}
+	// get free entry in control structure
+	uiCount = 0;
+	uiFreeCon = EPL_SDO_MAX_CONNECTION_ASND;
+	puiConnection = &SdoAsndInstance_g.m_auiSdoAsndConnection[0];
+	while (uiCount < EPL_SDO_MAX_CONNECTION_ASND) {
+		if (*puiConnection == uiTargetNodeId_p) {	// existing connection to target node found
+			// save handle for higher layer
+			*pSdoConHandle_p = (uiCount | EPL_SDO_ASND_HANDLE);
+
+			goto Exit;
+		} else if (*puiConnection == 0) {	// free entry-> save target nodeId
+			uiFreeCon = uiCount;
+		}
+		uiCount++;
+		puiConnection++;
+	}
+
+	if (uiFreeCon == EPL_SDO_MAX_CONNECTION_ASND) {
+		// no free connection
+		Ret = kEplSdoAsndNoFreeHandle;
+	} else {
+		puiConnection =
+		    &SdoAsndInstance_g.m_auiSdoAsndConnection[uiFreeCon];
+		*puiConnection = uiTargetNodeId_p;
+		// save handle for higher layer
+		*pSdoConHandle_p = (uiFreeCon | EPL_SDO_ASND_HANDLE);
+
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsnduSendData
+//
+// Description: send data using exisiting connection
+//
+//
+//
+// Parameters:  SdoConHandle_p  = connection handle
+//              pSrcData_p      = pointer to data
+//              dwDataSize_p    = number of databyte
+//                                  -> without asnd-header!!!
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsnduSendData(tEplSdoConHdl SdoConHandle_p,
+				      tEplFrame * pSrcData_p,
+				      DWORD dwDataSize_p)
+{
+	tEplKernel Ret;
+	unsigned int uiArray;
+	tEplFrameInfo FrameInfo;
+
+	Ret = kEplSuccessful;
+
+	uiArray = (SdoConHandle_p & ~EPL_SDO_ASY_HANDLE_MASK);
+
+	if (uiArray > EPL_SDO_MAX_CONNECTION_ASND) {
+		Ret = kEplSdoAsndInvalidHandle;
+		goto Exit;
+	}
+	// fillout Asnd header
+	// own node id not needed -> filled by DLL
+
+	// set message type
+	AmiSetByteToLe(&pSrcData_p->m_le_bMessageType, (BYTE) kEplMsgTypeAsnd);	// ASnd == 0x06
+	// target node id
+	AmiSetByteToLe(&pSrcData_p->m_le_bDstNodeId,
+		       (BYTE) SdoAsndInstance_g.
+		       m_auiSdoAsndConnection[uiArray]);
+	// set source-nodeid (filled by DLL 0)
+	AmiSetByteToLe(&pSrcData_p->m_le_bSrcNodeId, 0x00);
+
+	// calc size
+	dwDataSize_p += EPL_ASND_HEADER_SIZE;
+
+	// send function of DLL
+	FrameInfo.m_uiFrameSize = dwDataSize_p;
+	FrameInfo.m_pFrame = pSrcData_p;
+	EPL_MEMSET(&FrameInfo.m_NetTime, 0x00, sizeof(tEplNetTime));
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	Ret = EplDlluCalAsyncSend(&FrameInfo, kEplDllAsyncReqPrioGeneric);
+#endif
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsnduDelCon
+//
+// Description: delete connection from intern structure
+//
+//
+//
+// Parameters:  SdoConHandle_p  = connection handle
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsnduDelCon(tEplSdoConHdl SdoConHandle_p)
+{
+	tEplKernel Ret;
+	unsigned int uiArray;
+
+	Ret = kEplSuccessful;
+
+	uiArray = (SdoConHandle_p & ~EPL_SDO_ASY_HANDLE_MASK);
+	// check parameter
+	if (uiArray > EPL_SDO_MAX_CONNECTION_ASND) {
+		Ret = kEplSdoAsndInvalidHandle;
+		goto Exit;
+	}
+	// set target nodeId to 0
+	SdoAsndInstance_g.m_auiSdoAsndConnection[uiArray] = 0;
+
+      Exit:
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsnduCb
+//
+// Description: callback function for SDO ASnd frames
+//
+//
+//
+// Parameters:      pFrameInfo_p = Frame with SDO payload
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsnduCb(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiCount;
+	unsigned int *puiConnection;
+	unsigned int uiNodeId;
+	unsigned int uiFreeEntry = 0xFFFF;
+	tEplSdoConHdl SdoConHdl;
+	tEplFrame *pFrame;
+
+	pFrame = pFrameInfo_p->m_pFrame;
+
+	uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bSrcNodeId);
+
+	// search corresponding entry in control structure
+	uiCount = 0;
+	puiConnection = &SdoAsndInstance_g.m_auiSdoAsndConnection[0];
+	while (uiCount < EPL_SDO_MAX_CONNECTION_ASND) {
+		if (uiNodeId == *puiConnection) {
+			break;
+		} else if ((*puiConnection == 0)
+			   && (uiFreeEntry == 0xFFFF)) {	// free entry
+			uiFreeEntry = uiCount;
+		}
+		uiCount++;
+		puiConnection++;
+	}
+
+	if (uiCount == EPL_SDO_MAX_CONNECTION_ASND) {
+		if (uiFreeEntry != 0xFFFF) {
+			puiConnection =
+			    &SdoAsndInstance_g.
+			    m_auiSdoAsndConnection[uiFreeEntry];
+			*puiConnection = uiNodeId;
+			uiCount = uiFreeEntry;
+		} else {
+			EPL_DBGLVL_SDO_TRACE0
+			    ("EplSdoAsnduCb(): no free handle\n");
+			goto Exit;
+		}
+	}
+//    if (uiNodeId == *puiConnection)
+	{			// entry found or created
+		SdoConHdl = (uiCount | EPL_SDO_ASND_HANDLE);
+
+		SdoAsndInstance_g.m_fpSdoAsySeqCb(SdoConHdl,
+						  &pFrame->m_Data.m_Asnd.
+						  m_Payload.m_SdoSequenceFrame,
+						  (pFrameInfo_p->m_uiFrameSize -
+						   18));
+	}
+
+      Exit:
+	return Ret;
+
+}
+
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+// EOF
diff --git a/drivers/staging/epl/EplSdoAsySequ.c b/drivers/staging/epl/EplSdoAsySequ.c
new file mode 100644
index 000000000000..6b6a9975d78b
--- /dev/null
+++ b/drivers/staging/epl/EplSdoAsySequ.c
@@ -0,0 +1,2522 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for asychronous SDO Sequence Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoAsySequ.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.10 $  $Date: 2008/11/13 17:13:09 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/26 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplSdoAsySequ.h"
+
+#if ((((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) == 0) &&\
+     (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) == 0)   )
+
+#error 'ERROR: At least UDP or Asnd module needed!'
+
+#endif
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define EPL_SDO_HISTORY_SIZE        5
+
+#ifndef EPL_MAX_SDO_SEQ_CON
+#define EPL_MAX_SDO_SEQ_CON         10
+#endif
+
+#define EPL_SEQ_DEFAULT_TIMEOUT     5000	// in [ms] => 5 sec
+
+#define EPL_SEQ_RETRY_COUNT         5	// => max. Timeout 30 sec
+
+#define EPL_SEQ_NUM_THRESHOLD       100	// threshold which distinguishes between old and new sequence numbers
+
+// define frame with size of Asnd-Header-, SDO Sequenze Header size, SDO Command header
+// and Ethernet-Header size
+#define EPL_SEQ_FRAME_SIZE          24
+// size of the header of the asynchronus SDO Sequence layer
+#define EPL_SEQ_HEADER_SIZE         4
+
+// buffersize for one frame in history
+#define EPL_SEQ_HISTROY_FRAME_SIZE  EPL_MAX_SDO_FRAME_SIZE
+
+// mask to get scon and rcon
+#define EPL_ASY_SDO_CON_MASK        0x03
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+// events for processfunction
+typedef enum {
+	kAsySdoSeqEventNoEvent = 0x00,	// no Event
+	kAsySdoSeqEventInitCon = 0x01,	// init connection
+	kAsySdoSeqEventFrameRec = 0x02,	// frame received
+	kAsySdoSeqEventFrameSend = 0x03,	// frame to send
+	kAsySdoSeqEventTimeout = 0x04,	// Timeout for connection
+	kAsySdoSeqEventCloseCon = 0x05	// higher layer close connection
+} tEplAsySdoSeqEvent;
+
+// structure for History-Buffer
+typedef struct {
+	BYTE m_bFreeEntries;
+	BYTE m_bWrite;		// index of the next free buffer entry
+	BYTE m_bAck;		// index of the next message which should become acknowledged
+	BYTE m_bRead;		// index between m_bAck and m_bWrite to the next message for retransmission
+	BYTE m_aabHistoryFrame[EPL_SDO_HISTORY_SIZE]
+	    [EPL_SEQ_HISTROY_FRAME_SIZE];
+	unsigned int m_auiFrameSize[EPL_SDO_HISTORY_SIZE];
+
+} tEplAsySdoConHistory;
+
+// state of the statemaschine
+typedef enum {
+	kEplAsySdoStateIdle = 0x00,
+	kEplAsySdoStateInit1 = 0x01,
+	kEplAsySdoStateInit2 = 0x02,
+	kEplAsySdoStateInit3 = 0x03,
+	kEplAsySdoStateConnected = 0x04,
+	kEplAsySdoStateWaitAck = 0x05
+} tEplAsySdoState;
+
+// connection control structure
+typedef struct {
+	tEplSdoConHdl m_ConHandle;
+	tEplAsySdoState m_SdoState;
+	BYTE m_bRecSeqNum;	// name from view of the communication partner
+	BYTE m_bSendSeqNum;	// name from view of the communication partner
+	tEplAsySdoConHistory m_SdoConHistory;
+	tEplTimerHdl m_EplTimerHdl;
+	unsigned int m_uiRetryCount;	// retry counter
+	unsigned int m_uiUseCount;	// one sequence layer connection may be used by
+	// multiple command layer connections
+
+} tEplAsySdoSeqCon;
+
+// instance structure
+typedef struct {
+	tEplAsySdoSeqCon m_AsySdoConnection[EPL_MAX_SDO_SEQ_CON];
+	tEplSdoComReceiveCb m_fpSdoComReceiveCb;
+	tEplSdoComConCb m_fpSdoComConCb;
+
+#if defined(WIN32) || defined(_WIN32)
+	LPCRITICAL_SECTION m_pCriticalSection;
+	CRITICAL_SECTION m_CriticalSection;
+
+	LPCRITICAL_SECTION m_pCriticalSectionReceive;
+	CRITICAL_SECTION m_CriticalSectionReceive;
+#endif
+
+} tEplAsySdoSequInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+static tEplAsySdoSequInstance AsySdoSequInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplKernel EplSdoAsySeqProcess(unsigned int uiHandle_p,
+				      unsigned int uiDataSize_p,
+				      tEplFrame * pData_p,
+				      tEplAsySdoSeq * pRecFrame_p,
+				      tEplAsySdoSeqEvent Event_p);
+
+static tEplKernel EplSdoAsySeqSendIntern(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					 unsigned int uiDataSize_p,
+					 tEplFrame * pData_p,
+					 BOOL fFrameInHistory);
+
+static tEplKernel EplSdoAsySeqSendLowerLayer(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					     unsigned int uiDataSize_p,
+					     tEplFrame * pEplFrame_p);
+
+tEplKernel PUBLIC EplSdoAsyReceiveCb(tEplSdoConHdl ConHdl_p,
+				     tEplAsySdoSeq * pSdoSeqData_p,
+				     unsigned int uiDataSize_p);
+
+static tEplKernel EplSdoAsyInitHistory(void);
+
+static tEplKernel EplSdoAsyAddFrameToHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					     tEplFrame * pFrame_p,
+					     unsigned int uiSize_p);
+
+static tEplKernel EplSdoAsyAckFrameToHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					     BYTE bRecSeqNumber_p);
+
+static tEplKernel EplSdoAsyReadFromHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					   tEplFrame ** ppFrame_p,
+					   unsigned int *puiSize_p,
+					   BOOL fInitRead);
+
+static unsigned int EplSdoAsyGetFreeEntriesFromHistory(tEplAsySdoSeqCon *
+						       pAsySdoSeqCon_p);
+
+static tEplKernel EplSdoAsySeqSetTimer(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+				       unsigned long ulTimeout);
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <EPL asychronus SDO Sequence layer>                 */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: this module contains the asynchronus SDO Sequence Layer for
+//              the EPL SDO service
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqInit
+//
+// Description: init first instance
+//
+//
+//
+// Parameters:  fpSdoComCb_p    = callback function to inform Command layer
+//                                about new frames
+//              fpSdoComConCb_p = callback function to inform command layer
+//                                about connection state
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqInit(tEplSdoComReceiveCb fpSdoComCb_p,
+				   tEplSdoComConCb fpSdoComConCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplSdoAsySeqAddInstance(fpSdoComCb_p, fpSdoComConCb_p);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqAddInstance
+//
+// Description: init following instances
+//
+//
+//
+// Parameters:  fpSdoComCb_p    = callback function to inform Command layer
+//                                about new frames
+//              fpSdoComConCb_p = callback function to inform command layer
+//                                about connection state
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqAddInstance(tEplSdoComReceiveCb fpSdoComCb_p,
+					  tEplSdoComConCb fpSdoComConCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// check functionpointer
+	if (fpSdoComCb_p == NULL) {
+		Ret = kEplSdoSeqMissCb;
+		goto Exit;
+	} else {
+		AsySdoSequInstance_g.m_fpSdoComReceiveCb = fpSdoComCb_p;
+	}
+
+	// check functionpointer
+	if (fpSdoComConCb_p == NULL) {
+		Ret = kEplSdoSeqMissCb;
+		goto Exit;
+	} else {
+		AsySdoSequInstance_g.m_fpSdoComConCb = fpSdoComConCb_p;
+	}
+
+	// set controllstructure to 0
+	EPL_MEMSET(&AsySdoSequInstance_g.m_AsySdoConnection[0], 0x00,
+		   sizeof(AsySdoSequInstance_g.m_AsySdoConnection));
+
+	// init History
+	Ret = EplSdoAsyInitHistory();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#if defined(WIN32) || defined(_WIN32)
+	// create critical section for process function
+	AsySdoSequInstance_g.m_pCriticalSection =
+	    &AsySdoSequInstance_g.m_CriticalSection;
+	InitializeCriticalSection(AsySdoSequInstance_g.m_pCriticalSection);
+
+	// init critical section for receive cb function
+	AsySdoSequInstance_g.m_pCriticalSectionReceive =
+	    &AsySdoSequInstance_g.m_CriticalSectionReceive;
+	InitializeCriticalSection(AsySdoSequInstance_g.
+				  m_pCriticalSectionReceive);
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+	// init lower layer
+	Ret = EplSdoUdpuAddInstance(EplSdoAsyReceiveCb);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+	// init lower layer
+	Ret = EplSdoAsnduAddInstance(EplSdoAsyReceiveCb);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqDelInstance
+//
+// Description: delete instances
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqDelInstance()
+{
+	tEplKernel Ret;
+	unsigned int uiCount;
+	tEplAsySdoSeqCon *pAsySdoSeqCon;
+
+	Ret = kEplSuccessful;
+
+	// delete timer of open connections
+	uiCount = 0;
+	pAsySdoSeqCon = &AsySdoSequInstance_g.m_AsySdoConnection[0];
+	while (uiCount < EPL_MAX_SDO_SEQ_CON) {
+		if (pAsySdoSeqCon->m_ConHandle != 0) {
+			EplTimeruDeleteTimer(&pAsySdoSeqCon->m_EplTimerHdl);
+		}
+		uiCount++;
+		pAsySdoSeqCon++;
+	}
+
+#if defined(WIN32) || defined(_WIN32)
+	// delete critical section for process function
+	DeleteCriticalSection(AsySdoSequInstance_g.m_pCriticalSection);
+#endif
+
+	// set instance-table to 0
+	EPL_MEMSET(&AsySdoSequInstance_g, 0x00, sizeof(AsySdoSequInstance_g));
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+	// delete lower layer
+	Ret = EplSdoUdpuDelInstance();
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+	// delete lower layer
+	Ret = EplSdoAsnduDelInstance();
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqInitCon
+//
+// Description: start initialization of a sequence layer connection.
+//              It tries to reuse an existing connection to the same node.
+//
+//
+// Parameters:  pSdoSeqConHdl_p = pointer to the variable for the connection handle
+//              uiNodeId_p      = Node Id of the target
+//              SdoType          = Type of the SDO connection
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqInitCon(tEplSdoSeqConHdl * pSdoSeqConHdl_p,
+				      unsigned int uiNodeId_p,
+				      tEplSdoType SdoType)
+{
+	tEplKernel Ret;
+	unsigned int uiCount;
+	unsigned int uiFreeCon;
+	tEplSdoConHdl ConHandle;
+	tEplAsySdoSeqCon *pAsySdoSeqCon;
+	Ret = kEplSuccessful;
+
+	// check SdoType
+	// call init function of the protcol abstraction layer
+	// which tries to find an existing connection to the same node
+	switch (SdoType) {
+		// SDO over UDP
+	case kEplSdoTypeUdp:
+		{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+			Ret = EplSdoUdpuInitCon(&ConHandle, uiNodeId_p);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+#else
+			Ret = kEplSdoSeqUnsupportedProt;
+#endif
+			break;
+		}
+
+		// SDO over Asnd
+	case kEplSdoTypeAsnd:
+		{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+			Ret = EplSdoAsnduInitCon(&ConHandle, uiNodeId_p);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+#else
+			Ret = kEplSdoSeqUnsupportedProt;
+#endif
+			break;
+		}
+
+		// unsupported protocols
+		// -> auto should be replaced by command layer
+	case kEplSdoTypeAuto:
+	case kEplSdoTypePdo:
+	default:
+		{
+			Ret = kEplSdoSeqUnsupportedProt;
+			goto Exit;
+		}
+
+	}			// end of switch(SdoType)
+
+	// find existing connection to the same node or find empty entry for connection
+	uiCount = 0;
+	uiFreeCon = EPL_MAX_SDO_SEQ_CON;
+	pAsySdoSeqCon = &AsySdoSequInstance_g.m_AsySdoConnection[0];
+
+	while (uiCount < EPL_MAX_SDO_SEQ_CON) {
+		if (pAsySdoSeqCon->m_ConHandle == ConHandle) {	// existing connection found
+			break;
+		}
+		if (pAsySdoSeqCon->m_ConHandle == 0) {
+			uiFreeCon = uiCount;
+		}
+		uiCount++;
+		pAsySdoSeqCon++;
+	}
+
+	if (uiCount == EPL_MAX_SDO_SEQ_CON) {
+		if (uiFreeCon == EPL_MAX_SDO_SEQ_CON) {	// no free entry found
+			switch (SdoType) {
+				// SDO over UDP
+			case kEplSdoTypeUdp:
+				{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+					Ret = EplSdoUdpuDelCon(ConHandle);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+#endif
+					break;
+				}
+
+				// SDO over Asnd
+			case kEplSdoTypeAsnd:
+				{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+					Ret = EplSdoAsnduDelCon(ConHandle);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+#endif
+					break;
+				}
+
+				// unsupported protocols
+				// -> auto should be replaced by command layer
+			case kEplSdoTypeAuto:
+			case kEplSdoTypePdo:
+			default:
+				{
+					Ret = kEplSdoSeqUnsupportedProt;
+					goto Exit;
+				}
+
+			}	// end of switch(SdoType)
+
+			Ret = kEplSdoSeqNoFreeHandle;
+			goto Exit;
+		} else {	// free entry found
+			pAsySdoSeqCon =
+			    &AsySdoSequInstance_g.m_AsySdoConnection[uiFreeCon];
+			pAsySdoSeqCon->m_ConHandle = ConHandle;
+			uiCount = uiFreeCon;
+		}
+	}
+	// set handle
+	*pSdoSeqConHdl_p = (uiCount | EPL_SDO_ASY_HANDLE);
+
+	// increment use counter
+	pAsySdoSeqCon->m_uiUseCount++;
+
+	// call intern process function
+	Ret = EplSdoAsySeqProcess(uiCount,
+				  0, NULL, NULL, kAsySdoSeqEventInitCon);
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqSendData
+//
+// Description: send sata unsing a established connection
+//
+//
+//
+// Parameters:  pSdoSeqConHdl_p = connection handle
+//              uiDataSize_p    = Size of Frame to send
+//                                  -> wihtout SDO sequence layer header, Asnd header
+//                                     and ethernetnet
+//                                  ==> SDO Sequence layer payload
+//              SdoType          = Type of the SDO connection
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqSendData(tEplSdoSeqConHdl SdoSeqConHdl_p,
+				       unsigned int uiDataSize_p,
+				       tEplFrame * pabData_p)
+{
+	tEplKernel Ret;
+	unsigned int uiHandle;
+
+	uiHandle = (SdoSeqConHdl_p & ~EPL_SDO_SEQ_HANDLE_MASK);
+
+	// check if connection ready
+	if (AsySdoSequInstance_g.m_AsySdoConnection[uiHandle].m_SdoState ==
+	    kEplAsySdoStateIdle) {
+		// no connection with this handle
+		Ret = kEplSdoSeqInvalidHdl;
+		goto Exit;
+	} else if (AsySdoSequInstance_g.m_AsySdoConnection[uiHandle].
+		   m_SdoState != kEplAsySdoStateConnected) {
+		Ret = kEplSdoSeqConnectionBusy;
+		goto Exit;
+	}
+
+	Ret = EplSdoAsySeqProcess(uiHandle,
+				  uiDataSize_p,
+				  pabData_p, NULL, kAsySdoSeqEventFrameSend);
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqProcessEvent
+//
+// Description: function processes extern events
+//              -> later needed for timeout controll with timer-module
+//
+//
+//
+// Parameters:  pEvent_p = pointer to event
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqProcessEvent(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+	tEplTimerEventArg *pTimerEventArg;
+	tEplAsySdoSeqCon *pAsySdoSeqCon;
+	tEplTimerHdl EplTimerHdl;
+	unsigned int uiCount;
+
+	Ret = kEplSuccessful;
+	// check parameter
+	if (pEvent_p == NULL) {
+		Ret = kEplSdoSeqInvalidEvent;
+		goto Exit;
+	}
+
+	if (pEvent_p->m_EventType != kEplEventTypeTimer) {
+		Ret = kEplSdoSeqInvalidEvent;
+		goto Exit;
+	}
+	// get timerhdl
+	pTimerEventArg = (tEplTimerEventArg *) pEvent_p->m_pArg;
+	EplTimerHdl = pTimerEventArg->m_TimerHdl;
+
+	// get pointer to intern control structure of connection
+	if (pTimerEventArg->m_ulArg == 0) {
+		goto Exit;
+	}
+	pAsySdoSeqCon = (tEplAsySdoSeqCon *) pTimerEventArg->m_ulArg;
+
+	// check if time is current
+	if (EplTimerHdl != pAsySdoSeqCon->m_EplTimerHdl) {
+		// delete timer
+		EplTimeruDeleteTimer(&EplTimerHdl);
+		goto Exit;
+	}
+	// delete timer
+	EplTimeruDeleteTimer(&pAsySdoSeqCon->m_EplTimerHdl);
+
+	// get indexnumber of control structure
+	uiCount = 0;
+	while ((&AsySdoSequInstance_g.m_AsySdoConnection[uiCount]) !=
+	       pAsySdoSeqCon) {
+		uiCount++;
+		if (uiCount > EPL_MAX_SDO_SEQ_CON) {
+			goto Exit;
+		}
+	}
+
+	// process event and call processfunction if needed
+	Ret = EplSdoAsySeqProcess(uiCount,
+				  0, NULL, NULL, kAsySdoSeqEventTimeout);
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqDelCon
+//
+// Description: del and close one connection
+//
+//
+//
+// Parameters:  SdoSeqConHdl_p = handle of connection
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqDelCon(tEplSdoSeqConHdl SdoSeqConHdl_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiHandle;
+	tEplAsySdoSeqCon *pAsySdoSeqCon;
+
+	uiHandle = (SdoSeqConHdl_p & ~EPL_SDO_SEQ_HANDLE_MASK);
+
+	// check if handle invalid
+	if (uiHandle >= EPL_MAX_SDO_SEQ_CON) {
+		Ret = kEplSdoSeqInvalidHdl;
+		goto Exit;
+	}
+	// get pointer to connection
+	pAsySdoSeqCon = &AsySdoSequInstance_g.m_AsySdoConnection[uiHandle];
+
+	// decrement use counter
+	pAsySdoSeqCon->m_uiUseCount--;
+
+	if (pAsySdoSeqCon->m_uiUseCount == 0) {
+		// process close in processfunction
+		Ret = EplSdoAsySeqProcess(uiHandle,
+					  0,
+					  NULL, NULL, kAsySdoSeqEventCloseCon);
+
+		//check protocol
+		if ((pAsySdoSeqCon->m_ConHandle & EPL_SDO_ASY_HANDLE_MASK) ==
+		    EPL_SDO_UDP_HANDLE) {
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+			// call close function of lower layer
+			EplSdoUdpuDelCon(pAsySdoSeqCon->m_ConHandle);
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+		} else {
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+			// call close function of lower layer
+			EplSdoAsnduDelCon(pAsySdoSeqCon->m_ConHandle);
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+		}
+
+		// delete timer
+		EplTimeruDeleteTimer(&pAsySdoSeqCon->m_EplTimerHdl);
+
+		// clean controllstructure
+		EPL_MEMSET(pAsySdoSeqCon, 0x00, sizeof(tEplAsySdoSeqCon));
+		pAsySdoSeqCon->m_SdoConHistory.m_bFreeEntries =
+		    EPL_SDO_HISTORY_SIZE;
+	}
+
+      Exit:
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEplSdoAsySeqProcess
+//
+// Description: intern function to process the asynchronus SDO Sequence Layer
+//              state maschine
+//
+//
+//
+// Parameters:  uiHandle_p      = index of the control structure of the connection
+//              uiDataSize_p    = size of data frame to process (can be 0)
+//                                  -> without size of sequence header and Asnd header!!!
+//
+//              pData_p         = pointer to frame to send (can be NULL)
+//              pRecFrame_p     = pointer to received frame (can be NULL)
+//              Event_p         = Event to process
+//
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsySeqProcess(unsigned int uiHandle_p,
+				      unsigned int uiDataSize_p,
+				      tEplFrame * pData_p,
+				      tEplAsySdoSeq * pRecFrame_p,
+				      tEplAsySdoSeqEvent Event_p)
+{
+	tEplKernel Ret;
+	unsigned int uiFrameSize;
+	tEplFrame *pEplFrame;
+	tEplAsySdoSeqCon *pAsySdoSeqCon;
+	tEplSdoSeqConHdl SdoSeqConHdl;
+	unsigned int uiFreeEntries;
+
+#if defined(WIN32) || defined(_WIN32)
+	// enter  critical section for process function
+	EnterCriticalSection(AsySdoSequInstance_g.m_pCriticalSection);
+#endif
+
+	Ret = kEplSuccessful;
+
+	// get handle for hinger layer
+	SdoSeqConHdl = uiHandle_p | EPL_SDO_ASY_HANDLE;
+
+	// check if handle invalid
+	if ((SdoSeqConHdl & ~EPL_SDO_SEQ_HANDLE_MASK) ==
+	    EPL_SDO_SEQ_INVALID_HDL) {
+		Ret = kEplSdoSeqInvalidHdl;
+		goto Exit;
+	}
+	// get pointer to connection
+	pAsySdoSeqCon = &AsySdoSequInstance_g.m_AsySdoConnection[uiHandle_p];
+
+	// check size
+	if ((pData_p == NULL) && (pRecFrame_p == NULL) && (uiDataSize_p != 0)) {
+		Ret = kEplSdoSeqInvalidFrame;
+		goto Exit;
+	}
+	// check state
+	switch (pAsySdoSeqCon->m_SdoState) {
+		// idle state
+	case kEplAsySdoStateIdle:
+		{
+			// check event
+			switch (Event_p) {
+				// new connection
+				// -> send init frame and change to
+				// kEplAsySdoStateInit1
+			case kAsySdoSeqEventInitCon:
+				{
+					// set sending scon to 1
+					pAsySdoSeqCon->m_bRecSeqNum = 0x01;
+					// set set send rcon to 0
+					pAsySdoSeqCon->m_bSendSeqNum = 0x00;
+					Ret =
+					    EplSdoAsySeqSendIntern
+					    (pAsySdoSeqCon, 0, NULL, FALSE);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+					// change state
+					pAsySdoSeqCon->m_SdoState =
+					    kEplAsySdoStateInit1;
+
+					// set timer
+					Ret =
+					    EplSdoAsySeqSetTimer(pAsySdoSeqCon,
+								 EPL_SEQ_DEFAULT_TIMEOUT);
+
+					break;
+				}
+
+				// init con from extern
+				// check rcon and scon
+				// -> send answer
+			case kAsySdoSeqEventFrameRec:
+				{
+/*
+                    PRINTF3("%s scon=%u rcon=%u\n",
+                            __func__,
+                            pRecFrame_p->m_le_bSendSeqNumCon,
+                            pRecFrame_p->m_le_bRecSeqNumCon);
+*/
+					// check if scon == 1 and rcon == 0
+					if (((pRecFrame_p->
+					      m_le_bRecSeqNumCon &
+					      EPL_ASY_SDO_CON_MASK) == 0x00)
+					    &&
+					    ((pRecFrame_p->
+					      m_le_bSendSeqNumCon &
+					      EPL_ASY_SDO_CON_MASK) == 0x01)) {
+						// save sequence numbers
+						pAsySdoSeqCon->m_bRecSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bRecSeqNumCon);
+						pAsySdoSeqCon->m_bSendSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bSendSeqNumCon);
+						// create answer and send answer
+						// set rcon to 1 (in send direction own scon)
+						pAsySdoSeqCon->m_bRecSeqNum++;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						// change state to kEplAsySdoStateInit2
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateInit2;
+
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+					} else {	// error -> close
+						// delete timer
+						EplTimeruDeleteTimer
+						    (&pAsySdoSeqCon->
+						     m_EplTimerHdl);
+						if (((pRecFrame_p->
+						      m_le_bRecSeqNumCon &
+						      EPL_ASY_SDO_CON_MASK) !=
+						     0x00)
+						    || ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) != 0x00)) {	// d.k. only answer with close message if the message sent was not a close message
+							// save sequence numbers
+							pAsySdoSeqCon->
+							    m_bRecSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bRecSeqNumCon);
+							pAsySdoSeqCon->
+							    m_bSendSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bSendSeqNumCon);
+							// set rcon and scon to 0
+							pAsySdoSeqCon->
+							    m_bSendSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							pAsySdoSeqCon->
+							    m_bRecSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							// send frame
+							EplSdoAsySeqSendIntern
+							    (pAsySdoSeqCon, 0,
+							     NULL, FALSE);
+						}
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateInitError);
+					}
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of switch(Event_p)
+			break;
+		}
+
+		// init connection step 1
+		// wait for frame with scon = 1
+		// and rcon = 1
+	case kEplAsySdoStateInit1:
+		{
+//            PRINTF0("EplSdoAsySequ: StateInit1\n");
+
+			// check event
+			switch (Event_p) {
+				// frame received
+			case kAsySdoSeqEventFrameRec:
+				{
+					// check scon == 1 and rcon == 1
+					if (((pRecFrame_p->
+					      m_le_bRecSeqNumCon &
+					      EPL_ASY_SDO_CON_MASK) == 0x01)
+					    && ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) == 0x01)) {	// create answer own scon = 2
+						// save sequence numbers
+						pAsySdoSeqCon->m_bRecSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bRecSeqNumCon);
+						pAsySdoSeqCon->m_bSendSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bSendSeqNumCon);
+
+						pAsySdoSeqCon->m_bRecSeqNum++;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						// change state to kEplAsySdoStateInit3
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateInit3;
+
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+
+					}
+					// check if scon == 1 and rcon == 0, i.e. other side wants me to be server
+					else if (((pRecFrame_p->
+						   m_le_bRecSeqNumCon &
+						   EPL_ASY_SDO_CON_MASK) ==
+						  0x00)
+						 &&
+						 ((pRecFrame_p->
+						   m_le_bSendSeqNumCon &
+						   EPL_ASY_SDO_CON_MASK) ==
+						  0x01)) {
+						// save sequence numbers
+						pAsySdoSeqCon->m_bRecSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bRecSeqNumCon);
+						pAsySdoSeqCon->m_bSendSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bSendSeqNumCon);
+						// create answer and send answer
+						// set rcon to 1 (in send direction own scon)
+						pAsySdoSeqCon->m_bRecSeqNum++;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						// change state to kEplAsySdoStateInit2
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateInit2;
+
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+					} else {	// error -> Close
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateIdle;
+						// delete timer
+						EplTimeruDeleteTimer
+						    (&pAsySdoSeqCon->
+						     m_EplTimerHdl);
+						if (((pRecFrame_p->
+						      m_le_bRecSeqNumCon &
+						      EPL_ASY_SDO_CON_MASK) !=
+						     0x00)
+						    || ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) != 0x00)) {	// d.k. only answer with close message if the message sent was not a close message
+							// save sequence numbers
+							pAsySdoSeqCon->
+							    m_bRecSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bRecSeqNumCon);
+							pAsySdoSeqCon->
+							    m_bSendSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bSendSeqNumCon);
+
+							// set rcon and scon to 0
+							pAsySdoSeqCon->
+							    m_bSendSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							pAsySdoSeqCon->
+							    m_bRecSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							// send frame
+							EplSdoAsySeqSendIntern
+							    (pAsySdoSeqCon, 0,
+							     NULL, FALSE);
+						}
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateInitError);
+					}
+					break;
+				}
+
+				// timeout
+			case kAsySdoSeqEventTimeout:
+				{	// error -> Close
+					pAsySdoSeqCon->m_SdoState =
+					    kEplAsySdoStateIdle;
+
+					// set rcon and scon to 0
+					pAsySdoSeqCon->m_bSendSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					pAsySdoSeqCon->m_bRecSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					// send frame
+					EplSdoAsySeqSendIntern(pAsySdoSeqCon,
+							       0, NULL, FALSE);
+					// call Command Layer Cb
+					AsySdoSequInstance_g.
+					    m_fpSdoComConCb(SdoSeqConHdl,
+							    kAsySdoConStateInitError);
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of switch(Event_p)
+			break;
+		}
+
+		// init connection step 2
+	case kEplAsySdoStateInit2:
+		{
+//            PRINTF0("EplSdoAsySequ: StateInit2\n");
+
+			// check event
+			switch (Event_p) {
+				// frame received
+			case kAsySdoSeqEventFrameRec:
+				{
+					// check scon == 2 and rcon == 1
+					if (((pRecFrame_p->
+					      m_le_bRecSeqNumCon &
+					      EPL_ASY_SDO_CON_MASK) == 0x01)
+					    && ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) == 0x02)) {	// create answer own rcon = 2
+						// save sequence numbers
+						pAsySdoSeqCon->m_bRecSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bRecSeqNumCon);
+						pAsySdoSeqCon->m_bSendSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bSendSeqNumCon);
+
+						pAsySdoSeqCon->m_bRecSeqNum++;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						// change state to kEplAsySdoStateConnected
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateConnected;
+
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateConnected);
+
+					}
+					// check scon == 1 and rcon == 1, i.e. other side wants me to initiate the connection
+					else if (((pRecFrame_p->
+						   m_le_bRecSeqNumCon &
+						   EPL_ASY_SDO_CON_MASK) ==
+						  0x01)
+						 &&
+						 ((pRecFrame_p->
+						   m_le_bSendSeqNumCon &
+						   EPL_ASY_SDO_CON_MASK) ==
+						  0x01)) {
+						// save sequence numbers
+						pAsySdoSeqCon->m_bRecSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bRecSeqNumCon);
+						pAsySdoSeqCon->m_bSendSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bSendSeqNumCon);
+						// create answer and send answer
+						// set rcon to 1 (in send direction own scon)
+						pAsySdoSeqCon->m_bRecSeqNum++;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+						// change state to kEplAsySdoStateInit3
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateInit3;
+
+					} else {	// error -> Close
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateIdle;
+						// delete timer
+						EplTimeruDeleteTimer
+						    (&pAsySdoSeqCon->
+						     m_EplTimerHdl);
+						if (((pRecFrame_p->
+						      m_le_bRecSeqNumCon &
+						      EPL_ASY_SDO_CON_MASK) !=
+						     0x00)
+						    || ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) != 0x00)) {	// d.k. only answer with close message if the message sent was not a close message
+							// save sequence numbers
+							pAsySdoSeqCon->
+							    m_bRecSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bRecSeqNumCon);
+							pAsySdoSeqCon->
+							    m_bSendSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bSendSeqNumCon);
+							// set rcon and scon to 0
+							pAsySdoSeqCon->
+							    m_bSendSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							pAsySdoSeqCon->
+							    m_bRecSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							// send frame
+							EplSdoAsySeqSendIntern
+							    (pAsySdoSeqCon, 0,
+							     NULL, FALSE);
+						}
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateInitError);
+					}
+					break;
+				}
+
+				// timeout
+			case kAsySdoSeqEventTimeout:
+				{	// error -> Close
+					pAsySdoSeqCon->m_SdoState =
+					    kEplAsySdoStateIdle;
+					// set rcon and scon to 0
+					pAsySdoSeqCon->m_bSendSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					pAsySdoSeqCon->m_bRecSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					// send frame
+					EplSdoAsySeqSendIntern(pAsySdoSeqCon,
+							       0, NULL, FALSE);
+
+					// call Command Layer Cb
+					AsySdoSequInstance_g.
+					    m_fpSdoComConCb(SdoSeqConHdl,
+							    kAsySdoConStateInitError);
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of switch(Event_p)
+			break;
+		}
+
+		// init connection step 3
+	case kEplAsySdoStateInit3:
+		{
+			// check event
+			switch (Event_p) {
+				// frame received
+			case kAsySdoSeqEventFrameRec:
+				{
+					// check scon == 2 and rcon == 2
+					if (((pRecFrame_p->
+					      m_le_bRecSeqNumCon &
+					      EPL_ASY_SDO_CON_MASK) == 0x02)
+					    &&
+					    ((pRecFrame_p->
+					      m_le_bSendSeqNumCon &
+					      EPL_ASY_SDO_CON_MASK) == 0x02)) {
+						// save sequence numbers
+						pAsySdoSeqCon->m_bRecSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bRecSeqNumCon);
+						pAsySdoSeqCon->m_bSendSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bSendSeqNumCon);
+						// change state to kEplAsySdoStateConnected
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateConnected;
+
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateConnected);
+
+					}
+					// check scon == 2 and rcon == 1
+					else if (((pRecFrame_p->
+						   m_le_bRecSeqNumCon &
+						   EPL_ASY_SDO_CON_MASK) ==
+						  0x01)
+						 && ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) == 0x02)) {	// create answer own rcon = 2
+						// save sequence numbers
+						pAsySdoSeqCon->m_bRecSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bRecSeqNumCon);
+						pAsySdoSeqCon->m_bSendSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bSendSeqNumCon);
+
+						pAsySdoSeqCon->m_bRecSeqNum++;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						// change state to kEplAsySdoStateConnected
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateConnected;
+
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateConnected);
+
+					} else {	// error -> Close
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateIdle;
+						// delete timer
+						EplTimeruDeleteTimer
+						    (&pAsySdoSeqCon->
+						     m_EplTimerHdl);
+						if (((pRecFrame_p->
+						      m_le_bRecSeqNumCon &
+						      EPL_ASY_SDO_CON_MASK) !=
+						     0x00)
+						    || ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) != 0x00)) {	// d.k. only answer with close message if the message sent was not a close message
+							// save sequence numbers
+							pAsySdoSeqCon->
+							    m_bRecSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bRecSeqNumCon);
+							pAsySdoSeqCon->
+							    m_bSendSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bSendSeqNumCon);
+							// set rcon and scon to 0
+							pAsySdoSeqCon->
+							    m_bSendSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							pAsySdoSeqCon->
+							    m_bRecSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							// send frame
+							EplSdoAsySeqSendIntern
+							    (pAsySdoSeqCon, 0,
+							     NULL, FALSE);
+						}
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateInitError);
+					}
+					break;
+				}
+
+				// timeout
+			case kAsySdoSeqEventTimeout:
+				{	// error -> Close
+					pAsySdoSeqCon->m_SdoState =
+					    kEplAsySdoStateIdle;
+					// set rcon and scon to 0
+					pAsySdoSeqCon->m_bSendSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					pAsySdoSeqCon->m_bRecSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					// send frame
+					EplSdoAsySeqSendIntern(pAsySdoSeqCon,
+							       0, NULL, FALSE);
+
+					// call Command Layer Cb
+					AsySdoSequInstance_g.
+					    m_fpSdoComConCb(SdoSeqConHdl,
+							    kAsySdoConStateInitError);
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of switch(Event_p)
+			break;
+		}
+
+		// connection established
+	case kEplAsySdoStateConnected:
+		{
+			// check event
+			switch (Event_p) {
+
+				// frame to send
+			case kAsySdoSeqEventFrameSend:
+				{
+					// set timer
+					Ret =
+					    EplSdoAsySeqSetTimer(pAsySdoSeqCon,
+								 EPL_SEQ_DEFAULT_TIMEOUT);
+					// check if data frame or ack
+					if (pData_p == NULL) {	// send ack
+						// inc scon
+						//pAsySdoSeqCon->m_bRecSeqNum += 4;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+					} else {	// send dataframe
+						// increment send sequence number
+						pAsySdoSeqCon->m_bRecSeqNum +=
+						    4;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon,
+						     uiDataSize_p, pData_p,
+						     TRUE);
+						if (Ret == kEplSdoSeqRequestAckNeeded) {	// request ack
+							// change state to wait ack
+							pAsySdoSeqCon->
+							    m_SdoState =
+							    kEplAsySdoStateWaitAck;
+							// set Ret to kEplSuccessful, because no error
+							// for higher layer
+							Ret = kEplSuccessful;
+
+						} else if (Ret !=
+							   kEplSuccessful) {
+							goto Exit;
+						} else {
+							// call Command Layer Cb
+							AsySdoSequInstance_g.
+							    m_fpSdoComConCb
+							    (SdoSeqConHdl,
+							     kAsySdoConStateFrameSended);
+						}
+					}
+					break;
+				}	// end of case kAsySdoSeqEventFrameSend
+
+				// frame received
+			case kAsySdoSeqEventFrameRec:
+				{
+					BYTE bSendSeqNumCon =
+					    AmiGetByteFromLe(&pRecFrame_p->
+							     m_le_bSendSeqNumCon);
+
+					// set timer
+					Ret =
+					    EplSdoAsySeqSetTimer(pAsySdoSeqCon,
+								 EPL_SEQ_DEFAULT_TIMEOUT);
+					// check scon
+					switch (bSendSeqNumCon &
+						EPL_ASY_SDO_CON_MASK) {
+						// close from other node
+					case 0:
+					case 1:
+						{
+							// return to idle
+							pAsySdoSeqCon->
+							    m_SdoState =
+							    kEplAsySdoStateIdle;
+							// delete timer
+							EplTimeruDeleteTimer
+							    (&pAsySdoSeqCon->
+							     m_EplTimerHdl);
+							// call Command Layer Cb
+							AsySdoSequInstance_g.
+							    m_fpSdoComConCb
+							    (SdoSeqConHdl,
+							     kAsySdoConStateConClosed);
+
+							break;
+						}
+
+						// Request Ack or Error Ack
+						// possible contain data
+					case 3:
+						// normal frame
+					case 2:
+						{
+							if ((AmiGetByteFromLe
+							     (&pRecFrame_p->
+							      m_le_bRecSeqNumCon)
+							     &
+							     EPL_ASY_SDO_CON_MASK)
+							    == 3) {
+//                                PRINTF0("EplSdoAsySequ: error response received\n");
+
+								// error response (retransmission request)
+								// resend frames from history
+
+								// read frame from history
+								Ret =
+								    EplSdoAsyReadFromHistory
+								    (pAsySdoSeqCon,
+								     &pEplFrame,
+								     &uiFrameSize,
+								     TRUE);
+								if (Ret !=
+								    kEplSuccessful)
+								{
+									goto Exit;
+								}
+
+								while ((pEplFrame != NULL)
+								       &&
+								       (uiFrameSize
+									!= 0)) {
+									// send frame
+									Ret =
+									    EplSdoAsySeqSendLowerLayer
+									    (pAsySdoSeqCon,
+									     uiFrameSize,
+									     pEplFrame);
+									if (Ret
+									    !=
+									    kEplSuccessful)
+									{
+										goto Exit;
+									}
+									// read next frame from history
+									Ret =
+									    EplSdoAsyReadFromHistory
+									    (pAsySdoSeqCon,
+									     &pEplFrame,
+									     &uiFrameSize,
+									     FALSE);
+									if (Ret
+									    !=
+									    kEplSuccessful)
+									{
+										goto Exit;
+									}
+								}	// end of while((pabFrame != NULL)
+							}	// end of if (error response)
+
+							if (((pAsySdoSeqCon->m_bSendSeqNum + 4) & EPL_SEQ_NUM_MASK) == (bSendSeqNumCon & EPL_SEQ_NUM_MASK)) {	// next frame of sequence received
+								// save send sequence number (without ack request)
+								pAsySdoSeqCon->
+								    m_bSendSeqNum
+								    =
+								    bSendSeqNumCon
+								    & ~0x01;
+
+								// check if ack or data-frame
+								//ignore ack -> already processed
+								if (uiDataSize_p
+								    >
+								    EPL_SEQ_HEADER_SIZE)
+								{
+									AsySdoSequInstance_g.
+									    m_fpSdoComReceiveCb
+									    (SdoSeqConHdl,
+									     ((tEplAsySdoCom *) & pRecFrame_p->m_le_abSdoSeqPayload), (uiDataSize_p - EPL_SEQ_HEADER_SIZE));
+									// call Command Layer Cb
+									AsySdoSequInstance_g.
+									    m_fpSdoComConCb
+									    (SdoSeqConHdl,
+									     kAsySdoConStateFrameSended);
+
+								} else {
+									// call Command Layer Cb
+									AsySdoSequInstance_g.
+									    m_fpSdoComConCb
+									    (SdoSeqConHdl,
+									     kAsySdoConStateAckReceived);
+								}
+							} else if (((bSendSeqNumCon - pAsySdoSeqCon->m_bSendSeqNum - 4) & EPL_SEQ_NUM_MASK) < EPL_SEQ_NUM_THRESHOLD) {	// frame of sequence was lost,
+								// because difference of received and old value
+								// is less then halve of the values range.
+
+								// send error frame with own rcon = 3
+								pAsySdoSeqCon->
+								    m_bSendSeqNum
+								    |= 0x03;
+								Ret =
+								    EplSdoAsySeqSendIntern
+								    (pAsySdoSeqCon,
+								     0, NULL,
+								     FALSE);
+								// restore send sequence number
+								pAsySdoSeqCon->
+								    m_bSendSeqNum
+								    =
+								    (pAsySdoSeqCon->
+								     m_bSendSeqNum
+								     &
+								     EPL_SEQ_NUM_MASK)
+								    | 0x02;
+								if (Ret !=
+								    kEplSuccessful)
+								{
+									goto Exit;
+								}
+								// break here, because a requested acknowledge
+								// was sent implicitly above
+								break;
+							}
+							// else, ignore repeated frame
+
+							if ((bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) == 3) {	// ack request received
+
+								// create ack with own scon = 2
+								Ret =
+								    EplSdoAsySeqSendIntern
+								    (pAsySdoSeqCon,
+								     0, NULL,
+								     FALSE);
+								if (Ret !=
+								    kEplSuccessful)
+								{
+									goto Exit;
+								}
+							}
+
+							break;
+						}
+
+					}	// switch(pAsySdoSeqCon->m_bSendSeqNum & EPL_ASY_SDO_CON_MASK)
+					break;
+				}	// end of case kAsySdoSeqEventFrameRec:
+
+				//close event from higher layer
+			case kAsySdoSeqEventCloseCon:
+				{
+					pAsySdoSeqCon->m_SdoState =
+					    kEplAsySdoStateIdle;
+					// set rcon and scon to 0
+					pAsySdoSeqCon->m_bSendSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					pAsySdoSeqCon->m_bRecSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					// send frame
+					EplSdoAsySeqSendIntern(pAsySdoSeqCon,
+							       0, NULL, FALSE);
+
+					// delete timer
+					EplTimeruDeleteTimer(&pAsySdoSeqCon->
+							     m_EplTimerHdl);
+					// call Command Layer Cb is not necessary, because the event came from there
+//                    AsySdoSequInstance_g.m_fpSdoComConCb(SdoSeqConHdl,
+//                                                            kAsySdoConStateInitError);
+					break;
+				}
+
+				// timeout
+			case kAsySdoSeqEventTimeout:
+				{
+
+					uiFreeEntries =
+					    EplSdoAsyGetFreeEntriesFromHistory
+					    (pAsySdoSeqCon);
+					if ((uiFreeEntries <
+					     EPL_SDO_HISTORY_SIZE)
+					    && (pAsySdoSeqCon->m_uiRetryCount < EPL_SEQ_RETRY_COUNT)) {	// unacknowlegded frames in history
+						// and retry counter not exceeded
+
+						// resend data with acknowledge request
+
+						// increment retry counter
+						pAsySdoSeqCon->m_uiRetryCount++;
+
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+
+						// read first frame from history
+						Ret =
+						    EplSdoAsyReadFromHistory
+						    (pAsySdoSeqCon, &pEplFrame,
+						     &uiFrameSize, TRUE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+
+						if ((pEplFrame != NULL)
+						    && (uiFrameSize != 0)) {
+
+							// set ack request in scon
+							AmiSetByteToLe
+							    (&pEplFrame->m_Data.
+							     m_Asnd.m_Payload.
+							     m_SdoSequenceFrame.
+							     m_le_bSendSeqNumCon,
+							     AmiGetByteFromLe
+							     (&pEplFrame->
+							      m_Data.m_Asnd.
+							      m_Payload.
+							      m_SdoSequenceFrame.
+							      m_le_bSendSeqNumCon)
+							     | 0x03);
+
+							// send frame
+							Ret =
+							    EplSdoAsySeqSendLowerLayer
+							    (pAsySdoSeqCon,
+							     uiFrameSize,
+							     pEplFrame);
+							if (Ret !=
+							    kEplSuccessful) {
+								goto Exit;
+							}
+
+						}
+					} else {
+						// timeout, because of no traffic -> Close
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateIdle;
+						// set rcon and scon to 0
+						pAsySdoSeqCon->m_bSendSeqNum &=
+						    EPL_SEQ_NUM_MASK;
+						pAsySdoSeqCon->m_bRecSeqNum &=
+						    EPL_SEQ_NUM_MASK;
+						// send frame
+						EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateTimeout);
+					}
+
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of switch(Event_p)
+			break;
+		}
+
+		// wait for Acknowledge (history buffer full)
+	case kEplAsySdoStateWaitAck:
+		{
+			PRINTF0("EplSdoAsySequ: StateWaitAck\n");
+
+			// set timer
+			Ret = EplSdoAsySeqSetTimer(pAsySdoSeqCon,
+						   EPL_SEQ_DEFAULT_TIMEOUT);
+
+			//TODO: retry of acknowledge
+			if (Event_p == kAsySdoSeqEventFrameRec) {
+				// check rcon
+				switch (pRecFrame_p->
+					m_le_bRecSeqNumCon &
+					EPL_ASY_SDO_CON_MASK) {
+					// close-frome other node
+				case 0:
+					{
+						// return to idle
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateIdle;
+						// delete timer
+						EplTimeruDeleteTimer
+						    (&pAsySdoSeqCon->
+						     m_EplTimerHdl);
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateConClosed);
+
+						break;
+					}
+
+					// normal frame
+				case 2:
+					{
+						// should be ack
+						// -> change to state kEplAsySdoStateConnected
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateConnected;
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateAckReceived);
+						// send data to higher layer if needed
+						if (uiDataSize_p >
+						    EPL_SEQ_HEADER_SIZE) {
+							AsySdoSequInstance_g.
+							    m_fpSdoComReceiveCb
+							    (SdoSeqConHdl,
+							     ((tEplAsySdoCom *)
+							      & pRecFrame_p->
+							      m_le_abSdoSeqPayload),
+							     (uiDataSize_p -
+							      EPL_SEQ_HEADER_SIZE));
+						}
+						break;
+					}
+
+					// Request Ack or Error Ack
+				case 3:
+					{
+						// -> change to state kEplAsySdoStateConnected
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateConnected;
+
+						if (pRecFrame_p->m_le_bRecSeqNumCon == pAsySdoSeqCon->m_bRecSeqNum) {	// ack request
+							// -> send ack
+							// save sequence numbers
+							pAsySdoSeqCon->
+							    m_bRecSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bRecSeqNumCon);
+							pAsySdoSeqCon->
+							    m_bSendSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bSendSeqNumCon);
+
+							// create answer own rcon = 2
+							pAsySdoSeqCon->
+							    m_bRecSeqNum--;
+
+							// check if ack or data-frame
+							if (uiDataSize_p >
+							    EPL_SEQ_HEADER_SIZE)
+							{
+								AsySdoSequInstance_g.
+								    m_fpSdoComReceiveCb
+								    (SdoSeqConHdl,
+								     ((tEplAsySdoCom *) & pRecFrame_p->m_le_abSdoSeqPayload), (uiDataSize_p - EPL_SEQ_HEADER_SIZE));
+								// call Command Layer Cb
+								AsySdoSequInstance_g.
+								    m_fpSdoComConCb
+								    (SdoSeqConHdl,
+								     kAsySdoConStateFrameSended);
+
+							} else {
+								Ret =
+								    EplSdoAsySeqSendIntern
+								    (pAsySdoSeqCon,
+								     0, NULL,
+								     FALSE);
+								if (Ret !=
+								    kEplSuccessful)
+								{
+									goto Exit;
+								}
+							}
+
+						} else {
+							// error ack
+							// resend frames from history
+
+							// read frame from history
+							Ret =
+							    EplSdoAsyReadFromHistory
+							    (pAsySdoSeqCon,
+							     &pEplFrame,
+							     &uiFrameSize,
+							     TRUE);
+							while ((pEplFrame !=
+								NULL)
+							       && (uiFrameSize
+								   != 0)) {
+								// send frame
+								Ret =
+								    EplSdoAsySeqSendLowerLayer
+								    (pAsySdoSeqCon,
+								     uiFrameSize,
+								     pEplFrame);
+								if (Ret !=
+								    kEplSuccessful)
+								{
+									goto Exit;
+								}
+								// read next frame
+
+								// read frame from history
+								Ret =
+								    EplSdoAsyReadFromHistory
+								    (pAsySdoSeqCon,
+								     &pEplFrame,
+								     &uiFrameSize,
+								     FALSE);
+							}	// end of while((pabFrame != NULL)
+						}
+						break;
+					}
+				}	// end of switch(pRecFrame_p->m_le_bRecSeqNumCon & EPL_ASY_SDO_CON_MASK)
+
+			} else if (Event_p == kAsySdoSeqEventTimeout) {	// error -> Close
+				pAsySdoSeqCon->m_SdoState = kEplAsySdoStateIdle;
+				// set rcon and scon to 0
+				pAsySdoSeqCon->m_bSendSeqNum &=
+				    EPL_SEQ_NUM_MASK;
+				pAsySdoSeqCon->m_bRecSeqNum &= EPL_SEQ_NUM_MASK;
+				// send frame
+				EplSdoAsySeqSendIntern(pAsySdoSeqCon,
+						       0, NULL, FALSE);
+
+				// call Command Layer Cb
+				AsySdoSequInstance_g.
+				    m_fpSdoComConCb(SdoSeqConHdl,
+						    kAsySdoConStateTimeout);
+			}
+
+			break;
+		}
+
+		// unknown state
+	default:
+		{
+			EPL_DBGLVL_SDO_TRACE0
+			    ("Error: Unknown State in EplSdoAsySeqProcess\n");
+
+		}
+	}			// end of switch(pAsySdoSeqCon->m_SdoState)
+
+      Exit:
+
+#if defined(WIN32) || defined(_WIN32)
+	// leave critical section for process function
+	LeaveCriticalSection(AsySdoSequInstance_g.m_pCriticalSection);
+#endif
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqSendIntern
+//
+// Description: intern function to create and send a frame
+//              -> if uiDataSize_p == 0 create a frame with infos from
+//                 pAsySdoSeqCon_p
+//
+//
+//
+// Parameters:  pAsySdoSeqCon_p = pointer to control structure of the connection
+//              uiDataSize_p    = size of data frame to process (can be 0)
+//                                  -> without size of sequence header and Asnd header!!!
+//              pData_p         = pointer to frame to process (can be NULL)
+//              fFrameInHistory = if TRUE frame is saved to history else not
+//
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsySeqSendIntern(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					 unsigned int uiDataSize_p,
+					 tEplFrame * pData_p,
+					 BOOL fFrameInHistory_p)
+{
+	tEplKernel Ret;
+	BYTE abFrame[EPL_SEQ_FRAME_SIZE];
+	tEplFrame *pEplFrame;
+	unsigned int uiFreeEntries;
+
+	if (pData_p == NULL) {	// set pointer to own frame
+		EPL_MEMSET(&abFrame[0], 0x00, sizeof(abFrame));
+		pEplFrame = (tEplFrame *) & abFrame[0];
+	} else {		// set pointer to frame from calling function
+		pEplFrame = pData_p;
+	}
+
+	if (fFrameInHistory_p != FALSE) {
+		// check if only one free entry in history buffer
+		uiFreeEntries =
+		    EplSdoAsyGetFreeEntriesFromHistory(pAsySdoSeqCon_p);
+		if (uiFreeEntries == 1) {	// request an acknowledge in dataframe
+			// own scon = 3
+			pAsySdoSeqCon_p->m_bRecSeqNum |= 0x03;
+		}
+	}
+	// fillin header informations
+	// set service id sdo
+	AmiSetByteToLe(&pEplFrame->m_Data.m_Asnd.m_le_bServiceId, 0x05);
+	AmiSetByteToLe(&pEplFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame.
+		       m_le_abReserved, 0x00);
+	// set receive sequence number and rcon
+	AmiSetByteToLe(&pEplFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame.
+		       m_le_bRecSeqNumCon, pAsySdoSeqCon_p->m_bSendSeqNum);
+	// set send sequence number and scon
+	AmiSetByteToLe(&pEplFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame.
+		       m_le_bSendSeqNumCon, pAsySdoSeqCon_p->m_bRecSeqNum);
+
+	// add size
+	uiDataSize_p += EPL_SEQ_HEADER_SIZE;
+
+	// forward frame to appropriate lower layer
+	Ret = EplSdoAsySeqSendLowerLayer(pAsySdoSeqCon_p, uiDataSize_p, pEplFrame);	// pointer to frame
+
+	// check if all allright
+	if ((Ret == kEplSuccessful)
+	    && (fFrameInHistory_p != FALSE)) {
+		// set own scon to 2 if needed
+		if ((pAsySdoSeqCon_p->m_bRecSeqNum & 0x03) == 0x03) {
+			pAsySdoSeqCon_p->m_bRecSeqNum--;
+		}
+		// save frame to history
+		Ret = EplSdoAsyAddFrameToHistory(pAsySdoSeqCon_p,
+						 pEplFrame, uiDataSize_p);
+		if (Ret == kEplSdoSeqNoFreeHistory) {	// request Ack needed
+			Ret = kEplSdoSeqRequestAckNeeded;
+		}
+
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqSendLowerLayer
+//
+// Description: intern function to send a previously created frame to lower layer
+//
+// Parameters:  pAsySdoSeqCon_p = pointer to control structure of the connection
+//              uiDataSize_p    = size of data frame to process (can be 0)
+//                                  -> without size of Asnd header!!!
+//              pData_p         = pointer to frame to process (can be NULL)
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsySeqSendLowerLayer(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					     unsigned int uiDataSize_p,
+					     tEplFrame * pEplFrame_p)
+{
+	tEplKernel Ret;
+
+	// call send-function
+	// check handle for UDP or Asnd
+	if ((pAsySdoSeqCon_p->m_ConHandle & EPL_SDO_ASY_HANDLE_MASK) == EPL_SDO_UDP_HANDLE) {	// send over UDP
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+		Ret = EplSdoUdpuSendData(pAsySdoSeqCon_p->m_ConHandle, pEplFrame_p,	// pointer to frame
+					 uiDataSize_p);
+#else
+		Ret = kEplSdoSeqUnsupportedProt;
+#endif
+
+	} else if ((pAsySdoSeqCon_p->m_ConHandle & EPL_SDO_ASY_HANDLE_MASK) == EPL_SDO_ASND_HANDLE) {	// ASND
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+		Ret = EplSdoAsnduSendData(pAsySdoSeqCon_p->m_ConHandle, pEplFrame_p,	// pointer to frame
+					  uiDataSize_p);
+#else
+		Ret = kEplSdoSeqUnsupportedProt;
+#endif
+	} else {		// error
+		Ret = kEplSdoSeqInvalidHdl;
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoAsyReceiveCb
+//
+// Description:     callback-function for received frames from lower layer
+//
+//
+//
+// Parameters:      ConHdl_p        = handle of the connection
+//                  pSdoSeqData_p   = pointer to frame
+//                  uiDataSize_p    = size of frame
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsyReceiveCb(tEplSdoConHdl ConHdl_p,
+				     tEplAsySdoSeq * pSdoSeqData_p,
+				     unsigned int uiDataSize_p)
+{
+	tEplKernel Ret;
+	unsigned int uiCount = 0;
+	unsigned int uiFreeEntry = EPL_MAX_SDO_SEQ_CON;
+	tEplAsySdoSeqCon *pAsySdoSeqCon;
+
+#if defined(WIN32) || defined(_WIN32)
+	// enter  critical section
+	EnterCriticalSection(AsySdoSequInstance_g.m_pCriticalSectionReceive);
+#endif
+
+	EPL_DBGLVL_SDO_TRACE2("Handle: 0x%x , First Databyte 0x%x\n", ConHdl_p,
+			      ((BYTE *) pSdoSeqData_p)[0]);
+
+	// search controll structure for this connection
+	pAsySdoSeqCon = &AsySdoSequInstance_g.m_AsySdoConnection[uiCount];
+	while (uiCount < EPL_MAX_SDO_SEQ_CON) {
+		if (pAsySdoSeqCon->m_ConHandle == ConHdl_p) {
+			break;
+		} else if ((pAsySdoSeqCon->m_ConHandle == 0)
+			   && (uiFreeEntry == EPL_MAX_SDO_SEQ_CON)) {
+			// free entry
+			uiFreeEntry = uiCount;
+		}
+		uiCount++;
+		pAsySdoSeqCon++;
+	}
+
+	if (uiCount == EPL_MAX_SDO_SEQ_CON) {	// new connection
+		if (uiFreeEntry == EPL_MAX_SDO_SEQ_CON) {
+			Ret = kEplSdoSeqNoFreeHandle;
+			goto Exit;
+		} else {
+			pAsySdoSeqCon =
+			    &AsySdoSequInstance_g.
+			    m_AsySdoConnection[uiFreeEntry];
+			// save handle from lower layer
+			pAsySdoSeqCon->m_ConHandle = ConHdl_p;
+			// increment use counter
+			pAsySdoSeqCon->m_uiUseCount++;
+			uiCount = uiFreeEntry;
+		}
+	}
+	// call history ack function
+	Ret = EplSdoAsyAckFrameToHistory(pAsySdoSeqCon,
+					 (AmiGetByteFromLe
+					  (&pSdoSeqData_p->
+					   m_le_bRecSeqNumCon) &
+					  EPL_SEQ_NUM_MASK));
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#if defined(WIN32) || defined(_WIN32)
+	// leave critical section
+	LeaveCriticalSection(AsySdoSequInstance_g.m_pCriticalSectionReceive);
+#endif
+
+	// call process function with pointer of frame and event kAsySdoSeqEventFrameRec
+	Ret = EplSdoAsySeqProcess(uiCount,
+				  uiDataSize_p,
+				  NULL, pSdoSeqData_p, kAsySdoSeqEventFrameRec);
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoAsyInitHistory
+//
+// Description:     inti function for history buffer
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsyInitHistory(void)
+{
+	tEplKernel Ret;
+	unsigned int uiCount;
+
+	Ret = kEplSuccessful;
+	// init m_bFreeEntries in history-buffer
+	for (uiCount = 0; uiCount < EPL_MAX_SDO_SEQ_CON; uiCount++) {
+		AsySdoSequInstance_g.m_AsySdoConnection[uiCount].
+		    m_SdoConHistory.m_bFreeEntries = EPL_SDO_HISTORY_SIZE;
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoAsyAddFrameToHistory
+//
+// Description:     function to add a frame to the history buffer
+//
+//
+//
+// Parameters:      pAsySdoSeqCon_p = pointer to control structure of this connection
+//                  pFrame_p        = pointer to frame
+//                  uiSize_p        = size of the frame
+//                                     -> without size of the ethernet header
+//                                        and the asnd header
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsyAddFrameToHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					     tEplFrame * pFrame_p,
+					     unsigned int uiSize_p)
+{
+	tEplKernel Ret;
+	tEplAsySdoConHistory *pHistory;
+
+	Ret = kEplSuccessful;
+
+	// add frame to history buffer
+
+	// check size
+	// $$$ d.k. EPL_SEQ_HISTORY_FRAME_SIZE includes the header size, but uiSize_p does not!!!
+	if (uiSize_p > EPL_SEQ_HISTROY_FRAME_SIZE) {
+		Ret = kEplSdoSeqFrameSizeError;
+		goto Exit;
+	}
+	// save pointer to history
+	pHistory = &pAsySdoSeqCon_p->m_SdoConHistory;
+
+	// check if a free entry is available
+	if (pHistory->m_bFreeEntries > 0) {	// write message in free entry
+		EPL_MEMCPY(&
+			   ((tEplFrame *) pHistory->
+			    m_aabHistoryFrame[pHistory->m_bWrite])->
+			   m_le_bMessageType, &pFrame_p->m_le_bMessageType,
+			   uiSize_p + EPL_ASND_HEADER_SIZE);
+		// store size
+		pHistory->m_auiFrameSize[pHistory->m_bWrite] = uiSize_p;
+
+		// decremend number of free bufferentries
+		pHistory->m_bFreeEntries--;
+
+		// increment writeindex
+		pHistory->m_bWrite++;
+
+		// check if write-index run over array-boarder
+		if (pHistory->m_bWrite == EPL_SDO_HISTORY_SIZE) {
+			pHistory->m_bWrite = 0;
+		}
+
+	} else {		// no free entry
+		Ret = kEplSdoSeqNoFreeHistory;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoAsyAckFrameToHistory
+//
+// Description:     function to delete acknowledged frames fron history buffer
+//
+//
+//
+// Parameters:      pAsySdoSeqCon_p = pointer to control structure of this connection
+//                  bRecSeqNumber_p = receive sequence number of the received frame
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsyAckFrameToHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					     BYTE bRecSeqNumber_p)
+{
+	tEplKernel Ret;
+	tEplAsySdoConHistory *pHistory;
+	BYTE bAckIndex;
+	BYTE bCurrentSeqNum;
+
+	Ret = kEplSuccessful;
+
+	// get pointer to history buffer
+	pHistory = &pAsySdoSeqCon_p->m_SdoConHistory;
+
+	// release all acknowledged frames from history buffer
+
+	// check if there are entries in history
+	if (pHistory->m_bFreeEntries < EPL_SDO_HISTORY_SIZE) {
+		bAckIndex = pHistory->m_bAck;
+		do {
+			bCurrentSeqNum =
+			    (((tEplFrame *) pHistory->
+			      m_aabHistoryFrame[bAckIndex])->m_Data.m_Asnd.
+			     m_Payload.m_SdoSequenceFrame.
+			     m_le_bSendSeqNumCon & EPL_SEQ_NUM_MASK);
+			if (((bRecSeqNumber_p -
+			      bCurrentSeqNum) & EPL_SEQ_NUM_MASK)
+			    < EPL_SEQ_NUM_THRESHOLD) {
+				pHistory->m_auiFrameSize[bAckIndex] = 0;
+				bAckIndex++;
+				pHistory->m_bFreeEntries++;
+				if (bAckIndex == EPL_SDO_HISTORY_SIZE) {	// read index run over array-boarder
+					bAckIndex = 0;
+				}
+			} else {	// nothing to do anymore,
+				// because any further frame in history has larger sequence
+				// number than the acknowledge
+				goto Exit;
+			}
+		}
+		while ((((bRecSeqNumber_p - 1 -
+			  bCurrentSeqNum) & EPL_SEQ_NUM_MASK)
+			< EPL_SEQ_NUM_THRESHOLD)
+		       && (pHistory->m_bWrite != bAckIndex));
+
+		// store local read-index to global var
+		pHistory->m_bAck = bAckIndex;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoAsyReadFromHistory
+//
+// Description:     function to one frame from history
+//
+//
+//
+// Parameters:      pAsySdoSeqCon_p = pointer to control structure of this connection
+//                  ppFrame_p       = pointer to pointer to the buffer of the stored frame
+//                  puiSize_p       = OUT: size of the frame
+//                  fInitRead       = bool which indicate a start of retransmission
+//                                      -> return last not acknowledged message if TRUE
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsyReadFromHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					   tEplFrame ** ppFrame_p,
+					   unsigned int *puiSize_p,
+					   BOOL fInitRead_p)
+{
+	tEplKernel Ret;
+	tEplAsySdoConHistory *pHistory;
+
+	Ret = kEplSuccessful;
+
+	// read one message from History
+
+	// get pointer to history buffer
+	pHistory = &pAsySdoSeqCon_p->m_SdoConHistory;
+
+	// check if init
+	if (fInitRead_p != FALSE) {	// initialize read index to the index which shall be acknowledged next
+		pHistory->m_bRead = pHistory->m_bAck;
+	}
+	// check if entries are available for reading
+	if ((pHistory->m_bFreeEntries < EPL_SDO_HISTORY_SIZE)
+	    && (pHistory->m_bWrite != pHistory->m_bRead)) {
+//        PRINTF4("EplSdoAsyReadFromHistory(): init = %d, read = %u, write = %u, ack = %u", (int) fInitRead_p, (WORD)pHistory->m_bRead, (WORD)pHistory->m_bWrite, (WORD)pHistory->m_bAck);
+//        PRINTF2(", free entries = %u, next frame size = %u\n", (WORD)pHistory->m_bFreeEntries, pHistory->m_auiFrameSize[pHistory->m_bRead]);
+
+		// return pointer to stored frame
+		*ppFrame_p =
+		    (tEplFrame *) pHistory->m_aabHistoryFrame[pHistory->
+							      m_bRead];
+
+		// save size
+		*puiSize_p = pHistory->m_auiFrameSize[pHistory->m_bRead];
+
+		pHistory->m_bRead++;
+		if (pHistory->m_bRead == EPL_SDO_HISTORY_SIZE) {
+			pHistory->m_bRead = 0;
+		}
+
+	} else {
+//        PRINTF3("EplSdoAsyReadFromHistory(): read = %u, ack = %u, free entries = %u, no frame\n", (WORD)pHistory->m_bRead, (WORD)pHistory->m_bAck, (WORD)pHistory->m_bFreeEntries);
+
+		// no more frames to send
+		// return null pointer
+		*ppFrame_p = NULL;
+
+		*puiSize_p = 0;
+	}
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoAsyGetFreeEntriesFromHistory
+//
+// Description:     function returns the number of free histroy entries
+//
+//
+//
+// Parameters:      pAsySdoSeqCon_p = pointer to control structure of this connection
+//
+//
+// Returns:         unsigned int    = number of free entries
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static unsigned int EplSdoAsyGetFreeEntriesFromHistory(tEplAsySdoSeqCon *
+						       pAsySdoSeqCon_p)
+{
+	unsigned int uiFreeEntries;
+
+	uiFreeEntries =
+	    (unsigned int)pAsySdoSeqCon_p->m_SdoConHistory.m_bFreeEntries;
+
+	return uiFreeEntries;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoAsySeqSetTimer
+//
+// Description:     function sets or modify timer in timermosule
+//
+//
+//
+// Parameters:      pAsySdoSeqCon_p = pointer to control structure of this connection
+//                  ulTimeout       = timeout in ms
+//
+//
+// Returns:         unsigned int    = number of free entries
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsySeqSetTimer(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+				       unsigned long ulTimeout)
+{
+	tEplKernel Ret;
+	tEplTimerArg TimerArg;
+
+	TimerArg.m_EventSink = kEplEventSinkSdoAsySeq;
+	TimerArg.m_ulArg = (unsigned long)pAsySdoSeqCon_p;
+
+	if (pAsySdoSeqCon_p->m_EplTimerHdl == 0) {	// create new timer
+		Ret = EplTimeruSetTimerMs(&pAsySdoSeqCon_p->m_EplTimerHdl,
+					  ulTimeout, TimerArg);
+	} else {		// modify exisiting timer
+		Ret = EplTimeruModifyTimerMs(&pAsySdoSeqCon_p->m_EplTimerHdl,
+					     ulTimeout, TimerArg);
+
+	}
+
+	return Ret;
+}
+
+// EOF
diff --git a/drivers/staging/epl/EplSdoComu.c b/drivers/staging/epl/EplSdoComu.c
new file mode 100644
index 000000000000..ce0eb33f4c41
--- /dev/null
+++ b/drivers/staging/epl/EplSdoComu.c
@@ -0,0 +1,3346 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for SDO Command Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoComu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.14 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/26 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplSdoComu.h"
+
+#if ((((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) == 0) &&\
+     (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) == 0)   )
+
+#error 'ERROR: At least SDO Server or SDO Client should be activate!'
+
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) == 0) && (EPL_OBD_USE_KERNEL == FALSE)
+
+#error 'ERROR: SDO Server needs OBDu module!'
+
+#endif
+
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef EPL_MAX_SDO_COM_CON
+#define EPL_MAX_SDO_COM_CON         5
+#endif
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+// intern events
+typedef enum {
+	kEplSdoComConEventSendFirst = 0x00,	// first frame to send
+	kEplSdoComConEventRec = 0x01,	// frame received
+	kEplSdoComConEventConEstablished = 0x02,	// connection established
+	kEplSdoComConEventConClosed = 0x03,	// connection closed
+	kEplSdoComConEventAckReceived = 0x04,	// acknowledge received by lower layer
+	// -> continue sending
+	kEplSdoComConEventFrameSended = 0x05,	// lower has send a frame
+	kEplSdoComConEventInitError = 0x06,	// error duringinitialisiation
+	// of the connection
+	kEplSdoComConEventTimeout = 0x07	// timeout in lower layer
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+	    ,
+
+	kEplSdoComConEventInitCon = 0x08,	// init connection (only client)
+	kEplSdoComConEventAbort = 0x09	// abort sdo transfer (only client)
+#endif
+} tEplSdoComConEvent;
+
+typedef enum {
+	kEplSdoComSendTypeReq = 0x00,	// send a request
+	kEplSdoComSendTypeAckRes = 0x01,	// send a resonse without data
+	kEplSdoComSendTypeRes = 0x02,	// send response with data
+	kEplSdoComSendTypeAbort = 0x03	// send abort
+} tEplSdoComSendType;
+
+// state of the state maschine
+typedef enum {
+	// General State
+	kEplSdoComStateIdle = 0x00,	// idle state
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+	// Server States
+	kEplSdoComStateServerSegmTrans = 0x01,	// send following frames
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+	// Client States
+	kEplSdoComStateClientWaitInit = 0x10,	// wait for init connection
+	// on lower layer
+	kEplSdoComStateClientConnected = 0x11,	// connection established
+	kEplSdoComStateClientSegmTrans = 0x12	// send following frames
+#endif
+} tEplSdoComState;
+
+// control structure for transaction
+typedef struct {
+	tEplSdoSeqConHdl m_SdoSeqConHdl;	// if != 0 -> entry used
+	tEplSdoComState m_SdoComState;
+	BYTE m_bTransactionId;
+	unsigned int m_uiNodeId;	// NodeId of the target
+	// -> needed to reinit connection
+	//    after timeout
+	tEplSdoTransType m_SdoTransType;	// Auto, Expedited, Segmented
+	tEplSdoServiceType m_SdoServiceType;	// WriteByIndex, ReadByIndex
+	tEplSdoType m_SdoProtType;	// protocol layer: Auto, Udp, Asnd, Pdo
+	BYTE *m_pData;		// pointer to data
+	unsigned int m_uiTransSize;	// number of bytes
+	// to transfer
+	unsigned int m_uiTransferredByte;	// number of bytes
+	// already transferred
+	tEplSdoFinishedCb m_pfnTransferFinished;	// callback function of the
+	// application
+	// -> called in the end of
+	//    the SDO transfer
+	void *m_pUserArg;	// user definable argument pointer
+
+	DWORD m_dwLastAbortCode;	// save the last abort code
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+	// only for client
+	unsigned int m_uiTargetIndex;	// index to access
+	unsigned int m_uiTargetSubIndex;	// subiondex to access
+
+	// for future use
+	unsigned int m_uiTimeout;	// timeout for this connection
+
+#endif
+
+} tEplSdoComCon;
+
+// instance table
+typedef struct {
+	tEplSdoComCon m_SdoComCon[EPL_MAX_SDO_COM_CON];
+
+#if defined(WIN32) || defined(_WIN32)
+	LPCRITICAL_SECTION m_pCriticalSection;
+	CRITICAL_SECTION m_CriticalSection;
+#endif
+
+} tEplSdoComInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+static tEplSdoComInstance SdoComInstance_g;
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoComReceiveCb(tEplSdoSeqConHdl SdoSeqConHdl_p,
+				     tEplAsySdoCom * pAsySdoCom_p,
+				     unsigned int uiDataSize_p);
+
+tEplKernel PUBLIC EplSdoComConCb(tEplSdoSeqConHdl SdoSeqConHdl_p,
+				 tEplAsySdoConState AsySdoConState_p);
+
+static tEplKernel EplSdoComSearchConIntern(tEplSdoSeqConHdl SdoSeqConHdl_p,
+					   tEplSdoComConEvent SdoComConEvent_p,
+					   tEplAsySdoCom * pAsySdoCom_p);
+
+static tEplKernel EplSdoComProcessIntern(tEplSdoComConHdl SdoComCon_p,
+					 tEplSdoComConEvent SdoComConEvent_p,
+					 tEplAsySdoCom * pAsySdoCom_p);
+
+static tEplKernel EplSdoComTransferFinished(tEplSdoComConHdl SdoComCon_p,
+					    tEplSdoComCon * pSdoComCon_p,
+					    tEplSdoComConState
+					    SdoComConState_p);
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+static tEplKernel EplSdoComServerInitReadByIndex(tEplSdoComCon * pSdoComCon_p,
+						 tEplAsySdoCom * pAsySdoCom_p);
+
+static tEplKernel EplSdoComServerSendFrameIntern(tEplSdoComCon * pSdoComCon_p,
+						 unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p,
+						 tEplSdoComSendType SendType_p);
+
+static tEplKernel EplSdoComServerInitWriteByIndex(tEplSdoComCon * pSdoComCon_p,
+						  tEplAsySdoCom * pAsySdoCom_p);
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+
+static tEplKernel EplSdoComClientSend(tEplSdoComCon * pSdoComCon_p);
+
+static tEplKernel EplSdoComClientProcessFrame(tEplSdoComConHdl SdoComCon_p,
+					      tEplAsySdoCom * pAsySdoCom_p);
+
+static tEplKernel EplSdoComClientSendAbort(tEplSdoComCon * pSdoComCon_p,
+					   DWORD dwAbortCode_p);
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <SDO Command Layer>                                 */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: SDO Command layer Modul
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComInit
+//
+// Description: Init first instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoComInit(void)
+{
+	tEplKernel Ret;
+
+	Ret = EplSdoComAddInstance();
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComAddInstance
+//
+// Description: Init additional instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoComAddInstance(void)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// init controll structure
+	EPL_MEMSET(&SdoComInstance_g, 0x00, sizeof(SdoComInstance_g));
+
+	// init instance of lower layer
+	Ret = EplSdoAsySeqAddInstance(EplSdoComReceiveCb, EplSdoComConCb);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#if defined(WIN32) || defined(_WIN32)
+	// create critical section for process function
+	SdoComInstance_g.m_pCriticalSection =
+	    &SdoComInstance_g.m_CriticalSection;
+	InitializeCriticalSection(SdoComInstance_g.m_pCriticalSection);
+#endif
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComDelInstance
+//
+// Description: delete instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoComDelInstance(void)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+#if defined(WIN32) || defined(_WIN32)
+	// delete critical section for process function
+	DeleteCriticalSection(SdoComInstance_g.m_pCriticalSection);
+#endif
+
+	Ret = EplSdoAsySeqDelInstance();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComDefineCon
+//
+// Description: function defines a SDO connection to another node
+//              -> init lower layer and returns a handle for the connection.
+//              Two client connections to the same node via the same protocol
+//              are not allowed. If this function detects such a situation
+//              it will return kEplSdoComHandleExists and the handle of
+//              the existing connection in pSdoComConHdl_p.
+//              Using of existing server connections is possible.
+//
+// Parameters:  pSdoComConHdl_p     = pointer to the buffer of the handle
+//              uiTargetNodeId_p    = NodeId of the targetnode
+//              ProtType_p          = type of protocol to use for connection
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+tEplKernel PUBLIC EplSdoComDefineCon(tEplSdoComConHdl * pSdoComConHdl_p,
+				     unsigned int uiTargetNodeId_p,
+				     tEplSdoType ProtType_p)
+{
+	tEplKernel Ret;
+	unsigned int uiCount;
+	unsigned int uiFreeHdl;
+	tEplSdoComCon *pSdoComCon;
+
+	// check Parameter
+	ASSERT(pSdoComConHdl_p != NULL);
+
+	// check NodeId
+	if ((uiTargetNodeId_p == EPL_C_ADR_INVALID)
+	    || (uiTargetNodeId_p >= EPL_C_ADR_BROADCAST)) {
+		Ret = kEplInvalidNodeId;
+
+	}
+	// search free control structure
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[0];
+	uiCount = 0;
+	uiFreeHdl = EPL_MAX_SDO_COM_CON;
+	while (uiCount < EPL_MAX_SDO_COM_CON) {
+		if (pSdoComCon->m_SdoSeqConHdl == 0) {	// free entry
+			uiFreeHdl = uiCount;
+		} else if ((pSdoComCon->m_uiNodeId == uiTargetNodeId_p)
+			   && (pSdoComCon->m_SdoProtType == ProtType_p)) {	// existing client connection with same node ID and same protocol type
+			*pSdoComConHdl_p = uiCount;
+			Ret = kEplSdoComHandleExists;
+			goto Exit;
+		}
+		uiCount++;
+		pSdoComCon++;
+	}
+
+	if (uiFreeHdl == EPL_MAX_SDO_COM_CON) {
+		Ret = kEplSdoComNoFreeHandle;
+		goto Exit;
+	}
+
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[uiFreeHdl];
+	// save handle for application
+	*pSdoComConHdl_p = uiFreeHdl;
+	// save parameters
+	pSdoComCon->m_SdoProtType = ProtType_p;
+	pSdoComCon->m_uiNodeId = uiTargetNodeId_p;
+
+	// set Transaction Id
+	pSdoComCon->m_bTransactionId = 0;
+
+	// check protocol
+	switch (ProtType_p) {
+		// udp
+	case kEplSdoTypeUdp:
+		{
+			// call connection int function of lower layer
+			Ret = EplSdoAsySeqInitCon(&pSdoComCon->m_SdoSeqConHdl,
+						  pSdoComCon->m_uiNodeId,
+						  kEplSdoTypeUdp);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+			break;
+		}
+
+		// Asend
+	case kEplSdoTypeAsnd:
+		{
+			// call connection int function of lower layer
+			Ret = EplSdoAsySeqInitCon(&pSdoComCon->m_SdoSeqConHdl,
+						  pSdoComCon->m_uiNodeId,
+						  kEplSdoTypeAsnd);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+			break;
+		}
+
+		// Pdo -> not supported
+	case kEplSdoTypePdo:
+	default:
+		{
+			Ret = kEplSdoComUnsupportedProt;
+			goto Exit;
+		}
+	}			// end of switch(m_ProtType_p)
+
+	// call process function
+	Ret = EplSdoComProcessIntern(uiFreeHdl,
+				     kEplSdoComConEventInitCon, NULL);
+
+      Exit:
+	return Ret;
+}
+#endif
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComInitTransferByIndex
+//
+// Description: function init SDO Transfer for a defined connection
+//
+//
+//
+// Parameters:  SdoComTransParam_p    = Structure with parameters for connection
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+tEplKernel PUBLIC EplSdoComInitTransferByIndex(tEplSdoComTransParamByIndex *
+					       pSdoComTransParam_p)
+{
+	tEplKernel Ret;
+	tEplSdoComCon *pSdoComCon;
+
+	// check parameter
+	if ((pSdoComTransParam_p->m_uiSubindex >= 0xFF)
+	    || (pSdoComTransParam_p->m_uiIndex == 0)
+	    || (pSdoComTransParam_p->m_uiIndex > 0xFFFF)
+	    || (pSdoComTransParam_p->m_pData == NULL)
+	    || (pSdoComTransParam_p->m_uiDataSize == 0)) {
+		Ret = kEplSdoComInvalidParam;
+		goto Exit;
+	}
+
+	if (pSdoComTransParam_p->m_SdoComConHdl >= EPL_MAX_SDO_COM_CON) {
+		Ret = kEplSdoComInvalidHandle;
+		goto Exit;
+	}
+	// get pointer to control structure of connection
+	pSdoComCon =
+	    &SdoComInstance_g.m_SdoComCon[pSdoComTransParam_p->m_SdoComConHdl];
+
+	// check if handle ok
+	if (pSdoComCon->m_SdoSeqConHdl == 0) {
+		Ret = kEplSdoComInvalidHandle;
+		goto Exit;
+	}
+	// check if command layer is idle
+	if ((pSdoComCon->m_uiTransferredByte + pSdoComCon->m_uiTransSize) > 0) {	// handle is not idle
+		Ret = kEplSdoComHandleBusy;
+		goto Exit;
+	}
+	// save parameter
+	// callback function for end of transfer
+	pSdoComCon->m_pfnTransferFinished =
+	    pSdoComTransParam_p->m_pfnSdoFinishedCb;
+	pSdoComCon->m_pUserArg = pSdoComTransParam_p->m_pUserArg;
+
+	// set type of SDO command
+	if (pSdoComTransParam_p->m_SdoAccessType == kEplSdoAccessTypeRead) {
+		pSdoComCon->m_SdoServiceType = kEplSdoServiceReadByIndex;
+	} else {
+		pSdoComCon->m_SdoServiceType = kEplSdoServiceWriteByIndex;
+
+	}
+	// save pointer to data
+	pSdoComCon->m_pData = pSdoComTransParam_p->m_pData;
+	// maximal bytes to transfer
+	pSdoComCon->m_uiTransSize = pSdoComTransParam_p->m_uiDataSize;
+	// bytes already transfered
+	pSdoComCon->m_uiTransferredByte = 0;
+
+	// reset parts of control structure
+	pSdoComCon->m_dwLastAbortCode = 0;
+	pSdoComCon->m_SdoTransType = kEplSdoTransAuto;
+	// save timeout
+	//pSdoComCon->m_uiTimeout = SdoComTransParam_p.m_uiTimeout;
+
+	// save index and subindex
+	pSdoComCon->m_uiTargetIndex = pSdoComTransParam_p->m_uiIndex;
+	pSdoComCon->m_uiTargetSubIndex = pSdoComTransParam_p->m_uiSubindex;
+
+	// call process function
+	Ret = EplSdoComProcessIntern(pSdoComTransParam_p->m_SdoComConHdl, kEplSdoComConEventSendFirst,	// event to start transfer
+				     NULL);
+
+      Exit:
+	return Ret;
+
+}
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComUndefineCon
+//
+// Description: function undefine a SDO connection
+//
+//
+//
+// Parameters:  SdoComConHdl_p    = handle for the connection
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+tEplKernel PUBLIC EplSdoComUndefineCon(tEplSdoComConHdl SdoComConHdl_p)
+{
+	tEplKernel Ret;
+	tEplSdoComCon *pSdoComCon;
+
+	Ret = kEplSuccessful;
+
+	if (SdoComConHdl_p >= EPL_MAX_SDO_COM_CON) {
+		Ret = kEplSdoComInvalidHandle;
+		goto Exit;
+	}
+	// get pointer to control structure
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[SdoComConHdl_p];
+
+	// $$$ d.k. abort a running transfer before closing the sequence layer
+
+	if (((pSdoComCon->m_SdoSeqConHdl & ~EPL_SDO_SEQ_HANDLE_MASK) !=
+	     EPL_SDO_SEQ_INVALID_HDL)
+	    && (pSdoComCon->m_SdoSeqConHdl != 0)) {
+		// close connection in lower layer
+		switch (pSdoComCon->m_SdoProtType) {
+		case kEplSdoTypeAsnd:
+		case kEplSdoTypeUdp:
+			{
+				Ret =
+				    EplSdoAsySeqDelCon(pSdoComCon->
+						       m_SdoSeqConHdl);
+				break;
+			}
+
+		case kEplSdoTypePdo:
+		case kEplSdoTypeAuto:
+		default:
+			{
+				Ret = kEplSdoComUnsupportedProt;
+				goto Exit;
+			}
+
+		}		// end of switch(pSdoComCon->m_SdoProtType)
+	}
+
+	// clean controll structure
+	EPL_MEMSET(pSdoComCon, 0x00, sizeof(tEplSdoComCon));
+      Exit:
+	return Ret;
+}
+#endif
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComGetState
+//
+// Description: function returns the state fo the connection
+//
+//
+//
+// Parameters:  SdoComConHdl_p    = handle for the connection
+//              pSdoComFinished_p = pointer to structur for sdo state
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+tEplKernel PUBLIC EplSdoComGetState(tEplSdoComConHdl SdoComConHdl_p,
+				    tEplSdoComFinished * pSdoComFinished_p)
+{
+	tEplKernel Ret;
+	tEplSdoComCon *pSdoComCon;
+
+	Ret = kEplSuccessful;
+
+	if (SdoComConHdl_p >= EPL_MAX_SDO_COM_CON) {
+		Ret = kEplSdoComInvalidHandle;
+		goto Exit;
+	}
+	// get pointer to control structure
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[SdoComConHdl_p];
+
+	// check if handle ok
+	if (pSdoComCon->m_SdoSeqConHdl == 0) {
+		Ret = kEplSdoComInvalidHandle;
+		goto Exit;
+	}
+
+	pSdoComFinished_p->m_pUserArg = pSdoComCon->m_pUserArg;
+	pSdoComFinished_p->m_uiNodeId = pSdoComCon->m_uiNodeId;
+	pSdoComFinished_p->m_uiTargetIndex = pSdoComCon->m_uiTargetIndex;
+	pSdoComFinished_p->m_uiTargetSubIndex = pSdoComCon->m_uiTargetSubIndex;
+	pSdoComFinished_p->m_uiTransferredByte =
+	    pSdoComCon->m_uiTransferredByte;
+	pSdoComFinished_p->m_dwAbortCode = pSdoComCon->m_dwLastAbortCode;
+	pSdoComFinished_p->m_SdoComConHdl = SdoComConHdl_p;
+	if (pSdoComCon->m_SdoServiceType == kEplSdoServiceWriteByIndex) {
+		pSdoComFinished_p->m_SdoAccessType = kEplSdoAccessTypeWrite;
+	} else {
+		pSdoComFinished_p->m_SdoAccessType = kEplSdoAccessTypeRead;
+	}
+
+	if (pSdoComCon->m_dwLastAbortCode != 0) {	// sdo abort
+		pSdoComFinished_p->m_SdoComConState =
+		    kEplSdoComTransferRxAborted;
+
+		// delete abort code
+		pSdoComCon->m_dwLastAbortCode = 0;
+
+	} else if ((pSdoComCon->m_SdoSeqConHdl & ~EPL_SDO_SEQ_HANDLE_MASK) == EPL_SDO_SEQ_INVALID_HDL) {	// check state
+		pSdoComFinished_p->m_SdoComConState =
+		    kEplSdoComTransferLowerLayerAbort;
+	} else if (pSdoComCon->m_SdoComState == kEplSdoComStateClientWaitInit) {
+		// finished
+		pSdoComFinished_p->m_SdoComConState =
+		    kEplSdoComTransferNotActive;
+	} else if (pSdoComCon->m_uiTransSize == 0) {	// finished
+		pSdoComFinished_p->m_SdoComConState =
+		    kEplSdoComTransferFinished;
+	}
+
+      Exit:
+	return Ret;
+
+}
+#endif
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComSdoAbort
+//
+// Description: function abort a sdo transfer
+//
+//
+//
+// Parameters:  SdoComConHdl_p    = handle for the connection
+//              dwAbortCode_p     = abort code
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+tEplKernel PUBLIC EplSdoComSdoAbort(tEplSdoComConHdl SdoComConHdl_p,
+				    DWORD dwAbortCode_p)
+{
+	tEplKernel Ret;
+	tEplSdoComCon *pSdoComCon;
+
+	if (SdoComConHdl_p >= EPL_MAX_SDO_COM_CON) {
+		Ret = kEplSdoComInvalidHandle;
+		goto Exit;
+	}
+	// get pointer to control structure of connection
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[SdoComConHdl_p];
+
+	// check if handle ok
+	if (pSdoComCon->m_SdoSeqConHdl == 0) {
+		Ret = kEplSdoComInvalidHandle;
+		goto Exit;
+	}
+	// save pointer to abort code
+	pSdoComCon->m_pData = (BYTE *) & dwAbortCode_p;
+
+	Ret = EplSdoComProcessIntern(SdoComConHdl_p,
+				     kEplSdoComConEventAbort,
+				     (tEplAsySdoCom *) NULL);
+
+      Exit:
+	return Ret;
+}
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComReceiveCb
+//
+// Description:     callback function for SDO Sequence Layer
+//                  -> indicates new data
+//
+//
+//
+// Parameters:      SdoSeqConHdl_p = Handle for connection
+//                  pAsySdoCom_p   = pointer to data
+//                  uiDataSize_p   = size of data ($$$ not used yet, but it should)
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoComReceiveCb(tEplSdoSeqConHdl SdoSeqConHdl_p,
+				     tEplAsySdoCom * pAsySdoCom_p,
+				     unsigned int uiDataSize_p)
+{
+	tEplKernel Ret;
+
+	// search connection internally
+	Ret = EplSdoComSearchConIntern(SdoSeqConHdl_p,
+				       kEplSdoComConEventRec, pAsySdoCom_p);
+
+	EPL_DBGLVL_SDO_TRACE3
+	    ("EplSdoComReceiveCb SdoSeqConHdl: 0x%X, First Byte of pAsySdoCom_p: 0x%02X, uiDataSize_p: 0x%04X\n",
+	     SdoSeqConHdl_p, (WORD) pAsySdoCom_p->m_le_abCommandData[0],
+	     uiDataSize_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComConCb
+//
+// Description:     callback function called by SDO Sequence Layer to inform
+//                  command layer about state change of connection
+//
+//
+//
+// Parameters:      SdoSeqConHdl_p      = Handle of the connection
+//                  AsySdoConState_p    = Event of the connection
+//
+//
+// Returns:         tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoComConCb(tEplSdoSeqConHdl SdoSeqConHdl_p,
+				 tEplAsySdoConState AsySdoConState_p)
+{
+	tEplKernel Ret;
+	tEplSdoComConEvent SdoComConEvent = kEplSdoComConEventSendFirst;
+
+	Ret = kEplSuccessful;
+
+	// check state
+	switch (AsySdoConState_p) {
+	case kAsySdoConStateConnected:
+		{
+			EPL_DBGLVL_SDO_TRACE0("Connection established\n");
+			SdoComConEvent = kEplSdoComConEventConEstablished;
+			// start transmission if needed
+			break;
+		}
+
+	case kAsySdoConStateInitError:
+		{
+			EPL_DBGLVL_SDO_TRACE0("Error during initialisation\n");
+			SdoComConEvent = kEplSdoComConEventInitError;
+			// inform app about error and close sequence layer handle
+			break;
+		}
+
+	case kAsySdoConStateConClosed:
+		{
+			EPL_DBGLVL_SDO_TRACE0("Connection closed\n");
+			SdoComConEvent = kEplSdoComConEventConClosed;
+			// close sequence layer handle
+			break;
+		}
+
+	case kAsySdoConStateAckReceived:
+		{
+			EPL_DBGLVL_SDO_TRACE0("Acknowlage received\n");
+			SdoComConEvent = kEplSdoComConEventAckReceived;
+			// continue transmission
+			break;
+		}
+
+	case kAsySdoConStateFrameSended:
+		{
+			EPL_DBGLVL_SDO_TRACE0("One Frame sent\n");
+			SdoComConEvent = kEplSdoComConEventFrameSended;
+			// to continue transmission
+			break;
+
+		}
+
+	case kAsySdoConStateTimeout:
+		{
+			EPL_DBGLVL_SDO_TRACE0("Timeout\n");
+			SdoComConEvent = kEplSdoComConEventTimeout;
+			// close sequence layer handle
+			break;
+
+		}
+	}			// end of switch(AsySdoConState_p)
+
+	Ret = EplSdoComSearchConIntern(SdoSeqConHdl_p,
+				       SdoComConEvent, (tEplAsySdoCom *) NULL);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComSearchConIntern
+//
+// Description:     search a Sdo Sequence Layer connection handle in the
+//                  control structure of the Command Layer
+//
+// Parameters:      SdoSeqConHdl_p     = Handle to search
+//                  SdoComConEvent_p = event to process
+//                  pAsySdoCom_p     = pointer to received frame
+//
+// Returns:         tEplKernel
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoComSearchConIntern(tEplSdoSeqConHdl SdoSeqConHdl_p,
+					   tEplSdoComConEvent SdoComConEvent_p,
+					   tEplAsySdoCom * pAsySdoCom_p)
+{
+	tEplKernel Ret;
+	tEplSdoComCon *pSdoComCon;
+	tEplSdoComConHdl HdlCount;
+	tEplSdoComConHdl HdlFree;
+
+	Ret = kEplSdoComNotResponsible;
+
+	// get pointer to first element of the array
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[0];
+	HdlCount = 0;
+	HdlFree = 0xFFFF;
+	while (HdlCount < EPL_MAX_SDO_COM_CON) {
+		if (pSdoComCon->m_SdoSeqConHdl == SdoSeqConHdl_p) {	// matching command layer handle found
+			Ret = EplSdoComProcessIntern(HdlCount,
+						     SdoComConEvent_p,
+						     pAsySdoCom_p);
+		} else if ((pSdoComCon->m_SdoSeqConHdl == 0)
+			   && (HdlFree == 0xFFFF)) {
+			HdlFree = HdlCount;
+		}
+
+		pSdoComCon++;
+		HdlCount++;
+	}
+
+	if (Ret == kEplSdoComNotResponsible) {	// no responsible command layer handle found
+		if (HdlFree == 0xFFFF) {	// no free handle
+			// delete connection immediately
+			// 2008/04/14 m.u./d.k. This connection actually does not exist.
+			//                      pSdoComCon is invalid.
+			// Ret = EplSdoAsySeqDelCon(pSdoComCon->m_SdoSeqConHdl);
+			Ret = kEplSdoComNoFreeHandle;
+		} else {	// create new handle
+			HdlCount = HdlFree;
+			pSdoComCon = &SdoComInstance_g.m_SdoComCon[HdlCount];
+			pSdoComCon->m_SdoSeqConHdl = SdoSeqConHdl_p;
+			Ret = EplSdoComProcessIntern(HdlCount,
+						     SdoComConEvent_p,
+						     pAsySdoCom_p);
+		}
+	}
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComProcessIntern
+//
+// Description:     search a Sdo Sequence Layer connection handle in the
+//                  control structer of the Command Layer
+//
+//
+//
+// Parameters:      SdoComCon_p     = index of control structure of connection
+//                  SdoComConEvent_p = event to process
+//                  pAsySdoCom_p     = pointer to received frame
+//
+// Returns:         tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoComProcessIntern(tEplSdoComConHdl SdoComCon_p,
+					 tEplSdoComConEvent SdoComConEvent_p,
+					 tEplAsySdoCom * pAsySdoCom_p)
+{
+	tEplKernel Ret;
+	tEplSdoComCon *pSdoComCon;
+	BYTE bFlag;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+	DWORD dwAbortCode;
+	unsigned int uiSize;
+#endif
+
+#if defined(WIN32) || defined(_WIN32)
+	// enter  critical section for process function
+	EnterCriticalSection(SdoComInstance_g.m_pCriticalSection);
+	EPL_DBGLVL_SDO_TRACE0
+	    ("\n\tEnterCiticalSection EplSdoComProcessIntern\n\n");
+#endif
+
+	Ret = kEplSuccessful;
+
+	// get pointer to control structure
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[SdoComCon_p];
+
+	// process state maschine
+	switch (pSdoComCon->m_SdoComState) {
+		// idle state
+	case kEplSdoComStateIdle:
+		{
+			// check events
+			switch (SdoComConEvent_p) {
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+				// init con for client
+			case kEplSdoComConEventInitCon:
+				{
+
+					// call of the init function already
+					// processed in EplSdoComDefineCon()
+					// only change state to kEplSdoComStateClientWaitInit
+					pSdoComCon->m_SdoComState =
+					    kEplSdoComStateClientWaitInit;
+					break;
+				}
+#endif
+
+				// int con for server
+			case kEplSdoComConEventRec:
+				{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+					// check if init of an transfer and no SDO abort
+					if ((pAsySdoCom_p->m_le_bFlags & 0x80) == 0) {	// SDO request
+						if ((pAsySdoCom_p->m_le_bFlags & 0x40) == 0) {	// no SDO abort
+							// save tansaction id
+							pSdoComCon->
+							    m_bTransactionId =
+							    AmiGetByteFromLe
+							    (&pAsySdoCom_p->
+							     m_le_bTransactionId);
+							// check command
+							switch (pAsySdoCom_p->
+								m_le_bCommandId)
+							{
+							case kEplSdoServiceNIL:
+								{	// simply acknowlegde NIL command on sequence layer
+
+									Ret =
+									    EplSdoAsySeqSendData
+									    (pSdoComCon->
+									     m_SdoSeqConHdl,
+									     0,
+									     (tEplFrame
+									      *)
+									     NULL);
+
+									break;
+								}
+
+							case kEplSdoServiceReadByIndex:
+								{	// read by index
+
+									// search entry an start transfer
+									EplSdoComServerInitReadByIndex
+									    (pSdoComCon,
+									     pAsySdoCom_p);
+									// check next state
+									if (pSdoComCon->m_uiTransSize == 0) {	// ready -> stay idle
+										pSdoComCon->
+										    m_SdoComState
+										    =
+										    kEplSdoComStateIdle;
+										// reset abort code
+										pSdoComCon->
+										    m_dwLastAbortCode
+										    =
+										    0;
+									} else {	// segmented transfer
+										pSdoComCon->
+										    m_SdoComState
+										    =
+										    kEplSdoComStateServerSegmTrans;
+									}
+
+									break;
+								}
+
+							case kEplSdoServiceWriteByIndex:
+								{
+
+									// search entry an start write
+									EplSdoComServerInitWriteByIndex
+									    (pSdoComCon,
+									     pAsySdoCom_p);
+									// check next state
+									if (pSdoComCon->m_uiTransSize == 0) {	// already -> stay idle
+										pSdoComCon->
+										    m_SdoComState
+										    =
+										    kEplSdoComStateIdle;
+										// reset abort code
+										pSdoComCon->
+										    m_dwLastAbortCode
+										    =
+										    0;
+									} else {	// segmented transfer
+										pSdoComCon->
+										    m_SdoComState
+										    =
+										    kEplSdoComStateServerSegmTrans;
+									}
+
+									break;
+								}
+
+							default:
+								{
+									//  unsupported command
+									//       -> abort senden
+									dwAbortCode
+									    =
+									    EPL_SDOAC_UNKNOWN_COMMAND_SPECIFIER;
+									// send abort
+									pSdoComCon->
+									    m_pData
+									    =
+									    (BYTE
+									     *)
+									    &
+									    dwAbortCode;
+									Ret =
+									    EplSdoComServerSendFrameIntern
+									    (pSdoComCon,
+									     0,
+									     0,
+									     kEplSdoComSendTypeAbort);
+
+								}
+
+							}	// end of switch(pAsySdoCom_p->m_le_bCommandId)
+						}
+					} else {	// this command layer handle is not responsible
+						// (wrong direction or wrong transaction ID)
+						Ret = kEplSdoComNotResponsible;
+						goto Exit;
+					}
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+
+					break;
+				}
+
+				// connection closed
+			case kEplSdoComConEventInitError:
+			case kEplSdoComConEventTimeout:
+			case kEplSdoComConEventConClosed:
+				{
+					Ret =
+					    EplSdoAsySeqDelCon(pSdoComCon->
+							       m_SdoSeqConHdl);
+					// clean control structure
+					EPL_MEMSET(pSdoComCon, 0x00,
+						   sizeof(tEplSdoComCon));
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+			}	// end of switch(SdoComConEvent_p)
+			break;
+		}
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+		//-------------------------------------------------------------------------
+		// SDO Server part
+		// segmented transfer
+	case kEplSdoComStateServerSegmTrans:
+		{
+			// check events
+			switch (SdoComConEvent_p) {
+				// send next frame
+			case kEplSdoComConEventAckReceived:
+			case kEplSdoComConEventFrameSended:
+				{
+					// check if it is a read
+					if (pSdoComCon->m_SdoServiceType ==
+					    kEplSdoServiceReadByIndex) {
+						// send next frame
+						EplSdoComServerSendFrameIntern
+						    (pSdoComCon, 0, 0,
+						     kEplSdoComSendTypeRes);
+						// if all send -> back to idle
+						if (pSdoComCon->m_uiTransSize == 0) {	// back to idle
+							pSdoComCon->
+							    m_SdoComState =
+							    kEplSdoComStateIdle;
+							// reset abort code
+							pSdoComCon->
+							    m_dwLastAbortCode =
+							    0;
+						}
+
+					}
+					break;
+				}
+
+				// process next frame
+			case kEplSdoComConEventRec:
+				{
+					// check if the frame is a SDO response and has the right transaction ID
+					bFlag =
+					    AmiGetByteFromLe(&pAsySdoCom_p->
+							     m_le_bFlags);
+					if (((bFlag & 0x80) != 0)
+					    &&
+					    (AmiGetByteFromLe
+					     (&pAsySdoCom_p->
+					      m_le_bTransactionId) ==
+					     pSdoComCon->m_bTransactionId)) {
+						// check if it is a abort
+						if ((bFlag & 0x40) != 0) {	// SDO abort
+							// clear control structure
+							pSdoComCon->
+							    m_uiTransSize = 0;
+							pSdoComCon->
+							    m_uiTransferredByte
+							    = 0;
+							// change state
+							pSdoComCon->
+							    m_SdoComState =
+							    kEplSdoComStateIdle;
+							// reset abort code
+							pSdoComCon->
+							    m_dwLastAbortCode =
+							    0;
+							// d.k.: do not execute anything further on this command
+							break;
+						}
+						// check if it is a write
+						if (pSdoComCon->
+						    m_SdoServiceType ==
+						    kEplSdoServiceWriteByIndex)
+						{
+							// write data to OD
+							uiSize =
+							    AmiGetWordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_wSegmentSize);
+							if (pSdoComCon->
+							    m_dwLastAbortCode ==
+							    0) {
+								EPL_MEMCPY
+								    (pSdoComCon->
+								     m_pData,
+								     &pAsySdoCom_p->
+								     m_le_abCommandData
+								     [0],
+								     uiSize);
+							}
+							// update counter
+							pSdoComCon->
+							    m_uiTransferredByte
+							    += uiSize;
+							pSdoComCon->
+							    m_uiTransSize -=
+							    uiSize;
+
+							// update pointer
+							if (pSdoComCon->
+							    m_dwLastAbortCode ==
+							    0) {
+								( /*(BYTE*) */
+								 pSdoComCon->
+								 m_pData) +=
+						      uiSize;
+							}
+							// check end of transfer
+							if ((pAsySdoCom_p->m_le_bFlags & 0x30) == 0x30) {	// transfer ready
+								pSdoComCon->
+								    m_uiTransSize
+								    = 0;
+
+								if (pSdoComCon->
+								    m_dwLastAbortCode
+								    == 0) {
+									// send response
+									// send next frame
+									EplSdoComServerSendFrameIntern
+									    (pSdoComCon,
+									     0,
+									     0,
+									     kEplSdoComSendTypeRes);
+									// if all send -> back to idle
+									if (pSdoComCon->m_uiTransSize == 0) {	// back to idle
+										pSdoComCon->
+										    m_SdoComState
+										    =
+										    kEplSdoComStateIdle;
+										// reset abort code
+										pSdoComCon->
+										    m_dwLastAbortCode
+										    =
+										    0;
+									}
+								} else {	// send dabort code
+									// send abort
+									pSdoComCon->
+									    m_pData
+									    =
+									    (BYTE
+									     *)
+									    &
+									    pSdoComCon->
+									    m_dwLastAbortCode;
+									Ret =
+									    EplSdoComServerSendFrameIntern
+									    (pSdoComCon,
+									     0,
+									     0,
+									     kEplSdoComSendTypeAbort);
+
+									// reset abort code
+									pSdoComCon->
+									    m_dwLastAbortCode
+									    = 0;
+
+								}
+							} else {
+								// send acknowledge without any Command layer data
+								Ret =
+								    EplSdoAsySeqSendData
+								    (pSdoComCon->
+								     m_SdoSeqConHdl,
+								     0,
+								     (tEplFrame
+								      *) NULL);
+							}
+						}
+					} else {	// this command layer handle is not responsible
+						// (wrong direction or wrong transaction ID)
+						Ret = kEplSdoComNotResponsible;
+						goto Exit;
+					}
+					break;
+				}
+
+				// connection closed
+			case kEplSdoComConEventInitError:
+			case kEplSdoComConEventTimeout:
+			case kEplSdoComConEventConClosed:
+				{
+					Ret =
+					    EplSdoAsySeqDelCon(pSdoComCon->
+							       m_SdoSeqConHdl);
+					// clean control structure
+					EPL_MEMSET(pSdoComCon, 0x00,
+						   sizeof(tEplSdoComCon));
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+			}	// end of switch(SdoComConEvent_p)
+
+			break;
+		}
+#endif // endif of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+		//-------------------------------------------------------------------------
+		// SDO Client part
+		// wait for finish of establishing connection
+	case kEplSdoComStateClientWaitInit:
+		{
+
+			// if connection handle is invalid reinit connection
+			// d.k.: this will be done only on new events (i.e. InitTransfer)
+			if ((pSdoComCon->
+			     m_SdoSeqConHdl & ~EPL_SDO_SEQ_HANDLE_MASK) ==
+			    EPL_SDO_SEQ_INVALID_HDL) {
+				// check kind of connection to reinit
+				// check protocol
+				switch (pSdoComCon->m_SdoProtType) {
+					// udp
+				case kEplSdoTypeUdp:
+					{
+						// call connection int function of lower layer
+						Ret =
+						    EplSdoAsySeqInitCon
+						    (&pSdoComCon->
+						     m_SdoSeqConHdl,
+						     pSdoComCon->m_uiNodeId,
+						     kEplSdoTypeUdp);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						break;
+					}
+
+					// Asend -> not supported
+				case kEplSdoTypeAsnd:
+					{
+						// call connection int function of lower layer
+						Ret =
+						    EplSdoAsySeqInitCon
+						    (&pSdoComCon->
+						     m_SdoSeqConHdl,
+						     pSdoComCon->m_uiNodeId,
+						     kEplSdoTypeAsnd);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						break;
+					}
+
+					// Pdo -> not supported
+				case kEplSdoTypePdo:
+				default:
+					{
+						Ret = kEplSdoComUnsupportedProt;
+						goto Exit;
+					}
+				}	// end of switch(m_ProtType_p)
+				// d.k.: reset transaction ID, because new sequence layer connection was initialized
+				// $$$ d.k. is this really necessary?
+				//pSdoComCon->m_bTransactionId = 0;
+			}
+			// check events
+			switch (SdoComConEvent_p) {
+				// connection established
+			case kEplSdoComConEventConEstablished:
+				{
+					//send first frame if needed
+					if ((pSdoComCon->m_uiTransSize > 0)
+					    && (pSdoComCon->m_uiTargetIndex != 0)) {	// start SDO transfer
+						Ret =
+						    EplSdoComClientSend
+						    (pSdoComCon);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						// check if segemted transfer
+						if (pSdoComCon->
+						    m_SdoTransType ==
+						    kEplSdoTransSegmented) {
+							pSdoComCon->
+							    m_SdoComState =
+							    kEplSdoComStateClientSegmTrans;
+							goto Exit;
+						}
+					}
+					// goto state kEplSdoComStateClientConnected
+					pSdoComCon->m_SdoComState =
+					    kEplSdoComStateClientConnected;
+					goto Exit;
+				}
+
+			case kEplSdoComConEventSendFirst:
+				{
+					// infos for transfer already saved by function EplSdoComInitTransferByIndex
+					break;
+				}
+
+			case kEplSdoComConEventConClosed:
+			case kEplSdoComConEventInitError:
+			case kEplSdoComConEventTimeout:
+				{
+					// close sequence layer handle
+					Ret =
+					    EplSdoAsySeqDelCon(pSdoComCon->
+							       m_SdoSeqConHdl);
+					pSdoComCon->m_SdoSeqConHdl |=
+					    EPL_SDO_SEQ_INVALID_HDL;
+					// call callback function
+					if (SdoComConEvent_p ==
+					    kEplSdoComConEventTimeout) {
+						pSdoComCon->m_dwLastAbortCode =
+						    EPL_SDOAC_TIME_OUT;
+					} else {
+						pSdoComCon->m_dwLastAbortCode =
+						    0;
+					}
+					Ret =
+					    EplSdoComTransferFinished
+					    (SdoComCon_p, pSdoComCon,
+					     kEplSdoComTransferLowerLayerAbort);
+					// d.k.: do not clean control structure
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of  switch(SdoComConEvent_p)
+			break;
+		}
+
+		// connected
+	case kEplSdoComStateClientConnected:
+		{
+			// check events
+			switch (SdoComConEvent_p) {
+				// send a frame
+			case kEplSdoComConEventSendFirst:
+			case kEplSdoComConEventAckReceived:
+			case kEplSdoComConEventFrameSended:
+				{
+					Ret = EplSdoComClientSend(pSdoComCon);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+					// check if read transfer finished
+					if ((pSdoComCon->m_uiTransSize == 0)
+					    && (pSdoComCon->
+						m_uiTransferredByte != 0)
+					    && (pSdoComCon->m_SdoServiceType ==
+						kEplSdoServiceReadByIndex)) {
+						// inc transaction id
+						pSdoComCon->m_bTransactionId++;
+						// call callback of application
+						pSdoComCon->m_dwLastAbortCode =
+						    0;
+						Ret =
+						    EplSdoComTransferFinished
+						    (SdoComCon_p, pSdoComCon,
+						     kEplSdoComTransferFinished);
+
+						goto Exit;
+					}
+					// check if segemted transfer
+					if (pSdoComCon->m_SdoTransType ==
+					    kEplSdoTransSegmented) {
+						pSdoComCon->m_SdoComState =
+						    kEplSdoComStateClientSegmTrans;
+						goto Exit;
+					}
+					break;
+				}
+
+				// frame received
+			case kEplSdoComConEventRec:
+				{
+					// check if the frame is a SDO response and has the right transaction ID
+					bFlag =
+					    AmiGetByteFromLe(&pAsySdoCom_p->
+							     m_le_bFlags);
+					if (((bFlag & 0x80) != 0)
+					    &&
+					    (AmiGetByteFromLe
+					     (&pAsySdoCom_p->
+					      m_le_bTransactionId) ==
+					     pSdoComCon->m_bTransactionId)) {
+						// check if abort or not
+						if ((bFlag & 0x40) != 0) {
+							// send acknowledge without any Command layer data
+							Ret =
+							    EplSdoAsySeqSendData
+							    (pSdoComCon->
+							     m_SdoSeqConHdl, 0,
+							     (tEplFrame *)
+							     NULL);
+							// inc transaction id
+							pSdoComCon->
+							    m_bTransactionId++;
+							// save abort code
+							pSdoComCon->
+							    m_dwLastAbortCode =
+							    AmiGetDwordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_abCommandData
+							     [0]);
+							// call callback of application
+							Ret =
+							    EplSdoComTransferFinished
+							    (SdoComCon_p,
+							     pSdoComCon,
+							     kEplSdoComTransferRxAborted);
+
+							goto Exit;
+						} else {	// normal frame received
+							// check frame
+							Ret =
+							    EplSdoComClientProcessFrame
+							    (SdoComCon_p,
+							     pAsySdoCom_p);
+
+							// check if transfer ready
+							if (pSdoComCon->
+							    m_uiTransSize ==
+							    0) {
+								// send acknowledge without any Command layer data
+								Ret =
+								    EplSdoAsySeqSendData
+								    (pSdoComCon->
+								     m_SdoSeqConHdl,
+								     0,
+								     (tEplFrame
+								      *) NULL);
+								// inc transaction id
+								pSdoComCon->
+								    m_bTransactionId++;
+								// call callback of application
+								pSdoComCon->
+								    m_dwLastAbortCode
+								    = 0;
+								Ret =
+								    EplSdoComTransferFinished
+								    (SdoComCon_p,
+								     pSdoComCon,
+								     kEplSdoComTransferFinished);
+
+								goto Exit;
+							}
+
+						}
+					} else {	// this command layer handle is not responsible
+						// (wrong direction or wrong transaction ID)
+						Ret = kEplSdoComNotResponsible;
+						goto Exit;
+					}
+					break;
+				}
+
+				// connection closed event go back to kEplSdoComStateClientWaitInit
+			case kEplSdoComConEventConClosed:
+				{	// connection closed by communication partner
+					// close sequence layer handle
+					Ret =
+					    EplSdoAsySeqDelCon(pSdoComCon->
+							       m_SdoSeqConHdl);
+					// set handle to invalid and enter kEplSdoComStateClientWaitInit
+					pSdoComCon->m_SdoSeqConHdl |=
+					    EPL_SDO_SEQ_INVALID_HDL;
+					// change state
+					pSdoComCon->m_SdoComState =
+					    kEplSdoComStateClientWaitInit;
+
+					// call callback of application
+					pSdoComCon->m_dwLastAbortCode = 0;
+					Ret =
+					    EplSdoComTransferFinished
+					    (SdoComCon_p, pSdoComCon,
+					     kEplSdoComTransferLowerLayerAbort);
+
+					goto Exit;
+
+					break;
+				}
+
+				// abort to send from higher layer
+			case kEplSdoComConEventAbort:
+				{
+					EplSdoComClientSendAbort(pSdoComCon,
+								 *((DWORD *)
+								   pSdoComCon->
+								   m_pData));
+
+					// inc transaction id
+					pSdoComCon->m_bTransactionId++;
+					// call callback of application
+					pSdoComCon->m_dwLastAbortCode =
+					    *((DWORD *) pSdoComCon->m_pData);
+					Ret =
+					    EplSdoComTransferFinished
+					    (SdoComCon_p, pSdoComCon,
+					     kEplSdoComTransferTxAborted);
+
+					break;
+				}
+
+			case kEplSdoComConEventInitError:
+			case kEplSdoComConEventTimeout:
+				{
+					// close sequence layer handle
+					Ret =
+					    EplSdoAsySeqDelCon(pSdoComCon->
+							       m_SdoSeqConHdl);
+					pSdoComCon->m_SdoSeqConHdl |=
+					    EPL_SDO_SEQ_INVALID_HDL;
+					// change state
+					pSdoComCon->m_SdoComState =
+					    kEplSdoComStateClientWaitInit;
+					// call callback of application
+					pSdoComCon->m_dwLastAbortCode =
+					    EPL_SDOAC_TIME_OUT;
+					Ret =
+					    EplSdoComTransferFinished
+					    (SdoComCon_p, pSdoComCon,
+					     kEplSdoComTransferLowerLayerAbort);
+
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of switch(SdoComConEvent_p)
+
+			break;
+		}
+
+		// process segmented transfer
+	case kEplSdoComStateClientSegmTrans:
+		{
+			// check events
+			switch (SdoComConEvent_p) {
+				// sned a frame
+			case kEplSdoComConEventSendFirst:
+			case kEplSdoComConEventAckReceived:
+			case kEplSdoComConEventFrameSended:
+				{
+					Ret = EplSdoComClientSend(pSdoComCon);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+					// check if read transfer finished
+					if ((pSdoComCon->m_uiTransSize == 0)
+					    && (pSdoComCon->m_SdoServiceType ==
+						kEplSdoServiceReadByIndex)) {
+						// inc transaction id
+						pSdoComCon->m_bTransactionId++;
+						// change state
+						pSdoComCon->m_SdoComState =
+						    kEplSdoComStateClientConnected;
+						// call callback of application
+						pSdoComCon->m_dwLastAbortCode =
+						    0;
+						Ret =
+						    EplSdoComTransferFinished
+						    (SdoComCon_p, pSdoComCon,
+						     kEplSdoComTransferFinished);
+
+						goto Exit;
+					}
+
+					break;
+				}
+
+				// frame received
+			case kEplSdoComConEventRec:
+				{
+					// check if the frame is a response
+					bFlag =
+					    AmiGetByteFromLe(&pAsySdoCom_p->
+							     m_le_bFlags);
+					if (((bFlag & 0x80) != 0)
+					    &&
+					    (AmiGetByteFromLe
+					     (&pAsySdoCom_p->
+					      m_le_bTransactionId) ==
+					     pSdoComCon->m_bTransactionId)) {
+						// check if abort or not
+						if ((bFlag & 0x40) != 0) {
+							// send acknowledge without any Command layer data
+							Ret =
+							    EplSdoAsySeqSendData
+							    (pSdoComCon->
+							     m_SdoSeqConHdl, 0,
+							     (tEplFrame *)
+							     NULL);
+							// inc transaction id
+							pSdoComCon->
+							    m_bTransactionId++;
+							// change state
+							pSdoComCon->
+							    m_SdoComState =
+							    kEplSdoComStateClientConnected;
+							// save abort code
+							pSdoComCon->
+							    m_dwLastAbortCode =
+							    AmiGetDwordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_abCommandData
+							     [0]);
+							// call callback of application
+							Ret =
+							    EplSdoComTransferFinished
+							    (SdoComCon_p,
+							     pSdoComCon,
+							     kEplSdoComTransferRxAborted);
+
+							goto Exit;
+						} else {	// normal frame received
+							// check frame
+							Ret =
+							    EplSdoComClientProcessFrame
+							    (SdoComCon_p,
+							     pAsySdoCom_p);
+
+							// check if transfer ready
+							if (pSdoComCon->
+							    m_uiTransSize ==
+							    0) {
+								// send acknowledge without any Command layer data
+								Ret =
+								    EplSdoAsySeqSendData
+								    (pSdoComCon->
+								     m_SdoSeqConHdl,
+								     0,
+								     (tEplFrame
+								      *) NULL);
+								// inc transaction id
+								pSdoComCon->
+								    m_bTransactionId++;
+								// change state
+								pSdoComCon->
+								    m_SdoComState
+								    =
+								    kEplSdoComStateClientConnected;
+								// call callback of application
+								pSdoComCon->
+								    m_dwLastAbortCode
+								    = 0;
+								Ret =
+								    EplSdoComTransferFinished
+								    (SdoComCon_p,
+								     pSdoComCon,
+								     kEplSdoComTransferFinished);
+
+							}
+
+						}
+					}
+					break;
+				}
+
+				// connection closed event go back to kEplSdoComStateClientWaitInit
+			case kEplSdoComConEventConClosed:
+				{	// connection closed by communication partner
+					// close sequence layer handle
+					Ret =
+					    EplSdoAsySeqDelCon(pSdoComCon->
+							       m_SdoSeqConHdl);
+					// set handle to invalid and enter kEplSdoComStateClientWaitInit
+					pSdoComCon->m_SdoSeqConHdl |=
+					    EPL_SDO_SEQ_INVALID_HDL;
+					// change state
+					pSdoComCon->m_SdoComState =
+					    kEplSdoComStateClientWaitInit;
+					// inc transaction id
+					pSdoComCon->m_bTransactionId++;
+					// call callback of application
+					pSdoComCon->m_dwLastAbortCode = 0;
+					Ret =
+					    EplSdoComTransferFinished
+					    (SdoComCon_p, pSdoComCon,
+					     kEplSdoComTransferFinished);
+
+					break;
+				}
+
+				// abort to send from higher layer
+			case kEplSdoComConEventAbort:
+				{
+					EplSdoComClientSendAbort(pSdoComCon,
+								 *((DWORD *)
+								   pSdoComCon->
+								   m_pData));
+
+					// inc transaction id
+					pSdoComCon->m_bTransactionId++;
+					// change state
+					pSdoComCon->m_SdoComState =
+					    kEplSdoComStateClientConnected;
+					// call callback of application
+					pSdoComCon->m_dwLastAbortCode =
+					    *((DWORD *) pSdoComCon->m_pData);
+					Ret =
+					    EplSdoComTransferFinished
+					    (SdoComCon_p, pSdoComCon,
+					     kEplSdoComTransferTxAborted);
+
+					break;
+				}
+
+			case kEplSdoComConEventInitError:
+			case kEplSdoComConEventTimeout:
+				{
+					// close sequence layer handle
+					Ret =
+					    EplSdoAsySeqDelCon(pSdoComCon->
+							       m_SdoSeqConHdl);
+					pSdoComCon->m_SdoSeqConHdl |=
+					    EPL_SDO_SEQ_INVALID_HDL;
+					// change state
+					pSdoComCon->m_SdoComState =
+					    kEplSdoComStateClientWaitInit;
+					// call callback of application
+					pSdoComCon->m_dwLastAbortCode =
+					    EPL_SDOAC_TIME_OUT;
+					Ret =
+					    EplSdoComTransferFinished
+					    (SdoComCon_p, pSdoComCon,
+					     kEplSdoComTransferLowerLayerAbort);
+
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of switch(SdoComConEvent_p)
+
+			break;
+		}
+#endif // endo of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+
+	}			// end of switch(pSdoComCon->m_SdoComState)
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+      Exit:
+#endif
+
+#if defined(WIN32) || defined(_WIN32)
+	// leave critical section for process function
+	EPL_DBGLVL_SDO_TRACE0
+	    ("\n\tLeaveCriticalSection EplSdoComProcessIntern\n\n");
+	LeaveCriticalSection(SdoComInstance_g.m_pCriticalSection);
+
+#endif
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComServerInitReadByIndex
+//
+// Description:    function start the processing of an read by index command
+//
+//
+//
+// Parameters:      pSdoComCon_p     = pointer to control structure of connection
+//                  pAsySdoCom_p     = pointer to received frame
+//
+// Returns:         tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+static tEplKernel EplSdoComServerInitReadByIndex(tEplSdoComCon * pSdoComCon_p,
+						 tEplAsySdoCom * pAsySdoCom_p)
+{
+	tEplKernel Ret;
+	unsigned int uiIndex;
+	unsigned int uiSubindex;
+	tEplObdSize EntrySize;
+	tEplObdAccess AccessType;
+	DWORD dwAbortCode;
+
+	dwAbortCode = 0;
+
+	// a init of a read could not be a segmented transfer
+	// -> no variable part of header
+
+	// get index and subindex
+	uiIndex = AmiGetWordFromLe(&pAsySdoCom_p->m_le_abCommandData[0]);
+	uiSubindex = AmiGetByteFromLe(&pAsySdoCom_p->m_le_abCommandData[2]);
+
+	// check accesstype of entry
+	// existens of entry
+//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+	Ret = EplObduGetAccessType(uiIndex, uiSubindex, &AccessType);
+/*#else
+    Ret = kEplObdSubindexNotExist;
+    AccessType = 0;
+#endif*/
+	if (Ret == kEplObdSubindexNotExist) {	// subentry doesn't exist
+		dwAbortCode = EPL_SDOAC_SUB_INDEX_NOT_EXIST;
+		// send abort
+		pSdoComCon_p->m_pData = (BYTE *) & dwAbortCode;
+		Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+						     uiIndex,
+						     uiSubindex,
+						     kEplSdoComSendTypeAbort);
+		goto Exit;
+	} else if (Ret != kEplSuccessful) {	// entry doesn't exist
+		dwAbortCode = EPL_SDOAC_OBJECT_NOT_EXIST;
+		// send abort
+		pSdoComCon_p->m_pData = (BYTE *) & dwAbortCode;
+		Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+						     uiIndex,
+						     uiSubindex,
+						     kEplSdoComSendTypeAbort);
+		goto Exit;
+	}
+	// compare accesstype must be read or const
+	if (((AccessType & kEplObdAccRead) == 0)
+	    && ((AccessType & kEplObdAccConst) == 0)) {
+
+		if ((AccessType & kEplObdAccWrite) != 0) {
+			// entry read a write only object
+			dwAbortCode = EPL_SDOAC_READ_TO_WRITE_ONLY_OBJ;
+		} else {
+			dwAbortCode = EPL_SDOAC_UNSUPPORTED_ACCESS;
+		}
+		// send abort
+		pSdoComCon_p->m_pData = (BYTE *) & dwAbortCode;
+		Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+						     uiIndex,
+						     uiSubindex,
+						     kEplSdoComSendTypeAbort);
+		goto Exit;
+	}
+	// save service
+	pSdoComCon_p->m_SdoServiceType = kEplSdoServiceReadByIndex;
+
+	// get size of object to see iof segmented or expedited transfer
+//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+	EntrySize = EplObduGetDataSize(uiIndex, uiSubindex);
+/*#else
+    EntrySize = 0;
+#endif*/
+	if (EntrySize > EPL_SDO_MAX_PAYLOAD) {	// segmented transfer
+		pSdoComCon_p->m_SdoTransType = kEplSdoTransSegmented;
+		// get pointer to object-entry data
+//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+		pSdoComCon_p->m_pData =
+		    EplObduGetObjectDataPtr(uiIndex, uiSubindex);
+//#endif
+	} else {		// expedited transfer
+		pSdoComCon_p->m_SdoTransType = kEplSdoTransExpedited;
+	}
+
+	pSdoComCon_p->m_uiTransSize = EntrySize;
+	pSdoComCon_p->m_uiTransferredByte = 0;
+
+	Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+					     uiIndex,
+					     uiSubindex, kEplSdoComSendTypeRes);
+	if (Ret != kEplSuccessful) {
+		// error -> abort
+		dwAbortCode = EPL_SDOAC_GENERAL_ERROR;
+		// send abort
+		pSdoComCon_p->m_pData = (BYTE *) & dwAbortCode;
+		Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+						     uiIndex,
+						     uiSubindex,
+						     kEplSdoComSendTypeAbort);
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComServerSendFrameIntern();
+//
+// Description:    function creats and send a frame for server
+//
+//
+//
+// Parameters:      pSdoComCon_p     = pointer to control structure of connection
+//                  uiIndex_p        = index to send if expedited transfer else 0
+//                  uiSubIndex_p     = subindex to send if expedited transfer else 0
+//                  SendType_p       = to of frame to send
+//
+// Returns:         tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+static tEplKernel EplSdoComServerSendFrameIntern(tEplSdoComCon * pSdoComCon_p,
+						 unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p,
+						 tEplSdoComSendType SendType_p)
+{
+	tEplKernel Ret;
+	BYTE abFrame[EPL_MAX_SDO_FRAME_SIZE];
+	tEplFrame *pFrame;
+	tEplAsySdoCom *pCommandFrame;
+	unsigned int uiSizeOfFrame;
+	BYTE bFlag;
+
+	Ret = kEplSuccessful;
+
+	pFrame = (tEplFrame *) & abFrame[0];
+
+	EPL_MEMSET(&abFrame[0], 0x00, sizeof(abFrame));
+
+	// build generic part of frame
+	// get pointer to command layerpart of frame
+	pCommandFrame =
+	    &pFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame.
+	    m_le_abSdoSeqPayload;
+	AmiSetByteToLe(&pCommandFrame->m_le_bCommandId,
+		       pSdoComCon_p->m_SdoServiceType);
+	AmiSetByteToLe(&pCommandFrame->m_le_bTransactionId,
+		       pSdoComCon_p->m_bTransactionId);
+
+	// set size to header size
+	uiSizeOfFrame = 8;
+
+	// check SendType
+	switch (SendType_p) {
+		// requestframe to send
+	case kEplSdoComSendTypeReq:
+		{
+			// nothing to do for server
+			//-> error
+			Ret = kEplSdoComInvalidSendType;
+			break;
+		}
+
+		// response without data to send
+	case kEplSdoComSendTypeAckRes:
+		{
+			// set response flag
+			AmiSetByteToLe(&pCommandFrame->m_le_bFlags, 0x80);
+
+			// send frame
+			Ret = EplSdoAsySeqSendData(pSdoComCon_p->m_SdoSeqConHdl,
+						   uiSizeOfFrame, pFrame);
+
+			break;
+		}
+
+		// responsframe to send
+	case kEplSdoComSendTypeRes:
+		{
+			// set response flag
+			bFlag = AmiGetByteFromLe(&pCommandFrame->m_le_bFlags);
+			bFlag |= 0x80;
+			AmiSetByteToLe(&pCommandFrame->m_le_bFlags, bFlag);
+
+			// check type of resonse
+			if (pSdoComCon_p->m_SdoTransType == kEplSdoTransExpedited) {	// Expedited transfer
+				// copy data in frame
+//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+				Ret = EplObduReadEntryToLe(uiIndex_p,
+							   uiSubIndex_p,
+							   &pCommandFrame->
+							   m_le_abCommandData
+							   [0],
+							   (tEplObdSize *) &
+							   pSdoComCon_p->
+							   m_uiTransSize);
+				if (Ret != kEplSuccessful) {
+					goto Exit;
+				}
+//#endif
+
+				// set size of frame
+				AmiSetWordToLe(&pCommandFrame->
+					       m_le_wSegmentSize,
+					       (WORD) pSdoComCon_p->
+					       m_uiTransSize);
+
+				// correct byte-counter
+				uiSizeOfFrame += pSdoComCon_p->m_uiTransSize;
+				pSdoComCon_p->m_uiTransferredByte +=
+				    pSdoComCon_p->m_uiTransSize;
+				pSdoComCon_p->m_uiTransSize = 0;
+
+				// send frame
+				uiSizeOfFrame += pSdoComCon_p->m_uiTransSize;
+				Ret =
+				    EplSdoAsySeqSendData(pSdoComCon_p->
+							 m_SdoSeqConHdl,
+							 uiSizeOfFrame, pFrame);
+			} else if (pSdoComCon_p->m_SdoTransType == kEplSdoTransSegmented) {	// segmented transfer
+				// distinguish between init, segment and complete
+				if (pSdoComCon_p->m_uiTransferredByte == 0) {	// init
+					// set init flag
+					bFlag =
+					    AmiGetByteFromLe(&pCommandFrame->
+							     m_le_bFlags);
+					bFlag |= 0x10;
+					AmiSetByteToLe(&pCommandFrame->
+						       m_le_bFlags, bFlag);
+					// init variable header
+					AmiSetDwordToLe(&pCommandFrame->
+							m_le_abCommandData[0],
+							pSdoComCon_p->
+							m_uiTransSize);
+					// copy data in frame
+					EPL_MEMCPY(&pCommandFrame->
+						   m_le_abCommandData[4],
+						   pSdoComCon_p->m_pData,
+						   (EPL_SDO_MAX_PAYLOAD - 4));
+
+					// correct byte-counter
+					pSdoComCon_p->m_uiTransSize -=
+					    (EPL_SDO_MAX_PAYLOAD - 4);
+					pSdoComCon_p->m_uiTransferredByte +=
+					    (EPL_SDO_MAX_PAYLOAD - 4);
+					// move data pointer
+					pSdoComCon_p->m_pData +=
+					    (EPL_SDO_MAX_PAYLOAD - 4);
+
+					// set segment size
+					AmiSetWordToLe(&pCommandFrame->
+						       m_le_wSegmentSize,
+						       (EPL_SDO_MAX_PAYLOAD -
+							4));
+
+					// send frame
+					uiSizeOfFrame += EPL_SDO_MAX_PAYLOAD;
+					Ret =
+					    EplSdoAsySeqSendData(pSdoComCon_p->
+								 m_SdoSeqConHdl,
+								 uiSizeOfFrame,
+								 pFrame);
+
+				} else
+				    if ((pSdoComCon_p->m_uiTransferredByte > 0)
+					&& (pSdoComCon_p->m_uiTransSize > EPL_SDO_MAX_PAYLOAD)) {	// segment
+					// set segment flag
+					bFlag =
+					    AmiGetByteFromLe(&pCommandFrame->
+							     m_le_bFlags);
+					bFlag |= 0x20;
+					AmiSetByteToLe(&pCommandFrame->
+						       m_le_bFlags, bFlag);
+
+					// copy data in frame
+					EPL_MEMCPY(&pCommandFrame->
+						   m_le_abCommandData[0],
+						   pSdoComCon_p->m_pData,
+						   EPL_SDO_MAX_PAYLOAD);
+
+					// correct byte-counter
+					pSdoComCon_p->m_uiTransSize -=
+					    EPL_SDO_MAX_PAYLOAD;
+					pSdoComCon_p->m_uiTransferredByte +=
+					    EPL_SDO_MAX_PAYLOAD;
+					// move data pointer
+					pSdoComCon_p->m_pData +=
+					    EPL_SDO_MAX_PAYLOAD;
+
+					// set segment size
+					AmiSetWordToLe(&pCommandFrame->
+						       m_le_wSegmentSize,
+						       EPL_SDO_MAX_PAYLOAD);
+
+					// send frame
+					uiSizeOfFrame += EPL_SDO_MAX_PAYLOAD;
+					Ret =
+					    EplSdoAsySeqSendData(pSdoComCon_p->
+								 m_SdoSeqConHdl,
+								 uiSizeOfFrame,
+								 pFrame);
+				} else {
+					if ((pSdoComCon_p->m_uiTransSize == 0)
+					    && (pSdoComCon_p->
+						m_SdoServiceType !=
+						kEplSdoServiceWriteByIndex)) {
+						goto Exit;
+					}
+					// complete
+					// set segment complete flag
+					bFlag =
+					    AmiGetByteFromLe(&pCommandFrame->
+							     m_le_bFlags);
+					bFlag |= 0x30;
+					AmiSetByteToLe(&pCommandFrame->
+						       m_le_bFlags, bFlag);
+
+					// copy data in frame
+					EPL_MEMCPY(&pCommandFrame->
+						   m_le_abCommandData[0],
+						   pSdoComCon_p->m_pData,
+						   pSdoComCon_p->m_uiTransSize);
+
+					// correct byte-counter
+					pSdoComCon_p->m_uiTransferredByte +=
+					    pSdoComCon_p->m_uiTransSize;
+
+					// move data pointer
+					pSdoComCon_p->m_pData +=
+					    pSdoComCon_p->m_uiTransSize;
+
+					// set segment size
+					AmiSetWordToLe(&pCommandFrame->
+						       m_le_wSegmentSize,
+						       (WORD) pSdoComCon_p->
+						       m_uiTransSize);
+
+					// send frame
+					uiSizeOfFrame +=
+					    pSdoComCon_p->m_uiTransSize;
+					pSdoComCon_p->m_uiTransSize = 0;
+					Ret =
+					    EplSdoAsySeqSendData(pSdoComCon_p->
+								 m_SdoSeqConHdl,
+								 uiSizeOfFrame,
+								 pFrame);
+				}
+
+			}
+			break;
+		}
+		// abort to send
+	case kEplSdoComSendTypeAbort:
+		{
+			// set response and abort flag
+			bFlag = AmiGetByteFromLe(&pCommandFrame->m_le_bFlags);
+			bFlag |= 0xC0;
+			AmiSetByteToLe(&pCommandFrame->m_le_bFlags, bFlag);
+
+			// copy abortcode to frame
+			AmiSetDwordToLe(&pCommandFrame->m_le_abCommandData[0],
+					*((DWORD *) pSdoComCon_p->m_pData));
+
+			// set size of segment
+			AmiSetWordToLe(&pCommandFrame->m_le_wSegmentSize,
+				       sizeof(DWORD));
+
+			// update counter
+			pSdoComCon_p->m_uiTransferredByte = sizeof(DWORD);
+			pSdoComCon_p->m_uiTransSize = 0;
+
+			// calc framesize
+			uiSizeOfFrame += sizeof(DWORD);
+			Ret = EplSdoAsySeqSendData(pSdoComCon_p->m_SdoSeqConHdl,
+						   uiSizeOfFrame, pFrame);
+			break;
+		}
+	}			// end of switch(SendType_p)
+
+      Exit:
+	return Ret;
+}
+#endif
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComServerInitWriteByIndex
+//
+// Description:    function start the processing of an write by index command
+//
+//
+//
+// Parameters:      pSdoComCon_p     = pointer to control structure of connection
+//                  pAsySdoCom_p     = pointer to received frame
+//
+// Returns:         tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+static tEplKernel EplSdoComServerInitWriteByIndex(tEplSdoComCon * pSdoComCon_p,
+						  tEplAsySdoCom * pAsySdoCom_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiIndex;
+	unsigned int uiSubindex;
+	unsigned int uiBytesToTransfer;
+	tEplObdSize EntrySize;
+	tEplObdAccess AccessType;
+	DWORD dwAbortCode;
+	BYTE *pbSrcData;
+
+	dwAbortCode = 0;
+
+	// a init of a write
+	// -> variable part of header possible
+
+	// check if expedited or segmented transfer
+	if ((pAsySdoCom_p->m_le_bFlags & 0x30) == 0x10) {	// initiate segmented transfer
+		pSdoComCon_p->m_SdoTransType = kEplSdoTransSegmented;
+		// get index and subindex
+		uiIndex =
+		    AmiGetWordFromLe(&pAsySdoCom_p->m_le_abCommandData[4]);
+		uiSubindex =
+		    AmiGetByteFromLe(&pAsySdoCom_p->m_le_abCommandData[6]);
+		// get source-pointer for copy
+		pbSrcData = &pAsySdoCom_p->m_le_abCommandData[8];
+		// save size
+		pSdoComCon_p->m_uiTransSize =
+		    AmiGetDwordFromLe(&pAsySdoCom_p->m_le_abCommandData[0]);
+
+	} else if ((pAsySdoCom_p->m_le_bFlags & 0x30) == 0x00) {	// expedited transfer
+		pSdoComCon_p->m_SdoTransType = kEplSdoTransExpedited;
+		// get index and subindex
+		uiIndex =
+		    AmiGetWordFromLe(&pAsySdoCom_p->m_le_abCommandData[0]);
+		uiSubindex =
+		    AmiGetByteFromLe(&pAsySdoCom_p->m_le_abCommandData[2]);
+		// get source-pointer for copy
+		pbSrcData = &pAsySdoCom_p->m_le_abCommandData[4];
+		// save size
+		pSdoComCon_p->m_uiTransSize =
+		    AmiGetWordFromLe(&pAsySdoCom_p->m_le_wSegmentSize);
+		// subtract header
+		pSdoComCon_p->m_uiTransSize -= 4;
+
+	} else {
+		// just ignore any other transfer type
+		goto Exit;
+	}
+
+	// check accesstype of entry
+	// existens of entry
+//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+	Ret = EplObduGetAccessType(uiIndex, uiSubindex, &AccessType);
+/*#else
+    Ret = kEplObdSubindexNotExist;
+    AccessType = 0;
+#endif*/
+	if (Ret == kEplObdSubindexNotExist) {	// subentry doesn't exist
+		pSdoComCon_p->m_dwLastAbortCode = EPL_SDOAC_SUB_INDEX_NOT_EXIST;
+		// send abort
+		// d.k. This is wrong: k.t. not needed send abort on end of write
+		/*pSdoComCon_p->m_pData = (BYTE*)pSdoComCon_p->m_dwLastAbortCode;
+		   Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+		   uiIndex,
+		   uiSubindex,
+		   kEplSdoComSendTypeAbort); */
+		goto Abort;
+	} else if (Ret != kEplSuccessful) {	// entry doesn't exist
+		pSdoComCon_p->m_dwLastAbortCode = EPL_SDOAC_OBJECT_NOT_EXIST;
+		// send abort
+		// d.k. This is wrong: k.t. not needed send abort on end of write
+		/*
+		   pSdoComCon_p->m_pData = (BYTE*)&dwAbortCode;
+		   Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+		   uiIndex,
+		   uiSubindex,
+		   kEplSdoComSendTypeAbort); */
+		goto Abort;
+	}
+	// compare accesstype must be read
+	if ((AccessType & kEplObdAccWrite) == 0) {
+
+		if ((AccessType & kEplObdAccRead) != 0) {
+			// entry write a read only object
+			pSdoComCon_p->m_dwLastAbortCode =
+			    EPL_SDOAC_WRITE_TO_READ_ONLY_OBJ;
+		} else {
+			pSdoComCon_p->m_dwLastAbortCode =
+			    EPL_SDOAC_UNSUPPORTED_ACCESS;
+		}
+		// send abort
+		// d.k. This is wrong: k.t. not needed send abort on end of write
+		/*pSdoComCon_p->m_pData = (BYTE*)&dwAbortCode;
+		   Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+		   uiIndex,
+		   uiSubindex,
+		   kEplSdoComSendTypeAbort); */
+		goto Abort;
+	}
+	// save service
+	pSdoComCon_p->m_SdoServiceType = kEplSdoServiceWriteByIndex;
+
+	pSdoComCon_p->m_uiTransferredByte = 0;
+
+	// write data to OD
+	if (pSdoComCon_p->m_SdoTransType == kEplSdoTransExpedited) {	// expedited transfer
+		// size checking is done by EplObduWriteEntryFromLe()
+
+//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+		Ret = EplObduWriteEntryFromLe(uiIndex,
+					      uiSubindex,
+					      pbSrcData,
+					      pSdoComCon_p->m_uiTransSize);
+		switch (Ret) {
+		case kEplSuccessful:
+			{
+				break;
+			}
+
+		case kEplObdAccessViolation:
+			{
+				pSdoComCon_p->m_dwLastAbortCode =
+				    EPL_SDOAC_UNSUPPORTED_ACCESS;
+				// send abort
+				goto Abort;
+			}
+
+		case kEplObdValueLengthError:
+			{
+				pSdoComCon_p->m_dwLastAbortCode =
+				    EPL_SDOAC_DATA_TYPE_LENGTH_NOT_MATCH;
+				// send abort
+				goto Abort;
+			}
+
+		case kEplObdValueTooHigh:
+			{
+				pSdoComCon_p->m_dwLastAbortCode =
+				    EPL_SDOAC_VALUE_RANGE_TOO_HIGH;
+				// send abort
+				goto Abort;
+			}
+
+		case kEplObdValueTooLow:
+			{
+				pSdoComCon_p->m_dwLastAbortCode =
+				    EPL_SDOAC_VALUE_RANGE_TOO_LOW;
+				// send abort
+				goto Abort;
+			}
+
+		default:
+			{
+				pSdoComCon_p->m_dwLastAbortCode =
+				    EPL_SDOAC_GENERAL_ERROR;
+				// send abort
+				goto Abort;
+			}
+		}
+//#endif
+		// send command acknowledge
+		Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+						     0,
+						     0,
+						     kEplSdoComSendTypeAckRes);
+
+		pSdoComCon_p->m_uiTransSize = 0;
+		goto Exit;
+	} else {
+		// get size of the object to check if it fits
+		// because we directly write to the destination memory
+		// d.k. no one calls the user OD callback function
+
+		//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+		EntrySize = EplObduGetDataSize(uiIndex, uiSubindex);
+		/*#else
+		   EntrySize = 0;
+		   #endif */
+		if (EntrySize < pSdoComCon_p->m_uiTransSize) {	// parameter too big
+			pSdoComCon_p->m_dwLastAbortCode =
+			    EPL_SDOAC_DATA_TYPE_LENGTH_TOO_HIGH;
+			// send abort
+			// d.k. This is wrong: k.t. not needed send abort on end of write
+			/*pSdoComCon_p->m_pData = (BYTE*)&dwAbortCode;
+			   Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+			   uiIndex,
+			   uiSubindex,
+			   kEplSdoComSendTypeAbort); */
+			goto Abort;
+		}
+
+		uiBytesToTransfer =
+		    AmiGetWordFromLe(&pAsySdoCom_p->m_le_wSegmentSize);
+		// eleminate header (Command header (8) + variable part (4) + Command header (4))
+		uiBytesToTransfer -= 16;
+		// get pointer to object entry
+//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+		pSdoComCon_p->m_pData = EplObduGetObjectDataPtr(uiIndex,
+								uiSubindex);
+//#endif
+		if (pSdoComCon_p->m_pData == NULL) {
+			pSdoComCon_p->m_dwLastAbortCode =
+			    EPL_SDOAC_GENERAL_ERROR;
+			// send abort
+			// d.k. This is wrong: k.t. not needed send abort on end of write
+/*            pSdoComCon_p->m_pData = (BYTE*)&pSdoComCon_p->m_dwLastAbortCode;
+            Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+                                        uiIndex,
+                                        uiSubindex,
+                                        kEplSdoComSendTypeAbort);*/
+			goto Abort;
+		}
+		// copy data
+		EPL_MEMCPY(pSdoComCon_p->m_pData, pbSrcData, uiBytesToTransfer);
+
+		// update internal counter
+		pSdoComCon_p->m_uiTransferredByte = uiBytesToTransfer;
+		pSdoComCon_p->m_uiTransSize -= uiBytesToTransfer;
+
+		// update target pointer
+		( /*(BYTE*) */ pSdoComCon_p->m_pData) += uiBytesToTransfer;
+
+		// send acknowledge without any Command layer data
+		Ret = EplSdoAsySeqSendData(pSdoComCon_p->m_SdoSeqConHdl,
+					   0, (tEplFrame *) NULL);
+		goto Exit;
+	}
+
+      Abort:
+	if (pSdoComCon_p->m_dwLastAbortCode != 0) {
+		// send abort
+		pSdoComCon_p->m_pData =
+		    (BYTE *) & pSdoComCon_p->m_dwLastAbortCode;
+		Ret =
+		    EplSdoComServerSendFrameIntern(pSdoComCon_p, uiIndex,
+						   uiSubindex,
+						   kEplSdoComSendTypeAbort);
+
+		// reset abort code
+		pSdoComCon_p->m_dwLastAbortCode = 0;
+		pSdoComCon_p->m_uiTransSize = 0;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComClientSend
+//
+// Description:    function starts an sdo transfer an send all further frames
+//
+//
+//
+// Parameters:      pSdoComCon_p     = pointer to control structure of connection
+//
+// Returns:         tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+static tEplKernel EplSdoComClientSend(tEplSdoComCon * pSdoComCon_p)
+{
+	tEplKernel Ret;
+	BYTE abFrame[EPL_MAX_SDO_FRAME_SIZE];
+	tEplFrame *pFrame;
+	tEplAsySdoCom *pCommandFrame;
+	unsigned int uiSizeOfFrame;
+	BYTE bFlags;
+	BYTE *pbPayload;
+
+	Ret = kEplSuccessful;
+
+	pFrame = (tEplFrame *) & abFrame[0];
+
+	EPL_MEMSET(&abFrame[0], 0x00, sizeof(abFrame));
+
+	// build generic part of frame
+	// get pointer to command layerpart of frame
+	pCommandFrame =
+	    &pFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame.
+	    m_le_abSdoSeqPayload;
+	AmiSetByteToLe(&pCommandFrame->m_le_bCommandId,
+		       pSdoComCon_p->m_SdoServiceType);
+	AmiSetByteToLe(&pCommandFrame->m_le_bTransactionId,
+		       pSdoComCon_p->m_bTransactionId);
+
+	// set size constant part of header
+	uiSizeOfFrame = 8;
+
+	// check if first frame to send -> command header needed
+	if (pSdoComCon_p->m_uiTransSize > 0) {
+		if (pSdoComCon_p->m_uiTransferredByte == 0) {	// start SDO transfer
+			// check if segmented or expedited transfer
+			// only for write commands
+			switch (pSdoComCon_p->m_SdoServiceType) {
+			case kEplSdoServiceReadByIndex:
+				{	// first frame of read access always expedited
+					pSdoComCon_p->m_SdoTransType =
+					    kEplSdoTransExpedited;
+					pbPayload =
+					    &pCommandFrame->
+					    m_le_abCommandData[0];
+					// fill rest of header
+					AmiSetWordToLe(&pCommandFrame->
+						       m_le_wSegmentSize, 4);
+
+					// create command header
+					AmiSetWordToLe(pbPayload,
+						       (WORD) pSdoComCon_p->
+						       m_uiTargetIndex);
+					pbPayload += 2;
+					AmiSetByteToLe(pbPayload,
+						       (BYTE) pSdoComCon_p->
+						       m_uiTargetSubIndex);
+					// calc size
+					uiSizeOfFrame += 4;
+
+					// set pSdoComCon_p->m_uiTransferredByte to one
+					pSdoComCon_p->m_uiTransferredByte = 1;
+					break;
+				}
+
+			case kEplSdoServiceWriteByIndex:
+				{
+					if (pSdoComCon_p->m_uiTransSize > EPL_SDO_MAX_PAYLOAD) {	// segmented transfer
+						// -> variable part of header needed
+						// save that transfer is segmented
+						pSdoComCon_p->m_SdoTransType =
+						    kEplSdoTransSegmented;
+						// fill variable part of header
+						AmiSetDwordToLe(&pCommandFrame->
+								m_le_abCommandData
+								[0],
+								pSdoComCon_p->
+								m_uiTransSize);
+						// set pointer to real payload
+						pbPayload =
+						    &pCommandFrame->
+						    m_le_abCommandData[4];
+						// fill rest of header
+						AmiSetWordToLe(&pCommandFrame->
+							       m_le_wSegmentSize,
+							       EPL_SDO_MAX_PAYLOAD);
+						bFlags = 0x10;
+						AmiSetByteToLe(&pCommandFrame->
+							       m_le_bFlags,
+							       bFlags);
+						// create command header
+						AmiSetWordToLe(pbPayload,
+							       (WORD)
+							       pSdoComCon_p->
+							       m_uiTargetIndex);
+						pbPayload += 2;
+						AmiSetByteToLe(pbPayload,
+							       (BYTE)
+							       pSdoComCon_p->
+							       m_uiTargetSubIndex);
+						// on byte for reserved
+						pbPayload += 2;
+						// calc size
+						uiSizeOfFrame +=
+						    EPL_SDO_MAX_PAYLOAD;
+
+						// copy payload
+						EPL_MEMCPY(pbPayload,
+							   pSdoComCon_p->
+							   m_pData,
+							   (EPL_SDO_MAX_PAYLOAD
+							    - 8));
+						pSdoComCon_p->m_pData +=
+						    (EPL_SDO_MAX_PAYLOAD - 8);
+						// correct intern counter
+						pSdoComCon_p->m_uiTransSize -=
+						    (EPL_SDO_MAX_PAYLOAD - 8);
+						pSdoComCon_p->
+						    m_uiTransferredByte =
+						    (EPL_SDO_MAX_PAYLOAD - 8);
+
+					} else {	// expedited trandsfer
+						// save that transfer is expedited
+						pSdoComCon_p->m_SdoTransType =
+						    kEplSdoTransExpedited;
+						pbPayload =
+						    &pCommandFrame->
+						    m_le_abCommandData[0];
+
+						// create command header
+						AmiSetWordToLe(pbPayload,
+							       (WORD)
+							       pSdoComCon_p->
+							       m_uiTargetIndex);
+						pbPayload += 2;
+						AmiSetByteToLe(pbPayload,
+							       (BYTE)
+							       pSdoComCon_p->
+							       m_uiTargetSubIndex);
+						// + 2 -> one byte for subindex and one byte reserved
+						pbPayload += 2;
+						// copy data
+						EPL_MEMCPY(pbPayload,
+							   pSdoComCon_p->
+							   m_pData,
+							   pSdoComCon_p->
+							   m_uiTransSize);
+						// calc size
+						uiSizeOfFrame +=
+						    (4 +
+						     pSdoComCon_p->
+						     m_uiTransSize);
+						// fill rest of header
+						AmiSetWordToLe(&pCommandFrame->
+							       m_le_wSegmentSize,
+							       (WORD) (4 +
+								       pSdoComCon_p->
+								       m_uiTransSize));
+
+						pSdoComCon_p->
+						    m_uiTransferredByte =
+						    pSdoComCon_p->m_uiTransSize;
+						pSdoComCon_p->m_uiTransSize = 0;
+					}
+					break;
+				}
+
+			case kEplSdoServiceNIL:
+			default:
+				// invalid service requested
+				Ret = kEplSdoComInvalidServiceType;
+				goto Exit;
+			}	// end of switch(pSdoComCon_p->m_SdoServiceType)
+		} else		// (pSdoComCon_p->m_uiTransferredByte > 0)
+		{		// continue SDO transfer
+			switch (pSdoComCon_p->m_SdoServiceType) {
+				// for expedited read is nothing to do
+				// -> server sends data
+
+			case kEplSdoServiceWriteByIndex:
+				{	// send next frame
+					if (pSdoComCon_p->m_SdoTransType ==
+					    kEplSdoTransSegmented) {
+						if (pSdoComCon_p->m_uiTransSize > EPL_SDO_MAX_PAYLOAD) {	// next segment
+							pbPayload =
+							    &pCommandFrame->
+							    m_le_abCommandData
+							    [0];
+							// fill rest of header
+							AmiSetWordToLe
+							    (&pCommandFrame->
+							     m_le_wSegmentSize,
+							     EPL_SDO_MAX_PAYLOAD);
+							bFlags = 0x20;
+							AmiSetByteToLe
+							    (&pCommandFrame->
+							     m_le_bFlags,
+							     bFlags);
+							// copy data
+							EPL_MEMCPY(pbPayload,
+								   pSdoComCon_p->
+								   m_pData,
+								   EPL_SDO_MAX_PAYLOAD);
+							pSdoComCon_p->m_pData +=
+							    EPL_SDO_MAX_PAYLOAD;
+							// correct intern counter
+							pSdoComCon_p->
+							    m_uiTransSize -=
+							    EPL_SDO_MAX_PAYLOAD;
+							pSdoComCon_p->
+							    m_uiTransferredByte
+							    =
+							    EPL_SDO_MAX_PAYLOAD;
+							// calc size
+							uiSizeOfFrame +=
+							    EPL_SDO_MAX_PAYLOAD;
+
+						} else {	// end of transfer
+							pbPayload =
+							    &pCommandFrame->
+							    m_le_abCommandData
+							    [0];
+							// fill rest of header
+							AmiSetWordToLe
+							    (&pCommandFrame->
+							     m_le_wSegmentSize,
+							     (WORD)
+							     pSdoComCon_p->
+							     m_uiTransSize);
+							bFlags = 0x30;
+							AmiSetByteToLe
+							    (&pCommandFrame->
+							     m_le_bFlags,
+							     bFlags);
+							// copy data
+							EPL_MEMCPY(pbPayload,
+								   pSdoComCon_p->
+								   m_pData,
+								   pSdoComCon_p->
+								   m_uiTransSize);
+							pSdoComCon_p->m_pData +=
+							    pSdoComCon_p->
+							    m_uiTransSize;
+							// calc size
+							uiSizeOfFrame +=
+							    pSdoComCon_p->
+							    m_uiTransSize;
+							// correct intern counter
+							pSdoComCon_p->
+							    m_uiTransSize = 0;
+							pSdoComCon_p->
+							    m_uiTransferredByte
+							    =
+							    pSdoComCon_p->
+							    m_uiTransSize;
+
+						}
+					} else {
+						goto Exit;
+					}
+					break;
+				}
+			default:
+				{
+					goto Exit;
+				}
+			}	// end of switch(pSdoComCon_p->m_SdoServiceType)
+		}
+	} else {
+		goto Exit;
+	}
+
+	// call send function of lower layer
+	switch (pSdoComCon_p->m_SdoProtType) {
+	case kEplSdoTypeAsnd:
+	case kEplSdoTypeUdp:
+		{
+			Ret = EplSdoAsySeqSendData(pSdoComCon_p->m_SdoSeqConHdl,
+						   uiSizeOfFrame, pFrame);
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplSdoComUnsupportedProt;
+		}
+	}			// end of switch(pSdoComCon_p->m_SdoProtType)
+
+      Exit:
+	return Ret;
+
+}
+#endif
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComClientProcessFrame
+//
+// Description:    function process a received frame
+//
+//
+//
+// Parameters:      SdoComCon_p      = connection handle
+//                  pAsySdoCom_p     = pointer to frame to process
+//
+// Returns:         tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+static tEplKernel EplSdoComClientProcessFrame(tEplSdoComConHdl SdoComCon_p,
+					      tEplAsySdoCom * pAsySdoCom_p)
+{
+	tEplKernel Ret;
+	BYTE bBuffer;
+	unsigned int uiBuffer;
+	unsigned int uiDataSize;
+	unsigned long ulBuffer;
+	tEplSdoComCon *pSdoComCon;
+
+	Ret = kEplSuccessful;
+
+	// get pointer to control structure
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[SdoComCon_p];
+
+	// check if transaction Id fit
+	bBuffer = AmiGetByteFromLe(&pAsySdoCom_p->m_le_bTransactionId);
+	if (pSdoComCon->m_bTransactionId != bBuffer) {
+		// incorrect transaction id
+
+		// if running transfer
+		if ((pSdoComCon->m_uiTransferredByte != 0)
+		    && (pSdoComCon->m_uiTransSize != 0)) {
+			pSdoComCon->m_dwLastAbortCode = EPL_SDOAC_GENERAL_ERROR;
+			// -> send abort
+			EplSdoComClientSendAbort(pSdoComCon,
+						 pSdoComCon->m_dwLastAbortCode);
+			// call callback of application
+			Ret =
+			    EplSdoComTransferFinished(SdoComCon_p, pSdoComCon,
+						      kEplSdoComTransferTxAborted);
+		}
+
+	} else {		// check if correct command
+		bBuffer = AmiGetByteFromLe(&pAsySdoCom_p->m_le_bCommandId);
+		if (pSdoComCon->m_SdoServiceType != bBuffer) {
+			// incorrect command
+			// if running transfer
+			if ((pSdoComCon->m_uiTransferredByte != 0)
+			    && (pSdoComCon->m_uiTransSize != 0)) {
+				pSdoComCon->m_dwLastAbortCode =
+				    EPL_SDOAC_GENERAL_ERROR;
+				// -> send abort
+				EplSdoComClientSendAbort(pSdoComCon,
+							 pSdoComCon->
+							 m_dwLastAbortCode);
+				// call callback of application
+				Ret =
+				    EplSdoComTransferFinished(SdoComCon_p,
+							      pSdoComCon,
+							      kEplSdoComTransferTxAborted);
+			}
+
+		} else {	// switch on command
+			switch (pSdoComCon->m_SdoServiceType) {
+			case kEplSdoServiceWriteByIndex:
+				{	// check if confirmation from server
+					// nothing more to do
+					break;
+				}
+
+			case kEplSdoServiceReadByIndex:
+				{	// check if it is an segmented or an expedited transfer
+					bBuffer =
+					    AmiGetByteFromLe(&pAsySdoCom_p->
+							     m_le_bFlags);
+					// mask uninteressting bits
+					bBuffer &= 0x30;
+					switch (bBuffer) {
+						// expedited transfer
+					case 0x00:
+						{
+							// check size of buffer
+							uiBuffer =
+							    AmiGetWordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_wSegmentSize);
+							if (uiBuffer > pSdoComCon->m_uiTransSize) {	// buffer provided by the application is to small
+								// copy only a part
+								uiDataSize =
+								    pSdoComCon->
+								    m_uiTransSize;
+							} else {	// buffer fits
+								uiDataSize =
+								    uiBuffer;
+							}
+
+							// copy data
+							EPL_MEMCPY(pSdoComCon->
+								   m_pData,
+								   &pAsySdoCom_p->
+								   m_le_abCommandData
+								   [0],
+								   uiDataSize);
+
+							// correct counter
+							pSdoComCon->
+							    m_uiTransSize = 0;
+							pSdoComCon->
+							    m_uiTransferredByte
+							    = uiDataSize;
+							break;
+						}
+
+						// start of a segmented transfer
+					case 0x10:
+						{	// get total size of transfer
+							ulBuffer =
+							    AmiGetDwordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_abCommandData
+							     [0]);
+							if (ulBuffer <= pSdoComCon->m_uiTransSize) {	// buffer fit
+								pSdoComCon->
+								    m_uiTransSize
+								    =
+								    (unsigned
+								     int)
+								    ulBuffer;
+							} else {	// buffer to small
+								// send abort
+								pSdoComCon->
+								    m_dwLastAbortCode
+								    =
+								    EPL_SDOAC_DATA_TYPE_LENGTH_TOO_HIGH;
+								// -> send abort
+								EplSdoComClientSendAbort
+								    (pSdoComCon,
+								     pSdoComCon->
+								     m_dwLastAbortCode);
+								// call callback of application
+								Ret =
+								    EplSdoComTransferFinished
+								    (SdoComCon_p,
+								     pSdoComCon,
+								     kEplSdoComTransferRxAborted);
+								goto Exit;
+							}
+
+							// get segment size
+							// check size of buffer
+							uiBuffer =
+							    AmiGetWordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_wSegmentSize);
+							// subtract size of vaiable header from datasize
+							uiBuffer -= 4;
+							// copy data
+							EPL_MEMCPY(pSdoComCon->
+								   m_pData,
+								   &pAsySdoCom_p->
+								   m_le_abCommandData
+								   [4],
+								   uiBuffer);
+
+							// correct counter an pointer
+							pSdoComCon->m_pData +=
+							    uiBuffer;
+							pSdoComCon->
+							    m_uiTransferredByte
+							    += uiBuffer;
+							pSdoComCon->
+							    m_uiTransSize -=
+							    uiBuffer;
+
+							break;
+						}
+
+						// segment
+					case 0x20:
+						{
+							// get segment size
+							// check size of buffer
+							uiBuffer =
+							    AmiGetWordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_wSegmentSize);
+							// check if data to copy fit to buffer
+							if (uiBuffer >= pSdoComCon->m_uiTransSize) {	// to much data
+								uiBuffer =
+								    (pSdoComCon->
+								     m_uiTransSize
+								     - 1);
+							}
+							// copy data
+							EPL_MEMCPY(pSdoComCon->
+								   m_pData,
+								   &pAsySdoCom_p->
+								   m_le_abCommandData
+								   [0],
+								   uiBuffer);
+
+							// correct counter an pointer
+							pSdoComCon->m_pData +=
+							    uiBuffer;
+							pSdoComCon->
+							    m_uiTransferredByte
+							    += uiBuffer;
+							pSdoComCon->
+							    m_uiTransSize -=
+							    uiBuffer;
+							break;
+						}
+
+						// last segment
+					case 0x30:
+						{
+							// get segment size
+							// check size of buffer
+							uiBuffer =
+							    AmiGetWordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_wSegmentSize);
+							// check if data to copy fit to buffer
+							if (uiBuffer > pSdoComCon->m_uiTransSize) {	// to much data
+								uiBuffer =
+								    (pSdoComCon->
+								     m_uiTransSize
+								     - 1);
+							}
+							// copy data
+							EPL_MEMCPY(pSdoComCon->
+								   m_pData,
+								   &pAsySdoCom_p->
+								   m_le_abCommandData
+								   [0],
+								   uiBuffer);
+
+							// correct counter an pointer
+							pSdoComCon->m_pData +=
+							    uiBuffer;
+							pSdoComCon->
+							    m_uiTransferredByte
+							    += uiBuffer;
+							pSdoComCon->
+							    m_uiTransSize = 0;
+
+							break;
+						}
+					}	// end of switch(bBuffer & 0x30)
+
+					break;
+				}
+
+			case kEplSdoServiceNIL:
+			default:
+				// invalid service requested
+				// $$$ d.k. What should we do?
+				break;
+			}	// end of switch(pSdoComCon->m_SdoServiceType)
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComClientSendAbort
+//
+// Description: function send a abort message
+//
+//
+//
+// Parameters:  pSdoComCon_p     = pointer to control structure of connection
+//              dwAbortCode_p    = Sdo abort code
+//
+// Returns:     tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+static tEplKernel EplSdoComClientSendAbort(tEplSdoComCon * pSdoComCon_p,
+					   DWORD dwAbortCode_p)
+{
+	tEplKernel Ret;
+	BYTE abFrame[EPL_MAX_SDO_FRAME_SIZE];
+	tEplFrame *pFrame;
+	tEplAsySdoCom *pCommandFrame;
+	unsigned int uiSizeOfFrame;
+
+	Ret = kEplSuccessful;
+
+	pFrame = (tEplFrame *) & abFrame[0];
+
+	EPL_MEMSET(&abFrame[0], 0x00, sizeof(abFrame));
+
+	// build generic part of frame
+	// get pointer to command layerpart of frame
+	pCommandFrame =
+	    &pFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame.
+	    m_le_abSdoSeqPayload;
+	AmiSetByteToLe(&pCommandFrame->m_le_bCommandId,
+		       pSdoComCon_p->m_SdoServiceType);
+	AmiSetByteToLe(&pCommandFrame->m_le_bTransactionId,
+		       pSdoComCon_p->m_bTransactionId);
+
+	uiSizeOfFrame = 8;
+
+	// set response and abort flag
+	pCommandFrame->m_le_bFlags |= 0x40;
+
+	// copy abortcode to frame
+	AmiSetDwordToLe(&pCommandFrame->m_le_abCommandData[0], dwAbortCode_p);
+
+	// set size of segment
+	AmiSetWordToLe(&pCommandFrame->m_le_wSegmentSize, sizeof(DWORD));
+
+	// update counter
+	pSdoComCon_p->m_uiTransferredByte = sizeof(DWORD);
+	pSdoComCon_p->m_uiTransSize = 0;
+
+	// calc framesize
+	uiSizeOfFrame += sizeof(DWORD);
+
+	// save abort code
+	pSdoComCon_p->m_dwLastAbortCode = dwAbortCode_p;
+
+	// call send function of lower layer
+	switch (pSdoComCon_p->m_SdoProtType) {
+	case kEplSdoTypeAsnd:
+	case kEplSdoTypeUdp:
+		{
+			Ret = EplSdoAsySeqSendData(pSdoComCon_p->m_SdoSeqConHdl,
+						   uiSizeOfFrame, pFrame);
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplSdoComUnsupportedProt;
+		}
+	}			// end of switch(pSdoComCon_p->m_SdoProtType)
+
+	return Ret;
+}
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComTransferFinished
+//
+// Description: calls callback function of application if available
+//              and clears entry in control structure
+//
+// Parameters:  pSdoComCon_p     = pointer to control structure of connection
+//              SdoComConState_p = state of SDO transfer
+//
+// Returns:     tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoComTransferFinished(tEplSdoComConHdl SdoComCon_p,
+					    tEplSdoComCon * pSdoComCon_p,
+					    tEplSdoComConState SdoComConState_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	if (pSdoComCon_p->m_pfnTransferFinished != NULL) {
+		tEplSdoFinishedCb pfnTransferFinished;
+		tEplSdoComFinished SdoComFinished;
+
+		SdoComFinished.m_pUserArg = pSdoComCon_p->m_pUserArg;
+		SdoComFinished.m_uiNodeId = pSdoComCon_p->m_uiNodeId;
+		SdoComFinished.m_uiTargetIndex = pSdoComCon_p->m_uiTargetIndex;
+		SdoComFinished.m_uiTargetSubIndex =
+		    pSdoComCon_p->m_uiTargetSubIndex;
+		SdoComFinished.m_uiTransferredByte =
+		    pSdoComCon_p->m_uiTransferredByte;
+		SdoComFinished.m_dwAbortCode = pSdoComCon_p->m_dwLastAbortCode;
+		SdoComFinished.m_SdoComConHdl = SdoComCon_p;
+		SdoComFinished.m_SdoComConState = SdoComConState_p;
+		if (pSdoComCon_p->m_SdoServiceType ==
+		    kEplSdoServiceWriteByIndex) {
+			SdoComFinished.m_SdoAccessType = kEplSdoAccessTypeWrite;
+		} else {
+			SdoComFinished.m_SdoAccessType = kEplSdoAccessTypeRead;
+		}
+
+		// reset transfer state so this handle is not busy anymore
+		pSdoComCon_p->m_uiTransferredByte = 0;
+		pSdoComCon_p->m_uiTransSize = 0;
+
+		pfnTransferFinished = pSdoComCon_p->m_pfnTransferFinished;
+		// delete function pointer to inform application only once for each transfer
+		pSdoComCon_p->m_pfnTransferFinished = NULL;
+
+		// call application's callback function
+		pfnTransferFinished(&SdoComFinished);
+
+	}
+
+	return Ret;
+}
+
+// EOF
diff --git a/drivers/staging/epl/EplSdoUdpu.c b/drivers/staging/epl/EplSdoUdpu.c
new file mode 100644
index 000000000000..be52233b3ee8
--- /dev/null
+++ b/drivers/staging/epl/EplSdoUdpu.c
@@ -0,0 +1,790 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for SDO/UDP-Protocolabstractionlayer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoUdpu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/26 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplSdoUdpu.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+
+#if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+#include "SocketLinuxKernel.h"
+#include <linux/completion.h>
+#include <linux/sched.h>
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef EPL_SDO_MAX_CONNECTION_UDP
+#define EPL_SDO_MAX_CONNECTION_UDP  5
+#endif
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	unsigned long m_ulIpAddr;	// in network byte order
+	unsigned int m_uiPort;	// in network byte order
+
+} tEplSdoUdpCon;
+
+// instance table
+typedef struct {
+	tEplSdoUdpCon m_aSdoAbsUdpConnection[EPL_SDO_MAX_CONNECTION_UDP];
+	tEplSequLayerReceiveCb m_fpSdoAsySeqCb;
+	SOCKET m_UdpSocket;
+
+#if (TARGET_SYSTEM == _WIN32_)
+	HANDLE m_ThreadHandle;
+	LPCRITICAL_SECTION m_pCriticalSection;
+	CRITICAL_SECTION m_CriticalSection;
+
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+	struct completion m_CompletionUdpThread;
+	int m_ThreadHandle;
+	int m_iTerminateThread;
+#endif
+
+} tEplSdoUdpInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+static tEplSdoUdpInstance SdoUdpInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+#if (TARGET_SYSTEM == _WIN32_)
+static DWORD PUBLIC EplSdoUdpThread(LPVOID lpParameter);
+
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+static int EplSdoUdpThread(void *pArg_p);
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <EPL-SDO-UDP-Layer>                                 */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: Protocolabstraction layer for UDP
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoUdpuInit
+//
+// Description: init first instance of the module
+//
+//
+//
+// Parameters:  pReceiveCb_p    =   functionpointer to Sdo-Sequence layer
+//                                  callback-function
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoUdpuInit(tEplSequLayerReceiveCb fpReceiveCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplSdoUdpuAddInstance(fpReceiveCb_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoUdpuAddInstance
+//
+// Description: init additional instance of the module
+//              înit socket and start Listen-Thread
+//
+//
+//
+// Parameters:  pReceiveCb_p    =   functionpointer to Sdo-Sequence layer
+//                                  callback-function
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoUdpuAddInstance(tEplSequLayerReceiveCb fpReceiveCb_p)
+{
+	tEplKernel Ret;
+
+#if (TARGET_SYSTEM == _WIN32_)
+	int iError;
+	WSADATA Wsa;
+
+#endif
+
+	// set instance variables to 0
+	EPL_MEMSET(&SdoUdpInstance_g, 0x00, sizeof(SdoUdpInstance_g));
+
+	Ret = kEplSuccessful;
+
+	// save pointer to callback-function
+	if (fpReceiveCb_p != NULL) {
+		SdoUdpInstance_g.m_fpSdoAsySeqCb = fpReceiveCb_p;
+	} else {
+		Ret = kEplSdoUdpMissCb;
+		goto Exit;
+	}
+
+#if (TARGET_SYSTEM == _WIN32_)
+	// start winsock2 for win32
+	// windows specific start of socket
+	iError = WSAStartup(MAKEWORD(2, 0), &Wsa);
+	if (iError != 0) {
+		Ret = kEplSdoUdpNoSocket;
+		goto Exit;
+	}
+	// create critical section for acccess of instnace variables
+	SdoUdpInstance_g.m_pCriticalSection =
+	    &SdoUdpInstance_g.m_CriticalSection;
+	InitializeCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
+
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+	init_completion(&SdoUdpInstance_g.m_CompletionUdpThread);
+	SdoUdpInstance_g.m_iTerminateThread = 0;
+#endif
+
+	SdoUdpInstance_g.m_ThreadHandle = 0;
+	SdoUdpInstance_g.m_UdpSocket = INVALID_SOCKET;
+
+	Ret = EplSdoUdpuConfig(INADDR_ANY, 0);
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoUdpuDelInstance
+//
+// Description: del instance of the module
+//              del socket and del Listen-Thread
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoUdpuDelInstance()
+{
+	tEplKernel Ret;
+
+#if (TARGET_SYSTEM == _WIN32_)
+	BOOL fTermError;
+#endif
+
+	Ret = kEplSuccessful;
+
+	if (SdoUdpInstance_g.m_ThreadHandle != 0) {	// listen thread was started
+		// close thread
+#if (TARGET_SYSTEM == _WIN32_)
+		fTermError =
+		    TerminateThread(SdoUdpInstance_g.m_ThreadHandle, 0);
+		if (fTermError == FALSE) {
+			Ret = kEplSdoUdpThreadError;
+			goto Exit;
+		}
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+		SdoUdpInstance_g.m_iTerminateThread = 1;
+		/* kill_proc(SdoUdpInstance_g.m_ThreadHandle, SIGTERM, 1 ); */
+		send_sig(SIGTERM, SdoUdpInstance_g.m_ThreadHandle, 1);
+		wait_for_completion(&SdoUdpInstance_g.m_CompletionUdpThread);
+#endif
+
+		SdoUdpInstance_g.m_ThreadHandle = 0;
+	}
+
+	if (SdoUdpInstance_g.m_UdpSocket != INVALID_SOCKET) {
+		// close socket
+		closesocket(SdoUdpInstance_g.m_UdpSocket);
+		SdoUdpInstance_g.m_UdpSocket = INVALID_SOCKET;
+	}
+#if (TARGET_SYSTEM == _WIN32_)
+	// delete critical section
+	DeleteCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
+#endif
+
+#if (TARGET_SYSTEM == _WIN32_)
+	// for win 32
+	WSACleanup();
+#endif
+
+#if (TARGET_SYSTEM == _WIN32_)
+      Exit:
+#endif
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoUdpuConfig
+//
+// Description: reconfigurate socket with new IP-Address
+//              -> needed for NMT ResetConfiguration
+//
+// Parameters:  ulIpAddr_p      = IpAddress in platform byte order
+//              uiPort_p        = port number in platform byte order
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoUdpuConfig(unsigned long ulIpAddr_p,
+				   unsigned int uiPort_p)
+{
+	tEplKernel Ret;
+	struct sockaddr_in Addr;
+	int iError;
+
+#if (TARGET_SYSTEM == _WIN32_)
+	BOOL fTermError;
+	unsigned long ulThreadId;
+#endif
+
+	Ret = kEplSuccessful;
+
+	if (uiPort_p == 0) {	// set UDP port to default port number
+		uiPort_p = EPL_C_SDO_EPL_PORT;
+	} else if (uiPort_p > 65535) {
+		Ret = kEplSdoUdpSocketError;
+		goto Exit;
+	}
+
+	if (SdoUdpInstance_g.m_ThreadHandle != 0) {	// listen thread was started
+
+		// close old thread
+#if (TARGET_SYSTEM == _WIN32_)
+		fTermError =
+		    TerminateThread(SdoUdpInstance_g.m_ThreadHandle, 0);
+		if (fTermError == FALSE) {
+			Ret = kEplSdoUdpThreadError;
+			goto Exit;
+		}
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+		SdoUdpInstance_g.m_iTerminateThread = 1;
+		/* kill_proc(SdoUdpInstance_g.m_ThreadHandle, SIGTERM, 1 ); */
+		send_sig(SIGTERM, SdoUdpInstance_g.m_ThreadHandle, 1);
+		wait_for_completion(&SdoUdpInstance_g.m_CompletionUdpThread);
+		SdoUdpInstance_g.m_iTerminateThread = 0;
+#endif
+
+		SdoUdpInstance_g.m_ThreadHandle = 0;
+	}
+
+	if (SdoUdpInstance_g.m_UdpSocket != INVALID_SOCKET) {
+		// close socket
+		iError = closesocket(SdoUdpInstance_g.m_UdpSocket);
+		SdoUdpInstance_g.m_UdpSocket = INVALID_SOCKET;
+		if (iError != 0) {
+			Ret = kEplSdoUdpSocketError;
+			goto Exit;
+		}
+	}
+	// create Socket
+	SdoUdpInstance_g.m_UdpSocket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
+	if (SdoUdpInstance_g.m_UdpSocket == INVALID_SOCKET) {
+		Ret = kEplSdoUdpNoSocket;
+		EPL_DBGLVL_SDO_TRACE0("EplSdoUdpuConfig: socket() failed\n");
+		goto Exit;
+	}
+	// bind socket
+	Addr.sin_family = AF_INET;
+	Addr.sin_port = htons((unsigned short)uiPort_p);
+	Addr.sin_addr.s_addr = htonl(ulIpAddr_p);
+	iError =
+	    bind(SdoUdpInstance_g.m_UdpSocket, (struct sockaddr *)&Addr,
+		 sizeof(Addr));
+	if (iError < 0) {
+		//iError = WSAGetLastError();
+		EPL_DBGLVL_SDO_TRACE1
+		    ("EplSdoUdpuConfig: bind() finished with %i\n", iError);
+		Ret = kEplSdoUdpNoSocket;
+		goto Exit;
+	}
+	// create Listen-Thread
+#if (TARGET_SYSTEM == _WIN32_)
+	// for win32
+
+	// create thread
+	SdoUdpInstance_g.m_ThreadHandle = CreateThread(NULL,
+						       0,
+						       EplSdoUdpThread,
+						       &SdoUdpInstance_g,
+						       0, &ulThreadId);
+	if (SdoUdpInstance_g.m_ThreadHandle == NULL) {
+		Ret = kEplSdoUdpThreadError;
+		goto Exit;
+	}
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+
+	SdoUdpInstance_g.m_ThreadHandle =
+	    kernel_thread(EplSdoUdpThread, &SdoUdpInstance_g, CLONE_KERNEL);
+	if (SdoUdpInstance_g.m_ThreadHandle == 0) {
+		Ret = kEplSdoUdpThreadError;
+		goto Exit;
+	}
+#endif
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoUdpuInitCon
+//
+// Description: init a new connect
+//
+//
+//
+// Parameters:  pSdoConHandle_p = pointer for the new connection handle
+//              uiTargetNodeId_p = NodeId of the target node
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoUdpuInitCon(tEplSdoConHdl * pSdoConHandle_p,
+				    unsigned int uiTargetNodeId_p)
+{
+	tEplKernel Ret;
+	unsigned int uiCount;
+	unsigned int uiFreeCon;
+	tEplSdoUdpCon *pSdoUdpCon;
+
+	Ret = kEplSuccessful;
+
+	// get free entry in control structure
+	uiCount = 0;
+	uiFreeCon = EPL_SDO_MAX_CONNECTION_UDP;
+	pSdoUdpCon = &SdoUdpInstance_g.m_aSdoAbsUdpConnection[0];
+	while (uiCount < EPL_SDO_MAX_CONNECTION_UDP) {
+		if ((pSdoUdpCon->m_ulIpAddr & htonl(0xFF)) == htonl(uiTargetNodeId_p)) {	// existing connection to target node found
+			// set handle
+			*pSdoConHandle_p = (uiCount | EPL_SDO_UDP_HANDLE);
+
+			goto Exit;
+		} else if ((pSdoUdpCon->m_ulIpAddr == 0)
+			   && (pSdoUdpCon->m_uiPort == 0)) {
+			uiFreeCon = uiCount;
+		}
+		uiCount++;
+		pSdoUdpCon++;
+	}
+
+	if (uiFreeCon == EPL_SDO_MAX_CONNECTION_UDP) {
+		// error no free handle
+		Ret = kEplSdoUdpNoFreeHandle;
+	} else {
+		pSdoUdpCon =
+		    &SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiFreeCon];
+		// save infos for connection
+		pSdoUdpCon->m_uiPort = htons(EPL_C_SDO_EPL_PORT);
+		pSdoUdpCon->m_ulIpAddr = htonl(0xC0A86400 | uiTargetNodeId_p);	// 192.168.100.uiTargetNodeId_p
+
+		// set handle
+		*pSdoConHandle_p = (uiFreeCon | EPL_SDO_UDP_HANDLE);
+
+	}
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoUdpuSendData
+//
+// Description: send data using exisiting connection
+//
+//
+//
+// Parameters:  SdoConHandle_p  = connection handle
+//              pSrcData_p      = pointer to data
+//              dwDataSize_p    = number of databyte
+//                                  -> without asend-header!!!
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoUdpuSendData(tEplSdoConHdl SdoConHandle_p,
+				     tEplFrame * pSrcData_p, DWORD dwDataSize_p)
+{
+	tEplKernel Ret;
+	int iError;
+	unsigned int uiArray;
+	struct sockaddr_in Addr;
+
+	Ret = kEplSuccessful;
+
+	uiArray = (SdoConHandle_p & ~EPL_SDO_ASY_HANDLE_MASK);
+	if (uiArray >= EPL_SDO_MAX_CONNECTION_UDP) {
+		Ret = kEplSdoUdpInvalidHdl;
+		goto Exit;
+	}
+	//set message type
+	AmiSetByteToLe(&pSrcData_p->m_le_bMessageType, 0x06);	// SDO
+	// target node id (for Udp = 0)
+	AmiSetByteToLe(&pSrcData_p->m_le_bDstNodeId, 0x00);
+	// set source-nodeid (for Udp = 0)
+	AmiSetByteToLe(&pSrcData_p->m_le_bSrcNodeId, 0x00);
+
+	// calc size
+	dwDataSize_p += EPL_ASND_HEADER_SIZE;
+
+	// call sendto
+	Addr.sin_family = AF_INET;
+#if (TARGET_SYSTEM == _WIN32_)
+	// enter  critical section for process function
+	EnterCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
+#endif
+
+	Addr.sin_port =
+	    (unsigned short)SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiArray].
+	    m_uiPort;
+	Addr.sin_addr.s_addr =
+	    SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiArray].m_ulIpAddr;
+
+#if (TARGET_SYSTEM == _WIN32_)
+	// leave critical section for process function
+	LeaveCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
+#endif
+
+	iError = sendto(SdoUdpInstance_g.m_UdpSocket,	// sockethandle
+			(const char *)&pSrcData_p->m_le_bMessageType,	// data to send
+			dwDataSize_p,	// number of bytes to send
+			0,	// flags
+			(struct sockaddr *)&Addr,	// target
+			sizeof(struct sockaddr_in));	// sizeof targetadress
+	if (iError < 0) {
+		EPL_DBGLVL_SDO_TRACE1
+		    ("EplSdoUdpuSendData: sendto() finished with %i\n", iError);
+		Ret = kEplSdoUdpSendError;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoUdpuDelCon
+//
+// Description: delete connection from intern structure
+//
+//
+//
+// Parameters:  SdoConHandle_p  = connection handle
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoUdpuDelCon(tEplSdoConHdl SdoConHandle_p)
+{
+	tEplKernel Ret;
+	unsigned int uiArray;
+
+	uiArray = (SdoConHandle_p & ~EPL_SDO_ASY_HANDLE_MASK);
+
+	if (uiArray >= EPL_SDO_MAX_CONNECTION_UDP) {
+		Ret = kEplSdoUdpInvalidHdl;
+		goto Exit;
+	} else {
+		Ret = kEplSuccessful;
+	}
+
+	// delete connection
+	SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiArray].m_ulIpAddr = 0;
+	SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiArray].m_uiPort = 0;
+
+      Exit:
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoUdpThread
+//
+// Description:     thread check socket for new data
+//
+//
+//
+// Parameters:      lpParameter = pointer to parameter type tEplSdoUdpThreadPara
+//
+//
+// Returns:         DWORD   =   errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if (TARGET_SYSTEM == _WIN32_)
+static DWORD PUBLIC EplSdoUdpThread(LPVOID lpParameter)
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+static int EplSdoUdpThread(void *pArg_p)
+#endif
+{
+
+	tEplSdoUdpInstance *pInstance;
+	struct sockaddr_in RemoteAddr;
+	int iError;
+	int iCount;
+	int iFreeEntry;
+	BYTE abBuffer[EPL_MAX_SDO_REC_FRAME_SIZE];
+	unsigned int uiSize;
+	tEplSdoConHdl SdoConHdl;
+
+#if (TARGET_SYSTEM == _WIN32_)
+	pInstance = (tEplSdoUdpInstance *) lpParameter;
+
+	for (;;)
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+	pInstance = (tEplSdoUdpInstance *) pArg_p;
+	daemonize("EplSdoUdpThread");
+	allow_signal(SIGTERM);
+
+	for (; pInstance->m_iTerminateThread == 0;)
+#endif
+
+	{
+		// wait for data
+		uiSize = sizeof(struct sockaddr);
+		iError = recvfrom(pInstance->m_UdpSocket,	// Socket
+				  (char *)&abBuffer[0],	// buffer for data
+				  sizeof(abBuffer),	// size of the buffer
+				  0,	// flags
+				  (struct sockaddr *)&RemoteAddr,
+				  (int *)&uiSize);
+#if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+		if (iError == -ERESTARTSYS) {
+			break;
+		}
+#endif
+		if (iError > 0) {
+			// get handle for higher layer
+			iCount = 0;
+			iFreeEntry = 0xFFFF;
+#if (TARGET_SYSTEM == _WIN32_)
+			// enter  critical section for process function
+			EnterCriticalSection(SdoUdpInstance_g.
+					     m_pCriticalSection);
+#endif
+			while (iCount < EPL_SDO_MAX_CONNECTION_UDP) {
+				// check if this connection is already known
+				if ((pInstance->m_aSdoAbsUdpConnection[iCount].
+				     m_ulIpAddr == RemoteAddr.sin_addr.s_addr)
+				    && (pInstance->
+					m_aSdoAbsUdpConnection[iCount].
+					m_uiPort == RemoteAddr.sin_port)) {
+					break;
+				}
+
+				if ((pInstance->m_aSdoAbsUdpConnection[iCount].
+				     m_ulIpAddr == 0)
+				    && (pInstance->
+					m_aSdoAbsUdpConnection[iCount].
+					m_uiPort == 0)
+				    && (iFreeEntry == 0xFFFF))
+				{
+					iFreeEntry = iCount;
+				}
+
+				iCount++;
+			}
+
+			if (iCount == EPL_SDO_MAX_CONNECTION_UDP) {
+				// connection unknown
+				// see if there is a free handle
+				if (iFreeEntry != 0xFFFF) {
+					// save adress infos
+					pInstance->
+					    m_aSdoAbsUdpConnection[iFreeEntry].
+					    m_ulIpAddr =
+					    RemoteAddr.sin_addr.s_addr;
+					pInstance->
+					    m_aSdoAbsUdpConnection[iFreeEntry].
+					    m_uiPort = RemoteAddr.sin_port;
+#if (TARGET_SYSTEM == _WIN32_)
+					// leave critical section for process function
+					LeaveCriticalSection(SdoUdpInstance_g.
+							     m_pCriticalSection);
+#endif
+					// call callback
+					SdoConHdl = iFreeEntry;
+					SdoConHdl |= EPL_SDO_UDP_HANDLE;
+					// offset 4 -> start of SDO Sequence header
+					pInstance->m_fpSdoAsySeqCb(SdoConHdl,
+								   (tEplAsySdoSeq
+								    *) &
+								   abBuffer[4],
+								   (iError -
+								    4));
+				} else {
+					EPL_DBGLVL_SDO_TRACE0
+					    ("Error in EplSdoUdpThread() no free handle\n");
+#if (TARGET_SYSTEM == _WIN32_)
+					// leave critical section for process function
+					LeaveCriticalSection(SdoUdpInstance_g.
+							     m_pCriticalSection);
+#endif
+				}
+
+			} else {
+				// known connection
+				// call callback with correct handle
+				SdoConHdl = iCount;
+				SdoConHdl |= EPL_SDO_UDP_HANDLE;
+#if (TARGET_SYSTEM == _WIN32_)
+				// leave critical section for process function
+				LeaveCriticalSection(SdoUdpInstance_g.
+						     m_pCriticalSection);
+#endif
+				// offset 4 -> start of SDO Sequence header
+				pInstance->m_fpSdoAsySeqCb(SdoConHdl,
+							   (tEplAsySdoSeq *) &
+							   abBuffer[4],
+							   (iError - 4));
+			}
+		}		// end of  if(iError!=SOCKET_ERROR)
+	}			// end of for(;;)
+
+#if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+	complete_and_exit(&SdoUdpInstance_g.m_CompletionUdpThread, 0);
+#endif
+
+	return 0;
+}
+
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplStatusu.c b/drivers/staging/epl/EplStatusu.c
new file mode 100644
index 000000000000..689f9124ae29
--- /dev/null
+++ b/drivers/staging/epl/EplStatusu.c
@@ -0,0 +1,380 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for Statusu-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplStatusu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/11/15 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplStatusu.h"
+#include "user/EplDlluCal.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <xxxxx>                                             */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	tEplStatusuCbResponse m_apfnCbResponse[254];
+
+} tEplStatusuInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+static tEplStatusuInstance EplStatusuInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplStatusuCbStatusResponse(tEplFrameInfo *
+						    pFrameInfo_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplStatusuInit
+//
+// Description: init first instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplStatusuInit()
+{
+	tEplKernel Ret;
+
+	Ret = EplStatusuAddInstance();
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplStatusuAddInstance
+//
+// Description: init other instances of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplStatusuAddInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// reset instance structure
+	EPL_MEMSET(&EplStatusuInstance_g, 0, sizeof(EplStatusuInstance_g));
+
+	// register StatusResponse callback function
+	Ret =
+	    EplDlluCalRegAsndService(kEplDllAsndStatusResponse,
+				     EplStatusuCbStatusResponse,
+				     kEplDllAsndFilterAny);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplStatusuDelInstance
+//
+// Description: delete instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplStatusuDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// deregister StatusResponse callback function
+	Ret =
+	    EplDlluCalRegAsndService(kEplDllAsndStatusResponse, NULL,
+				     kEplDllAsndFilterNone);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplStatusuReset
+//
+// Description: resets this instance
+//
+// Parameters:
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplStatusuReset()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// reset instance structure
+	EPL_MEMSET(&EplStatusuInstance_g, 0, sizeof(EplStatusuInstance_g));
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplStatusuRequestStatusResponse
+//
+// Description: returns the StatusResponse for the specified node.
+//
+// Parameters:  uiNodeId_p                  = IN: node ID
+//              pfnCbResponse_p             = IN: function pointer to callback function
+//                                            which will be called if StatusResponse is received
+//
+// Return:      tEplKernel                  = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplStatusuRequestStatusResponse(unsigned int uiNodeId_p,
+						  tEplStatusuCbResponse
+						  pfnCbResponse_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// decrement node ID, because array is zero based
+	uiNodeId_p--;
+	if (uiNodeId_p < tabentries(EplStatusuInstance_g.m_apfnCbResponse)) {
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+		if (EplStatusuInstance_g.m_apfnCbResponse[uiNodeId_p] != NULL) {	// request already issued (maybe by someone else)
+			Ret = kEplInvalidOperation;
+		} else {
+			EplStatusuInstance_g.m_apfnCbResponse[uiNodeId_p] =
+			    pfnCbResponse_p;
+			Ret =
+			    EplDlluCalIssueRequest(kEplDllReqServiceStatus,
+						   (uiNodeId_p + 1), 0xFF);
+		}
+#else
+		Ret = kEplInvalidOperation;
+#endif
+	} else {		// invalid node ID specified
+		Ret = kEplInvalidNodeId;
+	}
+
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplStatusuCbStatusResponse
+//
+// Description: callback funktion for StatusResponse
+//
+//
+//
+// Parameters:  pFrameInfo_p            = Frame with the StatusResponse
+//
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel PUBLIC EplStatusuCbStatusResponse(tEplFrameInfo *
+						    pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiNodeId;
+	unsigned int uiIndex;
+	tEplStatusuCbResponse pfnCbResponse;
+
+	uiNodeId = AmiGetByteFromLe(&pFrameInfo_p->m_pFrame->m_le_bSrcNodeId);
+
+	uiIndex = uiNodeId - 1;
+
+	if (uiIndex < tabentries(EplStatusuInstance_g.m_apfnCbResponse)) {
+		// memorize pointer to callback function
+		pfnCbResponse = EplStatusuInstance_g.m_apfnCbResponse[uiIndex];
+		if (pfnCbResponse == NULL) {	// response was not requested
+			goto Exit;
+		}
+		// reset callback function pointer so that caller may issue next request
+		EplStatusuInstance_g.m_apfnCbResponse[uiIndex] = NULL;
+
+		if (pFrameInfo_p->m_uiFrameSize < EPL_C_DLL_MINSIZE_STATUSRES) {	// StatusResponse not received or it has invalid size
+			Ret = pfnCbResponse(uiNodeId, NULL);
+		} else {	// StatusResponse received
+			Ret =
+			    pfnCbResponse(uiNodeId,
+					  &pFrameInfo_p->m_pFrame->m_Data.
+					  m_Asnd.m_Payload.m_StatusResponse);
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+// EOF
diff --git a/drivers/staging/epl/EplTarget.h b/drivers/staging/epl/EplTarget.h
new file mode 100644
index 000000000000..b2b66f82c035
--- /dev/null
+++ b/drivers/staging/epl/EplTarget.h
@@ -0,0 +1,233 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for target api function
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTarget.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2005/12/05 -as:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPLTARGET_H_
+#define _EPLTARGET_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+// =========================================================================
+// macros for memory access (depends on target system)
+// =========================================================================
+
+// NOTE:
+// The following macros are used to combine standard library definitions. Some
+// applications needs to use one common library function (e.g. memcpy()). So
+// you can set (or change) it here.
+
+#if (TARGET_SYSTEM == _WIN32_)
+
+#define _WIN32_WINDOWS 0x0401
+#define _WIN32_WINNT   0x0400
+
+#include <stdlib.h>
+#include <stdio.h>
+
+    //29.11.2004 f.j. sonst ist memcpy und memset unbekannt
+#include <string.h>
+
+#define EPL_MEMCPY(dst,src,siz)     memcpy((void*)(dst),(const void*)(src),(size_t)(siz));
+#define EPL_MEMSET(dst,val,siz)     memset((void*)(dst),(int)(val),(size_t)(siz));
+
+    // f.j.: die Funktionen für <MemAlloc> und <MemFree> sind in WinMem.c definiert
+    //definition der Prototypen
+void FAR *MemAlloc(DWORD dwMemSize_p);
+void MemFree(void FAR * pMem_p);
+
+#define EPL_MALLOC(siz)             malloc((size_t)(siz))
+#define EPL_FREE(ptr)               free((void *)ptr)
+
+#ifndef PRINTF0
+void trace(const char *fmt, ...);
+#define PRINTF                      TRACE
+#define PRINTF0(arg)                TRACE0(arg)
+#define PRINTF1(arg,p1)             TRACE1(arg,p1)
+#define PRINTF2(arg,p1,p2)          TRACE2(arg,p1,p2)
+#define PRINTF3(arg,p1,p2,p3)       TRACE3(arg,p1,p2,p3)
+#define PRINTF4(arg,p1,p2,p3,p4)    TRACE4(arg,p1,p2,p3,p4)
+	//#define PRINTF                      printf
+	//#define PRINTF0(arg)                PRINTF(arg)
+	//#define PRINTF1(arg,p1)             PRINTF(arg,p1)
+	//#define PRINTF2(arg,p1,p2)          PRINTF(arg,p1,p2)
+	//#define PRINTF3(arg,p1,p2,p3)       PRINTF(arg,p1,p2,p3)
+	//#define PRINTF4(arg,p1,p2,p3,p4)    PRINTF(arg,p1,p2,p3,p4)
+#endif
+
+#ifdef ASSERTMSG
+#undef ASSERTMSG
+#endif
+
+#define ASSERTMSG(expr,string)  if (!(expr)) { \
+                                        MessageBox (NULL, string, "Assertion failed", MB_OK | MB_ICONERROR); \
+                                        exit (-1);}
+
+#elif (TARGET_SYSTEM == _NO_OS_)
+
+#include <stdlib.h>
+#include <stdio.h>
+
+    //29.11.2004 f.j. sonst ist memcpy und memset unbekannt
+//    #include <string.h>
+
+#define EPL_MEMCPY(dst,src,siz)     memcpy((void*)(dst),(const void*)(src),(size_t)(siz));
+#define EPL_MEMSET(dst,val,siz)     memset((void*)(dst),(int)(val),(size_t)(siz));
+
+#define EPL_MALLOC(siz)             malloc((size_t)(siz))
+#define EPL_FREE(ptr)               free((void *)ptr)
+
+#ifndef PRINTF0
+#define PRINTF                      TRACE
+#define PRINTF0(arg)                TRACE0(arg)
+#define PRINTF1(arg,p1)             TRACE1(arg,p1)
+#define PRINTF2(arg,p1,p2)          TRACE2(arg,p1,p2)
+#define PRINTF3(arg,p1,p2,p3)       TRACE3(arg,p1,p2,p3)
+#define PRINTF4(arg,p1,p2,p3,p4)    TRACE4(arg,p1,p2,p3,p4)
+	//#define PRINTF                      printf
+	//#define PRINTF0(arg)                PRINTF(arg)
+	//#define PRINTF1(arg,p1)             PRINTF(arg,p1)
+	//#define PRINTF2(arg,p1,p2)          PRINTF(arg,p1,p2)
+	//#define PRINTF3(arg,p1,p2,p3)       PRINTF(arg,p1,p2,p3)
+	//#define PRINTF4(arg,p1,p2,p3,p4)    PRINTF(arg,p1,p2,p3,p4)
+#endif
+
+#elif (TARGET_SYSTEM == _LINUX_)
+
+#ifndef __KERNEL__
+#include <stdlib.h>
+#include <stdio.h>
+#else
+//        #include <linux/config.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/major.h>
+#include <linux/version.h>
+#endif
+
+    //29.11.2004 f.j. sonst ist memcpy und memset unbekannt
+//    #include <string.h>
+
+#define EPL_MEMCPY(dst,src,siz)     memcpy((void*)(dst),(const void*)(src),(size_t)(siz));
+#define EPL_MEMSET(dst,val,siz)     memset((void*)(dst),(int)(val),(size_t)(siz));
+
+#ifndef __KERNEL__
+#define EPL_MALLOC(siz)             malloc((size_t)(siz))
+#define EPL_FREE(ptr)               free((void *)ptr)
+#else
+#define EPL_MALLOC(siz)             kmalloc((size_t)(siz), GFP_KERNEL)
+#define EPL_FREE(ptr)               kfree((void *)ptr)
+#endif
+
+#ifndef PRINTF0
+#define PRINTF                      TRACE
+#define PRINTF0(arg)                TRACE0(arg)
+#define PRINTF1(arg,p1)             TRACE1(arg,p1)
+#define PRINTF2(arg,p1,p2)          TRACE2(arg,p1,p2)
+#define PRINTF3(arg,p1,p2,p3)       TRACE3(arg,p1,p2,p3)
+#define PRINTF4(arg,p1,p2,p3,p4)    TRACE4(arg,p1,p2,p3,p4)
+	//#define PRINTF                      printf
+	//#define PRINTF0(arg)                PRINTF(arg)
+	//#define PRINTF1(arg,p1)             PRINTF(arg,p1)
+	//#define PRINTF2(arg,p1,p2)          PRINTF(arg,p1,p2)
+	//#define PRINTF3(arg,p1,p2,p3)       PRINTF(arg,p1,p2,p3)
+	//#define PRINTF4(arg,p1,p2,p3,p4)    PRINTF(arg,p1,p2,p3,p4)
+#endif
+
+#endif
+
+#define EPL_TGT_INTMASK_ETH     0x0001	// ethernet interrupt
+#define EPL_TGT_INTMASK_DMA     0x0002	// DMA interrupt
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+// currently no Timer functions are needed by EPL stack
+// so they are not implemented yet
+//void  PUBLIC TgtTimerInit(void);
+//DWORD PUBLIC TgtGetTickCount(void);
+//void PUBLIC TgtGetNetTime(tEplNetTime * pNetTime_p);
+
+// functions for ethernet driver
+tEplKernel PUBLIC TgtInitEthIsr(void);
+void PUBLIC TgtFreeEthIsr(void);
+void PUBLIC TgtEnableGlobalInterrupt(BYTE fEnable_p);
+void PUBLIC TgtEnableEthInterrupt0(BYTE fEnable_p,
+				   unsigned int uiInterruptMask_p);
+void PUBLIC TgtEnableEthInterrupt1(BYTE fEnable_p,
+				   unsigned int uiInterruptMask_p);
+
+#endif // #ifndef _EPLTARGET_H_
diff --git a/drivers/staging/epl/EplTimer.h b/drivers/staging/epl/EplTimer.h
new file mode 100644
index 000000000000..facbfd8740e6
--- /dev/null
+++ b/drivers/staging/epl/EplTimer.h
@@ -0,0 +1,117 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for Epl Timer-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTimer.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/06 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "EplEvent.h"
+
+#ifndef _EPLTIMER_H_
+#define _EPLTIMER_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+// type for timer handle
+typedef unsigned long tEplTimerHdl;
+
+typedef struct {
+	tEplEventSink m_EventSink;
+	unsigned long m_ulArg;	// d.k.: converted to unsigned long because
+	// it is never accessed as a pointer by the
+	// timer module and the data the
+	// pointer points to is not saved in any way.
+	// It is just a value. The user is responsible
+	// to store the data statically and convert
+	// the pointer between address spaces.
+
+} tEplTimerArg;
+
+typedef struct {
+	tEplTimerHdl m_TimerHdl;
+	unsigned long m_ulArg;	// d.k.: converted to unsigned long because
+	// it is never accessed as a pointer by the
+	// timer module and the data the
+	// pointer points to is not saved in any way.
+	// It is just a value.
+
+} tEplTimerEventArg;
+
+typedef tEplKernel(PUBLIC * tEplTimerkCallback) (tEplTimerEventArg *
+						 pEventArg_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPLTIMER_H_
diff --git a/drivers/staging/epl/EplTimeruLinuxKernel.c b/drivers/staging/epl/EplTimeruLinuxKernel.c
new file mode 100644
index 000000000000..08820d184055
--- /dev/null
+++ b/drivers/staging/epl/EplTimeruLinuxKernel.c
@@ -0,0 +1,446 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for EPL User Timermodule for Linux kernel module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTimeruLinuxKernel.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/09/12 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplTimeru.h"
+#include <linux/timer.h>
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+typedef struct {
+	struct timer_list m_Timer;
+	tEplTimerArg TimerArgument;
+
+} tEplTimeruData;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+static void PUBLIC EplTimeruCbMs(unsigned long ulParameter_p);
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <Epl Userspace-Timermodule for Linux Kernel>              */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: Epl Userspace-Timermodule for Linux Kernel
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruInit
+//
+// Description: function inits first instance
+//
+// Parameters:  void
+//
+// Returns:     tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimeruInit()
+{
+	tEplKernel Ret;
+
+	Ret = EplTimeruAddInstance();
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruAddInstance
+//
+// Description: function inits additional instance
+//
+// Parameters:  void
+//
+// Returns:     tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimeruAddInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruDelInstance
+//
+// Description: function deletes instance
+//              -> under Linux nothing to do
+//              -> no instance table needed
+//
+// Parameters:  void
+//
+// Returns:     tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimeruDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruSetTimerMs
+//
+// Description: function creates a timer and returns the corresponding handle
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//              ulTime_p    = time for timer in ms
+//              Argument_p  = argument for timer
+//
+// Returns:     tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimeruSetTimerMs(tEplTimerHdl * pTimerHdl_p,
+				      unsigned long ulTime_p,
+				      tEplTimerArg Argument_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplTimeruData *pData;
+
+	// check pointer to handle
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+	pData = (tEplTimeruData *) EPL_MALLOC(sizeof(tEplTimeruData));
+	if (pData == NULL) {
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+	init_timer(&pData->m_Timer);
+	pData->m_Timer.function = EplTimeruCbMs;
+	pData->m_Timer.data = (unsigned long)pData;
+	pData->m_Timer.expires = jiffies + ulTime_p * HZ / 1000;
+
+	EPL_MEMCPY(&pData->TimerArgument, &Argument_p, sizeof(tEplTimerArg));
+
+	add_timer(&pData->m_Timer);
+
+	*pTimerHdl_p = (tEplTimerHdl) pData;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruModifyTimerMs
+//
+// Description: function changes a timer and returns the corresponding handle
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//              ulTime_p    = time for timer in ms
+//              Argument_p  = argument for timer
+//
+// Returns:     tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimeruModifyTimerMs(tEplTimerHdl * pTimerHdl_p,
+					 unsigned long ulTime_p,
+					 tEplTimerArg Argument_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplTimeruData *pData;
+
+	// check pointer to handle
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+	// check handle itself, i.e. was the handle initialized before
+	if (*pTimerHdl_p == 0) {
+		Ret = EplTimeruSetTimerMs(pTimerHdl_p, ulTime_p, Argument_p);
+		goto Exit;
+	}
+	pData = (tEplTimeruData *) * pTimerHdl_p;
+	if ((tEplTimeruData *) pData->m_Timer.data != pData) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+	mod_timer(&pData->m_Timer, (jiffies + ulTime_p * HZ / 1000));
+
+	// copy the TimerArg after the timer is restarted,
+	// so that a timer occured immediately before mod_timer
+	// won't use the new TimerArg and
+	// therefore the old timer cannot be distinguished from the new one.
+	// But if the new timer is too fast, it may get lost.
+	EPL_MEMCPY(&pData->TimerArgument, &Argument_p, sizeof(tEplTimerArg));
+
+	// check if timer is really running
+	if (timer_pending(&pData->m_Timer) == 0) {	// timer is not running
+		// retry starting it
+		add_timer(&pData->m_Timer);
+	}
+	// set handle to pointer of tEplTimeruData
+//    *pTimerHdl_p = (tEplTimerHdl) pData;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruDeleteTimer
+//
+// Description: function deletes a timer
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//
+// Returns:     tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimeruDeleteTimer(tEplTimerHdl * pTimerHdl_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplTimeruData *pData;
+
+	// check pointer to handle
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+	// check handle itself, i.e. was the handle initialized before
+	if (*pTimerHdl_p == 0) {
+		Ret = kEplSuccessful;
+		goto Exit;
+	}
+	pData = (tEplTimeruData *) * pTimerHdl_p;
+	if ((tEplTimeruData *) pData->m_Timer.data != pData) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+/*    if (del_timer(&pData->m_Timer) == 1)
+    {
+        kfree(pData);
+    }
+*/
+	// try to delete the timer
+	del_timer(&pData->m_Timer);
+	// free memory in any case
+	kfree(pData);
+
+	// uninitialize handle
+	*pTimerHdl_p = 0;
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruIsTimerActive
+//
+// Description: checks if the timer referenced by the handle is currently
+//              active.
+//
+// Parameters:  TimerHdl_p  = handle of the timer to check
+//
+// Returns:     BOOL        = TRUE, if active;
+//                            FALSE, otherwise
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+BOOL PUBLIC EplTimeruIsTimerActive(tEplTimerHdl TimerHdl_p)
+{
+	BOOL fActive = FALSE;
+	tEplTimeruData *pData;
+
+	// check handle itself, i.e. was the handle initialized before
+	if (TimerHdl_p == 0) {	// timer was not created yet, so it is not active
+		goto Exit;
+	}
+	pData = (tEplTimeruData *) TimerHdl_p;
+	if ((tEplTimeruData *) pData->m_Timer.data != pData) {	// invalid timer
+		goto Exit;
+	}
+	// check if timer is running
+	if (timer_pending(&pData->m_Timer) == 0) {	// timer is not running
+		goto Exit;
+	}
+
+	fActive = TRUE;
+
+      Exit:
+	return fActive;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruCbMs
+//
+// Description: function to process timer
+//
+//
+//
+// Parameters:  lpParameter = pointer to structur of type tEplTimeruData
+//
+//
+// Returns:     (none)
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static void PUBLIC EplTimeruCbMs(unsigned long ulParameter_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplTimeruData *pData;
+	tEplEvent EplEvent;
+	tEplTimerEventArg TimerEventArg;
+
+	pData = (tEplTimeruData *) ulParameter_p;
+
+	// call event function
+	TimerEventArg.m_TimerHdl = (tEplTimerHdl) pData;
+	TimerEventArg.m_ulArg = pData->TimerArgument.m_ulArg;
+
+	EplEvent.m_EventSink = pData->TimerArgument.m_EventSink;
+	EplEvent.m_EventType = kEplEventTypeTimer;
+	EPL_MEMSET(&EplEvent.m_NetTime, 0x00, sizeof(tEplNetTime));
+	EplEvent.m_pArg = &TimerEventArg;
+	EplEvent.m_uiSize = sizeof(TimerEventArg);
+
+	Ret = EplEventuPost(&EplEvent);
+
+	// d.k. do not free memory, user has to call EplTimeruDeleteTimer()
+	//kfree(pData);
+
+}
+
+// EOF
diff --git a/drivers/staging/epl/EplTimeruNull.c b/drivers/staging/epl/EplTimeruNull.c
new file mode 100644
index 000000000000..40ce403cbd03
--- /dev/null
+++ b/drivers/staging/epl/EplTimeruNull.c
@@ -0,0 +1,312 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for Epl Userspace-Timermodule NULL-Implementation
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTimeruNull.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/06 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplTimeru.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <Epl Userspace-Timermodule NULL-Implementation>     */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: Epl Userspace-Timermodule NULL-Implementation
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruInit
+//
+// Description: function init first instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruInit()
+{
+	tEplKernel Ret;
+
+	Ret = EplTimeruAddInstance();
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruAddInstance
+//
+// Description: function init additional instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruAddInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruDelInstance
+//
+// Description: function delte instance
+//              -> under Win32 nothing to do
+//              -> no instnace table needed
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruSetTimerMs
+//
+// Description: function create a timer and return a handle to the pointer
+//
+//
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//              ulTime_p    = time for timer in ms
+//              Argument_p  = argument for timer
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruSetTimerMs(tEplTimerHdl * pTimerHdl_p,
+				      unsigned long ulTime_p,
+				      tEplTimerArg Argument_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// check handle
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+ //---------------------------------------------------------------------------
+//
+// Function:    EplTimeruModifyTimerMs
+//
+// Description: function change a timer and return a handle to the pointer
+//
+//
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//              ulTime_p    = time for timer in ms
+//              Argument_p  = argument for timer
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruModifyTimerMs(tEplTimerHdl * pTimerHdl_p,
+					 unsigned long ulTime_p,
+					 tEplTimerArg Argument_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// check parameter
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+ //---------------------------------------------------------------------------
+//
+// Function:    EplTimeruDeleteTimer
+//
+// Description: function delte a timer
+//
+//
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruDeleteTimer(tEplTimerHdl * pTimerHdl_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// check parameter
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+	// set handle invalide
+	*pTimerHdl_p = 0;
+
+      Exit:
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+// EOF
diff --git a/drivers/staging/epl/EplTimeruWin32.c b/drivers/staging/epl/EplTimeruWin32.c
new file mode 100644
index 000000000000..a967b4e59d4b
--- /dev/null
+++ b/drivers/staging/epl/EplTimeruWin32.c
@@ -0,0 +1,513 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for Epl Userspace-Timermodule for Win32
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTimeruWin32.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/06 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplTimeru.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+typedef struct {
+	tEplTimerArg TimerArgument;
+	HANDLE DelteHandle;
+	unsigned long ulTimeout;
+
+} tEplTimeruThread;
+
+typedef struct {
+	LPCRITICAL_SECTION m_pCriticalSection;
+	CRITICAL_SECTION m_CriticalSection;
+} tEplTimeruInstance;
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+static tEplTimeruInstance EplTimeruInstance_g;
+static tEplTimeruThread ThreadData_l;
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+DWORD PUBLIC EplSdoTimeruThreadms(LPVOID lpParameter);
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <Epl Userspace-Timermodule for Win32>               */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: Epl Userspace-Timermodule for Win32
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruInit
+//
+// Description: function init first instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruInit()
+{
+	tEplKernel Ret;
+
+	Ret = EplTimeruAddInstance();
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruAddInstance
+//
+// Description: function init additional instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruAddInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// create critical section
+	EplTimeruInstance_g.m_pCriticalSection =
+	    &EplTimeruInstance_g.m_CriticalSection;
+	InitializeCriticalSection(EplTimeruInstance_g.m_pCriticalSection);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruDelInstance
+//
+// Description: function delte instance
+//              -> under Win32 nothing to do
+//              -> no instnace table needed
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruSetTimerMs
+//
+// Description: function create a timer and return a handle to the pointer
+//
+//
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//              ulTime_p    = time for timer in ms
+//              Argument_p  = argument for timer
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruSetTimerMs(tEplTimerHdl * pTimerHdl_p,
+				      unsigned long ulTime_p,
+				      tEplTimerArg Argument_p)
+{
+	tEplKernel Ret;
+	HANDLE DeleteHandle;
+	HANDLE ThreadHandle;
+	DWORD ThreadId;
+
+	Ret = kEplSuccessful;
+
+	// check handle
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+	// enter  critical section
+	EnterCriticalSection(EplTimeruInstance_g.m_pCriticalSection);
+
+	// first create event to delete timer
+	DeleteHandle = CreateEvent(NULL, FALSE, FALSE, NULL);
+	if (DeleteHandle == NULL) {
+		Ret = kEplTimerNoTimerCreated;
+		goto Exit;
+	}
+	// set handle for caller
+	*pTimerHdl_p = (tEplTimerHdl) DeleteHandle;
+
+	// fill data for thread
+	ThreadData_l.DelteHandle = DeleteHandle;
+	EPL_MEMCPY(&ThreadData_l.TimerArgument, &Argument_p,
+		   sizeof(tEplTimerArg));
+	ThreadData_l.ulTimeout = ulTime_p;
+
+	// create thread to create waitable timer and wait for timer
+	ThreadHandle = CreateThread(NULL,
+				    0,
+				    EplSdoTimeruThreadms,
+				    &ThreadData_l, 0, &ThreadId);
+	if (ThreadHandle == NULL) {
+		// leave critical section
+		LeaveCriticalSection(EplTimeruInstance_g.m_pCriticalSection);
+
+		// delte handle
+		CloseHandle(DeleteHandle);
+
+		Ret = kEplTimerNoTimerCreated;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+ //---------------------------------------------------------------------------
+//
+// Function:    EplTimeruModifyTimerMs
+//
+// Description: function change a timer and return a handle to the pointer
+//
+//
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//              ulTime_p    = time for timer in ms
+//              Argument_p  = argument for timer
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruModifyTimerMs(tEplTimerHdl * pTimerHdl_p,
+					 unsigned long ulTime_p,
+					 tEplTimerArg Argument_p)
+{
+	tEplKernel Ret;
+	HANDLE DeleteHandle;
+	HANDLE ThreadHandle;
+	DWORD ThreadId;
+
+	Ret = kEplSuccessful;
+
+	// check parameter
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+	DeleteHandle = (HANDLE) (*pTimerHdl_p);
+
+	// set event to end timer task for this timer
+	SetEvent(DeleteHandle);
+
+	// create new timer
+	// first create event to delete timer
+	DeleteHandle = CreateEvent(NULL, FALSE, FALSE, NULL);
+	if (DeleteHandle == NULL) {
+		Ret = kEplTimerNoTimerCreated;
+		goto Exit;
+	}
+	// set handle for caller
+	*pTimerHdl_p = (tEplTimerHdl) DeleteHandle;
+
+	// enter  critical section
+	EnterCriticalSection(EplTimeruInstance_g.m_pCriticalSection);
+
+	// fill data for thread
+	ThreadData_l.DelteHandle = DeleteHandle;
+	EPL_MEMCPY(&ThreadData_l.TimerArgument, &Argument_p,
+		   sizeof(tEplTimerArg));
+	ThreadData_l.ulTimeout = ulTime_p;
+
+	// create thread to create waitable timer and wait for timer
+	ThreadHandle = CreateThread(NULL,
+				    0,
+				    EplSdoTimeruThreadms,
+				    &ThreadData_l, 0, &ThreadId);
+	if (ThreadHandle == NULL) {
+		// leave critical section
+		LeaveCriticalSection(EplTimeruInstance_g.m_pCriticalSection);
+
+		// delte handle
+
+		Ret = kEplTimerNoTimerCreated;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+ //---------------------------------------------------------------------------
+//
+// Function:    EplTimeruDeleteTimer
+//
+// Description: function delte a timer
+//
+//
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruDeleteTimer(tEplTimerHdl * pTimerHdl_p)
+{
+	tEplKernel Ret;
+	HANDLE DeleteHandle;
+
+	Ret = kEplSuccessful;
+
+	// check parameter
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+	DeleteHandle = (HANDLE) (*pTimerHdl_p);
+
+	// set event to end timer task for this timer
+	SetEvent(DeleteHandle);
+
+	// set handle invalide
+	*pTimerHdl_p = 0;
+
+      Exit:
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoTimeruThreadms
+//
+// Description: function to process timer as thread
+//
+//
+//
+// Parameters:  lpParameter = pointer to structur of type tEplTimeruThread
+//
+//
+// Returns:     DWORD = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+DWORD PUBLIC EplSdoTimeruThreadms(LPVOID lpParameter)
+{
+	tEplKernel Ret;
+	tEplTimeruThread *pThreadData;
+	HANDLE aHandles[2];
+	BOOL fReturn;
+	LARGE_INTEGER TimeoutTime;
+	unsigned long ulEvent;
+	tEplEvent EplEvent;
+	tEplTimeruThread ThreadData;
+	tEplTimerEventArg TimerEventArg;
+
+	Ret = kEplSuccessful;
+
+	// get pointer to data
+	pThreadData = (tEplTimeruThread *) lpParameter;
+	// copy thread data
+	EPL_MEMCPY(&ThreadData, pThreadData, sizeof(ThreadData));
+	pThreadData = &ThreadData;
+
+	// leave critical section
+	LeaveCriticalSection(EplTimeruInstance_g.m_pCriticalSection);
+
+	// create waitable timer
+	aHandles[1] = CreateWaitableTimer(NULL, FALSE, NULL);
+	if (aHandles[1] == NULL) {
+		Ret = kEplTimerNoTimerCreated;
+		goto Exit;
+	}
+	// set timer
+	// set timeout interval -> needed to be negativ
+	// -> because relative timeout
+	// -> multiply by 10000 for 100 ns timebase of function
+	TimeoutTime.QuadPart = (((long long)pThreadData->ulTimeout) * -10000);
+	fReturn = SetWaitableTimer(aHandles[1],
+				   &TimeoutTime, 0, NULL, NULL, FALSE);
+	if (fReturn == 0) {
+		Ret = kEplTimerNoTimerCreated;
+		goto Exit;
+	}
+	// save delte event handle in handle array
+	aHandles[0] = pThreadData->DelteHandle;
+
+	// wait for one of the events
+	ulEvent = WaitForMultipleObjects(2, &aHandles[0], FALSE, INFINITE);
+	if (ulEvent == WAIT_OBJECT_0) {	// delte event
+
+		// close handels
+		CloseHandle(aHandles[1]);
+		// terminate thread
+		goto Exit;
+	} else if (ulEvent == (WAIT_OBJECT_0 + 1)) {	// timer event
+		// call event function
+		TimerEventArg.m_TimerHdl =
+		    (tEplTimerHdl) pThreadData->DelteHandle;
+		TimerEventArg.m_ulArg = pThreadData->TimerArgument.m_ulArg;
+
+		EplEvent.m_EventSink = pThreadData->TimerArgument.m_EventSink;
+		EplEvent.m_EventType = kEplEventTypeTimer;
+		EPL_MEMSET(&EplEvent.m_NetTime, 0x00, sizeof(tEplNetTime));
+		EplEvent.m_pArg = &TimerEventArg;
+		EplEvent.m_uiSize = sizeof(TimerEventArg);
+
+		Ret = EplEventuPost(&EplEvent);
+
+		// close handels
+		CloseHandle(aHandles[1]);
+		// terminate thread
+		goto Exit;
+
+	} else {		// error
+		ulEvent = GetLastError();
+		TRACE1("Error in WaitForMultipleObjects Errorcode: 0x%x\n",
+		       ulEvent);
+		// terminate thread
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+// EOF
diff --git a/drivers/staging/epl/EplVersion.h b/drivers/staging/epl/EplVersion.h
new file mode 100644
index 000000000000..75570d56b865
--- /dev/null
+++ b/drivers/staging/epl/EplVersion.h
@@ -0,0 +1,98 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  This file defines the EPL version for the stack, as string
+                and for object 0x1018 within object dictionary.
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplVersion.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    all
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+****************************************************************************/
+
+#ifndef _EPL_VERSION_H_
+#define _EPL_VERSION_H_
+
+// NOTE:
+// All version macros should contain the same version number. But do not use
+// defines instead of the numbers. Because the macro EPL_STRING_VERSION() can not
+// convert a define to a string.
+//
+// Format: maj.min.build
+//         maj            = major version
+//             min        = minor version (will be set to 0 if major version will be incremented)
+//                 build  = current build (will be set to 0 if minor version will be incremented)
+//
+#define DEFINED_STACK_VERSION       EPL_STACK_VERSION   (1, 3, 0)
+#define DEFINED_OBJ1018_VERSION     EPL_OBJ1018_VERSION (1, 3, 0)
+#define DEFINED_STRING_VERSION      EPL_STRING_VERSION  (1, 3, 0)
+
+// -----------------------------------------------------------------------------
+#define EPL_PRODUCT_NAME            "EPL V2"
+#define EPL_PRODUCT_VERSION         DEFINED_STRING_VERSION
+#define EPL_PRODUCT_MANUFACTURER    "SYS TEC electronic GmbH"
+
+#define EPL_PRODUCT_KEY         "SO-1083"
+#define EPL_PRODUCT_DESCRIPTION "openPOWERLINK Protocol Stack Source"
+
+#endif // _EPL_VERSION_H_
+
+// Die letzte Zeile muß unbedingt eine leere Zeile sein, weil manche Compiler
+// damit ein Problem haben, wenn das nicht so ist (z.B. GNU oder Borland C++ Builder).
diff --git a/drivers/staging/epl/Kconfig b/drivers/staging/epl/Kconfig
new file mode 100644
index 000000000000..9f939d5874ac
--- /dev/null
+++ b/drivers/staging/epl/Kconfig
@@ -0,0 +1,6 @@
+config EPL
+	tristate "openPOWERLINK protocol stack"
+	depends on NET && HIGH_RES_TIMERS && X86
+	default N
+	---help---
+	  Enable support for the openPOWERLINK network protocol stack.
diff --git a/drivers/staging/epl/Makefile b/drivers/staging/epl/Makefile
new file mode 100644
index 000000000000..a2c824187d21
--- /dev/null
+++ b/drivers/staging/epl/Makefile
@@ -0,0 +1,41 @@
+obj-$(CONFIG_EPL)	+= epl.o
+
+epl-objs := 	\
+	EplApiGeneric.o		\
+	EplApiLinuxKernel.o	\
+	EplApiProcessImage.o	\
+	EplDllk.o		\
+	EplDllkCal.o		\
+	EplDlluCal.o		\
+	EplErrorHandlerk.o	\
+	EplEventk.o		\
+	EplEventu.o		\
+	EplIdentu.o		\
+	EplNmtCnu.o		\
+	EplNmtk.o		\
+	EplNmtkCal.o		\
+	EplNmtMnu.o		\
+	EplNmtu.o		\
+	EplNmtuCal.o		\
+	EplObd.o		\
+	EplObdkCal.o		\
+	EplObdu.o		\
+	EplObduCal.o		\
+	EplPdok.o		\
+	EplPdokCal.o		\
+	EplPdou.o		\
+	EplSdoAsndu.o		\
+	EplSdoAsySequ.o		\
+	EplSdoComu.o		\
+	EplSdoUdpu.o		\
+	EplStatusu.o		\
+	EplTimeruLinuxKernel.o	\
+	amix86.o		\
+	SharedBuff.o		\
+	ShbIpc-LinuxKernel.o	\
+	TimerHighReskX86.o	\
+	VirtualEthernetLinux.o	\
+	SocketLinuxKernel.o	\
+	proc_fs.o		\
+	demo_main.o		\
+	Edrv8139.o		\
diff --git a/drivers/staging/epl/SharedBuff.c b/drivers/staging/epl/SharedBuff.c
new file mode 100644
index 000000000000..9fb09d6bc28e
--- /dev/null
+++ b/drivers/staging/epl/SharedBuff.c
@@ -0,0 +1,1799 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      Project independend shared buffer (linear + circular)
+
+  Description:  Implementation of platform independend part for the
+                shared buffer
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+  2006/06/27 -rs:   V 1.00 (initial version)
+
+****************************************************************************/
+
+#if defined(WIN32) || defined(_WIN32)
+
+#ifdef UNDER_RTSS
+	// RTX header
+#include <windows.h>
+#include <process.h>
+#include <rtapi.h>
+
+#elif __BORLANDC__
+	// borland C header
+#include <windows.h>
+#include <process.h>
+
+#elif WINCE
+#include <windows.h>
+
+#else
+	// MSVC needs to include windows.h at first
+	// the following defines ar necessary for function prototypes for waitable timers
+#define _WIN32_WINDOWS 0x0401
+#define _WIN32_WINNT   0x0400
+#include <windows.h>
+#include <process.h>
+#endif
+
+#endif
+
+#include "global.h"
+#include "SharedBuff.h"
+#include "ShbIpc.h"
+
+// d.k. Linux kernel modules needs other header files for memcpy()
+#if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+#include <linux/string.h>
+#else
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+#if (!defined(SHAREDBUFF_INLINED)) || defined(INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Configuration
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Constant definitions
+//---------------------------------------------------------------------------
+
+#define SBC_MAGIC_ID    0x53424323	// magic ID ("SBC#")
+#define SBL_MAGIC_ID    0x53424C23	// magic ID ("SBL#")
+
+//---------------------------------------------------------------------------
+//  Local types
+//---------------------------------------------------------------------------
+
+// structure to administrate circular shared buffer head
+typedef struct {
+	unsigned long m_ShbCirMagicID;	// magic ID ("SBC#")
+	unsigned long m_ulBufferTotalSize;	// over-all size of complete buffer
+	unsigned long m_ulBufferDataSize;	// size of complete data area
+	unsigned long m_ulWrIndex;	// current write index (set bevore write)
+	unsigned long m_ulRdIndex;	// current read index (set after read)
+	unsigned long m_ulNumOfWriteJobs;	// number of currently (parallel running) write operations
+	unsigned long m_ulDataInUse;	// currently used buffer size (incl. uncompleted write operations)
+	unsigned long m_ulDataApended;	// buffer size of complete new written but not yet readable data (in case of m_ulNumOfWriteJobs>1)
+	unsigned long m_ulBlocksApended;	// number of complete new written but not yet readable data blocks (in case of m_ulNumOfWriteJobs>1)
+	unsigned long m_ulDataReadable;	// buffer size with readable (complete written) data
+	unsigned long m_ulBlocksReadable;	// number of readable (complete written) data blocks
+	tShbCirSigHndlrNewData m_pfnSigHndlrNewData;	// application handler to signal new data
+	unsigned int m_fBufferLocked;	// TRUE if buffer is locked (because of pending reset request)
+	tShbCirSigHndlrReset m_pfnSigHndlrReset;	// application handler to signal buffer reset is done
+	unsigned char m_Data;	// start of data area (the real data size is unknown at this time)
+
+} tShbCirBuff;
+
+// structure to administrate linear shared buffer head
+typedef struct {
+	unsigned int m_ShbLinMagicID;	// magic ID ("SBL#")
+	unsigned long m_ulBufferTotalSize;	// over-all size of complete buffer
+	unsigned long m_ulBufferDataSize;	// size of complete data area
+	unsigned char m_Data;	// start of data area (the real data size is unknown at this time)
+
+} tShbLinBuff;
+
+// type to save size of a single data block inside the circular shared buffer
+typedef struct {
+	unsigned int m_uiFullBlockSize:28;	// a single block must not exceed a length of 256MByte :-)
+	unsigned int m_uiAlignFillBytes:4;
+
+} tShbCirBlockSize;
+
+#define SBC_BLOCK_ALIGNMENT                  4	// alignment must *not* be lower than sizeof(tShbCirBlockSize)!
+#define SBC_MAX_BLOCK_SIZE         ((1<<28)-1)	// = (2^28 - 1) = (256MByte - 1) -> should be enought for real life :-)
+
+#define SBL_BLOCK_ALIGNMENT                  4
+#define SBL_MAX_BLOCK_SIZE         ((1<<28)-1)	// = (2^28 - 1) = (256MByte - 1) -> should be enought for real life :-)
+
+//---------------------------------------------------------------------------
+//  Global variables
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Local variables
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Prototypes of internal functions
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Get pointer to Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbCirBuff *ShbCirGetBuffer(tShbInstance pShbInstance_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+
+	pShbCirBuff = (tShbCirBuff *) ShbIpcGetShMemPtr(pShbInstance_p);
+	ASSERT(pShbCirBuff->m_ShbCirMagicID == SBC_MAGIC_ID);
+
+	return (pShbCirBuff);
+
+}
+
+//---------------------------------------------------------------------------
+//  Get pointer to Linear Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbLinBuff *ShbLinGetBuffer(tShbInstance pShbInstance_p)
+{
+
+	tShbLinBuff *pShbLinBuff;
+
+	pShbLinBuff = (tShbLinBuff *) ShbIpcGetShMemPtr(pShbInstance_p);
+	ASSERT(pShbLinBuff->m_ShbLinMagicID == SBL_MAGIC_ID);
+
+	return (pShbLinBuff);
+
+}
+
+// not inlined internal functions
+int ShbCirSignalHandlerNewData(tShbInstance pShbInstance_p);
+void ShbCirSignalHandlerReset(tShbInstance pShbInstance_p,
+			      unsigned int fTimeOut_p);
+
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+#if !defined(INLINE_ENABLED)
+// not inlined external functions
+
+//---------------------------------------------------------------------------
+//  Initialize Shared Buffer Module
+//---------------------------------------------------------------------------
+
+tShbError ShbInit(void)
+{
+
+	tShbError ShbError;
+
+	ShbError = ShbIpcInit();
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Deinitialize Shared Buffer Module
+//---------------------------------------------------------------------------
+
+tShbError ShbExit(void)
+{
+
+	tShbError ShbError;
+
+	ShbError = ShbIpcExit();
+
+	return (ShbError);
+
+}
+
+//-------------------------------------------------------------------------//
+//                                                                         //
+//          C i r c u l a r   S h a r e d   B u f f e r                    //
+//                                                                         //
+//-------------------------------------------------------------------------//
+
+//---------------------------------------------------------------------------
+//  Allocate Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbCirAllocBuffer(unsigned long ulBufferSize_p,
+			    const char *pszBufferID_p,
+			    tShbInstance * ppShbInstance_p,
+			    unsigned int *pfShbNewCreated_p)
+{
+
+	tShbInstance pShbInstance;
+	tShbCirBuff *pShbCirBuff;
+	unsigned int fShbNewCreated;
+	unsigned long ulBufferDataSize;
+	unsigned long ulBufferTotalSize;
+	tShbError ShbError;
+
+	// check arguments
+	if ((ulBufferSize_p == 0) || (ppShbInstance_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+
+	// calculate length of memory to allocate
+	ulBufferDataSize =
+	    (ulBufferSize_p +
+	     (SBC_BLOCK_ALIGNMENT - 1)) & ~(SBC_BLOCK_ALIGNMENT - 1);
+	ulBufferTotalSize = ulBufferDataSize + sizeof(tShbCirBuff);
+
+	// allocate a new or open an existing shared buffer
+	ShbError = ShbIpcAllocBuffer(ulBufferTotalSize, pszBufferID_p,
+				     &pShbInstance, &fShbNewCreated);
+	if (ShbError != kShbOk) {
+		goto Exit;
+	}
+
+	if (pShbInstance == NULL) {
+		ShbError = kShbOutOfMem;
+		goto Exit;
+	}
+
+	// get pointer to shared buffer
+	pShbCirBuff = (tShbCirBuff *) ShbIpcGetShMemPtr(pShbInstance);
+
+	// if the shared buffer was new created, than this process has
+	// to initialize it, otherwise the buffer is already in use
+	// and *must not* be reseted
+	if (fShbNewCreated) {
+#ifndef NDEBUG
+		{
+			memset(pShbCirBuff, 0xCC, ulBufferTotalSize);
+		}
+#endif
+
+		pShbCirBuff->m_ShbCirMagicID = SBC_MAGIC_ID;
+		pShbCirBuff->m_ulBufferTotalSize = ulBufferTotalSize;
+		pShbCirBuff->m_ulBufferDataSize = ulBufferDataSize;
+		pShbCirBuff->m_ulWrIndex = 0;
+		pShbCirBuff->m_ulRdIndex = 0;
+		pShbCirBuff->m_ulNumOfWriteJobs = 0;
+		pShbCirBuff->m_ulDataInUse = 0;
+		pShbCirBuff->m_ulDataApended = 0;
+		pShbCirBuff->m_ulBlocksApended = 0;
+		pShbCirBuff->m_ulDataReadable = 0;
+		pShbCirBuff->m_ulBlocksReadable = 0;
+		pShbCirBuff->m_pfnSigHndlrNewData = NULL;
+		pShbCirBuff->m_fBufferLocked = FALSE;
+		pShbCirBuff->m_pfnSigHndlrReset = NULL;
+	} else {
+		if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+			ShbError = kShbInvalidBufferType;
+			goto Exit;
+		}
+	}
+
+      Exit:
+
+	*ppShbInstance_p = pShbInstance;
+	*pfShbNewCreated_p = fShbNewCreated;
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Release Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbCirReleaseBuffer(tShbInstance pShbInstance_p)
+{
+
+	tShbError ShbError;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbOk;
+		goto Exit;
+	}
+
+	ShbError = ShbIpcReleaseBuffer(pShbInstance_p);
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+#endif // !defined(INLINE_ENABLED)
+
+#if (!defined(SHAREDBUFF_INLINED)) || defined(INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Reset Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirResetBuffer(tShbInstance pShbInstance_p,
+					    unsigned long ulTimeOut_p,
+					    tShbCirSigHndlrReset
+					    pfnSignalHandlerReset_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	unsigned long ulNumOfWriteJobs = 0;	// d.k. GCC complains about uninitialized variable otherwise
+	tShbError ShbError;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	// start reset job by setting request request in buffer header
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		if (!pShbCirBuff->m_fBufferLocked) {
+			ulNumOfWriteJobs = pShbCirBuff->m_ulNumOfWriteJobs;
+
+			pShbCirBuff->m_fBufferLocked = TRUE;
+			pShbCirBuff->m_pfnSigHndlrReset =
+			    pfnSignalHandlerReset_p;
+		} else {
+			ShbError = kShbAlreadyReseting;
+		}
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	if (ShbError != kShbOk) {
+		goto Exit;
+	}
+
+	// if there is currently no running write operation then reset buffer
+	// immediately, otherwise wait until the last write job is ready by
+	// starting a signal process
+	if (ulNumOfWriteJobs == 0) {
+		// there is currently no running write operation
+		// -> reset buffer immediately
+		ShbCirSignalHandlerReset(pShbInstance_p, FALSE);
+		ShbError = kShbOk;
+	} else {
+		// there is currently at least one running write operation
+		// -> starting signal process to wait until the last write job is ready
+		ShbError =
+		    ShbIpcStartSignalingJobReady(pShbInstance_p, ulTimeOut_p,
+						 ShbCirSignalHandlerReset);
+	}
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Write data block to Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirWriteDataBlock(tShbInstance pShbInstance_p,
+					       const void *pSrcDataBlock_p,
+					       unsigned long ulDataBlockSize_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	tShbCirBlockSize ShbCirBlockSize;
+	unsigned int uiFullBlockSize;
+	unsigned int uiAlignFillBytes;
+	unsigned char *pShbCirDataPtr;
+	unsigned char *pScrDataPtr;
+	unsigned long ulDataSize;
+	unsigned long ulChunkSize;
+	unsigned long ulWrIndex = 0;	// d.k. GCC complains about uninitialized variable otherwise
+	unsigned int fSignalNewData;
+	unsigned int fSignalReset;
+	tShbError ShbError;
+	tShbError ShbError2;
+	int fRes;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	if ((pSrcDataBlock_p == NULL) || (ulDataBlockSize_p == 0)) {
+		// nothing to do here
+		ShbError = kShbOk;
+		goto Exit;
+	}
+
+	if (ulDataBlockSize_p > SBC_MAX_BLOCK_SIZE) {
+		ShbError = kShbExceedDataSizeLimit;
+		goto Exit;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	pScrDataPtr = (unsigned char *)pSrcDataBlock_p;
+	fSignalNewData = FALSE;
+	fSignalReset = FALSE;
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	// calculate data block size in circular buffer
+	ulDataSize =
+	    (ulDataBlockSize_p +
+	     (SBC_BLOCK_ALIGNMENT - 1)) & ~(SBC_BLOCK_ALIGNMENT - 1);
+	uiFullBlockSize = ulDataSize + sizeof(tShbCirBlockSize);	// data size + header
+	uiAlignFillBytes = ulDataSize - ulDataBlockSize_p;
+
+	ShbCirBlockSize.m_uiFullBlockSize = uiFullBlockSize;
+	ShbCirBlockSize.m_uiAlignFillBytes = uiAlignFillBytes;
+
+	// reserve the needed memory for the write operation to do now
+	// and make necessary adjustments in the circular buffer header
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		// check if there is sufficient memory available to store
+		// the new data
+		fRes =
+		    uiFullBlockSize <=
+		    (pShbCirBuff->m_ulBufferDataSize -
+		     pShbCirBuff->m_ulDataInUse);
+		if (fRes) {
+			// set write pointer for the write operation to do now
+			// to the current write pointer of the circular buffer
+			ulWrIndex = pShbCirBuff->m_ulWrIndex;
+
+			// reserve the needed memory for the write operation to do now
+			pShbCirBuff->m_ulDataInUse += uiFullBlockSize;
+
+			// set new write pointer behind the reserved memory
+			// for the write operation to do now
+			pShbCirBuff->m_ulWrIndex += uiFullBlockSize;
+			pShbCirBuff->m_ulWrIndex %=
+			    pShbCirBuff->m_ulBufferDataSize;
+
+			// increment number of currently (parallel running)
+			// write operations
+			pShbCirBuff->m_ulNumOfWriteJobs++;
+		}
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	if (!fRes) {
+		ShbError = kShbBufferFull;
+		goto Exit;
+	}
+
+	// copy the data to the circular buffer
+	// (the copy process itself will be done outside of any
+	// critical/locked section)
+	pShbCirDataPtr = &pShbCirBuff->m_Data;	// ptr to start of data area
+
+	// write real size of current block (incl. alignment fill bytes)
+	*(tShbCirBlockSize *) (pShbCirDataPtr + ulWrIndex) = ShbCirBlockSize;
+	ulWrIndex += sizeof(tShbCirBlockSize);
+	ulWrIndex %= pShbCirBuff->m_ulBufferDataSize;
+
+	if (ulWrIndex + ulDataBlockSize_p <= pShbCirBuff->m_ulBufferDataSize) {
+		// linear write operation
+		memcpy(pShbCirDataPtr + ulWrIndex, pScrDataPtr,
+		       ulDataBlockSize_p);
+	} else {
+		// wrap-around write operation
+		ulChunkSize = pShbCirBuff->m_ulBufferDataSize - ulWrIndex;
+		memcpy(pShbCirDataPtr + ulWrIndex, pScrDataPtr, ulChunkSize);
+		memcpy(pShbCirDataPtr, pScrDataPtr + ulChunkSize,
+		       ulDataBlockSize_p - ulChunkSize);
+	}
+
+	// adjust header information for circular buffer with properties
+	// of the wiritten data block
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		pShbCirBuff->m_ulDataApended += uiFullBlockSize;
+		pShbCirBuff->m_ulBlocksApended++;
+
+		// decrement number of currently (parallel running) write operations
+		if (!--pShbCirBuff->m_ulNumOfWriteJobs) {
+			// if there is no other write process running then
+			// set new size of readable (complete written) data and
+			// adjust number of readable blocks
+			pShbCirBuff->m_ulDataReadable +=
+			    pShbCirBuff->m_ulDataApended;
+			pShbCirBuff->m_ulBlocksReadable +=
+			    pShbCirBuff->m_ulBlocksApended;
+
+			pShbCirBuff->m_ulDataApended = 0;
+			pShbCirBuff->m_ulBlocksApended = 0;
+
+			fSignalNewData = TRUE;
+			fSignalReset = pShbCirBuff->m_fBufferLocked;
+		}
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	// signal new data event to a potentially reading application
+	if (fSignalNewData) {
+		ShbError2 = ShbIpcSignalNewData(pShbInstance_p);
+		if (ShbError == kShbOk) {
+			ShbError = ShbError2;
+		}
+	}
+	// signal that the last write job has been finished to allow
+	// a waiting application to reset the buffer now
+	if (fSignalReset) {
+		ShbError2 = ShbIpcSignalJobReady(pShbInstance_p);
+		if (ShbError == kShbOk) {
+			ShbError = ShbError2;
+		}
+	}
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Allocate block within the Circular Shared Buffer for chunk writing
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirAllocDataBlock(tShbInstance pShbInstance_p,
+					       tShbCirChunk * pShbCirChunk_p,
+					       unsigned long ulDataBufferSize_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	tShbCirBlockSize ShbCirBlockSize;
+	unsigned int uiFullBlockSize;
+	unsigned int uiAlignFillBytes;
+	unsigned char *pShbCirDataPtr;
+	unsigned long ulDataSize;
+	unsigned long ulWrIndex = 0;	// d.k. GCC complains about uninitialized variable otherwise
+	tShbError ShbError;
+	int fRes;
+
+	// check arguments
+	if ((pShbInstance_p == NULL) || (pShbCirChunk_p == NULL)) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	if (ulDataBufferSize_p == 0) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	if (ulDataBufferSize_p > SBC_MAX_BLOCK_SIZE) {
+		ShbError = kShbExceedDataSizeLimit;
+		goto Exit;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	// calculate data block size in circular buffer
+	ulDataSize =
+	    (ulDataBufferSize_p +
+	     (SBC_BLOCK_ALIGNMENT - 1)) & ~(SBC_BLOCK_ALIGNMENT - 1);
+	uiFullBlockSize = ulDataSize + sizeof(tShbCirBlockSize);	// data size + header
+	uiAlignFillBytes = ulDataSize - ulDataBufferSize_p;
+
+	ShbCirBlockSize.m_uiFullBlockSize = uiFullBlockSize;
+	ShbCirBlockSize.m_uiAlignFillBytes = uiAlignFillBytes;
+
+	// reserve the needed memory for the write operation to do now
+	// and make necessary adjustments in the circular buffer header
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		// check if there is sufficient memory available to store
+		// the new data
+		fRes =
+		    (uiFullBlockSize <=
+		     (pShbCirBuff->m_ulBufferDataSize -
+		      pShbCirBuff->m_ulDataInUse));
+		if (fRes) {
+			// set write pointer for the write operation to do now
+			// to the current write pointer of the circular buffer
+			ulWrIndex = pShbCirBuff->m_ulWrIndex;
+
+			// reserve the needed memory for the write operation to do now
+			pShbCirBuff->m_ulDataInUse += uiFullBlockSize;
+
+			// set new write pointer behind the reserved memory
+			// for the write operation to do now
+			pShbCirBuff->m_ulWrIndex += uiFullBlockSize;
+			pShbCirBuff->m_ulWrIndex %=
+			    pShbCirBuff->m_ulBufferDataSize;
+
+			// increment number of currently (parallel running)
+			// write operations
+			pShbCirBuff->m_ulNumOfWriteJobs++;
+		}
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	if (!fRes) {
+		ShbError = kShbBufferFull;
+		goto Exit;
+	}
+
+	// setup header information for allocated buffer
+	pShbCirDataPtr = &pShbCirBuff->m_Data;	// ptr to start of data area
+
+	// write real size of current block (incl. alignment fill bytes)
+	*(tShbCirBlockSize *) (pShbCirDataPtr + ulWrIndex) = ShbCirBlockSize;
+	ulWrIndex += sizeof(tShbCirBlockSize);
+	ulWrIndex %= pShbCirBuff->m_ulBufferDataSize;
+
+	// setup chunk descriptor
+	pShbCirChunk_p->m_uiFullBlockSize = uiFullBlockSize;
+	pShbCirChunk_p->m_ulAvailableSize = ulDataBufferSize_p;
+	pShbCirChunk_p->m_ulWrIndex = ulWrIndex;
+	pShbCirChunk_p->m_fBufferCompleted = FALSE;
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Write data chunk into an allocated buffer of the Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirWriteDataChunk(tShbInstance pShbInstance_p,
+					       tShbCirChunk * pShbCirChunk_p,
+					       const void *pSrcDataChunk_p,
+					       unsigned long ulDataChunkSize_p,
+					       unsigned int
+					       *pfBufferCompleted_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	unsigned char *pShbCirDataPtr;
+	unsigned char *pScrDataPtr;
+	unsigned long ulSubChunkSize;
+	unsigned long ulWrIndex;
+	unsigned int fBufferCompleted;
+	unsigned int fSignalNewData;
+	unsigned int fSignalReset;
+	tShbError ShbError;
+	tShbError ShbError2;
+
+	// check arguments
+	if ((pShbInstance_p == NULL) || (pShbCirChunk_p == NULL)
+	    || (pfBufferCompleted_p == NULL)) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	if ((pSrcDataChunk_p == NULL) || (ulDataChunkSize_p == 0)) {
+		// nothing to do here
+		ShbError = kShbOk;
+		goto Exit;
+	}
+
+	if (pShbCirChunk_p->m_fBufferCompleted) {
+		ShbError = kShbBufferAlreadyCompleted;
+		goto Exit;
+	}
+
+	if (ulDataChunkSize_p > pShbCirChunk_p->m_ulAvailableSize) {
+		ShbError = kShbExceedDataSizeLimit;
+		goto Exit;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	pScrDataPtr = (unsigned char *)pSrcDataChunk_p;
+	fSignalNewData = FALSE;
+	fSignalReset = FALSE;
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	ulWrIndex = pShbCirChunk_p->m_ulWrIndex;
+
+	// copy the data to the circular buffer
+	// (the copy process itself will be done outside of any
+	// critical/locked section)
+	pShbCirDataPtr = &pShbCirBuff->m_Data;	// ptr to start of data area
+
+	if (ulWrIndex + ulDataChunkSize_p <= pShbCirBuff->m_ulBufferDataSize) {
+		// linear write operation
+		memcpy(pShbCirDataPtr + ulWrIndex, pScrDataPtr,
+		       ulDataChunkSize_p);
+	} else {
+		// wrap-around write operation
+		ulSubChunkSize = pShbCirBuff->m_ulBufferDataSize - ulWrIndex;
+		memcpy(pShbCirDataPtr + ulWrIndex, pScrDataPtr, ulSubChunkSize);
+		memcpy(pShbCirDataPtr, pScrDataPtr + ulSubChunkSize,
+		       ulDataChunkSize_p - ulSubChunkSize);
+	}
+
+	// adjust chunk descriptor
+	ulWrIndex += ulDataChunkSize_p;
+	ulWrIndex %= pShbCirBuff->m_ulBufferDataSize;
+
+	pShbCirChunk_p->m_ulAvailableSize -= ulDataChunkSize_p;
+	pShbCirChunk_p->m_ulWrIndex = ulWrIndex;
+
+	fBufferCompleted = (pShbCirChunk_p->m_ulAvailableSize == 0);
+	pShbCirChunk_p->m_fBufferCompleted = fBufferCompleted;
+
+	// if the complete allocated buffer is filled with data then
+	// adjust header information for circular buffer with properties
+	// of the wiritten data block
+	if (fBufferCompleted) {
+		ShbIpcEnterAtomicSection(pShbInstance_p);
+		{
+			pShbCirBuff->m_ulDataApended +=
+			    pShbCirChunk_p->m_uiFullBlockSize;
+			pShbCirBuff->m_ulBlocksApended++;
+
+			// decrement number of currently (parallel running) write operations
+			if (!--pShbCirBuff->m_ulNumOfWriteJobs) {
+				// if there is no other write process running then
+				// set new size of readable (complete written) data and
+				// adjust number of readable blocks
+				pShbCirBuff->m_ulDataReadable +=
+				    pShbCirBuff->m_ulDataApended;
+				pShbCirBuff->m_ulBlocksReadable +=
+				    pShbCirBuff->m_ulBlocksApended;
+
+				pShbCirBuff->m_ulDataApended = 0;
+				pShbCirBuff->m_ulBlocksApended = 0;
+
+				fSignalNewData = TRUE;
+				fSignalReset = pShbCirBuff->m_fBufferLocked;
+			}
+		}
+		ShbIpcLeaveAtomicSection(pShbInstance_p);
+	}
+
+	// signal new data event to a potentially reading application
+	if (fSignalNewData) {
+		ShbError2 = ShbIpcSignalNewData(pShbInstance_p);
+		if (ShbError == kShbOk) {
+			ShbError = ShbError2;
+		}
+	}
+	// signal that the last write job has been finished to allow
+	// a waiting application to reset the buffer now
+	if (fSignalReset) {
+		ShbError2 = ShbIpcSignalJobReady(pShbInstance_p);
+		if (ShbError == kShbOk) {
+			ShbError = ShbError2;
+		}
+	}
+
+	*pfBufferCompleted_p = fBufferCompleted;
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Read data block from Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirReadDataBlock(tShbInstance pShbInstance_p,
+					      void *pDstDataBlock_p,
+					      unsigned long ulRdBuffSize_p,
+					      unsigned long *pulDataBlockSize_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	tShbCirBlockSize ShbCirBlockSize;
+	unsigned long ulDataReadable;
+	unsigned char *pShbCirDataPtr;
+	unsigned char *pDstDataPtr;
+	unsigned long ulDataSize = 0;	// d.k. GCC complains about uninitialized variable otherwise
+	unsigned long ulChunkSize;
+	unsigned long ulRdIndex;
+	tShbError ShbError;
+
+	// check arguments
+	if ((pShbInstance_p == NULL) || (pulDataBlockSize_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+
+	if ((pDstDataBlock_p == NULL) || (ulRdBuffSize_p == 0)) {
+		// nothing to do here
+		ShbError = kShbOk;
+		goto Exit;
+	}
+
+	ShbError = kShbOk;
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	pDstDataPtr = (unsigned char *)pDstDataBlock_p;
+	ulDataSize = 0;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	// get total number of readable bytes for the whole circular buffer
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		ulDataReadable = pShbCirBuff->m_ulDataReadable;
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	// if there are readable data available, then there must be at least
+	// one complete readable data block
+	if (ulDataReadable > 0) {
+		// get pointer to start of data area and current read index
+		pShbCirDataPtr = &pShbCirBuff->m_Data;	// ptr to start of data area
+		ulRdIndex = pShbCirBuff->m_ulRdIndex;
+
+		// get real size of current block (incl. alignment fill bytes)
+		ShbCirBlockSize =
+		    *(tShbCirBlockSize *) (pShbCirDataPtr + ulRdIndex);
+		ulRdIndex += sizeof(tShbCirBlockSize);
+		ulRdIndex %= pShbCirBuff->m_ulBufferDataSize;
+
+		// get size of user data inside the current block
+		ulDataSize =
+		    ShbCirBlockSize.m_uiFullBlockSize -
+		    ShbCirBlockSize.m_uiAlignFillBytes;
+		ulDataSize -= sizeof(tShbCirBlockSize);
+	}
+
+	// ulDataSize = MIN(ulDataSize, ulRdBuffSize_p);
+	if (ulDataSize > ulRdBuffSize_p) {
+		ulDataSize = ulRdBuffSize_p;
+		ShbError = kShbDataTruncated;
+	}
+
+	if (ulDataSize == 0) {
+		// nothing to do here
+		ShbError = kShbNoReadableData;
+		goto Exit;
+	}
+
+	// copy the data from the circular buffer
+	// (the copy process itself will be done outside of any
+	// critical/locked section)
+	if (ulRdIndex + ulDataSize <= pShbCirBuff->m_ulBufferDataSize) {
+		// linear read operation
+		memcpy(pDstDataPtr, pShbCirDataPtr + ulRdIndex, ulDataSize);
+	} else {
+		// wrap-around read operation
+		ulChunkSize = pShbCirBuff->m_ulBufferDataSize - ulRdIndex;
+		memcpy(pDstDataPtr, pShbCirDataPtr + ulRdIndex, ulChunkSize);
+		memcpy(pDstDataPtr + ulChunkSize, pShbCirDataPtr,
+		       ulDataSize - ulChunkSize);
+	}
+
+#ifndef NDEBUG
+	{
+		tShbCirBlockSize ClrShbCirBlockSize;
+
+		if (ulRdIndex + ulDataSize <= pShbCirBuff->m_ulBufferDataSize) {
+			// linear buffer
+			memset(pShbCirDataPtr + ulRdIndex, 0xDD, ulDataSize);
+		} else {
+			// wrap-around read operation
+			ulChunkSize =
+			    pShbCirBuff->m_ulBufferDataSize - ulRdIndex;
+			memset(pShbCirDataPtr + ulRdIndex, 0xDD, ulChunkSize);
+			memset(pShbCirDataPtr, 0xDD, ulDataSize - ulChunkSize);
+		}
+
+		ClrShbCirBlockSize.m_uiFullBlockSize = /*(unsigned int) */ -1;	// -1 = xFFFFFFF
+		ClrShbCirBlockSize.m_uiAlignFillBytes = /*(unsigned int) */ -1;	// -1 = Fxxxxxxx
+		*(tShbCirBlockSize *) (pShbCirDataPtr +
+				       pShbCirBuff->m_ulRdIndex) =
+		    ClrShbCirBlockSize;
+	}
+#endif // #ifndef NDEBUG
+
+	// set new size of readable data, data in use, new read index
+	// and adjust number of readable blocks
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		pShbCirBuff->m_ulDataInUse -= ShbCirBlockSize.m_uiFullBlockSize;
+		pShbCirBuff->m_ulDataReadable -=
+		    ShbCirBlockSize.m_uiFullBlockSize;
+		pShbCirBuff->m_ulBlocksReadable--;
+
+		//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
+		if ((pShbCirBuff->m_ulDataInUse == 0)
+		    && (pShbCirBuff->m_ulDataReadable == 0)) {
+			ASSERT(pShbCirBuff->m_ulBlocksReadable == 0);
+
+			pShbCirBuff->m_ulWrIndex = 0;
+			pShbCirBuff->m_ulRdIndex = 0;
+		} else
+			//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
+		{
+			pShbCirBuff->m_ulRdIndex +=
+			    ShbCirBlockSize.m_uiFullBlockSize;
+			pShbCirBuff->m_ulRdIndex %=
+			    pShbCirBuff->m_ulBufferDataSize;
+		}
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+      Exit:
+
+	*pulDataBlockSize_p = ulDataSize;
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Get data size of next readable block from Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirGetReadDataSize(tShbInstance pShbInstance_p,
+						unsigned long
+						*pulDataBlockSize_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	unsigned long ulDataReadable;
+	unsigned char *pShbCirDataPtr;
+	tShbCirBlockSize ShbCirBlockSize;
+	unsigned long ulDataSize;
+	tShbError ShbError;
+
+	// check arguments
+	if ((pShbInstance_p == NULL) || (pulDataBlockSize_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	ulDataSize = 0;
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	// get total number of readable bytes for the whole circular buffer
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		ulDataReadable = pShbCirBuff->m_ulDataReadable;
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	// if there are readable data available, then there must be at least
+	// one complete readable data block
+	if (ulDataReadable > 0) {
+		pShbCirDataPtr =
+		    &pShbCirBuff->m_Data + pShbCirBuff->m_ulRdIndex;
+
+		// get real size of current block (incl. alignment fill bytes)
+		ShbCirBlockSize = *(tShbCirBlockSize *) pShbCirDataPtr;
+
+		// get size of user data inside the current block
+		ulDataSize =
+		    ShbCirBlockSize.m_uiFullBlockSize -
+		    ShbCirBlockSize.m_uiAlignFillBytes;
+		ulDataSize -= sizeof(tShbCirBlockSize);
+	}
+
+      Exit:
+
+	*pulDataBlockSize_p = ulDataSize;
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Get number of readable blocks from Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirGetReadBlockCount(tShbInstance pShbInstance_p,
+						  unsigned long
+						  *pulDataBlockCount_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	unsigned long ulBlockCount;
+	tShbError ShbError;
+
+	// check arguments
+	if ((pShbInstance_p == NULL) || (pulDataBlockCount_p == NULL)) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	ulBlockCount = 0;
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		ulBlockCount = pShbCirBuff->m_ulBlocksReadable;
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	*pulDataBlockCount_p = ulBlockCount;
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Set application handler to signal new data for Circular Shared Buffer
+//  d.k.: new parameter priority as enum
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirSetSignalHandlerNewData(tShbInstance
+							pShbInstance_p,
+							tShbCirSigHndlrNewData
+							pfnSignalHandlerNewData_p,
+							tShbPriority
+							ShbPriority_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	tShbError ShbError;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	if (pfnSignalHandlerNewData_p != NULL) {
+		// set a new signal handler
+		if (pShbCirBuff->m_pfnSigHndlrNewData != NULL) {
+			ShbError = kShbAlreadySignaling;
+			goto Exit;
+		}
+
+		pShbCirBuff->m_pfnSigHndlrNewData = pfnSignalHandlerNewData_p;
+		ShbError =
+		    ShbIpcStartSignalingNewData(pShbInstance_p,
+						ShbCirSignalHandlerNewData,
+						ShbPriority_p);
+	} else {
+		// remove existing signal handler
+		ShbError = ShbIpcStopSignalingNewData(pShbInstance_p);
+		if (pShbCirBuff->m_pfnSigHndlrNewData != NULL) {
+			pShbCirBuff->m_pfnSigHndlrNewData(pShbInstance_p, 0);
+		}
+		pShbCirBuff->m_pfnSigHndlrNewData = NULL;
+	}
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+#endif
+
+#if !defined(INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  DEBUG: Trace Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+#ifndef NDEBUG
+tShbError ShbCirTraceBuffer(tShbInstance pShbInstance_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	char szMagigID[sizeof(SBC_MAGIC_ID) + 1];
+	tShbCirBlockSize ShbCirBlockSize;
+	unsigned long ulDataReadable;
+	unsigned char *pShbCirDataPtr;
+	unsigned long ulBlockIndex;
+	unsigned int nBlockCount;
+	unsigned long ulDataSize;
+	unsigned long ulChunkSize;
+	unsigned long ulRdIndex;
+	tShbError ShbError;
+
+	TRACE0("\n\n##### Circular Shared Buffer #####\n");
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		TRACE1("\nERROR: invalid buffer address (0x%08lX)\n",
+		       (unsigned long)pShbInstance_p);
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	*(unsigned long *)&szMagigID[0] = pShbCirBuff->m_ShbCirMagicID;
+	szMagigID[sizeof(SBC_MAGIC_ID)] = '\0';
+
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		TRACE1("\nBuffer Address:   0x%08lX\n",
+		       (unsigned long)pShbCirBuff);
+
+		TRACE0("\nHeader Info:");
+		TRACE2("\nMagigID:          '%s' (%08lX)", szMagigID,
+		       pShbCirBuff->m_ShbCirMagicID);
+		TRACE1("\nBufferTotalSize:  %4lu [Bytes]",
+		       pShbCirBuff->m_ulBufferTotalSize);
+		TRACE1("\nBufferDataSize:   %4lu [Bytes]",
+		       pShbCirBuff->m_ulBufferDataSize);
+		TRACE1("\nWrIndex:          %4lu", pShbCirBuff->m_ulWrIndex);
+		TRACE1("\nRdIndex:          %4lu", pShbCirBuff->m_ulRdIndex);
+		TRACE1("\nNumOfWriteJobs:   %4lu",
+		       pShbCirBuff->m_ulNumOfWriteJobs);
+		TRACE1("\nDataInUse:        %4lu [Bytes]",
+		       pShbCirBuff->m_ulDataInUse);
+		TRACE1("\nDataApended:      %4lu [Bytes]",
+		       pShbCirBuff->m_ulDataApended);
+		TRACE1("\nBlocksApended:    %4lu",
+		       pShbCirBuff->m_ulBlocksApended);
+		TRACE1("\nDataReadable:     %4lu [Bytes]",
+		       pShbCirBuff->m_ulDataReadable);
+		TRACE1("\nBlocksReadable:   %4lu",
+		       pShbCirBuff->m_ulBlocksReadable);
+		TRACE1("\nSigHndlrNewData:  %08lX",
+		       (unsigned long)pShbCirBuff->m_pfnSigHndlrNewData);
+		TRACE1("\nBufferLocked:     %d", pShbCirBuff->m_fBufferLocked);
+		TRACE1("\nSigHndlrReset:    %08lX",
+		       (unsigned long)pShbCirBuff->m_pfnSigHndlrReset);
+
+		ShbTraceDump(&pShbCirBuff->m_Data,
+			     pShbCirBuff->m_ulBufferDataSize, 0x00000000L,
+			     "\nData Area:");
+
+		ulDataReadable = pShbCirBuff->m_ulDataReadable;
+		nBlockCount = 1;
+		ulBlockIndex = pShbCirBuff->m_ulRdIndex;
+
+		while (ulDataReadable > 0) {
+			TRACE1("\n\n--- Block #%u ---", nBlockCount);
+
+			// get pointer to start of data area and current read index
+			pShbCirDataPtr = &pShbCirBuff->m_Data;	// ptr to start of data area
+			ulRdIndex = ulBlockIndex;
+
+			// get real size of current block (incl. alignment fill bytes)
+			ShbCirBlockSize =
+			    *(tShbCirBlockSize *) (pShbCirDataPtr + ulRdIndex);
+			ulRdIndex += sizeof(tShbCirBlockSize);
+			ulRdIndex %= pShbCirBuff->m_ulBufferDataSize;
+
+			// get size of user data inside the current block
+			ulDataSize =
+			    ShbCirBlockSize.m_uiFullBlockSize -
+			    ShbCirBlockSize.m_uiAlignFillBytes;
+			ulDataSize -= sizeof(tShbCirBlockSize);
+
+			TRACE1
+			    ("\nFull Data Size:       %4u [Bytes] (incl. header and alignment fill bytes)",
+			     ShbCirBlockSize.m_uiFullBlockSize);
+			TRACE1("\nUser Data Size:       %4lu [Bytes]",
+			       ulDataSize);
+			TRACE1("\nAlignment Fill Bytes: %4u [Bytes]",
+			       ShbCirBlockSize.m_uiAlignFillBytes);
+
+			if (ulRdIndex + ulDataSize <=
+			    pShbCirBuff->m_ulBufferDataSize) {
+				// linear data buffer
+				ShbTraceDump(pShbCirDataPtr + ulRdIndex,
+					     ulDataSize, 0x00000000L, NULL);
+			} else {
+				// wrap-around data buffer
+				ulChunkSize =
+				    pShbCirBuff->m_ulBufferDataSize - ulRdIndex;
+				ShbTraceDump(pShbCirDataPtr + ulRdIndex,
+					     ulChunkSize, 0x00000000L, NULL);
+				ShbTraceDump(pShbCirDataPtr,
+					     ulDataSize - ulChunkSize,
+					     ulChunkSize, NULL);
+			}
+
+			nBlockCount++;
+
+			ulBlockIndex += ShbCirBlockSize.m_uiFullBlockSize;
+			ulBlockIndex %= pShbCirBuff->m_ulBufferDataSize;
+
+			ulDataReadable -= ShbCirBlockSize.m_uiFullBlockSize;
+		}
+
+		ASSERT(pShbCirBuff->m_ulBlocksReadable == nBlockCount - 1);
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+      Exit:
+
+	return (ShbError);
+
+}
+#endif
+
+//-------------------------------------------------------------------------//
+//                                                                         //
+//          L i n e a r   S h a r e d   B u f f e r                        //
+//                                                                         //
+//-------------------------------------------------------------------------//
+
+//---------------------------------------------------------------------------
+//  Allocate Linear Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbLinAllocBuffer(unsigned long ulBufferSize_p,
+			    const char *pszBufferID_p,
+			    tShbInstance * ppShbInstance_p,
+			    unsigned int *pfShbNewCreated_p)
+{
+
+	tShbInstance pShbInstance;
+	tShbLinBuff *pShbLinBuff;
+	unsigned int fShbNewCreated;
+	unsigned long ulBufferDataSize;
+	unsigned long ulBufferTotalSize;
+	tShbError ShbError;
+
+	// check arguments
+	if ((ulBufferSize_p == 0) || (ppShbInstance_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+
+	// calculate length of memory to allocate
+	ulBufferDataSize =
+	    (ulBufferSize_p +
+	     (SBL_BLOCK_ALIGNMENT - 1)) & ~(SBL_BLOCK_ALIGNMENT - 1);
+	ulBufferTotalSize = ulBufferDataSize + sizeof(tShbLinBuff);
+
+	// allocate a new or open an existing shared buffer
+	ShbError = ShbIpcAllocBuffer(ulBufferTotalSize, pszBufferID_p,
+				     &pShbInstance, &fShbNewCreated);
+	if (ShbError != kShbOk) {
+		goto Exit;
+	}
+
+	if (pShbInstance == NULL) {
+		ShbError = kShbOutOfMem;
+		goto Exit;
+	}
+
+	// get pointer to shared buffer
+	pShbLinBuff = (tShbLinBuff *) ShbIpcGetShMemPtr(pShbInstance);
+
+	// if the shared buffer was new created, than this process has
+	// to initialize it, otherwise the buffer is already in use
+	// and *must not* be reseted
+	if (fShbNewCreated) {
+#ifndef NDEBUG
+		{
+			memset(pShbLinBuff, 0xCC, ulBufferTotalSize);
+		}
+#endif
+
+		pShbLinBuff->m_ShbLinMagicID = SBL_MAGIC_ID;
+		pShbLinBuff->m_ulBufferTotalSize = ulBufferTotalSize;
+		pShbLinBuff->m_ulBufferDataSize = ulBufferDataSize;
+	} else {
+		if (pShbLinBuff->m_ShbLinMagicID != SBL_MAGIC_ID) {
+			ShbError = kShbInvalidBufferType;
+			goto Exit;
+		}
+	}
+
+      Exit:
+
+	*ppShbInstance_p = pShbInstance;
+	*pfShbNewCreated_p = fShbNewCreated;
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Release Linear Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbLinReleaseBuffer(tShbInstance pShbInstance_p)
+{
+
+	tShbError ShbError;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbOk;
+		goto Exit;
+	}
+
+	ShbError = ShbIpcReleaseBuffer(pShbInstance_p);
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+#endif // !defined(INLINE_ENABLED)
+
+#if (!defined(SHAREDBUFF_INLINED)) || defined(INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Write data block to Linear Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbLinWriteDataBlock(tShbInstance pShbInstance_p,
+					       unsigned long ulDstBufferOffs_p,
+					       const void *pSrcDataBlock_p,
+					       unsigned long ulDataBlockSize_p)
+{
+
+	tShbLinBuff *pShbLinBuff;
+	unsigned char *pShbLinDataPtr;
+	unsigned char *pScrDataPtr;
+	unsigned long ulBufferDataSize;
+	tShbError ShbError;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	if ((pSrcDataBlock_p == NULL) || (ulDataBlockSize_p == 0)) {
+		// nothing to do here
+		ShbError = kShbOk;
+		goto Exit;
+	}
+
+	if (ulDataBlockSize_p > SBL_MAX_BLOCK_SIZE) {
+		ShbError = kShbExceedDataSizeLimit;
+		goto Exit;
+	}
+
+	pShbLinBuff = ShbLinGetBuffer(pShbInstance_p);
+	pScrDataPtr = (unsigned char *)pSrcDataBlock_p;
+	ShbError = kShbOk;
+
+	if (pShbLinBuff->m_ShbLinMagicID != SBL_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	// check if offeset and size for the write operation matches with
+	// the size of the shared buffer
+	ulBufferDataSize = pShbLinBuff->m_ulBufferDataSize;
+	if ((ulDstBufferOffs_p > ulBufferDataSize) ||
+	    (ulDataBlockSize_p > ulBufferDataSize) ||
+	    ((ulDstBufferOffs_p + ulDataBlockSize_p) > ulBufferDataSize)) {
+		ShbError = kShbDataOutsideBufferArea;
+		goto Exit;
+	}
+
+	// copy the data to the linear buffer
+	// (the copy process will be done inside of any critical/locked section)
+	pShbLinDataPtr = &pShbLinBuff->m_Data;	// ptr to start of data area
+	pShbLinDataPtr += ulDstBufferOffs_p;
+
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		memcpy(pShbLinDataPtr, pScrDataPtr, ulDataBlockSize_p);
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Read data block from Linear Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbLinReadDataBlock(tShbInstance pShbInstance_p,
+					      void *pDstDataBlock_p,
+					      unsigned long ulSrcBufferOffs_p,
+					      unsigned long ulDataBlockSize_p)
+{
+
+	tShbLinBuff *pShbLinBuff;
+	unsigned char *pShbLinDataPtr;
+	unsigned char *pDstDataPtr;
+	unsigned long ulBufferDataSize;
+	tShbError ShbError;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	if ((pDstDataBlock_p == NULL) || (ulDataBlockSize_p == 0)) {
+		// nothing to do here
+		ShbError = kShbOk;
+		goto Exit;
+	}
+
+	if (ulDataBlockSize_p > SBL_MAX_BLOCK_SIZE) {
+		ShbError = kShbExceedDataSizeLimit;
+		goto Exit;
+	}
+
+	pShbLinBuff = ShbLinGetBuffer(pShbInstance_p);
+	pDstDataPtr = (unsigned char *)pDstDataBlock_p;
+	ShbError = kShbOk;
+
+	if (pShbLinBuff->m_ShbLinMagicID != SBL_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	// check if offeset and size for the read operation matches with
+	// the size of the shared buffer
+	ulBufferDataSize = pShbLinBuff->m_ulBufferDataSize;
+	if ((ulSrcBufferOffs_p > ulBufferDataSize) ||
+	    (ulDataBlockSize_p > ulBufferDataSize) ||
+	    ((ulSrcBufferOffs_p + ulDataBlockSize_p) > ulBufferDataSize)) {
+		ShbError = kShbDataOutsideBufferArea;
+		goto Exit;
+	}
+
+	// copy the data to the linear buffer
+	// (the copy process will be done inside of any critical/locked section)
+	pShbLinDataPtr = &pShbLinBuff->m_Data;	// ptr to start of data area
+	pShbLinDataPtr += ulSrcBufferOffs_p;
+
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		memcpy(pDstDataPtr, pShbLinDataPtr, ulDataBlockSize_p);
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+#endif
+
+#if !defined(INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  DEBUG: Trace Linear Shared Buffer
+//---------------------------------------------------------------------------
+
+#ifndef NDEBUG
+tShbError ShbLinTraceBuffer(tShbInstance pShbInstance_p)
+{
+
+	tShbLinBuff *pShbLinBuff;
+	char szMagigID[sizeof(SBL_MAGIC_ID) + 1];
+	tShbError ShbError;
+
+	TRACE0("\n\n##### Linear Shared Buffer #####\n");
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		TRACE1("\nERROR: invalid buffer address (0x%08lX)\n",
+		       (unsigned long)pShbInstance_p);
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	pShbLinBuff = ShbLinGetBuffer(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if (pShbLinBuff->m_ShbLinMagicID != SBL_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	*(unsigned int *)&szMagigID[0] = pShbLinBuff->m_ShbLinMagicID;
+	szMagigID[sizeof(SBL_MAGIC_ID)] = '\0';
+
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		TRACE1("\nBuffer Address:   0x%08lX\n",
+		       (unsigned long)pShbLinBuff);
+
+		TRACE0("\nHeader Info:");
+		TRACE2("\nMagigID:          '%s' (%08X)", szMagigID,
+		       pShbLinBuff->m_ShbLinMagicID);
+		TRACE1("\nBufferTotalSize:  %4lu [Bytes]",
+		       pShbLinBuff->m_ulBufferTotalSize);
+		TRACE1("\nBufferDataSize:   %4lu [Bytes]",
+		       pShbLinBuff->m_ulBufferDataSize);
+
+		ShbTraceDump(&pShbLinBuff->m_Data,
+			     pShbLinBuff->m_ulBufferDataSize, 0x00000000L,
+			     "\nData Area:");
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+      Exit:
+
+	return (ShbError);
+
+}
+#endif
+
+//---------------------------------------------------------------------------
+//  Dump buffer contents
+//---------------------------------------------------------------------------
+
+#ifndef NDEBUG
+tShbError ShbTraceDump(const unsigned char *pabStartAddr_p,
+		       unsigned long ulDataSize_p,
+		       unsigned long ulAddrOffset_p, const char *pszInfoText_p)
+{
+
+	const unsigned char *pabBuffData;
+	unsigned long ulBuffSize;
+	unsigned char bData;
+	int nRow;
+	int nCol;
+
+	// get pointer to buffer and length of buffer
+	pabBuffData = pabStartAddr_p;
+	ulBuffSize = ulDataSize_p;
+
+	if (pszInfoText_p != NULL) {
+		TRACE0(pszInfoText_p);
+	}
+	// dump buffer contents
+	for (nRow = 0;; nRow++) {
+		TRACE1("\n%08lX:   ",
+		       (unsigned long)(nRow * 0x10) + ulAddrOffset_p);
+
+		for (nCol = 0; nCol < 16; nCol++) {
+			if ((unsigned long)nCol < ulBuffSize) {
+				TRACE1("%02X ",
+				       (unsigned int)*(pabBuffData + nCol));
+			} else {
+				TRACE0("   ");
+			}
+		}
+
+		TRACE0(" ");
+
+		for (nCol = 0; nCol < 16; nCol++) {
+			bData = *pabBuffData++;
+			if ((unsigned long)nCol < ulBuffSize) {
+				if ((bData >= 0x20) && (bData < 0x7F)) {
+					TRACE1("%c", bData);
+				} else {
+					TRACE0(".");
+				}
+			} else {
+				TRACE0(" ");
+			}
+		}
+
+		if (ulBuffSize > 16) {
+			ulBuffSize -= 16;
+		} else {
+			break;
+		}
+	}
+
+	return (kShbOk);
+
+}
+#endif // #ifndef NDEBUG
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//  Handler to signal new data event for Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+int ShbCirSignalHandlerNewData(tShbInstance pShbInstance_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	unsigned long ulDataSize;
+	unsigned long ulBlockCount;
+	tShbError ShbError;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		return FALSE;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		return FALSE;
+	}
+
+	// call application handler
+	if (pShbCirBuff->m_pfnSigHndlrNewData != NULL) {
+/*        do
+        {*/
+		ShbError = ShbCirGetReadDataSize(pShbInstance_p, &ulDataSize);
+		if ((ulDataSize > 0) && (ShbError == kShbOk)) {
+			pShbCirBuff->m_pfnSigHndlrNewData(pShbInstance_p,
+							  ulDataSize);
+		}
+
+		ShbError =
+		    ShbCirGetReadBlockCount(pShbInstance_p, &ulBlockCount);
+/*        }
+        while ((ulBlockCount > 0) && (ShbError == kShbOk));*/
+	}
+	// Return TRUE if there are pending blocks.
+	// In that case ShbIpc tries to call this function again immediately if there
+	// is no other filled shared buffer with higher priority.
+	return ((ulBlockCount > 0) && (ShbError == kShbOk));
+
+}
+
+//---------------------------------------------------------------------------
+//  Handler to reset Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+void ShbCirSignalHandlerReset(tShbInstance pShbInstance_p,
+			      unsigned int fTimeOut_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		return;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		return;
+	}
+
+	// reset buffer header
+	if (!fTimeOut_p) {
+		ShbIpcEnterAtomicSection(pShbInstance_p);
+		{
+			pShbCirBuff->m_ulWrIndex = 0;
+			pShbCirBuff->m_ulRdIndex = 0;
+			pShbCirBuff->m_ulNumOfWriteJobs = 0;
+			pShbCirBuff->m_ulDataInUse = 0;
+			pShbCirBuff->m_ulDataApended = 0;
+			pShbCirBuff->m_ulBlocksApended = 0;
+			pShbCirBuff->m_ulDataReadable = 0;
+			pShbCirBuff->m_ulBlocksReadable = 0;
+		}
+		ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+#ifndef NDEBUG
+		{
+			memset(&pShbCirBuff->m_Data, 0xCC,
+			       pShbCirBuff->m_ulBufferDataSize);
+		}
+#endif
+	}
+
+	// call application handler
+	if (pShbCirBuff->m_pfnSigHndlrReset != NULL) {
+		pShbCirBuff->m_pfnSigHndlrReset(pShbInstance_p, fTimeOut_p);
+	}
+
+	// unlock buffer
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		pShbCirBuff->m_fBufferLocked = FALSE;
+		pShbCirBuff->m_pfnSigHndlrReset = NULL;
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	return;
+
+}
+
+#endif
+
+// EOF
diff --git a/drivers/staging/epl/SharedBuff.h b/drivers/staging/epl/SharedBuff.h
new file mode 100644
index 000000000000..0ec1b4b9e6ae
--- /dev/null
+++ b/drivers/staging/epl/SharedBuff.h
@@ -0,0 +1,204 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      Project independend shared buffer (linear + circular)
+
+  Description:  Declaration of platform independend part for the
+                shared buffer
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+  2006/06/27 -rs:   V 1.00 (initial version)
+
+****************************************************************************/
+
+#ifndef _SHAREDBUFF_H_
+#define _SHAREDBUFF_H_
+
+//---------------------------------------------------------------------------
+//  Type definitions
+//---------------------------------------------------------------------------
+
+typedef enum {
+	kShbOk = 0,
+	kShbNoReadableData = 1,
+	kShbDataTruncated = 2,
+	kShbBufferFull = 3,
+	kShbDataOutsideBufferArea = 4,
+	kShbBufferAlreadyCompleted = 5,
+	kShbMemUsedByOtherProcs = 6,
+	kShbOpenMismatch = 7,
+	kShbInvalidBufferType = 8,
+	kShbInvalidArg = 9,
+	kShbBufferInvalid = 10,
+	kShbOutOfMem = 11,
+	kShbAlreadyReseting = 12,
+	kShbAlreadySignaling = 13,
+	kShbExceedDataSizeLimit = 14,
+
+} tShbError;
+
+// 2006/08/24 d.k.: Priority for threads (new data, job signaling)
+typedef enum {
+	kShbPriorityLow = 0,
+	kShbPriorityNormal = 1,
+	kshbPriorityHigh = 2
+} tShbPriority;
+
+typedef struct {
+	unsigned int m_uiFullBlockSize;	// real size of allocated block (incl. alignment fill bytes)
+	unsigned long m_ulAvailableSize;	// still available size for data
+	unsigned long m_ulWrIndex;	// current write index
+	unsigned int m_fBufferCompleted;	// TRUE if allocated block is complete filled with data
+
+} tShbCirChunk;
+
+typedef void *tShbInstance;
+
+typedef void (*tShbCirSigHndlrNewData) (tShbInstance pShbInstance_p,
+					unsigned long ulDataBlockSize_p);
+typedef void (*tShbCirSigHndlrReset) (tShbInstance pShbInstance_p,
+				      unsigned int fTimeOut_p);
+
+//---------------------------------------------------------------------------
+//  Prototypes
+//---------------------------------------------------------------------------
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*#if defined(INLINE_FUNCTION_DEF)
+    #undef  INLINE_FUNCTION
+    #define INLINE_FUNCTION     INLINE_FUNCTION_DEF
+    #define INLINE_ENABLED      TRUE
+    #define SHAREDBUFF_INLINED
+    #include "SharedBuff.c"
+#endif
+*/
+
+	tShbError ShbInit(void);
+	tShbError ShbExit(void);
+
+// Circular Shared Buffer
+	tShbError ShbCirAllocBuffer(unsigned long ulBufferSize_p,
+				    const char *pszBufferID_p,
+				    tShbInstance * ppShbInstance_p,
+				    unsigned int *pfShbNewCreated_p);
+	tShbError ShbCirReleaseBuffer(tShbInstance pShbInstance_p);
+
+#if !defined(INLINE_ENABLED)
+
+	tShbError ShbCirResetBuffer(tShbInstance pShbInstance_p,
+				    unsigned long ulTimeOut_p,
+				    tShbCirSigHndlrReset
+				    pfnSignalHandlerReset_p);
+	tShbError ShbCirWriteDataBlock(tShbInstance pShbInstance_p,
+				       const void *pSrcDataBlock_p,
+				       unsigned long ulDataBlockSize_p);
+	tShbError ShbCirAllocDataBlock(tShbInstance pShbInstance_p,
+				       tShbCirChunk * pShbCirChunk_p,
+				       unsigned long ulDataBufferSize_p);
+	tShbError ShbCirWriteDataChunk(tShbInstance pShbInstance_p,
+				       tShbCirChunk * pShbCirChunk_p,
+				       const void *pSrcDataChunk_p,
+				       unsigned long ulDataChunkSize_p,
+				       unsigned int *pfBufferCompleted_p);
+	tShbError ShbCirReadDataBlock(tShbInstance pShbInstance_p,
+				      void *pDstDataBlock_p,
+				      unsigned long ulRdBuffSize_p,
+				      unsigned long *pulDataBlockSize_p);
+	tShbError ShbCirGetReadDataSize(tShbInstance pShbInstance_p,
+					unsigned long *pulDataBlockSize_p);
+	tShbError ShbCirGetReadBlockCount(tShbInstance pShbInstance_p,
+					  unsigned long *pulDataBlockCount_p);
+	tShbError ShbCirSetSignalHandlerNewData(tShbInstance pShbInstance_p,
+						tShbCirSigHndlrNewData
+						pfnShbSignalHandlerNewData_p,
+						tShbPriority ShbPriority_p);
+
+#endif
+
+// Linear Shared Buffer
+	tShbError ShbLinAllocBuffer(unsigned long ulBufferSize_p,
+				    const char *pszBufferID_p,
+				    tShbInstance * ppShbInstance_p,
+				    unsigned int *pfShbNewCreated_p);
+	tShbError ShbLinReleaseBuffer(tShbInstance pShbInstance_p);
+
+#if !defined(INLINE_ENABLED)
+
+	tShbError ShbLinWriteDataBlock(tShbInstance pShbInstance_p,
+				       unsigned long ulDstBufferOffs_p,
+				       const void *pSrcDataBlock_p,
+				       unsigned long ulDataBlockSize_p);
+	tShbError ShbLinReadDataBlock(tShbInstance pShbInstance_p,
+				      void *pDstDataBlock_p,
+				      unsigned long ulSrcBufferOffs_p,
+				      unsigned long ulDataBlockSize_p);
+
+#endif
+
+#ifndef NDEBUG
+	tShbError ShbCirTraceBuffer(tShbInstance pShbInstance_p);
+	tShbError ShbLinTraceBuffer(tShbInstance pShbInstance_p);
+	tShbError ShbTraceDump(const unsigned char *pabStartAddr_p,
+			       unsigned long ulDataSize_p,
+			       unsigned long ulAddrOffset_p,
+			       const char *pszInfoText_p);
+#else
+#define ShbCirTraceBuffer(p0)
+#define ShbLinTraceBuffer(p0)
+#define ShbTraceDump(p0, p1, p2, p3)
+#endif
+
+#undef  INLINE_ENABLED		// disable actual inlining of functions
+#undef  INLINE_FUNCTION
+#define INLINE_FUNCTION		// define INLINE_FUNCTION to nothing
+
+#ifdef __cplusplus
+}
+#endif
+#endif				// #ifndef _SHAREDBUFF_H_
diff --git a/drivers/staging/epl/ShbIpc-LinuxKernel.c b/drivers/staging/epl/ShbIpc-LinuxKernel.c
new file mode 100644
index 000000000000..1d3cb3f13de1
--- /dev/null
+++ b/drivers/staging/epl/ShbIpc-LinuxKernel.c
@@ -0,0 +1,966 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      Project independend shared buffer (linear + circular)
+
+  Description:  Implementation of platform specific part for the
+                shared buffer
+                (Implementation for Linux KernelSpace)
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+  2006/06/28 -rs:   V 1.00 (initial version)
+
+****************************************************************************/
+
+#include "global.h"
+#include "SharedBuff.h"
+#include "ShbIpc.h"
+#include "ShbLinuxKernel.h"
+#include "Debug.h"
+
+#include <linux/string.h>
+#include <linux/module.h>
+#include <asm/processor.h>
+//#include <linux/vmalloc.h>
+#include <linux/sched.h>
+#include <linux/param.h>
+#include <linux/spinlock.h>
+#include <linux/wait.h>
+#include <linux/completion.h>
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+#if (!defined(SHBIPC_INLINED)) || defined(SHBIPC_INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Configuration
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Constant definitions
+//---------------------------------------------------------------------------
+
+#define MAX_LEN_BUFFER_ID       256
+
+#define TIMEOUT_ENTER_ATOMIC    1000	// (ms) for debgging: INFINITE
+#define TIMEOUT_TERM_THREAD     1000
+#define INFINITE                3600
+
+#define SBI_MAGIC_ID            0x5342492B	// magic ID ("SBI+")
+#define SBH_MAGIC_ID            0x5342482A	// magic ID ("SBH*")
+
+#define INVALID_ID              -1
+
+#define TABLE_SIZE              10
+
+//---------------------------------------------------------------------------
+//  Local types
+//---------------------------------------------------------------------------
+
+// This structure is the common header for the shared memory region used
+// by all processes attached this shared memory. It includes common
+// information to administrate/manage the shared buffer from a couple of
+// separated processes (e.g. the refernce counter). This structure is
+// located at the start of the shared memory region itself and exists
+// consequently only one times per shared memory instance.
+typedef struct {
+
+	unsigned long m_ulShMemSize;
+	unsigned long m_ulRefCount;
+	int m_iBufferId;
+//    int                 m_iUserSpaceMem;           //0 for userspace mem   !=0 kernelspace mem
+	spinlock_t m_SpinlockBuffAccess;
+	BOOL m_fNewData;
+	BOOL m_fJobReady;
+	wait_queue_head_t m_WaitQueueNewData;
+	wait_queue_head_t m_WaitQueueJobReady;
+
+#ifndef NDEBUG
+	unsigned long m_ulOwnerProcID;
+#endif
+
+} tShbMemHeader;
+
+// This structure is the "external entry point" from a separate process
+// to get access to a shared buffer. This structure includes all platform
+// resp. target specific information to administrate/manage the shared
+// buffer from a separate process. Every process attached to the shared
+// buffer has its own runtime instance of this structure with its individual
+// runtime data (e.g. the scope of an event handle is limitted to the
+// owner process only). The structure member <m_pShbMemHeader> points
+// to the (process specific) start address of the shared memory region
+// itself.
+typedef struct {
+	unsigned long m_SbiMagicID;	// magic ID ("SBI+")
+//    void*               m_pSharedMem;
+	int m_tThreadNewDataId;
+	long m_lThreadNewDataNice;	// nice value of the new data thread
+	int m_tThreadJobReadyId;
+	unsigned long m_ulFlagsBuffAccess;	// d.k. moved from tShbMemHeader, because each
+	// process needs to store the interrupt flags separately
+	tSigHndlrNewData m_pfnSigHndlrNewData;
+	unsigned long m_ulTimeOutJobReady;
+	tSigHndlrJobReady m_pfnSigHndlrJobReady;
+	tShbMemHeader *m_pShbMemHeader;
+	int m_iThreadTermFlag;
+	struct completion m_CompletionNewData;
+/*
+    struct semaphore    *m_pSemBuffAccess;
+    struct semaphore    *m_pSemNewData;
+    struct semaphore    *m_pSemStopSignalingNewData;
+    struct semaphore    *m_pSemJobReady;
+*/
+#ifndef NDEBUG
+	unsigned long m_ulThreadIDNewData;
+	unsigned long m_ulThreadIDJobReady;
+#endif
+} tShbMemInst;
+
+//---------------------------------------------------------------------------
+//  Prototypes of internal functions
+//---------------------------------------------------------------------------
+
+//tShbMemInst*            ShbIpcGetShbMemInst         (tShbInstance pShbInstance_p);
+//tShbMemHeader*          ShbIpcGetShbMemHeader       (tShbMemInst* pShbMemInst_p);
+
+//---------------------------------------------------------------------------
+//  Get pointer to process local information structure
+//---------------------------------------------------------------------------
+
+static inline tShbMemInst *ShbIpcGetShbMemInst(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+
+	pShbMemInst = (tShbMemInst *) pShbInstance_p;
+
+	return (pShbMemInst);
+
+}
+
+//---------------------------------------------------------------------------
+//  Get pointer to shared memory header
+//---------------------------------------------------------------------------
+
+static inline tShbMemHeader *ShbIpcGetShbMemHeader(tShbMemInst * pShbMemInst_p)
+{
+
+	tShbMemHeader *pShbMemHeader;
+
+	pShbMemHeader = pShbMemInst_p->m_pShbMemHeader;
+
+	return (pShbMemHeader);
+
+}
+
+//  Get pointer to process local information structure
+//#define ShbIpcGetShbMemInst(pShbInstance_p) ((tShbMemInst*)pShbInstance_p)
+
+//  Get pointer to shared memory header
+//#define ShbIpcGetShbMemHeader(pShbMemInst_p) (pShbMemInst_p->m_pShbMemHeader)
+
+// not inlined internal functions
+int ShbIpcThreadSignalNewData(void *pvThreadParam_p);
+int ShbIpcThreadSignalJobReady(void *pvThreadParam_p);
+#endif
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+#if !defined(SHBIPC_INLINE_ENABLED)
+struct sShbMemTable *psMemTableElementFirst_g;
+
+static void *ShbIpcAllocPrivateMem(unsigned long ulMemSize_p);
+static int ShbIpcFindListElement(int iBufferId,
+				 struct sShbMemTable
+				 **ppsReturnMemTableElement);
+static void ShbIpcAppendListElement(struct sShbMemTable *sNewMemTableElement);
+static void ShbIpcDeleteListElement(int iBufferId);
+static void ShbIpcCrc32GenTable(unsigned long aulCrcTable[256]);
+static unsigned long ShbIpcCrc32GetCrc(const char *pcString,
+				       unsigned long aulCrcTable[256]);
+
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+#if !defined(SHBIPC_INLINE_ENABLED)
+// not inlined external functions
+
+//---------------------------------------------------------------------------
+//  Initialize IPC for Shared Buffer Module
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcInit(void)
+{
+	psMemTableElementFirst_g = NULL;
+	return (kShbOk);
+
+}
+
+//---------------------------------------------------------------------------
+//  Deinitialize IPC for Shared Buffer Module
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcExit(void)
+{
+
+	return (kShbOk);
+
+}
+
+//---------------------------------------------------------------------------
+//  Allocate Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcAllocBuffer(unsigned long ulBufferSize_p,
+			    const char *pszBufferID_p,
+			    tShbInstance * ppShbInstance_p,
+			    unsigned int *pfShbNewCreated_p)
+{
+	tShbError ShbError;
+	int iBufferId = 0;
+	unsigned long ulCrc32 = 0;
+	unsigned int uiFirstProcess = 0;
+	unsigned long ulShMemSize;
+	tShbMemHeader *pShbMemHeader;
+	tShbMemInst *pShbMemInst = NULL;
+	tShbInstance pShbInstance;
+	unsigned int fShMemNewCreated = FALSE;
+	void *pSharedMem = NULL;
+	unsigned long aulCrcTable[256];
+	struct sShbMemTable *psMemTableElement;
+
+	DEBUG_LVL_29_TRACE0("ShbIpcAllocBuffer \n");
+	ulShMemSize = ulBufferSize_p + sizeof(tShbMemHeader);
+
+	//create Buffer ID
+	ShbIpcCrc32GenTable(aulCrcTable);
+	ulCrc32 = ShbIpcCrc32GetCrc(pszBufferID_p, aulCrcTable);
+	iBufferId = ulCrc32;
+	DEBUG_LVL_29_TRACE2
+	    ("ShbIpcAllocBuffer BufferSize:%d sizeof(tShb..):%d\n",
+	     ulBufferSize_p, sizeof(tShbMemHeader));
+	DEBUG_LVL_29_TRACE2("ShbIpcAllocBuffer BufferId:%d MemSize:%d\n",
+			    iBufferId, ulShMemSize);
+	//---------------------------------------------------------------
+	// (1) open an existing or create a new shared memory
+	//---------------------------------------------------------------
+	//test if buffer already exists
+	if (ShbIpcFindListElement(iBufferId, &psMemTableElement) == 0) {
+		//Buffer already exists
+		fShMemNewCreated = FALSE;
+		pSharedMem = psMemTableElement->m_pBuffer;
+		DEBUG_LVL_29_TRACE1
+		    ("ShbIpcAllocBuffer attach Buffer at:%p Id:%d\n",
+		     pSharedMem);
+		uiFirstProcess = 1;
+	} else {
+		//create new Buffer
+		fShMemNewCreated = TRUE;
+		uiFirstProcess = 0;
+		pSharedMem = kmalloc(ulShMemSize, GFP_KERNEL);
+		DEBUG_LVL_29_TRACE2
+		    ("ShbIpcAllocBuffer Create New Buffer at:%p Id:%d\n",
+		     pSharedMem, iBufferId);
+		if (pSharedMem == NULL) {
+			//unable to create mem
+			ShbError = kShbOutOfMem;
+			goto Exit;
+		}
+		DEBUG_LVL_29_TRACE0("ShbIpcAllocBuffer create semas\n");
+		// append Element to Mem Table
+		psMemTableElement =
+		    kmalloc(sizeof(struct sShbMemTable), GFP_KERNEL);
+		psMemTableElement->m_iBufferId = iBufferId;
+		psMemTableElement->m_pBuffer = pSharedMem;
+		psMemTableElement->m_psNextMemTableElement = NULL;
+		ShbIpcAppendListElement(psMemTableElement);
+	}
+
+	DEBUG_LVL_29_TRACE0("ShbIpcAllocBuffer update header\n");
+	//update header
+	pShbMemHeader = (tShbMemHeader *) pSharedMem;
+	DEBUG_LVL_29_TRACE1
+	    ("ShbIpcAllocBuffer 0 pShbMemHeader->m_ulShMemSize: %d\n",
+	     pShbMemHeader->m_ulShMemSize);
+	// allocate a memory block from process specific mempool to save
+	// process local information to administrate/manage the shared buffer
+	DEBUG_LVL_29_TRACE0("ShbIpcAllocBuffer alloc private mem\n");
+	pShbMemInst =
+	    (tShbMemInst *) ShbIpcAllocPrivateMem(sizeof(tShbMemInst));
+	if (pShbMemInst == NULL) {
+		ShbError = kShbOutOfMem;
+		goto Exit;
+	}
+	// reset complete header to default values
+	//pShbMemInst->m_SbiMagicID                             = SBI_MAGIC_ID;
+//    pShbMemInst->m_pSharedMem                               = pSharedMem;
+	pShbMemInst->m_tThreadNewDataId = INVALID_ID;
+	pShbMemInst->m_tThreadJobReadyId = INVALID_ID;
+	pShbMemInst->m_pfnSigHndlrNewData = NULL;
+	pShbMemInst->m_ulTimeOutJobReady = 0;
+	pShbMemInst->m_pfnSigHndlrJobReady = NULL;
+	pShbMemInst->m_pShbMemHeader = pShbMemHeader;
+	pShbMemInst->m_iThreadTermFlag = 0;
+
+	// initialize completion etc.
+	init_completion(&pShbMemInst->m_CompletionNewData);
+
+	ShbError = kShbOk;
+	if (fShMemNewCreated) {
+		// this process was the first who wanted to use the shared memory,
+		// so a new shared memory was created
+		// -> setup new header information inside the shared memory region
+		//    itself
+		pShbMemHeader->m_ulShMemSize = ulShMemSize;
+		pShbMemHeader->m_ulRefCount = 1;
+		pShbMemHeader->m_iBufferId = iBufferId;
+		// initialize spinlock
+		spin_lock_init(&pShbMemHeader->m_SpinlockBuffAccess);
+		// initialize wait queues
+		init_waitqueue_head(&pShbMemHeader->m_WaitQueueNewData);
+		init_waitqueue_head(&pShbMemHeader->m_WaitQueueJobReady);
+	} else {
+		// any other process has created the shared memory and this
+		// process only has to attach to it
+		// -> check and update existing header information inside the
+		//    shared memory region itself
+		if (pShbMemHeader->m_ulShMemSize != ulShMemSize) {
+			ShbError = kShbOpenMismatch;
+			goto Exit;
+		}
+		pShbMemHeader->m_ulRefCount++;
+	}
+
+      Exit:
+	pShbInstance = (tShbInstance *) pShbMemInst;
+	*pfShbNewCreated_p = fShMemNewCreated;
+	*ppShbInstance_p = pShbInstance;
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Release Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcReleaseBuffer(tShbInstance pShbInstance_p)
+{
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	tShbError ShbError;
+	tShbError ShbError2;
+
+	DEBUG_LVL_26_TRACE1("ShbIpcReleaseBuffer(%p)\n", pShbInstance_p);
+	if (pShbInstance_p == NULL) {
+		return (kShbOk);
+	}
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+
+	// stop threads in any case, because they are bound to that specific instance
+	ShbError2 = ShbIpcStopSignalingNewData(pShbInstance_p);
+	// d.k.: Whats up with JobReady thread?
+	//       Just wake it up, but without setting the semaphore variable
+	wake_up_interruptible(&pShbMemHeader->m_WaitQueueJobReady);
+
+	if (!--pShbMemHeader->m_ulRefCount) {
+		ShbError = kShbOk;
+		// delete mem table element
+		ShbIpcDeleteListElement(pShbMemHeader->m_iBufferId);
+		// delete shared mem
+		kfree(pShbMemInst->m_pShbMemHeader);
+	} else {
+		ShbError = kShbMemUsedByOtherProcs;
+	}
+	//delete privat mem
+	kfree(pShbMemInst);
+	return (ShbError);
+}
+
+#endif // !defined(SHBIPC_INLINE_ENABLED)
+
+#if (!defined(SHBIPC_INLINED)) || defined(SHBIPC_INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Enter atomic section for Shared Buffer access
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcEnterAtomicSection(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	tShbError ShbError = kShbOk;
+
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+	DEBUG_LVL_29_TRACE0("enter atomic\n");
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+
+	// lock interrupts
+	spin_lock_irqsave(&pShbMemHeader->m_SpinlockBuffAccess,
+			  pShbMemInst->m_ulFlagsBuffAccess);
+
+      Exit:
+	return ShbError;
+
+}
+
+//---------------------------------------------------------------------------
+//  Leave atomic section for Shared Buffer access
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcLeaveAtomicSection(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	tShbError ShbError = kShbOk;
+
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+	// unlock interrupts
+	spin_unlock_irqrestore(&pShbMemHeader->m_SpinlockBuffAccess,
+			       pShbMemInst->m_ulFlagsBuffAccess);
+
+      Exit:
+	DEBUG_LVL_29_TRACE0("Leave Atomic \n");
+	return ShbError;
+
+}
+
+//---------------------------------------------------------------------------
+//  Start signaling of new data (called from reading process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcStartSignalingNewData(tShbInstance
+						      pShbInstance_p,
+						      tSigHndlrNewData
+						      pfnSignalHandlerNewData_p,
+						      tShbPriority
+						      ShbPriority_p)
+{
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	tShbError ShbError;
+
+	DEBUG_LVL_29_TRACE0("------->ShbIpcStartSignalingNewData\n");
+	if ((pShbInstance_p == NULL) || (pfnSignalHandlerNewData_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+	ShbError = kShbOk;
+
+	if ((pShbMemInst->m_tThreadNewDataId != INVALID_ID)
+	    || (pShbMemInst->m_pfnSigHndlrNewData != NULL)) {
+		ShbError = kShbAlreadySignaling;
+		goto Exit;
+	}
+	DEBUG_LVL_26_TRACE2
+	    ("ShbIpcStartSignalingNewData(%p) m_pfnSigHndlrNewData = %p\n",
+	     pShbInstance_p, pfnSignalHandlerNewData_p);
+	pShbMemInst->m_pfnSigHndlrNewData = pfnSignalHandlerNewData_p;
+	pShbMemHeader->m_fNewData = FALSE;
+	pShbMemInst->m_iThreadTermFlag = 0;
+
+	switch (ShbPriority_p) {
+	case kShbPriorityLow:
+		pShbMemInst->m_lThreadNewDataNice = -2;
+		break;
+
+	case kShbPriorityNormal:
+		pShbMemInst->m_lThreadNewDataNice = -9;
+		break;
+
+	case kshbPriorityHigh:
+		pShbMemInst->m_lThreadNewDataNice = -20;
+		break;
+
+	}
+
+	//create thread for signalling new data
+	pShbMemInst->m_tThreadNewDataId =
+	    kernel_thread(ShbIpcThreadSignalNewData, pShbInstance_p,
+			  CLONE_KERNEL);
+
+      Exit:
+	return ShbError;
+
+}
+
+//---------------------------------------------------------------------------
+//  Stop signaling of new data (called from reading process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcStopSignalingNewData(tShbInstance
+						     pShbInstance_p)
+{
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	tShbError ShbError;
+
+	DEBUG_LVL_29_TRACE0("------->ShbIpcStopSignalingNewData\n");
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+	ShbError = kShbOk;
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+
+	DEBUG_LVL_26_TRACE2
+	    ("ShbIpcStopSignalingNewData(%p) pfnSignHndlrNewData=%p\n",
+	     pShbInstance_p, pShbMemInst->m_pfnSigHndlrNewData);
+	if (pShbMemInst->m_pfnSigHndlrNewData != NULL) {	// signal handler was set before
+		int iErr;
+		//set termination flag in mem header
+		pShbMemInst->m_iThreadTermFlag = 1;
+
+		// check if thread is still running at all by sending the null-signal to this thread
+		/* iErr = kill_proc(pShbMemInst->m_tThreadNewDataId, 0, 1); */
+		iErr = send_sig(0, pShbMemInst->m_tThreadNewDataId, 1);
+		if (iErr == 0) {
+			// wake up thread, because it is still running
+			wake_up_interruptible(&pShbMemHeader->
+					      m_WaitQueueNewData);
+
+			//wait for termination of thread
+			wait_for_completion(&pShbMemInst->m_CompletionNewData);
+		}
+
+		pShbMemInst->m_pfnSigHndlrNewData = NULL;
+		pShbMemInst->m_tThreadNewDataId = INVALID_ID;
+	}
+
+	return ShbError;
+
+}
+
+//---------------------------------------------------------------------------
+//  Signal new data (called from writing process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcSignalNewData(tShbInstance pShbInstance_p)
+{
+	tShbMemHeader *pShbMemHeader;
+
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+	pShbMemHeader =
+	    ShbIpcGetShbMemHeader(ShbIpcGetShbMemInst(pShbInstance_p));
+	//set semaphore
+	pShbMemHeader->m_fNewData = TRUE;
+	DEBUG_LVL_29_TRACE0("ShbIpcSignalNewData set Sem -> New Data\n");
+
+	wake_up_interruptible(&pShbMemHeader->m_WaitQueueNewData);
+	return (kShbOk);
+}
+
+//---------------------------------------------------------------------------
+//  Start signaling for job ready (called from waiting process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcStartSignalingJobReady(tShbInstance
+						       pShbInstance_p,
+						       unsigned long
+						       ulTimeOut_p,
+						       tSigHndlrJobReady
+						       pfnSignalHandlerJobReady_p)
+{
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	tShbError ShbError;
+
+	if ((pShbInstance_p == NULL) || (pfnSignalHandlerJobReady_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+
+	ShbError = kShbOk;
+	if ((pShbMemInst->m_tThreadJobReadyId != INVALID_ID)
+	    || (pShbMemInst->m_pfnSigHndlrJobReady != NULL)) {
+		ShbError = kShbAlreadySignaling;
+		goto Exit;
+	}
+	pShbMemInst->m_ulTimeOutJobReady = ulTimeOut_p;
+	pShbMemInst->m_pfnSigHndlrJobReady = pfnSignalHandlerJobReady_p;
+	pShbMemHeader->m_fJobReady = FALSE;
+	//create thread for signalling new data
+	pShbMemInst->m_tThreadJobReadyId =
+	    kernel_thread(ShbIpcThreadSignalJobReady, pShbInstance_p,
+			  CLONE_KERNEL);
+      Exit:
+	return ShbError;
+}
+
+//---------------------------------------------------------------------------
+//  Signal job ready (called from executing process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcSignalJobReady(tShbInstance pShbInstance_p)
+{
+	tShbMemHeader *pShbMemHeader;
+
+	DEBUG_LVL_29_TRACE0("ShbIpcSignalJobReady\n");
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+	pShbMemHeader =
+	    ShbIpcGetShbMemHeader(ShbIpcGetShbMemInst(pShbInstance_p));
+	//set semaphore
+	pShbMemHeader->m_fJobReady = TRUE;
+	DEBUG_LVL_29_TRACE0("ShbIpcSignalJobReady set Sem -> Job Ready \n");
+
+	wake_up_interruptible(&pShbMemHeader->m_WaitQueueJobReady);
+	return (kShbOk);
+}
+
+//---------------------------------------------------------------------------
+//  Get pointer to common used share memory area
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void *ShbIpcGetShMemPtr(tShbInstance pShbInstance_p)
+{
+
+	tShbMemHeader *pShbMemHeader;
+	void *pShbShMemPtr;
+
+	pShbMemHeader =
+	    ShbIpcGetShbMemHeader(ShbIpcGetShbMemInst(pShbInstance_p));
+	if (pShbMemHeader != NULL) {
+		pShbShMemPtr = (BYTE *) pShbMemHeader + sizeof(tShbMemHeader);
+	} else {
+		pShbShMemPtr = NULL;
+	}
+
+	return (pShbShMemPtr);
+
+}
+
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+#if !defined(SHBIPC_INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Get pointer to process local information structure
+//---------------------------------------------------------------------------
+
+/*tShbMemInst*  ShbIpcGetShbMemInst (
+    tShbInstance pShbInstance_p)
+{
+
+tShbMemInst*  pShbMemInst;
+
+    pShbMemInst = (tShbMemInst*)pShbInstance_p;
+
+    return (pShbMemInst);
+
+}
+*/
+
+//---------------------------------------------------------------------------
+//  Get pointer to shared memory header
+//---------------------------------------------------------------------------
+
+/*tShbMemHeader*  ShbIpcGetShbMemHeader (
+    tShbMemInst* pShbMemInst_p)
+{
+
+tShbMemHeader*  pShbMemHeader;
+
+    pShbMemHeader = pShbMemInst_p->m_pShbMemHeader;
+
+    return (pShbMemHeader);
+
+}
+*/
+
+//---------------------------------------------------------------------------
+//  Allocate a memory block from process specific mempool
+//---------------------------------------------------------------------------
+
+static void *ShbIpcAllocPrivateMem(unsigned long ulMemSize_p)
+{
+	tShbError ShbError;
+	void *pMem;
+
+	DEBUG_LVL_29_TRACE0("ShbIpcAllocPrivateMem \n");
+	//get private mem
+	pMem = kmalloc(ulMemSize_p, GFP_KERNEL);
+	if (pMem == NULL) {
+		//unable to create mem
+		ShbError = kShbOutOfMem;
+		goto Exit;
+	}
+      Exit:
+	return (pMem);
+
+}
+
+//---------------------------------------------------------------------------
+//  Thread for new data signaling
+//---------------------------------------------------------------------------
+
+int ShbIpcThreadSignalNewData(void *pvThreadParam_p)
+{
+	tShbInstance pShbInstance;
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	int iRetVal = -1;
+	int fCallAgain;
+
+	daemonize("ShbND%p", pvThreadParam_p);
+	allow_signal(SIGTERM);
+	pShbInstance = (tShbMemInst *) pvThreadParam_p;
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+
+	DEBUG_LVL_26_TRACE1("ShbIpcThreadSignalNewData(%p)\n", pvThreadParam_p);
+
+	set_user_nice(current, pShbMemInst->m_lThreadNewDataNice);
+
+//            DEBUG_LVL_29_TRACE1("ShbIpcThreadSignalNewData wait for New Data Sem %p\n",pShbMemInst->m_pSemNewData);
+	do {
+		iRetVal =
+		    wait_event_interruptible(pShbMemHeader->m_WaitQueueNewData,
+					     (pShbMemInst->m_iThreadTermFlag !=
+					      0)
+					     || (pShbMemHeader->m_fNewData !=
+						 FALSE));
+
+		if (iRetVal != 0) {	// signal pending
+			break;
+		}
+
+		if (pShbMemHeader->m_fNewData != FALSE) {
+			pShbMemHeader->m_fNewData = FALSE;
+			do {
+				fCallAgain =
+				    pShbMemInst->
+				    m_pfnSigHndlrNewData(pShbInstance);
+				// call scheduler, which will execute any task with higher priority
+				schedule();
+			} while (fCallAgain != FALSE);
+		}
+	} while (pShbMemInst->m_iThreadTermFlag == 0);
+	DEBUG_LVL_29_TRACE0("ShbIpcThreadSignalNewData terminated \n");
+	//set thread completed
+	complete_and_exit(&pShbMemInst->m_CompletionNewData, 0);
+	return 0;
+}
+
+//---------------------------------------------------------------------------
+//  Thread for new data Job Ready signaling
+//---------------------------------------------------------------------------
+
+int ShbIpcThreadSignalJobReady(void *pvThreadParam_p)
+{
+	tShbInstance pShbInstance;
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	long lTimeOut;
+	int iRetVal = -1;
+
+	daemonize("ShbJR%p", pvThreadParam_p);
+	allow_signal(SIGTERM);
+	pShbInstance = (tShbMemInst *) pvThreadParam_p;
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+
+	DEBUG_LVL_29_TRACE0
+	    ("ShbIpcThreadSignalJobReady wait for job ready Sem\n");
+	if (pShbMemInst->m_ulTimeOutJobReady != 0) {
+		lTimeOut = (long)pShbMemInst->m_ulTimeOutJobReady;
+		//wait for job ready semaphore
+		iRetVal =
+		    wait_event_interruptible_timeout(pShbMemHeader->
+						     m_WaitQueueJobReady,
+						     (pShbMemHeader->
+						      m_fJobReady != FALSE),
+						     lTimeOut);
+	} else {
+		//wait for job ready semaphore
+		iRetVal =
+		    wait_event_interruptible(pShbMemHeader->m_WaitQueueJobReady,
+					     (pShbMemHeader->m_fJobReady !=
+					      FALSE));
+	}
+
+	if (pShbMemInst->m_pfnSigHndlrJobReady != NULL) {
+		//call Handler
+		pShbMemInst->m_pfnSigHndlrJobReady(pShbInstance,
+						   !pShbMemHeader->m_fJobReady);
+	}
+
+	pShbMemInst->m_pfnSigHndlrJobReady = NULL;
+	return 0;
+}
+
+//Build the crc table
+static void ShbIpcCrc32GenTable(unsigned long aulCrcTable[256])
+{
+	unsigned long ulCrc, ulPoly;
+	int iIndexI, iIndexJ;
+
+	ulPoly = 0xEDB88320L;
+	for (iIndexI = 0; iIndexI < 256; iIndexI++) {
+		ulCrc = iIndexI;
+		for (iIndexJ = 8; iIndexJ > 0; iIndexJ--) {
+			if (ulCrc & 1) {
+				ulCrc = (ulCrc >> 1) ^ ulPoly;
+			} else {
+				ulCrc >>= 1;
+			}
+		}
+		aulCrcTable[iIndexI] = ulCrc;
+	}
+}
+
+//Calculate the crc value
+static unsigned long ShbIpcCrc32GetCrc(const char *pcString,
+				       unsigned long aulCrcTable[256])
+{
+	unsigned long ulCrc;
+	int iIndex;
+
+	ulCrc = 0xFFFFFFFF;
+	for (iIndex = 0; iIndex < strlen(pcString); iIndex++) {
+		ulCrc =
+		    ((ulCrc >> 8) & 0x00FFFFFF) ^
+		    aulCrcTable[(ulCrc ^ pcString[iIndex]) & 0xFF];
+	}
+	return (ulCrc ^ 0xFFFFFFFF);
+
+}
+
+static void ShbIpcAppendListElement(struct sShbMemTable *psNewMemTableElement)
+{
+	struct sShbMemTable *psMemTableElement = psMemTableElementFirst_g;
+	psNewMemTableElement->m_psNextMemTableElement = NULL;
+
+	if (psMemTableElementFirst_g != NULL) {	/* sind Elemente vorhanden */
+		while (psMemTableElement->m_psNextMemTableElement != NULL) {	/* suche das letzte Element */
+			psMemTableElement =
+			    psMemTableElement->m_psNextMemTableElement;
+		}
+		psMemTableElement->m_psNextMemTableElement = psNewMemTableElement;	/*  Haenge das Element hinten an */
+	} else {		/* wenn die liste leer ist, bin ich das erste Element */
+		psMemTableElementFirst_g = psNewMemTableElement;
+	}
+}
+
+static int ShbIpcFindListElement(int iBufferId,
+				 struct sShbMemTable **ppsReturnMemTableElement)
+{
+	struct sShbMemTable *psMemTableElement = psMemTableElementFirst_g;
+	while (psMemTableElement != NULL) {
+		if (psMemTableElement->m_iBufferId == iBufferId) {
+//printk("ShbIpcFindListElement Buffer at:%p Id:%d\n",psMemTableElement->m_pBuffer,psMemTableElement->m_iBufferId);
+			*ppsReturnMemTableElement = psMemTableElement;
+//printk("ShbIpcFindListElement Buffer at:%p Id:%d\n",(*ppsReturnMemTableElement)->m_pBuffer,(*ppsReturnMemTableElement)->m_iBufferId);
+			return 0;
+		}
+		psMemTableElement = psMemTableElement->m_psNextMemTableElement;
+	}
+	return -1;
+}
+
+static void ShbIpcDeleteListElement(int iBufferId)
+{
+	struct sShbMemTable *psMemTableElement = psMemTableElementFirst_g;
+	struct sShbMemTable *psMemTableElementOld = psMemTableElementFirst_g;
+	if (psMemTableElement != NULL) {
+		while ((psMemTableElement != NULL)
+		       && (psMemTableElement->m_iBufferId != iBufferId)) {
+			psMemTableElementOld = psMemTableElement;
+			psMemTableElement =
+			    psMemTableElement->m_psNextMemTableElement;
+		}
+		if (psMemTableElement != NULL) {
+			if (psMemTableElement != psMemTableElementFirst_g) {
+				psMemTableElementOld->m_psNextMemTableElement =
+				    psMemTableElement->m_psNextMemTableElement;
+				kfree(psMemTableElement);
+			} else {
+				kfree(psMemTableElement);
+				psMemTableElementFirst_g = NULL;
+			}
+
+		}
+	}
+
+}
+
+#endif
diff --git a/drivers/staging/epl/ShbIpc-Win32.c b/drivers/staging/epl/ShbIpc-Win32.c
new file mode 100644
index 000000000000..b9181471ae0b
--- /dev/null
+++ b/drivers/staging/epl/ShbIpc-Win32.c
@@ -0,0 +1,1202 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      Project independend shared buffer (linear + circular)
+
+  Description:  Implementation of platform specific part for the
+                shared buffer
+                (Implementation for Win32)
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+  2006/06/27 -rs:   V 1.00 (initial version)
+
+****************************************************************************/
+
+#define WINVER       0x0400	// #defines necessary for usage of
+#define _WIN32_WINNT 0x0400	// function <SignalObjectAndWait>
+
+#include <windows.h>
+#include <stdio.h>
+#include "global.h"
+#include "sharedbuff.h"
+#include "shbipc.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+#if (!defined(SHBIPC_INLINED)) || defined(SHBIPC_INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Configuration
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Constant definitions
+//---------------------------------------------------------------------------
+
+#define MAX_LEN_BUFFER_ID       MAX_PATH
+
+#define IDX_EVENT_NEW_DATA      0
+#define IDX_EVENT_TERM_REQU     1
+#define IDX_EVENT_TERM_RESP     2
+
+#define NAME_MUTEX_BUFF_ACCESS  "BuffAccess"
+#define NAME_EVENT_NEW_DATA     "NewData"
+#define NAME_EVENT_TERM_REQU    "TermRequ"
+#define NAME_EVENT_TERM_RESP    "TermResp"
+#define NAME_EVENT_JOB_READY    "JobReady"
+
+#define TIMEOUT_ENTER_ATOMIC    1000	// for debgging: INFINITE
+#define TIMEOUT_TERM_THREAD     2000
+
+#define SBI_MAGIC_ID            0x5342492B	// magic ID ("SBI+")
+#define SBH_MAGIC_ID            0x5342482A	// magic ID ("SBH*")
+
+//---------------------------------------------------------------------------
+//  Local types
+//---------------------------------------------------------------------------
+
+// This structure is the common header for the shared memory region used
+// by all processes attached this shared memory. It includes common
+// information to administrate/manage the shared buffer from a couple of
+// separated processes (e.g. the refernce counter). This structure is
+// located at the start of the shared memory region itself and exists
+// consequently only one times per shared memory instance.
+typedef struct {
+	unsigned long m_SbhMagicID;	// magic ID ("SBH*")
+	unsigned long m_ulShMemSize;
+	unsigned long m_ulRefCount;
+	char m_szBufferID[MAX_LEN_BUFFER_ID];
+
+#ifndef NDEBUG
+	unsigned long m_ulOwnerProcID;
+#endif
+
+} tShbMemHeader;
+
+// This structure is the "external entry point" from a separate process
+// to get access to a shared buffer. This structure includes all platform
+// resp. target specific information to administrate/manage the shared
+// buffer from a separate process. Every process attached to the shared
+// buffer has its own runtime instance of this structure with its individual
+// runtime data (e.g. the scope of an event handle is limitted to the
+// owner process only). The structure member <m_pShbMemHeader> points
+// to the (process specific) start address of the shared memory region
+// itself.
+typedef struct {
+	unsigned long m_SbiMagicID;	// magic ID ("SBI+")
+	HANDLE m_hSharedMem;
+	HANDLE m_hMutexBuffAccess;
+	HANDLE m_hThreadNewData;	// thraed to signal that new data are available
+	HANDLE m_ahEventNewData[3];	// IDX_EVENT_NEW_DATA + IDX_EVENT_TERM_REQU + ID_EVENT_TERM_RESP
+	tSigHndlrNewData m_pfnSigHndlrNewData;
+	HANDLE m_hThreadJobReady;	// thread to signal that a job/operation is ready now (e.g. reset buffer)
+	HANDLE m_hEventJobReady;
+	unsigned long m_ulTimeOutJobReady;
+	tSigHndlrJobReady m_pfnSigHndlrJobReady;
+	tShbMemHeader *m_pShbMemHeader;
+
+#ifndef NDEBUG
+	unsigned long m_ulThreadIDNewData;
+	unsigned long m_ulThreadIDJobReady;
+#endif
+
+} tShbMemInst;
+
+//---------------------------------------------------------------------------
+//  Global variables
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Local variables
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Prototypes of internal functions
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Get pointer to process local information structure
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbMemInst *ShbIpcGetShbMemInst(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+
+	pShbMemInst = (tShbMemInst *) pShbInstance_p;
+	ASSERT(pShbMemInst->m_SbiMagicID == SBI_MAGIC_ID);
+
+	return (pShbMemInst);
+
+}
+
+//---------------------------------------------------------------------------
+//  Get pointer to shared memory header
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbMemHeader *ShbIpcGetShbMemHeader(tShbInstance
+						     pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = pShbMemInst->m_pShbMemHeader;
+	ASSERT(pShbMemHeader->m_SbhMagicID == SBH_MAGIC_ID);
+
+	return (pShbMemHeader);
+
+}
+
+// not inlined internal functions
+DWORD WINAPI ShbIpcThreadSignalNewData(LPVOID pvThreadParam_p);
+DWORD WINAPI ShbIpcThreadSignalJobReady(LPVOID pvThreadParam_p);
+const char *ShbIpcGetUniformObjectName(const char *pszEventJobName_p,
+				       const char *pszBufferID_p,
+				       BOOL fGlobalObject_p);
+
+#endif
+
+#if !defined(SHBIPC_INLINE_ENABLED)
+// true internal functions (not inlined)
+static void *ShbIpcAllocPrivateMem(unsigned long ulMemSize_p);
+static void ShbIpcReleasePrivateMem(void *pMem_p);
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+#if !defined(SHBIPC_INLINE_ENABLED)
+// not inlined external functions
+
+//---------------------------------------------------------------------------
+//  Initialize IPC for Shared Buffer Module
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcInit(void)
+{
+
+	return (kShbOk);
+
+}
+
+//---------------------------------------------------------------------------
+//  Deinitialize IPC for Shared Buffer Module
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcExit(void)
+{
+
+	return (kShbOk);
+
+}
+
+//---------------------------------------------------------------------------
+//  Allocate Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcAllocBuffer(unsigned long ulBufferSize_p,
+			    const char *pszBufferID_p,
+			    tShbInstance * ppShbInstance_p,
+			    unsigned int *pfShbNewCreated_p)
+{
+
+	HANDLE hSharedMem;
+	LPVOID pSharedMem;
+	unsigned long ulShMemSize;
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	tShbInstance pShbInstance;
+	unsigned int fShMemNewCreated;
+	const char *pszObjectName;
+	HANDLE hMutexBuffAccess;
+	HANDLE hEventNewData;
+	HANDLE hEventJobReady;
+	tShbError ShbError;
+
+	ulShMemSize = ulBufferSize_p + sizeof(tShbMemHeader);
+	pSharedMem = NULL;
+	pShbInstance = NULL;
+	fShMemNewCreated = FALSE;
+	ShbError = kShbOk;
+
+	//---------------------------------------------------------------
+	// (1) open an existing or create a new shared memory
+	//---------------------------------------------------------------
+	// try to open an already existing shared memory
+	// (created by an another process)
+	hSharedMem = OpenFileMapping(FILE_MAP_ALL_ACCESS,	// DWORD dwDesiredAccess
+				     FALSE,	// BOOL bInheritHandle
+				     pszBufferID_p);	// LPCTSTR lpName
+	if (hSharedMem != NULL) {
+		// a shared memory already exists
+		fShMemNewCreated = FALSE;
+	} else {
+		// it seams that this process is the first who wants to use the
+		// shared memory, so it has to create a new shared memory
+		hSharedMem = CreateFileMapping(INVALID_HANDLE_VALUE,	// HANDLE hFile
+					       NULL,	// LPSECURITY_ATTRIBUTES lpAttributes
+					       PAGE_READWRITE,	// DWORD flProtect
+					       0,	// DWORD dwMaximumSizeHigh
+					       ulShMemSize,	// DWORD dwMaximumSizeLow
+					       pszBufferID_p);	// LPCTSTR lpName
+
+		fShMemNewCreated = TRUE;
+	}
+
+	if (hSharedMem == NULL) {
+		ShbError = kShbOutOfMem;
+		goto Exit;
+	}
+
+	//---------------------------------------------------------------
+	// (2) get the pointer to the shared memory
+	//---------------------------------------------------------------
+	pSharedMem = MapViewOfFile(hSharedMem,	// HANDLE hFileMappingObject
+				   FILE_MAP_ALL_ACCESS,	// DWORD dwDesiredAccess,
+				   0,	// DWORD dwFileOffsetHigh,
+				   0,	// DWORD dwFileOffsetLow,
+				   ulShMemSize);	// SIZE_T dwNumberOfBytesToMap
+
+	if (pSharedMem == NULL) {
+		ShbError = kShbOutOfMem;
+		goto Exit;
+	}
+
+	//---------------------------------------------------------------
+	// (3) setup or update header and management information
+	//---------------------------------------------------------------
+	pShbMemHeader = (tShbMemHeader *) pSharedMem;
+
+	// allocate a memory block from process specific mempool to save
+	// process local information to administrate/manage the shared buffer
+	pShbMemInst =
+	    (tShbMemInst *) ShbIpcAllocPrivateMem(sizeof(tShbMemInst));
+	if (pShbMemInst == NULL) {
+		ShbError = kShbOutOfMem;
+		goto Exit;
+	}
+	// reset complete header to default values
+	pShbMemInst->m_SbiMagicID = SBI_MAGIC_ID;
+	pShbMemInst->m_hSharedMem = hSharedMem;
+	pShbMemInst->m_hMutexBuffAccess = INVALID_HANDLE_VALUE;
+	pShbMemInst->m_hThreadNewData = INVALID_HANDLE_VALUE;
+	pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA] =
+	    INVALID_HANDLE_VALUE;
+	pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU] =
+	    INVALID_HANDLE_VALUE;
+	pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP] =
+	    INVALID_HANDLE_VALUE;
+	pShbMemInst->m_pfnSigHndlrNewData = NULL;
+	pShbMemInst->m_hThreadJobReady = INVALID_HANDLE_VALUE;
+	pShbMemInst->m_hEventJobReady = INVALID_HANDLE_VALUE;
+	pShbMemInst->m_ulTimeOutJobReady = 0;
+	pShbMemInst->m_pfnSigHndlrJobReady = NULL;
+	pShbMemInst->m_pShbMemHeader = pShbMemHeader;
+
+#ifndef NDEBUG
+	{
+		pShbMemInst->m_ulThreadIDNewData = 0;
+		pShbMemInst->m_ulThreadIDJobReady = 0;
+	}
+#endif
+
+	// create mutex for buffer access
+	pszObjectName =
+	    ShbIpcGetUniformObjectName(NAME_MUTEX_BUFF_ACCESS, pszBufferID_p,
+				       TRUE);
+	hMutexBuffAccess = CreateMutex(NULL,	// LPSECURITY_ATTRIBUTES lpMutexAttributes
+				       FALSE,	// BOOL bInitialOwner
+				       pszObjectName);	// LPCTSTR lpName
+	pShbMemInst->m_hMutexBuffAccess = hMutexBuffAccess;
+	ASSERT(pShbMemInst->m_hMutexBuffAccess != NULL);
+
+	// The EventNewData is used for signaling of new data after a write
+	// operation (SetEvent) as well as for waiting for new data on the
+	// reader side (WaitForMultipleObjects). Because it's not known if
+	// this process will be read or write data, the event will be
+	// always created here.
+	pszObjectName =
+	    ShbIpcGetUniformObjectName(NAME_EVENT_NEW_DATA, pszBufferID_p,
+				       TRUE);
+	hEventNewData = CreateEvent(NULL,	// LPSECURITY_ATTRIBUTES lpEventAttributes
+				    FALSE,	// BOOL bManualReset
+				    FALSE,	// BOOL bInitialState
+				    pszObjectName);	// LPCTSTR lpName
+	pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA] = hEventNewData;
+	ASSERT(pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA] != NULL);
+
+	// The EventJobReady is used for signaling that a job is done (SetEvent)
+	// as well as for waiting for finishing of a job (WaitForMultipleObjects).
+	// Because it's not known if this process will signal or wait, the event
+	// will be always created here.
+	pszObjectName =
+	    ShbIpcGetUniformObjectName(NAME_EVENT_JOB_READY, pszBufferID_p,
+				       TRUE);
+	hEventJobReady = CreateEvent(NULL,	// LPSECURITY_ATTRIBUTES lpEventAttributes
+				     FALSE,	// BOOL bManualReset
+				     FALSE,	// BOOL bInitialState
+				     pszObjectName);	// LPCTSTR lpName
+	pShbMemInst->m_hEventJobReady = hEventJobReady;
+	ASSERT(pShbMemInst->m_hEventJobReady != NULL);
+
+	if (fShMemNewCreated) {
+		// this process was the first who wanted to use the shared memory,
+		// so a new shared memory was created
+		// -> setup new header information inside the shared memory region
+		//    itself
+		pShbMemHeader->m_SbhMagicID = SBH_MAGIC_ID;
+		pShbMemHeader->m_ulShMemSize = ulShMemSize;
+		pShbMemHeader->m_ulRefCount = 1;
+		strncpy(pShbMemHeader->m_szBufferID, pszBufferID_p,
+			sizeof(pShbMemHeader->m_szBufferID) - 1);
+
+#ifndef NDEBUG
+		{
+			pShbMemHeader->m_ulOwnerProcID = GetCurrentProcessId();
+		}
+#endif
+	} else {
+		// any other process has created the shared memory and this
+		// process has only attached to it
+		// -> check and update existing header information inside the
+		//    shared memory region itself
+		if (pShbMemHeader->m_ulShMemSize != ulShMemSize) {
+			ShbError = kShbOpenMismatch;
+			goto Exit;
+		}
+#ifndef NDEBUG
+		{
+			if (strncmp
+			    (pShbMemHeader->m_szBufferID, pszBufferID_p,
+			     sizeof(pShbMemHeader->m_szBufferID) - 1)) {
+				ShbError = kShbOpenMismatch;
+				goto Exit;
+			}
+		}
+#endif
+
+		pShbMemHeader->m_ulRefCount++;
+	}
+
+	// set abstarct "handle" for returning to application
+	pShbInstance = (tShbInstance *) pShbMemInst;
+
+      Exit:
+
+	if (ShbError != kShbOk) {
+		if (pShbMemInst != NULL) {
+			ShbIpcReleasePrivateMem(pShbMemInst);
+		}
+		if (pSharedMem != NULL) {
+			UnmapViewOfFile(pSharedMem);
+		}
+		if (hSharedMem != NULL) {
+			CloseHandle(hSharedMem);
+		}
+	}
+
+	*pfShbNewCreated_p = fShMemNewCreated;
+	*ppShbInstance_p = pShbInstance;
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Release Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcReleaseBuffer(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	HANDLE hEventNewData;
+	HANDLE hMutexBuffAccess;
+	tShbError ShbError;
+	tShbError ShbError2;
+
+	if (pShbInstance_p == NULL) {
+		return (kShbOk);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbInstance_p);
+
+	if (!--pShbMemHeader->m_ulRefCount) {
+		ShbError = kShbOk;
+	} else {
+		ShbError = kShbMemUsedByOtherProcs;
+	}
+
+	ShbError2 = ShbIpcStopSignalingNewData(pShbInstance_p);
+	hEventNewData = pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA];
+	if (hEventNewData != INVALID_HANDLE_VALUE) {
+		CloseHandle(hEventNewData);
+		pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA] =
+		    INVALID_HANDLE_VALUE;
+	}
+
+	hMutexBuffAccess = pShbMemInst->m_hMutexBuffAccess;
+	if (hMutexBuffAccess != INVALID_HANDLE_VALUE) {
+		CloseHandle(hMutexBuffAccess);
+		pShbMemInst->m_hMutexBuffAccess = INVALID_HANDLE_VALUE;
+	}
+
+	UnmapViewOfFile(pShbMemHeader);
+	if (pShbMemInst->m_hSharedMem != INVALID_HANDLE_VALUE) {
+		CloseHandle(pShbMemInst->m_hSharedMem);
+		pShbMemInst->m_hSharedMem = INVALID_HANDLE_VALUE;
+	}
+
+	ShbIpcReleasePrivateMem(pShbMemInst);
+
+	if (ShbError == kShbOk) {
+		ShbError = ShbError2;
+	}
+
+	return (ShbError);
+
+}
+
+#endif // !defined(SHBIPC_INLINE_ENABLED)
+
+#if (!defined(SHBIPC_INLINED)) || defined(SHBIPC_INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Enter atomic section for Shared Buffer access
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcEnterAtomicSection(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	HANDLE hMutexBuffAccess;
+	DWORD dwWaitResult;
+	tShbError ShbError;
+
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	ShbError = kShbOk;
+
+	hMutexBuffAccess = pShbMemInst->m_hMutexBuffAccess;
+	if (hMutexBuffAccess != INVALID_HANDLE_VALUE) {
+		dwWaitResult =
+		    WaitForSingleObject(hMutexBuffAccess, TIMEOUT_ENTER_ATOMIC);
+		switch (dwWaitResult) {
+		case WAIT_OBJECT_0 + 0:
+			{
+				break;
+			}
+
+		case WAIT_TIMEOUT:
+			{
+				TRACE0
+				    ("\nShbIpcEnterAtomicSection(): WAIT_TIMEOUT");
+				ASSERT(0);
+				ShbError = kShbBufferInvalid;
+				break;
+			}
+
+		case WAIT_ABANDONED:
+			{
+				TRACE0
+				    ("\nShbIpcEnterAtomicSection(): WAIT_ABANDONED");
+				ASSERT(0);
+				ShbError = kShbBufferInvalid;
+				break;
+			}
+
+		case WAIT_FAILED:
+			{
+				TRACE1
+				    ("\nShbIpcEnterAtomicSection(): WAIT_FAILED -> LastError=%ld",
+				     GetLastError());
+				ASSERT(0);
+				ShbError = kShbBufferInvalid;
+				break;
+			}
+
+		default:
+			{
+				TRACE1
+				    ("\nShbIpcEnterAtomicSection(): unknown error -> LastError=%ld",
+				     GetLastError());
+				ASSERT(0);
+				ShbError = kShbBufferInvalid;
+				break;
+			}
+		}
+	} else {
+		ShbError = kShbBufferInvalid;
+	}
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Leave atomic section for Shared Buffer access
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcLeaveAtomicSection(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	HANDLE hMutexBuffAccess;
+	BOOL fRes;
+	tShbError ShbError;
+
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	ShbError = kShbOk;
+
+	hMutexBuffAccess = pShbMemInst->m_hMutexBuffAccess;
+	if (hMutexBuffAccess != INVALID_HANDLE_VALUE) {
+		fRes = ReleaseMutex(hMutexBuffAccess);
+		ASSERT(fRes);
+	} else {
+		ShbError = kShbBufferInvalid;
+	}
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Start signaling of new data (called from reading process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcStartSignalingNewData(tShbInstance
+						      pShbInstance_p,
+						      tSigHndlrNewData
+						      pfnSignalHandlerNewData_p,
+						      tShbPriority
+						      ShbPriority_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	const char *pszObjectName;
+	HANDLE hEventTermRequ;
+	HANDLE hEventTermResp;
+	HANDLE hThreadNewData;
+	unsigned long ulThreadIDNewData;
+	tShbError ShbError;
+	int iPriority;
+
+	if ((pShbInstance_p == NULL) || (pfnSignalHandlerNewData_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if ((pShbMemInst->m_hThreadNewData != INVALID_HANDLE_VALUE) ||
+	    (pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU] !=
+	     INVALID_HANDLE_VALUE)
+	    || (pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP] !=
+		INVALID_HANDLE_VALUE)
+	    || (pShbMemInst->m_pfnSigHndlrNewData != NULL)) {
+		ShbError = kShbAlreadySignaling;
+		goto Exit;
+	}
+
+	pShbMemInst->m_pfnSigHndlrNewData = pfnSignalHandlerNewData_p;
+
+	// Because the event <pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA]>
+	// is used for signaling of new data after a write operation too (using
+	// SetEvent), it is always created here (see <ShbIpcAllocBuffer>).
+
+	pszObjectName =
+	    ShbIpcGetUniformObjectName(NAME_EVENT_TERM_REQU,
+				       pShbMemHeader->m_szBufferID, FALSE);
+	hEventTermRequ = CreateEvent(NULL,	// LPSECURITY_ATTRIBUTES lpEventAttributes
+				     FALSE,	// BOOL bManualReset
+				     FALSE,	// BOOL bInitialState
+				     pszObjectName);	// LPCTSTR lpName
+	pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU] = hEventTermRequ;
+	ASSERT(pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU] != NULL);
+
+	pszObjectName =
+	    ShbIpcGetUniformObjectName(NAME_EVENT_TERM_RESP,
+				       pShbMemHeader->m_szBufferID, FALSE);
+	hEventTermResp = CreateEvent(NULL,	// LPSECURITY_ATTRIBUTES lpEventAttributes
+				     FALSE,	// BOOL bManualReset
+				     FALSE,	// BOOL bInitialState
+				     pszObjectName);	// LPCTSTR lpName
+	pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP] = hEventTermResp;
+	ASSERT(pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP] != NULL);
+
+	hThreadNewData = CreateThread(NULL,	// LPSECURITY_ATTRIBUTES lpThreadAttributes
+				      0,	// SIZE_T dwStackSize
+				      ShbIpcThreadSignalNewData,	// LPTHREAD_START_ROUTINE lpStartAddress
+				      pShbInstance_p,	// LPVOID lpParameter
+				      0,	// DWORD dwCreationFlags
+				      &ulThreadIDNewData);	// LPDWORD lpThreadId
+
+	switch (ShbPriority_p) {
+	case kShbPriorityLow:
+		iPriority = THREAD_PRIORITY_BELOW_NORMAL;
+		break;
+
+	case kShbPriorityNormal:
+		iPriority = THREAD_PRIORITY_NORMAL;
+		break;
+
+	case kshbPriorityHigh:
+		iPriority = THREAD_PRIORITY_ABOVE_NORMAL;
+		break;
+
+	}
+
+	ASSERT(pShbMemInst->m_hThreadNewData != NULL);
+
+	SetThreadPriority(hThreadNewData, iPriority);
+
+	pShbMemInst->m_hThreadNewData = hThreadNewData;
+
+#ifndef NDEBUG
+	{
+		pShbMemInst->m_ulThreadIDNewData = ulThreadIDNewData;
+	}
+#endif
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Stop signaling of new data (called from reading process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcStopSignalingNewData(tShbInstance
+						     pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	HANDLE hEventTermRequ;
+	HANDLE hEventTermResp;
+	DWORD dwWaitResult;
+
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+
+	// terminate new data signaling thread
+	// (set event <hEventTermRequ> to wakeup the thread and dispose it
+	// to exit, then wait for confirmation using event <hEventTermResp>)
+	hEventTermRequ = pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU];
+	hEventTermResp = pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP];
+	if ((hEventTermRequ != INVALID_HANDLE_VALUE) &&
+	    (hEventTermResp != INVALID_HANDLE_VALUE)) {
+		TRACE0("\nShbIpcStopSignalingNewData(): enter wait state");
+		dwWaitResult = SignalObjectAndWait(hEventTermRequ,	// HANDLE hObjectToSignal
+						   hEventTermResp,	// HANDLE hObjectToWaitOn
+						   TIMEOUT_TERM_THREAD,	// DWORD dwMilliseconds
+						   FALSE);	// BOOL bAlertable
+		TRACE0
+		    ("\nShbIpcStopSignalingNewData(): wait state leaved: ---> ");
+		switch (dwWaitResult) {
+		case WAIT_OBJECT_0 + 0:	// event "new data signaling thread terminated"
+			{
+				TRACE0("Event = WAIT_OBJECT_0+0");
+				break;
+			}
+
+		default:
+			{
+				TRACE0("Unhandled Event");
+				ASSERT(0);
+				break;
+			}
+		}
+	}
+
+	if (pShbMemInst->m_hThreadNewData != INVALID_HANDLE_VALUE) {
+		CloseHandle(pShbMemInst->m_hThreadNewData);
+		pShbMemInst->m_hThreadNewData = INVALID_HANDLE_VALUE;
+	}
+
+	if (pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU] !=
+	    INVALID_HANDLE_VALUE) {
+		CloseHandle(pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU]);
+		pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU] =
+		    INVALID_HANDLE_VALUE;
+	}
+
+	if (pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP] !=
+	    INVALID_HANDLE_VALUE) {
+		CloseHandle(pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP]);
+		pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP] =
+		    INVALID_HANDLE_VALUE;
+	}
+
+	pShbMemInst->m_pfnSigHndlrNewData = NULL;
+
+	return (kShbOk);
+
+}
+
+//---------------------------------------------------------------------------
+//  Signal new data (called from writing process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcSignalNewData(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	HANDLE hEventNewData;
+	BOOL fRes;
+
+	// TRACE0("\nShbIpcSignalNewData(): enter\n");
+
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+
+	ASSERT(pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA] !=
+	       INVALID_HANDLE_VALUE);
+	hEventNewData = pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA];
+	if (hEventNewData != INVALID_HANDLE_VALUE) {
+		fRes = SetEvent(hEventNewData);
+		// TRACE1("\nShbIpcSignalNewData(): EventNewData set (Result=%d)\n", (int)fRes);
+		ASSERT(fRes);
+	}
+	// TRACE0("\nShbIpcSignalNewData(): leave\n");
+	return (kShbOk);
+
+}
+
+//---------------------------------------------------------------------------
+//  Start signaling for job ready (called from waiting process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcStartSignalingJobReady(tShbInstance
+						       pShbInstance_p,
+						       unsigned long
+						       ulTimeOut_p,
+						       tSigHndlrJobReady
+						       pfnSignalHandlerJobReady_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	HANDLE hThreadJobReady;
+	unsigned long ulThreadIDJobReady;
+	tShbError ShbError;
+
+	if ((pShbInstance_p == NULL) || (pfnSignalHandlerJobReady_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if ((pShbMemInst->m_hThreadJobReady != INVALID_HANDLE_VALUE) ||
+	    (pShbMemInst->m_pfnSigHndlrJobReady != NULL)) {
+		ShbError = kShbAlreadySignaling;
+		goto Exit;
+	}
+
+	pShbMemInst->m_ulTimeOutJobReady = ulTimeOut_p;
+	pShbMemInst->m_pfnSigHndlrJobReady = pfnSignalHandlerJobReady_p;
+
+	// Because the event <pShbMemInst->m_ahEventJobReady> is used for
+	// signaling of a finished job too (using SetEvent), it is always
+	// created here (see <ShbIpcAllocBuffer>).
+
+	hThreadJobReady = CreateThread(NULL,	// LPSECURITY_ATTRIBUTES lpThreadAttributes
+				       0,	// SIZE_T dwStackSize
+				       ShbIpcThreadSignalJobReady,	// LPTHREAD_START_ROUTINE lpStartAddress
+				       pShbInstance_p,	// LPVOID lpParameter
+				       0,	// DWORD dwCreationFlags
+				       &ulThreadIDJobReady);	// LPDWORD lpThreadId
+
+	pShbMemInst->m_hThreadJobReady = hThreadJobReady;
+	ASSERT(pShbMemInst->m_hThreadJobReady != NULL);
+
+#ifndef NDEBUG
+	{
+		pShbMemInst->m_ulThreadIDJobReady = ulThreadIDJobReady;
+	}
+#endif
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Signal job ready (called from executing process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcSignalJobReady(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	HANDLE hEventJobReady;
+	BOOL fRes;
+
+	// TRACE0("\nShbIpcSignalJobReady(): enter\n");
+
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+
+	ASSERT(pShbMemInst->m_hEventJobReady != INVALID_HANDLE_VALUE);
+	hEventJobReady = pShbMemInst->m_hEventJobReady;
+	if (hEventJobReady != INVALID_HANDLE_VALUE) {
+		fRes = SetEvent(hEventJobReady);
+		// TRACE1("\nShbIpcSignalJobReady(): EventJobReady set (Result=%d)\n", (int)fRes);
+		ASSERT(fRes);
+	}
+	// TRACE0("\nShbIpcSignalJobReady(): leave\n");
+	return (kShbOk);
+
+}
+
+//---------------------------------------------------------------------------
+//  Get pointer to common used share memory area
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void *ShbIpcGetShMemPtr(tShbInstance pShbInstance_p)
+{
+
+	tShbMemHeader *pShbMemHeader;
+	void *pShbShMemPtr;
+
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbInstance_p);
+	if (pShbMemHeader != NULL) {
+		pShbShMemPtr = (BYTE *) pShbMemHeader + sizeof(tShbMemHeader);
+	} else {
+		pShbShMemPtr = NULL;
+	}
+
+	return (pShbShMemPtr);
+
+}
+
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+#if !defined(SHBIPC_INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Allocate a memory block from process specific mempool
+//---------------------------------------------------------------------------
+
+static void *ShbIpcAllocPrivateMem(unsigned long ulMemSize_p)
+{
+
+	HGLOBAL hMem;
+	void *pMem;
+
+	hMem = GlobalAlloc(GMEM_FIXED, ulMemSize_p + sizeof(HGLOBAL));
+	pMem = GlobalLock(hMem);
+	if (pMem != NULL) {
+		*(HGLOBAL *) pMem = hMem;
+		(BYTE *) pMem += sizeof(HGLOBAL);
+	}
+
+#ifndef NDEBUG
+	{
+		memset(pMem, 0xaa, ulMemSize_p);
+	}
+#endif
+
+	return (pMem);
+
+}
+
+//---------------------------------------------------------------------------
+//  Release a memory block from process specific mempool
+//---------------------------------------------------------------------------
+
+static void ShbIpcReleasePrivateMem(void *pMem_p)
+{
+
+	HGLOBAL hMem;
+
+	if (pMem_p == NULL) {
+		return;
+	}
+
+	(BYTE *) pMem_p -= sizeof(HGLOBAL);
+	hMem = *(HGLOBAL *) pMem_p;
+
+	GlobalUnlock(hMem);
+	GlobalFree(hMem);
+
+	return;
+
+}
+
+//---------------------------------------------------------------------------
+//  Create uniform object name (needed for inter-process communication)
+//---------------------------------------------------------------------------
+
+const char *ShbIpcGetUniformObjectName(const char *pszObjectJobName_p,
+				       const char *pszBufferID_p,
+				       BOOL fGlobalObject_p)
+{
+
+	static char szObjectName[MAX_PATH];
+	char szObjectPrefix[MAX_PATH];
+
+	if (fGlobalObject_p) {
+		strncpy(szObjectPrefix, "Global\\", sizeof(szObjectPrefix));
+	} else {
+		_snprintf(szObjectPrefix, sizeof(szObjectPrefix), "PID%08lX_",
+			  (unsigned long)GetCurrentProcessId());
+	}
+
+	_snprintf(szObjectName, sizeof(szObjectName), "%s%s#%s",
+		  szObjectPrefix, pszBufferID_p, pszObjectJobName_p);
+
+	return (szObjectName);
+
+}
+
+//---------------------------------------------------------------------------
+//  Thread for new data signaling
+//---------------------------------------------------------------------------
+
+DWORD WINAPI ShbIpcThreadSignalNewData(LPVOID pvThreadParam_p)
+{
+
+	tShbInstance pShbInstance;
+	tShbMemInst *pShbMemInst;
+	DWORD dwWaitResult;
+	BOOL fTermRequ;
+	int fCallAgain;
+
+	TRACE1
+	    ("\nShbIpcThreadSignalNewData(): SignalThread started (pShbInstance=0x%08lX)\n",
+	     (DWORD) pvThreadParam_p);
+
+	pShbInstance = (tShbMemInst *) pvThreadParam_p;
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance);
+	fTermRequ = FALSE;
+
+	do {
+		ASSERT((pShbMemInst->m_ahEventNewData[0] !=
+			INVALID_HANDLE_VALUE)
+		       && (pShbMemInst->m_ahEventNewData[0] != NULL));
+		ASSERT((pShbMemInst->m_ahEventNewData[1] !=
+			INVALID_HANDLE_VALUE)
+		       && (pShbMemInst->m_ahEventNewData[1] != NULL));
+
+		TRACE0("\nShbIpcThreadSignalNewData(): enter wait state");
+		dwWaitResult = WaitForMultipleObjects(2,	// DWORD nCount
+						      pShbMemInst->m_ahEventNewData,	// const HANDLE* lpHandles
+						      FALSE,	// BOOL bWaitAll
+						      INFINITE);	// DWORD dwMilliseconds
+		TRACE0
+		    ("\nShbIpcThreadSignalNewData(): wait state leaved: ---> ");
+		switch (dwWaitResult) {
+		case WAIT_OBJECT_0 + 0:	// event "new data"
+			{
+				TRACE0("Event = WAIT_OBJECT_0+0");
+				if (pShbMemInst->m_pfnSigHndlrNewData != NULL) {
+					TRACE0
+					    ("\nShbIpcThreadSignalNewData(): calling SignalHandlerNewData");
+					do {
+						fCallAgain =
+						    pShbMemInst->
+						    m_pfnSigHndlrNewData
+						    (pShbInstance);
+						// d.k.: try to run any shared buffer which has higher priority.
+						//           under Windows this is not really necessary because the Windows scheduler
+						//           already preempts tasks with lower priority.
+					} while (fCallAgain != FALSE);
+				}
+				break;
+			}
+
+		case WAIT_OBJECT_0 + 1:	// event "terminate"
+			{
+				TRACE0("Event = WAIT_OBJECT_0+1");
+				fTermRequ = TRUE;
+				break;
+			}
+
+		default:
+			{
+				TRACE0("Unhandled Event");
+				ASSERT(0);
+				fTermRequ = TRUE;
+				break;
+			}
+		}
+	}
+	while (!fTermRequ);
+
+	if (pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP] !=
+	    INVALID_HANDLE_VALUE) {
+		SetEvent(pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP]);
+	}
+
+	TRACE1
+	    ("\nShbIpcThreadSignalNewData(): SignalThread terminated (pShbInstance=0x%08lX)\n",
+	     (DWORD) pShbInstance);
+
+	ExitThread(0);
+
+}
+
+//---------------------------------------------------------------------------
+//  Thread for new data signaling
+//---------------------------------------------------------------------------
+
+DWORD WINAPI ShbIpcThreadSignalJobReady(LPVOID pvThreadParam_p)
+{
+
+	tShbInstance *pShbInstance;
+	tShbMemInst *pShbMemInst;
+	DWORD ulTimeOut;
+	DWORD dwWaitResult;
+	unsigned int fTimeOut;
+
+	TRACE1
+	    ("\nShbIpcThreadSignalJobReady(): SignalThread started (pShbInstance=0x%08lX)\n",
+	     (DWORD) pvThreadParam_p);
+
+	pShbInstance = (tShbInstance *) pvThreadParam_p;
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance);
+	fTimeOut = FALSE;
+
+	if (pShbMemInst->m_ulTimeOutJobReady != 0) {
+		ulTimeOut = pShbMemInst->m_ulTimeOutJobReady;
+	} else {
+		ulTimeOut = INFINITE;
+	}
+
+	ASSERT((pShbMemInst->m_hEventJobReady != INVALID_HANDLE_VALUE)
+	       && (pShbMemInst->m_hEventJobReady != NULL));
+
+	TRACE0("\nShbIpcThreadSignalJobReady(): enter wait state");
+	dwWaitResult = WaitForSingleObject(pShbMemInst->m_hEventJobReady,	// HANDLE hHandle
+					   ulTimeOut);	// DWORD dwMilliseconds
+	TRACE0("\nShbIpcThreadSignalJobReady(): wait state leaved: ---> ");
+	switch (dwWaitResult) {
+	case WAIT_OBJECT_0 + 0:	// event "new data"
+		{
+			TRACE0("Event = WAIT_OBJECT_0+0");
+			fTimeOut = FALSE;
+			break;
+		}
+
+	case WAIT_TIMEOUT:
+		{
+			TRACE0("\nEvent = WAIT_TIMEOUT");
+			fTimeOut = TRUE;
+			// ASSERT(0);
+			break;
+		}
+
+	default:
+		{
+			TRACE0("Unhandled Event");
+			fTimeOut = TRUE;
+			ASSERT(0);
+			break;
+		}
+	}
+
+	if (pShbMemInst->m_pfnSigHndlrJobReady != NULL) {
+		TRACE0
+		    ("\nShbIpcThreadSignalJobReady(): calling SignalHandlerJobReady");
+		pShbMemInst->m_pfnSigHndlrJobReady(pShbInstance, fTimeOut);
+	}
+
+	pShbMemInst->m_hThreadJobReady = INVALID_HANDLE_VALUE;
+	pShbMemInst->m_pfnSigHndlrJobReady = NULL;
+
+	TRACE1
+	    ("\nShbIpcThreadSignalJobReady(): SignalThread terminated (pShbInstance=0x%08lX)\n",
+	     (DWORD) pShbInstance);
+
+	ExitThread(0);
+
+}
+
+#endif
+
+// EOF
diff --git a/drivers/staging/epl/ShbIpc.h b/drivers/staging/epl/ShbIpc.h
new file mode 100644
index 000000000000..cad8846a0824
--- /dev/null
+++ b/drivers/staging/epl/ShbIpc.h
@@ -0,0 +1,125 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      Project independend shared buffer (linear + circular)
+
+  Description:  Declaration of platform specific part for the
+                shared buffer
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+  2006/06/27 -rs:   V 1.00 (initial version)
+
+****************************************************************************/
+
+#ifndef _SHBIPC_H_
+#define _SHBIPC_H_
+
+//---------------------------------------------------------------------------
+//  Type definitions
+//---------------------------------------------------------------------------
+
+typedef int (*tSigHndlrNewData) (tShbInstance pShbInstance_p);
+typedef void (*tSigHndlrJobReady) (tShbInstance pShbInstance_p,
+				   unsigned int fTimeOut_p);
+
+#if (TARGET_SYSTEM == _WIN32_)
+#if defined(INLINE_FUNCTION_DEF)
+#undef  INLINE_FUNCTION
+#define INLINE_FUNCTION     INLINE_FUNCTION_DEF
+#define SHBIPC_INLINE_ENABLED      TRUE
+#define SHBIPC_INLINED
+#include "ShbIpc-Win32.c"
+#endif
+
+#elif (TARGET_SYSTEM == _LINUX_)
+#if defined(INLINE_FUNCTION_DEF)
+#undef  INLINE_FUNCTION
+#define INLINE_FUNCTION     INLINE_FUNCTION_DEF
+#define SHBIPC_INLINE_ENABLED      TRUE
+#define SHBIPC_INLINED
+#include "ShbIpc-LinuxKernel.c"
+#endif
+#endif
+
+//---------------------------------------------------------------------------
+//  Prototypes
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcInit(void);
+tShbError ShbIpcExit(void);
+
+tShbError ShbIpcAllocBuffer(unsigned long ulBufferSize_p,
+			    const char *pszBufferID_p,
+			    tShbInstance * ppShbInstance_p,
+			    unsigned int *pfShbNewCreated_p);
+tShbError ShbIpcReleaseBuffer(tShbInstance pShbInstance_p);
+
+#if !defined(SHBIPC_INLINE_ENABLED)
+
+tShbError ShbIpcEnterAtomicSection(tShbInstance pShbInstance_p);
+tShbError ShbIpcLeaveAtomicSection(tShbInstance pShbInstance_p);
+
+tShbError ShbIpcStartSignalingNewData(tShbInstance pShbInstance_p,
+				      tSigHndlrNewData
+				      pfnSignalHandlerNewData_p,
+				      tShbPriority ShbPriority_p);
+tShbError ShbIpcStopSignalingNewData(tShbInstance pShbInstance_p);
+tShbError ShbIpcSignalNewData(tShbInstance pShbInstance_p);
+
+tShbError ShbIpcStartSignalingJobReady(tShbInstance pShbInstance_p,
+				       unsigned long ulTimeOut_p,
+				       tSigHndlrJobReady
+				       pfnSignalHandlerJobReady_p);
+tShbError ShbIpcSignalJobReady(tShbInstance pShbInstance_p);
+
+void *ShbIpcGetShMemPtr(tShbInstance pShbInstance_p);
+#endif
+
+#undef  SHBIPC_INLINE_ENABLED	// disable actual inlining of functions
+#undef  INLINE_FUNCTION
+#define INLINE_FUNCTION		// define INLINE_FUNCTION to nothing
+
+#endif // #ifndef _SHBIPC_H_
diff --git a/drivers/staging/epl/ShbLinuxKernel.h b/drivers/staging/epl/ShbLinuxKernel.h
new file mode 100644
index 000000000000..812702add4f0
--- /dev/null
+++ b/drivers/staging/epl/ShbLinuxKernel.h
@@ -0,0 +1,68 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      Project independend shared buffer (linear + circular)
+
+  Description:  Declaration of platform specific part for the
+                shared buffer
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+  2006/07/20 -rs:   V 1.00 (initial version)
+
+****************************************************************************/
+
+#ifndef _SHBLINUXKERNEL_H_
+#define _SHBLINUXKERNEL_H_
+
+struct sShbMemTable {
+	int m_iBufferId;
+	void *m_pBuffer;
+	struct sShbMemTable *m_psNextMemTableElement;
+};
+
+extern struct sShbMemTable *psMemTableElementFirst_g;
+
+#endif // _SHBLINUXKERNEL_H_
diff --git a/drivers/staging/epl/SocketLinuxKernel.c b/drivers/staging/epl/SocketLinuxKernel.c
new file mode 100644
index 000000000000..562bc4a3e562
--- /dev/null
+++ b/drivers/staging/epl/SocketLinuxKernel.c
@@ -0,0 +1,197 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  Wrapper for BSD socket API for Linux kernel
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: SocketLinuxKernel.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                Dev C++ and GNU-Compiler for m68k
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/08/25 d.k.:   start of implementation
+
+****************************************************************************/
+
+#include <linux/net.h>
+#include <linux/in.h>
+#include "SocketLinuxKernel.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Kernel Module specific Data Structures
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+SOCKET socket(int af, int type, int protocol)
+{
+	int rc;
+	SOCKET socket;
+
+	rc = sock_create_kern(af, type, protocol, &socket);
+	if (rc < 0) {
+		socket = NULL;
+		goto Exit;
+	}
+
+      Exit:
+	return socket;
+}
+
+int bind(SOCKET socket_p, const struct sockaddr *addr, int addrlen)
+{
+	int rc;
+
+	rc = socket_p->ops->bind(socket_p, (struct sockaddr *)addr, addrlen);
+
+	return rc;
+}
+
+int closesocket(SOCKET socket_p)
+{
+	sock_release(socket_p);
+
+	return 0;
+}
+
+int recvfrom(SOCKET socket_p, char *buf, int len, int flags,
+	     struct sockaddr *from, int *fromlen)
+{
+	int rc;
+	struct msghdr msg;
+	struct kvec iov;
+
+	msg.msg_control = NULL;
+	msg.msg_controllen = 0;
+	msg.msg_name = from;	// will be struct sock_addr
+	msg.msg_namelen = *fromlen;
+	iov.iov_len = len;
+	iov.iov_base = buf;
+
+	rc = kernel_recvmsg(socket_p, &msg, &iov, 1, iov.iov_len, 0);
+
+	return rc;
+}
+
+int sendto(SOCKET socket_p, const char *buf, int len, int flags,
+	   const struct sockaddr *to, int tolen)
+{
+	int rc;
+	struct msghdr msg;
+	struct kvec iov;
+
+	msg.msg_control = NULL;
+	msg.msg_controllen = 0;
+	msg.msg_name = (struct sockaddr *)to;	// will be struct sock_addr
+	msg.msg_namelen = tolen;
+	msg.msg_flags = 0;
+	iov.iov_len = len;
+	iov.iov_base = (char *)buf;
+
+	rc = kernel_sendmsg(socket_p, &msg, &iov, 1, len);
+
+	return rc;
+}
+
+// EOF
diff --git a/drivers/staging/epl/SocketLinuxKernel.h b/drivers/staging/epl/SocketLinuxKernel.h
new file mode 100644
index 000000000000..6e1d61989607
--- /dev/null
+++ b/drivers/staging/epl/SocketLinuxKernel.h
@@ -0,0 +1,105 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for BSD socket API for Linux kernel
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: SocketLinuxKernel.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/08/25 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#ifndef _SOCKETLINUXKERNEL_H_
+#define _SOCKETLINUXKERNEL_H_
+
+#include <linux/net.h>
+#include <linux/in.h>
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define INVALID_SOCKET  0
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef struct socket *SOCKET;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+int bind(SOCKET s, const struct sockaddr *addr, int addrlen);
+
+int closesocket(SOCKET s);
+
+int recvfrom(SOCKET s, char *buf, int len, int flags, struct sockaddr *from,
+	     int *fromlen);
+
+int sendto(SOCKET s, const char *buf, int len, int flags,
+	   const struct sockaddr *to, int tolen);
+
+SOCKET socket(int af, int type, int protocol);
+
+#endif // #ifndef _SOCKETLINUXKERNEL_H_
diff --git a/drivers/staging/epl/TimerHighReskX86.c b/drivers/staging/epl/TimerHighReskX86.c
new file mode 100644
index 000000000000..82eee4702aa6
--- /dev/null
+++ b/drivers/staging/epl/TimerHighReskX86.c
@@ -0,0 +1,522 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  target specific implementation of
+                high resolution timer module for X86 under Linux
+                The Linux kernel has to be compiled with high resolution
+                timers enabled. This is done by configuring the kernel
+                with CONFIG_HIGH_RES_TIMERS enabled.
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: TimerHighReskX86.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:38:01 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GNU
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "kernel/EplTimerHighResk.h"
+#include "Benchmark.h"
+
+//#include <linux/config.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/hrtimer.h>
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define TIMER_COUNT           2	/* max 15 timers selectable */
+#define TIMER_MIN_VAL_SINGLE  5000	/* min 5us */
+#define TIMER_MIN_VAL_CYCLE   100000	/* min 100us */
+
+#define PROVE_OVERRUN
+
+#ifndef CONFIG_HIGH_RES_TIMERS
+#error "Kernel symbol CONFIG_HIGH_RES_TIMERS is required."
+#endif
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)     TgtDbgPostTraceValue(v)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)
+#endif
+#define HRT_DBG_POST_TRACE_VALUE(Event_p, uiNodeId_p, wErrorCode_p) \
+    TGT_DBG_POST_TRACE_VALUE((0xE << 28) | (Event_p << 24) \
+                             | (uiNodeId_p << 16) | wErrorCode_p)
+
+#define TIMERHDL_MASK         0x0FFFFFFF
+#define TIMERHDL_SHIFT        28
+#define HDL_TO_IDX(Hdl)       ((Hdl >> TIMERHDL_SHIFT) - 1)
+#define HDL_INIT(Idx)         ((Idx + 1) << TIMERHDL_SHIFT)
+#define HDL_INC(Hdl)          (((Hdl + 1) & TIMERHDL_MASK) \
+                               | (Hdl & ~TIMERHDL_MASK))
+
+//---------------------------------------------------------------------------
+// modul global types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	tEplTimerEventArg m_EventArg;
+	tEplTimerkCallback m_pfnCallback;
+	struct hrtimer m_Timer;
+	BOOL m_fContinuously;
+	unsigned long long m_ullPeriod;
+
+} tEplTimerHighReskTimerInfo;
+
+typedef struct {
+	tEplTimerHighReskTimerInfo m_aTimerInfo[TIMER_COUNT];
+
+} tEplTimerHighReskInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+static tEplTimerHighReskInstance EplTimerHighReskInstance_l;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+enum hrtimer_restart EplTimerHighReskCallback(struct hrtimer *pTimer_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimerHighReskInit()
+//
+// Description: initializes the high resolution timer module.
+//
+// Parameters:  void
+//
+// Return:      tEplKernel      = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimerHighReskInit(void)
+{
+	tEplKernel Ret;
+
+	Ret = EplTimerHighReskAddInstance();
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimerHighReskAddInstance()
+//
+// Description: initializes the high resolution timer module.
+//
+// Parameters:  void
+//
+// Return:      tEplKernel      = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimerHighReskAddInstance(void)
+{
+	tEplKernel Ret;
+	unsigned int uiIndex;
+
+	Ret = kEplSuccessful;
+
+	EPL_MEMSET(&EplTimerHighReskInstance_l, 0,
+		   sizeof(EplTimerHighReskInstance_l));
+
+#ifndef CONFIG_HIGH_RES_TIMERS
+	printk
+	    ("EplTimerHighResk: Kernel symbol CONFIG_HIGH_RES_TIMERS is required.\n");
+	Ret = kEplNoResource;
+	return Ret;
+#endif
+
+	/*
+	 * Initialize hrtimer structures for all usable timers.
+	 */
+	for (uiIndex = 0; uiIndex < TIMER_COUNT; uiIndex++) {
+		tEplTimerHighReskTimerInfo *pTimerInfo;
+		struct hrtimer *pTimer;
+
+		pTimerInfo = &EplTimerHighReskInstance_l.m_aTimerInfo[uiIndex];
+		pTimer = &pTimerInfo->m_Timer;
+		hrtimer_init(pTimer, CLOCK_MONOTONIC, HRTIMER_MODE_ABS);
+
+		pTimer->function = EplTimerHighReskCallback;
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimerHighReskDelInstance()
+//
+// Description: shuts down the high resolution timer module.
+//
+// Parameters:  void
+//
+// Return:      tEplKernel      = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimerHighReskDelInstance(void)
+{
+	tEplTimerHighReskTimerInfo *pTimerInfo;
+	tEplKernel Ret;
+	unsigned int uiIndex;
+
+	Ret = kEplSuccessful;
+
+	for (uiIndex = 0; uiIndex < TIMER_COUNT; uiIndex++) {
+		pTimerInfo = &EplTimerHighReskInstance_l.m_aTimerInfo[0];
+		pTimerInfo->m_pfnCallback = NULL;
+		pTimerInfo->m_EventArg.m_TimerHdl = 0;
+		/*
+		 * In this case we can not just try to cancel the timer.
+		 * We actually have to wait until its callback function
+		 * has returned.
+		 */
+		hrtimer_cancel(&pTimerInfo->m_Timer);
+	}
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimerHighReskModifyTimerNs()
+//
+// Description: modifies the timeout of the timer with the specified handle.
+//              If the handle the pointer points to is zero, the timer must
+//              be created first.
+//              If it is not possible to stop the old timer,
+//              this function always assures that the old timer does not
+//              trigger the callback function with the same handle as the new
+//              timer. That means the callback function must check the passed
+//              handle with the one returned by this function. If these are
+//              unequal, the call can be discarded.
+//
+// Parameters:  pTimerHdl_p     = pointer to timer handle
+//              ullTimeNs_p     = relative timeout in [ns]
+//              pfnCallback_p   = callback function, which is called mutual
+//                                exclusive with the Edrv callback functions
+//                                (Rx and Tx).
+//              ulArgument_p    = user-specific argument
+//              fContinuously_p = if TRUE, callback function will be called
+//                                continuously;
+//                                otherwise, it is a oneshot timer.
+//
+// Return:      tEplKernel      = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimerHighReskModifyTimerNs(tEplTimerHdl * pTimerHdl_p,
+						unsigned long long ullTimeNs_p,
+						tEplTimerkCallback
+						pfnCallback_p,
+						unsigned long ulArgument_p,
+						BOOL fContinuously_p)
+{
+	tEplKernel Ret;
+	unsigned int uiIndex;
+	tEplTimerHighReskTimerInfo *pTimerInfo;
+	ktime_t RelTime;
+
+	Ret = kEplSuccessful;
+
+	// check pointer to handle
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+	if (*pTimerHdl_p == 0) {	// no timer created yet
+
+		// search free timer info structure
+		pTimerInfo = &EplTimerHighReskInstance_l.m_aTimerInfo[0];
+		for (uiIndex = 0; uiIndex < TIMER_COUNT;
+		     uiIndex++, pTimerInfo++) {
+			if (pTimerInfo->m_EventArg.m_TimerHdl == 0) {	// free structure found
+				break;
+			}
+		}
+		if (uiIndex >= TIMER_COUNT) {	// no free structure found
+			Ret = kEplTimerNoTimerCreated;
+			goto Exit;
+		}
+
+		pTimerInfo->m_EventArg.m_TimerHdl = HDL_INIT(uiIndex);
+	} else {
+		uiIndex = HDL_TO_IDX(*pTimerHdl_p);
+		if (uiIndex >= TIMER_COUNT) {	// invalid handle
+			Ret = kEplTimerInvalidHandle;
+			goto Exit;
+		}
+
+		pTimerInfo = &EplTimerHighReskInstance_l.m_aTimerInfo[uiIndex];
+	}
+
+	/*
+	 * increment timer handle
+	 * (if timer expires right after this statement, the user
+	 * would detect an unknown timer handle and discard it)
+	 */
+	pTimerInfo->m_EventArg.m_TimerHdl =
+	    HDL_INC(pTimerInfo->m_EventArg.m_TimerHdl);
+	*pTimerHdl_p = pTimerInfo->m_EventArg.m_TimerHdl;
+
+	// reject too small time values
+	if ((fContinuously_p && (ullTimeNs_p < TIMER_MIN_VAL_CYCLE))
+	    || (!fContinuously_p && (ullTimeNs_p < TIMER_MIN_VAL_SINGLE))) {
+		Ret = kEplTimerNoTimerCreated;
+		goto Exit;
+	}
+
+	pTimerInfo->m_EventArg.m_ulArg = ulArgument_p;
+	pTimerInfo->m_pfnCallback = pfnCallback_p;
+	pTimerInfo->m_fContinuously = fContinuously_p;
+	pTimerInfo->m_ullPeriod = ullTimeNs_p;
+
+	/*
+	 * HRTIMER_MODE_REL does not influence general handling of this timer.
+	 * It only sets relative mode for this start operation.
+	 * -> Expire time is calculated by: Now + RelTime
+	 * hrtimer_start also skips pending timer events.
+	 * The state HRTIMER_STATE_CALLBACK is ignored.
+	 * We have to cope with that in our callback function.
+	 */
+	RelTime = ktime_add_ns(ktime_set(0, 0), ullTimeNs_p);
+	hrtimer_start(&pTimerInfo->m_Timer, RelTime, HRTIMER_MODE_REL);
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimerHighReskDeleteTimer()
+//
+// Description: deletes the timer with the specified handle. Afterward the
+//              handle is set to zero.
+//
+// Parameters:  pTimerHdl_p     = pointer to timer handle
+//
+// Return:      tEplKernel      = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimerHighReskDeleteTimer(tEplTimerHdl * pTimerHdl_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiIndex;
+	tEplTimerHighReskTimerInfo *pTimerInfo;
+
+	// check pointer to handle
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+	if (*pTimerHdl_p == 0) {	// no timer created yet
+		goto Exit;
+	} else {
+		uiIndex = HDL_TO_IDX(*pTimerHdl_p);
+		if (uiIndex >= TIMER_COUNT) {	// invalid handle
+			Ret = kEplTimerInvalidHandle;
+			goto Exit;
+		}
+		pTimerInfo = &EplTimerHighReskInstance_l.m_aTimerInfo[uiIndex];
+		if (pTimerInfo->m_EventArg.m_TimerHdl != *pTimerHdl_p) {	// invalid handle
+			goto Exit;
+		}
+	}
+
+	*pTimerHdl_p = 0;
+	pTimerInfo->m_EventArg.m_TimerHdl = 0;
+	pTimerInfo->m_pfnCallback = NULL;
+
+	/*
+	 * Three return cases of hrtimer_try_to_cancel have to be tracked:
+	 *  1 - timer has been removed
+	 *  0 - timer was not active
+	 *      We need not do anything. hrtimer timers just consist of
+	 *      a hrtimer struct, which we might enqueue in the hrtimers
+	 *      event list by calling hrtimer_start().
+	 *      If a timer is not enqueued, it is not present in hrtimers.
+	 * -1 - callback function is running
+	 *      In this case we have to ensure that the timer is not
+	 *      continuously restarted. This has been done by clearing
+	 *      its handle.
+	 */
+	hrtimer_try_to_cancel(&pTimerInfo->m_Timer);
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimerHighReskCallback()
+//
+// Description: Callback function commonly used for all timers.
+//
+// Parameters:  pTimer_p = pointer to hrtimer
+//
+// Return:
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+enum hrtimer_restart EplTimerHighReskCallback(struct hrtimer *pTimer_p)
+{
+	unsigned int uiIndex;
+	tEplTimerHighReskTimerInfo *pTimerInfo;
+	tEplTimerHdl OrgTimerHdl;
+	enum hrtimer_restart Ret;
+
+	BENCHMARK_MOD_24_SET(4);
+
+	Ret = HRTIMER_NORESTART;
+	pTimerInfo =
+	    container_of(pTimer_p, tEplTimerHighReskTimerInfo, m_Timer);
+	uiIndex = HDL_TO_IDX(pTimerInfo->m_EventArg.m_TimerHdl);
+	if (uiIndex >= TIMER_COUNT) {	// invalid handle
+		goto Exit;
+	}
+
+	/*
+	 * We store the timer handle before calling the callback function
+	 * as the timer can be modified inside it.
+	 */
+	OrgTimerHdl = pTimerInfo->m_EventArg.m_TimerHdl;
+
+	if (pTimerInfo->m_pfnCallback != NULL) {
+		pTimerInfo->m_pfnCallback(&pTimerInfo->m_EventArg);
+	}
+
+	if (pTimerInfo->m_fContinuously) {
+		ktime_t Interval;
+#ifdef PROVE_OVERRUN
+		ktime_t Now;
+		unsigned long Overruns;
+#endif
+
+		if (OrgTimerHdl != pTimerInfo->m_EventArg.m_TimerHdl) {
+			/* modified timer has already been restarted */
+			goto Exit;
+		}
+#ifdef PROVE_OVERRUN
+		Now = ktime_get();
+		Interval =
+		    ktime_add_ns(ktime_set(0, 0), pTimerInfo->m_ullPeriod);
+		Overruns = hrtimer_forward(pTimer_p, Now, Interval);
+		if (Overruns > 1) {
+			printk
+			    ("EplTimerHighResk: Continuous timer (handle 0x%lX) had to skip %lu interval(s)!\n",
+			     pTimerInfo->m_EventArg.m_TimerHdl, Overruns - 1);
+		}
+#else
+		pTimer_p->expires = ktime_add_ns(pTimer_p->expires,
+						 pTimerInfo->m_ullPeriod);
+#endif
+
+		Ret = HRTIMER_RESTART;
+	}
+
+      Exit:
+	BENCHMARK_MOD_24_RESET(4);
+	return Ret;
+}
diff --git a/drivers/staging/epl/VirtualEthernetLinux.c b/drivers/staging/epl/VirtualEthernetLinux.c
new file mode 100644
index 000000000000..5d838dbf73a7
--- /dev/null
+++ b/drivers/staging/epl/VirtualEthernetLinux.c
@@ -0,0 +1,342 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  Virtual Ethernet Driver for Linux
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: VirtualEthernetLinux.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/11/20 17:06:51 $
+
+                $State: Exp $
+
+                Build Environment:
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/12 -ar:   start of the implementation, version 1.00
+
+  2006/09/18 d.k.:  integration into EPL DLLk module
+
+  ToDo:
+
+  void netif_carrier_off(struct net_device *dev);
+  void netif_carrier_on(struct net_device *dev);
+
+****************************************************************************/
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/if_arp.h>
+#include <net/arp.h>
+
+#include <net/protocol.h>
+#include <net/pkt_sched.h>
+#include <linux/if_ether.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/udp.h>
+#include <linux/mm.h>
+#include <linux/types.h>
+#include <linux/skbuff.h>	/* for struct sk_buff */
+
+#include "kernel/VirtualEthernet.h"
+#include "kernel/EplDllkCal.h"
+#include "kernel/EplDllk.h"
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_VETH)) != 0)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef EPL_VETH_TX_TIMEOUT
+//#define EPL_VETH_TX_TIMEOUT (2*HZ)
+#define EPL_VETH_TX_TIMEOUT 0	// d.k.: we use no timeout
+#endif
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+static struct net_device *pVEthNetDevice_g = NULL;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static int VEthOpen(struct net_device *pNetDevice_p);
+static int VEthClose(struct net_device *pNetDevice_p);
+static int VEthXmit(struct sk_buff *pSkb_p, struct net_device *pNetDevice_p);
+static struct net_device_stats *VEthGetStats(struct net_device *pNetDevice_p);
+static void VEthTimeout(struct net_device *pNetDevice_p);
+static tEplKernel VEthRecvFrame(tEplFrameInfo * pFrameInfo_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static int VEthOpen(struct net_device *pNetDevice_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	//open the device
+//	struct net_device_stats* pStats = netdev_priv(pNetDevice_p);
+
+	//start the interface queue for the network subsystem
+	netif_start_queue(pNetDevice_p);
+
+	// register callback function in DLL
+	Ret = EplDllkRegAsyncHandler(VEthRecvFrame);
+
+	EPL_DBGLVL_VETH_TRACE1
+	    ("VEthOpen: EplDllkRegAsyncHandler returned 0x%02X\n", Ret);
+
+	return 0;
+}
+
+static int VEthClose(struct net_device *pNetDevice_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	EPL_DBGLVL_VETH_TRACE0("VEthClose\n");
+
+	Ret = EplDllkDeregAsyncHandler(VEthRecvFrame);
+
+	//stop the interface queue for the network subsystem
+	netif_stop_queue(pNetDevice_p);
+	return 0;
+}
+
+static int VEthXmit(struct sk_buff *pSkb_p, struct net_device *pNetDevice_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplFrameInfo FrameInfo;
+
+	//transmit function
+	struct net_device_stats *pStats = netdev_priv(pNetDevice_p);
+
+	//save timestemp
+	pNetDevice_p->trans_start = jiffies;
+
+	FrameInfo.m_pFrame = (tEplFrame *) pSkb_p->data;
+	FrameInfo.m_uiFrameSize = pSkb_p->len;
+
+	//call send fkt on DLL
+	Ret = EplDllkCalAsyncSend(&FrameInfo, kEplDllAsyncReqPrioGeneric);
+	if (Ret != kEplSuccessful) {
+		EPL_DBGLVL_VETH_TRACE1
+		    ("VEthXmit: EplDllkCalAsyncSend returned 0x%02X\n", Ret);
+		netif_stop_queue(pNetDevice_p);
+		goto Exit;
+	} else {
+		EPL_DBGLVL_VETH_TRACE0("VEthXmit: frame passed to DLL\n");
+		dev_kfree_skb(pSkb_p);
+
+		//set stats for the device
+		pStats->tx_packets++;
+		pStats->tx_bytes += FrameInfo.m_uiFrameSize;
+	}
+
+      Exit:
+	return 0;
+
+}
+
+static struct net_device_stats *VEthGetStats(struct net_device *pNetDevice_p)
+{
+	EPL_DBGLVL_VETH_TRACE0("VEthGetStats\n");
+
+	return netdev_priv(pNetDevice_p);
+}
+
+static void VEthTimeout(struct net_device *pNetDevice_p)
+{
+	EPL_DBGLVL_VETH_TRACE0("VEthTimeout(\n");
+
+	// $$$ d.k.: move to extra function, which is called by DLL when new space is available in TxFifo
+	if (netif_queue_stopped(pNetDevice_p)) {
+		netif_wake_queue(pNetDevice_p);
+	}
+}
+
+static tEplKernel VEthRecvFrame(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	struct net_device *pNetDevice = pVEthNetDevice_g;
+	struct net_device_stats *pStats = netdev_priv(pNetDevice);
+	struct sk_buff *pSkb;
+
+	EPL_DBGLVL_VETH_TRACE1("VEthRecvFrame: FrameSize=%u\n",
+			       pFrameInfo_p->m_uiFrameSize);
+
+	pSkb = dev_alloc_skb(pFrameInfo_p->m_uiFrameSize + 2);
+	if (pSkb == NULL) {
+		pStats->rx_dropped++;
+		goto Exit;
+	}
+	pSkb->dev = pNetDevice;
+
+	skb_reserve(pSkb, 2);
+
+	memcpy((void *)skb_put(pSkb, pFrameInfo_p->m_uiFrameSize),
+	       pFrameInfo_p->m_pFrame, pFrameInfo_p->m_uiFrameSize);
+
+	pSkb->protocol = eth_type_trans(pSkb, pNetDevice);
+	pSkb->ip_summed = CHECKSUM_UNNECESSARY;
+
+	// call netif_rx with skb
+	netif_rx(pSkb);
+
+	EPL_DBGLVL_VETH_TRACE1("VEthRecvFrame: SrcMAC=0x%llx\n",
+			       AmiGetQword48FromBe(pFrameInfo_p->m_pFrame->
+						   m_be_abSrcMac));
+
+	// update receive statistics
+	pStats->rx_packets++;
+	pStats->rx_bytes += pFrameInfo_p->m_uiFrameSize;
+
+      Exit:
+	return Ret;
+}
+
+tEplKernel PUBLIC VEthAddInstance(tEplDllkInitParam * pInitParam_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// allocate net device structure with priv pointing to stats structure
+	pVEthNetDevice_g =
+	    alloc_netdev(sizeof(struct net_device_stats), EPL_VETH_NAME,
+			 ether_setup);
+//    pVEthNetDevice_g = alloc_etherdev(sizeof (struct net_device_stats));
+
+	if (pVEthNetDevice_g == NULL) {
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+	pVEthNetDevice_g->open = VEthOpen;
+	pVEthNetDevice_g->stop = VEthClose;
+	pVEthNetDevice_g->get_stats = VEthGetStats;
+	pVEthNetDevice_g->hard_start_xmit = VEthXmit;
+	pVEthNetDevice_g->tx_timeout = VEthTimeout;
+	pVEthNetDevice_g->watchdog_timeo = EPL_VETH_TX_TIMEOUT;
+	pVEthNetDevice_g->destructor = free_netdev;
+
+	// copy own MAC address to net device structure
+	memcpy(pVEthNetDevice_g->dev_addr, pInitParam_p->m_be_abSrcMac, 6);
+
+	//register VEth to the network subsystem
+	if (register_netdev(pVEthNetDevice_g)) {
+		EPL_DBGLVL_VETH_TRACE0
+		    ("VEthAddInstance: Could not register VEth...\n");
+	} else {
+		EPL_DBGLVL_VETH_TRACE0
+		    ("VEthAddInstance: Register VEth successfull...\n");
+	}
+
+      Exit:
+	return Ret;
+}
+
+tEplKernel PUBLIC VEthDelInstance(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (pVEthNetDevice_g != NULL) {
+		//unregister VEth from the network subsystem
+		unregister_netdev(pVEthNetDevice_g);
+		// destructor was set to free_netdev,
+		// so we do not need to call free_netdev here
+		pVEthNetDevice_g = NULL;
+	}
+
+	return Ret;
+}
+
+#endif // (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_VETH)) != 0)
diff --git a/drivers/staging/epl/amix86.c b/drivers/staging/epl/amix86.c
new file mode 100644
index 000000000000..9f742384dac0
--- /dev/null
+++ b/drivers/staging/epl/amix86.c
@@ -0,0 +1,905 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  Abstract Memory Interface for x86 compatible
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: amix86.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    ...
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  r.s.: first implemetation
+
+  2006-06-13  d.k.: duplicate functions for little endian and big endian
+
+****************************************************************************/
+
+//#include "global.h"
+//#include "EplAmi.h"
+#include "EplInc.h"
+
+#if (!defined(EPL_AMI_INLINED)) || defined(INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef struct {
+	WORD m_wWord;
+
+} twStruct;
+
+typedef struct {
+	DWORD m_dwDword;
+
+} tdwStruct;
+
+typedef struct {
+	QWORD m_qwQword;
+
+} tqwStruct;
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetXXXToBe()
+//
+// Description: writes the specified value to the absolute address in
+//              big endian
+//
+// Parameters:  pAddr_p                 = absolute address
+//              xXXXVal_p               = value
+//
+// Returns:     (none)
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+//------------< write BYTE in big endian >--------------------------
+/*
+void  PUBLIC  AmiSetByteToBe (void FAR* pAddr_p, BYTE bByteVal_p)
+{
+
+   *(BYTE FAR*)pAddr_p = bByteVal_p;
+
+}
+*/
+
+//------------< write WORD in big endian >--------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetWordToBe(void FAR * pAddr_p, WORD wWordVal_p)
+{
+	twStruct FAR *pwStruct;
+	twStruct wValue;
+
+	wValue.m_wWord = (WORD) ((wWordVal_p & 0x00FF) << 8);	//LSB to MSB
+	wValue.m_wWord |= (WORD) ((wWordVal_p & 0xFF00) >> 8);	//MSB to LSB
+
+	pwStruct = (twStruct FAR *) pAddr_p;
+	pwStruct->m_wWord = wValue.m_wWord;
+
+}
+
+//------------< write DWORD in big endian >-------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetDwordToBe(void FAR * pAddr_p,
+					    DWORD dwDwordVal_p)
+{
+	tdwStruct FAR *pdwStruct;
+	tdwStruct dwValue;
+
+	dwValue.m_dwDword = ((dwDwordVal_p & 0x000000FF) << 24);	//LSB to MSB
+	dwValue.m_dwDword |= ((dwDwordVal_p & 0x0000FF00) << 8);
+	dwValue.m_dwDword |= ((dwDwordVal_p & 0x00FF0000) >> 8);
+	dwValue.m_dwDword |= ((dwDwordVal_p & 0xFF000000) >> 24);	//MSB to LSB
+
+	pdwStruct = (tdwStruct FAR *) pAddr_p;
+	pdwStruct->m_dwDword = dwValue.m_dwDword;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetXXXToLe()
+//
+// Description: writes the specified value to the absolute address in
+//              little endian
+//
+// Parameters:  pAddr_p                 = absolute address
+//              xXXXVal_p               = value
+//
+// Returns:     (none)
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+//------------< write BYTE in little endian >--------------------------
+/*
+void  PUBLIC  AmiSetByteToLe (void FAR* pAddr_p, BYTE bByteVal_p)
+{
+
+   *(BYTE FAR*)pAddr_p = bByteVal_p;
+
+}
+*/
+
+//------------< write WORD in little endian >--------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetWordToLe(void FAR * pAddr_p, WORD wWordVal_p)
+{
+	twStruct FAR *pwStruct;
+
+	pwStruct = (twStruct FAR *) pAddr_p;
+	pwStruct->m_wWord = wWordVal_p;
+
+}
+
+//------------< write DWORD in little endian >-------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetDwordToLe(void FAR * pAddr_p,
+					    DWORD dwDwordVal_p)
+{
+	tdwStruct FAR *pdwStruct;
+
+	pdwStruct = (tdwStruct FAR *) pAddr_p;
+	pdwStruct->m_dwDword = dwDwordVal_p;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetXXXFromBe()
+//
+// Description: reads the specified value from the absolute address in
+//              big endian
+//
+// Parameters:  pAddr_p                 = absolute address
+//
+// Returns:     XXX                     = value
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+//------------< read BYTE in big endian >---------------------------
+/*
+BYTE  PUBLIC  AmiGetByteFromBe (void FAR* pAddr_p)
+{
+
+   return ( *(BYTE FAR*)pAddr_p );
+
+}
+*/
+
+//------------< read WORD in big endian >---------------------------
+
+INLINE_FUNCTION WORD PUBLIC AmiGetWordFromBe(void FAR * pAddr_p)
+{
+	twStruct FAR *pwStruct;
+	twStruct wValue;
+
+	pwStruct = (twStruct FAR *) pAddr_p;
+
+	wValue.m_wWord = (WORD) ((pwStruct->m_wWord & 0x00FF) << 8);	//LSB to MSB
+	wValue.m_wWord |= (WORD) ((pwStruct->m_wWord & 0xFF00) >> 8);	//MSB to LSB
+
+	return (wValue.m_wWord);
+
+}
+
+//------------< read DWORD in big endian >--------------------------
+
+INLINE_FUNCTION DWORD PUBLIC AmiGetDwordFromBe(void FAR * pAddr_p)
+{
+	tdwStruct FAR *pdwStruct;
+	tdwStruct dwValue;
+
+	pdwStruct = (tdwStruct FAR *) pAddr_p;
+
+	dwValue.m_dwDword = ((pdwStruct->m_dwDword & 0x000000FF) << 24);	//LSB to MSB
+	dwValue.m_dwDword |= ((pdwStruct->m_dwDword & 0x0000FF00) << 8);
+	dwValue.m_dwDword |= ((pdwStruct->m_dwDword & 0x00FF0000) >> 8);
+	dwValue.m_dwDword |= ((pdwStruct->m_dwDword & 0xFF000000) >> 24);	//MSB to LSB
+
+	return (dwValue.m_dwDword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetXXXFromLe()
+//
+// Description: reads the specified value from the absolute address in
+//              little endian
+//
+// Parameters:  pAddr_p                 = absolute address
+//
+// Returns:     XXX                     = value
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+//------------< read BYTE in little endian >---------------------------
+/*
+BYTE  PUBLIC  AmiGetByteFromLe (void FAR* pAddr_p)
+{
+
+   return ( *(BYTE FAR*)pAddr_p );
+
+}
+*/
+
+//------------< read WORD in little endian >---------------------------
+
+INLINE_FUNCTION WORD PUBLIC AmiGetWordFromLe(void FAR * pAddr_p)
+{
+	twStruct FAR *pwStruct;
+
+	pwStruct = (twStruct FAR *) pAddr_p;
+	return (pwStruct->m_wWord);
+
+}
+
+//------------< read DWORD in little endian >--------------------------
+
+INLINE_FUNCTION DWORD PUBLIC AmiGetDwordFromLe(void FAR * pAddr_p)
+{
+	tdwStruct FAR *pdwStruct;
+
+	pdwStruct = (tdwStruct FAR *) pAddr_p;
+	return (pdwStruct->m_dwDword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetDword24ToBe()
+//
+// Description: sets a 24 bit value to a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              dwDwordVal_p    = value to set
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetDword24ToBe(void FAR * pAddr_p,
+					      DWORD dwDwordVal_p)
+{
+
+	((BYTE FAR *) pAddr_p)[0] = ((BYTE FAR *) & dwDwordVal_p)[2];
+	((BYTE FAR *) pAddr_p)[1] = ((BYTE FAR *) & dwDwordVal_p)[1];
+	((BYTE FAR *) pAddr_p)[2] = ((BYTE FAR *) & dwDwordVal_p)[0];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetDword24ToLe()
+//
+// Description: sets a 24 bit value to a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              dwDwordVal_p    = value to set
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetDword24ToLe(void FAR * pAddr_p,
+					      DWORD dwDwordVal_p)
+{
+
+	((BYTE FAR *) pAddr_p)[0] = ((BYTE FAR *) & dwDwordVal_p)[0];
+	((BYTE FAR *) pAddr_p)[1] = ((BYTE FAR *) & dwDwordVal_p)[1];
+	((BYTE FAR *) pAddr_p)[2] = ((BYTE FAR *) & dwDwordVal_p)[2];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetDword24FromBe()
+//
+// Description: reads a 24 bit value from a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      DWORD           = read value
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION DWORD PUBLIC AmiGetDword24FromBe(void FAR * pAddr_p)
+{
+
+	tdwStruct dwStruct;
+
+	dwStruct.m_dwDword = AmiGetDwordFromBe(pAddr_p);
+	dwStruct.m_dwDword >>= 8;
+
+	return (dwStruct.m_dwDword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetDword24FromLe()
+//
+// Description: reads a 24 bit value from a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      DWORD           = read value
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION DWORD PUBLIC AmiGetDword24FromLe(void FAR * pAddr_p)
+{
+
+	tdwStruct dwStruct;
+
+	dwStruct.m_dwDword = AmiGetDwordFromLe(pAddr_p);
+	dwStruct.m_dwDword &= 0x00FFFFFF;
+
+	return (dwStruct.m_dwDword);
+
+}
+
+//#ifdef USE_VAR64
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword64ToBe()
+//
+// Description: sets a 64 bit value to a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword64ToBe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	((BYTE FAR *) pAddr_p)[0] = ((BYTE FAR *) & qwQwordVal_p)[7];
+	((BYTE FAR *) pAddr_p)[1] = ((BYTE FAR *) & qwQwordVal_p)[6];
+	((BYTE FAR *) pAddr_p)[2] = ((BYTE FAR *) & qwQwordVal_p)[5];
+	((BYTE FAR *) pAddr_p)[3] = ((BYTE FAR *) & qwQwordVal_p)[4];
+	((BYTE FAR *) pAddr_p)[4] = ((BYTE FAR *) & qwQwordVal_p)[3];
+	((BYTE FAR *) pAddr_p)[5] = ((BYTE FAR *) & qwQwordVal_p)[2];
+	((BYTE FAR *) pAddr_p)[6] = ((BYTE FAR *) & qwQwordVal_p)[1];
+	((BYTE FAR *) pAddr_p)[7] = ((BYTE FAR *) & qwQwordVal_p)[0];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword64ToLe()
+//
+// Description: sets a 64 bit value to a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword64ToLe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	QWORD FAR *pqwDst;
+
+	pqwDst = (QWORD FAR *) pAddr_p;
+	*pqwDst = qwQwordVal_p;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword64FromBe()
+//
+// Description: reads a 64 bit value from a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword64FromBe(void FAR * pAddr_p)
+{
+
+	tqwStruct qwStruct;
+
+	((BYTE FAR *) & qwStruct.m_qwQword)[0] = ((BYTE FAR *) pAddr_p)[7];
+	((BYTE FAR *) & qwStruct.m_qwQword)[1] = ((BYTE FAR *) pAddr_p)[6];
+	((BYTE FAR *) & qwStruct.m_qwQword)[2] = ((BYTE FAR *) pAddr_p)[5];
+	((BYTE FAR *) & qwStruct.m_qwQword)[3] = ((BYTE FAR *) pAddr_p)[4];
+	((BYTE FAR *) & qwStruct.m_qwQword)[4] = ((BYTE FAR *) pAddr_p)[3];
+	((BYTE FAR *) & qwStruct.m_qwQword)[5] = ((BYTE FAR *) pAddr_p)[2];
+	((BYTE FAR *) & qwStruct.m_qwQword)[6] = ((BYTE FAR *) pAddr_p)[1];
+	((BYTE FAR *) & qwStruct.m_qwQword)[7] = ((BYTE FAR *) pAddr_p)[0];
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword64FromLe()
+//
+// Description: reads a 64 bit value from a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword64FromLe(void FAR * pAddr_p)
+{
+
+	tqwStruct FAR *pqwStruct;
+	tqwStruct qwStruct;
+
+	pqwStruct = (tqwStruct FAR *) pAddr_p;
+	qwStruct.m_qwQword = pqwStruct->m_qwQword;
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword40ToBe()
+//
+// Description: sets a 40 bit value to a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword40ToBe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	((BYTE FAR *) pAddr_p)[0] = ((BYTE FAR *) & qwQwordVal_p)[4];
+	((BYTE FAR *) pAddr_p)[1] = ((BYTE FAR *) & qwQwordVal_p)[3];
+	((BYTE FAR *) pAddr_p)[2] = ((BYTE FAR *) & qwQwordVal_p)[2];
+	((BYTE FAR *) pAddr_p)[3] = ((BYTE FAR *) & qwQwordVal_p)[1];
+	((BYTE FAR *) pAddr_p)[4] = ((BYTE FAR *) & qwQwordVal_p)[0];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword40ToLe()
+//
+// Description: sets a 40 bit value to a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword40ToLe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	((DWORD FAR *) pAddr_p)[0] = ((DWORD FAR *) & qwQwordVal_p)[0];
+	((BYTE FAR *) pAddr_p)[4] = ((BYTE FAR *) & qwQwordVal_p)[4];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword40FromBe()
+//
+// Description: reads a 40 bit value from a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword40FromBe(void FAR * pAddr_p)
+{
+
+	tqwStruct qwStruct;
+
+	qwStruct.m_qwQword = AmiGetQword64FromBe(pAddr_p);
+	qwStruct.m_qwQword >>= 24;
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword40FromLe()
+//
+// Description: reads a 40 bit value from a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword40FromLe(void FAR * pAddr_p)
+{
+
+	tqwStruct qwStruct;
+
+	qwStruct.m_qwQword = AmiGetQword64FromLe(pAddr_p);
+	qwStruct.m_qwQword &= 0x000000FFFFFFFFFFLL;
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword48ToBe()
+//
+// Description: sets a 48 bit value to a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword48ToBe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	((BYTE FAR *) pAddr_p)[0] = ((BYTE FAR *) & qwQwordVal_p)[5];
+	((BYTE FAR *) pAddr_p)[1] = ((BYTE FAR *) & qwQwordVal_p)[4];
+	((BYTE FAR *) pAddr_p)[2] = ((BYTE FAR *) & qwQwordVal_p)[3];
+	((BYTE FAR *) pAddr_p)[3] = ((BYTE FAR *) & qwQwordVal_p)[2];
+	((BYTE FAR *) pAddr_p)[4] = ((BYTE FAR *) & qwQwordVal_p)[1];
+	((BYTE FAR *) pAddr_p)[5] = ((BYTE FAR *) & qwQwordVal_p)[0];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword48ToLe()
+//
+// Description: sets a 48 bit value to a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword48ToLe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	((DWORD FAR *) pAddr_p)[0] = ((DWORD FAR *) & qwQwordVal_p)[0];
+	((WORD FAR *) pAddr_p)[2] = ((WORD FAR *) & qwQwordVal_p)[2];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword48FromBe()
+//
+// Description: reads a 48 bit value from a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword48FromBe(void FAR * pAddr_p)
+{
+
+	tqwStruct qwStruct;
+
+	qwStruct.m_qwQword = AmiGetQword64FromBe(pAddr_p);
+	qwStruct.m_qwQword >>= 16;
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword48FromLe()
+//
+// Description: reads a 48 bit value from a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword48FromLe(void FAR * pAddr_p)
+{
+
+	tqwStruct qwStruct;
+
+	qwStruct.m_qwQword = AmiGetQword64FromLe(pAddr_p);
+	qwStruct.m_qwQword &= 0x0000FFFFFFFFFFFFLL;
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword56ToBe()
+//
+// Description: sets a 56 bit value to a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword56ToBe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	((BYTE FAR *) pAddr_p)[0] = ((BYTE FAR *) & qwQwordVal_p)[6];
+	((BYTE FAR *) pAddr_p)[1] = ((BYTE FAR *) & qwQwordVal_p)[5];
+	((BYTE FAR *) pAddr_p)[2] = ((BYTE FAR *) & qwQwordVal_p)[4];
+	((BYTE FAR *) pAddr_p)[3] = ((BYTE FAR *) & qwQwordVal_p)[3];
+	((BYTE FAR *) pAddr_p)[4] = ((BYTE FAR *) & qwQwordVal_p)[2];
+	((BYTE FAR *) pAddr_p)[5] = ((BYTE FAR *) & qwQwordVal_p)[1];
+	((BYTE FAR *) pAddr_p)[6] = ((BYTE FAR *) & qwQwordVal_p)[0];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword56ToLe()
+//
+// Description: sets a 56 bit value to a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword56ToLe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	((DWORD FAR *) pAddr_p)[0] = ((DWORD FAR *) & qwQwordVal_p)[0];
+	((WORD FAR *) pAddr_p)[2] = ((WORD FAR *) & qwQwordVal_p)[2];
+	((BYTE FAR *) pAddr_p)[6] = ((BYTE FAR *) & qwQwordVal_p)[6];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword56FromBe()
+//
+// Description: reads a 56 bit value from a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword56FromBe(void FAR * pAddr_p)
+{
+
+	tqwStruct qwStruct;
+
+	qwStruct.m_qwQword = AmiGetQword64FromBe(pAddr_p);
+	qwStruct.m_qwQword >>= 8;
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword56FromLe()
+//
+// Description: reads a 56 bit value from a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword56FromLe(void FAR * pAddr_p)
+{
+
+	tqwStruct qwStruct;
+
+	qwStruct.m_qwQword = AmiGetQword64FromLe(pAddr_p);
+	qwStruct.m_qwQword &= 0x00FFFFFFFFFFFFFFLL;
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetTimeOfDay()
+//
+// Description: sets a TIME_OF_DAY (CANopen) value to a buffer
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              pTimeOfDay_p    = pointer to struct TIME_OF_DAY
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetTimeOfDay(void FAR * pAddr_p,
+					    tTimeOfDay FAR * pTimeOfDay_p)
+{
+
+	AmiSetDwordToLe(((BYTE FAR *) pAddr_p),
+			pTimeOfDay_p->m_dwMs & 0x0FFFFFFF);
+	AmiSetWordToLe(((BYTE FAR *) pAddr_p) + 4, pTimeOfDay_p->m_wDays);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetTimeOfDay()
+//
+// Description: reads a TIME_OF_DAY (CANopen) value from a buffer
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//              pTimeOfDay_p    = pointer to struct TIME_OF_DAY
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiGetTimeOfDay(void FAR * pAddr_p,
+					    tTimeOfDay FAR * pTimeOfDay_p)
+{
+
+	pTimeOfDay_p->m_dwMs =
+	    AmiGetDwordFromLe(((BYTE FAR *) pAddr_p)) & 0x0FFFFFFF;
+	pTimeOfDay_p->m_wDays = AmiGetWordFromLe(((BYTE FAR *) pAddr_p) + 4);
+
+}
+
+#endif
+
+// EOF
+
+// Die letzte Zeile muß unbedingt eine leere Zeile sein, weil manche Compiler
+// damit ein Problem haben, wenn das nicht so ist (z.B. GNU oder Borland C++ Builder).
diff --git a/drivers/staging/epl/demo_main.c b/drivers/staging/epl/demo_main.c
new file mode 100644
index 000000000000..263fa042291d
--- /dev/null
+++ b/drivers/staging/epl/demo_main.c
@@ -0,0 +1,961 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  demoapplication for EPL MN (with SDO over UDP)
+                under Linux on X86 with RTL8139 Ethernet controller
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: demo_main.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.10 $  $Date: 2008/11/19 18:11:43 $
+
+                $State: Exp $
+
+                Build Environment:
+                GCC
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/09/01 d.k.:   start of implementation
+
+****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/major.h>
+#include <linux/version.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <asm/atomic.h>
+#include <linux/sched.h>
+#include <linux/kmod.h>
+#include <linux/slab.h>
+#include <linux/pci.h>
+#include <linux/proc_fs.h>
+
+#include "Epl.h"
+#include "proc_fs.h"
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
+    // remove ("make invisible") obsolete symbols for kernel versions 2.6
+    // and higher
+#define MOD_INC_USE_COUNT
+#define MOD_DEC_USE_COUNT
+#define EXPORT_NO_SYMBOLS
+#else
+#error "This driver needs a 2.6.x kernel or higher"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+// Metainformation
+MODULE_LICENSE("Dual BSD/GPL");
+#ifdef MODULE_AUTHOR
+MODULE_AUTHOR("Daniel.Krueger@SYSTEC-electronic.com");
+MODULE_DESCRIPTION("EPL MN demo");
+#endif
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#endif
+
+#define NODEID      0xF0	//=> MN
+#define CYCLE_LEN   5000	// [us]
+#define IP_ADDR     0xc0a86401	// 192.168.100.1
+#define SUBNET_MASK 0xFFFFFF00	// 255.255.255.0
+#define HOSTNAME    "SYS TEC electronic EPL Stack    "
+#define IF_ETH      EPL_VETH_NAME
+
+// LIGHT EFFECT
+#define DEFAULT_MAX_CYCLE_COUNT 20	// 6 is very fast
+#define APP_DEFAULT_MODE        0x01
+#define APP_LED_COUNT           5	// number of LEDs in one row
+#define APP_LED_MASK            ((1 << APP_LED_COUNT) - 1)
+#define APP_DOUBLE_LED_MASK     ((1 << (APP_LED_COUNT * 2)) - 1)
+#define APP_MODE_COUNT          5
+#define APP_MODE_MASK           ((1 << APP_MODE_COUNT) - 1)
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+CONST BYTE abMacAddr[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+
+BYTE bVarIn1_l;
+BYTE bVarOut1_l;
+BYTE bVarOut1Old_l;
+BYTE bModeSelect_l;		// state of the pushbuttons to select the mode
+BYTE bSpeedSelect_l;		// state of the pushbuttons to increase/decrease the speed
+BYTE bSpeedSelectOld_l;		// old state of the pushbuttons
+DWORD dwLeds_l;			// current state of all LEDs
+BYTE bLedsRow1_l;		// current state of the LEDs in row 1
+BYTE bLedsRow2_l;		// current state of the LEDs in row 2
+BYTE abSelect_l[3];		// pushbuttons from CNs
+
+DWORD dwMode_l;			// current mode
+int iCurCycleCount_l;		// current cycle count
+int iMaxCycleCount_l;		// maximum cycle count (i.e. number of cycles until next light movement step)
+int iToggle;			// indicates the light movement direction
+
+BYTE abDomain_l[3000];
+
+static wait_queue_head_t WaitQueueShutdown_g;	// wait queue for tEplNmtEventSwitchOff
+static atomic_t AtomicShutdown_g = ATOMIC_INIT(FALSE);
+
+static DWORD dw_le_CycleLen_g;
+
+static uint uiNodeId_g = EPL_C_ADR_INVALID;
+module_param_named(nodeid, uiNodeId_g, uint, 0);
+
+static uint uiCycleLen_g = CYCLE_LEN;
+module_param_named(cyclelen, uiCycleLen_g, uint, 0);
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+// This function is the entry point for your object dictionary. It is defined
+// in OBJDICT.C by define EPL_OBD_INIT_RAM_NAME. Use this function name to define
+// this function prototype here. If you want to use more than one Epl
+// instances then the function name of each object dictionary has to differ.
+
+tEplKernel PUBLIC EplObdInitRam(tEplObdInitParam MEM * pInitParam_p);
+
+tEplKernel PUBLIC AppCbEvent(tEplApiEventType EventType_p,	// IN: event type (enum)
+			     tEplApiEventArg * pEventArg_p,	// IN: event argument (union)
+			     void GENERIC * pUserArg_p);
+
+tEplKernel PUBLIC AppCbSync(void);
+
+static int __init EplLinInit(void);
+static void __exit EplLinExit(void);
+
+//---------------------------------------------------------------------------
+//  Kernel Module specific Data Structures
+//---------------------------------------------------------------------------
+
+EXPORT_NO_SYMBOLS;
+
+//module_init(EplLinInit);
+//module_exit(EplLinExit);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static int __init EplLinInit(void)
+{
+	tEplKernel EplRet;
+	int iRet;
+	static tEplApiInitParam EplApiInitParam = { 0 };
+	char *sHostname = HOSTNAME;
+	char *argv[4], *envp[3];
+	char sBuffer[16];
+	unsigned int uiVarEntries;
+	tEplObdSize ObdSize;
+
+	atomic_set(&AtomicShutdown_g, TRUE);
+
+	// get node ID from insmod command line
+	EplApiInitParam.m_uiNodeId = uiNodeId_g;
+
+	if (EplApiInitParam.m_uiNodeId == EPL_C_ADR_INVALID) {	// invalid node ID set
+		// set default node ID
+		EplApiInitParam.m_uiNodeId = NODEID;
+	}
+
+	uiNodeId_g = EplApiInitParam.m_uiNodeId;
+
+	// calculate IP address
+	EplApiInitParam.m_dwIpAddress =
+	    (0xFFFFFF00 & IP_ADDR) | EplApiInitParam.m_uiNodeId;
+
+	EplApiInitParam.m_fAsyncOnly = FALSE;
+
+	EplApiInitParam.m_uiSizeOfStruct = sizeof(EplApiInitParam);
+	EPL_MEMCPY(EplApiInitParam.m_abMacAddress, abMacAddr,
+		   sizeof(EplApiInitParam.m_abMacAddress));
+//    EplApiInitParam.m_abMacAddress[5] = (BYTE) EplApiInitParam.m_uiNodeId;
+	EplApiInitParam.m_dwFeatureFlags = -1;
+	EplApiInitParam.m_dwCycleLen = uiCycleLen_g;	// required for error detection
+	EplApiInitParam.m_uiIsochrTxMaxPayload = 100;	// const
+	EplApiInitParam.m_uiIsochrRxMaxPayload = 100;	// const
+	EplApiInitParam.m_dwPresMaxLatency = 50000;	// const; only required for IdentRes
+	EplApiInitParam.m_uiPreqActPayloadLimit = 36;	// required for initialisation (+28 bytes)
+	EplApiInitParam.m_uiPresActPayloadLimit = 36;	// required for initialisation of Pres frame (+28 bytes)
+	EplApiInitParam.m_dwAsndMaxLatency = 150000;	// const; only required for IdentRes
+	EplApiInitParam.m_uiMultiplCycleCnt = 0;	// required for error detection
+	EplApiInitParam.m_uiAsyncMtu = 1500;	// required to set up max frame size
+	EplApiInitParam.m_uiPrescaler = 2;	// required for sync
+	EplApiInitParam.m_dwLossOfFrameTolerance = 500000;
+	EplApiInitParam.m_dwAsyncSlotTimeout = 3000000;
+	EplApiInitParam.m_dwWaitSocPreq = 150000;
+	EplApiInitParam.m_dwDeviceType = -1;	// NMT_DeviceType_U32
+	EplApiInitParam.m_dwVendorId = -1;	// NMT_IdentityObject_REC.VendorId_U32
+	EplApiInitParam.m_dwProductCode = -1;	// NMT_IdentityObject_REC.ProductCode_U32
+	EplApiInitParam.m_dwRevisionNumber = -1;	// NMT_IdentityObject_REC.RevisionNo_U32
+	EplApiInitParam.m_dwSerialNumber = -1;	// NMT_IdentityObject_REC.SerialNo_U32
+	EplApiInitParam.m_dwSubnetMask = SUBNET_MASK;
+	EplApiInitParam.m_dwDefaultGateway = 0;
+	EPL_MEMCPY(EplApiInitParam.m_sHostname, sHostname,
+		   sizeof(EplApiInitParam.m_sHostname));
+
+	// currently unset parameters left at default value 0
+	//EplApiInitParam.m_qwVendorSpecificExt1;
+	//EplApiInitParam.m_dwVerifyConfigurationDate; // CFM_VerifyConfiguration_REC.ConfDate_U32
+	//EplApiInitParam.m_dwVerifyConfigurationTime; // CFM_VerifyConfiguration_REC.ConfTime_U32
+	//EplApiInitParam.m_dwApplicationSwDate;       // PDL_LocVerApplSw_REC.ApplSwDate_U32 on programmable device or date portion of NMT_ManufactSwVers_VS on non-programmable device
+	//EplApiInitParam.m_dwApplicationSwTime;       // PDL_LocVerApplSw_REC.ApplSwTime_U32 on programmable device or time portion of NMT_ManufactSwVers_VS on non-programmable device
+	//EplApiInitParam.m_abVendorSpecificExt2[48];
+
+	// set callback functions
+	EplApiInitParam.m_pfnCbEvent = AppCbEvent;
+	EplApiInitParam.m_pfnCbSync = AppCbSync;
+
+	printk
+	    ("\n\n Hello, I'm a simple POWERLINK node running as %s!\n  (build: %s / %s)\n\n",
+	     (uiNodeId_g ==
+	      EPL_C_ADR_MN_DEF_NODE_ID ? "Managing Node" : "Controlled Node"),
+	     __DATE__, __TIME__);
+
+	// initialize the Linux a wait queue for shutdown of this module
+	init_waitqueue_head(&WaitQueueShutdown_g);
+
+	// initialize the procfs device
+	EplRet = EplLinProcInit();
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+	// initialize POWERLINK stack
+	EplRet = EplApiInitialize(&EplApiInitParam);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+	// link process variables used by CN to object dictionary
+	ObdSize = sizeof(bVarIn1_l);
+	uiVarEntries = 1;
+	EplRet =
+	    EplApiLinkObject(0x6000, &bVarIn1_l, &uiVarEntries, &ObdSize, 0x01);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+
+	ObdSize = sizeof(bVarOut1_l);
+	uiVarEntries = 1;
+	EplRet =
+	    EplApiLinkObject(0x6200, &bVarOut1_l, &uiVarEntries, &ObdSize,
+			     0x01);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+	// link process variables used by MN to object dictionary
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	ObdSize = sizeof(bLedsRow1_l);
+	uiVarEntries = 1;
+	EplRet =
+	    EplApiLinkObject(0x2000, &bLedsRow1_l, &uiVarEntries, &ObdSize,
+			     0x01);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+
+	ObdSize = sizeof(bLedsRow2_l);
+	uiVarEntries = 1;
+	EplRet =
+	    EplApiLinkObject(0x2000, &bLedsRow2_l, &uiVarEntries, &ObdSize,
+			     0x02);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+
+	ObdSize = sizeof(bSpeedSelect_l);
+	uiVarEntries = 1;
+	EplRet =
+	    EplApiLinkObject(0x2000, &bSpeedSelect_l, &uiVarEntries, &ObdSize,
+			     0x03);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+
+	ObdSize = sizeof(bSpeedSelectOld_l);
+	uiVarEntries = 1;
+	EplRet =
+	    EplApiLinkObject(0x2000, &bSpeedSelectOld_l, &uiVarEntries,
+			     &ObdSize, 0x04);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+
+	ObdSize = sizeof(abSelect_l[0]);
+	uiVarEntries = sizeof(abSelect_l);
+	EplRet =
+	    EplApiLinkObject(0x2200, &abSelect_l[0], &uiVarEntries, &ObdSize,
+			     0x01);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// link a DOMAIN to object 0x6100, but do not exit, if it is missing
+	ObdSize = sizeof(abDomain_l);
+	uiVarEntries = 1;
+	EplRet =
+	    EplApiLinkObject(0x6100, &abDomain_l, &uiVarEntries, &ObdSize,
+			     0x00);
+	if (EplRet != kEplSuccessful) {
+		printk("EplApiLinkObject(0x6100): returns 0x%X\n", EplRet);
+	}
+	// reset old process variables
+	bVarOut1Old_l = 0;
+	bSpeedSelectOld_l = 0;
+	dwMode_l = APP_DEFAULT_MODE;
+	iMaxCycleCount_l = DEFAULT_MAX_CYCLE_COUNT;
+
+	// configure IP address of virtual network interface
+	// for TCP/IP communication over the POWERLINK network
+	sprintf(sBuffer, "%lu.%lu.%lu.%lu",
+		(EplApiInitParam.m_dwIpAddress >> 24),
+		((EplApiInitParam.m_dwIpAddress >> 16) & 0xFF),
+		((EplApiInitParam.m_dwIpAddress >> 8) & 0xFF),
+		(EplApiInitParam.m_dwIpAddress & 0xFF));
+	/* set up a minimal environment */
+	iRet = 0;
+	envp[iRet++] = "HOME=/";
+	envp[iRet++] = "PATH=/sbin:/bin:/usr/sbin:/usr/bin";
+	envp[iRet] = NULL;
+
+	/* set up the argument list */
+	iRet = 0;
+	argv[iRet++] = "/sbin/ifconfig";
+	argv[iRet++] = IF_ETH;
+	argv[iRet++] = sBuffer;
+	argv[iRet] = NULL;
+
+	/* call ifconfig to configure the virtual network interface */
+	iRet = call_usermodehelper(argv[0], argv, envp, 1);
+	printk("ifconfig %s %s returned %d\n", argv[1], argv[2], iRet);
+
+	// start the NMT state machine
+	EplRet = EplApiExecNmtCommand(kEplNmtEventSwReset);
+	atomic_set(&AtomicShutdown_g, FALSE);
+
+      Exit:
+	printk("EplLinInit(): returns 0x%X\n", EplRet);
+	return EplRet;
+}
+
+static void __exit EplLinExit(void)
+{
+	tEplKernel EplRet;
+
+	// halt the NMT state machine
+	// so the processing of POWERLINK frames stops
+	EplRet = EplApiExecNmtCommand(kEplNmtEventSwitchOff);
+
+	// wait until NMT state machine is shut down
+	wait_event_interruptible(WaitQueueShutdown_g,
+				 (atomic_read(&AtomicShutdown_g) == TRUE));
+/*    if ((iErr != 0) || (atomic_read(&AtomicShutdown_g) == EVENT_STATE_IOCTL))
+    {   // waiting was interrupted by signal or application called wrong function
+        EplRet = kEplShutdown;
+    }*/
+	// delete instance for all modules
+	EplRet = EplApiShutdown();
+	printk("EplApiShutdown():  0x%X\n", EplRet);
+
+	// deinitialize proc fs
+	EplRet = EplLinProcFree();
+	printk("EplLinProcFree():        0x%X\n", EplRet);
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    AppCbEvent
+//
+// Description: event callback function called by EPL API layer within
+//              user part (low priority).
+//
+// Parameters:  EventType_p     = event type
+//              pEventArg_p     = pointer to union, which describes
+//                                the event in detail
+//              pUserArg_p      = user specific argument
+//
+// Returns:     tEplKernel      = error code,
+//                                kEplSuccessful = no error
+//                                kEplReject = reject further processing
+//                                otherwise = post error event to API layer
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC AppCbEvent(tEplApiEventType EventType_p,	// IN: event type (enum)
+			     tEplApiEventArg * pEventArg_p,	// IN: event argument (union)
+			     void GENERIC * pUserArg_p)
+{
+	tEplKernel EplRet = kEplSuccessful;
+
+	// check if NMT_GS_OFF is reached
+	switch (EventType_p) {
+	case kEplApiEventNmtStateChange:
+		{
+			switch (pEventArg_p->m_NmtStateChange.m_NewNmtState) {
+			case kEplNmtGsOff:
+				{	// NMT state machine was shut down,
+					// because of user signal (CTRL-C) or critical EPL stack error
+					// -> also shut down EplApiProcess() and main()
+					EplRet = kEplShutdown;
+
+					printk
+					    ("AppCbEvent(kEplNmtGsOff) originating event = 0x%X\n",
+					     pEventArg_p->m_NmtStateChange.
+					     m_NmtEvent);
+
+					// wake up EplLinExit()
+					atomic_set(&AtomicShutdown_g, TRUE);
+					wake_up_interruptible
+					    (&WaitQueueShutdown_g);
+					break;
+				}
+
+			case kEplNmtGsResetCommunication:
+				{
+					DWORD dwBuffer;
+
+					// configure OD for MN in state ResetComm after reseting the OD
+					// TODO: setup your own network configuration here
+					dwBuffer = (EPL_NODEASSIGN_NODE_IS_CN | EPL_NODEASSIGN_NODE_EXISTS);	// 0x00000003L
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x01,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x02,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x03,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x04,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x05,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x06,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x07,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x08,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x20,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0xFE,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x6E,
+								   &dwBuffer,
+								   4);
+
+//                    dwBuffer |= EPL_NODEASSIGN_MANDATORY_CN;    // 0x0000000BL
+//                    EplRet = EplApiWriteLocalObject(0x1F81, 0x6E, &dwBuffer, 4);
+					dwBuffer = (EPL_NODEASSIGN_MN_PRES | EPL_NODEASSIGN_NODE_EXISTS);	// 0x00010001L
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0xF0,
+								   &dwBuffer,
+								   4);
+
+					// continue
+				}
+
+			case kEplNmtGsResetConfiguration:
+				{
+					unsigned int uiSize;
+
+					// fetch object 0x1006 NMT_CycleLen_U32 from local OD (in little endian byte order)
+					// for configuration of remote CN
+					uiSize = 4;
+					EplRet =
+					    EplApiReadObject(NULL, 0, 0x1006,
+							     0x00,
+							     &dw_le_CycleLen_g,
+							     &uiSize,
+							     kEplSdoTypeAsnd,
+							     NULL);
+					if (EplRet != kEplSuccessful) {	// local OD access failed
+						break;
+					}
+					// continue
+				}
+
+			case kEplNmtMsPreOperational1:
+				{
+					printk
+					    ("AppCbEvent(0x%X) originating event = 0x%X\n",
+					     pEventArg_p->m_NmtStateChange.
+					     m_NewNmtState,
+					     pEventArg_p->m_NmtStateChange.
+					     m_NmtEvent);
+
+					// continue
+				}
+
+			case kEplNmtGsInitialising:
+			case kEplNmtGsResetApplication:
+			case kEplNmtMsNotActive:
+			case kEplNmtCsNotActive:
+			case kEplNmtCsPreOperational1:
+				{
+					break;
+				}
+
+			case kEplNmtCsOperational:
+			case kEplNmtMsOperational:
+				{
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+			}
+
+/*
+            switch (pEventArg_p->m_NmtStateChange.m_NmtEvent)
+            {
+                case kEplNmtEventSwReset:
+                case kEplNmtEventResetNode:
+                case kEplNmtEventResetCom:
+                case kEplNmtEventResetConfig:
+                case kEplNmtEventInternComError:
+                case kEplNmtEventNmtCycleError:
+                {
+                    printk("AppCbEvent(0x%X) originating event = 0x%X\n",
+                           pEventArg_p->m_NmtStateChange.m_NewNmtState,
+                           pEventArg_p->m_NmtStateChange.m_NmtEvent);
+                    break;
+                }
+
+                default:
+                {
+                    break;
+                }
+            }
+*/
+			break;
+		}
+
+	case kEplApiEventCriticalError:
+	case kEplApiEventWarning:
+		{		// error or warning occured within the stack or the application
+			// on error the API layer stops the NMT state machine
+
+			printk
+			    ("AppCbEvent(Err/Warn): Source=%02X EplError=0x%03X",
+			     pEventArg_p->m_InternalError.m_EventSource,
+			     pEventArg_p->m_InternalError.m_EplError);
+			// check additional argument
+			switch (pEventArg_p->m_InternalError.m_EventSource) {
+			case kEplEventSourceEventk:
+			case kEplEventSourceEventu:
+				{	// error occured within event processing
+					// either in kernel or in user part
+					printk(" OrgSource=%02X\n",
+					       pEventArg_p->m_InternalError.
+					       m_Arg.m_EventSource);
+					break;
+				}
+
+			case kEplEventSourceDllk:
+				{	// error occured within the data link layer (e.g. interrupt processing)
+					// the DWORD argument contains the DLL state and the NMT event
+					printk(" val=%lX\n",
+					       pEventArg_p->m_InternalError.
+					       m_Arg.m_dwArg);
+					break;
+				}
+
+			default:
+				{
+					printk("\n");
+					break;
+				}
+			}
+			break;
+		}
+
+	case kEplApiEventNode:
+		{
+//            printk("AppCbEvent(Node): Source=%02X EplError=0x%03X", pEventArg_p->m_InternalError.m_EventSource, pEventArg_p->m_InternalError.m_EplError);
+			// check additional argument
+			switch (pEventArg_p->m_Node.m_NodeEvent) {
+			case kEplNmtNodeEventCheckConf:
+				{
+					tEplSdoComConHdl SdoComConHdl;
+					// update object 0x1006 on CN
+					EplRet =
+					    EplApiWriteObject(&SdoComConHdl,
+							      pEventArg_p->
+							      m_Node.m_uiNodeId,
+							      0x1006, 0x00,
+							      &dw_le_CycleLen_g,
+							      4,
+							      kEplSdoTypeAsnd,
+							      NULL);
+					if (EplRet == kEplApiTaskDeferred) {	// SDO transfer started
+						EplRet = kEplReject;
+					} else if (EplRet == kEplSuccessful) {	// local OD access (should not occur)
+						printk
+						    ("AppCbEvent(Node) write to local OD\n");
+					} else {	// error occured
+						TGT_DBG_SIGNAL_TRACE_POINT(1);
+
+						EplRet =
+						    EplApiFreeSdoChannel
+						    (SdoComConHdl);
+						SdoComConHdl = 0;
+
+						EplRet =
+						    EplApiWriteObject
+						    (&SdoComConHdl,
+						     pEventArg_p->m_Node.
+						     m_uiNodeId, 0x1006, 0x00,
+						     &dw_le_CycleLen_g, 4,
+						     kEplSdoTypeAsnd, NULL);
+						if (EplRet == kEplApiTaskDeferred) {	// SDO transfer started
+							EplRet = kEplReject;
+						} else {
+							printk
+							    ("AppCbEvent(Node): EplApiWriteObject() returned 0x%02X\n",
+							     EplRet);
+						}
+					}
+
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+			}
+			break;
+		}
+
+	case kEplApiEventSdo:
+		{		// SDO transfer finished
+			EplRet =
+			    EplApiFreeSdoChannel(pEventArg_p->m_Sdo.
+						 m_SdoComConHdl);
+			if (EplRet != kEplSuccessful) {
+				break;
+			}
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			if (pEventArg_p->m_Sdo.m_SdoComConState == kEplSdoComTransferFinished) {	// continue boot-up of CN with NMT command Reset Configuration
+				EplRet =
+				    EplApiMnTriggerStateChange(pEventArg_p->
+							       m_Sdo.m_uiNodeId,
+							       kEplNmtNodeCommandConfReset);
+			} else {	// indicate configuration error CN
+				EplRet =
+				    EplApiMnTriggerStateChange(pEventArg_p->
+							       m_Sdo.m_uiNodeId,
+							       kEplNmtNodeCommandConfErr);
+			}
+#endif
+
+			break;
+		}
+
+	default:
+		break;
+	}
+
+	return EplRet;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AppCbSync
+//
+// Description: sync event callback function called by event module within
+//              kernel part (high priority).
+//              This function sets the outputs, reads the inputs and runs
+//              the control loop.
+//
+// Parameters:  void
+//
+// Returns:     tEplKernel      = error code,
+//                                kEplSuccessful = no error
+//                                otherwise = post error event to API layer
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC AppCbSync(void)
+{
+	tEplKernel EplRet = kEplSuccessful;
+
+	if (bVarOut1Old_l != bVarOut1_l) {	// output variable has changed
+		bVarOut1Old_l = bVarOut1_l;
+		// set LEDs
+
+//        printk("bVarIn = 0x%02X bVarOut = 0x%02X\n", (WORD) bVarIn_l, (WORD) bVarOut_l);
+	}
+	if (uiNodeId_g != EPL_C_ADR_MN_DEF_NODE_ID) {
+		bVarIn1_l++;
+	}
+
+	if (uiNodeId_g == EPL_C_ADR_MN_DEF_NODE_ID) {	// we are the master and must run the control loop
+
+		// collect inputs from CNs and own input
+		bSpeedSelect_l = (bVarIn1_l | abSelect_l[0]) & 0x07;
+
+		bModeSelect_l = abSelect_l[1] | abSelect_l[2];
+
+		if ((bModeSelect_l & APP_MODE_MASK) != 0) {
+			dwMode_l = bModeSelect_l & APP_MODE_MASK;
+		}
+
+		iCurCycleCount_l--;
+
+		if (iCurCycleCount_l <= 0) {
+			if ((dwMode_l & 0x01) != 0) {	// fill-up
+				if (iToggle) {
+					if ((dwLeds_l & APP_DOUBLE_LED_MASK) ==
+					    0x00) {
+						dwLeds_l = 0x01;
+					} else {
+						dwLeds_l <<= 1;
+						dwLeds_l++;
+						if (dwLeds_l >=
+						    APP_DOUBLE_LED_MASK) {
+							iToggle = 0;
+						}
+					}
+				} else {
+					dwLeds_l <<= 1;
+					if ((dwLeds_l & APP_DOUBLE_LED_MASK) ==
+					    0x00) {
+						iToggle = 1;
+					}
+				}
+				bLedsRow1_l =
+				    (unsigned char)(dwLeds_l & APP_LED_MASK);
+				bLedsRow2_l =
+				    (unsigned char)((dwLeds_l >> APP_LED_COUNT)
+						    & APP_LED_MASK);
+			}
+
+			else if ((dwMode_l & 0x02) != 0) {	// running light forward
+				dwLeds_l <<= 1;
+				if ((dwLeds_l > APP_DOUBLE_LED_MASK)
+				    || (dwLeds_l == 0x00000000L)) {
+					dwLeds_l = 0x01;
+				}
+				bLedsRow1_l =
+				    (unsigned char)(dwLeds_l & APP_LED_MASK);
+				bLedsRow2_l =
+				    (unsigned char)((dwLeds_l >> APP_LED_COUNT)
+						    & APP_LED_MASK);
+			}
+
+			else if ((dwMode_l & 0x04) != 0) {	// running light backward
+				dwLeds_l >>= 1;
+				if ((dwLeds_l > APP_DOUBLE_LED_MASK)
+				    || (dwLeds_l == 0x00000000L)) {
+					dwLeds_l = 1 << (APP_LED_COUNT * 2);
+				}
+				bLedsRow1_l =
+				    (unsigned char)(dwLeds_l & APP_LED_MASK);
+				bLedsRow2_l =
+				    (unsigned char)((dwLeds_l >> APP_LED_COUNT)
+						    & APP_LED_MASK);
+			}
+
+			else if ((dwMode_l & 0x08) != 0) {	// Knightrider
+				if (bLedsRow1_l == 0x00) {
+					bLedsRow1_l = 0x01;
+					iToggle = 1;
+				} else if (iToggle) {
+					bLedsRow1_l <<= 1;
+					if (bLedsRow1_l >=
+					    (1 << (APP_LED_COUNT - 1))) {
+						iToggle = 0;
+					}
+				} else {
+					bLedsRow1_l >>= 1;
+					if (bLedsRow1_l <= 0x01) {
+						iToggle = 1;
+					}
+				}
+				bLedsRow2_l = bLedsRow1_l;
+			}
+
+			else if ((dwMode_l & 0x10) != 0) {	// Knightrider
+				if ((bLedsRow1_l == 0x00)
+				    || (bLedsRow2_l == 0x00)
+				    || ((bLedsRow2_l & ~APP_LED_MASK) != 0)) {
+					bLedsRow1_l = 0x01;
+					bLedsRow2_l =
+					    (1 << (APP_LED_COUNT - 1));
+					iToggle = 1;
+				} else if (iToggle) {
+					bLedsRow1_l <<= 1;
+					bLedsRow2_l >>= 1;
+					if (bLedsRow1_l >=
+					    (1 << (APP_LED_COUNT - 1))) {
+						iToggle = 0;
+					}
+				} else {
+					bLedsRow1_l >>= 1;
+					bLedsRow2_l <<= 1;
+					if (bLedsRow1_l <= 0x01) {
+						iToggle = 1;
+					}
+				}
+			}
+			// set own output
+			bVarOut1_l = bLedsRow1_l;
+//            bVarOut1_l = (bLedsRow1_l & 0x03) | (bLedsRow2_l << 2);
+
+			// restart cycle counter
+			iCurCycleCount_l = iMaxCycleCount_l;
+		}
+
+		if (bSpeedSelectOld_l == 0) {
+			if ((bSpeedSelect_l & 0x01) != 0) {
+				if (iMaxCycleCount_l < 200) {
+					iMaxCycleCount_l++;
+				}
+				bSpeedSelectOld_l = bSpeedSelect_l;
+			} else if ((bSpeedSelect_l & 0x02) != 0) {
+				if (iMaxCycleCount_l > 1) {
+					iMaxCycleCount_l--;
+				}
+				bSpeedSelectOld_l = bSpeedSelect_l;
+			} else if ((bSpeedSelect_l & 0x04) != 0) {
+				iMaxCycleCount_l = DEFAULT_MAX_CYCLE_COUNT;
+				bSpeedSelectOld_l = bSpeedSelect_l;
+			}
+		} else if (bSpeedSelect_l == 0) {
+			bSpeedSelectOld_l = 0;
+		}
+	}
+
+	TGT_DBG_SIGNAL_TRACE_POINT(1);
+
+	return EplRet;
+}
+
+// EOF
diff --git a/drivers/staging/epl/edrv.h b/drivers/staging/epl/edrv.h
new file mode 100644
index 000000000000..a45984dfb092
--- /dev/null
+++ b/drivers/staging/epl/edrv.h
@@ -0,0 +1,167 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  interface for ethernetdriver
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: edrv.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                Dev C++ and GNU-Compiler for m68k
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2005/08/01 m.b.:   start of implementation
+
+****************************************************************************/
+
+#ifndef _EDRV_H_
+#define _EDRV_H_
+
+#include "EplInc.h"
+#include "EplFrame.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+// --------------------------------------------------------------------------
+#define MAX_ETH_DATA_SIZE       1500
+#define MIN_ETH_DATA_SIZE         46
+
+#define ETH_HDR_OFFSET 	 0	// Ethernet header at the top of the frame
+#define ETH_HDR_SIZE	14	// size of Ethernet header
+#define MIN_ETH_SIZE     (MIN_ETH_DATA_SIZE + ETH_HDR_SIZE)	// without CRC
+
+#define ETH_CRC_SIZE	 4	// size of Ethernet CRC, i.e. FCS
+
+//---------------------------------------------------------------------------
+// types
+//---------------------------------------------------------------------------
+
+// position of a buffer in an ethernet-frame
+typedef enum {
+	kEdrvBufferFirstInFrame = 0x01,	// first data buffer in an ethernet frame
+	kEdrvBufferMiddleInFrame = 0x02,	// a middle data buffer in an ethernet frame
+	kEdrvBufferLastInFrame = 0x04	// last data buffer in an ethernet frame
+} tEdrvBufferInFrame;
+
+// format of a tx-buffer
+typedef struct _tEdrvTxBuffer {
+	tEplMsgType m_EplMsgType;	// IN: type of EPL message, set by calling function
+	unsigned int m_uiTxMsgLen;	// IN: length of message to be send (set for each transmit call)
+	// ----------------------
+	unsigned int m_uiBufferNumber;	// OUT: number of the buffer, set by ethernetdriver
+	BYTE *m_pbBuffer;	// OUT: pointer to the buffer, set by ethernetdriver
+	tEplNetTime m_NetTime;	// OUT: Timestamp of end of transmission, set by ethernetdriver
+	// ----------------------
+	unsigned int m_uiMaxBufferLen;	// IN/OUT: maximum length of the buffer
+} tEdrvTxBuffer;
+
+// format of a rx-buffer
+typedef struct _tEdrvRxBuffer {
+	tEdrvBufferInFrame m_BufferInFrame;	// OUT position of received buffer in an ethernet-frame
+	unsigned int m_uiRxMsgLen;	// OUT: length of received buffer (without CRC)
+	BYTE *m_pbBuffer;	// OUT: pointer to the buffer, set by ethernetdriver
+	tEplNetTime m_NetTime;	// OUT: Timestamp of end of receiption
+
+} tEdrvRxBuffer;
+
+//typedef void (*tEdrvRxHandler) (BYTE bBufferInFrame_p, tBufferDescr * pbBuffer_p);
+//typedef void (*tEdrvRxHandler) (BYTE bBufferInFrame_p, BYTE * pbEthernetData_p, WORD wDataLen_p);
+typedef void (*tEdrvRxHandler) (tEdrvRxBuffer * pRxBuffer_p);
+typedef void (*tEdrvTxHandler) (tEdrvTxBuffer * pTxBuffer_p);
+
+// format of init structure
+typedef struct {
+	BYTE m_abMyMacAddr[6];	// the own MAC address
+
+//    BYTE            m_bNoOfRxBuffDescr;     // number of entries in rx bufferdescriptor table
+//    tBufferDescr *  m_pRxBuffDescrTable;    // rx bufferdescriptor table
+//    WORD            m_wRxBufferSize;        // size of the whole rx buffer
+
+	tEdrvRxHandler m_pfnRxHandler;
+	tEdrvTxHandler m_pfnTxHandler;
+
+} tEdrvInitParam;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel EdrvInit(tEdrvInitParam * pEdrvInitParam_p);
+
+tEplKernel EdrvShutdown(void);
+
+tEplKernel EdrvDefineRxMacAddrEntry(BYTE * pbMacAddr_p);
+tEplKernel EdrvUndefineRxMacAddrEntry(BYTE * pbMacAddr_p);
+
+//tEplKernel EdrvDefineUnicastEntry     (BYTE * pbUCEntry_p);
+//tEplKernel EdrvUndfineUnicastEntry    (BYTE * pbUCEntry_p);
+
+tEplKernel EdrvAllocTxMsgBuffer(tEdrvTxBuffer * pBuffer_p);
+tEplKernel EdrvReleaseTxMsgBuffer(tEdrvTxBuffer * pBuffer_p);
+
+//tEplKernel EdrvWriteMsg               (tBufferDescr * pbBuffer_p);
+tEplKernel EdrvSendTxMsg(tEdrvTxBuffer * pBuffer_p);
+tEplKernel EdrvTxMsgReady(tEdrvTxBuffer * pBuffer_p);
+tEplKernel EdrvTxMsgStart(tEdrvTxBuffer * pBuffer_p);
+
+//tEplKernel EdrvReadMsg                (void);
+
+// interrupt handler called by target specific interrupt handler
+void EdrvInterruptHandler(void);
+
+#endif // #ifndef _EDRV_H_
diff --git a/drivers/staging/epl/global.h b/drivers/staging/epl/global.h
new file mode 100644
index 000000000000..fe167165a836
--- /dev/null
+++ b/drivers/staging/epl/global.h
@@ -0,0 +1,1391 @@
+/****************************************************************************
+
+    global project definition file
+
+    12.06.1998   -rs
+    11.02.2002   r.d. Erweiterungen, Ergaenzungen
+    20.08.2002   SYS TEC electronic -as
+                 Definition Schluesselwort 'GENERIC'
+                 fuer das Erzeugen von Generic Pointer
+    28.08.2002   r.d. erweiterter SYS TEC Debug Code
+    16.09.2002   r.d. komplette Uebersetzung in Englisch
+    11.04.2003   f.j. Ergaenzung fuer Mitsubishi NC30 Compiler
+    17.06.2003   -rs  Definition von Basistypen in <#ifndef _WINDEF_> gesetzt
+    16.04.2004   r.d. Ergaenzung fuer Borland C++ Builder
+    30.08.2004   -rs  TRACE5 eingefügt
+    23.12.2005   d.k. Definitions for IAR compiler
+
+    $Id: global.h,v 1.6 2008/11/07 13:55:56 D.Krueger Exp $
+
+****************************************************************************/
+
+#ifndef _GLOBAL_H_
+#define _GLOBAL_H_
+
+//---------------------------------------------------------------------------
+//  elements of defines for development system
+//---------------------------------------------------------------------------
+
+// these defines are necessary to check some of characteristics of the development system
+#define _DEV_BIGEND_            0x80000000L	// big endian (motorolla format)
+#define _DEV_ALIGNMENT_4_       0x00400000L	//                  the CPU needs alignment of 4 bytes
+#define _DEV_ONLY_INT_MAIN_     0x00004000L	//                  the compiler needs "int main(int)" instead of "void main(void)"
+#define _DEV_COMMA_EXT_         0x00002000L	//                  support of last comma in struct predefinition
+#define _DEV_64BIT_SUPPORT_     0x00001000L	//                  support of 64 bit operations
+#define _DEV_BIT64_             0x00000400L	// count of bits:   64 bit
+#define _DEV_BIT32_             0x00000300L	//                  32 bit
+#define _DEV_BIT16_             0x00000200L	//                  16 bit
+#define _DEV_BIT8_              0x00000100L	//                  8 bit
+#define _DEV_RVCT_ARM_          0x0000001CL	//                  RealView ARM
+#define _DEV_RENESASM32C        0x0000001BL	// compiler from:   Renesas
+#define _DEV_GNUC_MIPS2_        0x0000001AL	//                  GNU for MIPS2
+#define _DEV_MPLAB_C30_         0x00000019L	//                  MPLAB C30 for Microchip dsPIC33F series
+#define _DEV_GNUC_TC_           0x00000018L	//                  GNU for Infineon TriCore
+#define _DEV_GNUC_X86_          0x00000017L	//                  GNU for I386
+#define _DEV_IAR_ARM_           0x00000016L	//                  ARM IAR C/C++ Compiler
+#define _DEV_PARADGM_X86        0x00000015L	//                  Paradigm C/C++ for Beck 1x3
+#define _DEV_GNUC_CF_           0x00000014L	//                  GNU for Coldfire
+#define _DEV_KEIL_ARM_          0x00000013L	//                  Keil ARM
+#define _DEV_MSEVC_             0x00000012L	//                  Microsoft embedded Visual C/C++
+#define _DEV_HIGHTEC_GNUC_X86_  0x00000011L	//                  Hightec elf386 gcc
+#define _DEV_MSVC_RTX_          0x00000010L	//                  VC600 + RTX
+#define _DEV_MSVC_V1_5_         0x0000000FL	//                  Microsoft Visual C/C++ V1.5
+#define _DEV_GNUC_ARM7_         0x0000000EL	//                  GNU Compiler gcc for ARM7
+#define _DEV_METROWERKS_CW_     0x0000000DL	//                  Metrowerks Code Warrior
+#define _DEV_MITSUBISHIM16C_    0x0000000CL	//compiler from:    Mitsubishi
+#define _DEV_GNUC_C16X_         0x0000000BL	//                  GNU Compiler gcc166 for Infineon C16x
+#define _DEV_LINUX_GCC_         0x0000000AL	//                  Linux GNU Compiler gcc
+#define _DEV_GNUC_MPC5X5        0x00000009L	//                  GNU for Motorola PPC5x5
+#define _DEV_TASKINGM16C_       0x00000008L	//                  Tasking for Mitsubishi M16C
+#define _DEV_FUJITSU_           0x00000007L	//                  Fujitsu
+#define _DEV_TASKING8_          0x00000006L	//                  Tasking 8051
+#define _DEV_TASKING16_         0x00000005L	//                  Tasking 166
+#define _DEV_KEIL8_             0x00000004L	//                  Keil C51
+#define _DEV_KEIL16_            0x00000003L	//                  Keil C166
+#define _DEV_BORLANDC_          0x00000002L	//                  Borland C/C++
+#define _DEV_MSVC16_            0x00000001L	//                  Microsoft Visual C/C++
+#define _DEV_MSVC32_            0x00000000L	//                  Microsoft Visual C/C++
+
+// these defines can be used to mask previous elements
+#define _DEV_MASK_COMPILER      0x000000FFL
+#define _DEV_MASK_BITCOUNT      0x00000F00L
+#define _DEV_MASK_ADDSUPPORT    0x0000F000L
+#define _DEV_MASK_ALIGNMENT     0x00F00000L
+
+//---------------------------------------------------------------------------
+//  defines for development system (DEV_SYSTEM) including previous elements
+//---------------------------------------------------------------------------
+
+#define _DEV_WIN16_             (_DEV_BIT16_ | _DEV_MSVC16_                  )
+#define _DEV_WIN32_             (_DEV_BIT32_ | _DEV_MSVC32_                   | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_MSVC_DOS_          (_DEV_BIT32_ | _DEV_MSVC_V1_5_               )
+#define _DEV_BORLAND_DOS_       (_DEV_BIT32_ | _DEV_BORLANDC_                )	//| _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_KEIL_C51X_         (_DEV_BIT8_  | _DEV_KEIL8_     | _DEV_BIGEND_ | _DEV_COMMA_EXT_)	// at least C51 version 7.05 supports comma extension
+#define _DEV_KEIL_C16X_         (_DEV_BIT16_ | _DEV_KEIL16_                   | _DEV_COMMA_EXT_)	// at least C166 version 5.03 supports comma extension
+#define _DEV_TASKING_C51X_      (_DEV_BIT8_  | _DEV_TASKING8_  | _DEV_BIGEND_)
+#define _DEV_TASKING_C16X_      (_DEV_BIT16_ | _DEV_TASKING16_               )
+#define _DEV_FUJITSU_F590_      (_DEV_BIT8_  | _DEV_FUJITSU_                  | _DEV_COMMA_EXT_)	// softune is not able to support 64 bit variables QWORD !!!
+//f.j.29.04.03 M16C kann effektiv mit Bytes umgehen
+//#define _DEV_TASKING_M16C_      (_DEV_BIT16_ | _DEV_TASKINGM16C_             )
+#define _DEV_TASKING_M16C_      (_DEV_BIT8_  | _DEV_TASKINGM16C_             )
+#define _DEV_MITSUBISHI_M16C_   (_DEV_BIT8_  | _DEV_MITSUBISHIM16C_          )
+#define _DEV_GNU_MPC5X5_        (_DEV_BIT32_ | _DEV_GNUC_MPC5X5| _DEV_BIGEND_ | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_LINUX_             (_DEV_BIT32_ | _DEV_LINUX_GCC_                | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_GNU_C16X_          (_DEV_BIT16_ | _DEV_GNUC_C16X_               )	//| _DEV_COMMA_EXT_)
+#define _DEV_MCW_MPC5X5_        (_DEV_BIT32_ | _DEV_METROWERKS_CW_           )	//| _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_GNU_ARM7_          (_DEV_BIT32_ | _DEV_GNUC_ARM7_                | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_ | _DEV_ONLY_INT_MAIN_)
+#define _DEV_WIN32_RTX_         (_DEV_BIT32_ | _DEV_MSVC_RTX_                )	//| _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_HIGHTEC_X86_       (_DEV_BIT32_ | _DEV_HIGHTEC_GNUC_X86_        )	//| _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_WIN_CE_            (_DEV_BIT32_ | _DEV_MSEVC_                   )	//| _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_KEIL_CARM_         (_DEV_BIT32_ | _DEV_KEIL_ARM_                 | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_IAR_CARM_          (_DEV_BIT32_ | _DEV_IAR_ARM_                  | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_RVCT_CARM_         (_DEV_BIT32_ | _DEV_RVCT_ARM_                 | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_ | _DEV_ONLY_INT_MAIN_)
+#define _DEV_MCW_MCF5XXX_       (_DEV_BIT32_ | _DEV_METROWERKS_CW_           )	//| _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_GNU_CF5282_        (_DEV_BIT32_ | _DEV_GNUC_CF_   | _DEV_BIGEND_)
+#define _DEV_PAR_BECK1X3_       (_DEV_BIT16_ | _DEV_PARADGM_X86)
+#define _DEV_GNU_CF548X_        (_DEV_BIT32_ | _DEV_GNUC_CF_   | _DEV_BIGEND_ | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_GNU_I386_          (_DEV_BIT32_ | _DEV_GNUC_X86_                 | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_ | _DEV_ONLY_INT_MAIN_)
+#define _DEV_GNU_TRICORE_       (_DEV_BIT32_ | _DEV_GNUC_TC_                  | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_ | _DEV_ONLY_INT_MAIN_ | _DEV_ALIGNMENT_4_)
+#define _DEV_MPLAB_DSPIC33F_    (_DEV_BIT16_ | _DEV_MPLAB_C30_               )	//| _DEV_COMMA_EXT_)
+#define _DEV_GNU_MIPSEL_        (_DEV_BIT32_ | _DEV_GNUC_MIPS2_     | _DEV_BIGEND_ | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_ | _DEV_ONLY_INT_MAIN_)
+
+#define _DEV_RENESAS_M32C_      (_DEV_BIT32_ | _DEV_RENESASM32C)
+
+//---------------------------------------------------------------------------
+//  usefull macros
+//---------------------------------------------------------------------------
+
+#define CHECK_IF_ONLY_INT_MAIN()    (DEV_SYSTEM & _DEV_ONLY_INT_MAIN_)
+#define CHECK_MEMORY_ALINMENT()     (DEV_SYSTEM & _DEV_MASK_ALIGNMENT)
+
+//---------------------------------------------------------------------------
+//  defines for target system (TARGET_SYSTEM)
+//---------------------------------------------------------------------------
+
+#define _DOS_              (16 + 0x10000)
+#define _WIN16_             16
+#define _WIN32_             32
+#define _WINCE_            (32 + 0x20000)
+#define _NO_OS_              0
+#define _LINUX_              1
+#define _PXROS_              2
+#define _ECOSPRO_            3
+
+//---------------------------------------------------------------------------
+//  definitions for function inlining
+//---------------------------------------------------------------------------
+
+#define INLINE_FUNCTION		// empty define
+#undef  INLINE_ENABLED		// disable actual inlining of functions
+#undef  INLINE_FUNCTION_DEF	// disable inlining for all compilers per default
+
+//---------------------------------------------------------------------------
+//  definitions for Keil C51
+//---------------------------------------------------------------------------
+
+#ifdef  __C51__
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_KEIL_C51X_
+
+#pragma DEBUG OBJECTEXTEND
+#pragma WARNINGLEVEL(2)		// maximum warning level
+
+#define NEAR            idata	// variables mapped to internal data storage location
+#define FAR             xdata	// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM             code	// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC           xdata	// hardware access through external memory (i.e. CAN)
+#define LARGE           large	// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM             xdata	// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT       reentrant
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for GNU Compiler for Infineon C16x
+//  - it have to be befor Keil (it has __C166__ too)
+//---------------------------------------------------------------------------
+#elif  defined (__GNUC__) && defined (__C166__)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_GNU_C16X_
+
+//    #define NEAR            idata       // variables mapped to internal data storage location
+#define NEAR            near	// variables mapped to internal data storage location
+//    #define FAR             xhuge       // variables mapped to external data storage location
+#define FAR             huge	// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+//    #define HWACC           sdata       // hardware access through external memory (i.e. CAN)
+#define HWACC           huge	// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+//    #define GENERIC         xhuge       // generic pointer to point to application data
+#define GENERIC         huge	// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+
+#define ASSERT(p)  \
+            if (p)         \
+            {              \
+                ;          \
+            }              \
+            else           \
+            {              \
+                PRINTF0("Assert failed: " #p " (file %s line %d)\n", __FILE__, (int) __LINE__ ); \
+                while (1); \
+            }
+#else
+#define ASSERT(p)
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Keil C166
+//---------------------------------------------------------------------------
+#elif  defined (__C166__)	// 24.01.2005 r.d.: Keil ARM7 needs directive 'defined'
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_KEIL_C16X_
+
+#pragma CODE
+#pragma MOD167
+#pragma NOINIT
+#pragma DEBUG
+#pragma WARNINGLEVEL(3)		// maximum warning level
+#pragma WARNING DISABLE = 47	// warning <unreferenced parameter> = OFF
+#pragma WARNING DISABLE = 38	// warning <empty translation unit> = OFF
+//  #pragma WARNING DISABLE = 102       // warning <different const/volatile qualifiers> = OFF
+#pragma WARNING DISABLE = 174	// warning <unreferenced 'static' function> = OFF
+#pragma WARNING DISABLE = 183	// warning <dead assignement eliminated> = OFF
+
+#define NEAR            idata	// variables mapped to internal data storage location
+#define FAR             xhuge	// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+//    #define HWACC           sdata       // hardware access through external memory (i.e. CAN)
+#define HWACC           huge	// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC         xhuge	// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for MPLAB C30 for dsPIC33F series
+//---------------------------------------------------------------------------
+#elif  defined (__C30__)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_MPLAB_DSPIC33F_
+
+#define NEAR			// variables mapped to internal data storage location
+#define FAR			// variables mapped to external data storage location
+#define CONST        const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+//    #ifndef QWORD
+//        #define QWORD long long
+//    #endif
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Keil ARM
+//---------------------------------------------------------------------------
+#elif  defined (__CA__)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_KEIL_CARM_
+
+#define NEAR			// variables mapped to internal data storage location
+#define FAR			// variables mapped to external data storage location
+#define CONST        const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef QWORD
+#define QWORD long long
+#endif
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for RealView ARM compilation tools (provided by recent Keil Microcontroller Development Kits)
+//---------------------------------------------------------------------------
+#elif  defined (__ARMCC_VERSION)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_RVCT_CARM_
+
+#define NEAR			// variables mapped to internal data storage location
+#define FAR			// variables mapped to external data storage location
+#define CONST        const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef QWORD
+#define QWORD long long
+#endif
+
+#ifndef NDEBUG
+#define ASSERT(expr)    if (!(expr)) {\
+                                   TRACE0 ("Assertion failed: " #expr );\
+                                   while (1);}
+#else
+#define ASSERT(expr)
+#endif
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for ARM IAR C Compiler
+//---------------------------------------------------------------------------
+#elif  defined (__ICCARM__)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_IAR_CARM_
+
+#define NEAR			// variables mapped to internal data storage location
+#define FAR			// variables mapped to external data storage location
+#define CONST        const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef QWORD
+#define QWORD long long
+#endif
+
+    // Workaround:
+    // If we use IAR and want to debug but don't want to use C-Spy Debugger
+    // assert() doesn't work in debug mode because it needs support for FILE descriptors
+    // (_DLIB_FILE_DESCRIPTOR == 1).
+#ifndef NDEBUG
+#define ASSERT(expr)    if (!(expr)) {\
+                                   TRACE0 ("Assertion failed: " #expr );\
+                                   while (1);}
+#else
+#define ASSERT(expr)
+#endif
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+//        #define TRACE  PRINTF4
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Tasking 8051
+//---------------------------------------------------------------------------
+
+#elif defined (_CC51)
+
+#include <cc51.h>
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_TASKING_C51X_
+
+#define NEAR            _data	// variables mapped to internal data storage location
+#define FAR             _xdat	// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC           _xdat	// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM             _xdat	// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT       _reentrant
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Tasking C167CR and C164CI
+//---------------------------------------------------------------------------
+
+#elif defined (_C166)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_TASKING_C16X_
+
+#define NEAR            near	// variables mapped to internal data storage location
+#define FAR             far	// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC   /* to be defined */	// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+    // Stdio.h has to be alway included here. If printf() is used stdio.h defines NULL
+    // without checking if it is already included. So an error occurs while compiling.
+    // (r.d.)
+#include <stdio.h>		// prototype printf() (for TRACE)
+#ifndef NDEBUG
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for FUJITSU FFMC-16LX MB90590
+//---------------------------------------------------------------------------
+
+//#elif (defined (F590) || defined (F543) || defined (F598) || defined (F495) || defined (F350))
+#elif defined(__COMPILER_FCC907__)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_FUJITSU_F590_
+
+#define NEAR    /* to be defined */	// variables mapped to internal data storage location
+#define FAR     /* to be defined */	// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM     /* to be defined */	// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC   /* to be defined */	// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+    // softune is not able to support 64 bit variables QWORD !!!
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Mitsubishi M16C family for TASKING Compiler CM16
+//---------------------------------------------------------------------------
+
+#elif defined (_CM16C)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_TASKING_M16C_
+
+#define NEAR            _near	// variables mapped to internal data storage location
+#define FAR             _far	// variables mapped to external data storage location
+#define CONST           _farrom	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC           _near	// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC         _far	// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+					// do you use memory model SMALL, than you have to set _far
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+    // Stdio.h has to be alway included here. If printf() is used stdio.h defines NULL
+    // without checking if it is already included. So an error occurs while compiling.
+    // (r.d.)
+#include <stdio.h>		// prototype printf() (for TRACE)
+#ifndef NDEBUG
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Mitsubishi M16C family for Mitsubishi Compiler NC30
+//---------------------------------------------------------------------------
+// name NC30, andere Form will der Compiler nicht !!
+#elif defined (NC30)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_MITSUBISHI_M16C_
+
+#define NEAR            near	// variables mapped to internal data storage location
+#define FAR             far	// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC           near	// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC         far	// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Renesas M32C family for Renesas Compiler
+//---------------------------------------------------------------------------
+#elif defined (NC308)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_RENESAS_M32C_
+
+#define NEAR             near	// variables mapped to internal data storage location
+#define FAR              far	// variables mapped to external data storage location
+#define CONST            const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+#define HWACC			// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM              far	// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//    #error ("RENESAS o.k.")
+
+//---------------------------------------------------------------------------
+//  definitions for ARM7 family with GNU compiler
+//---------------------------------------------------------------------------
+
+#elif defined(__GNUC__) && defined(__arm__) && !defined(__LINUX_ARM_ARCH__)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_GNU_ARM7_
+
+#define NEAR			// variables mapped to internal data storage location
+#define FAR			// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef QWORD
+#define QWORD long long		// i.A. durch Herr Kuschel
+#endif
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Motorola PowerPC family 5x5 (555/565)
+//  definitions Linux-PC
+//---------------------------------------------------------------------------
+
+#elif defined (__GNUC__)
+
+#if defined (LINUX) || defined (linux) || defined (__linux__)
+#define LINUX_SYSTEM		// define 'LINUX_SYSTEM' uniform for all Linux based systems
+	// r.d.: We will need an other solution here! There are two sections here which do check the preproc-definitions:
+	//     LINUX and __linux__ . The first one was Linux for PC, the second one is this section for embedded Linux (MCF5xxx).
+	//     But Linux for PC does not need the definitions for embedded Linux.
+#endif
+
+    // GNU C compiler supports function inlining
+#define INLINE_FUNCTION_DEF extern inline
+
+    // to actually enable inlining just include the following three lines
+    // #undef INLINE_FUNCTION
+    // #define INLINE_FUNCTION     INLINE_FUNCTION_DEF
+    // #define INLINE_ENABLED      TRUE
+
+#ifdef PXROS
+#define TARGET_SYSTEM       _PXROS_
+#ifdef __i386__
+#undef LINUX			// this define seems to be set from compiler
+#define DEV_SYSTEM      _DEV_HIGHTEC_X86_
+#elif defined (__tricore__)
+#define DEV_SYSTEM      _DEV_GNU_TRICORE_
+#else // MPC5x5
+#define DEV_SYSTEM      _DEV_GNU_MPC5X5_
+#endif
+
+#elif defined (LINUX) || defined (__linux__)
+#define TARGET_SYSTEM       _LINUX_	// Linux definition
+#define DEV_SYSTEM          _DEV_LINUX_
+
+#elif defined (GNU_CF5282)
+#define TARGET_SYSTEM       _NO_OS_
+#define DEV_SYSTEM          _DEV_GNU_CF5282_
+
+#elif defined (ECOSPRO_I386_PEAK_PCI)
+#define TARGET_SYSTEM       _ECOSPRO_
+#define DEV_SYSTEM          _DEV_GNU_I386_
+
+#elif defined (GNU_CF548X)
+#define TARGET_SYSTEM       _NO_OS_
+#define DEV_SYSTEM          _DEV_GNU_CF548X_
+#else
+#error 'ERROR: DEV_SYSTEM not found!'
+#endif
+
+#ifndef QWORD
+#define QWORD long long int
+#endif
+
+#if (TARGET_SYSTEM == _PXROS_)
+
+#ifndef __KERNEL__
+#include <string.h>
+#endif
+
+#define NEAR			// variables mapped to internal data storage location
+#define FAR			// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM     /* to be defined */	// code or variables mapped to ROM (i.e. flash)
+					    // usage: CONST BYTE ROM foo = 0x00;
+#define LARGE			// functions set parameters to external data storage location
+
+	// These types can be adjusted by users to match application requirements. The goal is to
+	// minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					    // Variables with this attribute can be located in external
+					    // or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef QWORD
+#define QWORD long long int
+#endif
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+#endif
+
+    // ------------------ GNUC for I386 ---------------------------------------------
+
+#if (TARGET_SYSTEM == _LINUX_) || (TARGET_SYSTEM == _ECOSPRO_)
+
+#ifndef __KERNEL__
+#include <string.h>
+#endif
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+	// These types can be adjusted by users to match application requirements. The goal is to
+	// minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#ifndef NEAR
+#define NEAR			// variables mapped to internal data storage location
+#endif
+
+#ifndef FAR
+#define FAR			// variables mapped to external data storage location
+#endif
+
+#ifndef CONST
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#endif
+
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#ifndef __KERNEL__
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#else
+#define TRACE  printk
+#endif
+#endif
+#endif
+
+    // ------------------ GNU without OS ---------------------------------------------
+
+#if (TARGET_SYSTEM == _NO_OS_)
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+	// These types can be adjusted by users to match application requirements. The goal is to
+	// minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#ifndef NEAR
+#define NEAR			// variables mapped to internal data storage location
+#endif
+
+#ifndef FAR
+#define FAR			// variables mapped to external data storage location
+#endif
+
+#ifndef CONST
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#endif
+
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+//            #include "xuartdrv.h"
+//            #include <stdio.h>              // prototype printf() (for TRACE)
+#define TRACE  printf
+//            #define TRACE  mprintf
+//            #ifndef TRACE
+//                #define TRACE trace
+//                void trace (char *fmt, ...);
+//            #endif
+#endif
+
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for MPC565
+//---------------------------------------------------------------------------
+#elif __MWERKS__
+
+#ifdef __MC68K__
+
+#define TARGET_SYSTEM = _MCF548X_
+#define DEV_SYSTEM      _DEV_MCW_MCF5XXX_
+
+#else
+#define TARGET_SYSTEM = _MPC565_
+#define DEV_SYSTEM      _DEV_MCW_MPC5X5_
+#endif
+
+#define NEAR			// variables mapped to internal data storage location
+#define FAR			// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for BECK 1x3
+//---------------------------------------------------------------------------
+#elif defined (__BORLANDC__) && defined (__PARADIGM__)
+
+#define TARGET_SYSTEM      _NO_OS_
+#define DEV_SYSTEM         _DEV_PAR_BECK1X3_
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+				     // usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+     // These types can be adjusted by users to match application requirements. The goal is to
+     // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+				     // Variables with this attribute can be located in external
+				     // or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+#define NEAR __near		// variables mapped to internal data storage location
+#define FAR  __far		// variables mapped to external data storage location
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#ifndef TRACE
+#include <stdio.h>
+#define TRACE printf
+#endif
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for PC
+//---------------------------------------------------------------------------
+
+#elif defined (__BORLANDC__)
+
+    // ------------------ definition target system --------------------------
+
+#ifdef _WIN32
+#define TARGET_SYSTEM   _WIN32_	// WIN32 definition
+#define DEV_SYSTEM      _DEV_WIN32_
+#else
+#define TARGET_SYSTEM   _DOS_
+#define DEV_SYSTEM      _DEV_BORLAND_DOS_
+#endif
+
+    // ------------------ WIN32 ---------------------------------------------
+
+#if (TARGET_SYSTEM == _WIN32_)
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+	// These types can be adjusted by users to match application requirements. The goal is to
+	// minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#ifndef NEAR
+#define NEAR			// variables mapped to internal data storage location
+#endif
+
+#ifndef FAR
+#define FAR			// variables mapped to external data storage location
+#endif
+
+#ifndef CONST
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#endif
+
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC __stdcall
+
+#ifndef NDEBUG
+#ifndef TRACE
+#include <stdio.h>
+#define TRACE printf
+#endif
+#endif
+
+#elif (TARGET_SYSTEM == _DOS_)
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+	// These types can be adjusted by users to match application requirements. The goal is to
+	// minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+#define NEAR near		// variables mapped to internal data storage location
+#define FAR  far		// variables mapped to external data storage location
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#ifndef TRACE
+#include <stdio.h>
+#define TRACE printf
+#endif
+#endif
+
+#endif
+
+#elif (_MSC_VER == 800)		// PC MS Visual C/C++ for DOS applications
+
+#define TARGET_SYSTEM   _DOS_
+#define DEV_SYSTEM      _DEV_MSVC_DOS_
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+				    // usage: CONST BYTE ROM foo = 0x00;
+#define HWACC near		// hardware access through external memory (i.e. CAN)
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+				    // Variables with this attribute can be located in external
+				    // or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+#define NEAR near		// variables mapped to internal data storage location
+#define FAR  far		// variables mapped to external data storage location
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#ifndef TRACE
+#include <stdio.h>
+#define TRACE printf
+#endif
+#endif
+
+//---------------------------------------------------------------------------
+// definitions for RTX under WIN32
+//---------------------------------------------------------------------------
+#elif (defined (UNDER_RTSS) && defined (WIN32))
+
+    // ------------------ definition target system --------------------------
+#define TARGET_SYSTEM   _WIN32_RTX_
+#define DEV_SYSTEM      _DEV_WIN32_RTX_
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+				    // usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+				    // Variables with this attribute can be located in external
+				    // or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#ifndef NEAR
+#define NEAR			// variables mapped to internal data storage location
+#endif
+
+#ifndef FAR
+#define FAR			// variables mapped to external data storage location
+#endif
+
+#ifndef CONST
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#endif
+
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC __stdcall
+
+#ifndef NDEBUG
+#ifndef TRACE
+#define TRACE RtPrintf
+#endif
+#endif
+
+//---------------------------------------------------------------------------
+// definitions for WinCE
+//---------------------------------------------------------------------------
+#elif defined (_WIN32_WCE)
+
+    // ------------------ definition target system --------------------------
+#define TARGET_SYSTEM           _WINCE_
+#define DEV_SYSTEM              _DEV_WIN_CE_
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+				    // usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+				    // Variables with this attribute can be located in external
+				    // or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#ifndef NEAR
+#define NEAR			// variables mapped to internal data storage location
+#endif
+
+#ifndef FAR
+#define FAR			// variables mapped to external data storage location
+#endif
+
+#ifndef CONST
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#endif
+
+#define LARGE
+
+#ifndef QWORD
+      //#define QWORD long long int // MSVC .NET can use "long long int" too (like GNU)
+#define QWORD __int64
+#endif
+
+#define REENTRANT
+#define PUBLIC __cdecl
+
+#ifdef ASSERTMSG
+#undef ASSERTMSG
+#endif
+
+#ifndef NDEBUG
+#ifndef TRACE
+#define TRACE printf
+//            void trace (char *fmt, ...);
+#endif
+#endif
+
+#else // ===> PC MS Visual C/C++
+
+    // ------------------ definition target system --------------------------
+
+#ifdef _WIN32
+#define TARGET_SYSTEM   _WIN32_	// WIN32 definition
+#define DEV_SYSTEM      _DEV_WIN32_
+#else
+#define TARGET_SYSTEM   _WIN16_	// WIN16 definition
+#define DEV_SYSTEM      _DEV_WIN16_
+#endif
+
+    // ------------------ WIN16 ---------------------------------------------
+
+#if (TARGET_SYSTEM == _WIN16_)
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+	// These types can be adjusted by users to match application requirements. The goal is to
+	// minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#ifndef NEAR
+#define NEAR			// variables mapped to internal data storage location
+#endif
+
+#ifndef FAR
+#define FAR far			// variables mapped to external data storage location
+#endif
+
+#ifndef CONST
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#endif
+
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC _far _pascal _export
+
+#ifndef NDEBUG
+#ifndef TRACE
+#define TRACE trace
+#ifdef __cplusplus
+extern "C" {
+#endif
+	void trace(const char *fmt, ...);
+#ifdef __cplusplus
+}
+#endif
+#endif
+#endif
+#endif
+    // ------------------ WIN32 ---------------------------------------------
+#if (TARGET_SYSTEM == _WIN32_)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+	// These types can be adjusted by users to match application requirements. The goal is to// minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+#ifndef NEAR
+#define NEAR			// variables mapped to internal data storage location
+#endif
+#ifndef FAR
+#define FAR			// variables mapped to external data storage location
+#endif
+#ifndef CONST
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#endif
+#define LARGE
+#define REENTRANT
+#define PUBLIC __stdcall
+#ifndef QWORD
+	  //#define QWORD long long int // MSVC .NET can use "long long int" too (like GNU)
+#define QWORD __int64
+#endif
+#ifndef NDEBUG
+#ifndef TRACE
+#define TRACE trace
+#ifdef __cplusplus
+extern "C" {
+#endif
+	void trace(const char *fmt, ...);
+#ifdef __cplusplus
+}
+#endif
+#endif
+#endif
+	// MS Visual C++ compiler supports function inlining
+#define INLINE_FUNCTION_DEF __forceinline
+	// to actually enable inlining just include the following two lines// #define INLINE_FUNCTION     INLINE_FUNCTION_DEF// #define INLINE_ENABLED      TRUE
+#endif
+#endif				// ===> PC
+//---------------------------------------------------------------------------//  definitions of basic types//---------------------------------------------------------------------------
+#ifndef _WINDEF_		// defined in WINDEF.H, included by <windows.h>
+    // --- arithmetic types ---
+#ifndef SHORT
+#define SHORT short int
+#endif
+#ifndef USHORT
+#define USHORT unsigned short int
+#endif
+#ifndef INT
+#define INT int
+#endif
+#ifndef UINT
+#define UINT unsigned int
+#endif
+#ifndef LONG
+#define LONG long int
+#endif
+#ifndef ULONG
+#define ULONG unsigned long int
+#endif
+    // --- logic types ---
+#ifndef BYTE
+#define BYTE unsigned char
+#endif
+#ifndef WORD
+#define WORD unsigned short int
+#endif
+#ifndef DWORD
+#define DWORD unsigned long int
+#endif
+#ifndef BOOL
+#define BOOL unsigned char
+#endif
+    // --- alias types ---
+#ifndef TRUE
+#define TRUE  0xFF
+#endif
+#ifndef FALSE
+#define FALSE 0x00
+#endif
+#ifndef NULL
+#define NULL ((void *) 0)
+#endif
+#endif
+#ifndef _TIME_OF_DAY_DEFINED_
+typedef struct {
+	unsigned long int m_dwMs;
+	unsigned short int m_wDays;
+
+} tTimeOfDay;
+
+#define _TIME_OF_DAY_DEFINED_
+
+#endif
+
+//---------------------------------------------------------------------------
+//  Definition von TRACE
+//---------------------------------------------------------------------------
+
+#ifndef NDEBUG
+
+#ifndef TRACE0
+#define TRACE0(p0)                      TRACE(p0)
+#endif
+
+#ifndef TRACE1
+#define TRACE1(p0, p1)                  TRACE(p0, p1)
+#endif
+
+#ifndef TRACE2
+#define TRACE2(p0, p1, p2)              TRACE(p0, p1, p2)
+#endif
+
+#ifndef TRACE3
+#define TRACE3(p0, p1, p2, p3)          TRACE(p0, p1, p2, p3)
+#endif
+
+#ifndef TRACE4
+#define TRACE4(p0, p1, p2, p3, p4)      TRACE(p0, p1, p2, p3, p4)
+#endif
+
+#ifndef TRACE5
+#define TRACE5(p0, p1, p2, p3, p4, p5)  TRACE(p0, p1, p2, p3, p4, p5)
+#endif
+
+#ifndef TRACE6
+#define TRACE6(p0, p1, p2, p3, p4, p5, p6)  TRACE(p0, p1, p2, p3, p4, p5, p6)
+#endif
+
+#else
+
+#ifndef TRACE0
+#define TRACE0(p0)
+#endif
+
+#ifndef TRACE1
+#define TRACE1(p0, p1)
+#endif
+
+#ifndef TRACE2
+#define TRACE2(p0, p1, p2)
+#endif
+
+#ifndef TRACE3
+#define TRACE3(p0, p1, p2, p3)
+#endif
+
+#ifndef TRACE4
+#define TRACE4(p0, p1, p2, p3, p4)
+#endif
+
+#ifndef TRACE5
+#define TRACE5(p0, p1, p2, p3, p4, p5)
+#endif
+
+#ifndef TRACE6
+#define TRACE6(p0, p1, p2, p3, p4, p5, p6)
+#endif
+
+#endif
+
+//---------------------------------------------------------------------------
+//  definition of ASSERT
+//---------------------------------------------------------------------------
+
+#ifndef ASSERT
+#if !defined (__linux__) && !defined (__KERNEL__)
+#include <assert.h>
+#ifndef ASSERT
+#define ASSERT(p)    assert(p)
+#endif
+#else
+#define ASSERT(p)
+#endif
+#endif
+
+//---------------------------------------------------------------------------
+//  SYS TEC extensions
+//---------------------------------------------------------------------------
+
+// This macro doesn't print out C-file and line number of the failed assertion
+// but a string, which exactly names the mistake.
+#ifndef NDEBUG
+
+#define ASSERTMSG(expr,string)  if (!(expr)) {\
+                                        PRINTF0 ("Assertion failed: " string );\
+                                        while (1);}
+#else
+#define ASSERTMSG(expr,string)
+#endif
+
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _GLOBAL_H_
+
+// Please keep an empty line at the end of this file.
diff --git a/drivers/staging/epl/kernel/EplDllk.h b/drivers/staging/epl/kernel/EplDllk.h
new file mode 100644
index 000000000000..588e871a922b
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplDllk.h
@@ -0,0 +1,165 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for kernelspace DLL module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDllk.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/08 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_DLLK_H_
+#define _EPL_DLLK_H_
+
+#include "../EplDll.h"
+#include "../EplEvent.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef tEplKernel(*tEplDllkCbAsync) (tEplFrameInfo * pFrameInfo_p);
+
+typedef struct {
+	BYTE m_be_abSrcMac[6];
+
+} tEplDllkInitParam;
+
+// forward declaration
+struct _tEdrvTxBuffer;
+
+struct _tEplDllkNodeInfo {
+	struct _tEplDllkNodeInfo *m_pNextNodeInfo;
+	struct _tEdrvTxBuffer *m_pPreqTxBuffer;
+	unsigned int m_uiNodeId;
+	DWORD m_dwPresTimeout;
+	unsigned long m_ulDllErrorEvents;
+	tEplNmtState m_NmtState;
+	WORD m_wPresPayloadLimit;
+	BYTE m_be_abMacAddr[6];
+	BYTE m_bSoaFlag1;
+	BOOL m_fSoftDelete;	// delete node after error and ignore error
+
+};
+
+typedef struct _tEplDllkNodeInfo tEplDllkNodeInfo;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+tEplKernel EplDllkAddInstance(tEplDllkInitParam * pInitParam_p);
+
+tEplKernel EplDllkDelInstance(void);
+
+// called before NMT_GS_COMMUNICATING will be entered to configure fixed parameters
+tEplKernel EplDllkConfig(tEplDllConfigParam * pDllConfigParam_p);
+
+// set identity of local node (may be at any time, e.g. in case of hostname change)
+tEplKernel EplDllkSetIdentity(tEplDllIdentParam * pDllIdentParam_p);
+
+// process internal events and do work that cannot be done in interrupt-context
+tEplKernel EplDllkProcess(tEplEvent * pEvent_p);
+
+// registers handler for non-EPL frames
+tEplKernel EplDllkRegAsyncHandler(tEplDllkCbAsync pfnDllkCbAsync_p);
+
+// deregisters handler for non-EPL frames
+tEplKernel EplDllkDeregAsyncHandler(tEplDllkCbAsync pfnDllkCbAsync_p);
+
+// register C_DLL_MULTICAST_ASND in ethernet driver if any AsndServiceId is registered
+tEplKernel EplDllkSetAsndServiceIdFilter(tEplDllAsndServiceId ServiceId_p,
+					 tEplDllAsndFilter Filter_p);
+
+// creates the buffer for a Tx frame and registers it to the ethernet driver
+tEplKernel EplDllkCreateTxFrame(unsigned int *puiHandle_p,
+				tEplFrame ** ppFrame_p,
+				unsigned int *puiFrameSize_p,
+				tEplMsgType MsgType_p,
+				tEplDllAsndServiceId ServiceId_p);
+
+tEplKernel EplDllkDeleteTxFrame(unsigned int uiHandle_p);
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+tEplKernel EplDllkAddNode(tEplDllNodeInfo * pNodeInfo_p);
+
+tEplKernel EplDllkDeleteNode(unsigned int uiNodeId_p);
+
+tEplKernel EplDllkSoftDeleteNode(unsigned int uiNodeId_p);
+
+tEplKernel EplDllkSetFlag1OfNode(unsigned int uiNodeId_p, BYTE bSoaFlag1_p);
+
+tEplKernel EplDllkGetFirstNodeInfo(tEplDllkNodeInfo ** ppNodeInfo_p);
+
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+#endif // #ifndef _EPL_DLLK_H_
diff --git a/drivers/staging/epl/kernel/EplDllkCal.h b/drivers/staging/epl/kernel/EplDllkCal.h
new file mode 100644
index 000000000000..6c4dc7e4a80d
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplDllkCal.h
@@ -0,0 +1,141 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for kernelspace DLL Communication Abstraction Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDllkCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/11/13 17:13:09 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/13 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_DLLKCAL_H_
+#define _EPL_DLLKCAL_H_
+
+#include "../EplDll.h"
+#include "../EplEvent.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef struct {
+	unsigned long m_ulCurTxFrameCountGen;
+	unsigned long m_ulCurTxFrameCountNmt;
+	unsigned long m_ulCurRxFrameCount;
+	unsigned long m_ulMaxTxFrameCountGen;
+	unsigned long m_ulMaxTxFrameCountNmt;
+	unsigned long m_ulMaxRxFrameCount;
+
+} tEplDllkCalStatistics;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+tEplKernel EplDllkCalAddInstance(void);
+
+tEplKernel EplDllkCalDelInstance(void);
+
+tEplKernel EplDllkCalAsyncGetTxCount(tEplDllAsyncReqPriority * pPriority_p,
+				     unsigned int *puiCount_p);
+tEplKernel EplDllkCalAsyncGetTxFrame(void *pFrame_p,
+				     unsigned int *puiFrameSize_p,
+				     tEplDllAsyncReqPriority Priority_p);
+// only frames with registered AsndServiceIds are passed to CAL
+tEplKernel EplDllkCalAsyncFrameReceived(tEplFrameInfo * pFrameInfo_p);
+
+tEplKernel EplDllkCalAsyncSend(tEplFrameInfo * pFrameInfo_p,
+			       tEplDllAsyncReqPriority Priority_p);
+
+tEplKernel EplDllkCalAsyncClearBuffer(void);
+
+tEplKernel EplDllkCalGetStatistics(tEplDllkCalStatistics ** ppStatistics);
+
+tEplKernel EplDllkCalProcess(tEplEvent * pEvent_p);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+tEplKernel EplDllkCalAsyncClearQueues(void);
+
+tEplKernel EplDllkCalIssueRequest(tEplDllReqServiceId Service_p,
+				  unsigned int uiNodeId_p, BYTE bSoaFlag1_p);
+
+tEplKernel EplDllkCalAsyncGetSoaRequest(tEplDllReqServiceId * pReqServiceId_p,
+					unsigned int *puiNodeId_p);
+
+tEplKernel EplDllkCalAsyncSetPendingRequests(unsigned int uiNodeId_p,
+					     tEplDllAsyncReqPriority
+					     AsyncReqPrio_p,
+					     unsigned int uiCount_p);
+
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+#endif // #ifndef _EPL_DLLKCAL_H_
diff --git a/drivers/staging/epl/kernel/EplErrorHandlerk.h b/drivers/staging/epl/kernel/EplErrorHandlerk.h
new file mode 100644
index 000000000000..4a67ef88b97a
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplErrorHandlerk.h
@@ -0,0 +1,100 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for kernel error handler module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplErrorHandlerk.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/10/02 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_ERRORHANDLERK_H_
+#define _EPL_ERRORHANDLERK_H_
+
+#include "../EplEvent.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+// init function
+tEplKernel PUBLIC EplErrorHandlerkInit(void);
+
+// add instance
+tEplKernel PUBLIC EplErrorHandlerkAddInstance(void);
+
+// delete instance
+tEplKernel PUBLIC EplErrorHandlerkDelInstance(void);
+
+// processes error events
+tEplKernel PUBLIC EplErrorHandlerkProcess(tEplEvent * pEvent_p);
+
+#endif // #ifndef _EPL_ERRORHANDLERK_H_
diff --git a/drivers/staging/epl/kernel/EplEventk.h b/drivers/staging/epl/kernel/EplEventk.h
new file mode 100644
index 000000000000..1d25aaa2ed49
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplEventk.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for kernel event module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplEventk.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/12 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_EVENTK_H_
+#define _EPL_EVENTK_H_
+
+#include "../EplEvent.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+// init function
+tEplKernel PUBLIC EplEventkInit(tEplSyncCb fpSyncCb);
+
+// add instance
+tEplKernel PUBLIC EplEventkAddInstance(tEplSyncCb fpSyncCb);
+
+// delete instance
+tEplKernel PUBLIC EplEventkDelInstance(void);
+
+// Kernelthread that dispatches events in kernelspace
+tEplKernel PUBLIC EplEventkProcess(tEplEvent * pEvent_p);
+
+// post events from kernelspace
+tEplKernel PUBLIC EplEventkPost(tEplEvent * pEvent_p);
+
+// post errorevents from kernelspace
+tEplKernel PUBLIC EplEventkPostError(tEplEventSource EventSource_p,
+				     tEplKernel EplError_p,
+				     unsigned int uiArgSize_p, void *pArg_p);
+
+#endif // #ifndef _EPL_EVENTK_H_
diff --git a/drivers/staging/epl/kernel/EplNmtk.h b/drivers/staging/epl/kernel/EplNmtk.h
new file mode 100644
index 000000000000..53409cc89921
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplNmtk.h
@@ -0,0 +1,105 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for NMT-Kernelspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtk.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#ifndef _EPLNMTK_H_
+#define _EPLNMTK_H_
+
+#include "../EplNmt.h"
+#include "EplEventk.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtkInit(EPL_MCO_DECL_PTR_INSTANCE_PTR);
+
+EPLDLLEXPORT tEplKernel PUBLIC
+EplNmtkAddInstance(EPL_MCO_DECL_PTR_INSTANCE_PTR);
+
+EPLDLLEXPORT tEplKernel PUBLIC
+EplNmtkDelInstance(EPL_MCO_DECL_PTR_INSTANCE_PTR);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtkProcess(EPL_MCO_DECL_PTR_INSTANCE_PTR_
+					      tEplEvent * pEvent_p);
+
+EPLDLLEXPORT tEplNmtState PUBLIC
+EplNmtkGetNmtState(EPL_MCO_DECL_PTR_INSTANCE_PTR);
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+
+#endif // #ifndef _EPLNMTK_H_
diff --git a/drivers/staging/epl/kernel/EplNmtkCal.h b/drivers/staging/epl/kernel/EplNmtkCal.h
new file mode 100644
index 000000000000..9edeafca4920
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplNmtkCal.h
@@ -0,0 +1,89 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for communication abstraction layer of the
+                NMT-Kernel-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtkCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/16 -k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplNmtk.h"
+
+#ifndef _EPLNMTKCAL_H_
+#define _EPLNMTKCAL_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPLNMTKCAL_H_
diff --git a/drivers/staging/epl/kernel/EplObdk.h b/drivers/staging/epl/kernel/EplObdk.h
new file mode 100644
index 000000000000..cf9f5837dd38
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplObdk.h
@@ -0,0 +1,196 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for Epl-Obd-Kernel-Modul
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObdk.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/19 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplObd.h"
+
+#ifndef _EPLOBDK_H_
+#define _EPLOBDK_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// global variables
+//---------------------------------------------------------------------------
+
+extern BYTE MEM abEplObdTrashObject_g[8];
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdInit(EPL_MCO_DECL_PTR_INSTANCE_PTR_
+					  tEplObdInitParam MEM * pInitParam_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdAddInstance(EPL_MCO_DECL_PTR_INSTANCE_PTR_
+						 tEplObdInitParam MEM *
+						 pInitParam_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdDeleteInstance(EPL_MCO_DECL_INSTANCE_PTR);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdWriteEntry(EPL_MCO_DECL_INSTANCE_PTR_
+						unsigned int uiIndex_p,
+						unsigned int uiSubIndex_p,
+						void *pSrcData_p,
+						tEplObdSize Size_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdReadEntry(EPL_MCO_DECL_INSTANCE_PTR_
+					       unsigned int uiIndex_p,
+					       unsigned int uiSubIndex_p,
+					       void *pDstData_p,
+					       tEplObdSize * pSize_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC
+EplObdSetStoreLoadObjCallback(EPL_MCO_DECL_INSTANCE_PTR_
+			      tEplObdStoreLoadObjCallback fpCallback_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdAccessOdPart(EPL_MCO_DECL_INSTANCE_PTR_
+						  tEplObdPart ObdPart_p,
+						  tEplObdDir Direction_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdDefineVar(EPL_MCO_DECL_INSTANCE_PTR_
+					       tEplVarParam MEM * pVarParam_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT void *PUBLIC EplObdGetObjectDataPtr(EPL_MCO_DECL_INSTANCE_PTR_
+						 unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p);
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdRegisterUserOd(EPL_MCO_DECL_INSTANCE_PTR_
+						    tEplObdEntryPtr pUserOd_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT void PUBLIC EplObdInitVarEntry(EPL_MCO_DECL_INSTANCE_PTR_
+					    tEplObdVarEntry MEM * pVarEntry_p,
+					    tEplObdType Type_p,
+					    tEplObdSize ObdSize_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplObdSize PUBLIC EplObdGetDataSize(EPL_MCO_DECL_INSTANCE_PTR_
+						  unsigned int uiIndex_p,
+						  unsigned int uiSubIndex_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT unsigned int PUBLIC EplObdGetNodeId(EPL_MCO_DECL_INSTANCE_PTR);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdSetNodeId(EPL_MCO_DECL_INSTANCE_PTR_
+					       unsigned int uiNodeId_p,
+					       tEplObdNodeIdType NodeIdType_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdIsNumerical(EPL_MCO_DECL_INSTANCE_PTR_
+						 unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p,
+						 BOOL * pfEntryNumerical);
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdWriteEntryFromLe(EPL_MCO_DECL_INSTANCE_PTR_
+						      unsigned int uiIndex_p,
+						      unsigned int uiSubIndex_p,
+						      void *pSrcData_p,
+						      tEplObdSize Size_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdReadEntryToLe(EPL_MCO_DECL_INSTANCE_PTR_
+						   unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p,
+						   void *pDstData_p,
+						   tEplObdSize * pSize_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdGetAccessType(EPL_MCO_DECL_INSTANCE_PTR_
+						   unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p,
+						   tEplObdAccess *
+						   pAccessTyp_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdSearchVarEntry(EPL_MCO_DECL_INSTANCE_PTR_
+						    unsigned int uiIndex_p,
+						    unsigned int uiSubindex_p,
+						    tEplObdVarEntry MEM **
+						    ppVarEntry_p);
+
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+
+#endif // #ifndef _EPLOBDK_H_
diff --git a/drivers/staging/epl/kernel/EplObdkCal.h b/drivers/staging/epl/kernel/EplObdkCal.h
new file mode 100644
index 000000000000..c173a950054f
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplObdkCal.h
@@ -0,0 +1,89 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for communication abstraction layer
+                for the Epl-Obd-Kernelspace-Modul
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObdkCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/19 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplObd.h"
+
+#ifndef _EPLOBDKCAL_H_
+#define _EPLOBDKCAL_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPLOBDKCAL_H_
diff --git a/drivers/staging/epl/kernel/EplPdok.h b/drivers/staging/epl/kernel/EplPdok.h
new file mode 100644
index 000000000000..b5b18f4cf687
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplPdok.h
@@ -0,0 +1,110 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for kernel PDO module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplPdok.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/06/23 14:56:33 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_PDOK_H_
+#define _EPL_PDOK_H_
+
+#include "../EplPdo.h"
+#include "../EplEvent.h"
+#include "../EplDll.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+// process events from queue (PDOs/frames and SoA for synchronization)
+tEplKernel EplPdokProcess(tEplEvent * pEvent_p);
+
+// copies RPDO to event queue for processing
+// is called by DLL in NMT_CS_READY_TO_OPERATE and NMT_CS_OPERATIONAL
+// PDO needs not to be valid
+tEplKernel EplPdokCbPdoReceived(tEplFrameInfo * pFrameInfo_p);
+
+// posts pointer and size of TPDO to event queue
+// is called by DLL in NMT_CS_PRE_OPERATIONAL_2,
+//     NMT_CS_READY_TO_OPERATE and NMT_CS_OPERATIONAL
+tEplKernel EplPdokCbPdoTransmitted(tEplFrameInfo * pFrameInfo_p);
+
+// posts SoA event to queue
+tEplKernel EplPdokCbSoa(tEplFrameInfo * pFrameInfo_p);
+
+tEplKernel EplPdokAddInstance(void);
+
+tEplKernel EplPdokDelInstance(void);
+
+#endif // #ifndef _EPL_PDOK_H_
diff --git a/drivers/staging/epl/kernel/EplPdokCal.h b/drivers/staging/epl/kernel/EplPdokCal.h
new file mode 100644
index 000000000000..6a183ebe81ef
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplPdokCal.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for kernel PDO Communication Abstraction Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplPdokCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/26 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_PDOKCAL_H_
+#define _EPL_PDOKCAL_H_
+
+#include "../EplInc.h"
+//#include "EplPdo.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCalAddInstance(void);
+
+tEplKernel EplPdokCalDelInstance(void);
+
+// sets flag for validity of TPDOs in shared memory
+tEplKernel EplPdokCalSetTpdosValid(BOOL fValid_p);
+
+// gets flag for validity of TPDOs from shared memory
+tEplKernel EplPdokCalAreTpdosValid(BOOL * pfValid_p);
+
+#endif // #ifndef _EPL_PDOKCAL_H_
diff --git a/drivers/staging/epl/kernel/EplTimerHighResk.h b/drivers/staging/epl/kernel/EplTimerHighResk.h
new file mode 100644
index 000000000000..d5d046d4d370
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplTimerHighResk.h
@@ -0,0 +1,109 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for EPL high resolution Timermodule
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTimerHighResk.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/09/29 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplTimer.h"
+
+#ifndef _EPLTIMERHIGHRESK_H_
+#define _EPLTIMERHIGHRESK_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimerHighReskInit(void);
+
+tEplKernel PUBLIC EplTimerHighReskAddInstance(void);
+
+tEplKernel PUBLIC EplTimerHighReskDelInstance(void);
+
+tEplKernel PUBLIC EplTimerHighReskSetTimerNs(tEplTimerHdl * pTimerHdl_p,
+					     unsigned long long ullTimeNs_p,
+					     tEplTimerkCallback pfnCallback_p,
+					     unsigned long ulArgument_p,
+					     BOOL fContinuously_p);
+
+tEplKernel PUBLIC EplTimerHighReskModifyTimerNs(tEplTimerHdl * pTimerHdl_p,
+						unsigned long long ullTimeNs_p,
+						tEplTimerkCallback
+						pfnCallback_p,
+						unsigned long ulArgument_p,
+						BOOL fContinuously_p);
+
+tEplKernel PUBLIC EplTimerHighReskDeleteTimer(tEplTimerHdl * pTimerHdl_p);
+
+#endif // #ifndef _EPLTIMERHIGHRESK_H_
diff --git a/drivers/staging/epl/kernel/EplTimerk.h b/drivers/staging/epl/kernel/EplTimerk.h
new file mode 100644
index 000000000000..9160e7260de8
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplTimerk.h
@@ -0,0 +1,118 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for EPL Kernel-Timermodule
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTimerk.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/06 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplTimer.h"
+#include "../user/EplEventu.h"
+
+#ifndef _EPLTIMERK_H_
+#define _EPLTIMERK_H_
+
+#if EPL_TIMER_USE_USER != FALSE
+#include "../user/EplTimeru.h"
+#endif
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#if EPL_TIMER_USE_USER != FALSE
+#define EplTimerkInit           EplTimeruInit
+#define EplTimerkAddInstance    EplTimeruAddInstance
+#define EplTimerkDelInstance    EplTimeruDelInstance
+#define EplTimerkSetTimerMs     EplTimeruSetTimerMs
+#define EplTimerkModifyTimerMs  EplTimeruModifyTimerMs
+#define EplTimerkDeleteTimer    EplTimeruDeleteTimer
+#endif
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+#if EPL_TIMER_USE_USER == FALSE
+tEplKernel PUBLIC EplTimerkInit(void);
+
+tEplKernel PUBLIC EplTimerkAddInstance(void);
+
+tEplKernel PUBLIC EplTimerkDelInstance(void);
+
+tEplKernel PUBLIC EplTimerkSetTimerMs(tEplTimerHdl * pTimerHdl_p,
+				      unsigned long ulTime_p,
+				      tEplTimerArg Argument_p);
+
+tEplKernel PUBLIC EplTimerkModifyTimerMs(tEplTimerHdl * pTimerHdl_p,
+					 unsigned long ulTime_p,
+					 tEplTimerArg Argument_p);
+
+tEplKernel PUBLIC EplTimerkDeleteTimer(tEplTimerHdl * pTimerHdl_p);
+#endif
+#endif // #ifndef _EPLTIMERK_H_
diff --git a/drivers/staging/epl/kernel/VirtualEthernet.h b/drivers/staging/epl/kernel/VirtualEthernet.h
new file mode 100644
index 000000000000..deff8720e37f
--- /dev/null
+++ b/drivers/staging/epl/kernel/VirtualEthernet.h
@@ -0,0 +1,96 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for virtual ethernet driver module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: VirtualEthernet.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/09/19 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_VETH_H_
+#define _EPL_VETH_H_
+
+#include "EplDllk.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_VETH)) != 0)
+
+tEplKernel PUBLIC VEthAddInstance(tEplDllkInitParam * pInitParam_p);
+
+tEplKernel PUBLIC VEthDelInstance(void);
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_VETH)) != 0)
+
+#endif // #ifndef _EPL_VETH_H_
diff --git a/drivers/staging/epl/proc_fs.c b/drivers/staging/epl/proc_fs.c
new file mode 100644
index 000000000000..f4910332d3c6
--- /dev/null
+++ b/drivers/staging/epl/proc_fs.c
@@ -0,0 +1,409 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  proc fs entry with diagnostic information under Linux
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: proc_fs.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.13 $  $Date: 2008/11/07 13:55:56 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GNU
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/31 d.k.:   start of implementation
+
+****************************************************************************/
+
+#include "kernel/EplNmtk.h"
+#include "user/EplNmtu.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+#include "user/EplNmtMnu.h"
+#endif
+
+#include "kernel/EplDllkCal.h"
+
+//#include <linux/config.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/major.h>
+#include <linux/version.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <asm/atomic.h>
+#include <linux/proc_fs.h>
+#include <linux/spinlock.h>
+
+#ifdef CONFIG_COLDFIRE
+#include <asm/coldfire.h>
+#include "fec.h"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef EPL_PROC_DEV_NAME
+#define EPL_PROC_DEV_NAME    "epl"
+#endif
+
+#ifndef DBG_TRACE_POINTS
+#define DBG_TRACE_POINTS    23	// # of supported debug trace points
+#endif
+
+#ifndef DBG_TRACE_VALUES
+#define DBG_TRACE_VALUES    24	// # of supported debug trace values (size of circular buffer)
+#endif
+
+//---------------------------------------------------------------------------
+// modul global types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+#ifdef _DBG_TRACE_POINTS_
+atomic_t aatmDbgTracePoint_l[DBG_TRACE_POINTS];
+DWORD adwDbgTraceValue_l[DBG_TRACE_VALUES];
+DWORD dwDbgTraceValueOld_l;
+unsigned int uiDbgTraceValuePos_l;
+spinlock_t spinlockDbgTraceValue_l;
+unsigned long ulDbTraceValueFlags_l;
+#endif
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static int EplLinProcRead(char *pcBuffer_p, char **ppcStart_p, off_t Offset_p,
+			  int nBufferSize_p, int *pEof_p, void *pData_p);
+static int EplLinProcWrite(struct file *file, const char __user * buffer,
+			   unsigned long count, void *data);
+
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+
+EPLDLLEXPORT DWORD PUBLIC EplIdentuGetRunningRequests(void);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+tEplKernel EplLinProcInit(void)
+{
+	struct proc_dir_entry *pProcDirEntry;
+	pProcDirEntry = create_proc_entry(EPL_PROC_DEV_NAME, S_IRUGO, NULL);
+	if (pProcDirEntry != NULL) {
+		pProcDirEntry->read_proc = EplLinProcRead;
+		pProcDirEntry->write_proc = EplLinProcWrite;
+		pProcDirEntry->data = NULL;	// device number or something else
+
+	} else {
+		return kEplNoResource;
+	}
+
+#ifdef _DBG_TRACE_POINTS_
+	// initialize spinlock and circular buffer position
+	spin_lock_init(&spinlockDbgTraceValue_l);
+	uiDbgTraceValuePos_l = 0;
+	dwDbgTraceValueOld_l = 0;
+#endif
+
+	return kEplSuccessful;
+}
+
+tEplKernel EplLinProcFree(void)
+{
+	remove_proc_entry(EPL_PROC_DEV_NAME, NULL);
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//  Target specific event signaling (FEC Tx-/Rx-Interrupt, used by Edrv)
+//---------------------------------------------------------------------------
+
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p)
+{
+
+	if (bTracePointNumber_p >=
+	    (sizeof(aatmDbgTracePoint_l) / sizeof(aatmDbgTracePoint_l[0]))) {
+		goto Exit;
+	}
+
+	atomic_inc(&aatmDbgTracePoint_l[bTracePointNumber_p]);
+
+      Exit:
+
+	return;
+
+}
+
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p)
+{
+
+	spin_lock_irqsave(&spinlockDbgTraceValue_l, ulDbTraceValueFlags_l);
+	if (dwDbgTraceValueOld_l != dwTraceValue_p) {
+		adwDbgTraceValue_l[uiDbgTraceValuePos_l] = dwTraceValue_p;
+		uiDbgTraceValuePos_l =
+		    (uiDbgTraceValuePos_l + 1) % DBG_TRACE_VALUES;
+		dwDbgTraceValueOld_l = dwTraceValue_p;
+	}
+	spin_unlock_irqrestore(&spinlockDbgTraceValue_l, ulDbTraceValueFlags_l);
+
+	return;
+
+}
+#endif
+
+//---------------------------------------------------------------------------
+//  Read function for PROC-FS read access
+//---------------------------------------------------------------------------
+
+static int EplLinProcRead(char *pcBuffer_p,
+			  char **ppcStart_p,
+			  off_t Offset_p,
+			  int nBufferSize_p, int *pEof_p, void *pData_p)
+{
+
+	int nSize;
+	int Eof;
+	tEplDllkCalStatistics *pDllkCalStats;
+
+	nSize = 0;
+	Eof = 0;
+
+	// count calls of this function
+#ifdef _DBG_TRACE_POINTS_
+	TgtDbgSignalTracePoint(0);
+#endif
+
+	//---------------------------------------------------------------
+	// generate static information
+	//---------------------------------------------------------------
+
+	// ---- Driver information ----
+	nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+			  "%s    %s    (c) 2006 %s\n",
+			  EPL_PRODUCT_NAME, EPL_PRODUCT_VERSION,
+			  EPL_PRODUCT_MANUFACTURER);
+
+	//---------------------------------------------------------------
+	// generate process information
+	//---------------------------------------------------------------
+
+	// ---- EPL state ----
+	nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+			  "NMT state:                  0x%04X\n",
+			  (WORD) EplNmtkGetNmtState());
+
+	EplDllkCalGetStatistics(&pDllkCalStats);
+
+	nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+			  "CurAsyncTxGen=%lu CurAsyncTxNmt=%lu CurAsyncRx=%lu\nMaxAsyncTxGen=%lu MaxAsyncTxNmt=%lu MaxAsyncRx=%lu\n",
+			  pDllkCalStats->m_ulCurTxFrameCountGen,
+			  pDllkCalStats->m_ulCurTxFrameCountNmt,
+			  pDllkCalStats->m_ulCurRxFrameCount,
+			  pDllkCalStats->m_ulMaxTxFrameCountGen,
+			  pDllkCalStats->m_ulMaxTxFrameCountNmt,
+			  pDllkCalStats->m_ulMaxRxFrameCount);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	// fetch running IdentRequests
+	nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+			  "running IdentRequests:      0x%08lX\n",
+			  EplIdentuGetRunningRequests());
+
+	// fetch state of NmtMnu module
+	{
+		unsigned int uiMandatorySlaveCount;
+		unsigned int uiSignalSlaveCount;
+		WORD wFlags;
+
+		EplNmtMnuGetDiagnosticInfo(&uiMandatorySlaveCount,
+					   &uiSignalSlaveCount, &wFlags);
+
+		nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+				  "MN  MandSlaveCount: %u  SigSlaveCount: %u  Flags: 0x%X\n",
+				  uiMandatorySlaveCount, uiSignalSlaveCount,
+				  wFlags);
+
+	}
+#endif
+
+	// ---- FEC state ----
+#ifdef CONFIG_COLDFIRE
+	{
+		// Receive the base address
+		unsigned long base_addr;
+#if (EDRV_USED_ETH_CTRL == 0)
+		// Set the base address of FEC0
+		base_addr = FEC_BASE_ADDR_FEC0;
+#else
+		// Set the base address of FEC1
+		base_addr = FEC_BASE_ADDR_FEC1;
+#endif
+
+		nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+				  "FEC_ECR = 0x%08X FEC_EIR = 0x%08X FEC_EIMR = 0x%08X\nFEC_TCR = 0x%08X FECTFSR = 0x%08X FECRFSR = 0x%08X\n",
+				  FEC_ECR(base_addr), FEC_EIR(base_addr),
+				  FEC_EIMR(base_addr), FEC_TCR(base_addr),
+				  FEC_FECTFSR(base_addr),
+				  FEC_FECRFSR(base_addr));
+	}
+#endif
+
+	// ---- DBG: TracePoints ----
+#ifdef _DBG_TRACE_POINTS_
+	{
+		int nNum;
+
+		nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+				  "DbgTracePoints:\n");
+		for (nNum = 0;
+		     nNum < (sizeof(aatmDbgTracePoint_l) / sizeof(atomic_t));
+		     nNum++) {
+			nSize +=
+			    snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+				     " TracePoint[%2d]: %d\n", (int)nNum,
+				     atomic_read(&aatmDbgTracePoint_l[nNum]));
+		}
+
+		nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+				  "DbgTraceValues:\n");
+		for (nNum = 0; nNum < DBG_TRACE_VALUES; nNum++) {
+			if (nNum == uiDbgTraceValuePos_l) {	// next value will be stored at that position
+				nSize +=
+				    snprintf(pcBuffer_p + nSize,
+					     nBufferSize_p - nSize, "*%08lX",
+					     adwDbgTraceValue_l[nNum]);
+			} else {
+				nSize +=
+				    snprintf(pcBuffer_p + nSize,
+					     nBufferSize_p - nSize, " %08lX",
+					     adwDbgTraceValue_l[nNum]);
+			}
+			if ((nNum & 0x00000007) == 0x00000007) {	// 8 values printed -> end of line reached
+				nSize +=
+				    snprintf(pcBuffer_p + nSize,
+					     nBufferSize_p - nSize, "\n");
+			}
+		}
+		if ((nNum & 0x00000007) != 0x00000007) {	// number of values printed is not a multiple of 8 -> print new line
+			nSize +=
+			    snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+				     "\n");
+		}
+	}
+#endif
+
+	Eof = 1;
+	goto Exit;
+
+      Exit:
+
+	*pEof_p = Eof;
+
+	return (nSize);
+
+}
+
+//---------------------------------------------------------------------------
+//  Write function for PROC-FS write access
+//---------------------------------------------------------------------------
+
+static int EplLinProcWrite(struct file *file, const char __user * buffer,
+			   unsigned long count, void *data)
+{
+	char abBuffer[count + 1];
+	int iErr;
+	int iVal = 0;
+	tEplNmtEvent NmtEvent;
+
+	if (count > 0) {
+		iErr = copy_from_user(abBuffer, buffer, count);
+		if (iErr != 0) {
+			return count;
+		}
+		abBuffer[count] = '\0';
+
+		iErr = sscanf(abBuffer, "%i", &iVal);
+	}
+	if ((iVal <= 0) || (iVal > 0x2F)) {
+		NmtEvent = kEplNmtEventSwReset;
+	} else {
+		NmtEvent = (tEplNmtEvent) iVal;
+	}
+	// execute specified NMT command on write access of /proc/epl
+	EplNmtuNmtEvent(NmtEvent);
+
+	return count;
+}
diff --git a/drivers/staging/epl/proc_fs.h b/drivers/staging/epl/proc_fs.h
new file mode 100644
index 000000000000..0586f499553a
--- /dev/null
+++ b/drivers/staging/epl/proc_fs.h
@@ -0,0 +1,89 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  interface for proc fs entry under Linux
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: proc_fs.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:33 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GNU
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/31 d.k.:   start of implementation
+
+****************************************************************************/
+
+#ifndef _EPLPROCFS_H_
+#define _EPLPROCFS_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel EplLinProcInit(void);
+tEplKernel EplLinProcFree(void);
+
+#endif // #ifndef _EPLPROCFS_H_
diff --git a/drivers/staging/epl/user/EplCfgMau.h b/drivers/staging/epl/user/EplCfgMau.h
new file mode 100644
index 000000000000..d25a1e9d36cf
--- /dev/null
+++ b/drivers/staging/epl/user/EplCfgMau.h
@@ -0,0 +1,284 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for Epl Configuration Manager Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplCfgMau.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                VC7
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/14 k.t.:   start of the implementation
+                     -> based on CANopen CfgMa-Modul (CANopen version 5.34)
+
+****************************************************************************/
+
+#include "../EplInc.h"
+
+#ifndef _EPLCFGMA_H_
+#define _EPLCFGMA_H_
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_CFGMA)) != 0)
+
+#include "EplObdu.h"
+#include "EplSdoComu.h"
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+//define max number of timeouts for configuration of 1 device
+#define EPL_CFGMA_MAX_TIMEOUT   3
+
+//callbackfunction, called if configuration is finished
+typedef void (PUBLIC * tfpEplCfgMaCb) (unsigned int uiNodeId_p,
+				       tEplKernel Errorstate_p);
+
+//State for configuartion manager Statemachine
+typedef enum {
+	// general states
+	kEplCfgMaIdle = 0x0000,	// Configurationsprocess
+	// is idle
+	kEplCfgMaWaitForSdocEvent = 0x0001,	// wait until the last
+	// SDOC is finisched
+	kEplCfgMaSkipMappingSub0 = 0x0002,	// write Sub0 of mapping
+	// parameter with 0
+
+	kEplCfgMaFinished = 0x0004	// configuartion is finished
+} tEplCfgState;
+
+typedef enum {
+	kEplCfgMaDcfTypSystecSeg = 0x00,
+	kEplCfgMaDcfTypConDcf = 0x01,
+	kEplCfgMaDcfTypDcf = 0x02,	// not supported
+	kEplCfgMaDcfTypXdc = 0x03	// not supported
+} tEplCfgMaDcfTyp;
+
+typedef enum {
+	kEplCfgMaCommon = 0,	// all other index
+	kEplCfgMaPdoComm = 1,	// communication index
+	kEplCfgMaPdoMapp = 2,	// mapping index
+	kEplCfgMaPdoCommAfterMapp = 3,	// write PDO Cob-Id after mapping subindex 0(set PDO valid)
+
+} tEplCfgMaIndexType;
+
+//bitcoded answer about the last sdo transfer saved in m_SdocState
+// also used to singal start of the State Maschine
+typedef enum {
+	kEplCfgMaSdocBusy = 0x00,	// SDOC activ
+	kEplCfgMaSdocReady = 0x01,	// SDOC finished
+	kEplCfgMaSdocTimeout = 0x02,	// SDOC Timeout
+	kEplCfgMaSdocAbortReceived = 0x04,	// SDOC Abort, see Abortcode
+	kEplCfgMaSdocStart = 0x08	// start State Mschine
+} tEplSdocState;
+
+//internal structure (instancetable for modul configuration manager)
+typedef struct {
+	tEplCfgState m_CfgState;	// state of the configuration state maschine
+	tEplSdoComConHdl m_SdoComConHdl;	// handle for sdo connection
+	DWORD m_dwLastAbortCode;
+	unsigned int m_uiLastIndex;	// last index of configuration, to compair with actual index
+	BYTE *m_pbConcise;	// Ptr to concise DCF
+	BYTE *m_pbActualIndex;	// Ptr to actual index in the DCF segment
+	tfpEplCfgMaCb m_pfnCfgMaCb;	// Ptr to CfgMa Callback, is call if configuration finished
+	tEplKernel m_EplKernelError;	// errorcode
+	DWORD m_dwNumValueCopy;	// numeric values are copied in this variable
+	unsigned int m_uiPdoNodeId;	// buffer for PDO node id
+	BYTE m_bNrOfMappedObject;	// number of mapped objects
+	unsigned int m_uiNodeId;	// Epl node addresse
+	tEplSdocState m_SdocState;	// bitcoded state of the SDO transfer
+	unsigned int m_uiLastSubIndex;	// last subindex of configuration
+	BOOL m_fOneTranferOk;	// atleased one transfer was successful
+	BYTE m_bEventFlag;	// for Eventsignaling to the State Maschine
+	DWORD m_dwCntObjectInDcf;	// number of Objects in DCF
+	tEplCfgMaIndexType m_SkipCfg;	// TRUE if a adsitional Configurationprocess
+	// have to insert e.g. PDO-mapping
+	WORD m_wTimeOutCnt;	// Timeout Counter, break configuration is
+	// m_wTimeOutCnt == CFGMA_MAX_TIMEOUT
+
+} tEplCfgMaNode;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// Function:    EplCfgMaInit()
+//
+// Description: Function creates first instance of Configuration Manager
+//
+// Parameters:
+//
+// Returns:     tEplKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaInit();
+
+//---------------------------------------------------------------------------
+// Function:    EplCfgMaAddInstance()
+//
+// Description: Function creates additional instance of Configuration Manager
+//
+// Parameters:
+//
+// Returns:     tEplKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaAddInstance();
+
+//---------------------------------------------------------------------------
+// Function:    EplCfgMaDelInstance()
+//
+// Description: Function delete instance of Configuration Manager
+//
+// Parameters:
+//
+// Returns:     tEplKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaDelInstance();
+
+//---------------------------------------------------------------------------
+// Function:    EplCfgMaStartConfig()
+//
+// Description: Function starts the configuration process
+//              initialization the statemachine for CfgMa- process
+//
+// Parameters:  uiNodeId_p              = NodeId of the node to configure
+//              pbConcise_p             = pointer to DCF
+//              fpCfgMaCb_p             = pointer to callback function (should not be NULL)
+//              SizeOfConcise_p         = size of DCF in BYTE -> for future use
+//              DcfType_p               = type of the DCF
+//
+// Returns:     tCopKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaStartConfig(unsigned int uiNodeId_p,
+				      BYTE * pbConcise_p,
+				      tfpEplCfgMaCb fpCfgMaCb_p,
+				      tEplObdSize SizeOfConcise_p,
+				      tEplCfgMaDcfTyp DcfType_p);
+
+//---------------------------------------------------------------------------
+// Function:    CfgMaStartConfigurationNode()
+//
+// Description: Function started the configuration process
+//              with the DCF from according OD-entry Subindex == bNodeId_p
+//
+// Parameters:  uiNodeId_p              = NodeId of the node to configure
+//              fpCfgMaCb_p             = pointer to callback function (should not be NULL)
+//              DcfType_p               = type of the DCF
+//
+// Returns:     tCopKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaStartConfigNode(unsigned int uiNodeId_p,
+					  tfpEplCfgMaCb fpCfgMaCb_p,
+					  tEplCfgMaDcfTyp DcfType_p);
+
+//---------------------------------------------------------------------------
+// Function:    EplCfgMaStartConfigNodeDcf()
+//
+// Description: Function starts the configuration process
+//              and links the configuration data to the OD
+//
+// Parameters:  uiNodeId_p              = NodeId of the node to configure
+//              pbConcise_p             = pointer to DCF
+//              fpCfgMaCb_p             = pointer to callback function (should not be NULL)
+//              SizeOfConcise_p         = size of DCF in BYTE -> for future use
+//              DcfType_p               = type of the DCF
+//
+// Returns:     tCopKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaStartConfigNodeDcf(unsigned int uiNodeId_p,
+					     BYTE * pbConcise_p,
+					     tfpEplCfgMaCb fpCfgMaCb_p,
+					     tEplObdSize SizeOfConcise_p,
+					     tEplCfgMaDcfTyp DcfType_p);
+
+//---------------------------------------------------------------------------
+// Function:    EplCfgMaLinkDcf()
+//
+// Description: Function links the configuration data to the OD
+//
+// Parameters:  uiNodeId_p              = NodeId of the node to configure
+//              pbConcise_p             = pointer to DCF
+//              SizeOfConcise_p        = size of DCF in BYTE -> for future use
+//              DcfType_p               = type of the DCF
+//
+// Returns:     tCopKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaLinkDcf(unsigned int uiNodeId_p,
+				  BYTE * pbConcise_p,
+				  tEplObdSize SizeOfConcise_p,
+				  tEplCfgMaDcfTyp DcfType_p);
+
+//---------------------------------------------------------------------------
+// Function:    EplCfgMaCheckDcf()
+//
+// Description: Function check if there is allready a configuration file linked
+//              to the OD (type is given by DcfType_p)
+//
+// Parameters:  uiNodeId_p              = NodeId
+//              DcfType_p               = type of the DCF
+//
+// Returns:     tCopKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaCheckDcf(unsigned int uiNodeId_p,
+				   tEplCfgMaDcfTyp DcfType_p);
+
+#endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_CFGMA)) != 0)
+
+#endif // _EPLCFGMA_H_
+
+// EOF
diff --git a/drivers/staging/epl/user/EplDllu.h b/drivers/staging/epl/user/EplDllu.h
new file mode 100644
index 000000000000..36f8bb76f7ca
--- /dev/null
+++ b/drivers/staging/epl/user/EplDllu.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for userspace DLL module for asynchronous communication
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDllu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/20 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_DLLU_H_
+#define _EPL_DLLU_H_
+
+#include "../EplDll.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef tEplKernel(PUBLIC * tEplDlluCbAsnd) (tEplFrameInfo * pFrameInfo_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+
+tEplKernel EplDlluAddInstance(void);
+
+tEplKernel EplDlluDelInstance(void);
+
+tEplKernel EplDlluRegAsndService(tEplDllAsndServiceId ServiceId_p,
+				 tEplDlluCbAsnd pfnDlluCbAsnd_p,
+				 tEplDllAsndFilter Filter_p);
+
+tEplKernel EplDlluAsyncSend(tEplFrameInfo * pFrameInfo_p,
+			    tEplDllAsyncReqPriority Priority_p);
+
+// processes asynch frames
+tEplKernel EplDlluProcess(tEplFrameInfo * pFrameInfo_p);
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+
+#endif // #ifndef _EPL_DLLU_H_
diff --git a/drivers/staging/epl/user/EplDlluCal.h b/drivers/staging/epl/user/EplDlluCal.h
new file mode 100644
index 000000000000..b1dcd0609573
--- /dev/null
+++ b/drivers/staging/epl/user/EplDlluCal.h
@@ -0,0 +1,117 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for DLL Communication Abstraction Layer module in EPL user part
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDlluCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/20 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_DLLUCAL_H_
+#define _EPL_DLLUCAL_H_
+
+#include "../EplDll.h"
+#include "../EplEvent.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef tEplKernel(PUBLIC * tEplDlluCbAsnd) (tEplFrameInfo * pFrameInfo_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalAddInstance(void);
+
+tEplKernel EplDlluCalDelInstance(void);
+
+tEplKernel EplDlluCalRegAsndService(tEplDllAsndServiceId ServiceId_p,
+				    tEplDlluCbAsnd pfnDlluCbAsnd_p,
+				    tEplDllAsndFilter Filter_p);
+
+tEplKernel EplDlluCalAsyncSend(tEplFrameInfo * pFrameInfo,
+			       tEplDllAsyncReqPriority Priority_p);
+
+tEplKernel EplDlluCalProcess(tEplEvent * pEvent_p);
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+tEplKernel EplDlluCalAddNode(tEplDllNodeInfo * pNodeInfo_p);
+
+tEplKernel EplDlluCalDeleteNode(unsigned int uiNodeId_p);
+
+tEplKernel EplDlluCalSoftDeleteNode(unsigned int uiNodeId_p);
+
+tEplKernel EplDlluCalIssueRequest(tEplDllReqServiceId Service_p,
+				  unsigned int uiNodeId_p, BYTE bSoaFlag1_p);
+
+#endif
+
+#endif // #ifndef _EPL_DLLUCAL_H_
diff --git a/drivers/staging/epl/user/EplEventu.h b/drivers/staging/epl/user/EplEventu.h
new file mode 100644
index 000000000000..322cffd11874
--- /dev/null
+++ b/drivers/staging/epl/user/EplEventu.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for kernel event module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplEventu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/12 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_EVENTU_H_
+#define _EPL_EVENTU_H_
+
+#include "../EplEvent.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+// init function
+tEplKernel PUBLIC EplEventuInit(tEplProcessEventCb pfnApiProcessEventCb_p);
+
+// add instance
+tEplKernel PUBLIC EplEventuAddInstance(tEplProcessEventCb
+				       pfnApiProcessEventCb_p);
+
+// delete instance
+tEplKernel PUBLIC EplEventuDelInstance(void);
+
+// Task that dispatches events in userspace
+tEplKernel PUBLIC EplEventuProcess(tEplEvent * pEvent_p);
+
+// post events from userspace
+tEplKernel PUBLIC EplEventuPost(tEplEvent * pEvent_p);
+
+// post errorevents from userspace
+tEplKernel PUBLIC EplEventuPostError(tEplEventSource EventSource_p,
+				     tEplKernel EplError_p,
+				     unsigned int uiArgSize_p, void *pArg_p);
+
+#endif // #ifndef _EPL_EVENTU_H_
diff --git a/drivers/staging/epl/user/EplIdentu.h b/drivers/staging/epl/user/EplIdentu.h
new file mode 100644
index 000000000000..e7302106c741
--- /dev/null
+++ b/drivers/staging/epl/user/EplIdentu.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for Identu-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplIdentu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/11/15 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplDll.h"
+
+#ifndef _EPLIDENTU_H_
+#define _EPLIDENTU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef tEplKernel(PUBLIC * tEplIdentuCbResponse) (unsigned int uiNodeId_p,
+						   tEplIdentResponse *
+						   pIdentResponse_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplIdentuInit(void);
+
+tEplKernel PUBLIC EplIdentuAddInstance(void);
+
+tEplKernel PUBLIC EplIdentuDelInstance(void);
+
+tEplKernel PUBLIC EplIdentuReset(void);
+
+tEplKernel PUBLIC EplIdentuGetIdentResponse(unsigned int uiNodeId_p,
+					    tEplIdentResponse **
+					    ppIdentResponse_p);
+
+tEplKernel PUBLIC EplIdentuRequestIdentResponse(unsigned int uiNodeId_p,
+						tEplIdentuCbResponse
+						pfnCbResponse_p);
+
+#endif // #ifndef _EPLIDENTU_H_
diff --git a/drivers/staging/epl/user/EplLedu.h b/drivers/staging/epl/user/EplLedu.h
new file mode 100644
index 000000000000..78e70d064459
--- /dev/null
+++ b/drivers/staging/epl/user/EplLedu.h
@@ -0,0 +1,109 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for status and error LED user part module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplLedu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.1 $  $Date: 2008/11/17 16:40:39 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2008/11/17 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplLed.h"
+#include "../EplNmt.h"
+#include "EplEventu.h"
+
+#ifndef _EPLLEDU_H_
+#define _EPLLEDU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef tEplKernel(PUBLIC * tEplLeduStateChangeCallback) (tEplLedType LedType_p,
+							  BOOL fOn_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+
+tEplKernel PUBLIC EplLeduInit(tEplLeduStateChangeCallback pfnCbStateChange_p);
+
+tEplKernel PUBLIC EplLeduAddInstance(tEplLeduStateChangeCallback
+				     pfnCbStateChange_p);
+
+tEplKernel PUBLIC EplLeduDelInstance(void);
+
+tEplKernel PUBLIC EplLeduCbNmtStateChange(tEplEventNmtStateChange
+					  NmtStateChange_p);
+
+tEplKernel PUBLIC EplLeduProcessEvent(tEplEvent * pEplEvent_p);
+
+#endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+
+#endif // #ifndef _EPLLEDU_H_
diff --git a/drivers/staging/epl/user/EplNmtCnu.h b/drivers/staging/epl/user/EplNmtCnu.h
new file mode 100644
index 000000000000..e508055b5b8f
--- /dev/null
+++ b/drivers/staging/epl/user/EplNmtCnu.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for NMT-CN-Userspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtCnu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplNmtu.h"
+#include "../EplDll.h"
+#include "../EplFrame.h"
+
+#ifndef _EPLNMTCNU_H_
+#define _EPLNMTCNU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_CN)) != 0)
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuInit(unsigned int uiNodeId_p);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuAddInstance(unsigned int uiNodeId_p);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuDelInstance(void);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuSendNmtRequest(unsigned int uiNodeId_p,
+						       tEplNmtCommand
+						       NmtCommand_p);
+
+EPLDLLEXPORT tEplKernel PUBLIC
+EplNmtCnuRegisterCheckEventCb(tEplNmtuCheckEventCallback
+			      pfnEplNmtCheckEventCb_p);
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_CN)) != 0)
+
+#endif // #ifndef _EPLNMTCNU_H_
diff --git a/drivers/staging/epl/user/EplNmtMnu.h b/drivers/staging/epl/user/EplNmtMnu.h
new file mode 100644
index 000000000000..c54efeba303f
--- /dev/null
+++ b/drivers/staging/epl/user/EplNmtMnu.h
@@ -0,0 +1,131 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for NMT-MN-Userspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtMnu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplNmtu.h"
+
+#ifndef _EPLNMTMNU_H_
+#define _EPLNMTMNU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef tEplKernel(PUBLIC * tEplNmtMnuCbNodeEvent) (unsigned int uiNodeId_p,
+						    tEplNmtNodeEvent
+						    NodeEvent_p,
+						    tEplNmtState NmtState_p,
+						    WORD wErrorCode_p,
+						    BOOL fMandatory_p);
+
+typedef tEplKernel(PUBLIC *
+		   tEplNmtMnuCbBootEvent) (tEplNmtBootEvent BootEvent_p,
+					   tEplNmtState NmtState_p,
+					   WORD wErrorCode_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+tEplKernel EplNmtMnuInit(tEplNmtMnuCbNodeEvent pfnCbNodeEvent_p,
+			 tEplNmtMnuCbBootEvent pfnCbBootEvent_p);
+
+tEplKernel EplNmtMnuAddInstance(tEplNmtMnuCbNodeEvent pfnCbNodeEvent_p,
+				tEplNmtMnuCbBootEvent pfnCbBootEvent_p);
+
+tEplKernel EplNmtMnuDelInstance(void);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtMnuProcessEvent(tEplEvent * pEvent_p);
+
+tEplKernel EplNmtMnuSendNmtCommand(unsigned int uiNodeId_p,
+				   tEplNmtCommand NmtCommand_p);
+
+tEplKernel EplNmtMnuTriggerStateChange(unsigned int uiNodeId_p,
+				       tEplNmtNodeCommand NodeCommand_p);
+
+tEplKernel PUBLIC EplNmtMnuCbNmtStateChange(tEplEventNmtStateChange
+					    NmtStateChange_p);
+
+tEplKernel PUBLIC EplNmtMnuCbCheckEvent(tEplNmtEvent NmtEvent_p);
+
+tEplKernel PUBLIC EplNmtMnuGetDiagnosticInfo(unsigned int
+					     *puiMandatorySlaveCount_p,
+					     unsigned int
+					     *puiSignalSlaveCount_p,
+					     WORD * pwFlags_p);
+
+#endif
+
+#endif // #ifndef _EPLNMTMNU_H_
diff --git a/drivers/staging/epl/user/EplNmtu.h b/drivers/staging/epl/user/EplNmtu.h
new file mode 100644
index 000000000000..5a56c5830603
--- /dev/null
+++ b/drivers/staging/epl/user/EplNmtu.h
@@ -0,0 +1,155 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for NMT-Userspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplNmt.h"
+#include "EplEventu.h"
+
+#ifndef _EPLNMTU_H_
+#define _EPLNMTU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+// nmt commands
+typedef enum {
+	// requestable ASnd ServiceIds    0x01..0x1F
+	kEplNmtCmdIdentResponse = 0x01,
+	kEplNmtCmdStatusResponse = 0x02,
+	// plain NMT state commands       0x20..0x3F
+	kEplNmtCmdStartNode = 0x21,
+	kEplNmtCmdStopNode = 0x22,
+	kEplNmtCmdEnterPreOperational2 = 0x23,
+	kEplNmtCmdEnableReadyToOperate = 0x24,
+	kEplNmtCmdResetNode = 0x28,
+	kEplNmtCmdResetCommunication = 0x29,
+	kEplNmtCmdResetConfiguration = 0x2A,
+	kEplNmtCmdSwReset = 0x2B,
+	// extended NMT state commands    0x40..0x5F
+	kEplNmtCmdStartNodeEx = 0x41,
+	kEplNmtCmdStopNodeEx = 0x42,
+	kEplNmtCmdEnterPreOperational2Ex = 0x43,
+	kEplNmtCmdEnableReadyToOperateEx = 0x44,
+	kEplNmtCmdResetNodeEx = 0x48,
+	kEplNmtCmdResetCommunicationEx = 0x49,
+	kEplNmtCmdResetConfigurationEx = 0x4A,
+	kEplNmtCmdSwResetEx = 0x4B,
+	// NMT managing commands          0x60..0x7F
+	kEplNmtCmdNetHostNameSet = 0x62,
+	kEplNmtCmdFlushArpEntry = 0x63,
+	// NMT info services              0x80..0xBF
+	kEplNmtCmdPublishConfiguredCN = 0x80,
+	kEplNmtCmdPublishActiveCN = 0x90,
+	kEplNmtCmdPublishPreOperational1 = 0x91,
+	kEplNmtCmdPublishPreOperational2 = 0x92,
+	kEplNmtCmdPublishReadyToOperate = 0x93,
+	kEplNmtCmdPublishOperational = 0x94,
+	kEplNmtCmdPublishStopped = 0x95,
+	kEplNmtCmdPublishEmergencyNew = 0xA0,
+	kEplNmtCmdPublishTime = 0xB0,
+
+	kEplNmtCmdInvalidService = 0xFF
+} tEplNmtCommand;
+
+typedef tEplKernel(PUBLIC *
+		   tEplNmtuStateChangeCallback) (tEplEventNmtStateChange
+						 NmtStateChange_p);
+
+typedef tEplKernel(PUBLIC *
+		   tEplNmtuCheckEventCallback) (tEplNmtEvent NmtEvent_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuInit(void);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuAddInstance(void);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuDelInstance(void);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuNmtEvent(tEplNmtEvent NmtEvent_p);
+
+EPLDLLEXPORT tEplNmtState PUBLIC EplNmtuGetNmtState(void);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuProcessEvent(tEplEvent * pEplEvent_p);
+
+EPLDLLEXPORT tEplKernel PUBLIC
+EplNmtuRegisterStateChangeCb(tEplNmtuStateChangeCallback
+			     pfnEplNmtStateChangeCb_p);
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+
+#endif // #ifndef _EPLNMTU_H_
diff --git a/drivers/staging/epl/user/EplNmtuCal.h b/drivers/staging/epl/user/EplNmtuCal.h
new file mode 100644
index 000000000000..c881582702bb
--- /dev/null
+++ b/drivers/staging/epl/user/EplNmtuCal.h
@@ -0,0 +1,91 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for communication abstraction layer of the
+                NMT-Userspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtuCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/16 -k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplNmtu.h"
+#include "../kernel/EplNmtk.h"
+
+#ifndef _EPLNMTUCAL_H_
+#define _EPLNMTUCAL_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplNmtState PUBLIC EplNmtkCalGetNmtState(void);
+
+#endif // #ifndef _EPLNMTUCAL_H_
diff --git a/drivers/staging/epl/user/EplObdu.h b/drivers/staging/epl/user/EplObdu.h
new file mode 100644
index 000000000000..bc1e17303017
--- /dev/null
+++ b/drivers/staging/epl/user/EplObdu.h
@@ -0,0 +1,192 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for Epl-Obd-Userspace-module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObdu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/19 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplObd.h"
+
+#ifndef _EPLOBDU_H_
+#define _EPLOBDU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+
+#if EPL_OBD_USE_KERNEL != FALSE
+#error "EPL OBDu module enabled, but OBD_USE_KERNEL == TRUE"
+#endif
+
+EPLDLLEXPORT tEplKernel PUBLIC EplObduWriteEntry(unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p,
+						 void *pSrcData_p,
+						 tEplObdSize Size_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduReadEntry(unsigned int uiIndex_p,
+						unsigned int uiSubIndex_p,
+						void *pDstData_p,
+						tEplObdSize * pSize_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduAccessOdPart(tEplObdPart ObdPart_p,
+						   tEplObdDir Direction_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduDefineVar(tEplVarParam MEM * pVarParam_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT void *PUBLIC EplObduGetObjectDataPtr(unsigned int uiIndex_p,
+						  unsigned int uiSubIndex_p);
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduRegisterUserOd(tEplObdEntryPtr pUserOd_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT void PUBLIC EplObduInitVarEntry(tEplObdVarEntry MEM * pVarEntry_p,
+					     BYTE bType_p,
+					     tEplObdSize ObdSize_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplObdSize PUBLIC EplObduGetDataSize(unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT unsigned int PUBLIC EplObduGetNodeId(void);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduSetNodeId(unsigned int uiNodeId_p,
+						tEplObdNodeIdType NodeIdType_p);
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduGetAccessType(unsigned int uiIndex_p,
+						    unsigned int uiSubIndex_p,
+						    tEplObdAccess *
+						    pAccessTyp_p);
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduReadEntryToLe(unsigned int uiIndex_p,
+						    unsigned int uiSubIndex_p,
+						    void *pDstData_p,
+						    tEplObdSize * pSize_p);
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduWriteEntryFromLe(unsigned int uiIndex_p,
+						       unsigned int
+						       uiSubIndex_p,
+						       void *pSrcData_p,
+						       tEplObdSize Size_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduSearchVarEntry(EPL_MCO_DECL_INSTANCE_PTR_
+						     unsigned int uiIndex_p,
+						     unsigned int uiSubindex_p,
+						     tEplObdVarEntry MEM **
+						     ppVarEntry_p);
+
+#elif EPL_OBD_USE_KERNEL != FALSE
+#include "../kernel/EplObdk.h"
+
+#define EplObduWriteEntry       EplObdWriteEntry
+
+#define EplObduReadEntry        EplObdReadEntry
+
+#define EplObduAccessOdPart     EplObdAccessOdPart
+
+#define EplObduDefineVar        EplObdDefineVar
+
+#define EplObduGetObjectDataPtr EplObdGetObjectDataPtr
+
+#define EplObduRegisterUserOd   EplObdRegisterUserOd
+
+#define EplObduInitVarEntry     EplObdInitVarEntry
+
+#define EplObduGetDataSize      EplObdGetDataSize
+
+#define EplObduGetNodeId        EplObdGetNodeId
+
+#define EplObduSetNodeId        EplObdSetNodeId
+
+#define EplObduGetAccessType    EplObdGetAccessType
+
+#define EplObduReadEntryToLe    EplObdReadEntryToLe
+
+#define EplObduWriteEntryFromLe EplObdWriteEntryFromLe
+
+#define EplObduSearchVarEntry   EplObdSearchVarEntry
+
+#define EplObduIsNumerical      EplObdIsNumerical
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+
+#endif // #ifndef _EPLOBDU_H_
diff --git a/drivers/staging/epl/user/EplObduCal.h b/drivers/staging/epl/user/EplObduCal.h
new file mode 100644
index 000000000000..498e0112fac3
--- /dev/null
+++ b/drivers/staging/epl/user/EplObduCal.h
@@ -0,0 +1,148 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for communication abstraction layer
+                for the Epl-Obd-Userspace-Modul
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObduCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/19 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplObd.h"
+
+#ifndef _EPLOBDUCAL_H_
+#define _EPLOBDUCAL_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalWriteEntry(unsigned int uiIndex_p,
+						    unsigned int uiSubIndex_p,
+						    void *pSrcData_p,
+						    tEplObdSize Size_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalReadEntry(unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p,
+						   void *pDstData_p,
+						   tEplObdSize * pSize_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalAccessOdPart(tEplObdPart ObdPart_p,
+						      tEplObdDir Direction_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalDefineVar(tEplVarParam MEM *
+						   pVarParam_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT void *PUBLIC EplObduCalGetObjectDataPtr(unsigned int uiIndex_p,
+						     unsigned int uiSubIndex_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalRegisterUserOd(tEplObdEntryPtr
+							pUserOd_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT void PUBLIC EplObduCalInitVarEntry(tEplObdVarEntry MEM *
+						pVarEntry_p, BYTE bType_p,
+						tEplObdSize ObdSize_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplObdSize PUBLIC EplObduCalGetDataSize(unsigned int uiIndex_p,
+						      unsigned int
+						      uiSubIndex_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT unsigned int PUBLIC EplObduCalGetNodeId(void);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalSetNodeId(unsigned int uiNodeId_p,
+						   tEplObdNodeIdType
+						   NodeIdType_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalGetAccessType(unsigned int uiIndex_p,
+						       unsigned int
+						       uiSubIndex_p,
+						       tEplObdAccess *
+						       pAccessTyp_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalReadEntryToLe(unsigned int uiIndex_p,
+						       unsigned int
+						       uiSubIndex_p,
+						       void *pDstData_p,
+						       tEplObdSize * pSize_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalWriteEntryFromLe(unsigned int
+							  uiIndex_p,
+							  unsigned int
+							  uiSubIndex_p,
+							  void *pSrcData_p,
+							  tEplObdSize Size_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC
+EplObduCalSearchVarEntry(EPL_MCO_DECL_INSTANCE_PTR_ unsigned int uiIndex_p,
+			 unsigned int uiSubindex_p,
+			 tEplObdVarEntry MEM ** ppVarEntry_p);
+
+#endif // #ifndef _EPLOBDUCAL_H_
diff --git a/drivers/staging/epl/user/EplPdou.h b/drivers/staging/epl/user/EplPdou.h
new file mode 100644
index 000000000000..11de4862e8bb
--- /dev/null
+++ b/drivers/staging/epl/user/EplPdou.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for userspace PDO module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplPdou.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/11/19 17:14:38 $
+
+                $State: Exp $
+
+                Build Environment:
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_PDOU_H_
+#define _EPL_PDOU_H_
+
+#include "../EplPdo.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdouAddInstance(void);
+
+tEplKernel EplPdouDelInstance(void);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOU)) != 0)
+tEplKernel PUBLIC EplPdouCbObdAccess(tEplObdCbParam MEM * pParam_p);
+#else
+#define EplPdouCbObdAccess		NULL
+#endif
+
+// returns error if bPdoId_p is already valid
+/*
+tEplKernel EplPdouSetMapping(
+    BYTE bPdoId_p, BOOL fTxRx_p, BYTE bNodeId, BYTE bMappingVersion,
+    tEplPdoMapping * pMapping_p, BYTE bMaxEntries_p);
+
+tEplKernel EplPdouGetMapping(
+    BYTE bPdoId_p, BOOL fTxRx_p, BYTE * pbNodeId, BYTE * pbMappingVersion,
+    tEplPdoMapping * pMapping_p, BYTE * pbMaxEntries_p);
+*/
+
+#endif // #ifndef _EPL_PDOU_H_
diff --git a/drivers/staging/epl/user/EplSdoAsndu.h b/drivers/staging/epl/user/EplSdoAsndu.h
new file mode 100644
index 000000000000..e34959f42792
--- /dev/null
+++ b/drivers/staging/epl/user/EplSdoAsndu.h
@@ -0,0 +1,107 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for SDO/Asnd-Protocolabstractionlayer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoAsndu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/07 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplSdo.h"
+#include "../EplDll.h"
+
+#ifndef _EPLSDOASNDU_H_
+#define _EPLSDOASNDU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+
+tEplKernel PUBLIC EplSdoAsnduInit(tEplSequLayerReceiveCb fpReceiveCb_p);
+
+tEplKernel PUBLIC EplSdoAsnduAddInstance(tEplSequLayerReceiveCb fpReceiveCb_p);
+
+tEplKernel PUBLIC EplSdoAsnduDelInstance(void);
+
+tEplKernel PUBLIC EplSdoAsnduInitCon(tEplSdoConHdl * pSdoConHandle_p,
+				     unsigned int uiTargetNodeId_p);
+
+tEplKernel PUBLIC EplSdoAsnduSendData(tEplSdoConHdl SdoConHandle_p,
+				      tEplFrame * pSrcData_p,
+				      DWORD dwDataSize_p);
+
+tEplKernel PUBLIC EplSdoAsnduDelCon(tEplSdoConHdl SdoConHandle_p);
+
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+
+#endif // #ifndef _EPLSDOASNDU_H_
diff --git a/drivers/staging/epl/user/EplSdoAsySequ.h b/drivers/staging/epl/user/EplSdoAsySequ.h
new file mode 100644
index 000000000000..4658b5f8c538
--- /dev/null
+++ b/drivers/staging/epl/user/EplSdoAsySequ.h
@@ -0,0 +1,111 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for asychrionus SDO Sequence Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoAsySequ.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/26 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplSdo.h"
+#include "EplSdoUdpu.h"
+#include "EplSdoAsndu.h"
+#include "../EplEvent.h"
+#include "EplTimeru.h"
+
+#ifndef _EPLSDOASYSEQU_H_
+#define _EPLSDOASYSEQU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqInit(tEplSdoComReceiveCb fpSdoComCb_p,
+				   tEplSdoComConCb fpSdoComConCb_p);
+
+tEplKernel PUBLIC EplSdoAsySeqAddInstance(tEplSdoComReceiveCb fpSdoComCb_p,
+					  tEplSdoComConCb fpSdoComConCb_p);
+
+tEplKernel PUBLIC EplSdoAsySeqDelInstance(void);
+
+tEplKernel PUBLIC EplSdoAsySeqInitCon(tEplSdoSeqConHdl * pSdoSeqConHdl_p,
+				      unsigned int uiNodeId_p,
+				      tEplSdoType SdoType);
+
+tEplKernel PUBLIC EplSdoAsySeqSendData(tEplSdoSeqConHdl SdoSeqConHdl_p,
+				       unsigned int uiDataSize_p,
+				       tEplFrame * pData_p);
+
+tEplKernel PUBLIC EplSdoAsySeqProcessEvent(tEplEvent * pEvent_p);
+
+tEplKernel PUBLIC EplSdoAsySeqDelCon(tEplSdoSeqConHdl SdoSeqConHdl_p);
+
+#endif // #ifndef _EPLSDOASYSEQU_H_
diff --git a/drivers/staging/epl/user/EplSdoComu.h b/drivers/staging/epl/user/EplSdoComu.h
new file mode 100644
index 000000000000..3e454c7a3559
--- /dev/null
+++ b/drivers/staging/epl/user/EplSdoComu.h
@@ -0,0 +1,126 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for SDO Command Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoComu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/26 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplSdo.h"
+#include "../EplObd.h"
+#include "../EplSdoAc.h"
+#include "EplObdu.h"
+#include "EplSdoAsySequ.h"
+
+#ifndef _EPLSDOCOMU_H_
+#define _EPLSDOCOMU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoComInit(void);
+
+tEplKernel PUBLIC EplSdoComAddInstance(void);
+
+tEplKernel PUBLIC EplSdoComDelInstance(void);
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+
+tEplKernel PUBLIC EplSdoComDefineCon(tEplSdoComConHdl * pSdoComConHdl_p,
+				     unsigned int uiTargetNodeId_p,
+				     tEplSdoType ProtType_p);
+
+tEplKernel PUBLIC EplSdoComInitTransferByIndex(tEplSdoComTransParamByIndex *
+					       pSdoComTransParam_p);
+
+tEplKernel PUBLIC EplSdoComUndefineCon(tEplSdoComConHdl SdoComConHdl_p);
+
+tEplKernel PUBLIC EplSdoComGetState(tEplSdoComConHdl SdoComConHdl_p,
+				    tEplSdoComFinished * pSdoComFinished_p);
+
+tEplKernel PUBLIC EplSdoComSdoAbort(tEplSdoComConHdl SdoComConHdl_p,
+				    DWORD dwAbortCode_p);
+
+#endif
+
+// for future extention
+/*
+tEplKernel PUBLIC EplSdoComInitTransferAllByIndex(tEplSdoComTransParamAllByIndex* pSdoComTransParam_p);
+
+tEplKernel PUBLIC EplSdoComInitTransferByName(tEplSdoComTransParamByName* pSdoComTransParam_p);
+
+tEplKernel PUBLIC EplSdoComInitTransferFile(tEplSdoComTransParamFile* pSdoComTransParam_p);
+
+*/
+
+#endif // #ifndef _EPLSDOCOMU_H_
diff --git a/drivers/staging/epl/user/EplSdoUdpu.h b/drivers/staging/epl/user/EplSdoUdpu.h
new file mode 100644
index 000000000000..2d77b6fff199
--- /dev/null
+++ b/drivers/staging/epl/user/EplSdoUdpu.h
@@ -0,0 +1,109 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for SDO/UDP-Protocollabstractionlayer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoUdpu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/26 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplSdo.h"
+
+#ifndef _EPLSDOUDPU_H_
+#define _EPLSDOUDPU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+
+tEplKernel PUBLIC EplSdoUdpuInit(tEplSequLayerReceiveCb fpReceiveCb_p);
+
+tEplKernel PUBLIC EplSdoUdpuAddInstance(tEplSequLayerReceiveCb fpReceiveCb_p);
+
+tEplKernel PUBLIC EplSdoUdpuDelInstance(void);
+
+tEplKernel PUBLIC EplSdoUdpuConfig(unsigned long ulIpAddr_p,
+				   unsigned int uiPort_p);
+
+tEplKernel PUBLIC EplSdoUdpuInitCon(tEplSdoConHdl * pSdoConHandle_p,
+				    unsigned int uiTargetNodeId_p);
+
+tEplKernel PUBLIC EplSdoUdpuSendData(tEplSdoConHdl SdoConHandle_p,
+				     tEplFrame * pSrcData_p,
+				     DWORD dwDataSize_p);
+
+tEplKernel PUBLIC EplSdoUdpuDelCon(tEplSdoConHdl SdoConHandle_p);
+
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+
+#endif // #ifndef _EPLSDOUDPU_H_
diff --git a/drivers/staging/epl/user/EplStatusu.h b/drivers/staging/epl/user/EplStatusu.h
new file mode 100644
index 000000000000..d211935f0e83
--- /dev/null
+++ b/drivers/staging/epl/user/EplStatusu.h
@@ -0,0 +1,104 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for Statusu-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplStatusu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/11/15 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplDll.h"
+
+#ifndef _EPLSTATUSU_H_
+#define _EPLSTATUSU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef tEplKernel(PUBLIC * tEplStatusuCbResponse) (unsigned int uiNodeId_p,
+						    tEplStatusResponse *
+						    pStatusResponse_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplStatusuInit(void);
+
+tEplKernel PUBLIC EplStatusuAddInstance(void);
+
+tEplKernel PUBLIC EplStatusuDelInstance(void);
+
+tEplKernel PUBLIC EplStatusuReset(void);
+
+tEplKernel PUBLIC EplStatusuRequestStatusResponse(unsigned int uiNodeId_p,
+						  tEplStatusuCbResponse
+						  pfnCbResponse_p);
+
+#endif // #ifndef _EPLSTATUSU_H_
diff --git a/drivers/staging/epl/user/EplTimeru.h b/drivers/staging/epl/user/EplTimeru.h
new file mode 100644
index 000000000000..404495501b8d
--- /dev/null
+++ b/drivers/staging/epl/user/EplTimeru.h
@@ -0,0 +1,107 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for Epl Userspace-Timermodule
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTimeru.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/06 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplTimer.h"
+#include "EplEventu.h"
+
+#ifndef _EPLTIMERU_H_
+#define _EPLTIMERU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimeruInit(void);
+
+tEplKernel PUBLIC EplTimeruAddInstance(void);
+
+tEplKernel PUBLIC EplTimeruDelInstance(void);
+
+tEplKernel PUBLIC EplTimeruSetTimerMs(tEplTimerHdl * pTimerHdl_p,
+				      unsigned long ulTime_p,
+				      tEplTimerArg Argument_p);
+
+tEplKernel PUBLIC EplTimeruModifyTimerMs(tEplTimerHdl * pTimerHdl_p,
+					 unsigned long ulTime_p,
+					 tEplTimerArg Argument_p);
+
+tEplKernel PUBLIC EplTimeruDeleteTimer(tEplTimerHdl * pTimerHdl_p);
+
+BOOL PUBLIC EplTimeruIsTimerActive(tEplTimerHdl TimerHdl_p);
+
+#endif // #ifndef _EPLTIMERU_H_
diff --git a/drivers/staging/et131x/et1310_tx.c b/drivers/staging/et131x/et1310_tx.c
index a95c2608a0c0..30eaac4c8707 100644
--- a/drivers/staging/et131x/et1310_tx.c
+++ b/drivers/staging/et131x/et1310_tx.c
@@ -1345,7 +1345,6 @@ void et131x_free_busy_send_packets(struct et131x_adapter *pAdapter)
 {
 	PMP_TCB pMpTcb;
 	struct list_head *pEntry;
-	struct sk_buff *pPacket = NULL;
 	unsigned long lockflags;
 	uint32_t FreeCounter = 0;
 
@@ -1358,8 +1357,6 @@ void et131x_free_busy_send_packets(struct et131x_adapter *pAdapter)
 		spin_unlock_irqrestore(&pAdapter->SendWaitLock, lockflags);
 
 		pEntry = pAdapter->TxRing.SendWaitQueue.next;
-
-		pPacket = NULL;
 	}
 
 	pAdapter->TxRing.nWaitSend = 0;
diff --git a/drivers/staging/et131x/et131x_debug.h b/drivers/staging/et131x/et131x_debug.h
index dab608031d0b..994108eca663 100644
--- a/drivers/staging/et131x/et131x_debug.h
+++ b/drivers/staging/et131x/et131x_debug.h
@@ -82,11 +82,11 @@
 #define DBG_LVL	3
 #endif /* DBG_LVL */
 
-#define DBG_DEFAULTS		(DBG_ERROR_ON | DBG_WARNING_ON | DBG_BREAK_ON )
+#define DBG_DEFAULTS		(DBG_ERROR_ON | DBG_WARNING_ON | DBG_BREAK_ON)
 
-#define DBG_FLAGS(A)		(A)->dbgFlags
-#define DBG_NAME(A)		(A)->dbgName
-#define DBG_LEVEL(A)		(A)->dbgLevel
+#define DBG_FLAGS(A)		((A)->dbgFlags)
+#define DBG_NAME(A)		((A)->dbgName)
+#define DBG_LEVEL(A)		((A)->dbgLevel)
 
 #ifndef DBG_PRINT
 #define DBG_PRINT(S...)		printk(KERN_DEBUG S)
@@ -108,56 +108,110 @@
 #define _DBG_LEAVE(A)	printk(KERN_DEBUG "%s:%.*s:%s\n", DBG_NAME(A),	\
 				DBG_LEVEL(A)--, _LEAVE_STR, __func__)
 
-#define DBG_ENTER(A)        {if (DBG_FLAGS(A) & DBG_TRACE_ON) \
-                                _DBG_ENTER(A);}
+#define DBG_ENTER(A)							\
+	do {								\
+		if (DBG_FLAGS(A) & DBG_TRACE_ON)			\
+			_DBG_ENTER(A);					\
+	} while (0)
+
+#define DBG_LEAVE(A)							\
+	do {								\
+		if (DBG_FLAGS(A) & DBG_TRACE_ON)			\
+			_DBG_LEAVE(A);					\
+	} while (0)
+
+#define DBG_PARAM(A, N, F, S...)					\
+	do {								\
+		if (DBG_FLAGS(A) & DBG_PARAM_ON)			\
+			DBG_PRINT("  %s -- "F" ", N, S);		\
+	} while (0)
+
+#define DBG_ERROR(A, S...)						 \
+	do {								 \
+		if (DBG_FLAGS(A) & DBG_ERROR_ON) {			 \
+			DBG_PRINT("%s:ERROR:%s ", DBG_NAME(A), __func__);\
+			DBG_PRINTC(S);					 \
+			DBG_TRAP;					 \
+		}							 \
+	} while (0)
+
+#define DBG_WARNING(A, S...)						    \
+	do {								    \
+		if (DBG_FLAGS(A) & DBG_WARNING_ON) {			    \
+			DBG_PRINT("%s:WARNING:%s ", DBG_NAME(A), __func__); \
+			DBG_PRINTC(S);					    \
+		}							    \
+	} while (0)
+
+#define DBG_NOTICE(A, S...)						   \
+	do {								   \
+		if (DBG_FLAGS(A) & DBG_NOTICE_ON) {			   \
+			DBG_PRINT("%s:NOTICE:%s ", DBG_NAME(A), __func__); \
+			DBG_PRINTC(S);					   \
+		}							   \
+	} while (0)
+
+#define DBG_TRACE(A, S...)						  \
+	do {								  \
+		if (DBG_FLAGS(A) & DBG_TRACE_ON) {			  \
+			DBG_PRINT("%s:TRACE:%s ", DBG_NAME(A), __func__); \
+			DBG_PRINTC(S);					  \
+		}							  \
+	} while (0)
+
+#define DBG_VERBOSE(A, S...)						    \
+	do {								    \
+		if (DBG_FLAGS(A) & DBG_VERBOSE_ON) {			    \
+			DBG_PRINT("%s:VERBOSE:%s ", DBG_NAME(A), __func__); \
+			DBG_PRINTC(S);					    \
+		}							    \
+	} while (0)
+
+#define DBG_RX(A, S...)				\
+	do {					\
+		if (DBG_FLAGS(A) & DBG_RX_ON)	\
+			DBG_PRINT(S);		\
+	} while (0)
+
+#define DBG_RX_ENTER(A)				\
+	do {					\
+		if (DBG_FLAGS(A) & DBG_RX_ON)	\
+			_DBG_ENTER(A);		\
+	} while (0)
+
+#define DBG_RX_LEAVE(A)				\
+	do {					\
+		if (DBG_FLAGS(A) & DBG_RX_ON)	\
+			_DBG_LEAVE(A);		\
+	} while (0)
+
+#define DBG_TX(A, S...)				\
+	do {					\
+		if (DBG_FLAGS(A) & DBG_TX_ON)	\
+			DBG_PRINT(S);		\
+	} while (0)
+
+#define DBG_TX_ENTER(A)				\
+	do {					\
+		if (DBG_FLAGS(A) & DBG_TX_ON)	\
+			_DBG_ENTER(A);		\
+	} while (0)
+
+#define DBG_TX_LEAVE(A)				\
+	do {					\
+		if (DBG_FLAGS(A) & DBG_TX_ON)	\
+			_DBG_LEAVE(A);		\
+	} while (0)
+
+#define DBG_ASSERT(C)						\
+	do {							\
+		if (!(C)) {					\
+			DBG_PRINT("ASSERT(%s) -- %s#%d (%s) ",  \
+			     #C, __FILE__, __LINE__, __func__); \
+			DBG_TRAP;				\
+		}						\
+	} while (0)
 
-#define DBG_LEAVE(A)        {if (DBG_FLAGS(A) & DBG_TRACE_ON) \
-                                _DBG_LEAVE(A);}
-
-#define DBG_PARAM(A,N,F,S...)   {if (DBG_FLAGS(A) & DBG_PARAM_ON) \
-                                    DBG_PRINT("  %s -- "F"\n",N,S);}
-
-#define DBG_ERROR(A,S...)	\
-	if (DBG_FLAGS(A) & DBG_ERROR_ON) {				\
-		DBG_PRINT("%s:ERROR:%s ",DBG_NAME(A), __func__);	\
-		DBG_PRINTC(S);						\
-		DBG_TRAP;						\
-	}
-
-#define DBG_WARNING(A,S...) {if (DBG_FLAGS(A) & DBG_WARNING_ON) \
-                                {DBG_PRINT("%s:WARNING:%s ",DBG_NAME(A),__func__);DBG_PRINTC(S);}}
-
-#define DBG_NOTICE(A,S...)  {if (DBG_FLAGS(A) & DBG_NOTICE_ON) \
-                                {DBG_PRINT("%s:NOTICE:%s ",DBG_NAME(A),__func__);DBG_PRINTC(S);}}
-
-#define DBG_TRACE(A,S...)   {if (DBG_FLAGS(A) & DBG_TRACE_ON) \
-                                {DBG_PRINT("%s:TRACE:%s ",DBG_NAME(A), __func__);DBG_PRINTC(S);}}
-
-#define DBG_VERBOSE(A,S...) {if (DBG_FLAGS(A) & DBG_VERBOSE_ON) \
-                                {DBG_PRINT("%s:VERBOSE:%s ",DBG_NAME(A), __func__);DBG_PRINTC(S);}}
-
-#define DBG_RX(A,S...)      {if (DBG_FLAGS(A) & DBG_RX_ON) \
-                                {DBG_PRINT(S);}}
-
-#define DBG_RX_ENTER(A)     {if (DBG_FLAGS(A) & DBG_RX_ON) \
-                                _DBG_ENTER(A);}
-
-#define DBG_RX_LEAVE(A)     {if (DBG_FLAGS(A) & DBG_RX_ON) \
-                                _DBG_LEAVE(A);}
-
-#define DBG_TX(A,S...)      {if (DBG_FLAGS(A) & DBG_TX_ON) \
-                                {DBG_PRINT(S);}}
-
-#define DBG_TX_ENTER(A)     {if (DBG_FLAGS(A) & DBG_TX_ON) \
-                                _DBG_ENTER(A);}
-
-#define DBG_TX_LEAVE(A)     {if (DBG_FLAGS(A) & DBG_TX_ON) \
-                                _DBG_LEAVE(A);}
-
-#define DBG_ASSERT(C)       {if (!(C)) \
-                                {DBG_PRINT("ASSERT(%s) -- %s#%d (%s)\n", \
-                                    #C,__FILE__,__LINE__,__func__); \
-                                DBG_TRAP;}}
 #define STATIC
 
 typedef struct {
diff --git a/drivers/staging/frontier/Kconfig b/drivers/staging/frontier/Kconfig
new file mode 100644
index 000000000000..7121853bd397
--- /dev/null
+++ b/drivers/staging/frontier/Kconfig
@@ -0,0 +1,6 @@
+config TRANZPORT
+	tristate "Frontier Tranzport and Alphatrack support"
+	depends on USB
+	default N
+	---help---
+	  Enable support for the Frontier Tranzport and Alphatrack devices.
diff --git a/drivers/staging/frontier/Makefile b/drivers/staging/frontier/Makefile
new file mode 100644
index 000000000000..2d2ac97492dd
--- /dev/null
+++ b/drivers/staging/frontier/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_TRANZPORT)		+= tranzport.o
+obj-$(CONFIG_TRANZPORT)		+= alphatrack.o
diff --git a/drivers/staging/frontier/README b/drivers/staging/frontier/README
new file mode 100644
index 000000000000..07c9ef9b8fc4
--- /dev/null
+++ b/drivers/staging/frontier/README
@@ -0,0 +1,28 @@
+This directory contains the USB Tranzport and Alphatrack Kernel drivers for Linux.
+
+At present the tranzport does reads/writes of 8 byte cmds to /dev/tranzport0 to control
+the lights and screen and wheel
+
+At present the alphatrack accepts reads/writes of 12 byte cmds to /dev/tranzport0 to control
+the lights and screen and fader.
+
+Both drivers also have some sysfs hooks that are non-functional at the moment.
+
+The API is currently closely tied to the ardour revision and WILL change.
+
+A sysfs interface is PERFECT for simple userspace apps to do fun things with the
+lights and screen. It's fairly lousy for handling input events and very lousy
+for watching the state of the shuttle wheel.
+
+A linux input events interface is great for the input events and shuttle wheel. It's
+theoretically OK on LEDs. A Fader can be mapped to an absolute mouse device.
+But there is no LCD support at all.
+
+In the end this is going to be driven by a midi layer, which handles all those
+cases via a defined API, but - among other things - is slow, doesn't do
+flow control, and is a LOT of extra work. Frankly, I'd like to keep the
+core driver simple because the only realtime work really required is
+the bottom half interrupt handler and the output overlapping.
+
+Exposing some sort of clean aio api to userspace would be perfect. What that
+API looks like? Gah. beats me.
diff --git a/drivers/staging/frontier/TODO b/drivers/staging/frontier/TODO
new file mode 100644
index 000000000000..3620ad2df3ee
--- /dev/null
+++ b/drivers/staging/frontier/TODO
@@ -0,0 +1,9 @@
+TODO:
+	- checkpatch.pl clean
+	- sparse clean
+	- fix userspace interface to be sane
+	- possibly just port to userspace with libusb
+	- review by the USB developer community
+
+Please send any patches for this driver to Greg Kroah-Hartman <greg@kroah.com>
+and David Taht <d@teklibre.com>.
diff --git a/drivers/staging/frontier/alphatrack.c b/drivers/staging/frontier/alphatrack.c
new file mode 100644
index 000000000000..6136e3f8762d
--- /dev/null
+++ b/drivers/staging/frontier/alphatrack.c
@@ -0,0 +1,853 @@
+/*
+ * Frontier Designs Alphatrack driver
+ *
+ * Copyright (C) 2007 Michael Taht (m@taht.net)
+ *
+ * Based on the usbled driver and ldusb drivers by
+ *
+ * Copyright (C) 2004 Greg Kroah-Hartman (greg@kroah.com)
+ * Copyright (C) 2005 Michael Hund <mhund@ld-didactic.de>
+ *
+ * The ldusb driver was, in turn, derived from Lego USB Tower driver
+ * Copyright (C) 2003 David Glance <advidgsf@sourceforge.net>
+ *		 2001-2004 Juergen Stuber <starblue@users.sourceforge.net>
+ *
+ *	This program is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU General Public License as
+ *	published by the Free Software Foundation, version 2.
+ *
+ */
+
+/**
+ * This driver uses a ring buffer for time critical reading of
+ * interrupt in reports and provides read and write methods for
+ * raw interrupt reports.
+ */
+
+/* Note: this currently uses a dumb ringbuffer for reads and writes.
+ * A more optimal driver would cache and kill off outstanding urbs that are
+ * now invalid, and ignore ones that already were in the queue but valid
+ * as we only have 30 commands for the alphatrack. In particular this is
+ * key for getting lights to flash in time as otherwise many commands
+ * can be buffered up before the light change makes it to the interface.
+*/
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/kobject.h>
+#include <linux/mutex.h>
+#include <linux/version.h>
+
+#include <asm/uaccess.h>
+#include <linux/input.h>
+#include <linux/usb.h>
+#include <linux/poll.h>
+
+#include "surface_sysfs.h"
+
+/* make this work on older kernel versions */
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 19)
+#include "frontier_compat.h"
+#endif /* older kernel versions */
+
+#include "alphatrack.h"
+
+#define VENDOR_ID	0x165b
+#define PRODUCT_ID	0xfad1
+
+#ifdef CONFIG_USB_DYNAMIC_MINORS
+#define USB_ALPHATRACK_MINOR_BASE	0
+#else
+// FIXME 176 - is another driver's minor - apply for that
+// #define USB_ALPHATRACK_MINOR_BASE	177
+#define USB_ALPHATRACK_MINOR_BASE	176
+#endif
+
+/* table of devices that work with this driver */
+static struct usb_device_id usb_alphatrack_table [] = {
+	{ USB_DEVICE(VENDOR_ID, PRODUCT_ID) },
+	{ }					/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, usb_alphatrack_table);
+MODULE_VERSION("0.40");
+MODULE_AUTHOR("Mike Taht <m@taht.net>");
+MODULE_DESCRIPTION("Alphatrack USB Driver");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("Frontier Designs Alphatrack Control Surface");
+
+/* These aren't done yet */
+
+#define SUPPRESS_EXTRA_ONLINE_EVENTS 0
+#define BUFFERED_WRITES 0
+#define SUPPRESS_EXTRA_OFFLINE_EVENTS 0
+#define COMPRESS_FADER_EVENTS 0
+
+#define BUFFERED_READS 1
+#define RING_BUFFER_SIZE 512
+#define WRITE_BUFFER_SIZE 34
+#define ALPHATRACK_USB_TIMEOUT 10
+#define OUTPUT_CMD_SIZE 8
+#define INPUT_CMD_SIZE 12
+
+
+static int debug = 0;
+
+/* Use our own dbg macro */
+#define dbg_info(dev, format, arg...) do { if (debug) dev_info(dev , format , ## arg); } while (0)
+
+#define alphatrack_ocmd_info(dev, cmd, format, arg...)
+
+#define alphatrack_icmd_info(dev, cmd, format, arg...)
+
+
+/* Module parameters */
+
+module_param(debug, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Debug enabled or not");
+
+/* All interrupt in transfers are collected in a ring buffer to
+ * avoid racing conditions and get better performance of the driver.
+ */
+
+static int ring_buffer_size = RING_BUFFER_SIZE;
+
+module_param(ring_buffer_size, int,  S_IRUGO);
+MODULE_PARM_DESC(ring_buffer_size, "Read ring buffer size");
+
+/* The write_buffer can one day contain more than one interrupt out transfer.
+ */
+
+static int write_buffer_size = WRITE_BUFFER_SIZE;
+module_param(write_buffer_size, int,  S_IRUGO);
+MODULE_PARM_DESC(write_buffer_size, "Write buffer size");
+
+/*
+ * Increase the interval for debugging purposes.
+ * or set to 1 to use the standard interval from the endpoint descriptors.
+ */
+
+static int min_interrupt_in_interval = ALPHATRACK_USB_TIMEOUT;
+module_param(min_interrupt_in_interval, int, 0);
+MODULE_PARM_DESC(min_interrupt_in_interval, "Minimum interrupt in interval in ms");
+
+static int min_interrupt_out_interval = ALPHATRACK_USB_TIMEOUT;
+module_param(min_interrupt_out_interval, int, 0);
+MODULE_PARM_DESC(min_interrupt_out_interval, "Minimum interrupt out interval in ms");
+
+
+
+/* Structure to hold all of our device specific stuff */
+
+struct usb_alphatrack {
+	struct semaphore	sem;		/* locks this structure */
+	struct usb_interface*	intf;		/* save off the usb interface pointer */
+	int			open_count;	/* number of times this port has been opened */
+
+	struct alphatrack_icmd	(*ring_buffer)[RING_BUFFER_SIZE]; /* just make c happy */
+	struct alphatrack_ocmd	(*write_buffer)[WRITE_BUFFER_SIZE]; /* just make c happy */
+	unsigned int		ring_head;
+	unsigned int		ring_tail;
+
+	wait_queue_head_t	read_wait;
+	wait_queue_head_t	write_wait;
+
+	unsigned char*		interrupt_in_buffer;
+	unsigned char*		oldi_buffer;
+	struct usb_endpoint_descriptor* interrupt_in_endpoint;
+	struct urb*		interrupt_in_urb;
+	int			interrupt_in_interval;
+	size_t			interrupt_in_endpoint_size;
+	int			interrupt_in_running;
+	int			interrupt_in_done;
+
+	char*			interrupt_out_buffer;
+	struct usb_endpoint_descriptor* interrupt_out_endpoint;
+	struct urb*		interrupt_out_urb;
+	int			interrupt_out_interval;
+	size_t			interrupt_out_endpoint_size;
+	int			interrupt_out_busy;
+
+	atomic_t writes_pending;
+	int event; /* alternate interface to events */
+	int fader; /* 10 bits */
+	int lights; /* 23 bits */
+	unsigned char dump_state; /* 0 if disabled 1 if enabled */
+	unsigned char enable; /* 0 if disabled 1 if enabled */
+	unsigned char offline; /* if the device is out of range or asleep */
+	unsigned char verbose; /* be verbose in error reporting */
+	unsigned char  last_cmd[OUTPUT_CMD_SIZE];
+	unsigned char  screen[32];
+};
+
+/* prevent races between open() and disconnect() */
+static DEFINE_MUTEX(disconnect_mutex);
+
+/* forward declaration */
+
+static struct usb_driver usb_alphatrack_driver;
+
+/**
+ *	usb_alphatrack_abort_transfers
+ *      aborts transfers and frees associated data structures
+ */
+static void usb_alphatrack_abort_transfers(struct usb_alphatrack *dev)
+{
+	/* shutdown transfer */
+	if (dev->interrupt_in_running) {
+		dev->interrupt_in_running = 0;
+		if (dev->intf)
+			usb_kill_urb(dev->interrupt_in_urb);
+	}
+	if (dev->interrupt_out_busy)
+		if (dev->intf)
+			usb_kill_urb(dev->interrupt_out_urb);
+}
+
+/**
+ *	usb_alphatrack_delete
+ */
+static void usb_alphatrack_delete(struct usb_alphatrack *dev)
+{
+	usb_alphatrack_abort_transfers(dev);
+	usb_free_urb(dev->interrupt_in_urb);
+	usb_free_urb(dev->interrupt_out_urb);
+	kfree(dev->ring_buffer);
+	kfree(dev->interrupt_in_buffer);
+	kfree(dev->interrupt_out_buffer);
+	kfree(dev); // fixme oldi_buffer
+}
+
+/**
+ *	usb_alphatrack_interrupt_in_callback
+ */
+
+static void usb_alphatrack_interrupt_in_callback(struct urb *urb)
+{
+	struct usb_alphatrack *dev = urb->context;
+	unsigned int next_ring_head;
+	int retval = -1;
+
+	if (urb->status) {
+		if (urb->status == -ENOENT ||
+		    urb->status == -ECONNRESET ||
+		    urb->status == -ESHUTDOWN) {
+			goto exit;
+		} else {
+			dbg_info(&dev->intf->dev, "%s: nonzero status received: %d\n",
+				 __func__, urb->status);
+			goto resubmit; /* maybe we can recover */
+		}
+	}
+
+	if (urb->actual_length != INPUT_CMD_SIZE) {
+		dev_warn(&dev->intf->dev,
+			 "Urb length was %d bytes!! Do something intelligent \n", urb->actual_length);
+	} else {
+		 alphatrack_ocmd_info(&dev->intf->dev,&(*dev->ring_buffer)[dev->ring_tail].cmd,"%s", "bla");
+		 if(memcmp(dev->interrupt_in_buffer,dev->oldi_buffer,INPUT_CMD_SIZE)==0) {
+						goto resubmit;
+		}
+		memcpy(dev->oldi_buffer,dev->interrupt_in_buffer,INPUT_CMD_SIZE);
+
+#if SUPPRESS_EXTRA_OFFLINE_EVENTS
+	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; }
+/* Always pass one offline event up the stack */
+		if(dev->offline > 0 && dev->interrupt_in_buffer[1] != 0xff) { dev->offline = 0; }
+		if(dev->offline == 0 && dev->interrupt_in_buffer[1] == 0xff) { dev->offline = 1; }
+#endif
+		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;
+
+		if (next_ring_head != dev->ring_tail) {
+			memcpy(&((*dev->ring_buffer)[dev->ring_head]),
+						 dev->interrupt_in_buffer, urb->actual_length);
+			dev->ring_head = next_ring_head;
+			retval = 0;
+			memset(dev->interrupt_in_buffer, 0, urb->actual_length);
+		} else {
+			dev_warn(&dev->intf->dev,
+				 "Ring buffer overflow, %d bytes dropped\n",
+				 urb->actual_length);
+			memset(dev->interrupt_in_buffer, 0, urb->actual_length);
+		}
+	}
+
+resubmit:
+	/* resubmit if we're still running */
+	if (dev->interrupt_in_running && dev->intf) {
+		retval = usb_submit_urb(dev->interrupt_in_urb, GFP_ATOMIC);
+		if (retval)
+			dev_err(&dev->intf->dev,
+				"usb_submit_urb failed (%d)\n", retval);
+	}
+
+exit:
+	dev->interrupt_in_done = 1;
+	wake_up_interruptible(&dev->read_wait);
+}
+
+/**
+ *	usb_alphatrack_interrupt_out_callback
+ */
+static void usb_alphatrack_interrupt_out_callback(struct urb *urb)
+{
+	struct usb_alphatrack *dev = urb->context;
+
+	/* sync/async unlink faults aren't errors */
+	if (urb->status && !(urb->status == -ENOENT ||
+			     urb->status == -ECONNRESET ||
+			     urb->status == -ESHUTDOWN))
+		dbg_info(&dev->intf->dev,
+			 "%s - nonzero write interrupt status received: %d\n",
+			 __func__, urb->status);
+	atomic_dec(&dev->writes_pending);
+	dev->interrupt_out_busy = 0;
+	wake_up_interruptible(&dev->write_wait);
+}
+
+/**
+ *	usb_alphatrack_open
+ */
+static int usb_alphatrack_open(struct inode *inode, struct file *file)
+{
+	struct usb_alphatrack *dev;
+	int subminor;
+	int retval = 0;
+	struct usb_interface *interface;
+
+	nonseekable_open(inode, file);
+	subminor = iminor(inode);
+
+	mutex_lock(&disconnect_mutex);
+
+	interface = usb_find_interface(&usb_alphatrack_driver, subminor);
+
+	if (!interface) {
+		err("%s - error, can't find device for minor %d\n",
+		     __func__, subminor);
+		retval = -ENODEV;
+		goto unlock_disconnect_exit;
+	}
+
+	dev = usb_get_intfdata(interface);
+
+	if (!dev) {
+		retval = -ENODEV;
+		goto unlock_disconnect_exit;
+	}
+
+	/* lock this device */
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto unlock_disconnect_exit;
+	}
+
+	/* allow opening only once */
+	if (dev->open_count) {
+		retval = -EBUSY;
+		goto unlock_exit;
+	}
+	dev->open_count = 1;
+
+	/* initialize in direction */
+	dev->ring_head = 0;
+	dev->ring_tail = 0;
+	usb_fill_int_urb(dev->interrupt_in_urb,
+			 interface_to_usbdev(interface),
+			 usb_rcvintpipe(interface_to_usbdev(interface),
+					dev->interrupt_in_endpoint->bEndpointAddress),
+			 dev->interrupt_in_buffer,
+			 dev->interrupt_in_endpoint_size,
+			 usb_alphatrack_interrupt_in_callback,
+			 dev,
+			 dev->interrupt_in_interval);
+
+	dev->interrupt_in_running = 1;
+	dev->interrupt_in_done = 0;
+	dev->enable = 1;
+	dev->offline = 0;
+
+	retval = usb_submit_urb(dev->interrupt_in_urb, GFP_KERNEL);
+	if (retval) {
+		dev_err(&interface->dev, "Couldn't submit interrupt_in_urb %d\n", retval);
+		dev->interrupt_in_running = 0;
+		dev->open_count = 0;
+		goto unlock_exit;
+	}
+
+	/* save device in the file's private structure */
+	file->private_data = dev;
+
+
+unlock_exit:
+	up(&dev->sem);
+
+unlock_disconnect_exit:
+	mutex_unlock(&disconnect_mutex);
+
+	return retval;
+}
+
+/**
+ *	usb_alphatrack_release
+ */
+static int usb_alphatrack_release(struct inode *inode, struct file *file)
+{
+	struct usb_alphatrack *dev;
+	int retval = 0;
+
+	dev = file->private_data;
+
+	if (dev == NULL) {
+		retval = -ENODEV;
+		goto exit;
+	}
+
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto exit;
+	}
+
+	if (dev->open_count != 1) {
+		retval = -ENODEV;
+		goto unlock_exit;
+	}
+
+	if (dev->intf == NULL) {
+		/* the device was unplugged before the file was released */
+		up(&dev->sem);
+		/* unlock here as usb_alphatrack_delete frees dev */
+		usb_alphatrack_delete(dev);
+		retval = -ENODEV;
+		goto exit;
+	}
+
+	/* wait until write transfer is finished */
+	if (dev->interrupt_out_busy)
+		wait_event_interruptible_timeout(dev->write_wait, !dev->interrupt_out_busy, 2 * HZ);
+	usb_alphatrack_abort_transfers(dev);
+	dev->open_count = 0;
+
+unlock_exit:
+	up(&dev->sem);
+
+exit:
+	return retval;
+}
+
+/**
+ *	usb_alphatrack_poll
+ */
+static unsigned int usb_alphatrack_poll(struct file *file, poll_table *wait)
+{
+	struct usb_alphatrack *dev;
+	unsigned int mask = 0;
+
+	dev = file->private_data;
+
+	poll_wait(file, &dev->read_wait, wait);
+	poll_wait(file, &dev->write_wait, wait);
+
+	if (dev->ring_head != dev->ring_tail)
+		mask |= POLLIN | POLLRDNORM;
+	if (!dev->interrupt_out_busy)
+		mask |= POLLOUT | POLLWRNORM;
+
+	return mask;
+}
+
+/**
+ *	usb_alphatrack_read
+ */
+static ssize_t usb_alphatrack_read(struct file *file, char __user *buffer, size_t count,
+			   loff_t *ppos)
+{
+	struct usb_alphatrack *dev;
+	int retval = 0;
+
+	int c = 0;
+
+	dev = file->private_data;
+
+	/* verify that we actually have some data to read */
+	if (count == 0)
+		goto exit;
+
+	/* lock this object */
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto exit;
+	}
+
+	/* verify that the device wasn't unplugged */
+	if (dev->intf == NULL) {
+		retval = -ENODEV;
+		err("No device or device unplugged %d\n", retval);
+		goto unlock_exit;
+	}
+
+	while (dev->ring_head == dev->ring_tail) {
+					if (file->f_flags & O_NONBLOCK) {
+									retval = -EAGAIN;
+									goto unlock_exit;
+					}
+					dev->interrupt_in_done = 0 ;
+					retval = wait_event_interruptible(dev->read_wait, dev->interrupt_in_done);
+					if (retval < 0) {
+									goto unlock_exit;
+					}
+	}
+
+	alphatrack_ocmd_info(&dev->intf->dev, &(*dev->ring_buffer)[dev->ring_tail].cmd, "%s", ": copying to userspace");
+
+	   c = 0;
+	   while((c < count) && (dev->ring_tail != dev->ring_head)) {
+						 if (copy_to_user(&buffer[c], &(*dev->ring_buffer)[dev->ring_tail], INPUT_CMD_SIZE)) {
+										 retval = -EFAULT;
+										 goto unlock_exit;
+						 }
+						 dev->ring_tail = (dev->ring_tail+1) % ring_buffer_size;
+						 c+=INPUT_CMD_SIZE;
+						 dbg_info(&dev->intf->dev, "%s: head, tail are %x, %x\n", __func__,dev->ring_head,dev->ring_tail);
+	   }
+	   retval = c;
+
+unlock_exit:
+	/* unlock the device */
+	up(&dev->sem);
+
+exit:
+	return retval;
+}
+
+/**
+ *	usb_alphatrack_write
+ */
+static ssize_t usb_alphatrack_write(struct file *file, const char __user *buffer,
+			    size_t count, loff_t *ppos)
+{
+	struct usb_alphatrack *dev;
+	size_t bytes_to_write;
+	int retval = 0;
+
+	dev = file->private_data;
+
+	/* verify that we actually have some data to write */
+	if (count == 0)
+		goto exit;
+
+	/* lock this object */
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto exit;
+	}
+
+	/* verify that the device wasn't unplugged */
+	if (dev->intf == NULL) {
+		retval = -ENODEV;
+		err("No device or device unplugged %d\n", retval);
+		goto unlock_exit;
+	}
+
+	/* wait until previous transfer is finished */
+	if (dev->interrupt_out_busy) {
+		if (file->f_flags & O_NONBLOCK) {
+			retval = -EAGAIN;
+			goto unlock_exit;
+		}
+		retval = wait_event_interruptible(dev->write_wait, !dev->interrupt_out_busy);
+		if (retval < 0) {
+			goto unlock_exit;
+		}
+	}
+
+	/* write the data into interrupt_out_buffer from userspace */
+  /* FIXME - if you write more than 12 bytes this breaks */
+	bytes_to_write = min(count, write_buffer_size*dev->interrupt_out_endpoint_size);
+	if (bytes_to_write < count)
+		dev_warn(&dev->intf->dev, "Write buffer overflow, %zd bytes dropped\n",count-bytes_to_write);
+
+	dbg_info(&dev->intf->dev, "%s: count = %zd, bytes_to_write = %zd\n", __func__, count, bytes_to_write);
+
+	if (copy_from_user(dev->interrupt_out_buffer, buffer, bytes_to_write)) {
+		retval = -EFAULT;
+		goto unlock_exit;
+	}
+
+	if (dev->interrupt_out_endpoint == NULL) {
+		err("Endpoint should not be be null! \n");
+		goto unlock_exit;
+	}
+
+	/* send off the urb */
+	usb_fill_int_urb(dev->interrupt_out_urb,
+			 interface_to_usbdev(dev->intf),
+			 usb_sndintpipe(interface_to_usbdev(dev->intf),
+					dev->interrupt_out_endpoint->bEndpointAddress),
+			 dev->interrupt_out_buffer,
+			 bytes_to_write,
+			 usb_alphatrack_interrupt_out_callback,
+			 dev,
+			 dev->interrupt_out_interval);
+	dev->interrupt_out_busy = 1;
+	atomic_inc(&dev->writes_pending);
+	wmb();
+
+	retval = usb_submit_urb(dev->interrupt_out_urb, GFP_KERNEL);
+	if (retval) {
+		dev->interrupt_out_busy = 0;
+		err("Couldn't submit interrupt_out_urb %d\n", retval);
+		atomic_dec(&dev->writes_pending);
+		goto unlock_exit;
+	}
+	retval = bytes_to_write;
+
+unlock_exit:
+	/* unlock the device */
+	up(&dev->sem);
+
+exit:
+	return retval;
+}
+
+/* file operations needed when we register this driver */
+static const struct file_operations usb_alphatrack_fops = {
+	.owner =	THIS_MODULE,
+	.read  =	usb_alphatrack_read,
+	.write =	usb_alphatrack_write,
+	.open =		usb_alphatrack_open,
+	.release =	usb_alphatrack_release,
+	.poll =		usb_alphatrack_poll,
+};
+
+/*
+ * 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 usb_alphatrack_class = {
+	.name =		"alphatrack%d",
+	.fops =		&usb_alphatrack_fops,
+	.minor_base =	USB_ALPHATRACK_MINOR_BASE,
+};
+
+
+/**
+ *	usb_alphatrack_probe
+ *
+ *	Called by the usb core when a new device is connected that it thinks
+ *	this driver might be interested in.
+ */
+static int usb_alphatrack_probe(struct usb_interface *intf, const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct usb_alphatrack *dev = NULL;
+	struct usb_host_interface *iface_desc;
+	struct usb_endpoint_descriptor *endpoint;
+	int i;
+	int true_size;
+	int retval = -ENOMEM;
+
+	/* allocate memory for our device state and intialize it */
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (dev == NULL) {
+		dev_err(&intf->dev, "Out of memory\n");
+		goto exit;
+	}
+	init_MUTEX(&dev->sem);
+	dev->intf = intf;
+	init_waitqueue_head(&dev->read_wait);
+	init_waitqueue_head(&dev->write_wait);
+
+	iface_desc = intf->cur_altsetting;
+
+	/* set up the endpoint information */
+	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+		endpoint = &iface_desc->endpoint[i].desc;
+
+		if (usb_endpoint_is_int_in(endpoint))
+			dev->interrupt_in_endpoint = endpoint;
+
+		if (usb_endpoint_is_int_out(endpoint))
+			dev->interrupt_out_endpoint = endpoint;
+	}
+	if (dev->interrupt_in_endpoint == NULL) {
+		dev_err(&intf->dev, "Interrupt in endpoint not found\n");
+		goto error;
+	}
+	if (dev->interrupt_out_endpoint == NULL)
+		dev_warn(&intf->dev, "Interrupt out endpoint not found (using control endpoint instead)\n");
+
+	dev->interrupt_in_endpoint_size = le16_to_cpu(dev->interrupt_in_endpoint->wMaxPacketSize);
+
+	if (dev->interrupt_in_endpoint_size != 64)
+	    dev_warn(&intf->dev, "Interrupt in endpoint size is not 64!\n");
+
+	if(ring_buffer_size == 0) { ring_buffer_size = RING_BUFFER_SIZE; }
+
+	true_size = min(ring_buffer_size,RING_BUFFER_SIZE);
+
+	/* FIXME - there are more usb_alloc routines for dma correctness. Needed? */
+
+//	dev->ring_buffer = kmalloc((true_size*sizeof(struct alphatrack_icmd))+12, GFP_KERNEL);
+	dev->ring_buffer = kmalloc((true_size*sizeof(struct alphatrack_icmd)), GFP_KERNEL);
+
+	if (!dev->ring_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate input ring_buffer of size %d\n",true_size);
+		goto error;
+	}
+
+	dev->interrupt_in_buffer = kmalloc(dev->interrupt_in_endpoint_size, GFP_KERNEL);
+
+	if (!dev->interrupt_in_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_in_buffer\n");
+		goto error;
+	}
+	dev->oldi_buffer = kmalloc(dev->interrupt_in_endpoint_size, GFP_KERNEL);
+	if (!dev->oldi_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate old buffer\n");
+		goto error;
+	}
+	dev->interrupt_in_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!dev->interrupt_in_urb) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_in_urb\n");
+		goto error;
+	}
+
+	dev->interrupt_out_endpoint_size = dev->interrupt_out_endpoint ? le16_to_cpu(dev->interrupt_out_endpoint->wMaxPacketSize) :
+									 udev->descriptor.bMaxPacketSize0;
+
+	if (dev->interrupt_out_endpoint_size !=64)
+		dev_warn(&intf->dev, "Interrupt out endpoint size is not 64!)\n");
+
+	if(write_buffer_size == 0) { write_buffer_size = WRITE_BUFFER_SIZE; }
+	true_size = min(write_buffer_size,WRITE_BUFFER_SIZE);
+
+	dev->interrupt_out_buffer = kmalloc(true_size*dev->interrupt_out_endpoint_size, GFP_KERNEL);
+
+	if (!dev->interrupt_out_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_out_buffer\n");
+		goto error;
+	}
+
+	dev->write_buffer = kmalloc(sizeof(struct alphatrack_ocmd)*true_size, GFP_KERNEL);
+
+	if (!dev->write_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate write_buffer \n");
+		goto error;
+	}
+
+	dev->interrupt_out_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!dev->interrupt_out_urb) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_out_urb\n");
+		goto error;
+	}
+	dev->interrupt_in_interval = min_interrupt_in_interval > dev->interrupt_in_endpoint->bInterval ? min_interrupt_in_interval : dev->interrupt_in_endpoint->bInterval;
+	if (dev->interrupt_out_endpoint)
+		dev->interrupt_out_interval = min_interrupt_out_interval > dev->interrupt_out_endpoint->bInterval ? min_interrupt_out_interval : dev->interrupt_out_endpoint->bInterval;
+
+	/* we can register the device now, as it is ready */
+	usb_set_intfdata(intf, dev);
+
+	atomic_set(&dev->writes_pending,0);
+	retval = usb_register_dev(intf, &usb_alphatrack_class);
+	if (retval) {
+		/* something prevented us from registering this driver */
+		dev_err(&intf->dev, "Not able to get a minor for this device.\n");
+		usb_set_intfdata(intf, NULL);
+		goto error;
+	}
+
+	/* let the user know what node this device is now attached to */
+	dev_info(&intf->dev, "Alphatrack Device #%d now attached to major %d minor %d\n",
+		(intf->minor - USB_ALPHATRACK_MINOR_BASE), USB_MAJOR, intf->minor);
+
+exit:
+	return retval;
+
+error:
+	usb_alphatrack_delete(dev);
+
+	return retval;
+}
+
+/**
+ *	usb_alphatrack_disconnect
+ *
+ *	Called by the usb core when the device is removed from the system.
+ */
+static void usb_alphatrack_disconnect(struct usb_interface *intf)
+{
+	struct usb_alphatrack *dev;
+	int minor;
+
+	mutex_lock(&disconnect_mutex);
+
+	dev = usb_get_intfdata(intf);
+	usb_set_intfdata(intf, NULL);
+
+	down(&dev->sem);
+
+	minor = intf->minor;
+
+	/* give back our minor */
+	usb_deregister_dev(intf, &usb_alphatrack_class);
+
+	/* if the device is not opened, then we clean up right now */
+	if (!dev->open_count) {
+		up(&dev->sem);
+		usb_alphatrack_delete(dev);
+	} else {
+		dev->intf = NULL;
+		up(&dev->sem);
+	}
+
+	atomic_set(&dev->writes_pending,0);
+	mutex_unlock(&disconnect_mutex);
+
+	dev_info(&intf->dev, "Alphatrack Surface #%d now disconnected\n",
+		 (minor - USB_ALPHATRACK_MINOR_BASE));
+}
+
+/* usb specific object needed to register this driver with the usb subsystem */
+static struct usb_driver usb_alphatrack_driver = {
+	.name =		"alphatrack",
+	.probe =	usb_alphatrack_probe,
+	.disconnect =	usb_alphatrack_disconnect,
+	.id_table =	usb_alphatrack_table,
+};
+
+/**
+ *	usb_alphatrack_init
+ */
+static int __init usb_alphatrack_init(void)
+{
+	int retval;
+
+	/* register this driver with the USB subsystem */
+	retval = usb_register(&usb_alphatrack_driver);
+	if (retval)
+		err("usb_register failed for the "__FILE__" driver. Error number %d\n", retval);
+
+	return retval;
+}
+
+/**
+ *	usb_alphatrack_exit
+ */
+static void __exit usb_alphatrack_exit(void)
+{
+	/* deregister this driver with the USB subsystem */
+	usb_deregister(&usb_alphatrack_driver);
+}
+
+module_init(usb_alphatrack_init);
+module_exit(usb_alphatrack_exit);
+
diff --git a/drivers/staging/frontier/alphatrack.h b/drivers/staging/frontier/alphatrack.h
new file mode 100644
index 000000000000..35c90a90eb08
--- /dev/null
+++ b/drivers/staging/frontier/alphatrack.h
@@ -0,0 +1,92 @@
+#define show_set_bit(a) show_set_mbit(alphatrack,a)
+#define show_set_cmd(a) show_set_mcmd(alphatrack,a)
+#define show_set_int(a) show_set_mint(alphatrack,a)
+#define show_set_char(a) show_set_mchar(alphatrack,a)
+#define show_set_light(a) show_set_ebit(alphatrack,LightID,lights,a)
+#define show_set_button(a) show_set_ebit(alphatrack,ButtonID,button,a)
+
+struct alphatrack_icmd {
+    unsigned char cmd[12];
+};
+
+struct alphatrack_ocmd {
+    unsigned char cmd[8];
+};
+
+enum LightID {
+        LIGHT_EQ = 0,
+        LIGHT_OUT,
+        LIGHT_F2,
+        LIGHT_SEND,
+        LIGHT_IN,
+        LIGHT_F1,
+        LIGHT_PAN,
+        LIGHT_UNDEF1,
+        LIGHT_UNDEF2,
+        LIGHT_SHIFT,
+        LIGHT_TRACKMUTE,
+        LIGHT_TRACKSOLO,
+        LIGHT_TRACKREC,
+        LIGHT_READ,
+        LIGHT_WRITE,
+        LIGHT_ANYSOLO,
+        LIGHT_AUTO,
+        LIGHT_F4,
+        LIGHT_RECORD,
+        LIGHT_WINDOW,
+        LIGHT_PLUGIN,
+        LIGHT_F3,
+        LIGHT_LOOP
+};
+
+#define BUTTONMASK_BATTERY     0x00004000
+#define BUTTONMASK_BACKLIGHT   0x00008000
+#define BUTTONMASK_FASTFORWARD 0x04000000
+#define BUTTONMASK_TRACKMUTE   0x00040000
+#define BUTTONMASK_TRACKSOLO   0x00800000
+#define BUTTONMASK_TRACKLEFT   0x80000000
+#define BUTTONMASK_RECORD      0x02000000
+#define BUTTONMASK_SHIFT       0x20000000
+#define BUTTONMASK_PUNCH       0x00800000
+#define BUTTONMASK_TRACKRIGHT  0x00020000
+#define BUTTONMASK_REWIND      0x01000000
+#define BUTTONMASK_STOP        0x10000000
+#define BUTTONMASK_LOOP        0x00010000
+#define BUTTONMASK_TRACKREC    0x00001000
+#define BUTTONMASK_PLAY        0x08000000
+#define BUTTONMASK_TOUCH1      0x00000008
+#define BUTTONMASK_TOUCH2      0x00000010
+#define BUTTONMASK_TOUCH3      0x00000020
+
+#define BUTTONMASK_PRESS1      0x00000009
+#define BUTTONMASK_PRESS2      0x00008010
+#define BUTTONMASK_PRESS3      0x00002020
+
+// last 3 bytes are the slider position
+// 40 is the actual slider moving, the most sig bits, and 3 lsb
+
+#define BUTTONMASK_FLIP         0x40000000
+#define BUTTONMASK_F1           0x00100000
+#define BUTTONMASK_F2           0x00400000
+#define BUTTONMASK_F3           0x00200000
+#define BUTTONMASK_F4           0x00080000
+#define BUTTONMASK_PAN          0x00000200
+#define BUTTONMASK_SEND         0x00000800
+#define BUTTONMASK_EQ           0x00004000
+#define BUTTONMASK_PLUGIN       0x00000400
+#define BUTTONMASK_AUTO         0x00000100
+
+
+// #define BUTTONMASK_FOOTSWITCH FIXME
+
+// Lookup. name. midi out. midi in.
+
+struct buttonmap_t {
+	u32 mask;
+	short midi_in;
+	short midi_out;
+	char *name;
+//  	void (*function) (buttonmap_t *);
+  	void (*function) (void);
+};
+
diff --git a/drivers/staging/frontier/frontier_compat.h b/drivers/staging/frontier/frontier_compat.h
new file mode 100644
index 000000000000..00450e637ac8
--- /dev/null
+++ b/drivers/staging/frontier/frontier_compat.h
@@ -0,0 +1,63 @@
+/* USB defines for older kernels */
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 19)
+
+/**
+ * usb_endpoint_dir_out - check if the endpoint has OUT direction
+ * @epd: endpoint to be checked
+ *
+ * Returns true if the endpoint is of type OUT, otherwise it returns false.
+ */
+
+static inline int usb_endpoint_dir_out(const struct usb_endpoint_descriptor *epd)
+{
+       return ((epd->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT);
+}
+
+static inline int usb_endpoint_dir_in(const struct usb_endpoint_descriptor *epd)
+{
+       return ((epd->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_IN);
+}
+
+
+/**
+ * usb_endpoint_xfer_int - check if the endpoint has interrupt transfer type
+ * @epd: endpoint to be checked
+ *
+ * Returns true if the endpoint is of type interrupt, otherwise it returns
+ * false.
+ */
+static inline int usb_endpoint_xfer_int(const struct usb_endpoint_descriptor *epd)
+{
+       return ((epd->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) ==
+               USB_ENDPOINT_XFER_INT);
+}
+
+
+/**
+ * usb_endpoint_is_int_in - check if the endpoint is interrupt IN
+ * @epd: endpoint to be checked
+ *
+ * Returns true if the endpoint has interrupt transfer type and IN direction,
+ * otherwise it returns false.
+ */
+
+static inline int usb_endpoint_is_int_in(const struct usb_endpoint_descriptor *epd)
+{
+       return (usb_endpoint_xfer_int(epd) && usb_endpoint_dir_in(epd));
+}
+
+/**
+ * usb_endpoint_is_int_out - check if the endpoint is interrupt OUT
+ * @epd: endpoint to be checked
+ *
+ * Returns true if the endpoint has interrupt transfer type and OUT direction,
+ * otherwise it returns false.
+ */
+
+static inline int usb_endpoint_is_int_out(const struct usb_endpoint_descriptor *epd)
+{
+       return (usb_endpoint_xfer_int(epd) && usb_endpoint_dir_out(epd));
+}
+
+#endif /* older kernel versions */
diff --git a/drivers/staging/frontier/surface_sysfs.h b/drivers/staging/frontier/surface_sysfs.h
new file mode 100644
index 000000000000..d50a562d658a
--- /dev/null
+++ b/drivers/staging/frontier/surface_sysfs.h
@@ -0,0 +1,100 @@
+/* If you are going to abuse the preprocessor, why not ABUSE the preprocessor?
+   I stuck this header in a separate file so I don't have to look at it */
+
+// FIXME Need locking or atomic ops
+
+#define show_set_mbit(dname,value,bit)																			\
+static ssize_t show_##value(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	int temp = (1 && (t->value & (1 << bit)));					\
+	return sprintf(buf, "%d\n", temp);			\
+}									\
+static ssize_t set_##value(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+	if(temp > 0) { long b = 1 << bit; t->value |= b; } \
+	else { long b = ~(1 << bit); t->value &= b ;				 \
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
+
+#define show_set_ebit(dname,enumname,value,bit)																			\
+static ssize_t show_##bit(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+  enum enumname l = bit; \
+  int temp = t->value & (1 << l);						\
+	return sprintf(buf, "%d\n", temp);			\
+}									\
+static ssize_t set_##bit(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+  enum enumname l = bit;\
+	long b  = 1 << l; \
+	if(temp > 0) { t->value |= b; }	\
+	else { t->value &= ~b ;				\
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
+
+// FIXME FOR CORRECTLY SETTING HEX from a string
+#define show_set_mcmd(dname,value)																			\
+static ssize_t show_##value(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+  int count = 0;\
+	int i; \
+	for (i = 0,i<sizeof(dname); i++) count += snprintf(buf, "%02x",t->dname[i]); \
+  return(count);\
+}									\
+static ssize_t set_##value(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+	t->value = temp;						\
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
+
+#define show_set_mint(dname,value)				\
+static ssize_t show_##value(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	return sprintf(buf, "%d\n", t->value);			\
+}									\
+static ssize_t set_##value(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+	t->value = temp;						\
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
+
+#define show_set_mchar(dname,value)																				\
+static ssize_t show_##value(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	return sprintf(buf, "%c\n", t->value);			\
+}									\
+static ssize_t set_##value(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+	t->value = temp;						\
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
diff --git a/drivers/staging/frontier/tranzport.c b/drivers/staging/frontier/tranzport.c
new file mode 100644
index 000000000000..79abb6b16f74
--- /dev/null
+++ b/drivers/staging/frontier/tranzport.c
@@ -0,0 +1,1006 @@
+/*
+ * Frontier Designs Tranzport driver
+ *
+ * Copyright (C) 2007 Michael Taht (m@taht.net)
+ *
+ * Based on the usbled driver and ldusb drivers by
+ *
+ * Copyright (C) 2004 Greg Kroah-Hartman (greg@kroah.com)
+ * Copyright (C) 2005 Michael Hund <mhund@ld-didactic.de>
+ *
+ * The ldusb driver was, in turn, derived from Lego USB Tower driver
+ * Copyright (C) 2003 David Glance <advidgsf@sourceforge.net>
+ *		 2001-2004 Juergen Stuber <starblue@users.sourceforge.net>
+ *
+ *	This program is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU General Public License as
+ *	published by the Free Software Foundation, version 2.
+ *
+ */
+
+/**
+ * This driver uses a ring buffer for time critical reading of
+ * interrupt in reports and provides read and write methods for
+ * raw interrupt reports.
+ */
+
+/* Note: this currently uses a dumb ringbuffer for reads and writes.
+ * A more optimal driver would cache and kill off outstanding urbs that are
+ * now invalid, and ignore ones that already were in the queue but valid
+ * as we only have 17 commands for the tranzport. In particular this is
+ * key for getting lights to flash in time as otherwise many commands
+ * can be buffered up before the light change makes it to the interface.
+*/
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/version.h>
+
+#include <asm/uaccess.h>
+#include <linux/input.h>
+#include <linux/usb.h>
+#include <linux/poll.h>
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 19)
+#include frontier_compat.h
+#endif
+
+/* Define these values to match your devices */
+#define VENDOR_ID   0x165b
+#define PRODUCT_ID	0x8101
+
+#ifdef CONFIG_USB_DYNAMIC_MINORS
+#define USB_TRANZPORT_MINOR_BASE	0
+#else
+// FIXME 176 - is the ldusb driver's minor - apply for a minor soon
+#define USB_TRANZPORT_MINOR_BASE	177
+#endif
+
+/* table of devices that work with this driver */
+static struct usb_device_id usb_tranzport_table [] = {
+	{ USB_DEVICE(VENDOR_ID, PRODUCT_ID) },
+	{ }					/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, usb_tranzport_table);
+MODULE_VERSION("0.33");
+MODULE_AUTHOR("Mike Taht <m@taht.net>");
+MODULE_DESCRIPTION("Tranzport USB Driver");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("Frontier Designs Tranzport Control Surface");
+
+/* These two aren't done yet */
+
+#define SUPPRESS_EXTRA_ONLINE_EVENTS 0
+#define BUFFERED_WRITES 0
+
+#define SUPPRESS_EXTRA_OFFLINE_EVENTS 1
+#define COMPRESS_WHEEL_EVENTS 1
+#define BUFFERED_READS 1
+#define RING_BUFFER_SIZE 1000
+#define WRITE_BUFFER_SIZE 34
+#define TRANZPORT_USB_TIMEOUT 10
+
+
+static int debug = 0;
+
+/* Use our own dbg macro */
+#define dbg_info(dev, format, arg...) do { if (debug) dev_info(dev , format , ## arg); } while (0)
+
+/* Module parameters */
+
+module_param(debug, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Debug enabled or not");
+
+/* All interrupt in transfers are collected in a ring buffer to
+ * avoid racing conditions and get better performance of the driver.
+ */
+
+static int ring_buffer_size = RING_BUFFER_SIZE;
+
+module_param(ring_buffer_size, int,  S_IRUGO);
+MODULE_PARM_DESC(ring_buffer_size, "Read ring buffer size in reports");
+
+/* The write_buffer can one day contain more than one interrupt out transfer.
+ */
+static int write_buffer_size = WRITE_BUFFER_SIZE;
+module_param(write_buffer_size, int,  S_IRUGO);
+MODULE_PARM_DESC(write_buffer_size, "Write buffer size");
+
+/*
+ * Increase the interval for debugging purposes.
+ * or set to 1 to use the standard interval from the endpoint descriptors.
+ */
+
+static int min_interrupt_in_interval = TRANZPORT_USB_TIMEOUT;
+module_param(min_interrupt_in_interval, int, 0);
+MODULE_PARM_DESC(min_interrupt_in_interval, "Minimum interrupt in interval in ms");
+
+static int min_interrupt_out_interval = TRANZPORT_USB_TIMEOUT;
+module_param(min_interrupt_out_interval, int, 0);
+MODULE_PARM_DESC(min_interrupt_out_interval, "Minimum interrupt out interval in ms");
+
+struct tranzport_cmd {
+    unsigned char cmd[8];
+};
+
+enum LightID {
+	LightRecord = 0,
+	LightTrackrec,
+	LightTrackmute,
+	LightTracksolo,
+	LightAnysolo,
+	LightLoop,
+	LightPunch
+	};
+
+/* Structure to hold all of our device specific stuff */
+
+struct usb_tranzport {
+	struct semaphore	sem;		/* locks this structure */
+	struct usb_interface*	intf;		/* save off the usb interface pointer */
+
+	int			open_count;	/* number of times this port has been opened */
+
+	struct tranzport_cmd	(*ring_buffer)[RING_BUFFER_SIZE]; /* just make c happy */
+	unsigned int		ring_head;
+	unsigned int		ring_tail;
+
+	wait_queue_head_t	read_wait;
+	wait_queue_head_t	write_wait;
+
+	unsigned char*		interrupt_in_buffer;
+	struct usb_endpoint_descriptor* interrupt_in_endpoint;
+	struct urb*		interrupt_in_urb;
+	int			interrupt_in_interval;
+	size_t	interrupt_in_endpoint_size;
+	int			interrupt_in_running;
+	int			interrupt_in_done;
+
+	char*			interrupt_out_buffer;
+	struct usb_endpoint_descriptor* interrupt_out_endpoint;
+	struct urb*		interrupt_out_urb;
+	int			interrupt_out_interval;
+	size_t	interrupt_out_endpoint_size;
+	int			interrupt_out_busy;
+
+	/* Sysfs and translation support */
+
+	int event; /* alternate interface to events */
+	int wheel; /* - for negative, 0 for none, + for positive */
+	unsigned char dump_state; /* 0 if disabled 1 if enabled */
+	unsigned char enable; /* 0 if disabled 1 if enabled */
+	unsigned char offline; /* if the device is out of range or asleep */
+	unsigned char compress_wheel; /* flag to compress wheel events */
+	unsigned char light; /* 7 bits used */
+	unsigned char last_cmd[8];
+	unsigned char last_input[8];
+	unsigned char screen[40]; // We'll also have cells
+
+};
+
+/* prevent races between open() and disconnect() */
+static DEFINE_MUTEX(disconnect_mutex);
+
+static struct usb_driver usb_tranzport_driver;
+
+/**
+ *	usb_tranzport_abort_transfers
+ *      aborts transfers and frees associated data structures
+ */
+static void usb_tranzport_abort_transfers(struct usb_tranzport *dev)
+{
+	/* shutdown transfer */
+	if (dev->interrupt_in_running) {
+		dev->interrupt_in_running = 0;
+		if (dev->intf)
+			usb_kill_urb(dev->interrupt_in_urb);
+	}
+	if (dev->interrupt_out_busy)
+		if (dev->intf)
+			usb_kill_urb(dev->interrupt_out_urb);
+}
+
+// FIXME ~light not good enough or correct - need atomic set_bit
+
+#define show_set_light(value)	\
+static ssize_t show_##value(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_tranzport *t = usb_get_intfdata(intf);		\
+	enum LightID light = value;					\
+	int temp = (1 && (t->light & (1 << light)));			\
+	return sprintf(buf, "%d\n", temp );				\
+}									\
+static ssize_t set_##value(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_tranzport *t = usb_get_intfdata(intf);		\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+	enum LightID light = (temp << value) & (t->light << value);	\
+	t->light = (t->light & ~light) ;				\
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
+
+show_set_light(LightRecord);
+show_set_light(LightTrackrec);
+show_set_light(LightTrackmute);
+show_set_light(LightTracksolo);
+show_set_light(LightAnysolo);
+show_set_light(LightLoop);
+show_set_light(LightPunch);
+
+
+#define show_set_int(value)	\
+static ssize_t show_##value(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_tranzport *t = usb_get_intfdata(intf);			\
+									\
+	return sprintf(buf, "%d\n", t->value);			\
+}									\
+static ssize_t set_##value(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_tranzport *t = usb_get_intfdata(intf);			\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+									\
+	t->value = temp;						\
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
+
+show_set_int(enable);
+show_set_int(offline);
+show_set_int(compress_wheel);
+show_set_int(dump_state);
+show_set_int(wheel);
+show_set_int(event);
+
+#define show_set_cmd(value)	\
+static ssize_t show_##value(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_tranzport *t = usb_get_intfdata(intf);			\
+									\
+	return sprintf(buf, "%d\n", t->value);			\
+}									\
+static ssize_t set_##value(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_tranzport *t = usb_get_intfdata(intf);			\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+									\
+	t->value = temp;						\
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
+
+
+
+
+/**
+ *	usb_tranzport_delete
+ */
+static void usb_tranzport_delete(struct usb_tranzport *dev)
+{
+	usb_tranzport_abort_transfers(dev);
+	/*  This is just too twisted to be correct */
+	if(dev->intf != NULL) {
+	device_remove_file(&dev->intf->dev, &dev_attr_LightRecord);
+	device_remove_file(&dev->intf->dev, &dev_attr_LightTrackrec);
+	device_remove_file(&dev->intf->dev, &dev_attr_LightTrackmute);
+	device_remove_file(&dev->intf->dev, &dev_attr_LightTracksolo);
+	device_remove_file(&dev->intf->dev, &dev_attr_LightTrackmute);
+	device_remove_file(&dev->intf->dev, &dev_attr_LightAnysolo);
+	device_remove_file(&dev->intf->dev, &dev_attr_LightLoop);
+	device_remove_file(&dev->intf->dev, &dev_attr_LightPunch);
+	device_remove_file(&dev->intf->dev, &dev_attr_wheel);
+	device_remove_file(&dev->intf->dev, &dev_attr_enable);
+	device_remove_file(&dev->intf->dev, &dev_attr_event);
+	device_remove_file(&dev->intf->dev, &dev_attr_offline);
+	device_remove_file(&dev->intf->dev, &dev_attr_compress_wheel);
+
+	device_remove_file(&dev->intf->dev, &dev_attr_dump_state);
+	}
+
+	/* free data structures */
+	usb_free_urb(dev->interrupt_in_urb);
+	usb_free_urb(dev->interrupt_out_urb);
+	kfree(dev->ring_buffer);
+	kfree(dev->interrupt_in_buffer);
+	kfree(dev->interrupt_out_buffer);
+	kfree(dev);
+}
+
+/**
+ *	usb_tranzport_interrupt_in_callback
+ */
+
+static void usb_tranzport_interrupt_in_callback(struct urb *urb)
+{
+	struct usb_tranzport *dev = urb->context;
+	unsigned int next_ring_head;
+	int retval = -1;
+
+	if (urb->status) {
+		if (urb->status == -ENOENT ||
+		    urb->status == -ECONNRESET ||
+		    urb->status == -ESHUTDOWN) {
+			goto exit;
+		} else {
+			dbg_info(&dev->intf->dev, "%s: nonzero status received: %d\n",
+				 __func__, urb->status);
+			goto resubmit; /* maybe we can recover */
+		}
+	}
+
+	if (urb->actual_length != 8) {
+		dev_warn(&dev->intf->dev,
+			 "Urb length was %d bytes!! Do something intelligent \n", urb->actual_length);
+	} else {
+		dbg_info(&dev->intf->dev, "%s: received: %02x%02x%02x%02x%02x%02x%02x%02x\n",
+			 __func__, dev->interrupt_in_buffer[0],dev->interrupt_in_buffer[1],dev->interrupt_in_buffer[2],dev->interrupt_in_buffer[3],dev->interrupt_in_buffer[4],dev->interrupt_in_buffer[5],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 == 1 && dev->interrupt_in_buffer[1] == 0xff) { dev->offline = 2; goto resubmit; }
+
+/* Always pass one offline event up the stack */
+		if(dev->offline > 0 && dev->interrupt_in_buffer[1] != 0xff) { dev->offline = 0; }
+		if(dev->offline == 0 && dev->interrupt_in_buffer[1] == 0xff) { dev->offline = 1; }
+
+#endif
+		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;
+
+		if (next_ring_head != dev->ring_tail) {
+			memcpy(&((*dev->ring_buffer)[dev->ring_head]), dev->interrupt_in_buffer, urb->actual_length);
+			dev->ring_head = next_ring_head;
+			retval = 0;
+			memset(dev->interrupt_in_buffer, 0, urb->actual_length);
+		} else {
+			dev_warn(&dev->intf->dev,
+				 "Ring buffer overflow, %d bytes dropped\n",
+				 urb->actual_length);
+			memset(dev->interrupt_in_buffer, 0, urb->actual_length);
+		}
+	}
+
+resubmit:
+	/* resubmit if we're still running */
+	if (dev->interrupt_in_running && dev->intf) {
+		retval = usb_submit_urb(dev->interrupt_in_urb, GFP_ATOMIC);
+		if (retval)
+			dev_err(&dev->intf->dev,
+				"usb_submit_urb failed (%d)\n", retval);
+	}
+
+exit:
+	dev->interrupt_in_done = 1;
+	wake_up_interruptible(&dev->read_wait);
+}
+
+/**
+ *	usb_tranzport_interrupt_out_callback
+ */
+static void usb_tranzport_interrupt_out_callback(struct urb *urb)
+{
+	struct usb_tranzport *dev = urb->context;
+
+	/* sync/async unlink faults aren't errors */
+	if (urb->status && !(urb->status == -ENOENT ||
+			     urb->status == -ECONNRESET ||
+			     urb->status == -ESHUTDOWN))
+		dbg_info(&dev->intf->dev,
+			 "%s - nonzero write interrupt status received: %d\n",
+			 __func__, urb->status);
+
+	dev->interrupt_out_busy = 0;
+	wake_up_interruptible(&dev->write_wait);
+}
+
+/**
+ *	usb_tranzport_open
+ */
+static int usb_tranzport_open(struct inode *inode, struct file *file)
+{
+	struct usb_tranzport *dev;
+	int subminor;
+	int retval = 0;
+	struct usb_interface *interface;
+
+	nonseekable_open(inode, file);
+	subminor = iminor(inode);
+
+	mutex_lock(&disconnect_mutex);
+
+	interface = usb_find_interface(&usb_tranzport_driver, subminor);
+
+	if (!interface) {
+		err("%s - error, can't find device for minor %d\n",
+		     __func__, subminor);
+		retval = -ENODEV;
+		goto unlock_disconnect_exit;
+	}
+
+	dev = usb_get_intfdata(interface);
+
+	if (!dev) {
+		retval = -ENODEV;
+		goto unlock_disconnect_exit;
+	}
+
+	/* lock this device */
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto unlock_disconnect_exit;
+	}
+
+	/* allow opening only once */
+	if (dev->open_count) {
+		retval = -EBUSY;
+		goto unlock_exit;
+	}
+	dev->open_count = 1;
+
+	/* initialize in direction */
+	dev->ring_head = 0;
+	dev->ring_tail = 0;
+	usb_fill_int_urb(dev->interrupt_in_urb,
+			 interface_to_usbdev(interface),
+			 usb_rcvintpipe(interface_to_usbdev(interface),
+					dev->interrupt_in_endpoint->bEndpointAddress),
+			 dev->interrupt_in_buffer,
+			 dev->interrupt_in_endpoint_size,
+			 usb_tranzport_interrupt_in_callback,
+			 dev,
+			 dev->interrupt_in_interval);
+
+	dev->interrupt_in_running = 1;
+	dev->interrupt_in_done = 0;
+	dev->enable = 1;
+	dev->offline = 0;
+	dev->compress_wheel = 1;
+
+	retval = usb_submit_urb(dev->interrupt_in_urb, GFP_KERNEL);
+	if (retval) {
+		dev_err(&interface->dev, "Couldn't submit interrupt_in_urb %d\n", retval);
+		dev->interrupt_in_running = 0;
+		dev->open_count = 0;
+		goto unlock_exit;
+	}
+
+	/* save device in the file's private structure */
+	file->private_data = dev;
+
+
+unlock_exit:
+	up(&dev->sem);
+
+unlock_disconnect_exit:
+	mutex_unlock(&disconnect_mutex);
+
+	return retval;
+}
+
+/**
+ *	usb_tranzport_release
+ */
+static int usb_tranzport_release(struct inode *inode, struct file *file)
+{
+	struct usb_tranzport *dev;
+	int retval = 0;
+
+	dev = file->private_data;
+
+	if (dev == NULL) {
+		retval = -ENODEV;
+		goto exit;
+	}
+
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto exit;
+	}
+
+	if (dev->open_count != 1) {
+		retval = -ENODEV;
+		goto unlock_exit;
+	}
+
+	if (dev->intf == NULL) {
+		/* the device was unplugged before the file was released */
+		up(&dev->sem);
+		/* unlock here as usb_tranzport_delete frees dev */
+		usb_tranzport_delete(dev);
+		retval = -ENODEV;
+		goto exit;
+	}
+
+	/* wait until write transfer is finished */
+	if (dev->interrupt_out_busy)
+		wait_event_interruptible_timeout(dev->write_wait, !dev->interrupt_out_busy, 2 * HZ);
+	usb_tranzport_abort_transfers(dev);
+	dev->open_count = 0;
+
+unlock_exit:
+	up(&dev->sem);
+
+exit:
+	return retval;
+}
+
+/**
+ *	usb_tranzport_poll
+ */
+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);
+
+	if (dev->ring_head != dev->ring_tail)
+		mask |= POLLIN | POLLRDNORM;
+	if (!dev->interrupt_out_busy)
+		mask |= POLLOUT | POLLWRNORM;
+
+	return mask;
+}
+
+/**
+ *	usb_tranzport_read
+ */
+static ssize_t usb_tranzport_read(struct file *file, char __user *buffer, size_t count,
+			   loff_t *ppos)
+{
+	struct usb_tranzport *dev;
+	int retval = 0;
+
+#if BUFFERED_READS
+	int c = 0;
+#endif
+
+#if COMPRESS_WHEEL_EVENTS
+	signed char oldwheel;
+	signed char newwheel;
+	int cancompress = 1;
+	int next_tail;
+#endif
+
+/* do I have such a thing as a null event? */
+
+	dev = file->private_data;
+
+	/* verify that we actually have some data to read */
+	if (count == 0)
+		goto exit;
+
+	/* lock this object */
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto exit;
+	}
+
+	/* verify that the device wasn't unplugged */
+	if (dev->intf == NULL) {
+		retval = -ENODEV;
+		err("No device or device unplugged %d\n", retval);
+		goto unlock_exit;
+	}
+
+	while (dev->ring_head == dev->ring_tail) {
+
+		if (file->f_flags & O_NONBLOCK) {
+			retval = -EAGAIN;
+			goto unlock_exit;
+		}
+		// atomic_cmp_exchange(&dev->interrupt_in_done,0,0);
+		dev->interrupt_in_done = 0 ; /* tiny race - FIXME: make atomic? */
+		retval = wait_event_interruptible(dev->read_wait, dev->interrupt_in_done);
+		if (retval < 0) {
+			goto unlock_exit;
+		}
+	}
+
+	dbg_info(&dev->intf->dev, "%s: copying to userspace: %02x%02x%02x%02x%02x%02x%02x%02x\n",
+			 __func__, (*dev->ring_buffer)[dev->ring_tail].cmd[0],(*dev->ring_buffer)[dev->ring_tail].cmd[1],(*dev->ring_buffer)[dev->ring_tail].cmd[2],(*dev->ring_buffer)[dev->ring_tail].cmd[3],(*dev->ring_buffer)[dev->ring_tail].cmd[4],(*dev->ring_buffer)[dev->ring_tail].cmd[5],(*dev->ring_buffer)[dev->ring_tail].cmd[6],(*dev->ring_buffer)[dev->ring_tail].cmd[7]);
+
+#if BUFFERED_READS
+	   c = 0;
+	   while((c < count) && (dev->ring_tail != dev->ring_head)) {
+
+/* This started off in the lower level service routine, and I moved it here. Then my brain died. Not done yet. */
+#if COMPRESS_WHEEL_EVENTS
+		next_tail = (dev->ring_tail+1) % ring_buffer_size;
+		if(dev->compress_wheel) cancompress = 1;
+		while(dev->ring_head != next_tail && cancompress == 1 ) {
+			newwheel = (*dev->ring_buffer)[next_tail].cmd[6];
+			oldwheel = (*dev->ring_buffer)[dev->ring_tail].cmd[6];
+			// if both are wheel events, and no buttons have changes (FIXME, do I have to check?),
+			// and we are the same sign, we can compress +- 7F
+			// FIXME: saner check for overflow! - max of +- 7F
+			// FIXME the math is wrong for going in reverse, actually, as the midi spec doesn't allow signed chars
+
+	dbg_info(&dev->intf->dev, "%s: trying to compress: %02x%02x%02x%02x%02x %02x %02x %02x\n",
+			 __func__, (*dev->ring_buffer)[dev->ring_tail].cmd[0],(*dev->ring_buffer)[dev->ring_tail].cmd[1],(*dev->ring_buffer)[dev->ring_tail].cmd[2],(*dev->ring_buffer)[dev->ring_tail].cmd[3],(*dev->ring_buffer)[dev->ring_tail].cmd[4],(*dev->ring_buffer)[dev->ring_tail].cmd[5],(*dev->ring_buffer)[dev->ring_tail].cmd[6],(*dev->ring_buffer)[dev->ring_tail].cmd[7]);
+
+
+			if(((*dev->ring_buffer)[dev->ring_tail].cmd[6] != 0 &&
+			    (*dev->ring_buffer)[next_tail].cmd[6] != 0 ) &&
+				((newwheel > 0 && oldwheel > 0) ||
+				(newwheel < 0 && oldwheel < 0)) &&
+				((*dev->ring_buffer)[dev->ring_tail].cmd[2] == (*dev->ring_buffer)[next_tail].cmd[2]) &&
+				((*dev->ring_buffer)[dev->ring_tail].cmd[3] == (*dev->ring_buffer)[next_tail].cmd[3]) &&
+				((*dev->ring_buffer)[dev->ring_tail].cmd[4] == (*dev->ring_buffer)[next_tail].cmd[4]) &&
+				((*dev->ring_buffer)[dev->ring_tail].cmd[5] == (*dev->ring_buffer)[next_tail].cmd[5]))
+ {
+	dbg_info(&dev->intf->dev, "%s: should compress: %02x%02x%02x%02x%02x%02x%02x%02x\n",
+			 __func__, (*dev->ring_buffer)[dev->ring_tail].cmd[0],(*dev->ring_buffer)[dev->ring_tail].cmd[1],(*dev->ring_buffer)[dev->ring_tail].cmd[2],(*dev->ring_buffer)[dev->ring_tail].cmd[3],(*dev->ring_buffer)[dev->ring_tail].cmd[4],(*dev->ring_buffer)[dev->ring_tail].cmd[5],(*dev->ring_buffer)[dev->ring_tail].cmd[6],(*dev->ring_buffer)[dev->ring_tail].cmd[7]);
+
+				newwheel += oldwheel;
+				if(oldwheel > 0 && !(newwheel > 0)) {
+					newwheel = 0x7f;
+					cancompress = 0;
+				}
+				if(oldwheel < 0 && !(newwheel < 0)) {
+					newwheel = 0x80;
+					cancompress = 0;
+				}
+
+				(*dev->ring_buffer)[next_tail].cmd[6] = newwheel;
+				dev->ring_tail = next_tail;
+				next_tail = (dev->ring_tail+1) % ring_buffer_size;
+			} else {
+				cancompress = 0;
+			}
+		}
+#endif /* COMPRESS_WHEEL_EVENTS */
+
+		if (copy_to_user(&buffer[c], &(*dev->ring_buffer)[dev->ring_tail], 8)) {
+			retval = -EFAULT;
+			goto unlock_exit;
+		}
+
+		dev->ring_tail = (dev->ring_tail+1) % ring_buffer_size;
+		c+=8;
+		dbg_info(&dev->intf->dev, "%s: head, tail are %x, %x\n", __func__,dev->ring_head,dev->ring_tail);
+	   }
+	   retval = c;
+
+#else
+	if (copy_to_user(buffer, &(*dev->ring_buffer)[dev->ring_tail], 8)) {
+		retval = -EFAULT;
+		goto unlock_exit;
+	}
+
+	dev->ring_tail = (dev->ring_tail+1) % ring_buffer_size;
+	dbg_info(&dev->intf->dev, "%s: head, tail are %x, %x\n", __func__,dev->ring_head,dev->ring_tail);
+
+	retval = 8;
+#endif /* BUFFERED_READS */
+
+unlock_exit:
+	/* unlock the device */
+	up(&dev->sem);
+
+exit:
+	return retval;
+}
+
+/**
+ *	usb_tranzport_write
+ */
+static ssize_t usb_tranzport_write(struct file *file, const char __user *buffer,
+			    size_t count, loff_t *ppos)
+{
+	struct usb_tranzport *dev;
+	size_t bytes_to_write;
+	int retval = 0;
+
+	dev = file->private_data;
+
+	/* verify that we actually have some data to write */
+	if (count == 0)
+		goto exit;
+
+	/* lock this object */
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto exit;
+	}
+
+	/* verify that the device wasn't unplugged */
+	if (dev->intf == NULL) {
+		retval = -ENODEV;
+		err("No device or device unplugged %d\n", retval);
+		goto unlock_exit;
+	}
+
+	/* wait until previous transfer is finished */
+	if (dev->interrupt_out_busy) {
+		if (file->f_flags & O_NONBLOCK) {
+			retval = -EAGAIN;
+			goto unlock_exit;
+		}
+		retval = wait_event_interruptible(dev->write_wait, !dev->interrupt_out_busy);
+		if (retval < 0) {
+			goto unlock_exit;
+		}
+	}
+
+	/* write the data into interrupt_out_buffer from userspace */
+	bytes_to_write = min(count, write_buffer_size*dev->interrupt_out_endpoint_size);
+	if (bytes_to_write < count)
+		dev_warn(&dev->intf->dev, "Write buffer overflow, %zd bytes dropped\n",count-bytes_to_write);
+
+	dbg_info(&dev->intf->dev, "%s: count = %zd, bytes_to_write = %zd\n", __func__, count, bytes_to_write);
+
+	if (copy_from_user(dev->interrupt_out_buffer, buffer, bytes_to_write)) {
+		retval = -EFAULT;
+		goto unlock_exit;
+	}
+
+	if (dev->interrupt_out_endpoint == NULL) {
+		err("Endpoint should not be be null! \n");
+		goto unlock_exit;
+	}
+
+	/* send off the urb */
+	usb_fill_int_urb(dev->interrupt_out_urb,
+			 interface_to_usbdev(dev->intf),
+			 usb_sndintpipe(interface_to_usbdev(dev->intf),
+					dev->interrupt_out_endpoint->bEndpointAddress),
+			 dev->interrupt_out_buffer,
+			 bytes_to_write,
+			 usb_tranzport_interrupt_out_callback,
+			 dev,
+			 dev->interrupt_out_interval);
+
+	dev->interrupt_out_busy = 1;
+	wmb();
+
+	retval = usb_submit_urb(dev->interrupt_out_urb, GFP_KERNEL);
+	if (retval) {
+		dev->interrupt_out_busy = 0;
+		err("Couldn't submit interrupt_out_urb %d\n", retval);
+		goto unlock_exit;
+	}
+	retval = bytes_to_write;
+
+unlock_exit:
+	/* unlock the device */
+	up(&dev->sem);
+
+exit:
+	return retval;
+}
+
+/* file operations needed when we register this driver */
+static const struct file_operations usb_tranzport_fops = {
+	.owner =	THIS_MODULE,
+	.read  =	usb_tranzport_read,
+	.write =	usb_tranzport_write,
+	.open =		usb_tranzport_open,
+	.release =	usb_tranzport_release,
+	.poll =		usb_tranzport_poll,
+};
+
+/*
+ * 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 usb_tranzport_class = {
+	.name =		"tranzport%d",
+	.fops =		&usb_tranzport_fops,
+	.minor_base =	USB_TRANZPORT_MINOR_BASE,
+};
+
+
+/**
+ *	usb_tranzport_probe
+ *
+ *	Called by the usb core when a new device is connected that it thinks
+ *	this driver might be interested in.
+ */
+static int usb_tranzport_probe(struct usb_interface *intf, const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct usb_tranzport *dev = NULL;
+	struct usb_host_interface *iface_desc;
+	struct usb_endpoint_descriptor *endpoint;
+	int i;
+	int true_size;
+	int retval = -ENOMEM;
+
+	/* allocate memory for our device state and intialize it */
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (dev == NULL) {
+		dev_err(&intf->dev, "Out of memory\n");
+		goto exit;
+	}
+	init_MUTEX(&dev->sem);
+	dev->intf = intf;
+	init_waitqueue_head(&dev->read_wait);
+	init_waitqueue_head(&dev->write_wait);
+
+	iface_desc = intf->cur_altsetting;
+
+	/* set up the endpoint information */
+	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+		endpoint = &iface_desc->endpoint[i].desc;
+
+		if (usb_endpoint_is_int_in(endpoint))
+			dev->interrupt_in_endpoint = endpoint;
+
+		if (usb_endpoint_is_int_out(endpoint))
+			dev->interrupt_out_endpoint = endpoint;
+	}
+	if (dev->interrupt_in_endpoint == NULL) {
+		dev_err(&intf->dev, "Interrupt in endpoint not found\n");
+		goto error;
+	}
+	if (dev->interrupt_out_endpoint == NULL)
+		dev_warn(&intf->dev, "Interrupt out endpoint not found (using control endpoint instead)\n");
+
+
+	dev->interrupt_in_endpoint_size = le16_to_cpu(dev->interrupt_in_endpoint->wMaxPacketSize);
+
+	if (dev->interrupt_in_endpoint_size != 8)
+	    dev_warn(&intf->dev, "Interrupt in endpoint size is not 8!\n");
+
+	if(ring_buffer_size == 0) { ring_buffer_size = RING_BUFFER_SIZE; }
+	true_size = min(ring_buffer_size,RING_BUFFER_SIZE);
+	/* FIXME - there are more usb_alloc routines for dma correctness. Needed? */
+
+	dev->ring_buffer = kmalloc((true_size*sizeof(struct tranzport_cmd))+8, GFP_KERNEL);
+
+	if (!dev->ring_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate ring_buffer of size %d\n",true_size);
+		goto error;
+	}
+	dev->interrupt_in_buffer = kmalloc(dev->interrupt_in_endpoint_size, GFP_KERNEL);
+	if (!dev->interrupt_in_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_in_buffer\n");
+		goto error;
+	}
+	dev->interrupt_in_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!dev->interrupt_in_urb) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_in_urb\n");
+		goto error;
+	}
+	dev->interrupt_out_endpoint_size = dev->interrupt_out_endpoint ? le16_to_cpu(dev->interrupt_out_endpoint->wMaxPacketSize) :
+									 udev->descriptor.bMaxPacketSize0;
+
+	if (dev->interrupt_out_endpoint_size !=8)
+		dev_warn(&intf->dev, "Interrupt out endpoint size is not 8!)\n");
+
+	dev->interrupt_out_buffer = kmalloc(write_buffer_size*dev->interrupt_out_endpoint_size, GFP_KERNEL);
+	if (!dev->interrupt_out_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_out_buffer\n");
+		goto error;
+	}
+	dev->interrupt_out_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!dev->interrupt_out_urb) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_out_urb\n");
+		goto error;
+	}
+	dev->interrupt_in_interval = min_interrupt_in_interval > dev->interrupt_in_endpoint->bInterval ? min_interrupt_in_interval : dev->interrupt_in_endpoint->bInterval;
+	if (dev->interrupt_out_endpoint)
+		dev->interrupt_out_interval = min_interrupt_out_interval > dev->interrupt_out_endpoint->bInterval ? min_interrupt_out_interval : dev->interrupt_out_endpoint->bInterval;
+
+	/* we can register the device now, as it is ready */
+	usb_set_intfdata(intf, dev);
+
+	retval = usb_register_dev(intf, &usb_tranzport_class);
+	if (retval) {
+		/* something prevented us from registering this driver */
+		dev_err(&intf->dev, "Not able to get a minor for this device.\n");
+		usb_set_intfdata(intf, NULL);
+		goto error;
+	}
+
+	if((retval = device_create_file(&intf->dev, &dev_attr_LightRecord))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_LightTrackrec))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_LightTrackmute))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_LightTracksolo))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_LightAnysolo))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_LightLoop))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_LightPunch))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_wheel))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_event))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_dump_state))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_compress_wheel))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_enable))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_offline))) goto error;
+
+	/* let the user know what node this device is now attached to */
+	dev_info(&intf->dev, "Tranzport Device #%d now attached to major %d minor %d\n",
+		(intf->minor - USB_TRANZPORT_MINOR_BASE), USB_MAJOR, intf->minor);
+
+exit:
+	return retval;
+
+error:
+	usb_tranzport_delete(dev);
+
+	return retval;
+}
+
+/**
+ *	usb_tranzport_disconnect
+ *
+ *	Called by the usb core when the device is removed from the system.
+ */
+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);
+	down(&dev->sem);
+	minor = intf->minor;
+	/* give back our minor */
+	usb_deregister_dev(intf, &usb_tranzport_class);
+
+	/* if the device is not opened, then we clean up right now */
+	if (!dev->open_count) {
+		up(&dev->sem);
+		usb_tranzport_delete(dev);
+	} else {
+		dev->intf = NULL;
+		up(&dev->sem);
+	}
+
+	mutex_unlock(&disconnect_mutex);
+
+	dev_info(&intf->dev, "Tranzport Surface #%d now disconnected\n",
+		 (minor - USB_TRANZPORT_MINOR_BASE));
+}
+
+/* usb specific object needed to register this driver with the usb subsystem */
+static struct usb_driver usb_tranzport_driver = {
+	.name =		"tranzport",
+	.probe =	usb_tranzport_probe,
+	.disconnect =	usb_tranzport_disconnect,
+	.id_table =	usb_tranzport_table,
+};
+
+/**
+ *	usb_tranzport_init
+ */
+static int __init usb_tranzport_init(void)
+{
+	int retval;
+
+	/* register this driver with the USB subsystem */
+	retval = usb_register(&usb_tranzport_driver);
+	if (retval)
+		err("usb_register failed for the "__FILE__" driver. Error number %d\n", retval);
+
+	return retval;
+}
+
+/**
+ *	usb_tranzport_exit
+ */
+static void __exit usb_tranzport_exit(void)
+{
+	/* deregister this driver with the USB subsystem */
+	usb_deregister(&usb_tranzport_driver);
+}
+
+module_init(usb_tranzport_init);
+module_exit(usb_tranzport_exit);
+
diff --git a/drivers/staging/go7007/Kconfig b/drivers/staging/go7007/Kconfig
index 593fdb767aad..f2cf7f66ae05 100644
--- a/drivers/staging/go7007/Kconfig
+++ b/drivers/staging/go7007/Kconfig
@@ -25,3 +25,13 @@ config VIDEO_GO7007_USB
 	  To compile this driver as a module, choose M here: the
 	  module will be called go7007-usb
 
+config VIDEO_GO7007_USB_S2250_BOARD
+	tristate "Sensoray 2250/2251 support"
+	depends on VIDEO_GO7007_USB && DVB_USB
+	default N
+	---help---
+	  This is a video4linux driver for the Sensoray 2250/2251 device
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called s2250-board
+
diff --git a/drivers/staging/go7007/Makefile b/drivers/staging/go7007/Makefile
index 9b9310cae1ce..e514b4af6d06 100644
--- a/drivers/staging/go7007/Makefile
+++ b/drivers/staging/go7007/Makefile
@@ -5,14 +5,23 @@
 
 obj-$(CONFIG_VIDEO_GO7007) += go7007.o
 obj-$(CONFIG_VIDEO_GO7007_USB) += go7007-usb.o
+obj-$(CONFIG_VIDEO_GO7007_USB_S2250_BOARD) += s2250.o
 
-go7007-objs += go7007-v4l2.o go7007-driver.o go7007-i2c.o go7007-fw.o snd-go7007.o
+go7007-objs += go7007-v4l2.o go7007-driver.o go7007-i2c.o go7007-fw.o \
+		snd-go7007.o wis-saa7113.o
 
+s2250-objs += s2250-board.o s2250-loader.o
 
-#ifneq ($(SAA7134_BUILD),)
-#obj-m += saa7134-go7007.o
+# Uncompile when the saa7134 patches get into upstream
+#ifneq ($(CONFIG_VIDEO_SAA7134),)
+#obj-$(CONFIG_VIDEO_SAA7134) += saa7134-go7007.o
+#EXTRA_CFLAGS += -Idrivers/media/video/saa7134
 #endif
 
+ifneq ($(CONFIG_VIDEO_GO7007_USB_S2250_BOARD),)
+EXTRA_CFLAGS += -Idrivers/media/dvb/dvb-usb
+endif
+
 EXTRA_CFLAGS += -Idrivers/staging/saa7134
 EXTRA_CFLAGS += -Idrivers/media/dvb/frontends
 EXTRA_CFLAGS += -Idrivers/media/dvb/dvb-core
diff --git a/drivers/staging/go7007/go7007-driver.c b/drivers/staging/go7007/go7007-driver.c
index e4ead96679c8..58bfc8d81b3b 100644
--- a/drivers/staging/go7007/go7007-driver.c
+++ b/drivers/staging/go7007/go7007-driver.c
@@ -217,6 +217,9 @@ static int init_i2c_module(struct i2c_adapter *adapter, int id, int addr)
 	case I2C_DRIVERID_WIS_OV7640:
 		modname = "wis-ov7640";
 		break;
+	case I2C_DRIVERID_S2250:
+		modname = "s2250-board";
+		break;
 	default:
 		modname = NULL;
 		break;
@@ -227,7 +230,7 @@ static int init_i2c_module(struct i2c_adapter *adapter, int id, int addr)
 		return 0;
 	if (modname != NULL)
 		printk(KERN_INFO
-			"go7007: probing for module %s failed", modname);
+			"go7007: probing for module %s failed\n", modname);
 	else
 		printk(KERN_INFO
 			"go7007: sensor %u seems to be unsupported!\n", id);
diff --git a/drivers/staging/go7007/go7007-fw.c b/drivers/staging/go7007/go7007-fw.c
index a0e17b0e0ce3..871ed43e4e05 100644
--- a/drivers/staging/go7007/go7007-fw.c
+++ b/drivers/staging/go7007/go7007-fw.c
@@ -284,7 +284,7 @@ static const int zz[64] = {
 	58, 59, 52, 45, 38, 31, 39, 46, 53, 60, 61, 54, 47, 55, 62, 63
 };
 
-static int copy_packages(u16 *dest, u16 *src, int pkg_cnt, int space)
+static int copy_packages(__le16 *dest, u16 *src, int pkg_cnt, int space)
 {
 	int i, cnt = pkg_cnt * 32;
 
@@ -292,7 +292,7 @@ static int copy_packages(u16 *dest, u16 *src, int pkg_cnt, int space)
 		return -1;
 
 	for (i = 0; i < cnt; ++i)
-		dest[i] = __cpu_to_le16(src[i]);
+		dest[i] = cpu_to_le16p(src + i);
 
 	return cnt;
 }
@@ -372,7 +372,7 @@ static int mjpeg_frame_header(struct go7007 *go, unsigned char *buf, int q)
 	return p;
 }
 
-static int gen_mjpeghdr_to_package(struct go7007 *go, u16 *code, int space)
+static int gen_mjpeghdr_to_package(struct go7007 *go, __le16 *code, int space)
 {
 	u8 *buf;
 	u16 mem = 0x3e00;
@@ -643,7 +643,7 @@ static int mpeg1_sequence_header(struct go7007 *go, unsigned char *buf, int ext)
 }
 
 static int gen_mpeg1hdr_to_package(struct go7007 *go,
-					u16 *code, int space, int *framelen)
+					__le16 *code, int space, int *framelen)
 {
 	u8 *buf;
 	u16 mem = 0x3e00;
@@ -831,7 +831,7 @@ static int mpeg4_sequence_header(struct go7007 *go, unsigned char *buf, int ext)
 }
 
 static int gen_mpeg4hdr_to_package(struct go7007 *go,
-					u16 *code, int space, int *framelen)
+					__le16 *code, int space, int *framelen)
 {
 	u8 *buf;
 	u16 mem = 0x3e00;
@@ -936,7 +936,7 @@ done:
 }
 
 static int brctrl_to_package(struct go7007 *go,
-					u16 *code, int space, int *framelen)
+					__le16 *code, int space, int *framelen)
 {
 	int converge_speed = 0;
 	int lambda = (go->format == GO7007_FORMAT_MJPEG || go->dvd_mode) ?
@@ -1091,7 +1091,7 @@ static int brctrl_to_package(struct go7007 *go,
 	return copy_packages(code, pack, 6, space);
 }
 
-static int config_package(struct go7007 *go, u16 *code, int space)
+static int config_package(struct go7007 *go, __le16 *code, int space)
 {
 	int fps = go->sensor_framerate / go->fps_scale / 1000;
 	int rows = go->interlace_coding ? go->height / 32 : go->height / 16;
@@ -1213,7 +1213,7 @@ static int config_package(struct go7007 *go, u16 *code, int space)
 	return copy_packages(code, pack, 5, space);
 }
 
-static int seqhead_to_package(struct go7007 *go, u16 *code, int space,
+static int seqhead_to_package(struct go7007 *go, __le16 *code, int space,
 	int (*sequence_header_func)(struct go7007 *go,
 		unsigned char *buf, int ext))
 {
@@ -1292,7 +1292,7 @@ static int relative_prime(int big, int little)
 	return big;
 }
 
-static int avsync_to_package(struct go7007 *go, u16 *code, int space)
+static int avsync_to_package(struct go7007 *go, __le16 *code, int space)
 {
 	int arate = go->board_info->audio_rate * 1001 * go->fps_scale;
 	int ratio = arate / go->sensor_framerate;
@@ -1323,7 +1323,7 @@ static int avsync_to_package(struct go7007 *go, u16 *code, int space)
 	return copy_packages(code, pack, 1, space);
 }
 
-static int final_package(struct go7007 *go, u16 *code, int space)
+static int final_package(struct go7007 *go, __le16 *code, int space)
 {
 	int rows = go->interlace_coding ? go->height / 32 : go->height / 16;
 	u16 pack[] = {
@@ -1386,7 +1386,7 @@ static int final_package(struct go7007 *go, u16 *code, int space)
 	return copy_packages(code, pack, 1, space);
 }
 
-static int audio_to_package(struct go7007 *go, u16 *code, int space)
+static int audio_to_package(struct go7007 *go, __le16 *code, int space)
 {
 	int clock_config = ((go->board_info->audio_flags &
 				GO7007_AUDIO_I2S_MASTER ? 1 : 0) << 11) |
@@ -1436,7 +1436,7 @@ static int audio_to_package(struct go7007 *go, u16 *code, int space)
 	return copy_packages(code, pack, 2, space);
 }
 
-static int modet_to_package(struct go7007 *go, u16 *code, int space)
+static int modet_to_package(struct go7007 *go, __le16 *code, int space)
 {
 	int ret, mb, i, addr, cnt = 0;
 	u16 pack[32];
@@ -1505,7 +1505,7 @@ static int modet_to_package(struct go7007 *go, u16 *code, int space)
 	return cnt;
 }
 
-static int do_special(struct go7007 *go, u16 type, u16 *code, int space,
+static int do_special(struct go7007 *go, u16 type, __le16 *code, int space,
 			int *framelen)
 {
 	switch (type) {
@@ -1555,7 +1555,7 @@ static int do_special(struct go7007 *go, u16 type, u16 *code, int space,
 int go7007_construct_fw_image(struct go7007 *go, u8 **fw, int *fwlen)
 {
 	const struct firmware *fw_entry;
-	u16 *code, *src;
+	__le16 *code, *src;
 	int framelen[8] = { }; /* holds the lengths of empty frame templates */
 	int codespace = 64 * 1024, i = 0, srclen, chunk_len, chunk_flags;
 	int mode_flag;
@@ -1590,7 +1590,7 @@ int go7007_construct_fw_image(struct go7007 *go, u8 **fw, int *fwlen)
 		goto fw_failed;
 	}
 	memset(code, 0, codespace * 2);
-	src = (u16 *)fw_entry->data;
+	src = (__le16 *)fw_entry->data;
 	srclen = fw_entry->size / 2;
 	while (srclen >= 2) {
 		chunk_flags = __le16_to_cpu(src[0]);
diff --git a/drivers/staging/go7007/go7007-priv.h b/drivers/staging/go7007/go7007-priv.h
index 005542d16a56..372f1f1c09b2 100644
--- a/drivers/staging/go7007/go7007-priv.h
+++ b/drivers/staging/go7007/go7007-priv.h
@@ -40,6 +40,7 @@ struct go7007;
 #define GO7007_BOARDID_LIFEVIEW_LR192	21 /* TV Walker Ultra */
 #define GO7007_BOARDID_ENDURA		22
 #define GO7007_BOARDID_ADLINK_MPG24	23
+#define GO7007_BOARDID_SENSORAY_2250	24 /* Sensoray 2250/2251 */
 
 /* Various characteristics of each board */
 #define GO7007_BOARD_HAS_AUDIO		(1<<0)
@@ -104,6 +105,7 @@ struct go7007_hpi_ops {
 	int (*stream_start)(struct go7007 *go);
 	int (*stream_stop)(struct go7007 *go);
 	int (*send_firmware)(struct go7007 *go, u8 *data, int len);
+	int (*send_command)(struct go7007 *go, unsigned int cmd, void *arg);
 };
 
 /* The video buffer size must be a multiple of PAGE_SIZE */
diff --git a/drivers/staging/go7007/go7007-usb.c b/drivers/staging/go7007/go7007-usb.c
index 3f5ee3424e72..83eec920c7d3 100644
--- a/drivers/staging/go7007/go7007-usb.c
+++ b/drivers/staging/go7007/go7007-usb.c
@@ -225,7 +225,7 @@ static struct go7007_usb_board board_px_tv402u = {
 		.inputs 	 = {
 			{
 				.video_input	= 1,
-		.audio_input	 = TVAUDIO_INPUT_EXTERN,
+				.audio_input	= TVAUDIO_INPUT_EXTERN,
 				.name		= "Composite",
 			},
 			{
@@ -398,6 +398,41 @@ static struct go7007_usb_board board_adlink_mpg24 = {
 	},
 };
 
+static struct go7007_usb_board board_sensoray_2250 = {
+	.flags		= GO7007_USB_EZUSB | GO7007_USB_EZUSB_I2C,
+	.main_info	= {
+		.firmware	 = "go7007tv.bin",
+		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
+					GO7007_AUDIO_I2S_MASTER |
+					GO7007_AUDIO_WORD_16,
+		.flags		 = GO7007_BOARD_HAS_AUDIO,
+		.audio_rate	 = 48000,
+		.audio_bclk_div	 = 8,
+		.audio_main_div	 = 2,
+		.hpi_buffer_cap  = 7,
+		.sensor_flags	 = GO7007_SENSOR_656 |
+					GO7007_SENSOR_TV,
+		.num_i2c_devs	 = 1,
+		.i2c_devs	 = {
+			{
+				.id	= I2C_DRIVERID_S2250,
+				.addr	= 0x34,
+			},
+		},
+		.num_inputs	 = 2,
+		.inputs 	 = {
+			{
+				.video_input	= 0,
+				.name		= "Composite",
+			},
+			{
+				.video_input	= 1,
+				.name		= "S-Video",
+			},
+		},
+	},
+};
+
 static struct usb_device_id go7007_usb_id_table[] = {
 	{
 		.match_flags	= USB_DEVICE_ID_MATCH_DEVICE_AND_VERSION |
@@ -491,6 +526,14 @@ static struct usb_device_id go7007_usb_id_table[] = {
 		.bcdDevice_hi	= 0x1,
 		.driver_info	= (kernel_ulong_t)GO7007_BOARDID_LIFEVIEW_LR192,
 	},
+	{
+		.match_flags	= USB_DEVICE_ID_MATCH_DEVICE_AND_VERSION,
+		.idVendor	= 0x1943,  /* Vendor ID Sensoray */
+		.idProduct	= 0x2250,  /* Product ID of 2250/2251 */
+		.bcdDevice_lo	= 0x1,
+		.bcdDevice_hi	= 0x1,
+		.driver_info	= (kernel_ulong_t)GO7007_BOARDID_SENSORAY_2250,
+	},
 	{ }					/* Terminating entry */
 };
 
@@ -637,9 +680,10 @@ static void go7007_usb_readinterrupt_complete(struct urb *urb)
 {
 	struct go7007 *go = (struct go7007 *)urb->context;
 	u16 *regs = (u16 *)urb->transfer_buffer;
+	int status = urb->status;
 
-	if (urb->status != 0) {
-		if (urb->status != -ESHUTDOWN &&
+	if (status) {
+		if (status != -ESHUTDOWN &&
 				go->status != STATUS_SHUTDOWN) {
 			printk(KERN_ERR
 				"go7007-usb: error in read interrupt: %d\n",
@@ -680,15 +724,14 @@ static int go7007_usb_read_interrupt(struct go7007 *go)
 static void go7007_usb_read_video_pipe_complete(struct urb *urb)
 {
 	struct go7007 *go = (struct go7007 *)urb->context;
-	int r;
+	int r, status = urb-> status;
 
 	if (!go->streaming) {
 		wake_up_interruptible(&go->frame_waitq);
 		return;
 	}
-	if (urb->status != 0) {
-		printk(KERN_ERR "go7007-usb: error in video pipe: %d\n",
-				urb->status);
+	if (status) {
+		printk(KERN_ERR "go7007-usb: error in video pipe: %d\n", status);
 		return;
 	}
 	if (urb->actual_length != urb->transfer_buffer_length) {
@@ -704,13 +747,12 @@ static void go7007_usb_read_video_pipe_complete(struct urb *urb)
 static void go7007_usb_read_audio_pipe_complete(struct urb *urb)
 {
 	struct go7007 *go = (struct go7007 *)urb->context;
-	int r;
+	int r, status = urb->status;
 
 	if (!go->streaming)
 		return;
-	if (urb->status != 0) {
-		printk(KERN_ERR "go7007-usb: error in audio pipe: %d\n",
-				urb->status);
+	if (status) {
+		printk(KERN_ERR "go7007-usb: error in audio pipe: %d\n", status);
 		return;
 	}
 	if (urb->actual_length != urb->transfer_buffer_length) {
@@ -751,7 +793,7 @@ static int go7007_usb_stream_start(struct go7007 *go)
 	return 0;
 
 audio_submit_failed:
-	for (i = 0; i < 8; ++i)
+	for (i = 0; i < 7; ++i)
 		usb_kill_urb(usb->audio_urbs[i]);
 video_submit_failed:
 	for (i = 0; i < 8; ++i)
@@ -965,16 +1007,20 @@ static int go7007_usb_probe(struct usb_interface *intf,
 		name = "Lifeview TV Walker Ultra";
 		board = &board_lifeview_lr192;
 		break;
+	case GO7007_BOARDID_SENSORAY_2250:
+		printk(KERN_INFO "Sensoray 2250 found\n");
+		name = "Sensoray 2250/2251\n";
+		board = &board_sensoray_2250;
+		break;
 	default:
 		printk(KERN_ERR "go7007-usb: unknown board ID %d!\n",
 				(unsigned int)id->driver_info);
 		return 0;
 	}
 
-	usb = kmalloc(sizeof(struct go7007_usb), GFP_KERNEL);
+	usb = kzalloc(sizeof(struct go7007_usb), GFP_KERNEL);
 	if (usb == NULL)
 		return -ENOMEM;
-	memset(usb, 0, sizeof(struct go7007_usb));
 
 	/* Allocate the URB and buffer for receiving incoming interrupts */
 	usb->intr_urb = usb_alloc_urb(0, GFP_KERNEL);
@@ -1179,6 +1225,7 @@ static void go7007_usb_disconnect(struct usb_interface *intf)
 {
 	struct go7007 *go = usb_get_intfdata(intf);
 	struct go7007_usb *usb = go->hpi_context;
+	struct urb *vurb, *aurb;
 	int i;
 
 	go->status = STATUS_SHUTDOWN;
@@ -1186,15 +1233,19 @@ static void go7007_usb_disconnect(struct usb_interface *intf)
 
 	/* Free USB-related structs */
 	for (i = 0; i < 8; ++i) {
-		if (usb->video_urbs[i] != NULL) {
-			if (usb->video_urbs[i]->transfer_buffer != NULL)
-				kfree(usb->video_urbs[i]->transfer_buffer);
-			usb_free_urb(usb->video_urbs[i]);
+		vurb = usb->video_urbs[i];
+		if (vurb) {
+			usb_kill_urb(vurb);
+			if (vurb->transfer_buffer)
+				kfree(vurb->transfer_buffer);
+			usb_free_urb(vurb);
 		}
-		if (usb->audio_urbs[i] != NULL) {
-			if (usb->audio_urbs[i]->transfer_buffer != NULL)
-				kfree(usb->audio_urbs[i]->transfer_buffer);
-			usb_free_urb(usb->audio_urbs[i]);
+		aurb = usb->audio_urbs[i];
+		if (aurb) {
+			usb_kill_urb(aurb);
+			if (aurb->transfer_buffer)
+				kfree(aurb->transfer_buffer);
+			usb_free_urb(aurb);
 		}
 	}
 	kfree(usb->intr_urb->transfer_buffer);
diff --git a/drivers/staging/go7007/go7007-v4l2.c b/drivers/staging/go7007/go7007-v4l2.c
index 94e1141a1fcd..868edb65e7bf 100644
--- a/drivers/staging/go7007/go7007-v4l2.c
+++ b/drivers/staging/go7007/go7007-v4l2.c
@@ -38,6 +38,14 @@
 #include "go7007-priv.h"
 #include "wis-i2c.h"
 
+/* Temporary defines until accepted in v4l-dvb */
+#ifndef V4L2_MPEG_STREAM_TYPE_MPEG_ELEM
+#define	V4L2_MPEG_STREAM_TYPE_MPEG_ELEM   6 /* MPEG elementary stream */
+#endif
+#ifndef V4L2_MPEG_VIDEO_ENCODING_MPEG_4
+#define	V4L2_MPEG_VIDEO_ENCODING_MPEG_4   3
+#endif
+
 static void deactivate_buffer(struct go7007_buffer *gobuf)
 {
 	int i;
@@ -81,7 +89,7 @@ static int go7007_streamoff(struct go7007 *go)
 	return 0;
 }
 
-static int go7007_open(struct inode *inode, struct file *file)
+static int go7007_open(struct file *file)
 {
 	struct go7007 *go = video_get_drvdata(video_devdata(file));
 	struct go7007_file *gofh;
@@ -99,7 +107,7 @@ static int go7007_open(struct inode *inode, struct file *file)
 	return 0;
 }
 
-static int go7007_release(struct inode *inode, struct file *file)
+static int go7007_release(struct file *file)
 {
 	struct go7007_file *gofh = file->private_data;
 	struct go7007 *go = gofh->go;
@@ -319,6 +327,7 @@ static int set_capture_size(struct go7007 *go, struct v4l2_format *fmt, int try)
 	return 0;
 }
 
+#if 0
 static int clip_to_modet_map(struct go7007 *go, int region,
 		struct v4l2_clip *clip_list)
 {
@@ -375,499 +384,801 @@ static int clip_to_modet_map(struct go7007 *go, int region,
 	return 0;
 }
 
-static int go7007_do_ioctl(struct inode *inode, struct file *file,
-		unsigned int cmd, void *arg)
+static int mpeg_queryctrl(u32 id, struct v4l2_queryctrl *ctrl)
 {
-	struct go7007_file *gofh = file->private_data;
-	struct go7007 *go = gofh->go;
-	unsigned long flags;
-	int retval = 0;
-
-	switch (cmd) {
-	case VIDIOC_QUERYCAP:
-	{
-		struct v4l2_capability *cap = arg;
-
-		memset(cap, 0, sizeof(*cap));
-		strcpy(cap->driver, "go7007");
-		strncpy(cap->card, go->name, sizeof(cap->card));
-		cap->version = KERNEL_VERSION(0, 9, 8);
-		cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
-				V4L2_CAP_STREAMING; /* | V4L2_CAP_AUDIO; */
-		if (go->board_info->flags & GO7007_BOARD_HAS_TUNER)
-			cap->capabilities |= V4L2_CAP_TUNER;
+	static const u32 user_ctrls[] = {
+		V4L2_CID_USER_CLASS,
+		0
+	};
+	static const u32 mpeg_ctrls[] = {
+		V4L2_CID_MPEG_CLASS,
+		V4L2_CID_MPEG_STREAM_TYPE,
+		V4L2_CID_MPEG_VIDEO_ENCODING,
+		V4L2_CID_MPEG_VIDEO_ASPECT,
+		V4L2_CID_MPEG_VIDEO_GOP_SIZE,
+		V4L2_CID_MPEG_VIDEO_GOP_CLOSURE,
+		V4L2_CID_MPEG_VIDEO_BITRATE,
+		0
+	};
+	static const u32 *ctrl_classes[] = {
+		user_ctrls,
+		mpeg_ctrls,
+		NULL
+	};
+
+	/* The ctrl may already contain the queried i2c controls,
+	 * query the mpeg controls if the existing ctrl id is
+	 * greater than the next mpeg ctrl id.
+	 */
+	id = v4l2_ctrl_next(ctrl_classes, id);
+	if (id >= ctrl->id && ctrl->name[0])
 		return 0;
+
+	memset(ctrl, 0, sizeof(*ctrl));
+	ctrl->id = id;
+
+	switch (ctrl->id) {
+	case V4L2_CID_USER_CLASS:
+	case V4L2_CID_MPEG_CLASS:
+		return v4l2_ctrl_query_fill_std(ctrl);
+	case V4L2_CID_MPEG_STREAM_TYPE:
+		return v4l2_ctrl_query_fill(ctrl,
+				V4L2_MPEG_STREAM_TYPE_MPEG2_DVD,
+				V4L2_MPEG_STREAM_TYPE_MPEG_ELEM, 1,
+				V4L2_MPEG_STREAM_TYPE_MPEG_ELEM);
+	case V4L2_CID_MPEG_VIDEO_ENCODING:
+		return v4l2_ctrl_query_fill(ctrl,
+				V4L2_MPEG_VIDEO_ENCODING_MPEG_1,
+				V4L2_MPEG_VIDEO_ENCODING_MPEG_4, 1,
+				V4L2_MPEG_VIDEO_ENCODING_MPEG_2);
+	case V4L2_CID_MPEG_VIDEO_ASPECT:
+		return v4l2_ctrl_query_fill(ctrl,
+				V4L2_MPEG_VIDEO_ASPECT_1x1,
+				V4L2_MPEG_VIDEO_ASPECT_16x9, 1,
+				V4L2_MPEG_VIDEO_ASPECT_1x1);
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
+		return v4l2_ctrl_query_fill_std(ctrl);
+	case V4L2_CID_MPEG_VIDEO_BITRATE:
+		return v4l2_ctrl_query_fill(ctrl,
+				64000,
+				10000000, 1,
+				9800000);
+	default:
+		break;
 	}
-	case VIDIOC_ENUM_FMT:
-	{
-		struct v4l2_fmtdesc *fmt = arg;
-		unsigned int index;
-		char *desc;
-		u32 pixelformat;
+	return -EINVAL;
+}
+
+static int mpeg_s_control(struct v4l2_control *ctrl, struct go7007 *go)
+{
+	/* pretty sure we can't change any of these while streaming */
+	if (go->streaming)
+		return -EBUSY;
 
-		if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_STREAM_TYPE:
+		switch (ctrl->value) {
+		case V4L2_MPEG_STREAM_TYPE_MPEG2_DVD:
+			go->format = GO7007_FORMAT_MPEG2;
+			go->bitrate = 9800000;
+			go->gop_size = 15;
+			go->pali = 0x48;
+			go->closed_gop = 1;
+			go->repeat_seqhead = 0;
+			go->seq_header_enable = 1;
+			go->gop_header_enable = 1;
+			go->dvd_mode = 1;
+			break;
+		case V4L2_MPEG_STREAM_TYPE_MPEG_ELEM:
+			/* todo: */
+			break;
+		default:
 			return -EINVAL;
-		switch (fmt->index) {
-		case 0:
-			pixelformat = V4L2_PIX_FMT_MJPEG;
-			desc = "Motion-JPEG";
+		}
+		break;
+	case V4L2_CID_MPEG_VIDEO_ENCODING:
+		switch (ctrl->value) {
+		case V4L2_MPEG_VIDEO_ENCODING_MPEG_1:
+			go->format = GO7007_FORMAT_MPEG1;
+			go->pali = 0;
 			break;
-		case 1:
-			pixelformat = V4L2_PIX_FMT_MPEG;
-			desc = "MPEG1/MPEG2/MPEG4";
+		case V4L2_MPEG_VIDEO_ENCODING_MPEG_2:
+			go->format = GO7007_FORMAT_MPEG2;
+			/*if (mpeg->pali >> 24 == 2)
+				go->pali = mpeg->pali & 0xff;
+			else*/
+				go->pali = 0x48;
+			break;
+		case V4L2_MPEG_VIDEO_ENCODING_MPEG_4:
+			go->format = GO7007_FORMAT_MPEG4;
+			/*if (mpeg->pali >> 24 == 4)
+				go->pali = mpeg->pali & 0xff;
+			else*/
+				go->pali = 0xf5;
 			break;
 		default:
 			return -EINVAL;
 		}
-		index = fmt->index;
-		memset(fmt, 0, sizeof(*fmt));
-		fmt->index = index;
-		fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		fmt->flags = V4L2_FMT_FLAG_COMPRESSED;
-		strncpy(fmt->description, desc, sizeof(fmt->description));
-		fmt->pixelformat = pixelformat;
-
-		return 0;
-	}
-	case VIDIOC_TRY_FMT:
-	{
-		struct v4l2_format *fmt = arg;
-
-		if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		go->gop_header_enable =
+			/*mpeg->flags & GO7007_MPEG_OMIT_GOP_HEADER
+			? 0 :*/ 1;
+		/*if (mpeg->flags & GO7007_MPEG_REPEAT_SEQHEADER)
+			go->repeat_seqhead = 1;
+		else*/
+			go->repeat_seqhead = 0;
+		go->dvd_mode = 0;
+		break;
+	case V4L2_CID_MPEG_VIDEO_ASPECT:
+		if (go->format == GO7007_FORMAT_MJPEG)
 			return -EINVAL;
-		return set_capture_size(go, fmt, 1);
-	}
-	case VIDIOC_G_FMT:
-	{
-		struct v4l2_format *fmt = arg;
-
-		if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		switch (ctrl->value) {
+		case V4L2_MPEG_VIDEO_ASPECT_1x1:
+			go->aspect_ratio = GO7007_RATIO_1_1;
+			break;
+		case V4L2_MPEG_VIDEO_ASPECT_4x3:
+			go->aspect_ratio = GO7007_RATIO_4_3;
+			break;
+		case V4L2_MPEG_VIDEO_ASPECT_16x9:
+			go->aspect_ratio = GO7007_RATIO_16_9;
+			break;
+		case V4L2_MPEG_VIDEO_ASPECT_221x100:
+		default:
 			return -EINVAL;
-		memset(fmt, 0, sizeof(*fmt));
-		fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		fmt->fmt.pix.width = go->width;
-		fmt->fmt.pix.height = go->height;
-		fmt->fmt.pix.pixelformat = go->format == GO7007_FORMAT_MJPEG ?
-			V4L2_PIX_FMT_MJPEG : V4L2_PIX_FMT_MPEG;
-		fmt->fmt.pix.field = V4L2_FIELD_NONE;
-		fmt->fmt.pix.bytesperline = 0;
-		fmt->fmt.pix.sizeimage = GO7007_BUF_SIZE;
-		fmt->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M; /* ?? */
-		return 0;
-	}
-	case VIDIOC_S_FMT:
-	{
-		struct v4l2_format *fmt = arg;
-
-		if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		}
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+		go->gop_size = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
+		if (ctrl->value != 0 && ctrl->value != 1)
 			return -EINVAL;
-		if (go->streaming)
-			return -EBUSY;
-		return set_capture_size(go, fmt, 0);
-	}
-	case VIDIOC_G_FBUF:
-	case VIDIOC_S_FBUF:
-		return -EINVAL;
-	case VIDIOC_REQBUFS:
-	{
-		struct v4l2_requestbuffers *req = arg;
-		unsigned int count, i;
-
-		if (go->streaming)
-			return -EBUSY;
-		if (req->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-				req->memory != V4L2_MEMORY_MMAP)
+		go->closed_gop = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE:
+		/* Upper bound is kind of arbitrary here */
+		if (ctrl->value < 64000 || ctrl->value > 10000000)
 			return -EINVAL;
-
-		down(&gofh->lock);
-		retval = -EBUSY;
-		for (i = 0; i < gofh->buf_count; ++i)
-			if (gofh->bufs[i].mapped > 0)
-				goto unlock_and_return;
-		down(&go->hw_lock);
-		if (go->in_use > 0 && gofh->buf_count == 0) {
-			up(&go->hw_lock);
-			goto unlock_and_return;
-		}
-		if (gofh->buf_count > 0)
-			kfree(gofh->bufs);
-		retval = -ENOMEM;
-		count = req->count;
-		if (count > 0) {
-			if (count < 2)
-				count = 2;
-			if (count > 32)
-				count = 32;
-			gofh->bufs = kmalloc(count *
-						sizeof(struct go7007_buffer),
-					GFP_KERNEL);
-			if (gofh->bufs == NULL) {
-				up(&go->hw_lock);
-				goto unlock_and_return;
-			}
-			memset(gofh->bufs, 0,
-					count * sizeof(struct go7007_buffer));
-			for (i = 0; i < count; ++i) {
-				gofh->bufs[i].go = go;
-				gofh->bufs[i].index = i;
-				gofh->bufs[i].state = BUF_STATE_IDLE;
-				gofh->bufs[i].mapped = 0;
-			}
-			go->in_use = 1;
-		} else {
-			go->in_use = 0;
-		}
-		gofh->buf_count = count;
-		up(&go->hw_lock);
-		up(&gofh->lock);
-		memset(req, 0, sizeof(*req));
-		req->count = count;
-		req->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		req->memory = V4L2_MEMORY_MMAP;
-		return 0;
+		go->bitrate = ctrl->value;
+		break;
+	default:
+		return -EINVAL;
 	}
-	case VIDIOC_QUERYBUF:
-	{
-		struct v4l2_buffer *buf = arg;
-		unsigned int index;
+	return 0;
+}
 
-		retval = -EINVAL;
-		if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+static int mpeg_g_control(struct v4l2_control *ctrl, struct go7007 *go)
+{
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_STREAM_TYPE:
+		if (go->dvd_mode)
+			ctrl->value = V4L2_MPEG_STREAM_TYPE_MPEG2_DVD;
+		else
+			ctrl->value = V4L2_MPEG_STREAM_TYPE_MPEG_ELEM;
+		break;
+	case V4L2_CID_MPEG_VIDEO_ENCODING:
+		switch (go->format) {
+		case GO7007_FORMAT_MPEG1:
+			ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_1;
+			break;
+		case GO7007_FORMAT_MPEG2:
+			ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_2;
+			break;
+		case GO7007_FORMAT_MPEG4:
+			ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_4;
+			break;
+		default:
 			return -EINVAL;
-		index = buf->index;
-		down(&gofh->lock);
-		if (index >= gofh->buf_count)
-			goto unlock_and_return;
-		memset(buf, 0, sizeof(*buf));
-		buf->index = index;
-		buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		switch (gofh->bufs[index].state) {
-		case BUF_STATE_QUEUED:
-			buf->flags = V4L2_BUF_FLAG_QUEUED;
+		}
+		break;
+	case V4L2_CID_MPEG_VIDEO_ASPECT:
+		switch (go->aspect_ratio) {
+		case GO7007_RATIO_1_1:
+			ctrl->value = V4L2_MPEG_VIDEO_ASPECT_1x1;
 			break;
-		case BUF_STATE_DONE:
-			buf->flags = V4L2_BUF_FLAG_DONE;
+		case GO7007_RATIO_4_3:
+			ctrl->value = V4L2_MPEG_VIDEO_ASPECT_4x3;
+			break;
+		case GO7007_RATIO_16_9:
+			ctrl->value = V4L2_MPEG_VIDEO_ASPECT_16x9;
 			break;
 		default:
-			buf->flags = 0;
+			return -EINVAL;
 		}
-		if (gofh->bufs[index].mapped)
-			buf->flags |= V4L2_BUF_FLAG_MAPPED;
-		buf->memory = V4L2_MEMORY_MMAP;
-		buf->m.offset = index * GO7007_BUF_SIZE;
-		buf->length = GO7007_BUF_SIZE;
-		up(&gofh->lock);
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+		ctrl->value = go->gop_size;
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
+		ctrl->value = go->closed_gop;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE:
+		ctrl->value = go->bitrate;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+#endif
 
-		return 0;
+static int vidioc_querycap(struct file *file, void  *priv,
+					struct v4l2_capability *cap)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	strlcpy(cap->driver, "go7007", sizeof(cap->driver));
+	strlcpy(cap->card, go->name, sizeof(cap->card));
+#if 0
+	strlcpy(cap->bus_info, dev_name(&dev->udev->dev), sizeof(cap->bus_info));
+#endif
+
+	cap->version = KERNEL_VERSION(0, 9, 8);
+
+	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
+			    V4L2_CAP_STREAMING; /* | V4L2_CAP_AUDIO; */
+
+	if (go->board_info->flags & GO7007_BOARD_HAS_TUNER)
+		cap->capabilities |= V4L2_CAP_TUNER;
+
+	return 0;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *fmt)
+{
+	char *desc = NULL;
+
+	switch (fmt->index) {
+	case 0:
+		fmt->pixelformat = V4L2_PIX_FMT_MJPEG;
+		desc = "Motion-JPEG";
+		break;
+	case 1:
+		fmt->pixelformat = V4L2_PIX_FMT_MPEG;
+		desc = "MPEG1/MPEG2/MPEG4";
+		break;
+	default:
+		return -EINVAL;
 	}
-	case VIDIOC_QBUF:
-	{
-		struct v4l2_buffer *buf = arg;
-		struct go7007_buffer *gobuf;
-		int ret;
+	fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	fmt->flags = V4L2_FMT_FLAG_COMPRESSED;
 
-		retval = -EINVAL;
-		if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-				buf->memory != V4L2_MEMORY_MMAP)
-			return -EINVAL;
-		down(&gofh->lock);
-		if (buf->index < 0 || buf->index >= gofh->buf_count)
-			goto unlock_and_return;
-		gobuf = &gofh->bufs[buf->index];
-		if (gobuf->mapped == 0)
-			goto unlock_and_return;
-		retval = -EBUSY;
-		if (gobuf->state != BUF_STATE_IDLE)
-			goto unlock_and_return;
-		/* offset will be 0 until we really support USERPTR streaming */
-		gobuf->offset = gobuf->user_addr & ~PAGE_MASK;
-		gobuf->bytesused = 0;
-		gobuf->frame_offset = 0;
-		gobuf->modet_active = 0;
-		if (gobuf->offset > 0)
-			gobuf->page_count = GO7007_BUF_PAGES + 1;
-		else
-			gobuf->page_count = GO7007_BUF_PAGES;
-		retval = -ENOMEM;
-		down_read(&current->mm->mmap_sem);
-		ret = get_user_pages(current, current->mm,
-				gobuf->user_addr & PAGE_MASK, gobuf->page_count,
-				1, 1, gobuf->pages, NULL);
-		up_read(&current->mm->mmap_sem);
-		if (ret != gobuf->page_count) {
-			int i;
-			for (i = 0; i < ret; ++i)
-				page_cache_release(gobuf->pages[i]);
-			gobuf->page_count = 0;
+	strncpy(fmt->description, desc, sizeof(fmt->description));
+
+	return 0;
+}
+
+static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_format *fmt)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	fmt->fmt.pix.width = go->width;
+	fmt->fmt.pix.height = go->height;
+	fmt->fmt.pix.pixelformat = (go->format == GO7007_FORMAT_MJPEG) ?
+				   V4L2_PIX_FMT_MJPEG : V4L2_PIX_FMT_MPEG;
+	fmt->fmt.pix.field = V4L2_FIELD_NONE;
+	fmt->fmt.pix.bytesperline = 0;
+	fmt->fmt.pix.sizeimage = GO7007_BUF_SIZE;
+	fmt->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+
+	return 0;
+}
+
+static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+			struct v4l2_format *fmt)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	return set_capture_size(go, fmt, 1);
+}
+
+static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+			struct v4l2_format *fmt)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (go->streaming)
+		return -EBUSY;
+
+	return set_capture_size(go, fmt, 0);
+}
+
+static int vidioc_reqbufs(struct file *file, void *priv,
+			  struct v4l2_requestbuffers *req)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	int retval = -EBUSY;
+	unsigned int count, i;
+
+	if (go->streaming)
+		return retval;
+
+	if (req->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+			req->memory != V4L2_MEMORY_MMAP)
+		return -EINVAL;
+
+	down(&gofh->lock);
+	for (i = 0; i < gofh->buf_count; ++i)
+		if (gofh->bufs[i].mapped > 0)
 			goto unlock_and_return;
-		}
-		gobuf->state = BUF_STATE_QUEUED;
-		spin_lock_irqsave(&go->spinlock, flags);
-		list_add_tail(&gobuf->stream, &go->stream);
-		spin_unlock_irqrestore(&go->spinlock, flags);
-		up(&gofh->lock);
-		return 0;
+
+	down(&go->hw_lock);
+	if (go->in_use > 0 && gofh->buf_count == 0) {
+		up(&go->hw_lock);
+		goto unlock_and_return;
 	}
-	case VIDIOC_DQBUF:
-	{
-		struct v4l2_buffer *buf = arg;
-		struct go7007_buffer *gobuf;
-		u32 frame_type_flag;
-		DEFINE_WAIT(wait);
 
-		if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			return -EINVAL;
-		if (buf->memory != V4L2_MEMORY_MMAP)
-			return -EINVAL;
-		down(&gofh->lock);
-		retval = -EINVAL;
-		if (list_empty(&go->stream))
+	if (gofh->buf_count > 0)
+		kfree(gofh->bufs);
+
+	retval = -ENOMEM;
+	count = req->count;
+	if (count > 0) {
+		if (count < 2)
+			count = 2;
+		if (count > 32)
+			count = 32;
+
+		gofh->bufs = kmalloc(count * sizeof(struct go7007_buffer),
+				     GFP_KERNEL);
+
+		if (!gofh->bufs) {
+			up(&go->hw_lock);
 			goto unlock_and_return;
-		gobuf = list_entry(go->stream.next,
-				struct go7007_buffer, stream);
-		retval = -EAGAIN;
-		if (gobuf->state != BUF_STATE_DONE &&
-				!(file->f_flags & O_NONBLOCK)) {
-			for (;;) {
-				prepare_to_wait(&go->frame_waitq, &wait,
-						TASK_INTERRUPTIBLE);
-				if (gobuf->state == BUF_STATE_DONE)
-					break;
-				if (signal_pending(current)) {
-					retval = -ERESTARTSYS;
-					break;
-				}
-				schedule();
-			}
-			finish_wait(&go->frame_waitq, &wait);
 		}
-		if (gobuf->state != BUF_STATE_DONE)
-			goto unlock_and_return;
-		spin_lock_irqsave(&go->spinlock, flags);
-		deactivate_buffer(gobuf);
-		spin_unlock_irqrestore(&go->spinlock, flags);
-		frame_type_flag = get_frame_type_flag(gobuf, go->format);
-		gobuf->state = BUF_STATE_IDLE;
-		memset(buf, 0, sizeof(*buf));
-		buf->index = gobuf->index;
-		buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		buf->bytesused = gobuf->bytesused;
-		buf->flags = V4L2_BUF_FLAG_MAPPED | frame_type_flag;
-		buf->field = V4L2_FIELD_NONE;
-		buf->timestamp = gobuf->timestamp;
-		buf->sequence = gobuf->seq;
-		buf->memory = V4L2_MEMORY_MMAP;
-		buf->m.offset = gobuf->index * GO7007_BUF_SIZE;
-		buf->length = GO7007_BUF_SIZE;
-		buf->reserved = gobuf->modet_active;
-		up(&gofh->lock);
-		return 0;
-	}
-	case VIDIOC_STREAMON:
-	{
-		unsigned int *type = arg;
 
-		if (*type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			return -EINVAL;
-		down(&gofh->lock);
-		down(&go->hw_lock);
-		if (!go->streaming) {
-			go->streaming = 1;
-			go->next_seq = 0;
-			go->active_buf = NULL;
-			if (go7007_start_encoder(go) < 0)
-				retval = -EIO;
-			else
-				retval = 0;
+		memset(gofh->bufs, 0, count * sizeof(struct go7007_buffer));
+
+		for (i = 0; i < count; ++i) {
+			gofh->bufs[i].go = go;
+			gofh->bufs[i].index = i;
+			gofh->bufs[i].state = BUF_STATE_IDLE;
+			gofh->bufs[i].mapped = 0;
 		}
-		up(&go->hw_lock);
-		up(&gofh->lock);
-		return retval;
-	}
-	case VIDIOC_STREAMOFF:
-	{
-		unsigned int *type = arg;
 
-		if (*type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			return -EINVAL;
-		down(&gofh->lock);
-		go7007_streamoff(go);
-		up(&gofh->lock);
-		return 0;
+		go->in_use = 1;
+	} else {
+		go->in_use = 0;
 	}
-	case VIDIOC_QUERYCTRL:
-	{
-		struct v4l2_queryctrl *ctrl = arg;
-		u32 id;
 
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		id = ctrl->id;
-		memset(ctrl, 0, sizeof(*ctrl));
-		ctrl->id = id;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_QUERYCTRL, arg);
-		return ctrl->name[0] == 0 ? -EINVAL : 0;
-	}
-	case VIDIOC_G_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-		struct v4l2_queryctrl query;
+	gofh->buf_count = count;
+	up(&go->hw_lock);
+	up(&gofh->lock);
 
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		memset(&query, 0, sizeof(query));
-		query.id = ctrl->id;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_QUERYCTRL, &query);
-		if (query.name[0] == 0)
-			return -EINVAL;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_G_CTRL, arg);
-		return 0;
-	}
-	case VIDIOC_S_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-		struct v4l2_queryctrl query;
+	memset(req, 0, sizeof(*req));
 
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		memset(&query, 0, sizeof(query));
-		query.id = ctrl->id;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_QUERYCTRL, &query);
-		if (query.name[0] == 0)
-			return -EINVAL;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_S_CTRL, arg);
-		return 0;
-	}
-	case VIDIOC_G_PARM:
-	{
-		struct v4l2_streamparm *parm = arg;
-		struct v4l2_fract timeperframe = {
-			.numerator = 1001 *  go->fps_scale,
-			.denominator = go->sensor_framerate,
-		};
+	req->count = count;
+	req->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	req->memory = V4L2_MEMORY_MMAP;
 
-		if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			return -EINVAL;
-		memset(parm, 0, sizeof(*parm));
-		parm->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		parm->parm.capture.capability |= V4L2_CAP_TIMEPERFRAME;
-		parm->parm.capture.timeperframe = timeperframe;
-		return 0;
+	return 0;
+
+unlock_and_return:
+	up(&gofh->lock);
+	return retval;
+}
+
+static int vidioc_querybuf(struct file *file, void *priv,
+			   struct v4l2_buffer *buf)
+{
+	struct go7007_file *gofh = priv;
+	int retval = -EINVAL;
+	unsigned int index;
+
+	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return retval;
+
+	index = buf->index;
+
+	down(&gofh->lock);
+	if (index >= gofh->buf_count)
+		goto unlock_and_return;
+
+	memset(buf, 0, sizeof(*buf));
+	buf->index = index;
+	buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	switch (gofh->bufs[index].state) {
+	case BUF_STATE_QUEUED:
+		buf->flags = V4L2_BUF_FLAG_QUEUED;
+		break;
+	case BUF_STATE_DONE:
+		buf->flags = V4L2_BUF_FLAG_DONE;
+		break;
+	default:
+		buf->flags = 0;
 	}
-	case VIDIOC_S_PARM:
-	{
-		struct v4l2_streamparm *parm = arg;
-		unsigned int n, d;
 
-		if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			return -EINVAL;
-		if (parm->parm.capture.capturemode != 0)
-			return -EINVAL;
-		n = go->sensor_framerate *
-			parm->parm.capture.timeperframe.numerator;
-		d = 1001 * parm->parm.capture.timeperframe.denominator;
-		if (n != 0 && d != 0 && n > d)
-			go->fps_scale = (n + d/2) / d;
-		else
-			go->fps_scale = 1;
-		return 0;
+	if (gofh->bufs[index].mapped)
+		buf->flags |= V4L2_BUF_FLAG_MAPPED;
+	buf->memory = V4L2_MEMORY_MMAP;
+	buf->m.offset = index * GO7007_BUF_SIZE;
+	buf->length = GO7007_BUF_SIZE;
+	up(&gofh->lock);
+
+	return 0;
+
+unlock_and_return:
+	up(&gofh->lock);
+	return retval;
+}
+
+static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	struct go7007_buffer *gobuf;
+	unsigned long flags;
+	int retval = -EINVAL;
+	int ret;
+
+	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+			buf->memory != V4L2_MEMORY_MMAP)
+		return retval;
+
+	down(&gofh->lock);
+	if (buf->index < 0 || buf->index >= gofh->buf_count)
+		goto unlock_and_return;
+
+	gobuf = &gofh->bufs[buf->index];
+	if (!gobuf->mapped)
+		goto unlock_and_return;
+
+	retval = -EBUSY;
+	if (gobuf->state != BUF_STATE_IDLE)
+		goto unlock_and_return;
+
+	/* offset will be 0 until we really support USERPTR streaming */
+	gobuf->offset = gobuf->user_addr & ~PAGE_MASK;
+	gobuf->bytesused = 0;
+	gobuf->frame_offset = 0;
+	gobuf->modet_active = 0;
+	if (gobuf->offset > 0)
+		gobuf->page_count = GO7007_BUF_PAGES + 1;
+	else
+		gobuf->page_count = GO7007_BUF_PAGES;
+
+	retval = -ENOMEM;
+	down_read(&current->mm->mmap_sem);
+	ret = get_user_pages(current, current->mm,
+			gobuf->user_addr & PAGE_MASK, gobuf->page_count,
+			1, 1, gobuf->pages, NULL);
+	up_read(&current->mm->mmap_sem);
+
+	if (ret != gobuf->page_count) {
+		int i;
+		for (i = 0; i < ret; ++i)
+			page_cache_release(gobuf->pages[i]);
+		gobuf->page_count = 0;
+		goto unlock_and_return;
 	}
-	case VIDIOC_ENUMSTD:
-	{
-		struct v4l2_standard *std = arg;
 
-		if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
-				go->input == go->board_info->num_inputs - 1) {
-			if (!go->i2c_adapter_online)
-				return -EIO;
-			i2c_clients_command(&go->i2c_adapter,
-						VIDIOC_ENUMSTD, arg);
-			if (!std->id) /* hack to indicate EINVAL from tuner */
-				return -EINVAL;
-		} else if (go->board_info->sensor_flags & GO7007_SENSOR_TV) {
-			switch (std->index) {
-			case 0:
-				v4l2_video_std_construct(std,
-						V4L2_STD_NTSC, "NTSC");
+	gobuf->state = BUF_STATE_QUEUED;
+	spin_lock_irqsave(&go->spinlock, flags);
+	list_add_tail(&gobuf->stream, &go->stream);
+	spin_unlock_irqrestore(&go->spinlock, flags);
+	up(&gofh->lock);
+
+	return 0;
+
+unlock_and_return:
+	up(&gofh->lock);
+	return retval;
+}
+
+
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	struct go7007_buffer *gobuf;
+	int retval = -EINVAL;
+	unsigned long flags;
+	u32 frame_type_flag;
+	DEFINE_WAIT(wait);
+
+	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return retval;
+	if (buf->memory != V4L2_MEMORY_MMAP)
+		return retval;
+
+	down(&gofh->lock);
+	if (list_empty(&go->stream))
+		goto unlock_and_return;
+	gobuf = list_entry(go->stream.next,
+			struct go7007_buffer, stream);
+
+	retval = -EAGAIN;
+	if (gobuf->state != BUF_STATE_DONE &&
+			!(file->f_flags & O_NONBLOCK)) {
+		for (;;) {
+			prepare_to_wait(&go->frame_waitq, &wait,
+					TASK_INTERRUPTIBLE);
+			if (gobuf->state == BUF_STATE_DONE)
 				break;
-			case 1:
-				v4l2_video_std_construct(std,
-						V4L2_STD_PAL | V4L2_STD_SECAM,
-						"PAL/SECAM");
+			if (signal_pending(current)) {
+				retval = -ERESTARTSYS;
 				break;
-			default:
-				return -EINVAL;
 			}
-		} else {
-			if (std->index != 0)
-				return -EINVAL;
-			memset(std, 0, sizeof(*std));
-			snprintf(std->name, sizeof(std->name), "%dx%d, %dfps",
-				go->board_info->sensor_width,
-				go->board_info->sensor_height,
-				go->board_info->sensor_framerate / 1000);
-			std->frameperiod.numerator = 1001;
-			std->frameperiod.denominator =
-					go->board_info->sensor_framerate;
+			schedule();
 		}
-		return 0;
+		finish_wait(&go->frame_waitq, &wait);
 	}
-	case VIDIOC_G_STD:
-	{
-		v4l2_std_id *std = arg;
+	if (gobuf->state != BUF_STATE_DONE)
+		goto unlock_and_return;
+
+	spin_lock_irqsave(&go->spinlock, flags);
+	deactivate_buffer(gobuf);
+	spin_unlock_irqrestore(&go->spinlock, flags);
+	frame_type_flag = get_frame_type_flag(gobuf, go->format);
+	gobuf->state = BUF_STATE_IDLE;
+
+	memset(buf, 0, sizeof(*buf));
+	buf->index = gobuf->index;
+	buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	buf->bytesused = gobuf->bytesused;
+	buf->flags = V4L2_BUF_FLAG_MAPPED | frame_type_flag;
+	buf->field = V4L2_FIELD_NONE;
+	buf->timestamp = gobuf->timestamp;
+	buf->sequence = gobuf->seq;
+	buf->memory = V4L2_MEMORY_MMAP;
+	buf->m.offset = gobuf->index * GO7007_BUF_SIZE;
+	buf->length = GO7007_BUF_SIZE;
+	buf->reserved = gobuf->modet_active;
 
-		if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
-				go->input == go->board_info->num_inputs - 1) {
-			if (!go->i2c_adapter_online)
-				return -EIO;
-			i2c_clients_command(&go->i2c_adapter,
-						VIDIOC_G_STD, arg);
-		} else if (go->board_info->sensor_flags & GO7007_SENSOR_TV) {
-			if (go->standard == GO7007_STD_NTSC)
-				*std = V4L2_STD_NTSC;
-			else
-				*std = V4L2_STD_PAL | V4L2_STD_SECAM;
-		} else
-			*std = 0;
-		return 0;
+	up(&gofh->lock);
+	return 0;
+
+unlock_and_return:
+	up(&gofh->lock);
+	return retval;
+}
+
+static int vidioc_streamon(struct file *file, void *priv,
+					enum v4l2_buf_type type)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	int retval = 0;
+
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	down(&gofh->lock);
+	down(&go->hw_lock);
+
+	if (!go->streaming) {
+		go->streaming = 1;
+		go->next_seq = 0;
+		go->active_buf = NULL;
+		if (go7007_start_encoder(go) < 0)
+			retval = -EIO;
+		else
+			retval = 0;
 	}
-	case VIDIOC_S_STD:
-	{
-		v4l2_std_id *std = arg;
+	up(&go->hw_lock);
+	up(&gofh->lock);
 
-		if (go->streaming)
-			return -EBUSY;
-		if (!(go->board_info->sensor_flags & GO7007_SENSOR_TV) &&
-				*std != 0)
-			return -EINVAL;
-		if (*std == 0)
-			return -EINVAL;
-		if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
-				go->input == go->board_info->num_inputs - 1) {
-			if (!go->i2c_adapter_online)
-				return -EIO;
-			i2c_clients_command(&go->i2c_adapter,
-						VIDIOC_S_STD, arg);
-			if (!*std) /* hack to indicate EINVAL from tuner */
-				return -EINVAL;
-		}
-		if (*std & V4L2_STD_NTSC) {
-			go->standard = GO7007_STD_NTSC;
-			go->sensor_framerate = 30000;
-		} else if (*std & V4L2_STD_PAL) {
-			go->standard = GO7007_STD_PAL;
-			go->sensor_framerate = 25025;
-		} else if (*std & V4L2_STD_SECAM) {
-			go->standard = GO7007_STD_PAL;
-			go->sensor_framerate = 25025;
-		} else
+	return retval;
+}
+
+static int vidioc_streamoff(struct file *file, void *priv,
+					enum v4l2_buf_type type)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	down(&gofh->lock);
+	go7007_streamoff(go);
+	up(&gofh->lock);
+
+	return 0;
+}
+
+static int vidioc_queryctrl(struct file *file, void *priv,
+			   struct v4l2_queryctrl *query)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (!go->i2c_adapter_online)
+		return -EIO;
+
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_QUERYCTRL, query);
+
+	return (!query->name[0]) ? -EINVAL : 0;
+}
+
+static int vidioc_g_ctrl(struct file *file, void *priv,
+				struct v4l2_control *ctrl)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	struct v4l2_queryctrl query;
+
+	if (!go->i2c_adapter_online)
+		return -EIO;
+
+	memset(&query, 0, sizeof(query));
+	query.id = ctrl->id;
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_QUERYCTRL, &query);
+	if (query.name[0] == 0)
+		return -EINVAL;
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_G_CTRL, ctrl);
+
+	return 0;
+}
+
+static int vidioc_s_ctrl(struct file *file, void *priv,
+				struct v4l2_control *ctrl)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	struct v4l2_queryctrl query;
+
+	if (!go->i2c_adapter_online)
+		return -EIO;
+
+	memset(&query, 0, sizeof(query));
+	query.id = ctrl->id;
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_QUERYCTRL, &query);
+	if (query.name[0] == 0)
+		return -EINVAL;
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_S_CTRL, ctrl);
+
+	return 0;
+}
+
+static int vidioc_g_parm(struct file *filp, void *priv,
+		struct v4l2_streamparm *parm)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	struct v4l2_fract timeperframe = {
+		.numerator = 1001 *  go->fps_scale,
+		.denominator = go->sensor_framerate,
+	};
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	parm->parm.capture.capability |= V4L2_CAP_TIMEPERFRAME;
+	parm->parm.capture.timeperframe = timeperframe;
+
+	return 0;
+}
+
+static int vidioc_s_parm(struct file *filp, void *priv,
+		struct v4l2_streamparm *parm)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	unsigned int n, d;
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (parm->parm.capture.capturemode != 0)
+		return -EINVAL;
+
+	n = go->sensor_framerate *
+		parm->parm.capture.timeperframe.numerator;
+	d = 1001 * parm->parm.capture.timeperframe.denominator;
+	if (n != 0 && d != 0 && n > d)
+		go->fps_scale = (n + d/2) / d;
+	else
+		go->fps_scale = 1;
+
+	return 0;
+}
+
+/* VIDIOC_ENUMSTD on go7007 were used for enumberating the supported fps and
+   its resolution, when the device is not connected to TV.
+   This were an API abuse, probably used by the lack of specific IOCTL's to
+   enumberate it, by the time the driver were written.
+
+   However, since kernel 2.6.19, two new ioctls (VIDIOC_ENUM_FRAMEINTERVALS
+   and VIDIOC_ENUM_FRAMESIZES) were added for this purpose.
+
+   The two functions bellow implements the newer ioctls
+*/
+static int vidioc_enum_framesizes(struct file *filp, void *priv,
+				  struct v4l2_frmsizeenum *fsize)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	/* Return -EINVAL, if it is a TV board */
+	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) ||
+	    (go->board_info->sensor_flags & GO7007_SENSOR_TV))
+		return -EINVAL;
+
+	if (fsize->index > 0)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = go->board_info->sensor_width;
+	fsize->discrete.height = go->board_info->sensor_height;
+
+	return 0;
+}
+
+static int vidioc_enum_frameintervals(struct file *filp, void *priv,
+				      struct v4l2_frmivalenum *fival)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	/* Return -EINVAL, if it is a TV board */
+	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) ||
+	    (go->board_info->sensor_flags & GO7007_SENSOR_TV))
+		return -EINVAL;
+
+	if (fival->index > 0)
+		return -EINVAL;
+
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete.numerator = 1001;
+	fival->discrete.denominator = go->board_info->sensor_framerate;
+
+	return 0;
+}
+
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (go->streaming)
+		return -EBUSY;
+
+	if (!(go->board_info->sensor_flags & GO7007_SENSOR_TV) &&
+			*std != 0)
+		return -EINVAL;
+
+	if (*std == 0)
+		return -EINVAL;
+
+	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
+			go->input == go->board_info->num_inputs - 1) {
+		if (!go->i2c_adapter_online)
+			return -EIO;
+		i2c_clients_command(&go->i2c_adapter,
+					VIDIOC_S_STD, std);
+		if (!*std) /* hack to indicate EINVAL from tuner */
 			return -EINVAL;
-		if (go->i2c_adapter_online)
-			i2c_clients_command(&go->i2c_adapter,
-					    VIDIOC_S_STD, std);
-		set_capture_size(go, NULL, 0);
-		return 0;
 	}
+
+	if (*std & V4L2_STD_NTSC) {
+		go->standard = GO7007_STD_NTSC;
+		go->sensor_framerate = 30000;
+	} else if (*std & V4L2_STD_PAL) {
+		go->standard = GO7007_STD_PAL;
+		go->sensor_framerate = 25025;
+	} else if (*std & V4L2_STD_SECAM) {
+		go->standard = GO7007_STD_PAL;
+		go->sensor_framerate = 25025;
+	} else
+		return -EINVAL;
+
+	if (go->i2c_adapter_online)
+		i2c_clients_command(&go->i2c_adapter,
+					VIDIOC_S_STD, std);
+	set_capture_size(go, NULL, 0);
+
+	return 0;
+}
+
+#if 0
 	case VIDIOC_QUERYSTD:
 	{
 		v4l2_std_id *std = arg;
@@ -884,219 +1195,269 @@ static int go7007_do_ioctl(struct inode *inode, struct file *file,
 			*std = 0;
 		return 0;
 	}
-	case VIDIOC_ENUMINPUT:
-	{
-		struct v4l2_input *inp = arg;
-		int index;
+#endif
 
-		if (inp->index >= go->board_info->num_inputs)
-			return -EINVAL;
-		index = inp->index;
-		memset(inp, 0, sizeof(*inp));
-		inp->index = index;
-		strncpy(inp->name, go->board_info->inputs[index].name,
-				sizeof(inp->name));
-		/* If this board has a tuner, it will be the last input */
-		if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
-				index == go->board_info->num_inputs - 1)
-			inp->type = V4L2_INPUT_TYPE_TUNER;
-		else
-			inp->type = V4L2_INPUT_TYPE_CAMERA;
-		inp->audioset = 0;
-		inp->tuner = 0;
-		if (go->board_info->sensor_flags & GO7007_SENSOR_TV)
-			inp->std = V4L2_STD_NTSC | V4L2_STD_PAL |
-							V4L2_STD_SECAM;
-		else
-			inp->std = 0;
-		return 0;
-	}
-	case VIDIOC_G_INPUT:
-	{
-		int *input = arg;
+static int vidioc_enum_input(struct file *file, void *priv,
+				struct v4l2_input *inp)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
 
-		*input = go->input;
-		return 0;
-	}
-	case VIDIOC_S_INPUT:
-	{
-		int *input = arg;
+	if (inp->index >= go->board_info->num_inputs)
+		return -EINVAL;
 
-		if (*input >= go->board_info->num_inputs)
-			return -EINVAL;
-		if (go->streaming)
-			return -EBUSY;
-		go->input = *input;
-		if (go->i2c_adapter_online) {
-			i2c_clients_command(&go->i2c_adapter, VIDIOC_S_INPUT,
-				&go->board_info->inputs[*input].video_input);
-			i2c_clients_command(&go->i2c_adapter, VIDIOC_S_AUDIO,
-				&go->board_info->inputs[*input].audio_input);
-		}
-		return 0;
-	}
-	case VIDIOC_G_TUNER:
-	{
-		struct v4l2_tuner *t = arg;
+	strncpy(inp->name, go->board_info->inputs[inp->index].name,
+			sizeof(inp->name));
 
-		if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
-			return -EINVAL;
-		if (t->index != 0)
-			return -EINVAL;
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_G_TUNER, arg);
-		t->index = 0;
-		return 0;
-	}
-	case VIDIOC_S_TUNER:
-	{
-		struct v4l2_tuner *t = arg;
+	/* If this board has a tuner, it will be the last input */
+	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
+			inp->index == go->board_info->num_inputs - 1)
+		inp->type = V4L2_INPUT_TYPE_TUNER;
+	else
+		inp->type = V4L2_INPUT_TYPE_CAMERA;
 
-		if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
-			return -EINVAL;
-		if (t->index != 0)
-			return -EINVAL;
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		switch (go->board_id) {
-		case GO7007_BOARDID_PX_TV402U_NA:
-		case GO7007_BOARDID_PX_TV402U_JP:
-			/* No selectable options currently */
-			if (t->audmode != V4L2_TUNER_MODE_STEREO)
-				return -EINVAL;
-			break;
-		}
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_S_TUNER, arg);
-		return 0;
-	}
-	case VIDIOC_G_FREQUENCY:
-	{
-		struct v4l2_frequency *f = arg;
+	inp->audioset = 0;
+	inp->tuner = 0;
+	if (go->board_info->sensor_flags & GO7007_SENSOR_TV)
+		inp->std = V4L2_STD_NTSC | V4L2_STD_PAL |
+						V4L2_STD_SECAM;
+	else
+		inp->std = 0;
 
-		if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
-			return -EINVAL;
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		memset(f, 0, sizeof(*f));
-		f->type = V4L2_TUNER_ANALOG_TV;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_G_FREQUENCY, arg);
-		return 0;
-	}
-	case VIDIOC_S_FREQUENCY:
-	{
-		if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
-			return -EINVAL;
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_S_FREQUENCY, arg);
-		return 0;
-	}
-	case VIDIOC_CROPCAP:
-	{
-		struct v4l2_cropcap *cropcap = arg;
+	return 0;
+}
 
-		if (cropcap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			return -EINVAL;
-		memset(cropcap, 0, sizeof(*cropcap));
-		cropcap->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		/* These specify the raw input of the sensor */
-		switch (go->standard) {
-		case GO7007_STD_NTSC:
-			cropcap->bounds.top = 0;
-			cropcap->bounds.left = 0;
-			cropcap->bounds.width = 720;
-			cropcap->bounds.height = 480;
-			cropcap->defrect.top = 0;
-			cropcap->defrect.left = 0;
-			cropcap->defrect.width = 720;
-			cropcap->defrect.height = 480;
-			break;
-		case GO7007_STD_PAL:
-			cropcap->bounds.top = 0;
-			cropcap->bounds.left = 0;
-			cropcap->bounds.width = 720;
-			cropcap->bounds.height = 576;
-			cropcap->defrect.top = 0;
-			cropcap->defrect.left = 0;
-			cropcap->defrect.width = 720;
-			cropcap->defrect.height = 576;
-			break;
-		case GO7007_STD_OTHER:
-			cropcap->bounds.top = 0;
-			cropcap->bounds.left = 0;
-			cropcap->bounds.width = go->board_info->sensor_width;
-			cropcap->bounds.height = go->board_info->sensor_height;
-			cropcap->defrect.top = 0;
-			cropcap->defrect.left = 0;
-			cropcap->defrect.width = go->board_info->sensor_width;
-			cropcap->defrect.height = go->board_info->sensor_height;
-			break;
-		}
 
-		return 0;
-	}
-	case VIDIOC_G_CROP:
-	{
-		struct v4l2_crop *crop = arg;
+static int vidioc_g_input(struct file *file, void *priv, unsigned int *input)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
 
-		if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			return -EINVAL;
-		memset(crop, 0, sizeof(*crop));
-		crop->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		/* These specify the raw input of the sensor */
-		switch (go->standard) {
-		case GO7007_STD_NTSC:
-			crop->c.top = 0;
-			crop->c.left = 0;
-			crop->c.width = 720;
-			crop->c.height = 480;
-			break;
-		case GO7007_STD_PAL:
-			crop->c.top = 0;
-			crop->c.left = 0;
-			crop->c.width = 720;
-			crop->c.height = 576;
-			break;
-		case GO7007_STD_OTHER:
-			crop->c.top = 0;
-			crop->c.left = 0;
-			crop->c.width = go->board_info->sensor_width;
-			crop->c.height = go->board_info->sensor_height;
-			break;
-		}
+	*input = go->input;
 
-		return 0;
+	return 0;
+}
+
+static int vidioc_s_input(struct file *file, void *priv, unsigned int input)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (input >= go->board_info->num_inputs)
+		return -EINVAL;
+	if (go->streaming)
+		return -EBUSY;
+
+	go->input = input;
+	if (go->i2c_adapter_online) {
+		i2c_clients_command(&go->i2c_adapter, VIDIOC_S_INPUT,
+			&go->board_info->inputs[input].video_input);
+		i2c_clients_command(&go->i2c_adapter, VIDIOC_S_AUDIO,
+			&go->board_info->inputs[input].audio_input);
 	}
-	case VIDIOC_S_CROP:
-	{
-		struct v4l2_crop *crop = arg;
 
-		if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	return 0;
+}
+
+static int vidioc_g_tuner(struct file *file, void *priv,
+				struct v4l2_tuner *t)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
+		return -EINVAL;
+	if (t->index != 0)
+		return -EINVAL;
+	if (!go->i2c_adapter_online)
+		return -EIO;
+
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_G_TUNER, t);
+
+	t->index = 0;
+	return 0;
+}
+
+static int vidioc_s_tuner(struct file *file, void *priv,
+				struct v4l2_tuner *t)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
+		return -EINVAL;
+	if (t->index != 0)
+		return -EINVAL;
+	if (!go->i2c_adapter_online)
+		return -EIO;
+
+	switch (go->board_id) {
+	case GO7007_BOARDID_PX_TV402U_NA:
+	case GO7007_BOARDID_PX_TV402U_JP:
+		/* No selectable options currently */
+		if (t->audmode != V4L2_TUNER_MODE_STEREO)
 			return -EINVAL;
-		return 0;
+		break;
 	}
-	case VIDIOC_G_JPEGCOMP:
-	{
-		struct v4l2_jpegcompression *params = arg;
 
-		memset(params, 0, sizeof(*params));
-		params->quality = 50; /* ?? */
-		params->jpeg_markers = V4L2_JPEG_MARKER_DHT |
-					V4L2_JPEG_MARKER_DQT;
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_S_TUNER, t);
 
-		return 0;
+	return 0;
+}
+
+static int vidioc_g_frequency(struct file *file, void *priv,
+				struct v4l2_frequency *f)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
+		return -EINVAL;
+	if (!go->i2c_adapter_online)
+		return -EIO;
+
+	f->type = V4L2_TUNER_ANALOG_TV;
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_G_FREQUENCY, f);
+	return 0;
+}
+
+static int vidioc_s_frequency(struct file *file, void *priv,
+				struct v4l2_frequency *f)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
+		return -EINVAL;
+	if (!go->i2c_adapter_online)
+		return -EIO;
+
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_S_FREQUENCY, f);
+
+	return 0;
+}
+
+static int vidioc_cropcap(struct file *file, void *priv,
+					struct v4l2_cropcap *cropcap)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (cropcap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	/* These specify the raw input of the sensor */
+	switch (go->standard) {
+	case GO7007_STD_NTSC:
+		cropcap->bounds.top = 0;
+		cropcap->bounds.left = 0;
+		cropcap->bounds.width = 720;
+		cropcap->bounds.height = 480;
+		cropcap->defrect.top = 0;
+		cropcap->defrect.left = 0;
+		cropcap->defrect.width = 720;
+		cropcap->defrect.height = 480;
+		break;
+	case GO7007_STD_PAL:
+		cropcap->bounds.top = 0;
+		cropcap->bounds.left = 0;
+		cropcap->bounds.width = 720;
+		cropcap->bounds.height = 576;
+		cropcap->defrect.top = 0;
+		cropcap->defrect.left = 0;
+		cropcap->defrect.width = 720;
+		cropcap->defrect.height = 576;
+		break;
+	case GO7007_STD_OTHER:
+		cropcap->bounds.top = 0;
+		cropcap->bounds.left = 0;
+		cropcap->bounds.width = go->board_info->sensor_width;
+		cropcap->bounds.height = go->board_info->sensor_height;
+		cropcap->defrect.top = 0;
+		cropcap->defrect.left = 0;
+		cropcap->defrect.width = go->board_info->sensor_width;
+		cropcap->defrect.height = go->board_info->sensor_height;
+		break;
 	}
-	case VIDIOC_S_JPEGCOMP:
-	{
-		struct v4l2_jpegcompression *params = arg;
 
-		if (params->quality != 50 ||
-				params->jpeg_markers != (V4L2_JPEG_MARKER_DHT |
-							V4L2_JPEG_MARKER_DQT))
-			return -EINVAL;
-		return 0;
+	return 0;
+}
+
+static int vidioc_g_crop(struct file *file, void *priv, struct v4l2_crop *crop)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	crop->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	/* These specify the raw input of the sensor */
+	switch (go->standard) {
+	case GO7007_STD_NTSC:
+		crop->c.top = 0;
+		crop->c.left = 0;
+		crop->c.width = 720;
+		crop->c.height = 480;
+		break;
+	case GO7007_STD_PAL:
+		crop->c.top = 0;
+		crop->c.left = 0;
+		crop->c.width = 720;
+		crop->c.height = 576;
+		break;
+	case GO7007_STD_OTHER:
+		crop->c.top = 0;
+		crop->c.left = 0;
+		crop->c.width = go->board_info->sensor_width;
+		crop->c.height = go->board_info->sensor_height;
+		break;
 	}
+
+	return 0;
+}
+
+/* FIXME: vidioc_s_crop is not really implemented!!!
+ */
+static int vidioc_s_crop(struct file *file, void *priv, struct v4l2_crop *crop)
+{
+	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int vidioc_g_jpegcomp(struct file *file, void *priv,
+			 struct v4l2_jpegcompression *params)
+{
+	memset(params, 0, sizeof(*params));
+	params->quality = 50; /* ?? */
+	params->jpeg_markers = V4L2_JPEG_MARKER_DHT |
+				V4L2_JPEG_MARKER_DQT;
+
+	return 0;
+}
+
+static int vidioc_s_jpegcomp(struct file *file, void *priv,
+			 struct v4l2_jpegcompression *params)
+{
+	if (params->quality != 50 ||
+			params->jpeg_markers != (V4L2_JPEG_MARKER_DHT |
+						V4L2_JPEG_MARKER_DQT))
+		return -EINVAL;
+
+	return 0;
+}
+
+/* FIXME:
+	Those ioctls are private, and not needed, since several standard
+	extended controls already provide streaming control.
+	So, those ioctls should be converted into vidioc_g_ext_ctrls()
+	and vidioc_s_ext_ctrls()
+ */
+
+#if 0
 	/* Temporary ioctls for controlling compression characteristics */
 	case GO7007IOC_S_BITRATE:
 	{
@@ -1316,27 +1677,7 @@ static int go7007_do_ioctl(struct inode *inode, struct file *file,
 			return -EINVAL;
 		return clip_to_modet_map(go, region->region, region->clips);
 	}
-	default:
-		printk(KERN_DEBUG "go7007: unsupported ioctl %d\n", cmd);
-		return -ENOIOCTLCMD;
-	}
-	return 0;
-
-unlock_and_return:
-	up(&gofh->lock);
-	return retval;
-}
-
-static int go7007_ioctl(struct inode *inode, struct file *file,
-		unsigned int cmd, unsigned long arg)
-{
-	struct go7007_file *gofh = file->private_data;
-
-	if (gofh->go->status != STATUS_ONLINE)
-		return -EIO;
-
-	return video_usercopy(inode, file, cmd, arg, go7007_do_ioctl);
-}
+#endif
 
 static ssize_t go7007_read(struct file *file, char __user *data,
 		size_t count, loff_t *ppos)
@@ -1371,8 +1712,7 @@ static int go7007_vm_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 	page = alloc_page(GFP_USER | __GFP_DMA32);
 	if (!page)
 		return VM_FAULT_OOM;
-	clear_user_page(page_address(page), (unsigned long)vmf->virtual_address,
-			page);
+	clear_user_highpage(page, (unsigned long)vmf->virtual_address);
 	vmf->page = page;
 	return 0;
 }
@@ -1441,23 +1781,59 @@ static void go7007_vfl_release(struct video_device *vfd)
 		kfree(go);
 }
 
-static struct file_operations go7007_fops = {
+static struct v4l2_file_operations go7007_fops = {
 	.owner		= THIS_MODULE,
 	.open		= go7007_open,
 	.release	= go7007_release,
-	.ioctl		= go7007_ioctl,
-	.llseek		= no_llseek,
+	.ioctl		= video_ioctl2,
 	.read		= go7007_read,
 	.mmap		= go7007_mmap,
 	.poll		= go7007_poll,
 };
 
+static const struct v4l2_ioctl_ops video_ioctl_ops = {
+	.vidioc_querycap          = vidioc_querycap,
+	.vidioc_enum_fmt_vid_cap  = vidioc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap     = vidioc_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap   = vidioc_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap     = vidioc_s_fmt_vid_cap,
+	.vidioc_reqbufs           = vidioc_reqbufs,
+	.vidioc_querybuf          = vidioc_querybuf,
+	.vidioc_qbuf              = vidioc_qbuf,
+	.vidioc_dqbuf             = vidioc_dqbuf,
+	.vidioc_s_std             = vidioc_s_std,
+	.vidioc_enum_input        = vidioc_enum_input,
+	.vidioc_g_input           = vidioc_g_input,
+	.vidioc_s_input           = vidioc_s_input,
+	.vidioc_queryctrl         = vidioc_queryctrl,
+	.vidioc_g_ctrl            = vidioc_g_ctrl,
+	.vidioc_s_ctrl            = vidioc_s_ctrl,
+	.vidioc_streamon          = vidioc_streamon,
+	.vidioc_streamoff         = vidioc_streamoff,
+	.vidioc_g_tuner           = vidioc_g_tuner,
+	.vidioc_s_tuner           = vidioc_s_tuner,
+	.vidioc_g_frequency       = vidioc_g_frequency,
+	.vidioc_s_frequency       = vidioc_s_frequency,
+	.vidioc_g_parm            = vidioc_g_parm,
+	.vidioc_s_parm            = vidioc_s_parm,
+	.vidioc_enum_framesizes   = vidioc_enum_framesizes,
+	.vidioc_enum_frameintervals = vidioc_enum_frameintervals,
+	.vidioc_cropcap           = vidioc_cropcap,
+	.vidioc_g_crop            = vidioc_g_crop,
+	.vidioc_s_crop            = vidioc_s_crop,
+	.vidioc_g_jpegcomp        = vidioc_g_jpegcomp,
+	.vidioc_s_jpegcomp        = vidioc_s_jpegcomp,
+};
+
 static struct video_device go7007_template = {
 	.name		= "go7007",
 	.vfl_type	= VID_TYPE_CAPTURE,
 	.fops		= &go7007_fops,
 	.minor		= -1,
 	.release	= go7007_vfl_release,
+	.ioctl_ops	= &video_ioctl_ops,
+	.tvnorms	= V4L2_STD_ALL,
+	.current_norm	= V4L2_STD_NTSC,
 };
 
 int go7007_v4l2_init(struct go7007 *go)
@@ -1477,6 +1853,8 @@ int go7007_v4l2_init(struct go7007 *go)
 	}
 	video_set_drvdata(go->video_dev, go);
 	++go->ref_count;
+	printk(KERN_INFO "%s: registered device video%d [v4l2]\n",
+	       go->video_dev->name, go->video_dev->num);
 
 	return 0;
 }
diff --git a/drivers/staging/go7007/go7007.txt b/drivers/staging/go7007/go7007.txt
new file mode 100644
index 000000000000..9f6772bc68c2
--- /dev/null
+++ b/drivers/staging/go7007/go7007.txt
@@ -0,0 +1,481 @@
+This is a driver for the WIS GO7007SB multi-format video encoder.
+
+Pete Eberlein <pete@sensoray.com>
+
+The driver was orignally released under the GPL and is currently hosted at:
+http://nikosapi.org/wiki/index.php/WIS_Go7007_Linux_driver
+The go7007 firmware can be acquired from the package on the site above.
+
+I've modified the driver to support the following Video4Linux2 MPEG
+controls, with acceptable values:
+
+V4L2_CID_MPEG_STREAM_TYPE	V4L2_MPEG_STREAM_TYPE_MPEG2_DVD
+				V4L2_MPEG_STREAM_TYPE_MPEG_ELEM
+V4L2_CID_MPEG_VIDEO_ENCODING	V4L2_MPEG_VIDEO_ENCODING_MPEG_1
+				V4L2_MPEG_VIDEO_ENCODING_MPEG_2
+				V4L2_MPEG_VIDEO_ENCODING_MPEG_4
+V4L2_CID_MPEG_VIDEO_ASPECT	V4L2_MPEG_VIDEO_ASPECT_1x1
+				V4L2_MPEG_VIDEO_ASPECT_4x3
+				V4L2_MPEG_VIDEO_ASPECT_16x9
+V4L2_CID_MPEG_VIDEO_GOP_SIZE	integer
+V4L2_CID_MPEG_VIDEO_BITRATE	64000 .. 10000000
+
+These should be used instead of the non-standard GO7007 ioctls described
+below.
+
+
+The README files from the orignal package appear below:
+
+---------------------------------------------------------------------------
+                     WIS GO7007SB Public Linux Driver
+---------------------------------------------------------------------------
+
+
+*** Please see the file RELEASE-NOTES for important last-minute updates ***
+
+
+  0. OVERVIEW AND LICENSING/DISCLAIMER
+
+
+This driver kit contains Linux drivers for the WIS GO7007SB multi-format
+video encoder.  Only kernel version 2.6.x is supported.  The video stream
+is available through the Video4Linux2 API and the audio stream is available
+through the ALSA API (or the OSS emulation layer of the ALSA system).
+
+The files in kernel/ and hotplug/ are licensed under the GNU General Public
+License Version 2 from the Free Software Foundation.  A copy of the license
+is included in the file COPYING.
+
+The example applications in apps/ and C header files in include/ are
+licensed under a permissive license included in the source files which
+allows copying, modification and redistribution for any purpose without
+attribution.
+
+The firmware files included in the firmware/ directory may be freely
+redistributed only in conjunction with this document; but modification,
+tampering and reverse engineering are prohibited.
+
+MICRONAS USA, INC., MAKES NO WARRANTIES TO ANY PERSON OR ENTITY WITH
+RESPECT TO THE SOFTWARE OR ANY DERIVATIVES THEREOF OR ANY SERVICES OR
+LICENSES AND DISCLAIMS ALL IMPLIED WARRANTIES, INCLUDING WITHOUT LIMITATION
+WARRANTIES OF MERCHANTABILITY, SUPPORT, AND FITNESS FOR A PARTICULAR
+PURPOSE AND NON-INFRINGEMENT.
+
+
+  1. SYSTEM REQUIREMENTS
+
+
+This driver requires Linux kernel 2.6.  Kernel 2.4 is not supported.  Using
+kernel 2.6.10 or later is recommended, as earlier kernels are known to have
+unstable USB 2.0 support.
+
+A fully built kernel source tree must be available.  Typically this will be
+linked from "/lib/modules/<KERNEL VERSION>/build" for convenience.  If this
+link does not exist, an extra parameter will need to be passed to the
+`make` command.
+
+All vendor-built kernels should already be configured properly.  However,
+for custom-built kernels, the following options need to be enabled in the
+kernel as built-in or modules:
+
+        CONFIG_HOTPLUG           - Support for hot-pluggable devices
+        CONFIG_MODULES           - Enable loadable module support
+        CONFIG_KMOD              - Automatic kernel module loading
+        CONFIG_FW_LOADER         - Hotplug firmware loading support
+        CONFIG_I2C               - I2C support
+        CONFIG_VIDEO_DEV         - Video For Linux
+        CONFIG_SOUND             - Sound card support
+        CONFIG_SND               - Advanced Linux Sound Architecture
+        CONFIG_USB               - Support for Host-side USB
+        CONFIG_USB_DEVICEFS      - USB device filesystem
+        CONFIG_USB_EHCI_HCD      - EHCI HCD (USB 2.0) support
+
+Additionally, to use the example application, the following options need to
+be enabled in the ALSA section:
+
+        CONFIG_SND_MIXER_OSS     - OSS Mixer API
+        CONFIG_SND_PCM_OSS       - OSS PCM (digital audio) API
+
+The hotplug scripts, along with the fxload utility, must also be installed.
+These scripts can be obtained from <http://linux-hotplug.sourceforge.net/>.
+Hotplugging is used for loading firmware into the Cypruss EZ-USB chip using
+fxload and for loading firmware into the driver using the firmware agent.
+
+
+  2. COMPILING AND INSTALLING THE DRIVER
+
+
+Most users should be able to compile the driver by simply running:
+
+        $ make
+
+in the top-level directory of the driver kit.  First the kernel modules
+will be built, followed by the example applications.
+
+If the build system is unable to locate the kernel source tree for the
+currently-running kernel, or if the module should be built for a kernel
+other than the currently-running kernel, an additional parameter will need
+to be passed to make to specify the appropriate kernel source directory:
+
+        $ make KERNELSRC=/usr/src/linux-2.6.10-custom3
+
+Once the compile completes, the driver and firmware files should be
+installed by running:
+
+        $ make install
+
+The kernel modules will be placed in "/lib/modules/<KERNEL VERSION>/extra"
+and the firmware files will be placed in the appropriate hotplug firmware
+directory, usually /lib/firmware.  In addition, USB maps and scripts will
+be placed in /etc/hotplug/usb to enable fxload to initialize the EZ-USB
+control chip when the device is connected.
+
+
+  3. PAL/SECAM TUNER CONFIGURATION (TV402U-EU only)
+
+
+The PAL model of the Plextor ConvertX TV402U may require additional
+configuration to correctly select the appropriate TV frequency band and
+audio subchannel.
+
+Users with a device other than the Plextor ConvertX TV402U-EU should skip
+this section.
+
+The wide variety of PAL TV systems used in Europe requires that additional
+information about the local TV standards be passed to the driver in order
+to properly tune TV channels.  The two necessary parameters are (a) the PAL
+TV band, and (b) the audio subchannel format in use.
+
+In many cases, the appropriate TV band selection is passed to the driver
+from applications.  However, in some cases, the application only specifies
+that the driver should use PAL but not the specific information about the
+appropriate TV band.  To work around this issue, the correct TV band may be
+specified in the "force_band" parameter to the wis-sony-tuner module:
+
+     TV band           force_band
+     -------           ----------
+     PAL B/G                B
+     PAL I                  I
+     PAL D/K                D
+     SECAM L                L
+
+If the "force_band" parameter is specified, the driver will ignore any TV
+band specified by applications and will always use the band provided in the
+module parameter.
+
+The other parameter that can be specified is the audio subchannel format.
+There are several stereo audio carrier systems in use, including NICAM and
+three varieties of A2.  To receive audio broadcast on one of these stereo
+carriers, the "force_mpx_mode" parameter must be specified to the
+wis-sony-tuner module.
+
+     TV band           Audio subcarrier       force_mpx_mode
+     -------           ----------------       --------------
+     PAL B/G            Mono (default)               1
+     PAL B/G                  A2                     2
+     PAL B/G                 NICAM                   3
+     PAL I              Mono (default)               4
+     PAL I                   NICAM                   5
+     PAL D/K            Mono (default)               6
+     PAL D/K                 A2 (1)                  7
+     PAL D/K                 A2 (2)                  8
+     PAL D/K                 A2 (3)                  9
+     PAL D/K                 NICAM                  10
+     SECAM L            Mono (default)              11
+     SECAM L                 NICAM                  12
+
+If the "force_mpx_mode" parameter is not specified, the correct mono-only
+mode will be chosen based on the TV band.  However, the tuner will not
+receive stereo audio or bilingual broadcasts correctly.
+
+To pass the "force_band" or "force_mpx_mode" parameters to the
+wis-sony-tuner module, the following line must be added to the modprobe
+configuration file, which varies from one Linux distribution to another.
+
+     options wis-sony-tuner force_band=B force_mpx_mode=2
+
+The above example would force the tuner to the PAL B/G TV band and receive
+stereo audio broadcasts on the A2 carrier.
+
+To verify that the configuration has been placed in the correct location,
+execute:
+
+        $ modprobe -c | grep wis-sony-tuner
+
+If the configuration line appears, then modprobe will pass the parameters
+correctly the next time the wis-sony-tuner module is loaded into the
+kernel.
+
+
+  4. TESTING THE DRIVER
+
+
+Because few Linux applications are able to correctly capture from
+Video4Linux2 devices with only compressed formats supported, the new driver
+should be tested with the "gorecord" application in the apps/ directory.
+
+First connect a video source to the device, such as a DVD player or VCR.
+This will be captured to a file for testing the driver.  If an input source
+is unavailable, a test file can still be captured, but the video will be
+black and the audio will be silent.
+
+This application will auto-detect the V4L2 and ALSA/OSS device names of the
+hardware and will record video and audio to an AVI file for a specified
+number of seconds.  For example:
+
+        $ apps/gorecord -duration 60 capture.avi
+
+If this application does not successfully record an AVI file, the error
+messages produced by gorecord and recorded in the system log (usually in
+/var/log/messages) should provide information to help resolve the problem.
+
+Supplying no parameters to gorecord will cause it to probe the available
+devices and exit.  Use the -help flag for usage information.
+
+
+  5. USING THE DRIVER
+
+
+The V4L2 device implemented by the driver provides a standard compressed
+format API, within the following criteria:
+
+  * Applications that only support the original Video4Linux1 API will not
+    be able to communicate with this driver at all.
+
+  * No raw video modes are supported, so applications like xawtv that
+    expect only uncompressed video will not function.
+
+  * Supported compression formats are: Motion-JPEG, MPEG1, MPEG2 and MPEG4.
+
+  * MPEG video formats are delivered as Video Elementary Streams only.
+    Program Stream (PS), Transport Stream (TS) and Packetized Elementary
+    Stream (PES) formats are not supported.
+
+  * Video parameters such as format and input port may not be changed while
+    the encoder is active.
+
+  * The audio capture device only functions when the video encoder is
+    actively capturing video.  Attempts to read from the audio device when
+    the encoder is inactive will result in an I/O error.
+
+  * The native format of the audio device is 48Khz 2-channel 16-bit
+    little-endian PCM, delivered through the ALSA system.  No audio
+    compression is implemented in the hardware.  ALSA may convert to other
+    uncompressed formats on the fly.
+
+The include/ directory contains a C header file describing non-standard
+features of the GO7007SB encoder, which are described below:
+
+
+  GO7007IOC_S_COMP_PARAMS, GO7007IOC_G_COMP_PARAMS
+
+    These ioctls are used to negotiate general compression parameters.
+
+    To query the current parameters, call the GO7007IOC_G_COMP_PARAMS ioctl
+    with a pointer to a struct go7007_comp_params.  If the driver is not
+    set to MPEG format, the EINVAL error code will be returned.
+
+    To change the current parameters, initialize all fields of a struct
+    go7007_comp_params and call the GO7007_IOC_S_COMP_PARAMS ioctl with a
+    pointer to this structure.  The driver will return the current
+    parameters with any necessary changes to conform to the limitations of
+    the hardware or current compression mode.  Any or all fields can be set
+    to zero to request a reasonable default value.  If the driver is not
+    set to MPEG format, the EINVAL error code will be returned.  When I/O
+    is in progress, the EBUSY error code will be returned.
+
+    Fields in struct go7007_comp_params:
+
+        __u32                        The maximum number of frames in each
+          gop_size                   Group Of Pictures; i.e. the maximum
+                                     number of frames minus one between
+                                     each key frame.
+
+        __u32                        The maximum number of sequential
+          max_b_frames               bidirectionally-predicted frames.
+                                     (B-frames are not yet supported.)
+
+        enum go7007_aspect_ratio     The aspect ratio to be encoded in the
+          aspect_ratio               meta-data of the compressed format.
+
+                                     Choices are:
+                                        GO7007_ASPECT_RATIO_1_1
+                                        GO7007_ASPECT_RATIO_4_3_NTSC
+                                        GO7007_ASPECT_RATIO_4_3_PAL
+                                        GO7007_ASPECT_RATIO_16_9_NTSC
+                                        GO7007_ASPECT_RATIO_16_9_PAL
+
+        __u32                        Bit-wise OR of control flags (below)
+          flags
+
+    Flags in struct go7007_comp_params:
+
+        GO7007_COMP_CLOSED_GOP       Only produce self-contained GOPs, used
+                                     to produce streams appropriate for
+                                     random seeking.
+
+        GO7007_COMP_OMIT_SEQ_HEADER  Omit the stream sequence header.
+
+
+  GO7007IOC_S_MPEG_PARAMS, GO7007IOC_G_MPEG_PARAMS
+
+    These ioctls are used to negotiate MPEG-specific stream parameters when
+    the pixelformat has been set to V4L2_PIX_FMT_MPEG.
+
+    To query the current parameters, call the GO7007IOC_G_MPEG_PARAMS ioctl
+    with a pointer to a struct go7007_mpeg_params.  If the driver is not
+    set to MPEG format, the EINVAL error code will be returned.
+
+    To change the current parameters, initialize all fields of a struct
+    go7007_mpeg_params and call the GO7007_IOC_S_MPEG_PARAMS ioctl with a
+    pointer to this structure.  The driver will return the current
+    parameters with any necessary changes to conform to the limitations of
+    the hardware or selected MPEG mode.  Any or all fields can be set to
+    zero to request a reasonable default value.  If the driver is not set
+    to MPEG format, the EINVAL error code will be returned.  When I/O is in
+    progress, the EBUSY error code will be returned.
+
+    Fields in struct go7007_mpeg_params:
+
+        enum go7007_mpeg_video_standard
+          mpeg_video_standard        The MPEG video standard in which to
+                                     compress the video.
+
+                                     Choices are:
+                                        GO7007_MPEG_VIDEO_MPEG1
+                                        GO7007_MPEG_VIDEO_MPEG2
+                                        GO7007_MPEG_VIDEO_MPEG4
+
+        __u32                        Bit-wise OR of control flags (below)
+          flags
+
+        __u32                        The profile and level indication to be
+          pali                       stored in the sequence header.  This
+                                     is only used as an indicator to the
+                                     decoder, and does not affect the MPEG
+                                     features used in the video stream.
+                                     Not valid for MPEG1.
+
+                                     Choices for MPEG2 are:
+                                        GO7007_MPEG2_PROFILE_MAIN_MAIN
+
+                                     Choices for MPEG4 are:
+                                        GO7007_MPEG4_PROFILE_S_L0
+                                        GO7007_MPEG4_PROFILE_S_L1
+                                        GO7007_MPEG4_PROFILE_S_L2
+                                        GO7007_MPEG4_PROFILE_S_L3
+                                        GO7007_MPEG4_PROFILE_ARTS_L1
+                                        GO7007_MPEG4_PROFILE_ARTS_L2
+                                        GO7007_MPEG4_PROFILE_ARTS_L3
+                                        GO7007_MPEG4_PROFILE_ARTS_L4
+                                        GO7007_MPEG4_PROFILE_AS_L0
+                                        GO7007_MPEG4_PROFILE_AS_L1
+                                        GO7007_MPEG4_PROFILE_AS_L2
+                                        GO7007_MPEG4_PROFILE_AS_L3
+                                        GO7007_MPEG4_PROFILE_AS_L4
+                                        GO7007_MPEG4_PROFILE_AS_L5
+
+    Flags in struct go7007_mpeg_params:
+
+        GO7007_MPEG_FORCE_DVD_MODE   Force all compression parameters and
+                                     bitrate control settings to comply
+                                     with DVD MPEG2 stream requirements.
+                                     This overrides most compression and
+                                     bitrate settings!
+
+        GO7007_MPEG_OMIT_GOP_HEADER  Omit the GOP header.
+
+        GO7007_MPEG_REPEAT_SEQHEADER Repeat the MPEG sequence header at
+                                     the start of each GOP.
+
+
+  GO7007IOC_S_BITRATE, GO7007IOC_G_BITRATE
+
+    These ioctls are used to set and query the target bitrate value for the
+    compressed video stream.  The bitrate may be selected by storing the
+    target bits per second in an int and calling GO7007IOC_S_BITRATE with a
+    pointer to the int.  The bitrate may be queried by calling
+    GO7007IOC_G_BITRATE with a pointer to an int where the current bitrate
+    will be stored.
+
+    Note that this is the primary means of controlling the video quality
+    for all compression modes, including V4L2_PIX_FMT_MJPEG.  The
+    VIDIOC_S_JPEGCOMP ioctl is not supported.
+
+
+----------------------------------------------------------------------------
+                   Installing the WIS PCI Voyager Driver
+---------------------------------------------------------------------------
+
+The WIS PCI Voyager driver requires several patches to the Linux 2.6.11.x
+kernel source tree before compiling the driver.  These patches update the
+in-kernel SAA7134 driver to the newest development version and patch bugs
+in the TDA8290/TDA8275 tuner driver.
+
+The following patches must be downloaded from Gerd Knorr's website and
+applied in the order listed:
+
+	http://dl.bytesex.org/patches/2.6.11-2/i2c-tuner
+	http://dl.bytesex.org/patches/2.6.11-2/i2c-tuner2
+	http://dl.bytesex.org/patches/2.6.11-2/v4l2-api-mpeg
+	http://dl.bytesex.org/patches/2.6.11-2/saa7134-update
+
+The following patches are included with this SDK and can be applied in any
+order:
+
+	patches/2.6.11/saa7134-voyager.diff
+	patches/2.6.11/tda8275-newaddr.diff
+	patches/2.6.11/tda8290-ntsc.diff
+
+Check to make sure the CONFIG_VIDEO_SAA7134 option is enabled in the kernel
+configuration, and build and install the kernel.
+
+After rebooting into the new kernel, the GO7007 driver can be compiled and
+installed:
+
+	$ make SAA7134_BUILD=y
+	$ make install
+	$ modprobe saa7134-go7007
+
+There will be two V4L video devices associated with the PCI Voyager.  The
+first device (most likely /dev/video0) provides access to the raw video
+capture mode of the SAA7133 device and is used to configure the source
+video parameters and tune the TV tuner.  This device can be used with xawtv
+or other V4L(2) video software as a standard uncompressed device.
+
+The second device (most likely /dev/video1) provides access to the
+compression functions of the GO7007.  It can be tested using the gorecord
+application in the apps/ directory of this SDK:
+
+	$ apps/gorecord -vdevice /dev/video1 -noaudio test.avi
+
+Currently the frame resolution is fixed at 720x480 (NTSC) or 720x576 (PAL),
+and the video standard must be specified to both the raw and the compressed
+video devices (xawtv and gorecord, for example).
+
+
+--------------------------------------------------------------------------
+RELEASE NOTES FOR WIS GO7007SB LINUX DRIVER
+---------------------------------------------------------------------------
+
+Last updated: 5 November 2005
+
+ - Release 0.9.7 includes new support for using udev to run fxload.  The
+   install script should automatically detect whether the old hotplug
+   scripts or the new udev rules should be used.  To force the use of
+   hotplug, run "make install USE_UDEV=n".  To force the use of udev, run
+   "make install USE_UDEV=y".
+
+ - Motion detection is supported but undocumented.  Try the `modet` app
+   for a demonstration of how to use the facility.
+
+ - Using USB2.0 devices such as the TV402U with USB1.1 HCDs or hubs can
+   cause buffer overruns and frame drops, even at low framerates, due to
+   inconsistency in the bitrate control mechanism.
+
+ - On devices with an SAA7115, including the Plextor ConvertX, video height
+   values of 96, 128, 160, 192, 256, 320, and 384 do not work in NTSC mode.
+   All valid heights up to 512 work correctly in PAL mode.
+
+ - The WIS Star Trek and PCI Voyager boards have no support yet for audio
+   or the TV tuner.
diff --git a/drivers/staging/go7007/s2250-board.c b/drivers/staging/go7007/s2250-board.c
new file mode 100644
index 000000000000..fb6845e37884
--- /dev/null
+++ b/drivers/staging/go7007/s2250-board.c
@@ -0,0 +1,630 @@
+/*
+ * Copyright (C) 2008 Sensoray Company Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License (Version 2) as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/usb.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include "go7007-priv.h"
+#include "wis-i2c.h"
+
+extern int s2250loader_init(void);
+extern void s2250loader_cleanup(void);
+
+#define TLV320_ADDRESS      0x34
+#define S2250_VIDDEC        0x86
+#define VPX322_ADDR_ANALOGCONTROL1	0x02
+#define VPX322_ADDR_BRIGHTNESS0		0x0127
+#define VPX322_ADDR_BRIGHTNESS1		0x0131
+#define VPX322_ADDR_CONTRAST0		0x0128
+#define VPX322_ADDR_CONTRAST1		0x0132
+#define VPX322_ADDR_HUE			0x00dc
+#define VPX322_ADDR_SAT	 	        0x0030
+
+struct go7007_usb_board {
+	unsigned int flags;
+	struct go7007_board_info main_info;
+};
+
+struct go7007_usb {
+	struct go7007_usb_board *board;
+	struct semaphore i2c_lock;
+	struct usb_device *usbdev;
+	struct urb *video_urbs[8];
+	struct urb *audio_urbs[8];
+	struct urb *intr_urb;
+};
+
+static unsigned char aud_regs[] = {
+	0x1e, 0x00,
+	0x00, 0x17,
+	0x02, 0x17,
+	0x04, 0xf9,
+	0x06, 0xf9,
+	0x08, 0x02,
+	0x0a, 0x00,
+	0x0c, 0x00,
+	0x0a, 0x00,
+	0x0c, 0x00,
+	0x0e, 0x02,
+	0x10, 0x00,
+	0x12, 0x01,
+	0x00, 0x00,
+};
+
+
+static unsigned char vid_regs[] = {
+	0xF2, 0x0f,
+	0xAA, 0x00,
+	0xF8, 0xff,
+	0x00, 0x00,
+};
+
+static u16 vid_regs_fp[] = {
+	0x028, 0x067,
+	0x120, 0x016,
+	0x121, 0xcF2,
+	0x122, 0x0F2,
+	0x123, 0x00c,
+	0x124, 0x2d0,
+	0x125, 0x2e0,
+	0x126, 0x004,
+	0x128, 0x1E0,
+	0x12A, 0x016,
+	0x12B, 0x0F2,
+	0x12C, 0x0F2,
+	0x12D, 0x00c,
+	0x12E, 0x2d0,
+	0x12F, 0x2e0,
+	0x130, 0x004,
+	0x132, 0x1E0,
+	0x140, 0x060,
+	0x153, 0x00C,
+	0x154, 0x200,
+	0x150, 0x801,
+	0x000, 0x000
+};
+
+/* PAL specific values */
+static u16 vid_regs_fp_pal[] =
+{
+	0x120, 0x017,
+	0x121, 0xd22,
+	0x122, 0x122,
+	0x12A, 0x017,
+	0x12B, 0x122,
+	0x12C, 0x122,
+	0x140, 0x060,
+	0x000, 0x000,
+};
+
+struct s2250 {
+	int std;
+	int input;
+	int brightness;
+	int contrast;
+	int saturation;
+	int hue;
+	int reg12b_val;
+	int audio_input;
+};
+
+/* from go7007-usb.c which is Copyright (C) 2005-2006 Micronas USA Inc.*/
+static int go7007_usb_vendor_request(struct go7007 *go, u16 request,
+	u16 value, u16 index, void *transfer_buffer, int length, int in)
+{
+	struct go7007_usb *usb = go->hpi_context;
+	int timeout = 5000;
+
+	if (in) {
+		return usb_control_msg(usb->usbdev,
+				usb_rcvctrlpipe(usb->usbdev, 0), request,
+				USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
+				value, index, transfer_buffer, length, timeout);
+	} else {
+		return usb_control_msg(usb->usbdev,
+				usb_sndctrlpipe(usb->usbdev, 0), request,
+				USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+				value, index, transfer_buffer, length, timeout);
+	}
+}
+/* end from go7007-usb.c which is Copyright (C) 2005-2006 Micronas USA Inc.*/
+
+static int write_reg(struct i2c_client *client, u8 reg, u8 value)
+{
+	struct go7007 *go = i2c_get_adapdata(client->adapter);
+	struct go7007_usb *usb = go->hpi_context;
+	int rc;
+	int dev_addr = client->addr;
+	u8 *buf;
+
+	if (go == NULL)
+		return -ENODEV;
+
+	if (go->status == STATUS_SHUTDOWN)
+		return -EBUSY;
+
+	buf = kzalloc(16, GFP_KERNEL);
+	if (buf == NULL)
+		return -ENOMEM;
+
+	if (down_interruptible(&usb->i2c_lock) != 0) {
+		printk(KERN_INFO "i2c lock failed\n");
+		return -EINTR;
+	}
+	rc = go7007_usb_vendor_request(go, 0x55, dev_addr,
+				       (reg<<8 | value),
+				       buf,
+				       16, 1);
+
+	up(&usb->i2c_lock);
+	kfree(buf);
+	return rc;
+}
+
+static int write_reg_fp(struct i2c_client *client, u16 addr, u16 val)
+{
+	struct go7007 *go = i2c_get_adapdata(client->adapter);
+	struct go7007_usb *usb = go->hpi_context;
+	u8 *buf;
+	struct s2250 *dec = i2c_get_clientdata(client);
+
+	if (go == NULL)
+		return -ENODEV;
+
+	if (go->status == STATUS_SHUTDOWN)
+		return -EBUSY;
+
+	buf = kzalloc(16, GFP_KERNEL);
+
+	if (buf == NULL)
+		return -ENOMEM;
+
+
+
+	memset(buf, 0xcd, 6);
+
+	if (down_interruptible(&usb->i2c_lock) != 0) {
+		printk(KERN_INFO "i2c lock failed\n");
+		return -EINTR;
+	}
+	if (go7007_usb_vendor_request(go, 0x57, addr, val, buf, 16, 1) < 0)
+		return -EFAULT;
+
+	up(&usb->i2c_lock);
+	if (buf[0] == 0) {
+		unsigned int subaddr, val_read;
+
+		subaddr = (buf[4] << 8) + buf[5];
+		val_read = (buf[2] << 8) + buf[3];
+		if (val_read != val) {
+			printk(KERN_INFO "invalid fp write %x %x\n",
+			       val_read, val);
+			return -EFAULT;
+		}
+		if (subaddr != addr) {
+			printk(KERN_INFO "invalid fp write addr %x %x\n",
+			       subaddr, addr);
+			return -EFAULT;
+		}
+	} else
+		return -EFAULT;
+
+	/* save last 12b value */
+	if (addr == 0x12b)
+		dec->reg12b_val = val;
+
+	return 0;
+}
+
+static int write_regs(struct i2c_client *client, u8 *regs)
+{
+	int i;
+
+	for (i = 0; !((regs[i] == 0x00) && (regs[i+1] == 0x00)); i += 2) {
+		if (write_reg(client, regs[i], regs[i+1]) < 0) {
+			printk(KERN_INFO "s2250: failed\n");
+			return -1;
+		}
+	}
+	return 0;
+}
+
+static int write_regs_fp(struct i2c_client *client, u16 *regs)
+{
+	int i;
+
+	for (i = 0; !((regs[i] == 0x00) && (regs[i+1] == 0x00)); i += 2) {
+		if (write_reg_fp(client, regs[i], regs[i+1]) < 0) {
+			printk(KERN_INFO "s2250: failed fp\n");
+			return -1;
+		}
+	}
+	return 0;
+}
+
+
+static int s2250_command(struct i2c_client *client,
+			 unsigned int cmd, void *arg)
+{
+	struct s2250 *dec = i2c_get_clientdata(client);
+
+	switch (cmd) {
+	case VIDIOC_S_INPUT:
+	{
+		int vidsys;
+		int *input = arg;
+
+		vidsys = (dec->std == V4L2_STD_NTSC) ? 0x01 : 0x00;
+		if (*input == 0) {
+			/* composite */
+			write_reg_fp(client, 0x20, 0x020 | vidsys);
+			write_reg_fp(client, 0x21, 0x662);
+			write_reg_fp(client, 0x140, 0x060);
+		} else {
+			/* S-Video */
+			write_reg_fp(client, 0x20, 0x040 | vidsys);
+			write_reg_fp(client, 0x21, 0x666);
+			write_reg_fp(client, 0x140, 0x060);
+		}
+		dec->input = *input;
+		break;
+	}
+	case VIDIOC_S_STD:
+	{
+		v4l2_std_id *std = arg;
+		u16 vidsource;
+
+		vidsource = (dec->input == 1) ? 0x040 : 0x020;
+		dec->std = *std;
+		switch (dec->std) {
+		case V4L2_STD_NTSC:
+			write_regs_fp(client, vid_regs_fp);
+			write_reg_fp(client, 0x20, vidsource | 1);
+			break;
+		case V4L2_STD_PAL:
+			write_regs_fp(client, vid_regs_fp);
+			write_regs_fp(client, vid_regs_fp_pal);
+			write_reg_fp(client, 0x20, vidsource);
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	}
+	case VIDIOC_QUERYCTRL:
+	{
+		struct v4l2_queryctrl *ctrl = arg;
+		static const u32 user_ctrls[] = {
+			V4L2_CID_BRIGHTNESS,
+			V4L2_CID_CONTRAST,
+			V4L2_CID_SATURATION,
+			V4L2_CID_HUE,
+			0
+		};
+		static const u32 *ctrl_classes[] = {
+			user_ctrls,
+			NULL
+		};
+
+		ctrl->id = v4l2_ctrl_next(ctrl_classes, ctrl->id);
+		switch (ctrl->id) {
+		case V4L2_CID_BRIGHTNESS:
+			v4l2_ctrl_query_fill(ctrl, 0, 100, 1, 50);
+			break;
+		case V4L2_CID_CONTRAST:
+			v4l2_ctrl_query_fill(ctrl, 0, 100, 1, 50);
+			break;
+		case V4L2_CID_SATURATION:
+			v4l2_ctrl_query_fill(ctrl, 0, 100, 1, 50);
+			break;
+		case V4L2_CID_HUE:
+			v4l2_ctrl_query_fill(ctrl, -50, 50, 1, 0);
+			break;
+		default:
+			ctrl->name[0] = '\0';
+			return -EINVAL;
+		}
+		break;
+	}
+	case VIDIOC_S_CTRL:
+	{
+		struct v4l2_control *ctrl = arg;
+		int value1;
+
+		switch (ctrl->id) {
+		case V4L2_CID_BRIGHTNESS:
+			printk(KERN_INFO "s2250: future setting\n");
+			return -EINVAL;
+		case V4L2_CID_CONTRAST:
+			printk(KERN_INFO "s2250: future setting\n");
+			return -EINVAL;
+			break;
+		case V4L2_CID_SATURATION:
+			if (ctrl->value > 127)
+				dec->saturation = 127;
+			else if (ctrl->value < 0)
+				dec->saturation = 0;
+			else
+				dec->saturation = ctrl->value;
+
+			value1 = dec->saturation * 4140 / 100;
+			if (value1 > 4094)
+				value1 = 4094;
+			write_reg_fp(client, VPX322_ADDR_SAT, value1);
+			break;
+		case V4L2_CID_HUE:
+			if (ctrl->value > 50)
+				dec->hue = 50;
+			else if (ctrl->value < -50)
+				dec->hue = -50;
+			else
+				dec->hue = ctrl->value;
+			/* clamp the hue range */
+			value1 = dec->hue * 280 / 50;
+			write_reg_fp(client, VPX322_ADDR_HUE, value1);
+			break;
+		}
+		break;
+	}
+	case VIDIOC_G_CTRL:
+	{
+		struct v4l2_control *ctrl = arg;
+
+		switch (ctrl->id) {
+		case V4L2_CID_BRIGHTNESS:
+			ctrl->value = dec->brightness;
+			break;
+		case V4L2_CID_CONTRAST:
+			ctrl->value = dec->contrast;
+			break;
+		case V4L2_CID_SATURATION:
+			ctrl->value = dec->saturation;
+			break;
+		case V4L2_CID_HUE:
+			ctrl->value = dec->hue;
+			break;
+		}
+		break;
+	}
+	case VIDIOC_S_FMT:
+	{
+		struct v4l2_format *fmt = arg;
+		if (fmt->fmt.pix.height < 640) {
+			write_reg_fp(client, 0x12b, dec->reg12b_val | 0x400);
+			write_reg_fp(client, 0x140, 0x060);
+		} else {
+			write_reg_fp(client, 0x12b, dec->reg12b_val & ~0x400);
+			write_reg_fp(client, 0x140, 0x060);
+		}
+		return 0;
+	}
+	case VIDIOC_G_AUDIO:
+	{
+		struct v4l2_audio *audio = arg;
+
+		memset(audio, 0, sizeof(*audio));
+		audio->index = dec->audio_input;
+		/* fall through */
+	}
+	case VIDIOC_ENUMAUDIO:
+	{
+		struct v4l2_audio *audio = arg;
+
+		switch (audio->index) {
+		case 0:
+			strcpy(audio->name, "Line In");
+			break;
+		case 1:
+			strcpy(audio->name, "Mic");
+			break;
+		case 2:
+			strcpy(audio->name, "Mic Boost");
+			break;
+		default:
+			audio->name[0] = '\0';
+			return 0;
+		}
+		audio->capability = V4L2_AUDCAP_STEREO;
+		audio->mode = 0;
+		return 0;
+	}
+	case VIDIOC_S_AUDIO:
+	{
+		struct v4l2_audio *audio = arg;
+
+		client->addr = TLV320_ADDRESS;
+		switch (audio->index) {
+		case 0:
+			write_reg(client, 0x08, 0x02); /* Line In */
+			break;
+		case 1:
+			write_reg(client, 0x08, 0x04); /* Mic */
+			break;
+		case 2:
+			write_reg(client, 0x08, 0x05); /* Mic Boost */
+			break;
+		default:
+			return -EINVAL;
+		}
+		dec->audio_input = audio->index;
+		return 0;
+	}
+
+	default:
+		printk(KERN_INFO "s2250: unknown command 0x%x\n", cmd);
+		break;
+	}
+	return 0;
+}
+
+static struct i2c_driver s2250_driver;
+
+static struct i2c_client s2250_client_templ = {
+	.name		= "Sensoray 2250",
+	.driver		= &s2250_driver,
+};
+
+static int s2250_detect(struct i2c_adapter *adapter, int addr, int kind)
+{
+	struct i2c_client *client;
+	struct s2250 *dec;
+	u8 *data;
+	struct go7007 *go = i2c_get_adapdata(adapter);
+	struct go7007_usb *usb = go->hpi_context;
+
+	client = kmalloc(sizeof(struct i2c_client), GFP_KERNEL);
+	if (client == NULL)
+		return -ENOMEM;
+	memcpy(client, &s2250_client_templ,
+	       sizeof(s2250_client_templ));
+	client->adapter = adapter;
+
+	dec = kmalloc(sizeof(struct s2250), GFP_KERNEL);
+	if (dec == NULL) {
+		kfree(client);
+		return -ENOMEM;
+	}
+
+	dec->std = V4L2_STD_NTSC;
+	dec->brightness = 50;
+	dec->contrast = 50;
+	dec->saturation = 50;
+	dec->hue = 0;
+	client->addr = TLV320_ADDRESS;
+	i2c_set_clientdata(client, dec);
+
+	printk(KERN_DEBUG
+	       "s2250: initializing video decoder on %s\n",
+	       adapter->name);
+
+	/* initialize the audio */
+	client->addr = TLV320_ADDRESS;
+	if (write_regs(client, aud_regs) < 0) {
+		printk(KERN_ERR
+		       "s2250: error initializing audio\n");
+		kfree(client);
+		kfree(dec);
+		return 0;
+	}
+	client->addr = S2250_VIDDEC;
+	i2c_set_clientdata(client, dec);
+
+	if (write_regs(client, vid_regs) < 0) {
+		printk(KERN_ERR
+		       "s2250: error initializing decoder\n");
+		kfree(client);
+		kfree(dec);
+		return 0;
+	}
+	if (write_regs_fp(client, vid_regs_fp) < 0) {
+		printk(KERN_ERR
+		       "s2250: error initializing decoder\n");
+		kfree(client);
+		kfree(dec);
+		return 0;
+	}
+	/* set default channel */
+	/* composite */
+	write_reg_fp(client, 0x20, 0x020 | 1);
+	write_reg_fp(client, 0x21, 0x662);
+	write_reg_fp(client, 0x140, 0x060);
+
+	/* set default audio input */
+	dec->audio_input = 0;
+	write_reg(client, 0x08, 0x02); /* Line In */
+
+	if (down_interruptible(&usb->i2c_lock) == 0) {
+		data = kzalloc(16, GFP_KERNEL);
+		if (data != NULL) {
+			int rc;
+			rc = go7007_usb_vendor_request(go, 0x41, 0, 0,
+						       data, 16, 1);
+			if (rc > 0) {
+				u8 mask;
+				data[0] = 0;
+				mask = 1<<5;
+				data[0] &= ~mask;
+				data[1] |= mask;
+				go7007_usb_vendor_request(go, 0x40, 0,
+							  (data[1]<<8)
+							  + data[1],
+							  data, 16, 0);
+			}
+			kfree(data);
+		}
+		up(&usb->i2c_lock);
+	}
+
+	i2c_attach_client(client);
+	printk("s2250: initialized successfully\n");
+	return 0;
+}
+
+static int s2250_detach(struct i2c_client *client)
+{
+	struct s2250 *dec = i2c_get_clientdata(client);
+	int r;
+
+	r = i2c_detach_client(client);
+	if (r < 0)
+		return r;
+
+	kfree(client);
+	kfree(dec);
+	return 0;
+}
+
+static struct i2c_driver s2250_driver = {
+	.driver = {
+		.name	= "Sensoray 2250 board driver",
+	},
+	.id		= I2C_DRIVERID_S2250,
+	.detach_client	= s2250_detach,
+	.command	= s2250_command,
+};
+
+static int __init s2250_init(void)
+{
+	int r;
+
+	r = s2250loader_init();
+	if (r < 0)
+		return r;
+
+	r = i2c_add_driver(&s2250_driver);
+	if (r < 0)
+		return r;
+	return wis_i2c_add_driver(s2250_driver.id, s2250_detect);
+}
+
+static void __exit s2250_cleanup(void)
+{
+	wis_i2c_del_driver(s2250_detect);
+	i2c_del_driver(&s2250_driver);
+
+	s2250loader_cleanup();
+}
+
+module_init(s2250_init);
+module_exit(s2250_cleanup);
+
+MODULE_AUTHOR("");
+MODULE_DESCRIPTION("Board driver for Sensoryray 2250");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/go7007/s2250-loader.c b/drivers/staging/go7007/s2250-loader.c
new file mode 100644
index 000000000000..a5e4acab089e
--- /dev/null
+++ b/drivers/staging/go7007/s2250-loader.c
@@ -0,0 +1,188 @@
+/*
+ * Copyright (C) 2008 Sensoray Company Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License (Version 2) as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/usb.h>
+#include <dvb-usb.h>
+
+#define S2250_LOADER_FIRMWARE	"s2250_loader.fw"
+#define S2250_FIRMWARE		"s2250.fw"
+
+typedef struct device_extension_s {
+    struct kref     kref;
+    int minor;
+    struct usb_device *usbdev;
+} device_extension_t, *pdevice_extension_t;
+
+#define USB_s2250loader_MAJOR 240
+#define USB_s2250loader_MINOR_BASE 0
+#define MAX_DEVICES 256
+
+static pdevice_extension_t s2250_dev_table[MAX_DEVICES];
+static DECLARE_MUTEX(s2250_dev_table_mutex);
+
+#define to_s2250loader_dev_common(d) container_of(d, device_extension_t, kref)
+static void s2250loader_delete(struct kref *kref)
+{
+	pdevice_extension_t s = to_s2250loader_dev_common(kref);
+	s2250_dev_table[s->minor] = NULL;
+	kfree(s);
+}
+
+static int s2250loader_probe(struct usb_interface *interface,
+				const struct usb_device_id *id)
+{
+	struct usb_device *usbdev;
+	int minor, ret;
+	pdevice_extension_t s = NULL;
+	const struct firmware *fw;
+
+	usbdev = usb_get_dev(interface_to_usbdev(interface));
+	if (!usbdev) {
+		printk(KERN_ERR "Enter s2250loader_probe failed\n");
+		return -1;
+	}
+	printk(KERN_INFO "Enter s2250loader_probe 2.6 kernel\n");
+	printk(KERN_INFO "vendor id 0x%x, device id 0x%x devnum:%d\n",
+	   usbdev->descriptor.idVendor, usbdev->descriptor.idProduct,
+	   usbdev->devnum);
+
+	if (usbdev->descriptor.bNumConfigurations != 1) {
+		printk(KERN_ERR "can't handle multiple config\n");
+		return -1;
+	}
+	down(&s2250_dev_table_mutex);
+
+	for (minor = 0; minor < MAX_DEVICES; minor++) {
+		if (s2250_dev_table[minor] == NULL)
+			break;
+	}
+
+	if (minor < 0 || minor >= MAX_DEVICES) {
+		printk(KERN_ERR "Invalid minor: %d\n", minor);
+		goto failed;
+	}
+
+	/* Allocate dev data structure */
+	s = kmalloc(sizeof(device_extension_t), GFP_KERNEL);
+	if (s == NULL) {
+		printk(KERN_ERR "Out of memory\n");
+		goto failed;
+	}
+	s2250_dev_table[minor] = s;
+
+	printk(KERN_INFO "s2250loader_probe: Device %d on Bus %d Minor %d\n",
+		usbdev->devnum, usbdev->bus->busnum, minor);
+
+	memset(s, 0, sizeof(device_extension_t));
+	s->usbdev = usbdev;
+	printk(KERN_INFO "loading 2250 loader\n");
+
+	kref_init(&(s->kref));
+
+	up(&s2250_dev_table_mutex);
+
+	if (request_firmware(&fw, S2250_LOADER_FIRMWARE, &usbdev->dev)) {
+		printk(KERN_ERR
+			"s2250: unable to load firmware from file \"%s\"\n",
+			S2250_LOADER_FIRMWARE);
+		goto failed2;
+	}
+	ret = usb_cypress_load_firmware(usbdev, fw, CYPRESS_FX2);
+	release_firmware(fw);
+	if (0 != ret) {
+		printk(KERN_ERR "loader download failed\n");
+		goto failed2;
+	}
+
+	if (request_firmware(&fw, S2250_FIRMWARE, &usbdev->dev)) {
+		printk(KERN_ERR
+			"s2250: unable to load firmware from file \"%s\"\n",
+			S2250_FIRMWARE);
+		goto failed2;
+	}
+	ret = usb_cypress_load_firmware(usbdev, fw, CYPRESS_FX2);
+	release_firmware(fw);
+	if (0 != ret) {
+		printk(KERN_ERR "firmware_s2250 download failed\n");
+		goto failed2;
+	}
+
+	usb_set_intfdata(interface, s);
+	return 0;
+
+failed:
+	up(&s2250_dev_table_mutex);
+failed2:
+	if (s)
+		kref_put(&(s->kref), s2250loader_delete);
+
+	printk(KERN_ERR "probe failed\n");
+	return -1;
+}
+
+static void s2250loader_disconnect(struct usb_interface *interface)
+{
+	pdevice_extension_t s = usb_get_intfdata(interface);
+	printk(KERN_INFO "s2250: disconnect\n");
+	lock_kernel();
+	s = usb_get_intfdata(interface);
+	usb_set_intfdata(interface, NULL);
+	kref_put(&(s->kref), s2250loader_delete);
+	unlock_kernel();
+}
+
+static struct usb_device_id s2250loader_ids[] = {
+	{USB_DEVICE(0x1943, 0xa250)},
+	{}                          /* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, s2250loader_ids);
+
+static struct usb_driver s2250loader_driver = {
+	.name		= "s2250-loader",
+	.probe		= s2250loader_probe,
+	.disconnect	= s2250loader_disconnect,
+	.id_table	= s2250loader_ids,
+};
+
+int s2250loader_init(void)
+{
+	int r;
+	unsigned i = 0;
+
+	for (i = 0; i < MAX_DEVICES; i++)
+		s2250_dev_table[i] = NULL;
+
+	r = usb_register(&s2250loader_driver);
+	if (r) {
+		printk(KERN_ERR "usb_register failed. Error number %d\n", r);
+		return -1;
+	}
+
+	printk(KERN_INFO "s2250loader_init: driver registered\n");
+	return 0;
+}
+EXPORT_SYMBOL(s2250loader_init);
+
+void s2250loader_cleanup(void)
+{
+	printk(KERN_INFO "s2250loader_cleanup\n");
+	usb_deregister(&s2250loader_driver);
+}
+EXPORT_SYMBOL(s2250loader_cleanup);
diff --git a/drivers/staging/go7007/saa7134-go7007.c b/drivers/staging/go7007/saa7134-go7007.c
index c4a6d8ef9078..665bbf59d026 100644
--- a/drivers/staging/go7007/saa7134-go7007.c
+++ b/drivers/staging/go7007/saa7134-go7007.c
@@ -27,7 +27,7 @@
 #include <linux/usb.h>
 #include <linux/i2c.h>
 #include <asm/byteorder.h>
-#include <media/audiochip.h>
+#include <media/v4l2-common.h>
 
 #include "saa7134-reg.h"
 #include "saa7134.h"
@@ -314,7 +314,13 @@ static int saa7134_go7007_stream_start(struct go7007 *go)
 static int saa7134_go7007_stream_stop(struct go7007 *go)
 {
 	struct saa7134_go7007 *saa = go->hpi_context;
-	struct saa7134_dev *dev = saa->dev;
+	struct saa7134_dev *dev;
+
+	if (!saa)
+		return -EINVAL;
+	dev = saa->dev;
+	if (!dev)
+		return -EINVAL;
 
 	/* Shut down TS FIFO */
 	saa_clearl(SAA7134_MAIN_CTRL, SAA7134_MAIN_CTRL_TE5);
@@ -373,6 +379,47 @@ static int saa7134_go7007_send_firmware(struct go7007 *go, u8 *data, int len)
 	return 0;
 }
 
+static int saa7134_go7007_send_command(struct go7007 *go, unsigned int cmd,
+					void *arg)
+{
+	struct saa7134_go7007 *saa = go->hpi_context;
+	struct saa7134_dev *dev = saa->dev;
+
+	switch (cmd) {
+	case VIDIOC_S_STD:
+	{
+		v4l2_std_id *std = arg;
+		return saa7134_s_std_internal(dev, NULL, std);
+	}
+	case VIDIOC_G_STD:
+	{
+		v4l2_std_id *std = arg;
+		*std = dev->tvnorm->id;
+		return 0;
+	}
+	case VIDIOC_QUERYCTRL:
+	{
+		struct v4l2_queryctrl *ctrl = arg;
+		if (V4L2_CTRL_ID2CLASS(ctrl->id) == V4L2_CTRL_CLASS_USER)
+			return saa7134_queryctrl(NULL, NULL, ctrl);
+	}
+	case VIDIOC_G_CTRL:
+	{
+		struct v4l2_control *ctrl = arg;
+		if (V4L2_CTRL_ID2CLASS(ctrl->id) == V4L2_CTRL_CLASS_USER)
+			return saa7134_g_ctrl_internal(dev, NULL, ctrl);
+	}
+	case VIDIOC_S_CTRL:
+	{
+		struct v4l2_control *ctrl = arg;
+		if (V4L2_CTRL_ID2CLASS(ctrl->id) == V4L2_CTRL_CLASS_USER)
+			return saa7134_s_ctrl_internal(dev, NULL, ctrl);
+	}
+	}
+	return -EINVAL;
+
+}
+
 static struct go7007_hpi_ops saa7134_go7007_hpi_ops = {
 	.interface_reset	= saa7134_go7007_interface_reset,
 	.write_interrupt	= saa7134_go7007_write_interrupt,
@@ -380,6 +427,7 @@ static struct go7007_hpi_ops saa7134_go7007_hpi_ops = {
 	.stream_start		= saa7134_go7007_stream_start,
 	.stream_stop		= saa7134_go7007_stream_stop,
 	.send_firmware		= saa7134_go7007_send_firmware,
+	.send_command		= saa7134_go7007_send_command,
 };
 
 /********************* Add/remove functions *********************/
diff --git a/drivers/staging/go7007/wis-i2c.h b/drivers/staging/go7007/wis-i2c.h
index 993f658ad731..431f41dd3966 100644
--- a/drivers/staging/go7007/wis-i2c.h
+++ b/drivers/staging/go7007/wis-i2c.h
@@ -23,6 +23,7 @@
 #define	I2C_DRIVERID_WIS_SAA7113	0xf0f4
 #define	I2C_DRIVERID_WIS_OV7640		0xf0f5
 #define	I2C_DRIVERID_WIS_TW2804		0xf0f6
+#define	I2C_DRIVERID_S2250		0xf0f7
 #define	I2C_ALGO_GO7007			0xf00000
 #define	I2C_ALGO_GO7007_USB		0xf10000
 
diff --git a/drivers/staging/go7007/wis-sony-tuner.c b/drivers/staging/go7007/wis-sony-tuner.c
index 5a91ee409a7c..58fddb122372 100644
--- a/drivers/staging/go7007/wis-sony-tuner.c
+++ b/drivers/staging/go7007/wis-sony-tuner.c
@@ -604,7 +604,7 @@ static int tuner_command(struct i2c_client *client, unsigned int cmd, void *arg)
 	{
 		struct v4l2_tuner *tun = arg;
 
-		memset(t, 0, sizeof(*tun));
+		memset(tun, 0, sizeof(*tun));
 		strcpy(tun->name, "Television");
 		tun->type = V4L2_TUNER_ANALOG_TV;
 		tun->rangelow = 0UL; /* does anything use these? */
diff --git a/drivers/staging/me4000/me4000.c b/drivers/staging/me4000/me4000.c
index 0394e2709278..e1c4a8078901 100644
--- a/drivers/staging/me4000/me4000.c
+++ b/drivers/staging/me4000/me4000.c
@@ -536,25 +536,19 @@ module_init(me4000_init_module);
 
 static void clear_board_info_list(void)
 {
-	struct list_head *board_p;
-	struct list_head *dac_p;
-	struct me4000_info *board_info;
-	struct me4000_ao_context *ao_context;
+	struct me4000_info *board_info, *board_info_safe;
+	struct me4000_ao_context *ao_context, *ao_context_safe;
 
 	/* Clear context lists */
-	for (board_p = me4000_board_info_list.next;
-	     board_p != &me4000_board_info_list; board_p = board_p->next) {
-		board_info = list_entry(board_p, struct me4000_info, list);
+	list_for_each_entry(board_info, &me4000_board_info_list, list) {
 		/* Clear analog output context list */
-		while (!list_empty(&board_info->ao_context_list)) {
-			dac_p = board_info->ao_context_list.next;
-			ao_context =
-			    list_entry(dac_p, struct me4000_ao_context, list);
+		list_for_each_entry_safe(ao_context, ao_context_safe,
+				&board_info->ao_context_list, list) {
 			me4000_ao_reset(ao_context);
 			free_irq(ao_context->irq, ao_context);
 			if (ao_context->circ_buf.buf)
 				kfree(ao_context->circ_buf.buf);
-			list_del(dac_p);
+			list_del(&ao_context->list);
 			kfree(ao_context);
 		}
 
@@ -574,11 +568,10 @@ static void clear_board_info_list(void)
 	}
 
 	/* Clear the board info list */
-	while (!list_empty(&me4000_board_info_list)) {
-		board_p = me4000_board_info_list.next;
-		board_info = list_entry(board_p, struct me4000_info, list);
+	list_for_each_entry_safe(board_info, board_info_safe,
+			&me4000_board_info_list, list) {
 		pci_release_regions(board_info->pci_dev_p);
-		list_del(board_p);
+		list_del(&board_info->list);
 		kfree(board_info);
 	}
 }
@@ -663,16 +656,17 @@ static int init_board_info(struct pci_dev *pci_dev_p,
 	}
 
 	/* Get the index of the board in the global list */
-	for (board_p = me4000_board_info_list.next, i = 0;
-	     board_p != &me4000_board_info_list; board_p = board_p->next, i++) {
+	i = 0;
+	list_for_each(board_p, &me4000_board_info_list) {
 		if (board_p == &board_info->list) {
 			board_info->board_count = i;
 			break;
 		}
+		i++;
 	}
 	if (board_p == &me4000_board_info_list) {
 		printk(KERN_ERR
-		       "ME4000:init_board_info():Cannot get index of baord\n");
+		       "ME4000:init_board_info():Cannot get index of board\n");
 		return -ENODEV;
 	}
 
@@ -863,16 +857,14 @@ static int alloc_ao_contexts(struct me4000_info *info)
 
 static void release_ao_contexts(struct me4000_info *board_info)
 {
-	struct list_head *dac_p;
-	struct me4000_ao_context *ao_context;
+	struct me4000_ao_context *ao_context, *ao_context_safe;
 
 	/* Clear analog output context list */
-	while (!list_empty(&board_info->ao_context_list)) {
-		dac_p = board_info->ao_context_list.next;
-		ao_context = list_entry(dac_p, struct me4000_ao_context, list);
+	list_for_each_entry_safe(ao_context, ao_context_safe,
+			&board_info->ao_context_list, list) {
 		free_irq(ao_context->irq, ao_context);
 		kfree(ao_context->circ_buf.buf);
-		list_del(dac_p);
+		list_del(&ao_context->list);
 		kfree(ao_context);
 	}
 }
@@ -1180,7 +1172,7 @@ static int me4000_xilinx_download(struct me4000_info *info)
 
 	/* Wait until /INIT pin is set */
 	udelay(20);
-	if (!inl(info->plx_regbase + PLX_INTCSR) & 0x20) {
+	if (!(inl(info->plx_regbase + PLX_INTCSR) & 0x20)) {
 		printk(KERN_ERR "%s:Can't init Xilinx\n", __func__);
 		return -EIO;
 	}
@@ -1303,12 +1295,13 @@ static int me4000_open(struct inode *inode_p, struct file *file_p)
 		       dev, mode);
 
 		/* Search for the board context */
-		for (ptr = me4000_board_info_list.next, i = 0;
-		     ptr != &me4000_board_info_list; ptr = ptr->next, i++) {
-			board_info = list_entry(ptr, struct me4000_info, list);
+		i = 0;
+		list_for_each(ptr, &me4000_board_info_list) {
 			if (i == board)
 				break;
+			i++;
 		}
+		board_info = list_entry(ptr, struct me4000_info, list);
 
 		if (ptr == &me4000_board_info_list) {
 			printk(KERN_ERR
@@ -1318,14 +1311,13 @@ static int me4000_open(struct inode *inode_p, struct file *file_p)
 		}
 
 		/* Search for the dac context */
-		for (ptr = board_info->ao_context_list.next, i = 0;
-		     ptr != &board_info->ao_context_list;
-		     ptr = ptr->next, i++) {
-			ao_context = list_entry(ptr, struct me4000_ao_context,
-									list);
+		i = 0;
+		list_for_each(ptr, &board_info->ao_context_list) {
 			if (i == dev)
 				break;
+			i++;
 		}
+		ao_context = list_entry(ptr, struct me4000_ao_context, list);
 
 		if (ptr == &board_info->ao_context_list) {
 			printk(KERN_ERR
@@ -1384,12 +1376,13 @@ static int me4000_open(struct inode *inode_p, struct file *file_p)
 		PDEBUG("me4000_open():ai board = %d mode = %d\n", board, mode);
 
 		/* Search for the board context */
-		for (ptr = me4000_board_info_list.next, i = 0;
-		     ptr != &me4000_board_info_list; ptr = ptr->next, i++) {
-			board_info = list_entry(ptr, struct me4000_info, list);
+		i = 0;
+		list_for_each(ptr, &me4000_board_info_list) {
 			if (i == board)
 				break;
+			i++;
 		}
+		board_info = list_entry(ptr, struct me4000_info, list);
 
 		if (ptr == &me4000_board_info_list) {
 			printk(KERN_ERR
@@ -1438,14 +1431,12 @@ static int me4000_open(struct inode *inode_p, struct file *file_p)
 		PDEBUG("me4000_open():board = %d\n", board);
 
 		/* Search for the board context */
-		for (ptr = me4000_board_info_list.next;
-		     ptr != &me4000_board_info_list; ptr = ptr->next) {
-			board_info = list_entry(ptr, struct me4000_info, list);
+		list_for_each_entry(board_info, &me4000_board_info_list, list) {
 			if (board_info->board_count == board)
 				break;
 		}
 
-		if (ptr == &me4000_board_info_list) {
+		if (&board_info->list == &me4000_board_info_list) {
 			printk(KERN_ERR
 			       "ME4000:me4000_open():Board %d not in device list\n",
 			       board);
@@ -1483,14 +1474,12 @@ static int me4000_open(struct inode *inode_p, struct file *file_p)
 		PDEBUG("me4000_open():board = %d\n", board);
 
 		/* Search for the board context */
-		for (ptr = me4000_board_info_list.next;
-		     ptr != &me4000_board_info_list; ptr = ptr->next) {
-			board_info = list_entry(ptr, struct me4000_info, list);
+		list_for_each_entry(board_info, &me4000_board_info_list, list) {
 			if (board_info->board_count == board)
 				break;
 		}
 
-		if (ptr == &me4000_board_info_list) {
+		if (&board_info->list == &me4000_board_info_list) {
 			printk(KERN_ERR
 			       "ME4000:me4000_open():Board %d not in device list\n",
 			       board);
@@ -1526,14 +1515,12 @@ static int me4000_open(struct inode *inode_p, struct file *file_p)
 		PDEBUG("me4000_open():board = %d\n", board);
 
 		/* Search for the board context */
-		for (ptr = me4000_board_info_list.next;
-		     ptr != &me4000_board_info_list; ptr = ptr->next) {
-			board_info = list_entry(ptr, struct me4000_info, list);
+		list_for_each_entry(board_info, &me4000_board_info_list, list) {
 			if (board_info->board_count == board)
 				break;
 		}
 
-		if (ptr == &me4000_board_info_list) {
+		if (&board_info->list == &me4000_board_info_list) {
 			printk(KERN_ERR
 			       "ME4000:me4000_open():Board %d not in device list\n",
 			       board);
@@ -5955,7 +5942,6 @@ static irqreturn_t me4000_ext_int_isr(int irq, void *dev_id)
 
 static void __exit me4000_module_exit(void)
 {
-	struct list_head *board_p;
 	struct me4000_info *board_info;
 
 	CALL_PDEBUG("cleanup_module() is executed\n");
@@ -5975,9 +5961,7 @@ static void __exit me4000_module_exit(void)
 	pci_unregister_driver(&me4000_driver);
 
 	/* Reset the boards */
-	for (board_p = me4000_board_info_list.next;
-	     board_p != &me4000_board_info_list; board_p = board_p->next) {
-		board_info = list_entry(board_p, struct me4000_info, list);
+	list_for_each_entry(board_info, &me4000_board_info_list, list) {
 		me4000_reset_board(board_info);
 	}
 
@@ -5992,7 +5976,6 @@ static int me4000_read_procmem(char *buf, char **start, off_t offset, int count,
 	int len = 0;
 	int limit = count - 1000;
 	struct me4000_info *board_info;
-	struct list_head *ptr;
 
 	len += sprintf(buf + len, "\nME4000 DRIVER VERSION %X.%X.%X\n\n",
 		       (ME4000_DRIVER_VERSION & 0xFF0000) >> 16,
@@ -6000,11 +5983,7 @@ static int me4000_read_procmem(char *buf, char **start, off_t offset, int count,
 		       (ME4000_DRIVER_VERSION & 0xFF));
 
 	/* Search for the board context */
-	for (ptr = me4000_board_info_list.next;
-	     (ptr != &me4000_board_info_list) && (len < limit);
-	     ptr = ptr->next) {
-		board_info = list_entry(ptr, struct me4000_info, list);
-
+	list_for_each_entry(board_info, &me4000_board_info_list, list) {
 		len +=
 		    sprintf(buf + len, "Board number %d:\n",
 			    board_info->board_count);
@@ -6110,6 +6089,8 @@ static int me4000_read_procmem(char *buf, char **start, off_t offset, int count,
 		    sprintf(buf + len, "AO 3 status register = 0x%08X\n",
 			    inl(board_info->me4000_regbase +
 				ME4000_AO_03_STATUS_REG));
+		if (len >= limit)
+			break;
 	}
 
 	*eof = 1;
diff --git a/drivers/staging/meilhaus/Kconfig b/drivers/staging/meilhaus/Kconfig
new file mode 100644
index 000000000000..6def83fa2c96
--- /dev/null
+++ b/drivers/staging/meilhaus/Kconfig
@@ -0,0 +1,127 @@
+#
+# Meilhaus configuration
+#
+
+menuconfig MEILHAUS
+	tristate "Meilhaus support"
+	---help---
+	  If you have a Meilhaus card, say Y (or M) here.
+
+	  You need both this driver, and the driver for the particular
+	  data collection card.
+
+	  To compile this driver as a module, choose M here. The module will
+	  be called memain.
+
+if MEILHAUS
+
+config ME0600
+	tristate "Meilhaus ME-600 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-600 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me0600.
+
+config ME0900
+	tristate "Meilhaus ME-900 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-900 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me0900.
+
+config ME1000
+	tristate "Meilhaus ME-1000 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-1000 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me1000.
+
+config ME1400
+	tristate "Meilhaus ME-1400 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-1400 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me1400.
+
+config ME1600
+	tristate "Meilhaus ME-1600 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-1600 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me1600.
+
+config ME4600
+	tristate "Meilhaus ME-4600 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-4600 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me4600.
+
+config ME6000
+	tristate "Meilhaus ME-6000 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-6000 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me6000.
+
+config ME8100
+	tristate "Meilhaus ME-8100 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-8100 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me8100.
+
+config ME8200
+	tristate "Meilhaus ME-8200 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-8200 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me8200.
+
+config MEDUMMY
+	tristate "Meilhaus dummy driver"
+	default n
+	depends on PCI
+	help
+	  This provides a dummy driver for the Meilhaus driver package
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called medummy.
+
+endif # MEILHAUS
diff --git a/drivers/staging/meilhaus/Makefile b/drivers/staging/meilhaus/Makefile
new file mode 100644
index 000000000000..5ab2c1c9c861
--- /dev/null
+++ b/drivers/staging/meilhaus/Makefile
@@ -0,0 +1,43 @@
+#
+# Makefile for Meilhaus linux driver system
+#
+
+obj-$(CONFIG_MEILHAUS) += memain.o
+obj-$(CONFIG_ME1600) += me1600.o
+obj-$(CONFIG_ME1000) += me1000.o
+obj-$(CONFIG_ME1400) += me1400.o
+obj-$(CONFIG_ME4600) += me4600.o
+obj-$(CONFIG_ME6000) += me6000.o
+obj-$(CONFIG_ME0600) += me0600.o
+obj-$(CONFIG_ME8100) += me8100.o
+obj-$(CONFIG_ME8200) += me8200.o
+obj-$(CONFIG_ME0900) += me0900.o
+obj-$(CONFIG_MEDUMMY) += medummy.o
+
+
+me1600-objs := medevice.o medlist.o medlock.o me1600_device.o
+me1600-objs += mesubdevice.o meslist.o meslock.o me1600_ao.o
+
+me1000-objs := medevice.o medlist.o medlock.o me1000_device.o
+me1000-objs += mesubdevice.o meslist.o meslock.o me1000_dio.o
+
+me1400-objs := medevice.o medlist.o medlock.o me1400_device.o
+me1400-objs += mesubdevice.o meslist.o meslock.o me8254.o me8255.o me1400_ext_irq.o
+
+me4600-objs := medevice.o medlist.o medlock.o mefirmware.o me4600_device.o
+me4600-objs += mesubdevice.o meslist.o meslock.o me4600_do.o me4600_di.o me4600_dio.o me8254.o me4600_ai.o me4600_ao.o me4600_ext_irq.o
+
+me6000-objs := medevice.o medlist.o medlock.o mefirmware.o me6000_device.o
+me6000-objs += mesubdevice.o meslist.o meslock.o me6000_dio.o me6000_ao.o
+
+me0600-objs := medevice.o medlist.o medlock.o me0600_device.o
+me0600-objs += mesubdevice.o meslist.o meslock.o me0600_relay.o me0600_ttli.o me0600_optoi.o me0600_dio.o me0600_ext_irq.o
+
+me8100-objs := medevice.o medlist.o medlock.o me8100_device.o
+me8100-objs += mesubdevice.o meslist.o meslock.o me8100_di.o me8100_do.o me8254.o
+
+me8200-objs := medevice.o medlist.o medlock.o me8200_device.o
+me8200-objs += mesubdevice.o meslist.o meslock.o me8200_di.o me8200_do.o me8200_dio.o
+
+me0900-objs := medevice.o medlist.o medlock.o me0900_device.o
+me0900-objs += mesubdevice.o meslist.o meslock.o me0900_do.o me0900_di.o
diff --git a/drivers/staging/meilhaus/TODO b/drivers/staging/meilhaus/TODO
new file mode 100644
index 000000000000..6ec25203089c
--- /dev/null
+++ b/drivers/staging/meilhaus/TODO
@@ -0,0 +1,10 @@
+TODO:
+	- checkpatch.pl cleanups
+	- sparse issues
+	- Lindent
+	- audit userspace interface
+	- handle firmware properly
+	- possible comedi merge
+
+Please send cleanup patches to Greg Kroah-Hartman <greg@kroah.com>
+and CC: David Kiliani <mail@davidkiliani.de>
diff --git a/drivers/staging/meilhaus/me0600_device.c b/drivers/staging/meilhaus/me0600_device.c
new file mode 100644
index 000000000000..8950e47e0e86
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_device.c
@@ -0,0 +1,215 @@
+/**
+ * @file me0600_device.c
+ *
+ * @brief ME-630 device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "medevice.h"
+#include "me0600_device.h"
+#include "mesubdevice.h"
+#include "me0600_relay.h"
+#include "me0600_ttli.h"
+#include "me0600_optoi.h"
+#include "me0600_dio.h"
+#include "me0600_ext_irq.h"
+
+me_device_t *me0600_pci_constructor(struct pci_dev *pci_device)
+{
+	me0600_device_t *me0600_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	int err;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me0600_device = kmalloc(sizeof(me0600_device_t), GFP_KERNEL);
+
+	if (!me0600_device) {
+		PERROR("Cannot get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(me0600_device, 0, sizeof(me0600_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me0600_device, pci_device);
+
+	if (err) {
+		kfree(me0600_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+
+	/* Get the index in the device version information table. */
+	version_idx =
+	    me0600_versions_get_device_index(me0600_device->base.info.pci.
+					     device_id);
+
+	// Initialize spin lock .
+	spin_lock_init(&me0600_device->dio_ctrl_reg_lock);
+	spin_lock_init(&me0600_device->intcsr_lock);
+
+	// Create subdevice instances.
+
+	for (i = 0; i < me0600_versions[version_idx].optoi_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me0600_optoi_constructor(me0600_device->
+								base.info.pci.
+								reg_bases[2]);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me0600_device);
+			kfree(me0600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me0600_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me0600_versions[version_idx].relay_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me0600_relay_constructor(me0600_device->
+								base.info.pci.
+								reg_bases[2]);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me0600_device);
+			kfree(me0600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me0600_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me0600_versions[version_idx].ttli_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me0600_ttli_constructor(me0600_device->
+							       base.info.pci.
+							       reg_bases[2]);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me0600_device);
+			kfree(me0600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me0600_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me0600_versions[version_idx].dio_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me0600_dio_constructor(me0600_device->
+							      base.info.pci.
+							      reg_bases[2], i,
+							      &me0600_device->
+							      dio_ctrl_reg_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me0600_device);
+			kfree(me0600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me0600_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me0600_versions[version_idx].ext_irq_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *)
+		    me0600_ext_irq_constructor(me0600_device->base.info.pci.
+					       reg_bases[1],
+					       me0600_device->base.info.pci.
+					       reg_bases[2],
+					       &me0600_device->intcsr_lock, i,
+					       me0600_device->base.irq);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me0600_device);
+			kfree(me0600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me0600_device->base.slist,
+					    subdevice);
+	}
+
+	return (me_device_t *) me0600_device;
+}
+
+// Init and exit of module.
+
+static int __init me0600_init(void)
+{
+	PDEBUG("executed.\n");
+	return 0;
+}
+
+static void __exit me0600_exit(void)
+{
+	PDEBUG("executed.\n");
+}
+
+module_init(me0600_init);
+
+module_exit(me0600_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for ME-6xx Device");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-6xx Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me0600_pci_constructor);
diff --git a/drivers/staging/meilhaus/me0600_device.h b/drivers/staging/meilhaus/me0600_device.h
new file mode 100644
index 000000000000..d93a8aee581b
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_device.h
@@ -0,0 +1,97 @@
+/**
+ * @file me0600_device.h
+ *
+ * @brief ME-630 device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_DEVICE_H
+#define _ME0600_DEVICE_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure holding ME-630 device capabilities.
+ */
+typedef struct me0600_version {
+	uint16_t device_id;
+	unsigned int relay_subdevices;
+	unsigned int ttli_subdevices;
+	unsigned int optoi_subdevices;
+	unsigned int dio_subdevices;
+	unsigned int ext_irq_subdevices;
+} me0600_version_t;
+
+/**
+ * @brief Device capabilities.
+ */
+static me0600_version_t me0600_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME0630, 1, 1, 1, 2, 2},
+	{0},
+};
+
+#define ME0600_DEVICE_VERSIONS (sizeof(me0600_versions) / sizeof(me0600_version_t) - 1)	/**< Returns the number of entries in #me0600_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #me0600_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me0600_versions.
+ */
+static inline unsigned int me0600_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME0600_DEVICE_VERSIONS; i++)
+		if (me0600_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The ME-630 device class structure.
+ */
+typedef struct me0600_device {
+	me_device_t base;			/**< The Meilhaus device base class. */
+
+	/* Child class attributes. */
+	spinlock_t dio_ctrl_reg_lock;
+	spinlock_t intcsr_lock;
+} me0600_device_t;
+
+/**
+ * @brief The ME-630 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-630 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me0600_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_dio.c b/drivers/staging/meilhaus/me0600_dio.c
new file mode 100644
index 000000000000..3a2775749a2c
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_dio.c
@@ -0,0 +1,415 @@
+/**
+ * @file me0600_dio.c
+ *
+ * @brief ME-630 digital input/output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me0600_dio_reg.h"
+#include "me0600_dio.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me0600_dio_io_reset_subdevice(struct me_subdevice *subdevice,
+					 struct file *filep, int flags)
+{
+	me0600_dio_subdevice_t *instance;
+	uint8_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_dio_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inb(instance->ctrl_reg);
+	mode &= ~(0x3 << (instance->dio_idx * 2));
+	outb(mode, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, mode);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outb(0x00, instance->port_reg);
+	PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0x00);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_dio_io_single_config(me_subdevice_t * subdevice,
+				       struct file *filep,
+				       int channel,
+				       int single_config,
+				       int ref,
+				       int trig_chan,
+				       int trig_type, int trig_edge, int flags)
+{
+	me0600_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+	int size =
+	    flags & (ME_IO_SINGLE_CONFIG_DIO_BIT | ME_IO_SINGLE_CONFIG_DIO_BYTE
+		     | ME_IO_SINGLE_CONFIG_DIO_WORD |
+		     ME_IO_SINGLE_CONFIG_DIO_DWORD);
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inb(instance->ctrl_reg);
+	switch (size) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+				mode &=
+				    ~((ME0600_DIO_CONFIG_BIT_OUT_0) <<
+				      (instance->dio_idx * 2));
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+				mode &=
+				    ~((ME0600_DIO_CONFIG_BIT_OUT_0) <<
+				      (instance->dio_idx * 2));
+				mode |=
+				    ME0600_DIO_CONFIG_BIT_OUT_0 << (instance->
+								    dio_idx *
+								    2);
+			} else {
+				PERROR
+				    ("Invalid port configuration specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!err) {
+		outb(mode, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, mode);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_dio_io_single_read(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int *value, int time_out, int flags)
+{
+	me0600_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME0600_DIO_CONFIG_BIT_OUT_0) <<
+					     (instance->dio_idx * 2));
+
+			if ((mode ==
+			     (ME0600_DIO_CONFIG_BIT_OUT_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value =
+				    inb(instance->
+					port_reg) & (0x0001 << channel);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME0600_DIO_CONFIG_BIT_OUT_0) <<
+					     (instance->dio_idx * 2));
+
+			if ((mode ==
+			     (ME0600_DIO_CONFIG_BIT_OUT_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value = inb(instance->port_reg) & 0x00FF;
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+
+		break;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_dio_io_single_write(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int value, int time_out, int flags)
+{
+	me0600_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+	uint8_t byte;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME0600_DIO_CONFIG_BIT_OUT_0) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME0600_DIO_CONFIG_BIT_OUT_0 <<
+			     (instance->dio_idx * 2))) {
+				byte = inb(instance->port_reg);
+
+				if (value)
+					byte |= 0x1 << channel;
+				else
+					byte &= ~(0x1 << channel);
+
+				outb(byte, instance->port_reg);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME0600_DIO_CONFIG_BIT_OUT_0) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME0600_DIO_CONFIG_BIT_OUT_0 <<
+			     (instance->dio_idx * 2))) {
+				outb(value, instance->port_reg);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+
+		break;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_dio_query_number_channels(me_subdevice_t * subdevice,
+					    int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_dio_query_subdevice_type(me_subdevice_t * subdevice,
+					   int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DIO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_dio_query_subdevice_caps(me_subdevice_t * subdevice,
+					   int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_DIR_BYTE;
+	return ME_ERRNO_SUCCESS;
+}
+
+me0600_dio_subdevice_t *me0600_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock)
+{
+	me0600_dio_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me0600_dio_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me0600_dio_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save digital i/o index */
+	subdevice->dio_idx = dio_idx;
+
+	/* Save the subdevice index */
+	subdevice->ctrl_reg = reg_base + ME0600_DIO_CONFIG_REG;
+	subdevice->port_reg = reg_base + ME0600_DIO_PORT_REG + dio_idx;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me0600_dio_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me0600_dio_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me0600_dio_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me0600_dio_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me0600_dio_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me0600_dio_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me0600_dio_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me0600_dio.h b/drivers/staging/meilhaus/me0600_dio.h
new file mode 100644
index 000000000000..5d075c7d6882
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_dio.h
@@ -0,0 +1,68 @@
+/**
+ * @file me0600_dio.h
+ *
+ * @brief ME-630 digital input/output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_DIO_H_
+#define _ME0600_DIO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me0600_dio_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+	unsigned int dio_idx;			/**< The index of the digital i/o on the device. */
+
+	unsigned long port_reg;			/**< Register holding the port status. */
+	unsigned long ctrl_reg;			/**< Register to configure the port direction. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me0600_dio_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-630 digital input/ouput subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param dio_idx The index of the digital i/o port on the device.
+ * @param ctrl_reg_lock Spin lock protecting the control register.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me0600_dio_subdevice_t *me0600_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_dio_reg.h b/drivers/staging/meilhaus/me0600_dio_reg.h
new file mode 100644
index 000000000000..f116ea3b79d2
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_dio_reg.h
@@ -0,0 +1,41 @@
+/**
+ * @file me0600_dio_reg.h
+ *
+ * @brief ME-630 digital input/output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_DIO_REG_H_
+#define _ME0600_DIO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME0600_DIO_CONFIG_REG		0x0007
+#define ME0600_DIO_PORT_0_REG		0x0008
+#define ME0600_DIO_PORT_1_REG		0x0009
+#define ME0600_DIO_PORT_REG			ME0600_DIO_PORT_0_REG
+
+#define ME0600_DIO_CONFIG_BIT_OUT_0	0x0001
+#define ME0600_DIO_CONFIG_BIT_OUT_1	0x0004
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_ext_irq.c b/drivers/staging/meilhaus/me0600_ext_irq.c
new file mode 100644
index 000000000000..eba18adecb72
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_ext_irq.c
@@ -0,0 +1,478 @@
+/**
+ * @file me0600_ext_irq.c
+ *
+ * @brief ME-630 external interrupt subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/version.h>
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+#include "meids.h"
+#include "medebug.h"
+
+#include "meplx_reg.h"
+#include "me0600_ext_irq_reg.h"
+#include "me0600_ext_irq.h"
+
+/*
+ * Functions
+ */
+
+static int me0600_ext_irq_io_irq_start(struct me_subdevice *subdevice,
+				       struct file *filep,
+				       int channel,
+				       int irq_source,
+				       int irq_edge, int irq_arg, int flags)
+{
+	me0600_ext_irq_subdevice_t *instance;
+	uint32_t tmp;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags & ~ME_IO_IRQ_START_DIO_BIT) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (instance->lintno > 1) {
+		PERROR("Wrong idx=%d.\n", instance->lintno);
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (irq_source != ME_IRQ_SOURCE_DIO_LINE) {
+		PERROR("Invalid irq source specified.\n");
+		return ME_ERRNO_INVALID_IRQ_SOURCE;
+	}
+
+	if (irq_edge != ME_IRQ_EDGE_RISING) {
+		PERROR("Invalid irq edge specified.\n");
+		return ME_ERRNO_INVALID_IRQ_EDGE;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->intcsr_lock);
+	tmp = inl(instance->intcsr);
+	switch (instance->lintno) {
+	case 0:
+		tmp |=
+		    PLX_INTCSR_LOCAL_INT1_EN | PLX_INTCSR_LOCAL_INT1_POL |
+		    PLX_INTCSR_PCI_INT_EN;
+		break;
+	case 1:
+		tmp |=
+		    PLX_INTCSR_LOCAL_INT2_EN | PLX_INTCSR_LOCAL_INT2_POL |
+		    PLX_INTCSR_PCI_INT_EN;
+		break;
+	}
+	outl(tmp, instance->intcsr);
+	PDEBUG_REG("intcsr outl(plx:0x%X)=0x%x\n", instance->intcsr, tmp);
+	spin_unlock(instance->intcsr_lock);
+	instance->rised = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_ext_irq_io_irq_wait(struct me_subdevice *subdevice,
+				      struct file *filep,
+				      int channel,
+				      int *irq_count,
+				      int *value, int time_out, int flags)
+{
+	me0600_ext_irq_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->rised <= 0) {
+		instance->rised = 0;
+
+		if (time_out) {
+			t = wait_event_interruptible_timeout(instance->
+							     wait_queue,
+							     (instance->rised !=
+							      0), t);
+
+			if (t == 0) {
+				PERROR("Wait on interrupt timed out.\n");
+				err = ME_ERRNO_TIMEOUT;
+			}
+		} else {
+			wait_event_interruptible(instance->wait_queue,
+						 (instance->rised != 0));
+		}
+
+		if (instance->rised < 0) {
+			PERROR("Wait on interrupt aborted by user.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+	}
+
+	if (signal_pending(current)) {
+		PERROR("Wait on interrupt aborted by signal.\n");
+		err = ME_ERRNO_SIGNAL;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	instance->rised = 0;
+	*irq_count = instance->n;
+	*value = 1;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_ext_irq_io_irq_stop(struct me_subdevice *subdevice,
+				      struct file *filep,
+				      int channel, int flags)
+{
+	me0600_ext_irq_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t tmp;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (instance->lintno > 1) {
+		PERROR("Wrong idx=%d.\n", instance->lintno);
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->intcsr_lock);
+	tmp = inl(instance->intcsr);
+	switch (instance->lintno) {
+	case 0:
+		tmp &= ~PLX_INTCSR_LOCAL_INT1_EN;
+		break;
+	case 1:
+		tmp &= ~PLX_INTCSR_LOCAL_INT2_EN;
+		break;
+	}
+	outl(tmp, instance->intcsr);
+	PDEBUG_REG("intcsr outl(plx:0x%X)=0x%x\n", instance->intcsr, tmp);
+	spin_unlock(instance->intcsr_lock);
+	instance->rised = -1;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_ext_irq_io_reset_subdevice(struct me_subdevice *subdevice,
+					     struct file *filep, int flags)
+{
+	me0600_ext_irq_subdevice_t *instance;
+	uint32_t tmp;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->intcsr_lock);
+	tmp = inl(instance->intcsr);
+	switch (instance->lintno) {
+	case 0:
+		tmp |= PLX_INTCSR_LOCAL_INT1_POL | PLX_INTCSR_PCI_INT_EN;
+		tmp &= ~PLX_INTCSR_LOCAL_INT1_EN;
+		break;
+	case 1:
+		tmp |= PLX_INTCSR_LOCAL_INT2_POL | PLX_INTCSR_PCI_INT_EN;
+		tmp &= ~PLX_INTCSR_LOCAL_INT2_EN;
+		break;
+	}
+	outl(tmp, instance->intcsr);
+	PDEBUG_REG("intcsr outl(plx:0x%X)=0x%x\n", instance->intcsr, tmp);
+	spin_unlock(instance->intcsr_lock);
+
+	instance->rised = -1;
+	instance->n = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_ext_irq_query_number_channels(struct me_subdevice *subdevice,
+						int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 1;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_ext_irq_query_subdevice_type(struct me_subdevice *subdevice,
+					       int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_EXT_IRQ;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_ext_irq_query_subdevice_caps(struct me_subdevice *subdevice,
+					       int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_EXT_IRQ_EDGE_RISING;
+	return ME_ERRNO_SUCCESS;
+}
+
+static void me0600_ext_irq_destructor(struct me_subdevice *subdevice)
+{
+	me0600_ext_irq_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_ext_irq_subdevice_t *) subdevice;
+
+	free_irq(instance->irq, (void *)instance);
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me0600_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me0600_isr(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{
+	me0600_ext_irq_subdevice_t *instance;
+	uint32_t status;
+	uint32_t mask = PLX_INTCSR_PCI_INT_EN;
+	irqreturn_t ret = IRQ_HANDLED;
+
+	instance = (me0600_ext_irq_subdevice_t *) dev_id;
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	PDEBUG("executed.\n");
+
+	if (instance->lintno > 1) {
+		PERROR_CRITICAL
+		    ("%s():Wrong subdevice index=%d plx:irq_status_reg=0x%04X.\n",
+		     __func__, instance->lintno, inl(instance->intcsr));
+		return IRQ_NONE;
+	}
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->intcsr_lock);
+	status = inl(instance->intcsr);
+	switch (instance->lintno) {
+	case 0:
+		mask |= PLX_INTCSR_LOCAL_INT1_STATE | PLX_INTCSR_LOCAL_INT1_EN;
+		break;
+	case 1:
+		mask |= PLX_INTCSR_LOCAL_INT2_STATE | PLX_INTCSR_LOCAL_INT2_EN;
+		break;
+	}
+
+	if ((status & mask) == mask) {
+		instance->rised = 1;
+		instance->n++;
+		inb(instance->reset_reg);
+		PDEBUG("Interrupt detected.\n");
+	} else {
+		PINFO
+		    ("%ld Shared interrupt. %s(): idx=0 plx:irq_status_reg=0x%04X\n",
+		     jiffies, __func__, status);
+		ret = IRQ_NONE;
+	}
+	spin_unlock(instance->intcsr_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return ret;
+}
+
+me0600_ext_irq_subdevice_t *me0600_ext_irq_constructor(uint32_t plx_reg_base,
+						       uint32_t me0600_reg_base,
+						       spinlock_t * intcsr_lock,
+						       unsigned ext_irq_idx,
+						       int irq)
+{
+	me0600_ext_irq_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me0600_ext_irq_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for 630_ext_irq instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me0600_ext_irq_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->intcsr_lock = intcsr_lock;
+
+	/* Initialize wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	subdevice->lintno = ext_irq_idx;
+
+	/* Request interrupt line */
+	subdevice->irq = irq;
+
+	err = request_irq(subdevice->irq, me0600_isr,
+#ifdef IRQF_DISABLED
+			  IRQF_DISABLED | IRQF_SHARED,
+#else
+			  SA_INTERRUPT | SA_SHIRQ,
+#endif
+			  ME0600_NAME, (void *)subdevice);
+
+	if (err) {
+		PERROR("Cannot get interrupt line.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	PINFO("Registered irq=%d.\n", subdevice->irq);
+
+	/* Initialize registers */
+	subdevice->intcsr = plx_reg_base + PLX_INTCSR;
+	subdevice->reset_reg =
+	    me0600_reg_base + ME0600_INT_0_RESET_REG + ext_irq_idx;
+
+	/* Initialize the subdevice methods */
+	subdevice->base.me_subdevice_io_irq_start = me0600_ext_irq_io_irq_start;
+	subdevice->base.me_subdevice_io_irq_wait = me0600_ext_irq_io_irq_wait;
+	subdevice->base.me_subdevice_io_irq_stop = me0600_ext_irq_io_irq_stop;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me0600_ext_irq_io_reset_subdevice;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me0600_ext_irq_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me0600_ext_irq_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me0600_ext_irq_query_subdevice_caps;
+	subdevice->base.me_subdevice_destructor = me0600_ext_irq_destructor;
+
+	subdevice->rised = 0;
+	subdevice->n = 0;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me0600_ext_irq.h b/drivers/staging/meilhaus/me0600_ext_irq.h
new file mode 100644
index 000000000000..f5f2204b49a0
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_ext_irq.h
@@ -0,0 +1,58 @@
+/**
+ * @file me0600_ext_irq.h
+ *
+ * @brief ME-630 external interrupt implementation.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME0600_EXT_IRQ_H_
+#define _ME0600_EXT_IRQ_H_
+
+#include <linux/sched.h>
+
+#include "mesubdevice.h"
+#include "meslock.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The ME-630 external interrupt subdevice class.
+ */
+typedef struct me0600_ext_irq_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *intcsr_lock;		/**< Spin lock to protect #intcsr. */
+
+	wait_queue_head_t wait_queue;		/**< Queue to put on threads waiting for an interrupt. */
+
+	int irq;				/**< The irq number assigned by PCI BIOS. */
+	int rised;				/**< If true an interrupt has occured. */
+	unsigned int n;				/**< The number of interrupt since the driver was loaded. */
+	unsigned int lintno;			/**< The number of the local PCI interrupt. */
+
+	uint32_t intcsr;			/**< The PLX interrupt control and status register. */
+	uint32_t reset_reg;			/**< The control register. */
+} me0600_ext_irq_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-630 external interrupt instance.
+ *
+ * @param plx_reg_base The register base address of the PLX chip as returned by the PCI BIOS.
+ * @param me0600_reg_base The register base address of the ME-630 device as returned by the PCI BIOS.
+ * @param irq The irq assigned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me0600_ext_irq_subdevice_t *me0600_ext_irq_constructor(uint32_t plx_reg_base,
+						       uint32_t me0600_reg_base,
+						       spinlock_t * intcsr_lock,
+						       unsigned int ext_irq_idx,
+						       int irq);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_ext_irq_reg.h b/drivers/staging/meilhaus/me0600_ext_irq_reg.h
new file mode 100644
index 000000000000..f6198fa6d2b2
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_ext_irq_reg.h
@@ -0,0 +1,18 @@
+/**
+ * @file me0600_ext_irq_reg.h
+ *
+ * @brief ME-630 external interrupt register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME0600_EXT_IRQ_REG_H_
+#define _ME0600_EXT_IRQ_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME0600_INT_0_RESET_REG     0x0005
+#define ME0600_INT_1_RESET_REG     0x0006
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_optoi.c b/drivers/staging/meilhaus/me0600_optoi.c
new file mode 100644
index 000000000000..b6d977f228ca
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_optoi.c
@@ -0,0 +1,243 @@
+/**
+ * @file me0600_optoi.c
+ *
+ * @brief ME-630 Optoisolated input subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me0600_optoi_reg.h"
+#include "me0600_optoi.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me0600_optoi_io_reset_subdevice(struct me_subdevice *subdevice,
+					   struct file *filep, int flags)
+{
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	PDEBUG("executed.\n");
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_optoi_io_single_config(me_subdevice_t * subdevice,
+					 struct file *filep,
+					 int channel,
+					 int single_config,
+					 int ref,
+					 int trig_chan,
+					 int trig_type,
+					 int trig_edge, int flags)
+{
+	me0600_optoi_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_optoi_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config != ME_SINGLE_CONFIG_DIO_INPUT) {
+				PERROR("Invalid port direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+
+		break;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_optoi_io_single_read(me_subdevice_t * subdevice,
+				       struct file *filep,
+				       int channel,
+				       int *value, int time_out, int flags)
+{
+	me0600_optoi_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_optoi_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = inb(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inb(instance->port_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_optoi_query_number_channels(me_subdevice_t * subdevice,
+					      int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_optoi_query_subdevice_type(me_subdevice_t * subdevice,
+					     int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DI;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_optoi_query_subdevice_caps(me_subdevice_t * subdevice,
+					     int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+me0600_optoi_subdevice_t *me0600_optoi_constructor(uint32_t reg_base)
+{
+	me0600_optoi_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me0600_optoi_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me0600_optoi_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	/* Save the subdevice index */
+	subdevice->port_reg = reg_base + ME0600_OPTO_INPUT_REG;
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me0600_optoi_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me0600_optoi_io_single_config;
+	subdevice->base.me_subdevice_io_single_read =
+	    me0600_optoi_io_single_read;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me0600_optoi_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me0600_optoi_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me0600_optoi_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me0600_optoi.h b/drivers/staging/meilhaus/me0600_optoi.h
new file mode 100644
index 000000000000..e7e69bcde9c9
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_optoi.h
@@ -0,0 +1,58 @@
+/**
+ * @file me0600_optoi.h
+ *
+ * @brief ME-630 Optoisolated input subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_OPTOI_H_
+#define _ME0600_OPTOI_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me0600_optoi_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+
+	uint32_t port_reg;			/**< Register holding the port status. */
+} me0600_optoi_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-630 Optoisolated input subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me0600_optoi_subdevice_t *me0600_optoi_constructor(uint32_t reg_base);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_optoi_reg.h b/drivers/staging/meilhaus/me0600_optoi_reg.h
new file mode 100644
index 000000000000..e0bc45054000
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_optoi_reg.h
@@ -0,0 +1,35 @@
+/**
+ * @file me0600_optoi_reg.h
+ *
+ * @brief ME-630 Optoisolated input subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_OPTOI_REG_H_
+#define _ME0600_OPTOI_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME0600_OPTO_INPUT_REG      0x0004
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_relay.c b/drivers/staging/meilhaus/me0600_relay.c
new file mode 100644
index 000000000000..2665c69addd2
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_relay.c
@@ -0,0 +1,359 @@
+/**
+ * @file me0600_relay.c
+ *
+ * @brief ME-630 relay subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me0600_relay_reg.h"
+#include "me0600_relay.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me0600_relay_io_reset_subdevice(struct me_subdevice *subdevice,
+					   struct file *filep, int flags)
+{
+	me0600_relay_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_relay_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	outb(0x0, instance->port_0_reg);
+	PDEBUG_REG("port_0_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_0_reg - instance->reg_base, 0);
+	outb(0x0, instance->port_1_reg);
+	PDEBUG_REG("port_1_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_1_reg - instance->reg_base, 0);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_relay_io_single_config(me_subdevice_t * subdevice,
+					 struct file *filep,
+					 int channel,
+					 int single_config,
+					 int ref,
+					 int trig_chan,
+					 int trig_type,
+					 int trig_edge, int flags)
+{
+	me0600_relay_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_relay_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_WORD:
+		if (channel == 0) {
+			if (single_config != ME_SINGLE_CONFIG_DIO_OUTPUT) {
+				PERROR("Invalid word direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+
+		break;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_relay_io_single_read(me_subdevice_t * subdevice,
+				       struct file *filep,
+				       int channel,
+				       int *value, int time_out, int flags)
+{
+	me0600_relay_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_relay_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = inb(instance->port_0_reg) & (0x1 << channel);
+		} else if ((channel >= 8) && (channel < 16)) {
+			*value =
+			    inb(instance->port_1_reg) & (0x1 << (channel - 8));
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inb(instance->port_0_reg);
+		} else if (channel == 1) {
+			*value = inb(instance->port_1_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_WORD:
+		if (channel == 0) {
+			*value = (uint32_t) inb(instance->port_1_reg) << 8;
+			*value |= inb(instance->port_0_reg);
+		} else {
+			PERROR("Invalid word number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_relay_io_single_write(me_subdevice_t * subdevice,
+					struct file *filep,
+					int channel,
+					int value, int time_out, int flags)
+{
+	me0600_relay_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t state;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_relay_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			state = inb(instance->port_0_reg);
+			state =
+			    value ? (state | (0x1 << channel)) : (state &
+								  ~(0x1 <<
+								    channel));
+			outb(state, instance->port_0_reg);
+		} else if ((channel >= 8) && (channel < 16)) {
+			state = inb(instance->port_1_reg);
+			state =
+			    value ? (state | (0x1 << (channel - 8))) : (state &
+									~(0x1 <<
+									  (channel
+									   -
+									   8)));
+			outb(state, instance->port_1_reg);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			outb(value, instance->port_0_reg);
+		} else if (channel == 1) {
+			outb(value, instance->port_1_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_WORD:
+		if (channel == 0) {
+			outb(value, instance->port_0_reg);
+			outb(value >> 8, instance->port_1_reg);
+		} else {
+			PERROR("Invalid word number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+		break;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_relay_query_number_channels(me_subdevice_t * subdevice,
+					      int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 16;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_relay_query_subdevice_type(me_subdevice_t * subdevice,
+					     int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_relay_query_subdevice_caps(me_subdevice_t * subdevice,
+					     int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+me0600_relay_subdevice_t *me0600_relay_constructor(uint32_t reg_base)
+{
+	me0600_relay_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me0600_relay_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me0600_relay_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	/* Save the subdevice index */
+	subdevice->port_0_reg = reg_base + ME0600_RELAIS_0_REG;
+	subdevice->port_1_reg = reg_base + ME0600_RELAIS_1_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me0600_relay_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me0600_relay_io_single_config;
+	subdevice->base.me_subdevice_io_single_read =
+	    me0600_relay_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me0600_relay_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me0600_relay_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me0600_relay_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me0600_relay_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me0600_relay.h b/drivers/staging/meilhaus/me0600_relay.h
new file mode 100644
index 000000000000..2ce7dcab8b39
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_relay.h
@@ -0,0 +1,63 @@
+/**
+ * @file me0600_relay.h
+ *
+ * @brief ME-630 relay subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_RELAY_H_
+#define _ME0600_RELAY_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me0600_relay_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+
+	unsigned long port_0_reg;			/**< Register holding the port status. */
+	unsigned long port_1_reg;			/**< Register holding the port status. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me0600_relay_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-630 relay subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param ctrl_reg_lock Spin lock protecting the control register.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me0600_relay_subdevice_t *me0600_relay_constructor(uint32_t reg_base);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_relay_reg.h b/drivers/staging/meilhaus/me0600_relay_reg.h
new file mode 100644
index 000000000000..ba4db2e223c5
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_relay_reg.h
@@ -0,0 +1,36 @@
+/**
+ * @file me0600_relay_reg.h
+ *
+ * @brief ME-630 relay subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_RELAY_REG_H_
+#define _ME0600_RELAY_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME0600_RELAIS_0_REG        0x0001
+#define ME0600_RELAIS_1_REG        0x0002
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_ttli.c b/drivers/staging/meilhaus/me0600_ttli.c
new file mode 100644
index 000000000000..ab8e13b6f329
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_ttli.c
@@ -0,0 +1,238 @@
+/**
+ * @file me0600_ttli.c
+ *
+ * @brief ME-630 TTL input subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me0600_ttli_reg.h"
+#include "me0600_ttli.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me0600_ttli_io_reset_subdevice(struct me_subdevice *subdevice,
+					  struct file *filep, int flags)
+{
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	PDEBUG("executed.\n");
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_ttli_io_single_config(me_subdevice_t * subdevice,
+					struct file *filep,
+					int channel,
+					int single_config,
+					int ref,
+					int trig_chan,
+					int trig_type, int trig_edge, int flags)
+{
+	me0600_ttli_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_ttli_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config != ME_SINGLE_CONFIG_DIO_INPUT) {
+				PERROR("Invalid port direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+
+		break;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_ttli_io_single_read(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int *value, int time_out, int flags)
+{
+	me0600_ttli_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_ttli_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = inb(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inb(instance->port_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_ttli_query_number_channels(me_subdevice_t * subdevice,
+					     int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_ttli_query_subdevice_type(me_subdevice_t * subdevice,
+					    int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DI;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_ttli_query_subdevice_caps(me_subdevice_t * subdevice,
+					    int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+me0600_ttli_subdevice_t *me0600_ttli_constructor(uint32_t reg_base)
+{
+	me0600_ttli_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me0600_ttli_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me0600_ttli_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	/* Save the subdevice index */
+	subdevice->port_reg = reg_base + ME0600_TTL_INPUT_REG;
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me0600_ttli_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me0600_ttli_io_single_config;
+	subdevice->base.me_subdevice_io_single_read =
+	    me0600_ttli_io_single_read;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me0600_ttli_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me0600_ttli_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me0600_ttli_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me0600_ttli.h b/drivers/staging/meilhaus/me0600_ttli.h
new file mode 100644
index 000000000000..6c9039614867
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_ttli.h
@@ -0,0 +1,58 @@
+/**
+ * @file me0600_ttli.h
+ *
+ * @brief ME-630 TTL input subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_TTLI_H_
+#define _ME0600_TTLI_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me0600_ttli_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+
+	uint32_t port_reg;			/**< Register holding the port status. */
+} me0600_ttli_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-630 TTL input subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me0600_ttli_subdevice_t *me0600_ttli_constructor(uint32_t reg_base);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_ttli_reg.h b/drivers/staging/meilhaus/me0600_ttli_reg.h
new file mode 100644
index 000000000000..4f986d160934
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_ttli_reg.h
@@ -0,0 +1,35 @@
+/**
+ * @file me0600_ttli_reg.h
+ *
+ * @brief ME-630 TTL input subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_TTLI_REG_H_
+#define _ME0600_TTLI_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME0600_TTL_INPUT_REG       0x0003
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0900_device.c b/drivers/staging/meilhaus/me0900_device.c
new file mode 100644
index 000000000000..764d5d307c44
--- /dev/null
+++ b/drivers/staging/meilhaus/me0900_device.c
@@ -0,0 +1,180 @@
+/**
+ * @file me0900_device.c
+ *
+ * @brief ME-9x device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+*/
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "medevice.h"
+#include "me0900_device.h"
+#include "me0900_reg.h"
+#include "mesubdevice.h"
+#include "me0900_do.h"
+#include "me0900_di.h"
+
+me_device_t *me0900_pci_constructor(struct pci_dev *pci_device)
+{
+	me0900_device_t *me0900_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	int err;
+	int i;
+	int port_shift;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me0900_device = kmalloc(sizeof(me0900_device_t), GFP_KERNEL);
+
+	if (!me0900_device) {
+		PERROR("Cannot get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(me0900_device, 0, sizeof(me0900_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me0900_device, pci_device);
+
+	if (err) {
+		kfree(me0900_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+
+	/* Get the index in the device version information table. */
+	version_idx =
+	    me0900_versions_get_device_index(me0900_device->base.info.pci.
+					     device_id);
+
+	/* Initialize 8255 chip to desired mode */
+	if (me0900_device->base.info.pci.device_id ==
+	    PCI_DEVICE_ID_MEILHAUS_ME0940) {
+		outb(0x9B,
+		     me0900_device->base.info.pci.reg_bases[2] +
+		     ME0900_CTRL_REG);
+	} else if (me0900_device->base.info.pci.device_id ==
+		   PCI_DEVICE_ID_MEILHAUS_ME0950) {
+		outb(0x89,
+		     me0900_device->base.info.pci.reg_bases[2] +
+		     ME0900_CTRL_REG);
+		outb(0x00,
+		     me0900_device->base.info.pci.reg_bases[2] +
+		     ME0900_WRITE_ENABLE_REG);
+	} else if (me0900_device->base.info.pci.device_id ==
+		   PCI_DEVICE_ID_MEILHAUS_ME0960) {
+		outb(0x8B,
+		     me0900_device->base.info.pci.reg_bases[2] +
+		     ME0900_CTRL_REG);
+		outb(0x00,
+		     me0900_device->base.info.pci.reg_bases[2] +
+		     ME0900_WRITE_ENABLE_REG);
+	}
+
+	port_shift =
+	    (me0900_device->base.info.pci.device_id ==
+	     PCI_DEVICE_ID_MEILHAUS_ME0960) ? 1 : 0;
+	// Create subdevice instances.
+
+	for (i = 0; i < me0900_versions[version_idx].di_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me0900_di_constructor(me0900_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     i + port_shift);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me0900_device);
+			kfree(me0900_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me0900_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me0900_versions[version_idx].do_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me0900_do_constructor(me0900_device->
+							     base.info.pci.
+							     reg_bases[2], i);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me0900_device);
+			kfree(me0900_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me0900_device->base.slist,
+					    subdevice);
+	}
+
+	return (me_device_t *) me0900_device;
+}
+
+// Init and exit of module.
+
+static int __init me0900_init(void)
+{
+	PDEBUG("executed.\n.");
+	return 0;
+}
+
+static void __exit me0900_exit(void)
+{
+	PDEBUG("executed.\n.");
+}
+
+module_init(me0900_init);
+module_exit(me0900_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for ME-9x Device");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-9x Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me0900_pci_constructor);
diff --git a/drivers/staging/meilhaus/me0900_device.h b/drivers/staging/meilhaus/me0900_device.h
new file mode 100644
index 000000000000..bd17f2521511
--- /dev/null
+++ b/drivers/staging/meilhaus/me0900_device.h
@@ -0,0 +1,92 @@
+/**
+ * @file me0900_device.h
+ *
+ * @brief ME-0900 (ME-9x) device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0900_DEVICE_H
+#define _ME0900_DEVICE_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure holding ME-0900 (ME-9x) device capabilities.
+ */
+typedef struct me0900_version {
+	uint16_t device_id;
+	unsigned int di_subdevices;
+	unsigned int do_subdevices;
+} me0900_version_t;
+
+/**
+ * @brief Device capabilities.
+ */
+static me0900_version_t me0900_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME0940, 2, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME0950, 0, 2},
+	{PCI_DEVICE_ID_MEILHAUS_ME0960, 1, 1},
+	{0},
+};
+
+#define ME0900_DEVICE_VERSIONS (sizeof(me0900_versions) / sizeof(me0900_version_t) - 1)	/**< Returns the number of entries in #me0900_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #me0900_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me0900_versions.
+ */
+static inline unsigned int me0900_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME0900_DEVICE_VERSIONS; i++)
+		if (me0900_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The ME-0900 (ME-9x) device class structure.
+ */
+typedef struct me0900_device {
+	me_device_t base;			/**< The Meilhaus device base class. */
+} me0900_device_t;
+
+/**
+ * @brief The ME-9x device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-0900 (ME-9x) device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me0900_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0900_di.c b/drivers/staging/meilhaus/me0900_di.c
new file mode 100644
index 000000000000..d7d7394f800a
--- /dev/null
+++ b/drivers/staging/meilhaus/me0900_di.c
@@ -0,0 +1,246 @@
+/**
+ * @file me0900_di.c
+ *
+ * @brief ME-9x digital input subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/version.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "meids.h"
+#include "medebug.h"
+#include "meplx_reg.h"
+#include "me0900_reg.h"
+#include "me0900_di.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me0900_di_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	PDEBUG("executed.\n");
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0900_di_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me0900_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0900_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+			} else {
+				PERROR("Invalid byte direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0900_di_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me0900_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0900_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = (~inb(instance->port_reg)) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = ~inb(instance->port_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0900_di_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0900_di_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DI;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0900_di_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+me0900_di_subdevice_t *me0900_di_constructor(uint32_t reg_base,
+					     unsigned int di_idx)
+{
+	me0900_di_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me0900_di_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me0900_di_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	/* Save the subdevice index. */
+	subdevice->di_idx = di_idx;
+
+	/* Initialize registers */
+	if (di_idx == 0) {
+		subdevice->ctrl_reg = reg_base + ME0900_CTRL_REG;
+		subdevice->port_reg = reg_base + ME0900_PORT_A_REG;
+	} else {
+		subdevice->ctrl_reg = reg_base + ME0900_CTRL_REG;
+		subdevice->port_reg = reg_base + ME0900_PORT_B_REG;
+	}
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me0900_di_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me0900_di_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me0900_di_io_single_read;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me0900_di_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me0900_di_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me0900_di_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me0900_di.h b/drivers/staging/meilhaus/me0900_di.h
new file mode 100644
index 000000000000..014f1348fc9f
--- /dev/null
+++ b/drivers/staging/meilhaus/me0900_di.h
@@ -0,0 +1,65 @@
+/**
+ * @file me0900_di.h
+ *
+ * @brief ME-9x digital input subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0900_DI_H_
+#define _ME0900_DI_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me0900_di_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+
+	unsigned int di_idx;
+
+	unsigned long ctrl_reg;
+	unsigned long port_reg;
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me0900_di_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-9x digital input subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me0900_di_subdevice_t *me0900_di_constructor(uint32_t me0900_reg_base,
+					     unsigned int di_idx);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0900_do.c b/drivers/staging/meilhaus/me0900_do.c
new file mode 100644
index 000000000000..b5b9c3a98c94
--- /dev/null
+++ b/drivers/staging/meilhaus/me0900_do.c
@@ -0,0 +1,314 @@
+/**
+ * @file me0900_do.c
+ *
+ * @brief ME-9x digital output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me0900_reg.h"
+#include "me0900_do.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me0900_do_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	me0900_do_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0900_do_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	outb(0xFF, instance->port_reg);
+	PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0xff);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0900_do_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me0900_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0900_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+			} else {
+				PERROR("Invalid byte direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0900_do_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me0900_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0900_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = (~inb(instance->port_reg)) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = ~inb(instance->port_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0900_do_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me0900_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long state;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0900_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			state = inb(instance->port_reg);
+			state =
+			    (!value) ? (state | (0x1 << channel)) : (state &
+								     ~(0x1 <<
+								       channel));
+			outb(state, instance->port_reg);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			outb(~(value), instance->port_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0900_do_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0900_do_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0900_do_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+me0900_do_subdevice_t *me0900_do_constructor(uint32_t reg_base,
+					     unsigned int do_idx)
+{
+	me0900_do_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me0900_do_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me0900_do_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	/* Save the subdevice index */
+	subdevice->do_idx = do_idx;
+
+	/* Initialize registers */
+	if (do_idx == 0) {
+		subdevice->ctrl_reg = reg_base + ME0900_CTRL_REG;
+		subdevice->port_reg = reg_base + ME0900_PORT_A_REG;
+		subdevice->enable_reg = reg_base + ME0900_WRITE_ENABLE_REG;
+		subdevice->disable_reg = reg_base + ME0900_WRITE_DISABLE_REG;
+	} else {
+		subdevice->ctrl_reg = reg_base + ME0900_CTRL_REG;
+		subdevice->port_reg = reg_base + ME0900_PORT_B_REG;
+		subdevice->enable_reg = reg_base + ME0900_WRITE_ENABLE_REG;
+		subdevice->disable_reg = reg_base + ME0900_WRITE_DISABLE_REG;
+	}
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me0900_do_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me0900_do_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me0900_do_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me0900_do_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me0900_do_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me0900_do_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me0900_do_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me0900_do.h b/drivers/staging/meilhaus/me0900_do.h
new file mode 100644
index 000000000000..13e8a8b94cfa
--- /dev/null
+++ b/drivers/staging/meilhaus/me0900_do.h
@@ -0,0 +1,68 @@
+/**
+ * @file me0900_do.h
+ *
+ * @brief ME-9x digital output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0900_DO_H_
+#define _ME0900_DO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me0900_do_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+
+	unsigned int do_idx;
+
+	unsigned long ctrl_reg;
+	unsigned long port_reg;
+	unsigned long enable_reg;
+	unsigned long disable_reg;
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me0900_do_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-9x digital output subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param do_idx The index of the digital output subdevice on this device.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me0900_do_subdevice_t *me0900_do_constructor(uint32_t reg_base,
+					     unsigned int do_idx);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0900_reg.h b/drivers/staging/meilhaus/me0900_reg.h
new file mode 100644
index 000000000000..3bf163b6ac49
--- /dev/null
+++ b/drivers/staging/meilhaus/me0900_reg.h
@@ -0,0 +1,40 @@
+/**
+ * @file me0900_reg.h
+ *
+ * @brief ME-9x register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0900_REG_H_
+#define _ME0900_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME0900_PORT_A_REG          0x00
+#define ME0900_PORT_B_REG          0x01
+#define ME0900_PORT_C_REG          0x02
+#define ME0900_CTRL_REG            0x03	// ( ,w)
+#define ME0900_WRITE_ENABLE_REG    0x04	// (r,w)
+#define ME0900_WRITE_DISABLE_REG   0x08	// (r,w)
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me1000_device.c b/drivers/staging/meilhaus/me1000_device.c
new file mode 100644
index 000000000000..c44e214af26c
--- /dev/null
+++ b/drivers/staging/meilhaus/me1000_device.c
@@ -0,0 +1,208 @@
+/**
+ * @file me1000_device.c
+ *
+ * @brief ME-1000 device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "medevice.h"
+#include "me1000_device.h"
+#include "mesubdevice.h"
+#include "me1000_dio.h"
+
+static int me1000_config_load(me_device_t * me_device, struct file *filep,
+			      me_cfg_device_entry_t * config)
+{
+	me1000_device_t *me1000_device;
+	me1000_dio_subdevice_t *dio;
+
+	PDEBUG("executed.\n");
+
+	me1000_device = (me1000_device_t *) me_device;
+
+	if (config->count == 2) {
+		if (me_slist_get_number_subdevices(&me1000_device->base.slist)
+		    == 2) {
+			// Nothing to do.
+		} else {
+			// Remove 2 extra subdevices
+			dio =
+			    (me1000_dio_subdevice_t *)
+			    me_slist_del_subdevice_tail(&me1000_device->base.
+							slist);
+			if (dio)
+				dio->base.
+				    me_subdevice_destructor((me_subdevice_t *)
+							    dio);
+
+			dio =
+			    (me1000_dio_subdevice_t *)
+			    me_slist_del_subdevice_tail(&me1000_device->base.
+							slist);
+			if (dio)
+				dio->base.
+				    me_subdevice_destructor((me_subdevice_t *)
+							    dio);
+		}
+	} else if (config->count == 4) {
+		//Add 2 subdevices
+		if (me_slist_get_number_subdevices(&me1000_device->base.slist)
+		    == 2) {
+			dio =
+			    me1000_dio_constructor(me1000_device->base.info.pci.
+						   reg_bases[2], 2,
+						   &me1000_device->ctrl_lock);
+			if (!dio) {
+				PERROR("Cannot create dio subdevice.\n");
+				return ME_ERRNO_INTERNAL;
+			}
+			me_slist_add_subdevice_tail(&me1000_device->base.slist,
+						    (me_subdevice_t *) dio);
+
+			dio =
+			    me1000_dio_constructor(me1000_device->base.info.pci.
+						   reg_bases[2], 3,
+						   &me1000_device->ctrl_lock);
+			if (!dio) {
+				dio =
+				    (me1000_dio_subdevice_t *)
+				    me_slist_del_subdevice_tail(&me1000_device->
+								base.slist);
+				if (dio)
+					dio->base.
+					    me_subdevice_destructor((me_subdevice_t *) dio);
+
+				PERROR("Cannot create dio subdevice.\n");
+				return ME_ERRNO_INTERNAL;
+			}
+			me_slist_add_subdevice_tail(&me1000_device->base.slist,
+						    (me_subdevice_t *) dio);
+		} else {
+			// Nothing to do.
+		}
+	} else {
+		PERROR("Invalid configuration.\n");
+		return ME_ERRNO_INTERNAL;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+me_device_t *me1000_pci_constructor(struct pci_dev * pci_device)
+{
+	me1000_device_t *me1000_device;
+	me_subdevice_t *subdevice;
+	int err;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me1000_device = kmalloc(sizeof(me1000_device_t), GFP_KERNEL);
+
+	if (!me1000_device) {
+		PERROR("Cannot get memory for ME-1000 device instance.\n");
+		return NULL;
+	}
+
+	memset(me1000_device, 0, sizeof(me1000_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me1000_device, pci_device);
+
+	if (err) {
+		kfree(me1000_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+	// Initialize spin lock .
+	spin_lock_init(&me1000_device->ctrl_lock);
+
+	for (i = 0; i < 4; i++) {
+		subdevice =
+		    (me_subdevice_t *) me1000_dio_constructor(me1000_device->
+							      base.info.pci.
+							      reg_bases[2], i,
+							      &me1000_device->
+							      ctrl_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me1000_device);
+			kfree(me1000_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me1000_device->base.slist,
+					    subdevice);
+	}
+
+	// Overwrite base class methods.
+	me1000_device->base.me_device_config_load = me1000_config_load;
+
+	return (me_device_t *) me1000_device;
+}
+
+// Init and exit of module.
+static int __init me1000_init(void)
+{
+	PDEBUG("executed.\n");
+	return 0;
+}
+
+static void __exit me1000_exit(void)
+{
+	PDEBUG("executed.\n");
+}
+
+module_init(me1000_init);
+module_exit(me1000_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for Meilhaus ME-1000 Devices");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-1000 Digital I/O Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me1000_pci_constructor);
diff --git a/drivers/staging/meilhaus/me1000_device.h b/drivers/staging/meilhaus/me1000_device.h
new file mode 100644
index 000000000000..cbbe1263017d
--- /dev/null
+++ b/drivers/staging/meilhaus/me1000_device.h
@@ -0,0 +1,59 @@
+/**
+ * @file me1000_device.h
+ *
+ * @brief ME-1000 device class instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1000_H_
+#define _ME1000_H_
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+#define ME1000_MAGIC_NUMBER	1000
+
+/**
+ * @brief The ME-1000 device class structure.
+ */
+typedef struct me1000_device {
+	me_device_t base;		/**< The Meilhaus device base class. */
+	spinlock_t ctrl_lock;		/**< Guards the DIO mode register. */
+} me1000_device_t;
+
+/**
+ * @brief The ME-1000 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-1000 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me1000_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me1000_dio.c b/drivers/staging/meilhaus/me1000_dio.c
new file mode 100644
index 000000000000..87605a9108ae
--- /dev/null
+++ b/drivers/staging/meilhaus/me1000_dio.c
@@ -0,0 +1,438 @@
+/**
+ * @file me1000_dio.c
+ *
+ * @brief ME-1000 DIO subdevice instance.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+#include "medebug.h"
+
+#include "me1000_dio_reg.h"
+#include "me1000_dio.h"
+
+/*
+ * Defines
+ */
+#define ME1000_DIO_MAGIC_NUMBER	0x1000	/**< The magic number of the class structure. */
+
+/*
+ * Functions
+ */
+
+static int me1000_dio_io_reset_subdevice(struct me_subdevice *subdevice,
+					 struct file *filep, int flags)
+{
+	me1000_dio_subdevice_t *instance;
+	uint32_t tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1000_dio_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	tmp = inl(instance->ctrl_reg);
+	tmp &= ~(0x1 << instance->dio_idx);
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outl(0x00000000, instance->port_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, 0);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1000_dio_io_single_config(struct me_subdevice *subdevice,
+				       struct file *filep,
+				       int channel,
+				       int single_config,
+				       int ref,
+				       int trig_chan,
+				       int trig_type, int trig_edge, int flags)
+{
+	me1000_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	int ctrl;
+	int size =
+	    flags & (ME_IO_SINGLE_CONFIG_DIO_BIT | ME_IO_SINGLE_CONFIG_DIO_BYTE
+		     | ME_IO_SINGLE_CONFIG_DIO_WORD |
+		     ME_IO_SINGLE_CONFIG_DIO_DWORD);
+
+	PDEBUG("executed.\n");
+
+	instance = (me1000_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	ctrl = inl(instance->ctrl_reg);
+
+	switch (size) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_DWORD:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+				ctrl &= ~(0x1 << instance->dio_idx);
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+				ctrl |= 0x1 << instance->dio_idx;
+			} else {
+				PERROR("Invalid port direction.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!err) {
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me1000_dio_io_single_read(struct me_subdevice *subdevice,
+				     struct file *filep,
+				     int channel,
+				     int *value, int time_out, int flags)
+{
+	me1000_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1000_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 32)) {
+			*value = inl(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if ((channel >= 0) && (channel < 4)) {
+			*value =
+			    (inl(instance->port_reg) >> (channel * 8)) & 0xFF;
+		} else {
+			PERROR("Invalid byte number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_WORD:
+		if ((channel >= 0) && (channel < 2)) {
+			*value =
+			    (inl(instance->port_reg) >> (channel * 16)) &
+			    0xFFFF;
+		} else {
+			PERROR("Invalid word number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_DWORD:
+		if (channel == 0) {
+			*value = inl(instance->port_reg);
+		} else {
+			PERROR("Invalid dword number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me1000_dio_io_single_write(struct me_subdevice *subdevice,
+				      struct file *filep,
+				      int channel,
+				      int value, int time_out, int flags)
+{
+	me1000_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t config;
+	uint32_t state;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1000_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	config = inl(instance->ctrl_reg) & (0x1 << instance->dio_idx);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 32)) {
+			if (config) {
+				state = inl(instance->port_reg);
+				state =
+				    value ? (state | (0x1 << channel)) : (state
+									  &
+									  ~(0x1
+									    <<
+									    channel));
+				outl(state, instance->port_reg);
+				PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->port_reg -
+					   instance->reg_base, state);
+			} else {
+				PERROR("Port is not in output mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if ((channel >= 0) && (channel < 4)) {
+			if (config) {
+				state = inl(instance->port_reg);
+				state &= ~(0xFF << (channel * 8));
+				state |= (value & 0xFF) << (channel * 8);
+				outl(state, instance->port_reg);
+				PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->port_reg -
+					   instance->reg_base, state);
+			} else {
+				PERROR("Port is not in output mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_WORD:
+		if ((channel >= 0) && (channel < 2)) {
+			if (config) {
+				state = inl(instance->port_reg);
+				state &= ~(0xFFFF << (channel * 16));
+				state |= (value & 0xFFFF) << (channel * 16);
+				outl(state, instance->port_reg);
+				PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->port_reg -
+					   instance->reg_base, state);
+			} else {
+				PERROR("Port is not in output mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid word number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_DWORD:
+		if (channel == 0) {
+			if (config) {
+				outl(value, instance->port_reg);
+				PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->port_reg -
+					   instance->reg_base, value);
+			} else {
+				PERROR("Port is not in output mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid dword number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me1000_dio_query_number_channels(struct me_subdevice *subdevice,
+					    int *number)
+{
+	PDEBUG("executed.\n");
+	*number = ME1000_DIO_NUMBER_CHANNELS;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1000_dio_query_subdevice_type(struct me_subdevice *subdevice,
+					   int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DIO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1000_dio_query_subdevice_caps(struct me_subdevice *subdevice,
+					   int *caps)
+{
+	me1000_dio_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1000_dio_subdevice_t *) subdevice;
+
+	*caps = ME_CAPS_DIO_DIR_DWORD;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+me1000_dio_subdevice_t *me1000_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock)
+{
+	me1000_dio_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me1000_dio_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for ME-1000 DIO instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me1000_dio_subdevice_t));
+
+	/* Check if counter index is out of range */
+
+	if (dio_idx >= ME1000_DIO_NUMBER_PORTS) {
+		PERROR("DIO index is out of range.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save the DIO index */
+	subdevice->dio_idx = dio_idx;
+
+	/* Initialize registers. */
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+	subdevice->ctrl_reg = reg_base + ME1000_PORT_MODE;
+	subdevice->port_reg =
+	    reg_base + ME1000_PORT + (dio_idx * ME1000_PORT_STEP);
+
+	/* Override base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me1000_dio_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me1000_dio_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me1000_dio_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me1000_dio_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me1000_dio_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me1000_dio_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me1000_dio_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me1000_dio.h b/drivers/staging/meilhaus/me1000_dio.h
new file mode 100644
index 000000000000..d26e93f531af
--- /dev/null
+++ b/drivers/staging/meilhaus/me1000_dio.h
@@ -0,0 +1,71 @@
+/**
+ * @file me1000_dio.h
+ *
+ * @brief Meilhaus ME-1000 digital i/o implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1000_DIO_H_
+#define _ME1000_DIO_H_
+
+#include "mesubdevice.h"
+#include "meslock.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The ME-1000 DIO subdevice class.
+ */
+typedef struct me1000_dio_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+//      uint32_t magic;                                 /**< The magic number unique for this structure. */
+
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg and #ctrl_reg_mirror from concurrent access. */
+	int dio_idx;					/**< The index of the DIO port on the device. */
+
+	unsigned long port_reg;			/**< Register to read or write a value from or to the port respectively. */
+	unsigned long ctrl_reg;			/**< Register to configure the DIO modes. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me1000_dio_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-1000 DIO instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param dio_idx The index of the DIO on the device.
+ * @param ctrl_reg_lock Pointer to spin lock protecting the control register and from concurrent access.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me1000_dio_subdevice_t *me1000_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me1000_dio_reg.h b/drivers/staging/meilhaus/me1000_dio_reg.h
new file mode 100644
index 000000000000..4d5b38df437f
--- /dev/null
+++ b/drivers/staging/meilhaus/me1000_dio_reg.h
@@ -0,0 +1,50 @@
+/**
+ * @file me1000_dio_reg.h
+ *
+ * @brief ME-1000 digital i/o register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1000_DIO_REG_H_
+# define _ME1000_DIO_REG_H_
+
+# ifdef __KERNEL__
+
+# define ME1000_DIO_NUMBER_CHANNELS	32				/**< The number of channels per DIO port. */
+# define ME1000_DIO_NUMBER_PORTS	4				/**< The number of ports per ME-1000. */
+
+// # define ME1000_PORT_A                               0x0000                  /**< Port A base register offset. */
+// # define ME1000_PORT_B                               0x0004                  /**< Port B base register offset. */
+// # define ME1000_PORT_C                               0x0008                  /**< Port C base register offset. */
+// # define ME1000_PORT_D                               0x000C                  /**< Port D base register offset. */
+# define ME1000_PORT				0x0000			/**< Base for port's register. */
+# define ME1000_PORT_STEP			4				/**< Distance between port's register. */
+
+# define ME1000_PORT_MODE			0x0010			/**< Configuration register to switch the port direction. */
+// # define ME1000_PORT_MODE_OUTPUT_A   (1 << 0)                /**< If set, port A is in output, otherwise in input mode. */
+// # define ME1000_PORT_MODE_OUTPUT_B   (1 << 1)                /**< If set, port B is in output, otherwise in input mode. */
+// # define ME1000_PORT_MODE_OUTPUT_C   (1 << 2)                /**< If set, port C is in output, otherwise in input mode. */
+// # define ME1000_PORT_MODE_OUTPUT_D   (1 << 3)                /**< If set, port D is in output, otherwise in input mode. */
+
+# endif	//__KERNEL__
+#endif //_ME1000_DIO_REG_H_
diff --git a/drivers/staging/meilhaus/me1400_device.c b/drivers/staging/meilhaus/me1400_device.c
new file mode 100644
index 000000000000..b95bb4fce6ab
--- /dev/null
+++ b/drivers/staging/meilhaus/me1400_device.c
@@ -0,0 +1,256 @@
+/**
+ * @file me1400_device.c
+ *
+ * @brief ME-1400 device instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file 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.
+ */
+
+/*
+ * User application could also include the kernel header files. But the
+ * real kernel functions are protected by #ifdef __KERNEL__.
+ */
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * This must be defined before module.h is included. Not needed, when
+ * it is a built in driver.
+ */
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+#include <linux/sched.h>
+#include <linux/interrupt.h>
+#include <linux/version.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+
+#include "me1400_device.h"
+#include "me8254.h"
+#include "me8254_reg.h"
+#include "me8255.h"
+#include "me1400_ext_irq.h"
+
+me_device_t *me1400_pci_constructor(struct pci_dev *pci_device)
+{
+	int err;
+	me1400_device_t *me1400_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	unsigned int me8255_idx;
+	unsigned int dio_idx;
+	unsigned int me8254_idx;
+	unsigned int ctr_idx;
+	unsigned int ext_irq_idx;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me1400_device = kmalloc(sizeof(me1400_device_t), GFP_KERNEL);
+
+	if (!me1400_device) {
+		PERROR("Cannot get memory for 1400ate device instance.\n");
+		return NULL;
+	}
+
+	memset(me1400_device, 0, sizeof(me1400_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me1400_device, pci_device);
+
+	if (err) {
+		kfree(me1400_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+
+	/* Check for ME1400 extension device. If detected we fake a ME-1400 D device id. */
+	if (me1400_device->base.info.pci.device_id ==
+	    PCI_DEVICE_ID_MEILHAUS_ME140C) {
+		uint8_t ctrl;
+		ctrl =
+		    inb(me1400_device->base.info.pci.reg_bases[2] +
+			ME1400D_CLK_SRC_2_REG);
+		PDEBUG_REG("xxx_reg inb(0x%X+0x%X)=0x%x\n",
+			   me1400_device->base.info.pci.reg_bases[2],
+			   ME1400D_CLK_SRC_2_REG, ctrl);
+		outb(ctrl | 0xF0,
+		     me1400_device->base.info.pci.reg_bases[2] +
+		     ME1400D_CLK_SRC_2_REG);
+		PDEBUG_REG("xxx_reg outb(0x%X+0x%X)=0x%x\n",
+			   me1400_device->base.info.pci.reg_bases[2],
+			   ME1400D_CLK_SRC_2_REG, ctrl | 0xF0);
+		ctrl =
+		    inb(me1400_device->base.info.pci.reg_bases[2] +
+			ME1400D_CLK_SRC_2_REG);
+		PDEBUG_REG("xxx_reg inb(0x%X+0x%X)=0x%x\n",
+			   me1400_device->base.info.pci.reg_bases[2],
+			   ME1400D_CLK_SRC_2_REG, ctrl);
+
+		if ((ctrl & 0xF0) == 0xF0) {
+			PINFO("ME1400 D detected.\n");
+			me1400_device->base.info.pci.device_id =
+			    PCI_DEVICE_ID_MEILHAUS_ME140D;
+		}
+	}
+
+	/* Initialize global stuff of digital i/o subdevices. */
+	for (me8255_idx = 0; me8255_idx < ME1400_MAX_8255; me8255_idx++) {
+		me1400_device->dio_current_mode[me8255_idx] = 0;
+		spin_lock_init(&me1400_device->dio_ctrl_reg_lock[me8255_idx]);
+	}
+
+	/* Initialize global stuff of counter subdevices. */
+	spin_lock_init(&me1400_device->clk_src_reg_lock);
+
+	for (me8254_idx = 0; me8254_idx < ME1400_MAX_8254; me8254_idx++)
+		spin_lock_init(&me1400_device->ctr_ctrl_reg_lock[me8254_idx]);
+
+	/* Get the index in the device version information table. */
+	version_idx =
+	    me1400_versions_get_device_index(me1400_device->base.info.pci.
+					     device_id);
+
+	/* Generate DIO subdevice instances. */
+	for (me8255_idx = 0;
+	     me8255_idx < me1400_versions[version_idx].dio_chips;
+	     me8255_idx++) {
+		for (dio_idx = 0; dio_idx < 3; dio_idx++) {
+			subdevice =
+			    (me_subdevice_t *)
+			    me8255_constructor(me1400_versions[version_idx].
+					       device_id,
+					       me1400_device->base.info.pci.
+					       reg_bases[2], me8255_idx,
+					       dio_idx,
+					       &me1400_device->
+					       dio_current_mode[me8255_idx],
+					       &me1400_device->
+					       dio_ctrl_reg_lock[me8255_idx]);
+
+			if (!subdevice) {
+				me_device_deinit((me_device_t *) me1400_device);
+				kfree(me1400_device);
+				PERROR("Cannot get memory for subdevice.\n");
+				return NULL;
+			}
+
+			me_slist_add_subdevice_tail(&me1400_device->base.slist,
+						    subdevice);
+		}
+	}
+
+	/* Generate counter subdevice instances. */
+	for (me8254_idx = 0;
+	     me8254_idx < me1400_versions[version_idx].ctr_chips;
+	     me8254_idx++) {
+		for (ctr_idx = 0; ctr_idx < 3; ctr_idx++) {
+			subdevice =
+			    (me_subdevice_t *)
+			    me8254_constructor(me1400_device->base.info.pci.
+					       device_id,
+					       me1400_device->base.info.pci.
+					       reg_bases[2], me8254_idx,
+					       ctr_idx,
+					       &me1400_device->
+					       ctr_ctrl_reg_lock[me8254_idx],
+					       &me1400_device->
+					       clk_src_reg_lock);
+
+			if (!subdevice) {
+				me_device_deinit((me_device_t *) me1400_device);
+				kfree(me1400_device);
+				PERROR("Cannot get memory for subdevice.\n");
+				return NULL;
+			}
+
+			me_slist_add_subdevice_tail(&me1400_device->base.slist,
+						    subdevice);
+		}
+	}
+
+	/* Generate external interrupt subdevice instances. */
+	for (ext_irq_idx = 0;
+	     ext_irq_idx < me1400_versions[version_idx].ext_irq_subdevices;
+	     ext_irq_idx++) {
+		subdevice =
+		    (me_subdevice_t *)
+		    me1400_ext_irq_constructor(me1400_device->base.info.pci.
+					       device_id,
+					       me1400_device->base.info.pci.
+					       reg_bases[1],
+					       me1400_device->base.info.pci.
+					       reg_bases[2],
+					       &me1400_device->clk_src_reg_lock,
+					       me1400_device->base.irq);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me1400_device);
+			kfree(me1400_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me1400_device->base.slist,
+					    subdevice);
+	}
+
+	return (me_device_t *) me1400_device;
+}
+
+// Init and exit of module.
+
+static int __init me1400_init(void)
+{
+	PDEBUG("executed.\n");
+	return 0;
+}
+
+static void __exit me1400_exit(void)
+{
+	PDEBUG("executed.\n");
+}
+
+module_init(me1400_init);
+module_exit(me1400_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for Meilhaus ME-14xx devices");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-14xx MIO devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me1400_pci_constructor);
diff --git a/drivers/staging/meilhaus/me1400_device.h b/drivers/staging/meilhaus/me1400_device.h
new file mode 100644
index 000000000000..6215b250047d
--- /dev/null
+++ b/drivers/staging/meilhaus/me1400_device.h
@@ -0,0 +1,108 @@
+/**
+ * @file me1400_device.c
+ *
+ * @brief ME-1400 device family instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1400_DEVICE_H_
+#define _ME1400_DEVICE_H_
+
+#include "metypes.h"
+#include "medefines.h"
+#include "meinternal.h"
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure to store device capabilities.
+ */
+typedef struct me1400_version {
+	uint16_t device_id;					/**< The PCI device id of the device. */
+	unsigned int dio_chips;				/**< The number of 8255 chips on the device. */
+	unsigned int ctr_chips;				/**< The number of 8254 chips on the device. */
+	unsigned int ext_irq_subdevices;	/**< The number of external interrupt inputs on the device. */
+} me1400_version_t;
+
+/**
+  * @brief Defines for each ME-1400 device version its capabilities.
+ */
+static me1400_version_t me1400_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME1400, 1, 0, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME140A, 1, 1, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME140B, 2, 2, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME14E0, 1, 0, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME14EA, 1, 1, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME14EB, 2, 2, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME140C, 1, 5, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME140D, 2, 10, 1},
+	{0}
+};
+
+#define ME1400_DEVICE_VERSIONS (sizeof(me1400_versions) / sizeof(me1400_version_t) - 1)	/**< Returns the number of entries in #me1400_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #me1400_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me1400_versions.
+ */
+static inline unsigned int me1400_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME1400_DEVICE_VERSIONS; i++)
+		if (me1400_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+#define ME1400_MAX_8254		10	/**< The maximum number of 8254 counter subdevices available on any ME-1400 device. */
+#define ME1400_MAX_8255		2	/**< The maximum number of 8255 digital i/o subdevices available on any ME-1400 device. */
+
+/**
+ * @brief The ME-1400 device class.
+ */
+typedef struct me1400_device {
+	me_device_t base;									/**< The Meilhaus device base class. */
+
+	spinlock_t clk_src_reg_lock;						/**< Guards the 8254 clock source registers. */
+	spinlock_t ctr_ctrl_reg_lock[ME1400_MAX_8254];		/**< Guards the 8254 ctrl registers. */
+
+	int dio_current_mode[ME1400_MAX_8255];				/**< Saves the current mode setting of a single 8255 DIO chip. */
+	spinlock_t dio_ctrl_reg_lock[ME1400_MAX_8255];		/**< Guards the 8255 ctrl register and #dio_current_mode. */
+} me1400_device_t;
+
+/**
+ * @brief The ME-1400 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-1400 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me1400_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me1400_ext_irq.c b/drivers/staging/meilhaus/me1400_ext_irq.c
new file mode 100644
index 000000000000..b4df7cc58ab7
--- /dev/null
+++ b/drivers/staging/meilhaus/me1400_ext_irq.c
@@ -0,0 +1,517 @@
+/**
+ * @file me1400_ext_irq.c
+ *
+ * @brief ME-1400 external interrupt subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/version.h>
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+#include "medebug.h"
+#include "meids.h"
+
+#include "me1400_ext_irq.h"
+#include "me1400_ext_irq_reg.h"
+
+/*
+ * Defines
+ */
+#define ME1400_EXT_IRQ_MAGIC_NUMBER	0x1401	/**< The magic number of the class structure. */
+#define ME1400_EXT_IRQ_NUMBER_CHANNELS 1	/**< One channel per counter. */
+
+/*
+ * Functions
+ */
+
+static int me1400_ext_irq_io_irq_start(struct me_subdevice *subdevice,
+				       struct file *filep,
+				       int channel,
+				       int irq_source,
+				       int irq_edge, int irq_arg, int flags)
+{
+	me1400_ext_irq_subdevice_t *instance;
+	unsigned long cpu_flags;
+	uint8_t tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1400_ext_irq_subdevice_t *) subdevice;
+
+	if (flags & ~ME_IO_IRQ_START_DIO_BIT) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (irq_source != ME_IRQ_SOURCE_DIO_LINE) {
+		PERROR("Invalid irq source.\n");
+		return ME_ERRNO_INVALID_IRQ_SOURCE;
+	}
+
+	if (irq_edge != ME_IRQ_EDGE_RISING) {
+		PERROR("Invalid irq edge.\n");
+		return ME_ERRNO_INVALID_IRQ_EDGE;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	spin_lock(instance->clk_src_reg_lock);
+//                      // Enable IRQ on PLX
+//                      tmp = inb(instance->plx_intcs_reg) | (PLX_LOCAL_INT1_EN | PLX_LOCAL_INT1_POL | PLX_PCI_INT_EN);
+//                      outb(tmp, instance->plx_intcs_reg);
+//                      PDEBUG_REG("ctrl_reg outb(PLX:0x%lX)=0x%x\n", instance->plx_intcs_reg, tmp);
+
+	// Enable IRQ
+	switch (instance->device_id) {
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		tmp = inb(instance->ctrl_reg);
+		tmp |= ME1400CD_EXT_IRQ_CLK_EN;
+		outb(tmp, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, tmp);
+		break;
+
+	default:
+		outb(ME1400AB_EXT_IRQ_IRQ_EN, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base,
+			   ME1400AB_EXT_IRQ_IRQ_EN);
+		break;
+	}
+	spin_unlock(instance->clk_src_reg_lock);
+	instance->rised = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1400_ext_irq_io_irq_wait(struct me_subdevice *subdevice,
+				      struct file *filep,
+				      int channel,
+				      int *irq_count,
+				      int *value, int time_out, int flags)
+{
+	me1400_ext_irq_subdevice_t *instance;
+	unsigned long cpu_flags;
+	long t = 0;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1400_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time out.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		/* Convert to ticks */
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->rised <= 0) {
+		instance->rised = 0;
+		if (time_out) {
+			t = wait_event_interruptible_timeout(instance->
+							     wait_queue,
+							     (instance->rised !=
+							      0), t);
+
+			if (t == 0) {
+				PERROR("Wait on interrupt timed out.\n");
+				err = ME_ERRNO_TIMEOUT;
+			}
+		} else {
+			wait_event_interruptible(instance->wait_queue,
+						 (instance->rised != 0));
+		}
+
+		if (instance->rised < 0) {
+			PERROR("Wait on interrupt aborted by user.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+	}
+
+	if (signal_pending(current)) {
+		PERROR("Wait on interrupt aborted by signal.\n");
+		err = ME_ERRNO_SIGNAL;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	instance->rised = 0;
+	*irq_count = instance->n;
+	*value = 1;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me1400_ext_irq_io_irq_stop(struct me_subdevice *subdevice,
+				      struct file *filep,
+				      int channel, int flags)
+{
+	me1400_ext_irq_subdevice_t *instance;
+	unsigned long cpu_flags;
+	uint8_t tmp;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1400_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->clk_src_reg_lock);
+//                      // Disable IRQ on PLX
+//                      tmp = inb(instance->plx_intcs_reg) & ( ~(PLX_LOCAL_INT1_EN | PLX_LOCAL_INT1_POL | PLX_PCI_INT_EN));
+//                      outb(tmp, instance->plx_intcs_reg);
+//                      PDEBUG_REG("ctrl_reg outb(PLX:0x%lX)=0x%x\n", instance->plx_intcs_reg, tmp);
+
+	switch (instance->device_id) {
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		tmp = inb(instance->ctrl_reg);
+		tmp &= ~ME1400CD_EXT_IRQ_CLK_EN;
+		outb(tmp, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, tmp);
+
+		break;
+
+	default:
+		outb(0x00, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, 0x00);
+		break;
+	}
+	spin_unlock(instance->clk_src_reg_lock);
+	instance->rised = -1;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me1400_ext_irq_io_reset_subdevice(struct me_subdevice *subdevice,
+					     struct file *filep, int flags)
+{
+	me1400_ext_irq_subdevice_t *instance =
+	    (me1400_ext_irq_subdevice_t *) subdevice;
+
+	PDEBUG("executed.\n");
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	instance->n = 0;
+	return me1400_ext_irq_io_irq_stop(subdevice, filep, 0, flags);
+}
+
+static int me1400_ext_irq_query_number_channels(struct me_subdevice *subdevice,
+						int *number)
+{
+	PDEBUG("executed.\n");
+	*number = ME1400_EXT_IRQ_NUMBER_CHANNELS;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1400_ext_irq_query_subdevice_type(struct me_subdevice *subdevice,
+					       int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_EXT_IRQ;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1400_ext_irq_query_subdevice_caps(struct me_subdevice *subdevice,
+					       int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_EXT_IRQ_EDGE_RISING;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1400_ext_irq_query_subdevice_caps_args(struct me_subdevice
+						    *subdevice, int cap,
+						    int *args, int count)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me1400_ext_irq_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me1400_ext_irq_isr(int irq, void *dev_id,
+				      struct pt_regs *regs)
+#endif
+{
+	me1400_ext_irq_subdevice_t *instance;
+	uint32_t status;
+	uint8_t tmp;
+
+	instance = (me1400_ext_irq_subdevice_t *) dev_id;
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	spin_lock(&instance->subdevice_lock);
+	status = inl(instance->plx_intcs_reg);
+//              if (!((status & PLX_LOCAL_INT1_STATE) && (status & PLX_LOCAL_INT1_EN) && (status & PLX_PCI_INT_EN)))
+	if ((status &
+	     (PLX_LOCAL_INT1_STATE | PLX_LOCAL_INT1_EN | PLX_PCI_INT_EN)) !=
+	    (PLX_LOCAL_INT1_STATE | PLX_LOCAL_INT1_EN | PLX_PCI_INT_EN)) {
+		spin_unlock(&instance->subdevice_lock);
+		PINFO("%ld Shared interrupt. %s(): irq_status_reg=0x%04X\n",
+		      jiffies, __func__, status);
+		return IRQ_NONE;
+	}
+
+	inl(instance->ctrl_reg);
+
+	PDEBUG("executed.\n");
+
+	instance->n++;
+	instance->rised = 1;
+
+	switch (instance->device_id) {
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		spin_lock(instance->clk_src_reg_lock);
+		tmp = inb(instance->ctrl_reg);
+		tmp &= ~ME1400CD_EXT_IRQ_CLK_EN;
+		outb(tmp, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outb(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, tmp);
+		tmp |= ME1400CD_EXT_IRQ_CLK_EN;
+		outb(tmp, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outb(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, tmp);
+		spin_unlock(instance->clk_src_reg_lock);
+
+		break;
+
+	default:
+		outb(0, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outb(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, 0);
+		outb(ME1400AB_EXT_IRQ_IRQ_EN, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outb(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base,
+			   ME1400AB_EXT_IRQ_IRQ_EN);
+		break;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+static void me1400_ext_irq_destructor(struct me_subdevice *subdevice)
+{
+	me1400_ext_irq_subdevice_t *instance;
+	uint8_t tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1400_ext_irq_subdevice_t *) subdevice;
+
+	// Disable IRQ on PLX
+	tmp =
+	    inb(instance->
+		plx_intcs_reg) & (~(PLX_LOCAL_INT1_EN | PLX_LOCAL_INT1_POL |
+				    PLX_PCI_INT_EN));
+	outb(tmp, instance->plx_intcs_reg);
+	PDEBUG_REG("ctrl_reg outb(plx:0x%lX)=0x%x\n", instance->plx_intcs_reg,
+		   tmp);
+
+	free_irq(instance->irq, (void *)instance);
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+me1400_ext_irq_subdevice_t *me1400_ext_irq_constructor(uint32_t device_id,
+						       uint32_t plx_reg_base,
+						       uint32_t me1400_reg_base,
+						       spinlock_t *
+						       clk_src_reg_lock,
+						       int irq)
+{
+	me1400_ext_irq_subdevice_t *subdevice;
+	int err;
+	uint8_t tmp;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me1400_ext_irq_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for 1400_ext_irq instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me1400_ext_irq_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+	subdevice->clk_src_reg_lock = clk_src_reg_lock;
+
+	/* Initialize wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	subdevice->irq = irq;
+
+	err = request_irq(irq, me1400_ext_irq_isr,
+#ifdef IRQF_DISABLED
+			  IRQF_DISABLED | IRQF_SHARED,
+#else
+			  SA_INTERRUPT | SA_SHIRQ,
+#endif
+			  ME1400_NAME, (void *)subdevice);
+
+	if (err) {
+		PERROR("Can't get irq.\n");
+		me_subdevice_deinit(&subdevice->base);
+		kfree(subdevice);
+		return NULL;
+	}
+	PINFO("Registered irq=%d.\n", subdevice->irq);
+
+	/* Initialize registers */
+	subdevice->plx_intcs_reg = plx_reg_base + PLX_INTCSR_REG;
+	subdevice->ctrl_reg = me1400_reg_base + ME1400AB_EXT_IRQ_CTRL_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = me1400_reg_base;
+#endif
+
+	// Enable IRQ on PLX
+	tmp =
+	    inb(subdevice->
+		plx_intcs_reg) | (PLX_LOCAL_INT1_EN | PLX_LOCAL_INT1_POL |
+				  PLX_PCI_INT_EN);
+	outb(tmp, subdevice->plx_intcs_reg);
+	PDEBUG_REG("ctrl_reg outb(Pplx:0x%lX)=0x%x\n", subdevice->plx_intcs_reg,
+		   tmp);
+
+	/* Initialize the subdevice methods */
+	subdevice->base.me_subdevice_io_irq_start = me1400_ext_irq_io_irq_start;
+	subdevice->base.me_subdevice_io_irq_wait = me1400_ext_irq_io_irq_wait;
+	subdevice->base.me_subdevice_io_irq_stop = me1400_ext_irq_io_irq_stop;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me1400_ext_irq_io_reset_subdevice;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me1400_ext_irq_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me1400_ext_irq_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me1400_ext_irq_query_subdevice_caps;
+	subdevice->base.me_subdevice_query_subdevice_caps_args =
+	    me1400_ext_irq_query_subdevice_caps_args;
+	subdevice->base.me_subdevice_destructor = me1400_ext_irq_destructor;
+
+	subdevice->rised = 0;
+	subdevice->n = 0;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me1400_ext_irq.h b/drivers/staging/meilhaus/me1400_ext_irq.h
new file mode 100644
index 000000000000..9b72a04701c0
--- /dev/null
+++ b/drivers/staging/meilhaus/me1400_ext_irq.h
@@ -0,0 +1,62 @@
+/**
+ * @file me1400_ext_irq.h
+ *
+ * @brief ME-1400 external interrupt implementation.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME1400_EXT_IRQ_H_
+#define _ME1400_EXT_IRQ_H_
+
+#include <linux/sched.h>
+
+#include "mesubdevice.h"
+#include "meslock.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The ME-1400 external interrupt subdevice class.
+ */
+typedef struct me1400_ext_irq_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *clk_src_reg_lock;	/**< Lock protecting the clock control register. */
+
+	wait_queue_head_t wait_queue;	/**< Queue to put on threads waiting for an interrupt. */
+
+	uint32_t device_id;				/**< The device id of the device holding the subdevice. */
+	int irq;						/**< The irq number assigned by PCI BIOS. */
+	int rised;						/**< If true an interrupt has occured. */
+	unsigned int n;					/**< The number of interrupt since the driver was loaded. */
+
+	unsigned long plx_intcs_reg;	/**< The PLX interrupt control and status register. */
+	unsigned long ctrl_reg;			/**< The control register. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me1400_ext_irq_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-1400 external interrupt instance.
+ *
+ * @param plx_reg_base The register base address of the PLX chip as returned by the PCI BIOS.
+ * @param me1400_reg_base The register base address of the ME-1400 device as returned by the PCI BIOS.
+ * @param irq The irq assigned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me1400_ext_irq_subdevice_t *me1400_ext_irq_constructor(uint32_t device_id,
+						       uint32_t plx_reg_base,
+						       uint32_t me1400_reg_base,
+						       spinlock_t *
+						       clk_src_reg_lock,
+						       int irq);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me1400_ext_irq_reg.h b/drivers/staging/meilhaus/me1400_ext_irq_reg.h
new file mode 100644
index 000000000000..c9740f2dd3a7
--- /dev/null
+++ b/drivers/staging/meilhaus/me1400_ext_irq_reg.h
@@ -0,0 +1,56 @@
+/**
+ * @file me1400_ext_irq_reg.h
+ *
+ * @brief ME-1400 external interrupt register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1400_EXT_IRQ_REG_H_
+# define _ME1400_EXT_IRQ_REG_H_
+
+# ifdef __KERNEL__
+
+#  define PLX_INTCSR_REG				0x4C	/**< The PLX interrupt control and status register offset. */
+#  define PLX_ICR_REG					0x50	/**< The PLX initialization control register offset. */
+
+#  define PLX_LOCAL_INT1_EN				0x01	/**< If set the local interrupt 1 is enabled. */
+#  define PLX_LOCAL_INT1_POL			0x02	/**< If set the local interrupt 1 polarity is high active. */
+#  define PLX_LOCAL_INT1_STATE			0x04	/**< If set the local interrupt 1 is activ. */
+#  define PLX_LOCAL_INT2_EN				0x08	/**< If set the local interrupt 2 is enabled. */
+#  define PLX_LOCAL_INT2_POL			0x10	/**< If set the local interrupt 2 polarity is high active. */
+#  define PLX_LOCAL_INT2_STATE			0x20	/**< If set the local interrupt 2 is activ. */
+#  define PLX_PCI_INT_EN				0x40	/**< If set the PCI interrupt is enabled. */
+#  define PLX_SOFT_INT					0x80	/**< If set an interrupt is generated. */
+
+#  define ME1400AB_EXT_IRQ_CTRL_REG		0x11	/**< The external interrupt control register offset. */
+
+#  define ME1400AB_EXT_IRQ_CLK_EN		0x01	/**< If this bit is set, the clock output is enabled. */
+#  define ME1400AB_EXT_IRQ_IRQ_EN		0x02	/**< If set the external interrupt is enabled. Clearing this bit clears a pending interrupt. */
+
+#  define ME1400CD_EXT_IRQ_CTRL_REG		0x11	/**< The external interrupt control register offset. */
+
+#  define ME1400CD_EXT_IRQ_CLK_EN		0x10	/**< If set the external interrupt is enabled. Clearing this bit clears a pending interrupt.*/
+
+# endif	//__KERNEL__
+
+#endif //_ME1400_EXT_IRQ_REG_H_
diff --git a/drivers/staging/meilhaus/me1600_ao.c b/drivers/staging/meilhaus/me1600_ao.c
new file mode 100644
index 000000000000..d127c6b00307
--- /dev/null
+++ b/drivers/staging/meilhaus/me1600_ao.c
@@ -0,0 +1,1033 @@
+/**
+ * @file me1600_ao.c
+ *
+ * @brief ME-1600 analog output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/* Includes
+ */
+
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/sched.h>
+
+#include <linux/workqueue.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+#include "medebug.h"
+
+#include "me1600_ao_reg.h"
+#include "me1600_ao.h"
+
+/* Defines
+ */
+
+static void me1600_ao_destructor(struct me_subdevice *subdevice);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void me1600_ao_work_control_task(void *subdevice);
+#else
+static void me1600_ao_work_control_task(struct work_struct *work);
+#endif
+
+static int me1600_ao_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags);
+static int me1600_ao_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep, int channel,
+				      int single_config, int ref, int trig_chan,
+				      int trig_type, int trig_edge, int flags);
+static int me1600_ao_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep, int channel, int *value,
+				    int time_out, int flags);
+static int me1600_ao_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep, int channel, int value,
+				     int time_out, int flags);
+static int me1600_ao_query_number_channels(me_subdevice_t * subdevice,
+					   int *number);
+static int me1600_ao_query_subdevice_type(me_subdevice_t * subdevice, int *type,
+					  int *subtype);
+static int me1600_ao_query_subdevice_caps(me_subdevice_t * subdevice,
+					  int *caps);
+static int me1600_ao_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit, int *min, int *max,
+					    int *maxdata, int *range);
+static int me1600_ao_query_number_ranges(me_subdevice_t * subdevice, int unit,
+					 int *count);
+static int me1600_ao_query_range_info(me_subdevice_t * subdevice, int range,
+				      int *unit, int *min, int *max,
+				      int *maxdata);
+
+/* Functions
+ */
+
+me1600_ao_subdevice_t *me1600_ao_constructor(uint32_t reg_base,
+					     unsigned int ao_idx,
+					     int curr,
+					     spinlock_t * config_regs_lock,
+					     spinlock_t * ao_shadows_lock,
+					     me1600_ao_shadow_t *
+					     ao_regs_shadows,
+					     struct workqueue_struct *me1600_wq)
+{
+	me1600_ao_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed. idx=%d\n", ao_idx);
+
+	// Allocate memory for subdevice instance.
+	subdevice = kmalloc(sizeof(me1600_ao_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR
+		    ("Cannot get memory for analog output subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me1600_ao_subdevice_t));
+
+	// Initialize subdevice base class.
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+	subdevice->config_regs_lock = config_regs_lock;
+	subdevice->ao_shadows_lock = ao_shadows_lock;
+
+	// Save the subdevice index.
+	subdevice->ao_idx = ao_idx;
+
+	// Initialize range lists.
+	subdevice->u_ranges_count = 2;
+
+	subdevice->u_ranges[0].min = 0;	//0V
+	subdevice->u_ranges[0].max = 9997558;	//10V
+
+	subdevice->u_ranges[1].min = -10E6;	//-10V
+	subdevice->u_ranges[1].max = 9995117;	//10V
+
+	if (curr) {		// This is version with current outputs.
+		subdevice->i_ranges_count = 2;
+
+		subdevice->i_ranges[0].min = 0;	//0mA
+		subdevice->i_ranges[0].max = 19995117;	//20mA
+
+		subdevice->i_ranges[1].min = 4E3;	//4mA
+		subdevice->i_ranges[1].max = 19995118;	//20mA
+	} else {		// This is version without current outputs.
+		subdevice->i_ranges_count = 0;
+
+		subdevice->i_ranges[0].min = 0;	//0mA
+		subdevice->i_ranges[0].max = 0;	//0mA
+
+		subdevice->i_ranges[1].min = 0;	//0mA
+		subdevice->i_ranges[1].max = 0;	//0mA
+	}
+
+	// Initialize registers.
+	subdevice->uni_bi_reg = reg_base + ME1600_UNI_BI_REG;
+	subdevice->i_range_reg = reg_base + ME1600_020_420_REG;
+	subdevice->sim_output_reg = reg_base + ME1600_SIM_OUTPUT_REG;
+	subdevice->current_on_reg = reg_base + ME1600_CURRENT_ON_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	// Initialize shadow structure.
+	subdevice->ao_regs_shadows = ao_regs_shadows;
+
+	// Override base class methods.
+	subdevice->base.me_subdevice_destructor = me1600_ao_destructor;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me1600_ao_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me1600_ao_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me1600_ao_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me1600_ao_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me1600_ao_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me1600_ao_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me1600_ao_query_subdevice_caps;
+	subdevice->base.me_subdevice_query_range_by_min_max =
+	    me1600_ao_query_range_by_min_max;
+	subdevice->base.me_subdevice_query_number_ranges =
+	    me1600_ao_query_number_ranges;
+	subdevice->base.me_subdevice_query_range_info =
+	    me1600_ao_query_range_info;
+
+	// Initialize wait queue.
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	// Prepare work queue.
+	subdevice->me1600_workqueue = me1600_wq;
+
+/* workqueue API changed in kernel 2.6.20 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20) )
+	INIT_WORK(&subdevice->ao_control_task, me1600_ao_work_control_task,
+		  (void *)subdevice);
+#else
+	INIT_DELAYED_WORK(&subdevice->ao_control_task,
+			  me1600_ao_work_control_task);
+#endif
+	return subdevice;
+}
+
+static void me1600_ao_destructor(struct me_subdevice *subdevice)
+{
+	me1600_ao_subdevice_t *instance;
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	instance->ao_control_task_flag = 0;
+
+	// Reset subdevice to asure clean exit.
+	me1600_ao_io_reset_subdevice(subdevice, NULL,
+				     ME_IO_RESET_SUBDEVICE_NO_FLAGS);
+
+	// Remove any tasks from work queue. This is paranoic because it was done allready in reset().
+	if (!cancel_delayed_work(&instance->ao_control_task)) {	//Wait 2 ticks to be sure that control task is removed from queue.
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(2);
+	}
+}
+
+static int me1600_ao_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags)
+{
+	me1600_ao_subdevice_t *instance;
+	uint16_t tmp;
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+	(instance->ao_regs_shadows)->trigger &= ~(0x1 << instance->ao_idx);	//Cancell waiting for trigger.
+
+	// Reset all settings.
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ao_shadows_lock);
+	(instance->ao_regs_shadows)->shadow[instance->ao_idx] = 0;
+	(instance->ao_regs_shadows)->mirror[instance->ao_idx] = 0;
+	(instance->ao_regs_shadows)->trigger &= ~(0x1 << instance->ao_idx);	//Not waiting for triggering.
+	(instance->ao_regs_shadows)->synchronous &= ~(0x1 << instance->ao_idx);	//Individual triggering.
+
+	// Set output to default (safe) state.
+	spin_lock(instance->config_regs_lock);
+	tmp = inw(instance->uni_bi_reg);	// unipolar
+	tmp |= (0x1 << instance->ao_idx);
+	outw(tmp, instance->uni_bi_reg);
+	PDEBUG_REG("uni_bi_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->uni_bi_reg - instance->reg_base, tmp);
+
+	tmp = inw(instance->current_on_reg);	// Volts only!
+	tmp &= ~(0x1 << instance->ao_idx);
+	tmp &= 0x00FF;
+	outw(tmp, instance->current_on_reg);
+	PDEBUG_REG("current_on_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->current_on_reg - instance->reg_base, tmp);
+
+	tmp = inw(instance->i_range_reg);	// 0..20mA <= If exists.
+	tmp &= ~(0x1 << instance->ao_idx);
+	outw(tmp, instance->i_range_reg);
+	PDEBUG_REG("i_range_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->i_range_reg - instance->reg_base, tmp);
+
+	outw(0, (instance->ao_regs_shadows)->registry[instance->ao_idx]);
+	PDEBUG_REG("channel_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   (instance->ao_regs_shadows)->registry[instance->ao_idx] -
+		   instance->reg_base, 0);
+
+	// Trigger output.
+	outw(0x0000, instance->sim_output_reg);
+	PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->sim_output_reg - instance->reg_base, 0x0000);
+	outw(0xFFFF, instance->sim_output_reg);
+	PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->sim_output_reg - instance->reg_base, 0xFFFF);
+	spin_unlock(instance->config_regs_lock);
+	spin_unlock(instance->ao_shadows_lock);
+
+	// Set status to 'none'
+	instance->status = ao_status_none;
+	spin_unlock(&instance->subdevice_lock);
+
+	//Signal reset if user is on wait.
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1600_ao_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me1600_ao_subdevice_t *instance;
+	uint16_t tmp;
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	// Checking parameters.
+	if (flags) {
+		PERROR
+		    ("Invalid flag specified. Must be ME_IO_SINGLE_CONFIG_NO_FLAGS.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (trig_edge != ME_TRIG_EDGE_NONE) {
+		PERROR
+		    ("Invalid trigger edge. Software trigger has not edge. Must be ME_TRIG_EDGE_NONE\n");
+		return ME_ERRNO_INVALID_TRIG_EDGE;
+	}
+
+	if (trig_type != ME_TRIG_TYPE_SW) {
+		PERROR("Invalid trigger edge. Must be ME_TRIG_TYPE_SW.\n");
+		return ME_ERRNO_INVALID_TRIG_TYPE;
+	}
+
+	if ((trig_chan != ME_TRIG_CHAN_DEFAULT)
+	    && (trig_chan != ME_TRIG_CHAN_SYNCHRONOUS)) {
+		PERROR("Invalid trigger channel specified.\n");
+		return ME_ERRNO_INVALID_TRIG_CHAN;
+	}
+
+	if (ref != ME_REF_AO_GROUND) {
+		PERROR
+		    ("Invalid reference. Analog outputs have to have got REF_AO_GROUND.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if (((single_config + 1) >
+	     (instance->u_ranges_count + instance->i_ranges_count))
+	    || (single_config < 0)) {
+		PERROR("Invalid range specified.\n");
+		return ME_ERRNO_INVALID_SINGLE_CONFIG;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+	// Checking parameters - done. All is fine. Do config.
+
+	ME_SUBDEVICE_ENTER;
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ao_shadows_lock);
+	(instance->ao_regs_shadows)->trigger &= ~(0x1 << instance->ao_idx);	//Cancell waiting for trigger.
+	(instance->ao_regs_shadows)->shadow[instance->ao_idx] = 0;
+	(instance->ao_regs_shadows)->mirror[instance->ao_idx] = 0;
+
+	spin_lock(instance->config_regs_lock);
+	switch (single_config) {
+	case 0:		// 0V 10V
+		tmp = inw(instance->current_on_reg);	// Volts
+		tmp &= ~(0x1 << instance->ao_idx);
+		outw(tmp, instance->current_on_reg);
+		PDEBUG_REG("current_on_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->current_on_reg - instance->reg_base, tmp);
+
+		// 0V
+		outw(0,
+		     (instance->ao_regs_shadows)->registry[instance->ao_idx]);
+		PDEBUG_REG("channel_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   (instance->ao_regs_shadows)->registry[instance->
+								 ao_idx] -
+			   instance->reg_base, 0);
+
+		tmp = inw(instance->uni_bi_reg);	// unipolar
+		tmp |= (0x1 << instance->ao_idx);
+		outw(tmp, instance->uni_bi_reg);
+		PDEBUG_REG("uni_bi_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->uni_bi_reg - instance->reg_base, tmp);
+
+		tmp = inw(instance->i_range_reg);	// 0..20mA <= If exists.
+		tmp &= ~(0x1 << instance->ao_idx);
+		outw(tmp, instance->i_range_reg);
+		PDEBUG_REG("i_range_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->i_range_reg - instance->reg_base, tmp);
+		break;
+
+	case 1:		// -10V 10V
+		tmp = inw(instance->current_on_reg);	// Volts
+		tmp &= ~(0x1 << instance->ao_idx);
+		outw(tmp, instance->current_on_reg);
+		PDEBUG_REG("current_on_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->current_on_reg - instance->reg_base, tmp);
+
+		// 0V
+		outw(0x0800,
+		     (instance->ao_regs_shadows)->registry[instance->ao_idx]);
+		PDEBUG_REG("channel_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   (instance->ao_regs_shadows)->registry[instance->
+								 ao_idx] -
+			   instance->reg_base, 0x0800);
+
+		tmp = inw(instance->uni_bi_reg);	// bipolar
+		tmp &= ~(0x1 << instance->ao_idx);
+		outw(tmp, instance->uni_bi_reg);
+		PDEBUG_REG("uni_bi_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->uni_bi_reg - instance->reg_base, tmp);
+
+		tmp = inw(instance->i_range_reg);	// 0..20mA <= If exists.
+		tmp &= ~(0x1 << instance->ao_idx);
+		outw(tmp, instance->i_range_reg);
+		PDEBUG_REG("i_range_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->i_range_reg - instance->reg_base, tmp);
+		break;
+
+	case 2:		// 0mA 20mA
+		tmp = inw(instance->current_on_reg);	// mAmpers
+		tmp |= (0x1 << instance->ao_idx);
+		outw(tmp, instance->current_on_reg);
+		PDEBUG_REG("current_on_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->current_on_reg - instance->reg_base, tmp);
+
+		tmp = inw(instance->i_range_reg);	// 0..20mA
+		tmp &= ~(0x1 << instance->ao_idx);
+		outw(tmp, instance->i_range_reg);
+		PDEBUG_REG("i_range_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->i_range_reg - instance->reg_base, tmp);
+
+		// 0mA
+		outw(0,
+		     (instance->ao_regs_shadows)->registry[instance->ao_idx]);
+		PDEBUG_REG("channel_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   (instance->ao_regs_shadows)->registry[instance->
+								 ao_idx] -
+			   instance->reg_base, 0);
+
+		tmp = inw(instance->uni_bi_reg);	// unipolar
+		tmp |= (0x1 << instance->ao_idx);
+		outw(tmp, instance->uni_bi_reg);
+		PDEBUG_REG("uni_bi_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->uni_bi_reg - instance->reg_base, tmp);
+		break;
+
+	case 3:		// 4mA 20mA
+		tmp = inw(instance->current_on_reg);	// mAmpers
+		tmp |= (0x1 << instance->ao_idx);
+		outw(tmp, instance->current_on_reg);
+		PDEBUG_REG("current_on_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->current_on_reg - instance->reg_base, tmp);
+
+		tmp = inw(instance->i_range_reg);	// 4..20mA
+		tmp |= (0x1 << instance->ao_idx);
+		outw(tmp, instance->i_range_reg);
+		PDEBUG_REG("i_range_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->i_range_reg - instance->reg_base, tmp);
+
+		// 4mA
+		outw(0,
+		     (instance->ao_regs_shadows)->registry[instance->ao_idx]);
+		PDEBUG_REG("channel_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   (instance->ao_regs_shadows)->registry[instance->
+								 ao_idx] -
+			   instance->reg_base, 0);
+
+		tmp = inw(instance->uni_bi_reg);	// unipolar
+		tmp |= (0x1 << instance->ao_idx);
+		outw(tmp, instance->uni_bi_reg);
+		PDEBUG_REG("uni_bi_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->uni_bi_reg - instance->reg_base, tmp);
+		break;
+	}
+
+	// Trigger output.
+	outw(0x0000, instance->sim_output_reg);
+	PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->sim_output_reg - instance->reg_base, 0x0000);
+	outw(0xFFFF, instance->sim_output_reg);
+	PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->sim_output_reg - instance->reg_base, 0xFFFF);
+
+	if (trig_chan == ME_TRIG_CHAN_DEFAULT) {	// Individual triggering.
+		(instance->ao_regs_shadows)->synchronous &=
+		    ~(0x1 << instance->ao_idx);
+		PDEBUG("Individual triggering.\n");
+	} else if (trig_chan == ME_TRIG_CHAN_SYNCHRONOUS) {	// Synchronous triggering.
+		(instance->ao_regs_shadows)->synchronous |=
+		    (0x1 << instance->ao_idx);
+		PDEBUG("Synchronous triggering.\n");
+	}
+	spin_unlock(instance->config_regs_lock);
+	spin_unlock(instance->ao_shadows_lock);
+
+	instance->status = ao_status_single_configured;
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1600_ao_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me1600_ao_subdevice_t *instance;
+	unsigned long delay = 0;
+	unsigned long j = 0;
+	int err = ME_ERRNO_SUCCESS;
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags & ~ME_IO_SINGLE_NONBLOCKING) {
+		PERROR("Invalid flag specified. %d\n", flags);
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if ((!flags) && ((instance->ao_regs_shadows)->trigger & instance->ao_idx)) {	//Blocking mode. Wait for software trigger.
+		if (time_out) {
+			delay = (time_out * HZ) / 1000;
+			if (delay == 0)
+				delay = 1;
+		}
+
+		j = jiffies;
+
+		//Only runing process will interrupt this call. Events are signaled when status change. This procedure has own timeout.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (!((instance->
+						     ao_regs_shadows)->
+						    trigger & instance->
+						    ao_idx)),
+						 (delay) ? delay : LONG_MAX);
+
+		if (instance == ao_status_none) {	// Reset was called.
+			PDEBUG("Single canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		if ((delay) && ((jiffies - j) >= delay)) {
+			PDEBUG("Timeout reached.\n");
+			err = ME_ERRNO_TIMEOUT;
+		}
+	}
+
+	*value = (instance->ao_regs_shadows)->mirror[instance->ao_idx];
+
+	return err;
+}
+
+static int me1600_ao_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me1600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long delay = 0;
+	int i;
+	unsigned long j = 0;
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags &
+	    ~(ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS |
+	      ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING)) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (value & ~ME1600_AO_MAX_DATA) {
+		PERROR("Invalid value provided.\n");
+		return ME_ERRNO_VALUE_OUT_OF_RANGE;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+	(instance->ao_regs_shadows)->trigger &= ~(0x1 << instance->ao_idx);	//Cancell waiting for trigger.
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+
+		if (delay == 0)
+			delay = 1;
+	}
+	//Write value.
+	spin_lock(instance->ao_shadows_lock);
+	(instance->ao_regs_shadows)->shadow[instance->ao_idx] =
+	    (uint16_t) value;
+
+	if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {	// Trigger all outputs from synchronous list.
+		for (i = 0; i < (instance->ao_regs_shadows)->count; i++) {
+			if (((instance->ao_regs_shadows)->synchronous & (0x1 << i)) || (i == instance->ao_idx)) {	// Set all from synchronous list to correct state.
+				PDEBUG
+				    ("Synchronous triggering: output %d. idx=%d\n",
+				     i, instance->ao_idx);
+				(instance->ao_regs_shadows)->mirror[i] =
+				    (instance->ao_regs_shadows)->shadow[i];
+
+				outw((instance->ao_regs_shadows)->shadow[i],
+				     (instance->ao_regs_shadows)->registry[i]);
+				PDEBUG_REG
+				    ("channel_reg outw(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     (instance->ao_regs_shadows)->registry[i] -
+				     instance->reg_base,
+				     (instance->ao_regs_shadows)->shadow[i]);
+
+				(instance->ao_regs_shadows)->trigger &=
+				    ~(0x1 << i);
+			}
+		}
+
+		// Trigger output.
+		outw(0x0000, instance->sim_output_reg);
+		PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->sim_output_reg - instance->reg_base, 0);
+		outw(0xFFFF, instance->sim_output_reg);
+		PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->sim_output_reg - instance->reg_base,
+			   0xFFFF);
+		instance->status = ao_status_single_end;
+	} else {		// Individual mode.
+		if ((instance->ao_regs_shadows)->synchronous & (0x1 << instance->ao_idx)) {	// Put on synchronous start list. Set output as waiting for trigger.
+			PDEBUG("Add to synchronous list. idx=%d\n",
+			       instance->ao_idx);
+			(instance->ao_regs_shadows)->trigger |=
+			    (0x1 << instance->ao_idx);
+			instance->status = ao_status_single_run;
+			PDEBUG("Synchronous list: 0x%x.\n",
+			       (instance->ao_regs_shadows)->synchronous);
+		} else {	// Fired this one.
+			PDEBUG("Triggering. idx=%d\n", instance->ao_idx);
+			(instance->ao_regs_shadows)->mirror[instance->ao_idx] =
+			    (instance->ao_regs_shadows)->shadow[instance->
+								ao_idx];
+
+			outw((instance->ao_regs_shadows)->
+			     shadow[instance->ao_idx],
+			     (instance->ao_regs_shadows)->registry[instance->
+								   ao_idx]);
+			PDEBUG_REG("channel_reg outw(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   (instance->ao_regs_shadows)->
+				   registry[instance->ao_idx] -
+				   instance->reg_base,
+				   (instance->ao_regs_shadows)->
+				   shadow[instance->ao_idx]);
+
+			// Set output as triggered.
+			(instance->ao_regs_shadows)->trigger &=
+			    ~(0x1 << instance->ao_idx);
+
+			// Trigger output.
+			outw(0x0000, instance->sim_output_reg);
+			PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->sim_output_reg -
+				   instance->reg_base, 0);
+			outw(0xFFFF, instance->sim_output_reg);
+			PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->sim_output_reg -
+				   instance->reg_base, 0xFFFF);
+			instance->status = ao_status_single_end;
+		}
+	}
+	spin_unlock(instance->ao_shadows_lock);
+
+	//Init control task
+	instance->timeout.delay = delay;
+	instance->timeout.start_time = jiffies;
+	instance->ao_control_task_flag = 1;
+	queue_delayed_work(instance->me1600_workqueue,
+			   &instance->ao_control_task, 1);
+
+	if ((!flags & ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING) && ((instance->ao_regs_shadows)->trigger & instance->ao_idx)) {	//Blocking mode. Wait for software trigger.
+		if (time_out) {
+			delay = (time_out * HZ) / 1000;
+			if (delay == 0)
+				delay = 1;
+		}
+
+		j = jiffies;
+
+		//Only runing process will interrupt this call. Events are signaled when status change. This procedure has own timeout.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (!((instance->
+						     ao_regs_shadows)->
+						    trigger & instance->
+						    ao_idx)),
+						 (delay) ? delay : LONG_MAX);
+
+		if (instance == ao_status_none) {
+			PDEBUG("Single canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		if ((delay) && ((jiffies - j) >= delay)) {
+			PDEBUG("Timeout reached.\n");
+			err = ME_ERRNO_TIMEOUT;
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me1600_ao_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	me1600_ao_subdevice_t *instance;
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*number = 1;		//Every subdevice has only 1 channel.
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1600_ao_query_subdevice_type(me_subdevice_t * subdevice, int *type,
+					  int *subtype)
+{
+	me1600_ao_subdevice_t *instance;
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*type = ME_TYPE_AO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1600_ao_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_AO_TRIG_SYNCHRONOUS;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1600_ao_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range)
+{
+	me1600_ao_subdevice_t *instance;
+	int i;
+	int r = -1;
+	int diff = 21E6;
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if ((*max - *min) < 0) {
+		PERROR("Invalid minimum and maximum values specified.\n");
+		return ME_ERRNO_INVALID_MIN_MAX;
+	}
+	// Maximum ranges are slightly less then 10V or 20mA. For convenient we accepted this value as valid one.
+	if (unit == ME_UNIT_VOLT) {
+		for (i = 0; i < instance->u_ranges_count; i++) {
+			if ((instance->u_ranges[i].min <= *min)
+			    && ((instance->u_ranges[i].max + 5000) >= *max)) {
+				if ((instance->u_ranges[i].max -
+				     instance->u_ranges[i].min) - (*max -
+								   *min) <
+				    diff) {
+					r = i;
+					diff =
+					    (instance->u_ranges[i].max -
+					     instance->u_ranges[i].min) -
+					    (*max - *min);
+				}
+			}
+		}
+
+		if (r < 0) {
+			PERROR("No matching range found.\n");
+			return ME_ERRNO_NO_RANGE;
+		} else {
+			*min = instance->u_ranges[r].min;
+			*max = instance->u_ranges[r].max;
+			*range = r;
+		}
+	} else if (unit == ME_UNIT_AMPERE) {
+		for (i = 0; i < instance->i_ranges_count; i++) {
+			if ((instance->i_ranges[i].min <= *min)
+			    && (instance->i_ranges[i].max + 5000 >= *max)) {
+				if ((instance->i_ranges[i].max -
+				     instance->i_ranges[i].min) - (*max -
+								   *min) <
+				    diff) {
+					r = i;
+					diff =
+					    (instance->i_ranges[i].max -
+					     instance->i_ranges[i].min) -
+					    (*max - *min);
+				}
+			}
+		}
+
+		if (r < 0) {
+			PERROR("No matching range found.\n");
+			return ME_ERRNO_NO_RANGE;
+		} else {
+			*min = instance->i_ranges[r].min;
+			*max = instance->i_ranges[r].max;
+			*range = r + instance->u_ranges_count;
+		}
+	} else {
+		PERROR("Invalid physical unit specified.\n");
+		return ME_ERRNO_INVALID_UNIT;
+	}
+	*maxdata = ME1600_AO_MAX_DATA;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1600_ao_query_number_ranges(me_subdevice_t * subdevice,
+					 int unit, int *count)
+{
+	me1600_ao_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+	switch (unit) {
+	case ME_UNIT_VOLT:
+		*count = instance->u_ranges_count;
+		break;
+	case ME_UNIT_AMPERE:
+		*count = instance->i_ranges_count;
+		break;
+	case ME_UNIT_ANY:
+		*count = instance->u_ranges_count + instance->i_ranges_count;
+		break;
+	default:
+		*count = 0;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1600_ao_query_range_info(me_subdevice_t * subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata)
+{
+	me1600_ao_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	if (((range + 1) >
+	     (instance->u_ranges_count + instance->i_ranges_count))
+	    || (range < 0)) {
+		PERROR("Invalid range number specified.\n");
+		return ME_ERRNO_INVALID_RANGE;
+	}
+
+	if (range < instance->u_ranges_count) {
+		*unit = ME_UNIT_VOLT;
+		*min = instance->u_ranges[range].min;
+		*max = instance->u_ranges[range].max;
+	} else if (range < instance->u_ranges_count + instance->i_ranges_count) {
+		*unit = ME_UNIT_AMPERE;
+		*min = instance->i_ranges[range - instance->u_ranges_count].min;
+		*max = instance->i_ranges[range - instance->u_ranges_count].max;
+	}
+	*maxdata = ME1600_AO_MAX_DATA;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void me1600_ao_work_control_task(void *subdevice)
+#else
+static void me1600_ao_work_control_task(struct work_struct *work)
+#endif
+{
+	me1600_ao_subdevice_t *instance;
+	int reschedule = 1;
+	int signaling = 0;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	instance = (me1600_ao_subdevice_t *) subdevice;
+#else
+	instance =
+	    container_of((void *)work, me1600_ao_subdevice_t, ao_control_task);
+#endif
+
+	PINFO("<%s: %ld> executed. idx=%d\n", __func__, jiffies,
+	      instance->ao_idx);
+
+	if (!((instance->ao_regs_shadows)->trigger & instance->ao_idx)) {	// Output was triggerd.
+		// Signal the end.
+		signaling = 1;
+		reschedule = 0;
+		if (instance->status == ao_status_single_run) {
+			instance->status = ao_status_single_end;
+		}
+
+	} else if ((instance->timeout.delay) && ((jiffies - instance->timeout.start_time) >= instance->timeout.delay)) {	// Timeout
+		PDEBUG("Timeout reached.\n");
+		spin_lock(instance->ao_shadows_lock);
+		// Restore old settings.
+		PDEBUG("Write old value back to register.\n");
+		(instance->ao_regs_shadows)->shadow[instance->ao_idx] =
+		    (instance->ao_regs_shadows)->mirror[instance->ao_idx];
+
+		outw((instance->ao_regs_shadows)->mirror[instance->ao_idx],
+		     (instance->ao_regs_shadows)->registry[instance->ao_idx]);
+		PDEBUG_REG("channel_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   (instance->ao_regs_shadows)->registry[instance->
+								 ao_idx] -
+			   instance->reg_base,
+			   (instance->ao_regs_shadows)->mirror[instance->
+							       ao_idx]);
+
+		//Remove from synchronous strt list.
+		(instance->ao_regs_shadows)->trigger &=
+		    ~(0x1 << instance->ao_idx);
+		if (instance->status == ao_status_none) {
+			instance->status = ao_status_single_end;
+		}
+		spin_unlock(instance->ao_shadows_lock);
+
+		// Signal the end.
+		signaling = 1;
+		reschedule = 0;
+	}
+
+	if (signaling) {	//Signal it.
+		wake_up_interruptible_all(&instance->wait_queue);
+	}
+
+	if (instance->ao_control_task_flag && reschedule) {	// Reschedule task
+		queue_delayed_work(instance->me1600_workqueue,
+				   &instance->ao_control_task, 1);
+	} else {
+		PINFO("<%s> Ending control task.\n", __func__);
+	}
+
+}
diff --git a/drivers/staging/meilhaus/me1600_ao.h b/drivers/staging/meilhaus/me1600_ao.h
new file mode 100644
index 000000000000..b82bf5a1676e
--- /dev/null
+++ b/drivers/staging/meilhaus/me1600_ao.h
@@ -0,0 +1,132 @@
+/**
+ * @file me1600_ao.h
+ *
+ * @brief Meilhaus ME-1600 analog output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1600_AO_H_
+#define _ME1600_AO_H_
+
+# include <linux/version.h>
+# include "mesubdevice.h"
+
+# ifdef __KERNEL__
+
+#  define ME1600_MAX_RANGES	2	/**< Specifies the maximum number of ranges in me1600_ao_subdevice_t::u_ranges und me1600_ao_subdevice_t::i_ranges. */
+
+/**
+ * @brief Defines a entry in the range table.
+ */
+typedef struct me1600_ao_range_entry {
+	int32_t min;
+	int32_t max;
+} me1600_ao_range_entry_t;
+
+typedef struct me1600_ao_timeout {
+	unsigned long start_time;
+	unsigned long delay;
+} me1600_ao_timeout_t;
+
+typedef struct me1600_ao_shadow {
+	int count;
+	unsigned long *registry;
+	uint16_t *shadow;
+	uint16_t *mirror;
+	uint16_t synchronous;									/**< Synchronization list. */
+	uint16_t trigger;										/**< Synchronization flag. */
+} me1600_ao_shadow_t;
+
+typedef enum ME1600_AO_STATUS {
+	ao_status_none = 0,
+	ao_status_single_configured,
+	ao_status_single_run,
+	ao_status_single_end,
+	ao_status_last
+} ME1600_AO_STATUS;
+
+/**
+ * @brief The ME-1600 analog output subdevice class.
+ */
+typedef struct me1600_ao_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;									/**< The subdevice base class. */
+
+	/* Attributes */
+	int ao_idx;												/**< The index of the analog output subdevice on the device. */
+
+	spinlock_t subdevice_lock;								/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *config_regs_lock;							/**< Spin lock to protect configuration registers from concurrent access. */
+
+	int u_ranges_count;										/**< The number of voltage ranges available on this subdevice. */
+	me1600_ao_range_entry_t u_ranges[ME1600_MAX_RANGES];	/**< Array holding the voltage ranges on this subdevice. */
+	int i_ranges_count;										/**< The number of current ranges available on this subdevice. */
+	me1600_ao_range_entry_t i_ranges[ME1600_MAX_RANGES];	/**< Array holding the current ranges on this subdevice. */
+
+	/* Registers */
+	unsigned long uni_bi_reg;								/**< Register for switching between unipoar and bipolar output mode. */
+	unsigned long i_range_reg;								/**< Register for switching between ranges. */
+	unsigned long sim_output_reg;							/**< Register used in order to update all channels simultaneously. */
+	unsigned long current_on_reg;							/**< Register enabling current output on the fourth subdevice. */
+#   ifdef PDEBUG_REG
+	unsigned long reg_base;
+#   endif
+
+	ME1600_AO_STATUS status;
+	me1600_ao_shadow_t *ao_regs_shadows;					/**< Addresses and shadows of output's registers. */
+	spinlock_t *ao_shadows_lock;							/**< Protects the shadow's struct. */
+	int mode;												/**< Mode in witch output should works. */
+	wait_queue_head_t wait_queue;							/**< Wait queue to put on tasks waiting for data to arrive. */
+	me1600_ao_timeout_t timeout;							/**< The timeout for start in blocking and non-blocking mode. */
+	struct workqueue_struct *me1600_workqueue;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	struct work_struct ao_control_task;
+#else
+	struct delayed_work ao_control_task;
+#endif
+
+	volatile int ao_control_task_flag;						/**< Flag controling reexecuting of control task */
+} me1600_ao_subdevice_t;
+
+/**
+ * @brief The constructor to generate a subdevice template instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param ao_idx The index of the analog output subdevice on the device.
+ * @param current Flag indicating that analog output with #ao_idx of 3 is capable of current output.
+ * @param config_regs_lock Pointer to spin lock protecting the configuration registers and from concurrent access.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me1600_ao_subdevice_t *me1600_ao_constructor(uint32_t reg_base,
+					     unsigned int ao_idx,
+					     int curr,
+					     spinlock_t * config_regs_lock,
+					     spinlock_t * ao_shadows_lock,
+					     me1600_ao_shadow_t *
+					     ao_regs_shadows,
+					     struct workqueue_struct
+					     *me1600_wq);
+
+# endif	//__KERNEL__
+#endif //_ME1600_AO_H_
diff --git a/drivers/staging/meilhaus/me1600_ao_reg.h b/drivers/staging/meilhaus/me1600_ao_reg.h
new file mode 100644
index 000000000000..31e7800e8074
--- /dev/null
+++ b/drivers/staging/meilhaus/me1600_ao_reg.h
@@ -0,0 +1,66 @@
+/**
+ * @file me1600_ao_reg.h
+ *
+ * @brief ME-1600 analog output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1600_AO_REG_H_
+#define _ME1600_AO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME1600_CHANNEL_0_REG    0x00	/**< Register to set a digital value on channel 0. */
+#define ME1600_CHANNEL_1_REG    0x02	/**< Register to set a digital value on channel 1. */
+#define ME1600_CHANNEL_2_REG    0x04	/**< Register to set a digital value on channel 2. */
+#define ME1600_CHANNEL_3_REG    0x06	/**< Register to set a digital value on channel 3. */
+#define ME1600_CHANNEL_4_REG    0x08	/**< Register to set a digital value on channel 4. */
+#define ME1600_CHANNEL_5_REG    0x0A	/**< Register to set a digital value on channel 5. */
+#define ME1600_CHANNEL_6_REG    0x0C	/**< Register to set a digital value on channel 6. */
+#define ME1600_CHANNEL_7_REG    0x0E	/**< Register to set a digital value on channel 7. */
+#define ME1600_CHANNEL_8_REG    0x10	/**< Register to set a digital value on channel 8. */
+#define ME1600_CHANNEL_9_REG    0x12	/**< Register to set a digital value on channel 9. */
+#define ME1600_CHANNEL_10_REG   0x14	/**< Register to set a digital value on channel 10. */
+#define ME1600_CHANNEL_11_REG   0x16	/**< Register to set a digital value on channel 11. */
+#define ME1600_CHANNEL_12_REG   0x18	/**< Register to set a digital value on channel 12. */
+#define ME1600_CHANNEL_13_REG   0x1A	/**< Register to set a digital value on channel 13. */
+#define ME1600_CHANNEL_14_REG   0x1C	/**< Register to set a digital value on channel 14. */
+#define ME1600_CHANNEL_15_REG   0x1E	/**< Register to set a digital value on channel 15. */
+
+/* Every channel one bit: bipolar = 0, unipolar = 1 */
+#define ME1600_UNI_BI_REG		0x20	/**< Register to switch between unipolar and bipolar. */
+
+/* Every channel one bit (only lower 8 Bits): 0..20mA = 0, 4..20mA = 1 */
+#define ME1600_020_420_REG		0x22	/**< Register to switch between the two current ranges. */
+
+/* If a bit is set, the corresponding DAC (4 ports each) is
+   not set at the moment you write to an output of it.
+   Clearing the bit updates the port. */
+#define ME1600_SIM_OUTPUT_REG	0x24	/**< Register to update all channels of a subdevice simultaneously. */
+
+/* Current on/off (only lower 8 bits): off = 0, on  = 1 */
+#define ME1600_CURRENT_ON_REG	0x26	/**< Register to swicht between voltage and current output. */
+
+#define ME1600_AO_MAX_DATA		0x0FFF	/**< The maximum digital data accepted by an analog output channel. */
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me1600_device.c b/drivers/staging/meilhaus/me1600_device.c
new file mode 100644
index 000000000000..3bc2cb1dc869
--- /dev/null
+++ b/drivers/staging/meilhaus/me1600_device.c
@@ -0,0 +1,261 @@
+/**
+ * @file me1600_device.c
+ *
+ * @brief ME-1600 device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "medevice.h"
+#include "mesubdevice.h"
+#include "me1600_device.h"
+
+static void me1600_set_registry(me1600_device_t * subdevice, uint32_t reg_base);
+static void me1600_destructor(struct me_device *device);
+
+/**
+ * @brief Global variable.
+ * This is working queue for runing a separate atask that will be responsible for work status (start, stop, timeouts).
+ */
+static struct workqueue_struct *me1600_workqueue;
+
+me_device_t *me1600_pci_constructor(struct pci_dev *pci_device)
+{
+	int err;
+	me1600_device_t *me1600_device;
+	me_subdevice_t *subdevice;
+	unsigned int chip_idx;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me1600_device = kmalloc(sizeof(me1600_device_t), GFP_KERNEL);
+
+	if (!me1600_device) {
+		PERROR("Cannot get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(me1600_device, 0, sizeof(me1600_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me1600_device, pci_device);
+
+	if (err) {
+		kfree(me1600_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+	// Initialize spin lock .
+	spin_lock_init(&me1600_device->config_regs_lock);
+	spin_lock_init(&me1600_device->ao_shadows_lock);
+
+	// Get the number of analog output subdevices.
+	chip_idx =
+	    me1600_versions_get_device_index(me1600_device->base.info.pci.
+					     device_id);
+
+	// Create shadow instance.
+	me1600_device->ao_regs_shadows.count =
+	    me1600_versions[chip_idx].ao_chips;
+	me1600_device->ao_regs_shadows.registry =
+	    kmalloc(me1600_versions[chip_idx].ao_chips * sizeof(unsigned long),
+		    GFP_KERNEL);
+	me1600_set_registry(me1600_device,
+			    me1600_device->base.info.pci.reg_bases[2]);
+	me1600_device->ao_regs_shadows.shadow =
+	    kmalloc(me1600_versions[chip_idx].ao_chips * sizeof(uint16_t),
+		    GFP_KERNEL);
+	me1600_device->ao_regs_shadows.mirror =
+	    kmalloc(me1600_versions[chip_idx].ao_chips * sizeof(uint16_t),
+		    GFP_KERNEL);
+
+	// Create subdevice instances.
+	for (i = 0; i < me1600_versions[chip_idx].ao_chips; i++) {
+		subdevice =
+		    (me_subdevice_t *) me1600_ao_constructor(me1600_device->
+							     base.info.pci.
+							     reg_bases[2], i,
+							     ((me1600_versions
+							       [chip_idx].curr >
+							       i) ? 1 : 0),
+							     &me1600_device->
+							     config_regs_lock,
+							     &me1600_device->
+							     ao_shadows_lock,
+							     &me1600_device->
+							     ao_regs_shadows,
+							     me1600_workqueue);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me1600_device);
+			kfree(me1600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me1600_device->base.slist,
+					    subdevice);
+	}
+
+	// Overwrite base class methods.
+	me1600_device->base.me_device_destructor = me1600_destructor;
+
+	return (me_device_t *) me1600_device;
+}
+
+static void me1600_destructor(struct me_device *device)
+{
+	me1600_device_t *me1600_device = (me1600_device_t *) device;
+	PDEBUG("executed.\n");
+
+	// Destroy shadow instance.
+	kfree(me1600_device->ao_regs_shadows.registry);
+	kfree(me1600_device->ao_regs_shadows.shadow);
+	kfree(me1600_device->ao_regs_shadows.mirror);
+
+	me_device_deinit((me_device_t *) me1600_device);
+	kfree(me1600_device);
+}
+
+static void me1600_set_registry(me1600_device_t * subdevice, uint32_t reg_base)
+{				// Create shadow structure.
+	if (subdevice->ao_regs_shadows.count >= 1) {
+		subdevice->ao_regs_shadows.registry[0] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_0_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 2) {
+		subdevice->ao_regs_shadows.registry[1] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_1_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 3) {
+		subdevice->ao_regs_shadows.registry[2] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_2_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 4) {
+		subdevice->ao_regs_shadows.registry[3] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_3_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 5) {
+		subdevice->ao_regs_shadows.registry[4] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_4_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 6) {
+		subdevice->ao_regs_shadows.registry[5] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_5_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 7) {
+		subdevice->ao_regs_shadows.registry[6] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_6_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 8) {
+		subdevice->ao_regs_shadows.registry[7] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_7_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 9) {
+		subdevice->ao_regs_shadows.registry[8] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_8_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 10) {
+		subdevice->ao_regs_shadows.registry[9] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_9_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 11) {
+		subdevice->ao_regs_shadows.registry[10] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_10_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 12) {
+		subdevice->ao_regs_shadows.registry[11] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_11_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 13) {
+		subdevice->ao_regs_shadows.registry[12] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_12_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 14) {
+		subdevice->ao_regs_shadows.registry[13] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_13_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 15) {
+		subdevice->ao_regs_shadows.registry[14] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_14_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 16) {
+		subdevice->ao_regs_shadows.registry[15] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_15_REG);
+	}
+	if (subdevice->ao_regs_shadows.count > 16) {
+		PERROR("More than 16 outputs! (%d)\n",
+		       subdevice->ao_regs_shadows.count);
+	}
+}
+
+// Init and exit of module.
+
+static int __init me1600_init(void)
+{
+	PDEBUG("executed\n.");
+
+	me1600_workqueue = create_singlethread_workqueue("me1600");
+	return 0;
+}
+
+static void __exit me1600_exit(void)
+{
+	PDEBUG("executed\n.");
+
+	flush_workqueue(me1600_workqueue);
+	destroy_workqueue(me1600_workqueue);
+}
+
+module_init(me1600_init);
+module_exit(me1600_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for ME-1600 Device");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-1600 Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me1600_pci_constructor);
diff --git a/drivers/staging/meilhaus/me1600_device.h b/drivers/staging/meilhaus/me1600_device.h
new file mode 100644
index 000000000000..f7b231f73ac8
--- /dev/null
+++ b/drivers/staging/meilhaus/me1600_device.h
@@ -0,0 +1,101 @@
+/**
+ * @file me1600_device.h
+ *
+ * @brief ME-1600 device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1600_H
+#define _ME1600_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+#include "me1600_ao.h"
+#include "me1600_ao_reg.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure to store device capabilities.
+ */
+typedef struct me1600_version {
+	uint16_t device_id;				/**< The PCI device id of the device. */
+	unsigned int ao_chips;			/**< The number of analog outputs on the device. */
+	int curr;						/**< Flag to identify amounts of current output. */
+} me1600_version_t;
+
+/**
+  * @brief Defines for each ME-1600 device version its capabilities.
+ */
+static me1600_version_t me1600_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME1600_4U, 4, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME1600_8U, 8, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME1600_12U, 12, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME1600_16U, 16, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME1600_16U_8I, 16, 8},
+	{0}
+};
+
+/**< Returns the number of entries in #me1600_versions. */
+#define ME1600_DEVICE_VERSIONS (sizeof(me1600_versions) / sizeof(me1600_version_t) - 1)
+
+/**
+ * @brief Returns the index of the device entry in #me1600_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me1600_versions.
+ */
+static inline unsigned int me1600_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME1600_DEVICE_VERSIONS; i++)
+		if (me1600_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The ME-1600 device class structure.
+ */
+typedef struct me1600_device {
+	me_device_t base;						/**< The Meilhaus device base class. */
+	spinlock_t config_regs_lock;			/**< Protects the configuration registers. */
+
+	me1600_ao_shadow_t ao_regs_shadows;		/**< Addresses and shadows of output's registers. */
+	spinlock_t ao_shadows_lock;				/**< Protects the shadow's struct. */
+} me1600_device_t;
+
+/**
+ * @brief The ME-1600 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-1600 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me1600_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_ai.c b/drivers/staging/meilhaus/me4600_ai.c
new file mode 100644
index 000000000000..0a8c9d737e90
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ai.c
@@ -0,0 +1,3434 @@
+/**
+ * @file me4600_ai.c
+ *
+ * @brief ME-4000 analog input subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+# define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+#include "medebug.h"
+#include "meids.h"
+
+#include "me4600_reg.h"
+#include "me4600_ai_reg.h"
+#include "me4600_ai.h"
+
+/*
+ * Declarations (local)
+ */
+
+static void me4600_ai_destructor(struct me_subdevice *subdevice);
+static int me4600_ai_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags);
+
+static int me4600_ai_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags);
+
+static int me4600_ai_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags);
+
+static int me4600_ai_io_stream_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags);
+static int me4600_ai_io_stream_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int read_mode,
+				    int *values, int *count, int flags);
+static int me4600_ai_io_stream_new_values(me_subdevice_t * subdevice,
+					  struct file *filep,
+					  int time_out, int *count, int flags);
+static int inline me4600_ai_io_stream_read_get_value(me4600_ai_subdevice_t *
+						     instance, int *values,
+						     const int count,
+						     const int flags);
+
+static int me4600_ai_io_stream_start(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int start_mode, int time_out, int flags);
+static int me4600_ai_io_stream_stop(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int stop_mode, int flags);
+static int me4600_ai_io_stream_status(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int wait,
+				      int *status, int *values, int flags);
+
+static int me4600_ai_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range);
+static int me4600_ai_query_number_ranges(me_subdevice_t * subdevice,
+					 int unit, int *count);
+static int me4600_ai_query_range_info(me_subdevice_t * subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata);
+static int me4600_ai_query_timer(me_subdevice_t * subdevice,
+				 int timer,
+				 int *base_frequency,
+				 long long *min_ticks, long long *max_ticks);
+static int me4600_ai_query_number_channels(me_subdevice_t * subdevice,
+					   int *number);
+static int me4600_ai_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype);
+static int me4600_ai_query_subdevice_caps(me_subdevice_t * subdevice,
+					  int *caps);
+static int me4600_ai_query_subdevice_caps_args(struct me_subdevice *subdevice,
+					       int cap, int *args, int count);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me4600_ai_isr(int irq, void *dev_id);
+#else
+static irqreturn_t me4600_ai_isr(int irq, void *dev_id, struct pt_regs *regs);
+#endif
+
+static int ai_mux_toggler(me4600_ai_subdevice_t * subdevice);
+
+/** Immidiate stop.
+* Reset all IRQ's sources. (block laches)
+* Preserve FIFO
+*/
+static int ai_stop_immediately(me4600_ai_subdevice_t * instance);
+
+/** Immidiate stop.
+* Reset all IRQ's sources. (block laches)
+* Reset data FIFO
+*/
+void inline ai_stop_isr(me4600_ai_subdevice_t * instance);
+
+/** Interrupt logics.
+* Read datas
+* Reset latches
+*/
+void ai_limited_isr(me4600_ai_subdevice_t * instance, const uint32_t irq_status,
+		    const uint32_t ctrl_status);
+void ai_infinite_isr(me4600_ai_subdevice_t * instance,
+		     const uint32_t irq_status, const uint32_t ctrl_status);
+
+/** Last chunck of datas. We must reschedule sample counter.
+* Leaving SC_RELOAD doesn't do any harm, but in some bad case can make extra interrupts.
+* When threshold is wrongly set some IRQ are lost.(!!!)
+*/
+void inline ai_reschedule_SC(me4600_ai_subdevice_t * instance);
+
+/** Read datas from FIFO and copy them to buffer */
+static int inline ai_read_data(me4600_ai_subdevice_t * instance,
+			       const int count);
+
+/** Copy rest of data from fifo to circular buffer.*/
+static int inline ai_read_data_pooling(me4600_ai_subdevice_t * instance);
+
+/** Set ISM to next state for infinite data aqusation mode*/
+void inline ai_infinite_ISM(me4600_ai_subdevice_t * instance);
+
+/** Set ISM to next state for define amount of data aqusation mode*/
+void inline ai_limited_ISM(me4600_ai_subdevice_t * instance,
+			   uint32_t irq_status);
+
+/** Set ISM to next stage for limited mode */
+void inline ai_data_acquisition_logic(me4600_ai_subdevice_t * instance);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void me4600_ai_work_control_task(void *subdevice);
+#else
+static void me4600_ai_work_control_task(struct work_struct *work);
+#endif
+
+/* Definitions
+ */
+
+me4600_ai_subdevice_t *me4600_ai_constructor(uint32_t reg_base,
+					     unsigned int channels,
+					     unsigned int ranges,
+					     int isolated,
+					     int sh,
+					     int irq,
+					     spinlock_t * ctrl_reg_lock,
+					     struct workqueue_struct *me4600_wq)
+{
+	me4600_ai_subdevice_t *subdevice;
+	int err;
+	unsigned int i;
+
+	PDEBUG("executed. idx=0\n");
+
+	// Allocate memory for subdevice instance.
+	subdevice = kmalloc(sizeof(me4600_ai_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me4600_ai_subdevice_t));
+
+	// Initialize subdevice base class.
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	// Initialize circular buffer.
+	subdevice->circ_buf.mask = ME4600_AI_CIRC_BUF_COUNT - 1;
+
+	subdevice->circ_buf.buf =
+	    (void *)__get_free_pages(GFP_KERNEL, ME4600_AI_CIRC_BUF_SIZE_ORDER);
+	PDEBUG("circ_buf = %p size=%ld\n", subdevice->circ_buf.buf,
+	       ME4600_AI_CIRC_BUF_SIZE);
+
+	if (!subdevice->circ_buf.buf) {
+		PERROR("Cannot get circular buffer.\n");
+		me_subdevice_deinit((me_subdevice_t *) subdevice);
+		kfree(subdevice);
+		return NULL;
+	}
+
+	memset(subdevice->circ_buf.buf, 0, ME4600_AI_CIRC_BUF_SIZE);
+	subdevice->circ_buf.head = 0;
+	subdevice->circ_buf.tail = 0;
+	subdevice->status = ai_status_none;
+
+	// Initialize wait queue.
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	// Save the number of channels.
+	subdevice->channels = channels;
+
+	/* Initialize the single config entries to reset values */
+	for (i = 0; i < channels; i++) {
+		subdevice->single_config[i].status = ME_SINGLE_CHANNEL_NOT_CONFIGURED;	//not configured
+	}
+
+	// Save if isolated device.
+	subdevice->isolated = isolated;
+
+	// Save if sample and hold is available.
+	subdevice->sh = sh;
+
+	// Set stream config to not configured state.
+	subdevice->fifo_irq_threshold = 0;
+	subdevice->data_required = 0;
+	subdevice->chan_list_len = 0;
+
+	// Initialize registers addresses.
+	subdevice->ctrl_reg = reg_base + ME4600_AI_CTRL_REG;
+	subdevice->status_reg = reg_base + ME4600_AI_STATUS_REG;
+	subdevice->channel_list_reg = reg_base + ME4600_AI_CHANNEL_LIST_REG;
+	subdevice->data_reg = reg_base + ME4600_AI_DATA_REG;
+	subdevice->chan_timer_reg = reg_base + ME4600_AI_CHAN_TIMER_REG;
+	subdevice->chan_pre_timer_reg = reg_base + ME4600_AI_CHAN_PRE_TIMER_REG;
+	subdevice->scan_timer_low_reg = reg_base + ME4600_AI_SCAN_TIMER_LOW_REG;
+	subdevice->scan_timer_high_reg =
+	    reg_base + ME4600_AI_SCAN_TIMER_HIGH_REG;
+	subdevice->scan_pre_timer_low_reg =
+	    reg_base + ME4600_AI_SCAN_PRE_TIMER_LOW_REG;
+	subdevice->scan_pre_timer_high_reg =
+	    reg_base + ME4600_AI_SCAN_PRE_TIMER_HIGH_REG;
+	subdevice->start_reg = reg_base + ME4600_AI_START_REG;
+	subdevice->irq_status_reg = reg_base + ME4600_IRQ_STATUS_REG;
+	subdevice->sample_counter_reg = reg_base + ME4600_AI_SAMPLE_COUNTER_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	// Initialize ranges.
+	subdevice->ranges_len = ranges;
+	subdevice->ranges[0].min = -10E6;
+	subdevice->ranges[0].max = 9999694;
+
+	subdevice->ranges[1].min = 0;
+	subdevice->ranges[1].max = 9999847;
+
+	subdevice->ranges[2].min = -25E5;
+	subdevice->ranges[2].max = 2499923;
+
+	subdevice->ranges[3].min = 0;
+	subdevice->ranges[3].max = 2499961;
+
+	// We have to switch the mux in order to get it work correctly.
+	ai_mux_toggler(subdevice);
+
+	// Register interrupt service routine.
+	subdevice->irq = irq;
+	if (request_irq(subdevice->irq, me4600_ai_isr,
+#ifdef IRQF_DISABLED
+			IRQF_DISABLED | IRQF_SHARED,
+#else
+			SA_INTERRUPT | SA_SHIRQ,
+#endif
+			ME4600_NAME, subdevice)) {
+		PERROR("Cannot register interrupt service routine.\n");
+		me_subdevice_deinit((me_subdevice_t *) subdevice);
+		free_pages((unsigned long)subdevice->circ_buf.buf,
+			   ME4600_AI_CIRC_BUF_SIZE_ORDER);
+		subdevice->circ_buf.buf = NULL;
+		kfree(subdevice);
+		return NULL;
+	}
+	PINFO("Registered irq=%d.\n", subdevice->irq);
+
+	// Override base class methods.
+	subdevice->base.me_subdevice_destructor = me4600_ai_destructor;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me4600_ai_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me4600_ai_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me4600_ai_io_single_read;
+	subdevice->base.me_subdevice_io_stream_config =
+	    me4600_ai_io_stream_config;
+	subdevice->base.me_subdevice_io_stream_new_values =
+	    me4600_ai_io_stream_new_values;
+	subdevice->base.me_subdevice_io_stream_read = me4600_ai_io_stream_read;
+	subdevice->base.me_subdevice_io_stream_start =
+	    me4600_ai_io_stream_start;
+	subdevice->base.me_subdevice_io_stream_status =
+	    me4600_ai_io_stream_status;
+	subdevice->base.me_subdevice_io_stream_stop = me4600_ai_io_stream_stop;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me4600_ai_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me4600_ai_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me4600_ai_query_subdevice_caps;
+	subdevice->base.me_subdevice_query_subdevice_caps_args =
+	    me4600_ai_query_subdevice_caps_args;
+	subdevice->base.me_subdevice_query_range_by_min_max =
+	    me4600_ai_query_range_by_min_max;
+	subdevice->base.me_subdevice_query_number_ranges =
+	    me4600_ai_query_number_ranges;
+	subdevice->base.me_subdevice_query_range_info =
+	    me4600_ai_query_range_info;
+	subdevice->base.me_subdevice_query_timer = me4600_ai_query_timer;
+
+	// Prepare work queue.
+	subdevice->me4600_workqueue = me4600_wq;
+
+/* workqueue API changed in kernel 2.6.20 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20) )
+	INIT_WORK(&subdevice->ai_control_task, me4600_ai_work_control_task,
+		  (void *)subdevice);
+#else
+	INIT_DELAYED_WORK(&subdevice->ai_control_task,
+			  me4600_ai_work_control_task);
+#endif
+
+	return subdevice;
+}
+
+static void me4600_ai_destructor(struct me_subdevice *subdevice)
+{
+	me4600_ai_subdevice_t *instance;
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance->ai_control_task_flag = 0;
+	// Reset subdevice to asure clean exit.
+	me4600_ai_io_reset_subdevice(subdevice, NULL,
+				     ME_IO_RESET_SUBDEVICE_NO_FLAGS);
+
+	// Remove any tasks from work queue. This is paranoic because it was done allready in reset().
+	if (!cancel_delayed_work(&instance->ai_control_task)) {	//Wait 2 ticks to be sure that control task is removed from queue.
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(2);
+	}
+
+	free_irq(instance->irq, instance);
+	free_pages((unsigned long)instance->circ_buf.buf,
+		   ME4600_AI_CIRC_BUF_SIZE_ORDER);
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+static int me4600_ai_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	volatile uint32_t ctrl;
+	unsigned long status;
+	const int timeout = HZ / 10;	//100ms
+	int i;
+
+	PDEBUG("executed. idx=0\n");
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	instance->ai_control_task_flag = 0;
+	instance->status = ai_status_none;
+
+	for (i = 0; i <= timeout; i++) {
+		spin_lock_irqsave(instance->ctrl_reg_lock, status);
+		ctrl = inl(instance->ctrl_reg);
+		//Stop DMA
+		ctrl &= ~ME4600_AI_CTRL_RPCI_FIFO;
+		// Stop all actions. No conditions!
+		ctrl &= ~ME4600_AI_CTRL_BIT_STOP;
+		ctrl |= ME4600_AI_CTRL_BIT_IMMEDIATE_STOP;
+
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		spin_unlock_irqrestore(instance->ctrl_reg_lock, status);
+
+		if (!(inl(instance->status_reg) & ME4600_AI_STATUS_BIT_FSM))
+			break;
+
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(1);
+	}
+
+	if (i > timeout) {
+		PERROR("FSM is still busy.\n");
+		ME_SUBDEVICE_EXIT;
+		return ME_ERRNO_INTERNAL;
+	}
+
+	spin_lock_irqsave(instance->ctrl_reg_lock, status);
+	ctrl = inl(instance->ctrl_reg);
+	// Clear all features. Dissable interrupts.
+	ctrl &= ~(ME4600_AI_CTRL_BIT_STOP
+		  | ME4600_AI_CTRL_BIT_LE_IRQ
+		  | ME4600_AI_CTRL_BIT_HF_IRQ | ME4600_AI_CTRL_BIT_SC_IRQ);
+	ctrl |= (ME4600_AI_CTRL_BIT_IMMEDIATE_STOP
+		 | ME4600_AI_CTRL_BIT_LE_IRQ_RESET
+		 | ME4600_AI_CTRL_BIT_HF_IRQ_RESET
+		 | ME4600_AI_CTRL_BIT_SC_IRQ_RESET);
+
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock_irqrestore(instance->ctrl_reg_lock, status);
+
+	outl(ME4600_AI_MIN_CHAN_TICKS - 1, instance->chan_timer_reg);
+	PDEBUG_REG("chan_timer_reg outl(0x%lX+0x%lX)=0x%llx\n",
+		   instance->reg_base,
+		   instance->chan_timer_reg - instance->reg_base,
+		   ME4600_AI_MIN_CHAN_TICKS);
+	outl(ME4600_AI_MIN_ACQ_TICKS - 1, instance->chan_pre_timer_reg);
+	PDEBUG_REG("chan_pre_timer_reg outl(0x%lX+0x%lX)=0x%llx\n",
+		   instance->reg_base,
+		   instance->chan_pre_timer_reg - instance->reg_base,
+		   ME4600_AI_MIN_ACQ_TICKS);
+	outl(0, instance->scan_timer_low_reg);
+	PDEBUG_REG("scan_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_timer_low_reg - instance->reg_base, 0);
+	outl(0, instance->scan_timer_high_reg);
+	PDEBUG_REG("scan_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_timer_high_reg - instance->reg_base, 0);
+	outl(0, instance->scan_pre_timer_low_reg);
+	PDEBUG_REG("scan_pre_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_low_reg - instance->reg_base, 0);
+	outl(0, instance->scan_pre_timer_high_reg);
+	PDEBUG_REG("scan_pre_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_high_reg - instance->reg_base, 0);
+	outl(0xEFFFFFFF, instance->sample_counter_reg);
+	PDEBUG_REG("sample_counter_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->sample_counter_reg - instance->reg_base,
+		   0xEFFFFFFF);
+
+	instance->circ_buf.head = 0;
+	instance->circ_buf.tail = 0;
+
+	instance->fifo_irq_threshold = 0;
+	instance->data_required = 0;
+	instance->chan_list_len = 0;
+
+	// Initialize the single config entries to reset values.
+	for (i = 0; i < instance->channels; i++) {
+		instance->single_config[i].status =
+		    ME_SINGLE_CHANNEL_NOT_CONFIGURED;
+	}
+	instance->status = ai_status_none;
+
+	//Signal reset if user is on wait.
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ai_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags;
+	int i;
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=0\n");
+
+	if (flags & ~ME_IO_SINGLE_CONFIG_CONTINUE) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	switch (trig_type) {
+	case ME_TRIG_TYPE_SW:
+		if (trig_edge != ME_TRIG_EDGE_NONE) {
+			PERROR
+			    ("Invalid trigger edge. Software trigger has not edge.\n");
+			return ME_ERRNO_INVALID_TRIG_EDGE;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		if (instance->channels <= 16)	//Only versions with 32 channels have analog trigger (4670 and 4680)
+		{
+			PERROR("Invalid trigger type specified.\n");
+			return ME_ERRNO_INVALID_TRIG_TYPE;
+		}
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		if ((trig_edge != ME_TRIG_EDGE_ANY)
+		    && (trig_edge != ME_TRIG_EDGE_RISING)
+		    && (trig_edge != ME_TRIG_EDGE_FALLING)) {
+			PERROR("Invalid trigger edge specified.\n");
+			return ME_ERRNO_INVALID_TRIG_EDGE;
+		}
+		break;
+
+	default:
+		PERROR("Invalid trigger type specified.\n");
+		return ME_ERRNO_INVALID_TRIG_TYPE;
+	}
+
+	if (trig_chan != ME_TRIG_CHAN_DEFAULT) {
+		PERROR("Invalid trigger channel specified.\n");
+		return ME_ERRNO_INVALID_TRIG_CHAN;
+	}
+
+	if ((single_config < 0) || (single_config >= instance->ranges_len)) {
+		PERROR("Invalid single config specified.\n");
+		return ME_ERRNO_INVALID_SINGLE_CONFIG;
+	}
+
+	if ((ref != ME_REF_AI_GROUND) && (ref != ME_REF_AI_DIFFERENTIAL)) {
+		PERROR("Invalid analog reference specified.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if ((single_config % 2) && (ref != ME_REF_AI_GROUND)) {
+		PERROR("Invalid analog reference specified.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if ((ref == ME_REF_AI_DIFFERENTIAL)
+	    && ((instance->channels == 16) || (channel >= 16))) {
+		PERROR("Invalid analog reference specified.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if (channel < 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (channel >= instance->channels) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	//Prepare data entry.
+	// Common for all modes.
+	instance->single_config[channel].entry =
+	    channel | ME4600_AI_LIST_LAST_ENTRY;
+
+	if (ref == ME_REF_AI_DIFFERENTIAL) {	// ME_REF_AI_DIFFERENTIAL
+		instance->single_config[channel].entry |=
+		    ME4600_AI_LIST_INPUT_DIFFERENTIAL;
+	}
+/*
+		// ME4600_AI_LIST_INPUT_SINGLE_ENDED = 0x0000
+		// 'entry |= ME4600_AI_LIST_INPUT_SINGLE_ENDED' <== Do nothing. Removed.
+		else
+		{// ME_REF_AI_GROUND
+			instance->single_config[channel].entry |= ME4600_AI_LIST_INPUT_SINGLE_ENDED;
+		}
+*/
+	switch (single_config) {
+	case 0:		//-10V..10V
+/*
+					// ME4600_AI_LIST_RANGE_BIPOLAR_10 = 0x0000
+					// 'entry |= ME4600_AI_LIST_RANGE_BIPOLAR_10' <== Do nothing. Removed.
+					instance->single_config[channel].entry |= ME4600_AI_LIST_RANGE_BIPOLAR_10;
+*/ break;
+
+	case 1:		//0V..10V
+		instance->single_config[channel].entry |=
+		    ME4600_AI_LIST_RANGE_UNIPOLAR_10;
+		break;
+
+	case 2:		//-2.5V..2.5V
+		instance->single_config[channel].entry |=
+		    ME4600_AI_LIST_RANGE_BIPOLAR_2_5;
+		break;
+
+	case 3:		//0V..2.5V
+		instance->single_config[channel].entry |=
+		    ME4600_AI_LIST_RANGE_UNIPOLAR_2_5;
+		break;
+	}
+
+	// Prepare control register.
+	// Common for all modes.
+	instance->single_config[channel].ctrl =
+	    ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO;
+
+	switch (trig_type) {
+	case ME_TRIG_TYPE_SW:
+		// Nothing to set.
+		break;
+
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		instance->single_config[channel].ctrl |=
+		    ME4600_AI_CTRL_BIT_EX_TRIG_ANALOG;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		instance->single_config[channel].ctrl |=
+		    ME4600_AI_CTRL_BIT_EX_TRIG;
+		break;
+	}
+
+	switch (trig_edge) {
+	case ME_TRIG_EDGE_RISING:
+		// Nothing to set.
+		break;
+
+	case ME_TRIG_EDGE_ANY:
+		instance->single_config[channel].ctrl |=
+		    ME4600_AI_CTRL_BIT_EX_TRIG_BOTH;
+
+	case ME_TRIG_EDGE_FALLING:
+		instance->single_config[channel].ctrl |=
+		    ME4600_AI_CTRL_BIT_EX_TRIG_FALLING;
+		break;
+	}
+
+	// Enable this channel
+	instance->single_config[channel].status = ME_SINGLE_CHANNEL_CONFIGURED;
+
+	// Copy this settings to other outputs.
+	if (flags == ME_IO_SINGLE_CONFIG_CONTINUE) {
+		for (i = channel + 1; i < instance->channels; i++) {
+			instance->single_config[i].ctrl =
+			    instance->single_config[channel].ctrl;
+			instance->single_config[i].entry =
+			    instance->single_config[channel].entry;
+			instance->single_config[i].status =
+			    ME_SINGLE_CHANNEL_CONFIGURED;
+		}
+	}
+
+	instance->status = ai_status_single_configured;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ai_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	volatile uint32_t tmp;
+	volatile uint32_t val;
+	unsigned long cpu_flags;
+	int err = ME_ERRNO_SUCCESS;
+
+	unsigned long j;
+	unsigned long delay = 0;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (instance->status != ai_status_single_configured) {
+		PERROR("Subdevice not configured to work in single mode!\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	if ((channel > instance->channels) || (channel < 0)) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (instance->single_config[channel].status !=
+	    ME_SINGLE_CHANNEL_CONFIGURED) {
+		PERROR("Channel is not configured to work in single mode!\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	if (inl(instance->status_reg) & ME4600_AI_STATUS_BIT_FSM) {
+		PERROR("Subdevice is busy.\n");
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	// Cancel control task
+	PDEBUG("Cancel control task.\n");
+	instance->ai_control_task_flag = 0;
+	cancel_delayed_work(&instance->ai_control_task);
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+
+		if (delay == 0)
+			delay = 1;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	// Mark that StreamConfig is removed.
+	instance->chan_list_len = 0;
+
+	spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+	/// @note Imprtant: Preserve EXT IRQ settings.
+	tmp = inl(instance->ctrl_reg);
+	// Clear FIFOs and dissable interrupts
+	tmp &=
+	    ~(ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO);
+
+	tmp &=
+	    ~(ME4600_AI_CTRL_BIT_SC_IRQ | ME4600_AI_CTRL_BIT_HF_IRQ |
+	      ME4600_AI_CTRL_BIT_LE_IRQ);
+	tmp |=
+	    ME4600_AI_CTRL_BIT_SC_IRQ_RESET | ME4600_AI_CTRL_BIT_HF_IRQ_RESET |
+	    ME4600_AI_CTRL_BIT_LE_IRQ_RESET;
+
+	tmp |= ME4600_AI_CTRL_BIT_IMMEDIATE_STOP;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	outl(0, instance->scan_pre_timer_low_reg);
+	PDEBUG_REG("scan_pre_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_low_reg - instance->reg_base, 0);
+	outl(0, instance->scan_pre_timer_high_reg);
+	PDEBUG_REG("scan_pre_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_high_reg - instance->reg_base, 0);
+	outl(0, instance->scan_timer_low_reg);
+	PDEBUG_REG("scan_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_timer_low_reg - instance->reg_base, 0);
+	outl(0, instance->scan_timer_high_reg);
+	PDEBUG_REG("scan_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_timer_high_reg - instance->reg_base, 0);
+	outl(65, instance->chan_timer_reg);
+	PDEBUG_REG("chan_timer_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->chan_timer_reg - instance->reg_base, 65);
+	outl(65, instance->chan_pre_timer_reg);
+	PDEBUG_REG("chan_pre_timer_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->chan_pre_timer_reg - instance->reg_base, 65);
+
+	//Reactive FIFOs. Enable work.
+	tmp |= ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	outl(instance->single_config[channel].entry,
+	     instance->channel_list_reg);
+	PDEBUG_REG("channel_list_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->channel_list_reg - instance->reg_base,
+		   instance->single_config[channel].entry);
+
+	// Preserve EXT IRQ settings.
+	tmp &= (ME4600_AI_CTRL_BIT_EX_IRQ | ME4600_AI_CTRL_BIT_EX_IRQ_RESET);
+	outl(instance->single_config[channel].ctrl | tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base,
+		   instance->single_config[channel].ctrl | tmp);
+
+	spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+
+	if (!(instance->single_config[channel].ctrl & ME4600_AI_CTRL_BIT_EX_TRIG)) {	// Software start
+		inl(instance->start_reg);
+		PDEBUG_REG("start_reg inl(0x%lX+0x%lX)\n", instance->reg_base,
+			   instance->start_reg - instance->reg_base);
+
+		delay = 2;
+	}
+
+	j = jiffies;
+
+	while (!(inl(instance->status_reg) & ME4600_AI_STATUS_BIT_EF_DATA)) {
+		if (delay && ((jiffies - j) >= delay)) {
+			if (!(instance->single_config[channel].ctrl & ME4600_AI_CTRL_BIT_EX_TRIG)) {	// Software start.
+				PERROR("Value not available after wait.\n");
+				err = ME_ERRNO_INTERNAL;
+			} else {	// External start.
+				PERROR("Timeout reached.\n");
+				err = ME_ERRNO_TIMEOUT;
+			}
+			break;
+		}
+		// Wait
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(1);
+
+		if (signal_pending(current)) {
+			PERROR
+			    ("Wait on external trigger interrupted by signal.\n");
+			err = ME_ERRNO_SIGNAL;
+			break;
+		}
+
+		if (instance->status != ai_status_single_configured) {
+			PERROR("Wait interrupted by reset.\n");
+			err = ME_ERRNO_CANCELLED;
+			break;
+		}
+	}
+
+	// Read value.
+	if (!err) {
+		val = inl(instance->data_reg) ^ 0x8000;
+		PDEBUG_REG("data_reg inl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->data_reg - instance->reg_base, val);
+		*value = val & ME4600_AI_MAX_DATA;
+	} else {
+		*value = 0xFFFFFFFF;
+	}
+
+	// Restore settings.
+	spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+	tmp = inl(instance->ctrl_reg);
+	// Clear FIFOs and dissable interrupts.
+	tmp &=
+	    ~(ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO);
+	tmp |= ME4600_AI_CTRL_BIT_SC_IRQ | ME4600_AI_CTRL_BIT_HF_IRQ;
+	tmp |=
+	    ME4600_AI_CTRL_BIT_SC_IRQ_RESET | ME4600_AI_CTRL_BIT_HF_IRQ_RESET |
+	    ME4600_AI_CTRL_BIT_LE_IRQ_RESET | ME4600_AI_CTRL_BIT_IMMEDIATE_STOP;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ai_io_stream_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	int i;			// internal multipurpose variable
+	unsigned long long data_required;
+
+	volatile uint32_t entry;
+	volatile uint32_t ctrl = ME4600_AI_CTRL_BIT_IMMEDIATE_STOP;
+	volatile uint32_t tmp;	// use when current copy of register's value needed
+	unsigned long cpu_flags;
+
+	uint64_t acq_ticks;
+	uint64_t scan_ticks;
+	uint64_t conv_ticks;
+	unsigned int acq_start_ticks_low = trigger->iAcqStartTicksLow;
+	unsigned int acq_start_ticks_high = trigger->iAcqStartTicksHigh;
+	unsigned int scan_start_ticks_low = trigger->iScanStartTicksLow;
+	unsigned int scan_start_ticks_high = trigger->iScanStartTicksHigh;
+	unsigned int conv_start_ticks_low = trigger->iConvStartTicksLow;
+	unsigned int conv_start_ticks_high = trigger->iConvStartTicksHigh;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER
+	    // Convert ticks to 64 bit long values
+	    acq_ticks =
+	    (uint64_t) acq_start_ticks_low +
+	    ((uint64_t) acq_start_ticks_high << 32);
+	scan_ticks =
+	    (uint64_t) scan_start_ticks_low +
+	    ((uint64_t) scan_start_ticks_high << 32);
+	conv_ticks =
+	    (uint64_t) conv_start_ticks_low +
+	    ((uint64_t) conv_start_ticks_high << 32);
+
+	// Check settings - begin
+	switch (trigger->iAcqStartTrigType) {
+	case ME_TRIG_TYPE_SW:
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		break;
+
+	default:
+		PERROR("Invalid acquisition start trigger type specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_START_TRIG_TYPE;
+		goto ERROR;
+		break;
+	}
+
+	if ((trigger->iAcqStartTrigType == ME_TRIG_TYPE_SW)
+	    && (trigger->iAcqStartTrigEdge != ME_TRIG_EDGE_NONE)) {
+		PERROR("Invalid acquisition start trigger edge specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+		goto ERROR;
+	}
+
+	if (trigger->iAcqStartTrigType != ME_TRIG_TYPE_SW) {
+		switch (trigger->iAcqStartTrigEdge) {
+		case ME_TRIG_EDGE_RISING:
+		case ME_TRIG_EDGE_FALLING:
+		case ME_TRIG_EDGE_ANY:
+			break;
+
+		default:
+			PERROR
+			    ("Invalid acquisition start trigger edge specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+			goto ERROR;
+			break;
+		}
+	}
+
+	if (trigger->iAcqStartTrigChan != ME_TRIG_CHAN_DEFAULT) {
+		PERROR
+		    ("Invalid acquisition start trigger channel specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_START_TRIG_CHAN;
+		goto ERROR;
+	}
+
+	if ((acq_ticks < ME4600_AI_MIN_ACQ_TICKS)
+	    || (acq_ticks > ME4600_AI_MAX_ACQ_TICKS)) {
+		PERROR
+		    ("Invalid acquisition start trigger argument specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_START_ARG;
+		goto ERROR;
+	}
+
+	switch (trigger->iScanStartTrigType) {
+
+	case ME_TRIG_TYPE_TIMER:
+		if ((scan_ticks < ME4600_AI_MIN_SCAN_TICKS)
+		    || (scan_ticks > ME4600_AI_MAX_SCAN_TICKS)
+		    || (scan_ticks < count * conv_ticks)
+		    ) {
+			PERROR("Invalid scan start argument specified.\n");
+			err = ME_ERRNO_INVALID_SCAN_START_ARG;
+			goto ERROR;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		if (trigger->iAcqStartTrigType != ME_TRIG_TYPE_EXT_DIGITAL) {
+			PERROR
+			    ("Invalid scan start trigger type specified (Acq is HW digital)\n");
+			err = ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE;
+			goto ERROR;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		if (trigger->iAcqStartTrigType != ME_TRIG_TYPE_EXT_ANALOG) {
+			PERROR
+			    ("Invalid scan start trigger type specified (Acq is HW analog)\n");
+			err = ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE;
+			goto ERROR;
+		}
+		break;
+
+	case ME_TRIG_TYPE_FOLLOW:
+		break;
+
+	default:
+		PERROR("Invalid scan start trigger type specified.\n");
+		err = ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE;
+		goto ERROR;
+		break;
+	}
+
+	switch (trigger->iConvStartTrigType) {
+
+	case ME_TRIG_TYPE_TIMER:
+		if ((conv_ticks < ME4600_AI_MIN_CHAN_TICKS)
+		    || (conv_ticks > ME4600_AI_MAX_CHAN_TICKS)) {
+			PERROR
+			    ("Invalid conv start trigger argument specified.\n");
+			err = ME_ERRNO_INVALID_CONV_START_ARG;
+			goto ERROR;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		if ((trigger->iScanStartTrigType != ME_TRIG_TYPE_FOLLOW)
+		    || (trigger->iAcqStartTrigType !=
+			ME_TRIG_TYPE_EXT_DIGITAL)) {
+			PERROR("Invalid conv start trigger type specified.\n");
+			err = ME_ERRNO_INVALID_CONV_START_TRIG_TYPE;
+			goto ERROR;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		if ((trigger->iScanStartTrigType != ME_TRIG_TYPE_FOLLOW)
+		    || (trigger->iAcqStartTrigType !=
+			ME_TRIG_TYPE_EXT_ANALOG)) {
+			PERROR("Invalid conv start trigger type specified.\n");
+			err = ME_ERRNO_INVALID_CONV_START_TRIG_TYPE;
+			goto ERROR;
+		}
+		break;
+
+	default:
+		PERROR("Invalid conv start trigger type specified.\n");
+		err = ME_ERRNO_INVALID_CONV_START_TRIG_TYPE;
+		goto ERROR;
+
+		break;
+	}
+/**
+* Aceptable settings:
+* iScanStopTrigType		:	iAcqStopTrigType
+*
+* ME_TRIG_TYPE_NONE		:	ME_TRIG_TYPE_NONE	-> infinite count with manual stop
+* ME_TRIG_TYPE_NONE		:	ME_TRIG_TYPE_COUNT	-> stop after getting iScanStopCount list of values (iScanStopCount * count)
+* ME_TRIG_TYPE_COUNT	:	ME_TRIG_TYPE_FOLLOW	-> stop after getting iAcqStopCount values (it can stops in midle of the list)
+*/
+	switch (trigger->iScanStopTrigType) {
+
+	case ME_TRIG_TYPE_NONE:
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (trigger->iScanStopCount <= 0) {
+			PERROR("Invalid scan stop argument specified.\n");
+			err = ME_ERRNO_INVALID_SCAN_STOP_ARG;
+			goto ERROR;
+		}
+		break;
+
+	default:
+		PERROR("Invalid scan stop trigger type specified.\n");
+		err = ME_ERRNO_INVALID_SCAN_STOP_TRIG_TYPE;
+		goto ERROR;
+		break;
+	}
+
+	switch (trigger->iAcqStopTrigType) {
+
+	case ME_TRIG_TYPE_NONE:
+		if (trigger->iScanStopTrigType != ME_TRIG_TYPE_NONE) {
+			PERROR("Invalid acq stop trigger type specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+			goto ERROR;
+		}
+		break;
+
+	case ME_TRIG_TYPE_FOLLOW:
+		if (trigger->iScanStopTrigType != ME_TRIG_TYPE_COUNT) {
+			PERROR("Invalid acq stop trigger type specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+			goto ERROR;
+		}
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (trigger->iScanStopTrigType != ME_TRIG_TYPE_NONE) {
+			PERROR("Invalid acq stop trigger type specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+			goto ERROR;
+		}
+
+		if (trigger->iAcqStopCount <= 0) {
+			PERROR
+			    ("Invalid acquisition or scan stop argument specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_STOP_ARG;
+			goto ERROR;
+		}
+		break;
+
+	default:
+		PERROR("Invalid acq stop trigger type specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+		goto ERROR;
+		break;
+	}
+
+	if ((count <= 0) || (count > ME4600_AI_LIST_COUNT)) {
+		PERROR("Invalid channel list count specified.\n");
+		err = ME_ERRNO_INVALID_CONFIG_LIST_COUNT;
+		goto ERROR;
+	}
+///This is general limitation
+//      if (fifo_irq_threshold < 0 || fifo_irq_threshold >= ME4600_AI_CIRC_BUF_COUNT)
+///This is limitation from Windows. I use it for compatibility.
+	if (fifo_irq_threshold < 0
+	    || fifo_irq_threshold >= ME4600_AI_FIFO_COUNT) {
+		PERROR("Invalid fifo irq threshold specified.\n");
+		err = ME_ERRNO_INVALID_FIFO_IRQ_THRESHOLD;
+		goto ERROR;
+	}
+
+	if ((config_list[0].iRef == ME_REF_AI_DIFFERENTIAL)
+	    && (instance->channels == 16)) {
+		PERROR
+		    ("Differential reference is not available on this subdevice.\n");
+		err = ME_ERRNO_INVALID_REF;
+		goto ERROR;
+	}
+
+	if (flags & ME_IO_STREAM_CONFIG_SAMPLE_AND_HOLD) {
+		if (!instance->sh) {
+			PERROR
+			    ("Sample and hold is not available for this board.\n");
+			err = ME_ERRNO_INVALID_FLAGS;
+			goto ERROR;
+		}
+		if (config_list[0].iRef == ME_REF_AI_DIFFERENTIAL) {
+			PERROR
+			    ("Sample and hold is not available in differential mode.\n");
+			err = ME_ERRNO_INVALID_FLAGS;
+			goto ERROR;
+		}
+	}
+
+	for (i = 0; i < count; i++) {
+		if ((config_list[i].iStreamConfig < 0)
+		    || (config_list[i].iStreamConfig >= instance->ranges_len)) {
+			PERROR("Invalid stream config specified.\n");
+			err = ME_ERRNO_INVALID_STREAM_CONFIG;
+			goto ERROR;
+		}
+
+		if ((config_list[i].iRef != ME_REF_AI_GROUND)
+		    && (config_list[i].iRef != ME_REF_AI_DIFFERENTIAL)) {
+			PERROR("Invalid references in the list. Ref=0x%x\n",
+			       config_list[i].iRef);
+			err = ME_ERRNO_INVALID_REF;
+			goto ERROR;
+		}
+
+		if (config_list[i].iStreamConfig % 2) {	// StreamConfig: 1 or 3
+			if (config_list[i].iRef == ME_REF_AI_DIFFERENTIAL) {
+				PERROR
+				    ("Only bipolar modes support differential measurement.\n");
+				err = ME_ERRNO_INVALID_REF;
+				goto ERROR;
+			}
+		}
+
+		if (config_list[i].iRef != config_list[0].iRef) {
+			PERROR
+			    ("Not all references in the configuration list are equal. Ref[0]=0x%x Ref[%d]=0x%x\n",
+			     config_list[0].iRef, i, config_list[i].iRef);
+			err = ME_ERRNO_INVALID_REF;
+			goto ERROR;
+		}
+
+		if ((config_list[i].iRef == ME_REF_AI_DIFFERENTIAL)
+		    && (config_list[i].iChannel >= 16)) {
+			PERROR("Channel not available in differential mode.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+			goto ERROR;
+		}
+
+		if ((config_list[i].iChannel < 0)
+		    || (config_list[i].iChannel >= instance->channels)) {
+			PERROR("Invalid channel number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+			goto ERROR;
+		}
+	}
+
+	// Check settings - end
+
+	//Cancel control task
+	PDEBUG("Cancel control task.\n");
+	instance->ai_control_task_flag = 0;
+	cancel_delayed_work(&instance->ai_control_task);
+
+	// Work around from Keith Hartley - begin
+	if (trigger->iScanStartTrigType == ME_TRIG_TYPE_TIMER) {
+		if (count == 1) {
+			// The hardware does not work properly with a non-zero scan time
+			// if there is only ONE channel in the channel list. In this case
+			// we must set the scan time to zero and use the channel time.
+
+			conv_ticks = scan_ticks;
+			trigger->iScanStartTrigType = ME_TRIG_TYPE_FOLLOW;
+		} else if (scan_ticks == count * conv_ticks) {
+			// Another hardware problem. If the number of scan ticks is
+			// exactly equal to the number of channel ticks multiplied by
+			// the number of channels then the sampling rate is reduced
+			// by half.
+			trigger->iScanStartTrigType = ME_TRIG_TYPE_FOLLOW;
+		}
+	}
+	// Work around from Keith Hartley - end
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	if (inl(instance->status_reg) & ME4600_AI_STATUS_BIT_FSM) {
+		PERROR("Subdevice is busy.\n");
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+		ME_SUBDEVICE_EXIT;
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+
+	instance->status = ai_status_none;
+	spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+	// Stop all actions. Block all interrupts. Clear (disable) FIFOs.
+	ctrl =
+	    ME4600_AI_CTRL_BIT_LE_IRQ_RESET | ME4600_AI_CTRL_BIT_HF_IRQ_RESET |
+	    ME4600_AI_CTRL_BIT_SC_IRQ_RESET;
+
+	tmp = inl(instance->ctrl_reg);
+	// Preserve EXT IRQ and OFFSET settings. Clean other bits.
+	tmp &=
+	    (ME4600_AI_CTRL_BIT_EX_IRQ | ME4600_AI_CTRL_BIT_EX_IRQ_RESET |
+	     ME4600_AI_CTRL_BIT_FULLSCALE | ME4600_AI_CTRL_BIT_OFFSET);
+
+	// Send it to register.
+	outl(tmp | ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp | ctrl);
+
+	// Enable channel fifo -> data fifo in stream_start().
+	ctrl |= ME4600_AI_CTRL_BIT_CHANNEL_FIFO;
+	outl(tmp | ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp | ctrl);
+	spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+
+	// Write the channel list
+	for (i = 0; i < count; i++) {
+		entry = config_list[i].iChannel;
+
+		switch (config_list[i].iStreamConfig) {
+		case 0:	//BIPOLAR 10V
+/*
+				// ME4600_AI_LIST_RANGE_BIPOLAR_10 = 0x0000
+				// 'entry |= ME4600_AI_LIST_RANGE_BIPOLAR_10' <== Do nothing. Removed.
+				entry |= ME4600_AI_LIST_RANGE_BIPOLAR_10;
+*/
+			break;
+		case 1:	//UNIPOLAR 10V
+			entry |= ME4600_AI_LIST_RANGE_UNIPOLAR_10;
+			break;
+		case 2:	//BIPOLAR 2.5V
+			entry |= ME4600_AI_LIST_RANGE_BIPOLAR_2_5;
+			break;
+		case 3:	//UNIPOLAR 2.5V
+			entry |= ME4600_AI_LIST_RANGE_UNIPOLAR_2_5;
+			break;
+		default:
+			PERROR_CRITICAL("UNCHECK ERROR in config_list!\n");
+			PERROR_CRITICAL
+			    ("WRONG range\nPosition:%d Range:0x%04X\n", i,
+			     config_list[i].iStreamConfig);
+			goto VERIFY_ERROR;
+			break;
+		}
+
+		switch (config_list[i].iRef) {
+		case ME_REF_AI_GROUND:	//SINGLE ENDED
+/*
+				// ME4600_AI_LIST_INPUT_SINGLE_ENDED = 0x0000
+				// 'entry |= ME4600_AI_LIST_INPUT_SINGLE_ENDED' ==> Do nothing. Removed.
+				entry |= ME4600_AI_LIST_INPUT_SINGLE_ENDED;
+*/ break;
+		case ME_REF_AI_DIFFERENTIAL:	//DIFFERENTIAL
+			entry |= ME4600_AI_LIST_INPUT_DIFFERENTIAL;
+			break;
+		default:
+			PERROR_CRITICAL("UNCHECK ERROR in config_list!\n");
+			PERROR_CRITICAL
+			    ("WRONG reference\nPosition:%d Reference:0x%04X\n",
+			     i, config_list[i].iRef);
+			goto VERIFY_ERROR;
+			break;
+		}
+
+		//Add last entry flag
+		if (i == (count - 1)) {
+			entry |= ME4600_AI_LIST_LAST_ENTRY;
+		}
+
+		outl(entry, instance->channel_list_reg);
+		PDEBUG_REG("channel_list_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->channel_list_reg - instance->reg_base,
+			   entry);
+	}
+
+	// Set triggering registers
+	--acq_ticks;
+	outl(acq_ticks, instance->chan_pre_timer_reg);
+	PDEBUG_REG("chan_pre_timer_reg outl(0x%lX+0x%lX)=0x%llX\n",
+		   instance->reg_base,
+		   instance->chan_pre_timer_reg - instance->reg_base,
+		   acq_ticks);
+	outl(acq_ticks, instance->scan_pre_timer_low_reg);
+	PDEBUG_REG("scan_pre_timer_low_reg outl(0x%lX+0x%lX)=0x%llX\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_low_reg - instance->reg_base,
+		   acq_ticks & 0xFFFFFFFF);
+	outl((acq_ticks >> 32), instance->scan_pre_timer_high_reg);
+	PDEBUG_REG("scan_pre_timer_high_reg outl(0x%lX+0x%lX)=0x%llX\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_high_reg - instance->reg_base,
+		   (acq_ticks >> 32) & 0xFFFFFFFF);
+
+	// Set triggers
+	switch (trigger->iAcqStartTrigType) {
+		// Internal
+	case ME_TRIG_TYPE_SW:
+		// Nothing to set.
+		break;
+
+		// External
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		ctrl |= ME4600_AI_CTRL_BIT_EX_TRIG_ANALOG;
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		ctrl |= ME4600_AI_CTRL_BIT_EX_TRIG;
+
+		// External trigger needs edge's definition
+		switch (trigger->iAcqStartTrigEdge) {
+		case ME_TRIG_EDGE_RISING:
+			// Nothing to set.
+			break;
+
+		case ME_TRIG_EDGE_FALLING:
+			ctrl |= ME4600_AI_CTRL_BIT_EX_TRIG_FALLING;
+			break;
+
+		case ME_TRIG_EDGE_ANY:
+			ctrl |=
+			    ME4600_AI_CTRL_BIT_EX_TRIG_FALLING |
+			    ME4600_AI_CTRL_BIT_EX_TRIG_BOTH;
+			break;
+
+		default:
+			PERROR_CRITICAL
+			    ("UNCHECK TRIGGER EDGE in triggers structure!\n");
+			PERROR_CRITICAL
+			    ("WRONG acquisition start trigger:0x%04X.\n",
+			     trigger->iAcqStartTrigEdge);
+			err = ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+			goto VERIFY_ERROR;
+			break;
+		}
+		break;
+
+	default:
+		PERROR_CRITICAL("UNCHECK TRIGGER in triggers structure!\n");
+		PERROR_CRITICAL("WRONG acquisition start trigger:0x%04X.\n",
+				trigger->iAcqStartTrigType);
+		err = ME_ERRNO_INVALID_ACQ_START_TRIG_TYPE;
+		goto VERIFY_ERROR;
+		break;
+	}
+
+	switch (trigger->iScanStartTrigType) {
+	case ME_TRIG_TYPE_TIMER:
+		--scan_ticks;
+		outl(scan_ticks, instance->scan_timer_low_reg);
+		PDEBUG_REG("scan_timer_low_reg outl(0x%lX+0x%lX)=0x%llX\n",
+			   instance->reg_base,
+			   instance->scan_timer_low_reg - instance->reg_base,
+			   scan_ticks & 0xFFFFFFFF);
+		outl((scan_ticks >> 32), instance->scan_timer_high_reg);
+		PDEBUG_REG("scan_timer_high_reg outl(0x%lX+0x%lX)=0x%llX\n",
+			   instance->reg_base,
+			   instance->scan_timer_high_reg - instance->reg_base,
+			   (scan_ticks >> 32) & 0xFFFFFFFF);
+
+		if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_SW) {
+			ctrl |= ME4600_AI_CTRL_BIT_MODE_0;
+		} else {
+			ctrl |= ME4600_AI_CTRL_BIT_MODE_1;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		outl(0, instance->scan_timer_low_reg);
+		PDEBUG_REG("scan_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->scan_timer_low_reg - instance->reg_base,
+			   0);
+		outl(0, instance->scan_timer_high_reg);
+		PDEBUG_REG("scan_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->scan_timer_high_reg - instance->reg_base,
+			   0);
+		ctrl |= ME4600_AI_CTRL_BIT_MODE_2;
+		break;
+
+	case ME_TRIG_TYPE_FOLLOW:
+		outl(0, instance->scan_timer_low_reg);
+		PDEBUG_REG("scan_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->scan_timer_low_reg - instance->reg_base,
+			   0);
+		outl(0, instance->scan_timer_high_reg);
+		PDEBUG_REG("scan_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->scan_timer_high_reg - instance->reg_base,
+			   0);
+
+		if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_SW) {
+			ctrl |= ME4600_AI_CTRL_BIT_MODE_0;
+		} else {
+			ctrl |= ME4600_AI_CTRL_BIT_MODE_1;
+		}
+		break;
+
+	default:
+		PERROR_CRITICAL("UNCHECK TRIGGER in triggers structure!\n");
+		PERROR_CRITICAL("WRONG scan start trigger:0x%04X.\n",
+				trigger->iScanStartTrigType);
+		err = ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE;
+		goto VERIFY_ERROR;
+		break;
+	}
+
+	switch (trigger->iConvStartTrigType) {
+
+	case ME_TRIG_TYPE_TIMER:
+		--conv_ticks;
+		outl(conv_ticks, instance->chan_timer_reg);
+		PDEBUG_REG("chan_timer_reg outl(0x%lX+0x%lX)=0x%llX\n",
+			   instance->reg_base,
+			   instance->chan_timer_reg - instance->reg_base,
+			   conv_ticks);
+		break;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		outl(0, instance->chan_timer_reg);
+		PDEBUG_REG("chan_timer_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->chan_timer_reg - instance->reg_base, 0);
+		ctrl |= ME4600_AI_CTRL_BIT_MODE_0 | ME4600_AI_CTRL_BIT_MODE_1;
+		break;
+
+	default:
+		PERROR_CRITICAL("UNCHECK TRIGGER in triggers structure!\n");
+		PERROR_CRITICAL("WRONG conv start trigger:0x%04X.\n",
+				trigger->iConvStartTrigType);
+		err = ME_ERRNO_INVALID_CONV_START_TRIG_TYPE;
+		goto VERIFY_ERROR;
+
+		break;
+	}
+
+	//Sample & Hold feature
+	if (flags & ME_IO_STREAM_CONFIG_SAMPLE_AND_HOLD) {
+		if (instance->sh) {
+			ctrl |= ME4600_AI_CTRL_BIT_SAMPLE_HOLD;
+		} else {
+			PERROR_CRITICAL("UNCHECK S&H feature!\n");
+			err = ME_ERRNO_INVALID_FLAGS;
+			goto VERIFY_ERROR;
+		}
+	}
+	//Enable IRQs sources but leave latches blocked.
+	ctrl |= (ME4600_AI_CTRL_BIT_HF_IRQ | ME4600_AI_CTRL_BIT_SC_IRQ | ME4600_AI_CTRL_BIT_LE_IRQ);	//The last IRQ source (ME4600_AI_CTRL_BIT_LE_IRQ) is unused!
+
+	//Everything is good. Finalize
+	spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+	tmp = inl(instance->ctrl_reg);
+
+	//Preserve EXT IRQ and OFFSET settings. Clean other bits.
+	tmp &=
+	    (ME4600_AI_CTRL_BIT_EX_IRQ | ME4600_AI_CTRL_BIT_EX_IRQ_RESET |
+	     ME4600_AI_CTRL_BIT_FULLSCALE | ME4600_AI_CTRL_BIT_OFFSET);
+
+	// write the control word
+	outl(ctrl | tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl | tmp);
+	spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+
+	//Set the global parameters end exit.
+	instance->chan_list_len = count;
+	instance->fifo_irq_threshold = fifo_irq_threshold;
+
+	if (trigger->iAcqStopTrigType == ME_TRIG_TYPE_COUNT) {
+		data_required =
+		    (unsigned long long)trigger->iAcqStopCount *
+		    (unsigned long long)count;
+		if (data_required > UINT_MAX)
+			data_required = UINT_MAX;
+		instance->data_required = (unsigned int)data_required;
+	} else if (trigger->iScanStopTrigType == ME_TRIG_TYPE_COUNT)
+		instance->data_required =
+		    (unsigned long long)trigger->iScanStopCount;
+	else
+		instance->data_required = 0;
+
+	// Mark subdevice as configured to work in stream mode.
+	instance->status = ai_status_stream_configured;
+
+	// Deinit single config. Set all entries to NOT_CONFIGURED.
+	for (i = 0; i < instance->channels; i++) {
+		instance->single_config[i].status =
+		    ME_SINGLE_CHANNEL_NOT_CONFIGURED;
+	}
+
+      VERIFY_ERROR:		// Error in code. Wrong setting check. This should never ever happend!
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+      ERROR:			// Error in settings.
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ai_io_stream_new_values(me_subdevice_t * subdevice,
+					  struct file *filep,
+					  int time_out, int *count, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long t;
+	unsigned long j;
+	int volatile head;
+
+	PDEBUG("executed. idx=0\n");
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	} else {		// Max time.
+		t = LONG_MAX;
+	}
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	j = jiffies;
+
+	while (1) {
+		// Only runing device can generate break.
+		head = instance->circ_buf.head;
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 ((head !=
+						   instance->circ_buf.head)
+						  ||
+						  ((instance->status <=
+						    ai_status_stream_run_wait)
+						   && (instance->status >=
+						       ai_status_stream_end_wait))),
+						 t);
+
+		if (head != instance->circ_buf.head) {	// New data in buffer.
+			break;
+		} else if (instance->status == ai_status_stream_end) {	// End of work.
+			break;
+		} else if (instance->status == ai_status_stream_fifo_error) {
+			err = ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+			break;
+		} else if (instance->status == ai_status_stream_buffer_error) {
+			err = ME_ERRNO_RING_BUFFER_OVERFLOW;
+			break;
+		} else if (instance->status == ai_status_stream_error) {
+			err = ME_ERRNO_INTERNAL;
+			break;
+		} else if ((jiffies - j) >= t) {
+			PERROR("Wait on values timed out.\n");
+			err = ME_ERRNO_TIMEOUT;
+			break;
+		} else if (signal_pending(current)) {
+			PERROR("Wait on values interrupted from signal.\n");
+			err = ME_ERRNO_SIGNAL;
+			break;
+		}
+		// Correct timeout.
+		t -= jiffies - j;
+	}
+
+	*count = me_circ_buf_values(&instance->circ_buf);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int inline me4600_ai_io_stream_read_get_value(me4600_ai_subdevice_t *
+						     instance, int *values,
+						     const int count,
+						     const int flags)
+{
+	int n;
+	int i;
+	uint32_t value;
+
+	///Checking how many datas can be copied.
+	n = me_circ_buf_values(&instance->circ_buf);
+	if (n <= 0)
+		return 0;
+
+	if (n > count)
+		n = count;
+
+	if (flags & ME_IO_STREAM_READ_FRAMES) {
+		if (n < instance->chan_list_len)	//Not enough data!
+			return 0;
+		n -= n % instance->chan_list_len;
+	}
+
+	for (i = 0; i < n; i++) {
+		value = *(instance->circ_buf.buf + instance->circ_buf.tail);
+		if (put_user(value, values + i)) {
+			PERROR("Cannot copy new values to user.\n");
+			return -ME_ERRNO_INTERNAL;
+		}
+		instance->circ_buf.tail++;
+		instance->circ_buf.tail &= instance->circ_buf.mask;
+	}
+	return n;
+}
+
+static int me4600_ai_io_stream_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int read_mode,
+				    int *values, int *count, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	int ret;
+
+	int c = *count;
+	int min = c;
+
+	PDEBUG("executed. idx=0\n");
+
+	if (flags & ~ME_IO_STREAM_READ_FRAMES) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!values || !count) {
+		PERROR("Request has invalid pointer.\n");
+		return ME_ERRNO_INVALID_POINTER;
+	}
+
+	if (c < 0) {
+		PERROR("Request has invalid value's counter.\n");
+		return ME_ERRNO_INVALID_VALUE_COUNT;
+	}
+
+	if ((read_mode != ME_READ_MODE_BLOCKING)
+	    && (read_mode != ME_READ_MODE_NONBLOCKING)) {
+		PERROR("Invalid read mode specified.\n");
+		return ME_ERRNO_INVALID_READ_MODE;
+	}
+
+	if (c == 0) {		//You get what you want! Nothing more or less.
+		return ME_ERRNO_SUCCESS;
+	}
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+	ME_SUBDEVICE_ENTER;
+
+	//Check if subdevice is configured.
+	if (instance->chan_list_len <= 0) {
+		PERROR("Subdevice wasn't configured.\n");
+		ME_SUBDEVICE_EXIT;
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	if (flags & ME_IO_STREAM_READ_FRAMES) {
+		if (c < instance->chan_list_len) {	//Not enough data requested.
+			PERROR
+			    ("When using FRAME_READ mode minimal size is defined by channel list.\n");
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INVALID_VALUE_COUNT;
+		}
+	}
+
+	if (c > (ME4600_AI_CIRC_BUF_COUNT - instance->chan_list_len)) {	// To return acceptable amount of data when user pass too big value.
+		min = ME4600_AI_CIRC_BUF_COUNT - instance->chan_list_len;
+	}
+
+	if (flags & ME_IO_STREAM_READ_FRAMES) {
+		//Wait for whole list.
+		if (read_mode == ME_READ_MODE_BLOCKING) {
+			min = c - (c % instance->chan_list_len);
+		}
+
+		if (read_mode == ME_READ_MODE_NONBLOCKING) {
+			min = instance->chan_list_len;
+		}
+	}
+
+	if ((inl(instance->status_reg) & ME4600_AI_STATUS_BIT_FSM)) {	//Working
+		//If blocking mode -> wait for data.
+		if ((me_circ_buf_values(&instance->circ_buf) < min)
+		    && (read_mode == ME_READ_MODE_BLOCKING)) {
+			wait_event_interruptible(instance->wait_queue,
+						 ((me_circ_buf_values
+						   (&instance->circ_buf) >= min)
+						  || !(inl(instance->status_reg)
+						       &
+						       ME4600_AI_STATUS_BIT_FSM)));
+
+			if (signal_pending(current)) {
+				PERROR
+				    ("Wait on values interrupted from signal.\n");
+				err = ME_ERRNO_SIGNAL;
+			}
+		}
+	}
+
+	ret = me4600_ai_io_stream_read_get_value(instance, values, c, flags);
+	if (ret < 0) {
+		err = -ret;
+		*count = 0;
+	} else if (ret == 0) {
+		*count = 0;
+		if (instance->status == ai_status_stream_fifo_error) {
+			err = ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+			instance->status = ai_status_stream_end;
+		} else if (instance->status == ai_status_stream_buffer_error) {
+			err = ME_ERRNO_RING_BUFFER_OVERFLOW;
+			instance->status = ai_status_stream_end;
+		} else if (instance->status == ai_status_stream_end) {
+			err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+		} else if (instance->status == ai_status_stream_error) {
+			err = ME_ERRNO_INTERNAL;
+		} else if (instance->status == ai_status_none) {
+			PDEBUG("Stream canceled.\n");
+			err = ME_ERRNO_INTERNAL;
+		}
+	} else {
+		*count = ret;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+/** @brief Stop aqusation. Preserve FIFOs.
+*
+* @param instance The subdevice instance (pointer).
+*/
+
+static int ai_stop_immediately(me4600_ai_subdevice_t * instance)
+{
+	unsigned long cpu_flags = 0;
+	volatile uint32_t ctrl;
+	const int timeout = HZ / 10;	//100ms
+	int i;
+
+	for (i = 0; i <= timeout; i++) {
+		spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+		ctrl = inl(instance->ctrl_reg);
+		ctrl &= ~ME4600_AI_CTRL_BIT_STOP;
+		ctrl |=
+		    (ME4600_AI_CTRL_BIT_IMMEDIATE_STOP |
+		     ME4600_AI_CTRL_BIT_HF_IRQ_RESET |
+		     ME4600_AI_CTRL_BIT_SC_IRQ_RESET);
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+
+		if (!(inl(instance->status_reg) & ME4600_AI_STATUS_BIT_FSM)) {	// Exit.
+			break;
+		}
+
+		PINFO("Wait for stop: %d\n", i + 1);
+		//Still working!
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(1);
+	}
+
+	if (i > timeout) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		return ME_ERRNO_INTERNAL;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_io_stream_start(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int start_mode, int time_out, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags = 0;
+	unsigned long ref;
+	unsigned long delay = 0;
+
+	volatile uint32_t tmp;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((start_mode != ME_START_MODE_BLOCKING)
+	    && (start_mode != ME_START_MODE_NONBLOCKING)) {
+		PERROR("Invalid start mode specified.\n");
+		return ME_ERRNO_INVALID_START_MODE;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+
+		if (delay == 0)
+			delay = 1;
+	}
+
+	ME_SUBDEVICE_ENTER
+	    spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+
+	tmp = inl(instance->ctrl_reg);
+
+	if ((tmp & ME4600_AI_STATUS_BIT_FSM)) {
+		PERROR("Conversion is already running.\n");
+		spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+		err = ME_ERRNO_SUBDEVICE_BUSY;
+		goto ERROR;
+	}
+
+	if (instance->chan_list_len == 0) {	//Not configured!
+		PERROR("Subdevice is not configured to work in stream mode!\n");
+		spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+		goto ERROR;
+	}
+
+	if (!(tmp & (ME4600_AI_CTRL_BIT_MODE_0 | ME4600_AI_CTRL_BIT_MODE_1 | ME4600_AI_CTRL_BIT_MODE_2))) {	//Mode 0 = single work => no stream config
+		PERROR("Subdevice is configured to work in single mode.\n");
+		spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+		goto ERROR;
+	}
+	//Reset stop bits.
+	tmp |= ME4600_AI_CTRL_BIT_IMMEDIATE_STOP | ME4600_AI_CTRL_BIT_STOP;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	//Start datas' FIFO.
+	tmp |= ME4600_AI_CTRL_BIT_DATA_FIFO;
+	//Free stop bits.
+	tmp &= ~(ME4600_AI_CTRL_BIT_IMMEDIATE_STOP | ME4600_AI_CTRL_BIT_STOP);
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+
+	//Cancel control task
+	PDEBUG("Cancel control task.\n");
+	instance->ai_control_task_flag = 0;
+	cancel_delayed_work(&instance->ai_control_task);
+
+	//Set the starting values.
+	instance->ISM.global_read = 0;
+	instance->ISM.read = 0;
+	//Clear circular buffer
+	instance->circ_buf.head = 0;
+	instance->circ_buf.tail = 0;
+
+	//Set everything.
+	ai_data_acquisition_logic(instance);
+
+	//Set status to 'wait for start'
+	instance->status = ai_status_stream_run_wait;
+
+	// Set control task's timeout
+	instance->timeout.delay = delay;
+	instance->timeout.start_time = jiffies;
+
+	//Lets go! Start work
+	inl(instance->start_reg);
+	PDEBUG_REG("start_reg inl(0x%lX+0x%lX)\n", instance->reg_base,
+		   instance->start_reg - instance->reg_base);
+
+	// Schedule control task
+	instance->ai_control_task_flag = 1;
+	queue_delayed_work(instance->me4600_workqueue,
+			   &instance->ai_control_task, 1);
+
+	PDEVELOP("Delay:%ld\n", delay);
+
+	if (start_mode == ME_START_MODE_BLOCKING) {	//Wait for start.
+		ref = jiffies;
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ai_status_stream_run_wait),
+						 (delay) ? delay +
+						 1 : LONG_MAX);
+
+		if ((instance->status != ai_status_stream_run)
+		    && (instance->status != ai_status_stream_end)) {
+			PDEBUG("Starting stream canceled. %d\n",
+			       instance->status);
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			instance->status = ai_status_none;
+			ai_stop_isr(instance);
+			err = ME_ERRNO_SIGNAL;
+		} else if ((delay) && ((jiffies - ref) > delay)) {
+			if (instance->status != ai_status_stream_run) {
+				if (instance->status == ai_status_stream_end) {
+					PDEBUG("Timeout reached.\n");
+				} else if ((jiffies - ref) > delay + 1) {
+					PERROR
+					    ("Timeout reached. Not handled by control task!\n");
+					ai_stop_isr(instance);
+					instance->status =
+					    ai_status_stream_error;
+				} else {
+					PERROR
+					    ("Timeout reached. Signal come but status is strange: %d\n",
+					     instance->status);
+					ai_stop_isr(instance);
+					instance->status =
+					    ai_status_stream_error;
+				}
+
+				instance->ai_control_task_flag = 0;
+				cancel_delayed_work(&instance->ai_control_task);
+				err = ME_ERRNO_TIMEOUT;
+			}
+		}
+	}
+#ifdef MEDEBUG_INFO
+	tmp = inl(instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	PINFO("STATUS_BIT_FSM=%s.\n",
+	      (tmp & ME4600_AI_STATUS_BIT_FSM) ? "on" : "off");
+	PINFO("CTRL_BIT_HF_IRQ=%s.\n",
+	      (tmp & ME4600_AI_CTRL_BIT_HF_IRQ) ? "enable" : "disable");
+	PINFO("CTRL_BIT_HF_IRQ_RESET=%s.\n",
+	      (tmp & ME4600_AI_CTRL_BIT_HF_IRQ_RESET) ? "reset" : "work");
+	PINFO("CTRL_BIT_SC_IRQ=%s.\n",
+	      (tmp & ME4600_AI_CTRL_BIT_SC_IRQ) ? "enable" : "disable");
+	PINFO("CTRL_BIT_SC_RELOAD=%s.\n",
+	      (tmp & ME4600_AI_CTRL_BIT_SC_RELOAD) ? "on" : "off");
+	PINFO("CTRL_BIT_SC_IRQ_RESET=%s.\n",
+	      (tmp & ME4600_AI_CTRL_BIT_SC_IRQ_RESET) ? "reset" : "work");
+#endif
+
+      ERROR:
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ai_io_stream_status(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int wait,
+				      int *status, int *values, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	switch (instance->status) {
+	case ai_status_single_configured:
+	case ai_status_stream_configured:
+	case ai_status_stream_end:
+	case ai_status_stream_fifo_error:
+	case ai_status_stream_buffer_error:
+	case ai_status_stream_error:
+		*status = ME_STATUS_IDLE;
+		break;
+
+	case ai_status_stream_run_wait:
+	case ai_status_stream_run:
+	case ai_status_stream_end_wait:
+		*status = ME_STATUS_BUSY;
+		break;
+
+	case ai_status_none:
+	default:
+		*status =
+		    (inl(instance->status_reg) & ME4600_AI_STATUS_BIT_FSM) ?
+		    ME_STATUS_BUSY : ME_STATUS_IDLE;
+		break;
+	}
+
+	if ((wait == ME_WAIT_IDLE) && (*status == ME_STATUS_BUSY)) {
+		// Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 ((instance->status !=
+						   ai_status_stream_run_wait)
+						  && (instance->status !=
+						      ai_status_stream_run)
+						  && (instance->status !=
+						      ai_status_stream_end_wait)),
+						 LONG_MAX);
+
+		if (instance->status != ai_status_stream_end) {
+			PDEBUG("Wait for IDLE canceled. %d\n",
+			       instance->status);
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait for IDLE interrupted.\n");
+			instance->status = ai_status_none;
+			ai_stop_isr(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		*status = ME_STATUS_IDLE;
+	}
+
+	*values = me_circ_buf_values(&instance->circ_buf);
+	PDEBUG("me_circ_buf_values(&instance->circ_buf)=%d.\n", *values);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ai_io_stream_stop(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int stop_mode, int flags)
+{
+/**
+ @note Stop is implemented only in blocking mode.
+ @note Function return when state machine is stoped.
+*/
+	me4600_ai_subdevice_t *instance;
+	unsigned long cpu_flags;
+	uint32_t ctrl;
+	int ret;
+
+	PDEBUG("executed. idx=0\n");
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((stop_mode != ME_STOP_MODE_IMMEDIATE)
+	    && (stop_mode != ME_STOP_MODE_LAST_VALUE)) {
+		PERROR("Invalid stop mode specified.\n");
+		return ME_ERRNO_INVALID_STOP_MODE;
+	}
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	// Mark as stopping. => Software stop.
+	instance->status = ai_status_stream_end_wait;
+
+	if (stop_mode == ME_STOP_MODE_IMMEDIATE) {
+		ret = ai_stop_immediately(instance);
+
+		if (ret) {
+			PERROR("FSM is still busy.\n");
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_SUBDEVICE_BUSY;
+		}
+		instance->ai_control_task_flag = 0;
+
+	} else if (stop_mode == ME_STOP_MODE_LAST_VALUE) {
+		// Set stop bit in registry.
+		spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+		ctrl = inl(instance->ctrl_reg);
+		ctrl |= ME4600_AI_CTRL_BIT_STOP;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+
+		// Only runing process will interrupt this call. Events are signaled when status change.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ai_status_stream_end_wait),
+						 LONG_MAX);
+
+		if (instance->status != ai_status_stream_end) {
+			PDEBUG("Stopping stream canceled.\n");
+			ret = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Stopping stream interrupted.\n");
+			instance->status = ai_status_none;
+			ret = ME_ERRNO_SIGNAL;
+		}
+		// End of work.
+		ai_stop_immediately(instance);
+
+	}
+
+	ret = ai_read_data_pooling(instance);
+	if (ret > 0) {		// Everything fine. More datas put to software buffer.
+		instance->status = ai_status_stream_end;
+		ret = ME_ERRNO_SUCCESS;
+		// Signal that we put last data to software buffer.
+		wake_up_interruptible_all(&instance->wait_queue);
+	} else if (ret == 0) {	// Everything fine. No more datas in FIFO.
+		instance->status = ai_status_stream_end;
+		ret = ME_ERRNO_SUCCESS;
+	} else if (ret == -ME_ERRNO_RING_BUFFER_OVERFLOW) {	// Stop is unsuccessful, buffer is overflow.
+		instance->status = ai_status_stream_buffer_error;
+		ret = ME_ERRNO_SUCCESS;
+	} else {		// Stop is unsuccessful
+		instance->status = ai_status_stream_end;
+		ret = -ret;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return ret;
+}
+
+static int me4600_ai_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range)
+{
+	me4600_ai_subdevice_t *instance;
+	int i;
+	int r = -1;
+	int diff = 21E6;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	if ((*max - *min) < 0) {
+		PERROR("Invalid minimum and maximum values specified.\n");
+		return ME_ERRNO_INVALID_MIN_MAX;
+	}
+
+	if ((unit == ME_UNIT_VOLT) || (unit == ME_UNIT_ANY)) {
+		for (i = 0; i < instance->ranges_len; i++) {
+			if ((instance->ranges[i].min <= *min)
+			    && ((instance->ranges[i].max + 1000) >= *max)) {
+				if ((instance->ranges[i].max -
+				     instance->ranges[i].min) - (*max - *min) <
+				    diff) {
+					r = i;
+					diff =
+					    (instance->ranges[i].max -
+					     instance->ranges[i].min) - (*max -
+									 *min);
+				}
+			}
+		}
+
+		if (r < 0) {
+			PERROR("No matching range found.\n");
+			return ME_ERRNO_NO_RANGE;
+		} else {
+			*min = instance->ranges[r].min;
+			*max = instance->ranges[r].max;
+			*maxdata = ME4600_AI_MAX_DATA;
+			*range = r;
+		}
+	} else {
+		PERROR("Invalid physical unit specified.\n");
+		return ME_ERRNO_INVALID_UNIT;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_query_number_ranges(me_subdevice_t * subdevice,
+					 int unit, int *count)
+{
+	me4600_ai_subdevice_t *instance;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	if ((unit == ME_UNIT_VOLT) || (unit == ME_UNIT_ANY)) {
+		*count = instance->ranges_len;
+	} else {
+		*count = 0;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_query_range_info(me_subdevice_t * subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata)
+{
+	me4600_ai_subdevice_t *instance;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	if ((range < instance->ranges_len) && (range >= 0)) {
+		*unit = ME_UNIT_VOLT;
+		*min = instance->ranges[range].min;
+		*max = instance->ranges[range].max;
+		*maxdata = ME4600_AI_MAX_DATA;
+	} else {
+		PERROR("Invalid range number specified.\n");
+		return ME_ERRNO_INVALID_RANGE;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_query_timer(me_subdevice_t * subdevice,
+				 int timer,
+				 int *base_frequency,
+				 long long *min_ticks, long long *max_ticks)
+{
+	me4600_ai_subdevice_t *instance;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	switch (timer) {
+
+	case ME_TIMER_ACQ_START:
+		*base_frequency = ME4600_AI_BASE_FREQUENCY;
+		*min_ticks = ME4600_AI_MIN_ACQ_TICKS;
+		*max_ticks = ME4600_AI_MAX_ACQ_TICKS;
+		break;
+
+	case ME_TIMER_SCAN_START:
+		*base_frequency = ME4600_AI_BASE_FREQUENCY;
+		*min_ticks = ME4600_AI_MIN_SCAN_TICKS;
+		*max_ticks = ME4600_AI_MAX_SCAN_TICKS;
+		break;
+
+	case ME_TIMER_CONV_START:
+		*base_frequency = ME4600_AI_BASE_FREQUENCY;
+		*min_ticks = ME4600_AI_MIN_CHAN_TICKS;
+		*max_ticks = ME4600_AI_MAX_CHAN_TICKS;
+		break;
+
+	default:
+		PERROR("Invalid timer specified.(0x%04x)\n", timer);
+
+		return ME_ERRNO_INVALID_TIMER;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	me4600_ai_subdevice_t *instance;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+	*number = instance->channels;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed. idx=0\n");
+
+	*type = ME_TYPE_AI;
+	*subtype = ME_SUBTYPE_STREAMING;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed. idx=0\n");
+
+	*caps =
+	    ME_CAPS_AI_TRIG_SYNCHRONOUS | ME_CAPS_AI_FIFO |
+	    ME_CAPS_AI_FIFO_THRESHOLD;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_query_subdevice_caps_args(struct me_subdevice *subdevice,
+					       int cap, int *args, int count)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=0\n");
+
+	if (count != 1) {
+		PERROR("Invalid capability argument count.\n");
+		return ME_ERRNO_INVALID_CAP_ARG_COUNT;
+	}
+
+	switch (cap) {
+	case ME_CAP_AI_FIFO_SIZE:
+		args[0] = ME4600_AI_FIFO_COUNT;
+		break;
+
+	case ME_CAP_AI_BUFFER_SIZE:
+		args[0] =
+		    (instance->circ_buf.buf) ? ME4600_AI_CIRC_BUF_COUNT : 0;
+		break;
+
+	default:
+		PERROR("Invalid capability.\n");
+		err = ME_ERRNO_INVALID_CAP;
+		args[0] = 0;
+	}
+
+	return err;
+}
+
+void ai_limited_isr(me4600_ai_subdevice_t * instance, const uint32_t irq_status,
+		    const uint32_t ctrl_status)
+{
+	int to_read;
+
+	if (!instance->fifo_irq_threshold) {	//No threshold provided. SC ends work. HF need reseting.
+		if (irq_status & ME4600_IRQ_STATUS_BIT_SC) {
+			if (ai_read_data(instance, instance->ISM.next) != instance->ISM.next) {	//ERROR!
+				PERROR
+				    ("Limited amounts aqusition with TH=0: Circular buffer full!\n");
+				instance->status =
+				    ai_status_stream_buffer_error;
+			} else {
+				instance->status = ai_status_stream_end;
+			}
+			//End of work.
+			ai_stop_isr(instance);
+		} else if (irq_status & ME4600_IRQ_STATUS_BIT_AI_HF) {
+			instance->ISM.global_read += ME4600_AI_FIFO_HALF;
+
+			if (ai_read_data(instance, ME4600_AI_FIFO_HALF) != ME4600_AI_FIFO_HALF) {	//ERROR!
+				PERROR
+				    ("Limited amounts aqusition with TH = 0: Circular buffer full!\n");
+				//End of work.
+				ai_stop_isr(instance);
+				instance->status =
+				    ai_status_stream_buffer_error;
+			} else {
+				//Continue.
+				ai_limited_ISM(instance, irq_status);
+			}
+		}
+		//Signal user.
+		wake_up_interruptible_all(&instance->wait_queue);
+	} else			//if(instance->fifo_irq_threshold)
+	{
+		if (irq_status & ME4600_IRQ_STATUS_BIT_SC) {
+			instance->ISM.read = 0;
+			if ((instance->fifo_irq_threshold < ME4600_AI_FIFO_HALF)
+			    && (!(ctrl_status & ME4600_AI_STATUS_BIT_HF_DATA)))
+			{
+				to_read =
+				    ME4600_AI_FIFO_HALF -
+				    (ME4600_AI_FIFO_HALF %
+				     instance->fifo_irq_threshold);
+				PDEBUG
+				    ("Limited amounts aqusition with TH != 0: Not fast enough data aqusition! correction=%d\n",
+				     to_read);
+			} else {
+				to_read = instance->ISM.next;
+			}
+			instance->ISM.global_read += to_read;
+
+			ai_reschedule_SC(instance);
+
+			if (ai_read_data(instance, to_read) != to_read) {	//ERROR!
+				PERROR
+				    ("Limited amounts aqusition with TH != 0: Circular buffer full!\n");
+				//End of work.
+				ai_stop_isr(instance);
+				instance->status =
+				    ai_status_stream_buffer_error;
+			} else {
+				//Continue.
+				ai_limited_ISM(instance, irq_status);
+			}
+
+			//Signal user.
+			wake_up_interruptible_all(&instance->wait_queue);
+		} else if (irq_status & ME4600_IRQ_STATUS_BIT_AI_HF) {
+			instance->ISM.read += ME4600_AI_FIFO_HALF;
+			instance->ISM.global_read += ME4600_AI_FIFO_HALF;
+
+			if (ai_read_data(instance, ME4600_AI_FIFO_HALF) != ME4600_AI_FIFO_HALF) {	//ERROR!
+				PERROR
+				    ("Limited amounts aqusition with TH != 0: Circular buffer full!\n");
+				ai_stop_isr(instance);
+
+				instance->status =
+				    ai_status_stream_buffer_error;
+				//Signal user.
+				wake_up_interruptible_all(&instance->
+							  wait_queue);
+			} else {
+				//Countinue.
+				ai_limited_ISM(instance, irq_status);
+			}
+		}
+
+		if (instance->ISM.global_read >= instance->data_required) {	//End of work. Next paranoid pice of code: '>=' instead od '==' only to be sure.
+			ai_stop_isr(instance);
+			if (instance->status < ai_status_stream_end) {
+				instance->status = ai_status_stream_end;
+			}
+#ifdef MEDEBUG_ERROR
+			if (instance->ISM.global_read > instance->data_required) {	//This is security check case. This should never ever happend!
+				PERROR
+				    ("Limited amounts aqusition: Read more data than necessary! data_required=%d < read=%d\n",
+				     instance->data_required,
+				     instance->ISM.global_read);
+				//Signal error (warning??).
+				instance->status = ai_status_stream_error;
+			}
+#endif
+		}
+	}
+}
+
+void ai_infinite_isr(me4600_ai_subdevice_t * instance,
+		     const uint32_t irq_status, const uint32_t ctrl_status)
+{
+	int to_read;
+
+	if (irq_status & ME4600_IRQ_STATUS_BIT_SC) {	//next chunck of data -> read fifo
+		//Set new state in ISM.
+		if ((instance->fifo_irq_threshold < ME4600_AI_FIFO_HALF) && (!(ctrl_status & ME4600_AI_STATUS_BIT_HF_DATA))) {	//There is more data than we ecpected. Propably we aren't fast enough. Read as many as possible.
+			if (instance->fifo_irq_threshold) {
+				to_read =
+				    ME4600_AI_FIFO_HALF -
+				    (ME4600_AI_FIFO_HALF %
+				     instance->fifo_irq_threshold);
+				if (to_read > instance->fifo_irq_threshold) {
+					PDEBUG
+					    ("Infinite aqusition: Not fast enough data aqusition! TH != 0: correction=%d\n",
+					     to_read);
+				}
+			} else {	//No threshold specified.
+				to_read = ME4600_AI_FIFO_HALF;
+			}
+		} else {
+			to_read = instance->ISM.next;
+		}
+
+		instance->ISM.read += to_read;
+
+		//Get data
+		if (ai_read_data(instance, to_read) != to_read) {	//ERROR!
+			PERROR("Infinite aqusition: Circular buffer full!\n");
+			ai_stop_isr(instance);
+			instance->status = ai_status_stream_buffer_error;
+		} else {
+			ai_infinite_ISM(instance);
+			instance->ISM.global_read += instance->ISM.read;
+			instance->ISM.read = 0;
+		}
+
+		//Signal data to user
+		wake_up_interruptible_all(&instance->wait_queue);
+	} else if (irq_status & ME4600_IRQ_STATUS_BIT_AI_HF) {	//fifo is half full -> read fifo       Large blocks only!
+		instance->ISM.read += ME4600_AI_FIFO_HALF;
+
+		if (ai_read_data(instance, ME4600_AI_FIFO_HALF) != ME4600_AI_FIFO_HALF) {	//ERROR!
+			PERROR("Infinite aqusition: Circular buffer full!\n");
+			ai_stop_isr(instance);
+			instance->status = ai_status_stream_buffer_error;
+
+			//Signal it.
+			wake_up_interruptible_all(&instance->wait_queue);
+		} else {
+			ai_infinite_ISM(instance);
+		}
+	}
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me4600_ai_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me4600_ai_isr(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{				/// @note This is time critical function!
+	uint32_t irq_status;
+	uint32_t ctrl_status;
+	me4600_ai_subdevice_t *instance = dev_id;
+	//int to_read;
+
+	PDEBUG("executed. idx=0\n");
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	irq_status = inl(instance->irq_status_reg);
+	if (!
+	    (irq_status &
+	     (ME4600_IRQ_STATUS_BIT_AI_HF | ME4600_IRQ_STATUS_BIT_SC))) {
+#ifdef MEDEBUG_INFO
+		if ((irq_status & (ME4600_IRQ_STATUS_BIT_AI_HF | ME4600_IRQ_STATUS_BIT_SC | ME4600_IRQ_STATUS_BIT_LE)) == ME4600_IRQ_STATUS_BIT_LE) {	//This is security check case. LE is unused. This should never ever happend.
+			PINFO
+			    ("%ld Shared interrupt. %s(): irq_status_reg=LE_IRQ\n",
+			     jiffies, __func__);
+		} else {
+			PINFO
+			    ("%ld Shared interrupt. %s(): irq_status_reg=0x%04X\n",
+			     jiffies, __func__, irq_status);
+		}
+#endif
+		return IRQ_NONE;
+	}
+
+	if (!instance->circ_buf.buf) {	//Security check.
+		PERROR_CRITICAL("CIRCULAR BUFFER NOT EXISTS!\n");
+		ai_stop_isr(instance);
+		return IRQ_HANDLED;
+	}
+	//Get the status register.
+	ctrl_status = inl(instance->status_reg);
+
+#ifdef MEDEBUG_INFO
+	if (irq_status & ME4600_IRQ_STATUS_BIT_AI_HF)
+		PINFO("HF interrupt active\n");
+	if (irq_status & ME4600_IRQ_STATUS_BIT_SC)
+		PINFO("SC interrupt active\n");
+	if (irq_status & ME4600_IRQ_STATUS_BIT_LE)
+		PINFO("LE interrupt active\n");
+#endif
+
+	//This is safety check!
+	if ((irq_status & ME4600_IRQ_STATUS_BIT_AI_HF)
+	    && (ctrl_status & ME4600_AI_STATUS_BIT_HF_DATA)) {
+		PDEBUG("HF interrupt active but FIFO under half\n");
+		//Reset HF interrupt latch.
+		spin_lock(instance->ctrl_reg_lock);
+		outl(ctrl_status | ME4600_AI_CTRL_BIT_HF_IRQ_RESET,
+		     instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base,
+			   ctrl_status);
+		outl(ctrl_status, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base,
+			   ctrl_status);
+		spin_unlock(instance->ctrl_reg_lock);
+		return IRQ_HANDLED;
+	}
+#ifdef MEDEBUG_INFO
+	PINFO("STATUS_BIT_FSM=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_FSM) ? "on" : "off");
+
+	PINFO("STATUS_BIT_EF_CHANNEL=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_EF_CHANNEL) ? "not empty" :
+	      "empty");
+	PINFO("STATUS_BIT_HF_CHANNEL=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_HF_CHANNEL) ? " < HF" :
+	      " > HF");
+	PINFO("STATUS_BIT_FF_CHANNEL=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_FF_CHANNEL) ? "not full" :
+	      "full");
+
+	PINFO("STATUS_BIT_EF_DATA=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_EF_DATA) ? "not empty" :
+	      "empty");
+	PINFO("STATUS_BIT_HF_DATA=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_HF_DATA) ? " < HF" : " > HF");
+	PINFO("STATUS_BIT_FF_DATA=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_FF_DATA) ? "not full" :
+	      "full");
+
+	PINFO("CTRL_BIT_HF_IRQ=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_HF_IRQ) ? "enable" : "disable");
+	PINFO("CTRL_BIT_HF_IRQ_RESET=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_HF_IRQ_RESET) ? "reset" :
+	      "work");
+	PINFO("CTRL_BIT_SC_IRQ=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_SC_IRQ) ? "enable" : "disable");
+	PINFO("CTRL_BIT_SC_RELOAD=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_SC_RELOAD) ? "on" : "off");
+	PINFO("CTRL_BIT_SC_IRQ_RESET=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_SC_IRQ_RESET) ? "reset" :
+	      "work");
+#endif
+
+	//Look for overflow error.
+	if (!(ctrl_status & ME4600_AI_STATUS_BIT_FF_DATA)) {
+		//FIFO is full. Read datas and reset all settings.
+		PERROR("FIFO overflow.\n");
+		ai_read_data(instance, ME4600_AI_FIFO_COUNT);
+		ai_stop_isr(instance);
+
+		instance->status = ai_status_stream_fifo_error;
+		//Signal it.
+		wake_up_interruptible_all(&instance->wait_queue);
+
+		return IRQ_HANDLED;
+	}
+
+	if (!instance->data_required) {	//This is infinite aqusition.
+#ifdef MEDEBUG_ERROR
+		if ((irq_status &
+		     (ME4600_IRQ_STATUS_BIT_AI_HF | ME4600_IRQ_STATUS_BIT_SC))
+		    ==
+		    (ME4600_IRQ_STATUS_BIT_AI_HF | ME4600_IRQ_STATUS_BIT_SC)) {
+			///In infinite mode only one interrupt source should be reported!
+			PERROR
+			    ("Error in ISM! Infinite aqusition: HF and SC interrupts active! threshold=%d next=%d ctrl=0x%04X irq_status_reg=0x%04X",
+			     instance->fifo_irq_threshold, instance->ISM.next,
+			     ctrl_status, irq_status);
+		}
+#endif
+
+		ai_infinite_isr(instance, irq_status, ctrl_status);
+
+#ifdef MEDEBUG_INFO
+		ctrl_status = inl(instance->ctrl_reg);
+#endif
+	} else {
+
+		ai_limited_isr(instance, irq_status, ctrl_status);
+		ctrl_status = inl(instance->status_reg);
+		if (!(ctrl_status & (ME4600_AI_STATUS_BIT_HF_DATA | ME4600_AI_CTRL_BIT_HF_IRQ_RESET))) {	//HF active, but we have more than half already => HF will never come
+			PDEBUG
+			    ("MISSED HF. data_required=%d ISM.read=%d ISM.global=%d ISM.next=%d\n",
+			     instance->data_required, instance->ISM.read,
+			     instance->ISM.global_read, instance->ISM.next);
+			ai_limited_isr(instance, ME4600_IRQ_STATUS_BIT_AI_HF,
+				       ctrl_status);
+		}
+	}
+
+#ifdef MEDEBUG_INFO
+	PINFO("STATUS_BIT_FSM=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_FSM) ? "on" : "off");
+
+	PINFO("STATUS_BIT_EF_CHANNEL=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_EF_CHANNEL) ? "not empty" :
+	      "empty");
+	PINFO("STATUS_BIT_HF_CHANNEL=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_HF_CHANNEL) ? " < HF" :
+	      " > HF");
+	PINFO("STATUS_BIT_FF_CHANNEL=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_FF_CHANNEL) ? "not full" :
+	      "full");
+
+	PINFO("STATUS_BIT_EF_DATA=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_EF_DATA) ? "not empty" :
+	      "empty");
+	PINFO("STATUS_BIT_HF_DATA=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_HF_DATA) ? " < HF" : " > HF");
+	PINFO("STATUS_BIT_FF_DATA=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_FF_DATA) ? "not full" :
+	      "full");
+
+	PINFO("CTRL_BIT_HF_IRQ_RESET=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_HF_IRQ_RESET) ? "reset" :
+	      "work");
+	PINFO("CTRL_BIT_SC_IRQ=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_SC_IRQ) ? "enable" : "disable");
+	PINFO("CTRL_BIT_SC_RELOAD=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_SC_RELOAD) ? "on" : "off");
+	PINFO("CTRL_BIT_SC_IRQ_RESET=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_SC_IRQ_RESET) ? "reset" :
+	      "work");
+	PINFO("%ld END\n", jiffies);
+#endif
+
+	return IRQ_HANDLED;
+}
+
+/** @brief Stop aqusation of data. Reset interrupts' laches. Clear data's FIFO.
+*
+* @param instance The subdevice instance (pointer).
+*/
+void inline ai_stop_isr(me4600_ai_subdevice_t * instance)
+{				/// @note This is soft time critical function!
+	register uint32_t tmp;
+
+	spin_lock(instance->ctrl_reg_lock);
+	//Stop all. Reset interrupt laches. Reset data FIFO.
+	tmp = inl(instance->ctrl_reg);
+	tmp |=
+	    (ME4600_AI_CTRL_BIT_IMMEDIATE_STOP | ME4600_AI_CTRL_BIT_HF_IRQ_RESET
+	     | ME4600_AI_CTRL_BIT_LE_IRQ_RESET |
+	     ME4600_AI_CTRL_BIT_SC_IRQ_RESET);
+	tmp &= ~ME4600_AI_CTRL_BIT_DATA_FIFO;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->ctrl_reg_lock);
+}
+
+/** @brief Copy data from fifo to circular buffer.
+*
+* @param instance The subdevice instance (pointer).
+* @param count The number of requested data.
+*
+* @return On success: Number of copied values.
+* @return On error: -ME_ERRNO_RING_BUFFER_OVERFLOW.
+*/
+static int inline ai_read_data(me4600_ai_subdevice_t * instance,
+			       const int count)
+{				/// @note This is time critical function!
+	int c = count;
+	int empty_space;
+	int copied = 0;
+	int i, j;
+
+	empty_space = me_circ_buf_space_to_end(&instance->circ_buf);
+	if (empty_space <= 0) {
+		PDEBUG("Circular buffer full.\n");
+		return -ME_ERRNO_RING_BUFFER_OVERFLOW;
+	}
+
+	if (empty_space < c) {	//Copy first part. Max to end of buffer.
+		PDEBUG
+		    ("Try to copy %d values from FIFO to circular buffer (pass 1).\n",
+		     empty_space);
+		for (i = 0; i < empty_space; i++) {
+			*(instance->circ_buf.buf + instance->circ_buf.head) =
+			    (inw(instance->data_reg) ^ 0x8000);
+			instance->circ_buf.head++;
+		}
+		instance->circ_buf.head &= instance->circ_buf.mask;
+		c -= empty_space;
+		copied = empty_space;
+
+		empty_space = me_circ_buf_space_to_end(&instance->circ_buf);
+	}
+
+	if (empty_space > 0) {
+		j = (empty_space < c) ? empty_space : c;
+		PDEBUG
+		    ("Try to copy %d values from FIFO to circular buffer (pass 2).\n",
+		     c);
+		for (i = 0; i < j; i++) {
+			*(instance->circ_buf.buf + instance->circ_buf.head) =
+			    (inw(instance->data_reg) ^ 0x8000);
+			instance->circ_buf.head++;
+		}
+		instance->circ_buf.head &= instance->circ_buf.mask;
+		copied += j;
+	}
+	return copied;
+}
+
+void inline ai_infinite_ISM(me4600_ai_subdevice_t * instance)
+{				/// @note This is time critical function!
+	register volatile uint32_t ctrl_set, ctrl_reset, tmp;
+
+	if (instance->fifo_irq_threshold < ME4600_AI_FIFO_MAX_SC) {	// Only sample counter with reloadnig is working. Reset it.
+		PINFO
+		    ("Only sample counter with reloadnig is working. Reset it.\n");
+		ctrl_set = ME4600_AI_CTRL_BIT_SC_IRQ_RESET;
+		ctrl_reset = ~ME4600_AI_CTRL_BIT_SC_IRQ_RESET;
+	} else if (instance->fifo_irq_threshold == instance->ISM.read) {	//This is SC interrupt for large block. The whole section is done. Reset SC_IRQ an HF_IRQ and start everything again from beginning.
+		PINFO
+		    ("This is SC interrupt for large block. The whole section is done. Reset SC_IRQ an HF_IRQ and start everything again from beginning.\n");
+		ctrl_set =
+		    ME4600_AI_CTRL_BIT_SC_IRQ_RESET |
+		    ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+		ctrl_reset =
+		    ~(ME4600_AI_CTRL_BIT_SC_IRQ_RESET |
+		      ME4600_AI_CTRL_BIT_HF_IRQ_RESET);
+	} else if (instance->fifo_irq_threshold >= (ME4600_AI_FIFO_MAX_SC + instance->ISM.read)) {	//This is HF interrupt for large block.The next interrupt should be from HF, also. Reset HF.
+		PINFO
+		    ("This is HF interrupt for large block.The next interrupt should be from HF, also. Reset HF.\n");
+		ctrl_set = ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+		ctrl_reset = ~ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+	} else {		//This is HF interrupt for large block.The next interrupt should be from SC. Don't reset HF!
+		PINFO
+		    ("This is HF interrupt for large block.The next interrupt should be from SC. Don't reset HF!\n");
+		ctrl_set = ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+		ctrl_reset = 0xFFFFFFFF;
+	}
+
+	//Reset interrupt latch.
+	spin_lock(instance->ctrl_reg_lock);
+	tmp = inl(instance->ctrl_reg);
+	PINFO("ctrl=0x%x ctrl_set=0x%x ctrl_reset=0x%x\n", tmp, ctrl_set,
+	      ctrl_reset);
+	tmp |= ctrl_set;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	if (ctrl_reset != 0xFFFFFFFF) {
+		outl(tmp & ctrl_reset, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reset outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base,
+			   tmp & ctrl_reset);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+
+}
+
+void inline ai_limited_ISM(me4600_ai_subdevice_t * instance,
+			   uint32_t irq_status)
+{				/// @note This is time critical function!
+	register volatile uint32_t ctrl_set, ctrl_reset = 0xFFFFFFFF, tmp;
+
+	if (!instance->fifo_irq_threshold) {	//No threshold provided. SC ends work.
+		PINFO("No threshold provided. SC ends work.\n");
+		ctrl_set = ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+		if (instance->data_required > (ME4600_AI_FIFO_COUNT - 1 + instance->ISM.global_read)) {	//HF need reseting.
+			ctrl_reset &= ~ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+		}
+	} else			//if(instance->fifo_irq_threshold)
+	{
+		if (irq_status & ME4600_IRQ_STATUS_BIT_AI_HF) {
+			PINFO("Threshold provided. Clear HF latch.\n");
+			ctrl_set = ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+
+			if (instance->fifo_irq_threshold >= (ME4600_AI_FIFO_MAX_SC + instance->ISM.read)) {	//This is not the last one. HF need reseting.
+				PINFO
+				    ("The next interrupt is HF. HF need be activating.\n");
+				ctrl_reset = ~ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+			}
+		}
+
+		if (irq_status & ME4600_IRQ_STATUS_BIT_SC) {
+			PINFO("Threshold provided. Restart SC.\n");
+			ctrl_set = ME4600_AI_CTRL_BIT_SC_IRQ_RESET;
+			ctrl_reset &= ~ME4600_AI_CTRL_BIT_SC_IRQ_RESET;
+
+			if (instance->fifo_irq_threshold >= ME4600_AI_FIFO_MAX_SC) {	//This is not the last one. HF need to be activating.
+				PINFO
+				    ("The next interrupt is HF. HF need to be activating.\n");
+				ctrl_reset &= ~ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+			}
+		}
+	}
+
+	//Reset interrupt latch.
+	spin_lock(instance->ctrl_reg_lock);
+	tmp = inl(instance->ctrl_reg);
+	tmp |= ctrl_set;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	if (ctrl_reset != 0xFFFFFFFF) {
+		outl(tmp & ctrl_reset, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base,
+			   tmp & ctrl_reset);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+
+}
+
+/** @brief Last chunck of datas. We must reschedule sample counter.
+*	@note Last chunck.
+*	Leaving SC_RELOAD doesn't do any harm, but in some bad case can make extra interrupts.
+*	@warning When threshold is wrongly set some IRQ are lost.(!!!)
+*/
+void inline ai_reschedule_SC(me4600_ai_subdevice_t * instance)
+{
+	register uint32_t rest;
+
+	if (instance->data_required <= instance->ISM.global_read)
+		return;
+
+	rest = instance->data_required - instance->ISM.global_read;
+	if (rest < instance->fifo_irq_threshold) {	//End of work soon ....
+		PDEBUG("Rescheduling SC from %d to %d.\n",
+		       instance->fifo_irq_threshold, rest);
+		/// @note Write new value to SC <==  DANGER! This is not safe solution! We can miss some inputs.
+		outl(rest, instance->sample_counter_reg);
+		PDEBUG_REG("sample_counter_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->sample_counter_reg - instance->reg_base,
+			   rest);
+		instance->fifo_irq_threshold = rest;
+
+		if (rest < ME4600_AI_FIFO_MAX_SC) {
+			instance->ISM.next = rest;
+		} else {
+			instance->ISM.next = rest % ME4600_AI_FIFO_HALF;
+			if (instance->ISM.next + ME4600_AI_FIFO_HALF <
+			    ME4600_AI_FIFO_MAX_SC) {
+				instance->ISM.next += ME4600_AI_FIFO_HALF;
+			}
+		}
+	}
+}
+
+/** Start the ISM. All must be reseted before enter to this function. */
+void inline ai_data_acquisition_logic(me4600_ai_subdevice_t * instance)
+{
+	register uint32_t tmp;
+
+	if (!instance->data_required) {	//This is infinite aqusition.
+		if (!instance->fifo_irq_threshold) {	//No threshold provided. Set SC to 0.5*FIFO. Clear the SC's latch.
+			//Set the sample counter
+			outl(ME4600_AI_FIFO_HALF, instance->sample_counter_reg);
+			PDEBUG_REG
+			    ("sample_counter_reg outl(0x%lX+0x%lX)=0x%x\n",
+			     instance->reg_base,
+			     instance->sample_counter_reg - instance->reg_base,
+			     ME4600_AI_FIFO_HALF);
+		} else {	//Threshold provided. Set SC to treshold. Clear the SC's latch.
+			//Set the sample counter
+			outl(instance->fifo_irq_threshold,
+			     instance->sample_counter_reg);
+			PDEBUG_REG
+			    ("sample_counter_reg outl(0x%lX+0x%lX)=0x%x\n",
+			     instance->reg_base,
+			     instance->sample_counter_reg - instance->reg_base,
+			     instance->fifo_irq_threshold);
+		}
+
+		if (instance->fifo_irq_threshold < ME4600_AI_FIFO_MAX_SC) {	//Enable only sample counter's interrupt. Set reload bit. Clear the SC's latch.
+			spin_lock(instance->ctrl_reg_lock);
+			tmp = inl(instance->ctrl_reg);
+			tmp |= ME4600_AI_CTRL_BIT_SC_RELOAD;
+			tmp &= ~ME4600_AI_CTRL_BIT_SC_IRQ_RESET;
+			outl(tmp, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   tmp);
+			spin_unlock(instance->ctrl_reg_lock);
+			if (!instance->fifo_irq_threshold) {	//No threshold provided. Set ISM.next to 0.5*FIFO.
+				instance->ISM.next = ME4600_AI_FIFO_HALF;
+			} else {	//Threshold provided. Set ISM.next to treshold.
+				instance->ISM.next =
+				    instance->fifo_irq_threshold;
+			}
+		} else {	//Enable sample counter's and HF's interrupts.
+			spin_lock(instance->ctrl_reg_lock);
+			tmp = inl(instance->ctrl_reg);
+			tmp |= ME4600_AI_CTRL_BIT_SC_RELOAD;
+			tmp &=
+			    ~(ME4600_AI_CTRL_BIT_SC_IRQ_RESET |
+			      ME4600_AI_CTRL_BIT_HF_IRQ_RESET);
+			outl(tmp, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   tmp);
+			spin_unlock(instance->ctrl_reg_lock);
+
+			instance->ISM.next =
+			    instance->fifo_irq_threshold % ME4600_AI_FIFO_HALF;
+			if (instance->ISM.next + ME4600_AI_FIFO_HALF <
+			    ME4600_AI_FIFO_MAX_SC) {
+				instance->ISM.next += ME4600_AI_FIFO_HALF;
+			}
+		}
+	} else {		//This aqusition is limited to set number of data.
+		if (instance->fifo_irq_threshold >= instance->data_required) {	//Stupid situation.
+			instance->fifo_irq_threshold = 0;
+			PDEBUG
+			    ("Stupid situation: data_required(%d) < threshold(%d).\n",
+			     instance->fifo_irq_threshold,
+			     instance->data_required);
+		}
+
+		if (!instance->fifo_irq_threshold) {	//No threshold provided. Easy case: HF=read and SC=end.
+			//Set the sample counter to data_required.
+			outl(instance->data_required,
+			     instance->sample_counter_reg);
+			PDEBUG_REG
+			    ("sample_counter_reg outl(0x%lX+0x%lX)=0x%x\n",
+			     instance->reg_base,
+			     instance->sample_counter_reg - instance->reg_base,
+			     instance->data_required);
+
+			//Reset the latches of sample counter and HF (if SC>FIFO).
+			//No SC reload!
+			spin_lock(instance->ctrl_reg_lock);
+			tmp = inl(instance->ctrl_reg);
+			tmp &=
+			    ~(ME4600_AI_CTRL_BIT_SC_IRQ_RESET |
+			      ME4600_AI_CTRL_BIT_SC_RELOAD);
+			if (instance->data_required >
+			    (ME4600_AI_FIFO_COUNT - 1)) {
+				tmp &= ~ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+				instance->ISM.next =
+				    instance->data_required %
+				    ME4600_AI_FIFO_HALF;
+				instance->ISM.next += ME4600_AI_FIFO_HALF;
+
+			} else {
+				instance->ISM.next = instance->data_required;
+			}
+			outl(tmp, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   tmp);
+			spin_unlock(instance->ctrl_reg_lock);
+
+		} else {	//The most general case. We have concret numbe of required data and threshold. SC=TH
+			//Set the sample counter to threshold.
+			outl(instance->fifo_irq_threshold,
+			     instance->sample_counter_reg);
+			PDEBUG_REG
+			    ("sample_counter_reg outl(0x%lX+0x%lX)=0x%x\n",
+			     instance->reg_base,
+			     instance->sample_counter_reg - instance->reg_base,
+			     instance->fifo_irq_threshold);
+
+			spin_lock(instance->ctrl_reg_lock);
+			tmp = inl(instance->ctrl_reg);
+			//In this moment we are sure that SC will come more than once.
+			tmp |= ME4600_AI_CTRL_BIT_SC_RELOAD;
+
+			if (instance->fifo_irq_threshold < ME4600_AI_FIFO_MAX_SC) {	//The threshold is so small that we do need HF.
+				tmp &= ~ME4600_AI_CTRL_BIT_SC_IRQ_RESET;
+				instance->ISM.next =
+				    instance->fifo_irq_threshold;
+			} else {	//The threshold is large. The HF must be use.
+				tmp &=
+				    ~(ME4600_AI_CTRL_BIT_SC_IRQ_RESET |
+				      ME4600_AI_CTRL_BIT_HF_IRQ_RESET);
+				instance->ISM.next =
+				    instance->fifo_irq_threshold %
+				    ME4600_AI_FIFO_HALF;
+				if (instance->ISM.next + ME4600_AI_FIFO_HALF <
+				    ME4600_AI_FIFO_MAX_SC) {
+					instance->ISM.next +=
+					    ME4600_AI_FIFO_HALF;
+				}
+			}
+			outl(tmp, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   tmp);
+			spin_unlock(instance->ctrl_reg_lock);
+		}
+	}
+}
+
+static int ai_mux_toggler(me4600_ai_subdevice_t * instance)
+{
+	uint32_t tmp;
+
+	PDEBUG("executed. idx=0\n");
+
+	outl(0, instance->scan_pre_timer_low_reg);
+	PDEBUG_REG("scan_pre_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_low_reg - instance->reg_base, 0);
+	outl(0, instance->scan_pre_timer_high_reg);
+	PDEBUG_REG("scan_pre_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_high_reg - instance->reg_base, 0);
+	outl(0, instance->scan_timer_low_reg);
+	PDEBUG_REG("scan_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_timer_low_reg - instance->reg_base, 0);
+	outl(0, instance->scan_timer_high_reg);
+	PDEBUG_REG("scan_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_timer_high_reg - instance->reg_base, 0);
+	outl(65, instance->chan_timer_reg);
+	PDEBUG_REG("chan_timer_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->chan_timer_reg - instance->reg_base, 65);
+	outl(65, instance->chan_pre_timer_reg);
+	PDEBUG_REG("chan_pre_timer_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->chan_pre_timer_reg - instance->reg_base, 65);
+
+	// Turn on internal reference.
+	tmp = inl(instance->ctrl_reg);
+	tmp |= ME4600_AI_CTRL_BIT_FULLSCALE;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	// Clear data and channel fifo.
+	tmp &=
+	    ~(ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO);
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	tmp |= ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	// Write channel entry.
+	outl(ME4600_AI_LIST_INPUT_DIFFERENTIAL |
+	     ME4600_AI_LIST_RANGE_UNIPOLAR_2_5 | 31,
+	     instance->channel_list_reg);
+	PDEBUG_REG("channel_list_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->channel_list_reg - instance->reg_base,
+		   ME4600_AI_LIST_INPUT_DIFFERENTIAL |
+		   ME4600_AI_LIST_RANGE_UNIPOLAR_2_5 | 31);
+
+	// Start conversion.
+	inl(instance->start_reg);
+	PDEBUG_REG("start_reg inl(0x%lX+0x%lX)\n", instance->reg_base,
+		   instance->start_reg - instance->reg_base);
+	udelay(10);
+
+	// Clear data and channel fifo.
+	tmp &=
+	    ~(ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO);
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	tmp |= ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	// Write channel entry.
+	// ME4600_AI_LIST_INPUT_SINGLE_ENDED | ME4600_AI_LIST_RANGE_BIPOLAR_10 <= 0x0000
+	outl(ME4600_AI_LIST_INPUT_SINGLE_ENDED |
+	     ME4600_AI_LIST_RANGE_BIPOLAR_10, instance->channel_list_reg);
+	PDEBUG_REG("channel_list_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->channel_list_reg - instance->reg_base,
+		   ME4600_AI_LIST_INPUT_SINGLE_ENDED |
+		   ME4600_AI_LIST_RANGE_BIPOLAR_10);
+
+	// Start conversion.
+	inl(instance->start_reg);
+	PDEBUG_REG("start_reg inl(0x%lX+0x%lX)\n", instance->reg_base,
+		   instance->start_reg - instance->reg_base);
+	udelay(10);
+
+	// Clear control register.
+	tmp &= (ME4600_AI_CTRL_BIT_EX_IRQ | ME4600_AI_CTRL_BIT_EX_IRQ_RESET);
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+/** @brief Copy rest of data from fifo to circular buffer.
+* @note Helper for STOP command. After FSM is stopped.
+* @note This is slow function that copy all remainig data from FIFO to buffer.
+*
+* @param instance The subdevice instance (pointer).
+*
+* @return On success: Number of copied values.
+* @return On error: Negative error code -ME_ERRNO_RING_BUFFER_OVERFLOW.
+*/
+static int inline ai_read_data_pooling(me4600_ai_subdevice_t * instance)
+{				/// @note This is time critical function!
+	int empty_space;
+	int copied = 0;
+	int status = ME_ERRNO_SUCCESS;
+
+	PDEBUG("Space left in circular buffer = %d.\n",
+	       me_circ_buf_space(&instance->circ_buf));
+
+	while ((empty_space = me_circ_buf_space(&instance->circ_buf))) {
+		if (!(status = inl(instance->status_reg) & ME4600_AI_STATUS_BIT_EF_DATA)) {	//No more data. status = ME_ERRNO_SUCCESS = 0
+			break;
+		}
+		*(instance->circ_buf.buf + instance->circ_buf.head) =
+		    (inw(instance->data_reg) ^ 0x8000);
+		instance->circ_buf.head++;
+		instance->circ_buf.head &= instance->circ_buf.mask;
+	}
+
+#ifdef MEDEBUG_ERROR
+	if (!status)
+		PDEBUG
+		    ("Copied all remaining datas (%d) from FIFO to circular buffer.\n",
+		     copied);
+	else {
+		PDEBUG("No more empty space in buffer.\n");
+		PDEBUG("Copied %d datas from FIFO to circular buffer.\n",
+		       copied);
+		PDEBUG("FIFO still not empty.\n");
+	}
+#endif
+	return (!status) ? copied : -ME_ERRNO_RING_BUFFER_OVERFLOW;
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void me4600_ai_work_control_task(void *subdevice)
+#else
+static void me4600_ai_work_control_task(struct work_struct *work)
+#endif
+{
+	me4600_ai_subdevice_t *instance;
+	uint32_t status;
+	uint32_t ctrl;
+	unsigned long cpu_flags = 0;
+	int reschedule = 0;
+	int signaling = 0;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	instance = (me4600_ai_subdevice_t *) subdevice;
+#else
+	instance =
+	    container_of((void *)work, me4600_ai_subdevice_t, ai_control_task);
+#endif
+	PINFO("<%s: %ld> executed.\n", __func__, jiffies);
+
+	status = inl(instance->status_reg);
+	PDEBUG_REG("status_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->status_reg - instance->reg_base, status);
+
+	switch (instance->status) {	// Checking actual mode.
+		// Not configured for work.
+	case ai_status_none:
+		break;
+
+		//This are stable modes. No need to do anything. (?)
+	case ai_status_single_configured:
+	case ai_status_stream_configured:
+	case ai_status_stream_fifo_error:
+	case ai_status_stream_buffer_error:
+	case ai_status_stream_error:
+		PERROR("Shouldn't be running!.\n");
+		break;
+
+		// Stream modes
+	case ai_status_stream_run_wait:
+		if (status & ME4600_AI_STATUS_BIT_FSM) {	// ISM started..
+			instance->status = ai_status_stream_run;
+			// Signal the end of wait for start.
+			signaling = 1;
+			// Wait now for stop.
+			reschedule = 1;
+			break;
+
+			// Check timeout.
+			if ((instance->timeout.delay) && ((jiffies - instance->timeout.start_time) >= instance->timeout.delay)) {	// Timeout
+				PDEBUG("Timeout reached.\n");
+				// Stop all actions. No conditions! Block interrupts. Reset FIFO => Too late!
+				ai_stop_isr(instance);
+
+				instance->status = ai_status_stream_end;
+
+				// Signal the end.
+				signaling = 1;
+			}
+		}
+		break;
+
+	case ai_status_stream_run:
+		// Wait for stop ISM.
+		reschedule = 1;
+		break;
+
+	case ai_status_stream_end_wait:
+		if (!(status & ME4600_AI_STATUS_BIT_FSM)) {	// ISM stoped. Overwrite ISR.
+			instance->status = ai_status_stream_end;
+			// Signal the end of wait for stop.
+			signaling = 1;
+		} else {
+			// Wait for stop ISM.
+			reschedule = 1;
+		}
+		break;
+
+	case ai_status_stream_end:
+		//End work.
+		if (status & ME4600_AI_STATUS_BIT_FSM) {	// Still working? Stop it!
+			PERROR
+			    ("Status is 'ai_status_stream_end' but hardware is still working!\n");
+			spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |=
+			    (ME4600_AI_CTRL_BIT_IMMEDIATE_STOP |
+			     ME4600_AI_CTRL_BIT_HF_IRQ_RESET |
+			     ME4600_AI_CTRL_BIT_SC_IRQ_RESET);
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(instance->ctrl_reg_lock,
+					       cpu_flags);
+		}
+		break;
+
+	default:
+		PERROR_CRITICAL("Status is in wrong state (%d)!\n",
+				instance->status);
+		instance->status = ai_status_stream_error;
+		// Signal the end.
+		signaling = 1;
+		break;
+
+	}
+
+	if (signaling) {	//Signal it.
+		wake_up_interruptible_all(&instance->wait_queue);
+	}
+
+	if (instance->ai_control_task_flag && reschedule) {	// Reschedule task
+		queue_delayed_work(instance->me4600_workqueue,
+				   &instance->ai_control_task, 1);
+	} else {
+		PINFO("<%s> Ending control task.\n", __func__);
+	}
+
+}
diff --git a/drivers/staging/meilhaus/me4600_ai.h b/drivers/staging/meilhaus/me4600_ai.h
new file mode 100644
index 000000000000..1d5a1b9c6f91
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ai.h
@@ -0,0 +1,180 @@
+/**
+ * @file me4600_ai.h
+ *
+ * @brief Meilhaus ME-4000 analog input subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke  (k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_AI_H_
+#define _ME4600_AI_H_
+
+#include <linux/version.h>
+#include "mesubdevice.h"
+#include "meioctl.h"
+#include "mecirc_buf.h"
+
+#ifdef __KERNEL__
+
+#define ME4600_AI_MAX_DATA				0xFFFF
+
+#ifdef ME_SYNAPSE
+# define ME4600_AI_CIRC_BUF_SIZE_ORDER	8	// 2^n PAGES =>> Maximum value of 1MB for Synapse
+#else
+# define ME4600_AI_CIRC_BUF_SIZE_ORDER	5	// 2^n PAGES =>> 128KB
+#endif
+#define ME4600_AI_CIRC_BUF_SIZE 		PAGE_SIZE<<ME4600_AI_CIRC_BUF_SIZE_ORDER	// Buffer size in bytes.
+
+#ifdef _CBUFF_32b_t
+# define ME4600_AI_CIRC_BUF_COUNT		((ME4600_AI_CIRC_BUF_SIZE) / sizeof(uint32_t))	// Size in values
+#else
+# define ME4600_AI_CIRC_BUF_COUNT		((ME4600_AI_CIRC_BUF_SIZE) / sizeof(uint16_t))	// Size in values
+#endif
+
+#define ME4600_AI_FIFO_HALF				1024	//ME4600_AI_FIFO_COUNT/2                //1024
+#define ME4600_AI_FIFO_MAX_SC			1352	//0.66*ME4600_AI_FIFO_COUNT             //1352
+
+typedef enum ME4600_AI_STATUS {
+	ai_status_none = 0,
+	ai_status_single_configured,
+	ai_status_stream_configured,
+	ai_status_stream_run_wait,
+	ai_status_stream_run,
+	ai_status_stream_end_wait,
+	ai_status_stream_end,
+	ai_status_stream_fifo_error,
+	ai_status_stream_buffer_error,
+	ai_status_stream_error,
+	ai_status_last
+} ME4600_AI_STATUS;
+
+typedef struct me4600_single_config_entry {
+	unsigned short status;
+	uint32_t entry;
+	uint32_t ctrl;
+} me4600_single_config_entry_t;
+
+typedef struct me4600_range_entry {
+	int min;
+	int max;
+} me4600_range_entry_t;
+
+typedef struct me4600_ai_ISM {
+	volatile unsigned int global_read;				/**< The number of data read in total. */
+	volatile unsigned int read;						/**< The number of data read for this chunck. */
+	volatile unsigned int next;						/**< The number of data request by user. */
+} me4600_ai_ISM_t;
+
+typedef struct me4600_ai_timeout {
+	unsigned long start_time;
+	unsigned long delay;
+} me4600_ai_timeout_t;
+
+/**
+ * @brief The ME-4000 analog input subdevice class.
+ */
+typedef struct me4600_ai_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;							/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;						/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;						/**< Spin lock to protect #ctrl_reg from concurrent access. */
+
+	/* Hardware feautres */
+	unsigned int irq;								/**< The interrupt request number assigned by the PCI BIOS. */
+	int isolated;									/**< Marks if this subdevice is on an optoisolated device. */
+	int sh;											/**< Marks if this subdevice has sample and hold devices. */
+
+	unsigned int channels;							/**< The number of channels available on this subdevice. */
+	me4600_single_config_entry_t single_config[32];	/**< The configuration set for single acquisition. */
+
+	unsigned int data_required;						/**< The number of data request by user. */
+	unsigned int fifo_irq_threshold;				/**< The user adjusted FIFO high water interrupt level. */
+	unsigned int chan_list_len;						/**< The length of the user defined channel list. */
+
+	me4600_ai_ISM_t ISM;							/**< The information request by Interrupt-State-Machine. */
+	volatile enum ME4600_AI_STATUS status;			/**< The current stream status flag. */
+	me4600_ai_timeout_t timeout;					/**< The timeout for start in blocking and non-blocking mode. */
+
+											/* Registers *//**< All registers are 32 bits long. */
+	unsigned long ctrl_reg;
+	unsigned long status_reg;
+	unsigned long channel_list_reg;
+	unsigned long data_reg;
+	unsigned long chan_timer_reg;
+	unsigned long chan_pre_timer_reg;
+	unsigned long scan_timer_low_reg;
+	unsigned long scan_timer_high_reg;
+	unsigned long scan_pre_timer_low_reg;
+	unsigned long scan_pre_timer_high_reg;
+	unsigned long start_reg;
+	unsigned long irq_status_reg;
+	unsigned long sample_counter_reg;
+
+	unsigned int ranges_len;
+	me4600_range_entry_t ranges[4];					/**< The ranges available on this subdevice. */
+
+	/* Software buffer */
+	me_circ_buf_t circ_buf;							/**< Circular buffer holding measurment data. */
+	wait_queue_head_t wait_queue;					/**< Wait queue to put on tasks waiting for data to arrive. */
+
+	struct workqueue_struct *me4600_workqueue;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	struct work_struct ai_control_task;
+#else
+	struct delayed_work ai_control_task;
+#endif
+
+	volatile int ai_control_task_flag;				/**< Flag controling reexecuting of control task */
+
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me4600_ai_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-4000 analog input subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param channels The number of analog input channels available on this subdevice.
+ * @param channels The number of analog input ranges available on this subdevice.
+ * @param isolated Flag indicating if this device is opto isolated.
+ * @param sh Flag indicating if sample and hold devices are available.
+ * @param irq The irq number assigned by PCI BIOS.
+ * @param ctrl_reg_lock Pointer to spin lock protecting the control register from concurrent access.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me4600_ai_subdevice_t *me4600_ai_constructor(uint32_t reg_base,
+					     unsigned int channels,
+					     unsigned int ranges,
+					     int isolated,
+					     int sh,
+					     int irq,
+					     spinlock_t * ctrl_reg_lock,
+					     struct workqueue_struct
+					     *me4600_wq);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_ai_reg.h b/drivers/staging/meilhaus/me4600_ai_reg.h
new file mode 100644
index 000000000000..083fac7685f5
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ai_reg.h
@@ -0,0 +1,107 @@
+/**
+ * @file me4600_ai_reg.h
+ *
+ * @brief ME-4000 analog input subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_AI_REG_H_
+#define _ME4600_AI_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME4600_AI_CTRL_REG					0x74	// _/W
+#define ME4600_AI_STATUS_REG				0x74	// R/_
+#define ME4600_AI_CHANNEL_LIST_REG			0x78	// _/W
+#define ME4600_AI_DATA_REG					0x7C	// R/_
+#define ME4600_AI_CHAN_TIMER_REG			0x80	// _/W
+#define ME4600_AI_CHAN_PRE_TIMER_REG		0x84	// _/W
+#define ME4600_AI_SCAN_TIMER_LOW_REG		0x88	// _/W
+#define ME4600_AI_SCAN_TIMER_HIGH_REG		0x8C	// _/W
+#define ME4600_AI_SCAN_PRE_TIMER_LOW_REG	0x90	// _/W
+#define ME4600_AI_SCAN_PRE_TIMER_HIGH_REG	0x94	// _/W
+#define ME4600_AI_START_REG					0x98	// R/_
+
+#define ME4600_AI_SAMPLE_COUNTER_REG		0xC0	// _/W
+
+#define ME4600_AI_CTRL_BIT_MODE_0			0x00000001
+#define ME4600_AI_CTRL_BIT_MODE_1			0x00000002
+#define ME4600_AI_CTRL_BIT_MODE_2			0x00000004
+#define ME4600_AI_CTRL_BIT_SAMPLE_HOLD		0x00000008
+#define ME4600_AI_CTRL_BIT_IMMEDIATE_STOP	0x00000010
+#define ME4600_AI_CTRL_BIT_STOP				0x00000020
+#define ME4600_AI_CTRL_BIT_CHANNEL_FIFO		0x00000040
+#define ME4600_AI_CTRL_BIT_DATA_FIFO		0x00000080
+#define ME4600_AI_CTRL_BIT_FULLSCALE		0x00000100
+#define ME4600_AI_CTRL_BIT_OFFSET			0x00000200
+#define ME4600_AI_CTRL_BIT_EX_TRIG_ANALOG	0x00000400
+#define ME4600_AI_CTRL_BIT_EX_TRIG			0x00000800
+#define ME4600_AI_CTRL_BIT_EX_TRIG_FALLING	0x00001000
+#define ME4600_AI_CTRL_BIT_EX_IRQ			0x00002000
+#define ME4600_AI_CTRL_BIT_EX_IRQ_RESET		0x00004000
+#define ME4600_AI_CTRL_BIT_LE_IRQ			0x00008000
+#define ME4600_AI_CTRL_BIT_LE_IRQ_RESET		0x00010000
+#define ME4600_AI_CTRL_BIT_HF_IRQ			0x00020000
+#define ME4600_AI_CTRL_BIT_HF_IRQ_RESET		0x00040000
+#define ME4600_AI_CTRL_BIT_SC_IRQ			0x00080000
+#define ME4600_AI_CTRL_BIT_SC_IRQ_RESET		0x00100000
+#define ME4600_AI_CTRL_BIT_SC_RELOAD		0x00200000
+#define ME4600_AI_CTRL_BIT_EX_TRIG_BOTH		0x80000000
+
+#define ME4600_AI_STATUS_BIT_EF_CHANNEL		0x00400000
+#define ME4600_AI_STATUS_BIT_HF_CHANNEL		0x00800000
+#define ME4600_AI_STATUS_BIT_FF_CHANNEL		0x01000000
+#define ME4600_AI_STATUS_BIT_EF_DATA		0x02000000
+#define ME4600_AI_STATUS_BIT_HF_DATA		0x04000000
+#define ME4600_AI_STATUS_BIT_FF_DATA		0x08000000
+#define ME4600_AI_STATUS_BIT_LE				0x10000000
+#define ME4600_AI_STATUS_BIT_FSM			0x20000000
+
+#define ME4600_AI_CTRL_RPCI_FIFO			0x40000000	//Always set to zero!
+
+#define ME4600_AI_BASE_FREQUENCY			33E6
+
+#define ME4600_AI_MIN_ACQ_TICKS				66LL
+#define ME4600_AI_MAX_ACQ_TICKS				0xFFFFFFFFLL
+
+#define ME4600_AI_MIN_SCAN_TICKS			66LL
+#define ME4600_AI_MAX_SCAN_TICKS			0xFFFFFFFFFLL
+
+#define ME4600_AI_MIN_CHAN_TICKS			66LL
+#define ME4600_AI_MAX_CHAN_TICKS			0xFFFFFFFFLL
+
+#define ME4600_AI_FIFO_COUNT				2048
+
+#define ME4600_AI_LIST_COUNT				1024
+
+#define ME4600_AI_LIST_INPUT_SINGLE_ENDED	0x000
+#define ME4600_AI_LIST_INPUT_DIFFERENTIAL	0x020
+
+#define ME4600_AI_LIST_RANGE_BIPOLAR_10		0x000
+#define ME4600_AI_LIST_RANGE_BIPOLAR_2_5	0x040
+#define ME4600_AI_LIST_RANGE_UNIPOLAR_10	0x080
+#define ME4600_AI_LIST_RANGE_UNIPOLAR_2_5	0x0C0
+
+#define ME4600_AI_LIST_LAST_ENTRY		0x100
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_ao.c b/drivers/staging/meilhaus/me4600_ao.c
new file mode 100644
index 000000000000..e2bec8229abd
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ao.c
@@ -0,0 +1,6011 @@
+/**
+ * @file me4600_ao.c
+ *
+ * @brief ME-4000 analog output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+///Common part. (For normal and Bosch builds.)
+
+/* Includes
+ */
+
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <linux/types.h>
+#include <linux/version.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "meids.h"
+#include "me4600_reg.h"
+#include "me4600_ao_reg.h"
+#include "me4600_ao.h"
+
+/* Defines
+ */
+
+static int me4600_ao_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range);
+
+static int me4600_ao_query_number_ranges(me_subdevice_t * subdevice,
+					 int unit, int *count);
+
+static int me4600_ao_query_range_info(me_subdevice_t * subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata);
+
+static int me4600_ao_query_timer(me_subdevice_t * subdevice,
+				 int timer,
+				 int *base_frequency,
+				 long long *min_ticks, long long *max_ticks);
+
+static int me4600_ao_query_number_channels(me_subdevice_t * subdevice,
+					   int *number);
+
+static int me4600_ao_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype);
+
+static int me4600_ao_query_subdevice_caps(me_subdevice_t * subdevice,
+					  int *caps);
+
+static int me4600_ao_query_subdevice_caps_args(struct me_subdevice *subdevice,
+					       int cap, int *args, int count);
+
+#ifndef BOSCH
+/// @note NORMAL BUILD
+/// @author Krzysztof Gantzke   (k.gantzke@meilhaus.de)
+/* Includes
+ */
+
+# include <linux/workqueue.h>
+
+/* Defines
+ */
+
+/** Remove subdevice.
+*/
+static void me4600_ao_destructor(struct me_subdevice *subdevice);
+
+/** Reset subdevice. Stop all actions. Reset registry. Disable FIFO. Set output to 0V and status to 'none'.
+*/
+static int me4600_ao_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags);
+
+/** Set output as single
+*/
+static int me4600_ao_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags);
+
+/** Pass to user actual value of output.
+*/
+static int me4600_ao_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags);
+
+/** Write to output requed value.
+*/
+static int me4600_ao_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags);
+
+/** Set output as streamed device.
+*/
+static int me4600_ao_io_stream_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags);
+
+/** Wait for / Check empty space in buffer.
+*/
+static int me4600_ao_io_stream_new_values(me_subdevice_t * subdevice,
+					  struct file *filep,
+					  int time_out, int *count, int flags);
+
+/** Start streaming.
+*/
+static int me4600_ao_io_stream_start(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int start_mode, int time_out, int flags);
+
+/** Check actual state. / Wait for end.
+*/
+static int me4600_ao_io_stream_status(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int wait,
+				      int *status, int *values, int flags);
+
+/** Stop streaming.
+*/
+static int me4600_ao_io_stream_stop(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int stop_mode, int flags);
+
+/** Write datas to buffor.
+*/
+static int me4600_ao_io_stream_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int write_mode,
+				     int *values, int *count, int flags);
+
+/** Interrupt handler. Copy from buffer to FIFO.
+*/
+static irqreturn_t me4600_ao_isr(int irq, void *dev_id
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 19)
+				 , struct pt_regs *regs
+#endif
+    );
+/** Copy data from circular buffer to fifo (fast) in wraparound mode.
+*/
+int inline ao_write_data_wraparound(me4600_ao_subdevice_t * instance, int count,
+				    int start_pos);
+
+/** Copy data from circular buffer to fifo (fast).
+*/
+int inline ao_write_data(me4600_ao_subdevice_t * instance, int count,
+			 int start_pos);
+
+/** Copy data from circular buffer to fifo (slow).
+*/
+int inline ao_write_data_pooling(me4600_ao_subdevice_t * instance, int count,
+				 int start_pos);
+
+/** Copy data from user space to circular buffer.
+*/
+int inline ao_get_data_from_user(me4600_ao_subdevice_t * instance, int count,
+				 int *user_values);
+
+/** Stop presentation. Preserve FIFOs.
+*/
+int inline ao_stop_immediately(me4600_ao_subdevice_t * instance);
+
+/** Task for asynchronical state verifying.
+*/
+static void me4600_ao_work_control_task(
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+					       void *subdevice
+#else
+					       struct work_struct *work
+#endif
+    );
+/* Functions
+ */
+
+static int me4600_ao_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t tmp;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	instance->status = ao_status_none;
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+	instance->timeout.delay = 0;
+	instance->timeout.start_time = jiffies;
+
+	//Stop state machine.
+	err = ao_stop_immediately(instance);
+
+	//Remove from synchronous start.
+	spin_lock(instance->preload_reg_lock);
+	tmp = inl(instance->preload_reg);
+	tmp &=
+	    ~((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->
+	      ao_idx);
+	outl(tmp, instance->preload_reg);
+	PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, tmp);
+	*instance->preload_flags &=
+	    ~((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->
+	      ao_idx);
+	spin_unlock(instance->preload_reg_lock);
+
+	//Set single mode, dissable FIFO, dissable external trigger, set output to analog, block interrupt.
+	outl(ME4600_AO_MODE_SINGLE | ME4600_AO_CTRL_BIT_STOP |
+	     ME4600_AO_CTRL_BIT_IMMEDIATE_STOP | ME4600_AO_CTRL_BIT_RESET_IRQ,
+	     instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base,
+		   ME4600_AO_MODE_SINGLE | ME4600_AO_CTRL_BIT_STOP |
+		   ME4600_AO_CTRL_BIT_IMMEDIATE_STOP |
+		   ME4600_AO_CTRL_BIT_RESET_IRQ);
+
+	//Set output to 0V
+	outl(0x8000, instance->single_reg);
+	PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->single_reg - instance->reg_base, 0x8000);
+
+	instance->circ_buf.head = 0;
+	instance->circ_buf.tail = 0;
+	instance->preloaded_count = 0;
+	instance->data_count = 0;
+	instance->single_value = 0x8000;
+	instance->single_value_in_fifo = 0x8000;
+
+	//Set status to signal that device is unconfigured.
+	instance->status = ao_status_none;
+
+	//Signal reset if user is on wait.
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t ctrl;
+	uint32_t sync;
+	unsigned long cpu_flags;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	// Checking parameters
+	if (flags) {
+		PERROR
+		    ("Invalid flag specified. Must be ME_IO_SINGLE_CONFIG_NO_FLAGS.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	switch (trig_type) {
+	case ME_TRIG_TYPE_SW:
+		if (trig_edge != ME_TRIG_EDGE_NONE) {
+			PERROR
+			    ("Invalid trigger edge. Software trigger has not edge.\n");
+			return ME_ERRNO_INVALID_TRIG_EDGE;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		switch (trig_edge) {
+		case ME_TRIG_EDGE_ANY:
+		case ME_TRIG_EDGE_RISING:
+		case ME_TRIG_EDGE_FALLING:
+			break;
+
+		default:
+			PERROR("Invalid trigger edge.\n");
+			return ME_ERRNO_INVALID_TRIG_EDGE;
+		}
+		break;
+
+	default:
+		PERROR
+		    ("Invalid trigger type. Trigger must be software or digital.\n");
+		return ME_ERRNO_INVALID_TRIG_TYPE;
+	}
+
+	if ((trig_chan != ME_TRIG_CHAN_DEFAULT)
+	    && (trig_chan != ME_TRIG_CHAN_SYNCHRONOUS)) {
+		PERROR("Invalid trigger channel specified.\n");
+		return ME_ERRNO_INVALID_TRIG_CHAN;
+	}
+
+	if (ref != ME_REF_AO_GROUND) {
+		PERROR
+		    ("Invalid reference. Analog outputs have to have got REF_AO_GROUND.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if (single_config != 0) {
+		PERROR
+		    ("Invalid single config specified. Only one range for anlog outputs is available.\n");
+		return ME_ERRNO_INVALID_SINGLE_CONFIG;
+	}
+
+	if (channel != 0) {
+		PERROR
+		    ("Invalid channel number specified. Analog output have only one channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	//Subdevice running in stream mode!
+	if ((instance->status >= ao_status_stream_run_wait)
+	    && (instance->status < ao_status_stream_end)) {
+		PERROR("Subdevice is busy.\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+/// @note For single all calls (config and write) are erasing previous state!
+
+	instance->status = ao_status_none;
+
+	// Correct single mirrors
+	instance->single_value_in_fifo = instance->single_value;
+
+	//Stop device
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+	// Set control register.
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	// Set stop bit. Stop streaming mode.
+	ctrl = inl(instance->ctrl_reg);
+	//Reset all bits.
+	ctrl = ME4600_AO_CTRL_BIT_IMMEDIATE_STOP | ME4600_AO_CTRL_BIT_STOP;
+
+	if (trig_type == ME_TRIG_TYPE_EXT_DIGITAL) {
+		PINFO("External digital trigger.\n");
+
+		if (trig_edge == ME_TRIG_EDGE_ANY) {
+//                              ctrl |= ME4600_AO_CTRL_BIT_EX_TRIG_EDGE | ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+			instance->ctrl_trg =
+			    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE |
+			    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+		} else if (trig_edge == ME_TRIG_EDGE_FALLING) {
+//                              ctrl |= ME4600_AO_CTRL_BIT_EX_TRIG_EDGE;
+			instance->ctrl_trg = ME4600_AO_CTRL_BIT_EX_TRIG_EDGE;
+		} else if (trig_edge == ME_TRIG_EDGE_RISING) {
+			instance->ctrl_trg = 0x0;
+		}
+	} else if (trig_type == ME_TRIG_TYPE_SW) {
+		PDEBUG("Software trigger\n");
+		instance->ctrl_trg = 0x0;
+	}
+
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	// Set preload/synchronization register.
+	spin_lock(instance->preload_reg_lock);
+	if (trig_type == ME_TRIG_TYPE_SW) {
+		*instance->preload_flags &=
+		    ~(ME4600_AO_SYNC_EXT_TRIG << instance->ao_idx);
+	} else			//if (trig_type == ME_TRIG_TYPE_EXT_DIGITAL)
+	{
+		*instance->preload_flags |=
+		    ME4600_AO_SYNC_EXT_TRIG << instance->ao_idx;
+	}
+
+	if (trig_chan == ME_TRIG_CHAN_DEFAULT) {
+		*instance->preload_flags &=
+		    ~(ME4600_AO_SYNC_HOLD << instance->ao_idx);
+	} else			//if (trig_chan == ME_TRIG_CHAN_SYNCHRONOUS)
+	{
+		*instance->preload_flags |=
+		    ME4600_AO_SYNC_HOLD << instance->ao_idx;
+	}
+
+	//Reset hardware register
+	sync = inl(instance->preload_reg);
+	PDEBUG_REG("preload_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, sync);
+	sync &= ~(ME4600_AO_SYNC_EXT_TRIG << instance->ao_idx);
+	sync |= ME4600_AO_SYNC_HOLD << instance->ao_idx;
+
+	//Output configured in default (safe) mode.
+	outl(sync, instance->preload_reg);
+	PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, sync);
+	spin_unlock(instance->preload_reg_lock);
+
+	instance->status = ao_status_single_configured;
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	unsigned long j;
+	unsigned long delay = 0;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags & ~ME_IO_SINGLE_NONBLOCKING) {
+		PERROR("Invalid flag specified. %d\n", flags);
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (channel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if ((instance->status >= ao_status_stream_configured)
+	    && (instance->status <= ao_status_stream_end)) {
+		PERROR("Subdevice not configured to work in single mode!\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	ME_SUBDEVICE_ENTER;
+	if ((!flags) && (instance->status == ao_status_single_run_wait)) {	//Blocking mode. Wait for trigger.
+		if (time_out) {
+			delay = (time_out * HZ) / 1000;
+			if (delay == 0)
+				delay = 1;
+		}
+
+		j = jiffies;
+
+		//Only runing process will interrupt this call. Events are signaled when status change. This procedure has own timeout.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_single_run_wait),
+						 (delay) ? delay +
+						 1 : LONG_MAX);
+
+		if (instance->status == ao_status_none) {
+			PDEBUG("Single canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		if ((delay) && ((jiffies - j) >= delay)) {
+
+			PDEBUG("Timeout reached.\n");
+			err = ME_ERRNO_TIMEOUT;
+		}
+
+		*value =
+		    (!err) ? instance->single_value_in_fifo : instance->
+		    single_value;
+	} else {		//Non-blocking mode
+		//Read value
+		*value = instance->single_value;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags;
+	unsigned long j;
+	unsigned long delay = 0x0;
+
+	//Registry handling variables.
+	uint32_t sync_mask;
+	uint32_t mode;
+	uint32_t tmp;
+	uint32_t ctrl;
+	uint32_t status;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags &
+	    ~(ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS |
+	      ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING)) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (value & ~ME4600_AO_MAX_DATA) {
+		PERROR("Invalid value provided.\n");
+		return ME_ERRNO_VALUE_OUT_OF_RANGE;
+	}
+
+	if (channel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if ((instance->status == ao_status_none)
+	    || (instance->status > ao_status_single_end)) {
+		PERROR("Subdevice not configured to work in single mode!\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+/// @note For single all calls (config and write) are erasing previous state!
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+
+	// Correct single mirrors
+	instance->single_value_in_fifo = instance->single_value;
+
+	//Stop device
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+
+		if (delay == 0)
+			delay = 1;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	instance->single_value_in_fifo = value;
+
+	ctrl = inl(instance->ctrl_reg);
+
+	if (!instance->fifo) {	//No FIFO
+		//Set the single mode.
+		ctrl &= ~ME4600_AO_CTRL_MODE_MASK;
+
+		//Write value
+		PDEBUG("Write value\n");
+		outl(value, instance->single_reg);
+		PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->single_reg - instance->reg_base, value);
+	} else {		// mix-mode
+		//Set speed
+		outl(ME4600_AO_MIN_CHAN_TICKS - 1, instance->timer_reg);
+		PDEBUG_REG("timer_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->timer_reg - instance->reg_base,
+			   (int)ME4600_AO_MIN_CHAN_TICKS);
+		instance->hardware_stop_delay = HZ / 10;	//100ms
+
+		status = inl(instance->status_reg);
+
+		//Set the continous mode.
+		ctrl &= ~ME4600_AO_CTRL_MODE_MASK;
+		ctrl |= ME4600_AO_MODE_CONTINUOUS;
+
+		//Prepare FIFO
+		if (!(ctrl & ME4600_AO_CTRL_BIT_ENABLE_FIFO)) {	//FIFO wasn't enabeled. Do it.
+			PINFO("Enableing FIFO.\n");
+			ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			ctrl |=
+			    ME4600_AO_CTRL_BIT_ENABLE_FIFO |
+			    ME4600_AO_CTRL_BIT_RESET_IRQ;
+		} else {	//Check if FIFO is empty
+			if (status & ME4600_AO_STATUS_BIT_EF) {	//FIFO not empty
+				PINFO("Reseting FIFO.\n");
+				ctrl &=
+				    ~(ME4600_AO_CTRL_BIT_ENABLE_FIFO |
+				      ME4600_AO_CTRL_BIT_ENABLE_IRQ);
+				ctrl |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+				outl(ctrl, instance->ctrl_reg);
+				PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->ctrl_reg -
+					   instance->reg_base, ctrl);
+
+				ctrl |=
+				    ME4600_AO_CTRL_BIT_ENABLE_FIFO |
+				    ME4600_AO_CTRL_BIT_RESET_IRQ;
+			} else {	//FIFO empty, only interrupt needs to be disabled!
+				ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+				ctrl |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+			}
+		}
+
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		//Write output - 1 value to FIFO
+		if (instance->ao_idx & 0x1) {
+			outl(value <<= 16, instance->fifo_reg);
+			PDEBUG_REG("fifo_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->fifo_reg - instance->reg_base,
+				   value <<= 16);
+		} else {
+			outl(value, instance->fifo_reg);
+			PDEBUG_REG("fifo_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->fifo_reg - instance->reg_base,
+				   value);
+		}
+	}
+
+	mode = *instance->preload_flags >> instance->ao_idx;
+	mode &= (ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG);
+
+	PINFO("Triggering mode: 0x%x\n", mode);
+
+	spin_lock(instance->preload_reg_lock);
+	sync_mask = inl(instance->preload_reg);
+	PDEBUG_REG("preload_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, sync_mask);
+	switch (mode) {
+	case 0:		//Individual software
+		ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+		if (!instance->fifo) {	// No FIFO - In this case resetting 'ME4600_AO_SYNC_HOLD' will trigger output.
+			if ((sync_mask & ((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != ME4600_AO_SYNC_HOLD) {	//Now we can set correct mode. This is exception. It is set to synchronous and triggered later.
+				sync_mask &=
+				    ~(ME4600_AO_SYNC_EXT_TRIG << instance->
+				      ao_idx);
+				sync_mask |=
+				    ME4600_AO_SYNC_HOLD << instance->ao_idx;
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		} else {	// FIFO
+			if ((sync_mask & ((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != 0x0) {	//Now we can set correct mode.
+				sync_mask &=
+				    ~((ME4600_AO_SYNC_EXT_TRIG |
+				       ME4600_AO_SYNC_HOLD) << instance->
+				      ao_idx);
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		}
+		instance->single_value = value;
+		break;
+
+	case ME4600_AO_SYNC_EXT_TRIG:	//Individual hardware
+		ctrl |= ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+		if (!instance->fifo) {	// No FIFO - In this case resetting 'ME4600_AO_SYNC_HOLD' will trigger output.
+			if ((sync_mask & ((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != ME4600_AO_SYNC_HOLD) {	//Now we can set correct mode
+				sync_mask &=
+				    ~(ME4600_AO_SYNC_EXT_TRIG << instance->
+				      ao_idx);
+				sync_mask |=
+				    ME4600_AO_SYNC_HOLD << instance->ao_idx;
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		} else {	// FIFO
+			if ((sync_mask & ((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != 0x0) {	//Now we can set correct mode.
+				sync_mask &=
+				    ~((ME4600_AO_SYNC_EXT_TRIG |
+				       ME4600_AO_SYNC_HOLD) << instance->
+				      ao_idx);
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		}
+		break;
+
+	case ME4600_AO_SYNC_HOLD:	//Synchronous software
+		ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+//                                      if((sync_mask & ((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != ME4600_AO_SYNC_HOLD)
+		if ((sync_mask & ((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != (ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG)) {	//Now we can set correct mode
+			sync_mask |=
+			    ME4600_AO_SYNC_EXT_TRIG << instance->ao_idx;
+//                                              sync_mask &= ~(ME4600_AO_SYNC_EXT_TRIG << instance->ao_idx);
+			sync_mask |= ME4600_AO_SYNC_HOLD << instance->ao_idx;
+
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+		}
+		break;
+
+	case (ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG):	//Synchronous hardware
+		ctrl |= ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+		if ((sync_mask & ((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != (ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG)) {	//Now we can set correct mode
+			sync_mask |=
+			    (ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) <<
+			    instance->ao_idx;
+
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+		}
+		break;
+	}
+//              spin_unlock(instance->preload_reg_lock);        // Moved down.
+
+	//Activate ISM (remove 'stop' bits)
+	ctrl &=
+	    ~(ME4600_AO_CTRL_BIT_EX_TRIG_EDGE |
+	      ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH);
+	ctrl |= instance->ctrl_trg;
+	ctrl &= ~(ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+/// @note When flag 'ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS' is set than output is triggered. ALWAYS!
+
+	if (!instance->fifo) {	//No FIFO
+		if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {	//Fired all software synchronous outputs.
+			tmp = ~(*instance->preload_flags | 0xFFFF0000);
+			PINFO
+			    ("Fired all software synchronous outputs. mask:0x%08x\n",
+			     tmp);
+			tmp |= sync_mask & 0xFFFF0000;
+			// Add this channel to list
+			tmp &= ~(ME4600_AO_SYNC_HOLD << instance->ao_idx);
+
+			//Fire
+			PINFO("Software trigger.\n");
+			outl(tmp, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   tmp);
+
+			//Restore save settings
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+		} else if (!mode) {	// Add this channel to list
+			outl(sync_mask &
+			     ~(ME4600_AO_SYNC_HOLD << instance->ao_idx),
+			     instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask & ~(ME4600_AO_SYNC_HOLD <<
+						 instance->ao_idx));
+
+			//Fire
+			PINFO("Software trigger.\n");
+
+			//Restore save settings
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+		}
+
+	} else {		// mix-mode - begin
+		if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {	//Trigger outputs
+			//Add channel to start list
+			outl(sync_mask |
+			     (ME4600_AO_SYNC_HOLD << instance->ao_idx),
+			     instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask | (ME4600_AO_SYNC_HOLD <<
+						instance->ao_idx));
+
+			//Fire
+			PINFO
+			    ("Fired all software synchronous outputs by software trigger.\n");
+			outl(0x8000, instance->single_reg);
+			PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->single_reg - instance->reg_base,
+				   0x8000);
+
+			//Restore save settings
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+		} else if (!mode) {	//Trigger outputs
+/*			//Remove channel from start list //<== Unnecessary. Removed.
+			outl(sync_mask & ~(ME4600_AO_SYNC_HOLD << instance->ao_idx), instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, tmp);
+*/
+			//Fire
+			PINFO("Software trigger.\n");
+			outl(0x8000, instance->single_reg);
+			PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->single_reg - instance->reg_base,
+				   0x8000);
+
+/*			//Restore save settings //<== Unnecessary. Removed.
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, sync_mask);
+*/
+		}
+	}
+	spin_unlock(instance->preload_reg_lock);
+
+	j = jiffies;
+	instance->status = ao_status_single_run_wait;
+
+	instance->timeout.delay = delay;
+	instance->timeout.start_time = j;
+	instance->ao_control_task_flag = 1;
+	queue_delayed_work(instance->me4600_workqueue,
+			   &instance->ao_control_task, 1);
+
+	if (!(flags & ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING)) {
+
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_single_run_wait),
+						 (delay) ? delay +
+						 1 : LONG_MAX);
+
+		if (((!delay) || ((jiffies - j) <= delay))
+		    && (instance->status != ao_status_single_end)) {
+			PDEBUG("Single canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			instance->ao_control_task_flag = 0;
+			cancel_delayed_work(&instance->ao_control_task);
+			ao_stop_immediately(instance);
+			instance->status = ao_status_none;
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		if ((delay) && ((jiffies - j) >= delay)) {
+			if (instance->status == ao_status_single_end) {
+				PDEBUG("Timeout reached.\n");
+			} else {
+				if ((jiffies - j) > delay) {
+					PERROR
+					    ("Timeout reached. Not handled by control task!\n");
+				} else {
+					PERROR
+					    ("Timeout reached. Signal come but status is strange: %d\n",
+					     instance->status);
+				}
+
+				ao_stop_immediately(instance);
+			}
+
+			instance->ao_control_task_flag = 0;
+			cancel_delayed_work(&instance->ao_control_task);
+			instance->status = ao_status_single_end;
+			err = ME_ERRNO_TIMEOUT;
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t ctrl;
+	unsigned long cpu_flags;
+	uint64_t conv_ticks;
+	unsigned int conv_start_ticks_low = trigger->iConvStartTicksLow;
+	unsigned int conv_start_ticks_high = trigger->iConvStartTicksHigh;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	conv_ticks =
+	    (uint64_t) conv_start_ticks_low +
+	    ((uint64_t) conv_start_ticks_high << 32);
+
+	if (flags &
+	    ~(ME_IO_STREAM_CONFIG_HARDWARE_ONLY | ME_IO_STREAM_CONFIG_WRAPAROUND
+	      | ME_IO_STREAM_CONFIG_BIT_PATTERN)) {
+		PERROR("Invalid flags.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (flags & ME_IO_STREAM_CONFIG_HARDWARE_ONLY) {
+		if (!flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			PERROR
+			    ("Hardware ME_IO_STREAM_CONFIG_HARDWARE_ONLY has to be with ME_IO_STREAM_CONFIG_WRAPAROUND.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+
+		if ((trigger->iAcqStopTrigType != ME_TRIG_TYPE_NONE)
+		    || (trigger->iScanStopTrigType != ME_TRIG_TYPE_NONE)) {
+			PERROR
+			    ("Hardware wraparound mode must be in infinite mode.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+	}
+
+	if (count != 1) {
+		PERROR("Only 1 entry in config list acceptable.\n");
+		return ME_ERRNO_INVALID_CONFIG_LIST_COUNT;
+	}
+
+	if (config_list[0].iChannel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (config_list[0].iStreamConfig != 0) {
+		PERROR("Only one range available.\n");
+		return ME_ERRNO_INVALID_STREAM_CONFIG;
+	}
+
+	if (config_list[0].iRef != ME_REF_AO_GROUND) {
+		PERROR("Output is referenced to ground.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if ((trigger->iAcqStartTicksLow != 0)
+	    || (trigger->iAcqStartTicksHigh != 0)) {
+		PERROR
+		    ("Invalid acquisition start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_ARG;
+	}
+
+	if (config_list[0].iFlags) {
+		PERROR("Invalid config list flag.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	switch (trigger->iAcqStartTrigType) {
+	case ME_TRIG_TYPE_SW:
+		if (trigger->iAcqStartTrigEdge != ME_TRIG_EDGE_NONE) {
+			PERROR
+			    ("Invalid acquisition start trigger edge specified.\n");
+			return ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		switch (trigger->iAcqStartTrigEdge) {
+		case ME_TRIG_EDGE_ANY:
+		case ME_TRIG_EDGE_RISING:
+		case ME_TRIG_EDGE_FALLING:
+			break;
+
+		default:
+			PERROR
+			    ("Invalid acquisition start trigger edge specified.\n");
+			return ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+		}
+		break;
+
+	default:
+		PERROR("Invalid acquisition start trigger type specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_TRIG_TYPE;
+	}
+
+	if (trigger->iScanStartTrigType != ME_TRIG_TYPE_FOLLOW) {
+		PERROR("Invalid scan start trigger type specified.\n");
+		return ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE;
+	}
+
+	if (trigger->iConvStartTrigType != ME_TRIG_TYPE_TIMER) {
+		PERROR("Invalid conv start trigger type specified.\n");
+		return ME_ERRNO_INVALID_CONV_START_TRIG_TYPE;
+	}
+
+	if ((conv_ticks < ME4600_AO_MIN_CHAN_TICKS)
+	    || (conv_ticks > ME4600_AO_MAX_CHAN_TICKS)) {
+		PERROR("Invalid conv start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_CONV_START_ARG;
+	}
+
+	if (trigger->iAcqStartTicksLow || trigger->iAcqStartTicksHigh) {
+		PERROR("Invalid acq start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_ARG;
+	}
+
+	if (trigger->iScanStartTicksLow || trigger->iScanStartTicksHigh) {
+		PERROR("Invalid scan start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_SCAN_START_ARG;
+	}
+
+	switch (trigger->iScanStopTrigType) {
+	case ME_TRIG_TYPE_NONE:
+		if (trigger->iScanStopCount != 0) {
+			PERROR("Invalid scan stop count specified.\n");
+			return ME_ERRNO_INVALID_SCAN_STOP_ARG;
+		}
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			if (trigger->iScanStopCount <= 0) {
+				PERROR("Invalid scan stop count specified.\n");
+				return ME_ERRNO_INVALID_SCAN_STOP_ARG;
+			}
+		} else {
+			PERROR("The continous mode has not 'scan' contects.\n");
+			return ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+		}
+		break;
+
+	default:
+		PERROR("Invalid scan stop trigger type specified.\n");
+		return ME_ERRNO_INVALID_SCAN_STOP_TRIG_TYPE;
+	}
+
+	switch (trigger->iAcqStopTrigType) {
+	case ME_TRIG_TYPE_NONE:
+		if (trigger->iAcqStopCount != 0) {
+			PERROR("Invalid acq stop count specified.\n");
+			return ME_ERRNO_INVALID_ACQ_STOP_ARG;
+		}
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (trigger->iScanStopTrigType != ME_TRIG_TYPE_NONE) {
+			PERROR("Invalid acq stop trigger type specified.\n");
+			return ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+		}
+
+		if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			if (trigger->iAcqStopCount <= 0) {
+				PERROR
+				    ("The continous mode has not 'scan' contects.\n");
+				return ME_ERRNO_INVALID_ACQ_STOP_ARG;
+			}
+		}
+		break;
+
+	default:
+		PERROR("Invalid acq stop trigger type specified.\n");
+		return ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+	}
+
+	switch (trigger->iAcqStartTrigChan) {
+	case ME_TRIG_CHAN_DEFAULT:
+	case ME_TRIG_CHAN_SYNCHRONOUS:
+		break;
+
+	default:
+		PERROR("Invalid acq start trigger channel specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_TRIG_CHAN;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if ((flags & ME_IO_STREAM_CONFIG_BIT_PATTERN) && !instance->bitpattern) {
+		PERROR("This subdevice not support output redirection.\n");
+		ME_SUBDEVICE_EXIT;
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+	//Stop device
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+
+	//Check if state machine is stopped.
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	//Reset control register. Block all actions. Disable IRQ. Disable FIFO.
+	ctrl =
+	    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP | ME4600_AO_CTRL_BIT_STOP |
+	    ME4600_AO_CTRL_BIT_RESET_IRQ;
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+
+	//This is paranoic, but to be sure.
+	instance->preloaded_count = 0;
+	instance->data_count = 0;
+	instance->circ_buf.head = 0;
+	instance->circ_buf.tail = 0;
+
+	/* Set mode. */
+	if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {	//Wraparound
+		if (flags & ME_IO_STREAM_CONFIG_HARDWARE_ONLY) {	//Hardware wraparound
+			PINFO("Hardware wraparound.\n");
+			ctrl |= ME4600_AO_MODE_WRAPAROUND;
+			instance->mode = ME4600_AO_HW_WRAP_MODE;
+		} else {	//Software wraparound
+			PINFO("Software wraparound.\n");
+			ctrl |= ME4600_AO_MODE_CONTINUOUS;
+			instance->mode = ME4600_AO_SW_WRAP_MODE;
+		}
+	} else {		//Continous
+		PINFO("Continous.\n");
+		ctrl |= ME4600_AO_MODE_CONTINUOUS;
+		instance->mode = ME4600_AO_CONTINOUS;
+	}
+
+	//Set the trigger edge.
+	if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_EXT_DIGITAL) {	//Set the trigger type and edge for external trigger.
+		PINFO("External digital trigger.\n");
+		instance->start_mode = ME4600_AO_EXT_TRIG;
+/*
+			ctrl |= ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+*/
+		switch (trigger->iAcqStartTrigEdge) {
+		case ME_TRIG_EDGE_RISING:
+			PINFO("Set the trigger edge: rising.\n");
+			instance->ctrl_trg = 0x0;
+			break;
+
+		case ME_TRIG_EDGE_FALLING:
+			PINFO("Set the trigger edge: falling.\n");
+//                                      ctrl |= ME4600_AO_CTRL_BIT_EX_TRIG_EDGE;
+			instance->ctrl_trg = ME4600_AO_CTRL_BIT_EX_TRIG_EDGE;
+			break;
+
+		case ME_TRIG_EDGE_ANY:
+			PINFO("Set the trigger edge: both edges.\n");
+//                                      ctrl |= ME4600_AO_CTRL_BIT_EX_TRIG_EDGE | ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+			instance->ctrl_trg =
+			    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE |
+			    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+			break;
+		}
+	} else {
+		PINFO("Internal software trigger.\n");
+		instance->start_mode = 0;
+	}
+
+	//Set the stop mode and value.
+	if (trigger->iAcqStopTrigType == ME_TRIG_TYPE_COUNT) {	//Amount of data
+		instance->stop_mode = ME4600_AO_ACQ_STOP_MODE;
+		instance->stop_count = trigger->iAcqStopCount;
+	} else if (trigger->iScanStopTrigType == ME_TRIG_TYPE_COUNT) {	//Amount of 'scans'
+		instance->stop_mode = ME4600_AO_SCAN_STOP_MODE;
+		instance->stop_count = trigger->iScanStopCount;
+	} else {		//Infinite
+		instance->stop_mode = ME4600_AO_INF_STOP_MODE;
+		instance->stop_count = 0;
+	}
+
+	PINFO("Stop count: %d.\n", instance->stop_count);
+
+	if (trigger->iAcqStartTrigChan == ME_TRIG_CHAN_SYNCHRONOUS) {	//Synchronous start
+		instance->start_mode |= ME4600_AO_SYNC_HOLD;
+		if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_EXT_DIGITAL) {	//Externaly triggered
+			PINFO("Synchronous start. Externaly trigger active.\n");
+			instance->start_mode |= ME4600_AO_SYNC_EXT_TRIG;
+		}
+#ifdef MEDEBUG_INFO
+		else {
+			PINFO
+			    ("Synchronous start. Externaly trigger dissabled.\n");
+		}
+#endif
+
+	}
+	//Set speed
+	outl(conv_ticks - 2, instance->timer_reg);
+	PDEBUG_REG("timer_reg outl(0x%lX+0x%lX)=0x%llx\n", instance->reg_base,
+		   instance->timer_reg - instance->reg_base, conv_ticks - 2);
+	instance->hardware_stop_delay = (int)(conv_ticks * HZ) / ME4600_AO_BASE_FREQUENCY;	//<== MUST be with cast!
+
+	//Conect outputs to analog or digital port.
+	if (flags & ME_IO_STREAM_CONFIG_BIT_PATTERN) {
+		ctrl |= ME4600_AO_CTRL_BIT_ENABLE_DO;
+	}
+	// Write the control word
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+
+	//Set status.
+	instance->status = ao_status_stream_configured;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_new_values(me_subdevice_t * subdevice,
+					  struct file *filep,
+					  int time_out, int *count, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	long j;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!instance->circ_buf.buf) {
+		PERROR("Circular buffer not exists.\n");
+		return ME_ERRNO_INTERNAL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (me_circ_buf_space(&instance->circ_buf)) {	//The buffer is NOT full.
+		*count = me_circ_buf_space(&instance->circ_buf);
+	} else {		//The buffer is full.
+		if (time_out) {
+			t = (time_out * HZ) / 1000;
+
+			if (t == 0)
+				t = 1;
+		} else {	//Max time.
+			t = LONG_MAX;
+		}
+
+		*count = 0;
+
+		j = jiffies;
+
+		//Only runing process will interrupt this call. Interrupts are when FIFO HF is signaled.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 ((me_circ_buf_space
+						   (&instance->circ_buf))
+						  || !(inl(instance->status_reg)
+						       &
+						       ME4600_AO_STATUS_BIT_FSM)),
+						 t);
+
+		if (!(inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM)) {
+			PERROR("AO subdevice is not running.\n");
+			err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+		} else if (signal_pending(current)) {
+			PERROR("Wait on values interrupted from signal.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		} else if ((jiffies - j) >= t) {
+			PERROR("Wait on values timed out.\n");
+			err = ME_ERRNO_TIMEOUT;
+		} else {	//Uff... all is good. Inform user about empty space.
+			*count = me_circ_buf_space(&instance->circ_buf);
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_start(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int start_mode, int time_out, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags = 0;
+	uint32_t status;
+	uint32_t ctrl;
+	uint32_t synch;
+	int count = 0;
+	int circ_buffer_count;
+
+	unsigned long ref;
+	unsigned long delay = 0;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags & ~ME_IO_STREAM_START_TYPE_TRIG_SYNCHRONOUS) {
+		PERROR("Invalid flags.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if ((start_mode != ME_START_MODE_BLOCKING)
+	    && (start_mode != ME_START_MODE_NONBLOCKING)) {
+		PERROR("Invalid start mode specified.\n");
+		return ME_ERRNO_INVALID_START_MODE;
+	}
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+		if (delay == 0)
+			delay = 1;
+	}
+
+	switch (instance->status) {	//Checking actual mode.
+	case ao_status_stream_configured:
+	case ao_status_stream_end:
+		//Correct modes!
+		break;
+
+		//The device is in wrong mode.
+	case ao_status_none:
+	case ao_status_single_configured:
+	case ao_status_single_run_wait:
+	case ao_status_single_run:
+	case ao_status_single_end_wait:
+		PERROR
+		    ("Subdevice must be preinitialize correctly for streaming.\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+
+	case ao_status_stream_fifo_error:
+	case ao_status_stream_buffer_error:
+	case ao_status_stream_error:
+		PDEBUG("Before restart broke stream 'STOP' must be caled.\n");
+		return ME_STATUS_ERROR;
+
+	case ao_status_stream_run_wait:
+	case ao_status_stream_run:
+	case ao_status_stream_end_wait:
+		PDEBUG("Stream is already working.\n");
+		return ME_ERRNO_SUBDEVICE_BUSY;
+
+	default:
+		instance->status = ao_status_stream_error;
+		PERROR_CRITICAL("Status is in wrong state!\n");
+		return ME_ERRNO_INTERNAL;
+
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->mode == ME4600_AO_CONTINOUS) {	//Continous
+		instance->circ_buf.tail += instance->preloaded_count;
+		instance->circ_buf.tail &= instance->circ_buf.mask;
+	}
+	circ_buffer_count = me_circ_buf_values(&instance->circ_buf);
+
+	if (!circ_buffer_count && !instance->preloaded_count) {	//No values in buffer
+		ME_SUBDEVICE_EXIT;
+		PERROR("No values in buffer!\n");
+		return ME_ERRNO_LACK_OF_RESOURCES;
+	}
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+
+	//Stop device
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+	//Set values for single_read()
+	instance->single_value = ME4600_AO_MAX_DATA + 1;
+	instance->single_value_in_fifo = ME4600_AO_MAX_DATA + 1;
+
+	//Setting stop points
+	if (instance->stop_mode == ME4600_AO_SCAN_STOP_MODE) {
+		instance->stop_data_count =
+		    instance->stop_count * circ_buffer_count;
+	} else {
+		instance->stop_data_count = instance->stop_count;
+	}
+
+	if ((instance->stop_data_count != 0)
+	    && (instance->stop_data_count < circ_buffer_count)) {
+		PERROR("More data in buffer than previously set limit!\n");
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	ctrl = inl(instance->ctrl_reg);
+	//Check FIFO
+	if (!(ctrl & ME4600_AO_CTRL_BIT_ENABLE_FIFO)) {	//FIFO wasn't enabeled. Do it. <= This should be done by user call with ME_WRITE_MODE_PRELOAD
+		PINFO("Enableing FIFO.\n");
+		ctrl |=
+		    ME4600_AO_CTRL_BIT_ENABLE_FIFO |
+		    ME4600_AO_CTRL_BIT_RESET_IRQ;
+
+		instance->preloaded_count = 0;
+		instance->data_count = 0;
+	} else {		//Block IRQ
+		ctrl |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+	}
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base,
+		   ctrl | ME4600_AO_CTRL_BIT_RESET_IRQ);
+
+	//Fill FIFO <= Generaly this should be done by user pre-load call but this is second place to do it.
+	status = inl(instance->status_reg);
+	if (!(status & ME4600_AO_STATUS_BIT_EF)) {	//FIFO empty
+		if (instance->stop_data_count == 0) {
+			count = ME4600_AO_FIFO_COUNT;
+		} else {
+			count =
+			    (ME4600_AO_FIFO_COUNT <
+			     instance->
+			     stop_data_count) ? ME4600_AO_FIFO_COUNT :
+			    instance->stop_data_count;
+		}
+
+		//Copy data
+		count =
+		    ao_write_data(instance, count, instance->preloaded_count);
+
+		if (count < 0) {	//This should never happend!
+			PERROR_CRITICAL("COPY FINISH WITH ERROR!\n");
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+	}
+	//Set pre-load features.
+	spin_lock(instance->preload_reg_lock);
+	synch = inl(instance->preload_reg);
+	synch &=
+	    ~((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->
+	      ao_idx);
+	synch |=
+	    (instance->start_mode & ~ME4600_AO_EXT_TRIG) << instance->ao_idx;
+	outl(synch, instance->preload_reg);
+	PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, synch);
+	spin_unlock(instance->preload_reg_lock);
+
+	//Default count is '0'
+	if (instance->mode == ME4600_AO_CONTINOUS) {	//Continous
+		instance->preloaded_count = 0;
+		instance->circ_buf.tail += count;
+		instance->circ_buf.tail &= instance->circ_buf.mask;
+	} else {		//Wraparound
+		instance->preloaded_count += count;
+		instance->data_count += count;
+
+		//Special case: Infinite wraparound with less than FIFO datas always should runs in hardware mode.
+		if ((instance->stop_mode == ME4600_AO_INF_STOP_MODE)
+		    && (circ_buffer_count <= ME4600_AO_FIFO_COUNT)) {	//Change to hardware wraparound
+			PDEBUG
+			    ("Changeing mode from software wraparound to hardware wraparound.\n");
+			//Copy all data
+			count =
+			    ao_write_data(instance, circ_buffer_count,
+					  instance->preloaded_count);
+			ctrl &= ~ME4600_AO_CTRL_MODE_MASK;
+			ctrl |= ME4600_AO_MODE_WRAPAROUND;
+		}
+
+		if (instance->preloaded_count == me_circ_buf_values(&instance->circ_buf)) {	//Reset position indicator.
+			instance->preloaded_count = 0;
+		} else if (instance->preloaded_count > me_circ_buf_values(&instance->circ_buf)) {	//This should never happend!
+			PERROR_CRITICAL
+			    ("PRELOADED MORE VALUES THAN ARE IN BUFFER!\n");
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+	}
+
+	//Set status to 'wait for start'
+	instance->status = ao_status_stream_run_wait;
+
+	status = inl(instance->status_reg);
+	//Start state machine and interrupts
+	ctrl &= ~(ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+	if (instance->start_mode == ME4600_AO_EXT_TRIG) {	// External trigger.
+		PINFO("External trigger.\n");
+		ctrl |= ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+	}
+	if (!(status & ME4600_AO_STATUS_BIT_HF)) {	//More than half!
+		if ((ctrl & ME4600_AO_CTRL_MODE_MASK) == ME4600_AO_MODE_CONTINUOUS) {	//Enable IRQ only when hardware_continous is set and FIFO is more than half
+			ctrl &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+			ctrl |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+		}
+	}
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	//Trigger output
+	if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {	//Trigger outputs
+		spin_lock(instance->preload_reg_lock);
+		synch = inl(instance->preload_reg);
+		//Add channel to start list
+		outl(synch | (ME4600_AO_SYNC_HOLD << instance->ao_idx),
+		     instance->preload_reg);
+		PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->preload_reg - instance->reg_base,
+			   synch | (ME4600_AO_SYNC_HOLD << instance->ao_idx));
+
+		//Fire
+		PINFO
+		    ("Fired all software synchronous outputs by software trigger.\n");
+		outl(0x8000, instance->single_reg);
+		PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->single_reg - instance->reg_base, 0x8000);
+
+		//Restore save settings
+		outl(synch, instance->preload_reg);
+		PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->preload_reg - instance->reg_base, synch);
+		spin_unlock(instance->preload_reg_lock);
+	} else if (!instance->start_mode) {	//Trigger outputs
+/*
+		//Remove channel from start list.	// <== Unnecessary. Removed.
+		spin_lock(instance->preload_reg_lock);
+			synch = inl(instance->preload_reg);
+			outl(synch & ~(ME4600_AO_SYNC_HOLD << instance->ao_idx), instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, synch & ~(ME4600_AO_SYNC_HOLD << instance->ao_idx));
+*/
+		//Fire
+		PINFO("Software trigger.\n");
+		outl(0x8000, instance->single_reg);
+		PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->single_reg - instance->reg_base, 0x8000);
+
+/*
+			//Restore save settings.	// <== Unnecessary. Removed.
+			outl(synch, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, synch);
+		spin_unlock(instance->preload_reg_lock);
+*/
+	}
+	// Set control task's timeout
+	ref = jiffies;
+	instance->timeout.delay = delay;
+	instance->timeout.start_time = ref;
+
+	if (status & ME4600_AO_STATUS_BIT_HF) {	//Less than half but not empty!
+		PINFO("Less than half.\n");
+		if (instance->stop_data_count != 0) {
+			count = ME4600_AO_FIFO_COUNT / 2;
+		} else {
+			count =
+			    ((ME4600_AO_FIFO_COUNT / 2) <
+			     instance->stop_data_count) ? ME4600_AO_FIFO_COUNT /
+			    2 : instance->stop_data_count;
+		}
+
+		//Copy data
+		count =
+		    ao_write_data(instance, count, instance->preloaded_count);
+
+		if (count < 0) {	//This should never happend!
+			PERROR_CRITICAL("COPY FINISH WITH ERROR!\n");
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+
+		if (instance->mode == ME4600_AO_CONTINOUS) {	//Continous
+			instance->circ_buf.tail += count;
+			instance->circ_buf.tail &= instance->circ_buf.mask;
+		} else {	//Wraparound
+			instance->data_count += count;
+			instance->preloaded_count += count;
+
+			if (instance->preloaded_count == me_circ_buf_values(&instance->circ_buf)) {	//Reset position indicator.
+				instance->preloaded_count = 0;
+			} else if (instance->preloaded_count > me_circ_buf_values(&instance->circ_buf)) {	//This should never happend!
+				PERROR_CRITICAL
+				    ("PRELOADED MORE VALUES THAN ARE IN BUFFER!\n");
+				ME_SUBDEVICE_EXIT;
+				return ME_ERRNO_INTERNAL;
+			}
+		}
+
+		status = inl(instance->status_reg);
+		if (!(status & ME4600_AO_STATUS_BIT_HF)) {	//More than half!
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+			ctrl |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		}
+	}
+	//Special case: Limited wraparound with less than HALF FIFO datas need work around to generate first interrupt.
+	if ((instance->stop_mode != ME4600_AO_INF_STOP_MODE)
+	    && (instance->mode == ME4600_AO_SW_WRAP_MODE)
+	    && (circ_buffer_count <= (ME4600_AO_FIFO_COUNT / 2))) {	//Put more data to FIFO
+		PINFO("Limited wraparound with less than HALF FIFO datas.\n");
+		if (instance->preloaded_count) {	//This should never happend!
+			PERROR_CRITICAL
+			    ("ERROR WHEN LOADING VALUES FOR WRAPAROUND!\n");
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+
+		while (instance->stop_data_count > instance->data_count) {	//Maximum data not set jet.
+			//Copy to buffer
+			if (circ_buffer_count != ao_write_data(instance, circ_buffer_count, 0)) {	//This should never happend!
+				PERROR_CRITICAL
+				    ("ERROR WHEN LOADING VALUES FOR WRAPAROUND!\n");
+				ME_SUBDEVICE_EXIT;
+				return ME_ERRNO_INTERNAL;
+			}
+			instance->data_count += circ_buffer_count;
+
+			if (!((status = inl(instance->status_reg)) & ME4600_AO_STATUS_BIT_HF)) {	//FIFO is more than half. Enable IRQ and end copy.
+				spin_lock_irqsave(&instance->subdevice_lock,
+						  cpu_flags);
+				ctrl = inl(instance->ctrl_reg);
+				ctrl &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+				ctrl |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+				outl(ctrl, instance->ctrl_reg);
+				PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->ctrl_reg -
+					   instance->reg_base, ctrl);
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				break;
+			}
+		}
+	}
+	// Schedule control task.
+	instance->ao_control_task_flag = 1;
+	queue_delayed_work(instance->me4600_workqueue,
+			   &instance->ao_control_task, 1);
+
+	if (start_mode == ME_START_MODE_BLOCKING) {	//Wait for start.
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_stream_run_wait),
+						 (delay) ? delay +
+						 1 : LONG_MAX);
+
+		if ((instance->status != ao_status_stream_run)
+		    && (instance->status != ao_status_stream_end)) {
+			PDEBUG("Starting stream canceled. %d\n",
+			       instance->status);
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		} else if ((delay) && ((jiffies - ref) >= delay)) {
+			if (instance->status != ao_status_stream_run) {
+				if (instance->status == ao_status_stream_end) {
+					PDEBUG("Timeout reached.\n");
+				} else {
+					if ((jiffies - ref) > delay) {
+						PERROR
+						    ("Timeout reached. Not handled by control task!\n");
+					} else {
+						PERROR
+						    ("Timeout reached. Signal come but status is strange: %d\n",
+						     instance->status);
+					}
+					ao_stop_immediately(instance);
+				}
+
+				instance->ao_control_task_flag = 0;
+				cancel_delayed_work(&instance->ao_control_task);
+				instance->status = ao_status_stream_end;
+				err = ME_ERRNO_TIMEOUT;
+			}
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+	return err;
+}
+
+static int me4600_ao_io_stream_status(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int wait,
+				      int *status, int *values, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((wait != ME_WAIT_NONE) && (wait != ME_WAIT_IDLE)) {
+		PERROR("Invalid wait argument specified.\n");
+		*status = ME_STATUS_INVALID;
+		return ME_ERRNO_INVALID_WAIT;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	switch (instance->status) {
+	case ao_status_single_configured:
+	case ao_status_single_end:
+	case ao_status_stream_configured:
+	case ao_status_stream_end:
+	case ao_status_stream_fifo_error:
+	case ao_status_stream_buffer_error:
+	case ao_status_stream_error:
+		*status = ME_STATUS_IDLE;
+		break;
+
+	case ao_status_single_run_wait:
+	case ao_status_single_run:
+	case ao_status_single_end_wait:
+	case ao_status_stream_run_wait:
+	case ao_status_stream_run:
+	case ao_status_stream_end_wait:
+		*status = ME_STATUS_BUSY;
+		break;
+
+	case ao_status_none:
+	default:
+		*status =
+		    (inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM) ?
+		    ME_STATUS_BUSY : ME_STATUS_IDLE;
+		break;
+	}
+
+	if ((wait == ME_WAIT_IDLE) && (*status == ME_STATUS_BUSY)) {
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 ((instance->status !=
+						   ao_status_single_run_wait)
+						  && (instance->status !=
+						      ao_status_single_run)
+						  && (instance->status !=
+						      ao_status_single_end_wait)
+						  && (instance->status !=
+						      ao_status_stream_run_wait)
+						  && (instance->status !=
+						      ao_status_stream_run)
+						  && (instance->status !=
+						      ao_status_stream_end_wait)),
+						 LONG_MAX);
+
+		if (instance->status != ao_status_stream_end) {
+			PDEBUG("Wait for IDLE canceled. %d\n",
+			       instance->status);
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait for IDLE interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		*status = ME_STATUS_IDLE;
+	}
+
+	*values = me_circ_buf_space(&instance->circ_buf);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_stop(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int stop_mode, int flags)
+{				// Stop work and empty buffer and FIFO
+	int err = ME_ERRNO_SUCCESS;
+	me4600_ao_subdevice_t *instance;
+	unsigned long cpu_flags;
+	volatile uint32_t ctrl;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags & ~ME_IO_STREAM_STOP_PRESERVE_BUFFERS) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((stop_mode != ME_STOP_MODE_IMMEDIATE)
+	    && (stop_mode != ME_STOP_MODE_LAST_VALUE)) {
+		PERROR("Invalid stop mode specified.\n");
+		return ME_ERRNO_INVALID_STOP_MODE;
+	}
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (instance->status < ao_status_stream_configured) {
+		//There is nothing to stop!
+		PERROR("Subdevice not in streaming mode. %d\n",
+		       instance->status);
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	//Mark as stopping. => Software stop.
+	instance->status = ao_status_stream_end_wait;
+
+	if (stop_mode == ME_STOP_MODE_IMMEDIATE) {	//Stopped now!
+		err = ao_stop_immediately(instance);
+	} else if (stop_mode == ME_STOP_MODE_LAST_VALUE) {
+		ctrl = inl(instance->ctrl_reg) & ME4600_AO_CTRL_MODE_MASK;
+		if (ctrl == ME4600_AO_MODE_WRAPAROUND) {	//Hardware wraparound => Hardware stop.
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |=
+			    ME4600_AO_CTRL_BIT_STOP |
+			    ME4600_AO_CTRL_BIT_RESET_IRQ;
+			ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		}
+		//Only runing process will interrupt this call. Events are signaled when status change.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_stream_end_wait),
+						 LONG_MAX);
+
+		if (instance->status != ao_status_stream_end) {
+			PDEBUG("Stopping stream canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Stopping stream interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	ctrl = inl(instance->ctrl_reg);
+	ctrl |=
+	    ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP |
+	    ME4600_AO_CTRL_BIT_RESET_IRQ;
+	ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+	if (!flags) {		//Reset FIFO
+		ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_FIFO;
+	}
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	if (!flags) {		//Reset software buffer
+		instance->circ_buf.head = 0;
+		instance->circ_buf.tail = 0;
+		instance->preloaded_count = 0;
+		instance->data_count = 0;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int write_mode,
+				     int *values, int *count, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me4600_ao_subdevice_t *instance;
+	unsigned long cpu_flags = 0;
+	uint32_t reg_copy;
+
+	int copied_from_user = 0;
+	int left_to_copy_from_user = *count;
+
+	int copied_values;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	//Checking arguments
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (*count <= 0) {
+		PERROR("Invalid count of values specified.\n");
+		return ME_ERRNO_INVALID_VALUE_COUNT;
+	}
+
+	if (values == NULL) {
+		PERROR("Invalid address of values specified.\n");
+		return ME_ERRNO_INVALID_POINTER;
+	}
+
+	if ((instance->status == ao_status_none) || (instance->status == ao_status_single_configured)) {	//The device is in single mode.
+		PERROR
+		    ("Subdevice must be preinitialize correctly for streaming.\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+/// @note If no 'pre-load' is used. stream_start() will move data to FIFO.
+	switch (write_mode) {
+	case ME_WRITE_MODE_PRELOAD:
+
+		//Device must be stopped.
+		if ((instance->status != ao_status_stream_configured)
+		    && (instance->status != ao_status_stream_end)) {
+			PERROR
+			    ("Subdevice mustn't be runing when 'pre-load' mode is used.\n");
+			return ME_ERRNO_PREVIOUS_CONFIG;
+		}
+		break;
+	case ME_WRITE_MODE_NONBLOCKING:
+	case ME_WRITE_MODE_BLOCKING:
+		/// @note In blocking mode: When device is not runing and there is not enought space call will blocked up!
+		/// @note Some other thread must empty buffer by starting engine.
+		break;
+
+	default:
+		PERROR("Invalid write mode specified.\n");
+		return ME_ERRNO_INVALID_WRITE_MODE;
+	}
+
+	if (instance->mode & ME4600_AO_WRAP_MODE) {	//Wraparound mode. Device must be stopped.
+		if ((instance->status != ao_status_stream_configured)
+		    && (instance->status != ao_status_stream_end)) {
+			PERROR
+			    ("Subdevice mustn't be runing when 'pre-load' mode is used.\n");
+			return ME_ERRNO_INVALID_WRITE_MODE;
+		}
+	}
+
+	if ((instance->mode == ME4600_AO_HW_WRAP_MODE) && (write_mode != ME_WRITE_MODE_PRELOAD)) {	// hardware wrap_around mode.
+		//This is transparent for user.
+		PDEBUG("Changing write_mode to ME_WRITE_MODE_PRELOAD.\n");
+		write_mode = ME_WRITE_MODE_PRELOAD;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (write_mode == ME_WRITE_MODE_PRELOAD) {	//Init enviroment - preload
+		spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+		reg_copy = inl(instance->ctrl_reg);
+		//Check FIFO
+		if (!(reg_copy & ME4600_AO_CTRL_BIT_ENABLE_FIFO)) {	//FIFO not active. Enable it.
+			reg_copy |= ME4600_AO_CTRL_BIT_ENABLE_FIFO;
+			outl(reg_copy, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   reg_copy);
+			instance->preloaded_count = 0;
+		}
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	}
+
+	while (1) {
+		//Copy to buffer. This step is common for all modes.
+		copied_from_user =
+		    ao_get_data_from_user(instance, left_to_copy_from_user,
+					  values + (*count -
+						    left_to_copy_from_user));
+		left_to_copy_from_user -= copied_from_user;
+
+		reg_copy = inl(instance->status_reg);
+		if ((instance->status == ao_status_stream_run) && !(reg_copy & ME4600_AO_STATUS_BIT_FSM)) {	//BROKEN PIPE! The state machine is stoped but logical status show that should be working.
+			PERROR("Broken pipe in write.\n");
+			err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+			break;
+		}
+
+		if ((instance->status == ao_status_stream_run) && (instance->mode == ME4600_AO_CONTINOUS) && (reg_copy & ME4600_AO_STATUS_BIT_HF)) {	//Continous mode runing and data are below half!
+
+			// Block interrupts.
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			reg_copy = inl(instance->ctrl_reg);
+			//reg_copy &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			reg_copy |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+			outl(reg_copy, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   reg_copy);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			//Fast copy
+			copied_values =
+			    ao_write_data(instance, ME4600_AO_FIFO_COUNT / 2,
+					  0);
+			if (copied_values > 0) {
+				instance->circ_buf.tail += copied_values;
+				instance->circ_buf.tail &=
+				    instance->circ_buf.mask;
+				continue;
+			}
+			// Activate interrupts.
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			reg_copy = inl(instance->ctrl_reg);
+			//reg_copy |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			reg_copy &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+			outl(reg_copy, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   reg_copy);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if (copied_values == 0) {	//This was checked and never should happend!
+				PERROR_CRITICAL("COPING FINISH WITH 0!\n");
+			}
+
+			if (copied_values < 0) {	//This was checked and never should happend!
+				PERROR_CRITICAL
+				    ("COPING FINISH WITH AN ERROR!\n");
+				instance->status = ao_status_stream_fifo_error;
+				err = ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+				break;
+			}
+		}
+
+		if (!left_to_copy_from_user) {	//All datas were copied.
+			break;
+		} else {	//Not all datas were copied.
+			if (instance->mode & ME4600_AO_WRAP_MODE) {	//Error too much datas! Wraparound is limited in size!
+				PERROR
+				    ("Too much data for wraparound mode!  Exceeded size of %ld.\n",
+				     ME4600_AO_CIRC_BUF_COUNT - 1);
+				err = ME_ERRNO_RING_BUFFER_OVERFLOW;
+				break;
+			}
+
+			if (write_mode != ME_WRITE_MODE_BLOCKING) {	//Non blocking calls
+				break;
+			}
+
+			wait_event_interruptible(instance->wait_queue,
+						 me_circ_buf_space(&instance->
+								   circ_buf));
+
+			if (signal_pending(current)) {
+				PERROR("Writing interrupted by signal.\n");
+				instance->status = ao_status_none;
+				ao_stop_immediately(instance);
+				err = ME_ERRNO_SIGNAL;
+				break;
+			}
+
+			if (instance->status == ao_status_none) {	//Reset
+				PERROR("Writing interrupted by reset.\n");
+				err = ME_ERRNO_CANCELLED;
+				break;
+			}
+		}
+	}
+
+	if (write_mode == ME_WRITE_MODE_PRELOAD) {	//Copy data to FIFO - preload
+		copied_values =
+		    ao_write_data_pooling(instance, ME4600_AO_FIFO_COUNT,
+					  instance->preloaded_count);
+		instance->preloaded_count += copied_values;
+		instance->data_count += copied_values;
+
+		if ((instance->mode == ME4600_AO_HW_WRAP_MODE)
+		    && (me_circ_buf_values(&instance->circ_buf) >
+			ME4600_AO_FIFO_COUNT)) {
+			PERROR
+			    ("Too much data for hardware wraparound mode! Exceeded size of %d.\n",
+			     ME4600_AO_FIFO_COUNT);
+			err = ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+		}
+	}
+
+	*count = *count - left_to_copy_from_user;
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+static irqreturn_t me4600_ao_isr(int irq, void *dev_id
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 19)
+				 , struct pt_regs *regs
+#endif
+    )
+{
+	me4600_ao_subdevice_t *instance = dev_id;
+	uint32_t irq_status;
+	uint32_t ctrl;
+	uint32_t status;
+	int count = 0;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	irq_status = inl(instance->irq_status_reg);
+	if (!(irq_status & (ME4600_IRQ_STATUS_BIT_AO_HF << instance->ao_idx))) {
+		PINFO("%ld Shared interrupt. %s(): ID=%d: status_reg=0x%04X\n",
+		      jiffies, __func__, instance->ao_idx, irq_status);
+		return IRQ_NONE;
+	}
+
+	if (!instance->circ_buf.buf) {
+		instance->status = ao_status_stream_error;
+		PERROR_CRITICAL("CIRCULAR BUFFER NOT EXISTS!\n");
+		//Block interrupts. Stop machine.
+		ctrl = inl(instance->ctrl_reg);
+		ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+		ctrl |=
+		    ME4600_AO_CTRL_BIT_RESET_IRQ |
+		    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP | ME4600_AO_CTRL_BIT_STOP;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		//Inform user
+		wake_up_interruptible_all(&instance->wait_queue);
+		return IRQ_HANDLED;
+	}
+
+	status = inl(instance->status_reg);
+	if (!(status & ME4600_AO_STATUS_BIT_FSM)) {	//Too late. Not working! END? BROKEN PIPE?
+		PDEBUG("Interrupt come but ISM is not working!\n");
+		//Block interrupts. Stop machine.
+		ctrl = inl(instance->ctrl_reg);
+		ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+		ctrl |=
+		    ME4600_AO_CTRL_BIT_RESET_IRQ | ME4600_AO_CTRL_BIT_STOP |
+		    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		return IRQ_HANDLED;
+	}
+	//General procedure. Process more datas.
+
+#ifdef MEDEBUG_DEBUG
+	if (!me_circ_buf_values(&instance->circ_buf)) {	//Buffer is empty!
+		PDEBUG("Circular buffer empty!\n");
+	}
+#endif
+
+	//Check FIFO
+	if (status & ME4600_AO_STATUS_BIT_HF) {	//OK less than half
+
+		//Block interrupts
+		ctrl = inl(instance->ctrl_reg);
+		ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+		ctrl |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		do {
+			//Calculate how many should be copied.
+			count =
+			    (instance->stop_data_count) ? instance->
+			    stop_data_count -
+			    instance->data_count : ME4600_AO_FIFO_COUNT / 2;
+			if (ME4600_AO_FIFO_COUNT / 2 < count) {
+				count = ME4600_AO_FIFO_COUNT / 2;
+			}
+			//Copy data
+			if (instance->mode == ME4600_AO_CONTINOUS) {	//Continous
+				count = ao_write_data(instance, count, 0);
+				if (count > 0) {
+					instance->circ_buf.tail += count;
+					instance->circ_buf.tail &=
+					    instance->circ_buf.mask;
+					instance->data_count += count;
+
+					if ((instance->status == ao_status_stream_end_wait) && !me_circ_buf_values(&instance->circ_buf)) {	//Stoping. Whole buffer was copied.
+						break;
+					}
+				}
+			} else if ((instance->mode == ME4600_AO_SW_WRAP_MODE) && ((ctrl & ME4600_AO_CTRL_MODE_MASK) == ME4600_AO_MODE_CONTINUOUS)) {	//Wraparound (software)
+				if (instance->status == ao_status_stream_end_wait) {	//We stoping => Copy to the end of the buffer.
+					count =
+					    ao_write_data(instance, count, 0);
+				} else {	//Copy in wraparound mode.
+					count =
+					    ao_write_data_wraparound(instance,
+								     count,
+								     instance->
+								     preloaded_count);
+				}
+
+				if (count > 0) {
+					instance->data_count += count;
+					instance->preloaded_count += count;
+					instance->preloaded_count %=
+					    me_circ_buf_values(&instance->
+							       circ_buf);
+
+					if ((instance->status == ao_status_stream_end_wait) && !instance->preloaded_count) {	//Stoping. Whole buffer was copied.
+						break;
+					}
+				}
+			}
+
+			if ((count <= 0) || (instance->stop_data_count && (instance->stop_data_count <= instance->data_count))) {	//End of work.
+				break;
+			}
+		}		//Repeat if still is under half fifo
+		while ((status =
+			inl(instance->status_reg)) & ME4600_AO_STATUS_BIT_HF);
+
+		//Unblock interrupts
+		ctrl = inl(instance->ctrl_reg);
+		if (count >= 0) {	//Copy was successful.
+			if (instance->stop_data_count && (instance->stop_data_count <= instance->data_count)) {	//Finishing work. No more interrupts.
+				PDEBUG("Finishing work. Interrupt disabled.\n");
+				instance->status = ao_status_stream_end_wait;
+			} else if (count > 0) {	//Normal work. Enable interrupt.
+				PDEBUG("Normal work. Enable interrupt.\n");
+				ctrl &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+				ctrl |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			} else {	//Normal work but there are no more data in buffer. Interrupt active but blocked. stream_write() will unblock it.
+				PDEBUG
+				    ("No data in software buffer. Interrupt blocked.\n");
+				ctrl |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			}
+		} else {	//Error during copy.
+			instance->status = ao_status_stream_fifo_error;
+		}
+
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+	} else {		//?? more than half
+		PDEBUG
+		    ("Interrupt come but FIFO more than half full! Reset interrupt.\n");
+		//Reset pending interrupt
+		ctrl = inl(instance->ctrl_reg);
+		ctrl |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		ctrl &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+	}
+
+	PINFO("ISR: Buffer count: %d.(T:%d H:%d)\n",
+	      me_circ_buf_values(&instance->circ_buf), instance->circ_buf.tail,
+	      instance->circ_buf.head);
+	PINFO("ISR: Stop count: %d.\n", instance->stop_count);
+	PINFO("ISR: Stop data count: %d.\n", instance->stop_data_count);
+	PINFO("ISR: Data count: %d.\n", instance->data_count);
+
+	//Inform user
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+static void me4600_ao_destructor(struct me_subdevice *subdevice)
+{
+	me4600_ao_subdevice_t *instance;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	instance->ao_control_task_flag = 0;
+
+	// Reset subdevice to asure clean exit.
+	me4600_ao_io_reset_subdevice(subdevice, NULL,
+				     ME_IO_RESET_SUBDEVICE_NO_FLAGS);
+
+	// Remove any tasks from work queue. This is paranoic because it was done allready in reset().
+	if (!cancel_delayed_work(&instance->ao_control_task)) {	//Wait 2 ticks to be sure that control task is removed from queue.
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(2);
+	}
+
+	if (instance->fifo) {
+		if (instance->irq) {
+			free_irq(instance->irq, instance);
+			instance->irq = 0;
+		}
+
+		if (instance->circ_buf.buf) {
+			free_pages((unsigned long)instance->circ_buf.buf,
+				   ME4600_AO_CIRC_BUF_SIZE_ORDER);
+		}
+		instance->circ_buf.buf = NULL;
+	}
+
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+me4600_ao_subdevice_t *me4600_ao_constructor(uint32_t reg_base,
+					     spinlock_t * preload_reg_lock,
+					     uint32_t * preload_flags,
+					     int ao_idx,
+					     int fifo,
+					     int irq,
+					     struct workqueue_struct *me4600_wq)
+{
+	me4600_ao_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed. idx=%d\n", ao_idx);
+
+	// Allocate memory for subdevice instance.
+	subdevice = kmalloc(sizeof(me4600_ao_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me4600_ao_subdevice_t));
+
+	// Initialize subdevice base class.
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->preload_reg_lock = preload_reg_lock;
+	subdevice->preload_flags = preload_flags;
+
+	// Store analog output index.
+	subdevice->ao_idx = ao_idx;
+
+	// Store if analog output has fifo.
+	subdevice->fifo = (ao_idx < fifo) ? 1 : 0;
+
+	if (subdevice->fifo) {	// Allocate and initialize circular buffer.
+		subdevice->circ_buf.mask = ME4600_AO_CIRC_BUF_COUNT - 1;
+
+		subdevice->circ_buf.buf =
+		    (void *)__get_free_pages(GFP_KERNEL,
+					     ME4600_AO_CIRC_BUF_SIZE_ORDER);
+		PDEBUG("circ_buf = %p size=%ld\n", subdevice->circ_buf.buf,
+		       ME4600_AO_CIRC_BUF_SIZE);
+
+		if (!subdevice->circ_buf.buf) {
+			PERROR
+			    ("Cannot initialize subdevice base class instance.\n");
+			kfree(subdevice);
+			return NULL;
+		}
+
+		memset(subdevice->circ_buf.buf, 0, ME4600_AO_CIRC_BUF_SIZE);
+	} else {		// No FIFO.
+		subdevice->circ_buf.mask = 0;
+		subdevice->circ_buf.buf = NULL;
+	}
+
+	subdevice->circ_buf.head = 0;
+	subdevice->circ_buf.tail = 0;
+
+	subdevice->status = ao_status_none;
+	subdevice->ao_control_task_flag = 0;
+	subdevice->timeout.delay = 0;
+	subdevice->timeout.start_time = jiffies;
+
+	// Initialize wait queue.
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	// Initialize single value to 0V.
+	subdevice->single_value = 0x8000;
+	subdevice->single_value_in_fifo = 0x8000;
+
+	// Register interrupt service routine.
+	if (subdevice->fifo) {
+		subdevice->irq = irq;
+		if (request_irq(subdevice->irq, me4600_ao_isr,
+#ifdef IRQF_DISABLED
+				IRQF_DISABLED | IRQF_SHARED,
+#else
+				SA_INTERRUPT | SA_SHIRQ,
+#endif
+				ME4600_NAME, subdevice)) {
+			PERROR("Cannot get interrupt line.\n");
+			PDEBUG("free circ_buf = %p size=%d",
+			       subdevice->circ_buf.buf,
+			       PAGE_SHIFT << ME4600_AO_CIRC_BUF_SIZE_ORDER);
+			free_pages((unsigned long)subdevice->circ_buf.buf,
+				   ME4600_AO_CIRC_BUF_SIZE_ORDER);
+			me_subdevice_deinit((me_subdevice_t *) subdevice);
+			kfree(subdevice);
+			return NULL;
+		}
+		PINFO("Registered irq=%d.\n", subdevice->irq);
+	} else {
+		subdevice->irq = 0;
+	}
+
+	// Initialize registers.
+	subdevice->irq_status_reg = reg_base + ME4600_IRQ_STATUS_REG;
+	subdevice->preload_reg = reg_base + ME4600_AO_SYNC_REG;
+	if (ao_idx == 0) {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_00_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_00_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_00_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_00_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_00_TIMER_REG;
+		subdevice->reg_base = reg_base;
+		subdevice->bitpattern = 0;
+	} else if (ao_idx == 1) {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_01_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_01_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_01_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_01_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_01_TIMER_REG;
+		subdevice->reg_base = reg_base;
+		subdevice->bitpattern = 0;
+	} else if (ao_idx == 2) {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_02_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_02_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_02_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_02_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_02_TIMER_REG;
+		subdevice->reg_base = reg_base;
+		subdevice->bitpattern = 0;
+	} else if (ao_idx == 3) {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_03_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_03_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_03_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_03_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_03_TIMER_REG;
+		subdevice->reg_base = reg_base;
+		subdevice->bitpattern = 1;
+	} else {
+		PERROR_CRITICAL("WRONG SUBDEVICE idx=%d!", ao_idx);
+		me_subdevice_deinit((me_subdevice_t *) subdevice);
+		if (subdevice->fifo) {
+			free_pages((unsigned long)subdevice->circ_buf.buf,
+				   ME4600_AO_CIRC_BUF_SIZE_ORDER);
+		}
+		subdevice->circ_buf.buf = NULL;
+		kfree(subdevice);
+		return NULL;
+	}
+
+	// Override base class methods.
+	subdevice->base.me_subdevice_destructor = me4600_ao_destructor;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me4600_ao_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me4600_ao_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me4600_ao_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me4600_ao_io_single_write;
+	subdevice->base.me_subdevice_io_stream_config =
+	    me4600_ao_io_stream_config;
+	subdevice->base.me_subdevice_io_stream_new_values =
+	    me4600_ao_io_stream_new_values;
+	subdevice->base.me_subdevice_io_stream_write =
+	    me4600_ao_io_stream_write;
+	subdevice->base.me_subdevice_io_stream_start =
+	    me4600_ao_io_stream_start;
+	subdevice->base.me_subdevice_io_stream_status =
+	    me4600_ao_io_stream_status;
+	subdevice->base.me_subdevice_io_stream_stop = me4600_ao_io_stream_stop;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me4600_ao_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me4600_ao_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me4600_ao_query_subdevice_caps;
+	subdevice->base.me_subdevice_query_subdevice_caps_args =
+	    me4600_ao_query_subdevice_caps_args;
+	subdevice->base.me_subdevice_query_range_by_min_max =
+	    me4600_ao_query_range_by_min_max;
+	subdevice->base.me_subdevice_query_number_ranges =
+	    me4600_ao_query_number_ranges;
+	subdevice->base.me_subdevice_query_range_info =
+	    me4600_ao_query_range_info;
+	subdevice->base.me_subdevice_query_timer = me4600_ao_query_timer;
+
+	// Prepare work queue
+	subdevice->me4600_workqueue = me4600_wq;
+
+/* workqueue API changed in kernel 2.6.20 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20) )
+	INIT_WORK(&subdevice->ao_control_task, me4600_ao_work_control_task,
+		  (void *)subdevice);
+#else
+	INIT_DELAYED_WORK(&subdevice->ao_control_task,
+			  me4600_ao_work_control_task);
+#endif
+
+	if (subdevice->fifo) {	// Set speed for single operations.
+		outl(ME4600_AO_MIN_CHAN_TICKS - 1, subdevice->timer_reg);
+		subdevice->hardware_stop_delay = HZ / 10;	//100ms
+	}
+
+	return subdevice;
+}
+
+/** @brief Stop presentation. Preserve FIFOs.
+*
+* @param instance The subdevice instance (pointer).
+*/
+int inline ao_stop_immediately(me4600_ao_subdevice_t * instance)
+{
+	unsigned long cpu_flags;
+	uint32_t ctrl;
+	int timeout;
+	int i;
+
+	timeout =
+	    (instance->hardware_stop_delay >
+	     (HZ / 10)) ? instance->hardware_stop_delay : HZ / 10;
+	for (i = 0; i <= timeout; i++) {
+		spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+		// Stop all actions. No conditions! Block interrupts. Leave FIFO untouched!
+		ctrl = inl(instance->ctrl_reg);
+		ctrl |=
+		    ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP
+		    | ME4600_AO_CTRL_BIT_RESET_IRQ;
+		ctrl &=
+		    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+		      ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG);
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+		if (!(inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM)) {	// Exit.
+			break;
+		}
+		//Still working!
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(1);
+	}
+
+	if (i > timeout) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		return ME_ERRNO_INTERNAL;
+	}
+	return ME_ERRNO_SUCCESS;
+}
+
+/** @brief Copy data from circular buffer to fifo (fast) in wraparound.
+* @note This is time critical function. Checking is done at begining and end only.
+* @note The is not reasonable way to check how many walues was in FIFO at begining. The count must be managed externaly.
+*
+* @param instance The subdevice instance (pointer).
+* @param count Maximum number of copied data.
+* @param start_pos Position of the firs value in buffer.
+*
+* @return On success: Number of copied data.
+* @return On error/success: 0.	No datas were copied => no data in buffer.
+* @return On error: -ME_ERRNO_FIFO_BUFFER_OVERFLOW.
+*/
+int inline ao_write_data_wraparound(me4600_ao_subdevice_t * instance, int count,
+				    int start_pos)
+{				/// @note This is time critical function!
+	uint32_t status;
+	uint32_t value;
+	int pos =
+	    (instance->circ_buf.tail + start_pos) & instance->circ_buf.mask;
+	int local_count = count;
+	int i = 1;
+
+	if (count <= 0) {	//Wrong count!
+		return 0;
+	}
+
+	while (i < local_count) {
+		//Get value from buffer
+		value = *(instance->circ_buf.buf + pos);
+		//Prepare it
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+
+		pos++;
+		pos &= instance->circ_buf.mask;
+		if (pos == instance->circ_buf.head) {
+			pos = instance->circ_buf.tail;
+		}
+		i++;
+	}
+
+	status = inl(instance->status_reg);
+	if (!(status & ME4600_AO_STATUS_BIT_FF)) {	//FIFO is full before all datas were copied!
+		PERROR("FIFO was full before all datas were copied! idx=%d\n",
+		       instance->ao_idx);
+		return -ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+	} else {		//Add last value
+		value = *(instance->circ_buf.buf + pos);
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+	}
+
+	PINFO("WRAPAROUND LOADED %d values. idx=%d\n", local_count,
+	      instance->ao_idx);
+	return local_count;
+}
+
+/** @brief Copy data from software buffer to fifo (fast).
+* @note This is time critical function. Checking is done at begining and end only.
+* @note The is not reasonable way to check how many walues was in FIFO at begining. The count must be managed externaly.
+*
+* @param instance The subdevice instance (pointer).
+* @param count Maximum number of copied data.
+* @param start_pos Position of the firs value in buffer.
+*
+* @return On success: Number of copied data.
+* @return On error/success: 0.	No datas were copied => no data in buffer.
+* @return On error: -ME_ERRNO_FIFO_BUFFER_OVERFLOW.
+*/
+int inline ao_write_data(me4600_ao_subdevice_t * instance, int count,
+			 int start_pos)
+{				/// @note This is time critical function!
+	uint32_t status;
+	uint32_t value;
+	int pos =
+	    (instance->circ_buf.tail + start_pos) & instance->circ_buf.mask;
+	int local_count = count;
+	int max_count;
+	int i = 1;
+
+	if (count <= 0) {	//Wrong count!
+		return 0;
+	}
+
+	max_count = me_circ_buf_values(&instance->circ_buf) - start_pos;
+	if (max_count <= 0) {	//No data to copy!
+		return 0;
+	}
+
+	if (max_count < count) {
+		local_count = max_count;
+	}
+
+	while (i < local_count) {
+		//Get value from buffer
+		value = *(instance->circ_buf.buf + pos);
+		//Prepare it
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+
+		pos++;
+		pos &= instance->circ_buf.mask;
+		i++;
+	}
+
+	status = inl(instance->status_reg);
+	if (!(status & ME4600_AO_STATUS_BIT_FF)) {	//FIFO is full before all datas were copied!
+		PERROR("FIFO was full before all datas were copied! idx=%d\n",
+		       instance->ao_idx);
+		return -ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+	} else {		//Add last value
+		value = *(instance->circ_buf.buf + pos);
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+	}
+
+	PINFO("FAST LOADED %d values. idx=%d\n", local_count, instance->ao_idx);
+	return local_count;
+}
+
+/** @brief Copy data from software buffer to fifo (slow).
+* @note This is slow function that copy all data from buffer to FIFO with full control.
+*
+* @param instance The subdevice instance (pointer).
+* @param count Maximum number of copied data.
+* @param start_pos Position of the firs value in buffer.
+*
+* @return On success: Number of copied values.
+* @return On error/success: 0.	FIFO was full at begining.
+* @return On error: -ME_ERRNO_RING_BUFFER_UNDEFFLOW.
+*/
+int inline ao_write_data_pooling(me4600_ao_subdevice_t * instance, int count,
+				 int start_pos)
+{				/// @note This is slow function!
+	uint32_t status;
+	uint32_t value;
+	int pos =
+	    (instance->circ_buf.tail + start_pos) & instance->circ_buf.mask;
+	int local_count = count;
+	int i;
+	int max_count;
+
+	if (count <= 0) {	//Wrong count!
+		PERROR("SLOW LOADED: Wrong count! idx=%d\n", instance->ao_idx);
+		return 0;
+	}
+
+	max_count = me_circ_buf_values(&instance->circ_buf) - start_pos;
+	if (max_count <= 0) {	//No data to copy!
+		PERROR("SLOW LOADED: No data to copy! idx=%d\n",
+		       instance->ao_idx);
+		return 0;
+	}
+
+	if (max_count < count) {
+		local_count = max_count;
+	}
+
+	for (i = 0; i < local_count; i++) {
+		status = inl(instance->status_reg);
+		if (!(status & ME4600_AO_STATUS_BIT_FF)) {	//FIFO is full!
+			return i;
+		}
+		//Get value from buffer
+		value = *(instance->circ_buf.buf + pos);
+		//Prepare it
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+
+		pos++;
+		pos &= instance->circ_buf.mask;
+	}
+
+	PINFO("SLOW LOADED %d values. idx=%d\n", local_count, instance->ao_idx);
+	return local_count;
+}
+
+/** @brief Copy data from user space to circular buffer.
+* @param instance The subdevice instance (pointer).
+* @param count Number of datas in user space.
+* @param user_values Buffer's pointer.
+*
+* @return On success: Number of copied values.
+* @return On error: -ME_ERRNO_INTERNAL.
+*/
+int inline ao_get_data_from_user(me4600_ao_subdevice_t * instance, int count,
+				 int *user_values)
+{
+	int i, err;
+	int empty_space;
+	int copied;
+	int value;
+
+	empty_space = me_circ_buf_space(&instance->circ_buf);
+	//We have only this space free.
+	copied = (count < empty_space) ? count : empty_space;
+	for (i = 0; i < copied; i++) {	//Copy from user to buffer
+		if ((err = get_user(value, (int *)(user_values + i)))) {
+			PERROR
+			    ("BUFFER LOADED: get_user(0x%p) return an error: %d. idx=%d\n",
+			     user_values + i, err, instance->ao_idx);
+			return -ME_ERRNO_INTERNAL;
+		}
+		/// @note The analog output in me4600 series has size of 16 bits.
+		*(instance->circ_buf.buf + instance->circ_buf.head) =
+		    (uint16_t) value;
+		instance->circ_buf.head++;
+		instance->circ_buf.head &= instance->circ_buf.mask;
+	}
+
+	PINFO("BUFFER LOADED %d values. idx=%d\n", copied, instance->ao_idx);
+	return copied;
+}
+
+/** @brief Checking actual hardware and logical state.
+* @param instance The subdevice instance (pointer).
+*/
+static void me4600_ao_work_control_task(
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+					       void *subdevice
+#else
+					       struct work_struct *work
+#endif
+    )
+{
+	me4600_ao_subdevice_t *instance;
+	unsigned long cpu_flags = 0;
+	uint32_t status;
+	uint32_t ctrl;
+	uint32_t synch;
+	int reschedule = 0;
+	int signaling = 0;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	instance = (me4600_ao_subdevice_t *) subdevice;
+#else
+	instance =
+	    container_of((void *)work, me4600_ao_subdevice_t, ao_control_task);
+#endif
+	PINFO("<%s: %ld> executed. idx=%d\n", __func__, jiffies,
+	      instance->ao_idx);
+
+	status = inl(instance->status_reg);
+	PDEBUG_REG("status_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->status_reg - instance->reg_base, status);
+
+	switch (instance->status) {	// Checking actual mode.
+
+		// Not configured for work.
+	case ao_status_none:
+		break;
+
+		//This are stable modes. No need to do anything. (?)
+	case ao_status_single_configured:
+	case ao_status_stream_configured:
+	case ao_status_stream_fifo_error:
+	case ao_status_stream_buffer_error:
+	case ao_status_stream_error:
+		PERROR("Shouldn't be running!.\n");
+		break;
+
+	case ao_status_stream_end:
+		if (!instance->fifo) {
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+	case ao_status_single_end:
+		if (status & ME4600_AO_STATUS_BIT_FSM) {	// State machine is working but the status is set to end. Force stop.
+
+			// Wait for stop.
+			reschedule = 1;
+		}
+
+		spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+		// Stop all actions. No conditions! Block interrupts and trigger. Leave FIFO untouched!
+		ctrl = inl(instance->ctrl_reg);
+		ctrl |=
+		    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP | ME4600_AO_CTRL_BIT_STOP
+		    | ME4600_AO_CTRL_BIT_RESET_IRQ;
+		ctrl &=
+		    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+		      ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG);
+		ctrl &=
+		    ~(ME4600_AO_CTRL_BIT_EX_TRIG_EDGE |
+		      ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH);
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+		break;
+
+		// Single modes
+	case ao_status_single_run_wait:
+	case ao_status_single_run:
+	case ao_status_single_end_wait:
+
+		if (!(status & ME4600_AO_STATUS_BIT_FSM)) {	// State machine is not working.
+			if (((instance->fifo)
+			     && (!(status & ME4600_AO_STATUS_BIT_EF)))
+			    || (!(instance->fifo))) {	// Single is in end state.
+				PDEBUG("Single call has been complited.\n");
+
+				// Set correct value for single_read();
+				instance->single_value =
+				    instance->single_value_in_fifo;
+
+				// Set status as 'ao_status_single_end'
+				instance->status = ao_status_single_end;
+
+				// Signal the end.
+				signaling = 1;
+				// Wait for stop ISM.
+				reschedule = 1;
+
+				break;
+			}
+		}
+		// Check timeout.
+		if ((instance->timeout.delay) && ((jiffies - instance->timeout.start_time) >= instance->timeout.delay)) {	// Timeout
+			PDEBUG("Timeout reached.\n");
+			// Stop all actions. No conditions! Block interrupts and trigger. Leave FIFO untouched!
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |=
+			    ME4600_AO_CTRL_BIT_STOP |
+			    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP |
+			    ME4600_AO_CTRL_BIT_RESET_IRQ;
+			ctrl &=
+			    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG);
+			/// Fix for timeout error.
+			ctrl &=
+			    ~(ME4600_AO_CTRL_BIT_EX_TRIG_EDGE |
+			      ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH);
+			if (instance->fifo) {	//Disabling FIFO
+				ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_FIFO;
+			}
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			spin_lock(instance->preload_reg_lock);
+			//Remove from synchronous start. Block triggering from this output.
+			synch = inl(instance->preload_reg);
+			synch &=
+			    ~((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) <<
+			      instance->ao_idx);
+			if (!(instance->fifo)) {	// No FIFO - set to single safe mode
+				synch |=
+				    ME4600_AO_SYNC_HOLD << instance->ao_idx;
+			}
+			outl(synch, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   synch);
+			spin_unlock(instance->preload_reg_lock);
+
+			if (!(instance->fifo)) {	// No FIFO
+				// Restore old settings.
+				PDEBUG("Write old value back to register.\n");
+				outl(instance->single_value,
+				     instance->single_reg);
+				PDEBUG_REG
+				    ("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->single_reg - instance->reg_base,
+				     instance->single_value);
+			}
+			// Set correct value for single_read();
+			instance->single_value_in_fifo = instance->single_value;
+
+			instance->status = ao_status_single_end;
+
+			// Signal the end.
+			signaling = 1;
+		}
+		// Wait for stop.
+		reschedule = 1;
+		break;
+
+		// Stream modes
+	case ao_status_stream_run_wait:
+		if (!instance->fifo) {
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+
+		if (status & ME4600_AO_STATUS_BIT_FSM) {	// State machine is working. Waiting for start finish.
+			instance->status = ao_status_stream_run;
+
+			// Signal end of this step
+			signaling = 1;
+		} else {	// State machine is not working.
+			if (!(status & ME4600_AO_STATUS_BIT_EF)) {	// FIFO is empty. Procedure has started and finish already!
+				instance->status = ao_status_stream_end;
+
+				// Signal the end.
+				signaling = 1;
+				// Wait for stop.
+				reschedule = 1;
+				break;
+			}
+		}
+
+		// Check timeout.
+		if ((instance->timeout.delay) && ((jiffies - instance->timeout.start_time) >= instance->timeout.delay)) {	// Timeout
+			PDEBUG("Timeout reached.\n");
+			// Stop all actions. No conditions! Block interrupts. Leave FIFO untouched!
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |=
+			    ME4600_AO_CTRL_BIT_STOP |
+			    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP |
+			    ME4600_AO_CTRL_BIT_RESET_IRQ;
+			ctrl &=
+			    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG);
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+			spin_lock(instance->preload_reg_lock);
+			//Remove from synchronous start. Block triggering from this output.
+			synch = inl(instance->preload_reg);
+			synch &=
+			    ~((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) <<
+			      instance->ao_idx);
+			outl(synch, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   synch);
+			spin_unlock(instance->preload_reg_lock);
+
+			instance->status = ao_status_stream_end;
+
+			// Signal the end.
+			signaling = 1;
+		}
+		// Wait for stop.
+		reschedule = 1;
+		break;
+
+	case ao_status_stream_run:
+		if (!instance->fifo) {
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+
+		if (!(status & ME4600_AO_STATUS_BIT_FSM)) {	// State machine is not working. This is an error.
+			// BROKEN PIPE!
+			if (!(status & ME4600_AO_STATUS_BIT_EF)) {	// FIFO is empty.
+				if (me_circ_buf_values(&instance->circ_buf)) {	// Software buffer is not empty.
+					if (instance->stop_data_count && (instance->stop_data_count <= instance->data_count)) {	//Finishing work. Requed data shown.
+						PDEBUG
+						    ("ISM stoped. No data in FIFO. Buffer is not empty.\n");
+						instance->status =
+						    ao_status_stream_end;
+					} else {
+						PERROR
+						    ("Output stream has been broken. ISM stoped. No data in FIFO. Buffer is not empty.\n");
+						instance->status =
+						    ao_status_stream_buffer_error;
+					}
+				} else {	// Software buffer is empty.
+					PDEBUG
+					    ("ISM stoped. No data in FIFO. Buffer is empty.\n");
+					instance->status = ao_status_stream_end;
+				}
+			} else {	// There are still datas in FIFO.
+				if (me_circ_buf_values(&instance->circ_buf)) {	// Software buffer is not empty.
+					PERROR
+					    ("Output stream has been broken. ISM stoped but some data in FIFO and buffer.\n");
+				} else {	// Software buffer is empty.
+					PERROR
+					    ("Output stream has been broken. ISM stoped but some data in FIFO. Buffer is empty.\n");
+				}
+				instance->status = ao_status_stream_fifo_error;
+
+			}
+
+			// Signal the failure.
+			signaling = 1;
+			break;
+		}
+		// Wait for stop.
+		reschedule = 1;
+		break;
+
+	case ao_status_stream_end_wait:
+		if (!instance->fifo) {
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+
+		if (!(status & ME4600_AO_STATUS_BIT_FSM)) {	// State machine is not working. Waiting for stop finish.
+			instance->status = ao_status_stream_end;
+			signaling = 1;
+		}
+		// State machine is working.
+		reschedule = 1;
+		break;
+
+	default:
+		PERROR_CRITICAL("Status is in wrong state (%d)!\n",
+				instance->status);
+		instance->status = ao_status_stream_error;
+		// Signal the end.
+		signaling = 1;
+		break;
+
+	}
+
+	if (signaling) {	//Signal it.
+		wake_up_interruptible_all(&instance->wait_queue);
+	}
+
+	if (instance->ao_control_task_flag && reschedule) {	// Reschedule task
+		queue_delayed_work(instance->me4600_workqueue,
+				   &instance->ao_control_task, 1);
+	} else {
+		PINFO("<%s> Ending control task.\n", __func__);
+	}
+
+}
+#else
+/// @note SPECIAL BUILD FOR BOSCH
+/// @author Guenter Gebhardt
+static int me4600_ao_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t tmp;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER spin_lock_irqsave(&instance->subdevice_lock, status);
+	spin_lock(instance->preload_reg_lock);
+	tmp = inl(instance->preload_reg);
+	tmp &= ~(0x10001 << instance->ao_idx);
+	outl(tmp, instance->preload_reg);
+	*instance->preload_flags &= ~(0x1 << instance->ao_idx);
+	spin_unlock(instance->preload_reg_lock);
+
+	tmp = inl(instance->ctrl_reg);
+	tmp |= ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+	outl(tmp, instance->ctrl_reg);
+
+	while (inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM) ;
+
+	outl(ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP,
+	     instance->ctrl_reg);
+
+	outl(0x8000, instance->single_reg);
+
+	instance->single_value = 0x8000;
+	instance->circ_buf.head = 0;
+	instance->circ_buf.tail = 0;
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t tmp;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER
+	    spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	if (inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM) {
+		PERROR("Subdevice is busy.\n");
+		err = ME_ERRNO_SUBDEVICE_BUSY;
+		goto ERROR;
+	}
+
+	if (channel == 0) {
+		if (single_config == 0) {
+			if (ref == ME_REF_AO_GROUND) {
+				if (trig_chan == ME_TRIG_CHAN_DEFAULT) {
+					if (trig_type == ME_TRIG_TYPE_SW) {
+						tmp = inl(instance->ctrl_reg);
+						tmp |=
+						    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+						outl(tmp, instance->ctrl_reg);
+						tmp =
+						    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+						outl(tmp, instance->ctrl_reg);
+
+						spin_lock(instance->
+							  preload_reg_lock);
+						tmp =
+						    inl(instance->preload_reg);
+						tmp &=
+						    ~(0x10001 << instance->
+						      ao_idx);
+						outl(tmp,
+						     instance->preload_reg);
+						*instance->preload_flags &=
+						    ~(0x1 << instance->ao_idx);
+						spin_unlock(instance->
+							    preload_reg_lock);
+					} else if (trig_type ==
+						   ME_TRIG_TYPE_EXT_DIGITAL) {
+						if (trig_edge ==
+						    ME_TRIG_EDGE_RISING) {
+							tmp =
+							    inl(instance->
+								ctrl_reg);
+							tmp |=
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+							tmp =
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP
+							    |
+							    ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+						} else if (trig_edge ==
+							   ME_TRIG_EDGE_FALLING)
+						{
+							tmp =
+							    inl(instance->
+								ctrl_reg);
+							tmp |=
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+							tmp =
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP
+							    |
+							    ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG
+							    |
+							    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+						} else if (trig_edge ==
+							   ME_TRIG_EDGE_ANY) {
+							tmp =
+							    inl(instance->
+								ctrl_reg);
+							tmp |=
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+							tmp =
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP
+							    |
+							    ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG
+							    |
+							    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE
+							    |
+							    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+						} else {
+							PERROR
+							    ("Invalid trigger edge.\n");
+							err =
+							    ME_ERRNO_INVALID_TRIG_EDGE;
+							goto ERROR;
+						}
+
+						spin_lock(instance->
+							  preload_reg_lock);
+
+						tmp =
+						    inl(instance->preload_reg);
+						tmp &=
+						    ~(0x10001 << instance->
+						      ao_idx);
+						tmp |= 0x1 << instance->ao_idx;
+						outl(tmp,
+						     instance->preload_reg);
+						*instance->preload_flags &=
+						    ~(0x1 << instance->ao_idx);
+						spin_unlock(instance->
+							    preload_reg_lock);
+					} else {
+						PERROR
+						    ("Invalid trigger type.\n");
+						err =
+						    ME_ERRNO_INVALID_TRIG_TYPE;
+						goto ERROR;
+					}
+				} else if (trig_chan ==
+					   ME_TRIG_CHAN_SYNCHRONOUS) {
+					if (trig_type == ME_TRIG_TYPE_SW) {
+						tmp = inl(instance->ctrl_reg);
+						tmp |=
+						    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+						outl(tmp, instance->ctrl_reg);
+						tmp =
+						    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+						outl(tmp, instance->ctrl_reg);
+
+						spin_lock(instance->
+							  preload_reg_lock);
+						tmp =
+						    inl(instance->preload_reg);
+						tmp &=
+						    ~(0x10001 << instance->
+						      ao_idx);
+						tmp |= 0x1 << instance->ao_idx;
+						outl(tmp,
+						     instance->preload_reg);
+						*instance->preload_flags |=
+						    0x1 << instance->ao_idx;
+						spin_unlock(instance->
+							    preload_reg_lock);
+					} else if (trig_type ==
+						   ME_TRIG_TYPE_EXT_DIGITAL) {
+						if (trig_edge ==
+						    ME_TRIG_EDGE_RISING) {
+							tmp =
+							    inl(instance->
+								ctrl_reg);
+							tmp |=
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+							tmp =
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+						} else if (trig_edge ==
+							   ME_TRIG_EDGE_FALLING)
+						{
+							tmp =
+							    inl(instance->
+								ctrl_reg);
+							tmp |=
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+							tmp =
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP
+							    |
+							    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+						} else if (trig_edge ==
+							   ME_TRIG_EDGE_ANY) {
+							tmp =
+							    inl(instance->
+								ctrl_reg);
+							tmp |=
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+							tmp =
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP
+							    |
+							    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE
+							    |
+							    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+						} else {
+							PERROR
+							    ("Invalid trigger edge.\n");
+							err =
+							    ME_ERRNO_INVALID_TRIG_EDGE;
+							goto ERROR;
+						}
+
+						spin_lock(instance->
+							  preload_reg_lock);
+
+						tmp =
+						    inl(instance->preload_reg);
+						tmp |=
+						    0x10001 << instance->ao_idx;
+						outl(tmp,
+						     instance->preload_reg);
+						*instance->preload_flags &=
+						    ~(0x1 << instance->ao_idx);
+						spin_unlock(instance->
+							    preload_reg_lock);
+					} else {
+						PERROR
+						    ("Invalid trigger type.\n");
+						err =
+						    ME_ERRNO_INVALID_TRIG_TYPE;
+						goto ERROR;
+					}
+				} else {
+					PERROR
+					    ("Invalid trigger channel specified.\n");
+					err = ME_ERRNO_INVALID_REF;
+					goto ERROR;
+				}
+			} else {
+				PERROR("Invalid analog reference specified.\n");
+				err = ME_ERRNO_INVALID_REF;
+				goto ERROR;
+			}
+		} else {
+			PERROR("Invalid single config specified.\n");
+			err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			goto ERROR;
+		}
+	} else {
+		PERROR("Invalid channel number specified.\n");
+		err = ME_ERRNO_INVALID_CHANNEL;
+		goto ERROR;
+	}
+
+      ERROR:
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long tmp;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	if (channel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER
+	    spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	tmp = inl(instance->ctrl_reg);
+
+	if (tmp & 0x3) {
+		PERROR("Not in single mode.\n");
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+	} else {
+		*value = instance->single_value;
+	}
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long mask = 0;
+	unsigned long tmp;
+	unsigned long cpu_flags;
+	int i;
+	wait_queue_head_t queue;
+	unsigned long j;
+	unsigned long delay = 0;
+
+	PDEBUG("executed.\n");
+
+	init_waitqueue_head(&queue);
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	if (channel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+
+		if (delay == 0)
+			delay = 1;
+	}
+
+	ME_SUBDEVICE_ENTER
+	    spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	tmp = inl(instance->ctrl_reg);
+
+	if (tmp & 0x3) {
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+		PERROR("Not in single mode.\n");
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+		goto ERROR;
+	}
+
+	if (tmp & ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG) {
+		outl(value, instance->single_reg);
+		instance->single_value = value;
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+		if (!(flags & ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING)) {
+			j = jiffies;
+
+			while (inl(instance->status_reg) &
+			       ME4600_AO_STATUS_BIT_FSM) {
+				interruptible_sleep_on_timeout(&queue, 1);
+
+				if (signal_pending(current)) {
+					PERROR
+					    ("Wait on external trigger interrupted by signal.\n");
+					err = ME_ERRNO_SIGNAL;
+					goto ERROR;
+				}
+
+				if (delay && ((jiffies - j) > delay)) {
+					PERROR("Timeout reached.\n");
+					err = ME_ERRNO_TIMEOUT;
+					goto ERROR;
+				}
+			}
+		}
+	} else if ((inl(instance->preload_reg) & (0x10001 << instance->ao_idx))
+		   == (0x10001 << instance->ao_idx)) {
+		if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {
+			tmp |= ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+			outl(tmp, instance->ctrl_reg);
+			outl(value, instance->single_reg);
+			instance->single_value = value;
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if (!(flags & ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING)) {
+				j = jiffies;
+
+				while (inl(instance->status_reg) &
+				       ME4600_AO_STATUS_BIT_FSM) {
+					interruptible_sleep_on_timeout(&queue,
+								       1);
+
+					if (signal_pending(current)) {
+						PERROR
+						    ("Wait on external trigger interrupted by signal.\n");
+						err = ME_ERRNO_SIGNAL;
+						goto ERROR;
+					}
+
+					if (delay && ((jiffies - j) > delay)) {
+						PERROR("Timeout reached.\n");
+						err = ME_ERRNO_TIMEOUT;
+						goto ERROR;
+					}
+				}
+			}
+		} else {
+			outl(value, instance->single_reg);
+			instance->single_value = value;
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		}
+	} else if ((inl(instance->preload_reg) & (0x10001 << instance->ao_idx))
+		   == (0x1 << instance->ao_idx)) {
+		outl(value, instance->single_reg);
+		instance->single_value = value;
+
+		PDEBUG("Synchronous SW, flags = 0x%X.\n", flags);
+
+		if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {
+			PDEBUG("Trigger synchronous SW.\n");
+			spin_lock(instance->preload_reg_lock);
+			tmp = inl(instance->preload_reg);
+
+			for (i = 0; i < ME4600_AO_MAX_SUBDEVICES; i++) {
+				if ((*instance->preload_flags & (0x1 << i))) {
+					if ((tmp & (0x10001 << i)) ==
+					    (0x1 << i)) {
+						mask |= 0x1 << i;
+					}
+				}
+			}
+
+			tmp &= ~(mask);
+
+			outl(tmp, instance->preload_reg);
+			spin_unlock(instance->preload_reg_lock);
+		}
+
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	} else {
+		outl(value, instance->single_reg);
+		instance->single_value = value;
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	}
+
+      ERROR:
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long ctrl;
+	unsigned long tmp;
+	unsigned long cpu_flags;
+	uint64_t conv_ticks;
+	unsigned int conv_start_ticks_low = trigger->iConvStartTicksLow;
+	unsigned int conv_start_ticks_high = trigger->iConvStartTicksHigh;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	conv_ticks =
+	    (uint64_t) conv_start_ticks_low +
+	    ((uint64_t) conv_start_ticks_high << 32);
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	ME_SUBDEVICE_ENTER
+	    spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	if ((inl(instance->status_reg)) & ME4600_AO_STATUS_BIT_FSM) {
+		PERROR("Subdevice is busy.\n");
+		err = ME_ERRNO_SUBDEVICE_BUSY;
+		goto ERROR;
+	}
+
+	ctrl = inl(instance->ctrl_reg);
+	ctrl |= ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+	outl(ctrl, instance->ctrl_reg);
+	ctrl = ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+	outl(ctrl, instance->ctrl_reg);
+
+	if (count != 1) {
+		PERROR("Invalid stream configuration list count specified.\n");
+		err = ME_ERRNO_INVALID_CONFIG_LIST_COUNT;
+		goto ERROR;
+	}
+
+	if (config_list[0].iChannel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		err = ME_ERRNO_INVALID_CHANNEL;
+		goto ERROR;
+	}
+
+	if (config_list[0].iStreamConfig != 0) {
+		PERROR("Invalid stream config specified.\n");
+		err = ME_ERRNO_INVALID_STREAM_CONFIG;
+		goto ERROR;
+	}
+
+	if (config_list[0].iRef != ME_REF_AO_GROUND) {
+		PERROR("Invalid analog reference.\n");
+		err = ME_ERRNO_INVALID_REF;
+		goto ERROR;
+	}
+
+	if ((trigger->iAcqStartTicksLow != 0)
+	    || (trigger->iAcqStartTicksHigh != 0)) {
+		PERROR
+		    ("Invalid acquisition start trigger argument specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_START_ARG;
+		goto ERROR;
+	}
+
+	switch (trigger->iAcqStartTrigType) {
+
+	case ME_TRIG_TYPE_SW:
+		break;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		ctrl |= ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+		switch (trigger->iAcqStartTrigEdge) {
+
+		case ME_TRIG_EDGE_RISING:
+			break;
+
+		case ME_TRIG_EDGE_FALLING:
+			ctrl |= ME4600_AO_CTRL_BIT_EX_TRIG_EDGE;
+
+			break;
+
+		case ME_TRIG_EDGE_ANY:
+			ctrl |=
+			    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE |
+			    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+
+			break;
+
+		default:
+			PERROR
+			    ("Invalid acquisition start trigger edge specified.\n");
+
+			err = ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+
+			goto ERROR;
+
+			break;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid acquisition start trigger type specified.\n");
+
+		err = ME_ERRNO_INVALID_ACQ_START_TRIG_TYPE;
+
+		goto ERROR;
+
+		break;
+	}
+
+	switch (trigger->iScanStartTrigType) {
+
+	case ME_TRIG_TYPE_FOLLOW:
+		break;
+
+	default:
+		PERROR("Invalid scan start trigger type specified.\n");
+
+		err = ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE;
+
+		goto ERROR;
+
+		break;
+	}
+
+	switch (trigger->iConvStartTrigType) {
+
+	case ME_TRIG_TYPE_TIMER:
+		if ((conv_ticks < ME4600_AO_MIN_CHAN_TICKS)
+		    || (conv_ticks > ME4600_AO_MAX_CHAN_TICKS)) {
+			PERROR
+			    ("Invalid conv start trigger argument specified.\n");
+			err = ME_ERRNO_INVALID_CONV_START_ARG;
+			goto ERROR;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid conv start trigger type specified.\n");
+
+		err = ME_ERRNO_INVALID_CONV_START_TRIG_TYPE;
+
+		goto ERROR;
+
+		break;
+	}
+
+	/* Preset to hardware wraparound mode */
+	instance->flags &= ~(ME4600_AO_FLAGS_SW_WRAP_MODE_MASK);
+
+	switch (trigger->iScanStopTrigType) {
+
+	case ME_TRIG_TYPE_NONE:
+		if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			/* Set flags to indicate usage of software mode. */
+			instance->flags |= ME4600_AO_FLAGS_SW_WRAP_MODE_INF;
+			instance->wrap_count = 0;
+			instance->wrap_remaining = 0;
+		}
+
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			if (trigger->iScanStopCount <= 0) {
+				PERROR("Invalid scan stop count specified.\n");
+				err = ME_ERRNO_INVALID_SCAN_STOP_ARG;
+				goto ERROR;
+			}
+
+			/* Set flags to indicate usage of software mode. */
+			instance->flags |= ME4600_AO_FLAGS_SW_WRAP_MODE_FIN;
+			instance->wrap_count = trigger->iScanStopCount;
+			instance->wrap_remaining = trigger->iScanStopCount;
+		} else {
+			PERROR("Invalid scan stop trigger type specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+			goto ERROR;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid scan stop trigger type specified.\n");
+
+		err = ME_ERRNO_INVALID_SCAN_STOP_TRIG_TYPE;
+
+		goto ERROR;
+
+		break;
+	}
+
+	switch (trigger->iAcqStopTrigType) {
+
+	case ME_TRIG_TYPE_NONE:
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (trigger->iScanStopTrigType != ME_TRIG_TYPE_NONE) {
+			PERROR("Invalid acq stop trigger type specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+			goto ERROR;
+		}
+
+		if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			if (trigger->iAcqStopCount <= 0) {
+				PERROR("Invalid acq stop count specified.\n");
+				err = ME_ERRNO_INVALID_ACQ_STOP_ARG;
+				goto ERROR;
+			}
+
+			/* Set flags to indicate usage of software mode. */
+			instance->flags |= ME4600_AO_FLAGS_SW_WRAP_MODE_FIN;
+			instance->wrap_count = trigger->iAcqStopCount;
+			instance->wrap_remaining = trigger->iAcqStopCount;
+		} else {
+			PERROR("Invalid acp stop trigger type specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+			goto ERROR;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid acq stop trigger type specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+		goto ERROR;
+		break;
+	}
+
+	switch (trigger->iAcqStartTrigChan) {
+
+	case ME_TRIG_CHAN_DEFAULT:
+		spin_lock(instance->preload_reg_lock);
+		tmp = inl(instance->preload_reg);
+		tmp &= ~(0x10001 << instance->ao_idx);
+		outl(tmp, instance->preload_reg);
+		spin_unlock(instance->preload_reg_lock);
+
+		break;
+
+	case ME_TRIG_CHAN_SYNCHRONOUS:
+		if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_SW) {
+			spin_lock(instance->preload_reg_lock);
+			tmp = inl(instance->preload_reg);
+			tmp &= ~(0x10001 << instance->ao_idx);
+			outl(tmp, instance->preload_reg);
+			tmp |= 0x1 << instance->ao_idx;
+			outl(tmp, instance->preload_reg);
+			spin_unlock(instance->preload_reg_lock);
+		} else {
+			ctrl &= ~(ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG);
+			spin_lock(instance->preload_reg_lock);
+			tmp = inl(instance->preload_reg);
+			tmp &= ~(0x10001 << instance->ao_idx);
+			outl(tmp, instance->preload_reg);
+			tmp |= 0x10000 << instance->ao_idx;
+			outl(tmp, instance->preload_reg);
+			spin_unlock(instance->preload_reg_lock);
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid acq start trigger channel specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_START_TRIG_CHAN;
+		goto ERROR;
+
+		break;
+	}
+
+	outl(conv_ticks - 2, instance->timer_reg);
+
+	if (flags & ME_IO_STREAM_CONFIG_BIT_PATTERN) {
+		if (instance->ao_idx == 3) {
+			ctrl |= ME4600_AO_CTRL_BIT_ENABLE_DO;
+		} else {
+			err = ME_ERRNO_INVALID_FLAGS;
+			goto ERROR;
+		}
+	} else {
+		if (instance->ao_idx == 3) {
+			ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_DO;
+		}
+	}
+
+	/* Set hardware mode. */
+	if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+		ctrl |= ME4600_AO_CTRL_BIT_MODE_0;
+	} else {
+		ctrl |= ME4600_AO_CTRL_BIT_MODE_1;
+	}
+
+	PDEBUG("Preload word = 0x%X.\n", inl(instance->preload_reg));
+
+	PDEBUG("Ctrl word = 0x%lX.\n", ctrl);
+	outl(ctrl, instance->ctrl_reg);	// Write the control word
+
+      ERROR:
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_new_values(me_subdevice_t * subdevice,
+					  struct file *filep,
+					  int time_out, int *count, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	long j;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	}
+
+	*count = 0;
+
+	ME_SUBDEVICE_ENTER;
+
+	if (t) {
+		j = jiffies;
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 ((me_circ_buf_space
+						   (&instance->circ_buf))
+						  || !(inl(instance->status_reg)
+						       &
+						       ME4600_AO_STATUS_BIT_FSM)),
+						 t);
+
+		if (!(inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM)) {
+			PERROR("AO subdevice is not running.\n");
+			err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+		} else if (signal_pending(current)) {
+			PERROR("Wait on values interrupted from signal.\n");
+			err = ME_ERRNO_SIGNAL;
+		} else if ((jiffies - j) >= t) {
+			PERROR("Wait on values timed out.\n");
+			err = ME_ERRNO_TIMEOUT;
+		} else {
+			*count = me_circ_buf_space(&instance->circ_buf);
+		}
+	} else {
+		wait_event_interruptible(instance->wait_queue,
+					 ((me_circ_buf_space
+					   (&instance->circ_buf))
+					  || !(inl(instance->status_reg) &
+					       ME4600_AO_STATUS_BIT_FSM)));
+
+		if (!(inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM)) {
+			PERROR("AO subdevice is not running.\n");
+			err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+		} else if (signal_pending(current)) {
+			PERROR("Wait on values interrupted from signal.\n");
+			err = ME_ERRNO_SIGNAL;
+		} else {
+			*count = me_circ_buf_space(&instance->circ_buf);
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static void stop_immediately(me4600_ao_subdevice_t * instance)
+{
+	unsigned long cpu_flags;
+	uint32_t tmp;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	tmp = inl(instance->ctrl_reg);
+	tmp |= ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+	outl(tmp, instance->ctrl_reg);
+
+	while (inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM) ;
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+}
+
+static int me4600_ao_io_stream_start(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int start_mode, int time_out, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags = 0;
+	unsigned long ref;
+	unsigned long tmp;
+	unsigned long delay = 0;
+	wait_queue_head_t queue;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	init_waitqueue_head(&queue);
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+
+		if (delay == 0)
+			delay = 1;
+	}
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	ME_SUBDEVICE_ENTER
+	    spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	tmp = inl(instance->ctrl_reg);
+
+	switch (tmp & (ME4600_AO_CTRL_MASK_MODE)) {
+
+	case 0:		// Single mode
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+		PERROR("Subdevice is configured in single mode.\n");
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+		goto ERROR;
+
+	case 1:		// Wraparound mode
+		if (tmp & ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG) {	// Normal wraparound with external trigger
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp &=
+			    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME4600_AO_CTRL_BIT_STOP |
+			      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+
+			outl(tmp, instance->ctrl_reg);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if (start_mode == ME_START_MODE_BLOCKING) {
+				init_waitqueue_head(&queue);
+
+				if (delay) {
+					ref = jiffies;
+
+					while (!
+					       (inl(instance->status_reg) &
+						ME4600_AO_STATUS_BIT_FSM)) {
+						interruptible_sleep_on_timeout
+						    (&queue, 1);
+
+						if (signal_pending(current)) {
+							PERROR
+							    ("Wait on start of state machine interrupted.\n");
+							stop_immediately
+							    (instance);
+							err = ME_ERRNO_SIGNAL;
+							goto ERROR;
+						}
+
+						if (((jiffies - ref) >= delay)) {
+							PERROR
+							    ("Timeout reached.\n");
+							stop_immediately
+							    (instance);
+							err = ME_ERRNO_TIMEOUT;
+							goto ERROR;
+						}
+					}
+				} else {
+					while (!
+					       (inl(instance->status_reg) &
+						ME4600_AO_STATUS_BIT_FSM)) {
+						interruptible_sleep_on_timeout
+						    (&queue, 1);
+
+						if (signal_pending(current)) {
+							PERROR
+							    ("Wait on start of state machine interrupted.\n");
+							stop_immediately
+							    (instance);
+							err = ME_ERRNO_SIGNAL;
+							goto ERROR;
+						}
+					}
+				}
+			} else if (start_mode == ME_START_MODE_NONBLOCKING) {
+			} else {
+				PERROR("Invalid start mode specified.\n");
+				err = ME_ERRNO_INVALID_START_MODE;
+				goto ERROR;
+			}
+		} else if ((inl(instance->preload_reg) & (0x10001 << instance->ao_idx)) == (0x10000 << instance->ao_idx)) {	// Synchronous with external trigger
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			if (flags & ME_IO_STREAM_START_TYPE_TRIG_SYNCHRONOUS) {
+				tmp |= ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+				tmp &=
+				    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+				      ME4600_AO_CTRL_BIT_STOP |
+				      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+				outl(tmp, instance->ctrl_reg);
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+
+				if (start_mode == ME_START_MODE_BLOCKING) {
+					init_waitqueue_head(&queue);
+
+					if (delay) {
+						ref = jiffies;
+
+						while (!
+						       (inl
+							(instance->
+							 status_reg) &
+							ME4600_AO_STATUS_BIT_FSM))
+						{
+							interruptible_sleep_on_timeout
+							    (&queue, 1);
+
+							if (signal_pending
+							    (current)) {
+								PERROR
+								    ("Wait on start of state machine interrupted.\n");
+								stop_immediately
+								    (instance);
+								err =
+								    ME_ERRNO_SIGNAL;
+								goto ERROR;
+							}
+
+							if (((jiffies - ref) >=
+							     delay)) {
+								PERROR
+								    ("Timeout reached.\n");
+								stop_immediately
+								    (instance);
+								err =
+								    ME_ERRNO_TIMEOUT;
+								goto ERROR;
+							}
+						}
+					} else {
+						while (!
+						       (inl
+							(instance->
+							 status_reg) &
+							ME4600_AO_STATUS_BIT_FSM))
+						{
+							interruptible_sleep_on_timeout
+							    (&queue, 1);
+
+							if (signal_pending
+							    (current)) {
+								PERROR
+								    ("Wait on start of state machine interrupted.\n");
+								stop_immediately
+								    (instance);
+								err =
+								    ME_ERRNO_SIGNAL;
+								goto ERROR;
+							}
+						}
+					}
+				} else if (start_mode ==
+					   ME_START_MODE_NONBLOCKING) {
+				} else {
+					PERROR
+					    ("Invalid start mode specified.\n");
+					err = ME_ERRNO_INVALID_START_MODE;
+					goto ERROR;
+				}
+			} else {
+				tmp &=
+				    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+				      ME4600_AO_CTRL_BIT_STOP |
+				      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+				outl(tmp, instance->ctrl_reg);
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+			}
+		} else if ((inl(instance->preload_reg) & (0x10001 << instance->ao_idx)) == (0x1 << instance->ao_idx)) {	// Synchronous wraparound with sw trigger
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp &=
+			    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME4600_AO_CTRL_BIT_STOP |
+			      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+
+			outl(tmp, instance->ctrl_reg);
+
+			if (flags & ME_IO_STREAM_START_TYPE_TRIG_SYNCHRONOUS) {
+				outl(0x8000, instance->single_reg);
+				instance->single_value = 0x8000;
+			}
+
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		} else {	// Software start
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp &=
+			    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME4600_AO_CTRL_BIT_STOP |
+			      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+
+			outl(tmp, instance->ctrl_reg);
+
+			outl(0x8000, instance->single_reg);
+			instance->single_value = 0x8000;
+
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		}
+
+		break;
+
+	case 2:		// Continuous mode
+		if (tmp & ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG) {	// Externally triggered
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp &=
+			    ~(ME4600_AO_CTRL_BIT_STOP |
+			      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+			tmp |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(tmp, instance->ctrl_reg);
+			instance->wrap_remaining = instance->wrap_count;
+			instance->circ_buf.tail = 0;
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if (start_mode == ME_START_MODE_BLOCKING) {
+				init_waitqueue_head(&queue);
+
+				if (delay) {
+					ref = jiffies;
+
+					while (!
+					       (inl(instance->status_reg) &
+						ME4600_AO_STATUS_BIT_FSM)) {
+						interruptible_sleep_on_timeout
+						    (&queue, 1);
+
+						if (signal_pending(current)) {
+							PERROR
+							    ("Wait on start of state machine interrupted.\n");
+							stop_immediately
+							    (instance);
+							err = ME_ERRNO_SIGNAL;
+							goto ERROR;
+						}
+
+						if (((jiffies - ref) >= delay)) {
+							PERROR
+							    ("Timeout reached.\n");
+							stop_immediately
+							    (instance);
+							err = ME_ERRNO_TIMEOUT;
+							goto ERROR;
+						}
+					}
+				} else {
+					while (!
+					       (inl(instance->status_reg) &
+						ME4600_AO_STATUS_BIT_FSM)) {
+						interruptible_sleep_on_timeout
+						    (&queue, 1);
+
+						if (signal_pending(current)) {
+							PERROR
+							    ("Wait on start of state machine interrupted.\n");
+							stop_immediately
+							    (instance);
+							err = ME_ERRNO_SIGNAL;
+							goto ERROR;
+						}
+					}
+				}
+			} else if (start_mode == ME_START_MODE_NONBLOCKING) {
+				/* Do nothing */
+			} else {
+				PERROR("Invalid start mode specified.\n");
+				stop_immediately(instance);
+				err = ME_ERRNO_INVALID_START_MODE;
+				goto ERROR;
+			}
+		} else if ((inl(instance->preload_reg) & (0x10001 << instance->ao_idx)) == (0x10000 << instance->ao_idx)) {	// Synchronous with external trigger
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			if (flags & ME_IO_STREAM_START_TYPE_TRIG_SYNCHRONOUS) {
+				tmp |=
+				    ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG |
+				    ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+				tmp &=
+				    ~(ME4600_AO_CTRL_BIT_STOP |
+				      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+				outl(tmp, instance->ctrl_reg);
+				instance->wrap_remaining = instance->wrap_count;
+				instance->circ_buf.tail = 0;
+
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+
+				if (start_mode == ME_START_MODE_BLOCKING) {
+					init_waitqueue_head(&queue);
+
+					if (delay) {
+						ref = jiffies;
+
+						while (!
+						       (inl
+							(instance->
+							 status_reg) &
+							ME4600_AO_STATUS_BIT_FSM))
+						{
+							interruptible_sleep_on_timeout
+							    (&queue, 1);
+
+							if (signal_pending
+							    (current)) {
+								PERROR
+								    ("Wait on start of state machine interrupted.\n");
+								stop_immediately
+								    (instance);
+								err =
+								    ME_ERRNO_SIGNAL;
+								goto ERROR;
+							}
+
+							if (((jiffies - ref) >=
+							     delay)) {
+								PERROR
+								    ("Timeout reached.\n");
+								stop_immediately
+								    (instance);
+								err =
+								    ME_ERRNO_TIMEOUT;
+								goto ERROR;
+							}
+						}
+					} else {
+						while (!
+						       (inl
+							(instance->
+							 status_reg) &
+							ME4600_AO_STATUS_BIT_FSM))
+						{
+							interruptible_sleep_on_timeout
+							    (&queue, 1);
+
+							if (signal_pending
+							    (current)) {
+								PERROR
+								    ("Wait on start of state machine interrupted.\n");
+								stop_immediately
+								    (instance);
+								err =
+								    ME_ERRNO_SIGNAL;
+								goto ERROR;
+							}
+						}
+					}
+				} else if (start_mode ==
+					   ME_START_MODE_NONBLOCKING) {
+				} else {
+					PERROR
+					    ("Invalid start mode specified.\n");
+					stop_immediately(instance);
+					err = ME_ERRNO_INVALID_START_MODE;
+					goto ERROR;
+				}
+			} else {
+				tmp |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+				tmp &=
+				    ~(ME4600_AO_CTRL_BIT_STOP |
+				      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+				outl(tmp, instance->ctrl_reg);
+				instance->wrap_remaining = instance->wrap_count;
+				instance->circ_buf.tail = 0;
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+			}
+		} else if ((inl(instance->preload_reg) & (0x10001 << instance->ao_idx)) == (0x1 << instance->ao_idx)) {	// Synchronous wraparound with sw trigger
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp &=
+			    ~(ME4600_AO_CTRL_BIT_STOP |
+			      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+			tmp |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			instance->wrap_remaining = instance->wrap_count;
+			instance->circ_buf.tail = 0;
+			PDEBUG("CTRL Reg = 0x%X.\n", inl(instance->ctrl_reg));
+			outl(tmp, instance->ctrl_reg);
+
+			if (flags & ME_IO_STREAM_START_TYPE_TRIG_SYNCHRONOUS) {
+				outl(0x8000, instance->single_reg);
+				instance->single_value = 0x8000;
+			}
+
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		} else {	// Software start
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp &=
+			    ~(ME4600_AO_CTRL_BIT_STOP |
+			      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+
+			tmp |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(tmp, instance->ctrl_reg);
+			outl(0x8000, instance->single_reg);
+			instance->single_value = 0x8000;
+			instance->wrap_remaining = instance->wrap_count;
+			instance->circ_buf.tail = 0;
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		}
+
+		break;
+
+	default:
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+		PERROR("Invalid mode configured.\n");
+		err = ME_ERRNO_INTERNAL;
+		goto ERROR;
+	}
+
+      ERROR:
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_status(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int wait,
+				      int *status, int *values, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	wait_queue_head_t queue;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	init_waitqueue_head(&queue);
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (wait == ME_WAIT_NONE) {
+		*status =
+		    (inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM) ?
+		    ME_STATUS_BUSY : ME_STATUS_IDLE;
+		*values = me_circ_buf_space(&instance->circ_buf);
+	} else if (wait == ME_WAIT_IDLE) {
+		while (inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM) {
+			interruptible_sleep_on_timeout(&queue, 1);
+
+			if (instance->flags & ME4600_AO_FLAGS_BROKEN_PIPE) {
+				PERROR("Output stream was interrupted.\n");
+				*status = ME_STATUS_ERROR;
+				err = ME_ERRNO_SUCCESS;
+				goto ERROR;
+			}
+
+			if (signal_pending(current)) {
+				PERROR
+				    ("Wait on state machine interrupted by signal.\n");
+				*status = ME_STATUS_INVALID;
+				err = ME_ERRNO_SIGNAL;
+				goto ERROR;
+			}
+		}
+
+		*status = ME_STATUS_IDLE;
+
+		*values = me_circ_buf_space(&instance->circ_buf);
+	} else {
+		PERROR("Invalid wait argument specified.\n");
+		*status = ME_STATUS_INVALID;
+		err = ME_ERRNO_INVALID_WAIT;
+		goto ERROR;
+	}
+
+      ERROR:
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_stop(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int stop_mode, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me4600_ao_subdevice_t *instance;
+	unsigned long cpu_flags;
+	unsigned long tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (stop_mode == ME_STOP_MODE_IMMEDIATE) {
+		spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+		tmp = inl(instance->ctrl_reg);
+		tmp |=
+		    ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+		outl(tmp, instance->ctrl_reg);
+
+		while (inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM) ;
+
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	} else if (stop_mode == ME_STOP_MODE_LAST_VALUE) {
+		spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+		tmp = inl(instance->ctrl_reg);
+		tmp |= ME4600_AO_CTRL_BIT_STOP;
+		outl(tmp, instance->ctrl_reg);
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	} else {
+		PERROR("Invalid stop mode specified.\n");
+		err = ME_ERRNO_INVALID_STOP_MODE;
+		goto ERROR;
+	}
+
+      ERROR:
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int write_mode,
+				     int *values, int *count, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me4600_ao_subdevice_t *instance;
+	unsigned long tmp;
+	int i;
+	int value;
+	int cnt = *count;
+	int c;
+	int k;
+	int ret = 0;
+	unsigned long cpu_flags = 0;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (*count <= 0) {
+		PERROR("Invalid count of values specified.\n");
+		err = ME_ERRNO_INVALID_VALUE_COUNT;
+		goto ERROR;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	tmp = inl(instance->ctrl_reg);
+
+	switch (tmp & 0x3) {
+
+	case 1:		// Wraparound mode
+		if (instance->bosch_fw) {	// Bosch firmware
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if (cnt != 7) {
+				PERROR
+				    ("Invalid count of values specified. 7 expected.\n");
+				err = ME_ERRNO_INVALID_VALUE_COUNT;
+				goto ERROR;
+			}
+
+			for (i = 0; i < 7; i++) {
+				if (get_user(value, values)) {
+					PERROR
+					    ("Can't copy value from user space.\n");
+					err = ME_ERRNO_INTERNAL;
+					goto ERROR;
+				}
+
+				if (i == 0) {
+					/* Maximum voltage */
+					value <<= 16;
+					value |=
+					    inl(instance->reg_base +
+						0xD4) & 0xFFFF;
+					outl(value, instance->reg_base + 0xD4);
+				} else if (i == 1) {
+					/* Minimum voltage */
+					value &= 0xFFFF;
+					value |=
+					    inl(instance->reg_base +
+						0xD4) & 0xFFFF0000;
+					outl(value, instance->reg_base + 0xD4);
+				} else if (i == 2) {
+					/* Delta up */
+					value <<= 16;
+					value |=
+					    inl(instance->reg_base +
+						0xD8) & 0xFFFF;
+					outl(value, instance->reg_base + 0xD8);
+				} else if (i == 3) {
+					/* Delta down */
+					value &= 0xFFFF;
+					value |=
+					    inl(instance->reg_base +
+						0xD8) & 0xFFFF0000;
+					outl(value, instance->reg_base + 0xD8);
+				} else if (i == 4) {
+					/* Start value */
+					outl(value, instance->reg_base + 0xDC);
+				} else if (i == 5) {
+					/* Invert */
+					if (value) {
+						value = inl(instance->ctrl_reg);
+						value |= 0x100;
+						outl(value, instance->ctrl_reg);
+					} else {
+						value = inl(instance->ctrl_reg);
+						value &= ~0x100;
+						outl(value, instance->ctrl_reg);
+					}
+				} else if (i == 6) {
+					/* Timer for positive ramp */
+					outl(value, instance->reg_base + 0xE0);
+				}
+
+				values++;
+			}
+		} else {	// Normal firmware
+			PDEBUG("Write for wraparound mode.\n");
+
+			if (inl(instance->status_reg) &
+			    ME4600_AO_STATUS_BIT_FSM) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR
+				    ("There is already a conversion running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp |= ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+			tmp &= ~ME4600_AO_CTRL_BIT_ENABLE_FIFO;
+			outl(tmp, instance->ctrl_reg);
+			tmp |= ME4600_AO_CTRL_BIT_ENABLE_FIFO;
+
+			if ((*count > ME4600_AO_FIFO_COUNT) ||
+			    ((instance->
+			      flags & ME4600_AO_FLAGS_SW_WRAP_MODE_MASK) ==
+			     ME4600_AO_FLAGS_SW_WRAP_MODE_FIN)) {
+				tmp &=
+				    ~(ME4600_AO_CTRL_BIT_MODE_0 |
+				      ME4600_AO_CTRL_BIT_MODE_1);
+				tmp |= ME4600_AO_CTRL_BIT_MODE_1;
+			}
+
+			outl(tmp, instance->ctrl_reg);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if ((*count <= ME4600_AO_FIFO_COUNT) &&
+			    ((instance->
+			      flags & ME4600_AO_FLAGS_SW_WRAP_MODE_MASK) ==
+			     ME4600_AO_FLAGS_SW_WRAP_MODE_INF)) {
+				for (i = 0; i < *count; i++) {
+					if (get_user(value, values + i)) {
+						PERROR
+						    ("Cannot copy value from user space.\n");
+						err = ME_ERRNO_INTERNAL;
+						goto ERROR;
+					}
+
+					if (instance->ao_idx & 0x1)
+						value <<= 16;
+
+					outl(value, instance->fifo_reg);
+				}
+			} else if ((*count <= ME4600_AO_CIRC_BUF_COUNT) &&
+				   ((instance->
+				     flags & ME4600_AO_FLAGS_SW_WRAP_MODE_MASK)
+				    == ME4600_AO_FLAGS_SW_WRAP_MODE_INF)) {
+				for (i = 0; i < *count; i++) {
+					if (get_user(value, values + i)) {
+						PERROR
+						    ("Cannot copy value from user space.\n");
+						err = ME_ERRNO_INTERNAL;
+						goto ERROR;
+					}
+
+					instance->circ_buf.buf[i] = value;	/* Used to hold the values. */
+				}
+
+				instance->circ_buf.tail = 0;	/* Used as the current read position. */
+				instance->circ_buf.head = *count;	/* Used as the buffer size. */
+
+				/* Preload the FIFO. */
+
+				for (i = 0; i < ME4600_AO_FIFO_COUNT;
+				     i++, instance->circ_buf.tail++) {
+					if (instance->circ_buf.tail >=
+					    instance->circ_buf.head)
+						instance->circ_buf.tail = 0;
+
+					if (instance->ao_idx & 0x1)
+						outl(instance->circ_buf.
+						     buf[instance->circ_buf.
+							 tail] << 16,
+						     instance->fifo_reg);
+					else
+						outl(instance->circ_buf.
+						     buf[instance->circ_buf.
+							 tail],
+						     instance->fifo_reg);
+				}
+			} else if ((*count <= ME4600_AO_CIRC_BUF_COUNT) &&
+				   ((instance->
+				     flags & ME4600_AO_FLAGS_SW_WRAP_MODE_MASK)
+				    == ME4600_AO_FLAGS_SW_WRAP_MODE_FIN)) {
+				unsigned int preload_count;
+
+				for (i = 0; i < *count; i++) {
+					if (get_user(value, values + i)) {
+						PERROR
+						    ("Cannot copy value from user space.\n");
+						err = ME_ERRNO_INTERNAL;
+						goto ERROR;
+					}
+
+					instance->circ_buf.buf[i] = value;	/* Used to hold the values. */
+				}
+
+				instance->circ_buf.tail = 0;	/* Used as the current read position. */
+				instance->circ_buf.head = *count;	/* Used as the buffer size. */
+
+				/* Try to preload the whole FIFO. */
+				preload_count = ME4600_AO_FIFO_COUNT;
+
+				if (preload_count > instance->wrap_count)
+					preload_count = instance->wrap_count;
+
+				/* Preload the FIFO. */
+				for (i = 0; i < preload_count;
+				     i++, instance->circ_buf.tail++) {
+					if (instance->circ_buf.tail >=
+					    instance->circ_buf.head)
+						instance->circ_buf.tail = 0;
+
+					if (instance->ao_idx & 0x1)
+						outl(instance->circ_buf.
+						     buf[instance->circ_buf.
+							 tail] << 16,
+						     instance->fifo_reg);
+					else
+						outl(instance->circ_buf.
+						     buf[instance->circ_buf.
+							 tail],
+						     instance->fifo_reg);
+				}
+
+				instance->wrap_remaining =
+				    instance->wrap_count - preload_count;
+			} else {
+				PERROR("To many values written.\n");
+				err = ME_ERRNO_INVALID_VALUE_COUNT;
+				goto ERROR;
+			}
+		}
+
+		break;
+
+	case 2:		// Continuous mode
+		/* Check if in SW wrapround mode */
+		if (instance->flags & ME4600_AO_FLAGS_SW_WRAP_MODE_MASK) {
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+			PERROR("Subdevice is configured SW wrapround mode.\n");
+			err = ME_ERRNO_PREVIOUS_CONFIG;
+			goto ERROR;
+		}
+
+		switch (write_mode) {
+
+		case ME_WRITE_MODE_BLOCKING:
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			PDEBUG("Write for blocking continuous mode.\n");
+
+			while (cnt > 0) {
+				wait_event_interruptible(instance->wait_queue,
+							 (c =
+							  me_circ_buf_space_to_end
+							  (&instance->
+							   circ_buf)));
+
+				if (instance->
+				    flags & ME4600_AO_FLAGS_BROKEN_PIPE) {
+					PERROR
+					    ("Broken pipe in blocking write.\n");
+					err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+					goto ERROR;
+				} else if (signal_pending(current)) {
+					PERROR
+					    ("Wait for free buffer interrupted from signal.\n");
+					err = ME_ERRNO_SIGNAL;
+					goto ERROR;
+				}
+
+				PDEBUG("Space to end = %d.\n", c);
+
+				/* Only able to write size of free buffer or size of count */
+
+				if (cnt < c)
+					c = cnt;
+				k = sizeof(int) * c;
+				k -= copy_from_user(instance->circ_buf.buf +
+						    instance->circ_buf.head,
+						    values, k);
+				c = k / sizeof(int);
+
+				PDEBUG("Copy %d values from user space.\n", c);
+
+				if (!c) {
+					PERROR
+					    ("Cannot copy values from user space.\n");
+					err = ME_ERRNO_INTERNAL;
+					goto ERROR;
+				}
+
+				instance->circ_buf.head =
+				    (instance->circ_buf.head +
+				     c) & (instance->circ_buf.mask);
+
+				values += c;
+				cnt -= c;
+				ret += c;
+
+				/* Values are now available so enable interrupts */
+				spin_lock_irqsave(&instance->subdevice_lock,
+						  cpu_flags);
+
+				if (me_circ_buf_space(&instance->circ_buf)) {
+					tmp = inl(instance->ctrl_reg);
+					tmp |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+					outl(tmp, instance->ctrl_reg);
+				}
+
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+			}
+
+			*count = ret;
+
+			break;
+
+		case ME_WRITE_MODE_NONBLOCKING:
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			PDEBUG("Write for non blocking continuous mode.\n");
+
+			while (cnt > 0) {
+				if (instance->
+				    flags & ME4600_AO_FLAGS_BROKEN_PIPE) {
+					PERROR
+					    ("ME4600:Broken pipe in nonblocking write.\n");
+					err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+					goto ERROR;
+				}
+
+				c = me_circ_buf_space_to_end(&instance->
+							     circ_buf);
+
+				if (!c) {
+					PDEBUG
+					    ("Returning from nonblocking write.\n");
+					break;
+				}
+
+				PDEBUG("Space to end = %d.\n", c);
+
+				/* Only able to write size of free buffer or size of count */
+
+				if (cnt < c)
+					c = cnt;
+				k = sizeof(int) * c;
+				k -= copy_from_user(instance->circ_buf.buf +
+						    instance->circ_buf.head,
+						    values, k);
+				c = k / sizeof(int);
+
+				PDEBUG("Copy %d values from user space.\n", c);
+
+				if (!c) {
+					PERROR
+					    ("Cannot copy values from user space.\n");
+					err = ME_ERRNO_INTERNAL;
+					goto ERROR;
+				}
+
+				instance->circ_buf.head =
+				    (instance->circ_buf.head +
+				     c) & (instance->circ_buf.mask);
+
+				values += c;
+				cnt -= c;
+				ret += c;
+
+				/* Values are now available so enable interrupts */
+				spin_lock_irqsave(&instance->subdevice_lock,
+						  cpu_flags);
+
+				if (me_circ_buf_space(&instance->circ_buf)) {
+					tmp = inl(instance->ctrl_reg);
+					tmp |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+					outl(tmp, instance->ctrl_reg);
+				}
+
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+			}
+
+			*count = ret;
+
+			break;
+
+		case ME_WRITE_MODE_PRELOAD:
+			PDEBUG("Write for preload continuous mode.\n");
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR
+				    ("Can't Preload DAC FIFO while conversion is running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp = inl(instance->ctrl_reg);
+
+			tmp |=
+			    ME4600_AO_CTRL_BIT_STOP |
+			    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+			outl(tmp, instance->ctrl_reg);
+			tmp &=
+			    ~(ME4600_AO_CTRL_BIT_ENABLE_FIFO |
+			      ME4600_AO_CTRL_BIT_ENABLE_IRQ);
+			outl(tmp, instance->ctrl_reg);
+			tmp |= ME4600_AO_CTRL_BIT_ENABLE_FIFO;
+			outl(tmp, instance->ctrl_reg);
+
+			instance->circ_buf.head = 0;
+			instance->circ_buf.tail = 0;
+			instance->flags &= ~ME4600_AO_FLAGS_BROKEN_PIPE;
+
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			c = ME4600_AO_FIFO_COUNT;
+
+			if (cnt < c)
+				c = cnt;
+
+			for (i = 0; i < c; i++) {
+				if (get_user(value, values)) {
+					PERROR
+					    ("Can't copy value from user space.\n");
+					err = ME_ERRNO_INTERNAL;
+					goto ERROR;
+				}
+
+				if (instance->ao_idx & 0x1)
+					value <<= 16;
+
+				outl(value, instance->fifo_reg);
+
+				values++;
+			}
+
+			cnt -= c;
+
+			ret += c;
+
+			PDEBUG("Wrote %d values to fifo.\n", c);
+
+			while (1) {
+				c = me_circ_buf_space_to_end(&instance->
+							     circ_buf);
+
+				if (c == 0)
+					break;
+
+				if (cnt < c)
+					c = cnt;
+
+				if (c <= 0)
+					break;
+
+				k = sizeof(int) * c;
+
+				k -= copy_from_user(instance->circ_buf.buf +
+						    instance->circ_buf.head,
+						    values, k);
+
+				c = k / sizeof(int);
+
+				PDEBUG("Wrote %d values to circular buffer.\n",
+				       c);
+
+				if (!c) {
+					PERROR
+					    ("Can't copy values from user space.\n");
+					err = ME_ERRNO_INTERNAL;
+					goto ERROR;
+				}
+
+				instance->circ_buf.head =
+				    (instance->circ_buf.head +
+				     c) & (instance->circ_buf.mask);
+
+				values += c;
+				cnt -= c;
+				ret += c;
+			}
+
+			*count = ret;
+
+			break;
+
+		default:
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			PERROR("Invalid write mode specified.\n");
+
+			err = ME_ERRNO_INVALID_WRITE_MODE;
+
+			goto ERROR;
+		}
+
+		break;
+
+	default:		// Single mode of invalid
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+		PERROR("Subdevice is configured in single mode.\n");
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+		goto ERROR;
+	}
+
+      ERROR:
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me4600_ao_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me4600_ao_isr(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{
+	unsigned long tmp;
+	int value;
+	me4600_ao_subdevice_t *instance = dev_id;
+	int i;
+	int c = 0;
+	int c1 = 0;
+
+	if (irq != instance->irq) {
+		PDEBUG("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	if (!((0x1 << (instance->ao_idx + 3)) & inl(instance->irq_status_reg))) {
+		return IRQ_NONE;
+	}
+
+	PDEBUG("executed.\n");
+
+	tmp = inl(instance->status_reg);
+
+	if (!(tmp & ME4600_AO_STATUS_BIT_EF) &&
+	    (tmp & ME4600_AO_STATUS_BIT_HF) &&
+	    (tmp & ME4600_AO_STATUS_BIT_HF)) {
+		c = ME4600_AO_FIFO_COUNT;
+		PDEBUG("Fifo empty.\n");
+	} else if ((tmp & ME4600_AO_STATUS_BIT_EF) &&
+		   (tmp & ME4600_AO_STATUS_BIT_HF) &&
+		   (tmp & ME4600_AO_STATUS_BIT_HF)) {
+		c = ME4600_AO_FIFO_COUNT / 2;
+		PDEBUG("Fifo under half full.\n");
+	} else {
+		c = 0;
+		PDEBUG("Fifo full.\n");
+	}
+
+	PDEBUG("Try to write 0x%04X values.\n", c);
+
+	if ((instance->flags & ME4600_AO_FLAGS_SW_WRAP_MODE_MASK) ==
+	    ME4600_AO_FLAGS_SW_WRAP_MODE_INF) {
+		while (c) {
+			c1 = c;
+
+			if (c1 > (instance->circ_buf.head - instance->circ_buf.tail))	/* Only up to the end of the buffer */
+				c1 = (instance->circ_buf.head -
+				      instance->circ_buf.tail);
+
+			/* Write the values to the FIFO */
+			for (i = 0; i < c1; i++, instance->circ_buf.tail++, c--) {
+				if (instance->ao_idx & 0x1)
+					outl(instance->circ_buf.
+					     buf[instance->circ_buf.tail] << 16,
+					     instance->fifo_reg);
+				else
+					outl(instance->circ_buf.
+					     buf[instance->circ_buf.tail],
+					     instance->fifo_reg);
+			}
+
+			if (instance->circ_buf.tail >= instance->circ_buf.head)	/* Start from beginning */
+				instance->circ_buf.tail = 0;
+		}
+
+		spin_lock(&instance->subdevice_lock);
+
+		tmp = inl(instance->ctrl_reg);
+		tmp |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+		outl(tmp, instance->ctrl_reg);
+		tmp &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+		outl(tmp, instance->ctrl_reg);
+
+		if (!(inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM)) {
+			PERROR("Broken pipe.\n");
+			instance->flags |= ME4600_AO_FLAGS_BROKEN_PIPE;
+			tmp &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(tmp, instance->ctrl_reg);
+		}
+
+		spin_unlock(&instance->subdevice_lock);
+	} else if ((instance->flags & ME4600_AO_FLAGS_SW_WRAP_MODE_MASK) ==
+		   ME4600_AO_FLAGS_SW_WRAP_MODE_FIN) {
+		while (c && instance->wrap_remaining) {
+			c1 = c;
+
+			if (c1 > (instance->circ_buf.head - instance->circ_buf.tail))	/* Only up to the end of the buffer */
+				c1 = (instance->circ_buf.head -
+				      instance->circ_buf.tail);
+
+			if (c1 > instance->wrap_remaining)	/* Only up to count of user defined number of values */
+				c1 = instance->wrap_remaining;
+
+			/* Write the values to the FIFO */
+			for (i = 0; i < c1;
+			     i++, instance->circ_buf.tail++, c--,
+			     instance->wrap_remaining--) {
+				if (instance->ao_idx & 0x1)
+					outl(instance->circ_buf.
+					     buf[instance->circ_buf.tail] << 16,
+					     instance->fifo_reg);
+				else
+					outl(instance->circ_buf.
+					     buf[instance->circ_buf.tail],
+					     instance->fifo_reg);
+			}
+
+			if (instance->circ_buf.tail >= instance->circ_buf.head)	/* Start from beginning */
+				instance->circ_buf.tail = 0;
+		}
+
+		spin_lock(&instance->subdevice_lock);
+
+		tmp = inl(instance->ctrl_reg);
+
+		if (!instance->wrap_remaining) {
+			PDEBUG("Finite SW wraparound done.\n");
+			tmp &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+		}
+
+		tmp |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+
+		outl(tmp, instance->ctrl_reg);
+		tmp &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+		outl(tmp, instance->ctrl_reg);
+
+		if (!(inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM)) {
+			PERROR("Broken pipe.\n");
+			instance->flags |= ME4600_AO_FLAGS_BROKEN_PIPE;
+			tmp &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(tmp, instance->ctrl_reg);
+		}
+
+		spin_unlock(&instance->subdevice_lock);
+
+	} else {		/* Regular continuous mode */
+
+		while (1) {
+			c1 = me_circ_buf_values_to_end(&instance->circ_buf);
+			PDEBUG("Values to end = %d.\n", c1);
+
+			if (c1 > c)
+				c1 = c;
+
+			if (c1 <= 0) {
+				PDEBUG("Work done or buffer empty.\n");
+				break;
+			}
+
+			if (instance->ao_idx & 0x1) {
+				for (i = 0; i < c1; i++) {
+					value =
+					    *(instance->circ_buf.buf +
+					      instance->circ_buf.tail +
+					      i) << 16;
+					outl(value, instance->fifo_reg);
+				}
+			} else
+				outsl(instance->fifo_reg,
+				      instance->circ_buf.buf +
+				      instance->circ_buf.tail, c1);
+
+			instance->circ_buf.tail =
+			    (instance->circ_buf.tail +
+			     c1) & (instance->circ_buf.mask);
+
+			PDEBUG("%d values wrote to port 0x%04X.\n", c1,
+			       instance->fifo_reg);
+
+			c -= c1;
+		}
+
+		spin_lock(&instance->subdevice_lock);
+
+		tmp = inl(instance->ctrl_reg);
+
+		if (!me_circ_buf_values(&instance->circ_buf)) {
+			PDEBUG
+			    ("Disable Interrupt because no values left in buffer.\n");
+			tmp &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+		}
+
+		tmp |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+
+		outl(tmp, instance->ctrl_reg);
+		tmp &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+		outl(tmp, instance->ctrl_reg);
+
+		if (!(inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM)) {
+			PDEBUG("Broken pipe in me4600_ao_isr.\n");
+			instance->flags |= ME4600_AO_FLAGS_BROKEN_PIPE;
+			tmp &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(tmp, instance->ctrl_reg);
+		}
+
+		spin_unlock(&instance->subdevice_lock);
+
+		wake_up_interruptible(&instance->wait_queue);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void me4600_ao_destructor(struct me_subdevice *subdevice)
+{
+	me4600_ao_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	free_irq(instance->irq, instance);
+	kfree(instance->circ_buf.buf);
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+me4600_ao_subdevice_t *me4600_ao_constructor(uint32_t reg_base,
+					     spinlock_t * preload_reg_lock,
+					     uint32_t * preload_flags,
+					     int ao_idx, int fifo, int irq)
+{
+	me4600_ao_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me4600_ao_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me4600_ao_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->preload_reg_lock = preload_reg_lock;
+	subdevice->preload_flags = preload_flags;
+
+	/* Allocate and initialize circular buffer */
+	subdevice->circ_buf.mask = ME4600_AO_CIRC_BUF_COUNT - 1;
+	subdevice->circ_buf.buf = kmalloc(ME4600_AO_CIRC_BUF_SIZE, GFP_KERNEL);
+
+	if (!subdevice->circ_buf.buf) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		me_subdevice_deinit((me_subdevice_t *) subdevice);
+		kfree(subdevice);
+		return NULL;
+	}
+
+	memset(subdevice->circ_buf.buf, 0, ME4600_AO_CIRC_BUF_SIZE);
+
+	subdevice->circ_buf.head = 0;
+	subdevice->circ_buf.tail = 0;
+
+	/* Initialize wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	/* Initialize single value to 0V */
+	subdevice->single_value = 0x8000;
+
+	/* Store analog output index */
+	subdevice->ao_idx = ao_idx;
+
+	/* Store if analog output has fifo */
+	subdevice->fifo = fifo;
+
+	/* Initialize registers */
+
+	if (ao_idx == 0) {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_00_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_00_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_00_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_00_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_00_TIMER_REG;
+		subdevice->reg_base = reg_base;
+
+		if (inl(subdevice->reg_base + ME4600_AO_BOSCH_REG) == 0x20000) {
+			PINFO("Bosch firmware in use for channel 0.\n");
+			subdevice->bosch_fw = 1;
+		} else {
+			subdevice->bosch_fw = 0;
+		}
+	} else if (ao_idx == 1) {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_01_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_01_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_01_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_01_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_01_TIMER_REG;
+		subdevice->reg_base = reg_base;
+		subdevice->bosch_fw = 0;
+	} else if (ao_idx == 2) {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_02_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_02_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_02_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_02_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_02_TIMER_REG;
+		subdevice->reg_base = reg_base;
+		subdevice->bosch_fw = 0;
+	} else {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_03_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_03_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_03_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_03_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_03_TIMER_REG;
+		subdevice->reg_base = reg_base;
+		subdevice->bosch_fw = 0;
+	}
+
+	subdevice->irq_status_reg = reg_base + ME4600_IRQ_STATUS_REG;
+	subdevice->preload_reg = reg_base + ME4600_AO_LOADSETREG_XX;
+
+	/* Register interrupt service routine */
+	subdevice->irq = irq;
+
+	if (request_irq
+	    (subdevice->irq, me4600_ao_isr, SA_INTERRUPT | SA_SHIRQ,
+	     ME4600_NAME, subdevice)) {
+		PERROR("Cannot get interrupt line.\n");
+		me_subdevice_deinit((me_subdevice_t *) subdevice);
+		kfree(subdevice->circ_buf.buf);
+		kfree(subdevice);
+		return NULL;
+	}
+
+	/* Override base class methods. */
+	subdevice->base.me_subdevice_destructor = me4600_ao_destructor;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me4600_ao_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me4600_ao_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me4600_ao_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me4600_ao_io_single_write;
+	subdevice->base.me_subdevice_io_stream_config =
+	    me4600_ao_io_stream_config;
+	subdevice->base.me_subdevice_io_stream_new_values =
+	    me4600_ao_io_stream_new_values;
+	subdevice->base.me_subdevice_io_stream_write =
+	    me4600_ao_io_stream_write;
+	subdevice->base.me_subdevice_io_stream_start =
+	    me4600_ao_io_stream_start;
+	subdevice->base.me_subdevice_io_stream_status =
+	    me4600_ao_io_stream_status;
+	subdevice->base.me_subdevice_io_stream_stop = me4600_ao_io_stream_stop;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me4600_ao_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me4600_ao_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me4600_ao_query_subdevice_caps;
+	subdevice->base.me_subdevice_query_subdevice_caps_args =
+	    me4600_ao_query_subdevice_caps_args;
+	subdevice->base.me_subdevice_query_range_by_min_max =
+	    me4600_ao_query_range_by_min_max;
+	subdevice->base.me_subdevice_query_number_ranges =
+	    me4600_ao_query_number_ranges;
+	subdevice->base.me_subdevice_query_range_info =
+	    me4600_ao_query_range_info;
+	subdevice->base.me_subdevice_query_timer = me4600_ao_query_timer;
+
+	return subdevice;
+}
+
+#endif // BOSCH
+
+/* Common functions
+*/
+
+static int me4600_ao_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range)
+{
+	me4600_ao_subdevice_t *instance;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if ((*max - *min) < 0) {
+		PERROR("Invalid minimum and maximum values specified.\n");
+		return ME_ERRNO_INVALID_MIN_MAX;
+	}
+
+	if ((unit == ME_UNIT_VOLT) || (unit == ME_UNIT_ANY)) {
+		if ((*max <= (ME4600_AO_MAX_RANGE + 1000))
+		    && (*min >= ME4600_AO_MIN_RANGE)) {
+			*min = ME4600_AO_MIN_RANGE;
+			*max = ME4600_AO_MAX_RANGE;
+			*maxdata = ME4600_AO_MAX_DATA;
+			*range = 0;
+		} else {
+			PERROR("No matching range available.\n");
+			return ME_ERRNO_NO_RANGE;
+		}
+	} else {
+		PERROR("Invalid physical unit specified.\n");
+		return ME_ERRNO_INVALID_UNIT;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ao_query_number_ranges(me_subdevice_t * subdevice,
+					 int unit, int *count)
+{
+	me4600_ao_subdevice_t *instance;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if ((unit == ME_UNIT_VOLT) || (unit == ME_UNIT_ANY)) {
+		*count = 1;
+	} else {
+		*count = 0;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ao_query_range_info(me_subdevice_t * subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata)
+{
+	me4600_ao_subdevice_t *instance;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (range == 0) {
+		*unit = ME_UNIT_VOLT;
+		*min = ME4600_AO_MIN_RANGE;
+		*max = ME4600_AO_MAX_RANGE;
+		*maxdata = ME4600_AO_MAX_DATA;
+	} else {
+		PERROR("Invalid range number specified.\n");
+		return ME_ERRNO_INVALID_RANGE;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ao_query_timer(me_subdevice_t * subdevice,
+				 int timer,
+				 int *base_frequency,
+				 long long *min_ticks, long long *max_ticks)
+{
+	me4600_ao_subdevice_t *instance;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if ((timer != ME_TIMER_ACQ_START) && (timer != ME_TIMER_CONV_START)) {
+		PERROR("Invalid timer specified.\n");
+		return ME_ERRNO_INVALID_TIMER;
+	}
+
+	if (instance->fifo) {	//Streaming device.
+		*base_frequency = ME4600_AO_BASE_FREQUENCY;
+		if (timer == ME_TIMER_ACQ_START) {
+			*min_ticks = ME4600_AO_MIN_ACQ_TICKS;
+			*max_ticks = ME4600_AO_MAX_ACQ_TICKS;
+		} else if (timer == ME_TIMER_CONV_START) {
+			*min_ticks = ME4600_AO_MIN_CHAN_TICKS;
+			*max_ticks = ME4600_AO_MAX_CHAN_TICKS;
+		}
+	} else {		//Not streaming device!
+		*base_frequency = 0;
+		*min_ticks = 0;
+		*max_ticks = 0;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ao_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	me4600_ao_subdevice_t *instance;
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*number = 1;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ao_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	me4600_ao_subdevice_t *instance;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*type = ME_TYPE_AO;
+	*subtype = (instance->fifo) ? ME_SUBTYPE_STREAMING : ME_SUBTYPE_SINGLE;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ao_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	me4600_ao_subdevice_t *instance;
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*caps =
+	    ME_CAPS_AO_TRIG_SYNCHRONOUS | ((instance->fifo) ? ME_CAPS_AO_FIFO :
+					   ME_CAPS_NONE);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ao_query_subdevice_caps_args(struct me_subdevice *subdevice,
+					       int cap, int *args, int count)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (count != 1) {
+		PERROR("Invalid capability argument count.\n");
+		return ME_ERRNO_INVALID_CAP_ARG_COUNT;
+	}
+
+	switch (cap) {
+	case ME_CAP_AI_FIFO_SIZE:
+		args[0] = (instance->fifo) ? ME4600_AO_FIFO_COUNT : 0;
+		break;
+
+	case ME_CAP_AI_BUFFER_SIZE:
+		args[0] =
+		    (instance->circ_buf.buf) ? ME4600_AO_CIRC_BUF_COUNT : 0;
+		break;
+
+	default:
+		PERROR("Invalid capability.\n");
+		err = ME_ERRNO_INVALID_CAP;
+		args[0] = 0;
+	}
+
+	return err;
+}
diff --git a/drivers/staging/meilhaus/me4600_ao.h b/drivers/staging/meilhaus/me4600_ao.h
new file mode 100644
index 000000000000..6fbc4a2dd9dd
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ao.h
@@ -0,0 +1,263 @@
+/**
+ * @file me4600_ao.h
+ *
+ * @brief Meilhaus ME-4000 analog output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_AO_H_
+# define _ME4600_AO_H_
+
+# include <linux/version.h>
+# include "mesubdevice.h"
+# include "mecirc_buf.h"
+# include "meioctl.h"
+
+# ifdef __KERNEL__
+
+#  ifdef BOSCH
+#   undef ME_SYNAPSE
+#   ifndef _CBUFF_32b_t
+# 	 define _CBUFF_32b_t
+#   endif //_CBUFF_32b_t
+#  endif //BOSCH
+
+#  define ME4600_AO_MAX_SUBDEVICES		4
+#  define ME4600_AO_FIFO_COUNT			4096
+
+#  define ME4600_AO_BASE_FREQUENCY		33000000LL
+
+#  define ME4600_AO_MIN_ACQ_TICKS		0LL
+#  define ME4600_AO_MAX_ACQ_TICKS		0LL
+
+#  define ME4600_AO_MIN_CHAN_TICKS		66LL
+#  define ME4600_AO_MAX_CHAN_TICKS		0xFFFFFFFFLL
+
+#  define ME4600_AO_MIN_RANGE			-10000000
+#  define ME4600_AO_MAX_RANGE			9999694
+
+#  define ME4600_AO_MAX_DATA			0xFFFF
+
+#  ifdef ME_SYNAPSE
+#   define ME4600_AO_CIRC_BUF_SIZE_ORDER 		8	// 2^n PAGES =>> Maximum value of 1MB for Synapse
+#  else
+#   define ME4600_AO_CIRC_BUF_SIZE_ORDER 		5	// 2^n PAGES =>> 128KB
+#  endif
+#  define ME4600_AO_CIRC_BUF_SIZE 		PAGE_SIZE<<ME4600_AO_CIRC_BUF_SIZE_ORDER	// Buffer size in bytes.
+
+#  ifdef _CBUFF_32b_t
+#   define ME4600_AO_CIRC_BUF_COUNT	((ME4600_AO_CIRC_BUF_SIZE) / sizeof(uint32_t))	// Size in values
+#  else
+#   define ME4600_AO_CIRC_BUF_COUNT	((ME4600_AO_CIRC_BUF_SIZE) / sizeof(uint16_t))	// Size in values
+#  endif
+
+#  define ME4600_AO_CONTINOUS					0x0
+#  define ME4600_AO_WRAP_MODE					0x1
+#  define ME4600_AO_HW_MODE						0x2
+
+#  define ME4600_AO_HW_WRAP_MODE				(ME4600_AO_WRAP_MODE | ME4600_AO_HW_MODE)
+#  define ME4600_AO_SW_WRAP_MODE				ME4600_AO_WRAP_MODE
+
+#  define ME4600_AO_INF_STOP_MODE				0x0
+#  define ME4600_AO_ACQ_STOP_MODE				0x1
+#  define ME4600_AO_SCAN_STOP_MODE				0x2
+
+#  ifdef BOSCH			//SPECIAL BUILD FOR BOSCH
+
+/* Bits for flags attribute. */
+#   define ME4600_AO_FLAGS_BROKEN_PIPE			0x1
+#   define ME4600_AO_FLAGS_SW_WRAP_MODE_0		0x2
+#   define ME4600_AO_FLAGS_SW_WRAP_MODE_1		0x4
+#   define ME4600_AO_FLAGS_SW_WRAP_MODE_MASK	(ME4600_AO_FLAGS_SW_WRAP_MODE_0 | ME4600_AO_FLAGS_SW_WRAP_MODE_1)
+
+#   define ME4600_AO_FLAGS_SW_WRAP_MODE_NONE	0x0
+#   define ME4600_AO_FLAGS_SW_WRAP_MODE_INF		0x2
+#   define ME4600_AO_FLAGS_SW_WRAP_MODE_FIN		0x4
+
+	/**
+	* @brief The ME-4000 analog output subdevice class.
+	*/
+typedef struct me4600_ao_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;				/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;			/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *preload_reg_lock;		/**< Spin lock to protect #preload_reg from concurrent access. */
+	uint32_t *preload_flags;
+
+	unsigned int irq;					/**< The interrupt request number assigned by the PCI BIOS. */
+	me_circ_buf_t circ_buf;				/**< Circular buffer holding measurment data. */
+	wait_queue_head_t wait_queue;		/**< Wait queue to put on tasks waiting for data to arrive. */
+
+	int single_value;					/**< Mirror of the value written in single mode. */
+
+	int volatile flags;					/**< Flags used for storing SW wraparound setup and error signalling from ISR. */
+	unsigned int wrap_count;			/**< The user defined wraparound cycle count. */
+	unsigned int wrap_remaining;		/**< The wraparound cycle down counter used by a running conversion. */
+	unsigned int ao_idx;				/**< The index of this analog output on this device. */
+	int fifo;							/**< If set this device has a FIFO. */
+
+	int bosch_fw;						/**< If set the bosch firmware is in PROM. */
+
+	/* Registers */
+	uint32_t ctrl_reg;
+	uint32_t status_reg;
+	uint32_t fifo_reg;
+	uint32_t single_reg;
+	uint32_t timer_reg;
+	uint32_t irq_status_reg;
+	uint32_t preload_reg;
+	uint32_t reg_base;
+} me4600_ao_subdevice_t;
+
+	/**
+	* @brief The constructor to generate a ME-4000 analog output subdevice instance for BOSCH project.
+	*
+	* @param reg_base The register base address of the device as returned by the PCI BIOS.
+	* @param ctrl_reg_lock Pointer to spin lock protecting the control register from concurrent access.
+	* @param preload_flags Pointer to spin lock protecting the hold&trigger register from concurrent access.
+	* @param ao_idx Subdevice number.
+	* @param fifo Flag set if subdevice has hardware FIFO.
+	* @param irq IRQ number.
+	*
+	* @return Pointer to new instance on success.\n
+	* NULL on error.
+	*/
+me4600_ao_subdevice_t *me4600_ao_constructor(uint32_t reg_base,
+					     spinlock_t * preload_reg_lock,
+					     uint32_t * preload_flags,
+					     int ao_idx, int fifo, int irq);
+
+#  else	//~BOSCH
+
+//ME4600_AO_FLAGS_BROKEN_PIPE is OBSOLETE => Now problems are reported in status.
+
+typedef enum ME4600_AO_STATUS {
+	ao_status_none = 0,
+	ao_status_single_configured,
+	ao_status_single_run_wait,
+	ao_status_single_run,
+	ao_status_single_end_wait,
+	ao_status_single_end,
+	ao_status_stream_configured,
+	ao_status_stream_run_wait,
+	ao_status_stream_run,
+	ao_status_stream_end_wait,
+	ao_status_stream_end,
+	ao_status_stream_fifo_error,
+	ao_status_stream_buffer_error,
+	ao_status_stream_error,
+	ao_status_last
+} ME4600_AO_STATUS;
+
+typedef struct me4600_ao_timeout {
+	unsigned long start_time;
+	unsigned long delay;
+} me4600_ao_timeout_t;
+
+	/**
+	* @brief The ME-4600 analog output subdevice class.
+	*/
+typedef struct me4600_ao_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;						/**< The subdevice base class. */
+	unsigned int ao_idx;						/**< The index of this analog output on this device. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;					/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *preload_reg_lock;				/**< Spin lock to protect preload_reg from concurrent access. */
+
+	uint32_t *preload_flags;
+
+	/* Hardware feautres */
+	unsigned int irq;							/**< The interrupt request number assigned by the PCI BIOS. */
+	int fifo;									/**< If set this device has a FIFO. */
+	int bitpattern;								/**< If set this device use bitpattern. */
+
+	int single_value;							/**< Mirror of the output value in single mode. */
+	int single_value_in_fifo;					/**< Mirror of the value written in single mode. */
+	uint32_t ctrl_trg;							/**< Mirror of the trigger settings. */
+
+	volatile int mode;							/**< Flags used for storing SW wraparound setup*/
+	int stop_mode;								/**< The user defined stop condition flag. */
+	unsigned int start_mode;
+	unsigned int stop_count;					/**< The user defined dates presentation end count. */
+	unsigned int stop_data_count;				/**< The stop presentation count. */
+	unsigned int data_count;					/**< The real presentation count. */
+	unsigned int preloaded_count;				/**< The next data addres in buffer. <= for wraparound mode. */
+	int hardware_stop_delay;					/**< The time that stop can take. This is only to not show hardware bug to user. */
+
+	volatile enum ME4600_AO_STATUS status;		/**< The current stream status flag. */
+	me4600_ao_timeout_t timeout;				/**< The timeout for start in blocking and non-blocking mode. */
+
+										/* Registers *//**< All registers are 32 bits long. */
+	unsigned long ctrl_reg;
+	unsigned long status_reg;
+	unsigned long fifo_reg;
+	unsigned long single_reg;
+	unsigned long timer_reg;
+	unsigned long irq_status_reg;
+	unsigned long preload_reg;
+	unsigned long reg_base;
+
+	/* Software buffer */
+	me_circ_buf_t circ_buf;						/**< Circular buffer holding measurment data. 32 bit long */
+	wait_queue_head_t wait_queue;				/**< Wait queue to put on tasks waiting for data to arrive. */
+
+	struct workqueue_struct *me4600_workqueue;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	struct work_struct ao_control_task;
+#else
+	struct delayed_work ao_control_task;
+#endif
+
+	volatile int ao_control_task_flag;			/**< Flag controling reexecuting of control task */
+
+} me4600_ao_subdevice_t;
+
+	/**
+	* @brief The constructor to generate a ME-4600 analog output subdevice instance.
+	*
+	* @param reg_base The register base address of the device as returned by the PCI BIOS.
+	* @param ctrl_reg_lock Pointer to spin lock protecting the control register from concurrent access.
+	* @param preload_flags Pointer to spin lock protecting the hold&trigger register from concurrent access.
+	* @param ao_idx Subdevice number.
+	* @param fifo Flag set if subdevice has hardware FIFO.
+	* @param irq IRQ number.
+	* @param me4600_wq Queue for asynchronous task (1 queue for all subdevice on 1 board).
+	*
+	* @return Pointer to new instance on success.\n
+	* NULL on error.
+	*/
+me4600_ao_subdevice_t *me4600_ao_constructor(uint32_t reg_base,
+					     spinlock_t * preload_reg_lock,
+					     uint32_t * preload_flags,
+					     int ao_idx,
+					     int fifo,
+					     int irq,
+					     struct workqueue_struct
+					     *me4600_wq);
+
+#  endif //BOSCH
+# endif	//__KERNEL__
+#endif // ~_ME4600_AO_H_
diff --git a/drivers/staging/meilhaus/me4600_ao_reg.h b/drivers/staging/meilhaus/me4600_ao_reg.h
new file mode 100644
index 000000000000..f83d82ecd4bf
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ao_reg.h
@@ -0,0 +1,113 @@
+/**
+ * @file me4600_ao_reg.h
+ *
+ * @brief ME-4000 analog output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_AO_REG_H_
+#define _ME4600_AO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME4600_AO_00_CTRL_REG					0x00	// R/W
+#define ME4600_AO_00_STATUS_REG					0x04	// R/_
+#define ME4600_AO_00_FIFO_REG					0x08	// _/W
+#define ME4600_AO_00_SINGLE_REG					0x0C	// R/W
+#define ME4600_AO_00_TIMER_REG					0x10	// _/W
+
+#define ME4600_AO_01_CTRL_REG					0x18	// R/W
+#define ME4600_AO_01_STATUS_REG					0x1C	// R/_
+#define ME4600_AO_01_FIFO_REG					0x20	// _/W
+#define ME4600_AO_01_SINGLE_REG					0x24	// R/W
+#define ME4600_AO_01_TIMER_REG					0x28	// _/W
+
+#define ME4600_AO_02_CTRL_REG					0x30	// R/W
+#define ME4600_AO_02_STATUS_REG					0x34	// R/_
+#define ME4600_AO_02_FIFO_REG					0x38	// _/W
+#define ME4600_AO_02_SINGLE_REG					0x3C	// R/W
+#define ME4600_AO_02_TIMER_REG					0x40	// _/W
+
+#define ME4600_AO_03_CTRL_REG					0x48	// R/W
+#define ME4600_AO_03_STATUS_REG					0x4C	// R/_
+#define ME4600_AO_03_FIFO_REG					0x50	// _/W
+#define ME4600_AO_03_SINGLE_REG					0x54	// R/W
+#define ME4600_AO_03_TIMER_REG					0x58	// _/W
+
+#define ME4600_AO_DEMUX_ADJUST_REG				0xBC	// -/W
+#define ME4600_AO_DEMUX_ADJUST_VALUE			0x4C
+
+#ifdef BOSCH
+# define ME4600_AO_BOSCH_REG					0xC4
+
+# define ME4600_AO_LOADSETREG_XX				0xB4	// R/W
+
+# define ME4600_AO_CTRL_BIT_MODE_0				0x001
+# define ME4600_AO_CTRL_BIT_MODE_1				0x002
+# define ME4600_AO_CTRL_MASK_MODE				0x003
+
+#else //~BOSCH
+
+#define ME4600_AO_SYNC_REG						0xB4	// R/W    ///ME4600_AO_SYNC_REG <==> ME4600_AO_PRELOAD_REG <==> ME4600_AO_LOADSETREG_XX
+
+# define ME4600_AO_MODE_SINGLE					0x00000000
+# define ME4600_AO_MODE_WRAPAROUND				0x00000001
+# define ME4600_AO_MODE_CONTINUOUS				0x00000002
+# define ME4600_AO_CTRL_MODE_MASK				(ME4600_AO_MODE_WRAPAROUND | ME4600_AO_MODE_CONTINUOUS)
+#endif //BOSCH
+
+#define ME4600_AO_CTRL_BIT_MODE_WRAPAROUND		ME4600_AO_MODE_WRAPAROUND
+#define ME4600_AO_CTRL_BIT_MODE_CONTINOUS		ME4600_AO_MODE_CONTINUOUS
+#define ME4600_AO_CTRL_BIT_STOP					0x00000004
+#define ME4600_AO_CTRL_BIT_ENABLE_FIFO			0x00000008
+#define ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG		0x00000010
+#define ME4600_AO_CTRL_BIT_EX_TRIG_EDGE			0x00000020
+#define ME4600_AO_CTRL_BIT_IMMEDIATE_STOP		0x00000080
+#define ME4600_AO_CTRL_BIT_ENABLE_DO			0x00000100
+#define ME4600_AO_CTRL_BIT_ENABLE_IRQ			0x00000200
+#define ME4600_AO_CTRL_BIT_RESET_IRQ			0x00000400
+#define ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH	0x00000800
+/*
+#define ME4600_AO_SYNC_HOLD_0					0x00000001
+#define ME4600_AO_SYNC_HOLD_1					0x00000002
+#define ME4600_AO_SYNC_HOLD_2					0x00000004
+#define ME4600_AO_SYNC_HOLD_3					0x00000008
+*/
+#define ME4600_AO_SYNC_HOLD						0x00000001
+
+/*
+#define ME4600_AO_SYNC_EXT_TRIG_0				0x00010000
+#define ME4600_AO_SYNC_EXT_TRIG_1				0x00020000
+#define ME4600_AO_SYNC_EXT_TRIG_2				0x00040000
+#define ME4600_AO_SYNC_EXT_TRIG_3				0x00080000
+*/
+#define ME4600_AO_SYNC_EXT_TRIG					0x00010000
+
+#define ME4600_AO_EXT_TRIG						0x80000000
+
+#define ME4600_AO_STATUS_BIT_FSM				0x00000001
+#define ME4600_AO_STATUS_BIT_FF					0x00000002
+#define ME4600_AO_STATUS_BIT_HF					0x00000004
+#define ME4600_AO_STATUS_BIT_EF					0x00000008
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_device.c b/drivers/staging/meilhaus/me4600_device.c
new file mode 100644
index 000000000000..fa455844f4e3
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_device.c
@@ -0,0 +1,373 @@
+/**
+ * @file me4600_device.c
+ *
+ * @brief ME-4600 device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "medevice.h"
+#include "me4600_device.h"
+#include "meplx_reg.h"
+
+#include "mefirmware.h"
+
+#include "mesubdevice.h"
+#include "me4600_do.h"
+#include "me4600_di.h"
+#include "me4600_dio.h"
+#include "me8254.h"
+#include "me4600_ai.h"
+#include "me4600_ao.h"
+#include "me4600_ext_irq.h"
+
+/**
+ * @brief Global variable.
+ * This is working queue for runing a separate atask that will be responsible for work status (start, stop, timeouts).
+ */
+static struct workqueue_struct *me4600_workqueue;
+
+#ifdef BOSCH
+me_device_t *me4600_pci_constructor(struct pci_dev *pci_device, int me_bosch_fw)
+#else //~BOSCH
+me_device_t *me4600_pci_constructor(struct pci_dev *pci_device)
+#endif				//BOSCH
+{
+	me4600_device_t *me4600_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	int err;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me4600_device = kmalloc(sizeof(me4600_device_t), GFP_KERNEL);
+
+	if (!me4600_device) {
+		PERROR("Cannot get memory for ME-4600 device instance.\n");
+		return NULL;
+	}
+
+	memset(me4600_device, 0, sizeof(me4600_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me4600_device, pci_device);
+
+	if (err) {
+		kfree(me4600_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+	// Download the xilinx firmware.
+	if (me4600_device->base.info.pci.device_id == PCI_DEVICE_ID_MEILHAUS_ME4610) {	//Jekyll <=> me4610
+		err =
+		    me_xilinx_download(me4600_device->base.info.pci.
+				       reg_bases[1],
+				       me4600_device->base.info.pci.
+				       reg_bases[5], &pci_device->dev,
+				       "me4610.bin");
+	} else {		// General me4600 firmware
+#ifdef BOSCH
+		err =
+		    me_xilinx_download(me4600_device->base.info.pci.
+				       reg_bases[1],
+				       me4600_device->base.info.pci.
+				       reg_bases[5], &pci_device->dev,
+				       (me_bosch_fw) ? "me4600_bosch.bin" :
+				       "me4600.bin");
+#else //~BOSCH
+		err =
+		    me_xilinx_download(me4600_device->base.info.pci.
+				       reg_bases[1],
+				       me4600_device->base.info.pci.
+				       reg_bases[5], &pci_device->dev,
+				       "me4600.bin");
+#endif
+	}
+
+	if (err) {
+		me_device_deinit((me_device_t *) me4600_device);
+		kfree(me4600_device);
+		PERROR("Cannot download firmware.\n");
+		return NULL;
+	}
+	// Get the index in the device version information table.
+	version_idx =
+	    me4600_versions_get_device_index(me4600_device->base.info.pci.
+					     device_id);
+
+	// Initialize spin locks.
+	spin_lock_init(&me4600_device->preload_reg_lock);
+
+	me4600_device->preload_flags = 0;
+
+	spin_lock_init(&me4600_device->dio_lock);
+	spin_lock_init(&me4600_device->ai_ctrl_lock);
+	spin_lock_init(&me4600_device->ctr_ctrl_reg_lock);
+	spin_lock_init(&me4600_device->ctr_clk_src_reg_lock);
+
+	// Create digital input instances.
+	for (i = 0; i < me4600_versions[version_idx].di_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me4600_di_constructor(me4600_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     &me4600_device->
+							     dio_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me4600_device);
+			kfree(me4600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me4600_device->base.slist,
+					    subdevice);
+	}
+
+	// Create digital output instances.
+	for (i = 0; i < me4600_versions[version_idx].do_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me4600_do_constructor(me4600_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     &me4600_device->
+							     dio_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me4600_device);
+			kfree(me4600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me4600_device->base.slist,
+					    subdevice);
+	}
+
+	// Create digital input/output instances.
+	for (i = 0; i < me4600_versions[version_idx].dio_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me4600_dio_constructor(me4600_device->
+							      base.info.pci.
+							      reg_bases[2],
+							      me4600_versions
+							      [version_idx].
+							      do_subdevices +
+							      me4600_versions
+							      [version_idx].
+							      di_subdevices + i,
+							      &me4600_device->
+							      dio_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me4600_device);
+			kfree(me4600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me4600_device->base.slist,
+					    subdevice);
+	}
+
+	// Create analog input instances.
+	for (i = 0; i < me4600_versions[version_idx].ai_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me4600_ai_constructor(me4600_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     me4600_versions
+							     [version_idx].
+							     ai_channels,
+							     me4600_versions
+							     [version_idx].
+							     ai_ranges,
+							     me4600_versions
+							     [version_idx].
+							     ai_isolated,
+							     me4600_versions
+							     [version_idx].
+							     ai_sh,
+							     me4600_device->
+							     base.irq,
+							     &me4600_device->
+							     ai_ctrl_lock,
+							     me4600_workqueue);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me4600_device);
+			kfree(me4600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me4600_device->base.slist,
+					    subdevice);
+	}
+
+	// Create analog output instances.
+	for (i = 0; i < me4600_versions[version_idx].ao_subdevices; i++) {
+#ifdef BOSCH
+		subdevice =
+		    (me_subdevice_t *) me4600_ao_constructor(me4600_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     &me4600_device->
+							     preload_reg_lock,
+							     &me4600_device->
+							     preload_flags, i,
+							     me4600_versions
+							     [version_idx].
+							     ao_fifo,
+							     me4600_device->
+							     base.irq);
+#else //~BOSCH
+		subdevice =
+		    (me_subdevice_t *) me4600_ao_constructor(me4600_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     &me4600_device->
+							     preload_reg_lock,
+							     &me4600_device->
+							     preload_flags, i,
+							     me4600_versions
+							     [version_idx].
+							     ao_fifo,
+							     me4600_device->
+							     base.irq,
+							     me4600_workqueue);
+#endif
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me4600_device);
+			kfree(me4600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me4600_device->base.slist,
+					    subdevice);
+	}
+
+	// Create counter instances.
+	for (i = 0; i < me4600_versions[version_idx].ctr_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me8254_constructor(me4600_device->base.
+							  info.pci.device_id,
+							  me4600_device->base.
+							  info.pci.reg_bases[3],
+							  0, i,
+							  &me4600_device->
+							  ctr_ctrl_reg_lock,
+							  &me4600_device->
+							  ctr_clk_src_reg_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me4600_device);
+			kfree(me4600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me4600_device->base.slist,
+					    subdevice);
+	}
+
+	// Create external interrupt instances.
+	for (i = 0; i < me4600_versions[version_idx].ext_irq_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *)
+		    me4600_ext_irq_constructor(me4600_device->base.info.pci.
+					       reg_bases[2],
+					       me4600_device->base.irq,
+					       &me4600_device->ai_ctrl_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me4600_device);
+			kfree(me4600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me4600_device->base.slist,
+					    subdevice);
+	}
+
+	return (me_device_t *) me4600_device;
+}
+
+// Init and exit of module.
+
+static int __init me4600_init(void)
+{
+	PDEBUG("executed.\n");
+
+#ifndef BOSCH
+	me4600_workqueue = create_singlethread_workqueue("me4600");
+#endif
+	return 0;
+}
+
+static void __exit me4600_exit(void)
+{
+	PDEBUG("executed.\n");
+
+#ifndef BOSCH
+	flush_workqueue(me4600_workqueue);
+	destroy_workqueue(me4600_workqueue);
+#endif
+}
+
+module_init(me4600_init);
+module_exit(me4600_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for ME-46xx Devices");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-46xx Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me4600_pci_constructor);
diff --git a/drivers/staging/meilhaus/me4600_device.h b/drivers/staging/meilhaus/me4600_device.h
new file mode 100644
index 000000000000..fa812d4cc6dc
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_device.h
@@ -0,0 +1,151 @@
+/**
+ * @file me4600_device.h
+ *
+ * @brief ME-4600 device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_DEVICE_H
+#define _ME4600_DEVICE_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure holding ME-4600 device capabilities.
+ */
+typedef struct me4600_version {
+	uint16_t device_id;
+	unsigned int do_subdevices;
+	unsigned int di_subdevices;
+	unsigned int dio_subdevices;
+	unsigned int ctr_subdevices;
+	unsigned int ai_subdevices;
+	unsigned int ai_channels;
+	unsigned int ai_ranges;
+	unsigned int ai_isolated;
+	unsigned int ai_sh;
+	unsigned int ao_subdevices;
+	unsigned int ao_fifo;	//How many devices have FIFO
+	unsigned int ext_irq_subdevices;
+} me4600_version_t;
+
+/**
+ * @brief ME-4600 device capabilities.
+ */
+static me4600_version_t me4600_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME4610, 0, 0, 4, 3, 1, 16, 1, 0, 0, 0, 0, 1},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME4650, 0, 0, 4, 0, 1, 16, 4, 0, 0, 0, 0, 1},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME4660, 0, 0, 4, 3, 1, 16, 4, 0, 0, 2, 0, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4660I, 1, 1, 2, 3, 1, 16, 4, 1, 0, 2, 0, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4660S, 0, 0, 4, 3, 1, 16, 4, 0, 1, 2, 0, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4660IS, 1, 1, 2, 3, 1, 16, 4, 1, 1, 2, 0, 1},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME4670, 0, 0, 4, 3, 1, 32, 4, 0, 0, 4, 0, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4670I, 1, 1, 2, 3, 1, 32, 4, 1, 0, 4, 0, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4670S, 0, 0, 4, 3, 1, 32, 4, 0, 1, 4, 0, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4670IS, 1, 1, 2, 3, 1, 32, 4, 1, 1, 4, 0, 1},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME4680, 0, 0, 4, 3, 1, 32, 4, 0, 0, 4, 4, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4680I, 1, 1, 2, 3, 1, 32, 4, 1, 0, 4, 4, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4680S, 0, 0, 4, 3, 1, 32, 4, 0, 1, 4, 4, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4680IS, 1, 1, 2, 3, 1, 32, 4, 1, 1, 4, 4, 1},
+
+	{0},
+};
+
+#define ME4600_DEVICE_VERSIONS (sizeof(me4600_versions) / sizeof(me4600_version_t) - 1)	/**< Returns the number of entries in #me4600_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #me4600_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me4600_versions.
+ */
+static inline unsigned int me4600_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME4600_DEVICE_VERSIONS; i++)
+		if (me4600_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The ME-4600 device class structure.
+ */
+typedef struct me4600_device {
+	me_device_t base;					/**< The Meilhaus device base class. */
+
+	/* Child class attributes. */
+	spinlock_t preload_reg_lock;		/**< Guards the preload register of the anaolog output devices. */
+	unsigned int preload_flags;			/**< Used in conjunction with #preload_reg_lock. */
+	spinlock_t dio_lock;				/**< Locks the control register of the digital input/output subdevices. */
+	spinlock_t ai_ctrl_lock;			/**< Locks the control register of the analog input subdevice. */
+	spinlock_t ctr_ctrl_reg_lock;		/**< Locks the counter control register. */
+	spinlock_t ctr_clk_src_reg_lock;	/**< Not used on this device but needed for the me8254 subdevice constructor call. */
+} me4600_device_t;
+
+/**
+ * @brief The ME-4600 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ * @param me_bosch_fw If set the device shall use the bosch firmware. (Only for special BOSCH build)
+ *
+ * @return On succes a new ME-4600 device instance. \n
+ *         NULL on error.
+ */
+
+#ifdef BOSCH
+/**
+ * @brief The ME-4600 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ * @param me_bosch_fw If set the device shall use the bosch firmware.
+ *
+ * @return On succes a new ME-4600 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me4600_pci_constructor(struct pci_dev *pci_device, int me_bosch_fw)
+    __attribute__ ((weak));
+#else //~BOSCH
+/**
+ * @brief The ME-4600 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-4600 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me4600_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+#endif
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_di.c b/drivers/staging/meilhaus/me4600_di.c
new file mode 100644
index 000000000000..7e3c9f4d2df2
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_di.c
@@ -0,0 +1,256 @@
+/**
+ * @file me4600_di.c
+ *
+ * @brief ME-4000 digital input subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me4600_dio_reg.h"
+#include "me4600_di.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me4600_di_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	me4600_di_subdevice_t *instance;
+	uint32_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_di_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inl(instance->ctrl_reg);
+	mode &= ~(ME4600_DIO_CTRL_BIT_MODE_2 | ME4600_DIO_CTRL_BIT_MODE_3);	//0xFFF3
+	outl(mode, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, mode);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outl(0, instance->port_reg);
+	PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_di_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me4600_di_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+			} else {
+				PERROR("Invalid port direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_di_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me4600_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = inl(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inl(instance->port_reg) & 0xFF;
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_di_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_di_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DI;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_di_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+me4600_di_subdevice_t *me4600_di_constructor(uint32_t reg_base,
+					     spinlock_t * ctrl_reg_lock)
+{
+	me4600_di_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me4600_di_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me4600_di_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save the subdevice index */
+	subdevice->port_reg = reg_base + ME4600_DIO_PORT_1_REG;
+	subdevice->ctrl_reg = reg_base + ME4600_DIO_CTRL_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me4600_di_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me4600_di_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me4600_di_io_single_read;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me4600_di_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me4600_di_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me4600_di_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me4600_di.h b/drivers/staging/meilhaus/me4600_di.h
new file mode 100644
index 000000000000..ec8b175755be
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_di.h
@@ -0,0 +1,64 @@
+/**
+ * @file me4600_di.h
+ *
+ * @brief ME-4000 digital input subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_DI_H_
+#define _ME4600_DI_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me4600_di_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+
+	unsigned long port_reg;			/**< Register holding the port status. */
+	unsigned long ctrl_reg;			/**< Register to configure the port direction. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me4600_di_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-4000 digital input subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me4600_di_subdevice_t *me4600_di_constructor(uint32_t reg_base,
+					     spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_dio.c b/drivers/staging/meilhaus/me4600_dio.c
new file mode 100644
index 000000000000..0af95d1a8f5d
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_dio.c
@@ -0,0 +1,510 @@
+/**
+ * @file me4600_dio.c
+ *
+ * @brief ME-4000 digital input/output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me4600_dio_reg.h"
+#include "me4600_dio.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me4600_dio_io_reset_subdevice(struct me_subdevice *subdevice,
+					 struct file *filep, int flags)
+{
+	me4600_dio_subdevice_t *instance;
+	uint32_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_dio_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	/* Set port to input mode */
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inl(instance->ctrl_reg);
+	mode &=
+	    ~((ME4600_DIO_CTRL_BIT_MODE_0 | ME4600_DIO_CTRL_BIT_MODE_1) <<
+	      (instance->dio_idx * 2));
+	outl(mode, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, mode);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outl(0, instance->port_reg);
+	PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_dio_io_single_config(me_subdevice_t * subdevice,
+				       struct file *filep,
+				       int channel,
+				       int single_config,
+				       int ref,
+				       int trig_chan,
+				       int trig_type, int trig_edge, int flags)
+{
+	me4600_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t mode;
+	uint32_t size =
+	    flags & (ME_IO_SINGLE_CONFIG_DIO_BIT | ME_IO_SINGLE_CONFIG_DIO_BYTE
+		     | ME_IO_SINGLE_CONFIG_DIO_WORD |
+		     ME_IO_SINGLE_CONFIG_DIO_DWORD);
+	uint32_t mask;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inl(instance->ctrl_reg);
+	switch (size) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+				mode &=
+				    ~((ME4600_DIO_CTRL_BIT_MODE_0 |
+				       ME4600_DIO_CTRL_BIT_MODE_1) <<
+				      (instance->dio_idx * 2));
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+				mode &=
+				    ~((ME4600_DIO_CTRL_BIT_MODE_0 |
+				       ME4600_DIO_CTRL_BIT_MODE_1) <<
+				      (instance->dio_idx * 2));
+				mode |=
+				    ME4600_DIO_CTRL_BIT_MODE_0 << (instance->
+								   dio_idx * 2);
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_MUX32M) {
+				mask =
+				    (ME4600_DIO_CTRL_BIT_MODE_0 |
+				     ME4600_DIO_CTRL_BIT_MODE_1) << (instance->
+								     dio_idx *
+								     2);
+				mask |=
+				    ME4600_DIO_CTRL_BIT_FUNCTION_0 |
+				    ME4600_DIO_CTRL_BIT_FUNCTION_1;
+				mask |=
+				    ME4600_DIO_CTRL_BIT_FIFO_HIGH_0 <<
+				    instance->dio_idx;
+				mode &= ~mask;
+
+				if (ref == ME_REF_DIO_FIFO_LOW) {
+					mode |=
+					    (ME4600_DIO_CTRL_BIT_MODE_0 |
+					     ME4600_DIO_CTRL_BIT_MODE_1) <<
+					    (instance->dio_idx * 2);
+					mode |= ME4600_DIO_CTRL_BIT_FUNCTION_1;
+				} else if (ref == ME_REF_DIO_FIFO_HIGH) {
+					mode |=
+					    (ME4600_DIO_CTRL_BIT_MODE_0 |
+					     ME4600_DIO_CTRL_BIT_MODE_1) <<
+					    (instance->dio_idx * 2);
+					mode |= ME4600_DIO_CTRL_BIT_FUNCTION_1;
+					mode |=
+					    ME4600_DIO_CTRL_BIT_FIFO_HIGH_0 <<
+					    instance->dio_idx;
+				} else {
+					PERROR
+					    ("Invalid port reference specified.\n");
+					err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+				}
+			} else if (single_config ==
+				   ME_SINGLE_CONFIG_DIO_DEMUX32) {
+				mask =
+				    (ME4600_DIO_CTRL_BIT_MODE_0 |
+				     ME4600_DIO_CTRL_BIT_MODE_1) << (instance->
+								     dio_idx *
+								     2);
+				mask |=
+				    ME4600_DIO_CTRL_BIT_FUNCTION_0 |
+				    ME4600_DIO_CTRL_BIT_FUNCTION_1;
+				mask |=
+				    ME4600_DIO_CTRL_BIT_FIFO_HIGH_0 <<
+				    instance->dio_idx;
+				mode &= ~mask;
+
+				if (ref == ME_REF_DIO_FIFO_LOW) {
+					mode |=
+					    (ME4600_DIO_CTRL_BIT_MODE_0 |
+					     ME4600_DIO_CTRL_BIT_MODE_1) <<
+					    (instance->dio_idx * 2);
+					mode |= ME4600_DIO_CTRL_BIT_FUNCTION_0;
+				} else if (ref == ME_REF_DIO_FIFO_HIGH) {
+					mode |=
+					    (ME4600_DIO_CTRL_BIT_MODE_0 |
+					     ME4600_DIO_CTRL_BIT_MODE_1) <<
+					    (instance->dio_idx * 2);
+					mode |= ME4600_DIO_CTRL_BIT_FUNCTION_0;
+					mode |=
+					    ME4600_DIO_CTRL_BIT_FIFO_HIGH_0 <<
+					    instance->dio_idx;
+				} else {
+					PERROR
+					    ("Invalid port reference specified.\n");
+					err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+				}
+			} else if (single_config ==
+				   ME_SINGLE_CONFIG_DIO_BIT_PATTERN) {
+				mask =
+				    (ME4600_DIO_CTRL_BIT_MODE_0 |
+				     ME4600_DIO_CTRL_BIT_MODE_1) << (instance->
+								     dio_idx *
+								     2);
+				mask |=
+				    ME4600_DIO_CTRL_BIT_FUNCTION_0 |
+				    ME4600_DIO_CTRL_BIT_FUNCTION_1;
+				mask |=
+				    ME4600_DIO_CTRL_BIT_FIFO_HIGH_0 <<
+				    instance->dio_idx;
+				mode &= ~mask;
+
+				if (ref == ME_REF_DIO_FIFO_LOW) {
+					mode |=
+					    (ME4600_DIO_CTRL_BIT_MODE_0 |
+					     ME4600_DIO_CTRL_BIT_MODE_1) <<
+					    (instance->dio_idx * 2);
+				} else if (ref == ME_REF_DIO_FIFO_HIGH) {
+					mode |=
+					    (ME4600_DIO_CTRL_BIT_MODE_0 |
+					     ME4600_DIO_CTRL_BIT_MODE_1) <<
+					    (instance->dio_idx * 2);
+					mode |=
+					    ME4600_DIO_CTRL_BIT_FIFO_HIGH_0 <<
+					    instance->dio_idx;
+				} else {
+					PERROR
+					    ("Invalid port reference specified.\n");
+					err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+				}
+			} else {
+				PERROR
+				    ("Invalid port configuration specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!err) {
+		outl(mode, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, mode);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_dio_io_single_read(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int *value, int time_out, int flags)
+{
+	me4600_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inl(instance->
+				ctrl_reg) & ((ME4600_DIO_CTRL_BIT_MODE_0 |
+					      ME4600_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+			if ((mode ==
+			     (ME4600_DIO_CTRL_BIT_MODE_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value =
+				    inl(instance->port_reg) & (0x1 << channel);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inl(instance->
+				ctrl_reg) & ((ME4600_DIO_CTRL_BIT_MODE_0 |
+					      ME4600_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+			if ((mode ==
+			     (ME4600_DIO_CTRL_BIT_MODE_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value = inl(instance->port_reg) & 0xFF;
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_dio_io_single_write(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int value, int time_out, int flags)
+{
+	me4600_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t mode;
+	uint32_t byte;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inl(instance->
+				ctrl_reg) & ((ME4600_DIO_CTRL_BIT_MODE_0 |
+					      ME4600_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME4600_DIO_CTRL_BIT_MODE_0 <<
+			     (instance->dio_idx * 2))) {
+				byte = inl(instance->port_reg) & 0xFF;
+
+				if (value)
+					byte |= 0x1 << channel;
+				else
+					byte &= ~(0x1 << channel);
+
+				outl(byte, instance->port_reg);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inl(instance->
+				ctrl_reg) & ((ME4600_DIO_CTRL_BIT_MODE_0 |
+					      ME4600_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME4600_DIO_CTRL_BIT_MODE_0 <<
+			     (instance->dio_idx * 2))) {
+				outl(value, instance->port_reg);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_dio_query_number_channels(me_subdevice_t * subdevice,
+					    int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_dio_query_subdevice_type(me_subdevice_t * subdevice,
+					   int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DIO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_dio_query_subdevice_caps(me_subdevice_t * subdevice,
+					   int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_DIR_BYTE;
+	return ME_ERRNO_SUCCESS;
+}
+
+me4600_dio_subdevice_t *me4600_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock)
+{
+	me4600_dio_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me4600_dio_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me4600_dio_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save digital i/o index */
+	subdevice->dio_idx = dio_idx;
+
+	/* Save the subdevice index */
+	subdevice->ctrl_reg = reg_base + ME4600_DIO_CTRL_REG;
+	subdevice->port_reg = reg_base + ME4600_DIO_PORT_REG + (dio_idx * 4);
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me4600_dio_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me4600_dio_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me4600_dio_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me4600_dio_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me4600_dio_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me4600_dio_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me4600_dio_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me4600_dio.h b/drivers/staging/meilhaus/me4600_dio.h
new file mode 100644
index 000000000000..4625ba91f609
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_dio.h
@@ -0,0 +1,69 @@
+/**
+ * @file me4600_dio.h
+ *
+ * @brief ME-4000 digital input/output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_DIO_H_
+#define _ME4600_DIO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me4600_dio_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+	unsigned int dio_idx;			/**< The index of the digital i/o on the device. */
+
+	/* Registers */
+	unsigned long port_reg;			/**< Register holding the port status. */
+	unsigned long ctrl_reg;			/**< Register to configure the port direction. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me4600_dio_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-4000 digital input/ouput subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param dio_idx The index of the digital i/o port on the device.
+ * @param ctrl_reg_lock Spin lock protecting the control register.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me4600_dio_subdevice_t *me4600_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_dio_reg.h b/drivers/staging/meilhaus/me4600_dio_reg.h
new file mode 100644
index 000000000000..7a4016a80fd2
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_dio_reg.h
@@ -0,0 +1,63 @@
+/**
+ * @file me4600_dio_reg.h
+ *
+ * @brief ME-4000 digital input/output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_DIO_REG_H_
+#define _ME4600_DIO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME4600_DIO_PORT_0_REG				0xA0					/**< Port 0 register. */
+#define ME4600_DIO_PORT_1_REG				0xA4					/**< Port 1 register. */
+#define ME4600_DIO_PORT_2_REG				0xA8					/**< Port 2 register. */
+#define ME4600_DIO_PORT_3_REG				0xAC					/**< Port 3 register. */
+
+#define ME4600_DIO_DIR_REG					0xB0					/**< Direction register. */
+#define ME4600_DIO_PORT_REG					ME4600_DIO_PORT_0_REG	/**< Base for port's register. */
+
+#define ME4600_DIO_CTRL_REG					0xB8					/**< Control register. */
+/** Port A - DO */
+#define ME4600_DIO_CTRL_BIT_MODE_0			0x0001
+#define ME4600_DIO_CTRL_BIT_MODE_1			0x0002
+/** Port B - DI */
+#define ME4600_DIO_CTRL_BIT_MODE_2			0x0004
+#define ME4600_DIO_CTRL_BIT_MODE_3			0x0008
+/** Port C - DIO */
+#define ME4600_DIO_CTRL_BIT_MODE_4			0x0010
+#define ME4600_DIO_CTRL_BIT_MODE_5			0x0020
+/** Port D - DIO */
+#define ME4600_DIO_CTRL_BIT_MODE_6			0x0040
+#define ME4600_DIO_CTRL_BIT_MODE_7			0x0080
+
+#define ME4600_DIO_CTRL_BIT_FUNCTION_0		0x0100
+#define ME4600_DIO_CTRL_BIT_FUNCTION_1		0x0200
+
+#define ME4600_DIO_CTRL_BIT_FIFO_HIGH_0		0x0400
+#define ME4600_DIO_CTRL_BIT_FIFO_HIGH_1		0x0800
+#define ME4600_DIO_CTRL_BIT_FIFO_HIGH_2		0x1000
+#define ME4600_DIO_CTRL_BIT_FIFO_HIGH_3		0x2000
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_do.c b/drivers/staging/meilhaus/me4600_do.c
new file mode 100644
index 000000000000..ee591bc1185e
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_do.c
@@ -0,0 +1,433 @@
+/**
+ * @file me4600_do.c
+ *
+ * @brief ME-4000 digital output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me4600_dio_reg.h"
+#include "me4600_do.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me4600_do_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	me4600_do_subdevice_t *instance;
+	uint32_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_do_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	/* Set port to output mode */
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inl(instance->ctrl_reg);
+	mode &= ~ME4600_DIO_CTRL_BIT_MODE_1;	//0xFFFD
+	mode |= ME4600_DIO_CTRL_BIT_MODE_0;	//0x1
+	outl(mode, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, mode);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outl(0, instance->port_reg);
+	PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_do_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me4600_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t mode;
+	int size =
+	    flags & (ME_IO_SINGLE_CONFIG_DIO_BIT | ME_IO_SINGLE_CONFIG_DIO_BYTE
+		     | ME_IO_SINGLE_CONFIG_DIO_WORD |
+		     ME_IO_SINGLE_CONFIG_DIO_DWORD);
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inl(instance->ctrl_reg);
+
+	switch (size) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+				mode &= ~(ME4600_DIO_CTRL_BIT_MODE_0 |
+					  ME4600_DIO_CTRL_BIT_MODE_1);
+				mode |= (ME4600_DIO_CTRL_BIT_MODE_0);
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_MUX32M) {
+				mode &= ~(ME4600_DIO_CTRL_BIT_MODE_0 |
+					  ME4600_DIO_CTRL_BIT_MODE_1 |
+					  ME4600_DIO_CTRL_BIT_FUNCTION_0 |
+					  ME4600_DIO_CTRL_BIT_FUNCTION_1 |
+					  ME4600_DIO_CTRL_BIT_FIFO_HIGH_0);
+
+				if (ref == ME_REF_DIO_FIFO_LOW) {
+					mode |= (ME4600_DIO_CTRL_BIT_MODE_0 |
+						 ME4600_DIO_CTRL_BIT_MODE_1 |
+						 ME4600_DIO_CTRL_BIT_FUNCTION_1);
+				} else if (ref == ME_REF_DIO_FIFO_HIGH) {
+					mode |= (ME4600_DIO_CTRL_BIT_MODE_0 |
+						 ME4600_DIO_CTRL_BIT_MODE_1 |
+						 ME4600_DIO_CTRL_BIT_FUNCTION_1
+						 |
+						 ME4600_DIO_CTRL_BIT_FIFO_HIGH_0);
+				} else {
+					PERROR
+					    ("Invalid port reference specified.\n");
+					err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+				}
+			} else if (single_config ==
+				   ME_SINGLE_CONFIG_DIO_DEMUX32) {
+				mode &=
+				    ~(ME4600_DIO_CTRL_BIT_MODE_0 |
+				      ME4600_DIO_CTRL_BIT_MODE_1 |
+				      ME4600_DIO_CTRL_BIT_FUNCTION_0 |
+				      ME4600_DIO_CTRL_BIT_FUNCTION_1 |
+				      ME4600_DIO_CTRL_BIT_FIFO_HIGH_0);
+
+				if (ref == ME_REF_DIO_FIFO_LOW) {
+					mode |= (ME4600_DIO_CTRL_BIT_MODE_0 |
+						 ME4600_DIO_CTRL_BIT_MODE_1 |
+						 ME4600_DIO_CTRL_BIT_FUNCTION_0);
+				} else if (ref == ME_REF_DIO_FIFO_HIGH) {
+					mode |= (ME4600_DIO_CTRL_BIT_MODE_0 |
+						 ME4600_DIO_CTRL_BIT_MODE_1 |
+						 ME4600_DIO_CTRL_BIT_FUNCTION_0
+						 |
+						 ME4600_DIO_CTRL_BIT_FIFO_HIGH_0);
+				} else {
+					PERROR
+					    ("Invalid port reference specified.\n");
+					err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+				}
+			} else if (single_config ==
+				   ME_SINGLE_CONFIG_DIO_BIT_PATTERN) {
+				mode &=
+				    ~(ME4600_DIO_CTRL_BIT_MODE_0 |
+				      ME4600_DIO_CTRL_BIT_MODE_1 |
+				      ME4600_DIO_CTRL_BIT_FUNCTION_0 |
+				      ME4600_DIO_CTRL_BIT_FUNCTION_1 |
+				      ME4600_DIO_CTRL_BIT_FIFO_HIGH_0);
+
+				if (ref == ME_REF_DIO_FIFO_LOW) {
+					mode |= (ME4600_DIO_CTRL_BIT_MODE_0 |
+						 ME4600_DIO_CTRL_BIT_MODE_1);
+				} else if (ref == ME_REF_DIO_FIFO_HIGH) {
+					mode |= (ME4600_DIO_CTRL_BIT_MODE_0 |
+						 ME4600_DIO_CTRL_BIT_MODE_1 |
+						 ME4600_DIO_CTRL_BIT_FIFO_HIGH_0);
+				} else {
+					PERROR
+					    ("Invalid port reference specified.\n");
+					err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+				}
+			} else {
+				PERROR("Invalid port direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!err) {
+		outl(mode, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, mode);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_do_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me4600_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode =
+	    inl(instance->
+		ctrl_reg) & (ME4600_DIO_CTRL_BIT_MODE_0 |
+			     ME4600_DIO_CTRL_BIT_MODE_1);
+
+	if (mode == ME4600_DIO_CTRL_BIT_MODE_0) {
+		switch (flags) {
+		case ME_IO_SINGLE_TYPE_DIO_BIT:
+			if ((channel >= 0) && (channel < 8)) {
+				*value =
+				    inl(instance->port_reg) & (0x1 << channel);
+			} else {
+				PERROR("Invalid bit number specified.\n");
+				err = ME_ERRNO_INVALID_CHANNEL;
+			}
+			break;
+
+		case ME_IO_SINGLE_NO_FLAGS:
+		case ME_IO_SINGLE_TYPE_DIO_BYTE:
+			if (channel == 0) {
+				*value = inl(instance->port_reg) & 0xFF;
+			} else {
+				PERROR("Invalid byte number specified.\n");
+				err = ME_ERRNO_INVALID_CHANNEL;
+			}
+			break;
+
+		default:
+			PERROR("Invalid flags specified.\n");
+			err = ME_ERRNO_INVALID_FLAGS;
+		}
+	} else {
+		PERROR("Port not in output mode.\n");
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_do_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me4600_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t byte;
+	uint32_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode =
+	    inl(instance->
+		ctrl_reg) & (ME4600_DIO_CTRL_BIT_MODE_0 |
+			     ME4600_DIO_CTRL_BIT_MODE_1);
+
+	if (mode == ME4600_DIO_CTRL_BIT_MODE_0) {
+		switch (flags) {
+
+		case ME_IO_SINGLE_TYPE_DIO_BIT:
+			if ((channel >= 0) && (channel < 8)) {
+				byte = inl(instance->port_reg) & 0xFF;
+
+				if (value)
+					byte |= 0x1 << channel;
+				else
+					byte &= ~(0x1 << channel);
+
+				outl(byte, instance->port_reg);
+			} else {
+				PERROR("Invalid bit number specified.\n");
+				err = ME_ERRNO_INVALID_CHANNEL;
+			}
+			break;
+
+		case ME_IO_SINGLE_NO_FLAGS:
+		case ME_IO_SINGLE_TYPE_DIO_BYTE:
+			if (channel == 0) {
+				outl(value, instance->port_reg);
+			} else {
+				PERROR("Invalid byte number specified.\n");
+				err = ME_ERRNO_INVALID_CHANNEL;
+			}
+			break;
+
+		default:
+			PERROR("Invalid flags specified.\n");
+			err = ME_ERRNO_INVALID_FLAGS;
+		}
+	} else {
+		PERROR("Port not in output mode.\n");
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_do_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_do_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_do_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+me4600_do_subdevice_t *me4600_do_constructor(uint32_t reg_base,
+					     spinlock_t * ctrl_reg_lock)
+{
+	me4600_do_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me4600_do_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me4600_do_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save the subdevice index */
+	subdevice->ctrl_reg = reg_base + ME4600_DIO_CTRL_REG;
+	subdevice->port_reg = reg_base + ME4600_DIO_PORT_0_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me4600_do_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me4600_do_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me4600_do_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me4600_do_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me4600_do_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me4600_do_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me4600_do_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me4600_do.h b/drivers/staging/meilhaus/me4600_do.h
new file mode 100644
index 000000000000..e8385648e925
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_do.h
@@ -0,0 +1,65 @@
+/**
+ * @file me4600_do.h
+ *
+ * @brief ME-4000 digital output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_DO_H_
+#define _ME4600_DO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me4600_do_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+
+	unsigned long port_reg;			/**< Register holding the port status. */
+	unsigned long ctrl_reg;			/**< Register to configure the port direction. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me4600_do_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-4000 digital output subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param ctrl_reg_lock Spin lock protecting the control register.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me4600_do_subdevice_t *me4600_do_constructor(uint32_t reg_base,
+					     spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_ext_irq.c b/drivers/staging/meilhaus/me4600_ext_irq.c
new file mode 100644
index 000000000000..adc1e1babf40
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ext_irq.c
@@ -0,0 +1,467 @@
+/**
+ * @file me4600_ext_irq.c
+ *
+ * @brief ME-4000 external interrupt subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/version.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "meids.h"
+#include "me4600_reg.h"
+#include "me4600_ai_reg.h"
+#include "me4600_ext_irq_reg.h"
+#include "me4600_ext_irq.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me4600_ext_irq_io_irq_start(me_subdevice_t * subdevice,
+				       struct file *filep,
+				       int channel,
+				       int irq_source,
+				       int irq_edge, int irq_arg, int flags)
+{
+	me4600_ext_irq_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags;
+	uint32_t tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags & ~ME_IO_IRQ_START_DIO_BIT) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((irq_edge != ME_IRQ_EDGE_RISING)
+	    && (irq_edge != ME_IRQ_EDGE_FALLING)
+	    && (irq_edge != ME_IRQ_EDGE_ANY)
+	    ) {
+		PERROR("Invalid irq edge specified.\n");
+		return ME_ERRNO_INVALID_IRQ_EDGE;
+	}
+
+	if (irq_source != ME_IRQ_SOURCE_DIO_LINE) {
+		PERROR("Invalid irq source specified.\n");
+		return ME_ERRNO_INVALID_IRQ_SOURCE;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	tmp = 0x0;		//inl(instance->ext_irq_config_reg);
+
+	if (irq_edge == ME_IRQ_EDGE_RISING) {
+		//tmp &= ~ME4600_EXT_IRQ_CONFIG_MASK;
+		//tmp |= ME4600_EXT_IRQ_CONFIG_MASK_RISING;
+	} else if (irq_edge == ME_IRQ_EDGE_FALLING) {
+		//tmp &= ~ME4600_EXT_IRQ_CONFIG_MASK;
+		//tmp |= ME4600_EXT_IRQ_CONFIG_MASK_FALLING;
+		tmp = ME4600_EXT_IRQ_CONFIG_MASK_FALLING;
+	} else if (irq_edge == ME_IRQ_EDGE_ANY) {
+		//tmp &= ~ME4600_EXT_IRQ_CONFIG_MASK;
+		//tmp |= ME4600_EXT_IRQ_CONFIG_MASK_ANY;
+		tmp = ME4600_EXT_IRQ_CONFIG_MASK_ANY;
+	}
+
+	outl(tmp, instance->ext_irq_config_reg);
+	PDEBUG_REG("ext_irq_config_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->ext_irq_config_reg - instance->reg_base, tmp);
+
+	spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+	tmp = inl(instance->ctrl_reg);
+	tmp &= ~(ME4600_AI_CTRL_BIT_EX_IRQ | ME4600_AI_CTRL_BIT_EX_IRQ_RESET);
+	tmp |= ME4600_AI_CTRL_BIT_EX_IRQ;
+	outl(tmp, instance->ctrl_reg);
+	spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+	instance->rised = 0;
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ext_irq_io_irq_wait(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int *irq_count,
+				      int *value, int time_out, int flags)
+{
+	me4600_ext_irq_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->rised <= 0) {
+		instance->rised = 0;
+		if (time_out) {
+			t = wait_event_interruptible_timeout(instance->
+							     wait_queue,
+							     (instance->rised !=
+							      0), t);
+
+			if (t == 0) {
+				PERROR
+				    ("Wait on external interrupt timed out.\n");
+				err = ME_ERRNO_TIMEOUT;
+			}
+		} else {
+			wait_event_interruptible(instance->wait_queue,
+						 (instance->rised != 0));
+		}
+
+		if (instance->rised < 0) {
+			PERROR("Wait on interrupt aborted by user.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+	}
+
+	if (signal_pending(current)) {
+		PERROR("Wait on external interrupt aborted by signal.\n");
+		err = ME_ERRNO_SIGNAL;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	instance->rised = 0;
+	*irq_count = instance->count;
+	*value = instance->value;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ext_irq_io_irq_stop(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel, int flags)
+{
+	me4600_ext_irq_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags;
+	uint32_t tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->ctrl_reg_lock);
+	tmp = inl(instance->ctrl_reg);
+	tmp &= ~(ME4600_AI_CTRL_BIT_EX_IRQ | ME4600_AI_CTRL_BIT_EX_IRQ_RESET);
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_regv outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->ctrl_reg_lock);
+	instance->rised = -1;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ext_irq_io_reset_subdevice(me_subdevice_t * subdevice,
+					     struct file *filep, int flags)
+{
+	me4600_ext_irq_subdevice_t *instance;
+	unsigned long cpu_flags;
+	uint32_t tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->ctrl_reg_lock);
+	tmp = inl(instance->ctrl_reg);
+	tmp &= ~(ME4600_AI_CTRL_BIT_EX_IRQ | ME4600_AI_CTRL_BIT_EX_IRQ_RESET);
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_regv outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->ctrl_reg_lock);
+	instance->rised = -1;
+	instance->count = 0;
+	outl(ME4600_EXT_IRQ_CONFIG_MASK_ANY, instance->ext_irq_config_reg);
+	PDEBUG_REG("ext_irq_config_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->ext_irq_config_reg - instance->reg_base,
+		   ME4600_EXT_IRQ_CONFIG_MASK_ANY);
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static void me4600_ext_irq_destructor(struct me_subdevice *subdevice)
+{
+	me4600_ext_irq_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+	instance = (me4600_ext_irq_subdevice_t *) subdevice;
+	me_subdevice_deinit(&instance->base);
+	free_irq(instance->irq, instance);
+	kfree(instance);
+}
+
+static int me4600_ext_irq_query_number_channels(me_subdevice_t * subdevice,
+						int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 1;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ext_irq_query_subdevice_type(me_subdevice_t * subdevice,
+					       int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_EXT_IRQ;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ext_irq_query_subdevice_caps(me_subdevice_t * subdevice,
+					       int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps =
+	    ME_CAPS_EXT_IRQ_EDGE_RISING | ME_CAPS_EXT_IRQ_EDGE_FALLING |
+	    ME_CAPS_EXT_IRQ_EDGE_ANY;
+	return ME_ERRNO_SUCCESS;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me4600_ext_irq_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me4600_ext_irq_isr(int irq, void *dev_id,
+				      struct pt_regs *regs)
+#endif
+{
+	me4600_ext_irq_subdevice_t *instance;
+	uint32_t ctrl;
+	uint32_t irq_status;
+
+	instance = (me4600_ext_irq_subdevice_t *) dev_id;
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	irq_status = inl(instance->irq_status_reg);
+	if (!(irq_status & ME4600_IRQ_STATUS_BIT_EX)) {
+		PINFO("%ld Shared interrupt. %s(): irq_status_reg=0x%04X\n",
+		      jiffies, __func__, irq_status);
+		return IRQ_NONE;
+	}
+
+	PDEBUG("executed.\n");
+
+	spin_lock(&instance->subdevice_lock);
+	instance->rised = 1;
+	instance->value = inl(instance->ext_irq_value_reg);
+	instance->count++;
+
+	spin_lock(instance->ctrl_reg_lock);
+	ctrl = inl(instance->ctrl_reg);
+	ctrl |= ME4600_AI_CTRL_BIT_EX_IRQ_RESET;
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	ctrl &= ~ME4600_AI_CTRL_BIT_EX_IRQ_RESET;
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	spin_unlock(&instance->subdevice_lock);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+me4600_ext_irq_subdevice_t *me4600_ext_irq_constructor(uint32_t reg_base,
+						       int irq,
+						       spinlock_t *
+						       ctrl_reg_lock)
+{
+	me4600_ext_irq_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me4600_ext_irq_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me4600_ext_irq_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Initialize wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	/* Register interrupt */
+	subdevice->irq = irq;
+
+	if (request_irq(subdevice->irq, me4600_ext_irq_isr,
+#ifdef IRQF_DISABLED
+			IRQF_DISABLED | IRQF_SHARED,
+#else
+			SA_INTERRUPT | SA_SHIRQ,
+#endif
+			ME4600_NAME, subdevice)) {
+		PERROR("Cannot register interrupt.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	PINFO("Registered irq=%d.\n", subdevice->irq);
+
+	/* Initialize registers */
+	subdevice->irq_status_reg = reg_base + ME4600_IRQ_STATUS_REG;
+	subdevice->ctrl_reg = reg_base + ME4600_AI_CTRL_REG;
+	subdevice->ext_irq_config_reg = reg_base + ME4600_EXT_IRQ_CONFIG_REG;
+	subdevice->ext_irq_value_reg = reg_base + ME4600_EXT_IRQ_VALUE_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Override base class methods. */
+	subdevice->base.me_subdevice_destructor = me4600_ext_irq_destructor;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me4600_ext_irq_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_irq_start = me4600_ext_irq_io_irq_start;
+	subdevice->base.me_subdevice_io_irq_wait = me4600_ext_irq_io_irq_wait;
+	subdevice->base.me_subdevice_io_irq_stop = me4600_ext_irq_io_irq_stop;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me4600_ext_irq_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me4600_ext_irq_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me4600_ext_irq_query_subdevice_caps;
+
+	subdevice->rised = 0;
+	subdevice->count = 0;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me4600_ext_irq.h b/drivers/staging/meilhaus/me4600_ext_irq.h
new file mode 100644
index 000000000000..3c7b27f9e5dc
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ext_irq.h
@@ -0,0 +1,78 @@
+/**
+ * @file me4600_ext_irq.h
+ *
+ * @brief Meilhaus ME-4000 external interrupt subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_EXT_IRQ_H_
+#define _ME4600_EXT_IRQ_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The subdevice class.
+ */
+typedef struct me4600_ext_irq_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+
+	wait_queue_head_t wait_queue;
+
+	int irq;
+
+	int rised;
+	int value;
+	int count;
+
+	unsigned long ctrl_reg;
+	unsigned long irq_status_reg;
+	unsigned long ext_irq_config_reg;
+	unsigned long ext_irq_value_reg;
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me4600_ext_irq_subdevice_t;
+
+/**
+ * @brief The constructor to generate a external interrupt subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param irq The interrupt number assigned by the PCI BIOS.
+ * @param ctrl_reg_lock Pointer to spin lock protecting the control register from concurrent access.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me4600_ext_irq_subdevice_t *me4600_ext_irq_constructor(uint32_t reg_base,
+						       int irq,
+						       spinlock_t *
+						       ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_ext_irq_reg.h b/drivers/staging/meilhaus/me4600_ext_irq_reg.h
new file mode 100644
index 000000000000..898e1e74d9e7
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ext_irq_reg.h
@@ -0,0 +1,41 @@
+/**
+ * @file me4600_ext_irq_reg.h
+ *
+ * @brief ME-4000 external interrupt subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_EXT_IRQ_REG_H_
+#define _ME4600_EXT_IRQ_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME4600_EXT_IRQ_CONFIG_REG		0xCC	// R/_
+#define ME4600_EXT_IRQ_VALUE_REG		0xD0	// R/_
+
+#define ME4600_EXT_IRQ_CONFIG_MASK_RISING	0x0
+#define ME4600_EXT_IRQ_CONFIG_MASK_FALLING	0x1
+#define ME4600_EXT_IRQ_CONFIG_MASK_ANY		0x3
+#define ME4600_EXT_IRQ_CONFIG_MASK		0x3
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_reg.h b/drivers/staging/meilhaus/me4600_reg.h
new file mode 100644
index 000000000000..ae152bbc6a3d
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_reg.h
@@ -0,0 +1,46 @@
+/**
+ * @file me4600_reg.h
+ *
+ * @brief ME-4000 register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_REG_H_
+#define _ME4600_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME4600_IRQ_STATUS_REG			0x9C	// R/_
+
+#define ME4600_IRQ_STATUS_BIT_EX		0x01
+#define ME4600_IRQ_STATUS_BIT_LE		0x02
+#define ME4600_IRQ_STATUS_BIT_AI_HF		0x04
+#define ME4600_IRQ_STATUS_BIT_AO_0_HF	0x08
+#define ME4600_IRQ_STATUS_BIT_AO_1_HF	0x10
+#define ME4600_IRQ_STATUS_BIT_AO_2_HF	0x20
+#define ME4600_IRQ_STATUS_BIT_AO_3_HF	0x40
+#define ME4600_IRQ_STATUS_BIT_SC		0x80
+
+#define ME4600_IRQ_STATUS_BIT_AO_HF		ME4600_IRQ_STATUS_BIT_AO_0_HF
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me6000_ao.c b/drivers/staging/meilhaus/me6000_ao.c
new file mode 100644
index 000000000000..94f01231f79a
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_ao.c
@@ -0,0 +1,3739 @@
+/**
+ * @file me6000_ao.c
+ *
+ * @brief ME-6000 analog output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/* Includes
+ */
+#include <linux/version.h>
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+
+#include <linux/workqueue.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "meids.h"
+#include "me6000_reg.h"
+#include "me6000_ao_reg.h"
+#include "me6000_ao.h"
+
+/* Defines
+ */
+
+static int me6000_ao_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range);
+
+static int me6000_ao_query_number_ranges(me_subdevice_t * subdevice,
+					 int unit, int *count);
+
+static int me6000_ao_query_range_info(me_subdevice_t * subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata);
+
+static int me6000_ao_query_timer(me_subdevice_t * subdevice,
+				 int timer,
+				 int *base_frequency,
+				 long long *min_ticks, long long *max_ticks);
+
+static int me6000_ao_query_number_channels(me_subdevice_t * subdevice,
+					   int *number);
+
+static int me6000_ao_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype);
+
+static int me6000_ao_query_subdevice_caps(me_subdevice_t * subdevice,
+					  int *caps);
+
+static int me6000_ao_query_subdevice_caps_args(struct me_subdevice *subdevice,
+					       int cap, int *args, int count);
+
+/** Remove subdevice. */
+static void me6000_ao_destructor(struct me_subdevice *subdevice);
+
+/** Reset subdevice. Stop all actions. Reset registry. Disable FIFO. Set output to 0V and status to 'none'. */
+static int me6000_ao_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags);
+
+/** Set output as single */
+static int me6000_ao_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags);
+
+/** Pass to user actual value of output. */
+static int me6000_ao_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags);
+
+/** Write to output requed value. */
+static int me6000_ao_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags);
+
+/** Set output as streamed device. */
+static int me6000_ao_io_stream_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags);
+
+/** Wait for / Check empty space in buffer. */
+static int me6000_ao_io_stream_new_values(me_subdevice_t * subdevice,
+					  struct file *filep,
+					  int time_out, int *count, int flags);
+
+/** Start streaming. */
+static int me6000_ao_io_stream_start(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int start_mode, int time_out, int flags);
+
+/** Check actual state. / Wait for end. */
+static int me6000_ao_io_stream_status(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int wait,
+				      int *status, int *values, int flags);
+
+/** Stop streaming. */
+static int me6000_ao_io_stream_stop(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int stop_mode, int flags);
+
+/** Write datas to buffor. */
+static int me6000_ao_io_stream_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int write_mode,
+				     int *values, int *count, int flags);
+
+/** Interrupt handler. Copy from buffer to FIFO. */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me6000_ao_isr(int irq, void *dev_id);
+#else
+static irqreturn_t me6000_ao_isr(int irq, void *dev_id, struct pt_regs *regs);
+#endif
+
+/** Copy data from circular buffer to fifo (fast) in wraparound mode. */
+int inline ao_write_data_wraparound(me6000_ao_subdevice_t * instance, int count,
+				    int start_pos);
+
+/** Copy data from circular buffer to fifo (fast).*/
+int inline ao_write_data(me6000_ao_subdevice_t * instance, int count,
+			 int start_pos);
+
+/** Copy data from circular buffer to fifo (slow).*/
+int inline ao_write_data_pooling(me6000_ao_subdevice_t * instance, int count,
+				 int start_pos);
+
+/** Copy data from user space to circular buffer. */
+int inline ao_get_data_from_user(me6000_ao_subdevice_t * instance, int count,
+				 int *user_values);
+
+/** Stop presentation. Preserve FIFOs. */
+int inline ao_stop_immediately(me6000_ao_subdevice_t * instance);
+
+/** Function for checking timeout in non-blocking mode. */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void me6000_ao_work_control_task(void *subdevice);
+#else
+static void me6000_ao_work_control_task(struct work_struct *work);
+#endif
+
+/* Functions
+ */
+
+static int me6000_ao_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t tmp;
+	uint32_t ctrl;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	instance->status = ao_status_none;
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+	instance->timeout.delay = 0;
+	instance->timeout.start_time = jiffies;
+
+	//Stop state machine.
+	err = ao_stop_immediately(instance);
+
+	//Remove from synchronous start.
+	spin_lock(instance->preload_reg_lock);
+	tmp = inl(instance->preload_reg);
+	tmp &=
+	    ~((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) << instance->
+	      ao_idx);
+	outl(tmp, instance->preload_reg);
+	PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, tmp);
+	*instance->preload_flags &=
+	    ~((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) << instance->
+	      ao_idx);
+
+	//Reset triggering flag
+	*instance->triggering_flags &= ~(0x1 << instance->ao_idx);
+	spin_unlock(instance->preload_reg_lock);
+
+	if (instance->fifo) {
+		//Set single mode, dissable FIFO, dissable external trigger, block interrupt.
+		ctrl = ME6000_AO_MODE_SINGLE;
+
+		//Block ISM.
+		ctrl |=
+		    (ME6000_AO_CTRL_BIT_STOP |
+		     ME6000_AO_CTRL_BIT_IMMEDIATE_STOP);
+
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		//Set speed
+		outl(ME6000_AO_MIN_CHAN_TICKS - 1, instance->timer_reg);
+		//Reset interrupt latch
+		inl(instance->irq_reset_reg);
+	}
+
+	instance->hardware_stop_delay = HZ / 10;	//100ms
+
+	//Set output to 0V
+	outl(0x8000, instance->single_reg);
+	PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->single_reg - instance->reg_base, 0x8000);
+
+	instance->circ_buf.head = 0;
+	instance->circ_buf.tail = 0;
+	instance->preloaded_count = 0;
+	instance->data_count = 0;
+	instance->single_value = 0x8000;
+	instance->single_value_in_fifo = 0x8000;
+
+	//Set status to signal that device is unconfigured.
+	instance->status = ao_status_none;
+	//Signal reset if user is on wait.
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t ctrl;
+	uint32_t sync;
+	unsigned long cpu_flags;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. ID=%d\n", instance->ao_idx);
+
+	// Checking parameters
+	if (flags) {
+		PERROR
+		    ("Invalid flag specified. Must be ME_IO_SINGLE_CONFIG_NO_FLAGS.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (instance->fifo) {	//Stream hardware (with or without fifo)
+		if ((trig_edge == ME_TRIG_TYPE_SW)
+		    && (trig_edge != ME_TRIG_EDGE_NONE)) {
+			PERROR
+			    ("Invalid trigger edge. Software trigger has not edge.\n");
+			return ME_ERRNO_INVALID_TRIG_EDGE;
+		}
+
+		if (trig_type == ME_TRIG_TYPE_EXT_DIGITAL) {
+			switch (trig_edge) {
+			case ME_TRIG_EDGE_ANY:
+			case ME_TRIG_EDGE_RISING:
+			case ME_TRIG_EDGE_FALLING:
+				break;
+
+			default:
+				PERROR("Invalid trigger edge.\n");
+				return ME_ERRNO_INVALID_TRIG_EDGE;
+			}
+		}
+
+		if ((trig_type != ME_TRIG_TYPE_SW)
+		    && (trig_type != ME_TRIG_TYPE_EXT_DIGITAL)) {
+			PERROR
+			    ("Invalid trigger type. Trigger must be software or digital.\n");
+			return ME_ERRNO_INVALID_TRIG_TYPE;
+		}
+	} else {		//Single
+		if (trig_edge != ME_TRIG_EDGE_NONE) {
+			PERROR
+			    ("Invalid trigger edge. Single output trigger hasn't own edge.\n");
+			return ME_ERRNO_INVALID_TRIG_EDGE;
+		}
+
+		if (trig_type != ME_TRIG_TYPE_SW) {
+			PERROR
+			    ("Invalid trigger type. Trigger must be software.\n");
+			return ME_ERRNO_INVALID_TRIG_TYPE;
+		}
+
+	}
+
+	if ((trig_chan != ME_TRIG_CHAN_DEFAULT)
+	    && (trig_chan != ME_TRIG_CHAN_SYNCHRONOUS)) {
+		PERROR("Invalid trigger channel specified.\n");
+		return ME_ERRNO_INVALID_TRIG_CHAN;
+	}
+/*
+	if ((trig_type == ME_TRIG_TYPE_EXT_DIGITAL) && (trig_chan != ME_TRIG_CHAN_SYNCHRONOUS))
+	{
+		PERROR("Invalid trigger channel specified. Must be synchronous when digital is choose.\n");
+		return ME_ERRNO_INVALID_TRIG_CHAN;
+	}
+*/
+	if (ref != ME_REF_AO_GROUND) {
+		PERROR
+		    ("Invalid reference. Analog outputs have to have got REF_AO_GROUND.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if (single_config != 0) {
+		PERROR
+		    ("Invalid single config specified. Only one range for anlog outputs is available.\n");
+		return ME_ERRNO_INVALID_SINGLE_CONFIG;
+	}
+
+	if (channel != 0) {
+		PERROR
+		    ("Invalid channel number specified. Analog output have only one channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	//Subdevice running in stream mode!
+	if ((instance->status >= ao_status_stream_run_wait)
+	    && (instance->status < ao_status_stream_end)) {
+		PERROR("Subdevice is busy.\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+/// @note For single all calls (config and write) are erasing previous state!
+
+	instance->status = ao_status_none;
+
+	// Correct single mirrors
+	instance->single_value_in_fifo = instance->single_value;
+
+	//Stop device
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+
+	if (instance->fifo) {	// Set control register.
+		spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+		// Set stop bit. Stop streaming mode (If running.).
+		ctrl = inl(instance->ctrl_reg);
+		//Reset all bits.
+		ctrl =
+		    ME6000_AO_CTRL_BIT_IMMEDIATE_STOP | ME6000_AO_CTRL_BIT_STOP;
+		if (trig_type == ME_TRIG_TYPE_EXT_DIGITAL) {
+			PINFO("External digital trigger.\n");
+
+			if (trig_edge == ME_TRIG_EDGE_ANY) {
+//                                      ctrl |= ME6000_AO_CTRL_BIT_EX_TRIG_EDGE | ME6000_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+				instance->ctrl_trg =
+				    ME6000_AO_CTRL_BIT_EX_TRIG_EDGE |
+				    ME6000_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+			} else if (trig_edge == ME_TRIG_EDGE_FALLING) {
+//                                      ctrl |= ME6000_AO_CTRL_BIT_EX_TRIG_EDGE;
+				instance->ctrl_trg =
+				    ME6000_AO_CTRL_BIT_EX_TRIG_EDGE;
+			} else if (trig_edge == ME_TRIG_EDGE_RISING) {
+				instance->ctrl_trg = 0x0;
+			}
+		} else if (trig_type == ME_TRIG_TYPE_SW) {
+			PDEBUG("SOFTWARE TRIGGER\n");
+			instance->ctrl_trg = 0x0;
+		}
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	} else {
+		PDEBUG("SOFTWARE TRIGGER\n");
+	}
+
+	// Set preload/synchronization register.
+	spin_lock(instance->preload_reg_lock);
+
+	if (trig_type == ME_TRIG_TYPE_SW) {
+		*instance->preload_flags &=
+		    ~(ME6000_AO_SYNC_EXT_TRIG << instance->ao_idx);
+	} else			//if (trig_type == ME_TRIG_TYPE_EXT_DIGITAL)
+	{
+		*instance->preload_flags |=
+		    ME6000_AO_SYNC_EXT_TRIG << instance->ao_idx;
+	}
+
+	if (trig_chan == ME_TRIG_CHAN_DEFAULT) {
+		*instance->preload_flags &=
+		    ~(ME6000_AO_SYNC_HOLD << instance->ao_idx);
+	} else			//if (trig_chan == ME_TRIG_CHAN_SYNCHRONOUS)
+	{
+		*instance->preload_flags |=
+		    ME6000_AO_SYNC_HOLD << instance->ao_idx;
+	}
+
+	//Reset hardware register
+	sync = inl(instance->preload_reg);
+	PDEBUG_REG("preload_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, sync);
+	sync &= ~(ME6000_AO_SYNC_EXT_TRIG << instance->ao_idx);
+	sync |= ME6000_AO_SYNC_HOLD << instance->ao_idx;
+
+	//Output configured in default mode (safe one)
+	outl(sync, instance->preload_reg);
+	PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, sync);
+	spin_unlock(instance->preload_reg_lock);
+
+	instance->status = ao_status_single_configured;
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	unsigned long j;
+	unsigned long delay = 0;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags & ~ME_IO_SINGLE_NONBLOCKING) {
+		PERROR("Invalid flag specified. %d\n", flags);
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((instance->status >= ao_status_stream_configured)
+	    && (instance->status <= ao_status_stream_end)) {
+		PERROR("Subdevice not configured to work in single mode!\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	if (channel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	ME_SUBDEVICE_ENTER;
+	if ((!flags) && (instance->status == ao_status_single_run_wait)) {	//Blocking mode. Wait for trigger.
+		if (time_out) {
+			delay = (time_out * HZ) / 1000;
+			if (delay == 0)
+				delay = 1;
+		}
+
+		j = jiffies;
+
+		//Only runing process will interrupt this call. Events are signaled when status change. This procedure has own timeout.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_single_run_wait),
+						 (delay) ? delay : LONG_MAX);
+
+		if (instance->status == ao_status_none) {
+			PDEBUG("Single canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		if ((delay) && ((jiffies - j) >= delay)) {
+			PDEBUG("Timeout reached.\n");
+			err = ME_ERRNO_TIMEOUT;
+		}
+
+		*value =
+		    (!err) ? instance->single_value_in_fifo : instance->
+		    single_value;
+	} else {		//Non-blocking mode
+		//Read value
+		*value = instance->single_value;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags;
+	unsigned long j;
+	unsigned long delay = 0;
+
+	uint32_t sync_mask;
+	uint32_t mode;
+
+	uint32_t tmp;
+
+/// Workaround for mix-mode - begin
+	uint32_t ctrl = 0x0;
+	uint32_t status;
+/// Workaround for mix-mode - end
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags &
+	    ~(ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS |
+	      ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING)) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((instance->status == ao_status_none)
+	    || (instance->status > ao_status_single_end)) {
+		PERROR("Subdevice not configured to work in single mode!\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	if (channel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (value & ~ME6000_AO_MAX_DATA) {
+		PERROR("Invalid value provided.\n");
+		return ME_ERRNO_VALUE_OUT_OF_RANGE;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+/// @note For single all calls (config and write) are erasing previous state!
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+
+	// Correct single mirrors
+	instance->single_value_in_fifo = instance->single_value;
+
+	//Stop device
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+
+		if (delay == 0)
+			delay = 1;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	instance->single_value_in_fifo = value;
+
+	if (instance->fifo) {
+		ctrl = inl(instance->ctrl_reg);
+	}
+
+	if (instance->fifo & ME6000_AO_HAS_FIFO) {	/// Workaround for mix-mode - begin
+		//Set speed
+		outl(ME6000_AO_MIN_CHAN_TICKS - 1, instance->timer_reg);
+		PDEBUG_REG("timer_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->timer_reg - instance->reg_base,
+			   (int)ME6000_AO_MIN_CHAN_TICKS);
+		instance->hardware_stop_delay = HZ / 10;	//100ms
+
+		status = inl(instance->status_reg);
+
+		//Set the continous mode.
+		ctrl &= ~ME6000_AO_CTRL_MODE_MASK;
+		ctrl |= ME6000_AO_MODE_CONTINUOUS;
+
+		//Prepare FIFO
+		if (!(ctrl & ME6000_AO_CTRL_BIT_ENABLE_FIFO)) {	//FIFO wasn't enabeled. Do it.
+			PINFO("Enableing FIFO.\n");
+			ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+			ctrl |= ME6000_AO_CTRL_BIT_ENABLE_FIFO;
+		} else {	//Check if FIFO is empty
+			if (status & ME6000_AO_STATUS_BIT_EF) {	//FIFO not empty
+				PINFO("Reseting FIFO.\n");
+				ctrl &=
+				    ~(ME6000_AO_CTRL_BIT_ENABLE_FIFO |
+				      ME6000_AO_CTRL_BIT_ENABLE_IRQ);
+				outl(ctrl, instance->ctrl_reg);
+				PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->ctrl_reg -
+					   instance->reg_base, ctrl);
+
+				ctrl |= ME6000_AO_CTRL_BIT_ENABLE_FIFO;
+			} else {	//FIFO empty, only interrupt needs to be disabled!
+				ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+			}
+		}
+
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		//Reset interrupt latch
+		inl(instance->irq_reset_reg);
+
+		//Write output - 1 value to FIFO
+		if (instance->ao_idx & 0x1) {
+			outl(value <<= 16, instance->fifo_reg);
+			PDEBUG_REG("fifo_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->fifo_reg - instance->reg_base,
+				   value <<= 16);
+		} else {
+			outl(value, instance->fifo_reg);
+			PDEBUG_REG("fifo_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->fifo_reg - instance->reg_base,
+				   value);
+		}
+		/// Workaround for mix-mode - end
+	} else {		//No FIFO - always in single mode
+		//Write value
+		PDEBUG("Write value\n");
+		outl(value, instance->single_reg);
+		PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->single_reg - instance->reg_base, value);
+	}
+
+	mode = *instance->preload_flags >> instance->ao_idx;
+	mode &= (ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG);
+
+	PINFO("Triggering mode: 0x%08x\n", mode);
+
+	spin_lock(instance->preload_reg_lock);
+	sync_mask = inl(instance->preload_reg);
+	PDEBUG_REG("preload_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, sync_mask);
+	switch (mode) {
+	case 0:		//0x00000000: Individual software
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+		if (instance->fifo & ME6000_AO_HAS_FIFO) {	// FIFO - Continous mode
+			ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG;
+			if ((sync_mask & ((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != 0x0) {	//Now we can set correct mode.
+				sync_mask &=
+				    ~((ME6000_AO_SYNC_EXT_TRIG |
+				       ME6000_AO_SYNC_HOLD) << instance->
+				      ao_idx);
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		} else {	// No FIFO - Single mode: In this case resetting 'ME6000_AO_SYNC_HOLD' will trigger output.
+			if ((sync_mask & ((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != ME6000_AO_SYNC_HOLD) {	//Now we can set correct mode. This is exception. It is set to synchronous and triggered later.
+				sync_mask &=
+				    ~(ME6000_AO_SYNC_EXT_TRIG << instance->
+				      ao_idx);
+				sync_mask |=
+				    ME6000_AO_SYNC_HOLD << instance->ao_idx;
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		}
+		instance->single_value = value;
+		break;
+
+	case ME6000_AO_SYNC_EXT_TRIG:	//0x00010000: Individual hardware
+		PDEBUG("DIGITAL TRIGGER\n");
+		ctrl |= ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+		if (instance->fifo & ME6000_AO_HAS_FIFO) {	// FIFO - Continous mode
+			if ((sync_mask & ((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != 0x0) {	//Now we can set correct mode.
+				sync_mask &=
+				    ~((ME6000_AO_SYNC_EXT_TRIG |
+				       ME6000_AO_SYNC_HOLD) << instance->
+				      ao_idx);
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		} else {	// No FIFO - Single mode
+			if ((sync_mask &
+			     ((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) <<
+			      instance->ao_idx)) != ME6000_AO_SYNC_HOLD) {
+				//Now we can set correct mode
+				sync_mask &=
+				    ~(ME6000_AO_SYNC_EXT_TRIG << instance->
+				      ao_idx);
+				sync_mask |=
+				    ME6000_AO_SYNC_HOLD << instance->ao_idx;
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		}
+		break;
+
+	case ME6000_AO_SYNC_HOLD:	//0x00000001: Synchronous software
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+		if ((sync_mask &
+		     ((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) <<
+		      instance->ao_idx)) !=
+		    (ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG)) {
+			//Now we can set correct mode
+			sync_mask |=
+			    ME6000_AO_SYNC_EXT_TRIG << instance->ao_idx;
+			sync_mask |= ME6000_AO_SYNC_HOLD << instance->ao_idx;
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+		}
+		//Set triggering flag
+		*instance->triggering_flags |= 0x1 << instance->ao_idx;
+		break;
+
+	case (ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG):	//0x00010001: Synchronous hardware
+		PDEBUG("DIGITAL TRIGGER\n");
+		ctrl |= ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+		if ((sync_mask &
+		     ((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) <<
+		      instance->ao_idx)) !=
+		    (ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG)) {
+			//Now we can set correct mode
+			sync_mask |=
+			    (ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) <<
+			    instance->ao_idx;
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+		}
+		//Set triggering flag
+		*instance->triggering_flags |= 0x1 << instance->ao_idx;
+		break;
+	}
+//              spin_unlock(instance->preload_reg_lock);        // Moved down.
+
+	if (instance->fifo) {	//Activate ISM (remove 'stop' bits)
+		ctrl &=
+		    ~(ME6000_AO_CTRL_BIT_EX_TRIG_EDGE |
+		      ME6000_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH);
+		ctrl |= instance->ctrl_trg;
+		ctrl &=
+		    ~(ME6000_AO_CTRL_BIT_STOP |
+		      ME6000_AO_CTRL_BIT_IMMEDIATE_STOP);
+
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+	}
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+/// @note When flag 'ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS' is set than output is triggered. ALWAYS!
+
+	PINFO("<%s> start mode= 0x%08x %s\n", __func__, mode,
+	      (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) ? "SYNCHRONOUS" :
+	      "");
+	if (instance->fifo & ME6000_AO_HAS_FIFO) {	// FIFO - Continous mode
+		if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {	//Trigger outputs
+			//Add channel to start list
+			outl(sync_mask |
+			     (ME6000_AO_SYNC_HOLD << instance->ao_idx),
+			     instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask | (ME6000_AO_SYNC_HOLD <<
+						instance->ao_idx));
+
+			//Fire
+			PINFO
+			    ("Fired all software synchronous outputs by software trigger.\n");
+			outl(0x8000, instance->single_reg);
+			PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->single_reg - instance->reg_base,
+				   0x8000);
+
+			//Restore save settings
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+
+		} else if (!mode) {	//Trigger outputs
+/*			//Remove channel from start list
+			outl(sync_mask & ~(ME6000_AO_SYNC_HOLD << instance->ao_idx), instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, sync_mask & ~(ME6000_AO_SYNC_HOLD << instance->ao_idx));
+*/
+			//Fire
+			PINFO("Software trigger.\n");
+			outl(0x8000, instance->single_reg);
+			PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->single_reg - instance->reg_base,
+				   0x8000);
+
+/*			//Restore save settings
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, sync_mask);
+*/
+		}
+/// @note This is mix-mode case. For now I do not have possibility to trigger first 4 channels (continous mode) and other (single) ones at once.
+/// @note Because triggering is not working it can not be add to synchronous list. First 4 channels don't need this information, anyway.
+		*instance->triggering_flags &= 0xFFFFFFF0;
+	} else {		// No FIFO - Single mode
+		if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {	//Fired all software synchronous outputs.
+			tmp = ~(*instance->preload_flags | 0xFFFF0000);
+			PINFO
+			    ("Fired all software synchronous outputs. mask:0x%08x\n",
+			     tmp);
+			tmp |= sync_mask & 0xFFFF0000;
+			// Add this channel to list
+			tmp &= ~(ME6000_AO_SYNC_HOLD << instance->ao_idx);
+
+			//Fire
+			PINFO("Software trigger.\n");
+			outl(tmp, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   tmp);
+
+			//Restore save settings
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+
+			//Set all as triggered.
+			*instance->triggering_flags = 0x0;
+		} else if (!mode) {	// Add this channel to list
+			outl(sync_mask &
+			     ~(ME6000_AO_SYNC_HOLD << instance->ao_idx),
+			     instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask & ~(ME6000_AO_SYNC_HOLD <<
+						 instance->ao_idx));
+
+			//Fire
+			PINFO("Software trigger.\n");
+
+			//Restore save settings
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+
+			//Set all as triggered.
+			*instance->triggering_flags = 0x0;
+		}
+
+	}
+	spin_unlock(instance->preload_reg_lock);
+
+	instance->status = ao_status_single_run_wait;
+
+	instance->timeout.delay = delay;
+	instance->timeout.start_time = jiffies;
+	instance->ao_control_task_flag = 1;
+	queue_delayed_work(instance->me6000_workqueue,
+			   &instance->ao_control_task, 1);
+
+	if (!(flags & ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING)) {
+		j = jiffies;
+
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_single_run_wait),
+						 (delay) ? delay +
+						 1 : LONG_MAX);
+
+		if (instance->status != ao_status_single_end) {
+			PDEBUG("Single canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			instance->ao_control_task_flag = 0;
+			cancel_delayed_work(&instance->ao_control_task);
+			ao_stop_immediately(instance);
+			instance->status = ao_status_none;
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		if ((delay) && ((jiffies - j) >= delay)) {
+			if (instance->status == ao_status_single_end) {
+				PDEBUG("Timeout reached.\n");
+			} else if ((jiffies - j) > delay) {
+				PERROR
+				    ("Timeout reached. Not handled by control task!\n");
+				ao_stop_immediately(instance);
+			} else {
+				PERROR
+				    ("Timeout reached. Signal come but status is strange: %d\n",
+				     instance->status);
+				ao_stop_immediately(instance);
+			}
+
+			instance->ao_control_task_flag = 0;
+			cancel_delayed_work(&instance->ao_control_task);
+			instance->status = ao_status_single_end;
+			err = ME_ERRNO_TIMEOUT;
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_stream_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t ctrl;
+	unsigned long cpu_flags;
+	uint64_t conv_ticks;
+	unsigned int conv_start_ticks_low = trigger->iConvStartTicksLow;
+	unsigned int conv_start_ticks_high = trigger->iConvStartTicksHigh;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	conv_ticks =
+	    (uint64_t) conv_start_ticks_low +
+	    ((uint64_t) conv_start_ticks_high << 32);
+
+	if (flags &
+	    ~(ME_IO_STREAM_CONFIG_HARDWARE_ONLY |
+	      ME_IO_STREAM_CONFIG_WRAPAROUND)) {
+		PERROR("Invalid flags.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (flags & ME_IO_STREAM_CONFIG_HARDWARE_ONLY) {
+		if (!flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			PERROR
+			    ("Hardware ME_IO_STREAM_CONFIG_HARDWARE_ONLY has to be with ME_IO_STREAM_CONFIG_WRAPAROUND.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+
+		if ((trigger->iAcqStopTrigType != ME_TRIG_TYPE_NONE)
+		    || (trigger->iScanStopTrigType != ME_TRIG_TYPE_NONE)) {
+			PERROR
+			    ("Hardware wraparound mode must be in infinite mode.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+	}
+
+	if (count != 1) {
+		PERROR("Only 1 entry in config list acceptable.\n");
+		return ME_ERRNO_INVALID_CONFIG_LIST_COUNT;
+	}
+
+	if (config_list[0].iChannel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (config_list[0].iStreamConfig != 0) {
+		PERROR("Only one range available.\n");
+		return ME_ERRNO_INVALID_STREAM_CONFIG;
+	}
+
+	if (config_list[0].iRef != ME_REF_AO_GROUND) {
+		PERROR("Output is referenced to ground.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if ((trigger->iAcqStartTicksLow != 0)
+	    || (trigger->iAcqStartTicksHigh != 0)) {
+		PERROR
+		    ("Invalid acquisition start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_ARG;
+	}
+
+	if (config_list[0].iFlags) {
+		PERROR("Invalid config list flag.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((trigger->iAcqStartTrigType != ME_TRIG_TYPE_SW)
+	    && (trigger->iAcqStartTrigType != ME_TRIG_TYPE_EXT_DIGITAL)) {
+		PERROR("Invalid acquisition start trigger type specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_TRIG_TYPE;
+	}
+
+	if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_EXT_DIGITAL) {
+		switch (trigger->iAcqStartTrigEdge) {
+		case ME_TRIG_EDGE_RISING:
+		case ME_TRIG_EDGE_FALLING:
+		case ME_TRIG_EDGE_ANY:
+			break;
+
+		default:
+			PERROR
+			    ("Invalid acquisition start trigger edge specified.\n");
+			return ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+		}
+	}
+
+	if ((trigger->iAcqStartTrigType == ME_TRIG_TYPE_SW)
+	    && (trigger->iAcqStartTrigEdge != ME_TRIG_TYPE_NONE)) {
+		PERROR("Invalid acquisition start trigger edge specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+	}
+
+	if (trigger->iScanStartTrigType != ME_TRIG_TYPE_FOLLOW) {
+		PERROR("Invalid scan start trigger type specified.\n");
+		return ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE;
+	}
+
+	if (trigger->iConvStartTrigType != ME_TRIG_TYPE_TIMER) {
+		PERROR("Invalid conv start trigger type specified.\n");
+		return ME_ERRNO_INVALID_CONV_START_TRIG_TYPE;
+	}
+
+	if ((conv_ticks < ME6000_AO_MIN_CHAN_TICKS)
+	    || (conv_ticks > ME6000_AO_MAX_CHAN_TICKS)) {
+		PERROR("Invalid conv start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_CONV_START_ARG;
+	}
+
+	if (trigger->iAcqStartTicksLow || trigger->iAcqStartTicksHigh) {
+		PERROR("Invalid acq start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_ARG;
+	}
+
+	if (trigger->iScanStartTicksLow || trigger->iScanStartTicksHigh) {
+		PERROR("Invalid scan start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_SCAN_START_ARG;
+	}
+
+	switch (trigger->iScanStopTrigType) {
+	case ME_TRIG_TYPE_NONE:
+		if (trigger->iScanStopCount != 0) {
+			PERROR("Invalid scan stop count specified.\n");
+			return ME_ERRNO_INVALID_SCAN_STOP_ARG;
+		}
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			if (trigger->iScanStopCount <= 0) {
+				PERROR("Invalid scan stop count specified.\n");
+				return ME_ERRNO_INVALID_SCAN_STOP_ARG;
+			}
+		} else {
+			PERROR("The continous mode has not 'scan' contects.\n");
+			return ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+		}
+		break;
+
+	default:
+		PERROR("Invalid scan stop trigger type specified.\n");
+		return ME_ERRNO_INVALID_SCAN_STOP_TRIG_TYPE;
+	}
+
+	switch (trigger->iAcqStopTrigType) {
+	case ME_TRIG_TYPE_NONE:
+		if (trigger->iAcqStopCount != 0) {
+			PERROR("Invalid acq stop count specified.\n");
+			return ME_ERRNO_INVALID_ACQ_STOP_ARG;
+		}
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (trigger->iScanStopTrigType != ME_TRIG_TYPE_NONE) {
+			PERROR("Invalid acq stop trigger type specified.\n");
+			return ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+		}
+
+		if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			if (trigger->iAcqStopCount <= 0) {
+				PERROR
+				    ("The continous mode has not 'scan' contects.\n");
+				return ME_ERRNO_INVALID_ACQ_STOP_ARG;
+			}
+		}
+//                      else
+//                      {
+//                              PERROR("Invalid acq stop trigger type specified.\n");
+//                              return ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+//                      }
+
+		break;
+
+	default:
+		PERROR("Invalid acq stop trigger type specified.\n");
+		return ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+	}
+
+	switch (trigger->iAcqStartTrigChan) {
+	case ME_TRIG_CHAN_DEFAULT:
+	case ME_TRIG_CHAN_SYNCHRONOUS:
+		break;
+
+	default:
+		PERROR("Invalid acq start trigger channel specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_TRIG_CHAN;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	//Stop device
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+
+	//Check if state machine is stopped.
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	//Reset control register. Block all actions. Disable IRQ. Disable FIFO.
+	ctrl = ME6000_AO_CTRL_BIT_IMMEDIATE_STOP | ME6000_AO_CTRL_BIT_STOP;
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+
+	//Reset interrupt latch
+	inl(instance->irq_reset_reg);
+
+	//This is paranoic, but to be sure.
+	instance->preloaded_count = 0;
+	instance->data_count = 0;
+	instance->circ_buf.head = 0;
+	instance->circ_buf.tail = 0;
+
+	/* Set mode. */
+	if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {	//Wraparound
+		if (flags & ME_IO_STREAM_CONFIG_HARDWARE_ONLY) {	//Hardware wraparound
+			PINFO("Hardware wraparound.\n");
+			ctrl |= ME6000_AO_MODE_WRAPAROUND;
+			instance->mode = ME6000_AO_HW_WRAP_MODE;
+		} else {	//Software wraparound
+			PINFO("Software wraparound.\n");
+			ctrl |= ME6000_AO_MODE_CONTINUOUS;
+			instance->mode = ME6000_AO_SW_WRAP_MODE;
+		}
+	} else {		//Continous
+		PINFO("Continous.\n");
+		ctrl |= ME6000_AO_MODE_CONTINUOUS;
+		instance->mode = ME6000_AO_CONTINOUS;
+	}
+
+	//Set the trigger edge.
+	if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_EXT_DIGITAL) {	//Set the trigger type and edge for external trigger.
+		PINFO("External digital trigger.\n");
+		instance->start_mode = ME6000_AO_EXT_TRIG;
+
+		switch (trigger->iAcqStartTrigEdge) {
+		case ME_TRIG_EDGE_RISING:
+			PINFO("Set the trigger edge: rising.\n");
+			instance->ctrl_trg = 0x0;
+			break;
+
+		case ME_TRIG_EDGE_FALLING:
+			PINFO("Set the trigger edge: falling.\n");
+//                                      ctrl |= ME6000_AO_CTRL_BIT_EX_TRIG_EDGE;
+			instance->ctrl_trg = ME6000_AO_CTRL_BIT_EX_TRIG_EDGE;
+			break;
+
+		case ME_TRIG_EDGE_ANY:
+			PINFO("Set the trigger edge: both edges.\n");
+//                                      ctrl |= ME6000_AO_CTRL_BIT_EX_TRIG_EDGE | ME6000_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+			instance->ctrl_trg =
+			    ME6000_AO_CTRL_BIT_EX_TRIG_EDGE |
+			    ME6000_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+			break;
+		}
+	} else {
+		PINFO("Internal software trigger.\n");
+		instance->start_mode = 0;
+	}
+
+	//Set the stop mode and value.
+	if (trigger->iAcqStopTrigType == ME_TRIG_TYPE_COUNT) {	//Amount of data
+		instance->stop_mode = ME6000_AO_ACQ_STOP_MODE;
+		instance->stop_count = trigger->iAcqStopCount;
+	} else if (trigger->iScanStopTrigType == ME_TRIG_TYPE_COUNT) {	//Amount of 'scans'
+		instance->stop_mode = ME6000_AO_SCAN_STOP_MODE;
+		instance->stop_count = trigger->iScanStopCount;
+	} else {		//Infinite
+		instance->stop_mode = ME6000_AO_INF_STOP_MODE;
+		instance->stop_count = 0;
+	}
+
+	PINFO("Stop count: %d.\n", instance->stop_count);
+
+	if (trigger->iAcqStartTrigChan == ME_TRIG_CHAN_SYNCHRONOUS) {	//Synchronous start
+		instance->start_mode |= ME6000_AO_SYNC_HOLD;
+		if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_EXT_DIGITAL) {	//Externaly triggered
+			PINFO("Synchronous start. Externaly trigger active.\n");
+			instance->start_mode |= ME6000_AO_SYNC_EXT_TRIG;
+		}
+#ifdef MEDEBUG_INFO
+		else {
+			PINFO
+			    ("Synchronous start. Externaly trigger dissabled.\n");
+		}
+#endif
+
+	}
+	//Set speed
+	outl(conv_ticks - 2, instance->timer_reg);
+	PDEBUG_REG("timer_reg outl(0x%lX+0x%lX)=0x%llx\n", instance->reg_base,
+		   instance->timer_reg - instance->reg_base, conv_ticks - 2);
+	instance->hardware_stop_delay = (int)(conv_ticks * HZ) / ME6000_AO_BASE_FREQUENCY;	//<== MUST be with cast!
+
+	// Write the control word
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+
+	//Set status.
+	instance->status = ao_status_stream_configured;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_stream_new_values(me_subdevice_t * subdevice,
+					  struct file *filep,
+					  int time_out, int *count, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	long j;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!instance->circ_buf.buf) {
+		PERROR("Circular buffer not exists.\n");
+		return ME_ERRNO_INTERNAL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (me_circ_buf_space(&instance->circ_buf)) {	//The buffer is NOT full.
+		*count = me_circ_buf_space(&instance->circ_buf);
+	} else {		//The buffer is full.
+		if (time_out) {
+			t = (time_out * HZ) / 1000;
+
+			if (t == 0)
+				t = 1;
+		} else {	//Max time.
+			t = LONG_MAX;
+		}
+
+		*count = 0;
+
+		j = jiffies;
+
+		//Only runing process will interrupt this call. Interrupts are when FIFO HF is signaled.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 ((me_circ_buf_space
+						   (&instance->circ_buf))
+						  || !(inl(instance->status_reg)
+						       &
+						       ME6000_AO_STATUS_BIT_FSM)),
+						 t);
+
+		if (!(inl(instance->status_reg) & ME6000_AO_STATUS_BIT_FSM)) {
+			PERROR("AO subdevice is not running.\n");
+			err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+		} else if (signal_pending(current)) {
+			PERROR("Wait on values interrupted from signal.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		} else if ((jiffies - j) >= t) {
+			PERROR("Wait on values timed out.\n");
+			err = ME_ERRNO_TIMEOUT;
+		} else {	//Uff... all is good. Inform user about empty space.
+			*count = me_circ_buf_space(&instance->circ_buf);
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_stream_start(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int start_mode, int time_out, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags = 0;
+	uint32_t status;
+	uint32_t ctrl;
+	uint32_t synch;
+	int count = 0;
+	int circ_buffer_count;
+
+	unsigned long ref;
+	unsigned long delay = 0;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags & ~ME_IO_STREAM_START_TYPE_TRIG_SYNCHRONOUS) {
+		PERROR("Invalid flags.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if ((start_mode != ME_START_MODE_BLOCKING)
+	    && (start_mode != ME_START_MODE_NONBLOCKING)) {
+		PERROR("Invalid start mode specified.\n");
+		return ME_ERRNO_INVALID_START_MODE;
+	}
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+		if (delay == 0)
+			delay = 1;
+	}
+
+	switch (instance->status) {	//Checking actual mode.
+	case ao_status_stream_configured:
+	case ao_status_stream_end:
+		//Correct modes!
+		break;
+
+		//The device is in wrong mode.
+	case ao_status_none:
+	case ao_status_single_configured:
+	case ao_status_single_run_wait:
+	case ao_status_single_run:
+	case ao_status_single_end_wait:
+		PERROR
+		    ("Subdevice must be preinitialize correctly for streaming.\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+
+	case ao_status_stream_fifo_error:
+	case ao_status_stream_buffer_error:
+	case ao_status_stream_error:
+		PDEBUG("Before restart broke stream 'STOP' must be caled.\n");
+		return ME_STATUS_ERROR;
+
+	case ao_status_stream_run_wait:
+	case ao_status_stream_run:
+	case ao_status_stream_end_wait:
+		PDEBUG("Stream is already working.\n");
+		return ME_ERRNO_SUBDEVICE_BUSY;
+
+	default:
+		instance->status = ao_status_stream_error;
+		PERROR_CRITICAL("Status is in wrong state!\n");
+		return ME_ERRNO_INTERNAL;
+
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->mode == ME6000_AO_CONTINOUS) {	//Continous
+		instance->circ_buf.tail += instance->preloaded_count;
+		instance->circ_buf.tail &= instance->circ_buf.mask;
+	}
+	circ_buffer_count = me_circ_buf_values(&instance->circ_buf);
+
+	if (!circ_buffer_count && !instance->preloaded_count) {	//No values in buffer
+		ME_SUBDEVICE_EXIT;
+		PERROR("No values in buffer!\n");
+		return ME_ERRNO_LACK_OF_RESOURCES;
+	}
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+
+	//Stop device
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+	//Set values for single_read()
+	instance->single_value = ME6000_AO_MAX_DATA + 1;
+	instance->single_value_in_fifo = ME6000_AO_MAX_DATA + 1;
+
+	//Setting stop points
+	if (instance->stop_mode == ME6000_AO_SCAN_STOP_MODE) {
+		instance->stop_data_count =
+		    instance->stop_count * circ_buffer_count;
+	} else {
+		instance->stop_data_count = instance->stop_count;
+	}
+
+	if ((instance->stop_data_count != 0)
+	    && (instance->stop_data_count < circ_buffer_count)) {
+		PERROR("More data in buffer than previously set limit!\n");
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	ctrl = inl(instance->ctrl_reg);
+	//Check FIFO
+	if (!(ctrl & ME6000_AO_CTRL_BIT_ENABLE_FIFO)) {	//FIFO wasn't enabeled. Do it. <= This should be done by user call with ME_WRITE_MODE_PRELOAD
+		PINFO("Enableing FIFO.\n");
+		ctrl |= ME6000_AO_CTRL_BIT_ENABLE_FIFO;
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+
+		instance->preloaded_count = 0;
+		instance->data_count = 0;
+	} else {		//Block IRQ
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+	}
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+
+	//Reset interrupt latch
+	inl(instance->irq_reset_reg);
+
+	//Fill FIFO <= Generaly this should be done by user pre-load call but this is second place to do it.
+	status = inl(instance->status_reg);
+	if (!(status & ME6000_AO_STATUS_BIT_EF)) {	//FIFO empty
+		if (instance->stop_data_count != 0) {
+			count = ME6000_AO_FIFO_COUNT;
+		} else {
+			count =
+			    (ME6000_AO_FIFO_COUNT <
+			     instance->
+			     stop_data_count) ? ME6000_AO_FIFO_COUNT :
+			    instance->stop_data_count;
+		}
+
+		//Copy data
+		count =
+		    ao_write_data(instance, count, instance->preloaded_count);
+
+		if (count < 0) {	//This should never happend!
+			PERROR_CRITICAL("COPY FINISH WITH ERROR!\n");
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+	}
+	//Set pre-load features.
+	spin_lock(instance->preload_reg_lock);
+	synch = inl(instance->preload_reg);
+	synch &=
+	    ~((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) << instance->
+	      ao_idx);
+	synch |=
+	    (instance->start_mode & ~ME6000_AO_EXT_TRIG) << instance->ao_idx;
+	outl(synch, instance->preload_reg);
+	PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, synch);
+	spin_unlock(instance->preload_reg_lock);
+
+	//Default count is '0'
+	if (instance->mode == ME6000_AO_CONTINOUS) {	//Continous
+		instance->preloaded_count = 0;
+		instance->circ_buf.tail += count;
+		instance->circ_buf.tail &= instance->circ_buf.mask;
+	} else {		//Wraparound
+		instance->preloaded_count += count;
+		instance->data_count += count;
+
+		//Special case: Infinite wraparound with less than FIFO datas always should runs in hardware mode.
+		if ((instance->stop_mode == ME6000_AO_INF_STOP_MODE)
+		    && (circ_buffer_count <= ME6000_AO_FIFO_COUNT)) {	//Change to hardware wraparound
+			PDEBUG
+			    ("Changeing mode from software wraparound to hardware wraparound.\n");
+			//Copy all data
+			count =
+			    ao_write_data(instance, circ_buffer_count,
+					  instance->preloaded_count);
+			ctrl &= ~ME6000_AO_CTRL_MODE_MASK;
+			ctrl |= ME6000_AO_MODE_WRAPAROUND;
+		}
+
+		if (instance->preloaded_count == me_circ_buf_values(&instance->circ_buf)) {	//Reset position indicator.
+			instance->preloaded_count = 0;
+		} else if (instance->preloaded_count > me_circ_buf_values(&instance->circ_buf)) {	//This should never happend!
+			PERROR_CRITICAL
+			    ("PRELOADED MORE VALUES THAN ARE IN BUFFER!\n");
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+	}
+
+	//Set status to 'wait for start'
+	instance->status = ao_status_stream_run_wait;
+
+	status = inl(instance->status_reg);
+	//Start state machine and interrupts
+	PINFO("<%s:%d> Start state machine.\n", __func__, __LINE__);
+	ctrl &= ~(ME6000_AO_CTRL_BIT_STOP | ME6000_AO_CTRL_BIT_IMMEDIATE_STOP);
+	if (instance->start_mode == ME6000_AO_EXT_TRIG) {
+		PDEBUG("DIGITAL TRIGGER\n");
+		ctrl |= ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG;
+	}
+	if (!(status & ME6000_AO_STATUS_BIT_HF)) {	//More than half!
+		if ((ctrl & ME6000_AO_CTRL_MODE_MASK) == ME6000_AO_MODE_CONTINUOUS) {	//Enable IRQ only when hardware_continous is set and FIFO is more than half
+			PINFO("<%s:%d> Start interrupts.\n", __func__,
+			      __LINE__);
+			ctrl |= ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+		}
+	}
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	//Trigger output
+	PINFO("<%s> start mode= 0x%x %s\n", __func__, instance->start_mode,
+	      (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) ? "SYNCHRONOUS" :
+	      "");
+	if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {	//Trigger outputs
+		spin_lock(instance->preload_reg_lock);
+		synch = inl(instance->preload_reg);
+		//Add channel to start list
+		outl(synch | (ME6000_AO_SYNC_HOLD << instance->ao_idx),
+		     instance->preload_reg);
+		PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->preload_reg - instance->reg_base,
+			   synch | (ME6000_AO_SYNC_HOLD << instance->ao_idx));
+
+		//Fire
+		PINFO
+		    ("Fired all software synchronous outputs by software trigger.\n");
+		outl(0x8000, instance->single_reg);
+		PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->single_reg - instance->reg_base, 0x8000);
+
+		//Restore save settings
+		outl(synch, instance->preload_reg);
+		PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->preload_reg - instance->reg_base, synch);
+		spin_unlock(instance->preload_reg_lock);
+	} else if (!instance->start_mode) {	//Trigger outputs
+/*
+		spin_lock(instance->preload_reg_lock);
+			synch = inl(instance->preload_reg);
+			//Remove channel from start list
+			outl(synch & ~(ME6000_AO_SYNC_HOLD << instance->ao_idx), instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, synch & ~(ME6000_AO_SYNC_HOLD << instance->ao_idx));
+*/
+		//Fire
+		PINFO("Software trigger.\n");
+		outl(0x8000, instance->single_reg);
+		PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->single_reg - instance->reg_base, 0x8000);
+
+/*
+			//Restore save settings
+			outl(synch, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, synch);
+		spin_unlock(instance->preload_reg_lock);
+*/
+	}
+	// Set control task's timeout
+	instance->timeout.delay = delay;
+	instance->timeout.start_time = jiffies;
+
+	if (status & ME6000_AO_STATUS_BIT_HF) {	//Less than half but not empty!
+		PINFO("Less than half.\n");
+		if (instance->stop_data_count == 0) {
+			count = ME6000_AO_FIFO_COUNT / 2;
+		} else {
+			count =
+			    ((ME6000_AO_FIFO_COUNT / 2) <
+			     instance->stop_data_count) ? ME6000_AO_FIFO_COUNT /
+			    2 : instance->stop_data_count;
+		}
+
+		//Copy data
+		count =
+		    ao_write_data(instance, count, instance->preloaded_count);
+
+		if (count < 0) {	//This should never happend!
+			PERROR_CRITICAL("COPY FINISH WITH ERROR!\n");
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+
+		if (instance->mode == ME6000_AO_CONTINOUS) {	//Continous
+			instance->circ_buf.tail += count;
+			instance->circ_buf.tail &= instance->circ_buf.mask;
+		} else {	//Wraparound
+			instance->data_count += count;
+			instance->preloaded_count += count;
+
+			if (instance->preloaded_count == me_circ_buf_values(&instance->circ_buf)) {	//Reset position indicator.
+				instance->preloaded_count = 0;
+			} else if (instance->preloaded_count > me_circ_buf_values(&instance->circ_buf)) {	//This should never happend!
+				PERROR_CRITICAL
+				    ("PRELOADED MORE VALUES THAN ARE IN BUFFER!\n");
+				ME_SUBDEVICE_EXIT;
+				return ME_ERRNO_INTERNAL;
+			}
+		}
+
+		status = inl(instance->status_reg);
+		if (!(status & ME6000_AO_STATUS_BIT_HF)) {	//More than half!
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			PINFO("<%s:%d> Start interrupts.\n", __func__,
+			      __LINE__);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |= ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		}
+	}
+	//Special case: Limited wraparound with less than HALF FIFO datas need work around to generate first interrupt.
+	if ((instance->stop_mode != ME6000_AO_INF_STOP_MODE)
+	    && (instance->mode == ME6000_AO_SW_WRAP_MODE)
+	    && (circ_buffer_count <= (ME6000_AO_FIFO_COUNT / 2))) {	//Put more data to FIFO
+		PINFO("Limited wraparound with less than HALF FIFO datas.\n");
+		if (instance->preloaded_count) {	//This should never happend!
+			PERROR_CRITICAL
+			    ("ERROR WHEN LOADING VALUES FOR WRAPAROUND!\n");
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+
+		while (instance->stop_data_count > instance->data_count) {	//Maximum data not set jet.
+			//Copy to buffer
+			if (circ_buffer_count != ao_write_data(instance, circ_buffer_count, 0)) {	//This should never happend!
+				PERROR_CRITICAL
+				    ("ERROR WHEN LOADING VALUES FOR WRAPAROUND!\n");
+				ME_SUBDEVICE_EXIT;
+				return ME_ERRNO_INTERNAL;
+			}
+			instance->data_count += circ_buffer_count;
+
+			if (!((status = inl(instance->status_reg)) & ME6000_AO_STATUS_BIT_HF)) {	//FIFO is more than half. Enable IRQ and end copy.
+				//Reset interrupt latch
+				inl(instance->irq_reset_reg);
+
+				spin_lock_irqsave(&instance->subdevice_lock,
+						  cpu_flags);
+				PINFO("<%s:%d> Start interrupts.\n",
+				      __func__, __LINE__);
+				ctrl = inl(instance->ctrl_reg);
+				ctrl |= ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+				outl(ctrl, instance->ctrl_reg);
+				PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->ctrl_reg -
+					   instance->reg_base, ctrl);
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				break;
+			}
+		}
+	}
+	// Schedule control task
+	instance->ao_control_task_flag = 1;
+	queue_delayed_work(instance->me6000_workqueue,
+			   &instance->ao_control_task, 1);
+
+	if (start_mode == ME_START_MODE_BLOCKING) {	//Wait for start.
+		ref = jiffies;
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_stream_run_wait),
+						 (delay) ? delay +
+						 1 : LONG_MAX);
+
+		if ((instance->status != ao_status_stream_run)
+		    && (instance->status != ao_status_stream_end)) {
+			PDEBUG("Starting stream canceled. %d\n",
+			       instance->status);
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		if ((delay) && ((jiffies - ref) >= delay)) {
+			if (instance->status != ao_status_stream_run) {
+				if (instance->status == ao_status_stream_end) {
+					PDEBUG("Timeout reached.\n");
+				} else if ((jiffies - ref) > delay) {
+					PERROR
+					    ("Timeout reached. Not handled by control task!\n");
+					ao_stop_immediately(instance);
+				} else {
+					PERROR
+					    ("Timeout reached. Signal come but status is strange: %d\n",
+					     instance->status);
+					ao_stop_immediately(instance);
+				}
+
+				instance->ao_control_task_flag = 0;
+				cancel_delayed_work(&instance->ao_control_task);
+				instance->status = ao_status_stream_end;
+				err = ME_ERRNO_TIMEOUT;
+			}
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+	return err;
+}
+
+static int me6000_ao_io_stream_status(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int wait,
+				      int *status, int *values, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((wait != ME_WAIT_NONE) && (wait != ME_WAIT_IDLE)) {
+		PERROR("Invalid wait argument specified.\n");
+		*status = ME_STATUS_INVALID;
+		return ME_ERRNO_INVALID_WAIT;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	switch (instance->status) {
+	case ao_status_single_configured:
+	case ao_status_single_end:
+	case ao_status_stream_configured:
+	case ao_status_stream_end:
+	case ao_status_stream_fifo_error:
+	case ao_status_stream_buffer_error:
+	case ao_status_stream_error:
+		*status = ME_STATUS_IDLE;
+		break;
+
+	case ao_status_single_run_wait:
+	case ao_status_single_run:
+	case ao_status_single_end_wait:
+	case ao_status_stream_run_wait:
+	case ao_status_stream_run:
+	case ao_status_stream_end_wait:
+		*status = ME_STATUS_BUSY;
+		break;
+
+	case ao_status_none:
+	default:
+		*status =
+		    (inl(instance->status_reg) & ME6000_AO_STATUS_BIT_FSM) ?
+		    ME_STATUS_BUSY : ME_STATUS_IDLE;
+		break;
+	}
+
+	if ((wait == ME_WAIT_IDLE) && (*status == ME_STATUS_BUSY)) {
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 ((instance->status !=
+						   ao_status_single_run_wait)
+						  && (instance->status !=
+						      ao_status_single_run)
+						  && (instance->status !=
+						      ao_status_single_end_wait)
+						  && (instance->status !=
+						      ao_status_stream_run_wait)
+						  && (instance->status !=
+						      ao_status_stream_run)
+						  && (instance->status !=
+						      ao_status_stream_end_wait)),
+						 LONG_MAX);
+
+		if (instance->status != ao_status_stream_end) {
+			PDEBUG("Wait for IDLE canceled. %d\n",
+			       instance->status);
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait for IDLE interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		*status = ME_STATUS_IDLE;
+	}
+
+	*values = me_circ_buf_space(&instance->circ_buf);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_stream_stop(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int stop_mode, int flags)
+{				/// @note Stop work and empty buffer and FIFO
+	int err = ME_ERRNO_SUCCESS;
+	me6000_ao_subdevice_t *instance;
+	unsigned long cpu_flags;
+	volatile uint32_t ctrl;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags & ~ME_IO_STREAM_STOP_PRESERVE_BUFFERS) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((stop_mode != ME_STOP_MODE_IMMEDIATE)
+	    && (stop_mode != ME_STOP_MODE_LAST_VALUE)) {
+		PERROR("Invalid stop mode specified.\n");
+		return ME_ERRNO_INVALID_STOP_MODE;
+	}
+
+	if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (instance->status < ao_status_stream_configured) {
+		//There is nothing to stop!
+		PERROR("Subdevice not in streaming mode. %d\n",
+		       instance->status);
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	//Mark as stopping. => Software stop.
+	instance->status = ao_status_stream_end_wait;
+
+	if (stop_mode == ME_STOP_MODE_IMMEDIATE) {	//Stopped now!
+		err = ao_stop_immediately(instance);
+	} else if (stop_mode == ME_STOP_MODE_LAST_VALUE) {
+		ctrl = inl(instance->ctrl_reg) & ME6000_AO_CTRL_MODE_MASK;
+		if (ctrl == ME6000_AO_MODE_WRAPAROUND) {	//Hardware wraparound => Hardware stop.
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |= ME6000_AO_CTRL_BIT_STOP;
+			ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			//Reset interrupt latch
+			inl(instance->irq_reset_reg);
+		}
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_stream_end_wait),
+						 LONG_MAX);
+
+		if (instance->status != ao_status_stream_end) {
+			PDEBUG("Stopping stream canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Stopping stream interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	ctrl = inl(instance->ctrl_reg);
+	ctrl |= ME6000_AO_CTRL_BIT_STOP | ME6000_AO_CTRL_BIT_IMMEDIATE_STOP;
+	ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+	if (!flags) {		//Reset FIFO
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_FIFO;
+	}
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	//Reset interrupt latch
+	inl(instance->irq_reset_reg);
+
+	if (!flags) {		//Reset software buffer
+		instance->circ_buf.head = 0;
+		instance->circ_buf.tail = 0;
+		instance->preloaded_count = 0;
+		instance->data_count = 0;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_stream_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int write_mode,
+				     int *values, int *count, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me6000_ao_subdevice_t *instance;
+	unsigned long cpu_flags = 0;
+	uint32_t reg_copy;
+
+	int copied_from_user = 0;
+	int left_to_copy_from_user = *count;
+
+	int copied_values;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	//Checking arguments
+	if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (*count <= 0) {
+		PERROR("Invalid count of values specified.\n");
+		return ME_ERRNO_INVALID_VALUE_COUNT;
+	}
+
+	if (values == NULL) {
+		PERROR("Invalid address of values specified.\n");
+		return ME_ERRNO_INVALID_POINTER;
+	}
+
+	if ((instance->status == ao_status_none) || (instance->status == ao_status_single_configured)) {	//The device is in single mode.
+		PERROR
+		    ("Subdevice must be preinitialize correctly for streaming.\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	switch (write_mode) {
+	case ME_WRITE_MODE_PRELOAD:
+
+		//Device must be stopped.
+		if ((instance->status != ao_status_stream_configured)
+		    && (instance->status != ao_status_stream_end)) {
+			PERROR
+			    ("Subdevice mustn't be runing when 'pre-load' mode is used.\n");
+			return ME_ERRNO_PREVIOUS_CONFIG;
+		}
+		break;
+	case ME_WRITE_MODE_NONBLOCKING:
+	case ME_WRITE_MODE_BLOCKING:
+		/// @note In blocking mode: When device is not runing and there is not enought space call will blocked up!
+		/// @note Some other thread must empty buffer by strating engine.
+		break;
+
+	default:
+		PERROR("Invalid write mode specified.\n");
+		return ME_ERRNO_INVALID_WRITE_MODE;
+	}
+
+	if (instance->mode & ME6000_AO_WRAP_MODE) {	//Wraparound mode. Device must be stopped.
+		if ((instance->status != ao_status_stream_configured)
+		    && (instance->status != ao_status_stream_end)) {
+			PERROR
+			    ("Subdevice mustn't be runing when 'pre-load' mode is used.\n");
+			return ME_ERRNO_INVALID_WRITE_MODE;
+		}
+	}
+
+	if ((instance->mode == ME6000_AO_HW_WRAP_MODE)
+	    && (write_mode != ME_WRITE_MODE_PRELOAD)) {
+/*
+		PERROR("Only 'pre-load' write is acceptable in hardware wraparound mode.\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+*/
+		//This is transparent for user.
+		PDEBUG("Changing write_mode to ME_WRITE_MODE_PRELOAD.\n");
+		write_mode = ME_WRITE_MODE_PRELOAD;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (write_mode == ME_WRITE_MODE_PRELOAD) {	//Init enviroment - preload
+		spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+		reg_copy = inl(instance->ctrl_reg);
+		//Check FIFO
+		if (!(reg_copy & ME6000_AO_CTRL_BIT_ENABLE_FIFO)) {	//FIFO not active. Enable it.
+			reg_copy |= ME6000_AO_CTRL_BIT_ENABLE_FIFO;
+			outl(reg_copy, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   reg_copy);
+			instance->preloaded_count = 0;
+		}
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	}
+
+	while (1) {
+		//Copy to buffer. This step is common for all modes.
+		copied_from_user =
+		    ao_get_data_from_user(instance, left_to_copy_from_user,
+					  values + (*count -
+						    left_to_copy_from_user));
+		left_to_copy_from_user -= copied_from_user;
+
+		reg_copy = inl(instance->status_reg);
+		if ((instance->status == ao_status_stream_run) && !(reg_copy & ME6000_AO_STATUS_BIT_FSM)) {	//BROKEN PIPE! The state machine is stoped but logical status show that should be working.
+			PERROR("Broken pipe in write.\n");
+			err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+			break;
+		}
+
+		if ((instance->status == ao_status_stream_run) && (instance->mode == ME6000_AO_CONTINOUS) && (reg_copy & ME6000_AO_STATUS_BIT_HF)) {	//Continous mode runing and data are below half!
+
+			// Block interrupts.
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			reg_copy = inl(instance->ctrl_reg);
+			reg_copy &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(reg_copy, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   reg_copy);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			//Fast copy
+			copied_values =
+			    ao_write_data(instance, ME6000_AO_FIFO_COUNT / 2,
+					  0);
+			if (copied_values > 0) {
+				instance->circ_buf.tail += copied_values;
+				instance->circ_buf.tail &=
+				    instance->circ_buf.mask;
+				continue;
+			}
+			//Reset interrupt latch
+			inl(instance->irq_reset_reg);
+
+			// Activate interrupts.
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			reg_copy = inl(instance->ctrl_reg);
+			reg_copy |= ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(reg_copy, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   reg_copy);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if (copied_values == 0) {	//This was checked and never should happend!
+				PERROR_CRITICAL("COPY FINISH WITH 0!\n");
+			}
+
+			if (copied_values < 0) {	//This was checked and never should happend!
+				PERROR_CRITICAL("COPY FINISH WITH ERROR!\n");
+				instance->status = ao_status_stream_fifo_error;
+				err = ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+				break;
+			}
+		}
+
+		if (!left_to_copy_from_user) {	//All datas were copied.
+			break;
+		} else {	//Not all datas were copied.
+			if (instance->mode & ME6000_AO_WRAP_MODE) {	//Error too much datas! Wraparound is limited in size!
+				PERROR
+				    ("Too much data for wraparound mode!  Exceeded size of %ld.\n",
+				     ME6000_AO_CIRC_BUF_COUNT - 1);
+				err = ME_ERRNO_RING_BUFFER_OVERFLOW;
+				break;
+			}
+
+			if (write_mode != ME_WRITE_MODE_BLOCKING) {	//Non blocking calls
+				break;
+			}
+
+			wait_event_interruptible(instance->wait_queue,
+						 me_circ_buf_space(&instance->
+								   circ_buf));
+
+			if (signal_pending(current)) {
+				PERROR("Writing interrupted by signal.\n");
+				instance->status = ao_status_none;
+				ao_stop_immediately(instance);
+				err = ME_ERRNO_SIGNAL;
+				break;
+			}
+
+			if (instance->status == ao_status_none) {	//Reset
+				PERROR("Writing interrupted by reset.\n");
+				err = ME_ERRNO_CANCELLED;
+				break;
+			}
+		}
+	}
+
+	if (write_mode == ME_WRITE_MODE_PRELOAD) {	//Copy data to FIFO - preload
+		copied_values =
+		    ao_write_data_pooling(instance, ME6000_AO_FIFO_COUNT,
+					  instance->preloaded_count);
+		instance->preloaded_count += copied_values;
+		instance->data_count += copied_values;
+
+		if ((instance->mode == ME6000_AO_HW_WRAP_MODE)
+		    && (me_circ_buf_values(&instance->circ_buf) >
+			ME6000_AO_FIFO_COUNT)) {
+			PERROR
+			    ("Too much data for hardware wraparound mode! Exceeded size of %d.\n",
+			     ME6000_AO_FIFO_COUNT);
+			err = ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+		}
+	}
+
+	*count = *count - left_to_copy_from_user;
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me6000_ao_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me6000_ao_isr(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{
+	me6000_ao_subdevice_t *instance = dev_id;
+	uint32_t irq_status;
+	uint32_t ctrl;
+	uint32_t status;
+	int count = 0;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	irq_status = inl(instance->irq_status_reg);
+	if (!(irq_status & (ME6000_IRQ_STATUS_BIT_AO_HF << instance->ao_idx))) {
+		PINFO("%ld Shared interrupt. %s(): ID=%d: status_reg=0x%04X\n",
+		      jiffies, __func__, instance->ao_idx, irq_status);
+		return IRQ_NONE;
+	}
+
+	if (!instance->circ_buf.buf) {
+		instance->status = ao_status_stream_error;
+		PERROR_CRITICAL("CIRCULAR BUFFER NOT EXISTS!\n");
+		//Block interrupts. Stop machine.
+		ctrl = inl(instance->ctrl_reg);
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+		ctrl |=
+		    ME6000_AO_CTRL_BIT_IMMEDIATE_STOP | ME6000_AO_CTRL_BIT_STOP;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		//Inform user
+		wake_up_interruptible_all(&instance->wait_queue);
+		return IRQ_HANDLED;
+	}
+
+	status = inl(instance->status_reg);
+	if (!(status & ME6000_AO_STATUS_BIT_FSM)) {	//Too late. Not working! END? BROKEN PIPE?
+		/// @note Error checking was moved to separate task.
+		PDEBUG("Interrupt come but ISM is not working!\n");
+		//Block interrupts. Stop machine.
+		ctrl = inl(instance->ctrl_reg);
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+		ctrl |=
+		    ME6000_AO_CTRL_BIT_STOP | ME6000_AO_CTRL_BIT_IMMEDIATE_STOP;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		//Reset interrupt latch
+		inl(instance->irq_reset_reg);
+
+		/// @note User notification was also moved to separate task.
+		return IRQ_HANDLED;
+	}
+	//General procedure. Process more datas.
+
+#ifdef MEDEBUG_DEBUG
+	if (!me_circ_buf_values(&instance->circ_buf)) {	//Buffer is empty!
+		PDEBUG("Circular buffer empty!\n");
+	}
+#endif
+
+	//Check FIFO
+	if (status & ME6000_AO_STATUS_BIT_HF) {	//OK less than half
+
+		//Block interrupts
+		ctrl = inl(instance->ctrl_reg);
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		do {
+			//Calculate how many should be copied.
+			count =
+			    (instance->stop_data_count) ? instance->
+			    stop_data_count -
+			    instance->data_count : ME6000_AO_FIFO_COUNT / 2;
+			if (ME6000_AO_FIFO_COUNT / 2 < count) {
+				count = ME6000_AO_FIFO_COUNT / 2;
+			}
+			//Copy data
+			if (instance->mode == ME6000_AO_CONTINOUS) {	//Continous
+				count = ao_write_data(instance, count, 0);
+				if (count > 0) {
+					instance->circ_buf.tail += count;
+					instance->circ_buf.tail &=
+					    instance->circ_buf.mask;
+					instance->data_count += count;
+
+					if ((instance->status == ao_status_stream_end_wait) && !me_circ_buf_values(&instance->circ_buf)) {	//Stoping. Whole buffer was copied.
+						break;
+					}
+				}
+			} else if ((instance->mode == ME6000_AO_SW_WRAP_MODE) && ((ctrl & ME6000_AO_CTRL_MODE_MASK) == ME6000_AO_MODE_CONTINUOUS)) {	//Wraparound (software)
+				if (instance->status == ao_status_stream_end_wait) {	//We stoping => Copy to the end of the buffer.
+					count =
+					    ao_write_data(instance, count, 0);
+				} else {	//Copy in wraparound mode.
+					count =
+					    ao_write_data_wraparound(instance,
+								     count,
+								     instance->
+								     preloaded_count);
+				}
+
+				if (count > 0) {
+					instance->data_count += count;
+					instance->preloaded_count += count;
+					instance->preloaded_count %=
+					    me_circ_buf_values(&instance->
+							       circ_buf);
+
+					if ((instance->status == ao_status_stream_end_wait) && !instance->preloaded_count) {	//Stoping. Whole buffer was copied.
+						break;
+					}
+				}
+			}
+
+			if ((count <= 0) || (instance->stop_data_count && (instance->stop_data_count <= instance->data_count))) {	//End of work.
+				break;
+			}
+		}		//Repeat if still is under half fifo
+		while ((status =
+			inl(instance->status_reg)) & ME6000_AO_STATUS_BIT_HF);
+
+		//Unblock interrupts
+		ctrl = inl(instance->ctrl_reg);
+		if (count >= 0) {	//Copy was successful.
+			if (instance->stop_data_count && (instance->stop_data_count <= instance->data_count)) {	//Finishing work. No more interrupts.
+				PDEBUG("Finishing work. Interrupt disabled.\n");
+				instance->status = ao_status_stream_end_wait;
+			} else if (count > 0) {	//Normal work. Enable interrupt.
+				PDEBUG("Normal work. Enable interrupt.\n");
+				ctrl |= ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+			} else {	//Normal work but there are no more data in buffer. Interrupt blocked. stream_write() will unblock it.
+				PDEBUG
+				    ("No data in software buffer. Interrupt blocked.\n");
+			}
+		} else {	//Error during copy.
+			instance->status = ao_status_stream_fifo_error;
+		}
+
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+	} else {		//?? more than half
+		PDEBUG
+		    ("Interrupt come but FIFO more than half full! Reset interrupt.\n");
+	}
+
+	PINFO("ISR: Buffer count: %d.(T:%d H:%d)\n",
+	      me_circ_buf_values(&instance->circ_buf), instance->circ_buf.tail,
+	      instance->circ_buf.head);
+	PINFO("ISR: Stop count: %d.\n", instance->stop_count);
+	PINFO("ISR: Stop data count: %d.\n", instance->stop_data_count);
+	PINFO("ISR: Data count: %d.\n", instance->data_count);
+
+	//Reset interrupt latch
+	inl(instance->irq_reset_reg);
+
+	//Inform user
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+static void me6000_ao_destructor(struct me_subdevice *subdevice)
+{
+	me6000_ao_subdevice_t *instance;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	instance->ao_control_task_flag = 0;
+
+	// Reset subdevice to asure clean exit.
+	me6000_ao_io_reset_subdevice(subdevice, NULL,
+				     ME_IO_RESET_SUBDEVICE_NO_FLAGS);
+
+	// Remove any tasks from work queue. This is paranoic because it was done allready in reset().
+	if (!cancel_delayed_work(&instance->ao_control_task)) {	//Wait 2 ticks to be sure that control task is removed from queue.
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(2);
+	}
+
+	if (instance->fifo & ME6000_AO_HAS_FIFO) {
+		if (instance->irq) {
+			free_irq(instance->irq, instance);
+			instance->irq = 0;
+		}
+
+		if (instance->circ_buf.buf) {
+			PDEBUG("free circ_buf = %p size=%d",
+			       instance->circ_buf.buf,
+			       PAGE_SHIFT << ME6000_AO_CIRC_BUF_SIZE_ORDER);
+			free_pages((unsigned long)instance->circ_buf.buf,
+				   ME6000_AO_CIRC_BUF_SIZE_ORDER);
+		}
+		instance->circ_buf.buf = NULL;
+	}
+
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+me6000_ao_subdevice_t *me6000_ao_constructor(uint32_t reg_base,
+					     spinlock_t * preload_reg_lock,
+					     uint32_t * preload_flags,
+					     uint32_t * triggering_flags,
+					     int ao_idx,
+					     int fifo,
+					     int irq,
+					     int high_range,
+					     struct workqueue_struct *me6000_wq)
+{
+	me6000_ao_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed ID=%d.\n", ao_idx);
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me6000_ao_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me6000_ao_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->preload_reg_lock = preload_reg_lock;
+	subdevice->preload_flags = preload_flags;
+	subdevice->triggering_flags = triggering_flags;
+
+	/* Store analog output index */
+	subdevice->ao_idx = ao_idx;
+
+	/* Store if analog output has fifo */
+	subdevice->fifo = fifo;
+
+	if (subdevice->fifo & ME6000_AO_HAS_FIFO) {
+		/* Allocate and initialize circular buffer */
+		subdevice->circ_buf.mask = ME6000_AO_CIRC_BUF_COUNT - 1;
+		subdevice->circ_buf.buf =
+		    (void *)__get_free_pages(GFP_KERNEL,
+					     ME6000_AO_CIRC_BUF_SIZE_ORDER);
+		PDEBUG("circ_buf = %p size=%ld\n", subdevice->circ_buf.buf,
+		       ME6000_AO_CIRC_BUF_SIZE);
+
+		if (!subdevice->circ_buf.buf) {
+			PERROR
+			    ("Cannot initialize subdevice base class instance.\n");
+			kfree(subdevice);
+			return NULL;
+		}
+
+		memset(subdevice->circ_buf.buf, 0, ME6000_AO_CIRC_BUF_SIZE);
+	} else {
+		subdevice->circ_buf.mask = 0;
+		subdevice->circ_buf.buf = NULL;
+	}
+	subdevice->circ_buf.head = 0;
+	subdevice->circ_buf.tail = 0;
+
+	subdevice->status = ao_status_none;
+	subdevice->ao_control_task_flag = 0;
+	subdevice->timeout.delay = 0;
+	subdevice->timeout.start_time = jiffies;
+
+	/* Initialize wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	/* Initialize single value to 0V */
+	subdevice->single_value = 0x8000;
+	subdevice->single_value_in_fifo = 0x8000;
+
+	/* Initialize range boarders */
+	if (high_range) {
+		subdevice->min = ME6000_AO_MIN_RANGE_HIGH;
+		subdevice->max = ME6000_AO_MAX_RANGE_HIGH;
+	} else {
+		subdevice->min = ME6000_AO_MIN_RANGE;
+		subdevice->max = ME6000_AO_MAX_RANGE;
+	}
+
+	/* Register interrupt service routine */
+
+	if (subdevice->fifo & ME6000_AO_HAS_FIFO) {
+		subdevice->irq = irq;
+		if (request_irq(subdevice->irq, me6000_ao_isr,
+#ifdef IRQF_DISABLED
+				IRQF_DISABLED | IRQF_SHARED,
+#else
+				SA_INTERRUPT | SA_SHIRQ,
+#endif
+				ME6000_NAME, subdevice)) {
+			PERROR("Cannot get interrupt line.\n");
+			PDEBUG("free circ_buf = %p size=%d",
+			       subdevice->circ_buf.buf,
+			       PAGE_SHIFT << ME6000_AO_CIRC_BUF_SIZE_ORDER);
+			free_pages((unsigned long)subdevice->circ_buf.buf,
+				   ME6000_AO_CIRC_BUF_SIZE_ORDER);
+			subdevice->circ_buf.buf = NULL;
+			kfree(subdevice);
+			return NULL;
+		}
+		PINFO("Registered irq=%d.\n", subdevice->irq);
+	} else {
+		subdevice->irq = 0;
+	}
+
+	/* Initialize registers */
+	// Only streamed subdevices support interrupts. For the rest this register has no meaning.
+	subdevice->irq_status_reg = reg_base + ME6000_AO_IRQ_STATUS_REG;
+	subdevice->preload_reg = reg_base + ME6000_AO_PRELOAD_REG;
+
+	if (ao_idx == 0) {
+		subdevice->ctrl_reg = reg_base + ME6000_AO_00_CTRL_REG;
+		subdevice->status_reg = reg_base + ME6000_AO_00_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME6000_AO_00_FIFO_REG;
+		subdevice->timer_reg = reg_base + ME6000_AO_00_TIMER_REG;
+		subdevice->irq_reset_reg =
+		    reg_base + ME6000_AO_00_IRQ_RESET_REG;
+		subdevice->single_reg = reg_base + ME6000_AO_00_SINGLE_REG;
+	} else if (ao_idx == 1) {
+		subdevice->ctrl_reg = reg_base + ME6000_AO_01_CTRL_REG;
+		subdevice->status_reg = reg_base + ME6000_AO_01_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME6000_AO_01_FIFO_REG;
+		subdevice->timer_reg = reg_base + ME6000_AO_01_TIMER_REG;
+		subdevice->irq_reset_reg =
+		    reg_base + ME6000_AO_01_IRQ_RESET_REG;
+		subdevice->single_reg = reg_base + ME6000_AO_01_SINGLE_REG;
+	} else if (ao_idx == 2) {
+		subdevice->ctrl_reg = reg_base + ME6000_AO_02_CTRL_REG;
+		subdevice->status_reg = reg_base + ME6000_AO_02_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME6000_AO_02_FIFO_REG;
+		subdevice->timer_reg = reg_base + ME6000_AO_02_TIMER_REG;
+		subdevice->irq_reset_reg =
+		    reg_base + ME6000_AO_02_IRQ_RESET_REG;
+		subdevice->single_reg = reg_base + ME6000_AO_02_SINGLE_REG;
+	} else if (ao_idx == 3) {
+		subdevice->ctrl_reg = reg_base + ME6000_AO_03_CTRL_REG;
+		subdevice->status_reg = reg_base + ME6000_AO_03_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME6000_AO_03_FIFO_REG;
+		subdevice->timer_reg = reg_base + ME6000_AO_03_TIMER_REG;
+		subdevice->irq_reset_reg =
+		    reg_base + ME6000_AO_03_IRQ_RESET_REG;
+		subdevice->single_reg = reg_base + ME6000_AO_03_SINGLE_REG;
+	} else {
+		subdevice->ctrl_reg = reg_base + ME6000_AO_DUMY;
+		subdevice->fifo_reg = reg_base + ME6000_AO_DUMY;
+		subdevice->timer_reg = reg_base + ME6000_AO_DUMY;
+		subdevice->irq_reset_reg = reg_base + ME6000_AO_DUMY;
+		subdevice->single_reg = reg_base + ME6000_AO_DUMY;
+
+		subdevice->status_reg = reg_base + ME6000_AO_SINGLE_STATUS_REG;
+		if (ao_idx == 4) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_04_SINGLE_REG;
+		} else if (ao_idx == 5) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_05_SINGLE_REG;
+		} else if (ao_idx == 6) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_06_SINGLE_REG;
+		} else if (ao_idx == 7) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_07_SINGLE_REG;
+		} else if (ao_idx == 8) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_08_SINGLE_REG;
+		} else if (ao_idx == 9) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_09_SINGLE_REG;
+		} else if (ao_idx == 10) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_10_SINGLE_REG;
+		} else if (ao_idx == 11) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_11_SINGLE_REG;
+		} else if (ao_idx == 12) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_12_SINGLE_REG;
+		} else if (ao_idx == 13) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_13_SINGLE_REG;
+		} else if (ao_idx == 14) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_14_SINGLE_REG;
+		} else if (ao_idx == 15) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_15_SINGLE_REG;
+		} else {
+			PERROR_CRITICAL("WRONG SUBDEVICE ID=%d!", ao_idx);
+			me_subdevice_deinit((me_subdevice_t *) subdevice);
+			if (subdevice->fifo) {
+				free_pages((unsigned long)subdevice->circ_buf.
+					   buf, ME6000_AO_CIRC_BUF_SIZE_ORDER);
+			}
+			subdevice->circ_buf.buf = NULL;
+			kfree(subdevice);
+			return NULL;
+		}
+	}
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Override base class methods. */
+	subdevice->base.me_subdevice_destructor = me6000_ao_destructor;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me6000_ao_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me6000_ao_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me6000_ao_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me6000_ao_io_single_write;
+	subdevice->base.me_subdevice_io_stream_config =
+	    me6000_ao_io_stream_config;
+	subdevice->base.me_subdevice_io_stream_new_values =
+	    me6000_ao_io_stream_new_values;
+	subdevice->base.me_subdevice_io_stream_write =
+	    me6000_ao_io_stream_write;
+	subdevice->base.me_subdevice_io_stream_start =
+	    me6000_ao_io_stream_start;
+	subdevice->base.me_subdevice_io_stream_status =
+	    me6000_ao_io_stream_status;
+	subdevice->base.me_subdevice_io_stream_stop = me6000_ao_io_stream_stop;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me6000_ao_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me6000_ao_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me6000_ao_query_subdevice_caps;
+	subdevice->base.me_subdevice_query_subdevice_caps_args =
+	    me6000_ao_query_subdevice_caps_args;
+	subdevice->base.me_subdevice_query_range_by_min_max =
+	    me6000_ao_query_range_by_min_max;
+	subdevice->base.me_subdevice_query_number_ranges =
+	    me6000_ao_query_number_ranges;
+	subdevice->base.me_subdevice_query_range_info =
+	    me6000_ao_query_range_info;
+	subdevice->base.me_subdevice_query_timer = me6000_ao_query_timer;
+
+	//prepare work queue and work function
+	subdevice->me6000_workqueue = me6000_wq;
+
+/* workqueue API changed in kernel 2.6.20 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20) )
+	INIT_WORK(&subdevice->ao_control_task, me6000_ao_work_control_task,
+		  (void *)subdevice);
+#else
+	INIT_DELAYED_WORK(&subdevice->ao_control_task,
+			  me6000_ao_work_control_task);
+#endif
+
+	if (subdevice->fifo) {	//Set speed
+		outl(ME6000_AO_MIN_CHAN_TICKS - 1, subdevice->timer_reg);
+		subdevice->hardware_stop_delay = HZ / 10;	//100ms
+	}
+
+	return subdevice;
+}
+
+/** @brief Stop presentation. Preserve FIFOs.
+*
+* @param instance The subdevice instance (pointer).
+*/
+int inline ao_stop_immediately(me6000_ao_subdevice_t * instance)
+{
+	unsigned long cpu_flags;
+	uint32_t ctrl;
+	int timeout;
+	int i;
+	uint32_t single_mask;
+
+	single_mask =
+	    (instance->ao_idx - ME6000_AO_SINGLE_STATUS_OFFSET <
+	     0) ? 0x0000 : (0x0001 << (instance->ao_idx -
+				       ME6000_AO_SINGLE_STATUS_OFFSET));
+
+	timeout =
+	    (instance->hardware_stop_delay >
+	     (HZ / 10)) ? instance->hardware_stop_delay : HZ / 10;
+	for (i = 0; i <= timeout; i++) {
+		if (instance->fifo) {
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			// Stop all actions. No conditions! Block interrupts. Leave FIFO untouched!
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |=
+			    ME6000_AO_CTRL_BIT_STOP |
+			    ME6000_AO_CTRL_BIT_IMMEDIATE_STOP;
+			ctrl &=
+			    ~(ME6000_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG);
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if (!(inl(instance->status_reg) & ME6000_AO_STATUS_BIT_FSM)) {	// Exit.
+				break;
+			}
+		} else {
+			if (!(inl(instance->status_reg) & single_mask)) {	// Exit.
+				break;
+			}
+		}
+
+		PINFO("<%s> Wait for stop: %d\n", __func__, i);
+
+		//Still working!
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(1);
+	}
+
+	if (i > timeout) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		return ME_ERRNO_INTERNAL;
+	}
+	return ME_ERRNO_SUCCESS;
+}
+
+/** @brief Copy data from circular buffer to fifo (fast) in wraparound.
+* @note This is time critical function. Checking is done at begining and end only.
+* @note The is not reasonable way to check how many walues was in FIFO at begining. The count must be managed externaly.
+*
+* @param instance The subdevice instance (pointer).
+* @param count Maximum number of copied data.
+* @param start_pos Position of the firs value in buffer.
+*
+* @return On success: Number of copied data.
+* @return On error/success: 0.	No datas were copied => no data in buffer.
+* @return On error: -ME_ERRNO_FIFO_BUFFER_OVERFLOW.
+*/
+int inline ao_write_data_wraparound(me6000_ao_subdevice_t * instance, int count,
+				    int start_pos)
+{				/// @note This is time critical function!
+	uint32_t status;
+	uint32_t value;
+	int pos =
+	    (instance->circ_buf.tail + start_pos) & instance->circ_buf.mask;
+	int local_count = count;
+	int i = 1;
+
+	if (count <= 0) {	//Wrong count!
+		return 0;
+	}
+
+	while (i < local_count) {
+		//Get value from buffer
+		value = *(instance->circ_buf.buf + pos);
+		//Prepare it
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+
+		pos++;
+		pos &= instance->circ_buf.mask;
+		if (pos == instance->circ_buf.head) {
+			pos = instance->circ_buf.tail;
+		}
+		i++;
+	}
+
+	status = inl(instance->status_reg);
+	if (!(status & ME6000_AO_STATUS_BIT_FF)) {	//FIFO is full before all datas were copied!
+		PERROR("idx=%d FIFO is full before all datas were copied!\n",
+		       instance->ao_idx);
+		return -ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+	} else {		//Add last value
+		value = *(instance->circ_buf.buf + pos);
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+	}
+
+	PINFO("idx=%d WRAPAROUND LOADED %d values\n", instance->ao_idx,
+	      local_count);
+	return local_count;
+}
+
+/** @brief Copy data from software buffer to fifo (fast).
+* @note This is time critical function. Checking is done at begining and end only.
+* @note The is not reasonable way to check how many walues was in FIFO at begining. The count must be managed externaly.
+*
+* @param instance The subdevice instance (pointer).
+* @param count Maximum number of copied data.
+* @param start_pos Position of the firs value in buffer.
+*
+* @return On success: Number of copied data.
+* @return On error/success: 0.	No datas were copied => no data in buffer.
+* @return On error: -ME_ERRNO_FIFO_BUFFER_OVERFLOW.
+*/
+int inline ao_write_data(me6000_ao_subdevice_t * instance, int count,
+			 int start_pos)
+{				/// @note This is time critical function!
+	uint32_t status;
+	uint32_t value;
+	int pos =
+	    (instance->circ_buf.tail + start_pos) & instance->circ_buf.mask;
+	int local_count = count;
+	int max_count;
+	int i = 1;
+
+	if (count <= 0) {	//Wrong count!
+		return 0;
+	}
+
+	max_count = me_circ_buf_values(&instance->circ_buf) - start_pos;
+	if (max_count <= 0) {	//No data to copy!
+		return 0;
+	}
+
+	if (max_count < count) {
+		local_count = max_count;
+	}
+
+	while (i < local_count) {
+		//Get value from buffer
+		value = *(instance->circ_buf.buf + pos);
+		//Prepare it
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+
+		pos++;
+		pos &= instance->circ_buf.mask;
+		i++;
+	}
+
+	status = inl(instance->status_reg);
+	if (!(status & ME6000_AO_STATUS_BIT_FF)) {	//FIFO is full before all datas were copied!
+		PERROR("idx=%d FIFO is full before all datas were copied!\n",
+		       instance->ao_idx);
+		return -ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+	} else {		//Add last value
+		value = *(instance->circ_buf.buf + pos);
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+	}
+
+	PINFO("idx=%d FAST LOADED %d values\n", instance->ao_idx, local_count);
+	return local_count;
+}
+
+/** @brief Copy data from software buffer to fifo (slow).
+* @note This is slow function that copy all data from buffer to FIFO with full control.
+*
+* @param instance The subdevice instance (pointer).
+* @param count Maximum number of copied data.
+* @param start_pos Position of the firs value in buffer.
+*
+* @return On success: Number of copied values.
+* @return On error/success: 0.	FIFO was full at begining.
+* @return On error: -ME_ERRNO_RING_BUFFER_UNDEFFLOW.
+*/
+int inline ao_write_data_pooling(me6000_ao_subdevice_t * instance, int count,
+				 int start_pos)
+{				/// @note This is slow function!
+	uint32_t status;
+	uint32_t value;
+	int pos =
+	    (instance->circ_buf.tail + start_pos) & instance->circ_buf.mask;
+	int local_count = count;
+	int i;
+	int max_count;
+
+	if (count <= 0) {	//Wrong count!
+		PERROR("idx=%d SLOW LOADED: Wrong count!\n", instance->ao_idx);
+		return 0;
+	}
+
+	max_count = me_circ_buf_values(&instance->circ_buf) - start_pos;
+	if (max_count <= 0) {	//No data to copy!
+		PERROR("idx=%d SLOW LOADED: No data to copy!\n",
+		       instance->ao_idx);
+		return 0;
+	}
+
+	if (max_count < count) {
+		local_count = max_count;
+	}
+
+	for (i = 0; i < local_count; i++) {
+		status = inl(instance->status_reg);
+		if (!(status & ME6000_AO_STATUS_BIT_FF)) {	//FIFO is full!
+			return i;
+		}
+		//Get value from buffer
+		value = *(instance->circ_buf.buf + pos);
+		//Prepare it
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+
+		pos++;
+		pos &= instance->circ_buf.mask;
+	}
+
+	PINFO("idx=%d SLOW LOADED %d values\n", instance->ao_idx, local_count);
+	return local_count;
+}
+
+/** @brief Copy data from user space to circular buffer.
+* @param instance The subdevice instance (pointer).
+* @param count Number of datas in user space.
+* @param user_values Buffer's pointer.
+*
+* @return On success: Number of copied values.
+* @return On error: -ME_ERRNO_INTERNAL.
+*/
+int inline ao_get_data_from_user(me6000_ao_subdevice_t * instance, int count,
+				 int *user_values)
+{
+	int i, err;
+	int empty_space;
+	int copied;
+	int value;
+
+	empty_space = me_circ_buf_space(&instance->circ_buf);
+	//We have only this space free.
+	copied = (count < empty_space) ? count : empty_space;
+	for (i = 0; i < copied; i++) {	//Copy from user to buffer
+		if ((err = get_user(value, (int *)(user_values + i)))) {
+			PERROR
+			    ("idx=%d BUFFER LOADED: get_user(0x%p) return an error: %d\n",
+			     instance->ao_idx, user_values + i, err);
+			return -ME_ERRNO_INTERNAL;
+		}
+		/// @note The analog output in me6000 series has size of 16 bits.
+		*(instance->circ_buf.buf + instance->circ_buf.head) =
+		    (uint16_t) value;
+		instance->circ_buf.head++;
+		instance->circ_buf.head &= instance->circ_buf.mask;
+	}
+
+	PINFO("idx=%d BUFFER LOADED %d values\n", instance->ao_idx, copied);
+	return copied;
+}
+
+static void me6000_ao_work_control_task(
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+					       void *subdevice
+#else
+					       struct work_struct *work
+#endif
+    )
+{
+	me6000_ao_subdevice_t *instance;
+	unsigned long cpu_flags = 0;
+	uint32_t status;
+	uint32_t ctrl;
+	uint32_t synch;
+	int reschedule = 0;
+	int signaling = 0;
+	uint32_t single_mask;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	instance = (me6000_ao_subdevice_t *) subdevice;
+#else
+	instance =
+	    container_of((void *)work, me6000_ao_subdevice_t, ao_control_task);
+#endif
+	PINFO("<%s: %ld> executed. idx=%d\n", __func__, jiffies,
+	      instance->ao_idx);
+
+	status = inl(instance->status_reg);
+	PDEBUG_REG("status_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->status_reg - instance->reg_base, status);
+
+/// @note AO_STATUS_BIT_FSM doesn't work as should be for pure single channels (idx>=4)
+//      single_mask = (instance->ao_idx-ME6000_AO_SINGLE_STATUS_OFFSET < 0) ? 0x0000 : (0x0001 << (instance->ao_idx-ME6000_AO_SINGLE_STATUS_OFFSET));
+	single_mask = *instance->triggering_flags & (0x1 << instance->ao_idx);
+
+	switch (instance->status) {	// Checking actual mode.
+
+		// Not configured for work.
+	case ao_status_none:
+		break;
+
+		//This are stable modes. No need to do anything. (?)
+	case ao_status_single_configured:
+	case ao_status_stream_configured:
+	case ao_status_stream_fifo_error:
+	case ao_status_stream_buffer_error:
+	case ao_status_stream_error:
+		PERROR("Shouldn't be running!.\n");
+		break;
+
+		// Single modes
+	case ao_status_single_run_wait:
+	case ao_status_single_run:
+	case ao_status_single_end_wait:
+		if (instance->fifo) {	// Extra registers.
+			if (!(status & ME6000_AO_STATUS_BIT_FSM)) {	// State machine is not working.
+				if (((instance->fifo & ME6000_AO_HAS_FIFO)
+				     && (!(status & ME6000_AO_STATUS_BIT_EF)))
+				    || (!(instance->fifo & ME6000_AO_HAS_FIFO))) {	// Single is in end state.
+					PDEBUG
+					    ("Single call has been complited.\n");
+
+					// Set correct value for single_read();
+					instance->single_value =
+					    instance->single_value_in_fifo;
+
+					// Set status as 'ao_status_single_end'
+					instance->status = ao_status_single_end;
+
+					spin_lock(instance->preload_reg_lock);
+					if ((single_mask) && (*instance->preload_flags & (ME6000_AO_SYNC_HOLD << instance->ao_idx))) {	// This is one of synchronous start channels. Set all as triggered.
+						*instance->triggering_flags =
+						    0x00000000;
+					} else {
+						//Set this channel as triggered (none active).
+						*instance->triggering_flags &=
+						    ~(0x1 << instance->ao_idx);
+					}
+					spin_unlock(instance->preload_reg_lock);
+
+					// Signal the end.
+					signaling = 1;
+					// Wait for stop ISM.
+					reschedule = 1;
+
+					break;
+				}
+			}
+			// Check timeout.
+			if ((instance->timeout.delay) && ((jiffies - instance->timeout.start_time) >= instance->timeout.delay)) {	// Timeout
+				PDEBUG("Timeout reached.\n");
+				// Stop all actions. No conditions! Block interrupts and trigger. Leave FIFO untouched!
+				spin_lock_irqsave(&instance->subdevice_lock,
+						  cpu_flags);
+				ctrl = inl(instance->ctrl_reg);
+				ctrl |=
+				    ME6000_AO_CTRL_BIT_STOP |
+				    ME6000_AO_CTRL_BIT_IMMEDIATE_STOP;
+				ctrl &=
+				    ~(ME6000_AO_CTRL_BIT_ENABLE_IRQ |
+				      ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG);
+				ctrl &=
+				    ~(ME6000_AO_CTRL_BIT_EX_TRIG_EDGE |
+				      ME6000_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH);
+				//Disabling FIFO
+				ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_FIFO;
+
+				outl(ctrl, instance->ctrl_reg);
+				PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->ctrl_reg -
+					   instance->reg_base, ctrl);
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+
+				//Reset interrupt latch
+				inl(instance->irq_reset_reg);
+
+				spin_lock(instance->preload_reg_lock);
+				//Remove from synchronous start. Block triggering from this output.
+				synch = inl(instance->preload_reg);
+				synch &=
+				    ~((ME6000_AO_SYNC_HOLD |
+				       ME6000_AO_SYNC_EXT_TRIG) << instance->
+				      ao_idx);
+				if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {	// No FIFO - set to single safe mode
+					synch |=
+					    ME6000_AO_SYNC_HOLD << instance->
+					    ao_idx;
+				}
+				outl(synch, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     synch);
+				//Set this channel as triggered (none active).
+				*instance->triggering_flags &=
+				    ~(0x1 << instance->ao_idx);
+				spin_unlock(instance->preload_reg_lock);
+
+				// Set correct value for single_read();
+				instance->single_value_in_fifo =
+				    instance->single_value;
+
+				instance->status = ao_status_single_end;
+
+				// Signal the end.
+				signaling = 1;
+			}
+		} else {	// No extra registers.
+/*
+				if (!(status & single_mask))
+				{// State machine is not working.
+					PDEBUG("Single call has been complited.\n");
+
+					// Set correct value for single_read();
+					instance->single_value = instance->single_value_in_fifo;
+
+					// Set status as 'ao_status_single_end'
+					instance->status = ao_status_single_end;
+
+					// Signal the end.
+					signaling = 1;
+					// Wait for stop ISM.
+					reschedule = 1;
+
+					break;
+				}
+*/
+			if (!single_mask) {	// Was triggered.
+				PDEBUG("Single call has been complited.\n");
+
+				// Set correct value for single_read();
+				instance->single_value =
+				    instance->single_value_in_fifo;
+
+				// Set status as 'ao_status_single_end'
+				instance->status = ao_status_single_end;
+
+				// Signal the end.
+				signaling = 1;
+
+				break;
+			}
+			// Check timeout.
+			if ((instance->timeout.delay) && ((jiffies - instance->timeout.start_time) >= instance->timeout.delay)) {	// Timeout
+				PDEBUG("Timeout reached.\n");
+
+				spin_lock(instance->preload_reg_lock);
+				//Remove from synchronous start. Block triggering from this output.
+				synch = inl(instance->preload_reg);
+				synch &=
+				    ~(ME6000_AO_SYNC_EXT_TRIG << instance->
+				      ao_idx);
+				synch |=
+				    ME6000_AO_SYNC_HOLD << instance->ao_idx;
+
+				outl(synch, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     synch);
+				//Set this channel as triggered (none active).
+				*instance->triggering_flags &=
+				    ~(0x1 << instance->ao_idx);
+				spin_unlock(instance->preload_reg_lock);
+
+				// Restore old settings.
+				PDEBUG("Write old value back to register.\n");
+				outl(instance->single_value,
+				     instance->single_reg);
+				PDEBUG_REG
+				    ("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->single_reg - instance->reg_base,
+				     instance->single_value);
+
+				// Set correct value for single_read();
+				instance->single_value_in_fifo =
+				    instance->single_value;
+
+				instance->status = ao_status_single_end;
+
+				// Signal the end.
+				signaling = 1;
+			}
+		}
+
+		// Wait for stop.
+		reschedule = 1;
+		break;
+
+	case ao_status_stream_end:
+		if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {	// No FIFO
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+	case ao_status_single_end:
+		if (instance->fifo) {	// Extra registers.
+			if (status & ME6000_AO_STATUS_BIT_FSM) {	// State machine is working but the status is set to end. Force stop.
+
+				// Wait for stop.
+				reschedule = 1;
+			}
+
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			// Stop all actions. No conditions! Block interrupts and trigger. Leave FIFO untouched!
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |=
+			    ME6000_AO_CTRL_BIT_IMMEDIATE_STOP |
+			    ME6000_AO_CTRL_BIT_STOP;
+			ctrl &=
+			    ~(ME6000_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG);
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			//Reset interrupt latch
+			inl(instance->irq_reset_reg);
+		} else {	// No extra registers.
+/*
+				if (status & single_mask)
+				{// State machine is working but the status is set to end. Force stop.
+
+					// Wait for stop.
+					reschedule = 1;
+				}
+*/
+		}
+		break;
+
+		// Stream modes
+	case ao_status_stream_run_wait:
+		if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {	// No FIFO
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+
+		if (status & ME6000_AO_STATUS_BIT_FSM) {	// State machine is working. Waiting for start finish.
+			instance->status = ao_status_stream_run;
+
+			// Signal end of this step
+			signaling = 1;
+		} else {	// State machine is not working.
+			if (!(status & ME6000_AO_STATUS_BIT_EF)) {	// FIFO is empty. Procedure has started and finish already!
+				instance->status = ao_status_stream_end;
+
+				// Signal the end.
+				signaling = 1;
+				// Wait for stop.
+				reschedule = 1;
+				break;
+			}
+		}
+
+		// Check timeout.
+		if ((instance->timeout.delay) && ((jiffies - instance->timeout.start_time) >= instance->timeout.delay)) {	// Timeout
+			PDEBUG("Timeout reached.\n");
+			// Stop all actions. No conditions! Block interrupts. Leave FIFO untouched!
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |=
+			    ME6000_AO_CTRL_BIT_STOP |
+			    ME6000_AO_CTRL_BIT_IMMEDIATE_STOP;
+			ctrl &=
+			    ~(ME6000_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG);
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			//Reset interrupt latch
+			inl(instance->irq_reset_reg);
+
+			spin_lock(instance->preload_reg_lock);
+			//Remove from synchronous start. Block triggering from this output.
+			synch = inl(instance->preload_reg);
+			synch &=
+			    ~((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) <<
+			      instance->ao_idx);
+			outl(synch, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   synch);
+			spin_unlock(instance->preload_reg_lock);
+
+			instance->status = ao_status_stream_end;
+
+			// Signal the end.
+			signaling = 1;
+		}
+		// Wait for stop.
+		reschedule = 1;
+		break;
+
+	case ao_status_stream_run:
+		if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {	// No FIFO
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+
+		if (!(status & ME6000_AO_STATUS_BIT_FSM)) {	// State machine is not working. This is an error.
+			// BROKEN PIPE!
+			if (!(status & ME6000_AO_STATUS_BIT_EF)) {	// FIFO is empty.
+				if (me_circ_buf_values(&instance->circ_buf)) {	// Software buffer is not empty.
+					if (instance->stop_data_count && (instance->stop_data_count <= instance->data_count)) {	//Finishing work. Requed data shown.
+						PDEBUG
+						    ("ISM stoped. No data in FIFO. Buffer is not empty.\n");
+						instance->status =
+						    ao_status_stream_end;
+					} else {
+						PERROR
+						    ("Output stream has been broken. ISM stoped. No data in FIFO. Buffer is not empty.\n");
+						instance->status =
+						    ao_status_stream_buffer_error;
+					}
+				} else {	// Software buffer is empty.
+					PDEBUG
+					    ("ISM stoped. No data in FIFO. Buffer is empty.\n");
+					instance->status = ao_status_stream_end;
+				}
+			} else {	// There are still datas in FIFO.
+				if (me_circ_buf_values(&instance->circ_buf)) {	// Software buffer is not empty.
+					PERROR
+					    ("Output stream has been broken. ISM stoped but some data in FIFO and buffer.\n");
+				} else {	// Software buffer is empty.
+					PERROR
+					    ("Output stream has been broken. ISM stoped but some data in FIFO. Buffer is empty.\n");
+				}
+				instance->status = ao_status_stream_fifo_error;
+
+			}
+
+			// Signal the failure.
+			signaling = 1;
+			break;
+		}
+		// Wait for stop.
+		reschedule = 1;
+		break;
+
+	case ao_status_stream_end_wait:
+		if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {	// No FIFO
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+
+		if (!(status & ME6000_AO_STATUS_BIT_FSM)) {	// State machine is not working. Waiting for stop finish.
+			instance->status = ao_status_stream_end;
+			signaling = 1;
+		}
+		// State machine is working.
+		reschedule = 1;
+		break;
+
+	default:
+		PERROR_CRITICAL("Status is in wrong state (%d)!\n",
+				instance->status);
+		instance->status = ao_status_stream_error;
+		// Signal the end.
+		signaling = 1;
+		break;
+
+	}
+
+	if (signaling) {	//Signal it.
+		wake_up_interruptible_all(&instance->wait_queue);
+	}
+
+	if (instance->ao_control_task_flag && reschedule) {	// Reschedule task
+		queue_delayed_work(instance->me6000_workqueue,
+				   &instance->ao_control_task, 1);
+	} else {
+		PINFO("<%s> Ending control task.\n", __func__);
+	}
+
+}
+
+static int me6000_ao_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range)
+{
+	me6000_ao_subdevice_t *instance;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if ((*max - *min) < 0) {
+		PERROR("Invalid minimum and maximum values specified.\n");
+		return ME_ERRNO_INVALID_MIN_MAX;
+	}
+
+	if ((unit == ME_UNIT_VOLT) || (unit == ME_UNIT_ANY)) {
+		if ((*max <= (instance->max + 1000)) && (*min >= instance->min)) {
+			*min = instance->min;
+			*max = instance->max;
+			*maxdata = ME6000_AO_MAX_DATA;
+			*range = 0;
+		} else {
+			PERROR("No matching range available.\n");
+			return ME_ERRNO_NO_RANGE;
+		}
+	} else {
+		PERROR("Invalid physical unit specified.\n");
+		return ME_ERRNO_INVALID_UNIT;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_ao_query_number_ranges(me_subdevice_t * subdevice,
+					 int unit, int *count)
+{
+	me6000_ao_subdevice_t *instance;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if ((unit == ME_UNIT_VOLT) || (unit == ME_UNIT_ANY)) {
+		*count = 1;
+	} else {
+		*count = 0;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_ao_query_range_info(me_subdevice_t * subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata)
+{
+	me6000_ao_subdevice_t *instance;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (range == 0) {
+		*unit = ME_UNIT_VOLT;
+		*min = instance->min;
+		*max = instance->max;
+		*maxdata = ME6000_AO_MAX_DATA;
+	} else {
+		PERROR("Invalid range number specified.\n");
+		return ME_ERRNO_INVALID_RANGE;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_ao_query_timer(me_subdevice_t * subdevice,
+				 int timer,
+				 int *base_frequency,
+				 long long *min_ticks, long long *max_ticks)
+{
+	me6000_ao_subdevice_t *instance;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (instance->fifo) {	//Streaming device.
+		*base_frequency = ME6000_AO_BASE_FREQUENCY;
+		if (timer == ME_TIMER_ACQ_START) {
+			*min_ticks = ME6000_AO_MIN_ACQ_TICKS;
+			*max_ticks = ME6000_AO_MAX_ACQ_TICKS;
+		} else if (timer == ME_TIMER_CONV_START) {
+			*min_ticks = ME6000_AO_MIN_CHAN_TICKS;
+			*max_ticks = ME6000_AO_MAX_CHAN_TICKS;
+		}
+	} else {		//Not streaming device!
+		*base_frequency = 0;
+		*min_ticks = 0;
+		*max_ticks = 0;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_ao_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	me6000_ao_subdevice_t *instance;
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*number = 1;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_ao_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	me6000_ao_subdevice_t *instance;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*type = ME_TYPE_AO;
+	*subtype =
+	    (instance->
+	     fifo & ME6000_AO_HAS_FIFO) ? ME_SUBTYPE_STREAMING :
+	    ME_SUBTYPE_SINGLE;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_ao_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	me6000_ao_subdevice_t *instance;
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*caps =
+	    ME_CAPS_AO_TRIG_SYNCHRONOUS | ((instance->fifo) ? ME_CAPS_AO_FIFO :
+					   ME_CAPS_NONE);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_ao_query_subdevice_caps_args(struct me_subdevice *subdevice,
+					       int cap, int *args, int count)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (count != 1) {
+		PERROR("Invalid capability argument count.\n");
+		return ME_ERRNO_INVALID_CAP_ARG_COUNT;
+	}
+
+	switch (cap) {
+	case ME_CAP_AI_FIFO_SIZE:
+		args[0] = (instance->fifo) ? ME6000_AO_FIFO_COUNT : 0;
+		break;
+
+	case ME_CAP_AI_BUFFER_SIZE:
+		args[0] =
+		    (instance->circ_buf.buf) ? ME6000_AO_CIRC_BUF_COUNT : 0;
+		break;
+
+	default:
+		PERROR("Invalid capability.\n");
+		err = ME_ERRNO_INVALID_CAP;
+		args[0] = 0;
+	}
+
+	return err;
+}
diff --git a/drivers/staging/meilhaus/me6000_ao.h b/drivers/staging/meilhaus/me6000_ao.h
new file mode 100644
index 000000000000..9629649cd410
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_ao.h
@@ -0,0 +1,200 @@
+/**
+ * @file me6000_ao.h
+ *
+ * @brief Meilhaus ME-6000 analog output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME6000_AO_H_
+#define _ME6000_AO_H_
+
+#include <linux/version.h>
+#include "mesubdevice.h"
+#include "mecirc_buf.h"
+#include "meioctl.h"
+
+#ifdef __KERNEL__
+
+#define ME6000_AO_MAX_SUBDEVICES	16
+#define ME6000_AO_FIFO_COUNT		8192
+
+#define ME6000_AO_BASE_FREQUENCY	33000000L
+
+#define ME6000_AO_MIN_ACQ_TICKS		0LL
+#define ME6000_AO_MAX_ACQ_TICKS		0LL
+
+#define ME6000_AO_MIN_CHAN_TICKS	66LL
+#define ME6000_AO_MAX_CHAN_TICKS	0xFFFFFFFFLL
+
+#define ME6000_AO_MIN_RANGE			-10000000
+#define ME6000_AO_MAX_RANGE			9999694
+
+#define ME6000_AO_MIN_RANGE_HIGH	0
+#define ME6000_AO_MAX_RANGE_HIGH	49999237
+
+#define ME6000_AO_MAX_DATA			0xFFFF
+
+#ifdef ME_SYNAPSE
+# define ME6000_AO_CIRC_BUF_SIZE_ORDER 		8	// 2^n PAGES =>> Maximum value of 1MB for Synapse
+#else
+# define ME6000_AO_CIRC_BUF_SIZE_ORDER 		5	// 2^n PAGES =>> 128KB
+#endif
+#define ME6000_AO_CIRC_BUF_SIZE 		PAGE_SIZE<<ME6000_AO_CIRC_BUF_SIZE_ORDER	// Buffer size in bytes.
+
+#  ifdef _CBUFF_32b_t
+#   define ME6000_AO_CIRC_BUF_COUNT	((ME6000_AO_CIRC_BUF_SIZE) / sizeof(uint32_t))	// Size in values
+#  else
+#   define ME6000_AO_CIRC_BUF_COUNT	((ME6000_AO_CIRC_BUF_SIZE) / sizeof(uint16_t))	// Size in values
+#  endif
+
+#  define ME6000_AO_CONTINOUS					0x0
+#  define ME6000_AO_WRAP_MODE					0x1
+#  define ME6000_AO_HW_MODE						0x2
+
+#  define ME6000_AO_HW_WRAP_MODE				(ME6000_AO_WRAP_MODE | ME6000_AO_HW_MODE)
+#  define ME6000_AO_SW_WRAP_MODE				ME6000_AO_WRAP_MODE
+
+#  define ME6000_AO_INF_STOP_MODE				0x0
+#  define ME6000_AO_ACQ_STOP_MODE				0x1
+#  define ME6000_AO_SCAN_STOP_MODE				0x2
+
+#  define ME6000_AO_EXTRA_HARDWARE				0x1
+#  define ME6000_AO_HAS_FIFO					0x2
+
+typedef enum ME6000_AO_STATUS {
+	ao_status_none = 0,
+	ao_status_single_configured,
+	ao_status_single_run_wait,
+	ao_status_single_run,
+	ao_status_single_end_wait,
+	ao_status_single_end,
+	ao_status_stream_configured,
+	ao_status_stream_run_wait,
+	ao_status_stream_run,
+	ao_status_stream_end_wait,
+	ao_status_stream_end,
+	ao_status_stream_fifo_error,
+	ao_status_stream_buffer_error,
+	ao_status_stream_error,
+	ao_status_last
+} ME6000_AO_STATUS;
+
+typedef struct me6000_ao_timeout {
+	unsigned long start_time;
+	unsigned long delay;
+} me6000_ao_timeout_t;
+
+/**
+ * @brief The ME-6000 analog output subdevice class.
+ */
+typedef struct me6000_ao_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;					/**< The subdevice base class. */
+	unsigned int ao_idx;
+
+	/* Attributes */
+	spinlock_t subdevice_lock;				/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *preload_reg_lock;			/**< Spin lock to protect preload_reg from concurrent access. */
+
+	uint32_t *preload_flags;
+	uint32_t *triggering_flags;
+
+	/* Hardware feautres */
+	unsigned int irq;						/**< The interrupt request number assigned by the PCI BIOS. */
+	int fifo;								/**< If set this device has a FIFO. */
+
+	//Range
+	int min;
+	int max;
+
+	int single_value;						/**< Mirror of the output value in single mode. */
+	int single_value_in_fifo;				/**< Mirror of the value written in single mode. */
+	uint32_t ctrl_trg;						/**< Mirror of the trigger settings. */
+
+	volatile int mode;						/**< Flags used for storing SW wraparound setup*/
+	int stop_mode;							/**< The user defined stop condition flag. */
+	unsigned int start_mode;
+	unsigned int stop_count;				/**< The user defined dates presentation end count. */
+	unsigned int stop_data_count;			/**< The stop presentation count. */
+	unsigned int data_count;				/**< The real presentation count. */
+	unsigned int preloaded_count;			/**< The next data addres in buffer. <= for wraparound mode. */
+	int hardware_stop_delay;				/**< The time that stop can take. This is only to not show hardware bug to user. */
+
+	volatile enum ME6000_AO_STATUS status;	/**< The current stream status flag. */
+	me6000_ao_timeout_t timeout;			/**< The timeout for start in blocking and non-blocking mode. */
+
+									/* Registers *//**< All registers are 32 bits long. */
+	unsigned long ctrl_reg;
+	unsigned long status_reg;
+	unsigned long fifo_reg;
+	unsigned long single_reg;
+	unsigned long timer_reg;
+	unsigned long irq_status_reg;
+	unsigned long preload_reg;
+	unsigned long irq_reset_reg;
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+
+	/* Software buffer */
+	me_circ_buf_t circ_buf;					/**< Circular buffer holding measurment data. */
+	wait_queue_head_t wait_queue;			/**< Wait queue to put on tasks waiting for data to arrive. */
+
+	struct workqueue_struct *me6000_workqueue;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	struct work_struct ao_control_task;
+#else
+	struct delayed_work ao_control_task;
+#endif
+
+	volatile int ao_control_task_flag;		/**< Flag controling reexecuting of control task */
+
+} me6000_ao_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-6000 analog output subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param ctrl_reg_lock Pointer to spin lock protecting the control register from concurrent access.
+ * @param preload_flags Pointer to spin lock protecting the hold&trigger register from concurrent access.
+ * @param ao_idx Subdevice number.
+ * @param fifo Flag set if subdevice has hardware FIFO.
+ * @param irq IRQ number.
+ * @param high_range Flag set if subdevice has high curren output.
+ * @param me6000_wq Queue for asynchronous task (1 queue for all subdevice on 1 board).
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me6000_ao_subdevice_t *me6000_ao_constructor(uint32_t reg_base,
+					     spinlock_t * preload_reg_lock,
+					     uint32_t * preload_flags,
+					     uint32_t * triggering_flags,
+					     int ao_idx,
+					     int fifo,
+					     int irq,
+					     int high_range,
+					     struct workqueue_struct
+					     *me6000_wq);
+
+#endif //__KERNEL__
+#endif //_ME6000_AO_H_
diff --git a/drivers/staging/meilhaus/me6000_ao_reg.h b/drivers/staging/meilhaus/me6000_ao_reg.h
new file mode 100644
index 000000000000..eb8f46e1b75b
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_ao_reg.h
@@ -0,0 +1,177 @@
+/**
+ * @file me6000_ao_reg.h
+ *
+ * @brief ME-6000 analog output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME6000_AO_REG_H_
+#define _ME6000_AO_REG_H_
+
+#ifdef __KERNEL__
+
+// AO
+#define ME6000_AO_00_CTRL_REG					0x00	// R/W
+#define ME6000_AO_00_STATUS_REG					0x04	// R/_
+#define ME6000_AO_00_FIFO_REG					0x08	// _/W
+#define ME6000_AO_00_SINGLE_REG					0x0C	// R/W
+#define ME6000_AO_00_TIMER_REG					0x10	// _/W
+
+#define ME6000_AO_01_CTRL_REG					0x18	// R/W
+#define ME6000_AO_01_STATUS_REG					0x1C	// R/_
+#define ME6000_AO_01_FIFO_REG					0x20	// _/W
+#define ME6000_AO_01_SINGLE_REG					0x24	// R/W
+#define ME6000_AO_01_TIMER_REG					0x28	// _/W
+
+#define ME6000_AO_02_CTRL_REG					0x30	// R/W
+#define ME6000_AO_02_STATUS_REG					0x34	// R/_
+#define ME6000_AO_02_FIFO_REG					0x38	// _/W
+#define ME6000_AO_02_SINGLE_REG					0x3C	// R/W
+#define ME6000_AO_02_TIMER_REG					0x40	// _/W
+
+#define ME6000_AO_03_CTRL_REG					0x48	// R/W
+#define ME6000_AO_03_STATUS_REG					0x4C	// R/_
+#define ME6000_AO_03_FIFO_REG					0x50	// _/W
+#define ME6000_AO_03_SINGLE_REG					0x54	// R/W
+#define ME6000_AO_03_TIMER_REG					0x58	// _/W
+
+#define ME6000_AO_SINGLE_STATUS_REG				0xA4	// R/_
+#define ME6000_AO_SINGLE_STATUS_OFFSET			4	//The first single subdevice => bit 0 in ME6000_AO_SINGLE_STATUS_REG.
+
+#define ME6000_AO_04_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_04_SINGLE_REG					0x74	// _/W
+
+#define ME6000_AO_05_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_05_SINGLE_REG					0x78	// _/W
+
+#define ME6000_AO_06_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_06_SINGLE_REG					0x7C	// _/W
+
+#define ME6000_AO_07_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_07_SINGLE_REG					0x80	// _/W
+
+#define ME6000_AO_08_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_08_SINGLE_REG					0x84	// _/W
+
+#define ME6000_AO_09_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_09_SINGLE_REG					0x88	// _/W
+
+#define ME6000_AO_10_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_10_SINGLE_REG					0x8C	// _/W
+
+#define ME6000_AO_11_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_11_SINGLE_REG					0x90	// _/W
+
+#define ME6000_AO_12_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_12_SINGLE_REG					0x94	// _/W
+
+#define ME6000_AO_13_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_13_SINGLE_REG					0x98	// _/W
+
+#define ME6000_AO_14_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_14_SINGLE_REG					0x9C	// _/W
+
+#define ME6000_AO_15_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_15_SINGLE_REG					0xA0	// _/W
+
+//ME6000_AO_CTRL_REG
+#define ME6000_AO_MODE_SINGLE					0x00
+#define ME6000_AO_MODE_WRAPAROUND				0x01
+#define ME6000_AO_MODE_CONTINUOUS				0x02
+#define ME6000_AO_CTRL_MODE_MASK				(ME6000_AO_MODE_WRAPAROUND | ME6000_AO_MODE_CONTINUOUS)
+
+#define ME6000_AO_CTRL_BIT_MODE_WRAPAROUND		0x001
+#define ME6000_AO_CTRL_BIT_MODE_CONTINUOUS		0x002
+#define ME6000_AO_CTRL_BIT_STOP					0x004
+#define ME6000_AO_CTRL_BIT_ENABLE_FIFO			0x008
+#define ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG		0x010
+#define ME6000_AO_CTRL_BIT_EX_TRIG_EDGE			0x020
+#define ME6000_AO_CTRL_BIT_ENABLE_IRQ			0x040
+#define ME6000_AO_CTRL_BIT_IMMEDIATE_STOP		0x080
+#define ME6000_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH 	0x800
+
+//ME6000_AO_STATUS_REG
+#define ME6000_AO_STATUS_BIT_FSM				0x01
+#define ME6000_AO_STATUS_BIT_FF					0x02
+#define ME6000_AO_STATUS_BIT_HF					0x04
+#define ME6000_AO_STATUS_BIT_EF					0x08
+
+#define ME6000_AO_PRELOAD_REG					0xA8	// R/W    ///ME6000_AO_SYNC_REG <==> ME6000_AO_PRELOAD_REG
+/*
+#define ME6000_AO_SYNC_HOLD_0					0x00000001
+#define ME6000_AO_SYNC_HOLD_1					0x00000002
+#define ME6000_AO_SYNC_HOLD_2					0x00000004
+#define ME6000_AO_SYNC_HOLD_3					0x00000008
+#define ME6000_AO_SYNC_HOLD_4					0x00000010
+#define ME6000_AO_SYNC_HOLD_5					0x00000020
+#define ME6000_AO_SYNC_HOLD_6					0x00000040
+#define ME6000_AO_SYNC_HOLD_7					0x00000080
+#define ME6000_AO_SYNC_HOLD_8					0x00000100
+#define ME6000_AO_SYNC_HOLD_9					0x00000200
+#define ME6000_AO_SYNC_HOLD_10					0x00000400
+#define ME6000_AO_SYNC_HOLD_11					0x00000800
+#define ME6000_AO_SYNC_HOLD_12					0x00001000
+#define ME6000_AO_SYNC_HOLD_13					0x00002000
+#define ME6000_AO_SYNC_HOLD_14					0x00004000
+#define ME6000_AO_SYNC_HOLD_15					0x00008000
+*/
+#define ME6000_AO_SYNC_HOLD						0x00000001
+/*
+#define ME6000_AO_SYNC_EXT_TRIG_0				0x00010000
+#define ME6000_AO_SYNC_EXT_TRIG_1				0x00020000
+#define ME6000_AO_SYNC_EXT_TRIG_2				0x00040000
+#define ME6000_AO_SYNC_EXT_TRIG_3				0x00080000
+#define ME6000_AO_SYNC_EXT_TRIG_4				0x00100000
+#define ME6000_AO_SYNC_EXT_TRIG_5				0x00200000
+#define ME6000_AO_SYNC_EXT_TRIG_6				0x00400000
+#define ME6000_AO_SYNC_EXT_TRIG_7				0x00800000
+#define ME6000_AO_SYNC_EXT_TRIG_8				0x01000000
+#define ME6000_AO_SYNC_EXT_TRIG_9				0x02000000
+#define ME6000_AO_SYNC_EXT_TRIG_10				0x04000000
+#define ME6000_AO_SYNC_EXT_TRIG_11				0x08000000
+#define ME6000_AO_SYNC_EXT_TRIG_12				0x10000000
+#define ME6000_AO_SYNC_EXT_TRIG_13				0x20000000
+#define ME6000_AO_SYNC_EXT_TRIG_14				0x40000000
+#define ME6000_AO_SYNC_EXT_TRIG_15				0x80000000
+*/
+#define ME6000_AO_SYNC_EXT_TRIG					0x00010000
+
+#define ME6000_AO_EXT_TRIG						0x80000000
+
+// AO-IRQ
+#define ME6000_AO_IRQ_STATUS_REG				0x60	// R/_
+#define ME6000_AO_00_IRQ_RESET_REG				0x64	// R/_
+#define ME6000_AO_01_IRQ_RESET_REG				0x68	// R/_
+#define ME6000_AO_02_IRQ_RESET_REG				0x6C	// R/_
+#define ME6000_AO_03_IRQ_RESET_REG				0x70	// R/_
+
+#define ME6000_IRQ_STATUS_BIT_0					0x01
+#define ME6000_IRQ_STATUS_BIT_1					0x02
+#define ME6000_IRQ_STATUS_BIT_2					0x04
+#define ME6000_IRQ_STATUS_BIT_3					0x08
+
+#define ME6000_IRQ_STATUS_BIT_AO_HF				ME6000_IRQ_STATUS_BIT_0
+
+//DUMY register
+#define ME6000_AO_DUMY									0xFC
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me6000_device.c b/drivers/staging/meilhaus/me6000_device.c
new file mode 100644
index 000000000000..fee4c58b8464
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_device.c
@@ -0,0 +1,211 @@
+/**
+ * @file me6000_device.c
+ *
+ * @brief Device class template implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "mefirmware.h"
+
+#include "mesubdevice.h"
+#include "medebug.h"
+#include "medevice.h"
+#include "me6000_reg.h"
+#include "me6000_device.h"
+#include "meplx_reg.h"
+#include "me6000_dio.h"
+#include "me6000_ao.h"
+
+/**
+ * @brief Global variable.
+ * This is working queue for runing a separate atask that will be responsible for work status (start, stop, timeouts).
+ */
+static struct workqueue_struct *me6000_workqueue;
+
+me_device_t *me6000_pci_constructor(struct pci_dev *pci_device)
+{
+	me6000_device_t *me6000_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	int err;
+	int i;
+	int high_range = 0;
+	int fifo;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me6000_device = kmalloc(sizeof(me6000_device_t), GFP_KERNEL);
+
+	if (!me6000_device) {
+		PERROR("Cannot get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(me6000_device, 0, sizeof(me6000_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me6000_device, pci_device);
+
+	if (err) {
+		kfree(me6000_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+
+	/* Download the xilinx firmware */
+	err = me_xilinx_download(me6000_device->base.info.pci.reg_bases[1],
+				 me6000_device->base.info.pci.reg_bases[2],
+				 &pci_device->dev, "me6000.bin");
+
+	if (err) {
+		me_device_deinit((me_device_t *) me6000_device);
+		kfree(me6000_device);
+		PERROR("Can't download firmware.\n");
+		return NULL;
+	}
+
+	/* Get the index in the device version information table. */
+	version_idx =
+	    me6000_versions_get_device_index(me6000_device->base.info.pci.
+					     device_id);
+
+	// Initialize spin lock .
+	spin_lock_init(&me6000_device->preload_reg_lock);
+	spin_lock_init(&me6000_device->dio_ctrl_reg_lock);
+
+	/* Create digital input/output instances. */
+	for (i = 0; i < me6000_versions[version_idx].dio_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me6000_dio_constructor(me6000_device->
+							      base.info.pci.
+							      reg_bases[3], i,
+							      &me6000_device->
+							      dio_ctrl_reg_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me6000_device);
+			kfree(me6000_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me6000_device->base.slist,
+					    subdevice);
+	}
+
+	/* Create analog output instances. */
+	for (i = 0; i < me6000_versions[version_idx].ao_subdevices; i++) {
+		high_range = ((i == 8)
+			      &&
+			      ((me6000_device->base.info.pci.device_id ==
+				PCI_DEVICE_ID_MEILHAUS_ME6359)
+			       || (me6000_device->base.info.pci.device_id ==
+				   PCI_DEVICE_ID_MEILHAUS_ME6259)
+			      )
+		    )? 1 : 0;
+
+		fifo =
+		    (i <
+		     me6000_versions[version_idx].
+		     ao_fifo) ? ME6000_AO_HAS_FIFO : 0x0;
+		fifo |= (i < 4) ? ME6000_AO_EXTRA_HARDWARE : 0x0;
+
+		subdevice =
+		    (me_subdevice_t *) me6000_ao_constructor(me6000_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     &me6000_device->
+							     preload_reg_lock,
+							     &me6000_device->
+							     preload_flags,
+							     &me6000_device->
+							     triggering_flags,
+							     i, fifo,
+							     me6000_device->
+							     base.irq,
+							     high_range,
+							     me6000_workqueue);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me6000_device);
+			kfree(me6000_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me6000_device->base.slist,
+					    subdevice);
+	}
+
+	return (me_device_t *) me6000_device;
+}
+
+// Init and exit of module.
+
+static int __init me6000_init(void)
+{
+	PDEBUG("executed.\n");
+
+	me6000_workqueue = create_singlethread_workqueue("me6000");
+	return 0;
+}
+
+static void __exit me6000_exit(void)
+{
+	PDEBUG("executed.\n");
+
+	flush_workqueue(me6000_workqueue);
+	destroy_workqueue(me6000_workqueue);
+}
+
+module_init(me6000_init);
+module_exit(me6000_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for ME-6000 Device");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-6000 Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me6000_pci_constructor);
diff --git a/drivers/staging/meilhaus/me6000_device.h b/drivers/staging/meilhaus/me6000_device.h
new file mode 100644
index 000000000000..18cc7d1e14f1
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_device.h
@@ -0,0 +1,149 @@
+/**
+ * @file me6000_device.h
+ *
+ * @brief ME-6000 device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME6000_DEVICE_H
+#define _ME6000_DEVICE_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure holding ME-6000 device capabilities.
+ */
+typedef struct me6000_version {
+	uint16_t device_id;
+	unsigned int dio_subdevices;
+	unsigned int ao_subdevices;
+	unsigned int ao_fifo;	//How many devices have FIFO
+} me6000_version_t;
+
+/**
+ * @brief ME-6000 device capabilities.
+ */
+static me6000_version_t me6000_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME6004, 0, 4, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME6008, 0, 8, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME600F, 0, 16, 0},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6014, 0, 4, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME6018, 0, 8, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME601F, 0, 16, 0},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6034, 0, 4, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME6038, 0, 8, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME603F, 0, 16, 0},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6104, 0, 4, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME6108, 0, 8, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME610F, 0, 16, 4},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6114, 0, 4, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME6118, 0, 8, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME611F, 0, 16, 4},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6134, 0, 4, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME6138, 0, 8, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME613F, 0, 16, 4},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6044, 2, 4, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME6048, 2, 8, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME604F, 2, 16, 0},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6054, 2, 4, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME6058, 2, 8, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME605F, 2, 16, 0},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6074, 2, 4, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME6078, 2, 8, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME607F, 2, 16, 0},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6144, 2, 4, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME6148, 2, 8, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME614F, 2, 16, 4},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6154, 2, 4, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME6158, 2, 8, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME615F, 2, 16, 4},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6174, 2, 4, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME6178, 2, 8, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME617F, 2, 16, 4},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6259, 2, 9, 0},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6359, 2, 9, 4},
+
+	{0},
+};
+
+#define ME6000_DEVICE_VERSIONS (sizeof(me6000_versions) / sizeof(me6000_version_t) - 1)	/**< Returns the number of entries in #me6000_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #me6000_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me6000_versions.
+ */
+static inline unsigned int me6000_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME6000_DEVICE_VERSIONS; i++)
+		if (me6000_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The ME-6000 device class structure.
+ */
+typedef struct me6000_device {
+	me_device_t base;			/**< The Meilhaus device base class. */
+
+	/* Child class attributes. */
+	spinlock_t preload_reg_lock;		/**< Guards the preload register. */
+	uint32_t preload_flags;
+	uint32_t triggering_flags;
+
+	spinlock_t dio_ctrl_reg_lock;
+} me6000_device_t;
+
+/**
+ * @brief The ME-6000 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-6000 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me6000_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me6000_dio.c b/drivers/staging/meilhaus/me6000_dio.c
new file mode 100644
index 000000000000..07f1069f9ac6
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_dio.c
@@ -0,0 +1,415 @@
+/**
+ * @file me6000_dio.c
+ *
+ * @brief ME-6000 digital input/output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me6000_dio_reg.h"
+#include "me6000_dio.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me6000_dio_io_reset_subdevice(struct me_subdevice *subdevice,
+					 struct file *filep, int flags)
+{
+	me6000_dio_subdevice_t *instance;
+	uint8_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me6000_dio_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inb(instance->ctrl_reg);
+	mode &= ~(0x3 << (instance->dio_idx * 2));
+	outb(mode, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, mode);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outb(0x00, instance->port_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, 0x00);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_dio_io_single_config(me_subdevice_t * subdevice,
+				       struct file *filep,
+				       int channel,
+				       int single_config,
+				       int ref,
+				       int trig_chan,
+				       int trig_type, int trig_edge, int flags)
+{
+	me6000_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+	int size =
+	    flags & (ME_IO_SINGLE_CONFIG_DIO_BIT | ME_IO_SINGLE_CONFIG_DIO_BYTE
+		     | ME_IO_SINGLE_CONFIG_DIO_WORD |
+		     ME_IO_SINGLE_CONFIG_DIO_DWORD);
+
+	PDEBUG("executed.\n");
+
+	instance = (me6000_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inb(instance->ctrl_reg);
+	switch (size) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+				mode &=
+				    ~((ME6000_DIO_CTRL_BIT_MODE_0 |
+				       ME6000_DIO_CTRL_BIT_MODE_1) <<
+				      (instance->dio_idx * 2));
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+				mode &=
+				    ~((ME6000_DIO_CTRL_BIT_MODE_0 |
+				       ME6000_DIO_CTRL_BIT_MODE_1) <<
+				      (instance->dio_idx * 2));
+				mode |=
+				    ME6000_DIO_CTRL_BIT_MODE_0 << (instance->
+								   dio_idx * 2);
+			} else {
+				PERROR
+				    ("Invalid port configuration specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!err) {
+		outb(mode, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, mode);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_dio_io_single_read(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int *value, int time_out, int flags)
+{
+	me6000_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me6000_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME6000_DIO_CTRL_BIT_MODE_0 |
+					      ME6000_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+			if ((mode ==
+			     (ME6000_DIO_CTRL_BIT_MODE_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value =
+				    inb(instance->port_reg) & (0x1 << channel);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME6000_DIO_CTRL_BIT_MODE_0 |
+					      ME6000_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+			if ((mode ==
+			     (ME6000_DIO_CTRL_BIT_MODE_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value = inb(instance->port_reg) & 0x00FF;
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_dio_io_single_write(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int value, int time_out, int flags)
+{
+	me6000_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+	uint8_t byte;
+
+	PDEBUG("executed.\n");
+
+	instance = (me6000_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME6000_DIO_CTRL_BIT_MODE_0 |
+					      ME6000_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME6000_DIO_CTRL_BIT_MODE_0 <<
+			     (instance->dio_idx * 2))) {
+				byte = inb(instance->port_reg) & 0x00FF;
+
+				if (value)
+					byte |= 0x1 << channel;
+				else
+					byte &= ~(0x1 << channel);
+
+				outb(byte, instance->port_reg);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME6000_DIO_CTRL_BIT_MODE_0 |
+					      ME6000_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME6000_DIO_CTRL_BIT_MODE_0 <<
+			     (instance->dio_idx * 2))) {
+				outb(value, instance->port_reg);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_dio_query_number_channels(me_subdevice_t * subdevice,
+					    int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_dio_query_subdevice_type(me_subdevice_t * subdevice,
+					   int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DIO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_dio_query_subdevice_caps(me_subdevice_t * subdevice,
+					   int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_DIR_BYTE;
+	return ME_ERRNO_SUCCESS;
+}
+
+me6000_dio_subdevice_t *me6000_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock)
+{
+	me6000_dio_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me6000_dio_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me6000_dio_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+
+	/* Set the subdevice ports */
+	subdevice->ctrl_reg = reg_base + ME6000_DIO_CTRL_REG;
+	switch (dio_idx) {
+	case 0:
+		subdevice->port_reg = reg_base + ME6000_DIO_PORT_0_REG;
+		break;
+	case 1:
+		subdevice->port_reg = reg_base + ME6000_DIO_PORT_1_REG;
+		break;
+	default:
+		err = ME_ERRNO_INVALID_SUBDEVICE;
+	}
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save digital i/o index */
+	subdevice->dio_idx = dio_idx;
+
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me6000_dio_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me6000_dio_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me6000_dio_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me6000_dio_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me6000_dio_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me6000_dio_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me6000_dio_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me6000_dio.h b/drivers/staging/meilhaus/me6000_dio.h
new file mode 100644
index 000000000000..858bec1c4596
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_dio.h
@@ -0,0 +1,68 @@
+/**
+ * @file me6000_dio.h
+ *
+ * @brief ME-6000 digital input/output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME6000_DIO_H_
+#define _ME6000_DIO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me6000_dio_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+	unsigned int dio_idx;			/**< The index of the digital i/o on the device. */
+
+	unsigned long port_reg;			/**< Register holding the port status. */
+	unsigned long ctrl_reg;			/**< Register to configure the port direction. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me6000_dio_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-6000 digital input/ouput subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param dio_idx The index of the digital i/o port on the device.
+ * @param ctrl_reg_lock Spin lock protecting the control register.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me6000_dio_subdevice_t *me6000_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me6000_dio_reg.h b/drivers/staging/meilhaus/me6000_dio_reg.h
new file mode 100644
index 000000000000..e67a791a1e69
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_dio_reg.h
@@ -0,0 +1,43 @@
+/**
+ * @file me6000_dio_reg.h
+ *
+ * @brief ME-6000 digital input/output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME6000_DIO_REG_H_
+#define _ME6000_DIO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME6000_DIO_CTRL_REG				0x00	// R/W
+#define ME6000_DIO_PORT_0_REG			0x01	// R/W
+#define ME6000_DIO_PORT_1_REG			0x02	// R/W
+#define ME6000_DIO_PORT_REG				ME6000_DIO_PORT_0_REG	// R/W
+
+#define ME6000_DIO_CTRL_BIT_MODE_0		0x01
+#define ME6000_DIO_CTRL_BIT_MODE_1		0x02
+#define ME6000_DIO_CTRL_BIT_MODE_2		0x04
+#define ME6000_DIO_CTRL_BIT_MODE_3		0x08
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me6000_reg.h b/drivers/staging/meilhaus/me6000_reg.h
new file mode 100644
index 000000000000..d35273003415
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_reg.h
@@ -0,0 +1,35 @@
+/**
+ * @file me6000_reg.h
+ *
+ * @brief ME-6000 device register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME6000_REG_H_
+#define _ME6000_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME6000_INIT_XILINX_REG		0xAC	// R/-
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8100_device.c b/drivers/staging/meilhaus/me8100_device.c
new file mode 100644
index 000000000000..1fb79e490261
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_device.c
@@ -0,0 +1,187 @@
+/**
+ * @file me8100_device.c
+ *
+ * @brief ME-8100 device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "medevice.h"
+#include "me8100_device.h"
+#include "mesubdevice.h"
+#include "me8100_di.h"
+#include "me8100_do.h"
+#include "me8254.h"
+
+me_device_t *me8100_pci_constructor(struct pci_dev *pci_device)
+{
+	me8100_device_t *me8100_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	int err;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me8100_device = kmalloc(sizeof(me8100_device_t), GFP_KERNEL);
+
+	if (!me8100_device) {
+		PERROR("Cannot get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(me8100_device, 0, sizeof(me8100_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me8100_device, pci_device);
+
+	if (err) {
+		kfree(me8100_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+
+	/* Get the index in the device version information table. */
+	version_idx =
+	    me8100_versions_get_device_index(me8100_device->base.info.pci.
+					     device_id);
+
+	// Initialize spin lock .
+	spin_lock_init(&me8100_device->dio_ctrl_reg_lock);
+	spin_lock_init(&me8100_device->ctr_ctrl_reg_lock);
+	spin_lock_init(&me8100_device->clk_src_reg_lock);
+
+	// Create subdevice instances.
+
+	for (i = 0; i < me8100_versions[version_idx].di_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me8100_di_constructor(me8100_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     me8100_device->
+							     base.info.pci.
+							     reg_bases[1], i,
+							     me8100_device->
+							     base.irq,
+							     &me8100_device->
+							     dio_ctrl_reg_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me8100_device);
+			kfree(me8100_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me8100_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me8100_versions[version_idx].do_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me8100_do_constructor(me8100_device->
+							     base.info.pci.
+							     reg_bases[2], i,
+							     &me8100_device->
+							     dio_ctrl_reg_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me8100_device);
+			kfree(me8100_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me8100_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me8100_versions[version_idx].ctr_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me8254_constructor(me8100_device->base.
+							  info.pci.device_id,
+							  me8100_device->base.
+							  info.pci.reg_bases[2],
+							  0, i,
+							  &me8100_device->
+							  ctr_ctrl_reg_lock,
+							  &me8100_device->
+							  clk_src_reg_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me8100_device);
+			kfree(me8100_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me8100_device->base.slist,
+					    subdevice);
+	}
+
+	return (me_device_t *) me8100_device;
+}
+
+// Init and exit of module.
+
+static int __init me8100_init(void)
+{
+	PDEBUG("executed.\n.");
+	return ME_ERRNO_SUCCESS;
+}
+
+static void __exit me8100_exit(void)
+{
+	PDEBUG("executed.\n.");
+}
+
+module_init(me8100_init);
+
+module_exit(me8100_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR("Guenter Gebhardt <g.gebhardt@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for Template Device");
+MODULE_SUPPORTED_DEVICE("Meilhaus Template Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me8100_pci_constructor);
diff --git a/drivers/staging/meilhaus/me8100_device.h b/drivers/staging/meilhaus/me8100_device.h
new file mode 100644
index 000000000000..44c42efb04e2
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_device.h
@@ -0,0 +1,97 @@
+/**
+ * @file me8100_device.h
+ *
+ * @brief ME-8100 device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8100_DEVICE_H
+#define _ME8100_DEVICE_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure holding ME-8100 device capabilities.
+ */
+typedef struct me8100_version {
+	uint16_t device_id;
+	unsigned int di_subdevices;
+	unsigned int do_subdevices;
+	unsigned int ctr_subdevices;
+} me8100_version_t;
+
+/**
+ * @brief Device capabilities.
+ */
+static me8100_version_t me8100_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME8100_A, 1, 1, 3},
+	{PCI_DEVICE_ID_MEILHAUS_ME8100_B, 2, 2, 3},
+	{0},
+};
+
+#define ME8100_DEVICE_VERSIONS (sizeof(me8100_versions) / sizeof(me8100_version_t) - 1)	/**< Returns the number of entries in #me8100_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #me8100_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me8100_versions.
+ */
+static inline unsigned int me8100_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME8100_DEVICE_VERSIONS; i++)
+		if (me8100_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The ME-8100 device class structure.
+ */
+typedef struct me8100_device {
+	me_device_t base;			/**< The Meilhaus device base class. */
+
+	/* Child class attributes. */
+	spinlock_t dio_ctrl_reg_lock;
+	spinlock_t ctr_ctrl_reg_lock;
+	spinlock_t clk_src_reg_lock;
+} me8100_device_t;
+
+/**
+ * @brief The ME-8100 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-8100 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me8100_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8100_di.c b/drivers/staging/meilhaus/me8100_di.c
new file mode 100644
index 000000000000..971727c9e305
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_di.c
@@ -0,0 +1,693 @@
+/**
+ * @file me8100_di.c
+ *
+ * @brief ME-8100 digital input subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/version.h>
+
+#include "medefines.h"
+#include "meerror.h"
+
+#include "meids.h"
+#include "medebug.h"
+#include "meplx_reg.h"
+#include "me8100_reg.h"
+#include "me8100_di_reg.h"
+#include "me8100_di.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me8100_di_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	me8100_di_subdevice_t *instance;
+	unsigned short ctrl;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_di_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->ctrl_reg_lock);
+	ctrl = inw(instance->ctrl_reg);
+	ctrl &= ~(ME8100_DIO_CTRL_BIT_INTB_1 | ME8100_DIO_CTRL_BIT_INTB_0);
+	outw(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outw(0, instance->mask_reg);
+	PDEBUG_REG("mask_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->mask_reg - instance->reg_base, 0);
+	outw(0, instance->pattern_reg);
+	PDEBUG_REG("pattern_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->pattern_reg - instance->reg_base, 0);
+	instance->rised = -1;
+	instance->irq_count = 0;
+	instance->filtering_flag = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	outl(PLX_INTCSR_LOCAL_INT1_EN |
+	     PLX_INTCSR_LOCAL_INT1_POL |
+	     PLX_INTCSR_LOCAL_INT2_EN |
+	     PLX_INTCSR_LOCAL_INT2_POL |
+	     PLX_INTCSR_PCI_INT_EN, instance->irq_status_reg);
+	PDEBUG_REG("plx:irq_status_reg outl(0x%lX)=0x%x\n",
+		   instance->irq_status_reg,
+		   PLX_INTCSR_LOCAL_INT1_EN | PLX_INTCSR_LOCAL_INT1_POL |
+		   PLX_INTCSR_LOCAL_INT2_EN | PLX_INTCSR_LOCAL_INT2_POL |
+		   PLX_INTCSR_PCI_INT_EN);
+
+	wake_up_interruptible_all(&instance->wait_queue);
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_di_io_irq_start(me_subdevice_t * subdevice,
+				  struct file *filep,
+				  int channel,
+				  int irq_source,
+				  int irq_edge, int irq_arg, int flags)
+{
+	me8100_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint16_t ctrl;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_di_subdevice_t *) subdevice;
+
+	if (irq_source == ME_IRQ_SOURCE_DIO_PATTERN) {
+		if (flags &
+		    ~(ME_IO_IRQ_START_PATTERN_FILTERING |
+		      ME_IO_IRQ_START_DIO_WORD)) {
+			PERROR("Invalid flag specified.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+
+		if (irq_edge != ME_IRQ_EDGE_NOT_USED) {
+			PERROR("Invalid irq edge specified.\n");
+			return ME_ERRNO_INVALID_IRQ_EDGE;
+		}
+	} else if (irq_source == ME_IRQ_SOURCE_DIO_MASK) {
+		if (flags &
+		    ~(ME_IO_IRQ_START_EXTENDED_STATUS |
+		      ME_IO_IRQ_START_DIO_WORD)) {
+			PERROR("Invalid flag specified.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+
+		if (irq_edge != ME_IRQ_EDGE_ANY) {
+			PERROR("Invalid irq edge specified.\n");
+			return ME_ERRNO_INVALID_IRQ_EDGE;
+		}
+
+		if (!(irq_arg & 0xFFFF)) {
+			PERROR("No mask specified.\n");
+			return ME_ERRNO_INVALID_IRQ_ARG;
+		}
+	} else {
+		PERROR("Invalid irq source specified.\n");
+		return ME_ERRNO_INVALID_IRQ_SOURCE;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	if (irq_source == ME_IRQ_SOURCE_DIO_PATTERN) {
+		outw(irq_arg, instance->pattern_reg);
+		instance->compare_value = irq_arg;
+		instance->filtering_flag =
+		    (flags & ME_IO_IRQ_START_PATTERN_FILTERING) ? 1 : 0;
+	}
+	if (irq_source == ME_IRQ_SOURCE_DIO_MASK) {
+		outw(irq_arg, instance->mask_reg);
+	}
+
+	spin_lock(instance->ctrl_reg_lock);
+	ctrl = inw(instance->ctrl_reg);
+	ctrl |= ME8100_DIO_CTRL_BIT_INTB_0;
+	if (irq_source == ME_IRQ_SOURCE_DIO_PATTERN) {
+		ctrl &= ~ME8100_DIO_CTRL_BIT_INTB_1;
+	}
+
+	if (irq_source == ME_IRQ_SOURCE_DIO_MASK) {
+		ctrl |= ME8100_DIO_CTRL_BIT_INTB_1;
+	}
+	outw(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	instance->rised = 0;
+	instance->status_value = 0;
+	instance->status_value_edges = 0;
+	instance->line_value = inw(instance->port_reg);
+	instance->status_flag = flags & ME_IO_IRQ_START_EXTENDED_STATUS;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8100_di_io_irq_wait(me_subdevice_t * subdevice,
+				 struct file *filep,
+				 int channel,
+				 int *irq_count,
+				 int *value, int time_out, int flags)
+{
+	me8100_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	unsigned long cpu_flags;
+	int count;
+
+	PDEBUG("executed.\n");
+	PDEVELOP("PID: %d.\n", current->pid);
+
+	instance = (me8100_di_subdevice_t *) subdevice;
+
+	if (flags &
+	    ~(ME_IO_IRQ_WAIT_NORMAL_STATUS | ME_IO_IRQ_WAIT_EXTENDED_STATUS)) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->rised <= 0) {
+		instance->rised = 0;
+		count = instance->irq_count;
+
+		if (time_out) {
+			t = wait_event_interruptible_timeout(instance->
+							     wait_queue,
+							     ((count !=
+							       instance->
+							       irq_count)
+							      || (instance->
+								  rised < 0)),
+							     t);
+//                      t = wait_event_interruptible_timeout(instance->wait_queue, (instance->rised != 0), t);
+			if (t == 0) {
+				PERROR("Wait on interrupt timed out.\n");
+				err = ME_ERRNO_TIMEOUT;
+			}
+		} else {
+			wait_event_interruptible(instance->wait_queue,
+						 ((count != instance->irq_count)
+						  || (instance->rised < 0)));
+//                      wait_event_interruptible(instance->wait_queue, (instance->rised != 0));
+		}
+
+		if (instance->rised < 0) {
+			PERROR("Wait on interrupt aborted by user.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+	}
+
+	if (signal_pending(current)) {
+		PERROR("Wait on interrupt aborted by signal.\n");
+		err = ME_ERRNO_SIGNAL;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	*irq_count = instance->irq_count;
+	if (!err) {
+		if (flags & ME_IO_IRQ_WAIT_NORMAL_STATUS) {
+			*value = instance->status_value;
+		} else if (flags & ME_IO_IRQ_WAIT_EXTENDED_STATUS) {
+			*value = instance->status_value_edges;
+		} else {	// Use default
+			if (!instance->status_flag) {
+				*value = instance->status_value;
+			} else {
+				*value = instance->status_value_edges;
+			}
+		}
+		instance->rised = 0;
+/*
+			instance->status_value = 0;
+			instance->status_value_edges = 0;
+*/
+	} else {
+		*value = 0;
+	}
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8100_di_io_irq_stop(me_subdevice_t * subdevice,
+				 struct file *filep, int channel, int flags)
+{
+	me8100_di_subdevice_t *instance;
+	uint16_t ctrl;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_di_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->ctrl_reg_lock);
+	ctrl = inw(instance->ctrl_reg);
+	ctrl &= ~(ME8100_DIO_CTRL_BIT_INTB_1 | ME8100_DIO_CTRL_BIT_INTB_0);
+	outw(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock(instance->ctrl_reg_lock);
+	instance->rised = -1;
+	instance->status_value = 0;
+	instance->status_value_edges = 0;
+	instance->filtering_flag = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_di_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me8100_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_WORD:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+			} else {
+				PERROR
+				    ("Invalid port configuration specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8100_di_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me8100_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 16)) {
+			*value = inw(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inw(instance->port_reg) & 0xFF;
+		} else if (channel == 1) {
+			*value = (inw(instance->port_reg) >> 8) & 0xFF;
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_WORD:
+		if (channel == 0) {
+			*value = inw(instance->port_reg);
+		} else {
+			PERROR("Invalid word number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8100_di_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 16;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_di_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DI;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_di_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_BIT_PATTERN_IRQ | ME_CAPS_DIO_BIT_MASK_IRQ_EDGE_ANY;
+	return ME_ERRNO_SUCCESS;
+}
+
+static void me8100_di_destructor(struct me_subdevice *subdevice)
+{
+	me8100_di_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_di_subdevice_t *) subdevice;
+
+	free_irq(instance->irq, (void *)instance);
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me8100_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me8100_isr(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{
+	me8100_di_subdevice_t *instance;
+	uint32_t icsr;
+
+	uint16_t irq_status;
+	uint16_t line_value = 0;
+
+	uint32_t status_val = 0;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_di_subdevice_t *) dev_id;
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	icsr = inl(instance->irq_status_reg);
+	if (instance->di_idx == 0) {
+
+		if ((icsr &
+		     (PLX_INTCSR_LOCAL_INT1_STATE | PLX_INTCSR_PCI_INT_EN |
+		      PLX_INTCSR_LOCAL_INT1_EN)) !=
+		    (PLX_INTCSR_LOCAL_INT1_STATE | PLX_INTCSR_PCI_INT_EN |
+		     PLX_INTCSR_LOCAL_INT1_EN)) {
+			PINFO
+			    ("%ld Shared interrupt. %s(): idx=0 plx:irq_status_reg=0x%04X\n",
+			     jiffies, __func__, icsr);
+			return IRQ_NONE;
+		}
+	} else if (instance->di_idx == 1) {
+		if ((icsr &
+		     (PLX_INTCSR_LOCAL_INT2_STATE | PLX_INTCSR_PCI_INT_EN |
+		      PLX_INTCSR_LOCAL_INT2_EN)) !=
+		    (PLX_INTCSR_LOCAL_INT2_STATE | PLX_INTCSR_PCI_INT_EN |
+		     PLX_INTCSR_LOCAL_INT2_EN)) {
+			PINFO
+			    ("%ld Shared interrupt. %s(): idx=1 plx:irq_status_reg=0x%04X\n",
+			     jiffies, __func__, icsr);
+			return IRQ_NONE;
+		}
+	} else {
+		PERROR("%s():Wrong interrupt idx=%d csr=0x%X.\n", __func__,
+		       instance->di_idx, icsr);
+		return IRQ_NONE;
+	}
+
+	PDEBUG("me8100_isr():Interrupt from idx=%d occured.\n",
+	       instance->di_idx);
+	spin_lock(&instance->subdevice_lock);
+	inw(instance->irq_reset_reg);
+	line_value = inw(instance->port_reg);
+
+	irq_status = instance->line_value ^ line_value;
+
+	// Make extended information.
+	status_val |= (0x00FF & (~(uint16_t) instance->line_value & line_value)) << 16;	//Raise
+	status_val |= (0x00FF & ((uint16_t) instance->line_value & ~line_value));	//Fall
+
+	instance->line_value = line_value;
+
+	if (instance->rised == 0) {
+		instance->status_value = irq_status;
+		instance->status_value_edges = status_val;
+	} else {
+		instance->status_value |= irq_status;
+		instance->status_value_edges |= status_val;
+	}
+
+	if (instance->filtering_flag) {	// For compare mode only.
+		if (instance->compare_value == instance->line_value) {
+			instance->rised = 1;
+			instance->irq_count++;
+		}
+	} else {
+		instance->rised = 1;
+		instance->irq_count++;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+me8100_di_subdevice_t *me8100_di_constructor(uint32_t me8100_reg_base,
+					     uint32_t plx_reg_base,
+					     unsigned int di_idx,
+					     int irq,
+					     spinlock_t * ctrl_reg_lock)
+{
+	me8100_di_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me8100_di_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me8100_di_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save the subdevice index. */
+	subdevice->di_idx = di_idx;
+
+	/* Initialize wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	/* Register interrupt service routine. */
+	subdevice->irq = irq;
+	err = request_irq(subdevice->irq, me8100_isr,
+#ifdef IRQF_DISABLED
+			  IRQF_DISABLED | IRQF_SHARED,
+#else
+			  SA_INTERRUPT | SA_SHIRQ,
+#endif
+			  ME8100_NAME, (void *)subdevice);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	PINFO("Registered irq=%d.\n", subdevice->irq);
+
+	/* Initialize the registers */
+	subdevice->ctrl_reg =
+	    me8100_reg_base + ME8100_CTRL_REG_A + di_idx * ME8100_REG_OFFSET;
+	subdevice->port_reg =
+	    me8100_reg_base + ME8100_DI_REG_A + di_idx * ME8100_REG_OFFSET;
+	subdevice->mask_reg =
+	    me8100_reg_base + ME8100_MASK_REG_A + di_idx * ME8100_REG_OFFSET;
+	subdevice->pattern_reg =
+	    me8100_reg_base + ME8100_PATTERN_REG_A + di_idx * ME8100_REG_OFFSET;
+	subdevice->din_int_reg =
+	    me8100_reg_base + ME8100_INT_DI_REG_A + di_idx * ME8100_REG_OFFSET;
+	subdevice->irq_reset_reg =
+	    me8100_reg_base + ME8100_RES_INT_REG_A + di_idx * ME8100_REG_OFFSET;
+	subdevice->irq_status_reg = plx_reg_base + PLX_INTCSR;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = me8100_reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_irq_start = me8100_di_io_irq_start;
+	subdevice->base.me_subdevice_io_irq_wait = me8100_di_io_irq_wait;
+	subdevice->base.me_subdevice_io_irq_stop = me8100_di_io_irq_stop;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me8100_di_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me8100_di_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me8100_di_io_single_read;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me8100_di_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me8100_di_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me8100_di_query_subdevice_caps;
+	subdevice->base.me_subdevice_destructor = me8100_di_destructor;
+
+	subdevice->rised = 0;
+	subdevice->irq_count = 0;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me8100_di.h b/drivers/staging/meilhaus/me8100_di.h
new file mode 100644
index 000000000000..e1db79129175
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_di.h
@@ -0,0 +1,89 @@
+/**
+ * @file me8100_di.h
+ *
+ * @brief ME-8100 digital input subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8100_DI_H_
+#define _ME8100_DI_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me8100_di_subdevice {
+	// Inheritance
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;
+
+	unsigned di_idx;
+
+	int irq;
+	volatile int rised;
+	unsigned int irq_count;
+
+	uint status_flag;				/**< Default interupt status flag */
+	uint status_value;				/**< Interupt status */
+	uint status_value_edges;		/**< Extended interupt status */
+	uint line_value;
+
+	uint16_t compare_value;
+	uint8_t filtering_flag;
+
+	wait_queue_head_t wait_queue;
+
+	unsigned long ctrl_reg;
+	unsigned long port_reg;
+	unsigned long mask_reg;
+	unsigned long pattern_reg;
+	unsigned long long din_int_reg;
+	unsigned long irq_reset_reg;
+	unsigned long irq_status_reg;
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+
+} me8100_di_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-8100 digital input subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me8100_di_subdevice_t *me8100_di_constructor(uint32_t me8100_reg_base,
+					     uint32_t plx_reg_base,
+					     unsigned int di_idx,
+					     int irq,
+					     spinlock_t * ctrl_leg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8100_di_reg.h b/drivers/staging/meilhaus/me8100_di_reg.h
new file mode 100644
index 000000000000..063bd193709e
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_di_reg.h
@@ -0,0 +1,47 @@
+/**
+ * @file me8100_di_reg.h
+ *
+ * @brief ME-8100 digital input subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8100_DI_REG_H_
+#define _ME8100_DI_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME8100_RES_INT_REG_A		0x02	//(r, )
+#define ME8100_DI_REG_A			0x04	//(r, )
+#define ME8100_PATTERN_REG_A		0x08	//( ,w)
+#define ME8100_MASK_REG_A		0x0A	//( ,w)
+#define ME8100_INT_DI_REG_A		0x0A	//(r, )
+
+#define ME8100_RES_INT_REG_B		0x0E	//(r, )
+#define ME8100_DI_REG_B			0x10	//(r, )
+#define ME8100_PATTERN_REG_B		0x14	//( ,w)
+#define ME8100_MASK_REG_B		0x16	//( ,w)
+#define ME8100_INT_DI_REG_B		0x16	//(r, )
+
+#define ME8100_REG_OFFSET		0x0C
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8100_do.c b/drivers/staging/meilhaus/me8100_do.c
new file mode 100644
index 000000000000..957b9f92f760
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_do.c
@@ -0,0 +1,391 @@
+/**
+ * @file me8100_do.c
+ *
+ * @brief ME-8100 digital output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me8100_reg.h"
+#include "me8100_do_reg.h"
+#include "me8100_do.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me8100_do_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	me8100_do_subdevice_t *instance;
+	uint16_t ctrl;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_do_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	ctrl = inw(instance->ctrl_reg);
+	ctrl &= ME8100_DIO_CTRL_BIT_INTB_1 | ME8100_DIO_CTRL_BIT_INTB_0;
+	outw(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock(instance->ctrl_reg_lock);
+	outw(0, instance->port_reg);
+	instance->port_reg_mirror = 0;
+	PDEBUG_REG("port_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_do_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me8100_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	int config;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	config = inw(instance->ctrl_reg);
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_WORD:
+		if (channel == 0) {
+			if (single_config ==
+			    ME_SINGLE_CONFIG_DIO_HIGH_IMPEDANCE) {
+				config &= ~(ME8100_DIO_CTRL_BIT_ENABLE_DIO);
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_SINK) {
+				config |= ME8100_DIO_CTRL_BIT_ENABLE_DIO;
+				config &= ~ME8100_DIO_CTRL_BIT_SOURCE;
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_SOURCE) {
+				config |=
+				    ME8100_DIO_CTRL_BIT_ENABLE_DIO |
+				    ME8100_DIO_CTRL_BIT_SOURCE;
+			} else {
+				PERROR
+				    ("Invalid port configuration specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid word number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!err) {
+		outw(config, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, config);
+	}
+
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8100_do_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me8100_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 16)) {
+			*value = instance->port_reg_mirror & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = instance->port_reg_mirror & 0xFF;
+		} else if (channel == 1) {
+			*value = (instance->port_reg_mirror >> 8) & 0xFF;
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_WORD:
+		if (channel == 0) {
+			*value = instance->port_reg_mirror;
+		} else {
+			PERROR("Invalid word number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8100_do_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me8100_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 16)) {
+			instance->port_reg_mirror =
+			    value ? (instance->
+				     port_reg_mirror | (0x1 << channel))
+			    : (instance->port_reg_mirror & ~(0x1 << channel));
+			outw(instance->port_reg_mirror, instance->port_reg);
+			PDEBUG_REG("port_reg outw(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->port_reg - instance->reg_base,
+				   instance->port_reg_mirror);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			instance->port_reg_mirror &= ~0xFF;
+			instance->port_reg_mirror |= value & 0xFF;
+			outw(instance->port_reg_mirror, instance->port_reg);
+			PDEBUG_REG("port_reg outw(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->port_reg - instance->reg_base,
+				   instance->port_reg_mirror);
+		} else if (channel == 1) {
+			instance->port_reg_mirror &= ~0xFF00;
+			instance->port_reg_mirror |= (value << 8) & 0xFF00;
+			outw(instance->port_reg_mirror, instance->port_reg);
+			PDEBUG_REG("port_reg outw(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->port_reg - instance->reg_base,
+				   instance->port_reg_mirror);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_WORD:
+		if (channel == 0) {
+			instance->port_reg_mirror = value;
+			outw(value, instance->port_reg);
+			PDEBUG_REG("port_reg outw(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->port_reg - instance->reg_base,
+				   value);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8100_do_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 16;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_do_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_do_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_SINK_SOURCE;
+	return ME_ERRNO_SUCCESS;
+}
+
+me8100_do_subdevice_t *me8100_do_constructor(uint32_t reg_base,
+					     unsigned int do_idx,
+					     spinlock_t * ctrl_reg_lock)
+{
+	me8100_do_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me8100_do_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me8100_do_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+
+	/* Initialize registers */
+	if (do_idx == 0) {
+		subdevice->port_reg = reg_base + ME8100_DO_REG_A;
+		subdevice->ctrl_reg = reg_base + ME8100_CTRL_REG_A;
+	} else if (do_idx == 1) {
+		subdevice->port_reg = reg_base + ME8100_DO_REG_B;
+		subdevice->ctrl_reg = reg_base + ME8100_CTRL_REG_B;
+	} else {
+		PERROR("Wrong subdevice idx=%d.\n", do_idx);
+		kfree(subdevice);
+		return NULL;
+	}
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save the subdevice index */
+	subdevice->do_idx = do_idx;
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me8100_do_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me8100_do_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me8100_do_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me8100_do_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me8100_do_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me8100_do_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me8100_do_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me8100_do.h b/drivers/staging/meilhaus/me8100_do.h
new file mode 100644
index 000000000000..acf880136663
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_do.h
@@ -0,0 +1,70 @@
+/**
+ * @file me8100_do.h
+ *
+ * @brief ME-8100 digital output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8100_DO_H_
+#define _ME8100_DO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me8100_do_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect the #ctrl_reg. */
+
+	unsigned int do_idx;
+
+	uint16_t port_reg_mirror;		/**< Mirror used to store current port register setting which is write only. */
+
+	unsigned long port_reg;			/**< Register holding the port status. */
+	unsigned long ctrl_reg;			/**< Control register. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me8100_do_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-8100 digital output subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param do_idx The index of the digital output subdevice on this device.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me8100_do_subdevice_t *me8100_do_constructor(uint32_t reg_base,
+					     unsigned int do_idx,
+					     spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8100_do_reg.h b/drivers/staging/meilhaus/me8100_do_reg.h
new file mode 100644
index 000000000000..13a23802b31a
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_do_reg.h
@@ -0,0 +1,36 @@
+/**
+ * @file me8100_ao_reg.h
+ *
+ * @brief ME-8100 analog output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8100_DO_REG_H_
+#define _ME8100_DO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME8100_DO_REG_A			0x06	//( ,w)
+#define ME8100_DO_REG_B			0x12	//( ,w)
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8100_reg.h b/drivers/staging/meilhaus/me8100_reg.h
new file mode 100644
index 000000000000..d8c4b1c6b153
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_reg.h
@@ -0,0 +1,41 @@
+/**
+ * @file me8100_reg.h
+ *
+ * @brief ME-8100 register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8100_REG_H_
+#define _ME8100_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME8100_CTRL_REG_A			0x00	//( ,w)
+#define ME8100_CTRL_REG_B			0x0C	//( ,w)
+
+#define ME8100_DIO_CTRL_BIT_SOURCE		0x10
+#define ME8100_DIO_CTRL_BIT_INTB_1		0x20
+#define ME8100_DIO_CTRL_BIT_INTB_0		0x40
+#define ME8100_DIO_CTRL_BIT_ENABLE_DIO		0x80
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_device.c b/drivers/staging/meilhaus/me8200_device.c
new file mode 100644
index 000000000000..261c0cbd9d0a
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_device.c
@@ -0,0 +1,194 @@
+/**
+ * @file me8200_device.c
+ *
+ * @brief ME-8200 device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "meplx_reg.h"
+#include "medevice.h"
+#include "me8200_device.h"
+#include "mesubdevice.h"
+#include "me8200_di.h"
+#include "me8200_do.h"
+#include "me8200_dio.h"
+
+me_device_t *me8200_pci_constructor(struct pci_dev *pci_device)
+{
+	me8200_device_t *me8200_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	int err;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me8200_device = kmalloc(sizeof(me8200_device_t), GFP_KERNEL);
+
+	if (!me8200_device) {
+		PERROR("Cannot get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(me8200_device, 0, sizeof(me8200_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me8200_device, pci_device);
+
+	if (err) {
+		kfree(me8200_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+
+	/* Get the index in the device version information table. */
+	version_idx =
+	    me8200_versions_get_device_index(me8200_device->base.info.pci.
+					     device_id);
+
+	// Initialize spin lock .
+	spin_lock_init(&me8200_device->irq_ctrl_lock);
+	spin_lock_init(&me8200_device->irq_mode_lock);
+	spin_lock_init(&me8200_device->dio_ctrl_lock);
+
+	/* Setup the PLX interrupt configuration */
+	outl(PLX_INTCSR_LOCAL_INT1_EN |
+	     PLX_INTCSR_LOCAL_INT1_POL |
+	     PLX_INTCSR_LOCAL_INT2_EN |
+	     PLX_INTCSR_LOCAL_INT2_POL |
+	     PLX_INTCSR_PCI_INT_EN,
+	     me8200_device->base.info.pci.reg_bases[1] + PLX_INTCSR);
+
+	// Create subdevice instances.
+
+	for (i = 0; i < me8200_versions[version_idx].di_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me8200_di_constructor(me8200_device->
+							     base.info.pci.
+							     reg_bases[2], i,
+							     me8200_device->
+							     base.irq,
+							     &me8200_device->
+							     irq_ctrl_lock,
+							     &me8200_device->
+							     irq_mode_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me8200_device);
+			kfree(me8200_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me8200_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me8200_versions[version_idx].do_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me8200_do_constructor(me8200_device->
+							     base.info.pci.
+							     reg_bases[2], i,
+							     me8200_device->
+							     base.irq,
+							     &me8200_device->
+							     irq_mode_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me8200_device);
+			kfree(me8200_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me8200_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me8200_versions[version_idx].dio_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me8200_dio_constructor(me8200_device->
+							      base.info.pci.
+							      reg_bases[2], i,
+							      &me8200_device->
+							      dio_ctrl_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me8200_device);
+			kfree(me8200_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me8200_device->base.slist,
+					    subdevice);
+	}
+
+	return (me_device_t *) me8200_device;
+}
+
+// Init and exit of module.
+
+static int __init me8200_init(void)
+{
+	PDEBUG("executed.\n.");
+	return 0;
+}
+
+static void __exit me8200_exit(void)
+{
+	PDEBUG("executed.\n.");
+}
+
+module_init(me8200_init);
+
+module_exit(me8200_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR("Guenter Gebhardt <g.gebhardt@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for Template Device");
+MODULE_SUPPORTED_DEVICE("Meilhaus Template Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me8200_pci_constructor);
diff --git a/drivers/staging/meilhaus/me8200_device.h b/drivers/staging/meilhaus/me8200_device.h
new file mode 100644
index 000000000000..cbd2a01ddb41
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_device.h
@@ -0,0 +1,97 @@
+/**
+ * @file me8200_device.h
+ *
+ * @brief ME-8200 device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_DEVICE_H
+#define _ME8200_DEVICE_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure holding ME-8200 device capabilities.
+ */
+typedef struct me8200_version {
+	uint16_t device_id;
+	unsigned int di_subdevices;
+	unsigned int do_subdevices;
+	unsigned int dio_subdevices;
+} me8200_version_t;
+
+/**
+ * @brief Device capabilities.
+ */
+static me8200_version_t me8200_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME8200_A, 1, 1, 2},
+	{PCI_DEVICE_ID_MEILHAUS_ME8200_B, 2, 2, 2},
+	{0},
+};
+
+#define ME8200_DEVICE_VERSIONS (sizeof(me8200_versions) / sizeof(me8200_version_t) - 1)	/**< Returns the number of entries in #me8200_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #me8200_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me8200_versions.
+ */
+static inline unsigned int me8200_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME8200_DEVICE_VERSIONS; i++)
+		if (me8200_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The ME-8200 device class structure.
+ */
+typedef struct me8200_device {
+	me_device_t base;		/**< The Meilhaus device base class. */
+
+	/* Child class attributes. */
+	spinlock_t irq_ctrl_lock;	/**< Lock for the interrupt control register. */
+	spinlock_t irq_mode_lock;	/**< Lock for the interrupt mode register. */
+	spinlock_t dio_ctrl_lock;	/**< Lock for the digital i/o control register. */
+} me8200_device_t;
+
+/**
+ * @brief The ME-8200 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-8200 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me8200_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_di.c b/drivers/staging/meilhaus/me8200_di.c
new file mode 100644
index 000000000000..27525bc067b6
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_di.c
@@ -0,0 +1,857 @@
+/**
+ * @file me8200_di.c
+ *
+ * @brief ME-8200 digital input subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+///Includes
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/version.h>
+
+#include "medefines.h"
+#include "meerror.h"
+
+#include "meids.h"
+#include "medebug.h"
+#include "me8200_reg.h"
+#include "me8200_di_reg.h"
+#include "me8200_di.h"
+
+/// Defines
+static void me8200_di_destructor(struct me_subdevice *subdevice);
+static int me8200_di_io_irq_start(me_subdevice_t * subdevice,
+				  struct file *filep,
+				  int channel,
+				  int irq_source,
+				  int irq_edge, int irq_arg, int flags);
+static int me8200_di_io_irq_wait(me_subdevice_t * subdevice,
+				 struct file *filep,
+				 int channel,
+				 int *irq_count,
+				 int *value, int time_out, int flags);
+static int me8200_di_io_irq_stop(me_subdevice_t * subdevice,
+				 struct file *filep, int channel, int flags);
+static int me8200_di_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags);
+static int me8200_di_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags);
+static int me8200_di_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags);
+static int me8200_di_query_number_channels(me_subdevice_t * subdevice,
+					   int *number);
+static int me8200_di_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype);
+static int me8200_di_query_subdevice_caps(me_subdevice_t * subdevice,
+					  int *caps);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me8200_isr(int irq, void *dev_id);
+#else
+static irqreturn_t me8200_isr(int irq, void *dev_id, struct pt_regs *regs);
+#endif
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me8200_isr_EX(int irq, void *dev_id);
+#else
+static irqreturn_t me8200_isr_EX(int irq, void *dev_id, struct pt_regs *regs);
+#endif
+static void me8200_di_check_version(me8200_di_subdevice_t * instance,
+				    unsigned long addr);
+
+///Functions
+static int me8200_di_io_irq_start(me_subdevice_t * subdevice,
+				  struct file *filep,
+				  int channel,
+				  int irq_source,
+				  int irq_edge, int irq_arg, int flags)
+{
+	me8200_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	volatile uint8_t tmp;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_di_subdevice_t *) subdevice;
+
+	if (irq_source == ME_IRQ_SOURCE_DIO_PATTERN) {
+		if (flags &
+		    ~(ME_IO_IRQ_START_PATTERN_FILTERING |
+		      ME_IO_IRQ_START_DIO_BYTE)) {
+			PERROR("Invalid flag specified.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+
+		if (irq_edge != ME_IRQ_EDGE_NOT_USED) {
+			PERROR("Invalid irq edge specified.\n");
+			return ME_ERRNO_INVALID_IRQ_EDGE;
+		}
+	} else if (irq_source == ME_IRQ_SOURCE_DIO_MASK) {
+		if (flags &
+		    ~(ME_IO_IRQ_START_EXTENDED_STATUS |
+		      ME_IO_IRQ_START_DIO_BYTE)) {
+			PERROR("Invalid flag specified.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+
+		if ((irq_edge != ME_IRQ_EDGE_RISING)
+		    && (irq_edge != ME_IRQ_EDGE_FALLING)
+		    && (irq_edge != ME_IRQ_EDGE_ANY)) {
+			PERROR("Invalid irq edge specified.\n");
+			return ME_ERRNO_INVALID_IRQ_EDGE;
+		}
+
+		if (!(irq_arg & 0xFF)) {
+			PERROR("No mask specified.\n");
+			return ME_ERRNO_INVALID_IRQ_ARG;
+		}
+	} else {
+		PERROR("Invalid irq source specified.\n");
+		return ME_ERRNO_INVALID_IRQ_SOURCE;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, status);
+	if (irq_source == ME_IRQ_SOURCE_DIO_PATTERN) {
+		outb(irq_arg, instance->compare_reg);
+		PDEBUG_REG("compare_reg outb(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->compare_reg - instance->reg_base, irq_arg);
+		outb(0xFF, instance->mask_reg);
+		PDEBUG_REG("mask_reg outb(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->mask_reg - instance->reg_base, 0xff);
+		instance->compare_value = irq_arg;
+		instance->filtering_flag =
+		    (flags & ME_IO_IRQ_START_PATTERN_FILTERING) ? 1 : 0;
+	}
+	if (irq_source == ME_IRQ_SOURCE_DIO_MASK) {
+		outb(irq_arg, instance->mask_reg);
+		PDEBUG_REG("mask_reg outb(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->mask_reg - instance->reg_base, irq_arg);
+		instance->filtering_flag = 0;
+	}
+
+	spin_lock(instance->irq_mode_lock);
+	tmp = inb(instance->irq_mode_reg);
+	tmp &=
+	    ~(ME8200_IRQ_MODE_MASK <<
+	      (ME8200_IRQ_MODE_DI_SHIFT * instance->di_idx));
+	if (irq_source == ME_IRQ_SOURCE_DIO_PATTERN) {
+		tmp |=
+		    ME8200_IRQ_MODE_MASK_COMPARE << (ME8200_IRQ_MODE_DI_SHIFT *
+						     instance->di_idx);
+	}
+
+	if (irq_source == ME_IRQ_SOURCE_DIO_MASK) {
+		tmp |=
+		    ME8200_IRQ_MODE_MASK_MASK << (ME8200_IRQ_MODE_DI_SHIFT *
+						  instance->di_idx);
+	}
+	outb(tmp, instance->irq_mode_reg);
+	PDEBUG_REG("irq_mode_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_mode_reg - instance->reg_base, tmp);
+	spin_unlock(instance->irq_mode_lock);
+
+	spin_lock(instance->irq_ctrl_lock);
+	tmp = inb(instance->irq_ctrl_reg);
+	tmp |=
+	    (ME8200_DI_IRQ_CTRL_BIT_CLEAR <<
+	     (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+	tmp |=
+	    ME8200_DI_IRQ_CTRL_BIT_ENABLE << (ME8200_DI_IRQ_CTRL_SHIFT *
+					      instance->di_idx);
+
+	if (irq_source == ME_IRQ_SOURCE_DIO_MASK) {
+		tmp &=
+		    ~(ME8200_DI_IRQ_CTRL_MASK_EDGE <<
+		      (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+		if (irq_edge == ME_IRQ_EDGE_RISING) {
+			tmp |=
+			    ME8200_DI_IRQ_CTRL_MASK_EDGE_RISING <<
+			    (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx);
+		} else if (irq_edge == ME_IRQ_EDGE_FALLING) {
+			tmp |=
+			    ME8200_DI_IRQ_CTRL_MASK_EDGE_FALLING <<
+			    (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx);
+		} else if (irq_edge == ME_IRQ_EDGE_ANY) {
+			tmp |=
+			    ME8200_DI_IRQ_CTRL_MASK_EDGE_ANY <<
+			    (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx);
+		}
+	}
+	outb(tmp, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, tmp);
+	tmp &=
+	    ~(ME8200_DI_IRQ_CTRL_BIT_CLEAR <<
+	      (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+	outb(tmp, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, tmp);
+
+	instance->line_value = inb(instance->port_reg);
+	spin_unlock(instance->irq_ctrl_lock);
+
+	instance->rised = 0;
+	instance->status_value = 0;
+	instance->status_value_edges = 0;
+	instance->status_flag = flags & ME_IO_IRQ_START_EXTENDED_STATUS;
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_di_io_irq_wait(me_subdevice_t * subdevice,
+				 struct file *filep,
+				 int channel,
+				 int *irq_count,
+				 int *value, int time_out, int flags)
+{
+	me8200_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	unsigned long cpu_flags;
+	int count;
+
+	PDEBUG("executed.\n");
+	PDEVELOP("PID: %d.\n", current->pid);
+
+	instance = (me8200_di_subdevice_t *) subdevice;
+
+	if (flags &
+	    ~(ME_IO_IRQ_WAIT_NORMAL_STATUS | ME_IO_IRQ_WAIT_EXTENDED_STATUS)) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->rised <= 0) {
+		instance->rised = 0;
+		count = instance->count;
+
+		if (time_out) {
+			t = wait_event_interruptible_timeout(instance->
+							     wait_queue,
+							     ((count !=
+							       instance->count)
+							      || (instance->
+								  rised < 0)),
+							     t);
+//                      t = wait_event_interruptible_timeout(instance->wait_queue, (instance->rised != 0), t);
+			if (t == 0) {
+				PERROR("Wait on interrupt timed out.\n");
+				err = ME_ERRNO_TIMEOUT;
+			}
+		} else {
+			wait_event_interruptible(instance->wait_queue,
+						 ((count != instance->count)
+						  || (instance->rised < 0)));
+//                      wait_event_interruptible(instance->wait_queue, (instance->rised != 0));
+		}
+
+		if (instance->rised < 0) {
+			PERROR("Wait on interrupt aborted by user.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+	}
+
+	if (signal_pending(current)) {
+		PERROR("Wait on interrupt aborted by signal.\n");
+		err = ME_ERRNO_SIGNAL;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	*irq_count = instance->count;
+	if (!err) {
+		if (flags & ME_IO_IRQ_WAIT_NORMAL_STATUS) {
+			*value = instance->status_value;
+		} else if (flags & ME_IO_IRQ_WAIT_EXTENDED_STATUS) {
+			*value = instance->status_value_edges;
+		} else {	// Use default
+			if (!instance->status_flag) {
+				*value = instance->status_value;
+			} else {
+				*value = instance->status_value_edges;
+			}
+		}
+		instance->rised = 0;
+/*
+			instance->status_value = 0;
+			instance->status_value_edges = 0;
+*/
+	} else {
+		*value = 0;
+	}
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_di_io_irq_stop(me_subdevice_t * subdevice,
+				 struct file *filep, int channel, int flags)
+{
+	me8200_di_subdevice_t *instance;
+	uint8_t tmp;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_di_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER spin_lock_irqsave(&instance->subdevice_lock, status);
+	spin_lock(instance->irq_ctrl_lock);
+	tmp = inb(instance->irq_ctrl_reg);
+	tmp |=
+	    (ME8200_DI_IRQ_CTRL_BIT_ENABLE <<
+	     (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+	outb(tmp, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, tmp);
+	tmp &=
+	    ~(ME8200_DI_IRQ_CTRL_BIT_ENABLE <<
+	      (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+	tmp |=
+	    (ME8200_DI_IRQ_CTRL_BIT_CLEAR <<
+	     (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+//                      tmp &= ~(ME8200_DI_IRQ_CTRL_BIT_CLEAR << (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+	outb(tmp, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->irq_ctrl_lock);
+
+	instance->rised = -1;
+	instance->status_value = 0;
+	instance->status_value_edges = 0;
+	instance->filtering_flag = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_di_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me8200_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, status);
+
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+			} else {
+				PERROR("Invalid port direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_di_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me8200_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, status);
+
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = inb(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inb(instance->port_reg);
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_di_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	me8200_di_subdevice_t *instance = (me8200_di_subdevice_t *) subdevice;
+
+	PDEBUG("executed.\n");
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	instance->count = 0;
+	return me8200_di_io_irq_stop(subdevice, filep, 0, 0);
+}
+
+static int me8200_di_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_di_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DI;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_di_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps =
+	    ME_CAPS_DIO_BIT_PATTERN_IRQ |
+	    ME_CAPS_DIO_BIT_MASK_IRQ_EDGE_RISING |
+	    ME_CAPS_DIO_BIT_MASK_IRQ_EDGE_FALLING |
+	    ME_CAPS_DIO_BIT_MASK_IRQ_EDGE_ANY;
+	return ME_ERRNO_SUCCESS;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me8200_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me8200_isr(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{
+	me8200_di_subdevice_t *instance;
+	uint8_t ctrl;
+	uint8_t irq_status;
+	uint8_t line_value = 0;
+	uint8_t line_status = 0;
+	uint32_t status_val = 0;
+
+	instance = (me8200_di_subdevice_t *) dev_id;
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	irq_status = inb(instance->irq_status_reg);
+	if (!irq_status) {
+		PINFO
+		    ("%ld Shared interrupt. %s(): idx=%d irq_status_reg=0x%04X\n",
+		     jiffies, __func__, instance->di_idx, irq_status);
+		return IRQ_NONE;
+	}
+
+	PDEBUG("executed.\n");
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->irq_ctrl_lock);
+	ctrl = inb(instance->irq_ctrl_reg);
+	ctrl |=
+	    ME8200_DI_IRQ_CTRL_BIT_CLEAR << (ME8200_DI_IRQ_CTRL_SHIFT *
+					     instance->di_idx);
+	outb(ctrl, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, ctrl);
+	ctrl &=
+	    ~(ME8200_DI_IRQ_CTRL_BIT_CLEAR <<
+	      (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+	outb(ctrl, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, ctrl);
+
+	line_value = inb(instance->port_reg);
+	spin_unlock(instance->irq_ctrl_lock);
+
+	line_status = ((uint8_t) instance->line_value ^ line_value);
+
+	// Make extended information.
+	status_val |= (0x00FF & (~(uint8_t) instance->line_value & line_value)) << 16;	//Raise
+	status_val |= (0x00FF & ((uint8_t) instance->line_value & ~line_value));	//Fall
+
+	instance->line_value = (int)line_value;
+
+	if (instance->rised == 0) {
+		instance->status_value = irq_status | line_status;
+		instance->status_value_edges = status_val;
+	} else {
+		instance->status_value |= irq_status | line_status;
+		instance->status_value_edges |= status_val;
+	}
+
+	if (instance->filtering_flag) {	// For compare mode only.
+		if (instance->compare_value == instance->line_value) {
+			instance->rised = 1;
+			instance->count++;
+		}
+	} else {
+		instance->rised = 1;
+		instance->count++;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	spin_unlock(&instance->subdevice_lock);
+
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me8200_isr_EX(int irq, void *dev_id)
+#else
+static irqreturn_t me8200_isr_EX(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{
+	me8200_di_subdevice_t *instance;
+	uint8_t irq_status = 0;
+	uint16_t irq_status_EX = 0;
+	uint32_t status_val = 0;
+	int i, j;
+
+	instance = (me8200_di_subdevice_t *) dev_id;
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	PDEBUG("executed.\n");
+
+	//Reset latches. Copy status to extended registers.
+	irq_status = inb(instance->irq_status_reg);
+	PDEBUG_REG("idx=%d irq_status_reg=0x%02X\n", instance->di_idx,
+		   irq_status);
+
+	if (!irq_status) {
+		PINFO
+		    ("%ld Shared interrupt. %s(): idx=%d irq_status_reg=0x%04X\n",
+		     jiffies, __func__, instance->di_idx, irq_status);
+		return IRQ_NONE;
+	}
+
+	irq_status_EX = inb(instance->irq_status_low_reg);
+	irq_status_EX |= (inb(instance->irq_status_high_reg) << 8);
+
+	PDEVELOP("EXTENDED REG: 0x%04x\n", irq_status_EX);
+	instance->line_value = inb(instance->port_reg);
+
+	// Format extended information.
+	for (i = 0, j = 0; i < 8; i++, j += 2) {
+		status_val |= ((0x01 << j) & irq_status_EX) >> (j - i);	//Fall
+		status_val |= ((0x01 << (j + 1)) & irq_status_EX) << (15 - j + i);	//Raise
+	}
+
+	spin_lock(&instance->subdevice_lock);
+	if (instance->rised == 0) {
+		instance->status_value = irq_status;
+		instance->status_value_edges = status_val;
+	} else {
+		instance->status_value |= irq_status;
+		instance->status_value_edges |= status_val;
+	}
+
+	if (instance->filtering_flag) {	// For compare mode only.
+		if (instance->compare_value == instance->line_value) {
+			instance->rised = 1;
+			instance->count++;
+		}
+	} else {
+		instance->rised = 1;
+		instance->count++;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+static void me8200_di_destructor(struct me_subdevice *subdevice)
+{
+	me8200_di_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_di_subdevice_t *) subdevice;
+
+	free_irq(instance->irq, (void *)instance);
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+me8200_di_subdevice_t *me8200_di_constructor(uint32_t me8200_regbase,
+					     unsigned int di_idx,
+					     int irq,
+					     spinlock_t * irq_ctrl_lock,
+					     spinlock_t * irq_mode_lock)
+{
+	me8200_di_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me8200_di_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me8200_di_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Check firmware version.
+	me8200_di_check_version(subdevice,
+				me8200_regbase + ME8200_FIRMWARE_VERSION_REG);
+
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->irq_ctrl_lock = irq_ctrl_lock;
+	subdevice->irq_mode_lock = irq_mode_lock;
+
+	/* Save the subdevice index. */
+	subdevice->di_idx = di_idx;
+
+	/* Initialize registers */
+	if (di_idx == 0) {
+		subdevice->port_reg = me8200_regbase + ME8200_DI_PORT_0_REG;
+		subdevice->mask_reg = me8200_regbase + ME8200_DI_MASK_0_REG;
+		subdevice->compare_reg =
+		    me8200_regbase + ME8200_DI_COMPARE_0_REG;
+		subdevice->irq_status_reg =
+		    me8200_regbase + ME8200_DI_CHANGE_0_REG;
+
+		subdevice->irq_status_low_reg =
+		    me8200_regbase + ME8200_DI_EXTEND_CHANGE_0_LOW_REG;
+		subdevice->irq_status_high_reg =
+		    me8200_regbase + ME8200_DI_EXTEND_CHANGE_0_HIGH_REG;
+	} else if (di_idx == 1) {
+		subdevice->port_reg = me8200_regbase + ME8200_DI_PORT_1_REG;
+		subdevice->mask_reg = me8200_regbase + ME8200_DI_MASK_1_REG;
+		subdevice->compare_reg =
+		    me8200_regbase + ME8200_DI_COMPARE_1_REG;
+		subdevice->irq_status_reg =
+		    me8200_regbase + ME8200_DI_CHANGE_1_REG;
+
+		subdevice->irq_status_low_reg =
+		    me8200_regbase + ME8200_DI_EXTEND_CHANGE_1_LOW_REG;
+		subdevice->irq_status_high_reg =
+		    me8200_regbase + ME8200_DI_EXTEND_CHANGE_1_HIGH_REG;
+	} else {
+		PERROR("Wrong subdevice idx=%d.\n", di_idx);
+		kfree(subdevice);
+		return NULL;
+	}
+	subdevice->irq_ctrl_reg = me8200_regbase + ME8200_DI_IRQ_CTRL_REG;
+	subdevice->irq_mode_reg = me8200_regbase + ME8200_IRQ_MODE_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = me8200_regbase;
+#endif
+
+	/* Initialize wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_irq_start = me8200_di_io_irq_start;
+	subdevice->base.me_subdevice_io_irq_wait = me8200_di_io_irq_wait;
+	subdevice->base.me_subdevice_io_irq_stop = me8200_di_io_irq_stop;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me8200_di_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me8200_di_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me8200_di_io_single_read;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me8200_di_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me8200_di_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me8200_di_query_subdevice_caps;
+	subdevice->base.me_subdevice_destructor = me8200_di_destructor;
+
+	subdevice->rised = 0;
+	subdevice->count = 0;
+
+	/* Register interrupt service routine. */
+	subdevice->irq = irq;
+	if (subdevice->version > 0) {	// NEW
+		err = request_irq(subdevice->irq, me8200_isr_EX,
+#ifdef IRQF_DISABLED
+				  IRQF_DISABLED | IRQF_SHARED,
+#else
+				  SA_INTERRUPT | SA_SHIRQ,
+#endif
+				  ME8200_NAME, (void *)subdevice);
+	} else {		//OLD
+		err = request_irq(subdevice->irq, me8200_isr,
+#ifdef IRQF_DISABLED
+				  IRQF_DISABLED | IRQF_SHARED,
+#else
+				  SA_INTERRUPT | SA_SHIRQ,
+#endif
+				  ME8200_NAME, (void *)subdevice);
+	}
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	PDEBUG("Registred irq=%d.\n", subdevice->irq);
+
+	return subdevice;
+}
+
+static void me8200_di_check_version(me8200_di_subdevice_t * instance,
+				    unsigned long addr)
+{
+
+	PDEBUG("executed.\n");
+	instance->version = 0x000000FF & inb(addr);
+	PDEVELOP("me8200 firmware version: %d\n", instance->version);
+
+	/// @note Fix for wrong values in this registry.
+	if ((instance->version < 0x7) || (instance->version > 0x1F))
+		instance->version = 0x0;
+}
diff --git a/drivers/staging/meilhaus/me8200_di.h b/drivers/staging/meilhaus/me8200_di.h
new file mode 100644
index 000000000000..2a3b005b67d4
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_di.h
@@ -0,0 +1,92 @@
+/**
+ * @file me8200_di.h
+ *
+ * @brief ME-8200 digital input subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_DI_H_
+#define _ME8200_DI_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me8200_di_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;
+	spinlock_t *irq_ctrl_lock;
+	spinlock_t *irq_mode_lock;
+
+	unsigned int di_idx;
+	unsigned int version;
+
+	int irq;						/**< The number of the interrupt request. */
+	volatile int rised;				/**< Flag to indicate if an interrupt occured. */
+	uint status_flag;				/**< Default interupt status flag */
+	uint status_value;				/**< Interupt status */
+	uint status_value_edges;			/**< Extended interupt status */
+	uint line_value;
+	int count;						/**< Counts the number of interrupts occured. */
+	uint8_t compare_value;
+	uint8_t filtering_flag;
+
+	wait_queue_head_t wait_queue;	/**< To wait on interrupts. */
+
+	unsigned long port_reg;			/**< The digital input port. */
+	unsigned long compare_reg;		/**< The register to hold the value to compare with. */
+	unsigned long mask_reg;			/**< The register to hold the mask. */
+	unsigned long irq_mode_reg;		/**< The interrupt mode register. */
+	unsigned long irq_ctrl_reg;		/**< The interrupt control register. */
+	unsigned long irq_status_reg;	/**< The interrupt status register. Also interrupt reseting register (firmware version 7 and later).*/
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+	unsigned long firmware_version_reg;	/**< The interrupt reseting register. */
+
+	unsigned long irq_status_low_reg;	/**< The interrupt extended status register (low part). */
+	unsigned long irq_status_high_reg;	/**< The interrupt extended status register (high part). */
+} me8200_di_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-8200 digital input subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me8200_di_subdevice_t *me8200_di_constructor(uint32_t me8200_reg_base,
+					     unsigned int di_idx,
+					     int irq,
+					     spinlock_t * irq_ctrl_lock,
+					     spinlock_t * irq_mode_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_di_reg.h b/drivers/staging/meilhaus/me8200_di_reg.h
new file mode 100644
index 000000000000..b9a619d31c2c
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_di_reg.h
@@ -0,0 +1,75 @@
+/**
+ * @file me8200_di_reg.h
+ *
+ * @brief ME-8200 digital input subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_DI_REG_H_
+#define _ME8200_DI_REG_H_
+
+#ifdef __KERNEL__
+
+// Common registry for whole family.
+#define ME8200_DI_PORT_0_REG					0x3	// R
+#define ME8200_DI_PORT_1_REG					0x4	// R
+
+#define ME8200_DI_MASK_0_REG					0x5	// R/W
+#define ME8200_DI_MASK_1_REG					0x6	// R/W
+
+#define ME8200_DI_COMPARE_0_REG					0xA	// R/W
+#define ME8200_DI_COMPARE_1_REG					0xB	// R/W
+
+#define ME8200_DI_IRQ_CTRL_REG					0xC	// R/W
+
+#ifndef ME8200_IRQ_MODE_REG
+# define ME8200_IRQ_MODE_REG					0xD	// R/W
+#endif
+
+// This registry are for all versions
+#define ME8200_DI_CHANGE_0_REG					0xE	// R
+#define ME8200_DI_CHANGE_1_REG					0xF	// R
+
+#define ME8200_DI_IRQ_CTRL_BIT_CLEAR			0x4
+#define ME8200_DI_IRQ_CTRL_BIT_ENABLE			0x8
+
+// This registry are for firmware versions 7 and later
+#define ME8200_DI_EXTEND_CHANGE_0_LOW_REG		0x10	// R
+#define ME8200_DI_EXTEND_CHANGE_0_HIGH_REG		0x11	// R
+#define ME8200_DI_EXTEND_CHANGE_1_LOW_REG		0x12	// R
+#define ME8200_DI_EXTEND_CHANGE_1_HIGH_REG		0x13	// R
+
+#ifndef ME8200_FIRMWARE_VERSION_REG
+# define ME8200_FIRMWARE_VERSION_REG			0x14	// R
+#endif
+
+// Bit definitions
+#define ME8200_DI_IRQ_CTRL_MASK_EDGE			0x3
+#define ME8200_DI_IRQ_CTRL_MASK_EDGE_RISING		0x0
+#define ME8200_DI_IRQ_CTRL_MASK_EDGE_FALLING	0x1
+#define ME8200_DI_IRQ_CTRL_MASK_EDGE_ANY		0x3
+
+// Others
+#define ME8200_DI_IRQ_CTRL_SHIFT				4
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_dio.c b/drivers/staging/meilhaus/me8200_dio.c
new file mode 100644
index 000000000000..ff8ca1b8b7f3
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_dio.c
@@ -0,0 +1,418 @@
+/**
+ * @file me8200_dio.c
+ *
+ * @brief ME-8200 digital input/output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me8200_dio_reg.h"
+#include "me8200_dio.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me8200_dio_io_reset_subdevice(struct me_subdevice *subdevice,
+					 struct file *filep, int flags)
+{
+	me8200_dio_subdevice_t *instance;
+	uint8_t mode;
+
+	PDEBUG("executed.\n");
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	instance = (me8200_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inb(instance->ctrl_reg);
+	mode &= ~(0x3 << (instance->dio_idx * 2));
+	outb(mode, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, mode);
+	spin_unlock(instance->ctrl_reg_lock);
+	outb(0x00, instance->port_reg);
+	PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0x00);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_dio_io_single_config(me_subdevice_t * subdevice,
+				       struct file *filep,
+				       int channel,
+				       int single_config,
+				       int ref,
+				       int trig_chan,
+				       int trig_type, int trig_edge, int flags)
+{
+	me8200_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t mode;
+	uint32_t size =
+	    flags & (ME_IO_SINGLE_CONFIG_DIO_BIT | ME_IO_SINGLE_CONFIG_DIO_BYTE
+		     | ME_IO_SINGLE_CONFIG_DIO_WORD |
+		     ME_IO_SINGLE_CONFIG_DIO_DWORD);
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inb(instance->ctrl_reg);
+	switch (size) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+				mode &=
+				    ~((ME8200_DIO_CTRL_BIT_MODE_0 |
+				       ME8200_DIO_CTRL_BIT_MODE_1) <<
+				      (instance->dio_idx * 2));
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+				mode &=
+				    ~((ME8200_DIO_CTRL_BIT_MODE_0 |
+				       ME8200_DIO_CTRL_BIT_MODE_1) <<
+				      (instance->dio_idx * 2));
+				mode |=
+				    ME8200_DIO_CTRL_BIT_MODE_0 << (instance->
+								   dio_idx * 2);
+			} else {
+				PERROR
+				    ("Invalid port configuration specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!err) {
+		outb(mode, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, mode);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_dio_io_single_read(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int *value, int time_out, int flags)
+{
+	me8200_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME8200_DIO_CTRL_BIT_MODE_0 |
+					      ME8200_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if ((mode ==
+			     (ME8200_DIO_CTRL_BIT_MODE_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value =
+				    inb(instance->
+					port_reg) & (0x0001 << channel);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME8200_DIO_CTRL_BIT_MODE_0 |
+					      ME8200_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if ((mode ==
+			     (ME8200_DIO_CTRL_BIT_MODE_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value = inb(instance->port_reg) & 0x00FF;
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_dio_io_single_write(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int value, int time_out, int flags)
+{
+	me8200_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+	uint8_t byte;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME8200_DIO_CTRL_BIT_MODE_0 |
+					      ME8200_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME8200_DIO_CTRL_BIT_MODE_0 <<
+			     (instance->dio_idx * 2))) {
+				byte = inb(instance->port_reg);
+
+				if (value)
+					byte |= 0x1 << channel;
+				else
+					byte &= ~(0x1 << channel);
+
+				outb(byte, instance->port_reg);
+				PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->port_reg -
+					   instance->reg_base, byte);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME8200_DIO_CTRL_BIT_MODE_0 |
+					      ME8200_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME8200_DIO_CTRL_BIT_MODE_0 <<
+			     (instance->dio_idx * 2))) {
+				outb(value, instance->port_reg);
+				PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->port_reg -
+					   instance->reg_base, value);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_dio_query_number_channels(me_subdevice_t * subdevice,
+					    int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_dio_query_subdevice_type(me_subdevice_t * subdevice,
+					   int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DIO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_dio_query_subdevice_caps(me_subdevice_t * subdevice,
+					   int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_DIR_BYTE;
+	return ME_ERRNO_SUCCESS;
+}
+
+me8200_dio_subdevice_t *me8200_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock)
+{
+	me8200_dio_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me8200_dio_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me8200_dio_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save digital i/o index */
+	subdevice->dio_idx = dio_idx;
+
+	/* Save the subdevice index */
+	subdevice->ctrl_reg = reg_base + ME8200_DIO_CTRL_REG;
+	subdevice->port_reg = reg_base + ME8200_DIO_PORT_REG + dio_idx;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me8200_dio_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me8200_dio_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me8200_dio_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me8200_dio_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me8200_dio_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me8200_dio_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me8200_dio_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me8200_dio.h b/drivers/staging/meilhaus/me8200_dio.h
new file mode 100644
index 000000000000..9ddd93d26f15
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_dio.h
@@ -0,0 +1,68 @@
+/**
+ * @file me8200_dio.h
+ *
+ * @brief ME-8200 digital input/output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_DIO_H_
+#define _ME8200_DIO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me8200_dio_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+	unsigned int dio_idx;			/**< The index of the digital i/o on the device. */
+
+	unsigned long port_reg;			/**< Register holding the port status. */
+	unsigned long ctrl_reg;			/**< Register to configure the port direction. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me8200_dio_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-8200 digital input/ouput subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param dio_idx The index of the digital i/o port on the device.
+ * @param ctrl_reg_lock Spin lock protecting the control register.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me8200_dio_subdevice_t *me8200_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_dio_reg.h b/drivers/staging/meilhaus/me8200_dio_reg.h
new file mode 100644
index 000000000000..ac94a133abaf
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_dio_reg.h
@@ -0,0 +1,43 @@
+/**
+ * @file me8200_dio_reg.h
+ *
+ * @brief ME-8200 digital input/output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_DIO_REG_H_
+#define _ME8200_DIO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME8200_DIO_CTRL_REG					0x7	// R/W
+#define ME8200_DIO_PORT_0_REG				0x8	// R/W
+#define ME8200_DIO_PORT_1_REG				0x9	// R/W
+#define ME8200_DIO_PORT_REG					ME8200_DIO_PORT_0_REG	// R/W
+
+#define ME8200_DIO_CTRL_BIT_MODE_0			0x01
+#define ME8200_DIO_CTRL_BIT_MODE_1			0x02
+#define ME8200_DIO_CTRL_BIT_MODE_2			0x04
+#define ME8200_DIO_CTRL_BIT_MODE_3			0x08
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_do.c b/drivers/staging/meilhaus/me8200_do.c
new file mode 100644
index 000000000000..d2bebd16ff41
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_do.c
@@ -0,0 +1,600 @@
+/**
+ * @file me8200_do.c
+ *
+ * @brief ME-8200 digital output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/version.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "meids.h"
+#include "medebug.h"
+#include "me8200_reg.h"
+#include "me8200_do_reg.h"
+#include "me8200_do.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me8200_do_io_irq_start(me_subdevice_t * subdevice,
+				  struct file *filep,
+				  int channel,
+				  int irq_source,
+				  int irq_edge, int irq_arg, int flags)
+{
+	me8200_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t tmp;
+	unsigned long status;
+
+	if (flags & ~ME_IO_IRQ_START_DIO_BYTE) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel != 0) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (irq_source != ME_IRQ_SOURCE_DIO_OVER_TEMP) {
+		PERROR("Invalid interrupt source specified.\n");
+		return ME_ERRNO_INVALID_IRQ_SOURCE;
+	}
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, status);
+	spin_lock(instance->irq_mode_lock);
+	tmp = inb(instance->irq_ctrl_reg);
+	tmp |=
+	    ME8200_IRQ_MODE_BIT_ENABLE_POWER << (ME8200_IRQ_MODE_POWER_SHIFT *
+						 instance->do_idx);
+	outb(tmp, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->irq_mode_lock);
+	instance->rised = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_do_io_irq_wait(me_subdevice_t * subdevice,
+				 struct file *filep,
+				 int channel,
+				 int *irq_count,
+				 int *value, int time_out, int flags)
+{
+	me8200_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->rised <= 0) {
+		instance->rised = 0;
+
+		if (time_out) {
+			t = wait_event_interruptible_timeout(instance->
+							     wait_queue,
+							     (instance->rised !=
+							      0), t);
+
+			if (t == 0) {
+				PERROR
+				    ("Wait on external interrupt timed out.\n");
+				err = ME_ERRNO_TIMEOUT;
+			}
+		} else {
+			wait_event_interruptible(instance->wait_queue,
+						 (instance->rised != 0));
+		}
+
+		if (instance->rised < 0) {
+			PERROR("Wait on interrupt aborted by user.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+	}
+
+	if (signal_pending(current)) {
+		PERROR("Wait on external interrupt aborted by signal.\n");
+		err = ME_ERRNO_SIGNAL;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	instance->rised = 0;
+	*irq_count = instance->count;
+	*value = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_do_io_irq_stop(me_subdevice_t * subdevice,
+				 struct file *filep, int channel, int flags)
+{
+	me8200_do_subdevice_t *instance;
+	uint8_t tmp;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->irq_mode_lock);
+	tmp = inb(instance->irq_ctrl_reg);
+	tmp &=
+	    ~(ME8200_IRQ_MODE_BIT_ENABLE_POWER <<
+	      (ME8200_IRQ_MODE_POWER_SHIFT * instance->do_idx));
+	outb(tmp, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->irq_mode_lock);
+	instance->rised = -1;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_do_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	me8200_do_subdevice_t *instance;
+	unsigned long cpu_flags;
+	uint8_t tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	outb(0x00, instance->port_reg);
+	PDEBUG_REG("port_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0x00);
+	spin_lock(instance->irq_mode_lock);
+	tmp = inb(instance->irq_ctrl_reg);
+	tmp &=
+	    ~(ME8200_IRQ_MODE_BIT_ENABLE_POWER <<
+	      (ME8200_IRQ_MODE_POWER_SHIFT * instance->do_idx));
+	outb(tmp, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->irq_mode_lock);
+	instance->rised = -1;
+	instance->count = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_do_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me8200_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, status);
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+			} else {
+				PERROR("Invalid byte direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_do_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me8200_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, status);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = inb(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inb(instance->port_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_do_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me8200_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t state;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, status);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			state = inb(instance->port_reg);
+			state =
+			    value ? (state | (0x1 << channel)) : (state &
+								  ~(0x1 <<
+								    channel));
+			outb(state, instance->port_reg);
+			PDEBUG_REG("port_reg outb(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->port_reg - instance->reg_base,
+				   state);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			outb(value, instance->port_reg);
+			PDEBUG_REG("port_reg outb(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->port_reg - instance->reg_base,
+				   value);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_do_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_do_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_do_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_OVER_TEMP_IRQ;
+	return ME_ERRNO_SUCCESS;
+}
+
+static void me8200_do_destructor(struct me_subdevice *subdevice)
+{
+	me8200_do_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	free_irq(instance->irq, (void *)instance);
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me8200_do_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me8200_do_isr(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{
+	me8200_do_subdevice_t *instance;
+	uint16_t ctrl;
+	uint8_t irq_status;
+
+	instance = (me8200_do_subdevice_t *) dev_id;
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	irq_status = inb(instance->irq_status_reg);
+	if (!
+	    (irq_status &
+	     (ME8200_DO_IRQ_STATUS_BIT_ACTIVE << instance->do_idx))) {
+		PINFO
+		    ("%ld Shared interrupt. %s(): idx=%d irq_status_reg=0x%04X\n",
+		     jiffies, __func__, instance->do_idx, irq_status);
+		return IRQ_NONE;
+	}
+
+	PDEBUG("executed.\n");
+
+	spin_lock(&instance->subdevice_lock);
+	instance->rised = 1;
+	instance->count++;
+
+	spin_lock(instance->irq_mode_lock);
+	ctrl = inw(instance->irq_ctrl_reg);
+	ctrl |= ME8200_IRQ_MODE_BIT_CLEAR_POWER << instance->do_idx;
+	outw(ctrl, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, ctrl);
+	ctrl &= ~(ME8200_IRQ_MODE_BIT_CLEAR_POWER << instance->do_idx);
+	outw(ctrl, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock(instance->irq_mode_lock);
+	spin_unlock(&instance->subdevice_lock);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+me8200_do_subdevice_t *me8200_do_constructor(uint32_t reg_base,
+					     unsigned int do_idx,
+					     int irq,
+					     spinlock_t * irq_mode_lock)
+{
+	me8200_do_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me8200_do_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me8200_do_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->irq_mode_lock = irq_mode_lock;
+
+	/* Save the index of the digital output */
+	subdevice->do_idx = do_idx;
+	subdevice->irq = irq;
+
+	/* Initialize the registers */
+	if (do_idx == 0) {
+		subdevice->port_reg = reg_base + ME8200_DO_PORT_0_REG;
+	} else if (do_idx == 1) {
+		subdevice->port_reg = reg_base + ME8200_DO_PORT_1_REG;
+	} else {
+		PERROR("Wrong subdevice idx=%d.\n", do_idx);
+		kfree(subdevice);
+		return NULL;
+	}
+	subdevice->irq_ctrl_reg = reg_base + ME8200_IRQ_MODE_REG;
+	subdevice->irq_status_reg = reg_base + ME8200_DO_IRQ_STATUS_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Initialize the wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	/* Request the interrupt line */
+	err = request_irq(irq, me8200_do_isr,
+#ifdef IRQF_DISABLED
+			  IRQF_DISABLED | IRQF_SHARED,
+#else
+			  SA_INTERRUPT | SA_SHIRQ,
+#endif
+			  ME8200_NAME, (void *)subdevice);
+
+	if (err) {
+		PERROR("Cannot get interrupt line.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	PINFO("Registered irq=%d.\n", irq);
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_irq_start = me8200_do_io_irq_start;
+	subdevice->base.me_subdevice_io_irq_wait = me8200_do_io_irq_wait;
+	subdevice->base.me_subdevice_io_irq_stop = me8200_do_io_irq_stop;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me8200_do_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me8200_do_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me8200_do_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me8200_do_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me8200_do_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me8200_do_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me8200_do_query_subdevice_caps;
+	subdevice->base.me_subdevice_destructor = me8200_do_destructor;
+
+	subdevice->rised = 0;
+	subdevice->count = 0;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me8200_do.h b/drivers/staging/meilhaus/me8200_do.h
new file mode 100644
index 000000000000..27581251c847
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_do.h
@@ -0,0 +1,75 @@
+/**
+ * @file me8200_do.h
+ *
+ * @brief ME-8200 digital output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_DO_H_
+#define _ME8200_DO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me8200_do_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *irq_mode_lock;
+
+	int irq;				/**< The number of the interrupt request */
+	int rised;				/**< Flag to indicate if an interrupt occured */
+	int count;				/**< Counts the number of interrupts occured */
+	wait_queue_head_t wait_queue;		/**< To wait on interrupts */
+
+	unsigned int do_idx;			/**< The number of the digital output */
+
+	unsigned long port_reg;			/**< The digital output port */
+	unsigned long irq_ctrl_reg;		/**< The interrupt control register */
+	unsigned long irq_status_reg;		/**< The interrupt status register */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me8200_do_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-8200 digital output subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param do_idx The index of the digital output subdevice on this device.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me8200_do_subdevice_t *me8200_do_constructor(uint32_t reg_base,
+					     unsigned int do_idx,
+					     int irq,
+					     spinlock_t * irq_mode_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_do_reg.h b/drivers/staging/meilhaus/me8200_do_reg.h
new file mode 100644
index 000000000000..41095046037a
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_do_reg.h
@@ -0,0 +1,40 @@
+/**
+ * @file me8200_ao_reg.h
+ *
+ * @brief ME-8200 analog output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_DO_REG_H_
+#define _ME8200_DO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME8200_DO_IRQ_STATUS_REG			0x0	// R
+#define ME8200_DO_PORT_0_REG				0x1	// R/W
+#define ME8200_DO_PORT_1_REG				0x2	// R/W
+
+#define ME8200_DO_IRQ_STATUS_BIT_ACTIVE		0x1
+#define ME8200_DO_IRQ_STATUS_SHIFT			1
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_reg.h b/drivers/staging/meilhaus/me8200_reg.h
new file mode 100644
index 000000000000..a73fe4d5b0ff
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_reg.h
@@ -0,0 +1,46 @@
+/**
+ * @file me8200_reg.h
+ *
+ * @brief ME-8200 register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_REG_H_
+#define _ME8200_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME8200_IRQ_MODE_REG				0xD	// R/W
+
+#define ME8200_IRQ_MODE_MASK     			0x3
+
+#define ME8200_IRQ_MODE_MASK_MASK			0x0
+#define ME8200_IRQ_MODE_MASK_COMPARE			0x1
+
+#define ME8200_IRQ_MODE_BIT_ENABLE_POWER		0x10
+#define ME8200_IRQ_MODE_BIT_CLEAR_POWER			0x40
+
+#define ME8200_IRQ_MODE_DI_SHIFT			2
+#define ME8200_IRQ_MODE_POWER_SHIFT			1
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8254.c b/drivers/staging/meilhaus/me8254.c
new file mode 100644
index 000000000000..6e44c3d7a0c7
--- /dev/null
+++ b/drivers/staging/meilhaus/me8254.c
@@ -0,0 +1,1176 @@
+/**
+ * @file me8254.c
+ *
+ * @brief 8254 subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me8254_reg.h"
+#include "me8254.h"
+
+/*
+ * Defines
+ */
+#define ME8254_NUMBER_CHANNELS 1	/**< One channel per counter. */
+#define ME8254_CTR_WIDTH 16			/**< One counter has 16 bits. */
+
+/*
+ * Functions
+ */
+
+static int me8254_io_reset_subdevice(struct me_subdevice *subdevice,
+				     struct file *filep, int flags)
+{
+	me8254_subdevice_t *instance;
+	uint8_t clk_src;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8254_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	if (instance->ctr_idx == 0)
+		outb(ME8254_CTRL_SC0 | ME8254_CTRL_LM | ME8254_CTRL_M0 |
+		     ME8254_CTRL_BIN, instance->ctrl_reg);
+	else if (instance->ctr_idx == 1)
+		outb(ME8254_CTRL_SC1 | ME8254_CTRL_LM | ME8254_CTRL_M0 |
+		     ME8254_CTRL_BIN, instance->ctrl_reg);
+	else
+		outb(ME8254_CTRL_SC2 | ME8254_CTRL_LM | ME8254_CTRL_M0 |
+		     ME8254_CTRL_BIN, instance->ctrl_reg);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outb(0x00, instance->val_reg);
+	outb(0x00, instance->val_reg);
+
+	spin_lock(instance->clk_src_reg_lock);
+	clk_src = inb(instance->clk_src_reg);
+
+	switch (instance->device_id) {
+	case PCI_DEVICE_ID_MEILHAUS_ME1400:
+	case PCI_DEVICE_ID_MEILHAUS_ME140A:
+	case PCI_DEVICE_ID_MEILHAUS_ME140B:
+	case PCI_DEVICE_ID_MEILHAUS_ME14E0:
+	case PCI_DEVICE_ID_MEILHAUS_ME14EA:
+	case PCI_DEVICE_ID_MEILHAUS_ME14EB:
+		if (instance->me8254_idx == 0) {
+			if (instance->ctr_idx == 0)
+				clk_src &=
+				    ~(ME1400AB_8254_A_0_CLK_SRC_10MHZ |
+				      ME1400AB_8254_A_0_CLK_SRC_QUARZ);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400AB_8254_A_1_CLK_SRC_PREV);
+			else
+				clk_src &= ~(ME1400AB_8254_A_2_CLK_SRC_PREV);
+		} else {
+			if (instance->ctr_idx == 0)
+				clk_src &=
+				    ~(ME1400AB_8254_B_0_CLK_SRC_10MHZ |
+				      ME1400AB_8254_B_0_CLK_SRC_QUARZ);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400AB_8254_B_1_CLK_SRC_PREV);
+			else
+				clk_src &= ~(ME1400AB_8254_B_2_CLK_SRC_PREV);
+		}
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		switch (instance->me8254_idx) {
+		case 0:
+		case 2:
+		case 4:
+		case 6:
+		case 8:
+			if (instance->ctr_idx == 0)
+				clk_src &= ~(ME1400CD_8254_ACE_0_CLK_SRC_MASK);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400CD_8254_ACE_1_CLK_SRC_MASK);
+			else
+				clk_src &= ~(ME1400CD_8254_ACE_2_CLK_SRC_MASK);
+			break;
+
+		default:
+			if (instance->ctr_idx == 0)
+				clk_src &= ~(ME1400CD_8254_BD_0_CLK_SRC_MASK);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400CD_8254_BD_1_CLK_SRC_MASK);
+			else
+				clk_src &= ~(ME1400CD_8254_BD_2_CLK_SRC_MASK);
+			break;
+		}
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4610:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660IS:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670IS:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680IS:
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_A:
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_B:
+
+		/* No clock source register available */
+		break;
+
+	default:
+		PERROR("Invalid device type.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	if (!err)
+		outb(clk_src, instance->clk_src_reg);
+
+	spin_unlock(instance->clk_src_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me1400_ab_ref_config(me8254_subdevice_t * instance, int ref)
+{
+	uint8_t clk_src;
+
+	spin_lock(instance->clk_src_reg_lock);
+	clk_src = inb(instance->clk_src_reg);
+
+	switch (ref) {
+	case ME_REF_CTR_EXTERNAL:
+		if (instance->me8254_idx == 0) {
+			if (instance->ctr_idx == 0)
+				clk_src &= ~(ME1400AB_8254_A_0_CLK_SRC_QUARZ);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400AB_8254_A_1_CLK_SRC_PREV);
+			else
+				clk_src &= ~(ME1400AB_8254_A_2_CLK_SRC_PREV);
+		} else {
+			if (instance->ctr_idx == 0)
+				clk_src &= ~(ME1400AB_8254_B_0_CLK_SRC_QUARZ);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400AB_8254_B_1_CLK_SRC_PREV);
+			else
+				clk_src &= ~(ME1400AB_8254_B_2_CLK_SRC_PREV);
+		}
+
+		break;
+
+	case ME_REF_CTR_PREVIOUS:
+		if (instance->me8254_idx == 0) {
+			if (instance->ctr_idx == 0) {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_SINGLE_CONFIG;
+			} else if (instance->ctr_idx == 1)
+				clk_src |= (ME1400AB_8254_A_1_CLK_SRC_PREV);
+			else
+				clk_src |= (ME1400AB_8254_A_2_CLK_SRC_PREV);
+		} else {
+			if (instance->ctr_idx == 0) {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_SINGLE_CONFIG;
+			} else if (instance->ctr_idx == 1)
+				clk_src |= (ME1400AB_8254_B_1_CLK_SRC_PREV);
+			else
+				clk_src |= (ME1400AB_8254_B_2_CLK_SRC_PREV);
+		}
+
+		break;
+
+	case ME_REF_CTR_INTERNAL_1MHZ:
+		if (instance->me8254_idx == 0) {
+			if (instance->ctr_idx == 0) {
+				clk_src |= (ME1400AB_8254_A_0_CLK_SRC_QUARZ);
+				clk_src &= ~(ME1400AB_8254_A_0_CLK_SRC_10MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			if (instance->ctr_idx == 0) {
+				clk_src |= (ME1400AB_8254_B_0_CLK_SRC_QUARZ);
+				clk_src &= ~(ME1400AB_8254_B_0_CLK_SRC_10MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		}
+
+		break;
+
+	case ME_REF_CTR_INTERNAL_10MHZ:
+		if (instance->me8254_idx == 0) {
+			if (instance->ctr_idx == 0) {
+				clk_src |= (ME1400AB_8254_A_0_CLK_SRC_QUARZ);
+				clk_src |= (ME1400AB_8254_A_0_CLK_SRC_10MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			if (instance->ctr_idx == 0) {
+				clk_src |= (ME1400AB_8254_A_0_CLK_SRC_QUARZ);
+				clk_src |= (ME1400AB_8254_A_0_CLK_SRC_10MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid reference.\n");
+		spin_unlock(instance->clk_src_reg_lock);
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	outb(clk_src, instance->clk_src_reg);
+	spin_unlock(instance->clk_src_reg_lock);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1400_cd_ref_config(me8254_subdevice_t * instance, int ref)
+{
+	uint8_t clk_src;
+
+	spin_lock(instance->clk_src_reg_lock);
+	clk_src = inb(instance->clk_src_reg);
+
+	switch (ref) {
+	case ME_REF_CTR_EXTERNAL:
+		switch (instance->me8254_idx) {
+		case 0:
+		case 2:
+		case 4:
+		case 6:
+		case 8:
+			if (instance->ctr_idx == 0)
+				clk_src &= ~(ME1400CD_8254_ACE_0_CLK_SRC_MASK);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400CD_8254_ACE_1_CLK_SRC_MASK);
+			else
+				clk_src &= ~(ME1400CD_8254_ACE_2_CLK_SRC_MASK);
+			break;
+
+		default:
+			if (instance->ctr_idx == 0)
+				clk_src &= ~(ME1400CD_8254_BD_0_CLK_SRC_MASK);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400CD_8254_BD_1_CLK_SRC_MASK);
+			else
+				clk_src &= ~(ME1400CD_8254_BD_2_CLK_SRC_MASK);
+			break;
+		}
+		break;
+
+	case ME_REF_CTR_PREVIOUS:
+		switch (instance->me8254_idx) {
+		case 0:
+		case 2:
+		case 4:
+		case 6:
+		case 8:
+			if (instance->ctr_idx == 0) {
+				clk_src &= ~(ME1400CD_8254_ACE_0_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_ACE_0_CLK_SRC_PREV);
+			} else if (instance->ctr_idx == 1) {
+				clk_src &= ~(ME1400CD_8254_ACE_1_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_ACE_1_CLK_SRC_PREV);
+			} else {
+				clk_src &= ~(ME1400CD_8254_ACE_2_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_ACE_2_CLK_SRC_PREV);
+			}
+			break;
+
+		default:
+			if (instance->ctr_idx == 0) {
+				clk_src &= ~(ME1400CD_8254_BD_0_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_BD_0_CLK_SRC_PREV);
+			} else if (instance->ctr_idx == 1) {
+				clk_src &= ~(ME1400CD_8254_BD_1_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_BD_1_CLK_SRC_PREV);
+			} else {
+				clk_src &= ~(ME1400CD_8254_BD_2_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_BD_2_CLK_SRC_PREV);
+			}
+			break;
+		}
+
+		break;
+
+	case ME_REF_CTR_INTERNAL_1MHZ:
+		switch (instance->me8254_idx) {
+		case 0:
+		case 2:
+		case 4:
+		case 6:
+		case 8:
+			if (instance->ctr_idx == 0) {
+				clk_src &= ~(ME1400CD_8254_ACE_0_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_ACE_0_CLK_SRC_1MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_REF;
+			}
+
+			break;
+
+		default:
+			if (instance->ctr_idx == 0) {
+				clk_src &= ~(ME1400CD_8254_BD_0_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_BD_0_CLK_SRC_1MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_REF;
+			}
+			break;
+		}
+
+		break;
+
+	case ME_REF_CTR_INTERNAL_10MHZ:
+		switch (instance->me8254_idx) {
+		case 0:
+		case 2:
+		case 4:
+		case 6:
+		case 8:
+			if (instance->ctr_idx == 0) {
+				clk_src &= ~(ME1400CD_8254_ACE_0_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_ACE_0_CLK_SRC_10MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_REF;
+			}
+			break;
+
+		default:
+			if (instance->ctr_idx == 0) {
+				clk_src &= ~(ME1400CD_8254_BD_0_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_BD_0_CLK_SRC_10MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_REF;
+			}
+
+			break;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid reference.\n");
+		spin_unlock(instance->clk_src_reg_lock);
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	outb(clk_src, instance->clk_src_reg);
+	spin_unlock(instance->clk_src_reg_lock);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ref_config(me8254_subdevice_t * instance, int ref)
+{
+	switch (ref) {
+
+	case ME_REF_CTR_EXTERNAL:
+		// Nothing to do
+		break;
+
+	default:
+		PERROR("Invalid reference.\n");
+//                      spin_unlock(instance->clk_src_reg_lock);
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_ref_config(me8254_subdevice_t * instance, int ref)
+{
+	switch (ref) {
+
+	case ME_REF_CTR_EXTERNAL:
+		// Nothing to do
+		break;
+
+	default:
+		PERROR("Invalid reference.\n");
+//                      spin_unlock(instance->clk_src_reg_lock);
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8254_io_single_config(struct me_subdevice *subdevice,
+				   struct file *filep,
+				   int channel,
+				   int single_config,
+				   int ref,
+				   int trig_chan,
+				   int trig_type, int trig_edge, int flags)
+{
+	me8254_subdevice_t *instance;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	if (channel) {
+		PERROR("Invalid channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	instance = (me8254_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	// Configure the counter modes
+	if (instance->ctr_idx == 0) {
+		if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_0) {
+			outb(ME8254_CTRL_SC0 | ME8254_CTRL_LM | ME8254_CTRL_M0 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_1) {
+			outb(ME8254_CTRL_SC0 | ME8254_CTRL_LM | ME8254_CTRL_M1 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_2) {
+			outb(ME8254_CTRL_SC0 | ME8254_CTRL_LM | ME8254_CTRL_M2 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_3) {
+			outb(ME8254_CTRL_SC0 | ME8254_CTRL_LM | ME8254_CTRL_M3 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_4) {
+			outb(ME8254_CTRL_SC0 | ME8254_CTRL_LM | ME8254_CTRL_M4 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_5) {
+			outb(ME8254_CTRL_SC0 | ME8254_CTRL_LM | ME8254_CTRL_M5 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else {
+			PERROR("Invalid single configuration.\n");
+			spin_unlock(&instance->subdevice_lock);
+			return ME_ERRNO_INVALID_SINGLE_CONFIG;
+		}
+	} else if (instance->ctr_idx == 1) {
+		if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_0) {
+			outb(ME8254_CTRL_SC1 | ME8254_CTRL_LM | ME8254_CTRL_M0 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_1) {
+			outb(ME8254_CTRL_SC1 | ME8254_CTRL_LM | ME8254_CTRL_M1 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_2) {
+			outb(ME8254_CTRL_SC1 | ME8254_CTRL_LM | ME8254_CTRL_M2 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_3) {
+			outb(ME8254_CTRL_SC1 | ME8254_CTRL_LM | ME8254_CTRL_M3 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_4) {
+			outb(ME8254_CTRL_SC1 | ME8254_CTRL_LM | ME8254_CTRL_M4 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_5) {
+			outb(ME8254_CTRL_SC1 | ME8254_CTRL_LM | ME8254_CTRL_M5 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else {
+			PERROR("Invalid single configuration.\n");
+			spin_unlock(&instance->subdevice_lock);
+			return ME_ERRNO_INVALID_SINGLE_CONFIG;
+		}
+	} else {
+		if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_0) {
+			outb(ME8254_CTRL_SC2 | ME8254_CTRL_LM | ME8254_CTRL_M0 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_1) {
+			outb(ME8254_CTRL_SC2 | ME8254_CTRL_LM | ME8254_CTRL_M1 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_2) {
+			outb(ME8254_CTRL_SC2 | ME8254_CTRL_LM | ME8254_CTRL_M2 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_3) {
+			outb(ME8254_CTRL_SC2 | ME8254_CTRL_LM | ME8254_CTRL_M3 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_4) {
+			outb(ME8254_CTRL_SC2 | ME8254_CTRL_LM | ME8254_CTRL_M4 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_5) {
+			outb(ME8254_CTRL_SC2 | ME8254_CTRL_LM | ME8254_CTRL_M5 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else {
+			PERROR("Invalid single configuration.\n");
+			spin_unlock(&instance->subdevice_lock);
+			return ME_ERRNO_INVALID_SINGLE_CONFIG;
+		}
+	}
+
+	switch (instance->device_id) {
+	case PCI_DEVICE_ID_MEILHAUS_ME1400:
+	case PCI_DEVICE_ID_MEILHAUS_ME14E0:
+	case PCI_DEVICE_ID_MEILHAUS_ME140A:
+	case PCI_DEVICE_ID_MEILHAUS_ME14EA:
+	case PCI_DEVICE_ID_MEILHAUS_ME140B:
+	case PCI_DEVICE_ID_MEILHAUS_ME14EB:
+		err = me1400_ab_ref_config(instance, ref);
+
+		if (err) {
+			spin_unlock(&instance->subdevice_lock);
+			return err;
+		}
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		err = me1400_cd_ref_config(instance, ref);
+
+		if (err) {
+			spin_unlock(&instance->subdevice_lock);
+			return err;
+		}
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4610:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660IS:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670IS:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680IS:
+		err = me4600_ref_config(instance, ref);
+
+		if (err) {
+			spin_unlock(&instance->subdevice_lock);
+			return err;
+		}
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_A:
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_B:
+		err = me8100_ref_config(instance, ref);
+
+		if (err) {
+			spin_unlock(&instance->subdevice_lock);
+			return err;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid device type.\n");
+
+		spin_unlock(&instance->subdevice_lock);
+//                              spin_unlock(instance->clk_src_reg_lock);
+		return ME_ERRNO_INVALID_SINGLE_CONFIG;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8254_io_single_read(struct me_subdevice *subdevice,
+				 struct file *filep,
+				 int channel,
+				 int *value, int time_out, int flags)
+{
+	me8254_subdevice_t *instance;
+	uint16_t lo_byte;
+	uint16_t hi_byte;
+
+	PDEBUG("executed.\n");
+
+	if (channel) {
+		PERROR("Invalid channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	instance = (me8254_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	if (instance->ctr_idx == 0)
+		outb(ME8254_CTRL_SC0 | ME8254_CTRL_TLO, instance->ctrl_reg);
+	else if (instance->ctr_idx == 1)
+		outb(ME8254_CTRL_SC1 | ME8254_CTRL_TLO, instance->ctrl_reg);
+	else
+		outb(ME8254_CTRL_SC2 | ME8254_CTRL_TLO, instance->ctrl_reg);
+
+	lo_byte = inb(instance->val_reg);
+	hi_byte = inb(instance->val_reg);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	*value = lo_byte | (hi_byte << 8);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8254_io_single_write(struct me_subdevice *subdevice,
+				  struct file *filep,
+				  int channel,
+				  int value, int time_out, int flags)
+{
+	me8254_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	if (channel) {
+		PERROR("Invalid channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	instance = (me8254_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	outb(value, instance->val_reg);
+	outb((value >> 8), instance->val_reg);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8254_query_number_channels(struct me_subdevice *subdevice,
+					int *number)
+{
+	PDEBUG("executed.\n");
+	*number = ME8254_NUMBER_CHANNELS;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8254_query_subdevice_type(struct me_subdevice *subdevice,
+				       int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_CTR;
+	*subtype = ME_SUBTYPE_CTR_8254;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8254_query_subdevice_caps(struct me_subdevice *subdevice,
+				       int *caps)
+{
+	me8254_subdevice_t *instance;
+	PDEBUG("executed.\n");
+	instance = (me8254_subdevice_t *) subdevice;
+	*caps = instance->caps;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8254_query_subdevice_caps_args(struct me_subdevice *subdevice,
+					    int cap, int *args, int count)
+{
+	PDEBUG("executed.\n");
+
+	if (count != 1) {
+		PERROR("Invalid capability argument count.\n");
+		return ME_ERRNO_INVALID_CAP_ARG_COUNT;
+	}
+
+	if (cap == ME_CAP_CTR_WIDTH) {
+		args[0] = ME8254_CTR_WIDTH;
+	} else {
+		PERROR("Invalid capability.\n");
+		return ME_ERRNO_INVALID_CAP;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static uint32_t me1400AB_get_val_reg(uint32_t reg_base, unsigned int me8254_idx,
+				     unsigned int ctr_idx)
+{
+	switch (me8254_idx) {
+
+	case 0:
+		return (reg_base + ME1400AB_8254_A_0_VAL_REG + ctr_idx);
+
+	default:
+		return (reg_base + ME1400AB_8254_B_0_VAL_REG + ctr_idx);
+	}
+
+	return 0;
+}
+
+static uint32_t me1400AB_get_ctrl_reg(uint32_t reg_base,
+				      unsigned int me8254_idx,
+				      unsigned int ctr_idx)
+{
+	switch (me8254_idx) {
+	case 0:
+		return (reg_base + ME1400AB_8254_A_CTRL_REG);
+
+	default:
+		return (reg_base + ME1400AB_8254_B_CTRL_REG);
+	}
+
+	return 0;
+}
+
+static uint32_t me1400AB_get_clk_src_reg(uint32_t reg_base,
+					 unsigned int me8254_idx,
+					 unsigned int ctr_idx)
+{
+	switch (me8254_idx) {
+	case 0:
+		return (reg_base + ME1400AB_CLK_SRC_REG);
+
+	default:
+		return (reg_base + ME1400AB_CLK_SRC_REG);
+	}
+
+	return 0;
+}
+
+static uint32_t me1400CD_get_val_reg(uint32_t reg_base, unsigned int me8254_idx,
+				     unsigned int ctr_idx)
+{
+	switch (me8254_idx) {
+	case 0:
+		return (reg_base + ME1400C_8254_A_0_VAL_REG + ctr_idx);
+
+	case 1:
+		return (reg_base + ME1400C_8254_B_0_VAL_REG + ctr_idx);
+
+	case 2:
+		return (reg_base + ME1400C_8254_C_0_VAL_REG + ctr_idx);
+
+	case 3:
+		return (reg_base + ME1400C_8254_D_0_VAL_REG + ctr_idx);
+
+	case 4:
+		return (reg_base + ME1400C_8254_E_0_VAL_REG + ctr_idx);
+
+	case 5:
+		return (reg_base + ME1400D_8254_A_0_VAL_REG + ctr_idx);
+
+	case 6:
+		return (reg_base + ME1400D_8254_B_0_VAL_REG + ctr_idx);
+
+	case 7:
+		return (reg_base + ME1400D_8254_C_0_VAL_REG + ctr_idx);
+
+	case 8:
+		return (reg_base + ME1400D_8254_D_0_VAL_REG + ctr_idx);
+
+	default:
+		return (reg_base + ME1400D_8254_E_0_VAL_REG + ctr_idx);
+	}
+
+	return 0;
+}
+
+static uint32_t me1400CD_get_ctrl_reg(uint32_t reg_base,
+				      unsigned int me8254_idx,
+				      unsigned int ctr_idx)
+{
+	switch (me8254_idx) {
+	case 0:
+		return (reg_base + ME1400C_8254_A_CTRL_REG);
+
+	case 1:
+		return (reg_base + ME1400C_8254_B_CTRL_REG);
+
+	case 2:
+		return (reg_base + ME1400C_8254_C_CTRL_REG);
+
+	case 3:
+		return (reg_base + ME1400C_8254_D_CTRL_REG);
+
+	case 4:
+		return (reg_base + ME1400C_8254_E_CTRL_REG);
+
+	case 5:
+		return (reg_base + ME1400D_8254_A_CTRL_REG);
+
+	case 6:
+		return (reg_base + ME1400D_8254_B_CTRL_REG);
+
+	case 7:
+		return (reg_base + ME1400D_8254_C_CTRL_REG);
+
+	case 8:
+		return (reg_base + ME1400D_8254_D_CTRL_REG);
+
+	default:
+		return (reg_base + ME1400D_8254_E_CTRL_REG);
+	}
+
+	return 0;
+}
+
+static uint32_t me1400CD_get_clk_src_reg(uint32_t reg_base,
+					 unsigned int me8254_idx,
+					 unsigned int ctr_idx)
+{
+	switch (me8254_idx) {
+	case 0:
+		return (reg_base + ME1400C_CLK_SRC_0_REG);
+
+	case 1:
+		return (reg_base + ME1400C_CLK_SRC_0_REG);
+
+	case 2:
+		return (reg_base + ME1400C_CLK_SRC_1_REG);
+
+	case 3:
+		return (reg_base + ME1400C_CLK_SRC_1_REG);
+
+	case 4:
+		return (reg_base + ME1400C_CLK_SRC_2_REG);
+
+	case 5:
+		return (reg_base + ME1400D_CLK_SRC_0_REG);
+
+	case 6:
+		return (reg_base + ME1400D_CLK_SRC_0_REG);
+
+	case 7:
+		return (reg_base + ME1400D_CLK_SRC_1_REG);
+
+	case 8:
+		return (reg_base + ME1400D_CLK_SRC_1_REG);
+
+	default:
+		return (reg_base + ME1400D_CLK_SRC_2_REG);
+	}
+
+	return 0;
+}
+
+static uint32_t me4600_get_val_reg(uint32_t reg_base, unsigned int me8254_idx,
+				   unsigned int ctr_idx)
+{
+	return (reg_base + ME4600_8254_0_VAL_REG + ctr_idx);
+}
+
+static uint32_t me4600_get_ctrl_reg(uint32_t reg_base, unsigned int me8254_idx,
+				    unsigned int ctr_idx)
+{
+	return (reg_base + ME4600_8254_CTRL_REG);
+}
+
+static uint32_t me8100_get_val_reg(uint32_t reg_base, unsigned int me8254_idx,
+				   unsigned int ctr_idx)
+{
+	return (reg_base + ME8100_COUNTER_REG_0 + ctr_idx * 2);
+}
+
+static uint32_t me8100_get_ctrl_reg(uint32_t reg_base, unsigned int me8254_idx,
+				    unsigned int ctr_idx)
+{
+	return (reg_base + ME8100_COUNTER_CTRL_REG);
+}
+
+me8254_subdevice_t *me8254_constructor(uint32_t device_id,
+				       uint32_t reg_base,
+				       unsigned int me8254_idx,
+				       unsigned int ctr_idx,
+				       spinlock_t * ctrl_reg_lock,
+				       spinlock_t * clk_src_reg_lock)
+{
+	me8254_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	// Allocate memory for subdevice instance
+	subdevice = kmalloc(sizeof(me8254_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for 8254 instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me8254_subdevice_t));
+
+	// Check if counter index is out of range
+
+	if (ctr_idx > 2) {
+		PERROR("Counter index is out of range.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize subdevice base class
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+	subdevice->clk_src_reg_lock = clk_src_reg_lock;
+
+	// Save type of Meilhaus device
+	subdevice->device_id = device_id;
+
+	// Save the indices
+	subdevice->me8254_idx = me8254_idx;
+	subdevice->ctr_idx = ctr_idx;
+
+	// Do device specific initialization
+	switch (device_id) {
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140A:
+	case PCI_DEVICE_ID_MEILHAUS_ME14EA:
+		// Check if 8254 index is out of range
+		if (me8254_idx > 0) {
+			PERROR("8254 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140B:	// Fall through
+	case PCI_DEVICE_ID_MEILHAUS_ME14EB:
+		// Check if 8254 index is out of range
+		if (me8254_idx > 1) {
+			PERROR("8254 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+		// Initialize the counters capabilities
+		if (ctr_idx == 0)
+			subdevice->caps =
+			    ME_CAPS_CTR_CLK_INTERNAL_1MHZ |
+			    ME_CAPS_CTR_CLK_INTERNAL_10MHZ |
+			    ME_CAPS_CTR_CLK_EXTERNAL;
+		else
+			subdevice->caps =
+			    ME_CAPS_CTR_CLK_PREVIOUS | ME_CAPS_CTR_CLK_EXTERNAL;
+
+		// Get the counters registers
+		subdevice->val_reg =
+		    me1400AB_get_val_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->ctrl_reg =
+		    me1400AB_get_ctrl_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->clk_src_reg =
+		    me1400AB_get_clk_src_reg(reg_base, me8254_idx, ctr_idx);
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+		// Check if 8254 index is out of range
+		if (me8254_idx > 4) {
+			PERROR("8254 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		// Check if 8254 index is out of range
+		if (me8254_idx > 9) {
+			PERROR("8254 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+		// Initialize the counters capabilities
+		if (ctr_idx == 0) {
+			if (me8254_idx == 0)
+				subdevice->caps =
+				    ME_CAPS_CTR_CLK_PREVIOUS |
+				    ME_CAPS_CTR_CLK_INTERNAL_1MHZ |
+				    ME_CAPS_CTR_CLK_INTERNAL_10MHZ |
+				    ME_CAPS_CTR_CLK_EXTERNAL;
+			else
+				subdevice->caps =
+				    ME_CAPS_CTR_CLK_INTERNAL_1MHZ |
+				    ME_CAPS_CTR_CLK_INTERNAL_10MHZ |
+				    ME_CAPS_CTR_CLK_EXTERNAL;
+		} else
+			subdevice->caps =
+			    ME_CAPS_CTR_CLK_PREVIOUS | ME_CAPS_CTR_CLK_EXTERNAL;
+
+		// Get the counters registers
+		subdevice->val_reg =
+		    me1400CD_get_val_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->ctrl_reg =
+		    me1400CD_get_ctrl_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->clk_src_reg =
+		    me1400CD_get_clk_src_reg(reg_base, me8254_idx, ctr_idx);
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4610:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660IS:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670IS:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680IS:
+		// Check if 8254 index is out of range
+		if (me8254_idx > 0) {
+			PERROR("8254 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+		// Initialize the counters capabilities
+		subdevice->caps = ME_CAPS_CTR_CLK_EXTERNAL;
+
+		// Get the counters registers
+		subdevice->val_reg =
+		    me4600_get_val_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->ctrl_reg =
+		    me4600_get_ctrl_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->clk_src_reg = 0;	// Not used
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_A:
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_B:
+		// Check if 8254 index is out of range
+		if (me8254_idx > 0) {
+			PERROR("8254 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+		// Initialize the counters capabilities
+		subdevice->caps = ME_CAPS_CTR_CLK_EXTERNAL;
+
+		// Get the counters registers
+		subdevice->val_reg =
+		    me8100_get_val_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->ctrl_reg =
+		    me8100_get_ctrl_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->clk_src_reg = 0;	// Not used
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4650:
+	case PCI_DEVICE_ID_MEILHAUS_ME1400:
+	case PCI_DEVICE_ID_MEILHAUS_ME14E0:
+		PERROR("No 8254 subdevices available for subdevice device.\n");
+		me_subdevice_deinit(&subdevice->base);
+		kfree(subdevice);
+		return NULL;
+
+	default:
+		PERROR("Unknown device type.\n");
+		me_subdevice_deinit(&subdevice->base);
+		kfree(subdevice);
+		return NULL;
+	}
+
+	// Overload subdevice base class methods.
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me8254_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config = me8254_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me8254_io_single_read;
+	subdevice->base.me_subdevice_io_single_write = me8254_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me8254_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me8254_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me8254_query_subdevice_caps;
+	subdevice->base.me_subdevice_query_subdevice_caps_args =
+	    me8254_query_subdevice_caps_args;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me8254.h b/drivers/staging/meilhaus/me8254.h
new file mode 100644
index 000000000000..572b7196d5a8
--- /dev/null
+++ b/drivers/staging/meilhaus/me8254.h
@@ -0,0 +1,80 @@
+/**
+ * @file me8254.h
+ *
+ * @brief 8254 counter implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8254_H_
+#define _ME8254_H_
+
+#include "mesubdevice.h"
+#include "meslock.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The 8254 subdevice class.
+ */
+typedef struct me8254_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect the control register from concurrent access. */
+	spinlock_t *clk_src_reg_lock;		/**< Spin lock to protect the clock source register from concurrent access. */
+
+	uint32_t device_id;			/**< The Meilhaus device type carrying the 8254 chip. */
+	int me8254_idx;				/**< The index of the 8254 chip on the device. */
+	int ctr_idx;				/**< The index of the counter on the 8254 chip. */
+
+	int caps;				/**< Holds the device capabilities. */
+
+	unsigned long val_reg;			/**< Holds the actual counter value. */
+	unsigned long ctrl_reg;			/**< Register to configure the 8254 modes. */
+	unsigned long clk_src_reg;		/**< Register to configure the counter connections. */
+} me8254_subdevice_t;
+
+/**
+ * @brief The constructor to generate a 8254 instance.
+ *
+ * @param device_id The kind of Meilhaus device holding the 8254.
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param me8254_idx The index of the 8254 chip on the Meilhaus device.
+ * @param ctr_idx The index of the counter inside a 8254 chip.
+ * @param ctrl_reg_lock Pointer to spin lock protecting the 8254 control register from concurrent access.
+ * @param clk_src_reg_lock Pointer to spin lock protecting the clock source register from concurrent access.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me8254_subdevice_t *me8254_constructor(uint32_t device_id,
+				       uint32_t reg_base,
+				       unsigned int me8254_idx,
+				       unsigned int ctr_idx,
+				       spinlock_t * ctrl_reg_lock,
+				       spinlock_t * clk_src_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8254_reg.h b/drivers/staging/meilhaus/me8254_reg.h
new file mode 100644
index 000000000000..7e2c36b46f56
--- /dev/null
+++ b/drivers/staging/meilhaus/me8254_reg.h
@@ -0,0 +1,172 @@
+/**
+ * @file me8254_reg.h
+ *
+ * @brief 8254 counter register definitions.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME8254_REG_H_
+#define _ME8254_REG_H_
+
+#ifdef __KERNEL__
+
+/* ME1400 A/B register offsets */
+#define ME1400AB_8254_A_0_VAL_REG		0x0004 /**< Offset of 8254 A counter 0 value register. */
+#define ME1400AB_8254_A_1_VAL_REG		0x0005 /**< Offset of 8254 A counter 1 value register. */
+#define ME1400AB_8254_A_2_VAL_REG		0x0006 /**< Offset of 8254 A counter 2 value register. */
+#define ME1400AB_8254_A_CTRL_REG		0x0007 /**< Offset of 8254 A control register. */
+
+#define ME1400AB_8254_B_0_VAL_REG		0x000C /**< Offset of 8254 B counter 0 value register. */
+#define ME1400AB_8254_B_1_VAL_REG		0x000D /**< Offset of 8254 B counter 1 value register. */
+#define ME1400AB_8254_B_2_VAL_REG		0x000E /**< Offset of 8254 B counter 2 value register. */
+#define ME1400AB_8254_B_CTRL_REG		0x000F /**< Offset of 8254 B control register. */
+
+#define ME1400AB_CLK_SRC_REG			0x0010 /**< Offset of clock source register. */
+
+/* ME1400 C register offsets */
+#define ME1400C_8254_A_0_VAL_REG		0x0004 /**< Offset of 8254 A counter 0 value register. */
+#define ME1400C_8254_A_1_VAL_REG		0x0005 /**< Offset of 8254 A counter 0 value register. */
+#define ME1400C_8254_A_2_VAL_REG		0x0006 /**< Offset of 8254 A counter 0 value register. */
+#define ME1400C_8254_A_CTRL_REG			0x0007 /**< Offset of 8254 A control register. */
+
+#define ME1400C_8254_B_0_VAL_REG		0x000C /**< Offset of 8254 B counter 0 value register. */
+#define ME1400C_8254_B_1_VAL_REG		0x000D /**< Offset of 8254 B counter 0 value register. */
+#define ME1400C_8254_B_2_VAL_REG		0x000E /**< Offset of 8254 B counter 0 value register. */
+#define ME1400C_8254_B_CTRL_REG			0x000F /**< Offset of 8254 B control register. */
+
+#define ME1400C_8254_C_0_VAL_REG		0x0010 /**< Offset of 8254 C counter 0 value register. */
+#define ME1400C_8254_C_1_VAL_REG		0x0011 /**< Offset of 8254 C counter 0 value register. */
+#define ME1400C_8254_C_2_VAL_REG		0x0012 /**< Offset of 8254 C counter 0 value register. */
+#define ME1400C_8254_C_CTRL_REG			0x0013 /**< Offset of 8254 C control register. */
+
+#define ME1400C_8254_D_0_VAL_REG		0x0014 /**< Offset of 8254 D counter 0 value register. */
+#define ME1400C_8254_D_1_VAL_REG		0x0015 /**< Offset of 8254 D counter 0 value register. */
+#define ME1400C_8254_D_2_VAL_REG		0x0016 /**< Offset of 8254 D counter 0 value register. */
+#define ME1400C_8254_D_CTRL_REG			0x0017 /**< Offset of 8254 D control register. */
+
+#define ME1400C_8254_E_0_VAL_REG		0x0018 /**< Offset of 8254 E counter 0 value register. */
+#define ME1400C_8254_E_1_VAL_REG		0x0019 /**< Offset of 8254 E counter 0 value register. */
+#define ME1400C_8254_E_2_VAL_REG		0x001A /**< Offset of 8254 E counter 0 value register. */
+#define ME1400C_8254_E_CTRL_REG			0x001B /**< Offset of 8254 E control register. */
+
+#define ME1400C_CLK_SRC_0_REG  			0x001C /**< Offset of clock source register 0. */
+#define ME1400C_CLK_SRC_1_REG  			0x001D /**< Offset of clock source register 1. */
+#define ME1400C_CLK_SRC_2_REG  			0x001E /**< Offset of clock source register 2. */
+
+/* ME1400 D register offsets */
+#define ME1400D_8254_A_0_VAL_REG		0x0044 /**< Offset of 8254 A counter 0 value register. */
+#define ME1400D_8254_A_1_VAL_REG		0x0045 /**< Offset of 8254 A counter 0 value register. */
+#define ME1400D_8254_A_2_VAL_REG		0x0046 /**< Offset of 8254 A counter 0 value register. */
+#define ME1400D_8254_A_CTRL_REG			0x0047 /**< Offset of 8254 A control register. */
+
+#define ME1400D_8254_B_0_VAL_REG		0x004C /**< Offset of 8254 B counter 0 value register. */
+#define ME1400D_8254_B_1_VAL_REG		0x004D /**< Offset of 8254 B counter 0 value register. */
+#define ME1400D_8254_B_2_VAL_REG		0x004E /**< Offset of 8254 B counter 0 value register. */
+#define ME1400D_8254_B_CTRL_REG			0x004F /**< Offset of 8254 B control register. */
+
+#define ME1400D_8254_C_0_VAL_REG		0x0050 /**< Offset of 8254 C counter 0 value register. */
+#define ME1400D_8254_C_1_VAL_REG		0x0051 /**< Offset of 8254 C counter 0 value register. */
+#define ME1400D_8254_C_2_VAL_REG		0x0052 /**< Offset of 8254 C counter 0 value register. */
+#define ME1400D_8254_C_CTRL_REG			0x0053 /**< Offset of 8254 C control register. */
+
+#define ME1400D_8254_D_0_VAL_REG		0x0054 /**< Offset of 8254 D counter 0 value register. */
+#define ME1400D_8254_D_1_VAL_REG		0x0055 /**< Offset of 8254 D counter 0 value register. */
+#define ME1400D_8254_D_2_VAL_REG		0x0056 /**< Offset of 8254 D counter 0 value register. */
+#define ME1400D_8254_D_CTRL_REG			0x0057 /**< Offset of 8254 D control register. */
+
+#define ME1400D_8254_E_0_VAL_REG		0x0058 /**< Offset of 8254 E counter 0 value register. */
+#define ME1400D_8254_E_1_VAL_REG		0x0059 /**< Offset of 8254 E counter 0 value register. */
+#define ME1400D_8254_E_2_VAL_REG		0x005A /**< Offset of 8254 E counter 0 value register. */
+#define ME1400D_8254_E_CTRL_REG			0x005B /**< Offset of 8254 E control register. */
+
+#define ME1400D_CLK_SRC_0_REG  			0x005C /**< Offset of clock source register 0. */
+#define ME1400D_CLK_SRC_1_REG  			0x005D /**< Offset of clock source register 1. */
+#define ME1400D_CLK_SRC_2_REG  			0x005E /**< Offset of clock source register 2. */
+
+/* ME4600 register offsets */
+#define ME4600_8254_0_VAL_REG			0x0000 /**< Offset of 8254 A counter 0 value register. */
+#define ME4600_8254_1_VAL_REG			0x0001 /**< Offset of 8254 A counter 0 value register. */
+#define ME4600_8254_2_VAL_REG			0x0002 /**< Offset of 8254 A counter 0 value register. */
+#define ME4600_8254_CTRL_REG			0x0003 /**< Offset of 8254 A control register. */
+
+/* Command words for 8254 control register */
+#define ME8254_CTRL_SC0   0x00	 /**< Counter 0 selection. */
+#define ME8254_CTRL_SC1   0x40	 /**< Counter 1 selection. */
+#define ME8254_CTRL_SC2   0x80	 /**< Counter 2 selection. */
+
+#define ME8254_CTRL_TLO   0x00	 /**< Counter latching operation. */
+#define ME8254_CTRL_LSB   0x10	 /**< Only read LSB. */
+#define ME8254_CTRL_MSB   0x20	 /**< Only read MSB. */
+#define ME8254_CTRL_LM    0x30	 /**< First read LSB, then MSB.  */
+
+#define ME8254_CTRL_M0    0x00	 /**< Mode 0 selection. */
+#define ME8254_CTRL_M1    0x02	 /**< Mode 1 selection. */
+#define ME8254_CTRL_M2    0x04	 /**< Mode 2 selection. */
+#define ME8254_CTRL_M3    0x06	 /**< Mode 3 selection. */
+#define ME8254_CTRL_M4    0x08	 /**< Mode 4 selection. */
+#define ME8254_CTRL_M5    0x0A	 /**< Mode 5 selection. */
+
+#define ME8254_CTRL_BIN   0x00	 /**< Binary counter. */
+#define ME8254_CTRL_BCD   0x01	 /**< BCD counter. */
+
+/* ME-1400 A/B clock source register bits */
+#define ME1400AB_8254_A_0_CLK_SRC_1MHZ		(0 << 7)	/**< 1MHz clock. */
+#define ME1400AB_8254_A_0_CLK_SRC_10MHZ		(1 << 7)	/**< 10MHz clock. */
+#define ME1400AB_8254_A_0_CLK_SRC_PIN		(0 << 6)	/**< CLK 0 to SUB-D. */
+#define ME1400AB_8254_A_0_CLK_SRC_QUARZ		(1 << 6)	/**< Connect CLK 0 with quarz. */
+
+#define ME1400AB_8254_A_1_CLK_SRC_PIN		(0 << 5)	/**< CLK 1 to SUB-D. */
+#define ME1400AB_8254_A_1_CLK_SRC_PREV		(1 << 5)	/**< Connect OUT 0 with CLK 1. */
+
+#define ME1400AB_8254_A_2_CLK_SRC_PIN		(0 << 4)	/**< CLK 2 to SUB-D. */
+#define ME1400AB_8254_A_2_CLK_SRC_PREV		(1 << 4)	/**< Connect OUT 1 with CLK 2. */
+
+#define ME1400AB_8254_B_0_CLK_SRC_1MHZ		(0 << 3)	/**< 1MHz clock. */
+#define ME1400AB_8254_B_0_CLK_SRC_10MHZ		(1 << 3)	/**< 10MHz clock. */
+#define ME1400AB_8254_B_0_CLK_SRC_PIN		(0 << 2)	/**< CLK 0 to SUB-D. */
+#define ME1400AB_8254_B_0_CLK_SRC_QUARZ		(1 << 2)	/**< Connect CLK 0 with quarz. */
+
+#define ME1400AB_8254_B_1_CLK_SRC_PIN		(0 << 1)	/**< CLK 1 to SUB-D. */
+#define ME1400AB_8254_B_1_CLK_SRC_PREV		(1 << 1)	/**< Connect OUT 0 with CLK 1. */
+
+#define ME1400AB_8254_B_2_CLK_SRC_PIN		(0 << 0)	/**< CLK 2 to SUB-D. */
+#define ME1400AB_8254_B_2_CLK_SRC_PREV		(1 << 0)	/**< Connect OUT 1 with CLK 2. */
+
+/* ME-1400 C/D clock source registers bits */
+#define ME1400CD_8254_ACE_0_CLK_SRC_MASK	0x03	/**< Masks all CLK source bits. */
+#define ME1400CD_8254_ACE_0_CLK_SRC_PIN		0x00	/**< Connect CLK to SUB-D. */
+#define ME1400CD_8254_ACE_0_CLK_SRC_1MHZ	0x01	/**< Connect CLK to 1MHz. */
+#define ME1400CD_8254_ACE_0_CLK_SRC_10MHZ	0x02	/**< Connect CLK to 10MHz. */
+#define ME1400CD_8254_ACE_0_CLK_SRC_PREV	0x03	/**< Connect CLK to previous counter output on ME-1400 D extension. */
+
+#define ME1400CD_8254_ACE_1_CLK_SRC_MASK	0x04	/**< Masks all CLK source bits. */
+#define ME1400CD_8254_ACE_1_CLK_SRC_PIN		0x00	/**< Connect CLK to SUB-D. */
+#define ME1400CD_8254_ACE_1_CLK_SRC_PREV	0x04	/**< Connect CLK to previous counter output. */
+
+#define ME1400CD_8254_ACE_2_CLK_SRC_MASK	0x08	/**< Masks all CLK source bits. */
+#define ME1400CD_8254_ACE_2_CLK_SRC_PIN		0x00	/**< Connect to SUB-D. */
+#define ME1400CD_8254_ACE_2_CLK_SRC_PREV	0x08	/**< Connect CLK to previous counter output. */
+
+#define ME1400CD_8254_BD_0_CLK_SRC_MASK		0x30	/**< Masks all CLK source bits. */
+#define ME1400CD_8254_BD_0_CLK_SRC_PIN		0x00	/**< Connect CLK to SUB-D. */
+#define ME1400CD_8254_BD_0_CLK_SRC_1MHZ		0x10	/**< Connect CLK to 1MHz. */
+#define ME1400CD_8254_BD_0_CLK_SRC_10MHZ	0x20	/**< Connect CLK to 10MHz. */
+#define ME1400CD_8254_BD_0_CLK_SRC_PREV		0x30	/**< Connect CLK to previous counter output. */
+
+#define ME1400CD_8254_BD_1_CLK_SRC_MASK		0x40	/**< Masks all CLK source bits. */
+#define ME1400CD_8254_BD_1_CLK_SRC_PIN		0x00	/**< Connect CLK to SUB-D. */
+#define ME1400CD_8254_BD_1_CLK_SRC_PREV		0x40	/**< Connect CLK to previous counter output. */
+
+#define ME1400CD_8254_BD_2_CLK_SRC_MASK		0x80	/**< Masks all CLK source bits. */
+#define ME1400CD_8254_BD_2_CLK_SRC_PIN		0x00	/**< Connect CLK to SUB-D. */
+#define ME1400CD_8254_BD_2_CLK_SRC_PREV		0x80	/**< Connect CLK to previous counter output. */
+
+/* ME-8100 counter registers */
+#define ME8100_COUNTER_REG_0				0x18	//(r,w)
+#define ME8100_COUNTER_REG_1				0x1A	//(r,w)
+#define ME8100_COUNTER_REG_2				0x1C	//(r,w)
+#define ME8100_COUNTER_CTRL_REG				0x1E	//(r,w)
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8255.c b/drivers/staging/meilhaus/me8255.c
new file mode 100644
index 000000000000..180e7f8d2146
--- /dev/null
+++ b/drivers/staging/meilhaus/me8255.c
@@ -0,0 +1,462 @@
+/**
+ * @file me8255.c
+ *
+ * @brief 8255 subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+#include "medebug.h"
+
+#include "me8255_reg.h"
+#include "me8255.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static uint8_t get_mode_from_mirror(uint32_t mirror)
+{
+	PDEBUG("executed.\n");
+
+	if (mirror & ME8255_PORT_0_OUTPUT) {
+		if (mirror & ME8255_PORT_1_OUTPUT) {
+			if (mirror & ME8255_PORT_2_OUTPUT) {
+				return ME8255_MODE_OOO;
+			} else {
+				return ME8255_MODE_IOO;
+			}
+		} else {
+			if (mirror & ME8255_PORT_2_OUTPUT) {
+				return ME8255_MODE_OIO;
+			} else {
+				return ME8255_MODE_IIO;
+			}
+		}
+	} else {
+		if (mirror & ME8255_PORT_1_OUTPUT) {
+			if (mirror & ME8255_PORT_2_OUTPUT) {
+				return ME8255_MODE_OOI;
+			} else {
+				return ME8255_MODE_IOI;
+			}
+		} else {
+			if (mirror & ME8255_PORT_2_OUTPUT) {
+				return ME8255_MODE_OII;
+			} else {
+				return ME8255_MODE_III;
+			}
+		}
+	}
+}
+
+static int me8255_io_reset_subdevice(struct me_subdevice *subdevice,
+				     struct file *filep, int flags)
+{
+	me8255_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8255_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	*instance->ctrl_reg_mirror &=
+	    ~(ME8255_PORT_0_OUTPUT << instance->dio_idx);
+	outb(get_mode_from_mirror(*instance->ctrl_reg_mirror),
+	     instance->ctrl_reg);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outb(0, instance->port_reg);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8255_io_single_config(struct me_subdevice *subdevice,
+				   struct file *filep,
+				   int channel,
+				   int single_config,
+				   int ref,
+				   int trig_chan,
+				   int trig_type, int trig_edge, int flags)
+{
+	me8255_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8255_subdevice_t *) subdevice;
+
+	if (flags & ~ME_IO_SINGLE_CONFIG_DIO_BYTE) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+		spin_lock(instance->ctrl_reg_lock);
+		*instance->ctrl_reg_mirror &=
+		    ~(ME8255_PORT_0_OUTPUT << instance->dio_idx);
+		outb(get_mode_from_mirror(*instance->ctrl_reg_mirror),
+		     instance->ctrl_reg);
+		spin_unlock(instance->ctrl_reg_lock);
+	} else if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+		spin_lock(instance->ctrl_reg_lock);
+		*instance->ctrl_reg_mirror |=
+		    (ME8255_PORT_0_OUTPUT << instance->dio_idx);
+		outb(get_mode_from_mirror(*instance->ctrl_reg_mirror),
+		     instance->ctrl_reg);
+		spin_unlock(instance->ctrl_reg_lock);
+	} else {
+		PERROR("Invalid port direction.\n");
+		err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8255_io_single_read(struct me_subdevice *subdevice,
+				 struct file *filep,
+				 int channel,
+				 int *value, int time_out, int flags)
+{
+	me8255_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8255_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = inb(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inb(instance->port_reg);
+		} else {
+			PERROR("Invalid byte number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8255_io_single_write(struct me_subdevice *subdevice,
+				  struct file *filep,
+				  int channel,
+				  int value, int time_out, int flags)
+{
+	me8255_subdevice_t *instance;
+	uint8_t byte;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8255_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			if (*instance->
+			    ctrl_reg_mirror & (ME8255_PORT_0_OUTPUT <<
+					       instance->dio_idx)) {
+				byte = inb(instance->port_reg);
+
+				if (value)
+					byte |= 0x1 << channel;
+				else
+					byte &= ~(0x1 << channel);
+
+				outb(byte, instance->port_reg);
+			} else {
+				PERROR("Port not in output mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			if (*instance->
+			    ctrl_reg_mirror & (ME8255_PORT_0_OUTPUT <<
+					       instance->dio_idx)) {
+				outb(value, instance->port_reg);
+			} else {
+				PERROR("Port not in output mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8255_query_number_channels(struct me_subdevice *subdevice,
+					int *number)
+{
+	PDEBUG("executed.\n");
+	*number = ME8255_NUMBER_CHANNELS;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8255_query_subdevice_type(struct me_subdevice *subdevice,
+				       int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DIO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8255_query_subdevice_caps(struct me_subdevice *subdevice,
+				       int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_DIR_BYTE;
+	return ME_ERRNO_SUCCESS;
+}
+
+me8255_subdevice_t *me8255_constructor(uint32_t device_id,
+				       uint32_t reg_base,
+				       unsigned int me8255_idx,
+				       unsigned int dio_idx,
+				       int *ctrl_reg_mirror,
+				       spinlock_t * ctrl_reg_lock)
+{
+	me8255_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me8255_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for 8255 instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me8255_subdevice_t));
+
+	/* Check if counter index is out of range */
+
+	if (dio_idx > 2) {
+		PERROR("DIO index is out of range.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save the pointer to global port settings */
+	subdevice->ctrl_reg_mirror = ctrl_reg_mirror;
+
+	/* Save type of Meilhaus device */
+	subdevice->device_id = device_id;
+
+	/* Save the indices */
+	subdevice->me8255_idx = me8255_idx;
+	subdevice->dio_idx = dio_idx;
+
+	/* Do device specific initialization */
+	switch (device_id) {
+	case PCI_DEVICE_ID_MEILHAUS_ME1400:
+	case PCI_DEVICE_ID_MEILHAUS_ME14E0:
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140A:
+	case PCI_DEVICE_ID_MEILHAUS_ME14EA:
+		/* Check if 8255 index is out of range */
+		if (me8255_idx > 0) {
+			PERROR("8255 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140B:	/* Fall through */
+	case PCI_DEVICE_ID_MEILHAUS_ME14EB:
+		/* Check if 8255 index is out of range */
+		if (me8255_idx > 1) {
+			PERROR("8255 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+
+		/* Get the registers */
+		if (me8255_idx == 0) {
+			subdevice->ctrl_reg = reg_base + ME1400AB_PORT_A_CTRL;
+			subdevice->port_reg =
+			    reg_base + ME1400AB_PORT_A_0 + dio_idx;
+		} else if (me8255_idx == 1) {
+			subdevice->ctrl_reg = reg_base + ME1400AB_PORT_B_CTRL;
+			subdevice->port_reg =
+			    reg_base + ME1400AB_PORT_B_0 + dio_idx;
+		}
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+		/* Check if 8255 index is out of range */
+		if (me8255_idx > 0) {
+			PERROR("8255 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:	/* Fall through */
+		/* Check if 8255 index is out of range */
+		if (me8255_idx > 1) {
+			PERROR("8255 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+
+		/* Get the registers */
+		if (me8255_idx == 0) {
+			subdevice->ctrl_reg = reg_base + ME1400CD_PORT_A_CTRL;
+			subdevice->port_reg =
+			    reg_base + ME1400CD_PORT_A_0 + dio_idx;
+		} else if (me8255_idx == 1) {
+			subdevice->ctrl_reg = reg_base + ME1400CD_PORT_B_CTRL;
+			subdevice->port_reg =
+			    reg_base + ME1400CD_PORT_B_0 + dio_idx;
+		}
+
+		break;
+
+	default:
+		PERROR("Unknown device type. dev ID: 0x%04x\n", device_id);
+
+		me_subdevice_deinit(&subdevice->base);
+
+		kfree(subdevice);
+
+		return NULL;
+	}
+
+	/* Overload subdevice base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me8255_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config = me8255_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me8255_io_single_read;
+	subdevice->base.me_subdevice_io_single_write = me8255_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me8255_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me8255_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me8255_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me8255.h b/drivers/staging/meilhaus/me8255.h
new file mode 100644
index 000000000000..338230052b3c
--- /dev/null
+++ b/drivers/staging/meilhaus/me8255.h
@@ -0,0 +1,59 @@
+/**
+ * @file me8255.h
+ *
+ * @brief Meilhaus PIO 8255 implementation.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME8255_H_
+#define _ME8255_H_
+
+#include "mesubdevice.h"
+#include "meslock.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The 8255 subdevice class.
+ */
+typedef struct me8255_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+
+	int *ctrl_reg_mirror;			/**< Pointer to mirror of the control register. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg and #ctrl_reg_mirror from concurrent access. */
+
+	uint32_t device_id;				/**< The PCI device id of the device holding the 8255 chip. */
+	int me8255_idx;					/**< The index of the 8255 chip on the device. */
+	int dio_idx;					/**< The index of the DIO port on the 8255 chip. */
+
+	unsigned long port_reg;			/**< Register to read or write a value from or to the port respectively. */
+	unsigned long ctrl_reg;			/**< Register to configure the 8255 modes. */
+} me8255_subdevice_t;
+
+/**
+ * @brief The constructor to generate a 8255 instance.
+ *
+ * @param device_id The kind of Meilhaus device holding the 8255.
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param me8255_idx The index of the 8255 chip on the Meilhaus device.
+ * @param dio_idx The index of the counter inside a 8255 chip.
+ * @param ctr_reg_mirror Pointer to mirror of control register.
+ * @param ctrl_reg_lock Pointer to spin lock protecting the 8255 control register and #ctrl_reg_mirror from concurrent access.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me8255_subdevice_t *me8255_constructor(uint32_t device_id,
+				       uint32_t reg_base,
+				       unsigned int me8255_idx,
+				       unsigned int dio_idx,
+				       int *ctrl_reg_mirror,
+				       spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8255_reg.h b/drivers/staging/meilhaus/me8255_reg.h
new file mode 100644
index 000000000000..d1dea1a447f6
--- /dev/null
+++ b/drivers/staging/meilhaus/me8255_reg.h
@@ -0,0 +1,50 @@
+/**
+ * @file me8255_reg.h
+ *
+ * @brief 8255 counter register definitions.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME8255_REG_H_
+#define _ME8255_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME8255_NUMBER_CHANNELS		8		/**< The number of channels per 8255 port. */
+
+#define ME1400AB_PORT_A_0			0x0000	/**< Port 0 offset. */
+#define ME1400AB_PORT_A_1			0x0001	/**< Port 1 offset. */
+#define ME1400AB_PORT_A_2			0x0002	/**< Port 2 offset. */
+#define ME1400AB_PORT_A_CTRL		0x0003	/**< Control register for 8255 A. */
+
+#define ME1400AB_PORT_B_0			0x0008	/**< Port 0 offset. */
+#define ME1400AB_PORT_B_1			0x0009	/**< Port 1 offset. */
+#define ME1400AB_PORT_B_2			0x000A	/**< Port 2 offset. */
+#define ME1400AB_PORT_B_CTRL		0x000B	/**< Control register for 8255 B. */
+
+#define ME1400CD_PORT_A_0			0x0000	/**< Port 0 offset. */
+#define ME1400CD_PORT_A_1			0x0001	/**< Port 1 offset. */
+#define ME1400CD_PORT_A_2			0x0002	/**< Port 2 offset. */
+#define ME1400CD_PORT_A_CTRL		0x0003	/**< Control register for 8255 A. */
+
+#define ME1400CD_PORT_B_0			0x0040	/**< Port 0 offset. */
+#define ME1400CD_PORT_B_1			0x0041	/**< Port 1 offset. */
+#define ME1400CD_PORT_B_2			0x0042	/**< Port 2 offset. */
+#define ME1400CD_PORT_B_CTRL		0x0043	/**< Control register for 8255 B. */
+
+#define ME8255_MODE_OOO				0x80	/**< Port 2 = Output, Port 1 = Output, Port 0 = Output */
+#define ME8255_MODE_IOO				0x89	/**< Port 2 = Input,  Port 1 = Output, Port 0 = Output */
+#define ME8255_MODE_OIO				0x82	/**< Port 2 = Output, Port 1 = Input,  Port 0 = Output */
+#define ME8255_MODE_IIO				0x8B	/**< Port 2 = Input,  Port 1 = Input,  Port 0 = Output */
+#define ME8255_MODE_OOI				0x90	/**< Port 2 = Output, Port 1 = Output, Port 0 = Input */
+#define ME8255_MODE_IOI				0x99	/**< Port 2 = Input,  Port 1 = Output, Port 0 = Input */
+#define ME8255_MODE_OII				0x92	/**< Port 2 = Output, Port 1 = Input,  Port 0 = Input */
+#define ME8255_MODE_III				0x9B	/**< Port 2 = Input,  Port 1 = Input,  Port 0 = Input */
+
+#define ME8255_PORT_0_OUTPUT		0x1		/**< If set in mirror then port 0 is in output mode. */
+#define ME8255_PORT_1_OUTPUT		0x2		/**< If set in mirror then port 1 is in output mode. */
+#define ME8255_PORT_2_OUTPUT		0x4		/**< If set in mirror then port 2 is in output mode. */
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/mecirc_buf.h b/drivers/staging/meilhaus/mecirc_buf.h
new file mode 100644
index 000000000000..e9b591eaa349
--- /dev/null
+++ b/drivers/staging/meilhaus/mecirc_buf.h
@@ -0,0 +1,131 @@
+/**
+ * @file mecirc_buf.h
+ *
+ * @brief Meilhaus circular buffer implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke  (k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _MECIRC_BUF_H_
+#define _MECIRC_BUF_H_
+
+# ifdef __KERNEL__
+
+#  ifdef BOSCH
+
+typedef struct me_circ_buf {
+	unsigned int mask;
+//      unsigned int count;
+	uint32_t *buf;
+	int volatile head;
+	int volatile tail;
+} me_circ_buf_t;
+
+static int inline me_circ_buf_values(me_circ_buf_t * buf)
+{
+//      return ((buf->head - buf->tail) & (buf->count - 1));
+	return ((buf->head - buf->tail) & (buf->mask));
+}
+
+static int inline me_circ_buf_space(me_circ_buf_t * buf)
+{
+//      return ((buf->tail - (buf->head + 1)) & (buf->count - 1));
+	return ((buf->tail - (buf->head + 1)) & (buf->mask));
+}
+
+static int inline me_circ_buf_values_to_end(me_circ_buf_t * buf)
+{
+	int end;
+	int n;
+//      end = buf->count - buf->tail;
+//      n = (buf->head + end) & (buf->count - 1);
+	end = buf->mask + 1 - buf->tail;
+	n = (buf->head + end) & (buf->mask);
+	return (n < end) ? n : end;
+}
+
+static int inline me_circ_buf_space_to_end(me_circ_buf_t * buf)
+{
+	int end;
+	int n;
+
+//      end = buf->count - 1 - buf->head;
+//      n = (end + buf->tail) & (buf->count - 1);
+	end = buf->mask - buf->head;
+	n = (end + buf->tail) & (buf->mask);
+	return (n <= end) ? n : (end + 1);
+}
+
+#define _CBUFF_32b_t
+
+#  else	//~BOSCH
+/// @note buf->mask = buf->count-1 = ME4600_AI_CIRC_BUF_COUNT-1
+
+#   ifdef _CBUFF_32b_t
+	//32 bit
+typedef struct me_circ_buf_32b {
+	int volatile head;
+	int volatile tail;
+	unsigned int mask;	//buffor size-1 must be 2^n-1 to work
+	uint32_t *buf;
+} me_circ_buf_t;
+#   else
+	//16 bit
+typedef struct me_circ_buf_16b {
+	int volatile head;
+	int volatile tail;
+	unsigned int mask;	//buffor size-1 must be 2^n-1 to work
+	uint16_t *buf;
+} me_circ_buf_t;
+#   endif //_CBUFF_32b_t
+
+/** How many values is in buffer */
+static int inline me_circ_buf_values(me_circ_buf_t * buf)
+{
+	return ((buf->head - buf->tail) & (buf->mask));
+}
+
+/** How many space left */
+static int inline me_circ_buf_space(me_circ_buf_t * buf)
+{
+	return ((buf->tail - (buf->head + 1)) & (buf->mask));
+}
+
+/** How many values can be read from buffor in one chunck. */
+static int inline me_circ_buf_values_to_end(me_circ_buf_t * buf)
+{
+	return (buf->tail <=
+		buf->head) ? (buf->head - buf->tail) : (buf->mask - buf->tail +
+							1);
+}
+
+/** How many values can be write to buffer in one chunck. */
+static int inline me_circ_buf_space_to_end(me_circ_buf_t * buf)
+{
+	return (buf->tail <=
+		buf->head) ? (buf->mask - buf->head + 1) : (buf->tail -
+							    buf->head - 1);
+}
+
+#  endif //BOSCH
+# endif	//__KERNEL__
+#endif //_MECIRC_BUF_H_
diff --git a/drivers/staging/meilhaus/mecommon.h b/drivers/staging/meilhaus/mecommon.h
new file mode 100644
index 000000000000..ef47c384e018
--- /dev/null
+++ b/drivers/staging/meilhaus/mecommon.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File	:mecommon.h
+ * Author	:GG (Guenter Gebhardt)	<g.gebhardt@meilhaus.de>
+ * Author	:KG (Krzysztof Gantzke)	<k.gantzke@meilhaus.de>
+ */
+
+#ifndef _MECOMMON_H_
+#define _MECOMMON_H_
+
+/*==================================================================
+  The version of this release
+  ================================================================*/
+
+#ifndef ME_VERSION_DRIVER
+/* Unknown version */
+# define ME_VERSION_DRIVER	0xFFFFFFFF
+#endif
+
+#ifndef LIBMEDRIVER_VERSION
+/* Unknown version */
+# define LIBMEDRIVER_VERSION	0xFFFFFFFF
+#endif
+
+#endif
diff --git a/drivers/staging/meilhaus/medebug.h b/drivers/staging/meilhaus/medebug.h
new file mode 100644
index 000000000000..dcfb97c26fd1
--- /dev/null
+++ b/drivers/staging/meilhaus/medebug.h
@@ -0,0 +1,125 @@
+/**
+ * @file medebug.h
+ *
+ * @brief Debugging defines.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+#ifndef _MEDEBUG_H_
+#define _MEDEBUG_H_
+
+#ifdef __KERNEL__
+
+#include <linux/kernel.h>
+
+//Messages control.
+
+#ifdef MEDEBUG_TEST_ALL		/* Switch to enable all info messages. */
+# ifndef MEDEBUG_TEST
+#  define MEDEBUG_TEST
+# endif
+# ifndef MEDEBUG_TEST_INFO
+#  define MEDEBUG_TEST_INFO
+# endif
+# ifndef MEDEBUG_DEBUG_REG
+#  define MEDEBUG_DEBUG_REG	/* Switch to enable registry access debuging messages. */
+# endif
+# ifndef MEDEBUG_DEBUG_LOCKS
+#  define MEDEBUG_DEBUG_LOCKS	/* Switch to enable locking messages. */
+# endif
+#endif
+
+#ifdef MEDEBUG_TEST_INFO	/* Switch to enable info  and test messages. */
+# ifndef MEDEBUG_INFO
+#  define MEDEBUG_INFO		/* Switch to enable info messages. */
+# endif
+# ifndef MEDEBUG_TEST
+#  define MEDEBUG_TEST
+# endif
+#endif
+
+#ifdef MEDEBUG_TEST		/* Switch to enable debug test messages. */
+# ifndef MEDEBUG_DEBUG
+#  define MEDEBUG_DEBUG		/* Switch to enable debug messages. */
+# endif
+# ifndef MEDEBUG_ERROR
+#  define MEDEBUG_ERROR		/* Switch to enable error messages. */
+# endif
+#endif
+
+#ifdef MEDEBUG_ERROR		/* Switch to enable error messages. */
+# ifndef MEDEBUG_ERROR_CRITICAL	/* Also critical error messages. */
+#  define MEDEBUG_ERROR_CRITICAL	/* Switch to enable high importance error messages. */
+# endif
+#endif
+
+#undef PDEBUG			/* Only to be sure. */
+#undef PINFO			/* Only to be sure. */
+#undef PERROR			/* Only to be sure. */
+#undef PERROR_CRITICAL		/* Only to be sure. */
+#undef PDEBUG_REG		/* Only to be sure. */
+#undef PDEBUG_LOCKS		/* Only to be sure. */
+#undef PSECURITY		/* Only to be sure. */
+#undef PLOG			/* Only to be sure. */
+
+#ifdef MEDEBUG_DEBUG
+# define PDEBUG(fmt, args...) \
+	printk(KERN_DEBUG"ME_DRV D: <%s> " fmt, __func__, ##args)
+#else
+# define PDEBUG(fmt, args...)
+#endif
+
+#ifdef MEDEBUG_DEBUG_LOCKS
+# define PDEBUG_LOCKS(fmt, args...) \
+	printk(KERN_DEBUG"ME_DRV L: <%s> " fmt, __func__, ##args)
+#else
+# define PDEBUG_LOCKS(fmt, args...)
+#endif
+
+#ifdef MEDEBUG_DEBUG_REG
+# define PDEBUG_REG(fmt, args...) \
+	printk(KERN_DEBUG"ME_DRV R: <%s:%d> REG:" fmt, __func__, __LINE__, ##args)
+#else
+# define PDEBUG_REG(fmt, args...)
+#endif
+
+#ifdef MEDEBUG_INFO
+# define PINFO(fmt, args...) \
+	printk(KERN_INFO"ME_DRV I: " fmt, ##args)
+#else
+# define PINFO(fmt, args...)
+#endif
+
+#ifdef MEDEBUG_ERROR
+# define PERROR(fmt, args...) \
+	printk(KERN_ERR"ME_DRV E: <%s:%i> " fmt, __FILE__, __LINE__, ##args)
+#else
+# define PERROR(fmt, args...)
+#endif
+
+#ifdef MEDEBUG_ERROR_CRITICAL
+# define PERROR_CRITICAL(fmt, args...) \
+	printk(KERN_CRIT"ME_DRV C: <%s:%i> " fmt, __FILE__, __LINE__, ##args)
+#else
+# define PERROR_CRITICAL(fmt, args...)
+#endif
+
+//This debug is only to detect logical errors!
+# define PSECURITY(fmt, args...) \
+	printk(KERN_CRIT"ME_DRV SECURITY: <%s:%s:%i> " fmt, __FILE__, __func__, __LINE__, ##args)
+//This debug is to keep track in customers' system
+# define PLOG(fmt, args...) \
+	printk(KERN_INFO"ME_DRV: " fmt, ##args)
+
+//This debug is to check new parts during development
+#ifdef MEDEBUG_DEVELOP
+# define PDEVELOP(fmt, args...) \
+	printk(KERN_CRIT"ME_DRV: <%s:%s:%i> " fmt, __FILE__, __func__, __LINE__, ##args)
+#else
+# define PDEVELOP(fmt, args...)
+#endif
+
+#endif //__KERNEL__
+#endif //_MEDEBUG_H_
diff --git a/drivers/staging/meilhaus/medefines.h b/drivers/staging/meilhaus/medefines.h
new file mode 100644
index 000000000000..6158ef5b80e6
--- /dev/null
+++ b/drivers/staging/meilhaus/medefines.h
@@ -0,0 +1,449 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : medefines.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ * Author      : KG (Krzysztof Gantzke)  <k.gantzke@meilhaus.de>
+ */
+
+#ifndef _MEDEFINES_H_
+#define _MEDEFINES_H_
+
+/*==================================================================
+  General
+  ================================================================*/
+
+#define ME_VALUE_NOT_USED							0x0
+#define ME_VALUE_INVALID							~0x0
+
+/*==================================================================
+  Defines common to access functions
+  ================================================================*/
+
+#define ME_LOCK_RELEASE								0x00010001
+#define ME_LOCK_SET									0x00010002
+#define ME_LOCK_CHECK								0x00010003
+
+/*==================================================================
+  Defines meOpen function
+  ================================================================*/
+
+#define ME_OPEN_NO_FLAGS							0x0
+
+/*==================================================================
+  Defines meClose function
+  ================================================================*/
+
+#define ME_CLOSE_NO_FLAGS							0x0
+
+/*==================================================================
+  Defines meLockDriver function
+  ================================================================*/
+
+#define ME_LOCK_DRIVER_NO_FLAGS						0x0
+
+/*==================================================================
+  Defines meLockDevice function
+  ================================================================*/
+
+#define ME_LOCK_DEVICE_NO_FLAGS						0x0
+
+/*==================================================================
+  Defines meLockSubdevice function
+  ================================================================*/
+
+#define ME_LOCK_SUBDEVICE_NO_FLAGS					0x0
+
+
+/*==================================================================
+  Defines common to error functions
+  ================================================================*/
+
+#define ME_ERROR_MSG_MAX_COUNT						256
+
+#define ME_SWITCH_DISABLE							0x00020001
+#define ME_SWITCH_ENABLE							0x00020002
+
+/*==================================================================
+  Defines common to io functions
+  ================================================================*/
+
+#define ME_REF_DIO_FIFO_LOW							0x00030001
+#define ME_REF_DIO_FIFO_HIGH						0x00030002
+
+#define ME_REF_CTR_PREVIOUS							0x00040001
+#define ME_REF_CTR_INTERNAL_1MHZ					0x00040002
+#define ME_REF_CTR_INTERNAL_10MHZ					0x00040003
+#define ME_REF_CTR_EXTERNAL							0x00040004
+
+#define ME_REF_AI_GROUND							0x00050001
+#define ME_REF_AI_DIFFERENTIAL						0x00050002
+
+#define ME_REF_AO_GROUND							0x00060001
+
+#define ME_TRIG_CHAN_DEFAULT						0x00070001
+#define ME_TRIG_CHAN_SYNCHRONOUS					0x00070002
+
+#define ME_TRIG_TYPE_NONE							0x00000000
+#define ME_TRIG_TYPE_SW								0x00080001
+#define ME_TRIG_TYPE_THRESHOLD						0x00080002
+#define ME_TRIG_TYPE_WINDOW							0x00080003
+#define ME_TRIG_TYPE_EDGE							0x00080004
+#define ME_TRIG_TYPE_SLOPE							0x00080005
+#define ME_TRIG_TYPE_EXT_DIGITAL					0x00080006
+#define ME_TRIG_TYPE_EXT_ANALOG						0x00080007
+#define ME_TRIG_TYPE_PATTERN						0x00080008
+#define ME_TRIG_TYPE_TIMER							0x00080009
+#define ME_TRIG_TYPE_COUNT							0x0008000A
+#define ME_TRIG_TYPE_FOLLOW							0x0008000B
+
+#define ME_TRIG_EDGE_NONE							0x00000000
+#define ME_TRIG_EDGE_ABOVE							0x00090001
+#define ME_TRIG_EDGE_BELOW							0x00090002
+#define ME_TRIG_EDGE_ENTRY							0x00090003
+#define ME_TRIG_EDGE_EXIT							0x00090004
+#define ME_TRIG_EDGE_RISING							0x00090005
+#define ME_TRIG_EDGE_FALLING						0x00090006
+#define ME_TRIG_EDGE_ANY							0x00090007
+
+#define ME_TIMER_ACQ_START							0x000A0001
+#define ME_TIMER_SCAN_START							0x000A0002
+#define ME_TIMER_CONV_START							0x000A0003
+
+/*==================================================================
+  Defines for meIOFrequencyToTicks function
+  ================================================================*/
+
+#define ME_IO_FREQUENCY_TO_TICKS_NO_FLAGS			0x0
+
+/*==================================================================
+  Defines for meIOIrqStart function
+  ================================================================*/
+
+#define ME_IRQ_SOURCE_DIO_PATTERN					0x000B0001
+#define ME_IRQ_SOURCE_DIO_MASK						0x000B0002
+#define ME_IRQ_SOURCE_DIO_LINE						0x000B0003
+#define ME_IRQ_SOURCE_DIO_OVER_TEMP					0x000B0004
+
+#define ME_IRQ_EDGE_NOT_USED						0x00000000
+#define ME_IRQ_EDGE_RISING							0x000C0001
+#define ME_IRQ_EDGE_FALLING							0x000C0002
+#define ME_IRQ_EDGE_ANY								0x000C0003
+
+/*==================================================================
+  Defines for meIOIrqStart function
+  ================================================================*/
+
+#define ME_IO_IRQ_START_NO_FLAGS					0x000000
+#define ME_IO_IRQ_START_DIO_BIT						0x000001
+#define ME_IO_IRQ_START_DIO_BYTE					0x000002
+#define ME_IO_IRQ_START_DIO_WORD					0x000004
+#define ME_IO_IRQ_START_DIO_DWORD					0x000008
+#define ME_IO_IRQ_START_PATTERN_FILTERING			0x000010
+#define ME_IO_IRQ_START_EXTENDED_STATUS				0x000020
+
+/*==================================================================
+  Defines for meIOIrqWait function
+  ================================================================*/
+
+#define ME_IO_IRQ_WAIT_NO_FLAGS						0x000000
+#define ME_IO_IRQ_WAIT_NORMAL_STATUS				0x000001
+#define ME_IO_IRQ_WAIT_EXTENDED_STATUS				0x000002
+
+/*==================================================================
+  Defines for meIOIrqStop function
+  ================================================================*/
+
+#define ME_IO_IRQ_STOP_NO_FLAGS						0x000000
+
+/*==================================================================
+  Defines for meIOIrqSetCallback function
+  ================================================================*/
+
+#define ME_IO_IRQ_SET_CALLBACK_NO_FLAGS				0x0
+
+/*==================================================================
+  Defines for meIOResetDevice function
+  ================================================================*/
+
+#define ME_IO_RESET_DEVICE_NO_FLAGS					0x0
+
+/*==================================================================
+  Defines for meIOResetSubdevice function
+  ================================================================*/
+
+#define ME_IO_RESET_SUBDEVICE_NO_FLAGS				0x0
+
+/*==================================================================
+  Defines for meIOSingleConfig function
+  ================================================================*/
+
+#define ME_SINGLE_CONFIG_DIO_INPUT					0x000D0001
+#define ME_SINGLE_CONFIG_DIO_OUTPUT					0x000D0002
+#define ME_SINGLE_CONFIG_DIO_HIGH_IMPEDANCE			0x000D0003
+#define ME_SINGLE_CONFIG_DIO_SINK					0x000D0004
+#define ME_SINGLE_CONFIG_DIO_SOURCE					0x000D0005
+#define ME_SINGLE_CONFIG_DIO_MUX32M					0x000D0006
+#define ME_SINGLE_CONFIG_DIO_DEMUX32				0x000D0007
+#define ME_SINGLE_CONFIG_DIO_BIT_PATTERN			0x000D0008
+
+#define ME_SINGLE_CONFIG_CTR_8254_MODE_0			0x000E0001
+#define ME_SINGLE_CONFIG_CTR_8254_MODE_1			0x000E0002
+#define ME_SINGLE_CONFIG_CTR_8254_MODE_2			0x000E0003
+#define ME_SINGLE_CONFIG_CTR_8254_MODE_3			0x000E0004
+#define ME_SINGLE_CONFIG_CTR_8254_MODE_4			0x000E0005
+#define ME_SINGLE_CONFIG_CTR_8254_MODE_5			0x000E0006
+
+#define ME_IO_SINGLE_CONFIG_NO_FLAGS				0x00
+#define ME_IO_SINGLE_CONFIG_DIO_BIT					0x01
+#define ME_IO_SINGLE_CONFIG_DIO_BYTE				0x02
+#define ME_IO_SINGLE_CONFIG_DIO_WORD				0x04
+#define ME_IO_SINGLE_CONFIG_DIO_DWORD				0x08
+#define ME_IO_SINGLE_CONFIG_MULTISIG_LED_ON			0x10
+#define ME_IO_SINGLE_CONFIG_MULTISIG_LED_OFF		0x20
+#define ME_IO_SINGLE_CONFIG_AI_RMS					0x40
+#define ME_IO_SINGLE_CONFIG_CONTINUE				0x80
+
+/*==================================================================
+  Defines for meIOSingle function
+  ================================================================*/
+
+#define ME_IO_SINGLE_NO_FLAGS						0x0
+#define ME_IO_SINGLE_NONBLOCKING					0x20
+
+#define ME_DIR_INPUT								0x000F0001
+#define ME_DIR_OUTPUT								0x000F0002
+
+#define ME_IO_SINGLE_TYPE_NO_FLAGS					0x00
+#define ME_IO_SINGLE_TYPE_DIO_BIT					0x01
+#define ME_IO_SINGLE_TYPE_DIO_BYTE					0x02
+#define ME_IO_SINGLE_TYPE_DIO_WORD					0x04
+#define ME_IO_SINGLE_TYPE_DIO_DWORD					0x08
+#define ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS			0x10
+#define ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING			0x20
+
+/*==================================================================
+  Defines for meIOStreamConfig function
+  ================================================================*/
+
+#define ME_IO_STREAM_CONFIG_NO_FLAGS				0x0
+#define ME_IO_STREAM_CONFIG_BIT_PATTERN				0x1
+#define ME_IO_STREAM_CONFIG_WRAPAROUND				0x2
+#define ME_IO_STREAM_CONFIG_SAMPLE_AND_HOLD			0x4
+#define ME_IO_STREAM_CONFIG_HARDWARE_ONLY			0x8
+
+#define ME_IO_STREAM_CONFIG_TYPE_NO_FLAGS			0x0
+
+#define ME_IO_STREAM_TRIGGER_TYPE_NO_FLAGS			0x0
+
+/*==================================================================
+  Defines for meIOStreamRead function
+  ================================================================*/
+
+#define ME_READ_MODE_BLOCKING						0x00100001
+#define ME_READ_MODE_NONBLOCKING					0x00100002
+
+#define ME_IO_STREAM_READ_NO_FLAGS					0x0
+#define ME_IO_STREAM_READ_FRAMES					0x1
+
+/*==================================================================
+  Defines for meIOStreamWrite function
+  ================================================================*/
+
+#define ME_WRITE_MODE_BLOCKING						0x00110001
+#define ME_WRITE_MODE_NONBLOCKING					0x00110002
+#define ME_WRITE_MODE_PRELOAD						0x00110003
+
+#define ME_IO_STREAM_WRITE_NO_FLAGS					0x00000000
+
+/*==================================================================
+  Defines for meIOStreamStart function
+  ================================================================*/
+
+#define ME_IO_STREAM_START_NO_FLAGS					0x00000000
+
+#define ME_START_MODE_BLOCKING						0x00120001
+#define ME_START_MODE_NONBLOCKING					0x00120002
+
+#define ME_IO_STREAM_START_TYPE_NO_FLAGS			0x0
+#define ME_IO_STREAM_START_TYPE_TRIG_SYNCHRONOUS	0x1
+
+/*==================================================================
+  Defines for meIOStreamStop function
+  ================================================================*/
+
+#define ME_IO_STREAM_STOP_NO_FLAGS					0x00000000
+#define ME_IO_STREAM_STOP_PRESERVE_BUFFERS			0x00000001
+
+#define ME_STOP_MODE_IMMEDIATE						0x00130001
+#define ME_STOP_MODE_LAST_VALUE						0x00130002
+
+#define ME_IO_STREAM_STOP_TYPE_NO_FLAGS				0x00000000
+
+/*==================================================================
+  Defines for meIOStreamStatus function
+  ================================================================*/
+
+#define ME_WAIT_NONE								0x00140001
+#define ME_WAIT_IDLE								0x00140002
+
+#define ME_STATUS_INVALID							0x00000000
+#define ME_STATUS_IDLE								0x00150001
+#define ME_STATUS_BUSY								0x00150002
+#define ME_STATUS_ERROR								0x00150003
+
+#define ME_IO_STREAM_STATUS_NO_FLAGS				0x00000000
+
+/*==================================================================
+  Defines for meIOStreamSetCallbacks function
+  ================================================================*/
+
+#define ME_IO_STREAM_SET_CALLBACKS_NO_FLAGS			0x00000000
+
+/*==================================================================
+  Defines for meIOStreamNewValues function
+  ================================================================*/
+
+#define ME_IO_STREAM_NEW_VALUES_NO_FLAGS			0x00000000
+
+/*==================================================================
+  Defines for meIOTimeToTicks function
+  ================================================================*/
+
+#define ME_IO_STREAM_TIME_TO_TICKS_NO_FLAGS			0x00000000
+
+/*==================================================================
+  Defines for module types
+  ================================================================*/
+
+#define ME_MODULE_TYPE_MULTISIG_NONE				0x00000000
+#define ME_MODULE_TYPE_MULTISIG_DIFF16_10V			0x00160001
+#define ME_MODULE_TYPE_MULTISIG_DIFF16_20V			0x00160002
+#define ME_MODULE_TYPE_MULTISIG_DIFF16_50V			0x00160003
+#define ME_MODULE_TYPE_MULTISIG_CURRENT16_0_20MA	0x00160004
+#define ME_MODULE_TYPE_MULTISIG_RTD8_PT100			0x00160005
+#define ME_MODULE_TYPE_MULTISIG_RTD8_PT500			0x00160006
+#define ME_MODULE_TYPE_MULTISIG_RTD8_PT1000			0x00160007
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_B			0x00160008
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_E			0x00160009
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_J			0x0016000A
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_K			0x0016000B
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_N			0x0016000C
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_R			0x0016000D
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_S			0x0016000E
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_T			0x0016000F
+#define ME_MODULE_TYPE_MULTISIG_TE8_TEMP_SENSOR		0x00160010
+
+/*==================================================================
+  Defines for meQuerySubdeviceCaps function
+  ================================================================*/
+
+#define ME_CAPS_NONE								0x00000000
+
+#define ME_CAPS_DIO_DIR_BIT							0x00000001
+#define ME_CAPS_DIO_DIR_BYTE						0x00000002
+#define ME_CAPS_DIO_DIR_WORD						0x00000004
+#define ME_CAPS_DIO_DIR_DWORD						0x00000008
+#define ME_CAPS_DIO_SINK_SOURCE						0x00000010
+#define ME_CAPS_DIO_BIT_PATTERN_IRQ					0x00000020
+#define ME_CAPS_DIO_BIT_MASK_IRQ_EDGE_RISING		0x00000040
+#define ME_CAPS_DIO_BIT_MASK_IRQ_EDGE_FALLING		0x00000080
+#define ME_CAPS_DIO_BIT_MASK_IRQ_EDGE_ANY			0x00000100
+#define ME_CAPS_DIO_OVER_TEMP_IRQ					0x00000200
+
+#define ME_CAPS_CTR_CLK_PREVIOUS					0x00000001
+#define ME_CAPS_CTR_CLK_INTERNAL_1MHZ				0x00000002
+#define ME_CAPS_CTR_CLK_INTERNAL_10MHZ				0x00000004
+#define ME_CAPS_CTR_CLK_EXTERNAL					0x00000008
+
+#define ME_CAPS_AI_TRIG_SYNCHRONOUS					0x00000001
+/// @note Backward compatibility for me1600 in old style.
+#define ME_CAPS_AI_TRIG_SIMULTANEOUS				ME_CAPS_AI_TRIG_SYNCHRONOUS
+#define ME_CAPS_AI_FIFO								0x00000002
+#define ME_CAPS_AI_FIFO_THRESHOLD					0x00000004
+
+#define ME_CAPS_AO_TRIG_SYNCHRONOUS					0x00000001
+/// @note Backward compatibility for me1600 in old style.
+#define ME_CAPS_AO_TRIG_SIMULTANEOUS				ME_CAPS_AO_TRIG_SYNCHRONOUS
+#define ME_CAPS_AO_FIFO								0x00000002
+#define ME_CAPS_AO_FIFO_THRESHOLD					0x00000004
+
+#define ME_CAPS_EXT_IRQ_EDGE_RISING					0x00000001
+#define ME_CAPS_EXT_IRQ_EDGE_FALLING				0x00000002
+#define ME_CAPS_EXT_IRQ_EDGE_ANY					0x00000004
+
+/*==================================================================
+  Defines for meQuerySubdeviceCapsArgs function
+  ================================================================*/
+
+#define ME_CAP_AI_FIFO_SIZE							0x001D0000
+#define ME_CAP_AI_BUFFER_SIZE						0x001D0001
+
+#define ME_CAP_AO_FIFO_SIZE							0x001F0000
+#define ME_CAP_AO_BUFFER_SIZE						0x001F0001
+
+#define ME_CAP_CTR_WIDTH							0x00200000
+
+/*==================================================================
+  Defines common to query functions
+  ================================================================*/
+
+#define ME_UNIT_INVALID								0x00000000
+#define ME_UNIT_VOLT								0x00170001
+#define ME_UNIT_AMPERE								0x00170002
+#define ME_UNIT_ANY									0x00170003
+
+#define ME_TYPE_INVALID								0x00000000
+#define ME_TYPE_AO									0x00180001
+#define ME_TYPE_AI									0x00180002
+#define ME_TYPE_DIO									0x00180003
+#define ME_TYPE_DO									0x00180004
+#define ME_TYPE_DI									0x00180005
+#define ME_TYPE_CTR									0x00180006
+#define ME_TYPE_EXT_IRQ								0x00180007
+
+#define ME_SUBTYPE_INVALID							0x00000000
+#define ME_SUBTYPE_SINGLE							0x00190001
+#define ME_SUBTYPE_STREAMING						0x00190002
+#define ME_SUBTYPE_CTR_8254							0x00190003
+#define ME_SUBTYPE_ANY								0x00190004
+
+#define ME_DEVICE_DRIVER_NAME_MAX_COUNT				64
+#define ME_DEVICE_NAME_MAX_COUNT					64
+
+#define ME_DEVICE_DESCRIPTION_MAX_COUNT				256
+
+#define ME_BUS_TYPE_INVALID							0x00000000
+#define ME_BUS_TYPE_PCI								0x001A0001
+#define ME_BUS_TYPE_USB								0x001A0002
+
+#define ME_PLUGGED_INVALID							0x00000000
+#define ME_PLUGGED_IN								0x001B0001
+#define ME_PLUGGED_OUT								0x001B0002
+
+#define ME_EXTENSION_TYPE_INVALID					0x00000000
+#define ME_EXTENSION_TYPE_NONE						0x001C0001
+#define ME_EXTENSION_TYPE_MUX32M					0x001C0002
+#define ME_EXTENSION_TYPE_DEMUX32					0x001C0003
+#define ME_EXTENSION_TYPE_MUX32S					0x001C0004
+
+#define ME_ACCESS_TYPE_INVALID						0x00000000
+#define ME_ACCESS_TYPE_LOCAL						0x001D0001
+#define ME_ACCESS_TYPE_REMOTE						0x001D0002
+
+/// @note Add by KG
+
+/*==================================================================
+  Defines for meUtilityPWM
+  ================================================================*/
+#define ME_PWM_START_CONNECT_INTERNAL				0x00200001
+
+/* Flags for SingleConfig channels configure */
+#define ME_SINGLE_CHANNEL_NOT_CONFIGURED			0x00
+#define ME_SINGLE_CHANNEL_CONFIGURED				0x01
+
+/* Define if configuration should be downloaded to driver */
+#define ME_CONFIG_LOAD_NO_FLAGS						0x0
+#define ME_CONFIG_LOAD_TO_DRIVER					0x1
+
+#endif
diff --git a/drivers/staging/meilhaus/medevice.c b/drivers/staging/meilhaus/medevice.c
new file mode 100644
index 000000000000..8f62e16c7a37
--- /dev/null
+++ b/drivers/staging/meilhaus/medevice.c
@@ -0,0 +1,1740 @@
+/**
+ * @file medevice.c
+ *
+ * @brief Meilhaus device base class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "mecommon.h"
+#include "meinternal.h"
+#include "medefines.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "medevice.h"
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+static int me_device_io_irq_start(struct me_device *device,
+				  struct file *filep,
+				  int subdevice,
+				  int channel,
+				  int irq_source,
+				  int irq_edge, int irq_arg, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_irq_start(s,
+						   filep,
+						   channel,
+						   irq_source,
+						   irq_edge, irq_arg, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_irq_wait(struct me_device *device,
+				 struct file *filep,
+				 int subdevice,
+				 int channel,
+				 int *irq_count,
+				 int *value, int time_out, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_irq_wait(s,
+						  filep,
+						  channel,
+						  irq_count,
+						  value, time_out, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_irq_stop(struct me_device *device,
+				 struct file *filep,
+				 int subdevice, int channel, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_irq_stop(s, filep, channel, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_reset_device(struct me_device *device,
+				     struct file *filep, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+	int i, n;
+
+	PDEBUG("executed.\n");
+
+	/* Get the number of subdevices. */
+	n = me_slist_get_number_subdevices(&device->slist);
+
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+
+	/* Reset every subdevice in list. */
+	for (i = 0; i < n; i++) {
+		s = me_slist_get_subdevice(&device->slist, i);
+		err = s->me_subdevice_io_reset_subdevice(s, filep, flags);
+
+		if (err) {
+			PERROR("Cannot reset subdevice.\n");
+			break;
+		}
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_reset_subdevice(struct me_device *device,
+					struct file *filep,
+					int subdevice, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_reset_subdevice(s, filep, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_single_config(struct me_device *device,
+				      struct file *filep,
+				      int subdevice,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_single_config(s,
+						       filep,
+						       channel,
+						       single_config,
+						       ref,
+						       trig_chan,
+						       trig_type,
+						       trig_edge, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_single_read(struct me_device *device,
+				    struct file *filep,
+				    int subdevice,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_single_read(s,
+						     filep,
+						     channel,
+						     value, time_out, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_single_write(struct me_device *device,
+				     struct file *filep,
+				     int subdevice,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_single_write(s,
+						      filep,
+						      channel,
+						      value, time_out, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_stream_config(struct me_device *device,
+				      struct file *filep,
+				      int subdevice,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_stream_config(s,
+						       filep,
+						       config_list,
+						       count,
+						       trigger,
+						       fifo_irq_threshold,
+						       flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_stream_new_values(struct me_device *device,
+					  struct file *filep,
+					  int subdevice,
+					  int time_out, int *count, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_stream_new_values(s,
+							   filep,
+							   time_out,
+							   count, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_stream_read(struct me_device *device,
+				    struct file *filep,
+				    int subdevice,
+				    int read_mode,
+				    int *values, int *count, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_stream_read(s,
+						     filep,
+						     read_mode,
+						     values, count, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_stream_start(struct me_device *device,
+				     struct file *filep,
+				     int subdevice,
+				     int start_mode, int time_out, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_stream_start(s,
+						      filep,
+						      start_mode,
+						      time_out, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_stream_status(struct me_device *device,
+				      struct file *filep,
+				      int subdevice,
+				      int wait,
+				      int *status, int *count, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_stream_status(s,
+						       filep,
+						       wait,
+						       status, count, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_stream_stop(struct me_device *device,
+				    struct file *filep,
+				    int subdevice, int stop_mode, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_stream_stop(s,
+						     filep, stop_mode, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_stream_write(struct me_device *device,
+				     struct file *filep,
+				     int subdevice,
+				     int write_mode,
+				     int *values, int *count, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_stream_write(s,
+						      filep,
+						      write_mode,
+						      values, count, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_lock_device(struct me_device *device,
+				 struct file *filep, int lock, int flags)
+{
+	PDEBUG("executed.\n");
+
+	return me_dlock_lock(&device->dlock,
+			     filep, lock, flags, &device->slist);
+}
+
+static int me_device_lock_subdevice(struct me_device *device,
+				    struct file *filep,
+				    int subdevice, int lock, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_lock_subdevice(s, filep, lock, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_query_description_device(struct me_device *device,
+					      char **description)
+{
+	PDEBUG("executed.\n");
+	*description = device->device_description;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me_device_query_info_device(struct me_device *device,
+				       int *vendor_id,
+				       int *device_id,
+				       int *serial_no,
+				       int *bus_type,
+				       int *bus_no,
+				       int *dev_no, int *func_no, int *plugged)
+{
+	PDEBUG("executed.\n");
+
+	if (device->bus_type == ME_BUS_TYPE_PCI) {
+		*vendor_id = device->info.pci.vendor_id;
+		*device_id = device->info.pci.device_id;
+		*serial_no = device->info.pci.serial_no;
+		*bus_type = ME_BUS_TYPE_PCI;
+		*bus_no = device->info.pci.pci_bus_no;
+		*dev_no = device->info.pci.pci_dev_no;
+		*func_no = device->info.pci.pci_func_no;
+		*plugged = ME_PLUGGED_IN;
+	} else {
+		*plugged = ME_PLUGGED_OUT;
+	}
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me_device_query_name_device(struct me_device *device, char **name)
+{
+	PDEBUG("executed.\n");
+	*name = device->device_name;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me_device_query_name_device_driver(struct me_device *device,
+					      char **name)
+{
+	PDEBUG("executed.\n");
+	*name = device->driver_name;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me_device_query_number_subdevices(struct me_device *device,
+					     int *number)
+{
+	PDEBUG("executed.\n");
+	return me_slist_query_number_subdevices(&device->slist, number);
+}
+
+static int me_device_query_number_channels(struct me_device *device,
+					   int subdevice, int *number)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_number_channels(s, number);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_number_ranges(struct me_device *device,
+					 int subdevice, int unit, int *count)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_number_ranges(s, unit, count);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_range_by_min_max(struct me_device *device,
+					    int subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_range_by_min_max(s,
+							     unit,
+							     min,
+							     max,
+							     maxdata, range);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_range_info(struct me_device *device,
+				      int subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_range_info(s,
+						       range,
+						       unit, min, max, maxdata);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_subdevice_by_type(struct me_device *device,
+					     int start_subdevice,
+					     int type,
+					     int subtype, int *subdevice)
+{
+	PDEBUG("executed.\n");
+
+	return me_slist_get_subdevice_by_type(&device->slist,
+					      start_subdevice,
+					      type, subtype, subdevice);
+}
+
+static int me_device_query_subdevice_type(struct me_device *device,
+					  int subdevice,
+					  int *type, int *subtype)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_subdevice_type(s, type, subtype);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_subdevice_caps(struct me_device *device,
+					  int subdevice, int *caps)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_subdevice_caps(s, caps);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_subdevice_caps_args(struct me_device *device,
+					       int subdevice,
+					       int cap, int *args, int count)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_subdevice_caps_args(s,
+								cap,
+								args, count);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_timer(struct me_device *device,
+				 int subdevice,
+				 int timer,
+				 int *base_frequency,
+				 uint64_t * min_ticks, uint64_t * max_ticks)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_timer(s,
+						  timer,
+						  base_frequency,
+						  min_ticks, max_ticks);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_version_device_driver(struct me_device *device,
+						 int *version)
+/** @todo Versions shold be read from driver. I must overwrite this function in each module. Here should be returned an error!
+*/
+{
+	PDEBUG("executed.\n");
+	*version = ME_VERSION_DRIVER;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me_device_config_load(struct me_device *device, struct file *filep,
+				 me_cfg_device_entry_t * config)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_SUCCESS;	//If no need for config return success.
+//      return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static void me_device_destructor(me_device_t * me_device)
+{
+	PDEBUG("executed.\n");
+	me_device_deinit(me_device);
+	kfree(me_device);
+}
+
+/* //me_device_usb_init
+int me_device_usb_init(me_device_t *me_device, struct usb_interface *interface)
+{
+	PDEBUG("executed.\n");
+	return -1;
+}
+*/
+
+static int get_device_descriptions(uint16_t device_id,
+				   char **device_name,
+				   char **device_description,
+				   char **driver_name)
+/** @todo This is wrong concept! Static table has too strong limitations!
+* 'device_name' and 'driver_name' should be calculated from 'device_id'
+* 'device_description' should be read from device or moved to user space and handled by library!
+*/
+{
+	PDEBUG("executed.\n");
+
+	switch (device_id) {
+	case PCI_DEVICE_ID_MEILHAUS_ME1000:
+	case PCI_DEVICE_ID_MEILHAUS_ME1000_A:
+	case PCI_DEVICE_ID_MEILHAUS_ME1000_B:
+		*device_name = ME1000_NAME_DEVICE_ME1000;
+		*device_description = ME1000_DESCRIPTION_DEVICE_ME1000;
+		*driver_name = ME1000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1400:
+		*device_name = ME1400_NAME_DEVICE_ME1400;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140A:
+		*device_name = ME1400_NAME_DEVICE_ME1400A;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400A;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140B:
+		*device_name = ME1400_NAME_DEVICE_ME1400B;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400B;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14E0:
+		*device_name = ME1400_NAME_DEVICE_ME1400E;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400E;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14EA:
+		*device_name = ME1400_NAME_DEVICE_ME1400EA;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400EA;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14EB:
+		*device_name = ME1400_NAME_DEVICE_ME1400EB;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400EB;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+		*device_name = ME1400_NAME_DEVICE_ME1400C;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400C;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		*device_name = ME1400_NAME_DEVICE_ME1400D;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400D;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_4U:
+		*device_name = ME1600_NAME_DEVICE_ME16004U;
+		*device_description = ME1600_DESCRIPTION_DEVICE_ME16004U;
+		*driver_name = ME1600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_8U:
+		*device_name = ME1600_NAME_DEVICE_ME16008U;
+		*device_description = ME1600_DESCRIPTION_DEVICE_ME16008U;
+		*driver_name = ME1600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_12U:
+		*device_name = ME1600_NAME_DEVICE_ME160012U;
+		*device_description = ME1600_DESCRIPTION_DEVICE_ME160012U;
+		*driver_name = ME1600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_16U:
+		*device_name = ME1600_NAME_DEVICE_ME160016U;
+		*device_description = ME1600_DESCRIPTION_DEVICE_ME160016U;
+		*driver_name = ME1600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_16U_8I:
+		*device_name = ME1600_NAME_DEVICE_ME160016U8I;
+		*device_description = ME1600_DESCRIPTION_DEVICE_ME160016U8I;
+		*driver_name = ME1600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4610:
+		*device_name = ME4600_NAME_DEVICE_ME4610;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4610;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4650:
+		*device_name = ME4600_NAME_DEVICE_ME4650;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4650;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660:
+		*device_name = ME4600_NAME_DEVICE_ME4660;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4660;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660I:
+		*device_name = ME4600_NAME_DEVICE_ME4660I;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4660I;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660S:
+		*device_name = ME4600_NAME_DEVICE_ME4660S;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4660S;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660IS:
+		*device_name = ME4600_NAME_DEVICE_ME4660IS;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4660IS;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670:
+		*device_name = ME4600_NAME_DEVICE_ME4670;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4670;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670I:
+		*device_name = ME4600_NAME_DEVICE_ME4670I;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4670I;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670S:
+		*device_name = ME4600_NAME_DEVICE_ME4670S;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4670S;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670IS:
+		*device_name = ME4600_NAME_DEVICE_ME4670IS;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4670IS;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680:
+		*device_name = ME4600_NAME_DEVICE_ME4680;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4680;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680I:
+		*device_name = ME4600_NAME_DEVICE_ME4680I;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4680I;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680S:
+		*device_name = ME4600_NAME_DEVICE_ME4680S;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4680S;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680IS:
+		*device_name = ME4600_NAME_DEVICE_ME4680IS;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4680IS;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6004:
+		*device_name = ME6000_NAME_DEVICE_ME60004;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME60004;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6008:
+		*device_name = ME6000_NAME_DEVICE_ME60008;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME60008;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME600F:
+		*device_name = ME6000_NAME_DEVICE_ME600016;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME600016;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6014:
+		*device_name = ME6000_NAME_DEVICE_ME6000I4;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000I4;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6018:
+		*device_name = ME6000_NAME_DEVICE_ME6000I8;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000I8;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME601F:
+		*device_name = ME6000_NAME_DEVICE_ME6000I16;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000I16;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6034:
+		*device_name = ME6000_NAME_DEVICE_ME6000ISLE4;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE4;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6038:
+		*device_name = ME6000_NAME_DEVICE_ME6000ISLE8;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE8;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME603F:
+		*device_name = ME6000_NAME_DEVICE_ME6000ISLE16;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE16;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6104:
+		*device_name = ME6000_NAME_DEVICE_ME61004;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME61004;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6108:
+		*device_name = ME6000_NAME_DEVICE_ME61008;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME61008;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME610F:
+		*device_name = ME6000_NAME_DEVICE_ME610016;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME610016;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6114:
+		*device_name = ME6000_NAME_DEVICE_ME6100I4;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100I4;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6118:
+		*device_name = ME6000_NAME_DEVICE_ME6100I8;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100I8;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME611F:
+		*device_name = ME6000_NAME_DEVICE_ME6100I16;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100I16;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6134:
+		*device_name = ME6000_NAME_DEVICE_ME6100ISLE4;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE4;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6138:
+		*device_name = ME6000_NAME_DEVICE_ME6100ISLE8;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE8;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME613F:
+		*device_name = ME6000_NAME_DEVICE_ME6100ISLE16;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE16;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6044:
+		*device_name = ME6000_NAME_DEVICE_ME60004DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME60004DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6048:
+		*device_name = ME6000_NAME_DEVICE_ME60008DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME60008DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME604F:
+		*device_name = ME6000_NAME_DEVICE_ME600016DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME600016DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6054:
+		*device_name = ME6000_NAME_DEVICE_ME6000I4DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000I4DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6058:
+		*device_name = ME6000_NAME_DEVICE_ME6000I8DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000I8DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME605F:
+		*device_name = ME6000_NAME_DEVICE_ME6000I16DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000I16DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6074:
+		*device_name = ME6000_NAME_DEVICE_ME6000ISLE4DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE4DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6078:
+		*device_name = ME6000_NAME_DEVICE_ME6000ISLE8DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE8DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME607F:
+		*device_name = ME6000_NAME_DEVICE_ME6000ISLE16DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE16DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6144:
+		*device_name = ME6000_NAME_DEVICE_ME61004DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME61004DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6148:
+		*device_name = ME6000_NAME_DEVICE_ME61008DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME61008DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME614F:
+		*device_name = ME6000_NAME_DEVICE_ME610016DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME610016DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6154:
+		*device_name = ME6000_NAME_DEVICE_ME6100I4DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100I4DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6158:
+		*device_name = ME6000_NAME_DEVICE_ME6100I8DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100I8DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME615F:
+		*device_name = ME6000_NAME_DEVICE_ME6100I16DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100I16DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6174:
+		*device_name = ME6000_NAME_DEVICE_ME6100ISLE4DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE4DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6178:
+		*device_name = ME6000_NAME_DEVICE_ME6100ISLE8DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE8DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME617F:
+		*device_name = ME6000_NAME_DEVICE_ME6100ISLE16DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE16DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6259:
+		*device_name = ME6000_NAME_DEVICE_ME6200I9DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6200I9DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6359:
+		*device_name = ME6000_NAME_DEVICE_ME6300I9DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6300I9DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0630:
+		*device_name = ME0600_NAME_DEVICE_ME0630;
+		*device_description = ME0600_DESCRIPTION_DEVICE_ME0630;
+		*driver_name = ME0600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_A:
+		*device_name = ME8100_NAME_DEVICE_ME8100A;
+		*device_description = ME8100_DESCRIPTION_DEVICE_ME8100A;
+		*driver_name = ME8100_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_B:
+		*device_name = ME8100_NAME_DEVICE_ME8100B;
+		*device_description = ME8100_DESCRIPTION_DEVICE_ME8100B;
+		*driver_name = ME8100_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8200_A:
+		*device_name = ME8200_NAME_DEVICE_ME8200A;
+		*device_description = ME8200_DESCRIPTION_DEVICE_ME8200A;
+		*driver_name = ME8200_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8200_B:
+		*device_name = ME8200_NAME_DEVICE_ME8200B;
+		*device_description = ME8200_DESCRIPTION_DEVICE_ME8200B;
+		*driver_name = ME8200_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0940:
+		*device_name = ME0900_NAME_DEVICE_ME0940;
+		*device_description = ME0900_DESCRIPTION_DEVICE_ME0940;
+		*driver_name = ME0900_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0950:
+		*device_name = ME0900_NAME_DEVICE_ME0950;
+		*device_description = ME0900_DESCRIPTION_DEVICE_ME0950;
+		*driver_name = ME0900_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0960:
+		*device_name = ME0900_NAME_DEVICE_ME0960;
+		*device_description = ME0900_DESCRIPTION_DEVICE_ME0960;
+		*driver_name = ME0900_NAME_DRIVER;
+		break;
+/*
+		case USB_DEVICE_ID_MEPHISTO_S1:
+			*device_name = MEPHISTO_S1_NAME_DEVICE;
+			*device_description = MEPHISTO_S1_DESCRIPTION_DEVICE;
+			*driver_name = MEPHISTO_S1_NAME_DRIVER;
+			break;
+*/
+	default:
+		*device_name = EMPTY_NAME_DEVICE;
+		*device_description = EMPTY_DESCRIPTION_DEVICE;
+		*driver_name = EMPTY_NAME_DRIVER;
+
+		PERROR("Invalid device id.\n");
+
+		return 1;
+	}
+
+	return 0;
+}
+
+int me_device_pci_init(me_device_t * me_device, struct pci_dev *pci_device)
+{
+	int err;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Initialize device list head.
+	INIT_LIST_HEAD(&me_device->list);
+
+	// Initialize device description strings.
+	err = get_device_descriptions(pci_device->device,
+				      &me_device->device_name,
+				      &me_device->device_description,
+				      &me_device->driver_name);
+
+	if (err) {
+		PERROR("Cannot initialize device description strings.\n");
+		return 1;
+	}
+	// Enable the pci device.
+	err = pci_enable_device(pci_device);
+
+	if (err < 0) {
+		PERROR("Cannot enable PCI device.\n");
+		return 1;
+	}
+	// Request the PCI register regions.
+	err = pci_request_regions(pci_device, me_device->device_name);
+
+	if (err < 0) {
+		PERROR("Cannot request PCI regions.\n");
+		goto ERROR_0;
+	}
+	// The bus carrying the device is a PCI bus.
+	me_device->bus_type = ME_BUS_TYPE_PCI;
+
+	// Store the PCI information for later usage.
+	me_device->info.pci.pci_device = pci_device;
+
+	// Get PCI register bases and sizes.
+	for (i = 0; i < 6; i++) {
+		me_device->info.pci.reg_bases[i] =
+		    pci_resource_start(pci_device, i);
+		me_device->info.pci.reg_sizes[i] =
+		    pci_resource_len(pci_device, i);
+	}
+
+	// Get the PCI location.
+	me_device->info.pci.pci_bus_no = pci_device->bus->number;
+	me_device->info.pci.pci_dev_no = PCI_SLOT(pci_device->devfn);
+	me_device->info.pci.pci_func_no = PCI_FUNC(pci_device->devfn);
+
+	// Get Meilhaus specific device information.
+	me_device->info.pci.vendor_id = pci_device->vendor;
+	me_device->info.pci.device_id = pci_device->device;
+	pci_read_config_byte(pci_device, 0x08,
+			     &me_device->info.pci.hw_revision);
+	pci_read_config_dword(pci_device, 0x2C, &me_device->info.pci.serial_no);
+
+	// Get the interrupt request number.
+	me_device->irq = pci_device->irq;
+
+	// Initialize device lock instance.
+	err = me_dlock_init(&me_device->dlock);
+
+	if (err) {
+		PERROR("Cannot initialize device lock instance.\n");
+		goto ERROR_1;
+	}
+	// Initialize subdevice list instance.
+	me_slist_init(&me_device->slist);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice list instance.\n");
+		goto ERROR_2;
+	}
+	// Initialize method pointers.
+	me_device->me_device_io_irq_start = me_device_io_irq_start;
+	me_device->me_device_io_irq_wait = me_device_io_irq_wait;
+	me_device->me_device_io_irq_stop = me_device_io_irq_stop;
+	me_device->me_device_io_reset_device = me_device_io_reset_device;
+	me_device->me_device_io_reset_subdevice = me_device_io_reset_subdevice;
+	me_device->me_device_io_single_config = me_device_io_single_config;
+	me_device->me_device_io_single_read = me_device_io_single_read;
+	me_device->me_device_io_single_write = me_device_io_single_write;
+	me_device->me_device_io_stream_config = me_device_io_stream_config;
+	me_device->me_device_io_stream_new_values =
+	    me_device_io_stream_new_values;
+	me_device->me_device_io_stream_read = me_device_io_stream_read;
+	me_device->me_device_io_stream_start = me_device_io_stream_start;
+	me_device->me_device_io_stream_status = me_device_io_stream_status;
+	me_device->me_device_io_stream_stop = me_device_io_stream_stop;
+	me_device->me_device_io_stream_write = me_device_io_stream_write;
+	me_device->me_device_lock_device = me_device_lock_device;
+	me_device->me_device_lock_subdevice = me_device_lock_subdevice;
+	me_device->me_device_query_description_device =
+	    me_device_query_description_device;
+	me_device->me_device_query_info_device = me_device_query_info_device;
+	me_device->me_device_query_name_device = me_device_query_name_device;
+	me_device->me_device_query_name_device_driver =
+	    me_device_query_name_device_driver;
+	me_device->me_device_query_number_subdevices =
+	    me_device_query_number_subdevices;
+	me_device->me_device_query_number_channels =
+	    me_device_query_number_channels;
+	me_device->me_device_query_number_ranges =
+	    me_device_query_number_ranges;
+	me_device->me_device_query_range_by_min_max =
+	    me_device_query_range_by_min_max;
+	me_device->me_device_query_range_info = me_device_query_range_info;
+	me_device->me_device_query_subdevice_by_type =
+	    me_device_query_subdevice_by_type;
+	me_device->me_device_query_subdevice_type =
+	    me_device_query_subdevice_type;
+	me_device->me_device_query_subdevice_caps =
+	    me_device_query_subdevice_caps;
+	me_device->me_device_query_subdevice_caps_args =
+	    me_device_query_subdevice_caps_args;
+	me_device->me_device_query_timer = me_device_query_timer;
+	me_device->me_device_query_version_device_driver =
+	    me_device_query_version_device_driver;
+	me_device->me_device_config_load = me_device_config_load;
+	me_device->me_device_destructor = me_device_destructor;
+
+	return 0;
+
+      ERROR_0:
+	me_dlock_deinit(&me_device->dlock);
+
+      ERROR_1:
+	pci_release_regions(pci_device);
+
+      ERROR_2:
+	pci_disable_device(pci_device);
+
+	return 1;
+}
+
+void me_device_deinit(me_device_t * me_device)
+{
+	PDEBUG("executed.\n");
+
+	me_slist_deinit(&me_device->slist);
+	me_dlock_deinit(&me_device->dlock);
+
+	if (me_device->bus_type == ME_BUS_TYPE_PCI) {
+		pci_release_regions(me_device->info.pci.pci_device);
+		pci_disable_device(me_device->info.pci.pci_device);
+	}
+/*
+	else
+	{
+		// Must be an USB device.
+	}
+*/
+}
diff --git a/drivers/staging/meilhaus/medevice.h b/drivers/staging/meilhaus/medevice.h
new file mode 100644
index 000000000000..25da82883e1f
--- /dev/null
+++ b/drivers/staging/meilhaus/medevice.h
@@ -0,0 +1,304 @@
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : medevice.h
+ * Author      : GG (Guenter Gebhardt)  <support@meilhaus.de>
+ */
+
+#ifndef _MEDEVICE_H_
+#define _MEDEVICE_H_
+
+#ifndef KBUILD_MODNAME
+#  define KBUILD_MODNAME KBUILD_STR(memain)
+#endif
+
+#include <linux/pci.h>
+//#include <linux/usb.h>
+#include <linux/fs.h>
+#include <linux/spinlock.h>
+
+#include "metypes.h"
+#include "meslist.h"
+#include "medlock.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Defines a pointer type to a PCI constructor function.
+ */
+typedef struct me_device *(*me_pci_constructor_t) (struct pci_dev *);
+
+/**
+ * @brief Defines a pointer type to a ME-4000 PCI constructor function.
+ */
+#ifdef BOSCH
+typedef struct me_device *(*me_bosch_constructor_t) (struct pci_dev *,
+						     int me_bosch_fw);
+#endif
+
+/**
+ * @brief Defines a pointer type to a USB constructor function.
+ */
+//typedef struct me_device *(*me_usb_constructor_t)(struct usb_interface *);
+
+/**
+ * @brief Defines a pointer type to the dummy constructor function.
+ */
+typedef struct me_device *(*me_dummy_constructor_t) (unsigned short vendor_id,
+						     unsigned short device_id,
+						     unsigned int serial_no,
+						     int bus_type,
+						     int bus_no,
+						     int dev_no, int func_no);
+
+//extern me_usb_constructor_t mephisto_s1_constructor __attribute__ ((weak));
+
+/**
+ * @brief Holds the PCI device information.
+ */
+typedef struct me_pci_info {
+	struct pci_dev *pci_device;			/**< Kernel PCI device structure. */
+	uint32_t reg_bases[6];				/**< The base adresses of the PCI bars. */
+	uint32_t reg_sizes[6];				/**< The sizes of the PCI bars. */
+
+	uint32_t pci_bus_no;				/**< PCI bus number. */
+	uint32_t pci_dev_no;				/**< PCI device number. */
+	uint32_t pci_func_no;				/**< PCI function number. */
+
+	uint16_t vendor_id;					/**< Meilhaus PCI vendor id. */
+	uint16_t device_id;					/**< Meilhaus device id. */
+	uint8_t hw_revision;				/**< Hardware revision of the device. */
+	uint32_t serial_no;					/**< Serial number of the device. */
+} me_pci_info_t;
+
+/**
+ * @brief Holds the USB device information.
+ */
+//typedef struct me_usb_info {
+//} me_usb_info_t;
+
+/**
+ * @brief The Meilhaus device base class structure.
+ */
+typedef struct me_device {
+	/* Attributes */
+	struct list_head list;				/**< Enables the device to be added to a dynamic list. */
+//      int magic;                                                      /**< The magic number of the structure. */
+
+	int bus_type;						/**< The descriminator for the union. */
+	union {
+		me_pci_info_t pci;				/**< PCI specific device information. */
+//              me_usb_info_t usb;                              /**< USB specific device information. */
+	} info;								/**< Holds the device information. */
+
+	int irq;							/**< The irq assigned to this device. */
+
+	me_dlock_t dlock;					/**< The device locking structure. */
+	me_slist_t slist;					/**< The container holding all subdevices belonging to this device. */
+
+	char *device_name;					/**< The name of the Meilhaus device. */
+	char *device_description;			/**< The description of the Meilhaus device. */
+	char *driver_name;					/**< The name of the device driver module supporting the device family. */
+
+	/* Methods */
+	int (*me_device_io_irq_start) (struct me_device * device,
+				       struct file * filep,
+				       int subdevice,
+				       int channel,
+				       int irq_source,
+				       int irq_edge, int irq_arg, int flags);
+
+	int (*me_device_io_irq_wait) (struct me_device * device,
+				      struct file * filep,
+				      int subdevice,
+				      int channel,
+				      int *irq_count,
+				      int *value, int time_out, int flags);
+
+	int (*me_device_io_irq_stop) (struct me_device * device,
+				      struct file * filep,
+				      int subdevice, int channel, int flags);
+
+	int (*me_device_io_reset_device) (struct me_device * device,
+					  struct file * filep, int flags);
+
+	int (*me_device_io_reset_subdevice) (struct me_device * device,
+					     struct file * filep,
+					     int subdevice, int flags);
+
+	int (*me_device_io_single_config) (struct me_device * device,
+					   struct file * filep,
+					   int subdevice,
+					   int channel,
+					   int single_config,
+					   int ref,
+					   int trig_chan,
+					   int trig_type,
+					   int trig_edge, int flags);
+
+	int (*me_device_io_single_read) (struct me_device * device,
+					 struct file * filep,
+					 int subdevice,
+					 int channel,
+					 int *value, int time_out, int flags);
+
+	int (*me_device_io_single_write) (struct me_device * device,
+					  struct file * filep,
+					  int subdevice,
+					  int channel,
+					  int value, int time_out, int flags);
+
+	int (*me_device_io_stream_config) (struct me_device * device,
+					   struct file * filep,
+					   int subdevice,
+					   meIOStreamConfig_t * config_list,
+					   int count,
+					   meIOStreamTrigger_t * trigger,
+					   int fifo_irq_threshold, int flags);
+
+	int (*me_device_io_stream_new_values) (struct me_device * device,
+					       struct file * filep,
+					       int subdevice,
+					       int time_out,
+					       int *count, int flags);
+
+	int (*me_device_io_stream_read) (struct me_device * device,
+					 struct file * filep,
+					 int subdevice,
+					 int read_mode,
+					 int *values, int *count, int flags);
+
+	int (*me_device_io_stream_start) (struct me_device * device,
+					  struct file * filep,
+					  int subdevice,
+					  int start_mode,
+					  int time_out, int flags);
+
+	int (*me_device_io_stream_status) (struct me_device * device,
+					   struct file * filep,
+					   int subdevice,
+					   int wait,
+					   int *status, int *count, int flags);
+
+	int (*me_device_io_stream_stop) (struct me_device * device,
+					 struct file * filep,
+					 int subdevice,
+					 int stop_mode, int flags);
+
+	int (*me_device_io_stream_write) (struct me_device * device,
+					  struct file * filep,
+					  int subdevice,
+					  int write_mode,
+					  int *values, int *count, int flags);
+
+	int (*me_device_lock_device) (struct me_device * device,
+				      struct file * filep, int lock, int flags);
+
+	int (*me_device_lock_subdevice) (struct me_device * device,
+					 struct file * filep,
+					 int subdevice, int lock, int flags);
+
+	int (*me_device_query_description_device) (struct me_device * device,
+						   char **description);
+
+	int (*me_device_query_info_device) (struct me_device * device,
+					    int *vendor_id,
+					    int *device_id,
+					    int *serial_no,
+					    int *bus_type,
+					    int *bus_no,
+					    int *dev_no,
+					    int *func_no, int *plugged);
+
+	int (*me_device_query_name_device) (struct me_device * device,
+					    char **name);
+
+	int (*me_device_query_name_device_driver) (struct me_device * device,
+						   char **name);
+
+	int (*me_device_query_number_subdevices) (struct me_device * device,
+						  int *number);
+
+	int (*me_device_query_number_channels) (struct me_device * device,
+						int subdevice, int *number);
+
+	int (*me_device_query_number_ranges) (struct me_device * device,
+					      int subdevice,
+					      int unit, int *count);
+
+	int (*me_device_query_range_by_min_max) (struct me_device * device,
+						 int subdevice,
+						 int unit,
+						 int *min,
+						 int *max,
+						 int *maxdata, int *range);
+
+	int (*me_device_query_range_info) (struct me_device * device,
+					   int subdevice,
+					   int range,
+					   int *unit,
+					   int *min, int *max, int *maxdata);
+
+	int (*me_device_query_subdevice_by_type) (struct me_device * device,
+						  int start_subdevice,
+						  int type,
+						  int subtype, int *subdevice);
+
+	int (*me_device_query_subdevice_type) (struct me_device * device,
+					       int subdevice,
+					       int *type, int *subtype);
+
+	int (*me_device_query_subdevice_caps) (struct me_device * device,
+					       int subdevice, int *caps);
+
+	int (*me_device_query_subdevice_caps_args) (struct me_device * device,
+						    int subdevice,
+						    int cap,
+						    int *args, int count);
+
+	int (*me_device_query_timer) (struct me_device * device,
+				      int subdevice,
+				      int timer,
+				      int *base_frequency,
+				      uint64_t * min_ticks,
+				      uint64_t * max_ticks);
+
+	int (*me_device_query_version_device_driver) (struct me_device * device,
+						      int *version);
+
+	int (*me_device_config_load) (struct me_device * device,
+				      struct file * filep,
+				      me_cfg_device_entry_t * config);
+
+	void (*me_device_destructor) (struct me_device * device);
+} me_device_t;
+
+/**
+ * @brief Initializes a PCI device base class structure.
+ *
+ * @param pci_device The PCI device context as handed over by kernel.
+ *
+ * @return 0 on success.
+ */
+int me_device_pci_init(me_device_t * me_device, struct pci_dev *pci_device);
+
+/**
+ * @brief Initializes a USB device base class structure.
+ *
+ * @param usb_interface The USB device interface as handed over by kernel.
+ *
+ * @return 0 on success.
+ */
+//int me_device_usb_init(me_device_t *me_device, struct usb_interface *interface);
+
+/**
+  * @brief Deinitializes a device base class structure and frees any previously
+  * requested resources related with this structure. It also frees any subdevice
+  * instance hold by the subdevice list.
+  *
+  * @param me_device The device class to deinitialize.
+  */
+void me_device_deinit(me_device_t * me_device);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/medlist.c b/drivers/staging/meilhaus/medlist.c
new file mode 100644
index 000000000000..ef4e36955dc8
--- /dev/null
+++ b/drivers/staging/meilhaus/medlist.c
@@ -0,0 +1,127 @@
+/**
+ * @file me_dlist.c
+ *
+ * @brief Implements the device list class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "meerror.h"
+#include "medefines.h"
+
+#include "medlist.h"
+#include "medebug.h"
+
+int me_dlist_query_number_devices(struct me_dlist *dlist, int *number)
+{
+	PDEBUG_LOCKS("called.\n");
+	*number = dlist->n;
+	return ME_ERRNO_SUCCESS;
+}
+
+unsigned int me_dlist_get_number_devices(struct me_dlist *dlist)
+{
+	PDEBUG_LOCKS("called.\n");
+	return dlist->n;
+}
+
+me_device_t *me_dlist_get_device(struct me_dlist * dlist, unsigned int index)
+{
+
+	struct list_head *pos;
+	me_device_t *device = NULL;
+	unsigned int i = 0;
+
+	PDEBUG_LOCKS("called.\n");
+
+	if (index >= dlist->n) {
+		PERROR("Index out of range.\n");
+		return NULL;
+	}
+
+	list_for_each(pos, &dlist->head) {
+		if (i == index) {
+			device = list_entry(pos, me_device_t, list);
+			break;
+		}
+
+		++i;
+	}
+
+	return device;
+}
+
+void me_dlist_add_device_tail(struct me_dlist *dlist, me_device_t * device)
+{
+	PDEBUG_LOCKS("called.\n");
+
+	list_add_tail(&device->list, &dlist->head);
+	++dlist->n;
+}
+
+me_device_t *me_dlist_del_device_tail(struct me_dlist *dlist)
+{
+
+	struct list_head *last;
+	me_device_t *device;
+
+	PDEBUG_LOCKS("called.\n");
+
+	if (list_empty(&dlist->head))
+		return NULL;
+
+	last = dlist->head.prev;
+
+	device = list_entry(last, me_device_t, list);
+
+	list_del(last);
+
+	--dlist->n;
+
+	return device;
+}
+
+int me_dlist_init(me_dlist_t * dlist)
+{
+	PDEBUG_LOCKS("called.\n");
+
+	INIT_LIST_HEAD(&dlist->head);
+	dlist->n = 0;
+	return 0;
+}
+
+void me_dlist_deinit(me_dlist_t * dlist)
+{
+
+	struct list_head *s;
+	me_device_t *device;
+
+	PDEBUG_LOCKS("called.\n");
+
+	while (!list_empty(&dlist->head)) {
+		s = dlist->head.next;
+		list_del(s);
+		device = list_entry(s, me_device_t, list);
+		device->me_device_destructor(device);
+	}
+
+	dlist->n = 0;
+}
diff --git a/drivers/staging/meilhaus/medlist.h b/drivers/staging/meilhaus/medlist.h
new file mode 100644
index 000000000000..091c11e48ed2
--- /dev/null
+++ b/drivers/staging/meilhaus/medlist.h
@@ -0,0 +1,91 @@
+/**
+ * @file me_dlist.h
+ *
+ * @brief Provides the device list class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME_DLIST_H_
+#define _ME_DLIST_H_
+
+#include <linux/list.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The device list container.
+ */
+typedef struct me_dlist {
+	struct list_head head;		/**< The head of the internal list. */
+	unsigned int n;			/**< The number of devices in the list. */
+} me_dlist_t;
+
+/**
+ * @brief Queries the number of devices currently inside the list.
+ *
+ * @param dlist The device list to query.
+ * @param[out] number The number of devices.
+ *
+ * @return ME-iDS error code.
+ */
+int me_dlist_query_number_devices(struct me_dlist *dlist, int *number);
+
+/**
+ * @brief Returns the number of devices currently inside the list.
+ *
+ * @param dlist The device list to query.
+ *
+ * @return The number of devices in the list.
+ */
+unsigned int me_dlist_get_number_devices(struct me_dlist *dlist);
+
+/**
+ * @brief Get a device by index.
+ *
+ * @param dlist The device list to query.
+ * @param index The index of the device to get in the list.
+ *
+ * @return The device at index if available.\n
+ *         NULL if the index is out of range.
+ */
+me_device_t *me_dlist_get_device(struct me_dlist *dlist, unsigned int index);
+
+/**
+ * @brief Adds a device to the tail of the list.
+ *
+ * @param dlist The device list to add a device to.
+ * @param device The device to add to the list.
+ */
+void me_dlist_add_device_tail(struct me_dlist *dlist, me_device_t * device);
+
+/**
+ * @brief Removes a device from the tail of the list.
+ *
+ * @param dlist The device list.
+ *
+ * @return Pointer to the removed subdeivce.\n
+ *         NULL in cases where the list was empty.
+ */
+me_device_t *me_dlist_del_device_tail(struct me_dlist *dlist);
+
+/**
+ * @brief Initializes a device list structure.
+ *
+ * @param lock The device list structure to initialize.
+ * @return 0 on success.
+ */
+int me_dlist_init(me_dlist_t * dlist);
+
+/**
+ * @brief Deinitializes a device list structure and destructs every device in it.
+ *
+ * @param dlist The device list structure to deinitialize.
+ * @return 0 on success.
+ */
+void me_dlist_deinit(me_dlist_t * dlist);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/medlock.c b/drivers/staging/meilhaus/medlock.c
new file mode 100644
index 000000000000..f649e3da4f05
--- /dev/null
+++ b/drivers/staging/meilhaus/medlock.c
@@ -0,0 +1,195 @@
+/**
+ * @file medlock.c
+ *
+ * @brief Implements the device lock class.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/spinlock.h>
+
+#include "medefines.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "meslist.h"
+#include "mesubdevice.h"
+#include "medlock.h"
+
+int me_dlock_enter(struct me_dlock *dlock, struct file *filep)
+{
+	PDEBUG_LOCKS("executed.\n");
+
+	spin_lock(&dlock->spin_lock);
+
+	if ((dlock->filep) != NULL && (dlock->filep != filep)) {
+		PERROR("Device is locked by another process.\n");
+		spin_unlock(&dlock->spin_lock);
+		return ME_ERRNO_LOCKED;
+	}
+
+	dlock->count++;
+
+	spin_unlock(&dlock->spin_lock);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+int me_dlock_exit(struct me_dlock *dlock, struct file *filep)
+{
+	PDEBUG_LOCKS("executed.\n");
+
+	spin_lock(&dlock->spin_lock);
+	dlock->count--;
+	spin_unlock(&dlock->spin_lock);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+int me_dlock_lock(struct me_dlock *dlock,
+		  struct file *filep, int lock, int flags, me_slist_t * slist)
+{
+	int err = ME_ERRNO_SUCCESS;
+	int i;
+	me_subdevice_t *subdevice;
+
+	PDEBUG_LOCKS("executed.\n");
+
+	spin_lock(&dlock->spin_lock);
+
+	switch (lock) {
+
+	case ME_LOCK_RELEASE:
+		if ((dlock->filep == filep) || (dlock->filep == NULL)) {
+			dlock->filep = NULL;
+
+			/* Unlock all possibly locked subdevices. */
+
+			for (i = 0; i < me_slist_get_number_subdevices(slist);
+			     i++) {
+				subdevice = me_slist_get_subdevice(slist, i);
+
+				if (subdevice)
+					err =
+					    subdevice->
+					    me_subdevice_lock_subdevice
+					    (subdevice, filep, ME_LOCK_RELEASE,
+					     flags);
+				else
+					err = ME_ERRNO_INTERNAL;
+			}
+		}
+
+		break;
+
+	case ME_LOCK_SET:
+		if (dlock->count) {
+			PERROR("Device is used by another process.\n");
+			err = ME_ERRNO_USED;
+		} else if ((dlock->filep != NULL) && (dlock->filep != filep)) {
+			PERROR("Device is locked by another process.\n");
+			err = ME_ERRNO_LOCKED;
+		} else if (dlock->filep == NULL) {
+			/* Check any subdevice is locked by another process. */
+
+			for (i = 0; i < me_slist_get_number_subdevices(slist);
+			     i++) {
+				subdevice = me_slist_get_subdevice(slist, i);
+
+				if (subdevice) {
+					if ((err =
+					     subdevice->
+					     me_subdevice_lock_subdevice
+					     (subdevice, filep, ME_LOCK_CHECK,
+					      flags))) {
+						PERROR
+						    ("A subdevice is locked by another process.\n");
+						break;
+					}
+				} else {
+					err = ME_ERRNO_INTERNAL;
+				}
+			}
+
+			/* If no subdevices are locked by other processes,
+			   we can take ownership of the device. Otherwise we jump ahead. */
+			if (!err)
+				dlock->filep = filep;
+		}
+
+		break;
+
+	case ME_LOCK_CHECK:
+		if (dlock->count) {
+			err = ME_ERRNO_USED;
+		} else if ((dlock->filep != NULL) && (dlock->filep != filep)) {
+			err = ME_ERRNO_LOCKED;
+		} else if (dlock->filep == NULL) {
+			for (i = 0; i < me_slist_get_number_subdevices(slist);
+			     i++) {
+				subdevice = me_slist_get_subdevice(slist, i);
+
+				if (subdevice) {
+					if ((err =
+					     subdevice->
+					     me_subdevice_lock_subdevice
+					     (subdevice, filep, ME_LOCK_CHECK,
+					      flags))) {
+						PERROR
+						    ("A subdevice is locked by another process.\n");
+						break;
+					}
+				} else {
+					err = ME_ERRNO_INTERNAL;
+				}
+			}
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid lock.\n");
+
+		err = ME_ERRNO_INVALID_LOCK;
+
+		break;
+	}
+
+	spin_unlock(&dlock->spin_lock);
+
+	return err;
+}
+
+void me_dlock_deinit(struct me_dlock *dlock)
+{
+	PDEBUG_LOCKS("executed.\n");
+}
+
+int me_dlock_init(me_dlock_t * dlock)
+{
+	PDEBUG_LOCKS("executed.\n");
+
+	dlock->filep = NULL;
+	dlock->count = 0;
+	spin_lock_init(&dlock->spin_lock);
+
+	return 0;
+}
diff --git a/drivers/staging/meilhaus/medlock.h b/drivers/staging/meilhaus/medlock.h
new file mode 100644
index 000000000000..4d6ddc8e58a1
--- /dev/null
+++ b/drivers/staging/meilhaus/medlock.h
@@ -0,0 +1,76 @@
+/**
+ * @file medlock.h
+ *
+ * @brief Provides the device lock class.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _MEDLOCK_H_
+#define _MEDLOCK_H_
+
+#include <linux/spinlock.h>
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The device lock class.
+ */
+typedef struct me_dlock {
+	struct file *filep;		/**< Pointer to file structure holding the device. */
+	int count;				/**< Number of tasks which are inside the device. */
+	spinlock_t spin_lock;	/**< Spin lock protecting the attributes from concurrent access. */
+} me_dlock_t;
+
+/**
+ * @brief Tries to enter a device.
+ *
+ * @param dlock The device lock instance.
+ * @param filep The file structure identifying the calling process.
+ *
+ * @return 0 on success.
+ */
+int me_dlock_enter(struct me_dlock *dlock, struct file *filep);
+
+/**
+ * @brief Exits a device.
+ *
+ * @param dlock The device lock instance.
+ * @param filep The file structure identifying the calling process.
+ *
+ * @return 0 on success.
+ */
+int me_dlock_exit(struct me_dlock *dlock, struct file *filep);
+
+/**
+ * @brief Tries to perform a locking action on a device.
+ *
+ * @param dlock The device lock instance.
+ * @param filep The file structure identifying the calling process.
+ * @param The action to be done.
+ * @param flags Flags from user space.
+ * @param slist The subdevice list of the device.
+ *
+ * @return 0 on success.
+ */
+int me_dlock_lock(struct me_dlock *dlock,
+		  struct file *filep, int lock, int flags, me_slist_t * slist);
+
+/**
+ * @brief Initializes a lock structure.
+ *
+ * @param dlock The lock structure to initialize.
+ * @return 0 on success.
+ */
+int me_dlock_init(me_dlock_t * dlock);
+
+/**
+ * @brief Deinitializes a lock structure.
+ *
+ * @param dlock The lock structure to deinitialize.
+ * @return 0 on success.
+ */
+void me_dlock_deinit(me_dlock_t * dlock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/medriver.h b/drivers/staging/meilhaus/medriver.h
new file mode 100644
index 000000000000..02e2408ce5f3
--- /dev/null
+++ b/drivers/staging/meilhaus/medriver.h
@@ -0,0 +1,350 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : medriver.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ *  Author:	Krzysztof Gantzke	<k.gantzke@meilhaus.de>
+ */
+
+#ifndef _MEDRIVER_H_
+#define _MEDRIVER_H_
+
+#include "metypes.h"
+#include "meerror.h"
+#include "medefines.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+	/*===========================================================================
+	  Functions to access the driver system
+	  =========================================================================*/
+
+	int meOpen(int iFlags);
+	int meClose(int iFlags);
+
+	int meLockDriver(int iLock, int iFlags);
+	int meLockDevice(int iDevice, int iLock, int iFlags);
+	int meLockSubdevice(int iDevice, int iSubdevice, int iLock, int iFlags);
+
+	/*===========================================================================
+	  Error handling functions
+	  =========================================================================*/
+
+	int meErrorGetLastMessage(char *pcErrorMsg, int iCount);
+	int meErrorGetMessage(int iErrorCode, char *pcErrorMsg, int iCount);
+	int meErrorSetDefaultProc(int iSwitch);
+	int meErrorSetUserProc(meErrorCB_t pErrorProc);
+
+
+	/*===========================================================================
+	  Functions to perform I/O on a device
+	  =========================================================================*/
+
+	int meIOIrqSetCallback(
+			int iDevice,
+			int iSubdevice,
+			meIOIrqCB_t pCallback,
+			void *pCallbackContext,
+			int iFlags);
+	int meIOIrqStart(
+			int iDevice,
+			int iSubdevice,
+			int iChannel,
+			int iIrqSource,
+			int iIrqEdge,
+			int iIrqArg,
+			int iFlags);
+	int meIOIrqStop(
+			int iDevice,
+			int iSubdevice,
+			int iChannel,
+			int iFlags);
+	int meIOIrqWait(
+			int iDevice,
+			int iSubdevice,
+			int iChannel,
+			int *piIrqCount,
+			int *piValue,
+			int iTimeOut,
+			int iFlags);
+
+	int meIOResetDevice(int iDevice, int iFlags);
+	int meIOResetSubdevice(int iDevice, int iSubdevice, int iFlags);
+
+	int meIOStreamFrequencyToTicks(
+			int iDevice,
+			int iSubdevice,
+			int iTimer,
+			double *pdFrequency,
+			int *piTicksLow,
+			int *piTicksHigh,
+			int iFlags);
+
+	int meIOSingleConfig(
+			int iDevice,
+			int iSubdevice,
+			int iChannel,
+			int iSingleConfig,
+			int iRef,
+			int iTrigChan,
+			int iTrigType,
+			int iTrigEdge,
+			int iFlags);
+	int meIOSingle(meIOSingle_t *pSingleList, int iCount, int iFlags);
+
+	int meIOStreamConfig(
+			int iDevice,
+			int iSubdevice,
+			meIOStreamConfig_t *pConfigList,
+			int iCount,
+			meIOStreamTrigger_t *pTrigger,
+			int iFifoIrqThreshold,
+			int iFlags);
+	int meIOStreamNewValues(
+			int iDevice,
+			int iSubdevice,
+			int iTimeOut,
+			int *piCount,
+			int iFlags);
+	int meIOStreamRead(
+			int iDevice,
+			int iSubdevice,
+			int iReadMode,
+			int *piValues,
+			int *piCount,
+			int iFlags);
+	int meIOStreamWrite(
+			int iDevice,
+			int iSubdevice,
+			int iWriteMode,
+			int *piValues,
+			int *piCount,
+			int iFlags);
+	int meIOStreamStart(meIOStreamStart_t *pStartList, int iCount, int iFlags);
+	int meIOStreamStop(meIOStreamStop_t *pStopList, int iCount, int iFlags);
+	int meIOStreamStatus(
+			int iDevice,
+			int iSubdevice,
+			int iWait,
+			int *piStatus,
+			int *piCount,
+			int iFlags);
+	int meIOStreamSetCallbacks(
+			int iDevice,
+			int iSubdevice,
+			meIOStreamCB_t pStartCB,
+			void *pStartCBContext,
+			meIOStreamCB_t pNewValuesCB,
+			void *pNewValuesCBContext,
+			meIOStreamCB_t pEndCB,
+			void *pEndCBContext,
+			int iFlags);
+	int meIOStreamTimeToTicks(
+			int iDevice,
+			int iSubdevice,
+			int iTimer,
+			double *pdTime,
+			int *piTicksLow,
+			int *piTicksHigh,
+			int iFlags);
+
+
+	/*===========================================================================
+	  Functions to query the driver system
+	  =========================================================================*/
+
+	int meQueryDescriptionDevice(int iDevice, char *pcDescription, int iCount);
+
+	int meQueryInfoDevice(
+			int iDevice,
+			int *piVendorId,
+			int *piDeviceId,
+			int *piSerialNo,
+			int *piBusType,
+			int *piBusNo,
+			int *piDevNo,
+			int *piFuncNo,
+			int *piPlugged);
+
+	int meQueryNameDevice(int iDevice, char *pcName, int iCount);
+	int meQueryNameDeviceDriver(int iDevice, char *pcName, int iCount);
+
+	int meQueryNumberDevices(int *piNumber);
+	int meQueryNumberSubdevices(int iDevice, int *piNumber);
+	int meQueryNumberChannels(int iDevice, int iSubdevice, int *piNumber);
+	int meQueryNumberRanges(
+			int iDevice,
+			int iSubdevice,
+			int iUnit,
+			int *piNumber);
+
+	int meQueryRangeByMinMax(
+			int iDevice,
+			int iSubdevice,
+			int iUnit,
+			double *pdMin,
+			double *pdMax,
+			int *piMaxData,
+			int *piRange);
+	int meQueryRangeInfo(
+			int iDevice,
+			int iSubdevice,
+			int iRange,
+			int *piUnit,
+			double *pdMin,
+			double *pdMax,
+			int *piMaxData);
+
+	int meQuerySubdeviceByType(
+			int iDevice,
+			int iStartSubdevice,
+			int iType,
+			int iSubtype,
+			int *piSubdevice);
+	int meQuerySubdeviceType(
+			int iDevice,
+			int iSubdevice,
+			int *piType,
+			int *piSubtype);
+	int meQuerySubdeviceCaps(
+			int iDevice,
+			int iSubdevice,
+			int *piCaps);
+	int meQuerySubdeviceCapsArgs(
+			int iDevice,
+			int iSubdevice,
+			int iCap,
+			int *piArgs,
+			int iCount);
+
+	int meQueryVersionLibrary(int *piVersion);
+	int meQueryVersionMainDriver(int *piVersion);
+	int meQueryVersionDeviceDriver(int iDevice, int *piVersion);
+
+
+	/*===========================================================================
+	  Common utility functions
+	  =========================================================================*/
+
+	int meUtilityExtractValues(
+			int iChannel,
+			int *piAIBuffer,
+			int iAIBufferCount,
+			meIOStreamConfig_t *pConfigList,
+			int iConfigListCount,
+			int *piChanBuffer,
+			int *piChanBufferCount);
+	int meUtilityDigitalToPhysical(
+			double dMin,
+			double dMax,
+			int iMaxData,
+			int iData,
+			int iModuleType,
+			double dRefValue,
+			double *pdPhysical);
+	int meUtilityDigitalToPhysicalV(
+			double dMin,
+			double dMax,
+			int iMaxData,
+			int *piDataBuffer,
+			int iCount,
+			int iModuleType,
+			double dRefValue,
+			double *pdPhysicalBuffer);
+	int meUtilityPhysicalToDigital(
+			double dMin,
+			double dMax,
+			int iMaxData,
+			double dPhysical,
+			int *piData);
+	int meUtilityPWMStart(
+			int iDevice,
+			int iSubdevice1,
+			int iSubdevice2,
+			int iSubdevice3,
+			int iRef,
+			int iPrescaler,
+			int iDutyCycle,
+			int iFlag);
+	int meUtilityPWMStop(int iDevice,
+			int iSubdevice1);
+	int meUtilityPWMRestart(
+			int iDevice,
+			int iSubdevice1,
+			int iRef,
+			int iPrescaler);
+
+
+	/*===========================================================================
+	  Load configuration from file into driver system
+	  =========================================================================*/
+
+	int meConfigLoad(char *pcConfigFile);
+
+
+	/*===========================================================================
+	  Functions to query a remote driver system
+	  =========================================================================*/
+
+	int meRQueryDescriptionDevice(
+			char *location,
+			int iDevice,
+			char *pcDescription,
+			int iCount);
+
+	int meRQueryInfoDevice(
+			char *location,
+			int iDevice,
+			int *piVendorId,
+			int *piDeviceId,
+			int *piSerialNo,
+			int *piBusType,
+			int *piBusNo,
+			int *piDevNo,
+			int *piFuncNo,
+			int *piPlugged);
+
+	int meRQueryNameDevice(
+			char *location,
+			int iDevice,
+			char *pcName,
+			int iCount);
+
+	int meRQueryNumberDevices(char *location, int *piNumber);
+	int meRQueryNumberSubdevices(char *location, int iDevice, int *piNumber);
+	int meRQueryNumberChannels(
+			char *location,
+			int iDevice,
+			int iSubdevice,
+			int *piNumber);
+	int meRQueryNumberRanges(
+			char *location,
+			int iDevice,
+			int iSubdevice,
+			int iUnit,
+			int *piNumber);
+
+	int meRQueryRangeInfo(
+			char *location,
+			int iDevice,
+			int iSubdevice,
+			int iRange,
+			int *piUnit,
+			double *pdMin,
+			double *pdMax,
+			int *piMaxData);
+
+	int meRQuerySubdeviceType(
+			char *location,
+			int iDevice,
+			int iSubdevice,
+			int *piType,
+			int *piSubtype);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/staging/meilhaus/medummy.c b/drivers/staging/meilhaus/medummy.c
new file mode 100644
index 000000000000..6a9f08d50bb1
--- /dev/null
+++ b/drivers/staging/meilhaus/medummy.c
@@ -0,0 +1,1266 @@
+/* Device driver for Meilhaus ME-DUMMY devices.
+ * ===========================================
+ *
+ *    Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ *    This file 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.
+ */
+
+/*
+ * User application could also include the kernel header files. But the
+ * real kernel functions are protected by #ifdef __KERNEL__.
+ */
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * This must be defined before module.h is included. Not needed, when
+ * it is a built in driver.
+ */
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+#include <linux/slab.h>
+
+#include "meerror.h"
+#include "meinternal.h"
+
+#include "meids.h"
+#include "mecommon.h"
+#include "medevice.h"
+#include "medebug.h"
+
+#include "medummy.h"
+
+static int medummy_io_irq_start(me_device_t * device,
+				struct file *filep,
+				int subdevice,
+				int channel,
+				int irq_source,
+				int irq_edge, int irq_arg, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_irq_wait(me_device_t * device,
+			       struct file *filep,
+			       int subdevice,
+			       int channel,
+			       int *irq_count,
+			       int *value, int timeout, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_irq_stop(me_device_t * device,
+			       struct file *filep,
+			       int subdevice, int channel, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_reset_device(me_device_t * device,
+				   struct file *filep, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_reset_subdevice(me_device_t * device,
+				      struct file *filep,
+				      int subdevice, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_single_config(me_device_t * device,
+				    struct file *filep,
+				    int subdevice,
+				    int channel,
+				    int single_config,
+				    int ref,
+				    int trig_chan,
+				    int trig_type, int trig_edge, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_single_read(me_device_t * device,
+				  struct file *filep,
+				  int subdevice,
+				  int channel,
+				  int *value, int time_out, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_single_write(me_device_t * device,
+				   struct file *filep,
+				   int subdevice,
+				   int channel,
+				   int value, int time_out, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_stream_config(me_device_t * device,
+				    struct file *filep,
+				    int subdevice,
+				    meIOStreamConfig_t * config_list,
+				    int count,
+				    meIOStreamTrigger_t * trigger,
+				    int fifo_irq_threshold, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_stream_new_values(me_device_t * device,
+					struct file *filep,
+					int subdevice,
+					int timeout, int *count, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_stream_read(me_device_t * device,
+				  struct file *filep,
+				  int subdevice,
+				  int read_mode,
+				  int *values, int *count, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_stream_start(me_device_t * device,
+				   struct file *filep,
+				   int subdevice,
+				   int start_mode, int time_out, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_stream_status(me_device_t * device,
+				    struct file *filep,
+				    int subdevice,
+				    int wait,
+				    int *status, int *values, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_stream_stop(me_device_t * device,
+				  struct file *filep,
+				  int subdevice, int stop_mode, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_stream_write(me_device_t * device,
+				   struct file *filep,
+				   int subdevice,
+				   int write_mode,
+				   int *values, int *count, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_lock_device(me_device_t * device,
+			       struct file *filep, int lock, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_lock_subdevice(me_device_t * device,
+				  struct file *filep,
+				  int subdevice, int lock, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_description_device(me_device_t * device,
+					    char **description)
+{
+	medummy_device_t *instance = (medummy_device_t *) device;
+
+	PDEBUG("executed.\n");
+
+//      if (instance->magic != MEDUMMY_MAGIC_NUMBER)
+//      {
+//              PERROR("Wrong magic number.\n");
+//              return ME_ERRNO_INTERNAL;
+//      }
+
+	switch (instance->device_id) {
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1000:
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1000_A:
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1000_B:
+		*description = ME1000_DESCRIPTION_DEVICE_ME1000;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1400:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140A:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400A;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140B:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400B;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14E0:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400E;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14EA:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400EA;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14EB:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400EB;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400C;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400D;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_4U:
+		*description = ME1600_DESCRIPTION_DEVICE_ME16004U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_8U:
+		*description = ME1600_DESCRIPTION_DEVICE_ME16008U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_12U:
+		*description = ME1600_DESCRIPTION_DEVICE_ME160012U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_16U:
+		*description = ME1600_DESCRIPTION_DEVICE_ME160016U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_16U_8I:
+		*description = ME1600_DESCRIPTION_DEVICE_ME160016U8I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4610:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4610;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4650:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4650;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4660;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660I:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4660I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660S:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4660S;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660IS:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4660IS;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4670;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670I:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4670I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670S:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4670S;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670IS:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4670IS;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4680;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680I:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4680I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680S:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4680S;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680IS:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4680IS;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6004:
+		*description = ME6000_DESCRIPTION_DEVICE_ME60004;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6008:
+		*description = ME6000_DESCRIPTION_DEVICE_ME60008;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME600F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME600016;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6014:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000I4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6018:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000I8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME601F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000I16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6034:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6038:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME603F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6104:
+		*description = ME6000_DESCRIPTION_DEVICE_ME61004;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6108:
+		*description = ME6000_DESCRIPTION_DEVICE_ME61008;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME610F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME610016;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6114:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100I4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6118:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100I8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME611F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100I16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6134:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6138:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME613F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6044:
+		*description = ME6000_DESCRIPTION_DEVICE_ME60004DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6048:
+		*description = ME6000_DESCRIPTION_DEVICE_ME60008DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME604F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME600016DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6054:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000I4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6058:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000I8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME605F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000I16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6074:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6078:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME607F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6144:
+		*description = ME6000_DESCRIPTION_DEVICE_ME61004DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6148:
+		*description = ME6000_DESCRIPTION_DEVICE_ME61008DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME614F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME610016DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6154:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100I4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6158:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100I8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME615F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100I16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6174:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6178:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME617F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6259:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6200I9DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6359:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6300I9DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0630:
+		*description = ME0600_DESCRIPTION_DEVICE_ME0630;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_A:
+		*description = ME8100_DESCRIPTION_DEVICE_ME8100A;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_B:
+		*description = ME8100_DESCRIPTION_DEVICE_ME8100B;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0940:
+		*description = ME0900_DESCRIPTION_DEVICE_ME0940;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0950:
+		*description = ME0900_DESCRIPTION_DEVICE_ME0950;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0960:
+		*description = ME0900_DESCRIPTION_DEVICE_ME0960;
+
+		break;
+/*
+		case USB_DEVICE_ID_MEPHISTO_S1:
+			*description = MEPHISTO_S1_DESCRIPTION_DEVICE;
+
+			break;
+*/
+	default:
+		*description = EMPTY_DESCRIPTION_DEVICE;
+		PERROR("Invalid device id in device info.\n");
+
+		return ME_ERRNO_INTERNAL;
+	}
+
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_info_device(me_device_t * device,
+				     int *vendor_id,
+				     int *device_id,
+				     int *serial_no,
+				     int *bus_type,
+				     int *bus_no,
+				     int *dev_no, int *func_no, int *plugged)
+{
+	medummy_device_t *instance = (medummy_device_t *) device;
+
+	PDEBUG("executed.\n");
+
+//      if (instance->magic != MEDUMMY_MAGIC_NUMBER)
+//      {
+//              PERROR("Wrong magic number.\n");
+//              return ME_ERRNO_INTERNAL;
+//      }
+
+	*vendor_id = instance->vendor_id;
+	*device_id = instance->device_id;
+	*serial_no = instance->serial_no;
+	*bus_type = instance->bus_type;
+	*bus_no = instance->bus_no;
+	*dev_no = instance->dev_no;
+	*func_no = instance->func_no;
+	*plugged = ME_PLUGGED_OUT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int medummy_query_name_device_driver(me_device_t * device, char **name)
+{
+	PDEBUG("executed.\n");
+	*name = MEDUMMY_NAME_DRIVER;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int medummy_query_name_device(me_device_t * device, char **name)
+{
+	medummy_device_t *instance = (medummy_device_t *) device;
+
+	PDEBUG("executed.\n");
+
+// // //        if (instance->magic != MEDUMMY_MAGIC_NUMBER)
+// // //        {
+// // //                PERROR("Wrong magic number.\n");
+// // //                return ME_ERRNO_INTERNAL;
+// // //        }
+
+	switch (instance->device_id) {
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1000:
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1000_A:
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1000_B:
+		*name = ME1000_NAME_DEVICE_ME1000;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1400:
+		*name = ME1400_NAME_DEVICE_ME1400;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140A:
+		*name = ME1400_NAME_DEVICE_ME1400A;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140B:
+		*name = ME1400_NAME_DEVICE_ME1400B;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14E0:
+		*name = ME1400_NAME_DEVICE_ME1400E;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14EA:
+		*name = ME1400_NAME_DEVICE_ME1400EA;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14EB:
+		*name = ME1400_NAME_DEVICE_ME1400EB;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+		*name = ME1400_NAME_DEVICE_ME1400C;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		*name = ME1400_NAME_DEVICE_ME1400D;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_4U:
+		*name = ME1600_NAME_DEVICE_ME16004U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_8U:
+		*name = ME1600_NAME_DEVICE_ME16008U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_12U:
+		*name = ME1600_NAME_DEVICE_ME160012U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_16U:
+		*name = ME1600_NAME_DEVICE_ME160016U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_16U_8I:
+		*name = ME1600_NAME_DEVICE_ME160016U8I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4610:
+		*name = ME4600_NAME_DEVICE_ME4610;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4650:
+		*name = ME4600_NAME_DEVICE_ME4650;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660:
+		*name = ME4600_NAME_DEVICE_ME4660;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660I:
+		*name = ME4600_NAME_DEVICE_ME4660I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670:
+		*name = ME4600_NAME_DEVICE_ME4670;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670I:
+		*name = ME4600_NAME_DEVICE_ME4670I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670S:
+		*name = ME4600_NAME_DEVICE_ME4670S;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670IS:
+		*name = ME4600_NAME_DEVICE_ME4670IS;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680:
+		*name = ME4600_NAME_DEVICE_ME4680;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680I:
+		*name = ME4600_NAME_DEVICE_ME4680I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680S:
+		*name = ME4600_NAME_DEVICE_ME4680S;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680IS:
+		*name = ME4600_NAME_DEVICE_ME4680IS;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6004:
+		*name = ME6000_NAME_DEVICE_ME60004;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6008:
+		*name = ME6000_NAME_DEVICE_ME60008;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME600F:
+		*name = ME6000_NAME_DEVICE_ME600016;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6014:
+		*name = ME6000_NAME_DEVICE_ME6000I4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6018:
+		*name = ME6000_NAME_DEVICE_ME6000I8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME601F:
+		*name = ME6000_NAME_DEVICE_ME6000I16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6034:
+		*name = ME6000_NAME_DEVICE_ME6000ISLE4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6038:
+		*name = ME6000_NAME_DEVICE_ME6000ISLE8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME603F:
+		*name = ME6000_NAME_DEVICE_ME6000ISLE16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6104:
+		*name = ME6000_NAME_DEVICE_ME61004;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6108:
+		*name = ME6000_NAME_DEVICE_ME61008;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME610F:
+		*name = ME6000_NAME_DEVICE_ME610016;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6114:
+		*name = ME6000_NAME_DEVICE_ME6100I4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6118:
+		*name = ME6000_NAME_DEVICE_ME6100I8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME611F:
+		*name = ME6000_NAME_DEVICE_ME6100I16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6134:
+		*name = ME6000_NAME_DEVICE_ME6100ISLE4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6138:
+		*name = ME6000_NAME_DEVICE_ME6100ISLE8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME613F:
+		*name = ME6000_NAME_DEVICE_ME6100ISLE16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6044:
+		*name = ME6000_NAME_DEVICE_ME60004DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6048:
+		*name = ME6000_NAME_DEVICE_ME60008DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME604F:
+		*name = ME6000_NAME_DEVICE_ME600016DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6054:
+		*name = ME6000_NAME_DEVICE_ME6000I4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6058:
+		*name = ME6000_NAME_DEVICE_ME6000I8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME605F:
+		*name = ME6000_NAME_DEVICE_ME6000I16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6074:
+		*name = ME6000_NAME_DEVICE_ME6000ISLE4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6078:
+		*name = ME6000_NAME_DEVICE_ME6000ISLE8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME607F:
+		*name = ME6000_NAME_DEVICE_ME6000ISLE16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6144:
+		*name = ME6000_NAME_DEVICE_ME61004DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6148:
+		*name = ME6000_NAME_DEVICE_ME61008DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME614F:
+		*name = ME6000_NAME_DEVICE_ME610016DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6154:
+		*name = ME6000_NAME_DEVICE_ME6100I4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6158:
+		*name = ME6000_NAME_DEVICE_ME6100I8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME615F:
+		*name = ME6000_NAME_DEVICE_ME6100I16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6174:
+		*name = ME6000_NAME_DEVICE_ME6100ISLE4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6178:
+		*name = ME6000_NAME_DEVICE_ME6100ISLE8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME617F:
+		*name = ME6000_NAME_DEVICE_ME6100ISLE16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0630:
+		*name = ME0600_NAME_DEVICE_ME0630;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_A:
+		*name = ME8100_NAME_DEVICE_ME8100A;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_B:
+		*name = ME8100_NAME_DEVICE_ME8100B;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0940:
+		*name = ME0900_NAME_DEVICE_ME0940;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0950:
+		*name = ME0900_NAME_DEVICE_ME0950;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0960:
+		*name = ME0900_NAME_DEVICE_ME0960;
+
+		break;
+/*
+		case USB_DEVICE_ID_MEPHISTO_S1:
+			*name = MEPHISTO_S1_NAME_DEVICE;
+
+			break;
+*/
+	default:
+		*name = EMPTY_NAME_DEVICE;
+		PERROR("Invalid PCI device id.\n");
+
+		return ME_ERRNO_INTERNAL;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int medummy_query_number_subdevices(me_device_t * device, int *number)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_number_channels(me_device_t * device,
+					 int subdevice, int *number)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_number_ranges(me_device_t * device,
+				       int subdevice, int unit, int *count)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_subdevice_type(me_device_t * device,
+					int subdevice, int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_subdevice_caps(me_device_t * device,
+					int subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_subdevice_caps_args(me_device_t * device,
+					     int subdevice,
+					     int cap, int *args, int count)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int medummy_query_subdevice_by_type(me_device_t * device,
+					   int start_subdevice,
+					   int type,
+					   int subtype, int *subdevice)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_range_by_min_max(me_device_t * device,
+					  int subdevice,
+					  int unit,
+					  int *min,
+					  int *max, int *maxdata, int *range)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_range_info(me_device_t * device,
+				    int subdevice,
+				    int range,
+				    int *unit, int *min, int *max, int *maxdata)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+int medummy_query_timer(me_device_t * device,
+			int subdevice,
+			int timer,
+			int *base_frequency,
+			uint64_t * min_ticks, uint64_t * max_ticks)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_version_device_driver(me_device_t * device,
+					       int *version)
+{
+	PDEBUG("executed.\n");
+
+	*version = ME_VERSION_DRIVER;
+	return ME_ERRNO_SUCCESS;
+}
+
+static void medummy_destructor(me_device_t * device)
+{
+	PDEBUG("executed.\n");
+	kfree(device);
+}
+
+static int init_device_info(unsigned short vendor_id,
+			    unsigned short device_id,
+			    unsigned int serial_no,
+			    int bus_type,
+			    int bus_no,
+			    int dev_no,
+			    int func_no, medummy_device_t * instance)
+{
+	PDEBUG("executed.\n");
+
+//      instance->magic = MEDUMMY_MAGIC_NUMBER;
+	instance->vendor_id = vendor_id;
+	instance->device_id = device_id;
+	instance->serial_no = serial_no;
+	instance->bus_type = bus_type;
+	instance->bus_no = bus_no;
+	instance->dev_no = dev_no;
+	instance->func_no = func_no;
+
+	return 0;
+}
+
+static int medummy_config_load(me_device_t * device, struct file *filep,
+			       me_cfg_device_entry_t * config)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_SUCCESS;
+}
+
+static int init_device_instance(me_device_t * device)
+{
+	PDEBUG("executed.\n");
+
+	INIT_LIST_HEAD(&device->list);
+
+	device->me_device_io_irq_start = medummy_io_irq_start;
+	device->me_device_io_irq_wait = medummy_io_irq_wait;
+	device->me_device_io_irq_stop = medummy_io_irq_stop;
+	device->me_device_io_reset_device = medummy_io_reset_device;
+	device->me_device_io_reset_subdevice = medummy_io_reset_subdevice;
+	device->me_device_io_single_config = medummy_io_single_config;
+	device->me_device_io_single_read = medummy_io_single_read;
+	device->me_device_io_single_write = medummy_io_single_write;
+	device->me_device_io_stream_config = medummy_io_stream_config;
+	device->me_device_io_stream_new_values = medummy_io_stream_new_values;
+	device->me_device_io_stream_read = medummy_io_stream_read;
+	device->me_device_io_stream_start = medummy_io_stream_start;
+	device->me_device_io_stream_status = medummy_io_stream_status;
+	device->me_device_io_stream_stop = medummy_io_stream_stop;
+	device->me_device_io_stream_write = medummy_io_stream_write;
+
+	device->me_device_lock_device = medummy_lock_device;
+	device->me_device_lock_subdevice = medummy_lock_subdevice;
+
+	device->me_device_query_description_device =
+	    medummy_query_description_device;
+	device->me_device_query_info_device = medummy_query_info_device;
+	device->me_device_query_name_device_driver =
+	    medummy_query_name_device_driver;
+	device->me_device_query_name_device = medummy_query_name_device;
+
+	device->me_device_query_number_subdevices =
+	    medummy_query_number_subdevices;
+	device->me_device_query_number_channels = medummy_query_number_channels;
+	device->me_device_query_number_ranges = medummy_query_number_ranges;
+
+	device->me_device_query_range_by_min_max =
+	    medummy_query_range_by_min_max;
+	device->me_device_query_range_info = medummy_query_range_info;
+
+	device->me_device_query_subdevice_type = medummy_query_subdevice_type;
+	device->me_device_query_subdevice_by_type =
+	    medummy_query_subdevice_by_type;
+	device->me_device_query_subdevice_caps = medummy_query_subdevice_caps;
+	device->me_device_query_subdevice_caps_args =
+	    medummy_query_subdevice_caps_args;
+
+	device->me_device_query_timer = medummy_query_timer;
+
+	device->me_device_query_version_device_driver =
+	    medummy_query_version_device_driver;
+
+	device->me_device_destructor = medummy_destructor;
+	device->me_device_config_load = medummy_config_load;
+	return 0;
+}
+
+me_device_t *medummy_constructor(unsigned short vendor_id,
+				 unsigned short device_id,
+				 unsigned int serial_no,
+				 int bus_type,
+				 int bus_no, int dev_no, int func_no)
+{
+	int result = 0;
+	medummy_device_t *instance;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate structure for device attributes */
+	instance = kmalloc(sizeof(medummy_device_t), GFP_KERNEL);
+
+	if (!instance) {
+		PERROR("Can't get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(instance, 0, sizeof(medummy_device_t));
+
+	/* Initialize device info */
+	result = init_device_info(vendor_id,
+				  device_id,
+				  serial_no,
+				  bus_type, bus_no, dev_no, func_no, instance);
+
+	if (result) {
+		PERROR("Cannot init baord info.\n");
+		kfree(instance);
+		return NULL;
+	}
+
+	/* Initialize device instance */
+	result = init_device_instance((me_device_t *) instance);
+
+	if (result) {
+		PERROR("Cannot init baord info.\n");
+		kfree(instance);
+		return NULL;
+	}
+
+	return (me_device_t *) instance;
+}
+
+// Init and exit of module.
+
+static int __init dummy_init(void)
+{
+	PDEBUG("executed.\n");
+	return 0;
+}
+
+static void __exit dummy_exit(void)
+{
+	PDEBUG("executed.\n");
+}
+
+module_init(dummy_init);
+
+module_exit(dummy_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR("Guenter Gebhardt <g.gebhardt@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for Meilhaus ME-DUMMY Devices");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-DUMMY Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(medummy_constructor);
diff --git a/drivers/staging/meilhaus/medummy.h b/drivers/staging/meilhaus/medummy.h
new file mode 100644
index 000000000000..717000ff6c1c
--- /dev/null
+++ b/drivers/staging/meilhaus/medummy.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : medummy.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ */
+
+#ifndef _MEDUMMY_H_
+#define _MEDUMMY_H_
+
+#include "metypes.h"
+#include "medefines.h"
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+#define MEDUMMY_MAGIC_NUMBER	0xDDDD
+
+typedef struct medummy_device {
+	me_device_t base;			/**< The Meilhaus device base class. */
+//      int magic;                                      /**< The magic number of the structure */
+	unsigned short vendor_id;	/**< Vendor ID */
+	unsigned short device_id;	/**< Device ID */
+	unsigned int serial_no;		/**< Serial number of the device */
+	int bus_type;				/**< Bus type */
+	int bus_no;					/**< Bus number */
+	int dev_no;					/**< Device number */
+	int func_no;				/**< Function number */
+} medummy_device_t;
+
+me_device_t *medummy_constructor(unsigned short vendor_id,
+				 unsigned short device_id,
+				 unsigned int serial_no,
+				 int bus_type,
+				 int bus_no,
+				 int dev_no,
+				 int func_no) __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/meerror.h b/drivers/staging/meilhaus/meerror.h
new file mode 100644
index 000000000000..9eda4bf907ba
--- /dev/null
+++ b/drivers/staging/meilhaus/meerror.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : meerror.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ * Author      : KG (Krzysztof Gantzke)	<k.gantzke@meilhaus.de>
+ */
+
+#ifndef _MEERROR_H_
+#define _MEERROR_H_
+
+extern char *meErrorMsgTable[];
+
+#define ME_ERRNO_SUCCESS						0
+#define ME_ERRNO_INVALID_DEVICE					1
+#define ME_ERRNO_INVALID_SUBDEVICE				2
+#define ME_ERRNO_INVALID_CHANNEL				3
+#define ME_ERRNO_INVALID_SINGLE_CONFIG			4
+#define ME_ERRNO_INVALID_REF					5
+#define ME_ERRNO_INVALID_TRIG_CHAN				6
+#define ME_ERRNO_INVALID_TRIG_TYPE				7
+#define ME_ERRNO_INVALID_TRIG_EDGE				8
+#define ME_ERRNO_INVALID_TIMEOUT				9
+#define ME_ERRNO_INVALID_FLAGS					10
+#define ME_ERRNO_OPEN							11
+#define ME_ERRNO_CLOSE							12
+#define ME_ERRNO_NOT_OPEN						13
+#define ME_ERRNO_INVALID_DIR					14
+#define ME_ERRNO_PREVIOUS_CONFIG				15
+#define ME_ERRNO_NOT_SUPPORTED					16
+#define ME_ERRNO_SUBDEVICE_TYPE					17
+#define ME_ERRNO_USER_BUFFER_SIZE				18
+#define ME_ERRNO_LOCKED							19
+#define ME_ERRNO_NOMORE_SUBDEVICE_TYPE			20
+#define ME_ERRNO_TIMEOUT						21
+#define ME_ERRNO_SIGNAL							22
+#define ME_ERRNO_INVALID_IRQ_SOURCE				23
+#define ME_ERRNO_THREAD_RUNNING					24
+#define ME_ERRNO_START_THREAD					25
+#define ME_ERRNO_CANCEL_THREAD					26
+#define ME_ERRNO_NO_CALLBACK					27
+#define ME_ERRNO_USED							28
+#define ME_ERRNO_INVALID_UNIT					29
+#define ME_ERRNO_INVALID_MIN_MAX				30
+#define ME_ERRNO_NO_RANGE						31
+#define ME_ERRNO_INVALID_RANGE					32
+#define ME_ERRNO_SUBDEVICE_BUSY					33
+#define ME_ERRNO_INVALID_LOCK					34
+#define ME_ERRNO_INVALID_SWITCH					35
+#define ME_ERRNO_INVALID_ERROR_MSG_COUNT		36
+#define ME_ERRNO_INVALID_STREAM_CONFIG			37
+#define ME_ERRNO_INVALID_CONFIG_LIST_COUNT		38
+#define ME_ERRNO_INVALID_ACQ_START_TRIG_TYPE	39
+#define ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE	40
+#define ME_ERRNO_INVALID_ACQ_START_TRIG_CHAN	41
+#define ME_ERRNO_INVALID_ACQ_START_TIMEOUT		42
+#define ME_ERRNO_INVALID_ACQ_START_ARG			43
+#define ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE	44
+#define ME_ERRNO_INVALID_SCAN_START_ARG			45
+#define ME_ERRNO_INVALID_CONV_START_TRIG_TYPE	46
+#define ME_ERRNO_INVALID_CONV_START_ARG			47
+#define ME_ERRNO_INVALID_SCAN_STOP_TRIG_TYPE	48
+#define ME_ERRNO_INVALID_SCAN_STOP_ARG			49
+#define ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE		50
+#define ME_ERRNO_INVALID_ACQ_STOP_ARG			51
+#define ME_ERRNO_SUBDEVICE_NOT_RUNNING			52
+#define ME_ERRNO_INVALID_READ_MODE				53
+#define ME_ERRNO_INVALID_VALUE_COUNT			54
+#define ME_ERRNO_INVALID_WRITE_MODE				55
+#define ME_ERRNO_INVALID_TIMER					56
+#define ME_ERRNO_DEVICE_UNPLUGGED				57
+#define ME_ERRNO_USED_INTERNAL					58
+#define ME_ERRNO_INVALID_DUTY_CYCLE				59
+#define ME_ERRNO_INVALID_WAIT					60
+#define ME_ERRNO_CONNECT_REMOTE					61
+#define ME_ERRNO_COMMUNICATION					62
+#define ME_ERRNO_INVALID_SINGLE_LIST			63
+#define ME_ERRNO_INVALID_MODULE_TYPE			64
+#define ME_ERRNO_INVALID_START_MODE				65
+#define ME_ERRNO_INVALID_STOP_MODE				66
+#define ME_ERRNO_INVALID_FIFO_IRQ_THRESHOLD		67
+#define ME_ERRNO_INVALID_POINTER				68
+#define ME_ERRNO_CREATE_EVENT					69
+#define ME_ERRNO_LACK_OF_RESOURCES				70
+#define ME_ERRNO_CANCELLED						71
+#define ME_ERRNO_RING_BUFFER_OVERFLOW			72
+#define ME_ERRNO_RING_BUFFER_UNDEFFLOW			73
+#define ME_ERRNO_INVALID_IRQ_EDGE				74
+#define ME_ERRNO_INVALID_IRQ_ARG				75
+#define ME_ERRNO_INVALID_CAP					76
+#define ME_ERRNO_INVALID_CAP_ARG_COUNT			77
+#define ME_ERRNO_INTERNAL						78
+
+/** New error for range check */
+#define ME_ERRNO_VALUE_OUT_OF_RANGE				79
+#define ME_ERRNO_FIFO_BUFFER_OVERFLOW			80
+#define ME_ERRNO_FIFO_BUFFER_UNDEFFLOW			81
+
+#define ME_ERRNO_INVALID_ERROR_NUMBER			82
+#endif
diff --git a/drivers/staging/meilhaus/mefirmware.c b/drivers/staging/meilhaus/mefirmware.c
new file mode 100644
index 000000000000..c07d202e8cb5
--- /dev/null
+++ b/drivers/staging/meilhaus/mefirmware.c
@@ -0,0 +1,137 @@
+/**
+ * @file mefirmware.c
+ *
+ * @brief Implements the firmware handling.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/***************************************************************************
+ *   Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)     *
+ *   Copyright (C) 2007 by Krzysztof Gantzke k.gantzke@meilhaus.de         *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifndef __KERNEL__
+# define __KERNEL__
+#endif
+
+#ifndef KBUILD_MODNAME
+#  define KBUILD_MODNAME KBUILD_STR(mefirmware)
+#endif
+
+#include <linux/pci.h>
+#include <linux/delay.h>
+
+#include <linux/firmware.h>
+
+#include "meplx_reg.h"
+#include "medebug.h"
+
+#include "mefirmware.h"
+
+int me_xilinx_download(unsigned long register_base_control,
+		       unsigned long register_base_data,
+		       struct device *dev, const char *firmware_name)
+{
+	int err = ME_ERRNO_FIRMWARE;
+	uint32_t value = 0;
+	int idx = 0;
+
+	const struct firmware *fw;
+
+	PDEBUG("executed.\n");
+
+	if (!firmware_name) {
+		PERROR("Request for firmware failed. No name provided. \n");
+		return err;
+	}
+
+	PINFO("Request '%s' firmware.\n", firmware_name);
+	err = request_firmware(&fw, firmware_name, dev);
+
+	if (err) {
+		PERROR("Request for firmware failed.\n");
+		return err;
+	}
+	// Set PLX local interrupt 2 polarity to high.
+	// Interrupt is thrown by init pin of xilinx.
+	outl(PLX_INTCSR_LOCAL_INT2_POL, register_base_control + PLX_INTCSR);
+
+	// Set /CS and /WRITE of the Xilinx
+	value = inl(register_base_control + PLX_ICR);
+	value |= ME_FIRMWARE_CS_WRITE;
+	outl(value, register_base_control + PLX_ICR);
+
+	// Init Xilinx with CS1
+	inl(register_base_data + ME_XILINX_CS1_REG);
+
+	// Wait for init to complete
+	udelay(20);
+
+	// Checkl /INIT pin
+	if (!
+	    (inl(register_base_control + PLX_INTCSR) &
+	     PLX_INTCSR_LOCAL_INT2_STATE)) {
+		PERROR("Can't init Xilinx.\n");
+		release_firmware(fw);
+		return -EIO;
+	}
+	// Reset /CS and /WRITE of the Xilinx
+	value = inl(register_base_control + PLX_ICR);
+	value &= ~ME_FIRMWARE_CS_WRITE;
+	outl(value, register_base_control + PLX_ICR);
+
+	// Download Xilinx firmware
+	udelay(10);
+
+	for (idx = 0; idx < fw->size; idx++) {
+		outl(fw->data[idx], register_base_data);
+#ifdef ME6000_v2_4
+///     This checking only for board's version 2.4
+		// Check if BUSY flag is set (low = ready, high = busy)
+		if (inl(register_base_control + PLX_ICR) &
+		    ME_FIRMWARE_BUSY_FLAG) {
+			PERROR("Xilinx is still busy (idx = %d)\n", idx);
+			release_firmware(fw);
+			return -EIO;
+		}
+#endif //ME6000_v2_4
+	}
+	PDEBUG("Download finished. %d bytes written to PLX.\n", idx);
+
+	// If done flag is high download was successful
+	if (inl(register_base_control + PLX_ICR) & ME_FIRMWARE_DONE_FLAG) {
+		PDEBUG("SUCCESS. Done flag is set.\n");
+	} else {
+		PERROR("FAILURE. DONE flag is not set.\n");
+		release_firmware(fw);
+		return -EIO;
+	}
+
+	// Set /CS and /WRITE
+	value = inl(register_base_control + PLX_ICR);
+	value |= ME_FIRMWARE_CS_WRITE;
+	outl(value, register_base_control + PLX_ICR);
+
+	PDEBUG("Enable interrupts on the PCI interface.\n");
+	outl(ME_PLX_PCI_ACTIVATE, register_base_control + PLX_INTCSR);
+	release_firmware(fw);
+
+	return 0;
+}
diff --git a/drivers/staging/meilhaus/mefirmware.h b/drivers/staging/meilhaus/mefirmware.h
new file mode 100644
index 000000000000..a2685080c97b
--- /dev/null
+++ b/drivers/staging/meilhaus/mefirmware.h
@@ -0,0 +1,57 @@
+/**
+ * @file mefirmware.h
+ *
+ * @brief Definitions of the firmware handling functions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/***************************************************************************
+ *   Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)     *
+ *   Copyright (C) 2007 by Krzysztof Gantzke k.gantzke@meilhaus.de         *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifndef _MEFIRMWARE_H
+# define _MEFIRMWARE_H
+
+# ifdef __KERNEL__
+
+#define ME_ERRNO_FIRMWARE		-1
+
+/**
+* Registry
+*/
+#define ME_XILINX_CS1_REG		0x00C8
+
+/**
+* Flags (bits)
+*/
+
+#define ME_FIRMWARE_BUSY_FLAG	0x00000020
+#define ME_FIRMWARE_DONE_FLAG	0x00000004
+#define ME_FIRMWARE_CS_WRITE	0x00000100
+
+#define ME_PLX_PCI_ACTIVATE		0x43
+
+int me_xilinx_download(unsigned long register_base_control,
+		       unsigned long register_base_data,
+		       struct device *dev, const char *firmware_name);
+
+# endif	//__KERNEL__
+
+#endif //_MEFIRMWARE_H
diff --git a/drivers/staging/meilhaus/meids.h b/drivers/staging/meilhaus/meids.h
new file mode 100644
index 000000000000..b3e757cbdda6
--- /dev/null
+++ b/drivers/staging/meilhaus/meids.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : meids.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ */
+
+#ifndef _MEIDS_H_
+#define _MEIDS_H_
+
+#ifdef __KERNEL__
+
+/*=============================================================================
+  Driver names
+  ===========================================================================*/
+
+#define MEMAIN_NAME				"memain"
+#define ME1000_NAME				"me1000"
+#define ME1400_NAME				"me1400"
+#define ME1600_NAME				"me1600"
+#define ME4600_NAME				"me4600"
+#define ME6000_NAME				"me6000"
+#define ME0600_NAME				"me0600"	//"me630"
+#define ME8100_NAME				"me8100"
+#define ME8200_NAME				"me8200"
+#define ME0900_NAME				"me0900"	//"me9x"
+//#define MEPHISTO_S1_NAME                      "mephisto_s1"
+#define MEDUMMY_NAME			"medummy"
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/meinternal.h b/drivers/staging/meilhaus/meinternal.h
new file mode 100644
index 000000000000..8d126b4905a7
--- /dev/null
+++ b/drivers/staging/meilhaus/meinternal.h
@@ -0,0 +1,363 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : meinternal.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ */
+
+#ifndef _MEINTERNAL_H_
+#define _MEINTERNAL_H_
+
+/*=============================================================================
+  PCI Vendor IDs
+  ===========================================================================*/
+
+#define PCI_VENDOR_ID_MEILHAUS						0x1402
+
+/*=============================================================================
+  PCI Device IDs
+  ===========================================================================*/
+
+#define PCI_DEVICE_ID_MEILHAUS_ME1000				0x1000
+#define PCI_DEVICE_ID_MEILHAUS_ME1000_A				0x100A
+#define PCI_DEVICE_ID_MEILHAUS_ME1000_B				0x100B
+
+#define PCI_DEVICE_ID_MEILHAUS_ME1400				0x1400
+#define PCI_DEVICE_ID_MEILHAUS_ME140A				0x140A
+#define PCI_DEVICE_ID_MEILHAUS_ME140B				0x140B
+#define PCI_DEVICE_ID_MEILHAUS_ME14E0				0x14E0
+#define PCI_DEVICE_ID_MEILHAUS_ME14EA				0x14EA
+#define PCI_DEVICE_ID_MEILHAUS_ME14EB				0x14EB
+#define PCI_DEVICE_ID_MEILHAUS_ME140C				0X140C
+#define PCI_DEVICE_ID_MEILHAUS_ME140D				0X140D
+
+#define PCI_DEVICE_ID_MEILHAUS_ME1600_4U			0x1604 // 4 voltage outputs
+#define PCI_DEVICE_ID_MEILHAUS_ME1600_8U			0x1608 // 8 voltage outputs
+#define PCI_DEVICE_ID_MEILHAUS_ME1600_12U			0x160C // 12 voltage outputs
+#define PCI_DEVICE_ID_MEILHAUS_ME1600_16U			0x160F // 16 voltage outputs
+#define PCI_DEVICE_ID_MEILHAUS_ME1600_16U_8I		0x168F // 16 voltage/8 current o.
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4610				0x4610 // Jekyll
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4650				0x4650 // Low Cost version
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4660				0x4660 // Standard version
+#define PCI_DEVICE_ID_MEILHAUS_ME4660I				0x4661 // Isolated version
+#define PCI_DEVICE_ID_MEILHAUS_ME4660S				0x4662 // Standard version with Sample and Hold
+#define PCI_DEVICE_ID_MEILHAUS_ME4660IS				0x4663 // Isolated version with Sample and Hold
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4670				0x4670 // Standard version
+#define PCI_DEVICE_ID_MEILHAUS_ME4670I				0x4671 // Isolated version
+#define PCI_DEVICE_ID_MEILHAUS_ME4670S				0x4672 // Standard version with Sample and Hold
+#define PCI_DEVICE_ID_MEILHAUS_ME4670IS				0x4673 // Isolated version with Sample and Hold
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4680				0x4680 // Standard version
+#define PCI_DEVICE_ID_MEILHAUS_ME4680I				0x4681 // Isolated version
+#define PCI_DEVICE_ID_MEILHAUS_ME4680S				0x4682 // Standard version with Sample and Hold
+#define PCI_DEVICE_ID_MEILHAUS_ME4680IS				0x4683 // Isolated version with Sample and Hold
+
+/* ME6000 standard version */
+#define PCI_DEVICE_ID_MEILHAUS_ME6004   			0x6004
+#define PCI_DEVICE_ID_MEILHAUS_ME6008   			0x6008
+#define PCI_DEVICE_ID_MEILHAUS_ME600F   			0x600F
+
+/* ME6000 isolated version */
+#define PCI_DEVICE_ID_MEILHAUS_ME6014   			0x6014
+#define PCI_DEVICE_ID_MEILHAUS_ME6018   			0x6018
+#define PCI_DEVICE_ID_MEILHAUS_ME601F   			0x601F
+
+/* ME6000 isle version */
+#define PCI_DEVICE_ID_MEILHAUS_ME6034   			0x6034
+#define PCI_DEVICE_ID_MEILHAUS_ME6038   			0x6038
+#define PCI_DEVICE_ID_MEILHAUS_ME603F   			0x603F
+
+/* ME6000 standard version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6044   			0x6044
+#define PCI_DEVICE_ID_MEILHAUS_ME6048   			0x6048
+#define PCI_DEVICE_ID_MEILHAUS_ME604F   			0x604F
+
+/* ME6000 isolated version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6054   			0x6054
+#define PCI_DEVICE_ID_MEILHAUS_ME6058   			0x6058
+#define PCI_DEVICE_ID_MEILHAUS_ME605F   			0x605F
+
+/* ME6000 isle version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6074   			0x6074
+#define PCI_DEVICE_ID_MEILHAUS_ME6078   			0x6078
+#define PCI_DEVICE_ID_MEILHAUS_ME607F   			0x607F
+
+/* ME6100 standard version */
+#define PCI_DEVICE_ID_MEILHAUS_ME6104   			0x6104
+#define PCI_DEVICE_ID_MEILHAUS_ME6108   			0x6108
+#define PCI_DEVICE_ID_MEILHAUS_ME610F   			0x610F
+
+/* ME6100 isolated version */
+#define PCI_DEVICE_ID_MEILHAUS_ME6114   			0x6114
+#define PCI_DEVICE_ID_MEILHAUS_ME6118   			0x6118
+#define PCI_DEVICE_ID_MEILHAUS_ME611F   			0x611F
+
+/* ME6100 isle version */
+#define PCI_DEVICE_ID_MEILHAUS_ME6134   			0x6134
+#define PCI_DEVICE_ID_MEILHAUS_ME6138   			0x6138
+#define PCI_DEVICE_ID_MEILHAUS_ME613F   			0x613F
+
+/* ME6100 standard version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6144				0x6144
+#define PCI_DEVICE_ID_MEILHAUS_ME6148   			0x6148
+#define PCI_DEVICE_ID_MEILHAUS_ME614F   			0x614F
+
+/* ME6100 isolated version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6154   			0x6154
+#define PCI_DEVICE_ID_MEILHAUS_ME6158   			0x6158
+#define PCI_DEVICE_ID_MEILHAUS_ME615F   			0x615F
+
+/* ME6100 isle version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6174   			0x6174
+#define PCI_DEVICE_ID_MEILHAUS_ME6178   			0x6178
+#define PCI_DEVICE_ID_MEILHAUS_ME617F   			0x617F
+
+/* ME6200 isolated version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6259				0x6259
+
+/* ME6300 isolated version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6359				0x6359
+
+/* ME0630 */
+#define PCI_DEVICE_ID_MEILHAUS_ME0630				0x0630
+
+/* ME8100 */
+#define PCI_DEVICE_ID_MEILHAUS_ME8100_A				0x810A
+#define PCI_DEVICE_ID_MEILHAUS_ME8100_B  			0x810B
+
+/* ME8200 */
+#define PCI_DEVICE_ID_MEILHAUS_ME8200_A				0x820A
+#define PCI_DEVICE_ID_MEILHAUS_ME8200_B  			0x820B
+
+/* ME0900 */
+#define PCI_DEVICE_ID_MEILHAUS_ME0940				0x0940
+#define PCI_DEVICE_ID_MEILHAUS_ME0950				0x0950
+#define PCI_DEVICE_ID_MEILHAUS_ME0960				0x0960
+
+
+/*=============================================================================
+  USB Vendor IDs
+  ===========================================================================*/
+
+//#define USB_VENDOR_ID_MEPHISTO_S1					0x0403
+
+
+/*=============================================================================
+  USB Device IDs
+  ===========================================================================*/
+
+//#define USB_DEVICE_ID_MEPHISTO_S1					0xDCD0
+
+
+/* ME-1000 defines */
+#define ME1000_NAME_DRIVER							"ME-1000"
+
+#define ME1000_NAME_DEVICE_ME1000					"ME-1000"
+
+#define ME1000_DESCRIPTION_DEVICE_ME1000			"ME-1000 device, 128 digital i/o lines."
+
+/* ME-1400 defines */
+#define ME1400_NAME_DRIVER							"ME-1400"
+
+#define ME1400_NAME_DEVICE_ME1400					"ME-1400"
+#define ME1400_NAME_DEVICE_ME1400E					"ME-1400E"
+#define ME1400_NAME_DEVICE_ME1400A					"ME-1400A"
+#define ME1400_NAME_DEVICE_ME1400EA					"ME-1400EA"
+#define ME1400_NAME_DEVICE_ME1400B					"ME-1400B"
+#define ME1400_NAME_DEVICE_ME1400EB					"ME-1400EB"
+#define ME1400_NAME_DEVICE_ME1400C					"ME-1400C"
+#define ME1400_NAME_DEVICE_ME1400D					"ME-1400D"
+
+#define ME1400_DESCRIPTION_DEVICE_ME1400			"ME-1400 device, 24 digital i/o lines."
+#define ME1400_DESCRIPTION_DEVICE_ME1400E			"ME-1400E device, 24 digital i/o lines."
+#define ME1400_DESCRIPTION_DEVICE_ME1400A			"ME-1400A device, 24 digital i/o lines, 3 counters."
+#define ME1400_DESCRIPTION_DEVICE_ME1400EA			"ME-1400EA device, 24 digital i/o lines, 3 counters."
+#define ME1400_DESCRIPTION_DEVICE_ME1400B			"ME-1400B device, 48 digital i/o lines, 6 counters."
+#define ME1400_DESCRIPTION_DEVICE_ME1400EB			"ME-1400EB device, 48 digital i/o lines, 6 counters."
+#define ME1400_DESCRIPTION_DEVICE_ME1400C			"ME-1400C device, 24 digital i/o lines, 15 counters."
+#define ME1400_DESCRIPTION_DEVICE_ME1400D			"ME-1400D device, 48 digital i/o lines, 30 counters."
+
+/* ME-1600 defines */
+#define ME1600_NAME_DRIVER							"ME-1600"
+
+#define ME1600_NAME_DEVICE_ME16004U					"ME-1600/4U"
+#define ME1600_NAME_DEVICE_ME16008U					"ME-1600/8U"
+#define ME1600_NAME_DEVICE_ME160012U				"ME-1600/12U"
+#define ME1600_NAME_DEVICE_ME160016U				"ME-1600/16U"
+#define ME1600_NAME_DEVICE_ME160016U8I				"ME-1600/16U8I"
+
+#define ME1600_DESCRIPTION_DEVICE_ME16004U			"ME-1600/4U device, 4 voltage outputs."
+#define ME1600_DESCRIPTION_DEVICE_ME16008U			"ME-1600/8U device, 8 voltage outputs."
+#define ME1600_DESCRIPTION_DEVICE_ME160012U			"ME-1600/12U device, 12 voltage outputs."
+#define ME1600_DESCRIPTION_DEVICE_ME160016U			"ME-1600/16U device, 16 voltage outputs."
+#define ME1600_DESCRIPTION_DEVICE_ME160016U8I		"ME-1600/16U8I device, 16 voltage, 8 current outputs."
+
+/* ME-4000 defines */
+#define ME4600_NAME_DRIVER							"ME-4600"
+
+#define ME4600_NAME_DEVICE_ME4610					"ME-4610"
+#define ME4600_NAME_DEVICE_ME4650					"ME-4650"
+#define ME4600_NAME_DEVICE_ME4660					"ME-4660"
+#define ME4600_NAME_DEVICE_ME4660I					"ME-4660I"
+#define ME4600_NAME_DEVICE_ME4660S					"ME-4660S"
+#define ME4600_NAME_DEVICE_ME4660IS					"ME-4660IS"
+#define ME4600_NAME_DEVICE_ME4670					"ME-4670"
+#define ME4600_NAME_DEVICE_ME4670I					"ME-4670I"
+#define ME4600_NAME_DEVICE_ME4670S					"ME-4670S"
+#define ME4600_NAME_DEVICE_ME4670IS					"ME-4670IS"
+#define ME4600_NAME_DEVICE_ME4680					"ME-4680"
+#define ME4600_NAME_DEVICE_ME4680I					"ME-4680I"
+#define ME4600_NAME_DEVICE_ME4680S					"ME-4680S"
+#define ME4600_NAME_DEVICE_ME4680IS					"ME-4680IS"
+
+#define ME4600_DESCRIPTION_DEVICE_ME4610			"ME-4610 device, 16 streaming analog inputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4650			"ME-4650 device, 16 streaming analog inputs, 32 digital i/o lines, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4660			"ME-4660 device, 16 streaming analog inputs, 2 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4660I			"ME-4660I opto isolated device, 16 streaming analog inputs, 2 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4660S			"ME-4660 device, 16 streaming analog inputs (8 S&H), 2 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4660IS			"ME-4660I opto isolated device, 16 streaming analog inputs (8 S&H), 2 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4670			"ME-4670 device, 32 streaming analog inputs, 4 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4670I			"ME-4670I opto isolated device, 32 streaming analog inputs, 4 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4670S			"ME-4670S device, 32 streaming analog inputs (8 S&H), 4 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4670IS			"ME-4670IS opto isolated device, 32 streaming analog inputs (8 S&H), 4 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4680			"ME-4680 device, 32 streaming analog inputs, 4 streaming analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4680I			"ME-4680I opto isolated device, 32 streaming analog inputs, 4 streaming analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4680S			"ME-4680S device, 32 streaming analog inputs, 4 streaming analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4680IS			"ME-4680IS opto isolated device, 32 streaming analog inputs (8 S&H), 4 streaming analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+
+/* ME-6000 defines */
+#define ME6000_NAME_DRIVER							"ME-6000"
+
+#define ME6000_NAME_DEVICE_ME60004					"ME-6000/4"
+#define ME6000_NAME_DEVICE_ME60008					"ME-6000/8"
+#define ME6000_NAME_DEVICE_ME600016					"ME-6000/16"
+#define ME6000_NAME_DEVICE_ME6000I4					"ME-6000I/4"
+#define ME6000_NAME_DEVICE_ME6000I8					"ME-6000I/8"
+#define ME6000_NAME_DEVICE_ME6000I16				"ME-6000I/16"
+#define ME6000_NAME_DEVICE_ME6000ISLE4				"ME-6000ISLE/4"
+#define ME6000_NAME_DEVICE_ME6000ISLE8				"ME-6000ISLE/8"
+#define ME6000_NAME_DEVICE_ME6000ISLE16				"ME-6000ISLE/16"
+#define ME6000_NAME_DEVICE_ME61004					"ME-6100/4"
+#define ME6000_NAME_DEVICE_ME61008					"ME-6100/8"
+#define ME6000_NAME_DEVICE_ME610016					"ME-6100/16"
+#define ME6000_NAME_DEVICE_ME6100I4					"ME-6100I/4"
+#define ME6000_NAME_DEVICE_ME6100I8					"ME-6100I/8"
+#define ME6000_NAME_DEVICE_ME6100I16				"ME-6100I/16"
+#define ME6000_NAME_DEVICE_ME6100ISLE4				"ME-6100ISLE/4"
+#define ME6000_NAME_DEVICE_ME6100ISLE8				"ME-6100ISLE/8"
+#define ME6000_NAME_DEVICE_ME6100ISLE16				"ME-6100ISLE/16"
+#define ME6000_NAME_DEVICE_ME60004DIO				"ME-6000/4/DIO"
+#define ME6000_NAME_DEVICE_ME60008DIO				"ME-6000/8/DIO"
+#define ME6000_NAME_DEVICE_ME600016DIO				"ME-6000/16/DIO"
+#define ME6000_NAME_DEVICE_ME6000I4DIO				"ME-6000I/4/DIO"
+#define ME6000_NAME_DEVICE_ME6000I8DIO				"ME-6000I/8/DIO"
+#define ME6000_NAME_DEVICE_ME6000I16DIO				"ME-6000I/16/DIO"
+#define ME6000_NAME_DEVICE_ME6000ISLE4DIO			"ME-6000ISLE/4/DIO"
+#define ME6000_NAME_DEVICE_ME6000ISLE8DIO			"ME-6000ISLE/8/DIO"
+#define ME6000_NAME_DEVICE_ME6000ISLE16DIO			"ME-6000ISLE/16/DIO"
+#define ME6000_NAME_DEVICE_ME61004DIO				"ME-6100/4/DIO"
+#define ME6000_NAME_DEVICE_ME61008DIO				"ME-6100/8/DIO"
+#define ME6000_NAME_DEVICE_ME610016DIO				"ME-6100/16/DIO"
+#define ME6000_NAME_DEVICE_ME6100I4DIO				"ME-6100I/4/DIO"
+#define ME6000_NAME_DEVICE_ME6100I8DIO				"ME-6100I/8/DIO"
+#define ME6000_NAME_DEVICE_ME6100I16DIO				"ME-6100I/16/DIO"
+#define ME6000_NAME_DEVICE_ME6100ISLE4DIO			"ME-6100ISLE/4/DIO"
+#define ME6000_NAME_DEVICE_ME6100ISLE8DIO			"ME-6100ISLE/8/DIO"
+#define ME6000_NAME_DEVICE_ME6100ISLE16DIO			"ME-6100ISLE/16/DIO"
+#define ME6000_NAME_DEVICE_ME6200I9DIO				"ME-6200I/9/DIO"
+#define ME6000_NAME_DEVICE_ME6300I9DIO				"ME-6300I/9/DIO"
+
+#define ME6000_DESCRIPTION_DEVICE_ME60004			"ME-6000/4 device, 4 single analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME60008			"ME-6000/8 device, 8 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME600016			"ME-6000/16 device, 16 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME6000I4			"ME-6000I/4 isolated device, 4 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME6000I8			"ME-6000I/8 isolated device, 8 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME6000I16			"ME-6000I/16 isolated device, 16 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME6000ISLE4		"ME-6000ISLE/4 isle device, 4 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME6000ISLE8		"ME-6000ISLE/8 isle device, 8 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME6000ISLE16		"ME-6000ISLE/16 isle device, 16 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME61004			"ME-6100/4 device, 4 streaming analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME61008			"ME-6100/8 device, 4 streaming, 4 single analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME610016			"ME-6100/16 device, 4 streaming, 12 single analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME6100I4			"ME-6100I/4 isolated device, 4 streaming analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME6100I8			"ME-6100I/8 isolated device, 4 streaming, 4 single analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME6100I16			"ME-6100I/16 isolated device, 4 streaming, 12 single analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME6100ISLE4		"ME-6100ISLE/4 isle device, 4 streaming analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME6100ISLE8		"ME-6100ISLE/8 isle device, 4 streaming, 4 single analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME6100ISLE16		"ME-6100ISLE/16 isle device, 4 streaming, 12 single analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME60004DIO		"ME-6000/4/DIO device, 4 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME60008DIO		"ME-6000/8/DIO device, 8 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME600016DIO		"ME-6000/16/DIO device, 8 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6000I4DIO		"ME-6000I/4/DIO isolated device, 4 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6000I8DIO		"ME-6000I/8/DIO isolated device, 8 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6000I16DIO		"ME-6000I/16/DIO isolated device, 16 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6000ISLE4DIO	"ME-6000ISLE/4/DIO isle device, 4 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6000ISLE8DIO	"ME-6000ISLE/8/DIO isle device, 8 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6000ISLE16DIO	"ME-6000ISLE/16/DIO isle device, 16 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME61004DIO		"ME-6100/4/DIO device, 4 streaming analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME61008DIO		"ME-6100/8/DIO device, 4 streaming, 4 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME610016DIO		"ME-6100/16/DIO device, 4 streaming, 12 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6100I4DIO		"ME-6100I/4/DIO isolated device, 4 streaming analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6100I8DIO		"ME-6100I/8/DIO isolated device, 4 streaming, 4 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6100I16DIO		"ME-6100I/16/DIO isolated device, 4 streaming, 12 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6100ISLE4DIO	"ME-6100ISLE/4/DIO isle device, 4 streaming analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6100ISLE8DIO	"ME-6100ISLE/8/DIO isle device, 4 streaming, 4 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6100ISLE16DIO	"ME-6100ISLE/16/DIO isle device, 4 streaming, 12 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6200I9DIO		"ME-6200I/9/DIO isolated device, 9 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6300I9DIO		"ME-6300I/9/DIO isolated device, 4 streaming, 5 single analog outputs, 16 digital i/o lines."
+
+/* ME-630 defines */
+#define ME0600_NAME_DRIVER							"ME-0600"
+
+#define ME0600_NAME_DEVICE_ME0630					"ME-630"
+
+#define ME0600_DESCRIPTION_DEVICE_ME0630			"ME-630 device, up to 16 relay, 8 digital ttl input lines, 8 isolated digital input lines, 16 digital i/o lines, 2 external interrupts."
+
+/* ME-8100 defines */
+#define ME8100_NAME_DRIVER							"ME-8100"
+
+#define ME8100_NAME_DEVICE_ME8100A					"ME-8100A"
+#define ME8100_NAME_DEVICE_ME8100B					"ME-8100B"
+
+#define ME8100_DESCRIPTION_DEVICE_ME8100A			"ME-8100A opto isolated device, 16 digital input lines, 16 digital output lines."
+#define ME8100_DESCRIPTION_DEVICE_ME8100B			"ME-8100B opto isolated device, 32 digital input lines, 32 digital output lines, 3 counters."
+
+/* ME-8200 defines */
+#define ME8200_NAME_DRIVER							"ME-8200"
+
+#define ME8200_NAME_DEVICE_ME8200A					"ME-8200A"
+#define ME8200_NAME_DEVICE_ME8200B					"ME-8200B"
+
+#define ME8200_DESCRIPTION_DEVICE_ME8200A			"ME-8200A opto isolated device, 8 digital output lines, 8 digital input lines, 16 digital i/o lines."
+#define ME8200_DESCRIPTION_DEVICE_ME8200B			"ME-8200B opto isolated device, 16 digital output lines, 16 digital input lines, 16 digital i/o lines."
+
+/* ME-0900 defines */
+#define ME0900_NAME_DRIVER							"ME-0900"
+
+#define ME0900_NAME_DEVICE_ME0940					"ME-94"
+#define ME0900_NAME_DEVICE_ME0950					"ME-95"
+#define ME0900_NAME_DEVICE_ME0960					"ME-96"
+
+#define ME0900_DESCRIPTION_DEVICE_ME0940			"ME-94 device, 16 digital input lines, 2 external interrupt lines."
+#define ME0900_DESCRIPTION_DEVICE_ME0950			"ME-95 device, 16 digital output lines."
+#define ME0900_DESCRIPTION_DEVICE_ME0960			"ME-96 device, 8 digital input lines, 8 digital output lines, 2 external interrupt lines."
+
+/* ME-DUMMY defines */
+#define MEDUMMY_NAME_DRIVER							"ME-Dummy"
+
+/* MEPHISTO_S1 defines */
+/*
+#define MEPHISTO_S1_NAME_DRIVER						"MEphisto Scope 1"
+#define MEPHISTO_S1_NAME_DEVICE						"MEphisto Scope 1"
+#define MEPHISTO_S1_DESCRIPTION_DEVICE				"MEphisto Scope 1 device, 2 analog inputs, 24 digital i/o."
+*/
+/* Error defines */
+#define EMPTY_NAME_DRIVER							"ME-???"
+#define EMPTY_NAME_DEVICE							"ME-???"
+#define EMPTY_DESCRIPTION_DEVICE					"ME-??? unknown device"
+
+#endif
diff --git a/drivers/staging/meilhaus/meioctl.h b/drivers/staging/meilhaus/meioctl.h
new file mode 100644
index 000000000000..6dc719fba57c
--- /dev/null
+++ b/drivers/staging/meilhaus/meioctl.h
@@ -0,0 +1,515 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : meioctl.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ */
+
+#ifndef _MEIOCTL_H_
+#define _MEIOCTL_H_
+
+
+/*=============================================================================
+  Types for the input/output ioctls
+  ===========================================================================*/
+
+typedef struct me_io_irq_start {
+	int device;
+	int subdevice;
+	int channel;
+	int irq_source;
+	int irq_edge;
+	int irq_arg;
+	int flags;
+	int errno;
+} me_io_irq_start_t;
+
+
+typedef struct me_io_irq_wait {
+	int device;
+	int subdevice;
+	int channel;
+	int irq_count;
+	int value;
+	int time_out;
+	int flags;
+	int errno;
+} me_io_irq_wait_t;
+
+
+typedef struct me_io_irq_stop {
+	int device;
+	int subdevice;
+	int channel;
+	int flags;
+	int errno;
+} me_io_irq_stop_t;
+
+
+typedef struct me_io_reset_device {
+	int device;
+	int flags;
+	int errno;
+} me_io_reset_device_t;
+
+
+typedef struct me_io_reset_subdevice {
+	int device;
+	int subdevice;
+	int flags;
+	int errno;
+} me_io_reset_subdevice_t;
+
+
+typedef struct me_io_single_config {
+	int device;
+	int subdevice;
+	int channel;
+	int single_config;
+	int ref;
+	int trig_chan;
+	int trig_type;
+	int trig_edge;
+	int flags;
+	int errno;
+} me_io_single_config_t;
+
+
+typedef struct me_io_single {
+	meIOSingle_t *single_list;
+	int count;
+	int flags;
+	int errno;
+} me_io_single_t;
+
+
+typedef struct me_io_stream_config {
+	int device;
+	int subdevice;
+	meIOStreamConfig_t *config_list;
+	int count;
+	meIOStreamTrigger_t trigger;
+	int fifo_irq_threshold;
+	int flags;
+	int errno;
+} me_io_stream_config_t;
+
+
+typedef struct me_io_stream_new_values {
+	int device;
+	int subdevice;
+	int time_out;
+	int count;
+	int flags;
+	int errno;
+} me_io_stream_new_values_t;
+
+
+typedef struct me_io_stream_read {
+	int device;
+	int subdevice;
+	int read_mode;
+	int *values;
+	int count;
+	int flags;
+	int errno;
+} me_io_stream_read_t;
+
+
+typedef struct me_io_stream_start {
+	meIOStreamStart_t *start_list;
+	int count;
+	int flags;
+	int errno;
+} me_io_stream_start_t;
+
+
+typedef struct me_io_stream_status {
+	int device;
+	int subdevice;
+	int wait;
+	int status;
+	int count;
+	int flags;
+	int errno;
+} me_io_stream_status_t;
+
+
+typedef struct me_io_stream_stop {
+	meIOStreamStop_t *stop_list;
+	int count;
+	int flags;
+	int errno;
+} me_io_stream_stop_t;
+
+
+typedef struct me_io_stream_write {
+	int device;
+	int subdevice;
+	int write_mode;
+	int *values;
+	int count;
+	int flags;
+	int errno;
+} me_io_stream_write_t;
+
+
+/*=============================================================================
+  Types for the lock ioctls
+  ===========================================================================*/
+
+typedef struct me_lock_device {
+	int device;
+	int lock;
+	int flags;
+	int errno;
+} me_lock_device_t;
+
+
+typedef struct me_lock_driver {
+	int flags;
+	int lock;
+	int errno;
+} me_lock_driver_t;
+
+
+typedef struct me_lock_subdevice {
+	int device;
+	int subdevice;
+	int lock;
+	int flags;
+	int errno;
+} me_lock_subdevice_t;
+
+
+/*=============================================================================
+  Types for the query ioctls
+  ===========================================================================*/
+
+typedef struct me_query_info_device {
+	int device;
+	int vendor_id;
+	int device_id;
+	int serial_no;
+	int bus_type;
+	int bus_no;
+	int dev_no;
+	int func_no;
+	int plugged;
+	int errno;
+} me_query_info_device_t;
+
+
+typedef struct me_query_description_device {
+	int device;
+	char *name;
+	int count;
+	int errno;
+} me_query_description_device_t;
+
+
+typedef struct me_query_name_device {
+	int device;
+	char *name;
+	int count;
+	int errno;
+} me_query_name_device_t;
+
+
+typedef struct me_query_name_device_driver {
+	int device;
+	char *name;
+	int count;
+	int errno;
+} me_query_name_device_driver_t;
+
+
+typedef struct me_query_version_main_driver {
+	int version;
+	int errno;
+} me_query_version_main_driver_t;
+
+
+typedef struct me_query_version_device_driver {
+	int device;
+	int version;
+	int errno;
+} me_query_version_device_driver_t;
+
+
+typedef struct me_query_number_devices {
+	int number;
+	int errno;
+} me_query_number_devices_t;
+
+
+typedef struct me_query_number_subdevices {
+	int device;
+	int number;
+	int errno;
+} me_query_number_subdevices_t;
+
+
+typedef struct me_query_number_channels {
+	int device;
+	int subdevice;
+	int number;
+	int errno;
+} me_query_number_channels_t;
+
+
+typedef struct me_query_number_ranges {
+	int device;
+	int subdevice;
+	int channel;
+	int unit;
+	int number;
+	int errno;
+} me_query_number_ranges_t;
+
+
+typedef struct me_query_subdevice_by_type {
+	int device;
+	int start_subdevice;
+	int type;
+	int subtype;
+	int subdevice;
+	int errno;
+} me_query_subdevice_by_type_t;
+
+
+typedef struct me_query_subdevice_type {
+	int device;
+	int subdevice;
+	int type;
+	int subtype;
+	int errno;
+} me_query_subdevice_type_t;
+
+
+typedef struct me_query_subdevice_caps {
+	int device;
+	int subdevice;
+	int caps;
+	int errno;
+} me_query_subdevice_caps_t;
+
+
+typedef struct me_query_subdevice_caps_args {
+	int device;
+	int subdevice;
+	int cap;
+	int args[8];
+	int count;
+	int errno;
+} me_query_subdevice_caps_args_t;
+
+
+typedef struct me_query_timer {
+	int device;
+	int subdevice;
+	int timer;
+	int base_frequency;
+	long long min_ticks;
+	long long max_ticks;
+	int errno;
+} me_query_timer_t;
+
+
+typedef struct me_query_range_by_min_max {
+	int device;
+	int subdevice;
+	int channel;
+	int unit;
+	int min;
+	int max;
+	int max_data;
+	int range;
+	int errno;
+} me_query_range_by_min_max_t;
+
+
+typedef struct me_query_range_info {
+	int device;
+	int subdevice;
+	int channel;
+	int unit;
+	int range;
+	int min;
+	int max;
+	int max_data;
+	int errno;
+} me_query_range_info_t;
+
+
+/*=============================================================================
+  Types for the configuration ioctls
+  ===========================================================================*/
+
+typedef struct me_cfg_tcpip_location {
+	int access_type;
+	char *remote_host;
+	int remote_device_number;
+} me_cfg_tcpip_location_t;
+
+
+typedef union me_cfg_tcpip {
+	int access_type;
+	me_cfg_tcpip_location_t location;
+} me_cfg_tcpip_t;
+
+
+typedef struct me_cfg_pci_hw_location {
+	unsigned int bus_type;
+	unsigned int bus_no;
+	unsigned int device_no;
+	unsigned int function_no;
+} me_cfg_pci_hw_location_t;
+
+/*
+typedef struct me_cfg_usb_hw_location {
+	unsigned int bus_type;
+	unsigned int root_hub_no;
+} me_cfg_usb_hw_location_t;
+*/
+
+typedef union me_cfg_hw_location {
+	unsigned int bus_type;
+	me_cfg_pci_hw_location_t pci;
+//	me_cfg_usb_hw_location_t usb;
+} me_cfg_hw_location_t;
+
+
+typedef struct me_cfg_device_info {
+	unsigned int vendor_id;
+	unsigned int device_id;
+	unsigned int serial_no;
+	me_cfg_hw_location_t hw_location;
+} me_cfg_device_info_t;
+
+
+typedef struct me_cfg_subdevice_info {
+	int type;
+	int sub_type;
+	unsigned int number_channels;
+} me_cfg_subdevice_info_t;
+
+
+typedef struct me_cfg_range_entry {
+	int unit;
+	double min;
+	double max;
+	unsigned int max_data;
+} me_cfg_range_entry_t;
+
+
+typedef struct me_cfg_mux32m_device {
+	int type;
+	int timed;
+	unsigned int ai_channel;
+	unsigned int dio_device;
+	unsigned int dio_subdevice;
+	unsigned int timer_device;
+	unsigned int timer_subdevice;
+	unsigned int mux32s_count;
+} me_cfg_mux32m_device_t;
+
+
+typedef struct me_cfg_demux32_device {
+	int type;
+	int timed;
+	unsigned int ao_channel;
+	unsigned int dio_device;
+	unsigned int dio_subdevice;
+	unsigned int timer_device;
+	unsigned int timer_subdevice;
+} me_cfg_demux32_device_t;
+
+
+typedef union me_cfg_external_device {
+	int type;
+	me_cfg_mux32m_device_t mux32m;
+	me_cfg_demux32_device_t demux32;
+} me_cfg_external_device_t;
+
+
+typedef struct me_cfg_subdevice_entry {
+	me_cfg_subdevice_info_t info;
+	me_cfg_range_entry_t *range_list;
+	unsigned int count;
+	int locked;
+	me_cfg_external_device_t external_device;
+} me_cfg_subdevice_entry_t;
+
+
+typedef struct me_cfg_device_entry {
+	me_cfg_tcpip_t tcpip;
+	me_cfg_device_info_t info;
+	me_cfg_subdevice_entry_t *subdevice_list;
+	unsigned int count;
+} me_cfg_device_entry_t;
+
+
+typedef struct me_config_load {
+	me_cfg_device_entry_t *device_list;
+	unsigned int count;
+	int errno;
+} me_config_load_t;
+
+
+/*=============================================================================
+  The ioctls of the board
+  ===========================================================================*/
+
+#define MEMAIN_MAGIC 'y'
+
+#define ME_IO_IRQ_ENABLE				_IOR (MEMAIN_MAGIC, 1, me_io_irq_start_t)
+#define ME_IO_IRQ_WAIT					_IOR (MEMAIN_MAGIC, 2, me_io_irq_wait_t)
+#define ME_IO_IRQ_DISABLE				_IOR (MEMAIN_MAGIC, 3, me_io_irq_stop_t)
+
+#define ME_IO_RESET_DEVICE				_IOW (MEMAIN_MAGIC, 4, me_io_reset_device_t)
+#define ME_IO_RESET_SUBDEVICE			_IOW (MEMAIN_MAGIC, 5, me_io_reset_subdevice_t)
+
+#define ME_IO_SINGLE					_IOWR(MEMAIN_MAGIC, 6, me_io_single_t)
+#define ME_IO_SINGLE_CONFIG				_IOW (MEMAIN_MAGIC, 7, me_io_single_config_t)
+
+#define ME_IO_STREAM_CONFIG				_IOW (MEMAIN_MAGIC, 8, me_io_stream_config_t)
+#define ME_IO_STREAM_NEW_VALUES			_IOR (MEMAIN_MAGIC, 9, me_io_stream_new_values_t)
+#define ME_IO_STREAM_READ				_IOR (MEMAIN_MAGIC, 10, me_io_stream_read_t)
+#define ME_IO_STREAM_START				_IOW (MEMAIN_MAGIC, 11, me_io_stream_start_t)
+#define ME_IO_STREAM_STATUS				_IOR (MEMAIN_MAGIC, 12, me_io_stream_status_t)
+#define ME_IO_STREAM_STOP				_IOW (MEMAIN_MAGIC, 13, me_io_stream_stop_t)
+#define ME_IO_STREAM_WRITE				_IOW (MEMAIN_MAGIC, 14, me_io_stream_write_t)
+
+#define ME_LOCK_DRIVER					_IOW (MEMAIN_MAGIC, 15, me_lock_driver_t)
+#define ME_LOCK_DEVICE					_IOW (MEMAIN_MAGIC, 16, me_lock_device_t)
+#define ME_LOCK_SUBDEVICE				_IOW (MEMAIN_MAGIC, 17, me_lock_subdevice_t)
+
+#define ME_QUERY_DESCRIPTION_DEVICE		_IOR (MEMAIN_MAGIC, 18, me_query_description_device_t)
+
+#define ME_QUERY_INFO_DEVICE			_IOR (MEMAIN_MAGIC, 19, me_query_info_device_t)
+
+#define ME_QUERY_NAME_DEVICE			_IOR (MEMAIN_MAGIC, 20, me_query_name_device_t)
+#define ME_QUERY_NAME_DEVICE_DRIVER		_IOR (MEMAIN_MAGIC, 21, me_query_name_device_driver_t)
+
+#define ME_QUERY_NUMBER_DEVICES			_IOR (MEMAIN_MAGIC, 22, me_query_number_devices_t)
+#define ME_QUERY_NUMBER_SUBDEVICES		_IOR (MEMAIN_MAGIC, 23, me_query_number_subdevices_t)
+#define ME_QUERY_NUMBER_CHANNELS		_IOR (MEMAIN_MAGIC, 24, me_query_number_channels_t)
+#define ME_QUERY_NUMBER_RANGES			_IOR (MEMAIN_MAGIC, 25, me_query_number_ranges_t)
+
+#define ME_QUERY_RANGE_BY_MIN_MAX		_IOR (MEMAIN_MAGIC, 26, me_query_range_by_min_max_t)
+#define ME_QUERY_RANGE_INFO				_IOR (MEMAIN_MAGIC, 27, me_query_range_info_t)
+
+#define ME_QUERY_SUBDEVICE_BY_TYPE		_IOR (MEMAIN_MAGIC, 28, me_query_subdevice_by_type_t)
+#define ME_QUERY_SUBDEVICE_TYPE			_IOR (MEMAIN_MAGIC, 29, me_query_subdevice_type_t)
+#define ME_QUERY_SUBDEVICE_CAPS			_IOR (MEMAIN_MAGIC, 29, me_query_subdevice_caps_t)
+#define ME_QUERY_SUBDEVICE_CAPS_ARGS	_IOR (MEMAIN_MAGIC, 30, me_query_subdevice_caps_args_t)
+
+#define ME_QUERY_TIMER					_IOR (MEMAIN_MAGIC, 31, me_query_timer_t)
+
+#define ME_QUERY_VERSION_DEVICE_DRIVER	_IOR (MEMAIN_MAGIC, 32, me_query_version_device_driver_t)
+#define ME_QUERY_VERSION_MAIN_DRIVER	_IOR (MEMAIN_MAGIC, 33, me_query_version_main_driver_t)
+
+#define ME_CONFIG_LOAD					_IOWR(MEMAIN_MAGIC, 34, me_config_load_t)
+
+#endif
diff --git a/drivers/staging/meilhaus/memain.c b/drivers/staging/meilhaus/memain.c
new file mode 100644
index 000000000000..b09d1a6c766c
--- /dev/null
+++ b/drivers/staging/meilhaus/memain.c
@@ -0,0 +1,2022 @@
+/**
+ * @file memain.c
+ *
+ * @brief Main Meilhaus device driver.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+#include <linux/pci.h>
+//#include <linux/usb.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+#include <linux/cdev.h>
+#include <linux/rwsem.h>
+
+#include "medefines.h"
+#include "metypes.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "memain.h"
+#include "medevice.h"
+#include "meioctl.h"
+#include "mecommon.h"
+
+/* Module parameters
+*/
+
+#ifdef BOSCH
+static unsigned int me_bosch_fw = 0;
+
+# ifdef module_param
+module_param(me_bosch_fw, int, S_IRUGO);
+# else
+MODULE_PARM(me_bosch_fw, "i");
+# endif
+
+MODULE_PARM_DESC(me_bosch_fw,
+		 "Flags which signals the ME-4600 driver to load the bosch firmware (default = 0).");
+#endif //BOSCH
+
+static unsigned int major = 0;
+#ifdef module_param
+module_param(major, int, S_IRUGO);
+#else
+MODULE_PARM(major, "i");
+#endif
+
+/* Global Driver Lock
+*/
+
+static struct file *me_filep = NULL;
+static int me_count = 0;
+static spinlock_t me_lock = SPIN_LOCK_UNLOCKED;
+static DECLARE_RWSEM(me_rwsem);
+
+/* Board instances are kept in a global list */
+LIST_HEAD(me_device_list);
+
+/* Prototypes
+*/
+
+static int me_probe_pci(struct pci_dev *dev, const struct pci_device_id *id);
+static void me_remove_pci(struct pci_dev *dev);
+static int insert_to_device_list(me_device_t * n_device);
+static int replace_with_dummy(int vendor_id, int device_id, int serial_no);
+static void clear_device_list(void);
+static int me_open(struct inode *inode_ptr, struct file *filep);
+static int me_release(struct inode *, struct file *);
+static int me_ioctl(struct inode *, struct file *, unsigned int, unsigned long);
+//static int me_probe_usb(struct usb_interface *interface, const struct usb_device_id *id);
+//static void me_disconnect_usb(struct usb_interface *interface);
+
+/* Character device structure
+*/
+
+static struct cdev *cdevp;
+
+/* File operations provided by the module
+*/
+
+static struct file_operations me_file_operations = {
+	.owner = THIS_MODULE,
+	.ioctl = me_ioctl,
+	.open = me_open,
+	.release = me_release,
+};
+
+struct pci_driver me_pci_driver = {
+	.name = MEMAIN_NAME,
+	.id_table = me_pci_table,
+	.probe = me_probe_pci,
+	.remove = me_remove_pci
+};
+
+/* //me_usb_driver
+static struct usb_driver me_usb_driver =
+{
+	.name = MEMAIN_NAME,
+	.id_table = me_usb_table,
+	.probe = me_probe_usb,
+	.disconnect = me_disconnect_usb
+};
+*/
+
+#ifdef ME_LOCK_MULTIPLEX_TEMPLATE
+ME_LOCK_MULTIPLEX_TEMPLATE("me_lock_device",
+			   me_lock_device_t,
+			   me_lock_device,
+			   me_device_lock_device,
+			   (device, filep, karg.lock, karg.flags))
+
+    ME_LOCK_MULTIPLEX_TEMPLATE("me_lock_subdevice",
+			   me_lock_subdevice_t,
+			   me_lock_subdevice,
+			   me_device_lock_subdevice,
+			   (device, filep, karg.subdevice, karg.lock,
+			karg.flags))
+#else
+#error macro ME_LOCK_MULTIPLEX_TEMPLATE not defined
+#endif
+
+#ifdef ME_IO_MULTIPLEX_TEMPLATE
+ME_IO_MULTIPLEX_TEMPLATE("me_io_irq_start",
+			 me_io_irq_start_t,
+			 me_io_irq_start,
+			 me_device_io_irq_start,
+			 (device,
+			  filep,
+			  karg.subdevice,
+			  karg.channel,
+			  karg.irq_source,
+			  karg.irq_edge, karg.irq_arg, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_irq_wait",
+			 me_io_irq_wait_t,
+			 me_io_irq_wait,
+			 me_device_io_irq_wait,
+			 (device,
+		      filep,
+		      karg.subdevice,
+		      karg.channel,
+		      &karg.irq_count, &karg.value, karg.time_out, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_irq_stop",
+			 me_io_irq_stop_t,
+			 me_io_irq_stop,
+			 me_device_io_irq_stop,
+			 (device,
+		      filep, karg.subdevice, karg.channel, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_reset_device",
+			 me_io_reset_device_t,
+			 me_io_reset_device,
+			 me_device_io_reset_device, (device, filep, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_reset_subdevice",
+			 me_io_reset_subdevice_t,
+			 me_io_reset_subdevice,
+			 me_device_io_reset_subdevice,
+			 (device, filep, karg.subdevice, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_single_config",
+			 me_io_single_config_t,
+			 me_io_single_config,
+			 me_device_io_single_config,
+			 (device,
+		      filep,
+		      karg.subdevice,
+		      karg.channel,
+		      karg.single_config,
+		      karg.ref,
+		      karg.trig_chan,
+		      karg.trig_type, karg.trig_edge, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_stream_new_values",
+			 me_io_stream_new_values_t,
+			 me_io_stream_new_values,
+			 me_device_io_stream_new_values,
+			 (device,
+		      filep,
+		      karg.subdevice, karg.time_out, &karg.count, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_stream_read",
+			 me_io_stream_read_t,
+			 me_io_stream_read,
+			 me_device_io_stream_read,
+			 (device,
+		      filep,
+		      karg.subdevice,
+		      karg.read_mode, karg.values, &karg.count, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_stream_status",
+			 me_io_stream_status_t,
+			 me_io_stream_status,
+			 me_device_io_stream_status,
+			 (device,
+		      filep,
+		      karg.subdevice,
+		      karg.wait, &karg.status, &karg.count, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_stream_write",
+			 me_io_stream_write_t,
+			 me_io_stream_write,
+			 me_device_io_stream_write,
+			 (device,
+		      filep,
+		      karg.subdevice,
+		      karg.write_mode, karg.values, &karg.count, karg.flags))
+#else
+#error macro ME_IO_MULTIPLEX_TEMPLATE not defined
+#endif
+
+#ifdef ME_QUERY_MULTIPLEX_STR_TEMPLATE
+ME_QUERY_MULTIPLEX_STR_TEMPLATE("me_query_name_device",
+				me_query_name_device_t,
+				me_query_name_device,
+				me_device_query_name_device, (device, &msg))
+
+    ME_QUERY_MULTIPLEX_STR_TEMPLATE("me_query_name_device_driver",
+				me_query_name_device_driver_t,
+				me_query_name_device_driver,
+				me_device_query_name_device_driver,
+				(device, &msg))
+
+    ME_QUERY_MULTIPLEX_STR_TEMPLATE("me_query_description_device",
+				me_query_description_device_t,
+				me_query_description_device,
+				me_device_query_description_device,
+				(device, &msg))
+#else
+#error macro ME_QUERY_MULTIPLEX_STR_TEMPLATE not defined
+#endif
+
+#ifdef ME_QUERY_MULTIPLEX_TEMPLATE
+ME_QUERY_MULTIPLEX_TEMPLATE("me_query_info_device",
+			    me_query_info_device_t,
+			    me_query_info_device,
+			    me_device_query_info_device,
+			    (device,
+			     &karg.vendor_id,
+			     &karg.device_id,
+			     &karg.serial_no,
+			     &karg.bus_type,
+			     &karg.bus_no,
+			     &karg.dev_no, &karg.func_no, &karg.plugged))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_number_subdevices",
+			    me_query_number_subdevices_t,
+			    me_query_number_subdevices,
+			    me_device_query_number_subdevices,
+			    (device, &karg.number))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_number_channels",
+			    me_query_number_channels_t,
+			    me_query_number_channels,
+			    me_device_query_number_channels,
+			    (device, karg.subdevice, &karg.number))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_subdevice_by_type",
+			    me_query_subdevice_by_type_t,
+			    me_query_subdevice_by_type,
+			    me_device_query_subdevice_by_type,
+			    (device,
+			 karg.start_subdevice,
+			 karg.type, karg.subtype, &karg.subdevice))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_subdevice_type",
+			    me_query_subdevice_type_t,
+			    me_query_subdevice_type,
+			    me_device_query_subdevice_type,
+			    (device, karg.subdevice, &karg.type, &karg.subtype))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_subdevice_caps",
+			    me_query_subdevice_caps_t,
+			    me_query_subdevice_caps,
+			    me_device_query_subdevice_caps,
+			    (device, karg.subdevice, &karg.caps))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_subdevice_caps_args",
+			    me_query_subdevice_caps_args_t,
+			    me_query_subdevice_caps_args,
+			    me_device_query_subdevice_caps_args,
+			    (device, karg.subdevice, karg.cap, karg.args,
+			 karg.count))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_number_ranges",
+			    me_query_number_ranges_t,
+			    me_query_number_ranges,
+			    me_device_query_number_ranges,
+			    (device, karg.subdevice, karg.unit, &karg.number))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_range_by_min_max",
+			    me_query_range_by_min_max_t,
+			    me_query_range_by_min_max,
+			    me_device_query_range_by_min_max,
+			    (device,
+			 karg.subdevice,
+			 karg.unit,
+			 &karg.min, &karg.max, &karg.max_data, &karg.range))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_range_info",
+			    me_query_range_info_t,
+			    me_query_range_info,
+			    me_device_query_range_info,
+			    (device,
+			 karg.subdevice,
+			 karg.range,
+			 &karg.unit, &karg.min, &karg.max, &karg.max_data))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_timer",
+			    me_query_timer_t,
+			    me_query_timer,
+			    me_device_query_timer,
+			    (device,
+			 karg.subdevice,
+			 karg.timer,
+			 &karg.base_frequency,
+			 &karg.min_ticks, &karg.max_ticks))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_version_device_driver",
+			    me_query_version_device_driver_t,
+			    me_query_version_device_driver,
+			    me_device_query_version_device_driver,
+			    (device, &karg.version))
+#else
+#error macro ME_QUERY_MULTIPLEX_TEMPLATE not defined
+#endif
+
+/** ******************************************************************************** **/
+
+static me_device_t *get_dummy_instance(unsigned short vendor_id,
+				       unsigned short device_id,
+				       unsigned int serial_no,
+				       int bus_type,
+				       int bus_no, int dev_no, int func_no)
+{
+	int err;
+	me_dummy_constructor_t constructor = NULL;
+	me_device_t *instance;
+
+	PDEBUG("executed.\n");
+
+	if ((constructor = symbol_get(medummy_constructor)) == NULL) {
+		err = request_module(MEDUMMY_NAME);
+
+		if (err) {
+			PERROR("Error while request for module %s.\n",
+			       MEDUMMY_NAME);
+			return NULL;
+		}
+
+		if ((constructor = symbol_get(medummy_constructor)) == NULL) {
+			PERROR("Can't get %s driver module constructor.\n",
+			       MEDUMMY_NAME);
+			return NULL;
+		}
+	}
+
+	if ((instance = (*constructor) (vendor_id,
+					device_id,
+					serial_no,
+					bus_type,
+					bus_no, dev_no, func_no)) == NULL)
+		symbol_put(medummy_constructor);
+
+	return instance;
+}
+
+static int me_probe_pci(struct pci_dev *dev, const struct pci_device_id *id)
+{
+	int err;
+	me_pci_constructor_t constructor = NULL;
+#ifdef BOSCH
+	me_bosch_constructor_t constructor_bosch = NULL;
+#endif
+	me_device_t *n_device = NULL;
+	uint32_t device;
+
+	char constructor_name[24] = "me0000_pci_constructor";
+	char module_name[7] = "me0000";
+
+	PDEBUG("executed.\n");
+	device = dev->device;
+	if ((device & 0xF000) == 0x6000) {	// Exceptions: me61xx, me62xx, me63xx are handled by one driver.
+		device &= 0xF0FF;
+	}
+
+	constructor_name[2] += (char)((device >> 12) & 0x000F);
+	constructor_name[3] += (char)((device >> 8) & 0x000F);
+	PDEBUG("constructor_name: %s\n", constructor_name);
+	module_name[2] += (char)((device >> 12) & 0x000F);
+	module_name[3] += (char)((device >> 8) & 0x000F);
+	PDEBUG("module_name: %s\n", module_name);
+
+	if ((constructor =
+	     (me_pci_constructor_t) symbol_get(constructor_name)) == NULL) {
+		if (request_module(module_name)) {
+			PERROR("Error while request for module %s.\n",
+			       module_name);
+			return -ENODEV;
+		}
+
+		if ((constructor =
+		     (me_pci_constructor_t) symbol_get(constructor_name)) ==
+		    NULL) {
+			PERROR("Can't get %s driver module constructor.\n",
+			       module_name);
+			return -ENODEV;
+		}
+	}
+#ifdef BOSCH
+	if ((device & 0xF000) == 0x4000) {	// Bosch build has differnt constructor for me4600.
+		if ((n_device =
+		     (*constructor_bosch) (dev, me_bosch_fw)) == NULL) {
+			symbol_put(constructor_name);
+			PERROR
+			    ("Can't get device instance of %s driver module.\n",
+			     module_name);
+			return -ENODEV;
+		}
+	} else {
+#endif
+		if ((n_device = (*constructor) (dev)) == NULL) {
+			symbol_put(constructor_name);
+			PERROR
+			    ("Can't get device instance of %s driver module.\n",
+			     module_name);
+			return -ENODEV;
+		}
+#ifdef BOSCH
+	}
+#endif
+
+	insert_to_device_list(n_device);
+	err =
+	    n_device->me_device_io_reset_device(n_device, NULL,
+						ME_IO_RESET_DEVICE_NO_FLAGS);
+	if (err) {
+		PERROR("Error while reseting device.\n");
+	} else {
+		PDEBUG("Reseting device was sucessful.\n");
+	}
+	return ME_ERRNO_SUCCESS;
+}
+
+static void release_instance(me_device_t * device)
+{
+	int vendor_id;
+	int device_id;
+	int serial_no;
+	int bus_type;
+	int bus_no;
+	int dev_no;
+	int func_no;
+	int plugged;
+
+	uint32_t dev_id;
+
+	char constructor_name[24] = "me0000_pci_constructor";
+
+	PDEBUG("executed.\n");
+
+	device->me_device_query_info_device(device,
+					    &vendor_id,
+					    &device_id,
+					    &serial_no,
+					    &bus_type,
+					    &bus_no,
+					    &dev_no, &func_no, &plugged);
+
+	dev_id = device_id;
+	device->me_device_destructor(device);
+
+	if (plugged != ME_PLUGGED_IN) {
+		PDEBUG("release: medummy_constructor\n");
+
+		symbol_put("medummy_constructor");
+	} else {
+		if ((dev_id & 0xF000) == 0x6000) {	// Exceptions: me61xx, me62xx, me63xx are handled by one driver.
+			dev_id &= 0xF0FF;
+		}
+
+		constructor_name[2] += (char)((dev_id >> 12) & 0x000F);
+		constructor_name[3] += (char)((dev_id >> 8) & 0x000F);
+		PDEBUG("release: %s\n", constructor_name);
+
+		symbol_put(constructor_name);
+	}
+}
+
+static int insert_to_device_list(me_device_t * n_device)
+{
+	me_device_t *o_device = NULL;
+
+	struct list_head *pos;
+	int n_vendor_id;
+	int n_device_id;
+	int n_serial_no;
+	int n_bus_type;
+	int n_bus_no;
+	int n_dev_no;
+	int n_func_no;
+	int n_plugged;
+	int o_vendor_id;
+	int o_device_id;
+	int o_serial_no;
+	int o_bus_type;
+	int o_bus_no;
+	int o_dev_no;
+	int o_func_no;
+	int o_plugged;
+
+	PDEBUG("executed.\n");
+
+	n_device->me_device_query_info_device(n_device,
+					      &n_vendor_id,
+					      &n_device_id,
+					      &n_serial_no,
+					      &n_bus_type,
+					      &n_bus_no,
+					      &n_dev_no,
+					      &n_func_no, &n_plugged);
+
+	down_write(&me_rwsem);
+
+	list_for_each(pos, &me_device_list) {
+		o_device = list_entry(pos, me_device_t, list);
+		o_device->me_device_query_info_device(o_device,
+						      &o_vendor_id,
+						      &o_device_id,
+						      &o_serial_no,
+						      &o_bus_type,
+						      &o_bus_no,
+						      &o_dev_no,
+						      &o_func_no, &o_plugged);
+
+		if (o_plugged == ME_PLUGGED_OUT) {
+			if (((o_vendor_id == n_vendor_id) &&
+			     (o_device_id == n_device_id) &&
+			     (o_serial_no == n_serial_no) &&
+			     (o_bus_type == n_bus_type)) ||
+			    ((o_vendor_id == n_vendor_id) &&
+			     (o_device_id == n_device_id) &&
+			     (o_bus_type == n_bus_type) &&
+			     (o_bus_no == n_bus_no) &&
+			     (o_dev_no == n_dev_no) &&
+			     (o_func_no == n_func_no))) {
+				n_device->list.prev = pos->prev;
+				n_device->list.next = pos->next;
+				pos->prev->next = &n_device->list;
+				pos->next->prev = &n_device->list;
+				release_instance(o_device);
+				break;
+			}
+		}
+	}
+
+	if (pos == &me_device_list) {
+		list_add_tail(&n_device->list, &me_device_list);
+	}
+
+	up_write(&me_rwsem);
+
+	return 0;
+}
+
+static void me_remove_pci(struct pci_dev *dev)
+{
+	int vendor_id = dev->vendor;
+	int device_id = dev->device;
+	int subsystem_vendor = dev->subsystem_vendor;
+	int subsystem_device = dev->subsystem_device;
+	int serial_no = (subsystem_device << 16) | subsystem_vendor;
+
+	PDEBUG("executed.\n");
+
+	PINFO("Vendor id = 0x%08X\n", vendor_id);
+	PINFO("Device id = 0x%08X\n", device_id);
+	PINFO("Serial Number = 0x%08X\n", serial_no);
+
+	replace_with_dummy(vendor_id, device_id, serial_no);
+}
+
+static int replace_with_dummy(int vendor_id, int device_id, int serial_no)
+{
+
+	struct list_head *pos;
+	me_device_t *n_device = NULL;
+	me_device_t *o_device = NULL;
+	int o_vendor_id;
+	int o_device_id;
+	int o_serial_no;
+	int o_bus_type;
+	int o_bus_no;
+	int o_dev_no;
+	int o_func_no;
+	int o_plugged;
+
+	PDEBUG("executed.\n");
+
+	down_write(&me_rwsem);
+
+	list_for_each(pos, &me_device_list) {
+		o_device = list_entry(pos, me_device_t, list);
+		o_device->me_device_query_info_device(o_device,
+						      &o_vendor_id,
+						      &o_device_id,
+						      &o_serial_no,
+						      &o_bus_type,
+						      &o_bus_no,
+						      &o_dev_no,
+						      &o_func_no, &o_plugged);
+
+		if (o_plugged == ME_PLUGGED_IN) {
+			if (((o_vendor_id == vendor_id) &&
+			     (o_device_id == device_id) &&
+			     (o_serial_no == serial_no))) {
+				n_device = get_dummy_instance(o_vendor_id,
+							      o_device_id,
+							      o_serial_no,
+							      o_bus_type,
+							      o_bus_no,
+							      o_dev_no,
+							      o_func_no);
+
+				if (!n_device) {
+					up_write(&me_rwsem);
+					PERROR("Cannot get dummy instance.\n");
+					return 1;
+				}
+
+				n_device->list.prev = pos->prev;
+
+				n_device->list.next = pos->next;
+				pos->prev->next = &n_device->list;
+				pos->next->prev = &n_device->list;
+				release_instance(o_device);
+				break;
+			}
+		}
+	}
+
+	up_write(&me_rwsem);
+
+	return 0;
+}
+
+static void clear_device_list(void)
+{
+
+	struct list_head *entry;
+	me_device_t *device;
+
+	// Clear the device info list .
+	down_write(&me_rwsem);
+
+	while (!list_empty(&me_device_list)) {
+		entry = me_device_list.next;
+		device = list_entry(entry, me_device_t, list);
+		list_del(entry);
+		release_instance(device);
+	}
+
+	up_write(&me_rwsem);
+}
+
+static int lock_driver(struct file *filep, int lock, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_device_t *device;
+
+	PDEBUG("executed.\n");
+
+	down_read(&me_rwsem);
+
+	spin_lock(&me_lock);
+
+	switch (lock) {
+
+	case ME_LOCK_SET:
+		if (me_count) {
+			PERROR
+			    ("Driver System is currently used by another process.\n");
+			err = ME_ERRNO_USED;
+		} else if ((me_filep != NULL) && (me_filep != filep)) {
+			PERROR
+			    ("Driver System is already logged by another process.\n");
+			err = ME_ERRNO_LOCKED;
+		} else {
+			list_for_each_entry(device, &me_device_list, list) {
+				err =
+				    device->me_device_lock_device(device, filep,
+								  ME_LOCK_CHECK,
+								  flags);
+
+				if (err)
+					break;
+			}
+
+			if (!err)
+				me_filep = filep;
+		}
+
+		break;
+
+	case ME_LOCK_RELEASE:
+		if ((me_filep != NULL) && (me_filep != filep)) {
+			err = ME_ERRNO_SUCCESS;
+		} else {
+			list_for_each_entry(device, &me_device_list, list) {
+				device->me_device_lock_device(device, filep,
+							      ME_LOCK_RELEASE,
+							      flags);
+			}
+
+			me_filep = NULL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid lock specified.\n");
+
+		err = ME_ERRNO_INVALID_LOCK;
+
+		break;
+	}
+
+	spin_unlock(&me_lock);
+
+	up_read(&me_rwsem);
+
+	return err;
+}
+
+static int me_lock_driver(struct file *filep, me_lock_driver_t * arg)
+{
+	int err = 0;
+
+	me_lock_driver_t lock;
+
+	PDEBUG("executed.\n");
+
+	err = copy_from_user(&lock, arg, sizeof(me_lock_driver_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to kernel space.\n");
+		return -EFAULT;
+	}
+
+	lock.errno = lock_driver(filep, lock.lock, lock.flags);
+
+	err = copy_to_user(arg, &lock, sizeof(me_lock_driver_t));
+
+	if (err) {
+		PERROR("Can't copy query back to user space.\n");
+		return -EFAULT;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me_open(struct inode *inode_ptr, struct file *filep)
+{
+
+	PDEBUG("executed.\n");
+	// Nothing to do here.
+	return 0;
+}
+
+static int me_release(struct inode *inode_ptr, struct file *filep)
+{
+
+	PDEBUG("executed.\n");
+	lock_driver(filep, ME_LOCK_RELEASE, ME_LOCK_DRIVER_NO_FLAGS);
+
+	return 0;
+}
+
+static int me_query_version_main_driver(struct file *filep,
+					me_query_version_main_driver_t * arg)
+{
+	int err;
+	me_query_version_main_driver_t karg;
+
+	PDEBUG("executed.\n");
+
+	karg.version = ME_VERSION_DRIVER;
+	karg.errno = ME_ERRNO_SUCCESS;
+
+	err = copy_to_user(arg, &karg, sizeof(me_query_version_main_driver_t));
+
+	if (err) {
+		PERROR("Can't copy query back to user space.\n");
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int me_config_load_device(struct file *filep,
+				 me_cfg_device_entry_t * karg, int device_no)
+{
+
+	int err = ME_ERRNO_SUCCESS;
+	int k = 0;
+
+	struct list_head *pos = NULL;
+	me_device_t *device = NULL;
+
+	PDEBUG("executed.\n");
+
+	list_for_each(pos, &me_device_list) {
+		if (k == device_no) {
+			device = list_entry(pos, me_device_t, list);
+			break;
+		}
+
+		k++;
+	}
+
+	if (pos == &me_device_list) {
+		PERROR("Invalid device number specified.\n");
+		return ME_ERRNO_INVALID_DEVICE;
+	} else {
+		spin_lock(&me_lock);
+
+		if ((me_filep != NULL) && (me_filep != filep)) {
+			spin_unlock(&me_lock);
+			PERROR("Resource is locked by another process.\n");
+			return ME_ERRNO_LOCKED;
+		} else {
+			me_count++;
+			spin_unlock(&me_lock);
+
+			err =
+			    device->me_device_config_load(device, filep, karg);
+
+			spin_lock(&me_lock);
+			me_count--;
+			spin_unlock(&me_lock);
+		}
+	}
+
+	return err;
+}
+
+static int me_config_load(struct file *filep, me_config_load_t * arg)
+{
+	int err;
+	int i;
+	me_config_load_t cfg_setup;
+	me_config_load_t karg_cfg_setup;
+
+	struct list_head *pos = NULL;
+
+	struct list_head new_list;
+	me_device_t *o_device;
+	me_device_t *n_device;
+	int o_vendor_id;
+	int o_device_id;
+	int o_serial_no;
+	int o_bus_type;
+	int o_bus_no;
+	int o_dev_no;
+	int o_func_no;
+	int o_plugged;
+
+	PDEBUG("executed.\n");
+
+	// Copy argument to kernel space.
+	err = copy_from_user(&karg_cfg_setup, arg, sizeof(me_config_load_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to kernel space.\n");
+		return -EFAULT;
+	}
+	// Allocate kernel buffer for device list.
+	cfg_setup.device_list =
+	    kmalloc(sizeof(me_cfg_device_entry_t) * karg_cfg_setup.count,
+		    GFP_KERNEL);
+
+	if (!cfg_setup.device_list) {
+		PERROR("Can't get buffer %li for device list.\n",
+		       sizeof(me_cfg_device_entry_t) * karg_cfg_setup.count);
+		return -ENOMEM;
+	}
+	// Copy device list to kernel space.
+	err =
+	    copy_from_user(cfg_setup.device_list, karg_cfg_setup.device_list,
+			   sizeof(me_cfg_device_entry_t) *
+			   karg_cfg_setup.count);
+
+	if (err) {
+		PERROR("Can't copy device list to kernel space.\n");
+		kfree(cfg_setup.device_list);
+		return -EFAULT;
+	}
+
+	cfg_setup.count = karg_cfg_setup.count;
+
+	INIT_LIST_HEAD(&new_list);
+
+	down_write(&me_rwsem);
+
+	spin_lock(&me_lock);
+
+	if ((me_filep != NULL) && (me_filep != filep)) {
+		spin_unlock(&me_lock);
+		PERROR("Driver System is logged by another process.\n");
+		karg_cfg_setup.errno = ME_ERRNO_LOCKED;
+	} else {
+		me_count++;
+		spin_unlock(&me_lock);
+
+		for (i = 0; i < karg_cfg_setup.count; i++) {
+			PDEBUG("me_config_load() device=%d.\n", i);
+			if (cfg_setup.device_list[i].tcpip.access_type ==
+			    ME_ACCESS_TYPE_LOCAL) {
+				list_for_each(pos, &me_device_list) {
+					o_device =
+					    list_entry(pos, me_device_t, list);
+					o_device->
+					    me_device_query_info_device
+					    (o_device, &o_vendor_id,
+					     &o_device_id, &o_serial_no,
+					     &o_bus_type, &o_bus_no, &o_dev_no,
+					     &o_func_no, &o_plugged);
+
+					if (cfg_setup.device_list[i].info.
+					    hw_location.bus_type ==
+					    ME_BUS_TYPE_PCI) {
+						if (((o_vendor_id ==
+						      cfg_setup.device_list[i].
+						      info.vendor_id)
+						     && (o_device_id ==
+							 cfg_setup.
+							 device_list[i].info.
+							 device_id)
+						     && (o_serial_no ==
+							 cfg_setup.
+							 device_list[i].info.
+							 serial_no)
+						     && (o_bus_type ==
+							 cfg_setup.
+							 device_list[i].info.
+							 hw_location.bus_type))
+						    ||
+						    ((o_vendor_id ==
+						      cfg_setup.device_list[i].
+						      info.vendor_id)
+						     && (o_device_id ==
+							 cfg_setup.
+							 device_list[i].info.
+							 device_id)
+						     && (o_bus_type ==
+							 cfg_setup.
+							 device_list[i].info.
+							 hw_location.bus_type)
+						     && (o_bus_no ==
+							 cfg_setup.
+							 device_list[i].info.
+							 hw_location.pci.bus_no)
+						     && (o_dev_no ==
+							 cfg_setup.
+							 device_list[i].info.
+							 hw_location.pci.
+							 device_no)
+						     && (o_func_no ==
+							 cfg_setup.
+							 device_list[i].info.
+							 hw_location.pci.
+							 function_no))) {
+							list_move_tail(pos,
+								       &new_list);
+							break;
+						}
+					}
+/*
+					else if (cfg_setup.device_list[i].info.hw_location.bus_type == ME_BUS_TYPE_USB)
+					{
+						if (((o_vendor_id == cfg_setup.device_list[i].info.vendor_id) &&
+						        (o_device_id == cfg_setup.device_list[i].info.device_id) &&
+						        (o_serial_no == cfg_setup.device_list[i].info.serial_no) &&
+						        (o_bus_type == cfg_setup.device_list[i].info.hw_location.bus_type)) ||
+						        ((o_vendor_id == cfg_setup.device_list[i].info.vendor_id) &&
+						         (o_device_id == cfg_setup.device_list[i].info.device_id) &&
+						         (o_bus_type == cfg_setup.device_list[i].info.hw_location.bus_type) &&
+						         (o_bus_no == cfg_setup.device_list[i].info.hw_location.usb.root_hub_no)))
+						{
+							list_move_tail(pos, &new_list);
+							break;
+						}
+					}
+*/
+					else {
+						PERROR("Wrong bus type: %d.\n",
+						       cfg_setup.device_list[i].
+						       info.hw_location.
+						       bus_type);
+					}
+				}
+
+				if (pos == &me_device_list) {	// Device is not already in the list
+					if (cfg_setup.device_list[i].info.
+					    hw_location.bus_type ==
+					    ME_BUS_TYPE_PCI) {
+						n_device =
+						    get_dummy_instance
+						    (cfg_setup.device_list[i].
+						     info.vendor_id,
+						     cfg_setup.device_list[i].
+						     info.device_id,
+						     cfg_setup.device_list[i].
+						     info.serial_no,
+						     cfg_setup.device_list[i].
+						     info.hw_location.bus_type,
+						     cfg_setup.device_list[i].
+						     info.hw_location.pci.
+						     bus_no,
+						     cfg_setup.device_list[i].
+						     info.hw_location.pci.
+						     device_no,
+						     cfg_setup.device_list[i].
+						     info.hw_location.pci.
+						     function_no);
+
+						if (!n_device) {
+							PERROR
+							    ("Can't get dummy instance.\n");
+							kfree(cfg_setup.
+							      device_list);
+							spin_lock(&me_lock);
+							me_count--;
+							spin_unlock(&me_lock);
+							up_write(&me_rwsem);
+							return -EFAULT;
+						}
+
+						list_add_tail(&n_device->list,
+							      &new_list);
+					}
+/*
+					else if (cfg_setup.device_list[i].info.hw_location.bus_type == ME_BUS_TYPE_USB)
+					{
+						n_device = get_dummy_instance(
+						               cfg_setup.device_list[i].info.vendor_id,
+						               cfg_setup.device_list[i].info.device_id,
+						               cfg_setup.device_list[i].info.serial_no,
+						               cfg_setup.device_list[i].info.hw_location.bus_type,
+						               cfg_setup.device_list[i].info.hw_location.usb.root_hub_no,
+						               0,
+						               0);
+
+						if (!n_device)
+						{
+							PERROR("Can't get dummy instance.\n");
+							kfree(cfg_setup.device_list);
+							spin_lock(&me_lock);
+							me_count--;
+							spin_unlock(&me_lock);
+							up_write(&me_rwsem);
+							return -EFAULT;
+						}
+
+						list_add_tail(&n_device->list, &new_list);
+					}
+*/
+				}
+			} else {
+				n_device = get_dummy_instance(0,
+							      0, 0, 0, 0, 0, 0);
+
+				if (!n_device) {
+					PERROR("Can't get dummy instance.\n");
+					kfree(cfg_setup.device_list);
+					spin_lock(&me_lock);
+					me_count--;
+					spin_unlock(&me_lock);
+					up_write(&me_rwsem);
+					return -EFAULT;
+				}
+
+				list_add_tail(&n_device->list, &new_list);
+			}
+		}
+
+		while (!list_empty(&me_device_list)) {
+			o_device =
+			    list_entry(me_device_list.next, me_device_t, list);
+			o_device->me_device_query_info_device(o_device,
+							      &o_vendor_id,
+							      &o_device_id,
+							      &o_serial_no,
+							      &o_bus_type,
+							      &o_bus_no,
+							      &o_dev_no,
+							      &o_func_no,
+							      &o_plugged);
+
+			if (o_plugged == ME_PLUGGED_IN) {
+				list_move_tail(me_device_list.next, &new_list);
+			} else {
+				list_del(me_device_list.next);
+				release_instance(o_device);
+			}
+		}
+
+		// Move temporary new list to global driver list.
+		list_splice(&new_list, &me_device_list);
+
+		karg_cfg_setup.errno = ME_ERRNO_SUCCESS;
+	}
+
+	for (i = 0; i < cfg_setup.count; i++) {
+
+		karg_cfg_setup.errno =
+		    me_config_load_device(filep, &cfg_setup.device_list[i], i);
+		if (karg_cfg_setup.errno) {
+			PERROR("me_config_load_device(%d)=%d\n", i,
+			       karg_cfg_setup.errno);
+			break;
+		}
+	}
+
+	spin_lock(&me_lock);
+
+	me_count--;
+	spin_unlock(&me_lock);
+	up_write(&me_rwsem);
+
+	err = copy_to_user(arg, &karg_cfg_setup, sizeof(me_config_load_t));
+
+	if (err) {
+		PERROR("Can't copy config list to user space.\n");
+		kfree(cfg_setup.device_list);
+		return -EFAULT;
+	}
+
+	kfree(cfg_setup.device_list);
+	return 0;
+}
+
+static int me_io_stream_start(struct file *filep, me_io_stream_start_t * arg)
+{
+	int err;
+	int i, k;
+
+	struct list_head *pos;
+	me_device_t *device;
+	me_io_stream_start_t karg;
+	meIOStreamStart_t *list;
+
+	PDEBUG("executed.\n");
+
+	err = copy_from_user(&karg, arg, sizeof(me_io_stream_start_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to kernel space.\n");
+		return -EFAULT;
+	}
+
+	karg.errno = ME_ERRNO_SUCCESS;
+
+	list = kmalloc(sizeof(meIOStreamStart_t) * karg.count, GFP_KERNEL);
+
+	if (!list) {
+		PERROR("Can't get buffer for start list.\n");
+		return -ENOMEM;
+	}
+
+	err =
+	    copy_from_user(list, karg.start_list,
+			   sizeof(meIOStreamStart_t) * karg.count);
+
+	if (err) {
+		PERROR("Can't copy start list to kernel space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	spin_lock(&me_lock);
+
+	if ((me_filep != NULL) && (me_filep != filep)) {
+		spin_unlock(&me_lock);
+		PERROR("Driver System is logged by another process.\n");
+
+		for (i = 0; i < karg.count; i++) {
+			list[i].iErrno = ME_ERRNO_LOCKED;
+		}
+	} else {
+		me_count++;
+		spin_unlock(&me_lock);
+
+		for (i = 0; i < karg.count; i++) {
+			down_read(&me_rwsem);
+			k = 0;
+			list_for_each(pos, &me_device_list) {
+				if (k == list[i].iDevice) {
+					device =
+					    list_entry(pos, me_device_t, list);
+					break;
+				}
+
+				k++;
+			}
+
+			if (pos == &me_device_list) {
+				up_read(&me_rwsem);
+				PERROR("Invalid device number specified.\n");
+				list[i].iErrno = ME_ERRNO_INVALID_DEVICE;
+				karg.errno = ME_ERRNO_INVALID_DEVICE;
+				break;
+			} else {
+				list[i].iErrno =
+				    device->me_device_io_stream_start(device,
+								      filep,
+								      list[i].
+								      iSubdevice,
+								      list[i].
+								      iStartMode,
+								      list[i].
+								      iTimeOut,
+								      list[i].
+								      iFlags);
+
+				if (list[i].iErrno) {
+					up_read(&me_rwsem);
+					karg.errno = list[i].iErrno;
+					break;
+				}
+			}
+
+			up_read(&me_rwsem);
+		}
+
+		spin_lock(&me_lock);
+
+		me_count--;
+		spin_unlock(&me_lock);
+	}
+
+	err = copy_to_user(arg, &karg, sizeof(me_io_stream_start_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to user space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	err =
+	    copy_to_user(karg.start_list, list,
+			 sizeof(meIOStreamStart_t) * karg.count);
+
+	if (err) {
+		PERROR("Can't copy start list to user space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	kfree(list);
+
+	return err;
+}
+
+static int me_io_single(struct file *filep, me_io_single_t * arg)
+{
+	int err;
+	int i, k;
+
+	struct list_head *pos;
+	me_device_t *device;
+	me_io_single_t karg;
+	meIOSingle_t *list;
+
+	PDEBUG("executed.\n");
+
+	err = copy_from_user(&karg, arg, sizeof(me_io_single_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to kernel space.\n");
+		return -EFAULT;
+	}
+
+	karg.errno = ME_ERRNO_SUCCESS;
+
+	list = kmalloc(sizeof(meIOSingle_t) * karg.count, GFP_KERNEL);
+
+	if (!list) {
+		PERROR("Can't get buffer for single list.\n");
+		return -ENOMEM;
+	}
+
+	err =
+	    copy_from_user(list, karg.single_list,
+			   sizeof(meIOSingle_t) * karg.count);
+
+	if (err) {
+		PERROR("Can't copy single list to kernel space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	spin_lock(&me_lock);
+
+	if ((me_filep != NULL) && (me_filep != filep)) {
+		spin_unlock(&me_lock);
+		PERROR("Driver System is logged by another process.\n");
+
+		for (i = 0; i < karg.count; i++) {
+			list[i].iErrno = ME_ERRNO_LOCKED;
+		}
+	} else {
+		me_count++;
+		spin_unlock(&me_lock);
+
+		for (i = 0; i < karg.count; i++) {
+			k = 0;
+
+			down_read(&me_rwsem);
+
+			list_for_each(pos, &me_device_list) {
+				if (k == list[i].iDevice) {
+					device =
+					    list_entry(pos, me_device_t, list);
+					break;
+				}
+
+				k++;
+			}
+
+			if (pos == &me_device_list) {
+				up_read(&me_rwsem);
+				PERROR("Invalid device number specified.\n");
+				list[i].iErrno = ME_ERRNO_INVALID_DEVICE;
+				karg.errno = ME_ERRNO_INVALID_DEVICE;
+				break;
+			} else {
+				if (list[i].iDir == ME_DIR_OUTPUT) {
+					list[i].iErrno =
+					    device->
+					    me_device_io_single_write(device,
+								      filep,
+								      list[i].
+								      iSubdevice,
+								      list[i].
+								      iChannel,
+								      list[i].
+								      iValue,
+								      list[i].
+								      iTimeOut,
+								      list[i].
+								      iFlags);
+
+					if (list[i].iErrno) {
+						up_read(&me_rwsem);
+						karg.errno = list[i].iErrno;
+						break;
+					}
+				} else if (list[i].iDir == ME_DIR_INPUT) {
+					list[i].iErrno =
+					    device->
+					    me_device_io_single_read(device,
+								     filep,
+								     list[i].
+								     iSubdevice,
+								     list[i].
+								     iChannel,
+								     &list[i].
+								     iValue,
+								     list[i].
+								     iTimeOut,
+								     list[i].
+								     iFlags);
+
+					if (list[i].iErrno) {
+						up_read(&me_rwsem);
+						karg.errno = list[i].iErrno;
+						break;
+					}
+				} else {
+					up_read(&me_rwsem);
+					PERROR
+					    ("Invalid single direction specified.\n");
+					list[i].iErrno = ME_ERRNO_INVALID_DIR;
+					karg.errno = ME_ERRNO_INVALID_DIR;
+					break;
+				}
+			}
+
+			up_read(&me_rwsem);
+		}
+
+		spin_lock(&me_lock);
+
+		me_count--;
+		spin_unlock(&me_lock);
+	}
+
+	err = copy_to_user(arg, &karg, sizeof(me_io_single_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to user space.\n");
+		return -EFAULT;
+	}
+
+	err =
+	    copy_to_user(karg.single_list, list,
+			 sizeof(meIOSingle_t) * karg.count);
+
+	if (err) {
+		PERROR("Can't copy single list to user space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	kfree(list);
+
+	return err;
+}
+
+static int me_io_stream_config(struct file *filep, me_io_stream_config_t * arg)
+{
+	int err;
+	int k = 0;
+
+	struct list_head *pos;
+	me_device_t *device;
+	me_io_stream_config_t karg;
+	meIOStreamConfig_t *list;
+
+	PDEBUG("executed.\n");
+
+	err = copy_from_user(&karg, arg, sizeof(me_io_stream_config_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to kernel space.\n");
+		return -EFAULT;
+	}
+
+	list = kmalloc(sizeof(meIOStreamConfig_t) * karg.count, GFP_KERNEL);
+
+	if (!list) {
+		PERROR("Can't get buffer for config list.\n");
+		return -ENOMEM;
+	}
+
+	err =
+	    copy_from_user(list, karg.config_list,
+			   sizeof(meIOStreamConfig_t) * karg.count);
+
+	if (err) {
+		PERROR("Can't copy config list to kernel space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	spin_lock(&me_lock);
+
+	if ((me_filep != NULL) && (me_filep != filep)) {
+		spin_unlock(&me_lock);
+		PERROR("Driver System is logged by another process.\n");
+		karg.errno = ME_ERRNO_LOCKED;
+	} else {
+		me_count++;
+		spin_unlock(&me_lock);
+
+		down_read(&me_rwsem);
+
+		list_for_each(pos, &me_device_list) {
+			if (k == karg.device) {
+				device = list_entry(pos, me_device_t, list);
+				break;
+			}
+
+			k++;
+		}
+
+		if (pos == &me_device_list) {
+			PERROR("Invalid device number specified.\n");
+			karg.errno = ME_ERRNO_INVALID_DEVICE;
+		} else {
+			karg.errno =
+			    device->me_device_io_stream_config(device, filep,
+							       karg.subdevice,
+							       list, karg.count,
+							       &karg.trigger,
+							       karg.
+							       fifo_irq_threshold,
+							       karg.flags);
+		}
+
+		up_read(&me_rwsem);
+
+		spin_lock(&me_lock);
+		me_count--;
+		spin_unlock(&me_lock);
+	}
+
+	err = copy_to_user(arg, &karg, sizeof(me_io_stream_config_t));
+
+	if (err) {
+		PERROR("Can't copy back to user space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	kfree(list);
+
+	return err;
+}
+
+static int me_query_number_devices(struct file *filep,
+				   me_query_number_devices_t * arg)
+{
+	int err;
+	me_query_number_devices_t karg;
+
+	struct list_head *pos;
+
+	PDEBUG("executed.\n");
+
+	karg.number = 0;
+	down_read(&me_rwsem);
+	list_for_each(pos, &me_device_list) {
+		karg.number++;
+	}
+
+	up_read(&me_rwsem);
+
+	karg.errno = ME_ERRNO_SUCCESS;
+
+	err = copy_to_user(arg, &karg, sizeof(me_query_number_devices_t));
+
+	if (err) {
+		PERROR("Can't copy query back to user space.\n");
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int me_io_stream_stop(struct file *filep, me_io_stream_stop_t * arg)
+{
+	int err;
+	int i, k;
+
+	struct list_head *pos;
+	me_device_t *device;
+	me_io_stream_stop_t karg;
+	meIOStreamStop_t *list;
+
+	PDEBUG("executed.\n");
+
+	err = copy_from_user(&karg, arg, sizeof(me_io_stream_stop_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to kernel space.\n");
+		return -EFAULT;
+	}
+
+	karg.errno = ME_ERRNO_SUCCESS;
+
+	list = kmalloc(sizeof(meIOStreamStop_t) * karg.count, GFP_KERNEL);
+
+	if (!list) {
+		PERROR("Can't get buffer for stop list.\n");
+		return -ENOMEM;
+	}
+
+	err =
+	    copy_from_user(list, karg.stop_list,
+			   sizeof(meIOStreamStop_t) * karg.count);
+
+	if (err) {
+		PERROR("Can't copy stop list to kernel space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	spin_lock(&me_lock);
+
+	if ((me_filep != NULL) && (me_filep != filep)) {
+		spin_unlock(&me_lock);
+		PERROR("Driver System is logged by another process.\n");
+
+		for (i = 0; i < karg.count; i++) {
+			list[i].iErrno = ME_ERRNO_LOCKED;
+		}
+	} else {
+		me_count++;
+		spin_unlock(&me_lock);
+
+		for (i = 0; i < karg.count; i++) {
+			k = 0;
+			down_read(&me_rwsem);
+			list_for_each(pos, &me_device_list) {
+				if (k == list[i].iDevice) {
+					device =
+					    list_entry(pos, me_device_t, list);
+					break;
+				}
+
+				k++;
+			}
+
+			if (pos == &me_device_list) {
+				up_read(&me_rwsem);
+				PERROR("Invalid device number specified.\n");
+				list[i].iErrno = ME_ERRNO_INVALID_DEVICE;
+				karg.errno = ME_ERRNO_INVALID_DEVICE;
+				break;
+			} else {
+				list[i].iErrno =
+				    device->me_device_io_stream_stop(device,
+								     filep,
+								     list[i].
+								     iSubdevice,
+								     list[i].
+								     iStopMode,
+								     list[i].
+								     iFlags);
+
+				if (list[i].iErrno) {
+					up_read(&me_rwsem);
+					karg.errno = list[i].iErrno;
+					break;
+				}
+			}
+
+			up_read(&me_rwsem);
+		}
+
+		spin_lock(&me_lock);
+
+		me_count--;
+		spin_unlock(&me_lock);
+	}
+
+	err = copy_to_user(arg, &karg, sizeof(me_io_stream_stop_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to user space.\n");
+		return -EFAULT;
+	}
+
+	err =
+	    copy_to_user(karg.stop_list, list,
+			 sizeof(meIOStreamStop_t) * karg.count);
+
+	if (err) {
+		PERROR("Can't copy stop list to user space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	kfree(list);
+
+	return err;
+}
+
+/*  //me_probe_usb
+static int me_probe_usb(struct usb_interface *interface, const struct usb_device_id *id)
+{
+	//int err;
+	//me_usb_constructor_t *constructor = NULL;
+	me_device_t *n_device = NULL;
+
+	PDEBUG("executed.\n");
+
+	switch (id->idProduct)
+	{
+			case USB_DEVICE_ID_MEPHISTO_S1:
+				if((constructor = symbol_get(mephisto_s1_constructor)) == NULL){
+					err = request_module(MEPHISTO_S1_NAME);
+					if(err){
+						PERROR("Error while request for module %s.\n", MEPHISTO_S1_NAME);
+						return -ENODEV;
+					}
+					if((constructor = symbol_get(mephisto_s1_constructor)) == NULL){
+						PERROR("Can't get %s driver module constructor.\n", MEPHISTO_S1_NAME);
+						return -ENODEV;
+					}
+				}
+
+				if((n_device = (*constructor)(interface)) == NULL){
+					symbol_put(mephisto_s1_constructor);
+					PERROR("Can't get device instance of %s driver module.\n", MEPHISTO_S1_NAME);
+					return -ENODEV;
+				}
+
+				break;
+
+		default:
+			PERROR("Invalid product id.\n");
+
+			return -EINVAL;
+	}
+
+	return insert_to_device_list(n_device);
+}
+*/
+
+/*  //me_disconnect_usb
+static void me_disconnect_usb(struct usb_interface *interface)
+{
+
+	struct usb_device *device = interface_to_usbdev(interface);
+	int vendor_id = device->descriptor.idVendor;
+	int device_id = device->descriptor.idProduct;
+	int serial_no;
+
+	sscanf(&device->serial[2], "%x", &serial_no);
+
+	PDEBUG("executed.\n");
+
+	PINFO("Vendor id = 0x%08X\n", vendor_id);
+	PINFO("Device id = 0x%08X\n", device_id);
+	PINFO("Serial Number = 0x%08X\n", serial_no);
+
+	replace_with_dummy(vendor_id, device_id, serial_no);
+}
+*/
+
+static int me_ioctl(struct inode *inodep,
+		    struct file *filep, unsigned int service, unsigned long arg)
+{
+
+	PDEBUG("executed.\n");
+
+	if (_IOC_TYPE(service) != MEMAIN_MAGIC) {
+		PERROR("Invalid magic number.\n");
+		return -ENOTTY;
+	}
+
+	PDEBUG("service number: 0x%x.\n", service);
+
+	switch (service) {
+	case ME_IO_IRQ_ENABLE:
+		return me_io_irq_start(filep, (me_io_irq_start_t *) arg);
+
+	case ME_IO_IRQ_WAIT:
+		return me_io_irq_wait(filep, (me_io_irq_wait_t *) arg);
+
+	case ME_IO_IRQ_DISABLE:
+		return me_io_irq_stop(filep, (me_io_irq_stop_t *) arg);
+
+	case ME_IO_RESET_DEVICE:
+		return me_io_reset_device(filep, (me_io_reset_device_t *) arg);
+
+	case ME_IO_RESET_SUBDEVICE:
+		return me_io_reset_subdevice(filep,
+					     (me_io_reset_subdevice_t *) arg);
+
+	case ME_IO_SINGLE_CONFIG:
+		return me_io_single_config(filep,
+					   (me_io_single_config_t *) arg);
+
+	case ME_IO_SINGLE:
+		return me_io_single(filep, (me_io_single_t *) arg);
+
+	case ME_IO_STREAM_CONFIG:
+		return me_io_stream_config(filep,
+					   (me_io_stream_config_t *) arg);
+
+	case ME_IO_STREAM_NEW_VALUES:
+		return me_io_stream_new_values(filep,
+					       (me_io_stream_new_values_t *)
+					       arg);
+
+	case ME_IO_STREAM_READ:
+		return me_io_stream_read(filep, (me_io_stream_read_t *) arg);
+
+	case ME_IO_STREAM_START:
+		return me_io_stream_start(filep, (me_io_stream_start_t *) arg);
+
+	case ME_IO_STREAM_STATUS:
+		return me_io_stream_status(filep,
+					   (me_io_stream_status_t *) arg);
+
+	case ME_IO_STREAM_STOP:
+		return me_io_stream_stop(filep, (me_io_stream_stop_t *) arg);
+
+	case ME_IO_STREAM_WRITE:
+		return me_io_stream_write(filep, (me_io_stream_write_t *) arg);
+
+	case ME_LOCK_DRIVER:
+		return me_lock_driver(filep, (me_lock_driver_t *) arg);
+
+	case ME_LOCK_DEVICE:
+		return me_lock_device(filep, (me_lock_device_t *) arg);
+
+	case ME_LOCK_SUBDEVICE:
+		return me_lock_subdevice(filep, (me_lock_subdevice_t *) arg);
+
+	case ME_QUERY_INFO_DEVICE:
+		return me_query_info_device(filep,
+					    (me_query_info_device_t *) arg);
+
+	case ME_QUERY_DESCRIPTION_DEVICE:
+		return me_query_description_device(filep,
+						   (me_query_description_device_t
+						    *) arg);
+
+	case ME_QUERY_NAME_DEVICE:
+		return me_query_name_device(filep,
+					    (me_query_name_device_t *) arg);
+
+	case ME_QUERY_NAME_DEVICE_DRIVER:
+		return me_query_name_device_driver(filep,
+						   (me_query_name_device_driver_t
+						    *) arg);
+
+	case ME_QUERY_NUMBER_DEVICES:
+		return me_query_number_devices(filep,
+					       (me_query_number_devices_t *)
+					       arg);
+
+	case ME_QUERY_NUMBER_SUBDEVICES:
+		return me_query_number_subdevices(filep,
+						  (me_query_number_subdevices_t
+						   *) arg);
+
+	case ME_QUERY_NUMBER_CHANNELS:
+		return me_query_number_channels(filep,
+						(me_query_number_channels_t *)
+						arg);
+
+	case ME_QUERY_NUMBER_RANGES:
+		return me_query_number_ranges(filep,
+					      (me_query_number_ranges_t *) arg);
+
+	case ME_QUERY_RANGE_BY_MIN_MAX:
+		return me_query_range_by_min_max(filep,
+						 (me_query_range_by_min_max_t *)
+						 arg);
+
+	case ME_QUERY_RANGE_INFO:
+		return me_query_range_info(filep,
+					   (me_query_range_info_t *) arg);
+
+	case ME_QUERY_SUBDEVICE_BY_TYPE:
+		return me_query_subdevice_by_type(filep,
+						  (me_query_subdevice_by_type_t
+						   *) arg);
+
+	case ME_QUERY_SUBDEVICE_TYPE:
+		return me_query_subdevice_type(filep,
+					       (me_query_subdevice_type_t *)
+					       arg);
+
+	case ME_QUERY_SUBDEVICE_CAPS:
+		return me_query_subdevice_caps(filep,
+					       (me_query_subdevice_caps_t *)
+					       arg);
+
+	case ME_QUERY_SUBDEVICE_CAPS_ARGS:
+		return me_query_subdevice_caps_args(filep,
+						    (me_query_subdevice_caps_args_t
+						     *) arg);
+
+	case ME_QUERY_TIMER:
+		return me_query_timer(filep, (me_query_timer_t *) arg);
+
+	case ME_QUERY_VERSION_MAIN_DRIVER:
+		return me_query_version_main_driver(filep,
+						    (me_query_version_main_driver_t
+						     *) arg);
+
+	case ME_QUERY_VERSION_DEVICE_DRIVER:
+		return me_query_version_device_driver(filep,
+						      (me_query_version_device_driver_t
+						       *) arg);
+
+	case ME_CONFIG_LOAD:
+		return me_config_load(filep, (me_config_load_t *) arg);
+	}
+
+	PERROR("Invalid ioctl number.\n");
+	return -ENOTTY;
+}
+
+// Init and exit of module.
+static int memain_init(void)
+{
+	int result = 0;
+	dev_t dev = MKDEV(major, 0);
+
+	PDEBUG("executed.\n");
+
+	// Register pci driver. This will return 0 if the PCI subsystem is not available.
+	result = pci_register_driver(&me_pci_driver);
+
+	if (result < 0) {
+		PERROR("Can't register pci driver.\n");
+		goto INIT_ERROR_1;
+	}
+
+/*
+	// Register usb driver. This will return -ENODEV if no USB subsystem is available.
+	result = usb_register(&me_usb_driver);
+
+	if (result)
+	{
+		if (result == -ENODEV)
+		{
+			PERROR("No USB subsystem available.\n");
+		}
+		else
+		{
+			PERROR("Can't register usb driver.\n");
+			goto INIT_ERROR_2;
+		}
+	}
+*/
+	// Register the character device.
+	if (major) {
+		result = register_chrdev_region(dev, 1, MEMAIN_NAME);
+	} else {
+		result = alloc_chrdev_region(&dev, 0, 1, MEMAIN_NAME);
+		major = MAJOR(dev);
+	}
+
+	if (result < 0) {
+		PERROR("Can't get major driver no.\n");
+		goto INIT_ERROR_3;
+	}
+
+	cdevp = cdev_alloc();
+
+	if (!cdevp) {
+		PERROR("Can't get character device structure.\n");
+		result = -ENOMEM;
+		goto INIT_ERROR_4;
+	}
+
+	cdevp->ops = &me_file_operations;
+
+	cdevp->owner = THIS_MODULE;
+
+	result = cdev_add(cdevp, dev, 1);
+
+	if (result < 0) {
+		PERROR("Cannot add character device structure.\n");
+		goto INIT_ERROR_5;
+	}
+
+	return 0;
+
+      INIT_ERROR_5:
+	cdev_del(cdevp);
+
+      INIT_ERROR_4:
+	unregister_chrdev_region(dev, 1);
+
+      INIT_ERROR_3:
+//      usb_deregister(&me_usb_driver);
+
+//INIT_ERROR_2:
+	pci_unregister_driver(&me_pci_driver);
+	clear_device_list();
+
+      INIT_ERROR_1:
+	return result;
+}
+
+static void __exit memain_exit(void)
+{
+	dev_t dev = MKDEV(major, 0);
+
+	PDEBUG("executed.\n");
+
+	cdev_del(cdevp);
+	unregister_chrdev_region(dev, 1);
+	pci_unregister_driver(&me_pci_driver);
+//      usb_deregister(&me_usb_driver);
+	clear_device_list();
+}
+
+module_init(memain_init);
+module_exit(memain_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Central module for Meilhaus Driver System.");
+MODULE_SUPPORTED_DEVICE("Meilhaus PCI/cPCI boards.");
+MODULE_LICENSE("GPL");
+
+#ifdef BOSCH
+// Export the flag for the BOSCH firmware.
+EXPORT_SYMBOL(me_bosch_fw);
+#endif // BOSCH
diff --git a/drivers/staging/meilhaus/memain.h b/drivers/staging/meilhaus/memain.h
new file mode 100644
index 000000000000..7616ff7f65cb
--- /dev/null
+++ b/drivers/staging/meilhaus/memain.h
@@ -0,0 +1,460 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : memain.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ */
+
+#ifndef _MEMAIN_H_
+#define _MEMAIN_H_
+
+#include "meinternal.h"
+
+#include "meids.h"
+#include "medebug.h"
+
+#include "medevice.h"
+/*#include "me1000_device.h"
+#include "me1400_device.h"
+#include "me1600_device.h"*/
+#include "me4600_device.h"
+/*#include "me6000_device.h"
+#include "me0600_device.h"
+#include "me8100_device.h"
+#include "me8200_device.h"
+#include "me0900_device.h"*/
+#include "medummy.h"
+
+#ifdef __KERNEL__
+
+/*=============================================================================
+  PCI device table.
+  This is used by modprobe to translate PCI IDs to drivers.
+  ===========================================================================*/
+
+static struct pci_device_id me_pci_table[] __devinitdata = {
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1000, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1000_A, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1000_B, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1400, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME140A, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME140B, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME14E0, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME14EA, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME14EB, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME140C, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME140D, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1600_4U, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1600_8U, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1600_12U, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1600_16U, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1600_16U_8I,
+	 PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4610, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4650, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4660, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4660I, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670I, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670S, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670IS, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680I, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680S, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680IS, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6004, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6008, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME600F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6014, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6018, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME601F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6034, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6038, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME603F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6104, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6108, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME610F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6114, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6118, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME611F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6134, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6138, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME613F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6044, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6048, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME604F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6054, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6058, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME605F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6074, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6078, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME607F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6144, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6148, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME614F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6154, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6158, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME615F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6174, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6178, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME617F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6259, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6359, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME0630, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME8100_A, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME8100_B, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME8200_A, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME8200_B, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME0940, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME0950, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME0960, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, me_pci_table);
+
+/*=============================================================================
+  USB device table.
+  This is used by modprobe to translate USB IDs to drivers.
+  ===========================================================================*/
+/*
+static struct usb_device_id me_usb_table[] __devinitdata = {
+	{ USB_DEVICE(USB_VENDOR_ID_MEPHISTO_S1, USB_DEVICE_ID_MEPHISTO_S1) },
+	{ 0 }
+};
+
+MODULE_DEVICE_TABLE (usb, me_usb_table);
+*/
+
+/*=============================================================================
+  Templates
+  ===========================================================================*/
+
+#define ME_LOCK_MULTIPLEX_TEMPLATE(NAME, TYPE, CALL, DEV_CALL, ARGS)	\
+static int CALL(struct file *filep, TYPE *arg){	\
+	int err = 0; \
+	int k = 0; \
+	struct list_head *pos; \
+	me_device_t *device; \
+	TYPE karg; \
+	\
+	PDEBUG("executed.\n"); \
+	\
+	err = copy_from_user(&karg, arg, sizeof(TYPE)); \
+	if(err){ \
+		PERROR("Can't copy arguments to kernel space\n"); \
+		return -EFAULT; \
+	} \
+	\
+	down_read(&me_rwsem);	\
+	\
+	list_for_each(pos, &me_device_list){	\
+		if(k == karg.device){	\
+			device = list_entry(pos, me_device_t, list);	\
+				break;	\
+		}	\
+		k++;	\
+	}	\
+	\
+	if(pos == &me_device_list){ \
+		PERROR("Invalid device number specified\n"); \
+		karg.errno = ME_ERRNO_INVALID_DEVICE; \
+	} \
+	else{ \
+		spin_lock(&me_lock);	\
+		if((me_filep != NULL) && (me_filep != filep)){	\
+			spin_unlock(&me_lock);	\
+			PERROR("Resource is locked by another process\n");	\
+			if(karg.lock == ME_LOCK_SET)	\
+				karg.errno = ME_ERRNO_LOCKED;	\
+			else if(karg.lock == ME_LOCK_RELEASE)	\
+				karg.errno = ME_ERRNO_SUCCESS;	\
+			else{	\
+				PERROR("Invalid lock specified\n");	\
+				karg.errno = ME_ERRNO_INVALID_LOCK;	\
+			}\
+		}	\
+		else {	\
+			me_count++;	\
+			spin_unlock(&me_lock);	\
+			\
+			karg.errno = device->DEV_CALL ARGS;	\
+			\
+			spin_lock(&me_lock);	\
+			me_count--;	\
+			spin_unlock(&me_lock);	\
+		}	\
+	} \
+	\
+	up_read(&me_rwsem);	\
+	\
+	err = copy_to_user(arg, &karg, sizeof(TYPE)); \
+	if(err){ \
+		PERROR("Can't copy arguments back to user space\n"); \
+		return -EFAULT; \
+	} \
+	\
+	return ME_ERRNO_SUCCESS; \
+}
+
+#define ME_IO_MULTIPLEX_TEMPLATE(NAME, TYPE, CALL, DEV_CALL, ARGS)	\
+static int CALL(struct file *filep, TYPE *arg){	\
+	int err = 0; \
+	int k = 0; \
+	struct list_head *pos; \
+	me_device_t *device; \
+	TYPE karg; \
+	\
+	PDEBUG("executed.\n"); \
+	\
+	err = copy_from_user(&karg, arg, sizeof(TYPE)); \
+	if(err){ \
+		PERROR("Can't copy arguments to kernel space\n"); \
+		return -EFAULT; \
+	} \
+	\
+	down_read(&me_rwsem);	\
+	\
+	list_for_each(pos, &me_device_list){	\
+		if(k == karg.device){	\
+			device = list_entry(pos, me_device_t, list);	\
+				break;	\
+		}	\
+		k++;	\
+	}	\
+	\
+	if(pos == &me_device_list){ \
+		PERROR("Invalid device number specified\n"); \
+		karg.errno = ME_ERRNO_INVALID_DEVICE; \
+	} \
+	else{ \
+		spin_lock(&me_lock);	\
+		if((me_filep != NULL) && (me_filep != filep)){	\
+			spin_unlock(&me_lock);	\
+			PERROR("Resource is locked by another process\n");	\
+			karg.errno = ME_ERRNO_LOCKED;	\
+		}	\
+		else {	\
+			me_count++;	\
+			spin_unlock(&me_lock);	\
+			\
+			karg.errno = device->DEV_CALL ARGS;	\
+			\
+			spin_lock(&me_lock);	\
+			me_count--;	\
+			spin_unlock(&me_lock);	\
+		}	\
+	} \
+	\
+	up_read(&me_rwsem);	\
+	\
+	err = copy_to_user(arg, &karg, sizeof(TYPE)); \
+	if(err){ \
+		PERROR("Can't copy arguments back to user space\n"); \
+		return -EFAULT; \
+	} \
+ \
+	return ME_ERRNO_SUCCESS; \
+}
+
+#define ME_QUERY_MULTIPLEX_STR_TEMPLATE(NAME, TYPE, CALL, DEV_CALL, ARGS)	\
+static int CALL(struct file *filep, TYPE *arg){	\
+	int err = 0;	\
+	int k = 0;	\
+	struct list_head *pos;	\
+	me_device_t *device;	\
+	char *msg = NULL;	\
+	TYPE karg;	\
+	\
+	PDEBUG("executed.\n"); \
+	\
+	err = copy_from_user(&karg, arg, sizeof(TYPE));	\
+	if(err){	\
+		PERROR("Can't copy arguments to kernel space\n");	\
+		return -EFAULT;	\
+	}	\
+	\
+	down_read(&me_rwsem);	\
+	\
+	list_for_each(pos, &me_device_list){	\
+		if(k == karg.device){	\
+			device = list_entry(pos, me_device_t, list);	\
+				break;	\
+		}	\
+		k++;	\
+	}	\
+	\
+	if(pos == &me_device_list){	\
+		PERROR("Invalid device number specified\n");	\
+		karg.errno = ME_ERRNO_INVALID_DEVICE;	\
+	}	\
+	else{	\
+		karg.errno = device->DEV_CALL ARGS;	\
+		if(!karg.errno){	\
+			if((strlen(msg) + 1) > karg.count){	\
+				PERROR("User buffer for device name is to little\n");	\
+				karg.errno = ME_ERRNO_USER_BUFFER_SIZE;	\
+			}	\
+			else{	\
+				err = copy_to_user(karg.name, msg, strlen(msg) + 1);	\
+				if(err){	\
+					PERROR("Can't copy device name to user space\n");	\
+					return -EFAULT;	\
+				}	\
+			}	\
+		}	\
+	}	\
+	\
+	up_read(&me_rwsem);	\
+	\
+	err = copy_to_user(arg, &karg, sizeof(TYPE));	\
+	if(err){	\
+		PERROR("Can't copy query back to user space\n");	\
+		return -EFAULT;	\
+	}	\
+	\
+	return ME_ERRNO_SUCCESS;	\
+}
+
+#define ME_QUERY_MULTIPLEX_TEMPLATE(NAME, TYPE, CALL, DEV_CALL, ARGS)	\
+static int CALL(struct file *filep, TYPE *arg){	\
+	int err = 0;	\
+	int k = 0;	\
+	struct list_head *pos;	\
+	me_device_t *device;	\
+	TYPE karg;	\
+		\
+	PDEBUG("executed.\n"); \
+	 \
+	err = copy_from_user(&karg, arg, sizeof(TYPE));	\
+	if(err){	\
+		PERROR("Can't copy arguments from user space\n");	\
+		return -EFAULT;	\
+	}	\
+	\
+	down_read(&me_rwsem);	\
+	\
+	list_for_each(pos, &me_device_list){	\
+		if(k == karg.device){	\
+			device = list_entry(pos, me_device_t, list);	\
+			break;	\
+		}	\
+		k++;	\
+	}	\
+		\
+	if(pos == &me_device_list){	\
+		PERROR("Invalid device number specified\n");	\
+		karg.errno = ME_ERRNO_INVALID_DEVICE;	\
+	}	\
+	else{	\
+		karg.errno = device->DEV_CALL ARGS;	\
+	}	\
+	\
+	up_read(&me_rwsem);	\
+	\
+	err = copy_to_user(arg, &karg, sizeof(TYPE));	\
+	if(err){	\
+		PERROR("Can't copy arguments to user space\n");	\
+		return -EFAULT;	\
+	}	\
+		\
+	return ME_ERRNO_SUCCESS;	\
+}
+
+#endif //__KERNEL__
+#endif
diff --git a/drivers/staging/meilhaus/meplx_reg.h b/drivers/staging/meilhaus/meplx_reg.h
new file mode 100644
index 000000000000..1868614dc232
--- /dev/null
+++ b/drivers/staging/meilhaus/meplx_reg.h
@@ -0,0 +1,53 @@
+/**
+ * @file meplx_reg.h
+ *
+ * @brief PLX 9052 PCI bridge register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _MEPLX_REG_H_
+#define _MEPLX_REG_H_
+
+#ifdef __KERNEL__
+
+#define PLX_INTCSR				0x4C		/**< Interrupt control and status register. */
+#define PLX_INTCSR_LOCAL_INT1_EN		0x01		/**< If set, local interrupt 1 is enabled (r/w). */
+#define PLX_INTCSR_LOCAL_INT1_POL		0x02		/**< If set, local interrupt 1 polarity is active high (r/w). */
+#define PLX_INTCSR_LOCAL_INT1_STATE		0x04		/**< If set, local interrupt 1 is active (r/_). */
+#define PLX_INTCSR_LOCAL_INT2_EN		0x08		/**< If set, local interrupt 2 is enabled (r/w). */
+#define PLX_INTCSR_LOCAL_INT2_POL		0x10		/**< If set, local interrupt 2 polarity is active high (r/w). */
+#define PLX_INTCSR_LOCAL_INT2_STATE		0x20		/**< If set, local interrupt 2 is active  (r/_). */
+#define PLX_INTCSR_PCI_INT_EN			0x40		/**< If set, PCI interrupt is enabled (r/w). */
+#define PLX_INTCSR_SOFT_INT			0x80		/**< If set, a software interrupt is generated (r/w). */
+
+#define PLX_ICR					0x50		/**< Initialization control register. */
+#define PLX_ICR_BIT_EEPROM_CLOCK_SET		0x01000000
+#define PLX_ICR_BIT_EEPROM_CHIP_SELECT		0x02000000
+#define PLX_ICR_BIT_EEPROM_WRITE		0x04000000
+#define PLX_ICR_BIT_EEPROM_READ			0x08000000
+#define PLX_ICR_BIT_EEPROM_VALID		0x10000000
+
+#define PLX_ICR_MASK_EEPROM			0x1F000000
+#define EEPROM_DELAY				1
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/meslist.c b/drivers/staging/meilhaus/meslist.c
new file mode 100644
index 000000000000..7e8b66c05f7e
--- /dev/null
+++ b/drivers/staging/meilhaus/meslist.c
@@ -0,0 +1,173 @@
+/**
+ * @file me_slist.c
+ *
+ * @brief Implements the subdevice list class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "meerror.h"
+#include "medefines.h"
+
+#include "meslist.h"
+#include "medebug.h"
+
+int me_slist_query_number_subdevices(struct me_slist *slist, int *number)
+{
+	PDEBUG_LOCKS("called.\n");
+	*number = slist->n;
+	return ME_ERRNO_SUCCESS;
+}
+
+unsigned int me_slist_get_number_subdevices(struct me_slist *slist)
+{
+	PDEBUG_LOCKS("called.\n");
+	return slist->n;
+}
+
+me_subdevice_t *me_slist_get_subdevice(struct me_slist * slist,
+				       unsigned int index)
+{
+
+	struct list_head *pos;
+	me_subdevice_t *subdevice = NULL;
+	unsigned int i = 0;
+
+	PDEBUG_LOCKS("called.\n");
+
+	if (index >= slist->n) {
+		PERROR("Index out of range.\n");
+		return NULL;
+	}
+
+	list_for_each(pos, &slist->head) {
+		if (i == index) {
+			subdevice = list_entry(pos, me_subdevice_t, list);
+			break;
+		}
+
+		++i;
+	}
+
+	return subdevice;
+}
+
+int me_slist_get_subdevice_by_type(struct me_slist *slist,
+				   unsigned int start_subdevice,
+				   int type, int subtype, int *subdevice)
+{
+	me_subdevice_t *pos;
+	int s_type, s_subtype;
+	unsigned int index = 0;
+
+	PDEBUG_LOCKS("called.\n");
+
+	if (start_subdevice >= slist->n) {
+		PERROR("Start index out of range.\n");
+		return ME_ERRNO_NOMORE_SUBDEVICE_TYPE;
+	}
+
+	list_for_each_entry(pos, &slist->head, list) {
+		if (index < start_subdevice) {	// Go forward to start subdevice.
+			++index;
+			continue;
+		}
+
+		pos->me_subdevice_query_subdevice_type(pos,
+						       &s_type, &s_subtype);
+
+		if (subtype == ME_SUBTYPE_ANY) {
+			if (s_type == type)
+				break;
+		} else {
+			if ((s_type == type) && (s_subtype == subtype))
+				break;
+		}
+
+		++index;
+	}
+
+	if (index >= slist->n) {
+		return ME_ERRNO_NOMORE_SUBDEVICE_TYPE;
+	}
+
+	*subdevice = index;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+void me_slist_add_subdevice_tail(struct me_slist *slist,
+				 me_subdevice_t * subdevice)
+{
+	PDEBUG_LOCKS("called.\n");
+
+	list_add_tail(&subdevice->list, &slist->head);
+	++slist->n;
+}
+
+me_subdevice_t *me_slist_del_subdevice_tail(struct me_slist *slist)
+{
+
+	struct list_head *last;
+	me_subdevice_t *subdevice;
+
+	PDEBUG_LOCKS("called.\n");
+
+	if (list_empty(&slist->head))
+		return NULL;
+
+	last = slist->head.prev;
+
+	subdevice = list_entry(last, me_subdevice_t, list);
+
+	list_del(last);
+
+	--slist->n;
+
+	return subdevice;
+}
+
+int me_slist_init(me_slist_t * slist)
+{
+	PDEBUG_LOCKS("called.\n");
+
+	INIT_LIST_HEAD(&slist->head);
+	slist->n = 0;
+	return 0;
+}
+
+void me_slist_deinit(me_slist_t * slist)
+{
+
+	struct list_head *s;
+	me_subdevice_t *subdevice;
+
+	PDEBUG_LOCKS("called.\n");
+
+	while (!list_empty(&slist->head)) {
+		s = slist->head.next;
+		list_del(s);
+		subdevice = list_entry(s, me_subdevice_t, list);
+		subdevice->me_subdevice_destructor(subdevice);
+	}
+
+	slist->n = 0;
+}
diff --git a/drivers/staging/meilhaus/meslist.h b/drivers/staging/meilhaus/meslist.h
new file mode 100644
index 000000000000..d26c89693d2c
--- /dev/null
+++ b/drivers/staging/meilhaus/meslist.h
@@ -0,0 +1,108 @@
+/**
+ * @file me_slist.h
+ *
+ * @brief Provides the subdevice list class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME_SLIST_H_
+#define _ME_SLIST_H_
+
+#include <linux/list.h>
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The subdevice list container.
+ */
+typedef struct me_slist {
+	struct list_head head;		/**< The head of the internal list. */
+	unsigned int n;			/**< The number of subdevices in the list. */
+} me_slist_t;
+
+/**
+ * @brief Queries the number of subdevices currently inside the list.
+ *
+ * @param slist The subdevice list to query.
+ * @param[out] number The number of subdevices of the device.
+ *
+ * @return ME-iDS error code.
+ */
+int me_slist_query_number_subdevices(struct me_slist *slist, int *number);
+
+/**
+ * @brief Returns the number of subdevices currently inside the list.
+ *
+ * @param slist The subdevice list to query.
+ *
+ * @return The number of subdevices in the list.
+ */
+unsigned int me_slist_get_number_subdevices(struct me_slist *slist);
+
+/**
+ * @brief Get a subdevice by index.
+ *
+ * @param slist The subdevice list to query.
+ * @param index The index of the subdevice to get in the list.
+ *
+ * @return The subdevice at index if available.\n
+ *         NULL if the index is out of range.
+ */
+me_subdevice_t *me_slist_get_subdevice(struct me_slist *slist,
+				       unsigned int index);
+
+/**
+ * @brief Get a subdevice index by type and subtype.
+ *
+ * @param slist The subdevice list to query.
+ * @param start_subdevice The subdevice index at which the start shall begin.
+ * @param type The type of the subdevice to query.
+ * @param subtype The subtype of the subdevice to query.
+ * @param[out] subdevice On success this parameter returns the index of the subdevice matching the requested type.
+ *
+ * @return ME_ERRNO_SUCCESS on success.
+ */
+int me_slist_get_subdevice_by_type(struct me_slist *slist,
+				   unsigned int start_subdevice,
+				   int type, int subtype, int *subdevice);
+
+/**
+ * @brief Adds a subdevice to the tail of the list.
+ *
+ * @param slist The subdevice list to add a subdevice to.
+ * @param subdevice The subdevice to add to the list.
+ */
+void me_slist_add_subdevice_tail(struct me_slist *slist,
+				 me_subdevice_t * subdevice);
+
+/**
+ * @brief Removes a subdevice from the tail of the list.
+ *
+ * @param slist The subdevice list.
+ *
+ * @return Pointer to the removed subdeivce.\n
+ *         NULL in cases where the list was empty.
+ */
+me_subdevice_t *me_slist_del_subdevice_tail(struct me_slist *slist);
+
+/**
+ * @brief Initializes a subdevice list structure.
+ *
+ * @param lock The subdevice list structure to initialize.
+ * @return 0 on success.
+ */
+int me_slist_init(me_slist_t * slist);
+
+/**
+ * @brief Deinitializes a subdevice list structure and destructs every subdevice in it.
+ *
+ * @param slist The subdevice list structure to deinitialize.
+ * @return 0 on success.
+ */
+void me_slist_deinit(me_slist_t * slist);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/meslock.c b/drivers/staging/meilhaus/meslock.c
new file mode 100644
index 000000000000..5230b89b45b5
--- /dev/null
+++ b/drivers/staging/meilhaus/meslock.c
@@ -0,0 +1,136 @@
+/**
+ * @file meslock.c
+ *
+ * @brief Implements the subdevice lock class.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/spinlock.h>
+
+#include "medefines.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "meslock.h"
+
+int me_slock_enter(struct me_slock *slock, struct file *filep)
+{
+	PDEBUG_LOCKS("executed.\n");
+
+	spin_lock(&slock->spin_lock);
+
+	if ((slock->filep) != NULL && (slock->filep != filep)) {
+		PERROR("Subdevice is locked by another process.\n");
+		spin_unlock(&slock->spin_lock);
+		return ME_ERRNO_LOCKED;
+	}
+
+	slock->count++;
+
+	spin_unlock(&slock->spin_lock);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+int me_slock_exit(struct me_slock *slock, struct file *filep)
+{
+	PDEBUG_LOCKS("executed.\n");
+
+	spin_lock(&slock->spin_lock);
+	slock->count--;
+	spin_unlock(&slock->spin_lock);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+int me_slock_lock(struct me_slock *slock, struct file *filep, int lock)
+{
+	PDEBUG_LOCKS("executed.\n");
+
+	switch (lock) {
+
+	case ME_LOCK_RELEASE:
+		spin_lock(&slock->spin_lock);
+
+		if (slock->filep == filep)
+			slock->filep = NULL;
+
+		spin_unlock(&slock->spin_lock);
+
+		break;
+
+	case ME_LOCK_SET:
+		spin_lock(&slock->spin_lock);
+
+		if (slock->count) {
+			spin_unlock(&slock->spin_lock);
+			PERROR("Subdevice is used by another process.\n");
+			return ME_ERRNO_USED;
+		} else if (slock->filep == NULL)
+			slock->filep = filep;
+		else if (slock->filep != filep) {
+			spin_unlock(&slock->spin_lock);
+			PERROR("Subdevice is locked by another process.\n");
+			return ME_ERRNO_LOCKED;
+		}
+
+		spin_unlock(&slock->spin_lock);
+
+		break;
+
+	case ME_LOCK_CHECK:
+		spin_lock(&slock->spin_lock);
+
+		if (slock->count) {
+			spin_unlock(&slock->spin_lock);
+			return ME_ERRNO_USED;
+		} else if ((slock->filep != NULL) && (slock->filep != filep)) {
+			spin_unlock(&slock->spin_lock);
+			return ME_ERRNO_LOCKED;
+		}
+
+		spin_unlock(&slock->spin_lock);
+
+		break;
+
+	default:
+		break;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+void me_slock_deinit(struct me_slock *slock)
+{
+	PDEBUG_LOCKS("executed.\n");
+}
+
+int me_slock_init(me_slock_t * slock)
+{
+	PDEBUG_LOCKS("executed.\n");
+
+	slock->filep = NULL;
+	slock->count = 0;
+	spin_lock_init(&slock->spin_lock);
+
+	return 0;
+}
diff --git a/drivers/staging/meilhaus/meslock.h b/drivers/staging/meilhaus/meslock.h
new file mode 100644
index 000000000000..f42b25c3f622
--- /dev/null
+++ b/drivers/staging/meilhaus/meslock.h
@@ -0,0 +1,73 @@
+/**
+ * @file meslock.h
+ *
+ * @brief Provides the subdevice lock class.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _MESLOCK_H_
+#define _MESLOCK_H_
+
+#include <linux/spinlock.h>
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The subdevice lock class.
+ */
+typedef struct me_slock {
+	struct file *filep;		/**< Pointer to file structure holding the subdevice. */
+	int count;			/**< Number of tasks which are inside the subdevice. */
+	spinlock_t spin_lock;		/**< Spin lock protecting the attributes from concurrent access. */
+} me_slock_t;
+
+/**
+ * @brief Tries to enter a subdevice.
+ *
+ * @param slock The subdevice lock instance.
+ * @param filep The file structure identifying the calling process.
+ *
+ * @return 0 on success.
+ */
+int me_slock_enter(struct me_slock *slock, struct file *filep);
+
+/**
+ * @brief Exits a subdevice.
+ *
+ * @param slock The subdevice lock instance.
+ * @param filep The file structure identifying the calling process.
+ *
+ * @return 0 on success.
+ */
+int me_slock_exit(struct me_slock *slock, struct file *filep);
+
+/**
+ * @brief Tries to perform a locking action on a subdevice.
+ *
+ * @param slock The subdevice lock instance.
+ * @param filep The file structure identifying the calling process.
+ * @param The action to be done.
+ *
+ * @return 0 on success.
+ */
+int me_slock_lock(struct me_slock *slock, struct file *filep, int lock);
+
+/**
+ * @brief Initializes a lock structure.
+ *
+ * @param slock The lock structure to initialize.
+ * @return 0 on success.
+ */
+int me_slock_init(me_slock_t * slock);
+
+/**
+ * @brief Deinitializes a lock structure.
+ *
+ * @param slock The lock structure to deinitialize.
+ * @return 0 on success.
+ */
+void me_slock_deinit(me_slock_t * slock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/mesubdevice.c b/drivers/staging/meilhaus/mesubdevice.c
new file mode 100644
index 000000000000..98d4f1f7a824
--- /dev/null
+++ b/drivers/staging/meilhaus/mesubdevice.c
@@ -0,0 +1,317 @@
+/**
+ * @file mesubdevice.c
+ *
+ * @brief Subdevice base class implemention.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#include <linux/slab.h>
+
+#include "medefines.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "mesubdevice.h"
+
+static int me_subdevice_io_irq_start(struct me_subdevice *subdevice,
+				     struct file *filep,
+				     int channel,
+				     int irq_source,
+				     int irq_edge, int irq_arg, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_irq_wait(struct me_subdevice *subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *irq_count,
+				    int *value, int time_out, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_irq_stop(struct me_subdevice *subdevice,
+				    struct file *filep, int channel, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_reset_subdevice(struct me_subdevice *subdevice,
+					   struct file *filep, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_single_config(struct me_subdevice *subdevice,
+					 struct file *filep,
+					 int channel,
+					 int single_config,
+					 int ref,
+					 int trig_chan,
+					 int trig_type,
+					 int trig_edge, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_single_read(struct me_subdevice *subdevice,
+				       struct file *filep,
+				       int channel,
+				       int *value, int time_out, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_single_write(struct me_subdevice *subdevice,
+					struct file *filep,
+					int channel,
+					int value, int time_out, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_stream_config(struct me_subdevice *subdevice,
+					 struct file *filep,
+					 meIOStreamConfig_t * config_list,
+					 int count,
+					 meIOStreamTrigger_t * trigger,
+					 int fifo_irq_threshold, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_stream_new_values(struct me_subdevice *subdevice,
+					     struct file *filep,
+					     int time_out,
+					     int *count, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_stream_read(struct me_subdevice *subdevice,
+				       struct file *filep,
+				       int read_mode,
+				       int *values, int *count, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_stream_start(struct me_subdevice *subdevice,
+					struct file *filep,
+					int start_mode, int time_out, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_stream_status(struct me_subdevice *subdevice,
+					 struct file *filep,
+					 int wait,
+					 int *status, int *count, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_stream_stop(struct me_subdevice *subdevice,
+				       struct file *filep,
+				       int stop_mode, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_stream_write(struct me_subdevice *subdevice,
+					struct file *filep,
+					int write_mode,
+					int *values, int *count, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_lock_subdevice(me_subdevice_t * subdevice,
+				       struct file *filep, int lock, int flags)
+{
+	PDEBUG("executed.\n");
+	return me_slock_lock(&subdevice->lock, filep, lock);
+}
+
+static int me_subdevice_query_number_channels(struct me_subdevice *subdevice,
+					      int *number)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_query_number_ranges(struct me_subdevice *subdevice,
+					    int unit, int *count)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_query_range_by_min_max(struct me_subdevice *subdevice,
+					       int unit,
+					       int *min,
+					       int *max,
+					       int *maxdata, int *range)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_query_range_info(struct me_subdevice *subdevice,
+					 int range,
+					 int *unit,
+					 int *min, int *max, int *maxdata)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_query_subdevice_type(struct me_subdevice *subdevice,
+					     int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_query_subdevice_caps(struct me_subdevice *subdevice,
+					     int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me_subdevice_query_subdevice_caps_args(struct me_subdevice
+						  *subdevice, int cap,
+						  int *args, int count)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_query_timer(struct me_subdevice *subdevice,
+				    int timer,
+				    int *base_frequency,
+				    long long *min_ticks, long long *max_ticks)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_config_load(struct me_subdevice *subdevice,
+				    me_cfg_device_entry_t * config)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_SUCCESS;
+}
+
+static void me_subdevice_destructor(struct me_subdevice *subdevice)
+{
+	PDEBUG("executed.\n");
+	me_subdevice_deinit(subdevice);
+	kfree(subdevice);
+}
+
+int me_subdevice_init(me_subdevice_t * subdevice)
+{
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Init list head */
+	INIT_LIST_HEAD(&subdevice->list);
+
+	/* Initialize the subdevice lock instance */
+
+	err = me_slock_init(&subdevice->lock);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice lock instance.\n");
+		return 1;
+	}
+
+	/* Subdevice base class methods */
+	subdevice->me_subdevice_io_irq_start = me_subdevice_io_irq_start;
+	subdevice->me_subdevice_io_irq_wait = me_subdevice_io_irq_wait;
+	subdevice->me_subdevice_io_irq_stop = me_subdevice_io_irq_stop;
+	subdevice->me_subdevice_io_reset_subdevice =
+	    me_subdevice_io_reset_subdevice;
+	subdevice->me_subdevice_io_single_config =
+	    me_subdevice_io_single_config;
+	subdevice->me_subdevice_io_single_read = me_subdevice_io_single_read;
+	subdevice->me_subdevice_io_single_write = me_subdevice_io_single_write;
+	subdevice->me_subdevice_io_stream_config =
+	    me_subdevice_io_stream_config;
+	subdevice->me_subdevice_io_stream_new_values =
+	    me_subdevice_io_stream_new_values;
+	subdevice->me_subdevice_io_stream_read = me_subdevice_io_stream_read;
+	subdevice->me_subdevice_io_stream_start = me_subdevice_io_stream_start;
+	subdevice->me_subdevice_io_stream_status =
+	    me_subdevice_io_stream_status;
+	subdevice->me_subdevice_io_stream_stop = me_subdevice_io_stream_stop;
+	subdevice->me_subdevice_io_stream_write = me_subdevice_io_stream_write;
+	subdevice->me_subdevice_lock_subdevice = me_subdevice_lock_subdevice;
+	subdevice->me_subdevice_query_number_channels =
+	    me_subdevice_query_number_channels;
+	subdevice->me_subdevice_query_number_ranges =
+	    me_subdevice_query_number_ranges;
+	subdevice->me_subdevice_query_range_by_min_max =
+	    me_subdevice_query_range_by_min_max;
+	subdevice->me_subdevice_query_range_info =
+	    me_subdevice_query_range_info;
+	subdevice->me_subdevice_query_subdevice_type =
+	    me_subdevice_query_subdevice_type;
+	subdevice->me_subdevice_query_subdevice_caps =
+	    me_subdevice_query_subdevice_caps;
+	subdevice->me_subdevice_query_subdevice_caps_args =
+	    me_subdevice_query_subdevice_caps_args;
+	subdevice->me_subdevice_query_timer = me_subdevice_query_timer;
+	subdevice->me_subdevice_config_load = me_subdevice_config_load;
+	subdevice->me_subdevice_destructor = me_subdevice_destructor;
+
+	return 0;
+}
+
+void me_subdevice_deinit(me_subdevice_t * subdevice)
+{
+	PDEBUG("executed.\n");
+	me_subdevice_io_reset_subdevice(subdevice, NULL,
+					ME_IO_RESET_SUBDEVICE_NO_FLAGS);
+	me_slock_deinit(&subdevice->lock);
+}
diff --git a/drivers/staging/meilhaus/mesubdevice.h b/drivers/staging/meilhaus/mesubdevice.h
new file mode 100644
index 000000000000..19ec2b5d96f0
--- /dev/null
+++ b/drivers/staging/meilhaus/mesubdevice.h
@@ -0,0 +1,197 @@
+/**
+ * @file mesubdevice.h
+ *
+ * @brief Provides the subdevice base class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _MESUBDEVICE_H_
+#define _MESUBDEVICE_H_
+
+#include <linux/list.h>
+
+#include "metypes.h"
+#include "meioctl.h"
+#include "meslock.h"
+
+# include <linux/workqueue.h>
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Macro used to enter a subdevice.
+ */
+#define ME_SUBDEVICE_ENTER	\
+{ \
+	int err; \
+	err = me_slock_enter(&instance->base.lock, filep); \
+	if(err){ \
+		PERROR("Cannot enter subdevice.\n"); \
+		return err; \
+	} \
+}
+
+/**
+ * @brief Macro used to exit a subdevice.
+ */
+#define ME_SUBDEVICE_EXIT	\
+{\
+	int err; \
+	err = me_slock_exit(&instance->base.lock, filep); \
+	if(err){ \
+		PERROR("Cannot exit subdevice.\n"); \
+		return err; \
+	} \
+}
+
+/**
+ * @brief The subdevice base class.
+ */
+typedef struct me_subdevice {
+	/* Attributes */
+	struct list_head list;		/**< Enables the subdevice to be added to a dynamic list. */
+	me_slock_t lock;		/**< Used by user application in order to lock the subdevice for exclusive usage. */
+
+	/* Methods */
+	int (*me_subdevice_io_irq_start) (struct me_subdevice * subdevice,
+					  struct file * filep,
+					  int channel,
+					  int irq_source,
+					  int irq_edge, int irq_arg, int flags);
+
+	int (*me_subdevice_io_irq_wait) (struct me_subdevice * subdevice,
+					 struct file * filep,
+					 int channel,
+					 int *irq_count,
+					 int *value, int time_out, int flags);
+
+	int (*me_subdevice_io_irq_stop) (struct me_subdevice * subdevice,
+					 struct file * filep,
+					 int channel, int flags);
+
+	int (*me_subdevice_io_reset_subdevice) (struct me_subdevice * subdevice,
+						struct file * filep, int flags);
+
+	int (*me_subdevice_io_single_config) (struct me_subdevice * subdevice,
+					      struct file * filep,
+					      int channel,
+					      int single_config,
+					      int ref,
+					      int trig_chan,
+					      int trig_type,
+					      int trig_edge, int flags);
+
+	int (*me_subdevice_io_single_read) (struct me_subdevice * subdevice,
+					    struct file * filep,
+					    int channel,
+					    int *value,
+					    int time_out, int flags);
+
+	int (*me_subdevice_io_single_write) (struct me_subdevice * subdevice,
+					     struct file * filep,
+					     int channel,
+					     int value,
+					     int time_out, int flags);
+
+	int (*me_subdevice_io_stream_config) (struct me_subdevice * subdevice,
+					      struct file * filep,
+					      meIOStreamConfig_t * config_list,
+					      int count,
+					      meIOStreamTrigger_t * trigger,
+					      int fifo_irq_threshold,
+					      int flags);
+
+	int (*me_subdevice_io_stream_new_values) (struct me_subdevice *
+						  subdevice,
+						  struct file * filep,
+						  int time_out, int *count,
+						  int flags);
+
+	int (*me_subdevice_io_stream_read) (struct me_subdevice * subdevice,
+					    struct file * filep,
+					    int read_mode,
+					    int *values, int *count, int flags);
+
+	int (*me_subdevice_io_stream_start) (struct me_subdevice * subdevice,
+					     struct file * filep,
+					     int start_mode,
+					     int time_out, int flags);
+
+	int (*me_subdevice_io_stream_status) (struct me_subdevice * subdevice,
+					      struct file * filep,
+					      int wait,
+					      int *status,
+					      int *count, int flags);
+
+	int (*me_subdevice_io_stream_stop) (struct me_subdevice * subdevice,
+					    struct file * filep,
+					    int stop_mode, int flags);
+
+	int (*me_subdevice_io_stream_write) (struct me_subdevice * subdevice,
+					     struct file * filep,
+					     int write_mode,
+					     int *values,
+					     int *count, int flags);
+
+	int (*me_subdevice_lock_subdevice) (struct me_subdevice * subdevice,
+					    struct file * filep,
+					    int lock, int flags);
+
+	int (*me_subdevice_query_number_channels) (struct me_subdevice *
+						   subdevice, int *number);
+
+	int (*me_subdevice_query_number_ranges) (struct me_subdevice *
+						 subdevice, int unit,
+						 int *count);
+
+	int (*me_subdevice_query_range_by_min_max) (struct me_subdevice *
+						    subdevice, int unit,
+						    int *min, int *max,
+						    int *maxdata, int *range);
+
+	int (*me_subdevice_query_range_info) (struct me_subdevice * subdevice,
+					      int range,
+					      int *unit,
+					      int *min, int *max, int *maxdata);
+
+	int (*me_subdevice_query_subdevice_type) (struct me_subdevice *
+						  subdevice, int *type,
+						  int *subtype);
+
+	int (*me_subdevice_query_subdevice_caps) (struct me_subdevice *
+						  subdevice, int *caps);
+
+	int (*me_subdevice_query_subdevice_caps_args) (struct me_subdevice *
+						       subdevice, int cap,
+						       int *args, int count);
+
+	int (*me_subdevice_query_timer) (struct me_subdevice * subdevice,
+					 int timer,
+					 int *base_frequency,
+					 long long *min_ticks,
+					 long long *max_ticks);
+
+	int (*me_subdevice_config_load) (struct me_subdevice * subdevice,
+					 me_cfg_device_entry_t * config);
+
+	void (*me_subdevice_destructor) (struct me_subdevice * subdevice);
+} me_subdevice_t;
+
+/**
+ * @brief Initializes a subdevice structure.
+ *
+ * @param subdevice The subdevice structure to initialize.
+ * @return 0 on success.
+ */
+int me_subdevice_init(me_subdevice_t * subdevice);
+
+/**
+ * @brief Deinitializes a subdevice structure.
+ *
+ * @param subdevice The subdevice structure to initialize.
+ */
+void me_subdevice_deinit(me_subdevice_t * subdevice);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/metempl_device.c b/drivers/staging/meilhaus/metempl_device.c
new file mode 100644
index 000000000000..e48632ddc1aa
--- /dev/null
+++ b/drivers/staging/meilhaus/metempl_device.c
@@ -0,0 +1,137 @@
+/**
+ * @file metempl_device.c
+ *
+ * @brief template device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include <meids.h>
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "medevice.h"
+#include "metempl_device.h"
+#include "mesubdevice.h"
+#include "metempl_sub.h"
+
+me_device_t *metempl_pci_constructor(struct pci_dev *pci_device)
+{
+	metempl_device_t *metempl_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	int err;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	metempl_device = kmalloc(sizeof(metempl_device_t), GFP_KERNEL);
+
+	if (!metempl_device) {
+		PERROR("Cannot get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(metempl_device, 0, sizeof(metempl_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) metempl_device, pci_device);
+
+	if (err) {
+		kfree(metempl_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+
+	/* Get the index in the device version information table. */
+	version_idx =
+	    metempl_versions_get_device_index(metempl_device->base.info.pci.
+					      device_id);
+
+	// Initialize spin lock .
+	spin_lock_init(&metempl_device->ctrl_reg_lock);
+
+	// Create subdevice instances.
+	for (i = 0; i < metempl_versions[version_idx].subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) metempl_sub_constructor(metempl_device->
+							       base.info.pci.
+							       reg_bases[2], i,
+							       &metempl_device->
+							       ctrl_reg_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) metempl_device);
+			kfree(metempl_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&metempl_device->base.slist,
+					    subdevice);
+	}
+
+	/* Overwrite base class methods if applicable. */
+
+	return (me_device_t *) metempl_device;
+}
+
+// Init and exit of module.
+
+static int __init metempl_init(void)
+{
+	PDEBUG("executed.\n.");
+	return 0;
+}
+
+static void __exit metempl_exit(void)
+{
+	PDEBUG("executed.\n.");
+}
+
+module_init(metempl_init);
+
+module_exit(metempl_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR("Guenter Gebhardt <g.gebhardt@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for Template Device");
+MODULE_SUPPORTED_DEVICE("Meilhaus Template Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(metempl_pci_constructor);
diff --git a/drivers/staging/meilhaus/metempl_device.h b/drivers/staging/meilhaus/metempl_device.h
new file mode 100644
index 000000000000..3c3702cc72eb
--- /dev/null
+++ b/drivers/staging/meilhaus/metempl_device.h
@@ -0,0 +1,92 @@
+/**
+ * @file metempl_device.h
+ *
+ * @brief template device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _METEMPL_DEVICE_H
+#define _METEMPL_DEVICE_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure holding template device capabilities.
+ */
+typedef struct metempl_version {
+	uint16_t device_id;
+	unsigned int subdevices;
+} metempl_version_t;
+
+/**
+ * @brief Device capabilities.
+ */
+static metempl_version_t metempl_versions[] = {
+	{0xDEAD, 1},
+	{0},
+};
+
+#define METEMPL_DEVICE_VERSIONS (sizeof(metempl_versions) / sizeof(metempl_version_t) - 1) /**< Returns the number of entries in #metempl_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #metempl_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #metempl_versions.
+ */
+static inline unsigned int metempl_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < METEMPL_DEVICE_VERSIONS; i++)
+		if (metempl_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The template device class structure.
+ */
+typedef struct metempl_device {
+	me_device_t base;			/**< The Meilhaus device base class. */
+
+	/* Child class attributes. */
+	spinlock_t ctrl_reg_lock;
+} metempl_device_t;
+
+/**
+ * @brief The template device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new template device instance. \n
+ *         NULL on error.
+ */
+me_device_t *metempl_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/metempl_sub.c b/drivers/staging/meilhaus/metempl_sub.c
new file mode 100644
index 000000000000..f1d65d889e23
--- /dev/null
+++ b/drivers/staging/meilhaus/metempl_sub.c
@@ -0,0 +1,149 @@
+/**
+ * @file metempl_sub.c
+ *
+ * @brief Subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "metempl_sub_reg.h"
+#include "metempl_sub.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static void metempl_sub_destructor(struct me_subdevice *subdevice)
+{
+	metempl_sub_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+	instance = (metempl_sub_subdevice_t *) subdevice;
+
+	/* Until there this was the things the default constructor does.
+	   If you do not have any additional things to do you can wipe it out. */
+
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+static int metempl_sub_query_number_channels(me_subdevice_t * subdevice,
+					     int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int metempl_sub_query_subdevice_type(me_subdevice_t * subdevice,
+					    int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = 0;
+	*subtype = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int metempl_sub_query_subdevice_caps(me_subdevice_t * subdevice,
+					    int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+metempl_sub_subdevice_t *metempl_sub_constructor(uint32_t reg_base,
+						 unsigned int sub_idx,
+						 spinlock_t * ctrl_reg_lock)
+{
+	metempl_sub_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(metempl_sub_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(metempl_sub_subdevice_t));
+
+	/* Check if subdevice index is out of range */
+
+	if (sub_idx >= 2) {
+		PERROR("Template subdevice index is out of range.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save the subdevice index */
+	subdevice->sub_idx = sub_idx;
+
+	/* Override base class methods. */
+	subdevice->base.me_subdevice_destructor = metempl_sub_destructor;
+	subdevice->base.me_subdevice_query_number_channels =
+	    metempl_sub_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    metempl_sub_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    metempl_sub_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/metempl_sub.h b/drivers/staging/meilhaus/metempl_sub.h
new file mode 100644
index 000000000000..80c8af9a8c5a
--- /dev/null
+++ b/drivers/staging/meilhaus/metempl_sub.h
@@ -0,0 +1,64 @@
+/**
+ * @file metempl_sub.h
+ *
+ * @brief Meilhaus subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _METEMPL_SUB_H_
+#define _METEMPL_SUB_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The subdevice class.
+ */
+typedef struct metempl_sub_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+	int sub_idx;				/**< The index of the subdevice on the device. */
+
+	unsigned long ctrl_reg;			/**< Register to configure the modes. */
+} metempl_sub_subdevice_t;
+
+/**
+ * @brief The constructor to generate a subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param sub_idx The index of the subdevice on the device.
+ * @param ctrl_reg_lock Pointer to spin lock protecting the control register from concurrent access.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+metempl_sub_subdevice_t *metempl_sub_constructor(uint32_t reg_base,
+						 unsigned int sub_idx,
+						 spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/metempl_sub_reg.h b/drivers/staging/meilhaus/metempl_sub_reg.h
new file mode 100644
index 000000000000..1a2cab778a12
--- /dev/null
+++ b/drivers/staging/meilhaus/metempl_sub_reg.h
@@ -0,0 +1,35 @@
+/**
+ * @file metempl_sub_reg.h
+ *
+ * @brief Subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _METEMPL_SUB_REG_H_
+#define _METEMPL_SUB_REG_H_
+
+#ifdef __KERNEL__
+
+#define METEMPL_PORT_MODE			0x0010		/**< Configuration register. */
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/metypes.h b/drivers/staging/meilhaus/metypes.h
new file mode 100644
index 000000000000..228ea15753ea
--- /dev/null
+++ b/drivers/staging/meilhaus/metypes.h
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : metypes.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ */
+
+#ifndef _METYPES_H_
+#define _METYPES_H_
+
+
+typedef int (*meErrorCB_t)(char *pcFunctionName, int iErrorCode);
+
+typedef int (*meIOStreamCB_t)(
+		int iDevice,
+		int iSubdevice,
+		int iCount,
+	   	void *pvContext,
+	   	int iErrorCode);
+
+typedef int (*meIOIrqCB_t)(
+		int iDevice,
+	   	int iSubdevice,
+	   	int iChannel,
+		int iIrqCount,
+	   	int iValue,
+	   	void *pvContext,
+	   	int iErrorCode);
+
+
+typedef struct meIOSingle {
+	int iDevice;
+	int iSubdevice;
+	int iChannel;
+	int iDir;
+	int iValue;
+	int iTimeOut;
+	int iFlags;
+	int iErrno;
+} meIOSingle_t;
+
+
+typedef struct meIOStreamConfig {
+	int iChannel;
+	int iStreamConfig;
+	int iRef;
+	int iFlags;
+} meIOStreamConfig_t;
+
+
+typedef struct meIOStreamTrigger {
+	int iAcqStartTrigType;
+	int iAcqStartTrigEdge;
+	int iAcqStartTrigChan;
+	int iAcqStartTicksLow;
+	int iAcqStartTicksHigh;
+	int iAcqStartArgs[10];
+	int iScanStartTrigType;
+	int iScanStartTicksLow;
+	int iScanStartTicksHigh;
+	int iScanStartArgs[10];
+	int iConvStartTrigType;
+	int iConvStartTicksLow;
+	int iConvStartTicksHigh;
+	int iConvStartArgs[10];
+	int iScanStopTrigType;
+	int iScanStopCount;
+	int iScanStopArgs[10];
+	int iAcqStopTrigType;
+	int iAcqStopCount;
+	int iAcqStopArgs[10];
+	int iFlags;
+} meIOStreamTrigger_t;
+
+
+typedef struct meIOStreamStart {
+	int iDevice;
+	int iSubdevice;
+	int iStartMode;
+	int iTimeOut;
+	int iFlags;
+	int iErrno;
+} meIOStreamStart_t;
+
+
+typedef struct meIOStreamStop {
+	int iDevice;
+	int iSubdevice;
+	int iStopMode;
+	int iFlags;
+	int iErrno;
+} meIOStreamStop_t;
+
+
+#endif
diff --git a/drivers/staging/mimio/Kconfig b/drivers/staging/mimio/Kconfig
new file mode 100644
index 000000000000..c0ba4c800dff
--- /dev/null
+++ b/drivers/staging/mimio/Kconfig
@@ -0,0 +1,10 @@
+config INPUT_MIMIO
+	tristate "Mimio Xi interactive whiteboard support"
+	depends on USB
+	default N
+	help
+	  Say Y here if you want to use a Mimio Xi interactive
+	  whiteboard device.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mimio.
diff --git a/drivers/staging/mimio/Makefile b/drivers/staging/mimio/Makefile
new file mode 100644
index 000000000000..77807ee0450e
--- /dev/null
+++ b/drivers/staging/mimio/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_INPUT_MIMIO)	+= mimio.o
diff --git a/drivers/staging/mimio/mimio.c b/drivers/staging/mimio/mimio.c
new file mode 100644
index 000000000000..1ba8103f5003
--- /dev/null
+++ b/drivers/staging/mimio/mimio.c
@@ -0,0 +1,914 @@
+/*
+ * Hardware event => input event mapping:
+ *
+ *
+ *
+ input.h:#define BTN_TOOL_PEN            0x140 black
+ input.h:#define BTN_TOOL_RUBBER         0x141 blue
+ input.h:#define BTN_TOOL_BRUSH          0x142 green
+ input.h:#define BTN_TOOL_PENCIL         0x143 red
+ input.h:#define BTN_TOOL_AIRBRUSH       0x144 eraser
+ input.h:#define BTN_TOOL_FINGER         0x145 small eraser
+ input.h:#define BTN_TOOL_MOUSE          0x146 mimio interactive
+ input.h:#define BTN_TOOL_LENS           0x147 mimio interactive but1
+ input.h:#define LOCALBTN_TOOL_EXTRA1    0x14a mimio interactive but2 == BTN_TOUCH
+ input.h:#define LOCALBTN_TOOL_EXTRA2    0x14b mimio extra pens (orange, brown, yellow, purple) == BTN_STYLUS
+ input.h:#define LOCALBTN_TOOL_EXTRA3    0x14c unused == BTN_STYLUS2
+ input.h:#define BTN_TOOL_DOUBLETAP      0x14d unused
+ input.h:#define BTN_TOOL_TRIPLETAP      0x14e unused
+ *
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_K)     => EV_KEY BIT(BTN_TOOL_PEN)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_B)     => EV_KEY BIT(BTN_TOOL_RUBBER)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_G)     => EV_KEY BIT(BTN_TOOL_BRUSH)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_R)     => EV_KEY BIT(BTN_TOOL_PENCIL)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_E)     => EV_KEY BIT(BTN_TOOL_AIRBRUSH)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_ES)    => EV_KEY BIT(BTN_TOOL_FINGER)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_I)     => EV_KEY BIT(BTN_TOOL_MOUSE)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_IL)    => EV_KEY BIT(BTN_TOOL_LENS)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_IR)    => EV_KEY BIT(BTN_TOOL_DOUBLETAP)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_EX)    => EV_KEY BIT(BTN_TOOL_TRIPLETAP)
+ * MIMIO_EV_PENDATA                 => EV_ABS BIT(ABS_X), BIT(ABS_Y)
+ * MIMIO_EV_MEMRESET              => EV_KEY BIT(BTN_0)
+ * MIMIO_EV_ACC(ACC_NEWPAGE)       => EV_KEY BIT(BTN_1)
+ * MIMIO_EV_ACC(ACC_TAGPAGE)      => EV_KEY BIT(BTN_2)
+ * MIMIO_EV_ACC(ACC_PRINTPAGE)      => EV_KEY BIT(BTN_3)
+ * MIMIO_EV_ACC(ACC_MAXIMIZE)      => EV_KEY BIT(BTN_4)
+ * MIMIO_EV_ACC(ACC_FINDCTLPNL)      => EV_KEY BIT(BTN_5)
+ *
+ *
+ * open issues:
+ *      - cold-load of data captured when mimio in standalone mode not yet
+ *         supported; need to snoop Win32 box to see datastream for this.
+ *       - mimio mouse not yet supported; need to snoop Win32 box to see the
+ *         datastream for this.
+ */
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/input.h>
+#include <linux/usb.h>
+
+#define DRIVER_VERSION		"v0.031"
+#define DRIVER_AUTHOR		"mwilder@cs.nmsu.edu"
+#define DRIVER_DESC		"USB mimio-xi driver"
+
+enum {UPVALUE, DOWNVALUE, MOVEVALUE};
+
+#define MIMIO_XRANGE_MAX	9600
+#define MIMIO_YRANGE_MAX	4800
+
+#define LOCALBTN_TOOL_EXTRA1	BTN_TOUCH
+#define LOCALBTN_TOOL_EXTRA2	BTN_STYLUS
+#define LOCALBTN_TOOL_EXTRA3	BTN_STYLUS2
+
+#define MIMIO_VENDOR_ID		0x08d3
+#define MIMIO_PRODUCT_ID	0x0001
+#define MIMIO_MAXPAYLOAD	(8)
+#define MIMIO_MAXNAMELEN	(64)
+#define MIMIO_TXWAIT		(1)
+#define MIMIO_TXDONE		(2)
+
+#define MIMIO_EV_PENDOWN	(0x22)
+#define MIMIO_EV_PENDATA	(0x24)
+#define MIMIO_EV_PENUP		(0x51)
+#define MIMIO_EV_MEMRESET	(0x45)
+#define MIMIO_EV_ACC		(0xb2)
+
+#define MIMIO_PEN_K		(1)	/* black pen */
+#define MIMIO_PEN_B		(2)	/* blue pen */
+#define MIMIO_PEN_G		(3)	/* green pen */
+#define MIMIO_PEN_R		(4)	/* red pen */
+/* 5, 6, 7, 8 are extra pens */
+#define MIMIO_PEN_E		(9)	/* big eraser */
+#define MIMIO_PEN_ES		(10)	/* lil eraser */
+#define MIMIO_PENJUMP_START	(10)
+#define MIMIO_PENJUMP		(6)
+#define MIMIO_PEN_I		(17)	/* mimio interactive */
+#define MIMIO_PEN_IL		(18)	/* mimio interactive button 1 */
+#define MIMIO_PEN_IR		(19)	/* mimio interactive button 2 */
+
+#define MIMIO_PEN_MAX		(MIMIO_PEN_IR)
+
+#define ACC_DONE		(0)
+#define ACC_NEWPAGE		(1)
+#define ACC_TAGPAGE		(2)
+#define ACC_PRINTPAGE		(4)
+#define ACC_MAXIMIZE		(8)
+#define ACC_FINDCTLPNL		(16)
+
+#define isvalidtxsize(n)	((n) > 0 && (n) <= MIMIO_MAXPAYLOAD)
+
+
+struct pktbuf {
+	unsigned char instr;
+	unsigned char buf[16];
+	unsigned char *p;
+	unsigned char *q;
+};
+
+struct usbintendpt {
+	dma_addr_t dma;
+	struct urb *urb;
+	unsigned char *buf;
+	struct usb_endpoint_descriptor *desc;
+};
+
+struct mimio {
+	struct input_dev *idev;
+	struct usb_device *udev;
+	struct usb_interface *uifc;
+	int open;
+	int present;
+	int greeted;
+	int txflags;
+	char phys[MIMIO_MAXNAMELEN];
+	struct usbintendpt in;
+	struct usbintendpt out;
+	struct pktbuf pktbuf;
+	unsigned char minor;
+	wait_queue_head_t waitq;
+	spinlock_t txlock;
+	void (*rxhandler)(struct mimio *, unsigned char *, unsigned int);
+	int last_pen_down;
+};
+
+static void mimio_close(struct input_dev *);
+static void mimio_dealloc(struct mimio *);
+static void mimio_disconnect(struct usb_interface *);
+static int mimio_greet(struct mimio *);
+static void mimio_irq_in(struct urb *);
+static void mimio_irq_out(struct urb *);
+static int mimio_open(struct input_dev *);
+static int mimio_probe(struct usb_interface *, const struct usb_device_id *);
+static void mimio_rx_handler(struct mimio *, unsigned char *, unsigned int);
+static int mimio_tx(struct mimio *, const char *, int);
+
+static char mimio_name[] = "VirtualInk mimio-Xi";
+static struct usb_device_id mimio_table [] = {
+	{ USB_DEVICE(MIMIO_VENDOR_ID, MIMIO_PRODUCT_ID) },
+	{ USB_DEVICE(0x0525, 0xa4a0) }, /* gadget zero firmware */
+	{ }
+};
+
+MODULE_DEVICE_TABLE(usb, mimio_table);
+
+static struct usb_driver mimio_driver = {
+	.name = "mimio",
+	.probe = mimio_probe,
+	.disconnect = mimio_disconnect,
+	.id_table = mimio_table,
+};
+
+static DECLARE_MUTEX(disconnect_sem);
+
+static void mimio_close(struct input_dev *idev)
+{
+	struct mimio *mimio;
+
+	mimio = input_get_drvdata(idev);
+	if (!mimio) {
+		dev_err(&idev->dev, "null mimio attached to input device\n");
+		return;
+	}
+
+	if (mimio->open <= 0)
+		dev_err(&idev->dev, "mimio not open.\n");
+	else
+		mimio->open--;
+
+	if (mimio->present == 0 && mimio->open == 0)
+		mimio_dealloc(mimio);
+}
+
+static void mimio_dealloc(struct mimio *mimio)
+{
+	if (mimio == NULL)
+		return;
+
+	usb_kill_urb(mimio->in.urb);
+
+	usb_kill_urb(mimio->out.urb);
+
+	if (mimio->idev) {
+		input_unregister_device(mimio->idev);
+		if (mimio->idev->grab)
+			input_close_device(mimio->idev->grab);
+		else
+			dev_dbg(&mimio->idev->dev, "mimio->idev->grab == NULL"
+				" -- didn't call input_close_device\n");
+	}
+
+	usb_free_urb(mimio->in.urb);
+
+	usb_free_urb(mimio->out.urb);
+
+	if (mimio->in.buf) {
+		usb_buffer_free(mimio->udev, MIMIO_MAXPAYLOAD, mimio->in.buf,
+				mimio->in.dma);
+	}
+
+	if (mimio->out.buf)
+		usb_buffer_free(mimio->udev, MIMIO_MAXPAYLOAD, mimio->out.buf,
+				mimio->out.dma);
+
+	if (mimio->idev)
+		input_free_device(mimio->idev);
+
+	kfree(mimio);
+}
+
+static void mimio_disconnect(struct usb_interface *ifc)
+{
+	struct mimio *mimio;
+
+	down(&disconnect_sem);
+
+	mimio = usb_get_intfdata(ifc);
+	usb_set_intfdata(ifc, NULL);
+	dev_dbg(&mimio->idev->dev, "disconnect\n");
+
+	if (mimio) {
+		mimio->present = 0;
+
+		if (mimio->open <= 0)
+			mimio_dealloc(mimio);
+	}
+
+	up(&disconnect_sem);
+}
+
+static int mimio_greet(struct mimio *mimio)
+{
+	const struct grtpkt {
+		int nbytes;
+		unsigned delay;
+		char data[8];
+	} grtpkts[] = {
+		{ 3, 0, { 0x11, 0x55, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+		{ 5, 0, { 0x53, 0x55, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00 } },
+		{ 5, 0, { 0x43, 0x55, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00 } },
+		{ 5, 0, { 0x33, 0x55, 0x00, 0x00, 0x66, 0x00, 0x00, 0x00 } },
+		{ 5, 0, { 0x13, 0x00, 0x5e, 0x02, 0x4f, 0x00, 0x00, 0x00 } },
+		{ 5, 0, { 0x13, 0x00, 0x04, 0x03, 0x14, 0x00, 0x00, 0x00 } },
+		{ 5, 2, { 0x13, 0x00, 0x00, 0x04, 0x17, 0x00, 0x00, 0x00 } },
+		{ 5, 0, { 0x13, 0x00, 0x0d, 0x08, 0x16, 0x00, 0x00, 0x00 } },
+		{ 5, 0, { 0x13, 0x00, 0x4d, 0x01, 0x5f, 0x00, 0x00, 0x00 } },
+		{ 3, 0, { 0xf1, 0x55, 0xa4, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+		{ 7, 2, { 0x52, 0x55, 0x00, 0x07, 0x31, 0x55, 0x64, 0x00 } },
+		{ 0, 0, { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+	};
+	int rslt;
+	const struct grtpkt *pkt;
+
+	for (pkt = grtpkts; pkt->nbytes; pkt++) {
+		rslt = mimio_tx(mimio, pkt->data, pkt->nbytes);
+		if (rslt)
+			return rslt;
+		if (pkt->delay)
+			msleep(pkt->delay);
+	}
+
+	return 0;
+}
+
+static void mimio_irq_in(struct urb *urb)
+{
+	int rslt;
+	char *data;
+	const char *reason = "going down";
+	struct mimio *mimio;
+
+	mimio = urb->context;
+
+	if (mimio == NULL)
+		/* paranoia */
+		return;
+
+	switch (urb->status) {
+	case 0:
+		/* success */
+		break;
+	case -ETIMEDOUT:
+		reason = "timeout -- unplugged?";
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+		dev_dbg(&mimio->idev->dev, "%s.\n", reason);
+		return;
+	default:
+		dev_dbg(&mimio->idev->dev, "unknown urb-status: %d.\n",
+			urb->status);
+		goto exit;
+	}
+	data = mimio->in.buf;
+
+	if (mimio->rxhandler)
+		mimio->rxhandler(mimio, data, urb->actual_length);
+exit:
+	/*
+	 * Keep listening to device on same urb.
+	 */
+	rslt = usb_submit_urb(urb, GFP_ATOMIC);
+	if (rslt)
+		dev_err(&mimio->idev->dev, "usb_submit_urb failure: %d.\n",
+			rslt);
+}
+
+static void mimio_irq_out(struct urb *urb)
+{
+	unsigned long flags;
+	struct mimio *mimio;
+
+	mimio = urb->context;
+
+	if (urb->status)
+		dev_dbg(&mimio->idev->dev, "urb-status: %d.\n", urb->status);
+
+	spin_lock_irqsave(&mimio->txlock, flags);
+	mimio->txflags |= MIMIO_TXDONE;
+	spin_unlock_irqrestore(&mimio->txlock, flags);
+	wmb();
+	wake_up(&mimio->waitq);
+}
+
+static int mimio_open(struct input_dev *idev)
+{
+	int rslt;
+	struct mimio *mimio;
+
+	rslt = 0;
+	down(&disconnect_sem);
+	mimio = input_get_drvdata(idev);
+	dev_dbg(&idev->dev, "mimio_open\n");
+
+	if (mimio == NULL) {
+		dev_err(&idev->dev, "null mimio.\n");
+		rslt = -ENODEV;
+		goto exit;
+	}
+
+	if (mimio->open++)
+		goto exit;
+
+	if (mimio->present && !mimio->greeted) {
+		struct urb *urb = mimio->in.urb;
+		mimio->in.urb->dev = mimio->udev;
+		rslt = usb_submit_urb(mimio->in.urb, GFP_KERNEL);
+		if (rslt) {
+			dev_err(&idev->dev, "usb_submit_urb failure "
+				"(res = %d: %s). Not greeting.\n",
+				rslt,
+				(!urb ? "urb is NULL" :
+				 (urb->hcpriv ? "urb->hcpriv is non-NULL" :
+				  (!urb->complete ? "urb is not complete" :
+				   (urb->number_of_packets <= 0 ? "urb has no packets" :
+				    (urb->interval <= 0 ? "urb interval too small" :
+				     "urb interval too large or some other error"))))));
+			rslt = -EIO;
+			goto exit;
+		}
+		rslt = mimio_greet(mimio);
+		if (rslt == 0) {
+			dev_dbg(&idev->dev, "Mimio greeted OK.\n");
+			mimio->greeted = 1;
+		} else {
+			dev_dbg(&idev->dev, "Mimio greet Failure (%d)\n",
+				rslt);
+		}
+	}
+
+exit:
+	up(&disconnect_sem);
+	return rslt;
+}
+
+static int mimio_probe(struct usb_interface *ifc,
+		       const struct usb_device_id *id)
+{
+	char path[64];
+	int pipe, maxp;
+	struct mimio *mimio;
+	struct usb_device *udev;
+	struct usb_host_interface *hostifc;
+	struct input_dev *input_dev;
+	int res = 0;
+	int i;
+
+	udev = interface_to_usbdev(ifc);
+
+	mimio = kzalloc(sizeof(struct mimio), GFP_KERNEL);
+	if (!mimio)
+		return -ENOMEM;
+
+	input_dev = input_allocate_device();
+	if (!input_dev) {
+		mimio_dealloc(mimio);
+		return -ENOMEM;
+	}
+
+	mimio->uifc = ifc;
+	mimio->udev = udev;
+	mimio->pktbuf.p = mimio->pktbuf.buf;
+	mimio->pktbuf.q = mimio->pktbuf.buf;
+	/* init_input_dev(mimio->idev); */
+	mimio->idev = input_dev;
+	init_waitqueue_head(&mimio->waitq);
+	spin_lock_init(&mimio->txlock);
+	hostifc = ifc->cur_altsetting;
+
+	if (hostifc->desc.bNumEndpoints != 2) {
+		dev_err(&udev->dev, "Unexpected endpoint count: %d.\n",
+			hostifc->desc.bNumEndpoints);
+		mimio_dealloc(mimio);
+		return -ENODEV;
+	}
+
+	mimio->in.desc = &(hostifc->endpoint[0].desc);
+	mimio->out.desc = &(hostifc->endpoint[1].desc);
+
+	mimio->in.buf = usb_buffer_alloc(udev, MIMIO_MAXPAYLOAD, GFP_KERNEL,
+					 &mimio->in.dma);
+	mimio->out.buf = usb_buffer_alloc(udev, MIMIO_MAXPAYLOAD, GFP_KERNEL,
+					  &mimio->out.dma);
+
+	if (mimio->in.buf == NULL || mimio->out.buf == NULL) {
+		dev_err(&udev->dev, "usb_buffer_alloc failure.\n");
+		mimio_dealloc(mimio);
+		return -ENOMEM;
+	}
+
+	mimio->in.urb = usb_alloc_urb(0, GFP_KERNEL);
+	mimio->out.urb = usb_alloc_urb(0, GFP_KERNEL);
+
+	if (mimio->in.urb == NULL || mimio->out.urb == NULL) {
+		dev_err(&udev->dev, "usb_alloc_urb failure.\n");
+		mimio_dealloc(mimio);
+		return -ENOMEM;
+	}
+
+	/*
+	 * Build the input urb.
+	 */
+	pipe = usb_rcvintpipe(udev, mimio->in.desc->bEndpointAddress);
+	maxp = usb_maxpacket(udev, pipe, usb_pipeout(pipe));
+	if (maxp > MIMIO_MAXPAYLOAD)
+		maxp = MIMIO_MAXPAYLOAD;
+	usb_fill_int_urb(mimio->in.urb, udev, pipe, mimio->in.buf, maxp,
+			 mimio_irq_in, mimio, mimio->in.desc->bInterval);
+	mimio->in.urb->transfer_dma = mimio->in.dma;
+	mimio->in.urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+
+	/*
+	 * Build the output urb.
+	 */
+	pipe = usb_sndintpipe(udev, mimio->out.desc->bEndpointAddress);
+	maxp = usb_maxpacket(udev, pipe, usb_pipeout(pipe));
+	if (maxp > MIMIO_MAXPAYLOAD)
+		maxp = MIMIO_MAXPAYLOAD;
+	usb_fill_int_urb(mimio->out.urb, udev, pipe, mimio->out.buf, maxp,
+			 mimio_irq_out, mimio, mimio->out.desc->bInterval);
+	mimio->out.urb->transfer_dma = mimio->out.dma;
+	mimio->out.urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+
+	/*
+	 * Build input device info
+	 */
+	usb_make_path(udev, path, 64);
+	snprintf(mimio->phys, MIMIO_MAXNAMELEN, "%s/input0", path);
+	input_set_drvdata(input_dev, mimio);
+	/* input_dev->dev = &ifc->dev; */
+	input_dev->open = mimio_open;
+	input_dev->close = mimio_close;
+	input_dev->name = mimio_name;
+	input_dev->phys = mimio->phys;
+	input_dev->dev.parent = &ifc->dev;
+
+	input_dev->id.bustype = BUS_USB;
+	input_dev->id.vendor = le16_to_cpu(udev->descriptor.idVendor);
+	input_dev->id.product = le16_to_cpu(udev->descriptor.idProduct);
+	input_dev->id.version = le16_to_cpu(udev->descriptor.bcdDevice);
+
+	input_dev->evbit[0] |= BIT(EV_KEY) | BIT(EV_ABS);
+	for (i = BTN_TOOL_PEN; i <= LOCALBTN_TOOL_EXTRA2; ++i)
+		set_bit(i, input_dev->keybit);
+
+	input_dev->keybit[BIT_WORD(BTN_MISC)] |= BIT_MASK(BTN_0) |
+						 BIT_MASK(BTN_1) |
+						 BIT_MASK(BTN_2) |
+						 BIT_MASK(BTN_3) |
+						 BIT_MASK(BTN_4) |
+						 BIT_MASK(BTN_5);
+	/*   input_dev->keybit[BTN_MOUSE] |= BIT(BTN_LEFT); */
+	input_dev->absbit[0] |= BIT_MASK(ABS_X) | BIT_MASK(ABS_Y);
+	input_set_abs_params(input_dev, ABS_X, 0, MIMIO_XRANGE_MAX, 0, 0);
+	input_set_abs_params(input_dev, ABS_Y, 0, MIMIO_YRANGE_MAX, 0, 0);
+	input_dev->absbit[BIT_WORD(ABS_MISC)] |= BIT_MASK(ABS_MISC);
+
+#if 0
+	input_dev->absmin[ABS_X] = 0;
+	input_dev->absmin[ABS_Y] = 0;
+	input_dev->absmax[ABS_X] = 9600;
+	input_dev->absmax[ABS_Y] = 4800;
+	input_dev->absfuzz[ABS_X] = 0;
+	input_dev->absfuzz[ABS_Y] = 0;
+	input_dev->absflat[ABS_X] = 0;
+	input_dev->absflat[ABS_Y] = 0;
+#endif
+
+#if 0
+	/* this will just reduce the precision */
+	input_dev->absfuzz[ABS_X] = 8; /* experimental; may need to change */
+	input_dev->absfuzz[ABS_Y] = 8; /* experimental; may need to change */
+#endif
+
+	/*
+	 * Register the input device.
+	 */
+	res = input_register_device(mimio->idev);
+	if (res) {
+		dev_err(&udev->dev, "input_register_device failure (%d)\n",
+			res);
+		mimio_dealloc(mimio);
+		return -EIO;
+	}
+	dev_dbg(&mimio->idev->dev, "input: %s on %s (res = %d).\n",
+		input_dev->name, input_dev->phys, res);
+
+	usb_set_intfdata(ifc, mimio);
+	mimio->present = 1;
+
+	/*
+	 * Submit the input urb to the usb subsystem.
+	 */
+	mimio->in.urb->dev = mimio->udev;
+	res = usb_submit_urb(mimio->in.urb, GFP_KERNEL);
+	if (res) {
+		dev_err(&mimio->idev->dev, "usb_submit_urb failure (%d)\n",
+			res);
+		mimio_dealloc(mimio);
+		return -EIO;
+	}
+
+	/*
+	 * Attempt to greet the mimio after giving
+	 * it some post-init settling time.
+	 *
+	 * note: sometimes this sleep interval isn't
+	 * long enough to permit the device to re-init
+	 * after a hot-swap; maybe need to bump it up.
+	 *
+	 * As it is, this probably breaks module unloading support!
+	 */
+	msleep(1024);
+
+	res = mimio_greet(mimio);
+	if (res == 0) {
+		dev_dbg(&mimio->idev->dev, "Mimio greeted OK.\n");
+		mimio->greeted = 1;
+		mimio->rxhandler = mimio_rx_handler;
+	} else {
+		dev_dbg(&mimio->idev->dev, "Mimio greet Failure (%d)\n", res);
+	}
+
+	return 0;
+}
+
+static int handle_mimio_rx_penupdown(struct mimio *mimio,
+				     int down,
+				     const char *const instr[],
+				     const int instr_ofst[])
+{
+	int penid, x;
+	if (mimio->pktbuf.q - mimio->pktbuf.p < (down ? 4 : 3))
+		return 1; 		/* partial pkt */
+
+	if (down) {
+		x = *mimio->pktbuf.p ^ *(mimio->pktbuf.p + 1) ^
+			*(mimio->pktbuf.p + 2);
+		if (x != *(mimio->pktbuf.p + 3)) {
+			dev_dbg(&mimio->idev->dev, "EV_PEN%s: bad xsum.\n",
+				down ? "DOWN":"UP");
+			/* skip this event data */
+			mimio->pktbuf.p += 4;
+			/* decode any remaining events */
+			return 0;
+		}
+		penid = mimio->pktbuf.instr = *(mimio->pktbuf.p + 2);
+		if (penid > MIMIO_PEN_MAX) {
+			dev_dbg(&mimio->idev->dev,
+				"Unmapped penID (not in [0, %d]): %d\n",
+				MIMIO_PEN_MAX, (int)mimio->pktbuf.instr);
+			penid = mimio->pktbuf.instr = 0;
+		}
+		mimio->last_pen_down = penid;
+	} else {
+		penid = mimio->last_pen_down;
+	}
+	dev_dbg(&mimio->idev->dev, "%s (id %d, code %d) %s.\n", instr[penid],
+		instr_ofst[penid], penid, down ? "down" : "up");
+
+	if (instr_ofst[penid] >= 0) {
+		int code = BTN_TOOL_PEN + instr_ofst[penid];
+		int value = down ? DOWNVALUE : UPVALUE;
+		if (code > KEY_MAX)
+			dev_dbg(&mimio->idev->dev, "input_event will ignore "
+				"-- code (%d) > KEY_MAX\n", code);
+		if (!test_bit(code, mimio->idev->keybit))
+			dev_dbg(&mimio->idev->dev, "input_event will ignore "
+				"-- bit for code (%d) not enabled\n", code);
+		if (!!test_bit(code, mimio->idev->key) == value)
+			dev_dbg(&mimio->idev->dev, "input_event will ignore "
+				"-- bit for code (%d) already set to %d\n",
+				code, value);
+		if (value != DOWNVALUE) {
+			/* input_regs(mimio->idev, regs); */
+			input_report_key(mimio->idev, code, value);
+			input_sync(mimio->idev);
+		} else {
+			/* wait until we get some coordinates */
+		}
+	} else {
+		dev_dbg(&mimio->idev->dev, "penID offset[%d] == %d is < 0 "
+			"- not sending\n", penid, instr_ofst[penid]);
+	}
+	mimio->pktbuf.p += down ? 4 : 3; /* 3 for up, 4 for down */
+	return 0;
+}
+
+/*
+ * Stay tuned for partial-packet excitement.
+ *
+ * This routine buffers data packets received from the mimio device
+ * in the mimio's data space.  This buffering is necessary because
+ * the mimio's in endpoint can serve us partial packets of data, and
+ * we want the driver to support the servicing of multiple mimios.
+ * Empirical evidence gathered so far suggests that the method of
+ * buffering packet data in the mimio's data space works.  Previous
+ * versions of this driver did not buffer packet data in each mimio's
+ * data-space, and were therefore not able to service multiple mimios.
+ * Note that since the caller of this routine is running in interrupt
+ * context, care needs to be taken to ensure that this routine does not
+ * become bloated, and it may be that another spinlock is needed in each
+ * mimio to guard the buffered packet data properly.
+ */
+static void mimio_rx_handler(struct mimio *mimio,
+			     unsigned char *data,
+			     unsigned int nbytes)
+{
+	struct device *dev = &mimio->idev->dev;
+	unsigned int x;
+	unsigned int y;
+	static const char * const instr[] = {
+		"?0",
+		"black pen", "blue pen", "green pen", "red pen",
+		"brown pen", "orange pen", "purple pen", "yellow pen",
+		"big eraser", "lil eraser",
+		"?11", "?12", "?13", "?14", "?15", "?16",
+		"mimio interactive", "interactive button1",
+		"interactive button2"
+	};
+
+	/* Mimio Interactive gives:
+	 * down: [0x22 0x01 0x11 0x32 0x24]
+	 * b1  : [0x22 0x01 0x12 0x31 0x24]
+	 * b2  : [0x22 0x01 0x13 0x30 0x24]
+	 */
+	static const int instr_ofst[] = {
+		-1,
+		0, 1, 2, 3,
+		9, 9, 9, 9,
+		4, 5,
+		-1, -1, -1, -1, -1, -1,
+		6, 7, 8,
+	};
+
+	memcpy(mimio->pktbuf.q, data, nbytes);
+	mimio->pktbuf.q += nbytes;
+
+	while (mimio->pktbuf.p < mimio->pktbuf.q) {
+		int t = *mimio->pktbuf.p;
+		switch (t) {
+		case MIMIO_EV_PENUP:
+		case MIMIO_EV_PENDOWN:
+			if (handle_mimio_rx_penupdown(mimio,
+						      t == MIMIO_EV_PENDOWN,
+						      instr, instr_ofst))
+				return; /* partial packet */
+			break;
+
+		case MIMIO_EV_PENDATA:
+			if (mimio->pktbuf.q - mimio->pktbuf.p < 6)
+				/* partial pkt */
+				return;
+			x = *mimio->pktbuf.p ^ *(mimio->pktbuf.p + 1) ^
+				*(mimio->pktbuf.p + 2) ^
+				*(mimio->pktbuf.p + 3) ^
+				*(mimio->pktbuf.p + 4);
+			if (x != *(mimio->pktbuf.p + 5)) {
+				dev_dbg(dev, "EV_PENDATA: bad xsum.\n");
+				mimio->pktbuf.p += 6; /* skip this event data */
+				break; /* decode any remaining events */
+			}
+			x = *(mimio->pktbuf.p + 1);
+			x <<= 8;
+			x |= *(mimio->pktbuf.p + 2);
+			y = *(mimio->pktbuf.p + 3);
+			y <<= 8;
+			y |= *(mimio->pktbuf.p + 4);
+			dev_dbg(dev, "coord: (%d, %d)\n", x, y);
+			if (instr_ofst[mimio->pktbuf.instr] >= 0) {
+				int code = BTN_TOOL_PEN +
+					   instr_ofst[mimio->last_pen_down];
+#if 0
+				/* Utter hack to ensure we get forwarded _AND_
+				 * so we can identify when a complete signal is
+				 * received */
+				mimio->idev->abs[ABS_Y] = -1;
+				mimio->idev->abs[ABS_X] = -1;
+#endif
+				/* input_regs(mimio->idev, regs); */
+				input_report_abs(mimio->idev, ABS_X, x);
+				input_report_abs(mimio->idev, ABS_Y, y);
+				/* fake a penup */
+				change_bit(code, mimio->idev->key);
+				input_report_key(mimio->idev,
+						 code,
+						 DOWNVALUE);
+				/* always sync here */
+				mimio->idev->sync = 0;
+				input_sync(mimio->idev);
+			}
+			mimio->pktbuf.p += 6;
+			break;
+		case MIMIO_EV_MEMRESET:
+			if (mimio->pktbuf.q - mimio->pktbuf.p < 7)
+				/* partial pkt */
+				return;
+			dev_dbg(dev, "mem-reset.\n");
+			/* input_regs(mimio->idev, regs); */
+			input_event(mimio->idev, EV_KEY, BTN_0, 1);
+			input_event(mimio->idev, EV_KEY, BTN_0, 0);
+			input_sync(mimio->idev);
+			mimio->pktbuf.p += 7;
+			break;
+		case MIMIO_EV_ACC:
+			if (mimio->pktbuf.q - mimio->pktbuf.p < 4)
+				/* partial pkt */
+				return;
+			x = *mimio->pktbuf.p ^ *(mimio->pktbuf.p + 1) ^
+				*(mimio->pktbuf.p + 2);
+			if (x != *(mimio->pktbuf.p + 3)) {
+				dev_dbg(dev, "EV_ACC: bad xsum.\n");
+				mimio->pktbuf.p += 4; /* skip this event data */
+				break; /* decode any remaining events */
+			}
+			switch (*(mimio->pktbuf.p + 2)) {
+			case ACC_NEWPAGE:
+				dev_dbg(&mimio->idev->dev, "new-page.\n");
+				/* input_regs(mimio->idev, regs); */
+				input_event(mimio->idev, EV_KEY, BTN_1, 1);
+				input_event(mimio->idev, EV_KEY, BTN_1, 0);
+				input_sync(mimio->idev);
+				break;
+			case ACC_TAGPAGE:
+				dev_dbg(&mimio->idev->dev, "tag-page.\n");
+				/* input_regs(mimio->idev, regs); */
+				input_event(mimio->idev, EV_KEY, BTN_2, 1);
+				input_event(mimio->idev, EV_KEY, BTN_2, 0);
+				input_sync(mimio->idev);
+				break;
+			case ACC_PRINTPAGE:
+				dev_dbg(&mimio->idev->dev, "print-page.\n");
+				/* input_regs(mimio->idev, regs);*/
+				input_event(mimio->idev, EV_KEY, BTN_3, 1);
+				input_event(mimio->idev, EV_KEY, BTN_3, 0);
+				input_sync(mimio->idev);
+				break;
+			case ACC_MAXIMIZE:
+				dev_dbg(&mimio->idev->dev,
+					"maximize-window.\n");
+				/* input_regs(mimio->idev, regs); */
+				input_event(mimio->idev, EV_KEY, BTN_4, 1);
+				input_event(mimio->idev, EV_KEY, BTN_4, 0);
+				input_sync(mimio->idev);
+				break;
+			case ACC_FINDCTLPNL:
+				dev_dbg(&mimio->idev->dev, "find-ctl-panel.\n");
+				/* input_regs(mimio->idev, regs); */
+				input_event(mimio->idev, EV_KEY, BTN_5, 1);
+				input_event(mimio->idev, EV_KEY, BTN_5, 0);
+				input_sync(mimio->idev);
+				break;
+			case ACC_DONE:
+				dev_dbg(&mimio->idev->dev, "acc-done.\n");
+				/* no event is dispatched to the input
+				 * subsystem for this device event.
+				 */
+				break;
+			default:
+				dev_dbg(dev, "unknown acc event.\n");
+				break;
+			}
+			mimio->pktbuf.p += 4;
+			break;
+		default:
+			mimio->pktbuf.p++;
+			break;
+		}
+	}
+
+	/*
+	 * No partial event was received, so reset mimio's pktbuf ptrs.
+	 */
+	mimio->pktbuf.p = mimio->pktbuf.q = mimio->pktbuf.buf;
+}
+
+static int mimio_tx(struct mimio *mimio, const char *buf, int nbytes)
+{
+	int rslt;
+	int timeout;
+	unsigned long flags;
+	DECLARE_WAITQUEUE(wait, current);
+
+	if (!(isvalidtxsize(nbytes))) {
+		dev_err(&mimio->idev->dev, "invalid arg: nbytes: %d.\n",
+			nbytes);
+		return -EINVAL;
+	}
+
+	/*
+	 * Init the out urb and copy the data to send.
+	 */
+	mimio->out.urb->dev = mimio->udev;
+	mimio->out.urb->transfer_buffer_length = nbytes;
+	memcpy(mimio->out.urb->transfer_buffer, buf, nbytes);
+
+	/*
+	 * Send the data.
+	 */
+	spin_lock_irqsave(&mimio->txlock, flags);
+	mimio->txflags = MIMIO_TXWAIT;
+	rslt = usb_submit_urb(mimio->out.urb, GFP_ATOMIC);
+	spin_unlock_irqrestore(&mimio->txlock, flags);
+	dev_dbg(&mimio->idev->dev, "rslt: %d.\n", rslt);
+
+	if (rslt) {
+		dev_err(&mimio->idev->dev, "usb_submit_urb failure: %d.\n",
+			rslt);
+		return rslt;
+	}
+
+	/*
+	 * Wait for completion to be signalled (the mimio_irq_out
+	 * completion routine will or MIMIO_TXDONE in with txflags).
+	 */
+	timeout = HZ;
+	set_current_state(TASK_INTERRUPTIBLE);
+	add_wait_queue(&mimio->waitq, &wait);
+
+	while (timeout && ((mimio->txflags & MIMIO_TXDONE) == 0)) {
+		timeout = schedule_timeout(timeout);
+		rmb();
+	}
+
+	if ((mimio->txflags & MIMIO_TXDONE) == 0)
+		dev_dbg(&mimio->idev->dev, "tx timed out.\n");
+
+	/*
+	 * Now that completion has been signalled,
+	 * unlink the urb so that it can be recycled.
+	 */
+	set_current_state(TASK_RUNNING);
+	remove_wait_queue(&mimio->waitq, &wait);
+	usb_unlink_urb(mimio->out.urb);
+
+	return rslt;
+}
+
+static int __init mimio_init(void)
+{
+	int rslt;
+
+	rslt = usb_register(&mimio_driver);
+	if (rslt != 0) {
+		err("%s: usb_register failure: %d", __func__, rslt);
+		return rslt;
+	}
+
+	printk(KERN_INFO KBUILD_MODNAME ":"
+	       DRIVER_DESC " " DRIVER_VERSION "\n");
+	return rslt;
+}
+
+static void __exit mimio_exit(void)
+{
+	usb_deregister(&mimio_driver);
+}
+
+module_init(mimio_init);
+module_exit(mimio_exit);
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/otus/80211core/amsdu.c b/drivers/staging/otus/80211core/amsdu.c
new file mode 100644
index 000000000000..c9123d58b82f
--- /dev/null
+++ b/drivers/staging/otus/80211core/amsdu.c
@@ -0,0 +1,134 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfGetAmsduSubFrame          */
+/*      Get a subframe from a-MSDU.                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : A-MSDU frame buffer                                       */
+/*      offset : offset of subframe in the A-MSDU                       */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      NULL or subframe                                                */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+zbuf_t* zfGetAmsduSubFrame(zdev_t* dev, zbuf_t* buf, u16_t* offset)
+{
+    u16_t subframeLen;
+    u16_t amsduLen = zfwBufGetSize(dev, buf);
+    zbuf_t* newBuf;
+
+    ZM_PERFORMANCE_RX_AMSDU(dev, buf, amsduLen);
+
+    /* Verify A-MSDU length */
+    if (amsduLen < (*offset + 14))
+    {
+        return NULL;
+    }
+
+    /* Locate A-MSDU subframe by offset and verify subframe length */
+    subframeLen = (zmw_buf_readb(dev, buf, *offset + 12) << 8) +
+                  zmw_buf_readb(dev, buf, *offset + 13);
+    if (subframeLen == 0)
+    {
+        return NULL;
+    }
+
+    /* Verify A-MSDU subframe length */
+    if ((*offset+14+subframeLen) <= amsduLen)
+    {
+        /* Allocate a new buffer */
+        if ((newBuf = zfwBufAllocate(dev, 24+2+subframeLen)) != NULL)
+        {
+#ifdef ZM_ENABLE_NATIVE_WIFI
+            /* Copy and convert subframe to wlan frame format */
+            /* SHALL NOT INCLUDE QOS and AMSDU header. Ray 20070807 For Vista */
+            zfRxBufferCopy(dev, newBuf, buf, 0, 0, 24);
+            zfRxBufferCopy(dev, newBuf, buf, 24, *offset+14, subframeLen);
+            zfwBufSetSize(dev, newBuf, 24+subframeLen);
+#else
+            /* Copy subframe to new buffer */
+            zfRxBufferCopy(dev, newBuf, buf, 0, *offset, 14+subframeLen);
+            zfwBufSetSize(dev, newBuf, 14+subframeLen);
+#endif
+            /* Update offset */
+            *offset += (((14+subframeLen)+3) & 0xfffc);
+
+            /* Return buffer pointer */
+            return newBuf;
+        }
+    }
+    return NULL;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfDeAmsdu                   */
+/*      De-AMSDU.                                                       */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : A-MSDU frame buffer                                       */
+/*      vap : VAP port                                                  */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+void zfDeAmsdu(zdev_t* dev, zbuf_t* buf, u16_t vap, u8_t encryMode)
+{
+    u16_t offset = ZM_SIZE_OF_WLAN_DATA_HEADER+ZM_SIZE_OF_QOS_CTRL;
+    zbuf_t* subframeBuf;
+    zmw_get_wlan_dev(dev);
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    if (encryMode == ZM_AES || encryMode == ZM_TKIP)
+    {
+        offset += (ZM_SIZE_OF_IV + ZM_SIZE_OF_EXT_IV);
+    }
+    else if (encryMode == ZM_WEP64 || encryMode == ZM_WEP128)
+    {
+        offset += ZM_SIZE_OF_IV;
+    }
+
+    /* Repeatly calling zfGetAmsduSubFrame() until NULL returned */
+    while ((subframeBuf = zfGetAmsduSubFrame(dev, buf, &offset)) != NULL)
+    {
+        wd->commTally.NotifyNDISRxFrmCnt++;
+        if (wd->zfcbRecvEth != NULL)
+    	{
+            wd->zfcbRecvEth(dev, subframeBuf, (u8_t)vap);
+            ZM_PERFORMANCE_RX_MSDU(dev, wd->tick);
+        }
+    }
+    zfwBufFree(dev, buf, 0);
+
+    return;
+}
diff --git a/drivers/staging/otus/80211core/cagg.c b/drivers/staging/otus/80211core/cagg.c
new file mode 100644
index 000000000000..4942190747a1
--- /dev/null
+++ b/drivers/staging/otus/80211core/cagg.c
@@ -0,0 +1,3611 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : cagg.c                                                */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains A-MPDU aggregation related functions.      */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#include "cprecomp.h"
+
+extern u8_t zcUpToAc[8];
+const u8_t pri[] = {3,3,2,3,2,1,3,2,1,0};
+
+
+u16_t aggr_count;
+u32_t success_mpdu;
+u32_t total_mpdu;
+
+void zfAggInit(zdev_t* dev)
+{
+    u16_t i,j;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+    /*
+     * reset sta information
+     */
+
+    zmw_enter_critical_section(dev);
+    wd->aggInitiated = 0;
+    wd->addbaComplete = 0;
+    wd->addbaCount = 0;
+    wd->reorder = 1;
+    for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+    {
+        for (j=0; j<ZM_AC; j++)
+        {
+            //wd->aggSta[i].aggQNumber[j] = ZM_AGG_POOL_SIZE;
+            wd->aggSta[i].aggFlag[j] = wd->aggSta[i].count[j] = 0;
+            wd->aggSta[i].tid_tx[j] = NULL;
+            wd->aggSta[i].tid_tx[j+1] = NULL;
+
+        }
+    }
+
+    /*
+     * reset Tx/Rx aggregation queue information
+     */
+    wd->aggState = 0;
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        /*
+         * reset tx aggregation queue
+         */
+        wd->aggQPool[i] = zfwMemAllocate(dev, sizeof(struct aggQueue));
+        if(!wd->aggQPool[i])
+        {
+            zmw_leave_critical_section(dev);
+            return;
+        }
+        wd->aggQPool[i]->aggHead = wd->aggQPool[i]->aggTail =
+        wd->aggQPool[i]->aggQEnabled = wd->aggQPool[i]->aggReady =
+        wd->aggQPool[i]->clearFlag = wd->aggQPool[i]->deleteFlag = 0;
+        //wd->aggQPool[i]->aggSize = 16;
+
+        /*
+         * reset rx aggregation queue
+         */
+        wd->tid_rx[i] = zfwMemAllocate(dev, sizeof(struct agg_tid_rx));
+        if (!wd->tid_rx[i])
+        {
+            zmw_leave_critical_section(dev);
+            return;
+        }
+        wd->tid_rx[i]->aid = ZM_MAX_STA_SUPPORT;
+        wd->tid_rx[i]->seq_start = wd->tid_rx[i]->baw_head = \
+        wd->tid_rx[i]->baw_tail = 0;
+        wd->tid_rx[i]->sq_exceed_count = wd->tid_rx[i]->sq_behind_count = 0;
+        for (j=0; j<=ZM_AGG_BAW_SIZE; j++)
+            wd->tid_rx[i]->frame[j].buf = 0;
+        /*
+         * reset ADDBA exchange status code
+         * 0: NULL
+         * 1: ADDBA Request sent/received
+         * 2: ACK for ADDBA Request sent/received
+         * 3: ADDBA Response sent/received
+         * 4: ACK for ADDBA Response sent/received
+         */
+        wd->tid_rx[i]->addBaExchangeStatusCode = 0;
+
+    }
+    zmw_leave_critical_section(dev);
+    zfAggTallyReset(dev);
+    DESTQ.init = zfAggDestInit;
+    DESTQ.init(dev);
+    wd->aggInitiated = 1;
+    aggr_count = 0;
+    success_mpdu = 0;
+    total_mpdu = 0;
+#ifdef ZM_ENABLE_AGGREGATION
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+    BAW = zfwMemAllocate(dev, sizeof(struct baw_enabler));
+    if(!BAW)
+    {
+        return;
+    }
+    BAW->init = zfBawInit;
+    BAW->init(dev);
+#endif //disable BAW
+#endif
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggGetSta                 */
+/*      return STA AID.                                                 */
+/*      take buf as input, use the dest address of buf as index to      */
+/*      search STA AID.                                                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer for one particular packet                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      AID                                                             */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               ZyDAS Technology Corporation    2006.11     */
+/*                                                                      */
+/************************************************************************/
+
+
+
+u16_t zfAggGetSta(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t id;
+    u16_t dst[3];
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    dst[0] = zmw_rx_buf_readh(dev, buf, 0);
+    dst[1] = zmw_rx_buf_readh(dev, buf, 2);
+    dst[2] = zmw_rx_buf_readh(dev, buf, 4);
+
+    zmw_enter_critical_section(dev);
+
+    if(wd->wlanMode == ZM_MODE_AP) {
+        id = zfApFindSta(dev, dst);
+    }
+    else {
+        id = 0;
+    }
+    zmw_leave_critical_section(dev);
+
+#if ZM_AGG_FPGA_DEBUG
+    id = 0;
+#endif
+
+    return id;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxGetQueue             */
+/*      return Queue Pool index.                                        */
+/*      take aid as input, look for the queue index associated          */
+/*      with this aid.                                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      aid : associated id                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Queue number                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               ZyDAS Technology Corporation    2006.11     */
+/*                                                                      */
+/************************************************************************/
+TID_TX zfAggTxGetQueue(zdev_t* dev, u16_t aid, u16_t tid)
+{
+    //u16_t   i;
+    TID_TX  tid_tx;
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    /*
+     * not a STA aid
+     */
+    if (0xffff == aid)
+        return NULL;
+
+    //zmw_enter_critical_section(dev);
+
+    tid_tx = wd->aggSta[aid].tid_tx[tid];
+    if (!tid_tx) return NULL;
+    if (0 == tid_tx->aggQEnabled)
+        return NULL;
+
+    //zmw_leave_critical_section(dev);
+
+    return tid_tx;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxNewQueue             */
+/*      return Queue Pool index.                                        */
+/*      take aid as input, find a new queue for this aid.               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      aid : associated id                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Queue number                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               ZyDAS Technology Corporation    2006.12     */
+/*                                                                      */
+/************************************************************************/
+TID_TX zfAggTxNewQueue(zdev_t* dev, u16_t aid, u16_t tid, zbuf_t* buf)
+{
+    u16_t   i;
+    TID_TX  tid_tx=NULL;
+    u16_t   ac = zcUpToAc[tid&0x7] & 0x3;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /*
+     * not a STA aid
+     */
+    if (0xffff == aid)
+        return NULL;
+
+    zmw_enter_critical_section(dev);
+
+    /*
+     * find one new queue for sta
+     */
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (wd->aggQPool[i]->aggQEnabled)
+        {
+                /*
+                 * this q is enabled
+                 */
+        }
+        else
+        {
+            tid_tx = wd->aggQPool[i];
+            tid_tx->aggQEnabled = 1;
+            tid_tx->aggQSTA = aid;
+            tid_tx->ac = ac;
+            tid_tx->tid = tid;
+            tid_tx->aggHead = tid_tx->aggTail = tid_tx->size = 0;
+            tid_tx->aggReady = 0;
+            wd->aggSta[aid].tid_tx[tid] = tid_tx;
+            tid_tx->dst[0] = zmw_rx_buf_readh(dev, buf, 0);
+            tid_tx->dst[1] = zmw_rx_buf_readh(dev, buf, 2);
+            tid_tx->dst[2] = zmw_rx_buf_readh(dev, buf, 4);
+            break;
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return tid_tx;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxEnqueue              */
+/*      return Status code ZM_SUCCESS or error code                     */
+/*      take (aid,ac,qnum,buf) as input                                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      aid : associated id                                             */
+/*      ac  : access category                                           */
+/*      qnum: the queue number to which will be enqueued                */
+/*      buf : the packet to be queued                                   */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      status code                                                     */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxEnqueue(zdev_t* dev, zbuf_t* buf, u16_t aid, TID_TX tid_tx)
+{
+    //u16_t   qlen, frameLen;
+    u32_t   time;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+
+    if (tid_tx->size < (ZM_AGGQ_SIZE - 2))
+    {
+        /* Queue not full */
+
+
+        /*
+         * buffer copy
+         * in zfwBufFree will return a ndismsendcomplete
+         * to resolve the synchronize problem in aggregate
+         */
+
+        u8_t    sendComplete = 0;
+
+        tid_tx->aggvtxq[tid_tx->aggHead].buf = buf;
+        time = zm_agg_GetTime();
+        tid_tx->aggvtxq[tid_tx->aggHead].arrivalTime = time;
+        tid_tx->aggvtxq[tid_tx->aggHead].baw_retransmit = 0;
+
+        tid_tx->aggHead = ((tid_tx->aggHead + 1) & ZM_AGGQ_SIZE_MASK);
+        tid_tx->lastArrival = time;
+        tid_tx->size++;
+        tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+        if (buf && (tid_tx->size < (ZM_AGGQ_SIZE - 10))) {
+            tid_tx->complete = tid_tx->aggHead;
+            sendComplete = 1;
+        }
+        zmw_leave_critical_section(dev);
+
+        if (!DESTQ.exist(dev, 0, tid_tx->ac, tid_tx, NULL)) {
+            DESTQ.insert(dev, 0, tid_tx->ac, tid_tx, NULL);
+        }
+
+        zm_msg1_agg(ZM_LV_0, "tid_tx->size=", tid_tx->size);
+        //zm_debug_msg1("tid_tx->size=", tid_tx->size);
+
+        if (buf && sendComplete && wd->zfcbSendCompleteIndication) {
+            //zmw_leave_critical_section(dev);
+            wd->zfcbSendCompleteIndication(dev, buf);
+        }
+
+        /*if (tid_tx->size >= 16 && zfHpGetFreeTxdCount(dev) > 20)
+            zfAggTxSend(dev, zfHpGetFreeTxdCount(dev), tid_tx);
+        */
+        return ZM_SUCCESS;
+    }
+    else
+    {
+        zm_msg1_agg(ZM_LV_0, "can't enqueue, tid_tx->size=", tid_tx->size);
+        /*
+         * Queue Full
+         */
+
+        /*
+         * zm_msg1_agg(ZM_LV_0, "Queue full, qnum = ", qnum);
+         * wd->commTally.txQosDropCount[ac]++;
+         * zfwBufFree(dev, buf, ZM_SUCCESS);
+         * zm_msg1_agg(ZM_LV_1, "Packet discarded, VTXQ full, ac=", ac);
+         *
+         * return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
+         */
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (!DESTQ.exist(dev, 0, tid_tx->ac, tid_tx, NULL)) {
+            DESTQ.insert(dev, 0, tid_tx->ac, tid_tx, NULL);
+    }
+
+    return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
+}
+
+u16_t    zfAggDestExist(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq) {
+    struct dest* dest;
+    u16_t   exist = 0;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (!DESTQ.Head[ac]) {
+        exist = 0;
+    }
+    else {
+        dest = DESTQ.Head[ac];
+        if (dest->tid_tx == tid_tx) {
+            exist = 1;
+        }
+        else {
+            while (dest->next != DESTQ.Head[ac]) {
+                dest = dest->next;
+                if (dest->tid_tx == tid_tx){
+                    exist = 1;
+                    break;
+                }
+            }
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return exist;
+}
+
+void    zfAggDestInsert(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq)
+{
+    struct dest* new_dest;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    new_dest = zfwMemAllocate(dev, sizeof(struct dest));
+    if(!new_dest)
+    {
+        return;
+    }
+    new_dest->Qtype = Qtype;
+    new_dest->tid_tx = tid_tx;
+    if (0 == Qtype)
+        new_dest->tid_tx = tid_tx;
+    else
+        new_dest->vtxq = vtxq;
+    if (!DESTQ.Head[ac]) {
+
+        zmw_enter_critical_section(dev);
+        new_dest->next = new_dest;
+        DESTQ.Head[ac] = DESTQ.dest[ac] = new_dest;
+        zmw_leave_critical_section(dev);
+    }
+    else {
+
+        zmw_enter_critical_section(dev);
+        new_dest->next = DESTQ.dest[ac]->next;
+        DESTQ.dest[ac]->next = new_dest;
+        zmw_leave_critical_section(dev);
+    }
+
+
+    //DESTQ.size[ac]++;
+    return;
+}
+
+void    zfAggDestDelete(zdev_t* dev, u16_t Qtype, TID_TX tid_tx, void* vtxq)
+{
+    struct dest* dest, *temp;
+    u16_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (wd->destLock) {
+        zmw_leave_critical_section(dev);
+        return;
+    }
+
+
+    //zmw_declare_for_critical_section();
+    for (i=0; i<4; i++) {
+        if (!DESTQ.Head[i]) continue;
+        dest = DESTQ.Head[i];
+        if (!dest) continue;
+
+
+        while (dest && (dest->next != DESTQ.Head[i])) {
+            if (Qtype == 0 && dest->next->tid_tx == tid_tx){
+                break;
+            }
+            if (Qtype == 1 && dest->next->vtxq == vtxq) {
+                break;
+            }
+            dest = dest->next;
+        }
+
+        if ((Qtype == 0 && dest->next->tid_tx == tid_tx) || (Qtype == 1 && dest->next->vtxq == vtxq)) {
+
+            tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+            if (tid_tx->size) {
+                zmw_leave_critical_section(dev);
+                return;
+            }
+            if (!DESTQ.Head[i]) {
+                temp = NULL;
+            }
+            else {
+                temp = dest->next;
+                if (temp == dest) {
+                    DESTQ.Head[i] = DESTQ.dest[i] = NULL;
+                    //DESTQ.size[i] = 0;
+                }
+                else {
+                    dest->next = dest->next->next;
+                }
+            }
+
+            if (temp == NULL)
+                {/* do nothing */} //zfwMemFree(dev, temp, sizeof(struct dest));
+            else
+                zfwMemFree(dev, temp, sizeof(struct dest));
+
+            /*zmw_enter_critical_section(dev);
+            if (DESTQ.size[i] > 0)
+                DESTQ.size[i]--;
+            zmw_leave_critical_section(dev);
+            */
+        }
+
+    }
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+void    zfAggDestInit(zdev_t* dev)
+{
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    for (i=0; i<4; i++) {
+        //wd->destQ.Head[i].next = wd->destQ.Head[i];
+        //wd->destQ.dest[i] = wd->destQ.Head[i];
+        //DESTQ.size[i] = 0;
+        DESTQ.Head[i] = NULL;
+    }
+    DESTQ.insert  = zfAggDestInsert;
+    DESTQ.delete  = zfAggDestDelete;
+    DESTQ.init    = zfAggDestInit;
+    DESTQ.getNext = zfAggDestGetNext;
+    DESTQ.exist   = zfAggDestExist;
+    DESTQ.ppri = 0;
+    return;
+}
+
+struct dest* zfAggDestGetNext(zdev_t* dev, u16_t ac)
+{
+    struct dest *dest = NULL;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (DESTQ.dest[ac]) {
+        dest = DESTQ.dest[ac];
+        DESTQ.dest[ac] = DESTQ.dest[ac]->next;
+    }
+    else {
+        dest = NULL;
+    }
+    zmw_leave_critical_section(dev);
+
+    return dest;
+}
+
+#ifdef ZM_ENABLE_AGGREGATION
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+u16_t   zfAggTidTxInsertHead(zdev_t* dev, struct bufInfo *buf_info,TID_TX tid_tx)
+{
+    zbuf_t* buf;
+    u32_t time;
+    struct baw_header *baw_header;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+
+    buf = buf_info->buf;
+
+    zmw_enter_critical_section(dev);
+    tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+    zmw_leave_critical_section(dev);
+
+    if (tid_tx->size >= (ZM_AGGQ_SIZE - 2)) {
+        zfwBufFree(dev, buf, ZM_SUCCESS);
+        return 0;
+    }
+
+    zmw_enter_critical_section(dev);
+    tid_tx->aggTail = (tid_tx->aggTail == 0)? ZM_AGGQ_SIZE_MASK: tid_tx->aggTail - 1;
+    tid_tx->aggvtxq[tid_tx->aggTail].buf = buf;
+    //time = zm_agg_GetTime();
+    tid_tx->aggvtxq[tid_tx->aggTail].arrivalTime = buf_info->timestamp;
+    tid_tx->aggvtxq[tid_tx->aggTail].baw_retransmit = buf_info->baw_retransmit;
+
+    baw_header = &tid_tx->aggvtxq[tid_tx->aggTail].baw_header;
+    baw_header->headerLen   = buf_info->baw_header->headerLen;
+    baw_header->micLen      = buf_info->baw_header->micLen;
+    baw_header->snapLen     = buf_info->baw_header->snapLen;
+    baw_header->removeLen   = buf_info->baw_header->removeLen;
+    baw_header->keyIdx      = buf_info->baw_header->keyIdx;
+    zfwMemoryCopy((u8_t *)baw_header->header, (u8_t *)buf_info->baw_header->header, 58);
+    zfwMemoryCopy((u8_t *)baw_header->mic   , (u8_t *)buf_info->baw_header->mic   , 8);
+    zfwMemoryCopy((u8_t *)baw_header->snap  , (u8_t *)buf_info->baw_header->snap  , 8);
+
+    tid_tx->size++;
+    tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+    zmw_leave_critical_section(dev);
+
+    //tid_tx->lastArrival = time;
+    if (1 == tid_tx->size) {
+        DESTQ.insert(dev, 0, tid_tx->ac, tid_tx, NULL);
+    }
+
+
+    zm_msg1_agg(ZM_LV_0, "0xC2:insertHead, tid_tx->size=", tid_tx->size);
+
+    return TRUE;
+}
+#endif //disable BAW
+#endif
+
+void    zfiTxComplete(zdev_t* dev)
+{
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    if( (wd->wlanMode == ZM_MODE_AP) ||
+        (wd->wlanMode == ZM_MODE_INFRASTRUCTURE && wd->sta.EnableHT) ||
+        (wd->wlanMode == ZM_MODE_PSEUDO) ) {
+        zfAggTxScheduler(dev, 0);
+    }
+
+    return;
+}
+
+TID_TX  zfAggTxReady(zdev_t* dev) {
+    //struct dest* dest;
+    u16_t   i;
+    TID_TX  tid_tx = NULL;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (wd->aggQPool[i]->aggQEnabled)
+        {
+            if (wd->aggQPool[i]->size >= 16) {
+                tid_tx = wd->aggQPool[i];
+                break;
+            }
+        }
+        else {
+        }
+    }
+    zmw_leave_critical_section(dev);
+    return tid_tx;
+}
+
+u16_t   zfAggValidTidTx(zdev_t* dev, TID_TX tid_tx) {
+    u16_t   i, valid = 0;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (wd->aggQPool[i] == tid_tx)
+        {
+            valid = 1;
+            break;
+        }
+        else {
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+    return valid;
+}
+
+void    zfAggTxScheduler(zdev_t* dev, u8_t ScanAndClear)
+{
+    TID_TX  tid_tx = NULL;
+    void*   vtxq;
+    struct dest* dest;
+    zbuf_t*  buf;
+    u32_t txql, min_txql;
+    //u16_t aggr_size = 1;
+    u16_t txq_threshold;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if (!wd->aggInitiated)
+    {
+        return;
+    }
+
+    /* debug */
+    txql = TXQL;
+    min_txql = AGG_MIN_TXQL;
+
+    if(wd->txq_threshold)
+        txq_threshold = wd->txq_threshold;
+    else
+        txq_threshold = AGG_MIN_TXQL;
+
+    tid_tx = zfAggTxReady(dev);
+    if (tid_tx) ScanAndClear = 0;
+    while (zfHpGetFreeTxdCount(dev) > 20 && (TXQL < txq_threshold || tid_tx)) {
+    //while (zfHpGetFreeTxdCount(dev) > 20 && (ScanAndClear || tid_tx)) {
+    //while (TXQL < txq_threshold) {
+        u16_t i;
+        u8_t ac;
+        s8_t destQ_count = 0;
+    //while ((zfHpGetFreeTxdCount(dev)) > 32) {
+
+        //DbgPrint("zfAggTxScheduler: in while loop");
+        for (i=0; i<4; i++) {
+            if (DESTQ.Head[i]) destQ_count++;
+        }
+        if (0 >= destQ_count) break;
+
+        zmw_enter_critical_section(dev);
+        ac = pri[DESTQ.ppri]; DESTQ.ppri = (DESTQ.ppri + 1) % 10;
+        zmw_leave_critical_section(dev);
+
+        for (i=0; i<10; i++){
+            if(DESTQ.Head[ac]) break;
+
+            zmw_enter_critical_section(dev);
+            ac = pri[DESTQ.ppri]; DESTQ.ppri = (DESTQ.ppri + 1) % 10;
+            zmw_leave_critical_section(dev);
+        }
+        if (i == 10) break;
+        //DbgPrint("zfAggTxScheduler: have dest Q");
+        zmw_enter_critical_section(dev);
+        wd->destLock = 1;
+        zmw_leave_critical_section(dev);
+
+        dest = DESTQ.getNext(dev, ac);
+        if (!dest) {
+            zmw_enter_critical_section(dev);
+            wd->destLock = 0;
+            zmw_leave_critical_section(dev);
+
+            DbgPrint("bug report! DESTQ.getNext got nothing!");
+            break;
+        }
+        if (dest->Qtype == 0) {
+            tid_tx = dest->tid_tx;
+
+            //DbgPrint("zfAggTxScheduler: have tid_tx Q");
+
+            if(tid_tx && zfAggValidTidTx(dev, tid_tx))
+                tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+            else {
+                zmw_enter_critical_section(dev);
+                wd->destLock = 0;
+                zmw_leave_critical_section(dev);
+
+                tid_tx = zfAggTxReady(dev);
+                continue;
+            }
+
+            zmw_enter_critical_section(dev);
+            wd->destLock = 0;
+            zmw_leave_critical_section(dev);
+            //zmw_enter_critical_section(dev);
+            if (tid_tx && !tid_tx->size) {
+
+                //zmw_leave_critical_section(dev);
+                //DESTQ.delete(dev, 0, tid_tx, NULL);
+            }
+            else if(wd->aggState == 0){
+                //wd->aggState = 1;
+                //zmw_leave_critical_section(dev);
+                zfAggTxSend(dev, zfHpGetFreeTxdCount(dev), tid_tx);
+                //wd->aggState = 0;
+            }
+            else {
+                //zmw_leave_critical_section(dev);
+                break;
+            }
+        }
+        else {
+            vtxq = dest->vtxq;
+            buf = zfGetVtxq(dev, ac);
+            zm_assert( buf != 0 );
+
+            zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+
+        }
+        /*flush all but < 16 frames in tid_tx to TXQ*/
+        tid_tx = zfAggTxReady(dev);
+    }
+
+    /*while ((zfHpGetFreeTxdCount(dev)) > 32) {
+    //while ((zfHpGetFreeTxdCount(dev)) > 32) {
+
+        destQ_count = 0;
+        for (i=0; i<4; i++) destQ_count += wd->destQ.size[i];
+        if (0 >= destQ_count) break;
+
+        ac = pri[wd->destQ.ppri]; wd->destQ.ppri = (wd->destQ.ppri + 1) % 10;
+        for (i=0; i<10; i++){
+            if(wd->destQ.size[ac]!=0) break;
+            ac = pri[wd->destQ.ppri]; wd->destQ.ppri = (wd->destQ.ppri + 1) % 10;
+        }
+        if (i == 10) break;
+        dest = wd->destQ.getNext(dev, ac);
+        if (dest->Qtype == 0) {
+            tid_tx = dest->tid_tx;
+            tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+            if (!tid_tx->size) {
+                wd->destQ.delete(dev, 0, tid_tx, NULL);
+                break;
+            }
+            else if((wd->aggState == 0) && (tid_tx->size >= 16)){
+                zfAggTxSend(dev, zfHpGetFreeTxdCount(dev), tid_tx);
+            }
+            else {
+                break;
+            }
+        }
+
+    }
+    */
+    return;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTx                     */
+/*      return Status code ZM_SUCCESS or error code                     */
+/*      management A-MPDU aggregation function,                         */
+/*      management aggregation queue, calculate arrivalrate,            */
+/*      add/delete an aggregation queue of a stream,                    */
+/*      enqueue packets into responsible aggregate queue.               */
+/*      take (dev, buf, ac) as input                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : packet buff                                               */
+/*      ac  : access category                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      status code                                                     */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTx(zdev_t* dev, zbuf_t* buf, u16_t tid)
+{
+    u16_t aid;
+    //u16_t qnum;
+    //u16_t aggflag = 0;
+    //u16_t arrivalrate = 0;
+    TID_TX tid_tx;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if(!wd->aggInitiated)
+    {
+        return ZM_ERR_TX_BUFFER_UNAVAILABLE;
+    }
+
+    aid = zfAggGetSta(dev, buf);
+
+    //arrivalrate = zfAggTxArrivalRate(dev, aid, tid);
+
+    if (0xffff == aid)
+    {
+        /*
+         * STA not associated, this is a BC/MC or STA->AP packet
+         */
+
+        return ZM_ERR_TX_BUFFER_UNAVAILABLE;
+    }
+
+    /*
+     * STA associated, a unicast packet
+     */
+
+    tid_tx = zfAggTxGetQueue(dev, aid, tid);
+
+    /*tid_q.tid_tx = tid_tx;
+    wd->destQ.insert = zfAggDestInsert;
+    wd->destQ.insert(dev, 0, tid_q);
+    */
+    if (tid_tx != NULL)
+    {
+        /*
+         * this (aid, ac) is aggregated
+         */
+
+        //if (arrivalrate < ZM_AGG_LOW_THRESHOLD)
+        if (0)
+        {
+            /*
+             * arrival rate too low
+             * delete this aggregate queue
+             */
+
+            zmw_enter_critical_section(dev);
+
+            //wd->aggQPool[qnum]->clearFlag = wd->aggQPool[qnum]->deleteFlag =1;
+
+            zmw_leave_critical_section(dev);
+
+        }
+
+        return zfAggTxEnqueue(dev, buf, aid, tid_tx);
+
+    }
+    else
+    {
+        /*
+         * this (aid, ac) not yet aggregated
+         * queue not found
+         */
+
+        //if (arrivalrate > ZM_AGG_HIGH_THRESHOLD)
+        if (1)
+        {
+            /*
+             * arrivalrate high enough to get a new agg queue
+             */
+
+            tid_tx = zfAggTxNewQueue(dev, aid, tid, buf);
+
+            //zm_msg1_agg(ZM_LV_0, "get new AggQueue qnum = ", tid_tx->);
+
+            if (tid_tx)
+            {
+                /*
+                 * got a new aggregate queue
+                 */
+
+                //zmw_enter_critical_section(dev);
+
+                //wd->aggSta[aid].aggFlag[ac] = 1;
+
+                //zmw_leave_critical_section(dev);
+
+                /*
+                 * add ADDBA functions here
+                 * return ZM_ERR_TX_BUFFER_UNAVAILABLE;
+                 */
+
+
+                //zfAggSendAddbaRequest(dev, tid_tx->dst, tid_tx->ac, tid_tx->tid);
+                //zmw_enter_critical_section(dev);
+
+                //wd->aggSta[aid].aggFlag[ac] = 0;
+
+                //zmw_leave_critical_section(dev);
+
+                return zfAggTxEnqueue(dev, buf, aid, tid_tx);
+
+            }
+            else
+            {
+                /*
+                 * just can't get a new aggregate queue
+                 */
+
+                return ZM_ERR_TX_BUFFER_UNAVAILABLE;
+            }
+        }
+        else
+        {
+            /*
+             * arrival rate is not high enough to get a new agg queue
+             */
+
+            return ZM_ERR_TX_BUFFER_UNAVAILABLE;
+        }
+    }
+
+
+
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxReadyCount           */
+/*      return counter of ready to aggregate queues.                    */
+/*      take (dev, ac) as input, only calculate the ready to aggregate  */
+/*      queues of one particular ac.                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      ac  : access category                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      counter of ready to aggregate queues                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxReadyCount(zdev_t* dev, u16_t ac)
+{
+    u16_t i;
+    u16_t readycount = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    for (i=0 ; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (wd->aggQPool[i]->aggQEnabled && (wd->aggQPool[i]->aggReady || \
+                wd->aggQPool[i]->clearFlag) && ac == wd->aggQPool[i]->ac)
+            readycount++;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return readycount;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxPartial              */
+/*      return the number that Vtxq has to send.                        */
+/*      take (dev, ac, readycount) as input, calculate the ratio of     */
+/*      Vtxq length to (Vtxq length + readycount) of a particular ac,   */
+/*      and returns the Vtxq length * the ratio                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      ac  : access category                                           */
+/*      readycount: the number of ready to aggregate queues of this ac  */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Vtxq length * ratio                                             */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxPartial(zdev_t* dev, u16_t ac, u16_t readycount)
+{
+    u16_t qlen;
+    u16_t partial;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    qlen = zm_agg_qlen(dev, wd->vtxqHead[ac], wd->vtxqTail[ac]);
+
+    if ((qlen + readycount) > 0)
+    {
+        partial = (u16_t)( zm_agg_weight(ac) * ((u16_t)qlen/(qlen + \
+                        readycount)) );
+    }
+    else
+    {
+        partial = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (partial > qlen)
+        partial = qlen;
+
+    return partial;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxSend                 */
+/*      return sentcount                                                */
+/*      take (dev, ac, n) as input, n is the number of scheduled agg    */
+/*      queues to be sent of the particular ac.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      ac  : access category                                           */
+/*      n   : the number of scheduled aggregation queues to be sent     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      sentcount                                                       */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxSend(zdev_t* dev, u32_t freeTxd, TID_TX tid_tx)
+{
+    //u16_t   qnum;
+    //u16_t   qlen;
+    u16_t   j;
+    //u16_t   sentcount = 0;
+    zbuf_t* buf;
+    struct  aggControl aggControl;
+    u16_t   aggLen;
+    //zbuf_t*  newBuf;
+    //u16_t   bufLen;
+    //TID_BAW tid_baw = NULL;
+    //struct bufInfo *buf_info;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    //while (tid_tx->size > 0)
+
+    zmw_enter_critical_section(dev);
+    tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+    aggLen = zm_agg_min(16, zm_agg_min(tid_tx->size, (u16_t)(freeTxd - 2)));
+    zmw_leave_critical_section(dev);
+
+            /*
+             * why there have to be 2 free Txd?
+             */
+    if (aggLen <=0 )
+        return 0;
+
+
+    if (aggLen == 1) {
+        buf = zfAggTxGetVtxq(dev, tid_tx);
+        if (buf)
+            zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+        if (tid_tx->size == 0) {
+            //DESTQ.delete(dev, 0, tid_tx, NULL);
+        }
+
+        return 1;
+    }
+                /*
+                 * Free Txd queue is big enough to put aggregation
+                 */
+    zmw_enter_critical_section(dev);
+    if (wd->aggState == 1) {
+        zmw_leave_critical_section(dev);
+        return 0;
+    }
+    wd->aggState = 1;
+    zmw_leave_critical_section(dev);
+
+
+    zm_msg1_agg(ZM_LV_0, "aggLen=", aggLen);
+    tid_tx->aggFrameSize = 0;
+    for (j=0; j < aggLen; j++) {
+        buf = zfAggTxGetVtxq(dev, tid_tx);
+
+        zmw_enter_critical_section(dev);
+        tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+        zmw_leave_critical_section(dev);
+
+        if ( buf ) {
+            //struct aggTally *agg_tal;
+            u16_t completeIndex;
+
+            if (0 == j) {
+                aggControl.ampduIndication = ZM_AGG_FIRST_MPDU;
+
+            }
+            else if ((j == (aggLen - 1)) || tid_tx->size == 0)
+            {
+                aggControl.ampduIndication = ZM_AGG_LAST_MPDU;
+                //wd->aggState = 0;
+
+            }
+            else
+            {
+                aggControl.ampduIndication = ZM_AGG_MIDDLE_MPDU;
+                /* the packet is delayed more than 500 ms, drop it */
+
+            }
+            tid_tx->aggFrameSize += zfwBufGetSize(dev, buf);
+            aggControl.addbaIndication = 0;
+            aggControl.aggEnabled = 1;
+
+#ifdef ZM_AGG_TALLY
+            agg_tal = &wd->agg_tal;
+            agg_tal->sent_packets_sum++;
+
+#endif
+
+            zfAggTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0, &aggControl, tid_tx);
+
+            zmw_enter_critical_section(dev);
+            completeIndex = tid_tx->complete;
+            if(zm_agg_inQ(tid_tx, tid_tx->complete))
+                zm_agg_plus(tid_tx->complete);
+            zmw_leave_critical_section(dev);
+
+            if(zm_agg_inQ(tid_tx, completeIndex) && wd->zfcbSendCompleteIndication
+                    && tid_tx->aggvtxq[completeIndex].buf) {
+                wd->zfcbSendCompleteIndication(dev, tid_tx->aggvtxq[completeIndex].buf);
+                zm_debug_msg0("in queue complete worked!");
+            }
+
+        }
+        else {
+            /*
+             * this aggregation queue is empty
+             */
+            zm_msg1_agg(ZM_LV_0, "aggLen not reached, but no more frame, j=", j);
+
+            break;
+        }
+    }
+    zmw_enter_critical_section(dev);
+    wd->aggState = 0;
+    zmw_leave_critical_section(dev);
+
+    //zm_acquire_agg_spin_lock(Adapter);
+    tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+    //zm_release_agg_spin_lock(Adapter);
+
+    if (tid_tx->size == 0) {
+        //DESTQ.delete(dev, 0, tid_tx, NULL);
+    }
+
+
+
+    //zfAggInvokeBar(dev, tid_tx);
+    if(j>0) {
+        aggr_count++;
+        zm_msg1_agg(ZM_LV_0, "0xC2:sent 1 aggr, aggr_count=", aggr_count);
+        zm_msg1_agg(ZM_LV_0, "0xC2:sent 1 aggr, aggr_size=", j);
+    }
+    return j;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxGetReadyQueue        */
+/*      return the number of the aggregation queue                      */
+/*      take (dev, ac) as input, find the agg queue with smallest       */
+/*      arrival time (waited longest) among those ready or clearFlag    */
+/*      set queues.                                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      ac  : access category                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      aggregation queue number                                        */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+TID_TX zfAggTxGetReadyQueue(zdev_t* dev, u16_t ac)
+{
+    //u16_t       qnum = ZM_AGG_POOL_SIZE;
+    u16_t       i;
+    u32_t       time = 0;
+    TID_TX      tid_tx = NULL;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    for (i=0 ;i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (1 == wd->aggQPool[i]->aggQEnabled && ac == wd->aggQPool[i]->ac &&
+                (wd->aggQPool[i]->size > 0))
+        {
+            if (0 == time || time > wd->aggQPool[i]->aggvtxq[ \
+                            wd->aggQPool[i]->aggHead ].arrivalTime)
+            {
+                tid_tx = wd->aggQPool[i];
+                time = tid_tx->aggvtxq[ tid_tx->aggHead ].arrivalTime;
+            }
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return tid_tx;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxGetVtxq              */
+/*      return an MSDU                                                  */
+/*      take (dev, qnum) as input, return an MSDU out of the agg queue. */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      qnum: queue number                                              */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      a MSDU                                                          */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+zbuf_t* zfAggTxGetVtxq(zdev_t* dev, TID_TX tid_tx)
+{
+    zbuf_t* buf = NULL;
+
+    zmw_declare_for_critical_section();
+
+    if (tid_tx->aggHead != tid_tx->aggTail)
+    {
+        buf = tid_tx->aggvtxq[ tid_tx->aggTail ].buf;
+
+        tid_tx->aggvtxq[tid_tx->aggTail].buf = NULL;
+
+        zmw_enter_critical_section(dev);
+        tid_tx->aggTail = ((tid_tx->aggTail + 1) & ZM_AGGQ_SIZE_MASK);
+        if(tid_tx->size > 0) tid_tx->size--;
+        tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+        if (NULL == buf) {
+            //tid_tx->aggTail = tid_tx->aggHead = tid_tx->size = 0;
+            //zm_msg1_agg(ZM_LV_0, "GetVtxq buf == NULL, tid_tx->size=", tid_tx->size);
+        }
+        zmw_leave_critical_section(dev);
+    }
+    else
+    {
+        /*
+         * queue is empty
+         */
+        zm_msg1_agg(ZM_LV_0, "tid_tx->aggHead == tid_tx->aggTail, tid_tx->size=", tid_tx->size);
+
+    }
+
+    if (zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail) != tid_tx->size)
+        zm_msg1_agg(ZM_LV_0, "qlen!=tid_tx->size! tid_tx->size=", tid_tx->size);
+    return buf;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxDeleteQueue          */
+/*      return ZM_SUCCESS (can't fail)                                  */
+/*      take (dev, qnum) as input, reset (delete) this aggregate queue, */
+/*      this queue is virtually returned to the aggregate queue pool.   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      qnum: queue number                                              */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_SUCCESS                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxDeleteQueue(zdev_t* dev, u16_t qnum)
+{
+    u16_t ac, tid;
+    struct aggQueue *tx_tid;
+    struct aggSta   *agg_sta;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    tx_tid = wd->aggQPool[qnum];
+    agg_sta = &wd->aggSta[tx_tid->aggQSTA];
+    ac = tx_tid->ac;
+    tid = tx_tid->tid;
+
+    zmw_enter_critical_section(dev);
+
+    tx_tid->aggQEnabled = 0;
+    tx_tid->aggHead = tx_tid->aggTail = 0;
+    tx_tid->aggReady = 0;
+    tx_tid->clearFlag = tx_tid->deleteFlag = 0;
+    tx_tid->size = 0;
+    agg_sta->count[ac] = 0;
+
+    agg_sta->tid_tx[tid] = NULL;
+    agg_sta->aggFlag[ac] = 0;
+
+    zmw_leave_critical_section(dev);
+
+    zm_msg1_agg(ZM_LV_0, "queue deleted! qnum=", qnum);
+
+    return ZM_SUCCESS;
+}
+
+#ifdef ZM_ENABLE_AGGREGATION
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+void zfBawCore(zdev_t* dev, u16_t baw_seq, u32_t bitmap, u16_t aggLen) {
+    TID_BAW tid_baw;
+    s16_t i;
+    zbuf_t* buf;
+    struct bufInfo *buf_info;
+
+    zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+    tid_baw = BAW->getQ(dev, baw_seq);
+    //tid_baw = NULL;
+    if (NULL == tid_baw)
+        return;
+
+    total_mpdu += aggLen;
+    for (i = aggLen - 1; i>=0; i--) {
+        if (((bitmap >> i) & 0x1) == 0) {
+            buf_info = BAW->pop(dev, i, tid_baw);
+            buf = buf_info->buf;
+            if (buf) {
+                //wd->zfcbSetBawQ(dev, buf, 0);
+                zfAggTidTxInsertHead(dev, buf_info, tid_baw->tid_tx);
+            }
+        }
+        else {
+            success_mpdu++;
+        }
+    }
+    BAW->disable(dev, tid_baw);
+    zfAggTxScheduler(dev);
+    zm_debug_msg1("success_mpdu = ", success_mpdu);
+    zm_debug_msg1("  total_mpdu = ", total_mpdu);
+}
+
+void    zfBawInit(zdev_t* dev) {
+    TID_BAW tid_baw;
+    u16_t i,j;
+    zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+
+    for (i=0; i<ZM_BAW_POOL_SIZE; i++){
+        tid_baw = &BAW->tid_baw[i];
+        for (j=0; j<ZM_VTXQ_SIZE; j++) {
+            tid_baw->frame[j].buf = NULL;
+        }
+        tid_baw->enabled = tid_baw->head = tid_baw->tail = tid_baw->size = 0;
+        tid_baw->start_seq = 0;
+    }
+    BAW->delPoint = 0;
+    BAW->core = zfBawCore;
+    BAW->getNewQ = zfBawGetNewQ;
+    BAW->insert = zfBawInsert;
+    BAW->pop = zfBawPop;
+    BAW->enable = zfBawEnable;
+    BAW->disable = zfBawDisable;
+    BAW->getQ = zfBawGetQ;
+}
+
+
+
+TID_BAW zfBawGetNewQ(zdev_t* dev, u16_t start_seq, TID_TX tid_tx) {
+    TID_BAW tid_baw=NULL;
+    TID_BAW next_baw=NULL;
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+
+    /*
+    for (i=0; i<ZM_BAW_POOL_SIZE; i++){
+        tid_baw = &BAW->tid_baw[i];
+        if (FALSE == tid_baw->enabled)
+            break;
+    }
+    */
+
+    tid_baw = &BAW->tid_baw[BAW->delPoint];
+    i = BAW->delPoint;
+    //if (ZM_BAW_POOL_SIZE == i) {
+        //return NULL;
+    //    u8_t temp = BAW->delPoint;
+    //    tid_baw = &BAW->tid_baw[BAW->delPoint];
+    //    BAW->disable(dev, tid_baw);
+    //    BAW->delPoint = (BAW->delPoint < (ZM_BAW_POOL_SIZE - 1))? (BAW->delPoint + 1): 0;
+    //    temp = BAW->delPoint;
+    //}
+
+    zm_msg1_agg(ZM_LV_0, "get new tid_baw, index=", i);
+    BAW->delPoint = (i < (ZM_BAW_POOL_SIZE -1))? (i + 1): 0;
+    next_baw = &BAW->tid_baw[BAW->delPoint];
+    if (1 == next_baw->enabled) BAW->disable(dev, next_baw);
+
+    BAW->enable(dev, tid_baw, start_seq);
+    tid_baw->tid_tx = tid_tx;
+
+    return tid_baw;
+}
+
+u16_t   zfBawInsert(zdev_t* dev, zbuf_t* buf, u16_t baw_seq, TID_BAW tid_baw, u8_t baw_retransmit, struct baw_header_r *header_r) {
+    //TID_BAW tid_baw;
+    //u16_t   bufLen;
+
+    //zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+
+    if(tid_baw->size < (ZM_VTXQ_SIZE - 1)) {
+        struct baw_header *baw_header = &tid_baw->frame[tid_baw->head].baw_header;
+
+        baw_header->headerLen   = header_r->headerLen;
+        baw_header->micLen      = header_r->micLen;
+        baw_header->snapLen     = header_r->snapLen;
+        baw_header->removeLen   = header_r->removeLen;
+        baw_header->keyIdx      = header_r->keyIdx;
+        zfwMemoryCopy((u8_t *)baw_header->header, (u8_t *)header_r->header, 58);
+        zfwMemoryCopy((u8_t *)baw_header->mic   , (u8_t *)header_r->mic   , 8);
+        zfwMemoryCopy((u8_t *)baw_header->snap  , (u8_t *)header_r->snap  , 8);
+        //wd->zfcbSetBawQ(dev, buf, 1);
+        tid_baw->frame[tid_baw->head].buf = buf;
+        tid_baw->frame[tid_baw->head].baw_seq = baw_seq;
+        tid_baw->frame[tid_baw->head].baw_retransmit = baw_retransmit + 1;
+
+        //tid_baw->frame[tid_baw->head].data = pBuf->data;
+        tid_baw->head++;
+        tid_baw->size++;
+    }
+    else {
+        //wd->zfcbSetBawQ(dev, buf, 0);
+        zfwBufFree(dev, buf, ZM_SUCCESS);
+        return FALSE;
+    }
+    return TRUE;
+}
+
+struct bufInfo* zfBawPop(zdev_t* dev, u16_t index, TID_BAW tid_baw) {
+    //TID_BAW tid_baw;
+    //zbuf_t* buf;
+    struct bufInfo *buf_info;
+    zmw_get_wlan_dev(dev);
+
+    buf_info = &wd->buf_info;
+    buf_info->baw_header = NULL;
+
+    if (NULL == (buf_info->buf = tid_baw->frame[index].buf))
+        return buf_info;
+
+    buf_info->baw_retransmit = tid_baw->frame[index].baw_retransmit;
+    buf_info->baw_header = &tid_baw->frame[index].baw_header;
+    buf_info->timestamp = tid_baw->frame[index].timestamp;
+    //pBuf->data = pBuf->buffer;
+    //wd->zfcbRestoreBufData(dev, buf);
+    tid_baw->frame[index].buf = NULL;
+
+    return buf_info;
+}
+
+void    zfBawEnable(zdev_t* dev, TID_BAW tid_baw, u16_t start_seq) {
+    //TID_BAW tid_baw;
+
+    //zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+
+    tid_baw->enabled = TRUE;
+    tid_baw->head = tid_baw->tail = tid_baw->size = 0;
+    tid_baw->start_seq = start_seq;
+}
+
+void    zfBawDisable(zdev_t* dev, TID_BAW tid_baw) {
+    //TID_BAW tid_baw;
+    u16_t i;
+
+    //zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+    for (i=0; i<ZM_VTXQ_SIZE; i++) {
+        if (tid_baw->frame[i].buf) {
+
+            //wd->zfcbSetBawQ(dev, tid_baw->frame[i].buf, 0);
+            zfwBufFree(dev, tid_baw->frame[i].buf, ZM_SUCCESS);
+            tid_baw->frame[i].buf = NULL;
+        }
+    }
+
+    tid_baw->enabled = FALSE;
+}
+
+TID_BAW zfBawGetQ(zdev_t* dev, u16_t baw_seq) {
+    TID_BAW tid_baw=NULL;
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+    for (i=0; i<ZM_BAW_POOL_SIZE; i++){
+        tid_baw = &BAW->tid_baw[i];
+        if (TRUE == tid_baw->enabled)
+        {
+            zm_msg1_agg(ZM_LV_0, "get an old tid_baw, baw_seq=", baw_seq);
+            zm_msg1_agg(ZM_LV_0, "check a  tid_baw->start_seq=", tid_baw->start_seq);
+            if(baw_seq == tid_baw->start_seq)
+                break;
+        }
+
+    }
+    if (ZM_BAW_POOL_SIZE == i)
+        return NULL;
+    return tid_baw;
+}
+#endif //disable BAW
+#endif
+
+u16_t zfAggTallyReset(zdev_t* dev)
+{
+    struct aggTally* agg_tal;
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    agg_tal = &wd->agg_tal;
+    agg_tal->got_packets_sum = 0;
+    agg_tal->got_bytes_sum = 0;
+    agg_tal->sent_bytes_sum = 0;
+    agg_tal->sent_packets_sum = 0;
+    agg_tal->avg_got_packets = 0;
+    agg_tal->avg_got_bytes = 0;
+    agg_tal->avg_sent_packets = 0;
+    agg_tal->avg_sent_bytes = 0;
+    agg_tal->time = 0;
+    return 0;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggScanAndClear           */
+/*      If the packets in a queue have waited for too long, clear and   */
+/*      delete this aggregation queue.                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev     : device pointer                                        */
+/*      time    : current time                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_SUCCESS                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t   zfAggScanAndClear(zdev_t* dev, u32_t time)
+{
+    u16_t i;
+    u16_t head;
+    u16_t tail;
+    u32_t tick;
+    u32_t arrivalTime;
+    //u16_t aid, ac;
+    TID_TX tid_tx;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if(!(wd->state == ZM_WLAN_STATE_ENABLED)) return 0;
+    zfAggTxScheduler(dev, 1);
+    tick = zm_agg_GetTime();
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (!wd->aggQPool[i]) return 0;
+        if (1 == wd->aggQPool[i]->aggQEnabled)
+        {
+            tid_tx = wd->aggQPool[i];
+            zmw_enter_critical_section(dev);
+
+            head = tid_tx->aggHead;
+            tail = tid_tx->aggTail;
+
+            arrivalTime = (u32_t)tid_tx->aggvtxq[tid_tx->aggTail].arrivalTime;
+
+
+            if((tick - arrivalTime) <= ZM_AGG_CLEAR_TIME)
+            {
+
+            }
+            else if((tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail)) > 0)
+            {
+
+                tid_tx->clearFlag = 1;
+
+                //zm_msg1_agg(ZM_LV_0, "clear queue    tick =", tick);
+                //zm_msg1_agg(ZM_LV_0, "clear queue arrival =", arrivalTime);
+
+
+                //zmw_leave_critical_section(dev);
+                //zfAggTxScheduler(dev);
+                //zmw_enter_critical_section(dev);
+
+            }
+
+            if (tid_tx->size == 0)
+            {
+                /*
+                 * queue empty
+                 */
+                if (tick - tid_tx->lastArrival > ZM_AGG_DELETE_TIME)
+                {
+                    zm_msg1_agg(ZM_LV_0, "delete queue, idle for n sec. n = ", \
+                            ZM_AGG_DELETE_TIME/10);
+
+                    zmw_leave_critical_section(dev);
+                    zfAggTxDeleteQueue(dev, i);
+                    zmw_enter_critical_section(dev);
+                }
+            }
+
+            zmw_leave_critical_section(dev);
+        }
+    }
+
+        zfAggRxClear(dev, time);
+
+#ifdef ZM_AGG_TALLY
+    if((wd->tick % 100) == 0) {
+        zfAggPrintTally(dev);
+    }
+#endif
+
+    return ZM_SUCCESS;
+}
+
+u16_t   zfAggPrintTally(zdev_t* dev)
+{
+    struct aggTally* agg_tal;
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    agg_tal = &wd->agg_tal;
+
+    if(agg_tal->got_packets_sum < 10)
+    {
+        zfAggTallyReset(dev);
+        return 0;
+    }
+
+    agg_tal->time++;
+    agg_tal->avg_got_packets = (agg_tal->avg_got_packets * (agg_tal->time - 1) +
+            agg_tal->got_packets_sum) / agg_tal->time;
+    agg_tal->avg_got_bytes = (agg_tal->avg_got_bytes * (agg_tal->time - 1) +
+            agg_tal->got_bytes_sum) / agg_tal->time;
+    agg_tal->avg_sent_packets = (agg_tal->avg_sent_packets * (agg_tal->time - 1)
+            + agg_tal->sent_packets_sum) / agg_tal->time;
+    agg_tal->avg_sent_bytes = (agg_tal->avg_sent_bytes * (agg_tal->time - 1) +
+            agg_tal->sent_bytes_sum) / agg_tal->time;
+    zm_msg1_agg(ZM_LV_0, "got_packets_sum =", agg_tal->got_packets_sum);
+    zm_msg1_agg(ZM_LV_0, "  got_bytes_sum =", agg_tal->got_bytes_sum);
+    zm_msg1_agg(ZM_LV_0, "sent_packets_sum=", agg_tal->sent_packets_sum);
+    zm_msg1_agg(ZM_LV_0, " sent_bytes_sum =", agg_tal->sent_bytes_sum);
+    agg_tal->got_packets_sum = agg_tal->got_bytes_sum =agg_tal->sent_packets_sum
+                = agg_tal->sent_bytes_sum = 0;
+    zm_msg1_agg(ZM_LV_0, "avg_got_packets =", agg_tal->avg_got_packets);
+    zm_msg1_agg(ZM_LV_0, "  avg_got_bytes =", agg_tal->avg_got_bytes);
+    zm_msg1_agg(ZM_LV_0, "avg_sent_packets=", agg_tal->avg_sent_packets);
+    zm_msg1_agg(ZM_LV_0, " avg_sent_bytes =", agg_tal->avg_sent_bytes);
+    if ((wd->commTally.BA_Fail == 0) || (wd->commTally.Hw_Tx_MPDU == 0))
+    {
+        zm_msg1_agg(ZM_LV_0, "Hardware Tx MPDU=", wd->commTally.Hw_Tx_MPDU);
+        zm_msg1_agg(ZM_LV_0, "  BA Fail number=", wd->commTally.BA_Fail);
+    }
+    else
+        zm_msg1_agg(ZM_LV_0, "1/(BA fail rate)=", wd->commTally.Hw_Tx_MPDU/wd->commTally.BA_Fail);
+
+    return 0;
+}
+
+u16_t zfAggRxClear(zdev_t* dev, u32_t time)
+{
+    u16_t   i;
+    struct agg_tid_rx *tid_rx;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        zmw_enter_critical_section(dev);
+        tid_rx = wd->tid_rx[i];
+        if (tid_rx->baw_head != tid_rx->baw_tail)
+        {
+            u16_t j = tid_rx->baw_tail;
+            while ((j != tid_rx->baw_head) && !tid_rx->frame[j].buf) {
+            	j = (j + 1) & ZM_AGG_BAW_MASK;
+            }
+            if ((j != tid_rx->baw_head) && (time - tid_rx->frame[j].arrivalTime) >
+                    (ZM_AGG_CLEAR_TIME - 5))
+            {
+                zmw_leave_critical_section(dev);
+                zm_msg0_agg(ZM_LV_1, "queue RxFlush by RxClear");
+                zfAggRxFlush(dev, 0, tid_rx);
+                zmw_enter_critical_section(dev);
+            }
+        }
+        zmw_leave_critical_section(dev);
+    }
+
+    return ZM_SUCCESS;
+}
+
+struct agg_tid_rx* zfAggRxEnabled(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t   dst0, src[3], ac, aid, fragOff;
+    u8_t    up;
+    u16_t   offset = 0;
+    u16_t   seq_no;
+    u16_t frameType;
+    u16_t frameCtrl;
+    u16_t frameSubtype;
+    u32_t tcp_seq;
+    //struct aggSta *agg_sta;
+#if ZM_AGG_FPGA_REORDERING
+    struct agg_tid_rx *tid_rx;
+#endif
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    seq_no = zmw_rx_buf_readh(dev, buf, 22) >> 4;
+    //DbgPrint("Rx seq=%d\n", seq_no);
+    if (wd->sta.EnableHT == 0)
+    {
+        return NULL;
+    }
+
+    frameCtrl = zmw_rx_buf_readb(dev, buf, 0);
+    frameType = frameCtrl & 0xf;
+    frameSubtype = frameCtrl & 0xf0;
+
+
+    if (frameType != ZM_WLAN_DATA_FRAME) //non-Qos Data? (frameSubtype&0x80)
+    {
+        return NULL;
+    }
+#ifdef ZM_ENABLE_PERFORMANCE_EVALUATION
+    tcp_seq = zmw_rx_buf_readb(dev, buf, 22+36) << 24;
+    tcp_seq += zmw_rx_buf_readb(dev, buf, 22+37) << 16;
+    tcp_seq += zmw_rx_buf_readb(dev, buf, 22+38) << 8;
+    tcp_seq += zmw_rx_buf_readb(dev, buf, 22+39);
+#endif
+
+    ZM_SEQ_DEBUG("In                   %5d, %12u\n", seq_no, tcp_seq);
+    dst0 = zmw_rx_buf_readh(dev, buf, offset+4);
+
+    src[0] = zmw_rx_buf_readh(dev, buf, offset+10);
+    src[1] = zmw_rx_buf_readh(dev, buf, offset+12);
+    src[2] = zmw_rx_buf_readh(dev, buf, offset+14);
+
+#if ZM_AGG_FPGA_DEBUG
+    aid = 0;
+#else
+    aid = zfApFindSta(dev, src);
+#endif
+
+    //agg_sta = &wd->aggSta[aid];
+    //zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+    //ac = zcUpToAc[up&0x7] & 0x3;
+
+    /*
+     * Filter unicast frame only, aid == 0 is for debug only
+     */
+    if ((dst0 & 0x1) == 0 && aid == 0)
+    {
+#if ZM_AGG_FPGA_REORDERING
+        tid_rx = zfAggRxGetQueue(dev, buf) ;
+        if(!tid_rx)
+            return NULL;
+        else
+        {
+            //if (tid_rx->addBaExchangeStatusCode == ZM_AGG_ADDBA_RESPONSE)
+            return tid_rx;
+        }
+#else
+        return NULL;
+#endif
+    }
+
+    return NULL;
+}
+
+u16_t zfAggRx(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo *addInfo, struct agg_tid_rx *tid_rx)
+{
+    u16_t   seq_no;
+    s16_t   index;
+    u16_t   offset = 0;
+    zbuf_t* pbuf;
+    u8_t    frameSubType;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    ZM_PERFORMANCE_RX_REORDER(dev);
+
+    seq_no = zmw_rx_buf_readh(dev, buf, offset+22) >> 4;
+
+    index = seq_no - tid_rx->seq_start;
+    /*
+     * for debug
+     */
+
+    /* zm_msg2_agg(ZM_LV_0, "queue seq = ", seq_no);
+     * DbgPrint("%s:%s%lxh %s%lxh\n", __func__, "queue seq=", seq_no,
+     *   "; seq_start=", tid_rx->seq_start);
+     */
+
+    //DbgPrint("seq_no=%d, seq_start=%d\n", seq_no, tid_rx->seq_start);
+
+    /* In some APs, we found that it might transmit NULL data whose sequence number
+       is out or order. In order to avoid this problem, we ignore these NULL data.
+     */
+
+    frameSubType = (zmw_rx_buf_readh(dev, buf, 0) & 0xF0) >> 4;
+
+    /* If this is a NULL data instead of Qos NULL data */
+    if ((frameSubType & 0x0C) == 0x04)
+    {
+        s16_t seq_diff;
+
+        seq_diff = (seq_no > tid_rx->seq_start) ?
+                       seq_no - tid_rx->seq_start : tid_rx->seq_start - seq_no;
+
+        if (seq_diff > ZM_AGG_BAW_SIZE)
+        {
+            zm_debug_msg0("Free Rx NULL data in zfAggRx");
+
+            /* Free Rx buffer */
+            zfwBufFree(dev, buf, 0);
+            return ZM_ERR_OUT_OF_ORDER_NULL_DATA;
+        }
+    }
+
+    /*
+     * sequence number wrap at 4k
+     */
+    if (tid_rx->seq_start > seq_no)
+    {
+        //index += 4096;
+
+        zmw_enter_critical_section(dev);
+        if (tid_rx->seq_start >= 4096) {
+            tid_rx->seq_start = 0;
+        }
+        zmw_leave_critical_section(dev);
+
+    }
+
+    if (tid_rx->seq_start == seq_no) {
+    	zmw_enter_critical_section(dev);
+    	if (((tid_rx->baw_head - tid_rx->baw_tail) & ZM_AGG_BAW_MASK) > 0) {
+    	    //DbgPrint("head=%d, tail=%d", tid_rx->baw_head, tid_rx->baw_tail);
+            tid_rx->baw_tail = (tid_rx->baw_tail + 1) & ZM_AGG_BAW_MASK;
+        }
+        tid_rx->seq_start = (tid_rx->seq_start + 1) & (4096 - 1);
+    	zmw_leave_critical_section(dev);
+
+        ZM_PERFORMANCE_RX_SEQ(dev, buf);
+
+    	if (wd->zfcbRecv80211 != NULL) {
+            //seq_no = zmw_rx_buf_readh(dev, buf, offset+22) >> 4;
+            //DbgPrint("Recv indicate seq=%d\n", seq_no);
+            //DbgPrint("1. seq=%d\n", seq_no);
+
+            wd->zfcbRecv80211(dev, buf, addInfo);
+        }
+        else {
+            zfiRecv80211(dev, buf, addInfo);
+        }
+    }
+    else if (!zfAggRxEnqueue(dev, buf, tid_rx, addInfo))
+    {
+        /*
+         * duplicated packet
+         */
+        return 1;
+    }
+
+    while (tid_rx->baw_head != tid_rx->baw_tail) {// && tid_rx->frame[tid_rx->baw_tail].buf)
+        u16_t tailIndex;
+
+        zmw_enter_critical_section(dev);
+
+        tailIndex = tid_rx->baw_tail;
+        pbuf = tid_rx->frame[tailIndex].buf;
+        tid_rx->frame[tailIndex].buf = 0;
+        if (!pbuf)
+        {
+            zmw_leave_critical_section(dev);
+            break;
+        }
+
+        tid_rx->baw_tail = (tid_rx->baw_tail + 1) & ZM_AGG_BAW_MASK;
+        tid_rx->seq_start = (tid_rx->seq_start + 1) & (4096 - 1);
+
+
+        //if(pbuf && tid_rx->baw_size > 0)
+        //    tid_rx->baw_size--;
+
+        zmw_leave_critical_section(dev);
+
+        ZM_PERFORMANCE_RX_SEQ(dev, pbuf);
+
+        if (wd->zfcbRecv80211 != NULL)
+        {
+            //seq_no = zmw_rx_buf_readh(dev, pbuf, offset+22) >> 4;
+            //DbgPrint("Recv indicate seq=%d\n", seq_no);
+            //DbgPrint("1. seq=%d\n", seq_no);
+            wd->zfcbRecv80211(dev, pbuf, addInfo);
+        }
+        else
+        {
+            //seq_no = zmw_rx_buf_readh(dev, pbuf, offset+22) >> 4;
+            //DbgPrint("Recv indicate seq=%d\n", seq_no);
+            zfiRecv80211(dev, pbuf, addInfo);
+        }
+    }
+
+    return 1;
+}
+
+struct agg_tid_rx *zfAggRxGetQueue(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t   src[3];
+    u16_t   aid, ac, i;
+    u16_t   offset = 0;
+    struct agg_tid_rx *tid_rx = NULL;
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    src[0] = zmw_rx_buf_readh(dev, buf, offset+10);
+    src[1] = zmw_rx_buf_readh(dev, buf, offset+12);
+    src[2] = zmw_rx_buf_readh(dev, buf, offset+14);
+    aid = zfApFindSta(dev, src);
+
+    ac = (zmw_rx_buf_readh(dev, buf, 24) & 0xF);
+
+    // mark by spin lock debug
+    //zmw_enter_critical_section(dev);
+
+    for (i=0; i<ZM_AGG_POOL_SIZE ; i++)
+    {
+        if((wd->tid_rx[i]->aid == aid) && (wd->tid_rx[i]->ac == ac))
+        {
+            tid_rx = wd->tid_rx[i];
+            break;
+        }
+    }
+
+    // mark by spin lock debug
+    //zmw_leave_critical_section(dev);
+    return tid_rx;
+}
+
+
+u16_t   zfAggRxEnqueue(zdev_t* dev, zbuf_t* buf, struct agg_tid_rx *tid_rx, struct zsAdditionInfo *addInfo)
+{
+    u16_t seq_no, offset = 0;
+    u16_t q_index;
+    s16_t index;
+    u8_t  bdropframe = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    seq_no = zmw_rx_buf_readh(dev, buf, offset+22) >> 4;
+    index  = seq_no - tid_rx->seq_start;
+
+    /*
+     * sequence number wrap at 4k
+     * -1000: check for duplicate past packet
+     */
+    bdropframe = 0;
+    if (tid_rx->seq_start > seq_no) {
+        if ((tid_rx->seq_start > 3967) && (seq_no < 128)) {
+            index += 4096;
+        } else if (tid_rx->seq_start - seq_no > 70) {
+            zmw_enter_critical_section(dev);
+            tid_rx->sq_behind_count++;
+            if (tid_rx->sq_behind_count > 3) {
+                tid_rx->sq_behind_count = 0;
+            } else {
+                bdropframe = 1;
+            }
+            zmw_leave_critical_section(dev);
+        } else {
+            bdropframe = 1;
+        }
+    } else {
+        if (seq_no - tid_rx->seq_start > 70) {
+            zmw_enter_critical_section(dev);
+            tid_rx->sq_exceed_count++;
+            if (tid_rx->sq_exceed_count > 3) {
+                tid_rx->sq_exceed_count = 0;
+            } else {
+                bdropframe = 1;
+            }
+            zmw_leave_critical_section(dev);
+        }
+    }
+
+    if (bdropframe == 1) {
+        /*if (wd->zfcbRecv80211 != NULL) {
+            wd->zfcbRecv80211(dev, buf, addInfo);
+        }
+        else {
+            zfiRecv80211(dev, buf, addInfo);
+        }*/
+
+        ZM_PERFORMANCE_FREE(dev, buf);
+
+        zfwBufFree(dev, buf, 0);
+        /*zfAggRxFlush(dev, seq_no, tid_rx);
+        tid_rx->seq_start = seq_no;
+        index = seq_no - tid_rx->seq_start;
+        */
+
+        //DbgPrint("Free an old packet, seq_start=%d, seq_no=%d\n", tid_rx->seq_start, seq_no);
+
+        /*
+         * duplicate past packet
+         * happens only in simulated aggregation environment
+         */
+        return 0;
+    } else {
+        zmw_enter_critical_section(dev);
+        if (tid_rx->sq_exceed_count > 0){
+            tid_rx->sq_exceed_count--;
+        }
+
+        if (tid_rx->sq_behind_count > 0) {
+            tid_rx->sq_behind_count--;
+        }
+        zmw_leave_critical_section(dev);
+    }
+
+    if (index < 0) {
+        zfAggRxFlush(dev, seq_no, tid_rx);
+        tid_rx->seq_start = seq_no;
+        index = 0;
+    }
+
+    //if (index >= (ZM_AGG_BAW_SIZE - 1))
+    if (index >= (ZM_AGG_BAW_MASK))
+    {
+        /*
+         * queue full
+         */
+        //DbgPrint("index >= 64, seq_start=%d, seq_no=%d\n", tid_rx->seq_start, seq_no);
+        zfAggRxFlush(dev, seq_no, tid_rx);
+        //tid_rx->seq_start = seq_no;
+        index = seq_no - tid_rx->seq_start;
+        if ((tid_rx->seq_start > seq_no) && (tid_rx->seq_start > 1000) && (tid_rx->seq_start - 1000) > seq_no)
+        {
+        //index = seq_no - tid_rx->seq_start;
+            index += 4096;
+        }
+        //index = seq_no - tid_rx->seq_start;
+        while (index >= (ZM_AGG_BAW_MASK)) {
+            //DbgPrint("index >= 64, seq_start=%d, seq_no=%d\n", tid_rx->seq_start, seq_no);
+            tid_rx->seq_start = (tid_rx->seq_start + ZM_AGG_BAW_MASK) & (4096 - 1);
+            index = seq_no - tid_rx->seq_start;
+            if ((tid_rx->seq_start > seq_no) && (tid_rx->seq_start > 1000) && (tid_rx->seq_start - 1000) > seq_no)
+            {
+                index += 4096;
+            }
+        }
+    }
+
+
+    q_index = (tid_rx->baw_tail + index) & ZM_AGG_BAW_MASK;
+    if (tid_rx->frame[q_index].buf && (((tid_rx->baw_head - tid_rx->baw_tail) & ZM_AGG_BAW_MASK) >
+                (((q_index) - tid_rx->baw_tail) & ZM_AGG_BAW_MASK)))
+    {
+
+        ZM_PERFORMANCE_DUP(dev, tid_rx->frame[q_index].buf, buf);
+        zfwBufFree(dev, buf, 0);
+        //DbgPrint("Free a duplicate packet, seq_start=%d, seq_no=%d\n", tid_rx->seq_start, seq_no);
+        //DbgPrint("head=%d, tail=%d", tid_rx->baw_head, tid_rx->baw_tail);
+        /*
+         * duplicate packet
+         */
+        return 0;
+    }
+
+    zmw_enter_critical_section(dev);
+    if(tid_rx->frame[q_index].buf) {
+        zfwBufFree(dev, tid_rx->frame[q_index].buf, 0);
+        tid_rx->frame[q_index].buf = 0;
+    }
+
+    tid_rx->frame[q_index].buf = buf;
+    tid_rx->frame[q_index].arrivalTime = zm_agg_GetTime();
+    zfwMemoryCopy((void*)&tid_rx->frame[q_index].addInfo, (void*)addInfo, sizeof(struct zsAdditionInfo));
+
+    /*
+     * for debug simulated aggregation only,
+     * should be done in rx of ADDBA Request
+     */
+    //tid_rx->addInfo = addInfo;
+
+
+    if (((tid_rx->baw_head - tid_rx->baw_tail) & ZM_AGG_BAW_MASK) <= index)
+    {
+        //tid_rx->baw_size = index + 1;
+        if (((tid_rx->baw_head - tid_rx->baw_tail) & ZM_AGG_BAW_MASK) <=
+                //((q_index + 1) & ZM_AGG_BAW_MASK))
+                (((q_index) - tid_rx->baw_tail) & ZM_AGG_BAW_MASK))//tid_rx->baw_size )
+            tid_rx->baw_head = (q_index + 1) & ZM_AGG_BAW_MASK;
+    }
+    zmw_leave_critical_section(dev);
+
+    /*
+     * success
+     */
+    //DbgPrint("head=%d, tail=%d, start=%d", tid_rx->baw_head, tid_rx->baw_tail, tid_rx->seq_start);
+    return 1;
+}
+
+u16_t zfAggRxFlush(zdev_t* dev, u16_t seq_no, struct agg_tid_rx *tid_rx)
+{
+    zbuf_t* pbuf;
+    u16_t   seq;
+    struct zsAdditionInfo addInfo;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    ZM_PERFORMANCE_RX_FLUSH(dev);
+
+    while (1)
+    {
+        zmw_enter_critical_section(dev);
+        if (tid_rx->baw_tail == tid_rx->baw_head) {
+            zmw_leave_critical_section(dev);
+            break;
+        }
+
+        pbuf = tid_rx->frame[tid_rx->baw_tail].buf;
+        zfwMemoryCopy((void*)&addInfo, (void*)&tid_rx->frame[tid_rx->baw_tail].addInfo, sizeof(struct zsAdditionInfo));
+        tid_rx->frame[tid_rx->baw_tail].buf = 0;
+        //if(pbuf && tid_rx->baw_size > 0) tid_rx->baw_size--;
+        tid_rx->baw_tail = (tid_rx->baw_tail + 1) & ZM_AGG_BAW_MASK;
+        tid_rx->seq_start = (tid_rx->seq_start + 1) & (4096 - 1);
+	    zmw_leave_critical_section(dev);
+
+        if (pbuf)
+        {
+
+            ZM_PERFORMANCE_RX_SEQ(dev, pbuf);
+
+            if (wd->zfcbRecv80211 != NULL)
+            {
+                seq = zmw_rx_buf_readh(dev, pbuf, 22) >> 4;
+                //DbgPrint("Recv indicate seq=%d\n", seq);
+                //DbgPrint("2. seq=%d\n", seq);
+                wd->zfcbRecv80211(dev, pbuf, &addInfo);
+            }
+            else
+            {
+                seq = zmw_rx_buf_readh(dev, pbuf, 22) >> 4;
+                //DbgPrint("Recv indicate seq=%d\n", seq);
+                zfiRecv80211(dev, pbuf, &addInfo);
+            }
+        }
+    }
+
+    zmw_enter_critical_section(dev);
+    tid_rx->baw_head = tid_rx->baw_tail = 0;
+    zmw_leave_critical_section(dev);
+    return 1;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggRxFreeBuf              */
+/*      Frees all queued packets in buffer when the driver is down.     */
+/*      The zfFreeResource() will check if the buffer is all freed.     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev     : device pointer                                        */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_SUCCESS                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t   zfAggRxFreeBuf(zdev_t* dev, u16_t destroy)
+{
+    u16_t   i;
+    zbuf_t* buf;
+    struct agg_tid_rx *tid_rx;
+
+    TID_TX  tid_tx;
+    //struct bufInfo *buf_info;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        u16_t j;
+
+        tid_rx = wd->tid_rx[i];
+
+        for(j=0; j <= ZM_AGG_BAW_SIZE; j++)
+        {
+            zmw_enter_critical_section(dev);
+            buf = tid_rx->frame[j].buf;
+            tid_rx->frame[j].buf = 0;
+            zmw_leave_critical_section(dev);
+
+            if (buf)
+            {
+                zfwBufFree(dev, buf, 0);
+            }
+        }
+
+        #if 0
+        if ( tid_rx->baw_head != tid_rx->baw_tail )
+        {
+            while (tid_rx->baw_head != tid_rx->baw_tail)
+            {
+                buf = tid_rx->frame[tid_rx->baw_tail].buf;
+                tid_rx->frame[tid_rx->baw_tail].buf = 0;
+                if (buf)
+                {
+                    zfwBufFree(dev, buf, 0);
+
+                    zmw_enter_critical_section(dev);
+                    tid_rx->frame[tid_rx->baw_tail].buf = 0;
+                    zmw_leave_critical_section(dev);
+                }
+                zmw_enter_critical_section(dev);
+                //if (tid_rx->baw_size > 0)tid_rx->baw_size--;
+                tid_rx->baw_tail = (tid_rx->baw_tail + 1) & ZM_AGG_BAW_MASK;
+                tid_rx->seq_start++;
+                zmw_leave_critical_section(dev);
+            }
+        }
+        #endif
+
+        zmw_enter_critical_section(dev);
+        tid_rx->seq_start = 0;
+        tid_rx->baw_head = tid_rx->baw_tail = 0;
+        tid_rx->aid = ZM_MAX_STA_SUPPORT;
+        zmw_leave_critical_section(dev);
+
+        #ifdef ZM_ENABLE_AGGREGATION
+        #ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+        if (tid_baw->enabled) {
+            zm_msg1_agg(ZM_LV_0, "Device down, clear BAW queue:", i);
+            BAW->disable(dev, tid_baw);
+        }
+        #endif
+        #endif
+        if (1 == wd->aggQPool[i]->aggQEnabled) {
+            tid_tx = wd->aggQPool[i];
+            buf = zfAggTxGetVtxq(dev, tid_tx);
+            while (buf) {
+                zfwBufFree(dev, buf, 0);
+                buf = zfAggTxGetVtxq(dev, tid_tx);
+            }
+        }
+
+        if(destroy) {
+            zfwMemFree(dev, wd->aggQPool[i], sizeof(struct aggQueue));
+            zfwMemFree(dev, wd->tid_rx[i], sizeof(struct agg_tid_rx));
+        }
+    }
+    #ifdef ZM_ENABLE_AGGREGATION
+    #ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+    if(destroy) zfwMemFree(dev, BAW, sizeof(struct baw_enabler));
+    #endif
+    #endif
+    return ZM_SUCCESS;
+}
+
+
+void zfAggRecvBAR(zdev_t* dev, zbuf_t *buf) {
+    u16_t start_seq, len;
+    u8_t i, bitmap[8];
+    len = zfwBufGetSize(dev, buf);
+    start_seq = zmw_rx_buf_readh(dev, buf, len-2);
+    DbgPrint("Received a BAR Control frame, start_seq=%d", start_seq>>4);
+    /* todo: set the bitmap by reordering buffer! */
+    for (i=0; i<8; i++) bitmap[i]=0;
+    zfSendBA(dev, start_seq, bitmap);
+}
+
+#ifdef ZM_ENABLE_AGGREGATION
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+void zfAggTxRetransmit(zdev_t* dev, struct bufInfo *buf_info, struct aggControl *aggControl, TID_TX tid_tx) {
+    u16_t removeLen;
+    u16_t err;
+
+    zmw_get_wlan_dev(dev);
+    if (aggControl && (ZM_AGG_FIRST_MPDU == aggControl->ampduIndication) ) {
+        tid_tx->bar_ssn = buf_info->baw_header->header[15];
+        aggControl->tid_baw->start_seq = tid_tx->bar_ssn >> 4;
+        zm_msg1_agg(ZM_LV_0, "start seq=", tid_tx->bar_ssn >> 4);
+    }
+    buf_info->baw_header->header[4] |= (1 << 11);
+    if (aggControl && aggControl->aggEnabled) {
+        //if (wd->enableAggregation==0 && !(buf_info->baw_header->header[6]&0x1))
+        //{
+            //if (((buf_info->baw_header->header[2] & 0x3) == 2))
+            //{
+                /* Enable aggregation */
+                buf_info->baw_header->header[1] |= 0x20;
+                if (ZM_AGG_LAST_MPDU == aggControl->ampduIndication) {
+                    buf_info->baw_header->header[1] |= 0x4000;
+                }
+                else {
+                    buf_info->baw_header->header[1] &= ~0x4000;
+                    //zm_debug_msg0("ZM_AGG_LAST_MPDU");
+                }
+            //}
+            //else {
+            //    zm_debug_msg1("no aggr, header[2]&0x3 = ",buf_info->baw_header->header[2] & 0x3)
+            //    aggControl->aggEnabled = 0;
+            //}
+        //}
+        //else {
+        //    zm_debug_msg1("no aggr, wd->enableAggregation = ", wd->enableAggregation);
+        //    zm_debug_msg1("no aggr, !header[6]&0x1 = ",!(buf_info->baw_header->header[6]&0x1));
+        //    aggControl->aggEnabled = 0;
+        //}
+    }
+
+    /*if (aggControl->tid_baw) {
+        struct baw_header_r header_r;
+
+        header_r.header      = buf_info->baw_header->header;
+        header_r.mic         = buf_info->baw_header->mic;
+        header_r.snap        = buf_info->baw_header->snap;
+        header_r.headerLen   = buf_info->baw_header->headerLen;
+        header_r.micLen      = buf_info->baw_header->micLen;
+        header_r.snapLen     = buf_info->baw_header->snapLen;
+        header_r.removeLen   = buf_info->baw_header->removeLen;
+        header_r.keyIdx      = buf_info->baw_header->keyIdx;
+
+        BAW->insert(dev, buf_info->buf, tid_tx->bar_ssn >> 4, aggControl->tid_baw, buf_info->baw_retransmit, &header_r);
+    }*/
+
+    if ((err = zfHpSend(dev,
+                    buf_info->baw_header->header,
+                    buf_info->baw_header->headerLen,
+                    buf_info->baw_header->snap,
+                    buf_info->baw_header->snapLen,
+                    buf_info->baw_header->mic,
+                    buf_info->baw_header->micLen,
+                    buf_info->buf,
+                    buf_info->baw_header->removeLen,
+                    ZM_EXTERNAL_ALLOC_BUF,
+                    (u8_t)tid_tx->ac,
+                    buf_info->baw_header->keyIdx)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+
+    return;
+
+zlError:
+    zfwBufFree(dev, buf_info->buf, 0);
+    return;
+
+}
+#endif //disable BAW
+#endif
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxSendEth              */
+/*      Called to transmit Ethernet frame from upper elayer.            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      port : WLAN port, 0=>standard, 0x10-0x17=>VAP, 0x20-0x25=>WDS   */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen, Honda      Atheros Communications, Inc.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port, u16_t bufType, u8_t flag, struct aggControl *aggControl, TID_TX tid_tx)
+{
+    u16_t err;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t removeLen;
+    u16_t header[(8+30+2+18)/2];    /* ctr+(4+a1+a2+a3+2+a4)+qos+iv */
+    u16_t headerLen;
+    u16_t mic[8/2];
+    u16_t micLen;
+    u16_t snap[8/2];
+    u16_t snapLen;
+    u16_t fragLen;
+    u16_t frameLen;
+    u16_t fragNum;
+    struct zsFrag frag;
+    u16_t i, id;
+    u16_t da[3];
+    u16_t sa[3];
+    u8_t up;
+    u8_t qosType, keyIdx = 0;
+    u16_t fragOff;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zm_msg1_tx(ZM_LV_2, "zfTxSendEth(), port=", port);
+
+    /* Get IP TOS for QoS AC and IP frag offset */
+    zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 16);
+        da[1] = zmw_tx_buf_readh(dev, buf, 18);
+        da[2] = zmw_tx_buf_readh(dev, buf, 20);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 10);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 12);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 14);
+    }
+    else if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 4);
+        da[1] = zmw_tx_buf_readh(dev, buf, 6);
+        da[2] = zmw_tx_buf_readh(dev, buf, 8);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 10);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 12);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 14);
+    }
+    else if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 4);
+        da[1] = zmw_tx_buf_readh(dev, buf, 6);
+        da[2] = zmw_tx_buf_readh(dev, buf, 8);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 16);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 18);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 20);
+    }
+    else
+    {
+        //
+    }
+#else
+    /* DA */
+    da[0] = zmw_tx_buf_readh(dev, buf, 0);
+    da[1] = zmw_tx_buf_readh(dev, buf, 2);
+    da[2] = zmw_tx_buf_readh(dev, buf, 4);
+    /* SA */
+    sa[0] = zmw_tx_buf_readh(dev, buf, 6);
+    sa[1] = zmw_tx_buf_readh(dev, buf, 8);
+    sa[2] = zmw_tx_buf_readh(dev, buf, 10);
+#endif
+    //Decide Key Index in ATOM, No meaning in OTUS--CWYang(m)
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        keyIdx = wd->ap.bcHalKeyIdx[port];
+        id = zfApFindSta(dev, da);
+        if (id != 0xffff)
+        {
+            switch (wd->ap.staTable[id].encryMode)
+            {
+            case ZM_AES:
+            case ZM_TKIP:
+#ifdef ZM_ENABLE_CENC
+            case ZM_CENC:
+#endif //ZM_ENABLE_CENC
+                keyIdx = wd->ap.staTable[id].keyIdx;
+                break;
+            }
+        }
+    }
+    else
+    {
+        switch (wd->sta.encryMode)
+        {
+        case ZM_WEP64:
+        case ZM_WEP128:
+        case ZM_WEP256:
+            keyIdx = wd->sta.keyId;
+            break;
+        case ZM_AES:
+        case ZM_TKIP:
+            if ((da[0]& 0x1))
+                keyIdx = 5;
+            else
+                keyIdx = 4;
+            break;
+#ifdef ZM_ENABLE_CENC
+        case ZM_CENC:
+            keyIdx = wd->sta.cencKeyId;
+            break;
+#endif //ZM_ENABLE_CENC
+        }
+    }
+
+    /* Create SNAP */
+    removeLen = zfTxGenWlanSnap(dev, buf, snap, &snapLen);
+    //zm_msg1_tx(ZM_LV_0, "fragOff=", fragOff);
+
+    fragLen = wd->fragThreshold;
+    frameLen = zfwBufGetSize(dev, buf);
+    frameLen -= removeLen;
+
+#if 0
+    /* Create MIC */
+    if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)&&
+         (wd->sta.encryMode == ZM_TKIP) )
+    {
+        if ( frameLen > fragLen )
+        {
+            micLen = zfTxGenWlanTail(dev, buf, snap, snapLen, mic);
+        }
+        else
+        {
+            /* append MIC by HMAC */
+            micLen = 8;
+        }
+    }
+    else
+    {
+        micLen = 0;
+    }
+#else
+    if ( frameLen > fragLen )
+    {
+        micLen = zfTxGenWlanTail(dev, buf, snap, snapLen, mic);
+    }
+    else
+    {
+        /* append MIC by HMAC */
+        micLen = 0;
+    }
+#endif
+
+    /* Access Category */
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        zfApGetStaQosType(dev, da, &qosType);
+        if (qosType == 0)
+        {
+            up = 0;
+        }
+    }
+    else if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        if (wd->sta.wmeConnected == 0)
+        {
+            up = 0;
+        }
+    }
+    else
+    {
+        /* TODO : STA QoS control field */
+        up = 0;
+    }
+
+    /* Assign sequence number */
+    zmw_enter_critical_section(dev);
+    frag.seq[0] = ((wd->seq[zcUpToAc[up&0x7]]++) << 4);
+    if (aggControl && (ZM_AGG_FIRST_MPDU == aggControl->ampduIndication) ) {
+        tid_tx->bar_ssn = frag.seq[0];
+
+        zm_msg1_agg(ZM_LV_0, "start seq=", tid_tx->bar_ssn >> 4);
+    }
+    //tid_tx->baw_buf[tid_tx->baw_head-1].baw_seq=frag.seq[0];
+    zmw_leave_critical_section(dev);
+
+
+        frag.buf[0] = buf;
+        frag.bufType[0] = bufType;
+        frag.flag[0] = flag;
+        fragNum = 1;
+
+    for (i=0; i<fragNum; i++)
+    {
+        /* Create WLAN header(Control Setting + 802.11 header + IV) */
+        if (up !=0 ) zm_debug_msg1("up not 0, up=",up);
+        headerLen = zfTxGenWlanHeader(dev, frag.buf[i], header, frag.seq[i],
+                                      frag.flag[i], snapLen+micLen, removeLen,
+                                      port, da, sa, up, &micLen, snap, snapLen,
+                                      aggControl);
+
+        /* Get buffer DMA address */
+        //if ((addrTblSize = zfwBufMapDma(dev, frag.buf[i], &addrTbl)) == 0)
+        //if ((addrTblSize = zfwMapTxDma(dev, frag.buf[i], &addrTbl)) == 0)
+        //{
+        //    err = ZM_ERR_BUFFER_DMA_ADDR;
+        //    goto zlError;
+        //}
+
+        /* Flush buffer on cache */
+        //zfwBufFlush(dev, frag.buf[i]);
+
+#if 0
+        zm_msg1_tx(ZM_LV_0, "headerLen=", headerLen);
+        zm_msg1_tx(ZM_LV_0, "snapLen=", snapLen);
+        zm_msg1_tx(ZM_LV_0, "micLen=", micLen);
+        zm_msg1_tx(ZM_LV_0, "removeLen=", removeLen);
+        zm_msg1_tx(ZM_LV_0, "addrTblSize=", addrTblSize);
+        zm_msg1_tx(ZM_LV_0, "frag.bufType[0]=", frag.bufType[0]);
+#endif
+
+        fragLen = zfwBufGetSize(dev, frag.buf[i]);
+        if ((da[0]&0x1) == 0)
+        {
+            wd->commTally.txUnicastFrm++;
+            wd->commTally.txUnicastOctets += (fragLen+snapLen);
+        }
+        else if ((da[0]& 0x1))
+        {
+            wd->commTally.txBroadcastFrm++;
+            wd->commTally.txBroadcastOctets += (fragLen+snapLen);
+        }
+        else
+        {
+            wd->commTally.txMulticastFrm++;
+            wd->commTally.txMulticastOctets += (fragLen+snapLen);
+        }
+        wd->ledStruct.txTraffic++;
+
+#if 0 //Who care this?
+        if ( (i)&&(i == (fragNum-1)) )
+        {
+            wd->trafTally.txDataByteCount -= micLen;
+        }
+#endif
+
+        /*if (aggControl->tid_baw && aggControl->aggEnabled) {
+            struct baw_header_r header_r;
+
+            header_r.header      = header;
+            header_r.mic         = mic;
+            header_r.snap        = snap;
+            header_r.headerLen   = headerLen;
+            header_r.micLen      = micLen;
+            header_r.snapLen     = snapLen;
+            header_r.removeLen   = removeLen;
+            header_r.keyIdx      = keyIdx;
+
+            BAW->insert(dev, buf, tid_tx->bar_ssn >> 4, aggControl->tid_baw, 0, &header_r);
+        }*/
+
+        if ((err = zfHpSend(dev, header, headerLen, snap, snapLen,
+                             mic, micLen, frag.buf[i], removeLen,
+                             frag.bufType[i], zcUpToAc[up&0x7], keyIdx)) != ZM_SUCCESS)
+        {
+            goto zlError;
+        }
+
+
+        continue;
+
+zlError:
+        if (frag.bufType[i] == ZM_EXTERNAL_ALLOC_BUF)
+        {
+            zfwBufFree(dev, frag.buf[i], err);
+        }
+        else if (frag.bufType[i] == ZM_INTERNAL_ALLOC_BUF)
+        {
+            zfwBufFree(dev, frag.buf[i], 0);
+        }
+        else
+        {
+            zm_assert(0);
+        }
+    } /* for (i=0; i<fragNum; i++) */
+
+    return ZM_SUCCESS;
+}
+
+/*
+ * zfAggSendADDBA() refers zfSendMmFrame() in cmm.c
+ */
+u16_t   zfAggSendAddbaRequest(zdev_t* dev, u16_t *dst, u16_t ac, u16_t up)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    //u16_t err;
+    u16_t offset = 0;
+    u16_t hlen = 32;
+    u16_t header[(24+25+1)/2];
+    u16_t vap = 0;
+    u16_t i;
+    u8_t encrypt = 0;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+
+    /*
+     * TBD : Maximum size of managment frame
+     */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return ZM_SUCCESS;
+    }
+
+    /*
+     * Reserve room for wlan header
+     */
+    offset = hlen;
+
+    /*
+     * add addba frame body
+     */
+    offset = zfAggSetAddbaFrameBody(dev, buf, offset, ac, up);
+
+
+    zfwBufSetSize(dev, buf, offset);
+
+    /*
+     * Copy wlan header
+     */
+    zfAggGenAddbaHeader(dev, dst, header, offset-hlen, buf, vap, encrypt);
+    for (i=0; i<(hlen>>1); i++)
+    {
+        zmw_tx_buf_writeh(dev, buf, i*2, header[i]);
+    }
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    //zm_msg2_mm(ZM_LV_2, "offset=", offset);
+    //zm_msg2_mm(ZM_LV_2, "hlen=", hlen);
+    //zm_msg2_mm(ZM_LV_2, "addrTblSize=", addrTblSize);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.len[0]=", addrTbl.len[0]);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.physAddrl[0]=", addrTbl.physAddrl[0]);
+    //zm_msg2_mm(ZM_LV_2, "buf->data=", buf->data);
+
+    #if 0
+    if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+    #else
+    zfPutVmmq(dev, buf);
+    zfPushVtxq(dev);
+    #endif
+
+    return ZM_SUCCESS;
+
+}
+
+u16_t   zfAggSetAddbaFrameBody(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t ac, u16_t up)
+{
+    u16_t ba_parameter, start_seq;
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    /*
+     * ADDBA Request frame body
+     */
+
+    /*
+     * Category
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, 3);
+    /*
+     * Action details = 0
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_ADDBA_REQUEST_FRAME);
+    /*
+     * Dialog Token = nonzero
+     * TBD: define how to get dialog token?
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, 2);
+    /*
+     * Block Ack parameter set
+     * BA policy = 1 for immediate BA, 0 for delayed BA
+     * TID(4bits) & buffer size(4bits) (TID=up & buffer size=0x80)
+     * TBD: how to get buffer size?
+     * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+     * ¢x    B0    ¢x    B1     ¢x B2  B5 ¢x B6      B15 ¢x
+     * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+     * ¢x Reserved ¢x BA policy ¢x  TID   ¢x Buffer size ¢x
+     * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+     */
+    ba_parameter = 1 << 12;     // buffer size = 0x40(64)
+    ba_parameter |= up << 2;    // tid = up
+    ba_parameter |= 2;          // ba policy = 1
+    zmw_tx_buf_writeh(dev, buf, offset, ba_parameter);
+    offset+=2;
+    /*
+     * BA timeout value
+     */
+    zmw_tx_buf_writeh(dev, buf, offset, 0);
+    offset+=2;
+    /*
+     * BA starting sequence number
+     * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+     * ¢x B0       B3 ¢x B4              B15 ¢x
+     * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+     * ¢x Frag num(0) ¢x BA starting seq num ¢x
+     * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+     */
+    start_seq = ((wd->seq[ac]) << 4) & 0xFFF0;
+    zmw_tx_buf_writeh(dev, buf, offset, start_seq);
+    offset+=2;
+
+    return offset;
+}
+
+u16_t zfAggGenAddbaHeader(zdev_t* dev, u16_t* dst,
+        u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt)
+{
+    u8_t  hlen = 32;        // MAC ctrl + PHY ctrl + 802.11 MM header
+    //u8_t frameType = ZM_WLAN_FRAME_TYPE_ACTION;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /*
+     * Generate control setting
+     */
+    //bodyLen = zfwBufGetSize(dev, buf);
+    header[0] = 24+len+4;   //Length
+    header[1] = 0x8;        //MAC control, backoff + (ack)
+
+#if 0
+    /* CCK 1M */
+    header[2] = 0x0f00;          //PHY control L
+    header[3] = 0x0000;          //PHY control H
+#else
+    /* OFDM 6M */
+    header[2] = 0x0f01;          //PHY control L
+    header[3] = 0x000B;          //PHY control H
+#endif
+
+    /*
+     * Generate WLAN header
+     * Frame control frame type and subtype
+     */
+    header[4+0] = ZM_WLAN_FRAME_TYPE_ACTION;
+    /*
+     * Duration
+     */
+    header[4+1] = 0;
+
+    if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        header[4+8] = wd->sta.bssid[0];
+        header[4+9] = wd->sta.bssid[1];
+        header[4+10] = wd->sta.bssid[2];
+    }
+    else if (wd->wlanMode == ZM_MODE_PSEUDO)
+    {
+        /* Address 3 = 00:00:00:00:00:00 */
+        header[4+8] = 0;
+        header[4+9] = 0;
+        header[4+10] = 0;
+    }
+    else if (wd->wlanMode == ZM_MODE_IBSS)
+    {
+        header[4+8] = wd->sta.bssid[0];
+        header[4+9] = wd->sta.bssid[1];
+        header[4+10] = wd->sta.bssid[2];
+    }
+    else if (wd->wlanMode == ZM_MODE_AP)
+    {
+        /* Address 3 = BSSID */
+        header[4+8] = wd->macAddr[0];
+        header[4+9] = wd->macAddr[1];
+        header[4+10] = wd->macAddr[2] + (vap<<8);
+    }
+
+    /* Address 1 = DA */
+    header[4+2] = dst[0];
+    header[4+3] = dst[1];
+    header[4+4] = dst[2];
+
+    /* Address 2 = SA */
+    header[4+5] = wd->macAddr[0];
+    header[4+6] = wd->macAddr[1];
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        header[4+7] = wd->macAddr[2] + (vap<<8);
+    }
+    else
+    {
+        header[4+7] = wd->macAddr[2];
+    }
+
+    /* Sequence Control */
+    zmw_enter_critical_section(dev);
+    header[4+11] = ((wd->mmseq++)<<4);
+    zmw_leave_critical_section(dev);
+
+
+    return hlen;
+}
+
+
+u16_t   zfAggProcessAction(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t category;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    category = zmw_rx_buf_readb(dev, buf, 24);
+
+    switch (category)
+    {
+    case ZM_WLAN_BLOCK_ACK_ACTION_FRAME:
+        zfAggBlockAckActionFrame(dev, buf);
+        break;
+
+    }
+
+    return ZM_SUCCESS;
+}
+
+
+u16_t   zfAggBlockAckActionFrame(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t action;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    action = zmw_rx_buf_readb(dev, buf, 25);
+#ifdef ZM_ENABLE_AGGREGATION
+    switch (action)
+    {
+    case ZM_WLAN_ADDBA_REQUEST_FRAME:
+        zm_msg0_agg(ZM_LV_0, "Received BA Action frame is ADDBA request");
+        zfAggRecvAddbaRequest(dev, buf);
+        break;
+    case ZM_WLAN_ADDBA_RESPONSE_FRAME:
+        zm_msg0_agg(ZM_LV_0, "Received BA Action frame is ADDBA response");
+        zfAggRecvAddbaResponse(dev, buf);
+        break;
+    case ZM_WLAN_DELBA_FRAME:
+        zfAggRecvDelba(dev, buf);
+        break;
+    }
+#endif
+    return ZM_SUCCESS;
+}
+
+u16_t   zfAggRecvAddbaRequest(zdev_t* dev, zbuf_t* buf)
+{
+    //u16_t dialog;
+    struct aggBaFrameParameter bf;
+    u16_t i;
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    bf.buf = buf;
+    bf.dialog = zmw_rx_buf_readb(dev, buf, 26);
+    /*
+     * ba parameter set
+     */
+    bf.ba_parameter = zmw_rx_buf_readh(dev, buf, 27);
+    bf.ba_policy   = (bf.ba_parameter >> 1) & 1;
+    bf.tid         = (bf.ba_parameter >> 2) & 0xF;
+    bf.buffer_size = (bf.ba_parameter >> 6);
+    /*
+     * BA timeout value
+     */
+    bf.ba_timeout = zmw_rx_buf_readh(dev, buf, 29);
+    /*
+     * BA starting sequence number
+     */
+    bf.ba_start_seq = zmw_rx_buf_readh(dev, buf, 31) >> 4;
+
+    i=26;
+    while(i < 32) {
+        zm_debug_msg2("Recv ADDBA Req:", zmw_rx_buf_readb(dev,buf,i));
+        i++;
+    }
+
+    zfAggSendAddbaResponse(dev, &bf);
+
+    zfAggAddbaSetTidRx(dev, buf, &bf);
+
+    return ZM_SUCCESS;
+}
+
+u16_t   zfAggAddbaSetTidRx(zdev_t* dev, zbuf_t* buf, struct aggBaFrameParameter *bf)
+{
+    u16_t i, ac, aid, fragOff;
+    u16_t src[3];
+    u16_t offset = 0;
+    u8_t  up;
+    struct agg_tid_rx *tid_rx = NULL;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    src[0] = zmw_rx_buf_readh(dev, buf, offset+10);
+    src[1] = zmw_rx_buf_readh(dev, buf, offset+12);
+    src[2] = zmw_rx_buf_readh(dev, buf, offset+14);
+    aid = zfApFindSta(dev, src);
+
+    zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+    ac = zcUpToAc[up&0x7] & 0x3;
+
+    ac = bf->tid;
+
+    for (i=0; i<ZM_AGG_POOL_SIZE ; i++)
+    {
+        if((wd->tid_rx[i]->aid == aid) && (wd->tid_rx[i]->ac == ac))
+        {
+            tid_rx = wd->tid_rx[i];
+            break;
+        }
+    }
+
+    if (!tid_rx)
+    {
+        for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+        {
+            if (wd->tid_rx[i]->aid == ZM_MAX_STA_SUPPORT)
+            {
+                tid_rx = wd->tid_rx[i];
+                break;
+            }
+        }
+        if (!tid_rx)
+            return 0;
+    }
+
+    zmw_enter_critical_section(dev);
+
+    tid_rx->aid = aid;
+    tid_rx->ac = ac;
+    tid_rx->addBaExchangeStatusCode = ZM_AGG_ADDBA_RESPONSE;
+    tid_rx->seq_start = bf->ba_start_seq;
+    tid_rx->baw_head = tid_rx->baw_tail = 0;
+    tid_rx->sq_exceed_count = tid_rx->sq_behind_count = 0;
+    zmw_leave_critical_section(dev);
+
+    return 0;
+}
+
+u16_t   zfAggRecvAddbaResponse(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t i,ac, aid=0;
+    u16_t src[3];
+    struct aggBaFrameParameter bf;
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    src[0] = zmw_rx_buf_readh(dev, buf, 10);
+    src[1] = zmw_rx_buf_readh(dev, buf, 12);
+    src[2] = zmw_rx_buf_readh(dev, buf, 14);
+
+    if (wd->wlanMode == ZM_MODE_AP)
+        aid = zfApFindSta(dev, src);
+
+
+    bf.buf = buf;
+    bf.dialog = zmw_rx_buf_readb(dev, buf, 26);
+    bf.status_code = zmw_rx_buf_readh(dev, buf, 27);
+    if (!bf.status_code)
+    {
+        wd->addbaComplete=1;
+    }
+
+    /*
+     * ba parameter set
+     */
+    bf.ba_parameter = zmw_rx_buf_readh(dev, buf, 29);
+    bf.ba_policy   = (bf.ba_parameter >> 1) & 1;
+    bf.tid         = (bf.ba_parameter >> 2) & 0xF;
+    bf.buffer_size = (bf.ba_parameter >> 6);
+    /*
+     * BA timeout value
+     */
+    bf.ba_timeout = zmw_rx_buf_readh(dev, buf, 31);
+
+    i=26;
+    while(i < 32) {
+        zm_debug_msg2("Recv ADDBA Rsp:", zmw_rx_buf_readb(dev,buf,i));
+        i++;
+    }
+
+    ac = zcUpToAc[bf.tid&0x7] & 0x3;
+
+    //zmw_enter_critical_section(dev);
+
+    //wd->aggSta[aid].aggFlag[ac] = 0;
+
+    //zmw_leave_critical_section(dev);
+
+    return ZM_SUCCESS;
+}
+
+u16_t   zfAggRecvDelba(zdev_t* dev, zbuf_t* buf)
+{
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    return ZM_SUCCESS;
+}
+
+u16_t   zfAggSendAddbaResponse(zdev_t* dev, struct aggBaFrameParameter *bf)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    //u16_t err;
+    u16_t offset = 0;
+    u16_t hlen = 32;
+    u16_t header[(24+25+1)/2];
+    u16_t vap = 0;
+    u16_t i;
+    u8_t encrypt = 0;
+    u16_t dst[3];
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+
+    /*
+     * TBD : Maximum size of managment frame
+     */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return ZM_SUCCESS;
+    }
+
+    /*
+     * Reserve room for wlan header
+     */
+    offset = hlen;
+
+    /*
+     * add addba frame body
+     */
+    offset = zfAggSetAddbaResponseFrameBody(dev, buf, bf, offset);
+
+
+    zfwBufSetSize(dev, buf, offset);
+
+    /*
+     * Copy wlan header
+     */
+
+    dst[0] = zmw_rx_buf_readh(dev, bf->buf, 10);
+    dst[1] = zmw_rx_buf_readh(dev, bf->buf, 12);
+    dst[2] = zmw_rx_buf_readh(dev, bf->buf, 14);
+    zfAggGenAddbaHeader(dev, dst, header, offset-hlen, buf, vap, encrypt);
+    for (i=0; i<(hlen>>1); i++)
+    {
+        zmw_tx_buf_writeh(dev, buf, i*2, header[i]);
+    }
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    //zm_msg2_mm(ZM_LV_2, "offset=", offset);
+    //zm_msg2_mm(ZM_LV_2, "hlen=", hlen);
+    //zm_msg2_mm(ZM_LV_2, "addrTblSize=", addrTblSize);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.len[0]=", addrTbl.len[0]);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.physAddrl[0]=", addrTbl.physAddrl[0]);
+    //zm_msg2_mm(ZM_LV_2, "buf->data=", buf->data);
+
+    #if 0
+    if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+    #else
+    zfPutVmmq(dev, buf);
+    zfPushVtxq(dev);
+    #endif
+
+    //zfAggSendAddbaRequest(dev, dst, zcUpToAc[bf->tid&0x7] & 0x3, bf->tid);
+    return ZM_SUCCESS;
+
+}
+
+u16_t   zfAggSetAddbaResponseFrameBody(zdev_t* dev, zbuf_t* buf,
+                struct aggBaFrameParameter *bf, u16_t offset)
+{
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    /*
+     * ADDBA Request frame body
+     */
+
+    /*
+     * Category
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, 3);
+    /*
+     * Action details = 0
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_ADDBA_RESPONSE_FRAME);
+    /*
+     * Dialog Token = nonzero
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, bf->dialog);
+    /*
+     * Status code
+     */
+    zmw_tx_buf_writeh(dev, buf, offset, 0);
+    offset+=2;
+    /*
+     * Block Ack parameter set
+     * BA policy = 1 for immediate BA, 0 for delayed BA
+     * TID(4bits) & buffer size(4bits) (TID=0x1 & buffer size=0x80)
+     * TBD: how to get TID number and buffer size?
+     * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+     * ¢x    B0    ¢x    B1     ¢x B2  B5 ¢x B6      B15 ¢x
+     * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+     * ¢x Reserved ¢x BA policy ¢x  TID   ¢x Buffer size ¢x
+     * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+     */
+    zmw_tx_buf_writeh(dev, buf, offset, bf->ba_parameter);
+    offset+=2;
+    /*
+     * BA timeout value
+     */
+    zmw_tx_buf_writeh(dev, buf, offset, bf->ba_timeout);
+    offset+=2;
+
+    return offset;
+}
+
+void   zfAggInvokeBar(zdev_t* dev, TID_TX tid_tx)
+{
+    struct aggBarControl aggBarControl;
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    //bar_control = aggBarControl->tid_info << 12 | aggBarControl->compressed_bitmap << 2
+    //        | aggBarControl->multi_tid << 1 | aggBarControl->bar_ack_policy;
+    aggBarControl.bar_ack_policy = 0;
+    aggBarControl.multi_tid = 0;
+    aggBarControl.compressed_bitmap = 0;
+    aggBarControl.tid_info = tid_tx->tid;
+    zfAggSendBar(dev, tid_tx, &aggBarControl);
+
+    return;
+
+}
+/*
+ * zfAggSendBar() refers zfAggSendAddbaRequest()
+ */
+u16_t   zfAggSendBar(zdev_t* dev, TID_TX tid_tx, struct aggBarControl *aggBarControl)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    //u16_t err;
+    u16_t offset = 0;
+    u16_t hlen = 16+8;  /* mac header + control headers*/
+    u16_t header[(8+24+1)/2];
+    u16_t vap = 0;
+    u16_t i;
+    u8_t encrypt = 0;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+
+    /*
+     * TBD : Maximum size of managment frame
+     */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return ZM_SUCCESS;
+    }
+
+    /*
+     * Reserve room for wlan header
+     */
+    offset = hlen;
+
+    /*
+     * add addba frame body
+     */
+    offset = zfAggSetBarBody(dev, buf, offset, tid_tx, aggBarControl);
+
+
+    zfwBufSetSize(dev, buf, offset);
+
+    /*
+     * Copy wlan header
+     */
+    zfAggGenBarHeader(dev, tid_tx->dst, header, offset-hlen, buf, vap, encrypt);
+    for (i=0; i<(hlen>>1); i++)
+    {
+        zmw_tx_buf_writeh(dev, buf, i*2, header[i]);
+    }
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    //zm_msg2_mm(ZM_LV_2, "offset=", offset);
+    //zm_msg2_mm(ZM_LV_2, "hlen=", hlen);
+    //zm_msg2_mm(ZM_LV_2, "addrTblSize=", addrTblSize);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.len[0]=", addrTbl.len[0]);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.physAddrl[0]=", addrTbl.physAddrl[0]);
+    //zm_msg2_mm(ZM_LV_2, "buf->data=", buf->data);
+
+    #if 0
+    if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+    #else
+    zfPutVmmq(dev, buf);
+    zfPushVtxq(dev);
+    #endif
+
+    return ZM_SUCCESS;
+
+}
+
+u16_t   zfAggSetBarBody(zdev_t* dev, zbuf_t* buf, u16_t offset, TID_TX tid_tx, struct aggBarControl *aggBarControl)
+{
+    u16_t bar_control, start_seq;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    /*
+     * BAR Control frame body
+     */
+
+    /*
+     * BAR Control Field
+     * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+     * ¢x    B0   ¢x    B1     ¢x     B2     ¢x B3   B11 ¢x B12  B15 ¢x
+     * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+     * ¢x BAR Ack ¢x Multi-TID ¢x Compressed ¢x Reserved ¢x TID_INFO ¢x
+     * ¢x  Policy ¢x           ¢x   Bitmap   ¢x          ¢x          ¢x
+     * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+     */
+    bar_control = aggBarControl->tid_info << 12 | aggBarControl->compressed_bitmap << 2
+            | aggBarControl->multi_tid << 1 | aggBarControl->bar_ack_policy;
+
+    zmw_tx_buf_writeh(dev, buf, offset, bar_control);
+    offset+=2;
+    if (0 == aggBarControl->multi_tid) {
+        /*
+         * BA starting sequence number
+         * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+         * ¢x B0       B3 ¢x B4              B15 ¢x
+         * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+         * ¢x Frag num(0) ¢x BA starting seq num ¢x
+         * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+         */
+        start_seq = (tid_tx->bar_ssn << 4) & 0xFFF0;
+        zmw_tx_buf_writeh(dev, buf, offset, start_seq);
+        offset+=2;
+    }
+    if (1 == aggBarControl->multi_tid && 1 == aggBarControl->compressed_bitmap) {
+        /* multi-tid BlockAckReq variant, not implemented*/
+    }
+
+    return offset;
+}
+
+u16_t zfAggGenBarHeader(zdev_t* dev, u16_t* dst,
+        u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt)
+{
+    u8_t  hlen = 16+8;        // MAC ctrl + PHY ctrl + 802.11 MM header
+    //u8_t frameType = ZM_WLAN_FRAME_TYPE_ACTION;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /*
+     * Generate control setting
+     */
+    //bodyLen = zfwBufGetSize(dev, buf);
+    header[0] = 16+len+4;   //Length
+    header[1] = 0x8;        //MAC control, backoff + (ack)
+
+#if 1
+    /* CCK 1M */
+    header[2] = 0x0f00;          //PHY control L
+    header[3] = 0x0000;          //PHY control H
+#else
+    /* CCK 6M */
+    header[2] = 0x0f01;          //PHY control L
+    header[3] = 0x000B;          //PHY control H
+
+#endif
+    /*
+     * Generate WLAN header
+     * Frame control frame type and subtype
+     */
+    header[4+0] = ZM_WLAN_FRAME_TYPE_BAR;
+    /*
+     * Duration
+     */
+    header[4+1] = 0;
+
+    /* Address 1 = DA */
+    header[4+2] = dst[0];
+    header[4+3] = dst[1];
+    header[4+4] = dst[2];
+
+    /* Address 2 = SA */
+    header[4+5] = wd->macAddr[0];
+    header[4+6] = wd->macAddr[1];
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+#ifdef ZM_VAPMODE_MULTILE_SSID
+        header[4+7] = wd->macAddr[2]; //Multiple SSID
+#else
+        header[4+7] = wd->macAddr[2] + (vap<<8); //VAP
+#endif
+    }
+    else
+    {
+        header[4+7] = wd->macAddr[2];
+    }
+
+    /* Sequence Control */
+    zmw_enter_critical_section(dev);
+    header[4+11] = ((wd->mmseq++)<<4);
+    zmw_leave_critical_section(dev);
+
+
+    return hlen;
+}
diff --git a/drivers/staging/otus/80211core/cagg.h b/drivers/staging/otus/80211core/cagg.h
new file mode 100644
index 000000000000..1d87a564162b
--- /dev/null
+++ b/drivers/staging/otus/80211core/cagg.h
@@ -0,0 +1,435 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                          */
+/*  Module Name : cagg.h                                                    */
+/*                                                                          */
+/*  Abstract                                                                */
+/*      This module contains A-MPDU aggregation relatived functions.        */
+/*                                                                          */
+/*  NOTES                                                                   */
+/*      None                                                                */
+/*                                                                          */
+/****************************************************************************/
+/*Revision History:                                                         */
+/*    Who         When        What                                          */
+/*    --------    --------    ----------------------------------------------*/
+/*                                                                          */
+/*    Honda       12-4-06     created                                       */
+/*                                                                          */
+/****************************************************************************/
+
+#ifndef _CAGG_H
+#define _CAGG_H
+
+
+/*
+ * the aggregation functions flag, 0 if don't do aggregate
+ */
+
+#define ZM_AGG_FPGA_DEBUG                   1
+#define ZM_AGG_FPGA_REORDERING              1
+
+#ifndef ZM_AGG_TALLY
+//#define ZM_AGG_TALLY
+#endif
+/*
+ * Aggregate control
+ */
+
+
+#define ZM_AGG_POOL_SIZE                    20
+#define ZM_BAW_POOL_SIZE                    32
+#define ZM_AGGQ_SIZE                        64
+#define ZM_AGGQ_SIZE_MASK                   (ZM_AGGQ_SIZE-1)
+#define ZM_AGG_LOW_THRESHOLD                1
+#define ZM_AGG_HIGH_THRESHOLD               5
+
+/*
+ * number of access categories (ac)
+ */
+#define ZM_AC                               4
+/*
+ * the timer to clear aggregation queue, unit: 1 tick
+ * if the packet is too old (current time - arrival time)
+ * the packet and the aggregate queue will be cleared
+ */
+#define ZM_AGG_CLEAR_TIME                   10
+/*
+ * delete the queue if idle for ZM_DELETE_TIME
+ * unit: 10ms
+ */
+#define ZM_AGG_DELETE_TIME                  10000
+
+/*
+ * block ack window size
+ */
+#define ZM_AGG_BAW_SIZE                     64
+#define ZM_AGG_BAW_MASK                     (ZM_AGG_BAW_SIZE-1)
+/*
+ * originator     ADDBA Resquest    receiver
+ *      |----------------------------->|
+ *     1|          ACK                 |1
+ *      |<-----------------------------|
+ *     2|         ADDBA Response       |2
+ *      |<-----------------------------|
+ *     3|          ACK                 |3
+ *      |----------------------------->|
+ *     4                                4
+ */
+#define ZM_AGG_ADDBA_REQUEST                1
+#define ZM_AGG_ADDBA_REQUEST_ACK            2
+#define ZM_AGG_ADDBA_RESPONSE               3
+#define ZM_AGG_ADDBA_RESPONSE_ACK           4
+
+#define ZM_AGG_SINGLE_MPDU                  00
+#define ZM_AGG_FIRST_MPDU                   01
+#define ZM_AGG_MIDDLE_MPDU                  11
+#define ZM_AGG_LAST_MPDU                    10
+/*
+ * end of Aggregate control
+ */
+
+#define TID_TX  struct aggQueue*
+#define TID_BAW struct baw_q*
+#define BAW wd->baw_enabler
+#define DESTQ wd->destQ
+
+/*
+ * Queue access
+ */
+#define zm_agg_qlen(dev, head, tail) ((head - tail) & ZM_AGGQ_SIZE_MASK)
+#define zm_agg_inQ(tid_tx, pt) ((((pt - tid_tx->aggTail) & ZM_AGGQ_SIZE_MASK) < \
+        ((tid_tx->aggHead - tid_tx->aggTail) & ZM_AGGQ_SIZE_MASK))? TRUE:FALSE)
+#define zm_agg_plus(pt) pt = (pt + 1) & ZM_AGGQ_SIZE_MASK
+#define zm_agg_min(A, B) ((A>B)? B:A)
+#define zm_agg_GetTime() wd->tick
+#define TXQL (zfHpGetMaxTxdCount(dev) - zfHpGetFreeTxdCount(dev))
+
+/* don't change AGG_MIN_TXQL easily, this might cause BAW BSOD */
+#define AGG_MIN_TXQL                        2
+/*
+ * consider tcp,udp,ac(1234)
+ */
+#define zm_agg_dynamic_threshold(dev, ar)   ((ar > 16)? 11: \
+                                             (ar > 12)? 8: \
+                                             (ar > 8)? 5: \
+                                             (ar > 4)? 2:1)
+#define zm_agg_weight(ac)   ((3 == ac)? 4: \
+                             (2 == ac)? 3: \
+                             (0 == ac)? 2:1)
+/*
+ * the required free queue ratio per ac
+ */
+
+#define zm_agg_ratio(ac)    ((3 == ac)? 3: \
+                             (2 == ac)? (zfHpGetMaxTxdCount(dev)*1/4): \
+                             (0 == ac)? (zfHpGetMaxTxdCount(dev)*2/4): \
+                                        (zfHpGetMaxTxdCount(dev)*3/4))
+
+//#define zm_agg_ratio(ac)    3
+/*
+ * end of Queue access
+ */
+
+#define ZM_AGGMSG_LEV    ZM_LV_3
+#define zm_msg0_agg(lv, msg) if (ZM_AGGMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_agg(lv, msg, val) if (ZM_AGGMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_agg(lv, msg, val) if (ZM_AGGMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+struct baw_header_r {
+    u16_t       *header;
+    u16_t       *mic;
+    u16_t       *snap;
+    u16_t       headerLen;
+    u16_t       micLen;
+    u16_t       snapLen;
+    u16_t       removeLen;
+    u8_t        keyIdx;
+};
+
+struct baw_header {
+    u16_t       header[29];//[(8+30+2+18)/2];  58 bytes  /* ctr+(4+a1+a2+a3+2+a4)+qos+iv */
+    u16_t       headerLen;
+    u16_t       mic[4]; //[8/2]; 8 bytes
+    u16_t       micLen;
+    u16_t       snap[4]; //[8/2]; 8 bytes
+    u16_t       snapLen;
+    u16_t       removeLen;
+    u8_t        keyIdx;
+};
+
+struct bufInfo {
+    zbuf_t*     buf;
+    u8_t        baw_retransmit;
+    u32_t       timestamp;
+    struct baw_header   *baw_header;
+};
+#endif
+struct aggElement
+{
+    zbuf_t*     buf;
+    u32_t       arrivalTime;
+    u8_t        baw_retransmit;
+    struct zsAdditionInfo addInfo;
+    //struct baw_header  baw_header;
+};
+
+
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+struct baw_buf
+{
+    zbuf_t*     buf;
+    u16_t       baw_seq;
+    u32_t       timestamp;
+    u8_t        baw_retransmit;
+    struct baw_header baw_header;
+};
+
+struct baw_q {
+    struct baw_buf  frame[ZM_VTXQ_SIZE];
+    u16_t       enabled;
+    u16_t       start_seq;
+    u16_t       head;
+    u16_t       tail;
+    u16_t       size;
+    TID_TX      tid_tx;
+
+    //struct baw_header *baw_header;
+};
+
+struct baw_enabler
+{
+    struct baw_q    tid_baw[ZM_BAW_POOL_SIZE];
+    u8_t    delPoint;
+    void    (*core)(zdev_t* dev, u16_t baw_seq, u32_t bitmap, u16_t aggLen);
+    //void    (*core);
+    void    (*init)(zdev_t* dev);
+    TID_BAW (*getNewQ)(zdev_t* dev, u16_t start_seq, TID_TX tid_tx);
+    TID_BAW (*getQ)(zdev_t* dev, u16_t baw_seq);
+    u16_t   (*insert)(zdev_t* dev, zbuf_t* buf, u16_t baw_seq, TID_BAW tid_baw, u8_t baw_retransmit, struct baw_header_r *header_r);
+    struct bufInfo* (*pop)(zdev_t* dev, u16_t index, TID_BAW tid_baw);
+    void    (*enable)(zdev_t* dev, TID_BAW tid_baw, u16_t start_seq);
+    void    (*disable)(zdev_t* dev, TID_BAW tid_baw);
+
+};
+#endif
+struct aggQueue
+{
+    struct      aggElement  aggvtxq[ZM_AGGQ_SIZE];
+    u16_t       aggHead;
+    u16_t       aggTail;
+    s16_t       size;
+    u16_t       aggQSTA;
+    u16_t       aggQEnabled;
+    u16_t       ac;
+    u16_t       tid;
+    u16_t       aggReady;
+    u16_t       clearFlag;
+    u16_t       deleteFlag;
+    u32_t       lastArrival;
+    u16_t       aggFrameSize;
+    u16_t       bar_ssn;    /* starting sequence number in BAR */
+    u16_t       dst[3];
+    u16_t       complete;     /* complete indication pointer */
+};
+
+struct aggSta
+{
+    u16_t       count[ZM_AC];
+    TID_TX      tid_tx[8];
+    u16_t       aggFlag[ZM_AC];
+};
+
+struct agg_tid_rx
+{
+    u16_t       aid;
+    u16_t       ac;
+    u16_t       addBaExchangeStatusCode;
+    //struct zsAdditionInfo *addInfo;
+    u16_t       seq_start;		/* first seq expected next */
+    u16_t       baw_head;		/* head of valid block ack window */
+    u16_t       baw_tail;		/* tail of valid block ack window */
+    //u16_t       free_count;		/* block ack window size	*/
+    u8_t        sq_exceed_count;
+    u8_t        sq_behind_count;
+    struct aggElement frame[ZM_AGG_BAW_SIZE + 1]; /* out-of-order rx frames */
+};
+
+struct aggControl
+{
+    u16_t       aggEnabled;
+    u16_t       ampduIndication;
+    u16_t       addbaIndication;
+    //TID_BAW     tid_baw;
+    u32_t       timestamp;
+};
+
+struct aggBaFrameParameter
+{
+    zbuf_t*     buf;
+    u16_t       ba_parameter;
+    u8_t        dialog;
+    u16_t       ba_policy;
+    u16_t       tid;
+    u16_t       buffer_size;
+    u16_t       ba_timeout;
+    u16_t       ba_start_seq;
+    u16_t       status_code;
+};
+
+struct aggBarControl
+{
+    u16_t       bar_ack_policy      ;
+    u16_t       multi_tid           ;
+    u16_t       compressed_bitmap   ;
+    u16_t       tid_info            ;
+};
+
+struct aggTally
+{
+    u32_t       got_packets_sum;
+    u32_t       got_bytes_sum;
+    u32_t       sent_packets_sum;
+    u32_t       sent_bytes_sum;
+    u32_t       avg_got_packets;
+    u32_t       avg_got_bytes;
+    u32_t       avg_sent_packets;
+    u32_t       avg_sent_bytes;
+    u16_t       time;
+};
+
+
+struct destQ {
+    struct dest{
+        u16_t   Qtype : 1; /* 0 aggr, 1 vtxq */
+        TID_TX  tid_tx;
+        void*   vtxq;
+
+        struct dest* next;
+    } *dest[4];
+    struct dest* Head[4];
+    //s16_t   size[4];
+    u16_t   ppri;
+    void    (*insert)(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq);
+    void    (*delete)(zdev_t* dev, u16_t Qtype, TID_TX tid_tx, void* vtxq);
+    void    (*init)(zdev_t* dev);
+    struct dest* (*getNext)(zdev_t* dev, u16_t ac);
+    u16_t   (*exist)(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq);
+    //void    (*scan)(zdev_t* dev);
+};
+/*
+ * aggregation tx
+ */
+void    zfAggInit(zdev_t* dev);
+u16_t   zfApFindSta(zdev_t* dev, u16_t* addr);
+u16_t   zfAggGetSta(zdev_t* dev, zbuf_t* buf);
+TID_TX  zfAggTxGetQueue(zdev_t* dev, u16_t aid, u16_t tid);
+TID_TX  zfAggTxNewQueue(zdev_t* dev, u16_t aid, u16_t tid, zbuf_t* buf);
+u16_t   zfAggTxEnqueue(zdev_t* dev, zbuf_t* buf, u16_t aid, TID_TX tid_tx);
+u16_t   zfAggTx(zdev_t* dev, zbuf_t* buf, u16_t tid);
+u16_t   zfAggTxReadyCount(zdev_t* dev, u16_t ac);
+u16_t   zfAggTxPartial(zdev_t* dev, u16_t ac, u16_t readycount);
+u16_t   zfAggTxSend(zdev_t* dev, u32_t freeTxd, TID_TX tid_tx);
+TID_TX  zfAggTxGetReadyQueue(zdev_t* dev, u16_t ac);
+zbuf_t* zfAggTxGetVtxq(zdev_t* dev, TID_TX tid_tx);
+u16_t   zfAggTxDeleteQueue(zdev_t* dev, u16_t qnum);
+u16_t   zfAggScanAndClear(zdev_t* dev, u32_t time);
+u16_t   zfAggClearQueue(zdev_t* dev);
+void    zfAggTxScheduler(zdev_t* dev, u8_t ScanAndClear);
+
+/* tid_tx manipulation */
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+u16_t   zfAggTidTxInsertHead(zdev_t* dev, struct bufInfo* buf_info, TID_TX tid_tx);
+#endif
+void    zfAggDestInsert(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq);
+void    zfAggDestDelete(zdev_t* dev, u16_t Qtype, TID_TX tid_tx, void* vtxq);
+void    zfAggDestInit(zdev_t* dev);
+struct dest* zfAggDestGetNext(zdev_t* dev, u16_t ac);
+u16_t   zfAggDestExist(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq);
+/*
+ * aggregation rx
+ */
+struct agg_tid_rx *zfAggRxEnabled(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggRx(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo *addInfo, struct agg_tid_rx *tid_rx);
+struct agg_tid_rx *zfAggRxGetQueue(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggRxEnqueue(zdev_t* dev, zbuf_t* buf, struct agg_tid_rx *tid_rx, struct zsAdditionInfo *addInfo);
+u16_t   zfAggRxFlush(zdev_t* dev, u16_t seq_no, struct agg_tid_rx *tid_rx);
+u16_t   zfAggRxFreeBuf(zdev_t* dev, u16_t destroy);
+u16_t   zfAggRxClear(zdev_t* dev, u32_t time);
+void    zfAggRecvBAR(zdev_t* dev, zbuf_t* buf);
+/*
+ * end of aggregation rx
+ */
+
+/*
+ * ADDBA
+ */
+u16_t   zfAggSendAddbaRequest(zdev_t* dev, u16_t *dst, u16_t ac, u16_t up);
+u16_t   zfAggSetAddbaFrameBody(zdev_t* dev,zbuf_t* buf, u16_t offset, u16_t ac, u16_t up);
+u16_t   zfAggGenAddbaHeader(zdev_t* dev, u16_t* dst,
+                u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt);
+u16_t   zfAggProcessAction(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggBlockAckActionFrame(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggRecvAddbaRequest(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggRecvAddbaResponse(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggRecvDelba(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggSendAddbaResponse(zdev_t* dev, struct aggBaFrameParameter *bf);
+u16_t   zfAggSetAddbaResponseFrameBody(zdev_t* dev, zbuf_t* buf,
+                struct aggBaFrameParameter *bf, u16_t offset);
+u16_t   zfAggAddbaSetTidRx(zdev_t* dev, zbuf_t* buf,
+                struct aggBaFrameParameter *bf);
+/*
+ * zfAggTxSendEth
+ */
+u16_t zfAggTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port, u16_t bufType, u8_t flag, struct aggControl *aggControl, TID_TX tid_tx);
+
+/*
+ * statistics functions
+ */
+u16_t zfAggTallyReset(zdev_t* dev);
+
+u16_t   zfAggPrintTally(zdev_t* dev);
+
+/*
+ * BAR
+ */
+void    zfAggInvokeBar(zdev_t* dev, TID_TX tid_tx);
+u16_t   zfAggSendBar(zdev_t* dev, TID_TX tid_tx, struct aggBarControl *aggBarControl);
+u16_t   zfAggSetBarBody(zdev_t* dev, zbuf_t* buf, u16_t offset, TID_TX tid_tx, struct aggBarControl *aggBarControl);
+u16_t   zfAggGenBarHeader(zdev_t* dev, u16_t* dst,
+                u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt);
+
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+/* BAW BA retransmission */
+void    zfBawCore(zdev_t* dev, u16_t baw_seq, u32_t bitmap, u16_t aggLen);
+void    zfBawInit(zdev_t* dev);
+TID_BAW zfBawGetNewQ(zdev_t* dev, u16_t start_seq, TID_TX tid_tx);
+u16_t   zfBawInsert(zdev_t* dev, zbuf_t* buf, u16_t baw_seq, TID_BAW tid_baw, u8_t baw_retransmit, struct baw_header_r *header_r);
+struct bufInfo* zfBawPop(zdev_t* dev, u16_t index, TID_BAW tid_baw);
+void    zfBawEnable(zdev_t* dev, TID_BAW tid_baw, u16_t start_seq);
+void    zfBawDisable(zdev_t* dev, TID_BAW tid_baw);
+TID_BAW zfBawGetQ(zdev_t* dev, u16_t baw_seq);
+void zfAggTxRetransmit(zdev_t* dev, struct bufInfo *buf_info, struct aggControl *aggControl, TID_TX tid_tx);
+#endif
+/* extern functions */
+extern zbuf_t* zfGetVtxq(zdev_t* dev, u8_t ac);
+
+#endif /* #ifndef _CAGG_H */
+
diff --git a/drivers/staging/otus/80211core/ccmd.c b/drivers/staging/otus/80211core/ccmd.c
new file mode 100644
index 000000000000..479977973671
--- /dev/null
+++ b/drivers/staging/otus/80211core/ccmd.c
@@ -0,0 +1,1861 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : cmd.c                                                 */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains command interface functions.               */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "../hal/hpreg.h"
+
+
+u16_t zfWlanReset(zdev_t* dev);
+u32_t zfUpdateRxRate(zdev_t* dev);
+
+
+extern void zfiUsbRecv(zdev_t *dev, zbuf_t *buf);
+extern void zfiUsbRegIn(zdev_t* dev, u32_t* rsp, u16_t rspLen);
+extern void zfiUsbOutComplete(zdev_t* dev, zbuf_t *buf, u8_t status, u8_t *hdr);
+extern void zfiUsbRegOutComplete(zdev_t* dev);
+extern u16_t zfHpReinit(zdev_t* dev, u32_t frequency);
+
+/* Get size (byte) of driver core global data structure.    */
+/* This size will be used by driver wrapper to allocate     */
+/* a memory space for driver core to store global variables */
+u16_t zfiGlobalDataSize(zdev_t* dev)
+{
+    u32_t ret;
+    ret = (sizeof(struct zsWlanDev));
+    zm_assert((ret>>16) == 0);
+    return (u16_t)ret;
+}
+
+
+/* Initialize WLAN hardware and software, resource will be allocated */
+/* for WLAN operation, must be called first before other function.   */
+extern u16_t zfiWlanOpen(zdev_t* dev, struct zsCbFuncTbl* cbFuncTbl)
+{
+    //u16_t ret;
+    //u32_t i;
+    //u8_t* ch;
+    //u8_t  bPassive;
+    u32_t devSize;
+    struct zfCbUsbFuncTbl cbUsbFuncTbl;
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg0("start");
+
+    devSize = sizeof(struct zsWlanDev);
+    /* Zeroize zsWlanDev struct */
+    zfZeroMemory((u8_t*)wd, (u16_t)devSize);
+
+#ifdef ZM_ENABLE_AGGREGATION
+    zfAggInit(dev);
+#endif
+
+    zfCwmInit(dev);
+
+    wd->commTally.RateCtrlTxMPDU = 0;
+    wd->commTally.RateCtrlBAFail = 0;
+    wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_SHORT;
+
+    if (cbFuncTbl == NULL)
+    {
+        /* zfcbRecvEth() is mandatory */
+        zm_assert(0);
+    }
+    else
+    {
+        if (cbFuncTbl->zfcbRecvEth == NULL)
+        {
+            /* zfcbRecvEth() is mandatory */
+            zm_assert(0);
+        }
+        wd->zfcbAuthNotify = cbFuncTbl->zfcbAuthNotify;
+        wd->zfcbAuthNotify = cbFuncTbl->zfcbAuthNotify;
+        wd->zfcbAsocNotify = cbFuncTbl->zfcbAsocNotify;
+        wd->zfcbDisAsocNotify = cbFuncTbl->zfcbDisAsocNotify;
+        wd->zfcbApConnectNotify = cbFuncTbl->zfcbApConnectNotify;
+        wd->zfcbConnectNotify = cbFuncTbl->zfcbConnectNotify;
+        wd->zfcbScanNotify = cbFuncTbl->zfcbScanNotify;
+        wd->zfcbMicFailureNotify = cbFuncTbl->zfcbMicFailureNotify;
+        wd->zfcbApMicFailureNotify = cbFuncTbl->zfcbApMicFailureNotify;
+        wd->zfcbIbssPartnerNotify = cbFuncTbl->zfcbIbssPartnerNotify;
+        wd->zfcbMacAddressNotify = cbFuncTbl->zfcbMacAddressNotify;
+        wd->zfcbSendCompleteIndication = cbFuncTbl->zfcbSendCompleteIndication;
+        wd->zfcbRecvEth = cbFuncTbl->zfcbRecvEth;
+        wd->zfcbRestoreBufData = cbFuncTbl->zfcbRestoreBufData;
+        wd->zfcbRecv80211 = cbFuncTbl->zfcbRecv80211;
+#ifdef ZM_ENABLE_CENC
+        wd->zfcbCencAsocNotify = cbFuncTbl->zfcbCencAsocNotify;
+#endif //ZM_ENABLE_CENC
+        wd->zfcbClassifyTxPacket = cbFuncTbl->zfcbClassifyTxPacket;
+        wd->zfcbHwWatchDogNotify = cbFuncTbl->zfcbHwWatchDogNotify;
+    }
+
+    //add by honda 0330
+    cbUsbFuncTbl.zfcbUsbRecv = zfiUsbRecv;
+    cbUsbFuncTbl.zfcbUsbRegIn = zfiUsbRegIn;
+    cbUsbFuncTbl.zfcbUsbOutComplete = zfiUsbOutComplete;
+    cbUsbFuncTbl.zfcbUsbRegOutComplete = zfiUsbRegOutComplete;
+    zfwUsbRegisterCallBack(dev, &cbUsbFuncTbl);
+    /* Init OWN MAC address */
+    wd->macAddr[0] = 0x8000;
+    wd->macAddr[1] = 0x0000;
+    wd->macAddr[2] = 0x0000;
+
+    wd->regulationTable.regionCode = 0xffff;
+
+    zfHpInit(dev, wd->frequency);
+
+    /* init region code */
+    //wd->regulationTable.regionCode = NULL1_WORLD; //Only 2.4g RegCode
+    //zfHpGetRegulationTablefromRegionCode(dev, NULL1_WORLD);
+    //zfiWlanSetDot11DMode(dev , 1); // Enable 802.11d
+    /* Get the first channel */
+    //wd->frequency = zfChGetFirstChannel(dev, &bPassive);
+#ifdef ZM_AP_DEBUG
+    //wd->frequency = 2437;
+#endif
+
+    //STA mode
+    wd->sta.mTxRate = 0x0;
+    wd->sta.uTxRate = 0x3;
+    wd->sta.mmTxRate = 0x0;
+    wd->sta.adapterState = ZM_STA_STATE_DISCONNECT;
+    wd->sta.capability[0] = 0x01;
+    wd->sta.capability[1] = 0x00;
+
+    wd->sta.preambleTypeHT = 0;
+    wd->sta.htCtrlBandwidth = 0;
+    wd->sta.htCtrlSTBC = 0;
+    wd->sta.htCtrlSG = 0;
+    wd->sta.defaultTA = 0;
+    //wd->sta.activescanTickPerChannel = ZM_TIME_ACTIVE_SCAN/ZM_MS_PER_TICK;
+	{
+		u8_t Dur = ZM_TIME_ACTIVE_SCAN;
+		zfwGetActiveScanDur(dev, &Dur);
+		wd->sta.activescanTickPerChannel = Dur/ZM_MS_PER_TICK;
+
+	}
+    wd->sta.passiveScanTickPerChannel = ZM_TIME_PASSIVE_SCAN/ZM_MS_PER_TICK;
+    wd->sta.bAutoReconnect = TRUE;
+    wd->sta.dropUnencryptedPkts = FALSE;
+
+    /* set default to bypass all multicast packet for linux, window XP would set 0 by wrapper initialization */
+	wd->sta.bAllMulticast = 1;
+
+    /* Initial the RIFS Status / RIFS-like frame count / RIFS count */
+    wd->sta.rifsState = ZM_RIFS_STATE_DETECTING;
+    wd->sta.rifsLikeFrameCnt = 0;
+    wd->sta.rifsCount = 0;
+
+    wd->sta.osRxFilter = 0;
+    wd->sta.bSafeMode = 0;
+
+    //Common
+    zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_DISCONNECT);
+    wd->beaconInterval = 100;
+    wd->rtsThreshold = 2346;
+    wd->fragThreshold = 32767;
+    wd->wlanMode = ZM_MODE_INFRASTRUCTURE;
+    wd->txMCS = 0xff;    //AUTO
+    wd->dtim = 1;
+    //wd->txMT = 1;       //OFDM
+    wd->tick = 1;
+    wd->maxTxPower2 = 0xff;
+    wd->maxTxPower5 = 0xff;
+    wd->supportMode = 0xffffffff;
+    wd->ws.adhocMode = ZM_ADHOCBAND_G;
+    wd->ws.autoSetFrequency = 0xff;
+
+    //AP mode
+    //wd->bgMode = wd->ws.bgMode;
+    wd->ap.ssidLen[0] = 6;
+    wd->ap.ssid[0][0] = 'Z';
+    wd->ap.ssid[0][1] = 'D';
+    wd->ap.ssid[0][2] = '1';
+    wd->ap.ssid[0][3] = '2';
+    wd->ap.ssid[0][4] = '2';
+    wd->ap.ssid[0][5] = '1';
+
+    // Init the country iso name as NA
+    wd->ws.countryIsoName[0] = 0;
+    wd->ws.countryIsoName[1] = 0;
+    wd->ws.countryIsoName[2] = '\0';
+
+	/* init fragmentation is disabled */
+	//zfiWlanSetFragThreshold(dev, 0);
+
+	/* airopeek : swSniffer 1=>on  0=>off */
+	wd->swSniffer = 0;
+    wd->XLinkMode = 0;
+
+// jhlee HT 0
+#if 1
+    /* AP Mode*/
+    /* Init HT Capability Info */
+    wd->ap.HTCap.Data.ElementID = ZM_WLAN_EID_HT_CAPABILITY;
+    wd->ap.HTCap.Data.Length = 26;
+    //wd->ap.HTCap.Data.SupChannelWidthSet = 0;
+    //wd->ap.HTCap.Data.MIMOPowerSave = 3;
+    //wd->ap.HTCap.Data.ShortGIfor40MHz = 0;
+    //wd->ap.HTCap.Data.ShortGIfor20MHz = 0;
+    //wd->ap.HTCap.Data.DSSSandCCKin40MHz = 0;
+    wd->ap.HTCap.Data.AMPDUParam |= HTCAP_MaxRxAMPDU3;
+    wd->ap.HTCap.Data.MCSSet[0] = 0xFF; // MCS 0 ~  7
+    wd->ap.HTCap.Data.MCSSet[1] = 0xFF; // MCS 8 ~ 15
+
+    /* Init Extended HT Capability Info */
+    wd->ap.ExtHTCap.Data.ElementID = ZM_WLAN_EID_EXTENDED_HT_CAPABILITY;
+    wd->ap.ExtHTCap.Data.Length = 22;
+    wd->ap.ExtHTCap.Data.ControlChannel = 6;
+    //wd->ap.ExtHTCap.Data.ExtChannelOffset = 3;
+    wd->ap.ExtHTCap.Data.ChannelInfo |= ExtHtCap_RecomTxWidthSet;
+    //wd->ap.ExtHTCap.Data.RIFSMode = 1;
+    wd->ap.ExtHTCap.Data.OperatingInfo |= 1;
+
+    /* STA Mode*/
+    /* Init HT Capability Info */
+    wd->sta.HTCap.Data.ElementID = ZM_WLAN_EID_HT_CAPABILITY;
+    wd->sta.HTCap.Data.Length = 26;
+
+    /* Test with 5G-AP : 7603 */
+    //wd->sta.HTCap.Data.SupChannelWidthSet = 1;
+    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_SMEnabled;
+    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_SupChannelWidthSet;
+    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_ShortGIfor40MHz;
+    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_DSSSandCCKin40MHz;
+#ifndef ZM_DISABLE_AMSDU8K_SUPPORT
+    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_MaxAMSDULength;
+#endif
+    //wd->sta.HTCap.Data.MIMOPowerSave = 0;
+    //wd->sta.HTCap.Data.ShortGIfor40MHz = 0;
+    //wd->sta.HTCap.Data.ShortGIfor20MHz = 0;
+    //wd->sta.HTCap.Data.DSSSandCCKin40MHz = 0;
+    wd->sta.HTCap.Data.AMPDUParam |= HTCAP_MaxRxAMPDU3;
+    wd->sta.HTCap.Data.MCSSet[0] = 0xFF; // MCS 0 ~  7
+    wd->sta.HTCap.Data.MCSSet[1] = 0xFF; // MCS 8 ~ 15
+    wd->sta.HTCap.Data.PCO |= HTCAP_TransmissionTime3;
+    //wd->sta.HTCap.Data.TransmissionTime = 0;
+    /* Init Extended HT Capability Info */
+    wd->sta.ExtHTCap.Data.ElementID = ZM_WLAN_EID_EXTENDED_HT_CAPABILITY;
+    wd->sta.ExtHTCap.Data.Length = 22;
+    wd->sta.ExtHTCap.Data.ControlChannel = 6;
+
+    //wd->sta.ExtHTCap.Data.ExtChannelOffset |= 3;
+    wd->sta.ExtHTCap.Data.ChannelInfo |= ExtHtCap_ExtChannelOffsetBelow;
+
+    //wd->sta.ExtHTCap.Data.RecomTxWidthSet = 1;
+    //wd->sta.ExtHTCap.Data.RIFSMode = 1;
+    wd->sta.ExtHTCap.Data.OperatingInfo |= 1;
+#endif
+
+#if 0
+    /* WME test code */
+    wd->ap.qosMode[0] = 1;
+#endif
+
+    wd->ledStruct.ledMode[0] = 0x2221;
+    wd->ledStruct.ledMode[1] = 0x2221;
+
+    zfTimerInit(dev);
+
+    ZM_PERFORMANCE_INIT(dev);
+
+    zfBssInfoCreate(dev);
+    zfScanMgrInit(dev);
+    zfPowerSavingMgrInit(dev);
+
+#if 0
+    /* Test code */
+    {
+        u32_t key[4] = {0xffffffff, 0xff, 0, 0};
+        u16_t addr[3] = {0x8000, 0x01ab, 0x0000};
+        //zfSetKey(dev, 0, 0, ZM_WEP64, addr, key);
+        //zfSetKey(dev, 0, 0, ZM_AES, addr, key);
+        //zfSetKey(dev, 64, 0, 1, wd->macAddr, key);
+    }
+#endif
+
+    // WME settings
+    wd->ws.staWmeEnabled = 1;           // Enable WME by default
+    #define ZM_UAPSD_Q_SIZE 32 //2^N
+    wd->ap.uapsdQ = zfQueueCreate(dev, ZM_UAPSD_Q_SIZE);
+    zm_assert(wd->ap.uapsdQ != NULL);
+    wd->sta.uapsdQ = zfQueueCreate(dev, ZM_UAPSD_Q_SIZE);
+    zm_assert(wd->sta.uapsdQ != NULL);
+
+    //zfHpInit(dev, wd->frequency);
+
+    /* MAC address */
+    //zfHpSetMacAddress(dev, wd->macAddr, 0);
+    zfHpGetMacAddress(dev);
+
+    zfCoreSetFrequency(dev, wd->frequency);
+
+#if ZM_PCI_LOOP_BACK == 1
+    zfwWriteReg(dev, ZM_REG_PCI_CONTROL, 6);
+#endif /* #if ZM_PCI_LOOP_BACK == 1 */
+
+    //zfiWlanSetDot11DMode(dev , 1); // Enable 802.11d
+    //zfiWlanSetDot11HDFSMode(dev , 1); // Enable 802.11h DFS
+    wd->sta.DFSEnable = 1;
+    wd->sta.capability[1] |= ZM_BIT_0;
+
+    //zfiWlanSetFrequency(dev, 5260000, TRUE);
+    //zfiWlanSetAniMode(dev , 1); // Enable ANI
+
+    /* Trgger Rx DMA */
+    zfHpStartRecv(dev);
+
+    zm_debug_msg0("end");
+
+    return 0;
+}
+
+/* WLAN hardware will be shutdown and all resource will be release */
+u16_t zfiWlanClose(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_msg0_init(ZM_LV_0, "enter");
+
+    wd->state = ZM_WLAN_STATE_CLOSEDED;
+
+    //zfiWlanDisable(dev, 1);
+    zfWlanReset(dev);
+
+    zfHpStopRecv(dev);
+
+    /* Disable MAC */
+    /* Disable PHY */
+    /* Disable RF */
+
+    zfHpRelease(dev);
+
+    zfQueueDestroy(dev, wd->ap.uapsdQ);
+    zfQueueDestroy(dev, wd->sta.uapsdQ);
+
+    zfBssInfoDestroy(dev);
+
+#ifdef ZM_ENABLE_AGGREGATION
+    /* add by honda */
+    zfAggRxFreeBuf(dev, 1);  //1 for release structure memory
+    /* end of add by honda */
+#endif
+
+    zm_msg0_init(ZM_LV_0, "exit");
+
+    return 0;
+}
+
+void zfGetWrapperSetting(zdev_t* dev)
+{
+    u8_t   bPassive;
+    u16_t vapId = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+#if 0
+    if ( (wd->ws.countryIsoName[0] != 0)
+         || (wd->ws.countryIsoName[1] != 0)
+         || (wd->ws.countryIsoName[2] != '\0') )
+    {
+        zfHpGetRegulationTablefromRegionCode(
+            dev,
+            zfHpGetRegionCodeFromIsoName(dev, wd->ws.countryIsoName) );
+    }
+#endif
+    zmw_enter_critical_section(dev);
+
+    wd->wlanMode = wd->ws.wlanMode;
+
+    /* set channel */
+    if ( wd->ws.frequency )
+    {
+        wd->frequency = wd->ws.frequency;
+        wd->ws.frequency = 0;
+    }
+    else
+    {
+        wd->frequency = zfChGetFirstChannel(dev, &bPassive);
+
+        if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            if (wd->ws.adhocMode == ZM_ADHOCBAND_A)
+            {
+                wd->frequency = ZM_CH_A_36;
+            }
+            else
+            {
+            wd->frequency = ZM_CH_G_6;
+            }
+        }
+    }
+#ifdef ZM_AP_DEBUG
+    /* honda add for debug, 2437 channel 6, 2452 channel 9 */
+    wd->frequency = 2437;
+    /* end of add by honda */
+#endif
+
+    /* set preamble type */
+    switch (wd->ws.preambleType)
+    {
+    case ZM_PREAMBLE_TYPE_AUTO:
+    case ZM_PREAMBLE_TYPE_SHORT:
+    case ZM_PREAMBLE_TYPE_LONG:
+        wd->preambleType = wd->ws.preambleType;
+        break;
+    default:
+        wd->preambleType = ZM_PREAMBLE_TYPE_SHORT;
+        break;
+    }
+    wd->ws.preambleType = 0;
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        vapId = zfwGetVapId(dev);
+
+        if (vapId == 0xffff)
+        {
+            wd->ap.authAlgo[0] = wd->ws.authMode;
+            wd->ap.encryMode[0] = wd->ws.encryMode;
+        }
+        else
+        {
+            wd->ap.authAlgo[vapId + 1] = wd->ws.authMode;
+            wd->ap.encryMode[vapId + 1] = wd->ws.encryMode;
+        }
+        wd->ws.authMode = 0;
+        wd->ws.encryMode = ZM_NO_WEP;
+
+        /* Get beaconInterval from WrapperSetting */
+        if ((wd->ws.beaconInterval >= 20) && (wd->ws.beaconInterval <= 1000))
+        {
+            wd->beaconInterval = wd->ws.beaconInterval;
+        }
+        else
+        {
+            wd->beaconInterval = 100; //100ms
+        }
+
+        if (wd->ws.dtim > 0)
+        {
+            wd->dtim = wd->ws.dtim;
+        }
+        else
+        {
+            wd->dtim = 1;
+        }
+
+        wd->ap.qosMode = wd->ws.apWmeEnabled & 0x1;
+        wd->ap.uapsdEnabled = (wd->ws.apWmeEnabled & 0x2) >> 1;
+    }
+    else
+    {
+        wd->sta.authMode = wd->ws.authMode;
+        wd->sta.currentAuthMode = wd->ws.authMode;
+        wd->sta.wepStatus = wd->ws.wepStatus;
+
+        if ( wd->ws.beaconInterval )
+        {
+            wd->beaconInterval = wd->ws.beaconInterval;
+        }
+        else
+        {
+            wd->beaconInterval = 0x64;
+        }
+
+        if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            /* 1. Set default channel 6 (2437MHz) */
+//            wd->frequency = 2437;
+
+            /* 2. Otus support 802.11g Mode */
+            if ((wd->ws.adhocMode == ZM_ADHOCBAND_G) ||
+                (wd->ws.adhocMode == ZM_ADHOCBAND_BG) ||
+                (wd->ws.adhocMode == ZM_ADHOCBAND_ABG) ) {
+                wd->wfc.bIbssGMode = 1;
+            } else {
+                wd->wfc.bIbssGMode = 0;
+            }
+
+            /* 3. set short preamble  */
+            //wd->sta.preambleType = ZM_PREAMBLE_TYPE_SHORT ;
+        }
+
+        /* set ATIM window */
+        if ( wd->ws.atimWindow )
+        {
+            wd->sta.atimWindow = wd->ws.atimWindow;
+        }
+        else
+        {
+            //wd->sta.atimWindow = 0x0a;
+            wd->sta.atimWindow = 0;
+        }
+
+        //wd->sta.connectingHiddenAP = 1;//wd->ws.connectingHiddenAP;
+        wd->sta.dropUnencryptedPkts = wd->ws.dropUnencryptedPkts;
+        wd->sta.ibssJoinOnly = wd->ws.ibssJoinOnly;
+
+        if ( wd->ws.bDesiredBssid )
+        {
+            zfMemoryCopy(wd->sta.desiredBssid, wd->ws.desiredBssid, 6);
+            wd->sta.bDesiredBssid = TRUE;
+            wd->ws.bDesiredBssid = FALSE;
+        }
+        else
+        {
+            wd->sta.bDesiredBssid = FALSE;
+        }
+
+        /* check ssid */
+        if ( wd->ws.ssidLen != 0 )
+        {
+            if ( (!zfMemoryIsEqual(wd->ws.ssid, wd->sta.ssid,
+                                   wd->sta.ssidLen))||
+                 (wd->ws.ssidLen != wd->sta.ssidLen)||
+                 (wd->sta.authMode == ZM_AUTH_MODE_WPA)||
+                 (wd->sta.authMode == ZM_AUTH_MODE_WPAPSK) ||
+                 (wd->ws.staWmeQosInfo!= 0) )
+            {
+                /*if u-APSD test(set QosInfo), clear connectByReasso to do association (not reassociation)*/
+                wd->sta.connectByReasso = FALSE;
+                wd->sta.failCntOfReasso = 0;
+                wd->sta.pmkidInfo.bssidCount = 0;
+
+                wd->sta.ssidLen = wd->ws.ssidLen;
+                zfMemoryCopy(wd->sta.ssid, wd->ws.ssid, wd->sta.ssidLen);
+
+                if ( wd->sta.ssidLen < 32 )
+                {
+                    wd->sta.ssid[wd->sta.ssidLen] = 0;
+                }
+            }
+        }
+        else
+        {   /* ANY BSS */
+            wd->sta.ssid[0] = 0;
+            wd->sta.ssidLen = 0;
+        }
+
+        wd->sta.wmeEnabled = wd->ws.staWmeEnabled;
+        wd->sta.wmeQosInfo = wd->ws.staWmeQosInfo;
+
+    }
+
+    zmw_leave_critical_section(dev);
+}
+
+u16_t zfWlanEnable(zdev_t* dev)
+{
+    u8_t     bssid[6] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( wd->wlanMode == ZM_MODE_UNKNOWN )
+    {
+        zm_debug_msg0("Unknown Mode...Skip...");
+        return 0;
+    }
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        u16_t vapId;
+
+        vapId = zfwGetVapId(dev);
+
+        if (vapId == 0xffff)
+        {
+            /* AP mode */
+            zfApInitStaTbl(dev);
+
+            /* AP default parameters */
+            wd->bRate = 0xf;
+            wd->gRate = 0xff;
+            wd->bRateBasic = 0xf;
+            wd->gRateBasic = 0x0;
+            //wd->beaconInterval = 100;
+            wd->ap.apBitmap = 1;
+            wd->ap.beaconCounter = 0;
+            //wd->ap.vapNumber = 1;    //mark by ygwei for Vap
+
+            wd->ap.hideSsid[0] = 0;
+            wd->ap.staAgingTimeSec = 10*60;
+            wd->ap.staProbingTimeSec = 60;
+
+            for (i=0; i<ZM_MAX_AP_SUPPORT; i++)
+            {
+                wd->ap.bcmcHead[i] = wd->ap.bcmcTail[i] = 0;
+            }
+
+            //wd->ap.uniHead = wd->ap.uniTail = 0;
+
+            /* load AP parameters */
+            wd->bRateBasic = wd->ws.bRateBasic;
+            wd->gRateBasic = wd->ws.gRateBasic;
+            wd->bgMode = wd->ws.bgMode;
+            if ((wd->ws.ssidLen <= 32) && (wd->ws.ssidLen != 0))
+            {
+                wd->ap.ssidLen[0] = wd->ws.ssidLen;
+                for(i=0; i<wd->ws.ssidLen; i++)
+                {
+                    wd->ap.ssid[0][i] = wd->ws.ssid[i];
+                }
+                wd->ws.ssidLen = 0; // Reset Wrapper Variable
+            }
+
+            if (wd->ap.encryMode[0] == 0)
+            {
+                wd->ap.capab[0] = 0x001;
+            }
+            else
+            {
+                wd->ap.capab[0] = 0x011;
+            }
+            /* set Short Slot Time bit if not 11b */
+            if (wd->ap.wlanType[0] != ZM_WLAN_TYPE_PURE_B)
+            {
+              wd->ap.capab[0] |= 0x400;
+            }
+
+            // wd->ap.vapNumber = 1;    // mark by ygwei for Vap Test
+        }
+        else
+        {
+#if 0
+            /* VAP Test Code */
+            wd->ap.apBitmap = 0x3;
+            wd->ap.capab[1] = 0x401;
+            wd->ap.ssidLen[1] = 4;
+            wd->ap.ssid[1][0] = 'v';
+            wd->ap.ssid[1][1] = 'a';
+            wd->ap.ssid[1][2] = 'p';
+            wd->ap.ssid[1][3] = '1';
+            wd->ap.authAlgo[1] = wd->ws.authMode;
+            wd->ap.encryMode[1] = wd->ws.encryMode;
+            wd->ap.vapNumber = 2;
+#else
+            /* VAP Test Code */
+            wd->ap.apBitmap = 0x1 | (0x01 << (vapId+1));
+
+            if ((wd->ws.ssidLen <= 32) && (wd->ws.ssidLen != 0))
+            {
+                wd->ap.ssidLen[vapId+1] = wd->ws.ssidLen;
+                for(i=0; i<wd->ws.ssidLen; i++)
+                {
+                    wd->ap.ssid[vapId+1][i] = wd->ws.ssid[i];
+                }
+                wd->ws.ssidLen = 0; // Reset Wrapper Variable
+            }
+
+            if (wd->ap.encryMode[vapId+1] == 0)
+            {
+                wd->ap.capab[vapId+1] = 0x401;
+            }
+            else
+            {
+                wd->ap.capab[vapId+1] = 0x411;
+            }
+
+            wd->ap.authAlgo[vapId+1] = wd->ws.authMode;
+            wd->ap.encryMode[vapId+1] = wd->ws.encryMode;
+
+            /* Need to be modified when VAP is used */
+            //wd->ap.vapNumber = 2;
+#endif
+        }
+
+        wd->ap.vapNumber++;
+
+        zfCoreSetFrequency(dev, wd->frequency);
+
+        zfInitMacApMode(dev);
+
+        /* Disable protection mode */
+        zfApSetProtectionMode(dev, 0);
+
+        zfApSendBeacon(dev);
+    } /*if (wd->wlanMode == ZM_MODE_AP) */
+    else
+    {
+        zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+        zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_EXTERNAL);
+
+        zmw_enter_critical_section(dev);
+        wd->sta.oppositeCount = 0;    /* reset opposite count */
+        //wd->sta.bAutoReconnect = wd->sta.bAutoReconnectEnabled;
+        //wd->sta.scanWithSSID = 0;
+        zfStaInitOppositeInfo(dev);
+        zmw_leave_critical_section(dev);
+
+        zfStaResetStatus(dev, 0);
+
+        if ( (wd->sta.cmDisallowSsidLength != 0)&&
+             (wd->sta.ssidLen == wd->sta.cmDisallowSsidLength)&&
+             (zfMemoryIsEqual(wd->sta.ssid, wd->sta.cmDisallowSsid,
+                              wd->sta.ssidLen)) &&
+             (wd->sta.wepStatus == ZM_ENCRYPTION_TKIP))
+        {   /* countermeasures */
+            zm_debug_msg0("countermeasures disallow association");
+
+        }
+        else
+        {
+            switch( wd->wlanMode )
+            {
+                case ZM_MODE_IBSS:
+                    /* some registers may be set here */
+                    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK )
+                    {
+                        zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_IBSS_WPA2PSK);
+                    }
+                    else
+                    {
+                        zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_IBSS_GENERAL);
+                    }
+
+                    zm_msg0_mm(ZM_LV_0, "ZM_MODE_IBSS");
+                    zfIbssConnectNetwork(dev);
+                    break;
+
+                case ZM_MODE_INFRASTRUCTURE:
+                    /* some registers may be set here */
+                    zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_STA);
+
+                    zfInfraConnectNetwork(dev);
+                    break;
+
+                case ZM_MODE_PSEUDO:
+                    /* some registers may be set here */
+                    zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_STA);
+
+                    zfUpdateBssid(dev, bssid);
+                    zfCoreSetFrequency(dev, wd->frequency);
+                    break;
+
+                default:
+                    break;
+            }
+        }
+
+    }
+
+
+    //if ( (wd->wlanMode != ZM_MODE_INFRASTRUCTURE)&&
+    //     (wd->wlanMode != ZM_MODE_AP) )
+    if ( wd->wlanMode == ZM_MODE_PSEUDO )
+    {
+        /* Reset Wlan status */
+        zfWlanReset(dev);
+
+        if (wd->zfcbConnectNotify != NULL)
+        {
+            wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECT, wd->sta.bssid);
+        }
+        zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTED);
+    }
+
+
+    if(wd->wlanMode == ZM_MODE_AP)
+    {
+        if (wd->zfcbConnectNotify != NULL)
+        {
+            wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECT, wd->sta.bssid);
+        }
+        //zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTED);
+    }
+
+    // Assign default Tx Rate
+    if ( wd->sta.EnableHT )
+    {
+		u32_t oneTxStreamCap;
+		oneTxStreamCap = (zfHpCapability(dev) & ZM_HP_CAP_11N_ONE_TX_STREAM);
+		if(oneTxStreamCap)
+			wd->CurrentTxRateKbps = 135000;
+		else
+			wd->CurrentTxRateKbps = 270000;
+        wd->CurrentRxRateKbps = 270000;
+    }
+    else
+    {
+        wd->CurrentTxRateKbps = 54000;
+        wd->CurrentRxRateKbps = 54000;
+    }
+
+    wd->state = ZM_WLAN_STATE_ENABLED;
+
+    return 0;
+}
+
+/* Enable/disable Wlan operation */
+u16_t zfiWlanEnable(zdev_t* dev)
+{
+    u16_t ret;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_msg0_mm(ZM_LV_1, "Enable Wlan");
+
+    zfGetWrapperSetting(dev);
+
+    zfZeroMemory((u8_t*) &wd->trafTally, sizeof(struct zsTrafTally));
+
+    // Reset cmMicFailureCount to 0 for new association request
+    if ( wd->sta.cmMicFailureCount == 1 )
+    {
+        zfTimerCancel(dev, ZM_EVENT_CM_TIMER);
+        wd->sta.cmMicFailureCount = 0;
+    }
+
+    zfFlushVtxq(dev);
+    if ((wd->queueFlushed & 0x10) != 0)
+    {
+        zfHpUsbReset(dev);
+    }
+    ret = zfWlanEnable(dev);
+
+    return ret;
+}
+/* Add a flag named ResetKeyCache to show if KeyCache should be cleared.
+   for hostapd in AP mode, if driver receives iwconfig ioctl
+   after setting group key, it shouldn't clear KeyCache.                 */
+u16_t zfiWlanDisable(zdev_t* dev, u8_t ResetKeyCache)
+{
+    u16_t  i;
+    u8_t isConnected;
+
+    zmw_get_wlan_dev(dev);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    zmw_declare_for_critical_section();
+#endif
+    wd->state = ZM_WLAN_STATE_DISABLED;
+
+    zm_msg0_mm(ZM_LV_1, "Disable Wlan");
+
+    if ( wd->wlanMode != ZM_MODE_AP )
+    {
+        isConnected = zfStaIsConnected(dev);
+
+        if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)&&
+             (wd->sta.currentAuthMode != ZM_AUTH_MODE_WPA2) )
+        {
+            /* send deauthentication frame */
+            if (isConnected)
+            {
+                //zfiWlanDeauth(dev, NULL, 0);
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, wd->sta.bssid, 3, 0, 0);
+                //zmw_debug_msg0("send a Deauth frame!");
+            }
+        }
+
+        // Remove all the connected peer stations
+        if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            wd->sta.ibssBssIsCreator = 0;
+            zfTimerCancel(dev, ZM_EVENT_IBSS_MONITOR);
+            zfStaIbssMonitoring(dev, 1);
+        }
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+        zmw_enter_critical_section(dev);
+        wd->sta.ibssWpa2Psk = 0;
+        zmw_leave_critical_section(dev);
+#endif
+
+        wd->sta.wpaState = ZM_STA_WPA_STATE_INIT;
+
+        /* reset connect timeout counter */
+        wd->sta.connectTimeoutCount = 0;
+
+        /* reset connectState to None */
+        wd->sta.connectState = ZM_STA_CONN_STATE_NONE;
+
+        /* reset leap enable variable */
+        wd->sta.leapEnabled = 0;
+
+        /* Disable the RIFS Status / RIFS-like frame count / RIFS count */
+        if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTED )
+            zfHpDisableRifs(dev);
+        wd->sta.rifsState = ZM_RIFS_STATE_DETECTING;
+        wd->sta.rifsLikeFrameCnt = 0;
+        wd->sta.rifsCount = 0;
+
+        wd->sta.osRxFilter = 0;
+        wd->sta.bSafeMode = 0;
+
+        zfChangeAdapterState(dev, ZM_STA_STATE_DISCONNECT);
+        if (ResetKeyCache)
+            zfHpResetKeyCache(dev);
+
+        if (isConnected)
+        {
+            if (wd->zfcbConnectNotify != NULL)
+            {
+                wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECTION_DISABLED, wd->sta.bssid);
+            }
+        }
+        else
+        {
+            if (wd->zfcbConnectNotify != NULL)
+            {
+                wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_DISABLED, wd->sta.bssid);
+            }
+        }
+    }
+    else //if (wd->wlanMode == ZM_MODE_AP)
+    {
+        for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+        {
+            /* send deauthentication frame */
+            if (wd->ap.staTable[i].valid == 1)
+            {
+                /* Reason : Sending station is leaving */
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH,
+                        wd->ap.staTable[i].addr, 3, 0, 0);
+            }
+        }
+
+        if (ResetKeyCache)
+            zfHpResetKeyCache(dev);
+
+        wd->ap.vapNumber--;
+    }
+
+    /* stop beacon */
+    zfHpDisableBeacon(dev);
+
+    /* Flush VTxQ and MmQ */
+    zfFlushVtxq(dev);
+    /* Flush AP PS queues */
+    zfApFlushBufferedPsFrame(dev);
+    /* Free buffer in defragment list*/
+    zfAgingDefragList(dev, 1);
+
+    #ifdef ZM_ENABLE_AGGREGATION
+    /* add by honda */
+    zfAggRxFreeBuf(dev, 0);  //1 for release structure memory
+    /* end of add by honda */
+    #endif
+
+    // Clear the information for the peer stations of IBSS or AP of Station mode
+    zfZeroMemory((u8_t*)wd->sta.oppositeInfo, sizeof(struct zsOppositeInfo) * ZM_MAX_OPPOSITE_COUNT);
+
+    /* Turn off Software WEP/TKIP */
+    if (wd->sta.SWEncryptEnable != 0)
+    {
+        zm_debug_msg0("Disable software encryption");
+        zfStaDisableSWEncryption(dev);
+    }
+
+    /* Improve WEP/TKIP performace with HT AP, detail information please look bug#32495 */
+    //zfHpSetTTSIFSTime(dev, 0x8);
+
+    return 0;
+}
+
+u16_t zfiWlanSuspend(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    // Change the HAL state to init so that any packet can't be transmitted between
+    // resume & HAL reinit. This would cause the chip hang issue in OTUS.
+    zmw_enter_critical_section(dev);
+    wd->halState = ZM_HAL_STATE_INIT;
+    zmw_leave_critical_section(dev);
+
+    return 0;
+}
+
+u16_t zfiWlanResume(zdev_t* dev, u8_t doReconn)
+{
+    u16_t ret;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* Redownload firmware, Reinit MAC,PHY,RF */
+    zfHpReinit(dev, wd->frequency);
+
+    //Set channel according to AP's configuration
+    zfCoreSetFrequencyExV2(dev, wd->frequency, wd->BandWidth40,
+            wd->ExtOffset, NULL, 1);
+
+    zfHpSetMacAddress(dev, wd->macAddr, 0);
+
+    /* Start Rx */
+    zfHpStartRecv(dev);
+
+    zfFlushVtxq(dev);
+
+    if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE &&
+         wd->wlanMode != ZM_MODE_IBSS )
+    {
+        return 1;
+    }
+
+    zm_msg0_mm(ZM_LV_1, "Resume Wlan");
+    if ( (zfStaIsConnected(dev)) || (zfStaIsConnecting(dev)) )
+    {
+        if (doReconn == 1)
+        {
+            zm_msg0_mm(ZM_LV_1, "Re-connect...");
+            zmw_enter_critical_section(dev);
+            wd->sta.connectByReasso = FALSE;
+            zmw_leave_critical_section(dev);
+
+            zfWlanEnable(dev);
+        }
+        else if (doReconn == 0)
+        {
+            zfHpSetRollCallTable(dev);
+        }
+    }
+
+    ret = 0;
+
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                 zfiWlanFlushAllQueuedBuffers */
+/*      Flush Virtual TxQ, MmQ, PS frames and defragment list           */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfiWlanFlushAllQueuedBuffers(zdev_t* dev)
+{
+    /* Flush VTxQ and MmQ */
+    zfFlushVtxq(dev);
+    /* Flush AP PS queues */
+    zfApFlushBufferedPsFrame(dev);
+    /* Free buffer in defragment list*/
+    zfAgingDefragList(dev, 1);
+}
+
+/* Do WLAN site survey */
+u16_t zfiWlanScan(zdev_t* dev)
+{
+    u16_t ret = 1;
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg0("");
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        wd->heartBeatNotification |= ZM_BSSID_LIST_SCAN;
+        wd->sta.scanFrequency = 0;
+        //wd->sta.pUpdateBssList->bssCount = 0;
+        ret = 0;
+    }
+    else
+    {
+        #if 0
+        if ( !zfStaBlockWlanScan(dev) )
+        {
+            zm_debug_msg0("scan request");
+            //zfTimerSchedule(dev, ZM_EVENT_SCAN, ZM_TICK_ZERO);
+            ret = 0;
+            goto start_scan;
+        }
+        #else
+            goto start_scan;
+        #endif
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return ret;
+
+start_scan:
+    zmw_leave_critical_section(dev);
+
+    if(wd->ledStruct.LEDCtrlFlagFromReg & ZM_LED_CTRL_FLAG_ALPHA)   // flag for Alpha
+        wd->ledStruct.LEDCtrlFlag |= ZM_LED_CTRL_FLAG_ALPHA;
+
+    ret = zfScanMgrScanStart(dev, ZM_SCAN_MGR_SCAN_EXTERNAL);
+
+    zm_debug_msg1("ret = ", ret);
+
+    return ret;
+}
+
+
+/* rate         */
+/*    0 : AUTO  */
+/*    1 : CCK 1M */
+/*    2 : CCK 2M */
+/*    3 : CCK 5.5M */
+/*    4 : CCK 11M */
+/*    5 : OFDM 6M */
+/*    6 : OFDM 9M */
+/*    7 : OFDM 12M */
+/*    8 : OFDM 18M */
+/*    9 : OFDM 24M */
+/*    10 : OFDM 36M */
+/*    11 : OFDM 48M */
+/*    12 : OFDM 54M */
+/*    13 : MCS 0 */
+/*    28 : MCS 15 */
+u16_t zcRateToMCS[] =
+    {0xff, 0, 1, 2, 3, 0xb, 0xf, 0xa, 0xe, 0x9, 0xd, 0x8, 0xc};
+u16_t zcRateToMT[] = {0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1};
+
+u16_t zfiWlanSetTxRate(zdev_t* dev, u16_t rate)
+{   // jhlee HT 0
+    zmw_get_wlan_dev(dev);
+
+    if (rate <=12)
+    {
+        wd->txMCS = zcRateToMCS[rate];
+        wd->txMT = zcRateToMT[rate];
+        return ZM_SUCCESS;
+    }
+    else if ((rate<=28)||(rate==13+32))
+    {
+        wd->txMCS = rate - 12 - 1;
+        wd->txMT = 2;
+        return ZM_SUCCESS;
+    }
+
+    return ZM_ERR_INVALID_TX_RATE;
+}
+
+const u32_t zcRateIdToKbps40M[] =
+    {
+        1000, 2000, 5500, 11000,                  /* 1M, 2M, 5M, 11M          ,  0  1  2  3*/
+        6000, 9000, 12000, 18000,                 /* 6M  9M  12M  18M         ,  4  5  6  7*/
+        24000, 36000, 48000, 54000,               /* 24M  36M  48M  54M       ,  8  9 10 11*/
+        13500, 27000, 40500, 54000,               /* MCS0 MCS1 MCS2 MCS3      , 12 13 14 15*/
+        81000, 108000, 121500, 135000,            /* MCS4 MCS5 MCS6 MCS7      , 16 17 18 19*/
+        27000, 54000, 81000, 108000,              /* MCS8 MCS9 MCS10 MCS11    , 20 21 22 23*/
+        162000, 216000, 243000, 270000,           /* MCS12 MCS13 MCS14 MCS15  , 24 25 26 27*/
+        270000, 300000, 150000                    /* MCS14SG, MCS15SG, MCS7SG , 28 29 30*/
+    };
+
+const u32_t zcRateIdToKbps20M[] =
+    {
+        1000, 2000, 5500, 11000,                  /* 1M, 2M, 5M, 11M          ,  0  1  2  3*/
+        6000, 9000, 12000, 18000,                 /* 6M  9M  12M  18M         ,  4  5  6  7*/
+        24000, 36000, 48000, 54000,               /* 24M  36M  48M  54M       ,  8  9 10 11*/
+        6500, 13000, 19500, 26000,                /* MCS0 MCS1 MCS2 MCS3      , 12 13 14 15*/
+        39000, 52000, 58500, 65000,               /* MCS4 MCS5 MCS6 MCS7      , 16 17 18 19*/
+        13000, 26000, 39000, 52000,               /* MCS8 MCS9 MCS10 MCS11    , 20 21 22 23*/
+        78000, 104000, 117000, 130000,            /* MCS12 MCS13 MCS14 MCS15  , 24 25 26 27*/
+        130000, 144400, 72200                     /* MCS14SG, MCS15SG, MSG7SG , 28 29 30*/
+    };
+
+u32_t zfiWlanQueryTxRate(zdev_t* dev)
+{
+    u8_t rateId = 0xff;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* If Tx rate had not been trained, return maximum Tx rate instead */
+    if ((wd->wlanMode == ZM_MODE_INFRASTRUCTURE) && (zfStaIsConnected(dev)))
+    {
+        zmw_enter_critical_section(dev);
+        //Not in fixed rate mode
+        if (wd->txMCS == 0xff)
+        {
+            if ((wd->sta.oppositeInfo[0].rcCell.flag & ZM_RC_TRAINED_BIT) == 0)
+            {
+                rateId = wd->sta.oppositeInfo[0].rcCell.operationRateSet[wd->sta.oppositeInfo[0].rcCell.operationRateCount-1];
+            }
+            else
+            {
+                rateId = wd->sta.oppositeInfo[0].rcCell.operationRateSet[wd->sta.oppositeInfo[0].rcCell.currentRateIndex];
+            }
+        }
+        zmw_leave_critical_section(dev);
+    }
+    if (rateId != 0xff)
+    {
+        if (wd->sta.htCtrlBandwidth)
+        {
+            return zcRateIdToKbps40M[rateId];
+        }
+        else
+        {
+            return zcRateIdToKbps20M[rateId];
+        }
+    }
+    else
+    {
+        return wd->CurrentTxRateKbps;
+    }
+}
+
+void zfWlanUpdateRxRate(zdev_t* dev, struct zsAdditionInfo* addInfo)
+{
+    u32_t rxRateKbps;
+    zmw_get_wlan_dev(dev);
+    //zm_msg1_mm(ZM_LV_0, "addInfo->Tail.Data.RxMacStatus =", addInfo->Tail.Data.RxMacStatus & 0x03);
+
+    /* b5~b4: MPDU indication.                */
+    /*        00: Single MPDU.                */
+    /*        10: First MPDU of A-MPDU.       */
+    /*        11: Middle MPDU of A-MPDU.      */
+    /*        01: Last MPDU of A-MPDU.        */
+    /* Only First MPDU and Single MPDU have PLCP header */
+    /* First MPDU  : (mpduInd & 0x30) == 0x00 */
+    /* Single MPDU : (mpduInd & 0x30) == 0x20 */
+    if ((addInfo->Tail.Data.RxMacStatus & 0x10) == 0)
+    {
+        /* Modulation type */
+        wd->modulationType = addInfo->Tail.Data.RxMacStatus & 0x03;
+        switch(wd->modulationType)
+        {
+    		case 0x0: wd->rateField = addInfo->PlcpHeader[0] & 0xff; //CCK mode
+                      wd->rxInfo = 0;
+                      break;
+    		case 0x1: wd->rateField = addInfo->PlcpHeader[0] & 0x0f; //Legacy-OFDM mode
+                      wd->rxInfo = 0;
+                      break;
+    		case 0x2: wd->rateField = addInfo->PlcpHeader[3]; //HT-OFDM mode
+    		          wd->rxInfo = addInfo->PlcpHeader[6];
+                      break;
+            default: break;
+         }
+
+        rxRateKbps = zfUpdateRxRate(dev);
+        if (wd->CurrentRxRateUpdated == 1)
+        {
+            if (rxRateKbps > wd->CurrentRxRateKbps)
+            {
+                wd->CurrentRxRateKbps = rxRateKbps;
+            }
+        }
+        else
+        {
+            wd->CurrentRxRateKbps = rxRateKbps;
+            wd->CurrentRxRateUpdated = 1;
+        }
+    }
+}
+#if 0
+u16_t zcIndextoRateBG[16] = {1000, 2000, 5500, 11000, 0, 0, 0, 0, 48000,
+                               24000, 12000, 6000, 54000, 36000, 18000, 9000};
+u32_t zcIndextoRateN20L[16] = {6500, 13000, 19500, 26000, 39000, 52000, 58500,
+                              65000, 13000, 26000, 39000, 52000, 78000, 104000,
+                              117000, 130000};
+u32_t zcIndextoRateN20S[16] = {7200, 14400, 21700, 28900, 43300, 57800, 65000,
+                              72200, 14400, 28900, 43300, 57800, 86700, 115600,
+                              130000, 144400};
+u32_t zcIndextoRateN40L[16] = {13500, 27000, 40500, 54000, 81000, 108000, 121500,
+                              135000, 27000, 54000, 81000, 108000, 162000, 216000,
+                              243000, 270000};
+u32_t zcIndextoRateN40S[16] = {15000, 30000, 45000, 60000, 90000, 120000, 135000,
+                              150000, 30000, 60000, 90000, 120000, 180000, 240000,
+                              270000, 300000};
+#endif
+
+extern u16_t zcIndextoRateBG[16];
+extern u32_t zcIndextoRateN20L[16];
+extern u32_t zcIndextoRateN20S[16];
+extern u32_t zcIndextoRateN40L[16];
+extern u32_t zcIndextoRateN40S[16];
+
+u32_t zfiWlanQueryRxRate(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->CurrentRxRateUpdated = 0;
+    return wd->CurrentRxRateKbps;
+}
+
+u32_t zfUpdateRxRate(zdev_t* dev)
+{
+    u8_t mcs, bandwidth;
+    u32_t rxRateKbps = 130000;
+    zmw_get_wlan_dev(dev);
+
+    switch (wd->modulationType)
+    {
+		case 0x0: //CCK mode
+                  switch (wd->rateField)
+                  {
+                      case 0x0a: rxRateKbps = 1000;
+                            break;
+                      case 0x14: rxRateKbps = 2000;
+
+                      case 0x37: rxRateKbps = 5500;
+                            break;
+                      case 0x6e: rxRateKbps = 11000;
+                            break;
+                      default:
+                            break;
+                  }
+                  break;
+		case 0x1: //Legacy-OFDM mode
+                  if (wd->rateField <= 15)
+                  {
+                      rxRateKbps = zcIndextoRateBG[wd->rateField];
+                  }
+                  break;
+		case 0x2: //HT-OFDM mode
+		          mcs = wd->rateField & 0x7F;
+		          bandwidth = wd->rateField & 0x80;
+                  if (mcs <= 15)
+                  {
+                      if (bandwidth != 0)
+                      {
+                          if((wd->rxInfo & 0x80) != 0)
+                          {
+                              /* Short GI 40 MHz MIMO Rate */
+                              rxRateKbps = zcIndextoRateN40S[mcs];
+                          }
+                          else
+                          {
+                              /* Long GI 40 MHz MIMO Rate */
+                              rxRateKbps = zcIndextoRateN40L[mcs];
+                          }
+                      }
+                      else
+                      {
+                          if((wd->rxInfo & 0x80) != 0)
+                          {
+                              /* Short GI 20 MHz MIMO Rate */
+                              rxRateKbps = zcIndextoRateN20S[mcs];
+                          }
+                          else
+                          {
+                              /* Long GI 20 MHz MIMO Rate */
+                              rxRateKbps = zcIndextoRateN20L[mcs];
+                          }
+                      }
+                  }
+                  break;
+        default:
+                  break;
+    }
+    //zm_msg1_mm(ZM_LV_0, "wd->CurrentRxRateKbps=", wd->CurrentRxRateKbps);
+
+     // ToDo: use bandwith field to define 40MB
+    return rxRateKbps;
+}
+
+/* Get WLAN stastics */
+u16_t zfiWlanGetStatistics(zdev_t* dev)
+{
+    /* Return link statistics */
+    return 0;
+}
+
+u16_t zfiWlanReset(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->state = ZM_WLAN_STATE_DISABLED;
+
+    return zfWlanReset(dev);
+}
+
+/* Reset WLAN */
+u16_t zfWlanReset(zdev_t* dev)
+{
+    u8_t isConnected;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zm_debug_msg0("zfWlanReset");
+
+    isConnected = zfStaIsConnected(dev);
+
+    //if ( wd->wlanMode != ZM_MODE_AP )
+    {
+        if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)&&
+             (wd->sta.currentAuthMode != ZM_AUTH_MODE_WPA2) )
+        {
+            /* send deauthentication frame */
+            if (isConnected)
+            {
+                //zfiWlanDeauth(dev, NULL, 0);
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, wd->sta.bssid, 3, 0, 0);
+                //zmw_debug_msg0("send a Deauth frame!");
+            }
+        }
+    }
+
+    zfChangeAdapterState(dev, ZM_STA_STATE_DISCONNECT);
+    zfHpResetKeyCache(dev);
+
+    if (isConnected)
+    {
+        //zfiWlanDisable(dev);
+        if (wd->zfcbConnectNotify != NULL)
+        {
+            wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECTION_RESET, wd->sta.bssid);
+        }
+    }
+    else
+    {
+        if (wd->zfcbConnectNotify != NULL)
+        {
+            wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_RESET, wd->sta.bssid);
+        }
+    }
+
+    /* stop beacon */
+    zfHpDisableBeacon(dev);
+
+    /* Free buffer in defragment list*/
+    zfAgingDefragList(dev, 1);
+
+    /* Flush VTxQ and MmQ */
+    zfFlushVtxq(dev);
+
+    #ifdef ZM_ENABLE_AGGREGATION
+    /* add by honda */
+    zfAggRxFreeBuf(dev, 0);  //1 for release structure memory
+    /* end of add by honda */
+    #endif
+
+    zfStaRefreshBlockList(dev, 1);
+
+    zmw_enter_critical_section(dev);
+
+    zfTimerCancel(dev, ZM_EVENT_IBSS_MONITOR);
+    zfTimerCancel(dev, ZM_EVENT_CM_BLOCK_TIMER);
+    zfTimerCancel(dev, ZM_EVENT_CM_DISCONNECT);
+
+    wd->sta.connectState = ZM_STA_CONN_STATE_NONE;
+    wd->sta.connectByReasso = FALSE;
+    wd->sta.cmDisallowSsidLength = 0;
+    wd->sta.bAutoReconnect = 0;
+    wd->sta.InternalScanReq = 0;
+    wd->sta.encryMode = ZM_NO_WEP;
+    wd->sta.wepStatus = ZM_ENCRYPTION_WEP_DISABLED;
+    wd->sta.wpaState = ZM_STA_WPA_STATE_INIT;
+    wd->sta.cmMicFailureCount = 0;
+    wd->sta.ibssBssIsCreator = 0;
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    wd->sta.ibssWpa2Psk = 0;
+#endif
+    /* reset connect timeout counter */
+    wd->sta.connectTimeoutCount = 0;
+
+    /* reset leap enable variable */
+    wd->sta.leapEnabled = 0;
+
+    /* Reset the RIFS Status / RIFS-like frame count / RIFS count */
+    if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTED )
+        zfHpDisableRifs(dev);
+    wd->sta.rifsState = ZM_RIFS_STATE_DETECTING;
+    wd->sta.rifsLikeFrameCnt = 0;
+    wd->sta.rifsCount = 0;
+
+    wd->sta.osRxFilter = 0;
+    wd->sta.bSafeMode = 0;
+
+    // Clear the information for the peer stations of IBSS or AP of Station mode
+    zfZeroMemory((u8_t*)wd->sta.oppositeInfo, sizeof(struct zsOppositeInfo) * ZM_MAX_OPPOSITE_COUNT);
+
+    zmw_leave_critical_section(dev);
+
+    zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+    zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_EXTERNAL);
+
+    /* Turn off Software WEP/TKIP */
+    if (wd->sta.SWEncryptEnable != 0)
+    {
+        zm_debug_msg0("Disable software encryption");
+        zfStaDisableSWEncryption(dev);
+    }
+
+    /* Improve WEP/TKIP performace with HT AP, detail information please look bug#32495 */
+    //zfHpSetTTSIFSTime(dev, 0x8);
+
+    /* Keep Pseudo mode */
+    if ( wd->wlanMode != ZM_MODE_PSEUDO )
+    {
+        wd->wlanMode = ZM_MODE_INFRASTRUCTURE;
+    }
+    return 0;
+}
+
+/* Deauthenticate a STA */
+u16_t zfiWlanDeauth(zdev_t* dev, u16_t* macAddr, u16_t reason)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        //u16_t id;
+
+        /*
+         * we will reset all key in zfHpResetKeyCache() when call
+         * zfiWlanDisable(), if we want to reset PairwiseKey for each sta,
+         * need to use a nullAddr to let keyindex not match.
+         * otherwise hardware will still find PairwiseKey when AP change
+         * encryption mode from WPA to WEP
+         */
+
+        /*
+        if ((id = zfApFindSta(dev, macAddr)) != 0xffff)
+        {
+            u32_t key[8];
+            u16_t nullAddr[3] = { 0x0, 0x0, 0x0 };
+
+            if (wd->ap.staTable[i].encryMode != ZM_NO_WEP)
+            {
+                zfHpSetApPairwiseKey(dev, nullAddr,
+                        ZM_NO_WEP, &key[0], &key[4], i+1);
+            }
+            //zfHpSetApPairwiseKey(dev, (u16_t *)macAddr,
+            //        ZM_NO_WEP, &key[0], &key[4], id+1);
+            wd->ap.staTable[id].encryMode = ZM_NO_WEP;
+            wd->ap.staTable[id].keyIdx = 0xff;
+        }
+        */
+
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, macAddr, reason, 0, 0);
+    }
+    else
+    {
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, wd->sta.bssid, 3, 0, 0);
+    }
+
+    /* Issue DEAUTH command to FW */
+    return 0;
+}
+
+
+/* XP packet filter feature : */
+/* 1=>enable: All multicast address packets, not just the ones enumerated in the multicast address list. */
+/* 0=>disable */
+void zfiWlanSetAllMulticast(zdev_t* dev, u32_t setting)
+{
+    zmw_get_wlan_dev(dev);
+    zm_msg1_mm(ZM_LV_0, "sta.bAllMulticast = ", setting);
+    wd->sta.bAllMulticast = (u8_t)setting;
+}
+
+
+/* HT configure API */
+void zfiWlanSetHTCtrl(zdev_t* dev, u32_t *setting, u32_t forceTxTPC)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->preambleType        = (u8_t)setting[0];
+    wd->sta.preambleTypeHT  = (u8_t)setting[1];
+    wd->sta.htCtrlBandwidth = (u8_t)setting[2];
+    wd->sta.htCtrlSTBC      = (u8_t)setting[3];
+    wd->sta.htCtrlSG        = (u8_t)setting[4];
+    wd->sta.defaultTA       = (u8_t)setting[5];
+    wd->enableAggregation   = (u8_t)setting[6];
+    wd->enableWDS           = (u8_t)setting[7];
+
+    wd->forceTxTPC          = forceTxTPC;
+}
+
+/* FB50 in OS XP, RD private test code */
+void zfiWlanQueryHTCtrl(zdev_t* dev, u32_t *setting, u32_t *forceTxTPC)
+{
+    zmw_get_wlan_dev(dev);
+
+    setting[0] = wd->preambleType;
+    setting[1] = wd->sta.preambleTypeHT;
+    setting[2] = wd->sta.htCtrlBandwidth;
+    setting[3] = wd->sta.htCtrlSTBC;
+    setting[4] = wd->sta.htCtrlSG;
+    setting[5] = wd->sta.defaultTA;
+    setting[6] = wd->enableAggregation;
+    setting[7] = wd->enableWDS;
+
+    *forceTxTPC = wd->forceTxTPC;
+}
+
+void zfiWlanDbg(zdev_t* dev, u8_t setting)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->enableHALDbgInfo = setting;
+}
+
+/* FB50 in OS XP, RD private test code */
+void zfiWlanSetRxPacketDump(zdev_t* dev, u32_t setting)
+{
+    zmw_get_wlan_dev(dev);
+    if (setting)
+    {
+        wd->rxPacketDump = 1;   /* enable */
+    }
+    else
+    {
+        wd->rxPacketDump = 0;   /* disable */
+    }
+}
+
+
+/* FB50 in OS XP, RD private test code */
+/* Tally */
+void zfiWlanResetTally(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+	   wd->commTally.txUnicastFrm = 0;		//txUnicastFrames
+	   wd->commTally.txMulticastFrm = 0;		//txMulticastFrames
+	   wd->commTally.txUnicastOctets = 0;	//txUniOctets  byte size
+	   wd->commTally.txMulticastOctets = 0;	//txMultiOctets  byte size
+	   wd->commTally.txFrmUpperNDIS = 0;
+	   wd->commTally.txFrmDrvMgt = 0;
+	   wd->commTally.RetryFailCnt = 0;
+	   wd->commTally.Hw_TotalTxFrm = 0;		//Hardware total Tx Frame
+	   wd->commTally.Hw_RetryCnt = 0;		//txMultipleRetriesFrames
+	   wd->commTally.Hw_UnderrunCnt = 0;//
+	   wd->commTally.DriverRxFrmCnt = 0;//
+	   wd->commTally.rxUnicastFrm = 0;		//rxUnicastFrames
+	   wd->commTally.rxMulticastFrm = 0;	//rxMulticastFrames
+	   wd->commTally.NotifyNDISRxFrmCnt = 0;//
+	   wd->commTally.rxUnicastOctets = 0;	//rxUniOctets  byte size
+	   wd->commTally.rxMulticastOctets = 0;	//rxMultiOctets  byte size
+	   wd->commTally.DriverDiscardedFrm = 0;// Discard by ValidateFrame
+	   wd->commTally.LessThanDataMinLen = 0;//
+	   wd->commTally.GreaterThanMaxLen = 0;//
+	   wd->commTally.DriverDiscardedFrmCauseByMulticastList = 0;
+	   wd->commTally.DriverDiscardedFrmCauseByFrmCtrl = 0;
+	   wd->commTally.rxNeedFrgFrm = 0;		// need more frg frm
+	   wd->commTally.DriverRxMgtFrmCnt = 0;
+	   wd->commTally.rxBroadcastFrm = 0;	//Receive broadcast frame count
+	   wd->commTally.rxBroadcastOctets = 0;	//Receive broadcast frame byte size
+	   wd->commTally.Hw_TotalRxFrm = 0;//
+	   wd->commTally.Hw_CRC16Cnt = 0;		//rxPLCPCRCErrCnt
+	   wd->commTally.Hw_CRC32Cnt = 0;		//rxCRC32ErrCnt
+	   wd->commTally.Hw_DecrypErr_UNI = 0;//
+	   wd->commTally.Hw_DecrypErr_Mul = 0;//
+	   wd->commTally.Hw_RxFIFOOverrun = 0;//
+	   wd->commTally.Hw_RxTimeOut = 0;
+	   wd->commTally.LossAP = 0;//
+
+	   wd->commTally.Tx_MPDU = 0;
+	   wd->commTally.BA_Fail = 0;
+	   wd->commTally.Hw_Tx_AMPDU = 0;
+	   wd->commTally.Hw_Tx_MPDU = 0;
+
+    wd->commTally.txQosDropCount[0] = 0;
+    wd->commTally.txQosDropCount[1] = 0;
+    wd->commTally.txQosDropCount[2] = 0;
+    wd->commTally.txQosDropCount[3] = 0;
+    wd->commTally.txQosDropCount[4] = 0;
+
+    wd->commTally.Hw_RxMPDU = 0;
+    wd->commTally.Hw_RxDropMPDU = 0;
+    wd->commTally.Hw_RxDelMPDU = 0;
+
+    wd->commTally.Hw_RxPhyMiscError = 0;
+    wd->commTally.Hw_RxPhyXRError = 0;
+    wd->commTally.Hw_RxPhyOFDMError = 0;
+    wd->commTally.Hw_RxPhyCCKError = 0;
+    wd->commTally.Hw_RxPhyHTError = 0;
+    wd->commTally.Hw_RxPhyTotalCount = 0;
+
+#if	(defined(GCCK) && defined(OFDM))
+	   wd->commTally.rx11bDataFrame = 0;
+	   wd->commTally.rxOFDMDataFrame = 0;
+#endif
+
+    zmw_leave_critical_section(dev);
+}
+
+/* FB50 in OS XP, RD private test code */
+void zfiWlanQueryTally(zdev_t* dev, struct zsCommTally *tally)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    zfMemoryCopy((u8_t*)tally, (u8_t*)&wd->commTally, sizeof(struct zsCommTally));
+    zmw_leave_critical_section(dev);
+}
+void zfiWlanQueryTrafTally(zdev_t* dev, struct zsTrafTally *tally)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    zfMemoryCopy((u8_t*)tally, (u8_t*)&wd->trafTally, sizeof(struct zsTrafTally));
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanQueryMonHalRxInfo(zdev_t* dev, struct zsMonHalRxInfo *monHalRxInfo)
+{
+    zfHpQueryMonHalRxInfo(dev, (u8_t *)monHalRxInfo);
+}
+
+/* parse the modeMDKEnable to DrvCore */
+void zfiDKEnable(zdev_t* dev, u32_t enable)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->modeMDKEnable = enable;
+    zm_debug_msg1("modeMDKEnable = ", wd->modeMDKEnable);
+}
+
+/* airoPeek */
+u32_t zfiWlanQueryPacketTypePromiscuous(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->swSniffer;
+}
+
+/* airoPeek */
+void zfiWlanSetPacketTypePromiscuous(zdev_t* dev, u32_t setValue)
+{
+    zmw_get_wlan_dev(dev);
+
+	   wd->swSniffer = setValue;
+	   zm_msg1_mm(ZM_LV_0, "wd->swSniffer ", wd->swSniffer);
+	   if (setValue)
+	   {
+	       	/* write register for sniffer mode */
+            zfHpSetSnifferMode(dev, 1);
+            zm_msg0_mm(ZM_LV_1, "enalbe sniffer mode");
+	   }
+	   else
+	   {
+            zfHpSetSnifferMode(dev, 0);
+            zm_msg0_mm(ZM_LV_0, "disalbe sniffer mode");
+	   }
+}
+
+void zfiWlanSetXLinkMode(zdev_t* dev, u32_t setValue)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->XLinkMode = setValue;
+    if (setValue)
+    {
+        /* write register for sniffer mode */
+        zfHpSetSnifferMode(dev, 1);
+    }
+    else
+    {
+        zfHpSetSnifferMode(dev, 0);
+    }
+}
+
+extern void zfStaChannelManagement(zdev_t* dev, u8_t scan);
+void zfiSetChannelManagement(zdev_t* dev, u32_t setting)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch (setting)
+    {
+        case 1:
+            wd->sta.EnableHT = 1;
+            wd->BandWidth40 = 1;
+            wd->ExtOffset   = 1;
+            break;
+        case 3:
+            wd->sta.EnableHT = 1;
+            wd->BandWidth40 = 1;
+            wd->ExtOffset   = 3;
+            break;
+        case 0:
+            wd->sta.EnableHT = 1;
+            wd->BandWidth40 = 0;
+            wd->ExtOffset   = 0;
+            break;
+        default:
+            wd->BandWidth40 = 0;
+            wd->ExtOffset   = 0;
+            break;
+
+    }
+    zfCoreSetFrequencyEx(dev, wd->frequency, wd->BandWidth40,
+                    wd->ExtOffset, NULL);
+}
+
+void zfiSetRifs(zdev_t* dev, u16_t setting)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.ie.HtInfo.ChannelInfo |= ExtHtCap_RIFSMode;
+    wd->sta.EnableHT = 1;
+    switch (setting)
+    {
+        case 0:
+            wd->sta.HT2040 = 0;
+//            zfHpSetRifs(dev, 1, 0, (wd->sta.currentFrequency < 3000)? 1:0);
+            break;
+        case 1:
+            wd->sta.HT2040 = 1;
+//            zfHpSetRifs(dev, 1, 1, (wd->sta.currentFrequency < 3000)? 1:0);
+            break;
+        default:
+            wd->sta.HT2040 = 0;
+//            zfHpSetRifs(dev, 1, 0, (wd->sta.currentFrequency < 3000)? 1:0);
+            break;
+    }
+}
+
+void zfiCheckRifs(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if(wd->sta.ie.HtInfo.ChannelInfo & ExtHtCap_RIFSMode)
+    {
+//        zfHpSetRifs(dev, wd->sta.EnableHT, wd->sta.HT2040, (wd->sta.currentFrequency < 3000)? 1:0);
+    }
+}
+
+void zfiSetReorder(zdev_t* dev, u16_t value)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->reorder = value;
+}
+
+void zfiSetSeqDebug(zdev_t* dev, u16_t value)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->seq_debug = value;
+}
diff --git a/drivers/staging/otus/80211core/cfunc.c b/drivers/staging/otus/80211core/cfunc.c
new file mode 100644
index 000000000000..d7c49d7523df
--- /dev/null
+++ b/drivers/staging/otus/80211core/cfunc.c
@@ -0,0 +1,1227 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+
+u8_t zfQueryOppositeRate(zdev_t* dev, u8_t dst_mac[6], u8_t frameType)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* For AP's rate adaption */
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        return 0;
+    }
+
+    /* For STA's rate adaption */
+    if ( (frameType & 0x0c) == ZM_WLAN_DATA_FRAME )
+    {
+        if ( ZM_IS_MULTICAST(dst_mac) )
+        {
+            return wd->sta.mTxRate;
+        }
+        else
+        {
+            return wd->sta.uTxRate;
+        }
+    }
+
+    return wd->sta.mmTxRate;
+}
+
+void zfCopyToIntTxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* src,
+                         u16_t offset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length;i++)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset+i, src[i]);
+    }
+}
+
+void zfCopyToRxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* src,
+                      u16_t offset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length;i++)
+    {
+        zmw_rx_buf_writeb(dev, buf, offset+i, src[i]);
+    }
+}
+
+void zfCopyFromIntTxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* dst,
+                           u16_t offset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length; i++)
+    {
+        dst[i] = zmw_tx_buf_readb(dev, buf, offset+i);
+    }
+}
+
+void zfCopyFromRxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* dst,
+                        u16_t offset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length; i++)
+    {
+        dst[i] = zmw_rx_buf_readb(dev, buf, offset+i);
+    }
+}
+
+#if 1
+void zfMemoryCopy(u8_t* dst, u8_t* src, u16_t length)
+{
+    zfwMemoryCopy(dst, src, length);
+}
+
+void zfMemoryMove(u8_t* dst, u8_t* src, u16_t length)
+{
+    zfwMemoryMove(dst, src, length);
+}
+
+void zfZeroMemory(u8_t* va, u16_t length)
+{
+    zfwZeroMemory(va, length);
+}
+
+u8_t zfMemoryIsEqual(u8_t* m1, u8_t* m2, u16_t length)
+{
+    return zfwMemoryIsEqual(m1, m2, length);
+}
+#endif
+
+u8_t zfRxBufferEqualToStr(zdev_t* dev, zbuf_t* buf,
+                          const u8_t* str, u16_t offset, u16_t length)
+{
+    u16_t i;
+    u8_t ch;
+
+    for(i=0; i<length; i++)
+    {
+        ch = zmw_rx_buf_readb(dev, buf, offset+i);
+        if ( ch != str[i] )
+        {
+            return FALSE;
+        }
+    }
+
+    return TRUE;
+}
+
+void zfTxBufferCopy(zdev_t*dev, zbuf_t* dst, zbuf_t* src,
+                    u16_t dstOffset, u16_t srcOffset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length; i++)
+    {
+        zmw_tx_buf_writeb(dev, dst, dstOffset+i,
+                          zmw_tx_buf_readb(dev, src, srcOffset+i));
+    }
+}
+
+void zfRxBufferCopy(zdev_t*dev, zbuf_t* dst, zbuf_t* src,
+                    u16_t dstOffset, u16_t srcOffset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length; i++)
+    {
+        zmw_rx_buf_writeb(dev, dst, dstOffset+i,
+                             zmw_rx_buf_readb(dev, src, srcOffset+i));
+    }
+}
+
+
+void zfCollectHWTally(zdev_t*dev, u32_t* rsp, u8_t id)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (id == 0)
+    {
+        wd->commTally.Hw_UnderrunCnt += (0xFFFF & rsp[1]);
+        wd->commTally.Hw_TotalRxFrm += rsp[2];
+        wd->commTally.Hw_CRC32Cnt += rsp[3];
+        wd->commTally.Hw_CRC16Cnt += rsp[4];
+        #ifdef ZM_ENABLE_NATIVE_WIFI
+        /* These code are here to satisfy Vista DTM */
+        wd->commTally.Hw_DecrypErr_UNI += ((rsp[5]>50) && (rsp[5]<60))?50:rsp[5];
+        #else
+        wd->commTally.Hw_DecrypErr_UNI += rsp[5];
+        #endif
+        wd->commTally.Hw_RxFIFOOverrun += rsp[6];
+        wd->commTally.Hw_DecrypErr_Mul += rsp[7];
+        wd->commTally.Hw_RetryCnt += rsp[8];
+        wd->commTally.Hw_TotalTxFrm += rsp[9];
+        wd->commTally.Hw_RxTimeOut +=rsp[10];
+
+        wd->commTally.Tx_MPDU += rsp[11];
+        wd->commTally.BA_Fail += rsp[12];
+        wd->commTally.Hw_Tx_AMPDU += rsp[13];
+        wd->commTally.Hw_Tx_MPDU += rsp[14];
+        wd->commTally.RateCtrlTxMPDU += rsp[11];
+        wd->commTally.RateCtrlBAFail += rsp[12];
+    }
+    else
+    {
+        wd->commTally.Hw_RxMPDU += rsp[1];
+        wd->commTally.Hw_RxDropMPDU += rsp[2];
+        wd->commTally.Hw_RxDelMPDU += rsp[3];
+
+        wd->commTally.Hw_RxPhyMiscError += rsp[4];
+        wd->commTally.Hw_RxPhyXRError += rsp[5];
+        wd->commTally.Hw_RxPhyOFDMError += rsp[6];
+        wd->commTally.Hw_RxPhyCCKError += rsp[7];
+        wd->commTally.Hw_RxPhyHTError += rsp[8];
+        wd->commTally.Hw_RxPhyTotalCount += rsp[9];
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (id == 0)
+    {
+        zm_msg1_mm(ZM_LV_1, "rsplen =", rsp[0]);
+        zm_msg1_mm(ZM_LV_1, "Hw_UnderrunCnt    = ", (0xFFFF & rsp[1]));
+        zm_msg1_mm(ZM_LV_1, "Hw_TotalRxFrm     = ", rsp[2]);
+        zm_msg1_mm(ZM_LV_1, "Hw_CRC32Cnt       = ", rsp[3]);
+        zm_msg1_mm(ZM_LV_1, "Hw_CRC16Cnt       = ", rsp[4]);
+        zm_msg1_mm(ZM_LV_1, "Hw_DecrypErr_UNI  = ", rsp[5]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxFIFOOverrun  = ", rsp[6]);
+        zm_msg1_mm(ZM_LV_1, "Hw_DecrypErr_Mul  = ", rsp[7]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RetryCnt       = ", rsp[8]);
+        zm_msg1_mm(ZM_LV_1, "Hw_TotalTxFrm     = ", rsp[9]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxTimeOut      = ", rsp[10]);
+        zm_msg1_mm(ZM_LV_1, "Tx_MPDU           = ", rsp[11]);
+        zm_msg1_mm(ZM_LV_1, "BA_Fail           = ", rsp[12]);
+        zm_msg1_mm(ZM_LV_1, "Hw_Tx_AMPDU       = ", rsp[13]);
+        zm_msg1_mm(ZM_LV_1, "Hw_Tx_MPDU        = ", rsp[14]);
+    }
+    else
+    {
+        zm_msg1_mm(ZM_LV_1, "rsplen             = ", rsp[0]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxMPDU          = ", (0xFFFF & rsp[1]));
+        zm_msg1_mm(ZM_LV_1, "Hw_RxDropMPDU      = ", rsp[2]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxDelMPDU       = ", rsp[3]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyMiscError  = ", rsp[4]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyXRError    = ", rsp[5]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyOFDMError  = ", rsp[6]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyCCKError   = ", rsp[7]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyHTError    = ", rsp[8]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyTotalCount = ", rsp[9]);
+    }
+
+}
+
+/* Timer related functions */
+void zfTimerInit(zdev_t* dev)
+{
+    u8_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg0("");
+
+    wd->timerList.freeCount = ZM_MAX_TIMER_COUNT;
+    wd->timerList.head = &(wd->timerList.list[0]);
+    wd->timerList.tail = &(wd->timerList.list[ZM_MAX_TIMER_COUNT-1]);
+    wd->timerList.head->pre = NULL;
+    wd->timerList.head->next = &(wd->timerList.list[1]);
+    wd->timerList.tail->pre = &(wd->timerList.list[ZM_MAX_TIMER_COUNT-2]);
+    wd->timerList.tail->next = NULL;
+
+    for( i=1; i<(ZM_MAX_TIMER_COUNT-1); i++ )
+    {
+        wd->timerList.list[i].pre = &(wd->timerList.list[i-1]);
+        wd->timerList.list[i].next = &(wd->timerList.list[i+1]);
+    }
+
+    wd->bTimerReady = TRUE;
+}
+
+
+u16_t zfTimerSchedule(zdev_t* dev, u16_t event, u32_t tick)
+{
+    struct zsTimerEntry *pFreeEntry;
+    struct zsTimerEntry *pEntry;
+    u8_t   i, count;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->timerList.freeCount == 0 )
+    {
+        zm_debug_msg0("no more timer");
+        return 1;
+    }
+
+    //zm_debug_msg2("event = ", event);
+    //zm_debug_msg1("target tick = ", wd->tick + tick);
+
+    count = ZM_MAX_TIMER_COUNT - wd->timerList.freeCount;
+
+    if ( count == 0 )
+    {
+        wd->timerList.freeCount--;
+        wd->timerList.head->event = event;
+        wd->timerList.head->timer = wd->tick + tick;
+        //zm_debug_msg1("free timer count = ", wd->timerList.freeCount);
+
+        return 0;
+    }
+
+    pFreeEntry = wd->timerList.tail;
+    pFreeEntry->timer = wd->tick + tick;
+    pFreeEntry->event = event;
+    wd->timerList.tail = pFreeEntry->pre;
+    pEntry = wd->timerList.head;
+
+    for( i=0; i<count; i++ )
+    {
+        // prevent from the case of tick overflow
+        if ( ( pEntry->timer > pFreeEntry->timer )&&
+             ((pEntry->timer - pFreeEntry->timer) < 1000000000) )
+        {
+            if ( i != 0 )
+            {
+                pFreeEntry->pre = pEntry->pre;
+                pFreeEntry->pre->next = pFreeEntry;
+            }
+            else
+            {
+                pFreeEntry->pre = NULL;
+            }
+
+            pEntry->pre = pFreeEntry;
+            pFreeEntry->next = pEntry;
+            break;
+        }
+
+        pEntry = pEntry->next;
+    }
+
+    if ( i == 0 )
+    {
+        wd->timerList.head = pFreeEntry;
+    }
+
+    if ( i == count )
+    {
+        pFreeEntry->pre = pEntry->pre;
+        pFreeEntry->pre->next = pFreeEntry;
+        pEntry->pre = pFreeEntry;
+        pFreeEntry->next = pEntry;
+    }
+
+    wd->timerList.freeCount--;
+    //zm_debug_msg1("free timer count = ", wd->timerList.freeCount);
+
+    return 0;
+}
+
+u16_t zfTimerCancel(zdev_t* dev, u16_t event)
+{
+    struct zsTimerEntry *pEntry;
+    u8_t   i, count;
+
+    zmw_get_wlan_dev(dev);
+
+    //zm_debug_msg2("event = ", event);
+    //zm_debug_msg1("free timer count(b) = ", wd->timerList.freeCount);
+
+    pEntry = wd->timerList.head;
+    count = ZM_MAX_TIMER_COUNT - wd->timerList.freeCount;
+
+    for( i=0; i<count; i++ )
+    {
+        if ( pEntry->event == event )
+        {
+            if ( pEntry == wd->timerList.head )
+            {   /* remove head entry */
+                wd->timerList.head = pEntry->next;
+                wd->timerList.tail->next = pEntry;
+                pEntry->pre = wd->timerList.tail;
+                wd->timerList.tail = pEntry;
+                pEntry = wd->timerList.head;
+            }
+            else
+            {   /* remove non-head entry */
+                pEntry->pre->next = pEntry->next;
+                pEntry->next->pre = pEntry->pre;
+                wd->timerList.tail->next = pEntry;
+                pEntry->pre = wd->timerList.tail;
+                wd->timerList.tail = pEntry;
+                pEntry = pEntry->next;
+            }
+
+            wd->timerList.freeCount++;
+        }
+        else
+        {
+            pEntry = pEntry->next;
+        }
+    }
+
+    //zm_debug_msg1("free timer count(a) = ", wd->timerList.freeCount);
+
+    return 0;
+}
+
+void zfTimerClear(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->timerList.freeCount = ZM_MAX_TIMER_COUNT;
+}
+
+u16_t zfTimerCheckAndHandle(zdev_t* dev)
+{
+    struct zsTimerEntry *pEntry;
+    struct zsTimerEntry *pTheLastEntry = NULL;
+    u16_t  event[ZM_MAX_TIMER_COUNT];
+    u8_t   i, j=0, count;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( !wd->bTimerReady )
+    {
+        return 0;
+    }
+
+    zmw_enter_critical_section(dev);
+
+    pEntry = wd->timerList.head;
+    count = ZM_MAX_TIMER_COUNT - wd->timerList.freeCount;
+
+    for( i=0; i<count; i++ )
+    {
+        // prevent from the case of tick overflow
+        if ( ( pEntry->timer > wd->tick )&&
+             ((pEntry->timer - wd->tick) < 1000000000) )
+        {
+            break;
+        }
+
+        event[j++] = pEntry->event;
+        pTheLastEntry = pEntry;
+        pEntry = pEntry->next;
+    }
+
+    if ( j > 0 )
+    {
+        wd->timerList.tail->next = wd->timerList.head;
+        wd->timerList.head->pre = wd->timerList.tail;
+        wd->timerList.head = pEntry;
+        wd->timerList.tail = pTheLastEntry;
+        wd->timerList.freeCount += j;
+        //zm_debug_msg1("free timer count = ", wd->timerList.freeCount);
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zfProcessEvent(dev, event, j);
+
+    return 0;
+}
+
+u32_t zfCoreSetKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t type,
+        u16_t* mac, u32_t* key)
+{
+    u32_t ret;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->sta.flagKeyChanging++;
+    zm_debug_msg1("   zfCoreSetKey++++ ", wd->sta.flagKeyChanging);
+    zmw_leave_critical_section(dev);
+
+    ret = zfHpSetKey(dev, user, keyId, type, mac, key);
+    return ret;
+}
+
+void zfCoreSetKeyComplete(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+#if 0
+    wd->sta.flagKeyChanging = 0;
+#else
+    if(wd->sta.flagKeyChanging)
+    {
+        zmw_enter_critical_section(dev);
+        wd->sta.flagKeyChanging--;
+        zmw_leave_critical_section(dev);
+    }
+#endif
+    zm_debug_msg1("  zfCoreSetKeyComplete--- ", wd->sta.flagKeyChanging);
+
+    zfPushVtxq(dev);
+}
+
+void zfCoreHalInitComplete(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->halState = ZM_HAL_STATE_RUNNING;
+    zmw_leave_critical_section(dev);
+
+    zfPushVtxq(dev);
+}
+
+void zfCoreMacAddressNotify(zdev_t* dev, u8_t* addr)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->macAddr[0] = addr[0] | ((u16_t)addr[1]<<8);
+    wd->macAddr[1] = addr[2] | ((u16_t)addr[3]<<8);
+    wd->macAddr[2] = addr[4] | ((u16_t)addr[5]<<8);
+
+
+    //zfHpSetMacAddress(dev, wd->macAddr, 0);
+    if (wd->zfcbMacAddressNotify != NULL)
+    {
+        wd->zfcbMacAddressNotify(dev, addr);
+    }
+}
+
+void zfCoreSetIsoName(zdev_t* dev, u8_t* isoName)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.countryIsoName[0] = isoName[0];
+    wd->ws.countryIsoName[1] = isoName[1];
+    wd->ws.countryIsoName[2] = '\0';
+ }
+
+
+extern void zfScanMgrScanEventStart(zdev_t* dev);
+extern u8_t zfScanMgrScanEventTimeout(zdev_t* dev);
+extern void zfScanMgrScanEventRetry(zdev_t* dev);
+
+void zfProcessEvent(zdev_t* dev, u16_t* eventArray, u8_t eventCount)
+{
+    u8_t i, j, bypass = FALSE;
+    u16_t eventBypass[32];
+    u8_t eventBypassCount = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zfZeroMemory((u8_t*) eventBypass, 64);
+
+    for( i=0; i<eventCount; i++ )
+    {
+        for( j=0; j<eventBypassCount; j++ )
+        {
+            if ( eventBypass[j] == eventArray[i] )
+            {
+                bypass = TRUE;
+                break;
+            }
+        }
+
+        if ( bypass )
+        {
+            continue;
+        }
+
+        switch( eventArray[i] )
+        {
+            case ZM_EVENT_SCAN:
+                {
+                    zfScanMgrScanEventStart(dev);
+                    eventBypass[eventBypassCount++] = ZM_EVENT_IN_SCAN;
+                    eventBypass[eventBypassCount++] = ZM_EVENT_TIMEOUT_SCAN;
+                }
+                break;
+
+            case ZM_EVENT_TIMEOUT_SCAN:
+                {
+                    u8_t res;
+
+                    res = zfScanMgrScanEventTimeout(dev);
+                    if ( res == 0 )
+                    {
+                        eventBypass[eventBypassCount++] = ZM_EVENT_TIMEOUT_SCAN;
+                    }
+                    else if ( res == 1 )
+                    {
+                        eventBypass[eventBypassCount++] = ZM_EVENT_IN_SCAN;
+                    }
+                }
+                break;
+
+            case ZM_EVENT_IBSS_MONITOR:
+                {
+                    zfStaIbssMonitoring(dev, 0);
+                }
+                break;
+
+            case ZM_EVENT_IN_SCAN:
+                {
+                    zfScanMgrScanEventRetry(dev);
+                }
+                break;
+
+            case ZM_EVENT_CM_TIMER:
+                {
+                    zm_msg0_mm(ZM_LV_0, "ZM_EVENT_CM_TIMER");
+
+                    wd->sta.cmMicFailureCount = 0;
+                }
+                break;
+
+            case ZM_EVENT_CM_DISCONNECT:
+                {
+                    zm_msg0_mm(ZM_LV_0, "ZM_EVENT_CM_DISCONNECT");
+
+                    zfChangeAdapterState(dev, ZM_STA_STATE_DISCONNECT);
+
+                    zmw_enter_critical_section(dev);
+                    //zfTimerSchedule(dev, ZM_EVENT_CM_BLOCK_TIMER,
+                    //                ZM_TICK_CM_BLOCK_TIMEOUT);
+
+                    /* Timer Resolution on WinXP is 15/16 ms  */
+                    /* Decrease Time offset for <XP> Counter Measure */
+                    zfTimerSchedule(dev, ZM_EVENT_CM_BLOCK_TIMER,
+                                         ZM_TICK_CM_BLOCK_TIMEOUT - ZM_TICK_CM_BLOCK_TIMEOUT_OFFSET);
+
+                    zmw_leave_critical_section(dev);
+                    wd->sta.cmMicFailureCount = 0;
+                    //zfiWlanDisable(dev);
+                    zfHpResetKeyCache(dev);
+                    if (wd->zfcbConnectNotify != NULL)
+                    {
+                        wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_DISCONNECT_MIC_FAIL,
+                             wd->sta.bssid);
+                    }
+                }
+                break;
+
+            case ZM_EVENT_CM_BLOCK_TIMER:
+                {
+                    zm_msg0_mm(ZM_LV_0, "ZM_EVENT_CM_BLOCK_TIMER");
+
+                    //zmw_enter_critical_section(dev);
+                    wd->sta.cmDisallowSsidLength = 0;
+                    if ( wd->sta.bAutoReconnect )
+                    {
+                        zm_msg0_mm(ZM_LV_0, "ZM_EVENT_CM_BLOCK_TIMER:bAutoReconnect!=0");
+                        zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+                        zfScanMgrScanStart(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+                    }
+                    //zmw_leave_critical_section(dev);
+                }
+                break;
+
+            case ZM_EVENT_TIMEOUT_ADDBA:
+                {
+                    if (!wd->addbaComplete && (wd->addbaCount < 5))
+                    {
+                        zfAggSendAddbaRequest(dev, wd->sta.bssid, 0, 0);
+                        wd->addbaCount++;
+                        zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_ADDBA, 100);
+                    }
+                    else
+                    {
+                        zfTimerCancel(dev, ZM_EVENT_TIMEOUT_ADDBA);
+                    }
+                }
+                break;
+
+            #ifdef ZM_ENABLE_PERFORMANCE_EVALUATION
+            case ZM_EVENT_TIMEOUT_PERFORMANCE:
+                {
+                    zfiPerformanceRefresh(dev);
+                }
+                break;
+            #endif
+            case ZM_EVENT_SKIP_COUNTERMEASURE:
+				//enable the Countermeasure
+				{
+					zm_debug_msg0("Countermeasure : Enable MIC Check ");
+					wd->TKIP_Group_KeyChanging = 0x0;
+				}
+				break;
+
+            default:
+                break;
+        }
+    }
+}
+
+void zfBssInfoCreate(zdev_t* dev)
+{
+    u8_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    wd->sta.bssList.bssCount = 0;
+    wd->sta.bssList.head = NULL;
+    wd->sta.bssList.tail = NULL;
+    wd->sta.bssInfoArrayHead = 0;
+    wd->sta.bssInfoArrayTail = 0;
+    wd->sta.bssInfoFreeCount = ZM_MAX_BSS;
+
+    for( i=0; i< ZM_MAX_BSS; i++ )
+    {
+        //wd->sta.bssInfoArray[i] = &(wd->sta.bssInfoPool[i]);
+        wd->sta.bssInfoArray[i] = zfwMemAllocate(dev, sizeof(struct zsBssInfo));
+
+    }
+
+    zmw_leave_critical_section(dev);
+}
+
+void zfBssInfoDestroy(zdev_t* dev)
+{
+    u8_t   i;
+    zmw_get_wlan_dev(dev);
+
+    zfBssInfoRefresh(dev, 1);
+
+    for( i=0; i< ZM_MAX_BSS; i++ )
+    {
+        if (wd->sta.bssInfoArray[i] != NULL)
+        {
+            zfwMemFree(dev, wd->sta.bssInfoArray[i], sizeof(struct zsBssInfo));
+        }
+        else
+        {
+            zm_assert(0);
+        }
+    }
+    return;
+}
+
+struct zsBssInfo* zfBssInfoAllocate(zdev_t* dev)
+{
+    struct zsBssInfo* pBssInfo;
+
+    zmw_get_wlan_dev(dev);
+
+    if (wd->sta.bssInfoFreeCount == 0)
+        return NULL;
+
+    pBssInfo = wd->sta.bssInfoArray[wd->sta.bssInfoArrayHead];
+    wd->sta.bssInfoArray[wd->sta.bssInfoArrayHead] = NULL;
+    wd->sta.bssInfoArrayHead = (wd->sta.bssInfoArrayHead + 1) & (ZM_MAX_BSS - 1);
+    wd->sta.bssInfoFreeCount--;
+
+    zfZeroMemory((u8_t*)pBssInfo, sizeof(struct zsBssInfo));
+
+    return pBssInfo;
+}
+
+void zfBssInfoFree(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_assert(wd->sta.bssInfoArray[wd->sta.bssInfoArrayTail] == NULL);
+
+    pBssInfo->signalStrength = pBssInfo->signalQuality = 0;
+    pBssInfo->sortValue = 0;
+
+    wd->sta.bssInfoArray[wd->sta.bssInfoArrayTail] = pBssInfo;
+    wd->sta.bssInfoArrayTail = (wd->sta.bssInfoArrayTail + 1) & (ZM_MAX_BSS - 1);
+    wd->sta.bssInfoFreeCount++;
+}
+
+void zfBssInfoReorderList(zdev_t* dev)
+{
+    struct zsBssInfo* pBssInfo = NULL;
+    struct zsBssInfo* pInsBssInfo = NULL;
+    struct zsBssInfo* pNextBssInfo = NULL;
+    struct zsBssInfo* pPreBssInfo = NULL;
+    u8_t i = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->sta.bssList.bssCount > 1)
+    {
+        pInsBssInfo = wd->sta.bssList.head;
+        wd->sta.bssList.tail = pInsBssInfo;
+        pBssInfo = pInsBssInfo->next;
+        pInsBssInfo->next = NULL;
+        while (pBssInfo != NULL)
+        {
+            i = 0;
+            while (1)
+            {
+//                if (pBssInfo->signalStrength >= pInsBssInfo->signalStrength)
+                if( pBssInfo->sortValue >= pInsBssInfo->sortValue)
+                {
+                    if (i==0)
+                    {
+                        //Insert BssInfo to head
+                        wd->sta.bssList.head = pBssInfo;
+                        pNextBssInfo = pBssInfo->next;
+                        pBssInfo->next = pInsBssInfo;
+                        break;
+                    }
+                    else
+                    {
+                        //Insert BssInfo to neither head nor tail
+                        pPreBssInfo->next = pBssInfo;
+                        pNextBssInfo = pBssInfo->next;
+                        pBssInfo->next = pInsBssInfo;
+                        break;
+                    }
+                }
+                else
+                {
+                    if (pInsBssInfo->next != NULL)
+                    {
+                        //Signal strength smaller than current BssInfo, check next
+                        pPreBssInfo = pInsBssInfo;
+                        pInsBssInfo = pInsBssInfo->next;
+                    }
+                    else
+                    {
+                        //Insert BssInfo to tail
+                        pInsBssInfo->next = pBssInfo;
+                        pNextBssInfo = pBssInfo->next;
+                        wd->sta.bssList.tail = pBssInfo;
+                        pBssInfo->next = NULL;
+                        break;
+                    }
+                }
+                i++;
+            }
+            pBssInfo = pNextBssInfo;
+            pInsBssInfo = wd->sta.bssList.head;
+        }
+    } //if (wd->sta.bssList.bssCount > 1)
+
+    zmw_leave_critical_section(dev);
+}
+
+void zfBssInfoInsertToList(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_assert(pBssInfo);
+
+    //zm_debug_msg2("pBssInfo = ", pBssInfo);
+
+    if ( wd->sta.bssList.bssCount == 0 )
+    {
+        wd->sta.bssList.head = pBssInfo;
+        wd->sta.bssList.tail = pBssInfo;
+    }
+    else
+    {
+        wd->sta.bssList.tail->next = pBssInfo;
+        wd->sta.bssList.tail = pBssInfo;
+    }
+
+    pBssInfo->next = NULL;
+    wd->sta.bssList.bssCount++;
+
+    //zm_debug_msg2("bss count = ", wd->sta.bssList.bssCount);
+}
+
+void zfBssInfoRemoveFromList(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    struct zsBssInfo* pNowBssInfo;
+    struct zsBssInfo* pPreBssInfo = NULL;
+    u8_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_assert(pBssInfo);
+    zm_assert(wd->sta.bssList.bssCount);
+
+    //zm_debug_msg2("pBssInfo = ", pBssInfo);
+
+    pNowBssInfo = wd->sta.bssList.head;
+
+    for( i=0; i<wd->sta.bssList.bssCount; i++ )
+    {
+        if ( pNowBssInfo == pBssInfo )
+        {
+            if ( i == 0 )
+            {   /* remove head */
+                wd->sta.bssList.head = pBssInfo->next;
+            }
+            else
+            {
+                pPreBssInfo->next = pBssInfo->next;
+            }
+
+            if ( i == (wd->sta.bssList.bssCount - 1) )
+            {   /* remove tail */
+                wd->sta.bssList.tail = pPreBssInfo;
+            }
+
+            break;
+        }
+
+        pPreBssInfo = pNowBssInfo;
+        pNowBssInfo = pNowBssInfo->next;
+    }
+
+    zm_assert(i != wd->sta.bssList.bssCount);
+    wd->sta.bssList.bssCount--;
+
+    //zm_debug_msg2("bss count = ", wd->sta.bssList.bssCount);
+}
+
+void zfBssInfoRefresh(zdev_t* dev, u16_t mode)
+{
+    struct zsBssInfo*   pBssInfo;
+    struct zsBssInfo*   pNextBssInfo;
+    u8_t   i, bssCount;
+
+    zmw_get_wlan_dev(dev);
+
+    pBssInfo = wd->sta.bssList.head;
+    bssCount = wd->sta.bssList.bssCount;
+
+    for( i=0; i<bssCount; i++ )
+    {
+        if (mode == 1)
+        {
+            pNextBssInfo = pBssInfo->next;
+            zfBssInfoRemoveFromList(dev, pBssInfo);
+            zfBssInfoFree(dev, pBssInfo);
+            pBssInfo = pNextBssInfo;
+        }
+        else
+        {
+            if ( pBssInfo->flag & ZM_BSS_INFO_VALID_BIT )
+            {   /* this one must be kept */
+                pBssInfo->flag &= ~ZM_BSS_INFO_VALID_BIT;
+                pBssInfo = pBssInfo->next;
+            }
+            else
+            {
+                #define ZM_BSS_CACHE_TIME_IN_MS   20000
+                if ((wd->tick - pBssInfo->tick) > (ZM_BSS_CACHE_TIME_IN_MS/ZM_MS_PER_TICK))
+                {
+                    pNextBssInfo = pBssInfo->next;
+                    zfBssInfoRemoveFromList(dev, pBssInfo);
+                    zfBssInfoFree(dev, pBssInfo);
+                    pBssInfo = pNextBssInfo;
+                }
+                else
+                {
+                    pBssInfo = pBssInfo->next;
+                }
+            }
+        }
+    } //for( i=0; i<bssCount; i++ )
+    return;
+}
+
+void zfDumpSSID(u8_t length, u8_t *value)
+{
+    u8_t buf[50];
+    u8_t tmpLength = length;
+
+    if ( tmpLength > 49 )
+    {
+        tmpLength = 49;
+    }
+
+    zfMemoryCopy(buf, value, tmpLength);
+    buf[tmpLength] = '\0';
+    //printk("SSID: %s\n", buf);
+    //zm_debug_msg_s("ssid = ", value);
+}
+
+void zfCoreReinit(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.flagKeyChanging = 0;
+    wd->sta.flagFreqChanging = 0;
+}
+
+void zfGenerateRandomBSSID(zdev_t* dev, u8_t *MACAddr, u8_t *BSSID)
+{
+    //ULONGLONG   time;
+    u32_t time;
+
+    zmw_get_wlan_dev(dev);
+
+    time = wd->tick;
+
+    //
+    // Initialize the random BSSID to be the same as MAC address.
+    //
+
+    // RtlCopyMemory(BSSID, MACAddr, sizeof(DOT11_MAC_ADDRESS));
+    zfMemoryCopy(BSSID, MACAddr, 6);
+
+    //
+    // Get the system time in 10 millisecond.
+    //
+
+    // NdisGetCurrentSystemTime((PLARGE_INTEGER)&time);
+    // time /= 100000;
+
+    //
+    // Randomize the first 4 bytes of BSSID.
+    //
+
+    BSSID[0] ^= (u8_t)(time & 0xff);
+    BSSID[0] &= ~0x01;              // Turn off multicast bit
+    BSSID[0] |= 0x02;               // Turn on local bit
+
+    time >>= 8;
+    BSSID[1] ^= (u8_t)(time & 0xff);
+
+    time >>= 8;
+    BSSID[2] ^= (u8_t)(time & 0xff);
+
+    time >>= 8;
+    BSSID[3] ^= (u8_t)(time & 0xff);
+}
+
+u8_t zfiWlanGetDestAddrFromBuf(zdev_t *dev, zbuf_t *buf, u16_t *macAddr)
+{
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        /* DA */
+        macAddr[0] = zmw_tx_buf_readh(dev, buf, 16);
+        macAddr[1] = zmw_tx_buf_readh(dev, buf, 18);
+        macAddr[2] = zmw_tx_buf_readh(dev, buf, 20);
+    }
+    else if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        /* DA */
+        macAddr[0] = zmw_tx_buf_readh(dev, buf, 4);
+        macAddr[1] = zmw_tx_buf_readh(dev, buf, 6);
+        macAddr[2] = zmw_tx_buf_readh(dev, buf, 8);
+    }
+    else if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* DA */
+        macAddr[0] = zmw_tx_buf_readh(dev, buf, 4);
+        macAddr[1] = zmw_tx_buf_readh(dev, buf, 6);
+        macAddr[2] = zmw_tx_buf_readh(dev, buf, 8);
+    }
+    else
+    {
+        return 1;
+    }
+#else
+    /* DA */
+    macAddr[0] = zmw_tx_buf_readh(dev, buf, 0);
+    macAddr[1] = zmw_tx_buf_readh(dev, buf, 2);
+    macAddr[2] = zmw_tx_buf_readh(dev, buf, 4);
+#endif
+
+    return 0;
+}
+
+/* Leave an empty line below to remove warning message on some compiler */
+
+u16_t zfFindCleanFrequency(zdev_t* dev, u32_t adhocMode)
+{
+    u8_t   i, j;
+    u16_t  returnChannel;
+    u16_t  count_24G = 0, min24GIndex = 0;
+    u16_t  count_5G = 0,  min5GIndex = 0;
+    u16_t  CombinationBssNumberIn24G[15] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    u16_t  BssNumberIn24G[17]  = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    u16_t  Array_24G[15]       = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    u16_t  BssNumberIn5G[31]   = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    u16_t  Array_5G[31]        = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    struct zsBssInfo* pBssInfo;
+
+    zmw_get_wlan_dev(dev);
+
+    if ((pBssInfo = wd->sta.bssList.head) == NULL)
+    {
+        if( adhocMode == ZM_ADHOCBAND_B || adhocMode == ZM_ADHOCBAND_G ||
+            adhocMode == ZM_ADHOCBAND_BG || adhocMode == ZM_ADHOCBAND_ABG )
+        {
+            returnChannel = zfChGetFirst2GhzChannel(dev);
+        }
+        else
+        {
+            returnChannel = zfChGetFirst5GhzChannel(dev);
+        }
+
+        return returnChannel;
+    }
+
+    /* #1 Get Allowed Channel following Country Code ! */
+    zmw_declare_for_critical_section();
+    zmw_enter_critical_section(dev);
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if (wd->regulationTable.allowChannel[i].channel < 3000)
+        { // 2.4GHz
+            Array_24G[count_24G] = wd->regulationTable.allowChannel[i].channel;
+            count_24G++;
+        }
+        else
+        { // 5GHz
+            count_5G++;
+            Array_5G[i] = wd->regulationTable.allowChannel[i].channel;
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+    while( pBssInfo != NULL )
+    {
+        /* #2_1 Count BSS number in some specificed frequency in 2.4GHz band ! */
+        if( adhocMode == ZM_ADHOCBAND_B || adhocMode == ZM_ADHOCBAND_G ||
+            adhocMode == ZM_ADHOCBAND_BG || adhocMode == ZM_ADHOCBAND_ABG )
+        {
+            for( i=0; i<=(count_24G+3); i++ )
+            {
+                if( pBssInfo->frequency == Array_24G[i] )
+                { // Array_24G[0] correspond to BssNumberIn24G[2]
+                    BssNumberIn24G[pBssInfo->channel+1]++;
+                }
+            }
+        }
+
+        /* #2_2 Count BSS number in some specificed frequency in 5GHz band ! */
+        if( adhocMode == ZM_ADHOCBAND_A || adhocMode == ZM_ADHOCBAND_ABG )
+        {
+            for( i=0; i<count_5G; i++ )
+            { // 5GHz channel is not equal to array index
+                if( pBssInfo->frequency == Array_5G[i] )
+                { // Array_5G[0] correspond to BssNumberIn5G[0]
+                    BssNumberIn5G[i]++;
+                }
+            }
+        }
+
+        pBssInfo = pBssInfo->next;
+    }
+
+#if 0
+    for(i=0; i<=(count_24G+3); i++)
+    {
+        printk("2.4GHz Before combin, %d BSS network : %d", i, BssNumberIn24G[i]);
+    }
+
+    for(i=0; i<count_5G; i++)
+    {
+        printk("5GHz Before combin, %d BSS network : %d", i, BssNumberIn5G[i]);
+    }
+#endif
+
+    if( adhocMode == ZM_ADHOCBAND_B || adhocMode == ZM_ADHOCBAND_G ||
+        adhocMode == ZM_ADHOCBAND_BG || adhocMode == ZM_ADHOCBAND_ABG )
+    {
+        /* #3_1 Count BSS number that influence the specificed frequency in 2.4GHz ! */
+        for( j=0; j<count_24G; j++ )
+        {
+            CombinationBssNumberIn24G[j] = BssNumberIn24G[j]   + BssNumberIn24G[j+1] +
+                                           BssNumberIn24G[j+2] + BssNumberIn24G[j+3] +
+                                           BssNumberIn24G[j+4];
+            //printk("After combine, the number of BSS network channel %d is %d",
+            //                                   j , CombinationBssNumberIn24G[j]);
+        }
+
+        /* #4_1 Find the less utilized frequency in 2.4GHz band ! */
+        min24GIndex = zfFindMinimumUtilizationChannelIndex(dev, CombinationBssNumberIn24G, count_24G);
+    }
+
+    /* #4_2 Find the less utilized frequency in 5GHz band ! */
+    if( adhocMode == ZM_ADHOCBAND_A || adhocMode == ZM_ADHOCBAND_ABG )
+    {
+        min5GIndex = zfFindMinimumUtilizationChannelIndex(dev, BssNumberIn5G, count_5G);
+    }
+
+    if( adhocMode == ZM_ADHOCBAND_B || adhocMode == ZM_ADHOCBAND_G || adhocMode == ZM_ADHOCBAND_BG )
+    {
+        return Array_24G[min24GIndex];
+    }
+    else if( adhocMode == ZM_ADHOCBAND_A )
+    {
+        return Array_5G[min5GIndex];
+    }
+    else if( adhocMode == ZM_ADHOCBAND_ABG )
+    {
+        if ( CombinationBssNumberIn24G[min24GIndex] <= BssNumberIn5G[min5GIndex] )
+            return Array_24G[min24GIndex];
+        else
+            return Array_5G[min5GIndex];
+    }
+    else
+        return 2412;
+}
+
+u16_t zfFindMinimumUtilizationChannelIndex(zdev_t* dev, u16_t* array, u16_t count)
+{
+    u8_t   i;
+    u16_t  tempMinIndex, tempMinValue;
+
+    zmw_get_wlan_dev(dev);
+
+    i = 1;
+    tempMinIndex = 0;
+    tempMinValue = array[tempMinIndex];
+    while( i< count )
+    {
+        if( array[i] < tempMinValue )
+        {
+            tempMinValue = array[i];
+            tempMinIndex = i;
+        }
+        i++;
+    }
+
+    return tempMinIndex;
+}
+
+u8_t zfCompareWithBssid(zdev_t* dev, u16_t* bssid)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( zfMemoryIsEqual((u8_t*)bssid, (u8_t*)wd->sta.bssid, 6) )
+    {
+        return 1;
+    }
+    else
+    {
+        return 0;
+    }
+}
diff --git a/drivers/staging/otus/80211core/cfunc.h b/drivers/staging/otus/80211core/cfunc.h
new file mode 100644
index 000000000000..fc7548c39d1b
--- /dev/null
+++ b/drivers/staging/otus/80211core/cfunc.h
@@ -0,0 +1,449 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : func_extr.c                                           */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains function prototype.                        */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _CFUNC_H
+#define _CFUNC_H
+
+#include "queue.h"
+
+/* amsdu.c */
+void zfDeAmsdu(zdev_t* dev, zbuf_t* buf, u16_t vap, u8_t encryMode);
+
+/* cscanmgr.c */
+void zfScanMgrInit(zdev_t* dev);
+u8_t zfScanMgrScanStart(zdev_t* dev, u8_t scanType);
+void zfScanMgrScanStop(zdev_t* dev, u8_t scanType);
+void zfScanMgrScanAck(zdev_t* dev);
+
+/* cpsmgr.c */
+void zfPowerSavingMgrInit(zdev_t* dev);
+void zfPowerSavingMgrSetMode(zdev_t* dev, u8_t mode);
+void zfPowerSavingMgrMain(zdev_t* dev);
+void zfPowerSavingMgrWakeup(zdev_t* dev);
+u8_t zfPowerSavingMgrIsSleeping(zdev_t *dev);
+void zfPowerSavingMgrProcessBeacon(zdev_t* dev, zbuf_t* buf);
+void zfPowerSavingMgrAtimWinExpired(zdev_t* dev);
+void zfPowerSavingMgrConnectNotify(zdev_t *dev);
+void zfPowerSavingMgrPreTBTTInterrupt(zdev_t *dev);
+
+/* ccmd.c */
+u16_t zfWlanEnable(zdev_t* dev);
+
+/* cfunc.c */
+u8_t zfQueryOppositeRate(zdev_t* dev, u8_t dst_mac[6], u8_t frameType);
+void zfCopyToIntTxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* src,
+                         u16_t offset, u16_t length);
+void zfCopyToRxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* src,
+                      u16_t offset, u16_t length);
+void zfCopyFromIntTxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* dst,
+                           u16_t offset, u16_t length);
+void zfCopyFromRxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* dst,
+                        u16_t offset, u16_t length);
+void zfMemoryCopy(u8_t* dst, u8_t* src, u16_t length);
+void zfMemoryMove(u8_t* dst, u8_t* src, u16_t length);
+void zfZeroMemory(u8_t* va, u16_t length);
+u8_t zfMemoryIsEqual(u8_t* m1, u8_t* m2, u16_t length);
+u8_t zfRxBufferEqualToStr(zdev_t* dev, zbuf_t* buf, const u8_t* str,
+                          u16_t offset, u16_t length);
+void zfTxBufferCopy(zdev_t*dev, zbuf_t* dst, zbuf_t* src,
+                    u16_t dstOffset, u16_t srcOffset, u16_t length);
+void zfRxBufferCopy(zdev_t*dev, zbuf_t* dst, zbuf_t* src,
+                    u16_t dstOffset, u16_t srcOffset, u16_t length);
+
+void zfCollectHWTally(zdev_t*dev, u32_t* rsp, u8_t id);
+void zfTimerInit(zdev_t* dev);
+u16_t zfTimerSchedule(zdev_t* dev, u16_t event, u32_t tick);
+u16_t zfTimerCancel(zdev_t* dev, u16_t event);
+void zfTimerClear(zdev_t* dev);
+u16_t zfTimerCheckAndHandle(zdev_t* dev);
+void zfProcessEvent(zdev_t* dev, u16_t* eventArray, u8_t eventCount);
+
+void zfBssInfoCreate(zdev_t* dev);
+void zfBssInfoDestroy(zdev_t* dev);
+
+struct zsBssInfo* zfBssInfoAllocate(zdev_t* dev);
+void zfBssInfoFree(zdev_t* dev, struct zsBssInfo* pBssInfo);
+void zfBssInfoReorderList(zdev_t* dev);
+void zfBssInfoInsertToList(zdev_t* dev, struct zsBssInfo* pBssInfo);
+void zfBssInfoRemoveFromList(zdev_t* dev, struct zsBssInfo* pBssInfo);
+void zfBssInfoRefresh(zdev_t* dev, u16_t mode);
+void zfCoreSetFrequencyComplete(zdev_t* dev);
+void zfCoreSetFrequency(zdev_t* dev, u16_t frequency);
+void zfCoreSetFrequencyV2(zdev_t* dev, u16_t frequency,
+        zfpFreqChangeCompleteCb cb);
+void zfCoreSetFrequencyEx(zdev_t* dev, u16_t frequency, u8_t bw40,
+        u8_t extOffset, zfpFreqChangeCompleteCb cb);
+void zfCoreSetFrequencyExV2(zdev_t* dev, u16_t frequency, u8_t bw40,
+        u8_t extOffset, zfpFreqChangeCompleteCb cb, u8_t forceSetFreq);
+void zfReSetCurrentFrequency(zdev_t* dev);
+u32_t zfCoreSetKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t type,
+        u16_t* mac, u32_t* key);
+void zfCoreSetKeyComplete(zdev_t* dev);
+void zfCoreReinit(zdev_t* dev);
+void zfCoreMacAddressNotify(zdev_t* dev, u8_t *addr);
+void zfCoreSetIsoName(zdev_t* dev, u8_t* isoName);
+void zfGenerateRandomBSSID(zdev_t* dev, u8_t *MACAddr, u8_t *BSSID);
+void zfCoreHalInitComplete(zdev_t* dev);
+
+u16_t zfFindCleanFrequency(zdev_t* dev, u32_t adhocMode);
+u16_t zfFindMinimumUtilizationChannelIndex(zdev_t* dev, u16_t* array, u16_t count);
+u8_t zfCompareWithBssid(zdev_t* dev, u16_t* bssid);
+
+/* chb.c */
+void zfDumpBssList(zdev_t* dev);
+
+
+u16_t zfIssueCmd(zdev_t* dev, u32_t* cmd, u16_t cmdLen, u16_t src, u8_t* buf);
+
+
+/* cic.c */
+void zfUpdateBssid(zdev_t* dev, u8_t* bssid);
+void zfResetSupportRate(zdev_t* dev, u8_t type);
+void zfUpdateSupportRate(zdev_t* dev, u8_t* rateArray);
+u8_t zfIsGOnlyMode(zdev_t* dev, u16_t  frequency, u8_t* rateArray);
+void zfGatherBMode(zdev_t* dev, u8_t* rateArray, u8_t* extrateArray);
+u8_t zfPSDeviceSleep(zdev_t* dev);
+u16_t zfGetRandomNumber(zdev_t* dev, u16_t initValue);
+void zfCoreEvent(zdev_t* dev, u16_t event, u8_t* rsp);
+void zfBeaconCfgInterrupt(zdev_t* dev, u8_t* rsp);
+void zfEndOfAtimWindowInterrupt(zdev_t* dev);
+
+/* cinit.c */
+u16_t zfTxGenWlanHeader(zdev_t* dev, zbuf_t* buf, u16_t* header, u16_t seq,
+                        u8_t flag, u16_t plusLen, u16_t minusLen, u16_t port,
+                        u16_t* da, u16_t* sa, u8_t up, u16_t *micLen,
+                        u16_t* snap, u16_t snapLen, struct aggControl *aggControl);
+u16_t zfTxGenMmHeader(zdev_t* dev, u8_t frameType, u16_t* dst,
+        u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt);
+void zfInitMacApMode(zdev_t* dev);
+u16_t zfChGetNextChannel(zdev_t* dev, u16_t frequency, u8_t* pbPassive);
+u16_t zfChGetFirstChannel(zdev_t* dev, u8_t* pbPassive);
+u16_t zfChGetFirst2GhzChannel(zdev_t* dev);
+u16_t zfChGetFirst5GhzChannel(zdev_t* dev);
+u16_t zfChGetLastChannel(zdev_t* dev, u8_t* pbPassive);
+u16_t zfChGetLast5GhzChannel(zdev_t* dev);
+u16_t zfChNumToFreq(zdev_t* dev, u8_t ch, u8_t freqBand);
+u8_t zfChFreqToNum(u16_t freq, u8_t* bIs5GBand);
+
+/* cmm.c */
+void zfProcessManagement(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo); //CWYang(m)
+void zfSendMmFrame(zdev_t* dev, u8_t frameType, u16_t* dst,
+                   u32_t p1, u32_t p2, u32_t p3);
+u16_t zfFindElement(zdev_t* dev, zbuf_t* buf, u8_t eid);
+u16_t zfFindWifiElement(zdev_t* dev, zbuf_t* buf, u8_t type, u8_t subtype);
+u16_t zfFindSuperGElement(zdev_t* dev, zbuf_t* buf, u8_t type);
+u16_t zfFindXRElement(zdev_t* dev, zbuf_t* buf, u8_t type);
+u16_t zfRemoveElement(zdev_t* dev, u8_t* buf, u16_t size, u8_t eid);
+u16_t zfUpdateElement(zdev_t* dev, u8_t* buf, u16_t size, u8_t* updateeid);
+void zfProcessProbeReq(zdev_t* dev, zbuf_t* buf, u16_t* src);
+void zfProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo);
+u16_t zfSendProbeReq(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t bWithSSID);
+u16_t zfMmAddIeSupportRate(zdev_t* dev, zbuf_t* buf,
+                           u16_t offset, u8_t eid, u8_t rateSet);
+u16_t zfMmAddIeDs(zdev_t* dev, zbuf_t* buf, u16_t offset);
+u16_t zfMmAddIeErp(zdev_t* dev, zbuf_t* buf, u16_t offset);
+void zfUpdateDefaultQosParameter(zdev_t* dev, u8_t mode);
+u16_t zfMmAddIeWpa(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t apId);
+u16_t zfMmAddHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset); //CWYang(+)
+u16_t zfMmAddPreNHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset);
+u16_t zfMmAddExtendedHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset); //CWYang(+)
+u16_t zfFindATHExtCap(zdev_t* dev, zbuf_t* buf, u8_t type, u8_t subtype);
+u16_t zfFindBrdcmMrvlRlnkExtCap(zdev_t* dev, zbuf_t* buf);
+u16_t zfFindMarvelExtCap(zdev_t* dev, zbuf_t* buf);
+u16_t zfFindBroadcomExtCap(zdev_t* dev, zbuf_t* buf);
+u16_t zfFindRlnkExtCap(zdev_t* dev, zbuf_t* buf);
+
+/* cmmap.c */
+void zfMmApTimeTick(zdev_t* dev);
+void zfApAgingSta(zdev_t* dev);
+u16_t zfApAddSta(zdev_t* dev, u16_t* addr, u16_t state, u16_t apId, u8_t type,
+                 u8_t qosType, u8_t qosInfo);
+void zfApProtctionMonitor(zdev_t* dev);
+void zfApProcessBeacon(zdev_t* dev, zbuf_t* buf);
+void zfApProcessAuth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfApProcessAsocReq(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfApProcessAsocRsp(zdev_t* dev, zbuf_t* buf);
+void zfApProcessDeauth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfApProcessDisasoc(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfApProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo);
+void zfApStoreAsocReqIe(zdev_t* dev, zbuf_t* buf, u16_t aid);
+u16_t zfApAddIeSsid(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t vap);
+void zfApSendBeacon(zdev_t* dev);
+u16_t zfApGetSTAInfo(zdev_t* dev, u16_t* addr, u16_t* state, u8_t* vap);
+u16_t zfIntrabssForward(zdev_t* dev, zbuf_t* buf, u8_t srcVap);
+u16_t zfApBufferPsFrame(zdev_t* dev, zbuf_t* buf, u16_t port);
+void zfApInitStaTbl(zdev_t* dev);
+void zfApGetStaTxRateAndQosType(zdev_t* dev, u16_t* addr, u32_t* phyCtrl,
+                                u8_t* qosType, u16_t* rcProbingFlag);
+void zfApGetStaQosType(zdev_t* dev, u16_t* addr, u8_t* qosType);
+void zfApSetStaTxRate(zdev_t* dev, u16_t* addr, u32_t phyCtrl);
+struct zsMicVar* zfApGetRxMicKey(zdev_t* dev, zbuf_t* buf);
+struct zsMicVar* zfApGetTxMicKey(zdev_t* dev, zbuf_t* buf, u8_t* qosType);
+u16_t zfApAddIeWmePara(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t vap);
+u16_t zfApUpdatePsBit(zdev_t* dev, zbuf_t* buf, u8_t* vap, u8_t* uapsdTrig);
+void zfApProcessPsPoll(zdev_t* dev, zbuf_t* buf);
+u16_t zfApFindSta(zdev_t* dev, u16_t* addr);
+void zfApGetStaEncryType(zdev_t* dev, u16_t* addr, u8_t* encryType);
+void zfApGetStaWpaIv(zdev_t* dev, u16_t* addr, u16_t* iv16, u32_t* iv32);
+void zfApSetStaWpaIv(zdev_t* dev, u16_t* addr, u16_t iv16, u32_t iv32);
+void zfApClearStaKey(zdev_t* dev, u16_t* addr);
+#ifdef ZM_ENABLE_CENC
+void zfApGetStaCencIvAndKeyIdx(zdev_t* dev, u16_t* addr, u32_t *iv,
+        u8_t *keyIdx);
+void zfApSetStaCencIv(zdev_t* dev, u16_t* addr, u32_t *iv);
+#endif //ZM_ENABLE_CENC
+void zfApSetProtectionMode(zdev_t* dev, u16_t mode);
+void zfApFlushBufferedPsFrame(zdev_t* dev);
+void zfApSendFailure(zdev_t* dev, u8_t* addr);
+u8_t zfApRemoveFromPsQueue(zdev_t* dev, u16_t id, u16_t* src);
+void zfApProcessAction(zdev_t* dev, zbuf_t* buf);
+/* cmmsta.c */
+void zfMmStaTimeTick(zdev_t* dev);
+void zfReWriteBeaconStartAddress(zdev_t* dev);  // Mxzeng
+void zfStaProcessBeacon(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo); //CWYang(m)
+void zfStaProcessAuth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfStaProcessAsocReq(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfStaProcessAsocRsp(zdev_t* dev, zbuf_t* buf);
+void zfStaProcessDeauth(zdev_t* dev, zbuf_t* buf);
+void zfStaProcessDisasoc(zdev_t* dev, zbuf_t* buf);
+void zfStaProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo);
+void zfStaProcessAtim(zdev_t* dev, zbuf_t* buf);
+void zfStaStoreAsocRspIe(zdev_t* dev, zbuf_t* buf);
+void zfStaChannelManagement(zdev_t* dev, u8_t scan);
+void zfIbssConnectNetwork(zdev_t* dev);
+void zfInfraConnectNetwork(zdev_t* dev);
+u8_t zfCheckAuthentication(zdev_t* dev, struct zsBssInfo* pBssInfo);
+u8_t zfChangeAdapterState(zdev_t* dev, u8_t newState);
+u16_t zfStaAddIeSsid(zdev_t* dev, zbuf_t* buf, u16_t offset);
+u16_t zfStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType);
+u16_t zfStaAddIeIbss(zdev_t* dev, zbuf_t* buf, u16_t offset);
+void zfStaStartConnect(zdev_t* dev, u8_t bIsSharedKey);
+u8_t zfStaIsConnected(zdev_t* dev);
+u8_t zfStaIsConnecting(zdev_t* dev);
+u8_t zfStaIsDisconnect(zdev_t* dev);
+void zfStaSendBeacon(zdev_t* dev);
+void zfSendNullData(zdev_t* dev, u8_t type);
+void zfSendPSPoll(zdev_t* dev);
+void zfSendBA(zdev_t* dev, u16_t start_seq, u8_t *bitmap);
+void zdRateInfoCountTx(zdev_t* dev, u16_t* macAddr);
+struct zsMicVar* zfStaGetRxMicKey(zdev_t* dev, zbuf_t* buf);
+struct zsMicVar* zfStaGetTxMicKey(zdev_t* dev, zbuf_t* buf);
+u16_t zfStaRxValidateFrame(zdev_t* dev, zbuf_t* buf);
+void zfStaMicFailureHandling(zdev_t* dev, zbuf_t* buf);
+u8_t zfStaBlockWlanScan(zdev_t* dev);
+void zfStaIbssPSCheckState(zdev_t* dev, zbuf_t* buf);
+u8_t zfStaIbssPSQueueData(zdev_t* dev, zbuf_t* buf);
+void zfStaIbssPSSend(zdev_t* dev);
+void zfStaResetStatus(zdev_t* dev, u8_t bInit);
+u16_t zfStaAddIeWmeInfo(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t qosInfo);
+void zfInitPartnerNotifyEvent(zdev_t* dev, zbuf_t* buf, struct zsPartnerNotifyEvent *event);
+void zfStaInitOppositeInfo(zdev_t* dev);
+void zfStaIbssMonitoring(zdev_t* dev, u8_t reset);
+struct zsBssInfo* zfStaFindBssInfo(zdev_t* dev, zbuf_t* buf, struct zsWlanProbeRspFrameHeader *pProbeRspHeader);
+u8_t zfStaInitBssInfo(zdev_t* dev, zbuf_t* buf,
+        struct zsWlanProbeRspFrameHeader *pProbeRspHeader,
+        struct zsBssInfo* pBssInfo, struct zsAdditionInfo* AddInfo, u8_t type);
+s8_t zfStaFindFreeOpposite(zdev_t* dev, u16_t *sa, int *pFoundIdx);
+s8_t zfStaFindOppositeByMACAddr(zdev_t* dev, u16_t *sa, u8_t *pFoundIdx);
+void zfStaRefreshBlockList(zdev_t* dev, u16_t flushFlag);
+void zfStaConnectFail(zdev_t* dev, u16_t reason, u16_t* bssid, u8_t weight);
+void zfStaGetTxRate(zdev_t* dev, u16_t* macAddr, u32_t* phyCtrl,
+        u16_t* rcProbingFlag);
+u16_t zfStaProcessAction(zdev_t* dev, zbuf_t* buf);
+struct zsTkipSeed* zfStaGetRxSeed(zdev_t* dev, zbuf_t* buf);
+#ifdef ZM_ENABLE_CENC
+/* CENC */
+u16_t zfStaAddIeCenc(zdev_t* dev, zbuf_t* buf, u16_t offset);
+#endif //ZM_ENABLE_CENC
+void zfStaEnableSWEncryption(zdev_t *dev, u8_t value);
+void zfStaDisableSWEncryption(zdev_t *dev);
+u16_t zfComputeBssInfoWeightValue(zdev_t *dev, u8_t isBMode, u8_t isHT, u8_t isHT40, u8_t signalStrength);
+u16_t zfStaAddIbssAdditionalIE(zdev_t* dev, zbuf_t* buf, u16_t offset);
+
+/* ctkip.c */
+void zfTkipInit(u8_t* key, u8_t* ta, struct zsTkipSeed* pSeed, u8_t* initIv);
+void zfMicSetKey(u8_t* key, struct zsMicVar* pMic);
+void zfMicAppendByte(u8_t b, struct zsMicVar* pMic);
+void zfMicClear(struct zsMicVar* pMic);
+void zfMicAppendTxBuf(zdev_t* dev, zbuf_t* buf, u8_t* da, u8_t* sa,
+                      u16_t removeLen, u8_t* mic);
+u8_t zfMicRxVerify(zdev_t* dev, zbuf_t* buf);
+void zfMicGetMic(u8_t* dst, struct zsMicVar* pMic);
+void zfCalTxMic(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u16_t *da, u16_t *sa, u8_t up, u8_t *mic);
+void zfTKIPEncrypt(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u8_t keyLen, u8_t* key, u32_t* icv);
+u16_t zfTKIPDecrypt(zdev_t *dev, zbuf_t *buf, u16_t offset, u8_t keyLen, u8_t* key);
+void zfTkipGetseeds(u16_t iv16, u8_t *RC4Key, struct zsTkipSeed *Seed);
+u8_t zfTkipPhase1KeyMix(u32_t iv32, struct zsTkipSeed* pSeed);
+u8_t zfTkipPhase2KeyMix(u16_t iv16, struct zsTkipSeed* pSeed);
+void zfWEPEncrypt(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u8_t keyLen, u8_t* WepKey, u8_t *iv);
+u16_t zfWEPDecrypt(zdev_t *dev, zbuf_t *buf, u16_t offset, u8_t keyLen, u8_t* WepKey, u8_t *iv);
+
+/* ctxrx.c */
+u16_t zfSend80211Frame(zdev_t* dev, zbuf_t* buf);
+void zfIsrPciTxComp(zdev_t* dev);
+void zfTxPciDmaStart(zdev_t* dev);
+u16_t zfTxPortControl(zdev_t* dev, zbuf_t* buf, u16_t port);
+u16_t zfTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port,
+                  u16_t bufType, u16_t flag);
+u16_t zfTxGenWlanTail(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t snaplen,
+                      u16_t* mic);
+u16_t zfTxGenWlanSnap(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t* snaplen);
+void zfTxGetIpTosAndFrag(zdev_t* dev, zbuf_t* buf, u8_t* up, u16_t* fragOff);
+u16_t zfPutVtxq(zdev_t* dev, zbuf_t* buf);
+void zfPushVtxq(zdev_t* dev);
+u8_t zfIsVtxqEmpty(zdev_t* dev);
+u16_t zfGetSeqCtrl(zdev_t* dev, zbuf_t* buf, u16_t offset);
+u8_t zfGetFragNo(zdev_t* dev, zbuf_t* buf);
+void zfShowRxEAPOL(zdev_t* dev, zbuf_t* buf, u16_t offset);
+void zfShowTxEAPOL(zdev_t* dev, zbuf_t* buf, u16_t offset);
+void zfCoreRecv(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+u16_t zfPutVmmq(zdev_t* dev, zbuf_t* buf);
+void zfFlushVtxq(zdev_t* dev);
+void zfAgingDefragList(zdev_t* dev, u16_t flushFlag);
+
+void zfLed100msCtrl(zdev_t* dev);
+void zf80211FrameSend(zdev_t* dev, zbuf_t* buf, u16_t* header, u16_t snapLen,
+                           u16_t* da, u16_t* sa, u8_t up, u16_t headerLen, u16_t* snap,
+                           u16_t* tail, u16_t tailLen, u16_t offset, u16_t bufType,
+                           u8_t ac, u8_t keyIdx);
+void zfCheckIsRIFSFrame(zdev_t* dev, zbuf_t* buf, u16_t frameSubType);
+
+/* queue.c */
+struct zsQueue* zfQueueCreate(zdev_t* dev, u16_t size);
+void zfQueueDestroy(zdev_t* dev, struct zsQueue* q);
+u16_t zfQueuePutNcs(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick);
+u16_t zfQueuePut(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick);
+zbuf_t* zfQueueGet(zdev_t* dev, struct zsQueue* q);
+zbuf_t* zfQueueGetWithMac(zdev_t* dev, struct zsQueue* q, u8_t* addr, u8_t* mb);
+void zfQueueFlush(zdev_t* dev, struct zsQueue* q);
+void zfQueueAge(zdev_t* dev, struct zsQueue* q, u32_t tick, u32_t msAge);
+void zfQueueGenerateUapsdTim(zdev_t* dev, struct zsQueue* q,
+        u8_t* uniBitMap, u16_t* highestByte);
+
+/* hpmain.c */
+u16_t zfHpInit(zdev_t* dev, u32_t frequency);
+u16_t zfHpRelease(zdev_t* dev);
+void zfHpSetFrequencyEx(zdev_t* dev, u32_t frequency, u8_t bw40,
+        u8_t extOffset, u8_t initRF);
+u16_t zfHpStartRecv(zdev_t* dev);
+u16_t zfHpStopRecv(zdev_t* dev);
+u16_t zfHpResetKeyCache(zdev_t* dev);
+u16_t zfHpSetApStaMode(zdev_t* dev, u8_t mode);
+u16_t zfHpSetBssid(zdev_t* dev, u8_t* bssid);
+u16_t zfHpSetSnifferMode(zdev_t* dev, u16_t on);
+u8_t zfHpUpdateQosParameter(zdev_t* dev, u16_t* cwminTbl, u16_t* cwmaxTbl,
+        u16_t* aifsTbl, u16_t* txopTbl);
+void zfHpSetAtimWindow(zdev_t* dev, u16_t atimWin);
+void zfHpEnableBeacon(zdev_t* dev, u16_t mode, u16_t bcnInterval, u16_t dtim, u8_t enableAtim);
+void zfHpDisableBeacon(zdev_t* dev);
+void zfHpSetBasicRateSet(zdev_t* dev, u16_t bRateBasic, u16_t gRateBasic);
+void zfHpSetRTSCTSRate(zdev_t* dev, u32_t rate);
+void zfHpSetMacAddress(zdev_t* dev, u16_t* macAddr, u16_t macAddrId);
+u32_t zfHpGetMacAddress(zdev_t* dev);
+u32_t zfHpGetTransmitPower(zdev_t* dev);
+void zfHpSetMulticastList(zdev_t* dev, u8_t size, u8_t* pList, u8_t bAllMulticast);
+
+u16_t zfHpRemoveKey(zdev_t* dev, u16_t user);
+u32_t zfHpSetKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t type,
+        u16_t* mac, u32_t* key);
+//u32_t zfHpSetStaPairwiseKey(zdev_t* dev, u16_t* apMacAddr, u8_t type,
+//        u32_t* key, u32_t* micKey);
+//u32_t zfHpSetStaGroupKey(zdev_t* dev, u16_t* apMacAddr, u8_t type,
+//        u32_t* key, u32_t* micKey);
+u32_t zfHpSetApPairwiseKey(zdev_t* dev, u16_t* staMacAddr, u8_t type,
+        u32_t* key, u32_t* micKey, u16_t staAid);
+u32_t zfHpSetApGroupKey(zdev_t* dev, u16_t* apMacAddr, u8_t type,
+        u32_t* key, u32_t* micKey, u16_t vapId);
+u32_t zfHpSetDefaultKey(zdev_t* dev, u8_t keyId, u8_t type, u32_t* key, u32_t* micKey);
+u32_t zfHpSetPerUserKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t* mac, u8_t type, u32_t* key, u32_t* micKey);
+
+void zfHpSendBeacon(zdev_t* dev, zbuf_t* buf, u16_t len);
+u16_t zfHpGetPayloadLen(zdev_t* dev,
+                        zbuf_t* buf,
+                        u16_t len,
+                        u16_t plcpHdrLen,
+                        u32_t *rxMT,
+                        u32_t *rxMCS,
+                        u32_t *rxBW,
+                        u32_t *rxSG
+                        );
+u32_t zfHpGetFreeTxdCount(zdev_t* dev);
+u32_t zfHpGetMaxTxdCount(zdev_t* dev);
+u16_t zfHpSend(zdev_t* dev, u16_t* header, u16_t headerLen,
+        u16_t* snap, u16_t snapLen, u16_t* tail, u16_t tailLen, zbuf_t* buf,
+        u16_t offset, u16_t bufType, u8_t ac, u8_t keyIdx);
+void zfHpGetRegulationTablefromRegionCode(zdev_t* dev, u16_t regionCode);
+void zfHpGetRegulationTablefromCountry(zdev_t* dev, u16_t CountryCode);
+u8_t zfHpGetRegulationTablefromISO(zdev_t* dev, u8_t *countryInfo, u8_t length);
+const char* zfHpGetisoNamefromregionCode(zdev_t* dev, u16_t regionCode);
+u16_t zfHpGetRegionCodeFromIsoName(zdev_t* dev, u8_t *countryIsoName);
+u8_t zfHpGetRegulatoryDomain(zdev_t* dev);
+void zfHpLedCtrl(zdev_t* dev, u16_t ledId, u8_t mode);
+u16_t zfHpResetTxRx(zdev_t* dev);
+u16_t zfHpDeleteAllowChannel(zdev_t* dev, u16_t freq);
+u16_t zfHpAddAllowChannel(zdev_t* dev, u16_t freq);
+u32_t zfHpCwmUpdate(zdev_t* dev);
+u32_t zfHpAniUpdate(zdev_t* dev);
+u32_t zfHpAniUpdateRssi(zdev_t* dev, u8_t rssi);
+void zfHpAniAttach(zdev_t* dev);
+void zfHpAniArPoll(zdev_t* dev, u32_t listenTime, u32_t phyCnt1, u32_t phyCnt2);
+void zfHpHeartBeat(zdev_t* dev);
+void zfHpPowerSaveSetState(zdev_t* dev, u8_t psState);
+void zfHpPowerSaveSetMode(zdev_t* dev, u8_t staMode, u8_t psMode, u16_t bcnInterval);
+u16_t zfHpIsDfsChannel(zdev_t* dev, u16_t freq);
+u16_t zfHpIsDfsChannelNCS(zdev_t* dev, u16_t freq);
+u16_t zfHpFindFirstNonDfsChannel(zdev_t* dev, u16_t aBand);
+u16_t zfHpIsAllowedChannel(zdev_t* dev, u16_t freq);
+void zfHpDisableDfsChannel(zdev_t* dev, u8_t disableFlag);
+void zfHpSetTTSIFSTime(zdev_t* dev, u8_t sifs_time);
+
+void zfHpQueryMonHalRxInfo(zdev_t* dev, u8_t *monHalRxInfo);
+
+void zfDumpSSID(u8_t length, u8_t *value);
+void zfHpSetAggPktNum(zdev_t* dev, u32_t num);
+void zfHpSetMPDUDensity(zdev_t* dev, u8_t density);
+void zfHpSetSlotTime(zdev_t* dev, u8_t type);
+void zfHpSetSlotTimeRegister(zdev_t* dev, u8_t type);
+void zfHpSetRifs(zdev_t* dev, u8_t ht_enable, u8_t ht2040, u8_t g_mode);
+void zfHpBeginSiteSurvey(zdev_t* dev, u8_t status);
+void zfHpFinishSiteSurvey(zdev_t* dev, u8_t status);
+u16_t zfHpEnableHwRetry(zdev_t* dev);
+u16_t zfHpDisableHwRetry(zdev_t* dev);
+void zfHpSWDecrypt(zdev_t* dev, u8_t enable);
+void zfHpSWEncrypt(zdev_t* dev, u8_t enable);
+u32_t zfHpCapability(zdev_t* dev);
+void zfHpSetRollCallTable(zdev_t* dev);
+u8_t zfHpregulatoryDomain(zdev_t* dev);
+u16_t zfStaAddIePowerCap(zdev_t* dev, zbuf_t* buf, u16_t offset);
+u8_t zfHpGetMaxTxPower(zdev_t* dev);
+u8_t zfHpGetMinTxPower(zdev_t* dev);
+u16_t zfStaAddIeSupportCh(zdev_t* dev, zbuf_t* buf, u16_t offset);
+void zfHpEnableRifs(zdev_t* dev, u8_t mode24g, u8_t modeHt, u8_t modeHt2040);
+void zfHpDisableRifs(zdev_t* dev);
+u16_t zfHpUsbReset(zdev_t* dev);
+
+
+#endif /* #ifndef _CFUNC_H */
diff --git a/drivers/staging/otus/80211core/chb.c b/drivers/staging/otus/80211core/chb.c
new file mode 100644
index 000000000000..7fac15011256
--- /dev/null
+++ b/drivers/staging/otus/80211core/chb.c
@@ -0,0 +1,200 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : hb.c                                                  */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains house keeping and timer functions.         */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+
+/* Called by wrapper every 10 msec */
+void zfiHeartBeat(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->tick++;
+
+#if 0
+    /* => every 1.28 seconds */
+    if (wd->cwm.cw_enable && ((wd->tick & 0x7f) == 0x3f))
+    {
+        zfHpCwmUpdate(dev);
+    }
+#endif
+    /* => every 2.56 seconds */
+    if ((wd->tick & 0xff) == 0)
+    {
+        zfAgingDefragList(dev, 1);
+    }
+
+    /* Watch Dog */
+    //zfWatchDog();
+
+    /* LED Control (per 100ms) */
+    if ((wd->tick % 10) == 9)
+    {
+        zfLed100msCtrl(dev);
+#ifdef ZM_ENABLE_BA_RATECTRL
+        if (!wd->modeMDKEnable)
+        {
+            zfiDbgReadTally(dev);
+        }
+#endif
+    }
+
+#ifdef ZM_ENABLE_REWRITE_BEACON_START_ADDRESS
+    if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        if ( zfStaIsConnected(dev) )
+        {
+            zfReWriteBeaconStartAddress(dev);
+        }
+    }
+#endif
+
+    if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        if ( zfStaIsConnected(dev) )
+        {
+            wd->tickIbssReceiveBeacon++;  // add 10ms
+
+            if ( (wd->sta.ibssSiteSurveyStatus == 2) &&
+                 (wd->tickIbssReceiveBeacon == 300) &&
+                 (wd->sta.ibssReceiveBeaconCount < 3) )
+            {
+                zm_debug_msg0("It is happen!!! No error message");
+                zfReSetCurrentFrequency(dev);
+            }
+        }
+    }
+
+    if(wd->sta.ReceivedPacketRateCounter <= 0)
+    {
+        wd->sta.ReceivedPktRatePerSecond = wd->sta.TotalNumberOfReceivePackets;
+	//zm_debug_msg1("Receive Packet Per Second  = ", wd->sta.ReceivedPktRatePerSecond);
+	    if (wd->sta.TotalNumberOfReceivePackets != 0)
+	    {
+	        wd->sta.avgSizeOfReceivePackets = wd->sta.TotalNumberOfReceiveBytes/wd->sta.TotalNumberOfReceivePackets;
+	    }
+	    else
+	    {
+	        wd->sta.avgSizeOfReceivePackets = 640;
+	    }
+        wd->sta.TotalNumberOfReceivePackets = 0;
+        wd->sta.TotalNumberOfReceiveBytes = 0;
+        wd->sta.ReceivedPacketRateCounter = 100; /*for another 1s*/
+    }
+    else
+    {
+        wd->sta.ReceivedPacketRateCounter--;
+    }
+
+	/* => every 1.28 seconds */
+	if((wd->tick & 0x7f) == 0x3f)
+	{
+		if( wd->sta.NonNAPcount > 0)
+		{
+			wd->sta.RTSInAGGMode = TRUE;
+			wd->sta.NonNAPcount = 0;
+		}
+		else
+		{
+			wd->sta.RTSInAGGMode = FALSE;
+		}
+	}
+
+
+
+    /* Maintain management time tick */
+    zfMmApTimeTick(dev);
+    zfMmStaTimeTick(dev);
+
+    //zfPhyCrTuning(dev);
+
+    //zfTxPowerControl(dev);
+    zfHpHeartBeat(dev);
+
+}
+
+
+void zfDumpBssList(zdev_t* dev)
+{
+    struct zsBssInfo* pBssInfo;
+    u8_t   str[33];
+    u8_t   i, j;
+    u32_t  addr1, addr2;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zm_debug_msg0("***** Bss scan result *****");
+    zmw_enter_critical_section(dev);
+
+    pBssInfo = wd->sta.bssList.head;
+
+    for( i=0; i<wd->sta.bssList.bssCount; i++ )
+    {
+        if ( i )
+        {
+            zm_debug_msg0("---------------------------");
+        }
+
+        zm_debug_msg1("BSS #", i);
+        for(j=0; j<pBssInfo->ssid[1]; j++)
+        {
+            str[j] = pBssInfo->ssid[2+j];
+        }
+        str[pBssInfo->ssid[1]] = 0;
+        zm_debug_msg0("SSID = ");
+        zm_debug_msg0(str);
+
+        addr1 = (pBssInfo->bssid[0] << 16) + (pBssInfo->bssid[1] << 8 )
+                + pBssInfo->bssid[2];
+        addr2 = (pBssInfo->bssid[3] << 16) + (pBssInfo->bssid[4] << 8 )
+                + pBssInfo->bssid[5];
+        zm_debug_msg2("Bssid = ", addr1);
+        zm_debug_msg2("        ", addr2);
+        zm_debug_msg1("frequency = ", pBssInfo->frequency);
+        zm_debug_msg1("security type = ", pBssInfo->securityType);
+        zm_debug_msg1("WME = ", pBssInfo->wmeSupport);
+        zm_debug_msg1("beacon interval = ", pBssInfo->beaconInterval[0]
+                      + (pBssInfo->beaconInterval[1] << 8));
+        zm_debug_msg1("capability = ", pBssInfo->capability[0]
+                      + (pBssInfo->capability[1] << 8));
+        if ( pBssInfo->supportedRates[1] > 0 )
+        {
+            for( j=0; j<pBssInfo->supportedRates[1]; j++ )
+            {
+                zm_debug_msg2("supported rates = ", pBssInfo->supportedRates[2+j]);
+            }
+        }
+
+        for( j=0; j<pBssInfo->extSupportedRates[1]; j++ )
+        {
+            zm_debug_msg2("ext supported rates = ", pBssInfo->extSupportedRates[2+j]);
+        }
+
+        pBssInfo = pBssInfo->next;
+    }
+    zmw_leave_critical_section(dev);
+
+    zm_debug_msg0("***************************");
+}
+
diff --git a/drivers/staging/otus/80211core/cic.c b/drivers/staging/otus/80211core/cic.c
new file mode 100644
index 000000000000..c84f079e3d84
--- /dev/null
+++ b/drivers/staging/otus/80211core/cic.c
@@ -0,0 +1,496 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+#include "ratectrl.h"
+
+
+void zfUpdateBssid(zdev_t* dev, u8_t* bssid)
+{
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    //zmw_enter_critical_section(dev);
+    wd->sta.bssid[0] = bssid[0] + (((u16_t) bssid[1]) << 8);
+    wd->sta.bssid[1] = bssid[2] + (((u16_t) bssid[3]) << 8);
+    wd->sta.bssid[2] = bssid[4] + (((u16_t) bssid[5]) << 8);
+    //zmw_leave_critical_section(dev);
+
+    zfHpSetBssid(dev, bssid);
+
+}
+
+/************************************************************************************/
+/*                                                                                  */
+/*    FUNCTION DESCRIPTION                  zfResetSupportRate                      */
+/*      Reset support rate to default value.                                        */
+/*                                                                                  */
+/*    INPUTS                                                                        */
+/*      dev : device pointer                                                        */
+/*      type: ZM_DEFAULT_SUPPORT_RATE_ZERO       => reset to zero                   */
+/*            ZM_DEFAULT_SUPPORT_RATE_DISCONNECT => reset to disconnect status      */
+/*            ZM_DEFAULT_SUPPORT_RATE_IBSS_B     => reset to IBSS creator(b mode)   */
+/*            ZM_DEFAULT_SUPPORT_RATE_IBSS_AG    => reset to IBSS creator(a/g mode) */
+/*                                                                                  */
+/************************************************************************************/
+void zfResetSupportRate(zdev_t* dev, u8_t type)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch(type)
+    {
+    case ZM_DEFAULT_SUPPORT_RATE_ZERO:
+        wd->bRate = 0;
+        wd->bRateBasic = 0;
+        wd->gRate = 0;
+        wd->gRateBasic = 0;
+        break;
+    case ZM_DEFAULT_SUPPORT_RATE_DISCONNECT:
+        wd->bRate = 0xf;
+        wd->bRateBasic = 0xf;
+        wd->gRate = 0xff;
+        wd->gRateBasic = 0x15;
+        break;
+    case ZM_DEFAULT_SUPPORT_RATE_IBSS_B:
+        wd->bRate = 0xf;
+        wd->bRateBasic = 0xf;
+        wd->gRate = 0;
+        wd->gRateBasic = 0;
+        break;
+    case ZM_DEFAULT_SUPPORT_RATE_IBSS_AG:
+        wd->bRate = 0xf;
+        wd->bRateBasic = 0xf;
+        wd->gRate = 0xff;
+        wd->gRateBasic = 0;
+        break;
+    }
+}
+
+void zfUpdateSupportRate(zdev_t* dev, u8_t* rateArray)
+{
+    u8_t bRate=0, bRateBasic=0, gRate=0, gRateBasic=0;
+    u8_t length = rateArray[1];
+    u8_t i, j;
+
+    zmw_get_wlan_dev(dev);
+
+    for(i=2; i<length+2; i++)
+    {
+        for(j=0; j<4; j++)
+        {
+            if ( (rateArray[i] & 0x7f) == zg11bRateTbl[j] )
+            {
+                bRate |= (1 << j);
+                if ( rateArray[i] & 0x80 )
+                {
+                    bRateBasic |= (1 << j);
+                }
+            }
+        }
+
+        if ( j == 4 )
+        {
+            for(j=0; j<8; j++)
+            {
+                if ( (rateArray[i] & 0x7f) == zg11gRateTbl[j] )
+                {
+                    gRate |= (1 << j);
+                    if ( rateArray[i] & 0x80 )
+                    {
+                        gRateBasic |= (1 << j);
+                    }
+                }
+            }
+        }
+    }
+
+
+    wd->bRate |= bRate;
+    wd->bRateBasic |= bRateBasic;
+    wd->gRate |= gRate;
+    wd->gRateBasic |= gRateBasic;
+}
+
+u8_t zfIsGOnlyMode(zdev_t* dev, u16_t  frequency, u8_t* rateArray)
+{
+    u8_t length = rateArray[1];
+    u8_t i, j;
+
+    if (frequency < 3000) {
+        for (i = 2; i < length+2; i++) {
+            for (j = 0; j < 8; j++) {
+                if ( ((rateArray[i] & 0x7f) == zg11gRateTbl[j])
+                     && (rateArray[i] & 0x80) ) {
+                    return 1;
+                }
+            }
+        }
+    }
+
+    return 0;
+}
+
+void zfGatherBMode(zdev_t* dev, u8_t* rateArray, u8_t* extrateArray)
+{
+    u8_t gatherBMode[ZM_MAX_SUPP_RATES_IE_SIZE + 2];
+    u8_t i, j, k = 0;
+    u8_t length;
+
+    gatherBMode[0] = ZM_WLAN_EID_SUPPORT_RATE;
+    gatherBMode[1] = 0;
+
+    length = rateArray[1];
+    for (i = 2; i < length+2; i++) {
+        for (j = 0; j < 4; j++) {
+            if ( (rateArray[i] & 0x7f) == zg11bRateTbl[j] ) {
+                gatherBMode[2+k] = rateArray[i];
+
+                gatherBMode[1]++;
+                k++;
+            }
+        }
+    }
+
+    length = extrateArray[1];
+    for (i = 2; i < length+2; i++) {
+        for (j = 0; j < 4; j++) {
+            if ( (extrateArray[i] & 0x7f) == zg11bRateTbl[j] ) {
+                gatherBMode[2+k] = extrateArray[i];
+
+                gatherBMode[1]++;
+                k++;
+            }
+        }
+    }
+
+    extrateArray[0] = extrateArray[1] = 0;
+    zfMemoryCopy(rateArray, gatherBMode, gatherBMode[1]+2);
+}
+
+u16_t zfGetRandomNumber(zdev_t* dev, u16_t initValue)
+{
+#if 0
+    /* Compiler/Linker error on Linux */
+    if ( initValue )
+    {
+        srand(initValue);
+    }
+
+    return ((u16_t)rand());
+#endif
+    return 0;
+}
+
+u8_t zfPSDeviceSleep(zdev_t* dev)
+{
+    //zmw_get_wlan_dev(dev);
+
+    /* enter PS mode */
+
+    return 0;
+}
+
+u8_t zcOfdmPhyCrtlToRate[] =
+{
+    /* 0x8=48M, 0x9=24M, 0xa=12M, 0xb=6M, 0xc=54M, 0xd=36M, 0xe=18M, 0xf=9M */
+            10,       8,       6,      4,      11,       9,       7,      5
+};
+
+u8_t zfPhyCtrlToRate(u32_t phyCtrl)
+{
+    u32_t mt, mcs, sg;
+    u8_t rate = 0;
+
+    mt = phyCtrl & 0x3;
+    mcs = (phyCtrl>>18) & 0x3f;
+    sg = (phyCtrl>>31) & 0x1;
+
+    if ((mt == 0) && (mcs <=3))
+    {
+        rate = (u8_t)mcs;
+    }
+    else if ((mt == 1) && (mcs >= 0x8) && (mcs <= 0xf))
+    {
+        rate = zcOfdmPhyCrtlToRate[mcs-8];
+    }
+    else if ((mt == 2) && (mcs <= 15))
+    {
+        rate = (u8_t)mcs + 12;
+        if(sg) {
+            if (mcs != 7)
+            {
+                rate = (u8_t)mcs + 12 + 2;
+            }
+            else //MCS7-SG
+            {
+                rate = (u8_t)30;
+            }
+        }
+    }
+
+    return rate;
+}
+
+
+void zfCoreEvent(zdev_t* dev, u16_t event, u8_t* rsp)
+{
+    u16_t i;
+    zbuf_t* psBuf;
+    u8_t moreData;
+    u8_t vap = 0;
+    u8_t peerIdx;
+    s8_t res;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    if (event == 0) //Beacon Event
+    {
+        if ( wd->wlanMode == ZM_MODE_AP )
+        {
+            zfApSendBeacon(dev);
+
+            if (wd->CurrentDtimCount == 0)
+            {
+                /* TODO : Send queued broadcast frames at BC/MC event */
+                do
+                {
+                    psBuf = NULL;
+                    moreData = 0;
+                    zmw_enter_critical_section(dev);
+                    if (wd->ap.bcmcTail[vap] != wd->ap.bcmcHead[vap])
+                    {
+                        //zm_msg0_mm(ZM_LV_0, "Send BCMC frames");
+                        psBuf = wd->ap.bcmcArray[vap][wd->ap.bcmcHead[vap]];
+                        wd->ap.bcmcHead[vap] = (wd->ap.bcmcHead[vap] + 1)
+                                & (ZM_BCMC_ARRAY_SIZE - 1);
+                        if (wd->ap.bcmcTail[vap] != wd->ap.bcmcHead[vap])
+                        {
+                            moreData = 0x20;
+                        }
+                    }
+                    zmw_leave_critical_section(dev);
+                    if (psBuf != NULL)
+                    {
+                        /* TODO : config moreData bit */
+                        zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF,
+                                moreData);
+                    }
+                } while(psBuf != NULL);
+
+            }
+        }
+        else
+        {
+            /* STA mode */
+            if ( wd->sta.powerSaveMode > ZM_STA_PS_NONE )
+            {
+                /* send queued packets */
+                for(i=0; i<wd->sta.staPSDataCount; i++)
+                {
+                    zfTxSendEth(dev, wd->sta.staPSDataQueue[i], 0,
+                                ZM_EXTERNAL_ALLOC_BUF, 0);
+                }
+
+                wd->sta.staPSDataCount = 0;
+            }
+
+            if ( wd->wlanMode == ZM_MODE_IBSS )
+            {
+                zfStaSendBeacon(dev);
+                wd->sta.ibssAtimTimer = ZM_BIT_15 | wd->sta.atimWindow;
+            }
+
+            zfPowerSavingMgrPreTBTTInterrupt(dev);
+        }
+    } //if (event == 0) //Beacon Event
+    else if (event == 1) //Retry completed event
+    {
+        u32_t retryRate;
+
+        retryRate = (u32_t)(rsp[6]) + (((u32_t)(rsp[7]))<<8)
+                + (((u32_t)(rsp[8]))<<16) + (((u32_t)(rsp[9]))<<24);
+        /* Degrade Tx Rate */
+        if (wd->wlanMode == ZM_MODE_AP)
+        {
+            zmw_enter_critical_section(dev);
+            if ((i=zfApFindSta(dev, (u16_t*)rsp)) != 0xffff)
+            {
+                zfRateCtrlTxFailEvent(dev, &wd->ap.staTable[i].rcCell, 0,(u32_t)zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+        }
+        else
+        {
+            zmw_enter_critical_section(dev);
+            res = zfStaFindOppositeByMACAddr(dev, (u16_t*)rsp, &peerIdx);
+            if ( res == 0 )
+            {
+                zfRateCtrlTxFailEvent(dev, &wd->sta.oppositeInfo[peerIdx].rcCell, 0,(u32_t)zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+        }
+    } //else if (event == 1) //Retry completed event
+    else if (event == 2) //Tx Fail event
+    {
+        u32_t retryRate;
+
+        retryRate = (u32_t)(rsp[6]) + (((u32_t)(rsp[7]))<<8)
+                + (((u32_t)(rsp[8]))<<16) + (((u32_t)(rsp[9]))<<24);
+
+        /* Degrade Tx Rate */
+        if (wd->wlanMode == ZM_MODE_AP)
+        {
+            zmw_enter_critical_section(dev);
+            if ((i=zfApFindSta(dev, (u16_t*)rsp)) != 0xffff)
+            {
+                zfRateCtrlTxFailEvent(dev, &wd->ap.staTable[i].rcCell, 0,(u32_t)zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+
+            zfApSendFailure(dev, rsp);
+        }
+        else
+        {
+            zmw_enter_critical_section(dev);
+            res = zfStaFindOppositeByMACAddr(dev, (u16_t*)rsp, &peerIdx);
+            if ( res == 0 )
+            {
+                zfRateCtrlTxFailEvent(dev, &wd->sta.oppositeInfo[peerIdx].rcCell, 0,(u32_t)zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+        }
+    } //else if (event == 2) //Tx Fail event
+    else if (event == 3) //Tx Comp event
+    {
+        u32_t retryRate;
+
+        retryRate = (u32_t)(rsp[6]) + (((u32_t)(rsp[7]))<<8)
+                + (((u32_t)(rsp[8]))<<16) + (((u32_t)(rsp[9]))<<24);
+
+        /* TODO : Tx completed, used for rate control probing */
+        if (wd->wlanMode == ZM_MODE_AP)
+        {
+            zmw_enter_critical_section(dev);
+            if ((i=zfApFindSta(dev, (u16_t*)rsp)) != 0xffff)
+            {
+                zfRateCtrlTxSuccessEvent(dev, &wd->ap.staTable[i].rcCell, zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+        }
+        else
+        {
+            zmw_enter_critical_section(dev);
+            res = zfStaFindOppositeByMACAddr(dev, (u16_t*)rsp, &peerIdx);
+            if ( res == 0 )
+            {
+                zfRateCtrlTxSuccessEvent(dev, &wd->sta.oppositeInfo[peerIdx].rcCell, zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+        }
+    } //else if (event == 3) //Tx Comp event
+    else if (event == 4) //BA failed count
+    {
+        u32_t fail;
+        u32_t rate;
+        peerIdx = 0;
+
+        fail=((u32_t*)rsp)[0] & 0xFFFF;
+        rate=((u32_t*)rsp)[0] >> 16;
+
+        if (rate > 15) {
+            rate = (rate & 0xF) + 12 + 2;
+        }
+        else {
+            rate = rate + 12;
+        }
+
+        zmw_enter_critical_section(dev);
+        zfRateCtrlTxFailEvent(dev, &wd->sta.oppositeInfo[peerIdx].rcCell, (u8_t)rate, fail);
+        zmw_leave_critical_section(dev);
+    }
+}
+
+void zfBeaconCfgInterrupt(zdev_t* dev, u8_t* rsp)
+{
+    u32_t txBeaconCounter;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        txBeaconCounter = *((u32_t *)rsp);
+        if ( wd->sta.beaconTxCnt != txBeaconCounter )
+        {
+            wd->sta.txBeaconInd = 1;
+
+            zmw_enter_critical_section(dev);
+            wd->tickIbssSendBeacon = 0;
+            zmw_leave_critical_section(dev);
+        }
+        else
+        {
+            wd->sta.txBeaconInd = 0;
+        }
+
+#ifdef ZM_ENABLE_IBSS_DELAYED_JOIN_INDICATION
+        if ( wd->sta.txBeaconInd && wd->sta.ibssDelayedInd )
+        {
+            if (wd->zfcbIbssPartnerNotify != NULL)
+            {
+                wd->zfcbIbssPartnerNotify(dev, 1, &wd->sta.ibssDelayedIndEvent);
+            }
+
+            wd->sta.ibssDelayedInd = 0;
+        }
+#endif
+
+        wd->sta.beaconTxCnt = txBeaconCounter;
+
+        // Need to check if the time is expired after ATIM window??
+
+        // Check if we have buffered any data for those stations that are sleeping
+        // If it's true, then transmitting ATIM pkt to notify them
+
+#ifdef ZM_ENABLE_IBSS_PS
+        // TODO: Need to check if the station receive our ATIM pkt???
+        zfStaIbssPSSend(dev);
+
+        if ( wd->sta.atimWindow == 0 )
+        {
+            // We won't receive the end of ATIM isr so we fake it
+            zfPowerSavingMgrAtimWinExpired(dev);
+        }
+#endif
+    }
+}
+
+void zfEndOfAtimWindowInterrupt(zdev_t* dev)
+{
+#ifdef ZM_ENABLE_IBSS_PS
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        // Transmit any queued pkt for the stations!!
+        zfPowerSavingMgrAtimWinExpired(dev);
+    }
+#endif
+}
diff --git a/drivers/staging/otus/80211core/cinit.c b/drivers/staging/otus/80211core/cinit.c
new file mode 100644
index 000000000000..5f853ce79309
--- /dev/null
+++ b/drivers/staging/otus/80211core/cinit.c
@@ -0,0 +1,1911 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : init.c                                                */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains init functions.                            */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "../hal/hpreg.h"
+
+extern const u8_t zcUpToAc[8];
+
+u16_t zcIndextoRateBG[16] = {1000, 2000, 5500, 11000, 0, 0, 0, 0, 48000,
+                               24000, 12000, 6000, 54000, 36000, 18000, 9000};
+u32_t zcIndextoRateN20L[16] = {6500, 13000, 19500, 26000, 39000, 52000, 58500,
+                              65000, 13000, 26000, 39000, 52000, 78000, 104000,
+                              117000, 130000};
+u32_t zcIndextoRateN20S[16] = {7200, 14400, 21700, 28900, 43300, 57800, 65000,
+                              72200, 14400, 28900, 43300, 57800, 86700, 115600,
+                              130000, 144400};
+u32_t zcIndextoRateN40L[16] = {13500, 27000, 40500, 54000, 81000, 108000, 121500,
+                              135000, 27000, 54000, 81000, 108000, 162000, 216000,
+                              243000, 270000};
+u32_t zcIndextoRateN40S[16] = {15000, 30000, 45000, 60000, 90000, 120000, 135000,
+                              150000, 30000, 60000, 90000, 120000, 180000, 240000,
+                              270000, 300000};
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfTxGenWlanHeader           */
+/*      Generate WLAN MAC header and LLC header.                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      id : Index of TxD                                               */
+/*      port : WLAN port                                                */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      length of removed Ethernet header                               */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.5      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfTxGenWlanHeader(zdev_t* dev, zbuf_t* buf, u16_t* header, u16_t seq,
+                        u8_t flag, u16_t plusLen, u16_t minusLen, u16_t port,
+                        u16_t* da, u16_t* sa, u8_t up, u16_t *micLen,
+                        u16_t* snap, u16_t snapLen, struct aggControl *aggControl)
+{
+
+    u16_t len;
+    u16_t macCtrl;
+    u32_t phyCtrl;
+    u16_t hlen = 16;
+    u16_t icvLen = 0;
+    u16_t wdsPortId;
+    u16_t vap = 0;
+    u16_t mcs = 0;
+    u16_t mt = 0;
+    u8_t  qosType;
+    u8_t  b1, b2;
+    u16_t wdsPort;
+    u8_t  encExemptionActionType;
+    u16_t rateProbingFlag = 0;
+    u8_t  tkipFrameOffset = 0;
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    u8_t    res, peerIdx;
+    u8_t    userIdx=0;
+    u16_t   *iv16;
+    u32_t   *iv32;
+#endif
+
+    zmw_get_wlan_dev(dev);
+
+   /* Generate WLAN header */
+    /* Frame control */
+    header[4] = 0x0008 | (flag<<8);
+    /* Duration */
+    header[5] = 0x0000;
+
+    if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        /* ToDS bit */
+        header[4] |= 0x0100;
+
+        /*Sometimes we wake up to tx/rx but AP still think we are sleeping, so still need to set this bit*/
+        if ( zfPowerSavingMgrIsSleeping(dev) || wd->sta.psMgr.tempWakeUp == 1 )
+        {
+            header[4] |= 0x1000;
+        }
+
+        /* Address 1 = BSSID */
+        header[6] = wd->sta.bssid[0];
+        header[7] = wd->sta.bssid[1];
+        header[8] = wd->sta.bssid[2];
+        /* Address 3 = DA */
+        header[12] = da[0];
+        header[13] = da[1];
+        header[14] = da[2];
+    }
+    else if (wd->wlanMode == ZM_MODE_PSEUDO)
+    {
+        /* Address 1 = DA */
+        header[6] = da[0];
+        header[7] = da[1];
+        header[8] = da[2];
+        /* Address 3 = 00:00:00:00:00:00 */
+        header[12] = 0;
+        header[13] = 0;
+        header[14] = 0;
+
+        /* PSEUDO test : WDS */
+        if (wd->enableWDS)
+        {
+            /* ToDS and FromDS bit */
+            header[4] |= 0x0300;
+
+            /* Address 4 = SA */
+            header[16] = 0;
+            header[17] = 0;
+            header[18] = 0;
+
+            hlen = 19;
+        }
+    }
+    else if (wd->wlanMode == ZM_MODE_IBSS)
+    {
+        /* Address 1 = DA */
+        header[6] = da[0];
+        header[7] = da[1];
+        header[8] = da[2];
+        /* Address 3 = BSSID */
+        header[12] = wd->sta.bssid[0];
+        header[13] = wd->sta.bssid[1];
+        header[14] = wd->sta.bssid[2];
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+        zmw_enter_critical_section(dev);
+        res = zfStaFindOppositeByMACAddr(dev, da, &peerIdx);
+        if(res == 0) // Find opposite in our OppositeInfo Structure !
+        {
+            userIdx = peerIdx;
+        }
+        zmw_leave_critical_section(dev);
+#endif
+    }
+    else if (wd->wlanMode == ZM_MODE_AP)
+    {
+        if (port < 0x20)
+        /* AP mode */
+        {
+            /* FromDS bit */
+            header[4] |= 0x0200;
+
+            /* Address 1 = DA */
+            header[6] = da[0];
+            header[7] = da[1];
+            header[8] = da[2];
+            /* Address 3 = SA */
+            header[12] = sa[0];
+            header[13] = sa[1];
+            header[14] = sa[2];
+
+            if (port < ZM_MAX_AP_SUPPORT)
+            {
+                vap = port;
+                header[14] += (vap<<8);
+            }
+        }
+        else
+        /* WDS port */
+        {
+            /* ToDS and FromDS bit */
+            header[4] |= 0x0300;
+
+            wdsPortId = port - 0x20;
+
+            /* Address 1 = RA */
+            header[6] = wd->ap.wds.macAddr[wdsPortId][0];
+            header[7] = wd->ap.wds.macAddr[wdsPortId][1];
+            header[8] = wd->ap.wds.macAddr[wdsPortId][2];
+            /* Address 3 = DA */
+            header[12] = da[0];
+            header[13] = da[1];
+            header[14] = da[2];
+            /* Address 4 = SA */
+            header[16] = sa[0];
+            header[17] = sa[1];
+            header[18] = sa[2];
+
+            hlen = 19;
+        }
+    } /* else if (wd->wlanMode == ZM_MODE_AP) */
+
+    /* Address 2 = TA */
+    header[9] = wd->macAddr[0];
+    header[10] = wd->macAddr[1];
+#ifdef ZM_VAPMODE_MULTILE_SSID
+    header[11] = wd->macAddr[2]; //Multiple SSID
+#else
+    header[11] = wd->macAddr[2] + (vap<<8); //VAP
+#endif
+
+    if ( (wd->wlanMode == ZM_MODE_IBSS) && (wd->XLinkMode) )
+    {
+        header[9]  = sa[0];
+        header[10] = sa[1];
+        header[11] = sa[2];
+    }
+
+    /* Sequence Control */
+    header[15] = seq;
+
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        zfApGetStaTxRateAndQosType(dev, da, &phyCtrl, &qosType, &rateProbingFlag);
+        mt = (u16_t)(phyCtrl & 0x3);
+        mcs = (u16_t)((phyCtrl >> 16) & 0x3f);
+#if 1
+        //zfApGetStaQosType(dev, da, &qosType);
+
+        /* if DA == WME STA */
+        if (qosType == 1)
+        {
+            /* QoS data */
+            header[4] |= 0x0080;
+
+            /* QoS Control */
+            header[hlen] = up;
+            hlen += 1;
+        }
+#endif
+    }
+
+#if 0
+    //AGG Test Code
+    if (header[6] == 0x8000)
+    {
+        /* QoS data */
+        header[4] |= 0x0080;
+
+        /* QoS Control */
+        header[hlen] = 0;
+        hlen += 1;
+    }
+#endif
+
+    if (wd->wlanMode == ZM_MODE_AP) {
+        /* Todo: rate control here for qos field */
+    }
+    else {
+        /* Rate control */
+        zfStaGetTxRate(dev, da, &phyCtrl, &rateProbingFlag);
+        mt = (u16_t)(phyCtrl & 0x3);
+        mcs = (u16_t)((phyCtrl >> 16) & 0x3f);
+    }
+
+    if (wd->txMCS != 0xff)
+    {
+        /* fixed rate */
+	    phyCtrl = ((u32_t)wd->txMCS<<16) + wd->txMT;
+        mcs = wd->txMCS;
+        mt = wd->txMT;
+    }
+
+    if (wd->enableAggregation)
+    {
+        /* force enable aggregation */
+        if (wd->enableAggregation==2 && !(header[6]&0x1))
+        {
+            /* QoS data */
+            header[4] |= 0x0080;
+
+            /* QoS Control */
+            header[hlen] = 0;
+            hlen += 1;
+        }
+        /* if wd->enableAggregation=1 => force disable */
+        /* if wd->enableAggregation=0 => auto */
+    }
+
+#ifdef ZM_ENABLE_AGGREGATION
+    /*
+     * aggregation control
+     */
+
+    /*
+     * QoS data
+     */
+    if (wd->wlanMode == ZM_MODE_AP) {
+        if (aggControl && mt == 2) {
+            if (wd->enableAggregation==0 && !(header[6]&0x1))
+            {
+                header[4] |= 0x0080;
+
+                /*
+                 * QoS Control
+                 */
+                header[hlen] = 0;
+                hlen += 1;
+            }
+        }
+    }
+#endif
+
+    // MSDU Length
+    len = zfwBufGetSize(dev, buf);
+
+    /* Generate control setting */
+    /* Backoff, Non-Burst and hardware duration */
+    macCtrl = 0x208;
+
+    /* ACK */
+    if ((header[6] & 0x1) == 0x1)
+    {
+        /* multicast frame : Set NO-ACK bit */
+        macCtrl |= 0x4;
+    }
+    else
+    {
+        /* unicast frame */
+    #if 0
+        // Enable RTS according to MPDU Lengths ( not MSDU Lengths )
+        if (len >= wd->rtsThreshold)
+        {
+            /* Enable RTS */
+            macCtrl |= 1;
+        }
+    #endif
+    }
+    /* VAP test code */
+    //macCtrl |= 0x4;
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        u8_t encryType;
+        u16_t iv16;
+        u32_t iv32;
+
+        /* Check whether this is a multicast frame */
+        if ((header[6] & 0x1) == 0x1)
+        {
+            /* multicast frame */
+            if (wd->ap.encryMode[vap] == ZM_TKIP)
+            {
+                wd->ap.iv16[vap]++;
+
+                if(wd->ap.iv16[vap] == 0)
+                {
+                    wd->ap.iv32[vap]++;
+                }
+
+                b1 = (u8_t) (wd->ap.iv16[vap] >> 8);
+                b2 = (b1 | 0x20) & 0x7f;
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                b1 = (u8_t) wd->ap.iv16[vap];
+                b2 = 0x20 | (wd->ap.bcKeyIndex[vap] << 6);
+                header[hlen+1] = ((u16_t)b2 << 8) + b1;
+                header[hlen+2] = (u16_t) wd->ap.iv32[vap];
+                header[hlen+3] = (u16_t) (wd->ap.iv32[vap] >> 16);
+
+                //macCtrl |= 0x80;
+                macCtrl |= 0x40;
+                icvLen = 4;
+
+                /* set hardware MIC */
+                if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
+                {
+                    macCtrl |= 0x100;
+                    plusLen += 8;
+                    *micLen = 8;
+                }
+
+                header[4] |= 0x4000;
+                hlen += 4;
+            }
+            else if (wd->ap.encryMode[vap] == ZM_AES)
+            {
+                wd->ap.iv16[vap]++;
+
+                if(wd->ap.iv16[vap] == 0)
+                {
+                    wd->ap.iv32[vap]++;
+                }
+
+                b1 = (u8_t) wd->ap.iv16[vap];
+                b2 = (u8_t) (wd->ap.iv16[vap] >> 8);
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                header[hlen+1] = 0x2000 | (wd->ap.bcKeyIndex[vap] << 14);
+                header[hlen+2] = (u16_t) (wd->ap.iv32[vap]);
+                header[hlen+3] = (u16_t) (wd->ap.iv32[vap] >> 16);
+
+                macCtrl |= 0xc0;
+                icvLen = 8;  /* MIC */
+
+                header[4] |= 0x4000;
+                hlen += 4;
+            }
+            #ifdef ZM_ENABLE_CENC
+            else if (wd->ap.encryMode[vap] == ZM_CENC)
+            {
+                //u32_t txiv[4];
+
+                wd->ap.txiv[vap][0]++;
+
+                if (wd->ap.txiv[vap][0] == 0)
+                {
+                    wd->ap.txiv[vap][1]++;
+                }
+
+                if (wd->ap.txiv[vap][1] == 0)
+                {
+                    wd->ap.txiv[vap][2]++;
+                }
+
+                if (wd->ap.txiv[vap][2] == 0)
+                {
+                    wd->ap.txiv[vap][3]++;
+                }
+
+                if (wd->ap.txiv[vap][3] == 0)
+                {
+                    wd->ap.txiv[vap][0] = 0;
+                    wd->ap.txiv[vap][1] = 0;
+                    wd->ap.txiv[vap][2] = 0;
+                }
+
+                header[hlen] = (wd->ap.bcKeyIndex[vap] & 0x0001);    /* For Key Id and reserved field */
+                header[hlen+1] = (u16_t)wd->ap.txiv[vap][0];
+                header[hlen+2] = (u16_t)(wd->ap.txiv[vap][0] >> 16);
+                header[hlen+3] = (u16_t)wd->ap.txiv[vap][1];
+                header[hlen+4] = (u16_t)(wd->ap.txiv[vap][1] >> 16);
+                header[hlen+5] = (u16_t)wd->ap.txiv[vap][2];
+                header[hlen+6] = (u16_t)(wd->ap.txiv[vap][2] >> 16);
+                header[hlen+7] = (u16_t)wd->ap.txiv[vap][3];
+                header[hlen+8] = (u16_t)(wd->ap.txiv[vap][3] >> 16);
+
+                macCtrl |= 0x80;
+                icvLen = 16;  /* MIC */
+
+                header[4] |= 0x4000;
+                hlen += 9;
+            }
+            #endif //ZM_ENABLE_CENC
+        }
+        else
+        {
+            /* Get STA's encryption type */
+            zfApGetStaEncryType(dev, da, &encryType);
+
+            if (encryType == ZM_TKIP)
+            {
+                /* Get iv16 and iv32 */
+                zfApGetStaWpaIv(dev, da, &iv16, &iv32);
+
+                iv16++;
+                if (iv16 == 0)
+                {
+                    iv32++;
+                }
+
+                b1 = (u8_t) (iv16 >> 8);
+                b2 = (b1 | 0x20) & 0x7f;
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                b1 = (u8_t) iv16;
+                b2 = 0x20;
+                header[hlen+1] = ((u16_t)b2 << 8) + b1;
+                header[hlen+2] = (u16_t) iv32;
+                header[hlen+3] = (u16_t) (iv32 >> 16);
+
+                //macCtrl |= 0x80;
+                macCtrl |= 0x40;
+                icvLen = 4;
+
+                /* set hardware MIC */
+                if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
+                {
+                    macCtrl |= 0x100;
+                    plusLen += 8;
+                    *micLen = 8;
+                }
+
+                header[4] |= 0x4000;
+                hlen += 4;
+
+                /* Set iv16 and iv32 */
+                zfApSetStaWpaIv(dev, da, iv16, iv32);
+            }
+            else if (encryType == ZM_AES)
+            {
+                /* Get iv16 and iv32 */
+                zfApGetStaWpaIv(dev, da, &iv16, &iv32);
+
+                iv16++;
+                if (iv16 == 0)
+                {
+                    iv32++;
+                }
+
+                b1 = (u8_t) iv16;
+                b2 = (u8_t) (iv16 >> 8);
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                header[hlen+1] = 0x2000;
+                header[hlen+2] = (u16_t) (iv32);
+                header[hlen+3] = (u16_t) (iv32 >> 16);
+
+                macCtrl |= 0xc0;
+                icvLen = 8;  /* MIC */
+
+                header[4] |= 0x4000;
+                hlen += 4;
+
+                /* Set iv16 and iv32 */
+                zfApSetStaWpaIv(dev, da, iv16, iv32);
+            }
+            #ifdef ZM_ENABLE_CENC
+            else if (encryType == ZM_CENC)
+            {
+                u32_t txiv[4];
+                u8_t keyIdx;
+
+                /* Get CENC TxIV */
+                zfApGetStaCencIvAndKeyIdx(dev, da, txiv, &keyIdx);
+
+                txiv[0] += 2;
+
+                if (txiv[0] == 0 || txiv[0] == 1)
+                {
+                    txiv[1]++;
+                }
+
+                if (txiv[1] == 0)
+                {
+                    txiv[2]++;
+                }
+
+                if (txiv[2] == 0)
+                {
+                    txiv[3]++;
+                }
+
+                if (txiv[3] == 0)
+                {
+                    txiv[0] = 0;
+                    txiv[1] = 0;
+                    txiv[2] = 0;
+                }
+
+                header[hlen] = (keyIdx & 0x0001);    /* For Key Id and reserved field */
+                header[hlen+1] = (u16_t)txiv[0];
+                header[hlen+2] = (u16_t)(txiv[0] >> 16);
+                header[hlen+3] = (u16_t)txiv[1];
+                header[hlen+4] = (u16_t)(txiv[1] >> 16);
+                header[hlen+5] = (u16_t)txiv[2];
+                header[hlen+6] = (u16_t)(txiv[2] >> 16);
+                header[hlen+7] = (u16_t)txiv[3];
+                header[hlen+8] = (u16_t)(txiv[3] >> 16);
+
+                macCtrl |= 0x80;
+                icvLen = 16;  /* MIC */
+
+                header[4] |= 0x4000;
+                hlen += 9;
+
+                /* Set CENC IV */
+                zfApSetStaCencIv(dev, da, txiv);
+            }
+            #endif //ZM_ENABLE_CENC
+        }
+
+        /* protection mode */
+        if (wd->ap.protectionMode == 1)
+        {
+            /* Enable Self-CTS */
+            macCtrl &= 0xFFFC;
+            macCtrl |= 2;
+        }
+
+        /* Rate Control */
+        if (port < 0x20)
+        {
+            /* AP */
+            /* IV */
+            if ((wd->ap.encryMode[vap] == ZM_WEP64) ||
+                    (wd->ap.encryMode[vap] == ZM_WEP128) ||
+                    (wd->ap.encryMode[vap] == ZM_WEP256))
+            {
+                header[4] |= 0x4000;
+                header[hlen] = 0x0;   //IV
+                header[hlen+1] = wd->ap.bcKeyIndex[vap] << 14; //IV with Keyid--CWYang(m)
+                hlen += 2;
+                icvLen = 4;
+                macCtrl |= 0x40;
+            }
+        }
+        else
+        {
+            /* WDS */
+
+            /* TODO : Fixed rate to 54M */
+            phyCtrl = 0xc0001;   //PHY control L
+
+            /* WDS port checking */
+            if ((wdsPort = (port - 0x20)) >= ZM_MAX_WDS_SUPPORT)
+            {
+                wdsPort = 0;
+            }
+
+            #if 1
+            /* IV */
+            switch (wd->ap.wds.encryMode[wdsPort])
+            {
+            case ZM_WEP64:
+            case ZM_WEP128:
+            case ZM_WEP256:
+                    header[4] |= 0x4000;
+                    header[hlen] = 0x0;   //IV
+                    header[hlen+1] = wd->ap.bcKeyIndex[vap] << 14; //IV with Keyid
+                    hlen += 2;
+                    icvLen = 4;
+                    macCtrl |= 0x40;
+                    break;
+
+            case ZM_TKIP:
+                    wd->sta.iv16++;
+
+                    if ( wd->sta.iv16 == 0 )
+                    {
+                        wd->sta.iv32++;
+                    }
+
+                    b1 = (u8_t) (wd->sta.iv16 >> 8);
+                    b2 = (b1 | 0x20) & 0x7f;
+                    header[hlen] = ((u16_t)b2 << 8) + b1;
+                    b1 = (u8_t) wd->sta.iv16;
+                    b2 = 0x20;
+                    header[hlen+1] = ((u16_t)b2 << 8) + b1;
+                    header[hlen+2] = (u16_t) wd->sta.iv32;
+                    header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+
+                    //macCtrl |= 0x80;
+                    macCtrl |= 0x40;
+                    icvLen = 4;
+
+                    /* set hardware MIC */
+                    if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
+                    {
+                        macCtrl |= 0x100;
+                        plusLen += 8;
+                        *micLen = 8;
+                    }
+
+                    header[4] |= 0x4000;
+                    hlen += 4;
+                    break;
+
+            case ZM_AES:
+                    wd->sta.iv16++;
+                    if ( wd->sta.iv16 == 0 )
+                    {
+                        wd->sta.iv32++;
+                    }
+
+                    b1 = (u8_t) wd->sta.iv16;
+                    b2 = (u8_t) (wd->sta.iv16 >> 8);
+                    header[hlen] = ((u16_t)b2 << 8) + b1;
+                    header[hlen+1] = 0x2000;
+                    header[hlen+2] = (u16_t) (wd->sta.iv32);
+                    header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+
+                    macCtrl |= 0xc0; /* Set to AES in control setting */
+                    icvLen = 8;  /* MIC */
+
+                    header[4] |= 0x4000; /* Set WEP bit in wlan header */
+                    hlen += 4; /* plus IV length */
+                    break;
+            }/* end of switch */
+            #endif
+        }
+    }
+    else   /* wd->wlanMode != ZM_MODE_AP */
+    {
+        encExemptionActionType = zfwGetPktEncExemptionActionType(dev, buf);
+
+        if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+        {
+            #if 1
+            /* if WME AP */
+            if (wd->sta.wmeConnected != 0)
+            {
+                /* QoS data */
+                header[4] |= 0x0080;
+
+                /* QoS Control */
+                header[hlen] = up;
+                hlen += 1;
+            }
+            #endif
+
+            if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
+            {
+                if ( wd->sta.authMode < ZM_AUTH_MODE_WPA )
+                {   /* non-WPA */
+                    if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED )
+                    {
+                        if ( (wd->sta.encryMode == ZM_WEP64)||
+                             (wd->sta.encryMode == ZM_WEP128)||
+                             (wd->sta.encryMode == ZM_WEP256) )
+                        {
+                            header[4] |= 0x4000;
+                            header[hlen] = 0x0;   //IV
+                            header[hlen+1] = 0x0; //IV
+                            header[hlen+1] |= (((u16_t) wd->sta.keyId) << 14);
+                            hlen += 2;
+                            icvLen = 4;
+
+                            /* For Software WEP */
+                            if ((wd->sta.SWEncryptEnable & ZM_SW_WEP_ENCRY_EN) != 0)
+                            {
+                                u8_t keyLen = 5;
+                                u8_t iv[3];
+
+                                iv[0] = 0x0;
+                                iv[1] = 0x0;
+                                iv[2] = 0x0;
+
+                                if (wd->sta.SWEncryMode[wd->sta.keyId] == ZM_WEP64)
+                                {
+                                    keyLen = 5;
+                                }
+                                else if (wd->sta.SWEncryMode[wd->sta.keyId] == ZM_WEP128)
+                                {
+                                    keyLen = 13;
+                                }
+                                else if (wd->sta.SWEncryMode[wd->sta.keyId] == ZM_WEP256)
+                                {
+                                    keyLen = 29;
+                                }
+
+                                zfWEPEncrypt(dev, buf, (u8_t*) snap, snapLen, minusLen, keyLen,
+                                        wd->sta.wepKey[wd->sta.keyId], iv);
+                            }
+                            else
+                            {
+                                macCtrl |= 0x40;
+                            }
+                        }
+                    }
+                }
+                else
+                {   /* WPA */
+                    if ( wd->sta.wpaState >= ZM_STA_WPA_STATE_PK_OK )
+                    {
+                        wd->sta.iv16++;
+                        if ( wd->sta.iv16 == 0 )
+                        {
+                            wd->sta.iv32++;
+                        }
+
+                        /* set encryption mode */
+                        if ( wd->sta.encryMode == ZM_TKIP )
+                        {
+                            b1 = (u8_t) (wd->sta.iv16 >> 8);
+                            b2 = (b1 | 0x20) & 0x7f;
+                            header[hlen] = ((u16_t)b2 << 8) + b1;
+                            b1 = (u8_t) wd->sta.iv16;
+                            b2 = 0x20;
+
+                            // header[hlen+1] = (((u16_t) wd->sta.keyId) << 14) | (((u16_t)b2 << 8) + b1);
+                            // STA in infrastructure mode should use keyId = 0 to transmit unicast !
+                            header[hlen+1] = (((u16_t)b2 << 8) + b1);
+                            header[hlen+2] = (u16_t) wd->sta.iv32;
+                            header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+
+                            /* If software encryption enable */
+                            if ((wd->sta.SWEncryptEnable & ZM_SW_TKIP_ENCRY_EN) == 0)
+                            {
+                                //macCtrl |= 0x80;
+                                /* TKIP same to WEP */
+                                macCtrl |= 0x40;
+                                icvLen = 4;
+
+                                /* set hardware MIC */
+                                if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
+                                {
+                                    macCtrl |= 0x100;
+                                    plusLen += 8;
+                                    *micLen = 8;
+                                }
+                            }
+                            else
+                            {
+                                u8_t mic[8];
+                                u16_t offset;
+                                u32_t icv;
+                                u8_t RC4Key[16];
+
+                                /* TODO: Remove the criticial section here. */
+                                zmw_declare_for_critical_section();
+
+                                zmw_enter_critical_section(dev);
+                                /* Calculate MIC */
+                                zfCalTxMic(dev, buf, (u8_t *)snap, snapLen, minusLen, da, sa, up, mic);
+
+                                offset = zfwBufGetSize(dev, buf);
+
+                                /* Append MIC to the buffer */
+                                zfCopyToIntTxBuffer(dev, buf, mic, offset, 8);
+                                zfwBufSetSize(dev, buf, offset+8);
+                                zmw_leave_critical_section(dev);
+
+                                /* TKIP Key Mixing */
+                                zfTkipPhase1KeyMix(wd->sta.iv32, &wd->sta.txSeed);
+                                zfTkipPhase2KeyMix(wd->sta.iv16, &wd->sta.txSeed);
+                                zfTkipGetseeds(wd->sta.iv16, RC4Key, &wd->sta.txSeed);
+
+                                /* Encrypt Data */
+                                zfTKIPEncrypt(dev, buf, (u8_t *)snap, snapLen, minusLen, 16, RC4Key, &icv);
+
+                                icvLen = 4;
+                                len += 8;
+                            }
+
+                            header[4] |= 0x4000;
+                            hlen += 4;
+                        }
+                        else if ( wd->sta.encryMode == ZM_AES )
+                        {
+                            b1 = (u8_t) wd->sta.iv16;
+                            b2 = (u8_t) (wd->sta.iv16 >> 8);
+                            header[hlen] = ((u16_t)b2 << 8) + b1;
+                            // header[hlen+1] = (((u16_t) wd->sta.keyId) << 14) | (0x2000);
+                            // STA in infrastructure mode should use keyId = 0 to transmit unicast !
+                            header[hlen+1] = 0x2000;
+                            header[hlen+2] = (u16_t) (wd->sta.iv32);
+                            header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+
+                            macCtrl |= 0xc0;
+                            icvLen = 8;  /* MIC */
+
+                            header[4] |= 0x4000;
+                            hlen += 4;
+                        }
+                        #ifdef ZM_ENABLE_CENC
+                        else if ( wd->sta.encryMode == ZM_CENC )
+                        {
+                            /* Accumlate the PN sequence */
+                            wd->sta.txiv[0] += 2;
+
+                            if (wd->sta.txiv[0] == 0 || wd->sta.txiv[0] == 1)
+                            {
+                                wd->sta.txiv[1]++;
+                            }
+
+                            if (wd->sta.txiv[1] == 0)
+                            {
+                                wd->sta.txiv[2]++;
+                            }
+
+                            if (wd->sta.txiv[2] == 0)
+                            {
+                                wd->sta.txiv[3]++;
+                            }
+
+                            if (wd->sta.txiv[3] == 0)
+                            {
+                                wd->sta.txiv[0] = 0;
+                                wd->sta.txiv[1] = 0;
+                                wd->sta.txiv[2] = 0;
+                            }
+
+                            header[hlen] = (wd->sta.cencKeyId & 0x0001);    /* For Key Id and reserved field */
+                            header[hlen+1] = (u16_t) wd->sta.txiv[0];
+                            header[hlen+2] = (u16_t) (wd->sta.txiv[0] >> 16);
+                            header[hlen+3] = (u16_t) wd->sta.txiv[1];
+                            header[hlen+4] = (u16_t) (wd->sta.txiv[1] >> 16);
+                            header[hlen+5] = (u16_t) wd->sta.txiv[2];
+                            header[hlen+6] = (u16_t) (wd->sta.txiv[2] >> 16);
+                            header[hlen+7] = (u16_t) wd->sta.txiv[3];
+                            header[hlen+8] = (u16_t) (wd->sta.txiv[3] >> 16);
+
+                            macCtrl |= 0x80;
+                            icvLen = 16;  /* MIC */
+
+                            header[4] |= 0x4000;
+                            hlen += 9;
+                        }
+                        #endif //ZM_ENABLE_CENC
+                    }
+                }
+            } // if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
+        } /* if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE ) */
+
+        if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
+            {
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+                if( wd->sta.oppositeInfo[userIdx].wpaState >= ZM_STA_WPA_STATE_PK_OK || wd->sta.wpaState >= ZM_STA_WPA_STATE_PK_OK)
+                {
+                    int isUnicast = 1 ;
+
+                    if((da[0]& 0x1))
+                    {
+                        isUnicast = 0 ; // Not unicast , is broadcast
+                    }
+
+                    if( wd->sta.ibssWpa2Psk == 1 )
+                    { /* The IV order is not the same between unicast and broadcast ! */
+                        if ( isUnicast )
+                        {
+                            iv16 = &wd->sta.oppositeInfo[userIdx].iv16;
+                            iv32 = &wd->sta.oppositeInfo[userIdx].iv32;
+                        }
+                        else
+                        {
+                            iv16 = &wd->sta.iv16;
+                            iv32 = &wd->sta.iv32;
+                        }
+                    }
+                    else
+                    {
+                        iv16 = &wd->sta.iv16;
+                        iv32 = &wd->sta.iv32;
+                    }
+
+                    (*iv16)++;
+                    if ( *iv16 == 0 )
+                    {
+                        *iv32++;
+                    }
+
+                    if ( wd->sta.oppositeInfo[userIdx].encryMode == ZM_AES || wd->sta.encryMode == ZM_AES)
+                    {
+                        //printk("Station encryption mode is AES-CCMP\n") ;
+                        b1 = (u8_t) (*iv16);
+                        b2 = (u8_t) ((*iv16) >> 8);
+                        header[hlen] = ((u16_t)b2 << 8) + b1;
+
+                        if ( isUnicast )
+                        {
+                            header[hlen+1] = 0x2000;
+                        }
+                        else
+                        {
+                            header[hlen+1] = 0x2000 | (((u16_t) wd->sta.keyId) << 14);
+                        }
+
+                        header[hlen+2] = (u16_t) (*iv32);
+                        header[hlen+3] = (u16_t) ((*iv32) >> 16);
+                        macCtrl |= 0xc0;
+                        icvLen = 8;  /* MIC */
+                    }
+
+                    header[4] |= 0x4000;
+                    hlen += 4;
+                }
+                else if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED)
+                {
+                    if ( (wd->sta.encryMode == ZM_WEP64)||
+                        (wd->sta.encryMode == ZM_WEP128)||
+                        (wd->sta.encryMode == ZM_WEP256) )
+                    {
+                        header[4] |= 0x4000;
+                        header[hlen] = 0x0;   //IV
+                        header[hlen+1] = 0x0; //IV
+                        header[hlen+1] |= (((u16_t) wd->sta.keyId) << 14);
+                        hlen += 2;
+                        icvLen = 4;
+                        macCtrl |= 0x40;
+                    }
+                }
+#else
+                /* ----- 20070405 add by Mxzeng ----- */
+                if( wd->sta.wpaState >= ZM_STA_WPA_STATE_PK_OK )
+                {
+                    int isUnicast = 1 ;
+
+                    if((da[0]& 0x1))
+                    {
+                        isUnicast = 0 ; // Not unicast , is broadcast
+                    }
+
+                    wd->sta.iv16++;
+                    if ( wd->sta.iv16 == 0 )
+                    {
+                        wd->sta.iv32++;
+                    }
+
+                    if ( wd->sta.encryMode == ZM_AES )
+                    {
+                        //printk("Station encryption mode is AES-CCMP\n") ;
+                        b1 = (u8_t) wd->sta.iv16;
+                        b2 = (u8_t) (wd->sta.iv16 >> 8);
+                        header[hlen] = ((u16_t)b2 << 8) + b1;
+
+                        if ( isUnicast )
+                        {
+                            header[hlen+1] = 0x2000;
+                        }
+                        else
+                        {
+                            header[hlen+1] = 0x2000 | (((u16_t) wd->sta.keyId) << 14);
+                        }
+
+                            header[hlen+2] = (u16_t) (wd->sta.iv32);
+                            header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+                            macCtrl |= 0xc0;
+                            icvLen = 8;  /* MIC */
+                    }
+
+                    header[4] |= 0x4000;
+                    hlen += 4;
+                }
+                else if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED)
+                {
+                    if ( (wd->sta.encryMode == ZM_WEP64)||
+                         (wd->sta.encryMode == ZM_WEP128)||
+                         (wd->sta.encryMode == ZM_WEP256) )
+                    {
+                        header[4] |= 0x4000;
+                        header[hlen] = 0x0;   //IV
+                        header[hlen+1] = 0x0; //IV
+                        header[hlen+1] |= (((u16_t) wd->sta.keyId) << 14);
+                        hlen += 2;
+                        icvLen = 4;
+                        macCtrl |= 0x40;
+                    }
+                }
+#endif
+            }   // End if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
+        }   // End if ( wd->wlanMode == ZM_MODE_IBSS )
+        else if ( wd->wlanMode == ZM_MODE_PSEUDO )
+        {
+            switch (wd->sta.encryMode)
+	        {
+            case ZM_WEP64:
+            case ZM_WEP128:
+            case ZM_WEP256:
+                header[4] |= 0x4000;
+                header[hlen] = 0x0;   //IV
+                header[hlen+1] = 0x0; //IV
+                hlen += 2;
+                icvLen = 4;
+                macCtrl |= 0x40;
+                break;
+
+            case ZM_TKIP:
+            {
+                wd->sta.iv16++;
+                if ( wd->sta.iv16 == 0 )
+                {
+                    wd->sta.iv32++;
+                }
+
+                b1 = (u8_t) (wd->sta.iv16 >> 8);
+                b2 = (b1 | 0x20) & 0x7f;
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                b1 = (u8_t) wd->sta.iv16;
+                b2 = 0x20;
+                header[hlen+1] = ((u16_t)b2 << 8) + b1;
+                header[hlen+2] = (u16_t) wd->sta.iv32;
+                header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+
+                //macCtrl |= 0x80;
+                macCtrl |= 0x40;
+                icvLen = 4;
+
+                /* set hardware MIC */
+                if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
+                {
+                    macCtrl |= 0x100;
+                    plusLen += 8;
+                    *micLen = 8;
+                }
+
+                header[4] |= 0x4000;
+                hlen += 4;
+            }/* end of PSEUDO TKIP */
+                break;
+
+            case ZM_AES:
+            {
+                wd->sta.iv16++;
+                if ( wd->sta.iv16 == 0 )
+                {
+                    wd->sta.iv32++;
+                }
+
+                b1 = (u8_t) wd->sta.iv16;
+                b2 = (u8_t) (wd->sta.iv16 >> 8);
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                header[hlen+1] = 0x2000;
+                header[hlen+2] = (u16_t) (wd->sta.iv32);
+                header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+                macCtrl |= 0xc0;
+                icvLen = 8;  /* MIC */
+                header[4] |= 0x4000;
+                hlen += 4;
+            }/* end of PSEUDO AES */
+                    break;
+
+              #ifdef ZM_ENABLE_CENC
+              case ZM_CENC:
+                    /* Accumlate the PN sequence */
+                    wd->sta.txiv[0] += 2;
+
+                    if (wd->sta.txiv[0] == 0 || wd->sta.txiv[0] == 1)
+                    {
+                        wd->sta.txiv[1]++;
+                    }
+
+                    if (wd->sta.txiv[1] == 0)
+                    {
+                        wd->sta.txiv[2]++;
+                    }
+
+                    if (wd->sta.txiv[2] == 0)
+                    {
+                        wd->sta.txiv[3]++;
+                    }
+
+                    if (wd->sta.txiv[3] == 0)
+                    {
+                        wd->sta.txiv[0] = 0;
+                        wd->sta.txiv[1] = 0;
+                        wd->sta.txiv[2] = 0;
+                    }
+
+                    header[hlen] = 0;
+                    header[hlen+1] = (u16_t) wd->sta.txiv[0];
+                    header[hlen+2] = (u16_t) (wd->sta.txiv[0] >> 16);
+                    header[hlen+3] = (u16_t) wd->sta.txiv[1];
+                    header[hlen+4] = (u16_t) (wd->sta.txiv[1] >> 16);
+                    header[hlen+5] = (u16_t) wd->sta.txiv[2];
+                    header[hlen+6] = (u16_t) (wd->sta.txiv[2] >> 16);
+                    header[hlen+7] = (u16_t) wd->sta.txiv[3];
+                    header[hlen+8] = (u16_t) (wd->sta.txiv[3] >> 16);
+
+                    macCtrl |= 0x80;
+                    icvLen = 16;  /* MIC */
+
+                    header[4] |= 0x4000;
+                    hlen += 9;
+				break;
+		    #endif //ZM_ENABLE_CENC
+			}/* end of switch */
+		}
+
+        /* Generate control setting */
+
+        /* protection mode */
+        if (wd->enableProtectionMode)
+        {
+            if (wd->enableProtectionMode==2)
+            {
+                /* Force enable protection: self cts  */
+                macCtrl &= 0xFFFC;
+                macCtrl |= 2;
+            }
+            /* if wd->enableProtectionMode=1 => force disable */
+            /* if wd->enableProtectionMode=0 => auto */
+        }
+        else
+        {
+
+            /* protection mode */
+            if (wd->sta.bProtectionMode == TRUE)
+            {
+                /* Enable Self-CTS */
+                macCtrl &= 0xFFFC;
+                macCtrl |= 2;
+            }
+        }
+
+    }
+
+    if (wd->txMCS != 0xff)
+    {
+        /* fixed rate */
+	    phyCtrl = ((u32_t)wd->txMCS<<16) + wd->txMT;
+        mcs = wd->txMCS;
+        mt = wd->txMT;
+    }
+
+    if (mt == 2)
+    {
+#if 0
+        /* HT PT: 0 Mixed mode    1 Green field */
+	    if (wd->sta.preambleTypeHT == ZM_PREAMBLE_TYPE_GREEN_FIELD)
+	    {
+            phyCtrl |= 0x4;     /* Bit 2 */
+        }
+#endif
+        /* Bandwidth */
+        if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ)
+        {
+            phyCtrl |= (0x80<<16);  /* BIT 23 */
+        }
+#if 0
+        /* STBC */
+        if (wd->sta.htCtrlSTBC<=0x3)
+        {
+            phyCtrl |= (wd->sta.htCtrlSTBC<<28);   /* BIT 23 */
+        }
+#endif
+        /* Short GI */
+        if(wd->sta.htCtrlSG)
+        {
+            phyCtrl |= (0x8000<<16);         /* BIT 31 */
+        }
+
+        /* TA */
+        if ( ((mcs >=0x8) && (mcs<=0xf))  || (wd->sta.htCtrlSTBC) )
+        {
+       	    phyCtrl |= 0x1800;               /* BIT 11 12 */
+    	}
+    }
+    else if(mt == 1)
+    {
+        #if 0
+        //bug that cause OFDM rate become duplicate legacy rate
+        /* Bandwidth */
+        if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ)
+        {
+            phyCtrl |= (0x80<<16);  /* BIT 23 */
+            mt = 3;                 /* duplicate legacy */
+            phyCtrl |= mt;
+        }
+        #endif
+    }
+    else if(mt == 0)
+    {
+	/* CCK PT: Legcy Preamble: 1 long preamble    2 short preamble */
+        if (wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_SHORT)
+        {
+    	       //phyCtrl |= 0x4;    /* BIT 2 */
+    	}
+    }
+
+    /* TA */
+    if (wd->sta.defaultTA)
+    {
+        phyCtrl |= 0x1000;
+    }
+    else
+    {
+        phyCtrl |= 0x0800;
+    }
+
+    //Get CurrentTxRate -- CWYang(+)
+    if ((mt == 0) || (mt == 1)) //B,G Rate
+    {
+        if (mcs < 16)
+        {
+            wd->CurrentTxRateKbps = zcIndextoRateBG[mcs];
+        }
+    }
+    else if (mt == 2)
+    {
+        if (mcs < 16)
+        {
+            if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ)
+            {
+                if((phyCtrl & 0x80000000) != 0)
+                {
+                    /* Short GI 40 MHz MIMO Rate */
+                    wd->CurrentTxRateKbps = zcIndextoRateN40S[mcs];
+                }
+                else
+                {
+                    /* Long GI 40 MHz MIMO Rate */
+                    wd->CurrentTxRateKbps = zcIndextoRateN40L[mcs];
+                }
+            }
+            else
+            {
+                if((phyCtrl & 0x80000000) != 0)
+                {
+                    /* Short GI 20 MHz MIMO Rate */
+                    wd->CurrentTxRateKbps = zcIndextoRateN20S[mcs];
+                }
+                else
+                {
+                    /* Long GI 20 MHz MIMO Rate */
+                    wd->CurrentTxRateKbps = zcIndextoRateN20L[mcs];
+                }
+            }
+        }
+    }
+
+    //802.11 header(include IV) = (hlen<<1)-8
+    //ethernet frame = len
+    //snap + mic = plusLen
+    //ethernet header = minusLen
+    //icv = icvLen
+    //crc32 = 4
+    //length=802.11 header+snap+(ethernet frame-ethernet header)+mic+icv+crc32
+    header[0] = ((hlen<<1)-8)+plusLen+(len-minusLen)+icvLen+4;  //Length
+
+    // header[0] : MPDU Lengths
+    if ((header[6] & 0x1) != 0x1) // Unicast Frame
+    {
+        if (header[0] >= wd->rtsThreshold)
+        {
+            /* Enable RTS */
+            macCtrl |= 1;
+        }
+    }
+
+    if ( wd->sta.encryMode == ZM_TKIP )
+        tkipFrameOffset = 8;
+
+    if( wd->sta.EnableHT != 1 )
+    { // Aggregation should not be fragmented !
+        if ( header[0] > ( wd->fragThreshold + tkipFrameOffset ) )
+        {
+            return 0; // Need to be fragmented ! !
+        }
+    }
+
+    //if ( wd->sta.encryMode == ZM_TKIP )
+    //{
+    //    zm_debug_msg1("ctrl length = ", header[0]);
+    //}
+
+    //MAC control
+    if (rateProbingFlag != 0)
+    {
+        macCtrl |= 0x8000;
+    }
+    header[1] = macCtrl;
+    //PHY control L
+    header[2] = (u16_t) ((phyCtrl&0xffff) | 0x700 | (zcUpToAc[up&0x7]<<13));
+    //PHY control H
+    header[3] = (u16_t) ((phyCtrl>>16) | 0x700);
+
+    if (wd->enableAggregation)
+    {
+        /* force enable aggregation */
+        if (wd->enableAggregation==2 && !(header[6]&0x1))
+        {
+            if (((header[2] & 0x3) == 2))
+            {
+                /* Enable aggregation */
+                header[1] |= 0x20;
+            }
+        }
+        /* if wd->enableAggregation=1 => force disable */
+        /* if wd->enableAggregation=0 => auto */
+    }
+
+#ifdef ZM_ENABLE_AGGREGATION
+    if (wd->addbaComplete) {
+        #ifdef ZM_BYPASS_AGGR_SCHEDULING
+        if (!(header[6]&0x1) && !rateProbingFlag && (wd->enableAggregation != 1))
+        {
+            if (((header[2] & 0x3) == 2))
+            {
+                /* Unicast frame with HT rate => Enable aggregation */
+                /* We only support software encryption in single packet mode */
+                if ((wd->sta.SWEncryptEnable & ZM_SW_TKIP_ENCRY_EN) == 0 &&
+                    (wd->sta.SWEncryptEnable & ZM_SW_WEP_ENCRY_EN) == 0)
+                {
+                    /* Set aggregation group bits per AC */
+                    header[1] |= (0x20 | (zcUpToAc[up&0x7]<<10));
+
+                    //if (wd->sta.currentFrequency < 3000)
+                    {
+                        /* issue: -PB42 Enable RTS/CTS to prevent OWL Tx hang up */
+                        /* If this is Owl Ap, enable RTS/CTS protect */
+                        if ( (wd->sta.athOwlAp == 1) || (wd->sta.RTSInAGGMode == TRUE) )
+                        {
+                            header[1] &= 0xfffc;
+                            header[1] |= 0x1;
+                        }
+
+                        /* Enable RIFS : workaround 854T RTS/CTS */
+                        /* Bit13 : TI enable RIFS */
+                        //header[1] |= 0x2000;
+                    }
+                }
+            }
+        }
+        #else
+        /*
+         * aggregation ampduIndication control
+         */
+        if (aggControl && aggControl->aggEnabled) {
+            if (wd->enableAggregation==0 && !(header[6]&0x1))
+            {
+                if (((header[2] & 0x3) == 2))
+                {
+                    /* Enable aggregation */
+                    header[1] |= 0x20;
+                    if (ZM_AGG_LAST_MPDU == aggControl->ampduIndication)
+                        header[1] |= 0x4000;
+                }
+                else {
+                    zm_debug_msg1("no aggr, header[2]&0x3 = ",header[2] & 0x3)
+                    aggControl->aggEnabled = 0;
+                }
+            }
+            else {
+                zm_debug_msg1("no aggr, wd->enableAggregation = ", wd->enableAggregation);
+                zm_debug_msg1("no aggr, !header[6]&0x1 = ",!(header[6]&0x1));
+                aggControl->aggEnabled = 0;
+            }
+        }
+        #endif
+
+        #ifdef ZM_AGGR_BIT_ON
+        if (!(header[6]&0x1) && !rateProbingFlag)
+        {
+            if (((header[2] & 0x3) == 2))
+            {
+                /* Unicast frame with HT rate => Enable aggregation */
+                /* Set aggregation group bits per AC */
+                header[1] |= (0x20 | (zcUpToAc[up&0x7]<<10));
+
+                //if (wd->sta.currentFrequency < 3000)
+                {
+                    /* Enable RTS/CTS to prevent OWL Tx hang up */
+                    header[1] &= 0xfffc;
+                    header[1] |= 0x1;
+                }
+            }
+        }
+        #endif
+    }
+#endif
+
+    return (hlen<<1);
+}
+
+
+u16_t zfTxGenMmHeader(zdev_t* dev, u8_t frameType, u16_t* dst,
+        u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt)
+{
+    //u16_t bodyLen;
+    u8_t  hlen = 32;        // MAC ctrl + PHY ctrl + 802.11 MM header
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /* Generate control setting */
+    //bodyLen = zfwBufGetSize(dev, buf);
+    header[0] = 24+len+4;   //Length
+    if ((dst[0] & 0x1) != 0) //Broadcast, multicast frames
+    {
+        header[1] = 0xc;            //MAC control, backoff + noack
+    }
+    else
+    {
+        header[1] = 0x8;            //MAC control, backoff + (ack)
+    }
+    /* Dualband Management frame tx Rate */
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        if (wd->frequency < 3000)
+        {
+            /* CCK 1M */
+            header[2] = 0x0f00;          //PHY control L
+            header[3] = 0x0000;          //PHY control H
+        }
+        else
+        {
+            /* CCK 6M */
+            header[2] = 0x0f01;          //PHY control L
+            header[3] = 0x000B;          //PHY control H
+        }
+    }
+    else
+    {
+        if (wd->sta.currentFrequency < 3000)
+        {
+            /* CCK 2M */
+            header[2] = 0x0f00;          //PHY control L
+            header[3] = 0x0001;          //PHY control H
+        }
+        else
+        {
+            /* CCK 6M */
+            header[2] = 0x0f01;          //PHY control L
+            header[3] = 0x000B;          //PHY control H
+        }
+    }
+    /* Generate WLAN header */
+    /* Frame control */
+    header[4+0] = frameType;
+    /* Duration */
+    header[4+1] = 0;
+
+    if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        if ( frameType == ZM_WLAN_FRAME_TYPE_PROBEREQ )
+        {
+            header[4+8] = 0xFFFF;
+            header[4+9] = 0xFFFF;
+            header[4+10] = 0xFFFF;
+        }
+        else if ( frameType == ZM_WLAN_FRAME_TYPE_BA ) {
+            /* do nothing */
+        }
+        else
+        {
+            header[4+8] = wd->sta.bssid[0];
+            header[4+9] = wd->sta.bssid[1];
+            header[4+10] = wd->sta.bssid[2];
+        }
+    }
+    else if (wd->wlanMode == ZM_MODE_PSEUDO)
+    {
+        /* Address 3 = 00:00:00:00:00:00 */
+        header[4+8] = 0;
+        header[4+9] = 0;
+        header[4+10] = 0;
+    }
+    else if (wd->wlanMode == ZM_MODE_IBSS)
+    {
+        header[4+8] = wd->sta.bssid[0];
+        header[4+9] = wd->sta.bssid[1];
+        header[4+10] = wd->sta.bssid[2];
+
+        if ( frameType == ZM_WLAN_FRAME_TYPE_ATIM )
+        {
+            /* put ATIM to queue 5th */
+            //header[2] |= (ZM_BIT_13|ZM_BIT_14);
+            header[2] |= ZM_BIT_15;
+        }
+    }
+    else if (wd->wlanMode == ZM_MODE_AP)
+    {
+        /* Address 3 = BSSID */
+        header[4+8] = wd->macAddr[0];
+        header[4+9] = wd->macAddr[1];
+#ifdef ZM_VAPMODE_MULTILE_SSID
+        header[4+10] = wd->macAddr[2]; //Multiple SSID
+#else
+        header[4+10] = wd->macAddr[2] + (vap<<8); //VAP
+#endif
+        //if in scan, must set address 3 to broadcast because of some ap would care this
+        //if ((wd->heartBeatNotification & ZM_BSSID_LIST_SCAN)
+        //        == ZM_BSSID_LIST_SCAN)
+        //if FrameType is Probe Request, Address3 should be boradcast
+        if (frameType == ZM_WLAN_FRAME_TYPE_PROBEREQ)
+        {
+            header[4+8] = 0xFFFF;
+            header[4+9] = 0xFFFF;
+            header[4+10] = 0xFFFF;
+        }
+    }
+
+    /* Address 1 = DA */
+    header[4+2] = dst[0];
+    header[4+3] = dst[1];
+    header[4+4] = dst[2];
+
+    /* Address 2 = SA */
+    header[4+5] = wd->macAddr[0];
+    header[4+6] = wd->macAddr[1];
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+#ifdef ZM_VAPMODE_MULTILE_SSID
+        header[4+7] = wd->macAddr[2]; //Multiple SSID
+#else
+        header[4+7] = wd->macAddr[2] + (vap<<8); //VAP
+#endif
+    }
+    else
+    {
+        header[4+7] = wd->macAddr[2];
+    }
+
+    /* Sequence Control */
+    zmw_enter_critical_section(dev);
+    header[4+11] = ((wd->mmseq++)<<4);
+    zmw_leave_critical_section(dev);
+
+    if( frameType == ZM_WLAN_FRAME_TYPE_QOS_NULL )
+    {
+        /*Qos Control*/
+        header[4+12] = 0x0;
+        hlen+=2;
+        header[0]+=2;
+    }
+
+    if ( encrypt )
+    {
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED )
+        {
+            if ( (wd->sta.encryMode == ZM_WEP64)||
+                 (wd->sta.encryMode == ZM_WEP128)||
+                 (wd->sta.encryMode == ZM_WEP256) )
+            {
+                header[4] |= 0x4000;
+                header[16] = 0x0;   //IV
+                header[17] = 0x0; //IV
+                header[17] |= (((u16_t) wd->sta.keyId) << 14);
+                hlen += 4;
+
+                header[0] += 8;         // icvLen = 4;
+                header[1] |= 0x40;      // enable encryption on macCtrl
+             }
+        }
+    }
+
+    // Enable HW duration
+    if ( frameType != ZM_WLAN_FRAME_TYPE_PSPOLL )
+    {
+        header[1] |= 0x200;
+    }
+
+    return hlen;
+}
+
+void zfInitMacApMode(zdev_t* dev)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    zfHpEnableBeacon(dev, ZM_MODE_AP, (wd->beaconInterval/wd->ap.vapNumber), 1, 0);
+
+    /* AP mode */
+    zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_AP);
+
+    /* VAP test code */
+    /* AP + VAP mode */
+    if (wd->ap.vapNumber >= 2)
+    {
+        for (i=1; i<ZM_MAX_AP_SUPPORT; i++)
+        {
+            if (((wd->ap.apBitmap >> i) & 0x1) != 0)
+            {
+                u16_t mac[3];
+                mac[0] = wd->macAddr[0];
+                mac[1] = wd->macAddr[1];
+#ifdef ZM_VAPMODE_MULTILE_SSID
+                mac[2] = wd->macAddr[2]; //Multiple SSID
+#else
+                mac[2] = wd->macAddr[2] + (i<<8); //VAP
+#endif
+                zfHpSetMacAddress(dev, mac, i);
+
+            }
+        }
+    }
+
+    /* basic rate setting */
+    zfHpSetBasicRateSet(dev, wd->bRateBasic, wd->gRateBasic);
+
+    /* Set TxQs CWMIN, CWMAX, AIFS and TXO to WME AP default. */
+    zfUpdateDefaultQosParameter(dev, 1);
+
+    return;
+}
+
+u16_t zfChGetNextChannel(zdev_t* dev, u16_t frequency, u8_t* pbPassive)
+{
+    u8_t   i;
+    u8_t   bPassive;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Avoid NULL value */
+    if ( pbPassive == NULL )
+    {
+        pbPassive = &bPassive;
+    }
+
+    for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
+    {
+        if ( wd->regulationTable.allowChannel[i].channel == frequency )
+        {
+            if ( i == (wd->regulationTable.allowChannelCnt-1) )
+            {
+                i = 0;
+            }
+            else
+            {
+                i++;
+            }
+
+            if ( wd->regulationTable.allowChannel[i].channelFlags
+                    & ZM_REG_FLAG_CHANNEL_PASSIVE )
+            {
+                *pbPassive = TRUE;
+            }
+            else
+            {
+                *pbPassive = FALSE;
+            }
+
+            return wd->regulationTable.allowChannel[i].channel;
+        }
+    }
+
+    return 0xffff;
+}
+
+u16_t zfChGetFirstChannel(zdev_t* dev, u8_t* pbPassive)
+{
+    u8_t   bPassive;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Avoid NULL value */
+    if ( pbPassive == NULL )
+    {
+        pbPassive = &bPassive;
+    }
+
+   if ( wd->regulationTable.allowChannel[0].channelFlags & ZM_REG_FLAG_CHANNEL_PASSIVE )
+    {
+        *pbPassive = TRUE;
+    }
+    else
+    {
+        *pbPassive = FALSE;
+    }
+
+    return wd->regulationTable.allowChannel[0].channel;
+}
+
+u16_t zfChGetFirst2GhzChannel(zdev_t* dev)
+{
+    u8_t    i;
+
+    zmw_get_wlan_dev(dev);
+
+    for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
+    {
+        if ( wd->regulationTable.allowChannel[i].channel < 3000 )
+        {
+            /* find the first 2Ghz channel */
+            return wd->regulationTable.allowChannel[i].channel;
+        }
+    }
+
+    /* Can not find any 2Ghz channel */
+    return 0;
+}
+
+u16_t zfChGetFirst5GhzChannel(zdev_t* dev)
+{
+    u8_t    i;
+
+    zmw_get_wlan_dev(dev);
+
+    for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
+    {
+        if ( wd->regulationTable.allowChannel[i].channel > 3000 )
+        {
+            /* find the first 5Ghz channel */
+            return wd->regulationTable.allowChannel[i].channel;
+        }
+    }
+
+    /* Can not find any 5Ghz channel */
+    return 0;
+}
+
+u16_t zfChGetLastChannel(zdev_t* dev, u8_t* pbPassive)
+{
+    u8_t   bPassive;
+    u8_t   ChannelIndex;
+
+    zmw_get_wlan_dev(dev);
+
+    ChannelIndex = wd->regulationTable.allowChannelCnt-1;
+
+    /* Avoid NULL value */
+    if ( pbPassive == NULL )
+    {
+        pbPassive = &bPassive;
+    }
+
+    if ( wd->regulationTable.allowChannel[ChannelIndex].channelFlags
+            & ZM_REG_FLAG_CHANNEL_PASSIVE )
+    {
+        *pbPassive = TRUE;
+    }
+    else
+    {
+        *pbPassive = FALSE;
+    }
+
+    return wd->regulationTable.allowChannel[ChannelIndex].channel;
+}
+
+u16_t zfChGetLast5GhzChannel(zdev_t* dev)
+{
+    u8_t    i;
+    u16_t   last5Ghzfrequency;
+
+    zmw_get_wlan_dev(dev);
+
+    last5Ghzfrequency = 0;
+    for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
+    {
+        if ( wd->regulationTable.allowChannel[i].channel > 3000 )
+        {
+            last5Ghzfrequency = wd->regulationTable.allowChannel[i].channel;
+        }
+    }
+
+    return last5Ghzfrequency;
+}
+
+/* freqBand = 0 => auto check   */
+/*          = 1 => 2.4 GHz band */
+/*          = 2 => 5 GHz band   */
+u16_t zfChNumToFreq(zdev_t* dev, u8_t ch, u8_t freqBand)
+{
+    u16_t freq = 0xffff;
+
+    if ( freqBand == 0 )
+    {
+        if (ch > 14)
+        {   /* adapter is at 5 GHz band */
+            freqBand = 2;
+        }
+        else
+        {
+            freqBand = 1;
+        }
+    }
+
+    if ( freqBand == 2 )
+    {   /* the channel belongs to 5 GHz band */
+        if ( (ch >= 184)&&(ch <= 196) )
+        {
+            freq = 4000 + ch*5;
+        }
+        else
+        {
+            freq = 5000 + ch*5;
+        }
+    }
+    else
+    {   /* the channel belongs to 2.4 GHz band */
+        if ( ch == 14 )
+        {
+            freq = ZM_CH_G_14;
+        }
+        else
+        {
+            freq = ZM_CH_G_1 + (ch-1)*5;
+        }
+    }
+
+    return freq;
+}
+
+u8_t zfChFreqToNum(u16_t freq, u8_t* pbIs5GBand)
+{
+    u8_t   ch;
+    u8_t   Is5GBand;
+
+    /* to avoid NULL value */
+    if ( pbIs5GBand == NULL )
+    {
+        pbIs5GBand = &Is5GBand;
+    }
+
+    *pbIs5GBand = FALSE;
+
+    if ( freq == ZM_CH_G_14 )
+    {
+        ch = 14;
+    }
+    else if ( freq < 4000 )
+    {
+        ch = (freq - ZM_CH_G_1) / 5 + 1;
+    }
+    else if ( freq < 5000 )
+    {
+        ch = (freq - 4000) / 5;
+        *pbIs5GBand = TRUE;
+    }
+    else
+    {
+        ch = (freq - 5000) / 5;
+        *pbIs5GBand = TRUE;
+    }
+
+    return ch;
+}
diff --git a/drivers/staging/otus/80211core/cmm.c b/drivers/staging/otus/80211core/cmm.c
new file mode 100644
index 000000000000..b74379d928f6
--- /dev/null
+++ b/drivers/staging/otus/80211core/cmm.c
@@ -0,0 +1,2141 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : mm.c                                                  */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains common functions for handle management     */
+/*      frame.                                                          */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "../hal/hpreg.h"
+
+/* TODO : put all constant tables to a file */
+const u8_t zg11bRateTbl[4] = {2, 4, 11, 22};
+const u8_t zg11gRateTbl[8] = {12, 18, 24, 36, 48, 72, 96, 108};
+
+/* 0xff => element does not exist */
+const u8_t zgElementOffsetTable[] =
+{
+    4,      /*  0 : asoc req */
+    6,      /*  1 : asoc rsp */
+    10,     /*  2 : reasoc req*/
+    6,      /*  3 : reasoc rsp */
+    0,      /*  4 : probe req */
+    12,     /*  5 : probe rsp */
+    0xff,   /*  6 : reserved */
+    0xff,   /*  7 : reserved */
+    12,     /*  8 : beacon */
+    4,      /*  9 : ATIM */
+    0xff,   /* 10 : disasoc */
+    6,      /* 11 : auth */
+    0xff,   /* 12 : deauth */
+    4,      /* 13 : action */
+    0xff,   /* 14 : reserved */
+    0xff,   /* 15 : reserved */
+};
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfFindElement               */
+/*      Find a specific element in management frame                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : management frame buffer                                   */
+/*      eid : target element id                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      byte offset of target element                                   */
+/*      or 0xffff if not found                                          */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfFindElement(zdev_t* dev, zbuf_t* buf, u8_t eid)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id, HTEid=0;
+    u8_t oui[4] = {0x00, 0x50, 0xf2, 0x01};
+    u8_t oui11n[3] = {0x00,0x90,0x4C};
+    u8_t HTType = 0;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    // jhlee HT 0
+
+    if ((eid == ZM_WLAN_EID_HT_CAPABILITY) ||
+        (eid == ZM_WLAN_EID_EXTENDED_HT_CAPABILITY))
+    {
+        HTEid = eid;
+        eid = ZM_WLAN_EID_WPA_IE;
+        HTType = 1;
+    }
+
+
+    bufLen = zfwBufGetSize(dev, buf);
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == eid)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 && eid != ZM_WLAN_EID_SSID)
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( eid == ZM_WLAN_EID_WPA_IE )
+            {
+                /* avoid sta to be thought use 11n when find a WPA_IE */
+                if ( (HTType == 0) && zfRxBufferEqualToStr(dev, buf, oui, offset+2, 4) )
+                {
+                    return offset;
+                }
+
+                // jhlee HT 0
+                // CWYang(+)
+
+                if ((HTType == 1) && ( zfRxBufferEqualToStr(dev, buf, oui11n, offset+2, 3) ))
+                {
+                    if ( zmw_rx_buf_readb(dev, buf, offset+5) == HTEid )
+                    {
+                        return offset + 5;
+                    }
+                }
+
+            }
+            else
+            {
+                return offset;
+            }
+        }
+        /* Advance to next element */
+        #if 1
+        elen = zmw_rx_buf_readb(dev, buf, offset+1);
+        #else
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        #endif
+
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfFindWifiElement           */
+/*      Find a specific Wifi element in management frame                */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : management frame buffer                                   */
+/*      type : OUI type                                                 */
+/*      subType : OUI subtype                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      byte offset of target element                                   */
+/*      or 0xffff if not found                                          */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.1      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfFindWifiElement(zdev_t* dev, zbuf_t* buf, u8_t type, u8_t subtype)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_WIFI_IE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x50)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0xF2)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+5)) == type))
+
+            {
+                if ( subtype != 0xff )
+                {
+                    if ( (tmp = zmw_rx_buf_readb(dev, buf, offset+6)) == subtype  )
+                    {
+                        return offset;
+                    }
+                }
+                else
+                {
+                    return offset;
+                }
+            }
+        }
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+u16_t zfRemoveElement(zdev_t* dev, u8_t* buf, u16_t size, u8_t eid)
+{
+    u16_t offset = 0;
+    u16_t elen;
+    u8_t  HTEid = 0;
+    u8_t  oui[4] = {0x00, 0x50, 0xf2, 0x01};
+    u8_t  oui11n[3] = {0x00,0x90,0x4C};
+    u8_t  HTType = 0;
+
+    if ((eid == ZM_WLAN_EID_HT_CAPABILITY) ||
+        (eid == ZM_WLAN_EID_EXTENDED_HT_CAPABILITY))
+    {
+        HTEid = eid;
+        eid = ZM_WLAN_EID_WPA_IE;
+        HTType = 1;
+    }
+
+    while (offset < size)
+    {
+        elen = *(buf+offset+1);
+
+        if (*(buf+offset) == eid)
+        {
+            if ( eid == ZM_WLAN_EID_WPA_IE )
+            {
+                if ( (HTType == 0)
+                     && (*(buf+offset+2) == oui[0])
+                     && (*(buf+offset+3) == oui[1])
+                     && (*(buf+offset+4) == oui[2])
+                     && (*(buf+offset+5) == oui[3]) )
+                {
+                    zfMemoryMove(buf+offset, buf+offset+elen+2, size-offset-elen-2);
+                    return (size-elen-2);
+                }
+
+                if ( (HTType == 1)
+                    && (*(buf+offset+2) == oui11n[0])
+                    && (*(buf+offset+3) == oui11n[1])
+                    && (*(buf+offset+4) == oui11n[2])
+                    && (*(buf+offset+5) == HTEid) )
+                {
+                    zfMemoryMove(buf+offset, buf+offset+elen+2, size-offset-elen-2);
+                    return (size-elen-2);
+                }
+            }
+            else
+            {
+                zfMemoryMove(buf+offset, buf+offset+elen+2, size-offset-elen-2);
+                return (size-elen-2);
+            }
+        }
+
+        offset += (elen+2);
+    }
+
+    return size;
+}
+
+u16_t zfUpdateElement(zdev_t* dev, u8_t* buf, u16_t size, u8_t* updateeid)
+{
+    u16_t offset = 0;
+    u16_t elen;
+
+    while (offset < size) {
+        elen = *(buf+offset+1);
+
+        if (*(buf+offset) == updateeid[0]) {
+            if (updateeid[1] <= elen) {
+                zfMemoryMove(buf+offset, updateeid, updateeid[1]+2);
+                zfMemoryMove(buf+offset+updateeid[1]+2, buf+offset+elen+2, size-offset-elen-2);
+
+                return size-(elen-updateeid[1]);
+            } else {
+                zfMemoryMove(buf+offset+updateeid[1]+2, buf+offset+elen+2, size-offset-elen-2);
+                zfMemoryMove(buf+offset, updateeid, updateeid[1]+2);
+
+                return size+(updateeid[1]-elen);
+            }
+        }
+
+        offset += (elen+2);
+    }
+
+    return size;
+}
+
+u16_t zfFindSuperGElement(zdev_t* dev, zbuf_t* buf, u8_t type)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t super_feature;
+    u8_t ouiSuperG[6] = {0x00,0x03,0x7f,0x01, 0x01, 0x00};
+
+    zmw_get_wlan_dev(dev);
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_VENDOR_PRIVATE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (zfRxBufferEqualToStr(dev, buf, ouiSuperG, offset+2, 6) && ( zmw_rx_buf_readb(dev, buf, offset+1) >= 6))
+            {
+                /* super_feature 0:useFastFrame, 1:useCompression, 2:useTurboPrime */
+                super_feature= zmw_rx_buf_readb(dev, buf, offset+8);
+                if ((super_feature & 0x01) || (super_feature & 0x02) || (super_feature & 0x04))
+                {
+                    return offset;
+                }
+            }
+        }
+        /* Advance to next element */
+        #if 1
+        elen = zmw_rx_buf_readb(dev, buf, offset+1);
+        #else
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        #endif
+
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+u16_t zfFindXRElement(zdev_t* dev, zbuf_t* buf, u8_t type)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t ouixr[6] = {0x00,0x03,0x7f,0x03, 0x01, 0x00};
+
+    zmw_get_wlan_dev(dev);
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_VENDOR_PRIVATE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (zfRxBufferEqualToStr(dev, buf, ouixr, offset+2, 6) && ( zmw_rx_buf_readb(dev, buf, offset+1) >= 6))
+            {
+                return offset;
+            }
+        }
+        /* Advance to next element */
+        #if 1
+        elen = zmw_rx_buf_readb(dev, buf, offset+1);
+        #else
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        #endif
+
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfMmAddIeSupportRate        */
+/*      Add information element Support Rate to buffer.                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*      eid : element ID                                                */
+/*      rateSet :  CCK or OFDM                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddIeSupportRate(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t eid, u8_t rateSet)
+{
+    u8_t len = 0;
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    //if ( (rateSet == ZM_RATE_SET_OFDM)&&((wd->gRate & 0xff) == 0) )
+    //{
+    //    return offset;
+    //}
+
+    /* Information : Support Rate */
+    if ( rateSet == ZM_RATE_SET_CCK )
+    {
+        for (i=0; i<4; i++)
+        {
+            if ((wd->bRate & (0x1<<i)) == (0x1<<i))
+            //if ((0xf & (0x1<<i)) == (0x1<<i))
+            {
+                zmw_tx_buf_writeb(dev, buf, offset+len+2,
+                                     zg11bRateTbl[i]+((wd->bRateBasic & (0x1<<i))<<(7-i)));
+                len++;
+            }
+        }
+    }
+    else if ( rateSet == ZM_RATE_SET_OFDM )
+    {
+        for (i=0; i<8; i++)
+        {
+            if ((wd->gRate & (0x1<<i)) == (0x1<<i))
+            //if ((0xff & (0x1<<i)) == (0x1<<i))
+            {
+                zmw_tx_buf_writeb(dev, buf, offset+len+2,
+                                     zg11gRateTbl[i]+((wd->gRateBasic & (0x1<<i))<<(7-i)));
+                len++;
+            }
+        }
+    }
+
+    if (len > 0)
+    {
+        /* Element ID */
+        zmw_tx_buf_writeb(dev, buf, offset, eid);
+
+        /* Element Length */
+        zmw_tx_buf_writeb(dev, buf, offset+1, len);
+
+        /* Return value */
+        offset += (2+len);
+    }
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfMmAddIeDs                 */
+/*      Add information element DS to buffer.                           */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddIeDs(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_DS);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 1);
+
+    /* Information : DS */
+    zmw_tx_buf_writeb(dev, buf, offset++,
+                         zfChFreqToNum(wd->frequency, NULL));
+
+    return offset;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfMmAddIeErp                */
+/*      Add information element ERP to buffer.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddIeErp(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_ERP);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 1);
+
+    /* Information : ERP */
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->erpElement);
+
+    return offset;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfMmAddIeWpa                */
+/*      Add information element WPA to buffer.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Yuan-Gu Wei         ZyDAS Technology Corporation    2006.2      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddIeWpa(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t apId)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    int i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    //zmw_inttx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WPA_IE);
+
+    /* Element Length */
+    //zmw_inttx_buf_writeb(dev, buf, offset++, wd->ap.wpaLen);
+    for(i = 0; i < wd->ap.wpaLen[apId]; i++)
+    {
+        /* Information : WPA */
+        zmw_tx_buf_writeb(dev, buf, offset++, wd->ap.wpaIe[apId][i]);
+    }
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfMmAddHTCapability         */
+/*      Add HT Capability Infomation to buffer.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Chao-Wen Yang     ZyDAS Technology Corporation       2006.06    */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u8_t OUI[3] = {0x0,0x90,0x4C};
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Prob ID */
+    zmw_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WPA_IE);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Data.Length + 4);
+
+        /* OUI Data */
+        for (i = 0; i < 3; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, OUI[i]);
+        }
+
+        /* Element Type ID */
+        zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Data.ElementID);
+
+        /* HT Capability Data */
+        for (i = 0; i < 26; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Byte[i+2]);
+        }
+    }
+    else
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Data.Length + 4);
+
+        /* OUI Data */
+        for (i = 0; i < 3; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, OUI[i]);
+        }
+
+        /* Element Type ID */
+        zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Data.ElementID);
+
+        /* HT Capability Data */
+        for (i = 0; i < 26; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Byte[i+2]);
+        }
+    }
+
+    return offset;
+}
+
+
+u16_t zfMmAddPreNHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    //u8_t OUI[3] = {0x0,0x90,0x4C};
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Prob ID */
+    zmw_buf_writeb(dev, buf, offset++, ZM_WLAN_PREN2_EID_HTCAPABILITY);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Data.Length);
+
+        /* HT Capability Data */
+        for (i = 0; i < 26; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Byte[i+2]);
+        }
+    }
+    else
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Data.Length);
+
+        /* HT Capability Data */
+        for (i = 0; i < 26; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Byte[i+2]);
+        }
+    }
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                zfMmAddExtendedHTCapability   */
+/*      Add Extended HT Capability Infomation to buffer.                */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Chao-Wen Yang     ZyDAS Technology Corporation       2006.06    */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddExtendedHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u8_t OUI[3] = {0x0,0x90,0x4C};
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Prob ID */
+    zmw_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WPA_IE);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->ap.ExtHTCap.Data.Length + 4);
+
+        /* OUI Data */
+        for (i = 0; i < 3; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, OUI[i]);
+        }
+
+        /* Element Type ID */
+        zmw_buf_writeb(dev, buf, offset++, wd->ap.ExtHTCap.Data.ElementID);
+
+        /* HT Capability Data */
+        for (i = 0; i < 22; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->ap.ExtHTCap.Byte[i+2]);
+        }
+    }
+    else
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->sta.ExtHTCap.Data.Length + 4);
+
+        /* OUI Data */
+        for (i = 0; i < 3; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, OUI[i]);
+        }
+
+        /* Element Type ID */
+        zmw_buf_writeb(dev, buf, offset++, wd->sta.ExtHTCap.Data.ElementID);
+
+        /* HT Capability Data */
+        for (i = 0; i < 22; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->sta.ExtHTCap.Byte[i+2]);
+        }
+    }
+
+    return offset;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfSendMmFrame               */
+/*      Send management frame.                                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      frameType : management frame type                               */
+/*      dst : destination MAC address                                   */
+/*      p1 : parameter 1                                                */
+/*      p2 : parameter 2                                                */
+/*      p3 : parameter 3                                                */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+/* probe req : p1=> bWithSSID, p2=>R, p3=>R                                      */
+/* probe rsp : p1=>R, p2=>R, p3=>VAP ID(AP)                             */
+/* deauth : p1=>Reason Code, p2=>R, p3=>VAP ID(AP)                      */
+/* Disasoc : p1=>Reason Code, p2=>R, p3=>VAP ID(AP)                     */
+/* ATIM : p1=>R, p2=>R, p3=>R                                           */
+/* (re)asoc rsp : p1=>Status Code, p2=>AID, p3=>VAP ID(AP)              */
+/* asoc req : p1=>R, p2=>R, p3=>R                                       */
+/* reasoc req : p1=>AP MAC[0], p2=>AP MAC[1], p3=>AP MAC[2]             */
+/* auth : p1=>low=Algorithm, high=Transaction, p2=>Status, p3=>VAP ID   */
+void zfSendMmFrame(zdev_t* dev, u8_t frameType, u16_t* dst,
+                   u32_t p1, u32_t p2, u32_t p3)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t offset = 0;
+    u16_t hlen = 32;
+    u16_t header[(24+25+1)/2];
+    u16_t vap = 0;
+    u16_t i;
+    u8_t encrypt = 0;
+    u16_t aid;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zm_msg2_mm(ZM_LV_2, "Send mm frame, type=", frameType);
+    /* TBD : Maximum size of managment frame */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return;
+    }
+
+    //Reserve room for wlan header
+    offset = hlen;
+
+    switch (frameType)
+    {
+        case ZM_WLAN_FRAME_TYPE_PROBEREQ :
+            offset = zfSendProbeReq(dev, buf, offset, (u8_t) p1);
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_PROBERSP :
+            zm_msg0_mm(ZM_LV_3, "probe rsp");
+            /* 24-31 Time Stamp : hardware WON'T fill this field */
+            zmw_tx_buf_writeh(dev, buf, offset, 0);
+            zmw_tx_buf_writeh(dev, buf, offset+2, 0);
+            zmw_tx_buf_writeh(dev, buf, offset+4, 0);
+            zmw_tx_buf_writeh(dev, buf, offset+6, 0);
+            offset+=8;
+
+            /* Beacon Interval */
+            zmw_tx_buf_writeh(dev, buf, offset, wd->beaconInterval);
+            offset+=2;
+
+            if (wd->wlanMode == ZM_MODE_AP)
+            {
+                vap = (u16_t) p3;
+                /* Capability */
+                zmw_tx_buf_writeh(dev, buf, offset, wd->ap.capab[vap]);
+                offset+=2;
+                /* SSID */
+                offset = zfApAddIeSsid(dev, buf, offset, vap);
+            }
+            else
+            {
+                /* Capability */
+                zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[0]);
+                zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[1]);
+                /* SSID */
+                offset = zfStaAddIeSsid(dev, buf, offset);
+            }
+
+            /* Support Rate */
+            if ( wd->frequency < 3000 )
+            {
+            offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                          ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+            }
+            else
+            {
+                offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                          ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+            }
+
+            /* DS parameter set */
+            offset = zfMmAddIeDs(dev, buf, offset);
+
+            /* TODO ¡G IBSS */
+            if ( wd->wlanMode == ZM_MODE_IBSS )
+            {
+                offset = zfStaAddIeIbss(dev, buf, offset);
+
+                if (wd->frequency < 3000)
+                {
+                    if( wd->wfc.bIbssGMode
+                        && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )    // Only accompany with enabling a mode .
+                    {
+                        /* ERP Information */
+                        wd->erpElement = 0;
+                        offset = zfMmAddIeErp(dev, buf, offset);
+
+                        /* Enable G Mode */
+                        /* Extended Supported Rates */
+                   	    offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+            	    }
+        	    }
+            }
+
+
+            if ((wd->wlanMode == ZM_MODE_AP)
+                 && (wd->ap.wlanType[vap] != ZM_WLAN_TYPE_PURE_B))
+            {
+                /* ERP Information */
+                offset = zfMmAddIeErp(dev, buf, offset);
+
+                /* Extended Supported Rates */
+		if ( wd->frequency < 3000 )
+                {
+                offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                   ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+            }
+            }
+
+            /* ERP Information */
+            //offset = zfMmAddIeErp(dev, buf, offset);
+
+            /* Extended Supported Rates */
+            //offset = zfMmAddIeSupportRate(dev, buf, offset,
+            //                              ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+
+            /* TODO : RSN */
+            if (wd->wlanMode == ZM_MODE_AP && wd->ap.wpaSupport[vap] == 1)
+            {
+                offset = zfMmAddIeWpa(dev, buf, offset, vap);
+            }
+            else if ( wd->wlanMode == ZM_MODE_IBSS && wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK)
+            {
+                offset = zfwStaAddIeWpaRsn(dev, buf, offset, ZM_WLAN_FRAME_TYPE_AUTH);
+            }
+
+            /* WME Parameters */
+            if (wd->wlanMode == ZM_MODE_AP)
+            {
+                if (wd->ap.qosMode == 1)
+                {
+                    offset = zfApAddIeWmePara(dev, buf, offset, vap);
+                }
+            }
+
+            if ( wd->wlanMode != ZM_MODE_IBSS )
+            {
+            // jhlee HT 0
+            //CWYang(+)
+                /* TODO : Need to check if it is ok */
+            /* HT Capabilities Info */
+            offset = zfMmAddHTCapability(dev, buf, offset);
+            //CWYang(+)
+            /* Extended HT Capabilities Info */
+            offset = zfMmAddExtendedHTCapability(dev, buf, offset);
+            }
+
+            if ( wd->sta.ibssAdditionalIESize )
+                offset = zfStaAddIbssAdditionalIE(dev, buf, offset);
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_AUTH :
+            if (p1 == 0x30001)
+            {
+                hlen += 4;
+                offset += 4;        // for reserving wep header
+                encrypt = 1;
+            }
+
+            /* Algotrithm Number */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)(p1&0xffff));
+            offset+=2;
+
+            /* Transaction Number */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)(p1>>16));
+            offset+=2;
+
+            /* Status Code */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)p2);
+            offset+=2;
+
+            if (wd->wlanMode == ZM_MODE_AP)
+            {
+                vap = (u16_t) p3;
+            }
+
+            /* Challenge Text => share-2 or share-3 */
+            if (p1 == 0x20001)
+            {
+                if (p2 == 0) //Status == success
+                {
+                    zmw_buf_writeh(dev, buf, offset, 0x8010);
+                    offset+=2;
+                    /* share-2 : AP generate challenge text */
+                    for (i=0; i<128; i++)
+                    {
+                        wd->ap.challengeText[i] = (u8_t)zfGetRandomNumber(dev, 0);
+                    }
+                    zfCopyToIntTxBuffer(dev, buf, wd->ap.challengeText, offset, 128);
+                    offset += 128;
+                }
+            }
+            else if (p1 == 0x30001)
+            {
+                /* share-3 : STA return challenge Text */
+                zfCopyToIntTxBuffer(dev, buf, wd->sta.challengeText, offset, wd->sta.challengeText[1]+2);
+                offset += (wd->sta.challengeText[1]+2);
+            }
+
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_ASOCREQ :
+        case ZM_WLAN_FRAME_TYPE_REASOCREQ :
+            /* Capability */
+            zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[0]);
+            zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[1]);
+
+            /* Listen Interval */
+            zmw_tx_buf_writeh(dev, buf, offset, 0x0005);
+            offset+=2;
+
+            /* Reassocaited Request : Current AP address */
+            if (frameType == ZM_WLAN_FRAME_TYPE_REASOCREQ)
+            {
+            zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[0]);
+                offset+=2;
+            zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[1]);
+                offset+=2;
+            zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[2]);
+                offset+=2;
+            }
+
+            /* SSID */
+            offset = zfStaAddIeSsid(dev, buf, offset);
+
+
+            if ( wd->sta.currentFrequency < 3000 )
+            {
+                /* Support Rate */
+                offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+            }
+            else
+            {
+                /* Support Rate */
+                offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+            }
+
+            if ((wd->sta.capability[1] & ZM_BIT_0) == 1)
+            {   //spectrum managment flag enable
+                offset = zfStaAddIePowerCap(dev, buf, offset);
+                offset = zfStaAddIeSupportCh(dev, buf, offset);
+            }
+
+            if (wd->sta.currentFrequency < 3000)
+            {
+                /* Extended Supported Rates */
+                if (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N))
+                {
+                    offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+                }
+            }
+
+
+            //offset = zfStaAddIeWpaRsn(dev, buf, offset, frameType);
+            //Move to wrapper function, for OS difference--CWYang(m)
+            //for windows wrapper, zfwStaAddIeWpaRsn() should be below:
+            //u16_t zfwStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType)
+            //{
+            //    return zfStaAddIeWpaRsn(dev, buf, offset, frameType);
+            //}
+            offset = zfwStaAddIeWpaRsn(dev, buf, offset, frameType);
+
+#ifdef ZM_ENABLE_CENC
+            /* CENC */
+            //if (wd->sta.encryMode == ZM_CENC)
+            offset = zfStaAddIeCenc(dev, buf, offset);
+#endif //ZM_ENABLE_CENC
+            if (((wd->sta.wmeEnabled & ZM_STA_WME_ENABLE_BIT) != 0) //WME enabled
+              && ((wd->sta.apWmeCapability & 0x1) != 0)) //WME AP
+            {
+                if (((wd->sta.apWmeCapability & 0x80) != 0) //UAPSD AP
+                 && ((wd->sta.wmeEnabled & ZM_STA_UAPSD_ENABLE_BIT) != 0)) //UAPSD enabled
+                {
+                    offset = zfStaAddIeWmeInfo(dev, buf, offset, wd->sta.wmeQosInfo);
+                }
+                else
+                {
+                    offset = zfStaAddIeWmeInfo(dev, buf, offset, 0);
+                }
+            }
+            // jhlee HT 0
+            //CWYang(+)
+            if (wd->sta.EnableHT != 0)
+            {
+                #ifndef ZM_DISABLE_AMSDU8K_SUPPORT
+                    //Support 8K A-MSDU
+                    if (wd->sta.wepStatus == ZM_ENCRYPTION_WEP_DISABLED)
+                    {
+                        wd->sta.HTCap.Data.HtCapInfo |= HTCAP_MaxAMSDULength;
+                    }
+                    else
+                    {
+                        wd->sta.HTCap.Data.HtCapInfo &= (~HTCAP_MaxAMSDULength);
+                    }
+                #else
+                    //Support 4K A-MSDU
+                    wd->sta.HTCap.Data.HtCapInfo &= (~HTCAP_MaxAMSDULength);
+                #endif
+
+                /* HT Capabilities Info */
+                if (wd->BandWidth40 == 1) {
+                    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_SupChannelWidthSet;
+                }
+                else {
+                    wd->sta.HTCap.Data.HtCapInfo &= ~HTCAP_SupChannelWidthSet;
+                    //wd->sta.HTCap.Data.HtCapInfo |= HTCAP_SupChannelWidthSet;
+                }
+
+                wd->sta.HTCap.Data.AMPDUParam &= ~HTCAP_MaxRxAMPDU3;
+                wd->sta.HTCap.Data.AMPDUParam |= HTCAP_MaxRxAMPDU3;
+                wd->sta.HTCap.Data.MCSSet[1] = 0xFF; // MCS 8 ~ 15
+                offset = zfMmAddHTCapability(dev, buf, offset);
+                offset = zfMmAddPreNHTCapability(dev, buf, offset);
+                //CWYang(+)
+                /* Extended HT Capabilities Info */
+                //offset = zfMmAddExtendedHTCapability(dev, buf, offset);
+            }
+
+
+            //Store asoc request frame body, for VISTA only
+            wd->sta.asocReqFrameBodySize = ((offset - hlen) >
+                    ZM_CACHED_FRAMEBODY_SIZE)?
+                    ZM_CACHED_FRAMEBODY_SIZE:(offset - hlen);
+            for (i=0; i<wd->sta.asocReqFrameBodySize; i++)
+            {
+                wd->sta.asocReqFrameBody[i] = zmw_tx_buf_readb(dev, buf, i + hlen);
+            }
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_ASOCRSP :
+        case ZM_WLAN_FRAME_TYPE_REASOCRSP :
+            vap = (u16_t) p3;
+
+            /* Capability */
+            zmw_tx_buf_writeh(dev, buf, offset, wd->ap.capab[vap]);
+            offset+=2;
+
+            /* Status Code */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)p1);
+            offset+=2;
+
+            /* AID */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)(p2|0xc000));
+            offset+=2;
+
+
+            if ( wd->frequency < 3000 )
+            {
+            /* Support Rate */
+            offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+
+            /* Extended Supported Rates */
+            offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+            }
+            else
+            {
+                /* Support Rate */
+                offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+            }
+
+
+
+            /* WME Parameters */
+            if (wd->wlanMode == ZM_MODE_AP)
+            {
+                /* TODO : if WME STA then send WME parameter element */
+                if (wd->ap.qosMode == 1)
+                {
+                    offset = zfApAddIeWmePara(dev, buf, offset, vap);
+                }
+            }
+            // jhlee HT 0
+            //CWYang(+)
+            /* HT Capabilities Info */
+            offset = zfMmAddHTCapability(dev, buf, offset);
+            //CWYang(+)
+            /* Extended HT Capabilities Info */
+            offset = zfMmAddExtendedHTCapability(dev, buf, offset);
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_ATIM :
+            /* NULL frame */
+            /* TODO : add two dumb bytes temporarily */
+            offset += 2;
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_QOS_NULL :
+            zmw_buf_writeh(dev, buf, offset, 0x0010);
+            offset += 2;
+            break;
+
+        case ZM_WLAN_DATA_FRAME :
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_DISASOC :
+        case ZM_WLAN_FRAME_TYPE_DEAUTH :
+            if (wd->wlanMode == ZM_MODE_AP)
+            {
+              vap = (u16_t) p3;
+
+              if ((aid = zfApFindSta(dev, dst)) != 0xffff)
+              {
+                  zmw_enter_critical_section(dev);
+                  /* Clear STA table */
+                  wd->ap.staTable[aid].valid = 0;
+
+                  zmw_leave_critical_section(dev);
+
+                  if (wd->zfcbDisAsocNotify != NULL)
+                  {
+                      wd->zfcbDisAsocNotify(dev, (u8_t*)dst, vap);
+                  }
+              }
+            }
+            /* Reason Code */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)p1);
+            offset+=2;
+            break;
+    }
+
+    zfwBufSetSize(dev, buf, offset);
+
+    zm_msg2_mm(ZM_LV_2, "management frame body size=", offset-hlen);
+
+    //Copy wlan header
+    zfTxGenMmHeader(dev, frameType, dst, header, offset-hlen, buf, vap, encrypt);
+    for (i=0; i<(hlen>>1); i++)
+    {
+        zmw_tx_buf_writeh(dev, buf, i*2, header[i]);
+    }
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    zm_msg2_mm(ZM_LV_2, "offset=", offset);
+    zm_msg2_mm(ZM_LV_2, "hlen=", hlen);
+    //zm_msg2_mm(ZM_LV_2, "addrTblSize=", addrTblSize);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.len[0]=", addrTbl.len[0]);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.physAddrl[0]=", addrTbl.physAddrl[0]);
+    //zm_msg2_mm(ZM_LV_2, "buf->data=", buf->data);
+
+    #if 0
+    if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+    #else
+    zfPutVmmq(dev, buf);
+    zfPushVtxq(dev);
+    #endif
+
+    return;
+#if 0
+zlError:
+
+    zfwBufFree(dev, buf, 0);
+    return;
+#endif
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessManagement         */
+/*      Process received management frame.                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : received management frame buffer                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfProcessManagement(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo) //CWYang(m)
+{
+    u8_t frameType;
+    u16_t ta[3];
+    u16_t ra[3];
+    u16_t vap = 0, index = 0;
+    //u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    ra[0] = zmw_rx_buf_readh(dev, buf, 4);
+    ra[1] = zmw_rx_buf_readh(dev, buf, 6);
+    ra[2] = zmw_rx_buf_readh(dev, buf, 8);
+
+    ta[0] = zmw_rx_buf_readh(dev, buf, 10);
+    ta[1] = zmw_rx_buf_readh(dev, buf, 12);
+    ta[2] = zmw_rx_buf_readh(dev, buf, 14);
+
+    frameType = zmw_rx_buf_readb(dev, buf, 0);
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+#if 1
+        vap = 0;
+        if ((ra[0] & 0x1) != 1)
+        {
+            /* AP : Find virtual AP */
+            if ((index = zfApFindSta(dev, ta)) != 0xffff)
+            {
+                vap = wd->ap.staTable[index].vap;
+            }
+        }
+        zm_msg2_mm(ZM_LV_2, "vap=", vap);
+#endif
+
+        /* Dispatch by frame type */
+        switch (frameType)
+        {
+                /* Beacon */
+            case ZM_WLAN_FRAME_TYPE_BEACON :
+                zfApProcessBeacon(dev, buf);
+                break;
+                /* Authentication */
+            case ZM_WLAN_FRAME_TYPE_AUTH :
+                zfApProcessAuth(dev, buf, ta, vap);
+                break;
+                /* Association request */
+            case ZM_WLAN_FRAME_TYPE_ASOCREQ :
+                /* Reassociation request */
+            case ZM_WLAN_FRAME_TYPE_REASOCREQ :
+                zfApProcessAsocReq(dev, buf, ta, vap);
+                break;
+                /* Association response */
+            case ZM_WLAN_FRAME_TYPE_ASOCRSP :
+                //zfApProcessAsocRsp(dev, buf);
+                break;
+                /* Deauthentication */
+            case ZM_WLAN_FRAME_TYPE_DEAUTH :
+                zfApProcessDeauth(dev, buf, ta, vap);
+                break;
+                /* Disassociation */
+            case ZM_WLAN_FRAME_TYPE_DISASOC :
+                zfApProcessDisasoc(dev, buf, ta, vap);
+                break;
+                /* Probe request */
+            case ZM_WLAN_FRAME_TYPE_PROBEREQ :
+                zfProcessProbeReq(dev, buf, ta);
+                break;
+                /* Probe response */
+            case ZM_WLAN_FRAME_TYPE_PROBERSP :
+                zfApProcessProbeRsp(dev, buf, AddInfo);
+                break;
+                /* Action */
+            case ZM_WLAN_FRAME_TYPE_ACTION :
+                zfApProcessAction(dev, buf);
+                break;
+        }
+    }
+    else //if ((wd->wlanMode == ZM_MODE_INFRASTRUCTURE) || (wd->wlanMode == ZM_MODE_IBSS))
+    {
+        /* Dispatch by frame type */
+        switch (frameType)
+        {
+                /* Beacon */
+            case ZM_WLAN_FRAME_TYPE_BEACON :
+                /* if enable 802.11h and current chanel is silent but receive beacon from other AP */
+                if (((wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
+                        & ZM_REG_FLAG_CHANNEL_CSA) != 0) && wd->sta.DFSEnable)
+                {
+                    wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
+                            &= ~(ZM_REG_FLAG_CHANNEL_CSA & ZM_REG_FLAG_CHANNEL_PASSIVE);
+                }
+                zfStaProcessBeacon(dev, buf, AddInfo); //CWYang(m)
+                break;
+                /* Authentication */
+            case ZM_WLAN_FRAME_TYPE_AUTH :
+                /* TODO : vap parameter is useless in STA mode, get rid of it */
+                zfStaProcessAuth(dev, buf, ta, 0);
+                break;
+                /* Association request */
+            case ZM_WLAN_FRAME_TYPE_ASOCREQ :
+                /* TODO : vap parameter is useless in STA mode, get rid of it */
+                zfStaProcessAsocReq(dev, buf, ta, 0);
+                break;
+                /* Association response */
+            case ZM_WLAN_FRAME_TYPE_ASOCRSP :
+                /* Reassociation request */
+            case ZM_WLAN_FRAME_TYPE_REASOCRSP :
+                zfStaProcessAsocRsp(dev, buf);
+                break;
+                /* Deauthentication */
+            case ZM_WLAN_FRAME_TYPE_DEAUTH :
+                zm_debug_msg0("Deauthentication received");
+                zfStaProcessDeauth(dev, buf);
+                break;
+                /* Disassociation */
+            case ZM_WLAN_FRAME_TYPE_DISASOC :
+                zm_debug_msg0("Disassociation received");
+                zfStaProcessDisasoc(dev, buf);
+                break;
+                /* Probe request */
+            case ZM_WLAN_FRAME_TYPE_PROBEREQ :
+                zfProcessProbeReq(dev, buf, ta);
+                break;
+                /* Probe response */
+            case ZM_WLAN_FRAME_TYPE_PROBERSP :
+                /* if enable 802.11h and current chanel is silent but receive probe response from other AP */
+                if (((wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
+                        & ZM_REG_FLAG_CHANNEL_CSA) != 0) && wd->sta.DFSEnable)
+                {
+                    wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
+                            &= ~(ZM_REG_FLAG_CHANNEL_CSA & ZM_REG_FLAG_CHANNEL_PASSIVE);
+                }
+                zfStaProcessProbeRsp(dev, buf, AddInfo);
+                break;
+
+            case ZM_WLAN_FRAME_TYPE_ATIM:
+                zfStaProcessAtim(dev, buf);
+                break;
+                /* Action */
+            case ZM_WLAN_FRAME_TYPE_ACTION :
+                zm_msg0_mm(ZM_LV_2, "ProcessActionMgtFrame");
+                zfStaProcessAction(dev, buf);
+                break;
+        }
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessProbeReq           */
+/*      Process probe request management frame.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : auth frame buffer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfProcessProbeReq(zdev_t* dev, zbuf_t* buf, u16_t* src)
+{
+    u16_t offset;
+    u8_t len;
+    u16_t i, j;
+    u8_t ch;
+    u16_t sendFlag;
+
+    zmw_get_wlan_dev(dev);
+
+    /* check mode : AP/IBSS */
+    if ((wd->wlanMode != ZM_MODE_AP) && (wd->wlanMode != ZM_MODE_IBSS))
+    {
+        zm_msg0_mm(ZM_LV_3, "Ignore probe req");
+        return;
+    }
+
+    if ((wd->wlanMode != ZM_MODE_AP) && (wd->sta.adapterState == ZM_STA_STATE_DISCONNECT))
+    {
+        zm_msg0_mm(ZM_LV_3, "Packets dropped due to disconnect state");
+        return;
+    }
+
+    if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBERSP, src, 0, 0, 0);
+
+        return;
+    }
+
+    /* check SSID */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID)) == 0xffff)
+    {
+        zm_msg0_mm(ZM_LV_3, "probe req SSID not found");
+        return;
+    }
+
+    len = zmw_rx_buf_readb(dev, buf, offset+1);
+
+    for (i=0; i<ZM_MAX_AP_SUPPORT; i++)
+    {
+        if ((wd->ap.apBitmap & (1<<i)) != 0)
+        {
+            zm_msg1_mm(ZM_LV_3, "len=", len);
+            sendFlag = 0;
+            /* boardcast SSID */
+            if (len == 0)
+            {
+                if (wd->ap.hideSsid[i] == 0)
+                {
+                    sendFlag = 1;
+                }
+            }
+            /* Not broadcast SSID */
+            else if (wd->ap.ssidLen[i] == len)
+            {
+                for (j=0; j<len; j++)
+                {
+                    if ((ch = zmw_rx_buf_readb(dev, buf, offset+2+j))
+                            != wd->ap.ssid[i][j])
+                    {
+                        break;
+                    }
+                }
+                if (j == len)
+                {
+                    sendFlag = 1;
+                }
+            }
+            if (sendFlag == 1)
+            {
+                /* Send probe response */
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBERSP, src, i, 0, i);
+            }
+        }
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessProbeRsp           */
+/*      Process probe response management frame.                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : auth frame buffer                                         */
+/*  AddInfo : Rx Header and Rx Mac Status                               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Aress Yang          ZyDAS Technology Corporation    2006.11     */
+/*                                                                      */
+/************************************************************************/
+void zfProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo)
+{
+    /* Gather scan result */
+    /* Parse TIM and send PS-POLL in power saving mode */
+    struct zsWlanProbeRspFrameHeader*  pProbeRspHeader;
+    struct zsBssInfo* pBssInfo;
+    u8_t   pBuf[sizeof(struct zsWlanProbeRspFrameHeader)];
+    int    res;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zfCopyFromRxBuffer(dev, buf, pBuf, 0,
+                       sizeof(struct zsWlanProbeRspFrameHeader));
+    pProbeRspHeader = (struct zsWlanProbeRspFrameHeader*) pBuf;
+
+    zmw_enter_critical_section(dev);
+
+    //zm_debug_msg1("bss count = ", wd->sta.bssList.bssCount);
+
+    pBssInfo = zfStaFindBssInfo(dev, buf, pProbeRspHeader);
+
+    //if ( i == wd->sta.bssList.bssCount )
+    if ( pBssInfo == NULL )
+    {
+        /* Allocate a new entry if BSS not in the scan list */
+        pBssInfo = zfBssInfoAllocate(dev);
+        if (pBssInfo != NULL)
+        {
+            res = zfStaInitBssInfo(dev, buf, pProbeRspHeader, pBssInfo, AddInfo, 0);
+            //zfDumpSSID(pBssInfo->ssid[1], &(pBssInfo->ssid[2]));
+            if ( res != 0 )
+            {
+                zfBssInfoFree(dev, pBssInfo);
+            }
+            else
+            {
+                zfBssInfoInsertToList(dev, pBssInfo);
+            }
+        }
+    }
+    else
+    {
+        res = zfStaInitBssInfo(dev, buf, pProbeRspHeader, pBssInfo, AddInfo, 1);
+        if (res == 2)
+        {
+            zfBssInfoRemoveFromList(dev, pBssInfo);
+            zfBssInfoFree(dev, pBssInfo);
+        }
+        else if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            int idx;
+
+            // It would reset the alive counter if the peer station is found!
+            zfStaFindFreeOpposite(dev, (u16_t *)pBssInfo->macaddr, &idx);
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfSendProbeReq              */
+/*      Send probe request management frame.                            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Ji-Huang Lee        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+
+u16_t zfSendProbeReq(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t bWithSSID)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    /* SSID */
+    if (bWithSSID == 0)  /* broadcast ssid */
+    {
+        //zmw_leave_critical_section(dev);
+        zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+        zmw_tx_buf_writeb(dev, buf, offset++, 0);   /* length = 0 */
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+        if (wd->ws.probingSsidList[bWithSSID-1].ssidLen == 0)
+        {
+            zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+            zmw_tx_buf_writeb(dev, buf, offset++, 0);   /* length = 0 */
+        }
+        else
+        {
+            zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+            zmw_tx_buf_writeb(dev, buf, offset++,
+                    wd->ws.probingSsidList[bWithSSID-1].ssidLen);
+            zfCopyToIntTxBuffer(dev, buf,
+                    wd->ws.probingSsidList[bWithSSID-1].ssid,
+                    offset,
+                    wd->ws.probingSsidList[bWithSSID-1].ssidLen); /* ssid */
+            offset += wd->ws.probingSsidList[bWithSSID-1].ssidLen;
+        }
+        zmw_leave_critical_section(dev);
+    }
+
+    /* Supported rates */
+    if ( wd->sta.currentFrequency < 3000 )
+    {   /* 802.11b+g */
+        offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                      ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+
+        if (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) {
+            if (wd->wlanMode == ZM_MODE_IBSS) {
+                if (wd->wfc.bIbssGMode) {
+                    offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                      ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+                }
+            } else {
+                offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                      ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+            }
+        }
+    }
+    else
+    {  /* 802.11a */
+        offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                      ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+    }
+
+    return offset;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfUpdateDefaultQosParameter */
+/*      Update TxQs CWMIN, CWMAX, AIFS and TXO to WME default value.    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      mode : 0=>STA, 1=>AP                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+void zfUpdateDefaultQosParameter(zdev_t* dev, u8_t mode)
+{
+    u16_t cwmin[5];
+    u16_t cwmax[5];
+    u16_t aifs[5];
+    u16_t txop[5];
+
+    /* WMM parameter for STA */
+    /* Best Effor */
+    cwmin[0] = 15;
+    cwmax[0] = 1023;
+    aifs[0] = 3 * 9 + 10;
+    txop[0] = 0;
+    /* Back Ground */
+    cwmin[1] = 15;
+    cwmax[1] = 1023;
+    aifs[1] = 7 * 9 + 10;
+    txop[1] = 0;
+    /* VIDEO */
+    cwmin[2] = 7;
+    cwmax[2] = 15;
+    aifs[2] = 2 * 9 + 10;
+    txop[2] = 94;
+    /* VOICE */
+    cwmin[3] = 3;
+    cwmax[3] = 7;
+    aifs[3] = 2 * 9 + 10;
+    txop[3] = 47;
+    /* Special TxQ */
+    cwmin[4] = 3;
+    cwmax[4] = 7;
+    aifs[4] = 2 * 9 + 10;
+    txop[4] = 0;
+
+    /* WMM parameter for AP */
+    if (mode == 1)
+    {
+        cwmax[0] = 63;
+        aifs[3] = 1 * 9 + 10;
+        aifs[4] = 1 * 9 + 10;
+    }
+    zfHpUpdateQosParameter(dev, cwmin, cwmax, aifs, txop);
+}
+
+u16_t zfFindATHExtCap(zdev_t* dev, zbuf_t* buf, u8_t type, u8_t subtype)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_WIFI_IE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x03)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x7f)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+5)) == type))
+
+            {
+                if ( subtype != 0xff )
+                {
+                    if ( (tmp = zmw_rx_buf_readb(dev, buf, offset+6)) == subtype  )
+                    {
+                        return offset;
+                    }
+                }
+                else
+                {
+                    return offset;
+                }
+            }
+        }
+
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+u16_t zfFindBrdcmMrvlRlnkExtCap(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_WIFI_IE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x10)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x18))
+
+            {
+                return offset;
+            }
+            else if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x50)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x43))
+
+            {
+                return offset;
+            }
+        }
+        else if ((id = zmw_rx_buf_readb(dev, buf, offset)) == 0x7F)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if ((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x01)
+
+            {
+                return offset;
+            }
+        }
+
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+u16_t zfFindMarvelExtCap(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_WIFI_IE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                  && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x50)
+                  && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x43))
+
+            {
+                return offset;
+            }
+        }
+
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+u16_t zfFindBroadcomExtCap(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+
+    /* Search loop */
+    while((offset+2) < bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_WIFI_IE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) > (bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if (elen == 0)
+            {
+                return 0xffff;
+            }
+
+            if ( ((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                 && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x10)
+                 && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x18) )
+            {
+                return offset;
+            }
+        }
+
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+
+        offset += (elen+2);
+    }
+
+    return 0xffff;
+}
+
+u16_t zfFindRlnkExtCap(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+
+    /* Search loop */
+    while((offset+2) < bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == 0x7F)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) > (bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if ((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x01)
+
+            {
+                return offset;
+            }
+        }
+
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+
+        offset += (elen+2);
+    }
+
+    return 0xffff;
+}
diff --git a/drivers/staging/otus/80211core/cmmap.c b/drivers/staging/otus/80211core/cmmap.c
new file mode 100644
index 000000000000..7f09fded459e
--- /dev/null
+++ b/drivers/staging/otus/80211core/cmmap.c
@@ -0,0 +1,2402 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : mm.c                                                  */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains common functions for handle AP             */
+/*      management frame.                                               */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "ratectrl.h"
+
+extern const u8_t zcUpToAc[];
+
+void zfMmApTimeTick(zdev_t* dev)
+{
+    u32_t now;
+    zmw_get_wlan_dev(dev);
+
+    //zm_debug_msg1("wd->wlanMode : ", wd->wlanMode);
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        /* => every 1.28 seconds */
+        /* AP : aging STA that does not active for wd->ap.staAgingTime    */
+        now = wd->tick & 0x7f;
+        if (now == 0x0)
+        {
+            zfApAgingSta(dev);
+        }
+        else if (now == 0x1f)
+        {
+            zfQueueAge(dev, wd->ap.uapsdQ, wd->tick, 10000);
+        }
+        /* AP : check (wd->ap.protectedObss) and (wd->ap.bStaAssociated)  */
+        /*      to enable NonErp and Protection mode                      */
+        else if (now == 0x3f)
+        {
+            //zfApProtctionMonitor(dev);
+        }
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApInitStaTbl              */
+/*      Init AP's station table.                                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfApInitStaTbl(zdev_t* dev)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+    {
+        wd->ap.staTable[i].valid = 0;
+        wd->ap.staTable[i].state = 0;
+        wd->ap.staTable[i].addr[0] = 0;
+        wd->ap.staTable[i].addr[1] = 0;
+        wd->ap.staTable[i].addr[2] = 0;
+        wd->ap.staTable[i].time = 0;
+        wd->ap.staTable[i].vap = 0;
+        wd->ap.staTable[i].encryMode = ZM_NO_WEP;
+    }
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApFindSta                 */
+/*      Find a STA in station table.                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : Target STA address                                       */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0xffff : fail                                                   */
+/*      other : STA table index                                         */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApFindSta(zdev_t* dev, u16_t* addr)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+    {
+        if (wd->ap.staTable[i].valid == 1)
+        {
+            if ((wd->ap.staTable[i].addr[0] == addr[0])
+                    && (wd->ap.staTable[i].addr[1] == addr[1])
+                    && (wd->ap.staTable[i].addr[2] == addr[2]))
+            {
+                return i;
+            }
+        }
+    }
+    return 0xffff;
+}
+
+u16_t zfApGetSTAInfo(zdev_t* dev, u16_t* addr, u16_t* state, u8_t* vap)
+{
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        *vap = wd->ap.staTable[id].vap;
+        *state = wd->ap.staTable[id++].state;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return id;
+}
+
+
+void zfApGetStaQosType(zdev_t* dev, u16_t* addr, u8_t* qosType)
+{
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        *qosType = wd->ap.staTable[id].qosType;
+    }
+    else
+    {
+        *qosType = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+
+void zfApGetStaTxRateAndQosType(zdev_t* dev, u16_t* addr, u32_t* phyCtrl,
+                                u8_t* qosType, u16_t* rcProbingFlag)
+{
+    u16_t id;
+    u8_t rate;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        rate = (u8_t)zfRateCtrlGetTxRate(dev, &wd->ap.staTable[id].rcCell, rcProbingFlag);
+#ifdef ZM_AP_DEBUG
+        //rate = 15;
+#endif
+        *phyCtrl = zcRateToPhyCtrl[rate];
+        *qosType = wd->ap.staTable[id].qosType;
+    }
+    else
+    {
+        if (wd->frequency < 3000)
+        {
+            /* CCK 1M */
+            //header[2] = 0x0f00;          //PHY control L
+            //header[3] = 0x0000;          //PHY control H
+            *phyCtrl = 0x00000F00;
+        }
+        else
+        {
+            /* CCK 6M */
+            //header[2] = 0x0f01;          //PHY control L
+            //header[3] = 0x000B;          //PHY control H
+            *phyCtrl = 0x000B0F01;
+        }
+        *qosType = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zm_msg2_mm(ZM_LV_3, "PhyCtrl=", *phyCtrl);
+    return;
+}
+
+void zfApGetStaEncryType(zdev_t* dev, u16_t* addr, u8_t* encryType)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        *encryType = wd->ap.staTable[id].encryMode;
+    }
+    else
+    {
+        *encryType = ZM_NO_WEP;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zm_msg2_mm(ZM_LV_3, "encyrType=", *encryType);
+    return;
+}
+
+void zfApGetStaWpaIv(zdev_t* dev, u16_t* addr, u16_t* iv16, u32_t* iv32)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        *iv16 = wd->ap.staTable[id].iv16;
+        *iv32 = wd->ap.staTable[id].iv32;
+    }
+    else
+    {
+        *iv16 = 0;
+        *iv32 = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zm_msg2_mm(ZM_LV_3, "iv16=", *iv16);
+    zm_msg2_mm(ZM_LV_3, "iv32=", *iv32);
+    return;
+}
+
+void zfApSetStaWpaIv(zdev_t* dev, u16_t* addr, u16_t iv16, u32_t iv32)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        wd->ap.staTable[id].iv16 = iv16;
+        wd->ap.staTable[id].iv32 = iv32;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zm_msg2_mm(ZM_LV_3, "iv16=", iv16);
+    zm_msg2_mm(ZM_LV_3, "iv32=", iv32);
+    return;
+}
+
+void zfApClearStaKey(zdev_t* dev, u16_t* addr)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t bcAddr[3] = { 0xffff, 0xffff, 0xffff };
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    if (zfMemoryIsEqual((u8_t*)bcAddr, (u8_t*)addr, sizeof(bcAddr)) == TRUE)
+    {
+        /* Turn off group key information */
+    //    zfClearKey(dev, 0);
+    }
+    else
+    {
+        zmw_declare_for_critical_section();
+
+        zmw_enter_critical_section(dev);
+
+        if ((id = zfApFindSta(dev, addr)) != 0xffff)
+        {
+            /* Turn off STA's key information */
+            zfHpRemoveKey(dev, id+1);
+
+            /* Update STA's Encryption Type */
+            wd->ap.staTable[id].encryMode = ZM_NO_WEP;
+        }
+        else
+        {
+            zm_msg0_mm(ZM_LV_3, "Can't find STA address\n");
+        }
+        zmw_leave_critical_section(dev);
+    }
+}
+
+#ifdef ZM_ENABLE_CENC
+void zfApGetStaCencIvAndKeyIdx(zdev_t* dev, u16_t* addr, u32_t *iv, u8_t *keyIdx)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t id;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        *iv++ = wd->ap.staTable[id].txiv[0];
+        *iv++ = wd->ap.staTable[id].txiv[1];
+        *iv++ = wd->ap.staTable[id].txiv[2];
+        *iv = wd->ap.staTable[id].txiv[3];
+        *keyIdx = wd->ap.staTable[id].cencKeyIdx;
+    }
+    else
+    {
+        *iv++ = 0x5c365c37;
+        *iv++ = 0x5c365c36;
+        *iv++ = 0x5c365c36;
+        *iv = 0x5c365c36;
+        *keyIdx = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+void zfApSetStaCencIv(zdev_t* dev, u16_t* addr, u32_t *iv)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t id;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        wd->ap.staTable[id].txiv[0] = *iv++;
+        wd->ap.staTable[id].txiv[1] = *iv++;
+        wd->ap.staTable[id].txiv[2] = *iv++;
+        wd->ap.staTable[id].txiv[3] = *iv;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+#endif //ZM_ENABLE_CENC
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApFlushBufferedPsFrame    */
+/*      Free buffered PS frames.                                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfApFlushBufferedPsFrame(zdev_t* dev)
+{
+    u16_t emptyFlag;
+    u16_t freeCount;
+    u16_t vap;
+    zbuf_t* psBuf = NULL;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    freeCount = 0;
+    emptyFlag = 0;
+    while (1)
+    {
+        psBuf = NULL;
+        zmw_enter_critical_section(dev);
+        if (wd->ap.uniHead != wd->ap.uniTail)
+        {
+            psBuf = wd->ap.uniArray[wd->ap.uniHead];
+            wd->ap.uniHead = (wd->ap.uniHead + 1) & (ZM_UNI_ARRAY_SIZE - 1);
+        }
+        else
+        {
+            emptyFlag = 1;
+        }
+        zmw_leave_critical_section(dev);
+
+        if (psBuf != NULL)
+        {
+            zfwBufFree(dev, psBuf, ZM_ERR_FLUSH_PS_QUEUE);
+        }
+        zm_assert(freeCount++ < (ZM_UNI_ARRAY_SIZE*2));
+
+        if (emptyFlag != 0)
+        {
+            break;
+        }
+    }
+
+    for (vap=0; vap<ZM_MAX_AP_SUPPORT; vap++)
+    {
+        freeCount = 0;
+        emptyFlag = 0;
+        while (1)
+        {
+            psBuf = NULL;
+            zmw_enter_critical_section(dev);
+            if (wd->ap.bcmcHead[vap] != wd->ap.bcmcTail[vap])
+            {
+                psBuf = wd->ap.bcmcArray[vap][wd->ap.bcmcHead[vap]];
+                wd->ap.bcmcHead[vap] = (wd->ap.bcmcHead[vap] + 1)
+                        & (ZM_BCMC_ARRAY_SIZE - 1);
+            }
+            else
+            {
+                emptyFlag = 1;
+            }
+            zmw_leave_critical_section(dev);
+
+            if (psBuf != NULL)
+            {
+                zfwBufFree(dev, psBuf, ZM_ERR_FLUSH_PS_QUEUE);
+            }
+            zm_assert(freeCount++ < (ZM_BCMC_ARRAY_SIZE*2));
+
+            if (emptyFlag != 0)
+            {
+                break;
+            }
+        }
+    }
+    return;
+}
+
+
+u16_t zfApBufferPsFrame(zdev_t* dev, zbuf_t* buf, u16_t port)
+{
+    u16_t id;
+    u16_t addr[3];
+    u16_t vap = 0;
+    u8_t up;
+    u16_t fragOff;
+    u8_t ac;
+    u16_t ret;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if (port < ZM_MAX_AP_SUPPORT)
+    {
+        vap = port;
+    }
+
+    addr[0] = zmw_rx_buf_readh(dev, buf, 0);
+    addr[1] = zmw_rx_buf_readh(dev, buf, 2);
+    addr[2] = zmw_rx_buf_readh(dev, buf, 4);
+
+    if ((addr[0] & 0x1) == 0x1)
+    {
+        if (wd->ap.staPowerSaving > 0)
+        {
+            zmw_enter_critical_section(dev);
+
+            /* Buffer this BC or MC frame */
+            if (((wd->ap.bcmcTail[vap]+1)&(ZM_BCMC_ARRAY_SIZE-1))
+                    != wd->ap.bcmcHead[vap])
+            {
+                wd->ap.bcmcArray[vap][wd->ap.bcmcTail[vap]++] = buf;
+                wd->ap.bcmcTail[vap] &= (ZM_BCMC_ARRAY_SIZE-1);
+                zmw_leave_critical_section(dev);
+
+                zm_msg0_tx(ZM_LV_0, "Buffer BCMC");
+            }
+            else
+            {
+                /* bcmcArray full */
+                zmw_leave_critical_section(dev);
+
+                zm_msg0_tx(ZM_LV_0, "BCMC buffer full");
+
+                /* free buffer according to buffer type */
+                zfwBufFree(dev, buf, ZM_ERR_BCMC_PS_BUFFER_UNAVAILABLE);
+            }
+            return 1;
+        }
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+
+        if ((id = zfApFindSta(dev, addr)) != 0xffff)
+        {
+            if (wd->ap.staTable[id].psMode == 1)
+            {
+
+                zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+                ac = zcUpToAc[up&0x7] & 0x3;
+
+                if ((wd->ap.staTable[id].qosType == 1) &&
+                        ((wd->ap.staTable[id].qosInfo & (0x8>>ac)) != 0))
+                {
+                    ret = zfQueuePutNcs(dev, wd->ap.uapsdQ, buf, wd->tick);
+                    zmw_leave_critical_section(dev);
+                    if (ret != ZM_SUCCESS)
+                    {
+                        zfwBufFree(dev, buf, ZM_ERR_AP_UAPSD_QUEUE_FULL);
+                    }
+                }
+                else
+                {
+                /* Buffer this unicast frame */
+                if (((wd->ap.uniTail+1)&(ZM_UNI_ARRAY_SIZE-1))
+                        != wd->ap.uniHead)
+                {
+                    wd->ap.uniArray[wd->ap.uniTail++] = buf;
+                    wd->ap.uniTail &= (ZM_UNI_ARRAY_SIZE-1);
+                    zmw_leave_critical_section(dev);
+                    zm_msg0_tx(ZM_LV_0, "Buffer UNI");
+
+                }
+                else
+                {
+                    /* uniArray full */
+                    zmw_leave_critical_section(dev);
+                    zm_msg0_tx(ZM_LV_0, "UNI buffer full");
+                    /* free buffer according to buffer type */
+                    zfwBufFree(dev, buf, ZM_ERR_UNI_PS_BUFFER_UNAVAILABLE);
+                }
+                }
+                return 1;
+            } /* if (wd->ap.staTable[id++].psMode == 1) */
+        } /* if ((id = zfApFindSta(dev, addr)) != 0xffff) */
+        zmw_leave_critical_section(dev);
+    }
+
+    return 0;
+}
+
+u16_t zfApGetSTAInfoAndUpdatePs(zdev_t* dev, u16_t* addr, u16_t* state,
+                                u8_t* vap, u16_t psMode, u8_t* uapsdTrig)
+{
+    u16_t id;
+    u8_t uapsdStaAwake = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+#ifdef ZM_AP_DEBUG
+    //psMode=0;
+#endif
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        if (psMode != 0)
+        {
+            zm_msg0_mm(ZM_LV_0, "psMode = 1");
+            if (wd->ap.staTable[id].psMode == 0)
+            {
+                wd->ap.staPowerSaving++;
+            }
+            else
+            {
+                if (wd->ap.staTable[id].qosType == 1)
+                {
+                    zm_msg0_mm(ZM_LV_0, "UAPSD trigger");
+                    *uapsdTrig = wd->ap.staTable[id].qosInfo;
+                }
+            }
+        }
+        else
+        {
+            if (wd->ap.staTable[id].psMode != 0)
+            {
+                wd->ap.staPowerSaving--;
+                if ((wd->ap.staTable[id].qosType == 1) && ((wd->ap.staTable[id].qosInfo&0xf)!=0))
+                {
+                    uapsdStaAwake = 1;
+                }
+            }
+        }
+
+        wd->ap.staTable[id].psMode = (u8_t) psMode;
+        wd->ap.staTable[id].time = wd->tick;
+        *vap = wd->ap.staTable[id].vap;
+        *state = wd->ap.staTable[id++].state;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (uapsdStaAwake == 1)
+    {
+        zbuf_t* psBuf;
+        u8_t mb;
+
+        while (1)
+        {
+            if ((psBuf = zfQueueGetWithMac(dev, wd->ap.uapsdQ, (u8_t*)addr, &mb)) != NULL)
+            {
+                zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+            }
+            else
+            {
+                break;
+            }
+        }
+    }
+
+    return id;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApGetNewSta               */
+/*      Get a new STA from station table.                               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0xffff : fail                                                   */
+/*      other : STA table index                                         */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApGetNewSta(zdev_t* dev)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+    {
+        if (wd->ap.staTable[i].valid == 0)
+        {
+            zm_msg2_mm(ZM_LV_0, "zfApGetNewSta=", i);
+            return i;
+        }
+    }
+    return 0xffff;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApAddSta                  */
+/*      Add a STA to station table.                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : STA MAC address                                          */
+/*      state : STA state                                               */
+/*      apId : Virtual AP ID                                            */
+/*      type : 0=>11b, 1=>11g                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0xffff : fail                                                   */
+/*      Other : index                                                   */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApAddSta(zdev_t* dev, u16_t* addr, u16_t state, u16_t apId, u8_t type,
+                 u8_t qosType, u8_t qosInfo)
+{
+    u16_t index;
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zm_msg1_mm(ZM_LV_0, "STA type=", type);
+
+    zmw_enter_critical_section(dev);
+
+    if ((index = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        zm_msg0_mm(ZM_LV_2, "found");
+        /* Update STA state */
+        if ((state == ZM_STATE_AUTH) || (state == ZM_STATE_PREAUTH))
+        {
+            wd->ap.staTable[index].state = state;
+            wd->ap.staTable[index].time = wd->tick;
+            wd->ap.staTable[index].vap = (u8_t)apId;
+        }
+        else if (state == ZM_STATE_ASOC)
+        {
+            if ((wd->ap.staTable[index].state == ZM_STATE_AUTH))
+                    //&& (wd->ap.staTable[index].vap == apId))
+            {
+                wd->ap.staTable[index].state = state;
+                wd->ap.staTable[index].time = wd->tick;
+                wd->ap.staTable[index].qosType = qosType;
+                wd->ap.staTable[index].vap = (u8_t)apId;
+                wd->ap.staTable[index].staType = type;
+                wd->ap.staTable[index].qosInfo = qosInfo;
+
+                if (wd->frequency < 3000)
+                {
+                    /* Init 11b/g */
+                    zfRateCtrlInitCell(dev, &wd->ap.staTable[index].rcCell, type, 1, 1);
+                }
+                else
+                {
+                    /* Init 11a */
+                    zfRateCtrlInitCell(dev, &wd->ap.staTable[index].rcCell, type, 0, 1);
+                }
+
+                if (wd->zfcbApConnectNotify != NULL)
+                {
+                    wd->zfcbApConnectNotify(dev, (u8_t*)addr, apId);
+                }
+            }
+            else
+            {
+                index = 0xffff;
+            }
+        }
+    }
+    else
+    {
+        zm_msg0_mm(ZM_LV_2, "Not found");
+        if ((state == ZM_STATE_AUTH) || (state == ZM_STATE_PREAUTH))
+        {
+            /* Get a new STA and update state */
+            index = zfApGetNewSta(dev);
+            zm_msg2_mm(ZM_LV_1, "new STA index=", index);
+
+            if (index != 0xffff)
+            {
+                for (i=0; i<3; i++)
+                {
+                    wd->ap.staTable[index].addr[i] = addr[i];
+                }
+                wd->ap.staTable[index].state = state;
+                wd->ap.staTable[index].valid = 1;
+                wd->ap.staTable[index].time = wd->tick;
+                wd->ap.staTable[index].vap = (u8_t)apId;
+                wd->ap.staTable[index].encryMode = ZM_NO_WEP;
+            }
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return index;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApAgingSta                */
+/*      Aging STA in station table.                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      number of 11b STA in STA table                                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfApAgingSta(zdev_t* dev)
+{
+    u16_t i;
+    u32_t deltaMs;
+    u16_t addr[3];
+    u16_t txFlag;
+    u16_t psStaCount = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    wd->ap.gStaAssociated = wd->ap.bStaAssociated = 0;
+
+    for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+    {
+        txFlag = 0;
+        zmw_enter_critical_section(dev);
+        if (wd->ap.staTable[i].valid == 1)
+        {
+            addr[0] = wd->ap.staTable[i].addr[0];
+            addr[1] = wd->ap.staTable[i].addr[1];
+            addr[2] = wd->ap.staTable[i].addr[2];
+            /* millisecond */
+            deltaMs = (u32_t)((u32_t)wd->tick-(u32_t)wd->ap.staTable[i].time)
+                      * ZM_MS_PER_TICK;
+
+            /* preauth */
+            if ((wd->ap.staTable[i].state == ZM_STATE_PREAUTH)
+                    && (deltaMs > ZM_PREAUTH_TIMEOUT_MS))
+            {
+                /* Aging STA */
+                wd->ap.staTable[i].valid = 0;
+                wd->ap.authSharing = 0;
+                txFlag = 1;
+            }
+
+            /* auth */
+            if ((wd->ap.staTable[i].state == ZM_STATE_AUTH)
+                    && (deltaMs > ZM_AUTH_TIMEOUT_MS))
+            {
+                /* Aging STA */
+                wd->ap.staTable[i].valid = 0;
+                txFlag = 1;
+            }
+
+            /* asoc */
+            if (wd->ap.staTable[i].state == ZM_STATE_ASOC)
+            {
+                if (wd->ap.staTable[i].psMode != 0)
+                {
+                    psStaCount++;
+                }
+
+                if (deltaMs > ((u32_t)wd->ap.staAgingTimeSec<<10))
+                {
+                    /* Aging STA */
+                    zm_msg1_mm(ZM_LV_0, "Age STA index=", i);
+                    wd->ap.staTable[i].valid = 0;
+                    txFlag = 1;
+                }
+                else if (deltaMs > ((u32_t)wd->ap.staProbingTimeSec<<10))
+                {
+                    if (wd->ap.staTable[i].psMode == 0)
+                    {
+                        /* Probing non-PS STA */
+                        zm_msg1_mm(ZM_LV_0, "Probing STA index=", i);
+                        wd->ap.staTable[i].time +=
+                                (wd->ap.staProbingTimeSec * ZM_TICK_PER_SECOND);
+                        txFlag = 2;
+                    }
+                }
+            }
+
+
+        }
+        zmw_leave_critical_section(dev);
+
+        if (txFlag == 1)
+        {
+            /* Send deauthentication management frame */
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, addr, 4, 0, 0);
+        }
+        else if (txFlag == 2)
+        {
+            zfSendMmFrame(dev, ZM_WLAN_DATA_FRAME, addr, 0, 0, 0);
+        }
+
+    }
+
+    wd->ap.staPowerSaving = psStaCount;
+
+    return;
+}
+
+void zfApProtctionMonitor(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* 11b STA associated => nonErp, Protect */
+    if (wd->ap.bStaAssociated > 0)
+    {
+        /* Enable NonErp bit in information element */
+        wd->erpElement = ZM_WLAN_NON_ERP_PRESENT_BIT
+                         | ZM_WLAN_USE_PROTECTION_BIT;
+
+        /* Enable protection mode */
+        zfApSetProtectionMode(dev, 1);
+
+    }
+    /* 11b STA not associated, protection OBSS present => Protect */
+    else if (wd->ap.protectedObss > 2) //Threshold
+    {
+        if (wd->disableSelfCts == 0)
+        {
+            /* Disable NonErp bit in information element */
+            wd->erpElement = ZM_WLAN_USE_PROTECTION_BIT;
+
+            /* Enable protection mode */
+            zfApSetProtectionMode(dev, 1);
+        }
+    }
+    else
+    {
+        /* Disable NonErp bit in information element */
+        wd->erpElement = 0;
+
+        /* Disable protection mode */
+        zfApSetProtectionMode(dev, 0);
+    }
+    wd->ap.protectedObss = 0;
+}
+
+
+void zfApProcessBeacon(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t offset;
+    u8_t ch;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_msg0_mm(ZM_LV_3, "Rx beacon");
+
+    /* update Non-ERP flag(wd->ap.nonErpObss) */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_ERP)) == 0xffff)
+    {
+        /* 11b OBSS */
+        wd->ap.protectedObss++;
+        return;
+    }
+
+    ch = zmw_rx_buf_readb(dev, buf, offset+2);
+    if ((ch & ZM_WLAN_USE_PROTECTION_BIT) == ZM_WLAN_USE_PROTECTION_BIT)
+    {
+        /* Protected OBSS */
+        wd->ap.protectedObss = 1;
+    }
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessAuth               */
+/*      Process authenticate management frame.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : auth frame buffer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+/* Note : AP allows one authenticating STA at a time, does not          */
+/*        support multiple authentication process. Make sure            */
+/*        authentication state machine will not be blocked due          */
+/*        to incompleted authentication handshake.                      */
+void zfApProcessAuth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+    u16_t algo, seq, status;
+    u8_t authSharing;
+    u16_t ret;
+    u16_t i;
+    u8_t challengePassed = 0;
+    u8_t frameCtrl;
+    u32_t retAlgoSeq;
+    u32_t retStatus;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    frameCtrl = zmw_rx_buf_readb(dev, buf, 1);
+    /* AP : Auth share 3 */
+    /* shift for WEP IV */
+    if ((frameCtrl & 0x40) != 0)
+    {
+        algo = zmw_rx_buf_readh(dev, buf, 28);
+        seq = zmw_rx_buf_readh(dev, buf, 30);
+        status = zmw_rx_buf_readh(dev, buf, 32);
+    }
+    else
+    {
+        algo = zmw_rx_buf_readh(dev, buf, 24);
+        seq = zmw_rx_buf_readh(dev, buf, 26);
+        status = zmw_rx_buf_readh(dev, buf, 28);
+    }
+
+    zm_msg2_mm(ZM_LV_0, "Rx Auth, seq=", seq);
+
+    /* Set default to authentication algorithm not support */
+    retAlgoSeq = 0x20000 | algo;
+    retStatus = 13; /* authentication algorithm not support */
+
+    /* AP : Auth open 1 */
+    if (algo == 0)
+    {
+        if (wd->ap.authAlgo[apId] == 0)
+        {
+            retAlgoSeq = 0x20000;
+            if (seq == 1)
+            {
+                /* AP : update STA to auth */
+                if ((ret = zfApAddSta(dev, src, ZM_STATE_AUTH, apId, 0, 0, 0)) != 0xffff)
+                {
+                    /* AP : call zfwAuthNotify() for host to judge */
+                    //zfwAuthNotify(dev, src);
+
+                    /* AP : response Auth seq=2, success */
+                    retStatus = 0;
+
+                }
+                else
+                {
+                    /* AP : response Auth seq=2, unspecific error */
+                    retStatus = 1;
+                }
+            }
+            else
+            {
+                /* AP : response Auth seq=2, sequence number out of expected */
+                retStatus = 14;
+            }
+        }
+    }
+    /* AP : Auth share 1 */
+    else if (algo == 1)
+    {
+        if (wd->ap.authAlgo[apId] == 1)
+        {
+            if (seq == 1)
+            {
+                retAlgoSeq = 0x20001;
+
+                /* critical section */
+                zmw_enter_critical_section(dev);
+                if (wd->ap.authSharing == 1)
+                {
+                    authSharing = 1;
+                }
+                else
+                {
+                    authSharing = 0;
+                    wd->ap.authSharing = 1;
+                }
+                /* end of critical section */
+                zmw_leave_critical_section(dev);
+
+                if (authSharing == 1)
+                {
+                    /* AP : response Auth seq=2, status = fail */
+                    retStatus = 1;
+                }
+                else
+                {
+                    /* AP : update STA to preauth */
+                    zfApAddSta(dev, src, ZM_STATE_PREAUTH, apId, 0, 0, 0);
+
+                    /* AP : call zfwAuthNotify() for host to judge */
+                    //zfwAuthNotify(dev, src);
+
+                    /* AP : response Auth seq=2 */
+                    retStatus = 0;
+                }
+            }
+            else if (seq == 3)
+            {
+                retAlgoSeq = 0x40001;
+
+                if (wd->ap.authSharing == 1)
+                {
+                    /* check challenge text */
+                    if (zmw_buf_readh(dev, buf, 30+4) == 0x8010)
+                    {
+                        for (i=0; i<128; i++)
+                        {
+                            if (wd->ap.challengeText[i]
+                                        != zmw_buf_readb(dev, buf, 32+i+4))
+                            {
+                                break;
+                            }
+                        }
+                        if (i == 128)
+                        {
+                            challengePassed = 1;
+                        }
+                    }
+
+                    if (challengePassed == 1)
+                    {
+                        /* AP : update STA to auth */
+                        zfApAddSta(dev, src, ZM_STATE_AUTH, apId, 0, 0, 0);
+
+                        /* AP : response Auth seq=2 */
+                        retStatus = 0;
+                    }
+                    else
+                    {
+                        /* AP : response Auth seq=2, challenge failure */
+                        retStatus = 15;
+
+                        /* TODO : delete STA */
+                    }
+
+                    wd->ap.authSharing = 0;
+                }
+            }
+            else
+            {
+                retAlgoSeq = 0x40001;
+                retStatus = 14;
+            }
+        }
+    }
+
+    zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_AUTH, src, retAlgoSeq,
+            retStatus, apId);
+    return;
+}
+
+void zfApProcessAsocReq(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+    u16_t aid = 0xffff;
+    u8_t frameType;
+    u16_t offset;
+    u8_t staType = 0;
+    u8_t qosType = 0;
+    u8_t qosInfo = 0;
+    u8_t tmp;
+    u16_t i, j, k;
+    u16_t encMode = 0;
+
+    zmw_get_wlan_dev(dev);
+    /* AP : check SSID */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID)) != 0xffff)
+    {
+        k = 0;
+        for (j = 0; j < wd->ap.vapNumber; j++)
+        {
+            if ((tmp = zmw_buf_readb(dev, buf, offset+1))
+                        != wd->ap.ssidLen[j])
+            {
+                k++;
+            }
+        }
+        if (k == wd->ap.vapNumber)
+        {
+            goto zlDeauth;
+        }
+
+        k = 0;
+        for (j = 0; j < wd->ap.vapNumber; j++)
+        {
+            for (i=0; i<wd->ap.ssidLen[j]; i++)
+            {
+                if ((tmp = zmw_buf_readb(dev, buf, offset+2+i))
+                        != wd->ap.ssid[j][i])
+                {
+                    break;
+                }
+            }
+            if (i == wd->ap.ssidLen[j])
+            {
+                apId = j;
+            }
+            else
+            {
+                k++;
+            }
+        }
+        if (k == wd->ap.vapNumber)
+        {
+            goto zlDeauth;
+        }
+    }
+
+    /* TODO : check capability */
+
+    /* AP : check support rate */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_RATE)) != 0xffff)
+    {
+        /* 11g STA */
+        staType = 1;
+    }
+    //CWYang(+)
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_HT_CAPABILITY)) != 0xffff)
+    {
+        /* 11n STA */
+        staType = 2;
+    }
+
+    /* TODO : do not allow 11b STA to associated in Pure G mode */
+    if (wd->ap.wlanType[apId] == ZM_WLAN_TYPE_PURE_G && staType == 0)
+    {
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, src, 3, 0, 0);
+        return;
+    }
+
+    /* In pure B mode, we set G STA into B mode */
+    if (wd->ap.wlanType[apId] == ZM_WLAN_TYPE_PURE_B && staType == 1)
+    {
+        staType = 0;
+    }
+
+    /* AP : check 11i and WPA */
+    /* AP : check 11h */
+
+    /* AP : check WME */
+    if ((offset = zfFindWifiElement(dev, buf, 2, 0)) != 0xffff)
+    {
+        /* WME STA */
+        qosType = 1;
+        zm_msg0_mm(ZM_LV_0, "WME STA");
+
+        if (wd->ap.uapsdEnabled != 0)
+        {
+            qosInfo = zmw_rx_buf_readb(dev, buf, offset+8);
+        }
+    }
+
+    if (wd->ap.wpaSupport[apId] == 1)
+    {
+        if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_WPA_IE)) != 0xffff )
+        {
+            /* get WPA IE */
+            u8_t length = zmw_rx_buf_readb(dev, buf, offset+1);
+            if (length+2 < ZM_MAX_WPAIE_SIZE)
+            {
+                zfCopyFromRxBuffer(dev, buf, wd->ap.stawpaIe[apId], offset, length+2);
+                wd->ap.stawpaLen[apId] = length+2;
+                encMode = 1;
+
+
+                zm_msg1_mm(ZM_LV_0, "WPA Mode zfwAsocNotify, apId=", apId);
+
+                /* AP : Call zfwAsocNotify() */
+                if (wd->zfcbAsocNotify != NULL)
+                {
+                    wd->zfcbAsocNotify(dev, src, wd->ap.stawpaIe[apId], wd->ap.stawpaLen[apId], apId);
+                }
+            }
+            else
+            {
+                goto zlDeauth;
+            }
+        }
+        else if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_RSN_IE)) != 0xffff )
+        {
+            /* get RSN IE */
+            u8_t length = zmw_rx_buf_readb(dev, buf, offset+1);
+            if (length+2 < ZM_MAX_WPAIE_SIZE)
+            {
+                zfCopyFromRxBuffer(dev, buf, wd->ap.stawpaIe[apId], offset, length+2);
+                wd->ap.stawpaLen[apId] = length+2;
+                encMode = 1;
+
+                zm_msg1_mm(ZM_LV_0, "RSN Mode zfwAsocNotify, apId=", apId);
+
+                /* AP : Call zfwAsocNotify() */
+                if (wd->zfcbAsocNotify != NULL)
+                {
+                    wd->zfcbAsocNotify(dev, src, wd->ap.stawpaIe[apId], wd->ap.stawpaLen[apId], apId);
+                }
+            }
+            else
+            {
+                goto zlDeauth;
+            }
+        }
+#ifdef ZM_ENABLE_CENC
+        else if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_CENC_IE)) != 0xffff )
+        {
+            /* get CENC IE */
+            u8_t length = zmw_rx_buf_readb(dev, buf, offset+1);
+
+            if (length+2 < ZM_MAX_WPAIE_SIZE)
+            {
+                zfCopyFromRxBuffer(dev, buf, wd->ap.stawpaIe[apId], offset, length+2);
+                wd->ap.stawpaLen[apId] = length+2;
+                encMode = 1;
+
+                zm_msg1_mm(ZM_LV_0, "CENC Mode zfwAsocNotify, apId=", apId);
+
+                /* AP : Call zfwAsocNotify() */
+                if (wd->zfcbCencAsocNotify != NULL)
+                {
+                    wd->zfcbCencAsocNotify(dev, src, wd->ap.stawpaIe[apId],
+                            wd->ap.stawpaLen[apId], apId);
+                }
+            }
+            else
+            {
+                goto zlDeauth;
+            }
+        }
+#endif //ZM_ENABLE_CENC
+        else
+        {   /* ap is encryption but sta has no wpa/rsn ie */
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, src, 6, 0, 0);
+            return;
+        }
+    }
+    /* sta has wpa/rsn ie but ap is no encryption */
+    if ((wd->ap.wpaSupport[apId] == 0) && (encMode == 1))
+    {
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, src, 6, 0, 0);
+        return;
+    }
+
+    /* AP : update STA to asoc */
+    aid = zfApAddSta(dev, src, ZM_STATE_ASOC, apId, staType, qosType, qosInfo);
+
+    zfApStoreAsocReqIe(dev, buf, aid);
+
+zlDeauth:
+    /* AP : send asoc rsp2 */
+    if (aid != 0xffff)
+    {
+        frameType = zmw_rx_buf_readb(dev, buf, 0);
+
+        if (frameType == ZM_WLAN_FRAME_TYPE_ASOCREQ)
+        {
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_ASOCRSP, src, 0, aid+1, apId);
+        }
+        else
+        {
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_REASOCRSP, src, 0, aid+1, apId);
+        }
+    }
+    else
+    {
+        /* TODO : send deauthentication */
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, src, 6, 0, 0);
+    }
+
+    return;
+}
+
+void zfApStoreAsocReqIe(zdev_t* dev, zbuf_t* buf, u16_t aid)
+{
+    //struct zsWlanAssoFrameHeader* pAssoFrame;
+    //u8_t  pBuf[sizeof(struct zsWlanAssoFrameHeader)];
+    u16_t offset;
+    u32_t i;
+    u16_t length;
+    u8_t  *htcap;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<wd->sta.asocRspFrameBodySize; i++)
+    {
+        wd->sta.asocRspFrameBody[i] = zmw_rx_buf_readb(dev, buf, i+24);
+    }
+    /* capability: 2 octets */
+    offset = 24;
+
+    /* Listen interval: 2 octets */
+    offset = 26;
+
+    /* SSID */
+    offset = 28;
+
+    /* supported rates */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_SUPPORT_RATE)) == 0xffff)
+        return;
+    length = zmw_rx_buf_readb(dev, buf, offset + 1);
+
+    /* extended supported rates */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_RATE)) == 0xffff)
+        return;
+    length = zmw_rx_buf_readb(dev, buf, offset + 1);
+
+    /* power capability:4 octets */
+    offset = offset + 2 + length;
+
+    /* supported channels: 4 octets */
+    offset = offset + 2 + 4;
+
+    /* RSN */
+
+    /* QoS */
+
+    /* HT capabilities: 28 octets */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_HT_CAPABILITY)) != 0xffff) {
+        /* atheros pre n */
+        htcap = (u8_t *)&wd->ap.ie[aid].HtCap;
+        htcap[0] = zmw_rx_buf_readb(dev, buf, offset);
+        htcap[1] = 26;
+        for (i=1; i<=26; i++)
+        {
+            htcap[i+1] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_debug_msg2("ASOC:  HT Capabilities, htcap=", htcap[i+1]);
+        }
+        return;
+    }
+    else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTCAPABILITY)) != 0xffff) {
+        /* pre n 2.0 standard */
+        htcap = (u8_t *)&wd->ap.ie[aid].HtCap;
+        for (i=0; i<28; i++)
+        {
+            htcap[i] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_debug_msg2("ASOC:  HT Capabilities, htcap=", htcap[i]);
+        }
+    }
+    else {
+        /* not 11n AP */
+        return;
+    }
+
+
+    /* supported regulatory classes */
+    offset = offset + length;
+    //length = zmw_rx_buf_readb(dev, buf, offset + 1);
+    {
+    u8_t *htcap;
+    htcap = (u8_t *)&wd->sta.ie.HtInfo;
+    //zm_debug_msg2("ASOC:  HT Capabilities info=", ((u16_t *)htcap)[1]);
+    //zm_debug_msg2("ASOC:  A-MPDU parameters=", htcap[4]);
+    //zm_debug_msg2("ASOC:  Supported MCS set=", ((u32_t *)htcap)[1]>>8);
+    }
+
+}
+
+void zfApProcessAsocRsp(zdev_t* dev, zbuf_t* buf)
+{
+
+}
+
+void zfApProcessDeauth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+    u16_t aid;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    /* AP : if SA=associated STA then deauthenticate STA */
+    if ((aid = zfApFindSta(dev, src)) != 0xffff)
+    {
+        /* Clear STA table */
+        wd->ap.staTable[aid].valid = 0;
+        if (wd->zfcbDisAsocNotify != NULL)
+        {
+            wd->zfcbDisAsocNotify(dev, (u8_t*)src, apId);
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+}
+
+void zfApProcessDisasoc(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+    u16_t aid;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    /* AP : if SA=associated STA then deauthenticate STA */
+    if ((aid = zfApFindSta(dev, src)) != 0xffff)
+    {
+        /* Clear STA table */
+        wd->ap.staTable[aid].valid = 0;
+        zmw_leave_critical_section(dev);
+        if (wd->zfcbDisAsocNotify != NULL)
+        {
+            wd->zfcbDisAsocNotify(dev, (u8_t*)src, apId);
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+}
+
+
+void zfApProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo)
+{
+#if 0
+    zmw_get_wlan_dev(dev);
+
+    zm_msg0_mm(ZM_LV_0, "Rx probersp");
+
+    /* Gather scan result */
+
+    //zm_debug_msg1("bssList Count = ", wd->sta.bssList.bssCount);
+    /* return if not in scanning */
+    if ((wd->heartBeatNotification & ZM_BSSID_LIST_SCAN)
+            != ZM_BSSID_LIST_SCAN)
+    {
+        return;
+    }
+
+    //if ( wd->sta.pUpdateBssList->bssCount == ZM_MAX_BSS )
+    if ( wd->sta.bssList.bssCount == ZM_MAX_BSS )
+    {
+        return;
+    }
+
+    zfProcessProbeRsp(dev, buf, AddInfo);
+
+#endif
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApAddIeSsid               */
+/*      Add AP information element SSID to buffer.                      */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*      vap : virtual AP ID                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApAddIeSsid(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t vap)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->ap.ssidLen[vap]);
+
+    /* Information : SSID */
+    for (i=0; i<wd->ap.ssidLen[vap]; i++)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, wd->ap.ssid[vap][i]);
+    }
+
+    return offset;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApAddIeTim                */
+/*      Add AP information element TIM to buffer.                       */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*      vap : virtual AP ID                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApAddIeTim(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t vap)
+{
+    u8_t uniBitMap[9];
+    u16_t highestByte;
+    u16_t i;
+    u16_t lenOffset;
+    u16_t id;
+    u16_t dst[3];
+    u16_t aid;
+    u16_t bitPosition;
+    u16_t bytePosition;
+    zbuf_t* psBuf;
+    zbuf_t* tmpBufArray[ZM_UNI_ARRAY_SIZE];
+    u16_t tmpBufArraySize = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_TIM);
+
+    /* offset of Element Length */
+    lenOffset = offset++;
+
+    /* Information : TIM */
+    /* DTIM count */
+    /* TODO : Doesn't work for Virtual AP's case */
+    wd->CurrentDtimCount++;
+    if (wd->CurrentDtimCount >= wd->dtim)
+    {
+        wd->CurrentDtimCount = 0;
+    }
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->CurrentDtimCount);
+    /* DTIM period */
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->dtim);
+    /* bitmap offset */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0);
+
+    /* Update BCMC bit */
+    if (wd->CurrentDtimCount == 0)
+    {
+        zmw_enter_critical_section(dev);
+        wd->ap.timBcmcBit[vap] = (wd->ap.bcmcTail[vap]!=wd->ap.bcmcHead[vap])?1:0;
+        zmw_leave_critical_section(dev);
+    }
+    else
+    {
+        wd->ap.timBcmcBit[vap] = 0;
+    }
+
+    /* Update Unicast bitmap */
+    /* reset bit map */
+    for (i=0; i<9; i++)
+    {
+        uniBitMap[i] = 0;
+    }
+    highestByte = 0;
+#if 1
+
+    zmw_enter_critical_section(dev);
+
+    id = wd->ap.uniHead;
+    while (id != wd->ap.uniTail)
+    {
+        psBuf = wd->ap.uniArray[id];
+
+        /* TODO : Aging PS frame after queuing for more than 10 seconds */
+
+        /* get destination STA's aid */
+        dst[0] = zmw_tx_buf_readh(dev, psBuf, 0);
+        dst[1] = zmw_tx_buf_readh(dev, psBuf, 2);
+        dst[2] = zmw_tx_buf_readh(dev, psBuf, 4);
+        if ((aid = zfApFindSta(dev, dst)) != 0xffff)
+        {
+            if (wd->ap.staTable[aid].psMode != 0)
+            {
+                zm_msg1_mm(ZM_LV_0, "aid=",aid);
+                aid++;
+                zm_assert(aid<=64);
+                bitPosition = (1 << (aid & 0x7));
+                bytePosition = (aid >> 3);
+                uniBitMap[bytePosition] |= bitPosition;
+
+                if (bytePosition>highestByte)
+                {
+                    highestByte = bytePosition;
+                }
+                id = (id+1) & (ZM_UNI_ARRAY_SIZE-1);
+            }
+            else
+            {
+                zm_msg0_mm(ZM_LV_0, "Send PS frame which STA no longer in PS mode");
+                /* Send PS frame which STA no longer in PS mode */
+                zfApRemoveFromPsQueue(dev, id, dst);
+                tmpBufArray[tmpBufArraySize++] = psBuf;
+            }
+        }
+        else
+        {
+            zm_msg0_mm(ZM_LV_0, "Free garbage PS frame");
+            /* Free garbage PS frame */
+            zfApRemoveFromPsQueue(dev, id, dst);
+            zfwBufFree(dev, psBuf, 0);
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+#endif
+
+    zfQueueGenerateUapsdTim(dev, wd->ap.uapsdQ, uniBitMap, &highestByte);
+
+    zm_msg1_mm(ZM_LV_3, "bm=",uniBitMap[0]);
+    zm_msg1_mm(ZM_LV_3, "highestByte=",highestByte);
+    zm_msg1_mm(ZM_LV_3, "timBcmcBit[]=",wd->ap.timBcmcBit[vap]);
+
+    /* bitmap */
+    zmw_tx_buf_writeb(dev, buf, offset++,
+                         uniBitMap[0] | wd->ap.timBcmcBit[vap]);
+    for (i=0; i<highestByte; i++)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, uniBitMap[i+1]);
+    }
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, lenOffset, highestByte+4);
+
+    for (i=0; i<tmpBufArraySize; i++)
+    {
+        /* Put to VTXQ[ac] */
+        zfPutVtxq(dev, tmpBufArray[i]);
+    }
+    /* Push VTXQ[ac] */
+    zfPushVtxq(dev);
+
+    return offset;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApRemoveFromPsQueue       */
+/*      Remove zbuf from PS queue.                                      */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      id : index in ps queue                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      more data bit                                                   */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfApRemoveFromPsQueue(zdev_t* dev, u16_t id, u16_t* addr)
+{
+    u16_t dst[3];
+    u16_t nid;
+    u8_t moreData = 0;
+    zmw_get_wlan_dev(dev);
+
+    wd->ap.uniTail = (wd->ap.uniTail-1) & (ZM_UNI_ARRAY_SIZE-1);
+    while (id != wd->ap.uniTail)
+    {
+        nid = (id + 1) & (ZM_UNI_ARRAY_SIZE - 1);
+        wd->ap.uniArray[id] = wd->ap.uniArray[nid];
+
+        /* Search until tail to config more data bit */
+        dst[0] = zmw_buf_readh(dev, wd->ap.uniArray[id], 0);
+        dst[1] = zmw_buf_readh(dev, wd->ap.uniArray[id], 2);
+        dst[2] = zmw_buf_readh(dev, wd->ap.uniArray[id], 4);
+        if ((addr[0] == dst[0]) && (addr[1] == dst[1])
+                && (addr[2] == dst[2]))
+        {
+            moreData = 0x20;
+        }
+
+        id = nid;
+    }
+    return moreData;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApAddIeWmePara            */
+/*      Add WME Parameter Element to buffer.                            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*      vap : virtual AP ID                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.1      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApAddIeWmePara(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t vap)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WIFI_IE);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 24);
+
+    /* OUI */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x50);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0xF2);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x02);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x01);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x01);
+
+    /* QoS Info */
+    if (wd->ap.uapsdEnabled)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, 0x81);
+    }
+    else
+    {
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x01);
+    }
+
+    /* Reserved */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+
+    /* Best Effort AC parameters */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x03);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0xA4);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    /* Backfround AC parameters */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x27);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0xA4);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    /* Video AC parameters */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x42);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x43);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x5E);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    /* Voice AC parameters */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x62);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x32);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x2F);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+
+    return offset;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApSendBeacon              */
+/*      Sned AP mode beacon.                                            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+void zfApSendBeacon(zdev_t* dev)
+{
+    zbuf_t* buf;
+    u16_t offset;
+    u16_t vap;
+    u16_t seq;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    wd->ap.beaconCounter++;
+    if (wd->ap.beaconCounter >= wd->ap.vapNumber)
+    {
+        wd->ap.beaconCounter = 0;
+    }
+    vap = wd->ap.beaconCounter;
+
+
+    zm_msg1_mm(ZM_LV_2, "Send beacon, vap=", vap);
+
+    /* TBD : Maximum size of beacon */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc beacon buf Fail!");
+        return;
+    }
+
+    offset = 0;
+
+    /* wlan header */
+    /* Frame control */
+    zmw_tx_buf_writeh(dev, buf, offset, 0x0080);
+    offset+=2;
+    /* Duration */
+    zmw_tx_buf_writeh(dev, buf, offset, 0x0000);
+    offset+=2;
+    /* Address 1 */
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    /* Address 2 */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[0]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[1]);
+    offset+=2;
+#ifdef ZM_VAPMODE_MULTILE_SSID
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[2]); //Multiple SSID
+#else
+    zmw_tx_buf_writeh(dev, buf, offset, (wd->macAddr[2]+(vap<<8))); //VAP
+#endif
+    offset+=2;
+    /* Address 3 */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[0]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[1]);
+    offset+=2;
+#ifdef ZM_VAPMODE_MULTILE_SSID
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[2]); //Multiple SSID
+#else
+    zmw_tx_buf_writeh(dev, buf, offset, (wd->macAddr[2]+(vap<<8))); //VAP
+#endif
+    offset+=2;
+
+    /* Sequence number */
+    zmw_enter_critical_section(dev);
+    seq = ((wd->mmseq++)<<4);
+    zmw_leave_critical_section(dev);
+    zmw_tx_buf_writeh(dev, buf, offset, seq);
+    offset+=2;
+
+    /* 24-31 Time Stamp : hardware will fill this field */
+    zmw_tx_buf_writeh(dev, buf, offset, 0);
+    zmw_tx_buf_writeh(dev, buf, offset+2, 0);
+    zmw_tx_buf_writeh(dev, buf, offset+4, 0);
+    zmw_tx_buf_writeh(dev, buf, offset+6, 0);
+    offset+=8;
+
+    /* Beacon Interval */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->beaconInterval);
+    offset+=2;
+
+    /* Capability */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->ap.capab[vap]);
+    offset+=2;
+
+    /* SSID */
+    if (wd->ap.hideSsid[vap] == 0)
+    {
+        offset = zfApAddIeSsid(dev, buf, offset, vap);
+    }
+    else
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+        zmw_tx_buf_writeb(dev, buf, offset++, 0);
+
+    }
+
+    /* Support Rate */
+    if ( wd->frequency < 3000 )
+    {
+    offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                  ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+    }
+    else
+    {
+        offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                  ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+    }
+
+    /* DS parameter set */
+    offset = zfMmAddIeDs(dev, buf, offset);
+
+    /* TIM */
+    offset = zfApAddIeTim(dev, buf, offset, vap);
+
+    /* If WLAN Type is not PURE B */
+    if (wd->ap.wlanType[vap] != ZM_WLAN_TYPE_PURE_B)
+    {
+        if ( wd->frequency < 3000 )
+        {
+        /* ERP Information */
+        offset = zfMmAddIeErp(dev, buf, offset);
+
+        /* Extended Supported Rates */
+        offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                      ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+    }
+    }
+
+    /* TODO : country information */
+    /* TODO : RSN */
+    if (wd->ap.wpaSupport[vap] == 1)
+    {
+        offset = zfMmAddIeWpa(dev, buf, offset, vap);
+    }
+
+    /* WME Parameters */
+    if (wd->ap.qosMode == 1)
+    {
+        offset = zfApAddIeWmePara(dev, buf, offset, vap);
+    }
+
+    /* HT Capabilities Info */
+    offset = zfMmAddHTCapability(dev, buf, offset);
+
+    /* Extended HT Capabilities Info */
+    offset = zfMmAddExtendedHTCapability(dev, buf, offset);
+
+    /* 1212 : write to beacon fifo */
+    /* 1221 : write to share memory */
+    zfHpSendBeacon(dev, buf, offset);
+
+    /* Free beacon buffer */
+    /* TODO: In order to fit the madwifi beacon architecture, we need to
+       free beacon buffer in the HAL layer.
+     */
+
+    //zfwBufFree(dev, buf, 0);
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfIntrabssForward           */
+/*      Called to transmit intra-BSS frame from upper layer.            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      vap : virtual AP                                                */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      1 : unicast intras-BSS frame                                    */
+/*      0 : other frames                                                */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfIntrabssForward(zdev_t* dev, zbuf_t* buf, u8_t srcVap)
+{
+    u16_t err;
+    u16_t asocFlag = 0;
+    u16_t dst[3];
+    u16_t aid;
+    u16_t staState;
+    zbuf_t* txBuf;
+    u16_t len;
+    u16_t i;
+    u16_t temp;
+    u16_t ret;
+    u8_t vap = 0;
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    dst[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET);
+    dst[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+2);
+    dst[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+4);
+#else
+    dst[0] = zmw_rx_buf_readh(dev, buf, 0);
+    dst[1] = zmw_rx_buf_readh(dev, buf, 2);
+    dst[2] = zmw_rx_buf_readh(dev, buf, 4);
+#endif  // ZM_ENABLE_NATIVE_WIFI
+
+    /* Do Intra-BSS forward(data copy) if necessary*/
+    if ((dst[0]&0x1) != 0x1)
+    {
+        aid = zfApGetSTAInfo(dev, dst, &staState, &vap);
+        if ((aid != 0xffff) && (staState == ZM_STATE_ASOC) && (srcVap == vap))
+        {
+            asocFlag = 1;
+            zm_msg0_rx(ZM_LV_2, "Intra-BSS forward : asoc STA");
+        }
+
+    }
+    else
+    {
+        vap = srcVap;
+        zm_msg0_rx(ZM_LV_2, "Intra-BSS forward : BCorMC");
+    }
+
+    /* destination address = associated STA or BC/MC */
+    if ((asocFlag == 1) || ((dst[0]&0x1) == 0x1))
+    {
+        /* Allocate frame */
+        if ((txBuf = zfwBufAllocate(dev, ZM_RX_FRAME_SIZE))
+                == NULL)
+        {
+            zm_msg0_rx(ZM_LV_1, "Alloc intra-bss buf Fail!");
+            goto zlAllocError;
+        }
+
+        /* Copy frame */
+        len = zfwBufGetSize(dev, buf);
+        for (i=0; i<len; i+=2)
+        {
+            temp = zmw_rx_buf_readh(dev, buf, i);
+            zmw_tx_buf_writeh(dev, txBuf, i, temp);
+        }
+        zfwBufSetSize(dev, txBuf, len);
+
+#ifdef ZM_ENABLE_NATIVE_WIFI
+        /* Tx-A2 = Rx-A1, Tx-A3 = Rx-A2, Tx-A1 = Rx-A3 */
+        for (i=0; i<6; i+=2)
+        {
+            temp = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+i);
+            zmw_tx_buf_writeh(dev, txBuf, ZM_WLAN_HEADER_A2_OFFSET+i, temp);
+            temp = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+i);
+            zmw_tx_buf_writeh(dev, txBuf, ZM_WLAN_HEADER_A3_OFFSET+i, temp);
+            temp = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+i);
+            zmw_tx_buf_writeh(dev, txBuf, ZM_WLAN_HEADER_A1_OFFSET+i, temp);
+        }
+
+        #endif
+
+        /* Transmit frame */
+        /* Return error if port is disabled */
+        if ((err = zfTxPortControl(dev, txBuf, vap)) == ZM_PORT_DISABLED)
+        {
+            err = ZM_ERR_TX_PORT_DISABLED;
+            goto zlTxError;
+        }
+
+#if 1
+        /* AP : Buffer frame for power saving STA */
+        if ((ret = zfApBufferPsFrame(dev, txBuf, vap)) == 0)
+        {
+            /* forward frame if not been buffered */
+            #if 1
+            /* Put to VTXQ[ac] */
+            ret = zfPutVtxq(dev, txBuf);
+            /* Push VTXQ[ac] */
+            zfPushVtxq(dev);
+            #else
+            zfTxSendEth(dev, txBuf, vap, ZM_INTERNAL_ALLOC_BUF, 0);
+            #endif
+
+        }
+#endif
+    }
+    return asocFlag;
+
+zlTxError:
+    zfwBufFree(dev, txBuf, 0);
+zlAllocError:
+    return asocFlag;
+}
+
+struct zsMicVar* zfApGetRxMicKey(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t sa[6];
+    u16_t id = 0, macAddr[3];
+
+    zmw_get_wlan_dev(dev);
+
+    zfCopyFromRxBuffer(dev, buf, sa, ZM_WLAN_HEADER_A2_OFFSET, 6);
+
+    macAddr[0] = sa[0] + (sa[1] << 8);
+    macAddr[1] = sa[2] + (sa[3] << 8);
+    macAddr[2] = sa[4] + (sa[5] << 8);
+
+    if ((id = zfApFindSta(dev, macAddr)) != 0xffff)
+        return (&wd->ap.staTable[id].rxMicKey);
+
+    return NULL;
+}
+
+struct zsMicVar* zfApGetTxMicKey(zdev_t* dev, zbuf_t* buf, u8_t* qosType)
+{
+    u8_t da[6];
+    u16_t id = 0, macAddr[3];
+
+    zmw_get_wlan_dev(dev);
+
+    zfCopyFromIntTxBuffer(dev, buf, da, 0, 6);
+
+    macAddr[0] = da[0] + (da[1] << 8);
+    macAddr[1] = da[2] + (da[3] << 8);
+    macAddr[2] = da[4] + (da[5] << 8);
+
+    if ((macAddr[0] & 0x1))
+    {
+        return (&wd->ap.bcMicKey[0]);
+    }
+    else if ((id = zfApFindSta(dev, macAddr)) != 0xffff)
+    {
+        *qosType = wd->ap.staTable[id].qosType;
+        return (&wd->ap.staTable[id].txMicKey);
+    }
+
+    return NULL;
+}
+
+u16_t zfApUpdatePsBit(zdev_t* dev, zbuf_t* buf, u8_t* vap, u8_t* uapsdTrig)
+{
+    u16_t staState;
+    u16_t aid;
+    u16_t psBit;
+    u16_t src[3];
+    u16_t dst[1];
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    src[0] = zmw_rx_buf_readh(dev, buf, 10);
+    src[1] = zmw_rx_buf_readh(dev, buf, 12);
+    src[2] = zmw_rx_buf_readh(dev, buf, 14);
+
+    if ((zmw_rx_buf_readb(dev, buf, 1) & 0x3) != 3)
+    {
+        /* AP */
+        dst[0] = zmw_rx_buf_readh(dev, buf, 4);
+
+        psBit = (zmw_rx_buf_readb(dev, buf, 1) & 0x10) >> 4;
+        /* Get AID and update STA PS mode */
+        aid = zfApGetSTAInfoAndUpdatePs(dev, src, &staState, vap, psBit, uapsdTrig);
+
+        /* if STA not associated, send deauth */
+        if ((aid == 0xffff) || (staState != ZM_STATE_ASOC))
+        {
+            if ((dst[0]&0x1)==0)
+            {
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, src, 0x7,
+                        0, 0);
+            }
+
+            return ZM_ERR_STA_NOT_ASSOCIATED;
+        }
+    } /* if ((zmw_rx_buf_readb(dev, buf, 1) & 0x3) != 3) */
+    else
+    {
+        /* WDS */
+        for (i=0; i<ZM_MAX_WDS_SUPPORT; i++)
+        {
+            if ((wd->ap.wds.wdsBitmap & (1<<i)) != 0)
+            {
+                if ((src[0] == wd->ap.wds.macAddr[i][0])
+                        && (src[1] == wd->ap.wds.macAddr[i][1])
+                        && (src[2] == wd->ap.wds.macAddr[i][2]))
+                {
+                    *vap = 0x20 + i;
+                    break;
+                }
+            }
+        }
+    }
+    return ZM_SUCCESS;
+}
+
+void zfApProcessPsPoll(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t src[3];
+    u16_t dst[3];
+    zbuf_t* psBuf = NULL;
+    u16_t id;
+    u8_t moreData = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    src[0] = zmw_tx_buf_readh(dev, buf, 10);
+    src[1] = zmw_tx_buf_readh(dev, buf, 12);
+    src[2] = zmw_tx_buf_readh(dev, buf, 14);
+
+    /* Find ps buffer for PsPoll */
+    zmw_enter_critical_section(dev);
+    id = wd->ap.uniHead;
+    while (id != wd->ap.uniTail)
+    {
+        psBuf = wd->ap.uniArray[id];
+
+        dst[0] = zmw_tx_buf_readh(dev, psBuf, 0);
+        dst[1] = zmw_tx_buf_readh(dev, psBuf, 2);
+        dst[2] = zmw_tx_buf_readh(dev, psBuf, 4);
+
+        if ((src[0] == dst[0]) && (src[1] == dst[1]) && (src[2] == dst[2]))
+        {
+            moreData = zfApRemoveFromPsQueue(dev, id, src);
+            break;
+        }
+        else
+        {
+            psBuf = NULL;
+        }
+        id = (id + 1) & (ZM_UNI_ARRAY_SIZE - 1);
+    }
+    zmw_leave_critical_section(dev);
+
+    /* Send ps buffer */
+    if (psBuf != NULL)
+    {
+        /* Send with more data bit */
+        zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF, moreData);
+    }
+
+    return;
+}
+
+void zfApSetProtectionMode(zdev_t* dev, u16_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (mode == 0)
+    {
+        if (wd->ap.protectionMode != mode)
+        {
+            /* Write MAC&PHY registers to disable protection */
+
+            wd->ap.protectionMode = mode;
+        }
+
+    }
+    else
+    {
+        if (wd->ap.protectionMode != mode)
+        {
+            /* Write MAC&PHY registers to enable protection */
+
+            wd->ap.protectionMode = mode;
+        }
+    }
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApSendFailure             */
+/*      Send failure.                                                   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : receiver address                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfApSendFailure(zdev_t* dev, u8_t* addr)
+{
+    u16_t id;
+    u16_t staAddr[3];
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    staAddr[0] = addr[0] + (((u16_t)addr[1])<<8);
+    staAddr[1] = addr[2] + (((u16_t)addr[3])<<8);
+    staAddr[2] = addr[4] + (((u16_t)addr[5])<<8);
+    zmw_enter_critical_section(dev);
+    if ((id = zfApFindSta(dev, staAddr)) != 0xffff)
+    {
+        /* Send failture : Add 3 minutes to inactive time that will */
+        /*                 will make STA been kicked out soon */
+        wd->ap.staTable[id].time -= (3*ZM_TICK_PER_MINUTE);
+    }
+    zmw_leave_critical_section(dev);
+}
+
+
+void zfApProcessAction(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t category;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    category = zmw_rx_buf_readb(dev, buf, 24);
+
+    switch (category)
+    {
+    case ZM_WLAN_BLOCK_ACK_ACTION_FRAME:
+        zfAggBlockAckActionFrame(dev, buf);
+        break;
+    default:
+        break;
+    }
+
+    return;
+}
diff --git a/drivers/staging/otus/80211core/cmmsta.c b/drivers/staging/otus/80211core/cmmsta.c
new file mode 100644
index 000000000000..c75ba11ee43d
--- /dev/null
+++ b/drivers/staging/otus/80211core/cmmsta.c
@@ -0,0 +1,5782 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+#include "ratectrl.h"
+#include "../hal/hpreg.h"
+
+/* TODO : change global variable to constant */
+u8_t   zgWpaRadiusOui[] = { 0x00, 0x50, 0xf2, 0x01 };
+u8_t   zgWpaAesOui[] = { 0x00, 0x50, 0xf2, 0x04 };
+u8_t   zgWpa2RadiusOui[] = { 0x00, 0x0f, 0xac, 0x01 };
+u8_t   zgWpa2AesOui[] = { 0x00, 0x0f, 0xac, 0x04 };
+
+const u16_t zcCwTlb[16] = {   0,    1,    3,    7,   15,   31,   63,  127,
+                            255,  511, 1023, 2047, 4095, 4095, 4095, 4095};
+
+void zfStaStartConnectCb(zdev_t* dev);
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaPutApIntoBlockingList  */
+/*      Put AP into blocking AP list.                                   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      bssid : AP's BSSID                                              */
+/*      weight : weight of AP                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+void zfStaPutApIntoBlockingList(zdev_t* dev, u8_t* bssid, u8_t weight)
+{
+    u16_t i, j;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if (weight > 0)
+    {
+        zmw_enter_critical_section(dev);
+        /*Find same bssid entry first*/
+        for (i=0; i<ZM_MAX_BLOCKING_AP_LIST_SIZE; i++)
+        {
+            for (j=0; j<6; j++)
+            {
+                if(wd->sta.blockingApList[i].addr[j]!= bssid[j])
+                {
+                    break;
+                }
+            }
+
+            if(j==6)
+            {
+                break;
+            }
+        }
+        /*This bssid doesn't have old record.Find an empty entry*/
+        if (i == ZM_MAX_BLOCKING_AP_LIST_SIZE)
+        {
+            for (i=0; i<ZM_MAX_BLOCKING_AP_LIST_SIZE; i++)
+            {
+                if (wd->sta.blockingApList[i].weight == 0)
+                {
+                    break;
+                }
+            }
+        }
+
+        /* If the list is full, pick one entry for replacement */
+        if (i == ZM_MAX_BLOCKING_AP_LIST_SIZE)
+        {
+            i = bssid[5] & (ZM_MAX_BLOCKING_AP_LIST_SIZE-1);
+        }
+
+        /* Update AP address and weight */
+        for (j=0; j<6; j++)
+        {
+            wd->sta.blockingApList[i].addr[j] = bssid[j];
+        }
+
+        wd->sta.blockingApList[i].weight = weight;
+        zmw_leave_critical_section(dev);
+    }
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaIsApInBlockingList     */
+/*      Is AP in blocking list.                                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      bssid : AP's BSSID                                              */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      TRUE : AP in blocking list                                      */
+/*      FALSE : AP not in blocking list                                 */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaIsApInBlockingList(zdev_t* dev, u8_t* bssid)
+{
+    u16_t i, j;
+    zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+
+    //zmw_enter_critical_section(dev);
+    for (i=0; i<ZM_MAX_BLOCKING_AP_LIST_SIZE; i++)
+    {
+        if (wd->sta.blockingApList[i].weight != 0)
+        {
+            for (j=0; j<6; j++)
+            {
+                if (wd->sta.blockingApList[i].addr[j] != bssid[j])
+                {
+                    break;
+                }
+            }
+            if (j == 6)
+            {
+                //zmw_leave_critical_section(dev);
+                return TRUE;
+            }
+        }
+    }
+    //zmw_leave_critical_section(dev);
+    return FALSE;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaRefreshBlockList       */
+/*      Is AP in blocking list.                                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      flushFlag : flush whole blocking list                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+void zfStaRefreshBlockList(zdev_t* dev, u16_t flushFlag)
+{
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    for (i=0; i<ZM_MAX_BLOCKING_AP_LIST_SIZE; i++)
+    {
+        if (wd->sta.blockingApList[i].weight != 0)
+        {
+            if (flushFlag != 0)
+            {
+                wd->sta.blockingApList[i].weight = 0;
+            }
+            else
+            {
+                wd->sta.blockingApList[i].weight--;
+            }
+        }
+    }
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaConnectFail            */
+/*      Handle Connect failure.                                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      bssid : BSSID                                                   */
+/*      reason : reason of failure                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+void zfStaConnectFail(zdev_t* dev, u16_t reason, u16_t* bssid, u8_t weight)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Change internal state */
+    zfChangeAdapterState(dev, ZM_STA_STATE_DISCONNECT);
+
+    /* Improve WEP/TKIP performace with HT AP, detail information please look bug#32495 */
+    //zfHpSetTTSIFSTime(dev, 0x8);
+
+    /* Notify wrapper of connection status changes */
+    if (wd->zfcbConnectNotify != NULL)
+    {
+        wd->zfcbConnectNotify(dev, reason, bssid);
+    }
+
+    /* Put AP into internal blocking list */
+    zfStaPutApIntoBlockingList(dev, (u8_t *)bssid, weight);
+
+    /* Issue another SCAN */
+    if ( wd->sta.bAutoReconnect )
+    {
+        zm_debug_msg0("Start internal scan...");
+        zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+        zfScanMgrScanStart(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+    }
+}
+
+u8_t zfiWlanIBSSGetPeerStationsCount(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.oppositeCount;
+}
+
+u8_t zfiWlanIBSSIteratePeerStations(zdev_t* dev, u8_t numToIterate, zfpIBSSIteratePeerStationCb callback, void *ctx)
+{
+    u8_t oppositeCount;
+    u8_t i;
+    u8_t index = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    oppositeCount = wd->sta.oppositeCount;
+    if ( oppositeCount > numToIterate )
+    {
+        oppositeCount = numToIterate;
+    }
+
+    for(i=0; i < ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        if ( oppositeCount == 0 )
+        {
+            break;
+        }
+
+        if ( wd->sta.oppositeInfo[i].valid == 0 )
+        {
+            continue;
+        }
+
+        callback(dev, &wd->sta.oppositeInfo[i], ctx, index++);
+        oppositeCount--;
+
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return index;
+}
+
+
+s8_t zfStaFindFreeOpposite(zdev_t* dev, u16_t *sa, int *pFoundIdx)
+{
+    int oppositeCount;
+    int i;
+
+    zmw_get_wlan_dev(dev);
+
+    oppositeCount = wd->sta.oppositeCount;
+
+    for(i=0; i < ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        if ( oppositeCount == 0 )
+        {
+            break;
+        }
+
+        if ( wd->sta.oppositeInfo[i].valid == 0 )
+        {
+            continue;
+        }
+
+        oppositeCount--;
+        if ( zfMemoryIsEqual((u8_t*) sa, wd->sta.oppositeInfo[i].macAddr, 6) )
+        {
+            //wd->sta.oppositeInfo[i].aliveCounter++;
+            wd->sta.oppositeInfo[i].aliveCounter = ZM_IBSS_PEER_ALIVE_COUNTER;
+
+            /* it is already stored */
+            return 1;
+        }
+    }
+
+    // Check if there's still space for new comer
+    if ( wd->sta.oppositeCount == ZM_MAX_OPPOSITE_COUNT )
+    {
+        return -1;
+    }
+
+    // Find an unused slot for new peer station
+    for(i=0; i < ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        if ( wd->sta.oppositeInfo[i].valid == 0 )
+        {
+            break;
+        }
+    }
+
+    *pFoundIdx = i;
+    return 0;
+}
+
+s8_t zfStaFindOppositeByMACAddr(zdev_t* dev, u16_t *sa, u8_t *pFoundIdx)
+{
+    u32_t oppositeCount;
+    u32_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    oppositeCount = wd->sta.oppositeCount;
+
+    for(i=0; i < ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        if ( oppositeCount == 0 )
+        {
+            break;
+        }
+
+        if ( wd->sta.oppositeInfo[i].valid == 0 )
+        {
+            continue;
+        }
+
+        oppositeCount--;
+        if ( zfMemoryIsEqual((u8_t*) sa, wd->sta.oppositeInfo[i].macAddr, 6) )
+        {
+            *pFoundIdx = (u8_t)i;
+
+            return 0;
+        }
+    }
+
+    *pFoundIdx = 0;
+    return 1;
+}
+
+static void zfStaInitCommonOppositeInfo(zdev_t* dev, int i)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* set the default rate to the highest rate */
+    wd->sta.oppositeInfo[i].valid = 1;
+    wd->sta.oppositeInfo[i].aliveCounter = ZM_IBSS_PEER_ALIVE_COUNTER;
+    wd->sta.oppositeCount++;
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    /* Set parameters for new opposite peer station !!! */
+    wd->sta.oppositeInfo[i].camIdx = 0xff;  // Not set key in this location
+    wd->sta.oppositeInfo[i].pkInstalled = 0;
+    wd->sta.oppositeInfo[i].wpaState = ZM_STA_WPA_STATE_INIT ;  // No encryption
+#endif
+}
+
+int zfStaSetOppositeInfoFromBSSInfo(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    int i;
+    u8_t*  dst;
+    u16_t  sa[3];
+    int res;
+    u32_t oneTxStreamCap;
+
+    zmw_get_wlan_dev(dev);
+
+    zfMemoryCopy((u8_t*) sa, pBssInfo->macaddr, 6);
+
+    res = zfStaFindFreeOpposite(dev, sa, &i);
+    if ( res != 0 )
+    {
+        goto zlReturn;
+    }
+
+    dst = wd->sta.oppositeInfo[i].macAddr;
+    zfMemoryCopy(dst, (u8_t *)sa, 6);
+
+    oneTxStreamCap = (zfHpCapability(dev) & ZM_HP_CAP_11N_ONE_TX_STREAM);
+
+    if (pBssInfo->extSupportedRates[1] != 0)
+    {
+        /* TODO : Handle 11n */
+        if (pBssInfo->frequency < 3000)
+        {
+            /* 2.4GHz */
+            if (pBssInfo->EnableHT == 1)
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 1, pBssInfo->SG40);
+            else
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 1, pBssInfo->SG40);
+        }
+        else
+        {
+            /* 5GHz */
+            if (pBssInfo->EnableHT == 1)
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 0, pBssInfo->SG40);
+            else
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 0, pBssInfo->SG40);
+        }
+    }
+    else
+    {
+        /* TODO : Handle 11n */
+        if (pBssInfo->frequency < 3000)
+        {
+            /* 2.4GHz */
+            if (pBssInfo->EnableHT == 1)
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 1, pBssInfo->SG40);
+            else
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 0, 1, pBssInfo->SG40);
+        }
+        else
+        {
+            /* 5GHz */
+            if (pBssInfo->EnableHT == 1)
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 0, pBssInfo->SG40);
+            else
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 0, pBssInfo->SG40);
+        }
+    }
+
+
+    zfStaInitCommonOppositeInfo(dev, i);
+zlReturn:
+    return 0;
+}
+
+int zfStaSetOppositeInfoFromRxBuf(zdev_t* dev, zbuf_t* buf)
+{
+    int   i;
+    u8_t*  dst;
+    u16_t  sa[3];
+    int res = 0;
+    u16_t  offset;
+    u8_t   bSupportExtRate;
+    u32_t rtsctsRate = 0xffffffff; /* CTS:OFDM 6M, RTS:OFDM 6M */
+    u32_t oneTxStreamCap;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    sa[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+    sa[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+2);
+    sa[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+4);
+
+    zmw_enter_critical_section(dev);
+
+    res = zfStaFindFreeOpposite(dev, sa, &i);
+    if ( res != 0 )
+    {
+        goto zlReturn;
+    }
+
+    dst = wd->sta.oppositeInfo[i].macAddr;
+    zfCopyFromRxBuffer(dev, buf, dst, ZM_WLAN_HEADER_A2_OFFSET, 6);
+
+    if ( (wd->sta.currentFrequency < 3000) && !(wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )
+    {
+        bSupportExtRate = 0;
+    } else {
+        bSupportExtRate = 1;
+    }
+
+    if ( (bSupportExtRate == 1)
+         && (wd->sta.currentFrequency < 3000)
+         && (wd->wlanMode == ZM_MODE_IBSS)
+         && (wd->wfc.bIbssGMode == 0) )
+    {
+        bSupportExtRate = 0;
+    }
+
+    wd->sta.connection_11b = 0;
+    oneTxStreamCap = (zfHpCapability(dev) & ZM_HP_CAP_11N_ONE_TX_STREAM);
+
+    if ( ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_RATE)) != 0xffff)
+         && (bSupportExtRate == 1) )
+    {
+        /* TODO : Handle 11n */
+        if (wd->sta.currentFrequency < 3000)
+        {
+            /* 2.4GHz */
+            if (wd->sta.EnableHT == 1)
+            {
+                //11ng
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 1, wd->sta.SG40);
+            }
+            else
+            {
+                //11g
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 1, wd->sta.SG40);
+            }
+            rtsctsRate = 0x00001bb; /* CTS:CCK 1M, RTS:OFDM 6M */
+        }
+        else
+        {
+            /* 5GHz */
+            if (wd->sta.EnableHT == 1)
+            {
+                //11na
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 0, wd->sta.SG40);
+            }
+            else
+            {
+                //11a
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 0, wd->sta.SG40);
+            }
+            rtsctsRate = 0x10b01bb; /* CTS:OFDM 6M, RTS:OFDM 6M */
+        }
+    }
+    else
+    {
+        /* TODO : Handle 11n */
+        if (wd->sta.currentFrequency < 3000)
+        {
+            /* 2.4GHz */
+            if (wd->sta.EnableHT == 1)
+            {
+                //11ng
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 1, wd->sta.SG40);
+                rtsctsRate = 0x00001bb; /* CTS:CCK 1M, RTS:OFDM 6M */
+            }
+            else
+            {
+                //11b
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 0, 1, wd->sta.SG40);
+                rtsctsRate = 0x0; /* CTS:CCK 1M, RTS:CCK 1M */
+                wd->sta.connection_11b = 1;
+            }
+        }
+        else
+        {
+            /* 5GHz */
+            if (wd->sta.EnableHT == 1)
+            {
+                //11na
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 0, wd->sta.SG40);
+            }
+            else
+            {
+                //11a
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 0, wd->sta.SG40);
+            }
+            rtsctsRate = 0x10b01bb; /* CTS:OFDM 6M, RTS:OFDM 6M */
+        }
+    }
+
+    zfStaInitCommonOppositeInfo(dev, i);
+
+zlReturn:
+    zmw_leave_critical_section(dev);
+
+    if (rtsctsRate != 0xffffffff)
+    {
+        zfHpSetRTSCTSRate(dev, rtsctsRate);
+    }
+    return res;
+}
+
+void zfStaProtErpMonitor(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t   offset;
+    u8_t    erp;
+    u8_t    bssid[6];
+
+    zmw_get_wlan_dev(dev);
+
+    if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)&&(zfStaIsConnected(dev)) )
+    {
+        ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, bssid);
+
+        if (zfRxBufferEqualToStr(dev, buf, bssid, ZM_WLAN_HEADER_A2_OFFSET, 6))
+        {
+            if ( (offset=zfFindElement(dev, buf, ZM_WLAN_EID_ERP)) != 0xffff )
+            {
+                erp = zmw_rx_buf_readb(dev, buf, offset+2);
+
+                if ( erp & ZM_BIT_1 )
+                {
+                    //zm_debug_msg0("protection mode on");
+                    if (wd->sta.bProtectionMode == FALSE)
+                    {
+                        wd->sta.bProtectionMode = TRUE;
+                        zfHpSetSlotTime(dev, 0);
+                    }
+                }
+                else
+                {
+                    //zm_debug_msg0("protection mode off");
+                    if (wd->sta.bProtectionMode == TRUE)
+                    {
+                        wd->sta.bProtectionMode = FALSE;
+                        zfHpSetSlotTime(dev, 1);
+                    }
+                }
+            }
+        }
+		//Check the existence of Non-N AP
+		//Follow the check the "pBssInfo->EnableHT"
+			if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_HT_CAPABILITY)) != 0xffff)
+			{}
+			else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTCAPABILITY)) != 0xffff)
+			{}
+			else
+			{wd->sta.NonNAPcount++;}
+    }
+}
+
+void zfStaUpdateWmeParameter(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t   tmp;
+    u16_t   aifs[5];
+    u16_t   cwmin[5];
+    u16_t   cwmax[5];
+    u16_t   txop[5];
+    u8_t    acm;
+    u8_t    ac;
+    u16_t   len;
+    u16_t   i;
+   	u16_t   offset;
+    u8_t    rxWmeParameterSetCount;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Update if WME parameter set count is changed */
+    /* If connect to WME AP */
+    if (wd->sta.wmeConnected != 0)
+    {
+        /* Find WME parameter element */
+        if ((offset = zfFindWifiElement(dev, buf, 2, 1)) != 0xffff)
+        {
+            if ((len = zmw_rx_buf_readb(dev, buf, offset+1)) >= 7)
+            {
+                rxWmeParameterSetCount=zmw_rx_buf_readb(dev, buf, offset+8);
+                if (rxWmeParameterSetCount != wd->sta.wmeParameterSetCount)
+                {
+                    zm_msg0_mm(ZM_LV_0, "wmeParameterSetCount changed!");
+                    wd->sta.wmeParameterSetCount = rxWmeParameterSetCount;
+                    /* retrieve WME parameter and update TxQ parameters */
+                    acm = 0xf;
+                    for (i=0; i<4; i++)
+                    {
+                        if (len >= (8+(i*4)+4))
+                        {
+                            tmp=zmw_rx_buf_readb(dev, buf, offset+10+i*4);
+                            ac = (tmp >> 5) & 0x3;
+                            if ((tmp & 0x10) == 0)
+                            {
+                                acm &= (~(1<<ac));
+                            }
+                            aifs[ac] = ((tmp & 0xf) * 9) + 10;
+                            tmp=zmw_rx_buf_readb(dev, buf, offset+11+i*4);
+                            /* Convert to 2^n */
+                            cwmin[ac] = zcCwTlb[(tmp & 0xf)];
+                            cwmax[ac] = zcCwTlb[(tmp >> 4)];
+                            txop[ac]=zmw_rx_buf_readh(dev, buf,
+                                    offset+12+i*4);
+                        }
+                    }
+
+                    if ((acm & 0x4) != 0)
+                    {
+                        cwmin[2] = cwmin[0];
+                        cwmax[2] = cwmax[0];
+                        aifs[2] = aifs[0];
+                        txop[2] = txop[0];
+                    }
+                    if ((acm & 0x8) != 0)
+                    {
+                        cwmin[3] = cwmin[2];
+                        cwmax[3] = cwmax[2];
+                        aifs[3] = aifs[2];
+                        txop[3] = txop[2];
+                    }
+                    cwmin[4] = 3;
+                    cwmax[4] = 7;
+                    aifs[4] = 28;
+
+                    if ((cwmin[2]+aifs[2]) > ((cwmin[0]+aifs[0])+1))
+                    {
+                        wd->sta.ac0PriorityHigherThanAc2 = 1;
+                    }
+                    else
+                    {
+                        wd->sta.ac0PriorityHigherThanAc2 = 0;
+                    }
+                    zfHpUpdateQosParameter(dev, cwmin, cwmax, aifs, txop);
+                }
+            }
+        }
+    } //if (wd->sta.wmeConnected != 0)
+}
+/* process 802.11h Dynamic Frequency Selection */
+void zfStaUpdateDot11HDFS(zdev_t* dev, zbuf_t* buf)
+{
+    zmw_get_wlan_dev(dev);
+
+    /*
+    Channel Switch Announcement Element Format
+    +------+----------+------+-------------------+------------------+--------------------+
+    |Format|Element ID|Length|Channel Switch Mode|New Channel Number|Channel Switch Count|
+    +------+----------+------+-------------------+------------------+--------------------+
+    |Bytes |   1      |  1   |	     1           |       1          |          1         |
+    +------+----------+------+-------------------+------------------+--------------------+
+    |Value |   37     |  3   |       0 or 1      |unsigned integer  |unsigned integer    |
+    +------+----------+------+-------------------+------------------+--------------------+
+    */
+    //u8_t    length, channel, is5G;
+    u16_t   offset;
+
+    /* get EID(Channel Switch Announcement) */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_CHANNEL_SWITCH_ANNOUNCE)) == 0xffff )
+    {
+        //zm_debug_msg0("EID(Channel Switch Announcement) not found");
+        return;
+    }
+    else if ( zmw_rx_buf_readb(dev, buf, offset+1) == 0x3 )
+    {
+        zm_debug_msg0("EID(Channel Switch Announcement) found");
+
+        //length = zmw_rx_buf_readb(dev, buf, offset+1);
+        //zfCopyFromRxBuffer(dev, buf, pBssInfo->supportedRates, offset, length+2);
+
+        //Chanell Switch Mode set to 1, driver should disable transmit immediate
+        //we do this by poll CCA high
+        if (zmw_rx_buf_readb(dev, buf, offset+2) == 0x1 )
+        {
+        	//use ZM_OID_INTERNAL_WRITE,ZM_CMD_RESET to notice firmware flush quene and stop dma,
+        	//then restart rx dma but not tx dma
+        	if (wd->sta.DFSDisableTx != TRUE)
+        	{
+                /* TODO : zfHpResetTxRx would cause Rx hang */
+                //zfHpResetTxRx(dev);
+                wd->sta.DFSDisableTx = TRUE;
+                /* Trgger Rx DMA */
+                zfHpStartRecv(dev);
+            }
+        	//Adapter->ZD80211HSetting.DisableTxBy80211H=TRUE;
+        	//AcquireCtrOfPhyReg(Adapter);
+        	//ZD1205_WRITE_REGISTER(Adapter,CR24, 0x0);
+        	//ReleaseDoNotSleep(Adapter);
+        }
+
+        if (zmw_rx_buf_readb(dev, buf, offset+4) <= 0x2 )
+        {
+        	//Channel Switch
+        	//if Channel Switch Count = 0 , STA should change channel immediately.
+        	//if Channel Switch Count > 0 , STA should change channel after TBTT*count
+        	//But it won't be accurate to let driver calculate TBTT*count, and the value of
+        	//Channel Switch Count will decrease by one each when continue receving beacon
+        	//So we change channel here when we receive count <=2.
+
+            zfHpDeleteAllowChannel(dev, wd->sta.currentFrequency);
+        	wd->frequency = zfChNumToFreq(dev, zmw_rx_buf_readb(dev, buf, offset+3), 0);
+        	//zfHpAddAllowChannel(dev, wd->frequency);
+        	zm_debug_msg1("CWY - jump to frequency = ", wd->frequency);
+        	zfCoreSetFrequency(dev, wd->frequency);
+        	wd->sta.DFSDisableTx = FALSE;
+            /* Increase rxBeaconCount to prevent beacon lost */
+            if (zfStaIsConnected(dev))
+            {
+                wd->sta.rxBeaconCount = 1 << 6; // 2 times of check would pass
+            }
+        	//start tx dma to transmit packet
+
+        	//if (zmw_rx_buf_readb(dev, buf, offset+3) != wd->frequency)
+        	//{
+        	//	//ZDDbgPrint(("Radar Detect by AP\n"));
+        	//	zfCoreSetFrequency();
+        	//	ProcessRadarDetectEvent(Adapter);
+        	//	Set_RF_Channel(Adapter, SwRfd->Rfd->RxBuffer[index+3], (UCHAR)Adapter->RF_Mode, 1);
+        	//	Adapter->CardSetting.Channel = SwRfd->Rfd->RxBuffer[index+3];
+        	//	Adapter->SaveChannel = Adapter->CardSetting.Channel;
+        	//	Adapter->UtilityChannel = Adapter->CardSetting.Channel;
+        	//}
+        }
+    }
+
+}
+/* TODO : process 802.11h Transmission Power Control */
+void zfStaUpdateDot11HTPC(zdev_t* dev, zbuf_t* buf)
+{
+}
+
+/* IBSS power-saving mode */
+void zfStaIbssPSCheckState(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t   i, frameCtrl;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( !zfStaIsConnected(dev) )
+    {
+        return;
+    }
+
+    if ( wd->wlanMode != ZM_MODE_IBSS )
+    {
+        return ;
+    }
+
+    /* check BSSID */
+    if ( !zfRxBufferEqualToStr(dev, buf, (u8_t*) wd->sta.bssid,
+                               ZM_WLAN_HEADER_A3_OFFSET, 6) )
+    {
+        return;
+    }
+
+    frameCtrl = zmw_rx_buf_readb(dev, buf, 1);
+
+    /* check power management bit */
+    if ( frameCtrl & ZM_BIT_4 )
+    {
+        for(i=1; i<ZM_MAX_PS_STA; i++)
+        {
+            if ( !wd->sta.staPSList.entity[i].bUsed )
+            {
+                continue;
+            }
+
+            /* check source address */
+            if ( zfRxBufferEqualToStr(dev, buf,
+                                      wd->sta.staPSList.entity[i].macAddr,
+                                      ZM_WLAN_HEADER_A2_OFFSET, 6) )
+            {
+                return;
+            }
+        }
+
+        for(i=1; i<ZM_MAX_PS_STA; i++)
+        {
+            if ( !wd->sta.staPSList.entity[i].bUsed )
+            {
+                wd->sta.staPSList.entity[i].bUsed = TRUE;
+                wd->sta.staPSList.entity[i].bDataQueued = FALSE;
+                break;
+            }
+        }
+
+        if ( i == ZM_MAX_PS_STA )
+        {
+            /* STA list is full */
+            return;
+        }
+
+        zfCopyFromRxBuffer(dev, buf, wd->sta.staPSList.entity[i].macAddr,
+                           ZM_WLAN_HEADER_A2_OFFSET, 6);
+
+        if ( wd->sta.staPSList.count == 0 )
+        {
+            // enable ATIM window
+            //zfEnableAtimWindow(dev);
+        }
+
+        wd->sta.staPSList.count++;
+    }
+    else if ( wd->sta.staPSList.count )
+    {
+        for(i=1; i<ZM_MAX_PS_STA; i++)
+        {
+            if ( wd->sta.staPSList.entity[i].bUsed )
+            {
+                if ( zfRxBufferEqualToStr(dev, buf,
+                                          wd->sta.staPSList.entity[i].macAddr,
+                                          ZM_WLAN_HEADER_A2_OFFSET, 6) )
+                {
+                    wd->sta.staPSList.entity[i].bUsed = FALSE;
+                    wd->sta.staPSList.count--;
+
+                    if ( wd->sta.staPSList.entity[i].bDataQueued )
+                    {
+                        /* send queued data */
+                    }
+                }
+            }
+        }
+
+        if ( wd->sta.staPSList.count == 0 )
+        {
+            /* disable ATIM window */
+            //zfDisableAtimWindow(dev);
+        }
+
+    }
+}
+
+/* IBSS power-saving mode */
+u8_t zfStaIbssPSQueueData(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t   i;
+    u16_t  da[3];
+
+    zmw_get_wlan_dev(dev);
+
+    if ( !zfStaIsConnected(dev) )
+    {
+        return 0;
+    }
+
+    if ( wd->wlanMode != ZM_MODE_IBSS )
+    {
+        return 0;
+    }
+
+    if ( wd->sta.staPSList.count == 0 && wd->sta.powerSaveMode <= ZM_STA_PS_NONE )
+    {
+        return 0;
+    }
+
+    /* DA */
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    da[0] = zmw_tx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+    da[1] = zmw_tx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET + 2);
+    da[2] = zmw_tx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET + 4);
+#else
+    da[0] = zmw_tx_buf_readh(dev, buf, 0);
+    da[1] = zmw_tx_buf_readh(dev, buf, 2);
+    da[2] = zmw_tx_buf_readh(dev, buf, 4);
+#endif
+
+    if ( ZM_IS_MULTICAST_OR_BROADCAST(da) )
+    {
+        wd->sta.staPSList.entity[0].bDataQueued = TRUE;
+        wd->sta.ibssPSDataQueue[wd->sta.ibssPSDataCount++] = buf;
+        return 1;
+    }
+
+    // Unicast packet...
+
+    for(i=1; i<ZM_MAX_PS_STA; i++)
+    {
+        if ( zfMemoryIsEqual(wd->sta.staPSList.entity[i].macAddr,
+                             (u8_t*) da, 6) )
+        {
+            wd->sta.staPSList.entity[i].bDataQueued = TRUE;
+            wd->sta.ibssPSDataQueue[wd->sta.ibssPSDataCount++] = buf;
+
+            return 1;
+        }
+    }
+
+#if 0
+    if ( wd->sta.powerSaveMode > ZM_STA_PS_NONE )
+    {
+        wd->sta.staPSDataQueue[wd->sta.staPSDataCount++] = buf;
+
+        return 1;
+    }
+#endif
+
+    return 0;
+}
+
+/* IBSS power-saving mode */
+void zfStaIbssPSSend(zdev_t* dev)
+{
+    u8_t   i;
+    u16_t  bcastAddr[3] = {0xffff, 0xffff, 0xffff};
+
+    zmw_get_wlan_dev(dev);
+
+    if ( !zfStaIsConnected(dev) )
+    {
+        return ;
+    }
+
+    if ( wd->wlanMode != ZM_MODE_IBSS )
+    {
+        return ;
+    }
+
+    for(i=0; i<ZM_MAX_PS_STA; i++)
+    {
+        if ( wd->sta.staPSList.entity[i].bDataQueued )
+        {
+            if ( i == 0 )
+            {
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_ATIM,
+                              bcastAddr,
+                              0, 0, 0);
+            }
+            else if ( wd->sta.staPSList.entity[i].bUsed )
+            {
+                // Send ATIM to prevent the peer to go to sleep
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_ATIM,
+                              (u16_t*) wd->sta.staPSList.entity[i].macAddr,
+                              0, 0, 0);
+            }
+
+            wd->sta.staPSList.entity[i].bDataQueued = FALSE;
+        }
+    }
+
+    for(i=0; i<wd->sta.ibssPSDataCount; i++)
+    {
+        zfTxSendEth(dev, wd->sta.ibssPSDataQueue[i], 0,
+                    ZM_EXTERNAL_ALLOC_BUF, 0);
+    }
+
+    wd->sta.ibssPrevPSDataCount = wd->sta.ibssPSDataCount;
+    wd->sta.ibssPSDataCount = 0;
+}
+
+
+void zfStaReconnect(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE &&
+         wd->wlanMode != ZM_MODE_IBSS )
+    {
+        return;
+    }
+
+    if ( (zfStaIsConnected(dev))||(zfStaIsConnecting(dev)) )
+    {
+        return;
+    }
+
+    if ( wd->sta.bChannelScan )
+    {
+        return;
+    }
+
+    /* Recover zero SSID length  */
+    if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE) && (wd->ws.ssidLen == 0))
+    {
+        zm_debug_msg0("zfStaReconnect: NOT Support!! Set SSID to any BSS");
+        /* ANY BSS */
+        zmw_enter_critical_section(dev);
+        wd->sta.ssid[0] = 0;
+        wd->sta.ssidLen = 0;
+        zmw_leave_critical_section(dev);
+    }
+
+    // RAY: To ensure no TX pending before re-connecting
+    zfFlushVtxq(dev);
+    zfWlanEnable(dev);
+    zfScanMgrScanAck(dev);
+}
+
+void zfStaTimer100ms(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( (wd->tick % 10) == 0 )
+    {
+        zfPushVtxq(dev);
+//        zfPowerSavingMgrMain(dev);
+    }
+}
+
+
+void zfStaCheckRxBeacon(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE ) && (zfStaIsConnected(dev)))
+    {
+        if (wd->beaconInterval == 0)
+        {
+            wd->beaconInterval = 100;
+        }
+        if ( (wd->tick % ((wd->beaconInterval * 10) / ZM_MS_PER_TICK)) == 0 )
+        {
+            /* Check rxBeaconCount */
+            if (wd->sta.rxBeaconCount == 0)
+            {
+                if (wd->sta.beaconMissState == 1)
+                {
+            	/*notify AP that we left*/
+            	zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, wd->sta.bssid, 3, 0, 0);
+                /* Beacon Lost */
+                zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_BEACON_MISS,
+                        wd->sta.bssid, 0);
+                }
+                else
+                {
+                    wd->sta.beaconMissState = 1;
+                    /* Reset channel */
+                    zfCoreSetFrequencyExV2(dev, wd->frequency, wd->BandWidth40,
+                            wd->ExtOffset, NULL, 1);
+                }
+            }
+            else
+            {
+                wd->sta.beaconMissState = 0;
+            }
+            wd->sta.rxBeaconCount = 0;
+        }
+    }
+}
+
+
+
+void zfStaCheckConnectTimeout(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE )
+    {
+        return;
+    }
+
+    if ( !zfStaIsConnecting(dev) )
+    {
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+    if ( (wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_OPEN)||
+         (wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_SHARE_1)||
+         (wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_SHARE_2)||
+         (wd->sta.connectState == ZM_STA_CONN_STATE_ASSOCIATE) )
+    {
+        if ( (wd->tick - wd->sta.connectTimer) > ZM_INTERVAL_CONNECT_TIMEOUT )
+        {
+            if ( wd->sta.connectByReasso )
+            {
+                wd->sta.failCntOfReasso++;
+                if ( wd->sta.failCntOfReasso > 2 )
+                {
+                    wd->sta.connectByReasso = FALSE;
+                }
+            }
+
+            wd->sta.connectState = ZM_STA_CONN_STATE_NONE;
+            zm_debug_msg1("connect timeout, state = ", wd->sta.connectState);
+            //zfiWlanDisable(dev);
+            goto failed;
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+    return;
+
+failed:
+    zmw_leave_critical_section(dev);
+    if(wd->sta.authMode == ZM_AUTH_MODE_AUTO)
+	{ // Fix some AP not send authentication failed message to sta and lead to connect timeout !
+            wd->sta.connectTimeoutCount++;
+	}
+    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_TIMEOUT, wd->sta.bssid, 2);
+    return;
+}
+
+void zfMmStaTimeTick(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* airopeek */
+    if (wd->wlanMode != ZM_MODE_AP && !wd->swSniffer)
+    {
+        if ( wd->tick & 1 )
+        {
+            zfTimerCheckAndHandle(dev);
+        }
+
+        zfStaCheckRxBeacon(dev);
+        zfStaTimer100ms(dev);
+        zfStaCheckConnectTimeout(dev);
+        zfPowerSavingMgrMain(dev);
+    }
+
+#ifdef ZM_ENABLE_AGGREGATION
+    /*
+     * add by honda
+     */
+    zfAggScanAndClear(dev, wd->tick);
+#endif
+}
+
+void zfStaSendBeacon(zdev_t* dev)
+{
+    zbuf_t* buf;
+    u16_t offset, seq;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    //zm_debug_msg0("\n");
+
+    /* TBD : Maximum size of beacon */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_debug_msg0("Allocate beacon buffer failed");
+        return;
+    }
+
+    offset = 0;
+    /* wlan header */
+    /* Frame control */
+    zmw_tx_buf_writeh(dev, buf, offset, 0x0080);
+    offset+=2;
+    /* Duration */
+    zmw_tx_buf_writeh(dev, buf, offset, 0x0000);
+    offset+=2;
+    /* Address 1 */
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    /* Address 2 */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[0]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[1]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[2]);
+    offset+=2;
+    /* Address 3 */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[0]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[1]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[2]);
+    offset+=2;
+
+    /* Sequence number */
+    zmw_enter_critical_section(dev);
+    seq = ((wd->mmseq++)<<4);
+    zmw_leave_critical_section(dev);
+    zmw_tx_buf_writeh(dev, buf, offset, seq);
+    offset+=2;
+
+    /* 24-31 Time Stamp : hardware will fill this field */
+    offset+=8;
+
+    /* Beacon Interval */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->beaconInterval);
+    offset+=2;
+
+    /* Capability */
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[0]);
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[1]);
+
+    /* SSID */
+    offset = zfStaAddIeSsid(dev, buf, offset);
+
+    if(wd->frequency <= ZM_CH_G_14)  // 2.4 GHz  b+g
+    {
+
+    	/* Support Rate */
+    	offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                  		ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+
+    	/* DS parameter set */
+    	offset = zfMmAddIeDs(dev, buf, offset);
+
+    	offset = zfStaAddIeIbss(dev, buf, offset);
+
+        if( wd->wfc.bIbssGMode
+            && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )    // Only accompany with enabling a mode .
+        {
+      	    /* ERP Information */
+       	    wd->erpElement = 0;
+       	    offset = zfMmAddIeErp(dev, buf, offset);
+       	}
+
+       	/* TODO : country information */
+        /* RSN */
+        if ( wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK )
+        {
+            offset = zfwStaAddIeWpaRsn(dev, buf, offset, ZM_WLAN_FRAME_TYPE_AUTH);
+        }
+
+        if( wd->wfc.bIbssGMode
+            && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )    // Only accompany with enabling a mode .
+        {
+            /* Enable G Mode */
+            /* Extended Supported Rates */
+       	    offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                   		    ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+	    }
+    }
+    else    // 5GHz a
+    {
+        /* Support Rate a Mode */
+    	offset = zfMmAddIeSupportRate(dev, buf, offset,
+        	                            ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+
+        /* DS parameter set */
+    	offset = zfMmAddIeDs(dev, buf, offset);
+
+    	offset = zfStaAddIeIbss(dev, buf, offset);
+
+        /* TODO : country information */
+        /* RSN */
+        if ( wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK )
+        {
+            offset = zfwStaAddIeWpaRsn(dev, buf, offset, ZM_WLAN_FRAME_TYPE_AUTH);
+        }
+    }
+
+    if ( wd->wlanMode != ZM_MODE_IBSS )
+    {
+        /* TODO : Need to check if it is ok */
+        /* HT Capabilities Info */
+        offset = zfMmAddHTCapability(dev, buf, offset);
+
+        /* Extended HT Capabilities Info */
+        offset = zfMmAddExtendedHTCapability(dev, buf, offset);
+    }
+
+    if ( wd->sta.ibssAdditionalIESize )
+        offset = zfStaAddIbssAdditionalIE(dev, buf, offset);
+
+    /* 1212 : write to beacon fifo */
+    /* 1221 : write to share memory */
+    zfHpSendBeacon(dev, buf, offset);
+
+    /* Free beacon buffer */
+    //zfwBufFree(dev, buf, 0);
+}
+
+void zfStaSignalStatistic(zdev_t* dev, u8_t SignalStrength, u8_t SignalQuality) //CWYang(+)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Add Your Code to Do Works Like Moving Average Here */
+    wd->SignalStrength = (wd->SignalStrength * 7 + SignalStrength * 3)/10;
+    wd->SignalQuality = (wd->SignalQuality * 7 + SignalQuality * 3)/10;
+
+}
+
+struct zsBssInfo* zfStaFindBssInfo(zdev_t* dev, zbuf_t* buf, struct zsWlanProbeRspFrameHeader *pProbeRspHeader)
+{
+    u8_t    i;
+    u8_t    j;
+    u8_t    k;
+    u8_t    isMatched, length, channel;
+    u16_t   offset, frequency;
+    struct zsBssInfo* pBssInfo;
+
+    zmw_get_wlan_dev(dev);
+
+    if ((pBssInfo = wd->sta.bssList.head) == NULL)
+    {
+        return NULL;
+    }
+
+    for( i=0; i<wd->sta.bssList.bssCount; i++ )
+    {
+        //zm_debug_msg2("check pBssInfo = ", pBssInfo);
+
+        /* Check BSSID */
+        for( j=0; j<6; j++ )
+        {
+            if ( pBssInfo->bssid[j] != pProbeRspHeader->bssid[j] )
+            {
+                break;
+            }
+        }
+
+		/* Check SSID */
+        if (j == 6)
+        {
+            if (pProbeRspHeader->ssid[1] <= 32)
+            {
+                /* compare length and ssid */
+                isMatched = 1;
+				if((pProbeRspHeader->ssid[1] != 0) && (pBssInfo->ssid[1] != 0))
+				{
+                for( k=1; k<pProbeRspHeader->ssid[1] + 1; k++ )
+                {
+                    if ( pBssInfo->ssid[k] != pProbeRspHeader->ssid[k] )
+                    {
+                        isMatched = 0;
+                        break;
+                    }
+                }
+            }
+            }
+            else
+            {
+                isMatched = 0;
+            }
+        }
+        else
+        {
+            isMatched = 0;
+        }
+
+        /* Check channel */
+        /* Add check channel to solve the bug #31222 */
+        if (isMatched) {
+            if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_DS)) != 0xffff) {
+                if ((length = zmw_rx_buf_readb(dev, buf, offset+1)) == 1) {
+                    channel = zmw_rx_buf_readb(dev, buf, offset+2);
+                    if (zfHpIsAllowedChannel(dev, zfChNumToFreq(dev, channel, 0)) == 0) {
+                        frequency = 0;
+                    } else {
+                        frequency = zfChNumToFreq(dev, channel, 0);;
+                    }
+                } else {
+                    frequency = 0;
+                }
+            } else {
+                frequency = wd->sta.currentFrequency;
+            }
+
+            if (frequency != 0) {
+                if ( ((frequency > 3000) && (pBssInfo->frequency > 3000))
+                     || ((frequency < 3000) && (pBssInfo->frequency < 3000)) ) {
+                    /* redundant */
+                    break;
+                }
+            }
+        }
+
+        pBssInfo = pBssInfo->next;
+    }
+
+    if ( i == wd->sta.bssList.bssCount )
+    {
+        pBssInfo = NULL;
+    }
+
+    return pBssInfo;
+}
+
+u8_t zfStaInitBssInfo(zdev_t* dev, zbuf_t* buf,
+        struct zsWlanProbeRspFrameHeader *pProbeRspHeader,
+        struct zsBssInfo* pBssInfo, struct zsAdditionInfo* AddInfo, u8_t type)
+{
+    u8_t    length, channel, is5G;
+    u16_t   i, offset;
+    u8_t    apQosInfo;
+    u16_t    eachIElength = 0;
+    u16_t   accumulateLen = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    if ((type == 1) && ((pBssInfo->flag & ZM_BSS_INFO_VALID_BIT) != 0))
+    {
+        goto zlUpdateRssi;
+    }
+
+    /* get SSID */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID)) == 0xffff )
+    {
+        zm_debug_msg0("EID(SSID) not found");
+        goto zlError;
+    }
+
+    length = zmw_rx_buf_readb(dev, buf, offset+1);
+
+	{
+		u8_t Show_Flag = 0;
+		zfwGetShowZeroLengthSSID(dev, &Show_Flag);
+
+		if(Show_Flag)
+		{
+			if (length > ZM_MAX_SSID_LENGTH )
+			{
+				zm_debug_msg0("EID(SSID) is invalid");
+				goto zlError;
+			}
+		}
+		else
+		{
+    if ( length == 0 || length > ZM_MAX_SSID_LENGTH )
+    {
+        zm_debug_msg0("EID(SSID) is invalid");
+        goto zlError;
+    }
+
+		}
+	}
+    zfCopyFromRxBuffer(dev, buf, pBssInfo->ssid, offset, length+2);
+
+    /* get DS parameter */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_DS)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if ( length != 1 )
+        {
+            zm_msg0_mm(ZM_LV_0, "Abnormal DS Param Set IE");
+            goto zlError;
+        }
+        channel = zmw_rx_buf_readb(dev, buf, offset+2);
+
+        if (zfHpIsAllowedChannel(dev, zfChNumToFreq(dev, channel, 0)) == 0)
+        {
+            goto zlError2;
+        }
+
+        pBssInfo->frequency = zfChNumToFreq(dev, channel, 0); // auto check
+        pBssInfo->channel = channel;
+
+
+    }
+    else
+    {
+        /* DS parameter not found */
+        pBssInfo->frequency = wd->sta.currentFrequency;
+        pBssInfo->channel = zfChFreqToNum(wd->sta.currentFrequency, &is5G);
+    }
+
+    /* initialize security type */
+    pBssInfo->securityType = ZM_SECURITY_TYPE_NONE;
+
+    /* get macaddr */
+    for( i=0; i<6; i++ )
+    {
+        pBssInfo->macaddr[i] = pProbeRspHeader->sa[i];
+    }
+
+    /* get bssid */
+    for( i=0; i<6; i++ )
+    {
+        pBssInfo->bssid[i] = pProbeRspHeader->bssid[i];
+    }
+
+    /* get timestamp */
+    for( i=0; i<8; i++ )
+    {
+        pBssInfo->timeStamp[i] = pProbeRspHeader->timeStamp[i];
+    }
+
+    /* get beacon interval */
+    pBssInfo->beaconInterval[0] = pProbeRspHeader->beaconInterval[0];
+    pBssInfo->beaconInterval[1] = pProbeRspHeader->beaconInterval[1];
+
+    /* get capability */
+    pBssInfo->capability[0] = pProbeRspHeader->capability[0];
+    pBssInfo->capability[1] = pProbeRspHeader->capability[1];
+
+    /* Copy frame body */
+    offset = 36;            // Copy from the start of variable IE
+    pBssInfo->frameBodysize = zfwBufGetSize(dev, buf)-offset;
+    if (pBssInfo->frameBodysize > (ZM_MAX_PROBE_FRAME_BODY_SIZE-1))
+    {
+        pBssInfo->frameBodysize = ZM_MAX_PROBE_FRAME_BODY_SIZE-1;
+    }
+    accumulateLen = 0;
+    do
+    {
+        eachIElength = zmw_rx_buf_readb(dev, buf, offset + accumulateLen+1) + 2;  //Len+(EID+Data)
+
+        if ( (eachIElength >= 2)
+             && ((accumulateLen + eachIElength) <= pBssInfo->frameBodysize) )
+        {
+            zfCopyFromRxBuffer(dev, buf, pBssInfo->frameBody+accumulateLen, offset+accumulateLen, eachIElength);
+            accumulateLen+=(u16_t)eachIElength;
+        }
+        else
+        {
+            zm_msg0_mm(ZM_LV_1, "probersp frameBodysize abnormal");
+            break;
+        }
+    }
+    while(accumulateLen < pBssInfo->frameBodysize);
+    pBssInfo->frameBodysize = accumulateLen;
+
+    /* get supported rates */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_SUPPORT_RATE)) == 0xffff )
+    {
+        zm_debug_msg0("EID(supported rates) not found");
+        goto zlError;
+    }
+
+    length = zmw_rx_buf_readb(dev, buf, offset+1);
+    if ( length == 0 || length > ZM_MAX_SUPP_RATES_IE_SIZE)
+    {
+        zm_msg0_mm(ZM_LV_0, "Supported rates IE length abnormal");
+        goto zlError;
+    }
+    zfCopyFromRxBuffer(dev, buf, pBssInfo->supportedRates, offset, length+2);
+
+
+
+    /* get Country information */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_COUNTRY)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_COUNTRY_INFO_SIZE)
+        {
+            length = ZM_MAX_COUNTRY_INFO_SIZE;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->countryInfo, offset, length+2);
+        /* check 802.11d support data */
+        if (wd->sta.b802_11D)
+        {
+            zfHpGetRegulationTablefromISO(dev, (u8_t *)&pBssInfo->countryInfo, 3);
+            /* only set regulatory one time */
+            wd->sta.b802_11D = 0;
+        }
+    }
+
+    /* get ERP information */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_ERP)) != 0xffff )
+    {
+        pBssInfo->erp = zmw_rx_buf_readb(dev, buf, offset+2);
+    }
+
+    /* get extended supported rates */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_RATE)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_SUPP_RATES_IE_SIZE)
+        {
+            zm_msg0_mm(ZM_LV_0, "Extended rates IE length abnormal");
+            goto zlError;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->extSupportedRates, offset, length+2);
+    }
+    else
+    {
+        pBssInfo->extSupportedRates[0] = 0;
+        pBssInfo->extSupportedRates[1] = 0;
+    }
+
+    /* get WPA IE */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_WPA_IE)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_IE_SIZE)
+        {
+            length = ZM_MAX_IE_SIZE;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->wpaIe, offset, length+2);
+        pBssInfo->securityType = ZM_SECURITY_TYPE_WPA;
+    }
+    else
+    {
+        pBssInfo->wpaIe[1] = 0;
+    }
+
+    /* get WPS IE */
+    if ((offset = zfFindWifiElement(dev, buf, 4, 0xff)) != 0xffff)
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_WPS_IE_SIZE )
+        {
+            length = ZM_MAX_WPS_IE_SIZE;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->wscIe, offset, length+2);
+    }
+    else
+    {
+        pBssInfo->wscIe[1] = 0;
+    }
+
+    /* get SuperG IE */
+    if ((offset = zfFindSuperGElement(dev, buf, ZM_WLAN_EID_VENDOR_PRIVATE)) != 0xffff)
+    {
+        pBssInfo->apCap |= ZM_SuperG_AP;
+    }
+
+    /* get XR IE */
+    if ((offset = zfFindXRElement(dev, buf, ZM_WLAN_EID_VENDOR_PRIVATE)) != 0xffff)
+    {
+        pBssInfo->apCap |= ZM_XR_AP;
+    }
+
+    /* get RSN IE */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_RSN_IE)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_IE_SIZE)
+        {
+            length = ZM_MAX_IE_SIZE;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->rsnIe, offset, length+2);
+        pBssInfo->securityType = ZM_SECURITY_TYPE_WPA;
+    }
+    else
+    {
+        pBssInfo->rsnIe[1] = 0;
+    }
+#ifdef ZM_ENABLE_CENC
+    /* get CENC IE */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_CENC_IE)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_IE_SIZE )
+        {
+            length = ZM_MAX_IE_SIZE;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->cencIe, offset, length+2);
+        pBssInfo->securityType = ZM_SECURITY_TYPE_CENC;
+        pBssInfo->capability[0] &= 0xffef;
+    }
+    else
+    {
+        pBssInfo->cencIe[1] = 0;
+    }
+#endif //ZM_ENABLE_CENC
+    /* get WME Parameter IE, probe rsp may contain WME parameter element */
+    //if ( wd->bQoSEnable )
+    {
+        if ((offset = zfFindWifiElement(dev, buf, 2, 1)) != 0xffff)
+        {
+            apQosInfo = zmw_rx_buf_readb(dev, buf, offset+8) & 0x80;
+            pBssInfo->wmeSupport = 1 | apQosInfo;
+        }
+        else if ((offset = zfFindWifiElement(dev, buf, 2, 0)) != 0xffff)
+        {
+            apQosInfo = zmw_rx_buf_readb(dev, buf, offset+8) & 0x80;
+            pBssInfo->wmeSupport = 1  | apQosInfo;
+        }
+        else
+        {
+            pBssInfo->wmeSupport = 0;
+        }
+    }
+    //CWYang(+)
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_HT_CAPABILITY)) != 0xffff)
+    {
+        /* 11n AP */
+        pBssInfo->EnableHT = 1;
+        if (zmw_rx_buf_readb(dev, buf, offset+1) & 0x02)
+        {
+            pBssInfo->enableHT40 = 1;
+        }
+        else
+        {
+            pBssInfo->enableHT40 = 0;
+        }
+
+        if (zmw_rx_buf_readb(dev, buf, offset+1) & 0x40)
+        {
+            pBssInfo->SG40 = 1;
+        }
+        else
+        {
+            pBssInfo->SG40 = 0;
+        }
+    }
+    else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTCAPABILITY)) != 0xffff)
+    {
+        /* 11n AP */
+        pBssInfo->EnableHT = 1;
+        pBssInfo->apCap |= ZM_All11N_AP;
+        if (zmw_rx_buf_readb(dev, buf, offset+2) & 0x02)
+        {
+            pBssInfo->enableHT40 = 1;
+        }
+        else
+        {
+            pBssInfo->enableHT40 = 0;
+        }
+
+        if (zmw_rx_buf_readb(dev, buf, offset+2) & 0x40)
+        {
+            pBssInfo->SG40 = 1;
+        }
+        else
+        {
+            pBssInfo->SG40 = 0;
+        }
+    }
+    else
+    {
+        pBssInfo->EnableHT = 0;
+    }
+    /* HT information */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY)) != 0xffff)
+    {
+        /* atheros pre n */
+        pBssInfo->extChOffset = zmw_rx_buf_readb(dev, buf, offset+2) & 0x03;
+    }
+    else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTINFORMATION)) != 0xffff)
+    {
+        /* pre n 2.0 standard */
+        pBssInfo->extChOffset = zmw_rx_buf_readb(dev, buf, offset+3) & 0x03;
+    }
+    else
+    {
+        pBssInfo->extChOffset = 0;
+    }
+
+    if ( (pBssInfo->enableHT40 == 1)
+         && ((pBssInfo->extChOffset != 1) && (pBssInfo->extChOffset != 3)) )
+    {
+        pBssInfo->enableHT40 = 0;
+    }
+
+    if (pBssInfo->enableHT40 == 1)
+    {
+        if (zfHpIsAllowedChannel(dev, pBssInfo->frequency+((pBssInfo->extChOffset==1)?20:-20)) == 0)
+        {
+            /* if extension channel is not an allowed channel, treat AP as non-HT mode */
+            pBssInfo->EnableHT = 0;
+            pBssInfo->enableHT40 = 0;
+            pBssInfo->extChOffset = 0;
+        }
+    }
+
+    /* get ATH Extended Capability */
+    if ( ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY)) != 0xffff)&&
+        ((offset = zfFindBrdcmMrvlRlnkExtCap(dev, buf)) == 0xffff))
+
+    {
+        pBssInfo->athOwlAp = 1;
+    }
+    else
+    {
+        pBssInfo->athOwlAp = 0;
+    }
+
+    /* get Broadcom Extended Capability */
+    if ( (pBssInfo->EnableHT == 1) //((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY)) != 0xffff)
+         && ((offset = zfFindBroadcomExtCap(dev, buf)) != 0xffff) )
+    {
+        pBssInfo->broadcomHTAp = 1;
+    }
+    else
+    {
+        pBssInfo->broadcomHTAp = 0;
+    }
+
+    /* get Marvel Extended Capability */
+    if ((offset = zfFindMarvelExtCap(dev, buf)) != 0xffff)
+    {
+        pBssInfo->marvelAp = 1;
+    }
+    else
+    {
+        pBssInfo->marvelAp = 0;
+    }
+
+    /* get ATIM window */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_IBSS)) != 0xffff )
+    {
+        pBssInfo->atimWindow = zmw_rx_buf_readh(dev, buf,offset+2);
+    }
+
+    /* Fit for support mode */
+    if (pBssInfo->frequency > 3000) {
+        if (wd->supportMode & ZM_WIRELESS_MODE_5_N) {
+#if 0
+            if (wd->supportMode & ZM_WIRELESS_MODE_5_54) {
+                /* support mode: a, n */
+                /* do nothing */
+            } else {
+                /* support mode: n */
+                /* reject non-n bss info */
+                if (!pBssInfo->EnableHT) {
+                    goto zlError2;
+                }
+            }
+#endif
+        } else {
+            if (wd->supportMode & ZM_WIRELESS_MODE_5_54) {
+                /* support mode: a */
+                /* delete n mode information */
+                pBssInfo->EnableHT = 0;
+                pBssInfo->enableHT40 = 0;
+                pBssInfo->apCap &= (~ZM_All11N_AP);
+                pBssInfo->extChOffset = 0;
+                pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                            pBssInfo->frameBodysize, ZM_WLAN_EID_HT_CAPABILITY);
+                pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                            pBssInfo->frameBodysize, ZM_WLAN_PREN2_EID_HTCAPABILITY);
+                pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                            pBssInfo->frameBodysize, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY);
+                pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                            pBssInfo->frameBodysize, ZM_WLAN_PREN2_EID_HTINFORMATION);
+            } else {
+                /* support mode: none */
+                goto zlError2;
+            }
+        }
+    } else {
+        if (wd->supportMode & ZM_WIRELESS_MODE_24_N) {
+#if 0
+            if (wd->supportMode & ZM_WIRELESS_MODE_24_54) {
+                if (wd->supportMode & ZM_WIRELESS_MODE_24_11) {
+                    /* support mode: b, g, n */
+                    /* do nothing */
+                } else {
+                    /* support mode: g, n */
+                    /* reject b-only bss info */
+                    if ( (!pBssInfo->EnableHT)
+                         && (pBssInfo->extSupportedRates[1] == 0) ) {
+                         goto zlError2;
+                    }
+                }
+            } else {
+                if (wd->supportMode & ZM_WIRELESS_MODE_24_11) {
+                    /* support mode: b, n */
+                    /* 1. reject g-only bss info
+                     * 2. if non g-only, delete g mode information
+                     */
+                    if ( !pBssInfo->EnableHT ) {
+                        if ( zfIsGOnlyMode(dev, pBssInfo->frequency, pBssInfo->supportedRates)
+                             || zfIsGOnlyMode(dev, pBssInfo->frequency, pBssInfo->extSupportedRates) ) {
+                            goto zlError2;
+                        } else {
+                            zfGatherBMode(dev, pBssInfo->supportedRates,
+                                          pBssInfo->extSupportedRates);
+                            pBssInfo->erp = 0;
+
+                            pBssInfo->frameBodysize = zfRemoveElement(dev,
+                                pBssInfo->frameBody, pBssInfo->frameBodysize,
+                                ZM_WLAN_EID_ERP);
+                            pBssInfo->frameBodysize = zfRemoveElement(dev,
+                                pBssInfo->frameBody, pBssInfo->frameBodysize,
+                                ZM_WLAN_EID_EXTENDED_RATE);
+
+                            pBssInfo->frameBodysize = zfUpdateElement(dev,
+                                pBssInfo->frameBody, pBssInfo->frameBodysize,
+                                pBssInfo->supportedRates);
+                        }
+                    }
+                } else {
+                    /* support mode: n */
+                    /* reject non-n bss info */
+                    if (!pBssInfo->EnableHT) {
+                        goto zlError2;
+                    }
+                }
+            }
+#endif
+        } else {
+            /* delete n mode information */
+            pBssInfo->EnableHT = 0;
+            pBssInfo->enableHT40 = 0;
+            pBssInfo->apCap &= (~ZM_All11N_AP);
+            pBssInfo->extChOffset = 0;
+            pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                        pBssInfo->frameBodysize, ZM_WLAN_EID_HT_CAPABILITY);
+            pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                        pBssInfo->frameBodysize, ZM_WLAN_PREN2_EID_HTCAPABILITY);
+            pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                        pBssInfo->frameBodysize, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY);
+            pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                        pBssInfo->frameBodysize, ZM_WLAN_PREN2_EID_HTINFORMATION);
+
+            if (wd->supportMode & ZM_WIRELESS_MODE_24_54) {
+#if 0
+                if (wd->supportMode & ZM_WIRELESS_MODE_24_11) {
+                    /* support mode: b, g */
+                    /* delete n mode information */
+                } else {
+                    /* support mode: g */
+                    /* delete n mode information */
+                    /* reject b-only bss info */
+                    if (pBssInfo->extSupportedRates[1] == 0) {
+                         goto zlError2;
+                    }
+                }
+#endif
+            } else {
+                if (wd->supportMode & ZM_WIRELESS_MODE_24_11) {
+                    /* support mode: b */
+                    /* delete n mode information */
+                    if ( zfIsGOnlyMode(dev, pBssInfo->frequency, pBssInfo->supportedRates)
+                         || zfIsGOnlyMode(dev, pBssInfo->frequency, pBssInfo->extSupportedRates) ) {
+                        goto zlError2;
+                    } else {
+                        zfGatherBMode(dev, pBssInfo->supportedRates,
+                                          pBssInfo->extSupportedRates);
+                        pBssInfo->erp = 0;
+
+                        pBssInfo->frameBodysize = zfRemoveElement(dev,
+                            pBssInfo->frameBody, pBssInfo->frameBodysize,
+                            ZM_WLAN_EID_ERP);
+                        pBssInfo->frameBodysize = zfRemoveElement(dev,
+                            pBssInfo->frameBody, pBssInfo->frameBodysize,
+                            ZM_WLAN_EID_EXTENDED_RATE);
+
+                        pBssInfo->frameBodysize = zfUpdateElement(dev,
+                            pBssInfo->frameBody, pBssInfo->frameBodysize,
+                            pBssInfo->supportedRates);
+                    }
+                } else {
+                    /* support mode: none */
+                    goto zlError2;
+                }
+            }
+        }
+    }
+
+    pBssInfo->flag |= ZM_BSS_INFO_VALID_BIT;
+
+zlUpdateRssi:
+    /* Update Timer information */
+    pBssInfo->tick = wd->tick;
+
+    /* Update ERP information */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_ERP)) != 0xffff )
+    {
+        pBssInfo->erp = zmw_rx_buf_readb(dev, buf, offset+2);
+    }
+
+    if( (s8_t)pBssInfo->signalStrength < (s8_t)AddInfo->Tail.Data.SignalStrength1 )
+    {
+        /* Update signal strength */
+        pBssInfo->signalStrength = (u8_t)AddInfo->Tail.Data.SignalStrength1;
+        /* Update signal quality */
+        pBssInfo->signalQuality = (u8_t)(AddInfo->Tail.Data.SignalStrength1 * 2);
+
+        /* Update the sorting value  */
+        pBssInfo->sortValue = zfComputeBssInfoWeightValue(dev,
+                                               (pBssInfo->supportedRates[6] + pBssInfo->extSupportedRates[0]),
+                                               pBssInfo->EnableHT,
+                                               pBssInfo->enableHT40,
+                                               pBssInfo->signalStrength);
+    }
+
+    return 0;
+
+zlError:
+
+    return 1;
+
+zlError2:
+
+    return 2;
+}
+
+void zfStaProcessBeacon(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo) //CWYang(m)
+{
+    /* Parse TIM and send PS-POLL in power saving mode */
+    struct zsWlanBeaconFrameHeader*  pBeaconHeader;
+    struct zsBssInfo* pBssInfo;
+    u8_t   pBuf[sizeof(struct zsWlanBeaconFrameHeader)];
+    u8_t   bssid[6];
+    int    res;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /* sta routine jobs */
+    zfStaProtErpMonitor(dev, buf);  /* check protection mode */
+
+    if (zfStaIsConnected(dev))
+    {
+        ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, bssid);
+
+        if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+        {
+            if ( zfRxBufferEqualToStr(dev, buf, bssid, ZM_WLAN_HEADER_A2_OFFSET, 6) )
+            {
+                zfPowerSavingMgrProcessBeacon(dev, buf);
+                zfStaUpdateWmeParameter(dev, buf);
+                if (wd->sta.DFSEnable)
+                    zfStaUpdateDot11HDFS(dev, buf);
+                if (wd->sta.TPCEnable)
+                    zfStaUpdateDot11HTPC(dev, buf);
+                /* update signal strength and signal quality */
+                zfStaSignalStatistic(dev, AddInfo->Tail.Data.SignalStrength1,
+                        AddInfo->Tail.Data.SignalQuality); //CWYang(+)
+                wd->sta.rxBeaconCount++;
+            }
+        }
+        else if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            if ( zfRxBufferEqualToStr(dev, buf, bssid, ZM_WLAN_HEADER_A3_OFFSET, 6) )
+            {
+                int res;
+                struct zsPartnerNotifyEvent event;
+
+                zm_debug_msg0("20070916 Receive opposite Beacon!");
+                zmw_enter_critical_section(dev);
+                wd->sta.ibssReceiveBeaconCount++;
+                zmw_leave_critical_section(dev);
+
+                res = zfStaSetOppositeInfoFromRxBuf(dev, buf);
+                if ( res == 0 )
+                {
+                    // New peer station found. Notify the wrapper now
+                    zfInitPartnerNotifyEvent(dev, buf, &event);
+                    if (wd->zfcbIbssPartnerNotify != NULL)
+                    {
+                        wd->zfcbIbssPartnerNotify(dev, 1, &event);
+                    }
+                }
+                /* update signal strength and signal quality */
+                zfStaSignalStatistic(dev, AddInfo->Tail.Data.SignalStrength1,
+                        AddInfo->Tail.Data.SignalQuality); //CWYang(+)
+            }
+            //else if ( wd->sta.ibssPartnerStatus == ZM_IBSS_PARTNER_LOST )
+            // Why does this happen in IBSS?? The impact of Vista since
+            // we need to tell it the BSSID
+#if 0
+            else if ( wd->sta.oppositeCount == 0 )
+            {   /* IBSS merge if SSID matched */
+                if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID)) != 0xffff )
+                {
+                    if ( (wd->sta.ssidLen == zmw_buf_readb(dev, buf, offset+1))&&
+                         (zfRxBufferEqualToStr(dev, buf, wd->sta.ssid,
+                                               offset+2, wd->sta.ssidLen)) )
+                    {
+                        capabilityInfo = zmw_buf_readh(dev, buf, 34);
+
+                        if ( capabilityInfo & ZM_BIT_1 )
+                        {
+                            if ( (wd->sta.capability[0] & ZM_BIT_4) ==
+                                 (capabilityInfo & ZM_BIT_4) )
+                            {
+                                zm_debug_msg0("IBSS merge");
+                                zfCopyFromRxBuffer(dev, buf, bssid,
+                                                   ZM_WLAN_HEADER_A3_OFFSET, 6);
+                                zfUpdateBssid(dev, bssid);
+                            }
+                        }
+                    }
+                }
+            }
+#endif
+        }
+    }
+
+    /* return if not channel scan */
+    if ( !wd->sta.bChannelScan )
+    {
+        goto zlReturn;
+    }
+
+    zfCopyFromRxBuffer(dev, buf, pBuf, 0, sizeof(struct zsWlanBeaconFrameHeader));
+    pBeaconHeader = (struct zsWlanBeaconFrameHeader*) pBuf;
+
+    zmw_enter_critical_section(dev);
+
+    //zm_debug_msg1("bss count = ", wd->sta.bssList.bssCount);
+
+    pBssInfo = zfStaFindBssInfo(dev, buf, pBeaconHeader);
+
+    if ( pBssInfo == NULL )
+    {
+        /* Allocate a new entry if BSS not in the scan list */
+        pBssInfo = zfBssInfoAllocate(dev);
+        if (pBssInfo != NULL)
+        {
+            res = zfStaInitBssInfo(dev, buf, pBeaconHeader, pBssInfo, AddInfo, 0);
+            //zfDumpSSID(pBssInfo->ssid[1], &(pBssInfo->ssid[2]));
+            if ( res != 0 )
+            {
+                zfBssInfoFree(dev, pBssInfo);
+            }
+            else
+            {
+                zfBssInfoInsertToList(dev, pBssInfo);
+            }
+        }
+    }
+    else
+    {
+        res = zfStaInitBssInfo(dev, buf, pBeaconHeader, pBssInfo, AddInfo, 1);
+        if (res == 2)
+        {
+            zfBssInfoRemoveFromList(dev, pBssInfo);
+            zfBssInfoFree(dev, pBssInfo);
+        }
+        else if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            int idx;
+
+            // It would reset the alive counter if the peer station is found!
+            zfStaFindFreeOpposite(dev, (u16_t *)pBssInfo->macaddr, &idx);
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+zlReturn:
+
+    return;
+}
+
+
+void zfAuthFreqCompleteCb(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_COMPLETED)
+    {
+        zm_debug_msg0("ZM_STA_CONN_STATE_ASSOCIATE");
+        wd->sta.connectTimer = wd->tick;
+        wd->sta.connectState = ZM_STA_CONN_STATE_ASSOCIATE;
+    }
+
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessAuth               */
+/*      Process authenticate management frame.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : auth frame buffer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+/* Note : AP allows one authenticating STA at a time, does not          */
+/*        support multiple authentication process. Make sure            */
+/*        authentication state machine will not be blocked due          */
+/*        to incompleted authentication handshake.                      */
+void zfStaProcessAuth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+    struct zsWlanAuthFrameHeader* pAuthFrame;
+    u8_t  pBuf[sizeof(struct zsWlanAuthFrameHeader)];
+    u32_t p1, p2;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if ( !zfStaIsConnecting(dev) )
+    {
+        return;
+    }
+
+    pAuthFrame = (struct zsWlanAuthFrameHeader*) pBuf;
+    zfCopyFromRxBuffer(dev, buf, pBuf, 0, sizeof(struct zsWlanAuthFrameHeader));
+
+    if ( wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_OPEN )
+    {
+        if ( (zmw_le16_to_cpu(pAuthFrame->seq) == 2)&&
+             (zmw_le16_to_cpu(pAuthFrame->algo) == 0)&&
+             (zmw_le16_to_cpu(pAuthFrame->status) == 0) )
+        {
+
+            zmw_enter_critical_section(dev);
+            wd->sta.connectTimer = wd->tick;
+            zm_debug_msg0("ZM_STA_CONN_STATE_AUTH_COMPLETED");
+            wd->sta.connectState = ZM_STA_CONN_STATE_AUTH_COMPLETED;
+            zmw_leave_critical_section(dev);
+
+            //Set channel according to AP's configuration
+            //Move to here because of Cisco 11n AP feature
+            zfCoreSetFrequencyEx(dev, wd->frequency, wd->BandWidth40,
+                    wd->ExtOffset, zfAuthFreqCompleteCb);
+
+            /* send association frame */
+            if ( wd->sta.connectByReasso )
+            {
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_REASOCREQ,
+                              wd->sta.bssid, 0, 0, 0);
+            }
+            else
+            {
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_ASOCREQ,
+                              wd->sta.bssid, 0, 0, 0);
+            }
+
+
+        }
+        else
+        {
+            zm_debug_msg1("authentication failed, status = ",
+                          pAuthFrame->status);
+
+            if (wd->sta.authMode == ZM_AUTH_MODE_AUTO)
+            {
+                wd->sta.bIsSharedKey = 1;
+                zfStaStartConnect(dev, wd->sta.bIsSharedKey);
+            }
+            else
+            {
+                zm_debug_msg0("ZM_STA_STATE_DISCONNECT");
+                zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_AUTH_FAILED, wd->sta.bssid, 3);
+            }
+        }
+    }
+    else if ( wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_SHARE_1 )
+    {
+        if ( (zmw_le16_to_cpu(pAuthFrame->algo) == 1) &&
+             (zmw_le16_to_cpu(pAuthFrame->seq) == 2) &&
+             (zmw_le16_to_cpu(pAuthFrame->status) == 0))
+              //&& (pAuthFrame->challengeText[1] <= 255) )
+        {
+            zfMemoryCopy(wd->sta.challengeText, pAuthFrame->challengeText,
+                         pAuthFrame->challengeText[1]+2);
+
+            /* send the 3rd authentication frame */
+            p1 = 0x30001;
+            p2 = 0;
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_AUTH,
+                          wd->sta.bssid, p1, p2, 0);
+
+            zmw_enter_critical_section(dev);
+            wd->sta.connectTimer = wd->tick;
+
+            zm_debug_msg0("ZM_STA_SUB_STATE_AUTH_SHARE_2");
+            wd->sta.connectState = ZM_STA_CONN_STATE_AUTH_SHARE_2;
+            zmw_leave_critical_section(dev);
+        }
+        else
+        {
+            zm_debug_msg1("authentication failed, status = ",
+                          pAuthFrame->status);
+
+            zm_debug_msg0("ZM_STA_STATE_DISCONNECT");
+            zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_AUTH_FAILED, wd->sta.bssid, 3);
+        }
+    }
+    else if ( wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_SHARE_2 )
+    {
+        if ( (zmw_le16_to_cpu(pAuthFrame->algo) == 1)&&
+             (zmw_le16_to_cpu(pAuthFrame->seq) == 4)&&
+             (zmw_le16_to_cpu(pAuthFrame->status) == 0) )
+        {
+            //Set channel according to AP's configuration
+            //Move to here because of Cisco 11n AP feature
+            zfCoreSetFrequencyEx(dev, wd->frequency, wd->BandWidth40,
+                    wd->ExtOffset, NULL);
+
+            /* send association frame */
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_ASOCREQ,
+                          wd->sta.bssid, 0, 0, 0);
+
+            zmw_enter_critical_section(dev);
+            wd->sta.connectTimer = wd->tick;
+
+            zm_debug_msg0("ZM_STA_SUB_STATE_ASSOCIATE");
+            wd->sta.connectState = ZM_STA_CONN_STATE_ASSOCIATE;
+            zmw_leave_critical_section(dev);
+        }
+        else
+        {
+            zm_debug_msg1("authentication failed, status = ",
+                          pAuthFrame->status);
+
+            zm_debug_msg0("ZM_STA_STATE_DISCONNECT");
+            zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_AUTH_FAILED, wd->sta.bssid, 3);
+        }
+    }
+    else
+    {
+        zm_debug_msg0("unknown case");
+    }
+}
+
+void zfStaProcessAsocReq(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+
+    return;
+}
+
+void zfStaProcessAsocRsp(zdev_t* dev, zbuf_t* buf)
+{
+    struct zsWlanAssoFrameHeader* pAssoFrame;
+    u8_t  pBuf[sizeof(struct zsWlanAssoFrameHeader)];
+    u16_t offset;
+    u32_t i;
+    u32_t oneTxStreamCap;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( !zfStaIsConnecting(dev) )
+    {
+        return;
+    }
+
+    pAssoFrame = (struct zsWlanAssoFrameHeader*) pBuf;
+    zfCopyFromRxBuffer(dev, buf, pBuf, 0, sizeof(struct zsWlanAssoFrameHeader));
+
+    if ( wd->sta.connectState == ZM_STA_CONN_STATE_ASSOCIATE )
+    {
+        if ( pAssoFrame->status == 0 )
+        {
+            zm_debug_msg0("ZM_STA_STATE_CONNECTED");
+
+            if (wd->sta.EnableHT == 1)
+            {
+                wd->sta.wmeConnected = 1;
+            }
+            if ((wd->sta.wmeEnabled & ZM_STA_WME_ENABLE_BIT) != 0) //WME enabled
+            {
+                /* Asoc rsp may contain WME parameter element */
+                if ((offset = zfFindWifiElement(dev, buf, 2, 1)) != 0xffff)
+                {
+                    zm_debug_msg0("WME enable");
+                    wd->sta.wmeConnected = 1;
+                    if ((wd->sta.wmeEnabled & ZM_STA_UAPSD_ENABLE_BIT) != 0)
+                    {
+                        if ((zmw_rx_buf_readb(dev, buf, offset+8) & 0x80) != 0)
+                        {
+                            zm_debug_msg0("UAPSD enable");
+                            wd->sta.qosInfo = wd->sta.wmeQosInfo;
+                        }
+                    }
+
+                    zfStaUpdateWmeParameter(dev, buf);
+                }
+            }
+
+
+            //Store asoc response frame body, for VISTA only
+            wd->sta.asocRspFrameBodySize = zfwBufGetSize(dev, buf)-24;
+            if (wd->sta.asocRspFrameBodySize > ZM_CACHED_FRAMEBODY_SIZE)
+            {
+                wd->sta.asocRspFrameBodySize = ZM_CACHED_FRAMEBODY_SIZE;
+            }
+            for (i=0; i<wd->sta.asocRspFrameBodySize; i++)
+            {
+                wd->sta.asocRspFrameBody[i] = zmw_rx_buf_readb(dev, buf, i+24);
+            }
+
+            zfStaStoreAsocRspIe(dev, buf);
+            if (wd->sta.EnableHT &&
+                ((wd->sta.ie.HtCap.HtCapInfo & HTCAP_SupChannelWidthSet) != 0) &&
+                (wd->ExtOffset != 0))
+            {
+                wd->sta.htCtrlBandwidth = 1;
+            }
+            else
+            {
+                wd->sta.htCtrlBandwidth = 0;
+            }
+
+            //Set channel according to AP's configuration
+            //zfCoreSetFrequencyEx(dev, wd->frequency, wd->BandWidth40,
+            //        wd->ExtOffset, NULL);
+
+            if (wd->sta.EnableHT == 1)
+            {
+                wd->addbaComplete = 0;
+
+                if ((wd->sta.SWEncryptEnable & ZM_SW_TKIP_ENCRY_EN) == 0 &&
+                    (wd->sta.SWEncryptEnable & ZM_SW_WEP_ENCRY_EN) == 0)
+                {
+                    wd->addbaCount = 1;
+                    zfAggSendAddbaRequest(dev, wd->sta.bssid, 0, 0);
+                    zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_ADDBA, 100);
+                }
+            }
+
+            /* set RIFS support */
+            if(wd->sta.ie.HtInfo.ChannelInfo & ExtHtCap_RIFSMode)
+            {
+                wd->sta.HT2040 = 1;
+//                zfHpSetRifs(dev, wd->sta.EnableHT, 1, (wd->sta.currentFrequency < 3000)? 1:0);
+            }
+
+            wd->sta.aid = pAssoFrame->aid & 0x3fff;
+            wd->sta.oppositeCount = 0;    /* reset opposite count */
+            zfStaSetOppositeInfoFromRxBuf(dev, buf);
+
+            wd->sta.rxBeaconCount = 16;
+
+            zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTED);
+            wd->sta.connPowerInHalfDbm = zfHpGetTransmitPower(dev);
+            if (wd->zfcbConnectNotify != NULL)
+            {
+                if (wd->sta.EnableHT != 0) /* 11n */
+            	{
+    		        oneTxStreamCap = (zfHpCapability(dev) & ZM_HP_CAP_11N_ONE_TX_STREAM);
+    		        if (wd->sta.htCtrlBandwidth == 1) /* HT40*/
+    		        {
+    					if(oneTxStreamCap) /* one Tx stream */
+    				    {
+    				        if (wd->sta.SG40)
+    				        {
+    				            wd->CurrentTxRateKbps = 150000;
+    						    wd->CurrentRxRateKbps = 300000;
+    				        }
+    				        else
+    				        {
+    				            wd->CurrentTxRateKbps = 135000;
+    						    wd->CurrentRxRateKbps = 270000;
+    				        }
+    				    }
+    				    else /* Two Tx streams */
+    				    {
+    				        if (wd->sta.SG40)
+    				        {
+    				            wd->CurrentTxRateKbps = 300000;
+    						    wd->CurrentRxRateKbps = 300000;
+    				        }
+    				        else
+    				        {
+    				            wd->CurrentTxRateKbps = 270000;
+    						    wd->CurrentRxRateKbps = 270000;
+    				        }
+    				    }
+    		        }
+    		        else /* HT20 */
+    		        {
+    		            if(oneTxStreamCap) /* one Tx stream */
+    				    {
+    				        wd->CurrentTxRateKbps = 650000;
+    						wd->CurrentRxRateKbps = 130000;
+    				    }
+    				    else /* Two Tx streams */
+    				    {
+    				        wd->CurrentTxRateKbps = 130000;
+    					    wd->CurrentRxRateKbps = 130000;
+    				    }
+    		        }
+                }
+                else /* 11abg */
+                {
+                    if (wd->sta.connection_11b != 0)
+                    {
+                        wd->CurrentTxRateKbps = 11000;
+    			        wd->CurrentRxRateKbps = 11000;
+                    }
+                    else
+                    {
+                        wd->CurrentTxRateKbps = 54000;
+    			        wd->CurrentRxRateKbps = 54000;
+    			    }
+                }
+
+
+                wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECT, wd->sta.bssid);
+            }
+            wd->sta.connectByReasso = TRUE;
+            wd->sta.failCntOfReasso = 0;
+
+            zfPowerSavingMgrConnectNotify(dev);
+
+            /* Disable here because fixed rate is only for test, TBD. */
+            //if (wd->sta.EnableHT)
+            //{
+            //    wd->txMCS = 7; //Rate = 65Mbps
+            //    wd->txMT = 2; // Ht rate
+            //    wd->enableAggregation = 2; // Enable Aggregation
+            //}
+        }
+        else
+        {
+            zm_debug_msg1("association failed, status = ",
+                          pAssoFrame->status);
+
+            zm_debug_msg0("ZM_STA_STATE_DISCONNECT");
+            wd->sta.connectByReasso = FALSE;
+            zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_ASOC_FAILED, wd->sta.bssid, 3);
+        }
+    }
+
+}
+
+void zfStaStoreAsocRspIe(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t offset;
+    u32_t i;
+    u16_t length;
+    u8_t  *htcap;
+    u8_t  asocBw40 = 0;
+    u8_t  asocExtOffset = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<wd->sta.asocRspFrameBodySize; i++)
+    {
+        wd->sta.asocRspFrameBody[i] = zmw_rx_buf_readb(dev, buf, i+24);
+    }
+
+    /* HT capabilities: 28 octets */
+    if (    ((wd->sta.currentFrequency > 3000) && !(wd->supportMode & ZM_WIRELESS_MODE_5_N))
+         || ((wd->sta.currentFrequency < 3000) && !(wd->supportMode & ZM_WIRELESS_MODE_24_N)) )
+    {
+        /* not 11n AP */
+        htcap = (u8_t *)&wd->sta.ie.HtCap;
+        for (i=0; i<28; i++)
+        {
+            htcap[i] = 0;
+        }
+        wd->BandWidth40 = 0;
+        wd->ExtOffset = 0;
+        return;
+    }
+
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_HT_CAPABILITY)) != 0xffff)
+    {
+        /* atheros pre n */
+        zm_debug_msg0("atheros pre n");
+        htcap = (u8_t *)&wd->sta.ie.HtCap;
+        htcap[0] = zmw_rx_buf_readb(dev, buf, offset);
+        htcap[1] = 26;
+        for (i=1; i<=26; i++)
+        {
+            htcap[i+1] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_msg2_mm(ZM_LV_1, "ASOC:  HT Capabilities, htcap=", htcap[i+1]);
+        }
+    }
+    else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTCAPABILITY)) != 0xffff)
+    {
+        /* pre n 2.0 standard */
+        zm_debug_msg0("pre n 2.0 standard");
+        htcap = (u8_t *)&wd->sta.ie.HtCap;
+        for (i=0; i<28; i++)
+        {
+            htcap[i] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_msg2_mm(ZM_LV_1, "ASOC:  HT Capabilities, htcap=", htcap[i]);
+        }
+    }
+    else
+    {
+        /* not 11n AP */
+        htcap = (u8_t *)&wd->sta.ie.HtCap;
+        for (i=0; i<28; i++)
+        {
+            htcap[i] = 0;
+        }
+        wd->BandWidth40 = 0;
+        wd->ExtOffset = 0;
+        return;
+    }
+
+    asocBw40 = (u8_t)((wd->sta.ie.HtCap.HtCapInfo & HTCAP_SupChannelWidthSet) >> 1);
+
+    /* HT information */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY)) != 0xffff)
+    {
+        /* atheros pre n */
+        zm_debug_msg0("atheros pre n HTINFO");
+        length = 22;
+        htcap = (u8_t *)&wd->sta.ie.HtInfo;
+        htcap[0] = zmw_rx_buf_readb(dev, buf, offset);
+        htcap[1] = 22;
+        for (i=1; i<=22; i++)
+        {
+            htcap[i+1] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_msg2_mm(ZM_LV_1, "ASOC:  HT Info, htinfo=", htcap[i+1]);
+        }
+    }
+    else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTINFORMATION)) != 0xffff)
+    {
+        /* pre n 2.0 standard */
+        zm_debug_msg0("pre n 2.0 standard HTINFO");
+        length = zmw_rx_buf_readb(dev, buf, offset + 1);
+        htcap = (u8_t *)&wd->sta.ie.HtInfo;
+        for (i=0; i<24; i++)
+        {
+            htcap[i] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_msg2_mm(ZM_LV_1, "ASOC:  HT Info, htinfo=", htcap[i]);
+        }
+    }
+    else
+    {
+        zm_debug_msg0("no HTINFO");
+        htcap = (u8_t *)&wd->sta.ie.HtInfo;
+        for (i=0; i<24; i++)
+        {
+            htcap[i] = 0;
+        }
+    }
+    asocExtOffset = wd->sta.ie.HtInfo.ChannelInfo & ExtHtCap_ExtChannelOffsetBelow;
+
+    if ((wd->sta.EnableHT == 1) && (asocBw40 == 1) && ((asocExtOffset == 1) || (asocExtOffset == 3)))
+    {
+        wd->BandWidth40 = asocBw40;
+        wd->ExtOffset = asocExtOffset;
+    }
+    else
+    {
+        wd->BandWidth40 = 0;
+        wd->ExtOffset = 0;
+    }
+
+    return;
+}
+
+void zfStaProcessDeauth(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t apMacAddr[3];
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* STA : if SA=connected AP then disconnect with AP */
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        apMacAddr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET);
+        apMacAddr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+2);
+        apMacAddr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+4);
+  	if ((apMacAddr[0] == wd->sta.bssid[0]) && (apMacAddr[1] == wd->sta.bssid[1]) && (apMacAddr[2] == wd->sta.bssid[2]))
+        {
+            if (zfwBufGetSize(dev, buf) >= 24+2) //not a malformed frame
+            {
+                if ( zfStaIsConnected(dev) )
+                {
+                    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_DEAUTH, wd->sta.bssid, 2);
+                }
+                else if (zfStaIsConnecting(dev))
+                {
+                    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_AUTH_FAILED, wd->sta.bssid, 3);
+                }
+                else
+                {
+                }
+            }
+        }
+    }
+    else if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        u16_t peerMacAddr[3];
+        u8_t  peerIdx;
+        s8_t  res;
+
+        if ( zfStaIsConnected(dev) )
+        {
+            peerMacAddr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+            peerMacAddr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+2);
+            peerMacAddr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+4);
+
+            zmw_enter_critical_section(dev);
+            res = zfStaFindOppositeByMACAddr(dev, peerMacAddr, &peerIdx);
+            if ( res == 0 )
+            {
+                wd->sta.oppositeInfo[peerIdx].aliveCounter = 0;
+            }
+            zmw_leave_critical_section(dev);
+        }
+    }
+}
+
+void zfStaProcessDisasoc(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t apMacAddr[3];
+
+    zmw_get_wlan_dev(dev);
+
+    /* STA : if SA=connected AP then disconnect with AP */
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        apMacAddr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET);
+        apMacAddr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+2);
+        apMacAddr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+4);
+
+        if ((apMacAddr[0] == wd->sta.bssid[0]) && (apMacAddr[1] == wd->sta.bssid[1]) && (apMacAddr[2] == wd->sta.bssid[2]))
+        {
+            if (zfwBufGetSize(dev, buf) >= 24+2) //not a malformed frame
+            {
+                if ( zfStaIsConnected(dev) )
+                {
+                    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_DISASOC, wd->sta.bssid, 2);
+                }
+                else
+                {
+                    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_ASOC_FAILED, wd->sta.bssid, 3);
+                }
+            }
+        }
+    }
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessProbeReq           */
+/*      Process probe request management frame.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : auth frame buffer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfStaProcessProbeReq(zdev_t* dev, zbuf_t* buf, u16_t* src)
+{
+    u16_t offset;
+    u8_t len;
+    u16_t i, j;
+    u16_t sendFlag;
+
+    zmw_get_wlan_dev(dev);
+
+    /* check mode : AP/IBSS */
+    if ((wd->wlanMode != ZM_MODE_AP) || (wd->wlanMode != ZM_MODE_IBSS))
+    {
+        zm_msg0_mm(ZM_LV_3, "Ignore probe req");
+        return;
+    }
+
+    /* check SSID */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID)) == 0xffff)
+    {
+        zm_msg0_mm(ZM_LV_3, "probe req SSID not found");
+        return;
+    }
+
+    len = zmw_rx_buf_readb(dev, buf, offset+1);
+
+    for (i=0; i<ZM_MAX_AP_SUPPORT; i++)
+    {
+        if ((wd->ap.apBitmap & (i<<i)) != 0)
+        {
+            sendFlag = 0;
+            /* boardcast SSID */
+            if ((len == 0) && (wd->ap.hideSsid[i] == 0))
+            {
+                sendFlag = 1;
+            }
+            /* Not broadcast SSID */
+            else if (wd->ap.ssidLen[i] == len)
+            {
+                for (j=0; j<len; j++)
+                {
+                    if (zmw_rx_buf_readb(dev, buf, offset+1+j)
+                            != wd->ap.ssid[i][j])
+                    {
+                        break;
+                    }
+                }
+                if (j == len)
+                {
+                    sendFlag = 1;
+                }
+            }
+            if (sendFlag == 1)
+            {
+                /* Send probe response */
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBERSP, src, i, 0, 0);
+            }
+        }
+    }
+}
+
+void zfStaProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo)
+{
+    /* return if not channel scan */
+    // Probe response is sent with unicast. Is this required?
+    // IBSS would send probe request and the code below would prevent
+    // the probe response from handling.
+    #if 0
+    zmw_get_wlan_dev(dev);
+
+    if ( !wd->sta.bChannelScan )
+    {
+        return;
+    }
+    #endif
+
+    zfProcessProbeRsp(dev, buf, AddInfo);
+}
+
+void zfIBSSSetupBssDesc(zdev_t *dev)
+{
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    u8_t i;
+#endif
+    struct zsBssInfo *pBssInfo;
+    u16_t offset = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    pBssInfo = &wd->sta.ibssBssDesc;
+    zfZeroMemory((u8_t *)pBssInfo, sizeof(struct zsBssInfo));
+
+    pBssInfo->signalStrength = 100;
+
+    zfMemoryCopy((u8_t *)pBssInfo->macaddr, (u8_t *)wd->macAddr,6);
+    zfMemoryCopy((u8_t *)pBssInfo->bssid, (u8_t *)wd->sta.bssid, 6);
+
+    pBssInfo->beaconInterval[0] = (u8_t)(wd->beaconInterval) ;
+    pBssInfo->beaconInterval[1] = (u8_t)((wd->beaconInterval) >> 8) ;
+
+    pBssInfo->capability[0] = wd->sta.capability[0];
+    pBssInfo->capability[1] = wd->sta.capability[1];
+
+    pBssInfo->ssid[0] = ZM_WLAN_EID_SSID;
+    pBssInfo->ssid[1] = wd->sta.ssidLen;
+    zfMemoryCopy((u8_t *)&pBssInfo->ssid[2], (u8_t *)wd->sta.ssid, wd->sta.ssidLen);
+    zfMemoryCopy((u8_t *)&pBssInfo->frameBody[offset], (u8_t *)pBssInfo->ssid,
+                 wd->sta.ssidLen + 2);
+    offset += wd->sta.ssidLen + 2;
+
+    /* support rate */
+
+    /* DS parameter set */
+    pBssInfo->channel = zfChFreqToNum(wd->frequency, NULL);
+    pBssInfo->frequency = wd->frequency;
+    pBssInfo->atimWindow = wd->sta.atimWindow;
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK )
+    {
+        u8_t rsn[64]=
+        {
+                    /* Element ID */
+                    0x30,
+                    /* Length */
+                    0x14,
+                    /* Version */
+                    0x01, 0x00,
+                    /* Group Cipher Suite, default=TKIP */
+                    0x00, 0x0f, 0xac, 0x04,
+                    /* Pairwise Cipher Suite Count */
+                    0x01, 0x00,
+                    /* Pairwise Cipher Suite, default=TKIP */
+                    0x00, 0x0f, 0xac, 0x02,
+                    /* Authentication and Key Management Suite Count */
+                    0x01, 0x00,
+                    /* Authentication type, default=PSK */
+                    0x00, 0x0f, 0xac, 0x02,
+                    /* RSN capability */
+                    0x00, 0x00
+        };
+
+        /* Overwrite Group Cipher Suite by AP's setting */
+        zfMemoryCopy(rsn+4, zgWpa2AesOui, 4);
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+        {
+            /* Overwrite Pairwise Cipher Suite by AES */
+            zfMemoryCopy(rsn+10, zgWpa2AesOui, 4);
+        }
+
+        // RSN element id
+        pBssInfo->frameBody[offset++] = ZM_WLAN_EID_RSN_IE ;
+
+        // RSN length
+        pBssInfo->frameBody[offset++] = rsn[1] ;
+
+        // RSN information
+        for(i=0; i<rsn[1]; i++)
+        {
+            pBssInfo->frameBody[offset++] = rsn[i+2] ;
+        }
+
+        zfMemoryCopy(pBssInfo->rsnIe, rsn, rsn[1]+2);
+    }
+#endif
+}
+
+void zfIbssConnectNetwork(zdev_t* dev)
+{
+    struct zsBssInfo* pBssInfo;
+    struct zsBssInfo tmpBssInfo;
+    u8_t   macAddr[6], bssid[6], bssNotFound = TRUE;
+    u16_t  i, j=100;
+    u16_t  k;
+    struct zsPartnerNotifyEvent event;
+    u32_t  channelFlags;
+    u16_t  oppositeWepStatus;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /* change state to CONNECTING and stop the channel scanning */
+    zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTING);
+    zfPowerSavingMgrWakeup(dev);
+
+    /* Set TxQs CWMIN, CWMAX, AIFS and TXO to WME STA default. */
+    zfUpdateDefaultQosParameter(dev, 0);
+
+    wd->sta.bProtectionMode = FALSE;
+    zfHpSetSlotTime(dev, 1);
+
+    /* ESS bit off */
+    wd->sta.capability[0] &= ~ZM_BIT_0;
+    /* IBSS bit on */
+    wd->sta.capability[0] |= ZM_BIT_1;
+    /* not not use short slot time */
+    wd->sta.capability[1] &= ~ZM_BIT_2;
+
+    wd->sta.wmeConnected = 0;
+    wd->sta.psMgr.tempWakeUp = 0;
+    wd->sta.qosInfo = 0;
+    wd->sta.EnableHT = 0;
+    wd->BandWidth40 = 0;
+    wd->ExtOffset = 0;
+
+    if ( wd->sta.bssList.bssCount )
+    {
+        //Reorder BssList by RSSI--CWYang(+)
+        zfBssInfoReorderList(dev);
+
+        zmw_enter_critical_section(dev);
+
+        pBssInfo = wd->sta.bssList.head;
+
+        for(i=0; i<wd->sta.bssList.bssCount; i++)
+        {
+            // 20070806 #1 Privacy bit
+            if ( pBssInfo->capability[0] & ZM_BIT_4 )
+            { // Privacy Ibss network
+//                zm_debug_msg0("Privacy bit on");
+                oppositeWepStatus = ZM_ENCRYPTION_WEP_ENABLED;
+
+                if ( pBssInfo->rsnIe[1] != 0 )
+                {
+                    if ( (pBssInfo->rsnIe[7] == 0x01) || (pBssInfo->rsnIe[7] == 0x05) )
+                    { // WEP-40 & WEP-104
+//                        zm_debug_msg0("WEP40 or WEP104");
+                        oppositeWepStatus = ZM_ENCRYPTION_WEP_ENABLED;
+                    }
+                    else if ( pBssInfo->rsnIe[7] == 0x02 )
+                    { // TKIP
+//                        zm_debug_msg0("TKIP");
+                        oppositeWepStatus = ZM_ENCRYPTION_TKIP;
+                    }
+                    else if ( pBssInfo->rsnIe[7] == 0x04 )
+                    { // AES
+//                        zm_debug_msg0("CCMP-AES");
+                        oppositeWepStatus = ZM_ENCRYPTION_AES;
+                    }
+                }
+            }
+            else
+            {
+//                zm_debug_msg0("Privacy bit off");
+                oppositeWepStatus = ZM_ENCRYPTION_WEP_DISABLED;
+            }
+
+            if ( (zfMemoryIsEqual(&(pBssInfo->ssid[2]), wd->sta.ssid,
+                                  wd->sta.ssidLen))&&
+                 (wd->sta.ssidLen == pBssInfo->ssid[1])&&
+                 (oppositeWepStatus == wd->sta.wepStatus) )
+            {
+                /* Check support mode */
+                if (pBssInfo->frequency > 3000) {
+                    if (   (pBssInfo->EnableHT == 1)
+                        || (pBssInfo->apCap & ZM_All11N_AP) ) //11n AP
+                    {
+                        channelFlags = CHANNEL_A_HT;
+                        if (pBssInfo->enableHT40 == 1) {
+                            channelFlags |= CHANNEL_HT40;
+                        }
+                    } else {
+                        channelFlags = CHANNEL_A;
+                    }
+                } else {
+                    if (   (pBssInfo->EnableHT == 1)
+                        || (pBssInfo->apCap & ZM_All11N_AP) ) //11n AP
+                    {
+                        channelFlags = CHANNEL_G_HT;
+                        if(pBssInfo->enableHT40 == 1) {
+                            channelFlags |= CHANNEL_HT40;
+                        }
+                    } else {
+                        if (pBssInfo->extSupportedRates[1] == 0) {
+                            channelFlags = CHANNEL_B;
+                        } else {
+                            channelFlags = CHANNEL_G;
+                        }
+                    }
+                }
+
+                if (   ((channelFlags == CHANNEL_B) && (wd->connectMode & ZM_BIT_0))
+                    || ((channelFlags == CHANNEL_G) && (wd->connectMode & ZM_BIT_1))
+                    || ((channelFlags == CHANNEL_A) && (wd->connectMode & ZM_BIT_2))
+                    || ((channelFlags & CHANNEL_HT20) && (wd->connectMode & ZM_BIT_3)) )
+                {
+                    pBssInfo = pBssInfo->next;
+                    continue;
+                }
+
+                /* Bypass DFS channel */
+                if (zfHpIsDfsChannelNCS(dev, pBssInfo->frequency))
+                {
+                    zm_debug_msg0("Bypass DFS channel");
+                    continue;
+                }
+
+                /* check IBSS bit */
+                if ( pBssInfo->capability[0] & ZM_BIT_1 )
+                {
+                    /* may check timestamp here */
+                    j = i;
+                    break;
+                }
+            }
+
+            pBssInfo = pBssInfo->next;
+        }
+
+        if ((j < wd->sta.bssList.bssCount) && (pBssInfo != NULL))
+        {
+            zfwMemoryCopy((u8_t*)&tmpBssInfo, (u8_t*)(pBssInfo), sizeof(struct zsBssInfo));
+            pBssInfo = &tmpBssInfo;
+        }
+        else
+        {
+            pBssInfo = NULL;
+        }
+
+        zmw_leave_critical_section(dev);
+
+        //if ( j < wd->sta.bssList.bssCount )
+        if (pBssInfo != NULL)
+        {
+            int res;
+
+            zm_debug_msg0("IBSS found");
+
+            /* Found IBSS, reset bssNotFoundCount */
+            zmw_enter_critical_section(dev);
+            wd->sta.bssNotFoundCount = 0;
+            zmw_leave_critical_section(dev);
+
+            bssNotFound = FALSE;
+            wd->sta.atimWindow = pBssInfo->atimWindow;
+            wd->frequency = pBssInfo->frequency;
+            //wd->sta.flagFreqChanging = 1;
+            zfCoreSetFrequency(dev, wd->frequency);
+            zfUpdateBssid(dev, pBssInfo->bssid);
+            zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_ZERO);
+            zfUpdateSupportRate(dev, pBssInfo->supportedRates);
+            zfUpdateSupportRate(dev, pBssInfo->extSupportedRates);
+            wd->beaconInterval = pBssInfo->beaconInterval[0] +
+                                 (((u16_t) pBssInfo->beaconInterval[1]) << 8);
+
+            if (wd->beaconInterval == 0)
+            {
+                wd->beaconInterval = 100;
+            }
+
+            /* rsn information element */
+            if ( pBssInfo->rsnIe[1] != 0 )
+            {
+                zfMemoryCopy(wd->sta.rsnIe, pBssInfo->rsnIe,
+                             pBssInfo->rsnIe[1]+2);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+                /* If not use RSNA , run traditional */
+                zmw_enter_critical_section(dev);
+                wd->sta.ibssWpa2Psk = 1;
+                zmw_leave_critical_section(dev);
+#endif
+            }
+            else
+            {
+                wd->sta.rsnIe[1] = 0;
+            }
+
+            /* privacy bit */
+            if ( pBssInfo->capability[0] & ZM_BIT_4 )
+            {
+                wd->sta.capability[0] |= ZM_BIT_4;
+            }
+            else
+            {
+                wd->sta.capability[0] &= ~ZM_BIT_4;
+            }
+
+            /* preamble type */
+            wd->preambleTypeInUsed = wd->preambleType;
+            if ( wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_AUTO )
+            {
+                if (pBssInfo->capability[0] & ZM_BIT_5)
+                {
+                    wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_SHORT;
+                }
+                else
+                {
+                    wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_LONG;
+                }
+            }
+
+            if (wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_LONG)
+            {
+                wd->sta.capability[0] &= ~ZM_BIT_5;
+            }
+            else
+            {
+                wd->sta.capability[0] |= ZM_BIT_5;
+            }
+
+            wd->sta.beaconFrameBodySize = pBssInfo->frameBodysize + 12;
+
+            if (wd->sta.beaconFrameBodySize > ZM_CACHED_FRAMEBODY_SIZE)
+            {
+                wd->sta.beaconFrameBodySize = ZM_CACHED_FRAMEBODY_SIZE;
+            }
+
+            for (k=0; k<8; k++)
+            {
+                wd->sta.beaconFrameBody[k] = pBssInfo->timeStamp[k];
+            }
+            wd->sta.beaconFrameBody[8] = pBssInfo->beaconInterval[0];
+            wd->sta.beaconFrameBody[9] = pBssInfo->beaconInterval[1];
+            wd->sta.beaconFrameBody[10] = pBssInfo->capability[0];
+            wd->sta.beaconFrameBody[11] = pBssInfo->capability[1];
+            //for (k=12; k<wd->sta.beaconFrameBodySize; k++)
+            for (k=0; k<pBssInfo->frameBodysize; k++)
+            {
+                wd->sta.beaconFrameBody[k+12] = pBssInfo->frameBody[k];
+            }
+
+            zmw_enter_critical_section(dev);
+            res = zfStaSetOppositeInfoFromBSSInfo(dev, pBssInfo);
+            if ( res == 0 )
+            {
+                zfMemoryCopy(event.bssid, (u8_t *)(pBssInfo->bssid), 6);
+                zfMemoryCopy(event.peerMacAddr, (u8_t *)(pBssInfo->macaddr), 6);
+            }
+            zmw_leave_critical_section(dev);
+
+            //zfwIbssPartnerNotify(dev, 1, &event);
+            goto connect_done;
+        }
+    }
+
+    /* IBSS not found */
+    if ( bssNotFound )
+    {
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+        u16_t offset ;
+#endif
+        if ( wd->sta.ibssJoinOnly )
+        {
+            zm_debug_msg0("IBSS join only...retry...");
+            goto retry_ibss;
+        }
+
+        if(wd->sta.bssNotFoundCount<2)
+        {
+            zmw_enter_critical_section(dev);
+            zm_debug_msg1("IBSS not found, do sitesurvey!!  bssNotFoundCount=", wd->sta.bssNotFoundCount);
+            wd->sta.bssNotFoundCount++;
+            zmw_leave_critical_section(dev);
+            goto retry_ibss;
+        }
+        else
+        {
+            zmw_enter_critical_section(dev);
+            /* Fail IBSS found, TODO create IBSS */
+            wd->sta.bssNotFoundCount = 0;
+            zmw_leave_critical_section(dev);
+        }
+
+
+        if (zfHpIsDfsChannel(dev, wd->frequency))
+        {
+            wd->frequency = zfHpFindFirstNonDfsChannel(dev, wd->frequency > 3000);
+        }
+
+        if( wd->ws.autoSetFrequency == 0 )
+        { /* Auto set frequency */
+            zm_debug_msg1("Create Ad Hoc Network Band ", wd->ws.adhocMode);
+            wd->frequency = zfFindCleanFrequency(dev, wd->ws.adhocMode);
+            wd->ws.autoSetFrequency = 0xff;
+        }
+        zm_debug_msg1("IBSS not found, created one in channel ", wd->frequency);
+
+        wd->sta.ibssBssIsCreator = 1;
+
+        //wd->sta.flagFreqChanging = 1;
+        zfCoreSetFrequency(dev, wd->frequency);
+        if (wd->sta.bDesiredBssid == TRUE)
+        {
+            for (k=0; k<6; k++)
+            {
+                bssid[k] = wd->sta.desiredBssid[k];
+            }
+        }
+        else
+        {
+            #if 1
+            macAddr[0] = (wd->macAddr[0] & 0xff);
+            macAddr[1] = (wd->macAddr[0] >> 8);
+            macAddr[2] = (wd->macAddr[1] & 0xff);
+            macAddr[3] = (wd->macAddr[1] >> 8);
+            macAddr[4] = (wd->macAddr[2] & 0xff);
+            macAddr[5] = (wd->macAddr[2] >> 8);
+            zfGenerateRandomBSSID(dev, (u8_t *)wd->macAddr, (u8_t *)bssid);
+            #else
+            for (k=0; k<6; k++)
+            {
+                bssid[k] = (u8_t) zfGetRandomNumber(dev, 0);
+            }
+            bssid[0] &= ~ZM_BIT_0;
+            bssid[0] |= ZM_BIT_1;
+            #endif
+        }
+
+        zfUpdateBssid(dev, bssid);
+        //wd->sta.atimWindow = 0x0a;
+
+        /* rate information */
+        if(wd->frequency <= ZM_CH_G_14)  // 2.4 GHz  b+g
+        {
+            if ( wd->wfc.bIbssGMode
+                 && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )
+            {
+                zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_IBSS_AG);
+            }
+            else
+            {
+                zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_IBSS_B);
+            }
+        } else {
+            zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_IBSS_AG);
+        }
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_DISABLED )
+        {
+            wd->sta.capability[0] &= ~ZM_BIT_4;
+        }
+        else
+        {
+            wd->sta.capability[0] |= ZM_BIT_4;
+        }
+
+        wd->preambleTypeInUsed = wd->preambleType;
+        if (wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_LONG)
+        {
+            wd->sta.capability[0] &= ~ZM_BIT_5;
+        }
+        else
+        {
+            wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_SHORT;
+            wd->sta.capability[0] |= ZM_BIT_5;
+        }
+
+        zfIBSSSetupBssDesc(dev);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+
+        // 20070411 Add WPA2PSK information to its IBSS network !!!
+        offset = 0 ;
+
+        /* timestamp */
+        offset += 8 ;
+
+        /* beacon interval */
+        wd->sta.beaconFrameBody[offset++] = (u8_t)(wd->beaconInterval) ;
+        wd->sta.beaconFrameBody[offset++] = (u8_t)((wd->beaconInterval) >> 8) ;
+
+        /* capability information */
+        wd->sta.beaconFrameBody[offset++] = wd->sta.capability[0] ;
+        wd->sta.beaconFrameBody[offset++] = wd->sta.capability[1] ;
+        #if 0
+        /* ssid */
+        // ssid element id
+        wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_SSID ;
+        // ssid length
+        wd->sta.beaconFrameBody[offset++] = wd->sta.ssidLen ;
+        // ssid information
+        for(i=0; i<wd->sta.ssidLen; i++)
+        {
+            wd->sta.beaconFrameBody[offset++] = wd->sta.ssid[i] ;
+        }
+
+        /* support rate */
+        rateSet = ZM_RATE_SET_CCK ;
+        if ( (rateSet == ZM_RATE_SET_OFDM)&&((wd->gRate & 0xff) == 0) )
+        {
+            offset += 0 ;
+        }
+        else
+        {
+            // support rate element id
+            wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_SUPPORT_RATE ;
+
+            // support rate length
+            lenOffset = offset++;
+
+            // support rate information
+            for (i=0; i<4; i++)
+            {
+                if ((wd->bRate & (0x1<<i)) == (0x1<<i))
+                {
+                    wd->sta.beaconFrameBody[offset++] =
+                	    zg11bRateTbl[i]+((wd->bRateBasic & (0x1<<i))<<(7-i)) ;
+                    len++;
+                }
+            }
+
+            // support rate length
+            wd->sta.beaconFrameBody[lenOffset] = len ;
+        }
+
+        /* DS parameter set */
+        // DS parameter set elemet id
+        wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_DS ;
+
+        // DS parameter set length
+        wd->sta.beaconFrameBody[offset++] = 1 ;
+
+        // DS parameter set information
+        wd->sta.beaconFrameBody[offset++] =
+         	zfChFreqToNum(wd->frequency, NULL) ;
+
+        /* IBSS parameter set */
+        // IBSS parameter set element id
+        wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_IBSS ;
+
+        // IBSS parameter set length
+        wd->sta.beaconFrameBody[offset++] = 2 ;
+
+        // IBSS parameter set information
+        wd->sta.beaconFrameBody[offset] = wd->sta.atimWindow ;
+        offset += 2 ;
+
+        /* ERP Information and Extended Supported Rates */
+        if ( wd->wfc.bIbssGMode
+             && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )
+        {
+            /* ERP Information */
+            wd->erpElement = 0;
+            // ERP element id
+            wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_ERP ;
+
+            // ERP length
+            wd->sta.beaconFrameBody[offset++] = 1 ;
+
+            // ERP information
+            wd->sta.beaconFrameBody[offset++] = wd->erpElement ;
+
+            /* Extended Supported Rates */
+            if ( (rateSet == ZM_RATE_SET_OFDM)&&((wd->gRate & 0xff) == 0) )
+            {
+                offset += 0 ;
+            }
+            else
+            {
+                len = 0 ;
+
+                // Extended Supported Rates element id
+                wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_EXTENDED_RATE ;
+
+                // Extended Supported Rates length
+                lenOffset = offset++ ;
+
+                // Extended Supported Rates information
+                for (i=0; i<8; i++)
+                {
+                    if ((wd->gRate & (0x1<<i)) == (0x1<<i))
+                    {
+                        wd->sta.beaconFrameBody[offset++] =
+                                     zg11gRateTbl[i]+((wd->gRateBasic & (0x1<<i))<<(7-i));
+                        len++;
+                    }
+                }
+
+                // extended support rate length
+            	  wd->sta.beaconFrameBody[lenOffset] = len ;
+            }
+        }
+        #endif
+
+        /* RSN : important information influence the result of creating an IBSS network */
+        if ( wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK )
+        {
+            u8_t frameType = ZM_WLAN_FRAME_TYPE_AUTH ;
+            u8_t    rsn[64]=
+            {
+                        /* Element ID */
+                        0x30,
+                        /* Length */
+                        0x14,
+                        /* Version */
+                        0x01, 0x00,
+                        /* Group Cipher Suite, default=TKIP */
+                        0x00, 0x0f, 0xac, 0x04,
+                        /* Pairwise Cipher Suite Count */
+                        0x01, 0x00,
+                        /* Pairwise Cipher Suite, default=TKIP */
+                        0x00, 0x0f, 0xac, 0x02,
+                        /* Authentication and Key Management Suite Count */
+                        0x01, 0x00,
+                        /* Authentication type, default=PSK */
+                        0x00, 0x0f, 0xac, 0x02,
+                        /* RSN capability */
+                        0x00, 0x00
+            };
+
+            /* Overwrite Group Cipher Suite by AP's setting */
+            zfMemoryCopy(rsn+4, zgWpa2AesOui, 4);
+
+            if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+            {
+                /* Overwrite Pairwise Cipher Suite by AES */
+                zfMemoryCopy(rsn+10, zgWpa2AesOui, 4);
+            }
+
+            // RSN element id
+            wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_RSN_IE ;
+
+            // RSN length
+            wd->sta.beaconFrameBody[offset++] = rsn[1] ;
+
+            // RSN information
+            for(i=0; i<rsn[1]; i++)
+                wd->sta.beaconFrameBody[offset++] = rsn[i+2] ;
+
+            zfMemoryCopy(wd->sta.rsnIe, rsn, rsn[1]+2);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+            /* If not use RSNA , run traditional */
+            zmw_enter_critical_section(dev);
+            wd->sta.ibssWpa2Psk = 1;
+            zmw_leave_critical_section(dev);
+#endif
+        }
+
+        #if 0
+        /* HT Capabilities Info */
+        {
+            u8_t OUI[3] = { 0x0 , 0x90 , 0x4C } ;
+
+            wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_WPA_IE ;
+
+            wd->sta.beaconFrameBody[offset++] = wd->sta.HTCap.Data.Length + 4 ;
+
+            for (i = 0; i < 3; i++)
+            {
+                wd->sta.beaconFrameBody[offset++] = OUI[i] ;
+            }
+
+            wd->sta.beaconFrameBody[offset++] = wd->sta.HTCap.Data.ElementID ;
+
+            for (i = 0; i < 26; i++)
+            {
+                wd->sta.beaconFrameBody[offset++] = wd->sta.HTCap.Byte[i+2] ;
+            }
+        }
+
+        /* Extended HT Capabilities Info */
+        {
+            u8_t OUI[3] = { 0x0 , 0x90 , 0x4C } ;
+
+            wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_WPA_IE ;
+
+            wd->sta.beaconFrameBody[offset++] = wd->sta.ExtHTCap.Data.Length + 4 ;
+
+            for (i = 0; i < 3; i++)
+            {
+                wd->sta.beaconFrameBody[offset++] = OUI[i] ;
+            }
+
+            wd->sta.beaconFrameBody[offset++] = wd->sta.ExtHTCap.Data.ElementID ;
+
+            for (i = 0; i < 22; i++)
+            {
+                wd->sta.beaconFrameBody[offset++] = wd->sta.ExtHTCap.Byte[i+2] ;
+            }
+        }
+        #endif
+
+        wd->sta.beaconFrameBodySize = offset ;
+
+        if (wd->sta.beaconFrameBodySize > ZM_CACHED_FRAMEBODY_SIZE)
+        {
+            wd->sta.beaconFrameBodySize = ZM_CACHED_FRAMEBODY_SIZE;
+        }
+
+        // 20070416 Let Create IBSS network could enter the zfwIbssPartnerNotify function
+        // bssNotFound = FALSE ;
+
+        printk("The capability info 1 = %02x\n", wd->sta.capability[0]) ;
+        printk("The capability info 2 = %02x\n", wd->sta.capability[1]) ;
+        for(k=0; k<wd->sta.beaconFrameBodySize; k++)
+        {
+	     printk("%02x ", wd->sta.beaconFrameBody[k]) ;
+        }
+        #if 0
+        zmw_enter_critical_section(dev);
+        zfMemoryCopy(event.bssid, (u8_t *)bssid, 6);
+        zfMemoryCopy(event.peerMacAddr, (u8_t *)wd->macAddr, 6);
+        zmw_leave_critical_section(dev);
+        #endif
+#endif
+
+        //zmw_enter_critical_section(dev);
+        //wd->sta.ibssPartnerStatus = ZM_IBSS_PARTNER_LOST;
+        //zmw_leave_critical_section(dev);
+    }
+    else
+    {
+        wd->sta.ibssBssIsCreator = 0;
+    }
+
+connect_done:
+    zfHpEnableBeacon(dev, ZM_MODE_IBSS, wd->beaconInterval, wd->dtim, (u8_t)wd->sta.atimWindow);
+    zfStaSendBeacon(dev); // Refresh Beacon content for ZD1211B HalPlus
+    zfHpSetAtimWindow(dev, wd->sta.atimWindow);
+
+    // Start the IBSS timer to monitor for new stations
+    zmw_enter_critical_section(dev);
+    zfTimerSchedule(dev, ZM_EVENT_IBSS_MONITOR, ZM_TICK_IBSS_MONITOR);
+    zmw_leave_critical_section(dev);
+
+
+    if (wd->zfcbConnectNotify != NULL)
+    {
+        wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECT, wd->sta.bssid);
+    }
+    zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTED);
+    wd->sta.connPowerInHalfDbm = zfHpGetTransmitPower(dev);
+
+#ifdef ZM_ENABLE_IBSS_DELAYED_JOIN_INDICATION
+    if ( !bssNotFound )
+    {
+        wd->sta.ibssDelayedInd = 1;
+        zfMemoryCopy((u8_t *)&wd->sta.ibssDelayedIndEvent, (u8_t *)&event, sizeof(struct zsPartnerNotifyEvent));
+    }
+#else
+    if ( !bssNotFound )
+    {
+        if (wd->zfcbIbssPartnerNotify != NULL)
+        {
+            wd->zfcbIbssPartnerNotify(dev, 1, &event);
+        }
+    }
+#endif
+
+    return;
+
+retry_ibss:
+    zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTING);
+    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_NOT_FOUND, wd->sta.bssid, 0);
+    return;
+}
+
+void zfStaProcessAtim(zdev_t* dev, zbuf_t* buf)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg0("Receiving Atim window notification");
+
+    wd->sta.recvAtim = 1;
+}
+
+static struct zsBssInfo* zfInfraFindAPToConnect(zdev_t* dev,
+        struct zsBssInfo* candidateBss)
+{
+    struct zsBssInfo* pBssInfo;
+    struct zsBssInfo* pNowBssInfo=NULL;
+    u16_t i;
+    u16_t ret, apWepStatus;
+    u32_t k;
+    u32_t channelFlags;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    pBssInfo = wd->sta.bssList.head;
+
+    for(i=0; i<wd->sta.bssList.bssCount; i++)
+    {
+        if ( pBssInfo->capability[0] & ZM_BIT_4 )
+        {
+            apWepStatus = ZM_ENCRYPTION_WEP_ENABLED;
+        }
+        else
+        {
+            apWepStatus = ZM_ENCRYPTION_WEP_DISABLED;
+        }
+
+        if ( ((zfMemoryIsEqual(&(pBssInfo->ssid[2]), wd->sta.ssid,
+                               wd->sta.ssidLen))&&
+              (wd->sta.ssidLen == pBssInfo->ssid[1]))||
+             ((wd->sta.ssidLen == 0)&&
+               /* connect to any BSS: AP's ans STA's WEP status must match */
+              (wd->sta.wepStatus == apWepStatus )&&
+              (pBssInfo->securityType != ZM_SECURITY_TYPE_WPA) ))
+        {
+            if ( wd->sta.ssidLen == 0 )
+            {
+                zm_debug_msg0("ANY BSS found");
+            }
+
+            if ( ((wd->sta.wepStatus == ZM_ENCRYPTION_WEP_DISABLED && apWepStatus == ZM_ENCRYPTION_WEP_ENABLED) ||
+                 (wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED &&
+                 (apWepStatus == ZM_ENCRYPTION_WEP_DISABLED && wd->sta.dropUnencryptedPkts == 1))) &&
+                 (wd->sta.authMode >= ZM_AUTH_MODE_OPEN && wd->sta.authMode <= ZM_AUTH_MODE_AUTO) )
+            {
+                zm_debug_msg0("Privacy policy is inconsistent");
+                pBssInfo = pBssInfo->next;
+                continue;
+            }
+
+            /* for WPA negative test */
+            if ( !zfCheckAuthentication(dev, pBssInfo) )
+            {
+                pBssInfo = pBssInfo->next;
+                continue;
+            }
+
+            /* Check bssid */
+            if (wd->sta.bDesiredBssid == TRUE)
+            {
+                for (k=0; k<6; k++)
+                {
+                    if (wd->sta.desiredBssid[k] != pBssInfo->bssid[k])
+                    {
+                        zm_msg0_mm(ZM_LV_1, "desired bssid not matched 1");
+                        break;
+                    }
+                }
+
+                if (k != 6)
+                {
+                    zm_msg0_mm(ZM_LV_1, "desired bssid not matched 2");
+                    pBssInfo = pBssInfo->next;
+                    continue;
+                }
+            }
+
+            /* Check support mode */
+            if (pBssInfo->frequency > 3000) {
+                if (   (pBssInfo->EnableHT == 1)
+                    || (pBssInfo->apCap & ZM_All11N_AP) ) //11n AP
+                {
+                    channelFlags = CHANNEL_A_HT;
+                    if (pBssInfo->enableHT40 == 1) {
+                        channelFlags |= CHANNEL_HT40;
+                    }
+                } else {
+                    channelFlags = CHANNEL_A;
+                }
+            } else {
+                if (   (pBssInfo->EnableHT == 1)
+                    || (pBssInfo->apCap & ZM_All11N_AP) ) //11n AP
+                {
+                    channelFlags = CHANNEL_G_HT;
+                    if(pBssInfo->enableHT40 == 1) {
+                        channelFlags |= CHANNEL_HT40;
+                    }
+                } else {
+                    if (pBssInfo->extSupportedRates[1] == 0) {
+                        channelFlags = CHANNEL_B;
+                    } else {
+                        channelFlags = CHANNEL_G;
+                    }
+                }
+            }
+
+            if (   ((channelFlags == CHANNEL_B) && (wd->connectMode & ZM_BIT_0))
+                || ((channelFlags == CHANNEL_G) && (wd->connectMode & ZM_BIT_1))
+                || ((channelFlags == CHANNEL_A) && (wd->connectMode & ZM_BIT_2))
+                || ((channelFlags & CHANNEL_HT20) && (wd->connectMode & ZM_BIT_3)) )
+            {
+                pBssInfo = pBssInfo->next;
+                continue;
+            }
+
+            /* Skip if AP in blocking list */
+            if ((ret = zfStaIsApInBlockingList(dev, pBssInfo->bssid)) == TRUE)
+            {
+                zm_msg0_mm(ZM_LV_0, "Candidate AP in blocking List, skip if there's stilla choice!");
+                pNowBssInfo = pBssInfo;
+                pBssInfo = pBssInfo->next;
+                continue;
+            }
+
+            if ( pBssInfo->capability[0] & ZM_BIT_0 )  // check if infra-BSS
+            {
+                    pNowBssInfo = pBssInfo;
+                    wd->sta.apWmeCapability = pBssInfo->wmeSupport;
+
+
+                    goto done;
+            }
+        }
+
+        pBssInfo = pBssInfo->next;
+    }
+
+done:
+    if (pNowBssInfo != NULL)
+    {
+        zfwMemoryCopy((void*)candidateBss, (void*)pNowBssInfo, sizeof(struct zsBssInfo));
+        pNowBssInfo = candidateBss;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return pNowBssInfo;
+}
+
+
+void zfInfraConnectNetwork(zdev_t* dev)
+{
+    struct zsBssInfo* pBssInfo;
+    struct zsBssInfo* pNowBssInfo=NULL;
+    struct zsBssInfo candidateBss;
+    //u16_t i, j=100, quality=10000;
+    //u8_t ret=FALSE, apWepStatus;
+    u8_t ret=FALSE;
+    u16_t k;
+    u8_t density = ZM_MPDU_DENSITY_NONE;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* Reset bssNotFoundCount for Ad-Hoc:IBSS */
+    /* Need review : IbssConn -> InfraConn -> IbssConn etc, flag/counter reset? */
+    zmw_enter_critical_section(dev);
+    wd->sta.bssNotFoundCount = 0;
+    zmw_leave_critical_section(dev);
+
+    /* Set TxQs CWMIN, CWMAX, AIFS and TXO to WME STA default. */
+    zfUpdateDefaultQosParameter(dev, 0);
+
+    zfStaRefreshBlockList(dev, 0);
+
+    /* change state to CONNECTING and stop the channel scanning */
+    zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTING);
+    zfPowerSavingMgrWakeup(dev);
+
+    wd->sta.wmeConnected = 0;
+    wd->sta.psMgr.tempWakeUp = 0;
+    wd->sta.qosInfo = 0;
+    zfQueueFlush(dev, wd->sta.uapsdQ);
+
+    wd->sta.connectState = ZM_STA_CONN_STATE_NONE;
+
+    //Reorder BssList by RSSI--CWYang(+)
+    zfBssInfoReorderList(dev);
+
+    pNowBssInfo = zfInfraFindAPToConnect(dev, &candidateBss);
+
+	if (wd->sta.SWEncryptEnable != 0)
+	{
+	    if (wd->sta.bSafeMode == 0)
+	    {
+		    zfStaDisableSWEncryption(dev);//Quickly reboot
+	    }
+	}
+    if ( pNowBssInfo != NULL )
+    {
+        //zm_assert(pNowBssInfo != NULL);
+
+        pBssInfo = pNowBssInfo;
+        wd->sta.ssidLen = pBssInfo->ssid[1];
+        zfMemoryCopy(wd->sta.ssid, &(pBssInfo->ssid[2]), pBssInfo->ssid[1]);
+        wd->frequency = pBssInfo->frequency;
+        //wd->sta.flagFreqChanging = 1;
+
+        //zfCoreSetFrequency(dev, wd->frequency);
+        zfUpdateBssid(dev, pBssInfo->bssid);
+        zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_ZERO);
+        zfUpdateSupportRate(dev, pBssInfo->supportedRates);
+        zfUpdateSupportRate(dev, pBssInfo->extSupportedRates);
+
+        wd->beaconInterval = pBssInfo->beaconInterval[0] +
+                             (((u16_t) pBssInfo->beaconInterval[1]) << 8);
+        if (wd->beaconInterval == 0)
+        {
+            wd->beaconInterval = 100;
+        }
+
+        /* ESS bit on */
+        wd->sta.capability[0] |= ZM_BIT_0;
+        /* IBSS bit off */
+        wd->sta.capability[0] &= ~ZM_BIT_1;
+
+        /* 11n AP flag */
+        wd->sta.EnableHT = pBssInfo->EnableHT;
+        wd->sta.SG40 = pBssInfo->SG40;
+#ifdef ZM_ENABLE_CENC
+        if ( pBssInfo->securityType == ZM_SECURITY_TYPE_CENC )
+        {
+            wd->sta.wmeEnabled = 0; //Disable WMM in CENC
+            cencInit(dev);
+            cencSetCENCMode(dev, NdisCENC_PSK);
+            wd->sta.wpaState = ZM_STA_WPA_STATE_INIT;
+            /* CENC */
+            if ( pBssInfo->cencIe[1] != 0 )
+            {
+                //wd->sta.wepStatus = ZM_ENCRYPTION_CENC;
+                //wd->sta.encryMode = ZM_CENC;
+                zfwCencHandleBeaconProbrespon(dev, (u8_t *)&pBssInfo->cencIe,
+                        (u8_t *)&pBssInfo->ssid, (u8_t *)&pBssInfo->macaddr);
+                zfMemoryCopy(wd->sta.cencIe, pBssInfo->cencIe,
+                        pBssInfo->cencIe[1]+2);
+            }
+            else
+            {
+                wd->sta.cencIe[1] = 0;
+            }
+        }
+#endif //ZM_ENABLE_CENC
+        if ( pBssInfo->securityType == ZM_SECURITY_TYPE_WPA )
+        {
+            wd->sta.wpaState = ZM_STA_WPA_STATE_INIT;
+
+            if ( wd->sta.wepStatus == ZM_ENCRYPTION_TKIP )
+            {
+                wd->sta.encryMode = ZM_TKIP;
+
+                /* Turn on software encryption/decryption for TKIP */
+                if (wd->sta.EnableHT == 1)
+                {
+                    zfStaEnableSWEncryption(dev, (ZM_SW_TKIP_ENCRY_EN|ZM_SW_TKIP_DECRY_EN));
+                }
+
+                /* Do not support TKIP in 11n mode */
+                //wd->sta.EnableHT = 0;
+                //pBssInfo->enableHT40 = 0;
+            }
+            else if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+            {
+                wd->sta.encryMode = ZM_AES;
+
+                /* If AP supports HT mode */
+                if (wd->sta.EnableHT)
+                {
+                    /* Set MPDU density to 8 us*/
+                    density = ZM_MPDU_DENSITY_8US;
+                }
+            }
+
+            if ( pBssInfo->wpaIe[1] != 0 )
+            {
+                zfMemoryCopy(wd->sta.wpaIe, pBssInfo->wpaIe,
+                             pBssInfo->wpaIe[1]+2);
+            }
+            else
+            {
+                wd->sta.wpaIe[1] = 0;
+            }
+
+            if ( pBssInfo->rsnIe[1] != 0 )
+            {
+                zfMemoryCopy(wd->sta.rsnIe, pBssInfo->rsnIe,
+                             pBssInfo->rsnIe[1]+2);
+            }
+            else
+            {
+                wd->sta.rsnIe[1] = 0;
+            }
+        }
+
+
+
+        /* check preamble bit */
+        wd->preambleTypeInUsed = wd->preambleType;
+        if ( wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_AUTO )
+        {
+            if (pBssInfo->capability[0] & ZM_BIT_5)
+            {
+                wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_SHORT;
+            }
+            else
+            {
+                wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_LONG;
+            }
+        }
+
+        if (wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_LONG)
+        {
+            wd->sta.capability[0] &= ~ZM_BIT_5;
+        }
+        else
+        {
+            wd->sta.capability[0] |= ZM_BIT_5;
+        }
+
+        /* check 802.11n 40MHz Setting */
+        if ((pBssInfo->enableHT40 == 1) &&
+            ((pBssInfo->extChOffset == 1) || (pBssInfo->extChOffset == 3)))
+        {
+            wd->BandWidth40 = pBssInfo->enableHT40;
+            wd->ExtOffset = pBssInfo->extChOffset;
+        }
+        else
+        {
+            wd->BandWidth40 = 0;
+            wd->ExtOffset = 0;
+        }
+
+        /* check 802.11H support bit */
+
+        /* check Owl Ap */
+        if ( pBssInfo->athOwlAp & ZM_BIT_0 )
+        {
+            /* In this function, FW retry will be enable, ZM_MAC_REG_RETRY_MAX
+               will be set to 0.
+             */
+            zfHpDisableHwRetry(dev);
+            wd->sta.athOwlAp = 1;
+            /* Set MPDU density to 8 us*/
+            density = ZM_MPDU_DENSITY_8US;
+        }
+        else
+        {
+            /* In this function, FW retry will be disable, ZM_MAC_REG_RETRY_MAX
+               will be set to 3.
+             */
+            zfHpEnableHwRetry(dev);
+            wd->sta.athOwlAp = 0;
+        }
+        wd->reorder = 1;
+
+        /* Set MPDU density */
+        zfHpSetMPDUDensity(dev, density);
+
+        /* check short slot time bit */
+        if ( pBssInfo->capability[1] & ZM_BIT_2 )
+        {
+            wd->sta.capability[1] |= ZM_BIT_2;
+        }
+
+        if ( pBssInfo->erp & ZM_BIT_1 )
+        {
+            //zm_debug_msg0("protection mode on");
+            wd->sta.bProtectionMode = TRUE;
+            zfHpSetSlotTime(dev, 0);
+        }
+        else
+        {
+            //zm_debug_msg0("protection mode off");
+            wd->sta.bProtectionMode = FALSE;
+            zfHpSetSlotTime(dev, 1);
+        }
+
+        if (pBssInfo->marvelAp == 1)
+        {
+            wd->sta.enableDrvBA = 0;
+            /*
+             * 8701 : NetGear 3500 (MARVELL)
+             * Downlink issue : set slottime to 20.
+             */
+            zfHpSetSlotTimeRegister(dev, 0);
+        }
+        else
+        {
+            wd->sta.enableDrvBA = 1;
+
+            /*
+             * This is not good for here do reset slot time.
+             * I think it should reset when leave MARVELL ap
+             * or enter disconnect state etc.
+             */
+            zfHpSetSlotTimeRegister(dev, 1);
+        }
+
+        //Store probe response frame body, for VISTA only
+        wd->sta.beaconFrameBodySize = pBssInfo->frameBodysize + 12;
+        if (wd->sta.beaconFrameBodySize > ZM_CACHED_FRAMEBODY_SIZE)
+        {
+            wd->sta.beaconFrameBodySize = ZM_CACHED_FRAMEBODY_SIZE;
+        }
+        for (k=0; k<8; k++)
+        {
+            wd->sta.beaconFrameBody[k] = pBssInfo->timeStamp[k];
+        }
+        wd->sta.beaconFrameBody[8] = pBssInfo->beaconInterval[0];
+        wd->sta.beaconFrameBody[9] = pBssInfo->beaconInterval[1];
+        wd->sta.beaconFrameBody[10] = pBssInfo->capability[0];
+        wd->sta.beaconFrameBody[11] = pBssInfo->capability[1];
+        for (k=0; k<(wd->sta.beaconFrameBodySize - 12); k++)
+        {
+            wd->sta.beaconFrameBody[k+12] = pBssInfo->frameBody[k];
+        }
+
+        if ( ( pBssInfo->capability[0] & ZM_BIT_4 )&&
+             (( wd->sta.authMode == ZM_AUTH_MODE_OPEN )||
+              ( wd->sta.authMode == ZM_AUTH_MODE_SHARED_KEY)||
+              (wd->sta.authMode == ZM_AUTH_MODE_AUTO)) )
+        {   /* privacy enabled */
+
+            if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_DISABLED )
+            {
+                zm_debug_msg0("Adapter is no WEP, try to connect to WEP AP");
+                ret = FALSE;
+            }
+
+            /* Do not support WEP in 11n mode */
+            if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED )
+            {
+                /* Turn on software encryption/decryption for WEP */
+                if (wd->sta.EnableHT == 1)
+                {
+                    zfStaEnableSWEncryption(dev, (ZM_SW_WEP_ENCRY_EN|ZM_SW_WEP_DECRY_EN));
+                }
+
+                //wd->sta.EnableHT = 0;
+                //wd->BandWidth40 = 0;
+                //wd->ExtOffset = 0;
+            }
+
+            wd->sta.capability[0] |= ZM_BIT_4;
+
+            if ( wd->sta.authMode == ZM_AUTH_MODE_AUTO )
+            { /* Try to use open and shared-key authehtication alternatively */
+                if ( (wd->sta.connectTimeoutCount % 2) == 0 )
+                    wd->sta.bIsSharedKey = 0;
+                else
+                    wd->sta.bIsSharedKey = 1;
+            }
+            else if ( wd->sta.authMode != ZM_AUTH_MODE_SHARED_KEY )
+            {   /* open  or auto */
+                //zfStaStartConnect(dev, 0);
+                wd->sta.bIsSharedKey = 0;
+            }
+            else if ( wd->sta.authMode != ZM_AUTH_MODE_OPEN )
+            {   /* shared key */
+                //zfStaStartConnect(dev, 1) ;
+                wd->sta.bIsSharedKey = 1;
+            }
+        }
+        else
+        {
+            if ( (pBssInfo->securityType == ZM_SECURITY_TYPE_WPA)||
+                 (pBssInfo->capability[0] & ZM_BIT_4) )
+            {
+                wd->sta.capability[0] |= ZM_BIT_4;
+                /* initialize WPA related parameters */
+            }
+            else
+            {
+                wd->sta.capability[0] &= (~ZM_BIT_4);
+            }
+
+            /* authentication with open system */
+            //zfStaStartConnect(dev, 0);
+            wd->sta.bIsSharedKey = 0;
+        }
+
+        /* Improve WEP/TKIP performace with HT AP, detail information please look bug#32495 */
+        /*
+        if ( (pBssInfo->broadcomHTAp == 1)
+             && (wd->sta.SWEncryptEnable != 0) )
+        {
+            zfHpSetTTSIFSTime(dev, 0xa);
+        }
+        else
+        {
+            zfHpSetTTSIFSTime(dev, 0x8);
+        }
+        */
+    }
+    else
+    {
+        zm_debug_msg0("Desired SSID not found");
+        goto zlConnectFailed;
+    }
+
+
+    zfCoreSetFrequencyV2(dev, wd->frequency, zfStaStartConnectCb);
+    return;
+
+zlConnectFailed:
+    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_NOT_FOUND, wd->sta.bssid, 0);
+    return;
+}
+
+u8_t zfCheckWPAAuth(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    u8_t   ret=TRUE;
+    u8_t   pmkCount;
+    u8_t   i;
+    u16_t   encAlgoType = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.wepStatus == ZM_ENCRYPTION_TKIP )
+    {
+        encAlgoType = ZM_TKIP;
+    }
+    else if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+    {
+        encAlgoType = ZM_AES;
+    }
+
+    switch(wd->sta.authMode)
+    {
+        case ZM_AUTH_MODE_WPA:
+        case ZM_AUTH_MODE_WPAPSK:
+            if ( pBssInfo->wpaIe[1] == 0 )
+            {
+                ret = FALSE;
+                break;
+            }
+
+            pmkCount = pBssInfo->wpaIe[12];
+            for(i=0; i < pmkCount; i++)
+            {
+                if ( pBssInfo->wpaIe[17 + 4*i] == encAlgoType )
+                {
+                    ret = TRUE;
+                    goto done;
+                }
+            }
+
+            ret = FALSE;
+            break;
+
+        case ZM_AUTH_MODE_WPA2:
+        case ZM_AUTH_MODE_WPA2PSK:
+            if ( pBssInfo->rsnIe[1] == 0 )
+            {
+                ret = FALSE;
+                break;
+            }
+
+            pmkCount = pBssInfo->rsnIe[8];
+            for(i=0; i < pmkCount; i++)
+            {
+                if ( pBssInfo->rsnIe[13 + 4*i] == encAlgoType )
+                {
+                    ret = TRUE;
+                    goto done;
+                }
+            }
+
+            ret = FALSE;
+            break;
+    }
+
+done:
+    return ret;
+}
+
+u8_t zfCheckAuthentication(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    u8_t   ret=TRUE;
+    u16_t  encAlgoType;
+    u16_t UnicastCipherNum;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Connecting to ANY has been checked */
+    if ( wd->sta.ssidLen == 0 )
+    {
+        return ret;
+    }
+
+
+	switch(wd->sta.authMode)
+	//switch(wd->ws.authMode)//Quickly reboot
+    {
+        case ZM_AUTH_MODE_WPA_AUTO:
+        case ZM_AUTH_MODE_WPAPSK_AUTO:
+            encAlgoType = 0;
+            if(pBssInfo->rsnIe[1] != 0)
+            {
+                UnicastCipherNum = (pBssInfo->rsnIe[8]) +
+                                   (pBssInfo->rsnIe[9] << 8);
+
+                /* If there is only one unicast cipher */
+                if (UnicastCipherNum == 1)
+                {
+                    encAlgoType = pBssInfo->rsnIe[13];
+                    //encAlgoType = pBssInfo->rsnIe[7];
+                }
+                else
+                {
+                    u16_t ii;
+                    u16_t desiredCipher = 0;
+                    u16_t IEOffSet = 13;
+
+                    /* Enumerate all the supported unicast cipher */
+                    for (ii = 0; ii < UnicastCipherNum; ii++)
+                    {
+                        if (pBssInfo->rsnIe[IEOffSet+ii*4] > desiredCipher)
+                        {
+                            desiredCipher = pBssInfo->rsnIe[IEOffSet+ii*4];
+                        }
+                    }
+
+                    encAlgoType = desiredCipher;
+                }
+
+                if ( encAlgoType == 0x02 )
+                {
+    			    wd->sta.wepStatus = ZM_ENCRYPTION_TKIP;
+
+    			    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA_AUTO )
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA2;
+                    }
+                    else //ZM_AUTH_MODE_WPAPSK_AUTO
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA2PSK;
+                    }
+                }
+                else if ( encAlgoType == 0x04 )
+                {
+                    wd->sta.wepStatus = ZM_ENCRYPTION_AES;
+
+                    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA_AUTO )
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA2;
+                    }
+                    else //ZM_AUTH_MODE_WPAPSK_AUTO
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA2PSK;
+                    }
+                }
+                else
+                {
+                    ret = FALSE;
+                }
+            }
+            else if(pBssInfo->wpaIe[1] != 0)
+            {
+                UnicastCipherNum = (pBssInfo->wpaIe[12]) +
+                                   (pBssInfo->wpaIe[13] << 8);
+
+                /* If there is only one unicast cipher */
+                if (UnicastCipherNum == 1)
+                {
+                    encAlgoType = pBssInfo->wpaIe[17];
+                    //encAlgoType = pBssInfo->wpaIe[11];
+                }
+                else
+                {
+                    u16_t ii;
+                    u16_t desiredCipher = 0;
+                    u16_t IEOffSet = 17;
+
+                    /* Enumerate all the supported unicast cipher */
+                    for (ii = 0; ii < UnicastCipherNum; ii++)
+                    {
+                        if (pBssInfo->wpaIe[IEOffSet+ii*4] > desiredCipher)
+                        {
+                            desiredCipher = pBssInfo->wpaIe[IEOffSet+ii*4];
+                        }
+                    }
+
+                    encAlgoType = desiredCipher;
+                }
+
+                if ( encAlgoType == 0x02 )
+                {
+    			    wd->sta.wepStatus = ZM_ENCRYPTION_TKIP;
+
+    			    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA_AUTO )
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA;
+                    }
+                    else //ZM_AUTH_MODE_WPAPSK_AUTO
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPAPSK;
+                    }
+                }
+                else if ( encAlgoType == 0x04 )
+                {
+                    wd->sta.wepStatus = ZM_ENCRYPTION_AES;
+
+                    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA_AUTO )
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA;
+                    }
+                    else //ZM_AUTH_MODE_WPAPSK_AUTO
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPAPSK;
+                    }
+                }
+                else
+                {
+                    ret = FALSE;
+                }
+
+
+            }
+            else
+            {
+                ret = FALSE;
+            }
+
+            break;
+
+        case ZM_AUTH_MODE_WPA:
+        case ZM_AUTH_MODE_WPAPSK:
+        case ZM_AUTH_MODE_WPA_NONE:
+        case ZM_AUTH_MODE_WPA2:
+        case ZM_AUTH_MODE_WPA2PSK:
+            {
+                if ( pBssInfo->securityType != ZM_SECURITY_TYPE_WPA )
+                {
+                    ret = FALSE;
+                }
+
+                ret = zfCheckWPAAuth(dev, pBssInfo);
+            }
+            break;
+
+        case ZM_AUTH_MODE_OPEN:
+        case ZM_AUTH_MODE_SHARED_KEY:
+        case ZM_AUTH_MODE_AUTO:
+            {
+                if ( pBssInfo->wscIe[1] )
+                {
+                    // If the AP is a Jumpstart AP, it's ok!! Ray
+                    break;
+                }
+                else if ( pBssInfo->securityType == ZM_SECURITY_TYPE_WPA )
+                {
+                    ret = FALSE;
+                }
+            }
+            break;
+
+        default:
+            break;
+    }
+
+    return ret;
+}
+
+u8_t zfStaIsConnected(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.adapterState == ZM_STA_STATE_CONNECTED )
+    {
+        return TRUE;
+    }
+
+    return FALSE;
+}
+
+u8_t zfStaIsConnecting(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.adapterState == ZM_STA_STATE_CONNECTING )
+    {
+        return TRUE;
+    }
+
+    return FALSE;
+}
+
+u8_t zfStaIsDisconnect(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.adapterState == ZM_STA_STATE_DISCONNECT )
+    {
+        return TRUE;
+    }
+
+    return FALSE;
+}
+
+u8_t zfChangeAdapterState(zdev_t* dev, u8_t newState)
+{
+    u8_t ret = TRUE;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    //if ( newState == wd->sta.adapterState )
+    //{
+    //    return FALSE;
+    //}
+
+    switch(newState)
+    {
+    case ZM_STA_STATE_DISCONNECT:
+        zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_DISCONNECT);
+
+        #if 1
+            zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+        #else
+            if ( wd->sta.bChannelScan )
+            {
+                /* stop the action of channel scanning */
+                wd->sta.bChannelScan = FALSE;
+                ret =  TRUE;
+                break;
+            }
+        #endif
+
+        break;
+    case ZM_STA_STATE_CONNECTING:
+        #if 1
+            zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+        #else
+            if ( wd->sta.bChannelScan )
+            {
+                /* stop the action of channel scanning */
+                wd->sta.bChannelScan = FALSE;
+                ret =  TRUE;
+                break;
+            }
+        #endif
+
+        break;
+    case ZM_STA_STATE_CONNECTED:
+        break;
+    default:
+        break;
+    }
+
+    //if ( ret )
+    //{
+        zmw_enter_critical_section(dev);
+        wd->sta.adapterState = newState;
+        zmw_leave_critical_section(dev);
+
+        zm_debug_msg1("change adapter state = ", newState);
+    //}
+
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaMmAddIeSsid            */
+/*      Add information element SSID to buffer.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Ji-Huang Lee        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIeSsid(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.ssidLen);
+
+    /* Information : SSID */
+    for (i=0; i<wd->sta.ssidLen; i++)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.ssid[i]);
+    }
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaMmAddIeWpa             */
+/*      Add information element SSID to buffer.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Ji-Huang Lee        ZyDAS Technology Corporation    2006.01     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType)
+{
+    u32_t  i;
+    u8_t    ssn[64]={
+                        /* Element ID */
+                        0xdd,
+                        /* Length */
+                        0x18,
+                        /* OUI type */
+                        0x00, 0x50, 0xf2, 0x01,
+                        /* Version */
+                        0x01, 0x00,
+                        /* Group Cipher Suite, default=TKIP */
+                        0x00, 0x50, 0xf2, 0x02,
+                        /* Pairwise Cipher Suite Count */
+                        0x01, 0x00,
+                        /* Pairwise Cipher Suite, default=TKIP */
+                        0x00, 0x50, 0xf2, 0x02,
+                        /* Authentication and Key Management Suite Count */
+                        0x01, 0x00,
+                        /* Authentication type, default=PSK */
+                        0x00, 0x50, 0xf2, 0x02,
+                        /* WPA capability */
+                        0x00, 0x00
+                    };
+
+    u8_t    rsn[64]={
+                        /* Element ID */
+                        0x30,
+                        /* Length */
+                        0x14,
+                        /* Version */
+                        0x01, 0x00,
+                        /* Group Cipher Suite, default=TKIP */
+                        0x00, 0x0f, 0xac, 0x02,
+                        /* Pairwise Cipher Suite Count */
+                        0x01, 0x00,
+                        /* Pairwise Cipher Suite, default=TKIP */
+                        0x00, 0x0f, 0xac, 0x02,
+                        /* Authentication and Key Management Suite Count */
+                        0x01, 0x00,
+                        /* Authentication type, default=PSK */
+                        0x00, 0x0f, 0xac, 0x02,
+                        /* RSN capability */
+                        0x00, 0x00
+                    };
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.currentAuthMode == ZM_AUTH_MODE_WPAPSK )
+    {
+        /* Overwrite Group Cipher Suite by AP's setting */
+        zfMemoryCopy(ssn+8, wd->sta.wpaIe+8, 4);
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+        {
+            /* Overwrite Pairwise Cipher Suite by AES */
+            zfMemoryCopy(ssn+14, zgWpaAesOui, 4);
+        }
+
+        zfCopyToIntTxBuffer(dev, buf, ssn, offset, ssn[1]+2);
+        zfMemoryCopy(wd->sta.wpaIe, ssn, ssn[1]+2);
+        offset += (ssn[1]+2);
+    }
+    else if ( wd->sta.currentAuthMode == ZM_AUTH_MODE_WPA )
+    {
+        /* Overwrite Group Cipher Suite by AP's setting */
+        zfMemoryCopy(ssn+8, wd->sta.wpaIe+8, 4);
+        /* Overwrite Key Management Suite by WPA-Radius */
+        zfMemoryCopy(ssn+20, zgWpaRadiusOui, 4);
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+        {
+            /* Overwrite Pairwise Cipher Suite by AES */
+            zfMemoryCopy(ssn+14, zgWpaAesOui, 4);
+        }
+
+        zfCopyToIntTxBuffer(dev, buf, ssn, offset, ssn[1]+2);
+        zfMemoryCopy(wd->sta.wpaIe, ssn, ssn[1]+2);
+        offset += (ssn[1]+2);
+    }
+    else if ( wd->sta.currentAuthMode == ZM_AUTH_MODE_WPA2PSK )
+    {
+        /* Overwrite Group Cipher Suite by AP's setting */
+        zfMemoryCopy(rsn+4, wd->sta.rsnIe+4, 4);
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+        {
+            /* Overwrite Pairwise Cipher Suite by AES */
+            zfMemoryCopy(rsn+10, zgWpa2AesOui, 4);
+        }
+
+        if ( frameType == ZM_WLAN_FRAME_TYPE_REASOCREQ )
+        {
+            for(i=0; i<wd->sta.pmkidInfo.bssidCount; i++)
+            {
+                if ( zfMemoryIsEqual((u8_t*) wd->sta.pmkidInfo.bssidInfo[i].bssid,
+                                     (u8_t*) wd->sta.bssid, 6) )
+                {
+                    /* matched */
+                    break;
+                }
+
+                if ( i < wd->sta.pmkidInfo.bssidCount )
+                {
+                    // Fill PMKID Count in RSN information element
+                    rsn[22] = 0x01;
+                    rsn[23] = 0x00;
+
+                    // Fill PMKID in RSN information element
+                    zfMemoryCopy(rsn+24,
+                                 wd->sta.pmkidInfo.bssidInfo[i].pmkid, 16);
+			                 rsn[1] += 18;
+                }
+            }
+        }
+
+        zfCopyToIntTxBuffer(dev, buf, rsn, offset, rsn[1]+2);
+        zfMemoryCopy(wd->sta.rsnIe, rsn, rsn[1]+2);
+        offset += (rsn[1]+2);
+    }
+    else if ( wd->sta.currentAuthMode == ZM_AUTH_MODE_WPA2 )
+    {
+        /* Overwrite Group Cipher Suite by AP's setting */
+        zfMemoryCopy(rsn+4, wd->sta.rsnIe+4, 4);
+        /* Overwrite Key Management Suite by WPA2-Radius */
+        zfMemoryCopy(rsn+16, zgWpa2RadiusOui, 4);
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+        {
+            /* Overwrite Pairwise Cipher Suite by AES */
+            zfMemoryCopy(rsn+10, zgWpa2AesOui, 4);
+        }
+
+        if (( frameType == ZM_WLAN_FRAME_TYPE_REASOCREQ || ( frameType == ZM_WLAN_FRAME_TYPE_ASOCREQ )))
+        {
+
+            if (wd->sta.pmkidInfo.bssidCount != 0) {
+                // Fill PMKID Count in RSN information element
+                rsn[22] = 1;
+                rsn[23] = 0;
+                /*
+                 *  The caller is respnsible to give us the relevant PMKID.
+                 *  We'll only accept 1 PMKID for now.
+                 */
+                for(i=0; i<wd->sta.pmkidInfo.bssidCount; i++)
+                {
+                    if ( zfMemoryIsEqual((u8_t*) wd->sta.pmkidInfo.bssidInfo[i].bssid, (u8_t*) wd->sta.bssid, 6) )
+                    {
+                        zfMemoryCopy(rsn+24, wd->sta.pmkidInfo.bssidInfo[i].pmkid, 16);
+                        break;
+                    }
+                }
+                rsn[1] += 18;
+            }
+
+        }
+
+        zfCopyToIntTxBuffer(dev, buf, rsn, offset, rsn[1]+2);
+        zfMemoryCopy(wd->sta.rsnIe, rsn, rsn[1]+2);
+        offset += (rsn[1]+2);
+    }
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaAddIeIbss              */
+/*      Add information element IBSS parameter to buffer.               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Ji-Huang Lee        ZyDAS Technology Corporation    2005.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIeIbss(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_IBSS);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 2);
+
+    /* ATIM window */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->sta.atimWindow);
+    offset += 2;
+
+    return offset;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaAddIeWmeInfo           */
+/*      Add WME Information Element to buffer.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIeWmeInfo(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t qosInfo)
+{
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WIFI_IE);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 7);
+
+    /* OUI */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x50);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0xF2);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x02);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x01);
+
+    /* QoS Info */
+    zmw_tx_buf_writeb(dev, buf, offset++, qosInfo);
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaAddIePowerCap          */
+/*      Add information element Power capability to buffer.             */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Sharon                                            2007.12       */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIePowerCap(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u8_t MaxTxPower;
+    u8_t MinTxPower;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_POWER_CAPABILITY);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 2);
+
+    MinTxPower = (u8_t)(zfHpGetMinTxPower(dev)/2);
+    MaxTxPower = (u8_t)(zfHpGetMaxTxPower(dev)/2);
+
+    /* Min Transmit Power Cap */
+    zmw_tx_buf_writeh(dev, buf, offset++, MinTxPower);
+
+    /* Max Transmit Power Cap */
+    zmw_tx_buf_writeh(dev, buf, offset++, MaxTxPower);
+
+    return offset;
+}
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaAddIeSupportCh              */
+/*      Add information element supported channels to buffer.               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Sharon            2007.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIeSupportCh(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+
+    u8_t   i;
+    u16_t  count_24G = 0;
+    u16_t  count_5G = 0;
+    u16_t  channelNum;
+    u8_t   length;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+    zmw_enter_critical_section(dev);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if (wd->regulationTable.allowChannel[i].channel < 3000)
+        { // 2.4Hz
+            count_24G++;
+        }
+        else
+        { // 5GHz
+            count_5G++;
+        }
+    }
+
+    length = (u8_t)(count_5G * 2 + 2); //5G fill by pair, 2,4G (continuous channels) fill 2 bytes
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SUPPORTED_CHANNELS );
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, length);
+
+    // 2.4GHz (continuous channels)
+    /* First channel number */
+    zmw_tx_buf_writeh(dev, buf, offset++, 1); //Start from channle 1
+    /* Number of channels */
+    zmw_tx_buf_writeh(dev, buf, offset++, count_24G);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt ; i++)
+    {
+        if (wd->regulationTable.allowChannel[i].channel > 4000 && wd->regulationTable.allowChannel[i].channel < 5000)
+        { // 5GHz 4000 -5000Mhz
+            channelNum = (wd->regulationTable.allowChannel[i].channel-4000)/5;
+            /* First channel number */
+            zmw_tx_buf_writeh(dev, buf, offset++, channelNum);
+            /* Number of channels */
+            zmw_tx_buf_writeh(dev, buf, offset++, 1);
+        }
+        else if (wd->regulationTable.allowChannel[i].channel >= 5000)
+        { // 5GHz >5000Mhz
+            channelNum = (wd->regulationTable.allowChannel[i].channel-5000)/5;
+            /* First channel number */
+            zmw_tx_buf_writeh(dev, buf, offset++, channelNum);
+            /* Number of channels */
+            zmw_tx_buf_writeh(dev, buf, offset++, 1);
+        }
+    }
+   zmw_leave_critical_section(dev);
+
+    return offset;
+}
+
+void zfStaStartConnectCb(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zfStaStartConnect(dev, wd->sta.bIsSharedKey);
+}
+
+void zfStaStartConnect(zdev_t* dev, u8_t bIsSharedKey)
+{
+    u32_t p1, p2;
+    u8_t newConnState;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* p1_low = algorithm number, p1_high = transaction sequence number */
+    if ( bIsSharedKey )
+    {
+        //wd->sta.connectState = ZM_STA_CONN_STATE_AUTH_SHARE_1;
+        newConnState = ZM_STA_CONN_STATE_AUTH_SHARE_1;
+        zm_debug_msg0("ZM_STA_CONN_STATE_AUTH_SHARE_1");
+        p1 = ZM_AUTH_ALGO_SHARED_KEY;
+    }
+    else
+    {
+        //wd->sta.connectState = ZM_STA_CONN_STATE_AUTH_OPEN;
+        newConnState = ZM_STA_CONN_STATE_AUTH_OPEN;
+        zm_debug_msg0("ZM_STA_CONN_STATE_AUTH_OPEN");
+        if( wd->sta.leapEnabled )
+            p1 = ZM_AUTH_ALGO_LEAP;
+        else
+            p1 = ZM_AUTH_ALGO_OPEN_SYSTEM;
+    }
+
+    /* status code */
+    p2 = 0x0;
+
+    zmw_enter_critical_section(dev);
+    wd->sta.connectTimer = wd->tick;
+    wd->sta.connectState = newConnState;
+    zmw_leave_critical_section(dev);
+
+    /* send the 1st authentication frame */
+    zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_AUTH, wd->sta.bssid, p1, p2, 0);
+
+    return;
+}
+
+void zfSendNullData(zdev_t* dev, u8_t type)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t err;
+    u16_t hlen;
+    u16_t header[(34+8+1)/2];
+    u16_t bcastAddr[3] = {0xffff,0xffff,0xffff};
+    u16_t *dstAddr;
+
+    zmw_get_wlan_dev(dev);
+
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return;
+    }
+
+    zfwBufSetSize(dev, buf, 0);
+
+    //zm_msg2_mm(ZM_LV_2, "buf->len=", buf->len);
+
+    if ( wd->wlanMode == ZM_MODE_IBSS)
+    {
+        dstAddr = bcastAddr;
+    }
+    else
+    {
+        dstAddr = wd->sta.bssid;
+    }
+
+    if (wd->sta.wmeConnected != 0)
+    {
+        /* If connect to a WMM AP, Send QoS Null data */
+        hlen = zfTxGenMmHeader(dev, ZM_WLAN_FRAME_TYPE_QOS_NULL, dstAddr, header, 0, buf, 0, 0);
+    }
+    else
+    {
+        hlen = zfTxGenMmHeader(dev, ZM_WLAN_FRAME_TYPE_NULL, dstAddr, header, 0, buf, 0, 0);
+    }
+
+    if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        header[4] |= 0x0100; //TODS bit
+    }
+
+    if ( type == 1 )
+    {
+        header[4] |= 0x1000;
+    }
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    /*increase unicast frame counter*/
+    wd->commTally.txUnicastFrm++;
+
+    if ((err = zfHpSend(dev, header, hlen, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+
+
+    return;
+
+zlError:
+
+    zfwBufFree(dev, buf, 0);
+    return;
+
+}
+
+void zfSendPSPoll(zdev_t* dev)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t err;
+    u16_t hlen;
+    u16_t header[(8+24+1)/2];
+
+    zmw_get_wlan_dev(dev);
+
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return;
+    }
+
+    zfwBufSetSize(dev, buf, 0);
+
+    //zm_msg2_mm(ZM_LV_2, "buf->len=", buf->len);
+
+    zfTxGenMmHeader(dev, ZM_WLAN_FRAME_TYPE_PSPOLL, wd->sta.bssid, header, 0, buf, 0, 0);
+
+    header[0] = 20;
+    header[4] |= 0x1000;
+    header[5] = wd->sta.aid | 0xc000; //Both bit-14 and bit-15 are 1
+    hlen = 16 + 8;
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    if ((err = zfHpSend(dev, header, hlen, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+
+    return;
+
+zlError:
+
+    zfwBufFree(dev, buf, 0);
+    return;
+
+}
+
+void zfSendBA(zdev_t* dev, u16_t start_seq, u8_t *bitmap)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t err;
+    u16_t hlen;
+    u16_t header[(8+24+1)/2];
+    u16_t i, offset = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return;
+    }
+
+    zfwBufSetSize(dev, buf, 12); // 28 = FC 2 + DU 2 + RA 6 + TA 6 + BAC 2 + SEQ 2 + BitMap 8
+                                 // 12 = BAC 2 + SEQ 2 + BitMap 8
+
+    //zm_msg2_mm(ZM_LV_2, "buf->len=", buf->len);
+
+    zfTxGenMmHeader(dev, ZM_WLAN_FRAME_TYPE_BA, wd->sta.bssid, header, 0, buf, 0, 0);
+
+    header[0] = 32; /* MAC header 16 + BA control 2 + BA info 10 + FCS 4*/
+    header[1] = 0x4;  /* No ACK */
+
+    /* send by OFDM 6M */
+    header[2] = (u16_t)(zcRateToPhyCtrl[4] & 0xffff);
+    header[3] = (u16_t)(zcRateToPhyCtrl[4]>>16) & 0xffff;
+
+    hlen = 16 + 8;  /* MAC header 16 + control 8*/
+    offset = 0;
+    zmw_tx_buf_writeh(dev, buf, offset, 0x05); /*compressed bitmap on*/
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, start_seq);
+    offset+=2;
+
+    for (i=0; i<8; i++) {
+        zmw_tx_buf_writeb(dev, buf, offset, bitmap[i]);
+        offset++;
+    }
+
+    if ((err = zfHpSend(dev, header, hlen, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+
+    return;
+
+zlError:
+
+    zfwBufFree(dev, buf, 0);
+    return;
+
+}
+
+void zfStaGetTxRate(zdev_t* dev, u16_t* macAddr, u32_t* phyCtrl,
+        u16_t* rcProbingFlag)
+{
+    u8_t   addr[6], i;
+    u8_t   rate;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    ZM_MAC_WORD_TO_BYTE(macAddr, addr);
+    *phyCtrl = 0;
+
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        zmw_enter_critical_section(dev);
+        rate = (u8_t)zfRateCtrlGetTxRate(dev, &wd->sta.oppositeInfo[0].rcCell, rcProbingFlag);
+//#ifdef ZM_FB50
+        //rate = 27;
+//#endif
+        *phyCtrl = zcRateToPhyCtrl[rate];
+        zmw_leave_critical_section(dev);
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+        for(i=0; i<wd->sta.oppositeCount; i++)
+        {
+            if ( addr[0] && 0x01 == 1 ) // The default beacon transmitted rate is CCK and 1 Mbps , but the a mode should use
+                                        // OFDM modulation and 6Mbps to transmit beacon.
+            {
+                //rate = (u8_t)zfRateCtrlGetTxRate(dev, &wd->sta.oppositeInfo[i].rcCell, rcProbingFlag);
+                rate = wd->sta.oppositeInfo[i].rcCell.operationRateSet[0];
+                *phyCtrl = zcRateToPhyCtrl[rate];
+                break;
+            }
+            else if ( zfMemoryIsEqual(addr, wd->sta.oppositeInfo[i].macAddr, 6) )
+            {
+                rate = (u8_t)zfRateCtrlGetTxRate(dev, &wd->sta.oppositeInfo[i].rcCell, rcProbingFlag);
+                *phyCtrl = zcRateToPhyCtrl[rate];
+                break;
+            }
+        }
+        zmw_leave_critical_section(dev);
+    }
+
+    return;
+}
+
+struct zsMicVar* zfStaGetRxMicKey(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t keyIndex;
+    u8_t da0;
+
+    zmw_get_wlan_dev(dev);
+
+    /* if need not check MIC, return NULL */
+    if ( ((wd->sta.encryMode != ZM_TKIP)&&(wd->sta.encryMode != ZM_AES))||
+         (wd->sta.wpaState < ZM_STA_WPA_STATE_PK_OK) )
+    {
+        return NULL;
+    }
+
+    da0 = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+
+    if ((zmw_rx_buf_readb(dev, buf, 0) & 0x80) == 0x80)
+        keyIndex = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_IV_OFFSET+5); /* Qos Packet*/
+    else
+        keyIndex = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_IV_OFFSET+3); /* normal Packet*/
+    keyIndex = (keyIndex & 0xc0) >> 6;
+
+    return (&wd->sta.rxMicKey[keyIndex]);
+}
+
+struct zsMicVar* zfStaGetTxMicKey(zdev_t* dev, zbuf_t* buf)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* if need not check MIC, return NULL */
+    //if ( ((wd->sta.encryMode != ZM_TKIP)&&(wd->sta.encryMode != ZM_AES))||
+    //     (wd->sta.wpaState < ZM_STA_WPA_STATE_PK_OK) )
+    if ( (wd->sta.encryMode != ZM_TKIP) || (wd->sta.wpaState < ZM_STA_WPA_STATE_PK_OK) )
+    {
+        return NULL;
+    }
+
+    return (&wd->sta.txMicKey);
+}
+
+u16_t zfStaRxValidateFrame(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t   frameType, frameCtrl;
+    u8_t   da0;
+    //u16_t  sa[3];
+    u16_t  ret;
+    u16_t  i;
+    //u8_t    sa0;
+
+    zmw_get_wlan_dev(dev);
+
+    frameType = zmw_rx_buf_readb(dev, buf, 0);
+    da0 = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+    //sa0 = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+
+    if ( (!zfStaIsConnected(dev))&&((frameType & 0xf) == ZM_WLAN_DATA_FRAME) )
+    {
+        return ZM_ERR_DATA_BEFORE_CONNECTED;
+    }
+
+
+    if ( (zfStaIsConnected(dev))&&((frameType & 0xf) == ZM_WLAN_DATA_FRAME) )
+    {
+        /* check BSSID */
+        if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+        {
+            /* Big Endian and Little Endian Compatibility */
+            u16_t mac[3];
+            mac[0] = zmw_cpu_to_le16(wd->sta.bssid[0]);
+            mac[1] = zmw_cpu_to_le16(wd->sta.bssid[1]);
+            mac[2] = zmw_cpu_to_le16(wd->sta.bssid[2]);
+            if ( !zfRxBufferEqualToStr(dev, buf, (u8_t *)mac,
+                                       ZM_WLAN_HEADER_A2_OFFSET, 6) )
+            {
+/*We will get lots of garbage data, especially in AES mode.*/
+/*To avoid sending too many deauthentication frames in STA mode, mark it.*/
+#if 0
+                /* If unicast frame, send deauth to the transmitter */
+                if (( da0 & 0x01 ) == 0)
+                {
+                    for (i=0; i<3; i++)
+                    {
+                        sa[i] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+(i*2));
+                    }
+					/* If mutilcast address, don't send deauthentication*/
+					if (( sa0 & 0x01 ) == 0)
+	                  	zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, sa, 7, 0, 0);
+                }
+#endif
+                return ZM_ERR_DATA_BSSID_NOT_MATCHED;
+            }
+        }
+        else if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            /* Big Endian and Little Endian Compatibility */
+            u16_t mac[3];
+            mac[0] = zmw_cpu_to_le16(wd->sta.bssid[0]);
+            mac[1] = zmw_cpu_to_le16(wd->sta.bssid[1]);
+            mac[2] = zmw_cpu_to_le16(wd->sta.bssid[2]);
+            if ( !zfRxBufferEqualToStr(dev, buf, (u8_t *)mac,
+                                       ZM_WLAN_HEADER_A3_OFFSET, 6) )
+            {
+                return ZM_ERR_DATA_BSSID_NOT_MATCHED;
+            }
+        }
+
+        frameCtrl = zmw_rx_buf_readb(dev, buf, 1);
+
+        /* check security bit */
+        if ( wd->sta.dropUnencryptedPkts &&
+             (wd->sta.wepStatus != ZM_ENCRYPTION_WEP_DISABLED )&&
+             ( !(frameCtrl & ZM_BIT_6) ) )
+        {   /* security on, but got data without encryption */
+
+            #if 1
+            ret = ZM_ERR_DATA_NOT_ENCRYPTED;
+            if ( wd->sta.pStaRxSecurityCheckCb != NULL )
+            {
+                ret = wd->sta.pStaRxSecurityCheckCb(dev, buf);
+            }
+            else
+            {
+                ret = ZM_ERR_DATA_NOT_ENCRYPTED;
+            }
+            if (ret == ZM_ERR_DATA_NOT_ENCRYPTED)
+            {
+                wd->commTally.swRxDropUnencryptedCount++;
+            }
+            return ret;
+            #else
+            if ( (wd->sta.wepStatus != ZM_ENCRYPTION_TKIP)&&
+                 (wd->sta.wepStatus != ZM_ENCRYPTION_AES) )
+            {
+                return ZM_ERR_DATA_NOT_ENCRYPTED;
+            }
+            #endif
+        }
+    }
+
+    return ZM_SUCCESS;
+}
+
+void zfStaMicFailureHandling(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t   da0;
+    u8_t   micNotify = 1;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( wd->sta.wpaState <  ZM_STA_WPA_STATE_PK_OK )
+    {
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+
+    wd->sta.cmMicFailureCount++;
+
+    if ( wd->sta.cmMicFailureCount == 1 )
+    {
+        zm_debug_msg0("get the first MIC failure");
+        //zfTimerSchedule(dev, ZM_EVENT_CM_TIMER, ZM_TICK_CM_TIMEOUT);
+
+        /* Timer Resolution on WinXP is 15/16 ms  */
+        /* Decrease Time offset for <XP> Counter Measure */
+        zfTimerSchedule(dev, ZM_EVENT_CM_TIMER, ZM_TICK_CM_TIMEOUT - ZM_TICK_CM_TIMEOUT_OFFSET);
+    }
+    else if ( wd->sta.cmMicFailureCount == 2 )
+    {
+        zm_debug_msg0("get the second MIC failure");
+        /* reserve 2 second for OS to send MIC failure report to AP */
+        wd->sta.cmDisallowSsidLength = wd->sta.ssidLen;
+        zfMemoryCopy(wd->sta.cmDisallowSsid, wd->sta.ssid, wd->sta.ssidLen);
+        //wd->sta.cmMicFailureCount = 0;
+        zfTimerCancel(dev, ZM_EVENT_CM_TIMER);
+        //zfTimerSchedule(dev, ZM_EVENT_CM_DISCONNECT, ZM_TICK_CM_DISCONNECT);
+
+        /* Timer Resolution on WinXP is 15/16 ms  */
+        /* Decrease Time offset for <XP> Counter Measure */
+        zfTimerSchedule(dev, ZM_EVENT_CM_DISCONNECT, ZM_TICK_CM_DISCONNECT - ZM_TICK_CM_DISCONNECT_OFFSET);
+    }
+    else
+    {
+        micNotify = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (micNotify == 1)
+    {
+        da0 = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+        if ( da0 & 0x01 )
+        {
+            if (wd->zfcbMicFailureNotify != NULL)
+            {
+                wd->zfcbMicFailureNotify(dev, wd->sta.bssid, ZM_MIC_GROUP_ERROR);
+            }
+        }
+        else
+        {
+            if (wd->zfcbMicFailureNotify != NULL)
+            {
+                wd->zfcbMicFailureNotify(dev, wd->sta.bssid, ZM_MIC_PAIRWISE_ERROR);
+            }
+        }
+    }
+}
+
+
+u8_t zfStaBlockWlanScan(zdev_t* dev)
+{
+    u8_t   ret=FALSE;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.bChannelScan )
+    {
+        return TRUE;
+    }
+
+    return ret;
+}
+
+void zfStaResetStatus(zdev_t* dev, u8_t bInit)
+{
+    u8_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zfHpDisableBeacon(dev);
+
+    wd->dtim = 1;
+    wd->sta.capability[0] = 0x01;
+    wd->sta.capability[1] = 0x00;
+    /* 802.11h */
+    if (wd->sta.DFSEnable || wd->sta.TPCEnable)
+        wd->sta.capability[1] |= ZM_BIT_0;
+
+    /* release queued packets */
+    for(i=0; i<wd->sta.ibssPSDataCount; i++)
+    {
+        zfwBufFree(dev, wd->sta.ibssPSDataQueue[i], 0);
+    }
+
+    for(i=0; i<wd->sta.staPSDataCount; i++)
+    {
+        zfwBufFree(dev, wd->sta.staPSDataQueue[i], 0);
+    }
+
+    wd->sta.ibssPSDataCount = 0;
+    wd->sta.staPSDataCount = 0;
+    zfZeroMemory((u8_t*) &wd->sta.staPSList, sizeof(struct zsStaPSList));
+
+    wd->sta.wmeConnected = 0;
+    wd->sta.psMgr.tempWakeUp = 0;
+    wd->sta.qosInfo = 0;
+    zfQueueFlush(dev, wd->sta.uapsdQ);
+
+    return;
+
+}
+
+void zfStaIbssMonitoring(zdev_t* dev, u8_t reset)
+{
+    u16_t i;
+    u16_t oppositeCount;
+    struct zsPartnerNotifyEvent event;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    //zm_debug_msg1("zfStaIbssMonitoring %d", wd->sta.oppositeCount);
+
+    zmw_enter_critical_section(dev);
+
+    if ( wd->sta.oppositeCount == 0 )
+    {
+        goto done;
+    }
+
+    if ( wd->sta.bChannelScan )
+    {
+        goto done;
+    }
+
+    oppositeCount = wd->sta.oppositeCount;
+
+    for(i=0; i < ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        if ( oppositeCount == 0 )
+        {
+            break;
+        }
+
+        if ( reset )
+        {
+            wd->sta.oppositeInfo[i].valid = 0;
+        }
+
+        if ( wd->sta.oppositeInfo[i].valid == 0 )
+        {
+            continue;
+        }
+
+        oppositeCount--;
+
+        if ( wd->sta.oppositeInfo[i].aliveCounter )
+        {
+            zm_debug_msg1("Setting alive to ", wd->sta.oppositeInfo[i].aliveCounter);
+
+            zmw_leave_critical_section(dev);
+
+            if ( wd->sta.oppositeInfo[i].aliveCounter != ZM_IBSS_PEER_ALIVE_COUNTER )
+            {
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBEREQ,
+                              (u16_t*)wd->sta.oppositeInfo[i].macAddr, 1, 0, 0);
+            }
+
+            zmw_enter_critical_section(dev);
+            wd->sta.oppositeInfo[i].aliveCounter--;
+        }
+        else
+        {
+            zm_debug_msg0("zfStaIbssMonitoring remove the peer station");
+            zfMemoryCopy(event.bssid, (u8_t *)(wd->sta.bssid), 6);
+            zfMemoryCopy(event.peerMacAddr, wd->sta.oppositeInfo[i].macAddr, 6);
+
+            wd->sta.oppositeInfo[i].valid = 0;
+            wd->sta.oppositeCount--;
+            if (wd->zfcbIbssPartnerNotify != NULL)
+            {
+                zmw_leave_critical_section(dev);
+                wd->zfcbIbssPartnerNotify(dev, 0, &event);
+                zmw_enter_critical_section(dev);
+            }
+        }
+    }
+
+done:
+    if ( reset == 0 )
+    {
+        zfTimerSchedule(dev, ZM_EVENT_IBSS_MONITOR, ZM_TICK_IBSS_MONITOR);
+    }
+
+    zmw_leave_critical_section(dev);
+}
+
+void zfInitPartnerNotifyEvent(zdev_t* dev, zbuf_t* buf, struct zsPartnerNotifyEvent *event)
+{
+    u16_t  *peerMacAddr;
+
+    zmw_get_wlan_dev(dev);
+
+    peerMacAddr = (u16_t *)event->peerMacAddr;
+
+    zfMemoryCopy(event->bssid, (u8_t *)(wd->sta.bssid), 6);
+    peerMacAddr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+    peerMacAddr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET + 2);
+    peerMacAddr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET + 4);
+}
+
+void zfStaInitOppositeInfo(zdev_t* dev)
+{
+    int i;
+
+    zmw_get_wlan_dev(dev);
+
+    for(i=0; i<ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        wd->sta.oppositeInfo[i].valid = 0;
+        wd->sta.oppositeInfo[i].aliveCounter = ZM_IBSS_PEER_ALIVE_COUNTER;
+    }
+}
+#ifdef ZM_ENABLE_CENC
+u16_t zfStaAddIeCenc(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (wd->sta.cencIe[1] != 0)
+    {
+        zfCopyToIntTxBuffer(dev, buf, wd->sta.cencIe, offset, wd->sta.cencIe[1]+2);
+        offset += (wd->sta.cencIe[1]+2);
+    }
+    return offset;
+}
+#endif //ZM_ENABLE_CENC
+u16_t zfStaProcessAction(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t category, actionDetails;
+    zmw_get_wlan_dev(dev);
+
+    category = zmw_rx_buf_readb(dev, buf, 24);
+    actionDetails = zmw_rx_buf_readb(dev, buf, 25);
+    switch (category)
+    {
+        case 0:		//Spectrum Management
+	        switch(actionDetails)
+	        {
+	        	case 0:			//Measurement Request
+	        		break;
+	        	case 1:			//Measurement Report
+	        		//ProcessActionSpectrumFrame_MeasurementReport(Adapter,pActionBody+3);
+	        		break;
+	        	case 2:			//TPC request
+                    //if (wd->sta.TPCEnable)
+                    //    zfStaUpdateDot11HTPC(dev, buf);
+	        		break;
+	        	case 3:			//TPC report
+                    //if (wd->sta.TPCEnable)
+                    //    zfStaUpdateDot11HTPC(dev, buf);
+	        		break;
+	        	case 4:			//Channel Switch Announcement
+                    if (wd->sta.DFSEnable)
+                        zfStaUpdateDot11HDFS(dev, buf);
+	        		break;
+	        	default:
+	        		zm_debug_msg1("Action Frame contain not support action field ", actionDetails);
+	        		break;
+	        }
+        	break;
+        case ZM_WLAN_BLOCK_ACK_ACTION_FRAME:
+            zfAggBlockAckActionFrame(dev, buf);
+            break;
+        case 17:	//Qos Management
+        	break;
+    }
+
+    return 0;
+}
+
+/* Determine the time not send beacon , if more than some value ,
+   re-write the beacon start address */
+void zfReWriteBeaconStartAddress(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->tickIbssSendBeacon++;    // Increase 1 per 10ms .
+    zmw_leave_critical_section(dev);
+
+    if ( wd->tickIbssSendBeacon == 40 )
+    {
+//        DbgPrint("20070727");
+        zfHpEnableBeacon(dev, ZM_MODE_IBSS, wd->beaconInterval, wd->dtim, (u8_t)wd->sta.atimWindow);
+        zmw_enter_critical_section(dev);
+        wd->tickIbssSendBeacon = 0;
+        zmw_leave_critical_section(dev);
+    }
+}
+
+struct zsTkipSeed* zfStaGetRxSeed(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t keyIndex;
+    u8_t da0;
+
+    zmw_get_wlan_dev(dev);
+
+    /* if need not check MIC, return NULL */
+    if ( ((wd->sta.encryMode != ZM_TKIP)&&(wd->sta.encryMode != ZM_AES))||
+         (wd->sta.wpaState < ZM_STA_WPA_STATE_PK_OK) )
+    {
+        return NULL;
+    }
+
+    da0 = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+
+    if ((zmw_rx_buf_readb(dev, buf, 0) & 0x80) == 0x80)
+        keyIndex = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_IV_OFFSET+5); /* Qos Packet*/
+    else
+        keyIndex = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_IV_OFFSET+3); /* normal Packet*/
+    keyIndex = (keyIndex & 0xc0) >> 6;
+
+    return (&wd->sta.rxSeed[keyIndex]);
+}
+
+void zfStaEnableSWEncryption(zdev_t *dev, u8_t value)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.SWEncryptEnable = value;
+    zfHpSWDecrypt(dev, 1);
+    zfHpSWEncrypt(dev, 1);
+}
+
+void zfStaDisableSWEncryption(zdev_t *dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.SWEncryptEnable = 0;
+    zfHpSWDecrypt(dev, 0);
+    zfHpSWEncrypt(dev, 0);
+}
+
+u16_t zfComputeBssInfoWeightValue(zdev_t *dev, u8_t isBMode, u8_t isHT, u8_t isHT40, u8_t signalStrength)
+{
+	u8_t  weightOfB           = 0;
+	u8_t  weightOfAGBelowThr  = 0;
+	u8_t  weightOfAGUpThr     = 15;
+	u8_t  weightOfN20BelowThr = 15;
+	u8_t  weightOfN20UpThr    = 30;
+	u8_t  weightOfN40BelowThr = 16;
+	u8_t  weightOfN40UpThr    = 32;
+
+    zmw_get_wlan_dev(dev);
+
+    if( isBMode == 0 )
+        return (signalStrength + weightOfB);    // pure b mode , do not add the weight value for this AP !
+    else
+    {
+        if( isHT == 0 && isHT40 == 0 )
+        { // a , g , b/g mode ! add the weight value 15 for this AP if it's signal strength is more than some value !
+            if( signalStrength < 18 ) // -77 dBm
+				return signalStrength + weightOfAGBelowThr;
+			else
+				return (signalStrength + weightOfAGUpThr);
+        }
+        else if( isHT == 1 && isHT40 == 0 )
+        { // 80211n mode use 20MHz
+            if( signalStrength < 23 ) // -72 dBm
+                return (signalStrength + weightOfN20BelowThr);
+            else
+                return (signalStrength + weightOfN20UpThr);
+        }
+        else // isHT == 1 && isHT40 == 1
+        { // 80211n mode use 40MHz
+            if( signalStrength < 16 ) // -79 dBm
+                return (signalStrength + weightOfN40BelowThr);
+            else
+                return (signalStrength + weightOfN40UpThr);
+        }
+    }
+}
+
+u16_t zfStaAddIbssAdditionalIE(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+	u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<wd->sta.ibssAdditionalIESize; i++)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.ibssAdditionalIE[i]);
+    }
+
+    return offset;
+}
diff --git a/drivers/staging/otus/80211core/coid.c b/drivers/staging/otus/80211core/coid.c
new file mode 100644
index 000000000000..6007f3131f8f
--- /dev/null
+++ b/drivers/staging/otus/80211core/coid.c
@@ -0,0 +1,2695 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : iod.c                                                 */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains OID functions.                             */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "../hal/hpreg.h"
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiWlanQueryMacAddress      */
+/*      Query OWN MAC address.                                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      addr : for return MAC address                                   */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfiWlanQueryMacAddress(zdev_t* dev, u8_t* addr)
+{
+    u16_t vapId = 0;
+    zmw_get_wlan_dev(dev);
+
+    vapId = zfwGetVapId(dev);
+
+    addr[0] = (u8_t)(wd->macAddr[0] & 0xff);
+    addr[1] = (u8_t)(wd->macAddr[0] >> 8);
+    addr[2] = (u8_t)(wd->macAddr[1] & 0xff);
+    addr[3] = (u8_t)(wd->macAddr[1] >> 8);
+    addr[4] = (u8_t)(wd->macAddr[2] & 0xff);
+    if (vapId == 0xffff)
+        addr[5] = (u8_t)(wd->macAddr[2] >> 8);
+    else
+    {
+#ifdef ZM_VAPMODE_MULTILE_SSID
+        addr[5] = (u8_t)(wd->macAddr[2] >> 8); // Multiple SSID
+#else
+        addr[5] = vapId + 1 + (u8_t)(wd->macAddr[2] >> 8); //VAP
+#endif
+    }
+
+    return;
+}
+
+void zfiWlanQueryBssList(zdev_t* dev, struct zsBssList* pBssList)
+{
+    struct zsBssInfo*   pBssInfo;
+    struct zsBssInfo*   pDstBssInfo;
+    u8_t   i;
+    u8_t*  pMemList;
+    u8_t*  pMemInfo;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    pMemList = (u8_t*) pBssList;
+    pMemInfo = pMemList + sizeof(struct zsBssList);
+    pBssList->head = (struct zsBssInfo*) pMemInfo;
+
+    zmw_enter_critical_section(dev);
+
+    pBssInfo = wd->sta.bssList.head;
+    pDstBssInfo = (struct zsBssInfo*) pMemInfo;
+    pBssList->bssCount = wd->sta.bssList.bssCount;
+
+    for( i=0; i<wd->sta.bssList.bssCount; i++ )
+    {
+        zfMemoryCopy((u8_t*)pDstBssInfo, (u8_t*)pBssInfo,
+                sizeof(struct zsBssInfo));
+
+        if ( pBssInfo->next != NULL )
+        {
+            pBssInfo = pBssInfo->next;
+            pDstBssInfo->next = pDstBssInfo + 1;
+            pDstBssInfo++;
+        }
+        else
+        {
+            zm_assert(i==(wd->sta.bssList.bssCount-1));
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zfScanMgrScanAck(dev);
+}
+
+void zfiWlanQueryBssListV1(zdev_t* dev, struct zsBssListV1* bssListV1)
+{
+    struct zsBssInfo*   pBssInfo;
+    //struct zsBssInfo*   pDstBssInfo;
+    u8_t   i, j, bdrop = 0, k = 0, Same_Count = 0;
+    u8_t   bssid[6];
+    //u8_t*  pMemList;
+    //u8_t*  pMemInfo;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    bssListV1->bssCount = wd->sta.bssList.bssCount;
+
+    pBssInfo = wd->sta.bssList.head;
+    ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, bssid);
+
+    for( i=0; i<wd->sta.bssList.bssCount; i++ )
+    {
+        bdrop = 0;
+        if ( zfStaIsConnected(dev)
+             && (wd->wlanMode == ZM_MODE_INFRASTRUCTURE ) )
+        {
+			for (j = 0; j < 6; j++)
+            {
+                if ( pBssInfo->bssid[j] != bssid[j] )
+                {
+                    break;
+                }
+            }
+
+            if  ( (j == 6)
+                  &&((pBssInfo->ssid[1] == wd->sta.ssidLen) || (pBssInfo->ssid[1] == 0) )&& (pBssInfo->frequency == wd->frequency) )
+            {
+				if(pBssInfo->ssid[1] == 0)
+					pBssInfo->ssid[1] = wd->sta.ssidLen;
+
+				if(Same_Count == 0)
+				{//First meet
+					Same_Count++;
+				}
+				else
+				{//same one
+					bdrop = 1;
+					bssListV1->bssCount--;
+				}
+
+            }
+        }
+
+        if (bdrop == 0)
+        {
+            zfMemoryCopy((u8_t*)(&bssListV1->bssInfo[k]), (u8_t*)pBssInfo,
+                sizeof(struct zsBssInfo));
+
+			if(Same_Count == 1)
+			{
+				zfMemoryCopy(&(bssListV1->bssInfo[k].ssid[2]), wd->sta.ssid, wd->sta.ssidLen);
+				Same_Count++;
+			}
+
+			k++;
+        }
+
+        if ( pBssInfo->next != NULL )
+        {
+            pBssInfo = pBssInfo->next;
+        }
+        else
+        {
+            zm_assert(i==(wd->sta.bssList.bssCount-1));
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zfScanMgrScanAck(dev);
+}
+
+void zfiWlanQueryAdHocCreatedBssDesc(zdev_t* dev, struct zsBssInfo *pBssInfo)
+{
+    zmw_get_wlan_dev(dev);
+
+    zfMemoryCopy((u8_t *)pBssInfo, (u8_t *)&wd->sta.ibssBssDesc, sizeof(struct zsBssInfo));
+}
+
+u8_t zfiWlanQueryAdHocIsCreator(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.ibssBssIsCreator;
+}
+
+u32_t zfiWlanQuerySupportMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->supportMode;
+}
+
+u32_t zfiWlanQueryTransmitPower(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    u32_t ret = 0;
+
+    if (zfStaIsConnected(dev)) {
+        ret = wd->sta.connPowerInHalfDbm;
+    } else {
+        ret = zfHpGetTransmitPower(dev);
+    }
+
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiWlanFlushBssList         */
+/*      Flush BSSID List.                                               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+void zfiWlanFlushBssList(zdev_t* dev)
+{
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    /* Call zfBssInfoRefresh() twice to remove all entry */
+    zfBssInfoRefresh(dev, 1);
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetWlanMode(zdev_t* dev, u8_t wlanMode)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->ws.wlanMode = wlanMode;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetAuthenticationMode(zdev_t* dev, u8_t authMode)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->ws.authMode = authMode;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetWepStatus(zdev_t* dev, u8_t wepStatus)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->ws.wepStatus = wepStatus;
+    zmw_leave_critical_section(dev);
+
+}
+
+void zfiWlanSetSSID(zdev_t* dev, u8_t* ssid, u8_t ssidLength)
+{
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( ssidLength <= 32 )
+    {
+        zmw_enter_critical_section(dev);
+
+        wd->ws.ssidLen = ssidLength;
+        zfMemoryCopy(wd->ws.ssid, ssid, ssidLength);
+
+        if ( ssidLength < 32 )
+        {
+            wd->ws.ssid[ssidLength] = 0;
+        }
+
+        wd->ws.probingSsidList[0].ssidLen = ssidLength;
+        zfMemoryCopy(wd->ws.probingSsidList[0].ssid, ssid, ssidLength);
+        for (i=1; i<ZM_MAX_PROBE_HIDDEN_SSID_SIZE; i++)
+        {
+            wd->ws.probingSsidList[i].ssidLen = 0;
+        }
+
+        zmw_leave_critical_section(dev);
+    }
+}
+
+void zfiWlanSetFragThreshold(zdev_t* dev, u16_t fragThreshold)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (fragThreshold == 0)
+    {   /* fragmentation is disabled */
+        wd->fragThreshold = 32767;
+    }
+    else if (fragThreshold < 256)
+    {
+        /* Minimum fragment threshold */
+        wd->fragThreshold = 256;
+    }
+    else if (fragThreshold > 2346)
+    {
+        wd->fragThreshold = 2346;
+    }
+    else
+    {
+        wd->fragThreshold = fragThreshold & 0xfffe;
+    }
+
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetRtsThreshold(zdev_t* dev, u16_t rtsThreshold)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->rtsThreshold = rtsThreshold;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetFrequency(zdev_t* dev, u32_t frequency, u8_t bImmediate)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( bImmediate )
+    {
+        zmw_enter_critical_section(dev);
+        wd->frequency = (u16_t) (frequency/1000);
+        zmw_leave_critical_section(dev);
+        zfCoreSetFrequency(dev, wd->frequency);
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+        if( frequency == 0 )
+        { // Auto select clean channel depend on wireless environment !
+            wd->ws.autoSetFrequency = 0;
+        }
+        wd->ws.frequency = (u16_t) (frequency/1000);
+        zmw_leave_critical_section(dev);
+    }
+}
+
+void zfiWlanSetBssid(zdev_t* dev, u8_t* bssid)
+{
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    for (i=0; i<6; i++)
+    {
+        wd->ws.desiredBssid[i] = bssid[i];
+    }
+    wd->ws.bDesiredBssid = TRUE;
+    zmw_leave_critical_section(dev);
+
+}
+
+void zfiWlanSetBeaconInterval(zdev_t* dev,
+                              u16_t  beaconInterval,
+                              u8_t   bImmediate)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( bImmediate )
+    {
+        zmw_enter_critical_section(dev);
+        wd->beaconInterval = beaconInterval;
+        zmw_leave_critical_section(dev);
+
+        /* update beacon interval here */
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+        wd->ws.beaconInterval = beaconInterval;
+        zmw_leave_critical_section(dev);
+    }
+}
+
+
+void zfiWlanSetDtimCount(zdev_t* dev, u8_t  dtim)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (dtim > 0)
+    {
+        wd->ws.dtim = dtim;
+    }
+    zmw_leave_critical_section(dev);
+}
+
+
+void zfiWlanSetAtimWindow(zdev_t* dev, u16_t atimWindow, u8_t bImmediate)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( bImmediate )
+    {
+        zmw_enter_critical_section(dev);
+        wd->sta.atimWindow = atimWindow;
+        zmw_leave_critical_section(dev);
+
+        /* atim window here */
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+        wd->ws.atimWindow = atimWindow;
+        zmw_leave_critical_section(dev);
+    }
+}
+
+
+void zfiWlanSetEncryMode(zdev_t* dev, u8_t encryMode)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        /* Hostapd Issue */
+        if ((wd->ws.encryMode != ZM_AES) && (wd->ws.encryMode != ZM_TKIP))
+            wd->ws.encryMode = encryMode;
+    }
+    else
+        wd->ws.encryMode = encryMode;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetDefaultKeyId(zdev_t* dev, u8_t keyId)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.keyId = keyId;
+}
+
+u8_t zfiWlanQueryIsPKInstalled(zdev_t *dev, u8_t *staMacAddr)
+{
+    u8_t isInstalled = 0;
+
+#if 1
+//#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    u8_t   res, peerIdx;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    res = zfStaFindOppositeByMACAddr(dev, (u16_t *)staMacAddr, &peerIdx);
+    if( res == 0 )
+    {
+        isInstalled = wd->sta.oppositeInfo[peerIdx].pkInstalled;
+    }
+    zmw_leave_critical_section(dev);
+//#endif
+#endif
+
+    return isInstalled;
+}
+
+u8_t zfiWlanSetKey(zdev_t* dev, struct zsKeyInfo keyInfo)
+{
+    u16_t  broadcast[3] = {0xffff, 0xffff, 0xffff};
+    u32_t* key;
+    u8_t   encryMode = ZM_NO_WEP;
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    u8_t   encryType = ZM_NO_WEP;
+#endif
+    u8_t   micKey[16];
+    u16_t  id = 0;
+    u8_t   vapId, i, addr[6];
+    u8_t   userIdx=0;
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    /* Determine opposite exist or not */
+    u8_t   res, peerIdx;
+//    u8_t   userIdx=0;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.ibssWpa2Psk == 1 )
+    {
+        zmw_enter_critical_section(dev);
+        res = zfStaFindOppositeByMACAddr(dev, (u16_t*)keyInfo.macAddr, &peerIdx);
+        if( res == 0 )
+        {
+            userIdx = peerIdx;
+            if ( wd->sta.oppositeInfo[userIdx].camIdx == 0xff )
+                wd->sta.oppositeInfo[userIdx].camIdx = userIdx;
+        }
+        zmw_leave_critical_section(dev);
+    }
+#else
+    zmw_get_wlan_dev(dev);
+#endif
+
+    if ( keyInfo.flag & ZM_KEY_FLAG_AUTHENTICATOR )
+    {   /* set key by authenticator */
+        /* set pairwise key */
+        if (keyInfo.flag & ZM_KEY_FLAG_PK)
+        {
+            /* Find STA's information */
+            if ((id = zfApFindSta(dev, keyInfo.macAddr)) == 0xffff)
+            {
+                /* Can't STA in the staTable */
+                return ZM_STATUS_FAILURE;
+            }
+
+            wd->ap.staTable[id].iv16 = 0;
+            wd->ap.staTable[id].iv32 = 0;
+
+            if (keyInfo.keyLength == 32)
+            {   /* TKIP */
+                //u8_t KeyRsc[6] = {0, 0, 0, 0, 0, 0};
+
+                /* In the current AP mode, we set KeyRsc to zero */
+                //zfTkipInit(keyInfo.key, (u8_t*) wd->macAddr,
+                //           &(wd->ap.staTable[id].txSeed), KeyRsc);
+                //zfTkipInit(keyInfo.key, (u8_t*) keyInfo.macAddr,
+                //           &(wd->ap.staTable[id].rxSeed), KeyRsc);
+#ifdef ZM_ENABLE_CENC
+                if (keyInfo.flag & ZM_KEY_FLAG_CENC)
+                {
+                    zm_debug_msg0("Set CENC pairwise Key");
+
+                    wd->ap.staTable[id].encryMode = ZM_CENC;
+
+                    /* Reset txiv and rxiv */
+                    wd->ap.staTable[id].txiv[0] = 0x5c365c37;
+                    wd->ap.staTable[id].txiv[1] = 0x5c365c36;
+                    wd->ap.staTable[id].txiv[2] = 0x5c365c36;
+                    wd->ap.staTable[id].txiv[3] = 0x5c365c36;
+
+                    wd->ap.staTable[id].rxiv[0] = 0x5c365c36;
+                    wd->ap.staTable[id].rxiv[1] = 0x5c365c36;
+                    wd->ap.staTable[id].rxiv[2] = 0x5c365c36;
+                    wd->ap.staTable[id].rxiv[3] = 0x5c365c36;
+
+                    /* Set Key Index */
+                    wd->ap.staTable[id].cencKeyIdx = keyInfo.keyIndex;
+
+                    //zfCoreSetKey(dev, id+1, 1, ZM_CENC, (u16_t *)keyInfo.macAddr,
+                    //          (u32_t*) &keyInfo.key[16]);
+                }
+                else
+#endif //ZM_ENABLE_CENC
+                {
+                    wd->ap.staTable[id].encryMode = ZM_TKIP;
+
+                    zfMemoryCopy(micKey, &keyInfo.key[16], 8);
+                    zfMemoryCopy(&micKey[8], &keyInfo.key[24], 8);
+
+                    //zfCoreSetKey(dev, id+1, 1, ZM_TKIP, (u16_t *)keyInfo.macAddr,
+                    //           (u32_t*) micKey);
+
+                    /* For fragmentation, we use software MIC */
+                    zfMemoryCopy((u8_t *)&(wd->ap.staTable[id].txMicKey), &(keyInfo.key[16]), 8);
+                    zfMemoryCopy((u8_t *)&(wd->ap.staTable[id].rxMicKey), &(keyInfo.key[24]), 8);
+
+                }
+            }
+            else if (keyInfo.keyLength == 16)
+            {   /* AES */
+                wd->ap.staTable[id].encryMode = ZM_AES;
+            }
+            else if (keyInfo.keyLength == 0)
+            {
+                /* Clear Key Info */
+                zfApClearStaKey(dev, (u16_t *)keyInfo.macAddr);
+
+                return ZM_STATUS_SUCCESS;
+            }
+            else
+            {
+                return ZM_STATUS_FAILURE;
+            }
+
+            //zfCoreSetKey(dev, id+1, 0, wd->ap.staTable[id].encryMode,
+            //      (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+            zfHpSetApPairwiseKey(dev, (u16_t *)keyInfo.macAddr,
+                    wd->ap.staTable[id].encryMode, (u32_t*) keyInfo.key,
+                    (u32_t*) &keyInfo.key[16], id+1);
+            wd->ap.staTable[id].keyIdx = id + 1 + 4;
+        }
+        else if (keyInfo.flag & ZM_KEY_FLAG_GK)
+        {
+            vapId = keyInfo.vapId;
+
+            wd->ap.iv16[vapId] = 0;
+            wd->ap.iv32[vapId] = 0;
+
+            if (keyInfo.keyLength == 32)
+            {   /* TKIP */
+                //u8_t KeyRsc[6] = {0, 0, 0, 0, 0, 0};
+
+                //zfTkipInit(keyInfo.key, (u8_t*) wd->macAddr,
+                //           &(wd->ap.bcSeed), KeyRsc);
+#ifdef ZM_ENABLE_CENC
+                if (keyInfo.flag & ZM_KEY_FLAG_CENC)
+                {
+                    encryMode = ZM_CENC;
+                    zm_debug_msg0("Set CENC group Key");
+
+                    /* Reset txiv and rxiv */
+                    wd->ap.txiv[vapId][0] = 0x5c365c36;
+                    wd->ap.txiv[vapId][1] = 0x5c365c36;
+                    wd->ap.txiv[vapId][2] = 0x5c365c36;
+                    wd->ap.txiv[vapId][3] = 0x5c365c36;
+
+                    //zfCoreSetKey(dev, 0, 1, ZM_CENC, keyInfo.vapAddr,
+                    //          (u32_t*) &keyInfo.key[16]);
+                    key = (u32_t*) keyInfo.key;
+                }
+                else
+#endif //ZM_ENABLE_CENC
+                {
+                    encryMode = ZM_TKIP;
+                    key = (u32_t *)keyInfo.key;
+
+                    /* set MIC key to HMAC */
+                    //zfCoreSetKey(dev, 0, 1, ZM_TKIP, broadcast,
+                    //         (u32_t*) (&keyInfo.key[16]));
+                    //zfCoreSetKey(dev, 0, 1, ZM_TKIP, keyInfo.vapAddr,
+                    //           (u32_t*) (&keyInfo.key[16]));
+
+                    zfMicSetKey(&(keyInfo.key[16]), &(wd->ap.bcMicKey[0]));
+                    key = (u32_t*) keyInfo.key;
+                }
+            }
+            else if (keyInfo.keyLength == 16)
+            {   /* AES */
+                encryMode = ZM_AES;
+                key = (u32_t *)keyInfo.key;
+                zm_debug_msg0("CWY - Set AES Group Key");
+            }
+            else if (keyInfo.keyLength == 0)
+            {
+                /* Clear Key Info */
+                zfApClearStaKey(dev, broadcast);
+
+                /* Turn off WEP bit in the capability field */
+                wd->ap.capab[vapId] &= 0xffef;
+
+                return ZM_STATUS_SUCCESS;
+            }
+            else
+            {   /* WEP */
+                if (keyInfo.keyLength == 5)
+                {
+                    encryMode = ZM_WEP64;
+                }
+                else if (keyInfo.keyLength == 13)
+                {
+                    encryMode = ZM_WEP128;
+                }
+                else if (keyInfo.keyLength == 29)
+                {
+                    encryMode = ZM_WEP256;
+                }
+
+                key = (u32_t*) keyInfo.key;
+            }
+
+            // Modification for CAM not support VAP search
+            //zfCoreSetKey(dev, 0, 0, encryMode, broadcast, key);
+            //zfCoreSetKey(dev, 0, 0, encryMode, wd->macAddr, key);
+            //zfCoreSetKey(dev, 0, 0, encryMode, keyInfo.vapAddr, key);
+            zfHpSetApGroupKey(dev, wd->macAddr, encryMode,
+                    key, (u32_t*) &keyInfo.key[16], vapId);
+
+            //zfiWlanSetEncryMode(dev, encryMode);
+            wd->ws.encryMode = encryMode;
+
+            /* set the multicast address encryption type */
+            wd->ap.encryMode[vapId] = encryMode;
+
+            /* set the multicast key index */
+            wd->ap.bcKeyIndex[vapId] = keyInfo.keyIndex;
+            wd->ap.bcHalKeyIdx[vapId] = vapId + 60;
+
+            /* Turn on WEP bit in the capability field */
+            wd->ap.capab[vapId] |= 0x10;
+        }
+    }
+    else
+    {   /* set by supplicant */
+
+        if ( keyInfo.flag & ZM_KEY_FLAG_PK )
+        {   /* set pairwise key */
+
+            //zfTkipInit(keyInfo.key, (u8_t*) wd->macAddr,
+            //           &wd->sta.txSeed, keyInfo.initIv);
+            //zfTkipInit(keyInfo.key, (u8_t*) wd->sta.bssid,
+            //           &wd->sta.rxSeed[keyInfo.keyIndex], keyInfo.initIv);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+            if ( wd->sta.ibssWpa2Psk == 1 )
+            {
+                /* unicast -- > pairwise key */
+                wd->sta.oppositeInfo[userIdx].iv16 = 0;
+                wd->sta.oppositeInfo[userIdx].iv32 = 0;
+            }
+            else
+            {
+                wd->sta.iv16 = 0;
+                wd->sta.iv32 = 0;
+            }
+
+            wd->sta.oppositeInfo[userIdx].pkInstalled = 1;
+#else
+            wd->sta.iv16 = 0;
+            wd->sta.iv32 = 0;
+
+            wd->sta.oppositeInfo[userIdx].pkInstalled = 1;
+#endif
+
+            if ( keyInfo.keyLength == 32 )
+            {   /* TKIP */
+                zfTkipInit(keyInfo.key, (u8_t*) wd->macAddr,
+                        &wd->sta.txSeed, keyInfo.initIv);
+                zfTkipInit(keyInfo.key, (u8_t*) wd->sta.bssid,
+                        &wd->sta.rxSeed[keyInfo.keyIndex], keyInfo.initIv);
+
+#ifdef ZM_ENABLE_CENC
+                if (keyInfo.flag & ZM_KEY_FLAG_CENC)
+                {
+                    zm_debug_msg0("Set CENC pairwise Key");
+
+                    wd->sta.encryMode = ZM_CENC;
+
+                    /* Reset txiv and rxiv */
+                    wd->sta.txiv[0] = 0x5c365c36;
+                    wd->sta.txiv[1] = 0x5c365c36;
+                    wd->sta.txiv[2] = 0x5c365c36;
+                    wd->sta.txiv[3] = 0x5c365c36;
+
+                    wd->sta.rxiv[0] = 0x5c365c37;
+                    wd->sta.rxiv[1] = 0x5c365c36;
+                    wd->sta.rxiv[2] = 0x5c365c36;
+                    wd->sta.rxiv[3] = 0x5c365c36;
+
+                    /* Set Key Index */
+                    wd->sta.cencKeyId = keyInfo.keyIndex;
+
+                    //zfCoreSetKey(dev, id+1, 1, ZM_CENC, (u16_t *)keyInfo.macAddr,
+                    //         (u32_t*) &keyInfo.key[16]);
+                }
+                else
+#endif //ZM_ENABLE_CENC
+                {
+                    wd->sta.encryMode = ZM_TKIP;
+
+                    //zfCoreSetKey(dev, 0, 1, ZM_TKIP, wd->sta.bssid,
+                    //         (u32_t*) &keyInfo.key[16]);
+
+                    zfMicSetKey(&keyInfo.key[16], &wd->sta.txMicKey);
+                    zfMicSetKey(&keyInfo.key[24],
+                                &wd->sta.rxMicKey[keyInfo.keyIndex]);
+                }
+            }
+            else if ( keyInfo.keyLength == 16 )
+            {   /* AES */
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+                if ( wd->sta.ibssWpa2Psk == 1 )
+                {
+                    wd->sta.oppositeInfo[userIdx].encryMode = ZM_AES;
+                    encryType = wd->sta.oppositeInfo[userIdx].encryMode;
+                }
+                else
+                {
+                    wd->sta.encryMode = ZM_AES;
+                    encryType = wd->sta.encryMode;
+                }
+#else
+                wd->sta.encryMode = ZM_AES;
+#endif
+            }
+            else
+            {
+                return ZM_STATUS_FAILURE;
+            }
+
+            /* user 0 */
+            //zfCoreSetKey(dev, 0, 0, wd->sta.encryMode,
+            //         wd->sta.bssid, (u32_t*) keyInfo.key);
+            //zfHpSetStaPairwiseKey(dev, wd->sta.bssid, wd->sta.encryMode,
+            //    (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+            if ( (keyInfo.keyLength==16) && (wd->sta.ibssWpa2Psk==1) )
+            { /* If not AES-CCMP and ibss network , use traditional */
+                zfHpSetPerUserKey(dev,
+                                userIdx,
+                                keyInfo.keyIndex,  // key id == 0 ( Pairwise key = 0 )
+                                (u8_t*)keyInfo.macAddr,   // RX need Source Address ( Address 2 )
+                                encryType,
+//                              wd->sta.encryMode,
+                                (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+                wd->sta.oppositeInfo[userIdx].wpaState = ZM_STA_WPA_STATE_PK_OK ;
+            }
+            else
+            {/* Big Endian and Little Endian Compatibility */
+                for (i = 0; i < 3; i++)
+                {
+                    addr[2 * i] = wd->sta.bssid[i] & 0xff;
+                    addr[2 * i + 1] = wd->sta.bssid[i] >> 8;
+                }
+                zfHpSetPerUserKey(dev,
+                                    ZM_USER_KEY_PK,   // user id
+                                    0,                // key id
+                                    addr,//(u8_t *)wd->sta.bssid,
+                              wd->sta.encryMode,
+                              (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+                wd->sta.keyId = 4;
+            }
+#else
+            /* Big Endian and Little Endian Compatibility */
+            for (i = 0; i < 3; i++)
+            {
+                addr[2 * i] = wd->sta.bssid[i] & 0xff;
+                addr[2 * i + 1] = wd->sta.bssid[i] >> 8;
+            }
+            zfHpSetPerUserKey(dev,
+                              ZM_USER_KEY_PK,   // user id
+                              0,                // key id
+                              addr,//(u8_t *)wd->sta.bssid,
+                              wd->sta.encryMode,
+                              (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+            wd->sta.keyId = 4;
+#endif
+
+            wd->sta.wpaState = ZM_STA_WPA_STATE_PK_OK;
+        }
+        else if ( keyInfo.flag & ZM_KEY_FLAG_GK )
+        {   /* set group key */
+
+            zfTkipInit(keyInfo.key, (u8_t*) wd->sta.bssid,
+                       &wd->sta.rxSeed[keyInfo.keyIndex], keyInfo.initIv);
+
+            if ( keyInfo.keyLength == 32 )
+            {   /* TKIP */
+#ifdef ZM_ENABLE_CENC
+                if (keyInfo.flag & ZM_KEY_FLAG_CENC)
+                {
+                    encryMode = ZM_CENC;
+                    zm_debug_msg0("Set CENC group Key");
+
+                    /* Reset txiv and rxiv */
+                    wd->sta.rxivGK[0] = 0x5c365c36;
+                    wd->sta.rxivGK[1] = 0x5c365c36;
+                    wd->sta.rxivGK[2] = 0x5c365c36;
+                    wd->sta.rxivGK[3] = 0x5c365c36;
+
+                    //zfCoreSetKey(dev, 0, 1, ZM_CENC, keyInfo.vapAddr,
+                    //         (u32_t*) &keyInfo.key[16]);
+                    key = (u32_t*) keyInfo.key;
+                }
+                else
+#endif //ZM_ENABLE_CENC
+                {
+                    encryMode = ZM_TKIP;
+                    key = (u32_t*) wd->sta.rxSeed[keyInfo.keyIndex].tk;
+
+                    if ( !(keyInfo.flag & ZM_KEY_FLAG_INIT_IV) )
+                    {
+                        wd->sta.rxSeed[keyInfo.keyIndex].iv16 = 0;
+                        wd->sta.rxSeed[keyInfo.keyIndex].iv32 = 0;
+                    }
+
+                    /* set MIC key to HMAC */
+                    //zfCoreSetKey(dev, 8, 1, ZM_TKIP, broadcast,
+                    //         (u32_t*) (&keyInfo.key[16]));
+
+                    zfMicSetKey(&keyInfo.key[24],
+                                &wd->sta.rxMicKey[keyInfo.keyIndex]);
+                }
+            }
+            else if ( keyInfo.keyLength == 16 )
+            {   /* AES */
+                encryMode = ZM_AES;
+                //key = (u32_t*) wd->sta.rxSeed[keyInfo.keyIndex].tk;
+            }
+            else
+            {   /* WEP */
+                if ( keyInfo.keyLength == 5 )
+                {
+                    encryMode = ZM_WEP64;
+                }
+                else if ( keyInfo.keyLength == 13 )
+                {
+                    encryMode = ZM_WEP128;
+                }
+                else if ( keyInfo.keyLength == 29 )
+                {
+                    encryMode = ZM_WEP256;
+                }
+
+                key = (u32_t*) keyInfo.key;
+            }
+
+            /* user 8 */
+            //zfCoreSetKey(dev, 8, 0, encryMode, broadcast, key);
+            //zfHpSetStaGroupKey(dev, broadcast, encryMode,
+            //        (u32_t*) keyInfo.key, (u32_t*) (&keyInfo.key[16]));
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+            if ( (keyInfo.keyLength==16) && (wd->sta.ibssWpa2Psk==1) )
+            {/* If not AES-CCMP and ibss network , use traditional */
+                zfHpSetPerUserKey(dev,
+                              userIdx,
+                              keyInfo.keyIndex,                // key id
+                              // (u8_t *)broadcast,                  // for only 2 stations IBSS netwrl ( A2 )
+                              (u8_t*)keyInfo.macAddr,   // for multiple ( > 2 ) stations IBSS network ( A2 )
+                              encryMode,
+                              (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+            }
+            else
+            {
+                zfHpSetPerUserKey(dev,
+                                ZM_USER_KEY_GK,   // user id
+                                0,                // key id
+                                (u8_t *)broadcast,
+                                encryMode,
+                                (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+                wd->sta.wpaState = ZM_STA_WPA_STATE_GK_OK;
+            }
+#else
+            zfHpSetPerUserKey(dev,
+                              ZM_USER_KEY_GK,   // user id
+                              0,                // key id
+                              (u8_t *)broadcast,
+                              encryMode,
+                              (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+            wd->sta.wpaState = ZM_STA_WPA_STATE_GK_OK;
+#endif
+        }
+        else
+        {   /* legacy WEP */
+            zm_debug_msg0("legacy WEP");
+
+            if ( keyInfo.keyIndex >= 4 )
+            {
+                return ZM_STATUS_FAILURE;
+            }
+
+            if ( keyInfo.keyLength == 5 )
+            {
+                zm_debug_msg0("WEP 64");
+
+                encryMode = ZM_WEP64;
+            }
+            else if ( keyInfo.keyLength == 13 )
+            {
+                zm_debug_msg0("WEP 128");
+
+                encryMode = ZM_WEP128;
+            }
+            else if ( keyInfo.keyLength == 32 )
+            {
+                /* TKIP */
+                #if 0
+                // Don't reset the IV since some AP would fail in IV check and drop our connection
+                if ( wd->sta.wpaState != ZM_STA_WPA_STATE_PK_OK )
+                {
+                    wd->sta.iv16 = 0;
+                    wd->sta.iv32 = 0;
+                }
+                #endif
+
+                encryMode = ZM_TKIP;
+
+                zfTkipInit(keyInfo.key, (u8_t*) wd->sta.bssid,
+                           &wd->sta.rxSeed[keyInfo.keyIndex], keyInfo.initIv);
+                zfMicSetKey(&keyInfo.key[24],
+                           &wd->sta.rxMicKey[keyInfo.keyIndex]);
+            }
+            else if ( keyInfo.keyLength == 16 )
+            {
+                /* AES */
+                #if 0
+                // Don't reset the IV since some AP would fail in IV check and drop our connection
+                if ( wd->sta.wpaState != ZM_STA_WPA_STATE_PK_OK )
+                {
+                    /* broadcast -- > group key */
+                    /* Only initialize when set our default key ! */
+                    wd->sta.iv16 = 0;
+                    wd->sta.iv32 = 0;
+                }
+                #endif
+
+                encryMode = ZM_AES;
+            }
+            else if ( keyInfo.keyLength == 29 )
+            {
+                zm_debug_msg0("WEP 256");
+
+                encryMode = ZM_WEP256;
+                //zfCoreSetKey(dev, 64, 1, wd->sta.encryMode,
+                //         wd->sta.bssid, (u32_t*) (&keyInfo.key[16]));
+            }
+            else
+            {
+                return ZM_STATUS_FAILURE;
+            }
+
+            {
+                u8_t i;
+
+                zm_debug_msg0("key = ");
+                for(i = 0; i < keyInfo.keyLength; i++)
+                {
+                    zm_debug_msg2("", keyInfo.key[i]);
+                }
+            }
+
+            if ( keyInfo.flag & ZM_KEY_FLAG_DEFAULT_KEY )
+            {
+                //for WEP default key 1~3 and ATOM platform--CWYang(+)
+                vapId = 0;
+                wd->ap.bcHalKeyIdx[vapId] = keyInfo.keyIndex;
+                wd->ap.bcKeyIndex[vapId] = keyInfo.keyIndex;
+                wd->sta.keyId = keyInfo.keyIndex;
+            }
+
+			if(encryMode == ZM_TKIP)
+			{
+				if(wd->TKIP_Group_KeyChanging == 0x1)
+				{
+					zm_debug_msg0("Countermeasure : Cancel Old Timer ");
+					zfTimerCancel(dev,	ZM_EVENT_SKIP_COUNTERMEASURE);
+				}
+				else
+				{
+					zm_debug_msg0("Countermeasure : Create New Timer ");
+				}
+
+				wd->TKIP_Group_KeyChanging = 0x1;
+				zfTimerSchedule(dev, ZM_EVENT_SKIP_COUNTERMEASURE, 150);
+			}
+
+
+
+			//------------------------------------------------------------------------
+
+            /* use default key */
+            //zfCoreSetKey(dev, ZM_USER_KEY_DEFAULT+keyInfo.keyIndex, 0,
+            //         wd->sta.encryMode, wd->sta.bssid, (u32_t*) keyInfo.key);
+
+            if ( encryMode == ZM_TKIP ||
+                 encryMode == ZM_AES )
+            {
+                zfHpSetDefaultKey(dev, keyInfo.keyIndex, encryMode,
+                                 (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+            if ( (keyInfo.keyLength==16) && (wd->sta.ibssWpa2Psk==1) )
+            {/* If not AES-CCMP and ibss network , use traditional */
+                wd->sta.wpaState = ZM_STA_WPA_STATE_PK_OK;
+            }
+            else
+            {
+                if (wd->sta.wpaState == ZM_STA_WPA_STATE_PK_OK)
+                    wd->sta.wpaState = ZM_STA_WPA_STATE_GK_OK;
+                else
+                {
+                    wd->sta.wpaState = ZM_STA_WPA_STATE_PK_OK;
+                    wd->sta.encryMode = encryMode;
+                    wd->ws.encryMode = encryMode;
+                }
+            }
+#else
+                if (wd->sta.wpaState == ZM_STA_WPA_STATE_PK_OK)
+                    wd->sta.wpaState = ZM_STA_WPA_STATE_GK_OK;
+                else if ( wd->sta.wpaState == ZM_STA_WPA_STATE_INIT )
+                {
+                    wd->sta.wpaState = ZM_STA_WPA_STATE_PK_OK;
+                    wd->sta.encryMode = encryMode;
+                    wd->ws.encryMode = encryMode;
+                }
+#endif
+            }
+            else
+            {
+                zfHpSetDefaultKey(dev, keyInfo.keyIndex, encryMode,
+                               (u32_t*) keyInfo.key, NULL);
+
+                /* Save key for software WEP */
+                zfMemoryCopy(wd->sta.wepKey[keyInfo.keyIndex], keyInfo.key,
+                        keyInfo.keyLength);
+
+                /* TODO: Check whether we need to save the SWEncryMode */
+                wd->sta.SWEncryMode[keyInfo.keyIndex] = encryMode;
+
+                wd->sta.encryMode = encryMode;
+                wd->ws.encryMode = encryMode;
+            }
+        }
+    }
+
+//    wd->sta.flagKeyChanging = 1;
+    return ZM_STATUS_SUCCESS;
+}
+
+/* PSEUDO test */
+u8_t zfiWlanPSEUDOSetKey(zdev_t* dev, struct zsKeyInfo keyInfo)
+{
+    //u16_t  broadcast[3] = {0xffff, 0xffff, 0xffff};
+    //u32_t* key;
+    u8_t   micKey[16];
+
+    zmw_get_wlan_dev(dev);
+
+    switch (keyInfo.keyLength)
+    {
+        case 5:
+            wd->sta.encryMode = ZM_WEP64;
+            /* use default key */
+            zfCoreSetKey(dev, 64, 0, ZM_WEP64, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+		          break;
+
+       	case 13:
+            wd->sta.encryMode = ZM_WEP128;
+            /* use default key */
+            zfCoreSetKey(dev, 64, 0, ZM_WEP128, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+          		break;
+
+       	case 29:
+            wd->sta.encryMode = ZM_WEP256;
+            /* use default key */
+            zfCoreSetKey(dev, 64, 1, ZM_WEP256,  (u16_t *)keyInfo.macAddr, (u32_t*) (&keyInfo.key[16]));
+            zfCoreSetKey(dev, 64, 0, ZM_WEP256, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+		          break;
+
+       	case 16:
+            wd->sta.encryMode = ZM_AES;
+            //zfCoreSetKey(dev, 0, 0, ZM_AES, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+            zfCoreSetKey(dev, 64, 0, ZM_AES, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+            break;
+
+       	case 32:
+#ifdef ZM_ENABLE_CENC
+            if (keyInfo.flag & ZM_KEY_FLAG_CENC)
+            {
+                u16_t boardcastAddr[3] = {0xffff, 0xffff, 0xffff};
+                u16_t Addr_a[] = { 0x0000, 0x0080, 0x0901};
+                u16_t Addr_b[] = { 0x0000, 0x0080, 0x0902};
+                /* CENC test: user0,1 and user2 for boardcast */
+                wd->sta.encryMode = ZM_CENC;
+                zfCoreSetKey(dev, 0, 1, ZM_CENC, (u16_t *)Addr_a, (u32_t*) (&keyInfo.key[16]));
+                zfCoreSetKey(dev, 0, 0, ZM_CENC, (u16_t *)Addr_a, (u32_t*) keyInfo.key);
+
+                zfCoreSetKey(dev, 1, 1, ZM_CENC, (u16_t *)Addr_b, (u32_t*) (&keyInfo.key[16]));
+                zfCoreSetKey(dev, 1, 0, ZM_CENC, (u16_t *)Addr_b, (u32_t*) keyInfo.key);
+
+                zfCoreSetKey(dev, 2, 1, ZM_CENC, (u16_t *)boardcastAddr, (u32_t*) (&keyInfo.key[16]));
+                zfCoreSetKey(dev, 2, 0, ZM_CENC, (u16_t *)boardcastAddr, (u32_t*) keyInfo.key);
+
+                /* Initialize PN sequence */
+                wd->sta.txiv[0] = 0x5c365c36;
+                wd->sta.txiv[1] = 0x5c365c36;
+                wd->sta.txiv[2] = 0x5c365c36;
+                wd->sta.txiv[3] = 0x5c365c36;
+            }
+            else
+#endif //ZM_ENABLE_CENC
+            {
+                wd->sta.encryMode = ZM_TKIP;
+                zfCoreSetKey(dev, 64, 1, ZM_TKIP, (u16_t *)keyInfo.macAddr, (u32_t*) micKey);
+                zfCoreSetKey(dev, 64, 0, ZM_TKIP, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+            }
+            break;
+        default:
+            wd->sta.encryMode = ZM_NO_WEP;
+    }
+
+    return ZM_STATUS_SUCCESS;
+}
+
+void zfiWlanSetPowerSaveMode(zdev_t* dev, u8_t mode)
+{
+#if 0
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.powerSaveMode = mode;
+
+    /* send null data with PwrBit to inform AP */
+    if ( mode > ZM_STA_PS_NONE )
+    {
+        if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+        {
+            zfSendNullData(dev, 1);
+        }
+
+        /* device into PS mode */
+        zfPSDeviceSleep(dev);
+    }
+#endif
+
+    zfPowerSavingMgrSetMode(dev, mode);
+}
+
+void zfiWlanSetMacAddress(zdev_t* dev, u16_t* mac)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->macAddr[0] = mac[0];
+    wd->macAddr[1] = mac[1];
+    wd->macAddr[2] = mac[2];
+
+    zfHpSetMacAddress(dev, mac, 0);
+}
+
+u8_t zfiWlanQueryWlanMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->wlanMode;
+}
+
+u8_t zfiWlanQueryAdapterState(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->state;
+}
+
+u8_t zfiWlanQueryAuthenticationMode(zdev_t* dev, u8_t bWrapper)
+{
+    u8_t   authMode;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( bWrapper )
+    {
+        authMode = wd->ws.authMode;
+    }
+    else
+    {
+        //authMode = wd->sta.authMode;
+        authMode = wd->sta.currentAuthMode;
+    }
+
+    return authMode;
+}
+
+u8_t zfiWlanQueryWepStatus(zdev_t* dev, u8_t bWrapper)
+{
+    u8_t wepStatus;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( bWrapper )
+    {
+        wepStatus = wd->ws.wepStatus;
+    }
+    else
+    {
+        wepStatus = wd->sta.wepStatus;
+    }
+
+    return wepStatus;
+}
+
+void zfiWlanQuerySSID(zdev_t* dev, u8_t* ssid, u8_t* pSsidLength)
+{
+    u16_t vapId = 0;
+    zmw_get_wlan_dev(dev);
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        vapId = zfwGetVapId(dev);
+
+        if (vapId == 0xffff)
+        {
+            *pSsidLength = wd->ap.ssidLen[0];
+            zfMemoryCopy(ssid, wd->ap.ssid[0], wd->ap.ssidLen[0]);
+        }
+        else
+        {
+            *pSsidLength = wd->ap.ssidLen[vapId + 1];
+            zfMemoryCopy(ssid, wd->ap.ssid[vapId + 1], wd->ap.ssidLen[vapId + 1]);
+        }
+    }
+    else
+    {
+        *pSsidLength = wd->sta.ssidLen;
+        zfMemoryCopy(ssid, wd->sta.ssid, wd->sta.ssidLen);
+    }
+}
+
+u16_t zfiWlanQueryFragThreshold(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->fragThreshold;
+}
+
+u16_t zfiWlanQueryRtsThreshold(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->rtsThreshold;
+}
+
+u32_t zfiWlanQueryFrequency(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return (wd->frequency*1000);
+}
+
+/***********************************************************
+ * Function: zfiWlanQueryCurrentFrequency
+ * Return value:
+ *   -   0 : no validate current frequency
+ *   - (>0): current frequency depend on "qmode"
+ * Input:
+ *   - qmode:
+ *      0: return value depend on the support mode, this
+           qmode is use to solve the bug #31223
+ *      1: return the actually current frequency
+ ***********************************************************/
+u32_t zfiWlanQueryCurrentFrequency(zdev_t* dev, u8_t qmode)
+{
+    u32_t frequency;
+
+    zmw_get_wlan_dev(dev);
+
+    switch (qmode)
+    {
+    case 0:
+        if (wd->sta.currentFrequency > 3000)
+        {
+            if (wd->supportMode & ZM_WIRELESS_MODE_5)
+            {
+                frequency = wd->sta.currentFrequency;
+            }
+            else if (wd->supportMode & ZM_WIRELESS_MODE_24)
+            {
+                frequency = zfChGetFirst2GhzChannel(dev);
+            }
+            else
+            {
+                frequency = 0;
+            }
+        }
+        else
+        {
+            if (wd->supportMode & ZM_WIRELESS_MODE_24)
+            {
+                frequency = wd->sta.currentFrequency;
+            }
+            else if (wd->supportMode & ZM_WIRELESS_MODE_5)
+            {
+                frequency = zfChGetLast5GhzChannel(dev);
+            }
+            else
+            {
+                frequency = 0;
+            }
+        }
+        break;
+
+    case 1:
+        frequency = wd->sta.currentFrequency;
+        break;
+
+    default:
+        frequency = 0;
+    }
+
+    return (frequency*1000);
+}
+
+u32_t zfiWlanQueryFrequencyAttribute(zdev_t* dev, u32_t freq)
+{
+    zmw_get_wlan_dev(dev);
+
+    u8_t  i;
+    u16_t frequency = (u16_t) (freq/1000);
+    u32_t ret = 0;
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if ( wd->regulationTable.allowChannel[i].channel == frequency )
+        {
+            ret = wd->regulationTable.allowChannel[i].channelFlags;
+        }
+    }
+
+    return ret;
+}
+
+/* BandWidth  0=>20  1=>40 */
+/* ExtOffset  0=>20  1=>high control 40   3=>low control 40 */
+void zfiWlanQueryFrequencyHT(zdev_t* dev, u32_t *bandWidth, u32_t *extOffset)
+{
+    zmw_get_wlan_dev(dev);
+
+    *bandWidth = wd->BandWidth40;
+    *extOffset = wd->ExtOffset;
+}
+
+u8_t zfiWlanQueryCWMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->cwm.cw_mode;
+}
+
+u32_t zfiWlanQueryCWEnable(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->cwm.cw_enable;
+}
+
+void zfiWlanQueryBssid(zdev_t* dev, u8_t* bssid)
+{
+    u8_t   addr[6];
+
+    zmw_get_wlan_dev(dev);
+
+    ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, addr);
+    zfMemoryCopy(bssid, addr, 6);
+}
+
+u16_t zfiWlanQueryBeaconInterval(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->beaconInterval;
+}
+
+u32_t zfiWlanQueryRxBeaconTotal(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    wd->sta.rxBeaconTotal += wd->sta.rxBeaconCount;
+
+    return wd->sta.rxBeaconTotal;
+}
+
+u16_t zfiWlanQueryAtimWindow(zdev_t* dev)
+{
+    u16_t atimWindow;
+
+    zmw_get_wlan_dev(dev);
+
+    atimWindow = wd->sta.atimWindow;
+
+    return atimWindow;
+}
+
+u8_t zfiWlanQueryEncryMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (wd->wlanMode == ZM_MODE_AP)
+        return wd->ap.encryMode[0];
+    else
+        return wd->sta.encryMode;
+}
+
+u16_t zfiWlanQueryCapability(zdev_t* dev)
+{
+    u16_t capability;
+
+    zmw_get_wlan_dev(dev);
+
+    capability = wd->sta.capability[0] +
+                 (((u16_t) wd->sta.capability[1]) << 8);
+
+    return capability;
+
+}
+
+u16_t zfiWlanQueryAid(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.aid;
+}
+
+void zfiWlanQuerySupportRate(zdev_t* dev, u8_t* rateArray, u8_t* pLength)
+{
+    u8_t   i, j=0;
+
+    zmw_get_wlan_dev(dev);
+
+    for( i=0; i<4; i++ )
+    {
+        if ( wd->bRate & (0x1 << i) )
+        {
+            rateArray[j] = zg11bRateTbl[i] +
+                           ((wd->bRateBasic & (0x1<<i))<<(7-i));
+            j++;
+        }
+    }
+
+    *pLength = j;
+}
+
+void zfiWlanQueryExtSupportRate(zdev_t* dev, u8_t* rateArray, u8_t* pLength)
+{
+    u8_t   i, j=0;
+
+    zmw_get_wlan_dev(dev);
+
+    for( i=0; i<8; i++ )
+    {
+        if ( wd->gRate & (0x1 << i) )
+        {
+            rateArray[j] = zg11gRateTbl[i] +
+                           ((wd->gRateBasic & (0x1<<i))<<(7-i));
+            j++;
+        }
+    }
+
+    *pLength = j;
+}
+
+void zfiWlanQueryRsnIe(zdev_t* dev, u8_t* ie, u8_t* pLength)
+{
+    u8_t len;
+
+    zmw_get_wlan_dev(dev);
+
+    len = wd->sta.rsnIe[1] + 2;
+    zfMemoryCopy(ie, wd->sta.rsnIe, len);
+    *pLength = len;
+}
+
+void zfiWlanQueryWpaIe(zdev_t* dev, u8_t* ie, u8_t* pLength)
+{
+    u8_t len;
+
+    zmw_get_wlan_dev(dev);
+
+    len = wd->sta.wpaIe[1] + 2;
+    zfMemoryCopy(ie, wd->sta.wpaIe, len);
+    *pLength = len;
+
+}
+
+u8_t zfiWlanQueryMulticastCipherAlgo(zdev_t *dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch( wd->sta.currentAuthMode )
+    {
+        case ZM_AUTH_MODE_WPA2PSK:
+        case ZM_AUTH_MODE_WPA2:
+            if ( wd->sta.rsnIe[7] == 2 )
+            {
+                return ZM_TKIP;
+            }
+            else
+            {
+                return ZM_AES;
+            }
+            break;
+
+        case ZM_AUTH_MODE_WPAPSK:
+        case ZM_AUTH_MODE_WPA:
+            if ( wd->sta.rsnIe[11] == 2 )
+            {
+                return ZM_TKIP;
+            }
+            else
+            {
+                return ZM_AES;
+            }
+            break;
+
+        default:
+            return wd->sta.encryMode;
+    }
+}
+
+u8_t zfiWlanQueryHTMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    // 0:Legancy, 1:N
+    return wd->sta.EnableHT;
+}
+
+u8_t zfiWlanQueryBandWidth40(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    // 0:20M, 1:40M
+    return wd->BandWidth40;
+}
+
+u16_t zfiWlanQueryRegionCode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->regulationTable.regionCode;
+}
+void zfiWlanSetWpaIe(zdev_t* dev, u8_t* ie, u8_t Length)
+{
+    u16_t vapId = 0;
+    zmw_get_wlan_dev(dev);
+
+    if (wd->wlanMode == ZM_MODE_AP) // AP Mode
+    {
+        vapId = zfwGetVapId(dev);
+
+        if (vapId == 0xffff)
+            vapId = 0;
+        else
+            vapId++;
+
+        zm_assert(Length < ZM_MAX_WPAIE_SIZE);
+        if (Length < ZM_MAX_WPAIE_SIZE)
+        {
+            wd->ap.wpaLen[vapId] = Length;
+            zfMemoryCopy(wd->ap.wpaIe[vapId], ie, wd->ap.wpaLen[vapId]);
+        }
+
+    }
+    else
+    {
+        wd->sta.wpaLen = Length;
+        zfMemoryCopy(wd->sta.wpaIe, ie, wd->sta.wpaLen);
+    }
+    //zfiWlanSetWpaSupport(dev, 1);
+    if (wd->wlanMode == ZM_MODE_AP) // AP Mode
+    {
+        wd->ap.wpaSupport[vapId] = 1;
+    }
+    else
+    {
+        wd->sta.wpaSupport = 1;
+    }
+
+}
+
+void zfiWlanSetWpaSupport(zdev_t* dev, u8_t WpaSupport)
+{
+    u16_t vapId = 0;
+    zmw_get_wlan_dev(dev);
+
+    if (wd->wlanMode == ZM_MODE_AP) // AP Mode
+    {
+        vapId = zfwGetVapId(dev);
+
+        if (vapId == 0xffff)
+            vapId = 0;
+        else
+            vapId++;
+
+        wd->ap.wpaSupport[vapId] = WpaSupport;
+    }
+    else
+    {
+        wd->sta.wpaSupport = WpaSupport;
+    }
+
+}
+
+void zfiWlanSetProtectionMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.bProtectionMode = mode;
+    if (wd->sta.bProtectionMode == TRUE)
+    {
+        zfHpSetSlotTime(dev, 0);
+    }
+    else
+    {
+        zfHpSetSlotTime(dev, 1);
+    }
+
+    zm_msg1_mm(ZM_LV_1, "wd->protectionMode=", wd->sta.bProtectionMode);
+}
+
+void zfiWlanSetBasicRate(zdev_t* dev, u8_t bRateSet, u8_t gRateSet,
+                         u32_t nRateSet)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.bRateBasic = bRateSet;
+    wd->ws.gRateBasic = gRateSet;
+    wd->ws.nRateBasic = nRateSet;
+}
+
+void zfiWlanSetBGMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.bgMode = mode;
+}
+
+void zfiWlanSetpreambleType(zdev_t* dev, u8_t type)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.preambleType = type;
+}
+
+u8_t zfiWlanQuerypreambleType(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->ws.preambleType;
+}
+
+u8_t zfiWlanQueryPowerSaveMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.powerSaveMode;
+}
+
+u8_t zfiWlanSetPmkidInfo(zdev_t* dev, u16_t* bssid, u8_t* pmkid)
+{
+    u32_t  i;
+
+    zmw_get_wlan_dev(dev);
+
+    for(i=0; i<wd->sta.pmkidInfo.bssidCount; i++)
+    {
+        if ( zfMemoryIsEqual((u8_t*) wd->sta.pmkidInfo.bssidInfo[i].bssid,
+                             (u8_t*) bssid, 6) )
+        {
+            /* matched */
+            break;
+        }
+    }
+
+    if ( i < wd->sta.pmkidInfo.bssidCount )
+    {
+        /* overwrite the original one */
+        zfMemoryCopy(wd->sta.pmkidInfo.bssidInfo[i].pmkid, pmkid, 16);
+    }
+    else
+    {
+        if ( i < ZM_PMKID_MAX_BSS_CNT )
+        {
+            wd->sta.pmkidInfo.bssidInfo[i].bssid[0] = bssid[0];
+            wd->sta.pmkidInfo.bssidInfo[i].bssid[1] = bssid[1];
+            wd->sta.pmkidInfo.bssidInfo[i].bssid[2] = bssid[2];
+
+            zfMemoryCopy(wd->sta.pmkidInfo.bssidInfo[i].pmkid, pmkid, 16);
+            wd->sta.pmkidInfo.bssidCount++;
+        }
+    }
+
+    return 0;
+}
+
+u32_t zfiWlanQueryPmkidInfo(zdev_t* dev, u8_t* buf, u32_t len)
+{
+    //struct zsPmkidInfo* pPmkidInfo = ( struct zsPmkidInfo* ) buf;
+    u32_t  size;
+
+    zmw_get_wlan_dev(dev);
+
+    size = sizeof(u32_t) +
+           wd->sta.pmkidInfo.bssidCount * sizeof(struct zsPmkidBssidInfo);
+
+    if ( len < size )
+    {
+        return wd->sta.pmkidInfo.bssidCount;
+    }
+
+    zfMemoryCopy(buf, (u8_t*) &wd->sta.pmkidInfo, (u16_t) size);
+
+    return 0;
+}
+
+void zfiWlanSetMulticastList(zdev_t* dev, u8_t size, u8_t* pList)
+{
+    struct zsMulticastAddr* pMacList = (struct zsMulticastAddr*) pList;
+    u8_t   i;
+    u8_t   bAllMulticast = 0;
+    //u32_t  value;
+
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.multicastList.size = size;
+    for(i=0; i<size; i++)
+    {
+        zfMemoryCopy(wd->sta.multicastList.macAddr[i].addr,
+                     pMacList[i].addr, 6);
+    }
+
+    if ( wd->sta.osRxFilter & ZM_PACKET_TYPE_ALL_MULTICAST )
+        bAllMulticast = 1;
+    zfHpSetMulticastList(dev, size, pList, bAllMulticast);
+
+}
+
+void zfiWlanRemoveKey(zdev_t* dev, u8_t keyType, u8_t keyId)
+{
+    u16_t  fakeMacAddr[3] = {0, 0, 0};
+    u32_t  fakeKey[4] = {0, 0, 0, 0};
+
+    zmw_get_wlan_dev(dev);
+
+    if ( keyType == 0 )
+    {
+        /* remove WEP key */
+        zm_debug_msg0("remove WEP key");
+        zfCoreSetKey(dev, ZM_USER_KEY_DEFAULT+keyId, 0,
+                 ZM_NO_WEP, fakeMacAddr, fakeKey);
+        wd->sta.encryMode = ZM_NO_WEP;
+    }
+    else if ( keyType == 1 )
+    {
+        /* remove pairwise key */
+        zm_debug_msg0("remove pairwise key");
+        zfHpRemoveKey(dev, ZM_USER_KEY_PK);
+        wd->sta.encryMode = ZM_NO_WEP;
+    }
+    else
+    {
+        /* remove group key */
+        zm_debug_msg0("remove group key");
+        zfHpRemoveKey(dev, ZM_USER_KEY_GK);
+    }
+}
+
+
+void zfiWlanQueryRegulationTable(zdev_t* dev, struct zsRegulationTable* pEntry)
+{
+    zmw_get_wlan_dev(dev);
+
+    zfMemoryCopy((u8_t*) pEntry, (u8_t*) &wd->regulationTable,
+                 sizeof(struct zsRegulationTable));
+}
+
+/* parameter "time" is specified in ms */
+void zfiWlanSetScanTimerPerChannel(zdev_t* dev, u16_t time)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg1("scan time (ms) = ", time);
+
+    wd->sta.activescanTickPerChannel = time / ZM_MS_PER_TICK;
+}
+
+void zfiWlanSetAutoReconnect(zdev_t* dev, u8_t enable)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.bAutoReconnect = enable;
+    //wd->sta.bAutoReconnectEnabled = enable;
+}
+
+void zfiWlanSetStaWme(zdev_t* dev, u8_t enable, u8_t uapsdInfo)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.staWmeEnabled = enable & 0x3;
+    if ((enable & 0x2) != 0)
+    {
+        wd->ws.staWmeQosInfo = uapsdInfo & 0x6f;
+    }
+    else
+    {
+        wd->ws.staWmeQosInfo = 0;
+    }
+}
+
+void zfiWlanSetApWme(zdev_t* dev, u8_t enable)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.apWmeEnabled = enable;
+}
+
+u8_t zfiWlanQuerywmeEnable(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->ws.staWmeEnabled;
+}
+
+void zfiWlanSetProbingHiddenSsid(zdev_t* dev, u8_t* ssid, u8_t ssidLen,
+    u16_t entry)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    if ((ssidLen <= 32) && (entry < ZM_MAX_PROBE_HIDDEN_SSID_SIZE))
+    {
+        zmw_enter_critical_section(dev);
+        wd->ws.probingSsidList[entry].ssidLen = ssidLen;
+        zfMemoryCopy(wd->ws.probingSsidList[entry].ssid, ssid, ssidLen);
+        zmw_leave_critical_section(dev);
+    }
+
+    return;
+}
+
+void zfiWlanSetDisableProbingWithSsid(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.disableProbingWithSsid = mode;
+
+    return;
+}
+
+void zfiWlanSetDropUnencryptedPackets(zdev_t* dev, u8_t enable)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.dropUnencryptedPkts = enable;
+}
+
+void zfiWlanSetStaRxSecurityCheckCb(zdev_t* dev, zfpStaRxSecurityCheckCb pStaRxSecurityCheckCb)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.pStaRxSecurityCheckCb = pStaRxSecurityCheckCb;
+}
+
+void zfiWlanSetIBSSJoinOnly(zdev_t* dev, u8_t joinOnly)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.ibssJoinOnly = joinOnly;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiConfigWdsPort            */
+/*      Configure WDS port.                                             */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      wdsPortId : WDS port ID, start from 0                           */
+/*      flag : 0=>disable WDS port, 1=>enable WDS port                  */
+/*      wdsAddr : WDS neighbor MAC address                              */
+/*      encType : encryption type for WDS port                          */
+/*      wdsKey : encryption key for WDS port                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfiConfigWdsPort(zdev_t* dev, u8_t wdsPortId, u16_t flag, u16_t* wdsAddr,
+        u16_t encType, u32_t* wdsKey)
+{
+    u16_t addr[3];
+    u32_t key[4];
+
+    zmw_get_wlan_dev(dev);
+
+    if (wdsPortId > ZM_MAX_WDS_SUPPORT)
+    {
+        return ZM_ERR_WDS_PORT_ID;
+    }
+
+    if (flag == 1)
+    {
+        /* Enable WDS port */
+        wd->ap.wds.macAddr[wdsPortId][0] = wdsAddr[0];
+        wd->ap.wds.macAddr[wdsPortId][1] = wdsAddr[1];
+        wd->ap.wds.macAddr[wdsPortId][2] = wdsAddr[2];
+
+        wd->ap.wds.wdsBitmap |= (1 << wdsPortId);
+        wd->ap.wds.encryMode[wdsPortId] = (u8_t) encType;
+
+        zfCoreSetKey(dev, 10+ZM_MAX_WDS_SUPPORT, 0, (u8_t) encType, wdsAddr, wdsKey);
+    }
+    else
+    {
+        /* Disable WDS port */
+        addr[0] = addr[1] = addr[2] = 0;
+        key[0] = key[1] = key[2] = key[3] = 0;
+        wd->ap.wds.wdsBitmap &= (~(1 << wdsPortId));
+        zfCoreSetKey(dev, 10+ZM_MAX_WDS_SUPPORT, 0, ZM_NO_WEP, addr, key);
+    }
+
+    return ZM_SUCCESS;
+}
+#ifdef ZM_ENABLE_CENC
+/* CENC */
+void zfiWlanQueryGSN(zdev_t* dev, u8_t *gsn, u16_t vapId)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u32_t txiv[4];
+    zmw_get_wlan_dev(dev);
+
+    /* convert little endian to big endian for 32 bits */
+    txiv[3] = wd->ap.txiv[vapId][0];
+    txiv[2] = wd->ap.txiv[vapId][1];
+    txiv[1] = wd->ap.txiv[vapId][2];
+    txiv[0] = wd->ap.txiv[vapId][3];
+
+    zfMemoryCopy(gsn, (u8_t*)txiv, 16);
+}
+#endif //ZM_ENABLE_CENC
+//CWYang(+)
+void zfiWlanQuerySignalInfo(zdev_t* dev, u8_t *buffer)
+{
+    zmw_get_wlan_dev(dev);
+
+    /*Change Signal Strength/Quality Value to Human Sense Here*/
+
+    buffer[0] = wd->SignalStrength;
+    buffer[1] = wd->SignalQuality;
+}
+
+/* OS-XP */
+u16_t zfiStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType)
+{
+    return  zfStaAddIeWpaRsn(dev, buf, offset, frameType);
+}
+
+/* zfiDebugCmd                                                                        */
+/*     cmd       value-description                                                  */
+/*         0       schedule timer                                                     */
+/*         1       cancel timer                                                         */
+/*         2       clear timer                                                           */
+/*         3       test timer                                                            */
+/*         4                                                                                 */
+/*         5                                                                                 */
+/*         6       checksum test     0/1                                           */
+/*         7       enableProtectionMode                                          */
+/*         8       rx packet content dump    0/1                               */
+
+u32_t zfiDebugCmd(zdev_t* dev, u32_t cmd, u32_t value)
+{
+    u16_t event;
+    u32_t tick;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+
+    zmw_enter_critical_section(dev);
+
+    if ( cmd == 0 )
+    {   /* schedule timer */
+        event = (u16_t) ((value >> 16) & 0xffff);
+        tick = value & 0xffff;
+        zfTimerSchedule(dev, event, tick);
+    }
+    else if ( cmd == 1 )
+    {   /* cancel timer */
+        event = (u16_t) (value & 0xffff);
+        zfTimerCancel(dev, event);
+    }
+    else if ( cmd == 2 )
+    {   /* clear timer */
+        zfTimerClear(dev);
+    }
+    else if ( cmd == 3 )
+    {   /* test timer */
+        zfTimerSchedule(dev, 1,  500);
+        zfTimerSchedule(dev, 2, 1000);
+        zfTimerSchedule(dev, 3, 1000);
+        zfTimerSchedule(dev, 4, 1000);
+        zfTimerSchedule(dev, 5, 1500);
+        zfTimerSchedule(dev, 6, 2000);
+        zfTimerSchedule(dev, 7, 2200);
+        zfTimerSchedule(dev, 6, 2500);
+        zfTimerSchedule(dev, 8, 2800);
+    }
+    else if ( cmd == 4)
+    {
+        zfTimerSchedule(dev, 1,  500);
+        zfTimerSchedule(dev, 2, 1000);
+        zfTimerSchedule(dev, 3, 1000);
+        zfTimerSchedule(dev, 4, 1000);
+        zfTimerSchedule(dev, 5, 1500);
+        zfTimerSchedule(dev, 6, 2000);
+        zfTimerSchedule(dev, 7, 2200);
+        zfTimerSchedule(dev, 6, 2500);
+        zfTimerSchedule(dev, 8, 2800);
+        zfTimerCancel(dev, 1);
+        zfTimerCancel(dev, 3);
+        zfTimerCancel(dev, 6);
+    }
+    else if ( cmd == 5 )
+    {
+        wd->sta.keyId = (u8_t) value;
+    }
+	else if ( cmd == 6 )
+	{
+	    /* 0: normal    1: always set TCP/UDP checksum zero */
+        wd->checksumTest = value;
+	}
+	else if ( cmd == 7 )
+	{
+        wd->enableProtectionMode = value;
+   	    zm_msg1_mm(ZM_LV_1, "wd->enableProtectionMode=", wd->enableProtectionMode);
+	}
+	else if ( cmd == 8 )
+	{
+        /* rx packet content dump */
+        if (value)
+        {
+            wd->rxPacketDump = 1;
+        }
+        else
+        {
+            wd->rxPacketDump = 0;
+        }
+	}
+
+
+    zmw_leave_critical_section(dev);
+
+    return 0;
+}
+
+#ifdef ZM_ENABLE_CENC
+u8_t zfiWlanSetCencPairwiseKey(zdev_t* dev, u8_t keyid, u32_t *txiv, u32_t *rxiv,
+        u8_t *key, u8_t *mic)
+{
+    struct zsKeyInfo keyInfo;
+    u8_t cencKey[32];
+    u8_t i;
+    u16_t macAddr[3];
+
+    zmw_get_wlan_dev(dev);
+
+    for (i = 0; i < 16; i++)
+        cencKey[i] = key[i];
+    for (i = 0; i < 16; i++)
+        cencKey[i + 16] = mic[i];
+    keyInfo.key = cencKey;
+    keyInfo.keyLength = 32;
+    keyInfo.keyIndex = keyid;
+    keyInfo.flag = ZM_KEY_FLAG_CENC | ZM_KEY_FLAG_PK;
+    for (i = 0; i < 3; i++)
+        macAddr[i] = wd->sta.bssid[i];
+    keyInfo.macAddr = macAddr;
+
+    zfiWlanSetKey(dev, keyInfo);
+
+    /* Reset txiv and rxiv */
+    //wd->sta.txiv[0] = txiv[0];
+    //wd->sta.txiv[1] = txiv[1];
+    //wd->sta.txiv[2] = txiv[2];
+    //wd->sta.txiv[3] = txiv[3];
+    //
+    //wd->sta.rxiv[0] = rxiv[0];
+    //wd->sta.rxiv[1] = rxiv[1];
+    //wd->sta.rxiv[2] = rxiv[2];
+    //wd->sta.rxiv[3] = rxiv[3];
+
+    return 0;
+}
+
+u8_t zfiWlanSetCencGroupKey(zdev_t* dev, u8_t keyid, u32_t *rxiv,
+        u8_t *key, u8_t *mic)
+{
+    struct zsKeyInfo keyInfo;
+    u8_t cencKey[32];
+    u8_t i;
+    u16_t macAddr[6] = {0xffff, 0xffff, 0xffff};
+
+    zmw_get_wlan_dev(dev);
+
+    for (i = 0; i < 16; i++)
+        cencKey[i] = key[i];
+    for (i = 0; i < 16; i++)
+        cencKey[i + 16] = mic[i];
+    keyInfo.key = cencKey;
+    keyInfo.keyLength = 32;
+    keyInfo.keyIndex = keyid;
+    keyInfo.flag = ZM_KEY_FLAG_CENC | ZM_KEY_FLAG_GK;
+    keyInfo.vapId = 0;
+    for (i = 0; i < 3; i++)
+        keyInfo.vapAddr[i] = wd->macAddr[i];
+    keyInfo.macAddr = macAddr;
+
+    zfiWlanSetKey(dev, keyInfo);
+
+    /* Reset txiv and rxiv */
+    wd->sta.rxivGK[0] = ((rxiv[3] >> 24) & 0xFF)
+                      + (((rxiv[3] >> 16) & 0xFF) << 8)
+                      + (((rxiv[3] >> 8) & 0xFF) << 16)
+                      + ((rxiv[3] & 0xFF) << 24);
+    wd->sta.rxivGK[1] = ((rxiv[2] >> 24) & 0xFF)
+                      + (((rxiv[2] >> 16) & 0xFF) << 8)
+                      + (((rxiv[2] >> 8) & 0xFF) << 16)
+                      + ((rxiv[2] & 0xFF) << 24);
+    wd->sta.rxivGK[2] = ((rxiv[1] >> 24) & 0xFF)
+                      + (((rxiv[1] >> 16) & 0xFF) << 8)
+                      + (((rxiv[1] >> 8) & 0xFF) << 16)
+                      + ((rxiv[1] & 0xFF) << 24);
+    wd->sta.rxivGK[3] = ((rxiv[0] >> 24) & 0xFF)
+                      + (((rxiv[0] >> 16) & 0xFF) << 8)
+                      + (((rxiv[0] >> 8) & 0xFF) << 16)
+                      + ((rxiv[0] & 0xFF) << 24);
+
+    wd->sta.authMode = ZM_AUTH_MODE_CENC;
+    wd->sta.currentAuthMode = ZM_AUTH_MODE_CENC;
+
+    return 0;
+}
+#endif //ZM_ENABLE_CENC
+
+u8_t zfiWlanSetDot11DMode(zdev_t* dev, u8_t mode)
+{
+    u8_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.b802_11D = mode;
+    if (mode) //Enable 802.11d
+    {
+        wd->regulationTable.regionCode = NO_ENUMRD;
+        for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+            wd->regulationTable.allowChannel[i].channelFlags |= ZM_REG_FLAG_CHANNEL_PASSIVE;
+    }
+    else //Disable
+    {
+        for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+            wd->regulationTable.allowChannel[i].channelFlags &= ~ZM_REG_FLAG_CHANNEL_PASSIVE;
+    }
+
+    return 0;
+}
+
+u8_t zfiWlanSetDot11HDFSMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    //zm_debug_msg0("CWY - Enable 802.11h DFS");
+
+    // TODO : DFS Enable in 5250 to 5350 MHz and 5470 to 5725 MHz .
+    //if ( Adapter->ZD80211HSupport &&
+    //   Adapter->CardSetting.NetworkTypeInUse == Ndis802_11OFDM5 &&
+    //   ((ChannelNo >=52 && ChannelNo <= 64)	||				//5250~5350 MHZ
+    //    (ChannelNo >=100 && ChannelNo <= 140))) 			//5470~5725 MHZ
+    //{
+    //   Adapter->ZD80211HSetting.DFSEnable=TRUE;
+    //}
+    //else
+    //{
+    //   Adapter->ZD80211HSetting.DFSEnable=FALSE;
+    //}
+
+    wd->sta.DFSEnable = mode;
+    if (mode)
+        wd->sta.capability[1] |= ZM_BIT_0;
+    else
+        wd->sta.capability[1] &= (~ZM_BIT_0);
+
+    return 0;
+}
+
+u8_t zfiWlanSetDot11HTPCMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    // TODO : TPC Enable in 5150~5350 MHz and 5470~5725MHz.
+    //if ( Adapter->ZD80211HSupport &&
+    //   Adapter->CardSetting.NetworkTypeInUse == Ndis802_11OFDM5 &&
+    //   ((ChannelNo == 36 || ChannelNo == 40 || ChannelNo == 44 || ChannelNo == 48) ||	//5150~5250 MHZ , Not Japan
+    //    (ChannelNo >=52 && ChannelNo <= 64) ||				//5250~5350 MHZ
+    //    (ChannelNo >=100 && ChannelNo <= 140))) 			//5470~5725 MHZ
+    //{
+    //   Adapter->ZD80211HSetting.TPCEnable=TRUE;
+    //}
+    //else
+    //{
+    //   Adapter->ZD80211HSetting.TPCEnable=FALSE;
+    //}
+
+    wd->sta.TPCEnable = mode;
+    if (mode)
+        wd->sta.capability[1] |= ZM_BIT_0;
+    else
+        wd->sta.capability[1] &= (~ZM_BIT_0);
+
+    return 0;
+}
+
+u8_t zfiWlanSetAniMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->aniEnable = mode;
+    if (mode)
+        zfHpAniAttach(dev);
+
+    return 0;
+}
+
+#ifdef ZM_OS_LINUX_FUNC
+void zfiWlanShowTally(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_msg1_mm(ZM_LV_0, "Hw_UnderrunCnt    = ", wd->commTally.Hw_UnderrunCnt);
+    zm_msg1_mm(ZM_LV_0, "Hw_TotalRxFrm     = ", wd->commTally.Hw_TotalRxFrm);
+    zm_msg1_mm(ZM_LV_0, "Hw_CRC32Cnt       = ", wd->commTally.Hw_CRC32Cnt);
+    zm_msg1_mm(ZM_LV_0, "Hw_CRC16Cnt       = ", wd->commTally.Hw_CRC16Cnt);
+    zm_msg1_mm(ZM_LV_1, "Hw_DecrypErr_UNI  = ", wd->commTally.Hw_DecrypErr_UNI);
+    zm_msg1_mm(ZM_LV_0, "Hw_RxFIFOOverrun  = ", wd->commTally.Hw_RxFIFOOverrun);
+    zm_msg1_mm(ZM_LV_1, "Hw_DecrypErr_Mul  = ", wd->commTally.Hw_DecrypErr_Mul);
+    zm_msg1_mm(ZM_LV_1, "Hw_RetryCnt       = ", wd->commTally.Hw_RetryCnt);
+    zm_msg1_mm(ZM_LV_0, "Hw_TotalTxFrm     = ", wd->commTally.Hw_TotalTxFrm);
+    zm_msg1_mm(ZM_LV_0, "Hw_RxTimeOut      = ", wd->commTally.Hw_RxTimeOut);
+    zm_msg1_mm(ZM_LV_0, "Tx_MPDU           = ", wd->commTally.Tx_MPDU);
+    zm_msg1_mm(ZM_LV_0, "BA_Fail           = ", wd->commTally.BA_Fail);
+    zm_msg1_mm(ZM_LV_0, "Hw_Tx_AMPDU       = ", wd->commTally.Hw_Tx_AMPDU);
+    zm_msg1_mm(ZM_LV_0, "Hw_Tx_MPDU        = ", wd->commTally.Hw_Tx_MPDU);
+
+    zm_msg1_mm(ZM_LV_1, "Hw_RxMPDU          = ", wd->commTally.Hw_RxMPDU);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxDropMPDU      = ", wd->commTally.Hw_RxDropMPDU);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxDelMPDU       = ", wd->commTally.Hw_RxDelMPDU);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyMiscError  = ", wd->commTally.Hw_RxPhyMiscError);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyXRError    = ", wd->commTally.Hw_RxPhyXRError);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyOFDMError  = ", wd->commTally.Hw_RxPhyOFDMError);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyCCKError   = ", wd->commTally.Hw_RxPhyCCKError);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyHTError    = ", wd->commTally.Hw_RxPhyHTError);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyTotalCount = ", wd->commTally.Hw_RxPhyTotalCount);
+
+    if (!((wd->commTally.Tx_MPDU == 0) && (wd->commTally.BA_Fail == 0)))
+    {
+        zm_debug_msg_p("BA Fail Ratio(%)  = ", wd->commTally.BA_Fail * 100,
+                (wd->commTally.BA_Fail + wd->commTally.Tx_MPDU));
+    }
+
+    if (!((wd->commTally.Hw_Tx_MPDU == 0) && (wd->commTally.Hw_Tx_AMPDU == 0)))
+    {
+        zm_debug_msg_p("Avg Agg Number    = ",
+                wd->commTally.Hw_Tx_MPDU, wd->commTally.Hw_Tx_AMPDU);
+    }
+}
+#endif
+
+void zfiWlanSetMaxTxPower(zdev_t* dev, u8_t power2, u8_t power5)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->maxTxPower2 = power2;
+    wd->maxTxPower5 = power5;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanQueryMaxTxPower(zdev_t* dev, u8_t *power2, u8_t *power5)
+{
+    zmw_get_wlan_dev(dev);
+
+    *power2 = wd->maxTxPower2;
+    *power5 = wd->maxTxPower5;
+}
+
+void zfiWlanSetConnectMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->connectMode = mode;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetSupportMode(zdev_t* dev, u32_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->supportMode = mode;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetAdhocMode(zdev_t* dev, u32_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.adhocMode = mode;
+}
+
+u32_t zfiWlanQueryAdhocMode(zdev_t* dev, u8_t bWrapper)
+{
+    u32_t adhocMode;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( bWrapper )
+    {
+        adhocMode = wd->ws.adhocMode;
+    }
+    else
+    {
+        adhocMode = wd->wfc.bIbssGMode;
+    }
+
+    return adhocMode;
+}
+
+
+u8_t zfiWlanSetCountryIsoName(zdev_t* dev, u8_t *countryIsoName, u8_t length)
+{
+    u8_t buf[5];
+    zmw_get_wlan_dev(dev);
+
+    if (length == 4)
+    {
+        buf[2] = wd->ws.countryIsoName[0] = countryIsoName[2];
+        buf[3] = wd->ws.countryIsoName[1] = countryIsoName[1];
+        buf[4] = wd->ws.countryIsoName[2] = countryIsoName[0];
+    }
+    else if (length == 3)
+    {
+        buf[2] = wd->ws.countryIsoName[0] = countryIsoName[1];
+        buf[3] = wd->ws.countryIsoName[1] = countryIsoName[0];
+        buf[4] = wd->ws.countryIsoName[2] = '\0';
+    }
+    else
+    {
+        return 1;
+    }
+
+    return zfHpGetRegulationTablefromISO(dev, buf, length);
+}
+
+
+const char* zfiWlanQueryCountryIsoName(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->ws.countryIsoName;
+}
+
+
+
+void zfiWlanSetRegulatory(zdev_t* dev, u8_t CCS, u16_t Code, u8_t bfirstChannel)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if (CCS)
+    {
+        /* Reset Regulation Table by Country Code */
+        zfHpGetRegulationTablefromCountry(dev, Code);
+    }
+    else
+    {
+        /* Reset Regulation Table by Region Code */
+        zfHpGetRegulationTablefromRegionCode(dev, Code);
+    }
+
+    if (bfirstChannel) {
+        zmw_enter_critical_section(dev);
+        wd->frequency = zfChGetFirstChannel(dev, NULL);
+        zmw_leave_critical_section(dev);
+        zfCoreSetFrequency(dev, wd->frequency);
+    }
+}
+
+
+const char* zfiHpGetisoNamefromregionCode(zdev_t* dev, u16_t regionCode)
+{
+    return zfHpGetisoNamefromregionCode(dev, regionCode);
+}
+
+u16_t zfiWlanChannelToFrequency(zdev_t* dev, u8_t channel)
+{
+    return zfChNumToFreq(dev, channel, 0);
+}
+
+u8_t zfiWlanFrequencyToChannel(zdev_t* dev, u16_t freq)
+{
+    u8_t is5GBand = 0;
+
+    return zfChFreqToNum(freq, &is5GBand);
+}
+
+void zfiWlanDisableDfsChannel(zdev_t* dev, u8_t disableFlag)
+{
+    zfHpDisableDfsChannel(dev, disableFlag);
+    return;
+}
+
+void zfiWlanSetLEDCtrlParam(zdev_t* dev, u8_t type, u8_t flag)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->ledStruct.LEDCtrlType = type;
+    wd->ledStruct.LEDCtrlFlagFromReg  = flag;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanEnableLeapConfig(zdev_t* dev, u8_t leapEnabled)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.leapEnabled = leapEnabled;
+}
+
+u32_t zfiWlanQueryHwCapability(zdev_t* dev)
+{
+    return zfHpCapability(dev);
+}
+
+u32_t zfiWlanQueryReceivedPacket(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.ReceivedPktRatePerSecond;
+}
+
+void zfiWlanCheckSWEncryption(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (wd->sta.SWEncryptEnable != 0)
+    {
+        zfHpSWDecrypt(dev, 1);
+    }
+}
+
+u16_t zfiWlanQueryAllowChannels(zdev_t* dev, u16_t *channels)
+{
+    u16_t ii;
+    zmw_get_wlan_dev(dev);
+
+    for (ii = 0; ii < wd->regulationTable.allowChannelCnt; ii++)
+    {
+        channels[ii] = wd->regulationTable.allowChannel[ii].channel;
+    }
+
+    return wd->regulationTable.allowChannelCnt;
+}
+
+void zfiWlanSetDynamicSIFSParam(zdev_t* dev, u8_t val)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->dynamicSIFSEnable = val;
+
+    zm_debug_msg1("wd->dynamicSIFSEnable = ", wd->dynamicSIFSEnable)
+}
+
+u16_t zfiWlanGetMulticastAddressCount(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.multicastList.size;
+}
+
+void zfiWlanGetMulticastList(zdev_t* dev, u8_t* pMCList)
+{
+    struct zsMulticastAddr* pMacList = (struct zsMulticastAddr*) pMCList;
+    u8_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    for ( i=0; i<wd->sta.multicastList.size; i++ )
+    {
+        zfMemoryCopy(pMacList[i].addr, wd->sta.multicastList.macAddr[i].addr, 6);
+    }
+}
+
+void zfiWlanSetPacketFilter(zdev_t* dev, u32_t PacketFilter)
+{
+    u8_t  bAllMulticast = 0;
+    u32_t oldFilter;
+
+    zmw_get_wlan_dev(dev);
+
+    oldFilter = wd->sta.osRxFilter;
+
+    wd->sta.osRxFilter = PacketFilter;
+
+    if ((oldFilter & ZM_PACKET_TYPE_ALL_MULTICAST) !=
+        (wd->sta.osRxFilter & ZM_PACKET_TYPE_ALL_MULTICAST))
+    {
+        if ( wd->sta.osRxFilter & ZM_PACKET_TYPE_ALL_MULTICAST )
+            bAllMulticast = 1;
+        zfHpSetMulticastList(dev, wd->sta.multicastList.size,
+                             (u8_t*)wd->sta.multicastList.macAddr, bAllMulticast);
+    }
+}
+
+u8_t zfiCompareWithMulticastListAddress(zdev_t* dev, u16_t* dstMacAddr)
+{
+    u8_t i;
+    u8_t bIsInMCListAddr = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    for ( i=0; i<wd->sta.multicastList.size; i++ )
+    {
+    	if ( zfwMemoryIsEqual((u8_t*)dstMacAddr, (u8_t*)wd->sta.multicastList.macAddr[i].addr, 6) )
+    	{
+            bIsInMCListAddr = 1;
+            break;
+    	}
+    }
+
+    return bIsInMCListAddr;
+}
+
+void zfiWlanSetSafeModeEnabled(zdev_t* dev, u8_t safeMode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.bSafeMode = safeMode;
+
+    if ( safeMode )
+    	zfStaEnableSWEncryption(dev, 1);
+    else
+        zfStaDisableSWEncryption(dev);
+}
+
+void zfiWlanSetIBSSAdditionalIELength(zdev_t* dev, u32_t ibssAdditionalIESize, u8_t* ibssAdditionalIE)
+{
+	zmw_get_wlan_dev(dev);
+
+	if ( ibssAdditionalIESize )
+    {
+	    wd->sta.ibssAdditionalIESize = ibssAdditionalIESize;
+        zfMemoryCopy(wd->sta.ibssAdditionalIE, ibssAdditionalIE, (u16_t)ibssAdditionalIESize);
+    }
+    else
+    	wd->sta.ibssAdditionalIESize = 0;
+}
diff --git a/drivers/staging/otus/80211core/cprecomp.h b/drivers/staging/otus/80211core/cprecomp.h
new file mode 100644
index 000000000000..1670bfc22587
--- /dev/null
+++ b/drivers/staging/otus/80211core/cprecomp.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _CPRECOMP_H
+#define _CPRECOMP_H
+
+#include "../oal_dt.h"
+#include "../oal_marc.h"
+#include "pub_zfi.h"
+#include "pub_zfw.h"
+#include "pub_usb.h"
+#include "wlan.h"
+#include "struct.h"
+#include "cfunc.h"
+#include "cagg.h"
+#include "cwm.h"
+#include "performance.h"
+#endif
+
diff --git a/drivers/staging/otus/80211core/cpsmgr.c b/drivers/staging/otus/80211core/cpsmgr.c
new file mode 100644
index 000000000000..cf73caca8e52
--- /dev/null
+++ b/drivers/staging/otus/80211core/cpsmgr.c
@@ -0,0 +1,731 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/**
+  *  The power saving manager is to save the power as much as possible.
+  *  Generally speaking, it controls:
+  *
+  *         - when to sleep
+  *         -
+  *
+  */
+#include "cprecomp.h"
+
+void zfPowerSavingMgrInit(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.powerSaveMode = ZM_STA_PS_NONE;
+    wd->sta.psMgr.state = ZM_PS_MSG_STATE_ACTIVE;
+    wd->sta.psMgr.isSleepAllowed = 0;
+    wd->sta.psMgr.maxSleepPeriods = 1;
+    wd->sta.psMgr.ticks = 0;
+    wd->sta.psMgr.sleepAllowedtick = 0;
+}
+
+static u16_t zfPowerSavingMgrHandlePsNone(zdev_t* dev, u8_t *isWakeUpRequired)
+{
+    u16_t ret = 0;
+    zmw_get_wlan_dev(dev);
+
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            *isWakeUpRequired = 0;
+            break;
+
+        case ZM_PS_MSG_STATE_T1:
+        case ZM_PS_MSG_STATE_T2:
+        case ZM_PS_MSG_STATE_SLEEP:
+        default:
+            *isWakeUpRequired = 1;
+zm_debug_msg0("zfPowerSavingMgrHandlePsNone: Wake up now\n");
+            if ( zfStaIsConnected(dev) )
+            {
+                zm_debug_msg0("zfPowerSavingMgrOnHandleT1 send Null data\n");
+                //zfSendNullData(dev, 0);
+                ret = 1;
+            }
+
+            wd->sta.psMgr.state = ZM_PS_MSG_STATE_ACTIVE;
+            break;
+    }
+    return ret;
+}
+
+static void zfPowerSavingMgrHandlePs(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            //zm_debug_msg0("zfPowerSavingMgrHandlePs: Prepare to sleep...\n");
+            //wd->sta.psMgr.state = ZM_PS_MSG_STATE_T1;
+            break;
+
+        case ZM_PS_MSG_STATE_T1:
+        case ZM_PS_MSG_STATE_T2:
+        case ZM_PS_MSG_STATE_SLEEP:
+        default:
+            break;
+    }
+}
+
+void zfPowerSavingMgrSetMode(zdev_t* dev, u8_t mode)
+{
+    u16_t sendNull = 0;
+    u8_t isWakeUpRequired = 0;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zm_debug_msg1("mode = ", mode);
+
+    if (mode > ZM_STA_PS_LIGHT)
+    {
+        zm_debug_msg0("return - wrong power save mode");
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+
+    #if 1
+    switch(mode)
+    {
+        case ZM_STA_PS_NONE:
+            sendNull = zfPowerSavingMgrHandlePsNone(dev, &isWakeUpRequired);
+            break;
+
+        case ZM_STA_PS_FAST:
+        case ZM_STA_PS_LIGHT:
+            wd->sta.psMgr.maxSleepPeriods = 1;
+            zfPowerSavingMgrHandlePs(dev);
+            break;
+
+        case ZM_STA_PS_MAX:
+            wd->sta.psMgr.maxSleepPeriods = ZM_PS_MAX_SLEEP_PERIODS;
+            zfPowerSavingMgrHandlePs(dev);
+            break;
+    }
+    #else
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            if ( mode != ZM_STA_PS_NONE )
+            {
+zm_debug_msg0("zfPowerSavingMgrSetMode: switch from ZM_PS_MSG_STATE_ACTIVE to ZM_PS_MSG_STATE_T1\n");
+                // Stall the TX & start to wait the pending TX to be completed
+                wd->sta.psMgr.state = ZM_PS_MSG_STATE_T1;
+            }
+            break;
+
+        case ZM_PS_MSG_STATE_SLEEP:
+            break;
+    }
+    #endif
+
+    wd->sta.powerSaveMode = mode;
+    zmw_leave_critical_section(dev);
+
+    if ( isWakeUpRequired )
+    {
+        zfHpPowerSaveSetState(dev, 0);
+        wd->sta.psMgr.tempWakeUp = 0;
+    }
+
+    if ( zfStaIsConnected(dev)
+         && (wd->wlanMode == ZM_MODE_INFRASTRUCTURE) )
+    {
+        switch(mode)
+        {
+            case ZM_STA_PS_NONE:
+                zfHpPowerSaveSetMode(dev, 0, 0, wd->beaconInterval);
+                break;
+
+            case ZM_STA_PS_FAST:
+            case ZM_STA_PS_MAX:
+            case ZM_STA_PS_LIGHT:
+                zfHpPowerSaveSetMode(dev, 0, 1, wd->beaconInterval);
+                break;
+
+            default:
+                zfHpPowerSaveSetMode(dev, 0, 0, wd->beaconInterval);
+                break;
+        }
+    }
+
+    if (sendNull == 1)
+    {
+        zfSendNullData(dev, 0);
+    }
+
+    return;
+}
+
+static void zfPowerSavingMgrNotifyPSToAP(zdev_t *dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if ( (wd->sta.psMgr.tempWakeUp != 1)&&
+         (wd->sta.psMgr.lastTxUnicastFrm != wd->commTally.txUnicastFrm ||
+          wd->sta.psMgr.lastTxBroadcastFrm != wd->commTally.txBroadcastFrm ||
+          wd->sta.psMgr.lastTxMulticastFrm != wd->commTally.txMulticastFrm) )
+    {
+        zmw_enter_critical_section(dev);
+        wd->sta.psMgr.lastTxUnicastFrm = wd->commTally.txUnicastFrm;
+        wd->sta.psMgr.lastTxBroadcastFrm = wd->commTally.txBroadcastFrm;
+        wd->sta.psMgr.lastTxMulticastFrm = wd->commTally.txMulticastFrm;
+        zmw_leave_critical_section(dev);
+
+        zfSendNullData(dev, 1);
+    }
+}
+
+static void zfPowerSavingMgrOnHandleT1(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    // If the tx Q is not empty...return
+    if ( zfIsVtxqEmpty(dev) == FALSE )
+    {
+        return;
+    }
+
+zm_debug_msg0("VtxQ is empty now...Check if HAL TXQ is empty\n");
+
+    // The the HAL TX Q is not empty...return
+    if ( zfHpGetFreeTxdCount(dev) != zfHpGetMaxTxdCount(dev) )
+    {
+        return;
+    }
+
+zm_debug_msg0("HAL TXQ is empty now...Could go to sleep...\n");
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->sta.powerSaveMode == ZM_STA_PS_LIGHT)
+    {
+        if (wd->sta.ReceivedPktRatePerSecond > 200)
+        {
+            zmw_leave_critical_section(dev);
+            return;
+        }
+
+        if ( zfStaIsConnected(dev)
+             && (wd->wlanMode == ZM_MODE_INFRASTRUCTURE) )
+        {
+            if (wd->sta.psMgr.sleepAllowedtick) {
+                wd->sta.psMgr.sleepAllowedtick--;
+                zmw_leave_critical_section(dev);
+                return;
+            }
+        }
+    }
+
+    wd->sta.psMgr.state = ZM_PS_MSG_STATE_T2;
+
+    zmw_leave_critical_section(dev);
+
+    // Send the Null pkt to AP to notify that I'm going to sleep
+    if ( zfStaIsConnected(dev) )
+    {
+zm_debug_msg0("zfPowerSavingMgrOnHandleT1 send Null data\n");
+        zfPowerSavingMgrNotifyPSToAP(dev);
+    }
+
+    // Stall the TX now
+    // zfTxEngineStop(dev);
+}
+
+static void zfPowerSavingMgrOnHandleT2(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    // Wait until the Null pkt is transmitted
+    if ( zfHpGetFreeTxdCount(dev) != zfHpGetMaxTxdCount(dev) )
+    {
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+    wd->sta.psMgr.state = ZM_PS_MSG_STATE_SLEEP;
+    wd->sta.psMgr.lastTxUnicastFrm = wd->commTally.txUnicastFrm;
+    wd->sta.psMgr.lastTxBroadcastFrm = wd->commTally.txBroadcastFrm;
+    wd->sta.psMgr.lastTxMulticastFrm = wd->commTally.txMulticastFrm;
+    zmw_leave_critical_section(dev);
+
+    // Let CHIP sleep now
+zm_debug_msg0("zfPowerSavingMgrOnHandleT2 zzzz....\n");
+    zfHpPowerSaveSetState(dev, 1);
+    wd->sta.psMgr.tempWakeUp = 0;
+}
+
+u8_t zfPowerSavingMgrIsSleeping(zdev_t *dev)
+{
+    u8_t isSleeping = FALSE;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if ( wd->sta.psMgr.state == ZM_PS_MSG_STATE_SLEEP ||
+         wd->sta.psMgr.state == ZM_PS_MSG_STATE_T2)
+    {
+        isSleeping = TRUE;
+    }
+    zmw_leave_critical_section(dev);
+    return isSleeping;
+}
+
+static u8_t zfPowerSavingMgrIsIdle(zdev_t *dev)
+{
+    u8_t isIdle = 0;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ( zfStaIsConnected(dev) && wd->sta.psMgr.isSleepAllowed == 0 )
+    {
+        goto done;
+    }
+
+    if ( wd->sta.bChannelScan )
+    {
+        goto done;
+    }
+
+    if ( zfStaIsConnecting(dev) )
+    {
+        goto done;
+    }
+
+    if (wd->sta.powerSaveMode == ZM_STA_PS_LIGHT)
+    {
+        if (wd->sta.ReceivedPktRatePerSecond > 200)
+        {
+            goto done;
+        }
+
+        if ( zfStaIsConnected(dev)
+             && (wd->wlanMode == ZM_MODE_INFRASTRUCTURE) )
+        {
+            if (wd->sta.psMgr.sleepAllowedtick) {
+                wd->sta.psMgr.sleepAllowedtick--;
+                goto done;
+            }
+        }
+    }
+
+    isIdle = 1;
+
+done:
+    zmw_leave_critical_section(dev);
+
+    if ( zfIsVtxqEmpty(dev) == FALSE )
+    {
+        isIdle = 0;
+    }
+
+    return isIdle;
+}
+
+static void zfPowerSavingMgrSleepIfIdle(zdev_t *dev)
+{
+    u8_t isIdle;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    isIdle = zfPowerSavingMgrIsIdle(dev);
+
+    if ( isIdle == 0 )
+    {
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+
+    switch(wd->sta.powerSaveMode)
+    {
+        case ZM_STA_PS_NONE:
+            break;
+
+        case ZM_STA_PS_MAX:
+        case ZM_STA_PS_FAST:
+        case ZM_STA_PS_LIGHT:
+            zm_debug_msg0("zfPowerSavingMgrSleepIfIdle: IDLE so slep now...\n");
+            wd->sta.psMgr.state = ZM_PS_MSG_STATE_T1;
+            break;
+    }
+
+    zmw_leave_critical_section(dev);
+}
+
+static void zfPowerSavingMgrDisconnectMain(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+#ifdef ZM_ENABLE_DISCONNECT_PS
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            zfPowerSavingMgrSleepIfIdle(dev);
+            break;
+
+        case ZM_PS_MSG_STATE_SLEEP:
+            break;
+
+        case ZM_PS_MSG_STATE_T1:
+            zfPowerSavingMgrOnHandleT1(dev);
+            break;
+
+        case ZM_PS_MSG_STATE_T2:
+            zfPowerSavingMgrOnHandleT2(dev);
+            break;
+    }
+#else
+    zfPowerSavingMgrWakeup(dev);
+#endif
+}
+
+static void zfPowerSavingMgrInfraMain(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            zfPowerSavingMgrSleepIfIdle(dev);
+            break;
+
+        case ZM_PS_MSG_STATE_SLEEP:
+            break;
+
+        case ZM_PS_MSG_STATE_T1:
+            zfPowerSavingMgrOnHandleT1(dev);
+            break;
+
+        case ZM_PS_MSG_STATE_T2:
+            zfPowerSavingMgrOnHandleT2(dev);
+            break;
+    }
+}
+
+void zfPowerSavingMgrAtimWinExpired(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+//printk("zfPowerSavingMgrAtimWinExpired #1\n");
+    if ( wd->sta.powerSaveMode == ZM_STA_PS_NONE )
+    {
+        return;
+    }
+
+//printk("zfPowerSavingMgrAtimWinExpired #2\n");
+    // if we received any ATIM window from the others to indicate we have buffered data
+    // at the other station, we can't go to sleep
+    if ( wd->sta.recvAtim )
+    {
+        wd->sta.recvAtim = 0;
+        zm_debug_msg0("Can't sleep due to receving ATIM window!");
+        return;
+    }
+
+    // if we are the one to tx beacon during last beacon interval. we can't go to sleep
+    // since we need to be alive to respond the probe request!
+    if ( wd->sta.txBeaconInd )
+    {
+        zm_debug_msg0("Can't sleep due to just transmit a beacon!");
+        return;
+    }
+
+    // If we buffer any data for the other stations. we could not go to sleep
+    if ( wd->sta.ibssPrevPSDataCount != 0 )
+    {
+        zm_debug_msg0("Can't sleep due to buffering data for the others!");
+        return;
+    }
+
+    // before sleeping, we still need to notify the others by transmitting null
+    // pkt with power mgmt bit turned on.
+    zfPowerSavingMgrOnHandleT1(dev);
+}
+
+static void zfPowerSavingMgrIBSSMain(zdev_t* dev)
+{
+    // wait for the end of
+    // if need to wait to know if we are the one to transmit the beacon
+    // during the beacon interval. If it's me, we can't go to sleep.
+
+    zmw_get_wlan_dev(dev);
+
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+        case ZM_PS_MSG_STATE_SLEEP:
+        case ZM_PS_MSG_STATE_T1:
+            break;
+
+        case ZM_PS_MSG_STATE_T2:
+            zfPowerSavingMgrOnHandleT2(dev);
+            break;
+    }
+
+    return;
+}
+
+#if 1
+void zfPowerSavingMgrMain(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch (wd->sta.adapterState)
+    {
+    case ZM_STA_STATE_DISCONNECT:
+        zfPowerSavingMgrDisconnectMain(dev);
+        break;
+    case ZM_STA_STATE_CONNECTED:
+        {
+            if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE) {
+                zfPowerSavingMgrInfraMain(dev);
+            } else if (wd->wlanMode == ZM_MODE_IBSS) {
+                zfPowerSavingMgrIBSSMain(dev);
+            }
+        }
+        break;
+    case ZM_STA_STATE_CONNECTING:
+    default:
+        break;
+    }
+}
+#else
+void zfPowerSavingMgrMain(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE )
+    {
+        return;
+    }
+
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            goto check_sleep;
+            break;
+
+        case ZM_PS_MSG_STATE_SLEEP:
+            goto sleeping;
+            break;
+
+        case ZM_PS_MSG_STATE_T1:
+            zfPowerSavingMgrOnHandleT1(dev);
+            break;
+
+        case ZM_PS_MSG_STATE_T2:
+            zfPowerSavingMgrOnHandleT2(dev);
+            break;
+    }
+
+    return;
+
+sleeping:
+    return;
+
+check_sleep:
+    zfPowerSavingMgrSleepIfIdle(dev);
+    return;
+}
+#endif
+
+#ifdef ZM_ENABLE_POWER_SAVE
+void zfPowerSavingMgrWakeup(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+//zm_debug_msg0("zfPowerSavingMgrWakeup");
+
+    //if ( wd->sta.psMgr.state != ZM_PS_MSG_STATE_ACTIVE && ( zfPowerSavingMgrIsIdle(dev) == 0 ))
+    if ( wd->sta.psMgr.state != ZM_PS_MSG_STATE_ACTIVE )
+    {
+        zmw_enter_critical_section(dev);
+
+        wd->sta.psMgr.isSleepAllowed = 0;
+        wd->sta.psMgr.state = ZM_PS_MSG_STATE_ACTIVE;
+
+        if ( wd->sta.powerSaveMode > ZM_STA_PS_NONE )
+            wd->sta.psMgr.tempWakeUp = 1;
+
+        zmw_leave_critical_section(dev);
+
+        // Wake up the CHIP now!!
+        zfHpPowerSaveSetState(dev, 0);
+    }
+}
+#else
+void zfPowerSavingMgrWakeup(zdev_t* dev)
+{
+}
+#endif
+
+void zfPowerSavingMgrProcessBeacon(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t   length, bitmap;
+    u16_t  offset, n1, n2, q, r;
+    zbuf_t* psBuf;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if ( wd->sta.powerSaveMode == ZM_STA_PS_NONE  )
+    //if ( wd->sta.psMgr.state != ZM_PS_MSG_STATE_SLEEP )
+    {
+        return;
+    }
+
+    wd->sta.psMgr.isSleepAllowed = 1;
+
+    if ( (offset=zfFindElement(dev, buf, ZM_WLAN_EID_TIM)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+
+        if ( length > 3 )
+        {
+            n1 = zmw_rx_buf_readb(dev, buf, offset+4) & (~ZM_BIT_0);
+            n2 = length + n1 - 4;
+            q = wd->sta.aid >> 3;
+            r = wd->sta.aid & 7;
+
+            if ((q >= n1) && (q <= n2))
+            {
+                bitmap = zmw_rx_buf_readb(dev, buf, offset+5+q-n1);
+
+                if ( (bitmap >> r) &  ZM_BIT_0 )
+                {
+                    //if ( wd->sta.powerSaveMode == ZM_STA_PS_FAST )
+                    if ( 0 )
+                    {
+                        wd->sta.psMgr.state = ZM_PS_MSG_STATE_S1;
+                        //zfSendPSPoll(dev);
+                        zfSendNullData(dev, 0);
+                    }
+                    else
+                    {
+                        if ((wd->sta.qosInfo&0xf) != 0xf)
+                        {
+                            /* send ps-poll */
+                            //printk("zfSendPSPoll #1\n");
+
+                            wd->sta.psMgr.isSleepAllowed = 0;
+
+                            switch (wd->sta.powerSaveMode)
+                            {
+                            case ZM_STA_PS_MAX:
+                            case ZM_STA_PS_FAST:
+                                //zm_debug_msg0("wake up and send PS-Poll\n");
+                                zfSendPSPoll(dev);
+                                break;
+                            case ZM_STA_PS_LIGHT:
+                                zm_debug_msg0("wake up and send null data\n");
+
+                                zmw_enter_critical_section(dev);
+                                wd->sta.psMgr.sleepAllowedtick = 400;
+                                zmw_leave_critical_section(dev);
+
+                                zfSendNullData(dev, 0);
+                                break;
+                            }
+
+                            wd->sta.psMgr.tempWakeUp = 0;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    while ((psBuf = zfQueueGet(dev, wd->sta.uapsdQ)) != NULL)
+    {
+        zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+    }
+
+    //printk("zfPowerSavingMgrProcessBeacon #1\n");
+    zfPowerSavingMgrMain(dev);
+}
+
+void zfPowerSavingMgrConnectNotify(zdev_t *dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        switch(wd->sta.powerSaveMode)
+        {
+            case ZM_STA_PS_NONE:
+                zfHpPowerSaveSetMode(dev, 0, 0, wd->beaconInterval);
+                break;
+
+            case ZM_STA_PS_FAST:
+            case ZM_STA_PS_MAX:
+            case ZM_STA_PS_LIGHT:
+                zfHpPowerSaveSetMode(dev, 0, 1, wd->beaconInterval);
+                break;
+
+            default:
+                zfHpPowerSaveSetMode(dev, 0, 0, wd->beaconInterval);
+                break;
+        }
+    }
+}
+
+void zfPowerSavingMgrPreTBTTInterrupt(zdev_t *dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* disable TBTT interrupt when change from connection to disconnect */
+    if (zfStaIsDisconnect(dev)) {
+        zfHpPowerSaveSetMode(dev, 0, 0, 0);
+        zfPowerSavingMgrWakeup(dev);
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+    wd->sta.psMgr.ticks++;
+
+    if ( wd->sta.psMgr.ticks < wd->sta.psMgr.maxSleepPeriods )
+    {
+        zmw_leave_critical_section(dev);
+        return;
+    }
+    else
+    {
+        wd->sta.psMgr.ticks = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zfPowerSavingMgrWakeup(dev);
+}
+
+/* Leave an empty line below to remove warning message on some compiler */
+
diff --git a/drivers/staging/otus/80211core/cscanmgr.c b/drivers/staging/otus/80211core/cscanmgr.c
new file mode 100644
index 000000000000..b32835c87590
--- /dev/null
+++ b/drivers/staging/otus/80211core/cscanmgr.c
@@ -0,0 +1,535 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+
+void zfScanMgrInit(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.scanMgr.scanReqs[0] = 0;
+    wd->sta.scanMgr.scanReqs[1] = 0;
+
+    wd->sta.scanMgr.currScanType = ZM_SCAN_MGR_SCAN_NONE;
+    wd->sta.scanMgr.scanStartDelay = 3;
+    //wd->sta.scanMgr.scanStartDelay = 0;
+}
+
+u8_t zfScanMgrScanStart(zdev_t* dev, u8_t scanType)
+{
+    u8_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg1("scanType = ", scanType);
+
+    zmw_declare_for_critical_section();
+
+    if ( scanType != ZM_SCAN_MGR_SCAN_INTERNAL &&
+         scanType != ZM_SCAN_MGR_SCAN_EXTERNAL )
+    {
+        zm_debug_msg0("unknown scanType");
+        return 1;
+    }
+    else if (zfStaIsConnecting(dev))
+    {
+        zm_debug_msg0("reject scan request due to connecting");
+        return 1;
+    }
+
+    i = scanType - 1;
+
+    zmw_enter_critical_section(dev);
+
+    if ( wd->sta.scanMgr.scanReqs[i] == 1 )
+    {
+        zm_debug_msg1("scan rescheduled", scanType);
+        goto scan_done;
+    }
+
+    wd->sta.scanMgr.scanReqs[i] = 1;
+    zm_debug_msg1("scan scheduled: ", scanType);
+
+    // If there's no scan pending, we do the scan right away.
+    // If there's an internal scan and the new scan request is external one,
+    // we will restart the scan.
+    if ( wd->sta.scanMgr.currScanType == ZM_SCAN_MGR_SCAN_NONE )
+    {
+        goto schedule_scan;
+    }
+    else if ( wd->sta.scanMgr.currScanType == ZM_SCAN_MGR_SCAN_INTERNAL &&
+              scanType == ZM_SCAN_MGR_SCAN_EXTERNAL )
+    {
+        // Stop the internal scan & schedule external scan first
+        zfTimerCancel(dev, ZM_EVENT_SCAN);
+
+        /* Fix for WHQL sendrecv => we do not apply delay time in which the device
+           stop transmitting packet when we already connect to some AP  */
+        wd->sta.bScheduleScan = FALSE;
+
+        zfTimerCancel(dev, ZM_EVENT_TIMEOUT_SCAN);
+        zfTimerCancel(dev, ZM_EVENT_IN_SCAN);
+
+        wd->sta.bChannelScan = FALSE;
+        goto schedule_scan;
+    }
+    else
+    {
+        zm_debug_msg0("Scan is busy...waiting later to start\n");
+    }
+
+    zmw_leave_critical_section(dev);
+    return 0;
+
+scan_done:
+    zmw_leave_critical_section(dev);
+    return 1;
+
+schedule_scan:
+
+    wd->sta.bScheduleScan = TRUE;
+
+    zfTimerSchedule(dev, ZM_EVENT_SCAN, wd->sta.scanMgr.scanStartDelay);
+    wd->sta.scanMgr.scanStartDelay = 3;
+    //wd->sta.scanMgr.scanStartDelay = 0;
+    wd->sta.scanMgr.currScanType = scanType;
+    zmw_leave_critical_section(dev);
+
+    if ((zfStaIsConnected(dev)) && (!zfPowerSavingMgrIsSleeping(dev)))
+    {
+        zfSendNullData(dev, 1);
+    }
+    return 0;
+}
+
+void zfScanMgrScanStop(zdev_t* dev, u8_t scanType)
+{
+    u8_t scanNotifyRequired = 0;
+    u8_t theOtherScan = ZM_SCAN_MGR_SCAN_NONE;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ( wd->sta.scanMgr.currScanType == ZM_SCAN_MGR_SCAN_NONE )
+    {
+        zm_assert(wd->sta.scanMgr.scanReqs[0] == 0);
+        zm_assert(wd->sta.scanMgr.scanReqs[1] == 0);
+        goto done;
+    }
+
+    switch(scanType)
+    {
+        case ZM_SCAN_MGR_SCAN_EXTERNAL:
+            scanNotifyRequired = 1;
+            theOtherScan = ZM_SCAN_MGR_SCAN_INTERNAL;
+            break;
+
+        case ZM_SCAN_MGR_SCAN_INTERNAL:
+            theOtherScan = ZM_SCAN_MGR_SCAN_EXTERNAL;
+            break;
+
+        default:
+            goto done;
+    }
+
+    if ( wd->sta.scanMgr.currScanType != scanType )
+    {
+        goto stop_done;
+    }
+
+    zfTimerCancel(dev, ZM_EVENT_SCAN);
+
+    /* Fix for WHQL sendrecv => we do not apply delay time in which the device
+       stop transmitting packet when we already connect to some AP  */
+    wd->sta.bScheduleScan = FALSE;
+
+    zfTimerCancel(dev, ZM_EVENT_TIMEOUT_SCAN);
+    zfTimerCancel(dev, ZM_EVENT_IN_SCAN);
+
+    wd->sta.bChannelScan = FALSE;
+    wd->sta.scanFrequency = 0;
+
+    if ( wd->sta.scanMgr.scanReqs[theOtherScan - 1] )
+    {
+        wd->sta.scanMgr.currScanType = theOtherScan;
+
+        // Schedule the other scan after 1 second later
+        zfTimerSchedule(dev, ZM_EVENT_SCAN, 100);
+    }
+    else
+    {
+        wd->sta.scanMgr.currScanType = ZM_SCAN_MGR_SCAN_NONE;
+    }
+
+stop_done:
+    wd->sta.scanMgr.scanReqs[scanType - 1] = 0;
+
+    zmw_leave_critical_section(dev);
+
+    /* avoid lose receive packet when site survey */
+    if ((zfStaIsConnected(dev)) && (!zfPowerSavingMgrIsSleeping(dev)))
+    {
+        zfSendNullData(dev, 0);
+    }
+
+    if ( scanNotifyRequired )
+    {
+        zm_debug_msg0("Scan notify after reset");
+        if (wd->zfcbScanNotify != NULL)
+        {
+            wd->zfcbScanNotify(dev, NULL);
+        }
+    }
+
+    return;
+
+done:
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+void zfScanMgrScanAck(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    wd->sta.scanMgr.scanStartDelay = 3;
+    //wd->sta.scanMgr.scanStartDelay = 0;
+
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+extern void zfStaReconnect(zdev_t* dev);
+
+static void zfScanSendProbeRequest(zdev_t* dev)
+{
+    u8_t k;
+    u16_t  dst[3] = { 0xffff, 0xffff, 0xffff };
+
+    zmw_get_wlan_dev(dev);
+
+    /* Increase rxBeaconCount to prevent beacon lost */
+    if (zfStaIsConnected(dev))
+    {
+        wd->sta.rxBeaconCount++;
+    }
+
+    if ( wd->sta.bPassiveScan )
+    {
+        return;
+    }
+    /* enable 802.l11h and in DFS Band , disable sending probe request */
+    if (wd->sta.DFSEnable)
+    {
+        if (zfHpIsDfsChannel(dev, wd->sta.scanFrequency))
+        {
+            return;
+        }
+    }
+
+    zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBEREQ, dst, 0, 0, 0);
+
+    if ( wd->sta.disableProbingWithSsid )
+    {
+        return;
+    }
+
+    for (k=1; k<=ZM_MAX_PROBE_HIDDEN_SSID_SIZE; k++)
+    {
+        if ( wd->ws.probingSsidList[k-1].ssidLen != 0 )
+        {
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBEREQ, dst, k, 0, 0);
+        }
+    }
+}
+
+static void zfScanMgrEventSetFreqCompleteCb(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+//printk("zfScanMgrEventSetFreqCompleteCb #1\n");
+
+    zmw_enter_critical_section(dev);
+    zfTimerSchedule(dev, ZM_EVENT_IN_SCAN, ZM_TICK_IN_SCAN);
+    if (wd->sta.bPassiveScan)
+    {
+        zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_SCAN, wd->sta.passiveScanTickPerChannel);
+    }
+    else
+    {
+        zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_SCAN, wd->sta.activescanTickPerChannel);
+    }
+    zmw_leave_critical_section(dev);
+
+    zfScanSendProbeRequest(dev);
+}
+
+
+static void zfScanMgrEventScanCompleteCb(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ((zfStaIsConnected(dev)) && (!zfPowerSavingMgrIsSleeping(dev)))
+    {
+        zfSendNullData(dev, 0);
+    }
+    return;
+}
+
+
+void zfScanMgrScanEventRetry(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( !wd->sta.bChannelScan )
+    {
+        return;
+    }
+
+    if ( !wd->sta.bPassiveScan )
+    {
+        zfScanSendProbeRequest(dev);
+        #if 0
+        zmw_enter_critical_section(dev);
+        zfTimerSchedule(dev, ZM_EVENT_IN_SCAN, ZM_TICK_IN_SCAN);
+        zmw_leave_critical_section(dev);
+        #endif
+    }
+}
+
+u8_t zfScanMgrScanEventTimeout(zdev_t* dev)
+{
+    u16_t   nextScanFrequency = 0;
+    u8_t    temp;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if ( wd->sta.scanFrequency == 0 )
+    {
+        zmw_leave_critical_section(dev);
+        return -1;
+    }
+
+    nextScanFrequency = zfChGetNextChannel(dev, wd->sta.scanFrequency,
+                                           &wd->sta.bPassiveScan);
+
+    if ( (nextScanFrequency == 0xffff)
+         || (wd->sta.scanFrequency == zfChGetLastChannel(dev, &temp)) )
+    {
+        u8_t currScanType;
+        u8_t isExternalScan = 0;
+        u8_t isInternalScan = 0;
+
+        //zm_debug_msg1("end scan = ", KeQueryInterruptTime());
+        wd->sta.scanFrequency = 0;
+
+        zm_debug_msg1("scan 1 type: ", wd->sta.scanMgr.currScanType);
+        zm_debug_msg1("scan channel count = ", wd->regulationTable.allowChannelCnt);
+
+        //zfBssInfoRefresh(dev);
+        zfTimerCancel(dev, ZM_EVENT_TIMEOUT_SCAN);
+
+        if ( wd->sta.bChannelScan == FALSE )
+        {
+            zm_debug_msg0("WOW!! scan is cancelled\n");
+            zmw_leave_critical_section(dev);
+            goto report_scan_result;
+        }
+
+
+        currScanType = wd->sta.scanMgr.currScanType;
+        switch(currScanType)
+        {
+            case ZM_SCAN_MGR_SCAN_EXTERNAL:
+                isExternalScan = 1;
+
+                if ( wd->sta.scanMgr.scanReqs[ZM_SCAN_MGR_SCAN_INTERNAL - 1] )
+                {
+                    wd->sta.scanMgr.scanReqs[ZM_SCAN_MGR_SCAN_INTERNAL - 1] = 0;
+                    isInternalScan = 1;
+                }
+
+                break;
+
+            case ZM_SCAN_MGR_SCAN_INTERNAL:
+                isInternalScan = 1;
+
+                if ( wd->sta.scanMgr.scanReqs[ZM_SCAN_MGR_SCAN_EXTERNAL - 1] )
+                {
+                    // Because the external scan should pre-empts internal scan.
+                    // So this shall not be happened!!
+                    zm_assert(0);
+                }
+
+                break;
+
+            default:
+                zm_assert(0);
+                break;
+        }
+
+        wd->sta.scanMgr.scanReqs[currScanType - 1] = 0;
+        wd->sta.scanMgr.scanStartDelay = 100;
+        wd->sta.scanMgr.currScanType = ZM_SCAN_MGR_SCAN_NONE;
+        zmw_leave_critical_section(dev);
+
+        //Set channel according to AP's configuration
+        zfCoreSetFrequencyEx(dev, wd->frequency, wd->BandWidth40,
+                wd->ExtOffset, zfScanMgrEventScanCompleteCb);
+
+        wd->sta.bChannelScan = FALSE;
+
+        #if 1
+        if (zfStaIsConnected(dev))
+        { // Finish site survey, reset the variable to detect using wrong frequency !
+            zfHpFinishSiteSurvey(dev, 1);
+            zmw_enter_critical_section(dev);
+            wd->sta.ibssSiteSurveyStatus = 2;
+            wd->tickIbssReceiveBeacon = 0;
+            wd->sta.ibssReceiveBeaconCount = 0;
+            zmw_leave_critical_section(dev);
+
+            /* #5 Re-enable RIFS function after the site survey ! */
+            /* This is because switch band will reset the BB register to initial value */
+            if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTED )
+            {
+                zfHpEnableRifs(dev, ((wd->sta.currentFrequency<3000)?1:0), wd->sta.EnableHT, wd->sta.HT2040);
+            }
+        }
+        else
+        {
+            zfHpFinishSiteSurvey(dev, 0);
+            zmw_enter_critical_section(dev);
+            wd->sta.ibssSiteSurveyStatus = 0;
+            zmw_leave_critical_section(dev);
+        }
+        #endif
+
+report_scan_result:
+        /* avoid lose receive packet when site survey */
+        //if ((zfStaIsConnected(dev)) && (!zfPowerSavingMgrIsSleeping(dev)))
+        //{
+        //    zfSendNullData(dev, 0);
+        //}
+
+        if ( isExternalScan )//Quickly reboot
+        {
+            if (wd->zfcbScanNotify != NULL)
+            {
+                wd->zfcbScanNotify(dev, NULL);
+            }
+        }
+
+        if ( isInternalScan )
+        {
+            //wd->sta.InternalScanReq = 0;
+            zfStaReconnect(dev);
+        }
+
+        return 0;
+    }
+    else
+    {
+        wd->sta.scanFrequency = nextScanFrequency;
+
+        //zmw_enter_critical_section(dev);
+        zfTimerCancel(dev, ZM_EVENT_IN_SCAN);
+        zmw_leave_critical_section(dev);
+
+        zm_debug_msg0("scan 2");
+        zfCoreSetFrequencyV2(dev, wd->sta.scanFrequency, zfScanMgrEventSetFreqCompleteCb);
+
+        return 1;
+    }
+}
+
+void zfScanMgrScanEventStart(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( wd->sta.bChannelScan )
+    {
+        return;
+    }
+
+    zfPowerSavingMgrWakeup(dev);
+
+    zmw_enter_critical_section(dev);
+
+    if ( wd->sta.scanMgr.currScanType == ZM_SCAN_MGR_SCAN_NONE )
+    {
+        goto no_scan;
+    }
+
+    //zfBssInfoRefresh(dev);
+    zfBssInfoRefresh(dev, 0);
+    wd->sta.bChannelScan = TRUE;
+    wd->sta.bScheduleScan = FALSE;
+    zfTimerCancel(dev, ZM_EVENT_IN_SCAN);
+    zfTimerCancel(dev, ZM_EVENT_TIMEOUT_SCAN);
+
+    //zm_debug_msg1("start scan = ", KeQueryInterruptTime());
+    wd->sta.scanFrequency = zfChGetFirstChannel(dev, &wd->sta.bPassiveScan);
+    zmw_leave_critical_section(dev);
+
+    /* avoid lose receive packet when site survey */
+    //if ((zfStaIsConnected(dev)) && (!zfPowerSavingMgrIsSleeping(dev)))
+    //{
+    //    zfSendNullData(dev, 1);
+    //}
+//    zm_debug_msg0("scan 0");
+//    zfCoreSetFrequencyV2(dev, wd->sta.scanFrequency, zfScanMgrEventSetFreqCompleteCb);
+
+    #if 1
+    if (zfStaIsConnected(dev))
+    {// If doing site survey !
+        zfHpBeginSiteSurvey(dev, 1);
+        zmw_enter_critical_section(dev);
+        wd->sta.ibssSiteSurveyStatus = 1;
+        zmw_leave_critical_section(dev);
+    }
+    else
+    {
+        zfHpBeginSiteSurvey(dev, 0);
+        zmw_enter_critical_section(dev);
+        wd->sta.ibssSiteSurveyStatus = 0;
+        zmw_leave_critical_section(dev);
+    }
+    #endif
+
+    zm_debug_msg0("scan 0");
+    zfCoreSetFrequencyV2(dev, wd->sta.scanFrequency, zfScanMgrEventSetFreqCompleteCb);
+
+    return;
+
+no_scan:
+    zmw_leave_critical_section(dev);
+    return;
+}
diff --git a/drivers/staging/otus/80211core/ctkip.c b/drivers/staging/otus/80211core/ctkip.c
new file mode 100644
index 000000000000..be42f7aaa37d
--- /dev/null
+++ b/drivers/staging/otus/80211core/ctkip.c
@@ -0,0 +1,598 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : ctkip.c                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains Tx and Rx functions.                       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+
+u16_t zgTkipSboxLower[256] =
+    {
+        0xA5,0x84,0x99,0x8D,0x0D,0xBD,0xB1,0x54,
+        0x50,0x03,0xA9,0x7D,0x19,0x62,0xE6,0x9A,
+        0x45,0x9D,0x40,0x87,0x15,0xEB,0xC9,0x0B,
+        0xEC,0x67,0xFD,0xEA,0xBF,0xF7,0x96,0x5B,
+        0xC2,0x1C,0xAE,0x6A,0x5A,0x41,0x02,0x4F,
+        0x5C,0xF4,0x34,0x08,0x93,0x73,0x53,0x3F,
+        0x0C,0x52,0x65,0x5E,0x28,0xA1,0x0F,0xB5,
+        0x09,0x36,0x9B,0x3D,0x26,0x69,0xCD,0x9F,
+        0x1B,0x9E,0x74,0x2E,0x2D,0xB2,0xEE,0xFB,
+        0xF6,0x4D,0x61,0xCE,0x7B,0x3E,0x71,0x97,
+        0xF5,0x68,0x00,0x2C,0x60,0x1F,0xC8,0xED,
+        0xBE,0x46,0xD9,0x4B,0xDE,0xD4,0xE8,0x4A,
+        0x6B,0x2A,0xE5,0x16,0xC5,0xD7,0x55,0x94,
+        0xCF,0x10,0x06,0x81,0xF0,0x44,0xBA,0xE3,
+        0xF3,0xFE,0xC0,0x8A,0xAD,0xBC,0x48,0x04,
+        0xDF,0xC1,0x75,0x63,0x30,0x1A,0x0E,0x6D,
+        0x4C,0x14,0x35,0x2F,0xE1,0xA2,0xCC,0x39,
+        0x57,0xF2,0x82,0x47,0xAC,0xE7,0x2B,0x95,
+        0xA0,0x98,0xD1,0x7F,0x66,0x7E,0xAB,0x83,
+        0xCA,0x29,0xD3,0x3C,0x79,0xE2,0x1D,0x76,
+        0x3B,0x56,0x4E,0x1E,0xDB,0x0A,0x6C,0xE4,
+        0x5D,0x6E,0xEF,0xA6,0xA8,0xA4,0x37,0x8B,
+        0x32,0x43,0x59,0xB7,0x8C,0x64,0xD2,0xE0,
+        0xB4,0xFA,0x07,0x25,0xAF,0x8E,0xE9,0x18,
+        0xD5,0x88,0x6F,0x72,0x24,0xF1,0xC7,0x51,
+        0x23,0x7C,0x9C,0x21,0xDD,0xDC,0x86,0x85,
+        0x90,0x42,0xC4,0xAA,0xD8,0x05,0x01,0x12,
+        0xA3,0x5F,0xF9,0xD0,0x91,0x58,0x27,0xB9,
+        0x38,0x13,0xB3,0x33,0xBB,0x70,0x89,0xA7,
+        0xB6,0x22,0x92,0x20,0x49,0xFF,0x78,0x7A,
+        0x8F,0xF8,0x80,0x17,0xDA,0x31,0xC6,0xB8,
+        0xC3,0xB0,0x77,0x11,0xCB,0xFC,0xD6,0x3A
+    };
+
+
+u16_t zgTkipSboxUpper[256] =
+    {
+        0xC6,0xF8,0xEE,0xF6,0xFF,0xD6,0xDE,0x91,
+        0x60,0x02,0xCE,0x56,0xE7,0xB5,0x4D,0xEC,
+        0x8F,0x1F,0x89,0xFA,0xEF,0xB2,0x8E,0xFB,
+        0x41,0xB3,0x5F,0x45,0x23,0x53,0xE4,0x9B,
+        0x75,0xE1,0x3D,0x4C,0x6C,0x7E,0xF5,0x83,
+        0x68,0x51,0xD1,0xF9,0xE2,0xAB,0x62,0x2A,
+        0x08,0x95,0x46,0x9D,0x30,0x37,0x0A,0x2F,
+        0x0E,0x24,0x1B,0xDF,0xCD,0x4E,0x7F,0xEA,
+        0x12,0x1D,0x58,0x34,0x36,0xDC,0xB4,0x5B,
+        0xA4,0x76,0xB7,0x7D,0x52,0xDD,0x5E,0x13,
+        0xA6,0xB9,0x00,0xC1,0x40,0xE3,0x79,0xB6,
+        0xD4,0x8D,0x67,0x72,0x94,0x98,0xB0,0x85,
+        0xBB,0xC5,0x4F,0xED,0x86,0x9A,0x66,0x11,
+        0x8A,0xE9,0x04,0xFE,0xA0,0x78,0x25,0x4B,
+        0xA2,0x5D,0x80,0x05,0x3F,0x21,0x70,0xF1,
+        0x63,0x77,0xAF,0x42,0x20,0xE5,0xFD,0xBF,
+        0x81,0x18,0x26,0xC3,0xBE,0x35,0x88,0x2E,
+        0x93,0x55,0xFC,0x7A,0xC8,0xBA,0x32,0xE6,
+        0xC0,0x19,0x9E,0xA3,0x44,0x54,0x3B,0x0B,
+        0x8C,0xC7,0x6B,0x28,0xA7,0xBC,0x16,0xAD,
+        0xDB,0x64,0x74,0x14,0x92,0x0C,0x48,0xB8,
+        0x9F,0xBD,0x43,0xC4,0x39,0x31,0xD3,0xF2,
+        0xD5,0x8B,0x6E,0xDA,0x01,0xB1,0x9C,0x49,
+        0xD8,0xAC,0xF3,0xCF,0xCA,0xF4,0x47,0x10,
+        0x6F,0xF0,0x4A,0x5C,0x38,0x57,0x73,0x97,
+        0xCB,0xA1,0xE8,0x3E,0x96,0x61,0x0D,0x0F,
+        0xE0,0x7C,0x71,0xCC,0x90,0x06,0xF7,0x1C,
+        0xC2,0x6A,0xAE,0x69,0x17,0x99,0x3A,0x27,
+        0xD9,0xEB,0x2B,0x22,0xD2,0xA9,0x07,0x33,
+        0x2D,0x3C,0x15,0xC9,0x87,0xAA,0x50,0xA5,
+        0x03,0x59,0x09,0x1A,0x65,0xD7,0x84,0xD0,
+        0x82,0x29,0x5A,0x1E,0x7B,0xA8,0x6D,0x2C
+    };
+
+u16_t zfrotr1(u16_t a)
+// rotate right by 1 bit.
+{
+    u16_t b;
+
+    if (a & 0x01)
+    {
+        b = (a >> 1) | 0x8000;
+    }
+    else
+    {
+        b = (a >> 1) & 0x7fff;
+    }
+    return b;
+}
+
+/*************************************************************/
+/* zfTkipSbox()                                              */
+/* Returns a 16 bit value from a 64K entry table. The Table  */
+/* is synthesized from two 256 entry byte wide tables.       */
+/*************************************************************/
+u16_t zfTkipSbox(u16_t index)
+{
+    u16_t   low;
+    u16_t   high;
+    u16_t   left, right;
+
+    low = (index & 0xFF);
+    high = ((index >> 8) & 0xFF);
+
+    left = zgTkipSboxLower[low] + (zgTkipSboxUpper[low] << 8 );
+    right = zgTkipSboxUpper[high] + (zgTkipSboxLower[high] << 8 );
+
+    return (left ^ right);
+}
+
+u8_t zfTkipPhase1KeyMix(u32_t iv32, struct zsTkipSeed* pSeed)
+{
+    u16_t   tsc0;
+    u16_t   tsc1;
+    u16_t   i, j;
+#if 0
+    /* Need not proceed this function with the same iv32 */
+    if ( iv32 == pSeed->iv32 )
+    {
+        return 1;
+    }
+#endif
+    tsc0 = (u16_t) ((iv32 >> 16) & 0xffff); /* msb */
+    tsc1 = (u16_t) (iv32 & 0xffff);
+
+    /* Phase 1, step 1 */
+    pSeed->ttak[0] = tsc1;
+    pSeed->ttak[1] = tsc0;
+    pSeed->ttak[2] = (u16_t) (pSeed->ta[0] + (pSeed->ta[1] <<8));
+    pSeed->ttak[3] = (u16_t) (pSeed->ta[2] + (pSeed->ta[3] <<8));
+    pSeed->ttak[4] = (u16_t) (pSeed->ta[4] + (pSeed->ta[5] <<8));
+
+    /* Phase 1, step 2 */
+    for (i=0; i<8; i++)
+    {
+        j = 2*(i & 1);
+        pSeed->ttak[0] =(pSeed->ttak[0] + zfTkipSbox(pSeed->ttak[4]
+                         ^ ZM_BYTE_TO_WORD(pSeed->tk[1+j], pSeed->tk[j])))
+                        & 0xffff;
+        pSeed->ttak[1] =(pSeed->ttak[1] + zfTkipSbox(pSeed->ttak[0]
+                         ^ ZM_BYTE_TO_WORD(pSeed->tk[5+j], pSeed->tk[4+j] )))
+                        & 0xffff;
+        pSeed->ttak[2] =(pSeed->ttak[2] + zfTkipSbox(pSeed->ttak[1]
+                         ^ ZM_BYTE_TO_WORD(pSeed->tk[9+j], pSeed->tk[8+j] )))
+                        & 0xffff;
+        pSeed->ttak[3] =(pSeed->ttak[3] + zfTkipSbox(pSeed->ttak[2]
+                         ^ ZM_BYTE_TO_WORD(pSeed->tk[13+j], pSeed->tk[12+j])))
+                        & 0xffff;
+        pSeed->ttak[4] =(pSeed->ttak[4] + zfTkipSbox(pSeed->ttak[3]
+                         ^ ZM_BYTE_TO_WORD(pSeed->tk[1+j], pSeed->tk[j]  )))
+                        & 0xffff;
+        pSeed->ttak[4] =(pSeed->ttak[4] + i) & 0xffff;
+    }
+
+    if ( iv32 == (pSeed->iv32+1) )
+    {
+        pSeed->iv32tmp = iv32;
+        return 1;
+    }
+
+    return 0;
+}
+
+u8_t zfTkipPhase2KeyMix(u16_t iv16, struct zsTkipSeed* pSeed)
+{
+    u16_t tsc2;
+
+    tsc2 = iv16;
+
+    /* Phase 2, Step 1 */
+    pSeed->ppk[0] = pSeed->ttak[0];
+    pSeed->ppk[1] = pSeed->ttak[1];
+    pSeed->ppk[2] = pSeed->ttak[2];
+    pSeed->ppk[3] = pSeed->ttak[3];
+    pSeed->ppk[4] = pSeed->ttak[4];
+    pSeed->ppk[5] = (pSeed->ttak[4] + tsc2) & 0xffff;
+
+    /* Phase2, Step 2 */
+    pSeed->ppk[0] = pSeed->ppk[0]
+                + zfTkipSbox(pSeed->ppk[5] ^ ZM_BYTE_TO_WORD(pSeed->tk[1],pSeed->tk[0]));
+    pSeed->ppk[1] = pSeed->ppk[1]
+                + zfTkipSbox(pSeed->ppk[0] ^ ZM_BYTE_TO_WORD(pSeed->tk[3],pSeed->tk[2]));
+    pSeed->ppk[2] = pSeed->ppk[2]
+                + zfTkipSbox(pSeed->ppk[1] ^ ZM_BYTE_TO_WORD(pSeed->tk[5],pSeed->tk[4]));
+    pSeed->ppk[3] = pSeed->ppk[3]
+                + zfTkipSbox(pSeed->ppk[2] ^ ZM_BYTE_TO_WORD(pSeed->tk[7],pSeed->tk[6]));
+    pSeed->ppk[4] = pSeed->ppk[4]
+                + zfTkipSbox(pSeed->ppk[3] ^ ZM_BYTE_TO_WORD(pSeed->tk[9],pSeed->tk[8]));
+    pSeed->ppk[5] = pSeed->ppk[5]
+                + zfTkipSbox(pSeed->ppk[4] ^ ZM_BYTE_TO_WORD(pSeed->tk[11],pSeed->tk[10]));
+
+    pSeed->ppk[0] = pSeed->ppk[0]
+                + zfrotr1(pSeed->ppk[5] ^ ZM_BYTE_TO_WORD(pSeed->tk[13],pSeed->tk[12]));
+    pSeed->ppk[1] = pSeed->ppk[1]
+                + zfrotr1(pSeed->ppk[0] ^ ZM_BYTE_TO_WORD(pSeed->tk[15],pSeed->tk[14]));
+    pSeed->ppk[2] = pSeed->ppk[2] + zfrotr1(pSeed->ppk[1]);
+    pSeed->ppk[3] = pSeed->ppk[3] + zfrotr1(pSeed->ppk[2]);
+    pSeed->ppk[4] = pSeed->ppk[4] + zfrotr1(pSeed->ppk[3]);
+    pSeed->ppk[5] = pSeed->ppk[5] + zfrotr1(pSeed->ppk[4]);
+
+    if (iv16 == 0)
+    {
+        if (pSeed->iv16 == 0xffff)
+        {
+            pSeed->iv16tmp=0;
+            return 1;
+        }
+        else
+            return 0;
+    }
+    else if (iv16 == (pSeed->iv16+1))
+    {
+        pSeed->iv16tmp = iv16;
+        return 1;
+    }
+    else
+        return 0;
+}
+
+void zfTkipInit(u8_t* key, u8_t* ta, struct zsTkipSeed* pSeed, u8_t* initIv)
+{
+    u16_t  iv16;
+    u32_t  iv32;
+    u16_t  i;
+
+    /* clear memory */
+    zfZeroMemory((u8_t*) pSeed, sizeof(struct zsTkipSeed));
+    /* set key to seed */
+    zfMemoryCopy(pSeed->ta, ta, 6);
+    zfMemoryCopy(pSeed->tk, key, 16);
+
+    iv16 = *initIv++;
+    iv16 += *initIv<<8;
+    initIv++;
+
+    iv32=0;
+
+    for(i=0; i<4; i++)      // initiv is little endian
+    {
+        iv32 += *initIv<<(i*8);
+        *initIv++;
+    }
+
+    pSeed->iv32 = iv32+1; // Force Recalculating on Tkip Phase1
+    zfTkipPhase1KeyMix(iv32, pSeed);
+
+    pSeed->iv16 = iv16;
+    pSeed->iv32 = iv32;
+}
+
+u32_t zfGetU32t(u8_t* p)
+{
+    u32_t res=0;
+    u16_t i;
+
+    for( i=0; i<4; i++ )
+    {
+        res |= (*p++) << (8*i);
+    }
+
+    return res;
+
+}
+
+void zfPutU32t(u8_t* p, u32_t value)
+{
+    u16_t i;
+
+    for(i=0; i<4; i++)
+    {
+        *p++ = (u8_t) (value & 0xff);
+        value >>= 8;
+    }
+}
+
+void zfMicClear(struct zsMicVar* pMic)
+{
+    pMic->left = pMic->k0;
+    pMic->right = pMic->k1;
+    pMic->nBytes = 0;
+    pMic->m = 0;
+}
+
+void zfMicSetKey(u8_t* key, struct zsMicVar* pMic)
+{
+    pMic->k0 = zfGetU32t(key);
+    pMic->k1 = zfGetU32t(key+4);
+    zfMicClear(pMic);
+}
+
+void zfMicAppendByte(u8_t b, struct zsMicVar* pMic)
+{
+    // Append the byte to our word-sized buffer
+    pMic->m |= b << (8* pMic->nBytes);
+    pMic->nBytes++;
+
+    // Process the word if it is full.
+    if ( pMic->nBytes >= 4 )
+    {
+        pMic->left ^= pMic->m;
+        pMic->right ^= ZM_ROL32(pMic->left, 17 );
+        pMic->left += pMic->right;
+        pMic->right ^= ((pMic->left & 0xff00ff00) >> 8) |
+                       ((pMic->left & 0x00ff00ff) << 8);
+        pMic->left += pMic->right;
+        pMic->right ^= ZM_ROL32( pMic->left, 3 );
+        pMic->left += pMic->right;
+        pMic->right ^= ZM_ROR32( pMic->left, 2 );
+        pMic->left += pMic->right;
+        // Clear the buffer
+        pMic->m = 0;
+        pMic->nBytes = 0;
+    }
+}
+
+void zfMicGetMic(u8_t* dst, struct zsMicVar* pMic)
+{
+    // Append the minimum padding
+    zfMicAppendByte(0x5a, pMic);
+    zfMicAppendByte(0, pMic);
+    zfMicAppendByte(0, pMic);
+    zfMicAppendByte(0, pMic);
+    zfMicAppendByte(0, pMic);
+
+    // and then zeroes until the length is a multiple of 4
+    while( pMic->nBytes != 0 )
+    {
+        zfMicAppendByte(0, pMic);
+    }
+
+    // The appendByte function has already computed the result.
+    zfPutU32t(dst, pMic->left);
+    zfPutU32t(dst+4, pMic->right);
+
+    // Reset to the empty message.
+    zfMicClear(pMic);
+
+}
+
+u8_t zfMicRxVerify(zdev_t* dev, zbuf_t* buf)
+{
+    struct zsMicVar*  pMicKey;
+    struct zsMicVar    MyMicKey;
+    u8_t   mic[8];
+    u8_t   da[6];
+    u8_t   sa[6];
+    u8_t   bValue;
+    u16_t  i, payloadOffset, tailOffset;
+
+    zmw_get_wlan_dev(dev);
+
+    /* need not check MIC if pMicKEy is equal to NULL */
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        pMicKey = zfApGetRxMicKey(dev, buf);
+
+        if ( pMicKey != NULL )
+        {
+            zfCopyFromRxBuffer(dev, buf, sa, ZM_WLAN_HEADER_A2_OFFSET, 6);
+            zfCopyFromRxBuffer(dev, buf, da, ZM_WLAN_HEADER_A3_OFFSET, 6);
+        }
+        else
+        {
+            return ZM_MIC_SUCCESS;
+        }
+    }
+    else if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        pMicKey = zfStaGetRxMicKey(dev, buf);
+
+        if ( pMicKey != NULL )
+        {
+            zfCopyFromRxBuffer(dev, buf, sa, ZM_WLAN_HEADER_A3_OFFSET, 6);
+            zfCopyFromRxBuffer(dev, buf, da, ZM_WLAN_HEADER_A1_OFFSET, 6);
+        }
+        else
+        {
+            return ZM_MIC_SUCCESS;
+        }
+    }
+    else
+    {
+        return ZM_MIC_SUCCESS;
+    }
+
+    MyMicKey.k0=pMicKey->k0;
+    MyMicKey.k1=pMicKey->k1;
+    pMicKey = &MyMicKey;
+
+    zfMicClear(pMicKey);
+    tailOffset = zfwBufGetSize(dev, buf);
+    tailOffset -= 8;
+
+    /* append DA */
+    for(i=0; i<6; i++)
+    {
+        zfMicAppendByte(da[i], pMicKey);
+    }
+    /* append SA */
+    for(i=0; i<6; i++)
+    {
+        zfMicAppendByte(sa[i], pMicKey);
+    }
+
+    /* append for alignment */
+    if ((zmw_rx_buf_readb(dev, buf, 0) & 0x80) != 0)
+        zfMicAppendByte(zmw_rx_buf_readb(dev, buf,24)&0x7, pMicKey);
+    else
+        zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+
+    /* append payload */
+    payloadOffset = ZM_SIZE_OF_WLAN_DATA_HEADER +
+                    ZM_SIZE_OF_IV +
+                    ZM_SIZE_OF_EXT_IV;
+
+    if ((zmw_rx_buf_readb(dev, buf, 0) & 0x80) != 0)
+    {
+        /* Qos Packet, Plcpheader + 2 */
+        if (wd->wlanMode == ZM_MODE_AP)
+        {
+            /* TODO : Rx Qos element offset in software MIC check */
+        }
+        else if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+        {
+            if (wd->sta.wmeConnected != 0)
+            {
+                payloadOffset += 2;
+            }
+        }
+    }
+
+    for(i=payloadOffset; i<tailOffset; i++)
+    {
+        bValue = zmw_rx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+
+    zfMicGetMic(mic, pMicKey);
+
+    if ( !zfRxBufferEqualToStr(dev, buf, mic, tailOffset, 8) )
+    {
+        return ZM_MIC_FAILURE;
+    }
+
+    return ZM_MIC_SUCCESS;
+}
+
+void zfTkipGetseeds(u16_t iv16, u8_t *RC4Key, struct zsTkipSeed *Seed)
+{
+    RC4Key[0]  = ZM_HI8(iv16);
+    RC4Key[1]  = (ZM_HI8(iv16) | 0x20) & 0x7f;
+    RC4Key[2]  = ZM_LO8(iv16);
+    RC4Key[3]  = ((Seed->ppk[5] ^ ZM_BYTE_TO_WORD(Seed->tk[1],Seed->tk[0]))>>1) & 0xff;
+    RC4Key[4]  = Seed->ppk[0] & 0xff;
+    RC4Key[5]  = Seed->ppk[0] >> 8;
+    RC4Key[6]  = Seed->ppk[1] & 0xff;
+    RC4Key[7]  = Seed->ppk[1] >> 8;
+    RC4Key[8]  = Seed->ppk[2] & 0xff;
+    RC4Key[9]  = Seed->ppk[2] >> 8;
+    RC4Key[10] = Seed->ppk[3] & 0xff;
+    RC4Key[11] = Seed->ppk[3] >> 8;
+    RC4Key[12] = Seed->ppk[4] & 0xff;
+    RC4Key[13] = Seed->ppk[4] >> 8;
+    RC4Key[14] = Seed->ppk[5] & 0xff;
+    RC4Key[15] = Seed->ppk[5] >> 8;
+}
+
+void zfCalTxMic(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u16_t *da, u16_t *sa, u8_t up, u8_t *mic)
+{
+    struct zsMicVar*  pMicKey;
+    u16_t  i;
+    u16_t len;
+    u8_t bValue;
+    u8_t qosType;
+    u8_t *pDa = (u8_t *)da;
+    u8_t *pSa = (u8_t *)sa;
+
+    zmw_get_wlan_dev(dev);
+
+    /* need not check MIC if pMicKEy is equal to NULL */
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        pMicKey = zfApGetTxMicKey(dev, buf, &qosType);
+
+        if ( pMicKey == NULL )
+            return;
+    }
+    else if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        pMicKey = zfStaGetTxMicKey(dev, buf);
+
+        if ( pMicKey == NULL )
+        {
+            zm_debug_msg0("pMicKey is NULL");
+            return;
+        }
+    }
+    else
+    {
+        return;
+    }
+
+    zfMicClear(pMicKey);
+    len = zfwBufGetSize(dev, buf);
+
+    /* append DA */
+    for(i = 0; i < 6; i++)
+    {
+        zfMicAppendByte(pDa[i], pMicKey);
+    }
+
+    /* append SA */
+    for(i = 0; i < 6; i++)
+    {
+        zfMicAppendByte(pSa[i], pMicKey);
+    }
+
+    if (up != 0)
+        zfMicAppendByte((up&0x7), pMicKey);
+    else
+        zfMicAppendByte(0, pMicKey);
+
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+
+    /* For Snap header */
+    for(i = 0; i < snapLen; i++)
+    {
+        zfMicAppendByte(snap[i], pMicKey);
+    }
+
+    for(i = offset; i < len; i++)
+    {
+        bValue = zmw_tx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+
+    zfMicGetMic(mic, pMicKey);
+}
+
+void zfTKIPEncrypt(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u8_t keyLen, u8_t* key, u32_t* icv)
+{
+    u8_t iv[3];
+
+    iv[0] = key[0];
+    iv[1] = key[1];
+    iv[2] = key[2];
+
+    keyLen -= 3;
+
+    zfWEPEncrypt(dev, buf, snap, snapLen, offset, keyLen, &key[3], iv);
+}
+
+u16_t zfTKIPDecrypt(zdev_t *dev, zbuf_t *buf, u16_t offset, u8_t keyLen, u8_t* key)
+{
+    u16_t ret = ZM_ICV_SUCCESS;
+    u8_t iv[3];
+
+    iv[0] = key[0];
+    iv[1] = key[1];
+    iv[2] = key[2];
+
+    keyLen -= 3;
+
+    ret = zfWEPDecrypt(dev, buf, offset, keyLen, &key[3], iv);
+
+    return ret;
+}
diff --git a/drivers/staging/otus/80211core/ctxrx.c b/drivers/staging/otus/80211core/ctxrx.c
new file mode 100644
index 000000000000..e258a7df5363
--- /dev/null
+++ b/drivers/staging/otus/80211core/ctxrx.c
@@ -0,0 +1,4096 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : htr.c                                                 */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains Tx and Rx functions.                       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+
+u16_t zfWlanRxValidate(zdev_t* dev, zbuf_t* buf);
+u16_t zfWlanRxFilter(zdev_t* dev, zbuf_t* buf);
+
+
+
+const u8_t zgSnapBridgeTunnel[6] = { 0xAA, 0xAA, 0x03, 0x00, 0x00, 0xF8 };
+const u8_t zgSnap8021h[6] = { 0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00 };
+/* Table for converting IP DSCP P2-P0 bits to 802.11e Access Category */
+const u8_t zcUpToAc[8] = {0, 1, 1, 0, 2, 2, 3, 3}; //WMM default
+//const u8_t zcUpToAc[8] = {0, 1, 1, 0, 0, 0, 0, 0}; //For 2 TxQ
+//const u8_t zcUpToAc[8] = {0, 0, 0, 0, 0, 0, 0, 0}; //For single TxQ
+const u8_t zcMaxspToPktNum[4] = {8, 2, 4, 6};
+
+u8_t zfGetEncryModeFromRxStatus(struct zsAdditionInfo* addInfo)
+{
+    u8_t securityByte;
+    u8_t encryMode;
+
+    securityByte = (addInfo->Tail.Data.SAIndex & 0xc0) >> 4;  /* byte4 */
+    securityByte |= (addInfo->Tail.Data.DAIndex & 0xc0) >> 6; /* byte5 */
+
+    switch( securityByte )
+    {
+        case ZM_NO_WEP:
+        case ZM_WEP64:
+        case ZM_WEP128:
+        case ZM_WEP256:
+#ifdef ZM_ENABLE_CENC
+        case ZM_CENC:
+#endif //ZM_ENABLE_CENC
+        case ZM_TKIP:
+        case ZM_AES:
+
+            encryMode = securityByte;
+            break;
+
+        default:
+
+            if ( (securityByte & 0xf8) == 0x08 )
+            {
+                // decrypted by software
+            }
+
+            encryMode = ZM_NO_WEP;
+            break;
+    }
+
+    return encryMode;
+}
+
+void zfGetRxIvIcvLength(zdev_t* dev, zbuf_t* buf, u8_t vap, u16_t* pIvLen,
+                        u16_t* pIcvLen, struct zsAdditionInfo* addInfo)
+{
+    u16_t wdsPort;
+    u8_t  encryMode;
+
+    zmw_get_wlan_dev(dev);
+
+    *pIvLen = 0;
+    *pIcvLen = 0;
+
+    encryMode = zfGetEncryModeFromRxStatus(addInfo);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        if (vap < ZM_MAX_AP_SUPPORT)
+        {
+            if (( wd->ap.encryMode[vap] == ZM_WEP64 ) ||
+                    ( wd->ap.encryMode[vap] == ZM_WEP128 ) ||
+                    ( wd->ap.encryMode[vap] == ZM_WEP256 ))
+            {
+                *pIvLen = 4;
+                *pIcvLen = 4;
+            }
+            else
+            {
+                u16_t id;
+                u16_t addr[3];
+
+                addr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+                addr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+2);
+                addr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+4);
+
+                /* Find STA's information */
+                if ((id = zfApFindSta(dev, addr)) != 0xffff)
+                {
+                    if (wd->ap.staTable[id].encryMode == ZM_TKIP)
+                    {
+                        *pIvLen = 8;
+                        *pIcvLen = 4;
+                    }
+                    else if (wd->ap.staTable[id].encryMode == ZM_AES)
+                    {
+                        *pIvLen = 8;
+                        *pIcvLen = 8; // AES MIC
+                        //*pIcvLen = 0;
+                    }
+#ifdef ZM_ENABLE_CENC
+                    else if (wd->ap.staTable[id].encryMode == ZM_CENC)
+                    {
+                        *pIvLen = 18;
+                        *pIcvLen= 16;
+                    }
+#endif //ZM_ENABLE_CENC
+                }
+            }
+            /* WDS port checking */
+            if ((wdsPort = vap - 0x20) >= ZM_MAX_WDS_SUPPORT)
+            {
+                wdsPort = 0;
+            }
+
+            switch (wd->ap.wds.encryMode[wdsPort])
+			{
+			case ZM_WEP64:
+			case ZM_WEP128:
+			case ZM_WEP256:
+                *pIvLen = 4;
+                *pIcvLen = 4;
+				break;
+			case ZM_TKIP:
+                *pIvLen = 8;
+                *pIcvLen = 4;
+				break;
+			case ZM_AES:
+                *pIvLen = 8;
+                *pIcvLen = 0;
+				break;
+#ifdef ZM_ENABLE_CENC
+            case ZM_CENC:
+                *pIvLen = 18;
+                *pIcvLen = 16;
+				break;
+#endif //ZM_ENABLE_CENC
+			}/* end of switch */
+        }
+    }
+	else if ( wd->wlanMode == ZM_MODE_PSEUDO)
+    {
+        /* test: 6518 for QA auto test */
+        switch (encryMode)
+		{
+        case ZM_WEP64:
+        case ZM_WEP128:
+        case ZM_WEP256:
+            *pIvLen = 4;
+            *pIcvLen = 4;
+			break;
+		case ZM_TKIP:
+            *pIvLen = 8;
+            *pIcvLen = 4;
+			break;
+		case ZM_AES:
+            *pIvLen = 8;
+            *pIcvLen = 0;
+			break;
+#ifdef ZM_ENABLE_CENC
+        case ZM_CENC:
+            *pIvLen = 18;
+            *pIcvLen = 16;
+#endif //ZM_ENABLE_CENC
+		}/* end of switch */
+    }
+    else
+    {
+        if ( (encryMode == ZM_WEP64)||
+             (encryMode == ZM_WEP128)||
+             (encryMode == ZM_WEP256) )
+        {
+            *pIvLen = 4;
+            *pIcvLen = 4;
+        }
+        else if ( encryMode == ZM_TKIP )
+        {
+            *pIvLen = 8;
+            *pIcvLen = 4;
+        }
+        else if ( encryMode == ZM_AES )
+        {
+            *pIvLen = 8;
+            *pIcvLen = 8; // AES MIC
+        }
+#ifdef ZM_ENABLE_CENC
+        else if ( encryMode == ZM_CENC)
+        {
+            *pIvLen = 18;
+            *pIcvLen= 16;
+        }
+#endif //ZM_ENABLE_CENC
+    }
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAgingDefragList           */
+/*      Force flushing whole defrag list or aging the buffer            */
+/*      in the defrag list.                                             */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      flushFlag : 1=>flushing, 0=>Aging                               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfAgingDefragList(zdev_t* dev, u16_t flushFlag)
+{
+    u16_t i, j;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    for(i=0; i<ZM_MAX_DEFRAG_ENTRIES; i++)
+    {
+        if (wd->defragTable.defragEntry[i].fragCount != 0 )
+        {
+            if (((wd->tick - wd->defragTable.defragEntry[i].tick) >
+                        (ZM_DEFRAG_AGING_TIME_SEC * ZM_TICK_PER_SECOND))
+               || (flushFlag != 0))
+            {
+                zm_msg1_rx(ZM_LV_2, "Aging defrag list :", i);
+                /* Free the buffers in the defrag list */
+                for (j=0; j<wd->defragTable.defragEntry[i].fragCount; j++)
+                {
+                    zfwBufFree(dev, wd->defragTable.defragEntry[i].fragment[j], 0);
+                }
+            }
+        }
+        wd->defragTable.defragEntry[i].fragCount = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAddFirstFragToDefragList  */
+/*      Add first fragment to defragment list, the first empty entry    */
+/*      will be selected. If the list is full, sequentially select      */
+/*      one entry for replacement.                                      */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : first fragment buffer                                     */
+/*      addr : address of first fragment buffer                         */
+/*      seqNum : sequence of first fragment buffer                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfAddFirstFragToDefragList(zdev_t* dev, zbuf_t* buf, u8_t* addr, u16_t seqNum)
+{
+    u16_t i, j;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    /* Find an empty one in defrag list */
+    for(i=0; i<ZM_MAX_DEFRAG_ENTRIES; i++)
+    {
+        if ( wd->defragTable.defragEntry[i].fragCount == 0 )
+        {
+            break;
+        }
+    }
+
+    /* If full, sequentially replace existing one */
+    if (i == ZM_MAX_DEFRAG_ENTRIES)
+    {
+        i = wd->defragTable.replaceNum++ & (ZM_MAX_DEFRAG_ENTRIES-1);
+        /* Free the buffers in the defrag list to be replaced */
+        for (j=0; j<wd->defragTable.defragEntry[i].fragCount; j++)
+        {
+            zfwBufFree(dev, wd->defragTable.defragEntry[i].fragment[j], 0);
+        }
+    }
+
+    wd->defragTable.defragEntry[i].fragCount = 1;
+    wd->defragTable.defragEntry[i].fragment[0] = buf;
+    wd->defragTable.defragEntry[i].seqNum = seqNum;
+    wd->defragTable.defragEntry[i].tick = wd->tick;
+
+    for (j=0; j<6; j++)
+    {
+        wd->defragTable.defragEntry[i].addr[j] = addr[j];
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAddFragToDefragList       */
+/*      Add middle or last fragment to defragment list.                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : first fragment buffer                                     */
+/*      addr : address of fragment buffer                               */
+/*      seqNum : sequence fragment buffer                               */
+/*      fragNum : fragment number of fragment buffer                    */
+/*      moreFrag : more frag bit of fragment buffer                     */
+/*      addInfo : addition info of fragment buffer                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+zbuf_t* zfAddFragToDefragList(zdev_t* dev, zbuf_t* buf, u8_t* addr,
+        u16_t seqNum, u8_t fragNum, u8_t moreFrag,
+        struct zsAdditionInfo* addInfo)
+{
+    u16_t i, j, k;
+    zbuf_t* returnBuf = NULL;
+    u16_t defragDone = 0;
+    u16_t lenErr = 0;
+    u16_t startAddr, fragHead, frameLen, ivLen, icvLen;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    /* Find frag in the defrag list */
+    for(i=0; i<ZM_MAX_DEFRAG_ENTRIES; i++)
+    {
+        if ( wd->defragTable.defragEntry[i].fragCount != 0 )
+        {
+            /* Compare address */
+            for (j=0; j<6; j++)
+            {
+                if (addr[j] != wd->defragTable.defragEntry[i].addr[j])
+                {
+                    break;
+                }
+            }
+            if (j == 6)
+            {
+                /* Compare sequence and fragment number */
+                if (seqNum == wd->defragTable.defragEntry[i].seqNum)
+                {
+                    if ((fragNum == wd->defragTable.defragEntry[i].fragCount)
+                                && (fragNum < 8))
+                    {
+                        /* Add frag frame to defrag list */
+                        wd->defragTable.defragEntry[i].fragment[fragNum] = buf;
+                        wd->defragTable.defragEntry[i].fragCount++;
+                        defragDone = 1;
+
+                        if (moreFrag == 0)
+                        {
+                            /* merge all fragment if more data bit is cleared */
+                            returnBuf = wd->defragTable.defragEntry[i].fragment[0];
+                            startAddr = zfwBufGetSize(dev, returnBuf);
+                            /* skip WLAN header 24(Data) or 26(QoS Data) */
+                            fragHead = 24 + ((zmw_rx_buf_readh(dev, returnBuf, 0) & 0x80) >> 6);
+                            zfGetRxIvIcvLength(dev, returnBuf, 0, &ivLen, &icvLen, addInfo);
+                            fragHead += ivLen; /* skip IV */
+                            for(k=1; k<wd->defragTable.defragEntry[i].fragCount; k++)
+                            {
+                                frameLen = zfwBufGetSize(dev,
+                                                         wd->defragTable.defragEntry[i].fragment[k]);
+                                if ((startAddr+frameLen-fragHead) < 1560)
+                                {
+                                    zfRxBufferCopy(dev, returnBuf, wd->defragTable.defragEntry[i].fragment[k],
+                                               startAddr, fragHead, frameLen-fragHead);
+                                    startAddr += (frameLen-fragHead);
+                                }
+                                else
+                                {
+                                    lenErr = 1;
+                                }
+                                zfwBufFree(dev, wd->defragTable.defragEntry[i].fragment[k], 0);
+                            }
+
+                            wd->defragTable.defragEntry[i].fragCount = 0;
+                            zfwBufSetSize(dev, returnBuf, startAddr);
+                        }
+                        break;
+                    }
+                }
+            }
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (lenErr == 1)
+    {
+        zfwBufFree(dev, returnBuf, 0);
+        return NULL;
+    }
+    if (defragDone == 0)
+    {
+        zfwBufFree(dev, buf, 0);
+        return NULL;
+    }
+
+    return returnBuf;
+}
+
+
+/* return value = NULL => save or free this frame         */
+zbuf_t* zfDefragment(zdev_t* dev, zbuf_t* buf, u8_t* pbIsDefrag,
+                     struct zsAdditionInfo* addInfo)
+{
+    u8_t fragNum;
+    u16_t seqNum;
+    u8_t moreFragBit;
+    u8_t addr[6];
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    *pbIsDefrag = FALSE;
+    seqNum = zmw_buf_readh(dev, buf, 22);
+    fragNum = (u8_t)(seqNum & 0xf);
+    moreFragBit = (zmw_buf_readb(dev, buf, 1) & ZM_BIT_2) >> 2;
+
+    if ((fragNum == 0) && (moreFragBit == 0))
+    {
+        /* Not part of a fragmentation */
+
+        return buf;
+    }
+    else
+    {
+        wd->commTally.swRxFragmentCount++;
+        seqNum = seqNum >> 4;
+        for (i=0; i<6; i++)
+        {
+            addr[i] = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+i);
+        }
+
+        if (fragNum == 0)
+        {
+            /* more frag = 1 */
+            /* First part of a fragmentation */
+            zm_msg1_rx(ZM_LV_2, "First Frag, seq=", seqNum);
+            zfAddFirstFragToDefragList(dev, buf, addr, seqNum);
+            buf = NULL;
+        }
+        else
+        {
+            /* Middle or last part of a fragmentation */
+            zm_msg1_rx(ZM_LV_2, "Frag seq=", seqNum);
+            zm_msg1_rx(ZM_LV_2, "Frag moreFragBit=", moreFragBit);
+            buf = zfAddFragToDefragList(dev, buf, addr, seqNum, fragNum, moreFragBit, addInfo);
+            if (buf != NULL)
+            {
+                *pbIsDefrag = TRUE;
+            }
+        }
+    }
+
+    return buf;
+}
+
+
+#if ZM_PROTOCOL_RESPONSE_SIMULATION
+u16_t zfSwap(u16_t num)
+{
+    return ((num >> 8) + ((num & 0xff) << 8));
+}
+
+
+void zfProtRspSim(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t ethType;
+    u16_t arpOp;
+    u16_t prot;
+    u16_t temp;
+    u16_t i;
+    u16_t dip[2];
+    u16_t dstPort;
+    u16_t srcPort;
+
+    ethType = zmw_rx_buf_readh(dev, buf, 12);
+    zm_msg2_rx(ZM_LV_2, "ethType=", ethType);
+
+    /* ARP */
+    if (ethType == 0x0608)
+    {
+        arpOp = zmw_rx_buf_readh(dev, buf, 20);
+        dip[0] = zmw_rx_buf_readh(dev, buf, 38);
+        dip[1] = zmw_rx_buf_readh(dev, buf, 40);
+        zm_msg2_rx(ZM_LV_2, "arpOp=", arpOp);
+        zm_msg2_rx(ZM_LV_2, "ip0=", dip[0]);
+        zm_msg2_rx(ZM_LV_2, "ip1=", dip[1]);
+
+        //ARP request to 192.168.1.15
+        if ((arpOp == 0x0100) && (dip[0] == 0xa8c0) && (dip[1] == 0x0f01));
+        {
+            zm_msg0_rx(ZM_LV_2, "ARP");
+            /* ARP response */
+            zmw_rx_buf_writeh(dev, buf, 20, 0x0200);
+
+            /* dst hardware address */
+
+            /* src hardware address */
+            //zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
+            //zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
+            //zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
+
+            /* dst ip address */
+            for (i=0; i<5; i++)
+            {
+                temp = zmw_rx_buf_readh(dev, buf, 22+(i*2));
+                zmw_rx_buf_writeh(dev, buf, 32+(i*2), temp);
+            }
+
+            /* src hardware address */
+            zmw_rx_buf_writeh(dev, buf, 22, 0xa000);
+            zmw_rx_buf_writeh(dev, buf, 24, 0x0000);
+            zmw_rx_buf_writeh(dev, buf, 26, 0x0000);
+
+            /* src ip address */
+            zmw_rx_buf_writeh(dev, buf, 28, 0xa8c0);
+            zmw_rx_buf_writeh(dev, buf, 30, 0x0f01);
+        }
+    }
+    /* ICMP */
+    else if (ethType == 0x0008)
+    {
+        zm_msg0_rx(ZM_LV_2, "IP");
+        prot = zmw_rx_buf_readb(dev, buf, 23);
+        dip[0] = zmw_rx_buf_readh(dev, buf, 30);
+        dip[1] = zmw_rx_buf_readh(dev, buf, 32);
+        zm_msg2_rx(ZM_LV_2, "prot=", prot);
+        zm_msg2_rx(ZM_LV_2, "ip0=", dip[0]);
+        zm_msg2_rx(ZM_LV_2, "ip1=", dip[1]);
+
+        /* PING request to 192.168.1.15 */
+        if ((prot == 0x1) && (dip[0] == 0xa8c0) && (dip[1] == 0x0f01))
+        {
+            zm_msg0_rx(ZM_LV_2, "ICMP");
+            /* change dst */
+            for (i=0; i<3; i++)
+            {
+                temp = zmw_rx_buf_readh(dev, buf, 6+(i*2));
+                zmw_rx_buf_writeh(dev, buf, i*2, temp);
+            }
+            /* change src */
+            zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
+            zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
+            zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
+
+            /* exchange src ip and dst ip */
+            for (i=0; i<2; i++)
+            {
+                temp = zmw_rx_buf_readh(dev, buf, 26+(i*2));
+                zmw_rx_buf_writeh(dev, buf, 30+(i*2), temp);
+            }
+            zmw_rx_buf_writeh(dev, buf, 26, 0xa8c0);
+            zmw_rx_buf_writeh(dev, buf, 28, 0x0f01);
+
+            /* change icmp type to echo reply */
+            zmw_rx_buf_writeb(dev, buf, 34, 0x0);
+
+            /* update icmp checksum */
+            temp = zmw_rx_buf_readh(dev, buf, 36);
+            temp += 8;
+            zmw_rx_buf_writeh(dev, buf, 36, temp);
+        }
+        else if (prot == 0x6)
+        {
+            zm_msg0_rx(ZM_LV_2, "TCP");
+            srcPort = zmw_rx_buf_readh(dev, buf, 34);
+            dstPort = zmw_rx_buf_readh(dev, buf, 36);
+            zm_msg2_rx(ZM_LV_2, "Src Port=", srcPort);
+            zm_msg2_rx(ZM_LV_2, "Dst Port=", dstPort);
+            if ((dstPort == 0x1500) || (srcPort == 0x1500))
+            {
+                zm_msg0_rx(ZM_LV_2, "FTP");
+
+                /* change dst */
+                for (i=0; i<3; i++)
+                {
+                    temp = zmw_rx_buf_readh(dev, buf, 6+(i*2));
+                    zmw_rx_buf_writeh(dev, buf, i*2, temp);
+                }
+                /* change src */
+                zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
+                zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
+                zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
+
+                /* exchange src ip and dst ip */
+                for (i=0; i<2; i++)
+                {
+                    temp = zmw_rx_buf_readh(dev, buf, 26+(i*2));
+                    zmw_rx_buf_writeh(dev, buf, 30+(i*2), temp);
+                }
+                zmw_rx_buf_writeh(dev, buf, 26, 0xa8c0);
+                zmw_rx_buf_writeh(dev, buf, 28, 0x0f01);
+#if 0
+                /* Patch src port */
+                temp = zmw_rx_buf_readh(dev, buf, 34);
+                temp = zfSwap(zfSwap(temp) + 1);
+                zmw_rx_buf_writeh(dev, buf, 34, temp);
+                temp = zmw_rx_buf_readh(dev, buf, 38);
+                temp = zfSwap(zfSwap(temp) + 1);
+                zmw_rx_buf_writeh(dev, buf, 38, temp);
+
+                /* Patch checksum */
+                temp = zmw_rx_buf_readh(dev, buf, 50);
+                temp = zfSwap(temp);
+                temp = ~temp;
+                temp += 2;
+                temp = ~temp;
+                temp = zfSwap(temp);
+                zmw_rx_buf_writeh(dev, buf, 50, temp);
+#endif
+            }
+
+        }
+        else if (prot == 0x11)
+        {
+            /* change dst */
+            for (i=0; i<3; i++)
+            {
+                temp = zmw_rx_buf_readh(dev, buf, 6+(i*2));
+                zmw_rx_buf_writeh(dev, buf, i*2, temp);
+            }
+            /* change src */
+            zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
+            zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
+            zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
+
+            zm_msg0_rx(ZM_LV_2, "UDP");
+            srcPort = zmw_rx_buf_readh(dev, buf, 34);
+            dstPort = zmw_rx_buf_readh(dev, buf, 36);
+            zm_msg2_rx(ZM_LV_2, "Src Port=", srcPort);
+            zm_msg2_rx(ZM_LV_2, "Dst Port=", dstPort);
+
+            /* exchange src ip and dst ip */
+            for (i=0; i<2; i++)
+            {
+                temp = zmw_rx_buf_readh(dev, buf, 26+(i*2));
+                zmw_rx_buf_writeh(dev, buf, 30+(i*2), temp);
+            }
+            zmw_rx_buf_writeh(dev, buf, 26, 0xa8c0);
+            zmw_rx_buf_writeh(dev, buf, 28, 0x0f01);
+
+            /* exchange port */
+            zmw_rx_buf_writeh(dev, buf, 34, srcPort+1);
+            zmw_rx_buf_writeh(dev, buf, 36, dstPort);
+
+            /* checksum = 0 */
+            zmw_rx_buf_writeh(dev, buf, 40, 0);
+        }
+
+    }
+    else if (ethType == 0x0060) /* =>0x0060 is port */
+    {
+        /* change src for Evl tool loop back receive */
+        zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
+        zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
+        zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
+    }
+
+}
+#endif
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiTxSendEth                */
+/*      Called to native 802.11 management frames                       */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      port : WLAN port, 0=>standard, 0x1-0x7=>VAP, 0x20-0x25=>WDS     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Ray             ZyDAS Technology Corporation    2005.5      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfiTxSend80211Mgmt(zdev_t* dev, zbuf_t* buf, u16_t port)
+{
+    u16_t err;
+    //u16_t addrTblSize = 0;
+    //struct zsAddrTbl addrTbl;
+    u16_t hlen;
+    u16_t header[(24+25+1)/2];
+    int i;
+
+    for(i=0;i<12;i++)
+    {
+        header[i] = zmw_buf_readh(dev, buf, i);
+    }
+    hlen = 24;
+
+    zfwBufRemoveHead(dev, buf, 24);
+
+    if ((err = zfHpSend(dev, header, hlen, NULL, 0, NULL, 0, buf, 0,
+            ZM_EXTERNAL_ALLOC_BUF, 0, 0)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+
+    return 0;
+
+zlError:
+
+    zfwBufFree(dev, buf, 0);
+    return 0;
+}
+
+u8_t zfiIsTxQueueFull(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if ((((wd->vtxqHead[0] + 1) & ZM_VTXQ_SIZE_MASK) != wd->vtxqTail[0]) )
+    {
+        zmw_leave_critical_section(dev);
+        return 0;
+    }
+    else
+    {
+        zmw_leave_critical_section(dev);
+        return 1;
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiTxSendEth                */
+/*      Called to transmit Ethernet frame from upper layer.             */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      port : WLAN port, 0=>standard, 0x1-0x7=>VAP, 0x20-0x25=>WDS     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.5      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfiTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port)
+{
+    u16_t err, ret;
+
+    zmw_get_wlan_dev(dev);
+
+    ZM_PERFORMANCE_TX_MSDU(dev, wd->tick);
+    zm_msg1_tx(ZM_LV_2, "zfiTxSendEth(), port=", port);
+    /* Return error if port is disabled */
+    if ((err = zfTxPortControl(dev, buf, port)) == ZM_PORT_DISABLED)
+    {
+        err = ZM_ERR_TX_PORT_DISABLED;
+        goto zlError;
+    }
+
+#if 1
+    if ((wd->wlanMode == ZM_MODE_AP) && (port < 0x20))
+    {
+        /* AP : Buffer frame for power saving STA */
+        if ((ret = zfApBufferPsFrame(dev, buf, port)) == 1)
+        {
+            return ZM_SUCCESS;
+        }
+    }
+    else
+#endif
+    if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        if ( zfPowerSavingMgrIsSleeping(dev) )
+        {
+            /*check ZM_ENABLE_POWER_SAVE flag*/
+            zfPowerSavingMgrWakeup(dev);
+        }
+    }
+#ifdef ZM_ENABLE_IBSS_PS
+    /* IBSS power-saving mode */
+    else if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        if ( zfStaIbssPSQueueData(dev, buf) )
+        {
+            return ZM_SUCCESS;
+        }
+    }
+#endif
+
+#if 1
+    //if ( wd->bQoSEnable )
+    if (1)
+    {
+        /* Put to VTXQ[ac] */
+        ret = zfPutVtxq(dev, buf);
+
+        /* Push VTXQ[ac] */
+        zfPushVtxq(dev);
+    }
+    else
+    {
+        ret = zfTxSendEth(dev, buf, port, ZM_EXTERNAL_ALLOC_BUF, 0);
+    }
+
+    return ret;
+#else
+    return zfTxSendEth(dev, buf, port, ZM_EXTERNAL_ALLOC_BUF, 0);
+#endif
+
+zlError:
+    zm_msg2_tx(ZM_LV_1, "Tx Comp err=", err);
+
+    zfwBufFree(dev, buf, err);
+    return err;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfTxSendEth                 */
+/*      Called to transmit Ethernet frame from upper layer.             */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      port : WLAN port, 0=>standard, 0x10-0x17=>VAP, 0x20-0x25=>WDS   */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.5      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port, u16_t bufType, u16_t flag)
+{
+    u16_t err;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t removeLen;
+    u16_t header[(8+30+2+18)/2];    /* ctr+(4+a1+a2+a3+2+a4)+qos+iv */
+    u16_t headerLen;
+    u16_t mic[8/2];
+    u16_t micLen;
+    u16_t snap[8/2];
+    u16_t snapLen;
+    u16_t fragLen;
+    u16_t frameLen;
+    u16_t fragNum;
+    struct zsFrag frag;
+    u16_t i, j, id;
+    u16_t offset;
+    u16_t da[3];
+    u16_t sa[3];
+    u8_t up;
+    u8_t qosType, keyIdx = 0;
+    u16_t fragOff;
+    u16_t newFlag;
+    struct zsMicVar*  pMicKey;
+    u8_t tkipFrameOffset = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    newFlag = flag & 0xff00;
+    flag = flag & 0xff;
+
+    zm_msg1_tx(ZM_LV_2, "zfTxSendEth(), port=", port);
+
+    /* Get IP TOS for QoS AC and IP frag offset */
+    zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+
+    //EOSP bit
+    if (newFlag & 0x100)
+    {
+        up |= 0x10;
+    }
+
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 16);
+        da[1] = zmw_tx_buf_readh(dev, buf, 18);
+        da[2] = zmw_tx_buf_readh(dev, buf, 20);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 10);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 12);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 14);
+    }
+    else if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 4);
+        da[1] = zmw_tx_buf_readh(dev, buf, 6);
+        da[2] = zmw_tx_buf_readh(dev, buf, 8);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 10);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 12);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 14);
+    }
+    else if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 4);
+        da[1] = zmw_tx_buf_readh(dev, buf, 6);
+        da[2] = zmw_tx_buf_readh(dev, buf, 8);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 16);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 18);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 20);
+    }
+    else
+    {
+        //
+    }
+#else
+    /* DA */
+    da[0] = zmw_tx_buf_readh(dev, buf, 0);
+    da[1] = zmw_tx_buf_readh(dev, buf, 2);
+    da[2] = zmw_tx_buf_readh(dev, buf, 4);
+    /* SA */
+    sa[0] = zmw_tx_buf_readh(dev, buf, 6);
+    sa[1] = zmw_tx_buf_readh(dev, buf, 8);
+    sa[2] = zmw_tx_buf_readh(dev, buf, 10);
+#endif
+    //Decide Key Index in ATOM, No meaning in OTUS--CWYang(m)
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        keyIdx = wd->ap.bcHalKeyIdx[port];
+        id = zfApFindSta(dev, da);
+        if (id != 0xffff)
+        {
+            switch (wd->ap.staTable[id].encryMode)
+            {
+            case ZM_AES:
+            case ZM_TKIP:
+#ifdef ZM_ENABLE_CENC
+            case ZM_CENC:
+#endif //ZM_ENABLE_CENC
+                keyIdx = wd->ap.staTable[id].keyIdx;
+                break;
+            }
+        }
+    }
+    else
+    {
+        switch (wd->sta.encryMode)
+        {
+        case ZM_WEP64:
+        case ZM_WEP128:
+        case ZM_WEP256:
+            keyIdx = wd->sta.keyId;
+            break;
+        case ZM_AES:
+        case ZM_TKIP:
+            if ((da[0] & 0x1))
+                keyIdx = 5;
+            else
+                keyIdx = 4;
+            break;
+#ifdef ZM_ENABLE_CENC
+        case ZM_CENC:
+            keyIdx = wd->sta.cencKeyId;
+            break;
+#endif //ZM_ENABLE_CENC
+        }
+    }
+
+    /* Create SNAP */
+    removeLen = zfTxGenWlanSnap(dev, buf, snap, &snapLen);
+    //zm_msg1_tx(ZM_LV_0, "fragOff=", fragOff);
+
+
+/* ********************************************************************************************** */
+/* Add 20071025 Mxzeng                                                                            */
+/* ********************************************************************************************** */
+/* ---------------------------------------------------------------------------------------------- */
+/*  Ethernet : frameLen = zfwBufGetSize(dev, buf);                                                */
+/* ---+--6--+--6--+--2--+-----20-----+-------------------------+------ Variable -------+--------- */
+/*    |  DA |  SA | Type|  IP Header | TCP(20) UDP(12) ICMP(8) | Application Payload L |          */
+/* ---+-----+-----+-----+------------+-------------------------+-----------------------+--------- */
+/*  MSDU = 6 + 6 + 2 + ( Network Layer header ) + ( Transport Layer header ) + L                  */
+/*                                                                                                */
+/*  MSDU - DA - SA : frameLen -= removeLen;                                                       */
+/* ---+--2--+-----20-----+-------------------------+------ Variable -------+--------------------- */
+/*    | Type| IP Header  | TCP(20) UDP(12) ICMP(8) | Application Payload L |                      */
+/* ---+-----+------------+-------------------------+-----------------------+--------------------- */
+/*												  */
+/*  MPDU : frameLen + mpduLengthOffset ;                                                          */
+/* -+---2---+----2---+-6-+-6-+--6--+---2----+--1--+--1-+---1---+-------3------+-frameLen-+---4--+- */
+/*  | frame |duration| DA|SA |BSSID|sequence|SNAP |SNAP|Control|    RFC 1042  |          |  FCS |  */
+/*  |Control|        |   |   |     | number |DSAP |SSAP|       | encapsulation|          |      |  */
+/* -+-------+--------+---+---+-----+--------+-----+----+-------+--------------+----------+------+- */
+/* ----------------------------------------------------------------------------------------------- */
+
+    if ( wd->sta.encryMode == ZM_TKIP )
+        tkipFrameOffset = 8;
+
+    fragLen = wd->fragThreshold + tkipFrameOffset;   // Fragmentation threshold for MPDU Lengths
+    frameLen = zfwBufGetSize(dev, buf);    // MSDU Lengths
+    frameLen -= removeLen;                 // MSDU Lengths - DA - SA
+
+    /* #1st create MIC Length manually */
+    micLen = 0;
+
+    /* Access Category */
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        zfApGetStaQosType(dev, da, &qosType);
+        if (qosType == 0)
+        {
+            up = 0;
+        }
+    }
+    else if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        if (wd->sta.wmeConnected == 0)
+        {
+            up = 0;
+        }
+    }
+    else
+    {
+        /* TODO : STA QoS control field */
+        up = 0;
+    }
+
+    /* #2nd Assign sequence number */
+    zmw_enter_critical_section(dev);
+    frag.seq[0] = ((wd->seq[zcUpToAc[up&0x7]]++) << 4);
+    zmw_leave_critical_section(dev);
+
+    /* #3rd Pass the total payload to generate MPDU length ! */
+    frag.buf[0] = buf;
+    frag.bufType[0] = bufType;
+    frag.flag[0] = (u8_t)flag;
+    fragNum = 1;
+
+    headerLen = zfTxGenWlanHeader(dev, frag.buf[0], header, frag.seq[0],
+                                  frag.flag[0], snapLen+micLen, removeLen, port, da, sa,
+                                  up, &micLen, snap, snapLen, NULL);
+
+    //zm_debug_msg1("#1 headerLen = ", headerLen);
+
+    /* #4th Check the HeaderLen and determine whether the MPDU Lengths bigger than Fragmentation threshold  */
+    /* If MPDU Lengths large than fragmentation threshold --> headerLen = 0 */
+    if( headerLen != 0 )
+    {
+        zf80211FrameSend(dev, frag.buf[0], header, snapLen, da, sa, up,
+                         headerLen, snap, mic, micLen, removeLen, frag.bufType[0],
+                         zcUpToAc[up&0x7], keyIdx);
+    }
+    else //if( headerLen == 0 ) // Need to be fragmented
+    {
+        u16_t mpduLengthOffset;
+        u16_t pseudSnapLen = 0;
+
+        mpduLengthOffset = header[0] - frameLen; // For fragmentation threshold !
+
+        micLen = zfTxGenWlanTail(dev, buf, snap, snapLen, mic); // Get snap and mic information
+
+        fragLen = fragLen - mpduLengthOffset;
+
+        //zm_debug_msg1("#2 frameLen = ", frameLen);
+        //zm_debug_msg1("#3 fragThreshold = ", fragLen);
+
+        /* fragmentation */
+        if (frameLen >= fragLen)
+        {
+            //copy fragLen to frag
+            i = 0;
+            while( frameLen > 0 )
+            {
+                if ((frag.buf[i] = zfwBufAllocate(dev, fragLen+32)) != NULL)
+                {
+                    frag.bufType[i] = ZM_INTERNAL_ALLOC_BUF;
+                    frag.seq[i] = frag.seq[0] + i;
+                    offset = removeLen + i*fragLen;
+
+                    /* Consider the offset if we consider snap length to the other fragmented frame */
+                    if ( i >= 1 )
+                        offset = offset + pseudSnapLen*(i-1);
+
+                    if (frameLen > fragLen + pseudSnapLen)
+                    {
+                        frag.flag[i] = flag | 0x4; /* More data */
+                        /* First fragment */
+                        if (i == 0)
+                        {
+                            /* Add SNAP */
+                            for (j=0; j<snapLen; j+=2)
+                            {
+                                zmw_tx_buf_writeh(dev, frag.buf[i], j, snap[(j>>1)]);
+                            }
+                            zfTxBufferCopy(dev, frag.buf[i], buf, snapLen, offset, fragLen);
+                            zfwBufSetSize(dev, frag.buf[i], snapLen+fragLen);
+
+                            /* Add pseud snap length to the other fragmented frame */
+                            pseudSnapLen = snapLen;
+
+                            frameLen -= fragLen;
+                        }
+                        /* Intermediate Fragment */
+                        else
+                        {
+                            //zfTxBufferCopy(dev, frag.buf[i], buf, 0, offset, fragLen);
+                            //zfwBufSetSize(dev, frag.buf[i], fragLen);
+
+                            zfTxBufferCopy(dev, frag.buf[i], buf, 0, offset, fragLen+pseudSnapLen );
+                            zfwBufSetSize(dev, frag.buf[i], fragLen+pseudSnapLen);
+
+                            frameLen -= (fragLen+pseudSnapLen);
+                        }
+                        //frameLen -= fragLen;
+                    }
+                    else
+                    {
+                        /* Last fragment  */
+                        zfTxBufferCopy(dev, frag.buf[i], buf, 0, offset, frameLen);
+                        /* Add MIC if need */
+                        if ( micLen )
+                        {
+                            zfCopyToRxBuffer(dev, frag.buf[i], (u8_t*) mic, frameLen, micLen);
+                        }
+                        zfwBufSetSize(dev, frag.buf[i], frameLen+micLen);
+                        frameLen = 0;
+                        frag.flag[i] = (u8_t)flag; /* No more data */
+                    }
+                    i++;
+                }
+                else
+                {
+                    break;
+                }
+
+                // Please pay attention to the index of the buf !!!
+                // If write to null buf , the OS will crash !!!
+                zfwCopyBufContext(dev, buf, frag.buf[i-1]);
+            }
+            fragNum = i;
+            snapLen = micLen = removeLen = 0;
+
+            zfwBufFree(dev, buf, 0);
+        }
+
+        for (i=0; i<fragNum; i++)
+        {
+            /* Create WLAN header(Control Setting + 802.11 header + IV) */
+            headerLen = zfTxGenWlanHeader(dev, frag.buf[i], header, frag.seq[i],
+                                    frag.flag[i], snapLen+micLen, removeLen, port, da, sa, up, &micLen,
+                                    snap, snapLen, NULL);
+
+            zf80211FrameSend(dev, frag.buf[i], header, snapLen, da, sa, up,
+                             headerLen, snap, mic, micLen, removeLen, frag.bufType[i],
+                             zcUpToAc[up&0x7], keyIdx);
+
+        } /* for (i=0; i<fragNum; i++) */
+    }
+
+    return ZM_SUCCESS;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfTxPortControl             */
+/*      Check port status.                                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      buf : buffer pointer                                            */
+/*      port : port number, 0=>standard, 10-17=>Virtual AP, 20-25=>WDS  */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_PORT_ENABLED or ZM_PORT_DISABLE                              */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Signature           ZyDAS Technology Corporation    2005.4      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfTxPortControl(zdev_t* dev, zbuf_t* buf, u16_t port)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        if ( wd->sta.adapterState == ZM_STA_STATE_DISCONNECT )
+        {
+            zm_msg0_tx(ZM_LV_3, "Packets dropped due to disconnect state");
+            return ZM_PORT_DISABLED;
+        }
+    }
+
+    return ZM_PORT_ENABLED;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfIdlRecv                   */
+/*      Do frame validation and filtering then pass to zfwRecv80211().  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : received 802.11 frame buffer.                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfCoreRecv(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo)
+{
+    u16_t ret = 0;
+    u16_t bssid[3];
+    struct agg_tid_rx *tid_rx;
+    zmw_get_wlan_dev(dev);
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    /* tally */
+    wd->commTally.DriverRxFrmCnt++;
+
+    bssid[0] = zmw_buf_readh(dev, buf, 16);
+    bssid[1] = zmw_buf_readh(dev, buf, 18);
+    bssid[2] = zmw_buf_readh(dev, buf, 20);
+
+    /* Validate Rx frame */
+    if ((ret = zfWlanRxValidate(dev, buf)) != ZM_SUCCESS)
+    {
+        zm_msg1_rx(ZM_LV_1, "Rx invalid:", ret);
+        goto zlError;
+    }
+
+#ifdef ZM_ENABLE_AGGREGATION
+    //#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION
+    /*
+     * add by honda
+     */
+    tid_rx = zfAggRxEnabled(dev, buf);
+    if (tid_rx && wd->reorder)
+    {
+        zfAggRx(dev, buf, addInfo, tid_rx);
+
+        return;
+    }
+    /*
+     * end of add by honda
+     */
+    //#endif
+#endif
+
+    /* Filter Rx frame */
+    if ((ret = zfWlanRxFilter(dev, buf)) != ZM_SUCCESS)
+    {
+        zm_msg1_rx(ZM_LV_1, "Rx duplicated:", ret);
+        goto zlError;
+    }
+
+    /* Discard error frame except mic failure */
+    if ((addInfo->Tail.Data.ErrorIndication & 0x3f) != 0)
+    {
+        if ( wd->XLinkMode && ((addInfo->Tail.Data.ErrorIndication & 0x3f)==0x10) &&
+             zfCompareWithBssid(dev, bssid) )
+        {
+            // Bypass frames !!!
+        }
+        else
+        {
+            goto zlError;
+        }
+    }
+
+
+    /* OTUS command-8212 dump rx packet */
+    if (wd->rxPacketDump)
+    {
+        zfwDumpBuf(dev, buf);
+    }
+
+    /* Call zfwRecv80211() wrapper function to deliver Rx packet */
+    /* to driver framework.                                      */
+
+    if (wd->zfcbRecv80211 != NULL)
+    {
+        wd->zfcbRecv80211(dev, buf, addInfo); //CWYang(m)
+    }
+    else
+    {
+        zfiRecv80211(dev, buf, addInfo);
+    }
+    return;
+
+zlError:
+    zm_msg1_rx(ZM_LV_1, "Free packet, error code:", ret);
+
+    wd->commTally.DriverDiscardedFrm++;
+
+    /* Free Rx buffer */
+    zfwBufFree(dev, buf, 0);
+
+    return;
+}
+
+
+void zfShowRxEAPOL(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u8_t   packetType, keyType, code, identifier, type, flags;
+    u16_t  packetLen, keyInfo, keyLen, keyDataLen, length, Op_Code;
+    u32_t  replayCounterH, replayCounterL, vendorId, VendorType;
+
+    /* EAPOL packet type */
+    packetType = zmw_rx_buf_readb(dev, buf, offset+1); // 0: EAP-Packet
+                                                       // 1: EAPOL-Start
+                                                       // 2: EAPOL-Logoff
+                                                       // 3: EAPOL-Key
+                                                       // 4: EAPOL-Encapsulated-ASF-Alert
+
+    /* EAPOL frame format */
+    /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
+    /* -----------------------------------------------   */
+    /*            PAE Ethernet Type (0x888e)             */
+    /* ----------------------------------------------- 2 */
+    /*     Protocol Version    |         Type            */
+    /* ----------------------------------------------- 4 */
+    /*                       Length                      */
+    /* ----------------------------------------------- 6 */
+    /*                    Packet Body                    */
+    /* ----------------------------------------------- N */
+
+    /* EAPOL body length */
+    packetLen = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+2)) << 8) +
+                zmw_rx_buf_readb(dev, buf, offset+3);
+
+    if( packetType == 0 )
+    { // EAP-Packet
+
+        /* EAP-Packet Code */
+        code = zmw_rx_buf_readb(dev, buf, offset+4); // 1 : Request
+                                                     // 2 : Response
+                                                     // 3 : Success
+                                                     // 4 : Failure
+        // An EAP packet of the type of Success and Failure has no Data field, and has a length of 4.
+
+        /* EAP Packet format */
+        /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
+        /* -----------------------------------------------   */
+        /*           Code          |        Identifier       */
+        /* ----------------------------------------------- 2 */
+        /*                       Length                      */
+        /* ----------------------------------------------- 4 */
+        /*                        Data                       */
+        /* ----------------------------------------------- N */
+
+        zm_debug_msg0("EAP-Packet");
+        zm_debug_msg1("Packet Length = ", packetLen);
+        zm_debug_msg1("EAP-Packet Code = ", code);
+
+        if( code == 1 )
+        {
+            zm_debug_msg0("EAP-Packet Request");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_rx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_rx_buf_readb(dev, buf, offset+7);
+            /* EAP-Packet Type */
+            type = zmw_rx_buf_readb(dev, buf, offset+8); // 1   : Identity
+                                                         // 2   : Notification
+                                                         // 3   : Nak (Response Only)
+                                                         // 4   : MD5-Challenge
+                                                         // 5   : One Time Password (OTP)
+                                                         // 6   : Generic Token Card (GTC)
+                                                         // 254 : (Expanded Types)Wi-Fi Protected Setup
+                                                         // 255 : Experimental Use
+
+            /* The data field in an EAP packet of the type of Request or Response is in the format shown bellowing */
+            /*  0  1  2  3  4  5  6  7             N             */
+            /* -----------------------------------------------   */
+            /*           Type          |        Type Data        */
+            /* -----------------------------------------------   */
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+            zm_debug_msg1("EAP-Packet Type = ", type);
+
+            if( type == 1 )
+            {
+                zm_debug_msg0("EAP-Packet Request Identity");
+            }
+            else if( type == 2 )
+            {
+                zm_debug_msg0("EAP-Packet Request Notification");
+            }
+            else if( type == 4 )
+            {
+                zm_debug_msg0("EAP-Packet Request MD5-Challenge");
+            }
+            else if( type == 5 )
+            {
+                zm_debug_msg0("EAP-Packet Request One Time Password");
+            }
+            else if( type == 6 )
+            {
+                zm_debug_msg0("EAP-Packet Request Generic Token Card");
+            }
+            else if( type == 254 )
+            {
+                zm_debug_msg0("EAP-Packet Request Wi-Fi Protected Setup");
+
+                /* 0                   1                   2                   3   */
+                /* 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 6 7 8 9 0 1 */
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|     Type      |               Vendor-Id                       |*/
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|                          Vendor-Type                          |*/
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|              Vendor data...                                    */
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+                        */
+
+                /* EAP-Packet Vendor ID */
+                vendorId = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+9)) << 16) +
+                           (((u32_t) zmw_rx_buf_readb(dev, buf, offset+10)) << 8) +
+                           zmw_rx_buf_readb(dev, buf, offset+11);
+                /* EAP-Packet Vendor Type */
+                VendorType = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+12)) << 24) +
+                             (((u32_t) zmw_rx_buf_readb(dev, buf, offset+13)) << 16) +
+                             (((u32_t) zmw_rx_buf_readb(dev, buf, offset+14)) << 8) +
+                             zmw_rx_buf_readb(dev, buf, offset+15);
+                /* EAP-Packet Op Code */
+                Op_Code = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+16)) << 8) +
+                          zmw_rx_buf_readb(dev, buf, offset+17);
+                /* EAP-Packet Flags */
+                flags = zmw_rx_buf_readb(dev, buf, offset+18);
+
+                zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
+                zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
+                zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
+                zm_debug_msg1("EAP-Packet Flags = ", flags);
+            }
+        }
+        else if( code == 2 )
+        {
+            zm_debug_msg0("EAP-Packet Response");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_rx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_rx_buf_readb(dev, buf, offset+7);
+            /* EAP-Packet Type */
+            type = zmw_rx_buf_readb(dev, buf, offset+8);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+            zm_debug_msg1("EAP-Packet Type = ", type);
+
+            if( type == 1 )
+            {
+                zm_debug_msg0("EAP-Packet Response Identity");
+            }
+            else if( type == 2 )
+            {
+                zm_debug_msg0("EAP-Packet Request Notification");
+            }
+            else if( type == 3 )
+            {
+                zm_debug_msg0("EAP-Packet Request Nak");
+            }
+            else if( type == 4 )
+            {
+                zm_debug_msg0("EAP-Packet Request MD5-Challenge");
+            }
+            else if( type == 5 )
+            {
+                zm_debug_msg0("EAP-Packet Request One Time Password");
+            }
+            else if( type == 6 )
+            {
+                zm_debug_msg0("EAP-Packet Request Generic Token Card");
+            }
+            else if( type == 254 )
+            {
+                zm_debug_msg0("EAP-Packet Response Wi-Fi Protected Setup");
+
+                /* EAP-Packet Vendor ID */
+                vendorId = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+9)) << 16) +
+                           (((u32_t) zmw_rx_buf_readb(dev, buf, offset+10)) << 8) +
+                           zmw_rx_buf_readb(dev, buf, offset+11);
+                /* EAP-Packet Vendor Type */
+                VendorType = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+12)) << 24) +
+                             (((u32_t) zmw_rx_buf_readb(dev, buf, offset+13)) << 16) +
+                             (((u32_t) zmw_rx_buf_readb(dev, buf, offset+14)) << 8) +
+                             zmw_rx_buf_readb(dev, buf, offset+15);
+                /* EAP-Packet Op Code */
+                Op_Code = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+16)) << 8) +
+                          zmw_rx_buf_readb(dev, buf, offset+17);
+                /* EAP-Packet Flags */
+                flags = zmw_rx_buf_readb(dev, buf, offset+18);
+
+                zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
+                zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
+                zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
+                zm_debug_msg1("EAP-Packet Flags = ", flags);
+            }
+        }
+        else if( code == 3 )
+        {
+            zm_debug_msg0("EAP-Packet Success");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_rx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_rx_buf_readb(dev, buf, offset+7);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+        }
+        else if( code == 4 )
+        {
+            zm_debug_msg0("EAP-Packet Failure");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_rx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_rx_buf_readb(dev, buf, offset+7);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+        }
+    }
+    else if( packetType == 1 )
+    { // EAPOL-Start
+        zm_debug_msg0("EAPOL-Start");
+    }
+    else if( packetType == 2 )
+    { // EAPOL-Logoff
+        zm_debug_msg0("EAPOL-Logoff");
+    }
+    else if( packetType == 3 )
+    { // EAPOL-Key
+        /* EAPOL-Key type */
+        keyType = zmw_rx_buf_readb(dev, buf, offset+4);
+        /* EAPOL-Key information */
+        keyInfo = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+5)) << 8) +
+                  zmw_rx_buf_readb(dev, buf, offset+6);
+        /* EAPOL-Key length */
+        keyLen = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+7)) << 8) +
+                 zmw_rx_buf_readb(dev, buf, offset+8);
+        /* EAPOL-Key replay counter (high double word) */
+        replayCounterH = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+9)) << 24) +
+                         (((u32_t) zmw_rx_buf_readb(dev, buf, offset+10)) << 16) +
+                         (((u32_t) zmw_rx_buf_readb(dev, buf, offset+11)) << 8) +
+                         zmw_rx_buf_readb(dev, buf, offset+12);
+        /* EAPOL-Key replay counter (low double word) */
+        replayCounterL = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+13)) << 24) +
+                         (((u32_t) zmw_rx_buf_readb(dev, buf, offset+14)) << 16) +
+                         (((u32_t) zmw_rx_buf_readb(dev, buf, offset+15)) << 8) +
+                         zmw_rx_buf_readb(dev, buf, offset+16);
+        /* EAPOL-Key data length */
+        keyDataLen = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+97)) << 8) +
+                     zmw_rx_buf_readb(dev, buf, offset+98);
+
+        zm_debug_msg0("EAPOL-Key");
+        zm_debug_msg1("packet length = ", packetLen);
+
+        if ( keyType == 254 )
+        {
+            zm_debug_msg0("key type = 254 (SSN key descriptor)");
+        }
+        else
+        {
+            zm_debug_msg2("key type = 0x", keyType);
+        }
+
+        zm_debug_msg2("replay counter(L) = ", replayCounterL);
+
+        zm_debug_msg2("key information = ", keyInfo);
+
+        if ( keyInfo & ZM_BIT_3 )
+        {
+            zm_debug_msg0("    - pairwise key");
+        }
+        else
+        {
+            zm_debug_msg0("    - group key");
+        }
+
+        if ( keyInfo & ZM_BIT_6 )
+        {
+            zm_debug_msg0("    - Tx key installed");
+        }
+        else
+        {
+            zm_debug_msg0("    - Tx key not set");
+        }
+
+        if ( keyInfo & ZM_BIT_7 )
+        {
+            zm_debug_msg0("    - Ack needed");
+        }
+        else
+        {
+            zm_debug_msg0("    - Ack not needed");
+        }
+
+        if ( keyInfo & ZM_BIT_8 )
+        {
+            zm_debug_msg0("    - MIC set");
+        }
+        else
+        {
+            zm_debug_msg0("    - MIC not set");
+        }
+
+        if ( keyInfo & ZM_BIT_9 )
+        {
+            zm_debug_msg0("    - packet encrypted");
+        }
+        else
+        {
+            zm_debug_msg0("    - packet not encrypted");
+        }
+
+        zm_debug_msg1("keyLen = ", keyLen);
+        zm_debug_msg1("keyDataLen = ", keyDataLen);
+    }
+    else if( packetType == 4 )
+    {
+        zm_debug_msg0("EAPOL-Encapsulated-ASF-Alert");
+    }
+}
+
+void zfShowTxEAPOL(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u8_t   packetType, keyType, code, identifier, type, flags;
+    u16_t  packetLen, keyInfo, keyLen, keyDataLen, length, Op_Code;
+    u32_t  replayCounterH, replayCounterL, vendorId, VendorType;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg1("EAPOL Packet size = ", zfwBufGetSize(dev, buf));
+
+    /* EAPOL packet type */
+    // 0: EAP-Packet
+    // 1: EAPOL-Start
+    // 2: EAPOL-Logoff
+    // 3: EAPOL-Key
+    // 4: EAPOL-Encapsulated-ASF-Alert
+
+    /* EAPOL frame format */
+    /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
+    /* -----------------------------------------------   */
+    /*            PAE Ethernet Type (0x888e)             */
+    /* ----------------------------------------------- 2 */
+    /*     Protocol Version    |         Type            */
+    /* ----------------------------------------------- 4 */
+    /*                       Length                      */
+    /* ----------------------------------------------- 6 */
+    /*                    Packet Body                    */
+    /* ----------------------------------------------- N */
+
+    packetType = zmw_tx_buf_readb(dev, buf, offset+1);
+    /* EAPOL body length */
+    packetLen = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+2)) << 8) +
+                zmw_tx_buf_readb(dev, buf, offset+3);
+
+    if( packetType == 0 )
+    { // EAP-Packet
+        /* EAP-Packet Code */
+        code = zmw_tx_buf_readb(dev, buf, offset+4); // 1 : Request
+                                                     // 2 : Response
+                                                     // 3 : Success
+                                                     // 4 : Failure
+
+        // An EAP packet of the type of Success and Failure has no Data field, and has a length of 4.
+
+        /* EAP Packet format */
+        /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
+        /* -----------------------------------------------   */
+        /*           Code          |        Identifier       */
+        /* ----------------------------------------------- 2 */
+        /*                       Length                      */
+        /* ----------------------------------------------- 4 */
+        /*                        Data                       */
+        /* ----------------------------------------------- N */
+
+        zm_debug_msg0("EAP-Packet");
+        zm_debug_msg1("Packet Length = ", packetLen);
+        zm_debug_msg1("EAP-Packet Code = ", code);
+
+        if( code == 1 )
+        {
+            zm_debug_msg0("EAP-Packet Request");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_tx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_tx_buf_readb(dev, buf, offset+7);
+            /* EAP-Packet Type */
+            type = zmw_tx_buf_readb(dev, buf, offset+8); // 1   : Identity
+                                                         // 2   : Notification
+                                                         // 3   : Nak (Response Only)
+                                                         // 4   : MD5-Challenge
+                                                         // 5   : One Time Password (OTP)
+                                                         // 6   : Generic Token Card (GTC)
+                                                         // 254 : (Expanded Types)Wi-Fi Protected Setup
+                                                         // 255 : Experimental Use
+
+            /* The data field in an EAP packet of the type of Request or Response is in the format shown bellowing */
+            /*  0  1  2  3  4  5  6  7             N             */
+            /* -----------------------------------------------   */
+            /*           Type          |        Type Data        */
+            /* -----------------------------------------------   */
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+            zm_debug_msg1("EAP-Packet Type = ", type);
+
+            if( type == 1 )
+            {
+                zm_debug_msg0("EAP-Packet Request Identity");
+            }
+            else if( type == 2 )
+            {
+                zm_debug_msg0("EAP-Packet Request Notification");
+            }
+            else if( type == 4 )
+            {
+                zm_debug_msg0("EAP-Packet Request MD5-Challenge");
+            }
+            else if( type == 5 )
+            {
+                zm_debug_msg0("EAP-Packet Request One Time Password");
+            }
+            else if( type == 6 )
+            {
+                zm_debug_msg0("EAP-Packet Request Generic Token Card");
+            }
+            else if( type == 254 )
+            {
+                zm_debug_msg0("EAP-Packet Request Wi-Fi Protected Setup");
+
+                /* 0                   1                   2                   3   */
+                /* 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 6 7 8 9 0 1 */
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|     Type      |               Vendor-Id                       |*/
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|                          Vendor-Type                          |*/
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|              Vendor data...                                    */
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+                        */
+
+                /* EAP-Packet Vendor ID */
+                vendorId = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+9)) << 16) +
+                           (((u32_t) zmw_tx_buf_readb(dev, buf, offset+10)) << 8) +
+                           zmw_tx_buf_readb(dev, buf, offset+11);
+                /* EAP-Packet Vendor Type */
+                VendorType = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+12)) << 24) +
+                             (((u32_t) zmw_tx_buf_readb(dev, buf, offset+13)) << 16) +
+                             (((u32_t) zmw_tx_buf_readb(dev, buf, offset+14)) << 8) +
+                             zmw_tx_buf_readb(dev, buf, offset+15);
+                /* EAP-Packet Op Code */
+                Op_Code = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+16)) << 8) +
+                          zmw_tx_buf_readb(dev, buf, offset+17);
+                /* EAP-Packet Flags */
+                flags = zmw_tx_buf_readb(dev, buf, offset+18);
+
+                zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
+                zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
+                zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
+                zm_debug_msg1("EAP-Packet Flags = ", flags);
+            }
+        }
+        else if( code == 2 )
+        {
+            zm_debug_msg0("EAP-Packet Response");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_tx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_tx_buf_readb(dev, buf, offset+7);
+            /* EAP-Packet Type */
+            type = zmw_tx_buf_readb(dev, buf, offset+8);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+            zm_debug_msg1("EAP-Packet Type = ", type);
+
+            if( type == 1 )
+            {
+                zm_debug_msg0("EAP-Packet Response Identity");
+            }
+            else if( type == 2 )
+            {
+                zm_debug_msg0("EAP-Packet Request Notification");
+            }
+            else if( type == 3 )
+            {
+                zm_debug_msg0("EAP-Packet Request Nak");
+            }
+            else if( type == 4 )
+            {
+                zm_debug_msg0("EAP-Packet Request MD5-Challenge");
+            }
+            else if( type == 5 )
+            {
+                zm_debug_msg0("EAP-Packet Request One Time Password");
+            }
+            else if( type == 6 )
+            {
+                zm_debug_msg0("EAP-Packet Request Generic Token Card");
+            }
+            else if( type == 254 )
+            {
+                zm_debug_msg0("EAP-Packet Response Wi-Fi Protected Setup");
+
+                /* EAP-Packet Vendor ID */
+                vendorId = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+9)) << 16) +
+                           (((u32_t) zmw_tx_buf_readb(dev, buf, offset+10)) << 8) +
+                           zmw_tx_buf_readb(dev, buf, offset+11);
+                /* EAP-Packet Vendor Type */
+                VendorType = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+12)) << 24) +
+                             (((u32_t) zmw_tx_buf_readb(dev, buf, offset+13)) << 16) +
+                             (((u32_t) zmw_tx_buf_readb(dev, buf, offset+14)) << 8) +
+                             zmw_tx_buf_readb(dev, buf, offset+15);
+                /* EAP-Packet Op Code */
+                Op_Code = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+16)) << 8) +
+                          zmw_tx_buf_readb(dev, buf, offset+17);
+                /* EAP-Packet Flags */
+                flags = zmw_tx_buf_readb(dev, buf, offset+18);
+
+                zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
+                zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
+                zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
+                zm_debug_msg1("EAP-Packet Flags = ", flags);
+            }
+        }
+        else if( code == 3 )
+        {
+            zm_debug_msg0("EAP-Packet Success");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_rx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_rx_buf_readb(dev, buf, offset+7);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+        }
+        else if( code == 4 )
+        {
+            zm_debug_msg0("EAP-Packet Failure");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_tx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_tx_buf_readb(dev, buf, offset+7);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+        }
+    }
+    else if( packetType == 1 )
+    { // EAPOL-Start
+        zm_debug_msg0("EAPOL-Start");
+    }
+    else if( packetType == 2 )
+    { // EAPOL-Logoff
+        zm_debug_msg0("EAPOL-Logoff");
+    }
+    else if( packetType == 3 )
+    { // EAPOL-Key
+        /* EAPOL-Key type */
+        keyType = zmw_tx_buf_readb(dev, buf, offset+4);
+        /* EAPOL-Key information */
+        keyInfo = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+5)) << 8) +
+                  zmw_tx_buf_readb(dev, buf, offset+6);
+        /* EAPOL-Key length */
+        keyLen = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+7)) << 8) +
+                 zmw_tx_buf_readb(dev, buf, offset+8);
+        /* EAPOL-Key replay counter (high double word) */
+        replayCounterH = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+9)) << 24) +
+                         (((u32_t) zmw_tx_buf_readb(dev, buf, offset+10)) << 16) +
+                         (((u32_t) zmw_tx_buf_readb(dev, buf, offset+11)) << 8) +
+                         zmw_tx_buf_readb(dev, buf, offset+12);
+        /* EAPOL-Key replay counter (low double word) */
+        replayCounterL = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+13)) << 24) +
+                         (((u32_t) zmw_tx_buf_readb(dev, buf, offset+14)) << 16) +
+                         (((u32_t) zmw_tx_buf_readb(dev, buf, offset+15)) << 8) +
+                         zmw_tx_buf_readb(dev, buf, offset+16);
+        /* EAPOL-Key data length */
+        keyDataLen = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+97)) << 8) +
+                     zmw_tx_buf_readb(dev, buf, offset+98);
+
+        zm_debug_msg0("EAPOL-Key");
+        zm_debug_msg1("packet length = ", packetLen);
+
+        if ( keyType == 254 )
+        {
+            zm_debug_msg0("key type = 254 (SSN key descriptor)");
+        }
+        else
+        {
+            zm_debug_msg2("key type = 0x", keyType);
+        }
+
+        zm_debug_msg2("replay counter(L) = ", replayCounterL);
+
+        zm_debug_msg2("key information = ", keyInfo);
+
+        if ( keyInfo & ZM_BIT_3 )
+        {
+            zm_debug_msg0("    - pairwise key");
+        }
+        else
+        {
+            zm_debug_msg0("    - group key");
+        }
+
+        if ( keyInfo & ZM_BIT_6 )
+        {
+            zm_debug_msg0("    - Tx key installed");
+        }
+        else
+        {
+            zm_debug_msg0("    - Tx key not set");
+        }
+
+        if ( keyInfo & ZM_BIT_7 )
+        {
+            zm_debug_msg0("    - Ack needed");
+        }
+        else
+        {
+            zm_debug_msg0("    - Ack not needed");
+        }
+
+        if ( keyInfo & ZM_BIT_8 )
+        {
+            zm_debug_msg0("    - MIC set");
+        }
+        else
+        {
+            zm_debug_msg0("    - MIC not set");
+        }
+
+        if ( keyInfo & ZM_BIT_9 )
+        {
+            zm_debug_msg0("    - packet encrypted");
+        }
+        else
+        {
+            zm_debug_msg0("    - packet not encrypted");
+        }
+
+        zm_debug_msg1("keyLen = ", keyLen);
+        zm_debug_msg1("keyDataLen = ", keyDataLen);
+    }
+    else if( packetType == 4 )
+    {
+        zm_debug_msg0("EAPOL-Encapsulated-ASF-Alert");
+    }
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiRecv80211                */
+/*      Called to receive 802.11 frame.                                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : received 802.11 frame buffer.                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.5      */
+/*                                                                      */
+/************************************************************************/
+void zfiRecv80211(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo)
+{
+    u8_t snapCase=0, encryMode;
+    u16_t frameType, typeLengthField;
+    u16_t frameCtrl;
+    u16_t frameSubtype;
+    u16_t ret;
+    u16_t len;
+    u8_t bIsDefrag = 0;
+    u16_t offset, tailLen;
+    u8_t vap = 0;
+    u16_t da[3], sa[3];
+    u16_t ii;
+    u8_t uapsdTrig = 0;
+    zbuf_t* psBuf;
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    u8_t i;
+#endif
+
+    zmw_get_wlan_dev(dev);
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    //zm_msg2_rx(ZM_LV_2, "zfiRecv80211(), buf=", buf);
+
+    //zm_msg2_rx(ZM_LV_0, "h[0]=", zmw_rx_buf_readh(dev, buf, 0));
+    //zm_msg2_rx(ZM_LV_0, "h[2]=", zmw_rx_buf_readh(dev, buf, 2));
+    //zm_msg2_rx(ZM_LV_0, "h[4]=", zmw_rx_buf_readh(dev, buf, 4));
+
+    frameCtrl = zmw_rx_buf_readb(dev, buf, 0);
+    frameType = frameCtrl & 0xf;
+    frameSubtype = frameCtrl & 0xf0;
+
+#if 0   // Move to ProcessBeacon to judge if there's a new peer station
+    if ( (wd->wlanMode == ZM_MODE_IBSS)&&
+         (wd->sta.ibssPartnerStatus != ZM_IBSS_PARTNER_ALIVE) )
+    {
+        zfStaIbssMonitoring(dev, buf);
+    }
+#endif
+
+    /* If data frame */
+    if (frameType == ZM_WLAN_DATA_FRAME)
+    {
+        wd->sta.TotalNumberOfReceivePackets++;
+        wd->sta.TotalNumberOfReceiveBytes += zfwBufGetSize(dev, buf);
+        //zm_debug_msg1("Receive packets     = ", wd->sta.TotalNumberOfReceivePackets);
+
+        //zm_msg0_rx(ZM_LV_0, "Rx data");
+        if (wd->wlanMode == ZM_MODE_AP)
+        {
+            if ((ret = zfApUpdatePsBit(dev, buf, &vap, &uapsdTrig)) != ZM_SUCCESS)
+            {
+                zfwBufFree(dev, buf, 0);
+                return;
+            }
+
+            if (((uapsdTrig&0xf) != 0) && ((frameSubtype & 0x80) != 0))
+            {
+                u8_t ac = zcUpToAc[zmw_buf_readb(dev, buf, 24)&0x7];
+                u8_t pktNum;
+                u8_t mb;
+                u16_t flag;
+                u8_t src[6];
+
+                //printk("QoS ctrl=%d\n", zmw_buf_readb(dev, buf, 24));
+                //printk("UAPSD trigger, ac=%d\n", ac);
+
+                if (((0x8>>ac) & uapsdTrig) != 0)
+                {
+                    pktNum = zcMaxspToPktNum[(uapsdTrig>>4) & 0x3];
+
+                    for (ii=0; ii<6; ii++)
+                    {
+                        src[ii] = zmw_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+ii);
+                    }
+
+                    for (ii=0; ii<pktNum; ii++)
+                    {
+                        //if ((psBuf = zfQueueGet(dev, wd->ap.uapsdQ)) != NULL)
+                        if ((psBuf = zfQueueGetWithMac(dev, wd->ap.uapsdQ, src, &mb)) != NULL)
+                        {
+                            if ((ii+1) == pktNum)
+                            {
+                                //EOSP anyway
+                                flag = 0x100 | (mb<<5);
+                            }
+                            else
+                            {
+                                if (mb != 0)
+                                {
+                                    //more data, not EOSP
+                                    flag = 0x20;
+                                }
+                                else
+                                {
+                                    //no more data, EOSP
+                                    flag = 0x100;
+                                }
+                            }
+                            zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF, flag);
+                        }
+
+                        if ((psBuf == NULL) || (mb == 0))
+                        {
+                            if ((ii == 0) && (psBuf == NULL))
+                            {
+                                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_QOS_NULL, (u16_t*)src, 0, 0, 0);
+                            }
+                            break;
+                        }
+                    }
+                }
+            }
+
+        }
+        else if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+        {
+            u16_t frameCtrlMSB;
+    		u8_t   bssid[6];
+
+            /* Check Is RIFS frame and decide to enable RIFS or not */
+            if( wd->sta.EnableHT )
+                zfCheckIsRIFSFrame(dev, buf, frameSubtype);
+
+            if ( zfPowerSavingMgrIsSleeping(dev) || wd->sta.psMgr.tempWakeUp == 1)
+            {
+                frameCtrlMSB = zmw_rx_buf_readb(dev, buf, 1);
+
+                /* check more data */
+                if ( frameCtrlMSB & ZM_BIT_5 )
+                {
+                    //if rx frame's AC is not delivery-enabled
+                    if ((wd->sta.qosInfo&0xf) != 0xf)
+                    {
+                        u8_t rxAc = 0;
+                        if ((frameSubtype & 0x80) != 0)
+                        {
+                            rxAc = zcUpToAc[zmw_buf_readb(dev, buf, 24)&0x7];
+                        }
+
+                        if (((0x8>>rxAc) & wd->sta.qosInfo) == 0)
+                        {
+                            zfSendPSPoll(dev);
+                            wd->sta.psMgr.tempWakeUp = 0;
+                        }
+                    }
+                }
+            }
+			/*increase beacon count when receive vaild data frame from AP*/
+        	ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, bssid);
+
+			if (zfStaIsConnected(dev)&&
+				zfRxBufferEqualToStr(dev, buf, bssid, ZM_WLAN_HEADER_A2_OFFSET, 6))
+			{
+                wd->sta.rxBeaconCount++;
+			}
+        }
+
+        zm_msg1_rx(ZM_LV_2, "Rx VAP=", vap);
+
+        /* handle IV, EXT-IV, ICV, and EXT-ICV */
+        zfGetRxIvIcvLength(dev, buf, vap, &offset, &tailLen, addInfo);
+
+        zfStaIbssPSCheckState(dev, buf);
+        //QoS data frame
+        if ((frameSubtype & 0x80) == 0x80)
+        {
+            offset += 2;
+        }
+
+        len = zfwBufGetSize(dev, buf);
+        /* remove ICV */
+        if (tailLen > 0)
+        {
+            if (len > tailLen)
+            {
+                len -= tailLen;
+                zfwBufSetSize(dev, buf, len);
+            }
+        }
+
+        /* Filter NULL data */
+        if (((frameSubtype&0x40) != 0) || ((len = zfwBufGetSize(dev, buf))<=24))
+        {
+            zm_msg1_rx(ZM_LV_1, "Free Rx NULL data, len=", len);
+            zfwBufFree(dev, buf, 0);
+            return;
+        }
+
+        /* check and handle defragmentation */
+        if ( wd->sta.bSafeMode && (wd->sta.wepStatus == ZM_ENCRYPTION_AES) && wd->sta.SWEncryptEnable )
+        {
+            zm_msg0_rx(ZM_LV_1, "Bypass defragmentation packets in safe mode");
+        }
+        else
+        {
+            if ( (buf = zfDefragment(dev, buf, &bIsDefrag, addInfo)) == NULL )
+            {
+                /* In this case, the buffer has been freed in zfDefragment */
+                return;
+            }
+        }
+
+        ret = ZM_MIC_SUCCESS;
+
+        /* If SW WEP/TKIP are not turned on */
+        if ((wd->sta.SWEncryptEnable & ZM_SW_TKIP_DECRY_EN) == 0 &&
+            (wd->sta.SWEncryptEnable & ZM_SW_WEP_DECRY_EN) == 0)
+        {
+            encryMode = zfGetEncryModeFromRxStatus(addInfo);
+
+            /* check if TKIP */
+            if ( encryMode == ZM_TKIP )
+            {
+                if ( bIsDefrag )
+                {
+                    ret = zfMicRxVerify(dev, buf);
+                }
+                else
+                {
+                    /* check MIC failure bit */
+                    if ( ZM_RX_STATUS_IS_MIC_FAIL(addInfo) )
+                    {
+                        ret = ZM_MIC_FAILURE;
+                    }
+                }
+
+                if ( ret == ZM_MIC_FAILURE )
+                {
+                    u8_t Unicast_Pkt = 0x0;
+
+                    if ((zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) & 0x1) == 0)
+                    {
+                        wd->commTally.swRxUnicastMicFailCount++;
+                        Unicast_Pkt = 0x1;
+                    }/*
+                    else if (zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) == 0xffff)
+                    {
+                        wd->commTally.swRxMulticastMicFailCount++;
+                    }*/
+                    else
+                    {
+                        wd->commTally.swRxMulticastMicFailCount++;
+                    }
+                    if ( wd->wlanMode == ZM_MODE_AP )
+                    {
+                        u16_t idx;
+                        u8_t addr[6];
+
+                        for (idx=0; idx<6; idx++)
+                        {
+                            addr[idx] = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+idx);
+                        }
+
+                        if (wd->zfcbApMicFailureNotify != NULL)
+                        {
+                            wd->zfcbApMicFailureNotify(dev, addr, buf);
+                        }
+                    }
+                    else
+                    {
+                        if(Unicast_Pkt)
+                        {
+                            zm_debug_msg0("Countermeasure : Unicast_Pkt ");
+                        }
+                        else
+                        {
+                            zm_debug_msg0("Countermeasure : Non-Unicast_Pkt ");
+                        }
+
+                        if((wd->TKIP_Group_KeyChanging == 0x0) || (Unicast_Pkt == 0x1))
+                        {
+                            zm_debug_msg0("Countermeasure : Do MIC Check ");
+                            zfStaMicFailureHandling(dev, buf);
+                        }
+                        else
+                        {
+                            zm_debug_msg0("Countermeasure : SKIP MIC Check due to Group Keychanging ");
+                        }
+                    }
+                    /* Discard MIC failed frame */
+                    zfwBufFree(dev, buf, 0);
+                    return;
+                }
+            }
+        }
+        else
+        {
+            u8_t IsEncryFrame;
+
+            /* TODO: Check whether WEP bit is turned on in MAC header */
+            encryMode = ZM_NO_WEP;
+
+            IsEncryFrame = (zmw_rx_buf_readb(dev, buf, 1) & 0x40);
+
+            if (IsEncryFrame)
+            {
+                /* Software decryption for TKIP */
+                if (wd->sta.SWEncryptEnable & ZM_SW_TKIP_DECRY_EN)
+                {
+                    u16_t iv16;
+                    u16_t iv32;
+                    u8_t RC4Key[16];
+                    u16_t IvOffset;
+                    struct zsTkipSeed *rxSeed;
+
+                    IvOffset = offset + ZM_SIZE_OF_WLAN_DATA_HEADER;
+
+                    rxSeed = zfStaGetRxSeed(dev, buf);
+
+                    if (rxSeed == NULL)
+                    {
+                        zm_debug_msg0("rxSeed is NULL");
+
+                        /* Discard this frame */
+                        zfwBufFree(dev, buf, 0);
+                        return;
+                    }
+
+                    iv16 = (zmw_rx_buf_readb(dev, buf, IvOffset) << 8) + zmw_rx_buf_readb(dev, buf, IvOffset+2);
+                    iv32 = zmw_rx_buf_readb(dev, buf, IvOffset+4) +
+                           (zmw_rx_buf_readb(dev, buf, IvOffset+5) << 8) +
+                           (zmw_rx_buf_readb(dev, buf, IvOffset+6) << 16) +
+                           (zmw_rx_buf_readb(dev, buf, IvOffset+7) << 24);
+
+                    /* TKIP Key Mixing */
+                    zfTkipPhase1KeyMix(iv32, rxSeed);
+                    zfTkipPhase2KeyMix(iv16, rxSeed);
+                    zfTkipGetseeds(iv16, RC4Key, rxSeed);
+
+                    /* Decrypt Data */
+                    ret = zfTKIPDecrypt(dev, buf, IvOffset+ZM_SIZE_OF_IV+ZM_SIZE_OF_EXT_IV, 16, RC4Key);
+
+                    if (ret == ZM_ICV_FAILURE)
+                    {
+                        zm_debug_msg0("TKIP ICV fail");
+
+                        /* Discard ICV failed frame */
+                        zfwBufFree(dev, buf, 0);
+                        return;
+                    }
+
+                    /* Remove ICV from buffer */
+                    zfwBufSetSize(dev, buf, len-4);
+
+                    /* Check MIC */
+                    ret = zfMicRxVerify(dev, buf);
+
+                    if (ret == ZM_MIC_FAILURE)
+                    {
+                        if ((zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) & 0x1) == 0)
+                        {
+                            wd->commTally.swRxUnicastMicFailCount++;
+                        }
+                        else if (zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) == 0xffff)
+                        {
+                            wd->commTally.swRxMulticastMicFailCount++;
+                        }
+                        else
+                        {
+                            wd->commTally.swRxMulticastMicFailCount++;
+                        }
+                        if ( wd->wlanMode == ZM_MODE_AP )
+                        {
+                            u16_t idx;
+                            u8_t addr[6];
+
+                            for (idx=0; idx<6; idx++)
+                            {
+                                addr[idx] = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+idx);
+                            }
+
+                            if (wd->zfcbApMicFailureNotify != NULL)
+                            {
+                                wd->zfcbApMicFailureNotify(dev, addr, buf);
+                            }
+                        }
+                        else
+                        {
+                            zfStaMicFailureHandling(dev, buf);
+                        }
+
+                        zm_debug_msg0("MIC fail");
+                        /* Discard MIC failed frame */
+                        zfwBufFree(dev, buf, 0);
+                        return;
+                    }
+
+                    encryMode = ZM_TKIP;
+                    offset += ZM_SIZE_OF_IV + ZM_SIZE_OF_EXT_IV;
+                }
+                else if(wd->sta.SWEncryptEnable & ZM_SW_WEP_DECRY_EN)
+                {
+                    u16_t IvOffset;
+                    u8_t keyLen = 5;
+                    u8_t iv[3];
+                    u8_t *wepKey;
+                    u8_t keyIdx;
+
+                    IvOffset = offset + ZM_SIZE_OF_WLAN_DATA_HEADER;
+
+                    /* Retrieve IV */
+                    iv[0] = zmw_rx_buf_readb(dev, buf, IvOffset);
+                    iv[1] = zmw_rx_buf_readb(dev, buf, IvOffset+1);
+                    iv[2] = zmw_rx_buf_readb(dev, buf, IvOffset+2);
+
+                    keyIdx = ((zmw_rx_buf_readb(dev, buf, IvOffset+3) >> 6) & 0x03);
+
+                    IvOffset += ZM_SIZE_OF_IV;
+
+                    if (wd->sta.SWEncryMode[keyIdx] == ZM_WEP64)
+                    {
+                        keyLen = 5;
+                    }
+                    else if (wd->sta.SWEncryMode[keyIdx] == ZM_WEP128)
+                    {
+                        keyLen = 13;
+                    }
+                    else if (wd->sta.SWEncryMode[keyIdx] == ZM_WEP256)
+                    {
+                        keyLen = 29;
+                    }
+
+                    zfWEPDecrypt(dev, buf, IvOffset, keyLen, wd->sta.wepKey[keyIdx], iv);
+
+                    if (ret == ZM_ICV_FAILURE)
+                    {
+                        zm_debug_msg0("WEP ICV fail");
+
+                        /* Discard ICV failed frame */
+                        zfwBufFree(dev, buf, 0);
+                        return;
+                    }
+
+                    encryMode = wd->sta.SWEncryMode[keyIdx];
+
+                    /* Remove ICV from buffer */
+                    zfwBufSetSize(dev, buf, len-4);
+
+                    offset += ZM_SIZE_OF_IV;
+                }
+            }
+        }
+
+#ifdef ZM_ENABLE_CENC
+        //else if ( encryMode == ZM_CENC ) /* check if CENC */
+        if ( encryMode == ZM_CENC )
+        {
+            u32_t rxIV[4];
+
+            rxIV[0] = (zmw_rx_buf_readh(dev, buf, 28) << 16)
+                     + zmw_rx_buf_readh(dev, buf, 26);
+            rxIV[1] = (zmw_rx_buf_readh(dev, buf, 32) << 16)
+                     + zmw_rx_buf_readh(dev, buf, 30);
+            rxIV[2] = (zmw_rx_buf_readh(dev, buf, 36) << 16)
+                     + zmw_rx_buf_readh(dev, buf, 34);
+            rxIV[3] = (zmw_rx_buf_readh(dev, buf, 40) << 16)
+                     + zmw_rx_buf_readh(dev, buf, 38);
+
+            //zm_debug_msg2("rxIV[0] = 0x", rxIV[0]);
+            //zm_debug_msg2("rxIV[1] = 0x", rxIV[1]);
+            //zm_debug_msg2("rxIV[2] = 0x", rxIV[2]);
+            //zm_debug_msg2("rxIV[3] = 0x", rxIV[3]);
+
+            /* destination address*/
+            da[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+            da[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+2);
+            da[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+4);
+
+            if ( wd->wlanMode == ZM_MODE_AP )
+            {
+            }
+            else
+            {
+                if ((da[0] & 0x1))
+                { //multicast frame
+                    /* Accumlate the PN sequence */
+                    wd->sta.rxivGK[0] ++;
+
+                    if (wd->sta.rxivGK[0] == 0)
+                    {
+                        wd->sta.rxivGK[1]++;
+                    }
+
+                    if (wd->sta.rxivGK[1] == 0)
+                    {
+                        wd->sta.rxivGK[2]++;
+                    }
+
+                    if (wd->sta.rxivGK[2] == 0)
+                    {
+                        wd->sta.rxivGK[3]++;
+                    }
+
+                    if (wd->sta.rxivGK[3] == 0)
+                    {
+                        wd->sta.rxivGK[0] = 0;
+                        wd->sta.rxivGK[1] = 0;
+                        wd->sta.rxivGK[2] = 0;
+                    }
+
+                    //zm_debug_msg2("wd->sta.rxivGK[0] = 0x", wd->sta.rxivGK[0]);
+                    //zm_debug_msg2("wd->sta.rxivGK[1] = 0x", wd->sta.rxivGK[1]);
+                    //zm_debug_msg2("wd->sta.rxivGK[2] = 0x", wd->sta.rxivGK[2]);
+                    //zm_debug_msg2("wd->sta.rxivGK[3] = 0x", wd->sta.rxivGK[3]);
+
+                    if ( !((wd->sta.rxivGK[0] == rxIV[0])
+                        && (wd->sta.rxivGK[1] == rxIV[1])
+                        && (wd->sta.rxivGK[2] == rxIV[2])
+                        && (wd->sta.rxivGK[3] == rxIV[3])))
+                    {
+                        u8_t PacketDiscard = 0;
+                        /* Discard PN Code Error frame */
+                        if (rxIV[0] < wd->sta.rxivGK[0])
+                        {
+                            PacketDiscard = 1;
+                        }
+                        if (wd->sta.rxivGK[0] > 0xfffffff0)
+                        { //boundary case
+                            if ((rxIV[0] < 0xfffffff0)
+                                && (((0xffffffff - wd->sta.rxivGK[0]) + rxIV[0]) > 16))
+                            {
+                                PacketDiscard = 1;
+                            }
+                        }
+                        else
+                        { //normal case
+                            if ((rxIV[0] - wd->sta.rxivGK[0]) > 16)
+                            {
+                                PacketDiscard = 1;
+                            }
+                        }
+                        // sync sta pn code with ap because of losting some packets
+                        wd->sta.rxivGK[0] = rxIV[0];
+                        wd->sta.rxivGK[1] = rxIV[1];
+                        wd->sta.rxivGK[2] = rxIV[2];
+                        wd->sta.rxivGK[3] = rxIV[3];
+                        if (PacketDiscard)
+                        {
+                            zm_debug_msg0("Discard PN Code lost too much multicast frame");
+                            zfwBufFree(dev, buf, 0);
+                            return;
+                        }
+                    }
+                }
+                else
+                { //unicast frame
+                    /* Accumlate the PN sequence */
+                    wd->sta.rxiv[0] += 2;
+
+                    if (wd->sta.rxiv[0] == 0 || wd->sta.rxiv[0] == 1)
+                    {
+                        wd->sta.rxiv[1]++;
+                    }
+
+                    if (wd->sta.rxiv[1] == 0)
+                    {
+                        wd->sta.rxiv[2]++;
+                    }
+
+                    if (wd->sta.rxiv[2] == 0)
+                    {
+                        wd->sta.rxiv[3]++;
+                    }
+
+                    if (wd->sta.rxiv[3] == 0)
+                    {
+                        wd->sta.rxiv[0] = 0;
+                        wd->sta.rxiv[1] = 0;
+                        wd->sta.rxiv[2] = 0;
+                    }
+
+                    //zm_debug_msg2("wd->sta.rxiv[0] = 0x", wd->sta.rxiv[0]);
+                    //zm_debug_msg2("wd->sta.rxiv[1] = 0x", wd->sta.rxiv[1]);
+                    //zm_debug_msg2("wd->sta.rxiv[2] = 0x", wd->sta.rxiv[2]);
+                    //zm_debug_msg2("wd->sta.rxiv[3] = 0x", wd->sta.rxiv[3]);
+
+                    if ( !((wd->sta.rxiv[0] == rxIV[0])
+                        && (wd->sta.rxiv[1] == rxIV[1])
+                        && (wd->sta.rxiv[2] == rxIV[2])
+                        && (wd->sta.rxiv[3] == rxIV[3])))
+                    {
+                        zm_debug_msg0("PN Code mismatch, lost unicast frame, sync pn code to recv packet");
+                        // sync sta pn code with ap because of losting some packets
+                        wd->sta.rxiv[0] = rxIV[0];
+                        wd->sta.rxiv[1] = rxIV[1];
+                        wd->sta.rxiv[2] = rxIV[2];
+                        wd->sta.rxiv[3] = rxIV[3];
+                        /* Discard PN Code Error frame */
+                        //zm_debug_msg0("Discard PN Code mismatch unicast frame");
+                        //zfwBufFree(dev, buf, 0);
+                        //return;
+                    }
+                }
+            }
+        }
+#endif //ZM_ENABLE_CENC
+
+        /* for tally */
+        if ((zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) & 0x1) == 0)
+        {
+            /* for ACU to display RxRate */
+            zfWlanUpdateRxRate(dev, addInfo);
+
+            wd->commTally.rxUnicastFrm++;
+            wd->commTally.rxUnicastOctets += (len-24);
+        }
+        else if (zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) == 0xffff)
+        {
+            wd->commTally.rxBroadcastFrm++;
+            wd->commTally.rxBroadcastOctets += (len-24);
+        }
+        else
+        {
+            wd->commTally.rxMulticastFrm++;
+            wd->commTally.rxMulticastOctets += (len-24);
+        }
+        wd->ledStruct.rxTraffic++;
+
+        if ((frameSubtype & 0x80) == 0x80)
+        {
+            /* if QoS control bit-7 is 1 => A-MSDU frame */
+            if ((zmw_rx_buf_readh(dev, buf, 24) & 0x80) != 0)
+            {
+                zfDeAmsdu(dev, buf, vap, encryMode);
+                return;
+            }
+        }
+
+        // Remove MIC of TKIP
+        if ( encryMode == ZM_TKIP )
+        {
+            zfwBufSetSize(dev, buf, zfwBufGetSize(dev, buf) - 8);
+        }
+
+        /* Convert 802.11 and SNAP header to ethernet header */
+        if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)||
+             (wd->wlanMode == ZM_MODE_IBSS) )
+        {
+            /* destination address*/
+            da[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+            da[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+2);
+            da[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+4);
+
+            /* check broadcast frame */
+            if ( (da[0] == 0xffff) && (da[1] == 0xffff) && (da[2] == 0xffff) )
+            {
+                // Ap send broadcast frame to the DUT !
+            }
+            /* check multicast frame */
+            /* TODO : Remove these code, hardware should be able to block */
+            /*        multicast frame on the multicast address list       */
+            /*        or bypass all multicast packet by flag bAllMulticast */
+            else if ((da[0] & 0x01) && (wd->sta.bAllMulticast == 0))
+            {
+                for(ii=0; ii<wd->sta.multicastList.size; ii++)
+                {
+                    if ( zfMemoryIsEqual(wd->sta.multicastList.macAddr[ii].addr,
+                                         (u8_t*) da, 6))
+                    {
+                        break;
+                    }
+                }
+
+                if ( ii == wd->sta.multicastList.size )
+                {   /* not found */
+                    zm_debug_msg0("discard unknown multicast frame");
+
+                    zfwBufFree(dev, buf, 0);
+                    return;
+                }
+            }
+
+#ifdef ZM_ENABLE_NATIVE_WIFI //Native Wifi : 1, Ethernet format : 0
+            //To remove IV
+            if (offset > 0)
+            {
+                for (i=12; i>0; i--)
+                {
+                    zmw_rx_buf_writeh(dev, buf, ((i-1)*2)+offset,
+                            zmw_rx_buf_readh(dev, buf, (i-1)*2));
+                }
+                zfwBufRemoveHead(dev, buf, offset);
+            }
+#else
+
+            if (zfRxBufferEqualToStr(dev, buf, zgSnapBridgeTunnel,
+                                     24+offset, 6))
+            {
+                snapCase = 1;
+            }
+            else if ( zfRxBufferEqualToStr(dev, buf, zgSnap8021h,
+                                           24+offset, 6) )
+            {
+                typeLengthField =
+                    (((u16_t) zmw_rx_buf_readb(dev, buf, 30+offset)) << 8) +
+                    zmw_rx_buf_readb(dev, buf, 31+offset);
+
+                //zm_debug_msg2("tpyeLengthField = ", typeLengthField);
+
+                //8137 : IPX, 80F3 : Appletalk
+                if ( (typeLengthField != 0x8137)&&
+                     (typeLengthField != 0x80F3) )
+                {
+                    snapCase = 2;
+                }
+
+                if ( typeLengthField == 0x888E )
+                {
+                    zfShowRxEAPOL(dev, buf, 32);
+                }
+            }
+            else
+            {
+                //zfwDumpBuf(dev, buf);
+            }
+
+            /* source address */
+            if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+            {
+                /* SA = Address 3 */
+                sa[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET);
+                sa[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+2);
+                sa[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+4);
+            }
+            else
+            {
+                /* SA = Address 2 */
+                sa[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+                sa[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+2);
+                sa[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+4);
+            }
+
+            if ( snapCase )
+            {
+                /* SA */
+                zmw_rx_buf_writeh(dev, buf, 24+offset, sa[0]);
+                zmw_rx_buf_writeh(dev, buf, 26+offset, sa[1]);
+                zmw_rx_buf_writeh(dev, buf, 28+offset, sa[2]);
+
+                /* DA = Address 1 */
+                zmw_rx_buf_writeh(dev, buf, 18+offset, da[0]);
+                zmw_rx_buf_writeh(dev, buf, 20+offset, da[1]);
+                zmw_rx_buf_writeh(dev, buf, 22+offset, da[2]);
+                zfwBufRemoveHead(dev, buf, 18+offset);
+            }
+            else
+            {
+                /* SA */
+                zmw_rx_buf_writeh(dev, buf, 16+offset, sa[0]);
+                zmw_rx_buf_writeh(dev, buf, 18+offset, sa[1]);
+                zmw_rx_buf_writeh(dev, buf, 20+offset, sa[2]);
+
+                /* DA = Address 1 */
+                zmw_rx_buf_writeh(dev, buf, 10+offset, da[0]);
+                zmw_rx_buf_writeh(dev, buf, 12+offset, da[1]);
+                zmw_rx_buf_writeh(dev, buf, 14+offset, da[2]);
+                zfwBufRemoveHead(dev, buf, 10+offset);
+                /* Ethernet payload length */
+                typeLengthField = zfwBufGetSize(dev, buf) - 14;
+                zmw_rx_buf_writeh(dev, buf, 12, (typeLengthField<<8)+(typeLengthField>>8));
+            }
+#endif  // ZM_ENABLE_NATIVE_WIFI
+        }
+        else if (wd->wlanMode == ZM_MODE_AP)
+        {
+            //if ((zmw_rx_buf_readb(dev, buf, 1) & 0x3) != 3)
+            if (vap < ZM_MAX_AP_SUPPORT)
+            /* AP mode */
+            {
+#ifdef ZM_ENABLE_NATIVE_WIFI //Native Wifi : 1, Ethernet format : 0
+                //To remove IV
+                if (offset > 0)
+                {
+                    for (i=12; i>0; i--)
+                    {
+                        zmw_rx_buf_writeh(dev, buf, ((i-1)*2)+offset,
+                                zmw_rx_buf_readh(dev, buf, (i-1)*2));
+                    }
+                    zfwBufRemoveHead(dev, buf, offset);
+                }
+#else
+                /* SA = Address 2 */
+                zmw_rx_buf_writeh(dev, buf, 24+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A2_OFFSET));
+                zmw_rx_buf_writeh(dev, buf, 26+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A2_OFFSET+2));
+                zmw_rx_buf_writeh(dev, buf, 28+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A2_OFFSET+4));
+                /* DA = Address 3 */
+                /* Seq : Read 20 write 22, read 18 write 20, read 16 write 18 */
+                /* sequence must not be inverted */
+                zmw_rx_buf_writeh(dev, buf, 22+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET+4));
+                zmw_rx_buf_writeh(dev, buf, 20+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET+2));
+                zmw_rx_buf_writeh(dev, buf, 18+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET));
+                zfwBufRemoveHead(dev, buf, 18+offset);
+#endif  // ZM_ENABLE_NATIVE_WIFI
+                #if 1
+                if ((ret = zfIntrabssForward(dev, buf, vap)) == 1)
+                {
+                    /* Free Rx buffer if intra-BSS unicast frame */
+                    zm_msg0_rx(ZM_LV_2, "Free intra-BSS unicast frame");
+                    zfwBufFree(dev, buf, 0);
+                    return;
+                }
+                #endif
+            }
+            else
+            /* WDS mode */
+            {
+                zm_msg0_rx(ZM_LV_2, "Rx WDS data");
+
+                /* SA = Address 4 */
+                zmw_rx_buf_writeh(dev, buf, 30+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A4_OFFSET));
+                zmw_rx_buf_writeh(dev, buf, 32+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A4_OFFSET+2));
+                zmw_rx_buf_writeh(dev, buf, 34+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A4_OFFSET+4));
+                /* DA = Address 3 */
+                /* Seq : Read 20 write 22, read 18 write 20, read 16 write 18 */
+                /* sequence must not be inverted */
+                zmw_rx_buf_writeh(dev, buf, 28+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET+4));
+                zmw_rx_buf_writeh(dev, buf, 26+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET+2));
+                zmw_rx_buf_writeh(dev, buf, 24+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET));
+                zfwBufRemoveHead(dev, buf, 24+offset);
+            }
+        }
+        else if (wd->wlanMode == ZM_MODE_PSEUDO)
+        {
+			/* WDS test: remove add4 */
+            if (wd->enableWDS)
+            {
+                offset += 6;
+            }
+
+            /* SA = Address 2 */
+            zmw_rx_buf_writeh(dev, buf, 24+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A2_OFFSET));
+            zmw_rx_buf_writeh(dev, buf, 26+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A2_OFFSET+2));
+            zmw_rx_buf_writeh(dev, buf, 28+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A2_OFFSET+4));
+            /* DA = Address 1 */
+            zmw_rx_buf_writeh(dev, buf, 18+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A1_OFFSET));
+            zmw_rx_buf_writeh(dev, buf, 20+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A1_OFFSET+2));
+            zmw_rx_buf_writeh(dev, buf, 22+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A1_OFFSET+4));
+            zfwBufRemoveHead(dev, buf, 18+offset);
+        }
+        else
+        {
+            zm_assert(0);
+        }
+
+        /* Call zfwRecvEth() to notify upper layer */
+        //zm_msg2_rx(ZM_LV_2, "Call zfwRecvEth(), buf=", buf);
+        //zfwDumpBuf(dev, buf);
+
+        #if ZM_PROTOCOL_RESPONSE_SIMULATION == 1
+        zfProtRspSim(dev, buf);
+        #endif
+        //zfwDumpBuf(dev, buf);
+
+        /* tally */
+    	wd->commTally.NotifyNDISRxFrmCnt++;
+
+    	if (wd->zfcbRecvEth != NULL)
+    	{
+            wd->zfcbRecvEth(dev, buf, vap);
+            ZM_PERFORMANCE_RX_MSDU(dev, wd->tick)
+        }
+    }
+    /* if management frame */
+    else if (frameType == ZM_WLAN_MANAGEMENT_FRAME)
+    {
+        zm_msg2_rx(ZM_LV_2, "Rx management,FC=", frameCtrl);
+        /* Call zfProcessManagement() to handle management frame */
+        zfProcessManagement(dev, buf, addInfo); //CWYang(m)
+        zfwBufFree(dev, buf, 0);
+    }
+    /* PsPoll */
+    else if ((wd->wlanMode == ZM_MODE_AP) && (frameCtrl == 0xa4))
+    {
+        zm_msg0_rx(ZM_LV_0, "Rx PsPoll");
+        zfApProcessPsPoll(dev, buf);
+        zfwBufFree(dev, buf, 0);
+    }
+    else
+    {
+        zm_msg0_rx(ZM_LV_1, "Rx discard!!");
+        wd->commTally.DriverDiscardedFrm++;
+
+        zfwBufFree(dev, buf, 0);
+    }
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfWlanRxValidate            */
+/*      Validate Rx frame.                                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : received 802.11 frame buffer.                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfWlanRxValidate(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t frameType;
+    u16_t frameCtrl;
+    u16_t frameLen;
+    u16_t ret;
+    u8_t  frameSubType;
+
+    zmw_get_wlan_dev(dev);
+
+    frameCtrl = zmw_rx_buf_readh(dev, buf, 0);
+    frameType = frameCtrl & 0xC;
+    frameSubType = (frameCtrl & 0xF0) >> 4;
+
+    frameLen = zfwBufGetSize(dev, buf);
+
+    /* Accept Data/Management frame with protocol version = 0 */
+    if ((frameType == 0x8) || (frameType == 0x0))
+    {
+
+        /* TODO : check rx status => erro bit */
+
+        /* Check Minimum Length with Wep */
+        if ((frameCtrl & 0x4000) != 0)
+        {
+            /* Minimum Length =                                       */
+            /*     PLCP(5)+Header(24)+IV(4)+ICV(4)+CRC(4)+RxStatus(8) */
+            if (frameLen < 32)
+            {
+                return ZM_ERR_MIN_RX_ENCRYPT_FRAME_LENGTH;
+            }
+        }
+        else if ( frameSubType == 0x5 || frameSubType == 0x8 )
+        {
+            /* Minimum Length = PLCP(5)+MACHeader(24)+Timestamp(8)+BeaconInterval(2)+Cap(2)+CRC(4)+RxStatus(8) */
+            if (frameLen < 36)
+            {
+                return ZM_ERR_MIN_RX_FRAME_LENGTH;
+            }
+        }
+        else
+        {
+            /* Minimum Length = PLCP(5)+MACHeader(24)+CRC(4)+RxStatus(8) */
+            if (frameLen < 24)
+            {
+                return ZM_ERR_MIN_RX_FRAME_LENGTH;
+            }
+        }
+
+        /* Check if frame Length > ZM_WLAN_MAX_RX_SIZE. */
+        if (frameLen > ZM_WLAN_MAX_RX_SIZE)
+        {
+            return ZM_ERR_MAX_RX_FRAME_LENGTH;
+        }
+    }
+    else if ((frameCtrl&0xff) == 0xa4)
+    {
+        /* PsPoll */
+        //zm_msg0_rx(ZM_LV_0, "rx pspoll");
+    }
+    else if ((frameCtrl&0xff) == ZM_WLAN_FRAME_TYPE_BAR)
+    {
+        if (wd->sta.enableDrvBA == 1)
+        {
+            zfAggRecvBAR(dev, buf);
+        }
+
+        return ZM_ERR_RX_BAR_FRAME;
+    }
+    else
+    {
+        return ZM_ERR_RX_FRAME_TYPE;
+    }
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+    }
+    else if ( wd->wlanMode != ZM_MODE_PSEUDO )
+    {
+        if ( (ret=zfStaRxValidateFrame(dev, buf))!=ZM_SUCCESS )
+        {
+            //zm_debug_msg1("discard frame, code = ", ret);
+            return ret;
+        }
+    }
+
+    return ZM_SUCCESS;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfWlanRxFilter              */
+/*      Filter duplicated frame.                                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : received 802.11 frame buffer.                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfWlanRxFilter(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t src[3];
+    u16_t dst0;
+    u16_t frameType;
+    u16_t seq;
+    u16_t offset;
+    u16_t index;
+    u16_t col;
+    u16_t i;
+    u8_t up = 0; /* User priority */
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    /* RX PREFIX */
+    offset = 0;
+
+    frameType = zmw_rx_buf_readh(dev, buf, offset);
+
+    // Don't divide 2^4 because we don't want the fragementation pkt to be treated as
+    // duplicated frames
+    seq = zmw_rx_buf_readh(dev, buf, offset+22);
+    dst0 = zmw_rx_buf_readh(dev, buf, offset+4);
+    src[0] = zmw_rx_buf_readh(dev, buf, offset+10);
+    src[1] = zmw_rx_buf_readh(dev, buf, offset+12);
+    src[2] = zmw_rx_buf_readh(dev, buf, offset+14);
+
+    /* QoS data frame */
+    if ((frameType & 0x88) == 0x88)
+    {
+        up = zmw_rx_buf_readb(dev, buf, offset+24);
+        up &= 0x7;
+    }
+
+    index = (src[2]+up) & (ZM_FILTER_TABLE_ROW-1);
+
+    /* TBD : filter frame with source address == own MAC adress */
+    if ((wd->macAddr[0] == src[0]) && (wd->macAddr[1] == src[1])
+            && (wd->macAddr[2] == src[2]))
+    {
+        //zm_msg0_rx(ZM_LV_0, "Rx filter=>src is own MAC");
+        wd->trafTally.rxSrcIsOwnMac++;
+#if 0
+        return ZM_ERR_RX_SRC_ADDR_IS_OWN_MAC;
+#endif
+    }
+
+    zm_msg2_rx(ZM_LV_2, "Rx seq=", seq);
+
+    /* Filter unicast frame only */
+    if ((dst0 & 0x1) == 0)
+    {
+        zmw_enter_critical_section(dev);
+
+        for(i=0; i<ZM_FILTER_TABLE_COL; i++)
+        {
+            if ((wd->rxFilterTbl[i][index].addr[0] == src[0])
+                    && (wd->rxFilterTbl[i][index].addr[1] == src[1])
+                    && (wd->rxFilterTbl[i][index].addr[2] == src[2])
+                    && (wd->rxFilterTbl[i][index].up == up))
+            {
+                if (((frameType&0x800)==0x800)
+                        &&(wd->rxFilterTbl[i][index].seq==seq))
+                {
+                    zmw_leave_critical_section(dev);
+                    /* hit : duplicated frame */
+                    zm_msg0_rx(ZM_LV_1, "Rx filter hit=>duplicated");
+                    wd->trafTally.rxDuplicate++;
+                    return ZM_ERR_RX_DUPLICATE;
+                }
+                else
+                {
+                    /* hit : not duplicated frame, update sequence number */
+                    wd->rxFilterTbl[i][index].seq = seq;
+                    zmw_leave_critical_section(dev);
+                    zm_msg0_rx(ZM_LV_2, "Rx filter hit");
+                    return ZM_SUCCESS;
+                }
+            }
+        } /* for(i=0; i<ZM_FILTER_TABLE_COL; i++) */
+
+        /* miss : add to table */
+        zm_msg0_rx(ZM_LV_1, "Rx filter miss");
+        /* TODO : Random select a column */
+        col = (u16_t)(wd->tick & (ZM_FILTER_TABLE_COL-1));
+        wd->rxFilterTbl[col][index].addr[0] = src[0];
+        wd->rxFilterTbl[col][index].addr[1] = src[1];
+        wd->rxFilterTbl[col][index].addr[2] = src[2];
+        wd->rxFilterTbl[col][index].seq = seq;
+        wd->rxFilterTbl[col][index].up = up;
+
+        zmw_leave_critical_section(dev);
+    } /* if ((dst0 & 0x1) == 0) */
+
+    return ZM_SUCCESS;
+}
+
+
+
+u16_t zfTxGenWlanTail(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t snaplen,
+                      u16_t* mic)
+{
+    struct zsMicVar*  pMicKey;
+    u16_t  i, length, payloadOffset;
+    u8_t   bValue, qosType = 0;
+    u8_t   snapByte[12];
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        pMicKey = zfApGetTxMicKey(dev, buf, &qosType);
+
+        if ( pMicKey == NULL )
+        {
+            return 0;
+        }
+    }
+    else if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        pMicKey = zfStaGetTxMicKey(dev, buf);
+
+        if ( pMicKey == NULL )
+        {
+            return 0;
+        }
+    }
+    else
+    {
+        return 0;
+    }
+
+    length = zfwBufGetSize(dev, buf);
+
+    zfMicClear(pMicKey);
+
+    /* append DA and SA */
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    for(i=16; i<22; i++)
+    { // VISTA DA
+        bValue = zmw_tx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+    for(i=10; i<16; i++)
+    { // VISTA SA
+        bValue = zmw_tx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+#else
+    for(i=0; i<12; i++)
+    {
+        bValue = zmw_tx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+#endif
+
+    /* append for alignment */
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        if (wd->sta.wmeConnected != 0)
+            zfMicAppendByte(zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET + 1) >> 5, pMicKey);
+        else
+            zfMicAppendByte(0, pMicKey);
+    }
+    else if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        if (qosType == 1)
+            zfMicAppendByte(zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET + 1) >> 5, pMicKey);
+        else
+            zfMicAppendByte(0, pMicKey);
+    }
+    else
+    {
+        /* TODO : Qos Software MIC in IBSS Mode */
+        zfMicAppendByte(0, pMicKey);
+    }
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+
+    if ( snaplen == 0 )
+    {
+        payloadOffset = ZM_80211_FRAME_IP_OFFSET;
+    }
+    else
+    {
+        payloadOffset = ZM_80211_FRAME_TYPE_OFFSET;
+
+        for(i=0; i<(snaplen>>1); i++)
+        {
+            snapByte[i*2] = (u8_t) (snap[i] & 0xff);
+            snapByte[i*2+1] = (u8_t) ((snap[i] >> 8) & 0xff);
+        }
+
+        for(i=0; i<snaplen; i++)
+        {
+            zfMicAppendByte(snapByte[i], pMicKey);
+        }
+    }
+
+    for(i=payloadOffset; i<length; i++)
+    {
+        bValue = zmw_tx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+
+    zfMicGetMic( (u8_t*) mic, pMicKey);
+
+    return ZM_SIZE_OF_MIC;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfTxGetIpTosAndFrag         */
+/*      Get IP TOS and frag offset from Tx buffer                       */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : Tx buffer pointer                                         */
+/*      up : pointer for returning user priority                        */
+/*      fragOff : pointer for returning ip frag offset                  */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+void zfTxGetIpTosAndFrag(zdev_t* dev, zbuf_t* buf, u8_t* up, u16_t* fragOff)
+{
+    u8_t ipv;
+    u16_t len;
+	u16_t etherType;
+    u8_t tos;
+
+    *up = 0;
+    *fragOff = 0;
+
+    len = zfwBufGetSize(dev, buf);
+
+    if (len >= 34) //Minimum IPv4 packet size, 14(Ether header)+20(IPv4 header)
+    {
+        etherType = (((u16_t)zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_TYPE_OFFSET))<<8)
+                    + zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_TYPE_OFFSET + 1);
+
+        /* protocol type = IP */
+        if (etherType == 0x0800)
+        {
+            ipv = zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET) >> 4;
+            if (ipv == 0x4) //IPv4
+            {
+                tos = zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET + 1);
+                *up = (tos >> 5);
+                *fragOff = zmw_tx_buf_readh(dev, buf, ZM_80211_FRAME_IP_OFFSET + 6);
+            }
+            /* TODO : handle VLAN tag and IPv6 packet */
+        }
+    }
+    return;
+}
+
+#ifdef ZM_ENABLE_NATIVE_WIFI
+u16_t zfTxGenWlanSnap(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t* snaplen)
+{
+    snap[0] = zmw_buf_readh(dev, buf, ZM_80211_FRAME_HEADER_LEN + 0);
+    snap[1] = zmw_buf_readh(dev, buf, ZM_80211_FRAME_HEADER_LEN + 2);
+    snap[2] = zmw_buf_readh(dev, buf, ZM_80211_FRAME_HEADER_LEN + 4);
+    *snaplen = 6;
+
+    return ZM_80211_FRAME_HEADER_LEN + *snaplen;
+}
+#else
+u16_t zfTxGenWlanSnap(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t* snaplen)
+{
+    u16_t removed;
+	   u16_t etherType;
+   	u16_t len;
+
+	   len = zfwBufGetSize(dev, buf);
+    if (len < 14) //Minimum Ethernet packet size, 14(Ether header)
+    {
+        /* TODO : Assert? */
+        *snaplen = 0;
+        return 0;
+    }
+
+    /* Generate RFC1042 header */
+    etherType = (((u16_t)zmw_tx_buf_readb(dev, buf, 12))<<8)
+                + zmw_tx_buf_readb(dev, buf, 13);
+
+    //zm_debug_msg2("ethernet type or length = ", etherType);
+
+    if (etherType > 1500)
+    {
+        /* ETHERNET format */
+        removed = 12;
+        snap[0] = 0xaaaa;
+        snap[1] = 0x0003;
+        if ((etherType ==0x8137) || (etherType == 0x80f3))
+        {
+            /* Bridge Tunnel */
+            snap[2] = 0xF800;
+        }
+        else
+        {
+            /* RFC 1042 */
+            snap[2] = 0x0000;
+        }
+        *snaplen = 6;
+
+        if ( etherType == 0x888E )
+        {
+            zfShowTxEAPOL(dev, buf, 14);
+        }
+    }
+    else
+    {
+        /* 802.3 format */
+        removed = 14;
+        *snaplen = 0;
+    }
+
+    return removed;
+}
+#endif
+
+u8_t zfIsVtxqEmpty(zdev_t* dev)
+{
+    u8_t isEmpty = TRUE;
+    u8_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->vmmqHead != wd->vmmqTail)
+    {
+        isEmpty = FALSE;
+        goto check_done;
+    }
+
+    for(i=0; i < 4; i++)
+    {
+        if (wd->vtxqHead[i] != wd->vtxqTail[i])
+        {
+            isEmpty = FALSE;
+            goto check_done;
+        }
+    }
+
+check_done:
+    zmw_leave_critical_section(dev);
+    return isEmpty;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfPutVtxq                   */
+/*      Put Tx buffer to virtual TxQ                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : Tx buffer pointer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_SUCCESS or error code                                        */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfPutVtxq(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t ac;
+    u8_t up;
+    u16_t fragOff;
+#ifdef ZM_AGG_TALLY
+    struct aggTally *agg_tal;
+#endif
+#ifdef ZM_ENABLE_AGGREGATION
+    #ifndef ZM_BYPASS_AGGR_SCHEDULING
+	u16_t ret;
+    u16_t tid;
+    #endif
+#endif
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+
+    if ( wd->zfcbClassifyTxPacket != NULL )
+    {
+        ac = wd->zfcbClassifyTxPacket(dev, buf);
+    }
+    else
+    {
+        ac = zcUpToAc[up&0x7] & 0x3;
+    }
+
+    /*
+     * add by honda
+     * main A-MPDU aggregation function
+     */
+#ifdef ZM_AGG_TALLY
+    agg_tal = &wd->agg_tal;
+    agg_tal->got_packets_sum++;
+
+#endif
+
+#ifdef ZM_ENABLE_AGGREGATION
+    #ifndef ZM_BYPASS_AGGR_SCHEDULING
+    tid = up&0x7;
+    if(wd->enableAggregation==0)
+    {
+        if( (wd->wlanMode == ZM_MODE_AP) ||
+            (wd->wlanMode == ZM_MODE_INFRASTRUCTURE && wd->sta.EnableHT) ||
+            (wd->wlanMode == ZM_MODE_PSEUDO) ) {
+            // (infrastructure_mode && connect_to_11n_ap) || (ap_mode && is_11n_ap)
+            //ret = zfAggPutVtxq(dev, buf);
+
+
+            ret = zfAggTx(dev, buf, tid);
+            if (ZM_SUCCESS == ret)
+            {
+                //zfwBufFree(dev, buf, ZM_SUCCESS);
+
+                return ZM_SUCCESS;
+            }
+            if (ZM_ERR_EXCEED_PRIORITY_THRESHOLD == ret)
+            {
+                wd->commTally.txQosDropCount[ac]++;
+                zfwBufFree(dev, buf, ZM_SUCCESS);
+
+                zm_msg1_tx(ZM_LV_1, "Packet discarded, VTXQ full, ac=", ac);
+
+                return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
+            }
+            if (ZM_ERR_TX_BUFFER_UNAVAILABLE == ret)
+            {
+                /*
+                * do nothing
+                * continue following procession, put into VTXQ
+                * return ZM_SUCCESS;
+                */
+            }
+        }
+    }
+    #endif
+#endif
+    /*
+     * end of add by honda
+     */
+
+    /* First Ip frag */
+    if ((fragOff & 0xff3f) == 0x0020)
+    {
+        /* Don't let ip frag in if VTXQ unable to hold */
+        /* whole ip frag burst(assume 20 frag)         */
+        zmw_enter_critical_section(dev);
+        if (((wd->vtxqHead[ac] - wd->vtxqTail[ac])& ZM_VTXQ_SIZE_MASK)
+                > (ZM_VTXQ_SIZE-20))
+        {
+            wd->qosDropIpFrag[ac] = 1;
+        }
+        else
+        {
+            wd->qosDropIpFrag[ac] = 0;
+        }
+        zmw_leave_critical_section(dev);
+
+        if (wd->qosDropIpFrag[ac] == 1)
+        {
+            //zm_debug_msg2("vtQ full, drop buf = ", buf);
+            wd->commTally.txQosDropCount[ac]++;
+            zfwBufFree(dev, buf, ZM_SUCCESS);
+            zm_msg1_tx(ZM_LV_1, "Packet discarded, first ip frag, ac=", ac);
+            //VTXQ[] can not hold whold ip frag burst(assume 20 frags)
+            return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
+        }
+    }
+    else if ((fragOff & 0xff3f) == 0)
+    {
+        wd->qosDropIpFrag[ac] = 0;
+    }
+
+    if (((fragOff &= 0xff1f) != 0) && (wd->qosDropIpFrag[ac] == 1))
+    {
+        wd->commTally.txQosDropCount[ac]++;
+        zfwBufFree(dev, buf, ZM_SUCCESS);
+        zm_msg1_tx(ZM_LV_1, "Packet discarded, ip frag, ac=", ac);
+        //Discard following ip frags
+        return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
+    }
+
+    zmw_enter_critical_section(dev);
+    if (((wd->vtxqHead[ac] + 1) & ZM_VTXQ_SIZE_MASK) != wd->vtxqTail[ac])
+    {
+        wd->vtxq[ac][wd->vtxqHead[ac]] = buf;
+        wd->vtxqHead[ac] = ((wd->vtxqHead[ac] + 1) & ZM_VTXQ_SIZE_MASK);
+        zmw_leave_critical_section(dev);
+        return ZM_SUCCESS;
+    }
+    else
+    {
+        zmw_leave_critical_section(dev);
+
+        wd->commTally.txQosDropCount[ac]++;
+        zfwBufFree(dev, buf, ZM_SUCCESS);
+        zm_msg1_tx(ZM_LV_1, "Packet discarded, VTXQ full, ac=", ac);
+        return ZM_ERR_EXCEED_PRIORITY_THRESHOLD; //VTXQ[] Full
+    }
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfGetVtxq                   */
+/*      Get Tx buffer from virtual TxQ                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Tx buffer pointer                                               */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+zbuf_t* zfGetVtxq(zdev_t* dev, u8_t ac)
+{
+    zbuf_t* buf;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    ac &= 0x3;
+    zmw_enter_critical_section(dev);
+    if (wd->vtxqHead[ac] != wd->vtxqTail[ac])
+    {
+        buf = wd->vtxq[ac][wd->vtxqTail[ac]];
+        wd->vtxqTail[ac] = ((wd->vtxqTail[ac] + 1) & ZM_VTXQ_SIZE_MASK);
+        zmw_leave_critical_section(dev);
+        return buf;
+    }
+    else
+    {
+        zmw_leave_critical_section(dev);
+        return 0; //VTXQ[] empty
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfPutVmmq                   */
+/*      Put Tx buffer to virtual MmQ                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : Tx buffer pointer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_SUCCESS or error code                                        */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfPutVmmq(zdev_t* dev, zbuf_t* buf)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (((wd->vmmqHead + 1) & ZM_VMMQ_SIZE_MASK) != wd->vmmqTail)
+    {
+        wd->vmmq[wd->vmmqHead] = buf;
+        wd->vmmqHead = ((wd->vmmqHead + 1) & ZM_VMMQ_SIZE_MASK);
+        zmw_leave_critical_section(dev);
+        return ZM_SUCCESS;
+    }
+    else
+    {
+        zmw_leave_critical_section(dev);
+
+        zfwBufFree(dev, buf, ZM_SUCCESS);
+        zm_msg0_mm(ZM_LV_0, "Packet discarded, VMmQ full");
+        return ZM_ERR_VMMQ_FULL; //VTXQ[] Full
+    }
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfGetVmmq                   */
+/*      Get Tx buffer from virtual MmQ                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Tx buffer pointer                                               */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.12     */
+/*                                                                      */
+/************************************************************************/
+zbuf_t* zfGetVmmq(zdev_t* dev)
+{
+    zbuf_t* buf;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (wd->vmmqHead != wd->vmmqTail)
+    {
+        buf = wd->vmmq[wd->vmmqTail];
+        wd->vmmqTail = ((wd->vmmqTail + 1) & ZM_VMMQ_SIZE_MASK);
+        zmw_leave_critical_section(dev);
+        return buf;
+    }
+    else
+    {
+        zmw_leave_critical_section(dev);
+        return 0; //VTXQ[] empty
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfPushVtxq                  */
+/*      Service Virtual TxQ (weighted round robin)                      */
+/*      Get Tx buffer form virtual TxQ and put to hardware TxD queue    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+void zfPushVtxq(zdev_t* dev)
+{
+    zbuf_t* buf;
+    u16_t i;
+    u16_t txed;
+    u32_t freeTxd;
+    u16_t err;
+    u16_t skipFlag = 0;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+
+    //zm_debug_msg1("zfHpGetFreeTxdCount = ", zfHpGetFreeTxdCount(dev));
+
+    if (wd->halState == ZM_HAL_STATE_INIT)
+    {
+        if (!wd->modeMDKEnable)
+        {
+            zm_debug_msg0("HAL is not ready for Tx");
+        }
+        return;
+    }
+    else if (wd->sta.DFSDisableTx)
+    {
+        zm_debug_msg0("return because 802.11h DFS Disable Tx");
+        return;
+    }
+    else if (wd->sta.flagFreqChanging != 0)
+    {
+        //Hold until RF frequency changed
+        return;
+    }
+    else if (( wd->sta.flagKeyChanging ) && ( wd->wlanMode != ZM_MODE_AP ))
+    {
+        return;
+    }
+#ifdef ZM_ENABLE_POWER_SAVE
+    else if ( zfPowerSavingMgrIsSleeping(dev) )
+    {
+        //zm_debug_msg0("Packets queued since the MAC is in power-saving mode\n");
+        return;
+    }
+#endif
+
+    zmw_enter_critical_section(dev);
+    if (wd->vtxqPushing != 0)
+    {
+        skipFlag = 1;
+    }
+    else
+    {
+        wd->vtxqPushing = 1;
+    }
+    zmw_leave_critical_section(dev);
+
+    if (skipFlag == 1)
+    {
+        return;
+    }
+
+    while (1)
+    {
+        txed = 0;
+
+        /* 2006.12.20, Serve Management queue */
+        while( zfHpGetFreeTxdCount(dev) > 0 )
+        {
+            if ((buf = zfGetVmmq(dev)) != 0)
+            {
+                txed = 1;
+                //zm_debug_msg2("send buf = ", buf);
+                if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
+                        ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+                {
+                    zfwBufFree(dev, buf, 0);
+                }
+            }
+            else
+            {
+                break;
+            }
+        }
+        if ((wd->sta.bScheduleScan) || ((wd->sta.bChannelScan == TRUE) && (zfStaIsConnected(dev))))
+        {
+            //Hold until Scan Stop
+            wd->vtxqPushing = 0;
+            return;
+        }
+
+#ifdef ZM_ENABLE_AGGREGATION
+    #ifndef ZM_BYPASS_AGGR_SCHEDULING
+        if( (wd->wlanMode == ZM_MODE_AP) ||
+            (wd->wlanMode == ZM_MODE_INFRASTRUCTURE && wd->sta.EnableHT) ||
+            (wd->wlanMode == ZM_MODE_PSEUDO) ) {
+
+            zfAggTxScheduler(dev, 0);
+
+            if (txed == 0) {
+                wd->vtxqPushing = 0;
+                return;
+            }
+            else {
+                continue;
+            }
+        }
+    #endif
+#endif
+
+        /* Service VTxQ[3] */
+        for (i=0; i<4; i++)
+        {
+            if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= 3)
+            {
+                if ((buf = zfGetVtxq(dev, 3)) != 0)
+                {
+                    txed = 1;
+                    //zm_debug_msg2("send buf = ", buf);
+                    zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+                    ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
+                }
+            }
+            else
+            {
+                break;
+            }
+        }
+
+        /* Service VTxQ[2] */
+        for (i=0; i<3; i++)
+        {
+            if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= (zfHpGetMaxTxdCount(dev)*1/4))
+            {
+                if ((buf = zfGetVtxq(dev, 2)) != 0)
+                {
+                    txed = 1;
+                    zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+                    ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
+                }
+                if (wd->sta.ac0PriorityHigherThanAc2 == 1)
+                {
+                    if ((buf = zfGetVtxq(dev, 0)) != 0)
+                    {
+                        txed = 1;
+                        zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+                        ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
+                    }
+                }
+            }
+            else
+            {
+                break;
+            }
+        }
+
+        /* Service VTxQ[0] */
+        for (i=0; i<2; i++)
+        {
+            if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= (zfHpGetMaxTxdCount(dev)*2/4))
+            {
+                if ((buf = zfGetVtxq(dev, 0)) != 0)
+                {
+                    txed = 1;
+                    zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+                    ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
+                }
+            }
+            else
+            {
+                break;
+            }
+
+        }
+
+        /* Service VTxQ[1] */
+        if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= (zfHpGetMaxTxdCount(dev)*3/4))
+        {
+            if ((buf = zfGetVtxq(dev, 1)) != 0)
+            {
+                txed = 1;
+                zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+                ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
+            }
+        }
+
+        /* All VTxQs are either empty or exceed their threshold */
+        if (txed == 0)
+        {
+            wd->vtxqPushing = 0;
+            return;
+        }
+    } //while (1)
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfFlushVtxq                 */
+/*      Flush Virtual TxQ and MmQ                                       */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfFlushVtxq(zdev_t* dev)
+{
+    zbuf_t* buf;
+    u8_t i;
+    zmw_get_wlan_dev(dev);
+
+    /* Flush MmQ */
+    while ((buf = zfGetVmmq(dev)) != 0)
+    {
+        zfwBufFree(dev, buf, 0);
+        zm_debug_msg0("zfFlushVtxq: [Vmmq]");
+        wd->queueFlushed  |= 0x10;
+    }
+
+    /* Flush VTxQ */
+    for (i=0; i<4; i++)
+    {
+        while ((buf = zfGetVtxq(dev, i)) != 0)
+        {
+            zfwBufFree(dev, buf, 0);
+            zm_debug_msg1("zfFlushVtxq: [zfGetVtxq]- ", i);
+            wd->queueFlushed |= (1<<i);
+        }
+    }
+}
+
+void zf80211FrameSend(zdev_t* dev, zbuf_t* buf, u16_t* header, u16_t snapLen,
+                           u16_t* da, u16_t* sa, u8_t up, u16_t headerLen, u16_t* snap,
+                           u16_t* tail, u16_t tailLen, u16_t offset, u16_t bufType,
+                           u8_t ac, u8_t keyIdx)
+{
+    u16_t err;
+    u16_t fragLen;
+
+    zmw_get_wlan_dev(dev);
+
+    fragLen = zfwBufGetSize(dev, buf);
+    if ((da[0]&0x1) == 0)
+    {
+        wd->commTally.txUnicastFrm++;
+        wd->commTally.txUnicastOctets += (fragLen+snapLen);
+    }
+    else if (da[0] == 0xffff)
+    {
+        wd->commTally.txBroadcastFrm++;
+        wd->commTally.txBroadcastOctets += (fragLen+snapLen);
+    }
+    else
+    {
+        wd->commTally.txMulticastFrm++;
+        wd->commTally.txMulticastOctets += (fragLen+snapLen);
+    }
+    wd->ledStruct.txTraffic++;
+
+    if ((err = zfHpSend(dev, header, headerLen, snap, snapLen,
+                        tail, tailLen, buf, offset,
+                        bufType, ac, keyIdx)) != ZM_SUCCESS)
+    {
+        if (bufType == ZM_EXTERNAL_ALLOC_BUF)
+        {
+            zfwBufFree(dev, buf, err);
+        }
+        else if (bufType == ZM_INTERNAL_ALLOC_BUF)
+        {
+            zfwBufFree(dev, buf, 0);
+        }
+        else
+        {
+            zm_assert(0);
+        }
+    }
+}
+
+void zfCheckIsRIFSFrame(zdev_t* dev, zbuf_t* buf, u16_t frameSubtype)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* #2 Record the sequence number to determine whether the unicast frame is separated by RIFS or not */
+    if (frameSubtype & 0x80)
+    {   //QoS data frame
+        u16_t sequenceNum;
+        u16_t qosControlField;
+
+        sequenceNum = ( zmw_buf_readh(dev, buf, 22) >> 4 ); // Discard fragment number !
+        qosControlField = zmw_buf_readh(dev, buf, 24); // Don't consider WDS (Wireless Distribution System)
+        //DbgPrint("The QoS Control Field                              : %d", qosControlField);
+        //DbgPrint("The RIFS Count                                     : %d", wd->sta.rifsCount);
+
+        if( qosControlField & ZM_BIT_5 )
+        {// ACK policy is "No ACK"
+            /* RIFS-Like frame */
+            wd->sta.rifsLikeFrameSequence[wd->sta.rifsLikeFrameCnt]   = sequenceNum;
+
+            if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTING )
+            {
+                if( wd->sta.rifsLikeFrameSequence[2] != 0 )
+                {// RIFS-like Pattern collected
+                    if( ( wd->sta.rifsLikeFrameSequence[2] - wd->sta.rifsLikeFrameSequence[1] == 2 ) &&
+                        ( wd->sta.rifsLikeFrameSequence[1] - wd->sta.rifsLikeFrameSequence[0] == 2 ) )
+                    {
+                        /* RIFS pattern matched */
+
+                        /* #3 Enable RIFS function if the RIFS pattern matched  */
+                        zfHpEnableRifs(dev, ((wd->sta.currentFrequency<3000)?1:0), wd->sta.EnableHT, wd->sta.HT2040);
+
+                        // Set RIFS timer
+                        wd->sta.rifsTimer = wd->tick;
+
+                        wd->sta.rifsCount++;
+
+                        // Set state to be Detected
+                        wd->sta.rifsState = ZM_RIFS_STATE_DETECTED;
+                    }
+                }
+            }
+            else
+            {// state = Detected
+                // Reset RIFS timer
+                if( (wd->tick - wd->sta.rifsTimer) < ZM_RIFS_TIMER_TIMEOUT )
+                    wd->sta.rifsTimer = wd->tick;
+            }
+
+            //DbgPrint("SN1 = %d, SN2 = %d, SN3 = %d\n", wd->sta.rifsLikeFrameSequence[0],
+            //                                           wd->sta.rifsLikeFrameSequence[1],
+            //                                           wd->sta.rifsLikeFrameSequence[2]);
+
+            // Update RIFS-like sequence number
+            if( wd->sta.rifsLikeFrameSequence[2] != 0 )
+            {
+                wd->sta.rifsLikeFrameSequence[0] = wd->sta.rifsLikeFrameSequence[1];
+                wd->sta.rifsLikeFrameSequence[1] = wd->sta.rifsLikeFrameSequence[2];
+                wd->sta.rifsLikeFrameSequence[2] = 0;
+            }
+
+            // Only record three adjacent frame
+            if( wd->sta.rifsLikeFrameCnt < 2 )
+                wd->sta.rifsLikeFrameCnt++;
+        }
+    }
+
+    /* #4 Disable RIFS function if the timer TIMEOUT  */
+    if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTED )
+    {
+        if( ( wd->tick - wd->sta.rifsTimer ) > ZM_RIFS_TIMER_TIMEOUT )
+        {// TIMEOUT
+            // Disable RIFS
+            zfHpDisableRifs(dev);
+
+            // Reset RIFS-like sequence number FIFO
+            wd->sta.rifsLikeFrameSequence[0] = 0;
+            wd->sta.rifsLikeFrameSequence[1] = 0;
+            wd->sta.rifsLikeFrameSequence[2] = 0;
+            wd->sta.rifsLikeFrameCnt = 0;
+
+            // Set state to be Detecting
+            wd->sta.rifsState = ZM_RIFS_STATE_DETECTING;
+        }
+    }
+}
diff --git a/drivers/staging/otus/80211core/cwep.c b/drivers/staging/otus/80211core/cwep.c
new file mode 100644
index 000000000000..ec31bb1ac283
--- /dev/null
+++ b/drivers/staging/otus/80211core/cwep.c
@@ -0,0 +1,299 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : cwep.c                                                */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains Tx and Rx functions.                       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+
+u32_t crc32_tab[] =
+{
+       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
+};
+
+void zfWEPEncrypt(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u8_t keyLen, u8_t* WepKey, u8_t *iv)
+{
+    u8_t    S[256],S2[256];
+    u16_t   ui;
+    u16_t   i;
+    u16_t   j;
+    u8_t    temp;
+    u8_t    K;
+    u32_t   ltemp;
+    u16_t   len;
+    u32_t   icv;
+    u8_t    key[32];
+
+    key[0] = iv[0];
+    key[1] = iv[1];
+    key[2] = iv[2];
+
+    /* Append Wep Key after IV */
+    zfMemoryCopy(&key[3], WepKey, keyLen);
+
+    keyLen += 3;
+
+    for(i = 0; i < 256; i++)
+    {
+        S[i] = (u8_t)i;
+        S2[i] = key[i&(keyLen-1)];
+    }
+
+    j = 0;
+    for(i = 0; i < 256; i++)
+    {
+        j = (j + S[i] + S2[i]) ;
+        j&=255 ;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+    }
+
+    i = j = 0;
+    icv = -1;
+
+    /* For Snap Header */
+    for (ui = 0; ui < snapLen; ui++)
+    {
+        u8_t In;
+
+        i++;
+        i &= 255;
+        j += S[i];
+        j &= 255;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+//          temp = (S[i] + temp) & 255;
+        temp += S[i];
+        temp &=255;
+        K = S[temp];        // Key used to Xor with input data
+
+        In = snap[ui];
+        icv =  (icv>>8) ^ crc32_tab[(icv^In)&0xff];
+
+        snap[ui] = In ^ K;
+        //zmw_tx_buf_writeb(dev, buf, ui, In ^ K);
+    }
+
+    len = zfwBufGetSize(dev, buf);
+
+    for (ui = offset; ui < len; ui++)
+    {
+        u8_t In;
+
+        i++;
+        i &= 255;
+        j += S[i];
+        j &= 255;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+//          temp = (S[i] + temp) & 255;
+        temp += S[i];
+        temp &=255;
+        K = S[temp];        // Key used to Xor with input data
+
+        In = zmw_tx_buf_readb(dev, buf, ui);
+        icv =  (icv>>8) ^ crc32_tab[(icv^In)&0xff];
+
+        zmw_tx_buf_writeb(dev, buf, ui, In ^ K);
+    } //End of for (ui = 0; ui < Num_Bytes; ui++)
+
+    icv = ~(icv);
+    ltemp = (u32_t) icv;
+
+    for (ui = 0; ui < 4; ui++)
+    {
+        i ++;
+        i &= 255;
+        j += S[i];
+        j &= 255;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+        temp += S[i];
+        temp &= 255;
+        K = S[temp];        // Key used to Xor with input data
+
+        //*Out++ = (u8_t)(ltemp ^ K)&0xff;
+        zmw_tx_buf_writeb(dev, buf, len+ui, (u8_t)(ltemp ^ K)&0xff);
+        ltemp >>= 8;
+    }
+
+    zfwBufSetSize(dev, buf, len+4);
+}
+
+u16_t zfWEPDecrypt(zdev_t *dev, zbuf_t *buf, u16_t offset, u8_t keyLen, u8_t* WepKey, u8_t *iv)
+{
+    u8_t    S[256];
+    u8_t    S2[256];
+    u16_t   ui;
+    u16_t   i;
+    u16_t   j;
+    u32_t   icv_tmp;
+    u32_t   *icv;
+    u32_t   rxbuf_icv;
+    u8_t    temp;
+    u8_t    K;
+    u16_t   len;
+    u8_t    key[32];
+
+    /* Retrieve IV */
+    key[0] = iv[0];
+    key[1] = iv[1];
+    key[2] = iv[2];
+
+    /* Append Wep Key after IV */
+    zfMemoryCopy(&key[3], WepKey, keyLen);
+
+    keyLen += 3;
+
+    for(i = 0; i < 256; i++)
+    {
+        S[i] = (u8_t)i;
+        S2[i] = key[i&(keyLen-1)];
+    }
+
+    j = 0;
+    for(i = 0; i < 256; i++)
+    {
+        j = (j + S[i] + S2[i]);
+        j&=255 ;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+    }
+
+    i = j = 0;
+
+    len = zfwBufGetSize(dev, buf);
+
+    for (ui = offset; ui < len; ui++)
+    {
+        u8_t In;
+
+        i++;
+        i &= 255;
+        j += S[i];
+        j &= 255;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+//          temp = (S[i] + temp) & 255;
+        temp += S[i];
+        temp &=255;
+        K = S[temp];        // Key used to Xor with input data
+
+        In = zmw_rx_buf_readb(dev, buf, ui);
+
+        zmw_rx_buf_writeb(dev, buf, ui, In ^ K);
+    } //End of for (ui = 0; ui < Num_Bytes; ui++)
+
+    icv = &icv_tmp;
+    *icv = -1;
+
+    for (ui = offset; ui < len - 4; ui++)
+    {
+        u8_t In;
+
+        In = zmw_rx_buf_readb(dev, buf, ui);
+        *icv =  (*icv>>8) ^ crc32_tab[(*icv^In)&0xff];
+    }
+
+    *icv = ~*icv;
+
+    rxbuf_icv = (zmw_rx_buf_readb(dev, buf, len-4) |
+         zmw_rx_buf_readb(dev, buf, len-3) << 8 |
+         zmw_rx_buf_readb(dev, buf, len-2) << 16 |
+         zmw_rx_buf_readb(dev, buf, len-1) << 24);
+
+    if (*icv != rxbuf_icv)
+    {
+        return ZM_ICV_FAILURE;
+    }
+
+    return ZM_ICV_SUCCESS;
+}
diff --git a/drivers/staging/otus/80211core/cwm.c b/drivers/staging/otus/80211core/cwm.c
new file mode 100644
index 000000000000..80f1141bf910
--- /dev/null
+++ b/drivers/staging/otus/80211core/cwm.c
@@ -0,0 +1,131 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : cwm.c                                                 */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains channel width related functions.           */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#include "cprecomp.h"
+
+
+
+void zfCwmInit(zdev_t* dev) {
+    //u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    switch (wd->wlanMode) {
+    case ZM_MODE_AP:
+        wd->cwm.cw_mode = CWM_MODE2040;
+        wd->cwm.cw_width = CWM_WIDTH40;
+        wd->cwm.cw_enable = 1;
+        break;
+    case ZM_MODE_INFRASTRUCTURE:
+    case ZM_MODE_PSEUDO:
+    case ZM_MODE_IBSS:
+    default:
+        wd->cwm.cw_mode = CWM_MODE2040;
+        wd->cwm.cw_width = CWM_WIDTH20;
+        wd->cwm.cw_enable = 1;
+        break;
+    }
+}
+
+
+void zfCoreCwmBusy(zdev_t* dev, u16_t busy)
+{
+
+    zmw_get_wlan_dev(dev);
+
+    zm_msg1_mm(ZM_LV_0, "CwmBusy=", busy);
+
+    if(wd->cwm.cw_mode == CWM_MODE20) {
+        wd->cwm.cw_width = CWM_WIDTH20;
+        return;
+    }
+
+    if(wd->cwm.cw_mode == CWM_MODE40) {
+        wd->cwm.cw_width = CWM_WIDTH40;
+        return;
+    }
+
+    if (busy) {
+        wd->cwm.cw_width = CWM_WIDTH20;
+        return;
+    }
+
+
+    if((wd->wlanMode == ZM_MODE_INFRASTRUCTURE || wd->wlanMode == ZM_MODE_PSEUDO ||
+        wd->wlanMode == ZM_MODE_IBSS)) {
+        if (wd->sta.ie.HtCap.HtCapInfo && HTCAP_SupChannelWidthSet != 0 &&
+            wd->sta.ie.HtInfo.ChannelInfo && ExtHtCap_RecomTxWidthSet != 0 &&
+            (wd->sta.ie.HtInfo.ChannelInfo && ExtHtCap_ExtChannelOffsetAbove) == 1) {
+
+            wd->cwm.cw_width = CWM_WIDTH40;
+        }
+        else {
+            wd->cwm.cw_width = CWM_WIDTH20;
+        }
+
+        return;
+    }
+
+    if(wd->wlanMode == ZM_MODE_AP) {
+        wd->cwm.cw_width = CWM_WIDTH40;
+    }
+
+}
+
+
+
+
+u16_t zfCwmIsExtChanBusy(u32_t ctlBusy, u32_t extBusy)
+{
+    u32_t busy; /* percentage */
+    u32_t cycleTime, ctlClear;
+
+    cycleTime = 1280000; //1.28 seconds
+
+    if (cycleTime > ctlBusy) {
+        ctlClear = cycleTime - ctlBusy;
+    }
+    else
+    {
+        ctlClear = 0;
+    }
+
+    /* Compute ratio of extension channel busy to control channel clear
+     * as an approximation to extension channel cleanliness.
+     *
+     * According to the hardware folks, ext rxclear is undefined
+     * if the ctrl rxclear is de-asserted (i.e. busy)
+     */
+    if (ctlClear) {
+        busy = (extBusy * 100) / ctlClear;
+    } else {
+        busy = 0;
+    }
+    if (busy > ATH_CWM_EXTCH_BUSY_THRESHOLD) {
+        return TRUE;
+    }
+
+    return FALSE;
+}
diff --git a/drivers/staging/otus/80211core/cwm.h b/drivers/staging/otus/80211core/cwm.h
new file mode 100644
index 000000000000..40c39fad5f44
--- /dev/null
+++ b/drivers/staging/otus/80211core/cwm.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                          */
+/*  Module Name : cwm.h                                                     */
+/*                                                                          */
+/*  Abstract                                                                */
+/*      This module contains channel width relatived functions.             */
+/*                                                                          */
+/*  NOTES                                                                   */
+/*      None                                                                */
+/*                                                                          */
+/****************************************************************************/
+/*Revision History:                                                         */
+/*    Who         When        What                                          */
+/*    --------    --------    ----------------------------------------------*/
+/*                                                                          */
+/*    Honda       3-19-07     created                                       */
+/*                                                                          */
+/****************************************************************************/
+
+#ifndef _CWM_H
+#define _CWM_H
+
+#define ATH_CWM_EXTCH_BUSY_THRESHOLD  30  /* Extension Channel Busy Threshold (0-100%) */
+
+void zfCwmInit(zdev_t* dev);
+void zfCoreCwmBusy(zdev_t* dev, u16_t busy);
+u16_t zfCwmIsExtChanBusy(u32_t ctlBusy, u32_t extBusy);
+
+
+
+#endif /* #ifndef _CWM_H */
diff --git a/drivers/staging/otus/80211core/freqctrl.c b/drivers/staging/otus/80211core/freqctrl.c
new file mode 100644
index 000000000000..bab0df08d826
--- /dev/null
+++ b/drivers/staging/otus/80211core/freqctrl.c
@@ -0,0 +1,259 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+
+/* zfAddFreqChangeReq should be called inside the critical section */
+static void zfAddFreqChangeReq(zdev_t* dev, u16_t frequency, u8_t bw40,
+        u8_t extOffset, zfpFreqChangeCompleteCb cb)
+{
+    zmw_get_wlan_dev(dev);
+
+//printk("zfAddFreqChangeReq  freqReqQueueTail%d\n", wd->freqCtrl.freqReqQueueTail);
+    wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueTail] = frequency;
+    wd->freqCtrl.freqReqBw40[wd->freqCtrl.freqReqQueueTail] = bw40;
+    wd->freqCtrl.freqReqExtOffset[wd->freqCtrl.freqReqQueueTail] = extOffset;
+    wd->freqCtrl.freqChangeCompCb[wd->freqCtrl.freqReqQueueTail] = cb;
+    wd->freqCtrl.freqReqQueueTail++;
+    if ( wd->freqCtrl.freqReqQueueTail >= ZM_MAX_FREQ_REQ_QUEUE )
+    {
+        wd->freqCtrl.freqReqQueueTail = 0;
+    }
+}
+
+void zfCoreSetFrequencyV2(zdev_t* dev, u16_t frequency, zfpFreqChangeCompleteCb cb)
+{
+    zfCoreSetFrequencyEx(dev, frequency, 0, 0, cb);
+}
+
+void zfCoreSetFrequencyExV2(zdev_t* dev, u16_t frequency, u8_t bw40,
+        u8_t extOffset, zfpFreqChangeCompleteCb cb, u8_t forceSetFreq)
+{
+    u8_t setFreqImmed = 0;
+    u8_t initRF = 0;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zm_msg1_scan(ZM_LV_1, "Freq=", frequency);
+
+    zmw_enter_critical_section(dev);
+    if ((wd->sta.currentFrequency == frequency)
+        && (wd->sta.currentBw40 == bw40)
+        && (wd->sta.currentExtOffset == extOffset))
+    {
+        if ( forceSetFreq == 0 && wd->sta.flagFreqChanging == 0 )
+        {
+            goto done;
+        }
+    }
+#ifdef ZM_FB50
+    /*if(frequency!=2437) {
+        zmw_leave_critical_section(dev);
+        return;
+    }*/
+#endif
+
+    zfAddFreqChangeReq(dev, frequency, bw40, extOffset, cb);
+
+//    zm_assert( wd->sta.flagFreqChanging == 0 );
+    //wd->sta.flagFreqChanging = 1;
+    if ( wd->sta.flagFreqChanging == 0 )
+    {
+        if ((wd->sta.currentBw40 != bw40) || (wd->sta.currentExtOffset != extOffset))
+        {
+            initRF = 1;
+        }
+        wd->sta.currentFrequency = frequency;
+        wd->sta.currentBw40 = bw40;
+        wd->sta.currentExtOffset = extOffset;
+        setFreqImmed = 1;
+    }
+    wd->sta.flagFreqChanging++;
+
+    zmw_leave_critical_section(dev);
+
+    if ( setFreqImmed )
+    {
+        //zfHpSetFrequency(dev, frequency, 0);
+        if ( forceSetFreq )
+        { // Cold reset to reset the frequency after scanning !
+            zm_debug_msg0("#6_1 20070917");
+            zm_debug_msg0("It is happen!!! No error message");
+            zfHpSetFrequencyEx(dev, frequency, bw40, extOffset, 2);
+        }
+        else
+        {
+        zfHpSetFrequencyEx(dev, frequency, bw40, extOffset, initRF);
+        }
+
+        if (    zfStaIsConnected(dev)
+             && (frequency == wd->frequency)) {
+            wd->sta.connPowerInHalfDbm = zfHpGetTransmitPower(dev);
+        }
+    }
+    return;
+
+done:
+    zmw_leave_critical_section(dev);
+
+    if ( cb != NULL )
+    {
+        cb(dev);
+    }
+    zfPushVtxq(dev);
+    return;
+}
+
+void zfCoreSetFrequencyEx(zdev_t* dev, u16_t frequency, u8_t bw40,
+        u8_t extOffset, zfpFreqChangeCompleteCb cb)
+{
+    zfCoreSetFrequencyExV2(dev, frequency, bw40, extOffset, cb, 0);
+}
+
+void zfCoreSetFrequency(zdev_t* dev, u16_t frequency)
+{
+    zfCoreSetFrequencyV2(dev, frequency, NULL);
+}
+
+/* zfRemoveFreqChangeReq SHOULD NOT be called inside the critical section */
+static void zfRemoveFreqChangeReq(zdev_t* dev)
+{
+    zfpFreqChangeCompleteCb cb = NULL;
+    u16_t frequency;
+    u8_t bw40;
+    u8_t extOffset;
+    u16_t compFreq = 0;
+    u8_t compBw40 = 0;
+    u8_t compExtOffset = 0;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->freqCtrl.freqReqQueueHead != wd->freqCtrl.freqReqQueueTail)
+    {
+        zm_msg1_scan(ZM_LV_1, "Freq=",
+                wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead]);
+        compFreq = wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead];
+        compBw40 = wd->freqCtrl.freqReqBw40[wd->freqCtrl.freqReqQueueHead];
+        compExtOffset = wd->freqCtrl.freqReqExtOffset[wd->freqCtrl.freqReqQueueHead];
+
+        wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead] = 0;
+        cb = wd->freqCtrl.freqChangeCompCb[wd->freqCtrl.freqReqQueueHead];
+        wd->freqCtrl.freqReqQueueHead++;
+        if ( wd->freqCtrl.freqReqQueueHead >= ZM_MAX_FREQ_REQ_QUEUE )
+        {
+            wd->freqCtrl.freqReqQueueHead = 0;
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+    if ( cb != NULL )
+    {
+        cb(dev);
+    }
+
+    zmw_enter_critical_section(dev);
+    while (wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead] != 0)
+    {
+        frequency = wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead];
+        bw40 = wd->freqCtrl.freqReqBw40[wd->freqCtrl.freqReqQueueHead];
+        extOffset=wd->freqCtrl.freqReqExtOffset[wd->freqCtrl.freqReqQueueHead];
+        if ((compFreq == frequency)
+            && (compBw40 == bw40)
+            && (compExtOffset == extOffset))
+        {
+            /* Duplicated frequency command */
+            zm_msg1_scan(ZM_LV_1, "Duplicated Freq=", frequency);
+
+            cb = wd->freqCtrl.freqChangeCompCb[wd->freqCtrl.freqReqQueueHead];
+            wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead] = 0;
+            wd->freqCtrl.freqReqQueueHead++;
+
+            if ( wd->freqCtrl.freqReqQueueHead >= ZM_MAX_FREQ_REQ_QUEUE )
+            {
+                wd->freqCtrl.freqReqQueueHead = 0;
+            }
+
+            if ( wd->sta.flagFreqChanging != 0 )
+            {
+                wd->sta.flagFreqChanging--;
+            }
+
+            zmw_leave_critical_section(dev);
+            if ( cb != NULL )
+            {
+                cb(dev);
+            }
+            zmw_enter_critical_section(dev);
+        }
+        else
+        {
+            u8_t    initRF = 0;
+            if ((wd->sta.currentBw40 != bw40) || (wd->sta.currentExtOffset != extOffset))
+            {
+                initRF = 1;
+            }
+            wd->sta.currentFrequency = frequency;
+            wd->sta.currentBw40 = bw40;
+            wd->sta.currentExtOffset = extOffset;
+            zmw_leave_critical_section(dev);
+
+            zfHpSetFrequencyEx(dev, frequency, bw40, extOffset, initRF);
+            if (    zfStaIsConnected(dev)
+                && (frequency == wd->frequency)) {
+                wd->sta.connPowerInHalfDbm = zfHpGetTransmitPower(dev);
+            }
+
+            return;
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+
+void zfCoreSetFrequencyComplete(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zm_msg1_scan(ZM_LV_1, "flagFreqChanging=", wd->sta.flagFreqChanging);
+
+    zmw_enter_critical_section(dev);
+    //wd->sta.flagFreqChanging = 0;
+    if ( wd->sta.flagFreqChanging != 0 )
+    {
+        wd->sta.flagFreqChanging--;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zfRemoveFreqChangeReq(dev);
+
+    zfPushVtxq(dev);
+    return;
+}
+
+void zfReSetCurrentFrequency(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg0("It is happen!!! No error message");
+
+    zfCoreSetFrequencyExV2(dev, wd->frequency, 0, 0, NULL, 1);
+}
diff --git a/drivers/staging/otus/80211core/ledmgr.c b/drivers/staging/otus/80211core/ledmgr.c
new file mode 100644
index 000000000000..1e104a928ca4
--- /dev/null
+++ b/drivers/staging/otus/80211core/ledmgr.c
@@ -0,0 +1,557 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfLedCtrlType1              */
+/*      Traditional single-LED state                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.6      */
+/*                                                                      */
+/************************************************************************/
+// bit 15-12 : Toff for Scan state
+//     11-8 : Ton for Scan state
+//     7 : Reserved
+//     6 : mode
+//--------------------------------------
+//     bit 6 = 0
+//     5-4 : Connect state
+//           00 => always off
+//           01 => always on
+//           10 => Idle off, acitve on
+//           11 => Idle on, active off
+//--------------------------------------
+//     bit 6 = 1
+//     5-4 : freq
+//           00 => 1Hz
+//           01 => 0.5Hz
+//           10 => 0.25Hz
+//           11 => 0.125Hz
+//--------------------------------------
+//     3 : Power save state
+//         0 => always off in power save state
+//         1 => works as connect state
+//     2 : Disable state
+//     1 : Reserved
+//     0 : Power-on state
+void zfLedCtrlType1(zdev_t* dev)
+{
+    u16_t i;
+    u32_t ton, toff, tmp, period;
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<ZM_MAX_LED_NUMBER; i++)
+    {
+        if (zfStaIsConnected(dev) != TRUE)
+        {
+            //Scan state
+            ton = ((wd->ledStruct.ledMode[i] & 0xf00) >> 8) * 5;
+            toff = ((wd->ledStruct.ledMode[i] & 0xf000) >> 12) * 5;
+
+            if ((ton + toff) != 0)
+            {
+                tmp = wd->ledStruct.counter / (ton+toff);
+                tmp = wd->ledStruct.counter - (tmp * (ton+toff));
+                if (tmp < ton)
+                {
+                    zfHpLedCtrl(dev, i, 1);
+                }
+                else
+                {
+                    zfHpLedCtrl(dev, i, 0);
+                }
+            }
+        }
+        else
+        {
+            if ((zfPowerSavingMgrIsSleeping(dev)) && ((wd->ledStruct.ledMode[i] & 0x8) == 0))
+            {
+                zfHpLedCtrl(dev, i, 0);
+            }
+            else
+            {
+                //Connect state
+                if ((wd->ledStruct.ledMode[i] & 0x40) == 0)
+                {
+                    if ((wd->ledStruct.counter & 1) == 0)
+                    {
+                        zfHpLedCtrl(dev, i, (wd->ledStruct.ledMode[i] & 0x10) >> 4);
+                    }
+                    else
+                    {
+                        if ((wd->ledStruct.txTraffic > 0) || (wd->ledStruct.rxTraffic > 0))
+                        {
+                            wd->ledStruct.txTraffic = wd->ledStruct.rxTraffic = 0;
+                            if ((wd->ledStruct.ledMode[i] & 0x20) != 0)
+                            {
+                                zfHpLedCtrl(dev, i, ((wd->ledStruct.ledMode[i] & 0x10) >> 4)^1);
+                            }
+                        }
+                    }
+                }// if ((wd->ledStruct.ledMode[i] & 0x40) == 0)
+                else
+                {
+                    period = 5 * (1 << ((wd->ledStruct.ledMode[i] & 0x30) >> 4));
+                    tmp = wd->ledStruct.counter / (period*2);
+                    tmp = wd->ledStruct.counter - (tmp * (period*2));
+                    if (tmp < period)
+                    {
+                        if ((wd->ledStruct.counter & 1) == 0)
+                        {
+                            zfHpLedCtrl(dev, i, 0);
+                        }
+                        else
+                        {
+                            if ((wd->ledStruct.txTraffic > 0) || (wd->ledStruct.rxTraffic > 0))
+                            {
+                                wd->ledStruct.txTraffic = wd->ledStruct.rxTraffic = 0;
+                                zfHpLedCtrl(dev, i, 1);
+                            }
+                        }
+                    }
+                    else
+                    {
+                        if ((wd->ledStruct.counter & 1) == 0)
+                        {
+                            zfHpLedCtrl(dev, i, 1);
+                        }
+                        else
+                        {
+                            if ((wd->ledStruct.txTraffic > 0) || (wd->ledStruct.rxTraffic > 0))
+                            {
+                                wd->ledStruct.txTraffic = wd->ledStruct.rxTraffic = 0;
+                                zfHpLedCtrl(dev, i, 0);
+                            }
+                        }
+                    }
+                } //else, if ((wd->ledStruct.ledMode[i] & 0x40) == 0)
+            } //else, if (zfPowerSavingMgrIsSleeping(dev))
+        } //else : if (zfStaIsConnected(dev) != TRUE)
+    } //for (i=0; i<ZM_MAX_LED_NUMBER; i++)
+}
+
+/******************************************************************************/
+/*                                                                            */
+/*    FUNCTION DESCRIPTION                  zfLedCtrlType2                    */
+/*      Customize for Netgear Dual-LED state ((bug#31292))                    */
+/*                                                                            */
+/*      1. Status:  When dongle does not connect to 2.4G or 5G but in site    */
+/*                  survey/association                                        */
+/*         LED status: Slow blinking, Amber then Blue per 500ms               */
+/*      2. Status:	Connection at 2.4G in site survey/association             */
+/*         LED status: Slow blinking, Amber/off per 500ms                     */
+/*      3. Status:	Connection at 5G in site survey/association               */
+/*         LED status: Slow blinking, Blue/off per 500ms                      */
+/*      4. Status:	When transfer the packet                                  */
+/*         LED status: Blink per packet, including TX and RX                  */
+/*      5. Status:	When linking is established but no traffic                */
+/*         LED status: Always on                                              */
+/*      6. Status:	When linking is dropped but no re-connection              */
+/*         LED status: Always off                                             */
+/*      7. Status:	From one connection(2.4G or 5G) to change to another band */
+/*         LED status: Amber/Blue =>Slow blinking, Amber then Blue per 500ms  */
+/*                                                                            */
+/*    INPUTS                                                                  */
+/*      dev : device pointer                                                  */
+/*                                                                            */
+/*    OUTPUTS                                                                 */
+/*      None                                                                  */
+/*                                                                            */
+/*    AUTHOR                                                                  */
+/*      Shang-Chun Liu        Atheros Communications, INC.    2007.11         */
+/*                                                                            */
+/******************************************************************************/
+void zfLedCtrlType2_scan(zdev_t* dev);
+
+void zfLedCtrlType2(zdev_t* dev)
+{
+    u32_t ton, toff, tmp, period;
+    u16_t OperateLED;
+    zmw_get_wlan_dev(dev);
+
+    if (zfStaIsConnected(dev) != TRUE)
+    {
+        // Disconnect state
+        if(wd->ledStruct.counter % 4 != 0)
+    	{
+      	    // Update LED each 400ms(4*100)
+      	    // Prevent this situation
+            //              _______         ___
+            // LED[0] ON   |       |       | x |
+            // ------ OFF->+-+-+-+-+-+-+-+-+-+-+-+->>>...
+            // LED[1] ON
+            //
+            return;
+        }
+
+        if (((wd->state == ZM_WLAN_STATE_DISABLED) && (wd->sta.bChannelScan))
+            || ((wd->state != ZM_WLAN_STATE_DISABLED) && (wd->sta.bAutoReconnect)))
+        {
+            // Scan/AutoReconnect state
+            zfLedCtrlType2_scan(dev);
+        }
+        else
+        {
+            // Neither Connected nor Scan
+            zfHpLedCtrl(dev, 0, 0);
+            zfHpLedCtrl(dev, 1, 0);
+        }
+    }
+    else
+    {
+        if( wd->sta.bChannelScan )
+        {
+            // Scan state
+            if(wd->ledStruct.counter % 4 != 0)
+                return;
+            zfLedCtrlType2_scan(dev);
+            return;
+        }
+
+        if(wd->frequency < 3000)
+        {
+            OperateLED = 0;     // LED[0]: work on 2.4G (b/g band)
+            zfHpLedCtrl(dev, 1, 0);
+        }
+        else
+        {
+            OperateLED = 1;     // LED[1]: work on 5G (a band)
+            zfHpLedCtrl(dev, 0, 0);
+        }
+
+        if ((zfPowerSavingMgrIsSleeping(dev)) && ((wd->ledStruct.ledMode[OperateLED] & 0x8) == 0))
+        {
+            // If Sleeping, turn OFF
+            zfHpLedCtrl(dev, OperateLED, 0);
+        }
+        else
+        {
+            //Connect state
+            if ((wd->ledStruct.counter & 1) == 0)   // even
+            {
+                // No traffic, always ON
+                zfHpLedCtrl(dev, OperateLED, 1);
+            }
+            else       // odd
+            {
+                if ((wd->ledStruct.txTraffic > 0) || (wd->ledStruct.rxTraffic > 0))
+                {
+                    // If have traffic, turn OFF
+		            //                   _____   _   _   _   _____
+		            // LED[Operate] ON        | | | | | | | |
+		            // ------------ OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+->>>...
+		            //
+                    wd->ledStruct.txTraffic = wd->ledStruct.rxTraffic = 0;
+                    zfHpLedCtrl(dev, OperateLED, 0);
+                }
+            }
+        }
+    }
+}
+
+void zfLedCtrlType2_scan(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    // When doing scan, blink(Amber/Blue) and off per 500ms (about 400ms in our driver)
+    //               _______                         _______
+    // LED[0] ON    |       |       8       12      |       |
+    // ------ OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+->>>...
+    // LED[1] ON    0       4       |_______|       0       3
+    //
+
+    switch(wd->ledStruct.counter % 16)
+    {
+        case 0:   // case 0~3, LED[0] on
+            if(wd->supportMode & ZM_WIRELESS_MODE_24)
+            {
+                zfHpLedCtrl(dev, 0, 1);
+                zfHpLedCtrl(dev, 1, 0);
+            }
+            else
+            {
+                zfHpLedCtrl(dev, 1, 1);
+                zfHpLedCtrl(dev, 0, 0);
+            }
+            break;
+
+        case 8:   // case 8~11, LED[1] on
+            if(wd->supportMode & ZM_WIRELESS_MODE_5)
+            {
+                zfHpLedCtrl(dev, 1, 1);
+                zfHpLedCtrl(dev, 0, 0);
+            }
+            else
+            {
+                zfHpLedCtrl(dev, 0, 1);
+                zfHpLedCtrl(dev, 1, 0);
+            }
+            break;
+
+        default:  // others, all off
+            zfHpLedCtrl(dev, 0, 0);
+            zfHpLedCtrl(dev, 1, 0);
+            break;
+    }
+}
+
+/**********************************************************************************/
+/*                                                                                */
+/*    FUNCTION DESCRIPTION                  zfLedCtrlType3                        */
+/*      Customize for Netgear Single-LED state ((bug#32243))                      */
+/*                                                                                */
+/*  ¡EOff: when the adapter is disabled or hasn't started to associate with AP    */
+/*         yet.                                                                          */
+/*  ¡EOn: Once adpater associate with AP successfully                             */
+/*  ¡ESlow blinking: whenever adapters do site-survey or try to associate with AP */
+/*    - If there is a connection already, and adapters do site-survey or          */
+/*      re-associate action, the LED should keep LED backgraoud as ON, thus       */
+/*      the blinking behavior SHOULD be OFF (200ms) - ON (800ms) and continue this*/
+/*      cycle.                                                                    */
+/*    - If there is no connection yet, and adapters start to do site-survey or    */
+/*      associate action, the LED should keep LED background as OFF, thus the     */
+/*      blinking behavior SHOULD be ON (200ms) - OFF (800ms) and continue this    */
+/*      cycle.                                                                    */
+/*    - For the case that associate fail, adpater should keep associating, and the*/
+/*      LED should also keep slow blinking.                                       */
+/*  ¡EQuick blinking: to blink OFF-ON cycle for each time that traffic packet is  */
+/*    received or is transmitted.                                                 */
+/*                                                                                */
+/*    INPUTS                                                                      */
+/*      dev : device pointer                                                      */
+/*                                                                                */
+/*    OUTPUTS                                                                     */
+/*      None                                                                      */
+/*                                                                                */
+/*    AUTHOR                                                                      */
+/*      Shang-Chun Liu        Atheros Communications, INC.    2008.01             */
+/*                                                                                */
+/**********************************************************************************/
+void zfLedCtrlType3_scan(zdev_t* dev, u16_t isConnect);
+
+void zfLedCtrlType3(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (zfStaIsConnected(dev) != TRUE)
+    {
+        // Disconnect state
+        if(wd->ledStruct.counter % 2 != 0)
+    	{
+      	    // Update LED each 200ms(2*100)
+      	    // Prevent this situation
+            //              ___     _
+            // LED[0] ON   |   |   |x|
+            // ------ OFF->+-+-+-+-+-+-+->>>...
+            //
+            return;
+        }
+
+        if (((wd->state == ZM_WLAN_STATE_DISABLED) && (wd->sta.bChannelScan))
+            || ((wd->state != ZM_WLAN_STATE_DISABLED) && (wd->sta.bAutoReconnect)))
+        {
+            // Scan/AutoReconnect state
+            zfLedCtrlType3_scan(dev, 0);
+        }
+        else
+        {
+            // Neither Connected nor Scan
+            zfHpLedCtrl(dev, 0, 0);
+            zfHpLedCtrl(dev, 1, 0);
+        }
+    }
+    else
+    {
+        if( wd->sta.bChannelScan )
+        {
+            // Scan state
+            if(wd->ledStruct.counter % 2 != 0)
+                return;
+            zfLedCtrlType3_scan(dev, 1);
+            return;
+        }
+
+        if ((zfPowerSavingMgrIsSleeping(dev)) && ((wd->ledStruct.ledMode[0] & 0x8) == 0))
+        {
+            // If Sleeping, turn OFF
+            zfHpLedCtrl(dev, 0, 0);
+            zfHpLedCtrl(dev, 1, 0);
+        }
+        else
+        {
+            //Connect state
+            if ((wd->ledStruct.counter & 1) == 0)   // even
+            {
+                // No traffic, always ON
+                zfHpLedCtrl(dev, 0, 1);
+                zfHpLedCtrl(dev, 1, 1);
+            }
+            else       // odd
+            {
+                if ((wd->ledStruct.txTraffic > 0) || (wd->ledStruct.rxTraffic > 0))
+                {
+                    // If have traffic, turn OFF
+		            //                   _____   _   _   _   _____
+		            // LED[Operate] ON        | | | | | | | |
+		            // ------------ OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+->>>...
+		            //
+                    wd->ledStruct.txTraffic = wd->ledStruct.rxTraffic = 0;
+                    zfHpLedCtrl(dev, 0, 0);
+                    zfHpLedCtrl(dev, 1, 0);
+                }
+            }
+        }
+    }
+}
+
+void zfLedCtrlType3_scan(zdev_t* dev, u16_t isConnect)
+{
+    u32_t ton, toff, tmp;
+    zmw_get_wlan_dev(dev);
+
+    // Doing scan when :
+    // 1. Disconnected: ON (200ms) - OFF (800ms) (200ms-600ms in our driver)
+    //               ___             ___             ___
+    // LED[0] ON    |   |           |   |           |   |
+    // ------ OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+->>>...
+    //              0   2   4   6   8  10  12  14  16
+    // 2. Connected:   ON (800ms) - OFF (200ms) (600ms-200ms in our driver)
+    //               ___________     ___________     ______
+    // LED[0] ON    |           |   |           |   |
+    // ------ OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+->>>...
+    //              0   2   4   6   8  10  12  14  16
+
+    //Scan state
+    if(!isConnect)
+        ton = 2, toff = 6;
+    else
+        ton = 6, toff = 2;
+
+    if ((ton + toff) != 0)
+    {
+        tmp = wd->ledStruct.counter % (ton+toff);
+       if (tmp < ton)
+        {
+            zfHpLedCtrl(dev, 0, 1);
+            zfHpLedCtrl(dev, 1, 1);
+        }
+        else
+        {
+            zfHpLedCtrl(dev, 0, 0);
+            zfHpLedCtrl(dev, 1, 0);
+        }
+    }
+}
+
+/******************************************************************************/
+/*                                                                            */
+/*    FUNCTION DESCRIPTION                  zfLedCtrl_BlinkWhenScan_Alpha     */
+/*      Customize for Alpha/DLink LED                                         */
+/*      - Blink LED 12 times within 3 seconds when doing Active Scan          */
+/*	                      ___   ___   ___   ___                               */
+/*	      LED[0] ON      |   | |   | |   | |   |                              */
+/*	      -------OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+--+-->>>...                   */
+/*                                                                            */
+/*    INPUTS                                                                  */
+/*      dev : device pointer                                                  */
+/*                                                                            */
+/*    OUTPUTS                                                                 */
+/*      None                                                                  */
+/*                                                                            */
+/*    AUTHOR                                                                  */
+/*      Shang-Chun Liu        Atheros Communications, INC.    2007.11         */
+/*                                                                            */
+/******************************************************************************/
+void zfLedCtrl_BlinkWhenScan_Alpha(zdev_t* dev)
+{
+    static u32_t counter = 0;
+    zmw_get_wlan_dev(dev);
+
+    if(counter > 34)        // counter for 3 sec
+    {
+        wd->ledStruct.LEDCtrlFlag &= ~(u8_t)ZM_LED_CTRL_FLAG_ALPHA;
+        counter = 0;
+    }
+
+    if( (counter % 3) < 2)
+        zfHpLedCtrl(dev, 0, 1);
+    else
+        zfHpLedCtrl(dev, 0, 0);
+
+    counter++;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfLed100msCtrl              */
+/*      LED 100 milliseconds timer.                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.6      */
+/*                                                                      */
+/************************************************************************/
+void zfLed100msCtrl(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ledStruct.counter++;
+
+    if(wd->ledStruct.LEDCtrlFlag)
+    {
+        switch(wd->ledStruct.LEDCtrlFlag) {
+        case ZM_LED_CTRL_FLAG_ALPHA:
+            zfLedCtrl_BlinkWhenScan_Alpha(dev);
+        break;
+        }
+    }
+    else
+    {
+        switch(wd->ledStruct.LEDCtrlType) {
+        case 1:			// Traditional 1 LED
+            zfLedCtrlType1(dev);
+        break;
+
+        case 2:			// Dual-LEDs for Netgear
+            zfLedCtrlType2(dev);
+        break;
+
+        case 3:			// Single-LED for Netgear (WN111v2)
+            zfLedCtrlType3(dev);
+        break;
+
+        default:
+            zfLedCtrlType1(dev);
+        break;
+        }
+    }
+}
+
diff --git a/drivers/staging/otus/80211core/performance.c b/drivers/staging/otus/80211core/performance.c
new file mode 100644
index 000000000000..51b42d54f653
--- /dev/null
+++ b/drivers/staging/otus/80211core/performance.c
@@ -0,0 +1,431 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : performance.c                                         */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module performance evaluation functions.                   */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#ifdef ZM_ENABLE_PERFORMANCE_EVALUATION
+
+#define ZM_TP_SIZE 50
+struct zsSummary zm_summary;
+struct zsVariation zm_var;
+struct zsThroughput zm_tp;
+
+void zfiPerformanceInit(zdev_t* dev)
+{
+    u16_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_summary.tick_base = wd->tick;
+    zm_summary.tx_msdu_count = 0;
+    zm_summary.tx_mpdu_count = 0;
+    zm_summary.rx_msdu_count = 0;
+    zm_summary.rx_mpdu_count = 0;
+    zm_summary.rx_broken_seq = 0;
+    zm_summary.rx_broken_sum = 0;
+    zm_summary.rx_seq_base = 0;
+    zm_summary.rx_broken_seq_dis = 0;
+    zm_summary.rx_duplicate_seq = 0;
+    zm_summary.rx_old_seq = 0;
+    zm_summary.reset_count = 0;
+    zm_summary.reset_sum = 0;
+    zm_summary.rx_lost_sum = 0;
+    zm_summary.rx_duplicate_error = 0;
+    zm_summary.rx_free = 0;
+    zm_summary.rx_amsdu_len = 0;
+    zm_summary.rx_flush = 0;
+    zm_summary.rx_clear = 0;
+    zm_summary.rx_reorder = 0;
+
+    for (i=0; i<100; i++)
+    {
+        zm_var.tx_msdu_tick[i] = zm_var.tx_mpdu_tick[i] = 0;
+        zm_var.rx_msdu_tick[i] = zm_var.rx_mpdu_tick[i] = 0;
+    }
+
+    zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_PERFORMANCE, 100);
+
+    zm_tp.size = ZM_TP_SIZE;
+    zm_tp.head = zm_tp.size - 1;
+    zm_tp.tail = 0;
+    for (i=0; i<zm_tp.size; i++)
+    {
+        zm_tp.tx[i]=0;
+        zm_tp.rx[i]=0;
+    }
+}
+
+void zfiPerformanceGraph(zdev_t* dev)
+{
+    s16_t   i,j;
+    u8_t    s[ZM_TP_SIZE+5];
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<(zm_tp.size-1); i++)
+    {
+        zm_tp.tx[i] = zm_tp.tx[i+1];
+        zm_tp.rx[i] = zm_tp.rx[i+1];
+    }
+    zm_tp.tx[zm_tp.size-1] = zm_summary.tx_mpdu_count*1500*8/1000000;
+    zm_tp.rx[zm_tp.size-1] = zm_summary.rx_msdu_count*1500*8/1000000;
+
+    for (i=15; i>0; i--)
+    {
+        s[0] = (i/10) + '0';
+        s[1] = (i%10) + '0';
+        s[2] = '0';
+        s[3] = '|';
+        for (j=0; j<zm_tp.size; j++)
+        {
+            if ((zm_tp.tx[j]/10 == i) && (zm_tp.rx[j]/10 == i))
+            {
+                s[4+j] = 'X';
+            }
+            else if (zm_tp.tx[j]/10 == i)
+            {
+                s[4+j] = 'T';
+            }
+            else if (zm_tp.rx[j]/10 == i)
+            {
+                s[4+j] = 'R';
+            }
+            else
+            {
+                s[4+j] = ' ';
+            }
+        }
+        s[zm_tp.size+4] = '\0';
+        DbgPrint("%s",s);
+    }
+    DbgPrint("000|__________________________________________________");
+
+}
+
+
+void zfiPerformanceRefresh(zdev_t* dev)
+{
+    u16_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zfiDbgReadReg(dev, 0x11772c);
+
+    zm_var.tx_msdu_mean = zm_summary.tx_msdu_count / 100;
+    zm_var.tx_mpdu_mean = zm_summary.tx_mpdu_count / 100;
+    zm_var.rx_msdu_mean = zm_summary.rx_msdu_count / 100;
+    zm_var.rx_mpdu_mean = zm_summary.rx_mpdu_count / 100;
+
+    zm_var.tx_msdu_sum = zm_var.tx_mpdu_sum = 0;
+    zm_var.rx_msdu_sum = zm_var.rx_mpdu_sum = 0;
+    zm_summary.tx_idle_count = zm_summary.rx_idle_count = 0;
+    for (i=0; i<100; i++)
+    {
+        zm_var.tx_msdu_sum += (zm_var.tx_msdu_tick[i] * zm_var.tx_msdu_tick[i]);
+        zm_var.tx_mpdu_sum += (zm_var.tx_mpdu_tick[i] * zm_var.tx_mpdu_tick[i]);
+        zm_var.rx_msdu_sum += (zm_var.rx_msdu_tick[i] * zm_var.rx_msdu_tick[i]);
+        zm_var.rx_mpdu_sum += (zm_var.rx_mpdu_tick[i] * zm_var.rx_mpdu_tick[i]);
+
+        if (!zm_var.tx_mpdu_tick[i]) zm_summary.tx_idle_count++;
+        if (!zm_var.rx_mpdu_tick[i]) zm_summary.rx_idle_count++;
+    }
+    zm_var.tx_msdu_var = (zm_var.tx_msdu_sum / 100) - (zm_var.tx_msdu_mean * zm_var.tx_msdu_mean);
+    zm_var.tx_mpdu_var = (zm_var.tx_mpdu_sum / 100) - (zm_var.tx_mpdu_mean * zm_var.tx_mpdu_mean);
+    zm_var.rx_msdu_var = (zm_var.rx_msdu_sum / 100) - (zm_var.rx_msdu_mean * zm_var.rx_msdu_mean);
+    zm_var.rx_mpdu_var = (zm_var.rx_mpdu_sum / 100) - (zm_var.rx_mpdu_mean * zm_var.rx_mpdu_mean);
+
+    zm_summary.tick_base = wd->tick;
+    zm_summary.rx_broken_sum += zm_summary.rx_broken_seq;
+    zm_summary.rx_lost_sum += (zm_summary.rx_broken_seq - zm_summary.rx_duplicate_seq - zm_summary.rx_old_seq);
+
+    zfiPerformanceGraph(dev);
+
+    DbgPrint("******************************************************\n");
+    DbgPrint("* TX: MSDU=%5d, VAR=%5d; MPDU=%5d, VAR=%5d\n", zm_summary.tx_msdu_count,
+        zm_var.tx_msdu_var, zm_summary.tx_mpdu_count, zm_var.tx_mpdu_var);
+    DbgPrint("* TX: idle=%5d,TxRate=%3d,  PER=%5d\n", zm_summary.tx_idle_count,
+        wd->CurrentTxRateKbps/1000,
+        (u16_t)wd->PER[wd->sta.oppositeInfo[0].rcCell.currentRate]);
+    DbgPrint("* RX: MSDU=%5d, VAR=%5d; MPDU=%5d, VAR=%5d\n", zm_summary.rx_msdu_count,
+        zm_var.rx_msdu_var, zm_summary.rx_mpdu_count, zm_var.rx_mpdu_var);
+    DbgPrint("* RX: idle=%5d,RxRate=%3d,AMSDU=%5d\n", zm_summary.rx_idle_count,
+        wd->CurrentRxRateKbps/1000, zm_summary.rx_amsdu_len);
+    DbgPrint("* RX broken seq=%4d, distances=%4d, duplicates=%4d\n", zm_summary.rx_broken_seq,
+        zm_summary.rx_broken_seq_dis, zm_summary.rx_duplicate_seq);
+    DbgPrint("* RX    old seq=%4d,      lost=%4d, broken sum=%4d\n", zm_summary.rx_old_seq,
+        (zm_summary.rx_broken_seq - zm_summary.rx_duplicate_seq - zm_summary.rx_old_seq),
+        zm_summary.rx_broken_sum);
+    DbgPrint("* Rx   lost sum=%4d,dup. error=%4d, free count=%4d\n", zm_summary.rx_lost_sum,
+        zm_summary.rx_duplicate_error, zm_summary.rx_free);
+    DbgPrint("* Rx  flush sum=%4d, clear sum=%4d, reorder=%7d\n", zm_summary.rx_flush,
+        zm_summary.rx_clear, zm_summary.rx_reorder);
+    DbgPrint("* Firmware reset=%3d, reset sum=%4d\n", zm_summary.reset_count,
+        zm_summary.reset_sum);
+    DbgPrint("******************************************************\n\n");
+    //reset count 11772c
+    zm_summary.tx_msdu_count = 0;
+    zm_summary.tx_mpdu_count = 0;
+    zm_summary.rx_msdu_count = 0;
+    zm_summary.rx_mpdu_count = 0;
+    zm_summary.rx_broken_seq = 0;
+    zm_summary.rx_broken_seq_dis = 0;
+    zm_summary.rx_duplicate_seq = 0;
+    zm_summary.rx_old_seq = 0;
+    zm_summary.reset_count = 0;
+    zm_summary.rx_amsdu_len = 0;
+
+    for (i=0; i<100; i++)
+    {
+        zm_var.tx_msdu_tick[i] = zm_var.tx_mpdu_tick[i] = 0;
+        zm_var.rx_msdu_tick[i] = zm_var.rx_mpdu_tick[i] = 0;
+    }
+
+    zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_PERFORMANCE, 100);
+}
+
+void zfiTxPerformanceMSDU(zdev_t* dev, u32_t tick)
+{
+    u32_t   index;
+    zm_summary.tx_msdu_count++;
+
+    index = tick - zm_summary.tick_base;
+
+    if (index < 100)
+    {
+        zm_var.tx_msdu_tick[index]++;
+    }
+    else
+    {
+        //DbgPrint("wd->tick exceeded tick_base+100!\n");
+    }
+}
+
+void zfiRxPerformanceMSDU(zdev_t* dev, u32_t tick)
+{
+    u32_t   index;
+    zm_summary.rx_msdu_count++;
+
+    index = tick - zm_summary.tick_base;
+
+    if (index < 100)
+    {
+        zm_var.rx_msdu_tick[index]++;
+    }
+    else
+    {
+        //DbgPrint("wd->tick exceeded tick_base+100!\n");
+    }
+}
+
+void zfiTxPerformanceMPDU(zdev_t* dev, u32_t tick)
+{
+    u32_t   index;
+    zm_summary.tx_mpdu_count++;
+
+    index = tick - zm_summary.tick_base;
+
+    if (index < 100)
+    {
+        zm_var.tx_mpdu_tick[index]++;
+    }
+    else
+    {
+        //DbgPrint("wd->tick exceeded tick_base+100!\n");
+    }
+}
+
+#ifndef ZM_INT_USE_EP2_HEADER_SIZE
+#define ZM_INT_USE_EP2_HEADER_SIZE   12
+#endif
+void zfiRxPerformanceMPDU(zdev_t* dev, zbuf_t* buf)
+{
+    u32_t   index;
+    u16_t   frameType;
+    u16_t   frameCtrl;
+    u8_t    mpduInd;
+    u16_t   plcpHdrLen;
+    u16_t   len;
+
+    zmw_get_wlan_dev(dev);
+
+    len = zfwBufGetSize(dev, buf);
+    mpduInd = zmw_rx_buf_readb(dev, buf, len-1);
+    /* First MPDU or Single MPDU */
+    if(((mpduInd & 0x30) == 0x00) || ((mpduInd & 0x30) == 0x20))
+    //if ((mpduInd & 0x10) == 0x00)
+    {
+        plcpHdrLen = 12;        // PLCP header length
+    }
+    else
+    {
+        if (zmw_rx_buf_readh(dev, buf, 4) == wd->macAddr[0] &&
+            zmw_rx_buf_readh(dev, buf, 6) == wd->macAddr[1] &&
+            zmw_rx_buf_readh(dev, buf, 8) == wd->macAddr[2]) {
+            plcpHdrLen = 0;
+        }
+        else if (zmw_rx_buf_readh(dev, buf, 16) == wd->macAddr[0] &&
+                 zmw_rx_buf_readh(dev, buf, 18) == wd->macAddr[1] &&
+                 zmw_rx_buf_readh(dev, buf, 20) == wd->macAddr[2]){
+            plcpHdrLen = 12;
+        }
+        else {
+            plcpHdrLen = 0;
+        }
+    }
+
+    frameCtrl = zmw_rx_buf_readb(dev, buf, plcpHdrLen + 0);
+    frameType = frameCtrl & 0xf;
+
+    if (frameType != ZM_WLAN_DATA_FRAME)
+    {
+        return;
+    }
+
+    zm_summary.rx_mpdu_count++;
+
+    index = wd->tick - zm_summary.tick_base;
+
+    if (index < 100)
+    {
+        zm_var.rx_mpdu_tick[index]++;
+    }
+    else
+    {
+        //DbgPrint("wd->tick exceeded tick_base+100!\n");
+    }
+}
+
+void zfiRxPerformanceSeq(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t   seq_no;
+    u16_t   offset = 0;
+    u16_t   old_dis = zm_summary.rx_broken_seq_dis;
+    //sys_time = KeQueryPerformanceCounter(&freq);
+
+    seq_no = zmw_rx_buf_readh(dev, buf, offset+22) >> 4;
+
+    ZM_SEQ_DEBUG("Out   %5d\n", seq_no);
+
+    if (seq_no < zm_summary.rx_seq_base)
+    {
+        if (seq_no == 0)
+        {
+            if (zm_summary.rx_seq_base != 4095)
+            {
+                zm_summary.rx_broken_seq++;
+                ZM_SEQ_DEBUG("Broken seq");
+                zm_summary.rx_broken_seq_dis+=(4096 - zm_summary.rx_seq_base);
+            }
+        }
+        else if ((seq_no < 300) && (zm_summary.rx_seq_base > 3800))
+        {
+            zm_summary.rx_broken_seq++;
+            ZM_SEQ_DEBUG("Broken seq");
+            zm_summary.rx_broken_seq_dis+=(4096 - zm_summary.rx_seq_base + seq_no);
+        }
+        else
+        {
+            zm_summary.rx_broken_seq++;
+            ZM_SEQ_DEBUG("Broken seq");
+            zm_summary.rx_broken_seq_dis+=(zm_summary.rx_seq_base - seq_no);
+            zm_summary.rx_old_seq++;
+        }
+    }
+    else
+    {
+        if (seq_no != (zm_summary.rx_seq_base + 1))
+        {
+            if ((seq_no > 3800) && (zm_summary.rx_seq_base < 300))
+            {
+                zm_summary.rx_broken_seq++;
+                ZM_SEQ_DEBUG("Broken seq");
+                zm_summary.rx_broken_seq_dis+=(4096 - seq_no + zm_summary.rx_seq_base);
+                zm_summary.rx_old_seq++;
+            }
+            else
+            {
+                zm_summary.rx_broken_seq++;
+                ZM_SEQ_DEBUG("Broken seq");
+                zm_summary.rx_broken_seq_dis+=(seq_no - zm_summary.rx_seq_base);
+            }
+        }
+    }
+    if (seq_no == zm_summary.rx_seq_base)
+    {
+        zm_summary.rx_duplicate_seq++;
+    }
+
+    if ((zm_summary.rx_broken_seq_dis - old_dis) > 100)
+    {
+        DbgPrint("* seq_no=%4d, base_seq=%4d, dis_diff=%4d", seq_no,
+            zm_summary.rx_seq_base, zm_summary.rx_broken_seq_dis - old_dis);
+    }
+    zm_summary.rx_seq_base = seq_no;
+}
+
+void zfiRxPerformanceReg(zdev_t* dev, u32_t reg, u32_t rsp)
+{
+    zm_summary.reset_count = (u16_t)rsp - zm_summary.reset_sum;
+    zm_summary.reset_sum = (u16_t)rsp;
+}
+
+void zfiRxPerformanceDup(zdev_t* dev, zbuf_t* buf1, zbuf_t* buf2)
+{
+    u16_t   seq_no1, seq_no2;
+
+    seq_no1 = zmw_rx_buf_readh(dev, buf1, 22) >> 4;
+    seq_no2 = zmw_rx_buf_readh(dev, buf2, 22) >> 4;
+    if (seq_no1 != seq_no2)
+    {
+        zm_summary.rx_duplicate_error++;
+    }
+}
+
+void zfiRxPerformanceFree(zdev_t* dev, zbuf_t* buf)
+{
+    zm_summary.rx_free++;
+}
+
+void zfiRxPerformanceAMSDU(zdev_t* dev, zbuf_t* buf, u16_t len)
+{
+    if (zm_summary.rx_amsdu_len < len)
+    {
+        zm_summary.rx_amsdu_len = len;
+    }
+}
+void zfiRxPerformanceFlush(zdev_t* dev)
+{
+    zm_summary.rx_flush++;
+}
+
+void zfiRxPerformanceClear(zdev_t* dev)
+{
+    zm_summary.rx_clear++;
+    ZM_SEQ_DEBUG("RxClear");
+}
+
+void zfiRxPerformanceReorder(zdev_t* dev)
+{
+    zm_summary.rx_reorder++;
+}
+#endif /* end of ZM_ENABLE_PERFORMANCE_EVALUATION */
diff --git a/drivers/staging/otus/80211core/performance.h b/drivers/staging/otus/80211core/performance.h
new file mode 100644
index 000000000000..29f658ae477c
--- /dev/null
+++ b/drivers/staging/otus/80211core/performance.h
@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#ifndef _PERFORMANCE_H
+#define _PERFORMANCE_H
+
+#ifdef ZM_ENABLE_PERFORMANCE_EVALUATION
+
+struct zsSummary
+{
+    u32_t tx_msdu_count;
+    u32_t tx_mpdu_count;
+    u32_t rx_msdu_count;
+    u32_t rx_mpdu_count;
+    u32_t tick_base;
+    u16_t rx_seq_base;
+    u16_t rx_broken_seq;
+    u16_t rx_broken_sum;
+    u16_t rx_broken_seq_dis;
+    u16_t rx_duplicate_seq;
+    u16_t rx_duplicate_error;
+    u16_t rx_old_seq;
+    u16_t rx_lost_sum;
+    u16_t tx_idle_count;
+    u16_t rx_idle_count;
+    u16_t reset_count;
+    u16_t reset_sum;
+    u16_t rx_free;
+    u16_t rx_amsdu_len;
+    u16_t rx_flush;
+    u16_t rx_clear;
+    u32_t rx_reorder;
+};
+
+struct zsVariation
+{
+    u32_t tx_msdu_tick[100];
+    u32_t tx_mpdu_tick[100];
+    u32_t rx_msdu_tick[100];
+    u32_t rx_mpdu_tick[100];
+
+    u32_t tx_msdu_mean;
+    u32_t tx_mpdu_mean;
+    u32_t rx_msdu_mean;
+    u32_t rx_mpdu_mean;
+
+    u32_t tx_msdu_sum;
+    u32_t tx_mpdu_sum;
+    u32_t rx_msdu_sum;
+    u32_t rx_mpdu_sum;
+
+    u32_t tx_msdu_var;
+    u32_t tx_mpdu_var;
+    u32_t rx_msdu_var;
+    u32_t rx_mpdu_var;
+};
+
+struct zsThroughput
+{
+    u32_t tx[50];
+    u32_t rx[50];
+    u16_t head;
+    u16_t tail;
+    u16_t size;
+    LARGE_INTEGER sys_time;
+    LARGE_INTEGER freq;
+};
+
+void zfiPerformanceInit(zdev_t* dev);
+void zfiPerformanceRefresh(zdev_t* dev);
+
+void zfiTxPerformanceMSDU(zdev_t* dev, u32_t tick);
+void zfiRxPerformanceMSDU(zdev_t* dev, u32_t tick);
+void zfiTxPerformanceMPDU(zdev_t* dev, u32_t tick);
+void zfiRxPerformanceMPDU(zdev_t* dev, zbuf_t* buf);
+void zfiRxPerformanceSeq(zdev_t* dev, zbuf_t* buf);
+void zfiRxPerformanceReg(zdev_t* dev, u32_t reg, u32_t rsp);
+void zfiRxPerformanceDup(zdev_t* dev, zbuf_t* buf1, zbuf_t* buf2);
+void zfiRxPerformanceFree(zdev_t* dev, zbuf_t* buf);
+void zfiRxPerformanceAMSDU(zdev_t* dev, zbuf_t* buf, u16_t len);
+void zfiRxPerformanceFlush(zdev_t* dev);
+void zfiRxPerformanceClear(zdev_t* dev);
+void zfiRxPerformanceReorder(zdev_t* dev);
+#endif /* end of ZM_ENABLE_PERFORMANCE_EVALUATION */
+#endif /* end of _PERFORMANCE_H */
diff --git a/drivers/staging/otus/80211core/pub_usb.h b/drivers/staging/otus/80211core/pub_usb.h
new file mode 100644
index 000000000000..c4b4bd25e828
--- /dev/null
+++ b/drivers/staging/otus/80211core/pub_usb.h
@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _PUB_USB_H
+#define _PUB_USB_H
+
+#include "../oal_dt.h"
+
+#define ZM_HAL_80211_MODE_AP              0
+#define ZM_HAL_80211_MODE_STA             1
+#define ZM_HAL_80211_MODE_IBSS_GENERAL    2
+#define ZM_HAL_80211_MODE_IBSS_WPA2PSK    3
+
+/* USB module description                                               */
+/* Queue Management                                                     */
+/* 80211core requires OAL to implement a transmission queue in OAL's    */
+/* USB module. Because there is only limited on-chip memory, so USB     */
+/* data transfer may be pending until on-chip memory is available.      */
+/* 80211core also requires OAL's USB module to provide two functions    */
+/* zfwUsbGetFreeTxQSize() and zfwUsbGetMaxTxQSize() for 80211core to    */
+/* query the status of this transmission queue. The main purpose of     */
+/* this queue is for QoS/WMM. Though there are hardware priority        */
+/* queues on the chip, and also software priority queues in the         */
+/* 80211core. There is still one and only one USB channel. So           */
+/* 80211core will use the information that zfwUsbGetFreeTxQSize()       */
+/* returned to schedule the traffic from the software priority          */
+/* queues to the hardware priority queues. For example, if 80211core    */
+/* found that USB transmission queue is going to be full, it will       */
+/* not allow packets with lower priority to enter the USB channel.      */
+
+
+/* Structure for USB call back functions */
+struct zfCbUsbFuncTbl {
+    void (*zfcbUsbRecv)(zdev_t *dev, zbuf_t *buf);
+    void (*zfcbUsbRegIn)(zdev_t* dev, u32_t* rsp, u16_t rspLen);
+    void (*zfcbUsbOutComplete)(zdev_t* dev, zbuf_t *buf, u8_t status, u8_t *hdr);
+    void (*zfcbUsbRegOutComplete)(zdev_t* dev);
+};
+
+/* Call back functions                                                  */
+/* Below are the functions that should be called by the OAL             */
+
+/* When data is available in endpoint 3, OAL shall embed the data in */
+/* zbuf_t and supply to 80211core by calling this function           */
+/* void (*zfcbUsbRecv)(zdev_t *dev, zbuf_t *buf); */
+
+/* When data is available in endpoint 2, OAL shall call this function */
+/* void (*zfcbUsbRegIn)(zdev_t* dev, u32_t* rsp, u16_t rspLen); */
+
+/* When USB data transfer completed in endpoint 1, OAL shall call this function */
+/* void (*zfcbUsbOutComplete)(zdev_t* dev, zbuf_t *buf, u8_t status, u8_t *hdr); */
+
+
+/* Call out functions                                                   */
+/* Below are the functions that supply by the OAL for 80211core to      */
+/* manipulate the USB                                                   */
+
+/* Return OAL's USB TxQ size */
+extern u32_t zfwUsbGetMaxTxQSize(zdev_t* dev);
+
+/* Return OAL's TxQ available size */
+extern u32_t zfwUsbGetFreeTxQSize(zdev_t* dev);
+
+/* Register call back function */
+extern void zfwUsbRegisterCallBack(zdev_t* dev, struct zfCbUsbFuncTbl *zfUsbFunc);
+
+/* Enable USB interrupt endpoint */
+extern u32_t zfwUsbEnableIntEpt(zdev_t *dev, u8_t endpt);
+
+/* Enable USB Rx endpoint */
+extern int zfwUsbEnableRxEpt(zdev_t* dev, u8_t endpt);
+
+/* 80211core call this function to send a USB request over endpoint 0 */
+extern u32_t zfwUsbSubmitControl(zdev_t* dev, u8_t req, u16_t value,
+        u16_t index, void *data, u32_t size);
+extern u32_t zfwUsbSubmitControlIo(zdev_t* dev, u8_t req, u8_t reqtype,
+        u16_t value, u16_t index, void *data, u32_t size);
+
+/* 80211core call this function to transfer data out over endpoint 1 */
+extern void zfwUsbCmd(zdev_t* dev, u8_t endpt, u32_t* cmd, u16_t cmdLen);
+
+/* 80211core call this function to transfer data out over endpoint 4 */
+extern u32_t zfwUsbSend(zdev_t* dev, u8_t endpt, u8_t *hdr, u16_t hdrlen, u8_t *snap, u16_t snapLen,
+                u8_t *tail, u16_t tailLen, zbuf_t *buf, u16_t offset);
+
+/* 80211core call this function to set USB configuration */
+extern u32_t zfwUsbSetConfiguration(zdev_t *dev, u16_t value);
+
+#endif
diff --git a/drivers/staging/otus/80211core/pub_zfi.h b/drivers/staging/otus/80211core/pub_zfi.h
new file mode 100644
index 000000000000..a35bd5d41d2c
--- /dev/null
+++ b/drivers/staging/otus/80211core/pub_zfi.h
@@ -0,0 +1,821 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _PUB_DEFS_H
+#define _PUB_DEFS_H
+
+#include "../oal_dt.h"
+
+/***** Section 1 : Tunable Parameters *****/
+/* The defintions in this section are tunabel parameters */
+
+/* Maximum number of BSS that could be scaned */
+#define ZM_MAX_BSS                          128
+
+/* Maximum number of WPA2 PMKID that supported */
+#define ZM_PMKID_MAX_BSS_CNT               8
+
+/* Enable aggregation and deaggregation */
+#define ZM_ENABLE_AGGREGATION
+
+#ifdef ZM_ENABLE_AGGREGATION
+    /* Enable BA failed retransmission in firmware */
+    #define ZM_ENABLE_FW_BA_RETRANSMISSION
+    #define ZM_BYPASS_AGGR_SCHEDULING
+    //#define ZM_AGGR_BIT_ON
+#endif
+
+
+#ifndef ZM_FB50
+//#define ZM_FB50
+#endif
+
+#ifndef ZM_AP_DEBUG
+//#define ZM_AP_DEBUG
+#endif
+
+//#define ZM_ENABLE_BA_RATECTRL
+
+/***** End of section 1 *****/
+
+
+/***** Section 2 : Public Definitions, data structures and prototypes *****/
+/* function return status */
+#define ZM_STATUS_SUCCESS                   0
+#define ZM_STATUS_FAILURE                   1
+
+// media connect status
+#define ZM_STATUS_MEDIA_CONNECT             0x00
+#define ZM_STATUS_MEDIA_DISCONNECT          0x01
+#define ZM_STATUS_MEDIA_DISCONNECT_NOT_FOUND    0x02
+#define ZM_STATUS_MEDIA_DISABLED            0x03
+#define ZM_STATUS_MEDIA_CONNECTION_DISABLED 0x04
+#define ZM_STATUS_MEDIA_CONNECTION_RESET    0x05
+#define ZM_STATUS_MEDIA_RESET               0x06
+#define ZM_STATUS_MEDIA_DISCONNECT_DEAUTH   0x07
+#define ZM_STATUS_MEDIA_DISCONNECT_DISASOC  0x08
+#define ZM_STATUS_MEDIA_DISCONNECT_TIMEOUT  0x09
+#define ZM_STATUS_MEDIA_DISCONNECT_AUTH_FAILED  0x0a
+#define ZM_STATUS_MEDIA_DISCONNECT_ASOC_FAILED  0x0b
+#define ZM_STATUS_MEDIA_DISCONNECT_MIC_FAIL   0x0c
+#define ZM_STATUS_MEDIA_DISCONNECT_UNREACHABLE 0x0d
+#define ZM_STATUS_MEDIA_DISCONNECT_BEACON_MISS  0x0e
+
+// Packet Filter
+#define ZM_PACKET_TYPE_DIRECTED             0x00000001
+#define ZM_PACKET_TYPE_MULTICAST            0x00000002
+#define ZM_PACKET_TYPE_ALL_MULTICAST        0x00000004
+#define ZM_PACKET_TYPE_BROADCAST            0x00000008
+#define ZM_PACKET_TYPE_PROMISCUOUS          0x00000020
+
+/* BSS mode definition */
+/* TODO : The definitions here are coupled with XP's NDIS OID. */
+/*        We can't be changed them freely, need to disarm this mine */
+#define ZM_MODE_IBSS                        0
+#define ZM_MODE_INFRASTRUCTURE              1
+#define ZM_MODE_UNKNOWN                     2
+#define ZM_MODE_INFRASTRUCTURE_MAX          3
+#define ZM_MODE_AP                          4
+#define ZM_MODE_PSEUDO                      5
+
+
+/* Authentication mode */
+#define ZM_AUTH_MODE_OPEN                   0
+#define ZM_AUTH_MODE_SHARED_KEY             1
+#define ZM_AUTH_MODE_AUTO                   2
+#define ZM_AUTH_MODE_WPA                    3
+#define ZM_AUTH_MODE_WPAPSK                 4
+#define ZM_AUTH_MODE_WPA_NONE               5
+#define ZM_AUTH_MODE_WPA2                   6
+#define ZM_AUTH_MODE_WPA2PSK                7
+#ifdef ZM_ENABLE_CENC
+#define ZM_AUTH_MODE_CENC                   8
+#endif //ZM_ENABLE_CENC
+#define ZM_AUTH_MODE_WPA_AUTO               9
+#define ZM_AUTH_MODE_WPAPSK_AUTO            10
+
+// Encryption mode
+#define ZM_NO_WEP                           0x0
+#define ZM_AES                              0x4
+#define ZM_TKIP                             0x2
+#define ZM_WEP64                            0x1
+#define ZM_WEP128                           0x5
+#define ZM_WEP256                           0x6
+#ifdef ZM_ENABLE_CENC
+#define ZM_CENC                             0x7
+#endif //ZM_ENABLE_CENC
+
+/* Encryption type for wep status */
+#define ZM_ENCRYPTION_WEP_DISABLED          0
+#define ZM_ENCRYPTION_WEP_ENABLED           1
+#define ZM_ENCRYPTION_WEP_KEY_ABSENT        2
+#define ZM_ENCRYPTION_NOT_SUPPORTED         3
+#define ZM_ENCRYPTION_TKIP                  4
+#define ZM_ENCRYPTION_TKIP_KEY_ABSENT       5
+#define ZM_ENCRYPTION_AES                   6
+#define ZM_ENCRYPTION_AES_KEY_ABSENT        7
+
+#ifdef ZM_ENABLE_CENC
+#define ZM_ENCRYPTION_CENC                  8
+#endif //ZM_ENABLE_CENC
+
+/* security type */
+#define ZM_SECURITY_TYPE_NONE               0
+#define ZM_SECURITY_TYPE_WEP                1
+#define ZM_SECURITY_TYPE_WPA                2
+
+#ifdef ZM_ENABLE_CENC
+#define ZM_SECURITY_TYPE_CENC               3
+#endif //ZM_ENABLE_CENC
+
+/* Encryption Exemption Action Type  */
+#define ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION   0
+#define ZM_ENCRYPTION_EXEMPT_ALWAYS         1
+
+/* MIC failure */
+#define ZM_MIC_PAIRWISE_ERROR               0x06
+#define ZM_MIC_GROUP_ERROR                  0x0E
+
+
+/* power save mode */
+#define ZM_STA_PS_NONE                    0
+#define ZM_STA_PS_MAX                     1
+#define ZM_STA_PS_FAST                    2
+#define ZM_STA_PS_LIGHT                   3
+
+/* WME AC Type */
+#define ZM_WME_AC_BK                        0       /* Background AC */
+#define ZM_WME_AC_BE                        1       /* Best-effort AC */
+#define ZM_WME_AC_VIDEO                     2       /* Video AC */
+#define ZM_WME_AC_VOICE                     3       /* Voice AC */
+
+/* Preamble type */
+#define ZM_PREAMBLE_TYPE_AUTO               0
+#define ZM_PREAMBLE_TYPE_LONG               1
+#define ZM_PREAMBLE_TYPE_SHORT              2
+
+/* wireless modes constants */
+#define ZM_WIRELESS_MODE_5_54        0x01   ///< 5 GHz 54 Mbps
+#define ZM_WIRELESS_MODE_5_108       0x02   ///< 5 GHz 108 Mbps
+#define ZM_WIRELESS_MODE_24_11       0x04   ///< 2.4 GHz 11 Mbps
+#define ZM_WIRELESS_MODE_24_54       0x08   ///< 2.4 GHz 54 Mbps
+#define ZM_WIRELESS_MODE_24_108      0x10   ///< 2.4 GHz 108 Mbps
+#define ZM_WIRELESS_MODE_49_13      0x100   ///< 4.9 GHz 13.5 Mbps, quarter rate chn-bandwidth = 5
+#define ZM_WIRELESS_MODE_49_27      0x200   ///< 4.9 GHz 27 Mbps, half rate chn-bandwidth = 10
+#define ZM_WIRELESS_MODE_49_54      0x400   ///< 4.9 GHz 54 Mbps, full rate chn-bandwidth = 20
+#define ZM_WIRELESS_MODE_5_300     0x1000   ///< 5 GHz 300 Mbps
+#define ZM_WIRELESS_MODE_24_300    0x2000   ///< 2.4 GHz 300 Mbps
+#define ZM_WIRELESS_MODE_5_130     0x4000   ///< 5 GHz 130 Mbps
+#define ZM_WIRELESS_MODE_24_130    0x8000   ///< 2.4 GHz 130 Mbps
+
+#define ZM_WIRELESS_MODE_24_N      (ZM_WIRELESS_MODE_24_130|ZM_WIRELESS_MODE_24_300)
+#define ZM_WIRELESS_MODE_5_N       (ZM_WIRELESS_MODE_5_130|ZM_WIRELESS_MODE_5_300)
+#define ZM_WIRELESS_MODE_24        (ZM_WIRELESS_MODE_24_11|ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)
+#define ZM_WIRELESS_MODE_5         (ZM_WIRELESS_MODE_5_54|ZM_WIRELESS_MODE_5_N)
+
+/* AdHoc Mode with different band */
+#define ZM_ADHOCBAND_A         1
+#define ZM_ADHOCBAND_B         2
+#define ZM_ADHOCBAND_G         3
+#define ZM_ADHOCBAND_BG        4
+#define ZM_ADHOCBAND_ABG       5
+
+/* Authentication algorithm in the field algNo of authentication frames */
+#define ZM_AUTH_ALGO_OPEN_SYSTEM      0x10000    /* Open system */
+#define ZM_AUTH_ALGO_SHARED_KEY       0x10001    /* Shared Key */
+#define ZM_AUTH_ALGO_LEAP             0x10080    /* Leap */
+
+struct zsScanResult
+{
+    u32_t reserved;
+};
+
+
+struct zsStastics
+{
+    u32_t reserved;
+};
+
+#define ZM_MAX_SUPP_RATES_IE_SIZE       12
+#define ZM_MAX_IE_SIZE                  50 //100
+#define ZM_MAX_WPS_IE_SIZE              150
+#define ZM_MAX_PROBE_FRAME_BODY_SIZE    512//300
+#define	ZM_MAX_COUNTRY_INFO_SIZE		20
+
+#define ZM_MAX_SSID_LENGTH          32
+struct zsBssInfo
+{
+    u8_t   macaddr[6];
+    u8_t   bssid[6];
+    u8_t   beaconInterval[2];
+    u8_t   capability[2];
+    u8_t   timeStamp[8];
+    u8_t   ssid[ZM_MAX_SSID_LENGTH + 2];   // EID(1) + Length(1) + SSID(32)
+    u8_t   supportedRates[ZM_MAX_SUPP_RATES_IE_SIZE + 2]; // EID(1) + Length(1) + supported rates [12]
+    u8_t   channel;
+    u16_t  frequency;
+    u16_t  atimWindow;
+    u8_t   erp;
+    u8_t   extSupportedRates[ZM_MAX_SUPP_RATES_IE_SIZE + 2]; // EID(1) + Length(1) + extended supported rates [12]
+    u8_t   wpaIe[ZM_MAX_IE_SIZE + 2];
+    u8_t   wscIe[ZM_MAX_WPS_IE_SIZE + 2];
+    u8_t   rsnIe[ZM_MAX_IE_SIZE + 2];
+#ifdef ZM_ENABLE_CENC
+    u8_t   cencIe[ZM_MAX_IE_SIZE + 2]; /* CENC */ /* half size because of memory exceed 64k boundary */
+#endif //ZM_ENABLE_CENC
+    u8_t   securityType;
+    u8_t   signalStrength;
+    u8_t   signalQuality;
+    u16_t  sortValue;
+    u8_t   wmeSupport;
+    u8_t   flag;
+    u8_t   EnableHT;
+    u8_t   enableHT40;
+    u8_t   SG40;
+    u8_t   extChOffset;
+    u8_t   apCap; // bit0:11N AP
+    u16_t  frameBodysize;
+    u8_t   frameBody[ZM_MAX_PROBE_FRAME_BODY_SIZE];
+    u8_t   countryInfo[ZM_MAX_COUNTRY_INFO_SIZE + 2];
+    u16_t  athOwlAp;
+    u16_t  marvelAp;
+    u16_t  broadcomHTAp;
+    u32_t  tick;
+    struct zsBssInfo* next;
+};
+
+struct zsBssList
+{
+    u8_t bssCount;
+    struct zsBssInfo* head;
+    struct zsBssInfo* tail;
+};
+
+struct zsBssListV1
+{
+    u8_t bssCount;
+    struct zsBssInfo bssInfo[ZM_MAX_BSS];
+};
+
+#define ZM_KEY_FLAG_GK                 0x0001
+#define ZM_KEY_FLAG_PK                 0X0002
+#define ZM_KEY_FLAG_AUTHENTICATOR      0x0004
+#define ZM_KEY_FLAG_INIT_IV            0x0008
+#define ZM_KEY_FLAG_DEFAULT_KEY        0x0010
+
+#ifdef ZM_ENABLE_CENC
+#define ZM_KEY_FLAG_CENC               0x0020
+#endif //ZM_ENABLE_CENC
+
+// Comment: For TKIP, key[0]~key[15]  => TKIP key
+//                    key[16]~key[23] => Tx MIC key
+//                    key[24]~key[31] => Rx MIC key
+struct zsKeyInfo
+{
+    u8_t*   key;
+    u8_t    keyLength;
+    u8_t    keyIndex;
+    u8_t*   initIv;
+    u16_t   flag;
+    u8_t    vapId;
+    u16_t    vapAddr[3];
+    u16_t*   macAddr;
+};
+
+
+
+/*
+ * Channels are specified by frequency.
+ */
+typedef struct {
+	u16_t	channel;	/* setting in Mhz */
+	u32_t	channelFlags;	/* see below */
+	u8_t	privFlags;
+	s8_t	maxRegTxPower;	/* max regulatory tx power in dBm */
+	s8_t	maxTxPower;	/* max true tx power in 0.25 dBm */
+	s8_t	minTxPower;	/* min true tx power in 0.25 dBm */
+} ZM_HAL_CHANNEL;
+
+struct zsRegulationTable
+{
+    u16_t   regionCode;
+    u16_t   CurChIndex;
+    u16_t   allowChannelCnt;
+    ZM_HAL_CHANNEL   allowChannel[60];   /* 2.4GHz: 14 channels, 5 GHz: 31 channels */
+};
+
+struct zsPartnerNotifyEvent
+{
+    u8_t bssid[6];                      // The BSSID of IBSS
+    u8_t peerMacAddr[6];                // The MAC address of peer station
+};
+
+#define ZM_RC_TRAINED_BIT   0x1
+struct zsRcCell
+{
+    u32_t txCount;
+    u32_t failCount;
+    u8_t currentRate;
+    u8_t currentRateIndex;
+    u32_t probingTime;
+    u8_t operationRateSet[24];
+    u8_t operationRateCount;
+    u16_t rxRssi;
+    u8_t flag;
+    u32_t  lasttxCount;
+    u32_t  lastTime;
+};
+
+struct zsOppositeInfo
+{
+    u8_t            macAddr[6];
+    struct zsRcCell rcCell;
+    u8_t            valid;              // This indicate if this opposite is still valid
+    u8_t            aliveCounter;
+    u8_t            pkInstalled;
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    /* For WPA2PSK ! */
+    u8_t 			wpaState;
+    u8_t            camIdx;
+    u8_t            encryMode;
+    u16_t           iv16;
+    u32_t           iv32;
+#endif
+};
+
+typedef void (*zfpIBSSIteratePeerStationCb)(
+    zdev_t* dev, struct zsOppositeInfo *peerInfo, void *ctx, u8_t index);
+
+typedef u16_t (*zfpStaRxSecurityCheckCb)(zdev_t* dev, zbuf_t* buf);
+
+
+/* Communication Tally data structure */
+struct zsCommTally
+{
+    u32_t txUnicastFrm;		    //  0 txUnicastFrames
+    u32_t txMulticastFrm;		//  1 txMulticastFrames
+    u32_t txUnicastOctets;	    //  2 txUniOctets  byte size
+    u32_t txMulticastOctets;	//  3 txMultiOctets  byte size
+    u32_t txFrmUpperNDIS;       //  4
+    u32_t txFrmDrvMgt;          //  5
+    u32_t RetryFailCnt;		    //  6
+    u32_t Hw_TotalTxFrm;		//  7 Hardware total Tx Frame
+    u32_t Hw_RetryCnt;		    //  8 txMultipleRetriesFrames
+    u32_t Hw_UnderrunCnt;       //  9
+
+    u32_t DriverRxFrmCnt;       // 10
+    u32_t rxUnicastFrm;		    // 11 rxUnicastFrames
+    u32_t rxMulticastFrm;	    // 12rxMulticastFrames
+
+    u32_t NotifyNDISRxFrmCnt;   // 14
+    u32_t rxUnicastOctets;	// 15 rxUniOctets  byte size
+    u32_t rxMulticastOctets;	    // 16 rxMultiOctets  byte size
+    u32_t DriverDiscardedFrm;       // 17 Discard by ValidateFrame
+    u32_t LessThanDataMinLen;       // 18
+    u32_t GreaterThanMaxLen;        // 19
+    u32_t DriverDiscardedFrmCauseByMulticastList;
+    u32_t DriverDiscardedFrmCauseByFrmCtrl;
+    u32_t rxNeedFrgFrm;		    // 22 need more frg frm
+    u32_t DriverRxMgtFrmCnt;
+    u32_t rxBroadcastFrm;	    // 24 Receive broadcast frame count
+    u32_t rxBroadcastOctets;	// 25 Receive broadcast frame byte size
+    u32_t rx11bDataFrame;		// 26 Measured quality 11b data frame count
+    u32_t rxOFDMDataFrame;	    // 27 Measured quality 11g data frame count
+
+
+    u32_t Hw_TotalRxFrm;        // 28
+    u32_t Hw_CRC16Cnt;		    // 29 rxPLCPCRCErrCnt
+    u32_t Hw_CRC32Cnt;		    // 30 rxCRC32ErrCnt
+    u32_t Hw_DecrypErr_UNI;     // 31
+    u32_t Hw_DecrypErr_Mul;     // 32
+
+    u32_t Hw_RxFIFOOverrun;     // 34
+    u32_t Hw_RxTimeOut;         // 35
+    u32_t LossAP;               // 36
+
+    u32_t Tx_MPDU;              // 37
+    u32_t BA_Fail;              // 38
+    u32_t Hw_Tx_AMPDU;          // 39
+    u32_t Hw_Tx_MPDU;           // 40
+
+    u32_t RateCtrlTxMPDU;
+    u32_t RateCtrlBAFail;
+
+    u32_t txQosDropCount[5];    //41 42 43 44 45
+
+	u32_t Hw_RxMPDU;            // 46
+	u32_t Hw_RxDropMPDU;        // 47
+	u32_t Hw_RxDelMPDU;         // 48
+
+	u32_t Hw_RxPhyMiscError;    // 49
+	u32_t Hw_RxPhyXRError;      // 50
+    u32_t Hw_RxPhyOFDMError;    // 51
+    u32_t Hw_RxPhyCCKError;     // 52
+    u32_t Hw_RxPhyHTError;      // 53
+    u32_t Hw_RxPhyTotalCount;   // 54
+
+    u32_t swRxFragmentCount;         // 55
+    u32_t swRxUnicastMicFailCount;   // 56
+    u32_t swRxMulticastMicFailCount; // 57
+    u32_t swRxDropUnencryptedCount;  // 58
+
+    u32_t txBroadcastFrm;
+    u32_t txBroadcastOctets;
+};
+
+/* Traffic Monitor Tally data structure */
+struct zsTrafTally
+{
+    u32_t rxDuplicate;
+    u32_t rxSrcIsOwnMac;
+    //u32_t rxDataFrameCount;
+    //u32_t rxDataByteCount;
+    //u32_t rxDataBytesIn1000ms;
+    //u32_t rxDataTmpFor1000ms;
+    //u32_t rxDataBytesIn2000ms;
+    //u32_t rxDataTmpFor2000ms;
+
+    //u32_t txDataFrameCount;
+    //u32_t txDataByteCount;
+    //u32_t txDataBytesIn1000ms;
+    //u32_t txDataTmpFor1000ms;
+    u32_t txDataBytesIn2000ms;
+    u32_t txDataTmpFor2000ms;
+};
+
+/* Hal rx packet moniter information */
+struct zsMonHalRxInfo
+{
+    u32_t currentRSSI[7];
+    u32_t currentRxEVM[14];
+    u32_t currentRxDataMT;
+    u32_t currentRxDataMCS;
+    u32_t currentRxDataBW;
+    u32_t currentRxDataSG;
+};
+
+struct zsTail
+{
+    u8_t SignalStrength1;
+    u8_t SignalStrength2;
+    u8_t SignalStrength3;
+    u8_t SignalQuality;
+    u8_t SAIndex;
+    u8_t DAIndex;
+    u8_t ErrorIndication;
+    u8_t RxMacStatus;
+};
+
+union zuTail
+{
+    struct zsTail Data;
+    u8_t Byte[8];
+};
+
+struct zsAdditionInfo
+{
+    u8_t PlcpHeader[12];
+    union zuTail   Tail;
+};
+
+
+struct zsPmkidBssidInfo
+{
+    u16_t      bssid[3];
+    u8_t       pmkid[16];
+};
+
+struct zsPmkidInfo
+{
+	   u32_t		bssidCount;
+	   struct zsPmkidBssidInfo	bssidInfo[ZM_PMKID_MAX_BSS_CNT];
+};
+
+
+struct zsCbFuncTbl
+{
+    u16_t (*zfcbAuthNotify)(zdev_t* dev, u16_t* macAddr);
+    u16_t (*zfcbAsocNotify)(zdev_t* dev, u16_t* macAddr, u8_t* body,
+            u16_t bodySize, u16_t port);
+    u16_t (*zfcbDisAsocNotify)(zdev_t* dev, u8_t* macAddr, u16_t port);
+    u16_t (*zfcbApConnectNotify)(zdev_t* dev, u8_t* macAddr, u16_t port);
+    void (*zfcbConnectNotify)(zdev_t* dev, u16_t status, u16_t* bssid);
+    void (*zfcbScanNotify)(zdev_t* dev, struct zsScanResult* result);
+    void (*zfcbMicFailureNotify)(zdev_t* dev, u16_t* addr, u16_t status);
+    void (*zfcbApMicFailureNotify)(zdev_t* dev, u8_t* addr, zbuf_t* buf);
+    void (*zfcbIbssPartnerNotify)(zdev_t* dev, u16_t status,
+            struct zsPartnerNotifyEvent *event);
+    void (*zfcbMacAddressNotify)(zdev_t* dev, u8_t* addr);
+    void (*zfcbSendCompleteIndication)(zdev_t* dev, zbuf_t* buf);
+    void (*zfcbRecvEth)(zdev_t* dev, zbuf_t* buf, u16_t port);
+    void (*zfcbRecv80211)(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+    void (*zfcbRestoreBufData)(zdev_t* dev, zbuf_t* buf);
+#ifdef ZM_ENABLE_CENC
+    u16_t (*zfcbCencAsocNotify)(zdev_t* dev, u16_t* macAddr, u8_t* body,
+            u16_t bodySize, u16_t port);
+#endif //ZM_ENABLE_CENC
+    u8_t (*zfcbClassifyTxPacket)(zdev_t* dev, zbuf_t* buf);
+
+    void (*zfcbHwWatchDogNotify)(zdev_t* dev);
+};
+
+extern void zfZeroMemory(u8_t* va, u16_t length);
+#define ZM_INIT_CB_FUNC_TABLE(p)        zfZeroMemory((u8_t *)p, sizeof(struct zsCbFuncTbl));
+
+//extern struct zsWlanDev zgWlanDev;
+
+/* Initialize WLAN hardware and software, resource will be allocated */
+/* for WLAN operation, must be called first before other function.   */
+extern u16_t zfiWlanOpen(zdev_t* dev, struct zsCbFuncTbl* cbFuncTbl);
+
+/* WLAN hardware will be shutdown and all resource will be release */
+extern u16_t zfiWlanClose(zdev_t* dev);
+
+/* Enable/disable Wlan operation */
+extern u16_t zfiWlanEnable(zdev_t* dev);
+extern u16_t zfiWlanDisable(zdev_t* dev, u8_t ResetKeyCache);
+extern u16_t zfiWlanResume(zdev_t* dev, u8_t doReconn);
+extern u16_t zfiWlanSuspend(zdev_t* dev);
+
+/* Enable/disable ISR interrupt */
+extern u16_t zfiWlanInterruptEnable(zdev_t* dev);
+extern u16_t zfiWlanInterruptDisable(zdev_t* dev);
+
+/* Do WLAN site survey */
+extern u16_t zfiWlanScan(zdev_t* dev);
+
+/* Get WLAN stastics */
+extern u16_t zfiWlanGetStatistics(zdev_t* dev);
+
+/* Reset WLAN */
+extern u16_t zfiWlanReset(zdev_t* dev);
+
+/* Deauthenticate a STA */
+extern u16_t zfiWlanDeauth(zdev_t* dev, u16_t* macAddr, u16_t reason);
+
+extern u16_t zfiTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port);
+extern u8_t zfiIsTxQueueFull(zdev_t* dev);
+extern u16_t zfiTxSend80211Mgmt(zdev_t* dev, zbuf_t* buf, u16_t port);
+
+extern void zfiIsrPci(zdev_t* dev);
+
+extern u8_t zfiWlanIBSSGetPeerStationsCount(zdev_t* dev);
+extern u8_t zfiWlanIBSSIteratePeerStations(zdev_t* dev, u8_t numToIterate, zfpIBSSIteratePeerStationCb callback, void *ctx);
+extern void zfiWlanFlushAllQueuedBuffers(zdev_t* dev);
+
+/* coid.c */
+extern void zfiWlanQueryMacAddress(zdev_t* dev, u8_t* addr);
+
+extern u16_t zfiGlobalDataSize(zdev_t* dev);
+
+extern void zfiHeartBeat(zdev_t* dev);
+
+extern void zfiWlanSetWlanMode(zdev_t* dev, u8_t wlanMode);
+extern void zfiWlanSetAuthenticationMode(zdev_t* dev, u8_t authMode);
+extern void zfiWlanSetWepStatus(zdev_t* dev, u8_t wepStatus);
+extern void zfiWlanSetSSID(zdev_t* dev, u8_t* ssid, u8_t ssidLength);
+extern void zfiWlanSetFragThreshold(zdev_t* dev, u16_t fragThreshold);
+extern void zfiWlanSetRtsThreshold(zdev_t* dev, u16_t rtsThreshold);
+extern void zfiWlanSetFrequency(zdev_t* dev, u32_t frequency, u8_t bImmediate);
+extern void zfiWlanSetBssid(zdev_t* dev, u8_t* bssid);
+extern void zfiWlanSetBeaconInterval(zdev_t* dev, u16_t beaconInterval,
+                              u8_t bImmediate);
+extern void zfiWlanSetDtimCount(zdev_t* dev, u8_t  dtim);
+extern void zfiWlanSetAtimWindow(zdev_t* dev, u16_t atimWindow, u8_t bImmediate);
+extern void zfiWlanSetEncryMode(zdev_t* dev, u8_t encryMode);
+extern u8_t zfiWlanSetKey(zdev_t* dev, struct zsKeyInfo keyInfo);
+extern u8_t zfiWlanPSEUDOSetKey(zdev_t* dev, struct zsKeyInfo keyInfo);
+extern void zfiWlanSetPowerSaveMode(zdev_t* dev, u8_t mode);
+extern void zfiWlanQueryBssListV1(zdev_t* dev, struct zsBssListV1* bssListV1);
+extern void zfiWlanQueryBssList(zdev_t* dev, struct zsBssList* pBssList);
+extern void zfiWlanSetProtectionMode(zdev_t* dev, u8_t mode);
+extern void zfiWlanFlushBssList(zdev_t* dev);
+
+void zfiWlanDisableDfsChannel(zdev_t* dev, u8_t disableFlag);
+
+extern u8_t zfiWlanQueryWlanMode(zdev_t* dev);
+extern u16_t zfiWlanChannelToFrequency(zdev_t* dev, u8_t channel);
+extern u8_t zfiWlanFrequencyToChannel(zdev_t* dev, u16_t freq);
+
+#define ZM_WLAN_STATE_OPENED        0
+#define ZM_WLAN_STATE_ENABLED       1
+#define ZM_WLAN_STATE_DISABLED      2
+#define ZM_WLAN_STATE_CLOSEDED      3
+extern u8_t zfiWlanQueryAdapterState(zdev_t* dev);
+extern u8_t zfiWlanQueryAuthenticationMode(zdev_t* dev, u8_t bWrapper);
+extern u8_t zfiWlanQueryWepStatus(zdev_t* dev, u8_t bWrapper);
+extern void zfiWlanQuerySSID(zdev_t* dev, u8_t* ssid, u8_t* pSsidLength);
+extern u16_t zfiWlanQueryFragThreshold(zdev_t* dev);
+extern u16_t zfiWlanQueryRtsThreshold(zdev_t* dev);
+extern u32_t zfiWlanQueryFrequency(zdev_t* dev);
+extern u32_t zfiWlanQueryCurrentFrequency(zdev_t* dev, u8_t qmode);
+extern u32_t zfiWlanQueryFrequencyAttribute(zdev_t* dev, u32_t frequency);
+extern void zfiWlanQueryFrequencyHT(zdev_t* dev, u32_t *bandWidth, u32_t *extOffset);
+extern u8_t zfiWlanQueryCWMode(zdev_t* dev);
+extern u32_t zfiWlanQueryCWEnable(zdev_t* dev);
+extern void zfiWlanQueryBssid(zdev_t* dev, u8_t* bssid);
+extern u16_t zfiWlanQueryBeaconInterval(zdev_t* dev);
+extern u32_t zfiWlanQueryRxBeaconTotal(zdev_t* dev);
+extern u16_t zfiWlanQueryAtimWindow(zdev_t* dev);
+extern u8_t zfiWlanQueryEncryMode(zdev_t* dev);
+extern u16_t zfiWlanQueryCapability(zdev_t* dev);
+extern u16_t zfiWlanQueryAid(zdev_t* dev);
+extern void zfiWlanQuerySupportRate(zdev_t* dev, u8_t* rateArray, u8_t* pLength);
+extern void zfiWlanQueryExtSupportRate(zdev_t* dev, u8_t* rateArray, u8_t* pLength);
+extern void zfiWlanQueryRsnIe(zdev_t* dev, u8_t* ie, u8_t* pLength);
+extern void zfiWlanQueryWpaIe(zdev_t* dev, u8_t* ie, u8_t* pLength);
+extern u8_t zfiWlanQueryHTMode(zdev_t* dev);
+extern u8_t zfiWlanQueryBandWidth40(zdev_t* dev);
+extern u8_t zfiWlanQueryMulticastCipherAlgo(zdev_t *dev);
+extern u16_t zfiWlanQueryRegionCode(zdev_t* dev);
+extern void zfiWlanSetWpaIe(zdev_t* dev, u8_t* ie, u8_t Length);
+extern void zfiWlanSetWpaSupport(zdev_t* dev, u8_t WpaSupport);
+extern void zfiWlanCheckStaWpaIe(zdev_t* dev);
+extern void zfiWlanSetBasicRate(zdev_t* dev, u8_t bRateSet, u8_t gRateSet,
+                         u32_t nRateSet);
+extern void zfiWlanSetBGMode(zdev_t* dev, u8_t mode);
+extern void zfiWlanSetpreambleType(zdev_t* dev, u8_t type);
+extern u8_t zfiWlanQuerypreambleType(zdev_t* dev);
+extern u8_t zfiWlanQueryPowerSaveMode(zdev_t* dev);
+extern void zfiWlanSetMacAddress(zdev_t* dev, u16_t* mac);
+extern u16_t zfiWlanSetTxRate(zdev_t* dev, u16_t rate);
+extern u32_t zfiWlanQueryTxRate(zdev_t* dev);
+extern void zfWlanUpdateRxRate(zdev_t* dev, struct zsAdditionInfo* addInfo);
+extern u32_t zfiWlanQueryRxRate(zdev_t* dev);
+extern u8_t zfiWlanSetPmkidInfo(zdev_t* dev, u16_t* bssid, u8_t* pmkid);
+extern u32_t zfiWlanQueryPmkidInfo(zdev_t* dev, u8_t* buf, u32_t len);
+extern void zfiWlanSetAllMulticast(zdev_t* dev, u32_t setting);
+extern void zfiWlanSetHTCtrl(zdev_t* dev, u32_t *setting, u32_t forceTxTPC);
+extern void zfiWlanQueryHTCtrl(zdev_t* dev, u32_t *setting, u32_t *forceTxTPC);
+extern void zfiWlanDbg(zdev_t* dev, u8_t setting);
+
+extern void zfiWlanResetTally(zdev_t* dev);
+extern void zfiWlanQueryTally(zdev_t* dev, struct zsCommTally *tally);
+extern void zfiWlanQueryTrafTally(zdev_t* dev, struct zsTrafTally *tally);
+extern void zfiWlanQueryMonHalRxInfo(zdev_t* dev, struct zsMonHalRxInfo *halRxInfo);
+
+extern u32_t zfiFWConfig(zdev_t* dev, u32_t size);
+
+extern void zfiDKEnable(zdev_t* dev, u32_t enable);
+
+extern void zfiWlanSetMulticastList(zdev_t* dev, u8_t size, u8_t* pList);
+extern void zfiWlanRemoveKey(zdev_t* dev, u8_t keyType, u8_t keyId);
+extern u8_t zfiWlanQueryIsPKInstalled(zdev_t *dev, u8_t *staMacAddr);
+extern u32_t zfiWlanQueryPacketTypePromiscuous(zdev_t* dev);
+extern void zfiWlanSetPacketTypePromiscuous(zdev_t* dev, u32_t setValue);
+extern void zfiSetChannelManagement(zdev_t* dev, u32_t setting);
+extern void zfiSetRifs(zdev_t* dev, u16_t setting);
+extern void zfiCheckRifs(zdev_t* dev);
+extern void zfiSetReorder(zdev_t* dev, u16_t value);
+extern void zfiSetSeqDebug(zdev_t* dev, u16_t value);
+
+extern u16_t zfiConfigWdsPort(zdev_t* dev, u8_t wdsPortId, u16_t flag, u16_t* wdsAddr,
+        u16_t encType, u32_t* wdsKey);
+extern void zfiWlanQueryRegulationTable(zdev_t* dev, struct zsRegulationTable* pEntry);
+extern void zfiWlanSetScanTimerPerChannel(zdev_t* dev, u16_t time);
+extern void zfiWlanSetAutoReconnect(zdev_t* dev, u8_t enable);
+extern u32_t zfiDebugCmd(zdev_t* dev, u32_t cmd, u32_t value);
+extern void zfiWlanSetProbingHiddenSsid(zdev_t* dev, u8_t* ssid, u8_t ssidLen,
+    u16_t entry);
+extern void zfiWlanSetDropUnencryptedPackets(zdev_t* dev, u8_t enable);
+extern void zfiWlanSetIBSSJoinOnly(zdev_t* dev, u8_t joinOnly);
+extern void zfiWlanSetDefaultKeyId(zdev_t* dev, u8_t keyId);
+extern void zfiWlanSetDisableProbingWithSsid(zdev_t* dev, u8_t mode);
+extern void zfiWlanQueryGSN(zdev_t* dev, u8_t *gsn, u16_t vapId);
+extern u16_t zfiStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType);
+extern u8_t zfiWlanSetDot11DMode(zdev_t* dev, u8_t mode);
+extern u8_t zfiWlanSetDot11HDFSMode(zdev_t* dev, u8_t mode);
+extern u8_t zfiWlanSetDot11HTPCMode(zdev_t* dev, u8_t mode);
+extern u8_t zfiWlanSetAniMode(zdev_t* dev, u8_t mode);
+extern void zfiWlanSetStaWme(zdev_t* dev, u8_t enable, u8_t uapsdInfo);
+extern void zfiWlanSetApWme(zdev_t* dev, u8_t enable);
+extern u8_t zfiWlanQuerywmeEnable(zdev_t* dev);
+#ifdef ZM_OS_LINUX_FUNC
+extern void zfiWlanShowTally(zdev_t* dev);
+#endif
+#ifdef ZM_ENABLE_CENC
+/* CENC */
+extern u8_t zfiWlanSetCencPairwiseKey(zdev_t* dev, u8_t keyid, u32_t *txiv, u32_t *rxiv,
+        u8_t *key, u8_t *mic);
+extern u8_t zfiWlanSetCencGroupKey(zdev_t* dev, u8_t keyid, u32_t *rxiv,
+        u8_t *key, u8_t *mic);
+#endif //ZM_ENABLE_CENC
+extern void zfiWlanQuerySignalInfo(zdev_t* dev, u8_t *buffer);
+extern void zfiWlanQueryAdHocCreatedBssDesc(zdev_t* dev, struct zsBssInfo *pBssInfo);
+extern u8_t zfiWlanQueryAdHocIsCreator(zdev_t* dev);
+extern u32_t zfiWlanQuerySupportMode(zdev_t* dev);
+extern u32_t zfiWlanQueryTransmitPower(zdev_t* dev);
+extern void zfiWlanEnableLeapConfig(zdev_t* dev, u8_t leapEnabled);
+
+/* returned buffer allocated by driver core */
+extern void zfiRecvEthComplete(zdev_t* dev, zbuf_t* buf);
+
+extern void zfiRecv80211(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+
+extern void zfiWlanSetMaxTxPower(zdev_t* dev, u8_t power2, u8_t power5);
+extern void zfiWlanQueryMaxTxPower(zdev_t* dev, u8_t *power2, u8_t *power5);
+extern void zfiWlanSetConnectMode(zdev_t* dev, u8_t mode);
+extern void zfiWlanSetSupportMode(zdev_t* dev, u32_t mode);
+extern void zfiWlanSetAdhocMode(zdev_t* dev, u32_t mode);
+extern u32_t zfiWlanQueryAdhocMode(zdev_t* dev, u8_t bWrapper);
+extern u8_t zfiWlanSetCountryIsoName(zdev_t* dev, u8_t *countryIsoName, u8_t length);
+extern const char* zfiWlanQueryCountryIsoName(zdev_t* dev);
+extern u8_t zfiWlanQueryregulatoryDomain(zdev_t* dev);
+extern u8_t zfiWlanQueryCCS(zdev_t* dev);
+extern void zfiWlanSetCCS(zdev_t* dev, u8_t mode);
+extern void zfiWlanSetRegulatory(zdev_t* dev, u8_t CCS, u16_t Code, u8_t bfirstChannel);
+extern const char* zfiHpGetisoNamefromregionCode(zdev_t* dev, u16_t regionCode);
+extern void  zfiWlanSetLEDCtrlParam(zdev_t* dev, u8_t type, u8_t flag);
+extern u32_t zfiWlanQueryReceivedPacket(zdev_t* dev);
+extern void zfiWlanCheckSWEncryption(zdev_t* dev);
+extern u16_t zfiWlanQueryAllowChannels(zdev_t *dev, u16_t *channels);
+extern u16_t zfiWlanGetMulticastAddressCount(zdev_t* dev);
+extern void zfiWlanGetMulticastList(zdev_t* dev, u8_t* pMCList);
+extern void zfiWlanSetPacketFilter(zdev_t* dev, u32_t PacketFilter);
+extern u8_t zfiCompareWithMulticastListAddress(zdev_t* dev, u16_t* dstMacAddr);
+extern void zfiWlanSetSafeModeEnabled(zdev_t* dev, u8_t safeMode);
+extern void zfiWlanSetIBSSAdditionalIELength(zdev_t* dev, u32_t ibssAdditionalIESize,  u8_t* ibssAdditionalIE);
+extern void zfiWlanSetXLinkMode(zdev_t* dev, u32_t setValue);
+
+/* hprw.c */
+extern u32_t zfiDbgWriteFlash(zdev_t* dev, u32_t addr, u32_t val);
+extern u32_t zfiDbgWriteReg(zdev_t* dev, u32_t addr, u32_t val);
+extern u32_t zfiDbgReadReg(zdev_t* dev, u32_t addr);
+
+extern u32_t zfiDbgWriteEeprom(zdev_t* dev, u32_t addr, u32_t val);
+extern u32_t zfiDbgBlockWriteEeprom(zdev_t* dev, u32_t addr, u32_t* buf);
+extern u32_t zfiDbgBlockWriteEeprom_v2(zdev_t* dev, u32_t addr, u32_t* buf, u32_t wrlen);
+
+extern u16_t zfiDbgChipEraseFlash(zdev_t *dev);
+extern u16_t zfiDbgProgramFlash(zdev_t *dev, u32_t offset, u32_t len, u32_t *data);
+extern u32_t zfiDbgGetFlashCheckSum(zdev_t *dev, u32_t addr, u32_t len);
+extern u32_t zfiDbgReadFlash(zdev_t *dev, u32_t addr, u32_t len);
+extern u32_t zfiDownloadFwSet(zdev_t *dev);
+
+extern u32_t zfiDbgDelayWriteReg(zdev_t* dev, u32_t addr, u32_t val);
+extern u32_t zfiDbgFlushDelayWrite(zdev_t* dev);
+
+extern u32_t zfiDbgSetIFSynthesizer(zdev_t* dev, u32_t value);
+extern u32_t zfiDbgReadTally(zdev_t* dev);
+
+extern u32_t zfiDbgQueryHwTxBusy(zdev_t* dev);
+
+extern u8_t zfiWlanGetDestAddrFromBuf(zdev_t *dev, zbuf_t *buf, u16_t *macAddr);
+
+extern u32_t zfiWlanQueryHwCapability(zdev_t* dev);
+
+extern void zfiWlanSetDynamicSIFSParam(zdev_t* dev, u8_t val);
+
+/***** End of section 2 *****/
+
+/***** section 3 performace evaluation *****/
+#ifdef ZM_ENABLE_PERFORMANCE_EVALUATION
+extern void zfiTxPerformanceMSDU(zdev_t* dev, u32_t tick);
+extern void zfiRxPerformanceMPDU(zdev_t* dev, zbuf_t* buf);
+extern void zfiRxPerformanceReg(zdev_t* dev, u32_t reg, u32_t rsp);
+#define ZM_PERFORMANCE_INIT(dev)                zfiPerformanceInit(dev);
+#define ZM_PERFORMANCE_TX_MSDU(dev, tick)       zfiTxPerformanceMSDU(dev, tick);
+#define ZM_PERFORMANCE_RX_MSDU(dev, tick)       zfiRxPerformanceMSDU(dev, tick);
+#define ZM_PERFORMANCE_TX_MPDU(dev, tick)       zfiTxPerformanceMPDU(dev, tick);
+#define ZM_PERFORMANCE_RX_MPDU(dev, buf)        zfiRxPerformanceMPDU(dev, buf);
+#define ZM_PERFORMANCE_RX_SEQ(dev, buf)         zfiRxPerformanceSeq(dev, buf);
+#define ZM_PERFORMANCE_REG(dev, reg, rsp)    {if(cmd[1] == reg) zfiRxPerformanceReg(dev, reg, rsp);}
+#define ZM_PERFORMANCE_DUP(dev, buf1, buf2)     zfiRxPerformanceDup(dev, buf1, buf2);
+#define ZM_PERFORMANCE_FREE(dev, buf)           zfiRxPerformanceFree(dev, buf);
+#define ZM_PERFORMANCE_RX_AMSDU(dev, buf, len)  zfiRxPerformanceAMSDU(dev, buf, len);
+#define ZM_PERFORMANCE_RX_FLUSH(dev)            zfiRxPerformanceFlush(dev);
+#define ZM_PERFORMANCE_RX_CLEAR(dev)            zfiRxPerformanceClear(dev);
+#define ZM_SEQ_DEBUG                            if (wd->seq_debug) DbgPrint
+#define ZM_PERFORMANCE_RX_REORDER(dev)          zfiRxPerformanceReorder(dev);
+#else
+#define ZM_PERFORMANCE_INIT(dev)
+#define ZM_PERFORMANCE_TX_MSDU(dev, tick)
+#define ZM_PERFORMANCE_RX_MSDU(dev, tick)
+#define ZM_PERFORMANCE_TX_MPDU(dev, tick)
+#define ZM_PERFORMANCE_RX_MPDU(dev, buf)
+#define ZM_PERFORMANCE_RX_SEQ(dev, buf)
+#define ZM_PERFORMANCE_REG(dev, reg, rsp)
+#define ZM_PERFORMANCE_DUP(dev, buf1, buf2)
+#define ZM_PERFORMANCE_FREE(dev, buf)
+#define ZM_PERFORMANCE_RX_AMSDU(dev, buf, len)
+#define ZM_PERFORMANCE_RX_FLUSH(dev)
+#define ZM_PERFORMANCE_RX_CLEAR(dev)
+#define ZM_SEQ_DEBUG
+#define ZM_PERFORMANCE_RX_REORDER(dev)
+#endif
+/***** End of section 3 *****/
+#endif
diff --git a/drivers/staging/otus/80211core/pub_zfw.h b/drivers/staging/otus/80211core/pub_zfw.h
new file mode 100644
index 000000000000..2474bb7536e8
--- /dev/null
+++ b/drivers/staging/otus/80211core/pub_zfw.h
@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _PUB_ZFW_H
+#define _PUB_ZFW_H
+
+#include "../oal_dt.h"
+
+
+/* Buffer management */
+#ifdef ZM_ENABLE_BUFFER_DEBUG
+extern zbuf_t* zfwBufAllocateWithContext(zdev_t* dev, u16_t len, u8_t *functionName, ULONG line);
+#define zfwBufAllocate(dev, len)  zfwBufAllocateWithContext(dev, len, (u8_t *)__func__, __LINE__)
+#else
+extern zbuf_t* zfwBufAllocate(zdev_t* dev, u16_t len);
+#endif
+extern void zfwBufFree(zdev_t* dev, zbuf_t* buf, u16_t errCode);
+extern u16_t zfwBufChain(zdev_t* dev, zbuf_t** head, zbuf_t* tail);
+extern u16_t zfwBufCopy(zdev_t* dev, zbuf_t* dst, zbuf_t* src);
+extern u16_t zfwBufSetSize(zdev_t* dev, zbuf_t* buf, u16_t size);
+extern u16_t zfwBufRemoveHead(zdev_t* dev, zbuf_t* buf, u16_t size);
+extern u16_t zfwBufGetSize(zdev_t* dev, zbuf_t* buf);
+extern void  zfwCopyBufContext(zdev_t* dev, zbuf_t* source, zbuf_t* dest);
+
+/* Memory management */
+extern void* zfwMemAllocate(zdev_t* dev, u32_t size);
+extern void zfwMemFree(zdev_t* dev, void* mem, u32_t size);
+extern void zfwMemoryCopy(u8_t* dst, u8_t* src, u16_t length);
+extern void zfwMemoryMove(u8_t* dst, u8_t* src, u16_t length);
+extern void zfwZeroMemory(u8_t* va, u16_t length);
+extern u8_t zfwMemoryIsEqual(u8_t* m1, u8_t* m2, u16_t length);
+
+/* Others */
+extern void zfwSleep(zdev_t* dev, u32_t ms);
+extern u16_t zfwGetVapId(zdev_t* dev);
+extern u16_t zfwStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType);
+extern u32_t zfwWaitForEvent(zdev_t *dev, u32_t event, u32_t timeout);
+extern void zfwSendEvent(zdev_t* dev);
+extern void zfwGetActiveScanDur(zdev_t* dev, u8_t* Dur );
+extern void zfwGetShowZeroLengthSSID(zdev_t* dev, u8_t* Dur );
+/* For debugging */
+extern void zfwDumpBuf(zdev_t* dev, zbuf_t* buf);
+extern void zfwDbgReadRegDone(zdev_t* dev, u32_t addr, u32_t val);
+/* For Evl */
+extern void zfwDbgDownloadFwInitDone(zdev_t* dev);
+extern void zfwDbgReadFlashDone(zdev_t* dev, u32_t addr, u32_t* rspdata, u32_t datalen);
+extern void zfwDbgGetFlashChkSumDone(zdev_t* dev, u32_t* rspdata);
+extern void zfwDbgProgrameFlashDone(zdev_t* dev);
+extern void zfwDbgProgrameFlashChkDone(zdev_t* dev);
+extern void zfwDbgWriteRegDone(zdev_t* dev, u32_t addr, u32_t val);
+extern void zfwDbgWriteEepromDone(zdev_t* dev, u32_t addr, u32_t val);
+extern void zfwDbgReadTallyDone(zdev_t* dev);
+extern void zfwWlanReadRegDone(zdev_t* dev, u32_t addr, u32_t val);
+extern void zfwWlanWriteRegDone(zdev_t* dev, u32_t addr, u32_t val);
+extern void zfwWlanReadTallyDone(zdev_t* dev);
+extern void zfwDbgQueryHwTxBusyDone(zdev_t* dev, u32_t val);
+extern u32_t zfwReadReg(zdev_t* dev, u32_t offset);
+extern u32_t zfwReadEeprom(zdev_t* dev, u32_t addr);
+
+/* Reserved for Vista, please return 0 */
+extern u8_t zfwGetPktEncExemptionActionType(zdev_t* dev, zbuf_t* buf);
+
+#ifdef ZM_ENABLE_CENC
+/* Reserved for CENC, please return 0 */
+extern u8_t zfwCencHandleBeaconProbrespon(zdev_t* dev, u8_t *pWIEc,
+        u8_t *pPeerSSIDc, u8_t *pPeerAddrc);
+#endif //ZM_ENABLE_CENC
+
+#ifdef ZM_HALPLUS_LOCK
+extern asmlinkage struct zsWlanDev *zfwGetWlanDev(zdev_t* dev);
+extern asmlinkage void zfwEnterCriticalSection(zdev_t* dev);
+extern asmlinkage void zfwLeaveCriticalSection(zdev_t* dev);
+extern asmlinkage u8_t zfwBufReadByte(zdev_t* dev, zbuf_t* buf, u16_t offset);
+extern asmlinkage u16_t zfwBufReadHalfWord(zdev_t* dev, zbuf_t* buf, u16_t offset);
+extern asmlinkage void zfwBufWriteByte(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t value);
+extern asmlinkage void zfwBufWriteHalfWord(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t value);
+extern asmlinkage u8_t *zfwGetBuffer(zdev_t* dev, zbuf_t* buf);
+#endif
+
+#endif //_PUB_ZFW_H
diff --git a/drivers/staging/otus/80211core/queue.c b/drivers/staging/otus/80211core/queue.c
new file mode 100644
index 000000000000..d294831b5c36
--- /dev/null
+++ b/drivers/staging/otus/80211core/queue.c
@@ -0,0 +1,303 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : queue.c                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains queue management functions.                */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "queue.h"
+
+
+struct zsQueue* zfQueueCreate(zdev_t* dev, u16_t size)
+{
+    struct zsQueue* q;
+
+    if ((q = (struct zsQueue*)zfwMemAllocate(dev, sizeof(struct zsQueue)
+            + (sizeof(struct zsQueueCell)*(size-1)))) != NULL)
+    {
+        q->size = size;
+        q->sizeMask = size-1;
+        q->head = 0;
+        q->tail = 0;
+    }
+    return q;
+}
+
+void zfQueueDestroy(zdev_t* dev, struct zsQueue* q)
+{
+    u16_t size = sizeof(struct zsQueue) + (sizeof(struct zsQueueCell)*(q->size-1));
+
+    zfQueueFlush(dev, q);
+    zfwMemFree(dev, q, size);
+
+    return;
+}
+
+u16_t zfQueuePutNcs(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick)
+{
+    u16_t ret = ZM_ERR_QUEUE_FULL;
+
+    zm_msg0_mm(ZM_LV_1, "zfQueuePutNcs()");
+
+    if (((q->tail+1)&q->sizeMask) != q->head)
+    {
+        q->cell[q->tail].buf = buf;
+        q->cell[q->tail].tick = tick;
+        q->tail = (q->tail+1) & q->sizeMask;
+        ret = ZM_SUCCESS;
+    }
+
+    return ret;
+}
+
+u16_t zfQueuePut(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick)
+{
+    u16_t ret;
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    ret = zfQueuePutNcs(dev, q, buf, tick);
+
+    zmw_leave_critical_section(dev);
+
+    return ret;
+}
+
+zbuf_t* zfQueueGet(zdev_t* dev, struct zsQueue* q)
+{
+    zbuf_t* buf = NULL;
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (q->head != q->tail)
+    {
+        buf = q->cell[q->head].buf;
+        q->head = (q->head+1) & q->sizeMask;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return buf;
+}
+
+u16_t zfCompareDstwithBuf(zdev_t* dev, zbuf_t* buf, u8_t* addr)
+{
+    u16_t i;
+    u8_t dst[6];
+
+    for (i=0; i<6; i++)
+    {
+        dst[i] = zmw_buf_readb(dev, buf, i);
+        if (dst[i] != addr[i])
+        {
+            return 1+i;
+        }
+    }
+
+    return 0;
+}
+
+
+zbuf_t* zfQueueGetWithMac(zdev_t* dev, struct zsQueue* q, u8_t* addr, u8_t* mb)
+{
+    zbuf_t* buf;
+    zbuf_t* retBuf = NULL;
+    u16_t index, next;
+    zmw_declare_for_critical_section();
+
+    *mb = 0;
+
+    zmw_enter_critical_section(dev);
+
+    index = q->head;
+
+    while (1)
+    {
+        if (index != q->tail)
+        {
+            buf = q->cell[index].buf;
+
+            //if buf's detination address == input addr
+            if (zfCompareDstwithBuf(dev, buf, addr) == 0)
+            {
+                retBuf = buf;
+                //Get it, and trace the whole queue to calculate more bit
+                while ((next =((index+1)&q->sizeMask)) != q->tail)
+                {
+                    q->cell[index].buf = q->cell[next].buf;
+                    q->cell[index].tick = q->cell[next].tick;
+
+                    if ((*mb == 0) && (zfCompareDstwithBuf(dev,
+                            q->cell[next].buf, addr) == 0))
+                    {
+                        *mb = 1;
+                    }
+
+                    index = next;
+                }
+                q->tail = (q->tail-1) & q->sizeMask;
+
+                zmw_leave_critical_section(dev);
+                return retBuf;
+            }
+            index = (index + 1) & q->sizeMask;
+        } //if (index != q->tail)
+        else
+        {
+            break;
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return retBuf;
+
+}
+
+void zfQueueFlush(zdev_t* dev, struct zsQueue* q)
+{
+    zbuf_t* buf;
+
+    while ((buf = zfQueueGet(dev, q)) != NULL)
+    {
+        zfwBufFree(dev, buf, 0);
+    }
+
+    return;
+}
+
+void zfQueueAge(zdev_t* dev, struct zsQueue* q, u32_t tick, u32_t msAge)
+{
+    zbuf_t* buf;
+    u32_t   buftick;
+    zmw_declare_for_critical_section();
+
+    while (1)
+    {
+        buf = NULL;
+        zmw_enter_critical_section(dev);
+
+        if (q->head != q->tail)
+        {
+            buftick = q->cell[q->head].tick;
+            if (((tick - buftick)*ZM_MS_PER_TICK) > msAge)
+            {
+                buf = q->cell[q->head].buf;
+                q->head = (q->head+1) & q->sizeMask;
+            }
+        }
+
+        zmw_leave_critical_section(dev);
+
+        if (buf != NULL)
+        {
+            zm_msg0_mm(ZM_LV_0, "Age frame in queue!");
+            zfwBufFree(dev, buf, 0);
+        }
+        else
+        {
+            break;
+        }
+    }
+    return;
+}
+
+
+u8_t zfQueueRemovewithIndex(zdev_t* dev, struct zsQueue* q, u16_t index, u8_t* addr)
+{
+    u16_t next;
+    u8_t mb = 0;
+
+    //trace the whole queue to calculate more bit
+    while ((next =((index+1)&q->sizeMask)) != q->tail)
+    {
+        q->cell[index].buf = q->cell[next].buf;
+        q->cell[index].tick = q->cell[next].tick;
+
+        if ((mb == 0) && (zfCompareDstwithBuf(dev,
+                q->cell[next].buf, addr) == 0))
+        {
+            mb = 1;
+        }
+
+        index = next;
+    }
+    q->tail = (q->tail-1) & q->sizeMask;
+
+    return mb;
+
+}
+
+void zfQueueGenerateUapsdTim(zdev_t* dev, struct zsQueue* q,
+        u8_t* uniBitMap, u16_t* highestByte)
+{
+    zbuf_t* psBuf;
+    u8_t dst[6];
+    u16_t id, aid, index, i;
+    u16_t bitPosition;
+    u16_t bytePosition;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    index = q->head;
+
+    while (index != q->tail)
+    {
+        psBuf = q->cell[index].buf;
+        for (i=0; i<6; i++)
+        {
+            dst[i] = zmw_buf_readb(dev, psBuf, i);
+        }
+        /* TODO : use u8_t* fot MAC address */
+        if (((id = zfApFindSta(dev, (u16_t*)dst)) != 0xffff)
+                && (wd->ap.staTable[id].psMode != 0))
+        {
+            /* Calculate PVB only when all AC are delivery-enabled */
+            if ((wd->ap.staTable[id].qosInfo & 0xf) == 0xf)
+            {
+                aid = id + 1;
+                bitPosition = (1 << (aid & 0x7));
+                bytePosition = (aid >> 3);
+                uniBitMap[bytePosition] |= bitPosition;
+
+                if (bytePosition>*highestByte)
+                {
+                    *highestByte = bytePosition;
+                }
+            }
+            index = (index+1) & q->sizeMask;
+        }
+        else
+        {
+            /* Free garbage UAPSD frame */
+            zfQueueRemovewithIndex(dev, q, index, dst);
+            zfwBufFree(dev, psBuf, 0);
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+    return;
+}
diff --git a/drivers/staging/otus/80211core/queue.h b/drivers/staging/otus/80211core/queue.h
new file mode 100644
index 000000000000..4526b882bd03
--- /dev/null
+++ b/drivers/staging/otus/80211core/queue.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _QUEUE_H
+#define _QUEUE_H
+
+#include "../oal_dt.h"
+
+struct zsQueueCell
+{
+    u32_t   tick;
+    zbuf_t* buf;
+};
+
+struct zsQueue
+{
+    u16_t   size;
+    u16_t   sizeMask;
+    u16_t   head;
+    u16_t   tail;
+    struct zsQueueCell cell[1];
+};
+
+#endif //#ifndef _QUEUE_H
diff --git a/drivers/staging/otus/80211core/ratectrl.c b/drivers/staging/otus/80211core/ratectrl.c
new file mode 100644
index 000000000000..a43104cd7f51
--- /dev/null
+++ b/drivers/staging/otus/80211core/ratectrl.c
@@ -0,0 +1,874 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+#include "ratectrl.h"
+
+const u32_t zcRateToPhyCtrl[] =
+    {
+        /* 1M,        2M,         5M,        11M ,  0  1  2  3*/
+        0x00000,    0x10000,    0x20000,    0x30000,
+        /* 6M         9M          12M        18M ,  4  5  6  7*/
+        0xb0001,    0xf0001,    0xa0001,    0xe0001,
+        /* 24M         36M        48M        54M ,  8  9  10 11*/
+        0x90001,    0xd0001,    0x80001,    0xc0001,
+        /* MCS0      MCS1        MCS2        MCS3,  12 13 14 15*/
+        0x00002,    0x10002,    0x20002,    0x30002,
+        /* MCS4      MCS5        MCS6        MCS7,  16 17 18 19*/
+        0x40002,    0x50002,    0x60002,    0x70002,
+        /* MCS8      MCS9        MCS10       MCS11, 20 21 22 23*/
+        0x80002,    0x90002,    0xa0002,    0xb0002,
+        /* MCS12     MCS13       MCS14       MCS15, 24 25 26 27*/
+        0xc0002,    0xd0002,    0xe0002,    0xf0002,
+        /* MCS14SG,  MCS15SG     MCS7SG           , 28 29, 30*/
+        0x800e0002, 0x800f0002, 0x80070002
+    };
+
+
+const u8_t zcHtRateTable[15][4] =
+    { /*[5G 20MHz]  [5G 40MHz] [2.4G 20MHz]  [2.4G 40MHz]*/
+        {  4,          4,          0,          0},   /*OFDM6M OFDM6M  CCK1M  CCK1M  */
+        {  5,          5,          1,          1},   /*OFDM9M OFDM9M  CCK2M  CCK2M  */
+        {  13,         12,         2,          2},   /*MCS1   MCS0    CCK5M  CCK5M  */
+        {  14,         13,         3,          3},   /*MCS2   MCS1    CCK11M CCK11M */
+        {  15,         14,         13,         12},  /*MCS3   MCS2    MCS1   MCS0   */
+        {  16,         15,         14,         13},  /*MCS4   MCS3    MCS2   MCS1   */
+        {  23,         16,         15,         14},  /*MCS11  MCS4    MCS3   MCS2   */
+        {  24,         23,         16,         15},  /*MCS12  MCS11   MCS4   MCS3   */
+        {  25,         24,         23,         16},  /*MCS13  MCS12   MCS11  MCS4   */
+        {  26,         25,         24,         23},  /*MCS14  MCS13   MCS12  MCS11  */
+        {  27,         26,         25,         24},  /*MCS15  MCS14   MCS13  MCS12  */
+        {  0,          27,         26,         25},  /*0      MCS15   MCS14  MCS13  */
+        {  0,          29,         27,         26},  /*0      MCS15SG MCS15  MCS14  */
+        {  0,          0,          0,          28},  /*0      0       0      MCS14SG*/
+        {  0,          0,          0,          29}   /*0      0       0      MCS15SG*/
+    };
+
+const u8_t zcHtOneTxStreamRateTable[15][4] =
+    { /*[5G 20MHz]  [5G 40MHz] [2.4G 20MHz]  [2.4G 40MHz]*/
+        {  4,          4,          0,          0},   /*OFDM6M OFDM6M  CCK1M  CCK1M  */
+        {  5,          5,          1,          1},   /*OFDM9M OFDM9M  CCK2M  CCK2M  */
+        {  13,         12,         2,          2},   /*MCS1   MCS0    CCK5M  CCK5M  */
+        {  14,         13,         3,          3},   /*MCS2   MCS1    CCK11M CCK11M */
+        {  15,         14,         13,         12},  /*MCS3   MCS2    MCS1   MCS0   */
+        {  16,         15,         14,         13},  /*MCS4   MCS3    MCS2   MCS1   */
+        {  17,         16,         15,         14},  /*MCS5   MCS4    MCS3   MCS2   */
+        {  18,         17,         16,         15},  /*MCS6   MCS5    MCS4   MCS3   */
+        {  19,         18,         17,         16},  /*MCS7   MCS6    MCS5   MCS4   */
+        {  0,          19,         18,         17},  /*0      MCS7    MCS6   MCS5   */
+        {  0,          30,         19,         18},  /*0      MCS7SG  MCS7   MCS6   */
+        {  0,          0,          0,          19},  /*0      0       0      MCS7   */
+        {  0,          0,          0,          30},  /*0      0       0      MCS7SG */
+        {  0,          0,          0,          0 },  /*0      0       0      0      */
+        {  0,          0,          0,          0 }   /*0      0       0      0      */
+    };
+
+const u16_t zcRate[] =
+    {
+        1, 2, 5, 11,                  /* 1M, 2M, 5M, 11M          ,  0  1  2  3*/
+        6, 9, 12, 18,                 /* 6M  9M  12M  18M         ,  4  5  6  7*/
+        24, 36, 48, 54,               /* 24M  36M  48M  54M       ,  8  9 10 11*/
+        13, 27, 40, 54,               /* MCS0 MCS1 MCS2 MCS3      , 12 13 14 15*/
+        81, 108, 121, 135,            /* MCS4 MCS5 MCS6 MCS7      , 16 17 18 19*/
+        27, 54, 81, 108,              /* MCS8 MCS9 MCS10 MCS11    , 20 21 22 23*/
+        162, 216, 243, 270,           /* MCS12 MCS13 MCS14 MCS15  , 24 25 26 27*/
+        270, 300, 150                 /* MCS14SG, MCS15SG, MCS7SG , 28 29 30*/
+    };
+
+const u16_t PERThreshold[] =
+    {
+        100, 50, 50, 50,    /* 1M, 2M, 5M, 11M          ,  0  1  2  3*/
+        50, 50, 30, 30,     /* 6M  9M  12M  18M         ,  4  5  6  7*/
+        25, 25, 25, 20,     /* 24M  36M  48M  54M       ,  8  9 10 11*/
+        50, 50, 50, 40,    /* MCS0 MCS1 MCS2 MCS3      , 12 13 14 15*/
+        30, 30, 30, 30,    /* MCS4 MCS5 MCS6 MCS7      , 16 17 18 19*/
+        30, 30, 25, 25,    /* MCS8 MCS9 MCS10 MCS11    , 20 21 22 23*/
+        25, 25, 15, 15,     /* MCS12 MCS13 MCS14 MCS15  , 24 25 26 27*/
+        15, 15, 10          /* MCS14SG, MCS15SG         , 28 29*/
+    };
+
+const u16_t FailDiff[] =
+    {
+        40, 46, 40, 0,          /* 1M, 2M, 5M, 11M          ,  0  1  2  3*/
+        24, 17, 22, 16,         /* 6M  9M  12M  18M         ,  4  5  6  7*/
+        19, 13, 5, 0,           /* 24M  36M  48M  54M       ,  8  9 10 11*/
+        36, 22, 15, 19,         /* MCS0 MCS1 MCS2 MCS3      , 12 13 14 15*/
+        12, 5, 4, 7,            /* MCS4 MCS5 MCS6 MCS7      , 16 17 18 19*/
+        0, 0, 0, 0,             /* MCS8 MCS9 MCS10 MCS11    , 20 21 22 23*/
+        9, 4, 3, 3,             /* MCS12 MCS13 MCS14 MCS15  , 24 25 26 27*/
+        3, 0, 0                 /* MCS14SG, MCS15SG         , 28 29*/
+    };
+
+
+#ifdef ZM_ENABLE_BA_RATECTRL
+u32_t TxMPDU[29];
+u32_t BAFail[29];
+u32_t BAPER[29];
+const u16_t BADiff[] =
+    {
+        0, 0, 0, 0,
+        0, 0, 0, 0,
+        0, 0, 0, 0,
+        361, 220, 151, 187,
+        122, 48, 41, 65,
+        0, 0, 0, 0,
+        88, 33, 27, 25,
+        0
+    };
+#endif
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlInitCell          */
+/*      Initialize rate control cell.                                   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      type : 0=>11b, 1=>11a/g, 2=>11n, 3=>11n one Tx stream           */
+/*      gBand : 1=>2.4G, 0=>5G                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+void zfRateCtrlInitCell(zdev_t* dev, struct zsRcCell* rcCell, u8_t type,
+        u8_t gBand, u8_t SG40)
+{
+    u8_t i;
+    u8_t maxrate;
+    zmw_get_wlan_dev(dev);
+
+    if (SG40) SG40 = 1;
+
+    if (gBand != 0)
+    {
+        if (type == 1) //11g
+        {
+            for (i=0; i<4; i++) //1M 2M 5M 11M
+            {
+                rcCell->operationRateSet[i] = (u8_t)i;
+            }
+            for (i=4; i<10; i++) //12M 18M 24M 36M 48M 54M
+            {
+                rcCell->operationRateSet[i] = 2+i;
+            }
+            rcCell->operationRateCount = 10;
+            rcCell->currentRateIndex = 5; //18M
+        }
+        else if (type == 2) //11ng
+        {
+            if (wd->wlanMode == ZM_MODE_AP) //AP 11ng 40M
+            {
+                for (i=0; i<15; i++)
+                {
+                    rcCell->operationRateSet[i] = zcHtRateTable[i][3];
+                }
+                if(!SG40) rcCell->operationRateSet[13] = 27;
+                rcCell->operationRateCount = 14+SG40;
+                rcCell->currentRateIndex = 10;
+            }
+            else //STA
+            {
+                if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ) //11ng 40M
+                {
+                    for (i=0; i<15; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtRateTable[i][3];
+                    }
+                    if(!SG40) rcCell->operationRateSet[13] = 27;
+                    rcCell->operationRateCount = 14+SG40;
+                    rcCell->currentRateIndex = 10;
+                }
+                else    //11ng 20M
+                {
+                    for (i=0; i<13; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtRateTable[i][2];
+                    }
+                    rcCell->operationRateCount = 13;
+                    rcCell->currentRateIndex = 9;
+                }
+            }
+        }
+        else if (type == 3) //11ng one Tx stream
+        {
+                if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ) //11ng 40M one Tx stream
+                {
+                    if(SG40 != 0)
+                    {
+                        maxrate = 13;
+                    }
+                    else
+                    {
+                        maxrate = 12;
+                    }
+                    for (i=0; i<maxrate; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtOneTxStreamRateTable[i][3];
+                    }
+                    rcCell->operationRateCount = i;
+                    rcCell->currentRateIndex = ((i+1)*3)/4;
+                }
+                else    //11ng 20M
+                {
+                    for (i=0; i<11; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtOneTxStreamRateTable[i][2];
+                    }
+                    rcCell->operationRateCount = i;
+                    rcCell->currentRateIndex = ((i+1)*3)/4;
+                }
+        }
+        else //if (type == 0) //11b
+        {
+            for (i=0; i<4; i++)
+            {
+                rcCell->operationRateSet[i] = (u8_t)i;
+            }
+            rcCell->operationRateCount = 4;
+            rcCell->currentRateIndex = rcCell->operationRateCount-1;
+        }
+    }
+    else
+    {
+        if (type == 2) //11na
+        {
+            if (wd->wlanMode == ZM_MODE_AP) //AP 11na 40M
+            {
+                for (i=0; i<(12+SG40); i++)
+                {
+                    rcCell->operationRateSet[i] = zcHtRateTable[i][1];
+                }
+                rcCell->operationRateCount = 12+SG40;
+                rcCell->currentRateIndex = 8;
+            }
+            else //STA
+            {
+                if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ) //11na 40M
+                {
+                    for (i=0; i<(12+SG40); i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtRateTable[i][1];
+                    }
+                    rcCell->operationRateCount = 12+SG40;
+                    rcCell->currentRateIndex = 8;
+                }
+                else    //11na 20M
+                {
+                    for (i=0; i<11; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtRateTable[i][0];
+                    }
+                    rcCell->operationRateCount = 11;
+                    rcCell->currentRateIndex = 7;
+                }
+            }
+        }
+        else if (type == 3) //11na one Tx stream
+        {
+                if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ) //11na 40M one Tx stream
+                {
+                    if(SG40 != 0)
+                    {
+                        maxrate = 11;
+                    }
+                    else
+                    {
+                        maxrate = 10;
+                    }
+                    for (i=0; i<maxrate; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtOneTxStreamRateTable[i][1];
+                    }
+                    rcCell->operationRateCount = i;
+                    rcCell->currentRateIndex = ((i+1)*3)/4;
+                }
+                else    //11ng 20M
+                {
+                    for (i=0; i<9; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtOneTxStreamRateTable[i][0];
+                    }
+                    rcCell->operationRateCount = i;
+                    rcCell->currentRateIndex = ((i+1)*3)/4;
+                }
+        }
+        else //if (type == 1) //11a
+        {
+            for (i=0; i<8; i++) //6M 9M 12M 18M 24M 36M 48M 54M
+            {
+                rcCell->operationRateSet[i] = i+4;
+            }
+            rcCell->operationRateCount = 8;
+            rcCell->currentRateIndex = 4;  //24M
+        }
+    }
+
+    rcCell->flag = 0;
+    rcCell->txCount = 0;
+    rcCell->failCount = 0;
+    rcCell->currentRate = rcCell->operationRateSet[rcCell->currentRateIndex];
+    rcCell->lasttxCount = 0;
+    rcCell->lastTime    = wd->tick;
+    rcCell->probingTime = wd->tick;
+    for (i=0; i<ZM_RATE_TABLE_SIZE; i++) {
+        wd->PER[i] = 0;
+        wd->txMPDU[i] = wd->txFail[i] = 0;
+    }
+    wd->probeCount = 0;
+    wd->probeInterval = 0;
+#ifdef ZM_ENABLE_BA_RATECTRL
+    for (i=0; i<29; i++) {
+        TxMPDU[i]=0;
+        BAFail[i]=0;
+        BAPER[i]=0;
+    }
+#endif
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlGetHigherRate     */
+/*      Get a higher rate.                                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      rate                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfRateCtrlGetHigherRate(struct zsRcCell* rcCell)
+{
+    u8_t rateIndex;
+
+    rateIndex = rcCell->currentRateIndex
+            + (((rcCell->currentRateIndex+1) < rcCell->operationRateCount)?1:0);
+    return rcCell->operationRateSet[rateIndex];
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlNextLowerRate     */
+/*      Get a lower rate.                                               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      rate                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfRateCtrlNextLowerRate(zdev_t* dev, struct zsRcCell* rcCell)
+{
+    zmw_get_wlan_dev(dev);
+    if (rcCell->currentRateIndex > 0)
+    {
+        rcCell->currentRateIndex--;
+        rcCell->currentRate = rcCell->operationRateSet[rcCell->currentRateIndex];
+    }
+    zm_msg1_tx(ZM_LV_0, "Lower Tx Rate=", rcCell->currentRate);
+    //DbgPrint("Lower Tx Rate=%d", rcCell->currentRate);
+    rcCell->failCount = rcCell->txCount = 0;
+    rcCell->lasttxCount = 0;
+    rcCell->lastTime  = wd->tick;
+    return rcCell->currentRate;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlRateDiff          */
+/*      Rate difference.                                                */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*      retryRate : retry rate                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      rate difference                                                 */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfRateCtrlRateDiff(struct zsRcCell* rcCell, u8_t retryRate)
+{
+    u16_t i;
+
+    /* Find retryRate in operationRateSet[] */
+    for (i=0; i<rcCell->operationRateCount; i++)
+    {
+        if (retryRate == rcCell->operationRateSet[i])
+        {
+            if (i < rcCell->currentRateIndex)
+            {
+                return ((rcCell->currentRateIndex - i)+1)>>1;
+            }
+            else if (i == rcCell->currentRateIndex == 0)
+            {
+                return 1;
+            }
+            else
+            {
+                return 0;
+            }
+        }
+    }
+    /* TODO : retry rate not in operation rate set */
+    zm_msg1_tx(ZM_LV_0, "Not in operation rate set:", retryRate);
+    return 1;
+
+}
+
+u32_t zfRateCtrlUDPTP(zdev_t* dev, u16_t Rate, u32_t PER) {
+    if ((PER < 100) && (Rate > 0) && PER)
+        return 1168000/(((12304/Rate)+197)*(100+100*PER/(100-PER)));
+    else
+        return 0;
+}
+
+u8_t zfRateCtrlFindMaxUDPTP(zdev_t* dev, struct zsRcCell* rcCell) {
+    u8_t i, maxIndex=0, rateIndex;
+    u32_t max=0, UDPThroughput;
+
+    zmw_get_wlan_dev(dev);
+
+    rateIndex = zm_agg_min(rcCell->currentRateIndex+3, rcCell->operationRateCount-1);
+    for (i=rcCell->currentRateIndex; i < rateIndex; i++) {
+        UDPThroughput = zfRateCtrlUDPTP(dev, zcRate[rcCell->operationRateSet[i]],
+            wd->PER[rcCell->operationRateSet[i]]);
+        if (max < UDPThroughput) {
+            max = UDPThroughput;
+            maxIndex = i;
+        }
+    }
+
+    return rcCell->operationRateSet[maxIndex];
+}
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlGetTxRate         */
+/*      Get transmission rate.                                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      rcCell : rate control cell                                      */
+/*      probing : rate probing flag                                     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Tx rate                                                         */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfRateCtrlGetTxRate(zdev_t* dev, struct zsRcCell* rcCell, u16_t* probing)
+{
+    u8_t newRate, highRate;
+    zmw_get_wlan_dev(dev);
+
+    zm_msg1_tx(ZM_LV_3, "txCount=", rcCell->txCount);
+    zm_msg1_tx(ZM_LV_3, "probingTime=", rcCell->probingTime);
+    zm_msg1_tx(ZM_LV_3, "tick=", wd->tick);
+    *probing = 0;
+    newRate = rcCell->currentRate;
+
+    if (wd->probeCount && (wd->probeCount < wd->success_probing))
+    {
+        if (wd->probeInterval < 50)
+        {
+            wd->probeInterval++;
+        }
+        else
+        {
+            wd->probeInterval++;
+            if (wd->probeInterval > 52) //probe 51, 52, 53 three packets every 50 packets
+            {
+                wd->probeInterval = 0;
+            }
+            newRate=zfRateCtrlGetHigherRate(rcCell);
+            *probing = 1;
+            wd->probeCount++;
+            rcCell->probingTime = wd->tick;
+        }
+    }
+    /* Accumulate at least 1000ms and 8 packets or Accumulate over 1K packets */
+    else if ((((wd->tick - rcCell->probingTime) > (ZM_RATE_CTRL_PROBING_INTERVAL_MS/ZM_MS_PER_TICK))
+                && (rcCell->txCount >= ZM_RATE_CTRL_MIN_PROBING_PACKET))
+        || (rcCell->txCount >= 1000))
+    {
+#ifndef ZM_DISABLE_RATE_CTRL
+        /* PER = fail/total */
+        wd->probeCount = 0;
+        wd->probeSuccessCount = 0;
+        if (wd->txMPDU[rcCell->currentRate] != 0) {
+            wd->PER[rcCell->currentRate] = zm_agg_min(100,
+                (wd->txFail[rcCell->currentRate]*100)/wd->txMPDU[rcCell->currentRate]);
+            if (!wd->PER[rcCell->currentRate]) wd->PER[rcCell->currentRate] ++;
+        }
+
+        /* if PER < threshold, do rate probing, return probing rate */
+        if ((wd->PER[rcCell->currentRate] <= (ZM_RATE_PROBING_THRESHOLD+15)) ||
+            ((rcCell->currentRate <= 16) &&
+            ((wd->PER[rcCell->currentRate]/2) <= ZM_RATE_PROBING_THRESHOLD)))
+        {
+            if ((newRate=zfRateCtrlGetHigherRate(rcCell)) != rcCell->currentRate)
+            {
+                *probing = 1;
+                wd->probeCount++;
+                wd->probeInterval = 0;
+                wd->success_probing =
+                    (rcCell->currentRate <= 16)? (ZM_RATE_SUCCESS_PROBING/2) : ZM_RATE_SUCCESS_PROBING;
+                //DbgPrint("Start Probing");
+                zm_msg1_tx(ZM_LV_0, "Probing Rate=", newRate);
+            }
+        }
+#endif
+
+        zm_msg0_tx(ZM_LV_1, "Diminish counter");
+        rcCell->failCount = rcCell->failCount>>1;
+        rcCell->txCount = rcCell->txCount>>1;
+        wd->txFail[rcCell->currentRate] = wd->txFail[rcCell->currentRate] >> 1;
+        wd->txMPDU[rcCell->currentRate] = wd->txMPDU[rcCell->currentRate] >> 1;
+
+
+        if (rcCell->currentRate > 15) {
+            highRate = zfRateCtrlGetHigherRate(rcCell);
+            if ((highRate != rcCell->currentRate) && wd->PER[highRate] &&
+                ((wd->PER[rcCell->currentRate] + FailDiff[rcCell->currentRate]) >
+                wd->PER[highRate])) {
+                //DbgPrint("PER compare force raise rate to %d", highRate);
+                wd->probeSuccessCount = wd->probeCount = ZM_RATE_SUCCESS_PROBING;
+                zfRateCtrlTxSuccessEvent(dev, rcCell, highRate);
+            }
+        }
+        else {
+            highRate = zfRateCtrlFindMaxUDPTP(dev, rcCell);
+            if (rcCell->currentRate < highRate) {
+                //DbgPrint("UDP Throughput compare force raise rate to %d", highRate);
+                wd->probeSuccessCount = wd->probeCount = ZM_RATE_SUCCESS_PROBING;
+                zfRateCtrlTxSuccessEvent(dev, rcCell, highRate);
+            }
+        }
+        rcCell->probingTime = wd->tick;
+    }
+
+    if( (wd->tick > 1000)
+        && ((wd->tick - rcCell->lastTime) > 3840) )
+    {
+        if (rcCell->lasttxCount < 70)
+        {
+            rcCell->failCount = rcCell->failCount>>1;
+            rcCell->txCount = rcCell->txCount>>1;
+            wd->txFail[rcCell->currentRate] = wd->txFail[rcCell->currentRate] >> 1;
+            wd->txMPDU[rcCell->currentRate] = wd->txMPDU[rcCell->currentRate] >> 1;
+
+            rcCell->failCount = (rcCell->failCount < rcCell->txCount)?
+                                rcCell->failCount : rcCell->txCount;
+            wd->txFail[rcCell->currentRate] = (wd->txFail[rcCell->currentRate] < wd->txMPDU[rcCell->currentRate])?
+                                              wd->txFail[rcCell->currentRate] : wd->txMPDU[rcCell->currentRate];
+        }
+
+        rcCell->lastTime    = wd->tick;
+        rcCell->lasttxCount = 0;
+    }
+
+    rcCell->txCount++;
+    rcCell->lasttxCount++;
+    wd->txMPDU[rcCell->currentRate]++;
+    zm_msg1_tx(ZM_LV_1, "Get Tx Rate=", newRate);
+    return newRate;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlTxFailEvent       */
+/*      Tx fail event. Calculate PER and lower Tx rate if under         */
+/*      PER under threshold.                                            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*      retryRate : retry rate                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+void zfRateCtrlTxFailEvent(zdev_t* dev, struct zsRcCell* rcCell, u8_t aggRate, u32_t retryRate)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+#ifndef ZM_DISABLE_RATE_CTRL
+    //DbgPrint("aggRate=%d, retryRate=%d", aggRate, retryRate);
+    if (aggRate && (aggRate != rcCell->currentRate)) {
+        wd->txFail[aggRate] += retryRate;
+        return;
+    }
+
+    if (!aggRate) {
+        retryRate = (zfRateCtrlRateDiff(rcCell, (u8_t)retryRate)+1)>>1;
+        if (rcCell->currentRate <12) //legacy rate
+        {
+            retryRate*=2;
+        }
+    }
+    rcCell->failCount += retryRate;
+    wd->txFail[rcCell->currentRate] += retryRate;
+
+    //DbgPrint("failCount=%d", rcCell->failCount);
+    if (rcCell->failCount > ZM_MIN_RATE_FAIL_COUNT)
+    {
+        if (wd->txMPDU[rcCell->currentRate] != 0) {
+            wd->PER[rcCell->currentRate] = zm_agg_min(100,
+                (wd->txFail[rcCell->currentRate]*100)/wd->txMPDU[rcCell->currentRate]);
+            if (!wd->PER[rcCell->currentRate]) wd->PER[rcCell->currentRate] ++;
+        }
+        //zm_msg1_tx(ZM_LV_1, "PER=", per);
+        //DbgPrint("PER=%d, txFail=%d, txMPDU=%d", wd->PER[rcCell->currentRate], wd->txFail[rcCell->currentRate], wd->txMPDU[rcCell->currentRate]);
+        if (wd->PER[rcCell->currentRate] > PERThreshold[rcCell->currentRate])
+        {
+            /* Lower Tx Rate if PER < THRESHOLD */
+            zfRateCtrlNextLowerRate(dev, rcCell);
+            rcCell->flag |= ZM_RC_TRAINED_BIT;
+
+            // Resolve compatibility problem with Marvell
+            if(rcCell->currentRate == 15)
+            {
+                zmw_leave_critical_section(dev);
+                zfHpSetAggPktNum(dev, 8);
+                zmw_enter_critical_section(dev);
+            }
+
+            wd->txFail[rcCell->currentRate] = wd->txFail[rcCell->currentRate] >> 1;
+            wd->txMPDU[rcCell->currentRate] = wd->txMPDU[rcCell->currentRate] >> 1;
+
+            wd->probeCount = wd->probeSuccessCount = 0;
+        }
+    }
+
+#endif
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlTxSuccessEvent    */
+/*      Tx success event. Raise Tx rate because rate probing success.   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*      successRate : success rate                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+void zfRateCtrlTxSuccessEvent(zdev_t* dev, struct zsRcCell* rcCell, u8_t successRate)
+{
+    /* Raise Tx Rate */
+    u16_t i, PERProbe;
+    u16_t pcount;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    //DbgPrint("Probing successRate=%d", successRate);
+    /* Find successRate in operationRateSet[] */
+    wd->probeSuccessCount++;
+    if (wd->probeCount < wd->success_probing)
+    {
+        return;
+    }
+
+    pcount = wd->probeCount;
+    if (pcount != 0)
+    {
+        PERProbe = wd->probeSuccessCount * 100 / pcount;
+    }
+    else
+    {
+        PERProbe = 1;
+    }
+
+    if (PERProbe < ((rcCell->currentRate < 16)? 80:100))
+    {
+        return;
+    }
+    //DbgPrint("wd->probeCount=%d, wd->probeSuccessCount=%d", wd->probeCount, wd->probeSuccessCount);
+    wd->probeCount = wd->probeSuccessCount = 0;
+    for (i=0; i<rcCell->operationRateCount; i++)
+    {
+        if (successRate == rcCell->operationRateSet[i])
+        {
+            if (i > rcCell->currentRateIndex)
+            {
+                /* Raise current Tx rate */
+                zm_msg1_tx(ZM_LV_0, "Raise Tx Rate=", successRate);
+                //DbgPrint("Raise Tx Rate=%d", successRate);
+
+                // Resolve compatibility problem with Marvell
+                if((rcCell->currentRate <= 15) && (successRate > 15))
+                {
+                    zmw_leave_critical_section(dev);
+                    zfHpSetAggPktNum(dev, 16);
+                    zmw_enter_critical_section(dev);
+                }
+
+                rcCell->currentRate = successRate;
+                rcCell->currentRateIndex = (u8_t)i;
+                rcCell->failCount = rcCell->txCount = 0;
+                rcCell->lasttxCount = 0;
+                rcCell->lastTime  = wd->tick;
+                wd->txFail[rcCell->currentRate] = wd->txFail[rcCell->currentRate] >> 1;
+                wd->txMPDU[rcCell->currentRate] = wd->txMPDU[rcCell->currentRate] >> 1;
+            }
+        }
+    }
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlRxRssiEvent       */
+/*      Rx RSSI event. Calculate RSSI moving average, accelarate        */
+/*      rate probing if RSSI variation over threshold.                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*      successRate : success rate                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+void zfRateCtrlRxRssiEvent(struct zsRcCell* rcCell, u16_t rxRssi)
+{
+    /* if delta(rcCell->rxRssi, rxRssi) > ZM_RATE_CTRL_RSSI_VARIATION */
+    if ((rcCell->rxRssi - rxRssi) > ZM_RATE_CTRL_RSSI_VARIATION)
+    {
+        /* Accelerate rate probing via decreaing rcCell->probingTime */
+        rcCell->probingTime -= ZM_RATE_CTRL_PROBING_INTERVAL_MS/ZM_MS_PER_TICK;
+    }
+
+    /* Update RSSI moving average */
+    rcCell->rxRssi = (((rcCell->rxRssi*7) + rxRssi)+4) >> 3;
+    return;
+}
+
+
+#ifdef ZM_ENABLE_BA_RATECTRL
+u8_t HigherRate(u8_t Rate) {
+    if (Rate < 28) Rate++; //28=MCS15SG, 27=MCS15, 26=MCS14, 25=MCS13
+    if (Rate > 28) Rate = 28;
+    while ((Rate >= 20) && (Rate <= 23)) {
+        Rate ++;
+    }
+    return Rate;
+}
+
+u8_t LowerRate(u8_t Rate) {
+    if (Rate > 1) Rate--;
+    while ((Rate >= 20) && (Rate <= 23)) {
+        Rate --;
+    }
+    return Rate;
+}
+
+u8_t RateMapToRateIndex(u8_t Rate, struct zsRcCell* rcCell) {
+    u8_t i;
+    for (i=0; i<rcCell->operationRateCount; i++) {
+        if (Rate == rcCell->operationRateSet[i]) {
+            return i;
+        }
+    }
+    return 0;
+}
+
+void zfRateCtrlAggrSta(zdev_t* dev) {
+    u8_t RateIndex, Rate;
+    u8_t HRate;
+    u8_t LRate;
+    u32_t RateCtrlTxMPDU, RateCtrlBAFail;
+    zmw_get_wlan_dev(dev);
+
+    RateIndex = wd->sta.oppositeInfo[0].rcCell.currentRateIndex;
+    Rate = wd->sta.oppositeInfo[0].rcCell.operationRateSet[RateIndex];
+
+    TxMPDU[Rate] = (TxMPDU[Rate] / 5) + (wd->commTally.RateCtrlTxMPDU * 4 / 5);
+    BAFail[Rate] = (BAFail[Rate] / 5) + (wd->commTally.RateCtrlBAFail * 4 / 5);
+    RateCtrlTxMPDU = wd->commTally.RateCtrlTxMPDU;
+    RateCtrlBAFail = wd->commTally.RateCtrlBAFail;
+    wd->commTally.RateCtrlTxMPDU = 0;
+    wd->commTally.RateCtrlBAFail = 0;
+    if (TxMPDU[Rate] > 0) {
+        BAPER[Rate] = BAFail[Rate] * 1000 / TxMPDU[Rate]; //PER*1000
+        BAPER[Rate] = (BAPER[Rate]>0)? BAPER[Rate]:1;
+    }
+    else {
+        return;
+    }
+
+    HRate = HigherRate(Rate);
+    LRate = LowerRate(Rate);
+    if (BAPER[Rate]>200) {
+        if ((RateCtrlTxMPDU > 100) && (BAPER[Rate]<300) && (HRate != Rate) && BAPER[HRate] &&
+            (BAPER[HRate] < BAPER[Rate] + BADiff[Rate])) {
+            Rate = HRate;
+            //DbgPrint("Rate improved to %d", Rate);
+        }
+        else {
+            Rate = LRate;
+            //DbgPrint("Rate decreased to %d", Rate);
+        }
+    }
+    else if (BAPER[Rate] && BAPER[Rate]<100) {
+        if (RateCtrlTxMPDU > 100) {
+            Rate = HRate;
+            //DbgPrint("Rate improved to %d", Rate);
+        }
+    }
+    wd->sta.oppositeInfo[0].rcCell.currentRate = Rate;
+    wd->sta.oppositeInfo[0].rcCell.currentRateIndex = RateMapToRateIndex(Rate, &wd->sta.oppositeInfo[0].rcCell);
+}
+#endif
diff --git a/drivers/staging/otus/80211core/ratectrl.h b/drivers/staging/otus/80211core/ratectrl.h
new file mode 100644
index 000000000000..92411d725cd8
--- /dev/null
+++ b/drivers/staging/otus/80211core/ratectrl.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _RATECTRL_H
+#define _RATECTRL_H
+
+#define ZM_RATE_CTRL_PROBING_INTERVAL_MS    1000 //1000ms
+#define ZM_RATE_CTRL_MIN_PROBING_PACKET     8
+
+#define ZM_MIN_RATE_FAIL_COUNT              20
+
+#define ZM_RATE_PROBING_THRESHOLD           15  //6%
+#define ZM_RATE_SUCCESS_PROBING             10
+
+#define ZM_RATE_CTRL_RSSI_VARIATION         5  //TBD
+
+extern const u32_t zcRateToPhyCtrl[];
+
+extern void zfRateCtrlInitCell(zdev_t* dev, struct zsRcCell* rcCell, u8_t type, u8_t gBand, u8_t SG40);
+extern u16_t zfRateCtrlGetTxRate(zdev_t* dev, struct zsRcCell* rcCell, u16_t* probing);
+extern void zfRateCtrlTxFailEvent(zdev_t* dev, struct zsRcCell* rcCell, u8_t aggRate, u32_t retryRate);
+extern void zfRateCtrlTxSuccessEvent(zdev_t* dev, struct zsRcCell* rcCell, u8_t successRate);
+extern void zfRateCtrlAggrSta(zdev_t* dev);
+#endif
diff --git a/drivers/staging/otus/80211core/struct.h b/drivers/staging/otus/80211core/struct.h
new file mode 100644
index 000000000000..17b5ce37ebb5
--- /dev/null
+++ b/drivers/staging/otus/80211core/struct.h
@@ -0,0 +1,1315 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _STRUCT_H
+#define _STRUCT_H
+
+#include "../oal_marc.h"
+
+#define ZM_SW_LOOP_BACK                     0 /* 1=>enable, 0=>disable */
+#define ZM_PCI_LOOP_BACK                    0 /* 1=>enable, 0=>disable */
+#define ZM_PROTOCOL_RESPONSE_SIMULATION     0
+
+#define ZM_RX_FRAME_SIZE               1600
+
+extern const u8_t zg11bRateTbl[4];
+extern const u8_t zg11gRateTbl[8];
+
+#define ZM_DRIVER_CORE_MAJOR_VERSION        1
+#define ZM_DRIVER_CORE_MINOR_VERSION        1
+#define ZM_DRIVER_CORE_BRANCH_MAJOR_VERSION 3
+#define ZM_DRIVER_CORE_BRANCH_MINOR_VERSION 39
+
+#ifndef ZM_VTXQ_SIZE
+#define ZM_VTXQ_SIZE                        1024 //2^N
+#endif
+
+#define ZM_VTXQ_SIZE_MASK                   (ZM_VTXQ_SIZE-1)
+#define ZM_VMMQ_SIZE                        8 //2^N
+#define ZM_VMMQ_SIZE_MASK                   (ZM_VMMQ_SIZE-1)
+
+#include "cagg.h"
+
+#define ZM_AGG_POOL_SIZE                    20
+#define ZM_RATE_TABLE_SIZE                  32
+
+#define ZM_MAX_BUF_DISCRETE_NUMBER          5
+
+
+
+
+
+
+
+
+
+/**********************************************************************************/
+/* IBSS macros                                                     */
+/**********************************************************************************/
+#define ZM_IBSS_PEER_ALIVE_COUNTER     4
+
+/**********************************************************************************/
+/* BIT mapping related macros                                                     */
+/**********************************************************************************/
+
+#define ZM_BIT_0       0x1
+#define ZM_BIT_1       0x2
+#define ZM_BIT_2       0x4
+#define ZM_BIT_3       0x8
+#define ZM_BIT_4       0x10
+#define ZM_BIT_5       0x20
+#define ZM_BIT_6       0x40
+#define ZM_BIT_7       0x80
+#define ZM_BIT_8       0x100
+#define ZM_BIT_9       0x200
+#define ZM_BIT_10      0x400
+#define ZM_BIT_11      0x800
+#define ZM_BIT_12      0x1000
+#define ZM_BIT_13      0x2000
+#define ZM_BIT_14      0x4000
+#define ZM_BIT_15      0x8000
+#define ZM_BIT_16      0x10000
+#define ZM_BIT_17      0x20000
+#define ZM_BIT_18      0x40000
+#define ZM_BIT_19      0x80000
+#define ZM_BIT_20      0x100000
+#define ZM_BIT_21      0x200000
+#define ZM_BIT_22      0x400000
+#define ZM_BIT_23      0x800000
+#define ZM_BIT_24      0x1000000
+#define ZM_BIT_25      0x2000000
+#define ZM_BIT_26      0x4000000
+#define ZM_BIT_27      0x8000000
+#define ZM_BIT_28      0x10000000
+#define ZM_BIT_29      0x20000000   //WPA support
+#define ZM_BIT_30      0x40000000
+#define ZM_BIT_31      0x80000000
+
+
+/**********************************************************************************/
+/* MAC address related macros                                                     */
+/**********************************************************************************/
+#define ZM_MAC_BYTE_TO_WORD(macb, macw)   macw[0] = macb[0] + (macb[1] << 8); \
+                                          macw[1] = macb[2] + (macb[3] << 8); \
+                                          macw[2] = macb[4] + (macb[5] << 8);
+
+#define ZM_MAC_WORD_TO_BYTE(macw, macb)   macb[0] = (u8_t) (macw[0] & 0xff); \
+                                          macb[1] = (u8_t) (macw[0] >> 8);   \
+                                          macb[2] = (u8_t) (macw[1] & 0xff); \
+                                          macb[3] = (u8_t) (macw[1] >> 8);   \
+                                          macb[4] = (u8_t) (macw[2] & 0xff); \
+                                          macb[5] = (u8_t) (macw[2] >> 8);
+
+#define ZM_MAC_0(macw)   ((u8_t)(macw[0] & 0xff))
+#define ZM_MAC_1(macw)   ((u8_t)(macw[0] >> 8))
+#define ZM_MAC_2(macw)   ((u8_t)(macw[1] & 0xff))
+#define ZM_MAC_3(macw)   ((u8_t)(macw[1] >> 8))
+#define ZM_MAC_4(macw)   ((u8_t)(macw[2] & 0xff))
+#define ZM_MAC_5(macw)   ((u8_t)(macw[2] >> 8))
+
+#define ZM_IS_MULTICAST_OR_BROADCAST(mac) (mac[0] & 0x01)
+#define ZM_IS_MULTICAST(mac) ((mac[0] & 0x01) && (((u8_t)mac[0]) != 0xFF))
+
+#define ZM_MAC_EQUAL(mac1, mac2)   ((mac1[0]==mac2[0])&&(mac1[1]==mac2[1])&&(mac1[2]==mac2[2]))
+#define ZM_MAC_NOT_EQUAL(mac1, mac2)   ((mac1[0]!=mac2[0])||(mac1[1]!=mac2[1])||(mac1[2]!=mac2[2]))
+/**********************************************************************************/
+/* MAC address related mac'ros (end)                                               */
+/**********************************************************************************/
+#define ZM_BYTE_TO_WORD(A, B)   ((A<<8)+B)
+#define ZM_ROL32( A, n ) \
+        ( ((A) << (n)) | ( ((A)>>(32-(n)))  & ( (1UL << (n)) - 1 ) ) )
+#define ZM_ROR32( A, n ) ZM_ROL32( (A), 32-(n) )
+#define ZM_LO8(v16)  ((u8_t)((v16) & 0xFF))
+#define ZM_HI8(v16)  ((u8_t)(((v16)>>8)&0xFF))
+
+#ifdef ZM_ENABLE_BUFFER_TRACE
+extern void zfwBufTrace(zdev_t* dev, zbuf_t *buf, u8_t *functionName);
+#define ZM_BUFFER_TRACE(dev, buf)       zfwBufTrace(dev, buf, __func__);
+#else
+#define ZM_BUFFER_TRACE(dev, buf)
+#endif
+
+/* notification events to heart beat function */
+#define ZM_BSSID_LIST_SCAN         0x01
+
+/* CAM mode */
+#define ZM_CAM_AP                       0x1
+#define ZM_CAM_STA                      0x2
+#define ZM_CAM_HOST                     0x4
+
+/* finite state machine for adapter */
+#define ZM_STA_STATE_DISCONNECT           1
+#define ZM_STA_STATE_CONNECTING           2
+#define ZM_STA_STATE_CONNECTED            3
+
+/* Event definitions for  finite state machine */
+#define ZM_EVENT_TIMEOUT_SCAN             0x0000
+#define ZM_EVENT_TIMEOUT_BG_SCAN          0x0001
+#define ZN_EVENT_TIMEOUT_RECONNECT        0x0002
+#define ZM_EVENT_TIMEOUT_INIT_SCAN        0x0003
+#define ZM_EVENT_TIMEOUT_AUTH             0x0004
+#define ZM_EVENT_TIMEOUT_ASSO             0x0005
+#define ZM_EVENT_TIMEOUT_AUTO_SCAN        0x0006
+#define ZM_EVENT_TIMEOUT_MIC_FAIL         0x0007
+#define ZM_EVENT_TIMEOUT_CHECK_AP         0x0008
+#define ZM_EVENT_CONNECT                  0x0009
+#define ZM_EVENT_INIT_SCAN                0x000a
+#define ZM_EVENT_SCAN                     0x000b
+#define ZM_EVENT_BG_SCAN                  0x000c
+#define ZM_EVENT_DISCONNECT               0x000d
+#define ZM_EVENT_WPA_MIC_FAIL             0x000e
+#define ZM_EVENT_AP_ALIVE                 0x000f
+#define ZM_EVENT_CHANGE_TO_AP             0x0010
+#define ZM_EVENT_CHANGE_TO_STA            0x0011
+#define ZM_EVENT_IDLE                     0x0012
+#define ZM_EVENT_AUTH                     0x0013
+#define ZM_EVENT_ASSO_RSP                 0x0014
+#define ZM_EVENT_WPA_PK_OK                0x0015
+#define ZM_EVENT_WPA_GK_OK                0x0016
+#define ZM_EVENT_RCV_BEACON               0x0017
+#define ZM_EVENT_RCV_PROBE_RSP            0x0018
+#define ZM_EVENT_SEND_DATA                0x0019
+#define ZM_EVENT_AUTO_SCAN                0x001a
+#define ZM_EVENT_MIC_FAIL1                0x001d
+#define ZM_EVENT_MIC_FAIL2                0x001e
+#define ZM_EVENT_IBSS_MONITOR             0x001f
+#define ZM_EVENT_IN_SCAN                  0x0020
+#define ZM_EVENT_CM_TIMER                 0x0021
+#define ZM_EVENT_CM_DISCONNECT            0x0022
+#define ZM_EVENT_CM_BLOCK_TIMER           0x0023
+#define ZM_EVENT_TIMEOUT_ADDBA            0x0024
+#define ZM_EVENT_TIMEOUT_PERFORMANCE      0x0025
+#define ZM_EVENT_SKIP_COUNTERMEASURE	  0x0026
+#define ZM_EVENT_NONE                     0xffff
+
+/* Actions after call finite state machine */
+#define ZM_ACTION_NONE                    0x0000
+#define ZM_ACTION_QUEUE_DATA              0x0001
+#define ZM_ACTION_DROP_DATA               0x0002
+
+/* Timers for finite state machine */
+#define ZM_TICK_ZERO                      0
+#define ZM_TICK_INIT_SCAN_END             8
+#define ZM_TICK_NEXT_BG_SCAN              50
+#define ZM_TICK_BG_SCAN_END               8
+#define ZM_TICK_AUTH_TIMEOUT              4
+#define ZM_TICK_ASSO_TIMEOUT              4
+#define ZM_TICK_AUTO_SCAN                 300
+#define ZM_TICK_MIC_FAIL_TIMEOUT          6000
+#define ZM_TICK_CHECK_AP1                 150
+#define ZM_TICK_CHECK_AP2                 350
+#define ZM_TICK_CHECK_AP3                 250
+#define ZM_TICK_IBSS_MONITOR              160
+#define ZM_TICK_IN_SCAN                   4
+#define ZM_TICK_CM_TIMEOUT                6000
+#define ZM_TICK_CM_DISCONNECT             200
+#define ZM_TICK_CM_BLOCK_TIMEOUT          6000
+
+/* Fix bug#33338 Counter Measure Issur */
+#ifdef NDIS_CM_FOR_XP
+#define ZM_TICK_CM_TIMEOUT_OFFSET        2160
+#define ZM_TICK_CM_DISCONNECT_OFFSET     72
+#define ZM_TICK_CM_BLOCK_TIMEOUT_OFFSET  2160
+#else
+#define ZM_TICK_CM_TIMEOUT_OFFSET        0
+#define ZM_TICK_CM_DISCONNECT_OFFSET     0
+#define ZM_TICK_CM_BLOCK_TIMEOUT_OFFSET  0
+#endif
+
+#define ZM_TIME_ACTIVE_SCAN               30 //ms
+#define ZM_TIME_PASSIVE_SCAN              110 //ms
+
+/* finite state machine for BSS connect */
+#define ZM_STA_CONN_STATE_NONE            0
+#define ZM_STA_CONN_STATE_AUTH_OPEN       1
+#define ZM_STA_CONN_STATE_AUTH_SHARE_1    2
+#define ZM_STA_CONN_STATE_AUTH_SHARE_2    3
+#define ZM_STA_CONN_STATE_ASSOCIATE       4
+#define ZM_STA_CONN_STATE_SSID_NOT_FOUND  5
+#define ZM_STA_CONN_STATE_AUTH_COMPLETED  6
+
+/* finite state machine for WPA handshaking */
+#define ZM_STA_WPA_STATE_INIT             0
+#define ZM_STA_WPA_STATE_PK_OK            1
+#define ZM_STA_WPA_STATE_GK_OK            2
+
+/* various timers */
+#define ZM_INTERVAL_CONNECT_TIMEOUT          20   /* 200 milisecond */
+
+/* IBSS definitions */
+#define ZM_IBSS_PARTNER_LOST                 0
+#define ZM_IBSS_PARTNER_ALIVE                1
+#define ZM_IBSS_PARTNER_CHECK                2
+
+#define ZM_BCMC_ARRAY_SIZE                  16 /* Must be 2^N */
+#define ZM_UNI_ARRAY_SIZE                   16 /* Must be 2^N */
+
+#define ZM_MAX_DEFRAG_ENTRIES               4  /* 2^N */
+#define ZM_DEFRAG_AGING_TIME_SEC            5  /* 5 seconds */
+
+#define ZM_MAX_WPAIE_SIZE                   128
+/* WEP related definitions */
+#define ZM_USER_KEY_DEFAULT                 64
+#define ZM_USER_KEY_PK                      0                /* Pairwise Key */
+#define ZM_USER_KEY_GK                      1                /* Group Key */
+/* AP WLAN Type */
+#define ZM_WLAN_TYPE_PURE_B                 2
+#define ZM_WLAN_TYPE_PURE_G                 1
+#define ZM_WLAN_TYPE_MIXED                  0
+
+/* HAL State */
+#define ZM_HAL_STATE_INIT                   0
+#define ZM_HAL_STATE_RUNNING                1
+
+/* AP Capability */
+#define ZM_All11N_AP                        0x01
+#define ZM_XR_AP                            0x02
+#define ZM_SuperG_AP                        0x04
+
+/* MPDU Density */
+#define ZM_MPDU_DENSITY_NONE                0
+#define ZM_MPDU_DENSITY_1_8US               1
+#define ZM_MPDU_DENSITY_1_4US               2
+#define ZM_MPDU_DENSITY_1_2US               3
+#define ZM_MPDU_DENSITY_1US                 4
+#define ZM_MPDU_DENSITY_2US                 5
+#define ZM_MPDU_DENSITY_4US                 6
+#define ZM_MPDU_DENSITY_8US                 7
+
+/* Software Encryption */
+#define ZM_SW_TKIP_ENCRY_EN                0x01
+#define ZM_SW_TKIP_DECRY_EN                0x02
+#define ZM_SW_WEP_ENCRY_EN                 0x04
+#define ZM_SW_WEP_DECRY_EN                 0x08
+
+/* Default Support Rate */
+#define ZM_DEFAULT_SUPPORT_RATE_ZERO       0x0
+#define ZM_DEFAULT_SUPPORT_RATE_DISCONNECT 0x1
+#define ZM_DEFAULT_SUPPORT_RATE_IBSS_B     0x2
+#define ZM_DEFAULT_SUPPORT_RATE_IBSS_AG    0x3
+
+/* security related definitions */
+struct zsTkipSeed
+{
+    u8_t   tk[32];     /* key */
+    u8_t   ta[6];
+    u16_t  ttak[5];
+    u16_t  ppk[6];
+    u16_t  iv16,iv16tmp;
+    u32_t  iv32,iv32tmp;
+};
+
+struct zsMicVar
+{
+    u32_t  k0, k1;        // Key
+    u32_t  left, right;   // Current state
+    u32_t  m;             // Message accumulator (single word)
+    u16_t  nBytes;        // # bytes in M
+};
+
+struct zsDefragEntry
+{
+    u8_t    fragCount;
+    u8_t    addr[6];
+    u16_t   seqNum;
+    zbuf_t* fragment[8];
+    u32_t   tick;
+};
+
+struct zsDefragList
+{
+    struct zsDefragEntry   defragEntry[ZM_MAX_DEFRAG_ENTRIES];
+    u8_t                   replaceNum;
+};
+
+#define ZM_MAX_OPPOSITE_COUNT      16
+#define ZM_MAX_TX_SAMPLES          15
+#define ZM_TX_RATE_DOWN_CRITERIA   80
+#define ZM_TX_RATE_UP_CRITERIA    200
+
+
+#define ZM_MAX_PROBE_HIDDEN_SSID_SIZE 2
+struct zsSsidList
+{
+    u8_t            ssid[32];
+    u8_t            ssidLen;
+};
+
+struct zsWrapperSetting
+{
+    u8_t            bDesiredBssid;
+    u8_t            desiredBssid[6];
+    u16_t           bssid[3];
+    u8_t            ssid[32];
+    u8_t            ssidLen;
+    u8_t            authMode;
+    u8_t            wepStatus;
+    u8_t            encryMode;
+    u8_t            wlanMode;
+    u16_t           frequency;
+    u16_t           beaconInterval;
+    u8_t            dtim;
+    u8_t            preambleType;
+    u16_t           atimWindow;
+
+    struct zsSsidList probingSsidList[ZM_MAX_PROBE_HIDDEN_SSID_SIZE];
+
+    u8_t            dropUnencryptedPkts;
+    u8_t            ibssJoinOnly;
+    u32_t           adhocMode;
+    u8_t            countryIsoName[4];
+    u16_t           autoSetFrequency;
+
+    /* AP */
+    u8_t            bRateBasic;
+    u8_t            gRateBasic;
+    u32_t           nRateBasic;
+    u8_t            bgMode;
+
+    /* Common */
+    u8_t            staWmeEnabled;
+    u8_t            staWmeQosInfo;
+    u8_t            apWmeEnabled;
+
+
+    /* rate information: added in the future */
+};
+
+struct zsWrapperFeatureCtrl
+{
+    u8_t           bIbssGMode;
+};
+
+#define  ZM_MAX_PS_STA            16
+#define  ZM_PS_QUEUE_SIZE         32
+
+struct zsStaPSEntity
+{
+    u8_t           bUsed;
+    u8_t           macAddr[6];
+    u8_t           bDataQueued;
+};
+
+struct zsStaPSList
+{
+    u8_t           count;
+    struct zsStaPSEntity    entity[ZM_MAX_PS_STA];
+};
+
+#define ZM_MAX_TIMER_COUNT   32
+
+/* double linked list */
+struct zsTimerEntry
+{
+    u16_t   event;
+    u32_t   timer;
+    struct zsTimerEntry *pre;
+    struct zsTimerEntry *next;
+};
+
+struct zsTimerList
+{
+    u8_t   freeCount;
+    struct zsTimerEntry list[ZM_MAX_TIMER_COUNT];
+    struct zsTimerEntry *head;
+    struct zsTimerEntry *tail;
+};
+
+/* Multicast list */
+#define ZM_MAX_MULTICAST_LIST_SIZE     64
+
+struct zsMulticastAddr
+{
+    u8_t addr[6];
+};
+
+struct zsMulticastList
+{
+    u8_t   size;
+    struct zsMulticastAddr macAddr[ZM_MAX_MULTICAST_LIST_SIZE];
+};
+
+enum ieee80211_cwm_mode {
+    CWM_MODE20,
+    CWM_MODE2040,
+    CWM_MODE40,
+    CWM_MODEMAX
+
+};
+
+enum ieee80211_cwm_extprotspacing {
+    CWM_EXTPROTSPACING20,
+    CWM_EXTPROTSPACING25,
+    CWM_EXTPROTSPACINGMAX
+};
+
+enum ieee80211_cwm_width {
+    CWM_WIDTH20,
+    CWM_WIDTH40
+};
+
+enum ieee80211_cwm_extprotmode {
+    CWM_EXTPROTNONE,  /* no protection */
+    CWM_EXTPROTCTSONLY,   /* CTS to self */
+    CWM_EXTPROTRTSCTS,    /* RTS-CTS */
+    CWM_EXTPROTMAX
+};
+
+struct ieee80211_cwm {
+
+    /* Configuration */
+    enum ieee80211_cwm_mode         cw_mode;            /* CWM mode */
+    u8_t                            cw_extoffset;       /* CWM Extension Channel Offset */
+    enum ieee80211_cwm_extprotmode  cw_extprotmode;     /* CWM Extension Channel Protection Mode */
+    enum ieee80211_cwm_extprotspacing cw_extprotspacing;/* CWM Extension Channel Protection Spacing */
+    u32_t                           cw_enable;          /* CWM State Machine Enabled */
+    u32_t                           cw_extbusythreshold;/* CWM Extension Channel Busy Threshold */
+
+    /* State */
+    enum ieee80211_cwm_width        cw_width;           /* CWM channel width */
+};
+
+
+/* AP : STA database structure */
+struct zsStaTable
+{
+    u32_t time;     /* tick time */
+    //u32_t phyCtrl;   /* Tx PHY CTRL */
+    u16_t addr[3];  /* STA MAC address */
+    u16_t state;    /* aut/asoc */
+    //u16_t retry;    /* Retry count */
+    struct zsRcCell rcCell;
+
+    u8_t valid;     /* Valid flag : 1=>valid */
+    u8_t psMode;    /* STA power saving mode */
+    u8_t staType;   /* 0=>11b, 1=>11g, 2=>11n */
+    u8_t qosType;   /* 0=>Legacy, 1=>WME */
+    u8_t qosInfo;   /* WME QoS info */
+    u8_t vap;       /* Virtual AP ID */
+    u8_t encryMode; /* Encryption type for this STA */
+    u8_t keyIdx;
+    struct zsMicVar     txMicKey;
+    struct zsMicVar     rxMicKey;
+    u16_t iv16;
+    u32_t iv32;
+#ifdef ZM_ENABLE_CENC
+    /* CENC */
+    u8_t cencKeyIdx;
+    u32_t txiv[4];
+    u32_t rxiv[4];
+#endif //ZM_ENABLE_CENC
+};
+
+struct zdStructWds
+{
+    u8_t    wdsBitmap;                      /* Set bit-N to 1 to enable WDS */
+    u8_t    encryMode[ZM_MAX_WDS_SUPPORT];  /* WDS encryption mode */
+    u16_t   macAddr[ZM_MAX_WDS_SUPPORT][3]; /* WDS neighbor MAC address */
+};
+
+    // htcapinfo 16bits
+#define HTCAP_AdvCodingCap          0x0001
+#define HTCAP_SupChannelWidthSet    0x0002
+#define HTCAP_DynamicSMPS           0x0004
+#define HTCAP_SMEnabled             0x000C
+#define HTCAP_GreenField            0x0010
+#define HTCAP_ShortGIfor20MHz       0x0020
+#define HTCAP_ShortGIfor40MHz       0x0040
+#define HTCAP_TxSTBC                0x0080
+#define HTCAP_RxOneStream           0x0100
+#define HTCAP_RxTwoStream           0x0200
+#define HTCAP_RxThreeStream         0x0300
+#define HTCAP_DelayedBlockACK       0x0400
+#define HTCAP_MaxAMSDULength        0x0800
+#define HTCAP_DSSSandCCKin40MHz     0x1000
+#define HTCAP_PSMPSup               0x2000
+#define HTCAP_STBCControlFrameSup   0x4000
+#define HTCAP_LSIGTXOPProtectionSUP 0x8000
+    // Ampdu HT Parameter Info 8bits
+#define HTCAP_MaxRxAMPDU0           0x00
+#define HTCAP_MaxRxAMPDU1           0x01
+#define HTCAP_MaxRxAMPDU2           0x02
+#define HTCAP_MaxRxAMPDU3           0x03
+    // PCO 8bits
+#define HTCAP_PCO                   0x01
+#define HTCAP_TransmissionTime1     0x02
+#define HTCAP_TransmissionTime2     0x04
+#define HTCAP_TransmissionTime3     0x06
+    // MCS FeedBack 8bits
+#define HTCAP_PlusHTCSupport        0x04
+#define HTCAP_RDResponder           0x08
+    // TX Beamforming 0 8bits
+#define HTCAP_TxBFCapable           0x01
+#define HTCAP_RxStaggeredSoundCap   0x02
+#define HTCAP_TxStaggeredSoundCap   0x04
+#define HTCAP_RxZLFCapable          0x08
+#define HTCAP_TxZLFCapable          0x10
+#define HTCAP_ImplicitTxBFCapable   0x20
+    // Tx Beamforming 1 8bits
+#define HTCAP_ExplicitCSITxBFCap    0x01
+#define HTCAP_ExpUncompSteerMatrCap 0x02
+    // Antenna Selection Capabilities 8bits
+#define HTCAP_AntennaSelectionCap       0x01
+#define HTCAP_ExplicitCSITxASCap        0x02
+#define HTCAP_AntennaIndFeeTxASCap      0x04
+#define HTCAP_ExplicitCSIFeedbackCap    0x08
+#define HTCAP_AntennaIndFeedbackCap     0x10
+#define HTCAP_RxASCap                   0x20
+#define HTCAP_TxSoundPPDUsCap           0x40
+
+
+
+struct zsHTCapability
+{
+    u8_t ElementID;
+    u8_t Length;
+    // HT Capability Info
+    u16_t HtCapInfo;
+    u8_t AMPDUParam;
+    u8_t MCSSet[16];    //16 bytes
+    // Extended HT Capability Info
+    u8_t PCO;
+    u8_t MCSFeedBack;
+
+    u8_t TxBFCap[4];
+    u8_t AselCap;
+};
+
+union zuHTCapability
+{
+    struct zsHTCapability Data;
+    u8_t Byte[28];
+};
+
+    //channelinfo 8bits
+#define ExtHtCap_ExtChannelOffsetAbove  0x01
+#define ExtHtCap_ExtChannelOffsetBelow  0x03
+#define ExtHtCap_RecomTxWidthSet        0x04
+#define ExtHtCap_RIFSMode               0x08
+#define ExtHtCap_ControlAccessOnly      0x10
+    //operatinginfo 16bits
+#define ExtHtCap_NonGFDevicePresent     0x0004
+    //beaconinfo 16bits
+#define ExtHtCap_DualBeacon             0x0040
+#define ExtHtCap_DualSTBCProtection     0x0080
+#define ExtHtCap_SecondaryBeacon        0x0100
+#define ExtHtCap_LSIGTXOPProtectFullSup 0x0200
+#define ExtHtCap_PCOActive              0x0400
+#define ExtHtCap_PCOPhase               0x0800
+
+
+struct zsExtHTCapability
+{
+    u8_t    ElementID;
+    u8_t    Length;
+    u8_t    ControlChannel;
+    u8_t    ChannelInfo;
+    u16_t   OperatingInfo;
+    u16_t   BeaconInfo;
+    // Supported MCS Set
+    u8_t    MCSSet[16];
+};
+
+union zuExtHTCapability
+{
+    struct zsExtHTCapability Data;
+    u8_t Byte[24];
+};
+
+struct InformationElementSta {
+    struct zsHTCapability       HtCap;
+    struct zsExtHTCapability    HtInfo;
+};
+
+struct InformationElementAp {
+    struct zsHTCapability       HtCap;
+};
+
+#define ZM_MAX_FREQ_REQ_QUEUE  32
+typedef void (*zfpFreqChangeCompleteCb)(zdev_t* dev);
+
+struct zsWlanDevFreqControl
+{
+    u16_t                     freqReqQueue[ZM_MAX_FREQ_REQ_QUEUE];
+    u8_t                     freqReqBw40[ZM_MAX_FREQ_REQ_QUEUE];
+    u8_t                     freqReqExtOffset[ZM_MAX_FREQ_REQ_QUEUE];
+    zfpFreqChangeCompleteCb   freqChangeCompCb[ZM_MAX_FREQ_REQ_QUEUE];
+    u8_t                      freqReqQueueHead;
+    u8_t                      freqReqQueueTail;
+};
+
+struct zsWlanDevAp
+{
+    u16_t   protectedObss;    /* protected overlap BSS */
+    u16_t   staAgingTimeSec;  /* in second, STA will be deathed if it does not */
+                              /* active for this long time                     */
+    u16_t   staProbingTimeSec;/* in second, STA will be probed if it does not  */
+                              /* active for this long time                     */
+    u8_t    authSharing;      /* authentication on going*/
+    u8_t    bStaAssociated;   /* 11b STA associated */
+    u8_t    gStaAssociated;   /* 11g STA associated */
+    u8_t    nStaAssociated;   /* 11n STA associated */
+    u16_t   protectionMode;   /* AP protection mode flag */
+    u16_t   staPowerSaving;   /* Set associated power saving STA count */
+
+
+
+    zbuf_t*  uniArray[ZM_UNI_ARRAY_SIZE]; /* array to store unicast frames */
+    u16_t   uniHead;
+    u16_t   uniTail;
+
+    /* HT Capability Info */
+    union zuHTCapability HTCap; //CWYang(+)
+
+    /* Extended HT Capability Info */
+    union zuExtHTCapability ExtHTCap; //CWYang(+)
+
+    /* STA table */
+    struct zsStaTable staTable[ZM_MAX_STA_SUPPORT];
+
+    /* WDS */
+    struct zdStructWds wds;
+    /* WPA */
+    u8_t wpaIe[ZM_MAX_AP_SUPPORT][ZM_MAX_WPAIE_SIZE];
+    u8_t wpaLen[ZM_MAX_AP_SUPPORT];
+    u8_t stawpaIe[ZM_MAX_AP_SUPPORT][ZM_MAX_WPAIE_SIZE];
+    u8_t stawpaLen[ZM_MAX_AP_SUPPORT];
+    u8_t wpaSupport[ZM_MAX_AP_SUPPORT];
+
+    //struct zsTkipSeed   bcSeed;
+    u8_t bcKeyIndex[ZM_MAX_AP_SUPPORT];
+    u8_t bcHalKeyIdx[ZM_MAX_AP_SUPPORT];
+    struct zsMicVar     bcMicKey[ZM_MAX_AP_SUPPORT];
+    u16_t iv16[ZM_MAX_AP_SUPPORT];
+    u32_t iv32[ZM_MAX_AP_SUPPORT];
+
+#ifdef ZM_ENABLE_CENC
+    /* CENC */
+    u32_t txiv[ZM_MAX_AP_SUPPORT][4];
+#endif //ZM_ENABLE_CENC
+
+    /* Virtual AP */
+    u8_t    beaconCounter;
+    u8_t    vapNumber;
+    u8_t    apBitmap;                         /* Set bit-N to 1 to enable VAP */
+    u8_t    hideSsid[ZM_MAX_AP_SUPPORT];
+    u8_t    authAlgo[ZM_MAX_AP_SUPPORT];
+    u8_t    ssid[ZM_MAX_AP_SUPPORT][32];      /* SSID */
+    u8_t    ssidLen[ZM_MAX_AP_SUPPORT];       /* SSID length */
+    u8_t    encryMode[ZM_MAX_AP_SUPPORT];
+    u8_t    wepStatus[ZM_MAX_AP_SUPPORT];
+    u16_t   capab[ZM_MAX_AP_SUPPORT];         /* Capability */
+    u8_t    timBcmcBit[ZM_MAX_AP_SUPPORT];    /* BMCM bit of TIM */
+    u8_t    wlanType[ZM_MAX_AP_SUPPORT];
+
+    /* Array to store BC or MC frames */
+    zbuf_t*  bcmcArray[ZM_MAX_AP_SUPPORT][ZM_BCMC_ARRAY_SIZE];
+    u16_t   bcmcHead[ZM_MAX_AP_SUPPORT];
+    u16_t   bcmcTail[ZM_MAX_AP_SUPPORT];
+
+    u8_t                    qosMode;                          /* 1=>WME */
+    u8_t                    uapsdEnabled;
+    struct zsQueue*         uapsdQ;
+
+    u8_t                    challengeText[128];
+
+    struct InformationElementAp ie[ZM_MAX_STA_SUPPORT];
+
+
+};
+
+#define ZM_MAX_BLOCKING_AP_LIST_SIZE    4 /* 2^N */
+struct zsBlockingAp
+{
+    u8_t addr[6];
+    u8_t weight;
+};
+
+#define ZM_SCAN_MGR_SCAN_NONE           0
+#define ZM_SCAN_MGR_SCAN_INTERNAL       1
+#define ZM_SCAN_MGR_SCAN_EXTERNAL       2
+
+struct zsWlanDevStaScanMgr
+{
+    u8_t                    scanReqs[2];
+    u8_t                    currScanType;
+    u8_t                    scanStartDelay;
+};
+
+#define ZM_PS_MSG_STATE_ACTIVE          0
+#define ZM_PS_MSG_STATE_SLEEP           1
+#define ZM_PS_MSG_STATE_T1              2
+#define ZM_PS_MSG_STATE_T2              3
+#define ZM_PS_MSG_STATE_S1              4
+
+#define ZM_PS_MAX_SLEEP_PERIODS         3       // The number of beacon periods
+
+struct zsWlanDevStaPSMgr
+{
+    u8_t                    state;
+    u8_t                    isSleepAllowed;
+    u8_t                    maxSleepPeriods;
+    u8_t                    ticks;
+    u32_t                   lastTxUnicastFrm;
+    u32_t                   lastTxMulticastFrm;
+    u32_t                   lastTxBroadcastFrm;
+    u8_t                    tempWakeUp; /*enable when wake up but still in ps mode */
+    u16_t                   sleepAllowedtick;
+};
+
+struct zsWlanDevSta
+{
+    u32_t                   beaconTxCnt;  /* Transmitted beacon counter (in IBSS) */
+    u8_t                    txBeaconInd;  /* In IBSS mode, true means that we just transmit a beacon during
+                                             last beacon period.
+                                           */
+    u16_t                   beaconCnt;    /* receive beacon count, will be perodically reset */
+    u16_t                   bssid[3];     /* BSSID of connected AP */
+    u8_t                    ssid[32];     /* SSID */
+    u8_t                    ssidLen;      /* SSID length */
+    u8_t                    mTxRate;      /* Tx rate for multicast */
+    u8_t                    uTxRate;      /* Tx rate for unicast */
+    u8_t                    mmTxRate;     /* Tx rate for management frame */
+    u8_t                    bChannelScan;
+    u8_t                    bScheduleScan;
+
+    u8_t                    InternalScanReq;
+    u16_t                   activescanTickPerChannel;
+    u16_t                   passiveScanTickPerChannel;
+    u16_t                   scanFrequency;
+    u32_t                   connPowerInHalfDbm;
+
+    u16_t                   currentFrequency;
+    u16_t                   currentBw40;
+    u16_t                   currentExtOffset;
+
+    u8_t                    bPassiveScan;
+
+    struct zsBlockingAp     blockingApList[ZM_MAX_BLOCKING_AP_LIST_SIZE];
+
+    //struct zsBssInfo        bssInfoPool[ZM_MAX_BSS];
+    struct zsBssInfo*       bssInfoArray[ZM_MAX_BSS];
+    struct zsBssList        bssList;
+    u8_t                    bssInfoArrayHead;
+    u8_t                    bssInfoArrayTail;
+    u8_t                    bssInfoFreeCount;
+
+    u8_t                    authMode;
+    u8_t                    currentAuthMode;
+    u8_t                    wepStatus;
+    u8_t                    encryMode;
+    u8_t                    keyId;
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    u8_t                    ibssWpa2Psk;
+#endif
+#ifdef ZM_ENABLE_CENC
+    u8_t                    cencKeyId; //CENC
+#endif //ZM_ENABLE_CENC
+    u8_t                    dropUnencryptedPkts;
+    u8_t                    ibssJoinOnly;
+    u8_t                    adapterState;
+    u8_t                    oldAdapterState;
+    u8_t                    connectState;
+    u8_t                    connectRetry;
+    u8_t                    wpaState;
+    u8_t                    wpaIe[ZM_MAX_IE_SIZE + 2];
+    u8_t                    rsnIe[ZM_MAX_IE_SIZE + 2];
+    u8_t                    challengeText[255+2];
+    u8_t                    capability[2];
+    //u8_t                    connectingHiddenAP;
+    //u8_t                    scanWithSSID;
+    u16_t                   aid;
+    u32_t                   mgtFrameCount;
+    u8_t                    bProtectionMode;
+	u32_t					NonNAPcount;
+	u8_t					RTSInAGGMode;
+    u32_t                   connectTimer;
+    u16_t                   atimWindow;
+    u8_t                    desiredBssid[6];
+    u8_t                    bDesiredBssid;
+    struct zsTkipSeed       txSeed;
+    struct zsTkipSeed       rxSeed[4];
+    struct zsMicVar         txMicKey;
+    struct zsMicVar         rxMicKey[4];
+    u16_t                   iv16;
+    u32_t                   iv32;
+    struct zsOppositeInfo   oppositeInfo[ZM_MAX_OPPOSITE_COUNT];
+    u8_t                    oppositeCount;
+    u8_t                    bssNotFoundCount;     /* sitesurvey for search desired ISBB threshold */
+    u16_t                   rxBeaconCount;
+    u8_t                    beaconMissState;
+    u32_t                   rxBeaconTotal;
+    u8_t                    bIsSharedKey;
+    u8_t                    connectTimeoutCount;
+
+    u8_t                    recvAtim;
+
+    /* ScanMgr Control block */
+    struct zsWlanDevStaScanMgr scanMgr;
+    struct zsWlanDevStaPSMgr   psMgr;
+
+    // The callback would be called if receiving an unencrypted packets but
+    // the station is in encrypted mode. The wrapper could decide whether
+    // to drop the packet by its OS setting.
+    zfpStaRxSecurityCheckCb pStaRxSecurityCheckCb;
+
+    /* WME */
+    u8_t                    apWmeCapability; //bit-0 => a WME AP
+                                             //bit-7 => a UAPSD AP
+    u8_t                    wmeParameterSetCount;
+
+    u8_t                    wmeEnabled;
+    #define ZM_STA_WME_ENABLE_BIT       0x1
+    #define ZM_STA_UAPSD_ENABLE_BIT     0x2
+    u8_t                    wmeQosInfo;
+
+    u8_t                    wmeConnected;
+    u8_t                    qosInfo;
+    struct zsQueue*         uapsdQ;
+
+    /* countermeasures */
+    u8_t                    cmMicFailureCount;
+    u8_t                    cmDisallowSsidLength;
+    u8_t                    cmDisallowSsid[32];
+
+    /* power-saving mode */
+    u8_t                    powerSaveMode;
+    zbuf_t*                 staPSDataQueue[ZM_PS_QUEUE_SIZE];
+    u8_t                    staPSDataCount;
+
+    /* IBSS power-saving mode */
+    /* record the STA which has entered the PS mode */
+    struct zsStaPSList      staPSList;
+    /* queue the data of the PS STAs */
+    zbuf_t*                  ibssPSDataQueue[ZM_PS_QUEUE_SIZE];
+    u8_t                    ibssPSDataCount;
+    u8_t                    ibssPrevPSDataCount;
+    u8_t                    bIbssPSEnable;
+    /* BIT_15: ON/OFF, BIT_0~14: Atim Timer */
+    u16_t                   ibssAtimTimer;
+
+    /* WPA2 */
+    struct zsPmkidInfo      pmkidInfo;
+
+    /* Multicast list related objects */
+    struct zsMulticastList  multicastList;
+
+    /* XP packet filter feature : */
+    /* 1=>enable: All multicast address packets, not just the ones enumerated in the multicast address list. */
+    /* 0=>disable */
+    u8_t                    bAllMulticast;
+
+    /* reassociation flag */
+    u8_t                    connectByReasso;
+    u8_t                    failCntOfReasso;
+
+	/* for HT configure control setting */
+    u8_t                    preambleTypeHT;  /* HT: 0 Mixed mode    1 Green field */
+	u8_t                    htCtrlBandwidth;
+	u8_t                    htCtrlSTBC;
+	u8_t                    htCtrlSG;
+    u8_t                    defaultTA;
+
+    u8_t                    connection_11b;
+
+    u8_t                    EnableHT;
+    u8_t                    SG40;
+    u8_t                    HT2040;
+    /* for WPA setting */
+    u8_t                    wpaSupport;
+    u8_t                    wpaLen;
+
+    /* IBSS related objects */
+    u8_t                    ibssDelayedInd;
+    struct zsPartnerNotifyEvent ibssDelayedIndEvent;
+    u8_t                    ibssPartnerStatus;
+
+    u8_t                    bAutoReconnect;
+
+    u8_t                    flagFreqChanging;
+    u8_t                    flagKeyChanging;
+    struct zsBssInfo        ibssBssDesc;
+    u8_t                    ibssBssIsCreator;
+    u16_t                   ibssReceiveBeaconCount;
+    u8_t                    ibssSiteSurveyStatus;
+
+    u8_t                    disableProbingWithSsid;
+#ifdef ZM_ENABLE_CENC
+    /* CENC */
+    u8_t                    cencIe[ZM_MAX_IE_SIZE + 2];
+#endif //ZM_ENABLE_CENC
+    u32_t txiv[4];  //Tx PN Sequence
+    u32_t rxiv[4];  //Rx PN Sequence
+    u32_t rxivGK[4];//Broadcast Rx PN Sequence
+    u8_t  wepKey[4][32];    // For Software WEP
+	u8_t  SWEncryMode[4];
+
+    /* 802.11d */
+    u8_t b802_11D;
+
+    /* 802.11h */
+    u8_t TPCEnable;
+    u8_t DFSEnable;
+    u8_t DFSDisableTx;
+
+    /* Owl AP */
+    u8_t athOwlAp;
+
+    /* Enable BA response in driver */
+    u8_t enableDrvBA;
+
+    /* HT Capability Info */
+    union zuHTCapability HTCap; //CWYang(+)
+
+    /* Extended HT Capability Info */
+    union zuExtHTCapability ExtHTCap; //CWYang(+)
+
+    struct InformationElementSta   ie;
+
+#define ZM_CACHED_FRAMEBODY_SIZE   200
+    u8_t                    asocReqFrameBody[ZM_CACHED_FRAMEBODY_SIZE];
+    u16_t                   asocReqFrameBodySize;
+    u8_t                    asocRspFrameBody[ZM_CACHED_FRAMEBODY_SIZE];
+    u16_t                   asocRspFrameBodySize;
+    u8_t                    beaconFrameBody[ZM_CACHED_FRAMEBODY_SIZE];
+    u16_t                   beaconFrameBodySize;
+
+    u8_t                    ac0PriorityHigherThanAc2;
+    u8_t                    SWEncryptEnable;
+
+    u8_t                    leapEnabled;
+
+    u32_t                    TotalNumberOfReceivePackets;
+    u32_t                    TotalNumberOfReceiveBytes;
+    u32_t                    avgSizeOfReceivePackets;
+
+    u32_t                    ReceivedPacketRateCounter;
+    u32_t                    ReceivedPktRatePerSecond;
+
+    /* #2 Record the sequence number to determine whether the unicast frame is separated by RIFS or not */
+#define    ZM_RIFS_STATE_DETECTING    0
+#define    ZM_RIFS_STATE_DETECTED     1
+#define    ZM_RIFS_TIMER_TIMEOUT      4480          // <Driver time>4480ms <Real time>7s
+    u8_t                    rifsState;
+    u8_t                    rifsLikeFrameCnt;
+    u16_t                   rifsLikeFrameSequence[3];
+    u32_t                   rifsTimer;
+    u32_t                   rifsCount;
+
+    /* RX filter desired by upper layers.  Note this contains some bits which must be filtered
+       by sw since the hw supports only a subset of possible filter actions.= */
+    u32_t  osRxFilter;
+
+    u8_t   bSafeMode;
+
+    u32_t  ibssAdditionalIESize;
+    u8_t   ibssAdditionalIE[256];
+}; //struct zsWlanDevSta
+
+#define ZM_CMD_QUEUE_SIZE                   256  //Roger Check, test 64 when ready
+
+#define ZM_OID_READ                         1
+#define ZM_OID_WRITE                        2
+#define ZM_OID_INTERNAL_WRITE               3
+#define ZM_CMD_SET_FREQUENCY                4
+#define ZM_CMD_SET_KEY                      5
+#define ZM_CWM_READ                         6
+#define ZM_MAC_READ                         7
+#define ZM_ANI_READ                         8
+#define ZM_EEPROM_READ                      9
+#define ZM_EEPROM_WRITE                     0x0A
+#define ZM_OID_CHAN							0x30
+#define ZM_OID_SYNTH						0x32
+#define ZM_OID_TALLY						0x81
+#define ZM_OID_TALLY_APD					0x82
+
+#define ZM_OID_DKTX_STATUS                  0x92
+#define ZM_OID_FLASH_CHKSUM                 0xD0
+#define ZM_OID_FLASH_READ                   0xD1
+#define ZM_OID_FLASH_PROGRAM                0xD2
+#define ZM_OID_FW_DL_INIT                   0xD3
+
+/* Driver to Firmware OID */
+#define ZM_CMD_ECHO         0x80
+#define ZM_CMD_TALLY        0x81
+#define ZM_CMD_TALLY_APD    0x82
+#define ZM_CMD_CONFIG       0x83
+#define ZM_CMD_RREG         0x00
+#define ZM_CMD_WREG         0x01
+#define ZM_CMD_RMEM         0x02
+#define ZM_CMD_WMEM         0x03
+#define ZM_CMD_BITAND       0x04
+#define ZM_CMD_BITOR        0x05
+#define ZM_CMD_EKEY         0x28
+#define ZM_CMD_DKEY         0x29
+#define ZM_CMD_FREQUENCY    0x30
+#define ZM_CMD_RF_INIT      0x31
+#define ZM_CMD_SYNTH        0x32
+#define ZM_CMD_FREQ_STRAT   0x33
+#define ZM_CMD_RESET        0x90
+#define ZM_CMD_DKRESET      0x91
+#define ZM_CMD_DKTX_STATUS  0x92
+#define ZM_CMD_FDC          0xA0
+#define ZM_CMD_WREEPROM     0xB0
+#define ZM_CMD_WFLASH       0xB0
+#define ZM_CMD_FLASH_ERASE  0xB1
+#define ZM_CMD_FLASH_PROG   0xB2
+#define ZM_CMD_FLASH_CHKSUM 0xB3
+#define ZM_CMD_FLASH_READ   0xB4
+#define ZM_CMD_FW_DL_INIT   0xB5
+#define ZM_CMD_MEM_WREEPROM 0xBB
+
+
+/* duplicate filter table column */
+#define ZM_FILTER_TABLE_COL                 2 /* 2^n */
+/* duplicate filter table Row */
+#define ZM_FILTER_TABLE_ROW                 8 /* 2^n */
+
+/* duplicate filter table structure */
+struct zsRxFilter
+{
+    u16_t addr[3];
+    u16_t seq;
+    u8_t up;
+};
+
+struct zsWlanDev
+{
+    /* AP global variables */
+    struct zsWlanDevAp ap;
+    /* STA global variables */
+    struct zsWlanDevSta sta;
+    /* save wrapper setting */
+    struct zsWrapperSetting ws;
+    /* features determined by wrapper (vendor) */
+    struct zsWrapperFeatureCtrl wfc;
+    /* Traffic Monitor tally */
+    struct zsTrafTally trafTally;
+    /* Communication tally */
+    struct zsCommTally commTally;
+    /* Duplicate frame filter table */
+    struct zsRxFilter rxFilterTbl[ZM_FILTER_TABLE_COL][ZM_FILTER_TABLE_ROW];
+    /* Regulatory table */
+    struct zsRegulationTable  regulationTable;
+
+    /* */
+    struct zsWlanDevFreqControl freqCtrl;
+
+    enum devState state;
+
+    u8_t  halState;
+    u8_t  wlanMode;         /* AP/INFRASTRUCTURE/IBSS/PSEUDO */
+    u16_t macAddr[3];       /* MAC address */
+    u16_t beaconInterval;   /* beacon Interval */
+    u8_t dtim;              /* DTIM period */
+    u8_t            CurrentDtimCount;
+    u8_t  preambleType;
+    u8_t  preambleTypeInUsed;
+    u8_t  maxTxPower2;	    /* 2.4 GHz Max Tx power (Unit: 0.5 dBm) */
+    u8_t  maxTxPower5;	    /* 5 GHz Max Tx power (Unit: 0.5 dBm) */
+    u8_t  connectMode;
+    u32_t supportMode;
+
+    u8_t bRate;             /* 11b Support Rate bit map */
+    u8_t bRateBasic;        /* 11b Basic Rate bit map */
+    u8_t gRate;             /* 11g Support Rate bit map */
+    u8_t gRateBasic;        /* 11g Basic Rate bit map */
+    /* channel index point to the item in regulation table */
+    u8_t channelIndex;
+
+    /* channel management */
+    u8_t    BandWidth40;
+    u8_t    ExtOffset;      //1 above, 3 below, 0 not present
+    u16_t   frequency;      /* operation frequency */
+
+    u8_t erpElement;        /* ERP information element data */
+
+    u8_t disableSelfCts;    /* set to 1 to disable Self-CTS */
+    u8_t bgMode;
+
+	/* private test flag */
+    u32_t enableProtectionMode;   /* force enable/disable self cts */
+	u32_t checksumTest;     /* OTUS checksum test 1=>zero checksum 0=>normal */
+	u32_t rxPacketDump;     /* rx packet dump */
+
+    u8_t enableAggregation; /* force enable/disable A-MSPU */
+    u8_t enableWDS;         /* force enable/disable WDS testing */
+	u8_t enableTxPathMode;  /* OTUS special testing mode 1=>diable, 0=>enable: ZM_SYSTEM_TEST_MODE */
+    u8_t enableHALDbgInfo;  /*  */
+
+    u32_t forceTxTPC;       /* force tx packet send TPC */
+
+    u16_t seq[4];
+    u16_t mmseq;
+
+    /* driver core time tick */
+    u32_t tick;
+    u16_t tickIbssSendBeacon;
+    u16_t tickIbssReceiveBeacon;
+
+    /* RTS threshold */
+    u16_t rtsThreshold;
+
+    /* fragmentation threshold,  256 <= value <= 2346, 0=disabled */
+    u16_t fragThreshold;
+
+    /* Tx Rate */
+    u16_t txMCS;
+    u16_t txMT;
+    u32_t CurrentTxRateKbps; //CWYang(+)
+    /* Rx Rate */
+    u32_t CurrentRxRateKbps; //Janet(+)
+    u8_t CurrentRxRateUpdated;
+    u8_t modulationType;
+    u8_t rxInfo;
+    u16_t rateField;
+
+    /* timer related objects */
+    struct zsTimerList  timerList;
+    u8_t                bTimerReady;
+
+    /* for defragmentation */
+    struct zsDefragList defragTable;
+
+    /* Data struct for Interface Dependent Layer */
+    //struct zsIdlStruct idlStruct;
+
+    /* Signal Strength/Quality Related Parameters */
+    u8_t SignalStrength; //CWYang(+)
+    u8_t SignalQuality;  //CWYang(+)
+
+
+
+    /* QoS */
+    zbuf_t* vtxq[4][ZM_VTXQ_SIZE];
+    u16_t vtxqHead[4];
+    u16_t vtxqTail[4];
+    u16_t qosDropIpFrag[4];
+
+    /* Management Tx queue */
+    zbuf_t* vmmq[ZM_VMMQ_SIZE];
+    u16_t vmmqHead;
+    u16_t vmmqTail;
+
+    u8_t                vtxqPushing;
+
+    /*
+     * add by honda
+     * 1. Aggregate queues
+     * 2. STA's associated information and queue number
+     * 3. rx aggregation re-ordering queue
+     */
+    struct aggQueue    *aggQPool[ZM_AGG_POOL_SIZE];
+    u8_t                aggInitiated;
+    u8_t                addbaComplete;
+    u8_t                addbaCount;
+    u8_t                aggState;
+    u8_t                destLock;
+    struct aggSta       aggSta[ZM_MAX_STA_SUPPORT];
+    struct agg_tid_rx  *tid_rx[ZM_AGG_POOL_SIZE];
+    struct aggTally     agg_tal;
+    struct destQ        destQ;
+    struct baw_enabler *baw_enabler;
+    struct ieee80211_cwm    cwm;
+    u16_t               reorder;
+    u16_t               seq_debug;
+    /* rate control */
+    u32_t txMPDU[ZM_RATE_TABLE_SIZE];
+    u32_t txFail[ZM_RATE_TABLE_SIZE];
+    u32_t PER[ZM_RATE_TABLE_SIZE];
+    u16_t probeCount;
+    u16_t probeSuccessCount;
+    u16_t probeInterval;
+    u16_t success_probing;
+    /*
+     * end of add by honda
+     */
+
+    /* airopeek sniffer mode for upper sw */
+    u32_t               swSniffer;   /* window: airoPeek */
+    u32_t               XLinkMode;
+
+    /* MDK mode */
+    /* init by 0=>normal driver 1=>MDK driver */
+    u32_t               modeMDKEnable;
+
+    u32_t               heartBeatNotification;
+
+    /* pointer for HAL Plus private memory */
+    void* hpPrivate;
+
+    /* for WPA setting */
+    //u8_t                    wpaSupport[ZM_MAX_AP_SUPPORT];
+    //u8_t                    wpaLen[ZM_MAX_AP_SUPPORT];
+    //u8_t                    wpaIe[ZM_MAX_AP_SUPPORT][ZM_MAX_IE_SIZE];
+
+    struct zsLedStruct      ledStruct;
+
+    /* ani flag */
+    u8_t aniEnable;
+    u16_t txq_threshold;
+
+	//Skip Mic Error Check
+	u8_t	TKIP_Group_KeyChanging;
+
+	u8_t    dynamicSIFSEnable;
+
+	u8_t    queueFlushed;
+
+    u16_t (*zfcbAuthNotify)(zdev_t* dev, u16_t* macAddr);
+    u16_t (*zfcbAsocNotify)(zdev_t* dev, u16_t* macAddr, u8_t* body, u16_t bodySize, u16_t port);
+    u16_t (*zfcbDisAsocNotify)(zdev_t* dev, u8_t* macAddr, u16_t port);
+    u16_t (*zfcbApConnectNotify)(zdev_t* dev, u8_t* macAddr, u16_t port);
+    void (*zfcbConnectNotify)(zdev_t* dev, u16_t status, u16_t* bssid);
+    void (*zfcbScanNotify)(zdev_t* dev, struct zsScanResult* result);
+    void (*zfcbMicFailureNotify)(zdev_t* dev, u16_t* addr, u16_t status);
+    void (*zfcbApMicFailureNotify)(zdev_t* dev, u8_t* addr, zbuf_t* buf);
+    void (*zfcbIbssPartnerNotify)(zdev_t* dev, u16_t status, struct zsPartnerNotifyEvent *event);
+    void (*zfcbMacAddressNotify)(zdev_t* dev, u8_t* addr);
+    void (*zfcbSendCompleteIndication)(zdev_t* dev, zbuf_t* buf);
+    void (*zfcbRecvEth)(zdev_t* dev, zbuf_t* buf, u16_t port);
+    void (*zfcbRecv80211)(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+    void (*zfcbRestoreBufData)(zdev_t* dev, zbuf_t* buf);
+#ifdef ZM_ENABLE_CENC
+    u16_t (*zfcbCencAsocNotify)(zdev_t* dev, u16_t* macAddr, u8_t* body,
+            u16_t bodySize, u16_t port);
+#endif //ZM_ENABLE_CENC
+    u8_t (*zfcbClassifyTxPacket)(zdev_t* dev, zbuf_t* buf);
+    void (*zfcbHwWatchDogNotify)(zdev_t* dev);
+};
+
+
+struct zsWlanKey
+{
+    u8_t key;
+};
+
+
+/* These macros are defined here for backward compatibility */
+/* Please leave them alone */
+/* For Tx packet allocated in upper layer layer */
+#define zmw_tx_buf_readb(dev, buf, offset) zmw_buf_readb(dev, buf, offset)
+#define zmw_tx_buf_readh(dev, buf, offset) zmw_buf_readh(dev, buf, offset)
+#define zmw_tx_buf_writeb(dev, buf, offset, value) zmw_buf_writeb(dev, buf, offset, value)
+#define zmw_tx_buf_writeh(dev, buf, offset, value) zmw_buf_writeh(dev, buf, offset, value)
+
+/* For Rx packet allocated in driver */
+#define zmw_rx_buf_readb(dev, buf, offset) zmw_buf_readb(dev, buf, offset)
+#define zmw_rx_buf_readh(dev, buf, offset) zmw_buf_readh(dev, buf, offset)
+#define zmw_rx_buf_writeb(dev, buf, offset, value) zmw_buf_writeb(dev, buf, offset, value)
+#define zmw_rx_buf_writeh(dev, buf, offset, value) zmw_buf_writeh(dev, buf, offset, value)
+
+#endif /* #ifndef _STRUCT_H */
diff --git a/drivers/staging/otus/80211core/wlan.h b/drivers/staging/otus/80211core/wlan.h
new file mode 100644
index 000000000000..26c18b837cfc
--- /dev/null
+++ b/drivers/staging/otus/80211core/wlan.h
@@ -0,0 +1,595 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : wlan_defs.h                                           */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains WLAN definitions.                          */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _WLAN_H
+#define _WLAN_H
+
+
+#define ZM_EXTERNAL_ALLOC_BUF               0
+#define ZM_INTERNAL_ALLOC_BUF               1
+
+#define ZM_SIZE_OF_CTRL_SET                 8
+#define ZM_SIZE_OF_IV                       4
+#define ZM_SIZE_OF_EXT_IV                   4
+#define ZM_SIZE_OF_MIC                      8
+#define ZM_SIZE_OF_CCX_MIC                  8
+#define ZM_SIZE_OF_WLAN_DATA_HEADER         24
+#define ZM_SIZE_OF_QOS_CTRL                 2
+
+/* Header definition */
+#define ZM_SIZE_OF_WLAN_WDS_HEADER          32
+#define ZM_SIZE_OF_SNAP_HEADER              8
+
+#define ZM_WLAN_HEADER_A1_OFFSET            4
+#define ZM_WLAN_HEADER_A2_OFFSET            10
+#define ZM_WLAN_HEADER_A3_OFFSET            16
+#define ZM_WLAN_HEADER_A4_OFFSET            24
+#define ZM_WLAN_HEADER_IV_OFFSET            24
+#define ZM_SIZE_OF_WLAN_DATA_HEADER         24
+
+/* Port definition */
+#define ZM_PORT_DISABLED                    0
+#define ZM_PORT_ENABLED                     1
+
+/* Frame Type */
+#define ZM_WLAN_MANAGEMENT_FRAME            0x0
+#define ZM_WLAN_CONTROL_FRAME               0x4
+#define ZM_WLAN_DATA_FRAME                  0x8
+
+/* Frame Subtype */
+#define ZM_WLAN_FRAME_TYPE_ASOCREQ          0x00
+#define ZM_WLAN_FRAME_TYPE_ASOCRSP          0x10
+#define ZM_WLAN_FRAME_TYPE_REASOCREQ        0x20
+#define ZM_WLAN_FRAME_TYPE_REASOCRSP        0x30
+#define ZM_WLAN_FRAME_TYPE_PROBEREQ         0x40
+#define ZM_WLAN_FRAME_TYPE_PROBERSP         0x50
+/* 0x60, 0x70 => Reserved */
+#define ZM_WLAN_FRAME_TYPE_BEACON           0x80
+#define ZM_WLAN_FRAME_TYPE_ATIM             0x90
+#define ZM_WLAN_FRAME_TYPE_DISASOC          0xA0
+#define ZM_WLAN_FRAME_TYPE_AUTH             0xB0
+#define ZM_WLAN_FRAME_TYPE_DEAUTH           0xC0
+#define ZM_WLAN_FRAME_TYPE_ACTION			0xD0
+
+/* Frame type and subtype */
+#define ZM_WLAN_FRAME_TYPE_NULL             0x48
+#define ZM_WLAN_FRAME_TYPE_BAR              0x84
+#define ZM_WLAN_FRAME_TYPE_BA               0x94
+#define ZM_WLAN_FRAME_TYPE_PSPOLL           0xA4
+#define ZM_WLAN_FRAME_TYPE_RTS              0xB4
+#define ZM_WLAN_FRAME_TYPE_CTS              0xC4
+#define ZM_WLAN_FRAME_TYPE_QOS_NULL         0xC8
+
+/* action frame */
+#define ZM_WLAN_SPECTRUM_MANAGEMENT_ACTION_FRAME    0
+#define ZM_WLAN_QOS_ACTION_FRAME            1
+#define ZM_WLAN_DLS_ACTION_FRAME            2
+#define ZM_WLAN_BLOCK_ACK_ACTION_FRAME      3
+/* block ack action frame*/
+#define ZM_WLAN_ADDBA_REQUEST_FRAME         0
+#define ZM_WLAN_ADDBA_RESPONSE_FRAME        1
+#define ZM_WLAN_DELBA_FRAME                 2
+
+/* Element ID */
+#define ZM_WLAN_EID_SSID                    0
+#define ZM_WLAN_EID_SUPPORT_RATE            1
+#define ZM_WLAN_EID_FH                      2
+#define ZM_WLAN_EID_DS                      3
+#define ZM_WLAN_EID_CFS                     4
+#define ZM_WLAN_EID_TIM                     5
+#define ZM_WLAN_EID_IBSS                    6
+#define ZM_WLAN_EID_COUNTRY                 7
+/* reserved 8-15 */
+#define ZM_WLAN_EID_CHALLENGE               16
+/* reserved 17-31 */
+#define ZM_WLAN_EID_POWER_CONSTRAINT        32
+#define ZM_WLAN_EID_POWER_CAPABILITY        33
+#define ZM_WLAN_EID_TPC_REQUEST             34
+#define ZM_WLAN_EID_TPC_REPORT              35
+#define ZM_WLAN_EID_SUPPORTED_CHANNELS      36
+#define ZM_WLAN_EID_CHANNEL_SWITCH_ANNOUNCE 37
+#define ZM_WLAN_EID_MEASUREMENT_REQUEST     38
+#define ZM_WLAN_EID_MEASUREMENT_REPORT      39
+#define ZM_WLAN_EID_QUIET                   40
+#define ZM_WLAN_EID_IBSS_DFS                41
+#define ZM_WLAN_EID_ERP                     42
+#define ZM_WLAN_PREN2_EID_HTCAPABILITY      45
+#define ZM_WLAN_EID_RSN_IE                  48
+#define ZM_WLAN_EID_EXTENDED_RATE           50
+#define ZM_WLAN_EID_HT_CAPABILITY           51
+#define ZM_WLAN_EID_EXTENDED_HT_CAPABILITY  52
+#define ZM_WLAN_EID_NEW_EXT_CHANNEL_OFFSET  53
+#define ZM_WLAN_PREN2_EID_HTINFORMATION     61
+#define ZM_WLAN_PREN2_EID_SECONDCHOFFSET    62
+#ifdef ZM_ENABLE_CENC
+#define ZM_WLAN_EID_CENC_IE                 68
+#endif //ZM_ENABLE_CENC
+#define ZM_WLAN_EID_VENDOR_PRIVATE          221     /* Vendor private space; must demux OUI */
+#define ZM_WLAN_EID_WPA_IE                  221
+#define ZM_WLAN_EID_WPS_IE                  221
+#define ZM_WLAN_EID_WIFI_IE                 221
+
+/* ERP information element */
+#define ZM_WLAN_NON_ERP_PRESENT_BIT         0x1
+#define ZM_WLAN_USE_PROTECTION_BIT          0x2
+#define ZM_WLAN_BARKER_PREAMBLE_MODE_BIT    0x4
+
+/* Channel frequency, in MHz */
+#define ZM_CH_G_1                          2412
+#define ZM_CH_G_2                          2417
+#define ZM_CH_G_3                          2422
+#define ZM_CH_G_4                          2427
+#define ZM_CH_G_5                          2432
+#define ZM_CH_G_6                          2437
+#define ZM_CH_G_7                          2442
+#define ZM_CH_G_8                          2447
+#define ZM_CH_G_9                          2452
+#define ZM_CH_G_10                         2457
+#define ZM_CH_G_11                         2462
+#define ZM_CH_G_12                         2467
+#define ZM_CH_G_13                         2472
+#define ZM_CH_G_14                         2484
+#define ZM_CH_A_184                        4920
+#define ZM_CH_A_188                        4940
+#define ZM_CH_A_192                        4960
+#define ZM_CH_A_196                        4980
+#define ZM_CH_A_8                          5040
+#define ZM_CH_A_12                         5060
+#define ZM_CH_A_16                         5080
+#define ZM_CH_A_36                         5180
+#define ZM_CH_A_40                         5200
+#define ZM_CH_A_44                         5220
+#define ZM_CH_A_48                         5240
+#define ZM_CH_A_52                         5260
+#define ZM_CH_A_56                         5280
+#define ZM_CH_A_60                         5300
+#define ZM_CH_A_64                         5320
+#define ZM_CH_A_100                        5500
+#define ZM_CH_A_104                        5520
+#define ZM_CH_A_108                        5540
+#define ZM_CH_A_112                        5560
+#define ZM_CH_A_116                        5580
+#define ZM_CH_A_120                        5600
+#define ZM_CH_A_124                        5620
+#define ZM_CH_A_128                        5640
+#define ZM_CH_A_132                        5660
+#define ZM_CH_A_136                        5680
+#define ZM_CH_A_140                        5700
+#define ZM_CH_A_149                        5745
+#define ZM_CH_A_153                        5765
+#define ZM_CH_A_157                        5785
+#define ZM_CH_A_161                        5805
+#define ZM_CH_A_165                        5825
+
+
+/* AP : STA table => STA Type */
+#define ZM_11B_STA                          0x0
+#define ZM_11G_STA                          0x2
+#define ZM_11N_STA                          0x4
+
+/* AP : timeout */
+#define ZM_MS_PER_TICK                      10
+#define ZM_TICK_PER_SECOND                  (1000/ZM_MS_PER_TICK)
+#define ZM_TICK_PER_MINUTE                  (60*1000/ZM_MS_PER_TICK)
+#define ZM_PREAUTH_TIMEOUT_MS               1000 /* 1 sec */
+#define ZM_AUTH_TIMEOUT_MS                  1000 /* 1 sec */
+
+/* Error code */
+#define ZM_SUCCESS                          0
+#define ZM_ERR_TX_PORT_DISABLED             1
+#define ZM_ERR_BUFFER_DMA_ADDR              2
+#define ZM_ERR_FREE_TXD_EXHAUSTED           3
+#define ZM_ERR_TX_BUFFER_UNAVAILABLE        4
+#define ZM_ERR_BCMC_PS_BUFFER_UNAVAILABLE   5
+#define ZM_ERR_UNI_PS_BUFFER_UNAVAILABLE    6
+#define ZM_ERR_EXCEED_PRIORITY_THRESHOLD    7
+#define ZM_ERR_VMMQ_FULL                    8
+#define ZM_ERR_FLUSH_PS_QUEUE               9
+#define ZM_ERR_CMD_INT_MISSED               15 /* Polling cmd int timeout*/
+/* Rx */
+#define ZM_ERR_RX_FRAME_TYPE                20
+#define ZM_ERR_MIN_RX_ENCRYPT_FRAME_LENGTH  21
+#define ZM_ERR_MIN_RX_FRAME_LENGTH          22
+#define ZM_ERR_MAX_RX_FRAME_LENGTH          23
+#define ZM_ERR_RX_DUPLICATE                 24
+#define ZM_ERR_RX_SRC_ADDR_IS_OWN_MAC       25
+#define ZM_ERR_MIN_RX_PROTOCOL_VERSION      26
+#define ZM_ERR_WPA_GK_NOT_INSTALLED         27
+#define ZM_ERR_STA_NOT_ASSOCIATED           28
+#define ZM_ERR_DATA_BEFORE_CONNECTED        29
+#define ZM_ERR_DATA_NOT_ENCRYPTED           30
+#define ZM_ERR_DATA_BSSID_NOT_MATCHED       31
+#define ZM_ERR_RX_BAR_FRAME                 32
+#define ZM_ERR_OUT_OF_ORDER_NULL_DATA       33
+
+/* ZFI */
+#define ZM_ERR_INVALID_TX_RATE              40
+#define ZM_ERR_WDS_PORT_ID                  41
+
+/* QUEUE */
+#define ZM_ERR_QUEUE_FULL                   50
+#define ZM_ERR_STA_UAPSD_QUEUE_FULL         51
+#define ZM_ERR_AP_UAPSD_QUEUE_FULL          52
+
+/* Maximum Rx frame length */
+#if ZM_LARGEPAYLOAD_TEST == 1
+#define ZM_WLAN_MAX_RX_SIZE                 16384
+#else
+#define ZM_WLAN_MAX_RX_SIZE                 8192
+#endif
+
+/* PCI DMA test error code */
+#define ZM_ERR_INTERRUPT_MISSED             100
+#define ZM_ERR_OWN_BIT_NOT_CLEARED          101
+#define ZM_ERR_RX_SEQ_NUMBER                102
+#define ZM_ERR_RX_LENGTH                    103
+#define ZM_ERR_RX_DATA                      104
+#define ZM_ERR_RX_DESCRIPTOR_NUM            105
+/* Common register test error code */
+#define ZM_ERR_REGISTER_ACCESS              110 /* Register R/W test fail*/
+#define ZM_ERR_CLEAR_INTERRUPT_FLAG         111
+#define ZM_ERR_COMMAND_RESPONSE             112
+#define ZM_ERR_INTERRUPT_GENERATE           113
+#define ZM_ERR_INTERRUPT_ACK                114
+#define ZM_ERR_SCRATCH_ACCESS               115
+#define ZM_ERR_INTERRUPT_MASK_ACCESS        116
+#define ZM_ERR_SHARE_MEMORY_PCI_ACCESS      117
+#define ZM_ERR_SHARE_MEMORY_FW_ACCESS       118
+#define ZM_ERR_SHARE_MEMORY_DISABLE         119
+#define ZM_ERR_SHARE_MEMORY_TEST_RESPONSE   120
+
+/* Firmware Download error code */
+#define ZM_ERR_FIRMWARE_DOWNLOAD_TIMEOUT    150
+#define ZM_ERR_FIRMWARE_DOWNLOAD_INT_FLAG   151
+#define ZM_ERR_FIRMWARE_READY_TIMEOUT       152
+#define ZM_ERR_FIRMWARE_WRONG_TYPE          153
+
+/* Debug */
+#define ZM_LV_0     0//Debug level 0, Disable debug message
+#define ZM_LV_1     1//Debug level 1, Show minimum information
+#define ZM_LV_2     2//Debug level 2, Show medium message
+#define ZM_LV_3     3//Debug level 3, Show all
+
+#define ZM_SCANMSG_LEV  ZM_LV_1
+#define ZM_TXMSG_LEV    ZM_LV_0//ZM_LV_0
+#define ZM_RXMSG_LEV    ZM_LV_0
+#define ZM_MMMSG_LEV    ZM_LV_0
+#define ZM_DESMSG_LEV   ZM_LV_0//ZM_LV_0
+#define ZM_BUFMSG_LEV   ZM_LV_0//ZM_LV_1
+#define ZM_INITMSG_LEV  ZM_LV_0
+
+#define zm_msg0_scan(lv, msg) if (ZM_SCANMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_scan(lv, msg, val) if (ZM_SCANMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_scan(lv, msg, val) if (ZM_SCANMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_tx(lv, msg) if (ZM_TXMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_tx(lv, msg, val) if (ZM_TXMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_tx(lv, msg, val) if (ZM_TXMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_rx(lv, msg) if (ZM_RXMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_rx(lv, msg, val) if (ZM_RXMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_rx(lv, msg, val) if (ZM_RXMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_mm(lv, msg) if (ZM_MMMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_mm(lv, msg, val) if (ZM_MMMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_mm(lv, msg, val) if (ZM_MMMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_des(lv, msg) if (ZM_DESMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_des(lv, msg, val) if (ZM_DESMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_des(lv, msg, val) if (ZM_DESMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_buf(lv, msg) if (ZM_BUFMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_buf(lv, msg, val) if (ZM_BUFMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_buf(lv, msg, val) if (ZM_BUFMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_init(lv, msg) if (ZM_INITMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_init(lv, msg, val) if (ZM_INITMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_init(lv, msg, val) if (ZM_INITMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define ZM_MAX_AP_SUPPORT                   2  /* Must <= 8 */
+#define ZM_MAX_WDS_SUPPORT                  6  /* Must <= 6 */
+#define ZM_MAX_STA_SUPPORT                  16 /* Must <= 64 */
+
+/* STA table state */
+#define ZM_STATE_AUTH                       1
+#define ZM_STATE_PREAUTH                    2
+#define ZM_STATE_ASOC                       3
+
+/* Rate set */
+#define ZM_RATE_SET_CCK                     0
+#define ZM_RATE_SET_OFDM                    1
+
+/* HT PT */
+#define ZM_PREAMBLE_TYPE_MIXED_MODE         0
+#define ZM_PREAMBLE_TYPE_GREEN_FIELD        1
+
+/* HT bandwidth */
+#define ZM_BANDWIDTH_20MHZ                  0
+#define ZM_BANDWIDTH_40MHZ                  1
+
+/* MIC status */
+#define ZM_MIC_SUCCESS                      0
+#define ZM_MIC_FAILURE                      1
+
+/* ICV status */
+#define ZM_ICV_SUCCESS                      0
+#define ZM_ICV_FAILURE                      1
+
+/* definition check */
+#if (ZM_MAX_AP_SUPPORT > 8)
+definition error, ZM_MAX_AP_SUPPORT > 8
+#endif
+#if (ZM_MAX_AP_SUPPORT > 64)
+definition error, ZM_MAX_STA_SUPPORT > 64
+#endif
+
+/*  Transmission Rate information */
+
+/* WLAN frame format */
+#define ZM_PLCP_HEADER_SIZE          5
+#define ZM_ETHERNET_ADDRESS_LENGTH   6
+#define ZM_TIMESTAMP_OFFSET          0
+#define ZM_BEACON_INTERVAL_OFFSET    8
+#define ZM_CAPABILITY_OFFSET        10
+
+/* Reason Code */
+/* An unsolicited notification management frame of       */
+/* type Disassocation or Deauthentication was generated. */
+#ifdef ZM_REASON_CODE
+#define ZM_WLAN_REASON_CODE_UNSPECIFIED   1
+#define ZM_WLAN_FRAME_DISASOC_DEAUTH_REASON_CODE  24
+#endif
+
+struct zsWlanManagementFrameHeader
+{
+    //u8_t      plcpHdr[ZM_PLCP_HEADER_SIZE];
+    u8_t        frameCtrl[2];
+    u8_t        duration[2];
+    u8_t        da[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        sa[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        bssid[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        seqCtrl[2];
+    u8_t        body[1];
+};
+
+struct zsWlanProbeRspFrameHeader
+{
+    //u8_t      plcpHdr[ZM_PLCP_HEADER_SIZE];
+    u8_t        frameCtrl[2];
+    u8_t        duration[2];
+    u8_t        da[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        sa[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        bssid[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        seqCtrl[2];
+    u8_t        timeStamp[8];
+    u8_t        beaconInterval[2];
+    u8_t        capability[2];
+    u8_t        ssid[ZM_MAX_SSID_LENGTH + 2];   // EID(1) + Length(1) + SSID(32)
+} ;
+
+#define zsWlanBeaconFrameHeader zsWlanProbeRspFrameHeader
+
+struct zsWlanAuthFrameHeader
+{
+    //u8_t      plcpHdr[ZM_PLCP_HEADER_SIZE];
+    u8_t        frameCtrl[2];
+    u8_t        duration[2];
+    u8_t        address1[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        address2[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        address3[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        seqCtrl[2];
+    u16_t       algo;
+    u16_t       seq;
+    u16_t       status;
+    u8_t        challengeText[255]; // the first 2 bytes are information ID, length
+};
+
+struct zsWlanAssoFrameHeader
+{
+    //u8_t      plcpHdr[PLCP_HEADER_SIZE];
+    u8_t        frameCtrl[2];
+    u8_t        duration[2];
+    u8_t        address1[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        address2[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        address3[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        seqCtrl[2];
+    u8_t        capability[2];
+    u16_t       status;
+    u16_t       aid;
+    //u8_t        supportedRates[10];
+};
+
+struct zsFrag
+{
+    zbuf_t* buf[16];
+    u16_t bufType[16];
+    u16_t seq[16];
+    u8_t flag[16];
+
+};
+
+//================================
+// Hardware related definitions
+//================================
+#define ZM_MAC_REG_BASE                         0x1c3000
+
+#define ZM_MAC_REG_ATIM_WINDOW                  (ZM_MAC_REG_BASE + 0x51C)
+#define ZM_MAC_REG_BCN_PERIOD                   (ZM_MAC_REG_BASE + 0x520)
+#define ZM_MAC_REG_PRETBTT                      (ZM_MAC_REG_BASE + 0x524)
+
+#define ZM_MAC_REG_MAC_ADDR_L                   (ZM_MAC_REG_BASE + 0x610)
+#define ZM_MAC_REG_MAC_ADDR_H                   (ZM_MAC_REG_BASE + 0x614)
+
+#define ZM_MAC_REG_GROUP_HASH_TBL_L             (ZM_MAC_REG_BASE + 0x624)
+#define ZM_MAC_REG_GROUP_HASH_TBL_H             (ZM_MAC_REG_BASE + 0x628)
+
+#define ZM_MAC_REG_BASIC_RATE                   (ZM_MAC_REG_BASE + 0x630)
+#define ZM_MAC_REG_MANDATORY_RATE               (ZM_MAC_REG_BASE + 0x634)
+#define ZM_MAC_REG_RTS_CTS_RATE                 (ZM_MAC_REG_BASE + 0x638)
+#define ZM_MAC_REG_BACKOFF_PROTECT              (ZM_MAC_REG_BASE + 0x63c)
+#define ZM_MAC_REG_RX_THRESHOLD                 (ZM_MAC_REG_BASE + 0x640)
+#define ZM_MAC_REG_RX_PE_DELAY                  (ZM_MAC_REG_BASE + 0x64C)
+
+#define ZM_MAC_REG_DYNAMIC_SIFS_ACK             (ZM_MAC_REG_BASE + 0x658)
+#define ZM_MAC_REG_SNIFFER                      (ZM_MAC_REG_BASE + 0x674)
+#define ZM_MAC_REG_TX_UNDERRUN		            (ZM_MAC_REG_BASE + 0x688)
+#define ZM_MAC_REG_RX_TOTAL			            (ZM_MAC_REG_BASE + 0x6A0)
+#define ZM_MAC_REG_RX_CRC32			            (ZM_MAC_REG_BASE + 0x6A4)
+#define ZM_MAC_REG_RX_CRC16			            (ZM_MAC_REG_BASE + 0x6A8)
+#define ZM_MAC_REG_RX_ERR_UNI		            (ZM_MAC_REG_BASE + 0x6AC)
+#define ZM_MAC_REG_RX_OVERRUN		            (ZM_MAC_REG_BASE + 0x6B0)
+#define ZM_MAC_REG_RX_ERR_MUL		            (ZM_MAC_REG_BASE + 0x6BC)
+#define ZM_MAC_REG_TX_RETRY			            (ZM_MAC_REG_BASE + 0x6CC)
+#define ZM_MAC_REG_TX_TOTAL			            (ZM_MAC_REG_BASE + 0x6F4)
+
+
+#define ZM_MAC_REG_ACK_EXTENSION                (ZM_MAC_REG_BASE + 0x690)
+#define ZM_MAC_REG_EIFS_AND_SIFS                (ZM_MAC_REG_BASE + 0x698)
+
+#define ZM_MAC_REG_SLOT_TIME                    (ZM_MAC_REG_BASE + 0x6F0)
+
+#define ZM_MAC_REG_ROLL_CALL_TBL_L              (ZM_MAC_REG_BASE + 0x704)
+#define ZM_MAC_REG_ROLL_CALL_TBL_H              (ZM_MAC_REG_BASE + 0x708)
+
+#define ZM_MAC_REG_AC0_CW                       (ZM_MAC_REG_BASE + 0xB00)
+#define ZM_MAC_REG_AC1_CW                       (ZM_MAC_REG_BASE + 0xB04)
+#define ZM_MAC_REG_AC2_CW                       (ZM_MAC_REG_BASE + 0xB08)
+#define ZM_MAC_REG_AC3_CW                       (ZM_MAC_REG_BASE + 0xB0C)
+#define ZM_MAC_REG_AC4_CW                       (ZM_MAC_REG_BASE + 0xB10)
+#define ZM_MAC_REG_AC1_AC0_AIFS                 (ZM_MAC_REG_BASE + 0xB14)
+#define ZM_MAC_REG_AC3_AC2_AIFS                 (ZM_MAC_REG_BASE + 0xB18)
+
+#define ZM_MAC_REG_RETRY_MAX                    (ZM_MAC_REG_BASE + 0xB28)
+
+#define ZM_MAC_REG_TXOP_NOT_ENOUGH_INDICATION   (ZM_MAC_REG_BASE + 0xB30)
+
+#define ZM_MAC_REG_AC1_AC0_TXOP                 (ZM_MAC_REG_BASE + 0xB44)
+#define ZM_MAC_REG_AC3_AC2_TXOP                 (ZM_MAC_REG_BASE + 0xB48)
+
+#define ZM_MAC_REG_ACK_TABLE                    (ZM_MAC_REG_BASE + 0xC00)
+
+#define ZM_MAC_REG_BCN_ADDR                     (ZM_MAC_REG_BASE + 0xD84)
+#define ZM_MAC_REG_BCN_LENGTH                   (ZM_MAC_REG_BASE + 0xD88)
+
+#define ZM_MAC_REG_BCN_PLCP                     (ZM_MAC_REG_BASE + 0xD90)
+#define ZM_MAC_REG_BCN_CTRL                     (ZM_MAC_REG_BASE + 0xD94)
+
+#define ZM_MAC_REG_BCN_HT1                      (ZM_MAC_REG_BASE + 0xDA0)
+#define ZM_MAC_REG_BCN_HT2                      (ZM_MAC_REG_BASE + 0xDA4)
+
+
+#define ZM_RX_STATUS_IS_MIC_FAIL(rxStatus) rxStatus->Tail.Data.ErrorIndication & ZM_BIT_6
+
+//================================
+//================================
+
+#ifdef ZM_ENABLE_NATIVE_WIFI
+#define ZM_80211_FRAME_HEADER_LEN           24
+#define ZM_80211_FRAME_TYPE_OFFSET          30    // ZM_80211_FRAME_HEADER_LEN + SNAP
+#define ZM_80211_FRAME_IP_OFFSET            32    // ZM_80211_FRAME_HEADER_LEN + SNAP + TYPE
+#else
+#define ZM_80211_FRAME_HEADER_LEN           14
+#define ZM_80211_FRAME_TYPE_OFFSET          12    // ZM_80211_FRAME_HEADER_LEN + SNAP
+#define ZM_80211_FRAME_IP_OFFSET            14    // ZM_80211_FRAME_HEADER_LEN + SNAP + TYPE
+#endif
+
+#define ZM_BSS_INFO_VALID_BIT      0x01
+#define ZM_BSS_INFO_UPDATED_BIT    0x02
+
+
+
+
+
+#define ZM_ERROR_INDICATION_RX_TIMEOUT      0x01
+#define ZM_ERROR_INDICATION_OVERRUN         0x02
+#define ZM_ERROR_INDICATION_DECRYPT_ERROR   0x04
+#define ZM_ERROR_INDICATION_CRC32_ERROR     0x08
+#define ZM_ERROR_INDICATION_ADDR_NOT_MATCH  0x10
+#define ZM_ERROR_INDICATION_CRC16_ERROR     0x20
+#define ZM_ERROR_INDICATION_MIC_ERROR       0x40
+
+#define ZM_RXMAC_STATUS_MOD_TYPE_CCK        0x00
+#define ZM_RXMAC_STATUS_MOD_TYPE_OFDM       0x01
+#define ZM_RXMAC_STATUS_MOD_TYPE_HT_OFDM    0x02
+#define ZM_RXMAC_STATUS_MOD_TYPE_DL_OFDM    0x03
+#define ZM_RXMAC_STATUS_TOTAL_ERROR         0x80
+
+
+
+
+
+#define ZM_MAX_LED_NUMBER       2
+
+#define ZM_LED_DISABLE_MODE     0x0
+#define ZM_LED_LINK_MODE        0x1
+#define ZM_LED_LINK_TR_MODE     0x2
+#define ZM_LED_TR_ON_MODE       0x3
+#define ZM_LED_TR_OFF_MODE      0x4
+
+#define ZM_LED_CTRL_FLAG_ALPHA      0x1
+
+struct zsLedStruct
+{
+    u32_t   counter;
+    u32_t   counter100ms;
+    u16_t   ledLinkState;
+    u16_t   ledMode[ZM_MAX_LED_NUMBER];
+    u32_t   txTraffic;
+    u32_t   rxTraffic;
+    u8_t    LEDCtrlType;
+    u8_t    LEDCtrlFlag;         // Control Flag for vendors
+    u8_t    LEDCtrlFlagFromReg;  // Control Flag for vendors in registry
+};
+
+
+//HAL+ capability bits definition
+#define ZM_HP_CAP_11N                   0x1
+#define ZM_HP_CAP_11N_ONE_TX_STREAM     0x2
+#define ZM_HP_CAP_2G                    0x4
+#define ZM_HP_CAP_5G                    0x8
+
+#endif /* #ifndef _WLAN_H */
diff --git a/drivers/staging/otus/Kconfig b/drivers/staging/otus/Kconfig
new file mode 100644
index 000000000000..d549d08fd495
--- /dev/null
+++ b/drivers/staging/otus/Kconfig
@@ -0,0 +1,32 @@
+config OTUS
+	tristate "Atheros OTUS 802.11n USB wireless support"
+	depends on USB && WLAN_80211 && MAC80211
+	default N
+	---help---
+	  Enable support for Atheros 802.11n USB hardware:
+	    * UB81 - 2x2 2.4 GHz
+	    * UB82 - 2x2 2.4 GHz and 5 GHz
+	    * UB83 - 1x2 2.4 GHz
+
+	  This includes the following devices currently on the market:
+	  Dlink DWA-160A1, Netgear WNDA3100 and WN111v2, TP-Link
+	  TL-WN821N, and AVM FRITZ!WLAN N USB Stick.
+
+	  This driver requires its own supplicant driver for
+	  wpa_supplicant 0.4.8. For your convenience you can find the
+	  tarball here:
+
+	  http://www.kernel.org/pub/linux/kernel/people/mcgrof/otus/wpa_supplicant-0.4.8_otus.tar.bz2
+
+	  Before compiling wpa_supplicant, ensure your .config has at
+	  least the following:
+		CONFIG_WIRELESS_EXTENSION=y
+		CONFIG_EAP_WSC=y
+		CONFIG_WSC_IE=y
+		CONFIG_DRIVER_WEXT=y
+		CONFIG_DRIVER_OTUS=y
+
+	  After a successful compile, you can use the Atheros device as
+	  shown in the example:
+	  $ wpa_supplicant -Dotus -i <atheros device from ifconfig> -c /path/to/wpa_supplicant.conf -d
+
diff --git a/drivers/staging/otus/Makefile b/drivers/staging/otus/Makefile
new file mode 100644
index 000000000000..debcc5cbf876
--- /dev/null
+++ b/drivers/staging/otus/Makefile
@@ -0,0 +1,67 @@
+obj-$(CONFIG_OTUS)	+= arusb_lnx.o
+
+EXTRA_CFLAGS += -DAMAC
+EXTRA_CFLAGS += -DGCCK
+EXTRA_CFLAGS += -DOFDM
+EXTRA_CFLAGS += -DTXQ_IN_ISR
+EXTRA_CFLAGS += -DWLAN_HOSTIF=0 #0:USB, 1:PCI
+
+#Test Mode
+EXTRA_CFLAGS += -DZM_USB_STREAM_MODE=1
+EXTRA_CFLAGS += -DZM_USB_TX_STREAM_MODE=0
+EXTRA_CFLAGS += -DZM_PCI_DMA_TEST=0
+EXTRA_CFLAGS += -DZM_LARGEPAYLOAD_TEST=0
+EXTRA_CFLAGS += -DZM_FW_LOOP_BACK=0
+EXTRA_CFLAGS += -DZM_LINUX_TPC=1
+#EXTRA_CFLAGS += -DZM_DONT_COPY_RX_BUFFER
+
+EXTRA_CFLAGS += -DZM_HOSTAPD_SUPPORT
+#EXTRA_CFLAGS += -DfTX_GAIN_OFDM=0
+#EXTRA_CFLAGS += -DZM_CONFIG_BIG_ENDIAN -DBIG_ENDIAN
+EXTRA_CFLAGS += -DZM_HALPLUS_LOCK
+EXTRA_CFLAGS += -DZM_OTUS_LINUX_PHASE_2
+
+arusb_lnx-objs := \
+	usbdrv.o \
+	zdusb.o \
+	ioctl.o \
+	wrap_buf.o \
+	wrap_mem.o \
+	wrap_ev.o \
+	wrap_usb.o \
+	wrap_pkt.o \
+	wrap_dbg.o \
+	wrap_mis.o \
+	wrap_sec.o \
+	wwrap.o \
+	80211core/ccmd.o \
+	80211core/chb.o \
+	80211core/cinit.o \
+	80211core/cmm.o \
+	80211core/cmmap.o \
+	80211core/cmmsta.o \
+	80211core/cfunc.o \
+	80211core/coid.o \
+	80211core/ctkip.o \
+	80211core/ctxrx.o \
+	80211core/cic.o \
+	80211core/cpsmgr.o \
+	80211core/cscanmgr.o \
+	80211core/ratectrl.o \
+	80211core/ledmgr.o \
+	80211core/amsdu.o \
+	80211core/cwm.o \
+	80211core/cagg.o \
+	80211core/queue.o \
+	80211core/freqctrl.o \
+	80211core/cwep.o \
+	hal/hprw.o \
+	hal/hpmain.o \
+	hal/hpusb.o \
+	hal/hpreg.o \
+	hal/hpfwuinit.o \
+	hal/hpfwbu.o \
+	hal/hpfw2.o \
+	hal/hpDKfwu.o \
+	hal/hpfwspiu.o \
+	hal/hpani.o
diff --git a/drivers/staging/otus/TODO b/drivers/staging/otus/TODO
new file mode 100644
index 000000000000..e912293ed1ed
--- /dev/null
+++ b/drivers/staging/otus/TODO
@@ -0,0 +1,9 @@
+TODO:
+	- checkpatch.pl cleanups
+	- sparse cleanups
+	- port to in-kernel 80211 stack
+	- proper network developer maintainer
+
+Please send any patches to Greg Kroah-Hartman <greg@kroah.com> and
+Luis Rodriguez <Luis.Rodriguez@Atheros.com> and the
+otus-devel@lists.madwifi-project.org mailing list.
diff --git a/drivers/staging/otus/apdbg.c b/drivers/staging/otus/apdbg.c
new file mode 100644
index 000000000000..d3e2f6224196
--- /dev/null
+++ b/drivers/staging/otus/apdbg.c
@@ -0,0 +1,457 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : apdbg.c                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      Debug tools                                                     */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <errno.h>
+#include <ctype.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <net/if.h>
+#include <netinet/in.h>
+
+#include <linux/sockios.h>
+
+#define ZM_IOCTL_REG_READ           0x01
+#define ZM_IOCTL_REG_WRITE          0x02
+#define ZM_IOCTL_MEM_DUMP           0x03
+#define ZM_IOCTL_REG_DUMP           0x05
+#define ZM_IOCTL_TXD_DUMP           0x06
+#define ZM_IOCTL_RXD_DUMP           0x07
+#define ZM_IOCTL_MEM_READ           0x0B
+#define ZM_IOCTL_MEM_WRITE          0x0C
+#define ZM_IOCTL_DMA_TEST           0x10
+#define ZM_IOCTL_REG_TEST           0x11
+#define ZM_IOCTL_TEST               0x80
+#define ZM_IOCTL_TALLY              0x81 //CWYang(+)
+#define ZM_IOCTL_RTS                0xA0
+#define ZM_IOCTL_MIX_MODE           0xA1
+#define ZM_IOCTL_FRAG               0xA2
+#define ZM_IOCTL_SCAN               0xA3
+#define ZM_IOCTL_KEY                0xA4
+#define ZM_IOCTL_RATE               0xA5
+#define ZM_IOCTL_ENCRYPTION_MODE    0xA6
+#define ZM_IOCTL_GET_TXCNT          0xA7
+#define ZM_IOCTL_GET_DEAGG_CNT      0xA8
+#define ZM_IOCTL_DURATION_MODE      0xA9
+#define ZM_IOCTL_SET_AES_KEY        0xAA
+#define ZM_IOCTL_SET_AES_MODE       0xAB
+#define ZM_IOCTL_SIGNAL_STRENGTH    0xAC //CWYang(+)
+#define ZM_IOCTL_SIGNAL_QUALITY     0xAD //CWYang(+)
+#define ZM_IOCTL_SET_PIBSS_MODE     0xAE
+#define	ZDAPIOCTL                   SIOCDEVPRIVATE
+
+struct zdap_ioctl {
+	unsigned short cmd;                /* Command to run */
+	unsigned int   addr;                /* Length of the data buffer */
+	unsigned int   value;               /* Pointer to the data buffer */
+	unsigned char data[0x100];
+};
+
+/* Declaration of macro and function for handling WEP Keys */
+
+#if 0
+
+#define SKIP_ELEM { \
+    while(isxdigit(*p)) \
+        p++; \
+}
+
+#define SKIP_DELIMETER { \
+    if(*p == ':' || *p == ' ') \
+        p++; \
+}
+
+#endif
+
+char hex(char);
+unsigned char asctohex(char *str);
+
+char *prgname;
+
+int set_ioctl(int sock, struct ifreq *req)
+{
+    if (ioctl(sock, ZDAPIOCTL, req) < 0) {
+        fprintf(stderr, "%s: ioctl(SIOCGIFMAP): %s\n",
+                prgname, strerror(errno));
+        return -1;
+    }
+
+    return 0;
+}
+
+
+int read_reg(int sock, struct ifreq *req)
+{
+    struct zdap_ioctl *zdreq = 0;
+
+    if (!set_ioctl(sock, req))
+        return -1;
+
+    //zdreq = (struct zdap_ioctl *)req->ifr_data;
+    //printf( "reg = %4x, value = %4x\n", zdreq->addr, zdreq->value);
+
+    return 0;
+}
+
+
+int read_mem(int sock, struct ifreq *req)
+{
+    struct zdap_ioctl *zdreq = 0;
+    int i;
+
+    if (!set_ioctl(sock, req))
+        return -1;
+
+    /*zdreq = (struct zdap_ioctl *)req->ifr_data;
+    printf( "dump mem from %x, length = %x\n", zdreq->addr, zdreq->value);
+
+    for (i=0; i<zdreq->value; i++) {
+        printf("%02x", zdreq->data[i]);
+        printf(" ");
+
+        if ((i>0) && ((i+1)%16 == 0))
+            printf("\n");
+    }*/
+
+    return 0;
+}
+
+
+int main(int argc, char **argv)
+{
+    int sock;
+    int addr, value;
+    struct ifreq req;
+    char *action = NULL;
+    struct zdap_ioctl zdreq;
+
+    prgname = argv[0];
+
+    if (argc < 3) {
+        fprintf(stderr,"%s: usage is \"%s <ifname> <operation> [<address>] [<value>]\"\n",
+                prgname, prgname);
+        fprintf(stderr,"valid operation: read, write, mem, reg,\n");
+        fprintf(stderr,"               : txd, rxd, rmem, wmem\n");
+        fprintf(stderr,"               : dmat, regt, test\n");
+
+        fprintf(stderr,"    scan, Channel Scan\n");
+        fprintf(stderr,"    rts <decimal>, Set RTS Threshold\n");
+        fprintf(stderr,"    frag <decimal>, Set Fragment Threshold\n");
+        fprintf(stderr,"    rate <0-28>, 0:AUTO, 1-4:CCK, 5-12:OFDM, 13-28:HT\n");
+        fprintf(stderr,"    TBD mix <0 or 1>, Set 1 to enable mixed mode\n");
+        fprintf(stderr,"    enc, <0-3>, 0=>OPEN, 1=>WEP64, 2=>WEP128, 3=>WEP256\n");
+        fprintf(stderr,"    skey <key>, Set WEP key\n");
+        fprintf(stderr,"    txcnt, Get TxQ Cnt\n");
+        fprintf(stderr,"    dagcnt, Get Deaggregate Cnt\n");
+        fprintf(stderr,"    durmode <mode>, Set Duration Mode 0=>HW, 1=>SW\n");
+        fprintf(stderr,"    aeskey <user> <key>\n");
+        fprintf(stderr,"    aesmode <mode>\n");
+        fprintf(stderr,"    wlanmode <0,1> 0:Station mode, 1:PIBSS mode\n");
+        fprintf(stderr,"    tal <0,1>, Get Current Tally Info, 0=>read, 1=>read and reset\n");
+
+        exit(1);
+    }
+
+    strcpy(req.ifr_name, argv[1]);
+    zdreq.addr = 0;
+    zdreq.value = 0;
+
+    /* a silly raw socket just for ioctl()ling it */
+    sock = socket(AF_INET, SOCK_RAW, IPPROTO_RAW);
+    if (sock < 0) {
+        fprintf(stderr, "%s: socket(): %s\n", argv[0], strerror(errno));
+        exit(1);
+    }
+
+    if (argc >= 4)
+    {
+        sscanf(argv[3], "%x", &addr);
+    }
+
+    if (argc >= 5)
+    {
+        sscanf(argv[4], "%x", &value);
+    }
+
+    zdreq.addr = addr;
+    zdreq.value = value;
+
+    if (!strcmp(argv[2], "read"))
+    {
+        zdreq.cmd = ZM_IOCTL_REG_READ;
+    }
+    else if (!strcmp(argv[2], "mem"))
+    {
+        zdreq.cmd = ZM_IOCTL_MEM_DUMP;
+    }
+    else if (!strcmp(argv[2], "write"))
+    {
+        zdreq.cmd = ZM_IOCTL_REG_WRITE;
+    }
+    else if (!strcmp(argv[2], "reg"))
+    {
+        zdreq.cmd = ZM_IOCTL_REG_DUMP;
+    }
+    else if (!strcmp(argv[2], "txd"))
+    {
+        zdreq.cmd = ZM_IOCTL_TXD_DUMP;
+    }
+    else if (!strcmp(argv[2], "rxd"))
+    {
+        zdreq.cmd = ZM_IOCTL_RXD_DUMP;
+    }
+    else if (!strcmp(argv[2], "rmem"))
+    {
+        zdreq.cmd = ZM_IOCTL_MEM_READ;
+    }
+    else if (!strcmp(argv[2], "wmem"))
+    {
+        zdreq.cmd = ZM_IOCTL_MEM_WRITE;
+    }
+    else if (!strcmp(argv[2], "dmat"))
+    {
+        zdreq.cmd = ZM_IOCTL_DMA_TEST;
+    }
+    else if (!strcmp(argv[2], "regt"))
+    {
+        zdreq.cmd = ZM_IOCTL_REG_TEST;
+    }
+    else if (!strcmp(argv[2], "test"))
+    {
+        zdreq.cmd = ZM_IOCTL_TEST;
+    }
+    else if (!strcmp(argv[2], "tal"))
+    {
+        sscanf(argv[3], "%d", &addr);
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_TALLY;
+    }
+    else if (!strcmp(argv[2], "rts"))
+    {
+        sscanf(argv[3], "%d", &addr);
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_RTS;
+    }
+    else if (!strcmp(argv[2], "mix"))
+    {
+        zdreq.cmd = ZM_IOCTL_MIX_MODE;
+    }
+    else if (!strcmp(argv[2], "frag"))
+    {
+        sscanf(argv[3], "%d", &addr);
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_FRAG;
+    }
+    else if (!strcmp(argv[2], "scan"))
+    {
+        zdreq.cmd = ZM_IOCTL_SCAN;
+    }
+    else if (!strcmp(argv[2], "skey"))
+    {
+        zdreq.cmd = ZM_IOCTL_KEY;
+
+        if (argc >= 4)
+        {
+            unsigned char temp[29];
+            int i;
+            int keyLen;
+            int encType;
+
+            keyLen = strlen(argv[3]);
+
+            if (keyLen == 10)
+            {
+                sscanf(argv[3], "%02x%02x%02x%02x%02x", &temp[0], &temp[1],
+                        &temp[2], &temp[3], &temp[4]);
+            }
+            else if (keyLen == 26)
+            {
+                sscanf(argv[3], "%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x",
+                        &temp[0], &temp[1], &temp[2], &temp[3], &temp[4],
+                        &temp[5], &temp[6], &temp[7], &temp[8], &temp[9],
+                         &temp[10], &temp[11], &temp[12]);
+            }
+            else if (keyLen == 58)
+            {
+                sscanf(argv[3], "%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x",
+                        &temp[0], &temp[1], &temp[2], &temp[3], &temp[4],
+                        &temp[5], &temp[6], &temp[7], &temp[8], &temp[9],
+                        &temp[10], &temp[11], &temp[12], &temp[13], &temp[14],
+                        &temp[15], &temp[16], &temp[17], &temp[18], &temp[19],
+                        &temp[20], &temp[21], &temp[22], &temp[23], &temp[24],
+                        &temp[25], &temp[26], &temp[27], &temp[28]);
+            }
+            else
+            {
+                fprintf(stderr, "Invalid key length\n");
+                exit(1);
+            }
+            zdreq.addr = keyLen/2;
+
+            for(i=0; i<zdreq.addr; i++)
+            {
+                zdreq.data[i] = temp[i];
+            }
+        }
+        else
+        {
+            printf("Error : Key required!\n");
+        }
+    }
+    else if (!strcmp(argv[2], "rate"))
+    {
+        sscanf(argv[3], "%d", &addr);
+
+        if (addr > 28)
+        {
+            fprintf(stderr, "Invalid rate, range:0~28\n");
+            exit(1);
+        }
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_RATE;
+    }
+    else if (!strcmp(argv[2], "enc"))
+    {
+        sscanf(argv[3], "%d", &addr);
+
+        if (addr > 3)
+        {
+            fprintf(stderr, "Invalid encryption mode, range:0~3\n");
+            exit(1);
+        }
+
+        if (addr == 2)
+        {
+            addr = 5;
+        }
+        else if (addr == 3)
+        {
+            addr = 6;
+        }
+
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_ENCRYPTION_MODE;
+    }
+    else if (!strcmp(argv[2], "txcnt"))
+    {
+        zdreq.cmd = ZM_IOCTL_GET_TXCNT;
+    }
+    else if (!strcmp(argv[2], "dagcnt"))
+    {
+        sscanf(argv[3], "%d", &addr);
+
+        if (addr != 0 && addr != 1)
+        {
+            fprintf(stderr, "The value should be 0 or 1\n");
+            exit(0);
+        }
+
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_GET_DEAGG_CNT;
+    }
+    else if (!strcmp(argv[2], "durmode"))
+    {
+        sscanf(argv[3], "%d", &addr);
+
+        if (addr != 0 && addr != 1)
+        {
+            fprintf(stderr, "The Duration mode should be 0 or 1\n");
+            exit(0);
+        }
+
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_DURATION_MODE;
+    }
+    else if (!strcmp(argv[2], "aeskey"))
+    {
+        unsigned char temp[16];
+        int i;
+
+        sscanf(argv[3], "%d", &addr);
+
+        sscanf(argv[4], "%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x", &temp[0], &temp[1], &temp[2], &temp[3], &temp[4], &temp[5], &temp[6], &temp[7], &temp[8], &temp[9], &temp[10], &temp[11], &temp[12], &temp[13], &temp[14], &temp[15]);
+
+        for(i = 0; i < 16; i++)
+        {
+            zdreq.data[i] = temp[i];
+        }
+
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_SET_AES_KEY;
+    }
+    else if (!strcmp(argv[2], "aesmode"))
+    {
+        sscanf(argv[3], "%d", &addr);
+
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_SET_AES_MODE;
+    }
+    else if (!strcmp(argv[2], "wlanmode"))
+    {
+        sscanf(argv[3], "%d", &addr);
+
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_SET_PIBSS_MODE;
+    }
+    else
+    {
+	    fprintf(stderr, "error action\n");
+        exit(1);
+    }
+
+    req.ifr_data = (char *)&zdreq;
+    set_ioctl(sock, &req);
+
+fail:
+    exit(0);
+}
+
+unsigned char asctohex(char *str)
+{
+    unsigned char value;
+
+    value = hex(*str) & 0x0f;
+    value = value << 4;
+    str++;
+    value |= hex(*str) & 0x0f;
+
+    return value;
+}
+
+char hex(char v)
+{
+    if(isdigit(v))
+        return v - '0';
+    else if(isxdigit(v))
+        return (tolower(v) - 'a' + 10);
+    else
+        return 0;
+}
+
diff --git a/drivers/staging/otus/athr_common.h b/drivers/staging/otus/athr_common.h
new file mode 100644
index 000000000000..620f78a41d5f
--- /dev/null
+++ b/drivers/staging/otus/athr_common.h
@@ -0,0 +1,141 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : athr_common.h                                         */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      WPA related function and data structure definitions.            */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      Platform dependent.                                             */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _ATHR_COMMON_H
+#define _ATHR_COMMON_H
+
+#define ZD_IOCTL_WPA			(SIOCDEVPRIVATE + 1)
+#define ZD_IOCTL_PARAM			(SIOCDEVPRIVATE + 2)
+#define ZD_IOCTL_GETWPAIE		(SIOCDEVPRIVATE + 3)
+#define ZD_PARAM_ROAMING		0x0001
+#define ZD_PARAM_PRIVACY		0x0002
+#define ZD_PARAM_WPA			0x0003
+#define ZD_PARAM_COUNTERMEASURES	0x0004
+#define ZD_PARAM_DROPUNENCRYPTED	0x0005
+#define ZD_PARAM_AUTH_ALGS		0x0006
+
+#define ZD_CMD_SET_ENCRYPT_KEY		0x0001
+#define ZD_CMD_SET_MLME			0x0002
+#define ZD_CMD_SCAN_REQ			0x0003
+#define ZD_CMD_SET_GENERIC_ELEMENT	0x0004
+#define ZD_CMD_GET_TSC			0x0005
+
+#define ZD_FLAG_SET_TX_KEY              0x0001
+
+#define ZD_GENERIC_ELEMENT_HDR_LEN \
+((int) (&((struct athr_wlan_param *) 0)->u.generic_elem.data))
+
+#define ZD_CRYPT_ALG_NAME_LEN		16
+#define ZD_MAX_KEY_SIZE			32
+#define ZD_MAX_GENERIC_SIZE		64
+
+#define IEEE80211_ADDR_LEN		6
+#define IEEE80211_MAX_IE_SIZE		256
+
+#ifdef ZM_ENALBE_WAPI
+#define ZM_CMD_WAPI_SETWAPI             0x0001
+#define ZM_CMD_WAPI_GETWAPI             0x0002
+#define ZM_CMD_WAPI_SETKEY              0x0003
+#define ZM_CMD_WAPI_GETKEY              0x0004
+#define ZM_CMD_WAPI_REKEY               0x0005
+
+#define ZM_WAPI_WAI_REQUEST             0x00f1
+#define ZM_WAPI_UNICAST_REKEY           0x00f2
+#define ZM_WAPI_STA_AGING               0x00f3
+#define ZM_WAPI_MULTI_REKEY             0x00f4
+
+#define ZM_WAPI_KEY_SIZE                32
+#define ZM_WAPI_IV_LEN                  16
+#endif //ZM_ENALBE_WAPI
+/* structure definition */
+
+struct athr_wlan_param {
+	u32 cmd;
+	u8 sta_addr[ETH_ALEN];
+	union {
+		struct {
+			u8 alg[ZD_CRYPT_ALG_NAME_LEN];
+			u32 flags;
+			u32 err;
+			u8 idx;
+			u8 seq[8]; /* sequence counter (set: RX, get: TX) */
+			u16 key_len;
+			u8 key[ZD_MAX_KEY_SIZE];
+		} crypt;
+		struct {
+			u32 flags_and;
+			u32 flags_or;
+		} set_flags_sta;
+		struct {
+			u8 len;
+			u8 data[ZD_MAX_GENERIC_SIZE];
+		} generic_elem;
+		struct {
+#define MLME_STA_DEAUTH 0
+#define MLME_STA_DISASSOC 1
+			u16 cmd;
+			u16 reason_code;
+		} mlme;
+		struct {
+			u8 ssid_len;
+			u8 ssid[32];
+		} scan_req;
+	} u;
+};
+
+struct ieee80211req_wpaie {
+	u8 wpa_macaddr[IEEE80211_ADDR_LEN];
+	u8 wpa_ie[IEEE80211_MAX_IE_SIZE];
+};
+
+#ifdef ZM_ENALBE_WAPI
+struct athr_wapi_param {
+	u16 cmd;
+	u16 len;
+
+	union {
+		struct {
+			u8 sta_addr[ETH_ALEN];
+			u8 reserved;
+			u8 keyid;
+			u8 key[ZM_WAPI_KEY_SIZE];
+		} crypt;
+		struct {
+                        u8 wapi_policy;
+		} info;
+	} u;
+};
+
+struct athr_wapi_sta_info
+{
+	u16	msg_type;
+	u16	datalen;
+    	u8	sta_mac[ETH_ALEN];
+	u8	reserve_data[2];
+    	u8	gsn[ZM_WAPI_IV_LEN];
+	u8	wie[256];
+};
+#endif //ZM_ENALBE_WAPI
+#endif
diff --git a/drivers/staging/otus/hal/hpDKfwu.c b/drivers/staging/otus/hal/hpDKfwu.c
new file mode 100644
index 000000000000..144c62dc8da2
--- /dev/null
+++ b/drivers/staging/otus/hal/hpDKfwu.c
@@ -0,0 +1,832 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+
+const u32_t zcDKFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE3E7FFC, 0xE114D73E,
+0x1E13D43E, 0x1E4C470B, 0x0009B017, 0x956EE600,
+0xC84060E2, 0x2F028F03, 0x8FF93652, 0xD4387601,
+0x4E0BDE38, 0xD4380009, 0x00094E0B, 0x4E0BD437,
+0x7F040009, 0xA0364F26, 0x4F226EF6, 0x410BD134,
+0xD4340009, 0x0009440B, 0x450BD533, 0xD7330009,
+0xD233E1FF, 0x2712611D, 0xD4325029, 0xE1FFCB01,
+0x1209E501, 0x12112212, 0xD52F2452, 0xD22F9740,
+0xE7002572, 0xD42FD12E, 0x2270D62F, 0x2172E201,
+0x26202420, 0xE4FFD62D, 0xE6002641, 0xE104D52C,
+0x6063666D, 0x626D7601, 0x32124000, 0x05458FF8,
+0x000B4F26, 0xEAC80009, 0xDB266AAC, 0xDD27DC26,
+0xD828DE27, 0x4C0BE901, 0x4D0B0009, 0x4E0B0009,
+0x60B20009, 0x89078801, 0x6242D423, 0x890332A6,
+0x6050D522, 0x8BEE8801, 0x2B92D41F, 0x26686642,
+0x480B89E9, 0xD51D0009, 0xAFE4E200, 0x27102520,
+0x00000FA0, 0x001C001C, 0x00200ED4, 0x0000B38E,
+0x00202F90, 0x00201356, 0x00202F9C, 0x00202FB4,
+0x00201314, 0x00201412, 0x00200EF8, 0x001C3510,
+0x001C3624, 0x001E212C, 0x00202F00, 0x00202A9C,
+0x00202F08, 0x00202F14, 0x00202F20, 0x00202F22,
+0x00202F26, 0x001C1028, 0x00201220, 0x0020294C,
+0x00201D10, 0x00201EC8, 0x00203220, 0x00202F24,
+0x2FB62F96, 0x2FD62FC6, 0x4F222FE6, 0xDE947F80,
+0x61E0E024, 0x0F14D493, 0x710161E3, 0xD7926210,
+0x470BE028, 0xD5910F24, 0x0009450B, 0x6D032008,
+0x1F0B8F11, 0xD48FDC8E, 0xDD8F67C0, 0x657C4D0B,
+0xDD8FD18E, 0x6B9C6910, 0x420862B3, 0x32B84208,
+0x3D2C4208, 0xE0281FDB, 0xE58004FC, 0x604C66E2,
+0x3050655C, 0x2D628F13, 0x01FCE024, 0x641CE500,
+0x625DDE84, 0x8B013243, 0x0009A39E, 0x6753655D,
+0x607037EC, 0x39DC6953, 0xAFF27501, 0x20088094,
+0xE0248B13, 0xE50001FC, 0xA009DE7A, 0x655D641C,
+0x32EC6253, 0x6C536B22, 0x3CDC67B2, 0x75041C71,
+0x3243625D, 0xA37F8BF3, 0x88012D10, 0xE0248B16,
+0xE40001FC, 0x671C2D40, 0x624DDE6E, 0x8B013273,
+0x0009A372, 0x6CE3644D, 0x7C046943, 0x39EC6B43,
+0x65923BCC, 0x74086DB2, 0x25D2AFEF, 0x8B198804,
+0x01FCE024, 0x2D70E700, 0x1FD86D1C, 0x627DDE61,
+0x8B0132D3, 0x0009A358, 0x6B73677D, 0x3BEC61E3,
+0x710464B2, 0x3C1C6C73, 0x694265C2, 0x29597708,
+0x2492AFED, 0x8B188805, 0x01FCE024, 0x2D40E400,
+0xDE54671C, 0x3273624D, 0xA33D8B01, 0x644D0009,
+0x6BE36D43, 0x65D23DEC, 0x61437B04, 0x6C1231BC,
+0x74086952, 0xAFED29CB, 0x88312592, 0xDE4A8B20,
+0x65E6DB4A, 0x61E6DC4A, 0x67E2D94A, 0x62E27E04,
+0x1FEC7EE8, 0x7E0464E2, 0x6EE21FED, 0x5BFD2BE0,
+0x60B27B04, 0xC9011FBE, 0x6BB22C00, 0x29B04B09,
+0xDC412F26, 0x66134C0B, 0xE2007F04, 0x2D20A30C,
+0x8B218830, 0xD939DE38, 0xE06465E6, 0x720462E3,
+0x672666E2, 0x6E23DC36, 0x62227EE8, 0x6BE261E6,
+0x29B01FEF, 0x7E040F16, 0xC90160E2, 0x6EE22C00,
+0x4E09DC30, 0x2F262CE0, 0xD130E068, 0x04FE410B,
+0xE2007F04, 0x2D20A2E8, 0x8B058833, 0x4E0BDE2C,
+0xE1000009, 0x2D10A2E0, 0x89018828, 0x0009A106,
+0xE143DE20, 0xE04062E1, 0x3217622D, 0x0FE68F04,
+0x6023E240, 0x262106FE, 0x8B013217, 0x0009A0EF,
+0x02FEE040, 0x8521E401, 0x8B013046, 0x0009A0E7,
+0xE501E040, 0x2D5007FE, 0x6471B2C7, 0x09FEE040,
+0x6291E143, 0x652DE068, 0x8D6B3517, 0xE6400F56,
+0x8B273563, 0xE048E600, 0xE11A0F65, 0x72C0A031,
+0x00117800, 0x00202FB8, 0x00201356, 0x00202480,
+0x00202F1A, 0x00202FBC, 0x002013A2, 0x00202F19,
+0x00202B40, 0x00117804, 0x00117810, 0x00202F15,
+0x00202F16, 0x00202F17, 0x00200B84, 0x00200BD8,
+0x00200BD4, 0x41216153, 0x41214121, 0x41214121,
+0x45214521, 0x60534521, 0x6603C903, 0x0F65E048,
+0xE0077118, 0xE0442209, 0x641D0F25, 0x65F3E04C,
+0x0F46B314, 0x04FDE048, 0x0BFDE044, 0x61BD674D,
+0x41084708, 0x0F16E050, 0xD2936073, 0x420B09FE,
+0x6C07E00F, 0x607329C9, 0xE0400F96, 0x65F30EFE,
+0x6D0D85E2, 0x01FEE050, 0x60D3420B, 0x6073290B,
+0xE04C0F96, 0x04FEB2D9, 0x06FEE040, 0x6261E068,
+0x0F56652D, 0x3563E640, 0xE000894E, 0x602381F8,
+0x4008C903, 0x6B034000, 0xE0546103, 0xE0580FB6,
+0xECFFDD7D, 0x6CCC0FF6, 0x0FD6E06C, 0x4D0B60C3,
+0x42216253, 0x42214221, 0x64234221, 0x324C4200,
+0xE05C6E07, 0x45214200, 0xE0400FE6, 0x0BFE4521,
+0xC9036053, 0x30FC4008, 0x6D037B06, 0x85F81F05,
+0x6C2D1FB7, 0x1FC66E03, 0x0FC6E060, 0x05FEE058,
+0x64C3B2B4, 0x33FCE354, 0x563262D2, 0x22696132,
+0x67B42D22, 0x490B5936, 0x220B607C, 0x05FEE058,
+0x64C32D22, 0x7E01B289, 0xE70662ED, 0x8FE33273,
+0xE0407C01, 0x626106FE, 0x06FEE040, 0x85614200,
+0x302C760C, 0x6103701B, 0x64F3E500, 0x7501E704,
+0x6B5D6966, 0x24923B73, 0x74048FF9, 0xB26C65F3,
+0xE040641D, 0xB20506FE, 0xA1DD6461, 0xD44F0009,
+0xE201D74F, 0x2D20470B, 0x0009A1D6, 0x8B078829,
+0xEC00DE4C, 0x61E22DC0, 0x641DB1D7, 0x0009A1CC,
+0x622CE281, 0x8B013020, 0x0009A118, 0x06FCE028,
+0xE682626C, 0x3260666C, 0xA0EE8B01, 0xE6830009,
+0x3260666C, 0xA0DC8B01, 0xE6900009, 0x3260666C,
+0xA0D08B01, 0xE6910009, 0x3260666C, 0xA0B98B01,
+0xE6B00009, 0x3260666C, 0xA07F8B01, 0xE6BB0009,
+0x3260666C, 0xE6928920, 0x3260666C, 0xE4008B14,
+0xEB04D531, 0x52516652, 0x8B073620, 0x624D7401,
+0x8FF732B7, 0xE6007508, 0x52FBA002, 0xE60152FB,
+0xE6041261, 0x2260A188, 0xD229D428, 0xD4296542,
+0x0009420B, 0x0009A180, 0xE100E670, 0x601336FC,
+0xE0248162, 0x0BFCD21F, 0x6BBC6722, 0x26727BFC,
+0xEB0416B2, 0x06FEE078, 0x3263621D, 0xA16B8B01,
+0xDE1D0009, 0x31EC611D, 0xD41C6E12, 0x410BD114,
+0xE0700009, 0x450BD51A, 0xD41A04FE, 0x420BD210,
+0xD2170009, 0x64E3420B, 0xD60DD417, 0x0009460B,
+0x05FEE070, 0x01FDE074, 0x611DE600, 0x6253351C,
+0x326C666D, 0x22E07601, 0x32B3626D, 0x4E198FF7,
+0xE0747104, 0x0F15AFCE, 0x002029F8, 0x00202FDC,
+0x00201356, 0x00117804, 0x00202B10, 0x00117800,
+0x002013A2, 0x00203014, 0x00117808, 0x00202FF4,
+0x0020139A, 0x00203008, 0x00203010, 0x02FCE024,
+0x672CE07C, 0xEC000F76, 0xE07CEB04, 0x62CD07FE,
+0x8B013273, 0x0009A118, 0x6CCDD7B9, 0x357C65C3,
+0x62C37704, 0xD4B7327C, 0x6D52D7B7, 0x6E22470B,
+0x470BD7B6, 0xD4B664D3, 0x420BD2B3, 0xD2B30009,
+0x64E3420B, 0xD6B0D4B3, 0x0009460B, 0x67D3E600,
+0x376C666D, 0x626D7601, 0x27E032B3, 0x4E198FF7,
+0x7C08AFD3, 0x6212D1A6, 0x2228622C, 0xD2AA8B04,
+0x0009420B, 0x0009A003, 0x420BD2A8, 0x56FB0009,
+0xA0E1E200, 0xB1A62620, 0x56FB0009, 0xA0DBE200,
+0x52FB2620, 0xE500D69A, 0x65622250, 0x7604D2A0,
+0x62622252, 0xA0CFD69F, 0x56FB2620, 0x2610E124,
+0x5217D19D, 0x52181621, 0x52191622, 0x521A1623,
+0x551B1624, 0x1655E200, 0x1656551C, 0x1657551D,
+0x1658551E, 0x1659551F, 0x11281127, 0x112A1129,
+0x112C112B, 0x112E112D, 0x112FA0AE, 0xD68FD18E,
+0x6262E040, 0x76046512, 0x2152352C, 0x55116266,
+0x1151352C, 0x62626563, 0x75085612, 0x1162362C,
+0x56136252, 0x362C75EC, 0x62521163, 0x75105614,
+0x1164362C, 0x62526653, 0x76105515, 0x1155352C,
+0x56166262, 0x362CD57E, 0x62561166, 0x362C5617,
+0x66531167, 0x55186252, 0x352C7604, 0x62661158,
+0x352C5519, 0x65631159, 0x561A6262, 0x362C7504,
+0x6252116A, 0x7504561B, 0x116B362C, 0x561C6256,
+0x116C362C, 0x561D6256, 0x116D362C, 0x62526653,
+0x7604551E, 0x115E352C, 0x561F6262, 0x362CD569,
+0x6252116F, 0x7594061E, 0x0166362C, 0x6653E044,
+0x051E6252, 0x352C7644, 0xE0480156, 0x061E6262,
+0x0166362C, 0xE054D660, 0x051E6262, 0x352C4229,
+0x76040156, 0xE0586262, 0x4229061E, 0x0166362C,
+0xE23856FB, 0xE0442620, 0xE048021E, 0x62121621,
+0x55111622, 0x1653E200, 0x16545512, 0x16555515,
+0x16565513, 0x16575516, 0xE040051E, 0x051E1658,
+0x1659E050, 0x165A5514, 0xE04C051E, 0x051E165B,
+0x165CE054, 0xE058051E, 0x051E165D, 0x165EE044,
+0xE0480126, 0x11212122, 0x11251122, 0x11261123,
+0xE0400126, 0xE0500126, 0x01261124, 0x0126E04C,
+0x0126E054, 0x0126E058, 0x3F3C9358, 0x6EF64F26,
+0x6CF66DF6, 0x000B6BF6, 0x4F2269F6, 0xE240614D,
+0x89143123, 0x3127E21F, 0x8B09D734, 0xD434614D,
+0xE00171E0, 0x5671440B, 0x26596507, 0x1761A007,
+0xE001D42F, 0x6672440B, 0x26596507, 0x4F262762,
+0x0009000B, 0x614D4F22, 0x3123E240, 0xE21F8912,
+0xD7263127, 0x614D8B08, 0x5671D225, 0x420B71E0,
+0x260BE001, 0x1761A006, 0x6672D221, 0xE001420B,
+0x2762260B, 0x000B4F26, 0xE6400009, 0x46284618,
+0x6252D51C, 0x89FC2268, 0x0009000B, 0x4618E680,
+0xD5184628, 0x22686252, 0x000B89FC, 0xA0010009,
+0x7201E200, 0x8BFC3242, 0x0009000B, 0x00000080,
+0x00117804, 0x00202FF4, 0x00201356, 0x0020139A,
+0x00203008, 0x00203010, 0x00200C38, 0x00200C12,
+0x00202F00, 0x00202F14, 0x00202AA4, 0x001C36A0,
+0x001C3CA0, 0x001C36F4, 0x001C3B88, 0x001C3704,
+0x002029F8, 0x001C373C, 0x4618E680, 0xD52F4628,
+0x22686252, 0x000B8BFC, 0x2FE60009, 0x7FFC4F22,
+0xBFF16E53, 0x61E22F42, 0xE280D629, 0x54E11615,
+0x16464218, 0x422855E2, 0x57E31657, 0x16786EF2,
+0x26E22E2B, 0x4F267F04, 0x6EF6AFA8, 0x2FD62FC6,
+0x4F222FE6, 0x6C53DD1E, 0x6E43BFD6, 0x2DE2BF95,
+0x0009BFD2, 0x2C1251D5, 0x1C4154D6, 0x1C5255D7,
+0x1C6356D8, 0x6EF64F26, 0x000B6DF6, 0x61636CF6,
+0xA004E600, 0x62564109, 0x24227601, 0x36127404,
+0x000B8BF9, 0x4F220009, 0xD10FD40E, 0x0009410B,
+0xD40FD20E, 0xE5056022, 0x2202CB20, 0xD50D2452,
+0x450BE700, 0xD70C2472, 0x0009470B, 0xE601D10B,
+0x2162D20B, 0x4F264618, 0x2262000B, 0x001C3700,
+0x001C370C, 0x00203028, 0x00201356, 0x001C3500,
+0x001D4004, 0x002013CC, 0x00200EF8, 0x001E212C,
+0x001C3D30, 0x0009A1A9, 0x2FE62FD6, 0xDD8F4F22,
+0xA0049EA7, 0xD48E0009, 0x420BD28E, 0x62D265D2,
+0x8BF822E8, 0x0009A004, 0xD28AD48B, 0x55D1420B,
+0x22E852D1, 0xA0048BF8, 0xD4880009, 0x420BD285,
+0x52D255D2, 0x8BF822E8, 0x0009A004, 0xD281D484,
+0x55D3420B, 0x22E852D3, 0xA0048BF8, 0xD4810009,
+0x420BD27C, 0x52D455D4, 0x8BF822E8, 0x6EF64F26,
+0x6DF6000B, 0x2FD62FC6, 0x4F222FE6, 0x6E636D73,
+0x6C53B018, 0x64C357F4, 0xB05465E3, 0xB06A66D3,
+0xB09A0009, 0xB09E0009, 0xB0A20009, 0xB0BE0009,
+0xB0C10009, 0xB1240009, 0x4F260009, 0x6DF66EF6,
+0x6CF6A023, 0x3412D16C, 0xD66C0529, 0x2650D76C,
+0x2742000B, 0x0009A014, 0x2FD62FC6, 0x4F222FE6,
+0x6E636D73, 0x6C53BFEE, 0x64C357F4, 0xB02A65E3,
+0xB10666D3, 0x4F260009, 0x6DF66EF6, 0x6CF6A005,
+0xE603D260, 0x000B4618, 0xD25E2262, 0x000BE600,
+0x4F222262, 0xE40ABF7E, 0x0009BF7E, 0xE104D25A,
+0xE5004118, 0x2212E40A, 0x2252BF74, 0x6072D757,
+0x4F26CB20, 0x2702000B, 0xD1554F22, 0xE400410B,
+0x452BD554, 0x2FE64F26, 0x6E63D153, 0x44186612,
+0x45289210, 0x26294408, 0x44084500, 0x4400265B,
+0x4708264B, 0x47082162, 0x27EBD14C, 0x000B2172,
+0x03F06EF6, 0x2FE61FFF, 0xDE494F22, 0xE40AE101,
+0x2E12BF48, 0x726C62E3, 0xE401E100, 0x22122212,
+0x22122212, 0x22122212, 0xE7302242, 0xE40AE503,
+0x22122212, 0x22122212, 0x22122212, 0x22122212,
+0x22122212, 0x22122212, 0x22522272, 0x22122212,
+0x22122212, 0x22122212, 0x22122212, 0x121ABF22,
+0x2E62E600, 0x000B4F26, 0xD2326EF6, 0xE441E101,
+0x000B2212, 0xD1302242, 0xE605D430, 0x000B2162,
+0xD52F2462, 0x6050D22F, 0x8B0E8801, 0x6040D42E,
+0x8B078801, 0x9626D52D, 0x88016050, 0x96238B0C,
+0x0009A00A, 0xA0079621, 0xE6000009, 0x2262D426,
+0x88016040, 0xE6048B00, 0xAEF3E40A, 0xD2242262,
+0xE40AE601, 0x2262AEEE, 0x2FC62FB6, 0x2FE62FD6,
+0xDC204F22, 0x60C2ED00, 0xCB01EB64, 0x60C22C02,
+0xA041C901, 0x03C46E03, 0x034003D4, 0x001C3B88,
+0x0020302C, 0x002013A2, 0x00203034, 0x0020303C,
+0x00203044, 0x0020304C, 0x0025E720, 0x0020321C,
+0x00202F04, 0x001C5968, 0x001D4004, 0x001C3500,
+0x00201154, 0x00201180, 0x001C5814, 0x001C59D0,
+0x001C5830, 0x001C6268, 0x001C59A4, 0x001C639C,
+0x00202F16, 0x001C5804, 0x00202F15, 0x00202F17,
+0x001C581C, 0x001C5860, 0x89073DB2, 0xE40A60C2,
+0xBE9FC901, 0x7D016E03, 0x8BF52EE8, 0x8B033DB2,
+0xD23ED43D, 0x0009420B, 0x4F26E40A, 0x6DF66EF6,
+0xAE8F6CF6, 0x44116BF6, 0x604B8F01, 0x000B6043,
+0x2FB60009, 0x2FD62FC6, 0x4F222FE6, 0xDC347FFC,
+0x60C2ED00, 0xCB02EB64, 0x60C22C02, 0xC9022F02,
+0x6E03A009, 0x89083DB3, 0xE40A60C2, 0xC9022F02,
+0x6E03BE70, 0x2EE87D01, 0x3DB38BF4, 0xD4298B08,
+0x7F04D226, 0x6EF64F26, 0x6CF66DF6, 0x6BF6422B,
+0x4F267F04, 0x6DF66EF6, 0x000B6CF6, 0xD5226BF6,
+0x60525651, 0x000B4628, 0x2FB6306C, 0x2FD62FC6,
+0x4F222FE6, 0x4F024F12, 0x6E43BFF1, 0xDC1B6B03,
+0xBFECDD1B, 0x30B80009, 0x060A3C05, 0x46094609,
+0x3D654601, 0x4209020A, 0x42094209, 0x8BF032E2,
+0x4F164F06, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x4F222FE6, 0xE102DE0F, 0xE403E500, 0xBFD42E12,
+0xE6062E52, 0xE7004618, 0x2E62E403, 0x4F262E72,
+0x6EF6AFCB, 0x0009000B, 0x00203054, 0x00201356,
+0x001C5860, 0x0020306C, 0x001C1040, 0xCCCCCCCD,
+0x10624DD3, 0x001D4004, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE5007FDC, 0x6453E110,
+0x6C534128, 0xED096E53, 0x6653655D, 0x365C4608,
+0x75014608, 0x6043361C, 0x0F66675D, 0xEB0060C3,
+0x26C137D3, 0x81628161, 0x16B28163, 0x16B416E3,
+0x74048FEA, 0xD9A668F2, 0x1981DAA6, 0x59F12982,
+0x1A91D1A5, 0x5AF22A92, 0x5DF45BF3, 0x54F65EF5,
+0x21A211A1, 0x11B211B3, 0x11D411D5, 0x11E611E7,
+0x11481149, 0x55F7EE00, 0x57F8DD9C, 0x64E3D29C,
+0xDB9DD99C, 0xE845EAB8, 0x2D521D51, 0x6AAC2272,
+0x6EED4808, 0x4D086DE3, 0x3DEC65E3, 0x4D084508,
+0x3D9C35EC, 0x450860C3, 0x81D12DC1, 0x4508E050,
+0x45084008, 0x60C381D2, 0xE60035BC, 0x81D334A2,
+0x1D531DD2, 0x8D01D489, 0xD4861D64, 0xB05C65D3,
+0x64ED7E01, 0x8BDC3482, 0xDB88D182, 0xD2806812,
+0x1B814829, 0x2FD26412, 0x2B92694D, 0xD97F6722,
+0x1B734729, 0xD77C6822, 0x1BA26A8D, 0xD2806B72,
+0x22B2D57B, 0xE0035D72, 0x5E7412D2, 0x12E44018,
+0xD67C5176, 0x54781216, 0x1248E103, 0xD4796792,
+0x6852127A, 0x28C1E7FF, 0x81916952, 0x6A52E050,
+0x81A24008, 0x60C36B52, 0x6C5281B3, 0x6E521CC2,
+0x62521E63, 0x1264E600, 0x46086563, 0x7501364C,
+0x665D2672, 0x8BF83613, 0x4F267F24, 0x6DF66EF6,
+0x6BF66CF6, 0x69F66AF6, 0x68F6000B, 0x60616642,
+0x8D04C803, 0x6061E500, 0x8802C903, 0x52628B03,
+0x51246563, 0x000B2412, 0x2FD66053, 0x4F222FE6,
+0x6E537FEC, 0xE5506253, 0xE4006D43, 0xA0014508,
+0x5224E101, 0x22116043, 0x81238121, 0x81226053,
+0x362056E2, 0xD2548BF5, 0x64F316E4, 0x420BE614,
+0x65E165E3, 0x2549E4FC, 0x61F12E51, 0x214965F3,
+0x54D12F11, 0x410BD14C, 0x57D1E614, 0xCB016071,
+0x1DE12701, 0x4F267F14, 0x000B6EF6, 0x2FD66DF6,
+0x4F222FE6, 0x6E537FEC, 0xE5FC6653, 0x60616D43,
+0xCB012059, 0x52E22601, 0x8B063260, 0x51E212E4,
+0x8B0431E0, 0xA00252D1, 0xAFF01E22, 0xD23A5664,
+0xE61464F3, 0x65E3420B, 0xE1FC67E1, 0x2E712719,
+0x54D167F1, 0xD1342719, 0xE61465F3, 0x2F71410B,
+0x602152D1, 0x2201CB01, 0x7F141DE1, 0x6EF64F26,
+0x6DF6000B, 0x4F222FE6, 0xDE23624C, 0x42004208,
+0x3E2CA005, 0xD41F5252, 0xBF8E5624, 0x65E22E62,
+0x352052E1, 0xD6228BF6, 0x4F262622, 0x6EF6000B,
+0x2FC62FB6, 0x2FE62FD6, 0xDC184F22, 0x52C1DB1F,
+0x362066C2, 0x6061891C, 0x8801C903, 0xDE138918,
+0xBF63DD1B, 0x650364E3, 0x66B28503, 0x3262620D,
+0xD40B8907, 0x0009BF9B, 0x4D0BD416, 0xAFE60009,
+0xBF620009, 0xD41464E3, 0x00094D0B, 0x0009AFDF,
+0x2262D212, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x00202B00, 0x00202B08, 0x00202B10, 0x00202B38,
+0x00202F1C, 0x001000B4, 0x00101680, 0x001E2108,
+0x001C3D00, 0x00117880, 0x00200A9E, 0x00202F00,
+0x00201356, 0x00203088, 0x0020308C, 0x001C3D28,
+0x2FC62FB6, 0x2FE62FD6, 0x7FFC4F22, 0x6022D22B,
+0x8D41C803, 0xDE2A2F01, 0xDB2BDC2A, 0xED01A017,
+0xC9036051, 0x89168801, 0xD128D426, 0x0009410B,
+0x61035503, 0xC8208551, 0xE0508903, 0x720102BE,
+0xD2230B26, 0x420B64E3, 0xD6226513, 0x52C126D2,
+0x352065C2, 0xDE208BE4, 0xDB21DD20, 0x52D1DC21,
+0x352065D2, 0x60518918, 0x8801C903, 0xD41B8914,
+0x460BD616, 0x57030009, 0x8F0437E0, 0xE2016503,
+0xAFEC2B20, 0xD4182C52, 0x420BD218, 0xD6110009,
+0x4118E101, 0x2612AFE3, 0xC80460F1, 0xD2148907,
+0x4F267F04, 0x6DF66EF6, 0x422B6CF6, 0x7F046BF6,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x001E2100,
+0x00202B10, 0x00202B08, 0x00202AA4, 0x0020106C,
+0x002010EE, 0x001C3D30, 0x00117880, 0x00202B00,
+0x00202F20, 0x00202F1C, 0x00202B38, 0x0020108A,
+0x00200170, 0xE601D203, 0x1265D503, 0x000B2252,
+0x00001266, 0x001C1010, 0x0000C34F, 0x0009000B,
+0x2FD62FC6, 0x4F222FE6, 0x6D436C53, 0xEE00A004,
+0x7E0164D4, 0x644CBFF2, 0x8BF93EC2, 0x6EF64F26,
+0x000B6DF6, 0xE5006CF6, 0x6643A002, 0x76017501,
+0x22286260, 0xAFE38BFA, 0x2FE60009, 0x75076253,
+0xE1086753, 0x6043EE0A, 0x4409C90F, 0x650330E2,
+0x8D014409, 0xE630E637, 0x4110365C, 0x8FF22760,
+0xE00077FF, 0x000B8028, 0x000B6EF6, 0x000BE000,
+0x2FE6E000, 0x7FEC4F22, 0x6E436253, 0xBFDC65F3,
+0xBFD06423, 0xBFCE64E3, 0xD40364F3, 0x0009BFCB,
+0x4F267F14, 0x6EF6000B, 0x00203090, 0xE4FDD59A,
+0xD69A6152, 0x25122149, 0x74016052, 0x2502CB01,
+0xD1976752, 0x25722749, 0xC8406010, 0x60628902,
+0x2602CB04, 0xE5016062, 0x2602CB08, 0xE4026062,
+0x2602C9CF, 0x45186062, 0x2602CB03, 0x000B1642,
+0xD58C1653, 0xD28DD78C, 0xE100D48D, 0x2511E600,
+0x22102711, 0x2461AFD2, 0xD28A664C, 0x362C4600,
+0xCB106060, 0x2600000B, 0xD286654C, 0x352C4500,
+0xE1EF6650, 0x000B2619, 0x664C2560, 0x4600D282,
+0x6060362C, 0x000BCB10, 0x654C2600, 0x4500D27E,
+0x6650352C, 0x2619E1EF, 0x2560000B, 0xD279664C,
+0x362C4600, 0xCB086060, 0x2600000B, 0xD275654C,
+0x352C4500, 0xE1F76650, 0x000B2619, 0x664C2560,
+0x4600D271, 0x6060362C, 0x000BCB08, 0x654C2600,
+0x4500D26D, 0x6650352C, 0x2619E1F7, 0x2560000B,
+0xD668624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0x624C600C, 0x4200D663, 0x6020326C,
+0x4021C908, 0x40214021, 0x600C000B, 0x644CD15F,
+0x6240341C, 0x602C000B, 0x644CD15D, 0x6240341C,
+0x602C000B, 0x4F222FE6, 0x645C6E43, 0x3467E60A,
+0xBFEB8914, 0x640C0009, 0x880160EC, 0xE00F8B02,
+0x2409A002, 0x44094409, 0xE60A624C, 0x89053263,
+0x644CBFE2, 0x6023620C, 0x8B00C880, 0x6023E200,
+0x000B4F26, 0x4F226EF6, 0x6062D64A, 0x8B038801,
+0x0009B246, 0x0009A003, 0xE640D247, 0xD6472260,
+0x4F26E200, 0x2622000B, 0xD6424F22, 0x88026062,
+0xB28F8B01, 0xD6410009, 0x4F26E200, 0x2622000B,
+0xD43DD53C, 0xE701E100, 0x000B2512, 0xD23A2470,
+0x000BE604, 0x4F222260, 0xD13AD439, 0x0009410B,
+0xE1FDD539, 0xD2396650, 0xE7002619, 0x4F262560,
+0x2270000B, 0xD4364F22, 0x410BD132, 0xD5320009,
+0x6650E7FB, 0x4F262679, 0x2560000B, 0xD4314F22,
+0x410BD12C, 0xD52C0009, 0x6650E7F7, 0x4F262679,
+0x2560000B, 0x942DD528, 0x22496250, 0x2520000B,
+0xE4BFD525, 0x22496250, 0x2520000B, 0xD2264F22,
+0x600D8522, 0x89112008, 0x89138801, 0x89158803,
+0x89438805, 0x89498806, 0x894F8808, 0x89558809,
+0x895B880A, 0x8961880B, 0x0009A068, 0x0009B06A,
+0x600CA065, 0x0009B078, 0x600CA061, 0x0009B081,
+0x600CA05D, 0x0000FF7F, 0x001E2148, 0x001E1108,
+0x001E1000, 0x00202F60, 0x00202F62, 0x00202F81,
+0x00202F44, 0x001E103F, 0x001E105F, 0x001E102F,
+0x001E1090, 0x00202F68, 0x001E100B, 0x00202F64,
+0x00203094, 0x00201356, 0x001E1028, 0x00202F80,
+0x002030A0, 0x002030B0, 0x00202F38, 0x6260D684,
+0x8B2B2228, 0x0009B061, 0x600CA029, 0x6260D680,
+0x8B232228, 0x0009B069, 0x600CA021, 0x6260D67C,
+0x8B1B2228, 0x0009B0C7, 0x600CA019, 0x6260D678,
+0x8B132228, 0x0009B0CD, 0x600CA011, 0x6260D674,
+0x8B0B2228, 0x0009B125, 0x600CA009, 0x6260D670,
+0x8B032228, 0x0009B13D, 0x600CA001, 0x4F26E000,
+0x0009000B, 0xD26CD16B, 0xD56C8412, 0x4000C90F,
+0xD76B012D, 0xE403D66B, 0xE20F611C, 0x2540E001,
+0x25202712, 0x2602000B, 0xE601D262, 0x30668523,
+0xE0008D05, 0xD663D260, 0xE0018122, 0x000B2602,
+0xD25C0009, 0x600D8523, 0x89052008, 0x8B0A8801,
+0x6060D65D, 0x2600CB01, 0xD457D65A, 0xE001E101,
+0x000B2612, 0x000B8142, 0xD152E000, 0x8513E501,
+0x640D4518, 0x66033453, 0xE0008D05, 0xD551D253,
+0x2260E001, 0x000B2502, 0x4F220009, 0x8513D149,
+0x6453650D, 0x62494419, 0x227D672E, 0x8801602C,
+0x88028909, 0x88038910, 0x8806891A, 0x88078935,
+0xA04C893B, 0xD5460009, 0x6652D746, 0x2762D446,
+0x622C6261, 0x2421A038, 0x2228625C, 0xD4438B3F,
+0x6642D540, 0x2562D440, 0x24018561, 0x6203A02C,
+0x2008605C, 0x88108907, 0x88208908, 0x88308909,
+0xA02C890A, 0xD23A0009, 0x6222A008, 0xA005D239,
+0xD2396222, 0x6222A002, 0x6262D638, 0xD432D531,
+0x66212522, 0xA00F626C, 0xD6352421, 0x6261D52D,
+0x622CD42D, 0xA0072562, 0xD6322421, 0x8561D529,
+0x2562D429, 0x62032401, 0x662D8515, 0x3617610D,
+0x65038F01, 0xB0CB2451, 0xA0010009, 0xE000E001,
+0x000B4F26, 0xD6190009, 0xD427E101, 0x65412610,
+0xD118D717, 0xE20F655D, 0x2752E001, 0x000B2620,
+0x2FE62102, 0xD20F4F22, 0x640C8523, 0x8B082448,
+0xD511D61D, 0x2621E200, 0x940F8451, 0xA0482049,
+0xDE0D8051, 0xC84060E0, 0xE2018D32, 0x89443427,
+0xD216D615, 0x2641420B, 0x0009A030, 0x0000FF7F,
+0x00202F81, 0x00202F38, 0x00202F44, 0x001E1100,
+0x001E100C, 0x00202F64, 0x001E1000, 0x001E1001,
+0x00202F6C, 0x00202F4C, 0x00202F50, 0x00202F54,
+0x00202F70, 0x00202F74, 0x00202F78, 0x00202F7C,
+0x00203280, 0x0020328A, 0x00202F5E, 0x0020225A,
+0x89123427, 0xD294D693, 0x2641420B, 0xCB8084E1,
+0x80E1B0F5, 0xD69160E0, 0x2E00CB04, 0xC93F6060,
+0xD68F2600, 0xA001E001, 0xE0002602, 0x000B4F26,
+0xD68C6EF6, 0xC8806060, 0xD2868919, 0x88016021,
+0xD2898B15, 0x8524E501, 0x89103056, 0xE203D187,
+0x2120D487, 0xE00B6541, 0x0656655D, 0xE40FD585,
+0x2140E702, 0xD77E2571, 0x000BE001, 0x000B2702,
+0x2FE6E000, 0xDE804F22, 0xC88084E1, 0xD57A892C,
+0x20088554, 0x61038F28, 0x8553D77C, 0x64036672,
+0x8566650C, 0x3520620C, 0xD6798B1E, 0x651CD774,
+0x2651644C, 0x60E02741, 0x8904C840, 0x420BD275,
+0xA0030009, 0xD2680009, 0x0009420B, 0x0009B09F,
+0xE201D167, 0x60E02122, 0xCB04D464, 0x60402E00,
+0x2400C93F, 0x6023A001, 0x4F26E000, 0x6EF6000B,
+0x2FB62FA6, 0x2FD62FC6, 0xDA622FE6, 0x66A1E240,
+0x3622DC5E, 0x62638900, 0x6ED36D2C, 0x4E2136D8,
+0x4E212A61, 0xDB61D460, 0xE700A00F, 0x770162B2,
+0x71026123, 0x66212B12, 0x71026213, 0x61212B12,
+0x651D666D, 0x356C4528, 0x627C2452, 0x8BED32E3,
+0xC90360D3, 0x8B108803, 0x617367B2, 0x2B127102,
+0x71026E13, 0x2B126571, 0x655D6DE1, 0x422862DD,
+0x325CE107, 0xA00C2C10, 0x88022422, 0xA0038B01,
+0x8801E203, 0xE2018B05, 0x66B22C20, 0x655D6561,
+0xE60F2452, 0x67A12C60, 0x8B052778, 0xDD38DC44,
+0xEB01EA00, 0x2DB22CA2, 0x6DF66EF6, 0x6BF66CF6,
+0x6AF6000B, 0x2FE62FD6, 0xE240DD36, 0x362266D1,
+0x62638900, 0x3678672C, 0x7703DE38, 0x47212D61,
+0x64E2D635, 0xA00E4721, 0x6562E100, 0x62537101,
+0x74012450, 0x24204219, 0x45297401, 0x74012450,
+0x24504519, 0x621C7401, 0x8BEE3273, 0x66E24200,
+0x420061D1, 0x2118362C, 0x2E628F06, 0xDD1CD728,
+0xE501E400, 0x2D522742, 0x000B6EF6, 0x2FD66DF6,
+0x4F222FE6, 0xED0AEE01, 0x64E3BC96, 0xBC9B64E3,
+0x62EC7E01, 0x8BF732D7, 0xBC9EEE01, 0x64E364E3,
+0x7E01BCA3, 0x32D762EC, 0x4F268BF7, 0x000B6EF6,
+0xD1186DF6, 0xD418920D, 0x72122122, 0x2422D617,
+0xD7177204, 0x72202622, 0x2722D116, 0x000B7230,
+0x137A2122, 0x00202F5E, 0x00202366, 0x001E1015,
+0x00202F64, 0x001E1001, 0x00202F38, 0x001E1100,
+0x00202F62, 0x00202F50, 0x001E1000, 0x00202F54,
+0x00202F60, 0x0020225A, 0x001E100C, 0x00202F4C,
+0x00202F68, 0x00202F6C, 0x00202F70, 0x00202F74,
+0x00202F78, 0x00202F7C, 0x4F222FE6, 0xD6507FFC,
+0x88016060, 0xE2018951, 0x2620BFBB, 0xD54ED14D,
+0xDE4E6010, 0x64E36552, 0x7402C840, 0x8D22D14C,
+0xD24C7502, 0xE601D74C, 0xE7042722, 0x76016255,
+0x626C2421, 0x8FF93273, 0xD4437402, 0x6242E601,
+0x640D8528, 0x67494419, 0x275D657E, 0x81E4607C,
+0xE417D542, 0x67557601, 0x3243626C, 0x8FF92171,
+0xA0207102, 0xD23E0009, 0xE601D73B, 0xE7042722,
+0x76016255, 0x626C2421, 0x8FF93273, 0xD4327402,
+0x6242E601, 0x640D8528, 0x67494419, 0x275D657E,
+0x81E4607C, 0xE417D533, 0x67557601, 0x3243626C,
+0x8FF92171, 0x924A7102, 0xD2262E21, 0x5E23D72E,
+0x64F22FE2, 0x604365F2, 0x2700C980, 0xC9606043,
+0x80716103, 0xC9036043, 0x80724519, 0x65F2605C,
+0x817266F2, 0x46194629, 0x606C4529, 0x4018645C,
+0x8173304C, 0x21185E23, 0x64F22FE2, 0x6E4C62F2,
+0x602C4219, 0x66F262F2, 0x46294018, 0x461930EC,
+0x42298174, 0x652C606C, 0x305C4018, 0x81758F07,
+0x0009BC9C, 0x2228620C, 0xA00A8908, 0x60130009,
+0x8B038840, 0x0009B009, 0x0009A003, 0xE202D60F,
+0x7F042622, 0x000B4F26, 0x000B6EF6, 0x060A0009,
+0x00202F80, 0x001E1000, 0x00202F6C, 0x00203280,
+0x0020328C, 0x00203224, 0x00202F54, 0x00203254,
+0x00203252, 0x00203226, 0x00202F38, 0x00202F64,
+0x4F222FE6, 0xDE937FFC, 0x200884E9, 0x2F008D06,
+0xD692D491, 0x0009460B, 0x64F0B194, 0x6620D290,
+0x89022668, 0xC9BF60E0, 0x7F042E00, 0x000B4F26,
+0x000B6EF6, 0x2FE60009, 0xDE8A4F22, 0x60E0D68A,
+0xCBC0D48A, 0x62602E00, 0xC803602C, 0x40218904,
+0x70014021, 0x6603A002, 0x66034009, 0xD684616D,
+0xE500A004, 0x75016262, 0x74042422, 0x3213625D,
+0xD2808BF8, 0x0009420B, 0xC9BF84E2, 0x4F2680E2,
+0x6EF6000B, 0x2FE62FD6, 0x7FFC4F22, 0x6260D67A,
+0x89442228, 0xD56FE100, 0x60502610, 0xCB40D477,
+0x2500440B, 0x8D052008, 0x62E06E03, 0x7104612C,
+0x2F11A006, 0xD472D66A, 0xDD726760, 0x657C4D0B,
+0xE23C6D1D, 0x8B033D27, 0xD264D46F, 0x0009420B,
+0x4D214D21, 0xA005D76D, 0x66E6E400, 0x357C4508,
+0x74012562, 0x35D3654D, 0xD7698BF7, 0x6172E003,
+0x81114018, 0x6E7260F1, 0x81E2700C, 0xD4656172,
+0xDD658113, 0x4D0BDE65, 0xE2016572, 0xD4642E22,
+0x420BD252, 0xD6530009, 0xC93F6060, 0x7F042600,
+0x6EF64F26, 0x6DF6000B, 0x2FC62FB6, 0x2FE62FD6,
+0xD25C4F22, 0x6B436E73, 0x420B6C53, 0x20086D63,
+0x61038F08, 0xD245D458, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6422B, 0x21B060C3, 0x60D38011, 0xE5008111,
+0x64BCA007, 0x6053655D, 0x665300EC, 0x7501361C,
+0x625D8064, 0x8BF53243, 0x6060D636, 0x2600C9BF,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x7FC44F22,
+0x720262F3, 0x22512F41, 0x45297202, 0x60632251,
+0xE5C4E682, 0x67F38121, 0x655C666C, 0xE408BFBC,
+0x4F267F3C, 0x0009000B, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE1007FC4, 0x6513ECFF,
+0x6B136CCD, 0xDE36D735, 0xEDFF64F3, 0xD835EA04,
+0x6053655C, 0x027D4000, 0x32C0622D, 0x66038D0D,
+0x09ED6063, 0x2491027D, 0x24217402, 0x698202ED,
+0x3928622D, 0x74022892, 0x75017104, 0x6063625C,
+0x07D532A2, 0x0EB58FE4, 0x2448641C, 0xE6808905,
+0x67F3E5C5, 0xBF7F666C, 0x7F3C655C, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0xD11E68F6,
+0x6012D21E, 0xCB20E405, 0x2102E500, 0x000B2242,
+0x00002252, 0x001E1017, 0x002030BC, 0x00201356,
+0x00202F1A, 0x001E1015, 0x001E10BF, 0x00117800,
+0x001E10FC, 0x00200170, 0x00202F20, 0x002024BE,
+0x002030C0, 0x002013A2, 0x002030DC, 0x0011788C,
+0x00202F1C, 0x00202B00, 0x002010EE, 0x001E2130,
+0x002030E4, 0x00202480, 0x002030E8, 0x00202F26,
+0x00202F2E, 0x00203220, 0x001C3500, 0x001D4004,
+0xD565D164, 0xE400D765, 0x2142E20F, 0x17411154,
+0xD5632722, 0x9669D763, 0x15412572, 0x96661562,
+0xE6011565, 0xD5601165, 0x666CE6F8, 0x25422542,
+0x25422542, 0x25422542, 0x25622542, 0x7601E727,
+0x67632572, 0x25627797, 0xE7042572, 0x2572E248,
+0xE2192522, 0xE2702522, 0x25422542, 0x25422542,
+0x25222542, 0x2522E20C, 0x25422542, 0x25422542,
+0x25422542, 0x25422542, 0x000B154A, 0xE2081145,
+0x0009422B, 0x2FE62FD6, 0x7FFC4F22, 0xC8206043,
+0x6E438D02, 0x0009BE6D, 0xC81060E3, 0xBE6A8901,
+0x60E30009, 0x8901C840, 0x0009BE8C, 0xC80160E3,
+0xDD3E8938, 0xC80260D0, 0x2F008D03, 0x460BD63C,
+0x60F00009, 0x8902C804, 0x460BD63A, 0x62F00009,
+0xC8806023, 0x60D08902, 0x2D00C97F, 0xC8016023,
+0xD6358906, 0x0009460B, 0x0009A007, 0x51630601,
+0x8902C808, 0x460BD631, 0x60F00009, 0x8902C810,
+0x420BD22F, 0xD52F0009, 0x88026052, 0xD22E8B03,
+0xA005E604, 0x88012260, 0xD22B8B02, 0x2260E601,
+0x2522E200, 0xC88060E3, 0xD628892E, 0x60E36E60,
+0x8902C880, 0x420BD226, 0x60E30009, 0x8902C840,
+0x420BD224, 0x60E30009, 0x8902C802, 0x420BD222,
+0x60E30009, 0x890EC804, 0x410BD120, 0xBF0E0009,
+0xBF4D0009, 0xD51E0009, 0x6050D41E, 0xC908D71E,
+0xBF842500, 0x60E32472, 0x8905C808, 0x7F04D21B,
+0x6EF64F26, 0x6DF6422B, 0x4F267F04, 0x000B6EF6,
+0x00006DF6, 0x001C581C, 0xA000A000, 0x001D0100,
+0x001D4000, 0x00040021, 0x001C589C, 0x001E1021,
+0x00201536, 0x00201558, 0x00201B98, 0x00201570,
+0x0020157E, 0x00202F64, 0x001E100B, 0x001E1028,
+0x002015D4, 0x002015E0, 0x00201586, 0x002015A4,
+0x001E1000, 0x0010F100, 0x12345678, 0x002015BC,
+0x644CD6A7, 0x000B346C, 0xD6A62450, 0x346C644C,
+0x2450000B, 0x644CD6A4, 0x000B346C, 0x625C2450,
+0x4208616D, 0x42084119, 0x42006019, 0x670E614C,
+0xD49E321C, 0x4200207D, 0x324CC90F, 0x2200000B,
+0x4208625C, 0x42004208, 0x324C644C, 0x4200D498,
+0x000B324C, 0x2FE62260, 0x614C4F12, 0x4100D493,
+0x6710314C, 0xE29F666D, 0x27294619, 0x6E536269,
+0x672E6573, 0x4221227D, 0x42214221, 0x7601662C,
+0xE4014608, 0x34E84608, 0x644C4600, 0x071A0467,
+0x2150257B, 0x000B4F16, 0x4F226EF6, 0xD2857FE8,
+0x88016021, 0xD2848B7B, 0x26686621, 0xD2838B77,
+0x26686621, 0xE50F8B73, 0xE401BFA2, 0xBFA4E501,
+0xE586E400, 0xE400655C, 0x2F50BFA4, 0xBFA1E401,
+0xE602E506, 0x60634618, 0x81F2E401, 0x6543BF9F,
+0xE40185F2, 0xBFAB6543, 0x85F26603, 0x6543E401,
+0x6603BFB1, 0xE40265F0, 0x6053756C, 0x80F8BF80,
+0xBF82E402, 0x84F8E512, 0x7090E402, 0x6503BF82,
+0x4618E602, 0x81F66063, 0xBF80E402, 0x85F6E500,
+0x6603E402, 0xE500BF8C, 0xE40285F6, 0xBF926603,
+0xE5FEE500, 0xE010655C, 0xBF61E403, 0xE5130F54,
+0xE40EBF63, 0x05FCE010, 0xBF63E40E, 0xE5007585,
+0xBF64E403, 0xE500E640, 0xBF71E403, 0xE500E640,
+0xBF78E403, 0xE5FFE640, 0xE014655C, 0xBF47E404,
+0xE40F0F54, 0xE504BF49, 0x05FCE014, 0xBF49E40F,
+0xE5017584, 0xBF4AE640, 0xE501E404, 0xBF57E640,
+0xE501E404, 0xE404E640, 0xAF5C7F18, 0x7F184F26,
+0x000B4F26, 0x4F220009, 0xD2427FF0, 0x88016021,
+0xD2418B71, 0x26686621, 0xD2408B6D, 0x26686621,
+0xE50F8B69, 0xE401BF1C, 0xBF1EE501, 0xE586E400,
+0xE400655C, 0x2F50BF1E, 0xBF1BE401, 0xE401E506,
+0xBF1C6543, 0xE401E640, 0xBF296543, 0xE401E640,
+0xBF306543, 0x65F0E640, 0x756CE402, 0xBEFF6053,
+0xE40280F4, 0xE512BF01, 0xE40284F4, 0xBF017090,
+0xE6406503, 0xBF02E402, 0xE640E500, 0xBF0FE402,
+0xE640E500, 0xBF16E402, 0xE5FEE500, 0x6053655C,
+0xBEE5E403, 0xE51380F8, 0xE40EBEE7, 0xE40E84F8,
+0xBEE77085, 0xE5006503, 0xBEE8E640, 0xE500E403,
+0xBEF5E640, 0xE500E403, 0xBEFCE640, 0xE5FFE403,
+0x6053655C, 0xBECBE404, 0xE40F80FC, 0xE504BECD,
+0xE40F84FC, 0xBECD7083, 0xE5016503, 0xBECEE640,
+0xE501E404, 0xBEDBE640, 0xE501E404, 0xE404E640,
+0xAEE07F10, 0x7F104F26, 0x000B4F26, 0x00000009,
+0x001E102F, 0x001E1080, 0x001E1090, 0x001E103F,
+0x001E103E, 0x00202F5E, 0x00202F60, 0x00202F62,
+0xD21DD11C, 0x66206010, 0x676C7001, 0x3700C90F,
+0xE5008D13, 0x67106210, 0x7701622C, 0x64232170,
+0xD6166010, 0x44084408, 0x3428C90F, 0x62602100,
+0x7201D513, 0x44082620, 0x000B354C, 0xD10F6053,
+0x25586510, 0xE6008D13, 0xD60DD40B, 0x655C6540,
+0x47086753, 0x37584708, 0x47086540, 0x24507501,
+0x367C6040, 0x2400C90F, 0x72FF6210, 0x000B2120,
+0x00006063, 0x00202F19, 0x00202F18, 0x00202F1A,
+0x00202B40, 0x7FFC4F22, 0xE680D1A8, 0x666C6212,
+0xD2A72F22, 0x67F36563, 0x420B7542, 0x7F04E404,
+0x000B4F26, 0xE6800009, 0xD2A1666C, 0xE7006563,
+0x422B7540, 0xE6806473, 0xD29D666C, 0xE7006563,
+0x422B7543, 0x2FB66473, 0x2FD62FC6, 0x4F222FE6,
+0x4D18ED01, 0xDB98DC97, 0x65C252C1, 0x89203520,
+0xC9036051, 0x891C8801, 0xD194DE92, 0x64E3410B,
+0x85036503, 0x670D66B2, 0x89073762, 0xD291D490,
+0x0009420B, 0xE701D190, 0x2172AFE6, 0xDE8F64E3,
+0x00094E0B, 0xD48FD68E, 0x410BD18F, 0xAFDB26D2,
+0x4F260009, 0x6DF66EF6, 0x000B6CF6, 0x4F226BF6,
+0x85467FF4, 0x2F01E681, 0x666C8547, 0x854881F1,
+0x81F2D27B, 0x67F38542, 0x854381F3, 0x81F4E40C,
+0x65636053, 0x420B81F5, 0x7F0C7540, 0x000B4F26,
+0x2F860009, 0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6,
+0x7FEC4F22, 0xE000D176, 0xD4782F12, 0x81F26103,
+0xDC771F42, 0xD6776B13, 0xE0014B08, 0x460BDE76,
+0x3BEC4B00, 0x66C21F03, 0x362052C1, 0xA1818B01,
+0x60610009, 0x8801C903, 0xA17B8B01, 0x85610009,
+0x8B01C801, 0x0009A080, 0x85D25D63, 0xC9036603,
+0x85D36403, 0x6053650D, 0x40214021, 0x4500C93F,
+0x322A6103, 0x6053252D, 0xC901E510, 0xD95E3153,
+0x6E038D21, 0x4408D761, 0x44086870, 0x44006213,
+0x28884200, 0x342C8F0E, 0x6043D25D, 0x60E3072D,
+0x4A196A7D, 0x658E68A9, 0x285D8801, 0x6A7C8F0B,
+0x6A13A009, 0x6043D257, 0x61ED0E2D, 0x68194119,
+0x287D678E, 0xD1546AEC, 0x22286210, 0xEAFF8901,
+0xEEFF6AAC, 0x6EEC65AD, 0x8B0F35E0, 0x4D0BDD3F,
+0x540364C3, 0xBF72E502, 0xD44C6D03, 0x410BD13F,
+0xD74B65D3, 0xD44BEE01, 0x27E2A025, 0x2679E7FC,
+0x81D26063, 0x946085D3, 0x61032049, 0x4508268B,
+0x251B6063, 0x605381D2, 0x85D481D3, 0x4118E108,
+0x81D4201B, 0xEE0262C2, 0x20798521, 0x64C28121,
+0x6041678D, 0xCB0137E3, 0x24018D04, 0xEEE785D2,
+0x81D220E9, 0x490BD438, 0x60C20009, 0x52F366F2,
+0x2B02CB01, 0x2622AF6F, 0xD2208561, 0x8F02C802,
+0xA0D264C3, 0x420B0009, 0xD9300009, 0x5E036503,
+0x079EE04C, 0x7701DD2E, 0x69D20976, 0x7901D626,
+0x6D602D92, 0x89062DD8, 0xD218D424, 0xED01420B,
+0xA0B3D723, 0x625127D2, 0x4118E10F, 0x2219E402,
+0x32404418, 0x85518B46, 0x20D9EDFC, 0x60518151,
+0xCB017DE3, 0x85E12501, 0x20D9D60A, 0x460B81E1,
+0x69F264C3, 0xA09957F3, 0x7E032972, 0x001C3D9C,
+0x00201E38, 0x00202B38, 0x00202F00, 0x0020106C,
+0x00202B00, 0x002010EE, 0x001E2130, 0x0020108A,
+0x001C3D30, 0x00203200, 0x00201356, 0x0020320C,
+0x00202B10, 0x002029F8, 0x001C3D00, 0x0020321C,
+0x00203100, 0x00203180, 0x00202F14, 0x00202B08,
+0x001E212C, 0x00203204, 0x00203208, 0x00202AA4,
+0x00203220, 0x6DDD6D51, 0x6DD94D19, 0x2D6D66DE,
+0x60DC7D01, 0x41186103, 0x8F458801, 0xD65B2511,
+0x74016462, 0x85E32642, 0x6063660D, 0x40214021,
+0x4600C93F, 0x322A6D03, 0x6063262D, 0xD154C801,
+0x8901D954, 0x2D6B96A1, 0xE010E600, 0x64DD0F64,
+0x07FCE010, 0x4000607C, 0x622D021D, 0x8D123240,
+0x60636603, 0xE7FF021D, 0x8B013270, 0x01D5A00B,
+0x02FCE010, 0x7201E604, 0x622C0F24, 0x8BE73262,
+0x666C06FC, 0x60634600, 0x7101019D, 0xD1420915,
+0x697D6711, 0x89073940, 0x602D6211, 0x890388FF,
+0xDD3E21D1, 0x2D20E201, 0xEDFC8551, 0x815120D9,
+0xD23B6051, 0x64C3CB01, 0x2501420B, 0x02FCE010,
+0x612CD438, 0x440BE001, 0x270267F2, 0xD23685EF,
+0x420B54F2, 0xAE96650D, 0x420B0009, 0x54030009,
+0x85446E03, 0x4D18ED08, 0x30D020D9, 0xBE568B03,
+0xA007E501, 0x85410009, 0x620DDD2C, 0x890122D8,
+0xE500BE4D, 0xD22BD42A, 0x65E3420B, 0xED01D72A,
+0x27D2AE79, 0xEE0485F2, 0x610D7001, 0x81F231E7,
+0x7C088D02, 0x0009AE66, 0x4F267F14, 0x6DF66EF6,
+0x6BF66CF6, 0x69F66AF6, 0x68F6000B, 0x4F222FE6,
+0x6E22D21E, 0xC84060E3, 0x22E28D02, 0x0009BDD2,
+0x4218E240, 0x89012E28, 0x0009BDDD, 0xC81060E3,
+0xD4178905, 0x420BD217, 0xBDDC0009, 0x60E30009,
+0x8901C805, 0x0009BE2D, 0xC80260E3, 0x4F268902,
+0x6EF6ADD9, 0x000B4F26, 0x80006EF6, 0x00203220,
+0x00202F26, 0x00202F2E, 0x00202F22, 0x00202F24,
+0x002010EE, 0x002029F8, 0x002013A2, 0x00008000,
+0x00202B08, 0x0020108A, 0x001E212C, 0x001C3510,
+0x00203214, 0x00201356, 0x080A0C0E, 0x00020406,
+0x1A1C1E20, 0x12141618, 0x2E303234, 0x26282A2C,
+0x3A3C3E40, 0x6C625648, 0x41112F26, 0xE2208F18,
+0x890B3123, 0x321CD204, 0xD1026220, 0x412B312C,
+0x00090009, 0x00202A22, 0x002029D8, 0x000BE000,
+0x400062F6, 0x40004000, 0x40004000, 0x40004000,
+0x62F6000B, 0x40004000, 0x40004000, 0x40004000,
+0x40184000, 0x62F6000B, 0x40004000, 0x40004000,
+0x40004000, 0x40284000, 0x62F6000B, 0x40004000,
+0x40184000, 0x000B4028, 0xC90F62F6, 0x40054005,
+0x40054005, 0x62F6000B, 0x4005C907, 0x40054005,
+0x62F6000B, 0x4005C903, 0x000B4005, 0xC90162F6,
+0x000B4005, 0x000062F6, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x544F0D0A, 0x46205355, 0x00003A57, 0x20636544,
+0x32203231, 0x20373030, 0x333A3132, 0x34323A36,
+0x00000000, 0x00000D0A, 0x00000043, 0x42707372,
+0x3D206675, 0x554E203D, 0x202C4C4C, 0x6E49677A,
+0x4E497274, 0x6D754E51, 0x0000003D, 0x61766E49,
+0x2064696C, 0x72657375, 0x20726F20, 0x2079656B,
+0x00214449, 0x52504545, 0x57204D4F, 0x65746972,
+0x6461202C, 0x003D7264, 0x6C617620, 0x0000003D,
+0x00000A0D, 0x6E6B6E55, 0x206E776F, 0x6D6D6F63,
+0x3D646E61, 0x00000000, 0x000A0D52, 0x203A3051,
+0x00000020, 0x203A3151, 0x00000020, 0x203A3251,
+0x00000020, 0x203A3351, 0x00000020, 0x203A3451,
+0x00000020, 0x61437748, 0x7262696C, 0x6F697461,
+0x6620206E, 0x0A6C6961, 0x0000000D, 0x73696F4E,
+0x61432065, 0x7262696C, 0x6F697461, 0x6166206E,
+0x21216C69, 0x00000D0A, 0x00000072, 0x00205220,
+0x00000D0A, 0x62735576, 0x7473725F, 0x00000A0D,
+0x62735576, 0x7375735F, 0x646E6570, 0x00000A0D,
+0x62735576, 0x7365725F, 0x000A0D6D, 0x00000042,
+0x72746E49, 0x6D652051, 0x2C797470, 0x49677A20,
+0x4972746E, 0x754E514E, 0x00003D6D, 0x654C7245,
+0x0000006E, 0x00000049, 0x20746F4E, 0x756F6E65,
+0x49206867, 0x4220514E, 0x0A0D6675, 0x00000000,
+0x000000FF, 0x00020001, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00020003, 0x01090108, 0x0002010A,
+0x02000003, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C0207, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00FF010F, 0x01090108, 0x010B010A,
+0x020000FF, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00205220, 0x00000046, 0x00000059, 0x73204142,
+0x003D7165, 0x49544120, 0x0000204D, 0x00000000,
+0x00000000, 0x002E0209, 0x80000101, 0x000409FA,
+0x00FF0400, 0x05070000, 0x02000201, 0x82050700,
+0x00020002, 0x03830507, 0x07010040, 0x40020405,
+0x02090000, 0x0101002E, 0x09FA8000, 0x04000004,
+0x000000FF, 0x02010507, 0x07000040, 0x40028205,
+0x05070000, 0x00400383, 0x04050701, 0x00004002,
+0x00000000, 0x00000000, 0x07090000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, };
+
+const u32_t zcDKFwImageSize=12988;
diff --git a/drivers/staging/otus/hal/hpani.c b/drivers/staging/otus/hal/hpani.c
new file mode 100644
index 000000000000..ba95b5d012a4
--- /dev/null
+++ b/drivers/staging/otus/hal/hpani.c
@@ -0,0 +1,732 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+#include "hpani.h"
+#include "hpusb.h"
+
+
+extern u16_t zfDelayWriteInternalReg(zdev_t* dev, u32_t addr, u32_t val);
+extern u16_t zfFlushDelayWrite(zdev_t* dev);
+
+/*
+ * Anti noise immunity support.  We track phy errors and react
+ * to excessive errors by adjusting the noise immunity parameters.
+ */
+
+/******************************************************************************
+ *
+ * New Ani Algorithm for Station side only
+ *
+ *****************************************************************************/
+
+#define ZM_HAL_NOISE_IMMUNE_MAX     4   /* Max noise immunity level */
+#define ZM_HAL_SPUR_IMMUNE_MAX      7   /* Max spur immunity level */
+#define ZM_HAL_FIRST_STEP_MAX       2   /* Max first step level */
+
+#define ZM_HAL_ANI_OFDM_TRIG_HIGH       500
+#define ZM_HAL_ANI_OFDM_TRIG_LOW        200
+#define ZM_HAL_ANI_CCK_TRIG_HIGH        200
+#define ZM_HAL_ANI_CCK_TRIG_LOW         100
+#define ZM_HAL_ANI_NOISE_IMMUNE_LVL     4
+#define ZM_HAL_ANI_USE_OFDM_WEAK_SIG    TRUE
+#define ZM_HAL_ANI_CCK_WEAK_SIG_THR     FALSE
+#define ZM_HAL_ANI_SPUR_IMMUNE_LVL      7
+#define ZM_HAL_ANI_FIRSTEP_LVL          0
+#define ZM_HAL_ANI_RSSI_THR_HIGH        40
+#define ZM_HAL_ANI_RSSI_THR_LOW         7
+#define ZM_HAL_ANI_PERIOD               100
+
+#define ZM_HAL_EP_RND(x, mul) \
+    ((((x)%(mul)) >= ((mul)/2)) ? ((x) + ((mul) - 1)) / (mul) : (x)/(mul))
+
+s32_t BEACON_RSSI(zdev_t* dev)
+{
+    s32_t rssi;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    rssi = ZM_HAL_EP_RND(HpPriv->stats.ast_nodestats.ns_avgbrssi, ZM_HAL_RSSI_EP_MULTIPLIER);
+
+    return rssi;
+}
+
+/*
+ * Setup ANI handling.  Sets all thresholds and levels to default level AND
+ * resets the channel statistics
+ */
+
+void zfHpAniAttach(zdev_t* dev)
+{
+#define N(a)     (sizeof(a) / sizeof(a[0]))
+    u32_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    const int totalSizeDesired[] = { -55, -55, -55, -55, -62 };
+    const int coarseHigh[]       = { -14, -14, -14, -14, -12 };
+    const int coarseLow[]        = { -64, -64, -64, -64, -70 };
+    const int firpwr[]           = { -78, -78, -78, -78, -80 };
+
+    for (i = 0; i < 5; i++)
+    {
+        HpPriv->totalSizeDesired[i] = totalSizeDesired[i];
+        HpPriv->coarseHigh[i] = coarseHigh[i];
+        HpPriv->coarseLow[i] = coarseLow[i];
+        HpPriv->firpwr[i] = firpwr[i];
+    }
+
+    /* owl has phy counters */
+    HpPriv->hasHwPhyCounters = 1;
+
+    memset((char *)&HpPriv->ani, 0, sizeof(HpPriv->ani));
+    for (i = 0; i < N(wd->regulationTable.allowChannel); i++)
+    {
+        /* New ANI stuff */
+        HpPriv->ani[i].ofdmTrigHigh = ZM_HAL_ANI_OFDM_TRIG_HIGH;
+        HpPriv->ani[i].ofdmTrigLow = ZM_HAL_ANI_OFDM_TRIG_LOW;
+        HpPriv->ani[i].cckTrigHigh = ZM_HAL_ANI_CCK_TRIG_HIGH;
+        HpPriv->ani[i].cckTrigLow = ZM_HAL_ANI_CCK_TRIG_LOW;
+        HpPriv->ani[i].rssiThrHigh = ZM_HAL_ANI_RSSI_THR_HIGH;
+        HpPriv->ani[i].rssiThrLow = ZM_HAL_ANI_RSSI_THR_LOW;
+        HpPriv->ani[i].ofdmWeakSigDetectOff = !ZM_HAL_ANI_USE_OFDM_WEAK_SIG;
+        HpPriv->ani[i].cckWeakSigThreshold = ZM_HAL_ANI_CCK_WEAK_SIG_THR;
+        HpPriv->ani[i].spurImmunityLevel = ZM_HAL_ANI_SPUR_IMMUNE_LVL;
+        HpPriv->ani[i].firstepLevel = ZM_HAL_ANI_FIRSTEP_LVL;
+        if (HpPriv->hasHwPhyCounters)
+        {
+            HpPriv->ani[i].ofdmPhyErrBase = 0;//AR_PHY_COUNTMAX - ZM_HAL_ANI_OFDM_TRIG_HIGH;
+            HpPriv->ani[i].cckPhyErrBase = 0;//AR_PHY_COUNTMAX - ZM_HAL_ANI_CCK_TRIG_HIGH;
+        }
+    }
+    if (HpPriv->hasHwPhyCounters)
+    {
+        //zm_debug_msg2("Setting OfdmErrBase = 0x", HpPriv->ani[0].ofdmPhyErrBase);
+        //zm_debug_msg2("Setting cckErrBase = 0x", HpPriv->ani[0].cckPhyErrBase);
+        //OS_REG_WRITE(ah, AR_PHY_ERR_1, HpPriv->ani[0].ofdmPhyErrBase);
+        //OS_REG_WRITE(ah, AR_PHY_ERR_2, HpPriv->ani[0].cckPhyErrBase);
+    }
+    HpPriv->aniPeriod = ZM_HAL_ANI_PERIOD;
+    //if (ath_hal_enableANI)
+    HpPriv->procPhyErr |= ZM_HAL_PROCESS_ANI;
+
+    HpPriv->stats.ast_nodestats.ns_avgbrssi = ZM_RSSI_DUMMY_MARKER;
+    HpPriv->stats.ast_nodestats.ns_avgrssi = ZM_RSSI_DUMMY_MARKER;
+    HpPriv->stats.ast_nodestats.ns_avgtxrssi = ZM_RSSI_DUMMY_MARKER;
+#undef N
+}
+
+/*
+ * Control Adaptive Noise Immunity Parameters
+ */
+u8_t zfHpAniControl(zdev_t* dev, ZM_HAL_ANI_CMD cmd, int param)
+{
+#define N(a) (sizeof(a)/sizeof(a[0]))
+    typedef s32_t TABLE[];
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    struct zsAniState *aniState = HpPriv->curani;
+
+    switch (cmd)
+    {
+    case ZM_HAL_ANI_NOISE_IMMUNITY_LEVEL:
+    {
+        u32_t level = param;
+
+        if (level >= N(HpPriv->totalSizeDesired))
+        {
+          zm_debug_msg1("level out of range, desired level : ", level);
+          zm_debug_msg1("max level : ", N(HpPriv->totalSizeDesired));
+          return FALSE;
+        }
+
+        zfDelayWriteInternalReg(dev, AR_PHY_DESIRED_SZ,
+                (HpPriv->regPHYDesiredSZ & ~AR_PHY_DESIRED_SZ_TOT_DES)
+                | ((HpPriv->totalSizeDesired[level] << AR_PHY_DESIRED_SZ_TOT_DES_S)
+                & AR_PHY_DESIRED_SZ_TOT_DES));
+        zfDelayWriteInternalReg(dev, AR_PHY_AGC_CTL1,
+                (HpPriv->regPHYAgcCtl1 & ~AR_PHY_AGC_CTL1_COARSE_LOW)
+                | ((HpPriv->coarseLow[level] << AR_PHY_AGC_CTL1_COARSE_LOW_S)
+                & AR_PHY_AGC_CTL1_COARSE_LOW));
+        zfDelayWriteInternalReg(dev, AR_PHY_AGC_CTL1,
+                (HpPriv->regPHYAgcCtl1 & ~AR_PHY_AGC_CTL1_COARSE_HIGH)
+                | ((HpPriv->coarseHigh[level] << AR_PHY_AGC_CTL1_COARSE_HIGH_S)
+                & AR_PHY_AGC_CTL1_COARSE_HIGH));
+        zfDelayWriteInternalReg(dev, AR_PHY_FIND_SIG,
+                (HpPriv->regPHYFindSig & ~AR_PHY_FIND_SIG_FIRPWR)
+                | ((HpPriv->firpwr[level] << AR_PHY_FIND_SIG_FIRPWR_S)
+                & AR_PHY_FIND_SIG_FIRPWR));
+        zfFlushDelayWrite(dev);
+
+        if (level > aniState->noiseImmunityLevel)
+            HpPriv->stats.ast_ani_niup++;
+        else if (level < aniState->noiseImmunityLevel)
+            HpPriv->stats.ast_ani_nidown++;
+        aniState->noiseImmunityLevel = (u8_t)level;
+        break;
+    }
+    case ZM_HAL_ANI_OFDM_WEAK_SIGNAL_DETECTION:
+    {
+        const TABLE m1ThreshLow   = { 127,   50 };
+        const TABLE m2ThreshLow   = { 127,   40 };
+        const TABLE m1Thresh      = { 127, 0x4d };
+        const TABLE m2Thresh      = { 127, 0x40 };
+        const TABLE m2CountThr    = {  31,   16 };
+        const TABLE m2CountThrLow = {  63,   48 };
+        u32_t on = param ? 1 : 0;
+
+        zfDelayWriteInternalReg(dev, AR_PHY_SFCORR_LOW,
+                (HpPriv->regPHYSfcorrLow & ~AR_PHY_SFCORR_LOW_M1_THRESH_LOW)
+                | ((m1ThreshLow[on] << AR_PHY_SFCORR_LOW_M1_THRESH_LOW_S)
+                & AR_PHY_SFCORR_LOW_M1_THRESH_LOW));
+        zfDelayWriteInternalReg(dev, AR_PHY_SFCORR_LOW,
+                (HpPriv->regPHYSfcorrLow & ~AR_PHY_SFCORR_LOW_M2_THRESH_LOW)
+                | ((m2ThreshLow[on] << AR_PHY_SFCORR_LOW_M2_THRESH_LOW_S)
+                & AR_PHY_SFCORR_LOW_M2_THRESH_LOW));
+        zfDelayWriteInternalReg(dev, AR_PHY_SFCORR,
+                (HpPriv->regPHYSfcorr & ~AR_PHY_SFCORR_M1_THRESH)
+                | ((m1Thresh[on] << AR_PHY_SFCORR_M1_THRESH_S)
+                & AR_PHY_SFCORR_M1_THRESH));
+        zfDelayWriteInternalReg(dev, AR_PHY_SFCORR,
+                (HpPriv->regPHYSfcorr & ~AR_PHY_SFCORR_M2_THRESH)
+                | ((m2Thresh[on] << AR_PHY_SFCORR_M2_THRESH_S)
+                & AR_PHY_SFCORR_M2_THRESH));
+        zfDelayWriteInternalReg(dev, AR_PHY_SFCORR,
+                (HpPriv->regPHYSfcorr & ~AR_PHY_SFCORR_M2COUNT_THR)
+                | ((m2CountThr[on] << AR_PHY_SFCORR_M2COUNT_THR_S)
+                & AR_PHY_SFCORR_M2COUNT_THR));
+        zfDelayWriteInternalReg(dev, AR_PHY_SFCORR_LOW,
+                (HpPriv->regPHYSfcorrLow & ~AR_PHY_SFCORR_LOW_M2COUNT_THR_LOW)
+                | ((m2CountThrLow[on] << AR_PHY_SFCORR_LOW_M2COUNT_THR_LOW_S)
+                & AR_PHY_SFCORR_LOW_M2COUNT_THR_LOW));
+
+        if (on)
+        {
+            zfDelayWriteInternalReg(dev, AR_PHY_SFCORR_LOW,
+                    HpPriv->regPHYSfcorrLow | AR_PHY_SFCORR_LOW_USE_SELF_CORR_LOW);
+        }
+        else
+        {
+            zfDelayWriteInternalReg(dev, AR_PHY_SFCORR_LOW,
+                    HpPriv->regPHYSfcorrLow & ~AR_PHY_SFCORR_LOW_USE_SELF_CORR_LOW);
+        }
+        zfFlushDelayWrite(dev);
+        if (!on != aniState->ofdmWeakSigDetectOff)
+        {
+            if (on)
+                HpPriv->stats.ast_ani_ofdmon++;
+            else
+                HpPriv->stats.ast_ani_ofdmoff++;
+            aniState->ofdmWeakSigDetectOff = !on;
+        }
+        break;
+    }
+    case ZM_HAL_ANI_CCK_WEAK_SIGNAL_THR:
+    {
+        const TABLE weakSigThrCck = { 8, 6 };
+        u32_t high = param ? 1 : 0;
+
+        zfDelayWriteInternalReg(dev, AR_PHY_CCK_DETECT,
+                (HpPriv->regPHYCckDetect & ~AR_PHY_CCK_DETECT_WEAK_SIG_THR_CCK)
+                | ((weakSigThrCck[high] << AR_PHY_CCK_DETECT_WEAK_SIG_THR_CCK_S)
+                & AR_PHY_CCK_DETECT_WEAK_SIG_THR_CCK));
+        zfFlushDelayWrite(dev);
+        if (high != aniState->cckWeakSigThreshold)
+        {
+            if (high)
+                HpPriv->stats.ast_ani_cckhigh++;
+            else
+                HpPriv->stats.ast_ani_ccklow++;
+            aniState->cckWeakSigThreshold = (u8_t)high;
+        }
+        break;
+    }
+    case ZM_HAL_ANI_FIRSTEP_LEVEL:
+    {
+        const TABLE firstep = { 0, 4, 8 };
+        u32_t level = param;
+
+        if (level >= N(firstep))
+        {
+            zm_debug_msg1("level out of range, desired level : ", level);
+            zm_debug_msg1("max level : ", N(firstep));
+            return FALSE;
+        }
+        zfDelayWriteInternalReg(dev, AR_PHY_FIND_SIG,
+                (HpPriv->regPHYFindSig & ~AR_PHY_FIND_SIG_FIRSTEP)
+                | ((firstep[level] << AR_PHY_FIND_SIG_FIRSTEP_S)
+                & AR_PHY_FIND_SIG_FIRSTEP));
+        zfFlushDelayWrite(dev);
+        if (level > aniState->firstepLevel)
+            HpPriv->stats.ast_ani_stepup++;
+        else if (level < aniState->firstepLevel)
+            HpPriv->stats.ast_ani_stepdown++;
+        aniState->firstepLevel = (u8_t)level;
+        break;
+    }
+    case ZM_HAL_ANI_SPUR_IMMUNITY_LEVEL:
+    {
+        const TABLE cycpwrThr1 = { 2, 4, 6, 8, 10, 12, 14, 16 };
+        u32_t level = param;
+
+        if (level >= N(cycpwrThr1))
+        {
+            zm_debug_msg1("level out of range, desired level : ", level);
+            zm_debug_msg1("max level : ", N(cycpwrThr1));
+            return FALSE;
+        }
+        zfDelayWriteInternalReg(dev, AR_PHY_TIMING5,
+                (HpPriv->regPHYTiming5 & ~AR_PHY_TIMING5_CYCPWR_THR1)
+                | ((cycpwrThr1[level] << AR_PHY_TIMING5_CYCPWR_THR1_S)
+                & AR_PHY_TIMING5_CYCPWR_THR1));
+        zfFlushDelayWrite(dev);
+        if (level > aniState->spurImmunityLevel)
+            HpPriv->stats.ast_ani_spurup++;
+        else if (level < aniState->spurImmunityLevel)
+            HpPriv->stats.ast_ani_spurdown++;
+        aniState->spurImmunityLevel = (u8_t)level;
+        break;
+    }
+    case ZM_HAL_ANI_PRESENT:
+        break;
+#ifdef AH_PRIVATE_DIAG
+    case ZM_HAL_ANI_MODE:
+        if (param == 0)
+        {
+            HpPriv->procPhyErr &= ~ZM_HAL_PROCESS_ANI;
+            /* Turn off HW counters if we have them */
+            zfHpAniDetach(dev);
+            //zfHpSetRxFilter(dev, zfHpGetRxFilter(dev) &~ HAL_RX_FILTER_PHYERR);
+        }
+        else
+        {           /* normal/auto mode */
+            HpPriv->procPhyErr |= ZM_HAL_PROCESS_ANI;
+            if (HpPriv->hasHwPhyCounters)
+            {
+                //zfHpSetRxFilter(dev, zfHpGetRxFilter(dev) &~ HAL_RX_FILTER_PHYERR);
+            }
+            else
+            {
+                //zfHpSetRxFilter(dev, zfHpGetRxFilter(dev) | HAL_RX_FILTER_PHYERR);
+            }
+        }
+        break;
+    case ZM_HAL_ANI_PHYERR_RESET:
+        HpPriv->stats.ast_ani_ofdmerrs = 0;
+        HpPriv->stats.ast_ani_cckerrs = 0;
+        break;
+#endif /* AH_PRIVATE_DIAG */
+    default:
+        zm_debug_msg1("invalid cmd ", cmd);
+        return FALSE;
+    }
+    return TRUE;
+#undef  N
+}
+
+void zfHpAniRestart(zdev_t* dev)
+{
+    struct zsAniState *aniState;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    aniState = HpPriv->curani;
+
+    aniState->listenTime = 0;
+    if (HpPriv->hasHwPhyCounters)
+    {
+        //if (aniState->ofdmTrigHigh > AR_PHY_COUNTMAX)
+        //{
+        //    aniState->ofdmPhyErrBase = 0;
+        //    zm_debug_msg0("OFDM Trigger is too high for hw counters");
+        //}
+        //else
+        //    aniState->ofdmPhyErrBase = AR_PHY_COUNTMAX - aniState->ofdmTrigHigh;
+        //if (aniState->cckTrigHigh > AR_PHY_COUNTMAX)
+        //{
+        //    aniState->cckPhyErrBase = 0;
+        //    zm_debug_msg0("CCK Trigger is too high for hw counters");
+        //}
+        //else
+        //    aniState->cckPhyErrBase = AR_PHY_COUNTMAX - aniState->cckTrigHigh;
+        //zm_debug_msg2("Writing ofdmbase = 0x", aniState->ofdmPhyErrBase);
+        //zm_debug_msg2("Writing cckbase = 0x", aniState->cckPhyErrBase);
+        //OS_REG_WRITE(ah, AR_PHY_ERR_1, aniState->ofdmPhyErrBase);
+        //OS_REG_WRITE(ah, AR_PHY_ERR_2, aniState->cckPhyErrBase);
+        //OS_REG_WRITE(ah, AR_PHY_ERR_MASK_1, AR_PHY_ERR_OFDM_TIMING);
+        //OS_REG_WRITE(ah, AR_PHY_ERR_MASK_2, AR_PHY_ERR_CCK_TIMING);
+        aniState->ofdmPhyErrBase = 0;
+        aniState->cckPhyErrBase = 0;
+    }
+    aniState->ofdmPhyErrCount = 0;
+    aniState->cckPhyErrCount = 0;
+}
+
+void zfHpAniOfdmErrTrigger(zdev_t* dev)
+{
+    struct zsAniState *aniState;
+    s32_t rssi;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    //HALASSERT(chan != NULL);
+
+    if ((HpPriv->procPhyErr & ZM_HAL_PROCESS_ANI) == 0)
+        return;
+
+    aniState = HpPriv->curani;
+    /* First, raise noise immunity level, up to max */
+    if (aniState->noiseImmunityLevel < ZM_HAL_NOISE_IMMUNE_MAX)
+    {
+        zfHpAniControl(dev, ZM_HAL_ANI_NOISE_IMMUNITY_LEVEL, aniState->noiseImmunityLevel + 1);
+        return;
+    }
+    /* then, raise spur immunity level, up to max */
+    if (aniState->spurImmunityLevel < ZM_HAL_SPUR_IMMUNE_MAX)
+    {
+        zfHpAniControl(dev, ZM_HAL_ANI_SPUR_IMMUNITY_LEVEL, aniState->spurImmunityLevel + 1);
+        return;
+    }
+    rssi = BEACON_RSSI(dev);
+    if (rssi > aniState->rssiThrHigh)
+    {
+        /*
+         * Beacon rssi is high, can turn off ofdm weak sig detect.
+         */
+        if (!aniState->ofdmWeakSigDetectOff)
+        {
+            zfHpAniControl(dev, ZM_HAL_ANI_OFDM_WEAK_SIGNAL_DETECTION, FALSE);
+            zfHpAniControl(dev, ZM_HAL_ANI_SPUR_IMMUNITY_LEVEL, 0);
+            return;
+        }
+        /*
+         * If weak sig detect is already off, as last resort, raise
+         * first step level
+         */
+        if (aniState->firstepLevel < ZM_HAL_FIRST_STEP_MAX)
+        {
+            zfHpAniControl(dev, ZM_HAL_ANI_FIRSTEP_LEVEL, aniState->firstepLevel + 1);
+            return;
+        }
+    }
+    else if (rssi > aniState->rssiThrLow)
+    {
+        /*
+         * Beacon rssi in mid range, need ofdm weak signal detect,
+         * but we can raise firststepLevel
+         */
+        if (aniState->ofdmWeakSigDetectOff)
+            zfHpAniControl(dev, ZM_HAL_ANI_OFDM_WEAK_SIGNAL_DETECTION, TRUE);
+        if (aniState->firstepLevel < ZM_HAL_FIRST_STEP_MAX)
+            zfHpAniControl(dev, ZM_HAL_ANI_FIRSTEP_LEVEL, aniState->firstepLevel + 1);
+        return;
+    }
+    else
+    {
+        /*
+         * Beacon rssi is low, if in 11b/g mode, turn off ofdm
+         * weak sign detction and zero firstepLevel to maximize
+         * CCK sensitivity
+         */
+        if (wd->frequency < 3000)
+        {
+            if (!aniState->ofdmWeakSigDetectOff)
+                zfHpAniControl(dev, ZM_HAL_ANI_OFDM_WEAK_SIGNAL_DETECTION, FALSE);
+            if (aniState->firstepLevel > 0)
+                zfHpAniControl(dev, ZM_HAL_ANI_FIRSTEP_LEVEL, 0);
+            return;
+        }
+    }
+}
+
+void zfHpAniCckErrTrigger(zdev_t* dev)
+{
+    struct zsAniState *aniState;
+    s32_t rssi;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    //HALASSERT(chan != NULL);
+
+    if ((HpPriv->procPhyErr & ZM_HAL_PROCESS_ANI) == 0)
+        return;
+
+    /* first, raise noise immunity level, up to max */
+    aniState = HpPriv->curani;
+    if (aniState->noiseImmunityLevel < ZM_HAL_NOISE_IMMUNE_MAX)
+    {
+        zfHpAniControl(dev, ZM_HAL_ANI_NOISE_IMMUNITY_LEVEL,
+                 aniState->noiseImmunityLevel + 1);
+        return;
+    }
+    rssi = BEACON_RSSI(dev);
+    if (rssi >  aniState->rssiThrLow)
+    {
+        /*
+         * Beacon signal in mid and high range, raise firsteplevel.
+         */
+        if (aniState->firstepLevel < ZM_HAL_FIRST_STEP_MAX)
+            zfHpAniControl(dev, ZM_HAL_ANI_FIRSTEP_LEVEL, aniState->firstepLevel + 1);
+    }
+    else
+    {
+        /*
+         * Beacon rssi is low, zero firstepLevel to maximize
+         * CCK sensitivity.
+         */
+        if (wd->frequency < 3000)
+        {
+            if (aniState->firstepLevel > 0)
+                zfHpAniControl(dev, ZM_HAL_ANI_FIRSTEP_LEVEL, 0);
+        }
+    }
+}
+
+void zfHpAniLowerImmunity(zdev_t* dev)
+{
+    struct zsAniState *aniState;
+    s32_t rssi;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    aniState = HpPriv->curani;
+
+    rssi = BEACON_RSSI(dev);
+    if (rssi > aniState->rssiThrHigh)
+    {
+        /*
+         * Beacon signal is high, leave ofdm weak signal detection off
+         * or it may oscillate. Let it fall through.
+         */
+    }
+    else if (rssi > aniState->rssiThrLow)
+    {
+        /*
+         * Beacon rssi in mid range, turn on ofdm weak signal
+         * detection or lower first step level.
+         */
+        if (aniState->ofdmWeakSigDetectOff)
+        {
+            zfHpAniControl(dev, ZM_HAL_ANI_OFDM_WEAK_SIGNAL_DETECTION, TRUE);
+            return;
+        }
+        if (aniState->firstepLevel > 0)
+        {
+            zfHpAniControl(dev, ZM_HAL_ANI_FIRSTEP_LEVEL, aniState->firstepLevel - 1);
+            return;
+        }
+    }
+    else
+    {
+        /*
+         * Beacon rssi is low, reduce first step level.
+         */
+        if (aniState->firstepLevel > 0)
+        {
+            zfHpAniControl(dev, ZM_HAL_ANI_FIRSTEP_LEVEL, aniState->firstepLevel - 1);
+            return;
+        }
+    }
+    /* then lower spur immunity level, down to zero */
+    if (aniState->spurImmunityLevel > 0)
+    {
+        zfHpAniControl(dev, ZM_HAL_ANI_SPUR_IMMUNITY_LEVEL, aniState->spurImmunityLevel - 1);
+        return;
+    }
+    /*
+     * if all else fails, lower noise immunity level down to a min value
+     * zero for now
+     */
+    if (aniState->noiseImmunityLevel > 0)
+    {
+        zfHpAniControl(dev, ZM_HAL_ANI_NOISE_IMMUNITY_LEVEL, aniState->noiseImmunityLevel - 1);
+        return;
+    }
+}
+
+#define CLOCK_RATE 44000    /* XXX use mac_usec or similar */
+/* convert HW counter values to ms using 11g clock rate, goo9d enough
+   for 11a and Turbo */
+
+/*
+ * Return an approximation of the time spent ``listening'' by
+ * deducting the cycles spent tx'ing and rx'ing from the total
+ * cycle count since our last call.  A return value <0 indicates
+ * an invalid/inconsistent time.
+ */
+s32_t zfHpAniGetListenTime(zdev_t* dev)
+{
+    struct zsAniState *aniState;
+    u32_t txFrameCount, rxFrameCount, cycleCount;
+    s32_t listenTime;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    txFrameCount = 0;//OS_REG_READ(ah, AR_TFCNT);
+    rxFrameCount = 0;//OS_REG_READ(ah, AR_RFCNT);
+    cycleCount = 0;//OS_REG_READ(ah, AR_CCCNT);
+
+    aniState = HpPriv->curani;
+    if (aniState->cycleCount == 0 || aniState->cycleCount > cycleCount)
+    {
+        /*
+         * Cycle counter wrap (or initial call); it's not possible
+         * to accurately calculate a value because the registers
+         * right shift rather than wrap--so punt and return 0.
+         */
+        listenTime = 0;
+        HpPriv->stats.ast_ani_lzero++;
+    }
+    else
+    {
+        s32_t ccdelta = cycleCount - aniState->cycleCount;
+        s32_t rfdelta = rxFrameCount - aniState->rxFrameCount;
+        s32_t tfdelta = txFrameCount - aniState->txFrameCount;
+        listenTime = (ccdelta - rfdelta - tfdelta) / CLOCK_RATE;
+    }
+    aniState->cycleCount = cycleCount;
+    aniState->txFrameCount = txFrameCount;
+    aniState->rxFrameCount = rxFrameCount;
+    return listenTime;
+}
+
+/*
+ * Do periodic processing.  This routine is called from the
+ * driver's rx interrupt handler after processing frames.
+ */
+void zfHpAniArPoll(zdev_t* dev, u32_t listenTime, u32_t phyCnt1, u32_t phyCnt2)
+{
+    struct zsAniState *aniState;
+    //s32_t listenTime;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    /*
+     * Since we're called from end of rx tasklet, we also check for
+     * AR processing now
+     */
+
+    aniState = HpPriv->curani;
+    //HpPriv->stats.ast_nodestats = *stats;       /* XXX optimize? */
+
+    //listenTime = zfHpAniGetListenTime(dev);
+    //if (listenTime < 0)
+    //{
+    //    HpPriv->stats.ast_ani_lneg++;
+    //    /* restart ANI period if listenTime is invalid */
+    //    zfHpAniRestart(dev);
+    //    return;
+    //}
+    /* XXX beware of overflow? */
+    aniState->listenTime += listenTime;
+
+    if (HpPriv->hasHwPhyCounters)
+    {
+        //u32_t phyCnt1, phyCnt2;
+        u32_t ofdmPhyErrCnt, cckPhyErrCnt;
+
+        /* NB: these are not reset-on-read */
+        //phyCnt1 = 0;//OS_REG_READ(ah, AR_PHY_ERR_1);
+        //phyCnt2 = 0;//OS_REG_READ(ah, AR_PHY_ERR_2);
+        /* XXX sometimes zero, why? */
+        //if (phyCnt1 < aniState->ofdmPhyErrBase ||
+        //    phyCnt2 < aniState->cckPhyErrBase)
+        //{
+        //    if (phyCnt1 < aniState->ofdmPhyErrBase)
+        //    {
+        //        zm_debug_msg2("phyCnt1 = 0x", phyCnt1);
+        //        zm_debug_msg2("resetting counter value to 0x", aniState->ofdmPhyErrBase);
+        //        //OS_REG_WRITE(ah, AR_PHY_ERR_1, aniState->ofdmPhyErrBase);
+        //        //OS_REG_WRITE(ah, AR_PHY_ERR_MASK_1, AR_PHY_ERR_OFDM_TIMING);
+        //    }
+        //    if (phyCnt2 < aniState->cckPhyErrBase)
+        //    {
+        //        zm_debug_msg2("phyCnt2 = 0x", phyCnt2);
+        //        zm_debug_msg2("resetting counter value to 0x", aniState->cckPhyErrBase);
+        //        //OS_REG_WRITE(ah, AR_PHY_ERR_2, aniState->cckPhyErrBase);
+        //        //OS_REG_WRITE(ah, AR_PHY_ERR_MASK_2, AR_PHY_ERR_CCK_TIMING);
+        //    }
+        //    return;     /* XXX */
+        //}
+        /* NB: only use ast_ani_*errs with AH_PRIVATE_DIAG */
+        //ofdmPhyErrCnt = phyCnt1 - aniState->ofdmPhyErrBase;
+        //HpPriv->stats.ast_ani_ofdmerrs += ofdmPhyErrCnt - aniState->ofdmPhyErrCount;
+        //aniState->ofdmPhyErrCount = ofdmPhyErrCnt;
+        ofdmPhyErrCnt = phyCnt1;
+        HpPriv->stats.ast_ani_ofdmerrs += ofdmPhyErrCnt;
+        aniState->ofdmPhyErrCount += ofdmPhyErrCnt;
+
+        //cckPhyErrCnt = phyCnt2 - aniState->cckPhyErrBase;
+        //HpPriv->stats.ast_ani_cckerrs += cckPhyErrCnt - aniState->cckPhyErrCount;
+        //aniState->cckPhyErrCount = cckPhyErrCnt;
+        cckPhyErrCnt = phyCnt2;
+        HpPriv->stats.ast_ani_cckerrs += cckPhyErrCnt;
+        aniState->cckPhyErrCount += cckPhyErrCnt;
+    }
+    /*
+     * If ani is not enabled, return after we've collected
+     * statistics
+     */
+    if ((HpPriv->procPhyErr & ZM_HAL_PROCESS_ANI) == 0)
+        return;
+    if (aniState->listenTime > 5 * HpPriv->aniPeriod)
+    {
+        /*
+         * Check to see if need to lower immunity if
+         * 5 aniPeriods have passed
+         */
+        if (aniState->ofdmPhyErrCount <= aniState->listenTime *
+             aniState->ofdmTrigLow/1000 &&
+            aniState->cckPhyErrCount <= aniState->listenTime *
+             aniState->cckTrigLow/1000)
+            zfHpAniLowerImmunity(dev);
+        zfHpAniRestart(dev);
+    }
+    else if (aniState->listenTime > HpPriv->aniPeriod)
+    {
+        /* check to see if need to raise immunity */
+        if (aniState->ofdmPhyErrCount > aniState->listenTime *
+            aniState->ofdmTrigHigh / 1000)
+        {
+            zfHpAniOfdmErrTrigger(dev);
+            zfHpAniRestart(dev);
+        }
+        else if (aniState->cckPhyErrCount > aniState->listenTime *
+               aniState->cckTrigHigh / 1000)
+        {
+            zfHpAniCckErrTrigger(dev);
+            zfHpAniRestart(dev);
+        }
+    }
+}
diff --git a/drivers/staging/otus/hal/hpani.h b/drivers/staging/otus/hal/hpani.h
new file mode 100644
index 000000000000..96e69af3c685
--- /dev/null
+++ b/drivers/staging/otus/hal/hpani.h
@@ -0,0 +1,420 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+
+typedef struct {
+	u32_t	ackrcv_bad;
+	u32_t	rts_bad;
+	u32_t	rts_good;
+	u32_t	fcs_bad;
+	u32_t	beacons;
+} ZM_HAL_MIB_STATS;
+
+/*
+ * Per-node statistics maintained by the driver for use in
+ * optimizing signal quality and other operational aspects.
+ */
+typedef struct {
+	u32_t	ns_avgbrssi;	/* average beacon rssi */
+	u32_t	ns_avgrssi;	    /* average data rssi */
+	u32_t	ns_avgtxrssi;	/* average tx rssi */
+} ZM_HAL_NODE_STATS;
+
+#define	ZM_HAL_RSSI_EP_MULTIPLIER	(1<<7)	/* pow2 to optimize out * and / */
+
+struct zsAniStats {
+    u32_t   ast_ani_niup;   /* ANI increased noise immunity */
+    u32_t   ast_ani_nidown; /* ANI decreased noise immunity */
+    u32_t   ast_ani_spurup; /* ANI increased spur immunity */
+    u32_t   ast_ani_spurdown;/* ANI descreased spur immunity */
+    u32_t   ast_ani_ofdmon; /* ANI OFDM weak signal detect on */
+    u32_t   ast_ani_ofdmoff;/* ANI OFDM weak signal detect off */
+    u32_t   ast_ani_cckhigh;/* ANI CCK weak signal threshold high */
+    u32_t   ast_ani_ccklow; /* ANI CCK weak signal threshold low */
+    u32_t   ast_ani_stepup; /* ANI increased first step level */
+    u32_t   ast_ani_stepdown;/* ANI decreased first step level */
+    u32_t   ast_ani_ofdmerrs;/* ANI cumulative ofdm phy err count */
+    u32_t   ast_ani_cckerrs;/* ANI cumulative cck phy err count */
+    u32_t   ast_ani_reset;  /* ANI parameters zero'd for non-STA */
+    u32_t   ast_ani_lzero;  /* ANI listen time forced to zero */
+    u32_t   ast_ani_lneg;   /* ANI listen time calculated < 0 */
+    ZM_HAL_MIB_STATS   ast_mibstats;   /* MIB counter stats */
+    ZM_HAL_NODE_STATS  ast_nodestats;  /* Latest rssi stats from driver */
+};
+
+/*
+ * Per-channel ANI state private to the driver.
+ */
+struct zsAniState {
+    ZM_HAL_CHANNEL c;
+    u8_t    noiseImmunityLevel;
+    u8_t    spurImmunityLevel;
+    u8_t    firstepLevel;
+    u8_t    ofdmWeakSigDetectOff;
+    u8_t    cckWeakSigThreshold;
+
+    /* Thresholds */
+    u32_t   listenTime;
+    u32_t   ofdmTrigHigh;
+    u32_t   ofdmTrigLow;
+    s32_t   cckTrigHigh;
+    s32_t   cckTrigLow;
+    s32_t   rssiThrLow;
+    s32_t   rssiThrHigh;
+
+    u32_t   noiseFloor; /* The current noise floor */
+    u32_t   txFrameCount;   /* Last txFrameCount */
+    u32_t   rxFrameCount;   /* Last rx Frame count */
+    u32_t   cycleCount; /* Last cycleCount (can detect wrap-around) */
+    u32_t   ofdmPhyErrCount;/* OFDM err count since last reset */
+    u32_t   cckPhyErrCount; /* CCK err count since last reset */
+    u32_t   ofdmPhyErrBase; /* Base value for ofdm err counter */
+    u32_t   cckPhyErrBase;  /* Base value for cck err counters */
+    s16_t   pktRssi[2]; /* Average rssi of pkts for 2 antennas */
+    s16_t   ofdmErrRssi[2]; /* Average rssi of ofdm phy errs for 2 ant */
+    s16_t   cckErrRssi[2];  /* Average rssi of cck phy errs for 2 ant */
+};
+
+typedef enum {
+	ZM_HAL_ANI_PRESENT,			/* is ANI support present */
+	ZM_HAL_ANI_NOISE_IMMUNITY_LEVEL,		/* set level */
+	ZM_HAL_ANI_OFDM_WEAK_SIGNAL_DETECTION,	/* enable/disable */
+	ZM_HAL_ANI_CCK_WEAK_SIGNAL_THR,		/* enable/disable */
+	ZM_HAL_ANI_FIRSTEP_LEVEL,			/* set level */
+	ZM_HAL_ANI_SPUR_IMMUNITY_LEVEL,		/* set level */
+	ZM_HAL_ANI_MODE,				/* 0 => manual, 1 => auto */
+	ZM_HAL_ANI_PHYERR_RESET,			/* reset phy error stats */
+} ZM_HAL_ANI_CMD;
+
+#define AR_PHY_COUNTMAX        (3 << 22)         // Max counted before intr
+#define ZM_HAL_PROCESS_ANI     0x00000001  /* ANI state setup */
+#define ZM_RSSI_DUMMY_MARKER	0x127
+
+/* PHY registers in ar5416, related base and register offsets
+   may need to be changed in otus BB */
+#define AR_PHY_BASE     0x1C5800      /* base address of phy regs */
+#define AR_PHY(_n)      (AR_PHY_BASE + ((_n)<<2))
+
+#define AR_PHY_TEST             0x1C5800          /* PHY test control */
+#define PHY_AGC_CLR             0x10000000      /* disable AGC to A2 */
+#define RFSILENT_BB             0x00002000      /* shush bb */
+
+#define AR_PHY_TURBO        0x1C5804      /* frame control register */
+#define AR_PHY_FC_TURBO_MODE        0x00000001  /* Set turbo mode bits */
+#define AR_PHY_FC_TURBO_SHORT       0x00000002  /* Set short symbols to turbo mode setting */
+#define AR_PHY_FC_DYN2040_EN        0x00000004      /* Enable dyn 20/40 mode */
+#define AR_PHY_FC_DYN2040_PRI_ONLY      0x00000008      /* dyn 20/40 - primary only */
+#define AR_PHY_FC_DYN2040_PRI_CH    0x00000010      /* dyn 20/40 - primary ch offset (0=+10MHz, 1=-10MHz)*/
+#define AR_PHY_FC_DYN2040_EXT_CH        0x00000020      /* dyn 20/40 - ext ch spacing (0=20MHz/ 1=25MHz) */
+#define AR_PHY_FC_HT_EN             0x00000040      /* ht enable */
+#define AR_PHY_FC_SHORT_GI_40       0x00000080      /* allow short GI for HT 40 */
+#define AR_PHY_FC_WALSH             0x00000100      /* walsh spatial spreading for 2 chains,2 streams TX */
+#define AR_PHY_FC_SINGLE_HT_LTF1        0x00000200      /* single length (4us) 1st HT long training symbol */
+
+#define AR_PHY_TIMING2      0x1C5810      /* Timing Control 2 */
+#define AR_PHY_TIMING2_USE_FORCE    0x00001000
+#define AR_PHY_TIMING2_FORCE_VAL    0x00000fff
+
+#define AR_PHY_TIMING3      0x1C5814      /* Timing control 3 */
+#define AR_PHY_TIMING3_DSC_MAN  0xFFFE0000
+#define AR_PHY_TIMING3_DSC_MAN_S    17
+#define AR_PHY_TIMING3_DSC_EXP  0x0001E000
+#define AR_PHY_TIMING3_DSC_EXP_S    13
+
+#define AR_PHY_CHIP_ID          0x1C5818      /* PHY chip revision ID */
+#define AR_PHY_CHIP_ID_REV_0    0x80        /* 5416 Rev 0 (owl 1.0) BB */
+#define AR_PHY_CHIP_ID_REV_1    0x81        /* 5416 Rev 1 (owl 2.0) BB */
+
+#define AR_PHY_ACTIVE       0x1C581C      /* activation register */
+#define AR_PHY_ACTIVE_EN    0x00000001  /* Activate PHY chips */
+#define AR_PHY_ACTIVE_DIS   0x00000000  /* Deactivate PHY chips */
+
+#define AR_PHY_RF_CTL2                      0x1C5824
+#define AR_PHY_TX_END_DATA_START  0x000000FF
+#define AR_PHY_TX_END_DATA_START_S  0
+#define AR_PHY_TX_END_PA_ON       0x0000FF00
+#define AR_PHY_TX_END_PA_ON_S       8
+
+
+#define AR_PHY_RF_CTL3                  0x1C5828
+#define AR_PHY_TX_END_TO_A2_RX_ON       0x00FF0000
+#define AR_PHY_TX_END_TO_A2_RX_ON_S     16
+
+#define AR_PHY_ADC_CTL      0x1C582C
+#define AR_PHY_ADC_CTL_OFF_INBUFGAIN    0x00000003
+#define AR_PHY_ADC_CTL_OFF_INBUFGAIN_S  0
+#define AR_PHY_ADC_CTL_OFF_PWDDAC   0x00002000
+#define AR_PHY_ADC_CTL_OFF_PWDBANDGAP   0x00004000 /* BB Rev 4.2+ only */
+#define AR_PHY_ADC_CTL_OFF_PWDADC   0x00008000 /* BB Rev 4.2+ only */
+#define AR_PHY_ADC_CTL_ON_INBUFGAIN 0x00030000
+#define AR_PHY_ADC_CTL_ON_INBUFGAIN_S   16
+
+#define AR_PHY_ADC_SERIAL_CTL       0x1C5830
+#define AR_PHY_SEL_INTERNAL_ADDAC   0x00000000
+#define AR_PHY_SEL_EXTERNAL_RADIO   0x00000001
+
+#define AR_PHY_RF_CTL4                    0x1C5834
+#define AR_PHY_RF_CTL4_TX_END_XPAB_OFF    0xFF000000
+#define AR_PHY_RF_CTL4_TX_END_XPAB_OFF_S  24
+#define AR_PHY_RF_CTL4_TX_END_XPAA_OFF    0x00FF0000
+#define AR_PHY_RF_CTL4_TX_END_XPAA_OFF_S  16
+#define AR_PHY_RF_CTL4_FRAME_XPAB_ON      0x0000FF00
+#define AR_PHY_RF_CTL4_FRAME_XPAB_ON_S    8
+#define AR_PHY_RF_CTL4_FRAME_XPAA_ON      0x000000FF
+#define AR_PHY_RF_CTL4_FRAME_XPAA_ON_S    0
+
+#define AR_PHY_SETTLING     0x1C5844
+#define AR_PHY_SETTLING_SWITCH  0x00003F80
+#define AR_PHY_SETTLING_SWITCH_S    7
+
+#define AR_PHY_RXGAIN       0x1C5848
+#define AR_PHY_RXGAIN_TXRX_ATTEN    0x0003F000
+#define AR_PHY_RXGAIN_TXRX_ATTEN_S  12
+#define AR_PHY_RXGAIN_TXRX_RF_MAX   0x007C0000
+#define AR_PHY_RXGAIN_TXRX_RF_MAX_S 18
+
+#define AR_PHY_DESIRED_SZ   0x1C5850
+#define AR_PHY_DESIRED_SZ_ADC       0x000000FF
+#define AR_PHY_DESIRED_SZ_ADC_S     0
+#define AR_PHY_DESIRED_SZ_PGA       0x0000FF00
+#define AR_PHY_DESIRED_SZ_PGA_S     8
+#define AR_PHY_DESIRED_SZ_TOT_DES   0x0FF00000
+#define AR_PHY_DESIRED_SZ_TOT_DES_S 20
+
+#define AR_PHY_FIND_SIG      0x1C5858
+#define AR_PHY_FIND_SIG_FIRSTEP  0x0003F000
+#define AR_PHY_FIND_SIG_FIRSTEP_S        12
+#define AR_PHY_FIND_SIG_FIRPWR   0x03FC0000
+#define AR_PHY_FIND_SIG_FIRPWR_S         18
+
+#define AR_PHY_AGC_CTL1      0x1C585C
+#define AR_PHY_AGC_CTL1_COARSE_LOW       0x00007F80
+#define AR_PHY_AGC_CTL1_COARSE_LOW_S         7
+#define AR_PHY_AGC_CTL1_COARSE_HIGH      0x003F8000
+#define AR_PHY_AGC_CTL1_COARSE_HIGH_S        15
+
+#define AR_PHY_AGC_CONTROL  0x1C5860      /* chip calibration and noise floor setting */
+#define AR_PHY_AGC_CONTROL_CAL  0x00000001  /* do internal calibration */
+#define AR_PHY_AGC_CONTROL_NF   0x00000002  /* do noise-floor calculation */
+
+#define AR_PHY_CCA              0x1C5864
+#define AR_PHY_MINCCA_PWR       0x1FF00000
+#define AR_PHY_MINCCA_PWR_S     19
+#define AR_PHY_CCA_THRESH62     0x0007F000
+#define AR_PHY_CCA_THRESH62_S   12
+
+#define AR_PHY_SFCORR_LOW    0x1C586C
+#define AR_PHY_SFCORR_LOW_USE_SELF_CORR_LOW  0x00000001
+#define AR_PHY_SFCORR_LOW_M2COUNT_THR_LOW    0x00003F00
+#define AR_PHY_SFCORR_LOW_M2COUNT_THR_LOW_S  8
+#define AR_PHY_SFCORR_LOW_M1_THRESH_LOW  0x001FC000
+#define AR_PHY_SFCORR_LOW_M1_THRESH_LOW_S    14
+#define AR_PHY_SFCORR_LOW_M2_THRESH_LOW  0x0FE00000
+#define AR_PHY_SFCORR_LOW_M2_THRESH_LOW_S    21
+
+#define AR_PHY_SFCORR       0x1C5868
+#define AR_PHY_SFCORR_M2COUNT_THR    0x0000001F
+#define AR_PHY_SFCORR_M2COUNT_THR_S  0
+#define AR_PHY_SFCORR_M1_THRESH  0x00FE0000
+#define AR_PHY_SFCORR_M1_THRESH_S    17
+#define AR_PHY_SFCORR_M2_THRESH  0x7F000000
+#define AR_PHY_SFCORR_M2_THRESH_S    24
+
+#define AR_PHY_SLEEP_CTR_CONTROL    0x1C5870
+#define AR_PHY_SLEEP_CTR_LIMIT      0x1C5874
+#define AR_PHY_SLEEP_SCAL       0x1C5878
+
+#define AR_PHY_PLL_CTL          0x1C587c      /* PLL control register */
+#define AR_PHY_PLL_CTL_40       0xaa        /* 40 MHz */
+#define AR_PHY_PLL_CTL_40_5413  0x04
+#define AR_PHY_PLL_CTL_44       0xab        /* 44 MHz for 11b, 11g */
+#define AR_PHY_PLL_CTL_44_2133  0xeb        /* 44 MHz for 11b, 11g */
+#define AR_PHY_PLL_CTL_40_2133  0xea        /* 40 MHz for 11a, turbos */
+
+#define AR_PHY_RX_DELAY     0x1C5914      /* analog pow-on time (100ns) */
+#define AR_PHY_RX_DELAY_DELAY   0x00003FFF  /* delay from wakeup to rx ena */
+
+#define AR_PHY_TIMING_CTRL4     0x1C5920      /* timing control */
+#define AR_PHY_TIMING_CTRL4_IQCORR_Q_Q_COFF 0x01F   /* Mask for kcos_theta-1 for q correction */
+#define AR_PHY_TIMING_CTRL4_IQCORR_Q_Q_COFF_S   0   /* shift for Q_COFF */
+#define AR_PHY_TIMING_CTRL4_IQCORR_Q_I_COFF 0x7E0   /* Mask for sin_theta for i correction */
+#define AR_PHY_TIMING_CTRL4_IQCORR_Q_I_COFF_S   5   /* Shift for sin_theta for i correction */
+#define AR_PHY_TIMING_CTRL4_IQCORR_ENABLE   0x800   /* enable IQ correction */
+#define AR_PHY_TIMING_CTRL4_IQCAL_LOG_COUNT_MAX 0xF000  /* Mask for max number of samples (logarithmic) */
+#define AR_PHY_TIMING_CTRL4_IQCAL_LOG_COUNT_MAX_S   12  /* Shift for max number of samples */
+#define AR_PHY_TIMING_CTRL4_DO_IQCAL    0x10000     /* perform IQ calibration */
+
+#define AR_PHY_TIMING5      0x1C5924
+#define AR_PHY_TIMING5_CYCPWR_THR1  0x000000FE
+#define AR_PHY_TIMING5_CYCPWR_THR1_S    1
+
+#define AR_PHY_POWER_TX_RATE1   0x1C5934
+#define AR_PHY_POWER_TX_RATE2   0x1C5938
+#define AR_PHY_POWER_TX_RATE_MAX    0x1C593c
+#define AR_PHY_POWER_TX_RATE_MAX_TPC_ENABLE 0x00000040
+
+#define AR_PHY_FRAME_CTL    0x1C5944
+#define AR_PHY_FRAME_CTL_TX_CLIP    0x00000038
+#define AR_PHY_FRAME_CTL_TX_CLIP_S  3
+
+#define AR_PHY_TXPWRADJ     0x1C594C      /* BB Rev 4.2+ only */
+#define AR_PHY_TXPWRADJ_CCK_GAIN_DELTA  0x00000FC0
+#define AR_PHY_TXPWRADJ_CCK_GAIN_DELTA_S    6
+#define AR_PHY_TXPWRADJ_CCK_PCDAC_INDEX 0x00FC0000
+#define AR_PHY_TXPWRADJ_CCK_PCDAC_INDEX_S   18
+
+#define AR_PHY_RADAR_0      0x1C5954      /* radar detection settings */
+#define AR_PHY_RADAR_0_ENA  0x00000001  /* Enable radar detection */
+#define AR_PHY_RADAR_0_INBAND   0x0000003e  /* Inband pulse threshold */
+#define AR_PHY_RADAR_0_INBAND_S 1
+#define AR_PHY_RADAR_0_PRSSI    0x00000FC0  /* Pulse rssi threshold */
+#define AR_PHY_RADAR_0_PRSSI_S  6
+#define AR_PHY_RADAR_0_HEIGHT   0x0003F000  /* Pulse height threshold */
+#define AR_PHY_RADAR_0_HEIGHT_S 12
+#define AR_PHY_RADAR_0_RRSSI    0x00FC0000  /* Radar rssi threshold */
+#define AR_PHY_RADAR_0_RRSSI_S  18
+#define AR_PHY_RADAR_0_FIRPWR   0x7F000000  /* Radar firpwr threshold */
+#define AR_PHY_RADAR_0_FIRPWR_S 24
+
+#define AR_PHY_SWITCH_CHAIN_0     0x1C5960
+#define AR_PHY_SWITCH_COM         0x1C5964
+
+#define AR_PHY_SIGMA_DELTA  0x1C596C      /* AR5312 only */
+#define AR_PHY_SIGMA_DELTA_ADC_SEL  0x00000003
+#define AR_PHY_SIGMA_DELTA_ADC_SEL_S    0
+#define AR_PHY_SIGMA_DELTA_FILT2    0x000000F8
+#define AR_PHY_SIGMA_DELTA_FILT2_S  3
+#define AR_PHY_SIGMA_DELTA_FILT1    0x00001F00
+#define AR_PHY_SIGMA_DELTA_FILT1_S  8
+#define AR_PHY_SIGMA_DELTA_ADC_CLIP 0x01FFE000
+#define AR_PHY_SIGMA_DELTA_ADC_CLIP_S   13
+
+#define AR_PHY_RESTART      0x1C5970      /* restart */
+#define AR_PHY_RESTART_DIV_GC   0x001C0000  /* bb_ant_fast_div_gc_limit */
+#define AR_PHY_RESTART_DIV_GC_S 18
+
+#define AR_PHY_RFBUS_REQ        0x1C597C
+#define AR_PHY_RFBUS_REQ_EN     0x00000001
+
+#define AR_PHY_RX_CHAINMASK     0x1C59a4
+
+#define AR_PHY_EXT_CCA          0x1C59bc
+#define AR_PHY_EXT_MINCCA_PWR   0xFF800000
+#define AR_PHY_EXT_MINCCA_PWR_S 23
+
+#define AR_PHY_HALFGI           0x1C59D0      /* Timing control 3 */
+#define AR_PHY_HALFGI_DSC_MAN   0x0007FFF0
+#define AR_PHY_HALFGI_DSC_MAN_S 4
+#define AR_PHY_HALFGI_DSC_EXP   0x0000000F
+#define AR_PHY_HALFGI_DSC_EXP_S 0
+
+#define AR_PHY_HEAVY_CLIP_ENABLE    0x1C59E0
+
+#define AR_PHY_M_SLEEP      0x1C59f0      /* sleep control registers */
+#define AR_PHY_REFCLKDLY    0x1C59f4
+#define AR_PHY_REFCLKPD     0x1C59f8
+
+/* PHY IQ calibration results */
+#define AR_PHY_IQCAL_RES_PWR_MEAS_I 0x1C5C10  /* power measurement for I */
+#define AR_PHY_IQCAL_RES_PWR_MEAS_Q 0x1C5C14  /* power measurement for Q */
+#define AR_PHY_IQCAL_RES_IQ_CORR_MEAS   0x1C5C18  /* IQ correlation measurement */
+
+#define AR_PHY_CURRENT_RSSI 0x1C5C1c      /* rssi of current frame rx'd */
+
+#define AR_PHY_RFBUS_GRANT       0x1C5C20
+#define AR_PHY_RFBUS_GRANT_EN    0x00000001
+
+#define AR_PHY_MODE     0x1C6200  /* Mode register */
+#define AR_PHY_MODE_AR2133  0x08    /* AR2133 */
+#define AR_PHY_MODE_AR5111  0x00    /* AR5111/AR2111 */
+#define AR_PHY_MODE_AR5112  0x08    /* AR5112*/
+#define AR_PHY_MODE_DYNAMIC 0x04    /* dynamic CCK/OFDM mode */
+#define AR_PHY_MODE_RF2GHZ  0x02    /* 2.4 GHz */
+#define AR_PHY_MODE_RF5GHZ  0x00    /* 5 GHz */
+#define AR_PHY_MODE_CCK     0x01    /* CCK */
+#define AR_PHY_MODE_OFDM    0x00    /* OFDM */
+
+#define AR_PHY_CCK_TX_CTRL  0x1C6204
+#define AR_PHY_CCK_TX_CTRL_JAPAN    0x00000010
+
+#define AR_PHY_CCK_DETECT                           0x1C6208
+#define AR_PHY_CCK_DETECT_WEAK_SIG_THR_CCK          0x0000003F
+#define AR_PHY_CCK_DETECT_WEAK_SIG_THR_CCK_S        0
+#define AR_PHY_CCK_DETECT_ANT_SWITCH_TIME           0x00001FC0 // [12:6] settling time for antenna switch
+#define AR_PHY_CCK_DETECT_ANT_SWITCH_TIME_S         6
+#define AR_PHY_CCK_DETECT_BB_ENABLE_ANT_FAST_DIV    0x2000
+
+#define AR_PHY_GAIN_2GHZ    0x1C620C
+#define AR_PHY_GAIN_2GHZ_RXTX_MARGIN    0x00FC0000
+#define AR_PHY_GAIN_2GHZ_RXTX_MARGIN_S  18
+#define AR_PHY_GAIN_2GHZ_BSW_MARGIN     0x00003C00
+#define AR_PHY_GAIN_2GHZ_BSW_MARGIN_S   10
+#define AR_PHY_GAIN_2GHZ_BSW_ATTEN      0x0000001F
+#define AR_PHY_GAIN_2GHZ_BSW_ATTEN_S    0
+
+#define AR_PHY_CCK_RXCTRL4  0x1C621C
+#define AR_PHY_CCK_RXCTRL4_FREQ_EST_SHORT   0x01F80000
+#define AR_PHY_CCK_RXCTRL4_FREQ_EST_SHORT_S 19
+
+#define AR_PHY_DAG_CTRLCCK  0x1C6228
+#define AR_PHY_DAG_CTRLCCK_EN_RSSI_THR  0x00000200 /* BB Rev 4.2+ only */
+#define AR_PHY_DAG_CTRLCCK_RSSI_THR 0x0001FC00 /* BB Rev 4.2+ only */
+#define AR_PHY_DAG_CTRLCCK_RSSI_THR_S   10     /* BB Rev 4.2+ only */
+
+#define AR_PHY_POWER_TX_RATE3   0x1C6234
+#define AR_PHY_POWER_TX_RATE4   0x1C6238
+
+#define AR_PHY_SCRM_SEQ_XR  0x1C623C
+#define AR_PHY_HEADER_DETECT_XR 0x1C6240
+#define AR_PHY_CHIRP_DETECTED_XR    0x1C6244
+#define AR_PHY_BLUETOOTH    0x1C6254
+
+#define AR_PHY_TPCRG1   0x1C6258  /* ar2413 power control */
+#define AR_PHY_TPCRG1_NUM_PD_GAIN   0x0000c000
+#define AR_PHY_TPCRG1_NUM_PD_GAIN_S 14
+
+#define AR_PHY_TPCRG1_PD_GAIN_1    0x00030000
+#define AR_PHY_TPCRG1_PD_GAIN_1_S  16
+#define AR_PHY_TPCRG1_PD_GAIN_2    0x000C0000
+#define AR_PHY_TPCRG1_PD_GAIN_2_S  18
+#define AR_PHY_TPCRG1_PD_GAIN_3    0x00300000
+#define AR_PHY_TPCRG1_PD_GAIN_3_S  20
+//
+
+#define AR_PHY_ANALOG_SWAP      0xa268
+#define AR_PHY_SWAP_ALT_CHAIN   0x00000040
+
+#define AR_PHY_TPCRG5   0x1C626C /* ar2413 power control */
+#define AR_PHY_TPCRG5_PD_GAIN_OVERLAP   0x0000000F
+#define AR_PHY_TPCRG5_PD_GAIN_OVERLAP_S     0
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_1    0x000003F0
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_1_S  4
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_2    0x0000FC00
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_2_S  10
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_3    0x003F0000
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_3_S  16
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_4    0x0FC00000
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_4_S  22
+
+#define AR_PHY_POWER_TX_RATE5   0x1C638C
+#define AR_PHY_POWER_TX_RATE6   0x1C6390
+
+#define AR_PHY_CAL_CHAINMASK    0x1C639C
+
+#define AR_PHY_POWER_TX_SUB     0x1C63C8
+#define AR_PHY_POWER_TX_RATE7   0x1C63CC
+#define AR_PHY_POWER_TX_RATE8   0x1C63D0
+#define AR_PHY_POWER_TX_RATE9   0x1C63D4
diff --git a/drivers/staging/otus/hal/hpfw2.c b/drivers/staging/otus/hal/hpfw2.c
new file mode 100644
index 000000000000..baceb0299765
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfw2.c
@@ -0,0 +1,1018 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+
+const u32_t zcP2FwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE947FFC, 0xE114D594,
+0x1E13D494, 0x67521E4C, 0xD494D693, 0x37402769,
+0x62528F06, 0x7201D692, 0x60602522, 0x2600C93F,
+0xD7906152, 0x2512611D, 0x264B6652, 0x2562470B,
+0x0009B00D, 0xE60095AC, 0xC84060E2, 0x2F028F03,
+0x8FF93652, 0x7F047601, 0xA05A4F26, 0x4F226EF6,
+0x410BD185, 0xD4850009, 0x0009440B, 0x450BD584,
+0xD7840009, 0xD284E1FF, 0x2712611D, 0xD4835029,
+0xE1FFCB01, 0x1209E501, 0x12112212, 0xE7202452,
+0x4718D57F, 0x2572D27F, 0xD17FE700, 0xD680D47F,
+0xE2012270, 0x24702172, 0xD67E2620, 0x2641E4FF,
+0xD57DE600, 0x666DE104, 0x76016063, 0x4000626D,
+0x8FF83212, 0xD5790545, 0x2520E201, 0xD279D778,
+0x2710E100, 0xE5802212, 0x655C6613, 0x666DD476,
+0x76046763, 0x374C626D, 0x8FF83253, 0xD4732712,
+0xD573E101, 0xD6732410, 0x2542E400, 0xE03AE501,
+0xD272D771, 0xE0390654, 0x27110654, 0x000B4F26,
+0x7FC82211, 0xD76FD16E, 0xDC70DB6F, 0xD271DE70,
+0xD572D471, 0x1F12D672, 0x1F76710C, 0x1FB877FC,
+0x1FEA1FC9, 0x72041F2B, 0xDE6FDC6E, 0x1F13EB10,
+0x1F511F44, 0x1F771F65, 0xD86C1F2C, 0xDD6DD96C,
+0xD26DEA00, 0x89003A22, 0xD1587A01, 0x88016010,
+0x56F98B03, 0x4218E201, 0xD1682622, 0x0009410B,
+0x440BD467, 0xD5670009, 0x0009450B, 0x6010D14C,
+0x8B108801, 0xE650D14B, 0x46186212, 0x8B083266,
+0x56FAD147, 0x2120E200, 0xCB016062, 0x2602A003,
+0x72012710, 0x60822122, 0x89098801, 0xE2C8D15A,
+0x622C6612, 0x89033626, 0x6010D158, 0x8BC88801,
+0x51F76792, 0x217252F6, 0xD6555191, 0x55FB2212,
+0x52FC6462, 0x55612542, 0x2252E400, 0x61436643,
+0x05DE6013, 0x36CC4608, 0x02DE2652, 0xC9036021,
+0x8B028801, 0x720162E2, 0x74012E22, 0x36B3664C,
+0x71048FEE, 0x66C2D147, 0x45286512, 0x265B4518,
+0x60822C62, 0x89018801, 0x0009A168, 0x6272D742,
+0x8B132228, 0xD726D541, 0x6552D441, 0x51436672,
+0x316C365C, 0x27622668, 0x14138D05, 0x6262D63D,
+0xB1A57201, 0xD61E2622, 0x2622E200, 0x52916692,
+0x8B013620, 0x0009A144, 0x6061A06E, 0x001C001C,
+0x001D4020, 0x0000B38E, 0xFFFF0000, 0x12340000,
+0x001E1015, 0x00201278, 0x002018A0, 0x00201922,
+0x0020128C, 0x001C3510, 0x001C3624, 0x001E212C,
+0x0020397C, 0x00203514, 0x00203984, 0x00203990,
+0x0020399C, 0x002039F8, 0x002039FC, 0x002039A4,
+0x002039A5, 0x002039A8, 0x00117700, 0x00203A12,
+0x00203578, 0x001142D8, 0x00203A14, 0x00203A16,
+0x001C3D30, 0x00117718, 0x001C3D00, 0x001C1000,
+0x001C36F8, 0x00117734, 0x001C3684, 0x00117710,
+0x001C3520, 0x00117600, 0x00117740, 0x001C1028,
+0x0020358C, 0x002039AC, 0x7FFFFFFF, 0x00201734,
+0x002032BE, 0x002022E8, 0x00203DC0, 0x002039FA,
+0x00203584, 0x002039EC, 0x001C3D2C, 0x001C36B0,
+0x0020351C, 0x0011775C, 0x8801C90F, 0xA0CF8901,
+0xD17C0009, 0x36206212, 0xD47B8904, 0x2421E200,
+0x2162A0CC, 0x6211D179, 0x89012228, 0x0009A0C3,
+0xE202D775, 0x75016571, 0x3123615D, 0x27518D02,
+0x0009A0BC, 0xD27255F2, 0x62226052, 0x40094019,
+0xC90F4009, 0x8F19880A, 0x52F31F2D, 0x40196022,
+0x40094009, 0x8808C90F, 0xA0A78901, 0x60630009,
+0x51F255F8, 0xE701CB01, 0x2502D263, 0xE1002172,
+0x2211D564, 0x74016452, 0x2542A098, 0x8B3F8805,
+0x602252F3, 0x40094019, 0xC90F4009, 0x8B168802,
+0xE5FFD45D, 0x655D6742, 0x8B102758, 0x6272D75B,
+0x8B0C3260, 0x55F257F8, 0x2762E101, 0xD5522512,
+0xD757E400, 0x62722541, 0xA0777201, 0x52F32722,
+0x40196022, 0x40094009, 0x8805C90F, 0x31B38B6E,
+0xD5508B6C, 0x615257F4, 0x7101E240, 0x64722512,
+0x1F4DD14D, 0x42182419, 0x8B033420, 0x6262D64B,
+0x26227201, 0xE200D640, 0x2621B0AA, 0x0009A056,
+0x3123E220, 0x88038B52, 0x52F38B1E, 0x40196022,
+0x40094009, 0x8803C90F, 0xD7418B16, 0x647251F4,
+0x7401D23D, 0x65122742, 0x1F5DE640, 0x46182529,
+0x8B033560, 0x6262D63B, 0x26227201, 0xE200D62E,
+0x2621B086, 0x0009A010, 0xD738D137, 0xD22A6412,
+0xE5007401, 0x21423A76, 0x22518F06, 0xEA00D634,
+0x72016262, 0x2622B074, 0x2FB2D532, 0x95406652,
+0xD4305BF1, 0x36205241, 0x60618910, 0x8B01C803,
+0x2B22E201, 0x8FF54510, 0x57F15664, 0x6272E1F0,
+0x41284118, 0x2722221B, 0x6BF2A008, 0x6BF2A006,
+0xE200D615, 0xD1152621, 0x2121E200, 0xE20256F5,
+0x42186662, 0x26284228, 0x1F6D8D0C, 0xD61FD11E,
+0x460B6511, 0x2008645D, 0x57F58904, 0x6272D11C,
+0x27222219, 0xD11BE201, 0x66122822, 0x8B012668,
+0x0009AE17, 0x450BD518, 0xD1180009, 0xAE10E600,
+0x07D12160, 0x00203A0C, 0x00203A10, 0x00203A18,
+0x001C3DC0, 0x0011772C, 0x001C3B88, 0x002039F4,
+0x0011773C, 0x00117744, 0x0000F000, 0x00117764,
+0x00117748, 0x00117768, 0x0011776C, 0x01FFFFFF,
+0x0011774C, 0x00203584, 0x001142D8, 0x00114774,
+0xFDFFFFFF, 0x00203DC0, 0x0020246C, 0x002039FA,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xD11F7FF4, 0x6212DE1F, 0x67E25411, 0xD41E1F41,
+0x1F722F22, 0x6743D51D, 0x7794D21D, 0x5A425841,
+0x6C726942, 0x6D225B16, 0xE6006052, 0x2502CB20,
+0x7601E540, 0x3253626D, 0x62F28BFB, 0x212255F1,
+0x55F21151, 0x2E52D613, 0x14A21481, 0xD4122492,
+0x11B627C2, 0x674226D2, 0xD911DA10, 0x2A72E801,
+0x1A8C490B, 0x4218E201, 0x7F0C1A2C, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0x000068F6,
+0x001C3B9C, 0x001C3D98, 0x001C3700, 0x001C3500,
+0x001C5960, 0x001C8960, 0x0020358C, 0x001C3D00,
+0x00201610, 0x2F962F86, 0x2FC62FA6, 0x2FE62FD6,
+0x4F124F22, 0x7F884F02, 0xE018DEB2, 0xD4B261E0,
+0x61E30F14, 0x62107101, 0x440BE01C, 0x20080F24,
+0x8F126D03, 0xD4AD1F08, 0x6740DDAD, 0x657CD4AD,
+0x470BD7AD, 0xD2AD0009, 0x621C6120, 0x46086623,
+0x36284608, 0x3D6C4608, 0xE01C1FD8, 0xE58004FC,
+0x604C66E2, 0x3050655C, 0x2D628F17, 0x01FCE018,
+0xDEA3E500, 0x641CA008, 0x6753655D, 0x607037EC,
+0x31DC6153, 0x80147501, 0x3243625D, 0xD49D8BF4,
+0xE200D59D, 0xA27F2421, 0x20082521, 0xE0188B13,
+0xE50001FC, 0xA009DE96, 0x655D641C, 0x32EC6253,
+0x62536722, 0x32DC6672, 0x75041261, 0x3243625D,
+0xA2698BF3, 0x88012D10, 0xE0188B16, 0xE40001FC,
+0x671C2D40, 0x624DDE8A, 0x8B013273, 0x0009A25C,
+0x6DE3644D, 0x7D046243, 0x32EC6643, 0x652236DC,
+0x74086162, 0x2512AFEF, 0x8B198804, 0x01FCE018,
+0x2D70E700, 0x1FD56D1C, 0x627DDE7D, 0x8B0132D3,
+0x0009A242, 0x6173677D, 0x31EC65E3, 0x75046412,
+0x365C6673, 0x61426262, 0x21297708, 0x2412AFED,
+0x8B198805, 0x01FCE018, 0x2D70E700, 0x1FD46D1C,
+0x627DDE6F, 0x8B0132D3, 0x0009A226, 0x6173677D,
+0x31EC65E3, 0x75046412, 0x365C6673, 0x61426262,
+0x212B7708, 0x2412AFED, 0x8B598831, 0x61E6DE67,
+0x61E31F19, 0x64E27104, 0x1F4A6216, 0x1F2B6416,
+0x75E46513, 0x66536712, 0x1F4C7604, 0x64521F7D,
+0xD75F6E66, 0x27E0D25F, 0xDE5F6062, 0xC9013245,
+0x65622E00, 0x4609060A, 0x4609D15C, 0x46094509,
+0x21501F4E, 0xB2B0646D, 0x620D1F6F, 0x8B012228,
+0x0009A1EA, 0xD756DE55, 0x661C61E0, 0x6410D150,
+0x470B654C, 0x7FFC54FF, 0x2FE25EFE, 0x51FE7FFC,
+0x2F12E040, 0x55FBD14F, 0x57FD56FC, 0x04FE410B,
+0xD24D7F08, 0xE11C640D, 0x1D412D10, 0xD44B6522,
+0x67421D52, 0x1D73DE4A, 0xD24A65E2, 0x67221D54,
+0x1D75D249, 0xD2496E22, 0x66221DE6, 0x1D67A1BC,
+0x89018830, 0x0009A08E, 0xE340D538, 0x33FC6156,
+0x23126456, 0x71046153, 0x67521341, 0x13726416,
+0x7EE46E13, 0x65E66212, 0x66E3D731, 0x13246EE2,
+0x760427E0, 0x6062D22F, 0x3255DE2F, 0x2E00C901,
+0x060A6E62, 0xD12D4609, 0x4E094609, 0x13434609,
+0x646D21E0, 0xB2501F5E, 0x620D1F6F, 0x8B012228,
+0x0009A18A, 0xDE25D522, 0x61E06450, 0xD724654C,
+0x470B54FF, 0x7FFC661C, 0x06FEE054, 0x7FFC2F62,
+0xEE4001FE, 0x2F123EFC, 0x55E2D125, 0x57E456E3,
+0x64E2410B, 0xD21C7F08, 0xE11C640D, 0x1D412D10,
+0xD61A6522, 0x67621D52, 0x1D73DE19, 0xD2196EE2,
+0x62221DE4, 0xD2181D25, 0x1D266222, 0x6222D217,
+0x1D27A15A, 0x00117800, 0x00202A18, 0x00203996,
+0x002035BC, 0x00203A7C, 0x002018D0, 0x00203995,
+0x00117804, 0x00203A14, 0x00203A16, 0x00117810,
+0x00203991, 0x10624DD3, 0x00203992, 0x00203993,
+0x00114AA4, 0x00200F68, 0x001C5864, 0x001C6864,
+0x001C7864, 0x001C59BC, 0x001C69BC, 0x001C79BC,
+0x00200FC0, 0x8B048833, 0x470BD7A2, 0xA123EE00,
+0x88282DE0, 0xA0D38901, 0xDE9F0009, 0x62E1E143,
+0x3216E054, 0x0FE68F02, 0x2E21E240, 0x622D62E1,
+0x8B013217, 0x0009A0BC, 0xE50185E1, 0x8B013056,
+0x0009A0B6, 0x2D10E101, 0x64E1B111, 0x06FEE054,
+0x6261E143, 0x3517652D, 0xE6408945, 0x8B0C3563,
+0xE058E41A, 0xE5000F45, 0x72C0E05C, 0x60230F55,
+0x6703C907, 0xA014E060, 0x66530F75, 0x46214621,
+0x46214621, 0x45214621, 0xE0587618, 0x0F654521,
+0xE0034521, 0xE05C2509, 0xE0070F55, 0xE0602209,
+0xE8540F25, 0x858238FC, 0x640D65F3, 0x1844B170,
+0xDD7A8584, 0x85866C0D, 0x610D4C08, 0x410860C3,
+0xE00F0EFE, 0x18154D0B, 0x2E296207, 0x668260C3,
+0x85620FE6, 0x4D0B5185, 0x2E0B600D, 0x548460C3,
+0xB13C0FE6, 0xE05465F3, 0xE5400EFE, 0xE06C62E1,
+0x3653662D, 0x0F668D41, 0xC9036023, 0x40004008,
+0x61036403, 0xD965E070, 0x0F46E5FF, 0xE074655C,
+0x60530F96, 0x6263490B, 0x42214221, 0x42214221,
+0x42006723, 0x4200327C, 0x6C074621, 0x4621E054,
+0x606309FE, 0x4008C903, 0x790630FC, 0x6A036D2D,
+0x65F3E800, 0x64D3B124, 0xE0706EA2, 0x2AE22EC9,
+0x01FE6694, 0x666CE074, 0x470B07FE, 0x2E0B6063,
+0x65F32AE2, 0xB0FA64D3, 0x628D7801, 0x32E3EE06,
+0x7D018FE7, 0x0EFEE054, 0xE05462E1, 0x420006FE,
+0x760C8561, 0x701B302C, 0xE4006103, 0xE70465F3,
+0x68667401, 0x3973694D, 0x8FF92582, 0x65F37504,
+0x641DB0DD, 0x0EFEE054, 0x64E1B09C, 0x0009A054,
+0xD43B56F8, 0xEA01D23B, 0x26A0420B, 0x0009A04C,
+0x06FCE01C, 0x8829606C, 0x5CF88B08, 0xE200D636,
+0x52612C20, 0x642DB04B, 0x0009A03E, 0x666CE681,
+0x8B043060, 0x420BD231, 0xA03554F8, 0xE6820009,
+0x3060666C, 0xD22E8B04, 0x54F8420B, 0x0009A02C,
+0x666CE683, 0x8B0A3060, 0xDA2755F8, 0x2590E900,
+0xD82855A1, 0x2852D628, 0xA01D52A2, 0xE6922620,
+0x3060666C, 0xD2208B08, 0x5C21D824, 0x6CCC52F8,
+0x28C1E600, 0x2260A010, 0x666CE693, 0x8B063060,
+0xD61F59F8, 0xE201EA00, 0xA00529A0, 0xD6162621,
+0xD21DD41C, 0x6562420B, 0x4F067F78, 0x4F264F16,
+0x6DF66EF6, 0x6AF66CF6, 0x000B69F6, 0x4F2268F6,
+0xE240614D, 0x89323123, 0x3127E21F, 0x8B27D713,
+0xD406614D, 0xE00171E0, 0x5671440B, 0x26596507,
+0x1761A025, 0x00200FBC, 0x00117804, 0x00203470,
+0x00203A9C, 0x002018C0, 0x00117800, 0x00115F00,
+0x00116058, 0x0020397C, 0x00203990, 0x00203A1A,
+0x00203A16, 0x00203AB4, 0x002018D0, 0x001C3704,
+0xE001D490, 0x6672440B, 0x26596507, 0x4F262762,
+0x0009000B, 0x614D4F22, 0x3123E240, 0xE21F8912,
+0xD7893127, 0x614D8B08, 0x5671D286, 0x420B71E0,
+0x260BE001, 0x1761A006, 0x6672D282, 0xE001420B,
+0x2762260B, 0x000B4F26, 0xE6400009, 0x46284618,
+0x6252D57E, 0x89FC2268, 0x0009000B, 0x4618E680,
+0xD57A4628, 0x22686252, 0x000B89FC, 0xA0010009,
+0x7201E200, 0x8BFC3242, 0x0009000B, 0x4618E680,
+0xD5734628, 0x22686252, 0x000B8BFC, 0x2FE60009,
+0x7FFC4F22, 0xBFF16E53, 0x61E22F42, 0xE280D66D,
+0x54E11615, 0x16464218, 0x422855E2, 0x57E31657,
+0x16786EF2, 0x26E22E2B, 0x4F267F04, 0x6EF6AFCE,
+0x2FD62FC6, 0x4F222FE6, 0x6C53DD62, 0x6E43BFD6,
+0x2DE2BFBB, 0x0009BFD2, 0x2C1251D5, 0x1C4154D6,
+0x1C5255D7, 0x1C6356D8, 0x6EF64F26, 0x000B6DF6,
+0x61636CF6, 0xA004E600, 0x62564109, 0x24227601,
+0x36127404, 0x000B8BF9, 0xD6530009, 0x8562E500,
+0xA00B674D, 0x655D610D, 0x40006053, 0x305CD44F,
+0x024D4008, 0x3270622D, 0x75018905, 0x3213625D,
+0x000B8BF1, 0x000BE000, 0x2FE6E001, 0x54416743,
+0x4E08EE7F, 0x4E28D246, 0x25E96543, 0x60436E21,
+0x9E7562ED, 0x4529C903, 0xE60032E3, 0x8D456103,
+0x21184509, 0xD23F8B05, 0x002C6053, 0xA08AC93F,
+0x60136603, 0x8B268801, 0x880C6053, 0xD53A8B04,
+0xC93F8453, 0x6603A07F, 0x8B048808, 0x84E2DE36,
+0xA078C93F, 0x880D6603, 0x8B03D633, 0xC93F8461,
+0x6603A071, 0x88096260, 0x622C8F09, 0xE014DE2C,
+0x655C05EC, 0x60233258, 0xA064C93F, 0x60236603,
+0xA060C93F, 0x88026603, 0xE0078B5D, 0x60432509,
+0x8905C810, 0x6053D225, 0xC93F002C, 0x6603A053,
+0x6053DE23, 0xC93F00EC, 0x6603A04D, 0x88016013,
+0x60538B19, 0x8B04880C, 0x8423D21E, 0xA042C93F,
+0x88086603, 0xD51B8B04, 0xC93F8452, 0x6603A03B,
+0xD618880D, 0x84618B03, 0xA034C93F, 0x60606603,
+0xA030C93F, 0x88026603, 0xE0078B2D, 0x60432509,
+0x8923C810, 0x6053DE10, 0xC93F00EC, 0x6603A023,
+0x00000BB8, 0x00203470, 0x001C3704, 0x001C373C,
+0x001C3700, 0x001C370C, 0x00114000, 0x00114008,
+0x001142D8, 0x001142E4, 0x001142E8, 0x001142F5,
+0x001142ED, 0x001142FD, 0x00114309, 0x6053D209,
+0xC93F002C, 0x60136603, 0x8B038802, 0xC8106043,
+0x76028900, 0xC93F6063, 0x40004018, 0x1741240B,
+0x6EF6000B, 0x00114301, 0x0009A16E, 0x2FE62FD6,
+0xDD944F22, 0xA0049EB2, 0xD4930009, 0x420BD293,
+0x62D265D2, 0x8BF822E8, 0x0009A004, 0xD28FD490,
+0x55D1420B, 0x22E852D1, 0xA0048BF8, 0xD48D0009,
+0x420BD28A, 0x52D255D2, 0x8BF822E8, 0x0009A004,
+0xD286D489, 0x55D3420B, 0x22E852D3, 0xA0048BF8,
+0xD4860009, 0x420BD281, 0x52D455D4, 0x8BF822E8,
+0x6EF64F26, 0x6DF6000B, 0x2FD62FC6, 0x4F222FE6,
+0x6E636C73, 0x6D53B01A, 0x64D357F4, 0xB05F65E3,
+0xB07566C3, 0xB0A40009, 0xB0A80009, 0xB0AC0009,
+0xB0AC0009, 0xB0AF0009, 0xB03154F5, 0x6CCD6C03,
+0x4F2660C3, 0x6DF66EF6, 0x6CF6000B, 0x3412D170,
+0xD6700529, 0x2650D770, 0x2742000B, 0x0009A018,
+0x2FD62FC6, 0x4F222FE6, 0x6E636C73, 0x6D53BFEE,
+0x64D357F4, 0xB03365E3, 0xB08D66C3, 0xB00F54F5,
+0x6CCD6C03, 0x4F2660C3, 0x6DF66EF6, 0x6CF6000B,
+0xE503D162, 0xD763D462, 0x21524518, 0x2472000B,
+0xD45FD15E, 0x2162E600, 0x2462000B, 0xBF734F22,
+0xBF73E40A, 0xD25C0009, 0x4118E104, 0xE40AE500,
+0xBF692212, 0xD7592252, 0xCB206072, 0x000B4F26,
+0x4F222702, 0x410BD156, 0xD556E400, 0x4F26452B,
+0xD1552FE6, 0x66126E63, 0x92104418, 0x44084528,
+0x45002629, 0x265B4408, 0x264B4400, 0x21624708,
+0xD14E4708, 0x217227EB, 0x6EF6000B, 0x1FFF03F0,
+0x4F222FE6, 0xE101DE4A, 0xBF3DE40A, 0x67E32E12,
+0xE500776C, 0xE204E130, 0x2752E40A, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x27222712, 0x27522752, 0x27522752, 0x27522752,
+0x27522752, 0x175ABF18, 0x2E62E600, 0x000B4F26,
+0xD2346EF6, 0xE441E101, 0x000B2212, 0xD1322242,
+0xE605D432, 0x000B2162, 0x000B2462, 0xD2300009,
+0xE40AE601, 0x2262AF00, 0x2FC62FB6, 0x2FE62FD6,
+0x7FFC4F22, 0x6C43DB2B, 0xED0060B2, 0x2B02CB03,
+0xC90360B2, 0x6E03A008, 0x89073DC2, 0xE46460B2,
+0xB07CC903, 0x7D016E03, 0x8BF52EE8, 0x8F043DC2,
+0xD4212FE1, 0x460BD621, 0x62F10009, 0x6023622D,
+0x89FFC801, 0x7F046023, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6000B, 0x001C3B88, 0x00203AC8, 0x002018D0,
+0x00203AD0, 0x00203AD8, 0x00203AE0, 0x00203AE8,
+0x0025E720, 0x00203DBC, 0x00203980, 0x001C5968,
+0x001C3B40, 0x000F8000, 0x001D4004, 0x001C3500,
+0x002015E4, 0x00201610, 0x001C5814, 0x001C59D0,
+0x001C5830, 0x001C6268, 0x001C59A4, 0x001C639C,
+0x001C581C, 0x001C5860, 0x00203AF0, 0x002018C0,
+0x8F014411, 0x6043604B, 0x0009000B, 0x5651D52B,
+0x46286052, 0x306C000B, 0x2FC62FB6, 0x2FE62FD6,
+0x4F124F22, 0xBFF14F02, 0x6B036E43, 0xDD25DC24,
+0x0009BFEC, 0x3C0530B8, 0x4609060A, 0x46014609,
+0x020A3D65, 0x42094209, 0x32E24209, 0x4F068BF0,
+0x4F264F16, 0x6DF66EF6, 0x000B6CF6, 0x2FC66BF6,
+0x2FE62FD6, 0x4F124F22, 0xBFCF4F02, 0x6C036E43,
+0xBFCBDD13, 0x30C80009, 0x060A3D05, 0x46094609,
+0x36E24601, 0x4F068BF5, 0x4F264F16, 0x6DF66EF6,
+0x6CF6000B, 0x4F222FE6, 0xE102DE0B, 0xE403E500,
+0xBFB92E12, 0xE6062E52, 0xE7004618, 0x2E62E403,
+0x4F262E72, 0x6EF6AFB0, 0x0009000B, 0x001C1040,
+0xCCCCCCCD, 0x10624DD3, 0x001D4004, 0x2F962F86,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0xE5007F98,
+0x6453E710, 0x6B534728, 0xEE1ADCBC, 0x6153655D,
+0x315C4108, 0x75014108, 0x6043317C, 0x0F16665D,
+0xED0060B3, 0x21B136E3, 0x81128111, 0x11D28113,
+0x11D411D3, 0x74048FEA, 0xD8B167F2, 0x1871D9B1,
+0x58F12872, 0x1981D1B0, 0x59F22982, 0x5DF45AF3,
+0x54F65EF5, 0x21921191, 0x11A211A3, 0x11D411D5,
+0x11E611E7, 0x11481149, 0xDAA855F7, 0x57F8EE00,
+0x52F9DDA7, 0x64E3D6A7, 0x2A521A51, 0xD8A7D9A6,
+0x2D72EAEF, 0x6AAC2622, 0x6DE36EED, 0x61E34D08,
+0x41083DEC, 0x31EC4D08, 0x60B33D9C, 0x2DB14108,
+0xE05081D1, 0xE79F4108, 0x41084008, 0x81D2677C,
+0x318C60B3, 0x3472E200, 0x1DD281D3, 0xD4931D13,
+0x1D248D01, 0x65D3D48F, 0x7E01B0B2, 0x34A264ED,
+0xDA8C8BDA, 0x68A22FD2, 0x4829DD91, 0x64A22D82,
+0x694D7DFC, 0x2D92D286, 0x4E296E22, 0x2DE27D0C,
+0x6AD36822, 0xD784618D, 0x6D722A16, 0xD583D489,
+0x5E7224D2, 0x14E2D688, 0xEE005174, 0x58761414,
+0x1486D186, 0xE7105978, 0x62521498, 0x142A65E3,
+0x64E326E2, 0x644DE600, 0x48086843, 0x4808384C,
+0x6053381C, 0x28B10C86, 0x60B309CE, 0x60538191,
+0x60430ACE, 0x605381A2, 0x60B30DCE, 0x605381D3,
+0x740108CE, 0x09CE1882, 0x19E3624D, 0x32730ACE,
+0x8FE01A64, 0xD96A7504, 0x6C92E003, 0x2CB14018,
+0xDA6F6D92, 0xE05081D1, 0x40086E92, 0x619281E2,
+0x811360B3, 0xE6006492, 0x67921442, 0x17A3D468,
+0xE1FF6892, 0xE7031864, 0x46086563, 0x7501364C,
+0x665D2612, 0x8BF83673, 0xE003DC5A, 0x40186DC2,
+0x6EC22DB1, 0x81E1D25F, 0xEE0061C2, 0x64C21112,
+0x1423E024, 0xD45B65C2, 0x67C215E4, 0x8172E580,
+0x66E368C2, 0x655C8183, 0x6963666D, 0x6A6D7604,
+0x3A53394C, 0x29E28FF8, 0xDC54DB53, 0x740424B2,
+0x7F6824C2, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0x614268F6, 0xC8036011, 0xE5008F03,
+0x3420D23C, 0x60118B06, 0x8802C903, 0xD2398B06,
+0x8B033420, 0x65135612, 0x24225264, 0x6053000B,
+0x2FE62FD6, 0x7FEC4F22, 0x62536E53, 0x6D43E550,
+0x4508E400, 0xE101A001, 0x60435224, 0x81212211,
+0x60538123, 0x56E28122, 0x8BF53620, 0x16E4D238,
+0xE61464F3, 0x65E3420B, 0xE4FC65E1, 0x2E512549,
+0x65F361F1, 0x2F112149, 0xD13154D1, 0xE614410B,
+0x607157D1, 0x2701CB01, 0x7F141DE1, 0x6EF64F26,
+0x6DF6000B, 0x2FE62FD6, 0x7FEC4F22, 0x66536E53,
+0x6D43E5FC, 0x20596061, 0x2601CB01, 0x326052E2,
+0x12E48B06, 0x31E051E2, 0x52D18B04, 0x1E22A002,
+0x5664AFF0, 0x64F3D21E, 0x420BE614, 0x67E165E3,
+0x2719E1FC, 0x67F12E71, 0x271954D1, 0x65F3D118,
+0x410BE614, 0x52D12F71, 0xCB016021, 0x1DE12201,
+0x4F267F14, 0x000B6EF6, 0x00006DF6, 0x002039AC,
+0x0020357C, 0x00203584, 0x0020358C, 0x002035B4,
+0x00203998, 0x002039A0, 0x00100208, 0x001014C0,
+0x001E210C, 0x001C3D00, 0x002039EC, 0x001000C8,
+0x00117880, 0x00117780, 0x00040020, 0x0026C401,
+0x00200D42, 0x4F222FE6, 0xDE42624C, 0x42004208,
+0x3E2CA005, 0xD4405252, 0xBF695624, 0x65E22E62,
+0x352052E1, 0xD63D8BF6, 0x4F262622, 0x6EF6000B,
+0x2FC62FB6, 0x2FE62FD6, 0xDC394F22, 0x52C1DB39,
+0x362066C2, 0x6061891C, 0x8801C903, 0xDE348918,
+0xBF38DD35, 0x650364E3, 0x66B28503, 0x3262620D,
+0xD4328907, 0x0009BF76, 0x4D0BD431, 0xAFE60009,
+0xBF3D0009, 0xD42F64E3, 0x00094D0B, 0x0009AFDF,
+0x2262D22D, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x2FD62FC6, 0x4F222FE6, 0xDD29DC28, 0x6E4360C2,
+0x04DE4008, 0xE614D127, 0x65E3410B, 0xD127D726,
+0x55E227E2, 0x35E05254, 0x21228F04, 0x400860C2,
+0x122202DE, 0x605365C2, 0x75014008, 0x0DE606DE,
+0xC90F6053, 0x60632C02, 0x6EF64F26, 0x000B6DF6,
+0x85436CF6, 0x650D5643, 0x622D6262, 0x35277204,
+0xE1008F0C, 0x2268960C, 0xD6158B03, 0x72015261,
+0xD6131621, 0x6262E101, 0x26227201, 0x6013000B,
+0x000001FF, 0x0020358C, 0x00203584, 0x001C3D00,
+0x002035B4, 0x0020397C, 0x002018C0, 0x0020357C,
+0x00203B18, 0x00203B1C, 0x001C3D28, 0x002039EC,
+0x002039AC, 0x00200D42, 0x002039F0, 0x002039F4,
+0x00117754, 0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6,
+0x7FF84F22, 0x6C22D241, 0xC80360C3, 0xDE40896E,
+0xDA41DB40, 0x52B1D941, 0x362066B2, 0x60618945,
+0x8801C903, 0xDD3B8941, 0x420BD23D, 0x650364D3,
+0x60A12F02, 0x89328801, 0x85145153, 0x8840600C,
+0x1F118F0C, 0xD5376191, 0x641D450B, 0x8B262008,
+0xD7356691, 0x646D470B, 0x8B202008, 0x420BD233,
+0x51F154F1, 0xC8208511, 0xD1318904, 0x021EE050,
+0x01267201, 0x420BD22F, 0x200864F2, 0x64D38907,
+0x4D0BDD2D, 0xD12D65F2, 0xAFC4E601, 0xD22C2162,
+0x420B65F2, 0xD72B64E3, 0xAFBCE601, 0xD2262762,
+0x420B65F2, 0xAFB664D3, 0xDE270009, 0xDA28DD27,
+0x52D1DB28, 0x362066D2, 0x60618918, 0x8801C903,
+0xD4228914, 0x450BD516, 0x56030009, 0x8F0436E0,
+0xE2016503, 0xAFEC2A20, 0xD41F2B52, 0x420BD216,
+0xD7180009, 0x4118E101, 0x2712AFE3, 0xC80460C3,
+0xD21A8902, 0x0009420B, 0x4F267F08, 0x6DF66EF6,
+0x6BF66CF6, 0x000B6AF6, 0x000069F6, 0x001E2100,
+0x0020358C, 0x00203584, 0x00203A14, 0x001142D8,
+0x002014A6, 0x00115EA2, 0x00114774, 0x00200D8A,
+0x0020351C, 0x002016C2, 0x002014D0, 0x001E212C,
+0x00201534, 0x001C3D30, 0x00117880, 0x0020357C,
+0x0020399C, 0x00203998, 0x002035B4, 0x00200644,
+0xE601D203, 0x1265D503, 0x000B2252, 0x00001266,
+0x001C1010, 0x0000C34F, 0x0009000B, 0x0009000B,
+0x0009000B, 0x0009000B, 0xE000000B, 0xE000000B,
+0x0009000B, 0xE4FDD59D, 0xD69D6152, 0x25122149,
+0x74016052, 0x2502CB01, 0xD19A6752, 0x25722749,
+0xC8406010, 0x60628902, 0x2602CB04, 0xE1F76462,
+0x26422419, 0xE7016062, 0x2602C9CF, 0xE5026062,
+0x2602CB10, 0x47186062, 0x2602CB03, 0x000B1652,
+0xD58D1673, 0xD28ED78D, 0xE100D48E, 0x2511E600,
+0x22102711, 0x2461AFCE, 0xD28B664C, 0x362C4600,
+0xCB106060, 0x2600000B, 0xD287654C, 0x352C4500,
+0xE1EF6650, 0x000B2619, 0x664C2560, 0x4600D283,
+0x6060362C, 0x000BCB10, 0x654C2600, 0x4500D27F,
+0x6650352C, 0x2619E1EF, 0x2560000B, 0xD27A664C,
+0x362C4600, 0xCB086060, 0x2600000B, 0xD276654C,
+0x352C4500, 0xE1F76650, 0x000B2619, 0x664C2560,
+0x4600D272, 0x6060362C, 0x000BCB08, 0x654C2600,
+0x4500D26E, 0x6650352C, 0x2619E1F7, 0x2560000B,
+0xD669624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0x624C600C, 0x4200D664, 0x6020326C,
+0x4021C908, 0x40214021, 0x600C000B, 0x644CD160,
+0x6240341C, 0x602C000B, 0x644CD15E, 0x6240341C,
+0x602C000B, 0x4F222FE6, 0x645C6E43, 0x3467E60A,
+0xBFEB8914, 0x640C0009, 0x880160EC, 0xE00F8B02,
+0x2409A002, 0x44094409, 0xE60A624C, 0x89053263,
+0x644CBFE2, 0x6023620C, 0x8B00C880, 0x6023E200,
+0x000B4F26, 0x4F226EF6, 0x6062D64B, 0x8B038801,
+0x0009B256, 0x0009A003, 0xE640D248, 0xD6482260,
+0x4F26E200, 0x2622000B, 0xD6434F22, 0x88026062,
+0xB29F8B01, 0xD6420009, 0x4F26E200, 0x2622000B,
+0xD43ED53D, 0xE701E100, 0x000B2512, 0xD23B2470,
+0x000BE604, 0x4F222260, 0xD13BD43A, 0x0009410B,
+0xE1FDD53A, 0xD23A6650, 0xE7002619, 0x4F262560,
+0x2270000B, 0xD5374F22, 0x6152D237, 0x611DD737,
+0x64522512, 0x242BE6FF, 0xD4352542, 0x666DD22E,
+0x2762420B, 0xE1FBD52D, 0x27196750, 0x000B4F26,
+0x4F222570, 0xD128D42F, 0x0009410B, 0xE7F7D527,
+0x26796650, 0x000B4F26, 0xD5242560, 0x62509425,
+0x000B2249, 0xD5212520, 0x6250E4BF, 0x000B2249,
+0x4F222520, 0x8522D224, 0x2008600D, 0x88018911,
+0x88038944, 0x88058946, 0x88068948, 0x8808894E,
+0x88098954, 0x880A895A, 0x880B8960, 0xA06D8966,
+0xB06F0009, 0xA06A0009, 0xFF7F600C, 0x001E2148,
+0x001E1108, 0x001E1000, 0x00203A4C, 0x00203A4E,
+0x00203A6D, 0x00203A30, 0x001E103F, 0x001E105F,
+0x001E102F, 0x001E1090, 0x00203A54, 0x001E100B,
+0x00203A50, 0x00203B20, 0x002018C0, 0x001E1028,
+0x00203A6C, 0x001D4020, 0x98760000, 0x001C1000,
+0x00203B2C, 0x00203B3C, 0x00203A24, 0x0009B04C,
+0x600CA035, 0x0009B055, 0x600CA031, 0x6260D684,
+0x8B2B2228, 0x0009B061, 0x600CA029, 0x6260D680,
+0x8B232228, 0x0009B069, 0x600CA021, 0x6260D67C,
+0x8B1B2228, 0x0009B0C7, 0x600CA019, 0x6260D678,
+0x8B132228, 0x0009B0CD, 0x600CA011, 0x6260D674,
+0x8B0B2228, 0x0009B125, 0x600CA009, 0x6260D670,
+0x8B032228, 0x0009B13D, 0x600CA001, 0x4F26E000,
+0x0009000B, 0xD26CD16B, 0xD56C8412, 0x4000C90F,
+0xD76B012D, 0xE403D66B, 0xE20F611C, 0x2540E001,
+0x25202712, 0x2602000B, 0xE601D262, 0x30668523,
+0xE0008D05, 0xD663D260, 0xE0018122, 0x000B2602,
+0xD25C0009, 0x600D8523, 0x89052008, 0x8B0A8801,
+0x6060D65D, 0x2600CB01, 0xD457D65A, 0xE001E101,
+0x000B2612, 0x000B8142, 0xD152E000, 0x8513E501,
+0x640D4518, 0x66033453, 0xE0008D05, 0xD551D253,
+0x2260E001, 0x000B2502, 0x4F220009, 0x8513D149,
+0x6453650D, 0x62494419, 0x227D672E, 0x8801602C,
+0x88028909, 0x88038910, 0x8806891A, 0x88078935,
+0xA04C893B, 0xD5460009, 0x6652D746, 0x2762D446,
+0x622C6261, 0x2421A038, 0x2228625C, 0xD4438B3F,
+0x6642D540, 0x2562D440, 0x24018561, 0x6203A02C,
+0x2008605C, 0x88108907, 0x88208908, 0x88308909,
+0xA02C890A, 0xD23A0009, 0x6222A008, 0xA005D239,
+0xD2396222, 0x6222A002, 0x6262D638, 0xD432D531,
+0x66212522, 0xA00F626C, 0xD6352421, 0x6261D52D,
+0x622CD42D, 0xA0072562, 0xD6322421, 0x8561D529,
+0x2562D429, 0x62032401, 0x662D8515, 0x3617610D,
+0x65038F01, 0xB0CB2451, 0xA0010009, 0xE000E001,
+0x000B4F26, 0xD6190009, 0xD427E101, 0x65412610,
+0xD118D717, 0xE20F655D, 0x2752E001, 0x000B2620,
+0x2FE62102, 0xD20F4F22, 0x640C8523, 0x8B082448,
+0xD511D61D, 0x2621E200, 0x940F8451, 0xA0482049,
+0xDE0D8051, 0xC84060E0, 0xE2018D32, 0x89443427,
+0xD216D615, 0x2641420B, 0x0009A030, 0x0000FF7F,
+0x00203A6D, 0x00203A24, 0x00203A30, 0x001E1100,
+0x001E100C, 0x00203A50, 0x001E1000, 0x001E1001,
+0x00203A58, 0x00203A38, 0x00203A3C, 0x00203A40,
+0x00203A5C, 0x00203A60, 0x00203A64, 0x00203A68,
+0x00203E20, 0x00203E2A, 0x00203A4A, 0x002027F2,
+0x89123427, 0xD294D693, 0x2641420B, 0xCB8084E1,
+0x80E1B0F5, 0xD69160E0, 0x2E00CB04, 0xC93F6060,
+0xD68F2600, 0xA001E001, 0xE0002602, 0x000B4F26,
+0xD68C6EF6, 0xC8806060, 0xD2868919, 0x88016021,
+0xD2898B15, 0x8524E501, 0x89103056, 0xE203D187,
+0x2120D487, 0xE00B6541, 0x0656655D, 0xE40FD585,
+0x2140E702, 0xD77E2571, 0x000BE001, 0x000B2702,
+0x2FE6E000, 0xDE804F22, 0xC88084E1, 0xD57A892C,
+0x20088554, 0x61038F28, 0x8553D77C, 0x64036672,
+0x8566650C, 0x3520620C, 0xD6798B1E, 0x651CD774,
+0x2651644C, 0x60E02741, 0x8904C840, 0x420BD275,
+0xA0030009, 0xD2680009, 0x0009420B, 0x0009B09F,
+0xE201D167, 0x60E02122, 0xCB04D464, 0x60402E00,
+0x2400C93F, 0x6023A001, 0x4F26E000, 0x6EF6000B,
+0x2FB62FA6, 0x2FD62FC6, 0xDA622FE6, 0x66A1E240,
+0x3622DC5E, 0x62638900, 0x6ED36D2C, 0x4E2136D8,
+0x4E212A61, 0xDB61D460, 0xE700A00F, 0x770162B2,
+0x71026123, 0x66212B12, 0x71026213, 0x61212B12,
+0x651D666D, 0x356C4528, 0x627C2452, 0x8BED32E3,
+0xC90360D3, 0x8B108803, 0x617367B2, 0x2B127102,
+0x71026E13, 0x2B126571, 0x655D6DE1, 0x422862DD,
+0x325CE107, 0xA00C2C10, 0x88022422, 0xA0038B01,
+0x8801E203, 0xE2018B05, 0x66B22C20, 0x655D6561,
+0xE60F2452, 0x67A12C60, 0x8B052778, 0xDD38DC44,
+0xEB01EA00, 0x2DB22CA2, 0x6DF66EF6, 0x6BF66CF6,
+0x6AF6000B, 0x2FE62FD6, 0xE240DD36, 0x362266D1,
+0x62638900, 0x3678672C, 0x7703DE38, 0x47212D61,
+0x64E2D635, 0xA00E4721, 0x6562E100, 0x62537101,
+0x74012450, 0x24204219, 0x45297401, 0x74012450,
+0x24504519, 0x621C7401, 0x8BEE3273, 0x66E24200,
+0x420061D1, 0x2118362C, 0x2E628F06, 0xDD1CD728,
+0xE501E400, 0x2D522742, 0x000B6EF6, 0x2FD66DF6,
+0x4F222FE6, 0xED0AEE01, 0x64E3BC86, 0xBC8B64E3,
+0x62EC7E01, 0x8BF732D7, 0xBC8EEE01, 0x64E364E3,
+0x7E01BC93, 0x32D762EC, 0x4F268BF7, 0x000B6EF6,
+0xD1186DF6, 0xD418920D, 0x72122122, 0x2422D617,
+0xD7177204, 0x72202622, 0x2722D116, 0x000B7230,
+0x137A2122, 0x00203A4A, 0x002028FE, 0x001E1015,
+0x00203A50, 0x001E1001, 0x00203A24, 0x001E1100,
+0x00203A4E, 0x00203A3C, 0x001E1000, 0x00203A40,
+0x00203A4C, 0x002027F2, 0x001E100C, 0x00203A38,
+0x00203A54, 0x00203A58, 0x00203A5C, 0x00203A60,
+0x00203A64, 0x00203A68, 0x4F222FE6, 0xD6707FFC,
+0x88016060, 0xE2018951, 0x2620BFBB, 0xD56ED16D,
+0xDE6E6010, 0x64E36552, 0x7402C840, 0x8D22D16C,
+0xD26C7502, 0xE601D76C, 0xE7042722, 0x76016255,
+0x626C2421, 0x8FF93273, 0xD4637402, 0x6242E601,
+0x640D8528, 0x67494419, 0x275D657E, 0x81E4607C,
+0xE417D562, 0x67557601, 0x3243626C, 0x8FF92171,
+0xA0207102, 0xD25E0009, 0xE601D75B, 0xE7042722,
+0x76016255, 0x626C2421, 0x8FF93273, 0xD4527402,
+0x6242E601, 0x640D8528, 0x67494419, 0x275D657E,
+0x81E4607C, 0xE417D553, 0x67557601, 0x3243626C,
+0x8FF92171, 0x92897102, 0xD2462E21, 0x5E23D74E,
+0x64F22FE2, 0x604365F2, 0x2700C980, 0xC9606043,
+0x80716103, 0xC9036043, 0x80724519, 0x65F2605C,
+0x817266F2, 0x46194629, 0x606C4529, 0x4018645C,
+0x8173304C, 0x21185E23, 0x64F22FE2, 0x6E4C62F2,
+0x602C4219, 0x66F262F2, 0x46294018, 0x461930EC,
+0x42298174, 0x652C606C, 0x305C4018, 0x81758F07,
+0x0009BC97, 0x2228620C, 0xA00A8908, 0x60130009,
+0x8B038840, 0x0009B009, 0x0009A003, 0xE202D62F,
+0x7F042622, 0x000B4F26, 0x4F226EF6, 0x8552D52A,
+0x8830600D, 0x88318903, 0xA0348923, 0x85550009,
+0xD428D727, 0x85532701, 0x610DD627, 0x24124118,
+0x460BD426, 0xD7230009, 0xD226D425, 0x6572420B,
+0xE230D120, 0x42286712, 0x2729E620, 0x37604628,
+0xD6218B03, 0xA016E200, 0xD61F2622, 0xA012E202,
+0xD1182622, 0x6212E530, 0xE6204528, 0x46282259,
+0x89083260, 0xD41AD119, 0xE601D513, 0x2160450B,
+0x472BD718, 0x4F264F26, 0x0009000B, 0x0000060A,
+0x00203A6C, 0x001E1000, 0x00203A58, 0x00203E20,
+0x00203E2C, 0x00203DC4, 0x00203A40, 0x00203DF4,
+0x00203DF2, 0x00203DC6, 0x00203A24, 0x00203A50,
+0x00203A3C, 0x00203A38, 0x002018C0, 0x00203B48,
+0x00203B4C, 0x002018D0, 0x00203A54, 0x001E100B,
+0x00203B60, 0x00114004, 0x4F222FE6, 0x84E9DE86,
+0x2448640C, 0xB17B8901, 0xD2840009, 0x26686620,
+0x60E08902, 0x2E00C9BF, 0x000B4F26, 0x000B6EF6,
+0x2FE60009, 0xDE7E4F22, 0x60E0D67E, 0xCBC0D47E,
+0x62602E00, 0xC803602C, 0x40218904, 0x70014021,
+0x6603A002, 0x66034009, 0xD678616D, 0xE500A004,
+0x75016262, 0x74042422, 0x3213625D, 0xD2748BF8,
+0x0009420B, 0xC9BF84E2, 0x4F2680E2, 0x6EF6000B,
+0x2FE62FD6, 0x7FFC4F22, 0x6260D66E, 0x89402228,
+0xD565E100, 0x60502610, 0xCB40D46B, 0x2500440B,
+0x8D052008, 0x62E06E03, 0x7104612C, 0x2F11A006,
+0xD466D65E, 0xDD666760, 0x657C4D0B, 0xE23C6D1D,
+0x8B033D27, 0xD264D463, 0x0009420B, 0x4D214D21,
+0xA005D762, 0x66E6E400, 0x357C4508, 0x74012562,
+0x35D3654D, 0xD75E8BF7, 0x6E72E003, 0x81E14018,
+0x6E7260F1, 0x81E2700C, 0xD45A6172, 0xDD5A8113,
+0x65724D0B, 0xD64AD259, 0x2212E101, 0xC93F6060,
+0x7F042600, 0x6EF64F26, 0x6DF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xD2524F22, 0x6B436E73, 0x420B6C53,
+0x20086D63, 0x64038D1C, 0xE50ED13C, 0x32526210,
+0x60C38916, 0x804124B0, 0x814160D3, 0xA007E500,
+0x655D61BC, 0x00EC6053, 0x364C6653, 0x80647501,
+0x3213625D, 0xD6308BF5, 0xC9BF6060, 0x2600A008,
+0xD239D440, 0x6EF64F26, 0x6CF66DF6, 0x6BF6422B,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x2F962F86,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0xE1007FC4,
+0x6513ECFF, 0x6B136CCD, 0xDE34D733, 0xEDFF64F3,
+0xD833EA04, 0x6053655C, 0x027D4000, 0x32C0622D,
+0x66038D0D, 0x09ED6063, 0x2491027D, 0x24217402,
+0x698202ED, 0x3928622D, 0x74022892, 0x75017104,
+0x6063625C, 0x07D532A2, 0x0EB58FE4, 0x2448641C,
+0xE6808905, 0x67F3E5C5, 0xBF8F666C, 0x7F3C655C,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0xD11C68F6, 0x6012D21C, 0xCB20E405, 0x2102E500,
+0x000B2242, 0x00002252, 0x001E1017, 0x00203996,
+0x001E1015, 0x001E10BF, 0x00117800, 0x001E10FC,
+0x00200644, 0x0020399C, 0x00202A56, 0x00203B64,
+0x002018D0, 0x00203B80, 0x002018C0, 0x0011788C,
+0x00203998, 0x0020357C, 0x00201534, 0x001E2130,
+0x00202A18, 0x00203B88, 0x002039FC, 0x00203A04,
+0x00203DC0, 0x001C3500, 0x001D4004, 0xD564D163,
+0xE400D764, 0x2142E20F, 0x17411154, 0xD5622722,
+0x9669D762, 0x15412572, 0x96661562, 0xE6011565,
+0xD55F1165, 0x666CE6F8, 0x25422542, 0x25422542,
+0x25422542, 0x25622542, 0x7601E727, 0x67632572,
+0x25627797, 0xE7042572, 0x2572E248, 0xE2192522,
+0xE2702522, 0x25422542, 0x25422542, 0x25222542,
+0x2522E20C, 0x25422542, 0x25422542, 0x25422542,
+0x25422542, 0x000B154A, 0xE2081145, 0x0009422B,
+0x2FE62FD6, 0x7FFC4F22, 0xC8206043, 0x6E438D02,
+0x0009BE85, 0xC81060E3, 0xBE828901, 0x60E30009,
+0x8901C840, 0x0009BEA4, 0xC80160E3, 0xDD3D8938,
+0xC80260D0, 0x2F008D03, 0x460BD63B, 0x60F00009,
+0x8902C804, 0x460BD639, 0x62F00009, 0xC8806023,
+0x60D08902, 0x2D00C97F, 0xC8016023, 0xD6348906,
+0x0009460B, 0x0009A007, 0x51630601, 0x8902C808,
+0x460BD630, 0x60F00009, 0x8902C810, 0x420BD22E,
+0xD52E0009, 0x88026052, 0xD22D8B03, 0xA005E604,
+0x88012260, 0xD22A8B02, 0x2260E601, 0x2522E200,
+0xC88060E3, 0xD227892D, 0x60E36E20, 0x8902C880,
+0x420BD225, 0x60E30009, 0x8902C840, 0x420BD223,
+0x60E30009, 0x8902C802, 0x420BD221, 0x60E30009,
+0x890DC804, 0xDD20D11F, 0x0009410B, 0x0009BF11,
+0x0009BF4C, 0xD51ED41D, 0x2470E708, 0x25D2BF85,
+0xC80860E3, 0xD21B8905, 0x4F267F04, 0x422B6EF6,
+0x7F046DF6, 0x6EF64F26, 0x6DF6000B, 0x001C581C,
+0xA000A000, 0x001D0100, 0x001D4000, 0x00040021,
+0x001C589C, 0x001E1021, 0x00201A46, 0x00201A68,
+0x002020C8, 0x00201A80, 0x00201A8E, 0x00203A50,
+0x001E100B, 0x001E1028, 0x00201AFA, 0x00201B06,
+0x00201A96, 0x00201AB4, 0x12345678, 0x001E1000,
+0x0010F100, 0x00201AE2, 0x644CD6A7, 0x000B346C,
+0xD6A62450, 0x346C644C, 0x2450000B, 0x644CD6A4,
+0x000B346C, 0x625C2450, 0x4208616D, 0x42084119,
+0x42006019, 0x670E614C, 0xD49E321C, 0x4200207D,
+0x324CC90F, 0x2200000B, 0x4208625C, 0x42004208,
+0x324C644C, 0x4200D498, 0x000B324C, 0x2FE62260,
+0x614C4F12, 0x4100D493, 0x6710314C, 0xE29F666D,
+0x27294619, 0x6E536269, 0x672E6573, 0x4221227D,
+0x42214221, 0x7601662C, 0xE4014608, 0x34E84608,
+0x644C4600, 0x071A0467, 0x2150257B, 0x000B4F16,
+0x4F226EF6, 0xD2857FE8, 0x88016021, 0xD2848B7B,
+0x26686621, 0xD2838B77, 0x26686621, 0xE50F8B73,
+0xE401BFA2, 0xBFA4E501, 0xE586E400, 0xE400655C,
+0x2F50BFA4, 0xBFA1E401, 0xE602E506, 0x60634618,
+0x81F2E401, 0x6543BF9F, 0xE40185F2, 0xBFAB6543,
+0x85F26603, 0x6543E401, 0x6603BFB1, 0xE40265F0,
+0x6053756C, 0x80F8BF80, 0xBF82E402, 0x84F8E512,
+0x7090E402, 0x6503BF82, 0x4618E602, 0x81F66063,
+0xBF80E402, 0x85F6E500, 0x6603E402, 0xE500BF8C,
+0xE40285F6, 0xBF926603, 0xE5FEE500, 0xE010655C,
+0xBF61E403, 0xE5130F54, 0xE40EBF63, 0x05FCE010,
+0xBF63E40E, 0xE5007585, 0xBF64E403, 0xE500E640,
+0xBF71E403, 0xE500E640, 0xBF78E403, 0xE5FFE640,
+0xE014655C, 0xBF47E404, 0xE40F0F54, 0xE504BF49,
+0x05FCE014, 0xBF49E40F, 0xE5017584, 0xBF4AE640,
+0xE501E404, 0xBF57E640, 0xE501E404, 0xE404E640,
+0xAF5C7F18, 0x7F184F26, 0x000B4F26, 0x4F220009,
+0xD2427FF0, 0x88016021, 0xD2418B71, 0x26686621,
+0xD2408B6D, 0x26686621, 0xE50F8B69, 0xE401BF1C,
+0xBF1EE501, 0xE586E400, 0xE400655C, 0x2F50BF1E,
+0xBF1BE401, 0xE401E506, 0xBF1C6543, 0xE401E640,
+0xBF296543, 0xE401E640, 0xBF306543, 0x65F0E640,
+0x756CE402, 0xBEFF6053, 0xE40280F4, 0xE512BF01,
+0xE40284F4, 0xBF017090, 0xE6406503, 0xBF02E402,
+0xE640E500, 0xBF0FE402, 0xE640E500, 0xBF16E402,
+0xE5FEE500, 0x6053655C, 0xBEE5E403, 0xE51380F8,
+0xE40EBEE7, 0xE40E84F8, 0xBEE77085, 0xE5006503,
+0xBEE8E640, 0xE500E403, 0xBEF5E640, 0xE500E403,
+0xBEFCE640, 0xE5FFE403, 0x6053655C, 0xBECBE404,
+0xE40F80FC, 0xE504BECD, 0xE40F84FC, 0xBECD7083,
+0xE5016503, 0xBECEE640, 0xE501E404, 0xBEDBE640,
+0xE501E404, 0xE404E640, 0xAEE07F10, 0x7F104F26,
+0x000B4F26, 0x00000009, 0x001E102F, 0x001E1080,
+0x001E1090, 0x001E103F, 0x001E103E, 0x00203A4A,
+0x00203A4C, 0x00203A4E, 0xD21DD11C, 0x66206010,
+0x676C7001, 0x3700C90F, 0xE5008D13, 0x67106210,
+0x7701622C, 0x64232170, 0xD6166010, 0x44084408,
+0x3428C90F, 0x62602100, 0x7201D513, 0x44082620,
+0x000B354C, 0xD10F6053, 0x25586510, 0xE6008D13,
+0xD60DD40B, 0x655C6540, 0x47086753, 0x37584708,
+0x47086540, 0x24507501, 0x367C6040, 0x2400C90F,
+0x72FF6210, 0x000B2120, 0x00006063, 0x00203995,
+0x00203994, 0x00203996, 0x002035BC, 0x7FFC4F22,
+0xE680D1A8, 0x666C6212, 0xD2A72F22, 0x67F36563,
+0x420B7542, 0x7F04E404, 0x000B4F26, 0xE6800009,
+0xD2A1666C, 0xE7006563, 0x422B7540, 0xE6806473,
+0xD29D666C, 0xE7006563, 0x422B7543, 0x2F866473,
+0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6, 0x7FC04F22,
+0xDB97D296, 0x72012F22, 0xD1961F21, 0x66125211,
+0x8B013620, 0x0009A0F9, 0xC9036061, 0x8B018801,
+0x0009A0F3, 0xD290DC8F, 0x64C3420B, 0x6503D18F,
+0x60111F02, 0x8B048801, 0x420BD28D, 0xAFE464C3,
+0x54530009, 0x844CEE84, 0x890130E0, 0x0009A0C3,
+0x6610D188, 0x6023626C, 0x8B718801, 0x6210D186,
+0x89662228, 0xDA86D285, 0xE0036122, 0x64221112,
+0x4018D881, 0xDD83E500, 0x814167A3, 0x77042850,
+0x647266A2, 0x6ED3D580, 0x1F457E04, 0x65521F56,
+0x64E368D2, 0x1F8874F8, 0x684369E2, 0x1F637894,
+0x1F991F74, 0x62826142, 0xD779D978, 0x1F2BD679,
+0x67726292, 0x1F1A6062, 0x2602CB20, 0xD176E600,
+0xE5401F57, 0x1F7D1F2C, 0x76011F1E, 0x3253626D,
+0x51F38BFB, 0x52F555F4, 0x25222A12, 0x55F757F6,
+0x27525AF8, 0x5DF92DA2, 0x2ED251FB, 0xD56B5EFA,
+0x54FC24E2, 0x281257FD, 0xD160D869, 0x25722942,
+0x69126782, 0x1974D866, 0xDD666A12, 0x56FE60A1,
+0x2A01CB01, 0xDA646412, 0xE9012842, 0x4A0B2D42,
+0x52FE2692, 0xD661EE01, 0x22E24E18, 0x72016262,
+0x60B22622, 0xCB01D14F, 0x2B02E202, 0x2120A03F,
+0x8B3C2228, 0xE601D55A, 0x2160E700, 0xE01C2572,
+0xC801004C, 0xD8578B0C, 0x1F8FD257, 0xE6002822,
+0x7601E57D, 0x3253626C, 0x56FF8BFB, 0x2622D253,
+0xE2FE69B2, 0x2B922929, 0x0A4CE01E, 0xE01F65F2,
+0x014C25A0, 0x741057F1, 0xEA062710, 0xDD4CE600,
+0x8446DE4C, 0x2D007601, 0x696C6844, 0x2E8039A3,
+0x8FF67E01, 0xDE487D01, 0x2EA0EA94, 0xE1007E01,
+0x7E0F2E10, 0xD12FE205, 0x64102E20, 0x6023624C,
+0x89088801, 0x55F2D22A, 0x64C3420B, 0xEE01D132,
+0xAF1A4E18, 0x55F221E2, 0x8553D13C, 0x620D6612,
+0x89063262, 0xD63BD43A, 0xE801460B, 0xAF0CD73A,
+0xD91F2782, 0x64C3490B, 0xEE01D127, 0xDA38D437,
+0x4A0B4E18, 0xAF0021E2, 0x7F400009, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0x4F2268F6,
+0x85467FF4, 0x2F01E681, 0x666C8547, 0x854881F1,
+0x81F2D209, 0x67F38542, 0x854381F3, 0x81F4E40C,
+0x65636053, 0x420B81F5, 0x7F0C7540, 0x000B4F26,
+0x00000009, 0x001C3D9C, 0x002023FC, 0x0011779A,
+0x001C36F8, 0x002035B4, 0x002014A6, 0x00203A16,
+0x002014D0, 0x002039A5, 0x002039A4, 0x002039A0,
+0x001C3B9C, 0x001C3704, 0x001C3D98, 0x001C3BB4,
+0x001C5960, 0x001C3500, 0x001C3D30, 0x001C8960,
+0x0020358C, 0x001C3D00, 0x00201610, 0x00117730,
+0x002039A8, 0x001C582C, 0x2000A000, 0x0000A000,
+0x0011778C, 0x00117792, 0x00117788, 0x0020397C,
+0x0020357C, 0x00201534, 0x001E2130, 0x00203DA0,
+0x002018C0, 0x2F962F86, 0x2FB62FA6, 0x2FD62FC6,
+0x4F222FE6, 0xD19B7FEC, 0x2F12E000, 0x6103D49A,
+0x1F4281F2, 0xDD9ADA99, 0xD69A6813, 0xE0014808,
+0x460BDE99, 0x38EC4800, 0x65A21F03, 0x352052A1,
+0xA23E8B01, 0x60510009, 0x8801C903, 0xA2388B01,
+0x52530009, 0x32E0DE91, 0xD9918B10, 0x64A3490B,
+0x4B0BDB90, 0xDE906403, 0xD791D690, 0xEC01D591,
+0x2E02E100, 0x271026C0, 0x2502AFDF, 0xC8018551,
+0xA1578B01, 0x62510009, 0x4200622D, 0x5E53366A,
+0x85E2226D, 0xC903642C, 0x85E36603, 0x6053650D,
+0x40214021, 0x4500C93F, 0x322A6703, 0x6053252D,
+0xC901D17F, 0x60106C03, 0x8801D97F, 0xDB7F8B05,
+0x2120E200, 0xCB0160B2, 0xD17D2B02, 0x88016011,
+0x65A28B0A, 0x8D042448, 0x9B9E6251, 0xA00322B9,
+0x919B2521, 0x2521221B, 0x37B3EB10, 0x2448895E,
+0xD4738B07, 0x22286241, 0x60638903, 0xA05781F8,
+0xD5706473, 0x46084608, 0x85E26273, 0x46006B50,
+0x362C4200, 0x2BB8C910, 0x8F1F6463, 0x26686603,
+0xD2698911, 0x062D6043, 0x4119616D, 0x6B0E6019,
+0x81F820BD, 0x880160C3, 0x646C8F2C, 0x880F6073,
+0xA0278B1B, 0xD2610009, 0x052D6043, 0x4119615D,
+0x670E6019, 0x645C207D, 0x81F8A01C, 0x890F2668,
+0x6043D25B, 0x6B5D052D, 0x60B94B19, 0x201D610E,
+0x60C381F8, 0x8F0D8801, 0x6473645C, 0xEC00A00A,
+0x6043D254, 0x625D052D, 0x60294219, 0x207D670E,
+0x81F8645C, 0x880285F8, 0x85E1890A, 0x8D07C820,
+0xE6DC6203, 0x60232269, 0x81E1A002, 0x644CE4FF,
+0x6210D149, 0x89012228, 0x644CE4FF, 0x654DEBFF,
+0x35B06BBC, 0xDB368B2B, 0x64A34B0B, 0x410BD135,
+0x54036403, 0x85446E03, 0xC948DB40, 0xDC408808,
+0xBEAC8B01, 0x64B3E502, 0x65E34C0B, 0xDB3DEC01,
+0xD13D2DC2, 0x621260B2, 0x72017001, 0x21228805,
+0x2B028F08, 0x666CE680, 0x6563D238, 0x7549E700,
+0x6473420B, 0xA030D436, 0x7FFF0009, 0x85E28000,
+0x20B9EBFC, 0x610381E2, 0x942A85E3, 0x62032049,
+0x450885F8, 0x81E2201B, 0xC90160C3, 0x40084018,
+0x40084008, 0x4000225B, 0x6023220B, 0x85E481E3,
+0x4118E108, 0x81E4201B, 0xE40262A2, 0x20B98521,
+0x67A28121, 0xCB016071, 0x85F82701, 0x89033042,
+0xECE785E2, 0x81E220C9, 0x490BD41E, 0xA03B0009,
+0x7E030009, 0x001C3D30, 0x00203DAC, 0x0020358C,
+0x001E212C, 0x00203470, 0x001C3D00, 0x00117780,
+0x002014A6, 0x00201670, 0x0011770C, 0x002039A4,
+0x002039A5, 0x002039A0, 0x002018C0, 0x001C36F8,
+0x00203A1A, 0x00203DBC, 0x00203BA0, 0x00203C20,
+0x00203CA0, 0x00203D20, 0x00203990, 0x00203584,
+0x002014D0, 0x00203A1C, 0x00203A20, 0x002023FC,
+0x00203DA4, 0x00203DA8, 0x602262F2, 0x40094019,
+0xC90F4009, 0x8B0B880A, 0x60E2DE8C, 0x40094019,
+0xC90F4009, 0x8B038808, 0xCB0160A2, 0x2802A006,
+0x65E2DE87, 0x2E527501, 0x286266A2, 0x52F366F2,
+0x2622AE83, 0xD2838551, 0xDE83C802, 0xA0958B01,
+0x420B0009, 0x4E0B64A3, 0x5E036403, 0x85E46503,
+0x4918E908, 0xD77D209B, 0xE04C81E4, 0xDC7C0B7E,
+0x7B01D97C, 0x61C207B6, 0x71016690, 0x8D062668,
+0xD4792C12, 0x420BD279, 0xA070EB01, 0x62512DB2,
+0x4B18EB0F, 0x22B9E102, 0x32104118, 0x85518B0F,
+0x2029E2FC, 0x60518151, 0xCB0172E0, 0x85E12501,
+0x202994A3, 0x85E481E1, 0xA0522049, 0x675181E4,
+0x4719677D, 0x667E6779, 0x7701276D, 0x6903607C,
+0x88014918, 0x25918F3E, 0x6B12D161, 0x21B27B01,
+0x660D85E3, 0x40216063, 0xC93F4021, 0x6C034600,
+0x262D322A, 0xC8016063, 0xDB5ED15D, 0x967D8901,
+0xE6002C6B, 0x666C67CD, 0x40006063, 0x622D021D,
+0x8D0E3270, 0x60436403, 0xE9FF021D, 0x8B013290,
+0x01C5A007, 0x626C7601, 0x3292E904, 0x646C8BEB,
+0x60434400, 0xD15004BD, 0x0B457401, 0x669D6911,
+0x89073670, 0x602D6211, 0x890388FF, 0xE201DB4B,
+0x2B2021C1, 0xECFC8551, 0x815120C9, 0xCB016051,
+0xDC472501, 0x64A34C0B, 0x51F366F2, 0x85EF2612,
+0x54F2D244, 0x650D420B, 0x0009ADE7, 0xE500DC42,
+0x420B2C52, 0x4E0B64A3, 0x54036403, 0x85446E03,
+0x6703E908, 0x65034918, 0x27998541, 0xDB323790,
+0x8F0BD932, 0x6013610D, 0x8B07C820, 0xC9486053,
+0x8B038808, 0xE501BD4B, 0x0009A005, 0x2128D233,
+0xBD448901, 0x64B3E500, 0x490B65E3, 0xADBCEC01,
+0x85F22DC2, 0x7001EE04, 0x31E7610D, 0x8D0281F2,
+0xADA97A08, 0x7F140009, 0x6EF64F26, 0x6CF66DF6,
+0x6AF66BF6, 0x000B69F6, 0xF7FF68F6, 0x2FE68000,
+0xD2234F22, 0x60E36E22, 0x8D02C840, 0xBBE522E2,
+0xE2400009, 0x2E284218, 0xBBF08901, 0x60E30009,
+0x8905C810, 0xD21CD41B, 0x0009420B, 0x0009BBEF,
+0xC80560E3, 0xBD6D8901, 0x60E30009, 0x8902C802,
+0xABEC4F26, 0x4F266EF6, 0x6EF6000B, 0x001C3D3C,
+0x00117760, 0x002014A6, 0x00201670, 0x0020351C,
+0x00203DC0, 0x00203990, 0x00203584, 0x002014D0,
+0x002039FC, 0x00203A04, 0x002039F8, 0x002039FA,
+0x00201534, 0x002018D0, 0x00203A1C, 0x00008000,
+0x001C3510, 0x00203DB4, 0x002018C0, 0x89014F22,
+0x611B600B, 0x611BB00A, 0x000B4F26, 0x600B600B,
+0x611BA004, 0x8DF12107, 0x8BF84011, 0x620D2F26,
+0x8F3E3020, 0x40180019, 0x8B0B3016, 0x31043104,
+0x31043104, 0x31043104, 0x31043104, 0x412462F6,
+0x601C000B, 0x41296219, 0x20084018, 0x31048926,
+0x31043104, 0x31043104, 0x31043104, 0x31043104,
+0x31043104, 0x31043104, 0x31043104, 0x61193104,
+0x3204221D, 0x32043204, 0x32043204, 0x32043204,
+0x32043204, 0x32043204, 0x32043204, 0x32043204,
+0x212D3204, 0x601962F6, 0x4024000B, 0x000BE000,
+0x621362F6, 0x41294228, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x602D4224, 0x62F6000B,
+0x080A0C0E, 0x00020406, 0x1A1C1E20, 0x12141618,
+0x2E303234, 0x26282A2C, 0x3A3C3E40, 0x6C625648,
+0x41112F26, 0xE2208F18, 0x890B3123, 0x321CD204,
+0xD1026220, 0x412B312C, 0x00090009, 0x0020349A,
+0x00203450, 0x000BE000, 0x400062F6, 0x40004000,
+0x40004000, 0x40004000, 0x62F6000B, 0x40004000,
+0x40004000, 0x40004000, 0x40184000, 0x62F6000B,
+0x40004000, 0x40004000, 0x40004000, 0x40284000,
+0x62F6000B, 0x40004000, 0x40184000, 0x000B4028,
+0xC90F62F6, 0x40054005, 0x40054005, 0x62F6000B,
+0x4005C907, 0x40054005, 0x62F6000B, 0x4005C903,
+0x000B4005, 0xC90162F6, 0x000B4005, 0x000062F6,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x42707372,
+0x3D206675, 0x554E203D, 0x202C4C4C, 0x6E49677A,
+0x4E497274, 0x6D754E51, 0x0000003D, 0x61766E49,
+0x2064696C, 0x72657375, 0x20726F20, 0x2079656B,
+0x00214449, 0x6E6B6E55, 0x206E776F, 0x6D6D6F63,
+0x3D646E61, 0x00000000, 0x203A3051, 0x00000020,
+0x203A3151, 0x00000020, 0x203A3251, 0x00000020,
+0x203A3351, 0x00000020, 0x203A3451, 0x00000020,
+0x2B434741, 0x73696F4E, 0x61432065, 0x7262696C,
+0x6F697461, 0x6166206E, 0x6F206C69, 0x6974206E,
+0x0D0A656D, 0x00000000, 0x00000072, 0x00205220,
+0x62735576, 0x7473725F, 0x00000A0D, 0x62735576,
+0x7375735F, 0x646E6570, 0x00000A0D, 0x62735576,
+0x7365725F, 0x000A0D6D, 0x00000044, 0x44387570,
+0x72637365, 0x6F747069, 0x3D584572, 0x00000000,
+0x00000047, 0x72746E49, 0x6D652051, 0x2C797470,
+0x49677A20, 0x4972746E, 0x754E514E, 0x00003D6D,
+0x654C7245, 0x0000006E, 0x20746F4E, 0x756F6E65,
+0x49206867, 0x4220514E, 0x0A0D6675, 0x00000000,
+0x000000FF, 0x00020001, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00020003, 0x01090108, 0x0002010A,
+0x02000003, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x000000FF, 0x00020001, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00020003, 0x01090108, 0x0002010A,
+0x00030003, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00FF010F, 0x01090108, 0x010B010A,
+0x0200010F, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00FF010F, 0x01090108, 0x010B010A,
+0x010F010F, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00205220, 0x00000046, 0x00000059, 0x73204142,
+0x003D7165, 0x49544120, 0x0000204D, 0x00000000,
+0x00000000, 0x002E0209, 0x80000101, 0x000409FA,
+0x00FF0400, 0x05070000, 0x02000201, 0x82050700,
+0x00020002, 0x03830507, 0x07010040, 0x40030405,
+0x02090100, 0x0101002E, 0x09FA8000, 0x04000004,
+0x000000FF, 0x02010507, 0x07000040, 0x40028205,
+0x05070000, 0x00400383, 0x04050701, 0x00004002,
+0x00000000, 0x00000000, 0x07090000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, };
+
+const u32_t zcP2FwImageSize=15964;
diff --git a/drivers/staging/otus/hal/hpfwbu.c b/drivers/staging/otus/hal/hpfwbu.c
new file mode 100644
index 000000000000..f60f57ed887a
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwbu.c
@@ -0,0 +1,5269 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+
+const u32_t zcFwBufImage[] = {
+0x3A4BCF18, 0xF44C076E, 0xF59452EA, 0x451BA755,
+0x140AC87A, 0xC07EE942, 0x3EF978AB, 0xF5B03DC6,
+0xB70080F0, 0xA89064EA, 0x54E2C1D6, 0xEB047DF4,
+0x1798390C, 0x00350624, 0x35B3ECF0, 0x59F2FABF,
+0xAF9D248E, 0xF9BDB9F0, 0xD05C8B47, 0xC08B5A16,
+0x990093C7, 0xD335A160, 0x1C04942C, 0xBF6E7A88,
+0xFD232B0F, 0x5C224387, 0xBF1E156C, 0xF24F2A27,
+0xFF56421D, 0xB213037C, 0x2BA67BA0, 0x4950CF8A,
+0x05F00F25, 0xA5E82085, 0x74168A0C, 0x2F2AB30B,
+0xC80C57EE, 0xB6BDF570, 0x89BC5A99, 0x7F3B5A67,
+0xF6C943B8, 0x0C9C9201, 0xE8383747, 0x0C9A72D6,
+0xE0520704, 0xA66D7F30, 0xE444A434, 0xE0C94AB7,
+0x8DD7751C, 0x1A659464, 0x6C9ABA4F, 0x792F2D2D,
+0x5936F66B, 0x061E580E, 0x59903F6C, 0x1FBFB8A0,
+0xCC822EFE, 0x4B030CAF, 0xB62457C9, 0x27E9BF15,
+0xB113A487, 0xFA0FC915, 0x447B184A, 0x5330CD51,
+0x00BCC622, 0xF30DE149, 0xFF718E1C, 0x7B5D2861,
+0xDBCA573E, 0xFB0D7BF9, 0xE1CFBAAC, 0xF99D4583,
+0x4BA7498A, 0x7CAEA7EB, 0xBA958E32, 0x36C530FF,
+0x8F88CA99, 0xF93CABC2, 0x8E47EF11, 0xFB0EED6F,
+0x5B3668A1, 0x9D63ADDE, 0xA0EEAB8C, 0x084915F1,
+0xFACAAA27, 0x209638FE, 0x1CED9EFF, 0xEEBD2335,
+0x38C6F424, 0x2D1F3D7E, 0x976E8106, 0xBE087AD2,
+0x32194845, 0x756066DB, 0xC70E3165, 0xC568DCB1,
+0x3212E4E1, 0xB5D991AD, 0x07C3CEF8, 0xDB4ABB38,
+0x1574C232, 0xF8C792BC, 0x14E62DBE, 0x5A48E7DC,
+0xEFDC5407, 0xC45B4017, 0x3B814E89, 0xF0936466,
+0x89491B2B, 0x9A359A41, 0x82287675, 0xA0F338D3,
+0x523FDD3C, 0x4E40B795, 0x458ADAA4, 0xED812957,
+0x7ADC73BC, 0x6FD7DB78, 0x2740FC04, 0x6392AEA3,
+0x185ABCEA, 0x6B50ABC3, 0x3681F07F, 0xC840F8CE,
+0x5733E7EC, 0x0805FA71, 0x0B34530A, 0x8CB3D033,
+0x81451551, 0x53B0B4EC, 0x908646D0, 0x10A3E642,
+0xF358DC34, 0xC1FA570C, 0x2B1284B0, 0x592322BB,
+0x9D587783, 0xE7D77988, 0xE1BE5D7B, 0x44B93E23,
+0xF8BE94A2, 0x506DC723, 0x6E0A7D09, 0x3FB1046F,
+0xDB3A166F, 0x9CB7D6A0, 0xE278DE6D, 0x88459334,
+0xB52BA3C9, 0x284740A2, 0x04D30792, 0x944D79CA,
+0x1D050EA9, 0xA404DB1B, 0x99526023, 0xBACE24E7,
+0xB9F20704, 0x284E6432, 0x47A593D1, 0x95F8DFCB,
+0x220C9167, 0x8FAABBBC, 0x93D34E8C, 0xCE077138,
+0x4FC18081, 0xE76DD7E5, 0x67465F6C, 0x7A479D77,
+0x74D61F82, 0x00559214, 0x2F66E42E, 0x8742A96B,
+0x62063950, 0xA2DBFAE5, 0x368B966F, 0xAB5FCCE1,
+0xCB4023B1, 0x1E7AF542, 0x05953E30, 0x8CA51CFC,
+0x2216547D, 0x29D562D4, 0xE9C9F8EE, 0xA90505C9,
+0x088D0EEB, 0xD7A290FA, 0x95E5B567, 0x53FAD3C0,
+0xB89FC625, 0x69A7519B, 0x3687C7EF, 0x7188CB55,
+0xCE5DB97E, 0xA260574A, 0xD453D173, 0x145D970B,
+0x12112CC6, 0x399839E0, 0x29C55BEB, 0xE467C71F,
+0x10B3C9D4, 0x8F1C9662, 0xF207A826, 0xE0245600,
+0x688B1812, 0x5A483031, 0x7048380A, 0x78E3D5BB,
+0x1951533D, 0x8FA5D8E3, 0xC5BE500D, 0x71DB5B2B,
+0xA17AA000, 0x408C9BE8, 0x161E12F5, 0xB1C38C45,
+0x22A88F05, 0xDE3F4405, 0x5078ADBB, 0xCE1BF1A6,
+0xB7A75B04, 0x6B8364E8, 0x0CE32E3E, 0x9BF65504,
+0x28C18157, 0x78359AC6, 0x617BF202, 0x1E76FA09,
+0x0F8E61A8, 0x6D02F0D5, 0x80356459, 0x79CEFAE7,
+0x7D00F155, 0x5C1C0128, 0xC75CA073, 0x32816090,
+0x9FF78DFC, 0x848D269C, 0xF811B314, 0xA86920FC,
+0x6F885D01, 0xACFE6525, 0xC726074D, 0xFED68599,
+0xF1D5C76A, 0x8799E5F5, 0xF85F5171, 0xD8DE2D3B,
+0xE7DD8E75, 0x43F8614A, 0x0684FC8D, 0x9683B8C8,
+0x74BE786B, 0x2514762D, 0x7D866682, 0xE711FE1F,
+0x0DE9E273, 0x12F53167, 0x4FA3A7FE, 0x2A00EB61,
+0xB3984A28, 0x4319F2B0, 0x42BA0CA2, 0x848771B6,
+0x995E945E, 0xD41115F5, 0x43D9834B, 0x54EEDC36,
+0x5C3C5407, 0x671B540E, 0xDCF18948, 0x150ED973,
+0x2D4922DE, 0xF93CA17D, 0xB24A76E5, 0xD1C01C22,
+0xF2963DD6, 0x3B860066, 0x08EF0EA4, 0x609B60CC,
+0xE2E901FA, 0x25BE9B93, 0xDF96D9BC, 0x86D415DF,
+0x75CCF6BB, 0x882D54B2, 0x7976E9AF, 0x88A0B178,
+0x5ADE5A55, 0x8A8C0112, 0xD896755A, 0xCB6789B3,
+0x8B63AE2F, 0x2545036C, 0xE4655B94, 0x20959977,
+0x29DFB4D1, 0xCDAAEBF4, 0x1C07EC05, 0x5A6F607D,
+0x88A9B31D, 0x118C74D2, 0x000BB065, 0x75C46712,
+0xEF1A58BD, 0x50ECA262, 0xCCE393B9, 0x6EDB92E8,
+0x700EF517, 0xBF6CF4AD, 0x57456DC0, 0xF629517C,
+0x40331F8B, 0xC10A454D, 0x6CCB02CF, 0x9BF11B1C,
+0xE0871437, 0x23623585, 0xF519F09C, 0x4DB2AFC8,
+0x88FCBD7B, 0xB512FE8D, 0xDE445894, 0x078AD03C,
+0x44375FB0, 0x0BABEDB1, 0x40D5E8E1, 0x13F20A86,
+0xF1406303, 0x7205C322, 0x3FC43779, 0x7A60D510,
+0x14469E04, 0xF4E77873, 0x2EAD7ECE, 0xA135D6EA,
+0x3F4C4B30, 0x21488077, 0x69F64F1C, 0xEEF4876E,
+0x63610C0B, 0xB7B24C5C, 0x324A76FE, 0x0CF651D3,
+0x9460F0B1, 0x81A83230, 0x0839CFF9, 0x70722F04,
+0xC278FB3B, 0x5DD1BDA4, 0x1E4B3DBA, 0xAE161A93,
+0x9E1033C3, 0xD938FCEC, 0xDA2B2F93, 0x28CD82EA,
+0x14AD1FAF, 0xE4EC9CB8, 0xE770AFDF, 0xEFB12898,
+0x500BE181, 0x602625C5, 0xF160631B, 0x78D3643F,
+0x4E13ED37, 0x647BB223, 0xCF18D75C, 0xF477F94C,
+0x786ECB89, 0xB3ED21F8, 0x1BEF3916, 0x240FB35A,
+0x5C69B7D9, 0x8E96290A, 0xD40DC98C, 0xD1370291,
+0x5870021E, 0x3F7CF23F, 0xFD4A6ADA, 0x36482457,
+0x926600AF, 0xDC8618BC, 0x67D3779F, 0x3422830C,
+0x87A41FBA, 0xCA0AFF53, 0x63BC45F3, 0x520BBBAE,
+0xEDE2E031, 0xB6FA9450, 0x258CA712, 0xD709C4E4,
+0x617709B2, 0xAACE0B41, 0x363DBF55, 0x701D6583,
+0x39F3C885, 0x7CD6297B, 0x078FE13B, 0xA398DABF,
+0xDB97C514, 0x039102E3, 0x5CA545AF, 0x9298BA18,
+0xD18DAF86, 0x3D70EEA2, 0x5266AD68, 0xB04945B5,
+0x402DDA5B, 0x01DC6CD1, 0x93AC5053, 0x08DF9EA9,
+0x485EBE97, 0xA5D05853, 0x6CBEE910, 0xD485F4E2,
+0x8F201D07, 0xEFC384A3, 0x7272AFBE, 0xC0B41FD7,
+0x8E54A971, 0xA7F9E0F7, 0xC21700B4, 0xC24A4ED0,
+0x5419EACF, 0xBC2D8FB1, 0x2C5B5AFF, 0x0345274C,
+0xC41DF47A, 0x37658AFF, 0x24CF3BE7, 0xA3086248,
+0xF82B5928, 0xB49A9B04, 0xD4105AEF, 0x444EBE8D,
+0x348368DF, 0xDC77A7A0, 0x68D37E0D, 0xD2EB54EE,
+0xDDAC8C33, 0xE5C93C79, 0xE4706ABF, 0x17536EFD,
+0x6C2B2B16, 0x038AA806, 0xDAD42458, 0xAE1D76A1,
+0xCC8DE95C, 0x1BA20647, 0x0521068C, 0x306FBE44,
+0x4E29D881, 0xD2A14D53, 0xA155853E, 0x44500CC4,
+0xFC4466B7, 0x5AACD51D, 0x506D3A73, 0x3F61E0FE,
+0x58F11F9D, 0xC92A2CAD, 0xD9A4F86B, 0x1FA747B1,
+0x77DEC5D2, 0xDFAB369A, 0xD471EA01, 0x724502DA,
+0x618CE21A, 0x52388BEB, 0x2E8A4CC5, 0x58332211,
+0x3FCC46E1, 0x501210E2, 0xE9D51D1A, 0x37237B55,
+0x8CE3E2F1, 0x6B2E98CC, 0xB56A11E5, 0x8819036B,
+0xA6AA2F27, 0xB0124A0E, 0x92F17364, 0xD4A89238,
+0x0507E337, 0x8ED95DEC, 0x9C014BA8, 0xBA5B11C6,
+0x9C15D38C, 0x52596C98, 0x9330DD3D, 0xD6147570,
+0x21701F1B, 0x5A2385F1, 0xE2F38C6C, 0xB3E94698,
+0x2F9C63FA, 0x7E0234D8, 0x4CDD3288, 0xE1969B5F,
+0x853B3C1D, 0xF61465A7, 0xF281C419, 0x46C5F072,
+0x9F1722DD, 0x64F2A994, 0x86AEE8A8, 0x55895E17,
+0x6047D1AC, 0x3375A934, 0x336BEACA, 0x90791174,
+0x4DACC4D2, 0x24253860, 0x2A7876FB, 0x9DBDF98D,
+0xD5BCE182, 0x67EB5F70, 0xCC06BA38, 0xE8F78715,
+0xFEB0EB44, 0xE9776E03, 0x892A0898, 0x7A070650,
+0x6D04DDC4, 0x99A5B7EA, 0x3B416BB6, 0xDADCE834,
+0xB3B03278, 0xDB73B70E, 0xB0F0224E, 0x538A4AF9,
+0xD25D6A37, 0x8F627FB0, 0x11ED9387, 0xB8C88457,
+0x0CF320CA, 0xA20E62A2, 0x1DACDD4A, 0xAB84575D,
+0x740DAF75, 0xAB9DB955, 0xFF787314, 0xA680B8E3,
+0xC976D38E, 0x1FD38F4D, 0x0AEB6633, 0xB69A03DF,
+0xB6CA8610, 0x106354C2, 0xC37D48C8, 0x3E5EED54,
+0x534CC9BA, 0xE37DFFAD, 0x9F69EB05, 0xF67217EE,
+0x50180B3D, 0xCC61C127, 0xC3598D73, 0xE5C00F01,
+0xFFE9B111, 0x5E23EA2F, 0xF6C45DCE, 0x44585E39,
+0xB02C6004, 0x37233902, 0x4F374C0D, 0x34288898,
+0xE274937D, 0xC81D472C, 0x17A43151, 0x2638F7D3,
+0x5304E5B5, 0xD5CE5EDE, 0x357FA7B3, 0xFBE27986,
+0x64E65D1F, 0xC28D1237, 0xA73D9AB3, 0x124CA6C8,
+0x770D7415, 0x5788C32C, 0x18DEFC00, 0xB3B2B06A,
+0x55CC86A0, 0x8D929309, 0x84AB381A, 0x9DEFE8DD,
+0x26C742C8, 0x952BAC34, 0x0A3B140F, 0x82A9304B,
+0x52CEC9F4, 0x47DF4D08, 0x15A116D8, 0x7B890B18,
+0xC87BEF1A, 0xB59601B6, 0xD37BFB28, 0x5D9F564D,
+0xFB002F8D, 0xE7602E57, 0xE429C852, 0x9C0A8C75,
+0xE02611DC, 0x8A1C9861, 0x7495D6DE, 0xCA059710,
+0xAE5969B8, 0xE5B2CBDC, 0xA49F6EC1, 0x85D2A553,
+0xE4719B0F, 0x40F68BBC, 0x092E24B5, 0x7B132678,
+0xD70C17E1, 0x309E6AA1, 0xE009657F, 0xA7238C7A,
+0xE0575D78, 0x1D6980E7, 0xEFCDD368, 0x19F08D93,
+0xFAC03B85, 0x51BADA8F, 0x037DF839, 0x8F4D29F4,
+0x1DC8A913, 0x50C55402, 0xDEE578F0, 0x2BA1C091,
+0x9ACA567E, 0xA8FFECFA, 0xA3C05D12, 0xF18C6283,
+0xEAAE6662, 0xB4DC6A79, 0xCEC5E782, 0x93A2E384,
+0x8F8A5E6F, 0xCA8379D5, 0x81BCD49E, 0x5FCE174B,
+0xD1543A5B, 0x845D635F, 0xD53125B9, 0x3B2121AE,
+0xF8ECDD01, 0xF84D2D11, 0x6579BC21, 0x5C2DC220,
+0x9EC1A688, 0x1148D831, 0x6C087799, 0x58944357,
+0x56F79FC6, 0x6B689B55, 0x740B5FD1, 0x9F7BFB5F,
+0x6B2F3E2D, 0x10E09273, 0x2E9E3213, 0xF3436AA0,
+0x14A9F681, 0x9087D3CE, 0x68D0430B, 0x9FAFE3EF,
+0xD45B8C61, 0xB982724A, 0x04448D7F, 0x8712E47A,
+0x2C188D15, 0x9C3F06CC, 0x6343B130, 0x56C6765C,
+0xF657BC9A, 0x15F1E973, 0x47E71181, 0x8639F5D7,
+0xC1F3FDD5, 0xDC522441, 0x56BB2908, 0xAA48AEC6,
+0xEC04087A, 0x8D375875, 0xE2941F88, 0xED31CC72,
+0x09BD8794, 0x4C81D5C1, 0x1CC96D9C, 0x98A89022,
+0xAA362C57, 0x924D583D, 0x270430E6, 0x0FD4040A,
+0xAF561155, 0x38DCD1CF, 0xE861D2AC, 0x24A2EF3C,
+0x2B7E3868, 0x13DA6C12, 0x69202EB6, 0x4A5FEC66,
+0x185417A9, 0x3C92EFF4, 0x949842E6, 0x02115D93,
+0xAD1726FF, 0x4E093D7D, 0xC3E41B9C, 0x27BBC1C1,
+0x4FFA49C7, 0x6C63D24C, 0x84255444, 0x282C3BA2,
+0x3D679D86, 0x03B410B1, 0x64DB454C, 0x535499D4,
+0x25B421A1, 0x7E68C8FE, 0x0477E3B9, 0xCEFB087D,
+0x9E59B89C, 0xBB787559, 0x1A550EE4, 0x078B48AB,
+0x73A865FE, 0xD7227471, 0x3A864049, 0xE5EE3A1D,
+0x201BC19D, 0xEB8DAE2C, 0x0E2AB31D, 0xCDAC2D79,
+0xDAAB08B1, 0x63ECD4F2, 0xC00F9716, 0xD415C6BB,
+0x8C20C39F, 0xDED8F5A2, 0x1D6A4190, 0x3D319167,
+0x56B3A26B, 0x0547BF52, 0xA056924F, 0x4DAA539A,
+0x557241D1, 0x42C9124E, 0x18723323, 0x6AD6E7EC,
+0x8E039337, 0xF6FDDD65, 0x5F3525F9, 0xC0AD9704,
+0x810EF049, 0xCE022EE0, 0x41CE7E52, 0x8E172A44,
+0x648808E2, 0xC7FF6896, 0x2AD0985C, 0x304B9631,
+0xD21EA39B, 0x279F5089, 0xCDB5C390, 0x21716A40,
+0x5E34B278, 0x39475D72, 0xBA4F4DB1, 0x8B25818F,
+0xE6E466F4, 0xC4A09DF8, 0x59F18AC7, 0x887AB5FE,
+0xEEA4BA42, 0x17371DA8, 0xA82193D1, 0x6DC30EF7,
+0xDEB9D349, 0x2B3271D4, 0x1FE83836, 0xEC755A29,
+0x05F07FCD, 0xC331D3AE, 0xC6208B76, 0x497FF280,
+0x4C579C5A, 0x22B71F94, 0x30FD620B, 0x31B71AE3,
+0xDF7D1A41, 0xF041ACA5, 0x9533261B, 0x3262D291,
+0x060E9672, 0x7D191A55, 0x6D0F0945, 0xF8C7777C,
+0x1C173808, 0x78308E77, 0xC1EEAD3B, 0x059CCD9D,
+0xA8FDBE19, 0xE47630FA, 0x88A49DE5, 0x03347DAB,
+0x4F31F969, 0xF9C62B12, 0x93AB126F, 0x8A7A3BFB,
+0x82591545, 0x2A1A2131, 0x1DEBB134, 0x449E28DD,
+0xFA7E0248, 0xC1E3A5BC, 0x1747E097, 0x4C69AA5C,
+0x1FD71B4B, 0xAC64CA6C, 0x5545F9F9, 0x5E5886F2,
+0x243DBA6C, 0x495BE163, 0x4ECF5A6C, 0x430C9019,
+0x89A980FA, 0x528945AE, 0x00CE6936, 0x9F9A73B2,
+0x9E59DC6B, 0xD57740CD, 0x0E0CB735, 0xB1202BE3,
+0xAA26C2A9, 0x267A77A6, 0x3FA12CF0, 0x4587C0AF,
+0x354ED831, 0xFFD8BD8E, 0x56CC0F26, 0x75717AE3,
+0x51B10674, 0x3E33EC26, 0x26CE80DB, 0x5C4A9140,
+0x017F6C2F, 0xF9038D9A, 0x0A22C29F, 0xBA1F7C8D,
+0x125CC934, 0x6CF66BFF, 0x48C13DCD, 0x63FC3D81,
+0x258C181D, 0x1A4C3DDD, 0x2E24BECC, 0x7C86A9ED,
+0x5BD1989C, 0x57CE595C, 0xDF291AFE, 0xEAF00887,
+0xD8DD4259, 0xDF67331E, 0x50D0CE88, 0x1FD090AE,
+0x632DA5F0, 0x95272A5B, 0x31172F25, 0x547FD7DF,
+0xAFBE11D9, 0x97189DFC, 0xC4881191, 0x1C92365D,
+0x843DEFDE, 0xCF0A399B, 0xCF327CAF, 0xDDAF0BCE,
+0x03AA7A2E, 0x411A8664, 0x6CCF7CD9, 0x61097EF5,
+0x07F3941E, 0x5BC3EB75, 0x2791945F, 0xBEBB526E,
+0x18631A34, 0x25FEBF10, 0x419834CF, 0xF642D176,
+0x372FFF10, 0x2A1BEA1B, 0x400FF345, 0x257A234A,
+0x9F15E99D, 0xE06AA1DB, 0x3A0DB315, 0x2BA30D99,
+0x0E9E831E, 0x1B25EE41, 0x8DB30E70, 0x9FBA6D64,
+0xAB8AA5E3, 0x5A96177A, 0x6BE03535, 0x97E37DCE,
+0xACA24F26, 0x5F0096F7, 0x5D02722F, 0xAF8F3EC7,
+0xA6824151, 0x70FAD406, 0xDEBA8513, 0x99C63E34,
+0x1CC4A3DF, 0x7F756508, 0xB7386527, 0x647C7FB8,
+0x43F1F4DF, 0xC7E4EC18, 0x302BA109, 0xD5E9175B,
+0x82856F77, 0x0F6D45D9, 0x95AE28B9, 0xE63385C3,
+0x8FB26619, 0xBD99F298, 0xC884B948, 0x0B596FF1,
+0xE061C3F9, 0xBC2F9A81, 0xC488CD91, 0x372EF590,
+0x3DA1BFE5, 0x10DE037B, 0x7210B4DC, 0x74E4EFF8,
+0x6365AFD2, 0x8CEABC85, 0x1D8FFD43, 0x4DE243F8,
+0xEC976FD9, 0xAD827765, 0xC679F15D, 0xC125EC31,
+0x95D3481C, 0xC4EA6EAC, 0xC8FC014F, 0x1352EB66,
+0x9C400EB5, 0x227BFAB9, 0xB12BF958, 0x85B6D782,
+0x78B6E44D, 0xE2232EEE, 0x4F101711, 0x9ABEBF69,
+0x66ACC682, 0x04AD5F55, 0xE4FC6238, 0xBA3D2266,
+0xA2BA3170, 0x083F39AB, 0xFF2075C4, 0x945C4B05,
+0x41E8C113, 0xEC7CAD67, 0x3653733E, 0x03510C3B,
+0x1E973158, 0xFBE507F3, 0x2CCD8D9A, 0x6EA9442F,
+0x0D48DE95, 0xC517BFAE, 0x04EBB5C9, 0xEFAB1823,
+0xD5FBFC0A, 0x6890F212, 0xA1C00CCD, 0x6DD561E6,
+0x20D39B1C, 0x56113FBA, 0xCF3A7FD7, 0x3AB5A0DB,
+0x3656572E, 0x7BC48CD3, 0x8902AE36, 0xD3E94AFF,
+0xC06EB447, 0xCC513C0C, 0x2544B7DD, 0x6F168877,
+0x53162607, 0x461DCEF0, 0xF47AF2BB, 0x8AF9F3CC,
+0x1EEFF9E6, 0x57CFB6B6, 0x7F712439, 0xAB20C93D,
+0x043F9003, 0x60C808BC, 0x86C2137C, 0x46ADB474,
+0x848B65F2, 0x5544789B, 0x18E9AEC7, 0xC889913E,
+0xFEB79B2F, 0xA3FBE518, 0x67922463, 0x93746398,
+0x968E160F, 0x8CA856A4, 0xA040202E, 0x660C00C6,
+0x8F0A8E62, 0xE2BA54DE, 0x4BD0C117, 0x1A1A3092,
+0x086CAA3A, 0x2BBA5676, 0x89610176, 0x00ED2F97,
+0xC72130C7, 0x5A053880, 0x7298E553, 0xD67971EA,
+0x0D41E477, 0x2FA8285F, 0xB856A190, 0x132DB916,
+0xCDFFDD11, 0xB5519A81, 0x1BC7001B, 0x97C824DC,
+0xBB4C707F, 0x90166DC2, 0x42DFAB7A, 0x90E33184,
+0x6C6B940C, 0xDC553814, 0xC4F5E7AA, 0x99434AE9,
+0x82BB09D4, 0xCB0A7DA3, 0x3A8033AE, 0x054D3481,
+0xE20AF761, 0x25F5F254, 0x7AD3AF3A, 0x23A34C29,
+0xA19C57BC, 0x39B57AD9, 0x55E1EC59, 0x5ECA4198,
+0xDB908BCD, 0x4871C3F4, 0xE7091328, 0x64A9B6EC,
+0x1CCAB2F3, 0xEDB22423, 0xFFB6A717, 0x6FA13548,
+0x361FF711, 0x24664017, 0xCBBF9970, 0x83A7B7DE,
+0x9B704690, 0x01A0B877, 0x95041B60, 0xC048F3E1,
+0xA31625F2, 0xE3DFBE27, 0xF657295B, 0x1F5C3AF5,
+0x60EE1637, 0x575EDFAC, 0x725844FB, 0x242723D0,
+0x04FA46FC, 0x1A8C3F44, 0x0E03A5FE, 0x8778079F,
+0x606E4E1A, 0x7C0AF3D5, 0x9578B266, 0x63BCE765,
+0xA8ED66D9, 0x9242377A, 0x817A5D5E, 0xD0981A98,
+0xC07F2E7F, 0x0E66F84A, 0x3635F854, 0xD7AD8359,
+0xDCF23230, 0xC1B9084C, 0xA7987FE5, 0xC3B27EB4,
+0x1F747061, 0xFD278601, 0xB6ED3B5A, 0x9CEF8AA0,
+0xA5023C46, 0xB49832AF, 0xB12055FD, 0xD85310E1,
+0x2C19ADE6, 0xEFBB17A8, 0xC246A4C7, 0xBE4B2666,
+0x13C2D7F9, 0x50063BA1, 0x9B00E02D, 0x335B9DF8,
+0xD424AF25, 0xBAE40C92, 0xE87BD6B7, 0x384D1EB1,
+0x8B91E8F4, 0x9E3FC6D5, 0x6BB1A51E, 0x21AE5533,
+0xFCB8E713, 0x188B66B1, 0x6572E9ED, 0x98829178,
+0x7BAE8CBF, 0xE00C32B4, 0xDAFC14D5, 0xEA8FC746,
+0x2C8D712E, 0x89A05FC9, 0x9A274641, 0xAC2450AD,
+0x2437784F, 0x3B1B80F0, 0x0B4A31FD, 0x277C0232,
+0xFDDC6829, 0x3F3C606C, 0x0EF62352, 0x3D07D04A,
+0x4E0939E8, 0xD59BF115, 0xA02752E7, 0x42BF7133,
+0x9FA0939E, 0x64764109, 0xD5D03EBA, 0x3D4433A3,
+0x1749B437, 0x137298B1, 0x677BE344, 0xA83CEF7E,
+0x17813A39, 0xBC71823F, 0x2070E9A7, 0x3873AEF8,
+0x5AF1E21B, 0x1F0CC692, 0xB8EFB04D, 0x1A1CC514,
+0xADED6C3D, 0xDF35A8D7, 0x6D93275E, 0x9C362545,
+0x62BF7583, 0xFC56D990, 0x0CD6A324, 0xF12A7939,
+0x52587029, 0xD00D5F16, 0x51622555, 0x1178E887,
+0x81E7BCC8, 0x92BB1C11, 0x097330E4, 0xCF8C5CAF,
+0xD076D6BC, 0xBA292918, 0xF835A829, 0x4280A51E,
+0x09CD7827, 0x11583487, 0xB8BA2CEF, 0xD598AE93,
+0x99F4FD77, 0xEB151110, 0x1571B076, 0x63F2103A,
+0x56C6BF44, 0x9E63B556, 0xFB981238, 0x5D8C978B,
+0x9501D936, 0x82A1E971, 0xE5A4F7E2, 0xC6E3727A,
+0x03329F07, 0x248ACDD6, 0x437E917B, 0x23B02B20,
+0x73F76AA0, 0x75EA06C5, 0xD7C662B3, 0x267777F8,
+0xDC96BF06, 0x54020346, 0xCBDF069B, 0x030133EC,
+0xA7EF1C2E, 0x568959AB, 0x4FC31DE0, 0x3A22890E,
+0x280F8652, 0x1BD8CB24, 0x9A8D92C9, 0x52718DE1,
+0x12033FC7, 0xD48490CC, 0x681ADEE2, 0xF91BF7B8,
+0xB8609B38, 0x34CF4BCA, 0x8F123290, 0x0D0F4FCD,
+0xC4F43323, 0x2FC04F1C, 0x4669B890, 0x1E8D2A7F,
+0x0658CAE6, 0x5489F3A3, 0x9CD362FE, 0xBA5190B1,
+0x06A58820, 0x7A9AF759, 0xDC94E672, 0xEB284B85,
+0xF8EFA022, 0x3837C379, 0x7C9E9A2A, 0xD2ED96BC,
+0x5D1E4C7E, 0x97F2169F, 0xFC3C37C2, 0xE039EDF1,
+0xDBE93909, 0x81FEAC6B, 0xFCD383FC, 0x170B91FB,
+0x05BA3243, 0x8FB2ADE1, 0x52AFB984, 0xE8262E9A,
+0x1E704638, 0x89B8DFD8, 0x18C0C641, 0x2760C7E6,
+0xD3AFF3C9, 0xC4E3543B, 0x0C0B7910, 0x1DEF7792,
+0x483D7194, 0x9AAF5864, 0x08607947, 0x626F0CF3,
+0xC0F6A486, 0xEB4525CE, 0xA8BBA8F8, 0xE450DA14,
+0x2DC4D114, 0xBCA527C9, 0x6682AA4D, 0xCBB48A5F,
+0x1B474C99, 0x7F5B526C, 0xEC435C0C, 0x9E8D3E1A,
+0x67D2EA29, 0xA3B7ADCD, 0x8328590E, 0x7345607B,
+0xB6057588, 0x1A8B034C, 0x5C8CA534, 0x8115DC5F,
+0x189C2ABE, 0xF1B92927, 0x78A3B62F, 0x4B621D49,
+0xDC176A68, 0xCBD3C1DC, 0xD82348BB, 0xEEF05FA7,
+0xC0DD3D83, 0xC1F2A7BF, 0xB2079D00, 0x14B5730E,
+0x73203CD7, 0xA8672433, 0xA171FFED, 0x9F181200,
+0x4E16A5C8, 0x56D8AC31, 0x73803D86, 0xD4685CA4,
+0xE8DE9FE2, 0xA35D2CE8, 0x808CF3E2, 0x198700AE,
+0x0034163F, 0x57BC76FE, 0x271ACF93, 0xAA3AF6D0,
+0x37003A7E, 0x450B74F4, 0x157401CB, 0xB79DDDA8,
+0xD60AB7A4, 0x3A4C8779, 0xB6990FC8, 0xA1668D5A,
+0x05B7965F, 0x7814376D, 0xFA0D2D8A, 0xD97A1142,
+0xE804DE3D, 0x4939089E, 0x78D40CAC, 0x01DEF5EA,
+0x3DD1CADA, 0x96465956, 0x6358CFB6, 0xACE02DE5,
+0xB4C9F6CE, 0xE9C95AFF, 0x70EAD28E, 0x58803693,
+0x89EF9972, 0x58F0273F, 0xDB17A277, 0x0B082B98,
+0xAAB13ABD, 0xE86381EC, 0xC18924D4, 0xE28D4348,
+0xC21895AB, 0xE17073AD, 0x9417539B, 0xA043E5F5,
+0x88FFD026, 0xD972F017, 0xD0C8B8D3, 0xB34F3D67,
+0xC525E4B5, 0x0189A5A1, 0x59224A35, 0xAA18F2D5,
+0xFC9E170C, 0x16D3795A, 0x35DB09FA, 0x1624DB1D,
+0x4A6E059F, 0xC5C88A93, 0x9051D373, 0x4B12B09C,
+0x4088AF39, 0x705394F6, 0x360F2BAC, 0x8A1F2420,
+0x641D4FA5, 0xA78B78F9, 0xA5A5302E, 0x691D2108,
+0x7CFB57FD, 0x1812FE68, 0x8A2BB5E0, 0xF181CA14,
+0x1846848E, 0xDC044F67, 0x17FCCA28, 0x21D7C5AC,
+0x4C43432F, 0xC457E26E, 0xB0C9ADD2, 0x791EE2B4,
+0x620F27BE, 0x229E0B1E, 0x746B4FFC, 0x02038738,
+0x1C7B971B, 0x05193430, 0x8645DBD7, 0x58678F98,
+0x141E912D, 0xD89C587E, 0x9FD7B43F, 0x21851D56,
+0x725311A7, 0x0605B1B2, 0xC18BF2B7, 0xC6F79EA9,
+0xBD84A01B, 0xC9B7F2DA, 0x04E47EE8, 0x1C1A14F5,
+0xBD5B4FF1, 0xE15FBC2E, 0xC4D43F01, 0x5D39AD4A,
+0xBD3BD983, 0xB2314A4B, 0x8DABA67E, 0xB5263B5A,
+0x9912F262, 0x82659C80, 0xC3610181, 0x3F229014,
+0x2685532F, 0xCE4EC210, 0xF46AB09A, 0xFAFA69C8,
+0xD1292944, 0x2EF880D9, 0xD03AAEAB, 0x0E83C435,
+0x842C482C, 0xA70951A1, 0x0E4EA07D, 0xE0332D0C,
+0x3EA27E55, 0x04721425, 0x7C8B56DC, 0x96391312,
+0xF600D78C, 0xC850517C, 0xB3F9F2AE, 0x59A99351,
+0x8D6AA838, 0xF586672E, 0xD81FE525, 0x3CEF31DF,
+0xABDC7079, 0x6E1BB8F6, 0x6B45B87B, 0x9FD2CAC4,
+0x648E357A, 0x6C57D30B, 0x23766B64, 0x8C8BD9C1,
+0x9A29001A, 0x206F47E3, 0x5F423D75, 0x293A32C4,
+0xDCC6432B, 0xA4280954, 0x457790B8, 0x11E84CEF,
+0xAB11D0BF, 0xD04258E3, 0xFB44C0CE, 0xED8231B2,
+0x0277A6B2, 0xD8E5C517, 0xCEDF4C8B, 0x19D90170,
+0x20555532, 0xFCB610B9, 0x88D5F5A9, 0xD35DC77E,
+0xEF5EA686, 0xD866959C, 0xF0886B56, 0x005CFB90,
+0x582AD255, 0x7381289F, 0xC18CED4D, 0x444F0A6B,
+0x9917AE56, 0x505A7BCD, 0xCBDC903B, 0x51EF0F3C,
+0xC4E6AF5A, 0xB148AD2F, 0x609A124A, 0xB5DA89E4,
+0x3A68C7D4, 0x98694F02, 0xE85B1766, 0x754BA5FF,
+0x1296A58E, 0x27736843, 0x9B6280BD, 0x2686032D,
+0xB428AC04, 0xB06DBA5C, 0x625FE034, 0xD4BCB25E,
+0xC91C5B3C, 0x73BB70E5, 0xA26A479A, 0x73173229,
+0x3AA1235C, 0xE16171D1, 0x42D0D42F, 0xFC624752,
+0xF1F5DCC2, 0x1B6F20A9, 0xFF9D626D, 0xDBF052C0,
+0x90E38D23, 0xFB72CC5E, 0x9186519C, 0xF2330093,
+0xE5251385, 0xA0094977, 0xE83FA066, 0x2E389CE2,
+0xD3A62E72, 0xA9422A8B, 0xC61CFD5B, 0x1B3A516A,
+0x58087800, 0x3A47462C, 0x557DDD8B, 0x94FD21D4,
+0xE1AEA942, 0x4B2CC532, 0xB2185B36, 0xDCA15259,
+0x1D044D7D, 0x781317B8, 0x49CB13E7, 0xDAAFFBC6,
+0x30A05644, 0x77B05F37, 0x065A567C, 0x94721C79,
+0x47316C60, 0x58AAC7C9, 0x410081AB, 0x7D4A36FA,
+0xCDF23455, 0x1873EF87, 0x186982B5, 0x7C78D9DA,
+0x3567D966, 0x10FF5E8E, 0xDB88E5B3, 0xFF1D39A1,
+0xB8A345A3, 0x7A7258F3, 0x9706B3CE, 0xB5ADCC26,
+0x4561EF5B, 0xB002FBF6, 0xF3F4C6FA, 0x57EC75AD,
+0xBCF37924, 0xBC05B0AD, 0x2AB19DAA, 0x0EBD25EA,
+0xF335D08C, 0xDFF79E19, 0xDD86D418, 0xECE11951,
+0xC06F4D50, 0xFD698DF8, 0xBA6192EF, 0x365A28CE,
+0x74DEC0B7, 0xE971F67B, 0xBF89DD42, 0x1E683399,
+0x164A7158, 0xA1E48475, 0xBE139E8E, 0xBDEBA7FE,
+0x74E03AEC, 0x88EA9618, 0x9B0048C2, 0x68C1DD20,
+0x8DC9FC85, 0x24B55E3B, 0x51C38BDA, 0x2ECD7B13,
+0x54D66C89, 0x69A3EBC1, 0x4B4E4F13, 0xAD37B7DF,
+0x030A1D8B, 0x85A114D9, 0x403BE495, 0xB5E40331,
+0x316E7310, 0xB36AA494, 0xDBFFCB9A, 0x5C0E5DA5,
+0x099BA9E8, 0x66826E9D, 0x0BC5849B, 0x1A20CBAB,
+0x0744FBE6, 0x2CB52040, 0x8B88533F, 0xA8A44BF1,
+0x62FEB4A8, 0xDB2ABC4D, 0x46F0B676, 0xCBD06470,
+0xDB6D71EF, 0x5DC3551A, 0x71B31A5B, 0x046D4C7F,
+0xC051A998, 0x1EC19FF9, 0xA9E21F9F, 0x7951E081,
+0x78BCBA62, 0x91B623F2, 0x8EF6A81D, 0x1023755E,
+0xCE47F5AA, 0x0EF27527, 0xE9E488D5, 0xD53E4A29,
+0x78A276E1, 0xB2100585, 0x01208E3C, 0xA38BCAFF,
+0x36221FB7, 0xB3C9194E, 0x51BD75D4, 0x9C8C73AC,
+0x7ACA9964, 0x17890C94, 0x9FDA51F4, 0xC4FDF688,
+0x2C8244B2, 0x0D834C74, 0x290973D3, 0x7F134553,
+0x296D2FC2, 0x4E08ED27, 0x1C51E53D, 0x3D892F49,
+0x945F76CC, 0x2E531E63, 0x71EE37E0, 0x9C47F346,
+0x2D8D920C, 0xC3E465BA, 0x3A72D142, 0x5B6AB80D,
+0x364C2AE7, 0x3B18389B, 0xB9442484, 0x5D687BB5,
+0x97C65A4F, 0xC7DBE8BE, 0x0F840061, 0x5A73EA89,
+0xCBBDD954, 0xAFE9CABD, 0x06ABDF95, 0xF139302D,
+0x3804FEA8, 0x7CE6542F, 0xDE47B8ED, 0xD34BE509,
+0x5EB9C9E1, 0xDC582534, 0xE77D7FC8, 0x2BEFED7E,
+0x4EA26DFD, 0x54670B81, 0x665C4531, 0x5B7A7023,
+0xA05D9A2E, 0x71BDDB2E, 0x9D51D8C2, 0xD8A665CC,
+0xA9B87A22, 0x581D28BF, 0xF9D40373, 0xE04D8F63,
+0x117B9842, 0x8868B9BE, 0x8397FAB9, 0xEF5CED75,
+0xF70F90D8, 0xD3DFD3A6, 0x1779F576, 0x3059520D,
+0xC38F4AA9, 0x6B7A6D0A, 0x4E73112A, 0x4FF9DCED,
+0xAEA1383A, 0xBAB0AA93, 0x41DBCBED, 0x266775A6,
+0x8EE0D5D5, 0xB522CB9E, 0xC6E5D0D3, 0x86E4C8FD,
+0xA894642F, 0xF69821A9, 0x88B41798, 0x4585A188,
+0x9D2130FC, 0xC5B18E0D, 0x6B92C9EE, 0x3C9289FB,
+0x1F02CBB6, 0x31FA86DE, 0x1B2295CD, 0x5B4DA19C,
+0x3134D8FC, 0xE5EABC44, 0xDF8C5095, 0xF6571881,
+0x1F2FBD62, 0xE585FE61, 0x020CEDF6, 0xD70ABC83,
+0x5F37746A, 0x6FDA3BF7, 0x5434E503, 0x44CF6915,
+0x561B2393, 0xEA4A2251, 0xA988C080, 0xE47B1791,
+0xD335CFBE, 0xEDA9DEE2, 0x4F70FB22, 0x83A2C29F,
+0xF44FA002, 0x069D25EC, 0x4D5043F5, 0x887464CA,
+0x661D1E9F, 0x98B856AD, 0x81A23FB0, 0x3693BD42,
+0xCE0AEB0B, 0x1F6E8322, 0xCBDF571B, 0x93688909,
+0xFA16A774, 0x25834437, 0xEE77FA98, 0x8DC68C60,
+0x155A8760, 0x22B8FCA3, 0x1B1BB054, 0xCA3AFFCA,
+0xC8EACEA4, 0xC86BADD9, 0x473770AB, 0x41D6E398,
+0x568B397D, 0x065C0BE5, 0x51D38A0D, 0x3BB3A0E1,
+0xBC386DCB, 0x7DCBA6B0, 0x19007254, 0x3F4FC726,
+0xF27DAE85, 0xF7FDA72A, 0x6D0B5C07, 0x64A0ED12,
+0xE26D8878, 0x210E4F6B, 0x65F92C0D, 0x4E4E2CA6,
+0x5E479D49, 0x7B287050, 0xE9A4836C, 0xC3A111A2,
+0x9B90D6FD, 0xA5F362E0, 0xADC9526B, 0x79B736E9,
+0x72A9A57B, 0x181B4E70, 0x5236F32A, 0x5567E3C9,
+0x23EFD063, 0x87113163, 0xCDF6D4F4, 0xF53A8722,
+0xB70CF941, 0x757F40C8, 0x6A652BE7, 0xD71DA5AA,
+0xF87D51C2, 0xB4A68E16, 0x763D8FEB, 0xB6DE5436,
+0x12184DCD, 0x38D1DE90, 0xB39E5209, 0x1600492A,
+0x073AE8F5, 0x0366AC0E, 0x1AD5014F, 0x398E0873,
+0xD653928E, 0x30B5B4DE, 0xAC68A06E, 0x8DAEF4D3,
+0x76A880D8, 0xF3B3BCC5, 0x2B631F58, 0x340914DB,
+0xB4771DCC, 0x7C9D4A43, 0xAFDB1138, 0x014B5A83,
+0x0D44185D, 0x20C89576, 0x994B4367, 0xA84BD792,
+0xB2E17CB1, 0x00CE5214, 0xFB93E54F, 0x03CCA7F1,
+0x956A82E6, 0x22329A71, 0x2A634374, 0xF18B7AD9,
+0x1F168BC4, 0xC2CB1EDC, 0x8E0AF6CD, 0x211AF22A,
+0xAB5DA374, 0x63F1F25E, 0xEC58D4CC, 0x48C65C46,
+0x5A7F7574, 0x7BA60047, 0x279EF299, 0xE0B77F48,
+0x647A03C3, 0xAE7C4D8F, 0xF65149D0, 0xAC9EF228,
+0xCD90B1CD, 0xCEEDA54C, 0xD8FD0A6A, 0x8D7C2291,
+0xB38EF6C1, 0x7F38E676, 0xDADD0A8F, 0x1125713C,
+0xAA78A299, 0x54033F20, 0x199C76C5, 0xCAF82A17,
+0x16F2EE8B, 0x20071D0F, 0x2CA000F8, 0x0178A24B,
+0x0029EE46, 0xA9D8C738, 0x123D2BBD, 0xEF7CAC52,
+0xBD241869, 0x435F8FF7, 0xB573A190, 0x402BFB2F,
+0xFDA3097C, 0xF3765889, 0x68E2C7D5, 0x4C26F858,
+0xD6814D1F, 0x6B043C7B, 0x173DB091, 0x95126C7C,
+0x0FE8E1BE, 0xFDEB233C, 0xB979B0CB, 0x00E00659,
+0x19952E52, 0xA0976F7E, 0x02FB462C, 0x798815C8,
+0xA2504EFE, 0x0F4811AD, 0xBA8F122E, 0x5EE5864F,
+0xD39B6799, 0x5319F6A3, 0xF6A66685, 0x988D106F,
+0x7ABA5220, 0x0320384B, 0x4DE48C79, 0xF5CB36E6,
+0x2B33270F, 0xFF4E6965, 0xD4D843D5, 0x7EEE861C,
+0xA96AE5EE, 0x310E5215, 0x6D20068E, 0xB149AE8B,
+0x0997D9EF, 0x5043FFFA, 0x0516E2B6, 0x3FCCDA32,
+0x8E604A04, 0x23012778, 0x9444A474, 0xB7F5DC24,
+0x3A58E6FB, 0x17B759FB, 0xF29C1EE7, 0x8893D2D1,
+0xC6CD235B, 0xAAB0CBCE, 0x2D84474C, 0x8A0BE027,
+0xFDB87FB5, 0xE6B507BD, 0x19B41927, 0x783FF4DA,
+0x485A1D5D, 0x8ED285C2, 0x25AFC4C5, 0xBF0D662B,
+0xC4238532, 0x4339FCCF, 0x14A784B6, 0x71665819,
+0xED76E473, 0x5F1BAE9E, 0xD0AEC17B, 0x4CE78814,
+0xD3609F61, 0xD4E49EB0, 0xE4E3EFDA, 0x9B7CAD1D,
+0xEF01ABB7, 0xD137BEE9, 0xEE87A81D, 0xD4B204FF,
+0x00B25737, 0x2770FBD1, 0x174AFF7F, 0x0A77A21C,
+0xF1B370E7, 0x9C093CB0, 0x080C1FFA, 0x83CE92D9,
+0x1707470C, 0x3303479F, 0x25F1B6AF, 0xF40EEB7F,
+0xB98A1677, 0xA54A1BA2, 0x43B4144A, 0x2F092A35,
+0x33286A77, 0xA0AB9C93, 0x4F8D70DC, 0x3A47BF6F,
+0xB6209AB5, 0xA4C94557, 0x5E757055, 0x706EAD9F,
+0x467BC02A, 0x6472A857, 0x42055C57, 0x66F2BA60,
+0x33C0536F, 0x3240BFBD, 0x3DD74E6B, 0x1F58A552,
+0x822E9577, 0xF49BFE77, 0x5490DC6D, 0x1D32BBA0,
+0x1C30B072, 0x78A4A5C0, 0x1EE88A57, 0x97CAC3C8,
+0x9912861F, 0xC916BBAF, 0xFC3A7F0E, 0xCA5E1F3A,
+0x630F09CD, 0xF6C8C210, 0xF0A12A72, 0xF3148619,
+0xDF1672E1, 0xFCE5C390, 0x29CAE554, 0xE984A45C,
+0x8A1F0A3A, 0x6A02C707, 0x8CFB3ED6, 0xC0A741BD,
+0x7A871FE5, 0x91021A69, 0x505FB05A, 0x8F85227B,
+0xC300ACF1, 0x0A1B201B, 0x224614B2, 0x54A23576,
+0x5360A5BA, 0xDCD23A31, 0xF98DF638, 0x79FF79D7,
+0xEAC8EAC3, 0x4D22C65D, 0xDFFBF1D9, 0x55FD8848,
+0x4BFD2347, 0xE2A08287, 0xE6A48824, 0x80625EA9,
+0x71AB3F7E, 0x99B84DE5, 0x6512ADBE, 0xFBF24C47,
+0x3EEF2564, 0x23DF9F1B, 0x24BE5199, 0xDEDD72D5,
+0xA2FE063B, 0x4FE520B1, 0x9E4E7BBE, 0xD615BDBE,
+0xC14E8184, 0x40F86FB1, 0xD403A65A, 0xC5AF6386,
+0x412F8434, 0x6D6012B0, 0x4EC57107, 0x3F76AF19,
+0x54A305BD, 0xEA9C4EB2, 0x584E0176, 0x20759805,
+0x1A16C84A, 0x50BB10DB, 0xE610AF45, 0x98CF1EA0,
+0x3F8C7756, 0xF9056BE0, 0xBAA66B7D, 0xF7076DCF,
+0x67F1994D, 0x92BFEB62, 0x86FBDE17, 0x389DB311,
+0x2A171F5A, 0xE14898B1, 0x4D11723F, 0x29889062,
+0xCBF3DD79, 0x2B7468FC, 0x4FB93770, 0xC5FCEFE8,
+0x8FEE6678, 0x9F4ABA9C, 0x6A6B23E1, 0xFEA7077F,
+0xC835F734, 0xCA67807C, 0x1BFBEB49, 0xB8B1E842,
+0x6A850623, 0x001C1E8D, 0x782AC01E, 0xA28A72D8,
+0x6CD66FC1, 0x77EF6F13, 0xFF40D7CF, 0x4A163DFB,
+0xDB21AA89, 0x29D03A9E, 0x3A4D1D57, 0x7A89CDC9,
+0xC5623E10, 0x8A444799, 0x1F620DF4, 0xFF876758,
+0xC9DEEF2E, 0x7F86911E, 0xE3196093, 0xA00EB422,
+0xCDB1743F, 0x4AAD1988, 0x70167700, 0x70595C5F,
+0x8E648013, 0x401D8770, 0xC762F0E7, 0xDB776926,
+0x2BDC55B3, 0x8F4AD2C1, 0x1A2EEB50, 0xBD4BF2A4,
+0xA43FFE90, 0x752935E7, 0xB02C7801, 0xDD4CD3DB,
+0x3815C394, 0xAF427695, 0x7455A8F9, 0xC444C7EC,
+0x9BC9B2C5, 0x08423BA7, 0x5D91ADD8, 0x59D866DB,
+0x0AD32258, 0x7BC397F6, 0x0EF7DB59, 0xC1034320,
+0x79073406, 0x991A12B9, 0x9D6776A0, 0x6348A5EB,
+0xBD98CDC4, 0x81A6C5C5, 0x76A3ABA6, 0xFA9CDF77,
+0x97772B59, 0xD987E42B, 0xA4B893D4, 0x61F78E38,
+0x82567691, 0xCB91CD58, 0xEEFA69AE, 0xF7D51178,
+0xA436C578, 0x99E86E08, 0xA8C3B16B, 0xD609054F,
+0x1E0ADCE8, 0x5DF6EF20, 0xEB3CC45B, 0x9FAEA24F,
+0x97F57F19, 0x66E2713F, 0x42A423C3, 0x2A21B17C,
+0x6A4C6B40, 0xFA0F4F2B, 0xD1F3F64A, 0xD0AAFA50,
+0x767D3AC2, 0x837E626D, 0x3B21279C, 0xCAE18855,
+0xFA8CA385, 0xA91BDE45, 0x1A953327, 0x733948CC,
+0x158B8CD2, 0x904AC43D, 0xA6BC8F82, 0x55F027DA,
+0x95B6BB32, 0x9265FF80, 0x8EEF0D24, 0x28F6796E,
+0x1D736700, 0xB621D4D6, 0xAB2F1A4A, 0xECD7DB83,
+0x35CAD419, 0x60604917, 0x5DE51335, 0xA3D7E122,
+0x685D04D4, 0x494739D4, 0x0060722C, 0x59149718,
+0x03C9F144, 0x43328818, 0xBB1AE189, 0xCA7B9250,
+0xC835666D, 0x83950220, 0xD774405F, 0xF6F4FCCE,
+0x0E38794D, 0xAF184A7E, 0xEF66E15B, 0xA0C2A74F,
+0x876112D5, 0x7D68C9CF, 0x8902011C, 0x6AB0E128,
+0x2A515520, 0xA99D1DA0, 0x9EACEB4D, 0xB669AA8F,
+0x6F96DCE2, 0xCFEB5CDF, 0x46EB36BD, 0xEDDF8317,
+0x4FA30C3E, 0x9541A8A1, 0xA5F75533, 0xEFE1FEF6,
+0x7F21B481, 0xDA11D5EA, 0x64642069, 0x083D2137,
+0xDF508726, 0x8F6CCC4B, 0xC5412D0A, 0x6A9F6BEA,
+0x3E3CC54F, 0x078BBB1E, 0xA6047468, 0xF1FA39C2,
+0x26143435, 0x90132EB3, 0x4216580C, 0xF6773B8C,
+0xA6B188BF, 0xE3B49523, 0x89E4563F, 0xD0B16538,
+0x2D9079FD, 0x69ABDE36, 0x669AC5EB, 0xD0618DD9,
+0x5080BFEF, 0xADC056D6, 0x72402C9C, 0x0AE79E07,
+0x8D6DF48E, 0x0502837E, 0x79BA17AD, 0xE4871C89,
+0xC4554CD5, 0x23FCB2A4, 0x646FA999, 0x212A9DB8,
+0xBD23DF0A, 0x890B5FE6, 0xB5D03292, 0x9FA3FD59,
+0xD612F8B1, 0x611365FB, 0x7E7C9FAB, 0x024194D2,
+0x46C2C617, 0xAEB0FAD9, 0xAE5D3A7E, 0xEA8B0ABB,
+0x760730A4, 0x50443E76, 0xECA64341, 0x538E5256,
+0x8A8505F5, 0xE0E4DC29, 0x105DC564, 0xC73D93D9,
+0xE3F27C90, 0x8CC01FC8, 0x400D0F76, 0xDCD01130,
+0x1E3416D4, 0x4C612E03, 0x0BFE7A5C, 0xFDB15334,
+0x5326A77F, 0x99549BDA, 0xDDE90BAB, 0x920BD872,
+0xC4B4F5DF, 0x7B39BAC2, 0x777C6694, 0xB4971103,
+0x9E7806A1, 0xD3141F2D, 0x2B40BAD0, 0x74AF248F,
+0xD1AEED43, 0x2F453736, 0x1880104E, 0xF9CD502F,
+0x7691FE59, 0x39C3FEC7, 0x72EA7BF2, 0x0C94BAB5,
+0x35D6F509, 0xAE86AC96, 0x0624C181, 0xA69DF699,
+0x5991FCE3, 0xAB20D4F1, 0xF30F1BC9, 0xB094CF62,
+0xA3B5A732, 0x3BC8C32F, 0xE7710370, 0x429A8D96,
+0xD8913A42, 0xCFBD0E4F, 0x710B7078, 0xC6501E93,
+0x241224AF, 0x978D2320, 0x8EF1064B, 0x273FAE07,
+0x316EC02C, 0xB3C16C0B, 0x8249C245, 0x21AD11CB,
+0x6265FE57, 0xA9F1D5FC, 0x0B52F1CD, 0x0381D983,
+0x2931D6B1, 0xD126CD94, 0x69D95197, 0x7CFB6AD0,
+0x46E6D50D, 0xE60BCBD2, 0x72FBB436, 0xC971A4CA,
+0xA580B9B9, 0xBC823514, 0x5D15A840, 0x87A91622,
+0x63490D13, 0x277189A8, 0x22CA2EDC, 0x1C56456D,
+0x1B5EB836, 0xD8BBF2EB, 0x20F56DFB, 0x99321E4B,
+0x9238B783, 0xE5E5D085, 0xC81DAA11, 0xEF8DD032,
+0xCEC28645, 0xFC40AAA5, 0xBFA5FC68, 0x1C2CF7C7,
+0xC0DFD194, 0x5AB730DA, 0xE3FB56A9, 0xA0AD00E9,
+0xB7BA2E2E, 0x579C8722, 0x04AA07FD, 0xF55C6C5C,
+0xE56CD6DD, 0xA7DA5100, 0x2A6BA1E5, 0x9B7E5104,
+0x81410420, 0xDC6130A8, 0x3EC8935B, 0xCC2EC782,
+0x142344EF, 0xF016E0CA, 0xA3ACFA8E, 0x019A7009,
+0xA0DAEC5D, 0xFA503565, 0xC907794E, 0x77AA4E69,
+0xB45B7E54, 0x929A056A, 0x46AA4AE1, 0x55E56EDF,
+0xFDD9D726, 0x35744D5C, 0xD6854700, 0x9A6E1EEE,
+0x0B00F6FB, 0x6BE65BFB, 0x9CF98DE0, 0xD80ACE66,
+0x1E5300E4, 0x745338DD, 0x4CB925DE, 0xB369B0D4,
+0x7A53A606, 0xD2B96E54, 0x88F96B30, 0xB72C3E19,
+0xC2A41177, 0x6206F879, 0xC1F6CD78, 0x879DA74F,
+0x763F9417, 0xD109B779, 0x6A58B34C, 0xDCD7C21A,
+0x1B0A0154, 0x45EE3A9C, 0x62C60161, 0x79E47020,
+0x42250A39, 0x9E2C2C59, 0xCE4F6206, 0xC2970386,
+0x983CC2C3, 0x0DAF0A85, 0x388626DA, 0x06A56D27,
+0x9223203A, 0x96E0148C, 0x22F0D052, 0xD5F1AA88,
+0x394BC8B9, 0x03CF58FA, 0xC0B1073C, 0xC16B35C7,
+0x7B7CF9F8, 0x2E3A24A5, 0xA19089C9, 0x4223FAE9,
+0x7751D977, 0x802E7062, 0x6D3651EF, 0x39E9B52E,
+0x946D07F8, 0x8E2EAEB7, 0xF9279A65, 0x14DEE911,
+0x8B92A149, 0x9611756E, 0x067DD22D, 0x59907967,
+0xB3417E3C, 0x3B72AB7A, 0x825D87C7, 0xCE5FA852,
+0x5D88C5F8, 0xE792BF66, 0x28DB3A4A, 0x118CA3A2,
+0xCC86284E, 0xA0AC4AE8, 0x33394B70, 0x974F96C2,
+0x86ADD3B5, 0xC87295B9, 0x1447D26F, 0xC9ECAE80,
+0x10CA01D7, 0xE04ECC68, 0xAE56597E, 0xAAA1248C,
+0x81C35460, 0x0087CA93, 0x943AABA2, 0x0AFCBFAA,
+0xEA77D5AB, 0x020D36D6, 0xF1CCBBB6, 0x8DF1426F,
+0xAE726D96, 0xA6E4C915, 0x58F15F91, 0x5B696D6F,
+0x00042B30, 0xC6AC90C3, 0xBD8E0187, 0xE73ED2E2,
+0xCEE64CF6, 0x48B56436, 0xA33994CA, 0xB3E3B7AB,
+0x060D5E14, 0xC1B176C6, 0x4A76C391, 0xD7C8DB1D,
+0x333E4998, 0xC20BAC4F, 0x523BE3E0, 0x237E87BC,
+0xE6CDBEC0, 0xC506F19C, 0x262C0039, 0x7F85A4AC,
+0x46160693, 0x2EA1BC36, 0x4CAC0DF2, 0x0066B83F,
+0xBCBC778D, 0x7F4AB507, 0x99CADB2F, 0xC95520D0,
+0xC5CBF067, 0x903ECD68, 0xF5D7B0FC, 0x08198C8F,
+0xA17879EC, 0x18C2723D, 0x5A4D6D37, 0x080198B6,
+0x3525186C, 0xEF8BE144, 0x44B05851, 0x28B5025A,
+0x0FDF085D, 0xDEB1F249, 0xA7C00F42, 0x7614A735,
+0x3BEBF467, 0x7871D305, 0xD4F63809, 0x9D044079,
+0xE585D3D6, 0xA89952F3, 0xF42C2B8E, 0x04179DA4,
+0x00A6CE87, 0x96CA92B8, 0x9DF2B156, 0x3ECF18BC,
+0xDE2509CF, 0x5CD85FCA, 0xF8A7CEEF, 0xCB7DC25E,
+0xF2847474, 0x35B501D1, 0x137BBB3E, 0x451E1BB9,
+0xD360D811, 0x792B3464, 0x4BF89A81, 0xA7E9C450,
+0x628BCB0C, 0x2AF7037D, 0xA45F628E, 0xF0EC875D,
+0x9CE3677D, 0x2CD0EA59, 0xA50A0217, 0x8BA45DD7,
+0x1735ACF1, 0x5804C4D9, 0xE619B352, 0x948F44A8,
+0xA9BF5C7F, 0x614D4F6C, 0x6D9FCA79, 0x29717B0C,
+0x50BF2D5C, 0xD5847B52, 0x0D4FAAA5, 0x1AABCA5D,
+0x779399E0, 0x58A90CD6, 0x37EC2615, 0x61B68C07,
+0xC49F4AEE, 0xFAC4D897, 0x9C68CC6D, 0xBB3352F6,
+0xF933436D, 0xD310078E, 0x2FBFA17A, 0x3D839C4C,
+0x186E69EF, 0xCBE7CC6A, 0x7434231A, 0x80F8130B,
+0x58CD7EA2, 0x2E46D714, 0x367286E2, 0xA6E2044D,
+0xC2ABC50A, 0x6FEDC9C4, 0xE2F26F03, 0x3B030D52,
+0x3674D8E7, 0x9096DF78, 0x90902892, 0x44A32190,
+0xD08D2649, 0xEFE0ED0A, 0xCE1BF4E9, 0x62C19753,
+0xFBF3D1A8, 0xD4AA5390, 0x4B32E77F, 0x9894F05E,
+0x41B9DBBE, 0xE9B09561, 0x46C883A0, 0xADD5D60F,
+0x69CE5BBE, 0xFD29CCF1, 0x2F209371, 0x4C6716E9,
+0x31E9A09F, 0x04089795, 0xB9EF9025, 0x97C6267D,
+0x63823150, 0x3AB346BA, 0xED3E0579, 0x85FC7062,
+0x37B35761, 0x4A32B6CD, 0xC38EB479, 0x203642CC,
+0x568FCAD7, 0x67D92B5D, 0xE51B8C3E, 0x02104078,
+0x026BC607, 0x5A06CDA7, 0xE27435D0, 0xC7C20CE7,
+0xFEA74022, 0x77310076, 0x35C6F953, 0xE1B199C5,
+0x262F139B, 0xFD2FE2C7, 0x3EEE02EB, 0x915A873F,
+0x2DE4AB8E, 0x2421DC15, 0xD1DD0D9E, 0xDE02B5AD,
+0x151C76CF, 0x798B90B7, 0x82EDDF4C, 0x795E18CF,
+0xF09CEC5A, 0x070ADF8F, 0xCDCF5232, 0xD498D43C,
+0xB4FC2662, 0x25678E54, 0x5D200482, 0xC31F21C9,
+0x35E5AF29, 0x8CC0E603, 0x995351AD, 0xD8EB54F6,
+0x564E35D9, 0x0C13E321, 0x34CFA33D, 0x33D1E5F9,
+0x2EAC9748, 0xFFB950D6, 0x2032206F, 0x4F871AE3,
+0xBD464C61, 0x06356EA0, 0xA15A290D, 0xA78456D0,
+0xD2F4EE88, 0x4D835908, 0x15DC87B3, 0x79EDB6C3,
+0xAEAF0F9E, 0x5C3E7EF9, 0x639A099E, 0xD375D8DA,
+0xB718510B, 0x090DF965, 0x9C8A362E, 0x25AD10BB,
+0xF9A42BE9, 0x8ADE3DF0, 0x5527424E, 0x301F0D0F,
+0x2F691C9A, 0x534FE1FC, 0x7D406016, 0xF98820A2,
+0x4D204871, 0xED145173, 0xD67ECE9A, 0x35F9F990,
+0x8ED4D787, 0x1F3F46E1, 0x5A68F171, 0x9A9D28B0,
+0xE726BD5C, 0x8119228D, 0x0ADBA4D2, 0xEA243204,
+0xE523C0D6, 0x261E3664, 0xB2D1211C, 0xB4D9293A,
+0x9C89D924, 0x15A6A3A9, 0x0D8C6C66, 0xEC04AD36,
+0x0CDF0F98, 0x9262C7DF, 0x8EE0E09B, 0x6B929EE9,
+0xDCC713BC, 0x75FD34FF, 0x2784E694, 0x23C23044,
+0xB7B04F09, 0xF10B753E, 0x2EC774DA, 0x470BE72E,
+0x054510E9, 0x9C7DDF10, 0x1466C277, 0x9F52F493,
+0x7F298608, 0xF1BA10D3, 0x8847A319, 0xEE8A63CA,
+0x8E64B34E, 0xEBB66933, 0x575ADB24, 0x041BFD76,
+0x727ED364, 0x00F4A008, 0x8F5EDA92, 0x21477637,
+0x0B360617, 0x56DC8978, 0x27F88944, 0x69B799EF,
+0xEA1E943B, 0x6FDD60B0, 0xCE2AD89F, 0xB98CCF43,
+0x2A3796BF, 0x4DD02535, 0xC6B524EA, 0x6B173341,
+0xDCE0A457, 0x91770646, 0x57A8D138, 0xFC218331,
+0xDC6B712D, 0x14C0B3B9, 0x30CA09AD, 0x759EB942,
+0xBC9634AB, 0x8F92A7E5, 0xF7F85B53, 0x6C831B3B,
+0x56A75B18, 0x43DB9F1C, 0xF81FC212, 0xB8EB9026,
+0x78A74B51, 0x870655E3, 0xA17B536D, 0xBDE866CF,
+0xFC609F11, 0xF34A7016, 0x7C4FD4DD, 0x236312F6,
+0xB50520A8, 0x4BEEA2C3, 0x2B690BA3, 0x18701667,
+0xBD791FA9, 0x236D36CF, 0x49E576CC, 0x316A77E1,
+0x93E9B0BF, 0x52715603, 0x83B9AAF2, 0x0F8F2A80,
+0xA87F764A, 0xD2079BEB, 0x48A24AB6, 0xAC370950,
+0x3077FB2F, 0x4BAFF3F5, 0x1A79926D, 0x8B369956,
+0xAD78F739, 0xED88CE42, 0xB96A7C15, 0xA7BBA2EE,
+0x47CC3233, 0x804DE962, 0xE0B431A3, 0x4A8257B8,
+0xA4B0E8E2, 0x2FFC49B8, 0xF0CDF5E5, 0xF089C32A,
+0x46328288, 0xEACBC054, 0xA48CB5CC, 0x77996530,
+0x83A4E184, 0x3C2F47D9, 0x5106177C, 0x33F1A787,
+0xA2266E7A, 0xEBC426C8, 0xD7E8ADD3, 0x2DF40477,
+0xF9E8D7BD, 0x80BD8EAB, 0xE61CE55F, 0xF6A7EF6F,
+0x5C67E1C0, 0xFBD0088A, 0x7ED37B24, 0xF5BFD58E,
+0xC29CFB0F, 0x61ECE08B, 0xA776CFD8, 0x9E0F3A05,
+0x8FC8B02F, 0xFDF82702, 0x028C2F2C, 0x169D3094,
+0xE4AA3228, 0xF2CD142D, 0x9C70574E, 0x057BFE78,
+0x782B9039, 0x0D01311F, 0x97552050, 0x6A097F2F,
+0x1B3242B8, 0xF43F32FB, 0x96004287, 0xC3DC0939,
+0x4215A0E1, 0xACD1A28A, 0x189932EC, 0x9BBA0475,
+0xFA154E5B, 0x4B4E8D01, 0x4D6B18B1, 0x31545B3C,
+0xC849C52D, 0x60958B9B, 0xE92CF090, 0xAC3E1B58,
+0x251D02A3, 0xFAEE4F8B, 0xB1CF6CCC, 0xC2A0D8B0,
+0x0501DF46, 0xD0369D94, 0xF3E11479, 0x397599F8,
+0xB90064D2, 0x341F6D57, 0x31F0141A, 0x2F899029,
+0xBC9EF6E8, 0x13B47347, 0xB93D59BB, 0x556E990F,
+0x5727BDFC, 0xBA9F5121, 0xD67BE7CA, 0xB167E84D,
+0x2C0ED0FF, 0x251FFD4A, 0xC98719F2, 0xD379D976,
+0x8B3A0A9B, 0x40BA5F66, 0xE40A93E8, 0x2F89FC04,
+0xFCBAFDD4, 0xF2424270, 0x1BDBDD15, 0x7F1459B0,
+0x5ACB6C6A, 0xFA20719F, 0x2F16FFB4, 0x820DDE50,
+0x468AAC15, 0x7816134C, 0x978D9570, 0x6745CD6D,
+0xC1E768C1, 0x15E243B5, 0xBA30AD61, 0x483FB6FE,
+0xCAA17D0F, 0x2F8F0974, 0x34AB68B4, 0xB3E864B0,
+0xC1DA3828, 0x5DAD43B0, 0x72D13B81, 0x01F274AB,
+0x9C0651AD, 0x0FC30C10, 0x0E7AA3CB, 0xDBE6B9D9,
+0xF423B9A7, 0x457B4E32, 0x40E8E269, 0x91DA042A,
+0x9DBF41E9, 0x308C0F2E, 0xCABFAC0D, 0x0E2C86B2,
+0x117BC3C6, 0xEEA538F8, 0xF31585DF, 0x0DF50281,
+0xEAA9601E, 0x8F408AFA, 0xF1144F9A, 0xA2AB2ECD,
+0xACB88685, 0x6F4EFFBD, 0x81EEF886, 0x46B02240,
+0x3C09D916, 0x4F0DAF68, 0x8337B3E3, 0x9A011BA6,
+0x4C63AC66, 0x2FCC669E, 0x0C7D15BB, 0x51279D9F,
+0xC1354779, 0xEFF940AF, 0xA956CB37, 0x0DB797E2,
+0xE665EE55, 0x79AF879D, 0x21BBC902, 0x30B264BF,
+0x411CDC98, 0xE453389F, 0x47C2C197, 0x3E6015F8,
+0xF9E7AA2B, 0xA9302474, 0x04C6888F, 0x4D118BF9,
+0x0DB7AAC0, 0x52A38EDB, 0x4DAB22F2, 0x7DBB6EAB,
+0xD4D17851, 0xFD944314, 0x40C5838C, 0xBA6EB0EF,
+0x9AA287A5, 0xF6D236F0, 0x41D9E2BA, 0x6968D776,
+0x31B1D129, 0x42C3F963, 0x27CCAD30, 0xCD61BF4E,
+0x2C7DABAB, 0xA78A9CC3, 0x7F856B6F, 0xB6D444A5,
+0x90CBB312, 0x95611781, 0x4916D531, 0xC496C30E,
+0x706D0CB7, 0x35D0064B, 0xFE26C36A, 0x6211F14B,
+0x2C2340BA, 0x58633567, 0x06B6BA8E, 0xA7EC3D8D,
+0x1071B0CD, 0x388EEFA8, 0x60D8FB1C, 0x5F99D147,
+0x52CA6EBF, 0xFA73602E, 0x0376C15C, 0x3C91B57D,
+0x9386AF17, 0x14A35A1A, 0xBDB42A39, 0x0E83C257,
+0xD4C5C775, 0xA607FA46, 0x91B9AD40, 0x7623C5D6,
+0xE3D53E6A, 0xA3C663E7, 0x5AD39BCE, 0x03B58394,
+0x38862C7A, 0x01D50B9F, 0xEAAB38EC, 0xAB3DFB8B,
+0x06795385, 0xB17F485E, 0xE2F57914, 0xB79A3BAA,
+0x13DA7886, 0x7136C7EB, 0x5E748AF7, 0xD34F16FC,
+0x968F6701, 0x99C5D7BE, 0x530F7FAC, 0xCDF5D567,
+0xE31DE0D3, 0xCF93BC68, 0x34C578AA, 0xA201F761,
+0x5CB8DC00, 0xCA24DB98, 0xF8AD7E4F, 0x808EC476,
+0x603BA751, 0x489555C6, 0xF2A03FF0, 0xD2461E9A,
+0x102C33BE, 0x7673933C, 0xC11A2424, 0x6A23C8C6,
+0x69499812, 0x19AA8510, 0xC8CDA75F, 0x34B5216A,
+0xD87F7420, 0xC8CEDB53, 0x8DF11BA2, 0xB10911C6,
+0x3F1E5955, 0xF075F4EB, 0x17874FC5, 0x0D55685B,
+0x5EE521E5, 0x46C72924, 0xF8540210, 0x5D5E4C5C,
+0xE87A133C, 0x91633DC9, 0x36B54D5D, 0xA8B5D440,
+0x7DB7D6C4, 0x5FA82C17, 0xAD679039, 0x86B3B839,
+0xDF5121B7, 0xC08B768A, 0x338A512F, 0xCF9A4F9A,
+0x5DEFBB5B, 0x4C9301B2, 0x08023702, 0x5B1D7E28,
+0xEC800505, 0x3A869E80, 0x4C50C8AE, 0xB1AE9064,
+0xAFFA34EB, 0xF2F006B9, 0xD8A9A3D1, 0x2C6C2134,
+0x677EE648, 0xBB6B6D5C, 0xA285136C, 0x6C47BF4C,
+0xAF158DC1, 0x0EF75E2B, 0x5B9C74D5, 0x9B8D4BE3,
+0xE495BE19, 0x5940B228, 0x55E62656, 0x3247E060,
+0xBF7094CD, 0x1C1AB380, 0xECEA2275, 0xB6DD8251,
+0xCCA39DD2, 0xAB85D992, 0x278197D2, 0xFB6C9FD0,
+0xBD53B458, 0x89EFE0EC, 0x52A3DFFD, 0xA6B7FF7B,
+0xFB043649, 0x93C93F79, 0xAEB4CD6D, 0x71DB5C90,
+0x9E8DFE92, 0x0F1A5B91, 0x55C5CF5D, 0x1A1847AC,
+0x8D25CF6C, 0x914FD316, 0x39FCFE20, 0xD8F66A07,
+0x2CDD3DC6, 0xE415AC72, 0x3D1BD09B, 0xA8322C59,
+0xBD3A826A, 0x2A988A40, 0xEBD8B1DD, 0x9F53EEEF,
+0xDF571816, 0xD4FCCDAE, 0xB85A1E50, 0xBE1A571F,
+0x0ED07534, 0x4C1E471A, 0x8B4D36F6, 0x0E388FC6,
+0x9ED2BC4D, 0x3E2D7F72, 0x752ACA15, 0x8960B48E,
+0x5892B3D7, 0x70F6F3CD, 0x26C485EF, 0xC83839B9,
+0xFE6C224B, 0x3547203F, 0xF73ACA84, 0x065DCDBC,
+0x8986EBDC, 0xCD59EA14, 0xC0EF58A8, 0xC5587229,
+0x484FBCEF, 0x9B8BF24D, 0x351CF946, 0xE10AA973,
+0x17919640, 0x95FF7B1C, 0x82AB65E5, 0x070BCC98,
+0x0E7CDB8D, 0x38DB27DE, 0xCA543C2B, 0x0131EB41,
+0x8300996B, 0x88B63D66, 0x03ADAC1D, 0xB205A87B,
+0xD8BDC0C6, 0x443F6071, 0x2CE69D2A, 0x6E1E5A53,
+0x4EFF93AC, 0x70322657, 0x5CCDD146, 0x04C435B6,
+0x5BF3CD69, 0x51E09115, 0x2545DFB2, 0xA52EF448,
+0x8D387046, 0x7C4F1F25, 0x2EFFD8AA, 0xFD6422B0,
+0xB82E26A7, 0xCF01CC45, 0x88899EBE, 0xDB621966,
+0xBBA1822F, 0xB264AAEB, 0x1076EAA5, 0xC24B0CD5,
+0x54D554B0, 0x4ECA7C05, 0xC8C9B053, 0x70A86D97,
+0x4E3265CA, 0xEA24F810, 0x873B172D, 0x79A74D18,
+0xEC3F49D5, 0xD1799602, 0xA21A28B6, 0x3FB99AD1,
+0xC2DB35B3, 0x63EC2E51, 0x17E4489F, 0xE8E19164,
+0x79ADD819, 0x10D66157, 0x5F621A73, 0x1CD063BA,
+0x6665815F, 0xFA0B7081, 0x6E0FA473, 0x0CE3571E,
+0xB5EAEF46, 0xAA04CF54, 0x336680CA, 0xDABBFF11,
+0x2259E797, 0xB57B4470, 0x111EB4BF, 0xC171D42B,
+0x5889A7A4, 0x419CCB3E, 0xBEA1F366, 0x41FE414B,
+0xA65CB898, 0x6C28363A, 0x8F82FC84, 0xDBED5A9C,
+0x4DBF3526, 0xF2F34E66, 0x9D2C9B11, 0x0C0D4DFB,
+0x4DBF79D4, 0xA256E86D, 0x6407376C, 0x3F3E8AFF,
+0x474B3593, 0xE55965C8, 0xCB20D358, 0x0C671A9B,
+0x169F8342, 0xD2E1C9E7, 0xBDDBAAEB, 0x93DF0C75,
+0xF27707F7, 0x5108305B, 0x4FF2C060, 0xEB9C08DE,
+0xDF11020E, 0xD2271046, 0x6D1BFD27, 0xED020CDC,
+0x2C22659B, 0x692050D9, 0xD14BE291, 0x3EBF8E86,
+0x8344B625, 0x7840B91C, 0xB702BD5F, 0x4935D318,
+0x01A22013, 0xF2A20B08, 0x651A1C38, 0x004FE633,
+0xE51DCC06, 0xF5B86138, 0x9FBFF118, 0x6F7B3CD4,
+0x028938B4, 0x071E96AE, 0xDF33DC9E, 0x79001AC7,
+0x7B5D20FC, 0x3F137794, 0x81165B04, 0x973F8FD4,
+0x0AE4CBF5, 0x7C48180B, 0x4A96BC89, 0x58066E74,
+0x86669DC6, 0xDC55A218, 0x858C3130, 0x99AEAC91,
+0x26983FC4, 0xEE4D4F06, 0xD8D6D657, 0x18EF262B,
+0x374A620F, 0x85995F9C, 0xCC814AC1, 0x39F487E0,
+0xC628177B, 0x2FAE2C39, 0x642525A2, 0xC1474F2D,
+0xBC7CD49E, 0xE81E13F7, 0x83F42BDB, 0x8AB7D99A,
+0xA8040B11, 0xD8AA68EC, 0x983B3739, 0xEE42ECDB,
+0xC9513498, 0xCAA06A14, 0xE4784094, 0xE6BEBB9E,
+0x13BE8018, 0x59E3D5D4, 0x0CF1728F, 0x963413BE,
+0x319533B7, 0x14662ABE, 0x3363B45D, 0x59A99687,
+0xBBB0FDA4, 0xCDBB8B21, 0x0240F3B1, 0x226DAC3B,
+0x30E1C49E, 0x76E076D7, 0x4B91C598, 0xB3C46E2F,
+0x4A657CC7, 0x66C3875A, 0xCBC6FC54, 0xF832EBE8,
+0xDD1EAD3D, 0xFEFDAF85, 0x8DE51B88, 0xAEAFD5D3,
+0x3E4CEA82, 0x55F47934, 0x9F8314CA, 0xD0220BC0,
+0x5ACEF81F, 0x71FDD8E9, 0x13A14ED8, 0x6F1FC1E4,
+0x75046A04, 0xC6C4FDAF, 0x4FFFF724, 0xF44FEDD6,
+0x7E1C5CBC, 0x784C6B4C, 0x8D85F220, 0x38B65C3E,
+0x8C992050, 0x2DE34C13, 0x9F2A4547, 0x48E58F65,
+0xA280B689, 0x6F540D8A, 0x10B61B39, 0x1C8A2849,
+0xA7316358, 0xDBFB7862, 0x182C553D, 0x92F04389,
+0x1FE7BADD, 0x6A724CBA, 0x970BE020, 0x93760058,
+0x2DF9E0AD, 0xCFF1F8B1, 0x170D810A, 0x45F4E6A2,
+0x37A0E8FD, 0x86D11C6D, 0x4F3C6A3A, 0x4B144452,
+0xCE9B87A1, 0x7C08C30D, 0x9CB9B0AB, 0xD55F2CC5,
+0xFF95180F, 0xF35505BD, 0xED5BDB96, 0x85CA2E41,
+0x8708B264, 0xD6079734, 0xCA76AB3D, 0xFD6CDF4F,
+0x9AAB840B, 0x92D3A5F7, 0x93A92C38, 0x0419AA7A,
+0x1D50006E, 0x126F48FF, 0xACDA412C, 0x01139454,
+0x8E23C486, 0x01D44F51, 0x7A5F6F10, 0x377D4D5E,
+0xB784E72F, 0xA9AC925F, 0xB9C66C79, 0x057331E6,
+0xCFF040E4, 0x77E8A960, 0x35E31EEC, 0xEB807A44,
+0x8594FFFC, 0xD27629B7, 0x5DDF526E, 0xBCF2F484,
+0x88805013, 0x41047850, 0xB8574ECD, 0x3E15082F,
+0x309C16DC, 0x297B6904, 0x30C39ECB, 0xD20B61AF,
+0x51A578AF, 0x4E0D24A9, 0xC61FBE5F, 0x7A89F4C6,
+0x9432299D, 0xFE261B95, 0xDD1FC4CA, 0x044BFB92,
+0x41BE56CA, 0x0A2B6831, 0xE135D75D, 0xAB2D00A0,
+0xB4374080, 0xFAA6DBD0, 0xA704C4A9, 0xD81385A4,
+0x51533312, 0xED5EDAF7, 0xE4EDFAEB, 0x74B7DAFE,
+0x9D810AA7, 0x40B91827, 0x65219BCB, 0x75431C16,
+0x94D923D3, 0x00B7AA4E, 0xB8A88FDA, 0x927278D7,
+0x7A237697, 0x45B14097, 0x2E3A562F, 0x93003322,
+0x0B88A5FF, 0xD13D4ADD, 0x6D7B7579, 0x72D834C4,
+0x0BCAA361, 0xC02E00B8, 0x15023551, 0x481C5E93,
+0x02E81A16, 0x8A846A33, 0x1239A971, 0x994818B4,
+0xFC3DBB6D, 0x43C8D2F2, 0xE3AE548C, 0x408032F1,
+0x02B05636, 0xE361A60C, 0xFE2CA292, 0x061D2374,
+0xDB285556, 0x70627EA4, 0x7FC64AF0, 0xFE100B6D,
+0x71AEB3F2, 0xA565A412, 0xA698731F, 0x49DD9767,
+0xC3627EBC, 0x75FB2DBF, 0xFDC0E971, 0xF6ED12A6,
+0xA23DC00F, 0x897E917B, 0x7F2031E0, 0x17DCE568,
+0xDF69CAD3, 0xC6FB5B6D, 0x097268B0, 0xE1102444,
+0x86DF9383, 0xBD7B9CC2, 0xBAAF7DCF, 0x985B45D1,
+0x4218E95A, 0xB2455EF4, 0xDB015F9B, 0x54CCCE76,
+0x56EDF561, 0x6F66F95E, 0xF8B1EBD0, 0xF7A39AE0,
+0xF66D8346, 0xA4677007, 0x02C4B3EB, 0x829987B0,
+0x7C0E1919, 0x51F7060B, 0x4B30F1D6, 0x85A4E0CA,
+0xEC049FA0, 0x17CBF1E4, 0x7A1AAD95, 0xEBA4C513,
+0xE8462E78, 0x54CDDA0C, 0xEE7B8378, 0x9858C8C1,
+0xBA33587C, 0x4D6F1B14, 0x7A2C0525, 0x7E6EE4D2,
+0xACA18692, 0xDD186820, 0x41198B03, 0x8AC85AB7,
+0xBD86900B, 0x36E2C354, 0xE65F9115, 0xB10645DA,
+0x7971D230, 0xC83D3583, 0x8C60C81D, 0x94DB5741,
+0x4FCB8934, 0x9A520FE2, 0xCE49446D, 0x8864E641,
+0xF5EF25A5, 0xC1DEED0A, 0xC8057F37, 0xFB305C73,
+0x392E670D, 0xA4D00D2A, 0x356A46F0, 0x2F675567,
+0xB7997CF0, 0x88AF3A4E, 0x56C9D51E, 0xDD746ECD,
+0x40CFA453, 0x5EA740CD, 0xE4DD6BB1, 0xCCB31429,
+0xA2227F3F, 0x18A1EAF0, 0xC155417B, 0x41FE735F,
+0x16D40B00, 0xC9F72AFC, 0x86B1D62D, 0x6A99A82A,
+0x09D33248, 0xEC44639C, 0x9B0AB2B2, 0x6969164C,
+0xEF602BB1, 0x0208FC6F, 0xC1109578, 0x2997AB87,
+0xE5626B14, 0xCDAF48E1, 0x20781633, 0x2EBE0A41,
+0x7379261E, 0xF216F7A1, 0x714D8258, 0x936FE68F,
+0x160856F9, 0x2A4D1416, 0xB558E412, 0x7DB196DF,
+0xDC88CCB2, 0xF37AB612, 0x7423F214, 0xD3B06A43,
+0x25A8012D, 0xC1C69FFA, 0x936F2C18, 0x56D77C19,
+0x774BFC69, 0xF5E85E24, 0xD79158C9, 0xA67C3E15,
+0xB958819E, 0x69F81278, 0xF2B35107, 0xBF2F4085,
+0x1C997A06, 0x6C238C3B, 0xC756D56E, 0xD15C1149,
+0x351E6EC4, 0x2311303F, 0x0621602C, 0xB11B6DD1,
+0xBE8E50B5, 0x34A5F589, 0xE4D308AE, 0x4344B297,
+0xA33AE98D, 0x0A303CDB, 0x388EA17B, 0x0107B5A5,
+0x38B39042, 0xFE678995, 0xB426FE69, 0x221FCF06,
+0xC45926AB, 0x21A430F9, 0x6D192D2E, 0x4168C10B,
+0x5BA6B132, 0x0519ECA7, 0x21127582, 0xF6C447E0,
+0x0C72FC31, 0x0941B3F0, 0x76F23877, 0x86CF0677,
+0xE7785105, 0xA4637864, 0x94C82B45, 0xF60FD6A0,
+0x46941C27, 0x7A33A698, 0xE1DF8BFB, 0x5249970B,
+0xDFE65E1C, 0xF4A4FB22, 0x599639F4, 0xFE0E9722,
+0x7BB48F58, 0x533465E3, 0x9E884B35, 0x2620429C,
+0x2875FFC1, 0xF11EC0CA, 0x663AF5F0, 0xB2C59C38,
+0x03556ED9, 0x271E9E39, 0x8556E062, 0x08207682,
+0xE5797F00, 0x66A362B5, 0x7ED8394D, 0x2922C374,
+0x271657BE, 0xAC15071B, 0xE296691E, 0x0FE2C740,
+0x19120FB5, 0x9ABD888A, 0xA200762C, 0x7837F41C,
+0xC6F4EA19, 0xF286ABF4, 0xFCA8998F, 0x97B0E7D5,
+0x1339C79F, 0xFED05D43, 0xB3392E71, 0xFC2A01EB,
+0xB720CBED, 0x4FA71358, 0x04A57F62, 0x3D558B0A,
+0x1DEB4D40, 0xC9C823F1, 0x470F630A, 0x08F22975,
+0x2BD85107, 0x3288A628, 0xB0C89675, 0x32D957C1,
+0x80B78426, 0x98A46953, 0xA493AF60, 0xC2B84AC4,
+0x486D658F, 0xFE119FF9, 0xB2FE565F, 0xEADB58CD,
+0x1F45F9B4, 0xCEAE62B6, 0x68EC702D, 0xF52ADDF7,
+0x0FFC0715, 0x4129E42C, 0x956AC4D9, 0x0035CD9C,
+0xF8FEBAA1, 0x29C58397, 0x7C2E2E41, 0x7BE74DAF,
+0x2791D34D, 0xB6D67B0D, 0x8F557528, 0x9DDEED5B,
+0xB3AA4BB7, 0x05E22E43, 0x4CDA600D, 0x432E2D32,
+0x405DA5BD, 0xAF23818C, 0x2F73FE09, 0xD4624626,
+0x653EFCB3, 0x77D65D3F, 0x51A3DCB3, 0x767F407C,
+0xC66452E3, 0x10B6842E, 0x93A0840E, 0xE453AD10,
+0xDE58FC3D, 0x6C227215, 0x1EE130EA, 0xB0BF64BE,
+0xA11E5D38, 0x0131B755, 0x191F70D0, 0xDB483959,
+0xAA8D2F9E, 0x5A002AA0, 0xF5A2996D, 0xFD0F95F9,
+0xD6A12864, 0x3AA48B74, 0x50F6679F, 0x0ADF5C49,
+0xE2F8CE68, 0xBF213E67, 0x5E9ACEEA, 0xCACD0EBE,
+0x6DF766A5, 0x33C0A156, 0x720868EA, 0x3112A0DC,
+0xB382350A, 0x369D9C50, 0xE8F890D0, 0x0A121399,
+0x2AB458EA, 0x51C8233D, 0xBF46403C, 0x0728CD55,
+0x23F6774B, 0x2FB59DB0, 0xFA2CF724, 0xB49FA848,
+0x5FFFA125, 0xDE2C0D15, 0x76B78C41, 0x192BA62C,
+0x4C9563E2, 0x8F742507, 0x882104E0, 0x357AD078,
+0x799E25A2, 0xEF3ED021, 0x69D54B46, 0x5EC57870,
+0x0FF418E0, 0x07C5AC7F, 0xC1ACBF9A, 0x80A830D9,
+0x837C7C5A, 0x04C11D86, 0xC14C8BC7, 0x92BA650B,
+0x94D34FA8, 0xDBDD5EDC, 0x9ED2A08F, 0xA1FAE485,
+0x5FD66C3D, 0x4CCB6F9F, 0xB7AA56B0, 0x0FB3C73A,
+0x03AF96E6, 0xDB2D38F9, 0x7AF20D60, 0xB57CBE90,
+0x20EB2D6E, 0xCF934452, 0x82EC26F6, 0x84B3737A,
+0x0972F1B7, 0x39B6DB4D, 0x13E53CC0, 0x67C41D72,
+0x94BAAC78, 0x663A9C6C, 0x36927448, 0xCFBC2610,
+0x980F53BA, 0x7E56C96A, 0x04C62DFB, 0xA471D579,
+0xDF9B2EE1, 0xE12DEBB7, 0x2DB9B042, 0xF0C74B96,
+0x6A3762E9, 0xF4DC39D9, 0x761A5884, 0xFA363D3B,
+0x92766759, 0xF3EAD441, 0x878269ED, 0x1AFFAFE5,
+0xCB432764, 0xFE19475C, 0xCF8776DA, 0x1F0AD906,
+0x7D99AC20, 0xC27317FB, 0x439944A4, 0x65D14C2D,
+0x43E45262, 0xCDE6B3BD, 0xE25C67CD, 0x321AA2E6,
+0x352A2764, 0x5569EF42, 0x005C370D, 0x290801E0,
+0x61883035, 0x2A2DBC48, 0xE2D559FF, 0x01F5DF13,
+0x69B61558, 0xE94BF364, 0x3CA76FCA, 0x2E016483,
+0xDB675F9C, 0x4FA5B6DC, 0x59A6C3EC, 0x56C6E6CF,
+0x24CD59F5, 0x46911834, 0x683B9E39, 0xB5AF6174,
+0x5C31E269, 0x679C9A12, 0x3787D3E6, 0xF1727EE6,
+0xB070882F, 0xFC37EACA, 0xBEE0783F, 0xF6218369,
+0x19372940, 0x3FF7D890, 0x69736919, 0xDD961CB9,
+0x883010F1, 0x6E472D5B, 0x2447E00D, 0xF39E1F0E,
+0x1DBD442F, 0xBE1977E0, 0xC8655F42, 0x37C84253,
+0x3480DAC4, 0x4CFE1DC8, 0xF1521AD5, 0xA45C4F8C,
+0x87FBAEE0, 0x3E41E9E2, 0xF47771E5, 0x16C74CDF,
+0xA33D4035, 0x38513A10, 0xABF3264D, 0xB8D80DF6,
+0xD9AD7256, 0xF78375B8, 0xD7661CF7, 0x1C363AF9,
+0xD425FA32, 0x001D7B98, 0xDB96A1CC, 0xA092E683,
+0x65CF5316, 0x5F282689, 0x9F52F912, 0x8958A1B7,
+0x6457A3F7, 0xAB43FADD, 0x061328C7, 0x9D31B5E3,
+0x75A77F6D, 0x4A764D4A, 0x488CE83E, 0x29887218,
+0x9A04BDD0, 0xEF331070, 0xBCD2F884, 0x6BF66A6F,
+0xB85143CB, 0xFA529278, 0x9EA3A354, 0x4A73BDAF,
+0x0CBB7563, 0xD01AE35F, 0xD2AC3DAA, 0xFC8243B7,
+0xD805D97B, 0xC162A75F, 0x1D49AC67, 0x9E1BC38C,
+0x1D06AAE8, 0xEAF80CD8, 0xCE825DD4, 0xACA3F06A,
+0x83D092EE, 0x3F2BAABC, 0x2482D120, 0xF301680C,
+0x7DAC373F, 0xF5D6178D, 0xB7E9217F, 0xCCFE8C13,
+0x976024E0, 0xA2F39F8C, 0xB6C65734, 0x10AE514A,
+0x696584CF, 0x2542113C, 0x479CB20F, 0x8D3A22E3,
+0xF7C4B88C, 0xF4F7FBE2, 0x2F553308, 0x9EA71E3A,
+0x7B958F48, 0x0927DAAB, 0xF08949B7, 0x7CD46C0E,
+0x7A892BBC, 0x882F32CE, 0x34C490C8, 0x8483ED04,
+0x07EB4EFC, 0x4BEBCD82, 0x83B15EE8, 0x8F3B78AC,
+0xF95EFDA9, 0x816BEBF9, 0x269BDA58, 0xEE373342,
+0xE09FDA9F, 0xC7651AAB, 0xB8D398B2, 0xC7F449B2,
+0x031310F5, 0xC869706F, 0xDA22F127, 0x8C68DF91,
+0xE676068A, 0xB85AAAC7, 0xD32F35BC, 0xE22DF031,
+0xFE142BD9, 0xD4FB2700, 0x2D197707, 0xA3A43A64,
+0x0C02B050, 0xE945AD56, 0x7DEE0A5D, 0x1075DE3E,
+0xD99AD91C, 0x6A7BB71D, 0x1774B3B8, 0x2228B112,
+0x0DEEE844, 0x38074EBE, 0x6DACF57B, 0x7E0094B7,
+0xCE46F8EC, 0x4DAF34F4, 0x5B961907, 0xC8236FF7,
+0xFD380AA7, 0x61EBA84A, 0xAE4892EB, 0x0F1B6365,
+0xB0C4C9A0, 0x04E6012D, 0xA5F90D01, 0xD6C8882E,
+0xBCB9C1EB, 0x0E5E0FEC, 0x53A46889, 0xA2C0FA51,
+0x520DA459, 0x3FD95FA2, 0x6E1D6FE8, 0xBC093220,
+0xAB16390A, 0x163E3D6D, 0x0A63517C, 0x3BF38F3D,
+0x88A1F66D, 0x96263536, 0x412DF008, 0x12FB126D,
+0x44441D7A, 0x31C9F726, 0xF66F60CF, 0xAE1453D4,
+0xDAEAD71B, 0x54EAEE0F, 0x948B73BB, 0x31EA3E74,
+0x355D4FDC, 0x2A1F3A9E, 0x586D08DF, 0x123AC2E8,
+0xF5AC0065, 0x8874ACAB, 0x05B03D63, 0x01BD6A4C,
+0x7A6A9880, 0x2BC16F93, 0xC4112F0C, 0x8287B40D,
+0x48EABF08, 0x29E56860, 0x0F505C84, 0x447DC08B,
+0x1665119C, 0x00347E37, 0x482EF03E, 0x01B15D44,
+0xE6C1B9FF, 0xB165E436, 0x0CF690F7, 0x7FC5BD01,
+0xB784C7F4, 0x9BE04EBB, 0x9F614431, 0x6C37A5A9,
+0x2D0DB87D, 0xF6511369, 0xE115073A, 0xF96C6AB6,
+0x04A13C3C, 0xBF30B2DA, 0x93D18FC6, 0xF67D2E47,
+0xCA089151, 0x51A6BC39, 0x8C1FCA93, 0xFBF2F2BB,
+0xAD0A3F33, 0x82AA2767, 0x81BF2313, 0x758A82B8,
+0xE103788E, 0xC00C4B5C, 0x5F52FF58, 0xABAD38F7,
+0xDA68EE9A, 0x9B6D405D, 0x803449D9, 0x6178B345,
+0x3C785FB4, 0xFEBABE55, 0x0E2458AB, 0x021F0D71,
+0x39201ED1, 0x741B1A7D, 0xE0B0AFF4, 0x45652CFF,
+0x907DA678, 0x313A93B4, 0x0B0D6B0D, 0x42C96E43,
+0xEEE3E7E1, 0xE83C83E9, 0x9052B867, 0xF9514243,
+0x61F20CB2, 0x57E1AC64, 0xC2443123, 0x432C96D4,
+0x616A824F, 0x3C8D1E06, 0x8E64222A, 0x65C1A21D,
+0x8686308A, 0x2A576A2F, 0x1CA0FF20, 0x2C8F9D3A,
+0xC98C9C69, 0x35322A29, 0xDFD33C93, 0x9634F411,
+0x0B4F8FFC, 0x3AED4B01, 0xEBBC7012, 0xED2387EA,
+0x48BF42AF, 0xD60399D6, 0x7A9B8CA9, 0x53886337,
+0x2DBB9429, 0x0A6AF764, 0xDE4D8F78, 0x1EDECEE4,
+0x4F8EE99E, 0xAF23EAFD, 0x929550B1, 0x2CBD8621,
+0x22A8FAA2, 0xBE2A0A8D, 0x06F7E794, 0x16E1F3EC,
+0x093AAEAA, 0x92D429F8, 0xBB79A7E7, 0x43EF89BB,
+0x0E097511, 0x748E68B0, 0x322C00AC, 0xA62EF42A,
+0xD03BB8BC, 0x9FF67810, 0xDE24BF03, 0x140CA6FD,
+0x68F16B41, 0x1B7C68C7, 0x32646342, 0xC5E714F8,
+0xEFFFD2B8, 0x27843628, 0xF8445F51, 0xB9E8519B,
+0x8EB01D04, 0x356FBF2F, 0x32E96BAD, 0x6A629BDE,
+0x52063313, 0x200069B0, 0xE161CF71, 0x84FB7A12,
+0x1805ADC0, 0x80F75012, 0xFE9E629E, 0x93395C33,
+0xFF075A91, 0xB61E46B8, 0xCA9FE7C8, 0x97DCCBCA,
+0xCEFFB6F8, 0x30EE7985, 0x1FABC829, 0x20B3F57B,
+0x27042B07, 0xE12C5151, 0x23482B8A, 0x7B9B8EB2,
+0xC997FEB3, 0x76AB2497, 0xD5CDA590, 0x9EBE90FD,
+0xE3732B18, 0xFF28CEC9, 0xC6582320, 0x6EF106FA,
+0x8ED74023, 0x1A0B69E5, 0x4A95DD91, 0xB41AF82C,
+0x83DF69D3, 0xC548861C, 0x2F60BA93, 0xFC815984,
+0x1A848B67, 0x1EAE87C4, 0xF7479103, 0x8E16DB51,
+0x040B95B9, 0x2A9DB812, 0x987AFCD1, 0x866DF413,
+0xBF9558ED, 0xACF1AF2F, 0xA65305CC, 0x168336F3,
+0x1E59B97F, 0x3F9F447C, 0x3D54B30D, 0xE939D598,
+0x36A40885, 0x02396794, 0xEB0F0A67, 0xCEAEA12F,
+0xC58B4AC8, 0xE6D49760, 0x0F8F2776, 0x66A8F436,
+0x31BACD7D, 0x376993DE, 0x32BD0431, 0x68BDC728,
+0x63EA6748, 0xE6B00E29, 0x7448CABC, 0x42A6517D,
+0xBB1313C4, 0xA04DC8FF, 0x3D402237, 0xA382645F,
+0x52ED55D6, 0x92D7D7B7, 0x541230FF, 0x7AFC0420,
+0x3DC4624F, 0xD9B2193D, 0xA73B9704, 0xBBDE0FF1,
+0x9EB56615, 0x8AB080B6, 0x3C4D8E14, 0x5001B43D,
+0x1EBFAA23, 0xD4AACD27, 0xCFAAB4BB, 0x6FFEE61F,
+0xAE5A7426, 0xDB942949, 0x452C0B16, 0x738E0637,
+0x36A5122D, 0xFF1F7A4E, 0x743D35CF, 0x847D54A9,
+0x42C3EABA, 0xD46728C5, 0x30B2708D, 0x4F6BE0BC,
+0x3C26790D, 0xB0B67C8A, 0xEE07EFDC, 0x9E380611,
+0xEAD6804C, 0x4EF66024, 0x8459AE38, 0x1DEAAFFB,
+0xF76573AE, 0x6CB1C8F0, 0xFFCC267E, 0x26A215F7,
+0x0B1A057C, 0x7DAB9CB7, 0xD40BCBA9, 0xE561F9FE,
+0xA44013A6, 0x7B22C0B9, 0x998A921F, 0xBD25244B,
+0x15E07FED, 0xF15B2E31, 0x54E80016, 0xA12BCE7F,
+0x658A2093, 0xB642C47B, 0xD731FC00, 0xC00E302D,
+0x55B251DC, 0x342939EB, 0x6EADB2F7, 0x0CF93318,
+0x61EBD85A, 0x99B715EF, 0x679C8D3A, 0x9CC1B803,
+0xABEF955E, 0xB8CFF9D4, 0x707A839F, 0xF5D02A7E,
+0x59E0D903, 0x5A425E3B, 0xBB61163C, 0x96ECE9AA,
+0x797B82AA, 0xA9FA6BB6, 0x797C00DC, 0xC1C1FC4C,
+0x8F7FDA66, 0x77902514, 0x6D1B843D, 0x4F881FA4,
+0xC24AD625, 0xBC237A45, 0x9A2E0F44, 0x82FAA3F3,
+0xD70E3489, 0x4F2B3417, 0x65CF65E4, 0xEAAE6A93,
+0x4BEAEC2C, 0x4918723D, 0x7D8F30B4, 0x7706F59A,
+0xCB2A7452, 0x5083D2D6, 0x4724B426, 0x84EB15DC,
+0xBAA2C6CF, 0x71FA984A, 0xDDF7A3DF, 0xB115BF1A,
+0x258AF0E3, 0xA1637D87, 0x03585DF8, 0x5EA4B80D,
+0x8641F318, 0x66EE2F24, 0xC81E505E, 0x5E640639,
+0xDB7739B8, 0x1A3B861F, 0x0F5ECC51, 0xB21C00DD,
+0x680FF30B, 0xDE697468, 0x57A43B33, 0xD7EF6B3B,
+0x4BFC7D25, 0x710F0752, 0xABAA9752, 0xCFCFD84D,
+0x3BCC1CDC, 0x2381C524, 0xB60CAD92, 0xE05BC1AA,
+0x2B887D88, 0xCD4566C5, 0x0D2976E7, 0xCB000A2C,
+0x667BECF6, 0xEFC7F221, 0x7A7584D1, 0xC41D8B2E,
+0xD9BB7D3F, 0x7CEB5626, 0x7D8165A0, 0xEE178F99,
+0x3E8A8CB7, 0x693D4501, 0xB0E228A5, 0xD55B73C1,
+0xAF9043BF, 0x6C627A2C, 0x7B9F490C, 0x7EA61899,
+0x92B980AF, 0x6D13C758, 0x2C007C73, 0x74336E0D,
+0xA39F13AC, 0x533F05D7, 0x75536CFB, 0x9708DE27,
+0xE2A14E87, 0x36673FEF, 0x71BA654F, 0xB98CD2FC,
+0x27F29A6E, 0x82478171, 0x1C2815F0, 0x8A8F4549,
+0x048A8D9B, 0x7CEE51F2, 0xA1648AC3, 0x004F8B8F,
+0xB6FE8EF0, 0x6D10A0A1, 0xAD7A24D8, 0x75039717,
+0x97847786, 0x2791CC05, 0x6937FD6F, 0x60F98115,
+0x5FAB6D35, 0xC0550A70, 0xC0F4D817, 0x7B5BFDDB,
+0xEF63B4D2, 0x6C87C6C5, 0x956D6B87, 0x69179257,
+0x10973C90, 0x8CDBE860, 0xC7C761EE, 0xF823E34E,
+0x6FA2CF3B, 0xA903ABCB, 0xC82C9B01, 0x60FE96E6,
+0xE5EC33C0, 0x73A3011C, 0x2A1B9054, 0xCF16F92D,
+0x4FAF6CC8, 0xD9DD74FE, 0xB3C639ED, 0x3F47AF63,
+0xC8E99D12, 0x92D95986, 0x835ACA6F, 0xD52930A2,
+0xC7DD54A5, 0x617FDD15, 0xE9A6D295, 0xF56C6087,
+0x7813B662, 0x1F8EA244, 0x1CDE3BAD, 0x58FC0F7B,
+0x02E31A5A, 0xA78EAC74, 0x10C06107, 0x22BA3C63,
+0xF84AD224, 0x6A8BF66C, 0x2A5CAAC5, 0x8ADC3FB5,
+0x9683451A, 0x1B52FCB4, 0x95491BA5, 0xFE6C3713,
+0xE9098CEF, 0x73C01EF9, 0x6E85EF1A, 0xEE189743,
+0x2E9E5286, 0xC1FAA665, 0xD861E384, 0x701C834D,
+0xDC5CA5CC, 0x52A3A6C4, 0xF2AF2C43, 0xC37C6465,
+0x6E94AD69, 0x98808AF4, 0xED8A99F2, 0x377257D3,
+0xE60F2096, 0x615EFCB8, 0x67A2BB3A, 0xB4DDD40F,
+0x1D47F918, 0x86F77D6E, 0xFD05D2B8, 0xE18C330C,
+0xA48260A4, 0x5615B83B, 0xBCD7D855, 0xF8073219,
+0x8622BB89, 0xD35CE05B, 0x17162483, 0x137BDB69,
+0xECD0F226, 0x61F8982A, 0x3C10ABD4, 0x2F33ABF4,
+0x9358B547, 0x58B277A7, 0x92456A7C, 0x4384B49A,
+0x5F1FF0EC, 0xA153EA4D, 0xA8E49100, 0xD3A75723,
+0xD1ADC606, 0x76C314B7, 0xBC6AB227, 0x257312AF,
+0x8B6AA1E3, 0xD87FF5E8, 0x2BAED373, 0xC848AB63,
+0xB72B1E5E, 0x730A73D8, 0x4915E5B6, 0xDF7D77AD,
+0xEAE247D7, 0x9556DDA8, 0xDE0C9C47, 0xA4E3296E,
+0x31F5BC94, 0x05258B24, 0x2837374F, 0xC7E4C81B,
+0x5A1AC819, 0x068074AE, 0xDF876732, 0xC0192EF9,
+0x7FFD84D8, 0xFF1CE148, 0x821B4AA3, 0x56674838,
+0xF9A147F4, 0x182EF58B, 0x16E17174, 0xDE27029E,
+0x8BEC55AD, 0x40646F89, 0xDBFF92FC, 0x9F24C017,
+0x711EAD18, 0xA663E1EF, 0xEF92F684, 0x4BD05E67,
+0x7E089B13, 0xCBF619BE, 0xCEBEF231, 0xC947586C,
+0x0F526C47, 0x6672600F, 0xDAAB63DD, 0x950D4FD0,
+0x199C3EC2, 0x0F201C9D, 0x06BCC8D3, 0xA7672C6D,
+0xB39C7D0C, 0xC74B0805, 0xC9BBD249, 0xACDD5396,
+0xAB7BDF8E, 0x12012B8E, 0x67236047, 0x0AE0741B,
+0x1D747E56, 0x7EC6C00C, 0xD08E8341, 0xB0ABDAD6,
+0x4FA4BDF6, 0x90CE8D0E, 0x6E734117, 0x3EF9192E,
+0xACA32DA2, 0xFDB9C58E, 0x256626B5, 0x5EA961B3,
+0xFBC15776, 0x36602B5F, 0xF8D08644, 0x5B693C23,
+0xC62EA3B1, 0xC664C7C3, 0x73BE8859, 0x17F44E8F,
+0xF9B8D923, 0xD168A3A5, 0x6CCD110C, 0xD353181F,
+0xC0E774EC, 0x5F9E127C, 0x6C824511, 0xFDA13494,
+0xCB588BA6, 0x47148694, 0xAB877E87, 0xE97F757B,
+0xF54D0A2A, 0x0FE11891, 0x5D8747FB, 0xE7800C7E,
+0xEF96298F, 0x400F458A, 0xE2D04518, 0x4B4E6EFC,
+0x9B15002C, 0x3CE1B537, 0xF5ACB9B8, 0x67030647,
+0x475FD148, 0x1E03A40A, 0x896C7C05, 0x85F70B68,
+0xC590CA84, 0x53B5440E, 0x1400F78F, 0x3ABE7F8A,
+0x19CA67FF, 0x68B54A34, 0x555988AC, 0x4AB16B4A,
+0x7511FA63, 0x248EC9EC, 0xC25AFE4F, 0x19F578E1,
+0xE92AF03D, 0xAF9DE18F, 0x2798C7A7, 0x6B46990F,
+0x41D45894, 0x74696A0A, 0xC6AAF5F8, 0x72CC10E0,
+0xDB9CA283, 0xD6BBD0F3, 0x58EA4C06, 0xDEA5E8B9,
+0x1908EBDB, 0x95D33DD5, 0x20D7013C, 0xE725C282,
+0xFD48C92F, 0xDBBA7D19, 0xC7BEBEA9, 0xB186B799,
+0xDD0DD17B, 0xD8090A41, 0xF98BC20B, 0xDD7E4B9D,
+0xEBAE4247, 0x4376FDC4, 0x7F3EFAC6, 0xA9B9A951,
+0x4AE390C4, 0x651863AF, 0x2CD42DBC, 0xC2A13962,
+0xEF0FC443, 0xAEE63246, 0x09B83E19, 0xC3C940AB,
+0x00B12826, 0xC0A30412, 0xFCF6ABCC, 0x3CFE721A,
+0x62C1F4C6, 0xE963A359, 0xAE11F3D6, 0xE490D12A,
+0xC45C928B, 0x05CCA78A, 0x1982E93F, 0x577F81CA,
+0x66D50D6E, 0xB4C7030F, 0x93092C3E, 0x118B08FF,
+0x178545B7, 0xEED74838, 0xF7D2CE48, 0x238969BC,
+0xB8EFAEAE, 0x75726A3B, 0xB1E0220F, 0xC4D60EB6,
+0x0EBC0243, 0x5FE0D6CA, 0x35456B45, 0x1F64AC2A,
+0x58484A1F, 0x2A11455D, 0x33BC4403, 0x56E4E62D,
+0x60B41E2B, 0xDB65D3F8, 0x7EC18D34, 0xF575DC85,
+0x6E0B9995, 0x1C14C91E, 0xB2A94718, 0xAEC4A823,
+0x993D374E, 0xF1E4210B, 0x8CFCC03A, 0x99BD1C28,
+0xA928E3F9, 0xBB957D0E, 0x77C865EF, 0x7FF50A45,
+0x4279A638, 0xE628FFA1, 0xBCCA171E, 0x284C9CEC,
+0xA476E346, 0x7E2F9C08, 0xBF65044F, 0x5B7C3D5B,
+0x6E60EE5D, 0xF5C99509, 0xFA352B7E, 0x6FDE8E8A,
+0xF2340FE1, 0xDF542B6C, 0x510CB30B, 0x367E7016,
+0x198A0A95, 0xA4DF508E, 0x593C2338, 0xB12BCDE1,
+0x554AD3C0, 0x4DDAB1C1, 0xD2BD1850, 0xF6E126CA,
+0xF87289C7, 0x86EC92A5, 0x4E033906, 0x52DC5F3F,
+0xCC6E2E59, 0xFF751753, 0xDF8B8BA2, 0xDBF5954A,
+0xBD367488, 0x6A0CDF1F, 0x4103139C, 0xDE49DBB0,
+0x5A8428F4, 0xA26872B1, 0x96BF7203, 0x99D5E78E,
+0x243850A6, 0x389DAD80, 0x6335D33F, 0xEC67B0A5,
+0x029C0CA9, 0xF5F6F6C9, 0xDF574C15, 0xE6D3EC29,
+0x1AA349BA, 0x453E7258, 0x7DB79BE3, 0x51FCA7F6,
+0x2B42FCA5, 0xBF0E4871, 0x58063C40, 0x193580E2,
+0x25605322, 0xBC49C479, 0x0ED70FC4, 0xA78B59A0,
+0xE6CE3E8C, 0x92EE657A, 0x63D12529, 0xF95DAF45,
+0xF92C3BF3, 0x7D514200, 0x694DF84A, 0xEF177E2D,
+0x4E119CCF, 0xA025C55D, 0xF96974D6, 0x26D13E7F,
+0x799ADC27, 0xD7925EC1, 0x8AE60BF7, 0xF9EF1A2E,
+0x89EADD3A, 0x9C28CACF, 0x63377EB7, 0x6D1EF7E5,
+0x6585B16C, 0x9972D115, 0x65F8F5E6, 0xF93DECB4,
+0x6D71605D, 0xC6FDBCB8, 0xD937BA31, 0xCED727EE,
+0xC34C5605, 0x25FA70B6, 0x5C0B7FB0, 0x8F9340F5,
+0xA3376693, 0x4498B66A, 0x2D21F377, 0xC0A4C6EA,
+0x0780736B, 0xF42D7F07, 0xE56D47E5, 0xB48C25D6,
+0xA48DA0DA, 0xFE69693F, 0xF01E19CA, 0x8A0C5C8F,
+0xDF702C23, 0xE18A93F0, 0xD4D5C91E, 0xD2A706F7,
+0x674F9E28, 0xAF0F80C7, 0x648D49E8, 0x6BE8640F,
+0xF5FCFFD5, 0x8EDC391E, 0xE583D8BC, 0x8426C090,
+0xF456A27D, 0x07249BF4, 0x054A2F45, 0xAC46B73B,
+0xB89EEDFB, 0x48EAF867, 0x69B2D7CC, 0xCA0CA0F1,
+0x38CD0428, 0x029808CF, 0x86EE75DC, 0xF4FEE9F0,
+0x6987D5E9, 0x56AB5537, 0x3DDD0940, 0x4742FF89,
+0x2C3B179E, 0xD05B5CB1, 0x3C4E9033, 0x6BCF0141,
+0xF2F6D3E2, 0xAD297B1F, 0xB1CC23D4, 0x5452038B,
+0x1751FCBE, 0x24AA465F, 0x94C62D18, 0xF49B2EC8,
+0x97AC47DF, 0xD66C19B5, 0x09AAB297, 0x89936144,
+0xD15C026B, 0x4CEC8778, 0x94050D61, 0xD812E96F,
+0xB6BD7B12, 0xA5F9BE77, 0x531A5C7A, 0x3605BA71,
+0xD500CE54, 0xE325964C, 0x323432FE, 0x580A9DC8,
+0xD25A3135, 0x089D6C9C, 0x58856F73, 0x7DFCEE30,
+0x7DE2580F, 0xF4E4488B, 0x71821DDF, 0xD194F5DC,
+0x7D070394, 0xBA28BF76, 0xAAF0A38E, 0xD4F6275E,
+0x1B742E66, 0xD9E68EA9, 0x68B0F939, 0x52AF9D7B,
+0x54A39705, 0x20F844C1, 0xE6981DDC, 0x80322E62,
+0x536235B9, 0x7A57F4FC, 0x14EBF376, 0x64BE2E5A,
+0x70A18910, 0x0FE09587, 0x10E9CA78, 0x8F90D3D2,
+0xAE74717D, 0xA544EAED, 0x6746AF3E, 0x430CB3FC,
+0xBC185576, 0xEAA35DC3, 0xDA6309D2, 0x40643F87,
+0x68859117, 0xA17AC84D, 0xD7922CA8, 0xEF7C0BEF,
+0x83337348, 0x9B4B1790, 0x8876A77E, 0xF293C9C7,
+0x20D399CD, 0xA78224BA, 0xFD1279C8, 0x8B7837C1,
+0x0F1DD415, 0xAE3FBD2E, 0xC4F77B52, 0x51E79FB3,
+0x7A856D9D, 0x14BFDAD7, 0x993FB625, 0x667C65EF,
+0x32F83338, 0xAA06EDCE, 0xACE7A099, 0xD26DAE89,
+0xDC6891CE, 0xCD2F6F04, 0x27425FB8, 0x7C301D8D,
+0x1EDEBE1A, 0xBE540AF8, 0x1D356C6A, 0x963E8639,
+0x9920CA55, 0xDEFE5F44, 0x107D5545, 0x3D079BE4,
+0xEF673F66, 0xDB3C2954, 0xDD76D666, 0x1DFBEF59,
+0x8F384B34, 0xBE6F773C, 0x079DD187, 0x2314AC8B,
+0x5FEB0114, 0x59E85CF3, 0x9BFE9190, 0xB360A31B,
+0x4F7EF967, 0xFEB0D561, 0xBFE779F2, 0xF33702B3,
+0xBB263417, 0x09607C65, 0xA877F109, 0xBB43CFF1,
+0x4A190DB2, 0x9B7BD38F, 0xAEB7C449, 0x3DB3A460,
+0x7D928522, 0xD18AC966, 0x187FE766, 0x97629792,
+0xF59D506E, 0x6FBA202C, 0x77035FF3, 0xDA068CDE,
+0xE195779A, 0xAEB92298, 0xD2A44EDD, 0x12577D85,
+0xA3B47B9E, 0x5BD07CB7, 0x4B6AE3FC, 0xBE35B6E2,
+0x9D7F7AF2, 0x9A38EA75, 0xD87FB055, 0x3339F2A3,
+0xD7CB82B4, 0x357721E4, 0xBEF46553, 0x9DE28CA3,
+0x1B1EC2DF, 0xE29B9CC0, 0xEFAE347E, 0xE5864917,
+0xA097B712, 0x6B67041E, 0x5B29542F, 0x01D96EED,
+0xF9A6DC07, 0xC0B5E3F0, 0x21E1899C, 0xE9373A86,
+0xF3176509, 0x950844A2, 0x7D24FFEB, 0x5DC0BCA0,
+0xC442B7C1, 0x37DC6EC1, 0xC65C8BA5, 0x18F0FA85,
+0x2AD80D2D, 0xC68CDCBB, 0x6AE5EC93, 0xE3955DBD,
+0x3E80C4B3, 0x50FED127, 0x743CABC0, 0xD0E91707,
+0x9BF7EB4B, 0x7A632755, 0x9A192482, 0x8F923E9E,
+0xE2E70FE5, 0x5F50AA16, 0x0EC496D1, 0xC6EC4862,
+0x040A0274, 0x2FC951C2, 0xF65D3A80, 0x8D585163,
+0xC6B529D1, 0xD2CAEE6E, 0xE3E112B7, 0x3244312F,
+0x1B393E58, 0x2444D538, 0xBE69AC21, 0xC92A0506,
+0xD1A74434, 0x49C3EA05, 0x0E53B319, 0x3843CE03,
+0x8DB8415E, 0x766B6FC7, 0x515B9E7A, 0x3BA05B32,
+0xBFAFC449, 0x31302A57, 0x1960A211, 0x66A097E0,
+0xBC65A9B4, 0x89E83065, 0x36FDBF2C, 0xDCD4664A,
+0x0ED6CFBF, 0xDD4DC6DC, 0xD76D2F00, 0xB6DA6540,
+0x9A396444, 0x28F185DE, 0xA0FEFA1D, 0xF476E0ED,
+0xEF15505A, 0x183365BF, 0x481FFD90, 0x29ABEE75,
+0x1EC90B07, 0xC10B2657, 0x0DBF6DDB, 0x52AD02B7,
+0xE87DDB54, 0xD3704106, 0xD4E2C592, 0x0CB2DD05,
+0x4BAA2FFB, 0x02611368, 0xD50F8F1C, 0x416FF25C,
+0x9A69782D, 0x268C6474, 0x2ECD4D64, 0x196DE2F5,
+0x47A8561C, 0x8C7CE6C9, 0xD2B1E2D2, 0xA038C165,
+0x3AB8844B, 0x4A699830, 0x0FFC0B17, 0x89B685AA,
+0xDA276D85, 0xE934C4CD, 0xF511226F, 0x9CDD2B1F,
+0x94F75492, 0x55ECEB42, 0x42F0A3D3, 0xD7EB482C,
+0xA78D0373, 0x62F088A6, 0x7ECF4602, 0x7A3404B6,
+0x40B36495, 0x60441DF4, 0x6722F539, 0xCFE76C48,
+0xB6B94C9F, 0x9ADB4B6A, 0x1EBBA65F, 0x5B5081AF,
+0xB764423C, 0xB6F910E3, 0x14AC4B6F, 0x5C811E82,
+0xAA36E5F1, 0x24EC82AF, 0xA2F1C050, 0x0504324C,
+0x304CED0F, 0x01E31DD9, 0xC82EC7E6, 0xD55AFFF9,
+0xFFB3047B, 0x3006F2E9, 0xC725BCD1, 0x7DCC1082,
+0xA9A22CF8, 0x64D5AF9D, 0x389C34AD, 0x7DFF37C6,
+0x41F1509D, 0x1845B3FE, 0x055C23F0, 0xC6291F5F,
+0xCDD3C7DD, 0x5F0356B4, 0x7FD2C387, 0x494A091E,
+0x50C69D3E, 0xFE769A5A, 0x63904701, 0x8960ABF2,
+0xE68EDF3A, 0x0AB57C8E, 0x0B9D0A6C, 0x51888148,
+0x50C5D533, 0xC69038FA, 0x3ACBE661, 0x0CAEB601,
+0x8C14AB6C, 0xBA86D94F, 0x0724056B, 0x0FEFFCBA,
+0x12449DDB, 0xABFFECCE, 0xB12A2BD7, 0x7260A0E8,
+0xBE184A48, 0xCFD3CA3F, 0xDF088660, 0x78EE9B67,
+0xA9EDB113, 0x4FD5D353, 0x8E348CC6, 0xD578C337,
+0xF0493BE9, 0xCCFB54EC, 0x9CEEF85C, 0x0CAAE15E,
+0x371AD12F, 0x9C5B9270, 0x2495F0DE, 0x06DE2DBB,
+0x911AE7EC, 0xEEDE3363, 0x6DD38D6C, 0x2AF7F3D9,
+0x51C8D118, 0xF23818A7, 0x95438AEA, 0x3A8A798F,
+0x230D2BEF, 0x3D16273C, 0x9C36FF83, 0x785C9537,
+0x3E42AF2F, 0x12A16741, 0xE58D0DC4, 0x33EBEFF9,
+0x6F1972DA, 0x128C9BAA, 0x858D6032, 0xDAF185E1,
+0xAE355065, 0xDE0086F3, 0x0F661A65, 0xF4334169,
+0xB1559BA6, 0x3892109A, 0xE903BA00, 0xAE0CBD58,
+0x073C21A0, 0xFCADB299, 0xB4E39AF1, 0x78475459,
+0xB46DC847, 0xDBA97661, 0x15D118F5, 0x01ED48D0,
+0x99F658BC, 0x399FDC8E, 0x44D4A919, 0x7C2CE4B9,
+0xCA0367CC, 0xCC2B9828, 0x16AACAA6, 0x7AA5B6BA,
+0xFEC77C66, 0x231B22F9, 0xC8BE0D04, 0x6FF2788C,
+0x5F9CEBB5, 0x901EAA5D, 0xDE682BBF, 0x998E70D4,
+0xBD9CCCDA, 0x6995441E, 0x5702F360, 0xBC035EED,
+0x20F60B51, 0xD57361D8, 0xC071113B, 0x73CE6CE4,
+0xC6569DC9, 0xD24B89ED, 0xA6052276, 0x8CEE2026,
+0xFBF5B58E, 0xF692DF81, 0x6B7CDD7C, 0xF5B6C04C,
+0xEC1BBA29, 0xD6AC8CDD, 0x320491F8, 0x1D812AC7,
+0x631B0051, 0xD08A4D2A, 0x569746DD, 0xAA653FCF,
+0xA92E8E70, 0xC59A6705, 0x278EA1FF, 0x63E5FA17,
+0x1C20E82D, 0x550F7CE3, 0x55CED415, 0x5F9C4C4A,
+0x7D746311, 0x5B07976A, 0x12477E31, 0xAB8113AA,
+0x796EDCEA, 0x4A90E4B4, 0xB36E6188, 0xEE7D5E0F,
+0x15CEA060, 0xB81AB2CA, 0x296D22B0, 0xFA0753E2,
+0x0D0D15BB, 0xD4AF8BD7, 0x951FA575, 0xCBEBD58A,
+0x0AF5C362, 0x9EF43FB0, 0xD97E5184, 0xA14469BC,
+0xCAE5D55E, 0x93D4CDF9, 0x95B013A8, 0x6998F35C,
+0xF1DDC0B1, 0x476F9FC7, 0xB6472B70, 0x1D55AC5C,
+0xF0E0C0C8, 0x95372BF5, 0x75CCCDBE, 0x9F9D2003,
+0xCAAD0D51, 0xEE54CC2E, 0xE5EBDBF0, 0x9B248BB3,
+0x4BF07D19, 0x542997E9, 0x17447C4B, 0xCF2B2768,
+0x86118A5B, 0x57579F12, 0xC5CD9E74, 0x97ED5724,
+0x01BD2EE4, 0x2A0403A6, 0x01833741, 0xA1E8D364,
+0x4D1A2EEA, 0x62760377, 0xA10D6861, 0x09C68E2F,
+0xAB482850, 0xACD24B74, 0x5038C8CA, 0x71DE3A93,
+0x671D25E4, 0x9EA7AC1A, 0x3E7287F5, 0x9FC963CF,
+0x73F90AB6, 0xC775D840, 0x00B868D9, 0xF6A9BE3D,
+0x17FFB472, 0x5D2389E3, 0x0D42A149, 0x2FAB1235,
+0x90A7998E, 0xD895F6EE, 0x19921013, 0xEE42EA48,
+0xC5D19A17, 0x5507890A, 0x9F893B29, 0x4FF39F19,
+0xD6EF85AD, 0x3FFB1599, 0xF1761017, 0xFC51B90D,
+0x8F6C566B, 0x44BAC7A4, 0x2B2E3755, 0xABECB8DB,
+0x5C4A1629, 0x837CC4F7, 0x3E732B0A, 0x803CE303,
+0x71865D8D, 0x346665AB, 0x58BF809B, 0x100626AA,
+0x9446AB13, 0xD53ADCDA, 0x75C0BFCD, 0x95853304,
+0xF4758E87, 0xD6B64517, 0x13293D0D, 0xEC9368FB,
+0xD449A2CC, 0xAA17B0BE, 0x9D0B85C0, 0x77BEED16,
+0x7699CAE7, 0xC776D10D, 0x962D48CE, 0x838D00BE,
+0x279AEBF9, 0x22EF837B, 0x58E46DAD, 0xB56B6305,
+0x3232D58B, 0x167969DB, 0x5B63F5B5, 0x7E82B175,
+0x05DDB402, 0x5AB29BBA, 0xF3B627D5, 0x97168C85,
+0xAD9EE022, 0x48F0CEEA, 0x84104C22, 0x690FCC19,
+0xCA2F2474, 0x76F95539, 0x9FD2B987, 0x79EFC557,
+0xCEE5DA4D, 0x27EB98F6, 0xA0628916, 0x8E05614F,
+0x8AC89026, 0x7705135E, 0x3F7E42B8, 0x7BCD773B,
+0xF98B9741, 0xCB8A514E, 0x9298220D, 0x5665FA3A,
+0xE66A1FF7, 0xAC4ECB71, 0xA7E56FEF, 0x9D1EF7F8,
+0x23566B64, 0xB4FE822E, 0x1AA53208, 0xF4545E5D,
+0xEA86C879, 0x18F6B7C2, 0xE10A17AC, 0xBD37011F,
+0xFBDF81B8, 0xA978A4EB, 0xD42437A7, 0x474E6A41,
+0xF8885248, 0xF750BAA9, 0xD238EA62, 0xD69BA74D,
+0x266EC6BF, 0xE7EDE077, 0xE8F0A303, 0x8B56A96D,
+0x41380980, 0xDDF0B16C, 0x00E83594, 0xA503EBF5,
+0x960A258E, 0x499827BD, 0x6C8E6F7B, 0x166C845D,
+0xC842C934, 0xBAEFC699, 0xD9846213, 0x832EC19B,
+0x1EAD7599, 0x221E7EE9, 0x8176A313, 0xB28D8E39,
+0xBAC29A96, 0xB964F91F, 0x3F268150, 0xD4BB7011,
+0x347EC445, 0x7FDC9E82, 0xEB70F4C9, 0xA6F38EBF,
+0x398CF137, 0xD7F88CF5, 0xCBDDCB3F, 0xA0DAFA74,
+0xD29D30AD, 0x822B6919, 0xCE059949, 0x3A946183,
+0xDE4C572D, 0xD1E6D844, 0xC43C7DAC, 0xDBBEEDD0,
+0xA656DF6D, 0x454C22A9, 0x9FA48790, 0x69B04531,
+0x99BB305F, 0x80500F71, 0xFE2363C2, 0xB67F538F,
+0x302EC0C3, 0x4A6E3458, 0x57E4CFD4, 0xE65CDAEB,
+0xF31ABB31, 0x62DF98AC, 0x894AE781, 0xB1588AB1,
+0x45D5CC3E, 0x3520F5B0, 0xC72D0CB7, 0xA1D6CBF9,
+0x742FFA63, 0xA0A5224F, 0x5EA1C85A, 0xB81E9F77,
+0x31D76C4F, 0x525257F5, 0xBFF85009, 0x2125B270,
+0x16E47E6E, 0x9128B981, 0x0D5FBE39, 0xF67A418C,
+0xCF3C71CB, 0xAC04ABE1, 0x9B550AAF, 0xB5077F18,
+0xFB7C5EC0, 0x64784DB4, 0x1E668B48, 0x84659836,
+0x604457BF, 0xF6F69C8D, 0x394301DC, 0xED0211BD,
+0x8BAC1A3A, 0xBB752FD2, 0x78B8C036, 0xBCB98E8A,
+0x33C595DE, 0xB3F3C5F8, 0x698666AC, 0xA1F42D7A,
+0x5751ACC8, 0xC069575B, 0x35D50F99, 0xB294BF38,
+0x82A4A331, 0x05147751, 0xCAE18C12, 0x9E89AAF1,
+0x3531C372, 0xB2114A88, 0x41797201, 0xDDDDEC10,
+0x01185F2A, 0xDED50CDC, 0x72156BAD, 0x88F3DB94,
+0x50450DDF, 0x6B1E7ABF, 0x3D317708, 0xFDFF5A15,
+0xDC8B1697, 0xCC2248FD, 0xD9196272, 0x4445195D,
+0x54D90281, 0x7A891C9D, 0x69FF98D5, 0xADE6D74B,
+0x26D27973, 0x0F14734F, 0x3F957FC8, 0x812AC874,
+0xEDC0F9B4, 0xD31D6D75, 0x7A2608C3, 0xD89984B1,
+0xF581081A, 0xEDB9DF6F, 0x16ECC191, 0x6B945724,
+0x1BCE8269, 0x02E6DB68, 0x56362541, 0x9D247CF4,
+0xA5265E72, 0x2C8B9413, 0x1157DB4B, 0x3145CFB2,
+0xFBDEBCF5, 0x1042B117, 0x284DAE18, 0x10575C21,
+0x1DDE578E, 0x80F59EDE, 0xCAB51C04, 0xB594BDA8,
+0x08ACEF85, 0x08C8D4C7, 0x7304D433, 0xE87D3A88,
+0x31CCFED8, 0x1D8E71E5, 0xC5A2F02C, 0xACBF3B5E,
+0xAA161BCA, 0xA10BE577, 0xF9CE41D2, 0x2B86F031,
+0x3D4A8D23, 0xED926DE4, 0x3844E21F, 0xFE57BCD0,
+0x36DC309D, 0x17137409, 0x9F6A8507, 0x14CF12EB,
+0xA770AFB5, 0x7C6DA2E4, 0x856B48B8, 0x2EA235DF,
+0x55BD1164, 0x5BD9FF0C, 0x5228C552, 0x9E719AFA,
+0x3EC3703B, 0xE06A94F3, 0x296FF0D9, 0xE468D9C9,
+0xD2A15CDC, 0x6C4EAAA2, 0x2AF3B8BF, 0x6B6EDC78,
+0x42B78972, 0x4C97A66C, 0x161C30BF, 0xCD2816DC,
+0x431BDA17, 0xD9653022, 0x67D95E39, 0xBCB18342,
+0x227982E7, 0x23C5B11B, 0x514420AB, 0x089F3A5C,
+0x2B2F8244, 0x2F2A80C8, 0xB0A90558, 0x75BAA243,
+0xE2FC4F62, 0xEB0A6104, 0xB7F221B2, 0x4ECD79DF,
+0xB3E08B8B, 0xBA25E1CB, 0xD39F3431, 0xB50202FE,
+0x78F15ECE, 0xEFF61ECF, 0xB3CDDD50, 0x3FD064A8,
+0x96B028BC, 0xB29DD4E1, 0x7E9EC629, 0xC407F4D1,
+0x8C21785B, 0xE11767BA, 0xCFE6DE26, 0x0DA98E22,
+0x33AC5670, 0x0FDBC175, 0xF11F8EF5, 0x60638843,
+0x8B67E55A, 0x3F27F75B, 0x6691FB98, 0x635A35A9,
+0xB317459C, 0xE7419C01, 0x8BAB28D7, 0xE347D791,
+0xEFC019A0, 0x45009041, 0xA6DEB3E8, 0x6F7379FF,
+0x0FF50390, 0x810BEE78, 0xAD13716B, 0xA7DBD7AB,
+0xEF439D4B, 0xDDA744A5, 0x31EDDE8D, 0xA85B71F2,
+0xDF439C70, 0xA7E3DA94, 0x525ED453, 0x3D913C32,
+0xD104CE61, 0x42F5FFED, 0x14C7625A, 0x4E5B314B,
+0xA7EAD1ED, 0xFA01D595, 0xE67BCF06, 0xE63685E2,
+0x3A32E9D3, 0x374C25F0, 0xA8E8A41D, 0xA403AEF5,
+0x901A194C, 0x17605BC9, 0x8522DD12, 0x27096BAA,
+0x017434B7, 0x99C8D2DA, 0x7F96B068, 0x8521CD09,
+0x529B46D6, 0x47852810, 0x021BC8BF, 0x93C98329,
+0x6FE73A78, 0x44DB69A9, 0xC839D490, 0xCAC42AFE,
+0xCF1ECCF4, 0x6F2E5F44, 0x795C8219, 0xA06C667B,
+0x80411F31, 0xB09926E1, 0xC62B6C18, 0x77C6E6DD,
+0x7622FC07, 0x02162DB2, 0x3EA31334, 0x6CC02B4A,
+0xAA6B81C3, 0x4424A9A5, 0x26BD2EF3, 0x334896D6,
+0xADDD2711, 0x76035757, 0x80AA328E, 0x2F39C06E,
+0x357520CB, 0xF62BDF46, 0xC59343C4, 0x7CA4CAE2,
+0x89B03EF3, 0x251A785B, 0xA4755BB9, 0x262D478D,
+0x462E6252, 0x6B5F6BED, 0xCA46E77B, 0xA2CF08AD,
+0x561E19EA, 0xBF31AA15, 0xD376F44C, 0xCC332150,
+0x8C0AEE42, 0xC06D5F91, 0xDADF8613, 0xBE0FA22C,
+0xF50AE482, 0xE3615501, 0xECC8D5AA, 0x58A7FD3E,
+0xD59B8CC9, 0x09DB0987, 0xF1D9753D, 0x9C79E20E,
+0x9A222AEA, 0xC4E58914, 0x6712E0A2, 0x8CD5C80E,
+0xEAB8AA56, 0xDBFA8D9C, 0x3515BD21, 0xB65B9E0C,
+0xF0D27FEE, 0xE33871C1, 0xEE8FE52F, 0x02ACCB3F,
+0xE9197277, 0xB7B70770, 0xA26E3581, 0x82481E7F,
+0x005AF99F, 0x8B970B4B, 0xEC74B662, 0x2F21C5A3,
+0x049DBA83, 0x495B3E1B, 0x112234B8, 0x95B42A5F,
+0x2C8FA833, 0x6D706E30, 0x2AAAEC09, 0xDE7C3377,
+0x06CE9D46, 0x7574EAAB, 0xFCB1A08D, 0x462AFB6C,
+0x192847B2, 0xCC149AC3, 0x427834CE, 0xE90180A0,
+0x946E526E, 0x6018BE4E, 0x20442F52, 0x1D39FA05,
+0x35F690AD, 0x29DB3A53, 0x6360158C, 0x3EC815F8,
+0xDED650AF, 0xFA168B37, 0x233F8A3D, 0x245009CF,
+0x71BB2237, 0x4989A01C, 0xD58AE4F1, 0x62C99EA0,
+0x48E9056E, 0x7E1A786D, 0xBF6CBAAB, 0x22669A6B,
+0x57857590, 0xE4558CE3, 0xBC6C63EC, 0x6AE02A61,
+0xA2ABFBBB, 0xD2B2FE90, 0xDF8BDB43, 0xEC2D59AC,
+0x7B6AFDC3, 0x6B001D5F, 0x3DFEE08F, 0xB9A597D6,
+0x09DEAC68, 0xE42D9E73, 0x2E33507C, 0x6525F051,
+0x0D7143C6, 0x01DD115B, 0x94180279, 0x28FC60D7,
+0xC0900603, 0xED4FBE53, 0xFC0677BD, 0x7DA2A878,
+0xA8D0EC73, 0xF6A09B2A, 0x24A129EE, 0x169BCA2F,
+0xE0BAE526, 0x5C8E2FCB, 0xA218EFFA, 0x842B61FB,
+0x87B860CD, 0x106E9B86, 0x930685F0, 0xC5A72109,
+0xFB977BD5, 0x9D3B4AC6, 0xDA378FE0, 0x0AAF747B,
+0x0408D50D, 0x488785B9, 0x81AE971D, 0x12ADFEF3,
+0xF0B64128, 0x3D4C90BB, 0xC994AAA1, 0xB854400E,
+0x901AE3DD, 0x7A4A0DE7, 0x18E07456, 0x20C38BCD,
+0x94441976, 0xE2E419C2, 0xDBD3C92F, 0x4DD63841,
+0xE2994959, 0xF41F196D, 0x0835431A, 0x93A2E9CF,
+0xB01FABED, 0xD0135535, 0xEBCEA18D, 0xC4F83A1B,
+0x5D72845C, 0x04335E3A, 0x68C4C987, 0x77178710,
+0xC5293A9A, 0x44E40AE1, 0xCE454FDE, 0x71DE89B7,
+0xA373D9D3, 0x6D19E483, 0x812896D6, 0xC3231C14,
+0xE960ABA4, 0xB7FB6F83, 0x1F7C4EB8, 0xD10DBE69,
+0x8575CF6E, 0xC03B15D5, 0x4D7F4EF3, 0xF0615F31,
+0x34E21762, 0x22D5A7A1, 0x729FA3F8, 0x2E1050FB,
+0x8A9F46DC, 0x535EB5A7, 0xD143560E, 0xF8EC3A4B,
+0x2249FD06, 0xE8E2AB08, 0x1E734127, 0xBA5B635A,
+0xD8F419DB, 0x0B5200D0, 0x8110304F, 0x3497DA80,
+0x35CA71CD, 0x0FD8227E, 0x086C74E2, 0xAB68A1AF,
+0xE3BD57EC, 0x83B42D29, 0x3C2D672D, 0x05D85CED,
+0x64F04926, 0x91364A12, 0x7FC73349, 0xEBA1FC77,
+0xECE0D20D, 0xB1DDDB9B, 0xEB6B492B, 0x0FC02BB6,
+0x56201D76, 0xED20F79E, 0xFC6034FB, 0x6A539F1D,
+0x520FECBF, 0x4E3AECF6, 0x76B01C74, 0xEFC421D4,
+0x82AC989A, 0x407A77CD, 0x6D287BFE, 0x26617425,
+0xEA2316C3, 0x8616554E, 0x9F4C4535, 0x88C0C6C1,
+0xEAC4F0F7, 0x32C7DD93, 0x41D9C37E, 0x2A9CBB2E,
+0x0591BAEF, 0x2BE43F21, 0x5E06EE4D, 0xDDDF5525,
+0xEC137DBE, 0xF0AA295C, 0xF2C9FDE2, 0x5DF9D693,
+0x10A6CAC0, 0xC6846D09, 0xF1DDABF3, 0xD56F8BBC,
+0xAA5DCE9D, 0x6F59004F, 0xB8A035BC, 0x61F47282,
+0xC89DAC9E, 0xFC7E5B3D, 0x4C5406DD, 0x54CFD147,
+0xBB44AB2A, 0x791269C0, 0x8CF66B4D, 0xD01A3190,
+0x636F45CA, 0xB32FC209, 0xCB8B9F49, 0xF46D74B9,
+0x5AFC9BD0, 0xC4C716C1, 0xF98C54F3, 0x36AFF013,
+0xB4D6D90B, 0x5F1299B6, 0xA3BFCFA4, 0xEA336AAD,
+0xCCD443DA, 0x74CA40B4, 0x31EF1614, 0x36D3FFEE,
+0x876AE252, 0xC8D62E9F, 0x6424F397, 0x1F730F2D,
+0xB20FDA53, 0xFCFEE60F, 0x676A61C3, 0x26C5E143,
+0xC201573E, 0x4A8C46BE, 0xEF87D0A9, 0xE07E80B4,
+0x34F20109, 0x8B936A70, 0x9F8E0305, 0xF3297CA0,
+0x4E7BF0E9, 0x0F374BB9, 0xCE78A01E, 0x5FE26DD8,
+0xA3826ACF, 0x321F69AB, 0x441AF14E, 0x8AC19CF7,
+0x4BFD1AD6, 0x5951ABD1, 0x098C17F0, 0xA9B75F76,
+0xA462551B, 0x6B703A12, 0xEDCB57B2, 0x8CD4C933,
+0xD338D3D8, 0xE343FC24, 0x9CDD52EB, 0x17A41942,
+0x63A8EF50, 0x215BB11A, 0xE1E25CB6, 0xB62C0A88,
+0xE58CDEC3, 0xC0E6389A, 0x2B7BEE55, 0xA3FCBD07,
+0x7CD451FE, 0xB06F6724, 0x5675A7EA, 0x141D52FC,
+0x05E86E9B, 0x53D75C3A, 0xE799AA2A, 0xE474384C,
+0x8C85E6E6, 0xA477A8D7, 0xA1E6AB0C, 0x9033E7CD,
+0x2F55D504, 0x4DAE81FB, 0xBD229A64, 0x862765C9,
+0x5B6A85F0, 0x95A39328, 0x38826CFB, 0xBF7DEBA4,
+0x42EFAB62, 0x2D0BBA60, 0xB06731AF, 0x16D4C4B0,
+0xCA4B9264, 0x3DF24AE2, 0xFED93848, 0x7CB33B08,
+0xAC9CAE9F, 0xA0F80B61, 0xA66CF713, 0x9364865F,
+0xDFA1E0B3, 0xFE6DF33F, 0x8039A612, 0x119F60BF,
+0xCEEDE309, 0xD28316A8, 0xCD61D2F5, 0x3CBEB015,
+0x85C0BF51, 0x6EDBBC15, 0x79F3D207, 0x485EE4FA,
+0xCEC302EA, 0x59D8B92D, 0x51C1FB36, 0xF4FE8B71,
+0x2DBD5718, 0x84024040, 0xFDD6590F, 0xA1CE9CC9,
+0xC4AEAB72, 0x0A2FE8BF, 0x28C33618, 0xBA4E15FB,
+0xA9C72819, 0xA3EE45D7, 0xD2DC52F1, 0x3FC84A2E,
+0x1C9DF73E, 0x632F9BDE, 0x7E9FBD20, 0x0D689B79,
+0x91E8D5C0, 0x6EE7952C, 0x905F192E, 0x2D79E712,
+0x8670A7A2, 0x1DBFC4D9, 0x64634429, 0xE636043B,
+0x643C6B0F, 0x50AF327B, 0x0E734D61, 0x2D7D6E46,
+0xB877DCD6, 0x7CCF4F1A, 0xDF4D8CF8, 0x0E7FA78E,
+0x0CBC4EC2, 0xAE9B4A22, 0x4F02D49C, 0x48F09C43,
+0x5031B1A0, 0xDCB8A1FC, 0x91C73599, 0xCF00A64D,
+0xDFCE561E, 0x8B18157D, 0xE1ED6A81, 0xCF94EF36,
+0xB412CE1A, 0x602E2076, 0x716B0F3F, 0xADEB32C0,
+0xD4E16094, 0xEC95D41F, 0x75858767, 0x438AD1A1,
+0xE61C5527, 0x0D71FBB2, 0x2A99D070, 0x5C018826,
+0xCCCC27FD, 0x053883D9, 0xF1D30EF5, 0x676AD38A,
+0xDF81AB28, 0x2257FB9D, 0x373313AE, 0x67E1FE8A,
+0xF4F66B02, 0xAFF8C7FA, 0x3B60D94D, 0xD44D0FE2,
+0x5FCDFE4B, 0xC63010B6, 0x06CFCCF4, 0x09D8DD85,
+0xAB79F2BE, 0xD5C0C498, 0x7364E4FD, 0xB295CEDF,
+0xDB89A068, 0x59A6A0C7, 0x0C823207, 0x7380FCFE,
+0x6E33C4B9, 0x0744E4F2, 0xF663BB33, 0x9EE512CE,
+0x870ED35B, 0xB4502654, 0x367CD4FD, 0x5D4238D9,
+0xEAB2B86E, 0x6E8ADDAA, 0xF080EDD6, 0x1DC90F46,
+0xB1FC9127, 0x63771392, 0x96729BF6, 0xD18E1413,
+0x5D85938D, 0xB8CED349, 0xF9B886C1, 0xCA486562,
+0xBAA9ED7A, 0x049718D8, 0x7CF8E67A, 0x1702843C,
+0x6DCDC34E, 0x93C51F83, 0x2415A4F3, 0xA8D77B3A,
+0x0FB823E8, 0x424F03C3, 0x9CAA503C, 0x7AA5433F,
+0x3BDD74FE, 0x99D3332E, 0x1E62231B, 0x90A4E595,
+0x7EDA974D, 0x43E2CD14, 0x27DB9D9F, 0x561F5CC6,
+0xA77EABA6, 0x97867B48, 0xAD6533CE, 0xEB726CF4,
+0x5857B217, 0x2D7DA10B, 0xD939C20E, 0x81F1F073,
+0xF42DEAF2, 0x3AD7780E, 0x88C77661, 0xD2E819B2,
+0xF872F581, 0x999F0C5A, 0x3887ABA4, 0x27F95B6D,
+0x991D9458, 0x9D1BB131, 0x6ECC5298, 0x9E9A7B26,
+0x6E65F271, 0xE90FA04C, 0x7B692AA0, 0x878943D5,
+0x924895E5, 0x041BC73A, 0x448E28B2, 0x61D22D1F,
+0xE7969773, 0xBC8E5980, 0x9A198852, 0xB94415C9,
+0xA02374BA, 0x340BD5F3, 0x27F2A0FF, 0x39BDB33F,
+0xCC042BCF, 0x83D6C135, 0x9C7A8D8E, 0x05823C23,
+0x2D7A3F91, 0xE792BCCA, 0xA2D82177, 0x73C82E7E,
+0xBEBC9613, 0x9F596CB0, 0x6E784AA7, 0x1B7BDA9F,
+0x846391F7, 0x852AD070, 0xF831E8CA, 0x16A78223,
+0xF68F5250, 0xE2554493, 0xD38F2AFB, 0x764BA7A8,
+0x3CAEFC55, 0x6E9B9037, 0xD87D486E, 0x7352AEA9,
+0x11987EE0, 0xDF7E84DA, 0x2838E736, 0xA8C7BAC2,
+0xF49E21EE, 0xFAD106E9, 0x7363AC6F, 0x5E9974CB,
+0xBA008BB0, 0xAF5DB3FC, 0x7AC3CFD7, 0x2D55EDC6,
+0x2C1C9AD7, 0x6A3AA494, 0x5F0E0A3A, 0x37422BFA,
+0x83B4D594, 0xB7ECCF66, 0x82FCCDD0, 0x8ECBFD79,
+0x664B9341, 0x02F178A2, 0x2095C8E0, 0xFC5F17B7,
+0x1810BA9B, 0x964E4CD1, 0xFBAED808, 0xDEE87796,
+0x63DE4F69, 0xC99275DD, 0x65242304, 0x7AB5C28B,
+0x01BB7A3B, 0xC85D7716, 0x32AFB9A3, 0x2ED2CBB1,
+0xB194218F, 0x21FE560D, 0xCB4503A5, 0x5CE0464D,
+0xC4AE9A3C, 0x061530CB, 0xEDA38E6B, 0x4029D3E6,
+0xB0C20336, 0xA37825C0, 0xC68F8B37, 0x9405AD3B,
+0x8B1A8F99, 0xA761DE8B, 0x683B3259, 0xA154C554,
+0x6BD835C9, 0x6DEAE35A, 0xBEAE6D49, 0x21D8B074,
+0x46C01B31, 0xBE9B3A16, 0x1D611EAA, 0x423AB74C,
+0x931F5AF5, 0xBB9E289A, 0xA4101132, 0x4A8BE0D7,
+0x3307E4B2, 0xDE78DB5E, 0x347EB5CE, 0x13EEE999,
+0x2C2D7955, 0xBA893EBA, 0x5DFC2EC1, 0xE7DD7A5F,
+0x5E1C64D8, 0x4552E447, 0x1837D8E4, 0x9711836B,
+0x3219F893, 0x04392C84, 0x3E94848C, 0x15E5F481,
+0x0EC58819, 0x7341D458, 0x4AE63711, 0x85C1FD1F,
+0x97B58BD7, 0xB0550EBE, 0xB9108743, 0x6F53B386,
+0x7A73F31B, 0xE07CF8B9, 0x61FF27C8, 0x06A9A8B4,
+0xEB0F2BB9, 0x46D275FB, 0xCF39B474, 0xC34F3B6D,
+0x52F2F119, 0xD87963BF, 0xC60BF16C, 0x7797D0AD,
+0x7EA4DBF0, 0xD21409C7, 0xF678A927, 0x638E67CD,
+0x93261AED, 0xEA9B25FE, 0x1EBCAFDC, 0x580CC829,
+0x58D1DA1A, 0x658881F8, 0xC48DB682, 0xD42E8CB4,
+0x1DF33D74, 0x31C04F68, 0x7D871E29, 0xAE11FD72,
+0xD7E8F8F6, 0x530D9D9C, 0x580A0715, 0x0F17B1A3,
+0xB863F42F, 0xA6A4DC08, 0x82773E76, 0x9354B309,
+0xE17D0770, 0x04E4DE5B, 0x712EA396, 0x49D37B55,
+0xAE4109BA, 0x03862DC9, 0x7BCF61D2, 0x43CA2017,
+0x23BDD50F, 0x74577459, 0x4E8F4E23, 0xBF924C1A,
+0xE4EC70CE, 0x37FBEC66, 0xA6DA8935, 0xE11F4090,
+0x5C8F9EE3, 0x19D167EC, 0x9EE4F2C5, 0x64A81E6C,
+0xB35642BB, 0x82083A01, 0x001CA1F6, 0xAA69C7E8,
+0x685F24D9, 0xE6868E31, 0x38ADD8F0, 0xA2FDD44E,
+0xEE0C491D, 0xC60B1E9A, 0xF7A89268, 0xFD784F35,
+0xC6B7335C, 0x75EFCEC1, 0xE2D9F7CF, 0xE1C364F8,
+0x7CC63B2C, 0xC179E2AD, 0x56C193A5, 0x5134FB69,
+0x35058BB5, 0x36F4BCD5, 0xDF4A08C2, 0x14AA2330,
+0x760C8CD8, 0x2C562394, 0x0BEB669B, 0x2301973A,
+0xAF5C4FF2, 0x1C770AAB, 0x25DD2087, 0x732AADC4,
+0x59054958, 0x59DDCBE4, 0x74CFC8A8, 0x7C015016,
+0x32A0276E, 0x8F1C2E93, 0x0CE91F71, 0x055C307A,
+0x435D967E, 0xF4C33704, 0x5BDF2AD7, 0x8855099C,
+0x307B2736, 0xBB6B19CB, 0x626349D3, 0x8F52ABFA,
+0x251A1ED6, 0xE0587BC0, 0x12831408, 0xDA83CABF,
+0xAB2C7DFD, 0x6BCF0271, 0x72058DF0, 0x17AFC1DD,
+0xFFC52C30, 0x551401E0, 0x9EED54DF, 0x14E951E4,
+0x14624B3F, 0x4C24650B, 0x5A65F86B, 0xE94F6143,
+0xDC7CE9CF, 0x94D5D8F3, 0x093B0A04, 0x22098D01,
+0xEDF09E7C, 0x165EDB0F, 0xD09CA774, 0xB96AA141,
+0xB5745978, 0x9D820434, 0x42B0E026, 0x96938A25,
+0x72E8634B, 0xBE36EC02, 0x42F3F74B, 0x358FA621,
+0xBD451484, 0xB43A75D1, 0xB0A57F91, 0x701A7C82,
+0x484B3F46, 0x047F78AD, 0x65F7371C, 0xEAC8A954,
+0xE59F6354, 0x3EEEFB4E, 0xF131954B, 0x1C00BAC2,
+0xE3897637, 0x5FEC83AB, 0x58CFA2C4, 0x1F4C0A6A,
+0x97956BC6, 0x63D11D7D, 0xB46179D0, 0x11039A75,
+0x1B50E088, 0x68E9476B, 0xAA68DB55, 0x8A4A051E,
+0xEFA0DDF5, 0x05A2A674, 0xFFE03E72, 0xC5A0295C,
+0x6FD4D834, 0x8E42BB94, 0xF3DFD88E, 0xBA691AD2,
+0x3458473E, 0x6269A348, 0x72962FB6, 0x86D5064B,
+0x8A153740, 0x54AC97D8, 0xED2CE057, 0x68200474,
+0xBBA8E19D, 0xBFDD08F3, 0xB0DF76D1, 0x62F29649,
+0x5AB77030, 0x1EE9A00E, 0x7DAB1C90, 0xAB608FFD,
+0x8506A853, 0x75B9339B, 0x1AE0CCBA, 0xFB60BB79,
+0x8650F92F, 0x4819E1F7, 0x0A7045A8, 0xB5BCE5F1,
+0x77A98B27, 0x03DE21E4, 0x3FE3F132, 0x106827EC,
+0xD4DC1469, 0xAAC82F9B, 0x1D5953A1, 0x8034B369,
+0xD4412B6F, 0x90FB9F25, 0x14279070, 0x6D98AF1C,
+0x3D286F37, 0x8324A732, 0x58123E4E, 0xEB051032,
+0xC15CD557, 0xEB82DE99, 0x6213434E, 0x39F0FC9C,
+0x5EBFE1C5, 0x8CEBF470, 0xFF7D8D8A, 0x740A6A3E,
+0x720D080C, 0xB73B74FA, 0x5173F96E, 0x9FC01794,
+0xDABF1C81, 0xCA813295, 0xBEA2DB8D, 0x4C7E0CE4,
+0x8051BA67, 0xE63399E2, 0x83A15EE4, 0x47F4A718,
+0xD8246E6A, 0x0B4F87BE, 0x031648B8, 0x99E3E3E6,
+0x4ABCC64F, 0x52768181, 0xE708372B, 0x2D0B1D2C,
+0x4DF52402, 0x389BE9F6, 0xDE2F3232, 0x5D43D74E,
+0xD37BB898, 0xE7272645, 0x9B5432DA, 0x9D7A9473,
+0xA69628A5, 0x583555A7, 0x255B08BD, 0xAD68EAE3,
+0x1A79982D, 0xACE09726, 0x15E576AD, 0x260EB406,
+0xA7440B46, 0x66B6D317, 0xBE6ECA3B, 0x3ADEA1C1,
+0xD80399C3, 0x0EF198D0, 0xFAEE2010, 0xEF2E8E56,
+0x5B6CC402, 0x3FD27BE2, 0x970AAB5F, 0x618C17C6,
+0x7F5022FB, 0x552FC1FA, 0x5DD82984, 0x09769539,
+0x98812D1F, 0xBD8B2539, 0xD78AD9A6, 0x1CE41D07,
+0x272A0AB7, 0x5CB7E101, 0x6F42D56A, 0x001D930E,
+0x3C17C305, 0x30AAE354, 0x2A4AABE0, 0x922BCB94,
+0x73F34C1C, 0xE07E1501, 0xCB55A3E1, 0x0CDC3669,
+0xD9C07DE7, 0x2DAB82BF, 0x963EACAA, 0x9B05E0F1,
+0xE2DA0EFA, 0x0613BFE5, 0xDFB605E9, 0x5DCCA8FD,
+0x6D433873, 0x81A9B4C5, 0xD1D1CB14, 0x9B6A9906,
+0xC104767C, 0x30101D37, 0x186FBB79, 0x8F95D488,
+0xA3094F43, 0x7F17C981, 0xFD92B3FE, 0xADAB3AB5,
+0x20D1406C, 0x9462C8E7, 0x5D64819D, 0xB3E85196,
+0x67B854FE, 0x7D039FC6, 0xAD98A85E, 0xF672E041,
+0x30FA19A9, 0x4A276EB8, 0xB7041D2E, 0x57BB21E2,
+0x4E251667, 0x15C5401E, 0xDAB59431, 0xD6C6FD1F,
+0x1726EB70, 0x900F4E84, 0xD327DE33, 0x7A0AE04B,
+0x76B1174E, 0xFD547B94, 0x370832DC, 0xDDE65CDD,
+0x74672C02, 0x164703FE, 0x34CAD31F, 0x3E692DED,
+0x4BC38FA5, 0x143F99E5, 0x61BB640E, 0xB957BC8D,
+0xC9DD9E35, 0x2B5CB310, 0xADD6EAD0, 0x91981D46,
+0xED803D57, 0x61D7737C, 0x92D3AC3E, 0x36A034CB,
+0xE1395DC5, 0x5F2070F8, 0xC5EE9F8A, 0x70546B88,
+0xC9EA230C, 0x58DC3073, 0x57CBBEB7, 0xA0B78CFE,
+0x0B3FE75B, 0x07ADACCD, 0xC292C338, 0xD70CD7E5,
+0x729D8F4E, 0x218FA041, 0x10EC1199, 0xAC1EC51D,
+0x5DECC8D1, 0xBA36230A, 0xBC41F5A5, 0x75864896,
+0xB4403D4A, 0xFEEE8F44, 0x8D94A256, 0x62BA0115,
+0x3A570C61, 0x9221C583, 0xD2981A6B, 0xFD8AAF5A,
+0x2A102D59, 0x64083BDD, 0xBD1AADE6, 0x7E6D1E99,
+0x20568A6D, 0x8DFA704B, 0x87D27122, 0x2EFDAB7D,
+0xF3AF9D39, 0xD8DED0B2, 0x2D4B34B9, 0x12F3E32C,
+0xA6BCBE65, 0x680029A1, 0x094B07B3, 0xDA5918ED,
+0xF7D0A86D, 0x1A7E18C8, 0x9285A97F, 0x2040282C,
+0x5B133531, 0xA48237AC, 0x3557BC1B, 0x7E6ED77B,
+0x436234C7, 0x9B2094DE, 0x5D967593, 0x8867D1C4,
+0x88EC3948, 0xE7F84AD4, 0x1871B3E6, 0xE8E992C6,
+0xA16DC2F8, 0x0DFDF590, 0x9B56238D, 0x329017F5,
+0xBF9BD409, 0x68BD9B1C, 0x4036C4FF, 0x3BF6D93C,
+0xAE100602, 0x90B43508, 0xA85B4013, 0x2C66EA54,
+0x227D32D7, 0x0BA526D1, 0x075213B8, 0x1A3DED07,
+0xD458DFFD, 0xDC8ACD43, 0xAC7809AB, 0x2D25408A,
+0xD8F0C887, 0xAD8CD30D, 0x4054F61E, 0xA9F0CCA3,
+0xBFEBD31D, 0x6D2BAB1E, 0xF8E42D8B, 0x6C94A4E4,
+0x1158D2A3, 0x93F44EFE, 0x8AD05A25, 0x8C229D32,
+0xB213D76E, 0xDFE63822, 0x561986EC, 0x806CA082,
+0x6DB3BF8D, 0x1E850D30, 0x8F7A44C0, 0x75BB3328,
+0x86C7BE12, 0xDE5C44BD, 0xDF4D048E, 0x968712C3,
+0xB1B41CF8, 0xCC194FE9, 0xDA2E1A8D, 0x72A08662,
+0x5ABA2536, 0x223E2013, 0xA5A923A5, 0x7565B5DD,
+0xBCA0A2B0, 0x0C29864B, 0xAAD8CB87, 0xE4C7E559,
+0x77E19E51, 0x194E54ED, 0x54DD1B54, 0x0FAD37A7,
+0x0EF6B0E3, 0x0E3A2FC8, 0xA0063995, 0xE17AE20E,
+0xDC11B7F8, 0x85F1A76D, 0xD97858D4, 0xB763E49C,
+0xB5BE7EC4, 0x3CE924C4, 0x4246019D, 0xD33DBB27,
+0x737863A7, 0x32C26BDD, 0x714897A3, 0x36091018,
+0xF26BC990, 0xDDB640B0, 0x448F5B12, 0xD7A5EB4B,
+0x5614EEA4, 0xCA4912FB, 0x011F9D6C, 0xA4FC90AB,
+0x9FB4982D, 0x20AD146F, 0x4B7AB74E, 0x107A9411,
+0x71DBA90A, 0xD510E3D2, 0x248D0D35, 0xB666229E,
+0x61EE1EEA, 0x702031B5, 0x36992A7B, 0xC90C08CB,
+0x6478995A, 0xE6C2BA7A, 0x8A9179AC, 0xC8EE2956,
+0x27B042C8, 0x48DB81D9, 0xAA39F2CB, 0x5E4D5F3C,
+0x24FFD6B9, 0x5B562C2F, 0x00FD33B6, 0x435F5F52,
+0xF392FFC1, 0x0E927C40, 0x5508CBAB, 0x976AA567,
+0xA13E7C52, 0x532109E9, 0x16B9021F, 0x60C615A1,
+0x1D23C258, 0xFD783147, 0x63600FB1, 0xAAA245F0,
+0x9B3DC1E1, 0x7B270D0D, 0x5B1632CE, 0x8B871F7F,
+0xC535EFF8, 0x73109C6A, 0xEB83D02D, 0xF7AE76FB,
+0x2E39E502, 0xA4128216, 0xF90D57E5, 0xFF0C465E,
+0x02008029, 0xE5CBBA1F, 0x4280FA3C, 0xCDBD75C8,
+0xCB4AF342, 0x17695A4E, 0xAA6162B5, 0x8660A679,
+0xD1A8701C, 0x47694CA7, 0xDA8D43FD, 0x44A4BC1B,
+0xAB34B9AA, 0xE55563DD, 0x08D4142B, 0x81197AC8,
+0x997B1DC2, 0x2E7CC50A, 0x7A326A21, 0xA76419DB,
+0xEA8B5428, 0x65729140, 0x051DAF66, 0x8871BCA9,
+0xA175E5BF, 0x60310C98, 0xB7DE8929, 0x35E2459E,
+0x08EB4547, 0x904D7B2B, 0x29382CC4, 0xCEC8664E,
+0x1E8C9C2C, 0x3B942134, 0x9CEC5D55, 0xDA548376,
+0x2E4EFD61, 0x26F65F09, 0x5A3DD7CA, 0x2FD4E58D,
+0x6B71B8C2, 0x13189115, 0x2B5542BA, 0x1CE85C2C,
+0x5B9FE09D, 0x68704BFE, 0xB15313B9, 0x3EF2729E,
+0x583ECC31, 0xA3DED8CA, 0xFCD27C3D, 0x904DAB39,
+0xFE1069A4, 0xE99A57BA, 0x112EB80C, 0xE1483C74,
+0x8A27B0D7, 0xA58F7325, 0x7CD050A1, 0x626D4F3E,
+0x51643657, 0xA967FC59, 0x5BACBC0B, 0x2CF3E459,
+0x7D8988D9, 0x53913DF8, 0x2381A6FC, 0x64D6D441,
+0x48AE9101, 0x185D9539, 0x1B044AEC, 0xB5ABCEDD,
+0xFA8ECA52, 0x8CCDD142, 0x96FD4442, 0xD865FEDF,
+0xCE4EE2FA, 0xA5160AE9, 0xC91B2B3A, 0xF993F45F,
+0x1509132C, 0x920ECC5F, 0xD813DDC1, 0x834B68E4,
+0xD5E876A0, 0x61DE0E41, 0x4C143913, 0xC7293985,
+0x17E226E7, 0x38830927, 0xDC604DF2, 0x799D1430,
+0x846585AB, 0xE5D21E38, 0x6381D136, 0x1B60633B,
+0x23B7AE14, 0x554E53CC, 0x5807A210, 0x30560866,
+0x12F79E62, 0xE27B5D45, 0x3889C1E5, 0x47F845FF,
+0xFFD9DE98, 0xB10E09D2, 0x4A184A72, 0x083D2971,
+0x8AB7478D, 0x92380377, 0x57A724EC, 0xBBBD5CA6,
+0xE2FB9D32, 0xAB6ADFC6, 0x3916DED4, 0x4E19438F,
+0xE21E15CF, 0x6AF4BCC9, 0x8D08924A, 0x1662BAA9,
+0x3064AD27, 0xB86D7EE4, 0x88624C62, 0x1A0BF3E7,
+0xF3E4A287, 0x6787F006, 0x01375D4B, 0x998BB38F,
+0x6D669A29, 0xD760B093, 0xC4768853, 0xF041100F,
+0x35DE10DD, 0xE06C8BB8, 0x2C79A902, 0x60600DAD,
+0x6E11CF5C, 0x18778777, 0x7CCE406C, 0xE54AF2EA,
+0x7472C475, 0x73DBEE7E, 0xE533DC40, 0xB07407DD,
+0xF6ACA8D3, 0xE71BD7D1, 0x4BD3514D, 0xC5C362CA,
+0x0690E5A1, 0x0FFDC8D8, 0x58188645, 0x8636413C,
+0x3412A033, 0xAF4FC340, 0xA5DFEAB8, 0xB87272E3,
+0xA4A9219F, 0x29696E90, 0x35D2F627, 0x8794DBD7,
+0x5D2D87F8, 0xFA73559D, 0x7D22F440, 0xF50197E9,
+0xEB74B829, 0x8F9649CF, 0x16F47D30, 0x5C7D9870,
+0x36FF6C0B, 0x313A92ED, 0x303B3654, 0xE3E33CCA,
+0x02C26ECC, 0x26949920, 0x4445DF20, 0x01FDBC98,
+0x49138C6F, 0x1B5555E2, 0x122B45D2, 0x4B2E0202,
+0x7B6014D4, 0xFAE0CD09, 0x77E165A0, 0xFBE76980,
+0xF5808BD3, 0xFD110E5E, 0x97450E11, 0x297F9B1F,
+0x607A2C41, 0xE384DFC9, 0x25D9A8DC, 0xF919D955,
+0x5E025993, 0xCC318847, 0x9717D2D5, 0x48F0DD1F,
+0x6CC4A8EB, 0x9BD0F4E1, 0x506F2A93, 0x18B8748E,
+0x16FFBA48, 0x552E4955, 0xB963F64F, 0xA1A34AC8,
+0x62E95CC7, 0x4D87EA89, 0x21E8C031, 0xC1F0ED07,
+0x28B7BB22, 0x0B838D04, 0x6361B440, 0xA653521C,
+0x92DA3F78, 0x4241CFED, 0xFAFCBD41, 0x3EFAB6BC,
+0x25F30607, 0x41BB70DA, 0x9FF3440A, 0x2502039E,
+0x3813EC82, 0xC6A4FD6B, 0xF8537C8C, 0x098ED49F,
+0xE0A0BD6E, 0x6BA2F2B3, 0xC35C9D9D, 0x1256E66A,
+0x790B2490, 0xD5C69889, 0x39E712FE, 0xCF73DE0B,
+0x41B3B614, 0x745ABD73, 0x654C79D8, 0x5B15923D,
+0x8C15F218, 0x585CCCF0, 0x624F7B44, 0x76BDDFDB,
+0x96F26B52, 0xE13058A1, 0x086C950E, 0x29519DEA,
+0xA42CFE04, 0x0D7A190B, 0xD0678C6A, 0xABB78679,
+0xBA48A2E4, 0x5F3DA10A, 0x11F04183, 0xAC720A3F,
+0x6A807781, 0x6F146BFB, 0xE8A67934, 0x54578834,
+0xAA60C8F0, 0x2061A1E6, 0x9E87799B, 0x68D91F86,
+0x8974F540, 0xB1C3F101, 0x99C21E56, 0xB57BA73F,
+0x8B2DAA3E, 0xF1E2D24E, 0x48F7D4EE, 0x7039FDB3,
+0xC666EEDC, 0x251F972E, 0x4D53F6BF, 0x6CC73EE7,
+0xCB07F7B9, 0x69ECB8CA, 0x363FD80C, 0x3B587AB3,
+0x738C1E5C, 0x5C9C1D92, 0xE7B52396, 0xEDE6324B,
+0xFE5B5045, 0xC90D8B3E, 0x371A0128, 0xF2C8DCF8,
+0x5B648CB5, 0x12F8E8FF, 0x5FE4BA71, 0xB925CFBE,
+0x7416E14F, 0x76489FFE, 0x1F4DE367, 0xA400F039,
+0x66390E83, 0x1AE79CEC, 0xDB573E98, 0xB6021F29,
+0xD01615E5, 0x02A2281F, 0xE85019C1, 0x027BB41F,
+0x8D9177C3, 0x79026E78, 0xF158B623, 0xBEFF5858,
+0x7B63518E, 0x8F42C08C, 0xB388227D, 0x940D607A,
+0xA4C79541, 0x9800CC91, 0xA356B535, 0x285BABB9,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0xE411E520, 0xA0024528, 0x442B4428, 0x96070009,
+0x46106246, 0x8FFB2522, 0xD4027504, 0x0009AFF5,
+0x00000FB3, 0x00200004, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x16D49357,
+0x0009000B, 0x4F222FE6, 0xDE947FFC, 0xE114D594,
+0x1E13D494, 0x67521E4C, 0xD494D693, 0x37402769,
+0x62528F06, 0x7201D692, 0x60602522, 0x2600C93F,
+0xD7906152, 0x2512611D, 0x264B6652, 0x2562470B,
+0x0009B00D, 0xE60095AC, 0xC84060E2, 0x2F028F03,
+0x8FF93652, 0x7F047601, 0xA05A4F26, 0x4F226EF6,
+0x410BD185, 0xD4850009, 0x0009440B, 0x450BD584,
+0xD7840009, 0xD284E1FF, 0x2712611D, 0xD4835029,
+0xE1FFCB01, 0x1209E501, 0x12112212, 0xE7202452,
+0x4718D57F, 0x2572D27F, 0xD17FE700, 0xD680D47F,
+0xE2012270, 0x24702172, 0xD67E2620, 0x2641E4FF,
+0xD57DE600, 0x666DE104, 0x76016063, 0x4000626D,
+0x8FF83212, 0xD5790545, 0x2520E201, 0xD279D778,
+0x2710E100, 0xE5802212, 0x655C6613, 0x666DD476,
+0x76046763, 0x374C626D, 0x8FF83253, 0xD4732712,
+0xD573E101, 0xD6732410, 0x2542E400, 0xE03AE501,
+0xD272D771, 0xE0390654, 0x27110654, 0x000B4F26,
+0x7FC82211, 0xD76FD16E, 0xDC70DB6F, 0xD271DE70,
+0xD572D471, 0x1F12D672, 0x1F76710C, 0x1FB877FC,
+0x1FEA1FC9, 0x72041F2B, 0xDE6FDC6E, 0x1F13EB10,
+0x1F511F44, 0x1F771F65, 0xD86C1F2C, 0xDD6DD96C,
+0xD26DEA00, 0x89003A22, 0xD1587A01, 0x88016010,
+0x56F98B03, 0x4218E201, 0xD1682622, 0x0009410B,
+0x440BD467, 0xD5670009, 0x0009450B, 0x6010D14C,
+0x8B108801, 0xE650D14B, 0x46186212, 0x8B083266,
+0x56FAD147, 0x2120E200, 0xCB016062, 0x2602A003,
+0x72012710, 0x60822122, 0x89098801, 0xE2C8D15A,
+0x622C6612, 0x89033626, 0x6010D158, 0x8BC88801,
+0x51F76792, 0x217252F6, 0xD6555191, 0x55FB2212,
+0x52FC6462, 0x55612542, 0x2252E400, 0x61436643,
+0x05DE6013, 0x36CC4608, 0x02DE2652, 0xC9036021,
+0x8B028801, 0x720162E2, 0x74012E22, 0x36B3664C,
+0x71048FEE, 0x66C2D147, 0x45286512, 0x265B4518,
+0x60822C62, 0x89018801, 0x0009A168, 0x6272D742,
+0x8B132228, 0xD726D541, 0x6552D441, 0x51436672,
+0x316C365C, 0x27622668, 0x14138D05, 0x6262D63D,
+0xB1A57201, 0xD61E2622, 0x2622E200, 0x52916692,
+0x8B013620, 0x0009A144, 0x6061A06E, 0x001C001C,
+0x001D4020, 0x0000B38E, 0xFFFF0000, 0x12340000,
+0x001E1015, 0x00201278, 0x002018A0, 0x00201922,
+0x0020128C, 0x001C3510, 0x001C3624, 0x001E212C,
+0x0020397C, 0x00203514, 0x00203984, 0x00203990,
+0x0020399C, 0x002039F8, 0x002039FC, 0x002039A4,
+0x002039A5, 0x002039A8, 0x00117700, 0x00203A12,
+0x00203578, 0x001142D8, 0x00203A14, 0x00203A16,
+0x001C3D30, 0x00117718, 0x001C3D00, 0x001C1000,
+0x001C36F8, 0x00117734, 0x001C3684, 0x00117710,
+0x001C3520, 0x00117600, 0x00117740, 0x001C1028,
+0x0020358C, 0x002039AC, 0x7FFFFFFF, 0x00201734,
+0x002032BE, 0x002022E8, 0x00203DC0, 0x002039FA,
+0x00203584, 0x002039EC, 0x001C3D2C, 0x001C36B0,
+0x0020351C, 0x0011775C, 0x8801C90F, 0xA0CF8901,
+0xD17C0009, 0x36206212, 0xD47B8904, 0x2421E200,
+0x2162A0CC, 0x6211D179, 0x89012228, 0x0009A0C3,
+0xE202D775, 0x75016571, 0x3123615D, 0x27518D02,
+0x0009A0BC, 0xD27255F2, 0x62226052, 0x40094019,
+0xC90F4009, 0x8F19880A, 0x52F31F2D, 0x40196022,
+0x40094009, 0x8808C90F, 0xA0A78901, 0x60630009,
+0x51F255F8, 0xE701CB01, 0x2502D263, 0xE1002172,
+0x2211D564, 0x74016452, 0x2542A098, 0x8B3F8805,
+0x602252F3, 0x40094019, 0xC90F4009, 0x8B168802,
+0xE5FFD45D, 0x655D6742, 0x8B102758, 0x6272D75B,
+0x8B0C3260, 0x55F257F8, 0x2762E101, 0xD5522512,
+0xD757E400, 0x62722541, 0xA0777201, 0x52F32722,
+0x40196022, 0x40094009, 0x8805C90F, 0x31B38B6E,
+0xD5508B6C, 0x615257F4, 0x7101E240, 0x64722512,
+0x1F4DD14D, 0x42182419, 0x8B033420, 0x6262D64B,
+0x26227201, 0xE200D640, 0x2621B0AA, 0x0009A056,
+0x3123E220, 0x88038B52, 0x52F38B1E, 0x40196022,
+0x40094009, 0x8803C90F, 0xD7418B16, 0x647251F4,
+0x7401D23D, 0x65122742, 0x1F5DE640, 0x46182529,
+0x8B033560, 0x6262D63B, 0x26227201, 0xE200D62E,
+0x2621B086, 0x0009A010, 0xD738D137, 0xD22A6412,
+0xE5007401, 0x21423A76, 0x22518F06, 0xEA00D634,
+0x72016262, 0x2622B074, 0x2FB2D532, 0x95406652,
+0xD4305BF1, 0x36205241, 0x60618910, 0x8B01C803,
+0x2B22E201, 0x8FF54510, 0x57F15664, 0x6272E1F0,
+0x41284118, 0x2722221B, 0x6BF2A008, 0x6BF2A006,
+0xE200D615, 0xD1152621, 0x2121E200, 0xE20256F5,
+0x42186662, 0x26284228, 0x1F6D8D0C, 0xD61FD11E,
+0x460B6511, 0x2008645D, 0x57F58904, 0x6272D11C,
+0x27222219, 0xD11BE201, 0x66122822, 0x8B012668,
+0x0009AE17, 0x450BD518, 0xD1180009, 0xAE10E600,
+0x07D12160, 0x00203A0C, 0x00203A10, 0x00203A18,
+0x001C3DC0, 0x0011772C, 0x001C3B88, 0x002039F4,
+0x0011773C, 0x00117744, 0x0000F000, 0x00117764,
+0x00117748, 0x00117768, 0x0011776C, 0x01FFFFFF,
+0x0011774C, 0x00203584, 0x001142D8, 0x00114774,
+0xFDFFFFFF, 0x00203DC0, 0x0020246C, 0x002039FA,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xD11F7FF4, 0x6212DE1F, 0x67E25411, 0xD41E1F41,
+0x1F722F22, 0x6743D51D, 0x7794D21D, 0x5A425841,
+0x6C726942, 0x6D225B16, 0xE6006052, 0x2502CB20,
+0x7601E540, 0x3253626D, 0x62F28BFB, 0x212255F1,
+0x55F21151, 0x2E52D613, 0x14A21481, 0xD4122492,
+0x11B627C2, 0x674226D2, 0xD911DA10, 0x2A72E801,
+0x1A8C490B, 0x4218E201, 0x7F0C1A2C, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0x000068F6,
+0x001C3B9C, 0x001C3D98, 0x001C3700, 0x001C3500,
+0x001C5960, 0x001C8960, 0x0020358C, 0x001C3D00,
+0x00201610, 0x2F962F86, 0x2FC62FA6, 0x2FE62FD6,
+0x4F124F22, 0x7F884F02, 0xE018DEB2, 0xD4B261E0,
+0x61E30F14, 0x62107101, 0x440BE01C, 0x20080F24,
+0x8F126D03, 0xD4AD1F08, 0x6740DDAD, 0x657CD4AD,
+0x470BD7AD, 0xD2AD0009, 0x621C6120, 0x46086623,
+0x36284608, 0x3D6C4608, 0xE01C1FD8, 0xE58004FC,
+0x604C66E2, 0x3050655C, 0x2D628F17, 0x01FCE018,
+0xDEA3E500, 0x641CA008, 0x6753655D, 0x607037EC,
+0x31DC6153, 0x80147501, 0x3243625D, 0xD49D8BF4,
+0xE200D59D, 0xA27F2421, 0x20082521, 0xE0188B13,
+0xE50001FC, 0xA009DE96, 0x655D641C, 0x32EC6253,
+0x62536722, 0x32DC6672, 0x75041261, 0x3243625D,
+0xA2698BF3, 0x88012D10, 0xE0188B16, 0xE40001FC,
+0x671C2D40, 0x624DDE8A, 0x8B013273, 0x0009A25C,
+0x6DE3644D, 0x7D046243, 0x32EC6643, 0x652236DC,
+0x74086162, 0x2512AFEF, 0x8B198804, 0x01FCE018,
+0x2D70E700, 0x1FD56D1C, 0x627DDE7D, 0x8B0132D3,
+0x0009A242, 0x6173677D, 0x31EC65E3, 0x75046412,
+0x365C6673, 0x61426262, 0x21297708, 0x2412AFED,
+0x8B198805, 0x01FCE018, 0x2D70E700, 0x1FD46D1C,
+0x627DDE6F, 0x8B0132D3, 0x0009A226, 0x6173677D,
+0x31EC65E3, 0x75046412, 0x365C6673, 0x61426262,
+0x212B7708, 0x2412AFED, 0x8B598831, 0x61E6DE67,
+0x61E31F19, 0x64E27104, 0x1F4A6216, 0x1F2B6416,
+0x75E46513, 0x66536712, 0x1F4C7604, 0x64521F7D,
+0xD75F6E66, 0x27E0D25F, 0xDE5F6062, 0xC9013245,
+0x65622E00, 0x4609060A, 0x4609D15C, 0x46094509,
+0x21501F4E, 0xB2B0646D, 0x620D1F6F, 0x8B012228,
+0x0009A1EA, 0xD756DE55, 0x661C61E0, 0x6410D150,
+0x470B654C, 0x7FFC54FF, 0x2FE25EFE, 0x51FE7FFC,
+0x2F12E040, 0x55FBD14F, 0x57FD56FC, 0x04FE410B,
+0xD24D7F08, 0xE11C640D, 0x1D412D10, 0xD44B6522,
+0x67421D52, 0x1D73DE4A, 0xD24A65E2, 0x67221D54,
+0x1D75D249, 0xD2496E22, 0x66221DE6, 0x1D67A1BC,
+0x89018830, 0x0009A08E, 0xE340D538, 0x33FC6156,
+0x23126456, 0x71046153, 0x67521341, 0x13726416,
+0x7EE46E13, 0x65E66212, 0x66E3D731, 0x13246EE2,
+0x760427E0, 0x6062D22F, 0x3255DE2F, 0x2E00C901,
+0x060A6E62, 0xD12D4609, 0x4E094609, 0x13434609,
+0x646D21E0, 0xB2501F5E, 0x620D1F6F, 0x8B012228,
+0x0009A18A, 0xDE25D522, 0x61E06450, 0xD724654C,
+0x470B54FF, 0x7FFC661C, 0x06FEE054, 0x7FFC2F62,
+0xEE4001FE, 0x2F123EFC, 0x55E2D125, 0x57E456E3,
+0x64E2410B, 0xD21C7F08, 0xE11C640D, 0x1D412D10,
+0xD61A6522, 0x67621D52, 0x1D73DE19, 0xD2196EE2,
+0x62221DE4, 0xD2181D25, 0x1D266222, 0x6222D217,
+0x1D27A15A, 0x00117800, 0x00202A18, 0x00203996,
+0x002035BC, 0x00203A7C, 0x002018D0, 0x00203995,
+0x00117804, 0x00203A14, 0x00203A16, 0x00117810,
+0x00203991, 0x10624DD3, 0x00203992, 0x00203993,
+0x00114AA4, 0x00200F68, 0x001C5864, 0x001C6864,
+0x001C7864, 0x001C59BC, 0x001C69BC, 0x001C79BC,
+0x00200FC0, 0x8B048833, 0x470BD7A2, 0xA123EE00,
+0x88282DE0, 0xA0D38901, 0xDE9F0009, 0x62E1E143,
+0x3216E054, 0x0FE68F02, 0x2E21E240, 0x622D62E1,
+0x8B013217, 0x0009A0BC, 0xE50185E1, 0x8B013056,
+0x0009A0B6, 0x2D10E101, 0x64E1B111, 0x06FEE054,
+0x6261E143, 0x3517652D, 0xE6408945, 0x8B0C3563,
+0xE058E41A, 0xE5000F45, 0x72C0E05C, 0x60230F55,
+0x6703C907, 0xA014E060, 0x66530F75, 0x46214621,
+0x46214621, 0x45214621, 0xE0587618, 0x0F654521,
+0xE0034521, 0xE05C2509, 0xE0070F55, 0xE0602209,
+0xE8540F25, 0x858238FC, 0x640D65F3, 0x1844B170,
+0xDD7A8584, 0x85866C0D, 0x610D4C08, 0x410860C3,
+0xE00F0EFE, 0x18154D0B, 0x2E296207, 0x668260C3,
+0x85620FE6, 0x4D0B5185, 0x2E0B600D, 0x548460C3,
+0xB13C0FE6, 0xE05465F3, 0xE5400EFE, 0xE06C62E1,
+0x3653662D, 0x0F668D41, 0xC9036023, 0x40004008,
+0x61036403, 0xD965E070, 0x0F46E5FF, 0xE074655C,
+0x60530F96, 0x6263490B, 0x42214221, 0x42214221,
+0x42006723, 0x4200327C, 0x6C074621, 0x4621E054,
+0x606309FE, 0x4008C903, 0x790630FC, 0x6A036D2D,
+0x65F3E800, 0x64D3B124, 0xE0706EA2, 0x2AE22EC9,
+0x01FE6694, 0x666CE074, 0x470B07FE, 0x2E0B6063,
+0x65F32AE2, 0xB0FA64D3, 0x628D7801, 0x32E3EE06,
+0x7D018FE7, 0x0EFEE054, 0xE05462E1, 0x420006FE,
+0x760C8561, 0x701B302C, 0xE4006103, 0xE70465F3,
+0x68667401, 0x3973694D, 0x8FF92582, 0x65F37504,
+0x641DB0DD, 0x0EFEE054, 0x64E1B09C, 0x0009A054,
+0xD43B56F8, 0xEA01D23B, 0x26A0420B, 0x0009A04C,
+0x06FCE01C, 0x8829606C, 0x5CF88B08, 0xE200D636,
+0x52612C20, 0x642DB04B, 0x0009A03E, 0x666CE681,
+0x8B043060, 0x420BD231, 0xA03554F8, 0xE6820009,
+0x3060666C, 0xD22E8B04, 0x54F8420B, 0x0009A02C,
+0x666CE683, 0x8B0A3060, 0xDA2755F8, 0x2590E900,
+0xD82855A1, 0x2852D628, 0xA01D52A2, 0xE6922620,
+0x3060666C, 0xD2208B08, 0x5C21D824, 0x6CCC52F8,
+0x28C1E600, 0x2260A010, 0x666CE693, 0x8B063060,
+0xD61F59F8, 0xE201EA00, 0xA00529A0, 0xD6162621,
+0xD21DD41C, 0x6562420B, 0x4F067F78, 0x4F264F16,
+0x6DF66EF6, 0x6AF66CF6, 0x000B69F6, 0x4F2268F6,
+0xE240614D, 0x89323123, 0x3127E21F, 0x8B27D713,
+0xD406614D, 0xE00171E0, 0x5671440B, 0x26596507,
+0x1761A025, 0x00200FBC, 0x00117804, 0x00203470,
+0x00203A9C, 0x002018C0, 0x00117800, 0x00115F00,
+0x00116058, 0x0020397C, 0x00203990, 0x00203A1A,
+0x00203A16, 0x00203AB4, 0x002018D0, 0x001C3704,
+0xE001D490, 0x6672440B, 0x26596507, 0x4F262762,
+0x0009000B, 0x614D4F22, 0x3123E240, 0xE21F8912,
+0xD7893127, 0x614D8B08, 0x5671D286, 0x420B71E0,
+0x260BE001, 0x1761A006, 0x6672D282, 0xE001420B,
+0x2762260B, 0x000B4F26, 0xE6400009, 0x46284618,
+0x6252D57E, 0x89FC2268, 0x0009000B, 0x4618E680,
+0xD57A4628, 0x22686252, 0x000B89FC, 0xA0010009,
+0x7201E200, 0x8BFC3242, 0x0009000B, 0x4618E680,
+0xD5734628, 0x22686252, 0x000B8BFC, 0x2FE60009,
+0x7FFC4F22, 0xBFF16E53, 0x61E22F42, 0xE280D66D,
+0x54E11615, 0x16464218, 0x422855E2, 0x57E31657,
+0x16786EF2, 0x26E22E2B, 0x4F267F04, 0x6EF6AFCE,
+0x2FD62FC6, 0x4F222FE6, 0x6C53DD62, 0x6E43BFD6,
+0x2DE2BFBB, 0x0009BFD2, 0x2C1251D5, 0x1C4154D6,
+0x1C5255D7, 0x1C6356D8, 0x6EF64F26, 0x000B6DF6,
+0x61636CF6, 0xA004E600, 0x62564109, 0x24227601,
+0x36127404, 0x000B8BF9, 0xD6530009, 0x8562E500,
+0xA00B674D, 0x655D610D, 0x40006053, 0x305CD44F,
+0x024D4008, 0x3270622D, 0x75018905, 0x3213625D,
+0x000B8BF1, 0x000BE000, 0x2FE6E001, 0x54416743,
+0x4E08EE7F, 0x4E28D246, 0x25E96543, 0x60436E21,
+0x9E7562ED, 0x4529C903, 0xE60032E3, 0x8D456103,
+0x21184509, 0xD23F8B05, 0x002C6053, 0xA08AC93F,
+0x60136603, 0x8B268801, 0x880C6053, 0xD53A8B04,
+0xC93F8453, 0x6603A07F, 0x8B048808, 0x84E2DE36,
+0xA078C93F, 0x880D6603, 0x8B03D633, 0xC93F8461,
+0x6603A071, 0x88096260, 0x622C8F09, 0xE014DE2C,
+0x655C05EC, 0x60233258, 0xA064C93F, 0x60236603,
+0xA060C93F, 0x88026603, 0xE0078B5D, 0x60432509,
+0x8905C810, 0x6053D225, 0xC93F002C, 0x6603A053,
+0x6053DE23, 0xC93F00EC, 0x6603A04D, 0x88016013,
+0x60538B19, 0x8B04880C, 0x8423D21E, 0xA042C93F,
+0x88086603, 0xD51B8B04, 0xC93F8452, 0x6603A03B,
+0xD618880D, 0x84618B03, 0xA034C93F, 0x60606603,
+0xA030C93F, 0x88026603, 0xE0078B2D, 0x60432509,
+0x8923C810, 0x6053DE10, 0xC93F00EC, 0x6603A023,
+0x00000BB8, 0x00203470, 0x001C3704, 0x001C373C,
+0x001C3700, 0x001C370C, 0x00114000, 0x00114008,
+0x001142D8, 0x001142E4, 0x001142E8, 0x001142F5,
+0x001142ED, 0x001142FD, 0x00114309, 0x6053D209,
+0xC93F002C, 0x60136603, 0x8B038802, 0xC8106043,
+0x76028900, 0xC93F6063, 0x40004018, 0x1741240B,
+0x6EF6000B, 0x00114301, 0x0009A16E, 0x2FE62FD6,
+0xDD944F22, 0xA0049EB2, 0xD4930009, 0x420BD293,
+0x62D265D2, 0x8BF822E8, 0x0009A004, 0xD28FD490,
+0x55D1420B, 0x22E852D1, 0xA0048BF8, 0xD48D0009,
+0x420BD28A, 0x52D255D2, 0x8BF822E8, 0x0009A004,
+0xD286D489, 0x55D3420B, 0x22E852D3, 0xA0048BF8,
+0xD4860009, 0x420BD281, 0x52D455D4, 0x8BF822E8,
+0x6EF64F26, 0x6DF6000B, 0x2FD62FC6, 0x4F222FE6,
+0x6E636C73, 0x6D53B01A, 0x64D357F4, 0xB05F65E3,
+0xB07566C3, 0xB0A40009, 0xB0A80009, 0xB0AC0009,
+0xB0AC0009, 0xB0AF0009, 0xB03154F5, 0x6CCD6C03,
+0x4F2660C3, 0x6DF66EF6, 0x6CF6000B, 0x3412D170,
+0xD6700529, 0x2650D770, 0x2742000B, 0x0009A018,
+0x2FD62FC6, 0x4F222FE6, 0x6E636C73, 0x6D53BFEE,
+0x64D357F4, 0xB03365E3, 0xB08D66C3, 0xB00F54F5,
+0x6CCD6C03, 0x4F2660C3, 0x6DF66EF6, 0x6CF6000B,
+0xE503D162, 0xD763D462, 0x21524518, 0x2472000B,
+0xD45FD15E, 0x2162E600, 0x2462000B, 0xBF734F22,
+0xBF73E40A, 0xD25C0009, 0x4118E104, 0xE40AE500,
+0xBF692212, 0xD7592252, 0xCB206072, 0x000B4F26,
+0x4F222702, 0x410BD156, 0xD556E400, 0x4F26452B,
+0xD1552FE6, 0x66126E63, 0x92104418, 0x44084528,
+0x45002629, 0x265B4408, 0x264B4400, 0x21624708,
+0xD14E4708, 0x217227EB, 0x6EF6000B, 0x1FFF03F0,
+0x4F222FE6, 0xE101DE4A, 0xBF3DE40A, 0x67E32E12,
+0xE500776C, 0xE204E130, 0x2752E40A, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x27222712, 0x27522752, 0x27522752, 0x27522752,
+0x27522752, 0x175ABF18, 0x2E62E600, 0x000B4F26,
+0xD2346EF6, 0xE441E101, 0x000B2212, 0xD1322242,
+0xE605D432, 0x000B2162, 0x000B2462, 0xD2300009,
+0xE40AE601, 0x2262AF00, 0x2FC62FB6, 0x2FE62FD6,
+0x7FFC4F22, 0x6C43DB2B, 0xED0060B2, 0x2B02CB03,
+0xC90360B2, 0x6E03A008, 0x89073DC2, 0xE46460B2,
+0xB07CC903, 0x7D016E03, 0x8BF52EE8, 0x8F043DC2,
+0xD4212FE1, 0x460BD621, 0x62F10009, 0x6023622D,
+0x89FFC801, 0x7F046023, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6000B, 0x001C3B88, 0x00203AC8, 0x002018D0,
+0x00203AD0, 0x00203AD8, 0x00203AE0, 0x00203AE8,
+0x0025E720, 0x00203DBC, 0x00203980, 0x001C5968,
+0x001C3B40, 0x000F8000, 0x001D4004, 0x001C3500,
+0x002015E4, 0x00201610, 0x001C5814, 0x001C59D0,
+0x001C5830, 0x001C6268, 0x001C59A4, 0x001C639C,
+0x001C581C, 0x001C5860, 0x00203AF0, 0x002018C0,
+0x8F014411, 0x6043604B, 0x0009000B, 0x5651D52B,
+0x46286052, 0x306C000B, 0x2FC62FB6, 0x2FE62FD6,
+0x4F124F22, 0xBFF14F02, 0x6B036E43, 0xDD25DC24,
+0x0009BFEC, 0x3C0530B8, 0x4609060A, 0x46014609,
+0x020A3D65, 0x42094209, 0x32E24209, 0x4F068BF0,
+0x4F264F16, 0x6DF66EF6, 0x000B6CF6, 0x2FC66BF6,
+0x2FE62FD6, 0x4F124F22, 0xBFCF4F02, 0x6C036E43,
+0xBFCBDD13, 0x30C80009, 0x060A3D05, 0x46094609,
+0x36E24601, 0x4F068BF5, 0x4F264F16, 0x6DF66EF6,
+0x6CF6000B, 0x4F222FE6, 0xE102DE0B, 0xE403E500,
+0xBFB92E12, 0xE6062E52, 0xE7004618, 0x2E62E403,
+0x4F262E72, 0x6EF6AFB0, 0x0009000B, 0x001C1040,
+0xCCCCCCCD, 0x10624DD3, 0x001D4004, 0x2F962F86,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0xE5007F98,
+0x6453E710, 0x6B534728, 0xEE1ADCBC, 0x6153655D,
+0x315C4108, 0x75014108, 0x6043317C, 0x0F16665D,
+0xED0060B3, 0x21B136E3, 0x81128111, 0x11D28113,
+0x11D411D3, 0x74048FEA, 0xD8B167F2, 0x1871D9B1,
+0x58F12872, 0x1981D1B0, 0x59F22982, 0x5DF45AF3,
+0x54F65EF5, 0x21921191, 0x11A211A3, 0x11D411D5,
+0x11E611E7, 0x11481149, 0xDAA855F7, 0x57F8EE00,
+0x52F9DDA7, 0x64E3D6A7, 0x2A521A51, 0xD8A7D9A6,
+0x2D72EAEF, 0x6AAC2622, 0x6DE36EED, 0x61E34D08,
+0x41083DEC, 0x31EC4D08, 0x60B33D9C, 0x2DB14108,
+0xE05081D1, 0xE79F4108, 0x41084008, 0x81D2677C,
+0x318C60B3, 0x3472E200, 0x1DD281D3, 0xD4931D13,
+0x1D248D01, 0x65D3D48F, 0x7E01B0B2, 0x34A264ED,
+0xDA8C8BDA, 0x68A22FD2, 0x4829DD91, 0x64A22D82,
+0x694D7DFC, 0x2D92D286, 0x4E296E22, 0x2DE27D0C,
+0x6AD36822, 0xD784618D, 0x6D722A16, 0xD583D489,
+0x5E7224D2, 0x14E2D688, 0xEE005174, 0x58761414,
+0x1486D186, 0xE7105978, 0x62521498, 0x142A65E3,
+0x64E326E2, 0x644DE600, 0x48086843, 0x4808384C,
+0x6053381C, 0x28B10C86, 0x60B309CE, 0x60538191,
+0x60430ACE, 0x605381A2, 0x60B30DCE, 0x605381D3,
+0x740108CE, 0x09CE1882, 0x19E3624D, 0x32730ACE,
+0x8FE01A64, 0xD96A7504, 0x6C92E003, 0x2CB14018,
+0xDA6F6D92, 0xE05081D1, 0x40086E92, 0x619281E2,
+0x811360B3, 0xE6006492, 0x67921442, 0x17A3D468,
+0xE1FF6892, 0xE7031864, 0x46086563, 0x7501364C,
+0x665D2612, 0x8BF83673, 0xE003DC5A, 0x40186DC2,
+0x6EC22DB1, 0x81E1D25F, 0xEE0061C2, 0x64C21112,
+0x1423E024, 0xD45B65C2, 0x67C215E4, 0x8172E580,
+0x66E368C2, 0x655C8183, 0x6963666D, 0x6A6D7604,
+0x3A53394C, 0x29E28FF8, 0xDC54DB53, 0x740424B2,
+0x7F6824C2, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0x614268F6, 0xC8036011, 0xE5008F03,
+0x3420D23C, 0x60118B06, 0x8802C903, 0xD2398B06,
+0x8B033420, 0x65135612, 0x24225264, 0x6053000B,
+0x2FE62FD6, 0x7FEC4F22, 0x62536E53, 0x6D43E550,
+0x4508E400, 0xE101A001, 0x60435224, 0x81212211,
+0x60538123, 0x56E28122, 0x8BF53620, 0x16E4D238,
+0xE61464F3, 0x65E3420B, 0xE4FC65E1, 0x2E512549,
+0x65F361F1, 0x2F112149, 0xD13154D1, 0xE614410B,
+0x607157D1, 0x2701CB01, 0x7F141DE1, 0x6EF64F26,
+0x6DF6000B, 0x2FE62FD6, 0x7FEC4F22, 0x66536E53,
+0x6D43E5FC, 0x20596061, 0x2601CB01, 0x326052E2,
+0x12E48B06, 0x31E051E2, 0x52D18B04, 0x1E22A002,
+0x5664AFF0, 0x64F3D21E, 0x420BE614, 0x67E165E3,
+0x2719E1FC, 0x67F12E71, 0x271954D1, 0x65F3D118,
+0x410BE614, 0x52D12F71, 0xCB016021, 0x1DE12201,
+0x4F267F14, 0x000B6EF6, 0x00006DF6, 0x002039AC,
+0x0020357C, 0x00203584, 0x0020358C, 0x002035B4,
+0x00203998, 0x002039A0, 0x00100208, 0x001014C0,
+0x001E210C, 0x001C3D00, 0x002039EC, 0x001000C8,
+0x00117880, 0x00117780, 0x00040020, 0x0026C401,
+0x00200D42, 0x4F222FE6, 0xDE42624C, 0x42004208,
+0x3E2CA005, 0xD4405252, 0xBF695624, 0x65E22E62,
+0x352052E1, 0xD63D8BF6, 0x4F262622, 0x6EF6000B,
+0x2FC62FB6, 0x2FE62FD6, 0xDC394F22, 0x52C1DB39,
+0x362066C2, 0x6061891C, 0x8801C903, 0xDE348918,
+0xBF38DD35, 0x650364E3, 0x66B28503, 0x3262620D,
+0xD4328907, 0x0009BF76, 0x4D0BD431, 0xAFE60009,
+0xBF3D0009, 0xD42F64E3, 0x00094D0B, 0x0009AFDF,
+0x2262D22D, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x2FD62FC6, 0x4F222FE6, 0xDD29DC28, 0x6E4360C2,
+0x04DE4008, 0xE614D127, 0x65E3410B, 0xD127D726,
+0x55E227E2, 0x35E05254, 0x21228F04, 0x400860C2,
+0x122202DE, 0x605365C2, 0x75014008, 0x0DE606DE,
+0xC90F6053, 0x60632C02, 0x6EF64F26, 0x000B6DF6,
+0x85436CF6, 0x650D5643, 0x622D6262, 0x35277204,
+0xE1008F0C, 0x2268960C, 0xD6158B03, 0x72015261,
+0xD6131621, 0x6262E101, 0x26227201, 0x6013000B,
+0x000001FF, 0x0020358C, 0x00203584, 0x001C3D00,
+0x002035B4, 0x0020397C, 0x002018C0, 0x0020357C,
+0x00203B18, 0x00203B1C, 0x001C3D28, 0x002039EC,
+0x002039AC, 0x00200D42, 0x002039F0, 0x002039F4,
+0x00117754, 0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6,
+0x7FF84F22, 0x6C22D241, 0xC80360C3, 0xDE40896E,
+0xDA41DB40, 0x52B1D941, 0x362066B2, 0x60618945,
+0x8801C903, 0xDD3B8941, 0x420BD23D, 0x650364D3,
+0x60A12F02, 0x89328801, 0x85145153, 0x8840600C,
+0x1F118F0C, 0xD5376191, 0x641D450B, 0x8B262008,
+0xD7356691, 0x646D470B, 0x8B202008, 0x420BD233,
+0x51F154F1, 0xC8208511, 0xD1318904, 0x021EE050,
+0x01267201, 0x420BD22F, 0x200864F2, 0x64D38907,
+0x4D0BDD2D, 0xD12D65F2, 0xAFC4E601, 0xD22C2162,
+0x420B65F2, 0xD72B64E3, 0xAFBCE601, 0xD2262762,
+0x420B65F2, 0xAFB664D3, 0xDE270009, 0xDA28DD27,
+0x52D1DB28, 0x362066D2, 0x60618918, 0x8801C903,
+0xD4228914, 0x450BD516, 0x56030009, 0x8F0436E0,
+0xE2016503, 0xAFEC2A20, 0xD41F2B52, 0x420BD216,
+0xD7180009, 0x4118E101, 0x2712AFE3, 0xC80460C3,
+0xD21A8902, 0x0009420B, 0x4F267F08, 0x6DF66EF6,
+0x6BF66CF6, 0x000B6AF6, 0x000069F6, 0x001E2100,
+0x0020358C, 0x00203584, 0x00203A14, 0x001142D8,
+0x002014A6, 0x00115EA2, 0x00114774, 0x00200D8A,
+0x0020351C, 0x002016C2, 0x002014D0, 0x001E212C,
+0x00201534, 0x001C3D30, 0x00117880, 0x0020357C,
+0x0020399C, 0x00203998, 0x002035B4, 0x00200644,
+0xE601D203, 0x1265D503, 0x000B2252, 0x00001266,
+0x001C1010, 0x0000C34F, 0x0009000B, 0x0009000B,
+0x0009000B, 0x0009000B, 0xE000000B, 0xE000000B,
+0x0009000B, 0xE4FDD59D, 0xD69D6152, 0x25122149,
+0x74016052, 0x2502CB01, 0xD19A6752, 0x25722749,
+0xC8406010, 0x60628902, 0x2602CB04, 0xE1F76462,
+0x26422419, 0xE7016062, 0x2602C9CF, 0xE5026062,
+0x2602CB10, 0x47186062, 0x2602CB03, 0x000B1652,
+0xD58D1673, 0xD28ED78D, 0xE100D48E, 0x2511E600,
+0x22102711, 0x2461AFCE, 0xD28B664C, 0x362C4600,
+0xCB106060, 0x2600000B, 0xD287654C, 0x352C4500,
+0xE1EF6650, 0x000B2619, 0x664C2560, 0x4600D283,
+0x6060362C, 0x000BCB10, 0x654C2600, 0x4500D27F,
+0x6650352C, 0x2619E1EF, 0x2560000B, 0xD27A664C,
+0x362C4600, 0xCB086060, 0x2600000B, 0xD276654C,
+0x352C4500, 0xE1F76650, 0x000B2619, 0x664C2560,
+0x4600D272, 0x6060362C, 0x000BCB08, 0x654C2600,
+0x4500D26E, 0x6650352C, 0x2619E1F7, 0x2560000B,
+0xD669624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0x624C600C, 0x4200D664, 0x6020326C,
+0x4021C908, 0x40214021, 0x600C000B, 0x644CD160,
+0x6240341C, 0x602C000B, 0x644CD15E, 0x6240341C,
+0x602C000B, 0x4F222FE6, 0x645C6E43, 0x3467E60A,
+0xBFEB8914, 0x640C0009, 0x880160EC, 0xE00F8B02,
+0x2409A002, 0x44094409, 0xE60A624C, 0x89053263,
+0x644CBFE2, 0x6023620C, 0x8B00C880, 0x6023E200,
+0x000B4F26, 0x4F226EF6, 0x6062D64B, 0x8B038801,
+0x0009B256, 0x0009A003, 0xE640D248, 0xD6482260,
+0x4F26E200, 0x2622000B, 0xD6434F22, 0x88026062,
+0xB29F8B01, 0xD6420009, 0x4F26E200, 0x2622000B,
+0xD43ED53D, 0xE701E100, 0x000B2512, 0xD23B2470,
+0x000BE604, 0x4F222260, 0xD13BD43A, 0x0009410B,
+0xE1FDD53A, 0xD23A6650, 0xE7002619, 0x4F262560,
+0x2270000B, 0xD5374F22, 0x6152D237, 0x611DD737,
+0x64522512, 0x242BE6FF, 0xD4352542, 0x666DD22E,
+0x2762420B, 0xE1FBD52D, 0x27196750, 0x000B4F26,
+0x4F222570, 0xD128D42F, 0x0009410B, 0xE7F7D527,
+0x26796650, 0x000B4F26, 0xD5242560, 0x62509425,
+0x000B2249, 0xD5212520, 0x6250E4BF, 0x000B2249,
+0x4F222520, 0x8522D224, 0x2008600D, 0x88018911,
+0x88038944, 0x88058946, 0x88068948, 0x8808894E,
+0x88098954, 0x880A895A, 0x880B8960, 0xA06D8966,
+0xB06F0009, 0xA06A0009, 0xFF7F600C, 0x001E2148,
+0x001E1108, 0x001E1000, 0x00203A4C, 0x00203A4E,
+0x00203A6D, 0x00203A30, 0x001E103F, 0x001E105F,
+0x001E102F, 0x001E1090, 0x00203A54, 0x001E100B,
+0x00203A50, 0x00203B20, 0x002018C0, 0x001E1028,
+0x00203A6C, 0x001D4020, 0x98760000, 0x001C1000,
+0x00203B2C, 0x00203B3C, 0x00203A24, 0x0009B04C,
+0x600CA035, 0x0009B055, 0x600CA031, 0x6260D684,
+0x8B2B2228, 0x0009B061, 0x600CA029, 0x6260D680,
+0x8B232228, 0x0009B069, 0x600CA021, 0x6260D67C,
+0x8B1B2228, 0x0009B0C7, 0x600CA019, 0x6260D678,
+0x8B132228, 0x0009B0CD, 0x600CA011, 0x6260D674,
+0x8B0B2228, 0x0009B125, 0x600CA009, 0x6260D670,
+0x8B032228, 0x0009B13D, 0x600CA001, 0x4F26E000,
+0x0009000B, 0xD26CD16B, 0xD56C8412, 0x4000C90F,
+0xD76B012D, 0xE403D66B, 0xE20F611C, 0x2540E001,
+0x25202712, 0x2602000B, 0xE601D262, 0x30668523,
+0xE0008D05, 0xD663D260, 0xE0018122, 0x000B2602,
+0xD25C0009, 0x600D8523, 0x89052008, 0x8B0A8801,
+0x6060D65D, 0x2600CB01, 0xD457D65A, 0xE001E101,
+0x000B2612, 0x000B8142, 0xD152E000, 0x8513E501,
+0x640D4518, 0x66033453, 0xE0008D05, 0xD551D253,
+0x2260E001, 0x000B2502, 0x4F220009, 0x8513D149,
+0x6453650D, 0x62494419, 0x227D672E, 0x8801602C,
+0x88028909, 0x88038910, 0x8806891A, 0x88078935,
+0xA04C893B, 0xD5460009, 0x6652D746, 0x2762D446,
+0x622C6261, 0x2421A038, 0x2228625C, 0xD4438B3F,
+0x6642D540, 0x2562D440, 0x24018561, 0x6203A02C,
+0x2008605C, 0x88108907, 0x88208908, 0x88308909,
+0xA02C890A, 0xD23A0009, 0x6222A008, 0xA005D239,
+0xD2396222, 0x6222A002, 0x6262D638, 0xD432D531,
+0x66212522, 0xA00F626C, 0xD6352421, 0x6261D52D,
+0x622CD42D, 0xA0072562, 0xD6322421, 0x8561D529,
+0x2562D429, 0x62032401, 0x662D8515, 0x3617610D,
+0x65038F01, 0xB0CB2451, 0xA0010009, 0xE000E001,
+0x000B4F26, 0xD6190009, 0xD427E101, 0x65412610,
+0xD118D717, 0xE20F655D, 0x2752E001, 0x000B2620,
+0x2FE62102, 0xD20F4F22, 0x640C8523, 0x8B082448,
+0xD511D61D, 0x2621E200, 0x940F8451, 0xA0482049,
+0xDE0D8051, 0xC84060E0, 0xE2018D32, 0x89443427,
+0xD216D615, 0x2641420B, 0x0009A030, 0x0000FF7F,
+0x00203A6D, 0x00203A24, 0x00203A30, 0x001E1100,
+0x001E100C, 0x00203A50, 0x001E1000, 0x001E1001,
+0x00203A58, 0x00203A38, 0x00203A3C, 0x00203A40,
+0x00203A5C, 0x00203A60, 0x00203A64, 0x00203A68,
+0x00203E20, 0x00203E2A, 0x00203A4A, 0x002027F2,
+0x89123427, 0xD294D693, 0x2641420B, 0xCB8084E1,
+0x80E1B0F5, 0xD69160E0, 0x2E00CB04, 0xC93F6060,
+0xD68F2600, 0xA001E001, 0xE0002602, 0x000B4F26,
+0xD68C6EF6, 0xC8806060, 0xD2868919, 0x88016021,
+0xD2898B15, 0x8524E501, 0x89103056, 0xE203D187,
+0x2120D487, 0xE00B6541, 0x0656655D, 0xE40FD585,
+0x2140E702, 0xD77E2571, 0x000BE001, 0x000B2702,
+0x2FE6E000, 0xDE804F22, 0xC88084E1, 0xD57A892C,
+0x20088554, 0x61038F28, 0x8553D77C, 0x64036672,
+0x8566650C, 0x3520620C, 0xD6798B1E, 0x651CD774,
+0x2651644C, 0x60E02741, 0x8904C840, 0x420BD275,
+0xA0030009, 0xD2680009, 0x0009420B, 0x0009B09F,
+0xE201D167, 0x60E02122, 0xCB04D464, 0x60402E00,
+0x2400C93F, 0x6023A001, 0x4F26E000, 0x6EF6000B,
+0x2FB62FA6, 0x2FD62FC6, 0xDA622FE6, 0x66A1E240,
+0x3622DC5E, 0x62638900, 0x6ED36D2C, 0x4E2136D8,
+0x4E212A61, 0xDB61D460, 0xE700A00F, 0x770162B2,
+0x71026123, 0x66212B12, 0x71026213, 0x61212B12,
+0x651D666D, 0x356C4528, 0x627C2452, 0x8BED32E3,
+0xC90360D3, 0x8B108803, 0x617367B2, 0x2B127102,
+0x71026E13, 0x2B126571, 0x655D6DE1, 0x422862DD,
+0x325CE107, 0xA00C2C10, 0x88022422, 0xA0038B01,
+0x8801E203, 0xE2018B05, 0x66B22C20, 0x655D6561,
+0xE60F2452, 0x67A12C60, 0x8B052778, 0xDD38DC44,
+0xEB01EA00, 0x2DB22CA2, 0x6DF66EF6, 0x6BF66CF6,
+0x6AF6000B, 0x2FE62FD6, 0xE240DD36, 0x362266D1,
+0x62638900, 0x3678672C, 0x7703DE38, 0x47212D61,
+0x64E2D635, 0xA00E4721, 0x6562E100, 0x62537101,
+0x74012450, 0x24204219, 0x45297401, 0x74012450,
+0x24504519, 0x621C7401, 0x8BEE3273, 0x66E24200,
+0x420061D1, 0x2118362C, 0x2E628F06, 0xDD1CD728,
+0xE501E400, 0x2D522742, 0x000B6EF6, 0x2FD66DF6,
+0x4F222FE6, 0xED0AEE01, 0x64E3BC86, 0xBC8B64E3,
+0x62EC7E01, 0x8BF732D7, 0xBC8EEE01, 0x64E364E3,
+0x7E01BC93, 0x32D762EC, 0x4F268BF7, 0x000B6EF6,
+0xD1186DF6, 0xD418920D, 0x72122122, 0x2422D617,
+0xD7177204, 0x72202622, 0x2722D116, 0x000B7230,
+0x137A2122, 0x00203A4A, 0x002028FE, 0x001E1015,
+0x00203A50, 0x001E1001, 0x00203A24, 0x001E1100,
+0x00203A4E, 0x00203A3C, 0x001E1000, 0x00203A40,
+0x00203A4C, 0x002027F2, 0x001E100C, 0x00203A38,
+0x00203A54, 0x00203A58, 0x00203A5C, 0x00203A60,
+0x00203A64, 0x00203A68, 0x4F222FE6, 0xD6707FFC,
+0x88016060, 0xE2018951, 0x2620BFBB, 0xD56ED16D,
+0xDE6E6010, 0x64E36552, 0x7402C840, 0x8D22D16C,
+0xD26C7502, 0xE601D76C, 0xE7042722, 0x76016255,
+0x626C2421, 0x8FF93273, 0xD4637402, 0x6242E601,
+0x640D8528, 0x67494419, 0x275D657E, 0x81E4607C,
+0xE417D562, 0x67557601, 0x3243626C, 0x8FF92171,
+0xA0207102, 0xD25E0009, 0xE601D75B, 0xE7042722,
+0x76016255, 0x626C2421, 0x8FF93273, 0xD4527402,
+0x6242E601, 0x640D8528, 0x67494419, 0x275D657E,
+0x81E4607C, 0xE417D553, 0x67557601, 0x3243626C,
+0x8FF92171, 0x92897102, 0xD2462E21, 0x5E23D74E,
+0x64F22FE2, 0x604365F2, 0x2700C980, 0xC9606043,
+0x80716103, 0xC9036043, 0x80724519, 0x65F2605C,
+0x817266F2, 0x46194629, 0x606C4529, 0x4018645C,
+0x8173304C, 0x21185E23, 0x64F22FE2, 0x6E4C62F2,
+0x602C4219, 0x66F262F2, 0x46294018, 0x461930EC,
+0x42298174, 0x652C606C, 0x305C4018, 0x81758F07,
+0x0009BC97, 0x2228620C, 0xA00A8908, 0x60130009,
+0x8B038840, 0x0009B009, 0x0009A003, 0xE202D62F,
+0x7F042622, 0x000B4F26, 0x4F226EF6, 0x8552D52A,
+0x8830600D, 0x88318903, 0xA0348923, 0x85550009,
+0xD428D727, 0x85532701, 0x610DD627, 0x24124118,
+0x460BD426, 0xD7230009, 0xD226D425, 0x6572420B,
+0xE230D120, 0x42286712, 0x2729E620, 0x37604628,
+0xD6218B03, 0xA016E200, 0xD61F2622, 0xA012E202,
+0xD1182622, 0x6212E530, 0xE6204528, 0x46282259,
+0x89083260, 0xD41AD119, 0xE601D513, 0x2160450B,
+0x472BD718, 0x4F264F26, 0x0009000B, 0x0000060A,
+0x00203A6C, 0x001E1000, 0x00203A58, 0x00203E20,
+0x00203E2C, 0x00203DC4, 0x00203A40, 0x00203DF4,
+0x00203DF2, 0x00203DC6, 0x00203A24, 0x00203A50,
+0x00203A3C, 0x00203A38, 0x002018C0, 0x00203B48,
+0x00203B4C, 0x002018D0, 0x00203A54, 0x001E100B,
+0x00203B60, 0x00114004, 0x4F222FE6, 0x84E9DE86,
+0x2448640C, 0xB17B8901, 0xD2840009, 0x26686620,
+0x60E08902, 0x2E00C9BF, 0x000B4F26, 0x000B6EF6,
+0x2FE60009, 0xDE7E4F22, 0x60E0D67E, 0xCBC0D47E,
+0x62602E00, 0xC803602C, 0x40218904, 0x70014021,
+0x6603A002, 0x66034009, 0xD678616D, 0xE500A004,
+0x75016262, 0x74042422, 0x3213625D, 0xD2748BF8,
+0x0009420B, 0xC9BF84E2, 0x4F2680E2, 0x6EF6000B,
+0x2FE62FD6, 0x7FFC4F22, 0x6260D66E, 0x89402228,
+0xD565E100, 0x60502610, 0xCB40D46B, 0x2500440B,
+0x8D052008, 0x62E06E03, 0x7104612C, 0x2F11A006,
+0xD466D65E, 0xDD666760, 0x657C4D0B, 0xE23C6D1D,
+0x8B033D27, 0xD264D463, 0x0009420B, 0x4D214D21,
+0xA005D762, 0x66E6E400, 0x357C4508, 0x74012562,
+0x35D3654D, 0xD75E8BF7, 0x6E72E003, 0x81E14018,
+0x6E7260F1, 0x81E2700C, 0xD45A6172, 0xDD5A8113,
+0x65724D0B, 0xD64AD259, 0x2212E101, 0xC93F6060,
+0x7F042600, 0x6EF64F26, 0x6DF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xD2524F22, 0x6B436E73, 0x420B6C53,
+0x20086D63, 0x64038D1C, 0xE50ED13C, 0x32526210,
+0x60C38916, 0x804124B0, 0x814160D3, 0xA007E500,
+0x655D61BC, 0x00EC6053, 0x364C6653, 0x80647501,
+0x3213625D, 0xD6308BF5, 0xC9BF6060, 0x2600A008,
+0xD239D440, 0x6EF64F26, 0x6CF66DF6, 0x6BF6422B,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x2F962F86,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0xE1007FC4,
+0x6513ECFF, 0x6B136CCD, 0xDE34D733, 0xEDFF64F3,
+0xD833EA04, 0x6053655C, 0x027D4000, 0x32C0622D,
+0x66038D0D, 0x09ED6063, 0x2491027D, 0x24217402,
+0x698202ED, 0x3928622D, 0x74022892, 0x75017104,
+0x6063625C, 0x07D532A2, 0x0EB58FE4, 0x2448641C,
+0xE6808905, 0x67F3E5C5, 0xBF8F666C, 0x7F3C655C,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0xD11C68F6, 0x6012D21C, 0xCB20E405, 0x2102E500,
+0x000B2242, 0x00002252, 0x001E1017, 0x00203996,
+0x001E1015, 0x001E10BF, 0x00117800, 0x001E10FC,
+0x00200644, 0x0020399C, 0x00202A56, 0x00203B64,
+0x002018D0, 0x00203B80, 0x002018C0, 0x0011788C,
+0x00203998, 0x0020357C, 0x00201534, 0x001E2130,
+0x00202A18, 0x00203B88, 0x002039FC, 0x00203A04,
+0x00203DC0, 0x001C3500, 0x001D4004, 0xD564D163,
+0xE400D764, 0x2142E20F, 0x17411154, 0xD5622722,
+0x9669D762, 0x15412572, 0x96661562, 0xE6011565,
+0xD55F1165, 0x666CE6F8, 0x25422542, 0x25422542,
+0x25422542, 0x25622542, 0x7601E727, 0x67632572,
+0x25627797, 0xE7042572, 0x2572E248, 0xE2192522,
+0xE2702522, 0x25422542, 0x25422542, 0x25222542,
+0x2522E20C, 0x25422542, 0x25422542, 0x25422542,
+0x25422542, 0x000B154A, 0xE2081145, 0x0009422B,
+0x2FE62FD6, 0x7FFC4F22, 0xC8206043, 0x6E438D02,
+0x0009BE85, 0xC81060E3, 0xBE828901, 0x60E30009,
+0x8901C840, 0x0009BEA4, 0xC80160E3, 0xDD3D8938,
+0xC80260D0, 0x2F008D03, 0x460BD63B, 0x60F00009,
+0x8902C804, 0x460BD639, 0x62F00009, 0xC8806023,
+0x60D08902, 0x2D00C97F, 0xC8016023, 0xD6348906,
+0x0009460B, 0x0009A007, 0x51630601, 0x8902C808,
+0x460BD630, 0x60F00009, 0x8902C810, 0x420BD22E,
+0xD52E0009, 0x88026052, 0xD22D8B03, 0xA005E604,
+0x88012260, 0xD22A8B02, 0x2260E601, 0x2522E200,
+0xC88060E3, 0xD227892D, 0x60E36E20, 0x8902C880,
+0x420BD225, 0x60E30009, 0x8902C840, 0x420BD223,
+0x60E30009, 0x8902C802, 0x420BD221, 0x60E30009,
+0x890DC804, 0xDD20D11F, 0x0009410B, 0x0009BF11,
+0x0009BF4C, 0xD51ED41D, 0x2470E708, 0x25D2BF85,
+0xC80860E3, 0xD21B8905, 0x4F267F04, 0x422B6EF6,
+0x7F046DF6, 0x6EF64F26, 0x6DF6000B, 0x001C581C,
+0xA000A000, 0x001D0100, 0x001D4000, 0x00040021,
+0x001C589C, 0x001E1021, 0x00201A46, 0x00201A68,
+0x002020C8, 0x00201A80, 0x00201A8E, 0x00203A50,
+0x001E100B, 0x001E1028, 0x00201AFA, 0x00201B06,
+0x00201A96, 0x00201AB4, 0x12345678, 0x001E1000,
+0x0010F100, 0x00201AE2, 0x644CD6A7, 0x000B346C,
+0xD6A62450, 0x346C644C, 0x2450000B, 0x644CD6A4,
+0x000B346C, 0x625C2450, 0x4208616D, 0x42084119,
+0x42006019, 0x670E614C, 0xD49E321C, 0x4200207D,
+0x324CC90F, 0x2200000B, 0x4208625C, 0x42004208,
+0x324C644C, 0x4200D498, 0x000B324C, 0x2FE62260,
+0x614C4F12, 0x4100D493, 0x6710314C, 0xE29F666D,
+0x27294619, 0x6E536269, 0x672E6573, 0x4221227D,
+0x42214221, 0x7601662C, 0xE4014608, 0x34E84608,
+0x644C4600, 0x071A0467, 0x2150257B, 0x000B4F16,
+0x4F226EF6, 0xD2857FE8, 0x88016021, 0xD2848B7B,
+0x26686621, 0xD2838B77, 0x26686621, 0xE50F8B73,
+0xE401BFA2, 0xBFA4E501, 0xE586E400, 0xE400655C,
+0x2F50BFA4, 0xBFA1E401, 0xE602E506, 0x60634618,
+0x81F2E401, 0x6543BF9F, 0xE40185F2, 0xBFAB6543,
+0x85F26603, 0x6543E401, 0x6603BFB1, 0xE40265F0,
+0x6053756C, 0x80F8BF80, 0xBF82E402, 0x84F8E512,
+0x7090E402, 0x6503BF82, 0x4618E602, 0x81F66063,
+0xBF80E402, 0x85F6E500, 0x6603E402, 0xE500BF8C,
+0xE40285F6, 0xBF926603, 0xE5FEE500, 0xE010655C,
+0xBF61E403, 0xE5130F54, 0xE40EBF63, 0x05FCE010,
+0xBF63E40E, 0xE5007585, 0xBF64E403, 0xE500E640,
+0xBF71E403, 0xE500E640, 0xBF78E403, 0xE5FFE640,
+0xE014655C, 0xBF47E404, 0xE40F0F54, 0xE504BF49,
+0x05FCE014, 0xBF49E40F, 0xE5017584, 0xBF4AE640,
+0xE501E404, 0xBF57E640, 0xE501E404, 0xE404E640,
+0xAF5C7F18, 0x7F184F26, 0x000B4F26, 0x4F220009,
+0xD2427FF0, 0x88016021, 0xD2418B71, 0x26686621,
+0xD2408B6D, 0x26686621, 0xE50F8B69, 0xE401BF1C,
+0xBF1EE501, 0xE586E400, 0xE400655C, 0x2F50BF1E,
+0xBF1BE401, 0xE401E506, 0xBF1C6543, 0xE401E640,
+0xBF296543, 0xE401E640, 0xBF306543, 0x65F0E640,
+0x756CE402, 0xBEFF6053, 0xE40280F4, 0xE512BF01,
+0xE40284F4, 0xBF017090, 0xE6406503, 0xBF02E402,
+0xE640E500, 0xBF0FE402, 0xE640E500, 0xBF16E402,
+0xE5FEE500, 0x6053655C, 0xBEE5E403, 0xE51380F8,
+0xE40EBEE7, 0xE40E84F8, 0xBEE77085, 0xE5006503,
+0xBEE8E640, 0xE500E403, 0xBEF5E640, 0xE500E403,
+0xBEFCE640, 0xE5FFE403, 0x6053655C, 0xBECBE404,
+0xE40F80FC, 0xE504BECD, 0xE40F84FC, 0xBECD7083,
+0xE5016503, 0xBECEE640, 0xE501E404, 0xBEDBE640,
+0xE501E404, 0xE404E640, 0xAEE07F10, 0x7F104F26,
+0x000B4F26, 0x00000009, 0x001E102F, 0x001E1080,
+0x001E1090, 0x001E103F, 0x001E103E, 0x00203A4A,
+0x00203A4C, 0x00203A4E, 0xD21DD11C, 0x66206010,
+0x676C7001, 0x3700C90F, 0xE5008D13, 0x67106210,
+0x7701622C, 0x64232170, 0xD6166010, 0x44084408,
+0x3428C90F, 0x62602100, 0x7201D513, 0x44082620,
+0x000B354C, 0xD10F6053, 0x25586510, 0xE6008D13,
+0xD60DD40B, 0x655C6540, 0x47086753, 0x37584708,
+0x47086540, 0x24507501, 0x367C6040, 0x2400C90F,
+0x72FF6210, 0x000B2120, 0x00006063, 0x00203995,
+0x00203994, 0x00203996, 0x002035BC, 0x7FFC4F22,
+0xE680D1A8, 0x666C6212, 0xD2A72F22, 0x67F36563,
+0x420B7542, 0x7F04E404, 0x000B4F26, 0xE6800009,
+0xD2A1666C, 0xE7006563, 0x422B7540, 0xE6806473,
+0xD29D666C, 0xE7006563, 0x422B7543, 0x2F866473,
+0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6, 0x7FC04F22,
+0xDB97D296, 0x72012F22, 0xD1961F21, 0x66125211,
+0x8B013620, 0x0009A0F9, 0xC9036061, 0x8B018801,
+0x0009A0F3, 0xD290DC8F, 0x64C3420B, 0x6503D18F,
+0x60111F02, 0x8B048801, 0x420BD28D, 0xAFE464C3,
+0x54530009, 0x844CEE84, 0x890130E0, 0x0009A0C3,
+0x6610D188, 0x6023626C, 0x8B718801, 0x6210D186,
+0x89662228, 0xDA86D285, 0xE0036122, 0x64221112,
+0x4018D881, 0xDD83E500, 0x814167A3, 0x77042850,
+0x647266A2, 0x6ED3D580, 0x1F457E04, 0x65521F56,
+0x64E368D2, 0x1F8874F8, 0x684369E2, 0x1F637894,
+0x1F991F74, 0x62826142, 0xD779D978, 0x1F2BD679,
+0x67726292, 0x1F1A6062, 0x2602CB20, 0xD176E600,
+0xE5401F57, 0x1F7D1F2C, 0x76011F1E, 0x3253626D,
+0x51F38BFB, 0x52F555F4, 0x25222A12, 0x55F757F6,
+0x27525AF8, 0x5DF92DA2, 0x2ED251FB, 0xD56B5EFA,
+0x54FC24E2, 0x281257FD, 0xD160D869, 0x25722942,
+0x69126782, 0x1974D866, 0xDD666A12, 0x56FE60A1,
+0x2A01CB01, 0xDA646412, 0xE9012842, 0x4A0B2D42,
+0x52FE2692, 0xD661EE01, 0x22E24E18, 0x72016262,
+0x60B22622, 0xCB01D14F, 0x2B02E202, 0x2120A03F,
+0x8B3C2228, 0xE601D55A, 0x2160E700, 0xE01C2572,
+0xC801004C, 0xD8578B0C, 0x1F8FD257, 0xE6002822,
+0x7601E57D, 0x3253626C, 0x56FF8BFB, 0x2622D253,
+0xE2FE69B2, 0x2B922929, 0x0A4CE01E, 0xE01F65F2,
+0x014C25A0, 0x741057F1, 0xEA062710, 0xDD4CE600,
+0x8446DE4C, 0x2D007601, 0x696C6844, 0x2E8039A3,
+0x8FF67E01, 0xDE487D01, 0x2EA0EA94, 0xE1007E01,
+0x7E0F2E10, 0xD12FE205, 0x64102E20, 0x6023624C,
+0x89088801, 0x55F2D22A, 0x64C3420B, 0xEE01D132,
+0xAF1A4E18, 0x55F221E2, 0x8553D13C, 0x620D6612,
+0x89063262, 0xD63BD43A, 0xE801460B, 0xAF0CD73A,
+0xD91F2782, 0x64C3490B, 0xEE01D127, 0xDA38D437,
+0x4A0B4E18, 0xAF0021E2, 0x7F400009, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0x4F2268F6,
+0x85467FF4, 0x2F01E681, 0x666C8547, 0x854881F1,
+0x81F2D209, 0x67F38542, 0x854381F3, 0x81F4E40C,
+0x65636053, 0x420B81F5, 0x7F0C7540, 0x000B4F26,
+0x00000009, 0x001C3D9C, 0x002023FC, 0x0011779A,
+0x001C36F8, 0x002035B4, 0x002014A6, 0x00203A16,
+0x002014D0, 0x002039A5, 0x002039A4, 0x002039A0,
+0x001C3B9C, 0x001C3704, 0x001C3D98, 0x001C3BB4,
+0x001C5960, 0x001C3500, 0x001C3D30, 0x001C8960,
+0x0020358C, 0x001C3D00, 0x00201610, 0x00117730,
+0x002039A8, 0x001C582C, 0x2000A000, 0x0000A000,
+0x0011778C, 0x00117792, 0x00117788, 0x0020397C,
+0x0020357C, 0x00201534, 0x001E2130, 0x00203DA0,
+0x002018C0, 0x2F962F86, 0x2FB62FA6, 0x2FD62FC6,
+0x4F222FE6, 0xD19B7FEC, 0x2F12E000, 0x6103D49A,
+0x1F4281F2, 0xDD9ADA99, 0xD69A6813, 0xE0014808,
+0x460BDE99, 0x38EC4800, 0x65A21F03, 0x352052A1,
+0xA23E8B01, 0x60510009, 0x8801C903, 0xA2388B01,
+0x52530009, 0x32E0DE91, 0xD9918B10, 0x64A3490B,
+0x4B0BDB90, 0xDE906403, 0xD791D690, 0xEC01D591,
+0x2E02E100, 0x271026C0, 0x2502AFDF, 0xC8018551,
+0xA1578B01, 0x62510009, 0x4200622D, 0x5E53366A,
+0x85E2226D, 0xC903642C, 0x85E36603, 0x6053650D,
+0x40214021, 0x4500C93F, 0x322A6703, 0x6053252D,
+0xC901D17F, 0x60106C03, 0x8801D97F, 0xDB7F8B05,
+0x2120E200, 0xCB0160B2, 0xD17D2B02, 0x88016011,
+0x65A28B0A, 0x8D042448, 0x9B9E6251, 0xA00322B9,
+0x919B2521, 0x2521221B, 0x37B3EB10, 0x2448895E,
+0xD4738B07, 0x22286241, 0x60638903, 0xA05781F8,
+0xD5706473, 0x46084608, 0x85E26273, 0x46006B50,
+0x362C4200, 0x2BB8C910, 0x8F1F6463, 0x26686603,
+0xD2698911, 0x062D6043, 0x4119616D, 0x6B0E6019,
+0x81F820BD, 0x880160C3, 0x646C8F2C, 0x880F6073,
+0xA0278B1B, 0xD2610009, 0x052D6043, 0x4119615D,
+0x670E6019, 0x645C207D, 0x81F8A01C, 0x890F2668,
+0x6043D25B, 0x6B5D052D, 0x60B94B19, 0x201D610E,
+0x60C381F8, 0x8F0D8801, 0x6473645C, 0xEC00A00A,
+0x6043D254, 0x625D052D, 0x60294219, 0x207D670E,
+0x81F8645C, 0x880285F8, 0x85E1890A, 0x8D07C820,
+0xE6DC6203, 0x60232269, 0x81E1A002, 0x644CE4FF,
+0x6210D149, 0x89012228, 0x644CE4FF, 0x654DEBFF,
+0x35B06BBC, 0xDB368B2B, 0x64A34B0B, 0x410BD135,
+0x54036403, 0x85446E03, 0xC948DB40, 0xDC408808,
+0xBEAC8B01, 0x64B3E502, 0x65E34C0B, 0xDB3DEC01,
+0xD13D2DC2, 0x621260B2, 0x72017001, 0x21228805,
+0x2B028F08, 0x666CE680, 0x6563D238, 0x7549E700,
+0x6473420B, 0xA030D436, 0x7FFF0009, 0x85E28000,
+0x20B9EBFC, 0x610381E2, 0x942A85E3, 0x62032049,
+0x450885F8, 0x81E2201B, 0xC90160C3, 0x40084018,
+0x40084008, 0x4000225B, 0x6023220B, 0x85E481E3,
+0x4118E108, 0x81E4201B, 0xE40262A2, 0x20B98521,
+0x67A28121, 0xCB016071, 0x85F82701, 0x89033042,
+0xECE785E2, 0x81E220C9, 0x490BD41E, 0xA03B0009,
+0x7E030009, 0x001C3D30, 0x00203DAC, 0x0020358C,
+0x001E212C, 0x00203470, 0x001C3D00, 0x00117780,
+0x002014A6, 0x00201670, 0x0011770C, 0x002039A4,
+0x002039A5, 0x002039A0, 0x002018C0, 0x001C36F8,
+0x00203A1A, 0x00203DBC, 0x00203BA0, 0x00203C20,
+0x00203CA0, 0x00203D20, 0x00203990, 0x00203584,
+0x002014D0, 0x00203A1C, 0x00203A20, 0x002023FC,
+0x00203DA4, 0x00203DA8, 0x602262F2, 0x40094019,
+0xC90F4009, 0x8B0B880A, 0x60E2DE8C, 0x40094019,
+0xC90F4009, 0x8B038808, 0xCB0160A2, 0x2802A006,
+0x65E2DE87, 0x2E527501, 0x286266A2, 0x52F366F2,
+0x2622AE83, 0xD2838551, 0xDE83C802, 0xA0958B01,
+0x420B0009, 0x4E0B64A3, 0x5E036403, 0x85E46503,
+0x4918E908, 0xD77D209B, 0xE04C81E4, 0xDC7C0B7E,
+0x7B01D97C, 0x61C207B6, 0x71016690, 0x8D062668,
+0xD4792C12, 0x420BD279, 0xA070EB01, 0x62512DB2,
+0x4B18EB0F, 0x22B9E102, 0x32104118, 0x85518B0F,
+0x2029E2FC, 0x60518151, 0xCB0172E0, 0x85E12501,
+0x202994A3, 0x85E481E1, 0xA0522049, 0x675181E4,
+0x4719677D, 0x667E6779, 0x7701276D, 0x6903607C,
+0x88014918, 0x25918F3E, 0x6B12D161, 0x21B27B01,
+0x660D85E3, 0x40216063, 0xC93F4021, 0x6C034600,
+0x262D322A, 0xC8016063, 0xDB5ED15D, 0x967D8901,
+0xE6002C6B, 0x666C67CD, 0x40006063, 0x622D021D,
+0x8D0E3270, 0x60436403, 0xE9FF021D, 0x8B013290,
+0x01C5A007, 0x626C7601, 0x3292E904, 0x646C8BEB,
+0x60434400, 0xD15004BD, 0x0B457401, 0x669D6911,
+0x89073670, 0x602D6211, 0x890388FF, 0xE201DB4B,
+0x2B2021C1, 0xECFC8551, 0x815120C9, 0xCB016051,
+0xDC472501, 0x64A34C0B, 0x51F366F2, 0x85EF2612,
+0x54F2D244, 0x650D420B, 0x0009ADE7, 0xE500DC42,
+0x420B2C52, 0x4E0B64A3, 0x54036403, 0x85446E03,
+0x6703E908, 0x65034918, 0x27998541, 0xDB323790,
+0x8F0BD932, 0x6013610D, 0x8B07C820, 0xC9486053,
+0x8B038808, 0xE501BD4B, 0x0009A005, 0x2128D233,
+0xBD448901, 0x64B3E500, 0x490B65E3, 0xADBCEC01,
+0x85F22DC2, 0x7001EE04, 0x31E7610D, 0x8D0281F2,
+0xADA97A08, 0x7F140009, 0x6EF64F26, 0x6CF66DF6,
+0x6AF66BF6, 0x000B69F6, 0xF7FF68F6, 0x2FE68000,
+0xD2234F22, 0x60E36E22, 0x8D02C840, 0xBBE522E2,
+0xE2400009, 0x2E284218, 0xBBF08901, 0x60E30009,
+0x8905C810, 0xD21CD41B, 0x0009420B, 0x0009BBEF,
+0xC80560E3, 0xBD6D8901, 0x60E30009, 0x8902C802,
+0xABEC4F26, 0x4F266EF6, 0x6EF6000B, 0x001C3D3C,
+0x00117760, 0x002014A6, 0x00201670, 0x0020351C,
+0x00203DC0, 0x00203990, 0x00203584, 0x002014D0,
+0x002039FC, 0x00203A04, 0x002039F8, 0x002039FA,
+0x00201534, 0x002018D0, 0x00203A1C, 0x00008000,
+0x001C3510, 0x00203DB4, 0x002018C0, 0x89014F22,
+0x611B600B, 0x611BB00A, 0x000B4F26, 0x600B600B,
+0x611BA004, 0x8DF12107, 0x8BF84011, 0x620D2F26,
+0x8F3E3020, 0x40180019, 0x8B0B3016, 0x31043104,
+0x31043104, 0x31043104, 0x31043104, 0x412462F6,
+0x601C000B, 0x41296219, 0x20084018, 0x31048926,
+0x31043104, 0x31043104, 0x31043104, 0x31043104,
+0x31043104, 0x31043104, 0x31043104, 0x61193104,
+0x3204221D, 0x32043204, 0x32043204, 0x32043204,
+0x32043204, 0x32043204, 0x32043204, 0x32043204,
+0x212D3204, 0x601962F6, 0x4024000B, 0x000BE000,
+0x621362F6, 0x41294228, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x602D4224, 0x62F6000B,
+0x080A0C0E, 0x00020406, 0x1A1C1E20, 0x12141618,
+0x2E303234, 0x26282A2C, 0x3A3C3E40, 0x6C625648,
+0x41112F26, 0xE2208F18, 0x890B3123, 0x321CD204,
+0xD1026220, 0x412B312C, 0x00090009, 0x0020349A,
+0x00203450, 0x000BE000, 0x400062F6, 0x40004000,
+0x40004000, 0x40004000, 0x62F6000B, 0x40004000,
+0x40004000, 0x40004000, 0x40184000, 0x62F6000B,
+0x40004000, 0x40004000, 0x40004000, 0x40284000,
+0x62F6000B, 0x40004000, 0x40184000, 0x000B4028,
+0xC90F62F6, 0x40054005, 0x40054005, 0x62F6000B,
+0x4005C907, 0x40054005, 0x62F6000B, 0x4005C903,
+0x000B4005, 0xC90162F6, 0x000B4005, 0x000062F6,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x42707372,
+0x3D206675, 0x554E203D, 0x202C4C4C, 0x6E49677A,
+0x4E497274, 0x6D754E51, 0x0000003D, 0x61766E49,
+0x2064696C, 0x72657375, 0x20726F20, 0x2079656B,
+0x00214449, 0x6E6B6E55, 0x206E776F, 0x6D6D6F63,
+0x3D646E61, 0x00000000, 0x203A3051, 0x00000020,
+0x203A3151, 0x00000020, 0x203A3251, 0x00000020,
+0x203A3351, 0x00000020, 0x203A3451, 0x00000020,
+0x2B434741, 0x73696F4E, 0x61432065, 0x7262696C,
+0x6F697461, 0x6166206E, 0x6F206C69, 0x6974206E,
+0x0D0A656D, 0x00000000, 0x00000072, 0x00205220,
+0x62735576, 0x7473725F, 0x00000A0D, 0x62735576,
+0x7375735F, 0x646E6570, 0x00000A0D, 0x62735576,
+0x7365725F, 0x000A0D6D, 0x00000044, 0x44387570,
+0x72637365, 0x6F747069, 0x3D584572, 0x00000000,
+0x00000047, 0x72746E49, 0x6D652051, 0x2C797470,
+0x49677A20, 0x4972746E, 0x754E514E, 0x00003D6D,
+0x654C7245, 0x0000006E, 0x20746F4E, 0x756F6E65,
+0x49206867, 0x4220514E, 0x0A0D6675, 0x00000000,
+0x000000FF, 0x00020001, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00020003, 0x01090108, 0x0002010A,
+0x02000003, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x000000FF, 0x00020001, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00020003, 0x01090108, 0x0002010A,
+0x00030003, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00FF010F, 0x01090108, 0x010B010A,
+0x0200010F, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00FF010F, 0x01090108, 0x010B010A,
+0x010F010F, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00205220, 0x00000046, 0x00000059, 0x73204142,
+0x003D7165, 0x49544120, 0x0000204D, 0x00000000,
+0x00000000, 0x002E0209, 0x80000101, 0x000409FA,
+0x00FF0400, 0x05070000, 0x02000201, 0x82050700,
+0x00020002, 0x03830507, 0x07010040, 0x40030405,
+0x02090100, 0x0101002E, 0x09FA8000, 0x04000004,
+0x000000FF, 0x02010507, 0x07000040, 0x40028205,
+0x05070000, 0x00400383, 0x04050701, 0x00004002,
+0x00000000, 0x00000000, 0x07090000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x6621D2A8, 0x2008606D, 0xA1B18B01, 0x88100009,
+0x88118922, 0x88128920, 0x8813891E, 0x8821891C,
+0x8822891A, 0x883A8918, 0x883B8916, 0xE6448914,
+0x30604608, 0xE6488910, 0x30604608, 0xE658890C,
+0x30604608, 0x963D8908, 0x89053060, 0x3060963B,
+0x96398902, 0x8B013060, 0xE010000B, 0x8B018820,
+0xE020000B, 0x892B8837, 0x89298832, 0x89278835,
+0x89258836, 0x89238830, 0x89218838, 0x891F8839,
+0x891D8834, 0x891B8833, 0x4608E64C, 0x89173060,
+0x3060961B, 0x96198914, 0x89113060, 0x30609617,
+0x9615890E, 0x890B3060, 0x30609613, 0x96118908,
+0x89053060, 0x3060960F, 0x960D8902, 0x8B0C3060,
+0xE030000B, 0x05100165, 0x02300A10, 0x04300330,
+0x06300530, 0x0B300A30, 0x88400C30, 0xA1428B01,
+0x88410009, 0xA13E8B01, 0x88430009, 0xA13A8B01,
+0x88480009, 0xA1368B01, 0x884A0009, 0xA1328B01,
+0x884B0009, 0xA12E8B01, 0x884C0009, 0xA12A8B01,
+0xE6800009, 0x3060666C, 0xA1248B01, 0xE6810009,
+0x3060666C, 0xA11E8B01, 0xE6820009, 0x3060666C,
+0xA1188B01, 0xE6830009, 0x3060666C, 0xA1128B01,
+0xE6840009, 0x3060666C, 0xA10C8B01, 0xE6850009,
+0x3060666C, 0xA1068B01, 0xE6860009, 0x3060666C,
+0xA1008B01, 0xE6870009, 0x3060666C, 0xA0FA8B01,
+0xE6880009, 0x3060666C, 0xA0F48B01, 0xE6890009,
+0x3060666C, 0xA0EE8B01, 0xE68A0009, 0x3060666C,
+0xA0E88B01, 0xE68B0009, 0x3060666C, 0xA0E28B01,
+0xE68C0009, 0x3060666C, 0xA0DC8B01, 0xE68D0009,
+0x3060666C, 0xA0D68B01, 0xE68E0009, 0x3060666C,
+0xA0D08B01, 0xE68F0009, 0x3060666C, 0xA0CA8B01,
+0xE6900009, 0x3060666C, 0xA0C48B01, 0xE6910009,
+0x3060666C, 0xA0BE8B01, 0xE6F80009, 0x3060666C,
+0xA0B88B01, 0xE6F90009, 0x3060666C, 0xA0B28B01,
+0xE6FA0009, 0x3060666C, 0xA0AC8B01, 0xE6FB0009,
+0x3060666C, 0xA0A68B01, 0xE6FC0009, 0x3060666C,
+0xA0A08B01, 0xE6FD0009, 0x3060666C, 0xA09A8B01,
+0xE6FE0009, 0x3060666C, 0xA0948B01, 0xE6FF0009,
+0x3060666C, 0xA08E8B01, 0xE6D00009, 0x3060666C,
+0xA0888B01, 0xE6D10009, 0x3060666C, 0xA0828B01,
+0xE6D20009, 0x3060666C, 0xA07C8B01, 0xE6D30009,
+0x3060666C, 0xE6D48977, 0x3060666C, 0xE6D58973,
+0x3060666C, 0xE6D6896F, 0x3060666C, 0xE6D7896B,
+0x3060666C, 0xE6D88967, 0x3060666C, 0xA0038963,
+0x00000009, 0x00114000, 0x666CE6D9, 0x895A3060,
+0x666CE6DA, 0x89563060, 0x666CE6DB, 0x89523060,
+0x666CE6DC, 0x894E3060, 0x666CE6DD, 0x894A3060,
+0x666CE6F0, 0x89463060, 0x666CE6F1, 0x89423060,
+0x666CE6F2, 0x893E3060, 0x666CE6F3, 0x893A3060,
+0x666CE6F4, 0x89363060, 0x666CE6F5, 0x89323060,
+0x666CE6F6, 0x892E3060, 0x666CE6F7, 0x892A3060,
+0x4608E650, 0x89263060, 0x3060969A, 0x96988923,
+0x89203060, 0x30609696, 0x9694891D, 0x891A3060,
+0x30609692, 0x96908917, 0x89143060, 0x3060968E,
+0x968C8911, 0x890E3060, 0x3060968A, 0x9688890B,
+0x89083060, 0x30609686, 0x96848905, 0x89023060,
+0x30609682, 0x000B8B01, 0xE0FFE040, 0x600C000B,
+0xE000000B, 0x6243D157, 0xE4028512, 0x662D670D,
+0xE500A00E, 0x6053655D, 0x305C4000, 0x4008D152,
+0x622D021D, 0x8B023260, 0xA0047108, 0x7501041C,
+0x3273625D, 0x60438BEE, 0xC90A000B, 0x674C76FE,
+0x025C606C, 0x3723622C, 0x20088906, 0x70FF8902,
+0x6603AFF6, 0xE000000B, 0x0009000B, 0x4F124F22,
+0x326052F2, 0x34508910, 0x3470890E, 0x3750890D,
+0x3268890A, 0x04273458, 0x60733758, 0x440BD43B,
+0x306C011A, 0x6203A001, 0x4F166263, 0x000B4F26,
+0x2FE66023, 0x4F124F22, 0x6E434F02, 0x614C54F4,
+0x2F164118, 0x666C677C, 0x64EC655C, 0x46184718,
+0xBFD34518, 0x65034418, 0x60537F04, 0xC980E702,
+0x6E034718, 0x37ED4728, 0x62594519, 0x010A652E,
+0x312C225D, 0x4F06601C, 0x4F264F16, 0x6EF6000B,
+0x03400240, 0x05400440, 0x07400640, 0x09400840,
+0x11400B40, 0x0A401240, 0x4F220A50, 0x614C8451,
+0x3127620C, 0xA00C8901, 0x8452E400, 0x3127620C,
+0xA0068901, 0x8453E401, 0x3127620C, 0xE4038D01,
+0x6263E402, 0x60437201, 0x677C072C, 0x62532F76,
+0x072C7201, 0x055C066C, 0x666C677C, 0xBFA8655C,
+0x7F046413, 0x000B4F26, 0x605C600C, 0x8F068801,
+0x606C6243, 0x8B018801, 0x720AA001, 0x000B72F6,
+0x00006023, 0x00114000, 0x00114008, 0x00203374,
+0xE040D690, 0x056E614C, 0x9274D78F, 0x352C357C,
+0xE400E718, 0x626C6650, 0x89043120, 0x624C7401,
+0x8FF73273, 0x000B7501, 0xE2FF6043, 0x622C644C,
+0x890D3420, 0x8801605C, 0x965D8B03, 0xA005346C,
+0x62436243, 0x324C4208, 0x326C9657, 0x6023000B,
+0x6043000B, 0x2F962F86, 0x2FB62FA6, 0x2FD62FC6,
+0x4F222FE6, 0x92497FF4, 0x6B533526, 0x00296943,
+0xE13FCA01, 0x6E03EAFF, 0x6AAC2F10, 0x6D43EC00,
+0x62D0E808, 0x34A0642C, 0xBFCE8939, 0x3B0065E3,
+0x6CCC8F0A, 0x420062C3, 0x362C6693, 0x1FC18461,
+0x4109610C, 0x2F10A02D, 0x891C2CC8, 0x65E364D0,
+0x644CBFBB, 0x89163B02, 0x70FF60C3, 0x049C4000,
+0x644C65E3, 0x1F02BFB1, 0x8D1A30B2, 0x56F21FC1,
+0x356C6593, 0xC9038451, 0x89122008, 0x660C8451,
+0xA00E4609, 0x7C012F60, 0x328362CC, 0x8FC87D02,
+0xA0061F21, 0x06250009, 0x12C008FC, 0x62CC09B4,
+0x50F11F21, 0x8B128808, 0x7CFF6CCC, 0x60C34C00,
+0x65E3049C, 0x644CBF89, 0x8B083B06, 0x849139CC,
+0x2008C903, 0x84918903, 0x4209620C, 0x60F02F20,
+0x4F267F0C, 0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6,
+0x68F6000B, 0x2F962F86, 0x2FB62FA6, 0x2FD62FC6,
+0x4F222FE6, 0x92727FFC, 0x3426E100, 0x6B436953,
+0x2F12666C, 0xCA010029, 0x8D032668, 0xE2F06E03,
+0x2F22622C, 0x6AACEAFF, 0x6C93ED00, 0x66C0E808,
+0x34A0646C, 0xBF508913, 0x3B0065E3, 0x6DDC8B0A,
+0x39DC4D00, 0xC9038491, 0x8B082008, 0xCB0F60F2,
+0x2F02A005, 0x62DC7D01, 0x8FE83283, 0x60F27C02,
+0x4F267F04, 0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6,
+0x68F6000B, 0x2F962F86, 0x2FB62FA6, 0x2FD62FC6,
+0x4F222FE6, 0x4F024F12, 0x3F3C9332, 0x4308E35B,
+0x605333FC, 0x80341351, 0xE7606063, 0x80381362,
+0x4708E012, 0xE03F8136, 0xD11237FC, 0x27008138,
+0x80788074, 0xE9166053, 0x60638012, 0x21414918,
+0x6B938013, 0xEDFF6AB4, 0x6AAC61B0, 0x6C1C4A18,
+0x68C82CAB, 0x6DDD688D, 0x234238D0, 0x8B131398,
+0xD207D406, 0x0009420B, 0x432BD306, 0x09B40009,
+0x0000FE10, 0x001142D8, 0x000DDD00, 0x001160B0,
+0x002018C0, 0x00115E88, 0x342292E3, 0x8F02E100,
+0xA1616593, 0x92DD0009, 0x352CE7FF, 0xEE04677C,
+0x622C6250, 0x89043270, 0x621C7101, 0x8FF732E3,
+0xE8FC7501, 0x3488688C, 0x9ACBE064, 0x40086893,
+0x0F4438AC, 0x661C6583, 0x644CBE18, 0x64E36E0C,
+0x65E37401, 0x45086643, 0x35EC4608, 0x4508364C,
+0x45004608, 0x369C4600, 0x61A39AB5, 0xE0656763,
+0x400837AC, 0x62637114, 0x321C0F76, 0x94AB7004,
+0x61430F26, 0x359C6263, 0x7004324C, 0x0F267114,
+0x7004361C, 0x0F666753, 0x700437AC, 0x7A140F76,
+0x37AC6753, 0x66537004, 0x364C0F76, 0x74147004,
+0x354C0F66, 0x0F567004, 0x395C958F, 0xED006A93,
+0x6BD3E956, 0xEC054908, 0x4008E065, 0x60B302FE,
+0x644C042C, 0x60E32F46, 0xE06A07AC, 0x01FE4008,
+0x061C60B3, 0x058C60E3, 0x4008E065, 0x677C01FC,
+0x655C666C, 0x641CBDED, 0x7F046403, 0x60D36DDC,
+0xE0660F44, 0x04FE4008, 0x054C60B3, 0x2F56655C,
+0x07AC60E3, 0x4008E06B, 0x60B301FE, 0x60E3061C,
+0xE065058C, 0x01FC4008, 0x666C677C, 0xBDD0655C,
+0x6403641C, 0x65F37F04, 0x60D37510, 0xE0650544,
+0x07FE4008, 0x057C60C3, 0x2F56655C, 0x07AC60E3,
+0x4008E06A, 0x60C301FE, 0x60E3061C, 0xE065058C,
+0x01FC4008, 0x666C677C, 0xBDB2655C, 0x6403641C,
+0x61F37F04, 0x60D37120, 0xE0660144, 0x02FE4008,
+0x052C60C3, 0x2F56655C, 0x07AC60E3, 0x4008E06B,
+0x60C301FE, 0x60E3061C, 0xE065058C, 0x01FC4008,
+0x666C677C, 0xBD94655C, 0x6503641C, 0x64F37F04,
+0x60D3349C, 0xE0670454, 0x07FE4008, 0x057C60B3,
+0x2F56655C, 0x07AC60E3, 0x4008E06C, 0xA00501FE,
+0x0BB860B3, 0x03C2013E, 0x013F0462, 0x60E3061C,
+0xE065058C, 0x01FC4008, 0x666C677C, 0xBD70655C,
+0x6203641C, 0xE1B87F04, 0x64F3611C, 0x60D3341C,
+0xE0680424, 0x05FE4008, 0x075C60B3, 0x2F76677C,
+0x07AC60E3, 0x4008E06D, 0x60B301FE, 0x60E3061C,
+0xE065058C, 0x02FC4008, 0x666C677C, 0xBD50655C,
+0x6703642C, 0xE2C07F04, 0x66F3622C, 0x60D3362C,
+0xE0670674, 0x07FE4008, 0x027C60C3, 0x2F26622C,
+0x07AC60E3, 0x4008E06C, 0x60C302FE, 0x60E3062C,
+0xE065058C, 0x02FC4008, 0x666C677C, 0xBD30655C,
+0x6203642C, 0xE7C87F04, 0x66F3677C, 0x60D3367C,
+0xE0680624, 0x06FE4008, 0x026C60C3, 0x2F26622C,
+0x07AC60E3, 0x4008E06D, 0x60C302FE, 0x60E3062C,
+0xE065058C, 0x02FC4008, 0x666C677C, 0xBD10655C,
+0x6103642C, 0x66937F04, 0x62F37608, 0x60D3326C,
+0x02147D01, 0xE60562DC, 0x7C013263, 0x7B018D02,
+0x0009AEFA, 0x0009A17B, 0xE7FF9BD5, 0x677C35BC,
+0x6250EE08, 0x3270622C, 0x71018904, 0x32E3621C,
+0x75018FF7, 0xDDD89CC8, 0x3D4534C8, 0x4008E064,
+0x4E090E0A, 0x0FE46593, 0x702435BC, 0x64EC661C,
+0x0F56BCB4, 0x64E36E0C, 0x65E37401, 0x45086243,
+0x35EC4208, 0x4508324C, 0x45004208, 0x329C4200,
+0x61B37B0C, 0x38BC6823, 0x7114E06E, 0x40086B23,
+0x91A23B1C, 0x70040F86, 0x68236413, 0x0FB6359C,
+0x7004381C, 0x0F867414, 0x7004342C, 0x67539896,
+0x0F466253, 0x7004378C, 0x6B537814, 0x7114321C,
+0x3B8C0F76, 0x351C7004, 0x0FB69789, 0x397C7004,
+0x70040F26, 0xED006893, 0x0F56EC05, 0x6AD3E956,
+0xE06E4908, 0x02FE4008, 0x012C60A3, 0x2F16611C,
+0x078C60E3, 0x4008E073, 0x60A304FE, 0xE06E064C,
+0x0BFE4008, 0x05BC60E3, 0x4008E065, 0x677C01FC,
+0x655C666C, 0x641CBC85, 0x7F046403, 0x60D36DDC,
+0xE06F0F44, 0x04FE4008, 0x054C60A3, 0x2F56655C,
+0x078C60E3, 0x4008E074, 0x60A30BFE, 0xE06E06BC,
+0x0BFE4008, 0x05BC60E3, 0x4008E065, 0x677C01FC,
+0x655C666C, 0x641CBC65, 0x7F046403, 0x751065F3,
+0x054460D3, 0x4008E06E, 0x60C307FE, 0x655C057C,
+0x60E32F56, 0xE073078C, 0x0BFE4008, 0x06BC60C3,
+0x4008E06E, 0x60E301FE, 0xE065051C, 0x0BFC4008,
+0x666C677C, 0xBC44655C, 0x610364BC, 0x6BF37F04,
+0x60D37B20, 0xE06F0B14, 0x01FE4008, 0x041C60C3,
+0x2F46644C, 0x078C60E3, 0x4008E074, 0x60C301FE,
+0xE06E061C, 0x0BFE4008, 0x05BC60E3, 0x4008E065,
+0xA00501FC, 0x0136677C, 0x028212C0, 0x01370142,
+0x655C666C, 0x641CBC1D, 0x7F046203, 0x349C64F3,
+0x042460D3, 0x4008E070, 0x60A304FE, 0x655C054C,
+0x60E32F56, 0xE075078C, 0x0BFE4008, 0x06BC60A3,
+0x4008E06E, 0x60E301FE, 0xE065051C, 0x0BFC4008,
+0x666C677C, 0xBBFC655C, 0x610364BC, 0xEBB87F04,
+0x65F36BBC, 0x60D335BC, 0xE0710514, 0x07FE4008,
+0x047C60A3, 0x2F46644C, 0x078C60E3, 0x4008E076,
+0x60A30BFE, 0xE06E06BC, 0x01FE4008, 0x051C60E3,
+0x4008E065, 0x677C0BFC, 0x655C666C, 0x64BCBBD9,
+0x7F046103, 0x622CE2C0, 0x3B2C6BF3, 0x0B1460D3,
+0x4008E070, 0x60C302FE, 0x677C072C, 0x60E32F76,
+0xE075078C, 0x02FE4008, 0x062C60C3, 0x4008E06E,
+0x60E302FE, 0xE065052C, 0x02FC4008, 0x666C677C,
+0xBBB6655C, 0x6703642C, 0xEBC87F04, 0x66F36BBC,
+0x60D336BC, 0xE0710674, 0x06FE4008, 0x026C60C3,
+0x2F26622C, 0x078C60E3, 0x4008E076, 0x60C302FE,
+0xE06E062C, 0x02FE4008, 0x052C60E3, 0x4008E065,
+0x677C02FC, 0x655C666C, 0x642CBB93, 0x7F046103,
+0x72086293, 0x362C66F3, 0x7D0160D3, 0x62DC0614,
+0x3263E605, 0x8D027C01, 0xAEE27A01, 0x6EF30009,
+0xE2B068F3, 0x6AF3E05A, 0x389C7E18, 0x69F3622C,
+0x7A084008, 0x67F36DF3, 0x392C61F3, 0x6CA30FE6,
+0x77207D10, 0xE4007128, 0xEB0565F3, 0x604C6654,
+0x66D4626C, 0x2E604221, 0x048C6674, 0x626C2C20,
+0x09444221, 0x21207001, 0x32B3620C, 0x71016403,
+0x8FEB7E01, 0xE05A7C01, 0x6EF34008, 0x7E300BFE,
+0xEC19ED00, 0x66B365A3, 0xBB7E64DC, 0x62DC7D01,
+0x2E0032C3, 0x7E018FF6, 0x666CE6B0, 0x6BF36EF3,
+0x7B283E6C, 0xEC4CA010, 0xCCCCCCCD, 0x64D36DDC,
+0x644C74F4, 0xBB6865B3, 0x67F366E3, 0x77306503,
+0x075460D3, 0x62DC7D01, 0x8BEF32C3, 0x7B306BF3,
+0x61B367B3, 0xED8064B3, 0x71027701, 0x6DDC7403,
+0xDC37E500, 0x605CDE37, 0x091C084C, 0x0A7C668C,
+0x699C4628, 0x49284618, 0x05BC6AAC, 0x4A18269B,
+0x70046803, 0x655C26AB, 0x620C38CC, 0x38EC265B,
+0x286232D3, 0x65038FE7, 0x644CE4B8, 0x3C4C6CF3,
+0x6EF37408, 0xE2B0E658, 0x3E4C6AF3, 0x74086BF3,
+0x460861F3, 0x622C68F3, 0x7A0869F3, 0x314C7B18,
+0x386C64F3, 0x6DA3392C, 0x742867B3, 0x66C4E500,
+0x626C605C, 0x422166E4, 0x66142760, 0x2D20058C,
+0x4221626C, 0x70010954, 0x620C2420, 0x3263E605,
+0x74016503, 0x8FEA7701, 0xE05E7D01, 0x02FD4008,
+0x6D2DE9D0, 0x699C7D07, 0xEE00A00B, 0x66B365A3,
+0x64ECBAFB, 0x620367F3, 0x60EC379C, 0x70010724,
+0x62EC6E03, 0x8BF132D3, 0x4008E05F, 0xEAB008FD,
+0x6DF36AAC, 0x6BF36C8D, 0x7C0D3DAC, 0x7B28A012,
+0x0000A280, 0x001BC000, 0x64E36EEC, 0x644C74F4,
+0xBADA65B3, 0x62F366D3, 0x329C6103, 0x7E0160E3,
+0x62EC0214, 0x8BEF32C3, 0x3D9C6DF3, 0x67D36ED3,
+0xEC8061D3, 0x77027E01, 0x6CCC7103, 0xDBB9E400,
+0x604CDAB9, 0x067C041C, 0x08EC654C, 0x666C4528,
+0x46284518, 0x09DC688C, 0x4818256B, 0x70046603,
+0x699C258B, 0x620C36BC, 0x36AC259B, 0x265232C3,
+0x64038FE7, 0x4008E064, 0x70E007FC, 0x706C0CFC,
+0x0F8668CC, 0x0DFC7098, 0x6ADC706C, 0x708C0FA6,
+0x9BBF0EFE, 0xE2543EB2, 0x697CE100, 0x42088F02,
+0x0009A163, 0x4008E063, 0x6EF305FE, 0x3E2C96B3,
+0x64E3356C, 0xEDFFE703, 0x32D06250, 0x622C8D07,
+0x681C7101, 0x24203873, 0x8FF57505, 0xE0647401,
+0x0AFC4008, 0x64AC65E3, 0x661CBA18, 0xE063670C,
+0x62734008, 0x42080BFE, 0x7701327C, 0x3A2C6AB3,
+0x48086873, 0x948F6EA3, 0x3E4C387C, 0x3B8C74FF,
+0x38283A4C, 0xEC003B4C, 0x6083DD88, 0x655C05EC,
+0xE0652F56, 0x67B04008, 0x65A066E4, 0x677C01FC,
+0x655C666C, 0x641CBA1D, 0x7C017F04, 0xE40462CC,
+0x2D003243, 0x7D018FE9, 0xE063E554, 0x67F34508,
+0x375C4008, 0x966805FE, 0x356CEDFF, 0x6DDC6473,
+0xEE04E100, 0x666C6650, 0x890636D0, 0x621C7101,
+0x246032E3, 0x8FF57505, 0xE0647401, 0x02FC4008,
+0x642C6573, 0x661CB9CA, 0x6E23620C, 0xE0634E08,
+0x72013E2C, 0x0BFE4008, 0x47086723, 0x6AB3372C,
+0x68733AEC, 0x6EA338E8, 0x3E1C9140, 0x3B7C71FF,
+0x3B1C3A1C, 0x0F96704C, 0xEC00E904, 0x6083DD60,
+0x644C04EC, 0xE0652F46, 0x67B04008, 0x65A066E4,
+0x677C01FC, 0x655C666C, 0x641CB9CB, 0x7C017F04,
+0x329362CC, 0x8FEA2D00, 0xE0767D01, 0x09FE4008,
+0x70B4E454, 0x67F34408, 0x374C05FE, 0xEDFF9617,
+0x356C6473, 0xE1006DDC, 0x6650EE04, 0x36D0666C,
+0x71018906, 0x32E3621C, 0x75092460, 0x74018FF5,
+0xE064A006, 0x05BA0BB8, 0x05C905BB, 0x05DD05CA,
+0x65734008, 0x661C07FC, 0x647CB970, 0x6623620C,
+0x4608E063, 0x46004008, 0x362C0BFE, 0x68237201,
+0x3A6C6AB3, 0x48004808, 0x91676EA3, 0x3E1C382C,
+0x3B8C71FF, 0x38683A1C, 0xEC003B1C, 0x6083DD35,
+0x644C04EC, 0xE0652F46, 0x67B04008, 0x65A066E4,
+0x677C01FC, 0x655C666C, 0x641CB973, 0x7C017F04,
+0xE50862CC, 0x2D003253, 0x7D018FE9, 0x4008E063,
+0x05FEE654, 0x64F34608, 0xECFF9741, 0x357C346C,
+0xEE006CCC, 0x6250ED04, 0x32C0622C, 0x7E018906,
+0x38D368EC, 0x75092420, 0x74018FF5, 0x4008E077,
+0x700405FE, 0x649306FE, 0xEA54B9A7, 0x65F34A08,
+0x640C35AC, 0x66ECB91A, 0x6613610C, 0x4608E063,
+0x46004008, 0x361C0BFE, 0x68137101, 0x3A6C6AB3,
+0x48004808, 0x92136EA3, 0x3E2C381C, 0x3B8C72FF,
+0x38683A2C, 0xEC003B2C, 0xE077DD0B, 0x05FE4008,
+0x06FE7004, 0x6493B981, 0x0009A010, 0x060105DE,
+0x00000602, 0x0000B280, 0x001BC000, 0x001142E4,
+0x001142E8, 0x001142ED, 0x001142F5, 0x60836403,
+0x677C07EC, 0x67B02F76, 0x65A066E4, 0x666C677C,
+0xB906655C, 0x7F04644C, 0x61CC7C01, 0x3123E208,
+0x8FD22D00, 0xA0FC7D01, 0xE0630009, 0x05FE4008,
+0x96D067F3, 0x356C372C, 0xEEFF6473, 0x32E06250,
+0x622C8D08, 0x681C7101, 0x3863E608, 0x75052420,
+0x74018FF4, 0x4008E064, 0x657302FC, 0xB8B5642C,
+0x650C661C, 0x4008E063, 0x0BFE6253, 0x325C4208,
+0x6AB37501, 0x68533A2C, 0x6EA34808, 0x385C94AC,
+0x74FF3E4C, 0x3A4C3B8C, 0x3B4C3828, 0xDD96EC00,
+0x06EC6083, 0x2F66666C, 0x4008E065, 0x66E467B0,
+0x01FC65A0, 0x666C677C, 0xB8BA655C, 0x7F04641C,
+0x62CC7C01, 0x3243E404, 0x8FE92D00, 0xE5547D01,
+0x4508E063, 0x400867F3, 0x05FE375C, 0xEEFF9685,
+0x6473356C, 0xE1006EEC, 0x666C6650, 0x890736E0,
+0x621C7101, 0x3283E808, 0x75092460, 0x74018FF4,
+0x4008E064, 0x65730AFC, 0xB86764AC, 0x620C661C,
+0xE0636623, 0x40084608, 0x0BFE4600, 0x7201362C,
+0x6AB36823, 0x48083A6C, 0x6EA34800, 0x382C915E,
+0x71FF3E1C, 0x3A1C3B8C, 0x3B1C3868, 0xDD6FEC00,
+0x04EC6083, 0x2F46644C, 0x4008E065, 0x66E467B0,
+0x01FC65A0, 0x666C677C, 0xB86A655C, 0x7F04641C,
+0x62CC7C01, 0x3253E508, 0x8FE92D00, 0xE0637D01,
+0xE6544008, 0x460805FE, 0x973864F3, 0x346CECFF,
+0x6CCC357C, 0xED08EE00, 0x666C6650, 0x890636C0,
+0x62EC7E01, 0x246032D3, 0x8FF57509, 0xE0777401,
+0x05FE4008, 0x06FE7004, 0xB89E6493, 0x4808E854,
+0x358C65F3, 0xB811640C, 0x610C66EC, 0xE0636613,
+0x40084608, 0x0BFE4600, 0x7101361C, 0x6AB36813,
+0x48083A6C, 0x6EA34800, 0x381C920A, 0x72FF3E2C,
+0xA0063B8C, 0x05023A2C, 0x052A0503, 0x0572052B,
+0x38680573, 0xEC003B2C, 0xE077DD41, 0x05FE4008,
+0x06FE7004, 0x6493B871, 0x60836403, 0x677C07EC,
+0x67B02F76, 0x65A066E4, 0x666C677C, 0xB808655C,
+0x7F04644C, 0x61CC7C01, 0x3123E208, 0x8FE42D00,
+0xD3347D01, 0x0009430B, 0xE079620C, 0x0F244008,
+0x88306023, 0xA24D8B01, 0x88400009, 0xA2498B01,
+0x22280009, 0xA2458B01, 0xE5FF0009, 0x655CD42A,
+0xE03AE601, 0x8F043250, 0xE0790464, 0x4008E210,
+0xE05B0F24, 0x05FE4008, 0x3566963B, 0xA1498B01,
+0x60230009, 0x640CCB01, 0x6E23B842, 0xE118660C,
+0x890F3613, 0x4008E063, 0x04FE4608, 0x97294608,
+0x460070E0, 0x05FE347C, 0x346CB85C, 0xE0606203,
+0x0F244008, 0xCB0260E3, 0x640CB82A, 0xE118660C,
+0x890F3613, 0x4008E063, 0x04FE4608, 0x91114608,
+0x460070E0, 0x05FE341C, 0x346CB844, 0xE0616203,
+0x0F244008, 0xCB0560E3, 0x640CB812, 0xA00D660C,
+0x09B4E07A, 0x0000064D, 0x001142FD, 0x00114301,
+0x00114309, 0x00114400, 0x001142D8, 0x4008E118,
+0x8F043613, 0xE0610F64, 0xA0104008, 0xE07A0DFC,
+0x06FC4008, 0x626C70A4, 0x04FE4208, 0x97B44208,
+0x420070E0, 0x05FE347C, 0x342CB814, 0xE0796D03,
+0x00FC4008, 0xCB07DB8E, 0x430BD38E, 0x610C640C,
+0x4008E07A, 0x709C0F14, 0xE61802FC, 0x8D1C3163,
+0xE05D682C, 0x01FC4008, 0x09FC70FC, 0x04FE70FC,
+0xD385661C, 0x659C430B, 0xE07A6503, 0x01FC4008,
+0x611C70A4, 0x04FE4108, 0x97864108, 0x347C4100,
+0x430BD37E, 0xA003341C, 0xE0616C03, 0x0CFC4008,
+0xE500D67B, 0x640D8562, 0x4008E05B, 0x0AFEA036,
+0x6053655C, 0x305C4000, 0x4008D676, 0x622D026D,
+0x8F2A32A0, 0xD3746E03, 0x64AD430B, 0x2228620D,
+0xD6728927, 0x066C60E3, 0x4008E060, 0x460002FC,
+0x3E676E2C, 0x62638B00, 0x4008E060, 0x0F243867,
+0x62638D03, 0x4008E061, 0xE06102FC, 0x400861DC,
+0x0F243167, 0x8F01682C, 0x626362D3, 0x346764CC,
+0x6D238D01, 0xA00466C3, 0x75016C63, 0x3243625C,
+0xE0608BC6, 0x07FC4008, 0x617CE400, 0xE904D55C,
+0x666C6650, 0x8B013617, 0x6673677C, 0x624C7401,
+0x25603293, 0x75018FF4, 0xE03AD656, 0xE400056C,
+0x8D012558, 0xE2026243, 0x4008E061, 0x67830EFC,
+0x3E283828, 0x9119E500, 0x6053655C, 0x3A1002BC,
+0x622C8D0B, 0x3A609613, 0x32778907, 0xE0618B02,
+0x02FC4008, 0xA01D6053, 0x25580B24, 0x32878908,
+0x62838B00, 0xA0156053, 0x064D0B24, 0x099E096C,
+0x8F083277, 0xE07B6623, 0x0F164008, 0x02FC7098,
+0x01FE7068, 0x626C662C, 0x32876053, 0x0B648F02,
+0x646336E8, 0x625C7501, 0x8BCD3293, 0xE014D635,
+0xE4000644, 0xD53461DC, 0x6250E708, 0x3217622C,
+0x6DDC8B01, 0x740162D3, 0x3673664C, 0x8FF42520,
+0xE4007501, 0xD52D61CC, 0x622C6250, 0x8B013217,
+0x62C36CCC, 0x664C7401, 0x25203673, 0x75018FF4,
+0x0009A0EC, 0x4008E079, 0x642C02FC, 0x430BD319,
+0x660C6E43, 0x3653E518, 0xE0638910, 0x46084008,
+0x460804FE, 0x70E09722, 0x347C4600, 0xD31305FE,
+0x346C430B, 0xE0626203, 0x0F244008, 0xCB0660E3,
+0x430BD30C, 0x660C6403, 0x3653E518, 0xE0638928,
+0x46084008, 0x460804FE, 0x70E09708, 0x347C4600,
+0xD30605FE, 0x346C430B, 0x6C03A01D, 0x0000064D,
+0x001142E8, 0x001148E0, 0x001148BA, 0x00114934,
+0x00114000, 0x00114008, 0x00114774, 0x00114011,
+0x001142E4, 0x001142D8, 0x001142ED, 0x001142F5,
+0x4008E062, 0x60E30CFC, 0xD39CCB08, 0x6403430B,
+0xE07A610C, 0x4008E618, 0x8D1C3163, 0xE05D0F14,
+0x07FC4008, 0x09FC70FC, 0x04FE70FC, 0xD394667C,
+0x659C430B, 0xE07A6503, 0x01FC4008, 0x611C70A4,
+0x04FE4108, 0x9D744108, 0x34DC4100, 0x430BD38D,
+0xA003341C, 0xE0626D03, 0x0DFC4008, 0xE500D68A,
+0x640D8562, 0x4008E05B, 0x01FEA02C, 0x6053655C,
+0x305C4000, 0x4008D685, 0x622D026D, 0x8F203210,
+0xD3836E03, 0x641D430B, 0x2228620D, 0xD681891D,
+0x066C60E3, 0x4008E062, 0x460002FC, 0x3167612C,
+0x62638B00, 0x64CCE062, 0x34674008, 0x8F010F24,
+0x626362C3, 0x356765DC, 0x6C238D01, 0xA00466D3,
+0x75016D63, 0x3243625C, 0xE0628BD0, 0x07FC4008,
+0x617CE400, 0xE904D570, 0x622C6250, 0x8B013217,
+0x6273677C, 0x664C7401, 0x25203693, 0x75018FF4,
+0x61CCE400, 0xE708D569, 0x666C6650, 0x8B013617,
+0x66C36CCC, 0x624C7401, 0x25603273, 0x75018FF4,
+0x61DCE400, 0x6650D562, 0x3617666C, 0x6DDC8B01,
+0x740166D3, 0x3273624C, 0x8FF42560, 0xA0057501,
+0x064D0009, 0xE200D65B, 0x0624E03A, 0xE03AD659,
+0x2228026C, 0xE039894B, 0x2228026C, 0xE05B8947,
+0x0EFE4008, 0x3E669690, 0xE0798941, 0x00FC4008,
+0x8D023E66, 0xCB02640C, 0xD344640C, 0x0009430B,
+0xE05C660C, 0x07FC4008, 0x4608701C, 0x617C05FE,
+0x977A4608, 0x357C4600, 0x6613356C, 0x430BD346,
+0xD54464E3, 0x62032008, 0x0029150F, 0x6603CA01,
+0x2668E03B, 0x05648D20, 0xC8F06023, 0xD53F8909,
+0x76FF6650, 0x84512560, 0x805170FF, 0x70FF8452,
+0x60238052, 0x890FC80F, 0x6260D639, 0x26207201,
+0x70018461, 0x84628061, 0xA0057001, 0xD6318062,
+0xE03BE200, 0x162F0624, 0x4008E05B, 0x964302FE,
+0x8B653266, 0xD72BD428, 0xD52E6040, 0x4028C93F,
+0x40084008, 0x50726203, 0xC802D12B, 0xE604891A,
+0x46284618, 0x2522226B, 0xE2086040, 0x6503C93F,
+0x66034508, 0x45004508, 0x46284218, 0x6263252B,
+0x42084208, 0x252B4200, 0x4218E208, 0x252B4228,
+0x2152A062, 0x4618E614, 0x226B4628, 0x60402522,
+0xC93FE428, 0x45086503, 0x45084028, 0x45004008,
+0x40084418, 0x254BE728, 0x47184000, 0x4728250B,
+0xD412257B, 0x2152A044, 0x064D09B4, 0x001148E0,
+0x001148BA, 0x00114934, 0x00114000, 0x00114008,
+0x00114774, 0x00114011, 0x001142FD, 0x00114301,
+0x00114309, 0x001142D8, 0x00114A24, 0x001142F5,
+0x001142ED, 0x001C3694, 0x001C3BB4, 0x001142E8,
+0xE214D429, 0x42186040, 0x4028C93F, 0x40084008,
+0xD6264228, 0x2602202B, 0xE7286040, 0x6503C93F,
+0x45084508, 0x45004028, 0x40084718, 0x4008257B,
+0x4000E728, 0x250B4718, 0xD21D4728, 0x2252257B,
+0xD71C6240, 0x0724E044, 0x3F3C932C, 0x4F164F06,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0x2FE668F6, 0x6243D114, 0xE4028512, 0x6E2D670D,
+0xE500A00F, 0x6053655D, 0x305C4000, 0x4008D10F,
+0x622D021D, 0x8B0332E0, 0x041C7108, 0x644CA004,
+0x625D7501, 0x8BED3273, 0x4618E602, 0x604D2469,
+0x6EF6000B, 0x000001F0, 0x001142E8, 0x001C3694,
+0x001C3BB4, 0x001142D8, 0x00114000, 0x00114008,
+0xD766D565, 0x62725151, 0x321CE340, 0x51522722,
+0x337C5271, 0x1721321C, 0x52725153, 0x321C644C,
+0x1722D15F, 0x66125255, 0x2162362C, 0x316C5173,
+0x61521713, 0xD65B5274, 0x1724321C, 0x52755154,
+0x1725321C, 0x52765158, 0x1726321C, 0x51776262,
+0x1717312C, 0x51785261, 0x1718312C, 0x51795262,
+0x1719312C, 0x517A5263, 0x171A312C, 0x517B5264,
+0x171B312C, 0x517C5265, 0x171C312C, 0x517D5266,
+0x171D312C, 0x517E5267, 0x171E312C, 0x527F5168,
+0x321CD645, 0x6262172F, 0x76946132, 0x2312312C,
+0x52316162, 0x321CD641, 0x515C1321, 0x351C5532,
+0x61621352, 0x41295235, 0x1325321C, 0x56365561,
+0x365C4529, 0x1366E538, 0x55312450, 0x71046143,
+0x66722152, 0x75086543, 0x56712562, 0x750C6543,
+0x56722562, 0x75106543, 0x56752562, 0x75146543,
+0x56732562, 0x75186543, 0x56762562, 0x751C6543,
+0x56322562, 0x75206543, 0x66322562, 0x75246543,
+0x56742562, 0x75286543, 0x56342562, 0x752C6543,
+0x55332562, 0x72306243, 0x55352252, 0x72346243,
+0x56362252, 0x24627438, 0x1341E400, 0x17412742,
+0x17451742, 0x17461743, 0x23421342, 0x13441744,
+0x13451343, 0x1346000B, 0xD510E124, 0x51572410,
+0x52581411, 0x57591422, 0x515A1473, 0x525B1414,
+0x575C1425, 0x525D1476, 0x1427E700, 0x1468565E,
+0x1469565F, 0x15781577, 0x157A1579, 0x157C157B,
+0x157E157D, 0x157F000B, 0x001C369C, 0x0020351C,
+0x00203578, 0x001C3CA0, 0x001C36F4, 0x001C3B88,
+0x6E726157, 0x21676E69, 0x69685420, 0x6F642073,
+0x656C676E, 0x746F6E20, 0x65656220, 0x6163206E,
+0x7262696C, 0x64657461, 0x0000000A, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+};
+
+const u32_t zcFwBufImageSize=83968;
diff --git a/drivers/staging/otus/hal/hpfwspiu.c b/drivers/staging/otus/hal/hpfwspiu.c
new file mode 100644
index 000000000000..eda7ff5e573c
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwspiu.c
@@ -0,0 +1,655 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+
+const u32_t zcFwImageSPI[]={
+0x0009000B, 0x4F222FE6, 0xB0187FFC, 0xE6000009,
+0x943DD520, 0xC8406052, 0x2F028F03, 0x8FF93642,
+0xD41D7601, 0x4E0BDE1D, 0xD41D0009, 0x00094E0B,
+0x4E0BD41C, 0x7F040009, 0xA0214F26, 0x4F226EF6,
+0xE205D119, 0x2122E400, 0x92222142, 0x8BFD4210,
+0x450BD516, 0xD6160009, 0x0009460B, 0xE5FFD715,
+0x2752655D, 0xE1FFD714, 0xD4145079, 0x1709CB01,
+0x17112712, 0x2412E101, 0x4F26D411, 0x2410000B,
+0xDE11DD10, 0x00094D0B, 0x00094E0B, 0x0009AFFA,
+0x03E82710, 0x001C001C, 0x00116594, 0x00114EBE,
+0x001165A4, 0x001165BC, 0x001D4004, 0x00114FA0,
+0x00114378, 0x001C3510, 0x001C3624, 0x001E212C,
+0x001164FC, 0x00114700, 0x0011589C, 0x2FA62F96,
+0x2FC62FB6, 0x2FE62FD6, 0x7FC84F22, 0xD28DDD8C,
+0x61D360D0, 0x80F47101, 0x420B6010, 0x200880F8,
+0x6E038F10, 0xDB89D488, 0xD4896A40, 0x4B0BDC89,
+0x67C065AC, 0x697CDE88, 0x41086193, 0x31984108,
+0x3E1C4108, 0x66D284F8, 0x2008600C, 0x2E628F13,
+0xE40084F4, 0xDA81670C, 0x3273624D, 0xA0D38B01,
+0x644D0009, 0x35AC6543, 0x69436652, 0x39EC6B62,
+0xAFF119B1, 0x88017404, 0x84F48B15, 0x2E70E700,
+0xDA766E0C, 0x32E3627D, 0xA0C48B01, 0x677D0009,
+0x6C7366A3, 0x65737604, 0x356C3CAC, 0x6D5264C2,
+0xAFEF7708, 0xE2B024D2, 0x3020622C, 0x84F48B30,
+0x650CEC00, 0xDA691F53, 0x55F3E904, 0x325362CD,
+0xA0A88B01, 0x6CCD0009, 0x67C36EA3, 0x6BC37E04,
+0x3BEC37AC, 0x6EB26D72, 0xDB62D461, 0x00094B0B,
+0x410BD161, 0xD46164D3, 0x00094B0B, 0x450BD55E,
+0xD45F64E3, 0x00094B0B, 0x61D3E600, 0x316C666D,
+0x646D7601, 0x21E03493, 0x4E198FF7, 0x7C08AFD5,
+0x622CE2B1, 0x8B113020, 0xD552D456, 0xDA56DC4F,
+0x0009450B, 0x4A0BE400, 0xD75467C2, 0x470BDB52,
+0x4B0B0009, 0xE900E403, 0x2E90A06D, 0x622CE2B2,
+0x89683020, 0x622CE2B3, 0x8B1D3020, 0xDA45D44C,
+0x4A0BD942, 0x65960009, 0x6792D44A, 0x1F74DD3B,
+0x1F5D4D0B, 0xD639D448, 0x460BDB48, 0x55F455F4,
+0x4B0BD936, 0xD44654FD, 0x490B6503, 0x5DF51F05,
+0x1ED1EC04, 0x2EC0A047, 0x622CE2B4, 0x8B3E3020,
+0xDA34D440, 0x4A0BDD31, 0x84F40009, 0x600C6CD2,
+0x1F072F02, 0x1FC6C903, 0xE6001F08, 0xD73AE030,
+0x6CF2DB3A, 0x1F790F65, 0xA0211FBA, 0x51F6E904,
+0x6D63666D, 0x4C1536EC, 0xD2353D1C, 0x1F6B8F05,
+0x89023C93, 0xA00264D3, 0xE50455F8, 0x420B64D3,
+0x5BFB0009, 0xD61954FA, 0x460B65D3, 0x54F91B01,
+0xDA1655B1, 0x7CFC4A0B, 0x06FDE030, 0x0F657604,
+0x626D55F7, 0x8BDA3253, 0xA00484F4, 0xD4252E00,
+0x420BD20E, 0x7F3865D2, 0x6EF64F26, 0x6CF66DF6,
+0x6AF66BF6, 0x69F6000B, 0xE6006163, 0x4109A004,
+0x76016256, 0x74042422, 0x8BF93612, 0x0009000B,
+0x00117800, 0x00115FF0, 0x001164F6, 0x00114F2C,
+0x001165C0, 0x001164F5, 0x0011611C, 0x00117804,
+0x001165E0, 0x00114EBE, 0x00114F02, 0x001165F4,
+0x001165FC, 0x00116600, 0x00114BF0, 0x001148FC,
+0x00116618, 0x00116634, 0x00116640, 0x00114E56,
+0x0011664C, 0x00116658, 0x0011667C, 0x00116670,
+0x00114BC4, 0x00116688, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE5007FD8, 0x6453E110,
+0x6C534128, 0x655DEE0A, 0x46086653, 0x4608365C,
+0x361C7501, 0x675D6043, 0x60C30F66, 0x37E3ED00,
+0x816126C1, 0x81638162, 0x16D316D2, 0x8FEA16D4,
+0x68F27404, 0xDAB3D9B2, 0x29821981, 0xD1B259F1,
+0x2A921A91, 0x5BF35AF2, 0x5EF55DF4, 0x11A154F6,
+0x11B321A2, 0x11D511B2, 0x11E711D4, 0x114911E6,
+0x55F71148, 0xEE00DBA9, 0xDDA957F8, 0xD6A952F9,
+0x1B5164E3, 0xDBA82B52, 0xEAB8D8A8, 0x2D72E945,
+0x6AAC2622, 0x6EED4908, 0x4D086DE3, 0x3DEC61E3,
+0x4D084108, 0x3DBC31EC, 0x410860C3, 0x81D12DC1,
+0x4108E050, 0x41084008, 0x60C381D2, 0xE500318C,
+0x81D334A2, 0x1D131DD2, 0x8D01D494, 0xD4911D54,
+0xB08165D3, 0x64ED7E01, 0x8BDC3492, 0xDB94D18D,
+0xD28B6812, 0x1B814829, 0x2FD26412, 0x2B92694D,
+0xD98A6722, 0x1B734729, 0xD7876822, 0x1BA26A8D,
+0xD28C6B72, 0x22B2D586, 0xE0035D72, 0x5E7412D2,
+0x12E44018, 0xD6885176, 0x54781216, 0x1248E1FF,
+0xD4856792, 0x6852127A, 0x28C1E703, 0x81916952,
+0x6A52E050, 0x81A24008, 0x60C36B52, 0x6D5281B3,
+0x6E521DD2, 0x62521E63, 0x1264E600, 0x46086563,
+0x7501364C, 0x665D2612, 0x8BF83673, 0xE003D471,
+0x40186542, 0x674225C1, 0x8171D274, 0xEE006842,
+0x69421882, 0x1923E024, 0xE5806A42, 0x6B421AE4,
+0x81B266E3, 0xD46D6C42, 0x655C81C3, 0x6D63666D,
+0x616D7604, 0x31533D4C, 0x2DE28FF8, 0xD569D268,
+0x74042422, 0x7F282452, 0x6EF64F26, 0x6CF66DF6,
+0x6AF66BF6, 0x000B69F6, 0x664268F6, 0xC8036061,
+0xE5008D04, 0xC9036061, 0x8B038802, 0x65635262,
+0x24125124, 0x6053000B, 0x2FE62FD6, 0x7FEC4F22,
+0x62536E53, 0x6D43E550, 0x4508E400, 0xE101A001,
+0x60435224, 0x81212211, 0x60538123, 0x56E28122,
+0x8BF53620, 0x16E4D250, 0xE61464F3, 0x65E3420B,
+0xE4FC65E1, 0x2E512549, 0x65F361F1, 0x2F112149,
+0xD14954D1, 0xE614410B, 0x607157D1, 0x2701CB01,
+0x7F141DE1, 0x6EF64F26, 0x6DF6000B, 0x2FE62FD6,
+0x7FEC4F22, 0x66536E53, 0x6D43E5FC, 0x20596061,
+0x2601CB01, 0x326052E2, 0x12E48B06, 0x31E051E2,
+0x52D18B04, 0x1E22A002, 0x5664AFF0, 0x64F3D236,
+0x420BE614, 0x67E165E3, 0x2719E1FC, 0x67F12E71,
+0x271954D1, 0x65F3D130, 0x410BE614, 0x52D12F71,
+0xCB016021, 0x1DE12201, 0x4F267F14, 0x000B6EF6,
+0x2FE66DF6, 0x624C4F22, 0x4208DE1B, 0xA0054200,
+0x52523E2C, 0x5624D417, 0x2E62BF8E, 0x52E165E2,
+0x8BF63520, 0x2622D61B, 0x000B4F26, 0x2FB66EF6,
+0x2FD62FC6, 0x4F222FE6, 0xDB1CDC10, 0x66C252C1,
+0x89403620, 0xC9036061, 0x893C8801, 0xDD18DE0B,
+0x64E3BF63, 0x85036503, 0x620D66B2, 0x892B3262,
+0xBF9BD403, 0xD4130009, 0x00094D0B, 0x0009AFE6,
+0x001160DC, 0x001160E4, 0x001160EC, 0x00116114,
+0x001164F8, 0x00116500, 0x001000C8, 0x00101680,
+0x001E2108, 0x001C3D00, 0x00117880, 0x00117780,
+0x00040020, 0x0026C401, 0x001142F8, 0x001164DC,
+0x00114EBE, 0x0011669C, 0x64E3BF3E, 0x4D0BD406,
+0xAFBB0009, 0xD2050009, 0x4F262262, 0x6DF66EF6,
+0x000B6CF6, 0x00006BF6, 0x001166A0, 0x001C3D28,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xD23C7FFC, 0xC8036022, 0x2F018F3D, 0x0009A061,
+0xC9036061, 0x893B8801, 0xD238D837, 0x420BD938,
+0xE4006483, 0x6A036D03, 0x5C02490B, 0xD236DB35,
+0x56D385D2, 0x650D6422, 0x4B0BE740, 0xD1326E03,
+0x64126EED, 0x214234EC, 0x3DC05DD4, 0x85D28BEF,
+0x70FF56D3, 0xE740650D, 0x6C034B0B, 0x490BDB2A,
+0x66B2E403, 0x36CC6CCD, 0xE700D928, 0x2B62E5C8,
+0x6473E650, 0x490BDC26, 0x6483655C, 0x65A34C0B,
+0xEE01D124, 0xD11C21E2, 0x66125211, 0x8BBF3620,
+0xDD22DE21, 0xDC23DB22, 0x65D252D1, 0x89183520,
+0xC9036051, 0x89148801, 0xD114D41C, 0x0009410B,
+0x36E05603, 0x65038F04, 0x2B20E201, 0x2C52AFEC,
+0xD213D419, 0x0009420B, 0xE101D618, 0xAFE34118,
+0x60F12612, 0x8902C804, 0x420BD215, 0x7F040009,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0x000068F6, 0x001E2100, 0x001160E4, 0x0011453A,
+0x00114BF0, 0x00114E0C, 0x00116714, 0x001159B0,
+0x00114558, 0x001E212C, 0x00117880, 0x001160DC,
+0x001164FC, 0x001164F8, 0x00116114, 0x001C3D30,
+0x001140CC, 0xD6C2D5C1, 0x26226252, 0xC8016060,
+0x000B8BFA, 0x2FE60009, 0xBFF34F22, 0xD2BD0009,
+0xE405E100, 0x22402212, 0x6422DEB8, 0xE700D5B8,
+0x25721E42, 0xC98F8451, 0xC9F0CB10, 0x8051CB02,
+0xCB026050, 0x62522500, 0x2E22BFDC, 0xD6B250E4,
+0x4F262602, 0x6EF6000B, 0x4F222FD6, 0x0009BFDB,
+0x620CDDAE, 0x60D02D22, 0x8906C801, 0x0009BFD3,
+0x2D22620C, 0xC80160D0, 0x4F268BF8, 0x6DF6000B,
+0x4F222FE6, 0x6E43BFE8, 0xE100D2A2, 0x22E02212,
+0x6422D59E, 0xE600DE9E, 0x2E621542, 0xC9F084E1,
+0x80E1CB01, 0xCB0260E0, 0x67E22E00, 0x4F262572,
+0x6EF6AFA8, 0xE406AFE4, 0xE404AFE2, 0xBFF94F22,
+0xE4C70009, 0x644CBFDC, 0x4F26AFF6, 0xE406AFD8,
+0xE404AFD6, 0x4F222FE6, 0x6E43BFF8, 0xD58DD28D,
+0xE401E100, 0x221260E3, 0x80512240, 0x6622D187,
+0xE700DE87, 0x2E721162, 0xC9F084E1, 0x80E1CB02,
+0xCB0260E0, 0x62E22E00, 0x2122BF7C, 0xAFDF4F26,
+0x2FD66EF6, 0x4F222FE6, 0xBFCB6D53, 0xBF9B6E43,
+0xD27C0009, 0x22E061D3, 0x6022DE7D, 0x411821E9,
+0x201BC9FF, 0x2202D577, 0xD6768453, 0x60D38051,
+0xD4728053, 0xD1726762, 0x1472ED00, 0x841121D2,
+0xCB04C9F0, 0x60108011, 0x2100CB02, 0xBF516212,
+0x4F262422, 0xAFA76EF6, 0x65436DF6, 0xAFD0E4D8,
+0x6543644C, 0xAFCCE4D8, 0x2FC6644C, 0x2FE62FD6,
+0x6E534F22, 0xBF676D43, 0xD7626C63, 0x27D0D264,
+0x61E36072, 0x41182129, 0x201BC9FF, 0x2702D45D,
+0xD15B8443, 0x60E38041, 0xDE588043, 0xE6006472,
+0x21621E42, 0x65DC8411, 0x60C36203, 0x4008C907,
+0x67034008, 0xE29F6023, 0x622CC98F, 0x3520207B,
+0x80118D18, 0x7C048411, 0x60C36603, 0x6203C90F,
+0xC9F06063, 0x8011202B, 0x880B6053, 0x84118B14,
+0xC90F6603, 0xC90F7001, 0x60636203, 0x202BC9F0,
+0x8011A00A, 0x7C018411, 0x60C36603, 0x6203C90F,
+0xC9F06063, 0x8011202B, 0xCB026010, 0x62122100,
+0x2E22BEF0, 0xD63C50E4, 0x4F262602, 0x6DF66EF6,
+0x6CF6000B, 0x2FC62FB6, 0x2FE62FD6, 0x6C634F22,
+0x6E436D53, 0x6B73BF36, 0x0009BF06, 0x61D3D231,
+0xDE3322E0, 0x21E96022, 0xC9FF4118, 0xD42D201B,
+0x84432202, 0x8041D72F, 0x804360D3, 0x6622D427,
+0x1462D127, 0x14C327C2, 0x21C2EC00, 0x7B048411,
+0x60B36D03, 0x6503C90F, 0xC9F060D3, 0x8011205B,
+0xCB026010, 0x62122100, 0x4F262422, 0x6DF66EF6,
+0xAEAF6CF6, 0x2FB66BF6, 0x2FD62FC6, 0x4F222FE6,
+0x6C536D63, 0xBEFD6E43, 0xBECD6B73, 0xD2150009,
+0x22E061C3, 0x6022DE16, 0x411821E9, 0x201BC9FF,
+0x2202D110, 0xD60F8413, 0x60C38011, 0xDE0B8013,
+0xD40B6762, 0xEC006BBD, 0x1EB51E72, 0x844124C2,
+0xC9F04B21, 0x8041CB04, 0xE1406040, 0x2400CB06,
+0xE5006242, 0x4B212E22, 0x4128A014, 0x001D1200,
+0x00116528, 0x00116530, 0x00116538, 0x00116544,
+0x00FFFFFF, 0x00116534, 0x6053655D, 0x06DE4008,
+0x21627501, 0x32B3625D, 0x4F268BF6, 0x6DF66EF6,
+0xAE5F6CF6, 0x4F226BF6, 0xBF73677C, 0xAEB3644C,
+0x4F224F26, 0xBFA6677D, 0xAEAD644C, 0x4F224F26,
+0xE500E49F, 0xBF08E603, 0x4F26644C, 0x600C000B,
+0xE49F4F22, 0xE603E500, 0x644CBEFF, 0x4F264019,
+0x600D000B, 0x6543665C, 0xE403AEF7, 0x6543665C,
+0xE40BAEF3, 0xD175D674, 0x60436262, 0xC8012122,
+0x8F016010, 0xC9EFCB10, 0x62122100, 0x2622000B,
+0x4F222FE6, 0xE0004F13, 0xBE2C401E, 0xD56C6E43,
+0x2522620C, 0xE401BFE6, 0x6063D669, 0x60ECCF80,
+0x89072008, 0x89098801, 0x890D8802, 0x89118803,
+0x0009A013, 0xC9E36060, 0x2600A00F, 0xCB106060,
+0xCB04C9F7, 0x2600A009, 0xCB106060, 0xCB08C9FB,
+0x2600A003, 0xCB1C6060, 0xD5592600, 0xBE616252,
+0xE400642C, 0x4F264F17, 0x6EF6AFBC, 0x2F962F86,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0x60C36C7C,
+0x6A638802, 0x69538F09, 0x65436290, 0x662CE4AF,
+0xBEF7E701, 0xA00A644C, 0x2CC80009, 0x88018901,
+0x65438B05, 0xE600E4AF, 0xBEEBE701, 0xBDD1644C,
+0xED010009, 0xDE43EBAF, 0xE800A02C, 0x0009BDF4,
+0x60C3D141, 0x8802E200, 0xD5402122, 0x21B08D06,
+0x89082CC8, 0x890A8801, 0x0009A00C, 0x009C60D3,
+0xA007D639, 0xD2388061, 0xA0036083, 0xD2368021,
+0x802160D3, 0xD1356412, 0x1E42E600, 0x84512162,
+0xC9F07D01, 0x8051CB02, 0xCB026050, 0x67122500,
+0x2E72BDA0, 0x8BD13DA2, 0x0009BDF6, 0x0009BDA3,
+0x620CD627, 0x4F262622, 0x6DF66EF6, 0x6BF66CF6,
+0x69F66AF6, 0x68F6000B, 0xE702AF98, 0x2F962F86,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0x3F3C9331,
+0x0F569030, 0xE8FF70FC, 0x688C0F46, 0xE900A049,
+0x4018E010, 0xE50404FE, 0xBF33349C, 0x88FF6A43,
+0x901F893E, 0xE1100CFE, 0x41183C98, 0x8B033C16,
+0x64A3BE1B, 0x0009A031, 0x4018E010, 0xED000BFE,
+0xA0073BCC, 0x64D36EF3, 0xBF1F34BC, 0x2E00E501,
+0x7E017D01, 0x8BF63DC2, 0x64A3BE07, 0xA01AED00,
+0xEFF86EF3, 0x00001004, 0x001D1204, 0x0011652C,
+0x00116544, 0x001D1200, 0x00116530, 0x00116528,
+0x666C66E0, 0x89043680, 0x35BC65D3, 0xBE51E701,
+0x7D01E402, 0x3DC27E01, 0xE1108BF2, 0x391C4118,
+0x90547904, 0x391201FE, 0x93518BB2, 0x4F263F3C,
+0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6, 0x68F6000B,
+0x676D6253, 0x66236543, 0xE402AEC3, 0x2FA62F96,
+0x2FC62FB6, 0x2FE62FD6, 0x697D4F22, 0x4A216A93,
+0x4A084A21, 0x6C436D63, 0xA0086B73, 0x64C36E53,
+0x669365D3, 0x6BBDBFE4, 0x3DAC3CBC, 0x6EEF3EB8,
+0x8BF42EE8, 0x4F26E000, 0x6DF66EF6, 0x6BF66CF6,
+0x000B6AF6, 0x2FA669F6, 0x2FC62FB6, 0x2FE62FD6,
+0xEC004F22, 0x6B536EC3, 0xA0066D43, 0x64D3EA01,
+0x65A3BEA8, 0x7D013C0C, 0x3EB27E01, 0x60C38BF7,
+0x6EF64F26, 0x6CF66DF6, 0x000B6BF6, 0x10046AF6,
+0x00001008, 0x0009000B, 0x2FD62FC6, 0x4F222FE6,
+0x6D436C53, 0xEE00A004, 0x7E0164D4, 0x644CBFF2,
+0x8BF93EC2, 0x6EF64F26, 0x000B6DF6, 0xE5006CF6,
+0x6643A002, 0x76017501, 0x22286260, 0xAFE38BFA,
+0x2FE60009, 0x75076253, 0xE1086753, 0x6043EE0A,
+0x4409C90F, 0x650330E2, 0x8D014409, 0xE630E637,
+0x4110365C, 0x8FF22760, 0xE00077FF, 0x000B8028,
+0x4F226EF6, 0xBFE47FEC, 0xBFD865F3, 0x7F1464F3,
+0x000B4F26, 0x4F22E000, 0xBFDA7FEC, 0x64F365F3,
+0x7406BFCD, 0x4F267F14, 0xE000000B, 0x4F222FE6,
+0x62537FEC, 0x65F36E43, 0x6423BFCB, 0x64E3BFBF,
+0x64F3BFBD, 0xBFBAD403, 0x7F140009, 0x000B4F26,
+0x00006EF6, 0x001166A4, 0xE4FDD29A, 0xD79A6122,
+0x22122149, 0x74016022, 0x2202CB01, 0xD5976622,
+0x22622649, 0xC8406070, 0x60528902, 0x2502CB04,
+0xE1F76452, 0x25422419, 0xE7016052, 0x2502C9CF,
+0xE6026052, 0x2502CB03, 0x15624718, 0x1573000B,
+0xD78CD58B, 0xD48DD28C, 0xE600E100, 0x27112511,
+0xAFD12210, 0x664C2461, 0x4600D289, 0x6060362C,
+0x000BCB10, 0x654C2600, 0x4500D285, 0x6650352C,
+0x2619E1EF, 0x2560000B, 0xD282664C, 0x362C4600,
+0xCB106060, 0x2600000B, 0xD27E654C, 0x352C4500,
+0xE1EF6650, 0x000B2619, 0x664C2560, 0x4600D278,
+0x6060362C, 0x000BCB08, 0x654C2600, 0x4500D274,
+0x6650352C, 0x2619E1F7, 0x2560000B, 0xD271664C,
+0x362C4600, 0xCB086060, 0x2600000B, 0xD26D654C,
+0x352C4500, 0xE1F76650, 0x000B2619, 0x624C2560,
+0x4200D667, 0x6020326C, 0x4021C908, 0x40214021,
+0x600C000B, 0xD663624C, 0x326C4200, 0xC9086020,
+0x40214021, 0x000B4021, 0xD15F600C, 0x341C644C,
+0x000B6240, 0xD15D602C, 0x341C644C, 0x000B6240,
+0x2FE6602C, 0x6E434F22, 0xE60A645C, 0x89143467,
+0x0009BFEB, 0x60EC640C, 0x8B028801, 0xA002E00F,
+0x44092409, 0x624C4409, 0x3263E60A, 0xBFE28905,
+0x620C644C, 0xC8806023, 0xE2008B00, 0x4F266023,
+0x6EF6000B, 0xD64A4F22, 0x88016062, 0xB2458B03,
+0xA0030009, 0xD2470009, 0x2260E640, 0xE200D646,
+0x000B4F26, 0x4F222622, 0x6062D641, 0x8B018802,
+0x0009B28E, 0xE200D640, 0x000B4F26, 0xD53C2622,
+0xE100D43C, 0x2512E701, 0x2470000B, 0xE604D239,
+0x2260000B, 0xD4394F22, 0x410BD139, 0xD5390009,
+0x6650E1FD, 0x2619D238, 0x2560E700, 0x000B4F26,
+0x4F222270, 0xD132D435, 0x0009410B, 0xE7FBD531,
+0x26796650, 0x000B4F26, 0x4F222560, 0xD12CD430,
+0x0009410B, 0xE7F7D52B, 0x26796650, 0x000B4F26,
+0xD5282560, 0x6250942D, 0x000B2249, 0xD5252520,
+0x6250E4BF, 0x000B2249, 0x4F222520, 0x8522D225,
+0x2008600D, 0x88018911, 0x88038913, 0x88058915,
+0x88068942, 0x88088948, 0x8809894E, 0x880A8954,
+0x880B895A, 0xA0678960, 0xB0690009, 0xA0640009,
+0xB077600C, 0xA0600009, 0xB080600C, 0xA05C0009,
+0xFF7F600C, 0x001E2148, 0x001E1000, 0x001E1108,
+0x00116570, 0x00116572, 0x00116591, 0x00116554,
+0x001E103F, 0x001E105F, 0x001E102F, 0x001E1090,
+0x00116578, 0x001E100B, 0x00116574, 0x001166A8,
+0x00114EBE, 0x001E1028, 0x00116590, 0x001166B4,
+0x001166C4, 0x00116548, 0x6260D684, 0x8B2B2228,
+0x0009B061, 0x600CA029, 0x6260D680, 0x8B232228,
+0x0009B069, 0x600CA021, 0x6260D67C, 0x8B1B2228,
+0x0009B0C7, 0x600CA019, 0x6260D678, 0x8B132228,
+0x0009B0CD, 0x600CA011, 0x6260D674, 0x8B0B2228,
+0x0009B125, 0x600CA009, 0x6260D670, 0x8B032228,
+0x0009B13D, 0x600CA001, 0x4F26E000, 0x0009000B,
+0xD26CD16B, 0xD56C8412, 0x4000C90F, 0xD76B012D,
+0xE403D66B, 0xE20F611C, 0x2540E001, 0x25202712,
+0x2602000B, 0xE601D262, 0x30668523, 0xE0008D05,
+0xD663D260, 0xE0018122, 0x000B2602, 0xD25C0009,
+0x600D8523, 0x89052008, 0x8B0A8801, 0x6060D65D,
+0x2600CB01, 0xD457D65A, 0xE001E101, 0x000B2612,
+0x000B8142, 0xD152E000, 0x8513E501, 0x640D4518,
+0x66033453, 0xE0008D05, 0xD551D253, 0x2260E001,
+0x000B2502, 0x4F220009, 0x8513D149, 0x6453650D,
+0x62494419, 0x227D672E, 0x8801602C, 0x88028909,
+0x88038910, 0x8806891A, 0x88078935, 0xA04C893B,
+0xD5460009, 0x6652D746, 0x2762D446, 0x622C6261,
+0x2421A038, 0x2228625C, 0xD4438B3F, 0x6642D540,
+0x2562D440, 0x24018561, 0x6203A02C, 0x2008605C,
+0x88108907, 0x88208908, 0x88308909, 0xA02C890A,
+0xD23A0009, 0x6222A008, 0xA005D239, 0xD2396222,
+0x6222A002, 0x6262D638, 0xD432D531, 0x66212522,
+0xA00F626C, 0xD6352421, 0x6261D52D, 0x622CD42D,
+0xA0072562, 0xD6322421, 0x8561D529, 0x2562D429,
+0x62032401, 0x662D8515, 0x3617610D, 0x65038F01,
+0xB0CB2451, 0xA0010009, 0xE000E001, 0x000B4F26,
+0xD6190009, 0xD427E101, 0x65412610, 0xD118D717,
+0xE20F655D, 0x2752E001, 0x000B2620, 0x2FE62102,
+0xD20F4F22, 0x640C8523, 0x8B082448, 0xD511D61D,
+0x2621E200, 0x940F8451, 0xA0482049, 0xDE0D8051,
+0xC84060E0, 0xE2018D32, 0x89443427, 0xD216D615,
+0x2641420B, 0x0009A030, 0x0000FF7F, 0x00116591,
+0x00116548, 0x00116554, 0x001E1100, 0x001E100C,
+0x00116574, 0x001E1000, 0x001E1001, 0x0011657C,
+0x0011655C, 0x00116560, 0x00116564, 0x00116580,
+0x00116584, 0x00116588, 0x0011658C, 0x00116774,
+0x0011677E, 0x0011656E, 0x00115DCA, 0x89123427,
+0xD294D693, 0x2641420B, 0xCB8084E1, 0x80E1B0F5,
+0xD69160E0, 0x2E00CB04, 0xC93F6060, 0xD68F2600,
+0xA001E001, 0xE0002602, 0x000B4F26, 0xD68C6EF6,
+0xC8806060, 0xD2868919, 0x88016021, 0xD2898B15,
+0x8524E501, 0x89103056, 0xE203D187, 0x2120D487,
+0xE00B6541, 0x0656655D, 0xE40FD585, 0x2140E702,
+0xD77E2571, 0x000BE001, 0x000B2702, 0x2FE6E000,
+0xDE804F22, 0xC88084E1, 0xD57A892C, 0x20088554,
+0x61038F28, 0x8553D77C, 0x64036672, 0x8566650C,
+0x3520620C, 0xD6798B1E, 0x651CD774, 0x2651644C,
+0x60E02741, 0x8904C840, 0x420BD275, 0xA0030009,
+0xD2680009, 0x0009420B, 0x0009B09F, 0xE201D167,
+0x60E02122, 0xCB04D464, 0x60402E00, 0x2400C93F,
+0x6023A001, 0x4F26E000, 0x6EF6000B, 0x2FB62FA6,
+0x2FD62FC6, 0xDA622FE6, 0x66A1E240, 0x3622DC5E,
+0x62638900, 0x6ED36D2C, 0x4E2136D8, 0x4E212A61,
+0xDB61D460, 0xE700A00F, 0x770162B2, 0x71026123,
+0x66212B12, 0x71026213, 0x61212B12, 0x651D666D,
+0x356C4528, 0x627C2452, 0x8BED32E3, 0xC90360D3,
+0x8B108803, 0x617367B2, 0x2B127102, 0x71026E13,
+0x2B126571, 0x655D6DE1, 0x422862DD, 0x325CE107,
+0xA00C2C10, 0x88022422, 0xA0038B01, 0x8801E203,
+0xE2018B05, 0x66B22C20, 0x655D6561, 0xE60F2452,
+0x67A12C60, 0x8B052778, 0xDD38DC44, 0xEB01EA00,
+0x2DB22CA2, 0x6DF66EF6, 0x6BF66CF6, 0x6AF6000B,
+0x2FE62FD6, 0xE240DD36, 0x362266D1, 0x62638900,
+0x3678672C, 0x7703DE38, 0x47212D61, 0x64E2D635,
+0xA00E4721, 0x6562E100, 0x62537101, 0x74012450,
+0x24204219, 0x45297401, 0x74012450, 0x24504519,
+0x621C7401, 0x8BEE3273, 0x66E24200, 0x420061D1,
+0x2118362C, 0x2E628F06, 0xDD1CD728, 0xE501E400,
+0x2D522742, 0x000B6EF6, 0x2FD66DF6, 0x4F222FE6,
+0xED0AEE01, 0x64E3BC97, 0xBC9C64E3, 0x62EC7E01,
+0x8BF732D7, 0xBC9FEE01, 0x64E364E3, 0x7E01BCA4,
+0x32D762EC, 0x4F268BF7, 0x000B6EF6, 0xD1186DF6,
+0xD418920D, 0x72122122, 0x2422D617, 0xD7177204,
+0x72202622, 0x2722D116, 0x000B7230, 0x137A2122,
+0x0011656E, 0x00115ED6, 0x001E1015, 0x00116574,
+0x001E1001, 0x00116548, 0x001E1100, 0x00116572,
+0x00116560, 0x001E1000, 0x00116564, 0x00116570,
+0x00115DCA, 0x001E100C, 0x0011655C, 0x00116578,
+0x0011657C, 0x00116580, 0x00116584, 0x00116588,
+0x0011658C, 0x4F222FE6, 0xD6507FFC, 0x88016060,
+0xE2018951, 0x2620BFBB, 0xD54ED14D, 0xDE4E6010,
+0x64E36552, 0x7402C840, 0x8D22D14C, 0xD24C7502,
+0xE601D74C, 0xE7042722, 0x76016255, 0x626C2421,
+0x8FF93273, 0xD4437402, 0x6242E601, 0x640D8528,
+0x67494419, 0x275D657E, 0x81E4607C, 0xE417D542,
+0x67557601, 0x3243626C, 0x8FF92171, 0xA0207102,
+0xD23E0009, 0xE601D73B, 0xE7042722, 0x76016255,
+0x626C2421, 0x8FF93273, 0xD4327402, 0x6242E601,
+0x640D8528, 0x67494419, 0x275D657E, 0x81E4607C,
+0xE417D533, 0x67557601, 0x3243626C, 0x8FF92171,
+0x924A7102, 0xD2262E21, 0x5E23D72E, 0x64F22FE2,
+0x604365F2, 0x2700C980, 0xC9606043, 0x80716103,
+0xC9036043, 0x80724519, 0x65F2605C, 0x817266F2,
+0x46194629, 0x606C4529, 0x4018645C, 0x8173304C,
+0x21185E23, 0x64F22FE2, 0x6E4C62F2, 0x602C4219,
+0x66F262F2, 0x46294018, 0x461930EC, 0x42298174,
+0x652C606C, 0x305C4018, 0x81758F07, 0x0009BC9D,
+0x2228620C, 0xA00A8908, 0x60130009, 0x8B038840,
+0x0009B009, 0x0009A003, 0xE202D60F, 0x7F042622,
+0x000B4F26, 0x000B6EF6, 0x060A0009, 0x00116590,
+0x001E1000, 0x0011657C, 0x00116774, 0x00116780,
+0x00116718, 0x00116564, 0x00116748, 0x00116746,
+0x0011671A, 0x00116548, 0x00116574, 0x4F222FE6,
+0x84E9DE8E, 0x2448640C, 0xB18B8901, 0xD28C0009,
+0x26686620, 0x60E08902, 0x2E00C9BF, 0x000B4F26,
+0x000B6EF6, 0x2FE60009, 0xDE864F22, 0x60E0D686,
+0xCBC0D486, 0x62602E00, 0xC803602C, 0x40218904,
+0x70014021, 0x6603A002, 0x66034009, 0xD680616D,
+0xE500A004, 0x75016262, 0x74042422, 0x3213625D,
+0xD27C8BF8, 0x0009420B, 0xC9BF84E2, 0x4F2680E2,
+0x6EF6000B, 0x2FE62FD6, 0x7FFC4F22, 0x6260D676,
+0x89402228, 0xD56DE100, 0x60502610, 0xCB40D473,
+0x2500440B, 0x8D052008, 0x62E06E03, 0x7104612C,
+0x2F11A006, 0xD46ED666, 0xDD6E6760, 0x657C4D0B,
+0xE23C6D1D, 0x8B033D27, 0xD26CD46B, 0x0009420B,
+0x4D214D21, 0xA005D76A, 0x66E6E400, 0x357C4508,
+0x74012562, 0x35D3654D, 0xD7668BF7, 0x6E72E003,
+0x81E14018, 0x6E7260F1, 0x81E2700C, 0xD4626172,
+0xDD628113, 0x65724D0B, 0xD652D261, 0x2212E101,
+0xC93F6060, 0x7F042600, 0x6EF64F26, 0x6DF6000B,
+0x2FC62FB6, 0x2FE62FD6, 0xD25A4F22, 0x6B436E73,
+0x420B6C53, 0x20086D63, 0x61038F08, 0xD24FD456,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6422B, 0x21B060C3,
+0x60D38011, 0xE5008111, 0x64BCA007, 0x6053655D,
+0x665300EC, 0x7501361C, 0x625D8064, 0x8BF53243,
+0x6060D636, 0x2600C9BF, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6000B, 0x7FC44F22, 0x720262F3, 0x22512F41,
+0x45297202, 0x60632251, 0xE5C4E682, 0x67F38121,
+0x655C666C, 0xE408BFBC, 0x4F267F3C, 0x0009000B,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xE1007FC4, 0x6513ECFF, 0x6B136CCD, 0xDE34D733,
+0xEDFF64F3, 0xD833EA04, 0x6053655C, 0x027D4000,
+0x32C0622D, 0x66038D0D, 0x09ED6063, 0x2491027D,
+0x24217402, 0x698202ED, 0x3928622D, 0x74022892,
+0x75017104, 0x6063625C, 0x07D532A2, 0x0EB58FE4,
+0x2448641C, 0xE6808905, 0x67F3E5C5, 0xBF7F666C,
+0x7F3C655C, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0xD11C68F6, 0x6012D21C, 0xCB20E405,
+0x2102E500, 0x000B2242, 0x00002252, 0x001E1017,
+0x001164F6, 0x001E1015, 0x001E10BF, 0x00117800,
+0x001E10FC, 0x001140CC, 0x001164FC, 0x0011602E,
+0x001166D0, 0x00114F2C, 0x001166EC, 0x00114EBE,
+0x0011788C, 0x001164F8, 0x001160DC, 0x001145BC,
+0x001E2130, 0x00115FF0, 0x001166F4, 0x00116510,
+0x00116518, 0x00116710, 0x001C3500, 0x001D4004,
+0xD565D164, 0xE400D765, 0x2142E20F, 0x17411154,
+0xD5632722, 0x9669D763, 0x15412572, 0x96661562,
+0xE6011565, 0xD5601165, 0x666CE6F8, 0x25422542,
+0x25422542, 0x25422542, 0x25622542, 0x7601E727,
+0x67632572, 0x25627797, 0xE7042572, 0x2572E248,
+0xE2192522, 0xE2702522, 0x25422542, 0x25422542,
+0x25222542, 0x2522E20C, 0x25422542, 0x25422542,
+0x25422542, 0x25422542, 0x000B154A, 0xE2081145,
+0x0009422B, 0x2FE62FD6, 0x7FFC4F22, 0xC8206043,
+0x6E438D02, 0x0009BE75, 0xC81060E3, 0xBE728901,
+0x60E30009, 0x8901C840, 0x0009BE94, 0xC80160E3,
+0xDD3E8938, 0xC80260D0, 0x2F008D03, 0x460BD63C,
+0x60F00009, 0x8902C804, 0x460BD63A, 0x62F00009,
+0xC8806023, 0x60D08902, 0x2D00C97F, 0xC8016023,
+0xD6358906, 0x0009460B, 0x0009A007, 0x51630601,
+0x8902C808, 0x460BD631, 0x60F00009, 0x8902C810,
+0x420BD22F, 0xD52F0009, 0x88026052, 0xD22E8B03,
+0xA005E604, 0x88012260, 0xD22B8B02, 0x2260E601,
+0x2522E200, 0xC88060E3, 0xD628892E, 0x60E36E60,
+0x8902C880, 0x420BD226, 0x60E30009, 0x8902C840,
+0x420BD224, 0x60E30009, 0x8902C802, 0x420BD222,
+0x60E30009, 0x890EC804, 0x410BD120, 0xBF120009,
+0xBF4D0009, 0xD51E0009, 0x6050D41E, 0xC908D71E,
+0xBF842500, 0x60E32472, 0x8905C808, 0x7F04D21B,
+0x6EF64F26, 0x6DF6422B, 0x4F267F04, 0x000B6EF6,
+0x00006DF6, 0x001C581C, 0xA000A000, 0x001D0100,
+0x001D4000, 0x00040021, 0x001C589C, 0x001E1021,
+0x001150C4, 0x001150E6, 0x00115724, 0x001150FE,
+0x0011510C, 0x00116574, 0x001E100B, 0x001E1028,
+0x00115162, 0x0011516E, 0x00115114, 0x00115132,
+0x001E1000, 0x0010F100, 0x12345678, 0x0011514A,
+0x644CD6A7, 0x000B346C, 0xD6A62450, 0x346C644C,
+0x2450000B, 0x644CD6A4, 0x000B346C, 0x625C2450,
+0x4208616D, 0x42084119, 0x42006019, 0x670E614C,
+0xD49E321C, 0x4200207D, 0x324CC90F, 0x2200000B,
+0x4208625C, 0x42004208, 0x324C644C, 0x4200D498,
+0x000B324C, 0x2FE62260, 0x614C4F12, 0x4100D493,
+0x6710314C, 0xE29F666D, 0x27294619, 0x6E536269,
+0x672E6573, 0x4221227D, 0x42214221, 0x7601662C,
+0xE4014608, 0x34E84608, 0x644C4600, 0x071A0467,
+0x2150257B, 0x000B4F16, 0x4F226EF6, 0xD2857FE8,
+0x88016021, 0xD2848B7B, 0x26686621, 0xD2838B77,
+0x26686621, 0xE50F8B73, 0xE401BFA2, 0xBFA4E501,
+0xE586E400, 0xE400655C, 0x2F50BFA4, 0xBFA1E401,
+0xE602E506, 0x60634618, 0x81F2E401, 0x6543BF9F,
+0xE40185F2, 0xBFAB6543, 0x85F26603, 0x6543E401,
+0x6603BFB1, 0xE40265F0, 0x6053756C, 0x80F8BF80,
+0xBF82E402, 0x84F8E512, 0x7090E402, 0x6503BF82,
+0x4618E602, 0x81F66063, 0xBF80E402, 0x85F6E500,
+0x6603E402, 0xE500BF8C, 0xE40285F6, 0xBF926603,
+0xE5FEE500, 0xE010655C, 0xBF61E403, 0xE5130F54,
+0xE40EBF63, 0x05FCE010, 0xBF63E40E, 0xE5007585,
+0xBF64E403, 0xE500E640, 0xBF71E403, 0xE500E640,
+0xBF78E403, 0xE5FFE640, 0xE014655C, 0xBF47E404,
+0xE40F0F54, 0xE504BF49, 0x05FCE014, 0xBF49E40F,
+0xE5017584, 0xBF4AE640, 0xE501E404, 0xBF57E640,
+0xE501E404, 0xE404E640, 0xAF5C7F18, 0x7F184F26,
+0x000B4F26, 0x4F220009, 0xD2427FF0, 0x88016021,
+0xD2418B71, 0x26686621, 0xD2408B6D, 0x26686621,
+0xE50F8B69, 0xE401BF1C, 0xBF1EE501, 0xE586E400,
+0xE400655C, 0x2F50BF1E, 0xBF1BE401, 0xE401E506,
+0xBF1C6543, 0xE401E640, 0xBF296543, 0xE401E640,
+0xBF306543, 0x65F0E640, 0x756CE402, 0xBEFF6053,
+0xE40280F4, 0xE512BF01, 0xE40284F4, 0xBF017090,
+0xE6406503, 0xBF02E402, 0xE640E500, 0xBF0FE402,
+0xE640E500, 0xBF16E402, 0xE5FEE500, 0x6053655C,
+0xBEE5E403, 0xE51380F8, 0xE40EBEE7, 0xE40E84F8,
+0xBEE77085, 0xE5006503, 0xBEE8E640, 0xE500E403,
+0xBEF5E640, 0xE500E403, 0xBEFCE640, 0xE5FFE403,
+0x6053655C, 0xBECBE404, 0xE40F80FC, 0xE504BECD,
+0xE40F84FC, 0xBECD7083, 0xE5016503, 0xBECEE640,
+0xE501E404, 0xBEDBE640, 0xE501E404, 0xE404E640,
+0xAEE07F10, 0x7F104F26, 0x000B4F26, 0x00000009,
+0x001E102F, 0x001E1080, 0x001E1090, 0x001E103F,
+0x001E103E, 0x0011656E, 0x00116570, 0x00116572,
+0xD21DD11C, 0x66206010, 0x676C7001, 0x3700C90F,
+0xE5008D13, 0x67106210, 0x7701622C, 0x64232170,
+0xD6166010, 0x44084408, 0x3428C90F, 0x62602100,
+0x7201D513, 0x44082620, 0x000B354C, 0xD10F6053,
+0x25586510, 0xE6008D13, 0xD60DD40B, 0x655C6540,
+0x47086753, 0x37584708, 0x47086540, 0x24507501,
+0x367C6040, 0x2400C90F, 0x72FF6210, 0x000B2120,
+0x00006063, 0x001164F5, 0x001164F4, 0x001164F6,
+0x0011611C, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x544F0D0A, 0x53205355, 0x46204950,
+0x00003A57, 0x2074634F, 0x32203220, 0x20373030,
+0x333A3831, 0x36343A32, 0x00000000, 0x00000D0A,
+0x42707372, 0x3D206675, 0x554E203D, 0x202C4C4C,
+0x6E49677A, 0x4E497274, 0x6D754E51, 0x0000003D,
+0x52504545, 0x57204D4F, 0x65746972, 0x6461202C,
+0x003D7264, 0x6C617620, 0x0000003D, 0x00000A0D,
+0x5A205746, 0x4D435F4D, 0x4C465F44, 0x5F485341,
+0x53415245, 0x000A0D45, 0x5A205746, 0x4D435F4D,
+0x4C465F44, 0x5F485341, 0x534B4843, 0x0A0D4D55,
+0x00000000, 0x2D495053, 0x72646461, 0x0000003D,
+0x2D495053, 0x676E656C, 0x003D6874, 0x2D495053,
+0x736B6863, 0x003D6D75, 0x5A205746, 0x4D435F4D,
+0x4C465F44, 0x5F485341, 0x44414552, 0x00000A0D,
+0x61202072, 0x3D726464, 0x00000000, 0x72202020,
+0x75427073, 0x00003D66, 0x6E6B6E55, 0x206E776F,
+0x6D6D6F63, 0x3D646E61, 0x00000000, 0x00000072,
+0x00205220, 0x00000D0A, 0x62735576, 0x7473725F,
+0x00000A0D, 0x62735576, 0x7375735F, 0x646E6570,
+0x00000A0D, 0x62735576, 0x7365725F, 0x000A0D6D,
+0x72746E49, 0x6D652051, 0x2C797470, 0x49677A20,
+0x4972746E, 0x754E514E, 0x00003D6D, 0x654C7245,
+0x0000006E, 0x20746F4E, 0x756F6E65, 0x49206867,
+0x4220514E, 0x0A0D6675, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x002E0209, 0x80000101,
+0x000409FA, 0x00FF0400, 0x05070000, 0x02000201,
+0x82050700, 0x00020002, 0x03830507, 0x07010040,
+0x40020405, 0x02090000, 0x0101002E, 0x09FA8000,
+0x04000004, 0x000000FF, 0x02010507, 0x07000040,
+0x40028205, 0x05070000, 0x00400383, 0x04050701,
+0x00004002, 0x00000000, 0x00000000, 0x07090000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, };
+
+const u32_t zcFwImageSPISize=10156;
diff --git a/drivers/staging/otus/hal/hpfwu.c b/drivers/staging/otus/hal/hpfwu.c
new file mode 100644
index 000000000000..2b77cbacc6d6
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwu.c
@@ -0,0 +1,1017 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE947FFC, 0xE114D594,
+0x1E13D494, 0x67521E4C, 0xD494D693, 0x37402769,
+0x62528F06, 0x7201D692, 0x60602522, 0x2600C93F,
+0xD7906152, 0x2512611D, 0x264B6652, 0x2562470B,
+0x0009B017, 0xE60095AC, 0xC84060E2, 0x2F028F03,
+0x8FF93652, 0xD4887601, 0x4E0BDE88, 0xD4880009,
+0x00094E0B, 0x4E0BD487, 0x7F040009, 0xA0524F26,
+0x4F226EF6, 0x410BD184, 0xD4840009, 0x0009440B,
+0x450BD583, 0xD7830009, 0xD283E1FF, 0x2712611D,
+0xD4825029, 0xE1FFCB01, 0x1209E501, 0x12112212,
+0xE7202452, 0x4718D57E, 0x2572D27E, 0xD17EE700,
+0xD67FD47E, 0xE2012270, 0x24702172, 0xD67D2620,
+0x2641E4FF, 0xD57CE600, 0x666DE104, 0x76016063,
+0x4000626D, 0x8FF83212, 0xD5780545, 0x2520E201,
+0xD278D777, 0xE480E100, 0x22122710, 0x6613D576,
+0x666D644C, 0x76046763, 0x375C626D, 0x8FF83243,
+0xD5722712, 0xD273D772, 0xE400E101, 0x27102511,
+0x000B4F26, 0x7FCC2242, 0xD170D56F, 0xD271DB70,
+0x1F51D471, 0xD6717508, 0x1F12D771, 0x1F55710C,
+0x1FB975FC, 0x72041F2A, 0x1F13EB10, 0x1F561F44,
+0x1F781F67, 0xD86B1F2B, 0xDD6CD96B, 0xDC6CEA00,
+0xD26DDE6C, 0x89003A22, 0xD15D7A01, 0x88016010,
+0x56F88B03, 0x4218E201, 0xD1682622, 0x0009410B,
+0x440BD467, 0xD5670009, 0x0009450B, 0x6010D150,
+0x8B108801, 0xE650D14F, 0x46186212, 0x8B083266,
+0x56F9D14B, 0x2120E200, 0xCB016062, 0x2602A003,
+0x72012710, 0x60822122, 0x89098801, 0xE2C8D15A,
+0x622C6612, 0x89033626, 0x6010D158, 0x8BC88801,
+0x51F66792, 0x217252F5, 0xD6555191, 0x55FA2212,
+0x52FB6462, 0x55612542, 0x2252E400, 0x61436643,
+0x05DE6013, 0x36CC4608, 0x07DE2652, 0xC9036071,
+0x8B028801, 0x720162E2, 0x74012E22, 0x36B3664C,
+0x71048FEE, 0x66C2D147, 0x45286512, 0x265B4518,
+0x60822C62, 0x89018801, 0x0009A168, 0x6272D742,
+0x8B132228, 0xD42BD741, 0x6772D541, 0x51536242,
+0x312C327C, 0x24222228, 0x15138D05, 0x6262D63D,
+0xB1627201, 0xD6232622, 0x2622E200, 0x52916692,
+0x8B013620, 0x0009A144, 0x6061A06E, 0x001C001C,
+0x001D4020, 0x0000B38E, 0xFFFF0000, 0x12340000,
+0x001E1015, 0x00201274, 0x002039F4, 0x002018A2,
+0x00203A00, 0x00203A18, 0x00201860, 0x0020196C,
+0x00201288, 0x001C3510, 0x001C3624, 0x001E212C,
+0x002038F4, 0x0020348C, 0x002038FC, 0x00203908,
+0x00203914, 0x00203970, 0x00203974, 0x0020391C,
+0x0020391D, 0x00203920, 0x00117700, 0x0020398C,
+0x0020398A, 0x002034F0, 0x00117710, 0x001C3D30,
+0x001C36F8, 0x00117734, 0x001C3684, 0x001C3D00,
+0x001C1000, 0x001C1028, 0x00203504, 0x00203924,
+0x00117600, 0x00117740, 0x7FFFFFFF, 0x00201730,
+0x0020332A, 0x00202334, 0x00203DA4, 0x00203972,
+0x002034FC, 0x00203964, 0x001C3D2C, 0x001C36B0,
+0x00203494, 0x0011775C, 0x8801C90F, 0xA0CF8901,
+0xD1960009, 0x36206212, 0xD4958904, 0x2421E200,
+0x2162A0CC, 0x6211D193, 0x89012228, 0x0009A0C3,
+0xE202D78F, 0x75016571, 0x3123615D, 0x27518D02,
+0x0009A0BC, 0xD28C57F2, 0x62226072, 0x40094019,
+0xC90F4009, 0x8F19880A, 0x52F31F2C, 0x40196022,
+0x40094009, 0x8808C90F, 0xA0A78901, 0x60630009,
+0xCB0154F7, 0xD27E55F2, 0xE7012402, 0xD47FE100,
+0x22112572, 0x72016242, 0x2422A098, 0x8B3F8805,
+0x602252F3, 0x40094019, 0xC90F4009, 0x8B168802,
+0xE4FFD577, 0x644D6752, 0x8B102748, 0x6272D775,
+0x8B0C3260, 0x51F255F7, 0xD26DE701, 0x21722562,
+0xD571E100, 0x64522211, 0xA0777401, 0x52F32542,
+0x40196022, 0x40094009, 0x8805C90F, 0x31B38B6E,
+0xD26A8B6C, 0x672254F4, 0x7701D569, 0x61422272,
+0x1F1CE640, 0x46182159, 0x8B033160, 0x6262D665,
+0x26227201, 0xE200D65A, 0x2621B067, 0x0009A056,
+0x3123E220, 0x88038B52, 0x52F38B1E, 0x40196022,
+0x40094009, 0x8803C90F, 0xD25B8B16, 0x672254F4,
+0x7701D557, 0x61422272, 0x1F1CE640, 0x46182159,
+0x8B033160, 0x6262D655, 0x26227201, 0xE200D648,
+0x2621B043, 0x0009A010, 0xD452D551, 0xD2446752,
+0xE1007701, 0x25723A46, 0x22118F06, 0xEA00D64E,
+0x72016262, 0x2622B031, 0x2FB2D54C, 0x95736652,
+0xD44A5BF1, 0x36205241, 0x60618910, 0x8B01C803,
+0x2B22E201, 0x8FF54510, 0x57F15664, 0x6272E1F0,
+0x41284118, 0x2722221B, 0x6BF2A008, 0x6BF2A006,
+0xE200D62F, 0xD12F2621, 0x2121E200, 0xD13CE201,
+0x66122822, 0x8B012668, 0x0009AE2B, 0x450BD539,
+0xD1390009, 0xAE24E600, 0x2F862160, 0x2FA62F96,
+0x2FC62FB6, 0x2FE62FD6, 0x7FF44F22, 0xDE34D133,
+0x54116212, 0x1F4167E2, 0x2F22D432, 0xD5321F72,
+0xD2326743, 0x58417794, 0x69425A42, 0x5B166C72,
+0x60526D22, 0xCB20E600, 0xE5402502, 0x626D7601,
+0x8BFB3253, 0x55F162F2, 0x11512122, 0xD62855F2,
+0x14812E52, 0x249214A2, 0x27C2D426, 0x26D211B6,
+0xDA256742, 0xE801D925, 0x490B2A72, 0xE2011A8C,
+0x1A2C4218, 0x4F267F0C, 0x6DF66EF6, 0x6BF66CF6,
+0x69F66AF6, 0x68F6000B, 0x000007D1, 0x00203984,
+0x00203988, 0x0020398E, 0x001C3DC0, 0x0011772C,
+0x001C3B88, 0x0020396C, 0x0011773C, 0x00117744,
+0x0000F000, 0x00117764, 0x00117748, 0x00117768,
+0x0011776C, 0x01FFFFFF, 0x0011774C, 0x002034FC,
+0x00203DA4, 0x002024F8, 0x00203972, 0x001C3B9C,
+0x001C3D98, 0x001C3700, 0x001C3500, 0x001C5960,
+0x001C8960, 0x00203504, 0x001C3D00, 0x0020160C,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xDE957FAC, 0x61E0E014, 0x0F14D494, 0x710161E3,
+0xE0186210, 0xD2920F24, 0x0009420B, 0x450BD591,
+0x20080009, 0x8F126D03, 0xD28F1F07, 0x6720D48F,
+0x657CDD8F, 0x470BD78F, 0xD18F0009, 0x619C6910,
+0x46086613, 0x36184608, 0x3D6C4608, 0xE0181FD7,
+0xE58004FC, 0x604C66E2, 0x3050655C, 0x2D628F15,
+0x01FCE014, 0xDE85E500, 0x641CA008, 0x6753655D,
+0x607037EC, 0x39DC6953, 0x80947501, 0x3243625D,
+0xD67F8BF4, 0xA34EE200, 0x20082621, 0xE0148B13,
+0xE40001FC, 0xA009DE79, 0x644D671C, 0x35EC6543,
+0x69436652, 0x39DC6262, 0x74041921, 0x3273624D,
+0xA3388BF3, 0x88012D10, 0xE0148B17, 0xE70001FC,
+0x6D1C2D70, 0xDE6D1FD4, 0x32D3627D, 0xA32A8B01,
+0x677D0009, 0x667365E3, 0x61737504, 0x315C36EC,
+0x69126462, 0xAFEF7708, 0x88042492, 0xE0148B18,
+0xE40001FC, 0x671C2D40, 0x624DDE60, 0x8B013273,
+0x0009A311, 0x6943644D, 0x39EC62E3, 0x72046592,
+0x3D2C6D43, 0x615266D2, 0x21697408, 0x2512AFED,
+0x8B188805, 0x01FCE014, 0x2D40E400, 0xDE53671C,
+0x3273624D, 0xA2F68B01, 0x644D0009, 0x62E36943,
+0x659239EC, 0x6D437204, 0x66D23D2C, 0x74086152,
+0xAFED216B, 0x88312512, 0xD44A8B3A, 0x6146D94A,
+0x75046543, 0x67566442, 0x6E531F48, 0x65527E04,
+0x7EE462E2, 0x7E0464E2, 0x6EE21FE9, 0x5EF929E0,
+0x7E04D942, 0x1FEA60E2, 0x2900C901, 0xD9406EE2,
+0x29E04E09, 0x2F562F26, 0x56FAD93E, 0x6513490B,
+0xD13D7F08, 0xE71C6E0D, 0x1DE12D70, 0xDE3B6912,
+0x64E21D92, 0x1D43D13A, 0xD23A6512, 0x67221D54,
+0x1D75D239, 0x1D666622, 0x6262D638, 0x1D27A2AB,
+0x8B398830, 0x6596D92B, 0x67926696, 0x61967904,
+0x74E46493, 0x6E436992, 0x1F9B7E04, 0x1FEC6442,
+0xD9256EE2, 0x5EFC29E0, 0x7E04D924, 0x1FED60E2,
+0x2900C901, 0xD9226EE2, 0x29E04E09, 0x59FC7FFC,
+0xDE272F92, 0x2F164E0B, 0xD41F7F08, 0xE21C610D,
+0x1D112D20, 0xD2206442, 0xD41C1D42, 0x1D536542,
+0x6752D51B, 0xD71B1D74, 0x1D156172, 0x1D666622,
+0x6262D61A, 0x1D27A26F, 0x8B358833, 0x490BD919,
+0xA268EE00, 0x00002DE0, 0x00117800, 0x00203A1C,
+0x002018A2, 0x00202AAC, 0x0020390E, 0x00203A20,
+0x00203534, 0x002018EE, 0x0020390D, 0x00117804,
+0x0020398C, 0x00117810, 0x00203909, 0x0020390A,
+0x0020390B, 0x00200F64, 0x001C5864, 0x001C6864,
+0x001C7864, 0x001C59BC, 0x001C69BC, 0x001C79BC,
+0x00200FBC, 0x00200FB8, 0x89018828, 0x0009A0C0,
+0xE643DEB5, 0x326662E1, 0x1FEE8F02, 0x2E21E240,
+0x622D62E1, 0x8B013267, 0x0009A0AA, 0xE50185E1,
+0x8B013056, 0x0009A0A4, 0x2D10E101, 0x64E1B225,
+0xE64357FE, 0x652D6271, 0x89443567, 0x3563E640,
+0xE6008B05, 0x0F65E040, 0xA00FE11A, 0x615372C0,
+0x41214121, 0x41214121, 0x45214121, 0x45214521,
+0xC9036053, 0xE0406603, 0x71180F65, 0x2209E007,
+0x0F25E03C, 0xE044641D, 0xB2A365F3, 0xE33C0F46,
+0x853233FC, 0x620DDE95, 0x42086031, 0x6023610D,
+0x1323E944, 0x06FE4108, 0xE00F39FC, 0x13144E0B,
+0x67075D91, 0x60D32679, 0x0F6654FE, 0x51928542,
+0x600D4E0B, 0x60D3260B, 0x0F666492, 0x65F3B237,
+0x696156FE, 0xE640659D, 0x89383563, 0xD78359FE,
+0x79066591, 0xC9036053, 0x40004008, 0x61036203,
+0x0F26E050, 0x470BE0FF, 0x6C07600C, 0x6603605D,
+0x46214621, 0x46214621, 0x42006263, 0x4200326C,
+0x40214021, 0x4008C903, 0x6D2D30FC, 0xE8006A03,
+0xB25765F3, 0x6EA264D3, 0x2EC9E050, 0x66942AE2,
+0xD76E01FE, 0x606C470B, 0x2AE22E0B, 0x64D365F3,
+0x7801B1FD, 0xEE06628D, 0x8FE932E3, 0x5EFE7D01,
+0x61E1E400, 0x410085E1, 0x66E3310C, 0x760C711B,
+0xE70465F3, 0x68667401, 0x3A736A4D, 0x8FF92582,
+0x65F37504, 0x641DB1E3, 0x64E1B1A4, 0x0009A17B,
+0xD45B56F7, 0xEC01D25B, 0x26C0420B, 0x0009A173,
+0x06FCE018, 0x8829606C, 0x58F78B08, 0xE400D252,
+0x66222840, 0x646DB171, 0x0009A165, 0x666CE681,
+0x89013060, 0x0009A0AC, 0xD550D14F, 0x62126A56,
+0x212232AC, 0x54116C56, 0x34CC6253, 0x64521141,
+0x72085812, 0xD44A384C, 0x68221182, 0x5A136C42,
+0x3ACC3C8C, 0x11A324C2, 0x6C2272EC, 0x72105814,
+0x118438CC, 0x5A156822, 0x11A53A8C, 0x6A227210,
+0xD6405816, 0x118638AC, 0x52176C62, 0x112732CC,
+0x5A185861, 0x11A83A8C, 0x5C195A62, 0x11C93CAC,
+0x521A5C63, 0x112A32CC, 0x5A1B5864, 0x11AB3A8C,
+0x5C1C5A65, 0x11CC3CAC, 0x521D5C66, 0x112D32CC,
+0x5A1E5867, 0x11AE3A8C, 0x561F5A68, 0x36ACE840,
+0x116FDA2D, 0x6CA2381C, 0x7A946682, 0x286236CC,
+0x5C8162A2, 0x18C13C2C, 0x62A27A44, 0x362C5682,
+0xD6261862, 0x5A856262, 0x3A2C4229, 0x760418A5,
+0x56866262, 0x362C4229, 0x56F71866, 0x2620E238,
+0x16C15C81, 0x16226212, 0xE2005C11, 0x551216C3,
+0x55151654, 0x55131655, 0x55161656, 0x55821657,
+0x65821658, 0x55141659, 0x5584165A, 0x5583165B,
+0x5585165C, 0x5586165D, 0x1821165E, 0x11212122,
+0x11251122, 0x11261123, 0x28221822, 0x18241124,
+0x18251823, 0x1826A0C7, 0x00117804, 0x002033E8,
+0x00203A40, 0x002018A2, 0x00203494, 0x001C36A0,
+0x002034F0, 0x001C3CA0, 0x001C36F4, 0x001C3B88,
+0x666CE682, 0x8B203060, 0xEA2456F7, 0x26A0D194,
+0x16C15C17, 0x16225218, 0x16835819, 0x16A45A1A,
+0x16C55C1B, 0x1626521C, 0xE200581D, 0x551E1687,
+0x551F1658, 0x11271659, 0x11291128, 0x112B112A,
+0x112D112C, 0xA08E112E, 0xE683112F, 0x3060666C,
+0x52F78B0B, 0xEA00D883, 0x658222A0, 0x7804DC82,
+0x62822C52, 0xA07ED681, 0xE6902620, 0x3060666C,
+0xDA7F8B06, 0x00094A0B, 0xE20056F7, 0x2620A073,
+0x666CE691, 0x8B103060, 0x6222D276, 0x2228622C,
+0xD2788904, 0x0009420B, 0x0009A003, 0x420BD276,
+0x56F70009, 0xA05EE200, 0xE6922620, 0x3060666C,
+0xE0188951, 0xE6B00BFC, 0x666C62BC, 0x8B2A3260,
+0x02FCE014, 0x682CEA00, 0x62ADE904, 0x894A3283,
+0x6AADDD64, 0x3CDC6CA3, 0x7D046EC2, 0xDB68D467,
+0x32DC62A3, 0x4B0BDC67, 0x4C0B6D22, 0xD46664E3,
+0x00094B0B, 0x64D34C0B, 0x4B0BD464, 0xE6000009,
+0x666D6BE3, 0x76013B6C, 0x3293626D, 0x8FF72BD0,
+0xAFDA4D19, 0xE6B57A08, 0x3260666C, 0xD45C8B13,
+0x4B0BDB57, 0xD25B0009, 0x6022DB5B, 0xCB20E6FF,
+0x2202666D, 0xDB592B62, 0xE014E200, 0x56F72B20,
+0xA01002FC, 0xD4562620, 0x6542D256, 0x420BD456,
+0xA0080009, 0xDB520009, 0x52B1E600, 0x622CDB53,
+0x52F72B21, 0x7F542260, 0x6EF64F26, 0x6CF66DF6,
+0x6AF66BF6, 0x000B69F6, 0x4F2268F6, 0xE240614D,
+0x89143123, 0x3127E21F, 0x8B09D749, 0xD449614D,
+0xE00171E0, 0x5671440B, 0x26596507, 0x1761A007,
+0xE001D444, 0x6672440B, 0x26596507, 0x4F262762,
+0x0009000B, 0x614D4F22, 0x3123E240, 0xE21F8912,
+0xD73B3127, 0x614D8B08, 0x5671D23A, 0x420B71E0,
+0x260BE001, 0x1761A006, 0x6672D236, 0xE001420B,
+0x2762260B, 0x000B4F26, 0xE6400009, 0x46284618,
+0x6252D531, 0x89FC2268, 0x0009000B, 0x4618E680,
+0xD52D4628, 0x22686252, 0x000B89FC, 0xA0010009,
+0x7201E200, 0x8BFC3242, 0x0009000B, 0x4618E680,
+0xD5264628, 0x22686252, 0x000B8BFC, 0x2FE60009,
+0x7FFC4F22, 0xBFF16E53, 0x61E22F42, 0xE280D620,
+0x54E11615, 0x16464218, 0x422855E2, 0x57E31657,
+0x16786EF2, 0x26E22E2B, 0x4F267F04, 0x6EF6AFCE,
+0x00203494, 0x00117804, 0x002038F4, 0x00203908,
+0x0020050A, 0x00201008, 0x0020102E, 0x00203A58,
+0x002018A2, 0x002018E6, 0x00203A6C, 0x00203A74,
+0x00203A78, 0x001C3500, 0x001C1000, 0x0020398A,
+0x00117800, 0x002018EE, 0x00203A8C, 0x00203990,
+0x001C3704, 0x002033E8, 0x001C373C, 0x001C3700,
+0x001C370C, 0x2FD62FC6, 0x4F222FE6, 0x6C53DD10,
+0x6E43BFA4, 0x2DE2BF89, 0x0009BFA0, 0x2C1251D5,
+0x1C4154D6, 0x1C5255D7, 0x1C6356D8, 0x6EF64F26,
+0x000B6DF6, 0x61636CF6, 0xA004E600, 0x62564109,
+0x24227601, 0x36127404, 0x000B8BF9, 0x00000009,
+0x001C370C, 0x0009A16E, 0x2FE62FD6, 0xDD944F22,
+0xA0049EB2, 0xD4930009, 0x420BD293, 0x62D265D2,
+0x8BF822E8, 0x0009A004, 0xD28FD490, 0x55D1420B,
+0x22E852D1, 0xA0048BF8, 0xD48D0009, 0x420BD28A,
+0x52D255D2, 0x8BF822E8, 0x0009A004, 0xD286D489,
+0x55D3420B, 0x22E852D3, 0xA0048BF8, 0xD4860009,
+0x420BD281, 0x52D455D4, 0x8BF822E8, 0x6EF64F26,
+0x6DF6000B, 0x2FD62FC6, 0x4F222FE6, 0x6E636C73,
+0x6D53B01A, 0x64D357F4, 0xB05F65E3, 0xB07566C3,
+0xB0A40009, 0xB0A80009, 0xB0AC0009, 0xB0AC0009,
+0xB0AF0009, 0xB03154F5, 0x6CCD6C03, 0x4F2660C3,
+0x6DF66EF6, 0x6CF6000B, 0x3412D170, 0xD6700529,
+0x2650D770, 0x2742000B, 0x0009A018, 0x2FD62FC6,
+0x4F222FE6, 0x6E636C73, 0x6D53BFEE, 0x64D357F4,
+0xB03365E3, 0xB08D66C3, 0xB00F54F5, 0x6CCD6C03,
+0x4F2660C3, 0x6DF66EF6, 0x6CF6000B, 0xE503D162,
+0xD763D462, 0x21524518, 0x2472000B, 0xD45FD15E,
+0x2162E600, 0x2462000B, 0xBF734F22, 0xBF73E40A,
+0xD25C0009, 0x4118E104, 0xE40AE500, 0xBF692212,
+0xD7592252, 0xCB206072, 0x000B4F26, 0x4F222702,
+0x410BD156, 0xD556E400, 0x4F26452B, 0xD1552FE6,
+0x66126E63, 0x92104418, 0x44084528, 0x45002629,
+0x265B4408, 0x264B4400, 0x21624708, 0xD14E4708,
+0x217227EB, 0x6EF6000B, 0x1FFF03F0, 0x4F222FE6,
+0xE101DE4A, 0xBF3DE40A, 0x67E32E12, 0xE500776C,
+0xE204E130, 0x2752E40A, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27222712,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x175ABF18, 0x2E62E600, 0x000B4F26, 0xD2346EF6,
+0xE441E101, 0x000B2212, 0xD1322242, 0xE605D432,
+0x000B2162, 0x000B2462, 0xD2300009, 0xE40AE601,
+0x2262AF00, 0x2FC62FB6, 0x2FE62FD6, 0x7FFC4F22,
+0x6C43DB2B, 0xED0060B2, 0x2B02CB03, 0xC90360B2,
+0x6E03A008, 0x89073DC2, 0xE46460B2, 0xB07CC903,
+0x7D016E03, 0x8BF52EE8, 0x8F043DC2, 0xD4212FE1,
+0x460BD621, 0x62F10009, 0x6023622D, 0x89FFC801,
+0x7F046023, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x001C3B88, 0x00203AA0, 0x002018EE, 0x00203AA8,
+0x00203AB0, 0x00203AB8, 0x00203AC0, 0x0025E720,
+0x00203DA0, 0x002038F8, 0x001C5968, 0x001C3B40,
+0x000F8000, 0x001D4004, 0x001C3500, 0x002015E0,
+0x0020160C, 0x001C5814, 0x001C59D0, 0x001C5830,
+0x001C6268, 0x001C59A4, 0x001C639C, 0x001C581C,
+0x001C5860, 0x00203AC8, 0x002018A2, 0x8F014411,
+0x6043604B, 0x0009000B, 0x5651D52B, 0x46286052,
+0x306C000B, 0x2FC62FB6, 0x2FE62FD6, 0x4F124F22,
+0xBFF14F02, 0x6B036E43, 0xDD25DC24, 0x0009BFEC,
+0x3C0530B8, 0x4609060A, 0x46014609, 0x020A3D65,
+0x42094209, 0x32E24209, 0x4F068BF0, 0x4F264F16,
+0x6DF66EF6, 0x000B6CF6, 0x2FC66BF6, 0x2FE62FD6,
+0x4F124F22, 0xBFCF4F02, 0x6C036E43, 0xBFCBDD13,
+0x30C80009, 0x060A3D05, 0x46094609, 0x36E24601,
+0x4F068BF5, 0x4F264F16, 0x6DF66EF6, 0x6CF6000B,
+0x4F222FE6, 0xE102DE0B, 0xE403E500, 0xBFB92E12,
+0xE6062E52, 0xE7004618, 0x2E62E403, 0x4F262E72,
+0x6EF6AFB0, 0x0009000B, 0x001C1040, 0xCCCCCCCD,
+0x10624DD3, 0x001D4004, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE5007F98, 0x6453E710,
+0x6B534728, 0xEE1ADCBC, 0x6153655D, 0x315C4108,
+0x75014108, 0x6043317C, 0x0F16665D, 0xED0060B3,
+0x21B136E3, 0x81128111, 0x11D28113, 0x11D411D3,
+0x74048FEA, 0xD8B167F2, 0x1871D9B1, 0x58F12872,
+0x1981D1B0, 0x59F22982, 0x5DF45AF3, 0x54F65EF5,
+0x21921191, 0x11A211A3, 0x11D411D5, 0x11E611E7,
+0x11481149, 0xDAA855F7, 0x57F8EE00, 0x52F9DDA7,
+0x64E3D6A7, 0x2A521A51, 0xD8A7D9A6, 0x2D729AD5,
+0x6EED2622, 0x4D086DE3, 0x3DEC61E3, 0x4D084108,
+0x3D9C31EC, 0x410860B3, 0x81D12DB1, 0x4108E050,
+0x4008E7B7, 0x677C4108, 0x60B381D2, 0xE200318C,
+0x81D33472, 0x1D131DD2, 0x8D01D493, 0xD4901D24,
+0xB0B365D3, 0x64ED7E01, 0x8BDA34A2, 0x2FD2DA8C,
+0xDD9268A2, 0x2D824829, 0x7DFC64A2, 0xD287694D,
+0x6E222D92, 0x7D0C4E29, 0x68222DE2, 0x618D6AD3,
+0x2A16D784, 0xD48A6D72, 0x24D2D583, 0xD6895E72,
+0x517414E2, 0x1414EE00, 0xD1875876, 0x59781486,
+0x1498E710, 0x65E36252, 0x26E2142A, 0xE60064E3,
+0x6843644D, 0x384C4808, 0x381C4808, 0x0C866053,
+0x09CE28B1, 0x819160B3, 0x0ACE6053, 0x81A26043,
+0x0DCE6053, 0x81D360B3, 0x08CE6053, 0x18827401,
+0x624D09CE, 0x0ACE19E3, 0x1A643273, 0x75048FE0,
+0xE003D96A, 0x40186C92, 0x6D922CB1, 0x81D1DA6F,
+0x6E92E050, 0x81E24008, 0x60B36192, 0x64928113,
+0x1442E600, 0xD4696792, 0x689217A3, 0x1864E1FF,
+0x6563E703, 0x364C4608, 0x26127501, 0x3673665D,
+0xDC5B8BF8, 0x6DC2E003, 0x2DB14018, 0xD2606EC2,
+0x61C281E1, 0x1112EE00, 0xE02464C2, 0x65C21423,
+0x15E4D45B, 0xE58067C2, 0x68C28172, 0x818366E3,
+0x666D655C, 0x76046963, 0x394C6A6D, 0x8FF83A53,
+0xDB5429E2, 0x24B2DC54, 0x24C27404, 0x4F267F68,
+0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6, 0x68F6000B,
+0x60116142, 0x8F03C803, 0xD23DE500, 0x8B063420,
+0xC9036011, 0x8B068802, 0x3420D239, 0x56128B03,
+0x52646513, 0x000B2422, 0x01136053, 0x2FE62FD6,
+0x7FEC4F22, 0x62536E53, 0x6D43E550, 0x4508E400,
+0xE101A001, 0x60435224, 0x81212211, 0x60538123,
+0x56E28122, 0x8BF53620, 0x16E4D238, 0xE61464F3,
+0x65E3420B, 0xE4FC65E1, 0x2E512549, 0x65F361F1,
+0x2F112149, 0xD13154D1, 0xE614410B, 0x607157D1,
+0x2701CB01, 0x7F141DE1, 0x6EF64F26, 0x6DF6000B,
+0x2FE62FD6, 0x7FEC4F22, 0x66536E53, 0x6D43E5FC,
+0x20596061, 0x2601CB01, 0x326052E2, 0x12E48B06,
+0x31E051E2, 0x52D18B04, 0x1E22A002, 0x5664AFF0,
+0x64F3D21E, 0x420BE614, 0x67E165E3, 0x2719E1FC,
+0x67F12E71, 0x271954D1, 0x65F3D118, 0x410BE614,
+0x52D12F71, 0xCB016021, 0x1DE12201, 0x4F267F14,
+0x000B6EF6, 0x00006DF6, 0x00203924, 0x002034F4,
+0x002034FC, 0x00203504, 0x0020352C, 0x00203910,
+0x00203918, 0x00100208, 0x001017C0, 0x001E210C,
+0x001C3D00, 0x00203964, 0x001000C8, 0x00117880,
+0x00117780, 0x00040020, 0x0026C401, 0x00200ED6,
+0x4F222FE6, 0xDE42624C, 0x42004208, 0x3E2CA005,
+0xD4405252, 0xBF695624, 0x65E22E62, 0x352052E1,
+0xD63D8BF6, 0x4F262622, 0x6EF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xDC394F22, 0x52C1DB39, 0x362066C2,
+0x6061891C, 0x8801C903, 0xDE348918, 0xBF37DD35,
+0x650364E3, 0x66B28503, 0x3262620D, 0xD4328907,
+0x0009BF76, 0x4D0BD431, 0xAFE60009, 0xBF3D0009,
+0xD42F64E3, 0x00094D0B, 0x0009AFDF, 0x2262D22D,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x2FD62FC6,
+0x4F222FE6, 0xDD29DC28, 0x6E4360C2, 0x04DE4008,
+0xE614D127, 0x65E3410B, 0xD127D726, 0x55E227E2,
+0x35E05254, 0x21228F04, 0x400860C2, 0x122202DE,
+0x605365C2, 0x75014008, 0x0DE606DE, 0xC90F6053,
+0x60632C02, 0x6EF64F26, 0x000B6DF6, 0x85436CF6,
+0x650D5643, 0x622D6262, 0x35277204, 0xE1008F0C,
+0x2268960C, 0xD6158B03, 0x72015261, 0xD6131621,
+0x6262E101, 0x26227201, 0x6013000B, 0x000001FF,
+0x00203504, 0x002034FC, 0x001C3D00, 0x0020352C,
+0x002038F4, 0x002018A2, 0x002034F4, 0x00203AF0,
+0x00203AF4, 0x001C3D28, 0x00203964, 0x00203924,
+0x00200ED6, 0x00203968, 0x0020396C, 0x00117754,
+0x2FC62FB6, 0x2FE62FD6, 0x7FF84F22, 0x6022D237,
+0x8D58C803, 0xDE362F01, 0xDB37DC36, 0x66C252C1,
+0x892F3620, 0xC9036061, 0x892B8801, 0xD233DD31,
+0x64D3420B, 0x1F016503, 0x880160B1, 0xD2308B04,
+0x64D3420B, 0x0009AFEA, 0x85615653, 0x8904C820,
+0xE050D72C, 0x7201027E, 0xD22B0726, 0x6453420B,
+0x89072008, 0x55F1D126, 0x64D3410B, 0xE601D727,
+0x2762AFD4, 0x55F1D226, 0x64E3420B, 0xE601D125,
+0x2162AFCC, 0xDD25DE24, 0xDC26DB25, 0x66D252D1,
+0x89183620, 0xC9036061, 0x89148801, 0xD117D41F,
+0x0009410B, 0x36E05603, 0x65038F04, 0x2B20E201,
+0x2C52AFEC, 0xD712D41C, 0x0009470B, 0xE601D115,
+0xAFE34618, 0x60F12162, 0x8907C804, 0x7F08D217,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6422B, 0x4F267F08,
+0x6DF66EF6, 0x000B6CF6, 0x00006BF6, 0x001E2100,
+0x00203504, 0x002034FC, 0x0020398C, 0x002014A0,
+0x002014CC, 0x00203494, 0x002016BE, 0x001E212C,
+0x00201530, 0x001C3D30, 0x00117880, 0x002034F4,
+0x00203914, 0x00203910, 0x0020352C, 0x00200610,
+0xE601D203, 0x1265D503, 0x000B2252, 0x00001266,
+0x001C1010, 0x0000C34F, 0x0009000B, 0x2FD62FC6,
+0x4F222FE6, 0x6D436C53, 0xEE00A004, 0x7E0164D4,
+0x644CBFF2, 0x8BF93EC2, 0x6EF64F26, 0x000B6DF6,
+0xE5006CF6, 0x6643A002, 0x76017501, 0x22286260,
+0xAFE38BFA, 0x2FE60009, 0x75076253, 0xE1086753,
+0x6043EE0A, 0x4409C90F, 0x650330E2, 0x8D014409,
+0xE630E637, 0x4110365C, 0x8FF22760, 0xE00077FF,
+0x000B8028, 0x000B6EF6, 0x000BE000, 0x2FE6E000,
+0x7FEC4F22, 0x6E436253, 0xBFDC65F3, 0xBFD06423,
+0xBFCE64E3, 0xD40364F3, 0x0009BFCB, 0x4F267F14,
+0x6EF6000B, 0x00203AF8, 0xE4FDD29F, 0xD79F6122,
+0x22122149, 0x74016022, 0x2202CB01, 0xD59C6622,
+0x22622649, 0xC8406070, 0x60528902, 0x2502CB04,
+0xE1F76452, 0x25422419, 0xE7016052, 0x2502CB40,
+0xE6026052, 0x2502C9CF, 0x47186052, 0x2502CB10,
+0xCB036052, 0x15622502, 0x1573000B, 0xD78ED58D,
+0xD48FD28E, 0xE600E100, 0x27112511, 0xAFCB2210,
+0x664C2461, 0x4600D28B, 0x6060362C, 0x000BCB10,
+0x654C2600, 0x4500D287, 0x6650352C, 0x2619E1EF,
+0x2560000B, 0xD284664C, 0x362C4600, 0xCB106060,
+0x2600000B, 0xD280654C, 0x352C4500, 0xE1EF6650,
+0x000B2619, 0x664C2560, 0x4600D27A, 0x6060362C,
+0x000BCB08, 0x654C2600, 0x4500D276, 0x6650352C,
+0x2619E1F7, 0x2560000B, 0xD273664C, 0x362C4600,
+0xCB086060, 0x2600000B, 0xD26F654C, 0x352C4500,
+0xE1F76650, 0x000B2619, 0x624C2560, 0x4200D669,
+0x6020326C, 0x4021C908, 0x40214021, 0x600C000B,
+0xD665624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0xD161600C, 0x341C644C, 0x000B6240,
+0xD15F602C, 0x341C644C, 0x000B6240, 0x2FE6602C,
+0x6E434F22, 0xE60A645C, 0x89143467, 0x0009BFEB,
+0x60EC640C, 0x8B028801, 0xA002E00F, 0x44092409,
+0x624C4409, 0x3263E60A, 0xBFE28905, 0x620C644C,
+0xC8806023, 0xE2008B00, 0x4F266023, 0x6EF6000B,
+0xD64C4F22, 0x88016062, 0xB2578B03, 0xA0030009,
+0xD2490009, 0x2260E640, 0xE200D648, 0x000B4F26,
+0x4F222622, 0x6062D643, 0x8B018802, 0x0009B2A0,
+0xE200D642, 0x000B4F26, 0xD53E2622, 0xE100D43E,
+0x2512E701, 0x2470000B, 0xE604D23B, 0x2260000B,
+0xD43B4F22, 0x410BD13B, 0xD53B0009, 0x6650E1FD,
+0x2619D23A, 0x2560E700, 0x000B4F26, 0x4F222270,
+0xD238D537, 0xD7386152, 0x2512611D, 0xE6FF6452,
+0x2542242B, 0xD22FD435, 0x420B666D, 0xD52E2762,
+0x6750E1FB, 0x4F262719, 0x2570000B, 0xD4304F22,
+0x410BD128, 0xD5280009, 0x6650E7F7, 0x4F262679,
+0x2560000B, 0x9425D524, 0x22496250, 0x2520000B,
+0xE4BFD521, 0x22496250, 0x2520000B, 0xD2254F22,
+0x600D8522, 0x89112008, 0x89458801, 0x89478803,
+0x89498805, 0x894F8806, 0x89558808, 0x895B8809,
+0x8961880A, 0x8967880B, 0x0009A06E, 0x0009B070,
+0x600CA06B, 0x0000FF7F, 0x001E2148, 0x001E1000,
+0x001E1108, 0x002039C4, 0x002039C6, 0x002039E5,
+0x002039A8, 0x001E103F, 0x001E105F, 0x001E102F,
+0x001E1090, 0x002039CC, 0x001E100B, 0x002039C8,
+0x00203AFC, 0x002018A2, 0x001E1028, 0x002039E4,
+0x001D4020, 0x98760000, 0x001C1000, 0x00203B08,
+0x00203B18, 0x0020399C, 0x0009B04C, 0x600CA035,
+0x0009B055, 0x600CA031, 0x6260D684, 0x8B2B2228,
+0x0009B061, 0x600CA029, 0x6260D680, 0x8B232228,
+0x0009B069, 0x600CA021, 0x6260D67C, 0x8B1B2228,
+0x0009B0C7, 0x600CA019, 0x6260D678, 0x8B132228,
+0x0009B0CD, 0x600CA011, 0x6260D674, 0x8B0B2228,
+0x0009B125, 0x600CA009, 0x6260D670, 0x8B032228,
+0x0009B13D, 0x600CA001, 0x4F26E000, 0x0009000B,
+0xD26CD16B, 0xD56C8412, 0x4000C90F, 0xD76B012D,
+0xE403D66B, 0xE20F611C, 0x2540E001, 0x25202712,
+0x2602000B, 0xE601D262, 0x30668523, 0xE0008D05,
+0xD663D260, 0xE0018122, 0x000B2602, 0xD25C0009,
+0x600D8523, 0x89052008, 0x8B0A8801, 0x6060D65D,
+0x2600CB01, 0xD457D65A, 0xE001E101, 0x000B2612,
+0x000B8142, 0xD152E000, 0x8513E501, 0x640D4518,
+0x66033453, 0xE0008D05, 0xD551D253, 0x2260E001,
+0x000B2502, 0x4F220009, 0x8513D149, 0x6453650D,
+0x62494419, 0x227D672E, 0x8801602C, 0x88028909,
+0x88038910, 0x8806891A, 0x88078935, 0xA04C893B,
+0xD5460009, 0x6652D746, 0x2762D446, 0x622C6261,
+0x2421A038, 0x2228625C, 0xD4438B3F, 0x6642D540,
+0x2562D440, 0x24018561, 0x6203A02C, 0x2008605C,
+0x88108907, 0x88208908, 0x88308909, 0xA02C890A,
+0xD23A0009, 0x6222A008, 0xA005D239, 0xD2396222,
+0x6222A002, 0x6262D638, 0xD432D531, 0x66212522,
+0xA00F626C, 0xD6352421, 0x6261D52D, 0x622CD42D,
+0xA0072562, 0xD6322421, 0x8561D529, 0x2562D429,
+0x62032401, 0x662D8515, 0x3617610D, 0x65038F01,
+0xB0CB2451, 0xA0010009, 0xE000E001, 0x000B4F26,
+0xD6190009, 0xD427E101, 0x65412610, 0xD118D717,
+0xE20F655D, 0x2752E001, 0x000B2620, 0x2FE62102,
+0xD20F4F22, 0x640C8523, 0x8B082448, 0xD511D61D,
+0x2621E200, 0x940F8451, 0xA0482049, 0xDE0D8051,
+0xC84060E0, 0xE2018D32, 0x89443427, 0xD216D615,
+0x2641420B, 0x0009A030, 0x0000FF7F, 0x002039E5,
+0x0020399C, 0x002039A8, 0x001E1100, 0x001E100C,
+0x002039C8, 0x001E1000, 0x001E1001, 0x002039D0,
+0x002039B0, 0x002039B4, 0x002039B8, 0x002039D4,
+0x002039D8, 0x002039DC, 0x002039E0, 0x00203E04,
+0x00203E0E, 0x002039C2, 0x00202886, 0x89123427,
+0xD294D693, 0x2641420B, 0xCB8084E1, 0x80E1B0F5,
+0xD69160E0, 0x2E00CB04, 0xC93F6060, 0xD68F2600,
+0xA001E001, 0xE0002602, 0x000B4F26, 0xD68C6EF6,
+0xC8806060, 0xD2868919, 0x88016021, 0xD2898B15,
+0x8524E501, 0x89103056, 0xE203D187, 0x2120D487,
+0xE00B6541, 0x0656655D, 0xE40FD585, 0x2140E702,
+0xD77E2571, 0x000BE001, 0x000B2702, 0x2FE6E000,
+0xDE804F22, 0xC88084E1, 0xD57A892C, 0x20088554,
+0x61038F28, 0x8553D77C, 0x64036672, 0x8566650C,
+0x3520620C, 0xD6798B1E, 0x651CD774, 0x2651644C,
+0x60E02741, 0x8904C840, 0x420BD275, 0xA0030009,
+0xD2680009, 0x0009420B, 0x0009B09F, 0xE201D167,
+0x60E02122, 0xCB04D464, 0x60402E00, 0x2400C93F,
+0x6023A001, 0x4F26E000, 0x6EF6000B, 0x2FB62FA6,
+0x2FD62FC6, 0xDA622FE6, 0x66A1E240, 0x3622DC5E,
+0x62638900, 0x6ED36D2C, 0x4E2136D8, 0x4E212A61,
+0xDB61D460, 0xE700A00F, 0x770162B2, 0x71026123,
+0x66212B12, 0x71026213, 0x61212B12, 0x651D666D,
+0x356C4528, 0x627C2452, 0x8BED32E3, 0xC90360D3,
+0x8B108803, 0x617367B2, 0x2B127102, 0x71026E13,
+0x2B126571, 0x655D6DE1, 0x422862DD, 0x325CE107,
+0xA00C2C10, 0x88022422, 0xA0038B01, 0x8801E203,
+0xE2018B05, 0x66B22C20, 0x655D6561, 0xE60F2452,
+0x67A12C60, 0x8B052778, 0xDD38DC44, 0xEB01EA00,
+0x2DB22CA2, 0x6DF66EF6, 0x6BF66CF6, 0x6AF6000B,
+0x2FE62FD6, 0xE240DD36, 0x362266D1, 0x62638900,
+0x3678672C, 0x7703DE38, 0x47212D61, 0x64E2D635,
+0xA00E4721, 0x6562E100, 0x62537101, 0x74012450,
+0x24204219, 0x45297401, 0x74012450, 0x24504519,
+0x621C7401, 0x8BEE3273, 0x66E24200, 0x420061D1,
+0x2118362C, 0x2E628F06, 0xDD1CD728, 0xE501E400,
+0x2D522742, 0x000B6EF6, 0x2FD66DF6, 0x4F222FE6,
+0xED0AEE01, 0x64E3BC85, 0xBC8A64E3, 0x62EC7E01,
+0x8BF732D7, 0xBC8DEE01, 0x64E364E3, 0x7E01BC92,
+0x32D762EC, 0x4F268BF7, 0x000B6EF6, 0xD1186DF6,
+0xD418920D, 0x72122122, 0x2422D617, 0xD7177204,
+0x72202622, 0x2722D116, 0x000B7230, 0x137A2122,
+0x002039C2, 0x00202992, 0x001E1015, 0x002039C8,
+0x001E1001, 0x0020399C, 0x001E1100, 0x002039C6,
+0x002039B4, 0x001E1000, 0x002039B8, 0x002039C4,
+0x00202886, 0x001E100C, 0x002039B0, 0x002039CC,
+0x002039D0, 0x002039D4, 0x002039D8, 0x002039DC,
+0x002039E0, 0x4F222FE6, 0xD6707FFC, 0x88016060,
+0xE2018951, 0x2620BFBB, 0xD56ED16D, 0xDE6E6010,
+0x64E36552, 0x7402C840, 0x8D22D16C, 0xD26C7502,
+0xE601D76C, 0xE7042722, 0x76016255, 0x626C2421,
+0x8FF93273, 0xD4637402, 0x6242E601, 0x640D8528,
+0x67494419, 0x275D657E, 0x81E4607C, 0xE417D562,
+0x67557601, 0x3243626C, 0x8FF92171, 0xA0207102,
+0xD25E0009, 0xE601D75B, 0xE7042722, 0x76016255,
+0x626C2421, 0x8FF93273, 0xD4527402, 0x6242E601,
+0x640D8528, 0x67494419, 0x275D657E, 0x81E4607C,
+0xE417D553, 0x67557601, 0x3243626C, 0x8FF92171,
+0x92897102, 0xD2462E21, 0x5E23D74E, 0x64F22FE2,
+0x604365F2, 0x2700C980, 0xC9606043, 0x80716103,
+0xC9036043, 0x80724519, 0x65F2605C, 0x817266F2,
+0x46194629, 0x606C4529, 0x4018645C, 0x8173304C,
+0x21185E23, 0x64F22FE2, 0x6E4C62F2, 0x602C4219,
+0x66F262F2, 0x46294018, 0x461930EC, 0x42298174,
+0x652C606C, 0x305C4018, 0x81758F07, 0x0009BC96,
+0x2228620C, 0xA00A8908, 0x60130009, 0x8B038840,
+0x0009B009, 0x0009A003, 0xE202D62F, 0x7F042622,
+0x000B4F26, 0x4F226EF6, 0x8552D52A, 0x8830600D,
+0x88318903, 0xA0348923, 0x85550009, 0xD428D727,
+0x85532701, 0x610DD627, 0x24124118, 0x460BD426,
+0xD7230009, 0xD226D425, 0x6572420B, 0xE230D120,
+0x42286712, 0x2729E620, 0x37604628, 0xD6218B03,
+0xA016E200, 0xD61F2622, 0xA012E202, 0xD1182622,
+0x6212E530, 0xE6204528, 0x46282259, 0x89083260,
+0xD41AD119, 0xE601D513, 0x2160450B, 0x472BD718,
+0x4F264F26, 0x0009000B, 0x0000060A, 0x002039E4,
+0x001E1000, 0x002039D0, 0x00203E04, 0x00203E10,
+0x00203DA8, 0x002039B8, 0x00203DD8, 0x00203DD6,
+0x00203DAA, 0x0020399C, 0x002039C8, 0x002039B4,
+0x002039B0, 0x002018A2, 0x00203B24, 0x00203B28,
+0x002018EE, 0x002039CC, 0x001E100B, 0x00203B3C,
+0x00114004, 0x4F222FE6, 0xDE967FFC, 0x200884E9,
+0x2F008D06, 0xD695D494, 0x0009460B, 0x64F0B19A,
+0x6620D293, 0x89022668, 0xC9BF60E0, 0x7F042E00,
+0x000B4F26, 0x000B6EF6, 0x2FE60009, 0xDE8D4F22,
+0x60E0D68D, 0xCBC0D48D, 0x62602E00, 0xC803602C,
+0x40218904, 0x70014021, 0x6603A002, 0x66034009,
+0xD687616D, 0xE500A004, 0x75016262, 0x74042422,
+0x3213625D, 0xD2838BF8, 0x0009420B, 0xC9BF84E2,
+0x4F2680E2, 0x6EF6000B, 0x2FE62FD6, 0x7FFC4F22,
+0x6260D67D, 0x89442228, 0xD572E100, 0x60502610,
+0xCB40D47A, 0x2500440B, 0x8D052008, 0x62E06E03,
+0x7104612C, 0x2F11A006, 0xD475D66D, 0xDD756760,
+0x657C4D0B, 0xE23C6D1D, 0x8B033D27, 0xD267D472,
+0x0009420B, 0x4D214D21, 0xA005D770, 0x66E6E400,
+0x357C4508, 0x74012562, 0x35D3654D, 0xD76C8BF7,
+0x6172E003, 0x81114018, 0x6E7260F1, 0x81E2700C,
+0xD4686172, 0xDD688113, 0x4D0BDE68, 0xE2016572,
+0xD4672E22, 0x420BD255, 0xD6560009, 0xC93F6060,
+0x7F042600, 0x6EF64F26, 0x6DF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xD25F4F22, 0x6B436E73, 0x420B6C53,
+0x20086D63, 0x64038D1C, 0xE50ED149, 0x32526210,
+0x60C38916, 0x804124B0, 0x814160D3, 0xA007E500,
+0x655D61BC, 0x00EC6053, 0x364C6653, 0x80647501,
+0x3213625D, 0xD63B8BF5, 0xC9BF6060, 0x2600A008,
+0xD23AD44D, 0x6EF64F26, 0x6CF66DF6, 0x6BF6422B,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x7FC44F22,
+0x720262F3, 0x22512F41, 0x45297202, 0x60632251,
+0xE5C4E682, 0x67F38121, 0x655C666C, 0xE408BFB6,
+0x4F267F3C, 0x0009000B, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE1007FC4, 0x6513ECFF,
+0x6B136CCD, 0xDE36D735, 0xEDFF64F3, 0xD835EA04,
+0x6053655C, 0x027D4000, 0x32C0622D, 0x66038D0D,
+0x09ED6063, 0x2491027D, 0x24217402, 0x698202ED,
+0x3928622D, 0x74022892, 0x75017104, 0x6063625C,
+0x07D532A2, 0x0EB58FE4, 0x2448641C, 0xE6808905,
+0x67F3E5C5, 0xBF79666C, 0x7F3C655C, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0xD11E68F6,
+0x6012D21E, 0xCB20E405, 0x2102E500, 0x000B2242,
+0x00002252, 0x001E1017, 0x00203B40, 0x002018A2,
+0x0020390E, 0x001E1015, 0x001E10BF, 0x00117800,
+0x001E10FC, 0x00200610, 0x00203914, 0x00202AEA,
+0x00203B44, 0x002018EE, 0x00203B60, 0x0011788C,
+0x00203910, 0x002034F4, 0x00201530, 0x001E2130,
+0x00203B68, 0x00202AAC, 0x00203B6C, 0x00203974,
+0x0020397C, 0x00203DA4, 0x001C3500, 0x001D4004,
+0xD564D163, 0xE400D764, 0x2142E20F, 0x17411154,
+0xD5622722, 0x9669D762, 0x15412572, 0x96661562,
+0xE6011565, 0xD55F1165, 0x666CE6F8, 0x25422542,
+0x25422542, 0x25422542, 0x25622542, 0x7601E727,
+0x67632572, 0x25627797, 0xE7042572, 0x2572E248,
+0xE2192522, 0xE2702522, 0x25422542, 0x25422542,
+0x25222542, 0x2522E20C, 0x25422542, 0x25422542,
+0x25422542, 0x25422542, 0x000B154A, 0xE2081145,
+0x0009422B, 0x2FE62FD6, 0x7FFC4F22, 0xC8206043,
+0x6E438D02, 0x0009BE67, 0xC81060E3, 0xBE648901,
+0x60E30009, 0x8901C840, 0x0009BE86, 0xC80160E3,
+0xDD3D8938, 0xC80260D0, 0x2F008D03, 0x460BD63B,
+0x60F00009, 0x8902C804, 0x460BD639, 0x62F00009,
+0xC8806023, 0x60D08902, 0x2D00C97F, 0xC8016023,
+0xD6348906, 0x0009460B, 0x0009A007, 0x51630601,
+0x8902C808, 0x460BD630, 0x60F00009, 0x8902C810,
+0x420BD22E, 0xD52E0009, 0x88026052, 0xD22D8B03,
+0xA005E604, 0x88012260, 0xD22A8B02, 0x2260E601,
+0x2522E200, 0xC88060E3, 0xD227892D, 0x60E36E20,
+0x8902C880, 0x420BD225, 0x60E30009, 0x8902C840,
+0x420BD223, 0x60E30009, 0x8902C802, 0x420BD221,
+0x60E30009, 0x890DC804, 0xDD20D11F, 0x0009410B,
+0x0009BF0D, 0x0009BF4C, 0xD51ED41D, 0x2470E708,
+0x25D2BF85, 0xC80860E3, 0xD21B8905, 0x4F267F04,
+0x422B6EF6, 0x7F046DF6, 0x6EF64F26, 0x6DF6000B,
+0x001C581C, 0xA000A000, 0x001D0100, 0x001D4000,
+0x00040021, 0x001C589C, 0x001E1021, 0x00201A90,
+0x00201AB2, 0x00202114, 0x00201ACA, 0x00201AD8,
+0x002039C8, 0x001E100B, 0x001E1028, 0x00201B44,
+0x00201B50, 0x00201AE0, 0x00201AFE, 0x12345678,
+0x001E1000, 0x0010F100, 0x00201B2C, 0x644CD6A7,
+0x000B346C, 0xD6A62450, 0x346C644C, 0x2450000B,
+0x644CD6A4, 0x000B346C, 0x625C2450, 0x4208616D,
+0x42084119, 0x42006019, 0x670E614C, 0xD49E321C,
+0x4200207D, 0x324CC90F, 0x2200000B, 0x4208625C,
+0x42004208, 0x324C644C, 0x4200D498, 0x000B324C,
+0x2FE62260, 0x614C4F12, 0x4100D493, 0x6710314C,
+0xE29F666D, 0x27294619, 0x6E536269, 0x672E6573,
+0x4221227D, 0x42214221, 0x7601662C, 0xE4014608,
+0x34E84608, 0x644C4600, 0x071A0467, 0x2150257B,
+0x000B4F16, 0x4F226EF6, 0xD2857FE8, 0x88016021,
+0xD2848B7B, 0x26686621, 0xD2838B77, 0x26686621,
+0xE50F8B73, 0xE401BFA2, 0xBFA4E501, 0xE586E400,
+0xE400655C, 0x2F50BFA4, 0xBFA1E401, 0xE602E506,
+0x60634618, 0x81F2E401, 0x6543BF9F, 0xE40185F2,
+0xBFAB6543, 0x85F26603, 0x6543E401, 0x6603BFB1,
+0xE40265F0, 0x6053756C, 0x80F8BF80, 0xBF82E402,
+0x84F8E512, 0x7090E402, 0x6503BF82, 0x4618E602,
+0x81F66063, 0xBF80E402, 0x85F6E500, 0x6603E402,
+0xE500BF8C, 0xE40285F6, 0xBF926603, 0xE5FEE500,
+0xE010655C, 0xBF61E403, 0xE5130F54, 0xE40EBF63,
+0x05FCE010, 0xBF63E40E, 0xE5007585, 0xBF64E403,
+0xE500E640, 0xBF71E403, 0xE500E640, 0xBF78E403,
+0xE5FFE640, 0xE014655C, 0xBF47E404, 0xE40F0F54,
+0xE504BF49, 0x05FCE014, 0xBF49E40F, 0xE5017584,
+0xBF4AE640, 0xE501E404, 0xBF57E640, 0xE501E404,
+0xE404E640, 0xAF5C7F18, 0x7F184F26, 0x000B4F26,
+0x4F220009, 0xD2427FF0, 0x88016021, 0xD2418B71,
+0x26686621, 0xD2408B6D, 0x26686621, 0xE50F8B69,
+0xE401BF1C, 0xBF1EE501, 0xE586E400, 0xE400655C,
+0x2F50BF1E, 0xBF1BE401, 0xE401E506, 0xBF1C6543,
+0xE401E640, 0xBF296543, 0xE401E640, 0xBF306543,
+0x65F0E640, 0x756CE402, 0xBEFF6053, 0xE40280F4,
+0xE512BF01, 0xE40284F4, 0xBF017090, 0xE6406503,
+0xBF02E402, 0xE640E500, 0xBF0FE402, 0xE640E500,
+0xBF16E402, 0xE5FEE500, 0x6053655C, 0xBEE5E403,
+0xE51380F8, 0xE40EBEE7, 0xE40E84F8, 0xBEE77085,
+0xE5006503, 0xBEE8E640, 0xE500E403, 0xBEF5E640,
+0xE500E403, 0xBEFCE640, 0xE5FFE403, 0x6053655C,
+0xBECBE404, 0xE40F80FC, 0xE504BECD, 0xE40F84FC,
+0xBECD7083, 0xE5016503, 0xBECEE640, 0xE501E404,
+0xBEDBE640, 0xE501E404, 0xE404E640, 0xAEE07F10,
+0x7F104F26, 0x000B4F26, 0x00000009, 0x001E102F,
+0x001E1080, 0x001E1090, 0x001E103F, 0x001E103E,
+0x002039C2, 0x002039C4, 0x002039C6, 0xD21DD11C,
+0x66206010, 0x676C7001, 0x3700C90F, 0xE5008D13,
+0x67106210, 0x7701622C, 0x64232170, 0xD6166010,
+0x44084408, 0x3428C90F, 0x62602100, 0x7201D513,
+0x44082620, 0x000B354C, 0xD10F6053, 0x25586510,
+0xE6008D13, 0xD60DD40B, 0x655C6540, 0x47086753,
+0x37584708, 0x47086540, 0x24507501, 0x367C6040,
+0x2400C90F, 0x72FF6210, 0x000B2120, 0x00006063,
+0x0020390D, 0x0020390C, 0x0020390E, 0x00203534,
+0x7FFC4F22, 0xE680D19F, 0x666C6212, 0xD29E2F22,
+0x67F36563, 0x420B7542, 0x7F04E404, 0x000B4F26,
+0xE6800009, 0xD298666C, 0xE7006563, 0x422B7540,
+0xE6806473, 0xD294666C, 0xE7006563, 0x422B7543,
+0x2F866473, 0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6,
+0x7FCC4F22, 0xDC8ED28D, 0x72011F21, 0xDB8D1F22,
+0xD18EDE8D, 0x66125211, 0x8B013620, 0x0009A0E5,
+0xC9036061, 0x8B018801, 0x0009A0DF, 0xD288D487,
+0xED84420B, 0x2F025503, 0x30D0845C, 0xA0B88901,
+0xD1840009, 0x626C6610, 0x88016023, 0xD1828B68,
+0x62101FC3, 0x895B2228, 0xE003D480, 0x40186742,
+0x68421772, 0xD57EE900, 0x81816DB3, 0x7D042190,
+0x67D26AB2, 0x64E26852, 0x1F491F57, 0x740464E3,
+0x1FA46542, 0x65431F5A, 0x625275F8, 0x1F761FD5,
+0x6D531F2B, 0xDA74D773, 0x7D94D274, 0x68D21F88,
+0x6AA26972, 0xD1726022, 0x2202CB20, 0xE1401F1C,
+0x7601E600, 0x3213626D, 0x56F48BFB, 0x52F651F5,
+0x21222B62, 0x52F851F7, 0x212256F9, 0x2E6251FA,
+0x51FB2412, 0x2D822512, 0xD9662792, 0x29A2DD5F,
+0x6AD2D965, 0xD9646892, 0x68D21A84, 0x6081DA63,
+0x2801CB01, 0xD86266D2, 0x2A622962, 0xED015AFC,
+0x2AD2480B, 0x2AD24D18, 0x62D2DD5E, 0x2D227201,
+0xD15056F3, 0xE2026062, 0x2602CB01, 0x2120A03D,
+0x8B3A2228, 0xE401DD58, 0x2140E600, 0xE01C2D62,
+0xC801005C, 0xD4558B0A, 0xE600D755, 0xED7D2472,
+0x626C7601, 0x8BFB32D3, 0x24D2DD52, 0xE2FE68C2,
+0x2C822829, 0x095CE01E, 0xE01F5DF1, 0x0A5C2D90,
+0x751051F2, 0xED0621A0, 0xD74BE600, 0x8456D44B,
+0x27007601, 0x696C6854, 0x248039D3, 0x8FF67401,
+0xDA477701, 0x2A10E194, 0xE2007A01, 0x7A0F2A20,
+0xD130E805, 0x66102A80, 0x6023626C, 0x89088801,
+0xD240D42A, 0x420B65F2, 0xD131ED01, 0xAF304D18,
+0x65F221D2, 0x8553D43C, 0x620D6642, 0x89073262,
+0xD13BD43A, 0x0009410B, 0xE601D73A, 0x2762AF1A,
+0xD134D41E, 0x410B65F2, 0xD125ED01, 0xD637D436,
+0x460B4D18, 0xAF0D21D2, 0x7F340009, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0x4F2268F6,
+0x85467FF4, 0x2F01E681, 0x666C8547, 0x854881F1,
+0x81F2D209, 0x67F38542, 0x854381F3, 0x81F4E40C,
+0x65636053, 0x420B81F5, 0x7F0C7540, 0x000B4F26,
+0x00000009, 0x001C3D9C, 0x0020245C, 0x0011779A,
+0x001C36F8, 0x001C3B9C, 0x001C3704, 0x0020352C,
+0x002014A0, 0x0020391D, 0x0020391C, 0x00203918,
+0x001C3D98, 0x001C3BB4, 0x001C5960, 0x001C3500,
+0x001C3D30, 0x001C8960, 0x00203504, 0x001C3D00,
+0x0020160C, 0x00117730, 0x00203920, 0x001C582C,
+0x2000A000, 0x0000A000, 0x0011778C, 0x00117792,
+0x00117788, 0x002014CC, 0x002038F4, 0x002034F4,
+0x00201530, 0x001E2130, 0x00203D84, 0x002018A2,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xD19B7FEC, 0x2F12E000, 0x6103D49A, 0x1F4281F2,
+0xDD9ADA99, 0xD69A6813, 0xE0014808, 0x460BDE99,
+0x38EC4800, 0x65A21F03, 0x352052A1, 0xA23E8B01,
+0x60510009, 0x8801C903, 0xA2388B01, 0x52530009,
+0x32E0DE91, 0xD9918B10, 0x64A3490B, 0x4B0BDB90,
+0xDE906403, 0xD791D690, 0xEC01D591, 0x2E02E100,
+0x271026C0, 0x2502AFDF, 0xC8018551, 0xA1578B01,
+0x62510009, 0x4200622D, 0x5E53366A, 0x85E2226D,
+0xC903642C, 0x85E36603, 0x6053650D, 0x40214021,
+0x4500C93F, 0x322A6703, 0x6053252D, 0xC901D17F,
+0x60106C03, 0x8801D97F, 0xDB7F8B05, 0x2120E200,
+0xCB0160B2, 0xD17D2B02, 0x88016011, 0x65A28B0A,
+0x8D042448, 0x9B9E6251, 0xA00322B9, 0x919B2521,
+0x2521221B, 0x37B3EB10, 0x2448895E, 0xD4738B07,
+0x22286241, 0x60638903, 0xA05781F8, 0xD5706473,
+0x46084608, 0x85E26273, 0x46006B50, 0x362C4200,
+0x2BB8C910, 0x8F1F6463, 0x26686603, 0xD2698911,
+0x062D6043, 0x4119616D, 0x6B0E6019, 0x81F820BD,
+0x880160C3, 0x646C8F2C, 0x880F6073, 0xA0278B1B,
+0xD2610009, 0x052D6043, 0x4119615D, 0x670E6019,
+0x645C207D, 0x81F8A01C, 0x890F2668, 0x6043D25B,
+0x6B5D052D, 0x60B94B19, 0x201D610E, 0x60C381F8,
+0x8F0D8801, 0x6473645C, 0xEC00A00A, 0x6043D254,
+0x625D052D, 0x60294219, 0x207D670E, 0x81F8645C,
+0x880285F8, 0x85E1890A, 0x8D07C820, 0xE6DC6203,
+0x60232269, 0x81E1A002, 0x644CE4FF, 0x6210D149,
+0x89012228, 0x644CE4FF, 0x654DEBFF, 0x35B06BBC,
+0xDB368B2B, 0x64A34B0B, 0x410BD135, 0x54036403,
+0x85446E03, 0xC948DB40, 0xDC408808, 0xBEAE8B01,
+0x64B3E502, 0x65E34C0B, 0xDB3DEC01, 0xD13D2DC2,
+0x621260B2, 0x72017001, 0x21228805, 0x2B028F08,
+0x666CE680, 0x6563D238, 0x7549E700, 0x6473420B,
+0xA030D436, 0x7FFF0009, 0x85E28000, 0x20B9EBFC,
+0x610381E2, 0x942A85E3, 0x62032049, 0x450885F8,
+0x81E2201B, 0xC90160C3, 0x40084018, 0x40084008,
+0x4000225B, 0x6023220B, 0x85E481E3, 0x4118E108,
+0x81E4201B, 0xE40262A2, 0x20B98521, 0x67A28121,
+0xCB016071, 0x85F82701, 0x89033042, 0xECE785E2,
+0x81E220C9, 0x490BD41E, 0xA03B0009, 0x7E030009,
+0x001C3D30, 0x00203D90, 0x00203504, 0x001E212C,
+0x002033E8, 0x001C3D00, 0x00117780, 0x002014A0,
+0x0020166C, 0x0011770C, 0x0020391C, 0x0020391D,
+0x00203918, 0x002018A2, 0x001C36F8, 0x00203990,
+0x00203DA0, 0x00203B84, 0x00203C04, 0x00203C84,
+0x00203D04, 0x00203908, 0x002034FC, 0x002014CC,
+0x00203994, 0x00203998, 0x0020245C, 0x00203D88,
+0x00203D8C, 0x602262F2, 0x40094019, 0xC90F4009,
+0x8B0B880A, 0x60E2DE8C, 0x40094019, 0xC90F4009,
+0x8B038808, 0xCB0160A2, 0x2802A006, 0x65E2DE87,
+0x2E527501, 0x286266A2, 0x52F366F2, 0x2622AE83,
+0xD2838551, 0xDE83C802, 0xA0958B01, 0x420B0009,
+0x4E0B64A3, 0x5E036403, 0x85E46503, 0x4918E908,
+0xD77D209B, 0xE04C81E4, 0xDC7C0B7E, 0x7B01D97C,
+0x61C207B6, 0x71016690, 0x8D062668, 0xD4792C12,
+0x420BD279, 0xA070EB01, 0x62512DB2, 0x4B18EB0F,
+0x22B9E102, 0x32104118, 0x85518B0F, 0x2029E2FC,
+0x60518151, 0xCB0172E0, 0x85E12501, 0x202994A3,
+0x85E481E1, 0xA0522049, 0x675181E4, 0x4719677D,
+0x667E6779, 0x7701276D, 0x6903607C, 0x88014918,
+0x25918F3E, 0x6B12D161, 0x21B27B01, 0x660D85E3,
+0x40216063, 0xC93F4021, 0x6C034600, 0x262D322A,
+0xC8016063, 0xDB5ED15D, 0x967D8901, 0xE6002C6B,
+0x666C67CD, 0x40006063, 0x622D021D, 0x8D0E3270,
+0x60436403, 0xE9FF021D, 0x8B013290, 0x01C5A007,
+0x626C7601, 0x3292E904, 0x646C8BEB, 0x60434400,
+0xD15004BD, 0x0B457401, 0x669D6911, 0x89073670,
+0x602D6211, 0x890388FF, 0xE201DB4B, 0x2B2021C1,
+0xECFC8551, 0x815120C9, 0xCB016051, 0xDC472501,
+0x64A34C0B, 0x51F366F2, 0x85EF2612, 0x54F2D244,
+0x650D420B, 0x0009ADE7, 0xE500DC42, 0x420B2C52,
+0x4E0B64A3, 0x54036403, 0x85446E03, 0x6703E908,
+0x65034918, 0x27998541, 0xDB323790, 0x8F0BD932,
+0x6013610D, 0x8B07C820, 0xC9486053, 0x8B038808,
+0xE501BD4D, 0x0009A005, 0x2128D233, 0xBD468901,
+0x64B3E500, 0x490B65E3, 0xADBCEC01, 0x85F22DC2,
+0x7001EE04, 0x31E7610D, 0x8D0281F2, 0xADA97A08,
+0x7F140009, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0xF7FF68F6, 0x2FE68000, 0xD2234F22,
+0x60E36E22, 0x8D02C840, 0xBBF922E2, 0xE2400009,
+0x2E284218, 0xBC048901, 0x60E30009, 0x8905C810,
+0xD21CD41B, 0x0009420B, 0x0009BC03, 0xC80560E3,
+0xBD6D8901, 0x60E30009, 0x8902C802, 0xAC004F26,
+0x4F266EF6, 0x6EF6000B, 0x001C3D3C, 0x00117760,
+0x002014A0, 0x0020166C, 0x00203494, 0x00203DA4,
+0x00203908, 0x002034FC, 0x002014CC, 0x00203974,
+0x0020397C, 0x00203970, 0x00203972, 0x00201530,
+0x002018EE, 0x00203994, 0x00008000, 0x001C3510,
+0x00203D98, 0x002018A2, 0x080A0C0E, 0x00020406,
+0x1A1C1E20, 0x12141618, 0x2E303234, 0x26282A2C,
+0x3A3C3E40, 0x6C625648, 0x41112F26, 0xE2208F18,
+0x890B3123, 0x321CD204, 0xD1026220, 0x412B312C,
+0x00090009, 0x00203412, 0x002033C8, 0x000BE000,
+0x400062F6, 0x40004000, 0x40004000, 0x40004000,
+0x62F6000B, 0x40004000, 0x40004000, 0x40004000,
+0x40184000, 0x62F6000B, 0x40004000, 0x40004000,
+0x40004000, 0x40284000, 0x62F6000B, 0x40004000,
+0x40184000, 0x000B4028, 0xC90F62F6, 0x40054005,
+0x40054005, 0x62F6000B, 0x4005C907, 0x40054005,
+0x62F6000B, 0x4005C903, 0x000B4005, 0xC90162F6,
+0x000B4005, 0x000062F6, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x544F0D0A, 0x46205355, 0x00003A57,
+0x206C754A, 0x32203120, 0x20383030, 0x323A3132,
+0x34333A38, 0x00000000, 0x00000D0A, 0x00000043,
+0x42707372, 0x3D206675, 0x554E203D, 0x202C4C4C,
+0x6E49677A, 0x4E497274, 0x6D754E51, 0x0000003D,
+0x61766E49, 0x2064696C, 0x72657375, 0x20726F20,
+0x2079656B, 0x00214449, 0x52504545, 0x57204D4F,
+0x65746972, 0x6461202C, 0x003D7264, 0x6C617620,
+0x0000003D, 0x00000A0D, 0x435F4D5A, 0x465F444D,
+0x4C445F57, 0x494E495F, 0x00000054, 0x6E6B6E55,
+0x206E776F, 0x6D6D6F63, 0x3D646E61, 0x00000000,
+0x203A3051, 0x00000020, 0x203A3151, 0x00000020,
+0x203A3251, 0x00000020, 0x203A3351, 0x00000020,
+0x203A3451, 0x00000020, 0x2B434741, 0x73696F4E,
+0x61432065, 0x7262696C, 0x6F697461, 0x6166206E,
+0x6F206C69, 0x6974206E, 0x0D0A656D, 0x00000000,
+0x00000072, 0x00205220, 0x00000D0A, 0x62735576,
+0x7473725F, 0x00000A0D, 0x62735576, 0x7375735F,
+0x646E6570, 0x00000A0D, 0x62735576, 0x7365725F,
+0x000A0D6D, 0x00000044, 0x44387570, 0x72637365,
+0x6F747069, 0x3D584572, 0x00000000, 0x00000047,
+0x00000042, 0x72746E49, 0x6D652051, 0x2C797470,
+0x49677A20, 0x4972746E, 0x754E514E, 0x00003D6D,
+0x654C7245, 0x0000006E, 0x00000049, 0x20746F4E,
+0x756F6E65, 0x49206867, 0x4220514E, 0x0A0D6675,
+0x00000000, 0x000000FF, 0x00020001, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00020003, 0x01090108,
+0x0002010A, 0x02000003, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x000000FF, 0x00020001, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00020003, 0x01090108,
+0x0002010A, 0x00030003, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00FF010F, 0x01090108,
+0x010B010A, 0x0200010F, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00FF010F, 0x01090108,
+0x010B010A, 0x010F010F, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00205220, 0x00000046, 0x00000059,
+0x73204142, 0x003D7165, 0x49544120, 0x0000204D,
+0x00000000, 0x00000000, 0x002E0209, 0x80000101,
+0x000409FA, 0x00FF0400, 0x05070000, 0x02000201,
+0x82050700, 0x00020002, 0x03830507, 0x07010040,
+0x40030405, 0x02090100, 0x0101002E, 0x09FA8000,
+0x04000004, 0x000000FF, 0x02010507, 0x07000040,
+0x40028205, 0x05070000, 0x00400383, 0x04050701,
+0x00004002, 0x00000000, 0x00000000, 0x07090000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+};
+
+const u32_t zcFwImageSize=15936;
diff --git a/drivers/staging/otus/hal/hpfwu.c.drv_ba_resend b/drivers/staging/otus/hal/hpfwu.c.drv_ba_resend
new file mode 100644
index 000000000000..7f5bcff57b59
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwu.c.drv_ba_resend
@@ -0,0 +1,742 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE297FFC, 0xE114D729,
+0x1E13D429, 0x1E4C470B, 0x0009B018, 0xA0039545,
+0x3652E600, 0x76018D04, 0xC84060E2, 0x2F028DF9,
+0xDE23D422, 0x00094E0B, 0x4E0BD422, 0xD4220009,
+0x00094E0B, 0x4F267F04, 0x6EF6A024, 0xD11F4F22,
+0x0009410B, 0x440BD41E, 0xD51E0009, 0x0009450B,
+0xE1FFD71D, 0xD21D611D, 0x50292712, 0xCB01D41C,
+0xE501E1FF, 0x22121209, 0x24521211, 0xD61AD519,
+0xE2009714, 0xD4192572, 0xD6192620, 0x4F262422,
+0x2622000B, 0xDD18DC17, 0x4C0BDE18, 0x4D0B0009,
+0x4E0B0009, 0xAFF80009, 0x27100009, 0x00000640,
+0x001C001C, 0x00200BC4, 0x0000B38E, 0x002029F8,
+0x00200F72, 0x00202A04, 0x00202A1C, 0x00200F20,
+0x00201056, 0x00200C1C, 0x001C3510, 0x001C3624,
+0x001E212C, 0x00202994, 0x00202530, 0x0020299C,
+0x002029A8, 0x00200E50, 0x002023E6, 0x00201920,
+0x2FC62F96, 0x2FE62FD6, 0x7F904F22, 0xE020DE8D,
+0xD48D61E0, 0x61E30F14, 0x62107101, 0xE024D78B,
+0x0F24470B, 0x450BD58A, 0x20080009, 0x8F116D03,
+0xDD881F0A, 0x67D0D488, 0x410BD188, 0xD288657C,
+0x6920DD88, 0x66C36C9C, 0x46084608, 0x460836C8,
+0x1FDA3D6C, 0x04FCE024, 0x66E2E580, 0x655C604C,
+0x8F163050, 0xE0202D62, 0xE50001FC, 0xDE7E641C,
+0x3243625D, 0xA32C8B01, 0x655D0009, 0x36EC6653,
+0xE02C6760, 0x69530F74, 0x39DC607E, 0xAFEF8094,
+0x20087501, 0xE0208B14, 0xE50001FC, 0xA00ADE72,
+0x655D641C, 0x39EC6953, 0x67536C92, 0x37DC62C2,
+0x75041721, 0x625D1F2C, 0x8BF23243, 0x2D10A309,
+0x8B178801, 0x01FCE020, 0x2D70E700, 0x1FD76D1C,
+0x627DDE65, 0x8B0132D3, 0x0009A2FB, 0x65E3677D,
+0x75046673, 0x36EC6C73, 0x64623C5C, 0x770869C2,
+0x2492AFEF, 0x8B188804, 0x01FCE020, 0x2D40E400,
+0xDE59671C, 0x3273624D, 0xA2E28B01, 0x644D0009,
+0x6CE36D43, 0x65D23DEC, 0x61437C04, 0x621231CC,
+0x74086952, 0xAFED2929, 0x88052592, 0xE0208B18,
+0xE40001FC, 0x671C2D40, 0x624DDE4B, 0x8B013273,
+0x0009A2C7, 0x6943644D, 0x39EC61E3, 0x71046592,
+0x3C1C6C43, 0x6D5262C2, 0x2D2B7408, 0x25D2AFED,
+0x8B1B8831, 0xD942D241, 0x72046422, 0x72046622,
+0x72046722, 0x72E86C22, 0x1F2E1F4D, 0x72046422,
+0x72046E22, 0x652229E0, 0x2950D93A, 0xDE3A2FC6,
+0x55FE4E0B, 0xE2007F04, 0x2D20A29B, 0x8B1D8830,
+0xDE33D232, 0x72046522, 0x72046122, 0x72046722,
+0x72E86922, 0x72046422, 0x72046C22, 0x6E222EC0,
+0x1F9FD62C, 0x7FFC26E0, 0x09FEE040, 0x2F92DC2B,
+0x66134C0B, 0xE2007F04, 0x2D20A27B, 0x89018828,
+0x0009A109, 0xE143DE20, 0xE04062E1, 0x3617662D,
+0x0FE68F03, 0x660302FE, 0x36172201, 0xA0F38B01,
+0xE0400009, 0xE50104FE, 0x30568541, 0xA0EB8B01,
+0xE0400009, 0x09FEE701, 0xB2612D70, 0xE0406491,
+0xE1430CFE, 0xE06862C1, 0x3517652D, 0x0F568D68,
+0x3563E640, 0xE6008B24, 0x0F65E048, 0xA02EE11A,
+0x000072C0, 0x00117800, 0x00202A20, 0x00200F72,
+0x00201FDC, 0x002029B0, 0x00202A24, 0x00200FBC,
+0x002029AF, 0x002025D4, 0x00117804, 0x00117810,
+0x002029AC, 0x002029AD, 0x00200948, 0x00200994,
+0x41216153, 0x41214121, 0x41214121, 0x45214521,
+0x60534521, 0x6603C903, 0x0F65E048, 0xE0077118,
+0xE0442209, 0x641D0F25, 0x65F3E04C, 0x0F46B291,
+0x0EFDE048, 0x0DFDE044, 0x61DD67ED, 0x41084708,
+0x0F16E050, 0xDD946073, 0x4D0B06FE, 0x6E07E00F,
+0x607326E9, 0xE0400F66, 0x65F30CFE, 0x690D85C2,
+0x01FEE050, 0x60934D0B, 0x6073260B, 0xE04C0F66,
+0x04FEB256, 0x07FEE040, 0x6271E068, 0x0F56652D,
+0x3563E640, 0xED008954, 0x0FD5E064, 0xC9036023,
+0x40004008, 0x61036903, 0x0F96E054, 0xDE7EE058,
+0x0FF6ECFF, 0xE06C6CCC, 0x60C30FE6, 0x62534E0B,
+0x42214221, 0x42214221, 0x42006723, 0x6107327C,
+0x4200E05C, 0x0F164521, 0x4521E040, 0x60530CFE,
+0x4008C903, 0x7C0630FC, 0x6E031FC6, 0x1FD56D2D,
+0x1F04A01E, 0x0FD6E060, 0x05FEE058, 0x64D3B231,
+0x62E2E05C, 0xE05409FE, 0x2E222299, 0x64D361C4,
+0x01FE661C, 0x07FEE06C, 0x6063470B, 0xE058220B,
+0xB20505FE, 0xE0642E22, 0x7D0102FD, 0x0F257201,
+0x02FDE064, 0x3262E606, 0xE0408BDC, 0x626106FE,
+0x05FEE040, 0x85514200, 0x302C750C, 0x6103701B,
+0x64F3E600, 0xE704A004, 0x76016256, 0x74042422,
+0x3273626D, 0x65F38BF8, 0x641DB1E2, 0x06FEE040,
+0x6461B19E, 0x0009A175, 0xD74DD44C, 0x470BE201,
+0xA16E2D20, 0x88290009, 0xDE4A8B07, 0x2D20E200,
+0xB16D66E2, 0xA164646D, 0xE2810009, 0x3020622C,
+0xA0A78B01, 0xE0240009, 0x626C06FC, 0x666CE682,
+0x8B213260, 0xE42452FA, 0xD43F2240, 0x12615647,
+0x12625648, 0x12635649, 0x1264564A, 0x1265564B,
+0x1266564C, 0x1267564D, 0x1268564E, 0x1269564F,
+0x1427E200, 0x14291428, 0x142B142A, 0x142D142C,
+0x142F142E, 0x1F6CA135, 0x666CE683, 0x8B073260,
+0xE60052FA, 0xD22B2260, 0x6222D62C, 0x2622A129,
+0x666CE690, 0x8B183260, 0xE60052FA, 0xD2282260,
+0x6022E605, 0x2202CB20, 0x2262D226, 0x2262E600,
+0x460BD625, 0xD2250009, 0x0009420B, 0xE601D224,
+0xD2242262, 0xA10C4618, 0xE6B02262, 0x3260666C,
+0xD5188B22, 0xD216D420, 0x75046D52, 0x6E52420B,
+0x420BD21E, 0xD41E64D3, 0x450BD511, 0xD21B0009,
+0x64E3420B, 0xD60ED41B, 0x0009460B, 0xE600E504,
+0x3253626D, 0xA0EC8B01, 0x666D0009, 0x326C62D3,
+0x22E07601, 0x4E19AFF4, 0xD214D413, 0xD4146542,
+0x0009420B, 0x0009A0DD, 0x0020248C, 0x00202A44,
+0x00200F72, 0x00117804, 0x00202538, 0x00202994,
+0x001C3500, 0x001D4004, 0x00201056, 0x00200C1C,
+0x001E212C, 0x001C3D30, 0x00202A5C, 0x00200FB4,
+0x00202A70, 0x00202A78, 0x00117800, 0x00200FBC,
+0x00202A7C, 0xD6AED4AD, 0x6262E040, 0x76046542,
+0x2452352C, 0x62626563, 0x75045641, 0x1461362C,
+0x62526653, 0x76085542, 0x1452352C, 0x55436262,
+0x352C76EC, 0x65631453, 0x56446262, 0x362C7510,
+0x66531464, 0x55456252, 0x352C7610, 0x65621455,
+0xD69C5246, 0x1426325C, 0x55476262, 0x352C7604,
+0x62621457, 0x76045548, 0x1458352C, 0x62626563,
+0x75045649, 0x1469362C, 0x564A6252, 0x362C7504,
+0x6653146A, 0x554B6252, 0x352C7604, 0x6262145B,
+0x7604554C, 0x145C352C, 0x62626563, 0x7504564D,
+0x146D362C, 0x62526653, 0x7604554E, 0x145E352C,
+0x524F6562, 0x325CD684, 0x6262142F, 0x7694054E,
+0x0456352C, 0x6263E044, 0x054E6662, 0x356C7244,
+0xE0480456, 0x054E6622, 0xD67C356C, 0x62620456,
+0x054EE054, 0x352C4229, 0x76040456, 0xE0586262,
+0x4229064E, 0x52FA362C, 0xE6380466, 0xE0442260,
+0xE048064E, 0x66421261, 0x56411262, 0x56421263,
+0x56451264, 0x56431265, 0x56461266, 0x064E1267,
+0x1268E040, 0xE050064E, 0x56441269, 0x064E126A,
+0x126BE04C, 0xE054064E, 0x064E126C, 0x126DE058,
+0xE044064E, 0xE200126E, 0xE0480426, 0x14212422,
+0x14251422, 0x14261423, 0xE0400426, 0xE0500426,
+0x04261424, 0x0426E04C, 0x0426E054, 0x0426E058,
+0x7F701F6C, 0x6EF64F26, 0x6CF66DF6, 0x69F6000B,
+0x614D4F22, 0x3123E240, 0xE21F8917, 0x89083127,
+0xD550D44F, 0x450BE001, 0x67076642, 0xA00C2679,
+0xE23F2462, 0x89083127, 0xD64AD749, 0xE00171E0,
+0x5571460B, 0x25296207, 0x4F261751, 0x0009000B,
+0x614D4F22, 0x3123E240, 0xE21F8915, 0x89073127,
+0xD240D43F, 0x420B6642, 0x260BE001, 0x2462A00B,
+0x3127E23F, 0xD73A8907, 0x5571D63A, 0x460B71E0,
+0x250BE001, 0x4F261751, 0x0009000B, 0x4618E640,
+0xD5354628, 0x22686252, 0x000B89FC, 0xE6800009,
+0x46284618, 0x6252D530, 0x89FC2268, 0x0009000B,
+0xE200A001, 0x32427201, 0x000B8BFC, 0xE6800009,
+0x46284618, 0x6252D529, 0x8BFC2268, 0x0009000B,
+0x4F222FE6, 0x6E537FFC, 0x2F42BFF1, 0xD62461E2,
+0x1615E280, 0x421854E1, 0x55E21646, 0x16574228,
+0x6EF257E3, 0x2E2B1678, 0x7F0426E2, 0xAFCE4F26,
+0x2FC66EF6, 0x2FE62FD6, 0xDD194F22, 0xBFD66C53,
+0xBFBB6E43, 0xBFD22DE2, 0x51D50009, 0x54D62C12,
+0x55D71C41, 0x56D81C52, 0x4F261C63, 0x6DF66EF6,
+0x6CF6000B, 0xE6006163, 0x4109A004, 0x76016256,
+0x74042422, 0x8BF93612, 0x0009000B, 0x00202538,
+0x001C36A0, 0x001C3CA0, 0x001C36F4, 0x001C3B88,
+0x001C3704, 0x0020248C, 0x001C373C, 0x001C3700,
+0x001C370C, 0x0009A109, 0x2FD62FC6, 0x4F222FE6,
+0x6E636D73, 0x6C53B016, 0x64C357F4, 0xB02965E3,
+0xB03D66D3, 0xB06D0009, 0xB0710009, 0xB0750009,
+0xB08A0009, 0xB08D0009, 0x4F260009, 0x6DF66EF6,
+0x6CF6A0B4, 0x3412D190, 0xD6900529, 0x2650D790,
+0x2742000B, 0x2FD62FC6, 0x4F222FE6, 0x6E636D73,
+0x6C53BFF0, 0x64C357F4, 0x66D365E3, 0x6EF64F26,
+0x6CF66DF6, 0xD1872FE6, 0x66126E63, 0x92BC4418,
+0x44084528, 0x45002629, 0x265B4408, 0x264B4400,
+0x21624708, 0xD1804708, 0x217227EB, 0x6EF6000B,
+0x4F222FE6, 0xE101DE7D, 0xBFABE40A, 0x62E32E12,
+0xE100726C, 0x2212E401, 0x22122212, 0x22122212,
+0x22422212, 0xE503E730, 0x2212E40A, 0x22122212,
+0x22122212, 0x22122212, 0x22122212, 0x22122212,
+0x22722212, 0x22122252, 0x22122212, 0x22122212,
+0x22122212, 0xBF852212, 0xE600121A, 0x4F262E62,
+0x6EF6000B, 0xE101D266, 0x2212E441, 0x2242000B,
+0xD465D164, 0x2162E605, 0x2462000B, 0xD264D563,
+0x88016050, 0xD4638B07, 0x60409668, 0x8B098801,
+0xA0079665, 0xE6000009, 0x2262D45E, 0x88016040,
+0xE6048B00, 0xAF5DE40A, 0xD25B2262, 0xE40AE601,
+0x2262AF58, 0x2FC62FB6, 0x2FE62FD6, 0xDC574F22,
+0x60C2ED00, 0xCB01EB64, 0x60C22C02, 0xA008C901,
+0x3DB26E03, 0x60C28907, 0xC901E40A, 0x6E03BF42,
+0x2EE87D01, 0x3DB28BF5, 0xD44D8B03, 0x420BD24D,
+0xE40A0009, 0x6EF64F26, 0x6CF66DF6, 0x6BF6AF32,
+0x8F014411, 0x6043604B, 0x0009000B, 0x2FC62FB6,
+0x2FE62FD6, 0x7FFC4F22, 0xED00DC40, 0xEB6460C2,
+0x2C02CB02, 0x2F0260C2, 0xA009C902, 0x3DB36E03,
+0x60C28908, 0x2F02E40A, 0xBF13C902, 0x7D016E03,
+0x8BF42EE8, 0x8B0B3DB3, 0xD236D437, 0x4F267F04,
+0x6DF66EF6, 0x422B6CF6, 0x1FFF6BF6, 0x03C40340,
+0x4F267F04, 0x6DF66EF6, 0x000B6CF6, 0xD52F6BF6,
+0x60525651, 0x000B4628, 0x2FB6306C, 0x2FD62FC6,
+0x4F222FE6, 0x4F024F12, 0x6E43BFF1, 0xDC286B03,
+0xBFECDD28, 0x30B80009, 0x060A3C05, 0x46094609,
+0x3D654601, 0x4209020A, 0x42094209, 0x8BF032E2,
+0x4F164F06, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x4F222FE6, 0xE102DE1C, 0xE403E500, 0xBFD42E12,
+0xE6062E52, 0xE7004618, 0x2E62E403, 0x4F262E72,
+0x6EF6AFCB, 0x0009000B, 0x0025E720, 0x00202C3C,
+0x00202998, 0x001C5814, 0x001C59D0, 0x001C5830,
+0x001C6268, 0x001C59A4, 0x001C639C, 0x002029AD,
+0x001C5804, 0x002029AC, 0x001C581C, 0x001C5860,
+0x00202A90, 0x00200F72, 0x00202AA8, 0x001C1040,
+0xCCCCCCCD, 0x10624DD3, 0x001D4004, 0x2F962F86,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0xE4007FE0,
+0x4528E510, 0x67436C43, 0xE108A00F, 0x6043644D,
+0x0F564008, 0xEE0060C3, 0x815125C1, 0x81538152,
+0x157315E2, 0x751415E4, 0x624D7401, 0x8BED3213,
+0xDA7251F1, 0x1A1154F2, 0xD1712A12, 0x56F455F3,
+0x58F657F5, 0x21421141, 0x11521153, 0x11641165,
+0x11761177, 0x11881189, 0xD96A6DF2, 0xDB6A52F7,
+0x29D219D1, 0x2B221B21, 0xD868EB45, 0xE9B8EA50,
+0x4A084B08, 0xA020699C, 0x6EEDEE00, 0x61E36DE3,
+0x41084D08, 0x31EC3DEC, 0x41084D08, 0x60C33D8C,
+0xD75F4108, 0x81D12DC1, 0x410860A3, 0x60C381D2,
+0xE200317C, 0x81D33492, 0x1D131DD2, 0x8D01D456,
+0xD4521D24, 0x65D3B03C, 0x64ED7E01, 0x8BDC34B2,
+0xDB54D14E, 0xD24F6512, 0x1B514529, 0xD14C6412,
+0x2B72674D, 0xD6506722, 0x1B734729, 0x2FD26922,
+0x1B82689D, 0x26926912, 0x16A25A12, 0xDA465B14,
+0x5C1616B4, 0x5D1816C6, 0x6EA216D8, 0x7F2016EA,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0x664268F6, 0xC8036061, 0xE5008D04, 0xC9036061,
+0x8B038802, 0x65635262, 0x24125124, 0x6053000B,
+0x2FE62FD6, 0x7FEC4F22, 0x62536E53, 0x6D43E550,
+0x4508E400, 0xE101A001, 0x60435224, 0x81212211,
+0x60538123, 0x56E28122, 0x8BF53620, 0x16E4D22F,
+0xE61464F3, 0x65E3420B, 0xE4FC65E1, 0x2E512549,
+0x65F361F1, 0x2F112149, 0xD12854D1, 0xE614410B,
+0x607157D1, 0x2701CB01, 0x7F141DE1, 0x6EF64F26,
+0x6DF6000B, 0x2FE62FD6, 0x7FEC4F22, 0x66536E53,
+0x6D43E5FC, 0x20596061, 0x2601CB01, 0x326052E2,
+0x12E48B06, 0x31E051E2, 0x52D18B04, 0x1E22A002,
+0x5664AFF0, 0x64F3D215, 0x420BE614, 0x67E165E3,
+0x2719E1FC, 0x67F12E71, 0x271954D1, 0x65F3D10F,
+0x410BE614, 0x52D12F71, 0xCB016021, 0x1DE12201,
+0x4F267F14, 0x000B6EF6, 0x00006DF6, 0x0020259C,
+0x002025A4, 0x00202594, 0x002025CC, 0x001000A0,
+0x00101640, 0x001E2108, 0x001C3D00, 0x00200904,
+0x2FC62FB6, 0x2FE62FD6, 0x7FFC4F22, 0x6022D225,
+0x8D35C803, 0xDE242F01, 0xDB25DC24, 0xED01A016,
+0xC9036061, 0x89158801, 0xD122D420, 0x0009410B,
+0x65035603, 0xC8208561, 0xE0508903, 0x720102BE,
+0xD21D0B26, 0x64E3420B, 0x21D2D11C, 0x66C252C1,
+0x8BE53620, 0xDD1AEE01, 0x4E18A00E, 0xC9036061,
+0x890D8801, 0xD713D416, 0x470BDB16, 0xD4160009,
+0x65034B0B, 0x21E2D111, 0x66D252D1, 0x8BED3620,
+0xC80460F1, 0xD2118907, 0x4F267F04, 0x6DF66EF6,
+0x422B6CF6, 0x7F046BF6, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6000B, 0x001E2100, 0x002025A4, 0x0020259C,
+0x00202538, 0x00200D42, 0x00200DC4, 0x001C3D30,
+0x00202594, 0x00200D60, 0x002025CC, 0x00200100,
+0xE601D203, 0x1265D503, 0x000B2252, 0x00001266,
+0x001C1010, 0x0000C34F, 0xD62A7FFC, 0x2642644C,
+0xC8205066, 0x2F028DFC, 0x7F04000B, 0x2FD62FC6,
+0x4F222FE6, 0x6D436C53, 0xEE00A004, 0x7E0164D4,
+0x644CBFEA, 0x8BF93EC2, 0x6EF64F26, 0x000B6DF6,
+0xA0016CF6, 0x76016643, 0x22286260, 0x36488BFB,
+0x6563AFE4, 0x62532FE6, 0x67537507, 0xEE0AE108,
+0xC90F6043, 0x30E24409, 0x44096503, 0xE6378D01,
+0x365CE630, 0x27604110, 0x77FF8FF2, 0x8028E000,
+0x6EF6000B, 0xE000000B, 0xE000000B, 0x4F222FE6,
+0x62537FEC, 0x65F36E43, 0x6423BFDC, 0x64E3BFD1,
+0x64F3BFCF, 0xBFCCD404, 0x7F140009, 0x000B4F26,
+0x00006EF6, 0x001C0004, 0x00202AC4, 0xE110D5A1,
+0xE6406050, 0x2500C9FD, 0xE0FF75E9, 0x80516453,
+0x80538052, 0x80568055, 0x251075EF, 0xE1EF6250,
+0x2219E001, 0xE7202520, 0x24608052, 0x2570000B,
+0xE4FDD595, 0xE7026152, 0x25122149, 0x74016052,
+0x2502CB01, 0xD1916652, 0x25622649, 0x92C46012,
+0x2102CB08, 0xC9CF6012, 0x60122102, 0x2102CB03,
+0x000B1172, 0x4F221123, 0xD78AD589, 0xD48BD28A,
+0xE600E100, 0x27112511, 0xBFBF2210, 0xAFD72461,
+0x664C4F26, 0x4600D286, 0x6060362C, 0x000BCB10,
+0x654C2600, 0x4500D282, 0x6650352C, 0x2619E1EF,
+0x2560000B, 0xD27F664C, 0x362C4600, 0xCB106060,
+0x2600000B, 0xD27B654C, 0x352C4500, 0xE1EF6650,
+0x000B2619, 0x664C2560, 0x4600D275, 0x6060362C,
+0x000BCB08, 0x654C2600, 0x4500D271, 0x6650352C,
+0x2619E1F7, 0x2560000B, 0xD26E664C, 0x362C4600,
+0xCB086060, 0x2600000B, 0xD26A654C, 0x352C4500,
+0xE1F76650, 0x000B2619, 0x624C2560, 0x4200D664,
+0x6020326C, 0x4021C908, 0x40214021, 0x600C000B,
+0xD660624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0x644C600C, 0x74FFD15B, 0x6240341C,
+0x602C000B, 0x644CD159, 0x6240341C, 0x602C000B,
+0x4F222FE6, 0xE60A655C, 0x8D153567, 0xBFEA6E43,
+0x640C6453, 0x880160EC, 0xE00F8B02, 0x2409A002,
+0x44094409, 0xE60A624C, 0x89053263, 0x644CBFE2,
+0x6023620C, 0x8B00C880, 0x6023E200, 0x000B4F26,
+0x4F226EF6, 0x6062D646, 0x8B038801, 0x0009B241,
+0x0009A003, 0xE640D243, 0xD6432260, 0x4F26E200,
+0x2622000B, 0xD63E4F22, 0x88026062, 0xB28B8B01,
+0xD63D0009, 0x4F26E200, 0x2622000B, 0xD439D538,
+0xE701E100, 0x000B2512, 0x0FFF2470, 0xE604D235,
+0x2260000B, 0xD4354F22, 0x410BD135, 0xD5250009,
+0x6650E1FD, 0x2619D233, 0x2560E700, 0x000B4F26,
+0x4F222270, 0xD12ED430, 0x0009410B, 0xE7FBD51D,
+0x26796650, 0x000B4F26, 0x4F222560, 0xD128D42B,
+0x0009410B, 0xE7F7D517, 0x26796650, 0x000B4F26,
+0xD5142560, 0x62509425, 0x000B2249, 0xD5112520,
+0x6250E4BF, 0x000B2249, 0x4F222520, 0x8522D220,
+0x2008600D, 0x88018911, 0x8803893C, 0x8805893E,
+0x88068940, 0x88088946, 0x8809894C, 0x880A8952,
+0x880B8958, 0xA065895E, 0xB0670009, 0xA0620009,
+0xFF7F600C, 0x001E1028, 0x001E2148, 0x001E1108,
+0x002029DC, 0x002029DE, 0x002029E9, 0x002029C0,
+0x001E103F, 0x001E105F, 0x001E1030, 0x001E1090,
+0x002029E4, 0x001E100B, 0x002029E0, 0x00202AC8,
+0x00200F72, 0x002029E8, 0x00202AD4, 0x00202AE4,
+0x002029B4, 0x0009B04C, 0x600CA035, 0x0009B056,
+0x600CA031, 0x6260D67C, 0x8B2B2228, 0x0009B062,
+0x600CA029, 0x6260D678, 0x8B232228, 0x0009B06A,
+0x600CA021, 0x6260D674, 0x8B1B2228, 0x0009B0B4,
+0x600CA019, 0x6260D670, 0x8B132228, 0x0009B0BA,
+0x600CA011, 0x6260D66C, 0x8B0B2228, 0x0009B11A,
+0x600CA009, 0x6260D668, 0x8B032228, 0x0009B132,
+0x600CA001, 0x4F26E000, 0x0009000B, 0xD264D163,
+0xD5648412, 0x4000C90F, 0xD763012D, 0x611CE403,
+0xD662E20F, 0x27122540, 0xE0012520, 0x2602000B,
+0xE601D25A, 0x30668523, 0xE0008D06, 0xE000D258,
+0x8122D65A, 0x2602E001, 0x0009000B, 0x8523D253,
+0x2008600D, 0x88018905, 0xD6558B0A, 0xCB016060,
+0xD6522600, 0xE101D44E, 0x2612E001, 0x8142000B,
+0xE000000B, 0xE501D149, 0x45188513, 0x3453640D,
+0x8D056603, 0xD24BE000, 0xE001D548, 0x25022260,
+0x0009000B, 0xD1414F22, 0x650D8513, 0x44196453,
+0x672E6249, 0x602C227D, 0x89098801, 0x890C8802,
+0x89108803, 0x89268806, 0x89298807, 0x0009A038,
+0xD63ED53D, 0xA027E212, 0x625C2652, 0x8B2F2228,
+0xA01ED63B, 0x605C6262, 0x89052008, 0x89088810,
+0x890B8820, 0x0009A024, 0xD634D436, 0xA013E204,
+0xD7352642, 0xE20CD631, 0x2672A00E, 0xD62FD533,
+0xA009E218, 0xD4322652, 0xE20AD62C, 0x2642A004,
+0xD62AD230, 0xE22E2622, 0xD42F8515, 0x3277670D,
+0x8F012421, 0x24516503, 0x0009B0DB, 0xE001A001,
+0x4F26E000, 0x0009000B, 0xE101D61A, 0x2610D427,
+0xD7196541, 0x655DD119, 0xE001E20F, 0x26202752,
+0x2102000B, 0x4F222FE6, 0x8523D210, 0x2448640C,
+0xD61E8B08, 0xE200D512, 0x84512621, 0x20499412,
+0x8051A050, 0x60E0DE0E, 0x8D35C840, 0x3427E201,
+0xD116894C, 0x420BD216, 0xD5162141, 0xCB046052,
+0x2502A035, 0x0000FF7F, 0x002029E9, 0x002029B4,
+0x002029C0, 0x001E1100, 0x001E100C, 0x002029E0,
+0x001E1000, 0x001E1001, 0x00202C40, 0x002029C8,
+0x002029D0, 0x00202CAE, 0x00202CB2, 0x00202CBE,
+0x00202CD6, 0x00202CE0, 0x002029CC, 0x002029DA,
+0x00201DB6, 0x001E1108, 0x89173427, 0xD794D293,
+0x2241470B, 0xE5FBD693, 0x21596162, 0x84E12612,
+0xB0FFCB80, 0x60E080E1, 0xCB04D68F, 0x60602E00,
+0x2600C93F, 0xE001D68D, 0x2602A001, 0x4F26E000,
+0x6EF6000B, 0x6060D68A, 0x8919C880, 0x6021D283,
+0x8B158801, 0xE501D287, 0x30568524, 0xD1868910,
+0xD486E203, 0x65412120, 0x655DE00B, 0xD5840656,
+0xE702E40F, 0x25712140, 0xE001D77C, 0x2702000B,
+0xE000000B, 0x4F222FE6, 0x84E1DE7E, 0x8934C880,
+0x8554D578, 0x8F302008, 0xD77B6103, 0x66728553,
+0x650C6403, 0x620C8566, 0x8B263520, 0xD773D677,
+0x644C651C, 0x27412651, 0xC84060E0, 0xD2748907,
+0x0009420B, 0x6062D667, 0xA008CB04, 0xD1642602,
+0x0009410B, 0xE5FBD663, 0x24596462, 0xB0A12642,
+0xD5620009, 0x2522E201, 0xD75F60E0, 0x2E00CB04,
+0xC93F6070, 0xA0012700, 0xE0006023, 0x000B4F26,
+0x2FA66EF6, 0x2FC62FB6, 0x2FE62FD6, 0xE240DA5C,
+0xDC5966A1, 0x3123616D, 0x62638900, 0x6ED36D2C,
+0x4E2136D8, 0x4E212A61, 0xDB5BD45A, 0xE700A00F,
+0x770166B2, 0x71026163, 0x65612B12, 0x71026613,
+0x62612B12, 0x622D655D, 0x325C4228, 0x627C2422,
+0x8BED32E3, 0xC90360D3, 0x8B108803, 0xED076EB2,
+0x710261E3, 0x67132B12, 0x62E17102, 0x65712B12,
+0x655D622D, 0x352C4528, 0xA00C2CD0, 0x88022452,
+0xA0038B01, 0x8801E203, 0xE2018B05, 0x66B22C20,
+0x677D6761, 0xEB0F2472, 0x6DA12CB0, 0x8B052DD8,
+0xD432D23E, 0xE101EE00, 0x241222E2, 0x6DF66EF6,
+0x6BF66CF6, 0x6AF6000B, 0x2FE62FD6, 0xE240DD30,
+0x616D66D1, 0x89003123, 0x672C6263, 0xDE323678,
+0x2D617703, 0xD62F4721, 0x472164E2, 0xE100A00E,
+0x71016562, 0x24506253, 0x42197401, 0x74012420,
+0x24504529, 0x45197401, 0x74012450, 0x3273621C,
+0x42008BEE, 0x64D166E2, 0x362C4200, 0x8F062448,
+0xDD222E62, 0xE500DE15, 0x2D52E701, 0x6EF62E72,
+0x6DF6000B, 0x2FE62FD6, 0xEE014F22, 0xED0AA005,
+0x64E3BC97, 0x64E3BC9D, 0x62EC7E01, 0x8BF732D7,
+0xEE01A005, 0x64E3BC9E, 0x64E3BCA4, 0x62EC7E01,
+0x8BF732D7, 0x6EF64F26, 0x6DF6000B, 0x002029DA,
+0x00201EC2, 0x001E1108, 0x001E1015, 0x002029E0,
+0x001E1001, 0x002029B4, 0x001E1100, 0x002029DE,
+0x002029CC, 0x001E1000, 0x002029D0, 0x002029DC,
+0x00201DB6, 0x001E100C, 0x002029C8, 0x002029E4,
+0x2FE62FD6, 0x7FFC4F22, 0x6060D64C, 0x89488801,
+0xE101D44B, 0xD74B8548, 0x650D2610, 0x45196070,
+0x6659DD49, 0x61D3626E, 0xC840262D, 0x74027102,
+0x8D1AD746, 0xD246666C, 0xE501DE46, 0xA0042E22,
+0x6245EE04, 0x21217501, 0x625C7102, 0x8BF832E3,
+0x81D46063, 0xD540E601, 0x626CE417, 0x891E3243,
+0x76016255, 0xAFF82721, 0xD23C7702, 0xE501DE39,
+0xA0042E22, 0x6245EE04, 0x21217501, 0x625C7102,
+0x8BF832E3, 0x81D46063, 0xD535E601, 0xE417A004,
+0x76016255, 0x77022721, 0x3243626C, 0x924B8BF8,
+0xD4302D21, 0x6142D730, 0x65F22F12, 0x60536DF2,
+0x2700C980, 0xC9606053, 0x80716103, 0x6EF26053,
+0xC90365F2, 0x45294D19, 0x60DC8072, 0x81724519,
+0x605C4E29, 0x401862EC, 0x8173302C, 0x21186D42,
+0x6EF22FD2, 0x66F262F2, 0x46294219, 0x66F2656C,
+0x64EC602C, 0x46294018, 0x4619304C, 0x606C8174,
+0x305C4018, 0x81758F07, 0x0009BCBF, 0x2228620C,
+0xA00A8908, 0x60130009, 0x8B038840, 0x0009B00A,
+0x0009A003, 0xE202D611, 0x7F042622, 0x6EF64F26,
+0x6DF6000B, 0x0009000B, 0x0000060A, 0x002029E8,
+0x00202C40, 0x001E1000, 0x00202CD6, 0x00202CE2,
+0x00202C52, 0x002029D0, 0x00202C82, 0x00202C80,
+0x00202C54, 0x001E100C, 0x002029B4, 0x002029E0,
+0x4F222FE6, 0xDE907FFC, 0x200884E9, 0x2F008D06,
+0xD68FD48E, 0x0009460B, 0x64F0B146, 0x6620D28D,
+0x89022668, 0xC9BF60E0, 0x7F042E00, 0x000B4F26,
+0x000B6EF6, 0x2FE60009, 0xDE874F22, 0x60E0D687,
+0xCBC0D487, 0x62602E00, 0xC803602C, 0x40218904,
+0x70014021, 0x6603A002, 0x66034009, 0xD681616D,
+0xE500A004, 0x75016262, 0x74042422, 0x3213625D,
+0xD27D8BF8, 0x0009420B, 0xC9BF84E2, 0x4F2680E2,
+0x6EF6000B, 0x2FD62FC6, 0x4F222FE6, 0xDC727FFC,
+0x84C2D276, 0xCB40DD76, 0x80C2420B, 0x8D042008,
+0x62E06E03, 0xA006642C, 0xD66A7404, 0x6160D471,
+0x470BD771, 0x644D651C, 0x45216543, 0xA0044521,
+0x62E6E600, 0x2F227601, 0x626D2D22, 0x8BF83253,
+0xC9036043, 0x89122008, 0x89058803, 0x89068802,
+0x89078801, 0x0009A008, 0xA005E007, 0xE00380D8,
+0x80D8A002, 0x80D8E001, 0x2F2262E2, 0xE00F2D22,
+0x80D8D65E, 0xCB086060, 0x60C02600, 0x2C00C93F,
+0x4F267F04, 0x6DF66EF6, 0x6CF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xD2564F22, 0x6E436D73, 0x420B6B53,
+0x20086C63, 0x64038F08, 0xD245D452, 0x6EF64F26,
+0x6CF66DF6, 0x6BF6422B, 0x24E060B3, 0x60C38041,
+0xA0078141, 0x655DE500, 0x00DC6053, 0x324C6253,
+0x80247501, 0x6EEC625D, 0x8BF432E3, 0x6060D636,
+0x2600C9BF, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x7FC44F22, 0x720262F3, 0x22512F41, 0x45297202,
+0x60632251, 0xE5C4E682, 0x67F38121, 0x655C666C,
+0xE408BFBC, 0x4F267F3C, 0x0009000B, 0xD237D136,
+0xE4056012, 0xE500CB20, 0x22422102, 0x2252000B,
+0xD534D133, 0xE400D734, 0x2142E20F, 0x17411154,
+0xD5322722, 0x9635D732, 0x15412572, 0x96321562,
+0xE6011565, 0xD52F1165, 0x666CE6F8, 0x25422542,
+0x25422542, 0x25422542, 0x25622542, 0x7601E727,
+0x67632572, 0x25627797, 0xE7042572, 0x2572E248,
+0xE2192522, 0xE2702522, 0x25422542, 0x25422542,
+0x25222542, 0x2522E20C, 0x25422542, 0x25422542,
+0x25422542, 0x25422542, 0x000B154A, 0xE2081145,
+0x0009422B, 0x51630601, 0x001E1017, 0x00202AF0,
+0x00200F72, 0x002029B0, 0x001E1015, 0x001E10BF,
+0x00117800, 0x001E10FC, 0x00200100, 0x0020201A,
+0x001E10F8, 0x00202AF4, 0x00200FBC, 0x001E10AE,
+0x00201FDC, 0x00202B10, 0x001C3500, 0x001D4004,
+0x001C581C, 0xA000A000, 0x001D0100, 0x001D4000,
+0x00040021, 0x001C589C, 0x2FE62FD6, 0x7FFC4F22,
+0xC8206043, 0x6E438D02, 0x0009BEBB, 0xC81060E3,
+0xBEB88901, 0x60E30009, 0x8901C840, 0x0009BEDA,
+0xC80160E3, 0xDD378936, 0xC80260D0, 0x2F008D03,
+0x460BD635, 0x60F00009, 0x8902C804, 0x460BD633,
+0x62F00009, 0xC8806023, 0x60D08902, 0x2D00C97F,
+0xC8016023, 0xD62E8904, 0x0009460B, 0x0009A005,
+0x8902C808, 0x460BD62B, 0x60F00009, 0x8902C810,
+0x420BD229, 0xD5290009, 0x88026052, 0xD2288B03,
+0xA005E604, 0x88012260, 0xD2258B02, 0x2260E601,
+0x2522E200, 0xC88060E3, 0xD622892E, 0x60E36E60,
+0x8902C880, 0x420BD220, 0x60E30009, 0x8902C840,
+0x420BD21E, 0x60E30009, 0x8902C802, 0x420BD21C,
+0x60E30009, 0x890EC804, 0x410BD11A, 0xBF150009,
+0xBF1D0009, 0xD5180009, 0x6050D418, 0xC908D718,
+0xBF542500, 0x60E32472, 0x8905C808, 0x7F04D215,
+0x6EF64F26, 0x6DF6422B, 0x4F267F04, 0x000B6EF6,
+0x00006DF6, 0x001E1021, 0x00201182, 0x002011A4,
+0x002017B0, 0x002011BC, 0x002011CC, 0x002029E0,
+0x001E100B, 0x001E1028, 0x00201222, 0x0020122E,
+0x002011D4, 0x002011F2, 0x001E1000, 0x0010F100,
+0x12345678, 0x0020120A, 0xD6A8644C, 0x346C74FF,
+0x2450000B, 0x644CD6A6, 0x000B346C, 0xD6A52450,
+0x346C644C, 0x2450000B, 0x616D625C, 0x41194208,
+0x60194208, 0x644C4200, 0x324C670E, 0x207DD19E,
+0xC90F4200, 0x000B321C, 0x67632200, 0x4208625C,
+0x42004208, 0x324C644C, 0x4200D198, 0x000B321C,
+0x2FE62270, 0x614C4F12, 0x4100D493, 0x6710314C,
+0x2729E29F, 0x65736E53, 0x4719676D, 0x672E6279,
+0x4221227D, 0x42214221, 0x7601662C, 0xE4014608,
+0x34E84608, 0x644C4600, 0x0E1A0467, 0x215025EB,
+0x000B4F16, 0x4F226EF6, 0xD2857FE8, 0x88016021,
+0xD2848B7B, 0x26686621, 0xD2838B77, 0x26686621,
+0xE50F8B73, 0xE401BFA0, 0xBFA3E501, 0xE586E400,
+0xE400655C, 0x2F50BFA3, 0xBFA0E401, 0xE602E506,
+0x60634618, 0x81F2E401, 0x6543BF9E, 0xE40185F2,
+0xBFAA6543, 0x85F26603, 0x6543E401, 0x6603BFB1,
+0xE40265F0, 0x6053756C, 0x80F8BF7E, 0xBF81E402,
+0x84F8E512, 0x7090E402, 0x6503BF81, 0x4618E602,
+0x81F66063, 0xBF7FE402, 0x85F6E500, 0x6603E402,
+0xE500BF8B, 0xE40285F6, 0xBF926603, 0xE5FEE500,
+0xE010655C, 0xBF5FE403, 0xE5130F54, 0xE40EBF62,
+0x05FCE010, 0xBF62E40E, 0xE5007585, 0xBF63E403,
+0xE500E640, 0xBF70E403, 0xE500E640, 0xBF78E403,
+0xE5FFE640, 0xE014655C, 0xBF45E404, 0xE40F0F54,
+0xE504BF48, 0x05FCE014, 0xBF48E40F, 0xE5017584,
+0xBF49E640, 0xE501E404, 0xBF56E640, 0xE501E404,
+0xE404E640, 0xAF5C7F18, 0x7F184F26, 0x000B4F26,
+0x4F220009, 0xD2427FF0, 0x88016021, 0xD2418B71,
+0x26686621, 0xD2408B6D, 0x26686621, 0xE50F8B69,
+0xE401BF1A, 0xBF1DE501, 0xE586E400, 0xE400655C,
+0x2F50BF1D, 0xBF1AE401, 0xE401E506, 0xBF1B6543,
+0xE401E640, 0xBF286543, 0xE401E640, 0xBF306543,
+0x65F0E640, 0x756CE402, 0xBEFD6053, 0xE40280F4,
+0xE512BF00, 0xE40284F4, 0xBF007090, 0xE6406503,
+0xBF01E402, 0xE640E500, 0xBF0EE402, 0xE640E500,
+0xBF16E402, 0xE5FEE500, 0x6053655C, 0xBEE3E403,
+0xE51380F8, 0xE40EBEE6, 0xE40E84F8, 0xBEE67085,
+0xE5006503, 0xBEE7E640, 0xE500E403, 0xBEF4E640,
+0xE500E403, 0xBEFCE640, 0xE5FFE403, 0x6053655C,
+0xBEC9E404, 0xE40F80FC, 0xE504BECC, 0xE40F84FC,
+0xBECC7083, 0xE5016503, 0xBECDE640, 0xE501E404,
+0xBEDAE640, 0xE501E404, 0xE404E640, 0xAEE07F10,
+0x7F104F26, 0x000B4F26, 0x00000009, 0x001E1030,
+0x001E1080, 0x001E1090, 0x001E103F, 0x001E103E,
+0x002029DA, 0x002029DC, 0x002029DE, 0xD21DD11C,
+0x66206010, 0x676C7001, 0x3700C90F, 0xE5008D13,
+0x67106210, 0x7701622C, 0x64232170, 0xD6166010,
+0x44084408, 0x3428C90F, 0x62602100, 0x7201D513,
+0x44082620, 0x000B354C, 0xD10F6053, 0x25586510,
+0xE6008D13, 0xD60DD40B, 0x655C6540, 0x47086753,
+0x37584708, 0x47086540, 0x24507501, 0x367C6040,
+0x2400C90F, 0x72FF6210, 0x000B2120, 0x00006063,
+0x002029AF, 0x002029AE, 0x002029B0, 0x002025D4,
+0x7FFC4F22, 0xE680D19D, 0x666C6212, 0xD29C2F22,
+0x67F36563, 0x420B7542, 0x7F04E404, 0x000B4F26,
+0xE6800009, 0xD296666C, 0xE7006563, 0x422B7540,
+0xE6806473, 0xD292666C, 0xE7006563, 0x422B7543,
+0x2FB66473, 0x2FD62FC6, 0x4F222FE6, 0x4D18ED01,
+0xDB8DDC8C, 0x65C252C1, 0x89203520, 0xC9036051,
+0x891C8801, 0xD189DE87, 0x64E3410B, 0x85036503,
+0x670D66B2, 0x89073762, 0xD286D485, 0x0009420B,
+0xE701D185, 0x2172AFE6, 0xDE8464E3, 0x00094E0B,
+0xD484D683, 0x410BD184, 0xAFDB26D2, 0x4F260009,
+0x6DF66EF6, 0x000B6CF6, 0x4F226BF6, 0x85467FF4,
+0x2F01E681, 0x666C8547, 0x854881F1, 0x81F2D270,
+0x67F38542, 0x854381F3, 0x81F4E40C, 0x65636053,
+0x420B81F5, 0x7F0C7540, 0x000B4F26, 0x2F860009,
+0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6, 0x7FF44F22,
+0xDC6EE200, 0x2F21A136, 0xDD6D6A13, 0xE0014A08,
+0x4D0BD96C, 0x3A9C4A00, 0x1F917930, 0x66C21F02,
+0x362052C1, 0xA1218B01, 0x60610009, 0x8801C903,
+0xA11B8B01, 0x85610009, 0x8977C801, 0x85D25D63,
+0xC9036603, 0x85D36403, 0x6053650D, 0x40214021,
+0x4500C93F, 0x322A6103, 0x6053252D, 0xC901E210,
+0xD9553123, 0x6E038D21, 0x4408D757, 0x44086570,
+0x44006213, 0x25584200, 0x342C8F0E, 0x6043D253,
+0x60E3072D, 0x4B196B7D, 0x658E68B9, 0x285D8801,
+0x6B7C8F0B, 0x6B13A009, 0x6043D24D, 0x61ED0E2D,
+0x68194119, 0x287D678E, 0xD14A6BEC, 0x22286212,
+0xEBFF8901, 0xEEFF6BBC, 0x6EEC65BD, 0x8B0F35E0,
+0x4D0BDD36, 0x540364C3, 0xBF76E502, 0xD4426D03,
+0x410BD136, 0xD74165D3, 0xD441EE01, 0x27E2A01D,
+0x26E9EEFC, 0x81D26063, 0x914E85D3, 0x81D32019,
+0x450885D2, 0x81D2208B, 0xE20885D3, 0x81D3205B,
+0x421885D4, 0x81D4202B, 0x854164C2, 0x814120E9,
+0xD43465C2, 0xCB016051, 0x490B2501, 0x60C20009,
+0x52F256F1, 0x2A02CB01, 0x2622AF79, 0x420BD21B,
+0x5E0364C3, 0x85E16D03, 0x6053650D, 0x897BC820,
+0x6210D129, 0x8B112228, 0xD72785EF, 0x4221620D,
+0x42214221, 0xE501D625, 0x27504221, 0xD725D924,
+0x2621D425, 0x2960E600, 0x24612762, 0x852162C2,
+0x8B43C802, 0xD912D71E, 0xE0016270, 0x612C490B,
+0x6692D91C, 0xA03E260B, 0x7E032962, 0x001C3D9C,
+0x00201A3C, 0x002025CC, 0x00202994, 0x00200D42,
+0x00202594, 0x00200DC4, 0x001E2130, 0x00200D60,
+0x001C3D30, 0x00202C28, 0x00200F72, 0x002025A4,
+0x0020248C, 0x001C3D00, 0x00202C3C, 0x00202B28,
+0x00202BA8, 0x002029A8, 0x0020259C, 0x001E212C,
+0x00202C2C, 0x00202C30, 0x00202D10, 0x002029EE,
+0x002029EC, 0x002029F0, 0x002029F4, 0xE04CD139,
+0x7201021E, 0xD9380126, 0x6290D438, 0x72016541,
+0x29207501, 0x85E12451, 0x4618E640, 0x891D2068,
+0xD934D733, 0x665D6171, 0x6592D733, 0x641D470B,
+0xE200DE32, 0x2E20A012, 0xE90885E4, 0x49186203,
+0x32902299, 0xE5018B04, 0x64E3BEB7, 0x0009A006,
+0x2598D92B, 0xE5008902, 0x64E3BEAF, 0xD22AD429,
+0x65D3420B, 0xEE01D729, 0x27E2AED9, 0x7C0862F1,
+0x2F217201, 0xEE0462F1, 0x31E7612D, 0xAEC38901,
+0x7F0C0009, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0x2FE668F6, 0xD21D4F22, 0x60E36E22,
+0x8D02C840, 0xBE3322E2, 0xE2400009, 0x2E284218,
+0xBE3E8901, 0x60E30009, 0x8905C810, 0xD216D415,
+0x0009420B, 0x0009BE3D, 0xC80560E3, 0xBE8E8901,
+0x60E30009, 0x8902C802, 0xAE3A4F26, 0x4F266EF6,
+0x6EF6000B, 0x00202538, 0x002029EC, 0x002029F4,
+0x002029EE, 0x002029F0, 0x00201AA0, 0x00202D10,
+0x00008000, 0x0020259C, 0x00200D60, 0x001E212C,
+0x001C3510, 0x00202C34, 0x00200F72, 0x080A0C0E,
+0x00020406, 0x1A1C1E20, 0x12141618, 0x2E303234,
+0x26282A2C, 0x3A3C3E40, 0x6C625648, 0x41112F26,
+0xE2208F18, 0x890B3123, 0x321CD204, 0xD1026220,
+0x412B312C, 0x00090009, 0x002024B6, 0x0020246C,
+0x000BE000, 0x400062F6, 0x40004000, 0x40004000,
+0x40004000, 0x62F6000B, 0x40004000, 0x40004000,
+0x40004000, 0x40184000, 0x62F6000B, 0x40004000,
+0x40004000, 0x40004000, 0x40284000, 0x62F6000B,
+0x40004000, 0x40184000, 0x000B4028, 0xC90F62F6,
+0x40054005, 0x40054005, 0x62F6000B, 0x4005C907,
+0x40054005, 0x62F6000B, 0x4005C903, 0x000B4005,
+0xC90162F6, 0x000B4005, 0x000062F6, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x544F0D0A, 0x46205355,
+0x00003A57, 0x2079614D, 0x32203033, 0x20373030,
+0x333A3231, 0x38313A37, 0x00000000, 0x00000D0A,
+0x00000043, 0x42707372, 0x3D206675, 0x554E203D,
+0x202C4C4C, 0x6E49677A, 0x4E497274, 0x6D754E51,
+0x0000003D, 0x61766E49, 0x2064696C, 0x72657375,
+0x20726F20, 0x2079656B, 0x00214449, 0x52504545,
+0x57204D4F, 0x65746972, 0x6461202C, 0x003D7264,
+0x6C617620, 0x0000003D, 0x00000A0D, 0x6E6B6E55,
+0x206E776F, 0x6D6D6F63, 0x3D646E61, 0x00000000,
+0x61437748, 0x7262696C, 0x6F697461, 0x6620206E,
+0x0A6C6961, 0x0000000D, 0x73696F4E, 0x61432065,
+0x7262696C, 0x6F697461, 0x6166206E, 0x21216C69,
+0x00000D0A, 0x00000D0A, 0x62735576, 0x7473725F,
+0x00000A0D, 0x62735576, 0x7375735F, 0x646E6570,
+0x00000A0D, 0x62735576, 0x7365725F, 0x000A0D6D,
+0x00000042, 0x72746E49, 0x6D652051, 0x2C797470,
+0x49677A20, 0x4972746E, 0x754E514E, 0x00003D6D,
+0x20746F4E, 0x756F6E65, 0x49206867, 0x4220514E,
+0x0A0D6675, 0x00000000, 0x000000FF, 0x00020001,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x010E010D, 0x00020003,
+0x01090108, 0x0002010A, 0x00030002, 0x02020201,
+0x02040203, 0x02060205, 0x02080207, 0x020A0209,
+0x020C020B, 0x020E020D, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x010E010D, 0x00FF010F,
+0x01090108, 0x010B010A, 0x00030002, 0x02020201,
+0x02040203, 0x02060205, 0x02080207, 0x020A0209,
+0x020C020B, 0x020E020D, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00205220, 0x00000046,
+0x00000059, 0x49544120, 0x0000204D, 0x00000000,
+0x02000112, 0x40FFFFFF, 0x91700CF3, 0x20104890,
+0x02090100, 0x0101002E, 0x09FA8000, 0x04000004,
+0x000000FF, 0x02010507, 0x07000200, 0x00028205,
+0x05070002, 0x00400383, 0x04050701, 0x01004003,
+0x002E0209, 0x80000101, 0x000409FA, 0x00FF0400,
+0x05070000, 0x00400201, 0x82050700, 0x00004002,
+0x03830507, 0x07010040, 0x40030405, 0x03040100,
+0x030C0409, 0x0079005A, 0x00410044, 0x03180053,
+0x00530055, 0x00320042, 0x0030002E, 0x00570020,
+0x0041004C, 0x0000004E, 0x00000000, 0x00000000,
+0x00000709, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, };
+
+const u32_t zcFwImageSize=11540;
diff --git a/drivers/staging/otus/hal/hpfwu_2k.c b/drivers/staging/otus/hal/hpfwu_2k.c
new file mode 100644
index 000000000000..94e2caca5369
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwu_2k.c
@@ -0,0 +1,1016 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE947FFC, 0xE114D594,
+0x1E13D494, 0x67521E4C, 0xD494D693, 0x37402769,
+0x62528F06, 0x7201D692, 0x60602522, 0x2600C93F,
+0xD7906152, 0x2512611D, 0x264B6652, 0x2562470B,
+0x0009B017, 0xE60095AC, 0xC84060E2, 0x2F028F03,
+0x8FF93652, 0xD4887601, 0x4E0BDE88, 0xD4880009,
+0x00094E0B, 0x4E0BD487, 0x7F040009, 0xA0524F26,
+0x4F226EF6, 0x410BD184, 0xD4840009, 0x0009440B,
+0x450BD583, 0xD7830009, 0xD283E1FF, 0x2712611D,
+0xD4825029, 0xE1FFCB01, 0x1209E501, 0x12112212,
+0xE7202452, 0x4718D57E, 0x2572D27E, 0xD17EE700,
+0xD67FD47E, 0xE2012270, 0x24702172, 0xD67D2620,
+0x2641E4FF, 0xD57CE600, 0x666DE104, 0x76016063,
+0x4000626D, 0x8FF83212, 0xD5780545, 0x2520E201,
+0xD278D777, 0xE480E100, 0x22122710, 0x6613D576,
+0x666D644C, 0x76046763, 0x375C626D, 0x8FF83243,
+0xD5722712, 0xD273D772, 0xE400E101, 0x27102511,
+0x000B4F26, 0x7FCC2242, 0xD170D56F, 0xD271DB70,
+0x1F51D471, 0xD6717508, 0x1F12D771, 0x1F55710C,
+0x1FB975FC, 0x72041F2A, 0x1F13EB10, 0x1F561F44,
+0x1F781F67, 0xD86B1F2B, 0xDD6CD96B, 0xDC6CEA00,
+0xD26DDE6C, 0x89003A22, 0xD15D7A01, 0x88016010,
+0x56F88B03, 0x4218E201, 0xD1682622, 0x0009410B,
+0x440BD467, 0xD5670009, 0x0009450B, 0x6010D150,
+0x8B108801, 0xE650D14F, 0x46186212, 0x8B083266,
+0x56F9D14B, 0x2120E200, 0xCB016062, 0x2602A003,
+0x72012710, 0x60822122, 0x89098801, 0xE2C8D15A,
+0x622C6612, 0x89033626, 0x6010D158, 0x8BC88801,
+0x51F66792, 0x217252F5, 0xD6555191, 0x55FA2212,
+0x52FB6462, 0x55612542, 0x2252E400, 0x61436643,
+0x05DE6013, 0x36CC4608, 0x07DE2652, 0xC9036071,
+0x8B028801, 0x720162E2, 0x74012E22, 0x36B3664C,
+0x71048FEE, 0x66C2D147, 0x45286512, 0x265B4518,
+0x60822C62, 0x89018801, 0x0009A168, 0x6272D742,
+0x8B132228, 0xD42BD741, 0x6772D541, 0x51536242,
+0x312C327C, 0x24222228, 0x15138D05, 0x6262D63D,
+0xB1627201, 0xD6232622, 0x2622E200, 0x52916692,
+0x8B013620, 0x0009A144, 0x6061A06E, 0x001C001C,
+0x001D4020, 0x0000B38E, 0xFFFF0000, 0x12340000,
+0x001E1015, 0x00201274, 0x002039EC, 0x002018A2,
+0x002039F8, 0x00203A10, 0x00201860, 0x00201964,
+0x00201288, 0x001C3510, 0x001C3624, 0x001E212C,
+0x002038EC, 0x00203484, 0x002038F4, 0x00203900,
+0x0020390C, 0x00203968, 0x0020396C, 0x00203914,
+0x00203915, 0x00203918, 0x00117700, 0x00203984,
+0x00203982, 0x002034E8, 0x00117710, 0x001C3D30,
+0x001C36F8, 0x00117734, 0x001C3684, 0x001C3D00,
+0x001C1000, 0x001C1028, 0x002034FC, 0x0020391C,
+0x00117600, 0x00117740, 0x7FFFFFFF, 0x00201730,
+0x00203322, 0x0020232C, 0x00203D9C, 0x0020396A,
+0x002034F4, 0x0020395C, 0x001C3D2C, 0x001C36B0,
+0x0020348C, 0x0011775C, 0x8801C90F, 0xA0CF8901,
+0xD1960009, 0x36206212, 0xD4958904, 0x2421E200,
+0x2162A0CC, 0x6211D193, 0x89012228, 0x0009A0C3,
+0xE202D78F, 0x75016571, 0x3123615D, 0x27518D02,
+0x0009A0BC, 0xD28C57F2, 0x62226072, 0x40094019,
+0xC90F4009, 0x8F19880A, 0x52F31F2C, 0x40196022,
+0x40094009, 0x8808C90F, 0xA0A78901, 0x60630009,
+0xCB0154F7, 0xD27E55F2, 0xE7012402, 0xD47FE100,
+0x22112572, 0x72016242, 0x2422A098, 0x8B3F8805,
+0x602252F3, 0x40094019, 0xC90F4009, 0x8B168802,
+0xE4FFD577, 0x644D6752, 0x8B102748, 0x6272D775,
+0x8B0C3260, 0x51F255F7, 0xD26DE701, 0x21722562,
+0xD571E100, 0x64522211, 0xA0777401, 0x52F32542,
+0x40196022, 0x40094009, 0x8805C90F, 0x31B38B6E,
+0xD26A8B6C, 0x672254F4, 0x7701D569, 0x61422272,
+0x1F1CE640, 0x46182159, 0x8B033160, 0x6262D665,
+0x26227201, 0xE200D65A, 0x2621B067, 0x0009A056,
+0x3123E220, 0x88038B52, 0x52F38B1E, 0x40196022,
+0x40094009, 0x8803C90F, 0xD25B8B16, 0x672254F4,
+0x7701D557, 0x61422272, 0x1F1CE640, 0x46182159,
+0x8B033160, 0x6262D655, 0x26227201, 0xE200D648,
+0x2621B043, 0x0009A010, 0xD452D551, 0xD2446752,
+0xE1007701, 0x25723A46, 0x22118F06, 0xEA00D64E,
+0x72016262, 0x2622B031, 0x2FB2D54C, 0x95736652,
+0xD44A5BF1, 0x36205241, 0x60618910, 0x8B01C803,
+0x2B22E201, 0x8FF54510, 0x57F15664, 0x6272E1F0,
+0x41284118, 0x2722221B, 0x6BF2A008, 0x6BF2A006,
+0xE200D62F, 0xD12F2621, 0x2121E200, 0xD13CE201,
+0x66122822, 0x8B012668, 0x0009AE2B, 0x450BD539,
+0xD1390009, 0xAE24E600, 0x2F862160, 0x2FA62F96,
+0x2FC62FB6, 0x2FE62FD6, 0x7FF44F22, 0xDE34D133,
+0x54116212, 0x1F4167E2, 0x2F22D432, 0xD5321F72,
+0xD2326743, 0x58417794, 0x69425A42, 0x5B166C72,
+0x60526D22, 0xCB20E600, 0xE5402502, 0x626D7601,
+0x8BFB3253, 0x55F162F2, 0x11512122, 0xD62855F2,
+0x14812E52, 0x249214A2, 0x27C2D426, 0x26D211B6,
+0xDA256742, 0xE801D925, 0x490B2A72, 0xE2011A8C,
+0x1A2C4218, 0x4F267F0C, 0x6DF66EF6, 0x6BF66CF6,
+0x69F66AF6, 0x68F6000B, 0x000007D1, 0x0020397C,
+0x00203980, 0x00203986, 0x001C3DC0, 0x0011772C,
+0x001C3B88, 0x00203964, 0x0011773C, 0x00117744,
+0x0000F000, 0x00117764, 0x00117748, 0x00117768,
+0x0011776C, 0x01FFFFFF, 0x0011774C, 0x002034F4,
+0x00203D9C, 0x002024F0, 0x0020396A, 0x001C3B9C,
+0x001C3D98, 0x001C3700, 0x001C3500, 0x001C5960,
+0x001C8960, 0x002034FC, 0x001C3D00, 0x0020160C,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xDE957FAC, 0x61E0E014, 0x0F14D494, 0x710161E3,
+0xE0186210, 0xD2920F24, 0x0009420B, 0x450BD591,
+0x20080009, 0x8F126D03, 0xD28F1F07, 0x6720D48F,
+0x657CDD8F, 0x470BD78F, 0xD18F0009, 0x619C6910,
+0x46086613, 0x36184608, 0x3D6C4608, 0xE0181FD7,
+0xE58004FC, 0x604C66E2, 0x3050655C, 0x2D628F15,
+0x01FCE014, 0xDE85E500, 0x641CA008, 0x6753655D,
+0x607037EC, 0x39DC6953, 0x80947501, 0x3243625D,
+0xD67F8BF4, 0xA34EE200, 0x20082621, 0xE0148B13,
+0xE40001FC, 0xA009DE79, 0x644D671C, 0x35EC6543,
+0x69436652, 0x39DC6262, 0x74041921, 0x3273624D,
+0xA3388BF3, 0x88012D10, 0xE0148B17, 0xE70001FC,
+0x6D1C2D70, 0xDE6D1FD4, 0x32D3627D, 0xA32A8B01,
+0x677D0009, 0x667365E3, 0x61737504, 0x315C36EC,
+0x69126462, 0xAFEF7708, 0x88042492, 0xE0148B18,
+0xE40001FC, 0x671C2D40, 0x624DDE60, 0x8B013273,
+0x0009A311, 0x6943644D, 0x39EC62E3, 0x72046592,
+0x3D2C6D43, 0x615266D2, 0x21697408, 0x2512AFED,
+0x8B188805, 0x01FCE014, 0x2D40E400, 0xDE53671C,
+0x3273624D, 0xA2F68B01, 0x644D0009, 0x62E36943,
+0x659239EC, 0x6D437204, 0x66D23D2C, 0x74086152,
+0xAFED216B, 0x88312512, 0xD44A8B3A, 0x6146D94A,
+0x75046543, 0x67566442, 0x6E531F48, 0x65527E04,
+0x7EE462E2, 0x7E0464E2, 0x6EE21FE9, 0x5EF929E0,
+0x7E04D942, 0x1FEA60E2, 0x2900C901, 0xD9406EE2,
+0x29E04E09, 0x2F562F26, 0x56FAD93E, 0x6513490B,
+0xD13D7F08, 0xE71C6E0D, 0x1DE12D70, 0xDE3B6912,
+0x64E21D92, 0x1D43D13A, 0xD23A6512, 0x67221D54,
+0x1D75D239, 0x1D666622, 0x6262D638, 0x1D27A2AB,
+0x8B398830, 0x6596D92B, 0x67926696, 0x61967904,
+0x74E46493, 0x6E436992, 0x1F9B7E04, 0x1FEC6442,
+0xD9256EE2, 0x5EFC29E0, 0x7E04D924, 0x1FED60E2,
+0x2900C901, 0xD9226EE2, 0x29E04E09, 0x59FC7FFC,
+0xDE272F92, 0x2F164E0B, 0xD41F7F08, 0xE21C610D,
+0x1D112D20, 0xD2206442, 0xD41C1D42, 0x1D536542,
+0x6752D51B, 0xD71B1D74, 0x1D156172, 0x1D666622,
+0x6262D61A, 0x1D27A26F, 0x8B358833, 0x490BD919,
+0xA268EE00, 0x00002DE0, 0x00117800, 0x00203A14,
+0x002018A2, 0x00202AA4, 0x00203906, 0x00203A18,
+0x0020352C, 0x002018EE, 0x00203905, 0x00117804,
+0x00203984, 0x00117810, 0x00203901, 0x00203902,
+0x00203903, 0x00200F64, 0x001C5864, 0x001C6864,
+0x001C7864, 0x001C59BC, 0x001C69BC, 0x001C79BC,
+0x00200FBC, 0x00200FB8, 0x89018828, 0x0009A0C0,
+0xE643DEB5, 0x326662E1, 0x1FEE8F02, 0x2E21E240,
+0x622D62E1, 0x8B013267, 0x0009A0AA, 0xE50185E1,
+0x8B013056, 0x0009A0A4, 0x2D10E101, 0x64E1B225,
+0xE64357FE, 0x652D6271, 0x89443567, 0x3563E640,
+0xE6008B05, 0x0F65E040, 0xA00FE11A, 0x615372C0,
+0x41214121, 0x41214121, 0x45214121, 0x45214521,
+0xC9036053, 0xE0406603, 0x71180F65, 0x2209E007,
+0x0F25E03C, 0xE044641D, 0xB2A365F3, 0xE33C0F46,
+0x853233FC, 0x620DDE95, 0x42086031, 0x6023610D,
+0x1323E944, 0x06FE4108, 0xE00F39FC, 0x13144E0B,
+0x67075D91, 0x60D32679, 0x0F6654FE, 0x51928542,
+0x600D4E0B, 0x60D3260B, 0x0F666492, 0x65F3B237,
+0x696156FE, 0xE640659D, 0x89383563, 0xD78359FE,
+0x79066591, 0xC9036053, 0x40004008, 0x61036203,
+0x0F26E050, 0x470BE0FF, 0x6C07600C, 0x6603605D,
+0x46214621, 0x46214621, 0x42006263, 0x4200326C,
+0x40214021, 0x4008C903, 0x6D2D30FC, 0xE8006A03,
+0xB25765F3, 0x6EA264D3, 0x2EC9E050, 0x66942AE2,
+0xD76E01FE, 0x606C470B, 0x2AE22E0B, 0x64D365F3,
+0x7801B1FD, 0xEE06628D, 0x8FE932E3, 0x5EFE7D01,
+0x61E1E400, 0x410085E1, 0x66E3310C, 0x760C711B,
+0xE70465F3, 0x68667401, 0x3A736A4D, 0x8FF92582,
+0x65F37504, 0x641DB1E3, 0x64E1B1A4, 0x0009A17B,
+0xD45B56F7, 0xEC01D25B, 0x26C0420B, 0x0009A173,
+0x06FCE018, 0x8829606C, 0x58F78B08, 0xE400D252,
+0x66222840, 0x646DB171, 0x0009A165, 0x666CE681,
+0x89013060, 0x0009A0AC, 0xD550D14F, 0x62126A56,
+0x212232AC, 0x54116C56, 0x34CC6253, 0x64521141,
+0x72085812, 0xD44A384C, 0x68221182, 0x5A136C42,
+0x3ACC3C8C, 0x11A324C2, 0x6C2272EC, 0x72105814,
+0x118438CC, 0x5A156822, 0x11A53A8C, 0x6A227210,
+0xD6405816, 0x118638AC, 0x52176C62, 0x112732CC,
+0x5A185861, 0x11A83A8C, 0x5C195A62, 0x11C93CAC,
+0x521A5C63, 0x112A32CC, 0x5A1B5864, 0x11AB3A8C,
+0x5C1C5A65, 0x11CC3CAC, 0x521D5C66, 0x112D32CC,
+0x5A1E5867, 0x11AE3A8C, 0x561F5A68, 0x36ACE840,
+0x116FDA2D, 0x6CA2381C, 0x7A946682, 0x286236CC,
+0x5C8162A2, 0x18C13C2C, 0x62A27A44, 0x362C5682,
+0xD6261862, 0x5A856262, 0x3A2C4229, 0x760418A5,
+0x56866262, 0x362C4229, 0x56F71866, 0x2620E238,
+0x16C15C81, 0x16226212, 0xE2005C11, 0x551216C3,
+0x55151654, 0x55131655, 0x55161656, 0x55821657,
+0x65821658, 0x55141659, 0x5584165A, 0x5583165B,
+0x5585165C, 0x5586165D, 0x1821165E, 0x11212122,
+0x11251122, 0x11261123, 0x28221822, 0x18241124,
+0x18251823, 0x1826A0C7, 0x00117804, 0x002033E0,
+0x00203A38, 0x002018A2, 0x0020348C, 0x001C36A0,
+0x002034E8, 0x001C3CA0, 0x001C36F4, 0x001C3B88,
+0x666CE682, 0x8B203060, 0xEA2456F7, 0x26A0D194,
+0x16C15C17, 0x16225218, 0x16835819, 0x16A45A1A,
+0x16C55C1B, 0x1626521C, 0xE200581D, 0x551E1687,
+0x551F1658, 0x11271659, 0x11291128, 0x112B112A,
+0x112D112C, 0xA08E112E, 0xE683112F, 0x3060666C,
+0x52F78B0B, 0xEA00D883, 0x658222A0, 0x7804DC82,
+0x62822C52, 0xA07ED681, 0xE6902620, 0x3060666C,
+0xDA7F8B06, 0x00094A0B, 0xE20056F7, 0x2620A073,
+0x666CE691, 0x8B103060, 0x6222D276, 0x2228622C,
+0xD2788904, 0x0009420B, 0x0009A003, 0x420BD276,
+0x56F70009, 0xA05EE200, 0xE6922620, 0x3060666C,
+0xE0188951, 0xE6B00BFC, 0x666C62BC, 0x8B2A3260,
+0x02FCE014, 0x682CEA00, 0x62ADE904, 0x894A3283,
+0x6AADDD64, 0x3CDC6CA3, 0x7D046EC2, 0xDB68D467,
+0x32DC62A3, 0x4B0BDC67, 0x4C0B6D22, 0xD46664E3,
+0x00094B0B, 0x64D34C0B, 0x4B0BD464, 0xE6000009,
+0x666D6BE3, 0x76013B6C, 0x3293626D, 0x8FF72BD0,
+0xAFDA4D19, 0xE6B57A08, 0x3260666C, 0xD45C8B13,
+0x4B0BDB57, 0xD25B0009, 0x6022DB5B, 0xCB20E6FF,
+0x2202666D, 0xDB592B62, 0xE014E200, 0x56F72B20,
+0xA01002FC, 0xD4562620, 0x6542D256, 0x420BD456,
+0xA0080009, 0xDB520009, 0x52B1E600, 0x622CDB53,
+0x52F72B21, 0x7F542260, 0x6EF64F26, 0x6CF66DF6,
+0x6AF66BF6, 0x000B69F6, 0x4F2268F6, 0xE240614D,
+0x89143123, 0x3127E21F, 0x8B09D749, 0xD449614D,
+0xE00171E0, 0x5671440B, 0x26596507, 0x1761A007,
+0xE001D444, 0x6672440B, 0x26596507, 0x4F262762,
+0x0009000B, 0x614D4F22, 0x3123E240, 0xE21F8912,
+0xD73B3127, 0x614D8B08, 0x5671D23A, 0x420B71E0,
+0x260BE001, 0x1761A006, 0x6672D236, 0xE001420B,
+0x2762260B, 0x000B4F26, 0xE6400009, 0x46284618,
+0x6252D531, 0x89FC2268, 0x0009000B, 0x4618E680,
+0xD52D4628, 0x22686252, 0x000B89FC, 0xA0010009,
+0x7201E200, 0x8BFC3242, 0x0009000B, 0x4618E680,
+0xD5264628, 0x22686252, 0x000B8BFC, 0x2FE60009,
+0x7FFC4F22, 0xBFF16E53, 0x61E22F42, 0xE280D620,
+0x54E11615, 0x16464218, 0x422855E2, 0x57E31657,
+0x16786EF2, 0x26E22E2B, 0x4F267F04, 0x6EF6AFCE,
+0x0020348C, 0x00117804, 0x002038EC, 0x00203900,
+0x0020050A, 0x00201008, 0x0020102E, 0x00203A50,
+0x002018A2, 0x002018E6, 0x00203A64, 0x00203A6C,
+0x00203A70, 0x001C3500, 0x001C1000, 0x00203982,
+0x00117800, 0x002018EE, 0x00203A84, 0x00203988,
+0x001C3704, 0x002033E0, 0x001C373C, 0x001C3700,
+0x001C370C, 0x2FD62FC6, 0x4F222FE6, 0x6C53DD10,
+0x6E43BFA4, 0x2DE2BF89, 0x0009BFA0, 0x2C1251D5,
+0x1C4154D6, 0x1C5255D7, 0x1C6356D8, 0x6EF64F26,
+0x000B6DF6, 0x61636CF6, 0xA004E600, 0x62564109,
+0x24227601, 0x36127404, 0x000B8BF9, 0x00000009,
+0x001C370C, 0x0009A16E, 0x2FE62FD6, 0xDD944F22,
+0xA0049EB2, 0xD4930009, 0x420BD293, 0x62D265D2,
+0x8BF822E8, 0x0009A004, 0xD28FD490, 0x55D1420B,
+0x22E852D1, 0xA0048BF8, 0xD48D0009, 0x420BD28A,
+0x52D255D2, 0x8BF822E8, 0x0009A004, 0xD286D489,
+0x55D3420B, 0x22E852D3, 0xA0048BF8, 0xD4860009,
+0x420BD281, 0x52D455D4, 0x8BF822E8, 0x6EF64F26,
+0x6DF6000B, 0x2FD62FC6, 0x4F222FE6, 0x6E636C73,
+0x6D53B01A, 0x64D357F4, 0xB05F65E3, 0xB07566C3,
+0xB0A40009, 0xB0A80009, 0xB0AC0009, 0xB0AC0009,
+0xB0AF0009, 0xB03154F5, 0x6CCD6C03, 0x4F2660C3,
+0x6DF66EF6, 0x6CF6000B, 0x3412D170, 0xD6700529,
+0x2650D770, 0x2742000B, 0x0009A018, 0x2FD62FC6,
+0x4F222FE6, 0x6E636C73, 0x6D53BFEE, 0x64D357F4,
+0xB03365E3, 0xB08D66C3, 0xB00F54F5, 0x6CCD6C03,
+0x4F2660C3, 0x6DF66EF6, 0x6CF6000B, 0xE503D162,
+0xD763D462, 0x21524518, 0x2472000B, 0xD45FD15E,
+0x2162E600, 0x2462000B, 0xBF734F22, 0xBF73E40A,
+0xD25C0009, 0x4118E104, 0xE40AE500, 0xBF692212,
+0xD7592252, 0xCB206072, 0x000B4F26, 0x4F222702,
+0x410BD156, 0xD556E400, 0x4F26452B, 0xD1552FE6,
+0x66126E63, 0x92104418, 0x44084528, 0x45002629,
+0x265B4408, 0x264B4400, 0x21624708, 0xD14E4708,
+0x217227EB, 0x6EF6000B, 0x1FFF03F0, 0x4F222FE6,
+0xE101DE4A, 0xBF3DE40A, 0x67E32E12, 0xE500776C,
+0xE204E130, 0x2752E40A, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27222712,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x175ABF18, 0x2E62E600, 0x000B4F26, 0xD2346EF6,
+0xE441E101, 0x000B2212, 0xD1322242, 0xE605D432,
+0x000B2162, 0x000B2462, 0xD2300009, 0xE40AE601,
+0x2262AF00, 0x2FC62FB6, 0x2FE62FD6, 0x7FFC4F22,
+0x6C43DB2B, 0xED0060B2, 0x2B02CB03, 0xC90360B2,
+0x6E03A008, 0x89073DC2, 0xE46460B2, 0xB07CC903,
+0x7D016E03, 0x8BF52EE8, 0x8F043DC2, 0xD4212FE1,
+0x460BD621, 0x62F10009, 0x6023622D, 0x89FFC801,
+0x7F046023, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x001C3B88, 0x00203A98, 0x002018EE, 0x00203AA0,
+0x00203AA8, 0x00203AB0, 0x00203AB8, 0x0025E720,
+0x00203D98, 0x002038F0, 0x001C5968, 0x001C3B40,
+0x000F8000, 0x001D4004, 0x001C3500, 0x002015E0,
+0x0020160C, 0x001C5814, 0x001C59D0, 0x001C5830,
+0x001C6268, 0x001C59A4, 0x001C639C, 0x001C581C,
+0x001C5860, 0x00203AC0, 0x002018A2, 0x8F014411,
+0x6043604B, 0x0009000B, 0x5651D52B, 0x46286052,
+0x306C000B, 0x2FC62FB6, 0x2FE62FD6, 0x4F124F22,
+0xBFF14F02, 0x6B036E43, 0xDD25DC24, 0x0009BFEC,
+0x3C0530B8, 0x4609060A, 0x46014609, 0x020A3D65,
+0x42094209, 0x32E24209, 0x4F068BF0, 0x4F264F16,
+0x6DF66EF6, 0x000B6CF6, 0x2FC66BF6, 0x2FE62FD6,
+0x4F124F22, 0xBFCF4F02, 0x6C036E43, 0xBFCBDD13,
+0x30C80009, 0x060A3D05, 0x46094609, 0x36E24601,
+0x4F068BF5, 0x4F264F16, 0x6DF66EF6, 0x6CF6000B,
+0x4F222FE6, 0xE102DE0B, 0xE403E500, 0xBFB92E12,
+0xE6062E52, 0xE7004618, 0x2E62E403, 0x4F262E72,
+0x6EF6AFB0, 0x0009000B, 0x001C1040, 0xCCCCCCCD,
+0x10624DD3, 0x001D4004, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE5007F98, 0x6453E710,
+0x6B534728, 0xEE1ADCBC, 0x6153655D, 0x315C4108,
+0x75014108, 0x6043317C, 0x0F16665D, 0xED0060B3,
+0x21B136E3, 0x81128111, 0x11D28113, 0x11D411D3,
+0x74048FEA, 0xD8B167F2, 0x1871D9B1, 0x58F12872,
+0x1981D1B0, 0x59F22982, 0x5DF45AF3, 0x54F65EF5,
+0x21921191, 0x11A211A3, 0x11D411D5, 0x11E611E7,
+0x11481149, 0xDAA855F7, 0x57F8EE00, 0x52F9DDA7,
+0x64E3D6A7, 0x2A521A51, 0xD8A7D9A6, 0x2D729AD5,
+0x6EED2622, 0x4D086DE3, 0x3DEC61E3, 0x4D084108,
+0x3D9C31EC, 0x410860B3, 0x81D12DB1, 0x4108E050,
+0x4008E7B7, 0x677C4108, 0x60B381D2, 0xE200318C,
+0x81D33472, 0x1D131DD2, 0x8D01D493, 0xD4901D24,
+0xB0B365D3, 0x64ED7E01, 0x8BDA34A2, 0x2FD2DA8C,
+0xDD9268A2, 0x2D824829, 0x7DFC64A2, 0xD287694D,
+0x6E222D92, 0x7D0C4E29, 0x68222DE2, 0x618D6AD3,
+0x2A16D784, 0xD48A6D72, 0x24D2D583, 0xD6895E72,
+0x517414E2, 0x1414EE00, 0xD1875876, 0x59781486,
+0x1498E710, 0x65E36252, 0x26E2142A, 0xE60064E3,
+0x6843644D, 0x384C4808, 0x381C4808, 0x0C866053,
+0x09CE28B1, 0x819160B3, 0x0ACE6053, 0x81A26043,
+0x0DCE6053, 0x81D360B3, 0x08CE6053, 0x18827401,
+0x624D09CE, 0x0ACE19E3, 0x1A643273, 0x75048FE0,
+0xE003D96A, 0x40186C92, 0x6D922CB1, 0x81D1DA6F,
+0x6E92E050, 0x81E24008, 0x60B36192, 0x64928113,
+0x1442E600, 0xD4696792, 0x689217A3, 0x1864E1FF,
+0x6563E703, 0x364C4608, 0x26127501, 0x3673665D,
+0xDC5B8BF8, 0x6DC2E003, 0x2DB14018, 0xD2606EC2,
+0x61C281E1, 0x1112EE00, 0xE02464C2, 0x65C21423,
+0x15E4D45B, 0xE58067C2, 0x68C28172, 0x818366E3,
+0x666D655C, 0x76046963, 0x394C6A6D, 0x8FF83A53,
+0xDB5429E2, 0x24B2DC54, 0x24C27404, 0x4F267F68,
+0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6, 0x68F6000B,
+0x60116142, 0x8F03C803, 0xD23DE500, 0x8B063420,
+0xC9036011, 0x8B068802, 0x3420D239, 0x56128B03,
+0x52646513, 0x000B2422, 0x01136053, 0x2FE62FD6,
+0x7FEC4F22, 0x62536E53, 0x6D43E550, 0x4508E400,
+0xE101A001, 0x60435224, 0x81212211, 0x60538123,
+0x56E28122, 0x8BF53620, 0x16E4D238, 0xE61464F3,
+0x65E3420B, 0xE4FC65E1, 0x2E512549, 0x65F361F1,
+0x2F112149, 0xD13154D1, 0xE614410B, 0x607157D1,
+0x2701CB01, 0x7F141DE1, 0x6EF64F26, 0x6DF6000B,
+0x2FE62FD6, 0x7FEC4F22, 0x66536E53, 0x6D43E5FC,
+0x20596061, 0x2601CB01, 0x326052E2, 0x12E48B06,
+0x31E051E2, 0x52D18B04, 0x1E22A002, 0x5664AFF0,
+0x64F3D21E, 0x420BE614, 0x67E165E3, 0x2719E1FC,
+0x67F12E71, 0x271954D1, 0x65F3D118, 0x410BE614,
+0x52D12F71, 0xCB016021, 0x1DE12201, 0x4F267F14,
+0x000B6EF6, 0x00006DF6, 0x0020391C, 0x002034EC,
+0x002034F4, 0x002034FC, 0x00203524, 0x00203908,
+0x00203910, 0x00100208, 0x001017C0, 0x001E210C,
+0x001C3D00, 0x0020395C, 0x001000C8, 0x00117880,
+0x00117780, 0x00040020, 0x0026C401, 0x00200ED6,
+0x4F222FE6, 0xDE42624C, 0x42004208, 0x3E2CA005,
+0xD4405252, 0xBF695624, 0x65E22E62, 0x352052E1,
+0xD63D8BF6, 0x4F262622, 0x6EF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xDC394F22, 0x52C1DB39, 0x362066C2,
+0x6061891C, 0x8801C903, 0xDE348918, 0xBF37DD35,
+0x650364E3, 0x66B28503, 0x3262620D, 0xD4328907,
+0x0009BF76, 0x4D0BD431, 0xAFE60009, 0xBF3D0009,
+0xD42F64E3, 0x00094D0B, 0x0009AFDF, 0x2262D22D,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x2FD62FC6,
+0x4F222FE6, 0xDD29DC28, 0x6E4360C2, 0x04DE4008,
+0xE614D127, 0x65E3410B, 0xD127D726, 0x55E227E2,
+0x35E05254, 0x21228F04, 0x400860C2, 0x122202DE,
+0x605365C2, 0x75014008, 0x0DE606DE, 0xC90F6053,
+0x60632C02, 0x6EF64F26, 0x000B6DF6, 0x85436CF6,
+0x650D5643, 0x622D6262, 0x35277204, 0xE1008F0C,
+0x2268960C, 0xD6158B03, 0x72015261, 0xD6131621,
+0x6262E101, 0x26227201, 0x6013000B, 0x000001FF,
+0x002034FC, 0x002034F4, 0x001C3D00, 0x00203524,
+0x002038EC, 0x002018A2, 0x002034EC, 0x00203AE8,
+0x00203AEC, 0x001C3D28, 0x0020395C, 0x0020391C,
+0x00200ED6, 0x00203960, 0x00203964, 0x00117754,
+0x2FC62FB6, 0x2FE62FD6, 0x7FF84F22, 0x6022D237,
+0x8D58C803, 0xDE362F01, 0xDB37DC36, 0x66C252C1,
+0x892F3620, 0xC9036061, 0x892B8801, 0xD233DD31,
+0x64D3420B, 0x1F016503, 0x880160B1, 0xD2308B04,
+0x64D3420B, 0x0009AFEA, 0x85615653, 0x8904C820,
+0xE050D72C, 0x7201027E, 0xD22B0726, 0x6453420B,
+0x89072008, 0x55F1D126, 0x64D3410B, 0xE601D727,
+0x2762AFD4, 0x55F1D226, 0x64E3420B, 0xE601D125,
+0x2162AFCC, 0xDD25DE24, 0xDC26DB25, 0x66D252D1,
+0x89183620, 0xC9036061, 0x89148801, 0xD117D41F,
+0x0009410B, 0x36E05603, 0x65038F04, 0x2B20E201,
+0x2C52AFEC, 0xD712D41C, 0x0009470B, 0xE601D115,
+0xAFE34618, 0x60F12162, 0x8907C804, 0x7F08D217,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6422B, 0x4F267F08,
+0x6DF66EF6, 0x000B6CF6, 0x00006BF6, 0x001E2100,
+0x002034FC, 0x002034F4, 0x00203984, 0x002014A0,
+0x002014CC, 0x0020348C, 0x002016BE, 0x001E212C,
+0x00201530, 0x001C3D30, 0x00117880, 0x002034EC,
+0x0020390C, 0x00203908, 0x00203524, 0x00200610,
+0xE601D203, 0x1265D503, 0x000B2252, 0x00001266,
+0x001C1010, 0x0000C34F, 0x0009000B, 0x2FD62FC6,
+0x4F222FE6, 0x6D436C53, 0xEE00A004, 0x7E0164D4,
+0x644CBFF2, 0x8BF93EC2, 0x6EF64F26, 0x000B6DF6,
+0xE5006CF6, 0x6643A002, 0x76017501, 0x22286260,
+0xAFE38BFA, 0x2FE60009, 0x75076253, 0xE1086753,
+0x6043EE0A, 0x4409C90F, 0x650330E2, 0x8D014409,
+0xE630E637, 0x4110365C, 0x8FF22760, 0xE00077FF,
+0x000B8028, 0x000B6EF6, 0x000BE000, 0x2FE6E000,
+0x7FEC4F22, 0x6E436253, 0xBFDC65F3, 0xBFD06423,
+0xBFCE64E3, 0xD40364F3, 0x0009BFCB, 0x4F267F14,
+0x6EF6000B, 0x00203AF0, 0xE4FDD29D, 0xD79D6122,
+0x22122149, 0x74016022, 0x2202CB01, 0xD59A6622,
+0x22622649, 0xC8406070, 0x60528902, 0x2502CB04,
+0xE6016052, 0x2502CB08, 0xE4026052, 0x2502C9CF,
+0x46186052, 0x2502CB10, 0xCB036052, 0x15422502,
+0x1563000B, 0xD78ED58D, 0xD48FD28E, 0xE600E100,
+0x27112511, 0xAFCF2210, 0x664C2461, 0x4600D28B,
+0x6060362C, 0x000BCB10, 0x654C2600, 0x4500D287,
+0x6650352C, 0x2619E1EF, 0x2560000B, 0xD284664C,
+0x362C4600, 0xCB106060, 0x2600000B, 0xD280654C,
+0x352C4500, 0xE1EF6650, 0x000B2619, 0x664C2560,
+0x4600D27A, 0x6060362C, 0x000BCB08, 0x654C2600,
+0x4500D276, 0x6650352C, 0x2619E1F7, 0x2560000B,
+0xD273664C, 0x362C4600, 0xCB086060, 0x2600000B,
+0xD26F654C, 0x352C4500, 0xE1F76650, 0x000B2619,
+0x624C2560, 0x4200D669, 0x6020326C, 0x4021C908,
+0x40214021, 0x600C000B, 0xD665624C, 0x326C4200,
+0xC9086020, 0x40214021, 0x000B4021, 0xD161600C,
+0x341C644C, 0x000B6240, 0xD15F602C, 0x341C644C,
+0x000B6240, 0x2FE6602C, 0x6E434F22, 0xE60A645C,
+0x89143467, 0x0009BFEB, 0x60EC640C, 0x8B028801,
+0xA002E00F, 0x44092409, 0x624C4409, 0x3263E60A,
+0xBFE28905, 0x620C644C, 0xC8806023, 0xE2008B00,
+0x4F266023, 0x6EF6000B, 0xD64C4F22, 0x88016062,
+0xB2578B03, 0xA0030009, 0xD2490009, 0x2260E640,
+0xE200D648, 0x000B4F26, 0x4F222622, 0x6062D643,
+0x8B018802, 0x0009B2A0, 0xE200D642, 0x000B4F26,
+0xD53E2622, 0xE100D43E, 0x2512E701, 0x2470000B,
+0xE604D23B, 0x2260000B, 0xD43B4F22, 0x410BD13B,
+0xD53B0009, 0x6650E1FD, 0x2619D23A, 0x2560E700,
+0x000B4F26, 0x4F222270, 0xD238D537, 0xD7386152,
+0x2512611D, 0xE6FF6452, 0x2542242B, 0xD22FD435,
+0x420B666D, 0xD52E2762, 0x6750E1FB, 0x4F262719,
+0x2570000B, 0xD4304F22, 0x410BD128, 0xD5280009,
+0x6650E7F7, 0x4F262679, 0x2560000B, 0x9425D524,
+0x22496250, 0x2520000B, 0xE4BFD521, 0x22496250,
+0x2520000B, 0xD2254F22, 0x600D8522, 0x89112008,
+0x89458801, 0x89478803, 0x89498805, 0x894F8806,
+0x89558808, 0x895B8809, 0x8961880A, 0x8967880B,
+0x0009A06E, 0x0009B070, 0x600CA06B, 0x0000FF7F,
+0x001E2148, 0x001E1000, 0x001E1108, 0x002039BC,
+0x002039BE, 0x002039DD, 0x002039A0, 0x001E103F,
+0x001E105F, 0x001E102F, 0x001E1090, 0x002039C4,
+0x001E100B, 0x002039C0, 0x00203AF4, 0x002018A2,
+0x001E1028, 0x002039DC, 0x001D4020, 0x98760000,
+0x001C1000, 0x00203B00, 0x00203B10, 0x00203994,
+0x0009B04C, 0x600CA035, 0x0009B055, 0x600CA031,
+0x6260D684, 0x8B2B2228, 0x0009B061, 0x600CA029,
+0x6260D680, 0x8B232228, 0x0009B069, 0x600CA021,
+0x6260D67C, 0x8B1B2228, 0x0009B0C7, 0x600CA019,
+0x6260D678, 0x8B132228, 0x0009B0CD, 0x600CA011,
+0x6260D674, 0x8B0B2228, 0x0009B125, 0x600CA009,
+0x6260D670, 0x8B032228, 0x0009B13D, 0x600CA001,
+0x4F26E000, 0x0009000B, 0xD26CD16B, 0xD56C8412,
+0x4000C90F, 0xD76B012D, 0xE403D66B, 0xE20F611C,
+0x2540E001, 0x25202712, 0x2602000B, 0xE601D262,
+0x30668523, 0xE0008D05, 0xD663D260, 0xE0018122,
+0x000B2602, 0xD25C0009, 0x600D8523, 0x89052008,
+0x8B0A8801, 0x6060D65D, 0x2600CB01, 0xD457D65A,
+0xE001E101, 0x000B2612, 0x000B8142, 0xD152E000,
+0x8513E501, 0x640D4518, 0x66033453, 0xE0008D05,
+0xD551D253, 0x2260E001, 0x000B2502, 0x4F220009,
+0x8513D149, 0x6453650D, 0x62494419, 0x227D672E,
+0x8801602C, 0x88028909, 0x88038910, 0x8806891A,
+0x88078935, 0xA04C893B, 0xD5460009, 0x6652D746,
+0x2762D446, 0x622C6261, 0x2421A038, 0x2228625C,
+0xD4438B3F, 0x6642D540, 0x2562D440, 0x24018561,
+0x6203A02C, 0x2008605C, 0x88108907, 0x88208908,
+0x88308909, 0xA02C890A, 0xD23A0009, 0x6222A008,
+0xA005D239, 0xD2396222, 0x6222A002, 0x6262D638,
+0xD432D531, 0x66212522, 0xA00F626C, 0xD6352421,
+0x6261D52D, 0x622CD42D, 0xA0072562, 0xD6322421,
+0x8561D529, 0x2562D429, 0x62032401, 0x662D8515,
+0x3617610D, 0x65038F01, 0xB0CB2451, 0xA0010009,
+0xE000E001, 0x000B4F26, 0xD6190009, 0xD427E101,
+0x65412610, 0xD118D717, 0xE20F655D, 0x2752E001,
+0x000B2620, 0x2FE62102, 0xD20F4F22, 0x640C8523,
+0x8B082448, 0xD511D61D, 0x2621E200, 0x940F8451,
+0xA0482049, 0xDE0D8051, 0xC84060E0, 0xE2018D32,
+0x89443427, 0xD216D615, 0x2641420B, 0x0009A030,
+0x0000FF7F, 0x002039DD, 0x00203994, 0x002039A0,
+0x001E1100, 0x001E100C, 0x002039C0, 0x001E1000,
+0x001E1001, 0x002039C8, 0x002039A8, 0x002039AC,
+0x002039B0, 0x002039CC, 0x002039D0, 0x002039D4,
+0x002039D8, 0x00203DFC, 0x00203E06, 0x002039BA,
+0x0020287E, 0x89123427, 0xD294D693, 0x2641420B,
+0xCB8084E1, 0x80E1B0F5, 0xD69160E0, 0x2E00CB04,
+0xC93F6060, 0xD68F2600, 0xA001E001, 0xE0002602,
+0x000B4F26, 0xD68C6EF6, 0xC8806060, 0xD2868919,
+0x88016021, 0xD2898B15, 0x8524E501, 0x89103056,
+0xE203D187, 0x2120D487, 0xE00B6541, 0x0656655D,
+0xE40FD585, 0x2140E702, 0xD77E2571, 0x000BE001,
+0x000B2702, 0x2FE6E000, 0xDE804F22, 0xC88084E1,
+0xD57A892C, 0x20088554, 0x61038F28, 0x8553D77C,
+0x64036672, 0x8566650C, 0x3520620C, 0xD6798B1E,
+0x651CD774, 0x2651644C, 0x60E02741, 0x8904C840,
+0x420BD275, 0xA0030009, 0xD2680009, 0x0009420B,
+0x0009B09F, 0xE201D167, 0x60E02122, 0xCB04D464,
+0x60402E00, 0x2400C93F, 0x6023A001, 0x4F26E000,
+0x6EF6000B, 0x2FB62FA6, 0x2FD62FC6, 0xDA622FE6,
+0x66A1E240, 0x3622DC5E, 0x62638900, 0x6ED36D2C,
+0x4E2136D8, 0x4E212A61, 0xDB61D460, 0xE700A00F,
+0x770162B2, 0x71026123, 0x66212B12, 0x71026213,
+0x61212B12, 0x651D666D, 0x356C4528, 0x627C2452,
+0x8BED32E3, 0xC90360D3, 0x8B108803, 0x617367B2,
+0x2B127102, 0x71026E13, 0x2B126571, 0x655D6DE1,
+0x422862DD, 0x325CE107, 0xA00C2C10, 0x88022422,
+0xA0038B01, 0x8801E203, 0xE2018B05, 0x66B22C20,
+0x655D6561, 0xE60F2452, 0x67A12C60, 0x8B052778,
+0xDD38DC44, 0xEB01EA00, 0x2DB22CA2, 0x6DF66EF6,
+0x6BF66CF6, 0x6AF6000B, 0x2FE62FD6, 0xE240DD36,
+0x362266D1, 0x62638900, 0x3678672C, 0x7703DE38,
+0x47212D61, 0x64E2D635, 0xA00E4721, 0x6562E100,
+0x62537101, 0x74012450, 0x24204219, 0x45297401,
+0x74012450, 0x24504519, 0x621C7401, 0x8BEE3273,
+0x66E24200, 0x420061D1, 0x2118362C, 0x2E628F06,
+0xDD1CD728, 0xE501E400, 0x2D522742, 0x000B6EF6,
+0x2FD66DF6, 0x4F222FE6, 0xED0AEE01, 0x64E3BC85,
+0xBC8A64E3, 0x62EC7E01, 0x8BF732D7, 0xBC8DEE01,
+0x64E364E3, 0x7E01BC92, 0x32D762EC, 0x4F268BF7,
+0x000B6EF6, 0xD1186DF6, 0xD418920D, 0x72122122,
+0x2422D617, 0xD7177204, 0x72202622, 0x2722D116,
+0x000B7230, 0x137A2122, 0x002039BA, 0x0020298A,
+0x001E1015, 0x002039C0, 0x001E1001, 0x00203994,
+0x001E1100, 0x002039BE, 0x002039AC, 0x001E1000,
+0x002039B0, 0x002039BC, 0x0020287E, 0x001E100C,
+0x002039A8, 0x002039C4, 0x002039C8, 0x002039CC,
+0x002039D0, 0x002039D4, 0x002039D8, 0x4F222FE6,
+0xD6707FFC, 0x88016060, 0xE2018951, 0x2620BFBB,
+0xD56ED16D, 0xDE6E6010, 0x64E36552, 0x7402C840,
+0x8D22D16C, 0xD26C7502, 0xE601D76C, 0xE7042722,
+0x76016255, 0x626C2421, 0x8FF93273, 0xD4637402,
+0x6242E601, 0x640D8528, 0x67494419, 0x275D657E,
+0x81E4607C, 0xE417D562, 0x67557601, 0x3243626C,
+0x8FF92171, 0xA0207102, 0xD25E0009, 0xE601D75B,
+0xE7042722, 0x76016255, 0x626C2421, 0x8FF93273,
+0xD4527402, 0x6242E601, 0x640D8528, 0x67494419,
+0x275D657E, 0x81E4607C, 0xE417D553, 0x67557601,
+0x3243626C, 0x8FF92171, 0x92897102, 0xD2462E21,
+0x5E23D74E, 0x64F22FE2, 0x604365F2, 0x2700C980,
+0xC9606043, 0x80716103, 0xC9036043, 0x80724519,
+0x65F2605C, 0x817266F2, 0x46194629, 0x606C4529,
+0x4018645C, 0x8173304C, 0x21185E23, 0x64F22FE2,
+0x6E4C62F2, 0x602C4219, 0x66F262F2, 0x46294018,
+0x461930EC, 0x42298174, 0x652C606C, 0x305C4018,
+0x81758F07, 0x0009BC96, 0x2228620C, 0xA00A8908,
+0x60130009, 0x8B038840, 0x0009B009, 0x0009A003,
+0xE202D62F, 0x7F042622, 0x000B4F26, 0x4F226EF6,
+0x8552D52A, 0x8830600D, 0x88318903, 0xA0348923,
+0x85550009, 0xD428D727, 0x85532701, 0x610DD627,
+0x24124118, 0x460BD426, 0xD7230009, 0xD226D425,
+0x6572420B, 0xE230D120, 0x42286712, 0x2729E620,
+0x37604628, 0xD6218B03, 0xA016E200, 0xD61F2622,
+0xA012E202, 0xD1182622, 0x6212E530, 0xE6204528,
+0x46282259, 0x89083260, 0xD41AD119, 0xE601D513,
+0x2160450B, 0x472BD718, 0x4F264F26, 0x0009000B,
+0x0000060A, 0x002039DC, 0x001E1000, 0x002039C8,
+0x00203DFC, 0x00203E08, 0x00203DA0, 0x002039B0,
+0x00203DD0, 0x00203DCE, 0x00203DA2, 0x00203994,
+0x002039C0, 0x002039AC, 0x002039A8, 0x002018A2,
+0x00203B1C, 0x00203B20, 0x002018EE, 0x002039C4,
+0x001E100B, 0x00203B34, 0x00114004, 0x4F222FE6,
+0xDE967FFC, 0x200884E9, 0x2F008D06, 0xD695D494,
+0x0009460B, 0x64F0B19A, 0x6620D293, 0x89022668,
+0xC9BF60E0, 0x7F042E00, 0x000B4F26, 0x000B6EF6,
+0x2FE60009, 0xDE8D4F22, 0x60E0D68D, 0xCBC0D48D,
+0x62602E00, 0xC803602C, 0x40218904, 0x70014021,
+0x6603A002, 0x66034009, 0xD687616D, 0xE500A004,
+0x75016262, 0x74042422, 0x3213625D, 0xD2838BF8,
+0x0009420B, 0xC9BF84E2, 0x4F2680E2, 0x6EF6000B,
+0x2FE62FD6, 0x7FFC4F22, 0x6260D67D, 0x89442228,
+0xD572E100, 0x60502610, 0xCB40D47A, 0x2500440B,
+0x8D052008, 0x62E06E03, 0x7104612C, 0x2F11A006,
+0xD475D66D, 0xDD756760, 0x657C4D0B, 0xE23C6D1D,
+0x8B033D27, 0xD267D472, 0x0009420B, 0x4D214D21,
+0xA005D770, 0x66E6E400, 0x357C4508, 0x74012562,
+0x35D3654D, 0xD76C8BF7, 0x6172E003, 0x81114018,
+0x6E7260F1, 0x81E2700C, 0xD4686172, 0xDD688113,
+0x4D0BDE68, 0xE2016572, 0xD4672E22, 0x420BD255,
+0xD6560009, 0xC93F6060, 0x7F042600, 0x6EF64F26,
+0x6DF6000B, 0x2FC62FB6, 0x2FE62FD6, 0xD25F4F22,
+0x6B436E73, 0x420B6C53, 0x20086D63, 0x64038D1C,
+0xE50ED149, 0x32526210, 0x60C38916, 0x804124B0,
+0x814160D3, 0xA007E500, 0x655D61BC, 0x00EC6053,
+0x364C6653, 0x80647501, 0x3213625D, 0xD63B8BF5,
+0xC9BF6060, 0x2600A008, 0xD23AD44D, 0x6EF64F26,
+0x6CF66DF6, 0x6BF6422B, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6000B, 0x7FC44F22, 0x720262F3, 0x22512F41,
+0x45297202, 0x60632251, 0xE5C4E682, 0x67F38121,
+0x655C666C, 0xE408BFB6, 0x4F267F3C, 0x0009000B,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xE1007FC4, 0x6513ECFF, 0x6B136CCD, 0xDE36D735,
+0xEDFF64F3, 0xD835EA04, 0x6053655C, 0x027D4000,
+0x32C0622D, 0x66038D0D, 0x09ED6063, 0x2491027D,
+0x24217402, 0x698202ED, 0x3928622D, 0x74022892,
+0x75017104, 0x6063625C, 0x07D532A2, 0x0EB58FE4,
+0x2448641C, 0xE6808905, 0x67F3E5C5, 0xBF79666C,
+0x7F3C655C, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0xD11E68F6, 0x6012D21E, 0xCB20E405,
+0x2102E500, 0x000B2242, 0x00002252, 0x001E1017,
+0x00203B38, 0x002018A2, 0x00203906, 0x001E1015,
+0x001E10BF, 0x00117800, 0x001E10FC, 0x00200610,
+0x0020390C, 0x00202AE2, 0x00203B3C, 0x002018EE,
+0x00203B58, 0x0011788C, 0x00203908, 0x002034EC,
+0x00201530, 0x001E2130, 0x00203B60, 0x00202AA4,
+0x00203B64, 0x0020396C, 0x00203974, 0x00203D9C,
+0x001C3500, 0x001D4004, 0xD564D163, 0xE400D764,
+0x2142E20F, 0x17411154, 0xD5622722, 0x9669D762,
+0x15412572, 0x96661562, 0xE6011565, 0xD55F1165,
+0x666CE6F8, 0x25422542, 0x25422542, 0x25422542,
+0x25622542, 0x7601E727, 0x67632572, 0x25627797,
+0xE7042572, 0x2572E248, 0xE2192522, 0xE2702522,
+0x25422542, 0x25422542, 0x25222542, 0x2522E20C,
+0x25422542, 0x25422542, 0x25422542, 0x25422542,
+0x000B154A, 0xE2081145, 0x0009422B, 0x2FE62FD6,
+0x7FFC4F22, 0xC8206043, 0x6E438D02, 0x0009BE67,
+0xC81060E3, 0xBE648901, 0x60E30009, 0x8901C840,
+0x0009BE86, 0xC80160E3, 0xDD3D8938, 0xC80260D0,
+0x2F008D03, 0x460BD63B, 0x60F00009, 0x8902C804,
+0x460BD639, 0x62F00009, 0xC8806023, 0x60D08902,
+0x2D00C97F, 0xC8016023, 0xD6348906, 0x0009460B,
+0x0009A007, 0x51630601, 0x8902C808, 0x460BD630,
+0x60F00009, 0x8902C810, 0x420BD22E, 0xD52E0009,
+0x88026052, 0xD22D8B03, 0xA005E604, 0x88012260,
+0xD22A8B02, 0x2260E601, 0x2522E200, 0xC88060E3,
+0xD227892D, 0x60E36E20, 0x8902C880, 0x420BD225,
+0x60E30009, 0x8902C840, 0x420BD223, 0x60E30009,
+0x8902C802, 0x420BD221, 0x60E30009, 0x890DC804,
+0xDD20D11F, 0x0009410B, 0x0009BF0D, 0x0009BF4C,
+0xD51ED41D, 0x2470E708, 0x25D2BF85, 0xC80860E3,
+0xD21B8905, 0x4F267F04, 0x422B6EF6, 0x7F046DF6,
+0x6EF64F26, 0x6DF6000B, 0x001C581C, 0xA000A000,
+0x001D0100, 0x001D4000, 0x00040021, 0x001C589C,
+0x001E1021, 0x00201A88, 0x00201AAA, 0x0020210C,
+0x00201AC2, 0x00201AD0, 0x002039C0, 0x001E100B,
+0x001E1028, 0x00201B3C, 0x00201B48, 0x00201AD8,
+0x00201AF6, 0x12345678, 0x001E1000, 0x0010F100,
+0x00201B24, 0x644CD6A7, 0x000B346C, 0xD6A62450,
+0x346C644C, 0x2450000B, 0x644CD6A4, 0x000B346C,
+0x625C2450, 0x4208616D, 0x42084119, 0x42006019,
+0x670E614C, 0xD49E321C, 0x4200207D, 0x324CC90F,
+0x2200000B, 0x4208625C, 0x42004208, 0x324C644C,
+0x4200D498, 0x000B324C, 0x2FE62260, 0x614C4F12,
+0x4100D493, 0x6710314C, 0xE29F666D, 0x27294619,
+0x6E536269, 0x672E6573, 0x4221227D, 0x42214221,
+0x7601662C, 0xE4014608, 0x34E84608, 0x644C4600,
+0x071A0467, 0x2150257B, 0x000B4F16, 0x4F226EF6,
+0xD2857FE8, 0x88016021, 0xD2848B7B, 0x26686621,
+0xD2838B77, 0x26686621, 0xE50F8B73, 0xE401BFA2,
+0xBFA4E501, 0xE586E400, 0xE400655C, 0x2F50BFA4,
+0xBFA1E401, 0xE602E506, 0x60634618, 0x81F2E401,
+0x6543BF9F, 0xE40185F2, 0xBFAB6543, 0x85F26603,
+0x6543E401, 0x6603BFB1, 0xE40265F0, 0x6053756C,
+0x80F8BF80, 0xBF82E402, 0x84F8E512, 0x7090E402,
+0x6503BF82, 0x4618E602, 0x81F66063, 0xBF80E402,
+0x85F6E500, 0x6603E402, 0xE500BF8C, 0xE40285F6,
+0xBF926603, 0xE5FEE500, 0xE010655C, 0xBF61E403,
+0xE5130F54, 0xE40EBF63, 0x05FCE010, 0xBF63E40E,
+0xE5007585, 0xBF64E403, 0xE500E640, 0xBF71E403,
+0xE500E640, 0xBF78E403, 0xE5FFE640, 0xE014655C,
+0xBF47E404, 0xE40F0F54, 0xE504BF49, 0x05FCE014,
+0xBF49E40F, 0xE5017584, 0xBF4AE640, 0xE501E404,
+0xBF57E640, 0xE501E404, 0xE404E640, 0xAF5C7F18,
+0x7F184F26, 0x000B4F26, 0x4F220009, 0xD2427FF0,
+0x88016021, 0xD2418B71, 0x26686621, 0xD2408B6D,
+0x26686621, 0xE50F8B69, 0xE401BF1C, 0xBF1EE501,
+0xE586E400, 0xE400655C, 0x2F50BF1E, 0xBF1BE401,
+0xE401E506, 0xBF1C6543, 0xE401E640, 0xBF296543,
+0xE401E640, 0xBF306543, 0x65F0E640, 0x756CE402,
+0xBEFF6053, 0xE40280F4, 0xE512BF01, 0xE40284F4,
+0xBF017090, 0xE6406503, 0xBF02E402, 0xE640E500,
+0xBF0FE402, 0xE640E500, 0xBF16E402, 0xE5FEE500,
+0x6053655C, 0xBEE5E403, 0xE51380F8, 0xE40EBEE7,
+0xE40E84F8, 0xBEE77085, 0xE5006503, 0xBEE8E640,
+0xE500E403, 0xBEF5E640, 0xE500E403, 0xBEFCE640,
+0xE5FFE403, 0x6053655C, 0xBECBE404, 0xE40F80FC,
+0xE504BECD, 0xE40F84FC, 0xBECD7083, 0xE5016503,
+0xBECEE640, 0xE501E404, 0xBEDBE640, 0xE501E404,
+0xE404E640, 0xAEE07F10, 0x7F104F26, 0x000B4F26,
+0x00000009, 0x001E102F, 0x001E1080, 0x001E1090,
+0x001E103F, 0x001E103E, 0x002039BA, 0x002039BC,
+0x002039BE, 0xD21DD11C, 0x66206010, 0x676C7001,
+0x3700C90F, 0xE5008D13, 0x67106210, 0x7701622C,
+0x64232170, 0xD6166010, 0x44084408, 0x3428C90F,
+0x62602100, 0x7201D513, 0x44082620, 0x000B354C,
+0xD10F6053, 0x25586510, 0xE6008D13, 0xD60DD40B,
+0x655C6540, 0x47086753, 0x37584708, 0x47086540,
+0x24507501, 0x367C6040, 0x2400C90F, 0x72FF6210,
+0x000B2120, 0x00006063, 0x00203905, 0x00203904,
+0x00203906, 0x0020352C, 0x7FFC4F22, 0xE680D19F,
+0x666C6212, 0xD29E2F22, 0x67F36563, 0x420B7542,
+0x7F04E404, 0x000B4F26, 0xE6800009, 0xD298666C,
+0xE7006563, 0x422B7540, 0xE6806473, 0xD294666C,
+0xE7006563, 0x422B7543, 0x2F866473, 0x2FA62F96,
+0x2FC62FB6, 0x2FE62FD6, 0x7FCC4F22, 0xDC8ED28D,
+0x72011F21, 0xDB8D1F22, 0xD18EDE8D, 0x66125211,
+0x8B013620, 0x0009A0E5, 0xC9036061, 0x8B018801,
+0x0009A0DF, 0xD288D487, 0xED84420B, 0x2F025503,
+0x30D0845C, 0xA0B88901, 0xD1840009, 0x626C6610,
+0x88016023, 0xD1828B68, 0x62101FC3, 0x895B2228,
+0xE003D480, 0x40186742, 0x68421772, 0xD57EE900,
+0x81816DB3, 0x7D042190, 0x67D26AB2, 0x64E26852,
+0x1F491F57, 0x740464E3, 0x1FA46542, 0x65431F5A,
+0x625275F8, 0x1F761FD5, 0x6D531F2B, 0xDA74D773,
+0x7D94D274, 0x68D21F88, 0x6AA26972, 0xD1726022,
+0x2202CB20, 0xE1401F1C, 0x7601E600, 0x3213626D,
+0x56F48BFB, 0x52F651F5, 0x21222B62, 0x52F851F7,
+0x212256F9, 0x2E6251FA, 0x51FB2412, 0x2D822512,
+0xD9662792, 0x29A2DD5F, 0x6AD2D965, 0xD9646892,
+0x68D21A84, 0x6081DA63, 0x2801CB01, 0xD86266D2,
+0x2A622962, 0xED015AFC, 0x2AD2480B, 0x2AD24D18,
+0x62D2DD5E, 0x2D227201, 0xD15056F3, 0xE2026062,
+0x2602CB01, 0x2120A03D, 0x8B3A2228, 0xE401DD58,
+0x2140E600, 0xE01C2D62, 0xC801005C, 0xD4558B0A,
+0xE600D755, 0xED7D2472, 0x626C7601, 0x8BFB32D3,
+0x24D2DD52, 0xE2FE68C2, 0x2C822829, 0x095CE01E,
+0xE01F5DF1, 0x0A5C2D90, 0x751051F2, 0xED0621A0,
+0xD74BE600, 0x8456D44B, 0x27007601, 0x696C6854,
+0x248039D3, 0x8FF67401, 0xDA477701, 0x2A10E194,
+0xE2007A01, 0x7A0F2A20, 0xD130E805, 0x66102A80,
+0x6023626C, 0x89088801, 0xD240D42A, 0x420B65F2,
+0xD131ED01, 0xAF304D18, 0x65F221D2, 0x8553D43C,
+0x620D6642, 0x89073262, 0xD13BD43A, 0x0009410B,
+0xE601D73A, 0x2762AF1A, 0xD134D41E, 0x410B65F2,
+0xD125ED01, 0xD637D436, 0x460B4D18, 0xAF0D21D2,
+0x7F340009, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0x4F2268F6, 0x85467FF4, 0x2F01E681,
+0x666C8547, 0x854881F1, 0x81F2D209, 0x67F38542,
+0x854381F3, 0x81F4E40C, 0x65636053, 0x420B81F5,
+0x7F0C7540, 0x000B4F26, 0x00000009, 0x001C3D9C,
+0x00202454, 0x0011779A, 0x001C36F8, 0x001C3B9C,
+0x001C3704, 0x00203524, 0x002014A0, 0x00203915,
+0x00203914, 0x00203910, 0x001C3D98, 0x001C3BB4,
+0x001C5960, 0x001C3500, 0x001C3D30, 0x001C8960,
+0x002034FC, 0x001C3D00, 0x0020160C, 0x00117730,
+0x00203918, 0x001C582C, 0x2000A000, 0x0000A000,
+0x0011778C, 0x00117792, 0x00117788, 0x002014CC,
+0x002038EC, 0x002034EC, 0x00201530, 0x001E2130,
+0x00203D7C, 0x002018A2, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xD19B7FEC, 0x2F12E000,
+0x6103D49A, 0x1F4281F2, 0xDD9ADA99, 0xD69A6813,
+0xE0014808, 0x460BDE99, 0x38EC4800, 0x65A21F03,
+0x352052A1, 0xA23E8B01, 0x60510009, 0x8801C903,
+0xA2388B01, 0x52530009, 0x32E0DE91, 0xD9918B10,
+0x64A3490B, 0x4B0BDB90, 0xDE906403, 0xD791D690,
+0xEC01D591, 0x2E02E100, 0x271026C0, 0x2502AFDF,
+0xC8018551, 0xA1578B01, 0x62510009, 0x4200622D,
+0x5E53366A, 0x85E2226D, 0xC903642C, 0x85E36603,
+0x6053650D, 0x40214021, 0x4500C93F, 0x322A6703,
+0x6053252D, 0xC901D17F, 0x60106C03, 0x8801D97F,
+0xDB7F8B05, 0x2120E200, 0xCB0160B2, 0xD17D2B02,
+0x88016011, 0x65A28B0A, 0x8D042448, 0x9B9E6251,
+0xA00322B9, 0x919B2521, 0x2521221B, 0x37B3EB10,
+0x2448895E, 0xD4738B07, 0x22286241, 0x60638903,
+0xA05781F8, 0xD5706473, 0x46084608, 0x85E26273,
+0x46006B50, 0x362C4200, 0x2BB8C910, 0x8F1F6463,
+0x26686603, 0xD2698911, 0x062D6043, 0x4119616D,
+0x6B0E6019, 0x81F820BD, 0x880160C3, 0x646C8F2C,
+0x880F6073, 0xA0278B1B, 0xD2610009, 0x052D6043,
+0x4119615D, 0x670E6019, 0x645C207D, 0x81F8A01C,
+0x890F2668, 0x6043D25B, 0x6B5D052D, 0x60B94B19,
+0x201D610E, 0x60C381F8, 0x8F0D8801, 0x6473645C,
+0xEC00A00A, 0x6043D254, 0x625D052D, 0x60294219,
+0x207D670E, 0x81F8645C, 0x880285F8, 0x85E1890A,
+0x8D07C820, 0xE6DC6203, 0x60232269, 0x81E1A002,
+0x644CE4FF, 0x6210D149, 0x89012228, 0x644CE4FF,
+0x654DEBFF, 0x35B06BBC, 0xDB368B2B, 0x64A34B0B,
+0x410BD135, 0x54036403, 0x85446E03, 0xC948DB40,
+0xDC408808, 0xBEAE8B01, 0x64B3E502, 0x65E34C0B,
+0xDB3DEC01, 0xD13D2DC2, 0x621260B2, 0x72017001,
+0x21228805, 0x2B028F08, 0x666CE680, 0x6563D238,
+0x7549E700, 0x6473420B, 0xA030D436, 0x7FFF0009,
+0x85E28000, 0x20B9EBFC, 0x610381E2, 0x942A85E3,
+0x62032049, 0x450885F8, 0x81E2201B, 0xC90160C3,
+0x40084018, 0x40084008, 0x4000225B, 0x6023220B,
+0x85E481E3, 0x4118E108, 0x81E4201B, 0xE40262A2,
+0x20B98521, 0x67A28121, 0xCB016071, 0x85F82701,
+0x89033042, 0xECE785E2, 0x81E220C9, 0x490BD41E,
+0xA03B0009, 0x7E030009, 0x001C3D30, 0x00203D88,
+0x002034FC, 0x001E212C, 0x002033E0, 0x001C3D00,
+0x00117780, 0x002014A0, 0x0020166C, 0x0011770C,
+0x00203914, 0x00203915, 0x00203910, 0x002018A2,
+0x001C36F8, 0x00203988, 0x00203D98, 0x00203B7C,
+0x00203BFC, 0x00203C7C, 0x00203CFC, 0x00203900,
+0x002034F4, 0x002014CC, 0x0020398C, 0x00203990,
+0x00202454, 0x00203D80, 0x00203D84, 0x602262F2,
+0x40094019, 0xC90F4009, 0x8B0B880A, 0x60E2DE8C,
+0x40094019, 0xC90F4009, 0x8B038808, 0xCB0160A2,
+0x2802A006, 0x65E2DE87, 0x2E527501, 0x286266A2,
+0x52F366F2, 0x2622AE83, 0xD2838551, 0xDE83C802,
+0xA0958B01, 0x420B0009, 0x4E0B64A3, 0x5E036403,
+0x85E46503, 0x4918E908, 0xD77D209B, 0xE04C81E4,
+0xDC7C0B7E, 0x7B01D97C, 0x61C207B6, 0x71016690,
+0x8D062668, 0xD4792C12, 0x420BD279, 0xA070EB01,
+0x62512DB2, 0x4B18EB0F, 0x22B9E102, 0x32104118,
+0x85518B0F, 0x2029E2FC, 0x60518151, 0xCB0172E0,
+0x85E12501, 0x202994A3, 0x85E481E1, 0xA0522049,
+0x675181E4, 0x4719677D, 0x667E6779, 0x7701276D,
+0x6903607C, 0x88014918, 0x25918F3E, 0x6B12D161,
+0x21B27B01, 0x660D85E3, 0x40216063, 0xC93F4021,
+0x6C034600, 0x262D322A, 0xC8016063, 0xDB5ED15D,
+0x967D8901, 0xE6002C6B, 0x666C67CD, 0x40006063,
+0x622D021D, 0x8D0E3270, 0x60436403, 0xE9FF021D,
+0x8B013290, 0x01C5A007, 0x626C7601, 0x3292E904,
+0x646C8BEB, 0x60434400, 0xD15004BD, 0x0B457401,
+0x669D6911, 0x89073670, 0x602D6211, 0x890388FF,
+0xE201DB4B, 0x2B2021C1, 0xECFC8551, 0x815120C9,
+0xCB016051, 0xDC472501, 0x64A34C0B, 0x51F366F2,
+0x85EF2612, 0x54F2D244, 0x650D420B, 0x0009ADE7,
+0xE500DC42, 0x420B2C52, 0x4E0B64A3, 0x54036403,
+0x85446E03, 0x6703E908, 0x65034918, 0x27998541,
+0xDB323790, 0x8F0BD932, 0x6013610D, 0x8B07C820,
+0xC9486053, 0x8B038808, 0xE501BD4D, 0x0009A005,
+0x2128D233, 0xBD468901, 0x64B3E500, 0x490B65E3,
+0xADBCEC01, 0x85F22DC2, 0x7001EE04, 0x31E7610D,
+0x8D0281F2, 0xADA97A08, 0x7F140009, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0xF7FF68F6,
+0x2FE68000, 0xD2234F22, 0x60E36E22, 0x8D02C840,
+0xBBF922E2, 0xE2400009, 0x2E284218, 0xBC048901,
+0x60E30009, 0x8905C810, 0xD21CD41B, 0x0009420B,
+0x0009BC03, 0xC80560E3, 0xBD6D8901, 0x60E30009,
+0x8902C802, 0xAC004F26, 0x4F266EF6, 0x6EF6000B,
+0x001C3D3C, 0x00117760, 0x002014A0, 0x0020166C,
+0x0020348C, 0x00203D9C, 0x00203900, 0x002034F4,
+0x002014CC, 0x0020396C, 0x00203974, 0x00203968,
+0x0020396A, 0x00201530, 0x002018EE, 0x0020398C,
+0x00008000, 0x001C3510, 0x00203D90, 0x002018A2,
+0x080A0C0E, 0x00020406, 0x1A1C1E20, 0x12141618,
+0x2E303234, 0x26282A2C, 0x3A3C3E40, 0x6C625648,
+0x41112F26, 0xE2208F18, 0x890B3123, 0x321CD204,
+0xD1026220, 0x412B312C, 0x00090009, 0x0020340A,
+0x002033C0, 0x000BE000, 0x400062F6, 0x40004000,
+0x40004000, 0x40004000, 0x62F6000B, 0x40004000,
+0x40004000, 0x40004000, 0x40184000, 0x62F6000B,
+0x40004000, 0x40004000, 0x40004000, 0x40284000,
+0x62F6000B, 0x40004000, 0x40184000, 0x000B4028,
+0xC90F62F6, 0x40054005, 0x40054005, 0x62F6000B,
+0x4005C907, 0x40054005, 0x62F6000B, 0x4005C903,
+0x000B4005, 0xC90162F6, 0x000B4005, 0x000062F6,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x544F0D0A,
+0x46205355, 0x00003A57, 0x206C754A, 0x32203120,
+0x20383030, 0x323A3132, 0x32313A37, 0x00000000,
+0x00000D0A, 0x00000043, 0x42707372, 0x3D206675,
+0x554E203D, 0x202C4C4C, 0x6E49677A, 0x4E497274,
+0x6D754E51, 0x0000003D, 0x61766E49, 0x2064696C,
+0x72657375, 0x20726F20, 0x2079656B, 0x00214449,
+0x52504545, 0x57204D4F, 0x65746972, 0x6461202C,
+0x003D7264, 0x6C617620, 0x0000003D, 0x00000A0D,
+0x435F4D5A, 0x465F444D, 0x4C445F57, 0x494E495F,
+0x00000054, 0x6E6B6E55, 0x206E776F, 0x6D6D6F63,
+0x3D646E61, 0x00000000, 0x203A3051, 0x00000020,
+0x203A3151, 0x00000020, 0x203A3251, 0x00000020,
+0x203A3351, 0x00000020, 0x203A3451, 0x00000020,
+0x2B434741, 0x73696F4E, 0x61432065, 0x7262696C,
+0x6F697461, 0x6166206E, 0x6F206C69, 0x6974206E,
+0x0D0A656D, 0x00000000, 0x00000072, 0x00205220,
+0x00000D0A, 0x62735576, 0x7473725F, 0x00000A0D,
+0x62735576, 0x7375735F, 0x646E6570, 0x00000A0D,
+0x62735576, 0x7365725F, 0x000A0D6D, 0x00000044,
+0x44387570, 0x72637365, 0x6F747069, 0x3D584572,
+0x00000000, 0x00000047, 0x00000042, 0x72746E49,
+0x6D652051, 0x2C797470, 0x49677A20, 0x4972746E,
+0x754E514E, 0x00003D6D, 0x654C7245, 0x0000006E,
+0x00000049, 0x20746F4E, 0x756F6E65, 0x49206867,
+0x4220514E, 0x0A0D6675, 0x00000000, 0x000000FF,
+0x00020001, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x010E010D,
+0x00020003, 0x01090108, 0x0002010A, 0x02000003,
+0x02020201, 0x02040203, 0x02060205, 0x02020200,
+0x02040203, 0x020C020B, 0x020E020D, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x000000FF,
+0x00020001, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x010E010D,
+0x00020003, 0x01090108, 0x0002010A, 0x00030003,
+0x02020201, 0x02040203, 0x02060205, 0x02020200,
+0x02040203, 0x020C020B, 0x020E020D, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x010E010D,
+0x00FF010F, 0x01090108, 0x010B010A, 0x0200010F,
+0x02020201, 0x02040203, 0x02060205, 0x02020200,
+0x02040203, 0x020C020B, 0x020E020D, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x010E010D,
+0x00FF010F, 0x01090108, 0x010B010A, 0x010F010F,
+0x02020201, 0x02040203, 0x02060205, 0x02020200,
+0x02040203, 0x020C020B, 0x020E020D, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00205220,
+0x00000046, 0x00000059, 0x73204142, 0x003D7165,
+0x49544120, 0x0000204D, 0x00000000, 0x00000000,
+0x002E0209, 0x80000101, 0x000409FA, 0x00FF0400,
+0x05070000, 0x02000201, 0x82050700, 0x00020002,
+0x03830507, 0x07010040, 0x40030405, 0x02090100,
+0x0101002E, 0x09FA8000, 0x04000004, 0x000000FF,
+0x02010507, 0x07000040, 0x40028205, 0x05070000,
+0x00400383, 0x04050701, 0x00004002, 0x00000000,
+0x00000000, 0x07090000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, };
+
+const u32_t zcFwImageSize=15928;
diff --git a/drivers/staging/otus/hal/hpfwu_BA.c b/drivers/staging/otus/hal/hpfwu_BA.c
new file mode 100644
index 000000000000..0c741571f2b5
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwu_BA.c
@@ -0,0 +1,874 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE917FFC, 0xE114D791,
+0x1E13D491, 0x1E4C470B, 0x0009B017, 0x95C2E600,
+0xC84060E2, 0x2F028F03, 0x8FF93652, 0xD48B7601,
+0x4E0BDE8B, 0xD48B0009, 0x00094E0B, 0x4E0BD48A,
+0x7F040009, 0xA0474F26, 0x4F226EF6, 0x410BD187,
+0xD4870009, 0x0009440B, 0x450BD586, 0xD7860009,
+0x611DE1FF, 0xD1852712, 0x6012E2FF, 0xCB01D484,
+0x71DC2102, 0x71042122, 0x2122E501, 0xD5812452,
+0xD2819792, 0xE7002572, 0xD481D180, 0x2270D681,
+0x2172E201, 0x26202470, 0xE4FFD67F, 0xE6002641,
+0xE104D57E, 0x6063666D, 0x626D7601, 0x32124000,
+0x05458FF8, 0xE501D27A, 0xD17A2250, 0xD57BD47A,
+0xE700E600, 0x25722470, 0x11622162, 0x11691166,
+0x4F26116A, 0x116E000B, 0xD1757FC4, 0x2F12D875,
+0xD476D175, 0xD577D676, 0x1F87D777, 0xD97778FC,
+0x1F1BD277, 0x1F417104, 0x1F647404, 0x1F887604,
+0x71F41F1C, 0x1F42E8C8, 0x1F651F53, 0x1F991F76,
+0x1F1D1F2A, 0xDD6F688C, 0xDA70DE6F, 0xDC71DB70,
+0x00094A0B, 0x00094B0B, 0x00094C0B, 0x6010D15E,
+0x8B0F8801, 0xE950D15D, 0x49186212, 0x8B073296,
+0x56FAD159, 0x2120E200, 0xCB016062, 0x2602A002,
+0x21227201, 0x880160D2, 0xD1638907, 0x32866212,
+0xD1628903, 0x88016010, 0x64E28BDA, 0x52F751F8,
+0x55E12142, 0x2252D15E, 0x661254FB, 0x246259FC,
+0x29725711, 0x880160D2, 0x66E28B53, 0x362052E1,
+0x6061894C, 0x8801C90F, 0xD1568B48, 0x36206212,
+0xA0438903, 0x27102162, 0xD5530FA0, 0x6651E710,
+0x626D7601, 0x8F3C3273, 0x65F22561, 0x695251F2,
+0x54F359F1, 0x679252F4, 0x61426512, 0x56F66922,
+0x642252F5, 0xCB206062, 0xE6002602, 0x76011F1E,
+0x626DE110, 0x32134118, 0x51FE8FF8, 0x267256F1,
+0x56F457F2, 0x55F32752, 0x251257F5, 0x27422692,
+0x51F969E2, 0x2192D43D, 0xE90161F2, 0x2192440B,
+0x491865F2, 0xD9382592, 0xE200D539, 0x62512921,
+0x720154FD, 0x622D2521, 0x2422A003, 0xE200D932,
+0xE9012921, 0x2D92D12C, 0x26686612, 0xAF6F8B01,
+0xD6300009, 0x0009460B, 0xE700D128, 0x2170AF68,
+0x001C001C, 0x00200F7C, 0x0000B38E, 0x0020322C,
+0x0020145E, 0x00203238, 0x00203250, 0x0020141C,
+0x0020151C, 0x00200FA0, 0x001C3510, 0x001C3648,
+0x001E212C, 0x00203188, 0x00202D24, 0x00203190,
+0x0020319C, 0x002031A8, 0x002031B8, 0x002031BC,
+0x002031B0, 0x00117708, 0x002031B1, 0x002031B4,
+0x001C3D30, 0x00117718, 0x00117734, 0x001C3B9C,
+0x001C3704, 0x001C3D98, 0x001C3500, 0x001C3D00,
+0x001C36F8, 0x001C1028, 0x00202D98, 0x00201328,
+0x00202C04, 0x00201E18, 0x002034BC, 0x002031BA,
+0x00202D90, 0x002031CC, 0x002031D0, 0x00201276,
+0x002031D2, 0x00201FD0, 0x2FB62F96, 0x2FD62FC6,
+0x4F222FE6, 0xDE947F8C, 0x61E0E024, 0x0F14D493,
+0x710161E3, 0xD7926210, 0x470BE028, 0xD5910F24,
+0x0009450B, 0x6D032008, 0x1F0B8F11, 0xD48FDC8E,
+0xDD8F67C0, 0x657C4D0B, 0xDD8FD18E, 0x6B9C6910,
+0x420862B3, 0x32B84208, 0x3D2C4208, 0xE0281FDB,
+0xE58004FC, 0x604C66E2, 0x3050655C, 0x2D628F13,
+0x01FCE024, 0x641CE500, 0x625DDE84, 0x8B013243,
+0x0009A33D, 0x6753655D, 0x607037EC, 0x39DC6953,
+0xAFF27501, 0x20088094, 0xE0248B13, 0xE50001FC,
+0xA009DE7A, 0x655D641C, 0x32EC6253, 0x6C536B22,
+0x3CDC67B2, 0x75041C71, 0x3243625D, 0xA31E8BF3,
+0x88012D10, 0xE0248B16, 0xE40001FC, 0x671C2D40,
+0x624DDE6E, 0x8B013273, 0x0009A311, 0x6CE3644D,
+0x7C046943, 0x39EC6B43, 0x65923BCC, 0x74086DB2,
+0x25D2AFEF, 0x8B198804, 0x01FCE024, 0x2D70E700,
+0x1FD86D1C, 0x627DDE61, 0x8B0132D3, 0x0009A2F7,
+0x6B73677D, 0x3BEC61E3, 0x710464B2, 0x3C1C6C73,
+0x694265C2, 0x29597708, 0x2492AFED, 0x8B188805,
+0x01FCE024, 0x2D40E400, 0xDE54671C, 0x3273624D,
+0xA2DC8B01, 0x644D0009, 0x6BE36D43, 0x65D23DEC,
+0x61437B04, 0x6C1231BC, 0x74086952, 0xAFED29CB,
+0x88312592, 0xDE4A8B20, 0x65E6DB4A, 0x61E6DC4A,
+0x67E2D94A, 0x62E27E04, 0x1FEC7EE8, 0x7E0464E2,
+0x6EE21FED, 0x5BFD2BE0, 0x60B27B04, 0xC9011FBE,
+0x6BB22C00, 0x29B04B09, 0xDC412F26, 0x66134C0B,
+0xE2007F04, 0x2D20A2AB, 0x8B218830, 0xD939DE38,
+0xE06465E6, 0x720462E3, 0x672666E2, 0x6E23DC36,
+0x62227EE8, 0x6BE261E6, 0x29B01FEF, 0x7E040F16,
+0xC90160E2, 0x6EE22C00, 0x4E09DC30, 0x2F262CE0,
+0xD130E068, 0x04FE410B, 0xE2007F04, 0x2D20A287,
+0x8B058833, 0x4E0BDE2C, 0xE1000009, 0x2D10A27F,
+0x89018828, 0x0009A106, 0xE143DE20, 0xE04062E1,
+0x3217622D, 0x0FE68F04, 0x6023E240, 0x262106FE,
+0x8B013217, 0x0009A0EF, 0x02FEE040, 0x8521E401,
+0x8B013046, 0x0009A0E7, 0xE501E040, 0x2D5007FE,
+0x6471B265, 0x09FEE040, 0x6291E143, 0x652DE068,
+0x8D6B3517, 0xE6400F56, 0x8B273563, 0xE048E600,
+0xE11A0F65, 0x72C0A031, 0x00117800, 0x00203254,
+0x0020145E, 0x00202588, 0x002031A2, 0x00203258,
+0x002014AA, 0x002031A1, 0x00202DC8, 0x00117804,
+0x00117810, 0x0020319D, 0x0020319E, 0x0020319F,
+0x00200C2C, 0x00200C80, 0x00200C7C, 0x41216153,
+0x41214121, 0x41214121, 0x45214521, 0x60534521,
+0x6603C903, 0x0F65E048, 0xE0077118, 0xE0442209,
+0x641D0F25, 0x65F3E04C, 0x0F46B28C, 0x04FDE048,
+0x0BFDE044, 0x61BD674D, 0x41084708, 0x0F16E050,
+0xD29B6073, 0x420B09FE, 0x6C07E00F, 0x607329C9,
+0xE0400F96, 0x65F30EFE, 0x6D0D85E2, 0x01FEE050,
+0x60D3420B, 0x6073290B, 0xE04C0F96, 0x04FEB251,
+0x06FEE040, 0x6261E068, 0x0F56652D, 0x3563E640,
+0xE000894E, 0x602381F8, 0x4008C903, 0x6B034000,
+0xE0546103, 0xE0580FB6, 0xECFFDD85, 0x6CCC0FF6,
+0x0FD6E06C, 0x4D0B60C3, 0x42216253, 0x42214221,
+0x64234221, 0x324C4200, 0xE05C6E07, 0x45214200,
+0xE0400FE6, 0x0BFE4521, 0xC9036053, 0x30FC4008,
+0x6D037B06, 0x85F81F05, 0x6C2D1FB7, 0x1FC66E03,
+0x0FC6E060, 0x05FEE058, 0x64C3B22C, 0x33FCE354,
+0x563262D2, 0x22696132, 0x67B42D22, 0x490B5936,
+0x220B607C, 0x05FEE058, 0x64C32D22, 0x7E01B201,
+0xE70662ED, 0x8FE33273, 0xE0407C01, 0x626106FE,
+0x06FEE040, 0x85614200, 0x302C760C, 0x6103701B,
+0x64F3E500, 0x7501E704, 0x6B5D6966, 0x24923B73,
+0x74048FF9, 0xB1E465F3, 0xE040641D, 0xB1A306FE,
+0xA17C6461, 0xD4570009, 0xE201D757, 0x2D20470B,
+0x0009A175, 0x8B078829, 0xEC00DE54, 0x61E22DC0,
+0x641DB175, 0x0009A16B, 0x622CE281, 0x8B013020,
+0x0009A0B6, 0x06FCE028, 0xE682626C, 0x3260666C,
+0x56FB8B20, 0x2610E124, 0x5217D149, 0x52181621,
+0x52191622, 0x521A1623, 0x551B1624, 0x1655E200,
+0x1656551C, 0x1657551D, 0x1658551E, 0x1659551F,
+0x11281127, 0x112A1129, 0x112C112B, 0x112E112D,
+0x112FA13D, 0x666CE683, 0x8B0B3260, 0xD63752FB,
+0x2250E500, 0xD2376562, 0x22527604, 0xD6366262,
+0x2620A12D, 0x666CE690, 0x8B033260, 0x0009B1C7,
+0x0009A011, 0x666CE691, 0x8B103260, 0x6252D52B,
+0x2228622C, 0xD22D8904, 0x0009420B, 0x0009A003,
+0x420BD22B, 0x56FB0009, 0xA110E200, 0xE6B02620,
+0x3260666C, 0xE0248B34, 0xE07002FC, 0x0F16612C,
+0xEB04EC00, 0x01FEE070, 0x321362CD, 0xA0FE8B01,
+0xD21A0009, 0x6DC36CCD, 0x72043D2C, 0x312C61C3,
+0x6D126ED2, 0xD114D41B, 0x0009410B, 0x410BD11A,
+0xD41A64E3, 0x420BD210, 0xD2170009, 0x64D3420B,
+0xD60DD417, 0x0009460B, 0x61E3E600, 0x316C666D,
+0x626D7601, 0x21D032B3, 0x4D198FF7, 0x7C08AFD2,
+0xD211D410, 0xD4116542, 0x0009420B, 0x0009A0CF,
+0x00202C80, 0x00203278, 0x0020145E, 0x00117804,
+0x00202D2C, 0x00203188, 0x0020319C, 0x00200CBA,
+0x00200CE0, 0x00203290, 0x002014A2, 0x002032A4,
+0x002032AC, 0x00117800, 0x002014AA, 0x002032B0,
+0xD5B5D1B4, 0x6252E040, 0x75046612, 0x2162362C,
+0x56116256, 0x1161362C, 0x62526653, 0x76085512,
+0x1152352C, 0x55136262, 0x352C76EC, 0x65631153,
+0x56146262, 0x362C7510, 0x66531164, 0x55156252,
+0x352C7610, 0x62621155, 0x362C5616, 0xD6A31166,
+0x55176262, 0x352C7604, 0x62661157, 0x352C5518,
+0x65631158, 0x56196262, 0x362C7504, 0x62561169,
+0x362C561A, 0x6256116A, 0x362C561B, 0x6653116B,
+0x551C6252, 0x352C7604, 0x6266115C, 0x352C551D,
+0x6263115D, 0x551E6662, 0x356C7204, 0x6622115E,
+0xD58F521F, 0x112F326C, 0x061E6252, 0x362C7594,
+0xE0440166, 0x62526653, 0x7644051E, 0x0156352C,
+0x6262E048, 0x362C061E, 0xD6860166, 0x6262E054,
+0x4229051E, 0x0156352C, 0x62627604, 0x061EE058,
+0x362C4229, 0x56FB0166, 0x2620E238, 0x021EE044,
+0x1621E048, 0x16226212, 0x16235211, 0xE2005512,
+0x55151654, 0x55131655, 0x55161656, 0x051E1657,
+0x1658E040, 0xE050051E, 0x55141659, 0x051E165A,
+0x165BE04C, 0xE054051E, 0x051E165C, 0x165DE058,
+0xE044051E, 0x0126165E, 0x2122E048, 0x11221121,
+0x11231125, 0x01261126, 0x0126E040, 0x1124E050,
+0xE04C0126, 0xE0540126, 0xE0580126, 0x7F740126,
+0x6EF64F26, 0x6CF66DF6, 0x000B6BF6, 0x4F2269F6,
+0xE240614D, 0x89143123, 0x3127E21F, 0x8B09D75A,
+0xD45A614D, 0xE00171E0, 0x5671440B, 0x26596507,
+0x1761A007, 0xE001D455, 0x6672440B, 0x26596507,
+0x4F262762, 0x0009000B, 0x614D4F22, 0x3123E240,
+0xE21F8912, 0xD74C3127, 0x614D8B08, 0x5671D24B,
+0x420B71E0, 0x260BE001, 0x1761A006, 0x6672D247,
+0xE001420B, 0x2762260B, 0x000B4F26, 0xE6400009,
+0x46284618, 0x6252D542, 0x89FC2268, 0x0009000B,
+0x4618E680, 0xD53E4628, 0x22686252, 0x000B89FC,
+0xA0010009, 0x7201E200, 0x8BFC3242, 0x0009000B,
+0x4618E680, 0xD5374628, 0x22686252, 0x000B8BFC,
+0x2FE60009, 0x7FFC4F22, 0xBFF16E53, 0x61E22F42,
+0xE280D631, 0x54E11615, 0x16464218, 0x422855E2,
+0x57E31657, 0x16786EF2, 0x26E22E2B, 0x4F267F04,
+0x6EF6AFCE, 0x2FD62FC6, 0x4F222FE6, 0x6C53DD26,
+0x6E43BFD6, 0x2DE2BFBB, 0x0009BFD2, 0x2C1251D5,
+0x1C4154D6, 0x1C5255D7, 0x1C6356D8, 0x6EF64F26,
+0x000B6DF6, 0x61636CF6, 0xA004E600, 0x62564109,
+0x24227601, 0x36127404, 0x000B8BF9, 0x4F220009,
+0xD117D416, 0x0009410B, 0xD417D216, 0xE5056022,
+0x2202CB20, 0xD5152452, 0x450BE700, 0xD7142472,
+0x0009470B, 0xE601D113, 0x2162D213, 0x4F264618,
+0x2262000B, 0x00202D2C, 0x001C36A0, 0x001C3CA0,
+0x001C36F4, 0x001C3B88, 0x001C3704, 0x00202C80,
+0x001C373C, 0x001C3700, 0x001C370C, 0x002032C4,
+0x0020145E, 0x001C3500, 0x001D4004, 0x002014D4,
+0x00200FA0, 0x001E212C, 0x001C3D30, 0x0009A1A9,
+0x2FE62FD6, 0xDD8F4F22, 0xA0049EA7, 0xD48E0009,
+0x420BD28E, 0x62D265D2, 0x8BF822E8, 0x0009A004,
+0xD28AD48B, 0x55D1420B, 0x22E852D1, 0xA0048BF8,
+0xD4880009, 0x420BD285, 0x52D255D2, 0x8BF822E8,
+0x0009A004, 0xD281D484, 0x55D3420B, 0x22E852D3,
+0xA0048BF8, 0xD4810009, 0x420BD27C, 0x52D455D4,
+0x8BF822E8, 0x6EF64F26, 0x6DF6000B, 0x2FD62FC6,
+0x4F222FE6, 0x6E636D73, 0x6C53B018, 0x64C357F4,
+0xB05465E3, 0xB06A66D3, 0xB09A0009, 0xB09E0009,
+0xB0A20009, 0xB0BE0009, 0xB0C10009, 0xB1240009,
+0x4F260009, 0x6DF66EF6, 0x6CF6A023, 0x3412D16C,
+0xD66C0529, 0x2650D76C, 0x2742000B, 0x0009A014,
+0x2FD62FC6, 0x4F222FE6, 0x6E636D73, 0x6C53BFEE,
+0x64C357F4, 0xB02A65E3, 0xB10666D3, 0x4F260009,
+0x6DF66EF6, 0x6CF6A005, 0xE603D260, 0x000B4618,
+0xD25E2262, 0x000BE600, 0x4F222262, 0xE40ABF7E,
+0x0009BF7E, 0xE104D25A, 0xE5004118, 0x2212E40A,
+0x2252BF74, 0x6072D757, 0x4F26CB20, 0x2702000B,
+0xD1554F22, 0xE400410B, 0x452BD554, 0x2FE64F26,
+0x6E63D153, 0x44186612, 0x45289210, 0x26294408,
+0x44084500, 0x4400265B, 0x4708264B, 0x47082162,
+0x27EBD14C, 0x000B2172, 0x03F06EF6, 0x2FE61FFF,
+0xDE494F22, 0xE40AE101, 0x2E12BF48, 0x726C62E3,
+0xE401E100, 0x22122212, 0x22122212, 0x22122212,
+0xE7302242, 0xE40AE503, 0x22122212, 0x22122212,
+0x22122212, 0x22122212, 0x22122212, 0x22122212,
+0x22522272, 0x22122212, 0x22122212, 0x22122212,
+0x22122212, 0x121ABF22, 0x2E62E600, 0x000B4F26,
+0xD2326EF6, 0xE441E101, 0x000B2212, 0xD1302242,
+0xE605D430, 0x000B2162, 0xD52F2462, 0x6050D22F,
+0x8B0E8801, 0x6040D42E, 0x8B078801, 0x9626D52D,
+0x88016050, 0x96238B0C, 0x0009A00A, 0xA0079621,
+0xE6000009, 0x2262D426, 0x88016040, 0xE6048B00,
+0xAEF3E40A, 0xD2242262, 0xE40AE601, 0x2262AEEE,
+0x2FC62FB6, 0x2FE62FD6, 0xDC204F22, 0x60C2ED00,
+0xCB01EB64, 0x60C22C02, 0xA041C901, 0x03C46E03,
+0x034003D4, 0x001C3B88, 0x002032C8, 0x002014AA,
+0x002032D0, 0x002032D8, 0x002032E0, 0x002032E8,
+0x0025E720, 0x002034B8, 0x0020318C, 0x001C5968,
+0x001D4004, 0x001C3500, 0x0020124A, 0x00201276,
+0x001C5814, 0x001C59D0, 0x001C5830, 0x001C6268,
+0x001C59A4, 0x001C639C, 0x0020319E, 0x001C5804,
+0x0020319D, 0x0020319F, 0x001C581C, 0x001C5860,
+0x89073DB2, 0xE40A60C2, 0xBE9FC901, 0x7D016E03,
+0x8BF52EE8, 0x8B033DB2, 0xD23ED43D, 0x0009420B,
+0x4F26E40A, 0x6DF66EF6, 0xAE8F6CF6, 0x44116BF6,
+0x604B8F01, 0x000B6043, 0x2FB60009, 0x2FD62FC6,
+0x4F222FE6, 0xDC347FFC, 0x60C2ED00, 0xCB02EB64,
+0x60C22C02, 0xC9022F02, 0x6E03A009, 0x89083DB3,
+0xE40A60C2, 0xC9022F02, 0x6E03BE70, 0x2EE87D01,
+0x3DB38BF4, 0xD4298B08, 0x7F04D226, 0x6EF64F26,
+0x6CF66DF6, 0x6BF6422B, 0x4F267F04, 0x6DF66EF6,
+0x000B6CF6, 0xD5226BF6, 0x60525651, 0x000B4628,
+0x2FB6306C, 0x2FD62FC6, 0x4F222FE6, 0x4F024F12,
+0x6E43BFF1, 0xDC1B6B03, 0xBFECDD1B, 0x30B80009,
+0x060A3C05, 0x46094609, 0x3D654601, 0x4209020A,
+0x42094209, 0x8BF032E2, 0x4F164F06, 0x6EF64F26,
+0x6CF66DF6, 0x6BF6000B, 0x4F222FE6, 0xE102DE0F,
+0xE403E500, 0xBFD42E12, 0xE6062E52, 0xE7004618,
+0x2E62E403, 0x4F262E72, 0x6EF6AFCB, 0x0009000B,
+0x002032F0, 0x0020145E, 0x001C5860, 0x00203308,
+0x001C1040, 0xCCCCCCCD, 0x10624DD3, 0x001D4004,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xE5007FD8, 0x6453E110, 0x6C534128, 0x655DEE0A,
+0x46086653, 0x4608365C, 0x361C7501, 0x675D6043,
+0x60C30F66, 0x37E3ED00, 0x816126C1, 0x81638162,
+0x16D316D2, 0x8FEA16D4, 0x68F27404, 0xDAB3D9B2,
+0x29821981, 0xD1B259F1, 0x2A921A91, 0x5BF35AF2,
+0x5EF55DF4, 0x11A154F6, 0x11B321A2, 0x11D511B2,
+0x11E711D4, 0x114911E6, 0x55F71148, 0xEE00DBA9,
+0xDDA957F8, 0xD6A952F9, 0x1B5164E3, 0xDBA82B52,
+0xEAB8D8A8, 0x2D72E945, 0x6AAC2622, 0x6EED4908,
+0x4D086DE3, 0x3DEC61E3, 0x4D084108, 0x3DBC31EC,
+0x410860C3, 0x81D12DC1, 0x4108E050, 0x41084008,
+0x60C381D2, 0xE500318C, 0x81D334A2, 0x1D131DD2,
+0x8D01D494, 0xD4911D54, 0xB08165D3, 0x64ED7E01,
+0x8BDC3492, 0xDB94D18D, 0xD28B6812, 0x1B814829,
+0x2FD26412, 0x2B92694D, 0xD98A6722, 0x1B734729,
+0xD7876822, 0x1BA26A8D, 0xD28C6B72, 0x22B2D586,
+0xE0035D72, 0x5E7412D2, 0x12E44018, 0xD6885176,
+0x54781216, 0x1248E1FF, 0xD4856792, 0x6852127A,
+0x28C1E703, 0x81916952, 0x6A52E050, 0x81A24008,
+0x60C36B52, 0x6D5281B3, 0x6E521DD2, 0x62521E63,
+0x1264E600, 0x46086563, 0x7501364C, 0x665D2612,
+0x8BF83673, 0xE003D471, 0x40186542, 0x674225C1,
+0x8171D274, 0xEE006842, 0x69421882, 0x1923E024,
+0xE5806A42, 0x6B421AE4, 0x81B266E3, 0xD46D6C42,
+0x655C81C3, 0x6D63666D, 0x616D7604, 0x31533D4C,
+0x2DE28FF8, 0xD569D268, 0x74042422, 0x7F282452,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0x664268F6, 0xC8036061, 0xE5008D04, 0xC9036061,
+0x8B038802, 0x65635262, 0x24125124, 0x6053000B,
+0x2FE62FD6, 0x7FEC4F22, 0x62536E53, 0x6D43E550,
+0x4508E400, 0xE101A001, 0x60435224, 0x81212211,
+0x60538123, 0x56E28122, 0x8BF53620, 0x16E4D250,
+0xE61464F3, 0x65E3420B, 0xE4FC65E1, 0x2E512549,
+0x65F361F1, 0x2F112149, 0xD14954D1, 0xE614410B,
+0x607157D1, 0x2701CB01, 0x7F141DE1, 0x6EF64F26,
+0x6DF6000B, 0x2FE62FD6, 0x7FEC4F22, 0x66536E53,
+0x6D43E5FC, 0x20596061, 0x2601CB01, 0x326052E2,
+0x12E48B06, 0x31E051E2, 0x52D18B04, 0x1E22A002,
+0x5664AFF0, 0x64F3D236, 0x420BE614, 0x67E165E3,
+0x2719E1FC, 0x67F12E71, 0x271954D1, 0x65F3D130,
+0x410BE614, 0x52D12F71, 0xCB016021, 0x1DE12201,
+0x4F267F14, 0x000B6EF6, 0x2FE66DF6, 0x624C4F22,
+0x4208DE1B, 0xA0054200, 0x52523E2C, 0x5624D417,
+0x2E62BF8E, 0x52E165E2, 0x8BF63520, 0x2622D61B,
+0x000B4F26, 0x2FB66EF6, 0x2FD62FC6, 0x4F222FE6,
+0xDB1CDC10, 0x66C252C1, 0x89403620, 0xC9036061,
+0x893C8801, 0xDD18DE0B, 0x64E3BF63, 0x85036503,
+0x620D66B2, 0x892B3262, 0xBF9BD403, 0xD4130009,
+0x00094D0B, 0x0009AFE6, 0x00202D88, 0x00202D90,
+0x00202D98, 0x00202DC0, 0x002031A4, 0x002031AC,
+0x001000C8, 0x00101680, 0x001E2108, 0x001C3D00,
+0x00117880, 0x00117780, 0x00040020, 0x0026C401,
+0x00200B26, 0x00203188, 0x0020145E, 0x00203324,
+0x64E3BF3E, 0x4D0BD406, 0xAFBB0009, 0xD2050009,
+0x4F262262, 0x6DF66EF6, 0x000B6CF6, 0x00006BF6,
+0x00203328, 0x001C3D28, 0x2FC62FB6, 0x2FE62FD6,
+0x7FFC4F22, 0x6022D22B, 0x8D41C803, 0xDE2A2F01,
+0xDB2BDC2A, 0xED01A017, 0xC9036051, 0x89168801,
+0xD128D426, 0x0009410B, 0x61035503, 0xC8208551,
+0xE0508903, 0x720102BE, 0xD2230B26, 0x420B64E3,
+0xD6226513, 0x52C126D2, 0x352065C2, 0xDE208BE4,
+0xDB21DD20, 0x52D1DC21, 0x352065D2, 0x60518918,
+0x8801C903, 0xD41B8914, 0x460BD616, 0x57030009,
+0x8F0437E0, 0xE2016503, 0xAFEC2B20, 0xD4182C52,
+0x420BD218, 0xD6110009, 0x4118E101, 0x2612AFE3,
+0xC80460F1, 0xD2148907, 0x4F267F04, 0x6DF66EF6,
+0x422B6CF6, 0x7F046BF6, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6000B, 0x001E2100, 0x00202D98, 0x00202D90,
+0x00202D2C, 0x00201162, 0x002011E4, 0x001C3D30,
+0x00117880, 0x00202D88, 0x002031A8, 0x002031A4,
+0x00202DC0, 0x00201180, 0x00200308, 0xE601D203,
+0x1265D503, 0x000B2252, 0x00001266, 0x001C1010,
+0x0000C34F, 0x0009000B, 0x2FD62FC6, 0x4F222FE6,
+0x6D436C53, 0xEE00A004, 0x7E0164D4, 0x644CBFF2,
+0x8BF93EC2, 0x6EF64F26, 0x000B6DF6, 0xE5006CF6,
+0x6643A002, 0x76017501, 0x22286260, 0xAFE38BFA,
+0x2FE60009, 0x75076253, 0xE1086753, 0x6043EE0A,
+0x4409C90F, 0x650330E2, 0x8D014409, 0xE630E637,
+0x4110365C, 0x8FF22760, 0xE00077FF, 0x000B8028,
+0x000B6EF6, 0x000BE000, 0x2FE6E000, 0x7FEC4F22,
+0x6E436253, 0xBFDC65F3, 0xBFD06423, 0xBFCE64E3,
+0xD40364F3, 0x0009BFCB, 0x4F267F14, 0x6EF6000B,
+0x0020332C, 0xE4FDD29A, 0xD79A6122, 0x22122149,
+0x74016022, 0x2202CB01, 0xD5976622, 0x22622649,
+0xC8406070, 0x60528902, 0x2502CB04, 0xE1F76452,
+0x25422419, 0xE7016052, 0x2502C9CF, 0xE6026052,
+0x2502CB03, 0x15624718, 0x1573000B, 0xD78CD58B,
+0xD48DD28C, 0xE600E100, 0x27112511, 0xAFD12210,
+0x664C2461, 0x4600D289, 0x6060362C, 0x000BCB10,
+0x654C2600, 0x4500D285, 0x6650352C, 0x2619E1EF,
+0x2560000B, 0xD282664C, 0x362C4600, 0xCB106060,
+0x2600000B, 0xD27E654C, 0x352C4500, 0xE1EF6650,
+0x000B2619, 0x664C2560, 0x4600D278, 0x6060362C,
+0x000BCB08, 0x654C2600, 0x4500D274, 0x6650352C,
+0x2619E1F7, 0x2560000B, 0xD271664C, 0x362C4600,
+0xCB086060, 0x2600000B, 0xD26D654C, 0x352C4500,
+0xE1F76650, 0x000B2619, 0x624C2560, 0x4200D667,
+0x6020326C, 0x4021C908, 0x40214021, 0x600C000B,
+0xD663624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0xD15F600C, 0x341C644C, 0x000B6240,
+0xD15D602C, 0x341C644C, 0x000B6240, 0x2FE6602C,
+0x6E434F22, 0xE60A645C, 0x89143467, 0x0009BFEB,
+0x60EC640C, 0x8B028801, 0xA002E00F, 0x44092409,
+0x624C4409, 0x3263E60A, 0xBFE28905, 0x620C644C,
+0xC8806023, 0xE2008B00, 0x4F266023, 0x6EF6000B,
+0xD64A4F22, 0x88016062, 0xB2458B03, 0xA0030009,
+0xD2470009, 0x2260E640, 0xE200D646, 0x000B4F26,
+0x4F222622, 0x6062D641, 0x8B018802, 0x0009B28E,
+0xE200D640, 0x000B4F26, 0xD53C2622, 0xE100D43C,
+0x2512E701, 0x2470000B, 0xE604D239, 0x2260000B,
+0xD4394F22, 0x410BD139, 0xD5390009, 0x6650E1FD,
+0x2619D238, 0x2560E700, 0x000B4F26, 0x4F222270,
+0xD132D435, 0x0009410B, 0xE7FBD531, 0x26796650,
+0x000B4F26, 0x4F222560, 0xD12CD430, 0x0009410B,
+0xE7F7D52B, 0x26796650, 0x000B4F26, 0xD5282560,
+0x6250942D, 0x000B2249, 0xD5252520, 0x6250E4BF,
+0x000B2249, 0x4F222520, 0x8522D225, 0x2008600D,
+0x88018911, 0x88038913, 0x88058915, 0x88068942,
+0x88088948, 0x8809894E, 0x880A8954, 0x880B895A,
+0xA0678960, 0xB0690009, 0xA0640009, 0xB077600C,
+0xA0600009, 0xB080600C, 0xA05C0009, 0xFF7F600C,
+0x001E2148, 0x001E1000, 0x001E1108, 0x002031FC,
+0x002031FE, 0x0020321D, 0x002031E0, 0x001E103F,
+0x001E105F, 0x001E102F, 0x001E1090, 0x00203204,
+0x001E100B, 0x00203200, 0x00203330, 0x0020145E,
+0x001E1028, 0x0020321C, 0x0020333C, 0x0020334C,
+0x002031D4, 0x6260D684, 0x8B2B2228, 0x0009B061,
+0x600CA029, 0x6260D680, 0x8B232228, 0x0009B069,
+0x600CA021, 0x6260D67C, 0x8B1B2228, 0x0009B0C7,
+0x600CA019, 0x6260D678, 0x8B132228, 0x0009B0CD,
+0x600CA011, 0x6260D674, 0x8B0B2228, 0x0009B125,
+0x600CA009, 0x6260D670, 0x8B032228, 0x0009B13D,
+0x600CA001, 0x4F26E000, 0x0009000B, 0xD26CD16B,
+0xD56C8412, 0x4000C90F, 0xD76B012D, 0xE403D66B,
+0xE20F611C, 0x2540E001, 0x25202712, 0x2602000B,
+0xE601D262, 0x30668523, 0xE0008D05, 0xD663D260,
+0xE0018122, 0x000B2602, 0xD25C0009, 0x600D8523,
+0x89052008, 0x8B0A8801, 0x6060D65D, 0x2600CB01,
+0xD457D65A, 0xE001E101, 0x000B2612, 0x000B8142,
+0xD152E000, 0x8513E501, 0x640D4518, 0x66033453,
+0xE0008D05, 0xD551D253, 0x2260E001, 0x000B2502,
+0x4F220009, 0x8513D149, 0x6453650D, 0x62494419,
+0x227D672E, 0x8801602C, 0x88028909, 0x88038910,
+0x8806891A, 0x88078935, 0xA04C893B, 0xD5460009,
+0x6652D746, 0x2762D446, 0x622C6261, 0x2421A038,
+0x2228625C, 0xD4438B3F, 0x6642D540, 0x2562D440,
+0x24018561, 0x6203A02C, 0x2008605C, 0x88108907,
+0x88208908, 0x88308909, 0xA02C890A, 0xD23A0009,
+0x6222A008, 0xA005D239, 0xD2396222, 0x6222A002,
+0x6262D638, 0xD432D531, 0x66212522, 0xA00F626C,
+0xD6352421, 0x6261D52D, 0x622CD42D, 0xA0072562,
+0xD6322421, 0x8561D529, 0x2562D429, 0x62032401,
+0x662D8515, 0x3617610D, 0x65038F01, 0xB0CB2451,
+0xA0010009, 0xE000E001, 0x000B4F26, 0xD6190009,
+0xD427E101, 0x65412610, 0xD118D717, 0xE20F655D,
+0x2752E001, 0x000B2620, 0x2FE62102, 0xD20F4F22,
+0x640C8523, 0x8B082448, 0xD511D61D, 0x2621E200,
+0x940F8451, 0xA0482049, 0xDE0D8051, 0xC84060E0,
+0xE2018D32, 0x89443427, 0xD216D615, 0x2641420B,
+0x0009A030, 0x0000FF7F, 0x0020321D, 0x002031D4,
+0x002031E0, 0x001E1100, 0x001E100C, 0x00203200,
+0x001E1000, 0x001E1001, 0x00203208, 0x002031E8,
+0x002031EC, 0x002031F0, 0x0020320C, 0x00203210,
+0x00203214, 0x00203218, 0x0020351C, 0x00203526,
+0x002031FA, 0x00202362, 0x89123427, 0xD294D693,
+0x2641420B, 0xCB8084E1, 0x80E1B0F5, 0xD69160E0,
+0x2E00CB04, 0xC93F6060, 0xD68F2600, 0xA001E001,
+0xE0002602, 0x000B4F26, 0xD68C6EF6, 0xC8806060,
+0xD2868919, 0x88016021, 0xD2898B15, 0x8524E501,
+0x89103056, 0xE203D187, 0x2120D487, 0xE00B6541,
+0x0656655D, 0xE40FD585, 0x2140E702, 0xD77E2571,
+0x000BE001, 0x000B2702, 0x2FE6E000, 0xDE804F22,
+0xC88084E1, 0xD57A892C, 0x20088554, 0x61038F28,
+0x8553D77C, 0x64036672, 0x8566650C, 0x3520620C,
+0xD6798B1E, 0x651CD774, 0x2651644C, 0x60E02741,
+0x8904C840, 0x420BD275, 0xA0030009, 0xD2680009,
+0x0009420B, 0x0009B09F, 0xE201D167, 0x60E02122,
+0xCB04D464, 0x60402E00, 0x2400C93F, 0x6023A001,
+0x4F26E000, 0x6EF6000B, 0x2FB62FA6, 0x2FD62FC6,
+0xDA622FE6, 0x66A1E240, 0x3622DC5E, 0x62638900,
+0x6ED36D2C, 0x4E2136D8, 0x4E212A61, 0xDB61D460,
+0xE700A00F, 0x770162B2, 0x71026123, 0x66212B12,
+0x71026213, 0x61212B12, 0x651D666D, 0x356C4528,
+0x627C2452, 0x8BED32E3, 0xC90360D3, 0x8B108803,
+0x617367B2, 0x2B127102, 0x71026E13, 0x2B126571,
+0x655D6DE1, 0x422862DD, 0x325CE107, 0xA00C2C10,
+0x88022422, 0xA0038B01, 0x8801E203, 0xE2018B05,
+0x66B22C20, 0x655D6561, 0xE60F2452, 0x67A12C60,
+0x8B052778, 0xDD38DC44, 0xEB01EA00, 0x2DB22CA2,
+0x6DF66EF6, 0x6BF66CF6, 0x6AF6000B, 0x2FE62FD6,
+0xE240DD36, 0x362266D1, 0x62638900, 0x3678672C,
+0x7703DE38, 0x47212D61, 0x64E2D635, 0xA00E4721,
+0x6562E100, 0x62537101, 0x74012450, 0x24204219,
+0x45297401, 0x74012450, 0x24504519, 0x621C7401,
+0x8BEE3273, 0x66E24200, 0x420061D1, 0x2118362C,
+0x2E628F06, 0xDD1CD728, 0xE501E400, 0x2D522742,
+0x000B6EF6, 0x2FD66DF6, 0x4F222FE6, 0xED0AEE01,
+0x64E3BC97, 0xBC9C64E3, 0x62EC7E01, 0x8BF732D7,
+0xBC9FEE01, 0x64E364E3, 0x7E01BCA4, 0x32D762EC,
+0x4F268BF7, 0x000B6EF6, 0xD1186DF6, 0xD418920D,
+0x72122122, 0x2422D617, 0xD7177204, 0x72202622,
+0x2722D116, 0x000B7230, 0x137A2122, 0x002031FA,
+0x0020246E, 0x001E1015, 0x00203200, 0x001E1001,
+0x002031D4, 0x001E1100, 0x002031FE, 0x002031EC,
+0x001E1000, 0x002031F0, 0x002031FC, 0x00202362,
+0x001E100C, 0x002031E8, 0x00203204, 0x00203208,
+0x0020320C, 0x00203210, 0x00203214, 0x00203218,
+0x4F222FE6, 0xD6507FFC, 0x88016060, 0xE2018951,
+0x2620BFBB, 0xD54ED14D, 0xDE4E6010, 0x64E36552,
+0x7402C840, 0x8D22D14C, 0xD24C7502, 0xE601D74C,
+0xE7042722, 0x76016255, 0x626C2421, 0x8FF93273,
+0xD4437402, 0x6242E601, 0x640D8528, 0x67494419,
+0x275D657E, 0x81E4607C, 0xE417D542, 0x67557601,
+0x3243626C, 0x8FF92171, 0xA0207102, 0xD23E0009,
+0xE601D73B, 0xE7042722, 0x76016255, 0x626C2421,
+0x8FF93273, 0xD4327402, 0x6242E601, 0x640D8528,
+0x67494419, 0x275D657E, 0x81E4607C, 0xE417D533,
+0x67557601, 0x3243626C, 0x8FF92171, 0x924A7102,
+0xD2262E21, 0x5E23D72E, 0x64F22FE2, 0x604365F2,
+0x2700C980, 0xC9606043, 0x80716103, 0xC9036043,
+0x80724519, 0x65F2605C, 0x817266F2, 0x46194629,
+0x606C4529, 0x4018645C, 0x8173304C, 0x21185E23,
+0x64F22FE2, 0x6E4C62F2, 0x602C4219, 0x66F262F2,
+0x46294018, 0x461930EC, 0x42298174, 0x652C606C,
+0x305C4018, 0x81758F07, 0x0009BC9D, 0x2228620C,
+0xA00A8908, 0x60130009, 0x8B038840, 0x0009B009,
+0x0009A003, 0xE202D60F, 0x7F042622, 0x000B4F26,
+0x000B6EF6, 0x060A0009, 0x0020321C, 0x001E1000,
+0x00203208, 0x0020351C, 0x00203528, 0x002034C0,
+0x002031F0, 0x002034F0, 0x002034EE, 0x002034C2,
+0x002031D4, 0x00203200, 0x4F222FE6, 0xDE937FFC,
+0x200884E9, 0x2F008D06, 0xD692D491, 0x0009460B,
+0x64F0B194, 0x6620D290, 0x89022668, 0xC9BF60E0,
+0x7F042E00, 0x000B4F26, 0x000B6EF6, 0x2FE60009,
+0xDE8A4F22, 0x60E0D68A, 0xCBC0D48A, 0x62602E00,
+0xC803602C, 0x40218904, 0x70014021, 0x6603A002,
+0x66034009, 0xD684616D, 0xE500A004, 0x75016262,
+0x74042422, 0x3213625D, 0xD2808BF8, 0x0009420B,
+0xC9BF84E2, 0x4F2680E2, 0x6EF6000B, 0x2FE62FD6,
+0x7FFC4F22, 0x6260D67A, 0x89442228, 0xD56FE100,
+0x60502610, 0xCB40D477, 0x2500440B, 0x8D052008,
+0x62E06E03, 0x7104612C, 0x2F11A006, 0xD472D66A,
+0xDD726760, 0x657C4D0B, 0xE23C6D1D, 0x8B033D27,
+0xD264D46F, 0x0009420B, 0x4D214D21, 0xA005D76D,
+0x66E6E400, 0x357C4508, 0x74012562, 0x35D3654D,
+0xD7698BF7, 0x6172E003, 0x81114018, 0x6E7260F1,
+0x81E2700C, 0xD4656172, 0xDD658113, 0x4D0BDE65,
+0xE2016572, 0xD4642E22, 0x420BD252, 0xD6530009,
+0xC93F6060, 0x7F042600, 0x6EF64F26, 0x6DF6000B,
+0x2FC62FB6, 0x2FE62FD6, 0xD25C4F22, 0x6B436E73,
+0x420B6C53, 0x20086D63, 0x61038F08, 0xD245D458,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6422B, 0x21B060C3,
+0x60D38011, 0xE5008111, 0x64BCA007, 0x6053655D,
+0x665300EC, 0x7501361C, 0x625D8064, 0x8BF53243,
+0x6060D636, 0x2600C9BF, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6000B, 0x7FC44F22, 0x720262F3, 0x22512F41,
+0x45297202, 0x60632251, 0xE5C4E682, 0x67F38121,
+0x655C666C, 0xE408BFBC, 0x4F267F3C, 0x0009000B,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xE1007FC4, 0x6513ECFF, 0x6B136CCD, 0xDE36D735,
+0xEDFF64F3, 0xD835EA04, 0x6053655C, 0x027D4000,
+0x32C0622D, 0x66038D0D, 0x09ED6063, 0x2491027D,
+0x24217402, 0x698202ED, 0x3928622D, 0x74022892,
+0x75017104, 0x6063625C, 0x07D532A2, 0x0EB58FE4,
+0x2448641C, 0xE6808905, 0x67F3E5C5, 0xBF7F666C,
+0x7F3C655C, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0xD11E68F6, 0x6012D21E, 0xCB20E405,
+0x2102E500, 0x000B2242, 0x00002252, 0x001E1017,
+0x00203358, 0x0020145E, 0x002031A2, 0x001E1015,
+0x001E10BF, 0x00117800, 0x001E10FC, 0x00200308,
+0x002031A8, 0x002025C6, 0x0020335C, 0x002014AA,
+0x00203378, 0x0011788C, 0x002031A4, 0x00202D88,
+0x002011E4, 0x001E2130, 0x00203380, 0x00202588,
+0x00203384, 0x002031BC, 0x002031C4, 0x002034BC,
+0x001C3500, 0x001D4004, 0xD565D164, 0xE400D765,
+0x2142E20F, 0x17411154, 0xD5632722, 0x9669D763,
+0x15412572, 0x96661562, 0xE6011565, 0xD5601165,
+0x666CE6F8, 0x25422542, 0x25422542, 0x25422542,
+0x25622542, 0x7601E727, 0x67632572, 0x25627797,
+0xE7042572, 0x2572E248, 0xE2192522, 0xE2702522,
+0x25422542, 0x25422542, 0x25222542, 0x2522E20C,
+0x25422542, 0x25422542, 0x25422542, 0x25422542,
+0x000B154A, 0xE2081145, 0x0009422B, 0x2FE62FD6,
+0x7FFC4F22, 0xC8206043, 0x6E438D02, 0x0009BE6D,
+0xC81060E3, 0xBE6A8901, 0x60E30009, 0x8901C840,
+0x0009BE8C, 0xC80160E3, 0xDD3E8938, 0xC80260D0,
+0x2F008D03, 0x460BD63C, 0x60F00009, 0x8902C804,
+0x460BD63A, 0x62F00009, 0xC8806023, 0x60D08902,
+0x2D00C97F, 0xC8016023, 0xD6358906, 0x0009460B,
+0x0009A007, 0x51630601, 0x8902C808, 0x460BD631,
+0x60F00009, 0x8902C810, 0x420BD22F, 0xD52F0009,
+0x88026052, 0xD22E8B03, 0xA005E604, 0x88012260,
+0xD22B8B02, 0x2260E601, 0x2522E200, 0xC88060E3,
+0xD628892E, 0x60E36E60, 0x8902C880, 0x420BD226,
+0x60E30009, 0x8902C840, 0x420BD224, 0x60E30009,
+0x8902C802, 0x420BD222, 0x60E30009, 0x890EC804,
+0x410BD120, 0xBF0E0009, 0xBF4D0009, 0xD51E0009,
+0x6050D41E, 0xC908D71E, 0xBF842500, 0x60E32472,
+0x8905C808, 0x7F04D21B, 0x6EF64F26, 0x6DF6422B,
+0x4F267F04, 0x000B6EF6, 0x00006DF6, 0x001C581C,
+0xA000A000, 0x001D0100, 0x001D4000, 0x00040021,
+0x001C589C, 0x001E1021, 0x00201640, 0x00201662,
+0x00201CA0, 0x0020167A, 0x00201688, 0x00203200,
+0x001E100B, 0x001E1028, 0x002016DE, 0x002016EA,
+0x00201690, 0x002016AE, 0x001E1000, 0x0010F100,
+0x12345678, 0x002016C6, 0x644CD6A7, 0x000B346C,
+0xD6A62450, 0x346C644C, 0x2450000B, 0x644CD6A4,
+0x000B346C, 0x625C2450, 0x4208616D, 0x42084119,
+0x42006019, 0x670E614C, 0xD49E321C, 0x4200207D,
+0x324CC90F, 0x2200000B, 0x4208625C, 0x42004208,
+0x324C644C, 0x4200D498, 0x000B324C, 0x2FE62260,
+0x614C4F12, 0x4100D493, 0x6710314C, 0xE29F666D,
+0x27294619, 0x6E536269, 0x672E6573, 0x4221227D,
+0x42214221, 0x7601662C, 0xE4014608, 0x34E84608,
+0x644C4600, 0x071A0467, 0x2150257B, 0x000B4F16,
+0x4F226EF6, 0xD2857FE8, 0x88016021, 0xD2848B7B,
+0x26686621, 0xD2838B77, 0x26686621, 0xE50F8B73,
+0xE401BFA2, 0xBFA4E501, 0xE586E400, 0xE400655C,
+0x2F50BFA4, 0xBFA1E401, 0xE602E506, 0x60634618,
+0x81F2E401, 0x6543BF9F, 0xE40185F2, 0xBFAB6543,
+0x85F26603, 0x6543E401, 0x6603BFB1, 0xE40265F0,
+0x6053756C, 0x80F8BF80, 0xBF82E402, 0x84F8E512,
+0x7090E402, 0x6503BF82, 0x4618E602, 0x81F66063,
+0xBF80E402, 0x85F6E500, 0x6603E402, 0xE500BF8C,
+0xE40285F6, 0xBF926603, 0xE5FEE500, 0xE010655C,
+0xBF61E403, 0xE5130F54, 0xE40EBF63, 0x05FCE010,
+0xBF63E40E, 0xE5007585, 0xBF64E403, 0xE500E640,
+0xBF71E403, 0xE500E640, 0xBF78E403, 0xE5FFE640,
+0xE014655C, 0xBF47E404, 0xE40F0F54, 0xE504BF49,
+0x05FCE014, 0xBF49E40F, 0xE5017584, 0xBF4AE640,
+0xE501E404, 0xBF57E640, 0xE501E404, 0xE404E640,
+0xAF5C7F18, 0x7F184F26, 0x000B4F26, 0x4F220009,
+0xD2427FF0, 0x88016021, 0xD2418B71, 0x26686621,
+0xD2408B6D, 0x26686621, 0xE50F8B69, 0xE401BF1C,
+0xBF1EE501, 0xE586E400, 0xE400655C, 0x2F50BF1E,
+0xBF1BE401, 0xE401E506, 0xBF1C6543, 0xE401E640,
+0xBF296543, 0xE401E640, 0xBF306543, 0x65F0E640,
+0x756CE402, 0xBEFF6053, 0xE40280F4, 0xE512BF01,
+0xE40284F4, 0xBF017090, 0xE6406503, 0xBF02E402,
+0xE640E500, 0xBF0FE402, 0xE640E500, 0xBF16E402,
+0xE5FEE500, 0x6053655C, 0xBEE5E403, 0xE51380F8,
+0xE40EBEE7, 0xE40E84F8, 0xBEE77085, 0xE5006503,
+0xBEE8E640, 0xE500E403, 0xBEF5E640, 0xE500E403,
+0xBEFCE640, 0xE5FFE403, 0x6053655C, 0xBECBE404,
+0xE40F80FC, 0xE504BECD, 0xE40F84FC, 0xBECD7083,
+0xE5016503, 0xBECEE640, 0xE501E404, 0xBEDBE640,
+0xE501E404, 0xE404E640, 0xAEE07F10, 0x7F104F26,
+0x000B4F26, 0x00000009, 0x001E102F, 0x001E1080,
+0x001E1090, 0x001E103F, 0x001E103E, 0x002031FA,
+0x002031FC, 0x002031FE, 0xD21DD11C, 0x66206010,
+0x676C7001, 0x3700C90F, 0xE5008D13, 0x67106210,
+0x7701622C, 0x64232170, 0xD6166010, 0x44084408,
+0x3428C90F, 0x62602100, 0x7201D513, 0x44082620,
+0x000B354C, 0xD10F6053, 0x25586510, 0xE6008D13,
+0xD60DD40B, 0x655C6540, 0x47086753, 0x37584708,
+0x47086540, 0x24507501, 0x367C6040, 0x2400C90F,
+0x72FF6210, 0x000B2120, 0x00006063, 0x002031A1,
+0x002031A0, 0x002031A2, 0x00202DC8, 0x7FFC4F22,
+0xE680D19D, 0x666C6212, 0xD29C2F22, 0x67F36563,
+0x420B7542, 0x7F04E404, 0x000B4F26, 0xE6800009,
+0xD296666C, 0xE7006563, 0x422B7540, 0xE6806473,
+0xD292666C, 0xE7006563, 0x422B7543, 0x2F866473,
+0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6, 0x7FF44F22,
+0xDD8CD28B, 0x72011F21, 0xDB8B1F22, 0x6AF2E840,
+0x5211D18A, 0x36206612, 0xA0A78B01, 0x60610009,
+0x8801C903, 0xA0A18B01, 0xD9840009, 0x420BD284,
+0x55036493, 0x845C6A03, 0x30E0EE84, 0xD1818B79,
+0x606C6610, 0x8B3D8801, 0x6210D17F, 0x892F2228,
+0xD57EE701, 0x64522B72, 0x1442E003, 0xD57C6252,
+0xE6004018, 0x21608121, 0xD17A6453, 0x6E527404,
+0x60126742, 0xCB20DC78, 0x76012102, 0x3283626D,
+0x25E28BFB, 0x2472DE71, 0x62E267C2, 0x1274D173,
+0x604164E2, 0x2401CB01, 0xEE0066E2, 0xDC702C62,
+0xEC012C62, 0x2DC2410B, 0x4C18EC01, 0x2BE22DC2,
+0xD764DE6C, 0xD16C60E2, 0xCB01E202, 0x27202E02,
+0x2122A02F, 0x8B2C2008, 0xE701DE68, 0xD466EC00,
+0x2170D264, 0xEE012EC2, 0x612224E2, 0x2169E6FE,
+0xE01E2212, 0x54F10C5C, 0x24C0E01F, 0x56F2025C,
+0x26207510, 0xD75EE600, 0xEE06D45E, 0x76018456,
+0x6C542700, 0x31E3616C, 0x740124C0, 0x77018FF6,
+0xE494D259, 0x72012240, 0x2250E500, 0xE605720F,
+0xD2562260, 0x65A36493, 0xEE01420B, 0xAF6F4E18,
+0x2FA22DE2, 0xD45265F2, 0x66428553, 0x3262620D,
+0xD4508907, 0x410BD150, 0xD7500009, 0xAF57E601,
+0xD43A2762, 0xDD37D149, 0x65F2410B, 0xD44CEE01,
+0x4E18D64C, 0x2DE2460B, 0x0009AF4A, 0x7F0C2FA2,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0x4F2268F6, 0x85467FF4, 0x2F01E681, 0x666C8547,
+0x854881F1, 0x81F2D225, 0x67F38542, 0x854381F3,
+0x81F4E40C, 0x65636053, 0x420B81F5, 0x7F0C7540,
+0x000B4F26, 0x2F860009, 0x2FA62F96, 0x2FC62FB6,
+0x2FE62FD6, 0x7FEC4F22, 0xE800D11A, 0xD4322F12,
+0x1F416183, 0x6A13DB20, 0x4A08D630, 0xDE20E001,
+0x4A00460B, 0x1F023AEC, 0x52B166B2, 0x8B013620,
+0x0009A19B, 0xC9036061, 0x8B018801, 0x0009A195,
+0xDE275263, 0x8B4F32E0, 0x420BD20D, 0xDE2564B3,
+0xD70DD50E, 0xED01DC0B, 0x2E02E100, 0x27D02502,
+0xAFE12C10, 0x00002E16, 0x001C3D9C, 0x00201F40,
+0x0011779A, 0x001C3D30, 0x001D0104, 0x00202DC0,
+0x00201162, 0x002031B1, 0x002031B0, 0x002031AC,
+0x001C3B9C, 0x001C3500, 0x00202D98, 0x00201276,
+0x001C3D00, 0x001C36F8, 0x00117708, 0x002031B4,
+0x0011778C, 0x00117792, 0x00117788, 0x00201180,
+0x00203188, 0x00202D88, 0x002011E4, 0x001E2130,
+0x0020349C, 0x0020145E, 0x002034A8, 0x00202C80,
+0x00117780, 0x0011770C, 0xC8018561, 0x5C63897A,
+0x660385C2, 0x6403C903, 0x650D85C3, 0x40216053,
+0xC93F4021, 0x6E034500, 0x252D322A, 0xE2106053,
+0x3E23C901, 0x6D038D23, 0x4408D79D, 0x44086570,
+0x440062E3, 0x25584200, 0x342C8F0F, 0x6043D299,
+0x697D072D, 0x60994919, 0x201D610E, 0x60D381F6,
+0x8F0C8801, 0xA00A697C, 0xD29369E3, 0x052D6043,
+0x4219625D, 0x670E6029, 0x81F6207D, 0xD18F695C,
+0x22286210, 0xE9FF8901, 0xEEFF699C, 0x6EEC659D,
+0x8B0F35E0, 0x4C0BDC8A, 0x540364B3, 0xBF20E502,
+0xD4886E03, 0x410BD188, 0xD78865E3, 0xD488ED01,
+0x27D2A01E, 0x26E9EEFC, 0x81C26063, 0x97C585C3,
+0x62032079, 0x450885F6, 0x6063260B, 0x81C2252B,
+0x81C36053, 0xE10885C4, 0x201B4118, 0x62B281C4,
+0x20E98521, 0x64B28121, 0xCB016041, 0xD4792401,
+0x450BD579, 0x60B20009, 0x57F266F2, 0x2A02CB01,
+0x2672AF22, 0xD26E8561, 0x8F02C802, 0xA09F64B3,
+0x420B0009, 0xDC710009, 0x5E036503, 0x07CEE04C,
+0x7701DD6F, 0x6CD20C76, 0x7C01D664, 0x6D602DC2,
+0x89062DD8, 0xD264D463, 0xED01420B, 0xA07ED763,
+0x625127D2, 0x4118E10F, 0x2219E402, 0x32404418,
+0x85518B11, 0x20D9EDFC, 0x60518151, 0xCB017DE3,
+0x85E12501, 0x20D9D65F, 0x460B81E1, 0x6CF264B3,
+0xA06457F2, 0x6D512C72, 0x4D196DDD, 0x66DE6DD9,
+0x7D012D6D, 0x610360DC, 0x88014118, 0x25118F45,
+0x6462D653, 0x26427401, 0x660D85E3, 0x40216063,
+0xC93F4021, 0x6D034600, 0x262D322A, 0xC8016063,
+0xDC4ED14D, 0x964A8901, 0xE6002D6B, 0x0F64E010,
+0xE01064DD, 0x607C07FC, 0x021D4000, 0x3240622D,
+0x66038D12, 0x021D6063, 0x3270E7FF, 0xA00B8B01,
+0xE01001D5, 0xE60402FC, 0x0F247201, 0x3262622C,
+0x06FC8BE7, 0x4600666C, 0x01CD6063, 0x0C157101,
+0x6711D13B, 0x3C406C7D, 0x62118907, 0x88FF602D,
+0x21D18903, 0xE201DD37, 0x85512D20, 0x20D9EDFC,
+0x60518151, 0xCB01D22F, 0x420B64B3, 0xE0102501,
+0xD43102FC, 0xE001612C, 0x67F2440B, 0x85EF2702,
+0x54F1D22E, 0x650D420B, 0x0009AE7E, 0x80007E03,
+0x0009420B, 0x6E035403, 0xED088544, 0x20D94D18,
+0x8B0330D0, 0xE501BE3D, 0x0009A007, 0xDD248541,
+0x22D8620D, 0xBE348901, 0xD412E500, 0x420BD212,
+0xD71265E3, 0xAE5FED01, 0x780127D2, 0xEE04618D,
+0x8D0231E7, 0xAE4E7B08, 0x7F140009, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0x000068F6,
+0x002034B8, 0x0020339C, 0x0020341C, 0x0020319C,
+0x00201162, 0x00202D90, 0x00201180, 0x001E212C,
+0x002034A0, 0x002034A4, 0x0020145E, 0x00202D2C,
+0x002034BC, 0x002011E4, 0x002031BC, 0x002031C4,
+0x002031B8, 0x002031BA, 0x00202C80, 0x002014AA,
+0x00008000, 0x4F222FE6, 0x6E22D212, 0xC84060E3,
+0x22E28D02, 0x0009BCFA, 0x4218E240, 0x89012E28,
+0x0009BD05, 0xC81060E3, 0xD40B8905, 0x420BD20B,
+0xBD040009, 0x60E30009, 0x8901C805, 0x0009BDEB,
+0xC80260E3, 0x4F268902, 0x6EF6AD01, 0x000B4F26,
+0x00006EF6, 0x001C3510, 0x002034B0, 0x0020145E,
+0x080A0C0E, 0x00020406, 0x1A1C1E20, 0x12141618,
+0x2E303234, 0x26282A2C, 0x3A3C3E40, 0x6C625648,
+0x41112F26, 0xE2208F18, 0x890B3123, 0x321CD204,
+0xD1026220, 0x412B312C, 0x00090009, 0x00202CAA,
+0x00202C60, 0x000BE000, 0x400062F6, 0x40004000,
+0x40004000, 0x40004000, 0x62F6000B, 0x40004000,
+0x40004000, 0x40004000, 0x40184000, 0x62F6000B,
+0x40004000, 0x40004000, 0x40004000, 0x40284000,
+0x62F6000B, 0x40004000, 0x40184000, 0x000B4028,
+0xC90F62F6, 0x40054005, 0x40054005, 0x62F6000B,
+0x4005C907, 0x40054005, 0x62F6000B, 0x4005C903,
+0x000B4005, 0xC90162F6, 0x000B4005, 0x000062F6,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x544F0D0A,
+0x46205355, 0x00003A57, 0x206C754A, 0x32203532,
+0x20373030, 0x313A3132, 0x37323A32, 0x00000000,
+0x00000D0A, 0x00000043, 0x42707372, 0x3D206675,
+0x554E203D, 0x202C4C4C, 0x6E49677A, 0x4E497274,
+0x6D754E51, 0x0000003D, 0x61766E49, 0x2064696C,
+0x72657375, 0x20726F20, 0x2079656B, 0x00214449,
+0x52504545, 0x57204D4F, 0x65746972, 0x6461202C,
+0x003D7264, 0x6C617620, 0x0000003D, 0x00000A0D,
+0x6E6B6E55, 0x206E776F, 0x6D6D6F63, 0x3D646E61,
+0x00000000, 0x000A0D52, 0x203A3051, 0x00000020,
+0x203A3151, 0x00000020, 0x203A3251, 0x00000020,
+0x203A3351, 0x00000020, 0x203A3451, 0x00000020,
+0x61437748, 0x7262696C, 0x6F697461, 0x6620206E,
+0x0A6C6961, 0x0000000D, 0x73696F4E, 0x61432065,
+0x7262696C, 0x6F697461, 0x6166206E, 0x21216C69,
+0x00000D0A, 0x00000072, 0x00205220, 0x00000D0A,
+0x62735576, 0x7473725F, 0x00000A0D, 0x62735576,
+0x7375735F, 0x646E6570, 0x00000A0D, 0x62735576,
+0x7365725F, 0x000A0D6D, 0x00000042, 0x72746E49,
+0x6D652051, 0x2C797470, 0x49677A20, 0x4972746E,
+0x754E514E, 0x00003D6D, 0x654C7245, 0x0000006E,
+0x00000049, 0x20746F4E, 0x756F6E65, 0x49206867,
+0x4220514E, 0x0A0D6675, 0x00000000, 0x000000FF,
+0x00020001, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x010E010D,
+0x00020003, 0x01090108, 0x0002010A, 0x02000003,
+0x02020201, 0x02040203, 0x02060205, 0x02020200,
+0x02040203, 0x020C0207, 0x020E020D, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x010E010D,
+0x00FF010F, 0x01090108, 0x010B010A, 0x020000FF,
+0x02020201, 0x02040203, 0x02060205, 0x02020200,
+0x02040203, 0x020C020B, 0x020E020D, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00205220,
+0x00000046, 0x00000059, 0x73204142, 0x003D7165,
+0x49544120, 0x0000204D, 0x00000000, 0x00000000,
+0x002E0209, 0x80000101, 0x000409FA, 0x00FF0400,
+0x05070000, 0x02000201, 0x82050700, 0x00020002,
+0x03830507, 0x07010040, 0x40020405, 0x02090000,
+0x0101002E, 0x09FA8000, 0x04000004, 0x000000FF,
+0x02010507, 0x07000040, 0x40028205, 0x05070000,
+0x00400383, 0x04050701, 0x00004002, 0x00000000,
+0x00000000, 0x07090000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, };
+
+const u32_t zcFwImageSize=13656;
diff --git a/drivers/staging/otus/hal/hpfwu_FB50_mdk.c b/drivers/staging/otus/hal/hpfwu_FB50_mdk.c
new file mode 100644
index 000000000000..ed1736f945ab
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwu_FB50_mdk.c
@@ -0,0 +1,721 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xD2287FFC, 0x0009420B,
+0x0009B019, 0x9446D526, 0xE600A003, 0x8D043642,
+0x60527601, 0x8DF9C840, 0xD4222F02, 0x4E0BDE22,
+0xD4220009, 0x00094E0B, 0x4E0BD421, 0x7F040009,
+0xA0254F26, 0x4F226EF6, 0x410BD11E, 0xD41E0009,
+0x0009440B, 0x450BD51D, 0xD71D0009, 0x611DE1FF,
+0x2712D21C, 0xD41C5029, 0xE1FFCB01, 0x1209E501,
+0x12112212, 0xD5192452, 0xD6199716, 0xE7002572,
+0x2670D218, 0x2272D618, 0x4F26E201, 0x2622000B,
+0xDD17DC16, 0x4C0BDE17, 0x4D0B0009, 0x4E0B0009,
+0xAFF80009, 0x27100009, 0x00000640, 0x0020095A,
+0x001C001C, 0x00202940, 0x00200E2A, 0x0020294C,
+0x00202964, 0x00200CF0, 0x00200F26, 0x002009C4,
+0x001C3510, 0x001C3624, 0x001E212C, 0x002028EC,
+0x00202850, 0x002028F4, 0x00202900, 0x00200BEC,
+0x00201FD4, 0x002017B8, 0x2FD62FC6, 0x4F222FE6,
+0xDEA17FA4, 0x61E0E01C, 0x7D016DE3, 0x61D00F14,
+0xD59FD49E, 0x450BE020, 0xE0200F14, 0xE78004FC,
+0x604C66E2, 0x7D7F677C, 0x1F693070, 0x2D628F17,
+0x01FCE01C, 0x641CE500, 0xD797DE96, 0x3243625D,
+0xA21A8B01, 0x655D0009, 0x31EC6153, 0xE0286C10,
+0x6D530FC4, 0x3D7C62CE, 0xAFEF2D20, 0x20087501,
+0xE01C8B15, 0xE50001FC, 0xD78BDE8A, 0x641CA00A,
+0x6C53655D, 0x66C23CEC, 0x66626253, 0x2262327C,
+0x1F697504, 0x3243625D, 0xA1F68BF2, 0x88012D10,
+0xE01C8B16, 0xE40001FC, 0x671C2D40, 0x624DDE7D,
+0x8B013273, 0x0009A1E9, 0x62E3644D, 0x72046D43,
+0x3DEC6143, 0x65D2312C, 0x74086C12, 0x25C2AFEF,
+0x8B188804, 0x01FCE01C, 0x2D40E400, 0xDE71671C,
+0x3273624D, 0xA1D08B01, 0x644D0009, 0x62E36D43,
+0x65D23DEC, 0x61437204, 0x6612312C, 0x74086C52,
+0xAFED2C69, 0x880525C2, 0xE01C8B18, 0xE40001FC,
+0x671C2D40, 0x624DDE63, 0x8B013273, 0x0009A1B5,
+0x6C43644D, 0x3CEC62E3, 0x720465C2, 0x3D2C6D43,
+0x615266D2, 0x216B7408, 0x2512AFED, 0x8B138830,
+0xE200DE58, 0x64E22D20, 0x8B042448, 0x420BD257,
+0xA19A0009, 0x55E10009, 0x57E356E2, 0xDD545CE4,
+0x2FC64D0B, 0x7F04A191, 0x89018828, 0x0009A0EA,
+0xE143DE4C, 0x622D62E1, 0x8F033217, 0x56FB1FEB,
+0x2621E240, 0x8B013217, 0x0009A0D5, 0xE1015EFB,
+0x301685E1, 0xA0CE8B01, 0xE4010009, 0x2D4055FB,
+0x6451B179, 0xE14357FB, 0xE0546271, 0x3517652D,
+0x0F568D41, 0x3563E640, 0xE6008B05, 0x0F65E034,
+0xA00FE11A, 0x615372C0, 0x41214121, 0x41214121,
+0x45214121, 0x45214521, 0xC9036053, 0xE0346603,
+0x71180F65, 0x2209E007, 0x641DE030, 0x0F2565F3,
+0x1F4EB1F1, 0x04FDE034, 0x674DE030, 0x47080CFD,
+0x607361CD, 0x4108D22B, 0xE00F0CFE, 0x1F1F420B,
+0x2CD96D07, 0x5EFB6073, 0x85E20FC6, 0x420B51FF,
+0x2C0B600D, 0x54FE6073, 0xB1BB0FC6, 0xE05465F3,
+0x652D62E1, 0xE6400F56, 0x89623563, 0xE050E100,
+0x60230F15, 0x4008C903, 0x6D034000, 0xE0406103,
+0xE0440FD6, 0xD217EEFF, 0x6EEC0FF6, 0x0F26E058,
+0x60E3420B, 0x42216253, 0x42214221, 0x66234221,
+0x326C4200, 0x45214200, 0xE0486707, 0x0F764521,
+0xC9036053, 0x40085CFB, 0x7C0630FC, 0x6E036D2D,
+0x1FD51FC6, 0x1F04A02E, 0x00117D00, 0x00202968,
+0x00200E2A, 0x00117D04, 0x00117D84, 0x00200700,
+0x0020074C, 0x00202034, 0x0FD6E04C, 0x05FEE044,
+0x64D3B189, 0x64E2E048, 0xE04006FE, 0x2E422469,
+0x01FE67C4, 0x667CE058, 0x420B02FE, 0x240B6063,
+0x05FEE044, 0xB15D2E42, 0xE05064D3, 0x7D0101FD,
+0x0F157101, 0x02FDE050, 0x3262E606, 0x56FB8BDC,
+0x55FB6261, 0x85514200, 0x302C750C, 0x6103701B,
+0x64F3E600, 0xE704A004, 0x76016256, 0x74042422,
+0x3273626D, 0x65F38BF8, 0x641DB13C, 0xB0D256FB,
+0xA0AA6461, 0xD4880009, 0xE201D588, 0x2D20450B,
+0x0009A0A3, 0x8B078829, 0xE200DE85, 0x66E22D20,
+0x646DB0A1, 0x0009A099, 0x622CE281, 0x8B3D3020,
+0xD680E738, 0xE0442D70, 0xE0480C6E, 0x6E621DC1,
+0x51611DE2, 0x54621D13, 0x55651D44, 0x57631D55,
+0x5C661D76, 0x0E6E1DC7, 0x1DE8E040, 0xE050016E,
+0x54641D19, 0x056E1D4A, 0x1D5BE04C, 0xE054076E,
+0x0C6E1D7C, 0x1DCDE058, 0xE044026E, 0xED001D2E,
+0xE04806D6, 0x16D126D2, 0x16D516D2, 0x16D616D3,
+0xE04006D6, 0xE05006D6, 0x06D616D4, 0x06D6E04C,
+0x06D6E054, 0x06D6E058, 0x1F29A057, 0x622CE282,
+0x89313020, 0x05FCE020, 0x625CE683, 0x3260666C,
+0xD65D8B07, 0x2650E500, 0x52617680, 0xA044D65B,
+0xE6902622, 0x3260666C, 0xD2578B16, 0xE500D658,
+0x60622250, 0xCB20D257, 0xE6052602, 0xD6562262,
+0x2252460B, 0x420BD255, 0xD2550009, 0x2262E601,
+0x4618D254, 0x2262A029, 0xD254D453, 0xD4546542,
+0x0009420B, 0x0009A021, 0xE524D647, 0xD5452650,
+0x16215257, 0x16225258, 0x16235259, 0x1624525A,
+0x1625525B, 0x1626525C, 0x1627525D, 0x1628525E,
+0x1F29525F, 0xE2001629, 0x15281527, 0x152A1529,
+0x152C152B, 0x152E152D, 0x7F5C152F, 0x6EF64F26,
+0x000B6DF6, 0x4F226CF6, 0xE240614D, 0x89173123,
+0x3127E21F, 0xD43B8908, 0xE001D53B, 0x6642450B,
+0x26796707, 0x2462A00C, 0x3127E23F, 0xD7358908,
+0x71E0D635, 0x460BE001, 0x62075571, 0x17512529,
+0x000B4F26, 0x4F220009, 0xE240614D, 0x89153123,
+0x3127E21F, 0xD42B8907, 0x6642D22B, 0xE001420B,
+0xA00B260B, 0xE23F2462, 0x89073127, 0xD626D725,
+0x71E05571, 0xE001460B, 0x1751250B, 0x000B4F26,
+0xE6400009, 0x46284618, 0x6252D520, 0x89FC2268,
+0x0009000B, 0x4618E680, 0xD51C4628, 0x22686252,
+0x000B89FC, 0xA0010009, 0x7201E200, 0x8BFC3242,
+0x0009000B, 0x4618E680, 0xD5154628, 0x22686252,
+0x000B8BFC, 0x00000009, 0x0020296C, 0x00200E2A,
+0x00117D04, 0x00202858, 0x00117D80, 0x002028EC,
+0x001C3500, 0x001D4004, 0x00200F26, 0x002009C4,
+0x001E212C, 0x001C3D28, 0x00117D00, 0x00200E8A,
+0x00202984, 0x001C3704, 0x00202034, 0x001C373C,
+0x001C3700, 0x4F222FE6, 0x6E537FFC, 0x2F42BFCA,
+0xD61561E2, 0x1615E280, 0x421854E1, 0x55E21646,
+0x16574228, 0x6EF257E3, 0x2E2B1678, 0x7F0426E2,
+0xAFA74F26, 0x2FC66EF6, 0x2FE62FD6, 0xDD0A4F22,
+0xBFAF6C53, 0xBF946E43, 0xBFAB2DE2, 0x51D50009,
+0x54D62C12, 0x55D71C41, 0x56D81C52, 0x4F261C63,
+0x6DF66EF6, 0x6CF6000B, 0x001C370C, 0x0009A0F8,
+0xD19B4F22, 0xD49B9299, 0x2122B00D, 0x9795E605,
+0xB0229595, 0xB0366463, 0xB03A0009, 0xB03D0009,
+0xA06C0009, 0x4F124F26, 0xD1934F02, 0x94873145,
+0x4609060A, 0x46094609, 0x00293646, 0xD78CD58F,
+0x2500CA01, 0x4F062762, 0x4F16000B, 0xBFEA4F22,
+0xB0230009, 0xA0520009, 0x2FE64F26, 0x6E63D188,
+0x44186612, 0x4528926D, 0x26294408, 0x44084500,
+0x4400265B, 0x4708264B, 0x47082162, 0x27EBD181,
+0x000B2172, 0xD1806EF6, 0xE603D480, 0x000B2162,
+0xD27F2462, 0xE40A9656, 0x2262AFB0, 0x2FC62FB6,
+0x2FE62FD6, 0xDC7B4F22, 0x2C22E201, 0xBFA5E40A,
+0x60C27C44, 0xCB01ED00, 0x60C22C02, 0xC901EB64,
+0x6E03A008, 0x89073DB2, 0xE40160C2, 0xBF95C901,
+0x7D016E03, 0x8BF52EE8, 0x8B033DB2, 0xD26FD46E,
+0x0009420B, 0x4F26E40A, 0x6DF66EF6, 0xAF856CF6,
+0x44116BF6, 0x604B8F01, 0x000B6043, 0x2F860009,
+0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6, 0x7FFC4F22,
+0x6DA3EA00, 0xDC626BA3, 0x9914E864, 0x8B4E2BB8,
+0x3AE3EE0A, 0x60C2894B, 0xCB02ED00, 0x62C22C02,
+0x2F0260C2, 0xA010C902, 0x096C6E03, 0x5BB45288,
+0x1FFF09B4, 0x01FF03C4, 0x89083D83, 0xE46460C2,
+0xC9022F02, 0x6E03BF52, 0x2EE87D01, 0xD1518BF4,
+0x54C1D551, 0x66526412, 0x6269EE01, 0x4220622F,
+0x622F4219, 0x4E182299, 0x8D0322E8, 0xE4FF6423,
+0x3428229A, 0x6572D749, 0x622F6259, 0x42194220,
+0x2299622F, 0x8D0322E8, 0xE6FF6623, 0x3628229A,
+0x3468BFA7, 0x30E2EE02, 0xAFB78901, 0xD240EB01,
+0x6EECEEE6, 0xBF21E40A, 0xAFAF22E2, 0xEE0A7A01,
+0x89013AE3, 0x8B033D83, 0xD234D43A, 0x0009420B,
+0x4F267F04, 0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6,
+0x68F6000B, 0x5651D534, 0x46286052, 0x306C000B,
+0x2FC62FB6, 0x2FE62FD6, 0x4F124F22, 0xBFF14F02,
+0x6B036E43, 0xDD1CDC2D, 0x0009BFEC, 0x3C0530B8,
+0x4609060A, 0x46014609, 0x020A3D65, 0x42094209,
+0x32E24209, 0x4F068BF0, 0x4F264F16, 0x6DF66EF6,
+0x000B6CF6, 0x2FE66BF6, 0xDE214F22, 0xE500E102,
+0x2E12E403, 0x2E52BFD4, 0x4618E606, 0xE403E700,
+0x2E722E62, 0xAFCB4F26, 0x4F226EF6, 0x0009BFEB,
+0xE6E6D213, 0xE40A666C, 0x2262BFC2, 0x4F26AFE3,
+0x002028F0, 0x0024CDE0, 0x10624DD3, 0x00202AF0,
+0x001C5814, 0x001C59D0, 0x001C59A4, 0x001C639C,
+0x001C5804, 0x001C581C, 0x00202998, 0x00200E2A,
+0x001C5860, 0x001C6864, 0x001C59BC, 0x001C69BC,
+0x001C947C, 0x002029B0, 0x001C1040, 0xCCCCCCCD,
+0x001D4004, 0x2F962F86, 0x2FB62FA6, 0x2FD62FC6,
+0x4F222FE6, 0xE4007FE4, 0x4528E510, 0x67436C43,
+0xE107A00F, 0x6043644D, 0x0F564008, 0xEE0060C3,
+0x815125C1, 0x81538152, 0x157315E2, 0x751415E4,
+0x624D7401, 0x8BED3213, 0xDA6F51F1, 0x1A1154F2,
+0xD16E2A12, 0x57F455F3, 0x6DF258F5, 0x1141D96C,
+0x11532142, 0x11751152, 0x11871174, 0x52F61186,
+0x19D1D668, 0xD86829D2, 0xDA68E950, 0x1621EBB4,
+0x6BBC2622, 0xA0214908, 0x6EEDEE00, 0x61E36DE3,
+0x41084D08, 0x31EC3DEC, 0x41084D08, 0x60C33D8C,
+0xE7904108, 0x81D12DC1, 0x41086093, 0x81D2677C,
+0x31AC60C3, 0x3472E200, 0x1DD281D3, 0xD4551D13,
+0x1D248D01, 0xB03AD450, 0x7E0165D3, 0x34B264ED,
+0xD14D8BDB, 0x6512DB52, 0x4529D24D, 0x64121B51,
+0x674DD14A, 0x67222B72, 0x4729D64E, 0x69221B73,
+0x689D2FD2, 0x69121B82, 0x5A122692, 0x5B1416A2,
+0x16B4DA44, 0x16C65C16, 0x16EA6EA2, 0x4F267F1C,
+0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6, 0x68F6000B,
+0x60616642, 0x8D04C803, 0x6061E500, 0x8802C903,
+0x52628B03, 0x51246563, 0x000B2412, 0x2FD66053,
+0x4F222FE6, 0x6E537FEC, 0xE5506253, 0xE4006D43,
+0xA0014508, 0x5224E101, 0x22116043, 0x81238121,
+0x81226053, 0x362056E2, 0xD22F8BF5, 0x64F316E4,
+0x420BE614, 0x65E165E3, 0x2549E4FC, 0x61F12E51,
+0x214965F3, 0x54D12F11, 0x410BD127, 0x57D1E614,
+0xCB016071, 0x1DE12701, 0x4F267F14, 0x000B6EF6,
+0x2FD66DF6, 0x4F222FE6, 0x6E537FEC, 0xE5FC6653,
+0x60616D43, 0xCB012059, 0x52E22601, 0x8B063260,
+0x51E212E4, 0x8B0431E0, 0xA00252D1, 0xAFF01E22,
+0xD2155664, 0xE61464F3, 0x65E3420B, 0xE1FC67E1,
+0x2E712719, 0x54D167F1, 0xD10F2719, 0xE61465F3,
+0x2F71410B, 0x602152D1, 0x2201CB01, 0x7F141DE1,
+0x6EF64F26, 0x6DF6000B, 0x002028BC, 0x002028C4,
+0x002028B4, 0x002028E4, 0x0010008C, 0x00100EC0,
+0x001E2108, 0x001C3D00, 0x00202194, 0x2FC62FB6,
+0x2FE62FD6, 0xD6314F22, 0x60D36D62, 0x894DC803,
+0xDB30DC2F, 0x0009A02C, 0xC9036061, 0x892B8801,
+0xD22DD42B, 0x0009420B, 0x65035603, 0xC8208561,
+0xE0508903, 0x720102BE, 0x85620B26, 0x4000600D,
+0x4000366A, 0x40004624, 0x206D4624, 0xD423C903,
+0x40086E03, 0xD1224000, 0x340C410B, 0x61E3D521,
+0xD721E001, 0x450BD221, 0x64E37E30, 0x2702420B,
+0x66C252C1, 0x8BCF3620, 0x4E18EE01, 0xA011DB1C,
+0x6061EC75, 0x8801C903, 0xD4198910, 0x460BD612,
+0xD4180009, 0x470BD718, 0xD2136503, 0x64C3D113,
+0x22E2410B, 0x66B252B1, 0x8BEA3620, 0xC80460D3,
+0xD2128906, 0x6EF64F26, 0x6CF66DF6, 0x6BF6422B,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x001E2100,
+0x002028BC, 0x00202858, 0x00200AE0, 0x002028C4,
+0x00200B62, 0x00202034, 0x001C3D30, 0x00200DF0,
+0x002028B4, 0x002028E4, 0x00200AFE, 0x002000F8,
+0xE601D237, 0x1265D537, 0x000B2252, 0xD6361266,
+0x88016062, 0xE1018B62, 0xD5342612, 0x5451D134,
+0xE0406212, 0x2122324C, 0x54115752, 0x1141347C,
+0x57125453, 0x1172374C, 0x52135755, 0x1123327C,
+0x56146452, 0x1164364C, 0x54155754, 0x1145347C,
+0x56165458, 0x1166364C, 0x6762D626, 0x327C5217,
+0x57611127, 0x327C5218, 0x57621128, 0x327C5219,
+0x57631129, 0x347C541A, 0x5764114A, 0x347C541B,
+0x5765114B, 0x347C541C, 0x5266114C, 0x372C571D,
+0x5267117D, 0x342C541E, 0x5268114E, 0x362C561F,
+0xD615116F, 0x041E6262, 0x342C7694, 0xE0440146,
+0x061E6262, 0x0166362C, 0x525CE048, 0xD60F051E,
+0x0156352C, 0xE0546262, 0x4229051E, 0x0156352C,
+0xE0585561, 0x4529061E, 0x0166365C, 0x0009000B,
+0x001C1010, 0x0000C34F, 0x001C1028, 0x001C369C,
+0x00202858, 0x001C3CA0, 0x001C36F4, 0x001C3B88,
+0xD62F7FFC, 0x2642644C, 0xC8205066, 0x2F028DFC,
+0x7F04000B, 0x2FD62FC6, 0x4F222FE6, 0x6D436C53,
+0xEE00A004, 0x7E0164D4, 0x644CBFEA, 0x8BF93EC2,
+0x6EF64F26, 0x000B6DF6, 0xA0016CF6, 0x76016643,
+0x22286260, 0x36488BFB, 0x6563AFE4, 0x2FB62F96,
+0x2FD62FC6, 0x4F222FE6, 0xEC1CED08, 0xDB196E53,
+0x61C3E90A, 0x60434B0B, 0x3092C90F, 0x66038D02,
+0x7630A001, 0x4D107637, 0x7E012E60, 0x7CFC8FF1,
+0x8058E000, 0x6EF64F26, 0x6CF66DF6, 0x000B6BF6,
+0x000B69F6, 0x000BE000, 0x2FE6E000, 0x7FEC4F22,
+0x6E436253, 0xBFD165F3, 0xBFC66423, 0xBFC464E3,
+0xD40564F3, 0x0009BFC1, 0x4F267F14, 0x6EF6000B,
+0x001C0004, 0x002020F4, 0x002029CC, 0xE110D59C,
+0xE6406050, 0x2500C9FD, 0xE0FF75E9, 0x80516453,
+0x80538052, 0x80568055, 0x251075EF, 0xE1EF6250,
+0x2219E001, 0xE7202520, 0x24608052, 0x2570000B,
+0xE4FDD590, 0xE7026152, 0x25122149, 0x74016052,
+0x2502CB01, 0xD18C6652, 0x25622649, 0x92C26012,
+0x2102CB08, 0xC9CF6012, 0x60122102, 0x2102CB03,
+0x000B1172, 0x4F221123, 0xE100D484, 0xD285D784,
+0xD5852410, 0x2711D485, 0x2211E700, 0xBFBD2511,
+0xD5832471, 0x2560E600, 0x4F26AFD2, 0xD281664C,
+0x362C4600, 0xCB106060, 0x2600000B, 0xD27D654C,
+0x352C4500, 0xE1EF6650, 0x000B2619, 0x664C2560,
+0x4600D279, 0x6060362C, 0x000BCB10, 0x654C2600,
+0x4500D275, 0x6650352C, 0x2619E1EF, 0x2560000B,
+0xD270664C, 0x362C4600, 0xCB086060, 0x2600000B,
+0xD26C654C, 0x352C4500, 0xE1F76650, 0x000B2619,
+0x664C2560, 0x4600D268, 0x6060362C, 0x000BCB08,
+0x654C2600, 0x4500D264, 0x6650352C, 0x2619E1F7,
+0x2560000B, 0xD65F624C, 0x326C4200, 0xC9086020,
+0x40214021, 0x000B4021, 0x624C600C, 0x4200D65A,
+0x6020326C, 0x4021C908, 0x40214021, 0x600C000B,
+0xD156644C, 0x341C74FF, 0x000B6240, 0xD154602C,
+0x341C644C, 0x000B6240, 0x2FE6602C, 0x655C4F22,
+0x3567E60A, 0x6E438D15, 0x6453BFEA, 0x60EC640C,
+0x8B028801, 0xA002E00F, 0x44092409, 0x624C4409,
+0x3263E60A, 0xBFE28905, 0x620C644C, 0xC8806023,
+0xE2008B00, 0x4F266023, 0x6EF6000B, 0xD6414F22,
+0x88016062, 0xB2228B03, 0xA0030009, 0xD23E0009,
+0x2260E640, 0xE200D63D, 0x000B4F26, 0x4F222622,
+0x6062D638, 0x8B018802, 0x0009B26C, 0xE200D637,
+0x000B4F26, 0x0FFF2622, 0xD433D532, 0xE701E100,
+0x000B2512, 0xD2302470, 0x000BE604, 0xD5202260,
+0x6150E4FD, 0x2149D62E, 0x2510E700, 0x2670000B,
+0xE4FBD51B, 0x22496250, 0x2520000B, 0xE4F7D518,
+0x22496250, 0x2520000B, 0xD2264F22, 0x600D8522,
+0x89112008, 0x89138801, 0x89158803, 0x89178805,
+0x89418806, 0x89478808, 0x894D8809, 0x8953880A,
+0x8959880B, 0x0009A060, 0x0009B062, 0x600CA05D,
+0x0009B070, 0x600CA059, 0x0009B07A, 0x600CA055,
+0x6260D606, 0x8B4F2228, 0x0009B086, 0x600CA04D,
+0x001E1028, 0x001E2148, 0x001E1108, 0x0020293D,
+0x0020292C, 0x0020292E, 0x00202930, 0x00202910,
+0x001E1008, 0x001E103F, 0x001E105F, 0x001E1030,
+0x001E1090, 0x00202938, 0x001E100B, 0x00202934,
+0x0020293C, 0x00202904, 0x6260D687, 0x8B232228,
+0x0009B06A, 0x600CA021, 0x6260D683, 0x8B1B2228,
+0x0009B0B4, 0x600CA019, 0x6260D67F, 0x8B132228,
+0x0009B0BA, 0x600CA011, 0x6260D67B, 0x8B0B2228,
+0x0009B11E, 0x600CA009, 0x6260D677, 0x8B032228,
+0x0009B136, 0x600CA001, 0x4F26E000, 0x0009000B,
+0xD273D172, 0xD5738412, 0x4000C90F, 0xD772012D,
+0x611CE403, 0xD671E20F, 0x27122540, 0xE0012520,
+0x2602000B, 0xE601D269, 0x30668523, 0xE0008D06,
+0xE000D267, 0x8122D669, 0x2602E001, 0x0009000B,
+0x8523D262, 0x2008600D, 0x88018905, 0xD6648B0A,
+0xCB016060, 0xD6612600, 0xE101D45D, 0x2612E001,
+0x8142000B, 0xE000000B, 0xE501D158, 0x45188513,
+0x3453640D, 0x8D056603, 0xD25AE000, 0xE001D557,
+0x25022260, 0x0009000B, 0xD1504F22, 0x650D8513,
+0x44196453, 0x672E6249, 0x602C227D, 0x89098801,
+0x890C8802, 0x89108803, 0x89268806, 0x89298807,
+0x0009A038, 0xD64DD54C, 0xA027E212, 0x625C2652,
+0x8B2F2228, 0xA01ED64A, 0x605C6262, 0x89052008,
+0x89088810, 0x890B8820, 0x0009A024, 0xD643D445,
+0xA013E204, 0xD7442642, 0xE20CD640, 0x2672A00E,
+0xD63ED542, 0xA009E218, 0xD4412652, 0xE20AD63B,
+0x2642A004, 0xD639D23F, 0xE22E2622, 0xD43E8515,
+0x3277670D, 0x8F012421, 0x24516503, 0x0009B0DF,
+0xE001A001, 0x4F26E000, 0x0009000B, 0xE101D629,
+0x2610D436, 0xD7286541, 0x655DD128, 0xE001E20F,
+0x26202752, 0x2102000B, 0x4F222FE6, 0x8523D21F,
+0x2448640C, 0xD62D8B08, 0xE200D521, 0x84512621,
+0x20499430, 0x8051A026, 0x60E0DE1D, 0x8D0BC840,
+0x3427E201, 0xD1258922, 0x420BD225, 0xD5252141,
+0xCB046052, 0x2502A00B, 0x89173427, 0xD722D21F,
+0x2241470B, 0xE5FBD61F, 0x21596162, 0x84E12612,
+0xB12DCB80, 0x60E080E1, 0xCB04D61C, 0x60602E00,
+0x2600C93F, 0xE001D609, 0x2602A001, 0x4F26E000,
+0x6EF6000B, 0x0000FF7F, 0x0020293D, 0x00202904,
+0x00202910, 0x001E1100, 0x001E100C, 0x00202934,
+0x001E1000, 0x001E1001, 0x00202AF4, 0x00202918,
+0x00202920, 0x00202B62, 0x00202B66, 0x00202B72,
+0x00202B8A, 0x00202B94, 0x0020291C, 0x0020292A,
+0x00201AB6, 0x001E1108, 0x00201BC2, 0x001E1015,
+0x6060D696, 0x8919C880, 0x6021D295, 0x8B158801,
+0xE501D294, 0x30568524, 0xD1938910, 0xD493E203,
+0x65412120, 0x655DE00B, 0xD5910656, 0xE702E40F,
+0x25712140, 0xE001D78F, 0x2702000B, 0xE000000B,
+0x4F222FE6, 0x84E1DE8C, 0x8934C880, 0x8554D585,
+0x8F302008, 0xD7896103, 0x66728553, 0x650C6403,
+0x620C8566, 0x8B263520, 0xD780D685, 0x644C651C,
+0x27412651, 0xC84060E0, 0xD2828907, 0x0009420B,
+0x6062D681, 0xA008CB04, 0xD1802602, 0x0009410B,
+0xE5FBD67D, 0x24596462, 0xB0A12642, 0xD5750009,
+0x2522E201, 0xD77A60E0, 0x2E00CB04, 0xC93F6070,
+0xA0012700, 0xE0006023, 0x000B4F26, 0x2FA66EF6,
+0x2FC62FB6, 0x2FE62FD6, 0xE240DA69, 0xDC6666A1,
+0x3123616D, 0x62638900, 0x6ED36D2C, 0x4E2136D8,
+0x4E212A61, 0xDB6CD46B, 0xE700A00F, 0x770166B2,
+0x71026163, 0x65612B12, 0x71026613, 0x62612B12,
+0x622D655D, 0x325C4228, 0x627C2422, 0x8BED32E3,
+0xC90360D3, 0x8B108803, 0xED076EB2, 0x710261E3,
+0x67132B12, 0x62E17102, 0x65712B12, 0x655D622D,
+0x352C4528, 0xA00C2CD0, 0x88022452, 0xA0038B01,
+0x8801E203, 0xE2018B05, 0x66B22C20, 0x677D6761,
+0xEB0F2472, 0x6DA12CB0, 0x8B052DD8, 0xD445D24F,
+0xE101EE00, 0x241222E2, 0x6DF66EF6, 0x6BF66CF6,
+0x6AF6000B, 0x2FE62FD6, 0xE240DD3D, 0x616D66D1,
+0x89003123, 0x672C6263, 0xDE433678, 0x2D617703,
+0xD6404721, 0x472164E2, 0xE100A00E, 0x71016562,
+0x24506253, 0x42197401, 0x74012420, 0x24504529,
+0x45197401, 0x74012450, 0x3273621C, 0x42008BEE,
+0x64D166E2, 0x362C4200, 0x8F062448, 0xDD332E62,
+0xE500DE28, 0x2D52E701, 0x6EF62E72, 0x6DF6000B,
+0x2FE62FD6, 0xEE014F22, 0xED0AA005, 0x64E3BCB6,
+0x64E3BCBC, 0x62EC7E01, 0x8BF732D7, 0xEE01A005,
+0x64E3BCBD, 0x64E3BCC3, 0x62EC7E01, 0x8BF732D7,
+0x6EF64F26, 0x6DF6000B, 0x2FE62FD6, 0x7FFC4F22,
+0x6060D61F, 0x89758801, 0xE101D41E, 0xD7128548,
+0x650D2610, 0x45196070, 0x6659DD1B, 0x61D3626E,
+0xC840262D, 0x74027102, 0x8D47D718, 0xD218666C,
+0xE501DE0A, 0xA0312E22, 0x0000EE04, 0x001E1001,
+0x0020292A, 0x00202904, 0x001E1100, 0x0020292E,
+0x0020291C, 0x00202934, 0x001E1000, 0x00202920,
+0x0020292C, 0x00201AB6, 0x001E1108, 0x00201BC2,
+0x001E1015, 0x001E100C, 0x00202918, 0x00202938,
+0x0020293C, 0x00202AF4, 0x00202B8A, 0x00202B96,
+0x00202B06, 0x75016245, 0x71022121, 0x32E3625C,
+0x60638BF8, 0xE60181D4, 0xE417D538, 0x3243626C,
+0x6255891E, 0x27217601, 0x7702AFF8, 0xDE35D234,
+0x2E22E501, 0xEE04A004, 0x75016245, 0x71022121,
+0x32E3625C, 0x60638BF8, 0xE60181D4, 0xA004D52E,
+0x6255E417, 0x27217601, 0x626C7702, 0x8BF83243,
+0x2D21924B, 0xD72AD429, 0x2F126142, 0x6DF265F2,
+0xC9806053, 0x60532700, 0x6103C960, 0x60538071,
+0x65F26EF2, 0x4D19C903, 0x80724529, 0x451960DC,
+0x4E298172, 0x62EC605C, 0x302C4018, 0x6D428173,
+0x2FD22118, 0x62F26EF2, 0x421966F2, 0x656C4629,
+0x602C66F2, 0x401864EC, 0x304C4629, 0x81744619,
+0x4018606C, 0x8F07305C, 0xBCB58175, 0x620C0009,
+0x89082228, 0x0009A00A, 0x88406013, 0xB00A8B03,
+0xA0030009, 0xD60B0009, 0x2622E202, 0x4F267F04,
+0x000B6EF6, 0x000B6DF6, 0x060A0009, 0x00202B36,
+0x00202B34, 0x00202920, 0x00202B08, 0x001E100C,
+0x00202904, 0x00202934, 0x7FFC4F22, 0x6620D27E,
+0x8D082668, 0xD47D2F60, 0x420BD27D, 0x64F00009,
+0xA0907F04, 0x7F044F26, 0x000B4F26, 0x000B0009,
+0x2FE60009, 0xDE774F22, 0x60E0D677, 0xCBC0D477,
+0x62602E00, 0xC803602C, 0x40218904, 0x70014021,
+0x6603A002, 0x66034009, 0xD671616D, 0xE500A004,
+0x75016262, 0x74042422, 0x3213625D, 0xD16D8BF8,
+0x0009410B, 0xE401D66C, 0x84E22641, 0x80E2C9BF,
+0x000B4F26, 0x2FE66EF6, 0xD5687FFC, 0x6250DE61,
+0x642C84E2, 0xCB407404, 0x80E2614D, 0x44216413,
+0xD7634421, 0xE600A004, 0x76016256, 0x27222F22,
+0x3243626D, 0x60138BF8, 0x2008C903, 0x88038912,
+0x88028905, 0x88018906, 0xA0088907, 0xE0070009,
+0x8078A005, 0xA002E003, 0xE0018078, 0x62528078,
+0x27222F22, 0xD650E00F, 0x60618078, 0x8B018801,
+0x2621E200, 0x6060D64F, 0x2600CB08, 0xC93F60E0,
+0x7F042E00, 0x6EF6000B, 0x6021D247, 0x8D188801,
+0xD2466143, 0x22106053, 0x60638021, 0xD4468121,
+0xE500A007, 0x027C605D, 0x364C6603, 0x26207001,
+0x625D6503, 0x3213611C, 0xD6408BF4, 0xC9BF6060,
+0x000B2600, 0x2FD60009, 0x4F222FE6, 0x60437FFC,
+0x8D02C820, 0xBF6A6E43, 0x60E30009, 0x8901C810,
+0x0009BF67, 0xC84060E3, 0xBF8C8901, 0x60E30009,
+0x8929C801, 0x60D0DD32, 0x8D03C802, 0xD6312F00,
+0x0009460B, 0xC80460F0, 0xD62F8902, 0x0009460B,
+0x602362F0, 0x8902C880, 0xC97F60D0, 0x60232D00,
+0x8902C801, 0x420BD229, 0xD5290009, 0x88026052,
+0xD2288B03, 0xA005E604, 0x88012260, 0xD2258B02,
+0x2260E601, 0x2522E200, 0xC88060E3, 0xD2228916,
+0x60E36E20, 0x8902C802, 0x420BD220, 0x60E30009,
+0x8902C804, 0x420BD21E, 0x60E30009, 0x8905C808,
+0x7F04D21C, 0x6EF64F26, 0x6DF6422B, 0x4F267F04,
+0x000B6EF6, 0x00006DF6, 0x001E1020, 0x002029D0,
+0x00200E2A, 0x001E1015, 0x001E10BF, 0x00117D00,
+0x001E10FC, 0x002000F8, 0x00202930, 0x00117D80,
+0x001E10F8, 0x001E10AE, 0x00117D84, 0x001E1017,
+0x001E1021, 0x0020105C, 0x0020107E, 0x00201608,
+0x00202934, 0x001E100B, 0x001E1028, 0x002010AE,
+0x002010C0, 0x002010CC, 0xD6A8644C, 0x346C74FF,
+0x2450000B, 0x644CD6A6, 0x000B346C, 0xD6A52450,
+0x346C644C, 0x2450000B, 0x616D625C, 0x41194208,
+0x60194208, 0x644C4200, 0x324C670E, 0x207DD19E,
+0xC90F4200, 0x000B321C, 0x67632200, 0x4208625C,
+0x42004208, 0x324C644C, 0x4200D198, 0x000B321C,
+0x2FE62270, 0x614C4F12, 0x4100D493, 0x6710314C,
+0x2729E29F, 0x65736E53, 0x4719676D, 0x672E6279,
+0x4221227D, 0x42214221, 0x7601662C, 0xE4014608,
+0x34E84608, 0x644C4600, 0x0E1A0467, 0x215025EB,
+0x000B4F16, 0x4F226EF6, 0xD2857FE8, 0x88016021,
+0xD2848B7B, 0x26686621, 0xD2838B77, 0x26686621,
+0xE50F8B73, 0xE401BFA0, 0xBFA3E501, 0xE586E400,
+0xE400655C, 0x2F50BFA3, 0xBFA0E401, 0xE602E506,
+0x60634618, 0x81F2E401, 0x6543BF9E, 0xE40185F2,
+0xBFAA6543, 0x85F26603, 0x6543E401, 0x6603BFB1,
+0xE40265F0, 0x6053756C, 0x80F8BF7E, 0xBF81E402,
+0x84F8E512, 0x7090E402, 0x6503BF81, 0x4618E602,
+0x81F66063, 0xBF7FE402, 0x85F6E500, 0x6603E402,
+0xE500BF8B, 0xE40285F6, 0xBF926603, 0xE5FEE500,
+0xE010655C, 0xBF5FE403, 0xE5130F54, 0xE40EBF62,
+0x05FCE010, 0xBF62E40E, 0xE5007585, 0xBF63E403,
+0xE500E640, 0xBF70E403, 0xE500E640, 0xBF78E403,
+0xE5FFE640, 0xE014655C, 0xBF45E404, 0xE40F0F54,
+0xE504BF48, 0x05FCE014, 0xBF48E40F, 0xE5017584,
+0xBF49E640, 0xE501E404, 0xBF56E640, 0xE501E404,
+0xE404E640, 0xAF5C7F18, 0x7F184F26, 0x000B4F26,
+0x4F220009, 0xD2427FF0, 0x88016021, 0xD2418B71,
+0x26686621, 0xD2408B6D, 0x26686621, 0xE50F8B69,
+0xE401BF1A, 0xBF1DE501, 0xE586E400, 0xE400655C,
+0x2F50BF1D, 0xBF1AE401, 0xE401E506, 0xBF1B6543,
+0xE401E640, 0xBF286543, 0xE401E640, 0xBF306543,
+0x65F0E640, 0x756CE402, 0xBEFD6053, 0xE40280F4,
+0xE512BF00, 0xE40284F4, 0xBF007090, 0xE6406503,
+0xBF01E402, 0xE640E500, 0xBF0EE402, 0xE640E500,
+0xBF16E402, 0xE5FEE500, 0x6053655C, 0xBEE3E403,
+0xE51380F8, 0xE40EBEE6, 0xE40E84F8, 0xBEE67085,
+0xE5006503, 0xBEE7E640, 0xE500E403, 0xBEF4E640,
+0xE500E403, 0xBEFCE640, 0xE5FFE403, 0x6053655C,
+0xBEC9E404, 0xE40F80FC, 0xE504BECC, 0xE40F84FC,
+0xBECC7083, 0xE5016503, 0xBECDE640, 0xE501E404,
+0xBEDAE640, 0xE501E404, 0xE404E640, 0xAEE07F10,
+0x7F104F26, 0x000B4F26, 0x00000009, 0x001E1030,
+0x001E1080, 0x001E1090, 0x001E103F, 0x001E103E,
+0x0020292A, 0x0020292C, 0x0020292E, 0x0009000B,
+0x666CE680, 0x6563D2A0, 0x7540E700, 0x6473422B,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0x4C18EC01,
+0xDA9BDB9A, 0x65B252B1, 0x89223520, 0xC9036051,
+0x891E8801, 0xD197DE95, 0x64E3410B, 0x85036503,
+0x670D66A2, 0xDD943762, 0xD494890A, 0x420BD294,
+0xD1940009, 0xE701D494, 0x21724D0B, 0x0009AFE2,
+0x420BD292, 0xD69264E3, 0x4D0BD492, 0xAFD926C2,
+0x4F260009, 0x6DF66EF6, 0x6BF66CF6, 0x6AF6000B,
+0x7FF44F22, 0xE6818546, 0x85472F01, 0x81F1666C,
+0xD27D8548, 0x854281F2, 0x81F367F3, 0xE40C8543,
+0x605381F4, 0x81F56563, 0x7540420B, 0x4F267F0C,
+0x0009000B, 0x2F962F86, 0x2FB62FA6, 0x2FD62FC6,
+0x4F222FE6, 0xE2007FEC, 0xA0CBDB7B, 0x6A132F21,
+0x4A08D27A, 0xDE7AE001, 0x4A00420B, 0x7E303AEC,
+0x1F021FE1, 0x66B2DD77, 0x362052B1, 0xA0B58B01,
+0x60610009, 0x8801C903, 0xA0AF8B01, 0x85610009,
+0x8974C801, 0xEE105163, 0xDC638512, 0xC9036603,
+0x85136403, 0x4021600D, 0xC93F4021, 0x8D2030E3,
+0xD7696503, 0x62704408, 0x44004408, 0x22284500,
+0x345C8F0C, 0x6043D265, 0x625D052D, 0x60294219,
+0x207D670E, 0x605C81F6, 0x81F8A00B, 0x6043D260,
+0x685D052D, 0x60894819, 0x209D690E, 0x605C81F6,
+0xD75C81F8, 0x22286272, 0xE0FF8902, 0x81F8600C,
+0xEEFF85F8, 0x6EEC650D, 0x8B0F35E0, 0x4E0BDE45,
+0x540364B3, 0xBF7BE502, 0xD4536803, 0x410BD147,
+0xD7526583, 0xD452E901, 0x2792A020, 0x26E9EEFC,
+0x81126063, 0x946E8513, 0x81132049, 0x45088512,
+0x62036953, 0xE50885F6, 0x8112202B, 0x45188513,
+0x8113209B, 0xD4478514, 0x8114205B, 0x851161B2,
+0x811120E9, 0x602162B2, 0x2201CB01, 0x00094C0B,
+0x56F160B2, 0xCB0152F2, 0xAF7C2A02, 0x85612622,
+0xC802DC3A, 0xD938D227, 0x8D0FD82C, 0x420B64B3,
+0x65030009, 0x480B6493, 0xE8015E03, 0x85EF2C82,
+0x650DD635, 0x64D3460B, 0x0009AF65, 0x0009420B,
+0x6E035403, 0xE5088544, 0x45186103, 0x31502159,
+0xBF258B03, 0xA007E501, 0x85410009, 0x620DD52B,
+0x89012258, 0xE500BF1C, 0x480B6493, 0xD42865E3,
+0xE801D611, 0x2C82460B, 0x0009AF45, 0x7B0862F1,
+0x2F217201, 0xEE0362F1, 0x31E7612D, 0xAF2E8901,
+0x7F140009, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0xFE0368F6, 0x002018B8, 0x002028E4,
+0x002028EC, 0x00200AE0, 0x00200E2A, 0x002028B4,
+0x00200B62, 0x001E2130, 0x00202AD4, 0x00200AFE,
+0x001C3D30, 0x00202AD8, 0x002028C4, 0x00202034,
+0x001C3D00, 0x00202AE4, 0x00202AF0, 0x002029D4,
+0x00202A54, 0x00202900, 0x002028BC, 0x001E212C,
+0x00202ADC, 0x00202AE0, 0x00200E8A, 0x00008000,
+0x00202AEC, 0x4F222FE6, 0x6E22D20D, 0xC84060E3,
+0x22E28D02, 0x0009BE7A, 0x4218E240, 0x89012E28,
+0x0009BE76, 0xC80560E3, 0xBECB8901, 0x60E30009,
+0x8902C802, 0xAE734F26, 0x4F266EF6, 0x6EF6000B,
+0x001C3510, 0x080A0C0E, 0x00020406, 0x1A1C1E20,
+0x12141618, 0x2E303234, 0x26282A2C, 0x3A3C3E40,
+0x6C625648, 0x41112F26, 0xE2208F18, 0x890B3123,
+0x321CD204, 0xD1026220, 0x412B312C, 0x00090009,
+0x0020205E, 0x00202014, 0x000BE000, 0x400062F6,
+0x40004000, 0x40004000, 0x40004000, 0x62F6000B,
+0x40004000, 0x40004000, 0x40004000, 0x40184000,
+0x62F6000B, 0x40004000, 0x40004000, 0x40004000,
+0x40284000, 0x62F6000B, 0x40004000, 0x40184000,
+0x000B4028, 0xC90F62F6, 0x40054005, 0x40054005,
+0x62F6000B, 0x4005C907, 0x40054005, 0x62F6000B,
+0x4005C903, 0x000B4005, 0xC90162F6, 0x000B4005,
+0x000062F6, 0x080A0C0E, 0x00020406, 0x1A1C1E20,
+0x12141618, 0x2E303234, 0x26282A2C, 0x3A3C3E40,
+0x6C625648, 0x41112F26, 0xE2208F18, 0x890B3123,
+0x321CD204, 0xD1026220, 0x412B312C, 0x00090009,
+0x0020211E, 0x002020D4, 0x000BE000, 0x400162F6,
+0x40014001, 0x40014001, 0x40014001, 0x62F6000B,
+0x40014001, 0x40014001, 0x40014001, 0x40194001,
+0x62F6000B, 0x40014001, 0x40014001, 0x40014001,
+0x40294001, 0x62F6000B, 0x40014001, 0x40194001,
+0x000B4029, 0x400462F6, 0x40044004, 0xC90F4004,
+0x62F6000B, 0x40044004, 0xC9074004, 0x62F6000B,
+0x40044004, 0x000BC903, 0x400462F6, 0x000BC901,
+0x000062F6, 0x3622E218, 0x67438F12, 0x0009A004,
+0x76FF6254, 0x74012420, 0xC8036053, 0x60438BF8,
+0x8902C803, 0x422BD22B, 0xD22B0009, 0x0009422B,
+0x2FE66473, 0x8D4A3450, 0x27786763, 0x62438947,
+0x227B225B, 0xC9016023, 0x8D203452, 0x2EE86E03,
+0x60238B15, 0x8B08C803, 0x47096643, 0x47106256,
+0x8FFB2622, 0xA0327604, 0x47010009, 0x61436673,
+0x46106255, 0x8FFB2121, 0xA0287102, 0x66430009,
+0x47106254, 0x8FFB2620, 0xA0207601, 0x61430009,
+0x2EE8357C, 0x8F15317C, 0x60236653, 0x8B07C803,
+0x76FC4709, 0x47106262, 0x21268FFB, 0x0009A00F,
+0x65634701, 0x75FE6673, 0x46106251, 0x21258FFB,
+0x0009A005, 0x626076FF, 0x8FFB4710, 0x60432124,
+0x6EF6000B, 0x00202306, 0x002027B2, 0xE21E2FE6,
+0x67633626, 0x8D1B6153, 0x3E106E43, 0x3E128916,
+0x65E38908, 0x3672E600, 0x62148910, 0x25207601,
+0x7501AFF9, 0x317C64E3, 0x6513347C, 0xE600A004,
+0x625075FF, 0x24247601, 0x8BF93672, 0x60E3A011,
+0x890831E2, 0x327C6213, 0x8B0432E6, 0x651364E3,
+0xA0086673, 0xD28F6EF6, 0x651364E3, 0x422B6673,
+0x000B6EF6, 0xE2046EF6, 0x67433622, 0x8F10356C,
+0xA004346C, 0x75FF0009, 0x76FF6250, 0x60532424,
+0x8BF8C803, 0xC8036043, 0xA1058901, 0xA2770009,
+0xA2990009, 0x2FB60009, 0x2FD62FC6, 0x7FE42FE6,
+0x6C636043, 0x66521F62, 0xC9037504, 0x1F516E53,
+0x45086503, 0xE1FC6D43, 0x2D194500, 0x1F732558,
+0x1F651F44, 0x2FD28D0B, 0x88086053, 0x88108923,
+0x8818895B, 0xA0898B01, 0xA0BD0009, 0x62630009,
+0x2D22E600, 0x7CFC7D04, 0xEB10A00D, 0xE60064E6,
+0x7CF065E6, 0x62E261E6, 0x1D512D42, 0x1D231D12,
+0x7E047D10, 0x3CB21FE1, 0x1F6589F0, 0x2FD21FC2,
+0xA0A11FE6, 0x64D21FD4, 0x44286263, 0x44294418,
+0x42184419, 0x4629242B, 0x2D424619, 0x65637D04,
+0xA0217CFD, 0x67E6EB10, 0x62E67CF0, 0x64E66673,
+0x256B4618, 0x2D5261E2, 0x65234729, 0x45184719,
+0x4229275B, 0x42191D71, 0x47186743, 0x4429227B,
+0x44196713, 0x247B4718, 0x1D431D22, 0x41194129,
+0x65137D10, 0x1FE17E04, 0x89DC3CB2, 0x1FE67EFF,
+0x1FC21F55, 0xA0672FD2, 0x6CF21FD4, 0x66C257F5,
+0x46286273, 0x42284629, 0x2C62262B, 0x7C045DF2,
+0x7DFE4729, 0xA01CEB10, 0x65E65EF1, 0x66E66273,
+0x47286753, 0x6763227B, 0x452961E6, 0x257B4728,
+0x2C2264E6, 0x65131C51, 0x45284629, 0x1C62265B,
+0x41296643, 0x216B4628, 0x44291C13, 0x67437C10,
+0x3DB27DF0, 0x1FD289E1, 0x7EFEA034, 0x51F56CF2,
+0x621366C2, 0x42284618, 0x42184619, 0x2C62262B,
+0x7C045DF2, 0x7DFF4119, 0xA01FEB10, 0x65E65EF1,
+0x64E67DF0, 0x42286253, 0x421867E6, 0x66E6212B,
+0x61432C12, 0x45194128, 0x251B4118, 0x65731C51,
+0x44194528, 0x245B4518, 0x64631C42, 0x47194428,
+0x274B4418, 0x46191C73, 0x61637C10, 0x89DE3DB2,
+0x7EFD1FD2, 0x1FC41FE6, 0x5DF2E704, 0xA00D5EF6,
+0x62E451F4, 0x66E47DFC, 0x65E464E4, 0x71012120,
+0x71012160, 0x71012140, 0x71012150, 0x89F03D72,
+0x66D357F3, 0x641365E3, 0x6EF67F1C, 0x6CF66DF6,
+0x6BF6A190, 0x00202194, 0x2FC62FB6, 0x2FE62FD6,
+0x60437FE4, 0x6C63C903, 0x66031F62, 0x460875FC,
+0x61526E43, 0x4600E2FC, 0x26682E29, 0x1F441F73,
+0x1F516D53, 0x8D0B1F15, 0x60632FE2, 0x891F8808,
+0x89538810, 0x8B018818, 0x0009A081, 0x0009A0B9,
+0xEB10A00D, 0x52D37DF0, 0x54D156D2, 0x2E1665D2,
+0x2E662E26, 0x2E427EFC, 0x1FD16153, 0x3CB27CF0,
+0x7D0489F0, 0x1F151FD6, 0x2FE21FC2, 0x1FE4A0A1,
+0x621366E2, 0x42294619, 0x42194618, 0x2E62262B,
+0x7CFF4118, 0xEB10A021, 0x54D37DF0, 0x624357D2,
+0x42194229, 0x55D1212B, 0x2E1666D2, 0x41296173,
+0x41194418, 0x2E46241B, 0x44296453, 0x44194718,
+0x2E76274B, 0x47296763, 0x47194518, 0x257B7EFC,
+0x46182E52, 0x1FD16163, 0x3CB27CF0, 0x7D0389DC,
+0x1F151FD6, 0x2FE21FC2, 0x1FE4A06B, 0x57F56EF2,
+0x627366E2, 0x46284629, 0x262B4229, 0x2E625CF2,
+0x7CFE4728, 0xA01BEB10, 0x7DF05DF1, 0x55D251D3,
+0x46296613, 0x54D1276B, 0x2E7662D2, 0x41286753,
+0x217B4729, 0x61432E16, 0x41294528, 0x2E56251B,
+0x44286523, 0x245B4529, 0x42282E46, 0x7CF06723,
+0x89E23CB2, 0x1FD67D02, 0xA03A1FC2, 0x67F21FE4,
+0x657251F5, 0x45296213, 0x45284519, 0x42194518,
+0x5CF2252B, 0x41282752, 0x7CFD4118, 0xA022EB10,
+0x7DF05DF1, 0x54D256D3, 0x45196563, 0x52D14628,
+0x4618215B, 0x6ED26543, 0x45192716, 0x265B4428,
+0x65436163, 0x45186423, 0x42284419, 0x4218254B,
+0x271664E3, 0x44196623, 0x264B2756, 0x4E282766,
+0x61E34E18, 0x3CB27CF0, 0x7D0189DB, 0x1FC21FD6,
+0xE7041F74, 0x51F45DF2, 0x5EF6A00D, 0x84E27EFC,
+0x620364E0, 0x7DFC84E1, 0x84E36503, 0x21646603,
+0x21542124, 0x3D722144, 0x57F389F0, 0x641366D3,
+0x7F1C65E3, 0x6DF66EF6, 0xA09D6CF6, 0x2F866BF6,
+0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6, 0x614374E0,
+0x6A636873, 0x6B56E920, 0x6C567AE0, 0x6D567120,
+0x6E563A92, 0x64566756, 0x62566656, 0x11C121B2,
+0x11E311D2, 0x11451174, 0x8DEC1166, 0x71201127,
+0x6613A004, 0x7AFF6254, 0x76012620, 0x8BF92AA8,
+0x6EF66083, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0x2F8668F6, 0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6,
+0x6A636873, 0x75E0E920, 0x56565257, 0x57545155,
+0x5D525E53, 0x6B525C51, 0x24662426, 0x24762416,
+0x7AE024E6, 0x24C624D6, 0x8DEC3A92, 0x66A324B6,
+0x6EF66783, 0x6CF66DF6, 0x6AF66BF6, 0xA04369F6,
+0x2FE668F6, 0xC8046063, 0x8D046E63, 0x62166153,
+0x24227EFC, 0x60E37404, 0x8908C818, 0x71046513,
+0x62526616, 0x24227EF8, 0xAFF41461, 0xE2047408,
+0x65133E22, 0x66E38D02, 0x6EF6A01C, 0x6EF6AF87,
+0xC8046063, 0x61638D04, 0x625275FC, 0x242671FC,
+0xC8186013, 0x75F88906, 0x66525251, 0x24662426,
+0x71F8AFF6, 0x3122E204, 0x66138F02, 0x0009AFA1,
+0x0009A00A, 0x0009A004, 0x76FF6254, 0x74012420,
+0x8BF92668, 0x6073000B, 0x0009A004, 0x625075FF,
+0x242476FF, 0x8BF92668, 0x6073000B, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x544F0D0A, 0x46205355, 0x00003A57, 0x2072614D,
+0x32203232, 0x20373030, 0x353A3731, 0x37333A32,
+0x00000000, 0x00000D0A, 0x00000043, 0x61766E49,
+0x2064696C, 0x72657375, 0x20726F20, 0x2079656B,
+0x00214449, 0x6E6B6E55, 0x206E776F, 0x6D6D6F63,
+0x3D646E61, 0x00000000, 0x61437748, 0x7262696C,
+0x6F697461, 0x6620206E, 0x0A6C6961, 0x0000000D,
+0x73696F4E, 0x61432065, 0x7262696C, 0x6F697461,
+0x6166206E, 0x21216C69, 0x00000D0A, 0x00000D0A,
+0x00000042, 0x000000FF, 0x00020001, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00020003, 0x01090108,
+0x0002010A, 0x00030002, 0x02020201, 0x02040203,
+0x02060205, 0x02080207, 0x020A0209, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00FF010F, 0x01090108,
+0x010B010A, 0x00030002, 0x02020201, 0x02040203,
+0x02060205, 0x02080207, 0x020A0209, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00000072, 0x00205220, 0x00000046,
+0x00000059, 0x73204142, 0x003D7165, 0x00000074,
+0x00000000, 0x02000112, 0x40FFFFFF, 0x12210ACE,
+0x20104890, 0x02090100, 0x0101002E, 0x09FA8000,
+0x04000004, 0x000000FF, 0x02010507, 0x07000200,
+0x00028205, 0x05070002, 0x00400383, 0x04050701,
+0x01004003, 0x002E0209, 0x80000101, 0x000409FA,
+0x00FF0400, 0x05070000, 0x00400201, 0x82050700,
+0x00004002, 0x03830507, 0x07010040, 0x40030405,
+0x03040100, 0x030C0409, 0x0079005A, 0x00410044,
+0x03180053, 0x00530055, 0x00320042, 0x0030002E,
+0x00570020, 0x0041004C, 0x0000004E, 0x00000000,
+0x00000000, 0x00000709, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, };
+
+const u32_t zcFwImageSize=11204;
diff --git a/drivers/staging/otus/hal/hpfwu_OTUS_RC.c b/drivers/staging/otus/hal/hpfwu_OTUS_RC.c
new file mode 100644
index 000000000000..089d3e0ad853
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwu_OTUS_RC.c
@@ -0,0 +1,715 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE287FFC, 0xE114D728,
+0x1E13D428, 0x1E4C470B, 0x0009B018, 0xA0039543,
+0x3652E600, 0x76018D04, 0xC84060E2, 0x2F028DF9,
+0xDE22D421, 0x00094E0B, 0x4E0BD421, 0xD4210009,
+0x00094E0B, 0x4F267F04, 0x6EF6A022, 0xD11E4F22,
+0x0009410B, 0x440BD41D, 0xD51D0009, 0x0009450B,
+0xE1FFD71C, 0xD21C611D, 0x50292712, 0xCB01E1FF,
+0xD61BD41A, 0x22121209, 0xE5011211, 0x2452E200,
+0xD5182622, 0x970FD618, 0x4F262572, 0x2620000B,
+0xDD17DC16, 0x4C0BDE17, 0x4D0B0009, 0x4E0B0009,
+0xAFF80009, 0x27100009, 0x00000640, 0x001C001C,
+0x002008EA, 0x0000B38E, 0x002028DC, 0x00200DA6,
+0x002028E8, 0x00202900, 0x00200C6C, 0x00200EA2,
+0x00200940, 0x001C3510, 0x001C3624, 0x001E212C,
+0x00202894, 0x0020288C, 0x002027F0, 0x00200B68,
+0x00201F74, 0x00201734, 0x2FD62FC6, 0x4F222FE6,
+0xDEA17FA4, 0x61E0E01C, 0x7D016DE3, 0x61D00F14,
+0xD59FD49E, 0x450BE020, 0xE0200F14, 0xE78004FC,
+0x604C66E2, 0x7D7F677C, 0x1F693070, 0x2D628F17,
+0x01FCE01C, 0x641CE500, 0xD797DE96, 0x3243625D,
+0xA21A8B01, 0x655D0009, 0x31EC6153, 0xE0286C10,
+0x6D530FC4, 0x3D7C62CE, 0xAFEF2D20, 0x20087501,
+0xE01C8B15, 0xE50001FC, 0xD78BDE8A, 0x641CA00A,
+0x6C53655D, 0x66C23CEC, 0x66626253, 0x2262327C,
+0x1F697504, 0x3243625D, 0xA1F68BF2, 0x88012D10,
+0xE01C8B16, 0xE40001FC, 0x671C2D40, 0x624DDE7D,
+0x8B013273, 0x0009A1E9, 0x62E3644D, 0x72046D43,
+0x3DEC6143, 0x65D2312C, 0x74086C12, 0x25C2AFEF,
+0x8B188804, 0x01FCE01C, 0x2D40E400, 0xDE71671C,
+0x3273624D, 0xA1D08B01, 0x644D0009, 0x62E36D43,
+0x65D23DEC, 0x61437204, 0x6612312C, 0x74086C52,
+0xAFED2C69, 0x880525C2, 0xE01C8B18, 0xE40001FC,
+0x671C2D40, 0x624DDE63, 0x8B013273, 0x0009A1B5,
+0x6C43644D, 0x3CEC62E3, 0x720465C2, 0x3D2C6D43,
+0x615266D2, 0x216B7408, 0x2512AFED, 0x8B138830,
+0xE200DE58, 0x64E22D20, 0x8B042448, 0x420BD257,
+0xA19A0009, 0x55E10009, 0x57E356E2, 0xDD545CE4,
+0x2FC64D0B, 0x7F04A191, 0x89018828, 0x0009A0EA,
+0xE143DE4C, 0x622D62E1, 0x8F033217, 0x56FB1FEB,
+0x2621E240, 0x8B013217, 0x0009A0D5, 0xE1015EFB,
+0x301685E1, 0xA0CE8B01, 0xE4010009, 0x2D4055FB,
+0x6451B179, 0xE14357FB, 0xE0546271, 0x3517652D,
+0x0F568D41, 0x3563E640, 0xE6008B05, 0x0F65E034,
+0xA00FE11A, 0x615372C0, 0x41214121, 0x41214121,
+0x45214121, 0x45214521, 0xC9036053, 0xE0346603,
+0x71180F65, 0x2209E007, 0x641DE030, 0x0F2565F3,
+0x1F4EB1F1, 0x04FDE034, 0x674DE030, 0x47080CFD,
+0x607361CD, 0x4108D22B, 0xE00F0CFE, 0x1F1F420B,
+0x2CD96D07, 0x5EFB6073, 0x85E20FC6, 0x420B51FF,
+0x2C0B600D, 0x54FE6073, 0xB1BB0FC6, 0xE05465F3,
+0x652D62E1, 0xE6400F56, 0x89623563, 0xE050E100,
+0x60230F15, 0x4008C903, 0x6D034000, 0xE0406103,
+0xE0440FD6, 0xD217EEFF, 0x6EEC0FF6, 0x0F26E058,
+0x60E3420B, 0x42216253, 0x42214221, 0x66234221,
+0x326C4200, 0x45214200, 0xE0486707, 0x0F764521,
+0xC9036053, 0x40085CFB, 0x7C0630FC, 0x6E036D2D,
+0x1FD51FC6, 0x1F04A02E, 0x00117D00, 0x00202904,
+0x00200DA6, 0x00117D04, 0x00117D84, 0x00200700,
+0x0020074C, 0x00201FD4, 0x0FD6E04C, 0x05FEE044,
+0x64D3B189, 0x64E2E048, 0xE04006FE, 0x2E422469,
+0x01FE67C4, 0x667CE058, 0x420B02FE, 0x240B6063,
+0x05FEE044, 0xB15D2E42, 0xE05064D3, 0x7D0101FD,
+0x0F157101, 0x02FDE050, 0x3262E606, 0x56FB8BDC,
+0x55FB6261, 0x85514200, 0x302C750C, 0x6103701B,
+0x64F3E600, 0xE704A004, 0x76016256, 0x74042422,
+0x3273626D, 0x65F38BF8, 0x641DB13C, 0xB0D256FB,
+0xA0AA6461, 0xD4880009, 0xE201D588, 0x2D20450B,
+0x0009A0A3, 0x8B078829, 0xE200DE85, 0x66E22D20,
+0x646DB0A1, 0x0009A099, 0x622CE281, 0x8B3D3020,
+0xD680E738, 0xE0442D70, 0xE0480C6E, 0x6E621DC1,
+0x51611DE2, 0x54621D13, 0x55651D44, 0x57631D55,
+0x5C661D76, 0x0E6E1DC7, 0x1DE8E040, 0xE050016E,
+0x54641D19, 0x056E1D4A, 0x1D5BE04C, 0xE054076E,
+0x0C6E1D7C, 0x1DCDE058, 0xE044026E, 0xED001D2E,
+0xE04806D6, 0x16D126D2, 0x16D516D2, 0x16D616D3,
+0xE04006D6, 0xE05006D6, 0x06D616D4, 0x06D6E04C,
+0x06D6E054, 0x06D6E058, 0x1F29A057, 0x622CE282,
+0x89313020, 0x05FCE020, 0x625CE683, 0x3260666C,
+0xD65D8B07, 0x2650E500, 0x52617680, 0xA044D65B,
+0xE6902622, 0x3260666C, 0xD2578B16, 0xE500D658,
+0x60622250, 0xCB20D257, 0xE6052602, 0xD6562262,
+0x2252460B, 0x420BD255, 0xD2550009, 0x2262E601,
+0x4618D254, 0x2262A029, 0xD254D453, 0xD4546542,
+0x0009420B, 0x0009A021, 0xE524D647, 0xD5452650,
+0x16215257, 0x16225258, 0x16235259, 0x1624525A,
+0x1625525B, 0x1626525C, 0x1627525D, 0x1628525E,
+0x1F29525F, 0xE2001629, 0x15281527, 0x152A1529,
+0x152C152B, 0x152E152D, 0x7F5C152F, 0x6EF64F26,
+0x000B6DF6, 0x4F226CF6, 0xE240614D, 0x89173123,
+0x3127E21F, 0xD43B8908, 0xE001D53B, 0x6642450B,
+0x26796707, 0x2462A00C, 0x3127E23F, 0xD7358908,
+0x71E0D635, 0x460BE001, 0x62075571, 0x17512529,
+0x000B4F26, 0x4F220009, 0xE240614D, 0x89153123,
+0x3127E21F, 0xD42B8907, 0x6642D22B, 0xE001420B,
+0xA00B260B, 0xE23F2462, 0x89073127, 0xD626D725,
+0x71E05571, 0xE001460B, 0x1751250B, 0x000B4F26,
+0xE6400009, 0x46284618, 0x6252D520, 0x89FC2268,
+0x0009000B, 0x4618E680, 0xD51C4628, 0x22686252,
+0x000B89FC, 0xA0010009, 0x7201E200, 0x8BFC3242,
+0x0009000B, 0x4618E680, 0xD5154628, 0x22686252,
+0x000B8BFC, 0x00000009, 0x00202908, 0x00200DA6,
+0x00117D04, 0x002027F8, 0x00117D80, 0x0020288C,
+0x001C3500, 0x001D4004, 0x00200EA2, 0x00200940,
+0x001E212C, 0x001C3D28, 0x00117D00, 0x00200E06,
+0x00202920, 0x001C3704, 0x00201FD4, 0x001C373C,
+0x001C3700, 0x4F222FE6, 0x6E537FFC, 0x2F42BFCA,
+0xD61561E2, 0x1615E280, 0x421854E1, 0x55E21646,
+0x16574228, 0x6EF257E3, 0x2E2B1678, 0x7F0426E2,
+0xAFA74F26, 0x2FC66EF6, 0x2FE62FD6, 0xDD0A4F22,
+0xBFAF6C53, 0xBF946E43, 0xBFAB2DE2, 0x51D50009,
+0x54D62C12, 0x55D71C41, 0x56D81C52, 0x4F261C63,
+0x6DF66EF6, 0x6CF6000B, 0x001C370C, 0x0009A0C0,
+0xD17B4F22, 0xD47B92B6, 0x2122B00D, 0x97B2E605,
+0xB02295B2, 0xB0366463, 0xB0360009, 0xB0390009,
+0xA0680009, 0x4F124F26, 0xD1734F02, 0x94A43145,
+0x4609060A, 0x46094609, 0x00293646, 0xD76CD56F,
+0x2500CA01, 0x4F062762, 0x4F16000B, 0xBFEA4F22,
+0xB01F0009, 0xA04E0009, 0x2FE64F26, 0x6E63D168,
+0x44186612, 0x4528928A, 0x26294408, 0x44084500,
+0x4400265B, 0x4708264B, 0x47082162, 0x27EBD161,
+0x000B2172, 0x000B6EF6, 0xD25F0009, 0xE40A9677,
+0x2262AFB4, 0x2FC62FB6, 0x2FE62FD6, 0xDC5B4F22,
+0x2C22E201, 0xBFA9E40A, 0x60C27C44, 0xCB01ED00,
+0x60C22C02, 0xC901EB64, 0x6E03A008, 0x89073DB2,
+0xE40160C2, 0xBF99C901, 0x7D016E03, 0x8BF52EE8,
+0x8B033DB2, 0xD24FD44E, 0x0009420B, 0x4F26E40A,
+0x6DF66EF6, 0xAF896CF6, 0x44116BF6, 0x604B8F01,
+0x000B6043, 0x2FB60009, 0x2FD62FC6, 0x4F222FE6,
+0xDC457FFC, 0x60C2ED00, 0xCB02EB64, 0x60C22C02,
+0xC9022F02, 0x6E03A009, 0x89083DB3, 0xE46460C2,
+0xC9022F02, 0x6E03BF6A, 0x2EE87D01, 0xD73B8BF4,
+0x617251C1, 0xDE3BDC3A, 0xD23CD13B, 0x64C23DB3,
+0x651264E2, 0x65228F09, 0xD232D439, 0x4F267F04,
+0x6DF66EF6, 0x422B6CF6, 0x7F046BF6, 0x6EF64F26,
+0x6CF66DF6, 0x6BF6000B, 0x5651D532, 0x46286052,
+0x306C000B, 0x5288096C, 0x09B45BB4, 0x03C41FFF,
+0x2FC62FB6, 0x2FE62FD6, 0x4F124F22, 0xBFEB4F02,
+0x6B036E43, 0xDD18DC28, 0x0009BFE6, 0x3C0530B8,
+0x4609060A, 0x46014609, 0x020A3D65, 0x42094209,
+0x32E24209, 0x4F068BF0, 0x4F264F16, 0x6DF66EF6,
+0x000B6CF6, 0x2FE66BF6, 0xDE1C4F22, 0xE500E102,
+0x2E12E403, 0x2E52BFD4, 0x4618E606, 0xE403E700,
+0x2E722E62, 0xAFCB4F26, 0x000B6EF6, 0x00000009,
+0x00202890, 0x0024CDE0, 0x10624DD3, 0x00202A8C,
+0x001C5814, 0x001C59D0, 0x001C5804, 0x001C581C,
+0x00202934, 0x00200DA6, 0x001C5860, 0x001C6864,
+0x001C7864, 0x001C59BC, 0x001C69BC, 0x001C79BC,
+0x0020294C, 0x001C1040, 0xCCCCCCCD, 0x001D4004,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xE4007FE4, 0x4528E510, 0x67436C43, 0xE107A00F,
+0x6043644D, 0x0F564008, 0xEE0060C3, 0x815125C1,
+0x81538152, 0x157315E2, 0x751415E4, 0x624D7401,
+0x8BED3213, 0xDA6F51F1, 0x1A1154F2, 0xD16E2A12,
+0x57F455F3, 0x6DF258F5, 0x1141D96C, 0x11532142,
+0x11751152, 0x11871174, 0x52F61186, 0x19D1D668,
+0xD86829D2, 0xDA68E950, 0x1621EBB4, 0x6BBC2622,
+0xA0214908, 0x6EEDEE00, 0x61E36DE3, 0x41084D08,
+0x31EC3DEC, 0x41084D08, 0x60C33D8C, 0xE7904108,
+0x81D12DC1, 0x41086093, 0x81D2677C, 0x31AC60C3,
+0x3472E200, 0x1DD281D3, 0xD4551D13, 0x1D248D01,
+0xB03AD450, 0x7E0165D3, 0x34B264ED, 0xD14D8BDB,
+0x6512DB52, 0x4529D24D, 0x64121B51, 0x674DD14A,
+0x67222B72, 0x4729D64E, 0x69221B73, 0x689D2FD2,
+0x69121B82, 0x5A122692, 0x5B1416A2, 0x16B4DA44,
+0x16C65C16, 0x16EA6EA2, 0x4F267F1C, 0x6DF66EF6,
+0x6BF66CF6, 0x69F66AF6, 0x68F6000B, 0x60616642,
+0x8D04C803, 0x6061E500, 0x8802C903, 0x52628B03,
+0x51246563, 0x000B2412, 0x2FD66053, 0x4F222FE6,
+0x6E537FEC, 0xE5506253, 0xE4006D43, 0xA0014508,
+0x5224E101, 0x22116043, 0x81238121, 0x81226053,
+0x362056E2, 0xD22F8BF5, 0x64F316E4, 0x420BE614,
+0x65E165E3, 0x2549E4FC, 0x61F12E51, 0x214965F3,
+0x54D12F11, 0x410BD127, 0x57D1E614, 0xCB016071,
+0x1DE12701, 0x4F267F14, 0x000B6EF6, 0x2FD66DF6,
+0x4F222FE6, 0x6E537FEC, 0xE5FC6653, 0x60616D43,
+0xCB012059, 0x52E22601, 0x8B063260, 0x51E212E4,
+0x8B0431E0, 0xA00252D1, 0xAFF01E22, 0xD2155664,
+0xE61464F3, 0x65E3420B, 0xE1FC67E1, 0x2E712719,
+0x54D167F1, 0xD10F2719, 0xE61465F3, 0x2F71410B,
+0x602152D1, 0x2201CB01, 0x7F141DE1, 0x6EF64F26,
+0x6DF6000B, 0x0020285C, 0x00202864, 0x00202854,
+0x00202884, 0x0010008C, 0x00100EC0, 0x001E2108,
+0x001C3D00, 0x00202134, 0x2FC62FB6, 0x2FE62FD6,
+0xD6314F22, 0x60D36D62, 0x894DC803, 0xDB30DC2F,
+0x0009A02C, 0xC9036061, 0x892B8801, 0xD22DD42B,
+0x0009420B, 0x65035603, 0xC8208561, 0xE0508903,
+0x720102BE, 0x85620B26, 0x4000600D, 0x4000366A,
+0x40004624, 0x206D4624, 0xD423C903, 0x40086E03,
+0xD1224000, 0x340C410B, 0x61E3D521, 0xD721E001,
+0x450BD221, 0x64E37E30, 0x2702420B, 0x66C252C1,
+0x8BCF3620, 0x4E18EE01, 0xA011DB1C, 0x6061EC75,
+0x8801C903, 0xD4198910, 0x460BD612, 0xD4180009,
+0x470BD718, 0xD2136503, 0x64C3D113, 0x22E2410B,
+0x66B252B1, 0x8BEA3620, 0xC80460D3, 0xD2128906,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6422B, 0x6EF64F26,
+0x6CF66DF6, 0x6BF6000B, 0x001E2100, 0x0020285C,
+0x002027F8, 0x00200A5C, 0x00202864, 0x00200ADE,
+0x00201FD4, 0x001C3D30, 0x00200D6C, 0x00202854,
+0x00202884, 0x00200A7A, 0x002000F8, 0xE601D237,
+0x1265D537, 0x000B2252, 0xD6361266, 0x88016062,
+0xE1018B62, 0xD5342612, 0x5451D134, 0xE0406212,
+0x2122324C, 0x54115752, 0x1141347C, 0x57125453,
+0x1172374C, 0x52135755, 0x1123327C, 0x56146452,
+0x1164364C, 0x54155754, 0x1145347C, 0x56165458,
+0x1166364C, 0x6762D626, 0x327C5217, 0x57611127,
+0x327C5218, 0x57621128, 0x327C5219, 0x57631129,
+0x347C541A, 0x5764114A, 0x347C541B, 0x5765114B,
+0x347C541C, 0x5266114C, 0x372C571D, 0x5267117D,
+0x342C541E, 0x5268114E, 0x362C561F, 0xD615116F,
+0x041E6262, 0x342C7694, 0xE0440146, 0x061E6262,
+0x0166362C, 0x525CE048, 0xD60F051E, 0x0156352C,
+0xE0546262, 0x4229051E, 0x0156352C, 0xE0585561,
+0x4529061E, 0x0166365C, 0x0009000B, 0x001C1010,
+0x0000C34F, 0x001C1028, 0x001C369C, 0x002027F8,
+0x001C3CA0, 0x001C36F4, 0x001C3B88, 0xD62F7FFC,
+0x2642644C, 0xC8205066, 0x2F028DFC, 0x7F04000B,
+0x2FD62FC6, 0x4F222FE6, 0x6D436C53, 0xEE00A004,
+0x7E0164D4, 0x644CBFEA, 0x8BF93EC2, 0x6EF64F26,
+0x000B6DF6, 0xA0016CF6, 0x76016643, 0x22286260,
+0x36488BFB, 0x6563AFE4, 0x2FB62F96, 0x2FD62FC6,
+0x4F222FE6, 0xEC1CED08, 0xDB196E53, 0x61C3E90A,
+0x60434B0B, 0x3092C90F, 0x66038D02, 0x7630A001,
+0x4D107637, 0x7E012E60, 0x7CFC8FF1, 0x8058E000,
+0x6EF64F26, 0x6CF66DF6, 0x000B6BF6, 0x000B69F6,
+0x000BE000, 0x2FE6E000, 0x7FEC4F22, 0x6E436253,
+0xBFD165F3, 0xBFC66423, 0xBFC464E3, 0xD40564F3,
+0x0009BFC1, 0x4F267F14, 0x6EF6000B, 0x001C0004,
+0x00202094, 0x00202968, 0xE110D59C, 0xE6406050,
+0x2500C9FD, 0xE0FF75E9, 0x80516453, 0x80538052,
+0x80568055, 0x251075EF, 0xE1EF6250, 0x2219E001,
+0xE7202520, 0x24608052, 0x2570000B, 0xE4FDD590,
+0xE7026152, 0x25122149, 0x74016052, 0x2502CB01,
+0xD18C6652, 0x25622649, 0x92C26012, 0x2102CB08,
+0xC9CF6012, 0x60122102, 0x2102CB03, 0x000B1172,
+0x4F221123, 0xE100D484, 0xD285D784, 0xD5852410,
+0x2711D485, 0x2211E700, 0xBFBD2511, 0xD5832471,
+0x2560E600, 0x4F26AFD2, 0xD281664C, 0x362C4600,
+0xCB106060, 0x2600000B, 0xD27D654C, 0x352C4500,
+0xE1EF6650, 0x000B2619, 0x664C2560, 0x4600D279,
+0x6060362C, 0x000BCB10, 0x654C2600, 0x4500D275,
+0x6650352C, 0x2619E1EF, 0x2560000B, 0xD270664C,
+0x362C4600, 0xCB086060, 0x2600000B, 0xD26C654C,
+0x352C4500, 0xE1F76650, 0x000B2619, 0x664C2560,
+0x4600D268, 0x6060362C, 0x000BCB08, 0x654C2600,
+0x4500D264, 0x6650352C, 0x2619E1F7, 0x2560000B,
+0xD65F624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0x624C600C, 0x4200D65A, 0x6020326C,
+0x4021C908, 0x40214021, 0x600C000B, 0xD156644C,
+0x341C74FF, 0x000B6240, 0xD154602C, 0x341C644C,
+0x000B6240, 0x2FE6602C, 0x655C4F22, 0x3567E60A,
+0x6E438D15, 0x6453BFEA, 0x60EC640C, 0x8B028801,
+0xA002E00F, 0x44092409, 0x624C4409, 0x3263E60A,
+0xBFE28905, 0x620C644C, 0xC8806023, 0xE2008B00,
+0x4F266023, 0x6EF6000B, 0xD6414F22, 0x88016062,
+0xB2228B03, 0xA0030009, 0xD23E0009, 0x2260E640,
+0xE200D63D, 0x000B4F26, 0x4F222622, 0x6062D638,
+0x8B018802, 0x0009B26C, 0xE200D637, 0x000B4F26,
+0x0FFF2622, 0xD433D532, 0xE701E100, 0x000B2512,
+0xD2302470, 0x000BE604, 0xD5202260, 0x6150E4FD,
+0x2149D62E, 0x2510E700, 0x2670000B, 0xE4FBD51B,
+0x22496250, 0x2520000B, 0xE4F7D518, 0x22496250,
+0x2520000B, 0xD2264F22, 0x600D8522, 0x89112008,
+0x89138801, 0x89158803, 0x89178805, 0x89418806,
+0x89478808, 0x894D8809, 0x8953880A, 0x8959880B,
+0x0009A060, 0x0009B062, 0x600CA05D, 0x0009B070,
+0x600CA059, 0x0009B07A, 0x600CA055, 0x6260D606,
+0x8B4F2228, 0x0009B086, 0x600CA04D, 0x001E1028,
+0x001E2148, 0x001E1108, 0x002028D9, 0x002028C8,
+0x002028CA, 0x002028CC, 0x002028AC, 0x001E1008,
+0x001E103F, 0x001E105F, 0x001E1030, 0x001E1090,
+0x002028D4, 0x001E100B, 0x002028D0, 0x002028D8,
+0x002028A0, 0x6260D687, 0x8B232228, 0x0009B06A,
+0x600CA021, 0x6260D683, 0x8B1B2228, 0x0009B0B4,
+0x600CA019, 0x6260D67F, 0x8B132228, 0x0009B0BA,
+0x600CA011, 0x6260D67B, 0x8B0B2228, 0x0009B11E,
+0x600CA009, 0x6260D677, 0x8B032228, 0x0009B136,
+0x600CA001, 0x4F26E000, 0x0009000B, 0xD273D172,
+0xD5738412, 0x4000C90F, 0xD772012D, 0x611CE403,
+0xD671E20F, 0x27122540, 0xE0012520, 0x2602000B,
+0xE601D269, 0x30668523, 0xE0008D06, 0xE000D267,
+0x8122D669, 0x2602E001, 0x0009000B, 0x8523D262,
+0x2008600D, 0x88018905, 0xD6648B0A, 0xCB016060,
+0xD6612600, 0xE101D45D, 0x2612E001, 0x8142000B,
+0xE000000B, 0xE501D158, 0x45188513, 0x3453640D,
+0x8D056603, 0xD25AE000, 0xE001D557, 0x25022260,
+0x0009000B, 0xD1504F22, 0x650D8513, 0x44196453,
+0x672E6249, 0x602C227D, 0x89098801, 0x890C8802,
+0x89108803, 0x89268806, 0x89298807, 0x0009A038,
+0xD64DD54C, 0xA027E212, 0x625C2652, 0x8B2F2228,
+0xA01ED64A, 0x605C6262, 0x89052008, 0x89088810,
+0x890B8820, 0x0009A024, 0xD643D445, 0xA013E204,
+0xD7442642, 0xE20CD640, 0x2672A00E, 0xD63ED542,
+0xA009E218, 0xD4412652, 0xE20AD63B, 0x2642A004,
+0xD639D23F, 0xE22E2622, 0xD43E8515, 0x3277670D,
+0x8F012421, 0x24516503, 0x0009B0DF, 0xE001A001,
+0x4F26E000, 0x0009000B, 0xE101D629, 0x2610D436,
+0xD7286541, 0x655DD128, 0xE001E20F, 0x26202752,
+0x2102000B, 0x4F222FE6, 0x8523D21F, 0x2448640C,
+0xD62D8B08, 0xE200D521, 0x84512621, 0x20499430,
+0x8051A026, 0x60E0DE1D, 0x8D0BC840, 0x3427E201,
+0xD1258922, 0x420BD225, 0xD5252141, 0xCB046052,
+0x2502A00B, 0x89173427, 0xD722D21F, 0x2241470B,
+0xE5FBD61F, 0x21596162, 0x84E12612, 0xB12DCB80,
+0x60E080E1, 0xCB04D61C, 0x60602E00, 0x2600C93F,
+0xE001D609, 0x2602A001, 0x4F26E000, 0x6EF6000B,
+0x0000FF7F, 0x002028D9, 0x002028A0, 0x002028AC,
+0x001E1100, 0x001E100C, 0x002028D0, 0x001E1000,
+0x001E1001, 0x00202A90, 0x002028B4, 0x002028BC,
+0x00202AFE, 0x00202B02, 0x00202B0E, 0x00202B26,
+0x00202B30, 0x002028B8, 0x002028C6, 0x00201A32,
+0x001E1108, 0x00201B3E, 0x001E1015, 0x6060D696,
+0x8919C880, 0x6021D295, 0x8B158801, 0xE501D294,
+0x30568524, 0xD1938910, 0xD493E203, 0x65412120,
+0x655DE00B, 0xD5910656, 0xE702E40F, 0x25712140,
+0xE001D78F, 0x2702000B, 0xE000000B, 0x4F222FE6,
+0x84E1DE8C, 0x8934C880, 0x8554D585, 0x8F302008,
+0xD7896103, 0x66728553, 0x650C6403, 0x620C8566,
+0x8B263520, 0xD780D685, 0x644C651C, 0x27412651,
+0xC84060E0, 0xD2828907, 0x0009420B, 0x6062D681,
+0xA008CB04, 0xD1802602, 0x0009410B, 0xE5FBD67D,
+0x24596462, 0xB0A12642, 0xD5750009, 0x2522E201,
+0xD77A60E0, 0x2E00CB04, 0xC93F6070, 0xA0012700,
+0xE0006023, 0x000B4F26, 0x2FA66EF6, 0x2FC62FB6,
+0x2FE62FD6, 0xE240DA69, 0xDC6666A1, 0x3123616D,
+0x62638900, 0x6ED36D2C, 0x4E2136D8, 0x4E212A61,
+0xDB6CD46B, 0xE700A00F, 0x770166B2, 0x71026163,
+0x65612B12, 0x71026613, 0x62612B12, 0x622D655D,
+0x325C4228, 0x627C2422, 0x8BED32E3, 0xC90360D3,
+0x8B108803, 0xED076EB2, 0x710261E3, 0x67132B12,
+0x62E17102, 0x65712B12, 0x655D622D, 0x352C4528,
+0xA00C2CD0, 0x88022452, 0xA0038B01, 0x8801E203,
+0xE2018B05, 0x66B22C20, 0x677D6761, 0xEB0F2472,
+0x6DA12CB0, 0x8B052DD8, 0xD445D24F, 0xE101EE00,
+0x241222E2, 0x6DF66EF6, 0x6BF66CF6, 0x6AF6000B,
+0x2FE62FD6, 0xE240DD3D, 0x616D66D1, 0x89003123,
+0x672C6263, 0xDE433678, 0x2D617703, 0xD6404721,
+0x472164E2, 0xE100A00E, 0x71016562, 0x24506253,
+0x42197401, 0x74012420, 0x24504529, 0x45197401,
+0x74012450, 0x3273621C, 0x42008BEE, 0x64D166E2,
+0x362C4200, 0x8F062448, 0xDD332E62, 0xE500DE28,
+0x2D52E701, 0x6EF62E72, 0x6DF6000B, 0x2FE62FD6,
+0xEE014F22, 0xED0AA005, 0x64E3BCB6, 0x64E3BCBC,
+0x62EC7E01, 0x8BF732D7, 0xEE01A005, 0x64E3BCBD,
+0x64E3BCC3, 0x62EC7E01, 0x8BF732D7, 0x6EF64F26,
+0x6DF6000B, 0x2FE62FD6, 0x7FFC4F22, 0x6060D61F,
+0x89758801, 0xE101D41E, 0xD7128548, 0x650D2610,
+0x45196070, 0x6659DD1B, 0x61D3626E, 0xC840262D,
+0x74027102, 0x8D47D718, 0xD218666C, 0xE501DE0A,
+0xA0312E22, 0x0000EE04, 0x001E1001, 0x002028C6,
+0x002028A0, 0x001E1100, 0x002028CA, 0x002028B8,
+0x002028D0, 0x001E1000, 0x002028BC, 0x002028C8,
+0x00201A32, 0x001E1108, 0x00201B3E, 0x001E1015,
+0x001E100C, 0x002028B4, 0x002028D4, 0x002028D8,
+0x00202A90, 0x00202B26, 0x00202B32, 0x00202AA2,
+0x75016245, 0x71022121, 0x32E3625C, 0x60638BF8,
+0xE60181D4, 0xE417D538, 0x3243626C, 0x6255891E,
+0x27217601, 0x7702AFF8, 0xDE35D234, 0x2E22E501,
+0xEE04A004, 0x75016245, 0x71022121, 0x32E3625C,
+0x60638BF8, 0xE60181D4, 0xA004D52E, 0x6255E417,
+0x27217601, 0x626C7702, 0x8BF83243, 0x2D21924B,
+0xD72AD429, 0x2F126142, 0x6DF265F2, 0xC9806053,
+0x60532700, 0x6103C960, 0x60538071, 0x65F26EF2,
+0x4D19C903, 0x80724529, 0x451960DC, 0x4E298172,
+0x62EC605C, 0x302C4018, 0x6D428173, 0x2FD22118,
+0x62F26EF2, 0x421966F2, 0x656C4629, 0x602C66F2,
+0x401864EC, 0x304C4629, 0x81744619, 0x4018606C,
+0x8F07305C, 0xBCB58175, 0x620C0009, 0x89082228,
+0x0009A00A, 0x88406013, 0xB00A8B03, 0xA0030009,
+0xD60B0009, 0x2622E202, 0x4F267F04, 0x000B6EF6,
+0x000B6DF6, 0x060A0009, 0x00202AD2, 0x00202AD0,
+0x002028BC, 0x00202AA4, 0x001E100C, 0x002028A0,
+0x002028D0, 0x7FFC4F22, 0x6620D27E, 0x8D082668,
+0xD47D2F60, 0x420BD27D, 0x64F00009, 0xA0907F04,
+0x7F044F26, 0x000B4F26, 0x000B0009, 0x2FE60009,
+0xDE774F22, 0x60E0D677, 0xCBC0D477, 0x62602E00,
+0xC803602C, 0x40218904, 0x70014021, 0x6603A002,
+0x66034009, 0xD671616D, 0xE500A004, 0x75016262,
+0x74042422, 0x3213625D, 0xD16D8BF8, 0x0009410B,
+0xE401D66C, 0x84E22641, 0x80E2C9BF, 0x000B4F26,
+0x2FE66EF6, 0xD5687FFC, 0x6250DE61, 0x642C84E2,
+0xCB407404, 0x80E2614D, 0x44216413, 0xD7634421,
+0xE600A004, 0x76016256, 0x27222F22, 0x3243626D,
+0x60138BF8, 0x2008C903, 0x88038912, 0x88028905,
+0x88018906, 0xA0088907, 0xE0070009, 0x8078A005,
+0xA002E003, 0xE0018078, 0x62528078, 0x27222F22,
+0xD650E00F, 0x60618078, 0x8B018801, 0x2621E200,
+0x6060D64F, 0x2600CB08, 0xC93F60E0, 0x7F042E00,
+0x6EF6000B, 0x6021D247, 0x8D188801, 0xD2466143,
+0x22106053, 0x60638021, 0xD4468121, 0xE500A007,
+0x027C605D, 0x364C6603, 0x26207001, 0x625D6503,
+0x3213611C, 0xD6408BF4, 0xC9BF6060, 0x000B2600,
+0x2FD60009, 0x4F222FE6, 0x60437FFC, 0x8D02C820,
+0xBF6A6E43, 0x60E30009, 0x8901C810, 0x0009BF67,
+0xC84060E3, 0xBF8C8901, 0x60E30009, 0x8929C801,
+0x60D0DD32, 0x8D03C802, 0xD6312F00, 0x0009460B,
+0xC80460F0, 0xD62F8902, 0x0009460B, 0x602362F0,
+0x8902C880, 0xC97F60D0, 0x60232D00, 0x8902C801,
+0x420BD229, 0xD5290009, 0x88026052, 0xD2288B03,
+0xA005E604, 0x88012260, 0xD2258B02, 0x2260E601,
+0x2522E200, 0xC88060E3, 0xD2228916, 0x60E36E20,
+0x8902C802, 0x420BD220, 0x60E30009, 0x8902C804,
+0x420BD21E, 0x60E30009, 0x8905C808, 0x7F04D21C,
+0x6EF64F26, 0x6DF6422B, 0x4F267F04, 0x000B6EF6,
+0x00006DF6, 0x001E1020, 0x0020296C, 0x00200DA6,
+0x001E1015, 0x001E10BF, 0x00117D00, 0x001E10FC,
+0x002000F8, 0x002028CC, 0x00117D80, 0x001E10F8,
+0x001E10AE, 0x00117D84, 0x001E1017, 0x001E1021,
+0x00200FD8, 0x00200FFA, 0x00201584, 0x002028D0,
+0x001E100B, 0x001E1028, 0x0020102A, 0x0020103C,
+0x00201048, 0xD6A8644C, 0x346C74FF, 0x2450000B,
+0x644CD6A6, 0x000B346C, 0xD6A52450, 0x346C644C,
+0x2450000B, 0x616D625C, 0x41194208, 0x60194208,
+0x644C4200, 0x324C670E, 0x207DD19E, 0xC90F4200,
+0x000B321C, 0x67632200, 0x4208625C, 0x42004208,
+0x324C644C, 0x4200D198, 0x000B321C, 0x2FE62270,
+0x614C4F12, 0x4100D493, 0x6710314C, 0x2729E29F,
+0x65736E53, 0x4719676D, 0x672E6279, 0x4221227D,
+0x42214221, 0x7601662C, 0xE4014608, 0x34E84608,
+0x644C4600, 0x0E1A0467, 0x215025EB, 0x000B4F16,
+0x4F226EF6, 0xD2857FE8, 0x88016021, 0xD2848B7B,
+0x26686621, 0xD2838B77, 0x26686621, 0xE50F8B73,
+0xE401BFA0, 0xBFA3E501, 0xE586E400, 0xE400655C,
+0x2F50BFA3, 0xBFA0E401, 0xE602E506, 0x60634618,
+0x81F2E401, 0x6543BF9E, 0xE40185F2, 0xBFAA6543,
+0x85F26603, 0x6543E401, 0x6603BFB1, 0xE40265F0,
+0x6053756C, 0x80F8BF7E, 0xBF81E402, 0x84F8E512,
+0x7090E402, 0x6503BF81, 0x4618E602, 0x81F66063,
+0xBF7FE402, 0x85F6E500, 0x6603E402, 0xE500BF8B,
+0xE40285F6, 0xBF926603, 0xE5FEE500, 0xE010655C,
+0xBF5FE403, 0xE5130F54, 0xE40EBF62, 0x05FCE010,
+0xBF62E40E, 0xE5007585, 0xBF63E403, 0xE500E640,
+0xBF70E403, 0xE500E640, 0xBF78E403, 0xE5FFE640,
+0xE014655C, 0xBF45E404, 0xE40F0F54, 0xE504BF48,
+0x05FCE014, 0xBF48E40F, 0xE5017584, 0xBF49E640,
+0xE501E404, 0xBF56E640, 0xE501E404, 0xE404E640,
+0xAF5C7F18, 0x7F184F26, 0x000B4F26, 0x4F220009,
+0xD2427FF0, 0x88016021, 0xD2418B71, 0x26686621,
+0xD2408B6D, 0x26686621, 0xE50F8B69, 0xE401BF1A,
+0xBF1DE501, 0xE586E400, 0xE400655C, 0x2F50BF1D,
+0xBF1AE401, 0xE401E506, 0xBF1B6543, 0xE401E640,
+0xBF286543, 0xE401E640, 0xBF306543, 0x65F0E640,
+0x756CE402, 0xBEFD6053, 0xE40280F4, 0xE512BF00,
+0xE40284F4, 0xBF007090, 0xE6406503, 0xBF01E402,
+0xE640E500, 0xBF0EE402, 0xE640E500, 0xBF16E402,
+0xE5FEE500, 0x6053655C, 0xBEE3E403, 0xE51380F8,
+0xE40EBEE6, 0xE40E84F8, 0xBEE67085, 0xE5006503,
+0xBEE7E640, 0xE500E403, 0xBEF4E640, 0xE500E403,
+0xBEFCE640, 0xE5FFE403, 0x6053655C, 0xBEC9E404,
+0xE40F80FC, 0xE504BECC, 0xE40F84FC, 0xBECC7083,
+0xE5016503, 0xBECDE640, 0xE501E404, 0xBEDAE640,
+0xE501E404, 0xE404E640, 0xAEE07F10, 0x7F104F26,
+0x000B4F26, 0x00000009, 0x001E1030, 0x001E1080,
+0x001E1090, 0x001E103F, 0x001E103E, 0x002028C6,
+0x002028C8, 0x002028CA, 0x0009000B, 0x666CE680,
+0x6563D2A8, 0x7540E700, 0x6473422B, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0x4C18EC01, 0xDAA3DBA2,
+0x65B252B1, 0x89223520, 0xC9036051, 0x891E8801,
+0xD19FDE9D, 0x64E3410B, 0x85036503, 0x670D66A2,
+0xDD9C3762, 0xD49C890A, 0x420BD29C, 0xD19C0009,
+0xE701D49C, 0x21724D0B, 0x0009AFE2, 0x420BD29A,
+0xD69A64E3, 0x4D0BD49A, 0xAFD926C2, 0x4F260009,
+0x6DF66EF6, 0x6BF66CF6, 0x6AF6000B, 0x7FF44F22,
+0xE6818546, 0x85472F01, 0x81F1666C, 0xD2858548,
+0x854281F2, 0x81F367F3, 0xE40C8543, 0x605381F4,
+0x81F56563, 0x7540420B, 0x4F267F0C, 0x0009000B,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xDC847FF0, 0xE800A0DD, 0xD2836B13, 0xE0014B08,
+0x4B00420B, 0x1F03DE81, 0x3BEC85F2, 0x2F827E30,
+0x1FE26803, 0x66C2DD7E, 0x362052C1, 0xA0C38B01,
+0x60610009, 0x8801C903, 0xA0BD8B01, 0x85610009,
+0x8965C801, 0xEE105163, 0xDA6A8512, 0xC9036603,
+0x85136403, 0x4021600D, 0xC93F4021, 0x8D1C30E3,
+0xD7706503, 0x62704408, 0x44004408, 0x22284500,
+0x345C8F0A, 0x6043D26C, 0x697D072D, 0x68994919,
+0x697C6E8E, 0x28EDA009, 0x6043D268, 0x697D072D,
+0x68994919, 0x697C6E8E, 0xEEFF28ED, 0x6EEC629D,
+0x8B0F32E0, 0x410BD152, 0x540364C3, 0xBF85E502,
+0xD45F6E03, 0x460BD654, 0xD75E65E3, 0xD45EEE01,
+0x27E2A01D, 0x26E9EEFC, 0x81126063, 0x97888513,
+0x20794208, 0x85128113, 0x8112208B, 0x202B8513,
+0x85148113, 0x4218E208, 0x8114202B, 0x854164C2,
+0x814120E9, 0xD45165C2, 0xCB016051, 0x4A0B2501,
+0x60C20009, 0x52F356F2, 0x2B02CB01, 0x2622AF8B,
+0xD2378561, 0x8D2EC802, 0x420B64C3, 0xD6480009,
+0x5E036503, 0x076EE04C, 0x7701D146, 0x60120676,
+0x8B058801, 0xEA0C85E1, 0x20AB4A18, 0x81E1A007,
+0x88026012, 0x85E18B03, 0x20A9EADF, 0x855181E1,
+0x20A9EAFC, 0x60518151, 0xCB01DA28, 0x4A0B64C3,
+0x56F22501, 0xD73851F3, 0x85EF2612, 0x470B64D3,
+0xAF58650D, 0x420B0009, 0x54030009, 0x85446E03,
+0x4A18EA08, 0x30A020A9, 0x8B03DA1A, 0xE501BF16,
+0x0009A007, 0xD62D8541, 0x2268620D, 0xBF0D8901,
+0xD423E500, 0x420BD218, 0xD72265E3, 0xEE01D428,
+0x27E24A0B, 0x0009AF37, 0x68F26083, 0x780181F2,
+0x618D7C08, 0x31E7EE03, 0xAF1D8901, 0x7F100009,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0xFE0368F6, 0x00201834, 0x00202884, 0x0020288C,
+0x00200A5C, 0x00200DA6, 0x00202854, 0x00200ADE,
+0x001E2130, 0x00202A70, 0x00200A7A, 0x001C3D30,
+0x00202A74, 0x00202864, 0x00201FD4, 0x001C3D00,
+0x00202A80, 0x00202A8C, 0x00202970, 0x002029F0,
+0x0020285C, 0x001E212C, 0x00202A78, 0x00202A7C,
+0x002027F8, 0x002027F4, 0x00200E06, 0x00008000,
+0x00202A88, 0x4F222FE6, 0x6E22D20D, 0xC84060E3,
+0x22E28D02, 0x0009BE68, 0x4218E240, 0x89012E28,
+0x0009BE64, 0xC80560E3, 0xBEB98901, 0x60E30009,
+0x8902C802, 0xAE614F26, 0x4F266EF6, 0x6EF6000B,
+0x001C3510, 0x080A0C0E, 0x00020406, 0x1A1C1E20,
+0x12141618, 0x2E303234, 0x26282A2C, 0x3A3C3E40,
+0x6C625648, 0x41112F26, 0xE2208F18, 0x890B3123,
+0x321CD204, 0xD1026220, 0x412B312C, 0x00090009,
+0x00201FFE, 0x00201FB4, 0x000BE000, 0x400062F6,
+0x40004000, 0x40004000, 0x40004000, 0x62F6000B,
+0x40004000, 0x40004000, 0x40004000, 0x40184000,
+0x62F6000B, 0x40004000, 0x40004000, 0x40004000,
+0x40284000, 0x62F6000B, 0x40004000, 0x40184000,
+0x000B4028, 0xC90F62F6, 0x40054005, 0x40054005,
+0x62F6000B, 0x4005C907, 0x40054005, 0x62F6000B,
+0x4005C903, 0x000B4005, 0xC90162F6, 0x000B4005,
+0x000062F6, 0x080A0C0E, 0x00020406, 0x1A1C1E20,
+0x12141618, 0x2E303234, 0x26282A2C, 0x3A3C3E40,
+0x6C625648, 0x41112F26, 0xE2208F18, 0x890B3123,
+0x321CD204, 0xD1026220, 0x412B312C, 0x00090009,
+0x002020BE, 0x00202074, 0x000BE000, 0x400162F6,
+0x40014001, 0x40014001, 0x40014001, 0x62F6000B,
+0x40014001, 0x40014001, 0x40014001, 0x40194001,
+0x62F6000B, 0x40014001, 0x40014001, 0x40014001,
+0x40294001, 0x62F6000B, 0x40014001, 0x40194001,
+0x000B4029, 0x400462F6, 0x40044004, 0xC90F4004,
+0x62F6000B, 0x40044004, 0xC9074004, 0x62F6000B,
+0x40044004, 0x000BC903, 0x400462F6, 0x000BC901,
+0x000062F6, 0x3622E218, 0x67438F12, 0x0009A004,
+0x76FF6254, 0x74012420, 0xC8036053, 0x60438BF8,
+0x8902C803, 0x422BD22B, 0xD22B0009, 0x0009422B,
+0x2FE66473, 0x8D4A3450, 0x27786763, 0x62438947,
+0x227B225B, 0xC9016023, 0x8D203452, 0x2EE86E03,
+0x60238B15, 0x8B08C803, 0x47096643, 0x47106256,
+0x8FFB2622, 0xA0327604, 0x47010009, 0x61436673,
+0x46106255, 0x8FFB2121, 0xA0287102, 0x66430009,
+0x47106254, 0x8FFB2620, 0xA0207601, 0x61430009,
+0x2EE8357C, 0x8F15317C, 0x60236653, 0x8B07C803,
+0x76FC4709, 0x47106262, 0x21268FFB, 0x0009A00F,
+0x65634701, 0x75FE6673, 0x46106251, 0x21258FFB,
+0x0009A005, 0x626076FF, 0x8FFB4710, 0x60432124,
+0x6EF6000B, 0x002022A6, 0x00202752, 0xE21E2FE6,
+0x67633626, 0x8D1B6153, 0x3E106E43, 0x3E128916,
+0x65E38908, 0x3672E600, 0x62148910, 0x25207601,
+0x7501AFF9, 0x317C64E3, 0x6513347C, 0xE600A004,
+0x625075FF, 0x24247601, 0x8BF93672, 0x60E3A011,
+0x890831E2, 0x327C6213, 0x8B0432E6, 0x651364E3,
+0xA0086673, 0xD28F6EF6, 0x651364E3, 0x422B6673,
+0x000B6EF6, 0xE2046EF6, 0x67433622, 0x8F10356C,
+0xA004346C, 0x75FF0009, 0x76FF6250, 0x60532424,
+0x8BF8C803, 0xC8036043, 0xA1058901, 0xA2770009,
+0xA2990009, 0x2FB60009, 0x2FD62FC6, 0x7FE42FE6,
+0x6C636043, 0x66521F62, 0xC9037504, 0x1F516E53,
+0x45086503, 0xE1FC6D43, 0x2D194500, 0x1F732558,
+0x1F651F44, 0x2FD28D0B, 0x88086053, 0x88108923,
+0x8818895B, 0xA0898B01, 0xA0BD0009, 0x62630009,
+0x2D22E600, 0x7CFC7D04, 0xEB10A00D, 0xE60064E6,
+0x7CF065E6, 0x62E261E6, 0x1D512D42, 0x1D231D12,
+0x7E047D10, 0x3CB21FE1, 0x1F6589F0, 0x2FD21FC2,
+0xA0A11FE6, 0x64D21FD4, 0x44286263, 0x44294418,
+0x42184419, 0x4629242B, 0x2D424619, 0x65637D04,
+0xA0217CFD, 0x67E6EB10, 0x62E67CF0, 0x64E66673,
+0x256B4618, 0x2D5261E2, 0x65234729, 0x45184719,
+0x4229275B, 0x42191D71, 0x47186743, 0x4429227B,
+0x44196713, 0x247B4718, 0x1D431D22, 0x41194129,
+0x65137D10, 0x1FE17E04, 0x89DC3CB2, 0x1FE67EFF,
+0x1FC21F55, 0xA0672FD2, 0x6CF21FD4, 0x66C257F5,
+0x46286273, 0x42284629, 0x2C62262B, 0x7C045DF2,
+0x7DFE4729, 0xA01CEB10, 0x65E65EF1, 0x66E66273,
+0x47286753, 0x6763227B, 0x452961E6, 0x257B4728,
+0x2C2264E6, 0x65131C51, 0x45284629, 0x1C62265B,
+0x41296643, 0x216B4628, 0x44291C13, 0x67437C10,
+0x3DB27DF0, 0x1FD289E1, 0x7EFEA034, 0x51F56CF2,
+0x621366C2, 0x42284618, 0x42184619, 0x2C62262B,
+0x7C045DF2, 0x7DFF4119, 0xA01FEB10, 0x65E65EF1,
+0x64E67DF0, 0x42286253, 0x421867E6, 0x66E6212B,
+0x61432C12, 0x45194128, 0x251B4118, 0x65731C51,
+0x44194528, 0x245B4518, 0x64631C42, 0x47194428,
+0x274B4418, 0x46191C73, 0x61637C10, 0x89DE3DB2,
+0x7EFD1FD2, 0x1FC41FE6, 0x5DF2E704, 0xA00D5EF6,
+0x62E451F4, 0x66E47DFC, 0x65E464E4, 0x71012120,
+0x71012160, 0x71012140, 0x71012150, 0x89F03D72,
+0x66D357F3, 0x641365E3, 0x6EF67F1C, 0x6CF66DF6,
+0x6BF6A190, 0x00202134, 0x2FC62FB6, 0x2FE62FD6,
+0x60437FE4, 0x6C63C903, 0x66031F62, 0x460875FC,
+0x61526E43, 0x4600E2FC, 0x26682E29, 0x1F441F73,
+0x1F516D53, 0x8D0B1F15, 0x60632FE2, 0x891F8808,
+0x89538810, 0x8B018818, 0x0009A081, 0x0009A0B9,
+0xEB10A00D, 0x52D37DF0, 0x54D156D2, 0x2E1665D2,
+0x2E662E26, 0x2E427EFC, 0x1FD16153, 0x3CB27CF0,
+0x7D0489F0, 0x1F151FD6, 0x2FE21FC2, 0x1FE4A0A1,
+0x621366E2, 0x42294619, 0x42194618, 0x2E62262B,
+0x7CFF4118, 0xEB10A021, 0x54D37DF0, 0x624357D2,
+0x42194229, 0x55D1212B, 0x2E1666D2, 0x41296173,
+0x41194418, 0x2E46241B, 0x44296453, 0x44194718,
+0x2E76274B, 0x47296763, 0x47194518, 0x257B7EFC,
+0x46182E52, 0x1FD16163, 0x3CB27CF0, 0x7D0389DC,
+0x1F151FD6, 0x2FE21FC2, 0x1FE4A06B, 0x57F56EF2,
+0x627366E2, 0x46284629, 0x262B4229, 0x2E625CF2,
+0x7CFE4728, 0xA01BEB10, 0x7DF05DF1, 0x55D251D3,
+0x46296613, 0x54D1276B, 0x2E7662D2, 0x41286753,
+0x217B4729, 0x61432E16, 0x41294528, 0x2E56251B,
+0x44286523, 0x245B4529, 0x42282E46, 0x7CF06723,
+0x89E23CB2, 0x1FD67D02, 0xA03A1FC2, 0x67F21FE4,
+0x657251F5, 0x45296213, 0x45284519, 0x42194518,
+0x5CF2252B, 0x41282752, 0x7CFD4118, 0xA022EB10,
+0x7DF05DF1, 0x54D256D3, 0x45196563, 0x52D14628,
+0x4618215B, 0x6ED26543, 0x45192716, 0x265B4428,
+0x65436163, 0x45186423, 0x42284419, 0x4218254B,
+0x271664E3, 0x44196623, 0x264B2756, 0x4E282766,
+0x61E34E18, 0x3CB27CF0, 0x7D0189DB, 0x1FC21FD6,
+0xE7041F74, 0x51F45DF2, 0x5EF6A00D, 0x84E27EFC,
+0x620364E0, 0x7DFC84E1, 0x84E36503, 0x21646603,
+0x21542124, 0x3D722144, 0x57F389F0, 0x641366D3,
+0x7F1C65E3, 0x6DF66EF6, 0xA09D6CF6, 0x2F866BF6,
+0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6, 0x614374E0,
+0x6A636873, 0x6B56E920, 0x6C567AE0, 0x6D567120,
+0x6E563A92, 0x64566756, 0x62566656, 0x11C121B2,
+0x11E311D2, 0x11451174, 0x8DEC1166, 0x71201127,
+0x6613A004, 0x7AFF6254, 0x76012620, 0x8BF92AA8,
+0x6EF66083, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0x2F8668F6, 0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6,
+0x6A636873, 0x75E0E920, 0x56565257, 0x57545155,
+0x5D525E53, 0x6B525C51, 0x24662426, 0x24762416,
+0x7AE024E6, 0x24C624D6, 0x8DEC3A92, 0x66A324B6,
+0x6EF66783, 0x6CF66DF6, 0x6AF66BF6, 0xA04369F6,
+0x2FE668F6, 0xC8046063, 0x8D046E63, 0x62166153,
+0x24227EFC, 0x60E37404, 0x8908C818, 0x71046513,
+0x62526616, 0x24227EF8, 0xAFF41461, 0xE2047408,
+0x65133E22, 0x66E38D02, 0x6EF6A01C, 0x6EF6AF87,
+0xC8046063, 0x61638D04, 0x625275FC, 0x242671FC,
+0xC8186013, 0x75F88906, 0x66525251, 0x24662426,
+0x71F8AFF6, 0x3122E204, 0x66138F02, 0x0009AFA1,
+0x0009A00A, 0x0009A004, 0x76FF6254, 0x74012420,
+0x8BF92668, 0x6073000B, 0x0009A004, 0x625075FF,
+0x242476FF, 0x8BF92668, 0x6073000B, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x544F0D0A,
+0x46205355, 0x00003A57, 0x2072614D, 0x32203232,
+0x20373030, 0x353A3431, 0x33353A34, 0x00000000,
+0x00000D0A, 0x00000043, 0x61766E49, 0x2064696C,
+0x72657375, 0x20726F20, 0x2079656B, 0x00214449,
+0x6E6B6E55, 0x206E776F, 0x6D6D6F63, 0x3D646E61,
+0x00000000, 0x61437748, 0x7262696C, 0x6F697461,
+0x6620206E, 0x0A6C6961, 0x0000000D, 0x73696F4E,
+0x61432065, 0x7262696C, 0x6F697461, 0x6166206E,
+0x21216C69, 0x00000D0A, 0x00000D0A, 0x00000042,
+0x000000FF, 0x00020001, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00020003, 0x01090108, 0x0002010A,
+0x00030002, 0x02020201, 0x02040203, 0x02060205,
+0x02080207, 0x020A0209, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00FF010F, 0x01090108, 0x010B010A,
+0x00030002, 0x02020201, 0x02040203, 0x02060205,
+0x02080207, 0x020A0209, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00000072, 0x00205220, 0x00000046, 0x00000059,
+0x73204142, 0x003D7165, 0x00000074, 0x00000000,
+0x02000112, 0x40FFFFFF, 0x12210ACE, 0x20104890,
+0x02090100, 0x0101002E, 0x09FA8000, 0x04000004,
+0x000000FF, 0x02010507, 0x07000200, 0x00028205,
+0x05070002, 0x00400383, 0x04050701, 0x01004003,
+0x002E0209, 0x80000101, 0x000409FA, 0x00FF0400,
+0x05070000, 0x00400201, 0x82050700, 0x00004002,
+0x03830507, 0x07010040, 0x40030405, 0x03040100,
+0x030C0409, 0x0079005A, 0x00410044, 0x03180053,
+0x00530055, 0x00320042, 0x0030002E, 0x00570020,
+0x0041004C, 0x0000004E, 0x00000000, 0x00000000,
+0x00000709, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+};
+
+const u32_t zcFwImageSize=11104;
diff --git a/drivers/staging/otus/hal/hpfwu_txstream.c b/drivers/staging/otus/hal/hpfwu_txstream.c
new file mode 100644
index 000000000000..2b77cbacc6d6
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwu_txstream.c
@@ -0,0 +1,1017 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE947FFC, 0xE114D594,
+0x1E13D494, 0x67521E4C, 0xD494D693, 0x37402769,
+0x62528F06, 0x7201D692, 0x60602522, 0x2600C93F,
+0xD7906152, 0x2512611D, 0x264B6652, 0x2562470B,
+0x0009B017, 0xE60095AC, 0xC84060E2, 0x2F028F03,
+0x8FF93652, 0xD4887601, 0x4E0BDE88, 0xD4880009,
+0x00094E0B, 0x4E0BD487, 0x7F040009, 0xA0524F26,
+0x4F226EF6, 0x410BD184, 0xD4840009, 0x0009440B,
+0x450BD583, 0xD7830009, 0xD283E1FF, 0x2712611D,
+0xD4825029, 0xE1FFCB01, 0x1209E501, 0x12112212,
+0xE7202452, 0x4718D57E, 0x2572D27E, 0xD17EE700,
+0xD67FD47E, 0xE2012270, 0x24702172, 0xD67D2620,
+0x2641E4FF, 0xD57CE600, 0x666DE104, 0x76016063,
+0x4000626D, 0x8FF83212, 0xD5780545, 0x2520E201,
+0xD278D777, 0xE480E100, 0x22122710, 0x6613D576,
+0x666D644C, 0x76046763, 0x375C626D, 0x8FF83243,
+0xD5722712, 0xD273D772, 0xE400E101, 0x27102511,
+0x000B4F26, 0x7FCC2242, 0xD170D56F, 0xD271DB70,
+0x1F51D471, 0xD6717508, 0x1F12D771, 0x1F55710C,
+0x1FB975FC, 0x72041F2A, 0x1F13EB10, 0x1F561F44,
+0x1F781F67, 0xD86B1F2B, 0xDD6CD96B, 0xDC6CEA00,
+0xD26DDE6C, 0x89003A22, 0xD15D7A01, 0x88016010,
+0x56F88B03, 0x4218E201, 0xD1682622, 0x0009410B,
+0x440BD467, 0xD5670009, 0x0009450B, 0x6010D150,
+0x8B108801, 0xE650D14F, 0x46186212, 0x8B083266,
+0x56F9D14B, 0x2120E200, 0xCB016062, 0x2602A003,
+0x72012710, 0x60822122, 0x89098801, 0xE2C8D15A,
+0x622C6612, 0x89033626, 0x6010D158, 0x8BC88801,
+0x51F66792, 0x217252F5, 0xD6555191, 0x55FA2212,
+0x52FB6462, 0x55612542, 0x2252E400, 0x61436643,
+0x05DE6013, 0x36CC4608, 0x07DE2652, 0xC9036071,
+0x8B028801, 0x720162E2, 0x74012E22, 0x36B3664C,
+0x71048FEE, 0x66C2D147, 0x45286512, 0x265B4518,
+0x60822C62, 0x89018801, 0x0009A168, 0x6272D742,
+0x8B132228, 0xD42BD741, 0x6772D541, 0x51536242,
+0x312C327C, 0x24222228, 0x15138D05, 0x6262D63D,
+0xB1627201, 0xD6232622, 0x2622E200, 0x52916692,
+0x8B013620, 0x0009A144, 0x6061A06E, 0x001C001C,
+0x001D4020, 0x0000B38E, 0xFFFF0000, 0x12340000,
+0x001E1015, 0x00201274, 0x002039F4, 0x002018A2,
+0x00203A00, 0x00203A18, 0x00201860, 0x0020196C,
+0x00201288, 0x001C3510, 0x001C3624, 0x001E212C,
+0x002038F4, 0x0020348C, 0x002038FC, 0x00203908,
+0x00203914, 0x00203970, 0x00203974, 0x0020391C,
+0x0020391D, 0x00203920, 0x00117700, 0x0020398C,
+0x0020398A, 0x002034F0, 0x00117710, 0x001C3D30,
+0x001C36F8, 0x00117734, 0x001C3684, 0x001C3D00,
+0x001C1000, 0x001C1028, 0x00203504, 0x00203924,
+0x00117600, 0x00117740, 0x7FFFFFFF, 0x00201730,
+0x0020332A, 0x00202334, 0x00203DA4, 0x00203972,
+0x002034FC, 0x00203964, 0x001C3D2C, 0x001C36B0,
+0x00203494, 0x0011775C, 0x8801C90F, 0xA0CF8901,
+0xD1960009, 0x36206212, 0xD4958904, 0x2421E200,
+0x2162A0CC, 0x6211D193, 0x89012228, 0x0009A0C3,
+0xE202D78F, 0x75016571, 0x3123615D, 0x27518D02,
+0x0009A0BC, 0xD28C57F2, 0x62226072, 0x40094019,
+0xC90F4009, 0x8F19880A, 0x52F31F2C, 0x40196022,
+0x40094009, 0x8808C90F, 0xA0A78901, 0x60630009,
+0xCB0154F7, 0xD27E55F2, 0xE7012402, 0xD47FE100,
+0x22112572, 0x72016242, 0x2422A098, 0x8B3F8805,
+0x602252F3, 0x40094019, 0xC90F4009, 0x8B168802,
+0xE4FFD577, 0x644D6752, 0x8B102748, 0x6272D775,
+0x8B0C3260, 0x51F255F7, 0xD26DE701, 0x21722562,
+0xD571E100, 0x64522211, 0xA0777401, 0x52F32542,
+0x40196022, 0x40094009, 0x8805C90F, 0x31B38B6E,
+0xD26A8B6C, 0x672254F4, 0x7701D569, 0x61422272,
+0x1F1CE640, 0x46182159, 0x8B033160, 0x6262D665,
+0x26227201, 0xE200D65A, 0x2621B067, 0x0009A056,
+0x3123E220, 0x88038B52, 0x52F38B1E, 0x40196022,
+0x40094009, 0x8803C90F, 0xD25B8B16, 0x672254F4,
+0x7701D557, 0x61422272, 0x1F1CE640, 0x46182159,
+0x8B033160, 0x6262D655, 0x26227201, 0xE200D648,
+0x2621B043, 0x0009A010, 0xD452D551, 0xD2446752,
+0xE1007701, 0x25723A46, 0x22118F06, 0xEA00D64E,
+0x72016262, 0x2622B031, 0x2FB2D54C, 0x95736652,
+0xD44A5BF1, 0x36205241, 0x60618910, 0x8B01C803,
+0x2B22E201, 0x8FF54510, 0x57F15664, 0x6272E1F0,
+0x41284118, 0x2722221B, 0x6BF2A008, 0x6BF2A006,
+0xE200D62F, 0xD12F2621, 0x2121E200, 0xD13CE201,
+0x66122822, 0x8B012668, 0x0009AE2B, 0x450BD539,
+0xD1390009, 0xAE24E600, 0x2F862160, 0x2FA62F96,
+0x2FC62FB6, 0x2FE62FD6, 0x7FF44F22, 0xDE34D133,
+0x54116212, 0x1F4167E2, 0x2F22D432, 0xD5321F72,
+0xD2326743, 0x58417794, 0x69425A42, 0x5B166C72,
+0x60526D22, 0xCB20E600, 0xE5402502, 0x626D7601,
+0x8BFB3253, 0x55F162F2, 0x11512122, 0xD62855F2,
+0x14812E52, 0x249214A2, 0x27C2D426, 0x26D211B6,
+0xDA256742, 0xE801D925, 0x490B2A72, 0xE2011A8C,
+0x1A2C4218, 0x4F267F0C, 0x6DF66EF6, 0x6BF66CF6,
+0x69F66AF6, 0x68F6000B, 0x000007D1, 0x00203984,
+0x00203988, 0x0020398E, 0x001C3DC0, 0x0011772C,
+0x001C3B88, 0x0020396C, 0x0011773C, 0x00117744,
+0x0000F000, 0x00117764, 0x00117748, 0x00117768,
+0x0011776C, 0x01FFFFFF, 0x0011774C, 0x002034FC,
+0x00203DA4, 0x002024F8, 0x00203972, 0x001C3B9C,
+0x001C3D98, 0x001C3700, 0x001C3500, 0x001C5960,
+0x001C8960, 0x00203504, 0x001C3D00, 0x0020160C,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xDE957FAC, 0x61E0E014, 0x0F14D494, 0x710161E3,
+0xE0186210, 0xD2920F24, 0x0009420B, 0x450BD591,
+0x20080009, 0x8F126D03, 0xD28F1F07, 0x6720D48F,
+0x657CDD8F, 0x470BD78F, 0xD18F0009, 0x619C6910,
+0x46086613, 0x36184608, 0x3D6C4608, 0xE0181FD7,
+0xE58004FC, 0x604C66E2, 0x3050655C, 0x2D628F15,
+0x01FCE014, 0xDE85E500, 0x641CA008, 0x6753655D,
+0x607037EC, 0x39DC6953, 0x80947501, 0x3243625D,
+0xD67F8BF4, 0xA34EE200, 0x20082621, 0xE0148B13,
+0xE40001FC, 0xA009DE79, 0x644D671C, 0x35EC6543,
+0x69436652, 0x39DC6262, 0x74041921, 0x3273624D,
+0xA3388BF3, 0x88012D10, 0xE0148B17, 0xE70001FC,
+0x6D1C2D70, 0xDE6D1FD4, 0x32D3627D, 0xA32A8B01,
+0x677D0009, 0x667365E3, 0x61737504, 0x315C36EC,
+0x69126462, 0xAFEF7708, 0x88042492, 0xE0148B18,
+0xE40001FC, 0x671C2D40, 0x624DDE60, 0x8B013273,
+0x0009A311, 0x6943644D, 0x39EC62E3, 0x72046592,
+0x3D2C6D43, 0x615266D2, 0x21697408, 0x2512AFED,
+0x8B188805, 0x01FCE014, 0x2D40E400, 0xDE53671C,
+0x3273624D, 0xA2F68B01, 0x644D0009, 0x62E36943,
+0x659239EC, 0x6D437204, 0x66D23D2C, 0x74086152,
+0xAFED216B, 0x88312512, 0xD44A8B3A, 0x6146D94A,
+0x75046543, 0x67566442, 0x6E531F48, 0x65527E04,
+0x7EE462E2, 0x7E0464E2, 0x6EE21FE9, 0x5EF929E0,
+0x7E04D942, 0x1FEA60E2, 0x2900C901, 0xD9406EE2,
+0x29E04E09, 0x2F562F26, 0x56FAD93E, 0x6513490B,
+0xD13D7F08, 0xE71C6E0D, 0x1DE12D70, 0xDE3B6912,
+0x64E21D92, 0x1D43D13A, 0xD23A6512, 0x67221D54,
+0x1D75D239, 0x1D666622, 0x6262D638, 0x1D27A2AB,
+0x8B398830, 0x6596D92B, 0x67926696, 0x61967904,
+0x74E46493, 0x6E436992, 0x1F9B7E04, 0x1FEC6442,
+0xD9256EE2, 0x5EFC29E0, 0x7E04D924, 0x1FED60E2,
+0x2900C901, 0xD9226EE2, 0x29E04E09, 0x59FC7FFC,
+0xDE272F92, 0x2F164E0B, 0xD41F7F08, 0xE21C610D,
+0x1D112D20, 0xD2206442, 0xD41C1D42, 0x1D536542,
+0x6752D51B, 0xD71B1D74, 0x1D156172, 0x1D666622,
+0x6262D61A, 0x1D27A26F, 0x8B358833, 0x490BD919,
+0xA268EE00, 0x00002DE0, 0x00117800, 0x00203A1C,
+0x002018A2, 0x00202AAC, 0x0020390E, 0x00203A20,
+0x00203534, 0x002018EE, 0x0020390D, 0x00117804,
+0x0020398C, 0x00117810, 0x00203909, 0x0020390A,
+0x0020390B, 0x00200F64, 0x001C5864, 0x001C6864,
+0x001C7864, 0x001C59BC, 0x001C69BC, 0x001C79BC,
+0x00200FBC, 0x00200FB8, 0x89018828, 0x0009A0C0,
+0xE643DEB5, 0x326662E1, 0x1FEE8F02, 0x2E21E240,
+0x622D62E1, 0x8B013267, 0x0009A0AA, 0xE50185E1,
+0x8B013056, 0x0009A0A4, 0x2D10E101, 0x64E1B225,
+0xE64357FE, 0x652D6271, 0x89443567, 0x3563E640,
+0xE6008B05, 0x0F65E040, 0xA00FE11A, 0x615372C0,
+0x41214121, 0x41214121, 0x45214121, 0x45214521,
+0xC9036053, 0xE0406603, 0x71180F65, 0x2209E007,
+0x0F25E03C, 0xE044641D, 0xB2A365F3, 0xE33C0F46,
+0x853233FC, 0x620DDE95, 0x42086031, 0x6023610D,
+0x1323E944, 0x06FE4108, 0xE00F39FC, 0x13144E0B,
+0x67075D91, 0x60D32679, 0x0F6654FE, 0x51928542,
+0x600D4E0B, 0x60D3260B, 0x0F666492, 0x65F3B237,
+0x696156FE, 0xE640659D, 0x89383563, 0xD78359FE,
+0x79066591, 0xC9036053, 0x40004008, 0x61036203,
+0x0F26E050, 0x470BE0FF, 0x6C07600C, 0x6603605D,
+0x46214621, 0x46214621, 0x42006263, 0x4200326C,
+0x40214021, 0x4008C903, 0x6D2D30FC, 0xE8006A03,
+0xB25765F3, 0x6EA264D3, 0x2EC9E050, 0x66942AE2,
+0xD76E01FE, 0x606C470B, 0x2AE22E0B, 0x64D365F3,
+0x7801B1FD, 0xEE06628D, 0x8FE932E3, 0x5EFE7D01,
+0x61E1E400, 0x410085E1, 0x66E3310C, 0x760C711B,
+0xE70465F3, 0x68667401, 0x3A736A4D, 0x8FF92582,
+0x65F37504, 0x641DB1E3, 0x64E1B1A4, 0x0009A17B,
+0xD45B56F7, 0xEC01D25B, 0x26C0420B, 0x0009A173,
+0x06FCE018, 0x8829606C, 0x58F78B08, 0xE400D252,
+0x66222840, 0x646DB171, 0x0009A165, 0x666CE681,
+0x89013060, 0x0009A0AC, 0xD550D14F, 0x62126A56,
+0x212232AC, 0x54116C56, 0x34CC6253, 0x64521141,
+0x72085812, 0xD44A384C, 0x68221182, 0x5A136C42,
+0x3ACC3C8C, 0x11A324C2, 0x6C2272EC, 0x72105814,
+0x118438CC, 0x5A156822, 0x11A53A8C, 0x6A227210,
+0xD6405816, 0x118638AC, 0x52176C62, 0x112732CC,
+0x5A185861, 0x11A83A8C, 0x5C195A62, 0x11C93CAC,
+0x521A5C63, 0x112A32CC, 0x5A1B5864, 0x11AB3A8C,
+0x5C1C5A65, 0x11CC3CAC, 0x521D5C66, 0x112D32CC,
+0x5A1E5867, 0x11AE3A8C, 0x561F5A68, 0x36ACE840,
+0x116FDA2D, 0x6CA2381C, 0x7A946682, 0x286236CC,
+0x5C8162A2, 0x18C13C2C, 0x62A27A44, 0x362C5682,
+0xD6261862, 0x5A856262, 0x3A2C4229, 0x760418A5,
+0x56866262, 0x362C4229, 0x56F71866, 0x2620E238,
+0x16C15C81, 0x16226212, 0xE2005C11, 0x551216C3,
+0x55151654, 0x55131655, 0x55161656, 0x55821657,
+0x65821658, 0x55141659, 0x5584165A, 0x5583165B,
+0x5585165C, 0x5586165D, 0x1821165E, 0x11212122,
+0x11251122, 0x11261123, 0x28221822, 0x18241124,
+0x18251823, 0x1826A0C7, 0x00117804, 0x002033E8,
+0x00203A40, 0x002018A2, 0x00203494, 0x001C36A0,
+0x002034F0, 0x001C3CA0, 0x001C36F4, 0x001C3B88,
+0x666CE682, 0x8B203060, 0xEA2456F7, 0x26A0D194,
+0x16C15C17, 0x16225218, 0x16835819, 0x16A45A1A,
+0x16C55C1B, 0x1626521C, 0xE200581D, 0x551E1687,
+0x551F1658, 0x11271659, 0x11291128, 0x112B112A,
+0x112D112C, 0xA08E112E, 0xE683112F, 0x3060666C,
+0x52F78B0B, 0xEA00D883, 0x658222A0, 0x7804DC82,
+0x62822C52, 0xA07ED681, 0xE6902620, 0x3060666C,
+0xDA7F8B06, 0x00094A0B, 0xE20056F7, 0x2620A073,
+0x666CE691, 0x8B103060, 0x6222D276, 0x2228622C,
+0xD2788904, 0x0009420B, 0x0009A003, 0x420BD276,
+0x56F70009, 0xA05EE200, 0xE6922620, 0x3060666C,
+0xE0188951, 0xE6B00BFC, 0x666C62BC, 0x8B2A3260,
+0x02FCE014, 0x682CEA00, 0x62ADE904, 0x894A3283,
+0x6AADDD64, 0x3CDC6CA3, 0x7D046EC2, 0xDB68D467,
+0x32DC62A3, 0x4B0BDC67, 0x4C0B6D22, 0xD46664E3,
+0x00094B0B, 0x64D34C0B, 0x4B0BD464, 0xE6000009,
+0x666D6BE3, 0x76013B6C, 0x3293626D, 0x8FF72BD0,
+0xAFDA4D19, 0xE6B57A08, 0x3260666C, 0xD45C8B13,
+0x4B0BDB57, 0xD25B0009, 0x6022DB5B, 0xCB20E6FF,
+0x2202666D, 0xDB592B62, 0xE014E200, 0x56F72B20,
+0xA01002FC, 0xD4562620, 0x6542D256, 0x420BD456,
+0xA0080009, 0xDB520009, 0x52B1E600, 0x622CDB53,
+0x52F72B21, 0x7F542260, 0x6EF64F26, 0x6CF66DF6,
+0x6AF66BF6, 0x000B69F6, 0x4F2268F6, 0xE240614D,
+0x89143123, 0x3127E21F, 0x8B09D749, 0xD449614D,
+0xE00171E0, 0x5671440B, 0x26596507, 0x1761A007,
+0xE001D444, 0x6672440B, 0x26596507, 0x4F262762,
+0x0009000B, 0x614D4F22, 0x3123E240, 0xE21F8912,
+0xD73B3127, 0x614D8B08, 0x5671D23A, 0x420B71E0,
+0x260BE001, 0x1761A006, 0x6672D236, 0xE001420B,
+0x2762260B, 0x000B4F26, 0xE6400009, 0x46284618,
+0x6252D531, 0x89FC2268, 0x0009000B, 0x4618E680,
+0xD52D4628, 0x22686252, 0x000B89FC, 0xA0010009,
+0x7201E200, 0x8BFC3242, 0x0009000B, 0x4618E680,
+0xD5264628, 0x22686252, 0x000B8BFC, 0x2FE60009,
+0x7FFC4F22, 0xBFF16E53, 0x61E22F42, 0xE280D620,
+0x54E11615, 0x16464218, 0x422855E2, 0x57E31657,
+0x16786EF2, 0x26E22E2B, 0x4F267F04, 0x6EF6AFCE,
+0x00203494, 0x00117804, 0x002038F4, 0x00203908,
+0x0020050A, 0x00201008, 0x0020102E, 0x00203A58,
+0x002018A2, 0x002018E6, 0x00203A6C, 0x00203A74,
+0x00203A78, 0x001C3500, 0x001C1000, 0x0020398A,
+0x00117800, 0x002018EE, 0x00203A8C, 0x00203990,
+0x001C3704, 0x002033E8, 0x001C373C, 0x001C3700,
+0x001C370C, 0x2FD62FC6, 0x4F222FE6, 0x6C53DD10,
+0x6E43BFA4, 0x2DE2BF89, 0x0009BFA0, 0x2C1251D5,
+0x1C4154D6, 0x1C5255D7, 0x1C6356D8, 0x6EF64F26,
+0x000B6DF6, 0x61636CF6, 0xA004E600, 0x62564109,
+0x24227601, 0x36127404, 0x000B8BF9, 0x00000009,
+0x001C370C, 0x0009A16E, 0x2FE62FD6, 0xDD944F22,
+0xA0049EB2, 0xD4930009, 0x420BD293, 0x62D265D2,
+0x8BF822E8, 0x0009A004, 0xD28FD490, 0x55D1420B,
+0x22E852D1, 0xA0048BF8, 0xD48D0009, 0x420BD28A,
+0x52D255D2, 0x8BF822E8, 0x0009A004, 0xD286D489,
+0x55D3420B, 0x22E852D3, 0xA0048BF8, 0xD4860009,
+0x420BD281, 0x52D455D4, 0x8BF822E8, 0x6EF64F26,
+0x6DF6000B, 0x2FD62FC6, 0x4F222FE6, 0x6E636C73,
+0x6D53B01A, 0x64D357F4, 0xB05F65E3, 0xB07566C3,
+0xB0A40009, 0xB0A80009, 0xB0AC0009, 0xB0AC0009,
+0xB0AF0009, 0xB03154F5, 0x6CCD6C03, 0x4F2660C3,
+0x6DF66EF6, 0x6CF6000B, 0x3412D170, 0xD6700529,
+0x2650D770, 0x2742000B, 0x0009A018, 0x2FD62FC6,
+0x4F222FE6, 0x6E636C73, 0x6D53BFEE, 0x64D357F4,
+0xB03365E3, 0xB08D66C3, 0xB00F54F5, 0x6CCD6C03,
+0x4F2660C3, 0x6DF66EF6, 0x6CF6000B, 0xE503D162,
+0xD763D462, 0x21524518, 0x2472000B, 0xD45FD15E,
+0x2162E600, 0x2462000B, 0xBF734F22, 0xBF73E40A,
+0xD25C0009, 0x4118E104, 0xE40AE500, 0xBF692212,
+0xD7592252, 0xCB206072, 0x000B4F26, 0x4F222702,
+0x410BD156, 0xD556E400, 0x4F26452B, 0xD1552FE6,
+0x66126E63, 0x92104418, 0x44084528, 0x45002629,
+0x265B4408, 0x264B4400, 0x21624708, 0xD14E4708,
+0x217227EB, 0x6EF6000B, 0x1FFF03F0, 0x4F222FE6,
+0xE101DE4A, 0xBF3DE40A, 0x67E32E12, 0xE500776C,
+0xE204E130, 0x2752E40A, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27222712,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x175ABF18, 0x2E62E600, 0x000B4F26, 0xD2346EF6,
+0xE441E101, 0x000B2212, 0xD1322242, 0xE605D432,
+0x000B2162, 0x000B2462, 0xD2300009, 0xE40AE601,
+0x2262AF00, 0x2FC62FB6, 0x2FE62FD6, 0x7FFC4F22,
+0x6C43DB2B, 0xED0060B2, 0x2B02CB03, 0xC90360B2,
+0x6E03A008, 0x89073DC2, 0xE46460B2, 0xB07CC903,
+0x7D016E03, 0x8BF52EE8, 0x8F043DC2, 0xD4212FE1,
+0x460BD621, 0x62F10009, 0x6023622D, 0x89FFC801,
+0x7F046023, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x001C3B88, 0x00203AA0, 0x002018EE, 0x00203AA8,
+0x00203AB0, 0x00203AB8, 0x00203AC0, 0x0025E720,
+0x00203DA0, 0x002038F8, 0x001C5968, 0x001C3B40,
+0x000F8000, 0x001D4004, 0x001C3500, 0x002015E0,
+0x0020160C, 0x001C5814, 0x001C59D0, 0x001C5830,
+0x001C6268, 0x001C59A4, 0x001C639C, 0x001C581C,
+0x001C5860, 0x00203AC8, 0x002018A2, 0x8F014411,
+0x6043604B, 0x0009000B, 0x5651D52B, 0x46286052,
+0x306C000B, 0x2FC62FB6, 0x2FE62FD6, 0x4F124F22,
+0xBFF14F02, 0x6B036E43, 0xDD25DC24, 0x0009BFEC,
+0x3C0530B8, 0x4609060A, 0x46014609, 0x020A3D65,
+0x42094209, 0x32E24209, 0x4F068BF0, 0x4F264F16,
+0x6DF66EF6, 0x000B6CF6, 0x2FC66BF6, 0x2FE62FD6,
+0x4F124F22, 0xBFCF4F02, 0x6C036E43, 0xBFCBDD13,
+0x30C80009, 0x060A3D05, 0x46094609, 0x36E24601,
+0x4F068BF5, 0x4F264F16, 0x6DF66EF6, 0x6CF6000B,
+0x4F222FE6, 0xE102DE0B, 0xE403E500, 0xBFB92E12,
+0xE6062E52, 0xE7004618, 0x2E62E403, 0x4F262E72,
+0x6EF6AFB0, 0x0009000B, 0x001C1040, 0xCCCCCCCD,
+0x10624DD3, 0x001D4004, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE5007F98, 0x6453E710,
+0x6B534728, 0xEE1ADCBC, 0x6153655D, 0x315C4108,
+0x75014108, 0x6043317C, 0x0F16665D, 0xED0060B3,
+0x21B136E3, 0x81128111, 0x11D28113, 0x11D411D3,
+0x74048FEA, 0xD8B167F2, 0x1871D9B1, 0x58F12872,
+0x1981D1B0, 0x59F22982, 0x5DF45AF3, 0x54F65EF5,
+0x21921191, 0x11A211A3, 0x11D411D5, 0x11E611E7,
+0x11481149, 0xDAA855F7, 0x57F8EE00, 0x52F9DDA7,
+0x64E3D6A7, 0x2A521A51, 0xD8A7D9A6, 0x2D729AD5,
+0x6EED2622, 0x4D086DE3, 0x3DEC61E3, 0x4D084108,
+0x3D9C31EC, 0x410860B3, 0x81D12DB1, 0x4108E050,
+0x4008E7B7, 0x677C4108, 0x60B381D2, 0xE200318C,
+0x81D33472, 0x1D131DD2, 0x8D01D493, 0xD4901D24,
+0xB0B365D3, 0x64ED7E01, 0x8BDA34A2, 0x2FD2DA8C,
+0xDD9268A2, 0x2D824829, 0x7DFC64A2, 0xD287694D,
+0x6E222D92, 0x7D0C4E29, 0x68222DE2, 0x618D6AD3,
+0x2A16D784, 0xD48A6D72, 0x24D2D583, 0xD6895E72,
+0x517414E2, 0x1414EE00, 0xD1875876, 0x59781486,
+0x1498E710, 0x65E36252, 0x26E2142A, 0xE60064E3,
+0x6843644D, 0x384C4808, 0x381C4808, 0x0C866053,
+0x09CE28B1, 0x819160B3, 0x0ACE6053, 0x81A26043,
+0x0DCE6053, 0x81D360B3, 0x08CE6053, 0x18827401,
+0x624D09CE, 0x0ACE19E3, 0x1A643273, 0x75048FE0,
+0xE003D96A, 0x40186C92, 0x6D922CB1, 0x81D1DA6F,
+0x6E92E050, 0x81E24008, 0x60B36192, 0x64928113,
+0x1442E600, 0xD4696792, 0x689217A3, 0x1864E1FF,
+0x6563E703, 0x364C4608, 0x26127501, 0x3673665D,
+0xDC5B8BF8, 0x6DC2E003, 0x2DB14018, 0xD2606EC2,
+0x61C281E1, 0x1112EE00, 0xE02464C2, 0x65C21423,
+0x15E4D45B, 0xE58067C2, 0x68C28172, 0x818366E3,
+0x666D655C, 0x76046963, 0x394C6A6D, 0x8FF83A53,
+0xDB5429E2, 0x24B2DC54, 0x24C27404, 0x4F267F68,
+0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6, 0x68F6000B,
+0x60116142, 0x8F03C803, 0xD23DE500, 0x8B063420,
+0xC9036011, 0x8B068802, 0x3420D239, 0x56128B03,
+0x52646513, 0x000B2422, 0x01136053, 0x2FE62FD6,
+0x7FEC4F22, 0x62536E53, 0x6D43E550, 0x4508E400,
+0xE101A001, 0x60435224, 0x81212211, 0x60538123,
+0x56E28122, 0x8BF53620, 0x16E4D238, 0xE61464F3,
+0x65E3420B, 0xE4FC65E1, 0x2E512549, 0x65F361F1,
+0x2F112149, 0xD13154D1, 0xE614410B, 0x607157D1,
+0x2701CB01, 0x7F141DE1, 0x6EF64F26, 0x6DF6000B,
+0x2FE62FD6, 0x7FEC4F22, 0x66536E53, 0x6D43E5FC,
+0x20596061, 0x2601CB01, 0x326052E2, 0x12E48B06,
+0x31E051E2, 0x52D18B04, 0x1E22A002, 0x5664AFF0,
+0x64F3D21E, 0x420BE614, 0x67E165E3, 0x2719E1FC,
+0x67F12E71, 0x271954D1, 0x65F3D118, 0x410BE614,
+0x52D12F71, 0xCB016021, 0x1DE12201, 0x4F267F14,
+0x000B6EF6, 0x00006DF6, 0x00203924, 0x002034F4,
+0x002034FC, 0x00203504, 0x0020352C, 0x00203910,
+0x00203918, 0x00100208, 0x001017C0, 0x001E210C,
+0x001C3D00, 0x00203964, 0x001000C8, 0x00117880,
+0x00117780, 0x00040020, 0x0026C401, 0x00200ED6,
+0x4F222FE6, 0xDE42624C, 0x42004208, 0x3E2CA005,
+0xD4405252, 0xBF695624, 0x65E22E62, 0x352052E1,
+0xD63D8BF6, 0x4F262622, 0x6EF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xDC394F22, 0x52C1DB39, 0x362066C2,
+0x6061891C, 0x8801C903, 0xDE348918, 0xBF37DD35,
+0x650364E3, 0x66B28503, 0x3262620D, 0xD4328907,
+0x0009BF76, 0x4D0BD431, 0xAFE60009, 0xBF3D0009,
+0xD42F64E3, 0x00094D0B, 0x0009AFDF, 0x2262D22D,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x2FD62FC6,
+0x4F222FE6, 0xDD29DC28, 0x6E4360C2, 0x04DE4008,
+0xE614D127, 0x65E3410B, 0xD127D726, 0x55E227E2,
+0x35E05254, 0x21228F04, 0x400860C2, 0x122202DE,
+0x605365C2, 0x75014008, 0x0DE606DE, 0xC90F6053,
+0x60632C02, 0x6EF64F26, 0x000B6DF6, 0x85436CF6,
+0x650D5643, 0x622D6262, 0x35277204, 0xE1008F0C,
+0x2268960C, 0xD6158B03, 0x72015261, 0xD6131621,
+0x6262E101, 0x26227201, 0x6013000B, 0x000001FF,
+0x00203504, 0x002034FC, 0x001C3D00, 0x0020352C,
+0x002038F4, 0x002018A2, 0x002034F4, 0x00203AF0,
+0x00203AF4, 0x001C3D28, 0x00203964, 0x00203924,
+0x00200ED6, 0x00203968, 0x0020396C, 0x00117754,
+0x2FC62FB6, 0x2FE62FD6, 0x7FF84F22, 0x6022D237,
+0x8D58C803, 0xDE362F01, 0xDB37DC36, 0x66C252C1,
+0x892F3620, 0xC9036061, 0x892B8801, 0xD233DD31,
+0x64D3420B, 0x1F016503, 0x880160B1, 0xD2308B04,
+0x64D3420B, 0x0009AFEA, 0x85615653, 0x8904C820,
+0xE050D72C, 0x7201027E, 0xD22B0726, 0x6453420B,
+0x89072008, 0x55F1D126, 0x64D3410B, 0xE601D727,
+0x2762AFD4, 0x55F1D226, 0x64E3420B, 0xE601D125,
+0x2162AFCC, 0xDD25DE24, 0xDC26DB25, 0x66D252D1,
+0x89183620, 0xC9036061, 0x89148801, 0xD117D41F,
+0x0009410B, 0x36E05603, 0x65038F04, 0x2B20E201,
+0x2C52AFEC, 0xD712D41C, 0x0009470B, 0xE601D115,
+0xAFE34618, 0x60F12162, 0x8907C804, 0x7F08D217,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6422B, 0x4F267F08,
+0x6DF66EF6, 0x000B6CF6, 0x00006BF6, 0x001E2100,
+0x00203504, 0x002034FC, 0x0020398C, 0x002014A0,
+0x002014CC, 0x00203494, 0x002016BE, 0x001E212C,
+0x00201530, 0x001C3D30, 0x00117880, 0x002034F4,
+0x00203914, 0x00203910, 0x0020352C, 0x00200610,
+0xE601D203, 0x1265D503, 0x000B2252, 0x00001266,
+0x001C1010, 0x0000C34F, 0x0009000B, 0x2FD62FC6,
+0x4F222FE6, 0x6D436C53, 0xEE00A004, 0x7E0164D4,
+0x644CBFF2, 0x8BF93EC2, 0x6EF64F26, 0x000B6DF6,
+0xE5006CF6, 0x6643A002, 0x76017501, 0x22286260,
+0xAFE38BFA, 0x2FE60009, 0x75076253, 0xE1086753,
+0x6043EE0A, 0x4409C90F, 0x650330E2, 0x8D014409,
+0xE630E637, 0x4110365C, 0x8FF22760, 0xE00077FF,
+0x000B8028, 0x000B6EF6, 0x000BE000, 0x2FE6E000,
+0x7FEC4F22, 0x6E436253, 0xBFDC65F3, 0xBFD06423,
+0xBFCE64E3, 0xD40364F3, 0x0009BFCB, 0x4F267F14,
+0x6EF6000B, 0x00203AF8, 0xE4FDD29F, 0xD79F6122,
+0x22122149, 0x74016022, 0x2202CB01, 0xD59C6622,
+0x22622649, 0xC8406070, 0x60528902, 0x2502CB04,
+0xE1F76452, 0x25422419, 0xE7016052, 0x2502CB40,
+0xE6026052, 0x2502C9CF, 0x47186052, 0x2502CB10,
+0xCB036052, 0x15622502, 0x1573000B, 0xD78ED58D,
+0xD48FD28E, 0xE600E100, 0x27112511, 0xAFCB2210,
+0x664C2461, 0x4600D28B, 0x6060362C, 0x000BCB10,
+0x654C2600, 0x4500D287, 0x6650352C, 0x2619E1EF,
+0x2560000B, 0xD284664C, 0x362C4600, 0xCB106060,
+0x2600000B, 0xD280654C, 0x352C4500, 0xE1EF6650,
+0x000B2619, 0x664C2560, 0x4600D27A, 0x6060362C,
+0x000BCB08, 0x654C2600, 0x4500D276, 0x6650352C,
+0x2619E1F7, 0x2560000B, 0xD273664C, 0x362C4600,
+0xCB086060, 0x2600000B, 0xD26F654C, 0x352C4500,
+0xE1F76650, 0x000B2619, 0x624C2560, 0x4200D669,
+0x6020326C, 0x4021C908, 0x40214021, 0x600C000B,
+0xD665624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0xD161600C, 0x341C644C, 0x000B6240,
+0xD15F602C, 0x341C644C, 0x000B6240, 0x2FE6602C,
+0x6E434F22, 0xE60A645C, 0x89143467, 0x0009BFEB,
+0x60EC640C, 0x8B028801, 0xA002E00F, 0x44092409,
+0x624C4409, 0x3263E60A, 0xBFE28905, 0x620C644C,
+0xC8806023, 0xE2008B00, 0x4F266023, 0x6EF6000B,
+0xD64C4F22, 0x88016062, 0xB2578B03, 0xA0030009,
+0xD2490009, 0x2260E640, 0xE200D648, 0x000B4F26,
+0x4F222622, 0x6062D643, 0x8B018802, 0x0009B2A0,
+0xE200D642, 0x000B4F26, 0xD53E2622, 0xE100D43E,
+0x2512E701, 0x2470000B, 0xE604D23B, 0x2260000B,
+0xD43B4F22, 0x410BD13B, 0xD53B0009, 0x6650E1FD,
+0x2619D23A, 0x2560E700, 0x000B4F26, 0x4F222270,
+0xD238D537, 0xD7386152, 0x2512611D, 0xE6FF6452,
+0x2542242B, 0xD22FD435, 0x420B666D, 0xD52E2762,
+0x6750E1FB, 0x4F262719, 0x2570000B, 0xD4304F22,
+0x410BD128, 0xD5280009, 0x6650E7F7, 0x4F262679,
+0x2560000B, 0x9425D524, 0x22496250, 0x2520000B,
+0xE4BFD521, 0x22496250, 0x2520000B, 0xD2254F22,
+0x600D8522, 0x89112008, 0x89458801, 0x89478803,
+0x89498805, 0x894F8806, 0x89558808, 0x895B8809,
+0x8961880A, 0x8967880B, 0x0009A06E, 0x0009B070,
+0x600CA06B, 0x0000FF7F, 0x001E2148, 0x001E1000,
+0x001E1108, 0x002039C4, 0x002039C6, 0x002039E5,
+0x002039A8, 0x001E103F, 0x001E105F, 0x001E102F,
+0x001E1090, 0x002039CC, 0x001E100B, 0x002039C8,
+0x00203AFC, 0x002018A2, 0x001E1028, 0x002039E4,
+0x001D4020, 0x98760000, 0x001C1000, 0x00203B08,
+0x00203B18, 0x0020399C, 0x0009B04C, 0x600CA035,
+0x0009B055, 0x600CA031, 0x6260D684, 0x8B2B2228,
+0x0009B061, 0x600CA029, 0x6260D680, 0x8B232228,
+0x0009B069, 0x600CA021, 0x6260D67C, 0x8B1B2228,
+0x0009B0C7, 0x600CA019, 0x6260D678, 0x8B132228,
+0x0009B0CD, 0x600CA011, 0x6260D674, 0x8B0B2228,
+0x0009B125, 0x600CA009, 0x6260D670, 0x8B032228,
+0x0009B13D, 0x600CA001, 0x4F26E000, 0x0009000B,
+0xD26CD16B, 0xD56C8412, 0x4000C90F, 0xD76B012D,
+0xE403D66B, 0xE20F611C, 0x2540E001, 0x25202712,
+0x2602000B, 0xE601D262, 0x30668523, 0xE0008D05,
+0xD663D260, 0xE0018122, 0x000B2602, 0xD25C0009,
+0x600D8523, 0x89052008, 0x8B0A8801, 0x6060D65D,
+0x2600CB01, 0xD457D65A, 0xE001E101, 0x000B2612,
+0x000B8142, 0xD152E000, 0x8513E501, 0x640D4518,
+0x66033453, 0xE0008D05, 0xD551D253, 0x2260E001,
+0x000B2502, 0x4F220009, 0x8513D149, 0x6453650D,
+0x62494419, 0x227D672E, 0x8801602C, 0x88028909,
+0x88038910, 0x8806891A, 0x88078935, 0xA04C893B,
+0xD5460009, 0x6652D746, 0x2762D446, 0x622C6261,
+0x2421A038, 0x2228625C, 0xD4438B3F, 0x6642D540,
+0x2562D440, 0x24018561, 0x6203A02C, 0x2008605C,
+0x88108907, 0x88208908, 0x88308909, 0xA02C890A,
+0xD23A0009, 0x6222A008, 0xA005D239, 0xD2396222,
+0x6222A002, 0x6262D638, 0xD432D531, 0x66212522,
+0xA00F626C, 0xD6352421, 0x6261D52D, 0x622CD42D,
+0xA0072562, 0xD6322421, 0x8561D529, 0x2562D429,
+0x62032401, 0x662D8515, 0x3617610D, 0x65038F01,
+0xB0CB2451, 0xA0010009, 0xE000E001, 0x000B4F26,
+0xD6190009, 0xD427E101, 0x65412610, 0xD118D717,
+0xE20F655D, 0x2752E001, 0x000B2620, 0x2FE62102,
+0xD20F4F22, 0x640C8523, 0x8B082448, 0xD511D61D,
+0x2621E200, 0x940F8451, 0xA0482049, 0xDE0D8051,
+0xC84060E0, 0xE2018D32, 0x89443427, 0xD216D615,
+0x2641420B, 0x0009A030, 0x0000FF7F, 0x002039E5,
+0x0020399C, 0x002039A8, 0x001E1100, 0x001E100C,
+0x002039C8, 0x001E1000, 0x001E1001, 0x002039D0,
+0x002039B0, 0x002039B4, 0x002039B8, 0x002039D4,
+0x002039D8, 0x002039DC, 0x002039E0, 0x00203E04,
+0x00203E0E, 0x002039C2, 0x00202886, 0x89123427,
+0xD294D693, 0x2641420B, 0xCB8084E1, 0x80E1B0F5,
+0xD69160E0, 0x2E00CB04, 0xC93F6060, 0xD68F2600,
+0xA001E001, 0xE0002602, 0x000B4F26, 0xD68C6EF6,
+0xC8806060, 0xD2868919, 0x88016021, 0xD2898B15,
+0x8524E501, 0x89103056, 0xE203D187, 0x2120D487,
+0xE00B6541, 0x0656655D, 0xE40FD585, 0x2140E702,
+0xD77E2571, 0x000BE001, 0x000B2702, 0x2FE6E000,
+0xDE804F22, 0xC88084E1, 0xD57A892C, 0x20088554,
+0x61038F28, 0x8553D77C, 0x64036672, 0x8566650C,
+0x3520620C, 0xD6798B1E, 0x651CD774, 0x2651644C,
+0x60E02741, 0x8904C840, 0x420BD275, 0xA0030009,
+0xD2680009, 0x0009420B, 0x0009B09F, 0xE201D167,
+0x60E02122, 0xCB04D464, 0x60402E00, 0x2400C93F,
+0x6023A001, 0x4F26E000, 0x6EF6000B, 0x2FB62FA6,
+0x2FD62FC6, 0xDA622FE6, 0x66A1E240, 0x3622DC5E,
+0x62638900, 0x6ED36D2C, 0x4E2136D8, 0x4E212A61,
+0xDB61D460, 0xE700A00F, 0x770162B2, 0x71026123,
+0x66212B12, 0x71026213, 0x61212B12, 0x651D666D,
+0x356C4528, 0x627C2452, 0x8BED32E3, 0xC90360D3,
+0x8B108803, 0x617367B2, 0x2B127102, 0x71026E13,
+0x2B126571, 0x655D6DE1, 0x422862DD, 0x325CE107,
+0xA00C2C10, 0x88022422, 0xA0038B01, 0x8801E203,
+0xE2018B05, 0x66B22C20, 0x655D6561, 0xE60F2452,
+0x67A12C60, 0x8B052778, 0xDD38DC44, 0xEB01EA00,
+0x2DB22CA2, 0x6DF66EF6, 0x6BF66CF6, 0x6AF6000B,
+0x2FE62FD6, 0xE240DD36, 0x362266D1, 0x62638900,
+0x3678672C, 0x7703DE38, 0x47212D61, 0x64E2D635,
+0xA00E4721, 0x6562E100, 0x62537101, 0x74012450,
+0x24204219, 0x45297401, 0x74012450, 0x24504519,
+0x621C7401, 0x8BEE3273, 0x66E24200, 0x420061D1,
+0x2118362C, 0x2E628F06, 0xDD1CD728, 0xE501E400,
+0x2D522742, 0x000B6EF6, 0x2FD66DF6, 0x4F222FE6,
+0xED0AEE01, 0x64E3BC85, 0xBC8A64E3, 0x62EC7E01,
+0x8BF732D7, 0xBC8DEE01, 0x64E364E3, 0x7E01BC92,
+0x32D762EC, 0x4F268BF7, 0x000B6EF6, 0xD1186DF6,
+0xD418920D, 0x72122122, 0x2422D617, 0xD7177204,
+0x72202622, 0x2722D116, 0x000B7230, 0x137A2122,
+0x002039C2, 0x00202992, 0x001E1015, 0x002039C8,
+0x001E1001, 0x0020399C, 0x001E1100, 0x002039C6,
+0x002039B4, 0x001E1000, 0x002039B8, 0x002039C4,
+0x00202886, 0x001E100C, 0x002039B0, 0x002039CC,
+0x002039D0, 0x002039D4, 0x002039D8, 0x002039DC,
+0x002039E0, 0x4F222FE6, 0xD6707FFC, 0x88016060,
+0xE2018951, 0x2620BFBB, 0xD56ED16D, 0xDE6E6010,
+0x64E36552, 0x7402C840, 0x8D22D16C, 0xD26C7502,
+0xE601D76C, 0xE7042722, 0x76016255, 0x626C2421,
+0x8FF93273, 0xD4637402, 0x6242E601, 0x640D8528,
+0x67494419, 0x275D657E, 0x81E4607C, 0xE417D562,
+0x67557601, 0x3243626C, 0x8FF92171, 0xA0207102,
+0xD25E0009, 0xE601D75B, 0xE7042722, 0x76016255,
+0x626C2421, 0x8FF93273, 0xD4527402, 0x6242E601,
+0x640D8528, 0x67494419, 0x275D657E, 0x81E4607C,
+0xE417D553, 0x67557601, 0x3243626C, 0x8FF92171,
+0x92897102, 0xD2462E21, 0x5E23D74E, 0x64F22FE2,
+0x604365F2, 0x2700C980, 0xC9606043, 0x80716103,
+0xC9036043, 0x80724519, 0x65F2605C, 0x817266F2,
+0x46194629, 0x606C4529, 0x4018645C, 0x8173304C,
+0x21185E23, 0x64F22FE2, 0x6E4C62F2, 0x602C4219,
+0x66F262F2, 0x46294018, 0x461930EC, 0x42298174,
+0x652C606C, 0x305C4018, 0x81758F07, 0x0009BC96,
+0x2228620C, 0xA00A8908, 0x60130009, 0x8B038840,
+0x0009B009, 0x0009A003, 0xE202D62F, 0x7F042622,
+0x000B4F26, 0x4F226EF6, 0x8552D52A, 0x8830600D,
+0x88318903, 0xA0348923, 0x85550009, 0xD428D727,
+0x85532701, 0x610DD627, 0x24124118, 0x460BD426,
+0xD7230009, 0xD226D425, 0x6572420B, 0xE230D120,
+0x42286712, 0x2729E620, 0x37604628, 0xD6218B03,
+0xA016E200, 0xD61F2622, 0xA012E202, 0xD1182622,
+0x6212E530, 0xE6204528, 0x46282259, 0x89083260,
+0xD41AD119, 0xE601D513, 0x2160450B, 0x472BD718,
+0x4F264F26, 0x0009000B, 0x0000060A, 0x002039E4,
+0x001E1000, 0x002039D0, 0x00203E04, 0x00203E10,
+0x00203DA8, 0x002039B8, 0x00203DD8, 0x00203DD6,
+0x00203DAA, 0x0020399C, 0x002039C8, 0x002039B4,
+0x002039B0, 0x002018A2, 0x00203B24, 0x00203B28,
+0x002018EE, 0x002039CC, 0x001E100B, 0x00203B3C,
+0x00114004, 0x4F222FE6, 0xDE967FFC, 0x200884E9,
+0x2F008D06, 0xD695D494, 0x0009460B, 0x64F0B19A,
+0x6620D293, 0x89022668, 0xC9BF60E0, 0x7F042E00,
+0x000B4F26, 0x000B6EF6, 0x2FE60009, 0xDE8D4F22,
+0x60E0D68D, 0xCBC0D48D, 0x62602E00, 0xC803602C,
+0x40218904, 0x70014021, 0x6603A002, 0x66034009,
+0xD687616D, 0xE500A004, 0x75016262, 0x74042422,
+0x3213625D, 0xD2838BF8, 0x0009420B, 0xC9BF84E2,
+0x4F2680E2, 0x6EF6000B, 0x2FE62FD6, 0x7FFC4F22,
+0x6260D67D, 0x89442228, 0xD572E100, 0x60502610,
+0xCB40D47A, 0x2500440B, 0x8D052008, 0x62E06E03,
+0x7104612C, 0x2F11A006, 0xD475D66D, 0xDD756760,
+0x657C4D0B, 0xE23C6D1D, 0x8B033D27, 0xD267D472,
+0x0009420B, 0x4D214D21, 0xA005D770, 0x66E6E400,
+0x357C4508, 0x74012562, 0x35D3654D, 0xD76C8BF7,
+0x6172E003, 0x81114018, 0x6E7260F1, 0x81E2700C,
+0xD4686172, 0xDD688113, 0x4D0BDE68, 0xE2016572,
+0xD4672E22, 0x420BD255, 0xD6560009, 0xC93F6060,
+0x7F042600, 0x6EF64F26, 0x6DF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xD25F4F22, 0x6B436E73, 0x420B6C53,
+0x20086D63, 0x64038D1C, 0xE50ED149, 0x32526210,
+0x60C38916, 0x804124B0, 0x814160D3, 0xA007E500,
+0x655D61BC, 0x00EC6053, 0x364C6653, 0x80647501,
+0x3213625D, 0xD63B8BF5, 0xC9BF6060, 0x2600A008,
+0xD23AD44D, 0x6EF64F26, 0x6CF66DF6, 0x6BF6422B,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x7FC44F22,
+0x720262F3, 0x22512F41, 0x45297202, 0x60632251,
+0xE5C4E682, 0x67F38121, 0x655C666C, 0xE408BFB6,
+0x4F267F3C, 0x0009000B, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE1007FC4, 0x6513ECFF,
+0x6B136CCD, 0xDE36D735, 0xEDFF64F3, 0xD835EA04,
+0x6053655C, 0x027D4000, 0x32C0622D, 0x66038D0D,
+0x09ED6063, 0x2491027D, 0x24217402, 0x698202ED,
+0x3928622D, 0x74022892, 0x75017104, 0x6063625C,
+0x07D532A2, 0x0EB58FE4, 0x2448641C, 0xE6808905,
+0x67F3E5C5, 0xBF79666C, 0x7F3C655C, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0xD11E68F6,
+0x6012D21E, 0xCB20E405, 0x2102E500, 0x000B2242,
+0x00002252, 0x001E1017, 0x00203B40, 0x002018A2,
+0x0020390E, 0x001E1015, 0x001E10BF, 0x00117800,
+0x001E10FC, 0x00200610, 0x00203914, 0x00202AEA,
+0x00203B44, 0x002018EE, 0x00203B60, 0x0011788C,
+0x00203910, 0x002034F4, 0x00201530, 0x001E2130,
+0x00203B68, 0x00202AAC, 0x00203B6C, 0x00203974,
+0x0020397C, 0x00203DA4, 0x001C3500, 0x001D4004,
+0xD564D163, 0xE400D764, 0x2142E20F, 0x17411154,
+0xD5622722, 0x9669D762, 0x15412572, 0x96661562,
+0xE6011565, 0xD55F1165, 0x666CE6F8, 0x25422542,
+0x25422542, 0x25422542, 0x25622542, 0x7601E727,
+0x67632572, 0x25627797, 0xE7042572, 0x2572E248,
+0xE2192522, 0xE2702522, 0x25422542, 0x25422542,
+0x25222542, 0x2522E20C, 0x25422542, 0x25422542,
+0x25422542, 0x25422542, 0x000B154A, 0xE2081145,
+0x0009422B, 0x2FE62FD6, 0x7FFC4F22, 0xC8206043,
+0x6E438D02, 0x0009BE67, 0xC81060E3, 0xBE648901,
+0x60E30009, 0x8901C840, 0x0009BE86, 0xC80160E3,
+0xDD3D8938, 0xC80260D0, 0x2F008D03, 0x460BD63B,
+0x60F00009, 0x8902C804, 0x460BD639, 0x62F00009,
+0xC8806023, 0x60D08902, 0x2D00C97F, 0xC8016023,
+0xD6348906, 0x0009460B, 0x0009A007, 0x51630601,
+0x8902C808, 0x460BD630, 0x60F00009, 0x8902C810,
+0x420BD22E, 0xD52E0009, 0x88026052, 0xD22D8B03,
+0xA005E604, 0x88012260, 0xD22A8B02, 0x2260E601,
+0x2522E200, 0xC88060E3, 0xD227892D, 0x60E36E20,
+0x8902C880, 0x420BD225, 0x60E30009, 0x8902C840,
+0x420BD223, 0x60E30009, 0x8902C802, 0x420BD221,
+0x60E30009, 0x890DC804, 0xDD20D11F, 0x0009410B,
+0x0009BF0D, 0x0009BF4C, 0xD51ED41D, 0x2470E708,
+0x25D2BF85, 0xC80860E3, 0xD21B8905, 0x4F267F04,
+0x422B6EF6, 0x7F046DF6, 0x6EF64F26, 0x6DF6000B,
+0x001C581C, 0xA000A000, 0x001D0100, 0x001D4000,
+0x00040021, 0x001C589C, 0x001E1021, 0x00201A90,
+0x00201AB2, 0x00202114, 0x00201ACA, 0x00201AD8,
+0x002039C8, 0x001E100B, 0x001E1028, 0x00201B44,
+0x00201B50, 0x00201AE0, 0x00201AFE, 0x12345678,
+0x001E1000, 0x0010F100, 0x00201B2C, 0x644CD6A7,
+0x000B346C, 0xD6A62450, 0x346C644C, 0x2450000B,
+0x644CD6A4, 0x000B346C, 0x625C2450, 0x4208616D,
+0x42084119, 0x42006019, 0x670E614C, 0xD49E321C,
+0x4200207D, 0x324CC90F, 0x2200000B, 0x4208625C,
+0x42004208, 0x324C644C, 0x4200D498, 0x000B324C,
+0x2FE62260, 0x614C4F12, 0x4100D493, 0x6710314C,
+0xE29F666D, 0x27294619, 0x6E536269, 0x672E6573,
+0x4221227D, 0x42214221, 0x7601662C, 0xE4014608,
+0x34E84608, 0x644C4600, 0x071A0467, 0x2150257B,
+0x000B4F16, 0x4F226EF6, 0xD2857FE8, 0x88016021,
+0xD2848B7B, 0x26686621, 0xD2838B77, 0x26686621,
+0xE50F8B73, 0xE401BFA2, 0xBFA4E501, 0xE586E400,
+0xE400655C, 0x2F50BFA4, 0xBFA1E401, 0xE602E506,
+0x60634618, 0x81F2E401, 0x6543BF9F, 0xE40185F2,
+0xBFAB6543, 0x85F26603, 0x6543E401, 0x6603BFB1,
+0xE40265F0, 0x6053756C, 0x80F8BF80, 0xBF82E402,
+0x84F8E512, 0x7090E402, 0x6503BF82, 0x4618E602,
+0x81F66063, 0xBF80E402, 0x85F6E500, 0x6603E402,
+0xE500BF8C, 0xE40285F6, 0xBF926603, 0xE5FEE500,
+0xE010655C, 0xBF61E403, 0xE5130F54, 0xE40EBF63,
+0x05FCE010, 0xBF63E40E, 0xE5007585, 0xBF64E403,
+0xE500E640, 0xBF71E403, 0xE500E640, 0xBF78E403,
+0xE5FFE640, 0xE014655C, 0xBF47E404, 0xE40F0F54,
+0xE504BF49, 0x05FCE014, 0xBF49E40F, 0xE5017584,
+0xBF4AE640, 0xE501E404, 0xBF57E640, 0xE501E404,
+0xE404E640, 0xAF5C7F18, 0x7F184F26, 0x000B4F26,
+0x4F220009, 0xD2427FF0, 0x88016021, 0xD2418B71,
+0x26686621, 0xD2408B6D, 0x26686621, 0xE50F8B69,
+0xE401BF1C, 0xBF1EE501, 0xE586E400, 0xE400655C,
+0x2F50BF1E, 0xBF1BE401, 0xE401E506, 0xBF1C6543,
+0xE401E640, 0xBF296543, 0xE401E640, 0xBF306543,
+0x65F0E640, 0x756CE402, 0xBEFF6053, 0xE40280F4,
+0xE512BF01, 0xE40284F4, 0xBF017090, 0xE6406503,
+0xBF02E402, 0xE640E500, 0xBF0FE402, 0xE640E500,
+0xBF16E402, 0xE5FEE500, 0x6053655C, 0xBEE5E403,
+0xE51380F8, 0xE40EBEE7, 0xE40E84F8, 0xBEE77085,
+0xE5006503, 0xBEE8E640, 0xE500E403, 0xBEF5E640,
+0xE500E403, 0xBEFCE640, 0xE5FFE403, 0x6053655C,
+0xBECBE404, 0xE40F80FC, 0xE504BECD, 0xE40F84FC,
+0xBECD7083, 0xE5016503, 0xBECEE640, 0xE501E404,
+0xBEDBE640, 0xE501E404, 0xE404E640, 0xAEE07F10,
+0x7F104F26, 0x000B4F26, 0x00000009, 0x001E102F,
+0x001E1080, 0x001E1090, 0x001E103F, 0x001E103E,
+0x002039C2, 0x002039C4, 0x002039C6, 0xD21DD11C,
+0x66206010, 0x676C7001, 0x3700C90F, 0xE5008D13,
+0x67106210, 0x7701622C, 0x64232170, 0xD6166010,
+0x44084408, 0x3428C90F, 0x62602100, 0x7201D513,
+0x44082620, 0x000B354C, 0xD10F6053, 0x25586510,
+0xE6008D13, 0xD60DD40B, 0x655C6540, 0x47086753,
+0x37584708, 0x47086540, 0x24507501, 0x367C6040,
+0x2400C90F, 0x72FF6210, 0x000B2120, 0x00006063,
+0x0020390D, 0x0020390C, 0x0020390E, 0x00203534,
+0x7FFC4F22, 0xE680D19F, 0x666C6212, 0xD29E2F22,
+0x67F36563, 0x420B7542, 0x7F04E404, 0x000B4F26,
+0xE6800009, 0xD298666C, 0xE7006563, 0x422B7540,
+0xE6806473, 0xD294666C, 0xE7006563, 0x422B7543,
+0x2F866473, 0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6,
+0x7FCC4F22, 0xDC8ED28D, 0x72011F21, 0xDB8D1F22,
+0xD18EDE8D, 0x66125211, 0x8B013620, 0x0009A0E5,
+0xC9036061, 0x8B018801, 0x0009A0DF, 0xD288D487,
+0xED84420B, 0x2F025503, 0x30D0845C, 0xA0B88901,
+0xD1840009, 0x626C6610, 0x88016023, 0xD1828B68,
+0x62101FC3, 0x895B2228, 0xE003D480, 0x40186742,
+0x68421772, 0xD57EE900, 0x81816DB3, 0x7D042190,
+0x67D26AB2, 0x64E26852, 0x1F491F57, 0x740464E3,
+0x1FA46542, 0x65431F5A, 0x625275F8, 0x1F761FD5,
+0x6D531F2B, 0xDA74D773, 0x7D94D274, 0x68D21F88,
+0x6AA26972, 0xD1726022, 0x2202CB20, 0xE1401F1C,
+0x7601E600, 0x3213626D, 0x56F48BFB, 0x52F651F5,
+0x21222B62, 0x52F851F7, 0x212256F9, 0x2E6251FA,
+0x51FB2412, 0x2D822512, 0xD9662792, 0x29A2DD5F,
+0x6AD2D965, 0xD9646892, 0x68D21A84, 0x6081DA63,
+0x2801CB01, 0xD86266D2, 0x2A622962, 0xED015AFC,
+0x2AD2480B, 0x2AD24D18, 0x62D2DD5E, 0x2D227201,
+0xD15056F3, 0xE2026062, 0x2602CB01, 0x2120A03D,
+0x8B3A2228, 0xE401DD58, 0x2140E600, 0xE01C2D62,
+0xC801005C, 0xD4558B0A, 0xE600D755, 0xED7D2472,
+0x626C7601, 0x8BFB32D3, 0x24D2DD52, 0xE2FE68C2,
+0x2C822829, 0x095CE01E, 0xE01F5DF1, 0x0A5C2D90,
+0x751051F2, 0xED0621A0, 0xD74BE600, 0x8456D44B,
+0x27007601, 0x696C6854, 0x248039D3, 0x8FF67401,
+0xDA477701, 0x2A10E194, 0xE2007A01, 0x7A0F2A20,
+0xD130E805, 0x66102A80, 0x6023626C, 0x89088801,
+0xD240D42A, 0x420B65F2, 0xD131ED01, 0xAF304D18,
+0x65F221D2, 0x8553D43C, 0x620D6642, 0x89073262,
+0xD13BD43A, 0x0009410B, 0xE601D73A, 0x2762AF1A,
+0xD134D41E, 0x410B65F2, 0xD125ED01, 0xD637D436,
+0x460B4D18, 0xAF0D21D2, 0x7F340009, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0x4F2268F6,
+0x85467FF4, 0x2F01E681, 0x666C8547, 0x854881F1,
+0x81F2D209, 0x67F38542, 0x854381F3, 0x81F4E40C,
+0x65636053, 0x420B81F5, 0x7F0C7540, 0x000B4F26,
+0x00000009, 0x001C3D9C, 0x0020245C, 0x0011779A,
+0x001C36F8, 0x001C3B9C, 0x001C3704, 0x0020352C,
+0x002014A0, 0x0020391D, 0x0020391C, 0x00203918,
+0x001C3D98, 0x001C3BB4, 0x001C5960, 0x001C3500,
+0x001C3D30, 0x001C8960, 0x00203504, 0x001C3D00,
+0x0020160C, 0x00117730, 0x00203920, 0x001C582C,
+0x2000A000, 0x0000A000, 0x0011778C, 0x00117792,
+0x00117788, 0x002014CC, 0x002038F4, 0x002034F4,
+0x00201530, 0x001E2130, 0x00203D84, 0x002018A2,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xD19B7FEC, 0x2F12E000, 0x6103D49A, 0x1F4281F2,
+0xDD9ADA99, 0xD69A6813, 0xE0014808, 0x460BDE99,
+0x38EC4800, 0x65A21F03, 0x352052A1, 0xA23E8B01,
+0x60510009, 0x8801C903, 0xA2388B01, 0x52530009,
+0x32E0DE91, 0xD9918B10, 0x64A3490B, 0x4B0BDB90,
+0xDE906403, 0xD791D690, 0xEC01D591, 0x2E02E100,
+0x271026C0, 0x2502AFDF, 0xC8018551, 0xA1578B01,
+0x62510009, 0x4200622D, 0x5E53366A, 0x85E2226D,
+0xC903642C, 0x85E36603, 0x6053650D, 0x40214021,
+0x4500C93F, 0x322A6703, 0x6053252D, 0xC901D17F,
+0x60106C03, 0x8801D97F, 0xDB7F8B05, 0x2120E200,
+0xCB0160B2, 0xD17D2B02, 0x88016011, 0x65A28B0A,
+0x8D042448, 0x9B9E6251, 0xA00322B9, 0x919B2521,
+0x2521221B, 0x37B3EB10, 0x2448895E, 0xD4738B07,
+0x22286241, 0x60638903, 0xA05781F8, 0xD5706473,
+0x46084608, 0x85E26273, 0x46006B50, 0x362C4200,
+0x2BB8C910, 0x8F1F6463, 0x26686603, 0xD2698911,
+0x062D6043, 0x4119616D, 0x6B0E6019, 0x81F820BD,
+0x880160C3, 0x646C8F2C, 0x880F6073, 0xA0278B1B,
+0xD2610009, 0x052D6043, 0x4119615D, 0x670E6019,
+0x645C207D, 0x81F8A01C, 0x890F2668, 0x6043D25B,
+0x6B5D052D, 0x60B94B19, 0x201D610E, 0x60C381F8,
+0x8F0D8801, 0x6473645C, 0xEC00A00A, 0x6043D254,
+0x625D052D, 0x60294219, 0x207D670E, 0x81F8645C,
+0x880285F8, 0x85E1890A, 0x8D07C820, 0xE6DC6203,
+0x60232269, 0x81E1A002, 0x644CE4FF, 0x6210D149,
+0x89012228, 0x644CE4FF, 0x654DEBFF, 0x35B06BBC,
+0xDB368B2B, 0x64A34B0B, 0x410BD135, 0x54036403,
+0x85446E03, 0xC948DB40, 0xDC408808, 0xBEAE8B01,
+0x64B3E502, 0x65E34C0B, 0xDB3DEC01, 0xD13D2DC2,
+0x621260B2, 0x72017001, 0x21228805, 0x2B028F08,
+0x666CE680, 0x6563D238, 0x7549E700, 0x6473420B,
+0xA030D436, 0x7FFF0009, 0x85E28000, 0x20B9EBFC,
+0x610381E2, 0x942A85E3, 0x62032049, 0x450885F8,
+0x81E2201B, 0xC90160C3, 0x40084018, 0x40084008,
+0x4000225B, 0x6023220B, 0x85E481E3, 0x4118E108,
+0x81E4201B, 0xE40262A2, 0x20B98521, 0x67A28121,
+0xCB016071, 0x85F82701, 0x89033042, 0xECE785E2,
+0x81E220C9, 0x490BD41E, 0xA03B0009, 0x7E030009,
+0x001C3D30, 0x00203D90, 0x00203504, 0x001E212C,
+0x002033E8, 0x001C3D00, 0x00117780, 0x002014A0,
+0x0020166C, 0x0011770C, 0x0020391C, 0x0020391D,
+0x00203918, 0x002018A2, 0x001C36F8, 0x00203990,
+0x00203DA0, 0x00203B84, 0x00203C04, 0x00203C84,
+0x00203D04, 0x00203908, 0x002034FC, 0x002014CC,
+0x00203994, 0x00203998, 0x0020245C, 0x00203D88,
+0x00203D8C, 0x602262F2, 0x40094019, 0xC90F4009,
+0x8B0B880A, 0x60E2DE8C, 0x40094019, 0xC90F4009,
+0x8B038808, 0xCB0160A2, 0x2802A006, 0x65E2DE87,
+0x2E527501, 0x286266A2, 0x52F366F2, 0x2622AE83,
+0xD2838551, 0xDE83C802, 0xA0958B01, 0x420B0009,
+0x4E0B64A3, 0x5E036403, 0x85E46503, 0x4918E908,
+0xD77D209B, 0xE04C81E4, 0xDC7C0B7E, 0x7B01D97C,
+0x61C207B6, 0x71016690, 0x8D062668, 0xD4792C12,
+0x420BD279, 0xA070EB01, 0x62512DB2, 0x4B18EB0F,
+0x22B9E102, 0x32104118, 0x85518B0F, 0x2029E2FC,
+0x60518151, 0xCB0172E0, 0x85E12501, 0x202994A3,
+0x85E481E1, 0xA0522049, 0x675181E4, 0x4719677D,
+0x667E6779, 0x7701276D, 0x6903607C, 0x88014918,
+0x25918F3E, 0x6B12D161, 0x21B27B01, 0x660D85E3,
+0x40216063, 0xC93F4021, 0x6C034600, 0x262D322A,
+0xC8016063, 0xDB5ED15D, 0x967D8901, 0xE6002C6B,
+0x666C67CD, 0x40006063, 0x622D021D, 0x8D0E3270,
+0x60436403, 0xE9FF021D, 0x8B013290, 0x01C5A007,
+0x626C7601, 0x3292E904, 0x646C8BEB, 0x60434400,
+0xD15004BD, 0x0B457401, 0x669D6911, 0x89073670,
+0x602D6211, 0x890388FF, 0xE201DB4B, 0x2B2021C1,
+0xECFC8551, 0x815120C9, 0xCB016051, 0xDC472501,
+0x64A34C0B, 0x51F366F2, 0x85EF2612, 0x54F2D244,
+0x650D420B, 0x0009ADE7, 0xE500DC42, 0x420B2C52,
+0x4E0B64A3, 0x54036403, 0x85446E03, 0x6703E908,
+0x65034918, 0x27998541, 0xDB323790, 0x8F0BD932,
+0x6013610D, 0x8B07C820, 0xC9486053, 0x8B038808,
+0xE501BD4D, 0x0009A005, 0x2128D233, 0xBD468901,
+0x64B3E500, 0x490B65E3, 0xADBCEC01, 0x85F22DC2,
+0x7001EE04, 0x31E7610D, 0x8D0281F2, 0xADA97A08,
+0x7F140009, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0xF7FF68F6, 0x2FE68000, 0xD2234F22,
+0x60E36E22, 0x8D02C840, 0xBBF922E2, 0xE2400009,
+0x2E284218, 0xBC048901, 0x60E30009, 0x8905C810,
+0xD21CD41B, 0x0009420B, 0x0009BC03, 0xC80560E3,
+0xBD6D8901, 0x60E30009, 0x8902C802, 0xAC004F26,
+0x4F266EF6, 0x6EF6000B, 0x001C3D3C, 0x00117760,
+0x002014A0, 0x0020166C, 0x00203494, 0x00203DA4,
+0x00203908, 0x002034FC, 0x002014CC, 0x00203974,
+0x0020397C, 0x00203970, 0x00203972, 0x00201530,
+0x002018EE, 0x00203994, 0x00008000, 0x001C3510,
+0x00203D98, 0x002018A2, 0x080A0C0E, 0x00020406,
+0x1A1C1E20, 0x12141618, 0x2E303234, 0x26282A2C,
+0x3A3C3E40, 0x6C625648, 0x41112F26, 0xE2208F18,
+0x890B3123, 0x321CD204, 0xD1026220, 0x412B312C,
+0x00090009, 0x00203412, 0x002033C8, 0x000BE000,
+0x400062F6, 0x40004000, 0x40004000, 0x40004000,
+0x62F6000B, 0x40004000, 0x40004000, 0x40004000,
+0x40184000, 0x62F6000B, 0x40004000, 0x40004000,
+0x40004000, 0x40284000, 0x62F6000B, 0x40004000,
+0x40184000, 0x000B4028, 0xC90F62F6, 0x40054005,
+0x40054005, 0x62F6000B, 0x4005C907, 0x40054005,
+0x62F6000B, 0x4005C903, 0x000B4005, 0xC90162F6,
+0x000B4005, 0x000062F6, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x544F0D0A, 0x46205355, 0x00003A57,
+0x206C754A, 0x32203120, 0x20383030, 0x323A3132,
+0x34333A38, 0x00000000, 0x00000D0A, 0x00000043,
+0x42707372, 0x3D206675, 0x554E203D, 0x202C4C4C,
+0x6E49677A, 0x4E497274, 0x6D754E51, 0x0000003D,
+0x61766E49, 0x2064696C, 0x72657375, 0x20726F20,
+0x2079656B, 0x00214449, 0x52504545, 0x57204D4F,
+0x65746972, 0x6461202C, 0x003D7264, 0x6C617620,
+0x0000003D, 0x00000A0D, 0x435F4D5A, 0x465F444D,
+0x4C445F57, 0x494E495F, 0x00000054, 0x6E6B6E55,
+0x206E776F, 0x6D6D6F63, 0x3D646E61, 0x00000000,
+0x203A3051, 0x00000020, 0x203A3151, 0x00000020,
+0x203A3251, 0x00000020, 0x203A3351, 0x00000020,
+0x203A3451, 0x00000020, 0x2B434741, 0x73696F4E,
+0x61432065, 0x7262696C, 0x6F697461, 0x6166206E,
+0x6F206C69, 0x6974206E, 0x0D0A656D, 0x00000000,
+0x00000072, 0x00205220, 0x00000D0A, 0x62735576,
+0x7473725F, 0x00000A0D, 0x62735576, 0x7375735F,
+0x646E6570, 0x00000A0D, 0x62735576, 0x7365725F,
+0x000A0D6D, 0x00000044, 0x44387570, 0x72637365,
+0x6F747069, 0x3D584572, 0x00000000, 0x00000047,
+0x00000042, 0x72746E49, 0x6D652051, 0x2C797470,
+0x49677A20, 0x4972746E, 0x754E514E, 0x00003D6D,
+0x654C7245, 0x0000006E, 0x00000049, 0x20746F4E,
+0x756F6E65, 0x49206867, 0x4220514E, 0x0A0D6675,
+0x00000000, 0x000000FF, 0x00020001, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00020003, 0x01090108,
+0x0002010A, 0x02000003, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x000000FF, 0x00020001, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00020003, 0x01090108,
+0x0002010A, 0x00030003, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00FF010F, 0x01090108,
+0x010B010A, 0x0200010F, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00FF010F, 0x01090108,
+0x010B010A, 0x010F010F, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00205220, 0x00000046, 0x00000059,
+0x73204142, 0x003D7165, 0x49544120, 0x0000204D,
+0x00000000, 0x00000000, 0x002E0209, 0x80000101,
+0x000409FA, 0x00FF0400, 0x05070000, 0x02000201,
+0x82050700, 0x00020002, 0x03830507, 0x07010040,
+0x40030405, 0x02090100, 0x0101002E, 0x09FA8000,
+0x04000004, 0x000000FF, 0x02010507, 0x07000040,
+0x40028205, 0x05070000, 0x00400383, 0x04050701,
+0x00004002, 0x00000000, 0x00000000, 0x07090000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+};
+
+const u32_t zcFwImageSize=15936;
diff --git a/drivers/staging/otus/hal/hpfwuinit.c b/drivers/staging/otus/hal/hpfwuinit.c
new file mode 100644
index 000000000000..ed80ffafaffc
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwuinit.c
@@ -0,0 +1,240 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x7FFC4F22, 0xD695D494, 0x0009460B,
+0xD494E570, 0x4518B01E, 0x89042008, 0xD690D492,
+0x462B7F04, 0xB0124F26, 0xD2900009, 0x420BD490,
+0xE6000009, 0x949AD58F, 0xC8406052, 0x2F028F03,
+0x8FF93642, 0x7F047601, 0x000B4F26, 0xD28A0009,
+0x0009422B, 0x2FD62FC6, 0x4F222FE6, 0xD6877FEC,
+0x626061F3, 0x2F208461, 0x846280F1, 0x80F27110,
+0x6D438463, 0x846480F3, 0x80F46413, 0x6C538465,
+0x846680F5, 0x80F6E500, 0xD77D8467, 0x846880F7,
+0x80F8EE04, 0x80F98469, 0x80FA846A, 0x80FB846B,
+0x80FC846C, 0x80FD846D, 0x80FE846E, 0x80FF846F,
+0x6653655C, 0x7501367C, 0x665C6260, 0x242036E3,
+0x74018FF6, 0x66F32F16, 0xE7107604, 0xB00D65C3,
+0x6E0364D3, 0xD46B7F04, 0x420BD26B, 0x60E36503,
+0x4F267F14, 0x6DF66EF6, 0x6CF6000B, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0x3F3C933A, 0x4108E141,
+0x31FCE200, 0x11733526, 0x21521162, 0x11418D02,
+0xE0FFA098, 0x4A18EA01, 0x262066F3, 0x32A27201,
+0x76018FFB, 0x6BE3EE00, 0xE0446CF3, 0x00FE4008,
+0x450BD556, 0x660361B3, 0x4008E043, 0x6DC004FE,
+0x014C6063, 0x31EC3EDC, 0x60E36E1C, 0x7B0107FC,
+0x2C703BA2, 0x8FE80FD4, 0xE0427C01, 0xEB004008,
+0x70FC07FE, 0x6EB36CB3, 0xA0200AFE, 0x2710EDFF,
+0x7C01FEE0, 0x60C36CCC, 0x657002FC, 0x6BBC3B2C,
+0x01FC60B3, 0x0F1460C3, 0x0F2460B3, 0x04FC60C3,
+0x342C7E01, 0x01FC604C, 0x251A62D3, 0xD43C225A,
+0x2750602C, 0x064E4008, 0x2D6A4D19, 0x3EA27701,
+0x66D78BDF, 0x4018E001, 0x0F646563, 0x70014519,
+0x0F544629, 0x0F647001, 0x70014619, 0x90420F64,
+0xE0450EFE, 0xEA014008, 0xE0460FF6, 0x4A184008,
+0xED0067F3, 0x0FF637AC, 0x0FF67004, 0xE345E104,
+0x7C014308, 0x6CCC33FC, 0x60C36432, 0x5531024C,
+0x6BBC3B2C, 0x045C60B3, 0x60C35A32, 0x60B30A44,
+0x60C30F24, 0x6A7006FC, 0x606C362C, 0x66E005FC,
+0x6A5C64AC, 0x626C24AA, 0x89053420, 0x4D084D08,
+0xCB0460D3, 0x600BA006, 0x7D014110, 0x8FD67701,
+0xE0007E01, 0x3F3C9308, 0x6EF64F26, 0x6CF66DF6,
+0x000B6BF6, 0x01386AF6, 0x00000120, 0x00200D54,
+0x002002BE, 0x00102800, 0x00200D64, 0x0010F00A,
+0x0010F000, 0x001C001C, 0x00103252, 0x00200DA0,
+0x0010FFFC, 0x00200D7C, 0x0020032C, 0x00200370,
+0x00200954, 0x0009000B, 0x2FD62FC6, 0x4F222FE6,
+0x6D436C53, 0xEE00A004, 0x7E0164D4, 0x644CBFF2,
+0x8BF93EC2, 0x6EF64F26, 0x000B6DF6, 0xE5006CF6,
+0x6643A002, 0x76017501, 0x22286260, 0xAFE38BFA,
+0x2FE60009, 0x75076253, 0xE1086753, 0x6043EE0A,
+0x4409C90F, 0x650330E2, 0x8D014409, 0xE630E637,
+0x4110365C, 0x8FF22760, 0xE00077FF, 0x000B8028,
+0x4F226EF6, 0xBFE47FEC, 0xBFD865F3, 0x7F1464F3,
+0x000B4F26, 0x4F22E000, 0xBFDA7FEC, 0x64F365F3,
+0x7406BFCD, 0x4F267F14, 0xE000000B, 0x4F222FE6,
+0x62537FEC, 0x65F36E43, 0x6423BFCB, 0x64E3BFBF,
+0x64F3BFBD, 0xBFBAD403, 0x7F140009, 0x000B4F26,
+0x00006EF6, 0x00200DB0, 0x89004011, 0x4111600B,
+0x4F228906, 0x611BB004, 0x000B4F26, 0x0009600B,
+0x620D2F26, 0x8F413020, 0x40180019, 0x8B0D3016,
+0x31043104, 0x31043104, 0x31043104, 0x31043104,
+0x890062F6, 0x4119310C, 0x6013000B, 0x41296219,
+0x20084018, 0x31048927, 0x31043104, 0x31043104,
+0x31043104, 0x31043104, 0x31043104, 0x31043104,
+0x31043104, 0x61193104, 0x3204221D, 0x32043204,
+0x32043204, 0x32043204, 0x32043204, 0x32043204,
+0x32043204, 0x32043204, 0x89003204, 0x4229320C,
+0x000B6023, 0xE00062F6, 0x62F6000B, 0x42286213,
+0x42244129, 0x42243104, 0x42243104, 0x42243104,
+0x42243104, 0x42243104, 0x42243104, 0x42243104,
+0x42243104, 0x42243104, 0x42243104, 0x42243104,
+0x42243104, 0x42243104, 0x42243104, 0x42243104,
+0x89003104, 0x6013310C, 0x62F6000B, 0x2F262F16,
+0x51F552F3, 0x52F22129, 0x52F41210, 0x212951F6,
+0x121152F2, 0x000B62F6, 0x000061F6, 0x51F32F16,
+0x310050F1, 0x51F48B02, 0x310050F2, 0x000B0029,
+0x000061F6, 0x51F32F16, 0x310050F1, 0x51F48B06,
+0x310050F2, 0xCA010029, 0x61F6000B, 0x000BE001,
+0x000061F6, 0x50F0000B, 0x2F262F16, 0xE10052F2,
+0x12001211, 0x000B62F6, 0x000061F6, 0x2F162F06,
+0x8B264115, 0x3103E040, 0x2F26892B, 0x52F62F36,
+0xE02053F5, 0x8B053103, 0xE3006233, 0x89093100,
+0x3108A002, 0x8B0F2338, 0xD0064F22, 0x6023400B,
+0x4F266203, 0x112151F4, 0x63F61130, 0x61F662F6,
+0x60F6000B, 0x002007F4, 0x4100C709, 0x0123011D,
+0x51F20009, 0x110150F4, 0x110050F3, 0x000B61F6,
+0x51F260F6, 0x1101E000, 0x61F61100, 0x60F6000B,
+0x01300000, 0x0128012C, 0x01200124, 0x0118011C,
+0x0106010A, 0x00FE0102, 0x00E200E6, 0x00DA00DE,
+0x00CC00D0, 0x00C400C8, 0x00A800AC, 0x00A000A4,
+0x008C0090, 0x00840088, 0x0066006A, 0x005E0062,
+0x42244300, 0x42244300, 0x42244300, 0x43286133,
+0x43084318, 0x42284308, 0x42084218, 0x41094208,
+0xAFAF4109, 0x4300221B, 0x43004224, 0x43004224,
+0x61334224, 0x43184328, 0x42184228, 0xAFA14119,
+0x4300221B, 0x43004224, 0x43004224, 0x61334224,
+0x43084328, 0x42284308, 0x42084208, 0x41094119,
+0xAF8F4109, 0x4300221B, 0x43004224, 0x43004224,
+0x61334224, 0x212D4328, 0x6213AF84, 0x42244300,
+0x42244300, 0x42244300, 0x43186133, 0x43084308,
+0x42084218, 0x41294208, 0x41094109, 0x221BAF72,
+0x42244300, 0x42244300, 0x42244300, 0x43186133,
+0x41294218, 0xAF654119, 0x4300221B, 0x43004224,
+0x43004224, 0x43004224, 0x43004224, 0x43004224,
+0x43004224, 0x4224AF56, 0x2F162F06, 0x8B264115,
+0x3103E040, 0x2F26892B, 0x52F62F36, 0xE02053F5,
+0x8B053103, 0xE2006323, 0x89093100, 0x3108A002,
+0x8B0F2228, 0xD0064F22, 0x6033400B, 0x4F266303,
+0x112151F4, 0x63F61130, 0x61F662F6, 0x60F6000B,
+0x002008B4, 0x4100C709, 0x0123011D, 0x51F20009,
+0x110150F4, 0x110050F3, 0x000B61F6, 0x51F260F6,
+0x1101E000, 0x61F61100, 0x60F6000B, 0x012E0000,
+0x0126012A, 0x011E0122, 0x0116011A, 0x01040108,
+0x00FC0100, 0x00E000E4, 0x00D800DC, 0x00CC00D0,
+0x00C400C8, 0x00A800AC, 0x00A000A4, 0x008C0090,
+0x00840088, 0x0066006A, 0x005E0062, 0x43254201,
+0x43254201, 0x43254201, 0x42296123, 0x42094219,
+0x43294209, 0x43094319, 0x41084309, 0xAFAF4108,
+0x4201231B, 0x42014325, 0x42014325, 0x61234325,
+0x42194229, 0x43194329, 0xAFA14118, 0x4201231B,
+0x42014325, 0x42014325, 0x61234325, 0x42094229,
+0x43294209, 0x43094309, 0x41084118, 0xAF8F4108,
+0x4201231B, 0x42014325, 0x42014325, 0x61234325,
+0xAF854229, 0x4201231D, 0x42014325, 0x42014325,
+0x61234325, 0x42094219, 0x43194209, 0x43094309,
+0x41084128, 0xAF734108, 0x4201231B, 0x42014325,
+0x42014325, 0x61234325, 0x43194219, 0x41184128,
+0x231BAF66, 0x43254201, 0x43254201, 0x43254201,
+0x43254201, 0x43254201, 0x43254201, 0xAF574201,
+0x00004325, 0x080A0C0E, 0x00020406, 0x1A1C1E20,
+0x12141618, 0x2E303234, 0x26282A2C, 0x3A3C3E40,
+0x6C625648, 0x41112F26, 0xE2208F18, 0x890B3123,
+0x321CD204, 0xD1026220, 0x412B312C, 0x00090009,
+0x0020081E, 0x002007D4, 0x000BE000, 0x400062F6,
+0x40004000, 0x40004000, 0x40004000, 0x62F6000B,
+0x40004000, 0x40004000, 0x40004000, 0x40184000,
+0x62F6000B, 0x40004000, 0x40004000, 0x40004000,
+0x40284000, 0x62F6000B, 0x40004000, 0x40184000,
+0x000B4028, 0xC90F62F6, 0x40054005, 0x40054005,
+0x62F6000B, 0x4005C907, 0x40054005, 0x62F6000B,
+0x4005C903, 0x000B4005, 0xC90162F6, 0x000B4005,
+0x000062F6, 0x080A0C0E, 0x00020406, 0x1A1C1E20,
+0x12141618, 0x2E303234, 0x26282A2C, 0x3A3C3E40,
+0x6C625648, 0x41112F26, 0xE2208F18, 0x890B3123,
+0x321CD204, 0xD1026220, 0x412B312C, 0x00090009,
+0x002008DE, 0x00200894, 0x000BE000, 0x400162F6,
+0x40014001, 0x40014001, 0x40014001, 0x62F6000B,
+0x40014001, 0x40014001, 0x40014001, 0x40194001,
+0x62F6000B, 0x40014001, 0x40014001, 0x40014001,
+0x40294001, 0x62F6000B, 0x40014001, 0x40194001,
+0x000B4029, 0x400462F6, 0x40044004, 0xC90F4004,
+0x62F6000B, 0x40044004, 0xC9074004, 0x62F6000B,
+0x40044004, 0x000BC903, 0x400462F6, 0x000BC901,
+0x000062F6, 0x00000000, 0x77073096, 0xEE0E612C,
+0x990951BA, 0x076DC419, 0x706AF48F, 0xE963A535,
+0x9E6495A3, 0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E,
+0x97D2D988, 0x09B64C2B, 0x7EB17CBD, 0xE7B82D07,
+0x90BF1D91, 0x1DB71064, 0x6AB020F2, 0xF3B97148,
+0x84BE41DE, 0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551,
+0x83D385C7, 0x136C9856, 0x646BA8C0, 0xFD62F97A,
+0x8A65C9EC, 0x14015C4F, 0x63066CD9, 0xFA0F3D63,
+0x8D080DF5, 0x3B6E20C8, 0x4C69105E, 0xD56041E4,
+0xA2677172, 0x3C03E4D1, 0x4B04D447, 0xD20D85FD,
+0xA50AB56B, 0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6,
+0xACBCF940, 0x32D86CE3, 0x45DF5C75, 0xDCD60DCF,
+0xABD13D59, 0x26D930AC, 0x51DE003A, 0xC8D75180,
+0xBFD06116, 0x21B4F4B5, 0x56B3C423, 0xCFBA9599,
+0xB8BDA50F, 0x2802B89E, 0x5F058808, 0xC60CD9B2,
+0xB10BE924, 0x2F6F7C87, 0x58684C11, 0xC1611DAB,
+0xB6662D3D, 0x76DC4190, 0x01DB7106, 0x98D220BC,
+0xEFD5102A, 0x71B18589, 0x06B6B51F, 0x9FBFE4A5,
+0xE8B8D433, 0x7807C9A2, 0x0F00F934, 0x9609A88E,
+0xE10E9818, 0x7F6A0DBB, 0x086D3D2D, 0x91646C97,
+0xE6635C01, 0x6B6B51F4, 0x1C6C6162, 0x856530D8,
+0xF262004E, 0x6C0695ED, 0x1B01A57B, 0x8208F4C1,
+0xF50FC457, 0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA,
+0xFCB9887C, 0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3,
+0xFBD44C65, 0x4DB26158, 0x3AB551CE, 0xA3BC0074,
+0xD4BB30E2, 0x4ADFA541, 0x3DD895D7, 0xA4D1C46D,
+0xD3D6F4FB, 0x4369E96A, 0x346ED9FC, 0xAD678846,
+0xDA60B8D0, 0x44042D73, 0x33031DE5, 0xAA0A4C5F,
+0xDD0D7CC9, 0x5005713C, 0x270241AA, 0xBE0B1010,
+0xC90C2086, 0x5768B525, 0x206F85B3, 0xB966D409,
+0xCE61E49F, 0x5EDEF90E, 0x29D9C998, 0xB0D09822,
+0xC7D7A8B4, 0x59B33D17, 0x2EB40D81, 0xB7BD5C3B,
+0xC0BA6CAD, 0xEDB88320, 0x9ABFB3B6, 0x03B6E20C,
+0x74B1D29A, 0xEAD54739, 0x9DD277AF, 0x04DB2615,
+0x73DC1683, 0xE3630B12, 0x94643B84, 0x0D6D6A3E,
+0x7A6A5AA8, 0xE40ECF0B, 0x9309FF9D, 0x0A00AE27,
+0x7D079EB1, 0xF00F9344, 0x8708A3D2, 0x1E01F268,
+0x6906C2FE, 0xF762575D, 0x806567CB, 0x196C3671,
+0x6E6B06E7, 0xFED41B76, 0x89D32BE0, 0x10DA7A5A,
+0x67DD4ACC, 0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43,
+0x60B08ED5, 0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4,
+0x4FDFF252, 0xD1BB67F1, 0xA6BC5767, 0x3FB506DD,
+0x48B2364B, 0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6,
+0x41047A60, 0xDF60EFC3, 0xA867DF55, 0x316E8EEF,
+0x4669BE79, 0xCB61B38C, 0xBC66831A, 0x256FD2A0,
+0x5268E236, 0xCC0C7795, 0xBB0B4703, 0x220216B9,
+0x5505262F, 0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92,
+0x5CB36A04, 0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B,
+0x5BDEAE1D, 0x9B64C2B0, 0xEC63F226, 0x756AA39C,
+0x026D930A, 0x9C0906A9, 0xEB0E363F, 0x72076785,
+0x05005713, 0x95BF4A82, 0xE2B87A14, 0x7BB12BAE,
+0x0CB61B38, 0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7,
+0x0BDBDF21, 0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8,
+0x1FDA836E, 0x81BE16CD, 0xF6B9265B, 0x6FB077E1,
+0x18B74777, 0x88085AE6, 0xFF0F6A70, 0x66063BCA,
+0x11010B5C, 0x8F659EFF, 0xF862AE69, 0x616BFFD3,
+0x166CCF45, 0xA00AE278, 0xD70DD2EE, 0x4E048354,
+0x3903B3C2, 0xA7672661, 0xD06016F7, 0x4969474D,
+0x3E6E77DB, 0xAED16A4A, 0xD9D65ADC, 0x40DF0B66,
+0x37D83BF0, 0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F,
+0x30B5FFE9, 0xBDBDF21C, 0xCABAC28A, 0x53B39330,
+0x24B4A3A6, 0xBAD03605, 0xCDD70693, 0x54DE5729,
+0x23D967BF, 0xB3667A2E, 0xC4614AB8, 0x5D681B02,
+0x2A6F2B94, 0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B,
+0x2D02EF8D, 0x544F0D0A, 0x50205355, 0x20312D48,
+0x003A5746, 0x72636564, 0x69747079, 0x65206E6F,
+0x726F7272, 0x0A0D2121, 0x00000000, 0x6564667A,
+0x70797263, 0x65725F74, 0x616C7567, 0x79726F74,
+0x6261745F, 0x7220656C, 0x203D7465, 0x00000000,
+0x45485441, 0x38731652, 0x89ACFF91, 0xEE55D178,
+0xEE000D0A, };
+
+const u32_t zcFwImageSize=3508;
diff --git a/drivers/staging/otus/hal/hpmain.c b/drivers/staging/otus/hal/hpmain.c
new file mode 100644
index 000000000000..2e65c466aae8
--- /dev/null
+++ b/drivers/staging/otus/hal/hpmain.c
@@ -0,0 +1,4643 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+#include "hpani.h"
+#include "hpusb.h"
+#include "otus.ini"
+
+extern const u32_t zcFwImage[];
+extern const u32_t zcFwImageSize;
+extern const u32_t zcDKFwImage[];
+extern const u32_t zcDKFwImageSize;
+extern const u32_t zcFwImageSPI[];
+extern const u32_t zcFwImageSPISize;
+
+#ifdef ZM_OTUS_LINUX_PHASE_2
+extern const u32_t zcFwBufImage[];
+extern const u32_t zcFwBufImageSize;
+extern const u32_t zcP2FwImage[];
+extern const u32_t zcP2FwImageSize;
+#endif
+extern void zfInitCmdQueue(zdev_t* dev);
+extern u16_t zfIssueCmd(zdev_t* dev, u32_t* cmd, u16_t cmdLen,
+        u16_t src, u8_t* buf);
+extern void zfIdlRsp(zdev_t* dev, u32_t* rsp, u16_t rspLen);
+extern u16_t zfDelayWriteInternalReg(zdev_t* dev, u32_t addr, u32_t val);
+extern u16_t zfFlushDelayWrite(zdev_t* dev);
+extern void zfUsbInit(zdev_t* dev);
+extern u16_t zfFirmwareDownload(zdev_t* dev, u32_t* fw, u32_t len, u32_t offset);
+extern u16_t zfFirmwareDownloadNotJump(zdev_t* dev, u32_t* fw, u32_t len, u32_t offset);
+extern void zfUsbFree(zdev_t* dev);
+extern u16_t zfCwmIsExtChanBusy(u32_t ctlBusy, u32_t extBusy);
+extern void zfCoreCwmBusy(zdev_t* dev, u16_t busy);
+
+/* Prototypes */
+void zfInitRf(zdev_t* dev, u32_t frequency);
+void zfInitPhy(zdev_t* dev, u32_t frequency, u8_t bw40);
+void zfInitMac(zdev_t* dev);
+
+void zfSetPowerCalTable(zdev_t* dev, u32_t frequency, u8_t bw40, u8_t extOffset);
+void zfInitPowerCal(zdev_t* dev);
+
+#ifdef ZM_DRV_INIT_USB_MODE
+void zfInitUsbMode(zdev_t* dev);
+u16_t zfHpUsbReset(zdev_t* dev);
+#endif
+
+/* Bank 0 1 2 3 5 6 7 */
+void zfSetRfRegs(zdev_t* dev, u32_t frequency);
+/* Bank 4 */
+void zfSetBank4AndPowerTable(zdev_t* dev, u32_t frequency, u8_t bw40,
+        u8_t extOffset);
+/* Get param for turnoffdyn */
+void zfGetHwTurnOffdynParam(zdev_t* dev,
+                            u32_t frequency, u8_t bw40, u8_t extOffset,
+                            int* delta_slope_coeff_exp,
+                            int* delta_slope_coeff_man,
+                            int* delta_slope_coeff_exp_shgi,
+                            int* delta_slope_coeff_man_shgi);
+
+void zfSelAdcClk(zdev_t* dev, u8_t bw40, u32_t frequency);
+u32_t zfHpEchoCommand(zdev_t* dev, u32_t value);
+
+
+
+#define zm_hp_priv(x) (((struct zsHpPriv*)wd->hpPrivate)->x)
+struct zsHpPriv zgHpPriv;
+
+#define ZM_FIRMWARE_WLAN_ADDR           0x200000
+#define ZM_FIRMWARE_SPI_ADDR      0x114000
+/* 0: real chip     1: FPGA test */
+#define ZM_FPGA_PHY  0
+
+#define reg_write(addr, val) zfDelayWriteInternalReg(dev, addr+0x1bc000, val)
+#define zm_min(A, B) ((A>B)? B:A)
+
+
+/******************** Intialization ********************/
+u16_t zfHpInit(zdev_t* dev, u32_t frequency)
+{
+    u16_t ret;
+    zmw_get_wlan_dev(dev);
+
+    /* Initializa HAL Plus private variables */
+    wd->hpPrivate = &zgHpPriv;
+
+    ((struct zsHpPriv*)wd->hpPrivate)->halCapability = ZM_HP_CAP_11N;
+
+    ((struct zsHpPriv*)wd->hpPrivate)->hwFrequency = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->hwBw40 = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->hwExtOffset = 0;
+
+    ((struct zsHpPriv*)wd->hpPrivate)->disableDfsCh = 0;
+
+    ((struct zsHpPriv*)wd->hpPrivate)->ledMode[0] = 1;
+    ((struct zsHpPriv*)wd->hpPrivate)->ledMode[1] = 1;
+    ((struct zsHpPriv*)wd->hpPrivate)->strongRSSI = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->rxStrongRSSI = 0;
+
+    ((struct zsHpPriv*)wd->hpPrivate)->slotType = 1;
+    ((struct zsHpPriv*)wd->hpPrivate)->aggPktNum = 0x10000a;
+
+    ((struct zsHpPriv*)wd->hpPrivate)->eepromImageIndex = 0;
+
+
+    ((struct zsHpPriv*)wd->hpPrivate)->eepromImageRdReq     = 0;
+#ifdef ZM_OTUS_RX_STREAM_MODE
+    ((struct zsHpPriv*)wd->hpPrivate)->remainBuf = NULL;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxRemainLen = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxPktLen = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxPadLen = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxTransferLen = 0;
+#endif
+
+    ((struct zsHpPriv*)wd->hpPrivate)->enableBBHeavyClip = 1;
+    ((struct zsHpPriv*)wd->hpPrivate)->hwBBHeavyClip     = 1; // force enable 8107
+    ((struct zsHpPriv*)wd->hpPrivate)->doBBHeavyClip     = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->setValueHeavyClip = 0;
+
+
+    /* Initialize driver core */
+    zfInitCmdQueue(dev);
+
+    /* Initialize USB */
+    zfUsbInit(dev);
+
+#if ZM_SW_LOOP_BACK != 1
+
+    /* TODO : [Download FW] */
+    if (wd->modeMDKEnable)
+    {
+        /* download the MDK firmware */
+        if ((ret = zfFirmwareDownload(dev, (u32_t*)zcDKFwImage,
+                (u32_t)zcDKFwImageSize, ZM_FIRMWARE_WLAN_ADDR)) != ZM_SUCCESS)
+        {
+            /* TODO : exception handling */
+            //return 1;
+        }
+    }
+    else
+    {
+    #ifndef ZM_OTUS_LINUX_PHASE_2
+        /* donwload the normal frimware */
+        if ((ret = zfFirmwareDownload(dev, (u32_t*)zcFwImage,
+                (u32_t)zcFwImageSize, ZM_FIRMWARE_WLAN_ADDR)) != ZM_SUCCESS)
+        {
+            /* TODO : exception handling */
+            //return 1;
+        }
+    #else
+
+        // 1-PH fw: ReadMac() store some global variable
+        if ((ret = zfFirmwareDownloadNotJump(dev, (u32_t*)zcFwBufImage,
+                (u32_t)zcFwBufImageSize, 0x102800)) != ZM_SUCCESS)
+        {
+            DbgPrint("Dl zcFwBufImage failed!");
+        }
+
+        zfwSleep(dev, 1000);
+
+        if ((ret = zfFirmwareDownload(dev, (u32_t*)zcFwImage,
+                (u32_t)zcFwImageSize, ZM_FIRMWARE_WLAN_ADDR)) != ZM_SUCCESS)
+        {
+            DbgPrint("Dl zcFwBufImage failed!");
+        }
+    #endif
+    }
+#endif
+
+#ifdef ZM_DRV_INIT_USB_MODE
+    /* Init USB Mode */
+    zfInitUsbMode(dev);
+
+    /* Do the USB Reset */
+    zfHpUsbReset(dev);
+#endif
+
+/* Register setting */
+/* ZM_DRIVER_MODEL_TYPE_MDK
+ *  1=>for MDK, disable init RF, PHY, and MAC,
+ *  0=>normal init
+ */
+//#if ((ZM_SW_LOOP_BACK != 1) && (ZM_DRIVER_MODEL_TYPE_MDK !=1))
+#if ZM_SW_LOOP_BACK != 1
+    if(!wd->modeMDKEnable)
+    {
+        /* Init MAC */
+        zfInitMac(dev);
+
+    #if ZM_FW_LOOP_BACK != 1
+        /* Init PHY */
+        zfInitPhy(dev, frequency, 0);
+
+        /* Init RF */
+        zfInitRf(dev, frequency);
+
+        #if ZM_FPGA_PHY == 0
+        /* BringUp issue */
+        //zfDelayWriteInternalReg(dev, 0x9800+0x1bc000, 0x10000007);
+        //zfFlushDelayWrite(dev);
+        #endif
+
+    #endif /* end of ZM_FW_LOOP_BACK != 1 */
+    }
+#endif /* end of ((ZM_SW_LOOP_BACK != 1) && (ZM_DRIVER_MODEL_TYPE_MDK !=1)) */
+
+    zfHpEchoCommand(dev, 0xAABBCCDD);
+
+    return 0;
+}
+
+
+u16_t zfHpReinit(zdev_t* dev, u32_t frequency)
+{
+    u16_t ret;
+    zmw_get_wlan_dev(dev);
+
+    ((struct zsHpPriv*)wd->hpPrivate)->halReInit = 1;
+
+    ((struct zsHpPriv*)wd->hpPrivate)->strongRSSI = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->rxStrongRSSI = 0;
+
+#ifdef ZM_OTUS_RX_STREAM_MODE
+    if (((struct zsHpPriv*)wd->hpPrivate)->remainBuf != NULL)
+    {
+        zfwBufFree(dev, ((struct zsHpPriv*)wd->hpPrivate)->remainBuf, 0);
+    }
+    ((struct zsHpPriv*)wd->hpPrivate)->remainBuf = NULL;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxRemainLen = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxPktLen = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxPadLen = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxTransferLen = 0;
+#endif
+
+    zfInitCmdQueue(dev);
+    zfCoreReinit(dev);
+
+    #ifndef ZM_OTUS_LINUX_PHASE_2
+    /* Download firmware */
+    if ((ret = zfFirmwareDownload(dev, (u32_t*)zcFwImage,
+            (u32_t)zcFwImageSize, ZM_FIRMWARE_WLAN_ADDR)) != ZM_SUCCESS)
+    {
+        /* TODO : exception handling */
+        //return 1;
+    }
+    #else
+    if ((ret = zfFirmwareDownload(dev, (u32_t*)zcP2FwImage,
+            (u32_t)zcP2FwImageSize, ZM_FIRMWARE_WLAN_ADDR)) != ZM_SUCCESS)
+    {
+        /* TODO : exception handling */
+        //return 1;
+    }
+    #endif
+
+#ifdef ZM_DRV_INIT_USB_MODE
+    /* Init USB Mode */
+    zfInitUsbMode(dev);
+
+    /* Do the USB Reset */
+    zfHpUsbReset(dev);
+#endif
+
+    /* Init MAC */
+    zfInitMac(dev);
+
+    /* Init PHY */
+    zfInitPhy(dev, frequency, 0);
+    /* Init RF */
+    zfInitRf(dev, frequency);
+
+    #if ZM_FPGA_PHY == 0
+    /* BringUp issue */
+    //zfDelayWriteInternalReg(dev, 0x9800+0x1bc000, 0x10000007);
+    //zfFlushDelayWrite(dev);
+    #endif
+
+    zfHpEchoCommand(dev, 0xAABBCCDD);
+
+    return 0;
+}
+
+
+u16_t zfHpRelease(zdev_t* dev)
+{
+    /* Free USB resource */
+    zfUsbFree(dev);
+
+    return 0;
+}
+
+/* MDK mode setting for dontRetransmit */
+void zfHpConfigFM(zdev_t* dev, u32_t RxMaxSize, u32_t DontRetransmit)
+{
+    u32_t cmd[3];
+    u16_t ret;
+
+    cmd[0] = 8 | (ZM_CMD_CONFIG << 8);
+    cmd[1] = RxMaxSize;          /* zgRxMaxSize */
+    cmd[2] = DontRetransmit;     /* zgDontRetransmit */
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_OID_INTERNAL_WRITE, 0);
+}
+
+const u8_t zcXpdToPd[16] =
+{
+ /* 0x0, 0x1, 0x2, 0x3, 0x4, 0x5, 0x6, 0x7, 0x8, 0x9, 0xA, 0xB, 0xC, 0xD, 0xE, 0xF */
+    0x2, 0x2, 0x2, 0x1, 0x2, 0x2, 0x6, 0x2, 0x2, 0x3, 0x7, 0x2, 0xB, 0x2, 0x2, 0x2
+};
+
+/******************** RF and PHY ********************/
+
+void zfInitPhy(zdev_t* dev,  u32_t frequency, u8_t bw40)
+{
+    u16_t i, j, k;
+    u16_t entries;
+    u16_t modesIndex = 0;
+    u16_t freqIndex = 0;
+    u32_t tmp, tmp1;
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+    u32_t eepromBoardData[15][6] = {
+    /* Register   A-20        A-20/40     G-20/40     G-20        G-Turbo    */
+        {0x9964,    0,      0,      0,      0,      0},
+        {0x9960,    0,      0,      0,      0,      0},
+        {0xb960,    0,      0,      0,      0,      0},
+        {0x9844,    0,      0,      0,      0,      0},
+        {0x9850,    0,      0,      0,      0,      0},
+        {0x9834,    0,      0,      0,      0,      0},
+        {0x9828,    0,      0,      0,      0,      0},
+        {0xc864,    0,      0,      0,      0,      0},
+        {0x9848,    0,      0,      0,      0,      0},
+        {0xb848,    0,      0,      0,      0,      0},
+        {0xa20c,    0,      0,      0,      0,      0},
+        {0xc20c,    0,      0,      0,      0,      0},
+        {0x9920,    0,      0,      0,      0,      0},
+        {0xb920,    0,      0,      0,      0,      0},
+        {0xa258,    0,      0,      0,      0,      0},
+    };
+
+    /* #1 Save the initial value of the related RIFS register settings */
+    //((struct zsHpPriv*)wd->hpPrivate)->isInitialPhy++;
+
+    /*
+     * Setup the indices for the next set of register array writes
+     * PHY mode is static20 / 2040
+     * Frequency is 2.4GHz (B) / 5GHz (A)
+     */
+    if ( frequency > ZM_CH_G_14 )
+    {
+        /* 5GHz */
+        freqIndex  = 1;
+        if (bw40)
+        {
+            modesIndex = 2;
+            zm_debug_msg0("init ar5416Modes in 2: A-20/40");
+        }
+        else
+        {
+            modesIndex = 1;
+            zm_debug_msg0("init ar5416Modes in 1: A-20");
+        }
+    }
+    else
+    {
+        /* 2.4GHz */
+        freqIndex  = 2;
+        if (bw40)
+        {
+            modesIndex = 3;
+            zm_debug_msg0("init ar5416Modes in 3: G-20/40");
+        }
+        else
+        {
+            modesIndex = 4;
+            zm_debug_msg0("init ar5416Modes in 4: G-20");
+        }
+    }
+
+
+#if ZM_FPGA_PHY == 1
+    /* Starting External Hainan Register Initialization */
+    /* TODO: */
+
+    zfwSleep(dev, 10);
+#endif
+
+    /*
+     *Set correct Baseband to analog shift setting to access analog chips.
+     */
+    //reg_write(PHY_BASE, 0x00000007);
+//    reg_write(0x9800, 0x00000007);
+
+    /*
+     * Write addac shifts
+     */
+     // do this in firmware
+
+
+
+    /* Zeroize board data */
+    for (j=0; j<15; j++)
+    {
+        for (k=1; k<=4; k++)
+        {
+            eepromBoardData[j][k] = 0;
+        }
+    }
+     /*
+     * Register setting by mode
+     */
+
+    entries = sizeof(ar5416Modes) / sizeof(*ar5416Modes);
+    zm_msg1_scan(ZM_LV_2, "Modes register setting entries=", entries);
+    for (i=0; i<entries; i++)
+    {
+#if 0
+        if ( ((struct zsHpPriv*)wd->hpPrivate)->hwNotFirstInit && (ar5416Modes[i][0] == 0xa27c) )
+        {
+            /* Force disable CR671 bit20 / 7823                                            */
+            /* The bug has to do with the polarity of the pdadc offset calibration.  There */
+            /* is an initial calibration that is OK, and there is a continuous             */
+            /* calibration that updates the pddac with the wrong polarity.  Fortunately    */
+            /* the second loop can be disabled with a bit called en_pd_dc_offset_thr.      */
+
+            reg_write(ar5416Modes[i][0], (ar5416Modes[i][modesIndex]& 0xffefffff) );
+            ((struct zsHpPriv*)wd->hpPrivate)->hwNotFirstInit = 1;
+        }
+        else
+        {
+#endif
+            /* FirstTime Init or not 0xa27c(CR671) */
+            reg_write(ar5416Modes[i][0], ar5416Modes[i][modesIndex]);
+//        }
+        /* Initialize board data */
+        for (j=0; j<15; j++)
+        {
+            if (ar5416Modes[i][0] == eepromBoardData[j][0])
+            {
+                for (k=1; k<=4; k++)
+                {
+                    eepromBoardData[j][k] = ar5416Modes[i][k];
+                }
+            }
+        }
+        /* #1 Save the initial value of the related RIFS register settings */
+        //if( ((struct zsHpPriv*)wd->hpPrivate)->isInitialPhy == 1 )
+        {
+            switch(ar5416Modes[i][0])
+            {
+                case 0x9850 :
+                    ((struct zsHpPriv*)wd->hpPrivate)->initDesiredSigSize           = ar5416Modes[i][modesIndex];
+                    break;
+                case 0x985c :
+                    ((struct zsHpPriv*)wd->hpPrivate)->initAGC                      = ar5416Modes[i][modesIndex];
+                    break;
+                case 0x9860 :
+                    ((struct zsHpPriv*)wd->hpPrivate)->initAgcControl               = ar5416Modes[i][modesIndex];
+                    break;
+                case 0x9918 :
+                    ((struct zsHpPriv*)wd->hpPrivate)->initSearchStartDelay         = ar5416Modes[i][modesIndex];
+                    break;
+                case 0x99ec :
+                    ((struct zsHpPriv*)wd->hpPrivate)->initRIFSSearchParams         = ar5416Modes[i][modesIndex];
+                    break;
+                case 0xa388 :
+                    ((struct zsHpPriv*)wd->hpPrivate)->initFastChannelChangeControl = ar5416Modes[i][modesIndex];
+                default :
+                    break;
+            }
+        }
+    }
+#if 0
+    zfFlushDelayWrite(dev);
+
+    /*
+     * Common Register setting
+     */
+    entries = sizeof(ar5416Common) / sizeof(*ar5416Common);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416Common[i][0], ar5416Common[i][1]);
+    }
+    zfFlushDelayWrite(dev);
+
+    /*
+     * RF Gain setting by freqIndex
+     */
+    entries = sizeof(ar5416BB_RfGain) / sizeof(*ar5416BB_RfGain);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416BB_RfGain[i][0], ar5416BB_RfGain[i][freqIndex]);
+    }
+    zfFlushDelayWrite(dev);
+
+    /*
+     * Moved ar5416InitChainMask() here to ensure the swap bit is set before
+     * the pdadc table is written.  Swap must occur before any radio dependent
+     * replicated register access.  The pdadc curve addressing in particular
+     * depends on the consistent setting of the swap bit.
+     */
+    //ar5416InitChainMask(pDev);
+
+    /* Setup the transmit power values. */
+    // TODO
+#endif
+
+    /* Update 5G board data */
+    //Ant control common
+    tmp = hpPriv->eepromImage[0x100+0x144*2/4];
+    eepromBoardData[0][1] = tmp;
+    eepromBoardData[0][2] = tmp;
+    //Ant control chain 0
+    tmp = hpPriv->eepromImage[0x100+0x140*2/4];
+    eepromBoardData[1][1] = tmp;
+    eepromBoardData[1][2] = tmp;
+    //Ant control chain 2
+    tmp = hpPriv->eepromImage[0x100+0x142*2/4];
+    eepromBoardData[2][1] = tmp;
+    eepromBoardData[2][2] = tmp;
+    //SwSettle
+    tmp = hpPriv->eepromImage[0x100+0x146*2/4];
+    tmp = (tmp >> 16) & 0x7f;
+    eepromBoardData[3][1] &= (~((u32_t)0x3f80));
+    eepromBoardData[3][1] |= (tmp << 7);
+#if 0
+    //swSettleHt40
+    tmp = hpPriv->eepromImage[0x100+0x158*2/4];
+    tmp = (tmp) & 0x7f;
+    eepromBoardData[3][2] &= (~((u32_t)0x3f80));
+    eepromBoardData[3][2] |= (tmp << 7);
+#endif
+    //adcDesired, pdaDesired
+    tmp = hpPriv->eepromImage[0x100+0x148*2/4];
+    tmp = (tmp >> 24);
+    tmp1 = hpPriv->eepromImage[0x100+0x14a*2/4];
+    tmp1 = tmp1 & 0xff;
+    tmp = tmp + (tmp1<<8);
+    eepromBoardData[4][1] &= (~((u32_t)0xffff));
+    eepromBoardData[4][1] |= tmp;
+    eepromBoardData[4][2] &= (~((u32_t)0xffff));
+    eepromBoardData[4][2] |= tmp;
+    //TxEndToXpaOff, TxFrameToXpaOn
+    tmp = hpPriv->eepromImage[0x100+0x14a*2/4];
+    tmp = (tmp >> 24) & 0xff;
+    tmp1 = hpPriv->eepromImage[0x100+0x14c*2/4];
+    tmp1 = (tmp1 >> 8) & 0xff;
+    tmp = (tmp<<24) + (tmp<<16) + (tmp1<<8) + tmp1;
+    eepromBoardData[5][1] = tmp;
+    eepromBoardData[5][2] = tmp;
+    //TxEnaToRxOm
+    tmp = hpPriv->eepromImage[0x100+0x14c*2/4] & 0xff;
+    eepromBoardData[6][1] &= (~((u32_t)0xff0000));
+    eepromBoardData[6][1] |= (tmp<<16);
+    eepromBoardData[6][2] &= (~((u32_t)0xff0000));
+    eepromBoardData[6][2] |= (tmp<<16);
+    //Thresh62
+    tmp = hpPriv->eepromImage[0x100+0x14c*2/4];
+    tmp = (tmp >> 16) & 0x7f;
+    eepromBoardData[7][1] &= (~((u32_t)0x7f000));
+    eepromBoardData[7][1] |= (tmp<<12);
+    eepromBoardData[7][2] &= (~((u32_t)0x7f000));
+    eepromBoardData[7][2] |= (tmp<<12);
+    //TxRxAtten chain_0
+    tmp = hpPriv->eepromImage[0x100+0x146*2/4];
+    tmp = (tmp >> 24) & 0x3f;
+    eepromBoardData[8][1] &= (~((u32_t)0x3f000));
+    eepromBoardData[8][1] |= (tmp<<12);
+    eepromBoardData[8][2] &= (~((u32_t)0x3f000));
+    eepromBoardData[8][2] |= (tmp<<12);
+    //TxRxAtten chain_2
+    tmp = hpPriv->eepromImage[0x100+0x148*2/4] & 0x3f;
+    eepromBoardData[9][1] &= (~((u32_t)0x3f000));
+    eepromBoardData[9][1] |= (tmp<<12);
+    eepromBoardData[9][2] &= (~((u32_t)0x3f000));
+    eepromBoardData[9][2] |= (tmp<<12);
+    //TxRxMargin chain_0
+    tmp = hpPriv->eepromImage[0x100+0x148*2/4];
+    tmp = (tmp >> 8) & 0x3f;
+    eepromBoardData[10][1] &= (~((u32_t)0xfc0000));
+    eepromBoardData[10][1] |= (tmp<<18);
+    eepromBoardData[10][2] &= (~((u32_t)0xfc0000));
+    eepromBoardData[10][2] |= (tmp<<18);
+    //TxRxMargin chain_2
+    tmp = hpPriv->eepromImage[0x100+0x148*2/4];
+    tmp = (tmp >> 16) & 0x3f;
+    eepromBoardData[11][1] &= (~((u32_t)0xfc0000));
+    eepromBoardData[11][1] |= (tmp<<18);
+    eepromBoardData[11][2] &= (~((u32_t)0xfc0000));
+    eepromBoardData[11][2] |= (tmp<<18);
+    //iqCall chain_0, iqCallQ chain_0
+    tmp = hpPriv->eepromImage[0x100+0x14e*2/4];
+    tmp = (tmp >> 24) & 0x3f;
+    tmp1 = hpPriv->eepromImage[0x100+0x150*2/4];
+    tmp1 = (tmp1 >> 8) & 0x1f;
+    tmp  = (tmp<<5) + tmp1;
+    eepromBoardData[12][1] &= (~((u32_t)0x7ff));
+    eepromBoardData[12][1] |= (tmp);
+    eepromBoardData[12][2] &= (~((u32_t)0x7ff));
+    eepromBoardData[12][2] |= (tmp);
+    //iqCall chain_2, iqCallQ chain_2
+    tmp = hpPriv->eepromImage[0x100+0x150*2/4];
+    tmp = tmp & 0x3f;
+    tmp1 = hpPriv->eepromImage[0x100+0x150*2/4];
+    tmp1 = (tmp1 >> 16) & 0x1f;
+    tmp  = (tmp<<5) + tmp1;
+    eepromBoardData[13][1] &= (~((u32_t)0x7ff));
+    eepromBoardData[13][1] |= (tmp);
+    eepromBoardData[13][2] &= (~((u32_t)0x7ff));
+    eepromBoardData[13][2] |= (tmp);
+    //bsw_Margin chain_0
+    tmp = hpPriv->eepromImage[0x100+0x156*2/4];
+    tmp = (tmp >> 16) & 0xf;
+    eepromBoardData[10][1] &= (~((u32_t)0x3c00));
+    eepromBoardData[10][1] |= (tmp << 10);
+    eepromBoardData[10][2] &= (~((u32_t)0x3c00));
+    eepromBoardData[10][2] |= (tmp << 10);
+    //xpd gain mask
+    tmp = hpPriv->eepromImage[0x100+0x14e*2/4];
+    tmp = (tmp >> 8) & 0xf;
+    eepromBoardData[14][1] &= (~((u32_t)0xf0000));
+    eepromBoardData[14][1] |= (zcXpdToPd[tmp] << 16);
+    eepromBoardData[14][2] &= (~((u32_t)0xf0000));
+    eepromBoardData[14][2] |= (zcXpdToPd[tmp] << 16);
+#if 0
+    //bsw_Atten chain_0
+    tmp = hpPriv->eepromImage[0x100+0x156*2/4];
+    tmp = (tmp) & 0x1f;
+    eepromBoardData[10][1] &= (~((u32_t)0x1f));
+    eepromBoardData[10][1] |= (tmp);
+    eepromBoardData[10][2] &= (~((u32_t)0x1f));
+    eepromBoardData[10][2] |= (tmp);
+    //bsw_Margin chain_2
+    tmp = hpPriv->eepromImage[0x100+0x156*2/4];
+    tmp = (tmp >> 24) & 0xf;
+    eepromBoardData[11][1] &= (~((u32_t)0x3c00));
+    eepromBoardData[11][1] |= (tmp << 10);
+    eepromBoardData[11][2] &= (~((u32_t)0x3c00));
+    eepromBoardData[11][2] |= (tmp << 10);
+    //bsw_Atten chain_2
+    tmp = hpPriv->eepromImage[0x100+0x156*2/4];
+    tmp = (tmp >> 8) & 0x1f;
+    eepromBoardData[11][1] &= (~((u32_t)0x1f));
+    eepromBoardData[11][1] |= (tmp);
+    eepromBoardData[11][2] &= (~((u32_t)0x1f));
+    eepromBoardData[11][2] |= (tmp);
+#endif
+
+    /* Update 2.4G board data */
+    //Ant control common
+    tmp = hpPriv->eepromImage[0x100+0x170*2/4];
+    tmp = tmp >> 24;
+    tmp1 = hpPriv->eepromImage[0x100+0x172*2/4];
+    tmp = tmp + (tmp1 << 8);
+    eepromBoardData[0][3] = tmp;
+    eepromBoardData[0][4] = tmp;
+    //Ant control chain 0
+    tmp = hpPriv->eepromImage[0x100+0x16c*2/4];
+    tmp = tmp >> 24;
+    tmp1 = hpPriv->eepromImage[0x100+0x16e*2/4];
+    tmp = tmp + (tmp1 << 8);
+    eepromBoardData[1][3] = tmp;
+    eepromBoardData[1][4] = tmp;
+    //Ant control chain 2
+    tmp = hpPriv->eepromImage[0x100+0x16e*2/4];
+    tmp = tmp >> 24;
+    tmp1 = hpPriv->eepromImage[0x100+0x170*2/4];
+    tmp = tmp + (tmp1 << 8);
+    eepromBoardData[2][3] = tmp;
+    eepromBoardData[2][4] = tmp;
+    //SwSettle
+    tmp = hpPriv->eepromImage[0x100+0x174*2/4];
+    tmp = (tmp >> 8) & 0x7f;
+    eepromBoardData[3][4] &= (~((u32_t)0x3f80));
+    eepromBoardData[3][4] |= (tmp << 7);
+#if 0
+    //swSettleHt40
+    tmp = hpPriv->eepromImage[0x100+0x184*2/4];
+    tmp = (tmp >> 24) & 0x7f;
+    eepromBoardData[3][3] &= (~((u32_t)0x3f80));
+    eepromBoardData[3][3] |= (tmp << 7);
+#endif
+    //adcDesired, pdaDesired
+    tmp = hpPriv->eepromImage[0x100+0x176*2/4];
+    tmp = (tmp >> 16) & 0xff;
+    tmp1 = hpPriv->eepromImage[0x100+0x176*2/4];
+    tmp1 = tmp1 >> 24;
+    tmp = tmp + (tmp1<<8);
+    eepromBoardData[4][3] &= (~((u32_t)0xffff));
+    eepromBoardData[4][3] |= tmp;
+    eepromBoardData[4][4] &= (~((u32_t)0xffff));
+    eepromBoardData[4][4] |= tmp;
+    //TxEndToXpaOff, TxFrameToXpaOn
+    tmp = hpPriv->eepromImage[0x100+0x178*2/4];
+    tmp = (tmp >> 16) & 0xff;
+    tmp1 = hpPriv->eepromImage[0x100+0x17a*2/4];
+    tmp1 = tmp1 & 0xff;
+    tmp = (tmp << 24) + (tmp << 16) + (tmp1 << 8) + tmp1;
+    eepromBoardData[5][3] = tmp;
+    eepromBoardData[5][4] = tmp;
+    //TxEnaToRxOm
+    tmp = hpPriv->eepromImage[0x100+0x178*2/4];
+    tmp = (tmp >> 24);
+    eepromBoardData[6][3] &= (~((u32_t)0xff0000));
+    eepromBoardData[6][3] |= (tmp<<16);
+    eepromBoardData[6][4] &= (~((u32_t)0xff0000));
+    eepromBoardData[6][4] |= (tmp<<16);
+    //Thresh62
+    tmp = hpPriv->eepromImage[0x100+0x17a*2/4];
+    tmp = (tmp >> 8) & 0x7f;
+    eepromBoardData[7][3] &= (~((u32_t)0x7f000));
+    eepromBoardData[7][3] |= (tmp<<12);
+    eepromBoardData[7][4] &= (~((u32_t)0x7f000));
+    eepromBoardData[7][4] |= (tmp<<12);
+    //TxRxAtten chain_0
+    tmp = hpPriv->eepromImage[0x100+0x174*2/4];
+    tmp = (tmp >> 16) & 0x3f;
+    eepromBoardData[8][3] &= (~((u32_t)0x3f000));
+    eepromBoardData[8][3] |= (tmp<<12);
+    eepromBoardData[8][4] &= (~((u32_t)0x3f000));
+    eepromBoardData[8][4] |= (tmp<<12);
+    //TxRxAtten chain_2
+    tmp = hpPriv->eepromImage[0x100+0x174*2/4];
+    tmp = (tmp >> 24) & 0x3f;
+    eepromBoardData[9][3] &= (~((u32_t)0x3f000));
+    eepromBoardData[9][3] |= (tmp<<12);
+    eepromBoardData[9][4] &= (~((u32_t)0x3f000));
+    eepromBoardData[9][4] |= (tmp<<12);
+    //TxRxMargin chain_0
+    tmp = hpPriv->eepromImage[0x100+0x176*2/4];
+    tmp = (tmp) & 0x3f;
+    eepromBoardData[10][3] &= (~((u32_t)0xfc0000));
+    eepromBoardData[10][3] |= (tmp<<18);
+    eepromBoardData[10][4] &= (~((u32_t)0xfc0000));
+    eepromBoardData[10][4] |= (tmp<<18);
+    //TxRxMargin chain_2
+    tmp = hpPriv->eepromImage[0x100+0x176*2/4];
+    tmp = (tmp >> 8) & 0x3f;
+    eepromBoardData[11][3] &= (~((u32_t)0xfc0000));
+    eepromBoardData[11][3] |= (tmp<<18);
+    eepromBoardData[11][4] &= (~((u32_t)0xfc0000));
+    eepromBoardData[11][4] |= (tmp<<18);
+    //iqCall chain_0, iqCallQ chain_0
+    tmp = hpPriv->eepromImage[0x100+0x17c*2/4];
+    tmp = (tmp >> 16) & 0x3f;
+    tmp1 = hpPriv->eepromImage[0x100+0x17e*2/4];
+    tmp1 = (tmp1) & 0x1f;
+    tmp  = (tmp<<5) + tmp1;
+    eepromBoardData[12][3] &= (~((u32_t)0x7ff));
+    eepromBoardData[12][3] |= (tmp);
+    eepromBoardData[12][4] &= (~((u32_t)0x7ff));
+    eepromBoardData[12][4] |= (tmp);
+    //iqCall chain_2, iqCallQ chain_2
+    tmp = hpPriv->eepromImage[0x100+0x17c*2/4];
+    tmp = (tmp>>24) & 0x3f;
+    tmp1 = hpPriv->eepromImage[0x100+0x17e*2/4];
+    tmp1 = (tmp1 >> 8) & 0x1f;
+    tmp  = (tmp<<5) + tmp1;
+    eepromBoardData[13][3] &= (~((u32_t)0x7ff));
+    eepromBoardData[13][3] |= (tmp);
+    eepromBoardData[13][4] &= (~((u32_t)0x7ff));
+    eepromBoardData[13][4] |= (tmp);
+    //xpd gain mask
+    tmp = hpPriv->eepromImage[0x100+0x17c*2/4];
+    tmp = tmp & 0xf;
+    DbgPrint("xpd=0x%x, pd=0x%x\n", tmp, zcXpdToPd[tmp]);
+    eepromBoardData[14][3] &= (~((u32_t)0xf0000));
+    eepromBoardData[14][3] |= (zcXpdToPd[tmp] << 16);
+    eepromBoardData[14][4] &= (~((u32_t)0xf0000));
+    eepromBoardData[14][4] |= (zcXpdToPd[tmp] << 16);
+#if 0
+    //bsw_Margin chain_0
+    tmp = hpPriv->eepromImage[0x100+0x184*2/4];
+    tmp = (tmp >> 8) & 0xf;
+    eepromBoardData[10][3] &= (~((u32_t)0x3c00));
+    eepromBoardData[10][3] |= (tmp << 10);
+    eepromBoardData[10][4] &= (~((u32_t)0x3c00));
+    eepromBoardData[10][4] |= (tmp << 10);
+    //bsw_Atten chain_0
+    tmp = hpPriv->eepromImage[0x100+0x182*2/4];
+    tmp = (tmp>>24) & 0x1f;
+    eepromBoardData[10][3] &= (~((u32_t)0x1f));
+    eepromBoardData[10][3] |= (tmp);
+    eepromBoardData[10][4] &= (~((u32_t)0x1f));
+    eepromBoardData[10][4] |= (tmp);
+    //bsw_Margin chain_2
+    tmp = hpPriv->eepromImage[0x100+0x184*2/4];
+    tmp = (tmp >> 16) & 0xf;
+    eepromBoardData[11][3] &= (~((u32_t)0x3c00));
+    eepromBoardData[11][3] |= (tmp << 10);
+    eepromBoardData[11][4] &= (~((u32_t)0x3c00));
+    eepromBoardData[11][4] |= (tmp << 10);
+    //bsw_Atten chain_2
+    tmp = hpPriv->eepromImage[0x100+0x184*2/4];
+    tmp = (tmp) & 0x1f;
+    eepromBoardData[11][3] &= (~((u32_t)0x1f));
+    eepromBoardData[11][3] |= (tmp);
+    eepromBoardData[11][4] &= (~((u32_t)0x1f));
+    eepromBoardData[11][4] |= (tmp);
+#endif
+
+#if 0
+    for (j=0; j<14; j++)
+    {
+        DbgPrint("%04x, %08x, %08x, %08x, %08x\n", eepromBoardData[j][0], eepromBoardData[j][1], eepromBoardData[j][2], eepromBoardData[j][3], eepromBoardData[j][4]);
+    }
+#endif
+
+    if ((hpPriv->eepromImage[0x100+0x110*2/4]&0xff) == 0x80) //FEM TYPE
+    {
+        /* Update board data to registers */
+        for (j=0; j<15; j++)
+        {
+            reg_write(eepromBoardData[j][0], eepromBoardData[j][modesIndex]);
+
+            /* #1 Save the initial value of the related RIFS register settings */
+            //if( ((struct zsHpPriv*)wd->hpPrivate)->isInitialPhy == 1 )
+            {
+                switch(eepromBoardData[j][0])
+                {
+                    case 0x9850 :
+                        ((struct zsHpPriv*)wd->hpPrivate)->initDesiredSigSize           = eepromBoardData[j][modesIndex];
+                        break;
+                    case 0x985c :
+                        ((struct zsHpPriv*)wd->hpPrivate)->initAGC                      = eepromBoardData[j][modesIndex];
+                        break;
+                    case 0x9860 :
+                        ((struct zsHpPriv*)wd->hpPrivate)->initAgcControl               = eepromBoardData[j][modesIndex];
+                        break;
+                    case 0x9918 :
+                        ((struct zsHpPriv*)wd->hpPrivate)->initSearchStartDelay         = eepromBoardData[j][modesIndex];
+                        break;
+                    case 0x99ec :
+                        ((struct zsHpPriv*)wd->hpPrivate)->initRIFSSearchParams         = eepromBoardData[j][modesIndex];
+                        break;
+                    case 0xa388 :
+                        ((struct zsHpPriv*)wd->hpPrivate)->initFastChannelChangeControl = eepromBoardData[j][modesIndex];
+                    default :
+                        break;
+                }
+            }
+        }
+    } /* if ((hpPriv->eepromImage[0x100+0x110*2/4]&0xff) == 0x80) //FEM TYPE */
+
+
+    /* Bringup issue : force tx gain */
+    //reg_write(0xa258, 0x0cc65381);
+    //reg_write(0xa274, 0x0a1a7c15);
+    zfInitPowerCal(dev);
+
+    if(frequency > ZM_CH_G_14)
+    {
+        zfDelayWriteInternalReg(dev, 0x1d4014, 0x5143);
+    }
+    else
+    {
+        zfDelayWriteInternalReg(dev, 0x1d4014, 0x5163);
+    }
+
+    zfFlushDelayWrite(dev);
+}
+
+
+void zfInitRf(zdev_t* dev, u32_t frequency)
+{
+    u32_t cmd[8];
+    u16_t ret;
+    int delta_slope_coeff_exp;
+    int delta_slope_coeff_man;
+    int delta_slope_coeff_exp_shgi;
+    int delta_slope_coeff_man_shgi;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg1(" initRf frequency = ", frequency);
+
+    if (frequency == 0)
+    {
+        frequency = 2412;
+    }
+
+    /* Bank 0 1 2 3 5 6 7 */
+    zfSetRfRegs(dev, frequency);
+    /* Bank 4 */
+    zfSetBank4AndPowerTable(dev, frequency, 0, 0);
+
+    /* stroe frequency */
+    ((struct zsHpPriv*)wd->hpPrivate)->hwFrequency = (u16_t)frequency;
+
+    zfGetHwTurnOffdynParam(dev,
+                           frequency, 0, 0,
+                           &delta_slope_coeff_exp,
+                           &delta_slope_coeff_man,
+                           &delta_slope_coeff_exp_shgi,
+                           &delta_slope_coeff_man_shgi);
+
+    /* related functions */
+    frequency = frequency*1000;
+    cmd[0] = 28 | (ZM_CMD_RF_INIT << 8);
+    cmd[1] = frequency;
+    cmd[2] = 0;//((struct zsHpPriv*)wd->hpPrivate)->hw_DYNAMIC_HT2040_EN;
+    cmd[3] = 1;//((wd->ExtOffset << 2) | ((struct zsHpPriv*)wd->hpPrivate)->hw_HT_ENABLE);
+    cmd[4] = delta_slope_coeff_exp;
+    cmd[5] = delta_slope_coeff_man;
+    cmd[6] = delta_slope_coeff_exp_shgi;
+    cmd[7] = delta_slope_coeff_man_shgi;
+
+    ret = zfIssueCmd(dev, cmd, 32, ZM_OID_INTERNAL_WRITE, 0);
+
+    // delay temporarily, wait for new PHY and RF
+    zfwSleep(dev, 1000);
+}
+
+int tn(int exp)
+{
+    int i;
+	int tmp = 1;
+    for(i=0; i<exp; i++)
+        tmp = tmp*2;
+
+    return tmp;
+}
+
+/*int zfFloor(double indata)
+{
+   if(indata<0)
+	   return (int)indata-1;
+   else
+	   return (int)indata;
+}
+*/
+u32_t reverse_bits(u32_t chan_sel)
+{
+	/* reverse_bits */
+    u32_t chansel = 0;
+	u8_t i;
+
+	for (i=0; i<8; i++)
+        chansel |= ((chan_sel>>(7-i) & 0x1) << i);
+	return chansel;
+}
+
+/* Bank 0 1 2 3 5 6 7 */
+void zfSetRfRegs(zdev_t* dev, u32_t frequency)
+{
+    u16_t entries;
+    u16_t freqIndex = 0;
+    u16_t i;
+
+    //zmw_get_wlan_dev(dev);
+
+    if ( frequency > ZM_CH_G_14 )
+    {
+        /* 5G */
+        freqIndex = 1;
+        zm_msg0_scan(ZM_LV_2, "Set to 5GHz");
+
+    }
+    else
+    {
+        /* 2.4G */
+        freqIndex = 2;
+        zm_msg0_scan(ZM_LV_2, "Set to 2.4GHz");
+    }
+
+#if 1
+    entries = sizeof(otusBank) / sizeof(*otusBank);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(otusBank[i][0], otusBank[i][freqIndex]);
+    }
+#else
+    /* Bank0 */
+    entries = sizeof(ar5416Bank0) / sizeof(*ar5416Bank0);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416Bank0[i][0], ar5416Bank0[i][1]);
+    }
+    /* Bank1 */
+    entries = sizeof(ar5416Bank1) / sizeof(*ar5416Bank1);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416Bank1[i][0], ar5416Bank1[i][1]);
+    }
+    /* Bank2 */
+    entries = sizeof(ar5416Bank2) / sizeof(*ar5416Bank2);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416Bank2[i][0], ar5416Bank2[i][1]);
+    }
+    /* Bank3 */
+    entries = sizeof(ar5416Bank3) / sizeof(*ar5416Bank3);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416Bank3[i][0], ar5416Bank3[i][freqIndex]);
+    }
+    /* Bank5 */
+    reg_write (0x98b0,  0x00000013);
+    reg_write (0x98e4,  0x00000002);
+    /* Bank6 */
+    entries = sizeof(ar5416Bank6) / sizeof(*ar5416Bank6);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416Bank6[i][0], ar5416Bank6[i][freqIndex]);
+    }
+    /* Bank7 */
+    entries = sizeof(ar5416Bank7) / sizeof(*ar5416Bank7);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416Bank7[i][0], ar5416Bank7[i][1]);
+    }
+#endif
+
+    zfFlushDelayWrite(dev);
+}
+
+/* Bank 4 */
+void zfSetBank4AndPowerTable(zdev_t* dev, u32_t frequency, u8_t bw40,
+        u8_t extOffset)
+{
+    u32_t chup = 1;
+	u32_t bmode_LF_synth_freq = 0;
+	u32_t amode_refsel_1 = 0;
+	u32_t amode_refsel_0 = 1;
+	u32_t addr2 = 1;
+	u32_t addr1 = 0;
+	u32_t addr0 = 0;
+
+	u32_t d1;
+	u32_t d0;
+	u32_t tmp_0;
+	u32_t tmp_1;
+	u32_t data0;
+	u32_t data1;
+
+	u8_t chansel;
+	u8_t chan_sel;
+	u32_t temp_chan_sel;
+
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+
+    /* if enable 802.11h, need to record curent channel index in channel array */
+    if (wd->sta.DFSEnable)
+    {
+        for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+        {
+            if (wd->regulationTable.allowChannel[i].channel == frequency)
+                break;
+        }
+        wd->regulationTable.CurChIndex = i;
+    }
+
+	if (bw40 == 1)
+	{
+        if (extOffset == 1)
+        {
+            frequency += 10;
+        }
+        else
+        {
+            frequency -= 10;
+        }
+
+	}
+
+
+	if ( frequency > 3000 )
+	{
+	    if ( frequency % 10 )
+	    {
+	        /* 5M */
+            chan_sel = (u8_t)((frequency - 4800)/5);
+            chan_sel = (u8_t)(chan_sel & 0xff);
+            chansel  = (u8_t)reverse_bits(chan_sel);
+        }
+        else
+        {
+            /* 10M : improve Tx EVM */
+            chan_sel = (u8_t)((frequency - 4800)/10);
+            chan_sel = (u8_t)(chan_sel & 0xff)<<1;
+            chansel  = (u8_t)reverse_bits(chan_sel);
+
+	        amode_refsel_1 = 1;
+	        amode_refsel_0 = 0;
+        }
+	}
+	else
+	{
+        //temp_chan_sel = (((frequency - 672)*2) - 3040)/10;
+        if (frequency == 2484)
+        {
+   	        temp_chan_sel = 10 + (frequency - 2274)/5 ;
+   	        bmode_LF_synth_freq = 1;
+        }
+        else
+        {
+            temp_chan_sel = 16 + (frequency - 2272)/5 ;
+            bmode_LF_synth_freq = 0;
+        }
+        chan_sel = (u8_t)(temp_chan_sel << 2) & 0xff;
+        chansel  = (u8_t)reverse_bits(chan_sel);
+	}
+
+	d1   = chansel;   //# 8 bits of chan
+	d0   = addr0<<7 | addr1<<6 | addr2<<5
+			| amode_refsel_0<<3 | amode_refsel_1<<2
+			| bmode_LF_synth_freq<<1 | chup;
+
+    tmp_0 = d0 & 0x1f;  //# 5-1
+    tmp_1 = d1 & 0x1f;  //# 5-1
+    data0 = tmp_1<<5 | tmp_0;
+
+    tmp_0 = d0>>5 & 0x7;  //# 8-6
+    tmp_1 = d1>>5 & 0x7;  //# 8-6
+    data1 = tmp_1<<5 | tmp_0;
+
+    /* Bank4 */
+	reg_write (0x9800+(0x2c<<2), data0);
+	reg_write (0x9800+(0x3a<<2), data1);
+	//zm_debug_msg1("0x9800+(0x2c<<2 =  ", data0);
+	//zm_debug_msg1("0x9800+(0x3a<<2 =  ", data1);
+
+
+    zfFlushDelayWrite(dev);
+
+    zfwSleep(dev, 10);
+
+    return;
+}
+
+
+struct zsPhyFreqPara
+{
+    u32_t coeff_exp;
+    u32_t coeff_man;
+    u32_t coeff_exp_shgi;
+    u32_t coeff_man_shgi;
+};
+
+struct zsPhyFreqTable
+{
+    u32_t frequency;
+    struct zsPhyFreqPara FpgaDynamicHT;
+    struct zsPhyFreqPara FpgaStaticHT;
+    struct zsPhyFreqPara ChipST20Mhz;
+    struct zsPhyFreqPara Chip2040Mhz;
+    struct zsPhyFreqPara Chip2040ExtAbove;
+};
+
+const struct zsPhyFreqTable zgPhyFreqCoeff[] =
+{
+/*Index   freq  FPGA DYNAMIC_HT2040_EN  FPGA STATIC_HT20    Real Chip static20MHz     Real Chip 2040MHz   Real Chip 2040Mhz  */
+       /* fclk =         10.8                21.6                  40                  ext below 40       ext above 40       */
+/*  0 */ {2412, {5, 23476, 5, 21128}, {4, 23476, 4, 21128}, {3, 21737, 3, 19563}, {3, 21827, 3, 19644}, {3, 21647, 3, 19482}},
+/*  1 */ {2417, {5, 23427, 5, 21084}, {4, 23427, 4, 21084}, {3, 21692, 3, 19523}, {3, 21782, 3, 19604}, {3, 21602, 3, 19442}},
+/*  2 */ {2422, {5, 23379, 5, 21041}, {4, 23379, 4, 21041}, {3, 21647, 3, 19482}, {3, 21737, 3, 19563}, {3, 21558, 3, 19402}},
+/*  3 */ {2427, {5, 23330, 5, 20997}, {4, 23330, 4, 20997}, {3, 21602, 3, 19442}, {3, 21692, 3, 19523}, {3, 21514, 3, 19362}},
+/*  4 */ {2432, {5, 23283, 5, 20954}, {4, 23283, 4, 20954}, {3, 21558, 3, 19402}, {3, 21647, 3, 19482}, {3, 21470, 3, 19323}},
+/*  5 */ {2437, {5, 23235, 5, 20911}, {4, 23235, 4, 20911}, {3, 21514, 3, 19362}, {3, 21602, 3, 19442}, {3, 21426, 3, 19283}},
+/*  6 */ {2442, {5, 23187, 5, 20868}, {4, 23187, 4, 20868}, {3, 21470, 3, 19323}, {3, 21558, 3, 19402}, {3, 21382, 3, 19244}},
+/*  7 */ {2447, {5, 23140, 5, 20826}, {4, 23140, 4, 20826}, {3, 21426, 3, 19283}, {3, 21514, 3, 19362}, {3, 21339, 3, 19205}},
+/*  8 */ {2452, {5, 23093, 5, 20783}, {4, 23093, 4, 20783}, {3, 21382, 3, 19244}, {3, 21470, 3, 19323}, {3, 21295, 3, 19166}},
+/*  9 */ {2457, {5, 23046, 5, 20741}, {4, 23046, 4, 20741}, {3, 21339, 3, 19205}, {3, 21426, 3, 19283}, {3, 21252, 3, 19127}},
+/* 10 */ {2462, {5, 22999, 5, 20699}, {4, 22999, 4, 20699}, {3, 21295, 3, 19166}, {3, 21382, 3, 19244}, {3, 21209, 3, 19088}},
+/* 11 */ {2467, {5, 22952, 5, 20657}, {4, 22952, 4, 20657}, {3, 21252, 3, 19127}, {3, 21339, 3, 19205}, {3, 21166, 3, 19050}},
+/* 12 */ {2472, {5, 22906, 5, 20615}, {4, 22906, 4, 20615}, {3, 21209, 3, 19088}, {3, 21295, 3, 19166}, {3, 21124, 3, 19011}},
+/* 13 */ {2484, {5, 22795, 5, 20516}, {4, 22795, 4, 20516}, {3, 21107, 3, 18996}, {3, 21192, 3, 19073}, {3, 21022, 3, 18920}},
+/* 14 */ {4920, {6, 23018, 6, 20716}, {5, 23018, 5, 20716}, {4, 21313, 4, 19181}, {4, 21356, 4, 19220}, {4, 21269, 4, 19142}},
+/* 15 */ {4940, {6, 22924, 6, 20632}, {5, 22924, 5, 20632}, {4, 21226, 4, 19104}, {4, 21269, 4, 19142}, {4, 21183, 4, 19065}},
+/* 16 */ {4960, {6, 22832, 6, 20549}, {5, 22832, 5, 20549}, {4, 21141, 4, 19027}, {4, 21183, 4, 19065}, {4, 21098, 4, 18988}},
+/* 17 */ {4980, {6, 22740, 6, 20466}, {5, 22740, 5, 20466}, {4, 21056, 4, 18950}, {4, 21098, 4, 18988}, {4, 21014, 4, 18912}},
+/* 18 */ {5040, {6, 22469, 6, 20223}, {5, 22469, 5, 20223}, {4, 20805, 4, 18725}, {4, 20846, 4, 18762}, {4, 20764, 4, 18687}},
+/* 19 */ {5060, {6, 22381, 6, 20143}, {5, 22381, 5, 20143}, {4, 20723, 4, 18651}, {4, 20764, 4, 18687}, {4, 20682, 4, 18614}},
+/* 20 */ {5080, {6, 22293, 6, 20063}, {5, 22293, 5, 20063}, {4, 20641, 4, 18577}, {4, 20682, 4, 18614}, {4, 20601, 4, 18541}},
+/* 21 */ {5180, {6, 21862, 6, 19676}, {5, 21862, 5, 19676}, {4, 20243, 4, 18219}, {4, 20282, 4, 18254}, {4, 20204, 4, 18183}},
+/* 22 */ {5200, {6, 21778, 6, 19600}, {5, 21778, 5, 19600}, {4, 20165, 4, 18148}, {4, 20204, 4, 18183}, {4, 20126, 4, 18114}},
+/* 23 */ {5220, {6, 21695, 6, 19525}, {5, 21695, 5, 19525}, {4, 20088, 4, 18079}, {4, 20126, 4, 18114}, {4, 20049, 4, 18044}},
+/* 24 */ {5240, {6, 21612, 6, 19451}, {5, 21612, 5, 19451}, {4, 20011, 4, 18010}, {4, 20049, 4, 18044}, {4, 19973, 4, 17976}},
+/* 25 */ {5260, {6, 21530, 6, 19377}, {5, 21530, 5, 19377}, {4, 19935, 4, 17941}, {4, 19973, 4, 17976}, {4, 19897, 4, 17907}},
+/* 26 */ {5280, {6, 21448, 6, 19303}, {5, 21448, 5, 19303}, {4, 19859, 4, 17873}, {4, 19897, 4, 17907}, {4, 19822, 4, 17840}},
+/* 27 */ {5300, {6, 21367, 6, 19230}, {5, 21367, 5, 19230}, {4, 19784, 4, 17806}, {4, 19822, 4, 17840}, {4, 19747, 4, 17772}},
+/* 28 */ {5320, {6, 21287, 6, 19158}, {5, 21287, 5, 19158}, {4, 19710, 4, 17739}, {4, 19747, 4, 17772}, {4, 19673, 4, 17706}},
+/* 29 */ {5500, {6, 20590, 6, 18531}, {5, 20590, 5, 18531}, {4, 19065, 4, 17159}, {4, 19100, 4, 17190}, {4, 19030, 4, 17127}},
+/* 30 */ {5520, {6, 20516, 6, 18464}, {5, 20516, 5, 18464}, {4, 18996, 4, 17096}, {4, 19030, 4, 17127}, {4, 18962, 4, 17065}},
+/* 31 */ {5540, {6, 20442, 6, 18397}, {5, 20442, 5, 18397}, {4, 18927, 4, 17035}, {4, 18962, 4, 17065}, {4, 18893, 4, 17004}},
+/* 32 */ {5560, {6, 20368, 6, 18331}, {5, 20368, 5, 18331}, {4, 18859, 4, 16973}, {4, 18893, 4, 17004}, {4, 18825, 4, 16943}},
+/* 33 */ {5580, {6, 20295, 6, 18266}, {5, 20295, 5, 18266}, {4, 18792, 4, 16913}, {4, 18825, 4, 16943}, {4, 18758, 4, 16882}},
+/* 34 */ {5600, {6, 20223, 6, 18200}, {5, 20223, 5, 18200}, {4, 18725, 4, 16852}, {4, 18758, 4, 16882}, {4, 18691, 4, 16822}},
+/* 35 */ {5620, {6, 20151, 6, 18136}, {5, 20151, 5, 18136}, {4, 18658, 4, 16792}, {4, 18691, 4, 16822}, {4, 18625, 4, 16762}},
+/* 36 */ {5640, {6, 20079, 6, 18071}, {5, 20079, 5, 18071}, {4, 18592, 4, 16733}, {4, 18625, 4, 16762}, {4, 18559, 4, 16703}},
+/* 37 */ {5660, {6, 20008, 6, 18007}, {5, 20008, 5, 18007}, {4, 18526, 4, 16673}, {4, 18559, 4, 16703}, {4, 18493, 4, 16644}},
+/* 38 */ {5680, {6, 19938, 6, 17944}, {5, 19938, 5, 17944}, {4, 18461, 4, 16615}, {4, 18493, 4, 16644}, {4, 18428, 4, 16586}},
+/* 39 */ {5700, {6, 19868, 6, 17881}, {5, 19868, 5, 17881}, {4, 18396, 4, 16556}, {4, 18428, 4, 16586}, {4, 18364, 4, 16527}},
+/* 40 */ {5745, {6, 19712, 6, 17741}, {5, 19712, 5, 17741}, {4, 18252, 4, 16427}, {4, 18284, 4, 16455}, {4, 18220, 4, 16398}},
+/* 41 */ {5765, {6, 19644, 6, 17679}, {5, 19644, 5, 17679}, {4, 18189, 5, 32740}, {4, 18220, 4, 16398}, {4, 18157, 5, 32683}},
+/* 42 */ {5785, {6, 19576, 6, 17618}, {5, 19576, 5, 17618}, {4, 18126, 5, 32626}, {4, 18157, 5, 32683}, {4, 18094, 5, 32570}},
+/* 43 */ {5805, {6, 19508, 6, 17558}, {5, 19508, 5, 17558}, {4, 18063, 5, 32514}, {4, 18094, 5, 32570}, {4, 18032, 5, 32458}},
+/* 44 */ {5825, {6, 19441, 6, 17497}, {5, 19441, 5, 17497}, {4, 18001, 5, 32402}, {4, 18032, 5, 32458}, {4, 17970, 5, 32347}},
+/* 45 */ {5170, {6, 21904, 6, 19714}, {5, 21904, 5, 19714}, {4, 20282, 4, 18254}, {4, 20321, 4, 18289}, {4, 20243, 4, 18219}},
+/* 46 */ {5190, {6, 21820, 6, 19638}, {5, 21820, 5, 19638}, {4, 20204, 4, 18183}, {4, 20243, 4, 18219}, {4, 20165, 4, 18148}},
+/* 47 */ {5210, {6, 21736, 6, 19563}, {5, 21736, 5, 19563}, {4, 20126, 4, 18114}, {4, 20165, 4, 18148}, {4, 20088, 4, 18079}},
+/* 48 */ {5230, {6, 21653, 6, 19488}, {5, 21653, 5, 19488}, {4, 20049, 4, 18044}, {4, 20088, 4, 18079}, {4, 20011, 4, 18010}}
+};
+/* to reduce search time, please modify this define if you add or delete channel in table */
+#define First5GChannelIndex 14
+
+void zfGetHwTurnOffdynParam(zdev_t* dev,
+                            u32_t frequency, u8_t bw40, u8_t extOffset,
+                            int* delta_slope_coeff_exp,
+                            int* delta_slope_coeff_man,
+                            int* delta_slope_coeff_exp_shgi,
+                            int* delta_slope_coeff_man_shgi)
+{
+    /* Get param for turnoffdyn */
+    u16_t i, arraySize;
+
+    //zmw_get_wlan_dev(dev);
+
+    arraySize = sizeof(zgPhyFreqCoeff)/sizeof(struct zsPhyFreqTable);
+    if (frequency < 3000)
+    {
+        /* 2.4GHz Channel */
+        for (i = 0; i < First5GChannelIndex; i++)
+        {
+            if (frequency == zgPhyFreqCoeff[i].frequency)
+                break;
+        }
+
+        if (i < First5GChannelIndex)
+        {
+        }
+        else
+        {
+            zm_msg1_scan(ZM_LV_0, "Unsupported 2.4G frequency = ", frequency);
+            return;
+        }
+    }
+    else
+    {
+        /* 5GHz Channel */
+        for (i = First5GChannelIndex; i < arraySize; i++)
+        {
+            if (frequency == zgPhyFreqCoeff[i].frequency)
+                break;
+        }
+
+        if (i < arraySize)
+        {
+        }
+        else
+        {
+            zm_msg1_scan(ZM_LV_0, "Unsupported 5G frequency = ", frequency);
+            return;
+        }
+    }
+
+    /* FPGA DYNAMIC_HT2040_EN        fclk = 10.8  */
+    /* FPGA STATIC_HT20_             fclk = 21.6  */
+    /* Real Chip                     fclk = 40    */
+    #if ZM_FPGA_PHY == 1
+    //fclk = 10.8;
+    *delta_slope_coeff_exp = zgPhyFreqCoeff[i].FpgaDynamicHT.coeff_exp;
+    *delta_slope_coeff_man = zgPhyFreqCoeff[i].FpgaDynamicHT.coeff_man;
+    *delta_slope_coeff_exp_shgi = zgPhyFreqCoeff[i].FpgaDynamicHT.coeff_exp_shgi;
+    *delta_slope_coeff_man_shgi = zgPhyFreqCoeff[i].FpgaDynamicHT.coeff_man_shgi;
+    #else
+    //fclk = 40;
+    if (bw40)
+    {
+        /* ht2040 */
+        if (extOffset == 1) {
+            *delta_slope_coeff_exp = zgPhyFreqCoeff[i].Chip2040ExtAbove.coeff_exp;
+            *delta_slope_coeff_man = zgPhyFreqCoeff[i].Chip2040ExtAbove.coeff_man;
+            *delta_slope_coeff_exp_shgi = zgPhyFreqCoeff[i].Chip2040ExtAbove.coeff_exp_shgi;
+            *delta_slope_coeff_man_shgi = zgPhyFreqCoeff[i].Chip2040ExtAbove.coeff_man_shgi;
+        }
+        else {
+            *delta_slope_coeff_exp = zgPhyFreqCoeff[i].Chip2040Mhz.coeff_exp;
+            *delta_slope_coeff_man = zgPhyFreqCoeff[i].Chip2040Mhz.coeff_man;
+            *delta_slope_coeff_exp_shgi = zgPhyFreqCoeff[i].Chip2040Mhz.coeff_exp_shgi;
+            *delta_slope_coeff_man_shgi = zgPhyFreqCoeff[i].Chip2040Mhz.coeff_man_shgi;
+        }
+    }
+    else
+    {
+        /* static 20 */
+        *delta_slope_coeff_exp = zgPhyFreqCoeff[i].ChipST20Mhz.coeff_exp;
+        *delta_slope_coeff_man = zgPhyFreqCoeff[i].ChipST20Mhz.coeff_man;
+        *delta_slope_coeff_exp_shgi = zgPhyFreqCoeff[i].ChipST20Mhz.coeff_exp_shgi;
+        *delta_slope_coeff_man_shgi = zgPhyFreqCoeff[i].ChipST20Mhz.coeff_man_shgi;
+    }
+    #endif
+}
+
+/* Main routin frequency setting function */
+/* If 2.4G/5G switch, PHY need resetting BB and RF for band switch */
+/* Do the setting switch in zfSendFrequencyCmd() */
+void zfHpSetFrequencyEx(zdev_t* dev, u32_t frequency, u8_t bw40,
+        u8_t extOffset, u8_t initRF)
+{
+    u32_t cmd[9];
+    u32_t cmdB[3];
+    u16_t ret;
+    u8_t old_band;
+    u8_t new_band;
+    u32_t checkLoopCount;
+    u32_t tmpValue;
+
+    int delta_slope_coeff_exp;
+    int delta_slope_coeff_man;
+    int delta_slope_coeff_exp_shgi;
+    int delta_slope_coeff_man_shgi;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+
+    zm_msg1_scan(ZM_LV_1, "Frequency = ", frequency);
+    zm_msg1_scan(ZM_LV_1, "bw40 = ", bw40);
+    zm_msg1_scan(ZM_LV_1, "extOffset = ", extOffset);
+
+    if ( hpPriv->coldResetNeedFreq )
+    {
+        hpPriv->coldResetNeedFreq = 0;
+        initRF = 2;
+        zm_debug_msg0("zfHpSetFrequencyEx: Do ColdReset ");
+    }
+    if ( hpPriv->isSiteSurvey == 2 )
+    {
+        /* wait time for AGC and noise calibration : not in sitesurvey and connected */
+        checkLoopCount = 2000; /* 2000*100 = 200ms */
+    }
+    else
+    {
+        /* wait time for AGC and noise calibration : in sitesurvey */
+        checkLoopCount = 1000; /* 1000*100 = 100ms */
+    }
+
+    hpPriv->latestFrequency = frequency;
+    hpPriv->latestBw40 = bw40;
+    hpPriv->latestExtOffset = extOffset;
+
+    if ((hpPriv->dot11Mode == ZM_HAL_80211_MODE_IBSS_GENERAL) ||
+        (hpPriv->dot11Mode == ZM_HAL_80211_MODE_IBSS_WPA2PSK))
+    {
+        if ( frequency <= ZM_CH_G_14 )
+        {
+            /* workaround for 11g Ad Hoc beacon distribution */
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC0_CW, 0x7f0007);
+            //zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_AC0_AIFS, 0x1c04901c);
+        }
+    }
+
+    /* AHB, DAC, ADC clock selection by static20/ht2040 */
+    zfSelAdcClk(dev, bw40, frequency);
+
+    /* clear bb_heavy_clip_enable */
+    reg_write(0x99e0, 0x200);
+    zfFlushDelayWrite(dev);
+
+    /* Set CTS/RTS rate */
+    if ( frequency > ZM_CH_G_14 )
+    {
+        //zfHpSetRTSCTSRate(dev, 0x10b010b);  /* OFDM 6M */
+	    new_band = 1;
+	}
+    else
+    {
+        //zfHpSetRTSCTSRate(dev, 0x30003);  /* CCK 11M */
+        new_band = 0;
+    }
+
+    if (((struct zsHpPriv*)wd->hpPrivate)->hwFrequency > ZM_CH_G_14)
+        old_band = 1;
+    else
+        old_band = 0;
+
+    //Workaround for 2.4GHz only device
+    if ((hpPriv->OpFlags & 0x1) == 0)
+    {
+        if ((((struct zsHpPriv*)wd->hpPrivate)->hwFrequency == ZM_CH_G_1) && (frequency == ZM_CH_G_2))
+        {
+            /* Force to do band switching */
+            old_band = 1;
+        }
+    }
+
+    /* Notify channel switch to firmware */
+    /* TX/RX must be stopped by now */
+    cmd[0] = 0 | (ZM_CMD_FREQ_STRAT << 8);
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_INTERNAL_WRITE, 0);
+
+    if ((initRF != 0) || (new_band != old_band)
+            || (((struct zsHpPriv*)wd->hpPrivate)->hwBw40 != bw40))
+    {
+        /* band switch */
+        zm_msg0_scan(ZM_LV_1, "=====band switch=====");
+
+        if (initRF == 2 )
+        {
+            //Cold reset BB/ADDA
+            zfDelayWriteInternalReg(dev, 0x1d4004, 0x800);
+            zfFlushDelayWrite(dev);
+            zm_msg0_scan(ZM_LV_1, "Do cold reset BB/ADDA");
+        }
+        else
+        {
+            //Warm reset BB/ADDA
+            zfDelayWriteInternalReg(dev, 0x1d4004, 0x400);
+            zfFlushDelayWrite(dev);
+        }
+
+        /* reset workaround state to default */
+        hpPriv->rxStrongRSSI = 0;
+        hpPriv->strongRSSI = 0;
+
+        zfDelayWriteInternalReg(dev, 0x1d4004, 0x0);
+        zfFlushDelayWrite(dev);
+
+        zfInitPhy(dev, frequency, bw40);
+
+//        zfiCheckRifs(dev);
+
+        /* Bank 0 1 2 3 5 6 7 */
+        zfSetRfRegs(dev, frequency);
+        /* Bank 4 */
+        zfSetBank4AndPowerTable(dev, frequency, bw40, extOffset);
+
+        cmd[0] = 32 | (ZM_CMD_RF_INIT << 8);
+    }
+    else //((new_band == old_band) && !initRF)
+    {
+       /* same band */
+
+       /* Force disable CR671 bit20 / 7823                                            */
+       /* The bug has to do with the polarity of the pdadc offset calibration.  There */
+       /* is an initial calibration that is OK, and there is a continuous             */
+       /* calibration that updates the pddac with the wrong polarity.  Fortunately    */
+       /* the second loop can be disabled with a bit called en_pd_dc_offset_thr.      */
+#if 0
+        cmdB[0] = 8 | (ZM_CMD_BITAND << 8);;
+        cmdB[1] = (0xa27c + 0x1bc000);
+        cmdB[2] = 0xffefffff;
+        ret = zfIssueCmd(dev, cmdB, 12, ZM_OID_INTERNAL_WRITE, 0);
+#endif
+
+       /* Bank 4 */
+       zfSetBank4AndPowerTable(dev, frequency, bw40, extOffset);
+
+
+        cmd[0] = 32 | (ZM_CMD_FREQUENCY << 8);
+    }
+
+    /* Compatibility for new layout UB83 */
+    /* Setting code at CR1 here move from the func:zfHwHTEnable() in firmware */
+    if (((struct zsHpPriv*)wd->hpPrivate)->halCapability & ZM_HP_CAP_11N_ONE_TX_STREAM)
+    {
+        /* UB83 : one stream */
+        tmpValue = 0;
+    }
+    else
+    {
+        /* UB81, UB82 : two stream */
+        tmpValue = 0x100;
+    }
+
+    if (1) //if (((struct zsHpPriv*)wd->hpPrivate)->hw_HT_ENABLE == 1)
+	{
+        if (bw40 == 1)
+		{
+			if (extOffset == 1) {
+            	reg_write(0x9804, tmpValue | 0x2d4); //3d4 for real
+			}
+			else {
+				reg_write(0x9804, tmpValue | 0x2c4);   //3c4 for real
+			}
+			//# Dyn HT2040.Refer to Reg 1.
+            //#[3]:single length (4us) 1st HT long training symbol; use Walsh spatial spreading for 2 chains 2 streams TX
+            //#[c]:allow short GI for HT40 packets; enable HT detection.
+            //#[4]:enable 20/40 MHz channel detection.
+        }
+        else
+	    {
+            reg_write(0x9804, tmpValue | 0x240);
+		    //# Static HT20
+            //#[3]:single length (4us) 1st HT long training symbol; use Walsh spatial spreading for 2 chains 2 streams TX
+            //#[4]:Otus don't allow short GI for HT20 packets yet; enable HT detection.
+            //#[0]:disable 20/40 MHz channel detection.
+        }
+    }
+    else
+	{
+        reg_write(0x9804, 0x0);
+		//# Legacy;# Direct Mapping for each chain.
+        //#Be modified by Oligo to add dynanic for legacy.
+        if (bw40 == 1)
+		{
+            reg_write(0x9804, 0x4);     //# Dyn Legacy .Refer to reg 1.
+        }
+        else
+		{
+            reg_write(0x9804, 0x0);    //# Static Legacy
+        }
+	}
+	zfFlushDelayWrite(dev);
+	/* end of ub83 compatibility */
+
+    /* Set Power, TPC, Gain table... */
+	zfSetPowerCalTable(dev, frequency, bw40, extOffset);
+
+
+    /* store frequency */
+    ((struct zsHpPriv*)wd->hpPrivate)->hwFrequency = (u16_t)frequency;
+    ((struct zsHpPriv*)wd->hpPrivate)->hwBw40 = bw40;
+    ((struct zsHpPriv*)wd->hpPrivate)->hwExtOffset = extOffset;
+
+    zfGetHwTurnOffdynParam(dev,
+                           frequency, bw40, extOffset,
+                           &delta_slope_coeff_exp,
+                           &delta_slope_coeff_man,
+                           &delta_slope_coeff_exp_shgi,
+                           &delta_slope_coeff_man_shgi);
+
+    /* related functions */
+    frequency = frequency*1000;
+    /* len[36] : type[0x30] : seq[?] */
+//    cmd[0] = 28 | (ZM_CMD_FREQUENCY << 8);
+    cmd[1] = frequency;
+    cmd[2] = bw40;//((struct zsHpPriv*)wd->hpPrivate)->hw_DYNAMIC_HT2040_EN;
+    cmd[3] = (extOffset<<2)|0x1;//((wd->ExtOffset << 2) | ((struct zsHpPriv*)wd->hpPrivate)->hw_HT_ENABLE);
+    cmd[4] = delta_slope_coeff_exp;
+    cmd[5] = delta_slope_coeff_man;
+    cmd[6] = delta_slope_coeff_exp_shgi;
+    cmd[7] = delta_slope_coeff_man_shgi;
+    cmd[8] = checkLoopCount;
+
+    ret = zfIssueCmd(dev, cmd, 36, ZM_CMD_SET_FREQUENCY, 0);
+
+    // delay temporarily, wait for new PHY and RF
+    //zfwSleep(dev, 1000);
+}
+
+
+/******************** Key ********************/
+
+u16_t zfHpResetKeyCache(zdev_t* dev)
+{
+    u8_t i;
+    u32_t key[4] = {0, 0, 0, 0};
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    for(i=0;i<4;i++)
+    {
+        zfHpSetDefaultKey(dev, i, ZM_WEP64, key, NULL);
+    }
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_ROLL_CALL_TBL_L, 0x00);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_ROLL_CALL_TBL_H, 0x00);
+    zfFlushDelayWrite(dev);
+
+    hpPriv->camRollCallTable = (u64_t) 0;
+
+    return 0;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfSetKey                    */
+/*      Set key.                                                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.1      */
+/*                                                                      */
+/************************************************************************/
+/* ! please use zfCoreSetKey() in 80211Core for SetKey */
+u32_t zfHpSetKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t type,
+        u16_t* mac, u32_t* key)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret;
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+#if 0   /* remove to zfCoreSetKey() */
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->sta.flagKeyChanging++;
+    zm_debug_msg1("   zfHpSetKey++++ ", wd->sta.flagKeyChanging);
+    zmw_leave_critical_section(dev);
+#endif
+
+    cmd[0] = 0x0000281C;
+    cmd[1] = ((u32_t)keyId<<16) + (u32_t)user;
+    cmd[2] = ((u32_t)mac[0]<<16) + (u32_t)type;
+    cmd[3] = ((u32_t)mac[2]<<16) + ((u32_t)mac[1]);
+
+    for (i=0; i<4; i++)
+    {
+        cmd[4+i] = key[i];
+    }
+
+    if (user < 64)
+    {
+        hpPriv->camRollCallTable |= ((u64_t) 1) << user;
+    }
+
+    //ret = zfIssueCmd(dev, cmd, 32, ZM_OID_INTERNAL_WRITE, NULL);
+    ret = zfIssueCmd(dev, cmd, 32, ZM_CMD_SET_KEY, NULL);
+    return ret;
+}
+
+
+u32_t zfHpSetApPairwiseKey(zdev_t* dev, u16_t* staMacAddr, u8_t type,
+        u32_t* key, u32_t* micKey, u16_t staAid)
+{
+    if ((staAid!=0) && (staAid<64))
+    {
+        zfHpSetKey(dev, (staAid-1), 0, type, staMacAddr, key);
+                if ((type == ZM_TKIP)
+#ifdef ZM_ENABLE_CENC
+         || (type == ZM_CENC)
+#endif //ZM_ENABLE_CENC
+           )
+            zfHpSetKey(dev, (staAid-1), 1, type, staMacAddr, micKey);
+        return 0;
+    }
+    return 1;
+}
+
+u32_t zfHpSetApGroupKey(zdev_t* dev, u16_t* apMacAddr, u8_t type,
+        u32_t* key, u32_t* micKey, u16_t vapId)
+{
+    zfHpSetKey(dev, ZM_USER_KEY_DEFAULT - 1 - vapId, 0, type, apMacAddr, key);	// 6D18 modify from 0 to 1 ??
+            if ((type == ZM_TKIP)
+#ifdef ZM_ENABLE_CENC
+         || (type == ZM_CENC)
+#endif //ZM_ENABLE_CENC
+           )
+        zfHpSetKey(dev, ZM_USER_KEY_DEFAULT - 1 - vapId, 1, type, apMacAddr, micKey);
+    return 0;
+}
+
+u32_t zfHpSetDefaultKey(zdev_t* dev, u8_t keyId, u8_t type, u32_t* key, u32_t* micKey)
+{
+    u16_t macAddr[3] = {0, 0, 0};
+
+    #ifdef ZM_ENABLE_IBSS_WPA2PSK
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+
+    if ( hpPriv->dot11Mode == ZM_HAL_80211_MODE_IBSS_WPA2PSK )
+    { /* If not wpa2psk , use traditional */
+      /* Because the bug of chip , defaultkey should follow the key map rule in register 700 */
+        if ( keyId == 0 )
+            zfHpSetKey(dev, ZM_USER_KEY_DEFAULT+keyId, 0, type, macAddr, key);
+        else
+            zfHpSetKey(dev, ZM_USER_KEY_DEFAULT+keyId, 1, type, macAddr, key);
+    }
+    else
+        zfHpSetKey(dev, ZM_USER_KEY_DEFAULT+keyId, 0, type, macAddr, key);
+    #else
+        zfHpSetKey(dev, ZM_USER_KEY_DEFAULT+keyId, 0, type, macAddr, key);
+    #endif
+            if ((type == ZM_TKIP)
+
+#ifdef ZM_ENABLE_CENC
+         || (type == ZM_CENC)
+#endif //ZM_ENABLE_CENC
+           )
+    {
+        zfHpSetKey(dev, ZM_USER_KEY_DEFAULT+keyId, 1, type, macAddr, micKey);
+    }
+
+    return 0;
+}
+
+u32_t zfHpSetPerUserKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t* mac, u8_t type, u32_t* key, u32_t* micKey)
+{
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+
+    if ( hpPriv->dot11Mode == ZM_HAL_80211_MODE_IBSS_WPA2PSK )
+    { /* If not wpa2psk , use traditional */
+        if(keyId)
+        {  /* Set Group Key */
+            zfHpSetKey(dev, user, 1, type, (u16_t *)mac, key);
+        }
+        else if(keyId == 0)
+        {  /* Set Pairwise Key */
+            zfHpSetKey(dev, user, 0, type, (u16_t *)mac, key);
+        }
+    }
+    else
+    {
+        zfHpSetKey(dev, user, keyId, type, (u16_t *)mac, key);
+    }
+#else
+    zfHpSetKey(dev, user, keyId, type, (u16_t *)mac, key);
+#endif
+
+            if ((type == ZM_TKIP)
+#ifdef ZM_ENABLE_CENC
+         || (type == ZM_CENC)
+#endif //ZM_ENABLE_CENC
+           )
+    {
+        zfHpSetKey(dev, user, keyId + 1, type, (u16_t *)mac, micKey);
+    }
+    return 0;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfHpRemoveKey               */
+/*      Remove key.                                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Yuan-Gu Wei         ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfHpRemoveKey(zdev_t* dev, u16_t user)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret = 0;
+
+    cmd[0] = 0x00002904;
+    cmd[1] = (u32_t)user;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_INTERNAL_WRITE, NULL);
+    return ret;
+}
+
+
+
+/******************** DMA ********************/
+u16_t zfHpStartRecv(zdev_t* dev)
+{
+    zfDelayWriteInternalReg(dev, 0x1c3d30, 0x100);
+    zfFlushDelayWrite(dev);
+
+    return 0;
+}
+
+u16_t zfHpStopRecv(zdev_t* dev)
+{
+    return 0;
+}
+
+
+/******************** MAC ********************/
+void zfInitMac(zdev_t* dev)
+{
+    /* ACK extension register */
+    // jhlee temp : change value 0x2c -> 0x40
+    // honda resolve short preamble problem : 0x40 -> 0x75
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_ACK_EXTENSION, 0x40); // 0x28 -> 0x2c 6522:yflee
+
+    /* TxQ0/1/2/3 Retry MAX=2 => transmit 3 times and degrade rate for retry */
+    /* PB42 AP crash issue:                                                  */
+    /* Workaround the crash issue by CTS/RTS, set retry max to zero for      */
+    /*   workaround tx underrun which enable CTS/RTS */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_RETRY_MAX, 0); // 0x11111 => 0
+
+    /* use hardware MIC check */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_SNIFFER, 0x2000000);
+
+    /* Set Rx threshold to 1600 */
+#if ZM_LARGEPAYLOAD_TEST == 1
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_RX_THRESHOLD, 0xc4000);
+#else
+    #ifndef ZM_DISABLE_AMSDU8K_SUPPORT
+    /* The maximum A-MSDU length is 3839/7935 */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_RX_THRESHOLD, 0xc1f80);
+    #else
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_RX_THRESHOLD, 0xc0f80);
+    #endif
+#endif
+
+    //zfDelayWriteInternalReg(dev, ZM_MAC_REG_DYNAMIC_SIFS_ACK, 0x10A);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_RX_PE_DELAY, 0x70);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_EIFS_AND_SIFS, 0xa144000);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_SLOT_TIME, 9<<10);
+
+    /* CF-END mode */
+    zfDelayWriteInternalReg(dev, 0x1c3b2c, 0x19000000);
+
+    //NAV protects ACK only (in TXOP)
+    zfDelayWriteInternalReg(dev, 0x1c3b38, 0x201);
+
+
+    /* Set Beacon PHY CTRL's TPC to 0x7, TA1=1 */
+    /* OTUS set AM to 0x1 */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_HT1, 0x8000170);
+
+    /* TODO : wep backoff protection 0x63c */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BACKOFF_PROTECT, 0x105);
+
+    /* AGG test code*/
+    /* Aggregation MAX number and timeout */
+    zfDelayWriteInternalReg(dev, 0x1c3b9c, 0x10000a);
+    /* Filter any control frames, BAR is bit 24 */
+    zfDelayWriteInternalReg(dev, 0x1c368c, 0x0500ffff);
+    /* Enable deaggregator */
+    zfDelayWriteInternalReg(dev, 0x1c3c40, 0x1);
+
+    /* Basic rate */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BASIC_RATE, 0x150f);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_MANDATORY_RATE, 0x150f);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_RTS_CTS_RATE, 0x10b01bb);
+
+    /* MIMO resposne control */
+    zfDelayWriteInternalReg(dev, 0x1c3694, 0x4003C1E);/* bit 26~28  otus-AM */
+
+    /* Enable LED0 and LED1 */
+    zfDelayWriteInternalReg(dev, 0x1d0100, 0x3);
+    zfDelayWriteInternalReg(dev, 0x1d0104, 0x3);
+
+    /* switch MAC to OTUS interface */
+    zfDelayWriteInternalReg(dev, 0x1c3600, 0x3);
+
+    /* RXMAC A-MPDU length threshold */
+    zfDelayWriteInternalReg(dev, 0x1c3c50, 0xffff);
+
+	/* Phy register read timeout */
+	zfDelayWriteInternalReg(dev, 0x1c3680, 0xf00008);
+
+	/* Disable Rx TimeOut : workaround for BB.
+	 *  OTUS would interrupt the rx frame that sent by OWL TxUnderRun
+	 *  because OTUS rx timeout behavior, then OTUS would not ack the BA for
+	 *  this AMPDU from OWL.
+	 *  Fix by Perry Hwang.  2007/05/10.
+	 *  0x1c362c : Rx timeout value : bit 27~16
+	 */
+	zfDelayWriteInternalReg(dev, 0x1c362c, 0x0);
+
+    //Set USB Rx stream mode MAX packet number to 2
+    //    Max packet number = *0x1e1110 + 1
+    zfDelayWriteInternalReg(dev, 0x1e1110, 0x4);
+    //Set USB Rx stream mode timeout to 10us
+    zfDelayWriteInternalReg(dev, 0x1e1114, 0x80);
+
+    //Set CPU clock frequency to 88/80MHz
+    zfDelayWriteInternalReg(dev, 0x1D4008, 0x73);
+
+    //Set WLAN DMA interrupt mode : generate int per packet
+    zfDelayWriteInternalReg(dev, 0x1c3d7c, 0x110011);
+
+    /* 7807 */
+    /* enable func : Reset FIFO1 and FIFO2 when queue-gnt is low */
+    /* 0x1c3bb0 Bit2 */
+    /* Disable SwReset in firmware for TxHang, enable reset FIFO func. */
+    zfDelayWriteInternalReg(dev, 0x1c3bb0, 0x4);
+
+    /* Disables the CF_END frame */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_TXOP_NOT_ENOUGH_INDICATION, 0x141E0F48);
+
+	/* Disable the SW Decrypt*/
+	zfDelayWriteInternalReg(dev, 0x1c3678, 0x70);
+    zfFlushDelayWrite(dev);
+    //---------------------
+
+    /* Set TxQs CWMIN, CWMAX, AIFS and TXO to WME STA default. */
+    zfUpdateDefaultQosParameter(dev, 0);
+
+    //zfSelAdcClk(dev, 0);
+
+    return;
+}
+
+
+u16_t zfHpSetSnifferMode(zdev_t* dev, u16_t on)
+{
+    if (on != 0)
+    {
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_SNIFFER, 0x2000001);
+    }
+    else
+    {
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_SNIFFER, 0x2000000);
+    }
+    zfFlushDelayWrite(dev);
+    return 0;
+}
+
+
+u16_t zfHpSetApStaMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+    hpPriv->dot11Mode = mode;
+
+    switch(mode)
+    {
+        case ZM_HAL_80211_MODE_AP:
+            zfDelayWriteInternalReg(dev, 0x1c3700, 0x0f0000a1);
+            zfDelayWriteInternalReg(dev, 0x1c3c40, 0x1);
+            break;
+
+        case ZM_HAL_80211_MODE_STA:
+            zfDelayWriteInternalReg(dev, 0x1c3700, 0x0f000002);
+            zfDelayWriteInternalReg(dev, 0x1c3c40, 0x1);
+            break;
+
+        case ZM_HAL_80211_MODE_IBSS_GENERAL:
+            zfDelayWriteInternalReg(dev, 0x1c3700, 0x0f000000);
+            zfDelayWriteInternalReg(dev, 0x1c3c40, 0x1);
+            break;
+
+        case ZM_HAL_80211_MODE_IBSS_WPA2PSK:
+            zfDelayWriteInternalReg(dev, 0x1c3700, 0x0f0000e0);
+            zfDelayWriteInternalReg(dev, 0x1c3c40, 0x41);       // for multiple ( > 2 ) stations IBSS network
+            break;
+
+        default:
+            goto skip;
+    }
+
+    zfFlushDelayWrite(dev);
+
+skip:
+    return 0;
+}
+
+
+u16_t zfHpSetBssid(zdev_t* dev, u8_t* bssidSrc)
+{
+    u32_t  address;
+    u16_t *bssid = (u16_t *)bssidSrc;
+
+    address = bssid[0] + (((u32_t)bssid[1]) << 16);
+    zfDelayWriteInternalReg(dev, 0x1c3618, address);
+
+    address = (u32_t)bssid[2];
+    zfDelayWriteInternalReg(dev, 0x1c361C, address);
+    zfFlushDelayWrite(dev);
+    return 0;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfHpUpdateQosParameter      */
+/*      Update TxQs CWMIN, CWMAX, AIFS and TXOP.                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      cwminTbl : CWMIN parameter for TxQs                             */
+/*      cwmaxTbl : CWMAX parameter for TxQs                             */
+/*      aifsTbl: AIFS parameter for TxQs                                */
+/*      txopTbl : TXOP parameter for TxQs                               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfHpUpdateQosParameter(zdev_t* dev, u16_t* cwminTbl, u16_t* cwmaxTbl,
+        u16_t* aifsTbl, u16_t* txopTbl)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+
+    zm_msg0_mm(ZM_LV_0, "zfHalUpdateQosParameter()");
+
+    /* Note : Do not change cwmin for Q0 in Ad Hoc mode              */
+    /*        otherwise driver will fail in Wifi beacon distribution */
+    if (hpPriv->dot11Mode == ZM_HAL_80211_MODE_STA)
+    {
+#if 0 //Restore CWmin to improve down link throughput
+        //cheating in BE traffic
+        if (wd->sta.EnableHT == 1)
+        {
+            //cheating in BE traffic
+            cwminTbl[0] = 7;//15;
+        }
+#endif
+        cwmaxTbl[0] = 127;//1023;
+        aifsTbl[0] = 2*9+10;//3 * 9 + 10;
+    }
+
+    /* CWMIN and CWMAX */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC0_CW, cwminTbl[0]
+            + ((u32_t)cwmaxTbl[0]<<16));
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_CW, cwminTbl[1]
+            + ((u32_t)cwmaxTbl[1]<<16));
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC2_CW, cwminTbl[2]
+            + ((u32_t)cwmaxTbl[2]<<16));
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC3_CW, cwminTbl[3]
+            + ((u32_t)cwmaxTbl[3]<<16));
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC4_CW, cwminTbl[4]
+            + ((u32_t)cwmaxTbl[4]<<16));
+
+    /* AIFS */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_AC0_AIFS, aifsTbl[0]
+            +((u32_t)aifsTbl[0]<<12)+((u32_t)aifsTbl[0]<<24));
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC3_AC2_AIFS, (aifsTbl[0]>>8)
+            +((u32_t)aifsTbl[0]<<4)+((u32_t)aifsTbl[0]<<16));
+
+    /* TXOP */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_AC0_TXOP, txopTbl[0]
+            + ((u32_t)txopTbl[1]<<16));
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC3_AC2_TXOP, txopTbl[2]
+            + ((u32_t)txopTbl[3]<<16));
+
+    zfFlushDelayWrite(dev);
+
+    hpPriv->txop[0] = txopTbl[0];
+    hpPriv->txop[1] = txopTbl[1];
+    hpPriv->txop[2] = txopTbl[2];
+    hpPriv->txop[3] = txopTbl[3];
+    hpPriv->cwmin[0] = cwminTbl[0];
+    hpPriv->cwmax[0] = cwmaxTbl[0];
+    hpPriv->cwmin[1] = cwminTbl[1];
+    hpPriv->cwmax[1] = cwmaxTbl[1];
+
+    return 0;
+}
+
+
+void zfHpSetAtimWindow(zdev_t* dev, u16_t atimWin)
+{
+    zm_msg1_mm(ZM_LV_0, "Set ATIM window to ", atimWin);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_ATIM_WINDOW, atimWin);
+    zfFlushDelayWrite(dev);
+}
+
+
+void zfHpSetBasicRateSet(zdev_t* dev, u16_t bRateBasic, u16_t gRateBasic)
+{
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BASIC_RATE, bRateBasic
+                            | ((u16_t)gRateBasic<<8));
+    zfFlushDelayWrite(dev);
+}
+
+
+/* HT40 send by OFDM 6M    */
+/* otherwise use reg 0x638 */
+void zfHpSetRTSCTSRate(zdev_t* dev, u32_t rate)
+{
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_RTS_CTS_RATE, rate);
+    zfFlushDelayWrite(dev);
+}
+
+void zfHpSetMacAddress(zdev_t* dev, u16_t* macAddr, u16_t macAddrId)
+{
+    if (macAddrId == 0)
+    {
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_MAC_ADDR_L,
+                (((u32_t)macAddr[1])<<16) | macAddr[0]);
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_MAC_ADDR_H, macAddr[2]);
+    }
+    else if (macAddrId <= 7)
+    {
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_ACK_TABLE+((macAddrId-1)*8),
+                macAddr[0] + ((u32_t)macAddr[1]<<16));
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_ACK_TABLE+((macAddrId-1)*8)+4,
+                macAddr[2]);
+    }
+    zfFlushDelayWrite(dev);
+}
+
+void zfHpSetMulticastList(zdev_t* dev, u8_t size, u8_t* pList, u8_t bAllMulticast)
+{
+    struct zsMulticastAddr* pMacList = (struct zsMulticastAddr*) pList;
+    u8_t   i;
+    u32_t  value;
+    u32_t  swRegMulHashValueH, swRegMulHashValueL;
+
+    swRegMulHashValueH = 0x80000000;
+    swRegMulHashValueL = 0;
+
+    if ( bAllMulticast )
+    {
+        swRegMulHashValueH = swRegMulHashValueL = ~0;
+    }
+    else
+    {
+        for(i=0; i<size; i++)
+        {
+            value = pMacList[i].addr[5] >> 2;
+
+            if ( value < 32 )
+            {
+                swRegMulHashValueL |= (1 << value);
+            }
+            else
+            {
+                swRegMulHashValueH |= (1 << (value-32));
+            }
+        }
+    }
+
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_GROUP_HASH_TBL_L,
+                            swRegMulHashValueL);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_GROUP_HASH_TBL_H,
+                            swRegMulHashValueH);
+    zfFlushDelayWrite(dev);
+    return;
+}
+
+/******************** Beacon ********************/
+void zfHpEnableBeacon(zdev_t* dev, u16_t mode, u16_t bcnInterval, u16_t dtim, u8_t enableAtim)
+{
+    u32_t  value;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Beacon Ready */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_CTRL, 0);
+    /* Beacon DMA buffer address */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_ADDR, ZM_BEACON_BUFFER_ADDRESS);
+
+    value = bcnInterval;
+
+    value |= (((u32_t) dtim) << 16);
+
+    if (mode == ZM_MODE_AP)
+    {
+
+        value |= 0x1000000;
+    }
+    else if (mode == ZM_MODE_IBSS)
+    {
+        value |= 0x2000000;
+
+		if ( enableAtim )
+		{
+			value |= 0x4000000;
+		}
+		((struct zsHpPriv*)wd->hpPrivate)->ibssBcnEnabled = 1;
+        ((struct zsHpPriv*)wd->hpPrivate)->ibssBcnInterval = value;
+    }
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_PRETBTT, (bcnInterval-6)<<16);
+
+    /* Beacon period and beacon enable */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PERIOD, value);
+    zfFlushDelayWrite(dev);
+}
+
+void zfHpDisableBeacon(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    ((struct zsHpPriv*)wd->hpPrivate)->ibssBcnEnabled = 0;
+
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PERIOD, 0);
+    zfFlushDelayWrite(dev);
+}
+
+void zfHpLedCtrl(zdev_t* dev, u16_t ledId, u8_t mode)
+{
+    u16_t state;
+    zmw_get_wlan_dev(dev);
+
+    //zm_debug_msg1("LED ID=", ledId);
+    //zm_debug_msg1("LED mode=", mode);
+    if (ledId < 2)
+    {
+        if (((struct zsHpPriv*)wd->hpPrivate)->ledMode[ledId] != mode)
+        {
+            ((struct zsHpPriv*)wd->hpPrivate)->ledMode[ledId] = mode;
+
+            state = ((struct zsHpPriv*)wd->hpPrivate)->ledMode[0]
+                    | (((struct zsHpPriv*)wd->hpPrivate)->ledMode[1]<<1);
+            zfDelayWriteInternalReg(dev, 0x1d0104, state);
+            zfFlushDelayWrite(dev);
+            //zm_debug_msg0("Update LED");
+        }
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfHpResetTxRx               */
+/*      Reset Tx and Rx Desc.                                           */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Chao-Wen Yang         ZyDAS Technology Corporation    2007.3    */
+/*                                                                      */
+/************************************************************************/
+u16_t zfHpUsbReset(zdev_t* dev)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret = 0;
+
+    //zm_debug_msg0("CWY - Reset Tx and Rx");
+
+    cmd[0] =  0 | (ZM_CMD_RESET << 8);
+
+    ret = zfIssueCmd(dev, cmd, 4, ZM_OID_INTERNAL_WRITE, NULL);
+    return ret;
+}
+
+u16_t zfHpDKReset(zdev_t* dev, u8_t flag)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret = 0;
+
+    //zm_debug_msg0("CWY - Reset Tx and Rx");
+
+    cmd[0] =  4 | (ZM_CMD_DKRESET << 8);
+    cmd[1] = flag;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_INTERNAL_WRITE, NULL);
+    return ret;
+}
+
+u32_t zfHpCwmUpdate(zdev_t* dev)
+{
+    //u32_t cmd[3];
+    //u16_t ret;
+    //
+    //cmd[0] = 0x00000008;
+    //cmd[1] = 0x1c36e8;
+    //cmd[2] = 0x1c36ec;
+    //
+    //ret = zfIssueCmd(dev, cmd, 12, ZM_CWM_READ, 0);
+    //return ret;
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zfCoreCwmBusy(dev, zfCwmIsExtChanBusy(hpPriv->ctlBusy, hpPriv->extBusy));
+
+    hpPriv->ctlBusy = 0;
+    hpPriv->extBusy = 0;
+
+    return 0;
+}
+
+u32_t zfHpAniUpdate(zdev_t* dev)
+{
+    u32_t cmd[5];
+    u16_t ret;
+
+    cmd[0] = 0x00000010;
+    cmd[1] = 0x1c36e8;
+    cmd[2] = 0x1c36ec;
+    cmd[3] = 0x1c3cb4;
+    cmd[4] = 0x1c3cb8;
+
+    ret = zfIssueCmd(dev, cmd, 20, ZM_ANI_READ, 0);
+    return ret;
+}
+
+/*
+ * Update Beacon RSSI in ANI
+ */
+u32_t zfHpAniUpdateRssi(zdev_t* dev, u8_t rssi)
+{
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    hpPriv->stats.ast_nodestats.ns_avgbrssi = rssi;
+
+    return 0;
+}
+
+#define ZM_SEEPROM_MAC_ADDRESS_OFFSET   (0x1400 + (0x106<<1))
+#define ZM_SEEPROM_REGDOMAIN_OFFSET   (0x1400 + (0x104<<1))
+#define ZM_SEEPROM_VERISON_OFFSET   (0x1400 + (0x102<<1))
+#define ZM_SEEPROM_HARDWARE_TYPE_OFFSET   (0x1374)
+#define ZM_SEEPROM_HW_HEAVY_CLIP          (0x161c)
+
+u32_t zfHpGetMacAddress(zdev_t* dev)
+{
+    u32_t cmd[7];
+    u16_t ret;
+
+    cmd[0] = 0x00000000 | 24;
+    cmd[1] = ZM_SEEPROM_MAC_ADDRESS_OFFSET;
+    cmd[2] = ZM_SEEPROM_MAC_ADDRESS_OFFSET+4;
+    cmd[3] = ZM_SEEPROM_REGDOMAIN_OFFSET;
+    cmd[4] = ZM_SEEPROM_VERISON_OFFSET;
+    cmd[5] = ZM_SEEPROM_HARDWARE_TYPE_OFFSET;
+    cmd[6] = ZM_SEEPROM_HW_HEAVY_CLIP;
+
+    ret = zfIssueCmd(dev, cmd, 28, ZM_MAC_READ, 0);
+    return ret;
+}
+
+u32_t zfHpGetTransmitPower(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv*    hpPriv  = wd->hpPrivate;
+    u16_t               tpc     = 0;
+
+    if (hpPriv->hwFrequency < 3000) {
+        tpc = hpPriv->tPow2x2g[0] & 0x3f;
+        wd->maxTxPower2 &= 0x3f;
+        tpc = (tpc > wd->maxTxPower2)? wd->maxTxPower2 : tpc;
+    } else {
+        tpc = hpPriv->tPow2x5g[0] & 0x3f;
+        wd->maxTxPower5 &= 0x3f;
+        tpc = (tpc > wd->maxTxPower5)? wd->maxTxPower5 : tpc;
+    }
+
+    return tpc;
+}
+
+u8_t zfHpGetMinTxPower(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv*    hpPriv  = wd->hpPrivate;
+    u8_t               tpc     = 0;
+
+    if (hpPriv->hwFrequency < 3000)
+    {
+        if(wd->BandWidth40)
+        {
+            //40M
+            tpc = (hpPriv->tPow2x2gHt40[7]&0x3f);
+        }
+        else
+        {
+            //20M
+            tpc = (hpPriv->tPow2x2gHt20[7]&0x3f);
+        }
+    }
+    else
+    {
+        if(wd->BandWidth40)
+        {
+            //40M
+            tpc = (hpPriv->tPow2x5gHt40[7]&0x3f);
+        }
+        else
+        {
+            //20M
+            tpc = (hpPriv->tPow2x5gHt20[7]&0x3f);
+        }
+    }
+
+    return tpc;
+}
+
+u8_t zfHpGetMaxTxPower(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv*    hpPriv  = wd->hpPrivate;
+    u8_t               tpc     = 0;
+
+    if (hpPriv->hwFrequency < 3000)
+    {
+        tpc = (hpPriv->tPow2xCck[0]&0x3f);
+    }
+    else
+    {
+        tpc =(hpPriv->tPow2x5g[0]&0x3f);
+    }
+
+    return tpc;
+}
+
+u32_t zfHpLoadEEPROMFromFW(zdev_t* dev)
+{
+    u32_t cmd[16];
+    u32_t ret=0, i, j;
+    zmw_get_wlan_dev(dev);
+
+    i = ((struct zsHpPriv*)wd->hpPrivate)->eepromImageRdReq;
+
+    cmd[0] = ZM_HAL_MAX_EEPROM_PRQ*4;
+
+    for (j=0; j<ZM_HAL_MAX_EEPROM_PRQ; j++)
+    {
+        cmd[j+1] = 0x1000 + (((i*ZM_HAL_MAX_EEPROM_PRQ) + j)*4);
+    }
+
+    ret = zfIssueCmd(dev, cmd, (ZM_HAL_MAX_EEPROM_PRQ+1)*4, ZM_EEPROM_READ, 0);
+
+    return ret;
+}
+
+void zfHpHeartBeat(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+    u8_t polluted = 0;
+    u8_t ackTpc;
+
+    /* Workaround : Make OTUS fire more beacon in ad hoc mode in 2.4GHz */
+    if (hpPriv->ibssBcnEnabled != 0)
+    {
+        if (hpPriv->hwFrequency <= ZM_CH_G_14)
+        {
+            if ((wd->tick % 10) == 0)
+            {
+                if ((wd->tick % 40) == 0)
+                {
+                    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PERIOD, hpPriv->ibssBcnInterval-1);
+                    polluted = 1;
+                }
+                else
+                {
+                    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PERIOD, hpPriv->ibssBcnInterval);
+                    polluted = 1;
+                }
+            }
+        }
+    }
+
+    if ((wd->tick & 0x3f) == 0x25)
+    {
+        /* Workaround for beacon stuck after SW reset */
+        if (hpPriv->ibssBcnEnabled != 0)
+        {
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_ADDR, ZM_BEACON_BUFFER_ADDRESS);
+            polluted = 1;
+        }
+
+        //DbgPrint("hpPriv->aggMaxDurationBE=%d", hpPriv->aggMaxDurationBE);
+        //DbgPrint("wd->sta.avgSizeOfReceivePackets=%d", wd->sta.avgSizeOfReceivePackets);
+        if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+            && (zfStaIsConnected(dev))
+            && (wd->sta.EnableHT == 1) //11n mode
+            && (wd->BandWidth40 == 1) //40MHz mode
+            && (wd->sta.enableDrvBA ==0) //Marvel AP
+            && (hpPriv->aggMaxDurationBE > 2000) //BE TXOP > 2ms
+            && (wd->sta.avgSizeOfReceivePackets > 1420))
+        {
+            zfDelayWriteInternalReg(dev, 0x1c3b9c, 0x8000a);
+            polluted = 1;
+        }
+        else
+        {
+            zfDelayWriteInternalReg(dev, 0x1c3b9c, hpPriv->aggPktNum);
+            polluted = 1;
+        }
+
+        if (wd->dynamicSIFSEnable == 0)
+        {
+            if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+                && (zfStaIsConnected(dev))
+                && (wd->sta.EnableHT == 1) //11n mode
+                && (wd->BandWidth40 == 0) //20MHz mode
+                && (wd->sta.enableDrvBA ==0)) //Marvel AP
+            {
+                zfDelayWriteInternalReg(dev, 0x1c3698, 0x5144000);
+                polluted = 1;
+            }
+            else
+            {
+                zfDelayWriteInternalReg(dev, 0x1c3698, 0xA144000);
+                polluted = 1;
+            }
+        }
+        else
+        {
+            if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+                && (zfStaIsConnected(dev))
+                && (wd->sta.EnableHT == 1) //11n mode
+                && (wd->sta.athOwlAp == 1)) //Atheros AP
+            {
+                if (hpPriv->retransmissionEvent)
+                {
+                    switch(hpPriv->latestSIFS)
+                    {
+                    case 0:
+                        hpPriv->latestSIFS = 1;
+                        zfDelayWriteInternalReg(dev, ZM_MAC_REG_EIFS_AND_SIFS, 0x8144000);
+                        break;
+                    case 1:
+                        hpPriv->latestSIFS = 2;
+                        zfDelayWriteInternalReg(dev, ZM_MAC_REG_EIFS_AND_SIFS, 0xa144000);
+                        break;
+                    case 2:
+                        hpPriv->latestSIFS = 3;
+                        zfDelayWriteInternalReg(dev, ZM_MAC_REG_EIFS_AND_SIFS, 0xc144000);
+                        break;
+                    case 3:
+                        hpPriv->latestSIFS = 0;
+                        zfDelayWriteInternalReg(dev, ZM_MAC_REG_EIFS_AND_SIFS, 0xa144000);
+                        break;
+                    default:
+                        hpPriv->latestSIFS = 0;
+                        zfDelayWriteInternalReg(dev, ZM_MAC_REG_EIFS_AND_SIFS, 0xa144000);
+                        break;
+                    }
+                    polluted = 1;
+                    zm_debug_msg1("##### Correct Tx retransmission issue #####, ", hpPriv->latestSIFS);
+                    hpPriv->retransmissionEvent = 0;
+                }
+            }
+            else
+            {
+                hpPriv->latestSIFS = 0;
+                hpPriv->retransmissionEvent = 0;
+                zfDelayWriteInternalReg(dev, 0x1c3698, 0xA144000);
+                polluted = 1;
+            }
+        }
+
+        if ((wd->sta.bScheduleScan == FALSE) && (wd->sta.bChannelScan == FALSE))
+        {
+#define ZM_SIGNAL_THRESHOLD  66
+        if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+            && (zfStaIsConnected(dev))
+            && (wd->SignalStrength > ZM_SIGNAL_THRESHOLD))
+        {
+                /* remove state handle, always rewrite register setting */
+                //if (hpPriv->strongRSSI == 0)
+            {
+                hpPriv->strongRSSI = 1;
+                /* Strong RSSI, set ACK to one Tx stream and lower Tx power 7dbm */
+                if (hpPriv->currentAckRtsTpc > (14+10))
+                {
+                    ackTpc = hpPriv->currentAckRtsTpc - 14;
+                }
+                else
+                {
+                    ackTpc = 10;
+                }
+                zfDelayWriteInternalReg(dev, 0x1c3694, ((ackTpc) << 20) | (0x1<<26));
+                zfDelayWriteInternalReg(dev, 0x1c3bb4, ((ackTpc) << 5 ) | (0x1<<11) |
+                                                       ((ackTpc) << 21) | (0x1<<27)  );
+                polluted = 1;
+            }
+        }
+        else
+        {
+                /* remove state handle, always rewrite register setting */
+                //if (hpPriv->strongRSSI == 1)
+            {
+                hpPriv->strongRSSI = 0;
+                if (hpPriv->halCapability & ZM_HP_CAP_11N_ONE_TX_STREAM)
+                {
+                    zfDelayWriteInternalReg(dev, 0x1c3694, ((hpPriv->currentAckRtsTpc&0x3f) << 20) | (0x1<<26));
+                    zfDelayWriteInternalReg(dev, 0x1c3bb4, ((hpPriv->currentAckRtsTpc&0x3f) << 5 ) | (0x1<<11) |
+                                                       ((hpPriv->currentAckRtsTpc&0x3f) << 21) | (0x1<<27)  );
+                }
+                else
+                {
+                    zfDelayWriteInternalReg(dev, 0x1c3694, ((hpPriv->currentAckRtsTpc&0x3f) << 20) | (0x5<<26));
+                    zfDelayWriteInternalReg(dev, 0x1c3bb4, ((hpPriv->currentAckRtsTpc&0x3f) << 5 ) | (0x5<<11) |
+                                                       ((hpPriv->currentAckRtsTpc&0x3f) << 21) | (0x5<<27)  );
+                }
+                polluted = 1;
+            }
+        }
+#undef ZM_SIGNAL_THRESHOLD
+        }
+
+        if ((hpPriv->halCapability & ZM_HP_CAP_11N_ONE_TX_STREAM) == 0)
+        {
+            if ((wd->sta.bScheduleScan == FALSE) && (wd->sta.bChannelScan == FALSE))
+            {
+    #define ZM_RX_SIGNAL_THRESHOLD_H  71
+    #define ZM_RX_SIGNAL_THRESHOLD_L  66
+                u8_t rxSignalThresholdH = ZM_RX_SIGNAL_THRESHOLD_H;
+                u8_t rxSignalThresholdL = ZM_RX_SIGNAL_THRESHOLD_L;
+    #undef ZM_RX_SIGNAL_THRESHOLD_H
+    #undef ZM_RX_SIGNAL_THRESHOLD_L
+
+                if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+                    && (zfStaIsConnected(dev))
+                    && (wd->SignalStrength > rxSignalThresholdH)
+                    )//&& (hpPriv->rxStrongRSSI == 0))
+                {
+                    hpPriv->rxStrongRSSI = 1;
+                    //zfDelayWriteInternalReg(dev, 0x1c5964, 0x1220);
+                    //zfDelayWriteInternalReg(dev, 0x1c5960, 0x900);
+                    //zfDelayWriteInternalReg(dev, 0x1c6960, 0x900);
+                    //zfDelayWriteInternalReg(dev, 0x1c7960, 0x900);
+                    if ((hpPriv->eepromImage[0x100+0x110*2/4]&0xff) == 0x80) //FEM TYPE
+                    {
+                        if (hpPriv->hwFrequency <= ZM_CH_G_14)
+                        {
+                            zfDelayWriteInternalReg(dev, 0x1c8960, 0x900);
+                        }
+                        else
+                        {
+                            zfDelayWriteInternalReg(dev, 0x1c8960, 0x9b49);
+                        }
+                    }
+                    else
+                    {
+                        zfDelayWriteInternalReg(dev, 0x1c8960, 0x0900);
+                    }
+                    polluted = 1;
+                }
+                else if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+                    && (zfStaIsConnected(dev))
+                    && (wd->SignalStrength > rxSignalThresholdL)
+                    )//&& (hpPriv->rxStrongRSSI == 1))
+                {
+                    //Do nothing to prevent frequently Rx switching
+                }
+                else
+                {
+                    /* remove state handle, always rewrite register setting */
+                    //if (hpPriv->rxStrongRSSI == 1)
+                    {
+                        hpPriv->rxStrongRSSI = 0;
+                        //zfDelayWriteInternalReg(dev, 0x1c5964, 0x1120);
+                        //zfDelayWriteInternalReg(dev, 0x1c5960, 0x9b40);
+                        //zfDelayWriteInternalReg(dev, 0x1c6960, 0x9b40);
+                        //zfDelayWriteInternalReg(dev, 0x1c7960, 0x9b40);
+                        if ((hpPriv->eepromImage[0x100+0x110*2/4]&0xff) == 0x80) //FEM TYPE
+                        {
+                            if (hpPriv->hwFrequency <= ZM_CH_G_14)
+                            {
+                                zfDelayWriteInternalReg(dev, 0x1c8960, 0x9b49);
+                            }
+                            else
+                            {
+                                zfDelayWriteInternalReg(dev, 0x1c8960, 0x0900);
+                            }
+                        }
+                        else
+                        {
+                            zfDelayWriteInternalReg(dev, 0x1c8960, 0x9b40);
+                        }
+                        polluted = 1;
+                    }
+                }
+
+            }
+        }
+
+        if (hpPriv->usbAcSendBytes[3] > (hpPriv->usbAcSendBytes[0]*2))
+        {
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_AC0_TXOP, hpPriv->txop[3]);
+            polluted = 1;
+        }
+        else if (hpPriv->usbAcSendBytes[2] > (hpPriv->usbAcSendBytes[0]*2))
+        {
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_AC0_TXOP, hpPriv->txop[2]);
+            polluted = 1;
+        }
+        else if (hpPriv->usbAcSendBytes[1] > (hpPriv->usbAcSendBytes[0]*2))
+        {
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC0_CW, hpPriv->cwmin[1]+((u32_t)hpPriv->cwmax[1]<<16));
+            polluted = 1;
+        }
+        else
+        {
+            if (hpPriv->slotType == 1)
+            {
+                if ((wd->sta.enableDrvBA ==0) //Marvel AP
+                   && (hpPriv->aggMaxDurationBE > 2000)) //BE TXOP > 2ms
+                {
+                    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC0_CW, (hpPriv->cwmin[0]/2)+((u32_t)hpPriv->cwmax[0]<<16));
+                }
+                else
+                {
+                    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC0_CW, hpPriv->cwmin[0]+((u32_t)hpPriv->cwmax[0]<<16));
+                }
+                polluted = 1;
+            }
+            else
+            {
+                /* Compensation for 20us slot time */
+                //zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC0_CW, 58+((u32_t)hpPriv->cwmax[0]<<16));
+                zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC0_CW, hpPriv->cwmin[0]+((u32_t)hpPriv->cwmax[0]<<16));
+                polluted = 1;
+            }
+
+            if ((wd->sta.SWEncryptEnable & (ZM_SW_TKIP_ENCRY_EN|ZM_SW_WEP_ENCRY_EN)) == 0)
+            {
+                zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_AC0_TXOP, hpPriv->txop[0]);
+                polluted = 1;
+            }
+            else
+            {
+                zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_AC0_TXOP, 0x30);
+                polluted = 1;
+            }
+
+        }
+        hpPriv->usbAcSendBytes[3] = 0;
+        hpPriv->usbAcSendBytes[2] = 0;
+        hpPriv->usbAcSendBytes[1] = 0;
+        hpPriv->usbAcSendBytes[0] = 0;
+    }
+
+    if (polluted == 1)
+    {
+        zfFlushDelayWrite(dev);
+    }
+
+    return;
+}
+
+/*
+ *  0x1d4008 : AHB, DAC, ADC clock selection
+ *             bit1~0  AHB_CLK : AHB clock selection,
+ *                               00 : OSC 40MHz;
+ *                               01 : 20MHz in A mode, 22MHz in G mode;
+ *                               10 : 40MHz in A mode, 44MHz in G mode;
+ *                               11 : 80MHz in A mode, 88MHz in G mode.
+ *             bit3~2  CLK_SEL : Select the clock source of clk160 in ADDAC.
+ *                               00 : PLL divider's output;
+ *                               01 : PLL divider's output divided by 2;
+ *                               10 : PLL divider's output divided by 4;
+ *                               11 : REFCLK from XTALOSCPAD.
+ */
+void zfSelAdcClk(zdev_t* dev, u8_t bw40, u32_t frequency)
+{
+    if(bw40 == 1)
+    {
+        //zfDelayWriteInternalReg(dev, 0x1D4008, 0x73);
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_DYNAMIC_SIFS_ACK, 0x10A);
+        zfFlushDelayWrite(dev);
+    }
+    else
+    {
+        //zfDelayWriteInternalReg(dev, 0x1D4008, 0x70);
+        if ( frequency <= ZM_CH_G_14 )
+        {
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_DYNAMIC_SIFS_ACK, 0x105);
+        }
+        else
+        {
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_DYNAMIC_SIFS_ACK, 0x104);
+        }
+        zfFlushDelayWrite(dev);
+    }
+}
+
+u32_t zfHpEchoCommand(zdev_t* dev, u32_t value)
+{
+    u32_t cmd[2];
+    u16_t ret;
+
+    cmd[0] = 0x00008004;
+    cmd[1] = value;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_CMD_ECHO, NULL);
+    return ret;
+}
+
+#ifdef ZM_DRV_INIT_USB_MODE
+
+#define ZM_USB_US_STREAM_MODE               0x00000000
+#define ZM_USB_US_PACKET_MODE               0x00000008
+#define ZM_USB_DS_ENABLE                    0x00000001
+#define ZM_USB_US_ENABLE                    0x00000002
+
+#define ZM_USB_RX_STREAM_4K                 0x00000000
+#define ZM_USB_RX_STREAM_8K                 0x00000010
+#define ZM_USB_RX_STREAM_16K                0x00000020
+#define ZM_USB_RX_STREAM_32K                0x00000030
+
+#define ZM_USB_TX_STREAM_MODE               0x00000040
+
+#define ZM_USB_MODE_CTRL_REG                0x001E1108
+
+void zfInitUsbMode(zdev_t* dev)
+{
+    u32_t mode;
+    zmw_get_wlan_dev(dev);
+
+    /* TODO: Set USB mode by reading registery */
+    mode = ZM_USB_DS_ENABLE | ZM_USB_US_ENABLE | ZM_USB_US_PACKET_MODE;
+
+    zfDelayWriteInternalReg(dev, ZM_USB_MODE_CTRL_REG, mode);
+    zfFlushDelayWrite(dev);
+}
+#endif
+
+void zfDumpEepBandEdges(struct ar5416Eeprom* eepromImage);
+void zfPrintTargetPower2G(u8_t* tPow2xCck, u8_t* tPow2x2g, u8_t* tPow2x2gHt20, u8_t* tPow2x2gHt40);
+void zfPrintTargetPower5G(u8_t* tPow2x5g, u8_t* tPow2x5gHt20, u8_t* tPow2x5gHt40);
+
+
+s32_t zfInterpolateFunc(s32_t x, s32_t x1, s32_t y1, s32_t x2, s32_t y2)
+{
+    s32_t y;
+
+    if (y2 == y1)
+    {
+        y = y1;
+    }
+    else if (x == x1)
+    {
+        y = y1;
+    }
+    else if (x == x2)
+    {
+        y = y2;
+    }
+    else if (x2 != x1)
+    {
+        y = y1 + (((y2-y1) * (x-x1))/(x2-x1));
+    }
+    else
+    {
+        y = y1;
+    }
+
+    return y;
+}
+
+//#define ZM_ENABLE_TPC_WINDOWS_DEBUG
+//#define ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+
+/* the tx power offset workaround for ART vs NDIS/MDK */
+#define HALTX_POWER_OFFSET      0
+
+u8_t zfInterpolateFuncX(u8_t x, u8_t x1, u8_t y1, u8_t x2, u8_t y2)
+{
+    s32_t y;
+    s32_t inc;
+
+    #define ZM_MULTIPLIER   8
+    y = zfInterpolateFunc((s32_t)x<<ZM_MULTIPLIER,
+                          (s32_t)x1<<ZM_MULTIPLIER,
+                          (s32_t)y1<<ZM_MULTIPLIER,
+                          (s32_t)x2<<ZM_MULTIPLIER,
+                          (s32_t)y2<<ZM_MULTIPLIER);
+
+    inc = (y & (1<<(ZM_MULTIPLIER-1))) >> (ZM_MULTIPLIER-1);
+    y = (y >> ZM_MULTIPLIER) + inc;
+    #undef ZM_MULTIPLIER
+
+    return (u8_t)y;
+}
+
+u8_t zfGetInterpolatedValue(u8_t x, u8_t* x_array, u8_t* y_array)
+{
+    s32_t y;
+    u16_t xIndex;
+
+    if (x <= x_array[1])
+    {
+        xIndex = 0;
+    }
+    else if (x <= x_array[2])
+    {
+        xIndex = 1;
+    }
+    else if (x <= x_array[3])
+    {
+        xIndex = 2;
+    }
+    else //(x > x_array[3])
+    {
+        xIndex = 3;
+    }
+
+    y = zfInterpolateFuncX(x,
+            x_array[xIndex],
+            y_array[xIndex],
+            x_array[xIndex+1],
+            y_array[xIndex+1]);
+
+    return (u8_t)y;
+}
+
+u8_t zfFindFreqIndex(u8_t f, u8_t* fArray, u8_t fArraySize)
+{
+    u8_t i;
+#ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    DbgPrint("f=%d ", f);
+    for (i=0; i<fArraySize; i++)
+    {
+        DbgPrint("%d ", fArray[i]);
+    }
+    DbgPrint("\n");
+#endif
+    i=fArraySize-2;
+    while(1)
+    {
+        if (f >= fArray[i])
+        {
+            return i;
+        }
+        if (i!=0)
+        {
+            i--;
+        }
+        else
+        {
+            return 0;
+        }
+    }
+}
+
+
+
+
+void zfInitPowerCal(zdev_t* dev)
+{
+    //Program PHY Tx power relatives registers
+#define zm_write_phy_reg(cr, val) reg_write((cr*4)+0x9800, val)
+
+    zm_write_phy_reg(79, 0x7f);
+    zm_write_phy_reg(77, 0x3f3f3f3f);
+    zm_write_phy_reg(78, 0x3f3f3f3f);
+    zm_write_phy_reg(653, 0x3f3f3f3f);
+    zm_write_phy_reg(654, 0x3f3f3f3f);
+    zm_write_phy_reg(739, 0x3f3f3f3f);
+    zm_write_phy_reg(740, 0x3f3f3f3f);
+    zm_write_phy_reg(755, 0x3f3f3f3f);
+    zm_write_phy_reg(756, 0x3f3f3f3f);
+    zm_write_phy_reg(757, 0x3f3f3f3f);
+
+#undef zm_write_phy_reg
+}
+
+
+
+void zfPrintTp(u8_t* pwr0, u8_t* vpd0, u8_t* pwr1, u8_t* vpd1)
+{
+    #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    DbgPrint("pwr0 : %d, %d, %d, %d ,%d\n", pwr0[0], pwr0[1], pwr0[2], pwr0[3], pwr0[4]);
+    DbgPrint("vpd0 : %d, %d, %d, %d ,%d\n", vpd0[0], vpd0[1], vpd0[2], vpd0[3], vpd0[4]);
+    DbgPrint("pwr1 : %d, %d, %d, %d ,%d\n", pwr1[0], pwr1[1], pwr1[2], pwr1[3], pwr1[4]);
+    DbgPrint("vpd1 : %d, %d, %d, %d ,%d\n", vpd1[0], vpd1[1], vpd1[2], vpd1[3], vpd1[4]);
+    #endif
+}
+
+
+/*
+ * To find CTL index(0~23)
+ * return 24(AR5416_NUM_CTLS)=>no desired index found
+ */
+u8_t zfFindCtlEdgesIndex(zdev_t* dev, u8_t desired_CtlIndex)
+{
+    u8_t i;
+    struct zsHpPriv* hpPriv;
+    struct ar5416Eeprom* eepromImage;
+
+    zmw_get_wlan_dev(dev);
+
+    hpPriv = wd->hpPrivate;
+
+    eepromImage = (struct ar5416Eeprom*)&(hpPriv->eepromImage[(1024+512)/4]);
+
+    //for (i = 0; (i < AR5416_NUM_CTLS) && eepromImage->ctlIndex[i]; i++)
+    for (i = 0; i < AR5416_NUM_CTLS; i++)
+    {
+        if(desired_CtlIndex == eepromImage->ctlIndex[i])
+            break;
+    }
+    return i;
+}
+
+/**************************************************************************
+ * fbin2freq
+ *
+ * Get channel value from binary representation held in eeprom
+ * RETURNS: the frequency in MHz
+ */
+u32_t
+fbin2freq(u8_t fbin, u8_t is2GHz)
+{
+    /*
+     * Reserved value 0xFF provides an empty definition both as
+     * an fbin and as a frequency - do not convert
+     */
+    if (fbin == AR5416_BCHAN_UNUSED) {
+        return fbin;
+    }
+
+    return (u32_t)((is2GHz==1) ? (2300 + fbin) : (4800 + 5 * fbin));
+}
+
+
+u8_t zfGetMaxEdgePower(zdev_t* dev, CAL_CTL_EDGES *pCtlEdges, u32_t freq)
+{
+    u8_t i;
+    u8_t maxEdgePower;
+    u8_t is2GHz;
+    struct zsHpPriv* hpPriv;
+    struct ar5416Eeprom* eepromImage;
+
+    zmw_get_wlan_dev(dev);
+
+    hpPriv = wd->hpPrivate;
+
+    eepromImage = (struct ar5416Eeprom*)&(hpPriv->eepromImage[(1024+512)/4]);
+
+    if(freq > ZM_CH_G_14)
+        is2GHz = 0;
+    else
+        is2GHz = 1;
+
+    maxEdgePower = AR5416_MAX_RATE_POWER;
+
+    /* Get the edge power */
+    for (i = 0; (i < AR5416_NUM_BAND_EDGES) && (pCtlEdges[i].bChannel != AR5416_BCHAN_UNUSED) ; i++)
+    {
+        /*
+         * If there's an exact channel match or an inband flag set
+         * on the lower channel use the given rdEdgePower
+         */
+        if (freq == fbin2freq(pCtlEdges[i].bChannel, is2GHz))
+        {
+            maxEdgePower = pCtlEdges[i].tPower;
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("zfGetMaxEdgePower index i = %d \n", i));
+            #endif
+            break;
+        }
+        else if ((i > 0) && (freq < fbin2freq(pCtlEdges[i].bChannel, is2GHz)))
+        {
+            if (fbin2freq(pCtlEdges[i - 1].bChannel, is2GHz) < freq && pCtlEdges[i - 1].flag)
+            {
+                maxEdgePower = pCtlEdges[i - 1].tPower;
+                #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+                zm_dbg(("zfGetMaxEdgePower index i-1 = %d \n", i-1));
+                #endif
+            }
+            /* Leave loop - no more affecting edges possible in this monotonic increasing list */
+            break;
+        }
+
+    }
+
+    if( i == AR5416_NUM_BAND_EDGES )
+    {
+        if (freq > fbin2freq(pCtlEdges[i - 1].bChannel, is2GHz) && pCtlEdges[i - 1].flag)
+        {
+            maxEdgePower = pCtlEdges[i - 1].tPower;
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("zfGetMaxEdgePower index=>i-1 = %d \n", i-1));
+            #endif
+        }
+    }
+
+    zm_assert(maxEdgePower > 0);
+
+  #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+    if ( maxEdgePower == AR5416_MAX_RATE_POWER )
+    {
+        zm_dbg(("zfGetMaxEdgePower = %d !!!\n", AR5416_MAX_RATE_POWER));
+    }
+  #endif
+    return maxEdgePower;
+}
+
+u32_t zfAdjustHT40FreqOffset(zdev_t* dev, u32_t frequency, u8_t bw40, u8_t extOffset)
+{
+    u32_t newFreq = frequency;
+
+	if (bw40 == 1)
+	{
+        if (extOffset == 1)
+        {
+            newFreq += 10;
+        }
+        else
+        {
+            newFreq -= 10;
+        }
+	}
+	return newFreq;
+}
+
+u32_t zfHpCheckDoHeavyClip(zdev_t* dev, u32_t freq, CAL_CTL_EDGES *pCtlEdges, u8_t bw40)
+{
+    u32_t ret = 0;
+    u8_t i;
+    u8_t is2GHz;
+    struct zsHpPriv* hpPriv;
+
+    zmw_get_wlan_dev(dev);
+
+    hpPriv = wd->hpPrivate;
+
+    if(freq > ZM_CH_G_14)
+        is2GHz = 0;
+    else
+        is2GHz = 1;
+
+    /* HT40 force enable heavy clip */
+    if (bw40)
+    {
+        ret |= 0xf0;
+    }
+#if 1
+    /* HT20 : frequency bandedge */
+    for (i = 0; (i < AR5416_NUM_BAND_EDGES) && (pCtlEdges[i].bChannel != AR5416_BCHAN_UNUSED) ; i++)
+    {
+        if (freq == fbin2freq(pCtlEdges[i].bChannel, is2GHz))
+        {
+            if (pCtlEdges[i].flag == 0)
+            {
+                ret |= 0xf;
+            }
+            break;
+        }
+    }
+#endif
+
+    return ret;
+}
+
+
+void zfSetPowerCalTable(zdev_t* dev, u32_t frequency, u8_t bw40, u8_t extOffset)
+{
+    struct ar5416Eeprom* eepromImage;
+    u8_t pwr0[5];
+    u8_t pwr1[5];
+    u8_t vpd0[5];
+    u8_t vpd1[5];
+    u8_t vpd_chain1[128];
+    u8_t vpd_chain3[128];
+    u16_t boundary1 = 18; //CR 667
+    u16_t powerTxMax = 63; //CR 79
+    u8_t i;
+    struct zsHpPriv* hpPriv;
+    u8_t fbin;
+    u8_t index, max2gIndex, max5gIndex;
+    u8_t chain0pwrPdg0[5];
+    u8_t chain0vpdPdg0[5];
+    u8_t chain0pwrPdg1[5];
+    u8_t chain0vpdPdg1[5];
+    u8_t chain2pwrPdg0[5];
+    u8_t chain2vpdPdg0[5];
+    u8_t chain2pwrPdg1[5];
+    u8_t chain2vpdPdg1[5];
+    u8_t fbinArray[8];
+
+    /* 4 CTL */
+    u8_t ctl_i;
+    u8_t desired_CtlIndex;
+
+    u8_t ctlEdgesMaxPowerCCK = AR5416_MAX_RATE_POWER;
+    u8_t ctlEdgesMaxPower2G = AR5416_MAX_RATE_POWER;
+    u8_t ctlEdgesMaxPower2GHT20 = AR5416_MAX_RATE_POWER;
+    u8_t ctlEdgesMaxPower2GHT40 = AR5416_MAX_RATE_POWER;
+    u8_t ctlEdgesMaxPower5G = AR5416_MAX_RATE_POWER;
+    u8_t ctlEdgesMaxPower5GHT20 = AR5416_MAX_RATE_POWER;
+    u8_t ctlEdgesMaxPower5GHT40 = AR5416_MAX_RATE_POWER;
+
+    u8_t ctlOffset;
+
+    zmw_get_wlan_dev(dev);
+
+    hpPriv = wd->hpPrivate;
+
+    eepromImage = (struct ar5416Eeprom*)&(hpPriv->eepromImage[(1024+512)/4]);
+
+    // Check the total bytes of the EEPROM structure to see the dongle have been calibrated or not.
+    if (eepromImage->baseEepHeader.length == 0xffff)
+    {
+        #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+        zm_dbg(("Warning! This dongle not been calibrated\n"));
+        #endif
+        return;
+    }
+
+    #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    DbgPrint("-----zfSetPowerCalTable : frequency=%d-----\n", frequency);
+    #endif
+    /* TODO : 1. boundary1 and powerTxMax should be refered to CR667 and CR79 */
+    /*           in otus.ini file                                          */
+
+    #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    /* 2. Interpolate pwr and vpd test points from frequency */
+    DbgPrint("calFreqPier5G : %d, %d, %d, %d ,%d, %d, %d, %d\n",
+                                            eepromImage->calFreqPier5G[0]*5+4800,
+                                            eepromImage->calFreqPier5G[1]*5+4800,
+                                            eepromImage->calFreqPier5G[2]*5+4800,
+                                            eepromImage->calFreqPier5G[3]*5+4800,
+                                            eepromImage->calFreqPier5G[4]*5+4800,
+                                            eepromImage->calFreqPier5G[5]*5+4800,
+                                            eepromImage->calFreqPier5G[6]*5+4800,
+                                            eepromImage->calFreqPier5G[7]*5+4800
+                                            );
+    DbgPrint("calFreqPier2G : %d, %d, %d, %d\n",
+                                            eepromImage->calFreqPier2G[0]+2300,
+                                            eepromImage->calFreqPier2G[1]+2300,
+                                            eepromImage->calFreqPier2G[2]+2300,
+                                            eepromImage->calFreqPier2G[3]+2300
+                                            );
+    #endif
+    if (frequency < 3000)
+    {
+        for (i=0; i<4; i++)
+        {
+            if (eepromImage->calFreqPier2G[i] == 0xff)
+            {
+                break;
+            }
+        }
+        max2gIndex = i;
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("max2gIndex : %d\n", max2gIndex);
+        #endif
+        fbin = (u8_t)(frequency - 2300);
+        index = zfFindFreqIndex(fbin, eepromImage->calFreqPier2G, max2gIndex);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("2G index : %d\n", index);
+        DbgPrint("chain 0 index\n");
+        #endif
+        zfPrintTp(&eepromImage->calPierData2G[0][index].pwrPdg[0][0],
+                  &eepromImage->calPierData2G[0][index].vpdPdg[0][0],
+                  &eepromImage->calPierData2G[0][index].pwrPdg[1][0],
+                  &eepromImage->calPierData2G[0][index].vpdPdg[1][0]
+                  );
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("chain 0 index+1\n");
+        #endif
+        zfPrintTp(&eepromImage->calPierData2G[0][index+1].pwrPdg[0][0],
+                  &eepromImage->calPierData2G[0][index+1].vpdPdg[0][0],
+                  &eepromImage->calPierData2G[0][index+1].pwrPdg[1][0],
+                  &eepromImage->calPierData2G[0][index+1].vpdPdg[1][0]
+                  );
+
+        for (i=0; i<5; i++)
+        {
+            chain0pwrPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[0][index].pwrPdg[0][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[0][index+1].pwrPdg[0][i]
+                    );
+            chain0vpdPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[0][index].vpdPdg[0][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[0][index+1].vpdPdg[0][i]
+                    );
+            chain0pwrPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[0][index].pwrPdg[1][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[0][index+1].pwrPdg[1][i]
+                    );
+            chain0vpdPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[0][index].vpdPdg[1][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[0][index+1].vpdPdg[1][i]
+                    );
+
+            chain2pwrPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[1][index].pwrPdg[0][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[1][index+1].pwrPdg[0][i]
+                    );
+            chain2vpdPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[1][index].vpdPdg[0][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[1][index+1].vpdPdg[0][i]
+                    );
+            chain2pwrPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[1][index].pwrPdg[1][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[1][index+1].pwrPdg[1][i]
+                    );
+            chain2vpdPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[1][index].vpdPdg[1][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[1][index+1].vpdPdg[1][i]
+                    );
+        }
+    }
+    else
+    {
+        for (i=0; i<8; i++)
+        {
+            if (eepromImage->calFreqPier5G[i] == 0xff)
+            {
+                break;
+            }
+        }
+        max5gIndex = i;
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("max5gIndex : %d\n", max5gIndex);
+        #endif
+        fbin = (u8_t)((frequency - 4800)/5);
+        index = zfFindFreqIndex(fbin, eepromImage->calFreqPier5G, max5gIndex);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("5G index : %d\n", index);
+        #endif
+
+        for (i=0; i<5; i++)
+        {
+            chain0pwrPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[0][index].pwrPdg[0][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[0][index+1].pwrPdg[0][i]
+                    );
+            chain0vpdPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[0][index].vpdPdg[0][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[0][index+1].vpdPdg[0][i]
+                    );
+            chain0pwrPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[0][index].pwrPdg[1][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[0][index+1].pwrPdg[1][i]
+                    );
+            chain0vpdPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[0][index].vpdPdg[1][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[0][index+1].vpdPdg[1][i]
+                    );
+
+            chain2pwrPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[1][index].pwrPdg[0][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[1][index+1].pwrPdg[0][i]
+                    );
+            chain2vpdPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[1][index].vpdPdg[0][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[1][index+1].vpdPdg[0][i]
+                    );
+            chain2pwrPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[1][index].pwrPdg[1][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[1][index+1].pwrPdg[1][i]
+                    );
+            chain2vpdPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[1][index].vpdPdg[1][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[1][index+1].vpdPdg[1][i]
+                    );
+        }
+
+    }
+
+
+    /* Chain 1 */
+    /* Get pwr and vpd test points from frequency */
+    for (i=0; i<5; i++)
+    {
+        pwr0[i] = chain0pwrPdg0[i]>>1;
+        vpd0[i] = chain0vpdPdg0[i];
+        pwr1[i] = chain0pwrPdg1[i]>>1;
+        vpd1[i] = chain0vpdPdg1[i];
+    }
+    #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    DbgPrint("Test Points\n");
+    DbgPrint("pwr0 : %d, %d, %d, %d ,%d\n", pwr0[0], pwr0[1], pwr0[2], pwr0[3], pwr0[4]);
+    DbgPrint("vpd0 : %d, %d, %d, %d ,%d\n", vpd0[0], vpd0[1], vpd0[2], vpd0[3], vpd0[4]);
+    DbgPrint("pwr1 : %d, %d, %d, %d ,%d\n", pwr1[0], pwr1[1], pwr1[2], pwr1[3], pwr1[4]);
+    DbgPrint("vpd1 : %d, %d, %d, %d ,%d\n", vpd1[0], vpd1[1], vpd1[2], vpd1[3], vpd1[4]);
+    #endif
+    /* Generate the vpd arrays */
+    for (i=0; i<boundary1+1+6; i++)
+    {
+        vpd_chain1[i] = zfGetInterpolatedValue(i, &pwr0[0], &vpd0[0]);
+    }
+    for (; i<powerTxMax+1+6+6; i++)
+    {
+        vpd_chain1[i] = zfGetInterpolatedValue(i-6-6, &pwr1[0], &vpd1[0]);
+    }
+    #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    DbgPrint("vpd_chain1\n");
+    for (i=0; i<powerTxMax+1+6+6; i+=10)
+    {
+        DbgPrint("%d, %d, %d, %d ,%d, %d, %d, %d, %d, %d\n",
+                vpd_chain1[i+0], vpd_chain1[i+1], vpd_chain1[i+2], vpd_chain1[i+3], vpd_chain1[i+4],
+                vpd_chain1[i+5], vpd_chain1[i+6], vpd_chain1[i+7], vpd_chain1[i+8], vpd_chain1[i+9]);
+    }
+    #endif
+    /* Write PHY regs 672-703 */
+    for (i=0; i<128; i+=4)
+    {
+        u32_t regAddr = 0x9800 + (672 * 4);
+        u32_t val;
+
+        val = ((u32_t)vpd_chain1[i+3]<<24) |
+                ((u32_t)vpd_chain1[i+2]<<16) |
+                ((u32_t)vpd_chain1[i+1]<<8) |
+                ((u32_t)vpd_chain1[i]);
+
+        #ifndef ZM_OTUS_LINUX_PHASE_2
+        reg_write(regAddr + i, val);  /* CR672 */
+        #endif
+    }
+
+    /* Chain 2 */
+    /* Get pwr and vpd test points from frequency */
+    for (i=0; i<5; i++)
+    {
+        pwr0[i] = chain2pwrPdg0[i]>>1;
+        vpd0[i] = chain2vpdPdg0[i];
+        pwr1[i] = chain2pwrPdg1[i]>>1;
+        vpd1[i] = chain2vpdPdg1[i];
+    }
+    #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    DbgPrint("Test Points\n");
+    DbgPrint("pwr0 : %d, %d, %d, %d ,%d\n", pwr0[0], pwr0[1], pwr0[2], pwr0[3], pwr0[4]);
+    DbgPrint("vpd0 : %d, %d, %d, %d ,%d\n", vpd0[0], vpd0[1], vpd0[2], vpd0[3], vpd0[4]);
+    DbgPrint("pwr1 : %d, %d, %d, %d ,%d\n", pwr1[0], pwr1[1], pwr1[2], pwr1[3], pwr1[4]);
+    DbgPrint("vpd1 : %d, %d, %d, %d ,%d\n", vpd1[0], vpd1[1], vpd1[2], vpd1[3], vpd1[4]);
+    #endif
+    /* Generate the vpd arrays */
+    for (i=0; i<boundary1+1+6; i++)
+    {
+        vpd_chain3[i] = zfGetInterpolatedValue(i, &pwr0[0], &vpd0[0]);
+    }
+    for (; i<powerTxMax+1+6+6; i++)
+    {
+        vpd_chain3[i] = zfGetInterpolatedValue(i-6-6, &pwr1[0], &vpd1[0]);
+    }
+    #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    DbgPrint("vpd_chain3\n");
+    for (i=0; i<powerTxMax+1+6+6; i+=10)
+    {
+        DbgPrint("%d, %d, %d, %d ,%d, %d, %d, %d, %d, %d\n",
+                vpd_chain3[i+0], vpd_chain3[i+1], vpd_chain3[i+2], vpd_chain3[i+3], vpd_chain3[i+4],
+                vpd_chain3[i+5], vpd_chain3[i+6], vpd_chain3[i+7], vpd_chain3[i+8], vpd_chain3[i+9]);
+    }
+    #endif
+
+    /* Write PHY regs 672-703 + 0x1000 */
+    for (i=0; i<128; i+=4)
+    {
+        u32_t regAddr = 0x9800 + (672 * 4) + 0x1000;
+        u32_t val;
+
+        val = ((u32_t)vpd_chain3[i+3]<<24) |
+                ((u32_t)vpd_chain3[i+2]<<16) |
+                ((u32_t)vpd_chain3[i+1]<<8) |
+                ((u32_t)vpd_chain3[i]);
+
+        #ifndef ZM_OTUS_LINUX_PHASE_2
+        reg_write(regAddr + i, val);  /* CR672 */
+        #endif
+    }
+
+    zfFlushDelayWrite(dev);
+
+    /* 3. Generate target power table */
+    if (frequency < 3000)
+    {
+        for (i=0; i<3; i++)
+        {
+            if (eepromImage->calTargetPowerCck[i].bChannel != 0xff)
+            {
+                fbinArray[i] = eepromImage->calTargetPowerCck[i].bChannel;
+            }
+            else
+            {
+                break;
+            }
+
+        }
+        index = zfFindFreqIndex(fbin, fbinArray, i);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("CCK index=%d\n", index);
+        #endif
+        for (i=0; i<4; i++)
+        {
+            hpPriv->tPow2xCck[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calTargetPowerCck[index].bChannel,
+                    eepromImage->calTargetPowerCck[index].tPow2x[i],
+                    eepromImage->calTargetPowerCck[index+1].bChannel,
+                    eepromImage->calTargetPowerCck[index+1].tPow2x[i]
+                    );
+        }
+
+        for (i=0; i<4; i++)
+        {
+            if (eepromImage->calTargetPower2G[i].bChannel != 0xff)
+            {
+                fbinArray[i] = eepromImage->calTargetPower2G[i].bChannel;
+            }
+            else
+            {
+                break;
+            }
+
+        }
+        index = zfFindFreqIndex(fbin, fbinArray, i);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("2G index=%d\n", index);
+        #endif
+        for (i=0; i<4; i++)
+        {
+            hpPriv->tPow2x2g[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calTargetPower2G[index].bChannel,
+                    eepromImage->calTargetPower2G[index].tPow2x[i],
+                    eepromImage->calTargetPower2G[index+1].bChannel,
+                    eepromImage->calTargetPower2G[index+1].tPow2x[i]
+                    );
+        }
+
+        for (i=0; i<4; i++)
+        {
+            if (eepromImage->calTargetPower2GHT20[i].bChannel != 0xff)
+            {
+                fbinArray[i] = eepromImage->calTargetPower2GHT20[i].bChannel;
+            }
+            else
+            {
+                break;
+            }
+
+        }
+        index = zfFindFreqIndex(fbin, fbinArray, i);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("2G HT20 index=%d\n", index);
+        #endif
+        for (i=0; i<8; i++)
+        {
+            hpPriv->tPow2x2gHt20[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calTargetPower2GHT20[index].bChannel,
+                    eepromImage->calTargetPower2GHT20[index].tPow2x[i],
+                    eepromImage->calTargetPower2GHT20[index+1].bChannel,
+                    eepromImage->calTargetPower2GHT20[index+1].tPow2x[i]
+                    );
+        }
+
+        for (i=0; i<4; i++)
+        {
+            if (eepromImage->calTargetPower2GHT40[i].bChannel != 0xff)
+            {
+                fbinArray[i] = eepromImage->calTargetPower2GHT40[i].bChannel;
+            }
+            else
+            {
+                break;
+            }
+
+        }
+        index = zfFindFreqIndex( (u8_t)zfAdjustHT40FreqOffset(dev, fbin, bw40, extOffset), fbinArray, i);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("2G HT40 index=%d\n", index);
+        #endif
+        for (i=0; i<8; i++)
+        {
+            hpPriv->tPow2x2gHt40[i] = zfInterpolateFuncX(
+                    (u8_t)zfAdjustHT40FreqOffset(dev, fbin, bw40, extOffset),
+                    eepromImage->calTargetPower2GHT40[index].bChannel,
+                    eepromImage->calTargetPower2GHT40[index].tPow2x[i],
+                    eepromImage->calTargetPower2GHT40[index+1].bChannel,
+                    eepromImage->calTargetPower2GHT40[index+1].tPow2x[i]
+                    );
+        }
+
+        zfPrintTargetPower2G(hpPriv->tPow2xCck,
+                hpPriv->tPow2x2g,
+                hpPriv->tPow2x2gHt20,
+                hpPriv->tPow2x2gHt40);
+    }
+    else
+    {
+        /* 5G */
+        for (i=0; i<8; i++)
+        {
+            if (eepromImage->calTargetPower5G[i].bChannel != 0xff)
+            {
+                fbinArray[i] = eepromImage->calTargetPower5G[i].bChannel;
+            }
+            else
+            {
+                break;
+            }
+
+        }
+        index = zfFindFreqIndex(fbin, fbinArray, i);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("5G index=%d\n", index);
+        #endif
+        for (i=0; i<4; i++)
+        {
+            hpPriv->tPow2x5g[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calTargetPower5G[index].bChannel,
+                    eepromImage->calTargetPower5G[index].tPow2x[i],
+                    eepromImage->calTargetPower5G[index+1].bChannel,
+                    eepromImage->calTargetPower5G[index+1].tPow2x[i]
+                    );
+        }
+
+        for (i=0; i<8; i++)
+        {
+            if (eepromImage->calTargetPower5GHT20[i].bChannel != 0xff)
+            {
+                fbinArray[i] = eepromImage->calTargetPower5GHT20[i].bChannel;
+            }
+            else
+            {
+                break;
+            }
+
+        }
+        index = zfFindFreqIndex(fbin, fbinArray, i);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("5G HT20 index=%d\n", index);
+        #endif
+        for (i=0; i<8; i++)
+        {
+            hpPriv->tPow2x5gHt20[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calTargetPower5GHT20[index].bChannel,
+                    eepromImage->calTargetPower5GHT20[index].tPow2x[i],
+                    eepromImage->calTargetPower5GHT20[index+1].bChannel,
+                    eepromImage->calTargetPower5GHT20[index+1].tPow2x[i]
+                    );
+        }
+
+        for (i=0; i<8; i++)
+        {
+            if (eepromImage->calTargetPower5GHT40[i].bChannel != 0xff)
+            {
+                fbinArray[i] = eepromImage->calTargetPower5GHT40[i].bChannel;
+            }
+            else
+            {
+                break;
+            }
+
+        }
+        index = zfFindFreqIndex((u8_t)zfAdjustHT40FreqOffset(dev, fbin, bw40, extOffset), fbinArray, i);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("5G HT40 index=%d\n", index);
+        #endif
+        for (i=0; i<8; i++)
+        {
+            hpPriv->tPow2x5gHt40[i] = zfInterpolateFuncX(
+                    (u8_t)zfAdjustHT40FreqOffset(dev, fbin, bw40, extOffset),
+                    eepromImage->calTargetPower5GHT40[index].bChannel,
+                    eepromImage->calTargetPower5GHT40[index].tPow2x[i],
+                    eepromImage->calTargetPower5GHT40[index+1].bChannel,
+                    eepromImage->calTargetPower5GHT40[index+1].tPow2x[i]
+                    );
+        }
+
+        zfPrintTargetPower5G(
+                hpPriv->tPow2x5g,
+                hpPriv->tPow2x5gHt20,
+                hpPriv->tPow2x5gHt40);
+    }
+
+
+
+    /* 4. CTL */
+    /*
+     * 4.1 Get the bandedges tx power by frequency
+     *      2.4G we get ctlEdgesMaxPowerCCK
+     *                  ctlEdgesMaxPower2G
+     *                  ctlEdgesMaxPower2GHT20
+     *                  ctlEdgesMaxPower2GHT40
+     *      5G we get   ctlEdgesMaxPower5G
+     *                  ctlEdgesMaxPower5GHT20
+     *                  ctlEdgesMaxPower5GHT40
+     * 4.2 Update (3.) target power table by 4.1
+     * 4.3 Tx power offset for ART - NDIS/MDK
+     * 4.4 Write MAC reg 0x694 for ACK's TPC
+     *
+     */
+
+    //zfDumpEepBandEdges(eepromImage);
+
+    /* get the cfg from Eeprom: regionCode => RegulatoryDomain : 0x10-FFC  0x30-eu 0x40-jap */
+    desired_CtlIndex = zfHpGetRegulatoryDomain(dev);
+    if ((desired_CtlIndex == 0x30) || (desired_CtlIndex == 0x40) || (desired_CtlIndex == 0x0))
+    {
+        /* skip CTL and heavy clip */
+        hpPriv->enableBBHeavyClip = 0;
+        #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+        zm_dbg(("RegulatoryDomain = 0, skip CTL and heavy clip\n"));
+        #endif
+    }
+    else
+    {
+        hpPriv->enableBBHeavyClip = 1;
+
+        if (desired_CtlIndex == 0xff)
+        {
+            /* desired index not found */
+            desired_CtlIndex = 0x10;
+        }
+
+        /* first part : 2.4G */
+        if (frequency <= ZM_CH_G_14)
+        {
+            /* 2.4G - CTL_11B */
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_11B);
+            if(ctl_i<AR5416_NUM_CTLS)
+            {
+                ctlEdgesMaxPowerCCK = zfGetMaxEdgePower(dev, eepromImage->ctlData[ctl_i].ctlEdges[1], frequency);
+            }
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("CTL_11B ctl_i = %d\n", ctl_i));
+            #endif
+
+            /* 2.4G - CTL_11G */
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_11G);
+            if(ctl_i<AR5416_NUM_CTLS)
+            {
+                ctlEdgesMaxPower2G = zfGetMaxEdgePower(dev, eepromImage->ctlData[ctl_i].ctlEdges[1], frequency);
+            }
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("CTL_11G ctl_i = %d\n", ctl_i));
+            #endif
+
+            /* 2.4G - CTL_2GHT20 */
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_2GHT20);
+            if(ctl_i<AR5416_NUM_CTLS)
+            {
+                ctlEdgesMaxPower2GHT20 = zfGetMaxEdgePower(dev, eepromImage->ctlData[ctl_i].ctlEdges[1], frequency);
+            }
+            else
+            {
+                /* workaround for no data in Eeprom, replace by normal 2G */
+                ctlEdgesMaxPower2GHT20 = ctlEdgesMaxPower2G;
+            }
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("CTL_2GHT20 ctl_i = %d\n", ctl_i));
+            #endif
+
+            /* 2.4G - CTL_2GHT40 */
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_2GHT40);
+            if(ctl_i<AR5416_NUM_CTLS)
+            {
+                ctlEdgesMaxPower2GHT40 = zfGetMaxEdgePower(dev, eepromImage->ctlData[ctl_i].ctlEdges[1],
+                                                                zfAdjustHT40FreqOffset(dev, frequency, bw40, extOffset));
+            }
+            else
+            {
+                /* workaround for no data in Eeprom, replace by normal 2G */
+                ctlEdgesMaxPower2GHT40 = ctlEdgesMaxPower2G;
+            }
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("CTL_2GHT40 ctl_i = %d\n", ctl_i));
+            #endif
+
+
+            /* 7a17 :  */
+            /* Max power (dBm) for channel range when using DFS define by madwifi*/
+            for (i=0; i<wd->regulationTable.allowChannelCnt; i++)
+            {
+                if (wd->regulationTable.allowChannel[i].channel == frequency)
+                {
+                    if (zfHpIsDfsChannel(dev, (u16_t)frequency))
+                    {
+                        zm_debug_msg1("frequency use DFS  -- ", frequency);
+                        ctlEdgesMaxPowerCCK     = zm_min(ctlEdgesMaxPowerCCK, wd->regulationTable.allowChannel[i].maxRegTxPower*2);
+                        ctlEdgesMaxPower2G      = zm_min(ctlEdgesMaxPower2G, wd->regulationTable.allowChannel[i].maxRegTxPower*2);
+                        ctlEdgesMaxPower2GHT20  = zm_min(ctlEdgesMaxPower2GHT20, wd->regulationTable.allowChannel[i].maxRegTxPower*2);
+                        ctlEdgesMaxPower2GHT40  = zm_min(ctlEdgesMaxPower2GHT40, wd->regulationTable.allowChannel[i].maxRegTxPower*2);
+                    }
+                    break;
+                }
+            }
+
+            /* Apply ctl mode to correct target power set */
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_debug_msg1("ctlEdgesMaxPowerCCK    = ", ctlEdgesMaxPowerCCK);
+            zm_debug_msg1("ctlEdgesMaxPower2G     = ", ctlEdgesMaxPower2G);
+            zm_debug_msg1("ctlEdgesMaxPower2GHT20 = ", ctlEdgesMaxPower2GHT20);
+            zm_debug_msg1("ctlEdgesMaxPower2GHT40 = ", ctlEdgesMaxPower2GHT40);
+            #endif
+            for (i=0; i<4; i++)
+            {
+                hpPriv->tPow2xCck[i] = zm_min(hpPriv->tPow2xCck[i], ctlEdgesMaxPowerCCK) + HALTX_POWER_OFFSET;
+            }
+            hpPriv->tPow2x2g24HeavyClipOffset = 0;
+            if (hpPriv->enableBBHeavyClip)
+            {
+                ctlOffset = 2;
+            }
+            else
+            {
+                ctlOffset = 0;
+            }
+            for (i=0; i<4; i++)
+            {
+                if (((frequency == 2412) || (frequency == 2462)))
+                {
+                    if (i != 0)
+                    {
+                        hpPriv->tPow2x2g[i] = zm_min(hpPriv->tPow2x2g[i], ctlEdgesMaxPower2G-ctlOffset) + HALTX_POWER_OFFSET;
+                    }
+                    else
+                    {
+                        hpPriv->tPow2x2g[i] = zm_min(hpPriv->tPow2x2g[i], ctlEdgesMaxPower2G) + HALTX_POWER_OFFSET;
+                        if (hpPriv->tPow2x2g[i] > (ctlEdgesMaxPower2G-ctlOffset))
+                        {
+                            hpPriv->tPow2x2g24HeavyClipOffset = hpPriv->tPow2x2g[i] - (ctlEdgesMaxPower2G-ctlOffset);
+                        }
+                    }
+                }
+                else
+                {
+                    hpPriv->tPow2x2g[i] = zm_min(hpPriv->tPow2x2g[i], ctlEdgesMaxPower2G) + HALTX_POWER_OFFSET;
+                }
+            }
+            for (i=0; i<8; i++)
+            {
+                if (((frequency == 2412) || (frequency == 2462)) && (i>=3))
+                {
+                    hpPriv->tPow2x2gHt20[i] = zm_min(hpPriv->tPow2x2gHt20[i], ctlEdgesMaxPower2GHT20-ctlOffset) + HALTX_POWER_OFFSET;
+                }
+                else
+                {
+                    hpPriv->tPow2x2gHt20[i] = zm_min(hpPriv->tPow2x2gHt20[i], ctlEdgesMaxPower2GHT20) + HALTX_POWER_OFFSET;
+                }
+            }
+            for (i=0; i<8; i++)
+            {
+                if ((frequency == 2412) && (i>=3))
+                {
+                    hpPriv->tPow2x2gHt40[i] = zm_min(hpPriv->tPow2x2gHt40[i], ctlEdgesMaxPower2GHT40-ctlOffset) + HALTX_POWER_OFFSET;
+                }
+                else if ((frequency == 2462) && (i>=3))
+                {
+                    hpPriv->tPow2x2gHt40[i] = zm_min(hpPriv->tPow2x2gHt40[i], ctlEdgesMaxPower2GHT40-(ctlOffset*2)) + HALTX_POWER_OFFSET;
+                }
+                else
+                {
+                    hpPriv->tPow2x2gHt40[i] = zm_min(hpPriv->tPow2x2gHt40[i], ctlEdgesMaxPower2GHT40) + HALTX_POWER_OFFSET;
+                }
+            }
+        }
+        else
+        {
+            /* 5G - CTL_11A */
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_11A);
+            if(ctl_i<AR5416_NUM_CTLS)
+            {
+                ctlEdgesMaxPower5G = zfGetMaxEdgePower(dev, eepromImage->ctlData[ctl_i].ctlEdges[1], frequency);
+            }
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("CTL_11A ctl_i = %d\n", ctl_i));
+            #endif
+
+            /* 5G - CTL_5GHT20 */
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_5GHT20);
+            if(ctl_i<AR5416_NUM_CTLS)
+            {
+                ctlEdgesMaxPower5GHT20 = zfGetMaxEdgePower(dev, eepromImage->ctlData[ctl_i].ctlEdges[1], frequency);
+            }
+            else
+            {
+                /* workaround for no data in Eeprom, replace by normal 5G */
+                ctlEdgesMaxPower5GHT20 = ctlEdgesMaxPower5G;
+            }
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("CTL_5GHT20 ctl_i = %d\n", ctl_i));
+            #endif
+
+            /* 5G - CTL_5GHT40 */
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_5GHT40);
+            if(ctl_i<AR5416_NUM_CTLS)
+            {
+                ctlEdgesMaxPower5GHT40 = zfGetMaxEdgePower(dev, eepromImage->ctlData[ctl_i].ctlEdges[1],
+                                                                zfAdjustHT40FreqOffset(dev, frequency, bw40, extOffset));
+            }
+            else
+            {
+                /* workaround for no data in Eeprom, replace by normal 5G */
+                ctlEdgesMaxPower5GHT40 = ctlEdgesMaxPower5G;
+            }
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("CTL_5GHT40 ctl_i = %d\n", ctl_i));
+            #endif
+
+            /* 7a17 :  */
+            /* Max power (dBm) for channel range when using DFS define by madwifi*/
+            for (i=0; i<wd->regulationTable.allowChannelCnt; i++)
+            {
+                if (wd->regulationTable.allowChannel[i].channel == frequency)
+                {
+                    if (zfHpIsDfsChannel(dev, (u16_t)frequency))
+                    {
+                        zm_debug_msg1("frequency use DFS  -- ", frequency);
+                        ctlEdgesMaxPower5G      = zm_min(ctlEdgesMaxPower5G, wd->regulationTable.allowChannel[i].maxRegTxPower*2);
+                        ctlEdgesMaxPower5GHT20  = zm_min(ctlEdgesMaxPower5GHT20, wd->regulationTable.allowChannel[i].maxRegTxPower*2);
+                        ctlEdgesMaxPower5GHT40  = zm_min(ctlEdgesMaxPower5GHT40, wd->regulationTable.allowChannel[i].maxRegTxPower*2);
+                    }
+                    break;
+                }
+            }
+
+
+            /* Apply ctl mode to correct target power set */
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_debug_msg1("ctlEdgesMaxPower5G     = ", ctlEdgesMaxPower5G);
+            zm_debug_msg1("ctlEdgesMaxPower5GHT20 = ", ctlEdgesMaxPower5GHT20);
+            zm_debug_msg1("ctlEdgesMaxPower5GHT40 = ", ctlEdgesMaxPower5GHT40);
+            #endif
+            for (i=0; i<4; i++)
+            {
+                hpPriv->tPow2x5g[i] = zm_min(hpPriv->tPow2x5g[i], ctlEdgesMaxPower5G) + HALTX_POWER_OFFSET;
+            }
+            for (i=0; i<8; i++)
+            {
+                hpPriv->tPow2x5gHt20[i] = zm_min(hpPriv->tPow2x5gHt20[i], ctlEdgesMaxPower5GHT20) + HALTX_POWER_OFFSET;
+            }
+            for (i=0; i<8; i++)
+            {
+                hpPriv->tPow2x5gHt40[i] = zm_min(hpPriv->tPow2x5gHt40[i], ctlEdgesMaxPower5GHT40) + HALTX_POWER_OFFSET;
+            }
+
+        }/* end of bandedges of 5G */
+    }/* end of  if ((desired_CtlIndex = zfHpGetRegulatoryDomain(dev)) == 0) */
+
+    /* workaround */
+    /* 5. BB heavy clip */
+    /*    only 2.4G do heavy clip */
+    if (hpPriv->enableBBHeavyClip && hpPriv->hwBBHeavyClip && (frequency <= ZM_CH_G_14))
+    {
+        if (frequency <= ZM_CH_G_14)
+        {
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_11G);
+        }
+        else
+        {
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_11A);
+        }
+
+        hpPriv->setValueHeavyClip = zfHpCheckDoHeavyClip(dev, frequency, eepromImage->ctlData[ctl_i].ctlEdges[1], bw40);
+
+        if (hpPriv->setValueHeavyClip)
+        {
+            hpPriv->doBBHeavyClip = 1;
+        }
+        else
+        {
+            hpPriv->doBBHeavyClip = 0;
+        }
+        #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+        zm_dbg(("zfHpCheckDoHeavyClip ret = %02x, doBBHeavyClip = %d\n",
+                 hpPriv->setValueHeavyClip, hpPriv->doBBHeavyClip));
+        #endif
+
+        if (hpPriv->doBBHeavyClip)
+        {
+            if (hpPriv->setValueHeavyClip & 0xf0)
+            {
+                hpPriv->tPow2x2gHt40[0] -= 1;
+                hpPriv->tPow2x2gHt40[1] -= 1;
+                hpPriv->tPow2x2gHt40[2] -= 1;
+            }
+
+            if (hpPriv->setValueHeavyClip & 0xf)
+            {
+                hpPriv->tPow2x2gHt20[0] += 1;
+                hpPriv->tPow2x2gHt20[1] += 1;
+                hpPriv->tPow2x2gHt20[2] += 1;
+            }
+        }
+    }
+    else
+    {
+        hpPriv->doBBHeavyClip = 0;
+        hpPriv->setValueHeavyClip = 0;
+    }
+
+    /* Final : write MAC register for some ctrl frame Tx power */
+    /* first part : 2.4G */
+    if (frequency <= ZM_CH_G_14)
+    {
+        /* Write MAC reg 0x694 for ACK's TPC */
+        /* Write MAC reg 0xbb4 RTS and SF-CTS frame power control */
+        /* Always use two stream for low legacy rate */
+        #if 0
+        //if (hpPriv->halCapability & ZM_HP_CAP_11N_ONE_TX_STREAM)
+        //{
+            zfDelayWriteInternalReg(dev, 0x1c3694, ((hpPriv->tPow2x2g[0]&0x3f) << 20) | (0x1<<26));
+            zfDelayWriteInternalReg(dev, 0x1c3bb4, ((hpPriv->tPow2x2g[0]&0x3f) << 5 ) | (0x1<<11) |
+                                                   ((hpPriv->tPow2x2g[0]&0x3f) << 21) | (0x1<<27)  );
+        //}
+        #endif
+        #if 1
+        //else
+        {
+            #ifndef ZM_OTUS_LINUX_PHASE_2
+            zfDelayWriteInternalReg(dev, 0x1c3694, ((hpPriv->tPow2x2g[0]&0x3f) << 20) | (0x5<<26));
+            zfDelayWriteInternalReg(dev, 0x1c3bb4, ((hpPriv->tPow2x2g[0]&0x3f) << 5 ) | (0x5<<11) |
+                                                   ((hpPriv->tPow2x2g[0]&0x3f) << 21) | (0x5<<27)  );
+            #endif
+            hpPriv->currentAckRtsTpc = hpPriv->tPow2x2g[0];
+    	}
+        #endif
+        zfFlushDelayWrite(dev);
+
+        zfPrintTargetPower2G(hpPriv->tPow2xCck,
+                hpPriv->tPow2x2g,
+                hpPriv->tPow2x2gHt20,
+                hpPriv->tPow2x2gHt40);
+    }
+    else
+    {
+        /* Write MAC reg 0x694 for ACK's TPC */
+        /* Write MAC reg 0xbb4 RTS and SF-CTS frame power control */
+        /* Always use two stream for low legacy rate */
+        if (hpPriv->halCapability & ZM_HP_CAP_11N_ONE_TX_STREAM)
+        {
+            #ifndef ZM_OTUS_LINUX_PHASE_2
+            zfDelayWriteInternalReg(dev, 0x1c3694, ((hpPriv->tPow2x5g[0]&0x3f) << 20) | (0x1<<26));
+            zfDelayWriteInternalReg(dev, 0x1c3bb4, ((hpPriv->tPow2x5g[0]&0x3f) << 5 ) | (0x1<<11) |
+                                                   ((hpPriv->tPow2x5g[0]&0x3f) << 21) | (0x1<<27)  );
+            #endif
+        }
+        else
+        {
+            #ifndef ZM_OTUS_LINUX_PHASE_2
+            zfDelayWriteInternalReg(dev, 0x1c3694, ((hpPriv->tPow2x5g[0]&0x3f) << 20) | (0x5<<26));
+            zfDelayWriteInternalReg(dev, 0x1c3bb4, ((hpPriv->tPow2x5g[0]&0x3f) << 5 ) | (0x5<<11) |
+                                                   ((hpPriv->tPow2x5g[0]&0x3f) << 21) | (0x5<<27)  );
+            #endif
+            hpPriv->currentAckRtsTpc = hpPriv->tPow2x2g[0];
+        }
+
+
+        zfFlushDelayWrite(dev);
+
+        zfPrintTargetPower5G(
+                hpPriv->tPow2x5g,
+                hpPriv->tPow2x5gHt20,
+                hpPriv->tPow2x5gHt40);
+    }/* end of bandedges of 5G */
+
+}
+
+void zfDumpEepBandEdges(struct ar5416Eeprom* eepromImage)
+{
+    #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+    u8_t i, j, k;
+
+#if 0
+    zm_dbg(("\n === BandEdges index dump ==== \n"));
+
+    for (i = 0; i < AR5416_NUM_CTLS; i++)
+    {
+        zm_dbg(("%02x ", eepromImage->ctlIndex[i]));
+    }
+
+    zm_dbg(("\n === BandEdges data dump ==== \n"));
+
+    for (i = 0; i < AR5416_NUM_CTLS; i++)
+    {
+        for (j = 0; j < 2; j++)
+        {
+            for(k = 0; k < AR5416_NUM_BAND_EDGES; k++)
+            {
+                u8_t *pdata = (u8_t*)&(eepromImage->ctlData[i].ctlEdges[j][k]);
+                zm_dbg(("(%02x %02x)", pdata[0], pdata[1]));
+            }
+            zm_dbg(("\n"));
+        }
+    }
+#else
+    zm_dbg(("\n === BandEdges index dump ==== \n"));
+    for (i = 0; i < 24; i+=8)
+    {
+        zm_dbg(("%02x %02x %02x %02x %02x %02x %02x %02x",
+               eepromImage->ctlIndex[i+0], eepromImage->ctlIndex[i+1], eepromImage->ctlIndex[i+2], eepromImage->ctlIndex[i+3],
+               eepromImage->ctlIndex[i+4], eepromImage->ctlIndex[i+5], eepromImage->ctlIndex[i+6], eepromImage->ctlIndex[i+7]
+               ));
+    }
+
+    zm_dbg(("\n === BandEdges data dump ==== \n"));
+
+    for (i = 0; i < AR5416_NUM_CTLS; i++)
+    {
+        for (j = 0; j < 2; j++)
+        {
+            u8_t *pdata = (u8_t*)&(eepromImage->ctlData[i].ctlEdges[j]);
+            zm_dbg(("(%03d %02x) (%03d %02x) (%03d %02x) (%03d %02x) \n",
+                   pdata[0], pdata[1], pdata[2], pdata[3],
+                   pdata[4], pdata[5], pdata[6], pdata[7]
+                   ));
+            zm_dbg(("(%03d %02x) (%03d %02x) (%03d %02x) (%03d %02x) \n",
+                   pdata[8], pdata[9], pdata[10], pdata[11],
+                   pdata[12], pdata[13], pdata[14], pdata[15]
+                   ));
+        }
+    }
+#endif
+    #endif
+}
+
+void zfPrintTargetPower2G(u8_t* tPow2xCck, u8_t* tPow2x2g, u8_t* tPow2x2gHt20, u8_t* tPow2x2gHt40)
+{
+    //#ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+    DbgPrint("targetPwr CCK : %d, %d, %d, %d\n",
+            tPow2xCck[0],
+            tPow2xCck[1],
+            tPow2xCck[2],
+            tPow2xCck[3]
+            );
+    DbgPrint("targetPwr 2G : %d, %d, %d, %d\n",
+            tPow2x2g[0],
+            tPow2x2g[1],
+            tPow2x2g[2],
+            tPow2x2g[3]
+            );
+    DbgPrint("targetPwr 2GHT20 : %d, %d, %d, %d, %d, %d, %d, %d\n",
+            tPow2x2gHt20[0],
+            tPow2x2gHt20[1],
+            tPow2x2gHt20[2],
+            tPow2x2gHt20[3],
+            tPow2x2gHt20[4],
+            tPow2x2gHt20[5],
+            tPow2x2gHt20[6],
+            tPow2x2gHt20[7]
+            );
+    DbgPrint("targetPwr 2GHT40 : %d, %d, %d, %d, %d, %d, %d, %d\n",
+            tPow2x2gHt40[0],
+            tPow2x2gHt40[1],
+            tPow2x2gHt40[2],
+            tPow2x2gHt40[3],
+            tPow2x2gHt40[4],
+            tPow2x2gHt40[5],
+            tPow2x2gHt40[6],
+            tPow2x2gHt40[7]
+            );
+    #endif
+    return;
+}
+
+void zfPrintTargetPower5G(u8_t* tPow2x5g, u8_t* tPow2x5gHt20, u8_t* tPow2x5gHt40)
+{
+    //#ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+    DbgPrint("targetPwr 5G : %d, %d, %d, %d\n",
+            tPow2x5g[0],
+            tPow2x5g[1],
+            tPow2x5g[2],
+            tPow2x5g[3]
+            );
+    DbgPrint("targetPwr 5GHT20 : %d, %d, %d, %d, %d, %d, %d, %d\n",
+            tPow2x5gHt20[0],
+            tPow2x5gHt20[1],
+            tPow2x5gHt20[2],
+            tPow2x5gHt20[3],
+            tPow2x5gHt20[4],
+            tPow2x5gHt20[5],
+            tPow2x5gHt20[6],
+            tPow2x5gHt20[7]
+            );
+    DbgPrint("targetPwr 5GHT40 : %d, %d, %d, %d, %d, %d, %d, %d\n",
+            tPow2x5gHt40[0],
+            tPow2x5gHt40[1],
+            tPow2x5gHt40[2],
+            tPow2x5gHt40[3],
+            tPow2x5gHt40[4],
+            tPow2x5gHt40[5],
+            tPow2x5gHt40[6],
+            tPow2x5gHt40[7]
+            );
+    #endif
+    return;
+}
+
+void zfHpPowerSaveSetMode(zdev_t* dev, u8_t staMode, u8_t psMode, u16_t bcnInterval)
+{
+    if ( staMode == 0 )
+    {
+        if ( psMode == 0 )
+        {
+            // Turn off pre-TBTT interrupt
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_PRETBTT, 0);
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PERIOD, 0);
+            zfFlushDelayWrite(dev);
+        }
+        else
+        {
+            // Turn on pre-TBTT interrupt
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_PRETBTT, (bcnInterval-6)<<16);
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PERIOD, bcnInterval);
+            zfFlushDelayWrite(dev);
+        }
+    }
+}
+
+void zfHpPowerSaveSetState(zdev_t* dev, u8_t psState)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+
+	//DbgPrint("INTO zfHpPowerSaveSetState");
+
+    if ( psState == 0 ) //power up
+    {
+        //DbgPrint("zfHpPowerSaveSetState Wake up from PS\n");
+        reg_write(0x982C, 0x0000a000); //wake up ADDAC
+        reg_write(0x9808, 0x0);        //enable all agc gain and offset updates to a2
+        //# bank 3
+        if (((struct zsHpPriv*)wd->hpPrivate)->hwFrequency <= ZM_CH_G_14)
+        {
+            /* 11g */
+            //reg_write (0x98f0,  0x01c00018);
+            reg_write (0x98f0,  0x01c20098);//syn_on+RX_ON
+        }
+        else
+        {
+            /* 11a */
+            //reg_write (0x98f0,  0x01400018);
+            reg_write (0x98f0,  0x01420098);//syn_on+RX_ON
+        }
+
+        ////#bank 5
+        //reg_write(0x98b0,  0x00000013);
+        //reg_write(0x98e4,  0x00000002);
+
+
+        zfFlushDelayWrite(dev);
+    }
+    else //power down
+    {
+        //DbgPrint("zfHpPowerSaveSetState Go to PS\n");
+        //reg_write(0x982C, 0xa000a000);
+        reg_write(0x9808, 0x8000000);    //disable all agc gain and offset updates to a2
+        reg_write(0x982C, 0xa000a000);   //power down ADDAC
+        //# bank 3
+        if (((struct zsHpPriv*)wd->hpPrivate)->hwFrequency <= ZM_CH_G_14)
+        {
+            /* 11g */
+            reg_write (0x98f0,  0x00c00018);//syn_off+RX_off
+        }
+        else
+        {
+            /* 11a */
+            reg_write (0x98f0,  0x00400018);//syn_off+RX_off
+        }
+
+        ////#bank 5
+        //reg_write(0x98b0,  0x000e0013);
+        //reg_write(0x98e4,  0x00018002);
+
+
+        zfFlushDelayWrite(dev);
+    }
+}
+
+void zfHpSetAggPktNum(zdev_t* dev, u32_t num)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+
+    num = (num << 16) | (0xa);
+
+    hpPriv->aggPktNum = num;
+
+    //aggregation number will be update in HAL heart beat
+    //zfDelayWriteInternalReg(dev, 0x1c3b9c, num);
+    //zfFlushDelayWrite(dev);
+}
+
+void zfHpSetMPDUDensity(zdev_t* dev, u8_t density)
+{
+    u32_t value;
+
+    if (density > ZM_MPDU_DENSITY_8US)
+    {
+        return;
+    }
+
+    /* Default value in this register */
+    value = 0x140A00 | density;
+
+    zfDelayWriteInternalReg(dev, 0x1c3ba0, value);
+    zfFlushDelayWrite(dev);
+    return;
+}
+
+void zfHpSetSlotTime(zdev_t* dev, u8_t type)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+
+    if (type == 0)
+    {
+        //normal slot = 20us
+        hpPriv->slotType = 0;
+    }
+    else //if (type == 1)
+    {
+        //short slot = 9us
+        hpPriv->slotType = 1;
+    }
+
+    return;
+}
+
+void zfHpSetSlotTimeRegister(zdev_t* dev, u8_t type)
+{
+    if(type == 0)
+    {
+        //normal slot = 20us
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_SLOT_TIME, 20<<10);
+    }
+    else
+    {
+        //short slot = 9us
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_SLOT_TIME, 9<<10);
+    }
+}
+
+void zfHpSetRifs(zdev_t* dev, u8_t ht_enable, u8_t ht2040, u8_t g_mode)
+{
+    zfDelayWriteInternalReg(dev, 0x1c6388, 0x0c000000);
+
+    zfDelayWriteInternalReg(dev, 0x1c59ec, 0x0cc80caa);
+
+    if (ht_enable)
+    {
+        if (ht2040)
+        {
+            zfDelayWriteInternalReg(dev, 0x1c5918, 40);
+        }
+        else
+        {
+            zfDelayWriteInternalReg(dev, 0x1c5918, 20);
+        }
+    }
+
+    if (g_mode)
+    {
+        zfDelayWriteInternalReg(dev, 0x1c5850, 0xec08b4e2);
+        zfDelayWriteInternalReg(dev, 0x1c585c, 0x313a5d5e);
+    }
+    else
+    {
+        zfDelayWriteInternalReg(dev, 0x1c5850, 0xede8b4e0);
+        zfDelayWriteInternalReg(dev, 0x1c585c, 0x3139605e);
+    }
+
+    zfFlushDelayWrite(dev);
+    return;
+}
+
+void zfHpBeginSiteSurvey(zdev_t* dev, u8_t status)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    if ( status == 1 )
+    { // Connected
+        hpPriv->isSiteSurvey = 1;
+    }
+    else
+    { // Not connected
+        hpPriv->isSiteSurvey = 0;
+    }
+
+    /* reset workaround state to default */
+//    if (hpPriv->rxStrongRSSI == 1)
+    {
+        hpPriv->rxStrongRSSI = 0;
+        if ((hpPriv->eepromImage[0x100+0x110*2/4]&0xff) == 0x80) //FEM TYPE
+        {
+            if (hpPriv->hwFrequency <= ZM_CH_G_14)
+            {
+                zfDelayWriteInternalReg(dev, 0x1c8960, 0x9b49);
+            }
+            else
+            {
+                zfDelayWriteInternalReg(dev, 0x1c8960, 0x0900);
+            }
+        }
+        else
+        {
+            zfDelayWriteInternalReg(dev, 0x1c8960, 0x9b40);
+        }
+        zfFlushDelayWrite(dev);
+    }
+//    if (hpPriv->strongRSSI == 1)
+    {
+        hpPriv->strongRSSI = 0;
+        zfDelayWriteInternalReg(dev, 0x1c3694, ((hpPriv->currentAckRtsTpc&0x3f) << 20) | (0x5<<26));
+        zfDelayWriteInternalReg(dev, 0x1c3bb4, ((hpPriv->currentAckRtsTpc&0x3f) << 5 ) | (0x5<<11) |
+                                               ((hpPriv->currentAckRtsTpc&0x3f) << 21) | (0x5<<27)  );
+        zfFlushDelayWrite(dev);
+    }
+}
+
+void zfHpFinishSiteSurvey(zdev_t* dev, u8_t status)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if ( status == 1 )
+    {
+        hpPriv->isSiteSurvey = 2;
+    }
+    else
+    {
+        hpPriv->isSiteSurvey = 0;
+    }
+    zmw_leave_critical_section(dev);
+}
+
+u16_t zfFwRetry(zdev_t* dev, u8_t enable)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret = 0;
+
+    cmd[0] = 4 | (0x92 << 8);
+    cmd[1] = (enable == 1) ? 0x01 : 0x00;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_INTERNAL_WRITE, NULL);
+    return ret;
+}
+
+u16_t zfHpEnableHwRetry(zdev_t* dev)
+{
+    u16_t ret;
+
+    ret = zfFwRetry(dev, 0);
+
+    zfDelayWriteInternalReg(dev, 0x1c3b28, 0x33333);
+    zfFlushDelayWrite(dev);
+
+    return ret;
+}
+
+u16_t zfHpDisableHwRetry(zdev_t* dev)
+{
+    u16_t ret;
+
+    ret = zfFwRetry(dev, 1);
+
+    zfDelayWriteInternalReg(dev, 0x1c3b28, 0x00000);
+    zfFlushDelayWrite(dev);
+
+    return ret;
+}
+
+/* Download SPI Fw */
+#define ZM_FIRMWARE_WLAN                0
+#define ZM_FIRMWARE_SPI_FLASH           1
+
+
+u16_t zfHpFirmwareDownload(zdev_t* dev, u8_t fwType)
+{
+    u16_t ret = ZM_SUCCESS;
+
+    if (fwType == ZM_FIRMWARE_WLAN)
+    {
+        ret = zfFirmwareDownload(dev, (u32_t*)zcFwImage,
+                (u32_t)zcFwImageSize, ZM_FIRMWARE_WLAN_ADDR);
+    }
+    else if (fwType == ZM_FIRMWARE_SPI_FLASH)
+    {
+        ret = zfFirmwareDownload(dev, (u32_t*)zcFwImageSPI,
+                (u32_t)zcFwImageSPISize, ZM_FIRMWARE_SPI_ADDR);
+    }
+    else
+    {
+        zm_debug_msg1("Unknown firmware type = ", fwType);
+        ret = ZM_ERR_FIRMWARE_WRONG_TYPE;
+    }
+
+    return ret;
+}
+
+/* Enable software decryption */
+void zfHpSWDecrypt(zdev_t* dev, u8_t enable)
+{
+    u32_t value = 0x70;
+
+    /* Bit 4 for enable software decryption */
+    if (enable == 1)
+    {
+        value = 0x78;
+    }
+
+    zfDelayWriteInternalReg(dev, 0x1c3678, value);
+    zfFlushDelayWrite(dev);
+}
+
+/* Enable software encryption */
+void zfHpSWEncrypt(zdev_t* dev, u8_t enable)
+{
+    /* Because encryption by software or hardware is judged by driver in Otus,
+       we don't need to do anything in the HAL layer.
+     */
+}
+
+u32_t zfHpCapability(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    return hpPriv->halCapability;
+}
+
+void zfHpSetRollCallTable(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    if (hpPriv->camRollCallTable != (u64_t) 0)
+    {
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_ROLL_CALL_TBL_L, (u32_t)(hpPriv->camRollCallTable & 0xffffffff));
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_ROLL_CALL_TBL_H, (u32_t)((hpPriv->camRollCallTable >> 32) & 0xffffffff));
+        zfFlushDelayWrite(dev);
+    }
+}
+
+void zfHpSetTTSIFSTime(zdev_t* dev, u8_t sifs_time)
+{
+    u32_t reg_value = 0;
+    zmw_get_wlan_dev(dev);
+
+    sifs_time &= 0x3f;
+    reg_value = 0x14400b | (((u32_t)sifs_time)<<24);
+
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_EIFS_AND_SIFS, reg_value);
+    zfFlushDelayWrite(dev);
+}
+
+/* #3 Enable RIFS function if the RIFS pattern matched ! */
+void zfHpEnableRifs(zdev_t* dev, u8_t mode24g, u8_t modeHt, u8_t modeHt2040)
+{
+
+    /* # Enable Reset TDOMAIN
+     * $rddata = &$phyreg_read(0x9800+(738<<2));
+     * $wrdata = $rddata | (0x1 << 26) | (0x1 << 27);
+     * &$phyreg_write(0x9800+(738<<2), $wrdata);
+     */
+    reg_write (0x9800+(738<<2), 0x08000000 | (0x1 << 26) | (0x1 << 27));
+    //reg_write (0x9800+(738<<2), 0x08000000 | (0x1 << 26));
+
+    /* # reg 123: heavy clip factor, xr / RIFS search parameters */
+    reg_write (0x99ec, 0x0cc80caa);
+
+    /* # Reduce Search Start Delay for RIFS    */
+    if (modeHt == 1) /* ($HT_ENABLE == 1) */
+    {
+        if (modeHt2040 == 0x1) /* ($DYNAMIC_HT2040_EN == 0x1) */
+        {
+            reg_write(0x9800+(70<<2), 40);/*40*/
+        }
+        else
+        {
+            reg_write(0x9800+(70<<2), 20);
+            if(mode24g == 0x0)
+            {
+                /* $rddata = &$phyreg_read(0x9800+(24<<2));#0x9860;0x1c5860
+                 *$wrdata = ($rddata & 0xffffffc7) | (0x4 << 3);
+                 * &$phyreg_write(0x9800+(24<<2), $wrdata);
+                 */
+                reg_write(0x9800+(24<<2), (0x0004dd10 & 0xffffffc7) | (0x4 << 3));
+            }
+        }
+    }
+
+    if (mode24g == 0x1)
+    {
+        reg_write(0x9850, 0xece8b4e4);/*org*/
+        //reg_write(0x9850, 0xece8b4e2);
+        reg_write(0x985c, 0x313a5d5e);
+    }
+    else
+    {
+        reg_write(0x9850, 0xede8b4e4);
+        reg_write(0x985c, 0x3139605e);
+    }
+
+    zfFlushDelayWrite(dev);
+
+    return;
+}
+
+/* #4 Disable RIFS function if the RIFS timer is timeout ! */
+void zfHpDisableRifs(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Disable RIFS function is to store these HW register initial value while the device plug-in and
+       re-write to these register if the RIFS function is disabled  */
+
+    // reg : 9850
+    reg_write(0x9850, ((struct zsHpPriv*)wd->hpPrivate)->initDesiredSigSize);
+
+    // reg : 985c
+    reg_write(0x985c, ((struct zsHpPriv*)wd->hpPrivate)->initAGC);
+
+    // reg : 9860
+    reg_write(0x9800+(24<<2), ((struct zsHpPriv*)wd->hpPrivate)->initAgcControl);
+
+    // reg : 9918
+    reg_write(0x9800+(70<<2), ((struct zsHpPriv*)wd->hpPrivate)->initSearchStartDelay);
+
+    // reg : 991c
+    reg_write (0x99ec, ((struct zsHpPriv*)wd->hpPrivate)->initRIFSSearchParams);
+
+    // reg : a388
+    reg_write (0x9800+(738<<2), ((struct zsHpPriv*)wd->hpPrivate)->initFastChannelChangeControl);
+
+    zfFlushDelayWrite(dev);
+
+    return;
+}
diff --git a/drivers/staging/otus/hal/hpreg.c b/drivers/staging/otus/hal/hpreg.c
new file mode 100644
index 000000000000..3cfeba8620f6
--- /dev/null
+++ b/drivers/staging/otus/hal/hpreg.c
@@ -0,0 +1,2481 @@
+/*
+ * Copyright (c) 2000-2005 ZyDAS Technology Corporation
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : hpreg.c                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains Regulatory Table and related function.     */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "../80211core/cprecomp.h"
+#include "hpani.h"
+#include "hpreg.h"
+#include "hpusb.h"
+
+/* used throughout this file... */
+#define	N(a)	(sizeof (a) / sizeof (a[0]))
+
+#define HAL_MODE_11A_TURBO	HAL_MODE_108A
+#define HAL_MODE_11G_TURBO	HAL_MODE_108G
+
+#if 0
+enum {
+    /* test groups */
+	FCC	       = 0x10,
+	MKK	       = 0x40,
+	ETSI	   = 0x30,
+    SD_NO_CTL  = 0xe0,
+	NO_CTL	   = 0xff,
+    /* test modes */
+    CTL_MODE_M = 0x0f,
+    CTL_11A    = 0,
+	CTL_11B	   = 1,
+	CTL_11G	   = 2,
+	CTL_TURBO  = 3,
+	CTL_108G   = 4,
+    CTL_2GHT20 = 5,
+    CTL_5GHT20 = 6,
+    CTL_2GHT40 = 7,
+    CTL_5GHT40 = 8
+};
+#endif
+
+/*
+ * The following are flags for different requirements per reg domain.
+ * These requirements are either inhereted from the reg domain pair or
+ * from the unitary reg domain if the reg domain pair flags value is
+ * 0
+ */
+
+enum {
+	NO_REQ			= 0x00000000,
+	DISALLOW_ADHOC_11A	= 0x00000001,
+	DISALLOW_ADHOC_11A_TURB	= 0x00000002,
+	NEED_NFC		= 0x00000004,
+
+	ADHOC_PER_11D		= 0x00000008,  /* Start Ad-Hoc mode */
+	ADHOC_NO_11A		= 0x00000010,
+
+	PUBLIC_SAFETY_DOMAIN	= 0x00000020, 	/* public safety domain */
+	LIMIT_FRAME_4MS 	= 0x00000040, 	/* 4msec limit on the frame length */
+};
+
+#define MKK5GHZ_FLAG1 (DISALLOW_ADHOC_11A_TURB | NEED_NFC | LIMIT_FRAME_4MS)
+#define MKK5GHZ_FLAG2 (DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB | NEED_NFC| LIMIT_FRAME_4MS)
+
+typedef enum {
+	DFS_UNINIT_DOMAIN	= 0,	/* Uninitialized dfs domain */
+	DFS_FCC_DOMAIN		= 1,	/* FCC3 dfs domain */
+	DFS_ETSI_DOMAIN		= 2,	/* ETSI dfs domain */
+} HAL_DFS_DOMAIN;
+
+/*
+ * Used to set the RegDomain bitmask which chooses which frequency
+ * band specs are used.
+ */
+
+#define BMLEN 2		/* Use 2 64 bit uint for channel bitmask
+			   NB: Must agree with macro below (BM) */
+#define BMZERO {(u64_t) 0, (u64_t) 0}	/* BMLEN zeros */
+
+#if 0
+
+#define BM(_fa, _fb, _fc, _fd, _fe, _ff, _fg, _fh, _fi, _fj, _fk, _fl) \
+      {((((_fa >= 0) && (_fa < 64)) ? (((u64_t) 1) << _fa) : (u64_t) 0) | \
+	(((_fb >= 0) && (_fb < 64)) ? (((u64_t) 1) << _fb) : (u64_t) 0) | \
+	(((_fc >= 0) && (_fc < 64)) ? (((u64_t) 1) << _fc) : (u64_t) 0) | \
+	(((_fd >= 0) && (_fd < 64)) ? (((u64_t) 1) << _fd) : (u64_t) 0) | \
+	(((_fe >= 0) && (_fe < 64)) ? (((u64_t) 1) << _fe) : (u64_t) 0) | \
+	(((_ff >= 0) && (_ff < 64)) ? (((u64_t) 1) << _ff) : (u64_t) 0) | \
+	(((_fg >= 0) && (_fg < 64)) ? (((u64_t) 1) << _fg) : (u64_t) 0) | \
+	(((_fh >= 0) && (_fh < 64)) ? (((u64_t) 1) << _fh) : (u64_t) 0) | \
+	(((_fi >= 0) && (_fi < 64)) ? (((u64_t) 1) << _fi) : (u64_t) 0) | \
+	(((_fj >= 0) && (_fj < 64)) ? (((u64_t) 1) << _fj) : (u64_t) 0) | \
+	(((_fk >= 0) && (_fk < 64)) ? (((u64_t) 1) << _fk) : (u64_t) 0) | \
+	(((_fl >= 0) && (_fl < 64)) ? (((u64_t) 1) << _fl) : (u64_t) 0) | \
+	       ((((_fa > 63) && (_fa < 128)) ? (((u64_t) 1) << (_fa - 64)) : (u64_t) 0) | \
+		(((_fb > 63) && (_fb < 128)) ? (((u64_t) 1) << (_fb - 64)) : (u64_t) 0) | \
+		(((_fc > 63) && (_fc < 128)) ? (((u64_t) 1) << (_fc - 64)) : (u64_t) 0) | \
+		(((_fd > 63) && (_fd < 128)) ? (((u64_t) 1) << (_fd - 64)) : (u64_t) 0) | \
+		(((_fe > 63) && (_fe < 128)) ? (((u64_t) 1) << (_fe - 64)) : (u64_t) 0) | \
+		(((_ff > 63) && (_ff < 128)) ? (((u64_t) 1) << (_ff - 64)) : (u64_t) 0) | \
+		(((_fg > 63) && (_fg < 128)) ? (((u64_t) 1) << (_fg - 64)) : (u64_t) 0) | \
+		(((_fh > 63) && (_fh < 128)) ? (((u64_t) 1) << (_fh - 64)) : (u64_t) 0) | \
+		(((_fi > 63) && (_fi < 128)) ? (((u64_t) 1) << (_fi - 64)) : (u64_t) 0) | \
+		(((_fj > 63) && (_fj < 128)) ? (((u64_t) 1) << (_fj - 64)) : (u64_t) 0) | \
+		(((_fk > 63) && (_fk < 128)) ? (((u64_t) 1) << (_fk - 64)) : (u64_t) 0) | \
+		(((_fl > 63) && (_fl < 128)) ? (((u64_t) 1) << (_fl - 64)) : (u64_t) 0)))}
+
+#else
+
+#define BM(_fa, _fb, _fc, _fd, _fe, _ff, _fg, _fh, _fi, _fj, _fk, _fl) \
+      {((((_fa >= 0) && (_fa < 64)) ? (((u64_t) 1) << (_fa&0x3f)) : (u64_t) 0) | \
+	(((_fb >= 0) && (_fb < 64)) ? (((u64_t) 1) << (_fb&0x3f)) : (u64_t) 0) | \
+	(((_fc >= 0) && (_fc < 64)) ? (((u64_t) 1) << (_fc&0x3f)) : (u64_t) 0) | \
+	(((_fd >= 0) && (_fd < 64)) ? (((u64_t) 1) << (_fd&0x3f)) : (u64_t) 0) | \
+	(((_fe >= 0) && (_fe < 64)) ? (((u64_t) 1) << (_fe&0x3f)) : (u64_t) 0) | \
+	(((_ff >= 0) && (_ff < 64)) ? (((u64_t) 1) << (_ff&0x3f)) : (u64_t) 0) | \
+	(((_fg >= 0) && (_fg < 64)) ? (((u64_t) 1) << (_fg&0x3f)) : (u64_t) 0) | \
+	(((_fh >= 0) && (_fh < 64)) ? (((u64_t) 1) << (_fh&0x3f)) : (u64_t) 0) | \
+	(((_fi >= 0) && (_fi < 64)) ? (((u64_t) 1) << (_fi&0x3f)) : (u64_t) 0) | \
+	(((_fj >= 0) && (_fj < 64)) ? (((u64_t) 1) << (_fj&0x3f)) : (u64_t) 0) | \
+	(((_fk >= 0) && (_fk < 64)) ? (((u64_t) 1) << (_fk&0x3f)) : (u64_t) 0) | \
+	(((_fl >= 0) && (_fl < 64)) ? (((u64_t) 1) << (_fl&0x3f)) : (u64_t) 0) | \
+	       ((((_fa > 63) && (_fa < 128)) ? (((u64_t) 1) << ((_fa - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fb > 63) && (_fb < 128)) ? (((u64_t) 1) << ((_fb - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fc > 63) && (_fc < 128)) ? (((u64_t) 1) << ((_fc - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fd > 63) && (_fd < 128)) ? (((u64_t) 1) << ((_fd - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fe > 63) && (_fe < 128)) ? (((u64_t) 1) << ((_fe - 64)&0x3f)) : (u64_t) 0) | \
+		(((_ff > 63) && (_ff < 128)) ? (((u64_t) 1) << ((_ff - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fg > 63) && (_fg < 128)) ? (((u64_t) 1) << ((_fg - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fh > 63) && (_fh < 128)) ? (((u64_t) 1) << ((_fh - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fi > 63) && (_fi < 128)) ? (((u64_t) 1) << ((_fi - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fj > 63) && (_fj < 128)) ? (((u64_t) 1) << ((_fj - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fk > 63) && (_fk < 128)) ? (((u64_t) 1) << ((_fk - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fl > 63) && (_fl < 128)) ? (((u64_t) 1) << ((_fl - 64)&0x3f)) : (u64_t) 0)))}
+
+#endif
+
+/* Mask to check whether a domain is a multidomain or a single
+   domain */
+
+#define MULTI_DOMAIN_MASK 0xFF00
+
+
+/*
+ * The following describe the bit masks for different passive scan
+ * capability/requirements per regdomain.
+ */
+#define	NO_PSCAN	0x0ULL
+#define	PSCAN_FCC	0x0000000000000001ULL
+#define	PSCAN_FCC_T	0x0000000000000002ULL
+#define	PSCAN_ETSI	0x0000000000000004ULL
+#define	PSCAN_MKK1	0x0000000000000008ULL
+#define	PSCAN_MKK2	0x0000000000000010ULL
+#define	PSCAN_MKKA	0x0000000000000020ULL
+#define	PSCAN_MKKA_G	0x0000000000000040ULL
+#define	PSCAN_ETSIA	0x0000000000000080ULL
+#define	PSCAN_ETSIB	0x0000000000000100ULL
+#define	PSCAN_ETSIC	0x0000000000000200ULL
+#define	PSCAN_WWR	0x0000000000000400ULL
+#define	PSCAN_MKKA1	0x0000000000000800ULL
+#define	PSCAN_MKKA1_G	0x0000000000001000ULL
+#define	PSCAN_MKKA2	0x0000000000002000ULL
+#define	PSCAN_MKKA2_G	0x0000000000004000ULL
+#define	PSCAN_MKK3	0x0000000000008000ULL
+#define	PSCAN_DEFER	0x7FFFFFFFFFFFFFFFULL
+#define	IS_ECM_CHAN	0x8000000000000000ULL
+
+/*
+ * THE following table is the mapping of regdomain pairs specified by
+ * an 8 bit regdomain value to the individual unitary reg domains
+ */
+
+typedef struct reg_dmn_pair_mapping {
+	u16_t regDmnEnum;	/* 16 bit reg domain pair */
+	u16_t regDmn5GHz;	/* 5GHz reg domain */
+	u16_t regDmn2GHz;	/* 2GHz reg domain */
+	u32_t flags5GHz;		/* Requirements flags (AdHoc
+					   disallow, noise floor cal needed,
+					   etc) */
+	u32_t flags2GHz;		/* Requirements flags (AdHoc
+					   disallow, noise floor cal needed,
+					   etc) */
+	u64_t pscanMask;		/* Passive Scan flags which
+					   can override unitary domain
+					   passive scan flags.  This
+					   value is used as a mask on
+					   the unitary flags*/
+	u16_t singleCC;		/* Country code of single country if
+					   a one-on-one mapping exists */
+}  REG_DMN_PAIR_MAPPING;
+
+static REG_DMN_PAIR_MAPPING regDomainPairs[] = {
+	{NO_ENUMRD,	    FCC2,	DEBUG_REG_DMN,  NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{NULL1_WORLD,	NULL1,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{NULL1_ETSIB,	NULL1,		ETSIB,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{NULL1_ETSIC,	NULL1,		ETSIC,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+
+	{FCC2_FCCA,     FCC2,		FCCA,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC2_WORLD,    FCC2,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC2_ETSIC,	FCC2,		ETSIC,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC3_FCCA,     FCC3,		FCCA,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC3_WORLD,    FCC3,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC4_FCCA,     FCC4,		FCCA,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC5_FCCA,     FCC5,		FCCA,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC6_FCCA,     FCC6,		FCCA,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC6_WORLD,    FCC6,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+
+	{ETSI1_WORLD,	ETSI1,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{ETSI2_WORLD,	ETSI2,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{ETSI3_WORLD,	ETSI3,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{ETSI4_WORLD,	ETSI4,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{ETSI5_WORLD,	ETSI5,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{ETSI6_WORLD,	ETSI6,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+
+	{ETSI3_ETSIA,	ETSI3,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{FRANCE_RES,	ETSI3,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+
+	{FCC1_WORLD,	FCC1,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC1_FCCA,     FCC1,		FCCA,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL1_WORLD,	APL1,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL2_WORLD,	APL2,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL3_WORLD,	APL3,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL4_WORLD,	APL4,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL5_WORLD,	APL5,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL6_WORLD,	APL6,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL8_WORLD,	APL8,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL9_WORLD,	APL9,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+
+	{APL3_FCCA,     APL3,		FCCA,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL1_ETSIC,	APL1,		ETSIC,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL2_ETSIC,	APL2,		ETSIC,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL2_FCCA,		APL2,		FCCA,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL2_APLD,     APL2,		APLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0},
+	{APL7_FCCA,		APL7,		FCCA,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+
+	{MKK1_MKKA,     MKK1,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKKA, CTRY_JAPAN },
+	{MKK1_MKKB,     MKK1,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKK1 | PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN1 },
+	{MKK1_FCCA,     MKK1,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1, CTRY_JAPAN2 },
+	{MKK1_MKKA1,    MKK1,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN4 },
+	{MKK1_MKKA2,    MKK1,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN5 },
+	{MKK1_MKKC,     MKK1,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1, CTRY_JAPAN6 },
+
+	/* MKK2 */
+	{MKK2_MKKA,     MKK2,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKK2 | PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN3 },
+
+	/* MKK3 */
+	{MKK3_MKKA,     MKK3,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN25 },
+	{MKK3_MKKB,     MKK3,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN7 },
+	{MKK3_MKKA1,    MKK3,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN26 },
+	{MKK3_MKKA2,    MKK3,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN8 },
+	{MKK3_MKKC,     MKK3,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN9 },
+	{MKK3_FCCA,     MKK3,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN27 },
+
+	/* MKK4 */
+	{MKK4_MKKB,     MKK4,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKK3 | PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN10 },
+	{MKK4_MKKA1,    MKK4,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK3 | PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN28 },
+	{MKK4_MKKA2,    MKK4,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK3 |PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN11 },
+	{MKK4_MKKC,     MKK4,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK3, CTRY_JAPAN12 },
+	{MKK4_FCCA,     MKK4,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN29 },
+	{MKK4_MKKA,     MKK4,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK3 | PSCAN_MKKA, CTRY_JAPAN36 },
+
+	/* MKK5 */
+	{MKK5_MKKB,     MKK5,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKK3 | PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN13 },
+	{MKK5_MKKA2,    MKK5,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK3 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN14 },
+	{MKK5_MKKC,     MKK5,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK3, CTRY_JAPAN15 },
+
+	/* MKK6 */
+	{MKK6_MKKB,     MKK6,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKK1 | PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN16 },
+	{MKK6_MKKA2,    MKK6,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN17 },
+	{MKK6_MKKC,     MKK6,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1, CTRY_JAPAN18 },
+	{MKK6_MKKA1,    MKK6,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN30 },
+	{MKK6_FCCA,     MKK6,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN31 },
+
+	/* MKK7 */
+	{MKK7_MKKB,     MKK7,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN19 },
+	{MKK7_MKKA,     MKK7,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN20 },
+	{MKK7_MKKC,     MKK7,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3, CTRY_JAPAN21 },
+	{MKK7_MKKA1,    MKK7,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN32 },
+	{MKK7_FCCA,     MKK7,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN33 },
+
+	/* MKK8 */
+	{MKK8_MKKB,     MKK8,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN22 },
+	{MKK8_MKKA2,    MKK8,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN23 },
+	{MKK8_MKKC,     MKK8,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 , CTRY_JAPAN24 },
+
+   	/* MKK9 */
+	{MKK9_MKKA,     MKK9,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN34 },
+	{MKK9_FCCA,     MKK9,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN37 },
+	{MKK9_MKKA1,    MKK9,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN38 },
+	{MKK9_MKKC,     MKK9,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN39 },
+	{MKK9_MKKA2,	MKK9,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN40 },
+
+	/* MKK10 */
+	{MKK10_MKKA,	MKK10,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN35 },
+	{MKK10_FCCA,	MKK10,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN41 },
+	{MKK10_MKKA1,	MKK10,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN42 },
+	{MKK10_MKKC,	MKK10,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN43 },
+	{MKK10_MKKA2,	MKK10,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN44 },
+
+	/* MKK11 */
+	{MKK11_MKKA,	MKK11,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN45 },
+	{MKK11_FCCA,	MKK11,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN46 },
+	{MKK11_MKKA1,	MKK11,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN47 },
+	{MKK11_MKKC,	MKK11,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN48 },
+	{MKK11_MKKA2,	MKK11,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN49 },
+
+	/* MKK12 */
+	{MKK12_MKKA,	MKK12,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN50 },
+	{MKK12_FCCA,	MKK12,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN51 },
+	{MKK12_MKKA1,	MKK12,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN52 },
+	{MKK12_MKKC,	MKK12,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN53 },
+	{MKK12_MKKA2,	MKK12,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN54 },
+
+
+	/* These are super domains */
+	{WOR0_WORLD,	WOR0_WORLD,	WOR0_WORLD,	NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR1_WORLD,	WOR1_WORLD,	WOR1_WORLD,	DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR2_WORLD,	WOR2_WORLD,	WOR2_WORLD,	DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR3_WORLD,	WOR3_WORLD,	WOR3_WORLD,	NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR4_WORLD,	WOR4_WORLD,	WOR4_WORLD,	DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR5_ETSIC,	WOR5_ETSIC,	WOR5_ETSIC,	DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR01_WORLD,	WOR01_WORLD,	WOR01_WORLD,	NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR02_WORLD,	WOR02_WORLD,	WOR02_WORLD,	NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{EU1_WORLD,	    EU1_WORLD,	EU1_WORLD,	NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR9_WORLD,	WOR9_WORLD,	WOR9_WORLD,	DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{WORA_WORLD,	WORA_WORLD,	WORA_WORLD,	DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+};
+
+/*
+ * The following table is the master list for all different freqeuncy
+ * bands with the complete matrix of all possible flags and settings
+ * for each band if it is used in ANY reg domain.
+ */
+
+#define DEF_REGDMN		FCC1_FCCA
+#define	DEF_DMN_5		FCC1
+#define	DEF_DMN_2		FCCA
+#define	COUNTRY_ERD_FLAG        0x8000
+#define WORLDWIDE_ROAMING_FLAG  0x4000
+#define	SUPER_DOMAIN_MASK	0x0fff
+#define	COUNTRY_CODE_MASK	0x03ff
+#define CF_INTERFERENCE		(CHANNEL_CW_INT | CHANNEL_RADAR_INT)
+#define CHANNEL_14		(2484)	/* 802.11g operation is not permitted on channel 14 */
+#define IS_11G_CH14(_ch,_cf) \
+	(((_ch) == CHANNEL_14) && ((_cf) == CHANNEL_G))
+
+#define	YES	TRUE
+#define	NO	FALSE
+
+enum {
+	CTRY_DEBUG	= 0x1ff,		/* debug country code */
+	CTRY_DEFAULT	= 0			/* default country code */
+};
+
+typedef struct {
+	HAL_CTRY_CODE	countryCode;
+	HAL_REG_DOMAIN	regDmnEnum;
+	const char*		isoName;
+	const char*		name;
+	HAL_BOOL		allow11g;
+	HAL_BOOL		allow11aTurbo;
+	HAL_BOOL		allow11gTurbo;
+    HAL_BOOL        allow11na;      /* HT-40 allowed in 5GHz? */
+    HAL_BOOL        allow11ng;      /* HT-40 allowed in 2GHz? */
+	u16_t		outdoorChanStart;
+} COUNTRY_CODE_TO_ENUM_RD;
+
+static COUNTRY_CODE_TO_ENUM_RD allCountries[] = {
+    {CTRY_DEBUG,       NO_ENUMRD,     "DB", "DEBUG",          YES, YES, YES, YES, YES, 7000 },
+    {CTRY_DEFAULT,     DEF_REGDMN,    "NA", "NO_COUNTRY_SET", YES, YES, YES, YES, YES, 7000 },
+    {CTRY_ALBANIA,     NULL1_WORLD,   "AL", "ALBANIA",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_ALGERIA,     NULL1_WORLD,   "DZ", "ALGERIA",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_ARGENTINA,   APL3_WORLD,    "AR", "ARGENTINA",      YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_ARMENIA,     ETSI4_WORLD,   "AM", "ARMENIA",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_AUSTRALIA,   FCC6_WORLD,    "AU", "AUSTRALIA",      YES, YES, YES, YES, YES, 7000 },
+    {CTRY_AUSTRIA,     ETSI2_WORLD,   "AT", "AUSTRIA",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_AZERBAIJAN,  ETSI4_WORLD,   "AZ", "AZERBAIJAN",     YES, YES, YES, YES, YES, 7000 },
+    {CTRY_BAHRAIN,     APL6_WORLD,    "BH", "BAHRAIN",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_BELARUS,     ETSI1_WORLD,   "BY", "BELARUS",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_BELGIUM,     ETSI1_WORLD,   "BE", "BELGIUM",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_BELIZE,      APL1_ETSIC,    "BZ", "BELIZE",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_BOLIVIA,     APL1_ETSIC,    "BO", "BOLVIA",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_BRAZIL,      FCC3_WORLD,    "BR", "BRAZIL",         NO,  NO,  NO,  NO,  NO,  7000 },
+    {CTRY_BRUNEI_DARUSSALAM,APL1_WORLD,"BN", "BRUNEI DARUSSALAM", YES, YES, YES,  YES, YES, 7000 },
+    {CTRY_BULGARIA,    ETSI6_WORLD,   "BG", "BULGARIA",       YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_CANADA,      FCC6_FCCA,     "CA", "CANADA",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_CHILE,       APL6_WORLD,    "CL", "CHILE",          YES, YES, YES, YES, YES, 7000 },
+    {CTRY_CHINA,       APL1_WORLD,    "CN", "CHINA",          YES, YES, YES, YES, YES, 7000 },
+    {CTRY_COLOMBIA,    FCC1_FCCA,     "CO", "COLOMBIA",       YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_COSTA_RICA,  FCC1_WORLD,    "CR", "COSTA RICA",     YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_CROATIA,     ETSI3_WORLD,   "HR", "CROATIA",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_CYPRUS,      ETSI3_WORLD,   "CY", "CYPRUS",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_CZECH,       ETSI3_WORLD,   "CZ", "CZECH REPUBLIC", YES, NO, YES,  YES, YES, 7000 },
+    {CTRY_DENMARK,     ETSI1_WORLD,   "DK", "DENMARK",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_DOMINICAN_REPUBLIC,FCC1_FCCA,"DO", "DOMINICAN REPUBLIC", YES, YES, YES, YES, YES, 7000 },
+    {CTRY_ECUADOR,     FCC1_WORLD,    "EC", "ECUADOR",        YES, NO,  NO,  NO,  YES, 7000 },
+    {CTRY_EGYPT,       ETSI3_WORLD,   "EG", "EGYPT",          YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_EL_SALVADOR, FCC1_WORLD,    "SV", "EL SALVADOR",    YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_ESTONIA,     ETSI1_WORLD,   "EE", "ESTONIA",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_FINLAND,     ETSI1_WORLD,   "FI", "FINLAND",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_FRANCE,      ETSI1_WORLD,   "FR", "FRANCE",         YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_FRANCE2,     ETSI3_WORLD,   "F2", "FRANCE_RES",     YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_GEORGIA,     ETSI4_WORLD,   "GE", "GEORGIA",        YES, YES, YES, YES, YES, 7000 },
+    {CTRY_GERMANY,     ETSI1_WORLD,   "DE", "GERMANY",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_GREECE,      ETSI1_WORLD,   "GR", "GREECE",         YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_GUATEMALA,   FCC1_FCCA,     "GT", "GUATEMALA",      YES, YES, YES, YES, YES, 7000 },
+    {CTRY_HONDURAS,    NULL1_WORLD,   "HN", "HONDURAS",       YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_HONG_KONG,   FCC2_WORLD,    "HK", "HONG KONG",      YES, YES, YES, YES, YES, 7000 },
+    {CTRY_HUNGARY,     ETSI4_WORLD,   "HU", "HUNGARY",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_ICELAND,     ETSI1_WORLD,   "IS", "ICELAND",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_INDIA,       APL6_WORLD,    "IN", "INDIA",          YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_INDONESIA,   APL1_WORLD,    "ID", "INDONESIA",      YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_IRAN,        APL1_WORLD,    "IR", "IRAN",           YES, YES, YES, YES, YES, 7000 },
+    {CTRY_IRELAND,     ETSI1_WORLD,   "IE", "IRELAND",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_ISRAEL,      ETSI3_WORLD,   "IL", "ISRAEL",         YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_ISRAEL2,     NULL1_ETSIB,   "ISR","ISRAEL_RES",     YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_ITALY,       ETSI1_WORLD,   "IT", "ITALY",          YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_JAMAICA,     ETSI1_WORLD,   "JM", "JAMAICA",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_JAPAN,       MKK1_MKKA,     "JP", "JAPAN",          YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN1,      MKK1_MKKB,     "J1", "JAPAN1",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN2,      MKK1_FCCA,     "J2", "JAPAN2",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN3,      MKK2_MKKA,     "J3", "JAPAN3",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN4,      MKK1_MKKA1,    "J4", "JAPAN4",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN5,      MKK1_MKKA2,    "J5", "JAPAN5",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN6,      MKK1_MKKC,     "J6", "JAPAN6",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN7,      MKK3_MKKB,     "J7", "JAPAN7",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN8,      MKK3_MKKA2,    "J8", "JAPAN8",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN9,      MKK3_MKKC,     "J9", "JAPAN9",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN10,     MKK4_MKKB,     "J10", "JAPAN10",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN11,     MKK4_MKKA2,    "J11", "JAPAN11",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN12,     MKK4_MKKC,     "J12", "JAPAN12",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN13,     MKK5_MKKB,     "J13", "JAPAN13",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN14,     MKK5_MKKA2,    "J14", "JAPAN14",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN15,     MKK5_MKKC,     "J15", "JAPAN15",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN16,     MKK6_MKKB,     "J16", "JAPAN16",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN17,     MKK6_MKKA2,    "J17", "JAPAN17",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN18,     MKK6_MKKC,     "J18", "JAPAN18",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN19,     MKK7_MKKB,     "J19", "JAPAN19",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN20,     MKK7_MKKA,     "J20", "JAPAN20",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN21,     MKK7_MKKC,     "J21", "JAPAN21",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN22,     MKK8_MKKB,     "J22", "JAPAN22",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN23,     MKK8_MKKA2,    "J23", "JAPAN23",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN24,     MKK8_MKKC,     "J24", "JAPAN24",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN25,     MKK3_MKKA,     "J25", "JAPAN25",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN26,     MKK3_MKKA1,    "J26", "JAPAN26",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN27,     MKK3_FCCA,     "J27", "JAPAN27",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN28,     MKK4_MKKA1,    "J28", "JAPAN28",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN29,     MKK4_FCCA,     "J29", "JAPAN29",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN30,     MKK6_MKKA1,    "J30", "JAPAN30",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN31,     MKK6_FCCA,     "J31", "JAPAN31",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN32,     MKK7_MKKA1,    "J32", "JAPAN32",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN33,     MKK7_FCCA,     "J33", "JAPAN33",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN34,     MKK9_MKKA,     "J34", "JAPAN34",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN35,     MKK10_MKKA,    "J35", "JAPAN35",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN36,     MKK4_MKKA,     "J36", "JAPAN36",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN37,     MKK9_FCCA,     "J37", "JAPAN37",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN38,     MKK9_MKKA1,    "J38", "JAPAN38",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN39,     MKK9_MKKC,     "J39", "JAPAN39",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN40,     MKK10_MKKA2,   "J40", "JAPAN40",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN41,     MKK10_FCCA,    "J41", "JAPAN41",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN42,     MKK10_MKKA1,   "J42", "JAPAN42",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN43,     MKK10_MKKC,    "J43", "JAPAN43",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN44,     MKK10_MKKA2,   "J44", "JAPAN44",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN45,     MKK11_MKKA,    "J45", "JAPAN45",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN46,     MKK11_FCCA,    "J46", "JAPAN46",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN47,     MKK11_MKKA1,   "J47", "JAPAN47",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN48,     MKK11_MKKC,    "J48", "JAPAN48",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN49,     MKK11_MKKA2,   "J49", "JAPAN49",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN50,     MKK12_MKKA,    "J50", "JAPAN50",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN51,     MKK12_FCCA,    "J51", "JAPAN51",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN52,     MKK12_MKKA1,   "J52", "JAPAN52",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN53,     MKK12_MKKC,    "J53", "JAPAN53",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN54,     MKK12_MKKA2,   "J54", "JAPAN54",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JORDAN,      ETSI2_WORLD,   "JO", "JORDAN",         YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_KAZAKHSTAN,  NULL1_WORLD,   "KZ", "KAZAKHSTAN",     YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_KOREA_NORTH, APL9_WORLD,    "KP", "NORTH KOREA",    YES, NO,  NO,  YES, YES, 7000 },
+    {CTRY_KOREA_ROC,   APL9_WORLD,    "KR", "KOREA REPUBLIC", YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_KOREA_ROC2,  APL2_APLD,     "K2", "KOREA REPUBLIC2",YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_KOREA_ROC3,  APL9_WORLD,    "K3", "KOREA REPUBLIC3",YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_KUWAIT,      NULL1_WORLD,   "KW", "KUWAIT",         YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_LATVIA,      ETSI1_WORLD,   "LV", "LATVIA",         YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_LEBANON,     NULL1_WORLD,   "LB", "LEBANON",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_LIECHTENSTEIN,ETSI1_WORLD,  "LI", "LIECHTENSTEIN",  YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_LITHUANIA,   ETSI1_WORLD,   "LT", "LITHUANIA",      YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_LUXEMBOURG,  ETSI1_WORLD,   "LU", "LUXEMBOURG",     YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_MACAU,       FCC2_WORLD,    "MO", "MACAU",          YES, YES, YES, YES, YES, 7000 },
+    {CTRY_MACEDONIA,   NULL1_WORLD,   "MK", "MACEDONIA",      YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_MALAYSIA,    APL8_WORLD,    "MY", "MALAYSIA",       NO,  NO,  NO,  NO,  NO,  7000 },
+    {CTRY_MALTA,       ETSI1_WORLD,   "MT", "MALTA",          YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_MEXICO,      FCC1_FCCA,     "MX", "MEXICO",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_MONACO,      ETSI4_WORLD,   "MC", "MONACO",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_MOROCCO,     NULL1_WORLD,   "MA", "MOROCCO",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_NETHERLANDS, ETSI1_WORLD,   "NL", "NETHERLANDS",    YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_NETHERLANDS_ANT, ETSI1_WORLD, "AN", "NETHERLANDS-ANTILLES", YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_NEW_ZEALAND, FCC2_ETSIC,    "NZ", "NEW ZEALAND",    YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_NORWAY,      ETSI1_WORLD,   "NO", "NORWAY",         YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_OMAN,        APL6_WORLD,    "OM", "OMAN",           YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_PAKISTAN,    NULL1_WORLD,   "PK", "PAKISTAN",       YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_PANAMA,      FCC1_FCCA,     "PA", "PANAMA",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_PERU,        APL1_WORLD,    "PE", "PERU",           YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_PHILIPPINES, APL1_WORLD,    "PH", "PHILIPPINES",    YES, YES, YES, YES, YES, 7000 },
+    {CTRY_POLAND,      ETSI1_WORLD,   "PL", "POLAND",         YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_PORTUGAL,    ETSI1_WORLD,   "PT", "PORTUGAL",       YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_PUERTO_RICO, FCC1_FCCA,     "PR", "PUERTO RICO",    YES, YES, YES, YES, YES, 7000 },
+    {CTRY_QATAR,       NULL1_WORLD,   "QA", "QATAR",          YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_ROMANIA,     NULL1_WORLD,   "RO", "ROMANIA",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_RUSSIA,      NULL1_WORLD,   "RU", "RUSSIA",         YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_SAUDI_ARABIA,NULL1_WORLD,   "SA", "SAUDI ARABIA",   YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_SERBIA_MONT, ETSI1_WORLD,   "CS", "SERBIA & MONTENEGRO", YES, NO,  YES, YES,  YES, 7000 },
+    {CTRY_SINGAPORE,   APL6_WORLD,    "SG", "SINGAPORE",      YES, YES, YES, YES, YES, 7000 },
+    {CTRY_SLOVAKIA,    ETSI1_WORLD,   "SK", "SLOVAK REPUBLIC",YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_SLOVENIA,    ETSI1_WORLD,   "SI", "SLOVENIA",       YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_SOUTH_AFRICA,FCC3_WORLD,    "ZA", "SOUTH AFRICA",   YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_SPAIN,       ETSI1_WORLD,   "ES", "SPAIN",          YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_SRILANKA,    FCC3_WORLD,    "LK", "SRI LANKA",      YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_SWEDEN,      ETSI1_WORLD,   "SE", "SWEDEN",         YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_SWITZERLAND, ETSI1_WORLD,   "CH", "SWITZERLAND",    YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_SYRIA,       NULL1_WORLD,   "SY", "SYRIA",          YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_TAIWAN,      APL3_FCCA,     "TW", "TAIWAN",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_THAILAND,    NULL1_WORLD,   "TH", "THAILAND",       YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_TRINIDAD_Y_TOBAGO,ETSI4_WORLD,"TT", "TRINIDAD & TOBAGO", YES, NO, YES, NO, YES, 7000 },
+    {CTRY_TUNISIA,     ETSI3_WORLD,   "TN", "TUNISIA",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_TURKEY,      ETSI3_WORLD,   "TR", "TURKEY",         YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_UKRAINE,     NULL1_WORLD,   "UA", "UKRAINE",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_UAE,         NULL1_WORLD,   "AE", "UNITED ARAB EMIRATES", YES, NO, YES, NO, YES, 7000 },
+    {CTRY_UNITED_KINGDOM, ETSI1_WORLD,"GB", "UNITED KINGDOM", YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_UNITED_STATES, FCC3_FCCA,   "US", "UNITED STATES",  YES, YES, YES, YES, YES, 5825 },
+    {CTRY_UNITED_STATES_FCC49, FCC4_FCCA,   "PS", "UNITED STATES (PUBLIC SAFETY)",  YES, YES, YES, YES, YES, 7000 },
+    {CTRY_URUGUAY,     FCC1_WORLD,    "UY", "URUGUAY",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_UZBEKISTAN,  FCC3_FCCA,     "UZ", "UZBEKISTAN",     YES, YES, YES, YES, YES, 7000 },
+    {CTRY_VENEZUELA,   APL2_ETSIC,    "VE", "VENEZUELA",      YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_VIET_NAM,    NULL1_WORLD,   "VN", "VIET NAM",       YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_YEMEN,       NULL1_WORLD,   "YE", "YEMEN",          YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_ZIMBABWE,    NULL1_WORLD,   "ZW", "ZIMBABWE",       YES, NO,  YES, NO,  YES, 7000 }
+};
+
+typedef struct RegDmnFreqBand {
+	u16_t	lowChannel;	/* Low channel center in MHz */
+	u16_t	highChannel;	/* High Channel center in MHz */
+	u8_t	powerDfs;	/* Max power (dBm) for channel
+					   range when using DFS */
+	u8_t	antennaMax;	/* Max allowed antenna gain */
+	u8_t	channelBW;	/* Bandwidth of the channel */
+	u8_t	channelSep;	/* Channel separation within
+					   the band */
+	u64_t	useDfs;		/* Use DFS in the RegDomain
+					   if corresponding bit is set */
+	u64_t	usePassScan;	/* Use Passive Scan in the RegDomain
+					   if corresponding bit is set */
+	u8_t	regClassId;	/* Regulatory class id */
+	u8_t	useExtChanDfs;	/* Regulatory class id */
+} REG_DMN_FREQ_BAND;
+
+/* Bit masks for DFS per regdomain */
+
+enum {
+	NO_DFS   = 0x0000000000000000ULL,
+	DFS_FCC3 = 0x0000000000000001ULL,
+	DFS_ETSI = 0x0000000000000002ULL,
+	DFS_MKK4 = 0x0000000000000004ULL,
+};
+
+/* The table of frequency bands is indexed by a bitmask.  The ordering
+ * must be consistent with the enum below.  When adding a new
+ * frequency band, be sure to match the location in the enum with the
+ * comments
+ */
+
+/*
+ * 5GHz 11A channel tags
+ */
+
+enum {
+	F1_4915_4925,
+	F1_4935_4945,
+	F1_4920_4980,
+	F1_4942_4987,
+	F1_4945_4985,
+	F1_4950_4980,
+	F1_5035_5040,
+	F1_5040_5080,
+	F1_5055_5055,
+
+	F1_5120_5240,
+
+	F1_5170_5230,
+	F2_5170_5230,
+
+	F1_5180_5240,
+	F2_5180_5240,
+	F3_5180_5240,
+	F4_5180_5240,
+	F5_5180_5240,
+	F6_5180_5240,
+	F7_5180_5240,
+
+	F1_5180_5320,
+
+	F1_5240_5280,
+
+	F1_5260_5280,
+
+	F1_5260_5320,
+	F2_5260_5320,
+	F3_5260_5320,
+	F4_5260_5320,
+	F5_5260_5320,
+	F6_5260_5320,
+	F7_5260_5320,
+
+	F1_5260_5700,
+
+	F1_5280_5320,
+
+    F1_5500_5580,
+
+	F1_5500_5620,
+
+	F1_5500_5700,
+	F2_5500_5700,
+	F3_5500_5700,
+	F4_5500_5700,
+
+    F1_5660_5700,
+
+	F1_5745_5805,
+	F2_5745_5805,
+	F3_5745_5805,
+
+	F1_5745_5825,
+	F2_5745_5825,
+	F3_5745_5825,
+	F4_5745_5825,
+	F5_5745_5825,
+	F6_5745_5825,
+
+	W1_4920_4980,
+	W1_5040_5080,
+	W1_5170_5230,
+	W1_5180_5240,
+	W1_5260_5320,
+	W1_5745_5825,
+	W1_5500_5700,
+	W2_5260_5320,
+        W2_5180_5240,
+	W2_5825_5825,
+};
+
+static REG_DMN_FREQ_BAND regDmn5GhzFreq[] = {
+	{ 4915, 4925, 23, 0, 10, 5, NO_DFS, PSCAN_MKK2, 16, 0 },		/* F1_4915_4925 */
+	{ 4935, 4945, 23, 0, 10, 5, NO_DFS, PSCAN_MKK2, 16, 0 },		/* F1_4935_4945 */
+	{ 4920, 4980, 23, 0, 20, 20, NO_DFS, PSCAN_MKK2, 7, 0 },		/* F1_4920_4980 */
+	{ 4942, 4987, 27, 6, 5,  5, DFS_FCC3, PSCAN_FCC, 0, 0 },		/* F1_4942_4987 */
+	{ 4945, 4985, 30, 6, 10, 5, DFS_FCC3, PSCAN_FCC, 0, 0 },		/* F1_4945_4985 */
+	{ 4950, 4980, 33, 6, 20, 5, DFS_FCC3, PSCAN_FCC, 0, 0 },		/* F1_4950_4980 */
+	{ 5035, 5040, 23, 0, 10, 5, NO_DFS, PSCAN_MKK2, 12, 0 },		/* F1_5035_5040 */
+	{ 5040, 5080, 23, 0, 20, 20, NO_DFS, PSCAN_MKK2, 2, 0 },		/* F1_5040_5080 */
+	{ 5055, 5055, 23, 0, 10, 5, NO_DFS, PSCAN_MKK2, 12, 0 },		/* F1_5055_5055 */
+
+	{ 5120, 5240, 5,  6, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },			/* F1_5120_5240 */
+
+	{ 5170, 5230, 23, 0, 20, 20, NO_DFS, PSCAN_MKK1 | PSCAN_MKK2, 1, 0 },	/* F1_5170_5230 */
+	{ 5170, 5230, 20, 0, 20, 20, NO_DFS, PSCAN_MKK1 | PSCAN_MKK2, 1, 0 },	/* F2_5170_5230 */
+
+	{ 5180, 5240, 15, 0, 20, 20, NO_DFS, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F1_5180_5240 */
+	{ 5180, 5240, 17, 6, 20, 20, NO_DFS, PSCAN_FCC, 1, 0 },				/* F2_5180_5240 */
+	{ 5180, 5240, 18, 0, 20, 20, NO_DFS, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F3_5180_5240 */
+	{ 5180, 5240, 20, 0, 20, 20, NO_DFS, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F4_5180_5240 */
+	{ 5180, 5240, 23, 0, 20, 20, NO_DFS, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F5_5180_5240 */
+	{ 5180, 5240, 23, 6, 20, 20, NO_DFS, PSCAN_FCC, 0, 0 },				/* F6_5180_5240 */
+  { 5180, 5240, 23, 6, 20, 20, NO_DFS, NO_PSCAN, 0 },           /* F7_5180_5240 */
+
+	{ 5180, 5320, 20, 6, 20, 20, DFS_ETSI, PSCAN_ETSI, 0, 0 },			/* F1_5180_5320 */
+
+	{ 5240, 5280, 23, 0, 20, 20, DFS_FCC3, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F1_5240_5280 */
+
+	{ 5260, 5280, 23, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F1_5260_5280 */
+
+	{ 5260, 5320, 18, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F1_5260_5320 */
+
+	{ 5260, 5320, 20, 0, 20, 20, DFS_FCC3 | DFS_ETSI | DFS_MKK4, PSCAN_FCC | PSCAN_ETSI | PSCAN_MKK3 , 0, 0 },
+											/* F2_5260_5320 */
+
+	{ 5260, 5320, 20, 6, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC, 2, 0 },	/* F3_5260_5320 */
+	{ 5260, 5320, 23, 6, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC, 2, 0 },	/* F4_5260_5320 */
+	{ 5260, 5320, 23, 6, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC, 0, 0 },	/* F5_5260_5320 */
+	{ 5260, 5320, 30, 0, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },				/* F6_5260_5320 */
+	{ 5260, 5320, 17, 6, 20, 20, DFS_ETSI, PSCAN_ETSI, 0, 0 },				/* F7_5260_5320 */
+
+	{ 5260, 5700, 5,  6, 20, 20, DFS_FCC3 | DFS_ETSI, NO_PSCAN, 0, 0 },		/* F1_5260_5700 */
+
+	{ 5280, 5320, 17, 6, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC, 0, 0 },	/* F1_5280_5320 */
+
+    { 5500, 5580, 23, 6, 20, 20, DFS_FCC3, PSCAN_FCC, 0},                           /* F1_5500_5580 */
+
+	{ 5500, 5620, 30, 6, 20, 20, DFS_ETSI, PSCAN_ETSI, 0, 0 },				/* F1_5500_5620 */
+
+	{ 5500, 5700, 20, 6, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC, 4, 0 },		/* F1_5500_5700 */
+	{ 5500, 5700, 27, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F2_5500_5700 */
+	{ 5500, 5700, 30, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F3_5500_5700 */
+	{ 5500, 5700, 20, 0, 20, 20, DFS_FCC3 | DFS_ETSI | DFS_MKK4, PSCAN_MKK3 | PSCAN_FCC, 0, 0 },
+											/* F4_5500_5700 */
+
+    { 5660, 5700, 23, 6, 20, 20, DFS_FCC3, PSCAN_FCC, 0},                           /* F1_5660_5700 */
+
+	{ 5745, 5805, 23, 0, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },				/* F1_5745_5805 */
+	{ 5745, 5805, 30, 6, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },				/* F2_5745_5805 */
+	{ 5745, 5805, 30, 6, 20, 20, DFS_ETSI, PSCAN_ETSI, 0, 0 },				/* F3_5745_5805 */
+	{ 5745, 5825, 5,  6, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },				/* F1_5745_5825 */
+	{ 5745, 5825, 17, 0, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },				/* F2_5745_5825 */
+	{ 5745, 5825, 20, 0, 20, 20, DFS_ETSI, NO_PSCAN, 0, 0 },				/* F3_5745_5825 */
+	{ 5745, 5825, 30, 0, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },				/* F4_5745_5825 */
+	{ 5745, 5825, 30, 6, 20, 20, NO_DFS, NO_PSCAN, 3, 0 },			/* F5_5745_5825 */
+	{ 5745, 5825, 30, 6, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },				/* F6_5745_5825 */
+
+	/*
+	 * Below are the world roaming channels
+	 * All WWR domains have no power limit, instead use the card's CTL
+	 * or max power settings.
+	 */
+	{ 4920, 4980, 30, 0, 20, 20, NO_DFS, PSCAN_WWR, 0, 0 },				/* W1_4920_4980 */
+	{ 5040, 5080, 30, 0, 20, 20, NO_DFS, PSCAN_WWR, 0 },				/* W1_5040_5080 */
+	{ 5170, 5230, 30, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, 0, 0 },		/* W1_5170_5230 */
+	{ 5180, 5240, 30, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, 0, 0 },		/* W1_5180_5240 */
+	{ 5260, 5320, 30, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, 0, 0 },		/* W1_5260_5320 */
+	{ 5745, 5825, 30, 0, 20, 20, NO_DFS, PSCAN_WWR, 0, 0 },				/* W1_5745_5825 */
+	{ 5500, 5700, 30, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, 0, 0 },		/* W1_5500_5700 */
+	{ 5260, 5320, 30, 0, 20, 20, NO_DFS, NO_PSCAN,  0, 0 },				/* W2_5260_5320 */
+	{ 5180, 5240, 30, 0, 20, 20, NO_DFS, NO_PSCAN,  0, 0 },				/* W2_5180_5240 */
+	{ 5825, 5825, 30, 0, 20, 20, NO_DFS, PSCAN_WWR, 0, 0 },				/* W2_5825_5825 */
+};
+/*
+ * 5GHz Turbo (dynamic & static) tags
+ */
+
+enum {
+	T1_5130_5210,
+	T1_5250_5330,
+	T1_5370_5490,
+	T1_5530_5650,
+
+	T1_5150_5190,
+	T1_5230_5310,
+	T1_5350_5470,
+	T1_5510_5670,
+
+	T1_5200_5240,
+	T2_5200_5240,
+	T1_5210_5210,
+	T2_5210_5210,
+
+	T1_5280_5280,
+	T2_5280_5280,
+	T1_5250_5250,
+	T1_5290_5290,
+	T1_5250_5290,
+	T2_5250_5290,
+
+	T1_5540_5660,
+	T1_5760_5800,
+	T2_5760_5800,
+
+	T1_5765_5805,
+
+	WT1_5210_5250,
+	WT1_5290_5290,
+	WT1_5540_5660,
+	WT1_5760_5800,
+};
+
+static REG_DMN_FREQ_BAND regDmn5GhzTurboFreq[] = {
+	{ 5130, 5210, 5,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5130_5210 */
+	{ 5250, 5330, 5,  6, 40, 40, DFS_FCC3, NO_PSCAN, 0, 0},	/* T1_5250_5330 */
+	{ 5370, 5490, 5,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5370_5490 */
+	{ 5530, 5650, 5,  6, 40, 40, DFS_FCC3, NO_PSCAN, 0, 0},	/* T1_5530_5650 */
+
+	{ 5150, 5190, 5,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5150_5190 */
+	{ 5230, 5310, 5,  6, 40, 40, DFS_FCC3, NO_PSCAN, 0, 0},	/* T1_5230_5310 */
+	{ 5350, 5470, 5,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5350_5470 */
+	{ 5510, 5670, 5,  6, 40, 40, DFS_FCC3, NO_PSCAN, 0, 0},	/* T1_5510_5670 */
+
+	{ 5200, 5240, 17, 6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5200_5240 */
+	{ 5200, 5240, 23, 6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T2_5200_5240 */
+	{ 5210, 5210, 17, 6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5210_5210 */
+	{ 5210, 5210, 23, 0, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T2_5210_5210 */
+
+	{ 5280, 5280, 23, 6, 40, 40, DFS_FCC3, PSCAN_FCC_T, 0, 0},	/* T1_5280_5280 */
+	{ 5280, 5280, 20, 6, 40, 40, DFS_FCC3, PSCAN_FCC_T, 0, 0},	/* T2_5280_5280 */
+	{ 5250, 5250, 17, 0, 40, 40, DFS_FCC3, PSCAN_FCC_T, 0, 0},	/* T1_5250_5250 */
+	{ 5290, 5290, 20, 0, 40, 40, DFS_FCC3, PSCAN_FCC_T, 0, 0},	/* T1_5290_5290 */
+	{ 5250, 5290, 20, 0, 40, 40, DFS_FCC3, PSCAN_FCC_T, 0, 0},	/* T1_5250_5290 */
+	{ 5250, 5290, 23, 6, 40, 40, DFS_FCC3, PSCAN_FCC_T, 0, 0},	/* T2_5250_5290 */
+
+	{ 5540, 5660, 20, 6, 40, 40, DFS_FCC3, PSCAN_FCC_T, 0, 0},	/* T1_5540_5660 */
+	{ 5760, 5800, 20, 0, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5760_5800 */
+	{ 5760, 5800, 30, 6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T2_5760_5800 */
+
+	{ 5765, 5805, 30, 6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5765_5805 */
+
+	/*
+	 * Below are the WWR frequencies
+	 */
+
+	{ 5210, 5250, 15, 0, 40, 40, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, 0, 0}, /* WT1_5210_5250 */
+	{ 5290, 5290, 18, 0, 40, 40, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, 0, 0}, /* WT1_5290_5290 */
+	{ 5540, 5660, 20, 0, 40, 40, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, 0, 0}, /* WT1_5540_5660 */
+	{ 5760, 5800, 20, 0, 40, 40, NO_DFS, PSCAN_WWR, 0, 0},	/* WT1_5760_5800 */
+};
+
+/*
+ * 2GHz 11b channel tags
+ */
+enum {
+	F1_2312_2372,
+	F2_2312_2372,
+
+	F1_2412_2472,
+	F2_2412_2472,
+	F3_2412_2472,
+
+	F1_2412_2462,
+	F2_2412_2462,
+
+	F1_2432_2442,
+
+	F1_2457_2472,
+
+	F1_2467_2472,
+
+	F1_2484_2484,
+	F2_2484_2484,
+
+	F1_2512_2732,
+
+	W1_2312_2372,
+	W1_2412_2412,
+	W1_2417_2432,
+	W1_2437_2442,
+	W1_2447_2457,
+	W1_2462_2462,
+	W1_2467_2467,
+	W2_2467_2467,
+	W1_2472_2472,
+	W2_2472_2472,
+	W1_2484_2484,
+	W2_2484_2484,
+};
+
+static REG_DMN_FREQ_BAND regDmn2GhzFreq[] = {
+	{ 2312, 2372, 5,  6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F1_2312_2372 */
+	{ 2312, 2372, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F2_2312_2372 */
+
+	{ 2412, 2472, 5,  6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F1_2412_2472 */
+	{ 2412, 2472, 20, 0, 20, 5, NO_DFS, PSCAN_MKKA, 0, 0},	/* F2_2412_2472 */
+	{ 2412, 2472, 30, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F3_2412_2472 */
+
+	{ 2412, 2462, 27, 6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F1_2412_2462 */
+	{ 2412, 2462, 20, 0, 20, 5, NO_DFS, PSCAN_MKKA, 0, 0},	/* F2_2412_2462 */
+	{ 2432, 2442, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F1_2432_2442 */
+
+	{ 2457, 2472, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F1_2457_2472 */
+
+	{ 2467, 2472, 20, 0, 20, 5, NO_DFS, PSCAN_MKKA2 | PSCAN_MKKA, 0, 0}, /* F1_2467_2472 */
+
+	{ 2484, 2484, 5,  6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F1_2484_2484 */
+	{ 2484, 2484, 20, 0, 20, 5, NO_DFS, PSCAN_MKKA | PSCAN_MKKA1 | PSCAN_MKKA2, 0, 0},	/* F2_2484_2484 */
+
+	{ 2512, 2732, 5,  6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F1_2512_2732 */
+
+	/*
+	 * WWR have powers opened up to 20dBm.  Limits should often come from CTL/Max powers
+	 */
+
+	{ 2312, 2372, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* W1_2312_2372 */
+	{ 2412, 2412, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* W1_2412_2412 */
+	{ 2417, 2432, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* W1_2417_2432 */
+	{ 2437, 2442, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* W1_2437_2442 */
+	{ 2447, 2457, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* W1_2447_2457 */
+	{ 2462, 2462, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* W1_2462_2462 */
+	{ 2467, 2467, 20, 0, 20, 5, NO_DFS, PSCAN_WWR | IS_ECM_CHAN, 0, 0}, /* W1_2467_2467 */
+	{ 2467, 2467, 20, 0, 20, 5, NO_DFS, NO_PSCAN | IS_ECM_CHAN, 0, 0},	/* W2_2467_2467 */
+	{ 2472, 2472, 20, 0, 20, 5, NO_DFS, PSCAN_WWR | IS_ECM_CHAN, 0, 0}, /* W1_2472_2472 */
+	{ 2472, 2472, 20, 0, 20, 5, NO_DFS, NO_PSCAN | IS_ECM_CHAN, 0, 0},	/* W2_2472_2472 */
+	{ 2484, 2484, 20, 0, 20, 5, NO_DFS, PSCAN_WWR | IS_ECM_CHAN, 0, 0}, /* W1_2484_2484 */
+	{ 2484, 2484, 20, 0, 20, 5, NO_DFS, NO_PSCAN | IS_ECM_CHAN, 0, 0},	/* W2_2484_2484 */
+};
+
+
+/*
+ * 2GHz 11g channel tags
+ */
+
+enum {
+	G1_2312_2372,
+	G2_2312_2372,
+
+	G1_2412_2472,
+	G2_2412_2472,
+	G3_2412_2472,
+
+	G1_2412_2462,
+	G2_2412_2462,
+
+	G1_2432_2442,
+
+	G1_2457_2472,
+
+	G1_2512_2732,
+
+	G1_2467_2472 ,
+
+	WG1_2312_2372,
+	WG1_2412_2412,
+	WG1_2417_2432,
+	WG1_2437_2442,
+	WG1_2447_2457,
+	WG1_2462_2462,
+	WG1_2467_2467,
+	WG2_2467_2467,
+	WG1_2472_2472,
+	WG2_2472_2472,
+
+};
+static REG_DMN_FREQ_BAND regDmn2Ghz11gFreq[] = {
+	{ 2312, 2372, 5,  6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G1_2312_2372 */
+	{ 2312, 2372, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G2_2312_2372 */
+
+	{ 2412, 2472, 5,  6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G1_2412_2472 */
+	{ 2412, 2472, 20, 0, 20, 5,  NO_DFS, PSCAN_MKKA_G, 0, 0},	/* G2_2412_2472 */
+	{ 2412, 2472, 30, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G3_2412_2472 */
+
+	{ 2412, 2462, 27, 6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G1_2412_2462 */
+	{ 2412, 2462, 20, 0, 20, 5, NO_DFS, PSCAN_MKKA_G, 0, 0},	/* G2_2412_2462 */
+	{ 2432, 2442, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G1_2432_2442 */
+
+	{ 2457, 2472, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G1_2457_2472 */
+
+	{ 2512, 2732, 5,  6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G1_2512_2732 */
+
+	{ 2467, 2472, 20, 0, 20, 5, NO_DFS, PSCAN_MKKA2 | PSCAN_MKKA, 0, 0 }, /* G1_2467_2472 */
+
+	/*
+	 * WWR open up the power to 20dBm
+	 */
+
+	{ 2312, 2372, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* WG1_2312_2372 */
+	{ 2412, 2412, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* WG1_2412_2412 */
+	{ 2417, 2432, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* WG1_2417_2432 */
+	{ 2437, 2442, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* WG1_2437_2442 */
+	{ 2447, 2457, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* WG1_2447_2457 */
+	{ 2462, 2462, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* WG1_2462_2462 */
+	{ 2467, 2467, 20, 0, 20, 5, NO_DFS, PSCAN_WWR | IS_ECM_CHAN, 0, 0}, /* WG1_2467_2467 */
+	{ 2467, 2467, 20, 0, 20, 5, NO_DFS, NO_PSCAN | IS_ECM_CHAN, 0, 0},	/* WG2_2467_2467 */
+	{ 2472, 2472, 20, 0, 20, 5, NO_DFS, PSCAN_WWR | IS_ECM_CHAN, 0, 0}, /* WG1_2472_2472 */
+	{ 2472, 2472, 20, 0, 20, 5, NO_DFS, NO_PSCAN | IS_ECM_CHAN, 0, 0},	/* WG2_2472_2472 */
+};
+/*
+ * 2GHz Dynamic turbo tags
+ */
+
+enum {
+	T1_2312_2372,
+	T1_2437_2437,
+	T2_2437_2437,
+	T3_2437_2437,
+	T1_2512_2732
+};
+
+static REG_DMN_FREQ_BAND regDmn2Ghz11gTurboFreq[] = {
+	{ 2312, 2372, 5,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},  /* T1_2312_2372 */
+	{ 2437, 2437, 5,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},  /* T1_2437_2437 */
+	{ 2437, 2437, 20, 6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},  /* T2_2437_2437 */
+	{ 2437, 2437, 18, 6, 40, 40, NO_DFS, PSCAN_WWR, 0, 0}, /* T3_2437_2437 */
+	{ 2512, 2732, 5,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},  /* T1_2512_2732 */
+};
+
+
+
+/*
+ * 2GHz 11n frequency tags
+ */
+enum {
+    NG1_2422_2452,
+    NG2_2422_2452,
+    NG3_2422_2452,
+
+    NG_DEMO_ALL_CHANNELS,
+};
+
+static REG_DMN_FREQ_BAND regDmn2Ghz11ngFreq[] = {
+    { 2422, 2452, 20, 0, 40, 5, NO_DFS, NO_PSCAN, 0, 0},    /* NG1_2422_2452 */
+    { 2422, 2452, 27, 0, 40, 5, NO_DFS, NO_PSCAN, 0, 0},    /* NG2_2422_2452 */
+    { 2422, 2452, 30, 0, 40, 5, NO_DFS, NO_PSCAN, 0, 0},    /* NG3_2422_2452 */
+
+	{ 2312, 2732, 27, 6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},    /* NG_DEMO_ALL_CHANNELS */
+};
+
+
+/*
+ * 5GHz 11n frequency tags
+ */
+enum {
+    NA1_5190_5230,
+    NA2_5190_5230,
+    NA3_5190_5230,
+    NA4_5190_5230,
+    NA5_5190_5230,
+
+    NA1_5270_5270,
+
+    NA1_5270_5310,
+    NA2_5270_5310,
+    NA3_5270_5310,
+    NA4_5270_5310,
+
+    NA1_5310_5310,
+
+    NA1_5510_5630,
+
+    NA1_5510_5670,
+    NA2_5510_5670,
+    NA3_5510_5670,
+
+    NA1_5755_5795,
+    NA2_5755_5795,
+    NA3_5755_5795,
+    NA4_5755_5795,
+    NA5_5755_5795,
+
+    NA1_5795_5795,
+
+    NA_DEMO_ALL_CHANNELS,
+};
+
+static REG_DMN_FREQ_BAND regDmn5Ghz11naFreq[] = {
+    /*
+     * ToDo: This table needs to be completely populated with 5GHz 11n properties
+     */
+	{ 5190, 5230, 15,  0, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	    /* NA1_5190_5230 */
+	{ 5190, 5230, 17,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* NA2_5190_5230 */
+	{ 5190, 5230, 18,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	    /* NA3_5190_5230 */
+	{ 5190, 5230, 20,  0, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* NA4_5190_5230 */
+	{ 5190, 5230, 23,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	    /* NA5_5190_5230 */
+
+	{ 5270, 5270, 23,  6, 40, 40, DFS_FCC3|DFS_ETSI, NO_PSCAN, 0, 1},  /* NA1_5270_5270 */
+
+	{ 5270, 5310, 18,  6, 40, 40, DFS_FCC3|DFS_ETSI, NO_PSCAN, 0, 1},  /* NA1_5270_5310 */
+	{ 5270, 5310, 20,  0, 40, 40, DFS_FCC3|DFS_ETSI|DFS_MKK4, NO_PSCAN, 0, 1},  /* NA2_5270_5310 */
+	{ 5270, 5310, 23,  6, 40, 40, DFS_FCC3|DFS_ETSI, NO_PSCAN, 0, 1},  /* NA3_5270_5310 */
+	{ 5270, 5310, 30,  6, 40, 40, DFS_FCC3|DFS_ETSI, NO_PSCAN, 0, 1},  /* NA4_5270_5310 */
+
+	{ 5310, 5310, 17,  6, 40, 40, DFS_FCC3|DFS_ETSI, NO_PSCAN, 0, 1},  /* NA1_5310_5310 */
+
+	{ 5510, 5630, 30,  6, 40, 40, DFS_FCC3|DFS_ETSI, NO_PSCAN, 0, 1},  /* NA1_5510_5630 */
+
+	{ 5510, 5670, 20,  6, 40, 40, DFS_FCC3|DFS_ETSI|DFS_MKK4, NO_PSCAN, 0, 1},  /* NA1_5510_5670 */
+	{ 5510, 5670, 27,  0, 40, 40, DFS_FCC3|DFS_ETSI, NO_PSCAN, 0, 1},  /* NA2_5510_5670 */
+	{ 5510, 5670, 30,  6, 40, 40, DFS_FCC3, NO_PSCAN, 0, 1},   /* NA3_5510_5670 */
+
+	{ 5755, 5795, 17,  0, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	    /* NA1_5755_5795 */
+	{ 5755, 5795, 20,  6, 40, 40, DFS_ETSI, NO_PSCAN, 0, 0},	    /* NA2_5755_5795 */
+	{ 5755, 5795, 23,  0, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	    /* NA3_5755_5795 */
+	{ 5755, 5795, 30,  0, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* NA4_5755_5795 */
+	{ 5755, 5795, 30,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	    /* NA5_5755_5795 */
+
+	{ 5795, 5795, 30,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	    /* NA1_5795_5795 */
+
+    { 4920, 6100, 30,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* NA_DEMO_ALL_CHANNELS */
+};
+
+typedef struct regDomain {
+	u16_t regDmnEnum;	/* value from EnumRd table */
+	u8_t conformanceTestLimit;
+	u64_t dfsMask;	/* DFS bitmask for 5Ghz tables */
+	u64_t pscan;	/* Bitmask for passive scan */
+	u32_t flags;	/* Requirement flags (AdHoc disallow, noise
+				   floor cal needed, etc) */
+	u64_t chan11a[BMLEN];/* 128 bit bitmask for channel/band
+				   selection */
+	u64_t chan11a_turbo[BMLEN];/* 128 bit bitmask for channel/band
+				   selection */
+	u64_t chan11a_dyn_turbo[BMLEN]; /* 128 bit bitmask for channel/band
+					       selection */
+	u64_t chan11b[BMLEN];/* 128 bit bitmask for channel/band
+				   selection */
+	u64_t chan11g[BMLEN];/* 128 bit bitmask for channel/band
+				   selection */
+	u64_t chan11g_turbo[BMLEN];/* 128 bit bitmask for channel/band
+					  selection */
+	u64_t chan11ng[BMLEN];/* 128 bit bitmask for 11n in 2GHz */
+	u64_t chan11na[BMLEN];/* 128 bit bitmask for 11n in 5GHz */
+} REG_DOMAIN;
+
+static REG_DOMAIN regDomains[] = {
+
+	{DEBUG_REG_DMN, FCC, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F1_5120_5240, F1_5260_5700, F1_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T1_5130_5210, T1_5250_5330, T1_5370_5490, T1_5530_5650, T1_5150_5190, T1_5230_5310, T1_5350_5470, T1_5510_5670, -1, -1, -1, -1),
+	 BM(T1_5200_5240, T1_5280_5280, T1_5540_5660, T1_5765_5805, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(F1_2312_2372, F1_2412_2472, F1_2484_2484, F1_2512_2732, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(G1_2312_2372, G1_2412_2472, G1_2512_2732, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T1_2312_2372, T1_2437_2437, T1_2512_2732, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(NG_DEMO_ALL_CHANNELS, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(NA_DEMO_ALL_CHANNELS, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL1, ETSI, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F4_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL2, ETSI, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F1_5745_5805, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA3_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL3, FCC, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F1_5280_5320, F2_5745_5805, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA1_5310_5310, NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL4, ETSI, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F4_5180_5240,  F3_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL5, ETSI, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F2_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA1_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL6, ETSI, DFS_ETSI, PSCAN_FCC_T | PSCAN_FCC , NO_REQ,
+	 BM(F4_5180_5240, F2_5260_5320, F3_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T2_5210_5210, T1_5250_5290, T1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, NA2_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL7, FCC, NO_DFS, PSCAN_FCC_T | PSCAN_FCC , NO_REQ,
+	 BM(F7_5260_5320, F4_5500_5700, F3_5745_5805, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA1_5310_5310, NA2_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+	{APL8, ETSI, NO_DFS, NO_PSCAN, DISALLOW_ADHOC_11A|DISALLOW_ADHOC_11A_TURB,
+	 BM(F6_5260_5320, F4_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5270_5310, NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL9, ETSI, DFS_ETSI, PSCAN_ETSI, DISALLOW_ADHOC_11A|DISALLOW_ADHOC_11A_TURB,
+	 BM(F1_5180_5320, F1_5500_5620, F3_5745_5805, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, NA1_5510_5630, NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{ETSI1, ETSI, DFS_ETSI, PSCAN_ETSI, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BM(W2_5180_5240, F2_5260_5320, F2_5500_5700, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, NA2_5510_5670, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{ETSI2, ETSI, DFS_ETSI, PSCAN_ETSI, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BM(F3_5180_5240, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA3_5190_5230, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{ETSI3, ETSI, DFS_ETSI, PSCAN_ETSI, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BM(W2_5180_5240, F2_5260_5320, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{ETSI4, ETSI, DFS_ETSI, PSCAN_ETSI, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BM(F3_5180_5240, F1_5260_5320, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA3_5190_5230, NA1_5270_5310, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{ETSI5, ETSI, DFS_ETSI, PSCAN_ETSI, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BM(F1_5180_5240, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA1_5190_5230, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{ETSI6, ETSI, DFS_ETSI, PSCAN_ETSI, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BM(F5_5180_5240, F1_5260_5280, F3_5500_5700, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA5_5190_5230, NA1_5270_5270, NA3_5510_5670, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{FCC1, FCC, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F2_5180_5240, F4_5260_5320, F5_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T1_5210_5210, T2_5250_5290, T2_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T1_5200_5240, T1_5280_5280, T1_5765_5805, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA2_5190_5230, NA3_5270_5310, NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{FCC2, FCC, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F6_5180_5240, F5_5260_5320, F6_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T2_5200_5240, T1_5280_5280, T1_5765_5805, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA5_5190_5230, NA3_5270_5310, NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{FCC3, FCC, DFS_FCC3, PSCAN_FCC | PSCAN_FCC_T, NO_REQ,
+	 BM(F2_5180_5240, F3_5260_5320, F1_5500_5700, F5_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T1_5210_5210, T1_5250_5250, T1_5290_5290, T2_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T1_5200_5240, T2_5280_5280, T1_5540_5660, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA2_5190_5230, NA2_5270_5310, NA3_5510_5670, NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{FCC4, FCC, DFS_FCC3, PSCAN_FCC | PSCAN_FCC_T, NO_REQ,
+	 BM(F1_4942_4987, F1_4945_4985, F1_4950_4980, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+	{FCC5, FCC, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F2_5180_5240, F5_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA2_5190_5230, NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+    {FCC6, FCC, DFS_FCC3, PSCAN_FCC, NO_REQ,
+	 BM(F7_5180_5240, F5_5260_5320, F1_5500_5580, F1_5660_5700, F6_5745_5825, -1, -1, -1, -1, -1, -1, -1),
+	 BM(-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T2_5200_5240, T1_5280_5280, T1_5765_5805, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+     BMZERO,
+     BMZERO,
+	 BM(NA5_5190_5230, NA5_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{MKK1, MKK, NO_DFS, PSCAN_MKK1, DISALLOW_ADHOC_11A_TURB,
+	 BM(F1_5170_5230, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+	{MKK2, MKK, NO_DFS, PSCAN_MKK2, DISALLOW_ADHOC_11A_TURB,
+	 BM(F1_4915_4925, F1_4935_4945, F1_4920_4980, F1_5035_5040, F1_5055_5055, F1_5040_5080, F1_5170_5230, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+	/* UNI-1 even */
+	{MKK3, MKK, NO_DFS, PSCAN_MKK3, DISALLOW_ADHOC_11A_TURB,
+	 BM(F4_5180_5240, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	/* UNI-1 even + UNI-2 */
+	{MKK4, MKK, DFS_MKK4, PSCAN_MKK3, DISALLOW_ADHOC_11A_TURB,
+	 BM(F4_5180_5240, F2_5260_5320, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	/* UNI-1 even + UNI-2 + mid-band */
+	{MKK5, MKK, DFS_MKK4, PSCAN_MKK3, DISALLOW_ADHOC_11A_TURB,
+	 BM(F4_5180_5240, F2_5260_5320, F4_5500_5700, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, NA1_5510_5670, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	/* UNI-1 odd + even */
+	{MKK6, MKK, DFS_MKK4, PSCAN_MKK1, DISALLOW_ADHOC_11A_TURB,
+	 BM(F2_5170_5230, F4_5180_5240, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	/* UNI-1 odd + UNI-1 even + UNI-2 */
+	{MKK7, MKK, DFS_MKK4, PSCAN_MKK1 | PSCAN_MKK3 , DISALLOW_ADHOC_11A_TURB,
+	 BM(F2_5170_5230, F4_5180_5240, F2_5260_5320, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	/* UNI-1 odd + UNI-1 even + UNI-2 + mid-band */
+	{MKK8, MKK, DFS_MKK4, PSCAN_MKK1 | PSCAN_MKK3 , DISALLOW_ADHOC_11A_TURB,
+	 BM(F2_5170_5230, F4_5180_5240, F2_5260_5320, F4_5500_5700, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, NA1_5510_5670, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+    /* UNI-1 even + 4.9 GHZ */
+    {MKK9, MKK, NO_DFS, NO_PSCAN, DISALLOW_ADHOC_11A_TURB,
+     BM(F1_4915_4925, F1_4935_4945, F1_4920_4980, F1_5035_5040, F1_5055_5055, F1_5040_5080, F4_5180_5240, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+    /* UNI-1 even + UNI-2 + 4.9 GHZ */
+	{MKK10, MKK, DFS_MKK4, PSCAN_MKK3, DISALLOW_ADHOC_11A_TURB,
+	 BM(F1_4915_4925, F1_4935_4945, F1_4920_4980, F1_5035_5040, F1_5055_5055, F1_5040_5080, F4_5180_5240, F2_5260_5320, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+	/* UNI-1 even + UNI-2 + 4.9 GHZ + mid-band */
+	{MKK11, MKK, DFS_MKK4, PSCAN_MKK3, DISALLOW_ADHOC_11A_TURB,
+	 BM(F1_4915_4925, F1_4935_4945, F1_4920_4980, F1_5035_5040, F1_5055_5055, F1_5040_5080, F4_5180_5240, F2_5260_5320, F4_5500_5700, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+	/* UNI-1 even + UNI-1 odd + UNI-2 + 4.9 GHZ + mid-band */
+	{MKK12, MKK, DFS_MKK4, PSCAN_MKK3, DISALLOW_ADHOC_11A_TURB,
+	 BM(F1_4915_4925, F1_4935_4945, F1_4920_4980, F1_5035_5040, F1_5055_5055, F1_5040_5080, F1_5170_5230, F4_5180_5240, F2_5260_5320, F4_5500_5700, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+	/* Defined here to use when 2G channels are authorised for country K2 */
+	{APLD, NO_CTL, NO_DFS, NO_PSCAN, NO_REQ,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F2_2312_2372,F2_2412_2472, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(G2_2312_2372,G2_2412_2472, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+	{ETSIA, NO_CTL, NO_DFS, PSCAN_ETSIA, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F1_2457_2472,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(G1_2457_2472,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T2_2437_2437,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{ETSIB, ETSI, NO_DFS, PSCAN_ETSIB, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F1_2432_2442,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(G1_2432_2442,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(T2_2437_2437,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{ETSIC, ETSI, NO_DFS, PSCAN_ETSIC, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F3_2412_2472,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(G3_2412_2472,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(T2_2437_2437,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{FCCA, FCC, NO_DFS, NO_PSCAN, NO_REQ,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F1_2412_2462,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(G1_2412_2462,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(T2_2437_2437,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(NG2_2422_2452,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO},
+
+	{MKKA, MKK, NO_DFS, PSCAN_MKKA | PSCAN_MKKA_G | PSCAN_MKKA1 | PSCAN_MKKA1_G | PSCAN_MKKA2 | PSCAN_MKKA2_G, DISALLOW_ADHOC_11A_TURB,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F2_2412_2462, F1_2467_2472, F2_2484_2484, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(G2_2412_2462, G1_2467_2472, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T2_2437_2437,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(NG1_2422_2452,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO},
+
+	{MKKC, MKK, NO_DFS, NO_PSCAN, NO_REQ,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F2_2412_2472,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(G2_2412_2472,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(T2_2437_2437,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(NG1_2422_2452,-1,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO},
+
+	{WORLD, ETSI, NO_DFS, NO_PSCAN, NO_REQ,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F2_2412_2472,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(G2_2412_2472,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(T2_2437_2437,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(NG1_2422_2452,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO},
+
+	{WOR0_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_PER_11D,
+	 BM(W1_5260_5320, W1_5180_5240, W1_5170_5230, W1_5745_5825, W1_5500_5700, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412,W1_2437_2442,W1_2462_2462,W1_2472_2472,W1_2417_2432, W1_2447_2457, W1_2467_2467, W1_2484_2484, -1, -1, -1, -1),
+	 BM(WG1_2412_2412,WG1_2437_2442,WG1_2462_2462,WG1_2472_2472,WG1_2417_2432,WG1_2447_2457,WG1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR01_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_PER_11D,
+	 BM(W1_5260_5320, W1_5180_5240, W1_5170_5230, W1_5745_5825, W1_5500_5700, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412, W1_2437_2442, W1_2462_2462, W1_2417_2432, W1_2447_2457, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412, WG1_2437_2442, WG1_2462_2462, WG1_2417_2432, WG1_2447_2457, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR02_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_PER_11D,
+	 BM(W1_5260_5320, W1_5180_5240,W1_5170_5230,W1_5745_5825,W1_5500_5700, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412,W1_2437_2442,W1_2462_2462, W1_2472_2472,W1_2417_2432, W1_2447_2457, W1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412,WG1_2437_2442,WG1_2462_2462, WG1_2472_2472,WG1_2417_2432, WG1_2447_2457, WG1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{EU1_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_PER_11D,
+	 BM(W1_5260_5320, W1_5180_5240,W1_5170_5230,W1_5745_5825,W1_5500_5700, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412,W1_2437_2442,W1_2462_2462, W2_2472_2472,W1_2417_2432, W1_2447_2457, W2_2467_2467, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412,WG1_2437_2442,WG1_2462_2462, WG2_2472_2472,WG1_2417_2432, WG1_2447_2457, WG2_2467_2467, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR1_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_NO_11A,
+	 BM(W1_5260_5320, W1_5180_5240, W1_5170_5230, W1_5745_5825, W1_5500_5700, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BM(W1_2412_2412,W1_2437_2442,W1_2462_2462,W1_2472_2472,W1_2417_2432, W1_2447_2457, W1_2467_2467, W1_2484_2484, -1, -1, -1, -1),
+	 BM(WG1_2412_2412,WG1_2437_2442,WG1_2462_2462,WG1_2472_2472,WG1_2417_2432,WG1_2447_2457,WG1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR2_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_NO_11A,
+	 BM(W1_5260_5320, W1_5180_5240, W1_5170_5230, W1_5745_5825, W1_5500_5700, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412,W1_2437_2442,W1_2462_2462,W1_2472_2472,W1_2417_2432, W1_2447_2457, W1_2467_2467, W1_2484_2484, -1, -1, -1, -1),
+	 BM(WG1_2412_2412,WG1_2437_2442,WG1_2462_2462,WG1_2472_2472,WG1_2417_2432,WG1_2447_2457,WG1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR3_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_PER_11D,
+	 BM(W1_5260_5320, W1_5180_5240, W1_5170_5230, W1_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412,W1_2437_2442,W1_2462_2462,W1_2472_2472,W1_2417_2432, W1_2447_2457, W1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412,WG1_2437_2442,WG1_2462_2462,WG1_2472_2472,WG1_2417_2432,WG1_2447_2457,WG1_2467_2467,-1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR4_WORLD, NO_CTL, DFS_FCC3, PSCAN_WWR, ADHOC_NO_11A,
+	 BM(W2_5260_5320, W2_5180_5240, F2_5745_5805, W2_5825_5825, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412,W1_2437_2442,W1_2462_2462, W1_2417_2432,W1_2447_2457,-1, -1, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412,WG1_2437_2442,WG1_2462_2462, WG1_2417_2432,WG1_2447_2457,-1, -1, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR5_ETSIC, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_NO_11A,
+	 BM(W1_5260_5320, W2_5180_5240, F6_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BM(W1_2412_2412, W1_2437_2442, W1_2462_2462, W2_2472_2472, W1_2417_2432, W1_2447_2457, W2_2467_2467, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412, WG1_2437_2442, WG1_2462_2462, WG1_2472_2472, WG1_2417_2432, WG1_2447_2457, WG1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR9_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_NO_11A,
+	 BM(W1_5260_5320, W1_5180_5240, W1_5745_5825, W1_5500_5700, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412, W1_2437_2442, W1_2462_2462, W1_2417_2432, W1_2447_2457, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412, WG1_2437_2442, WG1_2462_2462, WG1_2417_2432, WG1_2447_2457, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WORA_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_NO_11A,
+	 BM(W1_5260_5320, W1_5180_5240, W1_5745_5825, W1_5500_5700, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BM(W1_2412_2412, W1_2437_2442, W1_2462_2462, W1_2472_2472, W1_2417_2432, W1_2447_2457, W1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412, WG1_2437_2442, WG1_2462_2462, WG1_2472_2472, WG1_2417_2432, WG1_2447_2457, WG1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{NULL1, NO_CTL, NO_DFS, NO_PSCAN, NO_REQ,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+};
+
+struct cmode {
+	u16_t	mode;
+	u32_t	flags;
+};
+
+static const struct cmode modes[] = {
+	{ HAL_MODE_TURBO,	CHANNEL_ST},	/* TURBO means 11a Static Turbo */
+	{ HAL_MODE_11A,		CHANNEL_A},
+	{ HAL_MODE_11B,		CHANNEL_B},
+	{ HAL_MODE_11G,		CHANNEL_G},
+	{ HAL_MODE_11G_TURBO,	CHANNEL_108G},
+	{ HAL_MODE_11A_TURBO,	CHANNEL_108A},
+	{ HAL_MODE_11NA,	CHANNEL_A_HT40},
+	{ HAL_MODE_11NA,	CHANNEL_A_HT20},
+	{ HAL_MODE_11NG,	CHANNEL_G_HT40},
+	{ HAL_MODE_11NG,	CHANNEL_G_HT20},
+};
+
+/*
+ * Return the Wireless Mode Regulatory Domain based
+ * on the country code and the wireless mode.
+ */
+u8_t GetWmRD(u16_t regionCode, u16_t channelFlag, REG_DOMAIN *rd)
+{
+	s16_t i, found, regDmn;
+	u64_t flags=NO_REQ;
+	REG_DMN_PAIR_MAPPING *regPair=NULL;
+
+	for (i=0, found=0; (i<N(regDomainPairs))&&(!found); i++)
+	{
+		if (regDomainPairs[i].regDmnEnum == regionCode)
+		{
+			regPair = &regDomainPairs[i];
+			found = 1;
+		}
+	}
+	if (!found)
+	{
+		zm_debug_msg1("Failed to find reg domain pair ", regionCode);
+		return FALSE;
+	}
+
+	if (channelFlag & ZM_REG_FLAG_CHANNEL_2GHZ)
+	{
+		regDmn = regPair->regDmn2GHz;
+		flags = regPair->flags2GHz;
+	}
+    else
+	{
+		regDmn = regPair->regDmn5GHz;
+		flags = regPair->flags5GHz;
+	}
+
+	/*
+	 * We either started with a unitary reg domain or we've found the
+	 * unitary reg domain of the pair
+	 */
+
+	for (i=0;i<N(regDomains); i++)
+	{
+		if (regDomains[i].regDmnEnum == regDmn)
+		{
+			if (rd != NULL)
+			{
+				zfMemoryCopy((u8_t *)rd, (u8_t *)&regDomains[i],
+					  sizeof(REG_DOMAIN));
+			}
+		}
+	}
+	rd->pscan &= regPair->pscanMask;
+    rd->flags = (u32_t)flags;
+	return TRUE;
+}
+
+/*
+ * Test to see if the bitmask array is all zeros
+ */
+u8_t isChanBitMaskZero(u64_t *bitmask)
+{
+	u16_t i;
+
+	for (i=0; i<BMLEN; i++) {
+		if (bitmask[i] != 0)
+			return FALSE;
+	}
+	return TRUE;
+}
+
+u8_t IS_BIT_SET(u32_t bit, u64_t *bitmask)
+{
+	u32_t byteOffset, bitnum;
+	u64_t val;
+
+	byteOffset = bit/64;
+	bitnum = bit - byteOffset*64;
+	val = ((u64_t) 1) << bitnum;
+	if (bitmask[byteOffset] & val)
+		return TRUE;
+	else
+		return FALSE;
+}
+
+
+void zfHpGetRegulationTable(zdev_t* dev, u16_t regionCode, u16_t c_lo, u16_t c_hi)
+{
+	REG_DOMAIN rd5GHz, rd2GHz;
+	const struct cmode *cm;
+	s16_t next=0,b;
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zmw_declare_for_critical_section();
+
+	if (!GetWmRD(regionCode, ~ZM_REG_FLAG_CHANNEL_2GHZ, &rd5GHz))
+	{
+        zm_debug_msg1("couldn't find unitary 5GHz reg domain for Region Code ", regionCode);
+		return;
+	}
+	if (!GetWmRD(regionCode, ZM_REG_FLAG_CHANNEL_2GHZ, &rd2GHz))
+	{
+        zm_debug_msg1("couldn't find unitary 2GHz reg domain for Region Code ", regionCode);
+		return;
+	}
+    if (wd->regulationTable.regionCode == regionCode)
+    {
+        zm_debug_msg1("current region code is the same with Region Code ", regionCode);
+        return;
+    }
+    else
+    {
+        wd->regulationTable.regionCode = regionCode;
+    }
+
+    next = 0;
+
+    zmw_enter_critical_section(dev);
+
+	for (cm = modes; cm < &modes[N(modes)]; cm++)
+	{
+		u16_t c;
+		u64_t *channelBM=NULL;
+		REG_DOMAIN *rd=NULL;
+		REG_DMN_FREQ_BAND *fband=NULL,*freqs=NULL;
+
+		switch (cm->mode)
+		{
+		case HAL_MODE_TURBO:
+		    //we don't have turbo mode so we disable it
+            //zm_debug_msg0("CWY - HAL_MODE_TURBO");
+            channelBM = NULL;
+			//rd = &rd5GHz;
+			//channelBM = rd->chan11a_turbo;
+			//freqs = &regDmn5GhzTurboFreq[0];
+			//ctl = rd->conformanceTestLimit | CTL_TURBO;
+			break;
+		case HAL_MODE_11A:
+		    if ((hpPriv->OpFlags & 0x1) != 0)
+		    {
+    			rd = &rd5GHz;
+    			channelBM = rd->chan11a;
+    			freqs = &regDmn5GhzFreq[0];
+    			c_lo = 4920; //from channel 184
+    			c_hi = 5825; //to   channel 165
+    			//ctl = rd->conformanceTestLimit;
+                //zm_debug_msg2("CWY - HAL_MODE_11A, channelBM = 0x", *channelBM);
+            }
+            //else
+            {
+                //channelBM = NULL;
+            }
+			break;
+		case HAL_MODE_11B:
+		    //Disable 11B mode because it only has difference with 11G in PowerDFS Data,
+		    //and we don't use this now.
+			//zm_debug_msg0("CWY - HAL_MODE_11B");
+			channelBM = NULL;
+			//rd = &rd2GHz;
+			//channelBM = rd->chan11b;
+			//freqs = &regDmn2GhzFreq[0];
+			//ctl = rd->conformanceTestLimit | CTL_11B;
+            //zm_debug_msg2("CWY - HAL_MODE_11B, channelBM = 0x", *channelBM);
+			break;
+		case HAL_MODE_11G:
+		    if ((hpPriv->OpFlags & 0x2) != 0)
+		    {
+    			rd = &rd2GHz;
+    			channelBM = rd->chan11g;
+    			freqs = &regDmn2Ghz11gFreq[0];
+    			c_lo = 2412; //from channel  1
+    			//c_hi = 2462; //to   channel 11
+                c_hi = 2472; //to   channel 13
+    			//ctl = rd->conformanceTestLimit | CTL_11G;
+                //zm_debug_msg2("CWY - HAL_MODE_11G, channelBM = 0x", *channelBM);
+            }
+            //else
+            {
+                //channelBM = NULL;
+            }
+			break;
+		case HAL_MODE_11G_TURBO:
+		    //we don't have turbo mode so we disable it
+            //zm_debug_msg0("CWY - HAL_MODE_11G_TURBO");
+            channelBM = NULL;
+			//rd = &rd2GHz;
+			//channelBM = rd->chan11g_turbo;
+			//freqs = &regDmn2Ghz11gTurboFreq[0];
+			//ctl = rd->conformanceTestLimit | CTL_108G;
+			break;
+		case HAL_MODE_11A_TURBO:
+		    //we don't have turbo mode so we disable it
+            //zm_debug_msg0("CWY - HAL_MODE_11A_TURBO");
+            channelBM = NULL;
+			//rd = &rd5GHz;
+			//channelBM = rd->chan11a_dyn_turbo;
+			//freqs = &regDmn5GhzTurboFreq[0];
+			//ctl = rd->conformanceTestLimit | CTL_108G;
+			break;
+		default:
+            zm_debug_msg1("Unkonwn HAL mode ", cm->mode);
+			continue;
+		}
+		if (channelBM == NULL)
+		{
+		    //zm_debug_msg0("CWY - channelBM is NULL");
+		    continue;
+        }
+        if (isChanBitMaskZero(channelBM))
+        {
+	        //zm_debug_msg0("CWY - BitMask is Zero");
+	        continue;
+        }
+
+        // RAY:Is it ok??
+        if (freqs == NULL )
+        {
+            continue;
+        }
+
+		for (b=0;b<64*BMLEN; b++)
+		{
+			if (IS_BIT_SET(b,channelBM))
+			{
+				fband = &freqs[b];
+
+				//zm_debug_msg1("CWY - lowChannel = ", fband->lowChannel);
+				//zm_debug_msg1("CWY - highChannel = ", fband->highChannel);
+				//zm_debug_msg1("CWY - channelSep = ", fband->channelSep);
+				for (c=fband->lowChannel; c <= fband->highChannel;
+				     c += fband->channelSep)
+				{
+					ZM_HAL_CHANNEL icv;
+
+                    //Disable all DFS channel
+                    if ((hpPriv->disableDfsCh==0) || (!(fband->useDfs & rd->dfsMask)))
+                    {
+                        if( fband->channelBW < 20 )
+                        {
+                        	/**************************************************************/
+                            /*                                                            */
+                            /*   Temporary discard channel that BW < 20MHz (5 or 10MHz)   */
+                            /*   Our architecture does not implemnt it !!!                */
+                            /*                                                            */
+                            /**************************************************************/
+                            continue;
+                        }
+					if ((c >= c_lo) && (c <= c_hi))
+					{
+					    icv.channel = c;
+					    icv.channelFlags = cm->flags;
+					    icv.maxRegTxPower = fband->powerDfs;
+					    if (fband->usePassScan & rd->pscan)
+					    	icv.channelFlags |= ZM_REG_FLAG_CHANNEL_PASSIVE;
+					    else
+					    	icv.channelFlags &= ~ZM_REG_FLAG_CHANNEL_PASSIVE;
+					    if (fband->useDfs & rd->dfsMask)
+					    	icv.privFlags = ZM_REG_FLAG_CHANNEL_DFS;
+					    else
+					    	icv.privFlags = 0;
+
+					    /* For now disable radar for FCC3 */
+					    if (fband->useDfs & rd->dfsMask & DFS_FCC3)
+					    {
+					    	icv.privFlags &= ~ZM_REG_FLAG_CHANNEL_DFS;
+					    	icv.privFlags |= ZM_REG_FLAG_CHANNEL_DFS_CLEAR;
+					    }
+
+					    if(rd->flags & LIMIT_FRAME_4MS)
+					    	icv.privFlags |= ZM_REG_FLAG_CHANNEL_DFS_CLEAR;
+
+					    icv.minTxPower = 0;
+					    icv.maxTxPower = 0;
+
+        			    zm_assert(next < 60);
+
+					    wd->regulationTable.allowChannel[next++] = icv;
+				    }
+				}
+			}
+		}
+	}
+	}
+	wd->regulationTable.allowChannelCnt = next;
+
+    #if 0
+    {
+        /* debug print */
+        u32_t i;
+        DbgPrint("\n-------------------------------------------\n");
+        DbgPrint("zfHpGetRegulationTable print all channel info regincode = 0x%x\n", wd->regulationTable.regionCode);
+        DbgPrint("index  channel  channelFlags   maxRegTxPower  privFlags  useDFS\n");
+
+        for (i=0; i<wd->regulationTable.allowChannelCnt; i++)
+        {
+            DbgPrint("%02d       %d         %04x           %02d        %x     %x\n",
+                      i,
+                      wd->regulationTable.allowChannel[i].channel,
+                      wd->regulationTable.allowChannel[i].channelFlags,
+                      wd->regulationTable.allowChannel[i].maxRegTxPower,
+                      wd->regulationTable.allowChannel[i].privFlags,
+                      wd->regulationTable.allowChannel[i].privFlags & ZM_REG_FLAG_CHANNEL_DFS);
+        }
+    }
+    #endif
+
+    zmw_leave_critical_section(dev);
+}
+
+void zfHpGetRegulationTablefromRegionCode(zdev_t* dev, u16_t regionCode)
+{
+    u16_t c_lo = 2000, c_hi = 6000; //default channel is all enable
+    u8_t isoName[3] = {'N', 'A', 0};
+
+    zfCoreSetIsoName(dev, isoName);
+
+    zfHpGetRegulationTable(dev, regionCode, c_lo, c_hi);
+}
+
+void zfHpGetRegulationTablefromCountry(zdev_t* dev, u16_t CountryCode)
+{
+    u16_t i;
+    u16_t c_lo = 2000, c_hi = 6000; //default channel is all enable
+    u16_t RegDomain;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    for (i = 0; i < N(allCountries); i++)
+    {
+        if (CountryCode == allCountries[i].countryCode)
+        {
+            RegDomain = allCountries[i].regDmnEnum;
+
+            // read the ACU country code from EEPROM
+            zfCoreSetIsoName(dev, (u8_t*)allCountries[i].isoName);
+
+            //zm_debug_msg_s("CWY - Country Name = ", allCountries[i].name);
+
+            if (wd->regulationTable.regionCode != RegDomain)
+            {
+                //zm_debug_msg0("CWY - Change regulatory table");
+
+                zfHpGetRegulationTable(dev, RegDomain, c_lo, c_hi);
+            }
+            return;
+        }
+    }
+    zm_debug_msg1("Invalid CountryCode = ", CountryCode);
+}
+
+u8_t zfHpGetRegulationTablefromISO(zdev_t* dev, u8_t *countryInfo, u8_t length)
+{
+    u16_t i;
+    u16_t RegDomain;
+    u16_t c_lo = 2000, c_hi = 6000; //default channel is all enable
+    //u8_t strLen = 2;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if (countryInfo[4] != 0x20)
+    { // with (I)ndoor/(O)utdoor info
+        //strLen = 3;
+    }
+    //zm_debug_msg_s("Desired iso name = ", isoName);
+    for (i = 0; i < N(allCountries); i++)
+    {
+        //zm_debug_msg_s("Current iso name = ", allCountries[i].isoName);
+        if (zfMemoryIsEqual((u8_t *)allCountries[i].isoName, (u8_t *)&countryInfo[2], length-1))
+        {
+            //DbgPrint("Set current iso name = %s\n", allCountries[i].isoName);
+            //zm_debug_msg0("iso name hit!!");
+
+            RegDomain = allCountries[i].regDmnEnum;
+
+            if (wd->regulationTable.regionCode != RegDomain)
+            {
+                zfHpGetRegulationTable(dev, RegDomain, c_lo, c_hi);
+            }
+
+            //while (index < (countryInfo[1]+2))
+            //{
+            //  if (countryInfo[index] <= 14)
+            //  {
+            //        /* calculate 2.4GHz low boundary channel frequency */
+            //        ch = countryInfo[index];
+            //        if ( ch == 14 )
+            //            c_lo = ZM_CH_G_14;
+            //        else
+            //            c_lo = ZM_CH_G_1 + (ch - 1) * 5;
+            //        /* calculate 2.4GHz high boundary channel frequency */
+            //        ch = countryInfo[index] + countryInfo[index + 1] - 1;
+            //        if ( ch == 14 )
+            //            c_hi = ZM_CH_G_14;
+            //        else
+            //            c_hi = ZM_CH_G_1 + (ch - 1) * 5;
+            //  }
+            //  else
+            //  {
+            //        /* calculate 5GHz low boundary channel frequency */
+            //        ch = countryInfo[index];
+            //        if ( (ch >= 184)&&(ch <= 196) )
+            //            c_lo = 4000 + ch*5;
+            //        else
+            //            c_lo = 5000 + ch*5;
+            //        /* calculate 5GHz high boundary channel frequency */
+            //        ch = countryInfo[index] + countryInfo[index + 1] - 1;
+            //        if ( (ch >= 184)&&(ch <= 196) )
+            //            c_hi = 4000 + ch*5;
+            //        else
+            //            c_hi = 5000 + ch*5;
+            //  }
+            //
+            //  zfHpGetRegulationTable(dev, RegDomain, c_lo, c_hi);
+            //
+            //  index+=3;
+            //}
+
+            return 0;
+        }
+    }
+    //zm_debug_msg_s("Invalid iso name = ", &countryInfo[2]);
+    return 1;
+}
+
+const char* zfHpGetisoNamefromregionCode(zdev_t* dev, u16_t regionCode)
+{
+    u16_t i;
+
+    for (i = 0; i < N(allCountries); i++)
+    {
+        if (allCountries[i].regDmnEnum == regionCode)
+        {
+            return allCountries[i].isoName;
+        }
+    }
+    /* no matching item, return default */
+    return allCountries[0].isoName;
+}
+
+u16_t zfHpGetRegionCodeFromIsoName(zdev_t* dev, u8_t *countryIsoName)
+{
+    u16_t i;
+    u16_t regionCode;
+
+    /* if no matching item, return default */
+    regionCode = DEF_REGDMN;
+
+    for (i = 0; i < N(allCountries); i++)
+    {
+        if (zfMemoryIsEqual((u8_t *)allCountries[i].isoName, countryIsoName, 2))
+        {
+            regionCode = allCountries[i].regDmnEnum;
+            break;
+        }
+    }
+
+    return regionCode;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfHpDeleteAllowChannel      */
+/*      Delete Allow Channel.                                           */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev  : device pointer                                           */
+/*      freq : frequency                                                */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Chao-Wen Yang         ZyDAS Technology Corporation    2007.3    */
+/*                                                                      */
+/************************************************************************/
+u16_t zfHpDeleteAllowChannel(zdev_t* dev, u16_t freq)
+{
+    u16_t i, bandIndex = 0;
+    u16_t dfs5GBand[][2] = {{5150, 5240}, {5260, 5350}, {5450, 5700}, {5725, 5825}};
+
+    zmw_get_wlan_dev(dev);
+    /* Find which band does this frequency belong */
+    for (i = 0; i < 4; i++)
+    {
+        if ((freq >= dfs5GBand[i][0]) && (freq <= dfs5GBand[i][1]))
+            bandIndex = i + 1;
+    }
+
+    if (bandIndex == 0)
+    {
+        /* 2.4G, don't care */
+        return 0;
+    }
+    else
+    {
+        bandIndex--;
+    }
+    /* Set all channels in this band to passive scan */
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if ((wd->regulationTable.allowChannel[i].channel >= dfs5GBand[bandIndex][0]) &&
+            (wd->regulationTable.allowChannel[i].channel <= dfs5GBand[bandIndex][1]))
+        {
+            /* if channel is not passive, set it to be passive and mark it */
+            if ((wd->regulationTable.allowChannel[i].channelFlags &
+                    ZM_REG_FLAG_CHANNEL_PASSIVE) == 0)
+            {
+                wd->regulationTable.allowChannel[i].channelFlags |=
+                        (ZM_REG_FLAG_CHANNEL_PASSIVE | ZM_REG_FLAG_CHANNEL_CSA);
+            }
+        }
+    }
+
+    return 0;
+}
+
+u16_t zfHpAddAllowChannel(zdev_t* dev, u16_t freq)
+{
+    u16_t i, j, arrayIndex;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if (wd->regulationTable.allowChannel[i].channel == freq)
+            break;
+    }
+
+    if ( i == wd->regulationTable.allowChannelCnt)
+    {
+        for (j = 0; j < wd->regulationTable.allowChannelCnt; j++)
+        {
+            if (wd->regulationTable.allowChannel[j].channel > freq)
+                break;
+        }
+
+        //zm_debug_msg1("CWY - add frequency = ", freq);
+        //zm_debug_msg1("CWY - channel array index = ", j);
+
+        arrayIndex = j;
+
+        if (arrayIndex < wd->regulationTable.allowChannelCnt)
+        {
+            for (j = wd->regulationTable.allowChannelCnt; j > arrayIndex; j--)
+                wd->regulationTable.allowChannel[j] = wd->regulationTable.allowChannel[j - 1];
+        }
+        wd->regulationTable.allowChannel[arrayIndex].channel = freq;
+
+        wd->regulationTable.allowChannelCnt++;
+    }
+
+    return 0;
+}
+
+u16_t zfHpIsDfsChannelNCS(zdev_t* dev, u16_t freq)
+{
+    u8_t flag = ZM_REG_FLAG_CHANNEL_DFS;
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        //DbgPrint("DFS:freq=%d, chan=%d", freq, wd->regulationTable.allowChannel[i].channel);
+        if (wd->regulationTable.allowChannel[i].channel == freq)
+        {
+            flag = wd->regulationTable.allowChannel[i].privFlags;
+            break;
+        }
+    }
+
+    return (flag & (ZM_REG_FLAG_CHANNEL_DFS|ZM_REG_FLAG_CHANNEL_DFS_CLEAR));
+}
+
+u16_t zfHpIsDfsChannel(zdev_t* dev, u16_t freq)
+{
+    u8_t flag = ZM_REG_FLAG_CHANNEL_DFS;
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        //DbgPrint("DFS:freq=%d, chan=%d", freq, wd->regulationTable.allowChannel[i].channel);
+        if (wd->regulationTable.allowChannel[i].channel == freq)
+        {
+            flag = wd->regulationTable.allowChannel[i].privFlags;
+            break;
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return (flag & (ZM_REG_FLAG_CHANNEL_DFS|ZM_REG_FLAG_CHANNEL_DFS_CLEAR));
+}
+
+u16_t zfHpIsAllowedChannel(zdev_t* dev, u16_t freq)
+{
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if (wd->regulationTable.allowChannel[i].channel == freq)
+        {
+            return 1;
+        }
+    }
+
+    return 0;
+}
+
+u16_t zfHpFindFirstNonDfsChannel(zdev_t* dev, u16_t aBand)
+{
+    u16_t chan = 2412;
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if ((wd->regulationTable.allowChannel[i].privFlags & ZM_REG_FLAG_CHANNEL_DFS) != 0)
+        {
+            if (aBand)
+            {
+                if (wd->regulationTable.allowChannel[i].channel > 3000)
+                {
+                    chan = wd->regulationTable.allowChannel[i].channel;
+                    break;
+                }
+            }
+            else
+            {
+                if (wd->regulationTable.allowChannel[i].channel < 3000)
+                {
+                    chan = wd->regulationTable.allowChannel[i].channel;
+                    break;
+                }
+            }
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return chan;
+}
+
+
+/* porting from ACU */
+/* save RegulatoryDomain in hpriv */
+u8_t zfHpGetRegulatoryDomain(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch (wd->regulationTable.regionCode)
+    {
+        case NO_ENUMRD:
+            return 0;
+            break;
+        case FCC1_FCCA:
+        case FCC1_WORLD:
+        case FCC4_FCCA:
+        case FCC5_FCCA:
+        case FCC2_WORLD:
+        case FCC2_ETSIC:
+        case FCC3_FCCA:
+        case FCC3_WORLD:
+        case FCC1:
+        case FCC2:
+        case FCC3:
+        case FCC4:
+        case FCC5:
+        case FCCA:
+            return 0x10;//WG_AMERICAS DOT11_REG_DOMAIN_FCC  United States
+            break;
+
+        case FCC2_FCCA:
+            return 0x20;//DOT11_REG_DOMAIN_DOC  Canada
+            break;
+
+        case ETSI1_WORLD:
+        case ETSI3_ETSIA:
+        case ETSI2_WORLD:
+        case ETSI3_WORLD:
+        case ETSI4_WORLD:
+        case ETSI4_ETSIC:
+        case ETSI5_WORLD:
+        case ETSI6_WORLD:
+        case ETSI_RESERVED:
+        case ETSI1:
+        case ETSI2:
+        case ETSI3:
+        case ETSI4:
+        case ETSI5:
+        case ETSI6:
+        case ETSIA:
+        case ETSIB:
+        case ETSIC:
+            return 0x30;//WG_EMEA DOT11_REG_DOMAIN_ETSI  Most of Europe
+            break;
+
+        case MKK1_MKKA:
+        case MKK1_MKKB:
+        case MKK2_MKKA:
+        case MKK1_FCCA:
+        case MKK1_MKKA1:
+        case MKK1_MKKA2:
+        case MKK1_MKKC:
+        case MKK3_MKKB:
+        case MKK3_MKKA2:
+        case MKK3_MKKC:
+        case MKK4_MKKB:
+        case MKK4_MKKA2:
+        case MKK4_MKKC:
+        case MKK5_MKKB:
+        case MKK5_MKKA2:
+        case MKK5_MKKC:
+        case MKK6_MKKB:
+        case MKK6_MKKA2:
+        case MKK6_MKKC:
+        case MKK7_MKKB:
+        case MKK7_MKKA:
+        case MKK7_MKKC:
+        case MKK8_MKKB:
+        case MKK8_MKKA2:
+        case MKK8_MKKC:
+        case MKK6_MKKA1:
+        case MKK6_FCCA:
+        case MKK7_MKKA1:
+        case MKK7_FCCA:
+        case MKK9_FCCA:
+        case MKK9_MKKA1:
+        case MKK9_MKKC:
+        case MKK9_MKKA2:
+        case MKK10_FCCA:
+        case MKK10_MKKA1:
+        case MKK10_MKKC:
+        case MKK10_MKKA2:
+        case MKK11_MKKA:
+        case MKK11_FCCA:
+        case MKK11_MKKA1:
+        case MKK11_MKKC:
+        case MKK11_MKKA2:
+        case MKK12_MKKA:
+        case MKK12_FCCA:
+        case MKK12_MKKA1:
+        case MKK12_MKKC:
+        case MKK12_MKKA2:
+        case MKK3_MKKA:
+        case MKK3_MKKA1:
+        case MKK3_FCCA:
+        case MKK4_MKKA:
+        case MKK4_MKKA1:
+        case MKK4_FCCA:
+        case MKK9_MKKA:
+        case MKK10_MKKA:
+        case MKK1:
+        case MKK2:
+        case MKK3:
+        case MKK4:
+        case MKK5:
+        case MKK6:
+        case MKK7:
+        case MKK8:
+        case MKK9:
+        case MKK10:
+        case MKK11:
+        case MKK12:
+        case MKKA:
+        case MKKC:
+            return 0x40;//WG_JAPAN DOT11_REG_DOMAIN_MKK  Japan
+            break;
+
+        default:
+            break;
+    }
+    return 0xFF;// Didn't input RegDmn by mean to distinguish by customer
+
+}
+
+
+void zfHpDisableDfsChannel(zdev_t* dev, u8_t disableFlag)
+{
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+    hpPriv->disableDfsCh = disableFlag;
+    return;
+}
diff --git a/drivers/staging/otus/hal/hpreg.h b/drivers/staging/otus/hal/hpreg.h
new file mode 100644
index 000000000000..6f8c73fd42cc
--- /dev/null
+++ b/drivers/staging/otus/hal/hpreg.h
@@ -0,0 +1,524 @@
+/*
+ * Copyright (c) 2000-2005 ZyDAS Technology Corporation
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : hpreg.h                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains Regulatory Table definitions.              */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _HPREG_H
+#define _HPREG_H
+
+typedef u16_t HAL_CTRY_CODE;		/* country code */
+typedef u16_t HAL_REG_DOMAIN;		/* regulatory domain code */
+typedef enum {
+	AH_FALSE = 0,		/* NB: lots of code assumes false is zero */
+	AH_TRUE  = 1,
+} HAL_BOOL;
+
+
+/*
+ * Country/Region Codes from MS WINNLS.H
+ * Numbering from ISO 3166
+ */
+enum CountryCode {
+    CTRY_ALBANIA              = 8,       /* Albania */
+    CTRY_ALGERIA              = 12,      /* Algeria */
+    CTRY_ARGENTINA            = 32,      /* Argentina */
+    CTRY_ARMENIA              = 51,      /* Armenia */
+    CTRY_AUSTRALIA            = 36,      /* Australia */
+    CTRY_AUSTRIA              = 40,      /* Austria */
+    CTRY_AZERBAIJAN           = 31,      /* Azerbaijan */
+    CTRY_BAHRAIN              = 48,      /* Bahrain */
+    CTRY_BELARUS              = 112,     /* Belarus */
+    CTRY_BELGIUM              = 56,      /* Belgium */
+    CTRY_BELIZE               = 84,      /* Belize */
+    CTRY_BOLIVIA              = 68,      /* Bolivia */
+    CTRY_BOSNIA               = 70,      /* Bosnia */
+    CTRY_BRAZIL               = 76,      /* Brazil */
+    CTRY_BRUNEI_DARUSSALAM    = 96,      /* Brunei Darussalam */
+    CTRY_BULGARIA             = 100,     /* Bulgaria */
+    CTRY_CANADA               = 124,     /* Canada */
+    CTRY_CHILE                = 152,     /* Chile */
+    CTRY_CHINA                = 156,     /* People's Republic of China */
+    CTRY_COLOMBIA             = 170,     /* Colombia */
+    CTRY_COSTA_RICA           = 188,     /* Costa Rica */
+    CTRY_CROATIA              = 191,     /* Croatia */
+    CTRY_CYPRUS               = 196,     /* Cyprus */
+    CTRY_CZECH                = 203,     /* Czech Republic */
+    CTRY_DENMARK              = 208,     /* Denmark */
+    CTRY_DOMINICAN_REPUBLIC   = 214,     /* Dominican Republic */
+    CTRY_ECUADOR              = 218,     /* Ecuador */
+    CTRY_EGYPT                = 818,     /* Egypt */
+    CTRY_EL_SALVADOR          = 222,     /* El Salvador */
+    CTRY_ESTONIA              = 233,     /* Estonia */
+    CTRY_FAEROE_ISLANDS       = 234,     /* Faeroe Islands */
+    CTRY_FINLAND              = 246,     /* Finland */
+    CTRY_FRANCE               = 250,     /* France */
+    CTRY_FRANCE2              = 255,     /* France2 */
+    CTRY_GEORGIA              = 268,     /* Georgia */
+    CTRY_GERMANY              = 276,     /* Germany */
+    CTRY_GREECE               = 300,     /* Greece */
+    CTRY_GUATEMALA            = 320,     /* Guatemala */
+    CTRY_HONDURAS             = 340,     /* Honduras */
+    CTRY_HONG_KONG            = 344,     /* Hong Kong S.A.R., P.R.C. */
+    CTRY_HUNGARY              = 348,     /* Hungary */
+    CTRY_ICELAND              = 352,     /* Iceland */
+    CTRY_INDIA                = 356,     /* India */
+    CTRY_INDONESIA            = 360,     /* Indonesia */
+    CTRY_IRAN                 = 364,     /* Iran */
+    CTRY_IRAQ                 = 368,     /* Iraq */
+    CTRY_IRELAND              = 372,     /* Ireland */
+    CTRY_ISRAEL               = 376,     /* Israel */
+    CTRY_ISRAEL2              = 377,     /* Israel2 */
+    CTRY_ITALY                = 380,     /* Italy */
+    CTRY_JAMAICA              = 388,     /* Jamaica */
+    CTRY_JAPAN                = 392,     /* Japan */
+    CTRY_JAPAN1               = 393,     /* Japan (JP1) */
+    CTRY_JAPAN2               = 394,     /* Japan (JP0) */
+    CTRY_JAPAN3               = 395,     /* Japan (JP1-1) */
+    CTRY_JAPAN4               = 396,     /* Japan (JE1) */
+    CTRY_JAPAN5               = 397,     /* Japan (JE2) */
+    CTRY_JAPAN6               = 399,     /* Japan (JP6) */
+
+    CTRY_JAPAN7	              = 4007,    /* Japan (J7) */
+    CTRY_JAPAN8	              = 4008,    /* Japan (J8) */
+    CTRY_JAPAN9               = 4009,    /* Japan (J9) */
+
+    CTRY_JAPAN10              = 4010,    /* Japan (J10) */
+    CTRY_JAPAN11              = 4011,    /* Japan (J11) */
+    CTRY_JAPAN12              = 4012,    /* Japan (J12) */
+
+    CTRY_JAPAN13              = 4013,    /* Japan (J13) */
+    CTRY_JAPAN14              = 4014,    /* Japan (J14) */
+    CTRY_JAPAN15              = 4015,    /* Japan (J15) */
+
+    CTRY_JAPAN16              = 4016,    /* Japan (J16) */
+    CTRY_JAPAN17              = 4017,    /* Japan (J17) */
+    CTRY_JAPAN18              = 4018,    /* Japan (J18) */
+
+    CTRY_JAPAN19              = 4019,    /* Japan (J19) */
+    CTRY_JAPAN20              = 4020,    /* Japan (J20) */
+    CTRY_JAPAN21              = 4021,    /* Japan (J21) */
+
+    CTRY_JAPAN22              = 4022,    /* Japan (J22) */
+    CTRY_JAPAN23              = 4023,    /* Japan (J23) */
+    CTRY_JAPAN24              = 4024,    /* Japan (J24) */
+
+    CTRY_JAPAN25              = 4025,    /* Japan (J25) */
+    CTRY_JAPAN26              = 4026,    /* Japan (J26) */
+    CTRY_JAPAN27              = 4027,    /* Japan (J27) */
+
+    CTRY_JAPAN28              = 4028,    /* Japan (J28) */
+    CTRY_JAPAN29              = 4029,    /* Japan (J29) */
+    CTRY_JAPAN30              = 4030,    /* Japan (J30) */
+
+    CTRY_JAPAN31              = 4031,    /* Japan (J31) */
+    CTRY_JAPAN32              = 4032,    /* Japan (J32) */
+    CTRY_JAPAN33              = 4033,    /* Japan (J33) */
+
+    CTRY_JAPAN34              = 4034,    /* Japan (J34) */
+    CTRY_JAPAN35              = 4035,    /* Japan (J35) */
+    CTRY_JAPAN36              = 4036,    /* Japan (J36) */
+
+    CTRY_JAPAN37              = 4037,    /* Japan (J37) */
+    CTRY_JAPAN38              = 4038,    /* Japan (J38) */
+    CTRY_JAPAN39              = 4039,    /* Japan (J39) */
+
+    CTRY_JAPAN40              = 4040,    /* Japan (J40) */
+    CTRY_JAPAN41              = 4041,    /* Japan (J41) */
+    CTRY_JAPAN42              = 4042,    /* Japan (J42) */
+    CTRY_JAPAN43              = 4043,    /* Japan (J43) */
+    CTRY_JAPAN44              = 4044,    /* Japan (J44) */
+    CTRY_JAPAN45              = 4045,    /* Japan (J45) */
+    CTRY_JAPAN46              = 4046,    /* Japan (J46) */
+    CTRY_JAPAN47              = 4047,    /* Japan (J47) */
+    CTRY_JAPAN48              = 4048,    /* Japan (J48) */
+    CTRY_JAPAN49              = 4049,    /* Japan (J49) */
+
+    CTRY_JAPAN50              = 4050,    /* Japan (J50) */
+    CTRY_JAPAN51              = 4051,    /* Japan (J51) */
+    CTRY_JAPAN52              = 4052,    /* Japan (J52) */
+    CTRY_JAPAN53              = 4053,    /* Japan (J53) */
+    CTRY_JAPAN54              = 4054,    /* Japan (J54) */
+
+    CTRY_JORDAN               = 400,     /* Jordan */
+    CTRY_KAZAKHSTAN           = 398,     /* Kazakhstan */
+    CTRY_KENYA                = 404,     /* Kenya */
+    CTRY_KOREA_NORTH          = 408,     /* North Korea */
+    CTRY_KOREA_ROC            = 410,     /* South Korea */
+    CTRY_KOREA_ROC2           = 411,     /* South Korea */
+    CTRY_KOREA_ROC3           = 412,     /* South Korea */
+    CTRY_KUWAIT               = 414,     /* Kuwait */
+    CTRY_LATVIA               = 428,     /* Latvia */
+    CTRY_LEBANON              = 422,     /* Lebanon */
+    CTRY_LIBYA                = 434,     /* Libya */
+    CTRY_LIECHTENSTEIN        = 438,     /* Liechtenstein */
+    CTRY_LITHUANIA            = 440,     /* Lithuania */
+    CTRY_LUXEMBOURG           = 442,     /* Luxembourg */
+    CTRY_MACAU                = 446,     /* Macau */
+    CTRY_MACEDONIA            = 807,     /* the Former Yugoslav Republic of Macedonia */
+    CTRY_MALAYSIA             = 458,     /* Malaysia */
+    CTRY_MALTA	              = 470,     /* Malta */
+    CTRY_MEXICO               = 484,     /* Mexico */
+    CTRY_MONACO               = 492,     /* Principality of Monaco */
+    CTRY_MOROCCO              = 504,     /* Morocco */
+    CTRY_NETHERLANDS          = 528,     /* Netherlands */
+    CTRY_NETHERLANDS_ANT      = 530,     /* Netherlands-Antellis */
+    CTRY_NEW_ZEALAND          = 554,     /* New Zealand */
+    CTRY_NICARAGUA            = 558,     /* Nicaragua */
+    CTRY_NORWAY               = 578,     /* Norway */
+    CTRY_OMAN                 = 512,     /* Oman */
+    CTRY_PAKISTAN             = 586,     /* Islamic Republic of Pakistan */
+    CTRY_PANAMA               = 591,     /* Panama */
+    CTRY_PARAGUAY             = 600,     /* Paraguay */
+    CTRY_PERU                 = 604,     /* Peru */
+    CTRY_PHILIPPINES          = 608,     /* Republic of the Philippines */
+    CTRY_POLAND               = 616,     /* Poland */
+    CTRY_PORTUGAL             = 620,     /* Portugal */
+    CTRY_PUERTO_RICO          = 630,     /* Puerto Rico */
+    CTRY_QATAR                = 634,     /* Qatar */
+    CTRY_ROMANIA              = 642,     /* Romania */
+    CTRY_RUSSIA               = 643,     /* Russia */
+    CTRY_SAUDI_ARABIA         = 682,     /* Saudi Arabia */
+    CTRY_SERBIA_MONT          = 891,     /* Serbia and Montenegro */
+    CTRY_SINGAPORE            = 702,     /* Singapore */
+    CTRY_SLOVAKIA             = 703,     /* Slovak Republic */
+    CTRY_SLOVENIA             = 705,     /* Slovenia */
+    CTRY_SOUTH_AFRICA         = 710,     /* South Africa */
+    CTRY_SPAIN                = 724,     /* Spain */
+    CTRY_SRILANKA             = 144,     /* Srilanka */
+    CTRY_SWEDEN               = 752,     /* Sweden */
+    CTRY_SWITZERLAND          = 756,     /* Switzerland */
+    CTRY_SYRIA                = 760,     /* Syria */
+    CTRY_TAIWAN               = 158,     /* Taiwan */
+    CTRY_THAILAND             = 764,     /* Thailand */
+    CTRY_TRINIDAD_Y_TOBAGO    = 780,     /* Trinidad y Tobago */
+    CTRY_TUNISIA              = 788,     /* Tunisia */
+    CTRY_TURKEY               = 792,     /* Turkey */
+    CTRY_UAE                  = 784,     /* U.A.E. */
+    CTRY_UKRAINE              = 804,     /* Ukraine */
+    CTRY_UNITED_KINGDOM       = 826,     /* United Kingdom */
+    CTRY_UNITED_STATES        = 840,     /* United States */
+    CTRY_UNITED_STATES_FCC49  = 842,     /* United States (Public Safety)*/
+    CTRY_URUGUAY              = 858,     /* Uruguay */
+    CTRY_UZBEKISTAN           = 860,     /* Uzbekistan */
+    CTRY_VENEZUELA            = 862,     /* Venezuela */
+    CTRY_VIET_NAM             = 704,     /* Viet Nam */
+    CTRY_YEMEN                = 887,     /* Yemen */
+    CTRY_ZIMBABWE             = 716      /* Zimbabwe */
+};
+
+/* Enumerated Regulatory Domain Information 8 bit values indicate that
+ * the regdomain is really a pair of unitary regdomains.  12 bit values
+ * are the real unitary regdomains and are the only ones which have the
+ * frequency bitmasks and flags set.
+ */
+enum EnumRd {
+	/*
+	 * The following regulatory domain definitions are
+	 * found in the EEPROM. Each regulatory domain
+	 * can operate in either a 5GHz or 2.4GHz wireless mode or
+	 * both 5GHz and 2.4GHz wireless modes.
+	 * In general, the value holds no special
+	 * meaning and is used to decode into either specific
+	 * 2.4GHz or 5GHz wireless mode for that particular
+	 * regulatory domain.
+	 */
+	NO_ENUMRD	= 0x00,
+	NULL1_WORLD	= 0x03,		/* For 11b-only countries (no 11a allowed) */
+	NULL1_ETSIB	= 0x07,		/* Israel */
+	NULL1_ETSIC	= 0x08,
+	FCC1_FCCA	= 0x10,		/* USA */
+	FCC1_WORLD	= 0x11,		/* Hong Kong */
+	FCC4_FCCA	= 0x12,		/* USA - Public Safety */
+	FCC5_FCCA	= 0x13,		/* USA - with no DFS (UNII-1 + UNII-3 only) */
+  FCC6_FCCA       = 0x14,         /* Canada */
+
+	FCC2_FCCA	= 0x20,		/* Canada */
+	FCC2_WORLD	= 0x21,		/* Australia & HK */
+	FCC2_ETSIC	= 0x22,
+  FCC6_WORLD      = 0x23,         /* Australia */
+
+	FRANCE_RES	= 0x31,		/* Legacy France for OEM */
+	FCC3_FCCA	= 0x3A,		/* USA & Canada w/5470 band, 11h, DFS enabled */
+	FCC3_WORLD	= 0x3B,		/* USA & Canada w/5470 band, 11h, DFS enabled */
+
+	ETSI1_WORLD	= 0x37,
+	ETSI3_ETSIA	= 0x32,		/* France (optional) */
+	ETSI2_WORLD	= 0x35,		/* Hungary & others */
+	ETSI3_WORLD	= 0x36,		/* France & others */
+	ETSI4_WORLD	= 0x30,
+	ETSI4_ETSIC	= 0x38,
+	ETSI5_WORLD	= 0x39,
+	ETSI6_WORLD	= 0x34,		/* Bulgaria */
+	ETSI_RESERVED	= 0x33,		/* Reserved (Do not used) */
+
+	MKK1_MKKA	= 0x40,		/* Japan (JP1) */
+	MKK1_MKKB	= 0x41,		/* Japan (JP0) */
+	APL4_WORLD	= 0x42,		/* Singapore */
+	MKK2_MKKA	= 0x43,		/* Japan with 4.9G channels */
+	APL_RESERVED	= 0x44,		/* Reserved (Do not used)  */
+	APL2_WORLD	= 0x45,		/* Korea */
+	APL2_APLC	= 0x46,
+	APL3_WORLD	= 0x47,
+	MKK1_FCCA	= 0x48,		/* Japan (JP1-1) */
+	APL2_APLD	= 0x49,		/* Korea with 2.3G channels */
+	MKK1_MKKA1	= 0x4A,		/* Japan (JE1) */
+	MKK1_MKKA2	= 0x4B,		/* Japan (JE2) */
+	MKK1_MKKC	= 0x4C,		/* Japan (MKK1_MKKA,except Ch14) */
+
+	APL3_FCCA   = 0x50,
+	APL1_WORLD	= 0x52,		/* Latin America */
+	APL1_FCCA	= 0x53,
+	APL1_APLA	= 0x54,
+	APL1_ETSIC	= 0x55,
+	APL2_ETSIC	= 0x56,		/* Venezuela */
+	APL2_FCCA   = 0x57, 	/* new Latin America */
+	APL5_WORLD	= 0x58,		/* Chile */
+	APL6_WORLD	= 0x5B,		/* Singapore */
+	APL7_FCCA   = 0x5C,     /* Taiwan 5.47 Band */
+	APL8_WORLD  = 0x5D,     /* Malaysia 5GHz */
+	APL9_WORLD  = 0x5E,     /* Korea 5GHz */
+
+	/*
+	 * World mode SKUs
+	 */
+	WOR0_WORLD	= 0x60,		/* World0 (WO0 SKU) */
+	WOR1_WORLD	= 0x61,		/* World1 (WO1 SKU) */
+	WOR2_WORLD	= 0x62,		/* World2 (WO2 SKU) */
+	WOR3_WORLD	= 0x63,		/* World3 (WO3 SKU) */
+	WOR4_WORLD	= 0x64,		/* World4 (WO4 SKU) */
+	WOR5_ETSIC	= 0x65,		/* World5 (WO5 SKU) */
+
+	WOR01_WORLD	= 0x66,		/* World0-1 (WW0-1 SKU) */
+	WOR02_WORLD	= 0x67,		/* World0-2 (WW0-2 SKU) */
+	EU1_WORLD	= 0x68,		/* Same as World0-2 (WW0-2 SKU), except active scan ch1-13. No ch14 */
+
+	WOR9_WORLD	= 0x69,		/* World9 (WO9 SKU) */
+	WORA_WORLD	= 0x6A,		/* WorldA (WOA SKU) */
+
+	MKK3_MKKB	= 0x80,		/* Japan UNI-1 even + MKKB */
+	MKK3_MKKA2	= 0x81,		/* Japan UNI-1 even + MKKA2 */
+	MKK3_MKKC	= 0x82,		/* Japan UNI-1 even + MKKC */
+
+	MKK4_MKKB	= 0x83,		/* Japan UNI-1 even + UNI-2 + MKKB */
+	MKK4_MKKA2	= 0x84,		/* Japan UNI-1 even + UNI-2 + MKKA2 */
+	MKK4_MKKC	= 0x85,		/* Japan UNI-1 even + UNI-2 + MKKC */
+
+	MKK5_MKKB	= 0x86,		/* Japan UNI-1 even + UNI-2 + mid-band + MKKB */
+	MKK5_MKKA2	= 0x87,		/* Japan UNI-1 even + UNI-2 + mid-band + MKKA2 */
+	MKK5_MKKC	= 0x88,		/* Japan UNI-1 even + UNI-2 + mid-band + MKKC */
+
+	MKK6_MKKB	= 0x89,		/* Japan UNI-1 even + UNI-1 odd MKKB */
+	MKK6_MKKA2	= 0x8A,		/* Japan UNI-1 even + UNI-1 odd + MKKA2 */
+	MKK6_MKKC	= 0x8B,		/* Japan UNI-1 even + UNI-1 odd + MKKC */
+
+	MKK7_MKKB	= 0x8C,		/* Japan UNI-1 even + UNI-1 odd + UNI-2 + MKKB */
+	MKK7_MKKA	= 0x8D,		/* Japan UNI-1 even + UNI-1 odd + UNI-2 + MKKA2 */
+	MKK7_MKKC	= 0x8E,		/* Japan UNI-1 even + UNI-1 odd + UNI-2 + MKKC */
+
+	MKK8_MKKB	= 0x8F,		/* Japan UNI-1 even + UNI-1 odd + UNI-2 + mid-band + MKKB */
+	MKK8_MKKA2	= 0x90,		/* Japan UNI-1 even + UNI-1 odd + UNI-2 + mid-band + MKKA2 */
+	MKK8_MKKC	= 0x91,		/* Japan UNI-1 even + UNI-1 odd + UNI-2 + mid-band + MKKC */
+
+	MKK6_MKKA1      = 0xF8,         /* Japan UNI-1 even + UNI-1 odd + MKKA1 */
+	MKK6_FCCA       = 0xF9,         /* Japan UNI-1 even + UNI-1 odd + FCCA */
+	MKK7_MKKA1      = 0xFA,         /* Japan UNI-1 even + UNI-1 odd + UNI-2 + MKKA1 */
+	MKK7_FCCA       = 0xFB,         /* Japan UNI-1 even + UNI-1 odd + UNI-2 + FCCA */
+	MKK9_FCCA       = 0xFC,         /* Japan UNI-1 even + 4.9GHz + FCCA */
+	MKK9_MKKA1      = 0xFD,         /* Japan UNI-1 even + 4.9GHz + MKKA1 */
+	MKK9_MKKC       = 0xFE,         /* Japan UNI-1 even + 4.9GHz + MKKC */
+	MKK9_MKKA2      = 0xFF,         /* Japan UNI-1 even + 4.9GHz + MKKA2 */
+
+	MKK10_FCCA      = 0xD0,         /* Japan UNI-1 even + UNI-2 + 4.9GHz + FCCA */
+	MKK10_MKKA1     = 0xD1,         /* Japan UNI-1 even + UNI-2 + 4.9GHz + MKKA1 */
+	MKK10_MKKC      = 0xD2,         /* Japan UNI-1 even + UNI-2 + 4.9GHz + MKKC */
+	MKK10_MKKA2     = 0xD3,         /* Japan UNI-1 even + UNI-2 + 4.9GHz + MKKA2 */
+
+	MKK11_MKKA      = 0xD4,         /* Japan UNI-1 even + UNI-2 + Midband + 4.9GHz + MKKA */
+	MKK11_FCCA      = 0xD5,         /* Japan UNI-1 even + UNI-2 + Midband + 4.9GHz + FCCA */
+	MKK11_MKKA1     = 0xD6,         /* Japan UNI-1 even + UNI-2 + Midband + 4.9GHz + MKKA1 */
+	MKK11_MKKC      = 0xD7,         /* Japan UNI-1 even + UNI-2 + Midband + 4.9GHz + MKKC */
+	MKK11_MKKA2     = 0xD8,         /* Japan UNI-1 even + UNI-2 + Midband + 4.9GHz + MKKA2 */
+
+	MKK12_MKKA      = 0xD9,         /* Japan UNI-1 even + UNI-1 odd + UNI-2 + Midband + 4.9GHz + MKKA */
+	MKK12_FCCA      = 0xDA,         /* Japan UNI-1 even + UNI-1 odd + UNI-2 + Midband + 4.9GHz + FCCA */
+	MKK12_MKKA1     = 0xDB,         /* Japan UNI-1 even + UNI-1 odd + UNI-2 + Midband + 4.9GHz + MKKA1 */
+	MKK12_MKKC      = 0xDC,         /* Japan UNI-1 even + UNI-1 odd + UNI-2 + Midband + 4.9GHz + MKKC */
+	MKK12_MKKA2     = 0xDD,         /* Japan UNI-1 even + UNI-1 odd + UNI-2 + Midband + 4.9GHz + MKKA2 */
+
+	/* Following definitions are used only by s/w to map old
+ 	 * Japan SKUs.
+	 */
+	MKK3_MKKA       = 0xF0,         /* Japan UNI-1 even + MKKA */
+	MKK3_MKKA1      = 0xF1,         /* Japan UNI-1 even + MKKA1 */
+	MKK3_FCCA       = 0xF2,         /* Japan UNI-1 even + FCCA */
+	MKK4_MKKA       = 0xF3,         /* Japan UNI-1 even + UNI-2 + MKKA */
+	MKK4_MKKA1      = 0xF4,         /* Japan UNI-1 even + UNI-2 + MKKA1 */
+	MKK4_FCCA       = 0xF5,         /* Japan UNI-1 even + UNI-2 + FCCA */
+	MKK9_MKKA       = 0xF6,         /* Japan UNI-1 even + 4.9GHz + MKKA*/
+	MKK10_MKKA      = 0xF7,         /* Japan UNI-1 even + UNI-2 + 4.9GHz + MKKA */
+
+	/*
+	 * Regulator domains ending in a number (e.g. APL1,
+	 * MK1, ETSI4, etc) apply to 5GHz channel and power
+	 * information.  Regulator domains ending in a letter
+	 * (e.g. APLA, FCCA, etc) apply to 2.4GHz channel and
+	 * power information.
+	 */
+	APL1		= 0x0150,	/* LAT & Asia */
+	APL2		= 0x0250,	/* LAT & Asia */
+	APL3		= 0x0350,	/* Taiwan */
+	APL4		= 0x0450,	/* Jordan */
+	APL5		= 0x0550,	/* Chile */
+	APL6		= 0x0650,	/* Singapore */
+	APL7		= 0x0750,	/* Taiwan Middle */
+	APL8		= 0x0850,	/* Malaysia */
+	APL9		= 0x0950,	/* Korea (South) ROC 3 */
+
+	ETSI1		= 0x0130,	/* Europe & others */
+	ETSI2		= 0x0230,	/* Europe & others */
+	ETSI3		= 0x0330,	/* Europe & others */
+	ETSI4		= 0x0430,	/* Europe & others */
+	ETSI5		= 0x0530,	/* Europe & others */
+	ETSI6		= 0x0630,	/* Europe & others */
+	ETSIA		= 0x0A30,	/* France */
+	ETSIB		= 0x0B30,	/* Israel */
+	ETSIC		= 0x0C30,	/* Latin America */
+
+	FCC1		= 0x0110,	/* US & others */
+	FCC2		= 0x0120,	/* Canada, Australia & New Zealand */
+	FCC3		= 0x0160,	/* US w/new middle band & DFS */
+	FCC4		= 0x0165,	/* US Public Safety */
+	FCC5		= 0x0510,	/* US no DFS */
+    FCC6    = 0x0610, /* Canada & Australia */
+
+	FCCA		= 0x0A10,
+
+	APLD		= 0x0D50,	/* South Korea */
+
+	MKK1		= 0x0140,	/* Japan (UNI-1 odd)*/
+	MKK2		= 0x0240,	/* Japan (4.9 GHz + UNI-1 odd) */
+	MKK3		= 0x0340,	/* Japan (UNI-1 even) */
+	MKK4		= 0x0440,	/* Japan (UNI-1 even + UNI-2) */
+	MKK5		= 0x0540,	/* Japan (UNI-1 even + UNI-2 + mid-band) */
+	MKK6		= 0x0640,	/* Japan (UNI-1 odd + UNI-1 even) */
+	MKK7		= 0x0740,	/* Japan (UNI-1 odd + UNI-1 even + UNI-2 */
+	MKK8		= 0x0840,	/* Japan (UNI-1 odd + UNI-1 even + UNI-2 + mid-band) */
+	MKK9		= 0x0940,   /* Japan (UNI-1 even + 4.9 GHZ) */
+	MKK10		= 0x0B40,   /* Japan (UNI-1 even + UNI-2 + 4.9 GHZ) */
+	MKK11		= 0x1140,   /* Japan (UNI-1 even + UNI-2 + mid-band + 4.9 GHZ) */
+	MKK12		= 0x1240,   /* Japan (UNI-1 even + UNI-1 odd + UNI-2 + mid-band + 4.9 GHZ) */
+	MKKA		= 0x0A40,	/* Japan */
+	MKKC		= 0x0A50,
+
+	NULL1		= 0x0198,
+	WORLD		= 0x0199,
+	DEBUG_REG_DMN	= 0x01ff,
+};
+
+/* channelFlags */
+#define	ZM_REG_FLAG_CHANNEL_CW_INT	0x0002	/* CW interference detected on channel */
+#define	ZM_REG_FLAG_CHANNEL_TURBO	0x0010	/* Turbo Channel */
+#define	ZM_REG_FLAG_CHANNEL_CCK	    0x0020	/* CCK channel */
+#define	ZM_REG_FLAG_CHANNEL_OFDM	0x0040	/* OFDM channel */
+#define	ZM_REG_FLAG_CHANNEL_2GHZ	0x0080	/* 2 GHz spectrum channel. */
+#define	ZM_REG_FLAG_CHANNEL_5GHZ	0x0100	/* 5 GHz spectrum channel */
+#define	ZM_REG_FLAG_CHANNEL_PASSIVE	0x0200	/* Only passive scan allowed in the channel */
+#define	ZM_REG_FLAG_CHANNEL_DYN	    0x0400	/* dynamic CCK-OFDM channel */
+#define	ZM_REG_FLAG_CHANNEL_XR	    0x0800	/* XR channel */
+#define	ZM_REG_FLAG_CHANNEL_CSA 	0x1000	/* Channel by CSA(Channel Switch Announcement) */
+#define	ZM_REG_FLAG_CHANNEL_STURBO	0x2000	/* Static turbo, no 11a-only usage */
+#define ZM_REG_FLAG_CHANNEL_HALF    0x4000 	/* Half rate channel */
+#define ZM_REG_FLAG_CHANNEL_QUARTER 0x8000 	/* Quarter rate channel */
+
+/* channelFlags */
+#define CHANNEL_CW_INT  0x0002  /* CW interference detected on channel */
+#define CHANNEL_TURBO   0x0010  /* Turbo Channel */
+#define CHANNEL_CCK 0x0020  /* CCK channel */
+#define CHANNEL_OFDM    0x0040  /* OFDM channel */
+#define CHANNEL_2GHZ    0x0080  /* 2 GHz spectrum channel. */
+#define CHANNEL_5GHZ    0x0100  /* 5 GHz spectrum channel */
+#define CHANNEL_PASSIVE 0x0200  /* Only passive scan allowed in the channel */
+#define CHANNEL_DYN 0x0400  /* dynamic CCK-OFDM channel */
+#define CHANNEL_XR  0x0800  /* XR channel */
+#define CHANNEL_STURBO  0x2000  /* Static turbo, no 11a-only usage */
+#define CHANNEL_HALF    0x4000  /* Half rate channel */
+#define CHANNEL_QUARTER 0x8000  /* Quarter rate channel */
+#define CHANNEL_HT20    0x10000 /* HT20 channel */
+#define CHANNEL_HT40    0x20000 /* HT40 channel */
+#define CHANNEL_HT40U 	0x40000 /* control channel can be upper channel */
+#define CHANNEL_HT40L 	0x80000 /* control channel can be lower channel */
+
+/* privFlags */
+#define ZM_REG_FLAG_CHANNEL_INTERFERENCE   	0x01 /* Software use: channel interference
+				        used for as AR as well as RADAR
+				        interference detection */
+#define ZM_REG_FLAG_CHANNEL_DFS		0x02 /* DFS required on channel */
+#define ZM_REG_FLAG_CHANNEL_4MS_LIMIT	0x04 /* 4msec packet limit on this channel */
+#define ZM_REG_FLAG_CHANNEL_DFS_CLEAR       0x08 /* if channel has been checked for DFS */
+
+#define CHANNEL_A   (CHANNEL_5GHZ|CHANNEL_OFDM)
+#define CHANNEL_B   (CHANNEL_2GHZ|CHANNEL_CCK)
+#define CHANNEL_PUREG   (CHANNEL_2GHZ|CHANNEL_OFDM)
+#ifdef notdef
+#define CHANNEL_G   (CHANNEL_2GHZ|CHANNEL_DYN)
+#else
+#define CHANNEL_G   (CHANNEL_2GHZ|CHANNEL_OFDM)
+#endif
+#define CHANNEL_T   (CHANNEL_5GHZ|CHANNEL_OFDM|CHANNEL_TURBO)
+#define CHANNEL_ST  (CHANNEL_T|CHANNEL_STURBO)
+#define CHANNEL_108G    (CHANNEL_2GHZ|CHANNEL_OFDM|CHANNEL_TURBO)
+#define CHANNEL_108A    CHANNEL_T
+#define CHANNEL_X   (CHANNEL_5GHZ|CHANNEL_OFDM|CHANNEL_XR)
+#define CHANNEL_G_HT      (CHANNEL_2GHZ | CHANNEL_OFDM | CHANNEL_HT20)
+#define CHANNEL_A_HT      (CHANNEL_5GHZ | CHANNEL_OFDM | CHANNEL_HT20)
+
+#define CHANNEL_G_HT20  (CHANNEL_2GHZ|CHANNEL_HT20)
+#define CHANNEL_A_HT20  (CHANNEL_5GHZ|CHANNEL_HT20)
+#define CHANNEL_G_HT40  (CHANNEL_2GHZ|CHANNEL_HT20|CHANNEL_HT40)
+#define CHANNEL_A_HT40  (CHANNEL_5GHZ|CHANNEL_HT20|CHANNEL_HT40)
+#define CHANNEL_ALL \
+    (CHANNEL_OFDM|CHANNEL_CCK| CHANNEL_2GHZ | CHANNEL_5GHZ | CHANNEL_TURBO | CHANNEL_HT20 | CHANNEL_HT40)
+#define CHANNEL_ALL_NOTURBO     (CHANNEL_ALL &~ CHANNEL_TURBO)
+
+enum {
+    HAL_MODE_11A    = 0x001,        /* 11a channels */
+    HAL_MODE_TURBO  = 0x002,        /* 11a turbo-only channels */
+    HAL_MODE_11B    = 0x004,        /* 11b channels */
+    HAL_MODE_PUREG  = 0x008,        /* 11g channels (OFDM only) */
+#ifdef notdef
+    HAL_MODE_11G    = 0x010,        /* 11g channels (OFDM/CCK) */
+#else
+    HAL_MODE_11G    = 0x008,        /* XXX historical */
+#endif
+    HAL_MODE_108G   = 0x020,        /* 11a+Turbo channels */
+    HAL_MODE_108A   = 0x040,        /* 11g+Turbo channels */
+    HAL_MODE_XR     = 0x100,        /* XR channels */
+    HAL_MODE_11A_HALF_RATE = 0x200,     /* 11A half rate channels */
+    HAL_MODE_11A_QUARTER_RATE = 0x400,  /* 11A quarter rate channels */
+    HAL_MODE_11NG   = 0x4000,           /* 11ng channels */
+    HAL_MODE_11NA   = 0x8000,           /* 11na channels */
+    HAL_MODE_ALL    = 0xffff
+};
+
+#endif /* #ifndef _HPREG_H */
diff --git a/drivers/staging/otus/hal/hprw.c b/drivers/staging/otus/hal/hprw.c
new file mode 100644
index 000000000000..db7d49576456
--- /dev/null
+++ b/drivers/staging/otus/hal/hprw.c
@@ -0,0 +1,1557 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+#include "hpani.h"
+#include "hpusb.h"
+#include "hpreg.h"
+#include "../80211core/ratectrl.h"
+
+extern void zfIdlCmd(zdev_t* dev, u32_t* cmd, u16_t cmdLen);
+
+extern void zfCoreCwmBusy(zdev_t* dev, u16_t busy);
+u16_t zfDelayWriteInternalReg(zdev_t* dev, u32_t addr, u32_t val);
+u16_t zfFlushDelayWrite(zdev_t* dev);
+
+//#define zm_hp_priv(x) struct zsHpPriv* hpPriv=zgWlanDev.hpPrivate;
+
+void zfInitCmdQueue(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = (struct zsHpPriv*)(wd->hpPrivate);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+#ifdef ZM_XP_USB_MULTCMD
+    hpPriv->cmdTail = hpPriv->cmdHead = hpPriv->cmdSend = 0;
+#else
+    hpPriv->cmdTail = hpPriv->cmdHead = 0;
+#endif
+    hpPriv->cmdPending = 0;
+    hpPriv->cmd.delayWcmdCount = 0;
+    zmw_leave_critical_section(dev);
+}
+
+u16_t zfPutCmd(zdev_t* dev, u32_t* cmd, u16_t cmdLen, u16_t src, u8_t* buf)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    /* Make sure command length < ZM_MAX_CMD_SIZE */
+    zm_assert(cmdLen <= ZM_MAX_CMD_SIZE);
+    /* Make sure command queue not full */
+    //zm_assert(((hpPriv->cmdTail+1) & (ZM_CMD_QUEUE_SIZE-1)) != hpPriv->cmdHead);
+    if (((hpPriv->cmdTail+1) & (ZM_CMD_QUEUE_SIZE-1)) == hpPriv->cmdHead ) {
+        zm_debug_msg0("CMD queue full!!");
+        return 0;
+    }
+
+    hpPriv->cmdQ[hpPriv->cmdTail].cmdLen = cmdLen;
+    hpPriv->cmdQ[hpPriv->cmdTail].src = src;
+    hpPriv->cmdQ[hpPriv->cmdTail].buf = buf;
+    for (i=0; i<(cmdLen>>2); i++)
+    {
+        hpPriv->cmdQ[hpPriv->cmdTail].cmd[i] = cmd[i];
+    }
+
+    hpPriv->cmdTail = (hpPriv->cmdTail+1) & (ZM_CMD_QUEUE_SIZE-1);
+
+    return 0;
+}
+
+u16_t zfGetCmd(zdev_t* dev, u32_t* cmd, u16_t* cmdLen, u16_t* src, u8_t** buf)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    if (hpPriv->cmdTail == hpPriv->cmdHead)
+    {
+        return 3;
+    }
+
+    *cmdLen = hpPriv->cmdQ[hpPriv->cmdHead].cmdLen;
+    *src = hpPriv->cmdQ[hpPriv->cmdHead].src;
+    *buf = hpPriv->cmdQ[hpPriv->cmdHead].buf;
+    for (i=0; i<((*cmdLen)>>2); i++)
+    {
+        cmd[i] = hpPriv->cmdQ[hpPriv->cmdHead].cmd[i];
+    }
+
+    hpPriv->cmdHead = (hpPriv->cmdHead+1) & (ZM_CMD_QUEUE_SIZE-1);
+
+    return 0;
+}
+
+#ifdef ZM_XP_USB_MULTCMD
+void zfSendCmdEx(zdev_t* dev)
+{
+    u32_t ncmd[ZM_MAX_CMD_SIZE/4];
+    u16_t ncmdLen = 0;
+    u16_t cmdFlag = 0;
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (hpPriv->cmdPending == 0)
+    {
+        if (hpPriv->cmdTail != hpPriv->cmdSend)
+        {
+            cmdFlag = 1;
+            /* Get queueing command */
+            ncmdLen= hpPriv->cmdQ[hpPriv->cmdSend].cmdLen;
+            for (i=0; i<(ncmdLen>>2); i++)
+            {
+                ncmd[i] = hpPriv->cmdQ[hpPriv->cmdSend].cmd[i];
+            }
+            hpPriv->cmdSend = (hpPriv->cmdSend+1) & (ZM_CMD_QUEUE_SIZE-1);
+
+            hpPriv->cmdPending = 1;
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if ((cmdFlag == 1))
+    {
+        zfIdlCmd(dev, ncmd, ncmdLen);
+    }
+}
+
+void zfiSendCmdComp(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    hpPriv->cmdPending = 0;
+    zmw_leave_critical_section(dev);
+
+    zfSendCmdEx(dev);
+}
+#endif
+
+u16_t zfIssueCmd(zdev_t* dev, u32_t* cmd, u16_t cmdLen, u16_t src, u8_t* buf)
+{
+    u16_t cmdFlag = 0;
+    u16_t ret;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zmw_declare_for_critical_section();
+
+    zm_msg2_mm(ZM_LV_1, "cmdLen=", cmdLen);
+
+    zmw_enter_critical_section(dev);
+
+#ifdef ZM_XP_USB_MULTCMD
+    ret = zfPutCmd(dev, cmd, cmdLen, src, buf);
+    zmw_leave_critical_section(dev);
+
+    if (ret != 0)
+    {
+        return 1;
+    }
+
+    zfSendCmdEx(dev);
+#else
+    if (hpPriv->cmdPending == 0)
+    {
+        hpPriv->cmdPending = 1;
+        cmdFlag = 1;
+    }
+    ret = zfPutCmd(dev, cmd, cmdLen, src, buf);
+
+    zmw_leave_critical_section(dev);
+
+    if (ret != 0)
+    {
+        return 1;
+    }
+
+    if (cmdFlag == 1)
+    {
+        zfIdlCmd(dev, cmd, cmdLen);
+    }
+#endif
+    return 0;
+}
+
+void zfIdlRsp(zdev_t* dev, u32_t* rsp, u16_t rspLen)
+{
+    u32_t cmd[ZM_MAX_CMD_SIZE/4];
+    u16_t cmdLen;
+    u16_t src;
+    u8_t* buf;
+    u32_t ncmd[ZM_MAX_CMD_SIZE/4];
+    u16_t ncmdLen = 0;
+    u16_t ret;
+    u16_t cmdFlag = 0;
+    u16_t i;
+    s32_t nf;
+    s32_t noisefloor[4];
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    ret = zfGetCmd(dev, cmd, &cmdLen, &src, &buf);
+    #if 0
+    zm_assert(ret == 0);
+    #else
+    if (ret != 0)
+    {
+        zm_debug_msg0("Error IdlRsp because none cmd!!\n");
+        #ifndef ZM_XP_USB_MULTCMD
+        zmw_leave_critical_section(dev);
+        return;
+        #endif
+    }
+    #endif
+#ifdef ZM_XP_USB_MULTCMD
+    zmw_leave_critical_section(dev);
+#else
+    if (hpPriv->cmdTail != hpPriv->cmdHead)
+    {
+        cmdFlag = 1;
+        /* Get queueing command */
+        ncmdLen= hpPriv->cmdQ[hpPriv->cmdHead].cmdLen;
+        for (i=0; i<(ncmdLen>>2); i++)
+        {
+            ncmd[i] = hpPriv->cmdQ[hpPriv->cmdHead].cmd[i];
+        }
+    }
+    else
+    {
+        hpPriv->cmdPending = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (cmdFlag == 1)
+    {
+        zfIdlCmd(dev, ncmd, ncmdLen);
+    }
+#endif
+    if (src == ZM_OID_READ)
+    {
+        ZM_PERFORMANCE_REG(dev, 0x11772c, rsp[1]);
+        zfwDbgReadRegDone(dev, cmd[1], rsp[1]);
+    }
+    else if (src == ZM_OID_FLASH_CHKSUM)
+    {
+        zfwDbgGetFlashChkSumDone(dev, rsp+1);
+    }
+    else if (src == ZM_OID_FLASH_READ)
+    {
+        u32_t  datalen;
+        u16_t i;
+
+        datalen = (rsp[0] & 255);
+
+        zfwDbgReadFlashDone(dev, cmd[1], rsp+1, datalen);
+    }
+    else if (src == ZM_OID_FLASH_PROGRAM)
+    {
+        /* Non do */
+    }
+    else if (src == ZM_OID_WRITE)
+    {
+        zfwDbgWriteRegDone(dev, cmd[1], cmd[2]);
+    }
+    else if (src == ZM_OID_TALLY)
+    {
+		zfCollectHWTally(dev, rsp, 0);
+    }
+    else if (src == ZM_OID_TALLY_APD)
+    {
+		zfCollectHWTally(dev, rsp, 1);
+        zfwDbgReadTallyDone(dev);
+#ifdef ZM_ENABLE_BA_RATECTRL
+        zfRateCtrlAggrSta(dev);
+#endif
+    }
+    else if (src == ZM_OID_DKTX_STATUS)
+    {
+        zm_debug_msg0("src = zm_OID_DKTX_STATUS");
+        zfwDbgQueryHwTxBusyDone(dev, rsp[1]);
+    }
+    else if (src == ZM_CMD_SET_FREQUENCY)
+    {
+
+//#ifdef ZM_OTUS_ENABLE_RETRY_FREQ_CHANGE
+#if 0
+    zm_debug_msg1("Retry Set Frequency = ", rsp[1]);
+
+    #if 1
+    // Read the Noise Floor value !
+    nf = ((rsp[2]>>19) & 0x1ff);
+    if ((nf & 0x100) != 0x0)
+    {
+        noisefloor[0] = 0 - ((nf ^ 0x1ff) + 1);
+    }
+    else
+    {
+        noisefloor[0] = nf;
+    }
+
+    zm_debug_msg1("Noise Floor[1] = ", noisefloor[0]);
+
+    nf = ((rsp[3]>>19) & 0x1ff);
+    if ((nf & 0x100) != 0x0)
+    {
+        noisefloor[1] = 0 - ((nf ^ 0x1ff) + 1);
+    }
+    else
+    {
+        noisefloor[1] = nf;
+    }
+
+    zm_debug_msg1("Noise Floor[2] = ", noisefloor[1]);
+    zm_debug_msg1("Is Site Survey = ", hpPriv->isSiteSurvey);
+    #endif
+
+        if ( (rsp[1] && hpPriv->freqRetryCounter == 0) ||
+             (((noisefloor[0]>-60)||(noisefloor[1]>-60)) && hpPriv->freqRetryCounter==0) ||
+             ((abs(noisefloor[0]-noisefloor[1])>=9) && hpPriv->freqRetryCounter==0) )
+        {
+            zm_debug_msg0("Retry to issue the frequency change command");
+
+            if ( hpPriv->recordFreqRetryCounter == 1 )
+            {
+                zm_debug_msg0("Cold Reset");
+
+                zfHpSetFrequencyEx(dev, hpPriv->latestFrequency,
+                                        hpPriv->latestBw40,
+                                        hpPriv->latestExtOffset,
+                                        2);
+
+                if ( hpPriv->isSiteSurvey != 2 )
+                {
+                    hpPriv->freqRetryCounter++;
+                }
+                hpPriv->recordFreqRetryCounter = 0;
+            }
+            else
+            {
+                zfHpSetFrequencyEx(dev, hpPriv->latestFrequency,
+                                        hpPriv->latestBw40,
+                                        hpPriv->latestExtOffset,
+                                        0);
+            }
+            hpPriv->recordFreqRetryCounter++;
+        }
+        else
+#endif
+
+/* ret: Bit0: AGC calibration   0=>finish  1=>unfinish               */
+/*      Bit1: Noise calibration 0=>finish  1=>unfinish               */
+/*      Bit2: Noise calibration finish, but NF value unexcepted => 1 */
+        if ( (rsp[1] & 0x1) || (rsp[1] & 0x4) )
+        {
+            zm_debug_msg1("Set Frequency fail : ret = ", rsp[1]);
+
+            /* 1. AGC Calibration fail                                  */
+            /* 2. Noise Calibration finish but error NoiseFloor value   */
+            /*      and not in sitesurvey, try more twice               */
+            if ( hpPriv->isSiteSurvey == 2 )
+            {
+                if ( hpPriv->recordFreqRetryCounter < 2 )
+                {
+                    /* cold reset */
+                    zfHpSetFrequencyEx(dev, hpPriv->latestFrequency,
+                                            hpPriv->latestBw40,
+                                            hpPriv->latestExtOffset,
+                                            2);
+                    hpPriv->recordFreqRetryCounter++;
+                    zm_debug_msg1("Retry to issue the frequency change command(cold reset) counter = ", hpPriv->recordFreqRetryCounter);
+                }
+                else
+                {
+                    /* Fail : we would not accept this result! */
+                    zm_debug_msg0("\n\n\n\n  Fail twice cold reset \n\n\n\n");
+                    hpPriv->coldResetNeedFreq = 0;
+                    hpPriv->recordFreqRetryCounter = 0;
+                    zfCoreSetFrequencyComplete(dev);
+                }
+            }
+            else
+            {
+                /* in sitesurvey, coldreset in next channel */
+                hpPriv->coldResetNeedFreq = 1;
+                hpPriv->recordFreqRetryCounter = 0;
+                zfCoreSetFrequencyComplete(dev);
+            }
+        }
+        else if (rsp[1] & 0x2)
+        {
+            zm_debug_msg1("Set Frequency fail 2 : ret = ", rsp[1]);
+
+            /* Noise Calibration un-finish                          */
+            /*      and not in sitesurvey, try more once            */
+            if ( hpPriv->isSiteSurvey == 2 )
+            {
+                if ( hpPriv->recordFreqRetryCounter < 1 )
+                {
+                    /* cold reset */
+                    zfHpSetFrequencyEx(dev, hpPriv->latestFrequency,
+                                            hpPriv->latestBw40,
+                                            hpPriv->latestExtOffset,
+                                            2);
+                    hpPriv->recordFreqRetryCounter++;
+                    zm_debug_msg1("2 Retry to issue the frequency change command(cold reset) counter = ", hpPriv->recordFreqRetryCounter);
+                }
+                else
+                {
+                    /* Fail : we would not accept this result! */
+                    zm_debug_msg0("\n\n\n\n  2 Fail twice cold reset \n\n\n\n");
+                    hpPriv->coldResetNeedFreq = 0;
+                    hpPriv->recordFreqRetryCounter = 0;
+                    zfCoreSetFrequencyComplete(dev);
+                }
+            }
+            else
+            {
+                /* in sitesurvey, skip this frequency */
+                hpPriv->coldResetNeedFreq = 0;
+                hpPriv->recordFreqRetryCounter = 0;
+                zfCoreSetFrequencyComplete(dev);
+            }
+        }
+        //else if (rsp[1] & 0x4)
+        //{
+        //    zm_debug_msg1("Set Frequency fail 3 : ret = ", rsp[1]);
+        //    hpPriv->coldResetNeedFreq = 0;
+        //    hpPriv->recordFreqRetryCounter = 0;
+        //    zfCoreSetFrequencyComplete(dev);
+        //}
+        else
+        {
+            //hpPriv->freqRetryCounter = 0;
+            zm_debug_msg2(" return complete, ret = ", rsp[1]);
+
+            /* set bb_heavy_clip_enable */
+            if (hpPriv->enableBBHeavyClip && hpPriv->hwBBHeavyClip &&
+                hpPriv->doBBHeavyClip)
+            {
+                u32_t setValue = 0x200;
+
+                setValue |= hpPriv->setValueHeavyClip;
+
+                //zm_dbg(("Do heavy clip setValue = %d\n", setValue));
+
+                zfDelayWriteInternalReg(dev, 0x99e0+0x1bc000, setValue);
+                zfFlushDelayWrite(dev);
+            }
+
+            hpPriv->coldResetNeedFreq = 0;
+            hpPriv->recordFreqRetryCounter = 0;
+    	    zfCoreSetFrequencyComplete(dev);
+    	}
+
+        #if 1
+        // Read the Noise Floor value !
+        nf = ((rsp[2]>>19) & 0x1ff);
+        if ((nf & 0x100) != 0x0)
+        {
+            noisefloor[0] = 0 - ((nf ^ 0x1ff) + 1);
+        }
+        else
+        {
+            noisefloor[0] = nf;
+        }
+
+        //zm_debug_msg1("Noise Floor[1] = ", noisefloor[0]);
+
+        nf = ((rsp[3]>>19) & 0x1ff);
+        if ((nf & 0x100) != 0x0)
+        {
+            noisefloor[1] = 0 - ((nf ^ 0x1ff) + 1);
+        }
+        else
+        {
+            noisefloor[1] = nf;
+        }
+
+        //zm_debug_msg1("Noise Floor[2] = ", noisefloor[1]);
+
+        nf = ((rsp[5]>>23) & 0x1ff);
+        if ((nf & 0x100) != 0x0)
+        {
+            noisefloor[2] = 0 - ((nf ^ 0x1ff) + 1);
+        }
+        else
+        {
+            noisefloor[2] = nf;
+        }
+
+        //zm_debug_msg1("Noise Floor ext[1] = ", noisefloor[2]);
+
+        nf = ((rsp[6]>>23) & 0x1ff);
+        if ((nf & 0x100) != 0x0)
+        {
+            noisefloor[3] = 0 - ((nf ^ 0x1ff) + 1);
+        }
+        else
+        {
+            noisefloor[3] = nf;
+        }
+
+        //zm_debug_msg1("Noise Floor ext[2] = ", noisefloor[3]);
+
+        //zm_debug_msg1("Is Site Survey = ", hpPriv->isSiteSurvey);
+        #endif
+    }
+    else if (src == ZM_CMD_SET_KEY)
+    {
+        zfCoreSetKeyComplete(dev);
+    }
+    else if (src == ZM_CWM_READ)
+    {
+        zm_msg2_mm(ZM_LV_0, "CWM rsp[1]=", rsp[1]);
+        zm_msg2_mm(ZM_LV_0, "CWM rsp[2]=", rsp[2]);
+        zfCoreCwmBusy(dev, zfCwmIsExtChanBusy(rsp[1], rsp[2]));
+    }
+    else if (src == ZM_MAC_READ)
+    {
+        /* rsp[1] = ZM_SEEPROM_MAC_ADDRESS_OFFSET;   */
+        /* rsp[2] = ZM_SEEPROM_MAC_ADDRESS_OFFSET+4; */
+        /* rsp[3] = ZM_SEEPROM_REGDOMAIN_OFFSET;     */
+        /* rsp[4] = ZM_SEEPROM_VERISON_OFFSET;       */
+        /* rsp[5] = ZM_SEEPROM_HARDWARE_TYPE_OFFSET; */
+        /* rsp[6] = ZM_SEEPROM_HW_HEAVY_CLIP;        */
+
+        u8_t addr[6], CCS, WWR;
+        u16_t CountryDomainCode;
+
+        /* BB heavy clip */
+        //hpPriv->eepromHeavyClipFlag = (u8_t)((rsp[6]>>24) & 0xff); // force enable 8107
+        //zm_msg2_mm(ZM_LV_0, "eepromHeavyClipFlag", hpPriv->eepromHeavyClipFlag);
+        #if 0
+        if (hpPriv->hwBBHeavyClip)
+        {
+            zm_msg0_mm(ZM_LV_0, "enable BB Heavy Clip");
+        }
+        else
+        {
+            zm_msg0_mm(ZM_LV_0, "Not enable BB Heavy Clip");
+        }
+        #endif
+        zm_msg2_mm(ZM_LV_0, "MAC rsp[1]=", rsp[1]);
+        zm_msg2_mm(ZM_LV_0, "MAC rsp[2]=", rsp[2]);
+
+        addr[0] = (u8_t)(rsp[1] & 0xff);
+        addr[1] = (u8_t)((rsp[1]>>8) & 0xff);
+        addr[2] = (u8_t)((rsp[1]>>16) & 0xff);
+        addr[3] = (u8_t)((rsp[1]>>24) & 0xff);
+        addr[4] = (u8_t)(rsp[2] & 0xff);
+        addr[5] = (u8_t)((rsp[2]>>8) & 0xff);
+/*#ifdef ZM_FB50
+        addr[0] = (u8_t)(0 & 0xff);
+        addr[1] = (u8_t)(3 & 0xff);
+        addr[2] = (u8_t)(127 & 0xff);
+        addr[3] = (u8_t)(0 & 0xff);
+        addr[4] = (u8_t)(9 & 0xff);
+        addr[5] = (u8_t)(11 & 0xff);
+#endif*/
+
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_MAC_ADDR_L,
+                ((((u32_t)addr[3])<<24) | (((u32_t)addr[2])<<16) | (((u32_t)addr[1])<<8) | addr[0]));
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_MAC_ADDR_H,
+                ((((u32_t)addr[5])<<8) | addr[4]));
+        zfFlushDelayWrite(dev);
+
+        wd->ledStruct.ledMode[0] = (u16_t)(rsp[5]&0xffff);
+        wd->ledStruct.ledMode[1] = (u16_t)(rsp[5]>>16);
+        zm_msg2_mm(ZM_LV_0, "ledMode[0]=", wd->ledStruct.ledMode[0]);
+        zm_msg2_mm(ZM_LV_0, "ledMode[1]=", wd->ledStruct.ledMode[1]);
+
+        /* Regulatory Related Setting */
+        zm_msg2_mm(ZM_LV_0, "RegDomain rsp=", rsp[3]);
+        zm_msg2_mm(ZM_LV_0, "OpFlags+EepMisc=", rsp[4]);
+        hpPriv->OpFlags = (u8_t)((rsp[4]>>16) & 0xff);
+        if ((rsp[2] >> 24) == 0x1) //Tx mask == 0x1
+        {
+            zm_msg0_mm(ZM_LV_0, "OTUS 1x2");
+            hpPriv->halCapability |= ZM_HP_CAP_11N_ONE_TX_STREAM;
+        }
+        else
+        {
+            zm_msg0_mm(ZM_LV_0, "OTUS 2x2");
+        }
+        if (hpPriv->OpFlags & 0x1)
+        {
+            hpPriv->halCapability |= ZM_HP_CAP_5G;
+        }
+        if (hpPriv->OpFlags & 0x2)
+        {
+            hpPriv->halCapability |= ZM_HP_CAP_2G;
+        }
+
+
+        CCS = (u8_t)((rsp[3] & 0x8000) >> 15);
+        WWR = (u8_t)((rsp[3] & 0x4000) >> 14);
+        CountryDomainCode = (u16_t)(rsp[3] & 0x3FFF);
+
+        if (rsp[3] != 0xffffffff)
+        {
+            if (CCS)
+            {
+                //zm_debug_msg0("CWY - Get Regulation Table from Country Code");
+                zfHpGetRegulationTablefromCountry(dev, CountryDomainCode);
+            }
+            else
+            {
+                //zm_debug_msg0("CWY - Get Regulation Table from Reg Domain");
+                zfHpGetRegulationTablefromRegionCode(dev, CountryDomainCode);
+            }
+            if (WWR)
+            {
+                //zm_debug_msg0("CWY - Enable 802.11d");
+                /* below line shall be unmarked after A band is ready */
+                //zfiWlanSetDot11DMode(dev, 1);
+            }
+        }
+        else
+        {
+            zfHpGetRegulationTablefromRegionCode(dev, NO_ENUMRD);
+        }
+
+        zfCoreMacAddressNotify(dev, addr);
+
+    }
+    else if (src == ZM_EEPROM_READ)
+    {
+#if 0
+        u8_t addr[6], CCS, WWR;
+        u16_t CountryDomainCode;
+#endif
+        for (i=0; i<ZM_HAL_MAX_EEPROM_PRQ; i++)
+        {
+            if (hpPriv->eepromImageIndex < 1024)
+            {
+                hpPriv->eepromImage[hpPriv->eepromImageIndex++] = rsp[i+1];
+            }
+        }
+
+        if (hpPriv->eepromImageIndex == (ZM_HAL_MAX_EEPROM_REQ*ZM_HAL_MAX_EEPROM_PRQ))
+        {
+            #if 0
+            for (i=0; i<1024; i++)
+            {
+                zm_msg2_mm(ZM_LV_0, "index=", i);
+                zm_msg2_mm(ZM_LV_0, "eepromImage=", hpPriv->eepromImage[i]);
+            }
+            #endif
+            zm_msg2_mm(ZM_LV_0, "MAC [1]=", hpPriv->eepromImage[0x20c/4]);
+            zm_msg2_mm(ZM_LV_0, "MAC [2]=", hpPriv->eepromImage[0x210/4]);
+#if 0
+            addr[0] = (u8_t)(hpPriv->eepromImage[0x20c/4] & 0xff);
+            addr[1] = (u8_t)((hpPriv->eepromImage[0x20c/4]>>8) & 0xff);
+            addr[2] = (u8_t)((hpPriv->eepromImage[0x20c/4]>>16) & 0xff);
+            addr[3] = (u8_t)((hpPriv->eepromImage[0x20c/4]>>24) & 0xff);
+            addr[4] = (u8_t)(hpPriv->eepromImage[0x210/4] & 0xff);
+            addr[5] = (u8_t)((hpPriv->eepromImage[0x210/4]>>8) & 0xff);
+
+            zfCoreMacAddressNotify(dev, addr);
+
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_MAC_ADDR_L,
+                    ((((u32_t)addr[3])<<24) | (((u32_t)addr[2])<<16) | (((u32_t)addr[1])<<8) | addr[0]));
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_MAC_ADDR_H,
+                    ((((u32_t)addr[5])<<8) | addr[4]));
+            zfFlushDelayWrite(dev);
+
+            /* Regulatory Related Setting */
+            zm_msg2_mm(ZM_LV_0, "RegDomain =", hpPriv->eepromImage[0x208/4]);
+            CCS = (u8_t)((hpPriv->eepromImage[0x208/4] & 0x8000) >> 15);
+            WWR = (u8_t)((hpPriv->eepromImage[0x208/4] & 0x4000) >> 14);
+            /* below line shall be unmarked after A band is ready */
+            //CountryDomainCode = (u16_t)(hpPriv->eepromImage[0x208/4] & 0x3FFF);
+            CountryDomainCode = 8;
+            if (CCS)
+            {
+                //zm_debug_msg0("CWY - Get Regulation Table from Country Code");
+                zfHpGetRegulationTablefromCountry(dev, CountryDomainCode);
+            }
+            else
+            {
+                //zm_debug_msg0("CWY - Get Regulation Table from Reg Domain");
+                zfHpGetRegulationTablefromRegionCode(dev, CountryDomainCode);
+            }
+            if (WWR)
+            {
+                //zm_debug_msg0("CWY - Enable 802.11d");
+                /* below line shall be unmarked after A band is ready */
+                //zfiWlanSetDot11DMode(dev, 1);
+            }
+#endif
+            zfCoreHalInitComplete(dev);
+        }
+        else
+        {
+            hpPriv->eepromImageRdReq++;
+            zfHpLoadEEPROMFromFW(dev);
+        }
+    }
+    else if (src == ZM_EEPROM_WRITE)
+    {
+        zfwDbgWriteEepromDone(dev, cmd[1], cmd[2]);
+    }
+    else if (src == ZM_ANI_READ)
+    {
+        u32_t cycleTime, ctlClear;
+
+        zm_msg2_mm(ZM_LV_0, "ANI rsp[1]=", rsp[1]);
+        zm_msg2_mm(ZM_LV_0, "ANI rsp[2]=", rsp[2]);
+        zm_msg2_mm(ZM_LV_0, "ANI rsp[3]=", rsp[3]);
+        zm_msg2_mm(ZM_LV_0, "ANI rsp[4]=", rsp[4]);
+
+        hpPriv->ctlBusy += rsp[1];
+        hpPriv->extBusy += rsp[2];
+
+        cycleTime = 100000; //100 miniseconds
+
+        if (cycleTime > rsp[1])
+        {
+            ctlClear = (cycleTime - rsp[1]) / 100;
+        }
+        else
+        {
+            ctlClear = 0;
+        }
+        if (wd->aniEnable)
+            zfHpAniArPoll(dev, ctlClear, rsp[3], rsp[4]);
+    }
+    else if (src == ZM_CMD_ECHO)
+    {
+        if ( ((struct zsHpPriv*)wd->hpPrivate)->halReInit )
+        {
+            zfCoreHalInitComplete(dev);
+            ((struct zsHpPriv*)wd->hpPrivate)->halReInit = 0;
+        }
+        else
+        {
+            zfHpLoadEEPROMFromFW(dev);
+        }
+    }
+    else if (src == ZM_OID_FW_DL_INIT)
+    {
+        zfwDbgDownloadFwInitDone(dev);
+    }
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfWriteRegInternalReg       */
+/*      Write on chip internal register immediately.                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*      val : value                                                     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u32_t zfWriteRegInternalReg(zdev_t* dev, u32_t addr, u32_t val)
+{
+    u32_t cmd[3];
+    u16_t ret;
+
+    cmd[0] = 0x00000108;
+    cmd[1] = addr;
+    cmd[2] = val;
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_OID_INTERNAL_WRITE, NULL);
+    return ret;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfDelayWriteInternalReg     */
+/*      Write on chip internal register, write operation may be         */
+/*      postponed to form a multiple write command.                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*      val : value                                                     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : command been postponed                                      */
+/*      1 : commands been executed                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfDelayWriteInternalReg(zdev_t* dev, u32_t addr, u32_t val)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t i;
+    u16_t ret;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zmw_declare_for_critical_section();
+
+    /* enter critical section */
+    zmw_enter_critical_section(dev);
+
+    /* Store command to global buffer */
+    hpPriv->cmd.delayWcmdAddr[hpPriv->cmd.delayWcmdCount] = addr;
+    hpPriv->cmd.delayWcmdVal[hpPriv->cmd.delayWcmdCount++] = val;
+
+    /* If pending command reach size limit */
+    if ((hpPriv->cmd.delayWcmdCount) >= ((ZM_MAX_CMD_SIZE - 4) >> 3))
+    {
+        cmd[0] = 0x00000100 + (hpPriv->cmd.delayWcmdCount<<3);
+
+        /* copy command to cmd buffer */
+        for (i=0; i<hpPriv->cmd.delayWcmdCount; i++)
+        {
+            cmd[1+(i<<1)] = hpPriv->cmd.delayWcmdAddr[i];
+            cmd[2+(i<<1)] = hpPriv->cmd.delayWcmdVal[i];
+        }
+        /* reset pending command */
+        hpPriv->cmd.delayWcmdCount = 0;
+
+        /* leave critical section */
+        zmw_leave_critical_section(dev);
+
+        /* issue write command */
+        ret = zfIssueCmd(dev, cmd, 4+(i<<3), ZM_OID_INTERNAL_WRITE, NULL);
+
+        return 1;
+    }
+    else
+    {
+        /* leave critical section */
+        zmw_leave_critical_section(dev);
+
+        return 0;
+    }
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfFlushDelayWrite           */
+/*      Flush pending write command.                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : no pending command                                          */
+/*      1 : commands been executed                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfFlushDelayWrite(zdev_t* dev)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t i;
+    u16_t ret;
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zmw_declare_for_critical_section();
+
+    /* enter critical section */
+    zmw_enter_critical_section(dev);
+
+    /* If there is pending command */
+    if (hpPriv->cmd.delayWcmdCount > 0)
+    {
+        cmd[0] = 0x00000100 + (hpPriv->cmd.delayWcmdCount<<3);
+
+        /* copy command to cmd buffer */
+        for (i=0; i<hpPriv->cmd.delayWcmdCount; i++)
+        {
+            cmd[1+(i<<1)] = hpPriv->cmd.delayWcmdAddr[i];
+            cmd[2+(i<<1)] = hpPriv->cmd.delayWcmdVal[i];
+        }
+        /* reset pending command */
+        hpPriv->cmd.delayWcmdCount = 0;
+
+        /* leave critical section */
+        zmw_leave_critical_section(dev);
+
+        /* issue write command */
+        ret = zfIssueCmd(dev, cmd, 4+(i<<3), ZM_OID_INTERNAL_WRITE, NULL);
+
+        return 1;
+    }
+    else
+    {
+        /* leave critical section */
+        zmw_leave_critical_section(dev);
+
+        return 0;
+    }
+}
+
+
+u32_t zfiDbgDelayWriteReg(zdev_t* dev, u32_t addr, u32_t val)
+{
+	zfDelayWriteInternalReg(dev, addr, val);
+	return 0;
+}
+
+u32_t zfiDbgFlushDelayWrite(zdev_t* dev)
+{
+	zfFlushDelayWrite(dev);
+	return 0;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgWriteReg              */
+/*      Write register.                                                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*      val : value                                                     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgWriteReg(zdev_t* dev, u32_t addr, u32_t val)
+{
+    u32_t cmd[3];
+    u16_t ret;
+
+    cmd[0] = 0x00000108;
+    cmd[1] = addr;
+    cmd[2] = val;
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_OID_WRITE, 0);
+    return ret;
+}
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgWriteFlash            */
+/*      Write flash.                                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*      val : value                                                     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Yjsung        ZyDAS Technology Corporation    2007.02           */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgWriteFlash(zdev_t* dev, u32_t addr, u32_t val)
+{
+    u32_t cmd[3];
+    u16_t ret;
+
+    //cmd[0] = 0x0000B008;
+	/* len[0] : type[0xB0] : seq[?] */
+    cmd[0] = 8 | (ZM_CMD_WFLASH << 8);
+    cmd[1] = addr;
+    cmd[2] = val;
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_OID_WRITE, 0);
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgWriteEeprom            */
+/*      Write EEPROM.                                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*      val : value                                                     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul        ZyDAS Technology Corporation    2007.06             */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgWriteEeprom(zdev_t* dev, u32_t addr, u32_t val)
+{
+    u32_t cmd[3];
+    u16_t ret;
+
+    //cmd[0] = 0x0000B008;
+	/* len[0] : type[0xB0] : seq[?] */
+    cmd[0] = 8 | (ZM_CMD_WREEPROM << 8);
+    cmd[1] = addr;
+    cmd[2] = val;
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_EEPROM_WRITE, 0);
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgBlockWriteEeprom      */
+/*      Block Write Eeprom.                                             */
+/*                                                                      */
+/*      p.s: now,it will write 16 bytes register data per block (N=4)   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*      buf : input data buffer pointer                                 */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul        ZyDAS Technology Corporation    2007.06             */
+/*                                                                      */
+/************************************************************************/
+//#define N       buflen/4
+//#define SIZE    (2*N+1)
+
+u32_t zfiDbgBlockWriteEeprom(zdev_t* dev, u32_t addr, u32_t* buf)
+{
+    u32_t cmd[9];  //2N+1
+    u16_t ret,i;
+
+    //cmd[0] = 0x0000B008;
+	  /* len[0] : type[0xB0] : seq[?] */
+
+    //cmd[0] = (8*N) | (ZM_CMD_WFLASH << 8);
+    cmd[0] = 32 | (ZM_CMD_WREEPROM << 8);    //8N
+
+    for (i=0; i<4; i++)   // i<N
+    {
+        cmd[(2*i)+1] = addr+(4*i);
+        cmd[(2*i)+2] = *(buf+i);
+    }
+
+    ret = zfIssueCmd(dev, cmd, 36, ZM_EEPROM_WRITE, 0);    //8N+4
+
+    // added for EEPROMUpdate, wait a moment for prevent cmd queue full!
+    //zfwSleep(dev, 1);
+
+    return ret;
+}
+
+
+/* write EEPROM with wrlen : wrlen must be 4*n */
+/* command format : cmd_info(4) + addr(4) + eeprom(wrlen) */
+u32_t zfiDbgBlockWriteEeprom_v2(zdev_t* dev, u32_t addr, u32_t* buf, u32_t wrlen)
+{
+    u32_t cmd[16];
+    u16_t ret,i;
+
+	  /* len[0] : type[0xB0] : seq[?] */
+	  /* len = addr(4) + eeprom_block(wrlen) */
+    cmd[0] = (wrlen+4) | (ZM_CMD_MEM_WREEPROM << 8);
+    cmd[1] = addr;
+
+    for (i=0; i<(wrlen/4); i++)   // i<wrlen/4
+    {
+        cmd[2+i] = *(buf+i);
+    }
+    /* cmd_info(4) + addr(4) + eeprom(wrlen) */
+    ret = zfIssueCmd(dev, cmd, (u16_t)(wrlen+8), ZM_EEPROM_WRITE, 0);
+
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfDbgOpenEeprom            */
+/*      Open EEPROM.                                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                ZyDAS Technology Corporation    2007.06     */
+/*                                                                      */
+/************************************************************************/
+void zfDbgOpenEeprom(zdev_t* dev)
+{
+    // unlock EEPROM
+    zfDelayWriteInternalReg(dev, 0x1D1400, 0x12345678);
+    zfDelayWriteInternalReg(dev, 0x1D1404, 0x55aa00ff);
+    zfDelayWriteInternalReg(dev, 0x1D1408, 0x13579ace);
+    zfDelayWriteInternalReg(dev, 0x1D1414, 0x0);
+    zfFlushDelayWrite(dev);
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfDbgCloseEeprom            */
+/*      Close EEPROM.                                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                ZyDAS Technology Corporation    2007.05     */
+/*                                                                      */
+/************************************************************************/
+void zfDbgCloseEeprom(zdev_t* dev)
+{
+    // lock EEPROM
+    zfDelayWriteInternalReg(dev, 0x1D1400, 0x87654321);
+    //zfDelayWriteInternalReg(dev, 0x1D1404, 0xffffffff);
+    //zfDelayWriteInternalReg(dev, 0x1D1408, 0xffffffff);
+    //zfDelayWriteInternalReg(dev, 0x1D1414, 0x100);
+    zfFlushDelayWrite(dev);
+}
+#if 0
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiSeriallyWriteEeprom      */
+/*      Write EEPROM Serially.                                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : start address of writing EEPROM                          */
+/*      buf : input data buffer                                         */
+/*      buflen : size of input data buffer                              */
+/*               (length of data write into EEPROM)                     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*                                                                      */
+/*                                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                ZyDAS Technology Corporation    2007.06     */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiSeriallyWriteEeprom(zdev_t* dev, u32_t addr, u32_t* buf, u32_t buflen)
+{
+    u32_t count;
+    u16_t i,ret,blocksize;
+    u8_t  temp[2];
+
+    // per 4 bytes = 1 count
+    count = buflen/4;
+
+    // Open EEPROM
+    zfDbgOpenEeprom(dev);
+
+    // Write EEPROM
+    for (i=0; i<count; i++)
+    {
+        if (zfwWriteEeprom(dev, (addr+(4*i)), *(buf+i), 0) != 0)
+        {
+            // Update failed, Close EEPROM
+            zm_debug_msg0("zfwWriteEeprom failed \n");
+            zfDbgCloseEeprom(dev);
+            return 1;
+        }
+    }
+
+    // Close EEPROM
+    zfDbgCloseEeprom(dev);
+    return 0;
+}
+#endif
+#if 0
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiSeriallyBlockWriteEeprom */
+/*       Block Write EEPROM Serially.                                   */
+/*      (BlockWrite: per 16bytes write EEPROM once)                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*      buf : input data buffer                                         */
+/*      buflen : access data size of buf                                */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                ZyDAS Technology Corporation    2007.05     */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiSeriallyBlockWriteEeprom(zdev_t* dev, u32_t addr, u32_t* buf, u32_t buflen)
+{
+    u32_t count;
+    u16_t i,ret,blocksize;
+    u8_t  temp[2];
+
+    // per 4 bytes = 1 count
+    count = buflen/4;
+
+    // Open EEPROM
+    zfDbgOpenEeprom(dev);
+
+    // Write EEPROM
+    // EEPROM Write start address from: 0x1000!?
+    // per 16bytes(N=4) block write EEPROM once
+    for (i=0; i<(count/4); i++)   // count/N
+    {
+        //zfiDbgBlockWriteEeprom(dev, (addr+(4*N*i)), buf+(N*i));
+        //zfiDbgBlockWriteEeprom(dev, (addr+(16*i)), buf+(4*i));
+        if (zfwBlockWriteEeprom(dev, (addr+(16*i)), buf+(4*i), 0) != 0)
+        {
+            zm_debug_msg0("zfiDbgBlockWriteEeprom failed \n");
+            // Close EEPROM
+            zfDbgCloseEeprom(dev);
+            return 1;
+        }
+    }
+
+    // Close EEPROM
+    zfDbgCloseEeprom(dev);
+    return 0;
+}
+#endif
+#if 0
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgDumpEeprom            */
+/*      Dump EEPROM.                                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : start address of dumping EEPROM                          */
+/*      datalen :  length of access EEPROM data                           */
+/*      buf :  point of buffer, the buffer saved dump data              */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                ZyDAS Technology Corporation    2007.06     */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgDumpEeprom(zdev_t* dev, u32_t addr, u32_t datalen, u32_t* buf)
+{
+    u32_t count;
+    u16_t i,ret;
+
+    count = datalen/4;
+
+    // over EEPROM length
+    if(datalen > 0x2000)
+    {
+        return 1;
+    }
+
+    for(i=0; i<count; i++)
+    {
+        buf[i] = zfwReadEeprom(dev, addr+(4*i));
+    }
+
+    return 0;
+}
+#endif
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgReadReg               */
+/*      Read register.                                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgReadReg(zdev_t* dev, u32_t addr)
+{
+    u32_t cmd[2];
+    u16_t ret;
+
+    cmd[0] = 0x00000004;
+    cmd[1] = addr;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_READ, 0);
+    return ret;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgReadTally             */
+/*      Read register.                                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgReadTally(zdev_t* dev)
+{
+    u32_t cmd[1];
+    u16_t ret;
+	zmw_get_wlan_dev(dev);
+
+	if ( ((struct zsHpPriv*)wd->hpPrivate)->halReInit )
+	{
+	    return 1;
+	}
+
+	/* len[0] : type[0x81] : seq[?] */
+    cmd[0] = 0 | (ZM_CMD_TALLY << 8);
+    ret = zfIssueCmd(dev, cmd, 4, ZM_OID_TALLY, 0);
+
+	/* len[0] : type[0x82] : seq[?] */
+    cmd[0] = 0 | (ZM_CMD_TALLY_APD << 8);
+    ret = zfIssueCmd(dev, cmd, 4, ZM_OID_TALLY_APD, 0);
+
+    return ret;
+}
+
+
+u32_t zfiDbgSetIFSynthesizer(zdev_t* dev, u32_t value)
+{
+    u32_t cmd[2];
+    u16_t ret;
+
+	/* len[4] : type[0x32] : seq[?] */
+    cmd[0] = 0x4 | (ZM_OID_SYNTH << 8);
+    cmd[1] = value;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_SYNTH, 0);
+    return ret;
+}
+
+u32_t zfiDbgQueryHwTxBusy(zdev_t* dev)
+{
+    u32_t cmd[1];
+    u16_t ret;
+
+	/* len[4] : type[0xC0] : seq[?] */
+	cmd[0] = 0 | (ZM_CMD_DKTX_STATUS << 8);
+
+    ret = zfIssueCmd(dev, cmd, 4, ZM_OID_DKTX_STATUS, 0);
+    return ret;
+}
+
+//Paul++
+#if 0
+u16_t zfHpBlockEraseFlash(zdev_t *dev, u32_t addr)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret;
+
+    cmd[0] = 0x00000004 | (ZM_CMD_FLASH_ERASE << 8);
+    cmd[1] = addr;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_INTERNAL_WRITE, NULL);
+    return ret;
+}
+#endif
+
+#if 0
+u16_t zfiDbgProgramFlash(zdev_t *dev, u32_t offset, u32_t len, u32_t *data)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret;
+    u16_t i;
+
+
+    cmd[0] = (ZM_CMD_FLASH_PROG << 8) | ((len+8) & 0xff);
+    cmd[1] = offset;
+    cmd[2] = len;
+
+    for (i = 0; i < (len >> 2); i++)
+    {
+         cmd[3+i] = data[i];
+    }
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_OID_FLASH_PROGRAM, NULL);
+
+    return ret;
+}
+#endif
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgChipEraseFlash        */
+/*      Chip Erase Flash.                                               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                Atheros Technology Corporation    2007.09   */
+/*                                                                      */
+/************************************************************************/
+u16_t zfiDbgChipEraseFlash(zdev_t *dev)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret;
+
+    cmd[0] = 0x00000000 | (ZM_CMD_FLASH_ERASE << 8);
+
+    ret = zfIssueCmd(dev, cmd, 4, ZM_OID_INTERNAL_WRITE, NULL);
+    return ret;
+}
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgGetFlashCheckSum      */
+/*      Get FlashCheckSum.                                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : Start address of getchksum                               */
+/*      len : total lenth of calculate getchksum                        */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                Atheros Technology Corporation    2007.08   */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgGetFlashCheckSum(zdev_t *dev, u32_t addr, u32_t len)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u32_t ret;
+
+    cmd[0] = 0x00000008 | (ZM_CMD_FLASH_CHKSUM << 8);
+    cmd[1] = addr;
+    cmd[2] = len;
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_OID_FLASH_CHKSUM, NULL);
+
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgReadFlash             */
+/*      Read Flash.                                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : Start address of read flash                              */
+/*      len : total lenth of read flash data                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                Atheros Technology Corporation    2007.09   */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgReadFlash(zdev_t *dev, u32_t addr, u32_t len)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u32_t ret;
+
+    cmd[0] = len | (ZM_CMD_FLASH_READ << 8);
+    cmd[1] = addr;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_FLASH_READ, NULL);
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDownloadFwSet            */
+/*      Before Download FW,                                             */
+/*      Command FW to Software reset and close watch dog control.       */
+/*                                                                      */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                Atheros Technology Corporation    2007.09   */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDownloadFwSet(zdev_t *dev)
+{
+//softwarereset
+//close watch dog
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u32_t ret;
+
+    cmd[0] = 0x00000008 | (ZM_CMD_FW_DL_INIT << 8);
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_OID_FW_DL_INIT, NULL);
+
+    return ret;
+}
+//Paul--
diff --git a/drivers/staging/otus/hal/hpusb.c b/drivers/staging/otus/hal/hpusb.c
new file mode 100644
index 000000000000..4b76de93fff0
--- /dev/null
+++ b/drivers/staging/otus/hal/hpusb.c
@@ -0,0 +1,1584 @@
+/*
+ * Copyright (c) 2000-2005 ZyDAS Technology Corporation
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : ud.c                                                  */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains USB descriptor functions.                  */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "../80211core/cprecomp.h"
+#include "hpani.h"
+#include "hpusb.h"
+
+extern void zfwUsbCmd(zdev_t* dev, u8_t endpt, u32_t* cmd, u16_t cmdLen);
+
+extern void zfIdlRsp(zdev_t* dev, u32_t* rsp, u16_t rspLen);
+extern u16_t zfDelayWriteInternalReg(zdev_t* dev, u32_t addr, u32_t val);
+extern u16_t zfFlushDelayWrite(zdev_t* dev);
+
+
+#define USB_ENDPOINT_TX_INDEX   1
+#define USB_ENDPOINT_RX_INDEX   2
+#define USB_ENDPOINT_INT_INDEX  3
+#define USB_ENDPOINT_CMD_INDEX  4
+
+void zfIdlCmd(zdev_t* dev, u32_t* cmd, u16_t cmdLen)
+{
+#if ZM_SW_LOOP_BACK != 1
+    zfwUsbCmd(dev, USB_ENDPOINT_CMD_INDEX, cmd, cmdLen);
+#endif
+
+    return;
+}
+
+
+/* zfAdjustCtrlSetting: fit OUTS format */
+/*     convert MIMO2 to OUTS             */
+void zfAdjustCtrlSetting(zdev_t* dev, u16_t* header, zbuf_t* buf)
+{
+    /* MIMO2 => OUTS FB-50 */
+    /* length not change, only modify format */
+
+    u32_t oldMT;
+	u32_t oldMCS;
+
+    u32_t phyCtrl;
+    u32_t oldPhyCtrl;
+
+    u16_t tpc = 0;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+   /* mm */
+    if (header == NULL)
+    {
+        oldPhyCtrl = zmw_buf_readh(dev, buf, 4) | ((u32_t)zmw_buf_readh(dev, buf, 6) << 16);
+    }
+    else
+    {
+        oldPhyCtrl = header[2] | ((u32_t)header[3] <<16);
+    }
+
+	phyCtrl = 0;
+
+
+	/* MT : Bit[1~0] */
+	oldMT = oldPhyCtrl&0x3;
+	phyCtrl |= oldMT;
+    if ( oldMT == 0x3 )   /* DL-OFDM (Duplicate Legacy OFDM) */
+		phyCtrl |= 0x1;
+
+
+	/* PT : Bit[2]    HT PT: 0 Mixed mode    1 Green field */
+	phyCtrl |= (oldPhyCtrl&0x4);
+
+	/* Bandwidth control : Bit[4~3] */
+	if ( oldPhyCtrl&0x800000 )    /* Bit23 : 40M */
+	{
+		#if 0
+		if (oldMT == 0x3)             /* DL-OFDM */
+            phyCtrl |= (0x3<<3);   /* 40M duplicate */
+		else
+			phyCtrl |= (0x2<<3);   /* 40M shared */
+		#else
+		if (oldMT == 0x2 && ((struct zsHpPriv*)wd->hpPrivate)->hwBw40)
+		{
+			phyCtrl |= (0x2<<3);   /* 40M shared */
+		}
+		#endif
+	}
+	else {
+        oldPhyCtrl &= ~0x80000000;
+    }
+
+	/* MCS : Bit[24~18] */
+	oldMCS = (oldPhyCtrl&0x7f0000)>>16;  /* Bit[22~16] */
+	phyCtrl |= (oldMCS<<18);
+
+	/* Short GI : Bit[31]*/
+    phyCtrl |= (oldPhyCtrl&0x80000000);
+
+	/* AM : Antenna mask */
+	//if ((oldMT == 2) && (oldMCS > 7))
+	if (hpPriv->halCapability & ZM_HP_CAP_11N_ONE_TX_STREAM)
+	{
+	    phyCtrl |= (0x1<<15);
+	}
+	else
+	{
+	    /* HT                     Tx 2 chain */
+	    /* OFDM 6M/9M/12M/18M/24M Tx 2 chain */
+	    /* OFDM 36M/48M/54M/      Tx 1 chain */
+	    /* CCK                    Tx 2 chain */
+	    if ((oldMT == 2) || (oldMT == 3))
+	    {
+	        phyCtrl |= (0x5<<15);
+	    }
+	    else if (oldMT == 1)
+	    {
+	        if ((oldMCS == 0xb) || (oldMCS == 0xf) ||
+	            (oldMCS == 0xa) || (oldMCS == 0xe) ||
+	            (oldMCS == 0x9))                       //6M/9M/12M/18M/24M
+	        {
+	            phyCtrl |= (0x5<<15);
+	        }
+	        else
+	        {
+	            phyCtrl |= (0x1<<15);
+	        }
+	    }
+	    else //(oldMT==0)
+	    {
+	        phyCtrl |= (0x5<<15);
+	    }
+	}
+	//else
+	//    phyCtrl |= (0x1<<15);
+
+	/* TPC */
+	/* TODO : accelerating these code */
+	if (hpPriv->hwFrequency < 3000)
+	{
+        if (oldMT == 0)
+        {
+            /* CCK */
+            tpc = (hpPriv->tPow2xCck[oldMCS]&0x3f);
+        }
+        else if (oldMT == 1)
+        {
+            /* OFDM */
+            if (oldMCS == 0xc)
+            {
+                tpc = (hpPriv->tPow2x2g[3]&0x3f);
+            }
+            else if (oldMCS == 0x8)
+            {
+                tpc = (hpPriv->tPow2x2g[2]&0x3f);
+            }
+            else if (oldMCS == 0xd)
+            {
+                tpc = (hpPriv->tPow2x2g[1]&0x3f);
+            }
+            else if (oldMCS == 0x9)
+            {
+                tpc = ((hpPriv->tPow2x2g[0]-hpPriv->tPow2x2g24HeavyClipOffset)&0x3f);
+            }
+            else
+            {
+                tpc = (hpPriv->tPow2x2g[0]&0x3f);
+            }
+        }
+        else if (oldMT == 2)
+        {
+            if ( oldPhyCtrl&0x800000 )    /* Bit23 : 40M */
+            {
+                /* HT 40 */
+                tpc = (hpPriv->tPow2x2gHt40[oldMCS&0x7]&0x3f);
+            }
+            else
+            {
+                /* HT 20 */
+                tpc = (hpPriv->tPow2x2gHt20[oldMCS&0x7]&0x3f);
+            }
+        }
+    }
+    else  //5GHz
+    {
+        if (oldMT == 1)
+        {
+            /* OFDM */
+            if (oldMCS == 0xc)
+            {
+                tpc = (hpPriv->tPow2x5g[3]&0x3f);
+            }
+            else if (oldMCS == 0x8)
+            {
+                tpc = (hpPriv->tPow2x5g[2]&0x3f);
+            }
+            else if (oldMCS == 0xd)
+            {
+                tpc = (hpPriv->tPow2x5g[1]&0x3f);
+            }
+            else
+            {
+                tpc = (hpPriv->tPow2x5g[0]&0x3f);
+            }
+        }
+        else if (oldMT == 2)
+        {
+            if ( oldPhyCtrl&0x800000 )    /* Bit23 : 40M */
+            {
+                /* HT 40 */
+                tpc = (hpPriv->tPow2x5gHt40[oldMCS&0x7]&0x3f);
+            }
+            else
+            {
+                /* HT 20 */
+                tpc = (hpPriv->tPow2x5gHt20[oldMCS&0x7]&0x3f);
+            }
+        }
+    }
+
+    /* Tx power adjust for HT40 */
+	/* HT40   +1dBm */
+	if ((oldMT==2) && (oldPhyCtrl&0x800000) )
+	{
+	    tpc += 2;
+	}
+	tpc &= 0x3f;
+
+    /* Evl force tx TPC */
+    if(wd->forceTxTPC)
+    {
+        tpc = (u16_t)(wd->forceTxTPC & 0x3f);
+    }
+
+    if (hpPriv->hwFrequency < 3000) {
+        wd->maxTxPower2 &= 0x3f;
+        tpc = (tpc > wd->maxTxPower2)? wd->maxTxPower2 : tpc;
+    } else {
+        wd->maxTxPower5 &= 0x3f;
+        tpc = (tpc > wd->maxTxPower5)? wd->maxTxPower5 : tpc;
+    }
+
+
+#define ZM_MIN_TPC     5
+#define ZM_TPC_OFFSET  5
+#define ZM_SIGNAL_THRESHOLD  56
+    if ((wd->sta.bScheduleScan == FALSE) && (wd->sta.bChannelScan == FALSE))
+    {
+        if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+                && (zfStaIsConnected(dev))
+                && (wd->SignalStrength > ZM_SIGNAL_THRESHOLD))
+        {
+            if (tpc > ((ZM_MIN_TPC+ZM_TPC_OFFSET)*2))
+            {
+                tpc -= (ZM_TPC_OFFSET*2);
+            }
+            else if (tpc > (ZM_MIN_TPC*2))
+            {
+                tpc = (ZM_MIN_TPC*2);
+            }
+        }
+    }
+#undef ZM_MIN_TPC
+#undef ZM_TPC_OFFSET
+#undef ZM_SIGNAL_THRESHOLD
+
+    #ifndef ZM_OTUS_LINUX_PHASE_2
+    phyCtrl |= (tpc & 0x3f) << 9;
+    #endif
+
+    /* Set bits[8:6]BF-MCS for heavy clip */
+    if ((phyCtrl&0x3) == 2)
+	{
+	    phyCtrl |= ((phyCtrl >> 12) & 0x1c0);
+    }
+
+	/* PHY control */
+    if (header == NULL)
+    {
+        zmw_buf_writeh(dev, buf, 4, (u16_t) (phyCtrl&0xffff));
+        zmw_buf_writeh(dev, buf, 6, (u16_t) (phyCtrl>>16));
+    }
+    else
+    {
+        //PHY control L
+        header[2] = (u16_t) (phyCtrl&0xffff);
+        //PHY control H
+        header[3] = (u16_t) (phyCtrl>>16);
+    }
+
+	zm_msg2_tx(ZM_LV_2, "old phy ctrl = ", oldPhyCtrl);
+    zm_msg2_tx(ZM_LV_2, "new phy ctrl = ", phyCtrl);
+	//DbgPrint("old phy ctrl =%08x \n", oldPhyCtrl);
+    //DbgPrint("new phy ctrl =%08x \n", phyCtrl);
+}
+
+
+#define EXTRA_INFO_LEN      24    //RSSI(7) + EVM(12) + PHY(1) + MACStatus(4)
+u16_t zfHpSend(zdev_t* dev, u16_t* header, u16_t headerLen,
+                u16_t* snap, u16_t snapLen,
+                u16_t* tail, u16_t tailLen, zbuf_t* buf, u16_t offset,
+                u16_t bufType, u8_t ac, u8_t keyIdx)
+{
+#if ZM_SW_LOOP_BACK == 1
+    zbuf_t *rxbuf;
+    u8_t *puRxBuf;
+    u8_t *pHdr;
+	   u8_t *psnap;
+	   u16_t plcplen = 12;
+    u16_t i;
+   	u16_t swlpOffset;
+#endif /* #if ZM_SW_LOOP_BACK == 1 */
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zm_msg1_tx(ZM_LV_1, "zfHpSend(), len = ", 12 + headerLen-8 + snapLen + zfwBufGetSize(dev, buf) + 4 + 8);
+
+	/* Adjust ctrl setting : 6N14 yjsung */
+    zfAdjustCtrlSetting(dev, header, buf);
+
+#if ZM_SW_LOOP_BACK != 1
+    hpPriv->usbSendBytes += zfwBufGetSize(dev, buf);
+    hpPriv->usbAcSendBytes[ac&0x3] += zfwBufGetSize(dev, buf);
+
+    /* Submit USB Out Urb */
+    zfwUsbSend(dev, USB_ENDPOINT_TX_INDEX, (u8_t *)header, headerLen,
+                  (u8_t *)snap, snapLen, (u8_t *)tail, tailLen, buf, offset);
+#endif
+
+#if ZM_SW_LOOP_BACK == 1
+
+    rxbuf = zfwBufAllocate(dev, plcplen + headerLen-8 + snapLen + (zfwBufGetSize(dev, buf)-offset) + 4 + EXTRA_INFO_LEN);
+    pHdr = (u8_t *) header+8;
+   	psnap = (u8_t *) snap;
+
+    zmw_enter_critical_section(dev);
+    /* software loop back */
+    /* Copy WLAN header and packet buffer */
+   	swlpOffset = plcplen;
+
+    for(i = 0; i < headerLen-8; i++)
+    {
+        zmw_rx_buf_writeb(dev, rxbuf, swlpOffset+i, pHdr[i]);
+    }
+
+   	swlpOffset += headerLen-8;
+
+    /* Copy SNAP header */
+    for(i = 0; i < snapLen; i++)
+    {
+		      zmw_rx_buf_writeb(dev, rxbuf, swlpOffset+i, psnap[i]);
+    }
+
+	   swlpOffset += snapLen;
+
+    /* Copy body from tx buf to rxbuf */
+    for(i = 0; i < (zfwBufGetSize(dev, buf)-offset); i++)
+    {
+        u8_t value = zmw_rx_buf_readb(dev, buf, i+offset);
+        zmw_rx_buf_writeb(dev, rxbuf, swlpOffset+i, value);
+    }
+
+	   /* total length = PLCP +         MacHeader       + Payload   + FCS + RXstatus */
+	   /*                 12  +  headerLen-8  + snapLen + buf length + 4  + 8        */
+   	zfwSetBufSetSize(dev, rxbuf, swlpOffset + (zfwBufGetSize(dev, buf)-offset) + 4 + EXTRA_INFO_LEN );
+
+    zmw_leave_critical_section(dev);
+
+    zfwBufFree(dev, buf, 0);
+
+	   //zfwDumpBuf(dev, rxbuf);
+	   //-------------------------------------------------
+
+    //zfCoreRecv(dev, rxbuf);
+
+#endif /* #if ZM_SW_LOOP_BACK */
+
+    return ZM_SUCCESS;
+}
+
+/* Report moniter Hal rx information about rssi, evm, bandwidth, SG etc */
+void zfHpQueryMonHalRxInfo(zdev_t* dev, u8_t *monHalRxInfo)
+{
+    zmw_get_wlan_dev(dev);
+    zfMemoryCopy(monHalRxInfo,
+                (u8_t*)&(((struct zsHpPriv*)wd->hpPrivate)->halRxInfo),
+                sizeof(struct zsHalRxInfo));
+}
+
+
+u8_t zfIsDataFrame(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t frameType;
+    u8_t mpduInd;
+
+    mpduInd = zmw_rx_buf_readb(dev, buf, zfwBufGetSize(dev, buf)-1);
+
+    /* sinlge or First */
+    if ((mpduInd & 0x30) == 0x00 || (mpduInd & 0x30) == 0x20)
+    {
+        frameType = zmw_rx_buf_readb(dev, buf, 12);
+    }
+    else
+    {
+        frameType = zmw_rx_buf_readb(dev, buf, 0);
+    }
+
+    if((frameType & 0xf) == ZM_WLAN_DATA_FRAME)
+        return 1;
+    else
+        return 0;
+}
+
+u32_t zfcConvertRateOFDM(zdev_t* dev, zbuf_t* buf)
+{
+    // What's the default value??
+    u32_t MCS = 0;
+
+    switch(zmw_rx_buf_readb(dev, buf, 0)& 0xf)
+    {
+        case 0xb:
+            MCS = 0x4;
+            break;
+        case 0xf:
+            MCS = 0x5;
+            break;
+        case 0xa:
+            MCS = 0x6;
+            break;
+        case 0xe:
+            MCS = 0x7;
+            break;
+        case 0x9:
+            MCS = 0x8;
+            break;
+        case 0xd:
+            MCS = 0x9;
+            break;
+        case 0x8:
+            MCS = 0xa;
+            break;
+        case 0xc:
+            MCS = 0xb;
+            break;
+    }
+    return MCS;
+}
+
+u16_t zfHpGetPayloadLen(zdev_t* dev,
+                        zbuf_t* buf,
+                        u16_t len,
+                        u16_t plcpHdrLen,
+                        u32_t *rxMT,
+                        u32_t *rxMCS,
+                        u32_t *rxBW,
+                        u32_t *rxSG
+                        )
+{
+    u8_t modulation,mpduInd;
+    u16_t low, high, msb;
+    s16_t payloadLen = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    mpduInd = zmw_rx_buf_readb(dev, buf, len-1);
+    modulation = zmw_rx_buf_readb(dev, buf, (len-1)) & 0x3;
+    *rxMT = modulation;
+
+    //zm_debug_msg1(" modulation= ", modulation);
+    switch (modulation) {
+    case 0: /* CCK Mode */
+        low = zmw_rx_buf_readb(dev, buf, 2);
+        high = zmw_rx_buf_readb(dev, buf, 3);
+        payloadLen = (low | high << 8) - 4;
+        if (wd->enableHALDbgInfo)
+        {
+            *rxMCS = zmw_rx_buf_readb(dev, buf, 0);
+            *rxBW  = 0;
+            *rxSG  = 0;
+        }
+        break;
+    case 1: /* Legacy-OFDM mode */
+        low = zmw_rx_buf_readb(dev, buf, 0) >> 5;
+        high = zmw_rx_buf_readb(dev, buf, 1);
+        msb = zmw_rx_buf_readb(dev, buf, 2) & 0x1;
+        payloadLen = (low | (high << 3) | (msb << 11)) - 4;
+        if (wd->enableHALDbgInfo)
+        {
+            *rxMCS = zfcConvertRateOFDM(dev, buf);
+            *rxBW  = 0;
+            *rxSG  = 0;
+        }
+        break;
+    case 2: /* HT OFDM mode */
+        //zm_debug_msg1("aggregation= ", (zmw_rx_buf_readb(dev, buf, 6) >> 3) &0x1 );
+        if ((mpduInd & 0x30) == 0x00 || (mpduInd & 0x30) == 0x10)    //single or last mpdu
+            payloadLen = len - 24 - 4 - plcpHdrLen;  // - rxStatus - fcs
+        else {
+            payloadLen = len - 4 - 4 - plcpHdrLen;  // - rxStatus - fcs
+            //zm_debug_msg1("first or middle mpdu, plcpHdrLen= ", plcpHdrLen);
+        }
+        if (wd->enableHALDbgInfo)
+        {
+            *rxMCS = zmw_rx_buf_readb(dev, buf, 3) & 0x7f;
+            *rxBW  = (zmw_rx_buf_readb(dev, buf, 3) >> 7) & 0x1;
+            *rxSG  = (zmw_rx_buf_readb(dev, buf, 6) >> 7) & 0x1;
+        }
+        break;
+    default:
+        break;
+
+    }
+    /* return the payload length - FCS */
+    if (payloadLen < 0) payloadLen = 0;
+    return payloadLen;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiUsbRecv                  */
+/*      Callback function for USB IN Transfer.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev: device pointer                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Yuan-Gu Wei        ZyDAS Technology Corporation    2005.10      */
+/*                                                                      */
+/************************************************************************/
+#define ZM_INT_USE_EP2                1
+#define ZM_INT_USE_EP2_HEADER_SIZE   12
+
+#if ZM_INT_USE_EP2 == 1
+void zfiUsbRegIn(zdev_t* dev, u32_t* rsp, u16_t rspLen);
+#endif
+
+#ifdef ZM_OTUS_RX_STREAM_MODE
+void zfiUsbRecvPerPkt(zdev_t *dev, zbuf_t *buf)
+#else
+void zfiUsbRecv(zdev_t *dev, zbuf_t *buf)
+#endif
+{
+
+
+#if ZM_FW_LOOP_BACK != 1
+    u8_t mpduInd;
+    u16_t plcpHdrLen;
+    u16_t crcPlusRxStatusLen;
+    u16_t len, payloadLen=0;
+    u16_t i; //CWYang(+)
+    struct zsAdditionInfo addInfo;
+    u32_t               rxMT;
+    u32_t               rxMCS;
+    u32_t               rxBW;
+    u32_t               rxSG;
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    //zm_msg0_rx(ZM_LV_0, "zfiUsbRecv()");
+
+#if ZM_INT_USE_EP2 == 1
+
+    for (i=0; i<(ZM_INT_USE_EP2_HEADER_SIZE>>1); i++)
+    {
+        if (zmw_rx_buf_readh(dev, buf, i*2) != 0xffff)
+        	break;
+    }
+
+    if (i==(ZM_INT_USE_EP2_HEADER_SIZE>>1))
+    {
+        u32_t               rsp[ZM_USB_MAX_EPINT_BUFFER/4];
+        u16_t               rspLen;
+        u32_t               rspi;
+        u8_t*               pdst = (u8_t*)rsp;
+
+        /* Interrupt Rsp */
+        rspLen = (u16_t) zfwBufGetSize(dev, buf)-ZM_INT_USE_EP2_HEADER_SIZE;
+
+        if (rspLen > 60)
+        {
+            zm_debug_msg1("Get error len by EP2 = \n", rspLen);
+            /* free USB buf */
+	          zfwBufFree(dev, buf, 0);
+	          return;
+        }
+
+        for (rspi=0; rspi<rspLen; rspi++)
+        {
+        	*pdst = zmw_rx_buf_readb(dev, buf, rspi+ZM_INT_USE_EP2_HEADER_SIZE);
+        	pdst++;
+        }
+
+        //if (adapter->zfcbUsbRegIn)
+        //    adapter->zfcbUsbRegIn(adapter, rsp, rspLen);
+        zfiUsbRegIn(dev, rsp, rspLen);
+
+	      /* free USB buf */
+	      zfwBufFree(dev, buf, 0);
+	      return;
+    }
+#endif /* end of #if ZM_INT_USE_EP2 == 1 */
+
+    ZM_PERFORMANCE_RX_MPDU(dev, buf);
+
+    if (wd->swSniffer)
+    {
+        /* airopeek: Report everything up */
+        if (wd->zfcbRecv80211 != NULL)
+        {
+            wd->zfcbRecv80211(dev, buf, NULL);
+        }
+    }
+
+    /* Read the last byte */
+    len = zfwBufGetSize(dev, buf);
+    mpduInd = zmw_rx_buf_readb(dev, buf, len-1);
+
+    /* First MPDU */
+    if((mpduInd & 0x30) == 0x20)
+    {
+        u16_t duration;
+        if (zmw_rx_buf_readb(dev, buf, 36) == 0) //AC = BE
+        {
+            duration = zmw_rx_buf_readh(dev, buf, 14);
+            if (duration > hpPriv->aggMaxDurationBE)
+            {
+                hpPriv->aggMaxDurationBE = duration;
+            }
+            else
+            {
+                if (hpPriv->aggMaxDurationBE > 10)
+                {
+                    hpPriv->aggMaxDurationBE--;
+                }
+            }
+            //DbgPrint("aggMaxDurationBE=%d", hpPriv->aggMaxDurationBE);
+        }
+    }
+
+#if 1
+    /* First MPDU or Single MPDU */
+    if(((mpduInd & 0x30) == 0x00) || ((mpduInd & 0x30) == 0x20))
+    //if ((mpduInd & 0x10) == 0x00)
+    {
+        plcpHdrLen = 12;        // PLCP header length
+    }
+    else
+    {
+        if (zmw_rx_buf_readh(dev, buf, 4) == wd->macAddr[0] &&
+            zmw_rx_buf_readh(dev, buf, 6) == wd->macAddr[1] &&
+            zmw_rx_buf_readh(dev, buf, 8) == wd->macAddr[2]) {
+            plcpHdrLen = 0;
+        }
+        else if (zmw_rx_buf_readh(dev, buf, 16) == wd->macAddr[0] &&
+                 zmw_rx_buf_readh(dev, buf, 18) == wd->macAddr[1] &&
+                 zmw_rx_buf_readh(dev, buf, 20) == wd->macAddr[2]){
+            plcpHdrLen = 12;
+        }
+        else {
+            plcpHdrLen = 0;
+        }
+    }
+
+    /* Last MPDU or Single MPDU */
+    if ((mpduInd & 0x30) == 0x00 || (mpduInd & 0x30) == 0x10)
+    {
+        crcPlusRxStatusLen = EXTRA_INFO_LEN + 4;     // Extra bytes + FCS
+    }
+    else
+    {
+        crcPlusRxStatusLen = 4 + 4;     // Extra 4 bytes + FCS
+    }
+#else
+    plcpHdrLen = 12;
+    crcPlusRxStatusLen = EXTRA_INFO_LEN + 4;     // Extra bytes + FCS
+#endif
+
+    if (len < (plcpHdrLen+10+crcPlusRxStatusLen))
+    {
+        zm_msg1_rx(ZM_LV_0, "Invalid Rx length=", len);
+        //zfwDumpBuf(dev, buf);
+
+        zfwBufFree(dev, buf, 0);
+        return;
+    }
+
+    /* display RSSI combined */
+    /*
+     * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+     * ¢x PLCP Header ¢x  MPDU  ¢x RSSI ¢x  EVM ¢x PHY Err ¢x  MAC Status ¢x
+     * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+     * ¢x     12      ¢x    n   ¢x  7   ¢x  12  ¢x    1    ¢x      4      ¢x
+     * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+     *	RSSI filed (From BB and MAC just pass them to host)
+     *   Byte1: RSSI for antenna 0.
+     *   Byte2: RSSI for antenna 1.
+     *   Byte3: RSSI for antenna 2.
+     *   Byte4: RSSI for antenna 0 extension.
+     *   Byte5: RSSI for antenna 1 extension.
+     *   Byte6: RSSI for antenna 2 extension.
+     *   Byte7: RSSI for antenna combined.
+     */
+
+    //zm_debug_msg1(" recv RSSI = ", zmw_rx_buf_readb(dev, buf, (len-1)-17));
+
+    payloadLen = zfHpGetPayloadLen(dev, buf, len, plcpHdrLen, &rxMT, &rxMCS, &rxBW, &rxSG);
+
+    /* Hal Rx info */
+    /* First MPDU or Single MPDU */
+    if(((mpduInd & 0x30) == 0x00) || ((mpduInd & 0x30) == 0x20))
+    {
+        if (wd->enableHALDbgInfo && zfIsDataFrame(dev, buf))
+        {
+            ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxDataMT   = rxMT;
+            ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxDataMCS  = rxMCS;
+            ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxDataBW   = rxBW;
+            ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxDataSG   = rxSG;
+        }
+    }
+
+    if ((plcpHdrLen + payloadLen) > len) {
+        zm_msg1_rx(ZM_LV_0, "Invalid payload length=", payloadLen);
+        zfwBufFree(dev, buf, 0);
+        return;
+    }
+
+    //Store Rx Tail Infomation before Remove--CWYang(+)
+
+#if 0
+    for (i = 0; i < crcPlusRxStatusLen-4; i++)
+    {
+       addInfo.Tail.Byte[i] =
+               zmw_rx_buf_readb(dev, buf, len - crcPlusRxStatusLen + 4 + i);
+    }
+#else
+/*
+* Brief format of OUTS chip
+* ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+* ¢x PLCP Header ¢x  MPDU  ¢x RSSI ¢x  EVM ¢x PHY Err ¢x  MAC Status ¢x
+* ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+* ¢x     12      ¢x    n   ¢x  7   ¢x  12  ¢x    1    ¢x      4      ¢x
+* ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+* RSSI:
+*       Byte 1  antenna 0
+*       Byte 2  antenna 1
+*       Byte 3  antenna 2
+*       Byte 4  antenna 0 extension
+*       Byte 5  antenna 1 extension
+*       Byte 6  antenna 2 extension
+*       Byte 7  antenna combined
+* EVM:
+*       Byte 1  Stream 0 pilot 0
+*       Byte 2  Stream 0 pilot 1
+*       Byte 3  Stream 0 pilot 2
+*       Byte 4  Stream 0 pilot 3
+*       Byte 5  Stream 0 pilot 4
+*       Byte 6  Stream 0 pilot 5
+*       Byte 7  Stream 1 pilot 0
+*       Byte 8  Stream 1 pilot 1
+*       Byte 9  Stream 1 pilot 2
+*       Byte 10 Stream 1 pilot 3
+*       Byte 11 Stream 1 pilot 4
+*       Byte 12 Stream 1 pilot 5
+*/
+
+    /* Fill the Tail information */
+    /* Last MPDU or Single MPDU */
+    if ((mpduInd & 0x30) == 0x00 || (mpduInd & 0x30) == 0x10)
+    {
+#define ZM_RX_RSSI_COMPENSATION     27
+        u8_t zm_rx_rssi_compensation = ZM_RX_RSSI_COMPENSATION;
+
+    	/* RSSI information */
+        addInfo.Tail.Data.SignalStrength1 = zmw_rx_buf_readb(dev, buf,
+                (len-1) - 17) + ((hpPriv->rxStrongRSSI == 1)?zm_rx_rssi_compensation:0);
+#undef ZM_RX_RSSI_COMPENSATION
+
+      /* EVM */
+
+      /* TODO: for RD/BB debug message */
+      /* save current rx hw infomration, report to DrvCore/Application */
+      if (wd->enableHALDbgInfo && zfIsDataFrame(dev, buf))
+      {
+            u8_t trssi;
+            for (i=0; i<7; i++)
+            {
+                trssi = zmw_rx_buf_readb(dev, buf, (len-1) - 23 + i);
+	            if (trssi&0x80)
+	            {
+                    trssi = ((~((u8_t)trssi) & 0x7f) + 1) & 0x7f;
+                }
+                ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[i] = trssi;
+
+            }
+          if (rxMT==2)
+          {
+            //if (rxBW)
+            //{
+            	  for (i=0; i<12; i++)
+                    ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[i] =
+                                       zmw_rx_buf_readb(dev, buf, (len-1) - 16 + i);
+            //}
+            //else
+            //{
+            //	  for (i=0; i<4; i++)
+            //        ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[i] =
+            //                           zmw_rx_buf_readb(dev, buf, (len-1) - 16 + i);
+            //}
+          }
+
+          #if 0
+          /* print */
+            zm_dbg(("MT(%d) MCS(%d) BW(%d) SG(%d) RSSI:%d,%d,%d,%d,%d,%d,%d EVM:(%d,%d,%d,%d,%d,%d)(%d,%d,%d,%d,%d,%d)\n",
+                       rxMT,
+                       rxMCS,
+                       rxBW,
+                       rxSG,
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[0],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[1],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[2],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[3],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[4],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[5],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[6],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[0],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[1],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[2],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[3],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[4],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[5],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[6],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[7],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[8],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[9],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[10],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[11]
+                       ));
+          #endif
+      } /* if (wd->enableHALDbgInfo && zfIsDataFrame(dev, buf)) */
+
+    }
+    else
+    {
+        /* Mid or First aggregate frame without phy rx information */
+        addInfo.Tail.Data.SignalStrength1 = 0;
+    }
+
+    addInfo.Tail.Data.SignalStrength2 = 0;
+    addInfo.Tail.Data.SignalStrength3 = 0;
+    addInfo.Tail.Data.SignalQuality   = 0;
+
+    addInfo.Tail.Data.SAIndex           = zmw_rx_buf_readb(dev, buf, len - 4);
+    addInfo.Tail.Data.DAIndex           = zmw_rx_buf_readb(dev, buf, len - 3);
+    addInfo.Tail.Data.ErrorIndication   = zmw_rx_buf_readb(dev, buf, len - 2);
+    addInfo.Tail.Data.RxMacStatus       = zmw_rx_buf_readb(dev, buf, len - 1);
+
+#endif
+    /* Remove CRC and Rx Status */
+    zfwBufSetSize(dev, buf, (len-crcPlusRxStatusLen));
+    //zfwBufSetSize(dev, buf, payloadLen + plcpHdrLen);    /* payloadLen + PLCP 12 - FCS 4*/
+
+    //Store PLCP Header Infomation before Remove--CWYang(+)
+    if (plcpHdrLen != 0)
+    {
+        for (i = 0; i < plcpHdrLen; i++)
+        {
+            addInfo.PlcpHeader[i] = zmw_rx_buf_readb(dev, buf, i);
+        }
+    }
+    else
+    {
+        addInfo.PlcpHeader[0] = 0;
+    }
+    /* Remove PLCP header */
+    zfwBufRemoveHead(dev, buf, plcpHdrLen);
+
+    /* handle 802.11 frame */
+    zfCoreRecv(dev, buf, &addInfo);
+
+#else
+    /* Firmware loopback: Rx frame = Tx frame       */
+    /* convert Rx frame to fit receive frame format */
+    zbuf_t *new_buf;
+    u8_t    ctrl_offset = 8;
+    u8_t    PLCP_Len = 12;
+    u8_t    data;
+    u8_t    i;
+
+
+    /* Tx:  | ctrl_setting | Mac hdr | data | */
+    /*            8            24       x     */
+
+    /* Rx:          | PLCP | Mac hdr | data | FCS | Rxstatus | */
+    /*                 12      24        x     4       8       */
+
+    /* new allocate a rx format size buf */
+    new_buf = zfwBufAllocate(dev, zfwBufGetSize(dev, buf)-8+12+4+EXTRA_INFO_LEN);
+
+    for (i=0; i<zfwBufGetSize(dev, buf)-ctrl_offset; i++)
+    {
+        data = zmw_rx_buf_readb(dev, buf, ctrl_offset+i);
+        zmw_rx_buf_writeb(dev, new_buf, PLCP_Len+i, data);
+    }
+
+    zfwBufSetSize(dev, new_buf, zfwBufGetSize(dev, buf)-8+12+4+EXTRA_INFO_LEN);
+
+    zfwBufFree(dev, buf, 0);
+
+    /* receive the new_buf */
+    //zfCoreRecv(dev, new_buf);
+
+#endif
+
+}
+
+#ifdef ZM_OTUS_RX_STREAM_MODE
+void zfiUsbRecv(zdev_t *dev, zbuf_t *buf)
+{
+    u16_t index = 0;
+    u16_t chkIdx;
+    u32_t status = 0;
+    u16_t ii;
+    zbuf_t *newBuf;
+    zbuf_t *rxBufPool[8];
+    u16_t rxBufPoolIndex = 0;
+    struct zsHpPriv *halPriv;
+    u8_t *srcBufPtr;
+    u32_t bufferLength;
+    u16_t usbRxRemainLen;
+    u16_t usbRxPktLen;
+
+    zmw_get_wlan_dev(dev);
+
+    halPriv = (struct zsHpPriv*)wd->hpPrivate;
+    srcBufPtr = zmw_buf_get_buffer(dev, buf);
+
+    bufferLength = zfwBufGetSize(dev, buf);
+
+    /* Zero Length Transfer */
+    if (!bufferLength)
+    {
+        zfwBufFree(dev, buf, 0);
+        return;
+    }
+
+    usbRxRemainLen = halPriv->usbRxRemainLen;
+    usbRxPktLen = halPriv->usbRxTransferLen;
+
+    /* Check whether there is any data in the last transfer */
+    if (usbRxRemainLen != 0 )
+    {
+        zbuf_t *remainBufPtr = halPriv->remainBuf;
+        u8_t* BufPtr = NULL;
+
+        if ( remainBufPtr != NULL )
+        {
+            BufPtr = zmw_buf_get_buffer(dev, remainBufPtr);
+        }
+
+        index = usbRxRemainLen;
+        usbRxRemainLen -= halPriv->usbRxPadLen;
+
+        /*  Copy data */
+        if ( BufPtr != NULL )
+        {
+            zfwMemoryCopy(&(BufPtr[usbRxPktLen]), srcBufPtr, usbRxRemainLen);
+        }
+
+        usbRxPktLen += usbRxRemainLen;
+        halPriv->usbRxRemainLen = 0;
+
+        if ( remainBufPtr != NULL )
+        {
+            zfwBufSetSize(dev, remainBufPtr, usbRxPktLen);
+            rxBufPool[rxBufPoolIndex++] = remainBufPtr;
+        }
+        halPriv->remainBuf = NULL;
+    }
+
+    //zm_debug_msg1("length: %d\n", (int)pUsbRxTransfer->pRxUrb->UrbBulkOrInterruptTransfer.TransferBufferLength);
+
+    bufferLength = zfwBufGetSize(dev, buf);
+//printk("bufferLength %d\n", bufferLength);
+    while(index < bufferLength)
+    {
+        u16_t pktLen;
+        u16_t pktTag;
+        //u8_t *ptr = (u8_t*)((struct zsBuffer*)pUsbRxTransfer->buf)->data;
+        u8_t *ptr = srcBufPtr;
+
+        /* Retrieve packet length and tag */
+        pktLen = ptr[index] + (ptr[index+1] << 8);
+        pktTag = ptr[index+2] + (ptr[index+3] << 8);
+
+        if (pktTag == ZM_USB_STREAM_MODE_TAG)
+        {
+            u16_t padLen;
+
+            zm_assert(pktLen < ZM_WLAN_MAX_RX_SIZE);
+
+            //printk("Get a packet, pktLen: 0x%04x\n", pktLen);
+            #if 0
+            /* Dump data */
+            for (ii = index; ii < pkt_len+4;)
+            {
+                DbgPrint("0x%02x ",
+                        (zmw_rx_buf_readb(adapter, pUsbRxTransfer->buf, ii) & 0xff));
+
+                if ((++ii % 16) == 0)
+                    DbgPrint("\n");
+            }
+
+            DbgPrint("\n");
+            #endif
+
+            /* Calcuate the padding length, in the current design,
+               the length should be padded to 4 byte boundray. */
+            padLen = ZM_USB_STREAM_MODE_TAG_LEN - (pktLen & 0x3);
+
+            if(padLen == ZM_USB_STREAM_MODE_TAG_LEN)
+                padLen = 0;
+
+            chkIdx = index;
+            index = index + ZM_USB_STREAM_MODE_TAG_LEN + pktLen + padLen;
+
+            if (chkIdx > ZM_MAX_USB_IN_TRANSFER_SIZE)
+            {
+                zm_debug_msg1("chkIdx is too large, chkIdx: %d\n", chkIdx);
+                zm_assert(0);
+                status = 1;
+                break;
+            }
+
+            if (index > ZM_MAX_USB_IN_TRANSFER_SIZE)
+            {
+                //struct zsBuffer* BufPtr;
+                //struct zsBuffer* UsbBufPtr;
+                u8_t *BufPtr;
+                u8_t *UsbBufPtr;
+
+                halPriv->usbRxRemainLen = index - ZM_MAX_USB_IN_TRANSFER_SIZE; // - padLen;
+                halPriv->usbRxTransferLen = ZM_MAX_USB_IN_TRANSFER_SIZE -
+                        chkIdx - ZM_USB_STREAM_MODE_TAG_LEN;
+                halPriv->usbRxPadLen = padLen;
+                //check_index = index;
+
+                if (halPriv->usbRxTransferLen > ZM_WLAN_MAX_RX_SIZE)
+                {
+                    zm_debug_msg1("check_len is too large, chk_len: %d\n",
+                            halPriv->usbRxTransferLen);
+                    status = 1;
+                    break;
+                }
+
+                /* Allocate a skb buffer */
+                newBuf = zfwBufAllocate(dev, ZM_WLAN_MAX_RX_SIZE);
+
+                if ( newBuf != NULL )
+                {
+                    BufPtr = zmw_buf_get_buffer(dev, newBuf);
+                    UsbBufPtr = srcBufPtr;
+
+                    /* Copy the buffer */
+                    zfwMemoryCopy(BufPtr, &(UsbBufPtr[chkIdx+ZM_USB_STREAM_MODE_TAG_LEN]), halPriv->usbRxTransferLen);
+
+                    /* Record the buffer pointer */
+                    halPriv->remainBuf = newBuf;
+                }
+            }
+            else
+            {
+                u8_t* BufPtr;
+                u8_t* UsbBufPtr;
+
+                /* Allocate a skb buffer */
+                newBuf = zfwBufAllocate(dev, ZM_WLAN_MAX_RX_SIZE);
+                if ( newBuf != NULL )
+                {
+                    BufPtr = zmw_buf_get_buffer(dev, newBuf);
+                    UsbBufPtr = srcBufPtr;
+
+                    /* Copy the buffer */
+                    zfwMemoryCopy(BufPtr, &(UsbBufPtr[chkIdx+ZM_USB_STREAM_MODE_TAG_LEN]), pktLen);
+
+                    zfwBufSetSize(dev, newBuf, pktLen);
+                    rxBufPool[rxBufPoolIndex++] = newBuf;
+                }
+            }
+        }
+        else
+        {
+                u16_t i;
+
+                DbgPrint("Can't find tag, pkt_len: 0x%04x, tag: 0x%04x\n",
+                        pktLen, pktTag);
+
+                #if 0
+                for(i = 0; i < 32; i++)
+                {
+                    DbgPrint("%02x ", buf->data[index-16+i]);
+
+                    if ((i & 0xf) == 0xf)
+                        DbgPrint("\n");
+                }
+                #endif
+
+                break;
+        }
+    }
+
+    /* Free buffer */
+    //zfwBufFree(adapter, pUsbRxTransfer->buf, 0);
+    zfwBufFree(dev, buf, 0);
+
+    for(ii = 0; ii < rxBufPoolIndex; ii++)
+    {
+        zfiUsbRecvPerPkt(dev, rxBufPool[ii]);
+    }
+}
+#endif
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfUsbInit                   */
+/*      Initialize USB resource.                                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.12     */
+/*                                                                      */
+/************************************************************************/
+void zfUsbInit(zdev_t* dev)
+{
+    /* Initialize Rx & INT endpoint for receiving data & interrupt */
+    zfwUsbEnableRxEpt(dev, USB_ENDPOINT_RX_INDEX);
+    zfwUsbEnableIntEpt(dev, USB_ENDPOINT_INT_INDEX);
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfUsbFree                   */
+/*      Free PCI resource.                                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.12     */
+/*                                                                      */
+/************************************************************************/
+void zfUsbFree(zdev_t* dev)
+{
+    struct zsHpPriv *halPriv;
+
+    zmw_get_wlan_dev(dev);
+
+    halPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+#ifdef ZM_OTUS_RX_STREAM_MODE
+    if ( halPriv->remainBuf != NULL )
+    {
+        zfwBufFree(dev, halPriv->remainBuf, 0);
+    }
+#endif
+
+    return;
+}
+
+void zfHpSendBeacon(zdev_t* dev, zbuf_t* buf, u16_t len)
+{
+    u32_t hw, lw;
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    /* Write to beacon buffer (ZM_BEACON_BUFFER_ADDRESS) */
+    for (i = 0; i<len; i+=4)
+    {
+        lw = zmw_tx_buf_readh(dev, buf, i);
+        hw = zmw_tx_buf_readh(dev, buf, i+2);
+
+        zfDelayWriteInternalReg(dev, ZM_BEACON_BUFFER_ADDRESS+i, (hw<<16)+lw);
+    }
+
+    /* Beacon PCLP header */
+    if (((struct zsHpPriv*)wd->hpPrivate)->hwFrequency < 3000)
+    {
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PLCP, ((len+4)<<(3+16))+0x0400);
+    }
+    else
+    {
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PLCP, ((len+4)<<(16))+0x001b);
+    }
+
+    /* Beacon length (include CRC32) */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_LENGTH, len+4);
+
+    /* Beacon Ready */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_CTRL, 1);
+    zfFlushDelayWrite(dev);
+
+    /* Free beacon buf */
+    zfwBufFree(dev, buf, 0);
+
+    return;
+}
+
+
+#define ZM_STATUS_TX_COMP       0x00
+#define ZM_STATUS_RETRY_COMP    0x01
+#define ZM_STATUS_TX_FAILED     0x02
+void zfiUsbRegIn(zdev_t* dev, u32_t* rsp, u16_t rspLen)
+{
+    //u8_t len, type, i;
+    u8_t type;
+    u8_t *u8rsp;
+    u16_t status;
+    u32_t bitmap;
+    zmw_get_wlan_dev(dev);
+
+    zm_msg0_mm(ZM_LV_3, "zfiUsbRegIn()");
+
+    u8rsp = (u8_t *)rsp;
+
+    //len = *u8rsp;
+    type = *(u8rsp+1);
+    u8rsp = u8rsp+4;
+
+
+    /* Interrupt event */
+    if ((type & 0xC0) == 0xC0)
+    {
+        if (type == 0xC0)
+        {
+            zfCoreEvent(dev, 0, u8rsp);
+
+        }
+        else if (type == 0xC1)
+        {
+#if 0
+            {
+                u16_t i;
+                DbgPrint("rspLen=%d\n", rspLen);
+                for (i=0; i<(rspLen/4); i++)
+                {
+                    DbgPrint("rsp[%d]=0x%lx\n", i, rsp[i]);
+                }
+            }
+#endif
+            status = (u16_t)(rsp[3] >> 16);
+
+            ////6789
+            rsp[8] = rsp[8] >> 2 | (rsp[9] & 0x1) << 6;
+            switch (status)
+            {
+            case ZM_STATUS_RETRY_COMP :
+                zfCoreEvent(dev, 1, u8rsp);
+                break;
+            case ZM_STATUS_TX_FAILED :
+                zfCoreEvent(dev, 2, u8rsp);
+                break;
+            case ZM_STATUS_TX_COMP :
+                zfCoreEvent(dev, 3, u8rsp);
+                break;
+            }
+        }
+        else if (type == 0xC2)
+        {
+            zfBeaconCfgInterrupt(dev, u8rsp);
+        }
+        else if (type == 0xC3)
+        {
+            zfEndOfAtimWindowInterrupt(dev);
+        }
+        else if (type == 0xC4)
+        {
+#if 0
+            {
+                u16_t i;
+                DbgPrint("0xC2:rspLen=%d\n", rspLen);
+                for (i=0; i<(rspLen/4); i++)
+                {
+                    DbgPrint("0xC2:rsp[%d]=0x%lx\n", i, rsp[i]);
+                }
+            }
+#endif
+            bitmap = (rsp[1] >> 16) + ((rsp[2] & 0xFFFF) << 16 );
+            //zfBawCore(dev, (u16_t)rsp[1] & 0xFFFF, bitmap, (u16_t)(rsp[2] >> 16) & 0xFF);
+        }
+        else if (type == 0xC5)
+        {
+            u16_t i;
+#if 0
+
+            for (i=0; i<(rspLen/4); i++) {
+                DbgPrint("0xC5:rsp[%d]=0x%lx\n", i, rsp[i]);
+            }
+#endif
+            for (i=1; i<(rspLen/4); i++) {
+                u8rsp = (u8_t *)(rsp+i);
+                //DbgPrint("0xC5:rsp[%d]=0x%lx\n", i, ((u32_t*)u8rsp)[0]);
+                zfCoreEvent(dev, 4, u8rsp);
+            }
+        }
+        else if (type == 0xC6)
+        {
+            zm_debug_msg0("\n\n WatchDog interrupt!!! : 0xC6 \n\n");
+            if (wd->zfcbHwWatchDogNotify != NULL)
+            {
+                wd->zfcbHwWatchDogNotify(dev);
+            }
+        }
+        else if (type == 0xC8)
+        {
+            //PZSW_ADAPTER adapter;
+
+            // for SPI flash program chk Flag
+            zfwDbgProgrameFlashChkDone(dev);
+        }
+        else if (type == 0xC9)
+        {
+            struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+            zm_debug_msg0("##### Tx retransmission 5 times event #####");
+
+            /* correct tx retransmission issue */
+            hpPriv->retransmissionEvent = 1;
+        }
+    }
+    else
+    {
+        zfIdlRsp(dev, rsp, rspLen);
+    }
+}
+
+
+#define ZM_PROGRAM_RAM_ADDR     0x200000 //0x1000 //0x700000
+#define FIRMWARE_DOWNLOAD       0x30
+#define FIRMWARE_DOWNLOAD_COMP  0x31
+#define FIRMWARE_CONFIRM        0x32
+
+u16_t zfFirmwareDownload(zdev_t* dev, u32_t* fw, u32_t len, u32_t offset)
+{
+    u16_t ret = ZM_SUCCESS;
+    u32_t uCodeOfst = offset;
+    u8_t *image, *ptr;
+    u32_t result;
+
+    image = (u8_t*) fw;
+    ptr = image;
+
+    while (len > 0)
+    {
+        u32_t translen = (len > 4096) ? 4096 : len;
+
+        result = zfwUsbSubmitControl(dev, FIRMWARE_DOWNLOAD,
+                                     (u16_t) (uCodeOfst >> 8),
+                                     0, image, translen);
+
+        if (result != ZM_SUCCESS)
+        {
+            zm_msg0_init(ZM_LV_0, "FIRMWARE_DOWNLOAD failed");
+            ret = 1;
+            goto exit;
+        }
+
+        len -= translen;
+        image += translen;
+        uCodeOfst += translen; // in Word (16 bit)
+
+        result = 0;
+    }
+
+    /* If download firmware success, issue a command to firmware */
+    if (ret == 0)
+    {
+        result = zfwUsbSubmitControl(dev, FIRMWARE_DOWNLOAD_COMP,
+                                     0, 0, NULL, 0);
+
+        if (result != ZM_SUCCESS)
+        {
+            zm_msg0_init(ZM_LV_0, "FIRMWARE_DOWNLOAD_COMP failed");
+            ret = 1;
+            goto exit;
+        }
+    }
+
+#if 0
+    /* PCI code */
+    /* Wait for firmware ready */
+    result = zfwUsbSubmitControl(dev, FIRMWARE_CONFIRM, USB_DIR_IN | 0x40,
+                     0, 0, &ret_value, sizeof(ret_value), HZ);
+
+    if (result != 0)
+    {
+        zm_msg0_init(ZM_LV_0, "Can't receive firmware ready: ", result);
+        ret = 1;
+    }
+#endif
+
+exit:
+
+    return ret;
+
+}
+
+u16_t zfFirmwareDownloadNotJump(zdev_t* dev, u32_t* fw, u32_t len, u32_t offset)
+{
+    u16_t ret = ZM_SUCCESS;
+    u32_t uCodeOfst = offset;
+    u8_t *image, *ptr;
+    u32_t result;
+
+    image = (u8_t*) fw;
+    ptr = image;
+
+    while (len > 0)
+    {
+        u32_t translen = (len > 4096) ? 4096 : len;
+
+        result = zfwUsbSubmitControl(dev, FIRMWARE_DOWNLOAD,
+                                     (u16_t) (uCodeOfst >> 8),
+                                     0, image, translen);
+
+        if (result != ZM_SUCCESS)
+        {
+            zm_msg0_init(ZM_LV_0, "FIRMWARE_DOWNLOAD failed");
+            ret = 1;
+            goto exit;
+        }
+
+        len -= translen;
+        image += translen;
+        uCodeOfst += translen; // in Word (16 bit)
+
+        result = 0;
+    }
+
+exit:
+
+    return ret;
+
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfIdlGetFreeTxdCount        */
+/*      Get free PCI PCI TxD count.                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+u32_t zfHpGetFreeTxdCount(zdev_t* dev)
+{
+    return zfwUsbGetFreeTxQSize(dev);
+}
+
+u32_t zfHpGetMaxTxdCount(zdev_t* dev)
+{
+    //return 8;
+    return zfwUsbGetMaxTxQSize(dev);
+}
+
+void zfiUsbRegOutComplete(zdev_t* dev)
+{
+    return;
+}
+
+extern void zfPushVtxq(zdev_t* dev);
+
+void zfiUsbOutComplete(zdev_t* dev, zbuf_t *buf, u8_t status, u8_t *hdr) {
+#ifndef ZM_ENABLE_AGGREGATION
+    if (buf) {
+        zfwBufFree(dev, buf, 0);
+    }
+#else
+    #ifdef ZM_BYPASS_AGGR_SCHEDULING
+    //Simply free the buf since BA retransmission is done in the firmware
+    if (buf)
+    {
+        zfwBufFree(dev, buf, 0);
+    }
+    zfPushVtxq(dev);
+    #else
+    zmw_get_wlan_dev(dev);
+
+    #ifdef ZM_ENABLE_FW_BA_RETRANSMISSION
+    //Simply free the buf since BA retransmission is done in the firmware
+    if (buf)
+    {
+        zfwBufFree(dev, buf, 0);
+    }
+    #else
+    u8_t agg;
+    u16_t frameType;
+
+    if(!hdr && buf) {
+        zfwBufFree(dev, buf, 0);
+        //zm_debug_msg0("buf Free due to hdr == NULL");
+        return;
+    }
+
+    if(hdr && buf) {
+        frameType = hdr[8] & 0xf;
+        agg = (u8_t)(hdr[2] >> 5 ) & 0x1;
+        //zm_debug_msg1("AGG=", agg);
+
+        if (!status) {
+            if (agg) {
+                //delete buf in ba fail queue??
+                //not ganna happen?
+            }
+            else {
+                zfwBufFree(dev, buf, 0);
+            }
+        }
+        else {
+            if (agg) {
+                //don't do anything
+                //zfwBufFree(dev, buf, 0);
+            }
+            else {
+                zfwBufFree(dev, buf, 0);
+            }
+        }
+    }
+    #endif
+
+    if (wd->state != ZM_WLAN_STATE_ENABLED) {
+        return;
+    }
+
+    if( (wd->wlanMode == ZM_MODE_AP) ||
+        (wd->wlanMode == ZM_MODE_INFRASTRUCTURE && wd->sta.EnableHT) ||
+        (wd->wlanMode == ZM_MODE_PSEUDO) ) {
+        zfAggTxScheduler(dev, 0);
+    }
+    #endif
+#endif
+
+    return;
+
+}
+
diff --git a/drivers/staging/otus/hal/hpusb.h b/drivers/staging/otus/hal/hpusb.h
new file mode 100644
index 000000000000..35a0c5668cea
--- /dev/null
+++ b/drivers/staging/otus/hal/hpusb.h
@@ -0,0 +1,437 @@
+/*
+ * Copyright (c) 2000-2005 ZyDAS Technology Corporation
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : ud_defs.h                                             */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains USB data structure definitions.            */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _HPUSB_H
+#define _HPUSB_H
+
+#define ZM_OTUS_ENABLE_RETRY_FREQ_CHANGE
+#define ZM_BEACON_BUFFER_ADDRESS            0x117900
+
+#define ZM_MAX_CMD_SIZE                     64
+#define ZM_HAL_MAX_EEPROM_REQ               510
+#define ZM_HAL_MAX_EEPROM_PRQ               2
+
+/* For USB STREAM mode */
+#ifdef ZM_DISABLE_AMSDU8K_SUPPORT
+#define ZM_MAX_USB_IN_TRANSFER_SIZE         4096
+#else
+#define ZM_MAX_USB_IN_TRANSFER_SIZE         8192
+#endif
+#define ZM_USB_STREAM_MODE_TAG_LEN          4
+#define ZM_USB_STREAM_MODE_TAG              0x4e00
+#define ZM_USB_MAX_EPINT_BUFFER             64
+
+struct zsCmdQ
+{
+    u16_t src;
+    u16_t cmdLen;
+    u8_t* buf;
+    u32_t cmd[ZM_MAX_CMD_SIZE/4];
+};
+
+struct zsCommand
+{
+    u16_t delayWcmdCount;
+    u32_t delayWcmdAddr[(ZM_CMD_QUEUE_SIZE-4)/4];
+    u32_t delayWcmdVal[(ZM_CMD_QUEUE_SIZE-4)/4];
+};
+
+struct zsHalRxInfo
+{
+    u32_t currentRSSI[7];       /* RSSI combined */
+    u32_t currentRxEVM[14];
+    u32_t currentRxDataMT;
+    u32_t currentRxDataMCS;
+    u32_t currentRxDataBW;
+    u32_t currentRxDataSG;
+};
+
+struct zsHpPriv
+{
+    u16_t hwFrequency;
+    u8_t  hwBw40;
+    u8_t  hwExtOffset;
+
+    u8_t  disableDfsCh;
+
+    u32_t halCapability;
+
+    /* Fortunately the second loop can be disabled with a bit */
+    /* called en_pd_dc_offset_thr                             */
+    u8_t hwNotFirstInit;
+
+    /* command queue */
+    u16_t               cmdHead;
+    u16_t               cmdTail;
+#ifdef ZM_XP_USB_MULTCMD
+    u16_t               cmdSend;  // Used for Mult send USB cmd
+#endif
+    struct zsCmdQ       cmdQ[ZM_CMD_QUEUE_SIZE];
+    u16_t               cmdPending;
+    struct zsCommand    cmd; /* buffer for delayed commands */
+    u8_t                ledMode[2];
+    u32_t               ctlBusy;
+    u32_t               extBusy;
+
+    /*
+     * ANI & Radar support.
+     */
+    u32_t   procPhyErr;         /* Process Phy errs */
+    u8_t hasHwPhyCounters;   /* Hardware has phy counters */
+    u32_t   aniPeriod;          /* ani update list period */
+    struct zsAniStats   stats;      /* various statistics */
+    struct zsAniState   *curani;    /* cached last reference */
+    struct zsAniState   ani[50];   /* per-channel state */
+
+    /*
+     * Ani tables that change between the 5416 and 5312.
+     * These get set at attach time.
+     * XXX don't belong here
+     * XXX need better explanation
+     */
+    s32_t     totalSizeDesired[5];
+    s32_t     coarseHigh[5];
+    s32_t     coarseLow[5];
+    s32_t     firpwr[5];
+
+    /*
+     * ANI related PHY register value.
+     */
+    u32_t regPHYDesiredSZ;
+    u32_t regPHYFindSig;
+    u32_t regPHYAgcCtl1;
+    u32_t regPHYSfcorr;
+    u32_t regPHYSfcorrLow;
+    u32_t regPHYTiming5;
+    u32_t regPHYCckDetect;
+
+    u32_t eepromImage[1024];
+    u32_t eepromImageIndex;
+    u32_t eepromImageRdReq;
+
+    u8_t  halReInit;
+
+    u8_t  OpFlags;
+
+    u8_t tPow2xCck[4];
+    u8_t tPow2x2g[4];
+    u8_t tPow2x2g24HeavyClipOffset;
+    u8_t tPow2x2gHt20[8];
+    u8_t tPow2x2gHt40[8];
+    u8_t tPow2x5g[4];
+    u8_t tPow2x5gHt20[8];
+    u8_t tPow2x5gHt40[8];
+
+    /* hwBBHeavyClip : used compatibility           */
+    /*             0 : dongle not support.          */
+    /*             !0: support heavy clip.          */
+    u8_t hwBBHeavyClip;
+    u8_t enableBBHeavyClip; /* 0=>force disable 1=>enable */
+    u8_t doBBHeavyClip;     /* set 1 if heavy clip need by each frequency switch */
+    u32_t setValueHeavyClip; /* save setting value for heavy clip when completed routine */
+
+    /*
+     * Rxdata RSSI, EVM, Rate etc...
+     */
+    struct zsHalRxInfo halRxInfo;
+
+    u32_t usbSendBytes;
+    u32_t usbAcSendBytes[4];
+
+    u16_t aggMaxDurationBE;
+    u32_t aggPktNum;
+
+    u16_t txop[4];
+    u16_t cwmin[4];
+    u16_t cwmax[4];
+    u8_t  strongRSSI;
+    u8_t  rxStrongRSSI;
+
+    u8_t  slotType;  //0->20us, 1=>9us
+
+#ifdef ZM_OTUS_RX_STREAM_MODE
+    u16_t usbRxRemainLen;
+    u16_t usbRxPktLen;
+    u16_t usbRxPadLen;
+    u16_t usbRxTransferLen;
+    zbuf_t  *remainBuf;
+#endif
+
+    u8_t    dot11Mode;
+
+    u8_t    ibssBcnEnabled;
+    u32_t   ibssBcnInterval;
+
+    // For re-issue the frequency change command
+    u32_t   latestFrequency;
+    u8_t    latestBw40;
+    u8_t    latestExtOffset;
+    u8_t    freqRetryCounter;
+
+    u8_t    recordFreqRetryCounter;
+    u8_t    isSiteSurvey;
+    u8_t    coldResetNeedFreq;
+
+    u64_t   camRollCallTable;
+    u8_t    currentAckRtsTpc;
+
+    /* #1 Save the initial value of the related RIFS register settings */
+    //u32_t   isInitialPhy;
+    u32_t   initDesiredSigSize;
+    u32_t   initAGC;
+    u32_t   initAgcControl;
+    u32_t   initSearchStartDelay;
+    u32_t   initRIFSSearchParams;
+    u32_t   initFastChannelChangeControl;
+
+    /* Dynamic SIFS for retransmission event */
+    u8_t    retransmissionEvent;
+    u8_t    latestSIFS;
+};
+
+extern u32_t zfHpLoadEEPROMFromFW(zdev_t* dev);
+
+
+typedef u8_t A_UINT8;
+typedef s8_t A_INT8;
+typedef u16_t A_UINT16;
+typedef u32_t A_UINT32;
+#define __ATTRIB_PACK
+
+#pragma pack (push, 1)
+
+#define AR5416_EEP_VER               0xE
+#define AR5416_EEP_VER_MINOR_MASK    0xFFF
+#define AR5416_EEP_NO_BACK_VER       0x1
+#define AR5416_EEP_MINOR_VER_2       0x2  // Adds modal params txFrameToPaOn, txFrametoDataStart, ht40PowerInc
+#define AR5416_EEP_MINOR_VER_3       0x3  // Adds modal params bswAtten, bswMargin, swSettle and base OpFlags for HT20/40 Disable
+
+// 16-bit offset location start of calibration struct
+#define AR5416_EEP_START_LOC         256
+#define AR5416_NUM_5G_CAL_PIERS      8
+#define AR5416_NUM_2G_CAL_PIERS      4
+#define AR5416_NUM_5G_20_TARGET_POWERS  8
+#define AR5416_NUM_5G_40_TARGET_POWERS  8
+#define AR5416_NUM_2G_CCK_TARGET_POWERS 3
+#define AR5416_NUM_2G_20_TARGET_POWERS  4
+#define AR5416_NUM_2G_40_TARGET_POWERS  4
+#define AR5416_NUM_CTLS              24
+#define AR5416_NUM_BAND_EDGES        8
+#define AR5416_NUM_PD_GAINS          4
+#define AR5416_PD_GAINS_IN_MASK      4
+#define AR5416_PD_GAIN_ICEPTS        5
+#define AR5416_EEPROM_MODAL_SPURS    5
+#define AR5416_MAX_RATE_POWER        63
+#define AR5416_NUM_PDADC_VALUES      128
+#define AR5416_NUM_RATES             16
+#define AR5416_BCHAN_UNUSED          0xFF
+#define AR5416_MAX_PWR_RANGE_IN_HALF_DB 64
+#define AR5416_OPFLAGS_11A           0x01
+#define AR5416_OPFLAGS_11G           0x02
+#define AR5416_OPFLAGS_5G_HT40       0x04
+#define AR5416_OPFLAGS_2G_HT40       0x08
+#define AR5416_OPFLAGS_5G_HT20       0x10
+#define AR5416_OPFLAGS_2G_HT20       0x20
+#define AR5416_EEPMISC_BIG_ENDIAN    0x01
+#define FREQ2FBIN(x,y) ((y) ? ((x) - 2300) : (((x) - 4800) / 5))
+#define AR5416_MAX_CHAINS            2
+#define AR5416_ANT_16S               25
+
+#define AR5416_NUM_ANT_CHAIN_FIELDS     7
+#define AR5416_NUM_ANT_COMMON_FIELDS    4
+#define AR5416_SIZE_ANT_CHAIN_FIELD     3
+#define AR5416_SIZE_ANT_COMMON_FIELD    4
+#define AR5416_ANT_CHAIN_MASK           0x7
+#define AR5416_ANT_COMMON_MASK          0xf
+#define AR5416_CHAIN_0_IDX              0
+#define AR5416_CHAIN_1_IDX              1
+#define AR5416_CHAIN_2_IDX              2
+
+
+/* Capabilities Enum */
+typedef enum {
+    EEPCAP_COMPRESS_DIS  = 0x0001,
+    EEPCAP_AES_DIS       = 0x0002,
+    EEPCAP_FASTFRAME_DIS = 0x0004,
+    EEPCAP_BURST_DIS     = 0x0008,
+    EEPCAP_MAXQCU_M      = 0x01F0,
+    EEPCAP_MAXQCU_S      = 4,
+    EEPCAP_HEAVY_CLIP_EN = 0x0200,
+    EEPCAP_KC_ENTRIES_M  = 0xF000,
+    EEPCAP_KC_ENTRIES_S  = 12,
+} EEPROM_CAPABILITIES;
+
+typedef enum Ar5416_Rates {
+    rate6mb,  rate9mb,  rate12mb, rate18mb,
+    rate24mb, rate36mb, rate48mb, rate54mb,
+    rate1l,   rate2l,   rate2s,   rate5_5l,
+    rate5_5s, rate11l,  rate11s,  rateXr,
+    rateHt20_0, rateHt20_1, rateHt20_2, rateHt20_3,
+    rateHt20_4, rateHt20_5, rateHt20_6, rateHt20_7,
+    rateHt40_0, rateHt40_1, rateHt40_2, rateHt40_3,
+    rateHt40_4, rateHt40_5, rateHt40_6, rateHt40_7,
+    rateDupCck, rateDupOfdm, rateExtCck, rateExtOfdm,
+    Ar5416RateSize
+} AR5416_RATES;
+
+typedef struct eepFlags {
+    A_UINT8  opFlags;
+    A_UINT8  eepMisc;
+} __ATTRIB_PACK EEP_FLAGS;
+
+#define AR5416_CHECKSUM_LOCATION (AR5416_EEP_START_LOC + 1)
+typedef struct BaseEepHeader {
+    A_UINT16  length;
+    A_UINT16  checksum;
+    A_UINT16  version;
+    EEP_FLAGS opCapFlags;
+    A_UINT16  regDmn[2];
+    A_UINT8   macAddr[6];
+    A_UINT8   rxMask;
+    A_UINT8   txMask;
+    A_UINT16  rfSilent;
+    A_UINT16  blueToothOptions;
+    A_UINT16  deviceCap;
+    A_UINT32  binBuildNumber;
+    A_UINT8   deviceType;
+    A_UINT8   futureBase[33];
+} __ATTRIB_PACK BASE_EEP_HEADER; // 64 B
+
+typedef struct spurChanStruct {
+    A_UINT16 spurChan;
+    A_UINT8  spurRangeLow;
+    A_UINT8  spurRangeHigh;
+} __ATTRIB_PACK SPUR_CHAN;
+
+typedef struct ModalEepHeader {
+    A_UINT32  antCtrlChain[AR5416_MAX_CHAINS];       // 12
+    A_UINT32  antCtrlCommon;                         // 4
+    A_INT8    antennaGainCh[AR5416_MAX_CHAINS];      // 3
+    A_UINT8   switchSettling;                        // 1
+    A_UINT8   txRxAttenCh[AR5416_MAX_CHAINS];        // 3
+    A_UINT8   rxTxMarginCh[AR5416_MAX_CHAINS];       // 3
+    A_INT8    adcDesiredSize;                        // 1
+    A_INT8    pgaDesiredSize;                        // 1
+    A_UINT8   xlnaGainCh[AR5416_MAX_CHAINS];         // 3
+    A_UINT8   txEndToXpaOff;                         // 1
+    A_UINT8   txEndToRxOn;                           // 1
+    A_UINT8   txFrameToXpaOn;                        // 1
+    A_UINT8   thresh62;                              // 1
+    A_INT8    noiseFloorThreshCh[AR5416_MAX_CHAINS]; // 3
+    A_UINT8   xpdGain;                               // 1
+    A_UINT8   xpd;                                   // 1
+    A_INT8    iqCalICh[AR5416_MAX_CHAINS];           // 1
+    A_INT8    iqCalQCh[AR5416_MAX_CHAINS];           // 1
+    A_UINT8   pdGainOverlap;                         // 1
+    A_UINT8   ob;                                    // 1
+    A_UINT8   db;                                    // 1
+    A_UINT8   xpaBiasLvl;                            // 1
+    A_UINT8   pwrDecreaseFor2Chain;                  // 1
+    A_UINT8   pwrDecreaseFor3Chain;                  // 1 -> 48 B
+    A_UINT8   txFrameToDataStart;                    // 1
+    A_UINT8   txFrameToPaOn;                         // 1
+    A_UINT8   ht40PowerIncForPdadc;                  // 1
+    A_UINT8   bswAtten[AR5416_MAX_CHAINS];           // 3
+    A_UINT8   bswMargin[AR5416_MAX_CHAINS];          // 3
+    A_UINT8   swSettleHt40;                          // 1
+    A_UINT8   futureModal[22];                       //
+    SPUR_CHAN spurChans[AR5416_EEPROM_MODAL_SPURS];  // 20 B
+} __ATTRIB_PACK MODAL_EEP_HEADER;                    // == 100 B
+
+typedef struct calDataPerFreq {
+    A_UINT8 pwrPdg[AR5416_NUM_PD_GAINS][AR5416_PD_GAIN_ICEPTS];
+    A_UINT8 vpdPdg[AR5416_NUM_PD_GAINS][AR5416_PD_GAIN_ICEPTS];
+} __ATTRIB_PACK CAL_DATA_PER_FREQ;
+
+typedef struct CalTargetPowerLegacy {
+    A_UINT8  bChannel;
+    A_UINT8  tPow2x[4];
+} __ATTRIB_PACK CAL_TARGET_POWER_LEG;
+
+typedef struct CalTargetPowerHt {
+    A_UINT8  bChannel;
+    A_UINT8  tPow2x[8];
+} __ATTRIB_PACK CAL_TARGET_POWER_HT;
+
+#if defined(ARCH_BIG_ENDIAN) || defined(BIG_ENDIAN)
+typedef struct CalCtlEdges {
+    A_UINT8  bChannel;
+    A_UINT8  flag   :2,
+             tPower :6;
+} __ATTRIB_PACK CAL_CTL_EDGES;
+#else
+typedef struct CalCtlEdges {
+    A_UINT8  bChannel;
+    A_UINT8  tPower :6,
+             flag   :2;
+} __ATTRIB_PACK CAL_CTL_EDGES;
+#endif
+
+typedef struct CalCtlData {
+    CAL_CTL_EDGES  ctlEdges[AR5416_MAX_CHAINS][AR5416_NUM_BAND_EDGES];
+} __ATTRIB_PACK CAL_CTL_DATA;
+
+typedef struct ar5416Eeprom {
+    BASE_EEP_HEADER    baseEepHeader;         // 64 B
+    A_UINT8   custData[64];                   // 64 B
+    MODAL_EEP_HEADER   modalHeader[2];        // 200 B
+    A_UINT8            calFreqPier5G[AR5416_NUM_5G_CAL_PIERS];
+    A_UINT8            calFreqPier2G[AR5416_NUM_2G_CAL_PIERS];
+    CAL_DATA_PER_FREQ  calPierData5G[AR5416_MAX_CHAINS][AR5416_NUM_5G_CAL_PIERS];
+    CAL_DATA_PER_FREQ  calPierData2G[AR5416_MAX_CHAINS][AR5416_NUM_2G_CAL_PIERS];
+    CAL_TARGET_POWER_LEG calTargetPower5G[AR5416_NUM_5G_20_TARGET_POWERS];
+    CAL_TARGET_POWER_HT  calTargetPower5GHT20[AR5416_NUM_5G_20_TARGET_POWERS];
+    CAL_TARGET_POWER_HT  calTargetPower5GHT40[AR5416_NUM_5G_40_TARGET_POWERS];
+    CAL_TARGET_POWER_LEG calTargetPowerCck[AR5416_NUM_2G_CCK_TARGET_POWERS];
+    CAL_TARGET_POWER_LEG calTargetPower2G[AR5416_NUM_2G_20_TARGET_POWERS];
+    CAL_TARGET_POWER_HT  calTargetPower2GHT20[AR5416_NUM_2G_20_TARGET_POWERS];
+    CAL_TARGET_POWER_HT  calTargetPower2GHT40[AR5416_NUM_2G_40_TARGET_POWERS];
+    A_UINT8            ctlIndex[AR5416_NUM_CTLS];
+    CAL_CTL_DATA       ctlData[AR5416_NUM_CTLS];
+    A_UINT8            padding;
+} __ATTRIB_PACK AR5416_EEPROM;
+
+#pragma pack (pop)
+
+typedef enum ConformanceTestLimits {
+    FCC        = 0x10,
+    MKK        = 0x40,
+    ETSI       = 0x30,
+    SD_NO_CTL  = 0xE0,
+    NO_CTL     = 0xFF,
+    CTL_MODE_M = 0xF,
+    CTL_11A    = 0,
+    CTL_11B    = 1,
+    CTL_11G    = 2,
+    CTL_TURBO  = 3,
+    CTL_108G   = 4,
+    CTL_2GHT20 = 5,
+    CTL_5GHT20 = 6,
+    CTL_2GHT40 = 7,
+    CTL_5GHT40 = 8,
+} ATH_CTLS;
+
+#endif /* #ifndef _HPUSB_H */
diff --git a/drivers/staging/otus/hal/otus.ini b/drivers/staging/otus/hal/otus.ini
new file mode 100644
index 000000000000..34efeb6c285b
--- /dev/null
+++ b/drivers/staging/otus/hal/otus.ini
@@ -0,0 +1,414 @@
+/* 8602 : update mismatch register between NDIS and ART */
+static const u32_t ar5416Modes[][6] = {
+/* Register   A-20        A-20/40     G-20/40     G-20        G-Turbo    */
+     {0x9800, 0x00000007, 0x00000007, 0x00000007, 0x00000007, 0},
+     {0x9804, 0x00000300, 0x000003c4, 0x000003c4, 0x00000300, 0},
+     {0x9808, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x980c, 0xad848e19, 0xad848e19, 0xad848e19, 0xad848e19, 0},
+     {0x9810, 0x7d14e000, 0x7d14e000, 0x7d14e000, 0x7d14e000, 0},
+     {0x9814, 0x9c0a9f6b, 0x9c0a9f6b, 0x9c0a9f6b, 0x9c0a9f6b, 0},
+     {0x9818, 0x00000090, 0x00000090, 0x00000090, 0x00000090, 0},
+     {0x981c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9820, 0x02020200, 0x02020200, 0x02020200, 0x02020200, 0},
+     {0x9824, 0x00000e0e, 0x00000e0e, 0x00000e0e, 0x00000e0e, 0},
+     {0x9828, 0x0a020001, 0x0a020001, 0x0a020001, 0x0a020001, 0},
+     {0x982c, 0x0000a000, 0x0000a000, 0x0000a000, 0x0000a000, 0},
+     {0x9830, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9834, 0x00000e0e, 0x00000e0e, 0x00000e0e, 0x00000e0e, 0},
+     {0x9838, 0x00000007, 0x00000007, 0x00000007, 0x00000007, 0},
+     {0x983c, 0x00200400, 0x00200400, 0x00200400, 0x00200400, 0},
+     {0x9840, 0x206a002e, 0x206a002e, 0x206a002e, 0x206a002e, 0},
+     {0x9844, 0x1372161e, 0x13721c1e, 0x13721c24, 0x137216a4, 0},
+     {0x9848, 0x001a6a65, 0x001a6a65, 0x00197a68, 0x00197a68, 0},
+     {0x984c, 0x1284233c, 0x1284233c, 0x1284233c, 0x1284233c, 0},
+     {0x9850, 0x6c48b4e4, 0x6c48b4e4, 0x6c48b0e4, 0x6c48b0e4, 0},
+     {0x9854, 0x00000859, 0x00000859, 0x00000859, 0x00000859, 0},
+     {0x9858, 0x7ec80d2e, 0x7ec80d2e, 0x7ec80d2e, 0x7ec80d2e, 0},
+     {0x985c, 0x31395c5e, 0x31395c5e, 0x31395c5e, 0x31395c5e, 0},
+     {0x9860, 0x0004dd10, 0x0004dd10, 0x0004dd20, 0x0004dd20, 0},
+     {0x9868, 0x409a4190, 0x409a4190, 0x409a4190, 0x409a4190, 0},
+     {0x986c, 0x050cb081, 0x050cb081, 0x050cb081, 0x050cb081, 0},
+     {0x9900, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9904, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9908, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x990c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9914, 0x000007d0, 0x000007d0, 0x00000898, 0x00000898, 0},
+     {0x9918, 0x00000118, 0x00000230, 0x00000268, 0x00000134, 0},
+     {0x991c, 0x10000fff, 0x10000fff, 0x10000fff, 0x10000fff, 0},
+     {0x9920, 0x0510081c, 0x0510081c, 0x0510001c, 0x0510001c, 0},
+     {0x9924, 0xd0058a15, 0xd0058a15, 0xd0058a15, 0xd0058a15, 0},
+     {0x9928, 0x00000001, 0x00000001, 0x00000001, 0x00000001, 0},
+     {0x992c, 0x00000004, 0x00000004, 0x00000004, 0x00000004, 0},
+     {0x9934, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0x9938, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0x993c, 0x0000007f, 0x0000007f, 0x0000007f, 0x0000007f, 0},
+     {0x9944, 0xdfb81020, 0xdfb81020, 0xdfb81020, 0xdfb81020, 0},
+     {0x9948, 0x9280b212, 0x9280b212, 0x9280b212, 0x9280b212, 0},
+     {0x994c, 0x00020028, 0x00020028, 0x00020028, 0x00020028, 0},
+     {0x9954, 0x5d50e188, 0x5d50e188, 0x5d50e188, 0x5d50e188, 0},
+     {0x9958, 0x00081fff, 0x00081fff, 0x00081fff, 0x00081fff, 0},
+     {0x9960, 0x00009b40, 0x00009b40, 0x00009b40, 0x00009b40, 0},
+     {0x9964, 0x00001120, 0x00001120, 0x00001120, 0x00001120, 0},
+     {0x9970, 0x190fb515, 0x190fb515, 0x190fb515, 0x190fb515, 0},
+     {0x9974, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9978, 0x00000001, 0x00000001, 0x00000001, 0x00000001, 0},
+     {0x997c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9980, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9984, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9988, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x998c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9990, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9994, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9998, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x999c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x99a0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x99a4, 0x00000007, 0x00000007, 0x00000007, 0x00000007, 0},
+     {0x99a8, 0x001fff00, 0x001fff00, 0x001fff00, 0x001fff00, 0},
+     {0x99ac, 0x006f00c4, 0x006f00c4, 0x006f00c4, 0x006f00c4, 0},
+     {0x99b0, 0x03051000, 0x03051000, 0x03051000, 0x03051000, 0},
+     {0x99b4, 0x00000820, 0x00000820, 0x00000820, 0x00000820, 0},
+     {0x99c0, 0x038919be, 0x038919be, 0x038919be, 0x038919be, 0},
+     {0x99c4, 0x06336f77, 0x06336f77, 0x06336f77, 0x06336f77, 0},
+     {0x99c8, 0x60f6532c, 0x60f6532c, 0x60f6532c, 0x60f6532c, 0},
+     {0x99cc, 0x08f186c8, 0x08f186c8, 0x08f186c8, 0x08f186c8, 0},
+     {0x99d0, 0x00046384, 0x00046384, 0x00046384, 0x00046384, 0},
+     {0x99d4, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x99d8, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x99dc, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x99e0, 0x00000200, 0x00000200, 0x00000200, 0x00000200, 0},
+     {0x99e4, 0x64646464, 0x64646464, 0x64646464, 0x64646464, 0},
+     {0x99e8, 0x3c787878, 0x3c787878, 0x3c787878, 0x3c787878, 0},
+     {0x99ec, 0x000000aa, 0x000000aa, 0x000000aa, 0x000000aa, 0},
+     {0x99f0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x99fc, 0x00001042, 0x00001042, 0x00001042, 0x00001042, 0},
+     {0x9a00, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9a04, 0x00000040, 0x00000040, 0x00000040, 0x00000040, 0},
+     {0x9a08, 0x00000080, 0x00000080, 0x00000080, 0x00000080, 0},
+     {0x9a0c, 0x000001a1, 0x000001a1, 0x00000141, 0x00000141, 0},
+     {0x9a10, 0x000001e1, 0x000001e1, 0x00000181, 0x00000181, 0},
+     {0x9a14, 0x00000021, 0x00000021, 0x000001c1, 0x000001c1, 0},
+     {0x9a18, 0x00000061, 0x00000061, 0x00000001, 0x00000001, 0},
+     {0x9a1c, 0x00000168, 0x00000168, 0x00000041, 0x00000041, 0},
+     {0x9a20, 0x000001a8, 0x000001a8, 0x000001a8, 0x000001a8, 0},
+     {0x9a24, 0x000001e8, 0x000001e8, 0x000001e8, 0x000001e8, 0},
+     {0x9a28, 0x00000028, 0x00000028, 0x00000028, 0x00000028, 0},
+     {0x9a2c, 0x00000068, 0x00000068, 0x00000068, 0x00000068, 0},
+     {0x9a30, 0x00000189, 0x00000189, 0x000000a8, 0x000000a8, 0},
+     {0x9a34, 0x000001c9, 0x000001c9, 0x00000169, 0x00000169, 0},
+     {0x9a38, 0x00000009, 0x00000009, 0x000001a9, 0x000001a9, 0},
+     {0x9a3c, 0x00000049, 0x00000049, 0x000001e9, 0x000001e9, 0},
+     {0x9a40, 0x00000089, 0x00000089, 0x00000029, 0x00000029, 0},
+     {0x9a44, 0x00000170, 0x00000170, 0x00000069, 0x00000069, 0},
+     {0x9a48, 0x000001b0, 0x000001b0, 0x00000190, 0x00000190, 0},
+     {0x9a4c, 0x000001f0, 0x000001f0, 0x000001d0, 0x000001d0, 0},
+     {0x9a50, 0x00000030, 0x00000030, 0x00000010, 0x00000010, 0},
+     {0x9a54, 0x00000070, 0x00000070, 0x00000050, 0x00000050, 0},
+     {0x9a58, 0x00000191, 0x00000191, 0x00000090, 0x00000090, 0},
+     {0x9a5c, 0x000001d1, 0x000001d1, 0x00000151, 0x00000151, 0},
+     {0x9a60, 0x00000011, 0x00000011, 0x00000191, 0x00000191, 0},
+     {0x9a64, 0x00000051, 0x00000051, 0x000001d1, 0x000001d1, 0},
+     {0x9a68, 0x00000091, 0x00000091, 0x00000011, 0x00000011, 0},
+     {0x9a6c, 0x000001b8, 0x000001b8, 0x00000051, 0x00000051, 0},
+     {0x9a70, 0x000001f8, 0x000001f8, 0x00000198, 0x00000198, 0},
+     {0x9a74, 0x00000038, 0x00000038, 0x000001d8, 0x000001d8, 0},
+     {0x9a78, 0x00000078, 0x00000078, 0x00000018, 0x00000018, 0},
+     {0x9a7c, 0x00000199, 0x00000199, 0x00000058, 0x00000058, 0},
+     {0x9a80, 0x000001d9, 0x000001d9, 0x00000098, 0x00000098, 0},
+     {0x9a84, 0x00000019, 0x00000019, 0x00000159, 0x00000159, 0},
+     {0x9a88, 0x00000059, 0x00000059, 0x00000199, 0x00000199, 0},
+     {0x9a8c, 0x00000099, 0x00000099, 0x000001d9, 0x000001d9, 0},
+     {0x9a90, 0x000000d9, 0x000000d9, 0x00000019, 0x00000019, 0},
+     {0x9a94, 0x000000f9, 0x000000f9, 0x00000059, 0x00000059, 0},
+     {0x9a98, 0x000000f9, 0x000000f9, 0x00000099, 0x00000099, 0},
+     {0x9a9c, 0x000000f9, 0x000000f9, 0x000000d9, 0x000000d9, 0},
+     {0x9aa0, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9aa4, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9aa8, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9aac, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ab0, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ab4, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ab8, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9abc, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ac0, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ac4, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ac8, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9acc, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ad0, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ad4, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ad8, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9adc, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ae0, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ae4, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ae8, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9aec, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9af0, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9af4, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9af8, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9afc, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9b00, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9b04, 0x00000001, 0x00000001, 0x00000001, 0x00000001, 0},
+     {0x9b08, 0x00000002, 0x00000002, 0x00000002, 0x00000002, 0},
+     {0x9b0c, 0x00000003, 0x00000003, 0x00000003, 0x00000003, 0},
+     {0x9b10, 0x00000004, 0x00000004, 0x00000004, 0x00000004, 0},
+     {0x9b14, 0x00000005, 0x00000005, 0x00000005, 0x00000005, 0},
+     {0x9b18, 0x00000008, 0x00000008, 0x00000008, 0x00000008, 0},
+     {0x9b1c, 0x00000009, 0x00000009, 0x00000009, 0x00000009, 0},
+     {0x9b20, 0x0000000a, 0x0000000a, 0x0000000a, 0x0000000a, 0},
+     {0x9b24, 0x0000000b, 0x0000000b, 0x0000000b, 0x0000000b, 0},
+     {0x9b28, 0x0000000c, 0x0000000c, 0x0000000c, 0x0000000c, 0},
+     {0x9b2c, 0x0000000d, 0x0000000d, 0x0000000d, 0x0000000d, 0},
+     {0x9b30, 0x00000010, 0x00000010, 0x00000010, 0x00000010, 0},
+     {0x9b34, 0x00000011, 0x00000011, 0x00000011, 0x00000011, 0},
+     {0x9b38, 0x00000012, 0x00000012, 0x00000012, 0x00000012, 0},
+     {0x9b3c, 0x00000013, 0x00000013, 0x00000013, 0x00000013, 0},
+     {0x9b40, 0x00000014, 0x00000014, 0x00000014, 0x00000014, 0},
+     {0x9b44, 0x00000015, 0x00000015, 0x00000015, 0x00000015, 0},
+     {0x9b48, 0x00000018, 0x00000018, 0x00000018, 0x00000018, 0},
+     {0x9b4c, 0x00000019, 0x00000019, 0x00000019, 0x00000019, 0},
+     {0x9b50, 0x0000001a, 0x0000001a, 0x0000001a, 0x0000001a, 0},
+     {0x9b54, 0x0000001b, 0x0000001b, 0x0000001b, 0x0000001b, 0},
+     {0x9b58, 0x0000001c, 0x0000001c, 0x0000001c, 0x0000001c, 0},
+     {0x9b5c, 0x0000001d, 0x0000001d, 0x0000001d, 0x0000001d, 0},
+     {0x9b60, 0x00000020, 0x00000020, 0x00000020, 0x00000020, 0},
+     {0x9b64, 0x00000021, 0x00000021, 0x00000021, 0x00000021, 0},
+     {0x9b68, 0x00000022, 0x00000022, 0x00000022, 0x00000022, 0},
+     {0x9b6c, 0x00000023, 0x00000023, 0x00000023, 0x00000023, 0},
+     {0x9b70, 0x00000024, 0x00000024, 0x00000024, 0x00000024, 0},
+     {0x9b74, 0x00000025, 0x00000025, 0x00000025, 0x00000025, 0},
+     {0x9b78, 0x00000028, 0x00000028, 0x00000028, 0x00000028, 0},
+     {0x9b7c, 0x00000029, 0x00000029, 0x00000029, 0x00000029, 0},
+     {0x9b80, 0x0000002a, 0x0000002a, 0x0000002a, 0x0000002a, 0},
+     {0x9b84, 0x0000002b, 0x0000002b, 0x0000002b, 0x0000002b, 0},
+     {0x9b88, 0x0000002c, 0x0000002c, 0x0000002c, 0x0000002c, 0},
+     {0x9b8c, 0x0000002d, 0x0000002d, 0x0000002d, 0x0000002d, 0},
+     {0x9b90, 0x00000030, 0x00000030, 0x00000030, 0x00000030, 0},
+     {0x9b94, 0x00000031, 0x00000031, 0x00000031, 0x00000031, 0},
+     {0x9b98, 0x00000032, 0x00000032, 0x00000032, 0x00000032, 0},
+     {0x9b9c, 0x00000033, 0x00000033, 0x00000033, 0x00000033, 0},
+     {0x9ba0, 0x00000034, 0x00000034, 0x00000034, 0x00000034, 0},
+     {0x9ba4, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9ba8, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bac, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bb0, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bb4, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bb8, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bbc, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bc0, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bc4, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bc8, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bcc, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bd0, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bd4, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bd8, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bdc, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9be0, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9be4, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9be8, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bec, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bf0, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bf4, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bf8, 0x00000010, 0x00000010, 0x00000010, 0x00000010, 0},
+     {0x9bfc, 0x0000001a, 0x0000001a, 0x0000001a, 0x0000001a, 0},
+     {0x9c00, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c0c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c10, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c14, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c18, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c1c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c20, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c24, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c28, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c2c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c30, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c34, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c38, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c3c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9cf0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9cf4, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9cf8, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9cfc, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa200, 0x00000008, 0x00000008, 0x0000000e, 0x0000000e, 0},
+     {0xa204, 0x00000440, 0x00000440, 0x00000440, 0x00000440, 0},
+     {0xa208, 0xd6be4788, 0xd6be4788, 0xd03e4788, 0xd03e4788, 0},
+     {0xa20c, 0x012e8160, 0x012e8160, 0x012a8160, 0x012a8160, 0},
+     {0xa210, 0x40806333, 0x40806333, 0x40806333, 0x40806333, 0},
+     {0xa214, 0x00106c10, 0x00106c10, 0x00106c10, 0x00106c10, 0},
+     {0xa218, 0x009c4060, 0x009c4060, 0x009c4060, 0x009c4060, 0},
+     {0xa21c, 0x1883800a, 0x1883800a, 0x1883800a, 0x1883800a, 0},
+     {0xa220, 0x018830c6, 0x018830c6, 0x018830c6, 0x018830c6, 0},
+     {0xa224, 0x00000400, 0x00000400, 0x00000400, 0x00000400, 0},
+     {0xa228, 0x000009b5, 0x000009b5, 0x000009b5, 0x000009b5, 0},
+     {0xa22c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa230, 0x00000108, 0x00000210, 0x00000210, 0x00000108, 0},
+     {0xa234, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0xa238, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0xa23c, 0x13c889af, 0x13c889af, 0x13c889af, 0x13c889af, 0},
+     {0xa240, 0x38490a20, 0x38490a20, 0x38490a20, 0x38490a20, 0},
+     {0xa244, 0x00007bb6, 0x00007bb6, 0x00007bb6, 0x00007bb6, 0},
+     {0xa248, 0x0fff3ffc, 0x0fff3ffc, 0x0fff3ffc, 0x0fff3ffc, 0},
+     {0xa24c, 0x00000001, 0x00000001, 0x00000001, 0x00000001, 0},
+     {0xa250, 0x0000a000, 0x0000a000, 0x0000a000, 0x0000a000, 0},
+     {0xa254, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa258, 0x0cc75380, 0x0cc75380, 0x0cc75380, 0x0cc75380, 0},
+     {0xa25c, 0x0f0f0f01, 0x0f0f0f01, 0x0f0f0f01, 0x0f0f0f01, 0},
+     {0xa260, 0xdfa91f01, 0xdfa91f01, 0xdfa91f01, 0xdfa91f01, 0},
+     {0xa264, 0x00418a11, 0x00418a11, 0x00418a11, 0x00418a11, 0},
+     {0xa268, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa26c, 0x09249126, 0x09249126, 0x09249126, 0x09249126, 0},
+     {0xa274, 0x0a1a9caa, 0x0a1a9caa, 0x0a1a7caa, 0x0a1a7caa, 0},
+     {0xa278, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce, 0},
+     {0xa27c, 0x051701ce, 0x051701ce, 0x051701ce, 0x051701ce, 0},
+     {0xa300, 0x18010000, 0x18010000, 0x18010000, 0x18010000, 0},
+     {0xa304, 0x30032602, 0x30032602, 0x2e032402, 0x2e032402, 0},
+     {0xa308, 0x48073e06, 0x48073e06, 0x4a0a3c06, 0x4a0a3c06, 0},
+     {0xa30c, 0x560b4c0a, 0x560b4c0a, 0x621a540b, 0x621a540b, 0},
+     {0xa310, 0x641a600f, 0x641a600f, 0x764f6c1b, 0x764f6c1b, 0},
+     {0xa314, 0x7a4f6e1b, 0x7a4f6e1b, 0x845b7a5a, 0x845b7a5a, 0},
+     {0xa318, 0x8c5b7e5a, 0x8c5b7e5a, 0x950f8ccf, 0x950f8ccf, 0},
+     {0xa31c, 0x9d0f96cf, 0x9d0f96cf, 0xa5cf9b4f, 0xa5cf9b4f, 0},
+     {0xa320, 0xb51fa69f, 0xb51fa69f, 0xbddfaf1f, 0xbddfaf1f, 0},
+     {0xa324, 0xcb3fbd07, 0xcb3fbcbf, 0xd1ffc93f, 0xd1ffc93f, 0},
+     {0xa328, 0x0000d7bf, 0x0000d7bf, 0x00000000, 0x00000000, 0},
+     {0xa32c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa330, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa334, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa338, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa33c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa340, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa344, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa348, 0x3fffffff, 0x3fffffff, 0x3fffffff, 0x3fffffff, 0},
+     {0xa34c, 0x3fffffff, 0x3fffffff, 0x3fffffff, 0x3fffffff, 0},
+     {0xa350, 0x3fffffff, 0x3fffffff, 0x3fffffff, 0x3fffffff, 0},
+     {0xa354, 0x0003ffff, 0x0003ffff, 0x0003ffff, 0x0003ffff, 0},
+     {0xa358, 0x79a8aa1f, 0x79a8aa1f, 0x79a8aa1f, 0x79a8aa1f, 0},
+     {0xa388, 0x08000000, 0x08000000, 0x08000000, 0x08000000, 0},
+     {0xa38c, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0xa390, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0xa394, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce, 0},
+     {0xa398, 0x000001ce, 0x000001ce, 0x000001ce, 0x000001ce, 0},
+     {0xa39c, 0x00000007, 0x00000007, 0x00000007, 0x00000007, 0},
+     {0xa3a0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3a4, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3a8, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3ac, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3b0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3b4, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3b8, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3bc, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3c0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3c4, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3c8, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3cc, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0xa3d0, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0xa3d4, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0xa3d8, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3dc, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce, 0},
+     {0xa3e0, 0x000000c0, 0x000000c0, 0x000000c0, 0x000000c0, 0},
+     {0xa848, 0x00180a65, 0x00180a65, 0x00180a68, 0x00180a68, 0},
+     {0xa920, 0x0510001c, 0x0510001c, 0x0510001c, 0x0510001c, 0},
+     {0xa960, 0x00009b40, 0x00009b40, 0x00009b40, 0x00009b40, 0},
+     {0xb20c, 0x012e8160, 0x012e8160, 0x012a8160, 0x012a8160, 0},
+     {0xb26c, 0x09249126, 0x09249126, 0x09249126, 0x09249126, 0},
+     {0xb848, 0x00180a65, 0x00180a65, 0x00180a68, 0x00180a68, 0},
+     {0xb920, 0x0510001c, 0x0510001c, 0x0510001c, 0x0510001c, 0},
+     {0xb960, 0x00009b40, 0x00009b40, 0x00009b40, 0x00009b40, 0},
+     {0xc20c, 0x012e8160, 0x012e8160, 0x012a8160, 0x012a8160, 0},
+     {0xc26c, 0x09249126, 0x09249126, 0x09249126, 0x09249126, 0},
+     //{0xc864, 0x0001ce00, 0x0001ce00, 0x0001ce00, 0x0001ce00, 0},
+     {0xc864, 0x0001c600, 0x0001c600, 0x0001c600, 0x0001c600, 0},
+     {0xc95c, 0x004b6a8e, 0x004b6a8e, 0x004b6a8e, 0x004b6a8e, 0},
+     {0xc968, 0x000003ce, 0x000003ce, 0x000003ce, 0x000003ce, 0},
+     {0xc9bc, 0x00181400, 0x00181400, 0x00181400, 0x00181400, 0},
+     {0xd270, 0x00820820, 0x00820820, 0x00820820, 0x00820820, 0},
+     {0xd35c, 0x066c420f, 0x066c420f, 0x066c420f, 0x066c420f, 0},
+     {0xd360, 0x0f282207, 0x0f282207, 0x0f282207, 0x0f282207, 0},
+     {0xd364, 0x17601685, 0x17601685, 0x17601685, 0x17601685, 0},
+     {0xd368, 0x1f801104, 0x1f801104, 0x1f801104, 0x1f801104, 0},
+     {0xd36c, 0x37a00c03, 0x37a00c03, 0x37a00c03, 0x37a00c03, 0},
+     {0xd370, 0x3fc40883, 0x3fc40883, 0x3fc40883, 0x3fc40883, 0},
+     {0xd374, 0x57c00803, 0x57c00803, 0x57c00803, 0x57c00803, 0},
+     {0xd378, 0x5fd80682, 0x5fd80682, 0x5fd80682, 0x5fd80682, 0},
+     {0xd37c, 0x7fe00482, 0x7fe00482, 0x7fe00482, 0x7fe00482, 0},
+     {0xd380, 0x7f3c7bba, 0x7f3c7bba, 0x7f3c7bba, 0x7f3c7bba, 0},
+     {0xd384, 0xf3307ff0, 0xf3307ff0, 0xf3307ff0, 0xf3307ff0, 0}
+};
+
+
+static const u32_t otusBank[][3] = {
+     //# bank 0
+     {0x98b0,  0x1e5795e5,  0x1e5795e5},
+     {0x98e0,  0x02008020,  0x02008020},
+     //# bank 1
+     {0x98b0,  0x02108421,  0x02108421},
+     {0x98ec,  0x00000008,  0x00000008},
+     //# bank 2
+     {0x98b0,  0x0e73ff17,  0x0e73ff17},
+     {0x98e0,  0x00000420,  0x00000420},
+     //# bank 3
+     {0x98f0,  0x01400018,  0x01c00018},
+     //# bank 4
+     {0x98b0,  0x000001a1,  0x000001a1},
+     {0x98e8,  0x00000001,  0x00000001},
+     //# bank 5
+     {0x98b0,  0x00000013,  0x00000013},
+     {0x98e4,  0x00000002,  0x00000002},
+     //# bank 6
+     {0x98b0,  0x00000000,  0x00000000},
+     {0x98b0,  0x00000000,  0x00000000},
+     {0x98b0,  0x00000000,  0x00000000},
+     {0x98b0,  0x00000000,  0x00000000},
+     {0x98b0,  0x00000000,  0x00000000},
+     {0x98b0,  0x00004000,  0x00004000},
+     {0x98b0,  0x00006c00,  0x00006c00},
+     {0x98b0,  0x00002c00,  0x00002c00},
+     {0x98b0,  0x00004800,  0x00004800},
+     {0x98b0,  0x00004000,  0x00004000},
+     {0x98b0,  0x00006000,  0x00006000},
+     {0x98b0,  0x00001000,  0x00001000},
+     {0x98b0,  0x00004000,  0x00004000},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00087c00,  0x00087c00},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00005400,  0x00005400},
+     {0x98b0,  0x00000c00,  0x00000c00},
+     {0x98b0,  0x00001800,  0x00001800},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00006c00,  0x00006c00},
+     {0x98b0,  0x00006c00,  0x00006c00},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00002c00,  0x00002c00},
+     {0x98b0,  0x00003c00,  0x00003c00},
+     {0x98b0,  0x00003800,  0x00003800},
+     {0x98b0,  0x00001c00,  0x00001c00},
+     {0x98b0,  0x00000800,  0x00000800},
+     {0x98b0,  0x00000408,  0x00000408},
+     {0x98b0,  0x00004c15,  0x00004c15},
+     {0x98b0,  0x00004188,  0x00004188},
+     {0x98b0,  0x0000201e,  0x0000201e},
+     {0x98b0,  0x00010408,  0x00010408},
+     {0x98b0,  0x00000801,  0x00000801},
+     {0x98b0,  0x00000c08,  0x00000c08},
+     {0x98b0,  0x0000181e,  0x0000181e},
+     {0x98b0,  0x00001016,  0x00001016},
+     {0x98b0,  0x00002800,  0x00002800},
+     {0x98b0,  0x00004010,  0x00004010},
+     {0x98b0,  0x0000081c,  0x0000081c},
+     {0x98b0,  0x00000115,  0x00000115},
+     {0x98b0,  0x00000015,  0x00000015},
+     {0x98b0,  0x00000066,  0x00000066},
+     {0x98b0,  0x0000001c,  0x0000001c},
+     {0x98b0,  0x00000000,  0x00000000},
+     {0x98b0,  0x00000004,  0x00000004},
+     {0x98b0,  0x00000015,  0x00000015},
+     {0x98b0,  0x0000001f,  0x0000001f},
+     {0x98e0,  0x00000000,  0x00000400},
+     //# bank 7
+     {0x98b0,  0x000000a0,  0x000000a0},
+     {0x98b0,  0x00000000,  0x00000000},
+     {0x98b0,  0x00000040,  0x00000040},
+     {0x98f0,  0x0000001c,  0x0000001c}
+};
diff --git a/drivers/staging/otus/ioctl.c b/drivers/staging/otus/ioctl.c
new file mode 100644
index 000000000000..7a5c1e876b01
--- /dev/null
+++ b/drivers/staging/otus/ioctl.c
@@ -0,0 +1,2913 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : ioctl.c                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains Linux wireless extension related functons. */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+#include <linux/module.h>
+#include <linux/if_arp.h>
+#include <asm/uaccess.h>
+
+#include "usbdrv.h"
+
+#define ZD_IOCTL_WPA			    (SIOCDEVPRIVATE + 1)
+#define ZD_IOCTL_PARAM			    (SIOCDEVPRIVATE + 2)
+#define ZD_IOCTL_GETWPAIE		    (SIOCDEVPRIVATE + 3)
+#ifdef ZM_ENABLE_CENC
+#define ZM_IOCTL_CENC               (SIOCDEVPRIVATE + 4)
+#endif //ZM_ENABLE_CENC
+#define ZD_PARAM_ROAMING		    0x0001
+#define ZD_PARAM_PRIVACY		    0x0002
+#define ZD_PARAM_WPA			    0x0003
+#define ZD_PARAM_COUNTERMEASURES	0x0004
+#define ZD_PARAM_DROPUNENCRYPTED	0x0005
+#define ZD_PARAM_AUTH_ALGS		    0x0006
+#define ZD_PARAM_WPS_FILTER		    0x0007
+
+#ifdef ZM_ENABLE_CENC
+#define P80211_PACKET_CENCFLAG		0x0001
+#endif //ZM_ENABLE_CENC
+#define P80211_PACKET_SETKEY     	0x0003
+
+#define ZD_CMD_SET_ENCRYPT_KEY		0x0001
+#define ZD_CMD_SET_MLME			    0x0002
+#define ZD_CMD_SCAN_REQ			    0x0003
+#define ZD_CMD_SET_GENERIC_ELEMENT	0x0004
+#define ZD_CMD_GET_TSC			    0x0005
+
+#define ZD_CRYPT_ALG_NAME_LEN		16
+#define ZD_MAX_KEY_SIZE			    32
+#define ZD_MAX_GENERIC_SIZE		    64
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+extern u16_t zfLnxGetVapId(zdev_t* dev);
+
+static const u32_t channel_frequency_11A[] =
+{
+//Even element for Channel Number, Odd for Frequency
+    36,5180,
+    40,5200,
+    44,5220,
+    48,5240,
+    52,5260,
+    56,5280,
+    60,5300,
+    64,5320,
+    100,5500,
+    104,5520,
+    108,5540,
+    112,5560,
+    116,5580,
+    120,5600,
+    124,5620,
+    128,5640,
+    132,5660,
+    136,5680,
+    140,5700,
+//
+    184,4920,
+    188,4940,
+    192,4960,
+    196,4980,
+    8,5040,
+    12,5060,
+    16,5080,
+    34,5170,
+    38,5190,
+    42,5210,
+    46,5230,
+//
+    149,5745,
+    153,5765,
+    157,5785,
+    161,5805,
+    165,5825
+//
+};
+
+int usbdrv_freq2chan(u32_t freq)
+{
+    /* 2.4G Hz */
+    if (freq > 2400 && freq < 3000)
+    {
+        return ((freq-2412)/5) + 1;
+    }
+    else
+    {
+        u16_t ii;
+        u16_t num_chan = sizeof(channel_frequency_11A)/sizeof(u32_t);
+
+        for(ii = 1; ii < num_chan; ii += 2)
+        {
+            if (channel_frequency_11A[ii] == freq)
+                return channel_frequency_11A[ii-1];
+        }
+    }
+
+    return 0;
+}
+
+int usbdrv_chan2freq(int chan)
+{
+    int freq;
+
+    /* If channel number is out of range */
+    if (chan > 165 || chan <= 0)
+        return -1;
+
+    /* 2.4G band */
+    if (chan >= 1 && chan <= 13)
+    {
+        freq = (2412 + (chan - 1) * 5);
+        return freq;
+    }
+    else if (chan >= 36 && chan <= 165)
+    {
+        u16_t ii;
+        u16_t num_chan = sizeof(channel_frequency_11A)/sizeof(u32_t);
+
+        for(ii = 0; ii < num_chan; ii += 2)
+        {
+            if (channel_frequency_11A[ii] == chan)
+                return channel_frequency_11A[ii+1];
+        }
+
+        /* Can't find desired frequency */
+        if (ii == num_chan)
+           return -1;
+    }
+
+    /* Can't find deisred frequency */
+    return -1;
+}
+
+int usbdrv_ioctl_setessid(struct net_device *dev, struct iw_point *erq)
+{
+#ifdef ZM_HOSTAPD_SUPPORT
+    //struct usbdrv_private *macp = dev->ml_priv;
+    char essidbuf[IW_ESSID_MAX_SIZE+1];
+    int i;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    memset(essidbuf, 0, sizeof(essidbuf));
+
+    printk(KERN_ERR "usbdrv_ioctl_setessid\n");
+
+    //printk("ssidlen=%d\n", erq->length); //for any, it is 1.
+    if (erq->flags) {
+        if (erq->length > (IW_ESSID_MAX_SIZE+1))
+            return -E2BIG;
+
+        if (copy_from_user(essidbuf, erq->pointer, erq->length))
+            return -EFAULT;
+    }
+
+    //zd_DisasocAll(2);
+    //wait_ms(100);
+
+    printk(KERN_ERR "essidbuf: ");
+
+    for(i = 0; i < erq->length; i++)
+    {
+        printk(KERN_ERR "%02x ", essidbuf[i]);
+    }
+
+    printk(KERN_ERR "\n");
+
+    essidbuf[erq->length] = '\0';
+    //memcpy(macp->wd.ws.ssid, essidbuf, erq->length);
+    //macp->wd.ws.ssidLen = strlen(essidbuf)+2;
+    //macp->wd.ws.ssid[1] = strlen(essidbuf); // Update ssid length
+
+    zfiWlanSetSSID(dev, essidbuf, erq->length);
+#if 0
+    printk(KERN_ERR "macp->wd.ws.ssid: ");
+
+    for(i = 0; i < macp->wd.ws.ssidLen; i++)
+    {
+        printk(KERN_ERR "%02x ", macp->wd.ws.ssid[i]);
+    }
+
+    printk(KERN_ERR "\n");
+#endif
+    zfiWlanDisable(dev, 0);
+    zfiWlanEnable(dev);
+
+#endif
+
+    return 0;
+}
+
+int usbdrv_ioctl_getessid(struct net_device *dev, struct iw_point *erq)
+{
+     //struct usbdrv_private *macp = dev->ml_priv;
+     u8_t essidbuf[IW_ESSID_MAX_SIZE+1];
+     u8_t len;
+     u8_t i;
+
+
+     //len = macp->wd.ws.ssidLen;
+     //memcpy(essidbuf, macp->wd.ws.ssid, macp->wd.ws.ssidLen);
+     zfiWlanQuerySSID(dev, essidbuf, &len);
+
+     essidbuf[len] = 0;
+
+     printk(KERN_ERR "ESSID: ");
+
+     for(i = 0; i < len; i++)
+     {
+         printk(KERN_ERR "%c", essidbuf[i]);
+     }
+
+     printk(KERN_ERR "\n");
+
+     erq->flags= 1;
+     erq->length = strlen(essidbuf) + 1;
+
+     if (erq->pointer)
+         if (copy_to_user(erq->pointer, essidbuf, erq->length))
+             return -EFAULT;
+
+     return 0;
+}
+
+
+int usbdrv_ioctl_setrts(struct net_device *dev, struct iw_param *rrq)
+{
+
+    return 0;
+}
+
+#if WIRELESS_EXT > 14
+/*
+ * Encode a WPA or RSN information element as a custom
+ * element using the hostap format.
+ */
+u32 encode_ie(void *buf, u32 bufsize, const u8 *ie, u32 ielen, const u8 *leader, u32 leader_len)
+{
+    u8 *p;
+    u32 i;
+
+    if (bufsize < leader_len)
+        return 0;
+    p = buf;
+    memcpy(p, leader, leader_len);
+    bufsize -= leader_len;
+    p += leader_len;
+    for (i = 0; i < ielen && bufsize > 2; i++)
+        p += sprintf(p, "%02x", ie[i]);
+    return (i == ielen ? p - (u8 *)buf : 0);
+}
+#endif                                            /* WIRELESS_EXT > 14 */
+
+/*------------------------------------------------------------------*/
+/*
+ * Translate scan data returned from the card to a card independent
+ * format that the Wireless Tools will understand
+ */
+char *usbdrv_translate_scan(struct net_device *dev,
+	struct iw_request_info *info, char *current_ev,
+        char *end_buf, struct zsBssInfo *list)
+{
+    struct iw_event iwe;                          /* Temporary buffer */
+    u16_t capabilities;
+    char *current_val;                            /* For rates */
+    char *last_ev;
+    int i;
+#if WIRELESS_EXT > 14
+    char    buf[64*2 + 30];
+#endif
+
+    last_ev = current_ev;
+
+/* First entry *MUST* be the AP MAC address */
+    iwe.cmd = SIOCGIWAP;
+    iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+    memcpy(iwe.u.ap_addr.sa_data, list->bssid, ETH_ALEN);
+    current_ev = iwe_stream_add_event(
+	info,
+	current_ev,
+	end_buf, &iwe, IW_EV_ADDR_LEN);
+
+    /* Ran out of buffer */
+    if (last_ev == current_ev)
+    {
+        return end_buf;
+    }
+
+    last_ev = current_ev;
+
+/* Other entries will be displayed in the order we give them */
+
+/* Add the ESSID */
+    iwe.u.data.length = list->ssid[1];
+    if(iwe.u.data.length > 32)
+        iwe.u.data.length = 32;
+    iwe.cmd = SIOCGIWESSID;
+    iwe.u.data.flags = 1;
+    current_ev = iwe_stream_add_point(
+	info,
+	current_ev, end_buf, &iwe, &list->ssid[2]);
+
+    /* Ran out of buffer */
+    if (last_ev == current_ev)
+    {
+        return end_buf;
+    }
+
+    last_ev = current_ev;
+
+/* Add mode */
+    iwe.cmd = SIOCGIWMODE;
+    capabilities = (list->capability[1] << 8) + list->capability[0];
+    if(capabilities & (0x01 | 0x02))
+    {
+        if(capabilities & 0x01)
+            iwe.u.mode = IW_MODE_MASTER;
+        else
+            iwe.u.mode = IW_MODE_ADHOC;
+        current_ev = iwe_stream_add_event(
+		info,
+		current_ev, end_buf, &iwe, IW_EV_UINT_LEN);
+    }
+
+    /* Ran out of buffer */
+    if (last_ev == current_ev)
+    {
+        return end_buf;
+    }
+
+    last_ev = current_ev;
+
+/* Add frequency */
+    iwe.cmd = SIOCGIWFREQ;
+    iwe.u.freq.m = list->channel;
+/* Channel frequency in KHz */
+    if (iwe.u.freq.m > 14)
+    {
+        if ((184 <= iwe.u.freq.m) && (iwe.u.freq.m<=196))
+              iwe.u.freq.m = 4000 + iwe.u.freq.m * 5;
+        else
+              iwe.u.freq.m = 5000 + iwe.u.freq.m * 5;
+    }
+    else
+    {
+        if (iwe.u.freq.m == 14)
+              iwe.u.freq.m = 2484;
+        else
+              iwe.u.freq.m = 2412 + (iwe.u.freq.m - 1) * 5;
+    }
+    iwe.u.freq.e = 6;
+    current_ev = iwe_stream_add_event(
+	info,
+    	current_ev, end_buf, &iwe, IW_EV_FREQ_LEN);
+
+    /* Ran out of buffer */
+    if (last_ev == current_ev)
+    {
+        return end_buf;
+    }
+
+    last_ev = current_ev;
+
+/* Add quality statistics */
+    iwe.cmd = IWEVQUAL;
+#if WIRELESS_EXT > 18
+    iwe.u.qual.updated = IW_QUAL_QUAL_UPDATED | IW_QUAL_LEVEL_UPDATED
+                        |IW_QUAL_NOISE_UPDATED;
+#endif
+    iwe.u.qual.level = list->signalStrength;
+    iwe.u.qual.noise = 0;
+    iwe.u.qual.qual = list->signalQuality;
+    current_ev = iwe_stream_add_event(
+	info,
+	current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
+
+    /* Ran out of buffer */
+    if (last_ev == current_ev)
+    {
+        return end_buf;
+    }
+
+    last_ev = current_ev;
+
+/* Add encryption capability */
+
+    iwe.cmd = SIOCGIWENCODE;
+    if(capabilities & 0x10)
+        iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+    else
+        iwe.u.data.flags = IW_ENCODE_DISABLED;
+
+    iwe.u.data.length = 0;
+    current_ev = iwe_stream_add_point(
+	info,
+	current_ev, end_buf, &iwe, list->ssid);
+
+    /* Ran out of buffer */
+    if (last_ev == current_ev)
+    {
+        return end_buf;
+    }
+
+    last_ev = current_ev;
+
+/* Rate : stuffing multiple values in a single event require a bit
+ * more of magic */
+    current_val = current_ev + IW_EV_LCP_LEN;
+
+    iwe.cmd = SIOCGIWRATE;
+/* Those two flags are ignored... */
+    iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
+
+    for(i = 0 ; i < list->supportedRates[1] ; i++)
+    {
+/* Bit rate given in 500 kb/s units (+ 0x80) */
+        iwe.u.bitrate.value = ((list->supportedRates[i+2] & 0x7f) * 500000);
+/* Add new value to event */
+        current_val = iwe_stream_add_value(
+		info,
+		current_ev, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
+
+        /* Ran out of buffer */
+        if (last_ev == current_val)
+        {
+            return end_buf;
+        }
+
+        last_ev = current_val;
+    }
+
+    for (i = 0 ; i < list->extSupportedRates[1] ; i++)
+    {
+/* Bit rate given in 500 kb/s units (+ 0x80) */
+        iwe.u.bitrate.value = ((list->extSupportedRates[i+2] & 0x7f) * 500000);
+/* Add new value to event */
+        current_val = iwe_stream_add_value(
+		info,
+		current_ev, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
+
+        /* Ran out of buffer */
+        if (last_ev == current_val)
+        {
+            return end_buf;
+        }
+
+        last_ev = current_ev;
+    }
+
+/* Check if we added any event */
+    if((current_val - current_ev) > IW_EV_LCP_LEN)
+        current_ev = current_val;
+#if WIRELESS_EXT > 14
+#define IEEE80211_ELEMID_RSN 0x30
+    memset(&iwe, 0, sizeof(iwe));
+    iwe.cmd = IWEVCUSTOM;
+    snprintf(buf, sizeof(buf), "bcn_int=%d", (list->beaconInterval[1] << 8) + list->beaconInterval[0]);
+    iwe.u.data.length = strlen(buf);
+    current_ev = iwe_stream_add_point(
+		info,
+		current_ev, end_buf, &iwe, buf);
+
+    /* Ran out of buffer */
+    if (last_ev == current_ev)
+    {
+        return end_buf;
+    }
+
+    last_ev = current_ev;
+
+    if (list->wpaIe[1] != 0)
+    {
+        static const char rsn_leader[] = "rsn_ie=";
+        static const char wpa_leader[] = "wpa_ie=";
+
+        memset(&iwe, 0, sizeof(iwe));
+        iwe.cmd = IWEVCUSTOM;
+        if (list->wpaIe[0] == IEEE80211_ELEMID_RSN)
+            iwe.u.data.length = encode_ie(buf, sizeof(buf),
+                list->wpaIe, list->wpaIe[1]+2,
+                rsn_leader, sizeof(rsn_leader)-1);
+        else
+            iwe.u.data.length = encode_ie(buf, sizeof(buf),
+                list->wpaIe, list->wpaIe[1]+2,
+                wpa_leader, sizeof(wpa_leader)-1);
+
+        if (iwe.u.data.length != 0)
+            current_ev = iwe_stream_add_point(
+		info,
+		current_ev, end_buf, &iwe, buf);
+
+        /* Ran out of buffer */
+        if (last_ev == current_ev)
+        {
+            return end_buf;
+        }
+
+        last_ev = current_ev;
+    }
+    if (list->rsnIe[1] != 0)
+    {
+        static const char rsn_leader[] = "rsn_ie=";
+        memset(&iwe, 0, sizeof(iwe));
+        iwe.cmd = IWEVCUSTOM;
+
+        if (list->rsnIe[0] == IEEE80211_ELEMID_RSN)
+        {
+            iwe.u.data.length = encode_ie(buf, sizeof(buf),
+                list->rsnIe, list->rsnIe[1]+2,
+                rsn_leader, sizeof(rsn_leader)-1);
+            if (iwe.u.data.length != 0)
+                current_ev = iwe_stream_add_point(
+			info,
+			current_ev, end_buf,  &iwe, buf);
+
+            /* Ran out of buffer */
+            if (last_ev == current_ev)
+            {
+                return end_buf;
+            }
+
+            last_ev = current_ev;
+        }
+    }
+#endif
+/* The other data in the scan result are not really
+ * interesting, so for now drop it */
+    return current_ev;
+}
+
+int usbdrvwext_giwname(struct net_device *dev,
+            struct iw_request_info *info,
+            union iwreq_data *wrq, char *extra)
+{
+    //struct usbdrv_private *macp = dev->ml_priv;
+
+    strcpy(wrq->name, "IEEE 802.11-MIMO");
+
+    return 0;
+}
+
+int usbdrvwext_siwfreq(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_freq *freq, char *extra)
+{
+    u32_t FreqKHz;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (freq->e > 1)
+        return -EINVAL;
+
+    if (freq->e == 1)
+    {
+        FreqKHz = (freq->m / 100000);
+
+        if (FreqKHz > 4000000)
+        {
+            if (FreqKHz > 5825000)
+                FreqKHz = 5825000;
+            else if (FreqKHz < 4920000)
+                FreqKHz = 4920000;
+            else if (FreqKHz < 5000000)
+                FreqKHz = (((FreqKHz - 4000000) / 5000) * 5000) + 4000000;
+            else
+                FreqKHz = (((FreqKHz - 5000000) / 5000) * 5000) + 5000000;
+        }
+        else
+        {
+            if (FreqKHz > 2484000)
+                FreqKHz = 2484000;
+            else if (FreqKHz < 2412000)
+                FreqKHz = 2412000;
+            else
+                FreqKHz = (((FreqKHz - 2412000) / 5000) * 5000) + 2412000;
+        }
+
+    }
+    else
+    {
+        FreqKHz = usbdrv_chan2freq(freq->m);
+
+        if (FreqKHz != -1)
+            FreqKHz *= 1000;
+        else
+            FreqKHz = 2412000;
+    }
+
+    //printk("freq->m: %d, freq->e: %d\n", freq->m, freq->e);
+    //printk("FreqKHz: %d\n", FreqKHz);
+
+    if (macp->DeviceOpened == 1)
+    {
+        zfiWlanSetFrequency(dev, FreqKHz, 0); // Immediate
+        //u8_t wpaieLen,wpaie[50];
+        //zfiWlanQueryWpaIe(dev, wpaie, &wpaieLen);
+        zfiWlanDisable(dev, 0);
+        zfiWlanEnable(dev);
+        //if (wpaieLen > 2)
+        //    zfiWlanSetWpaIe(dev, wpaie, wpaieLen);
+    }
+
+    return 0;
+}
+
+int usbdrvwext_giwfreq(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_freq *freq, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    freq->m = zfiWlanQueryFrequency(dev);
+    freq->e = 3;
+
+    return 0;
+}
+
+int usbdrvwext_siwmode(struct net_device *dev,
+            struct iw_request_info *info,
+            union iwreq_data *wrq, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u8_t WlanMode;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    switch(wrq->mode)
+    {
+        case IW_MODE_MASTER:
+            WlanMode = ZM_MODE_AP;
+            break;
+        case IW_MODE_INFRA:
+            WlanMode = ZM_MODE_INFRASTRUCTURE;
+            break;
+        case IW_MODE_ADHOC:
+            WlanMode = ZM_MODE_IBSS;
+            break;
+        default:
+            WlanMode = ZM_MODE_IBSS;
+            break;
+    }
+
+    zfiWlanSetWlanMode(dev,WlanMode);
+    zfiWlanDisable(dev, 1);
+    zfiWlanEnable(dev);
+
+    return 0;
+}
+
+int usbdrvwext_giwmode(struct net_device *dev,
+            struct iw_request_info *info,
+            __u32 *mode, char *extra)
+{
+    unsigned long irqFlag;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    switch(zfiWlanQueryWlanMode(dev))
+    {
+        case ZM_MODE_AP:
+            *mode = IW_MODE_MASTER;
+            break;
+        case ZM_MODE_INFRASTRUCTURE:
+            *mode = IW_MODE_INFRA;
+            break;
+        case ZM_MODE_IBSS:
+            *mode = IW_MODE_ADHOC;
+            break;
+        default:
+            *mode = IW_MODE_ADHOC;
+            break;
+    }
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+
+    return 0;
+}
+
+int usbdrvwext_siwsens(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *sens, char *extra)
+{
+	return 0;
+}
+
+int usbdrvwext_giwsens(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *sens, char *extra)
+{
+	sens->value = 0;
+	sens->fixed = 1;
+
+	return 0;
+}
+
+int usbdrvwext_giwrange(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_point *data, char *extra)
+{
+    struct iw_range *range = (struct iw_range *) extra;
+    int i, val;
+    //int num_band_a;
+    u16_t channels[60];
+    u16_t channel_num;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+#if WIRELESS_EXT > 9
+    range->txpower_capa = IW_TXPOW_DBM;
+// XXX what about min/max_pmp, min/max_pmt, etc.
+#endif
+
+#if WIRELESS_EXT > 10
+    range->we_version_compiled = WIRELESS_EXT;
+    range->we_version_source = 13;
+
+    range->retry_capa = IW_RETRY_LIMIT;
+    range->retry_flags = IW_RETRY_LIMIT;
+    range->min_retry = 0;
+    range->max_retry = 255;
+#endif                                        /* WIRELESS_EXT > 10 */
+
+    channel_num = zfiWlanQueryAllowChannels(dev, channels);
+
+    /* Gurantee reported channel numbers is less or equal to IW_MAX_FREQUENCIES */
+    if (channel_num > IW_MAX_FREQUENCIES)
+        channel_num = IW_MAX_FREQUENCIES;
+
+    val = 0;
+
+    for (i = 0; i < channel_num; i++)
+    {
+        range->freq[val].i = usbdrv_freq2chan(channels[i]);
+        range->freq[val].m = channels[i];
+        range->freq[val].e = 6;
+        val++;
+    }
+
+    range->num_channels = channel_num;
+    range->num_frequency = channel_num;
+
+#if 0
+    range->num_channels = 14; // Only 2.4G
+
+/* XXX need to filter against the regulatory domain &| active set */
+    val = 0;
+    for (i = 1; i <= 14; i++) // B,G Bands
+    {
+        range->freq[val].i = i;
+        if (i == 14)
+              range->freq[val].m = 2484000;
+        else
+              range->freq[val].m = (2412+(i-1)*5)*1000;
+        range->freq[val].e = 3;
+        val++;
+    }
+
+    num_band_a = (IW_MAX_FREQUENCIES - val);
+
+    for (i = 0; i < num_band_a; i++) // A Bands
+    {
+        range->freq[val].i = channel_frequency_11A[2 * i];
+        range->freq[val].m = channel_frequency_11A[2 * i + 1] * 1000;
+        range->freq[val].e = 3;
+        val++;
+    }
+    // MIMO Rate Not Defined Now
+    //For 802.11a, there are too more frequency. We can't return them all
+    range->num_frequency = val;
+#endif
+
+/* Max of /proc/net/wireless */
+    range->max_qual.qual = 100; //??                  //92;
+    range->max_qual.level = 154; //??
+    range->max_qual.noise = 154; //??
+    range->sensitivity = 3; //??
+
+// XXX these need to be nsd-specific!
+    range->min_rts = 0;
+    range->max_rts = 2347;
+    range->min_frag = 256;
+    range->max_frag = 2346;
+    range->max_encoding_tokens = 4/*NUM_WEPKEYS*/; //??
+    range->num_encoding_sizes = 2; //??
+
+    range->encoding_size[0] = 5; //??               //WEP Key Encoding Size
+    range->encoding_size[1] = 13;//??
+
+// XXX what about num_bitrates/throughput?
+    range->num_bitrates = 0; //??
+
+/* estimated max throughput */
+// XXX need to cap it if we're running at ~2Mbps..
+
+    range->throughput = 300000000;
+
+    return 0;
+}
+
+int usbdrvwext_siwap(struct net_device *dev, struct iw_request_info *info,
+        struct sockaddr *MacAddr, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (zfiWlanQueryWlanMode(dev) == ZM_MODE_AP) // AP Mode
+        zfiWlanSetMacAddress(dev,(u16_t *)&MacAddr->sa_data[0]);
+    else                                 //STA Mode
+        zfiWlanSetBssid(dev,&MacAddr->sa_data[0]);
+
+    if (macp->DeviceOpened == 1)
+    {
+        //u8_t wpaieLen,wpaie[80];
+        //zfiWlanQueryWpaIe(dev, wpaie, &wpaieLen);
+        zfiWlanDisable(dev, 0);
+        zfiWlanEnable(dev);
+        //if (wpaieLen > 2)
+        //    zfiWlanSetWpaIe(dev, wpaie, wpaieLen);
+    }
+
+    return 0;
+}
+
+int usbdrvwext_giwap(struct net_device *dev,
+            struct iw_request_info *info,
+            struct sockaddr *MacAddr, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    if (zfiWlanQueryWlanMode(dev) == ZM_MODE_AP) // AP Mode
+        zfiWlanQueryMacAddress(dev, &MacAddr->sa_data[0]);
+    else                                 //STA Mode
+    {
+        if (macp->adapterState == ZM_STATUS_MEDIA_CONNECT)
+        {
+            zfiWlanQueryBssid(dev, &MacAddr->sa_data[0]);
+        }
+        else
+        {
+            u8_t zero_addr[6] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+            memcpy(&MacAddr->sa_data[0], zero_addr, sizeof(zero_addr));
+        }
+    }
+
+    return 0;
+}
+
+int usbdrvwext_iwaplist(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+    //Don't know how to do yet--CWYang(+)
+    return 0;
+
+}
+
+int usbdrvwext_siwscan(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *data, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    printk("CWY - usbdrvwext_siwscan\n");
+
+    zfiWlanScan(dev);
+
+    return 0;
+}
+
+int usbdrvwext_giwscan(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_point *data, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    char *current_ev = extra;
+    char *end_buf;
+    int i;
+    //struct zsBssList BssList;
+    struct zsBssListV1 *pBssList = kmalloc(sizeof(struct zsBssListV1), GFP_KERNEL);
+    //BssList = wd->sta.pBssList;
+    //zmw_get_wlan_dev(dev);
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    if (data->length == 0)
+    {
+       end_buf = extra + IW_SCAN_MAX_DATA;
+    }
+    else
+    {
+        end_buf = extra + data->length;
+    }
+
+    printk("giwscan - Report Scan Results\n");
+    //printk("giwscan - BssList Sreucture Len : %d\n", sizeof(BssList));
+    //printk("giwscan - BssList Count : %d\n", wd->sta.pBssList->bssCount);
+    //printk("giwscan - UpdateBssList Count : %d\n", wd->sta.pUpdateBssList->bssCount);
+    zfiWlanQueryBssListV1(dev, pBssList);
+    //zfiWlanQueryBssList(dev, &BssList);
+
+/* Read and parse all entries */
+    printk("giwscan - pBssList->bssCount : %d\n", pBssList->bssCount);
+    //printk("giwscan - BssList.bssCount : %d\n", BssList.bssCount);
+
+    for (i = 0; i < pBssList->bssCount; i++)
+    {
+/* Translate to WE format this entry */
+        //current_ev = usbdrv_translate_scan(dev, info, current_ev,
+        //    extra + IW_SCAN_MAX_DATA, &pBssList->bssInfo[i]);
+        current_ev = usbdrv_translate_scan(dev, info, current_ev,
+            end_buf, &pBssList->bssInfo[i]);
+
+#if WIRELESS_EXT > 16
+        if (current_ev == end_buf)
+        {
+            kfree(pBssList);
+            data->length = current_ev - extra;
+            return -E2BIG;
+        }
+#endif
+    }
+
+/* Length of data */
+    data->length = (current_ev - extra);
+    data->flags = 0;                              /* todo */
+
+    kfree(pBssList);
+
+    return 0;
+}
+
+int usbdrvwext_siwessid(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_point *essid, char *extra)
+{
+    char EssidBuf[IW_ESSID_MAX_SIZE+1];
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (essid->flags == 1)
+    {
+        if (essid->length > (IW_ESSID_MAX_SIZE+1))
+            return -E2BIG;
+
+        if (copy_from_user(&EssidBuf, essid->pointer, essid->length))
+            return -EFAULT;
+
+        EssidBuf[essid->length] = '\0';
+        //printk("siwessid - Set Essid : %s\n",EssidBuf);
+        //printk("siwessid - Essid Len : %d\n",essid->length);
+        //printk("siwessid - Essid Flag : %x\n",essid->flags);
+        if (macp->DeviceOpened == 1)
+        {
+            zfiWlanSetSSID(dev, EssidBuf, strlen(EssidBuf));
+            zfiWlanSetFrequency(dev, zfiWlanQueryFrequency(dev), FALSE);
+            zfiWlanSetEncryMode(dev, zfiWlanQueryEncryMode(dev));
+            //u8_t wpaieLen,wpaie[50];
+            //zfiWlanQueryWpaIe(dev, wpaie, &wpaieLen);
+            zfiWlanDisable(dev, 0);
+            zfiWlanEnable(dev);
+            //if (wpaieLen > 2)
+            //    zfiWlanSetWpaIe(dev, wpaie, wpaieLen);
+        }
+    }
+
+    return 0;
+}
+
+int usbdrvwext_giwessid(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_point *essid, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u8_t EssidLen;
+    char EssidBuf[IW_ESSID_MAX_SIZE+1];
+    int ssid_len;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    zfiWlanQuerySSID(dev, &EssidBuf[0], &EssidLen);
+
+    /* Convert type from unsigned char to char */
+    ssid_len = (int)EssidLen;
+
+    /* Make sure the essid length is not greater than IW_ESSID_MAX_SIZE */
+    if (ssid_len > IW_ESSID_MAX_SIZE)
+        ssid_len = IW_ESSID_MAX_SIZE;
+
+    EssidBuf[ssid_len] = '\0';
+
+    essid->flags = 1;
+    essid->length = strlen(EssidBuf);
+
+    memcpy(extra, EssidBuf, essid->length);
+    // wireless.c in Kernel would handle copy_to_user -- line 679
+    /*if (essid->pointer)
+    {
+        if ( copy_to_user(essid->pointer, EssidBuf, essid->length) )
+        {
+            printk("giwessid - copy_to_user Fail\n");
+            return -EFAULT;
+        }
+    }*/
+
+    return 0;
+}
+
+int usbdrvwext_siwnickn(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *nickname)
+{
+    //Exist but junk--CWYang(+)
+	return 0;
+}
+
+int usbdrvwext_giwnickn(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *nickname)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u8_t EssidLen;
+    char EssidBuf[IW_ESSID_MAX_SIZE+1];
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    zfiWlanQuerySSID(dev, &EssidBuf[0], &EssidLen);
+    EssidBuf[EssidLen] = 0;
+
+    data->flags = 1;
+    data->length = strlen(EssidBuf);
+
+    memcpy(nickname, EssidBuf, data->length);
+
+	return 0;
+}
+
+int usbdrvwext_siwrate(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *frq, char *extra)
+{
+	struct usbdrv_private *macp = dev->ml_priv;
+    //Array to Define Rate Number that Send to Driver
+    u16_t zcIndextoRateBG[16] = {1000, 2000, 5500, 11000, 0, 0, 0, 0, 48000,
+                               24000, 12000, 6000, 54000, 36000, 18000, 9000};
+    u16_t zcRateToMCS[] = {0xff, 0, 1, 2, 3, 0xb, 0xf, 0xa, 0xe, 0x9, 0xd,
+                           0x8, 0xc};
+    u8_t i,RateIndex = 4;
+    u16_t RateKbps;
+
+    //printk("frq->disabled : 0x%x\n",frq->disabled);
+    //printk("frq->value : 0x%x\n",frq->value);
+
+    RateKbps = frq->value / 1000;
+    //printk("RateKbps : %d\n", RateKbps);
+    for (i = 0; i < 16; i++)
+    {
+        if (RateKbps == zcIndextoRateBG[i])
+            RateIndex = i;
+    }
+    if (zcIndextoRateBG[RateIndex] == 0)
+        RateIndex = 0xff;
+    //printk("RateIndex : %x\n", RateIndex);
+    for (i = 0; i < 13; i++)
+        if (RateIndex == zcRateToMCS[i])
+            break;
+    //printk("Index : %x\n", i);
+    if (RateKbps == 65000)
+    {
+        RateIndex = 20;
+        printk("RateIndex : %d\n", RateIndex);
+    }
+    if (macp->DeviceOpened == 1)
+    {
+        zfiWlanSetTxRate(dev, i);
+        //zfiWlanDisable(dev);
+        //zfiWlanEnable(dev);
+    }
+
+    return 0;
+}
+
+int usbdrvwext_giwrate(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *frq, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    frq->fixed = 0;
+    frq->disabled = 0;
+    frq->value = zfiWlanQueryRxRate(dev) * 1000;
+
+    return 0;
+}
+
+int usbdrvwext_siwrts(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *rts, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    int val = rts->value;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    if (rts->disabled)
+        val = 2347;
+
+    if ((val < 0) || (val > 2347))
+        return -EINVAL;
+
+    zfiWlanSetRtsThreshold(dev,val);
+
+    return 0;
+}
+
+int usbdrvwext_giwrts(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *rts, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    rts->value = zfiWlanQueryRtsThreshold(dev);
+    rts->disabled = (rts->value >= 2347);
+    rts->fixed = 1;
+
+    return 0;
+
+}
+
+int usbdrvwext_siwfrag(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *frag, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t fragThreshold;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    if (frag->disabled)
+        fragThreshold = 0;
+    else
+        fragThreshold = frag->value;
+
+    zfiWlanSetFragThreshold(dev,fragThreshold);
+
+    return 0;
+}
+
+int usbdrvwext_giwfrag(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *frag, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16 val;
+    unsigned long irqFlag;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    val = zfiWlanQueryFragThreshold(dev);
+
+    frag->value = val;
+
+    frag->disabled = (val >= 2346);
+    frag->fixed = 1;
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+
+    return 0;
+}
+
+int usbdrvwext_siwtxpow(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *rrq, char *extra)
+{
+    //Not support yet--CWYng(+)
+	return 0;
+}
+
+int usbdrvwext_giwtxpow(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *rrq, char *extra)
+{
+    //Not support yet--CWYng(+)
+	return 0;
+}
+
+int usbdrvwext_siwretry(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *rrq, char *extra)
+{
+    //Do nothing--CWYang(+)
+	return 0;
+}
+
+int usbdrvwext_giwretry(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *rrq, char *extra)
+{
+    //Do nothing--CWYang(+)
+	return 0;
+}
+
+int usbdrvwext_siwencode(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_point *erq, char *key)
+{
+    struct zsKeyInfo keyInfo;
+    int i, WepState = ZM_ENCRYPTION_WEP_DISABLED;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if ((erq->flags & IW_ENCODE_DISABLED) == 0)
+    {
+        keyInfo.key = key;
+        keyInfo.keyLength = erq->length;
+        keyInfo.keyIndex = (erq->flags & IW_ENCODE_INDEX) - 1;
+        if (keyInfo.keyIndex >= 4)
+            keyInfo.keyIndex = 0;
+        keyInfo.flag = ZM_KEY_FLAG_DEFAULT_KEY;
+
+        zfiWlanSetKey(dev, keyInfo);
+        WepState = ZM_ENCRYPTION_WEP_ENABLED;
+    }
+    else
+    {
+        for (i = 1; i < 4; i++)
+            zfiWlanRemoveKey(dev, 0, i);
+        WepState = ZM_ENCRYPTION_WEP_DISABLED;
+        //zfiWlanSetEncryMode(dev, ZM_NO_WEP);
+    }
+
+    if (macp->DeviceOpened == 1)
+    {
+        zfiWlanSetWepStatus(dev, WepState);
+        zfiWlanSetFrequency(dev, zfiWlanQueryFrequency(dev), FALSE);
+        //zfiWlanSetEncryMode(dev, zfiWlanQueryEncryMode(dev));
+        //u8_t wpaieLen,wpaie[50];
+        //zfiWlanQueryWpaIe(dev, wpaie, &wpaieLen);
+        zfiWlanDisable(dev, 0);
+        zfiWlanEnable(dev);
+        //if (wpaieLen > 2)
+        //    zfiWlanSetWpaIe(dev, wpaie, wpaieLen);
+    }
+
+    return 0;
+}
+
+int usbdrvwext_giwencode(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_point *erq, char *key)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u8_t EncryptionMode;
+    u8_t keyLen = 0;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    EncryptionMode = zfiWlanQueryEncryMode(dev);
+
+    if (EncryptionMode)
+    {
+        erq->flags = IW_ENCODE_ENABLED;
+    }
+    else
+    {
+        erq->flags = IW_ENCODE_DISABLED;
+    }
+
+/* We can't return the key, so set the proper flag and return zero */
+    erq->flags |= IW_ENCODE_NOKEY;
+    memset(key, 0, 16);
+
+/* Copy the key to the user buffer */
+    switch(EncryptionMode)
+    {
+        case ZM_WEP64:
+            keyLen = 5;
+            break;
+        case ZM_WEP128:
+            keyLen = 13;
+            break;
+        case ZM_WEP256:
+            keyLen = 29;
+            break;
+        case ZM_AES:
+            keyLen = 16;
+            break;
+        case ZM_TKIP:
+            keyLen = 32;
+            break;
+#ifdef ZM_ENABLE_CENC
+        case ZM_CENC:
+            keyLen = 32;
+            break;
+#endif //ZM_ENABLE_CENC
+        case ZM_NO_WEP:
+            keyLen = 0;
+            break;
+        default :
+            keyLen = 0;
+            printk("Unknown EncryMode\n");
+            break;
+
+    }
+    erq->length = keyLen;
+
+    return 0;
+}
+
+int usbdrvwext_siwpower(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *frq, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u8_t PSMode;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    if (frq->disabled)
+        PSMode = ZM_STA_PS_NONE;
+    else
+        PSMode = ZM_STA_PS_MAX;
+
+    zfiWlanSetPowerSaveMode(dev,PSMode);
+
+    return 0;
+}
+
+int usbdrvwext_giwpower(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *frq, char *extra)
+{
+    unsigned long irqFlag;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    if (zfiWlanQueryPowerSaveMode(dev) == ZM_STA_PS_NONE)
+        frq->disabled = 1;
+    else
+        frq->disabled = 0;
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+
+    return 0;
+}
+
+//int usbdrvwext_setparam(struct net_device *dev, struct iw_request_info *info,
+//		   	 void *w, char *extra)
+//{
+//	struct ieee80211vap *vap = dev->ml_priv;
+//	struct ieee80211com *ic = vap->iv_ic;
+//	struct ieee80211_rsnparms *rsn = &vap->iv_bss->ni_rsn;
+//	int *i = (int *) extra;
+//	int param = i[0];		/* parameter id is 1st */
+//	int value = i[1];		/* NB: most values are TYPE_INT */
+//	int retv = 0;
+//	int j, caps;
+//	const struct ieee80211_authenticator *auth;
+//	const struct ieee80211_aclator *acl;
+//
+//	switch (param) {
+//	case IEEE80211_PARAM_AUTHMODE:
+//		switch (value) {
+//		case IEEE80211_AUTH_WPA:	/* WPA */
+//		case IEEE80211_AUTH_8021X:	/* 802.1x */
+//		case IEEE80211_AUTH_OPEN:	/* open */
+//		case IEEE80211_AUTH_SHARED:	/* shared-key */
+//		case IEEE80211_AUTH_AUTO:	/* auto */
+//			auth = ieee80211_authenticator_get(value);
+//			if (auth == NULL)
+//				return -EINVAL;
+//			break;
+//		default:
+//			return -EINVAL;
+//		}
+//		switch (value) {
+//		case IEEE80211_AUTH_WPA:	/* WPA w/ 802.1x */
+//			vap->iv_flags |= IEEE80211_F_PRIVACY;
+//			value = IEEE80211_AUTH_8021X;
+//			break;
+//		case IEEE80211_AUTH_OPEN:	/* open */
+//			vap->iv_flags &= ~(IEEE80211_F_WPA|IEEE80211_F_PRIVACY);
+//			break;
+//		case IEEE80211_AUTH_SHARED:	/* shared-key */
+//		case IEEE80211_AUTH_AUTO:	/* auto */
+//		case IEEE80211_AUTH_8021X:	/* 802.1x */
+//			vap->iv_flags &= ~IEEE80211_F_WPA;
+//			/* both require a key so mark the PRIVACY capability */
+//			vap->iv_flags |= IEEE80211_F_PRIVACY;
+//			break;
+//		}
+//		/* NB: authenticator attach/detach happens on state change */
+//		vap->iv_bss->ni_authmode = value;
+//		/* XXX mixed/mode/usage? */
+//		vap->iv_auth = auth;
+//		retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_PROTMODE:
+//		if (value > IEEE80211_PROT_RTSCTS)
+//			return -EINVAL;
+//		ic->ic_protmode = value;
+//		/* NB: if not operating in 11g this can wait */
+//		if (ic->ic_bsschan != IEEE80211_CHAN_ANYC &&
+//		    IEEE80211_IS_CHAN_ANYG(ic->ic_bsschan))
+//			retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_MCASTCIPHER:
+//		if ((vap->iv_caps & cipher2cap(value)) == 0 &&
+//		    !ieee80211_crypto_available(value))
+//			return -EINVAL;
+//		rsn->rsn_mcastcipher = value;
+//		if (vap->iv_flags & IEEE80211_F_WPA)
+//			retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_MCASTKEYLEN:
+//		if (!(0 < value && value < IEEE80211_KEYBUF_SIZE))
+//			return -EINVAL;
+//		/* XXX no way to verify driver capability */
+//		rsn->rsn_mcastkeylen = value;
+//		if (vap->iv_flags & IEEE80211_F_WPA)
+//			retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_UCASTCIPHERS:
+//		/*
+//		 * Convert cipher set to equivalent capabilities.
+//		 * NB: this logic intentionally ignores unknown and
+//		 * unsupported ciphers so folks can specify 0xff or
+//		 * similar and get all available ciphers.
+//		 */
+//		caps = 0;
+//		for (j = 1; j < 32; j++)	/* NB: skip WEP */
+//			if ((value & (1<<j)) &&
+//			    ((vap->iv_caps & cipher2cap(j)) ||
+//			     ieee80211_crypto_available(j)))
+//				caps |= 1<<j;
+//		if (caps == 0)			/* nothing available */
+//			return -EINVAL;
+//		/* XXX verify ciphers ok for unicast use? */
+//		/* XXX disallow if running as it'll have no effect */
+//		rsn->rsn_ucastcipherset = caps;
+//		if (vap->iv_flags & IEEE80211_F_WPA)
+//			retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_UCASTCIPHER:
+//		if ((rsn->rsn_ucastcipherset & cipher2cap(value)) == 0)
+//			return -EINVAL;
+//		rsn->rsn_ucastcipher = value;
+//		break;
+//	case IEEE80211_PARAM_UCASTKEYLEN:
+//		if (!(0 < value && value < IEEE80211_KEYBUF_SIZE))
+//			return -EINVAL;
+//		/* XXX no way to verify driver capability */
+//		rsn->rsn_ucastkeylen = value;
+//		break;
+//	case IEEE80211_PARAM_KEYMGTALGS:
+//		/* XXX check */
+//		rsn->rsn_keymgmtset = value;
+//		if (vap->iv_flags & IEEE80211_F_WPA)
+//			retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_RSNCAPS:
+//		/* XXX check */
+//		rsn->rsn_caps = value;
+//		if (vap->iv_flags & IEEE80211_F_WPA)
+//			retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_WPA:
+//		if (value > 3)
+//			return -EINVAL;
+//		/* XXX verify ciphers available */
+//		vap->iv_flags &= ~IEEE80211_F_WPA;
+//		switch (value) {
+//		case 1:
+//			vap->iv_flags |= IEEE80211_F_WPA1;
+//			break;
+//		case 2:
+//			vap->iv_flags |= IEEE80211_F_WPA2;
+//			break;
+//		case 3:
+//			vap->iv_flags |= IEEE80211_F_WPA1 | IEEE80211_F_WPA2;
+//			break;
+//		}
+//		retv = ENETRESET;		/* XXX? */
+//		break;
+//	case IEEE80211_PARAM_ROAMING:
+//		if (!(IEEE80211_ROAMING_DEVICE <= value &&
+//		    value <= IEEE80211_ROAMING_MANUAL))
+//			return -EINVAL;
+//		ic->ic_roaming = value;
+//		break;
+//	case IEEE80211_PARAM_PRIVACY:
+//		if (value) {
+//			/* XXX check for key state? */
+//			vap->iv_flags |= IEEE80211_F_PRIVACY;
+//		} else
+//			vap->iv_flags &= ~IEEE80211_F_PRIVACY;
+//		break;
+//	case IEEE80211_PARAM_DROPUNENCRYPTED:
+//		if (value)
+//			vap->iv_flags |= IEEE80211_F_DROPUNENC;
+//		else
+//			vap->iv_flags &= ~IEEE80211_F_DROPUNENC;
+//		break;
+//	case IEEE80211_PARAM_COUNTERMEASURES:
+//		if (value) {
+//			if ((vap->iv_flags & IEEE80211_F_WPA) == 0)
+//				return -EINVAL;
+//			vap->iv_flags |= IEEE80211_F_COUNTERM;
+//		} else
+//			vap->iv_flags &= ~IEEE80211_F_COUNTERM;
+//		break;
+//	case IEEE80211_PARAM_DRIVER_CAPS:
+//		vap->iv_caps = value;		/* NB: for testing */
+//		break;
+//	case IEEE80211_PARAM_MACCMD:
+//		acl = vap->iv_acl;
+//		switch (value) {
+//		case IEEE80211_MACCMD_POLICY_OPEN:
+//		case IEEE80211_MACCMD_POLICY_ALLOW:
+//		case IEEE80211_MACCMD_POLICY_DENY:
+//			if (acl == NULL) {
+//				acl = ieee80211_aclator_get("mac");
+//				if (acl == NULL || !acl->iac_attach(vap))
+//					return -EINVAL;
+//				vap->iv_acl = acl;
+//			}
+//			acl->iac_setpolicy(vap, value);
+//			break;
+//		case IEEE80211_MACCMD_FLUSH:
+//			if (acl != NULL)
+//				acl->iac_flush(vap);
+//			/* NB: silently ignore when not in use */
+//			break;
+//		case IEEE80211_MACCMD_DETACH:
+//			if (acl != NULL) {
+//				vap->iv_acl = NULL;
+//				acl->iac_detach(vap);
+//			}
+//			break;
+//		}
+//		break;
+//	case IEEE80211_PARAM_WMM:
+//		if (ic->ic_caps & IEEE80211_C_WME){
+//			if (value) {
+//				vap->iv_flags |= IEEE80211_F_WME;
+//				vap->iv_ic->ic_flags |= IEEE80211_F_WME; /* XXX needed by ic_reset */
+//			}
+//			else {
+//				vap->iv_flags &= ~IEEE80211_F_WME;
+//				vap->iv_ic->ic_flags &= ~IEEE80211_F_WME; /* XXX needed by ic_reset */
+//			}
+//			retv = ENETRESET;	/* Renegotiate for capabilities */
+//		}
+//		break;
+//	case IEEE80211_PARAM_HIDESSID:
+//		if (value)
+//			vap->iv_flags |= IEEE80211_F_HIDESSID;
+//		else
+//			vap->iv_flags &= ~IEEE80211_F_HIDESSID;
+//		retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_APBRIDGE:
+//		if (value == 0)
+//			vap->iv_flags |= IEEE80211_F_NOBRIDGE;
+//		else
+//			vap->iv_flags &= ~IEEE80211_F_NOBRIDGE;
+//		break;
+//	case IEEE80211_PARAM_INACT:
+//		vap->iv_inact_run = value / IEEE80211_INACT_WAIT;
+//		break;
+//	case IEEE80211_PARAM_INACT_AUTH:
+//		vap->iv_inact_auth = value / IEEE80211_INACT_WAIT;
+//		break;
+//	case IEEE80211_PARAM_INACT_INIT:
+//		vap->iv_inact_init = value / IEEE80211_INACT_WAIT;
+//		break;
+//	case IEEE80211_PARAM_ABOLT:
+//		caps = 0;
+//		/*
+//		 * Map abolt settings to capability bits;
+//		 * this also strips unknown/unwanted bits.
+//		 */
+//		if (value & IEEE80211_ABOLT_TURBO_PRIME)
+//			caps |= IEEE80211_ATHC_TURBOP;
+//		if (value & IEEE80211_ABOLT_COMPRESSION)
+//			caps |= IEEE80211_ATHC_COMP;
+//		if (value & IEEE80211_ABOLT_FAST_FRAME)
+//			caps |= IEEE80211_ATHC_FF;
+//		if (value & IEEE80211_ABOLT_XR)
+//			caps |= IEEE80211_ATHC_XR;
+//		if (value & IEEE80211_ABOLT_AR)
+//			caps |= IEEE80211_ATHC_AR;
+//		if (value & IEEE80211_ABOLT_BURST)
+//			caps |= IEEE80211_ATHC_BURST;
+//        if (value & IEEE80211_ABOLT_WME_ELE)
+//            caps |= IEEE80211_ATHC_WME;
+//		/* verify requested capabilities are supported */
+//		if ((caps & ic->ic_ath_cap) != caps)
+//			return -EINVAL;
+//		if (vap->iv_ath_cap != caps) {
+//			if ((vap->iv_ath_cap ^ caps) & IEEE80211_ATHC_TURBOP) {
+//				if (ieee80211_set_turbo(dev,  caps & IEEE80211_ATHC_TURBOP))
+//					return -EINVAL;
+//				ieee80211_scan_flush(ic);
+//			}
+//			vap->iv_ath_cap = caps;
+//			ic->ic_athcapsetup(vap->iv_ic, vap->iv_ath_cap);
+//			retv = ENETRESET;
+//		}
+//		break;
+//	case IEEE80211_PARAM_DTIM_PERIOD:
+//		if (vap->iv_opmode != IEEE80211_M_HOSTAP &&
+//		    vap->iv_opmode != IEEE80211_M_IBSS)
+//			return -EINVAL;
+//		if (IEEE80211_DTIM_MIN <= value &&
+//		    value <= IEEE80211_DTIM_MAX) {
+//			vap->iv_dtim_period = value;
+//			retv = ENETRESET;		/* requires restart */
+//		} else
+//			retv = EINVAL;
+//		break;
+//	case IEEE80211_PARAM_BEACON_INTERVAL:
+//		if (vap->iv_opmode != IEEE80211_M_HOSTAP &&
+//		    vap->iv_opmode != IEEE80211_M_IBSS)
+//			return -EINVAL;
+//		if (IEEE80211_BINTVAL_MIN <= value &&
+//		    value <= IEEE80211_BINTVAL_MAX) {
+//			ic->ic_lintval = value;		/* XXX multi-bss */
+//			retv = ENETRESET;		/* requires restart */
+//		} else
+//			retv = EINVAL;
+//		break;
+//	case IEEE80211_PARAM_DOTH:
+//		if (value) {
+//			ic->ic_flags |= IEEE80211_F_DOTH;
+//		}
+//		else
+//			ic->ic_flags &= ~IEEE80211_F_DOTH;
+//		retv = ENETRESET;	/* XXX: need something this drastic? */
+//		break;
+//	case IEEE80211_PARAM_PWRTARGET:
+//		ic->ic_curchanmaxpwr = value;
+//		break;
+//	case IEEE80211_PARAM_GENREASSOC:
+//		IEEE80211_SEND_MGMT(vap->iv_bss, IEEE80211_FC0_SUBTYPE_REASSOC_REQ, 0);
+//		break;
+//	case IEEE80211_PARAM_COMPRESSION:
+//		retv = ieee80211_setathcap(vap, IEEE80211_ATHC_COMP, value);
+//		break;
+//    case IEEE80211_PARAM_WMM_AGGRMODE:
+//        retv = ieee80211_setathcap(vap, IEEE80211_ATHC_WME, value);
+//        break;
+//	case IEEE80211_PARAM_FF:
+//		retv = ieee80211_setathcap(vap, IEEE80211_ATHC_FF, value);
+//		break;
+//	case IEEE80211_PARAM_TURBO:
+//		retv = ieee80211_setathcap(vap, IEEE80211_ATHC_TURBOP, value);
+//		if (retv == ENETRESET) {
+//			if(ieee80211_set_turbo(dev,value))
+//					return -EINVAL;
+//			ieee80211_scan_flush(ic);
+//		}
+//		break;
+//	case IEEE80211_PARAM_XR:
+//		retv = ieee80211_setathcap(vap, IEEE80211_ATHC_XR, value);
+//		break;
+//	case IEEE80211_PARAM_BURST:
+//		retv = ieee80211_setathcap(vap, IEEE80211_ATHC_BURST, value);
+//		break;
+//	case IEEE80211_PARAM_AR:
+//		retv = ieee80211_setathcap(vap, IEEE80211_ATHC_AR, value);
+//		break;
+//	case IEEE80211_PARAM_PUREG:
+//		if (value)
+//			vap->iv_flags |= IEEE80211_F_PUREG;
+//		else
+//			vap->iv_flags &= ~IEEE80211_F_PUREG;
+//		/* NB: reset only if we're operating on an 11g channel */
+//		if (ic->ic_bsschan != IEEE80211_CHAN_ANYC &&
+//		    IEEE80211_IS_CHAN_ANYG(ic->ic_bsschan))
+//			retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_WDS:
+//		if (value)
+//			vap->iv_flags_ext |= IEEE80211_FEXT_WDS;
+//		else
+//			vap->iv_flags_ext &= ~IEEE80211_FEXT_WDS;
+//		break;
+//	case IEEE80211_PARAM_BGSCAN:
+//		if (value) {
+//			if ((vap->iv_caps & IEEE80211_C_BGSCAN) == 0)
+//				return -EINVAL;
+//			vap->iv_flags |= IEEE80211_F_BGSCAN;
+//		} else {
+//			/* XXX racey? */
+//			vap->iv_flags &= ~IEEE80211_F_BGSCAN;
+//			ieee80211_cancel_scan(vap);	/* anything current */
+//		}
+//		break;
+//	case IEEE80211_PARAM_BGSCAN_IDLE:
+//		if (value >= IEEE80211_BGSCAN_IDLE_MIN)
+//			vap->iv_bgscanidle = value*HZ/1000;
+//		else
+//			retv = EINVAL;
+//		break;
+//	case IEEE80211_PARAM_BGSCAN_INTERVAL:
+//		if (value >= IEEE80211_BGSCAN_INTVAL_MIN)
+//			vap->iv_bgscanintvl = value*HZ;
+//		else
+//			retv = EINVAL;
+//		break;
+//	case IEEE80211_PARAM_MCAST_RATE:
+//		/* units are in KILObits per second */
+//		if (value >= 256 && value <= 54000)
+//			vap->iv_mcast_rate = value;
+//		else
+//			retv = EINVAL;
+//		break;
+//	case IEEE80211_PARAM_COVERAGE_CLASS:
+//		if (value >= 0 && value <= IEEE80211_COVERAGE_CLASS_MAX) {
+//			ic->ic_coverageclass = value;
+//			if (IS_UP_AUTO(vap))
+//				ieee80211_new_state(vap, IEEE80211_S_SCAN, 0);
+//			retv = 0;
+//		}
+//		else
+//			retv = EINVAL;
+//		break;
+//	case IEEE80211_PARAM_COUNTRY_IE:
+//		if (value)
+//			ic->ic_flags_ext |= IEEE80211_FEXT_COUNTRYIE;
+//		else
+//			ic->ic_flags_ext &= ~IEEE80211_FEXT_COUNTRYIE;
+//		retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_REGCLASS:
+//		if (value)
+//			ic->ic_flags_ext |= IEEE80211_FEXT_REGCLASS;
+//		else
+//			ic->ic_flags_ext &= ~IEEE80211_FEXT_REGCLASS;
+//		retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_SCANVALID:
+//		vap->iv_scanvalid = value*HZ;
+//		break;
+//	case IEEE80211_PARAM_ROAM_RSSI_11A:
+//		vap->iv_roam.rssi11a = value;
+//		break;
+//	case IEEE80211_PARAM_ROAM_RSSI_11B:
+//		vap->iv_roam.rssi11bOnly = value;
+//		break;
+//	case IEEE80211_PARAM_ROAM_RSSI_11G:
+//		vap->iv_roam.rssi11b = value;
+//		break;
+//	case IEEE80211_PARAM_ROAM_RATE_11A:
+//		vap->iv_roam.rate11a = value;
+//		break;
+//	case IEEE80211_PARAM_ROAM_RATE_11B:
+//		vap->iv_roam.rate11bOnly = value;
+//		break;
+//	case IEEE80211_PARAM_ROAM_RATE_11G:
+//		vap->iv_roam.rate11b = value;
+//		break;
+//	case IEEE80211_PARAM_UAPSDINFO:
+//		if (vap->iv_opmode == IEEE80211_M_HOSTAP) {
+//			if (ic->ic_caps & IEEE80211_C_UAPSD) {
+//				if (value)
+//					IEEE80211_VAP_UAPSD_ENABLE(vap);
+//				else
+//					IEEE80211_VAP_UAPSD_DISABLE(vap);
+//				retv = ENETRESET;
+//			}
+//		}
+//		else if (vap->iv_opmode == IEEE80211_M_STA) {
+//			vap->iv_uapsdinfo = value;
+//			IEEE80211_VAP_UAPSD_ENABLE(vap);
+//			retv = ENETRESET;
+//		}
+//		break;
+//	case IEEE80211_PARAM_SLEEP:
+//		/* XXX: Forced sleep for testing. Does not actually place the
+//		 *      HW in sleep mode yet. this only makes sense for STAs.
+//		 */
+//		if (value) {
+//			/* goto sleep */
+//			IEEE80211_VAP_GOTOSLEEP(vap);
+//		}
+//		else {
+//			/* wakeup */
+//			IEEE80211_VAP_WAKEUP(vap);
+//		}
+//		ieee80211_send_nulldata(ieee80211_ref_node(vap->iv_bss));
+//		break;
+//	case IEEE80211_PARAM_QOSNULL:
+//		/* Force a QoS Null for testing. */
+//		ieee80211_send_qosnulldata(vap->iv_bss, value);
+//		break;
+//	case IEEE80211_PARAM_PSPOLL:
+//		/* Force a PS-POLL for testing. */
+//		ieee80211_send_pspoll(vap->iv_bss);
+//		break;
+//	case IEEE80211_PARAM_EOSPDROP:
+//		if (vap->iv_opmode == IEEE80211_M_HOSTAP) {
+//			if (value) IEEE80211_VAP_EOSPDROP_ENABLE(vap);
+//			else IEEE80211_VAP_EOSPDROP_DISABLE(vap);
+//		}
+//		break;
+//	case IEEE80211_PARAM_MARKDFS:
+//		if (value)
+//			ic->ic_flags_ext |= IEEE80211_FEXT_MARKDFS;
+//		else
+//			ic->ic_flags_ext &= ~IEEE80211_FEXT_MARKDFS;
+//		break;
+//	case IEEE80211_PARAM_CHANBW:
+//		switch (value) {
+//			case 0:
+//				ic->ic_chanbwflag = 0;
+//				break;
+//			case 1:
+//				ic->ic_chanbwflag = IEEE80211_CHAN_HALF;
+//				break;
+//			case 2:
+//				ic->ic_chanbwflag = IEEE80211_CHAN_QUARTER;
+//				break;
+//			default:
+//				retv = EINVAL;
+//				break;
+//		}
+//		break;
+//	case IEEE80211_PARAM_SHORTPREAMBLE:
+//		if (value) {
+//			ic->ic_caps |= IEEE80211_C_SHPREAMBLE;
+//		} else {
+//			ic->ic_caps &= ~IEEE80211_C_SHPREAMBLE;
+//		}
+//		retv = ENETRESET;
+//		break;
+//	default:
+//		retv = EOPNOTSUPP;
+//		break;
+//	}
+//	/* XXX should any of these cause a rescan? */
+//	if (retv == ENETRESET)
+//		retv = IS_UP_AUTO(vap) ? ieee80211_open(vap->iv_dev) : 0;
+//	return -retv;
+//}
+
+int usbdrvwext_setmode(struct net_device *dev, struct iw_request_info *info,
+		   	 void *w, char *extra)
+{
+	return 0;
+}
+
+int usbdrvwext_getmode(struct net_device *dev, struct iw_request_info *info,
+			void *w, char *extra)
+{
+	//struct usbdrv_private *macp = dev->ml_priv;
+	struct iw_point *wri = (struct iw_point *)extra;
+	char mode[8];
+
+    strcpy(mode,"11g");
+	return (copy_to_user(wri->pointer, mode, 6) ? -EFAULT : 0);
+}
+
+int zfLnxPrivateIoctl(struct net_device *dev, struct zdap_ioctl* zdreq)
+{
+	//void* regp = macp->regp;
+	u16_t cmd;
+	//u32_t temp;
+	u32_t* p;
+	u32_t i;
+
+	cmd = zdreq->cmd;
+	switch(cmd)
+	{
+	case ZM_IOCTL_REG_READ:
+		zfiDbgReadReg(dev, zdreq->addr);
+		break;
+
+	case ZM_IOCTL_REG_WRITE:
+		zfiDbgWriteReg(dev, zdreq->addr, zdreq->value);
+		break;
+
+	case ZM_IOCTL_MEM_READ:
+		p = (u32_t *) bus_to_virt(zdreq->addr);
+		printk(KERN_DEBUG "usbdrv: read memory addr: 0x%08x value: 0x%08x\n", zdreq->addr, *p);
+		break;
+
+	case ZM_IOCTL_MEM_WRITE:
+		p = (u32_t *) bus_to_virt(zdreq->addr);
+		*p = zdreq->value;
+		printk(KERN_DEBUG "usbdrv: write value: 0x%08x to memory addr: 0x%08x\n", zdreq->value, zdreq->addr);
+		break;
+
+	case ZM_IOCTL_TALLY :
+		zfiWlanShowTally(dev);
+		if (zdreq->addr)
+			zfiWlanResetTally(dev);
+		break;
+
+	case ZM_IOCTL_TEST :
+            printk(KERN_DEBUG "ZM_IOCTL_TEST:len=%d\n", zdreq->addr);
+            //zfiWlanReadReg(dev, 0x10f400);
+            //zfiWlanReadReg(dev, 0x10f404);
+            printk("IOCTL TEST\n");
+            #if 1
+            //print packet
+            for (i=0; i<zdreq->addr; i++)
+            {
+                if ((i&0x7) == 0)
+                {
+                    printk("\n");
+                }
+                printk("%02X ", (unsigned char)zdreq->data[i]);
+            }
+            printk("\n");
+            #endif
+
+
+            #if 0 //For Test?? 1 to 0 by CWYang(-)
+            {
+            struct sk_buff* s;
+
+            /* Allocate a skb */
+            s = alloc_skb(2000, GFP_ATOMIC);
+
+            /* Copy data to skb */
+            for (i=0; i<zdreq->addr; i++)
+            {
+                s->data[i] = zdreq->data[i];
+            }
+            s->len = zdreq->addr;
+
+            /* Call zfIdlRecv() */
+            zfiRecv80211(dev, s, NULL);
+            }
+            #endif
+
+            break;
+
+
+/****************************** ZDCONFIG ******************************/
+        case ZM_IOCTL_FRAG :
+            zfiWlanSetFragThreshold(dev, zdreq->addr);
+            break;
+
+        case ZM_IOCTL_RTS :
+            zfiWlanSetRtsThreshold(dev, zdreq->addr);
+            break;
+
+        case ZM_IOCTL_SCAN :
+            zfiWlanScan(dev);
+            break;
+
+        case ZM_IOCTL_KEY :
+            {
+                u8_t key[29];
+                struct zsKeyInfo keyInfo;
+                u32_t i;
+
+                for (i=0; i<29; i++)
+                {
+                    key[i] = 0;
+                }
+
+                for (i=0; i<zdreq->addr; i++)
+                {
+                    key[i] = zdreq->data[i];
+                }
+
+                printk("key len=%d, key=%02x%02x%02x%02x%02x...\n",
+                        zdreq->addr, key[0], key[1], key[2], key[3], key[4]);
+
+                keyInfo.keyLength = zdreq->addr;
+                keyInfo.keyIndex = 0;
+                keyInfo.flag = 0;
+                keyInfo.key = key;
+                zfiWlanSetKey(dev, keyInfo);
+            }
+            break;
+
+        case ZM_IOCTL_RATE :
+            zfiWlanSetTxRate(dev, zdreq->addr);
+            break;
+
+        case ZM_IOCTL_ENCRYPTION_MODE :
+            zfiWlanSetEncryMode(dev, zdreq->addr);
+
+            zfiWlanDisable(dev, 0);
+            zfiWlanEnable(dev);
+            break;
+        //CWYang(+)
+        case ZM_IOCTL_SIGNAL_STRENGTH :
+            {
+                u8_t buffer[2];
+                zfiWlanQuerySignalInfo(dev, &buffer[0]);
+                printk("Current Signal Strength : %02d\n", buffer[0]);
+            }
+            break;
+        //CWYang(+)
+        case ZM_IOCTL_SIGNAL_QUALITY :
+            {
+                u8_t buffer[2];
+                zfiWlanQuerySignalInfo(dev, &buffer[0]);
+                printk("Current Signal Quality : %02d\n", buffer[1]);
+            }
+            break;
+
+	case ZM_IOCTL_SET_PIBSS_MODE:
+		if (zdreq->addr == 1)
+			zfiWlanSetWlanMode(dev, ZM_MODE_PSEUDO);
+		else
+			zfiWlanSetWlanMode(dev, ZM_MODE_INFRASTRUCTURE);
+
+		zfiWlanDisable(dev, 0);
+		zfiWlanEnable(dev);
+
+		break;
+/****************************** ZDCONFIG ******************************/
+
+	default :
+		printk(KERN_ERR "usbdrv: error command = %x\n", cmd);
+		break;
+	}
+
+	return 0;
+}
+
+int usbdrv_wpa_ioctl(struct net_device *dev, struct athr_wlan_param *zdparm)
+{
+    int ret = 0;
+    u8_t bc_addr[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+    u8_t mac_addr[80];
+    struct zsKeyInfo keyInfo;
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t vapId = 0;
+
+    //zmw_get_wlan_dev(dev);
+
+    switch(zdparm->cmd)
+    {
+        case ZD_CMD_SET_ENCRYPT_KEY:
+
+            /* Set up key information */
+            keyInfo.keyLength = zdparm->u.crypt.key_len;
+            keyInfo.keyIndex = zdparm->u.crypt.idx;
+            if (zfiWlanQueryWlanMode(dev) == ZM_MODE_AP) // AP Mode
+                keyInfo.flag = ZM_KEY_FLAG_AUTHENTICATOR;
+            else
+                keyInfo.flag = 0;
+            keyInfo.key = zdparm->u.crypt.key;
+            keyInfo.initIv = zdparm->u.crypt.seq;
+            keyInfo.macAddr = (u16_t *)zdparm->sta_addr;
+
+            /* Identify the MAC address information */
+            if (memcmp(zdparm->sta_addr, bc_addr, sizeof(bc_addr)) == 0)
+            {
+                keyInfo.flag |= ZM_KEY_FLAG_GK;
+            }
+            else
+            {
+                keyInfo.flag |= ZM_KEY_FLAG_PK;
+            }
+
+            if (!strcmp(zdparm->u.crypt.alg, "NONE"))
+            {
+                //u8_t zero_mac[]={0,0,0,0,0,0};
+
+                /* Set key length to zero */
+                keyInfo.keyLength = 0;
+
+                if (zdparm->sta_addr[0] & 1)//del group key
+                {
+                    //if (macp->cardSetting.WPAIeLen==0)
+                    //{//802.1x dynamic WEP
+                    //    mDynKeyMode = 0;
+                    //    mKeyFormat[0] = 0;
+                    //    mPrivacyInvoked[0]=FALSE;
+                    //    mCap[0] &= ~CAP_PRIVACY;
+                    //    macp->cardSetting.EncryOnOff[0]=0;
+                    //}
+                    //mWpaBcKeyLen = mGkInstalled = 0;
+                }
+                else
+                {
+                    //if (memcmp(zero_mac,zdparm->sta_addr, 6)==0)
+                    //{
+                    //    mDynKeyMode=0;
+                    //    mKeyFormat[0]=0;
+                    //    pSetting->DynKeyMode=0;
+                    //    pSetting->EncryMode[0]=0;
+                    //    mDynKeyMode=0;
+                    //}
+                }
+
+                printk(KERN_ERR "Set Encryption Type NONE\n");
+                return ret;
+            }
+            else if (!strcmp(zdparm->u.crypt.alg, "TKIP"))
+            {
+                zfiWlanSetEncryMode(dev, ZM_TKIP);
+                //Linux Supplicant will inverse Tx/Rx key
+                //So we inverse it back //CWYang(+)
+                //zfMemoryCopy(&temp[0], &keyInfo.key[16], 8);
+                //zfMemoryCopy(&keyInfo.key[16], keyInfo.key[24], 8);
+                //zfMemoryCopy(&keyInfo.key[24], &temp[0], 8);
+                //u8_t temp;
+                //int k;
+                //for (k = 0; k < 8; k++)
+                //{
+                //    temp = keyInfo.key[16 + k];
+                //    keyInfo.key[16 + k] = keyInfo.key[24 + k];
+                //    keyInfo.key[24 + k] = temp;
+                //}
+                //CamEncryType = ZM_TKIP;
+                ////if (idx == 0)
+                //{// Pairwise key
+                //    mKeyFormat[0] = CamEncryType;
+                //    mDynKeyMode = pSetting->DynKeyMode = DYN_KEY_TKIP;
+                //}
+            }
+            else if (!strcmp(zdparm->u.crypt.alg, "CCMP"))
+            {
+                zfiWlanSetEncryMode(dev, ZM_AES);
+                //CamEncryType = ZM_AES;
+                ////if (idx == 0)
+                //{// Pairwise key
+                //    mKeyFormat[0] = CamEncryType;
+                //    mDynKeyMode = pSetting->DynKeyMode = DYN_KEY_AES;
+                //}
+            }
+            else if (!strcmp(zdparm->u.crypt.alg, "WEP"))
+            {
+                if (keyInfo.keyLength == 5)
+                { // WEP 64
+                    zfiWlanSetEncryMode(dev, ZM_WEP64);
+                //    CamEncryType = ZM_WEP64;
+                //    tmpDynKeyMode=DYN_KEY_WEP64;
+                }
+                else if (keyInfo.keyLength == 13)
+                {//keylen=13, WEP 128
+                    zfiWlanSetEncryMode(dev, ZM_WEP128);
+                //    CamEncryType = ZM_WEP128;
+                //    tmpDynKeyMode=DYN_KEY_WEP128;
+                }
+                else
+                {
+                    zfiWlanSetEncryMode(dev, ZM_WEP256);
+                }
+
+                // For Dynamic WEP key (Non-WPA Radius), the key ID range: 0-3
+                // In WPA/RSN mode, the key ID range: 1-3, usually, a broadcast key.
+                // For WEP key setting: we set mDynKeyMode and mKeyFormat in following case:
+                //   1. For 802.1x dynamically generated WEP key method.
+                //   2. For WPA/RSN mode, but key id == 0. (But this is an impossible case)
+                // So, only check case 1.
+                //if (macp->cardSetting.WPAIeLen==0)
+                //{
+                //    mKeyFormat[0] = CamEncryType;
+                //    mDynKeyMode = pSetting->DynKeyMode = tmpDynKeyMode;
+                //    mPrivacyInvoked[0]=TRUE;
+                //    mCap[0] |= CAP_PRIVACY;
+                //    macp->cardSetting.EncryOnOff[0]=1;
+                //}
+            }
+
+            /* DUMP key context */
+//#ifdef WPA_DEBUG
+            if (keyInfo.keyLength > 0)
+            {
+                int ii;
+                printk("Otus: Key Context:\n");
+                for(ii = 0; ii < keyInfo.keyLength;)
+                {
+                    printk("0x%02x ", keyInfo.key[ii]);
+                    if((++ii % 16) == 0)
+                        printk("\n");
+                }
+                printk("\n");
+            }
+//#endif
+
+            /* Set encrypt mode */
+            //zfiWlanSetEncryMode(dev, CamEncryType);
+            vapId = zfLnxGetVapId(dev);
+            if (vapId == 0xffff)
+                keyInfo.vapId = 0;
+            else
+                keyInfo.vapId = vapId + 1;
+            keyInfo.vapAddr[0] = keyInfo.macAddr[0];
+            keyInfo.vapAddr[1] = keyInfo.macAddr[1];
+            keyInfo.vapAddr[2] = keyInfo.macAddr[2];
+
+            zfiWlanSetKey(dev, keyInfo);
+
+            //zfiWlanDisable(dev);
+            //zfiWlanEnable(dev);
+            break;
+
+        case ZD_CMD_SET_MLME:
+            printk(KERN_ERR "usbdrv_wpa_ioctl: ZD_CMD_SET_MLME\n");
+
+            /* Translate STA's address */
+            sprintf(mac_addr, "%02x:%02x:%02x:%02x:%02x:%02x", zdparm->sta_addr[0], zdparm->sta_addr[1],
+                zdparm->sta_addr[2], zdparm->sta_addr[3], zdparm->sta_addr[4], zdparm->sta_addr[5]);
+
+            switch(zdparm->u.mlme.cmd)
+            {
+                case MLME_STA_DEAUTH:
+                    printk(" -------Call zfiWlanDeauth, reason:%d\n",zdparm->u.mlme.reason_code);
+                    if(zfiWlanDeauth(dev, (u16_t*) zdparm->sta_addr, zdparm->u.mlme.reason_code) != 0)
+                        printk(KERN_ERR "Can't deauthencate STA: %s\n", mac_addr);
+                    else
+                        printk(KERN_ERR "Deauthenticate STA: %s with reason code: %d\n", mac_addr, zdparm->u.mlme.reason_code);
+                    break;
+
+                case MLME_STA_DISASSOC:
+                    printk(" -------Call zfiWlanDeauth, reason:%d\n",zdparm->u.mlme.reason_code);
+                    if(zfiWlanDeauth(dev, (u16_t*) zdparm->sta_addr, zdparm->u.mlme.reason_code) != 0)
+                        printk(KERN_ERR "Can't disassociate STA: %s\n", mac_addr);
+                    else
+                        printk(KERN_ERR "Disassociate STA: %s with reason code: %d\n", mac_addr, zdparm->u.mlme.reason_code);
+                    break;
+
+                default:
+                    printk(KERN_ERR "MLME command: 0x%04x not support\n", zdparm->u.mlme.cmd);
+                    break;
+            }
+
+            break;
+
+        case ZD_CMD_SCAN_REQ:
+            printk(KERN_ERR "usbdrv_wpa_ioctl: ZD_CMD_SCAN_REQ\n");
+            break;
+
+        case ZD_CMD_SET_GENERIC_ELEMENT:
+            printk(KERN_ERR "usbdrv_wpa_ioctl: ZD_CMD_SET_GENERIC_ELEMENT\n");
+
+            /* Copy the WPA IE */
+            //zm_msg1_mm(ZM_LV_0, "CWY - wpaie Length : ", zdparm->u.generic_elem.len);
+            printk(KERN_ERR "wpaie Length : %d\n", zdparm->u.generic_elem.len);
+            if (zfiWlanQueryWlanMode(dev) == ZM_MODE_AP) // AP Mode
+            {
+                zfiWlanSetWpaIe(dev, zdparm->u.generic_elem.data, zdparm->u.generic_elem.len);
+            }
+            else
+            {
+                macp->supLen = zdparm->u.generic_elem.len;
+                memcpy(macp->supIe, zdparm->u.generic_elem.data, zdparm->u.generic_elem.len);
+            }
+            zfiWlanSetWpaSupport(dev, 1);
+            //zfiWlanSetWpaIe(dev, zdparm->u.generic_elem.data, zdparm->u.generic_elem.len);
+            {
+                int ii;
+                u8_t len = zdparm->u.generic_elem.len;
+                u8_t *wpaie = (u8_t *)zdparm->u.generic_elem.data;
+
+                printk(KERN_ERR "wd->ap.wpaLen: %d\n", len);
+
+                /* DUMP WPA IE */
+                for(ii = 0; ii < len;)
+                {
+                    printk(KERN_ERR "0x%02x ", wpaie[ii]);
+
+                    if((++ii % 16) == 0)
+                        printk(KERN_ERR "\n");
+                }
+                printk(KERN_ERR "\n");
+            }
+
+//            #ifdef ZM_HOSTAPD_SUPPORT
+            //if (wd->wlanMode == ZM_MODE_AP)
+            //{// Update Beacon FIFO in the next TBTT.
+            //    memcpy(&mWPAIe, pSetting->WPAIe, pSetting->WPAIeLen);
+            //    printk(KERN_ERR "Copy WPA IE into mWPAIe\n");
+            //}
+//            #endif
+            break;
+
+//        #ifdef ZM_HOSTAPD_SUPPORT
+        case ZD_CMD_GET_TSC:
+            printk(KERN_ERR "usbdrv_wpa_ioctl: ZD_CMD_GET_TSC\n");
+            break;
+//        #endif
+
+        default:
+            printk(KERN_ERR "usbdrv_wpa_ioctl default: 0x%04x\n", zdparm->cmd);
+            ret = -EINVAL;
+            break;
+    }
+
+    return ret;
+}
+
+#ifdef ZM_ENABLE_CENC
+int usbdrv_cenc_ioctl(struct net_device *dev, struct zydas_cenc_param *zdparm)
+{
+    //struct usbdrv_private *macp = dev->ml_priv;
+    struct zsKeyInfo keyInfo;
+    u16_t apId;
+    u8_t bc_addr[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+    int ret = 0;
+    int ii;
+
+    /* Get the AP Id */
+    apId = zfLnxGetVapId(dev);
+
+    if (apId == 0xffff)
+    {
+        apId = 0;
+    }
+    else
+    {
+        apId = apId+1;
+    }
+
+    switch (zdparm->cmd)
+    {
+        case ZM_CMD_CENC_SETCENC:
+            printk(KERN_ERR "ZM_CMD_CENC_SETCENC\n");
+            printk(KERN_ERR "length: %d\n", zdparm->len);
+            printk(KERN_ERR "policy: %d\n", zdparm->u.info.cenc_policy);
+            break;
+        case ZM_CMD_CENC_SETKEY:
+            //ret = wai_ioctl_setkey(vap, ioctl_msg);
+            printk(KERN_ERR "ZM_CMD_CENC_SETKEY\n");
+
+            printk(KERN_ERR "MAC address= ");
+            for(ii = 0; ii < 6; ii++)
+            {
+                printk(KERN_ERR "0x%02x ", zdparm->u.crypt.sta_addr[ii]);
+            }
+            printk(KERN_ERR "\n");
+
+            printk(KERN_ERR "Key Index: %d\n", zdparm->u.crypt.keyid);
+            printk(KERN_ERR "Encryption key= ");
+            for(ii = 0; ii < 16; ii++)
+            {
+                printk(KERN_ERR "0x%02x ", zdparm->u.crypt.key[ii]);
+            }
+            printk(KERN_ERR "\n");
+
+            printk(KERN_ERR "MIC key= ");
+            for(ii = 16; ii < ZM_CENC_KEY_SIZE; ii++)
+            {
+                printk(KERN_ERR "0x%02x ", zdparm->u.crypt.key[ii]);
+            }
+            printk(KERN_ERR "\n");
+
+            /* Set up key information */
+            keyInfo.keyLength = ZM_CENC_KEY_SIZE;
+            keyInfo.keyIndex = zdparm->u.crypt.keyid;
+            keyInfo.flag = ZM_KEY_FLAG_AUTHENTICATOR | ZM_KEY_FLAG_CENC;
+            keyInfo.key = zdparm->u.crypt.key;
+            keyInfo.macAddr = (u16_t *)zdparm->u.crypt.sta_addr;
+
+            /* Identify the MAC address information */
+            if (memcmp(zdparm->u.crypt.sta_addr, bc_addr, sizeof(bc_addr)) == 0)
+            {
+                keyInfo.flag |= ZM_KEY_FLAG_GK;
+                keyInfo.vapId = apId;
+                memcpy(keyInfo.vapAddr, dev->dev_addr, ETH_ALEN);
+            }
+            else
+            {
+                keyInfo.flag |= ZM_KEY_FLAG_PK;
+            }
+
+            zfiWlanSetKey(dev, keyInfo);
+
+            break;
+        case ZM_CMD_CENC_REKEY:
+            //ret = wai_ioctl_rekey(vap, ioctl_msg);
+            printk(KERN_ERR "ZM_CMD_CENC_REKEY\n");
+            break;
+        default:
+            ret = -EOPNOTSUPP;
+            break;
+
+    }
+
+    //if (retv == ENETRESET)
+    //    retv = IS_UP_AUTO(vap) ? ieee80211_open(vap->iv_dev) : 0;
+
+    return ret;
+}
+#endif //ZM_ENABLE_CENC
+/////////////////////////////////////////
+int usbdrv_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+//    struct usbdrv_private *macp;
+//    void *regp;
+    struct zdap_ioctl zdreq;
+    struct iwreq *wrq = (struct iwreq *)ifr;
+    struct athr_wlan_param zdparm;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    int err = 0;
+    int changed = 0;
+
+//    regp = macp->regp;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    switch (cmd)
+    {
+            case SIOCGIWNAME:
+            strcpy(wrq->u.name, "IEEE 802.11-DS");
+            break;
+
+        case SIOCGIWAP:
+            err = usbdrvwext_giwap(dev, NULL, &wrq->u.ap_addr, NULL);
+            break;
+
+
+        case SIOCSIWAP:
+            err = usbdrvwext_siwap(dev, NULL, &wrq->u.ap_addr, NULL);
+            break;
+
+
+        case SIOCGIWMODE:
+            err = usbdrvwext_giwmode(dev, NULL, &wrq->u.mode, NULL);
+            break;
+
+
+        case SIOCSIWESSID:
+            printk(KERN_ERR "CWY - usbdrvwext_siwessid\n");
+            //err = usbdrv_ioctl_setessid(dev, &wrq->u.essid);
+            err = usbdrvwext_siwessid(dev, NULL, &wrq->u.essid, NULL);
+
+            if (! err)
+                changed = 1;
+            break;
+
+
+        case SIOCGIWESSID:
+            err = usbdrvwext_giwessid(dev, NULL, &wrq->u.essid, NULL);
+            break;
+
+
+        case SIOCSIWRTS:
+
+            err = usbdrv_ioctl_setrts(dev, &wrq->u.rts);
+            if (! err)
+                changed = 1;
+            break;
+
+
+		case SIOCIWFIRSTPRIV + 0x2: /* set_auth */
+		{
+			//printk("CWY - SIOCIWFIRSTPRIV + 0x2 (set_auth)\n");
+			if (! capable(CAP_NET_ADMIN))
+			{
+				err = -EPERM;
+				break;
+			}
+			{
+				int val = *( (int *) wrq->u.name );
+				if ((val < 0) || (val > 2))
+				{
+					err = -EINVAL;
+					break;
+				}
+				else
+				{
+					zfiWlanSetAuthenticationMode(dev, val);
+
+                    if (macp->DeviceOpened == 1)
+                    {
+                        zfiWlanDisable(dev, 0);
+                        zfiWlanEnable(dev);
+                    }
+
+					err = 0;
+					changed = 1;
+				}
+			}
+		}
+			break;
+
+		case SIOCIWFIRSTPRIV + 0x3: /* get_auth */
+		{
+		    int AuthMode = ZM_AUTH_MODE_OPEN;
+
+			//printk("CWY - SIOCIWFIRSTPRIV + 0x3 (get_auth)\n");
+
+			if (wrq->u.data.pointer)
+			{
+				wrq->u.data.flags = 1;
+
+				AuthMode = zfiWlanQueryAuthenticationMode(dev, 0);
+				if (AuthMode == ZM_AUTH_MODE_OPEN)
+				{
+					wrq->u.data.length = 12;
+
+					if (copy_to_user(wrq->u.data.pointer, "open system", 12))
+					{
+						return -EFAULT;
+					}
+				}
+				else if (AuthMode == ZM_AUTH_MODE_SHARED_KEY)
+				{
+					wrq->u.data.length = 11;
+
+					if (copy_to_user(wrq->u.data.pointer, "shared key", 11))
+					{
+						return -EFAULT;
+					}
+				}
+				else if (AuthMode == ZM_AUTH_MODE_AUTO)
+				{
+					wrq->u.data.length = 10;
+
+					if (copy_to_user(wrq->u.data.pointer, "auto mode", 10))
+					{
+						return -EFAULT;
+					}
+				}
+				else
+				{
+					return -EFAULT;
+				}
+			}
+		}
+			break;
+
+
+        case ZDAPIOCTL:    //debug command
+            if (copy_from_user(&zdreq, ifr->ifr_data, sizeof (zdreq)))
+            {
+                printk(KERN_ERR "usbdrv: copy_from_user error\n");
+                return -EFAULT;
+            }
+
+            //printk(KERN_DEBUG "usbdrv: cmd=%2x, reg=0x%04lx, value=0x%08lx\n",
+            //        zdreq.cmd, zdreq.addr, zdreq.value);
+
+			zfLnxPrivateIoctl(dev, &zdreq);
+
+            err = 0;
+            break;
+
+        case ZD_IOCTL_WPA:
+            if (copy_from_user(&zdparm, ifr->ifr_data, sizeof(struct athr_wlan_param)))
+            {
+                printk(KERN_ERR "usbdrv: copy_from_user error\n");
+                return -EFAULT;
+            }
+
+            usbdrv_wpa_ioctl(dev, &zdparm);
+            err = 0;
+            break;
+
+        case ZD_IOCTL_PARAM:
+        {
+            int *p;
+            int op;
+            int arg;
+
+            /* Point to the name field and retrieve the
+             * op and arg elements.          */
+            p = (int *)wrq->u.name;
+            op = *p++;
+            arg = *p;
+
+            if(op == ZD_PARAM_ROAMING)
+            {
+                printk(KERN_ERR "************* ZD_PARAM_ROAMING: %d\n", arg);
+                //macp->cardSetting.ap_scan=(U8)arg;
+            }
+            if(op == ZD_PARAM_PRIVACY)
+            {
+                printk(KERN_ERR "ZD_IOCTL_PRIVACY: ");
+
+                /* Turn on the privacy invoke flag */
+                if(arg)
+                {
+                //    mCap[0] |= CAP_PRIVACY;
+                //    macp->cardSetting.EncryOnOff[0] = 1;
+                    printk(KERN_ERR "enable\n");
+
+                }
+                else
+                {
+                //    mCap[0] &= ~CAP_PRIVACY;
+                //    macp->cardSetting.EncryOnOff[0] = 0;
+                    printk(KERN_ERR "disable\n");
+                }
+                                //changed=1;
+            }
+            if(op == ZD_PARAM_WPA)
+            {
+                printk(KERN_ERR "ZD_PARAM_WPA: ");
+
+                if(arg)
+                {
+                    printk(KERN_ERR "enable\n");
+
+                    if (zfiWlanQueryWlanMode(dev) != ZM_MODE_AP)
+                    {
+                        printk(KERN_ERR "Station Mode\n");
+                        //zfiWlanQueryWpaIe(dev, (u8_t *)&wpaIe, &wpalen);
+                        //printk("wpaIe : %2x,%2x,%2x\n", wpaIe[21], wpaIe[22], wpaIe[23]);
+                        //printk("rsnIe : %2x,%2x,%2x\n", wpaIe[17], wpaIe[18], wpaIe[19]);
+                        if ((macp->supIe[21] == 0x50) &&
+                            (macp->supIe[22] == 0xf2) &&
+                            (macp->supIe[23] == 0x2))
+                        {
+                            printk(KERN_ERR "wd->sta.authMode = ZM_AUTH_MODE_WPAPSK\n");
+                            //wd->sta.authMode = ZM_AUTH_MODE_WPAPSK;
+                            //wd->ws.authMode = ZM_AUTH_MODE_WPAPSK;
+                            zfiWlanSetAuthenticationMode(dev, ZM_AUTH_MODE_WPAPSK);
+                        }
+                        else if ((macp->supIe[21] == 0x50) &&
+                                 (macp->supIe[22] == 0xf2) &&
+                                 (macp->supIe[23] == 0x1))
+                        {
+                            printk(KERN_ERR "wd->sta.authMode = ZM_AUTH_MODE_WPA\n");
+                            //wd->sta.authMode = ZM_AUTH_MODE_WPA;
+                            //wd->ws.authMode = ZM_AUTH_MODE_WPA;
+                            zfiWlanSetAuthenticationMode(dev, ZM_AUTH_MODE_WPA);
+                        }
+                        else if ((macp->supIe[17] == 0xf) &&
+                                 (macp->supIe[18] == 0xac) &&
+                                 (macp->supIe[19] == 0x2))
+                        {
+                            printk(KERN_ERR "wd->sta.authMode = ZM_AUTH_MODE_WPA2PSK\n");
+                            //wd->sta.authMode = ZM_AUTH_MODE_WPA2PSK;
+                            //wd->ws.authMode = ZM_AUTH_MODE_WPA2PSK;
+                            zfiWlanSetAuthenticationMode(dev, ZM_AUTH_MODE_WPA2PSK);
+                        }
+                        else if ((macp->supIe[17] == 0xf) &&
+                                 (macp->supIe[18] == 0xac) &&
+                                 (macp->supIe[19] == 0x1))
+                        {
+                            printk(KERN_ERR "wd->sta.authMode = ZM_AUTH_MODE_WPA2\n");
+                            //wd->sta.authMode = ZM_AUTH_MODE_WPA2;
+                            //wd->ws.authMode = ZM_AUTH_MODE_WPA2;
+                            zfiWlanSetAuthenticationMode(dev, ZM_AUTH_MODE_WPA2);
+                        }
+                        if ((macp->supIe[21] == 0x50) || (macp->supIe[22] == 0xf2))//WPA or WPAPSK
+                        {
+                            if (macp->supIe[11] == 0x2)
+                            {
+                                printk(KERN_ERR "wd->sta.wepStatus = ZM_ENCRYPTION_TKIP\n");
+                                //wd->sta.wepStatus = ZM_ENCRYPTION_TKIP;
+                                //wd->ws.wepStatus = ZM_ENCRYPTION_TKIP;
+                                zfiWlanSetWepStatus(dev, ZM_ENCRYPTION_TKIP);
+                            }
+                            else
+                            {
+                                printk(KERN_ERR "wd->sta.wepStatus = ZM_ENCRYPTION_AES\n");
+                                //wd->sta.wepStatus = ZM_ENCRYPTION_AES;
+                                //wd->ws.wepStatus = ZM_ENCRYPTION_AES;
+                                zfiWlanSetWepStatus(dev, ZM_ENCRYPTION_AES);
+                            }
+                        }
+                        if ((macp->supIe[17] == 0xf) || (macp->supIe[18] == 0xac)) //WPA2 or WPA2PSK
+                        {
+                            if (macp->supIe[13] == 0x2)
+                            {
+                                printk(KERN_ERR "wd->sta.wepStatus = ZM_ENCRYPTION_TKIP\n");
+                                //wd->sta.wepStatus = ZM_ENCRYPTION_TKIP;
+                                //wd->ws.wepStatus = ZM_ENCRYPTION_TKIP;
+                                zfiWlanSetWepStatus(dev, ZM_ENCRYPTION_TKIP);
+                            }
+                            else
+                            {
+                                printk(KERN_ERR "wd->sta.wepStatus = ZM_ENCRYPTION_AES\n");
+                                //wd->sta.wepStatus = ZM_ENCRYPTION_AES;
+                                //wd->ws.wepStatus = ZM_ENCRYPTION_AES;
+                                zfiWlanSetWepStatus(dev, ZM_ENCRYPTION_AES);
+                            }
+                        }
+                    }
+                    zfiWlanSetWpaSupport(dev, 1);
+                }
+                else
+                {
+                    /* Reset the WPA related variables */
+                    printk(KERN_ERR "disable\n");
+
+                    zfiWlanSetWpaSupport(dev, 0);
+                    zfiWlanSetAuthenticationMode(dev, ZM_AUTH_MODE_OPEN);
+                    zfiWlanSetWepStatus(dev, ZM_ENCRYPTION_WEP_DISABLED);
+
+                    /* Now we only set the length in the WPA IE
+                     * field to zero.                         */
+                    //macp->cardSetting.WPAIe[1] = 0;
+                }
+            }
+            if(op == ZD_PARAM_COUNTERMEASURES)
+            {
+                printk(KERN_ERR "================ZD_PARAM_COUNTERMEASURES: ");
+
+                if(arg)
+                {
+                //    mCounterMeasureState=1;
+                    printk(KERN_ERR "enable\n");
+                }
+                else
+                {
+                //    mCounterMeasureState=0;
+                    printk(KERN_ERR "disable\n");
+                }
+            }
+            if(op == ZD_PARAM_DROPUNENCRYPTED)
+            {
+                printk(KERN_ERR "ZD_PARAM_DROPUNENCRYPTED: ");
+
+                if(arg)
+                {
+                    printk(KERN_ERR "enable\n");
+                }
+                else
+                {
+                    printk(KERN_ERR "disable\n");
+                }
+            }
+            if(op == ZD_PARAM_AUTH_ALGS)
+            {
+                printk(KERN_ERR "ZD_PARAM_AUTH_ALGS: ");
+
+                if(arg == 0)
+                {
+                    printk(KERN_ERR "OPEN_SYSTEM\n");
+                }
+                else
+                {
+                    printk(KERN_ERR "SHARED_KEY\n");
+                }
+            }
+            if(op == ZD_PARAM_WPS_FILTER)
+            {
+                printk(KERN_ERR "ZD_PARAM_WPS_FILTER: ");
+
+                if(arg)
+                {
+                //    mCounterMeasureState=1;
+                    macp->forwardMgmt = 1;
+                    printk(KERN_ERR "enable\n");
+                }
+                else
+                {
+                //    mCounterMeasureState=0;
+                    macp->forwardMgmt = 0;
+                    printk(KERN_ERR "disable\n");
+                }
+            }
+        }
+            err = 0;
+            break;
+
+        case ZD_IOCTL_GETWPAIE:
+        {
+            struct ieee80211req_wpaie req_wpaie;
+            u16_t apId, i, j;
+
+            /* Get the AP Id */
+            apId = zfLnxGetVapId(dev);
+
+            if (apId == 0xffff)
+            {
+                apId = 0;
+            }
+            else
+            {
+                apId = apId+1;
+            }
+
+            if (copy_from_user(&req_wpaie, ifr->ifr_data, sizeof(struct ieee80211req_wpaie))){
+                printk(KERN_ERR "usbdrv: copy_from_user error\n");
+                return -EFAULT;
+            }
+
+            for(i = 0; i < ZM_OAL_MAX_STA_SUPPORT; i++)
+            {
+                for(j = 0; j < IEEE80211_ADDR_LEN; j++)
+                {
+                    if (macp->stawpaie[i].wpa_macaddr[j] != req_wpaie.wpa_macaddr[j])
+                        break;
+                }
+                if (j == 6)
+                    break;
+            }
+            if (i < ZM_OAL_MAX_STA_SUPPORT)
+            {
+                //printk("ZD_IOCTL_GETWPAIE - sta index = %d\n", i);
+                memcpy(req_wpaie.wpa_ie, macp->stawpaie[i].wpa_ie, IEEE80211_MAX_IE_SIZE);
+            }
+
+            if (copy_to_user(wrq->u.data.pointer, &req_wpaie, sizeof(struct ieee80211req_wpaie)))
+            {
+                    return -EFAULT;
+            }
+        }
+
+            err = 0;
+            break;
+#ifdef ZM_ENABLE_CENC
+        case ZM_IOCTL_CENC:
+            if (copy_from_user(&macp->zd_wpa_req, ifr->ifr_data, sizeof(struct athr_wlan_param)))
+            {
+                printk(KERN_ERR "usbdrv: copy_from_user error\n");
+                return -EFAULT;
+            }
+
+            usbdrv_cenc_ioctl(dev, (struct zydas_cenc_param *)&macp->zd_wpa_req);
+            err = 0;
+            break;
+#endif //ZM_ENABLE_CENC
+        default:
+            err = -EOPNOTSUPP;
+            break;
+    }
+
+
+    return err;
+}
diff --git a/drivers/staging/otus/oal_dt.h b/drivers/staging/otus/oal_dt.h
new file mode 100644
index 000000000000..e82b9770fca1
--- /dev/null
+++ b/drivers/staging/otus/oal_dt.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : oal_dt.h                                              */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains data type definition.                      */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      Platform dependent.                                             */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _OAL_DT_H
+#define _OAL_DT_H
+
+/* Please include header files for buffer type in the beginning of this file */
+/* Please include header files for device type here */
+#include <linux/netdevice.h>
+
+typedef     unsigned long long  u64_t;
+typedef     unsigned int        u32_t;
+typedef     unsigned short      u16_t;
+typedef     unsigned char       u8_t;
+typedef     long long           s64_t;
+typedef     long                s32_t;
+typedef     short               s16_t;
+typedef     char                s8_t;
+
+#ifndef     TRUE
+#define     TRUE                (1==1)
+#endif
+
+#ifndef     FALSE
+#define     FALSE               (1==0)
+#endif
+
+#ifndef     NULL
+#define     NULL                0
+#endif
+
+/* Please include header files for buffer type in the beginning of this file */
+typedef     struct sk_buff      zbuf_t;
+
+/* Please include header files for device type in the beginning of this file */
+typedef     struct net_device   zdev_t;
+
+#endif /* #ifndef _OAL_DT_H */
diff --git a/drivers/staging/otus/oal_marc.h b/drivers/staging/otus/oal_marc.h
new file mode 100644
index 000000000000..206111616a03
--- /dev/null
+++ b/drivers/staging/otus/oal_marc.h
@@ -0,0 +1,135 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : oal_marc.h                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains warpper definitions.                       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      Platform dependent.                                             */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _OAL_MARC_H
+#define _OAL_MARC_H
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#define ZM_OS_LINUX_FUNC
+
+/***** Critical section *****/
+/* Declare for critical section */
+#ifndef ZM_HALPLUS_LOCK
+#define zmw_get_wlan_dev(dev)    struct zsWlanDev *wd = (struct zsWlanDev*) ((((struct usbdrv_private*)dev->priv)->wd))
+
+#define zmw_declare_for_critical_section() unsigned long irqFlag;
+
+/* Enter critical section */
+#define zmw_enter_critical_section(dev) \
+        spin_lock_irqsave(&(((struct usbdrv_private *)(dev->priv))->cs_lock), irqFlag);
+
+/* leave critical section */
+#define zmw_leave_critical_section(dev) \
+        spin_unlock_irqrestore(&(((struct usbdrv_private *)(dev->priv))->cs_lock), irqFlag);
+#else
+#define zmw_get_wlan_dev(dev)    struct zsWlanDev *wd = zfwGetWlanDev(dev);
+
+/* Declare for critical section */
+#define zmw_declare_for_critical_section()
+
+/* Enter critical section */
+#define zmw_enter_critical_section(dev) \
+        zfwEnterCriticalSection(dev);
+
+/* leave critical section */
+#define zmw_leave_critical_section(dev) \
+        zfwLeaveCriticalSection(dev);
+#endif
+
+/***** Byte order converting *****/
+#ifdef ZM_CONFIG_BIG_ENDIAN
+#define zmw_cpu_to_le32(v)    (((v & 0xff000000) >> 24) | \
+                               ((v & 0x00ff0000) >> 8)  | \
+                               ((v & 0x0000ff00) << 8)  | \
+                               ((v & 0x000000ff) << 24))
+
+#define zmw_le32_to_cpu(v)    (((v & 0xff000000) >> 24) | \
+                               ((v & 0x00ff0000) >> 8)  | \
+                               ((v & 0x0000ff00) << 8)  | \
+                               ((v & 0x000000ff) << 24))
+
+#define zmw_cpu_to_le16(v)    (((v & 0xff00) >> 8) | \
+                               ((v & 0x00ff) << 8))
+
+#define zmw_le16_to_cpu(v)    (((v & 0xff00) >> 8) | \
+                               ((v & 0x00ff) << 8))
+#else
+#define zmw_cpu_to_le32(v)    (v)
+#define zmw_le32_to_cpu(v)    (v)
+#define zmw_cpu_to_le16(v)    (v)
+#define zmw_le16_to_cpu(v)    (v)
+#endif
+
+/***** Buffer access *****/
+/* Called to read/write buffer */
+#ifndef ZM_HALPLUS_LOCK
+
+#define zmw_buf_readb(dev, buf, offset) *(u8_t*)((u8_t*)buf->data+offset)
+#define zmw_buf_readh(dev, buf, offset) zmw_cpu_to_le16(*(u16_t*)((u8_t*)buf->data+offset))
+#define zmw_buf_writeb(dev, buf, offset, value) *(u8_t*)((u8_t*)buf->data+offset) = value
+#define zmw_buf_writeh(dev, buf, offset, value) *(u16_t*)((u8_t*)buf->data+offset) = zmw_cpu_to_le16(value)
+#define zmw_buf_get_buffer(dev, buf) (u8_t*)(buf->data)
+
+#else
+
+#define zmw_buf_readb(dev, buf, offset) zfwBufReadByte(dev, buf, offset)
+#define zmw_buf_readh(dev, buf, offset) zfwBufReadHalfWord(dev, buf, offset)
+#define zmw_buf_writeb(dev, buf, offset, value) zfwBufWriteByte(dev, buf, offset, value)
+#define zmw_buf_writeh(dev, buf, offset, value) zfwBufWriteHalfWord(dev, buf, offset, value)
+#define zmw_buf_get_buffer(dev, buf) zfwGetBuffer(dev, buf)
+
+#endif
+
+/***** Debug message *****/
+#if 0
+#define zm_debug_msg0(msg) printk("%s:%s\n", __func__, msg);
+#define zm_debug_msg1(msg, val) printk("%s:%s%ld\n", __func__, \
+        msg, (u32_t)val);
+#define zm_debug_msg2(msg, val) printk("%s:%s%lxh\n", __func__, \
+        msg, (u32_t)val);
+#define zm_debug_msg_s(msg, val) printk("%s:%s%s\n", __func__, \
+        msg, val);
+#define zm_debug_msg_p(msg, val1, val2) printk("%s:%s%01ld.%02ld\n", __func__, \
+        msg, (val1/val2), (((val1*100)/val2)%100));
+#define zm_dbg(S) printk S
+#else
+#define zm_debug_msg0(msg)
+#define zm_debug_msg1(msg, val)
+#define zm_debug_msg2(msg, val)
+#define zm_debug_msg_s(msg, val)
+#define zm_debug_msg_p(msg, val1, val2)
+#define zm_dbg(S)
+#endif
+
+#define zm_assert(expr) if(!(expr)) {                           \
+        printk( "Atheors Assertion failed! %s,%s,%s,line=%d\n",   \
+        #expr,__FILE__,__func__,__LINE__);                  \
+        }
+
+#define DbgPrint printk
+
+#endif /* #ifndef _OAL_MARC_H */
diff --git a/drivers/staging/otus/usbdrv.c b/drivers/staging/otus/usbdrv.c
new file mode 100644
index 000000000000..dfe07072011f
--- /dev/null
+++ b/drivers/staging/otus/usbdrv.c
@@ -0,0 +1,1148 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : usbdrv.c                                              */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains network interface up/down related functions.*/
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+/* src/usbdrv.c */
+
+#define ZM_PIBSS_MODE   0
+#define ZM_AP_MODE      0
+#define ZM_CHANNEL      11
+#define ZM_WEP_MOME     0
+#define ZM_SHARE_AUTH   0
+#define ZM_DISABLE_XMIT 0
+
+#include "usbdrv.h"
+#include "oal_dt.h"
+#include "80211core/pub_zfi.h"
+
+#include "linux/netlink.h"
+#include "linux/rtnetlink.h"
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+#ifdef ZM_HOSTAPD_SUPPORT
+#include "athr_common.h"
+#endif
+
+extern void zfDumpDescriptor(zdev_t* dev, u16_t type);
+//extern void zfiWlanQueryMacAddress(zdev_t* dev, u8_t* addr);
+
+// ISR handler
+irqreturn_t usbdrv_intr(int, void *, struct pt_regs *);
+
+// Network Device interface related function
+int usbdrv_open(struct net_device *);
+int usbdrv_close(struct net_device *);
+int usbdrv_change_mtu(struct net_device *, int);
+int usbdrv_set_mac(struct net_device *, void *);
+int usbdrv_xmit_frame(struct sk_buff *, struct net_device *);
+void usbdrv_set_multi(struct net_device *);
+struct net_device_stats *usbdrv_get_stats(struct net_device *);
+
+//wireless extension helper functions
+int usbdrv_ioctl_setessid(struct net_device *dev, struct iw_point *erq);
+int usbdrv_ioctl_getessid(struct net_device *dev, struct iw_point *erq);
+int usbdrv_ioctl_setrts(struct net_device *dev, struct iw_param *rrq);
+/* Wireless Extension Handler functions */
+int usbdrvwext_giwmode(struct net_device *dev, struct iw_request_info* info,
+        __u32 *mode, char *extra);
+int zfLnxPrivateIoctl(struct usbdrv_private *macp, struct zdap_ioctl *zdreq);
+
+void zfLnx10msTimer(struct net_device* dev);
+int zfUnregisterWdsDev(struct net_device* parentDev, u16_t wdsId);
+int zfRegisterWdsDev(struct net_device* parentDev, u16_t wdsId);
+int zfWdsOpen(struct net_device *dev);
+int zfWdsClose(struct net_device *dev);
+int zfLnxVapOpen(struct net_device *dev);
+int zfLnxVapClose(struct net_device *dev);
+int zfLnxVapXmitFrame(struct sk_buff *skb, struct net_device *dev);
+int zfLnxRegisterVapDev(struct net_device* parentDev, u16_t vapId);
+int usbdrv_wpa_ioctl(struct net_device *dev, struct athr_wlan_param *zdparm);
+extern u16_t zfLnxGetVapId(zdev_t* dev);
+extern u16_t zfLnxCheckTxBufferCnt(zdev_t *dev);
+extern UsbTxQ_t *zfLnxGetUsbTxBuffer(zdev_t *dev);
+
+extern u16_t zfLnxAuthNotify(zdev_t* dev, u16_t* macAddr);
+extern u16_t zfLnxAsocNotify(zdev_t* dev, u16_t* macAddr, u8_t* body, u16_t bodySize, u16_t port);
+extern u16_t zfLnxDisAsocNotify(zdev_t* dev, u8_t* macAddr, u16_t port);
+extern u16_t zfLnxApConnectNotify(zdev_t* dev, u8_t* macAddr, u16_t port);
+extern void zfLnxConnectNotify(zdev_t* dev, u16_t status, u16_t* bssid);
+extern void zfLnxScanNotify(zdev_t* dev, struct zsScanResult* result);
+extern void zfLnxStatisticsNotify(zdev_t* dev, struct zsStastics* result);
+extern void zfLnxMicFailureNotify(zdev_t* dev, u16_t* addr, u16_t status);
+extern void zfLnxApMicFailureNotify(zdev_t* dev, u8_t* addr, zbuf_t* buf);
+extern void zfLnxIbssPartnerNotify(zdev_t* dev, u16_t status, struct zsPartnerNotifyEvent *event);
+extern void zfLnxMacAddressNotify(zdev_t* dev, u8_t* addr);
+extern void zfLnxSendCompleteIndication(zdev_t* dev, zbuf_t* buf);
+extern void zfLnxRecvEth(zdev_t* dev, zbuf_t* buf, u16_t port);
+extern void zfLnxRestoreBufData(zdev_t* dev, zbuf_t* buf);
+#ifdef ZM_ENABLE_CENC
+extern u16_t zfLnxCencAsocNotify(zdev_t* dev, u16_t* macAddr, u8_t* body, u16_t bodySize, u16_t port);
+#endif //ZM_ENABLE_CENC
+extern void zfLnxWatchDogNotify(zdev_t* dev);
+extern void zfLnxRecv80211(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+extern u8_t zfLnxCreateThread(zdev_t *dev);
+
+/******************************************************************************
+*                        P U B L I C   D A T A
+*******************************************************************************
+*/
+
+/* Definition of Wireless Extension */
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+//wireless extension helper functions
+extern int usbdrv_ioctl_setessid(struct net_device *dev, struct iw_point *erq);
+extern int usbdrv_ioctl_setrts(struct net_device *dev, struct iw_param *rrq);
+/* Wireless Extension Handler functions */
+extern int usbdrvwext_giwname(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrq, char *extra);
+extern int usbdrvwext_siwfreq(struct net_device *dev, struct iw_request_info *info,
+        struct iw_freq *freq, char *extra);
+extern int usbdrvwext_giwfreq(struct net_device *dev, struct iw_request_info *info,
+        struct iw_freq *freq, char *extra);
+extern int usbdrvwext_siwmode(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrq, char *extra);
+extern int usbdrvwext_giwmode(struct net_device *dev, struct iw_request_info *info,
+        __u32 *mode, char *extra);
+extern int usbdrvwext_siwsens(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *sens, char *extra);
+extern int usbdrvwext_giwsens(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *sens, char *extra);
+extern int usbdrvwext_giwrange(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *data, char *extra);
+extern int usbdrvwext_siwap(struct net_device *dev, struct iw_request_info *info,
+        struct sockaddr *MacAddr, char *extra);
+extern int usbdrvwext_giwap(struct net_device *dev, struct iw_request_info *info,
+        struct sockaddr *MacAddr, char *extra);
+extern int usbdrvwext_iwaplist(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *data, char *extra);
+extern int usbdrvwext_siwscan(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *data, char *extra);
+extern int usbdrvwext_giwscan(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *data, char *extra);
+extern int usbdrvwext_siwessid(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *essid, char *extra);
+extern int usbdrvwext_giwessid(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *essid, char *extra);
+extern int usbdrvwext_siwnickn(struct net_device *dev, struct iw_request_info *info,
+	    struct iw_point *data, char *nickname);
+extern int usbdrvwext_giwnickn(struct net_device *dev, struct iw_request_info *info,
+	    struct iw_point *data, char *nickname);
+extern int usbdrvwext_siwrate(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *frq, char *extra);
+extern int usbdrvwext_giwrate(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *frq, char *extra);
+extern int usbdrvwext_siwrts(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *rts, char *extra);
+extern int usbdrvwext_giwrts(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *rts, char *extra);
+extern int usbdrvwext_siwfrag(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *frag, char *extra);
+extern int usbdrvwext_giwfrag(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *frag, char *extra);
+extern int usbdrvwext_siwtxpow(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *rrq, char *extra);
+extern int usbdrvwext_giwtxpow(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *rrq, char *extra);
+extern int usbdrvwext_siwretry(struct net_device *dev, struct iw_request_info *info,
+	    struct iw_param *rrq, char *extra);
+extern int usbdrvwext_giwretry(struct net_device *dev, struct iw_request_info *info,
+	    struct iw_param *rrq, char *extra);
+extern int usbdrvwext_siwencode(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *erq, char *key);
+extern int usbdrvwext_giwencode(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *erq, char *key);
+extern int usbdrvwext_siwpower(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *frq, char *extra);
+extern int usbdrvwext_giwpower(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *frq, char *extra);
+extern int usbdrv_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd);
+/*
+ * Structures to export the Wireless Handlers
+ */
+
+struct iw_priv_args usbdrv_private_args[] = {
+//    { SIOCIWFIRSTPRIV + 0x0, 0, 0, "list_bss" },
+//    { SIOCIWFIRSTPRIV + 0x1, 0, 0, "card_reset" },
+    { SIOCIWFIRSTPRIV + 0x2, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "set_auth" },  /* 0 - open, 1 - shared key */
+    { SIOCIWFIRSTPRIV + 0x3, 0, IW_PRIV_TYPE_CHAR | 12, "get_auth" },
+//    { SIOCIWFIRSTPRIV + 0x4, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "set_preamble" },  /* 0 - long, 1 - short */
+//    { SIOCIWFIRSTPRIV + 0x5, 0, IW_PRIV_TYPE_CHAR | 6, "get_preamble" },
+//    { SIOCIWFIRSTPRIV + 0x6, 0, 0, "cnt" },
+//    { SIOCIWFIRSTPRIV + 0x7, 0, 0, "regs" },
+//    { SIOCIWFIRSTPRIV + 0x8, 0, 0, "probe" },
+//    { SIOCIWFIRSTPRIV + 0x9, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "dbg_flag" },
+//    { SIOCIWFIRSTPRIV + 0xA, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "connect" },
+//    { SIOCIWFIRSTPRIV + 0xB, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "set_mac_mode" },
+//    { SIOCIWFIRSTPRIV + 0xC, 0, IW_PRIV_TYPE_CHAR | 12, "get_mac_mode" },
+};
+
+#if WIRELESS_EXT > 12
+static iw_handler usbdrvwext_handler[] = {
+    (iw_handler) NULL,                              /* SIOCSIWCOMMIT */
+    (iw_handler) usbdrvwext_giwname,                /* SIOCGIWNAME */
+    (iw_handler) NULL,                              /* SIOCSIWNWID */
+    (iw_handler) NULL,                              /* SIOCGIWNWID */
+    (iw_handler) usbdrvwext_siwfreq,                /* SIOCSIWFREQ */
+    (iw_handler) usbdrvwext_giwfreq,                /* SIOCGIWFREQ */
+    (iw_handler) usbdrvwext_siwmode,                /* SIOCSIWMODE */
+    (iw_handler) usbdrvwext_giwmode,                /* SIOCGIWMODE */
+    (iw_handler) usbdrvwext_siwsens,                /* SIOCSIWSENS */
+    (iw_handler) usbdrvwext_giwsens,                /* SIOCGIWSENS */
+    (iw_handler) NULL, /* not used */               /* SIOCSIWRANGE */
+    (iw_handler) usbdrvwext_giwrange,               /* SIOCGIWRANGE */
+    (iw_handler) NULL, /* not used */               /* SIOCSIWPRIV */
+    (iw_handler) NULL, /* kernel code */            /* SIOCGIWPRIV */
+    (iw_handler) NULL, /* not used */               /* SIOCSIWSTATS */
+    (iw_handler) NULL, /* kernel code */            /* SIOCGIWSTATS */
+    (iw_handler) NULL,                              /* SIOCSIWSPY */
+    (iw_handler) NULL,                              /* SIOCGIWSPY */
+    (iw_handler) NULL,                              /* -- hole -- */
+    (iw_handler) NULL,                              /* -- hole -- */
+    (iw_handler) usbdrvwext_siwap,                  /* SIOCSIWAP */
+    (iw_handler) usbdrvwext_giwap,                  /* SIOCGIWAP */
+    (iw_handler) NULL,              /* -- hole -- */
+    (iw_handler) usbdrvwext_iwaplist,               /* SIOCGIWAPLIST */
+#if WIRELESS_EXT > 13
+    (iw_handler) usbdrvwext_siwscan,                /* SIOCSIWSCAN */
+    (iw_handler) usbdrvwext_giwscan,                /* SIOCGIWSCAN */
+#else /* WIRELESS_EXT > 13 */
+    (iw_handler) NULL, /* null */                   /* SIOCSIWSCAN */
+    (iw_handler) NULL, /* null */                   /* SIOCGIWSCAN */
+#endif /* WIRELESS_EXT > 13 */
+    (iw_handler) usbdrvwext_siwessid,               /* SIOCSIWESSID */
+    (iw_handler) usbdrvwext_giwessid,               /* SIOCGIWESSID */
+
+    (iw_handler) usbdrvwext_siwnickn,               /* SIOCSIWNICKN */
+    (iw_handler) usbdrvwext_giwnickn,               /* SIOCGIWNICKN */
+    (iw_handler) NULL,                              /* -- hole -- */
+    (iw_handler) NULL,                              /* -- hole -- */
+    (iw_handler) usbdrvwext_siwrate,                /* SIOCSIWRATE */
+    (iw_handler) usbdrvwext_giwrate,                /* SIOCGIWRATE */
+    (iw_handler) usbdrvwext_siwrts,                 /* SIOCSIWRTS */
+    (iw_handler) usbdrvwext_giwrts,                 /* SIOCGIWRTS */
+    (iw_handler) usbdrvwext_siwfrag,                /* SIOCSIWFRAG */
+    (iw_handler) usbdrvwext_giwfrag,                /* SIOCGIWFRAG */
+    (iw_handler) usbdrvwext_siwtxpow,               /* SIOCSIWTXPOW */
+    (iw_handler) usbdrvwext_giwtxpow,               /* SIOCGIWTXPOW */
+    (iw_handler) usbdrvwext_siwretry,               /* SIOCSIWRETRY */
+    (iw_handler) usbdrvwext_giwretry,               /* SIOCGIWRETRY */
+    (iw_handler) usbdrvwext_siwencode,              /* SIOCSIWENCODE */
+    (iw_handler) usbdrvwext_giwencode,              /* SIOCGIWENCODE */
+    (iw_handler) usbdrvwext_siwpower,               /* SIOCSIWPOWER */
+    (iw_handler) usbdrvwext_giwpower,               /* SIOCGIWPOWER */
+};
+
+static const iw_handler usbdrv_private_handler[] =
+{
+	//(iw_handler) usbdrvwext_setparam,		/* SIOCWFIRSTPRIV+0 */
+	//(iw_handler) usbdrvwext_getparam,		/* SIOCWFIRSTPRIV+1 */
+	//(iw_handler) usbdrvwext_setkey,		    /* SIOCWFIRSTPRIV+2 */
+	//(iw_handler) usbdrvwext_setwmmparams,	/* SIOCWFIRSTPRIV+3 */
+	//(iw_handler) usbdrvwext_delkey,		    /* SIOCWFIRSTPRIV+4 */
+	//(iw_handler) usbdrvwext_getwmmparams,	/* SIOCWFIRSTPRIV+5 */
+	//(iw_handler) usbdrvwext_setmlme,		/* SIOCWFIRSTPRIV+6 */
+	//(iw_handler) usbdrvwext_getchaninfo,	/* SIOCWFIRSTPRIV+7 */
+	//(iw_handler) usbdrvwext_setoptie,		/* SIOCWFIRSTPRIV+8 */
+	//(iw_handler) usbdrvwext_getoptie,		/* SIOCWFIRSTPRIV+9 */
+	//(iw_handler) usbdrvwext_addmac,		    /* SIOCWFIRSTPRIV+10 */
+	//(iw_handler) usbdrvwext_getscanresults,	/* SIOCWFIRSTPRIV+11 */
+	//(iw_handler) usbdrvwext_delmac,		    /* SIOCWFIRSTPRIV+12 */
+	//(iw_handler) usbdrvwext_getchanlist,	/* SIOCWFIRSTPRIV+13 */
+	//(iw_handler) usbdrvwext_setchanlist,	/* SIOCWFIRSTPRIV+14 */
+	//(iw_handler) NULL,				        /* SIOCWFIRSTPRIV+15 */
+	//(iw_handler) usbdrvwext_chanswitch,	    /* SIOCWFIRSTPRIV+16 */
+	//(iw_handler) usbdrvwext_setmode,		/* SIOCWFIRSTPRIV+17 */
+	//(iw_handler) usbdrvwext_getmode,		/* SIOCWFIRSTPRIV+18 */
+    NULL,               /* SIOCIWFIRSTPRIV */
+};
+
+static struct iw_handler_def p80211wext_handler_def = {
+    .num_standard = sizeof(usbdrvwext_handler) / sizeof(iw_handler),
+    .num_private = sizeof(usbdrv_private_handler)/sizeof(iw_handler),
+    .num_private_args = sizeof(usbdrv_private_args)/sizeof(struct iw_priv_args),
+    .standard = usbdrvwext_handler,
+    .private = (iw_handler *) usbdrv_private_handler,
+    .private_args = (struct iw_priv_args *) usbdrv_private_args
+};
+#endif
+
+/* WDS */
+//struct zsWdsStruct wds[ZM_WDS_PORT_NUMBER];
+//void zfInitWdsStruct(void);
+
+/* VAP */
+struct zsVapStruct vap[ZM_VAP_PORT_NUMBER];
+void zfLnxInitVapStruct(void);
+
+
+/**
+ * usbdrv_intr - interrupt handler
+ * @irq: the IRQ number
+ * @dev_inst: the net_device struct
+ * @regs: registers (unused)
+ *
+ * This routine is the ISR for the usbdrv board. It services
+ * the RX & TX queues & starts the RU if it has stopped due
+ * to no resources.
+ */
+irqreturn_t usbdrv_intr(int irq, void *dev_inst, struct pt_regs *regs)
+{
+    struct net_device *dev;
+    struct usbdrv_private *macp;
+
+    dev = dev_inst;
+    macp = dev->ml_priv;
+
+
+    /* Read register error, card may be unpluged */
+    if (0)//(intr_status == -1)
+        return IRQ_NONE;
+
+    /* the device is closed, don't continue or else bad things may happen. */
+    if (!netif_running(dev)) {
+        return IRQ_NONE;
+    }
+
+    if (macp->driver_isolated) {
+        return IRQ_NONE;
+    }
+
+#if (WLAN_HOSTIF == WLAN_PCI)
+    //zfiIsrPci(dev);
+#endif
+
+    return IRQ_HANDLED;
+}
+
+int usbdrv_open(struct net_device *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    int rc = 0;
+    u16_t size;
+    void* mem;
+    //unsigned char addr[6];
+    struct zsCbFuncTbl cbFuncTbl;
+
+    printk("Enter open()\n");
+
+//#ifndef CONFIG_SMP
+//    read_lock(&(macp->isolate_lock));
+//#endif
+    if (macp->driver_isolated) {
+        rc = -EBUSY;
+        goto exit;
+    }
+
+    size = zfiGlobalDataSize(dev);
+    if ((mem = kmalloc(size, GFP_KERNEL)) == NULL)
+    {
+        rc = -EBUSY;
+        goto exit;
+    }
+    macp->wd = mem;
+
+    memset(&cbFuncTbl, 0, sizeof(struct zsCbFuncTbl));
+    cbFuncTbl.zfcbAuthNotify = zfLnxAuthNotify;
+    cbFuncTbl.zfcbAuthNotify = zfLnxAuthNotify;
+    cbFuncTbl.zfcbAsocNotify = zfLnxAsocNotify;
+    cbFuncTbl.zfcbDisAsocNotify = zfLnxDisAsocNotify;
+    cbFuncTbl.zfcbApConnectNotify = zfLnxApConnectNotify;
+    cbFuncTbl.zfcbConnectNotify = zfLnxConnectNotify;
+    cbFuncTbl.zfcbScanNotify = zfLnxScanNotify;
+    cbFuncTbl.zfcbMicFailureNotify = zfLnxMicFailureNotify;
+    cbFuncTbl.zfcbApMicFailureNotify = zfLnxApMicFailureNotify;
+    cbFuncTbl.zfcbIbssPartnerNotify = zfLnxIbssPartnerNotify;
+    cbFuncTbl.zfcbMacAddressNotify = zfLnxMacAddressNotify;
+    cbFuncTbl.zfcbSendCompleteIndication = zfLnxSendCompleteIndication;
+    cbFuncTbl.zfcbRecvEth = zfLnxRecvEth;
+    cbFuncTbl.zfcbRecv80211 = zfLnxRecv80211;
+    cbFuncTbl.zfcbRestoreBufData = zfLnxRestoreBufData;
+#ifdef ZM_ENABLE_CENC
+    cbFuncTbl.zfcbCencAsocNotify = zfLnxCencAsocNotify;
+#endif //ZM_ENABLE_CENC
+    cbFuncTbl.zfcbHwWatchDogNotify = zfLnxWatchDogNotify;
+    zfiWlanOpen(dev, &cbFuncTbl);
+
+#if 0
+    {
+        //u16_t mac[3] = {0x1300, 0xb6d4, 0x5aaf};
+        u16_t mac[3] = {0x8000, 0x00ab, 0x0000};
+        //zfiWlanSetMacAddress(dev, mac);
+    }
+    /* MAC address */
+    zfiWlanQueryMacAddress(dev, addr);
+    dev->dev_addr[0] = addr[0];
+    dev->dev_addr[1] = addr[1];
+    dev->dev_addr[2] = addr[2];
+    dev->dev_addr[3] = addr[3];
+    dev->dev_addr[4] = addr[4];
+    dev->dev_addr[5] = addr[5];
+#endif
+    //zfwMacAddressNotify() will be called to setup dev->dev_addr[]
+
+    zfLnxCreateThread(dev);
+
+    mod_timer(&(macp->hbTimer10ms), jiffies + (1*HZ)/100);   //10 ms
+
+    netif_carrier_on(dev);
+
+    netif_start_queue(dev);
+
+#if ZM_AP_MODE == 1
+    zfiWlanSetWlanMode(dev, ZM_MODE_AP);
+    zfiWlanSetBasicRate(dev, 0xf, 0, 0);
+    zfiWlanSetSSID(dev, "OTUS_CWY", 8);
+    zfiWlanSetDtimCount(dev, 3);
+
+  #if ZM_WEP_MOME == 1
+    {
+        u8_t key[16] = {0x12, 0x34, 0x56, 0x78, 0x90};
+        struct zsKeyInfo keyInfo;
+
+        keyInfo.keyLength = 5;
+        keyInfo.keyIndex = 0;
+        keyInfo.flag = 0;
+        keyInfo.key = key;
+        zfiWlanSetKey(dev, keyInfo);
+
+        zfiWlanSetEncryMode(dev, ZM_WEP64);
+    }
+
+    #if ZM_SHARE_AUTH == 1
+    zfiWlanSetAuthenticationMode(dev, 1);
+    #endif //#if ZM_SHARE_AUTH == 1
+  #endif //#if ZM_WEP_MOME == 1
+
+#elif ZM_PIBSS_MODE == 1
+    zfiWlanSetWlanMode(dev, ZM_MODE_PSEUDO);
+#else
+    zfiWlanSetWlanMode(dev, ZM_MODE_INFRASTRUCTURE);
+#endif
+    //zfiWlanSetChannel(dev, ZM_CHANNEL, FALSE);
+    zfiWlanSetFrequency(dev, 2462000, FALSE);
+    zfiWlanSetRtsThreshold(dev, 32767);
+    zfiWlanSetFragThreshold(dev, 0);
+
+    zfiWlanEnable(dev);
+
+#ifdef ZM_ENABLE_CENC
+    macp->netlink_sk = netlink_kernel_create(NETLINK_USERSOCK, 1, NULL, THIS_MODULE);
+
+    if (macp->netlink_sk == NULL)
+    {
+        printk(KERN_ERR "Can't create NETLINK socket\n");
+    }
+#endif
+
+    macp->DeviceOpened = 1;
+exit:
+//#ifndef CONFIG_SMP
+//    read_unlock(&(macp->isolate_lock));
+//#endif
+    //zfRegisterWdsDev(dev, 0);
+    //zfLnxRegisterVapDev(dev, 0);
+
+    return rc;
+}
+
+
+
+
+/**
+ * usbdrv_get_stats - get driver statistics
+ * @dev: adapter's net_device struct
+ *
+ * This routine is called when the OS wants the adapter's stats returned.
+ * It returns the address of the net_device_stats stucture for the device.
+ * If the statistics are currently being updated, then they might be incorrect
+ * for a short while. However, since this cannot actually cause damage, no
+ * locking is used.
+ */
+
+struct net_device_stats * usbdrv_get_stats(struct net_device *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    macp->drv_stats.net_stats.tx_errors =
+        macp->drv_stats.net_stats.tx_carrier_errors +
+        macp->drv_stats.net_stats.tx_aborted_errors;
+
+    macp->drv_stats.net_stats.rx_errors =
+        macp->drv_stats.net_stats.rx_crc_errors +
+        macp->drv_stats.net_stats.rx_frame_errors +
+        macp->drv_stats.net_stats.rx_length_errors;
+
+
+    return &(macp->drv_stats.net_stats);
+}
+
+
+/**
+ * usbdrv_set_mac - set the MAC address
+ * @dev: adapter's net_device struct
+ * @addr: the new address
+ *
+ * This routine sets the ethernet address of the board
+ * Returns:
+ * 0  - if successful
+ * -1 - otherwise
+ */
+
+int usbdrv_set_mac(struct net_device *dev, void *addr)
+{
+    struct usbdrv_private *macp;
+    int rc = -1;
+
+    macp = dev->ml_priv;
+    read_lock(&(macp->isolate_lock));
+
+    if (macp->driver_isolated) {
+        goto exit;
+    }
+
+    rc = 0;
+
+
+exit:
+    read_unlock(&(macp->isolate_lock));
+    return rc;
+}
+
+
+
+void
+usbdrv_isolate_driver(struct usbdrv_private *macp)
+{
+#ifndef CONFIG_SMP
+    write_lock_irq(&(macp->isolate_lock));
+#endif
+    macp->driver_isolated = TRUE;
+#ifndef CONFIG_SMP
+    write_unlock_irq(&(macp->isolate_lock));
+#endif
+
+    if (netif_running(macp->device))
+    {
+        netif_carrier_off(macp->device);
+        netif_stop_queue(macp->device);
+    }
+}
+
+#define VLAN_SIZE   	4
+int usbdrv_change_mtu(struct net_device *dev, int new_mtu)
+{
+    if ((new_mtu < 68) || (new_mtu > (ETH_DATA_LEN + VLAN_SIZE)))
+        return -EINVAL;
+
+    dev->mtu = new_mtu;
+    return 0;
+}
+
+void zfLnxUnlinkAllUrbs(struct usbdrv_private *macp);
+
+int usbdrv_close(struct net_device *dev)
+{
+extern void zfHpLedCtrl(struct net_device *dev, u16_t ledId, u8_t mode);
+
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    printk(KERN_DEBUG "usbdrv_close\n");
+
+    netif_carrier_off(macp->device);
+
+    del_timer_sync(&macp->hbTimer10ms);
+
+    printk(KERN_DEBUG "usbdrv_netif_carrier_off\n");
+
+    usbdrv_isolate_driver(macp);
+
+    printk(KERN_DEBUG "usbdrv_isolate_driver\n");
+
+    netif_carrier_off(macp->device);
+#ifdef ZM_ENABLE_CENC
+    /* CENC */
+    if (macp->netlink_sk != NULL)
+    {
+    //    sock_release(macp->netlink_sk);
+        printk(KERN_ERR "usbdrv close netlink socket\n");
+    }
+#endif //ZM_ENABLE_CENC
+#if (WLAN_HOSTIF == WLAN_PCI)
+    //free_irq(dev->irq, dev);
+#endif
+
+    /* Turn off LED */
+    zfHpLedCtrl(dev, 0, 0);
+    zfHpLedCtrl(dev, 1, 0);
+
+    /* Delay for a while */
+    mdelay(10);
+
+    /* clear WPA/RSN IE */
+    macp->supIe[1] = 0;
+
+    /* set the isolate flag to false, so usbdrv_open can be called */
+    macp->driver_isolated = FALSE;
+
+    zfiWlanClose(dev);
+    kfree(macp->wd);
+
+    zfLnxUnlinkAllUrbs(macp);
+
+    return 0;
+}
+
+
+
+
+int usbdrv_xmit_frame(struct sk_buff *skb, struct net_device *dev)
+{
+    int notify_stop = FALSE;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+#if 0
+    /* Test code */
+    {
+        struct sk_buff* s;
+
+        s = skb_copy_expand(skb, 8, 0, GFP_ATOMIC);
+        skb_push(s, 8);
+        s->data[0] = 'z';
+        s->data[1] = 'y';
+        s->data[2] = 'd';
+        s->data[3] = 'a';
+        s->data[4] = 's';
+        printk("len1=%d, len2=%d", skb->len, s->len);
+        netlink_broadcast(rtnl, s, 0, RTMGRP_LINK, GFP_ATOMIC);
+    }
+#endif
+
+#if ZM_DISABLE_XMIT
+    dev_kfree_skb_irq(skb);
+#else
+    zfiTxSendEth(dev, skb, 0);
+#endif
+    macp->drv_stats.net_stats.tx_bytes += skb->len;
+    macp->drv_stats.net_stats.tx_packets++;
+
+    //dev_kfree_skb_irq(skb);
+
+    if (notify_stop) {
+        netif_carrier_off(dev);
+        netif_stop_queue(dev);
+    }
+
+    return 0;
+}
+
+
+
+
+void usbdrv_set_multi(struct net_device *dev)
+{
+
+
+    if (!(dev->flags & IFF_UP))
+        return;
+
+        return;
+
+}
+
+
+
+/**
+ * usbdrv_clear_structs - free resources
+
+ * @dev: adapter's net_device struct
+ *
+ * Free all device specific structs, unmap i/o address, etc.
+ */
+void usbdrv_clear_structs(struct net_device *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+
+#if (WLAN_HOSTIF == WLAN_PCI)
+    iounmap(macp->regp);
+
+    pci_release_regions(macp->pdev);
+    pci_disable_device(macp->pdev);
+    pci_set_drvdata(macp->pdev, NULL);
+#endif
+
+    kfree(macp);
+
+    kfree(dev);
+
+}
+
+void usbdrv_remove1(struct pci_dev *pcid)
+{
+    struct net_device *dev;
+    struct usbdrv_private *macp;
+
+    if (!(dev = (struct net_device *) pci_get_drvdata(pcid)))
+        return;
+
+    macp = dev->ml_priv;
+    unregister_netdev(dev);
+
+    usbdrv_clear_structs(dev);
+}
+
+
+void zfLnx10msTimer(struct net_device* dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    mod_timer(&(macp->hbTimer10ms), jiffies + (1*HZ)/100);   //10 ms
+    zfiHeartBeat(dev);
+    return;
+}
+
+void zfLnxInitVapStruct(void)
+{
+    u16_t i;
+
+    for (i=0; i<ZM_VAP_PORT_NUMBER; i++)
+    {
+        vap[i].dev = NULL;
+        vap[i].openFlag = 0;
+    }
+}
+
+int zfLnxVapOpen(struct net_device *dev)
+{
+    u16_t vapId;
+
+    vapId = zfLnxGetVapId(dev);
+
+    if (vap[vapId].openFlag == 0)
+    {
+        vap[vapId].openFlag = 1;
+    	printk("zfLnxVapOpen : device name=%s, vap ID=%d\n", dev->name, vapId);
+    	zfiWlanSetSSID(dev, "vap1", 4);
+    	zfiWlanEnable(dev);
+    	netif_start_queue(dev);
+    }
+    else
+    {
+        printk("VAP opened error : vap ID=%d\n", vapId);
+    }
+	return 0;
+}
+
+int zfLnxVapClose(struct net_device *dev)
+{
+    u16_t vapId;
+
+    vapId = zfLnxGetVapId(dev);
+
+    if (vapId != 0xffff)
+    {
+        if (vap[vapId].openFlag == 1)
+        {
+            printk("zfLnxVapClose: device name=%s, vap ID=%d\n", dev->name, vapId);
+
+            netif_stop_queue(dev);
+            vap[vapId].openFlag = 0;
+        }
+        else
+        {
+            printk("VAP port was not opened : vap ID=%d\n", vapId);
+        }
+    }
+	return 0;
+}
+
+int zfLnxVapXmitFrame(struct sk_buff *skb, struct net_device *dev)
+{
+    int notify_stop = FALSE;
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t vapId;
+
+    vapId = zfLnxGetVapId(dev);
+    //printk("zfLnxVapXmitFrame: vap ID=%d\n", vapId);
+    //printk("zfLnxVapXmitFrame(), skb=%lxh\n", (u32_t)skb);
+
+    if (vapId >= ZM_VAP_PORT_NUMBER)
+    {
+        dev_kfree_skb_irq(skb);
+        return 0;
+    }
+#if 1
+    if (vap[vapId].openFlag == 0)
+    {
+        dev_kfree_skb_irq(skb);
+        return 0;
+    }
+#endif
+
+
+    zfiTxSendEth(dev, skb, 0x1);
+
+    macp->drv_stats.net_stats.tx_bytes += skb->len;
+    macp->drv_stats.net_stats.tx_packets++;
+
+    //dev_kfree_skb_irq(skb);
+
+    if (notify_stop) {
+        netif_carrier_off(dev);
+        netif_stop_queue(dev);
+    }
+
+    return 0;
+}
+
+int zfLnxRegisterVapDev(struct net_device* parentDev, u16_t vapId)
+{
+    /* Allocate net device structure */
+    vap[vapId].dev = alloc_etherdev(0);
+    printk("Register vap dev=%x\n", (u32_t)vap[vapId].dev);
+
+    if(vap[vapId].dev == NULL) {
+        printk("alloc_etherdev fail\n");
+        return -ENOMEM;
+    }
+
+    /* Setup the default settings */
+    ether_setup(vap[vapId].dev);
+
+    /* MAC address */
+    memcpy(vap[vapId].dev->dev_addr, parentDev->dev_addr, ETH_ALEN);
+
+    vap[vapId].dev->irq = parentDev->irq;
+    vap[vapId].dev->base_addr = parentDev->base_addr;
+    vap[vapId].dev->mem_start = parentDev->mem_start;
+    vap[vapId].dev->mem_end = parentDev->mem_end;
+    vap[vapId].dev->ml_priv = parentDev->ml_priv;
+
+    //dev->hard_start_xmit = &zd1212_wds_xmit_frame;
+    vap[vapId].dev->hard_start_xmit = &zfLnxVapXmitFrame;
+    vap[vapId].dev->open = &zfLnxVapOpen;
+    vap[vapId].dev->stop = &zfLnxVapClose;
+    vap[vapId].dev->get_stats = &usbdrv_get_stats;
+    vap[vapId].dev->change_mtu = &usbdrv_change_mtu;
+#ifdef ZM_HOSTAPD_SUPPORT
+    vap[vapId].dev->do_ioctl = usbdrv_ioctl;
+#else
+    vap[vapId].dev->do_ioctl = NULL;
+#endif
+    vap[vapId].dev->destructor = free_netdev;
+
+    vap[vapId].dev->tx_queue_len = 0;
+
+    vap[vapId].dev->dev_addr[0] = parentDev->dev_addr[0];
+    vap[vapId].dev->dev_addr[1] = parentDev->dev_addr[1];
+    vap[vapId].dev->dev_addr[2] = parentDev->dev_addr[2];
+    vap[vapId].dev->dev_addr[3] = parentDev->dev_addr[3];
+    vap[vapId].dev->dev_addr[4] = parentDev->dev_addr[4];
+    vap[vapId].dev->dev_addr[5] = parentDev->dev_addr[5] + (vapId+1);
+
+    /* Stop the network queue first */
+    netif_stop_queue(vap[vapId].dev);
+
+    sprintf(vap[vapId].dev->name, "vap%d", vapId);
+    printk("Register VAP dev success : %s\n", vap[vapId].dev->name);
+
+    if(register_netdevice(vap[vapId].dev) != 0) {
+        printk("register VAP device fail\n");
+        vap[vapId].dev = NULL;
+        return -EINVAL;
+    }
+
+    return 0;
+}
+
+int zfLnxUnregisterVapDev(struct net_device* parentDev, u16_t vapId)
+{
+    int ret = 0;
+
+    printk("Unregister VAP dev : %s\n", vap[vapId].dev->name);
+
+    if(vap[vapId].dev != NULL) {
+        printk("Unregister vap dev=%x\n", (u32_t)vap[vapId].dev);
+        //
+        //unregister_netdevice(wds[wdsId].dev);
+        unregister_netdev(vap[vapId].dev);
+
+        printk("VAP unregister_netdevice\n");
+        vap[vapId].dev = NULL;
+    }
+    else {
+        printk("unregister VAP device: %d fail\n", vapId);
+        ret = -EINVAL;
+    }
+
+    return ret;
+}
+
+
+
+#  define SUBMIT_URB(u,f)       usb_submit_urb(u,f)
+#  define USB_ALLOC_URB(u,f)    usb_alloc_urb(u,f)
+
+//extern void zfiWlanQueryMacAddress(zdev_t* dev, u8_t* addr);
+
+extern int usbdrv_open(struct net_device *dev);
+extern int usbdrv_close(struct net_device *dev);
+extern int usbdrv_xmit_frame(struct sk_buff *skb, struct net_device *dev);
+extern int usbdrv_xmit_frame(struct sk_buff *skb, struct net_device *dev);
+extern int usbdrv_change_mtu(struct net_device *dev, int new_mtu);
+extern void usbdrv_set_multi(struct net_device *dev);
+extern int usbdrv_set_mac(struct net_device *dev, void *addr);
+extern struct net_device_stats * usbdrv_get_stats(struct net_device *dev);
+extern int usbdrv_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd);
+extern UsbTxQ_t *zfLnxGetUsbTxBuffer(struct net_device *dev);
+
+int zfLnxAllocAllUrbs(struct usbdrv_private *macp)
+{
+    struct usb_interface *interface = macp->interface;
+    struct usb_host_interface *iface_desc = &interface->altsetting[0];
+
+    struct usb_endpoint_descriptor *endpoint;
+    int i;
+
+    /* descriptor matches, let's find the endpoints needed */
+    /* check out the endpoints */
+    for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i)
+    {
+        endpoint = &iface_desc->endpoint[i].desc;
+        if ((endpoint->bEndpointAddress & 0x80) &&
+            ((endpoint->bmAttributes & 3) == 0x02))
+        {
+            /* we found a bulk in endpoint */
+            printk(KERN_ERR "bulk in: wMaxPacketSize = %x\n", le16_to_cpu(endpoint->wMaxPacketSize));
+        }
+
+        if (((endpoint->bEndpointAddress & 0x80) == 0x00) &&
+            ((endpoint->bmAttributes & 3) == 0x02))
+        {
+            /* we found a bulk out endpoint */
+            printk(KERN_ERR "bulk out: wMaxPacketSize = %x\n", le16_to_cpu(endpoint->wMaxPacketSize));
+        }
+
+        if ((endpoint->bEndpointAddress & 0x80) &&
+            ((endpoint->bmAttributes & 3) == 0x03))
+        {
+            /* we found a interrupt in endpoint */
+            printk(KERN_ERR "interrupt in: wMaxPacketSize = %x\n", le16_to_cpu(endpoint->wMaxPacketSize));
+            printk(KERN_ERR "interrupt in: int_interval = %d\n", endpoint->bInterval);
+        }
+
+        if (((endpoint->bEndpointAddress & 0x80) == 0x00) &&
+            ((endpoint->bmAttributes & 3) == 0x03))
+        {
+            /* we found a interrupt out endpoint */
+            printk(KERN_ERR "interrupt out: wMaxPacketSize = %x\n", le16_to_cpu(endpoint->wMaxPacketSize));
+            printk(KERN_ERR "interrupt out: int_interval = %d\n", endpoint->bInterval);
+        }
+    }
+
+    /* Allocate all Tx URBs */
+    for (i = 0; i < ZM_MAX_TX_URB_NUM; i++)
+    {
+        macp->WlanTxDataUrb[i] = USB_ALLOC_URB(0, GFP_KERNEL);
+
+        if (macp->WlanTxDataUrb[i] == 0)
+        {
+            int j;
+
+            /* Free all urbs */
+            for (j = 0; j < i; j++)
+            {
+                usb_free_urb(macp->WlanTxDataUrb[j]);
+            }
+
+            return 0;
+        }
+    }
+
+    /* Allocate all Rx URBs */
+    for (i = 0; i < ZM_MAX_RX_URB_NUM; i++)
+    {
+        macp->WlanRxDataUrb[i] = USB_ALLOC_URB(0, GFP_KERNEL);
+
+        if (macp->WlanRxDataUrb[i] == 0)
+        {
+            int j;
+
+            /* Free all urbs */
+            for (j = 0; j < i; j++)
+            {
+                usb_free_urb(macp->WlanRxDataUrb[j]);
+            }
+
+            for (j = 0; j < ZM_MAX_TX_URB_NUM; j++)
+            {
+                usb_free_urb(macp->WlanTxDataUrb[j]);
+            }
+
+            return 0;
+        }
+    }
+
+    /* Allocate Register Read/Write USB */
+    macp->RegOutUrb = USB_ALLOC_URB(0, GFP_KERNEL);
+    macp->RegInUrb = USB_ALLOC_URB(0, GFP_KERNEL);
+
+    return 1;
+}
+
+void zfLnxFreeAllUrbs(struct usbdrv_private *macp)
+{
+    int i;
+
+    /* Free all Tx URBs */
+    for (i = 0; i < ZM_MAX_TX_URB_NUM; i++)
+    {
+        if (macp->WlanTxDataUrb[i] != NULL)
+        {
+            usb_free_urb(macp->WlanTxDataUrb[i]);
+        }
+    }
+
+    /* Free all Rx URBs */
+    for (i = 0; i < ZM_MAX_RX_URB_NUM; i++)
+    {
+        if (macp->WlanRxDataUrb[i] != NULL)
+        {
+            usb_free_urb(macp->WlanRxDataUrb[i]);
+        }
+    }
+
+    /* Free USB Register Read/Write URB */
+    usb_free_urb(macp->RegOutUrb);
+    usb_free_urb(macp->RegInUrb);
+}
+
+void zfLnxUnlinkAllUrbs(struct usbdrv_private *macp)
+{
+    int i;
+
+    /* Unlink all Tx URBs */
+    for (i = 0; i < ZM_MAX_TX_URB_NUM; i++)
+    {
+        if (macp->WlanTxDataUrb[i] != NULL)
+        {
+            usb_unlink_urb(macp->WlanTxDataUrb[i]);
+        }
+    }
+
+    /* Unlink all Rx URBs */
+    for (i = 0; i < ZM_MAX_RX_URB_NUM; i++)
+    {
+        if (macp->WlanRxDataUrb[i] != NULL)
+        {
+            usb_unlink_urb(macp->WlanRxDataUrb[i]);
+        }
+    }
+
+    /* Unlink USB Register Read/Write URB */
+    usb_unlink_urb(macp->RegOutUrb);
+
+    usb_unlink_urb(macp->RegInUrb);
+}
+
+u8_t zfLnxInitSetup(struct net_device *dev, struct usbdrv_private *macp)
+{
+    //unsigned char addr[6];
+
+    //init_MUTEX(&macp->ps_sem);
+    //init_MUTEX(&macp->reg_sem);
+    //init_MUTEX(&macp->bcn_sem);
+    //init_MUTEX(&macp->config_sem);
+
+    spin_lock_init(&(macp->cs_lock));
+#if 0
+    /* MAC address */
+    zfiWlanQueryMacAddress(dev, addr);
+    dev->dev_addr[0] = addr[0];
+    dev->dev_addr[1] = addr[1];
+    dev->dev_addr[2] = addr[2];
+    dev->dev_addr[3] = addr[3];
+    dev->dev_addr[4] = addr[4];
+    dev->dev_addr[5] = addr[5];
+#endif
+#if WIRELESS_EXT > 12
+    dev->wireless_handlers = (struct iw_handler_def *)&p80211wext_handler_def;
+#endif
+
+    dev->open = usbdrv_open;
+    dev->hard_start_xmit = usbdrv_xmit_frame;
+    dev->stop = usbdrv_close;
+    dev->change_mtu = &usbdrv_change_mtu;
+    dev->get_stats = usbdrv_get_stats;
+    dev->set_multicast_list = usbdrv_set_multi;
+    dev->set_mac_address = usbdrv_set_mac;
+    dev->do_ioctl = usbdrv_ioctl;
+
+    dev->flags |= IFF_MULTICAST;
+
+    dev->dev_addr[0] = 0x00;
+    dev->dev_addr[1] = 0x03;
+    dev->dev_addr[2] = 0x7f;
+    dev->dev_addr[3] = 0x11;
+    dev->dev_addr[4] = 0x22;
+    dev->dev_addr[5] = 0x33;
+
+    /* Initialize Heart Beat timer */
+    init_timer(&macp->hbTimer10ms);
+    macp->hbTimer10ms.data = (unsigned long)dev;
+    macp->hbTimer10ms.function = (void *)&zfLnx10msTimer;
+
+    /* Initialize WDS and VAP data structure */
+    //zfInitWdsStruct();
+    zfLnxInitVapStruct();
+
+    return 1;
+}
+
+u8_t zfLnxClearStructs(struct net_device *dev)
+{
+    u16_t ii;
+    u16_t TxQCnt;
+
+    TxQCnt = zfLnxCheckTxBufferCnt(dev);
+
+    printk(KERN_ERR "TxQCnt: %d\n", TxQCnt);
+
+    for(ii = 0; ii < TxQCnt; ii++)
+    {
+        UsbTxQ_t *TxQ = zfLnxGetUsbTxBuffer(dev);
+
+        printk(KERN_ERR "dev_kfree_skb_any\n");
+        /* Free buffer */
+        dev_kfree_skb_any(TxQ->buf);
+    }
+
+    return 0;
+}
diff --git a/drivers/staging/otus/usbdrv.h b/drivers/staging/otus/usbdrv.h
new file mode 100644
index 000000000000..a11b3b36a906
--- /dev/null
+++ b/drivers/staging/otus/usbdrv.h
@@ -0,0 +1,252 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : usbdrv.h                                              */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains network interface up/down related definition*/
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _USBDRV_H
+#define _USBDRV_H
+
+#define WLAN_USB    0
+#define WLAN_PCI    1
+
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/skbuff.h>
+#include <linux/uaccess.h>
+#include <linux/wireless.h>
+#include <linux/if_arp.h>
+#include <linux/io.h>
+
+#include "zdcompat.h"
+
+#include "oal_dt.h"
+#include "oal_marc.h"
+#include "80211core/pub_zfi.h"
+//#include "pub_zfw.h"
+#include "80211core/pub_usb.h"
+
+#include <linux/usb.h>
+/* Please include header files for device type in the beginning of this file */
+#define urb_t                       struct urb
+
+#define usb_complete_t              usb_complete_t
+#define pipe_t                      u32_t
+
+/* USB Endpoint definition */
+#define USB_WLAN_TX_PIPE                    1
+#define USB_WLAN_RX_PIPE                    2
+#define USB_REG_IN_PIPE                     3
+#define USB_REG_OUT_PIPE                    4
+
+#if (WLAN_HOSTIF == WLAN_USB)
+#include <linux/usb.h>
+#endif
+
+#ifdef ZM_HOSTAPD_SUPPORT
+#include "athr_common.h"
+#endif
+
+/**************************************************************************
+**		Descriptor Data Structure
+***************************************************************************/
+struct driver_stats {
+	struct net_device_stats net_stats;
+};
+
+#define ZM_MAX_RX_BUFFER_SIZE               8192
+
+#if ZM_USB_TX_STREAM_MODE == 1
+#define ZM_MAX_TX_AGGREGATE_NUM             4
+#define ZM_USB_TX_BUF_SIZE                  8096
+#define ZM_MAX_TX_URB_NUM                   4
+#else
+#define ZM_USB_TX_BUF_SIZE                  2048
+#define ZM_MAX_TX_URB_NUM                   8
+#endif
+#define ZM_USB_REG_MAX_BUF_SIZE             64
+#define ZM_MAX_RX_URB_NUM                   16
+#define ZM_MAX_TX_BUF_NUM                   128
+
+typedef struct UsbTxQ
+{
+    zbuf_t *buf;
+    u8_t hdr[80];
+    u16_t hdrlen;
+    u8_t snap[8];
+    u16_t snapLen;
+    u8_t tail[16];
+    u16_t tailLen;
+    u16_t offset;
+} UsbTxQ_t;
+
+
+struct zdap_ioctl {
+	u16_t cmd;                /* Command to run */
+	u32_t addr;                /* Length of the data buffer */
+	u32_t value;              /* Pointer to the data buffer */
+	u8_t	data[0x100];
+};
+
+#define ZM_OAL_MAX_STA_SUPPORT 16
+
+struct usbdrv_private
+{
+	//linux used
+	struct net_device 	*device;
+#if (WLAN_HOSTIF == WLAN_PCI)
+	struct pci_dev 		*pdev;
+#endif
+#if (WLAN_HOSTIF == WLAN_USB)
+	struct usb_device	*udev;
+	struct usb_interface    *interface;
+#endif
+	struct driver_stats drv_stats;
+	char ifname[IFNAMSIZ];
+	int	                using_dac;
+	u8_t			rev_id;		/* adapter PCI revision ID */
+	rwlock_t 		isolate_lock;
+    spinlock_t      cs_lock;
+	int 			driver_isolated;
+#if (WLAN_HOSTIF == WLAN_PCI)
+	void			*regp;
+#endif
+
+        /* timer for heart beat */
+	struct timer_list hbTimer10ms;
+
+	/* For driver core */
+	void* wd;
+
+#if (WLAN_HOSTIF == WLAN_USB)
+	u8_t                    txUsbBuf[ZM_MAX_TX_URB_NUM][ZM_USB_TX_BUF_SIZE];
+	u8_t                    regUsbReadBuf[ZM_USB_REG_MAX_BUF_SIZE];
+	u8_t                    regUsbWriteBuf[ZM_USB_REG_MAX_BUF_SIZE];
+	urb_t			*WlanTxDataUrb[ZM_MAX_TX_URB_NUM];
+	urb_t			*WlanRxDataUrb[ZM_MAX_RX_URB_NUM];
+	urb_t			*RegOutUrb;
+	urb_t			*RegInUrb;
+	UsbTxQ_t                UsbTxBufQ[ZM_MAX_TX_BUF_NUM];
+	zbuf_t                  *UsbRxBufQ[ZM_MAX_RX_URB_NUM];
+        u16_t                   TxBufHead;
+        u16_t                   TxBufTail;
+        u16_t                   TxBufCnt;
+        u16_t                   TxUrbHead;
+        u16_t                   TxUrbTail;
+        u16_t                   TxUrbCnt;
+        u16_t                   RxBufHead;
+        u16_t                   RxBufTail;
+        u16_t                   RxBufCnt;
+#endif
+
+#if ZM_USB_STREAM_MODE == 1
+        zbuf_t                  *reamin_buf;
+#endif
+
+#ifdef ZM_HOSTAPD_SUPPORT
+        struct athr_wlan_param  athr_wpa_req;
+#endif
+        struct sock             *netlink_sk;
+        u8_t            DeviceOpened; //CWYang(+)
+        u8_t            supIe[50];
+        u8_t            supLen;
+        struct ieee80211req_wpaie stawpaie[ZM_OAL_MAX_STA_SUPPORT];
+        u8_t            forwardMgmt;
+
+        struct zfCbUsbFuncTbl usbCbFunctions;
+
+        /* For keventd */
+        u32_t                   flags;
+        unsigned long           kevent_flags;
+        u16_t                   kevent_ready;
+
+        struct semaphore        ioctl_sem;
+        struct work_struct      kevent;
+        wait_queue_head_t       wait_queue_event;
+#ifdef ZM_HALPLUS_LOCK
+        unsigned long           hal_irqFlag;
+#endif
+        u16_t                   adapterState;
+};
+
+/* WDS */
+#define ZM_WDS_PORT_NUMBER  6
+
+struct zsWdsStruct
+{
+    struct net_device* dev;
+    u16_t openFlag;
+};
+
+/* VAP */
+#define ZM_VAP_PORT_NUMBER  7
+
+struct zsVapStruct
+{
+    struct net_device* dev;
+    u16_t openFlag;
+};
+
+/***************************************/
+
+#define ZM_IOCTL_REG_READ			0x01
+#define ZM_IOCTL_REG_WRITE			0x02
+#define ZM_IOCTL_MEM_DUMP			0x03
+#define ZM_IOCTL_REG_DUMP			0x05
+#define ZM_IOCTL_TXD_DUMP 			0x06
+#define ZM_IOCTL_RXD_DUMP 			0x07
+#define ZM_IOCTL_MEM_READ			0x0B
+#define ZM_IOCTL_MEM_WRITE			0x0C
+#define ZM_IOCTL_DMA_TEST           0x10
+#define ZM_IOCTL_REG_TEST           0x11
+#define ZM_IOCTL_TEST               0x80
+#define ZM_IOCTL_TALLY              0x81 //CWYang(+)
+#define ZM_IOCTL_RTS                0xA0
+#define ZM_IOCTL_MIX_MODE           0xA1
+#define ZM_IOCTL_FRAG               0xA2
+#define ZM_IOCTL_SCAN               0xA3
+#define ZM_IOCTL_KEY                0xA4
+#define ZM_IOCTL_RATE               0xA5
+#define ZM_IOCTL_ENCRYPTION_MODE    0xA6
+#define ZM_IOCTL_GET_TXCNT          0xA7
+#define ZM_IOCTL_GET_DEAGG_CNT      0xA8
+#define ZM_IOCTL_DURATION_MODE      0xA9
+#define ZM_IOCTL_SET_AES_KEY        0xAA
+#define ZM_IOCTL_SET_AES_MODE       0xAB
+#define ZM_IOCTL_SIGNAL_STRENGTH    0xAC //CWYang(+)
+#define ZM_IOCTL_SIGNAL_QUALITY     0xAD //CWYang(+)
+#define ZM_IOCTL_SET_PIBSS_MODE     0xAE
+
+#define	ZDAPIOCTL				SIOCDEVPRIVATE
+
+enum devState {
+    Opened,
+    Enabled,
+    Disabled,
+    Closed
+};
+
+#endif	/* _USBDRV_H */
+
diff --git a/drivers/staging/otus/wrap_buf.c b/drivers/staging/otus/wrap_buf.c
new file mode 100644
index 000000000000..62496a0f8e3f
--- /dev/null
+++ b/drivers/staging/otus/wrap_buf.c
@@ -0,0 +1,114 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : wrap_buf.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for buffer management     */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+
+
+/* Called to allocate buffer, must return a continue buffer space */
+zbuf_t* zfwBufAllocate(zdev_t* dev, u16_t len)
+{
+    zbuf_t* buf;
+
+    /* Allocate SKB for packet*/
+    buf = dev_alloc_skb(len);
+
+    return buf;
+}
+
+
+/* Called to free buffer, replace below 3 functions */
+void zfwBufFree(zdev_t* dev, zbuf_t* buf, u16_t status)
+{
+    dev_kfree_skb_any(buf);
+}
+
+/* Called to adjust buffer size and head pointer */
+u16_t zfwBufRemoveHead(zdev_t* dev, zbuf_t* buf, u16_t size)
+{
+    //zm_assert(buf->len > size);
+
+    buf->data += size;
+    buf->len -= size;
+    return 0;
+}
+
+
+
+
+/* return tail if head==NULL, called to chain multiple buffer together */
+/* Used to chain Rx buffer to form a frame. if the prepared Rx buffer  */
+/* is greater than an ethernet frame(1518+32 byte), then this function    */
+/* will only be called with head=NULL.                                 */
+u16_t zfwBufChain(zdev_t* dev, zbuf_t** head, zbuf_t* tail)
+{
+
+    *head = tail;
+    return 0;
+}
+
+
+/* Called when doing infra-bss forwarding */
+u16_t zfwBufCopy(zdev_t* dev, zbuf_t* dst, zbuf_t* src)
+{
+    memcpy(dst->data, src->data, src->len);
+    dst->tail = dst->data;
+    skb_put(dst, src->len);
+    return 0;
+}
+
+
+/* Called to adjust buffer size and tail pointer */
+u16_t zfwBufSetSize(zdev_t* dev, zbuf_t* buf, u16_t size)
+{
+#ifdef NET_SKBUFF_DATA_USES_OFFSET
+    buf->tail = 0;
+    buf->len = 0;
+#else
+    buf->tail = buf->data;
+    buf->len = 0;
+#endif
+
+    skb_put(buf, size);
+    return 0;
+}
+
+u16_t zfwBufGetSize(zdev_t* dev, zbuf_t* buf)
+{
+    return buf->len;
+}
+
+void zfwCopyBufContext(zdev_t* dev, zbuf_t* source, zbuf_t* dst)
+{
+}
diff --git a/drivers/staging/otus/wrap_dbg.c b/drivers/staging/otus/wrap_dbg.c
new file mode 100644
index 000000000000..53763d9d2d85
--- /dev/null
+++ b/drivers/staging/otus/wrap_dbg.c
@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : wrap_dbg.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for debug functions       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+void zfwDumpBuf(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t i;
+
+    for (i=0; i<buf->len; i++)
+    {
+        printk("%02x ", *(((u8_t*)buf->data)+i));
+        if ((i&0xf)==0xf)
+        {
+            printk("\n");
+        }
+    }
+    printk("\n");
+}
+
+
+void zfwDbgReadRegDone(zdev_t* dev, u32_t addr, u32_t val)
+{
+    printk("Read addr:%x = %x\n", addr, val);
+}
+
+void zfwDbgWriteRegDone(zdev_t* dev, u32_t addr, u32_t val)
+{
+    printk("Write addr:%x = %x\n", addr, val);
+}
+
+void zfwDbgReadTallyDone(zdev_t* dev)
+{
+    //printk("Read Tall Done\n");
+}
+
+void zfwDbgWriteEepromDone(zdev_t* dev, u32_t addr, u32_t val)
+{
+}
+
+void zfwDbgQueryHwTxBusyDone(zdev_t* dev, u32_t val)
+{
+}
+
+//For Evl ++
+void zfwDbgReadFlashDone(zdev_t* dev, u32_t addr, u32_t* rspdata, u32_t datalen)
+{
+    printk("Read Flash addr:%x length:%x\n", addr, datalen);
+}
+
+void zfwDbgProgrameFlashDone(zdev_t* dev)
+{
+    printk("Program Flash Done\n");
+}
+
+void zfwDbgProgrameFlashChkDone(zdev_t* dev)
+{
+    printk("Program Flash Done\n");
+}
+
+void zfwDbgGetFlashChkSumDone(zdev_t* dev, u32_t* rspdata)
+{
+    printk("Get Flash ChkSum Done\n");
+}
+
+void zfwDbgDownloadFwInitDone(zdev_t* dev)
+{
+    printk("Download FW Init Done\n");
+}
+//For Evl --
+
+/* Leave an empty line below to remove warning message on some compiler */
diff --git a/drivers/staging/otus/wrap_ev.c b/drivers/staging/otus/wrap_ev.c
new file mode 100644
index 000000000000..966b787eef62
--- /dev/null
+++ b/drivers/staging/otus/wrap_ev.c
@@ -0,0 +1,283 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : wrap_ev.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for events                */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+
+/***** Management *****/
+u16_t zfLnxAuthNotify(zdev_t* dev, u16_t* macAddr)
+{
+    return 0;
+}
+
+u16_t zfLnxAsocNotify(zdev_t* dev, u16_t* macAddr, u8_t* body, u16_t bodySize, u16_t port)
+{
+//#ifdef ZM_HOSTAPD_SUPPORT
+    struct usbdrv_private *macp = dev->ml_priv;
+    union iwreq_data wreq;
+    u8_t *addr = (u8_t *) macAddr;
+    u16_t i, j;
+
+    memset(&wreq, 0, sizeof(wreq));
+    memcpy(wreq.addr.sa_data, macAddr, ETH_ALEN);
+    wreq.addr.sa_family = ARPHRD_ETHER;
+    printk(KERN_DEBUG "join_event of MAC: %02x:%02x:%02x:%02x:%02x:%02x\n",
+            addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
+
+    for(i = 0; i < ZM_OAL_MAX_STA_SUPPORT; i++)
+    {
+        for(j = 0; j < IEEE80211_ADDR_LEN; j++)
+        {
+            if ((macp->stawpaie[i].wpa_macaddr[j] != 0) &&
+                (macp->stawpaie[i].wpa_macaddr[j] != addr[j]))
+                break;
+        }
+        if (j == 6)
+            break;
+    }
+    if (i < ZM_OAL_MAX_STA_SUPPORT)
+    {
+        //printk("zfwAsocNotify - store wpa ie in macp, index = %d\n", i);
+        memcpy(macp->stawpaie[i].wpa_macaddr, macAddr, IEEE80211_ADDR_LEN);
+        memcpy(macp->stawpaie[i].wpa_ie, body, bodySize);
+    }
+    //if(macp->cardSetting.BssType == INFRASTRUCTURE_BSS) {
+    //            //wireless_send_event(macp->device, SIOCGIWSCAN, &wreq, NULL);
+    //    wireless_send_event(macp->device, SIOCGIWAP, &wreq, NULL);
+    //}
+#if WIRELESS_EXT >= 15
+    //else if(macp->cardSetting.BssType == AP_BSS) {
+//        if (port == 0)
+//        {
+            wireless_send_event(dev, IWEVREGISTERED, &wreq, NULL);
+//        }
+//        else
+//        {
+//            /* Check whether the VAP device is valid */
+//            if (vap[port].dev != NULL)
+//            {
+//                wireless_send_event(vap[port].dev, IWEVREGISTERED, &wreq, NULL);
+//            }
+//            else
+//            {
+//                printk(KERN_ERR "Can' find a valid VAP device, port: %d\n", port);
+//            }
+//        }
+    //}
+#endif
+//#endif
+
+    return 0;
+}
+
+
+/* Notification that a STA is disassociated from AP */
+/* AP mode only */
+u16_t zfLnxDisAsocNotify(zdev_t* dev, u8_t* macAddr, u16_t port)
+{
+    union iwreq_data wreq;
+    u8_t *addr = (u8_t *) macAddr;
+
+    memset(&wreq, 0, sizeof(wreq));
+    memcpy(wreq.addr.sa_data, macAddr, ETH_ALEN);
+    wreq.addr.sa_family = ARPHRD_ETHER;
+    printk(KERN_DEBUG "zfwDisAsocNotify(), MAC: %02x:%02x:%02x:%02x:%02x:%02x\n",
+            addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
+
+
+    return 0;
+}
+
+/* Notification that a STA is connect to AP */
+/* AP mode only */
+u16_t zfLnxApConnectNotify(zdev_t* dev, u8_t* macAddr, u16_t port)
+{
+    union iwreq_data wreq;
+    u8_t *addr = (u8_t *) macAddr;
+
+    memset(&wreq, 0, sizeof(wreq));
+    memcpy(wreq.addr.sa_data, macAddr, ETH_ALEN);
+    wreq.addr.sa_family = ARPHRD_ETHER;
+    printk(KERN_DEBUG "zfwApConnectNotify(), MAC: %02x:%02x:%02x:%02x:%02x:%02x\n",
+            addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
+
+
+    return 0;
+}
+
+
+
+void zfLnxConnectNotify(zdev_t* dev, u16_t status, u16_t* bssid)
+{
+    union iwreq_data wreq;
+    u8_t *addr = (u8_t *) bssid;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if (bssid != NULL)
+    {
+        memset(&wreq, 0, sizeof(wreq));
+        if (status == ZM_STATUS_MEDIA_CONNECT)
+            memcpy(wreq.addr.sa_data, bssid, ETH_ALEN);
+        wreq.addr.sa_family = ARPHRD_ETHER;
+
+        if (status == ZM_STATUS_MEDIA_CONNECT)
+        {
+#ifdef ZM_CONFIG_BIG_ENDIAN
+            printk(KERN_DEBUG "Connected to AP, MAC: %02x:%02x:%02x:%02x:%02x:%02x\n",
+                    addr[1], addr[0], addr[3], addr[2], addr[5], addr[4]);
+#else
+            printk(KERN_DEBUG "Connected to AP, MAC: %02x:%02x:%02x:%02x:%02x:%02x\n",
+                    addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
+#endif
+
+            netif_start_queue(dev);
+        }
+        else if ((status == ZM_STATUS_MEDIA_DISCONNECT) ||
+                 (status == ZM_STATUS_MEDIA_DISABLED) ||
+                 (status == ZM_STATUS_MEDIA_CONNECTION_DISABLED) ||
+	         (status == ZM_STATUS_MEDIA_CONNECTION_RESET) ||
+	         (status == ZM_STATUS_MEDIA_RESET) ||
+	         (status == ZM_STATUS_MEDIA_DISCONNECT_DEAUTH) ||
+	         (status == ZM_STATUS_MEDIA_DISCONNECT_DISASOC) ||
+	         (status == ZM_STATUS_MEDIA_DISCONNECT_BEACON_MISS) ||
+                 (status == ZM_STATUS_MEDIA_DISCONNECT_NOT_FOUND) ||
+	         (status == ZM_STATUS_MEDIA_DISCONNECT_TIMEOUT))
+        {
+            printk(KERN_DEBUG "Disconnection Notify\n");
+
+            netif_stop_queue(dev);
+        }
+
+	/* Save the connected status */
+	macp->adapterState = status;
+
+        if(zfiWlanQueryWlanMode(dev) == ZM_MODE_INFRASTRUCTURE) {
+        //            //wireless_send_event(dev, SIOCGIWSCAN, &wreq, NULL);
+            wireless_send_event(dev, SIOCGIWAP, &wreq, NULL);
+        }
+#if WIRELESS_EXT >= 15
+        else if(zfiWlanQueryWlanMode(dev) == ZM_MODE_AP) {
+            //if (port == 0)
+            //{
+                wireless_send_event(dev, IWEVREGISTERED, &wreq, NULL);
+            //}
+            //else
+            //{
+            //    /* Check whether the VAP device is valid */
+            //    if (vap[port].dev != NULL)
+            //    {
+            //        wireless_send_event(vap[port].dev, IWEVREGISTERED, &wreq, NULL);
+            //    }
+            //    else
+            //    {
+            //        printk(KERN_ERR "Can' find a valid VAP device, port: %d\n", port);
+            //    }
+            //}
+        }
+#endif
+    }
+    //return 0;
+}
+
+void zfLnxScanNotify(zdev_t* dev, struct zsScanResult* result)
+{
+    return;
+}
+
+void zfLnxStatisticsNotify(zdev_t* dev, struct zsStastics* result)
+{
+    return;
+}
+
+//void zfwMicFailureNotify(zdev_t* dev, u8_t* message, u16_t event)
+void zfLnxMicFailureNotify(zdev_t* dev, u16_t* addr, u16_t status)
+{
+	static const char *tag = "MLME-MICHAELMICFAILURE.indication";
+	union iwreq_data wrqu;
+	char buf[128];
+
+	/* TODO: needed parameters: count, type, src address */
+	//snprintf(buf, sizeof(buf), "%s(%scast addr=%s)", tag,
+	//    (status == ZM_MIC_GROUP_ERROR) ?  "broad" : "uni",
+	//    ether_sprintf((u8_t *)addr));
+
+	if (zfiWlanQueryWlanMode(dev) == ZM_MODE_INFRASTRUCTURE)
+	{
+		strcpy(buf, tag);
+	}
+
+	memset(&wrqu, 0, sizeof(wrqu));
+	wrqu.data.length = strlen(buf);
+	wireless_send_event(dev, IWEVCUSTOM, &wrqu, buf);
+}
+
+
+void zfLnxApMicFailureNotify(zdev_t* dev, u8_t* addr, zbuf_t* buf)
+{
+    union iwreq_data wreq;
+
+    memset(&wreq, 0, sizeof(wreq));
+    memcpy(wreq.addr.sa_data, addr, ETH_ALEN);
+    wreq.addr.sa_family = ARPHRD_ETHER;
+    printk(KERN_DEBUG "zfwApMicFailureNotify(), MAC: %02x:%02x:%02x:%02x:%02x:%02x\n",
+            addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
+
+    return;
+}
+
+// status = 0  => partner lost
+//        = 1  => partner alive
+//void zfwIbssPartnerNotify(zdev_t* dev, u8_t status)
+void zfLnxIbssPartnerNotify(zdev_t* dev, u16_t status, struct zsPartnerNotifyEvent *event)
+{
+}
+
+void zfLnxMacAddressNotify(zdev_t* dev, u8_t* addr)
+{
+    dev->dev_addr[0] = addr[0];
+    dev->dev_addr[1] = addr[1];
+    dev->dev_addr[2] = addr[2];
+    dev->dev_addr[3] = addr[3];
+    dev->dev_addr[4] = addr[4];
+    dev->dev_addr[5] = addr[5];
+}
+
+void zfLnxSendCompleteIndication(zdev_t* dev, zbuf_t* buf)
+{
+}
+
+
+void zfLnxRestoreBufData(zdev_t* dev, zbuf_t* buf) {
+
+}
+/* Leave an empty line below to remove warning message on some compiler */
diff --git a/drivers/staging/otus/wrap_mem.c b/drivers/staging/otus/wrap_mem.c
new file mode 100644
index 000000000000..8081bb2f8878
--- /dev/null
+++ b/drivers/staging/otus/wrap_mem.c
@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : wrap_mem.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for memory management     */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+/* Memory management */
+/* Called to allocate uncached memory, allocated memory must    */
+/* in 4-byte boundary                                           */
+void* zfwMemAllocate(zdev_t* dev, u32_t size)
+{
+    void* mem = NULL;
+    mem = kmalloc(size, GFP_ATOMIC);
+    return mem;
+}
+
+
+/* Called to free allocated memory */
+void zfwMemFree(zdev_t* dev, void* mem, u32_t size)
+{
+    kfree(mem);
+    return;
+}
+
+void zfwMemoryCopy(u8_t* dst, u8_t* src, u16_t length)
+{
+    //u16_t i;
+
+    memcpy(dst, src, length);
+    //for(i=0; i<length; i++)
+    //{
+    //    dst[i] = src[i];
+    //}
+    return;
+}
+
+void zfwZeroMemory(u8_t* va, u16_t length)
+{
+    //u16_t i;
+    memset(va, 0, length);
+    //for(i=0; i<length; i++)
+    //{
+    //    va[i] = 0;
+    //}
+    return;
+}
+
+void zfwMemoryMove(u8_t* dst, u8_t* src, u16_t length)
+{
+    memcpy(dst, src, length);
+    return;
+}
+
+u8_t zfwMemoryIsEqual(u8_t* m1, u8_t* m2, u16_t length)
+{
+    //u16_t i;
+    int ret;
+
+    ret = memcmp(m1, m2, length);
+
+    return ((ret==0)?TRUE:FALSE);
+    //for(i=0; i<length; i++)
+    //{
+    //    if ( m1[i] != m2[i] )
+    //    {
+    //        return FALSE;
+    //    }
+    //}
+
+    //return TRUE;
+}
+
+/* Leave an empty line below to remove warning message on some compiler */
diff --git a/drivers/staging/otus/wrap_mis.c b/drivers/staging/otus/wrap_mis.c
new file mode 100644
index 000000000000..337918b9de9a
--- /dev/null
+++ b/drivers/staging/otus/wrap_mis.c
@@ -0,0 +1,109 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : wrap_mis.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for misc functions        */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+//extern struct zsWdsStruct wds[ZM_WDS_PORT_NUMBER];
+extern struct zsVapStruct vap[ZM_VAP_PORT_NUMBER];
+extern u16_t zfLnxGetVapId(zdev_t* dev);
+
+/* Simply return 0xffff if VAP function is not supported */
+u16_t zfwGetVapId(zdev_t* dev)
+{
+    return zfLnxGetVapId(dev);
+}
+
+void zfwSleep(zdev_t* dev, u32_t ms)
+{
+    if (in_interrupt() == 0)
+    {
+        mdelay(ms);
+    }
+    else
+    {
+        int ii;
+        int iter = 100000 * ms;
+
+        for (ii = 0; ii < iter; ii++)
+        {
+
+        }
+    }
+}
+
+#ifdef ZM_HALPLUS_LOCK
+asmlinkage struct zsWlanDev *zfwGetWlanDev(zdev_t* dev)
+{
+	struct usbdrv_private *macp = dev->ml_priv;
+	return macp->wd;
+}
+
+asmlinkage void zfwEnterCriticalSection(zdev_t* dev)
+{
+	struct usbdrv_private *macp = dev->ml_priv;
+	spin_lock_irqsave(&macp->cs_lock, macp->hal_irqFlag);
+}
+
+asmlinkage void zfwLeaveCriticalSection(zdev_t* dev)
+{
+	struct usbdrv_private *macp = dev->ml_priv;
+	spin_unlock_irqrestore(&macp->cs_lock, macp->hal_irqFlag);
+}
+
+asmlinkage u8_t zfwBufReadByte(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    return *(u8_t*)((u8_t*)buf->data+offset);
+}
+
+asmlinkage u16_t zfwBufReadHalfWord(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    return zmw_cpu_to_le16(*(u16_t*)((u8_t*)buf->data+offset));
+}
+
+asmlinkage void zfwBufWriteByte(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t value)
+{
+    *(u8_t*)((u8_t*)buf->data+offset) = value;
+}
+
+asmlinkage void zfwBufWriteHalfWord(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t value)
+{
+    *(u16_t*)((u8_t*)buf->data+offset) = zmw_cpu_to_le16(value);
+}
+
+asmlinkage u8_t *zfwGetBuffer(zdev_t* dev, zbuf_t* buf)
+{
+    return (u8_t*)(buf->data);
+}
+#endif
+
+/* Leave an empty line below to remove warning message on some compiler */
diff --git a/drivers/staging/otus/wrap_pkt.c b/drivers/staging/otus/wrap_pkt.c
new file mode 100644
index 000000000000..5db0004c8739
--- /dev/null
+++ b/drivers/staging/otus/wrap_pkt.c
@@ -0,0 +1,173 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : wrap_pkt.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for packet handling       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+
+
+//extern struct zsWdsStruct wds[ZM_WDS_PORT_NUMBER];
+extern struct zsVapStruct vap[ZM_VAP_PORT_NUMBER];
+
+
+/***** Rx *****/
+void zfLnxRecv80211(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo)
+{
+    u16_t frameType;
+    u16_t frameCtrl;
+    u16_t frameSubtype;
+    zbuf_t *skb1;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    //frameCtrl = zmw_buf_readb(dev, buf, 0);
+    frameCtrl = *(u8_t*)((u8_t*)buf->data);
+    frameType = frameCtrl & 0xf;
+    frameSubtype = frameCtrl & 0xf0;
+
+    if ((frameType == 0x0) && (macp->forwardMgmt))
+    {
+        switch (frameSubtype)
+        {
+                /* Beacon */
+            case 0x80 :
+                /* Probe response */
+            case 0x50 :
+                skb1 = skb_copy(buf, GFP_ATOMIC);
+                if(skb1 != NULL)
+                {
+                    skb1->dev = dev;
+                    skb1->mac_header = skb1->data;
+	            skb1->ip_summed = CHECKSUM_NONE;
+	            skb1->pkt_type = PACKET_OTHERHOST;
+	            skb1->protocol = __constant_htons(0x0019);  /* ETH_P_80211_RAW */
+    	            netif_rx(skb1);
+	            }
+                break;
+            default:
+                break;
+        }
+    }
+
+    zfiRecv80211(dev, buf, addInfo);
+    return;
+}
+
+#define ZM_AVOID_UDP_LARGE_PACKET_FAIL
+void zfLnxRecvEth(zdev_t* dev, zbuf_t* buf, u16_t port)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+#ifdef ZM_AVOID_UDP_LARGE_PACKET_FAIL
+    zbuf_t *new_buf;
+
+    //new_buf = dev_alloc_skb(2048);
+    new_buf = dev_alloc_skb(buf->len);
+
+#ifdef NET_SKBUFF_DATA_USES_OFFSET
+    new_buf->tail = 0;
+    new_buf->len = 0;
+#else
+    new_buf->tail = new_buf->data;
+    new_buf->len = 0;
+#endif
+
+    skb_put(new_buf, buf->len);
+    memcpy(new_buf->data, buf->data, buf->len);
+
+    /* Free buffer */
+    dev_kfree_skb_any(buf);
+
+    if (port == 0)
+    {
+        new_buf->dev = dev;
+        new_buf->protocol = eth_type_trans(new_buf, dev);
+    }
+    else
+    {
+        /* VAP */
+        if (vap[0].dev != NULL)
+        {
+            new_buf->dev = vap[0].dev;
+            new_buf->protocol = eth_type_trans(new_buf, vap[0].dev);
+        }
+        else
+        {
+            new_buf->dev = dev;
+            new_buf->protocol = eth_type_trans(new_buf, dev);
+        }
+    }
+
+    new_buf->ip_summed = CHECKSUM_NONE;
+    dev->last_rx = jiffies;
+
+    switch(netif_rx(new_buf))
+#else
+    if (port == 0)
+    {
+        buf->dev = dev;
+        buf->protocol = eth_type_trans(buf, dev);
+    }
+    else
+    {
+        /* VAP */
+        if (vap[0].dev != NULL)
+        {
+            buf->dev = vap[0].dev;
+            buf->protocol = eth_type_trans(buf, vap[0].dev);
+        }
+        else
+        {
+            buf->dev = dev;
+            buf->protocol = eth_type_trans(buf, dev);
+        }
+    }
+
+    buf->ip_summed = CHECKSUM_NONE;
+    dev->last_rx = jiffies;
+
+    switch(netif_rx(buf))
+#endif
+    {
+    case NET_RX_BAD:
+    case NET_RX_DROP:
+    case NET_RX_CN_MOD:
+    case NET_RX_CN_HIGH:
+        break;
+    default:
+            macp->drv_stats.net_stats.rx_packets++;
+            macp->drv_stats.net_stats.rx_bytes += buf->len;
+        break;
+    }
+
+    return;
+}
+
+/* Leave an empty line below to remove warning message on some compiler */
diff --git a/drivers/staging/otus/wrap_sec.c b/drivers/staging/otus/wrap_sec.c
new file mode 100644
index 000000000000..f688d064175d
--- /dev/null
+++ b/drivers/staging/otus/wrap_sec.c
@@ -0,0 +1,127 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : wrap_sec.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for CENC.                 */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+#ifdef ZM_ENABLE_CENC
+extern int zfLnxCencSendMsg(struct sock *netlink_sk, u_int8_t *msg, int len);
+
+u16_t zfLnxCencAsocNotify(zdev_t* dev, u16_t* macAddr, u8_t* body, u16_t bodySize, u16_t port)
+{
+    struct usbdrv_private *macp = (struct usbdrv_private *)dev->priv;
+    struct zydas_cenc_sta_info cenc_info;
+    //struct sock *netlink_sk;
+    u8_t ie_len;
+    int ii;
+
+    /* Create NETLINK socket */
+    //netlink_sk = netlink_kernel_create(NETLINK_USERSOCK, NULL);
+
+    if (macp->netlink_sk == NULL)
+    {
+        printk(KERN_ERR "NETLINK Socket is NULL\n");
+        return -1;
+    }
+
+    memset(&cenc_info, 0, sizeof(cenc_info));
+
+    //memcpy(cenc_info.gsn, vap->iv_cencmsk_keys.wk_txiv, ZM_CENC_IV_LEN);
+    zfiWlanQueryGSN(dev, cenc_info.gsn, port);
+    cenc_info.datalen += ZM_CENC_IV_LEN;
+    ie_len = body[1] + 2;
+    memcpy(cenc_info.wie, body, ie_len);
+    cenc_info.datalen += ie_len;
+
+    memcpy(cenc_info.sta_mac, macAddr, 6);
+    cenc_info.msg_type = ZM_CENC_WAI_REQUEST;
+    cenc_info.datalen += 6 + 2;
+
+    printk(KERN_ERR "===== zfwCencSendMsg, bodySize: %d =====\n", bodySize);
+
+    for(ii = 0; ii < bodySize; ii++)
+    {
+        printk(KERN_ERR "%02x ", body[ii]);
+
+        if ((ii & 0xf) == 0xf)
+        {
+            printk(KERN_ERR "\n");
+        }
+    }
+
+    zfLnxCencSendMsg(macp->netlink_sk, (u8_t *)&cenc_info, cenc_info.datalen+4);
+
+    /* Close NETLINK socket */
+    //sock_release(netlink_sk);
+
+    return 0;
+}
+#endif //ZM_ENABLE_CENC
+
+u8_t zfwCencHandleBeaconProbrespon(zdev_t* dev, u8_t *pWIEc,
+        u8_t *pPeerSSIDc, u8_t *pPeerAddrc)
+{
+    return 0;
+}
+
+u8_t zfwGetPktEncExemptionActionType(zdev_t* dev, zbuf_t* buf)
+{
+    return ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION;
+}
+
+void copyToIntTxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* src,
+                         u16_t offset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length;i++)
+    {
+        //zmw_tx_buf_writeb(dev, buf, offset+i, src[i]);
+        *(u8_t*)((u8_t*)buf->data+offset+i) = src[i];
+    }
+}
+
+u16_t zfwStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    //zm_msg1_mm(ZM_LV_0, "CWY - add wpaie content Length : ", macp->supIe[1]);
+    if (macp->supIe[1] != 0)
+    {
+        copyToIntTxBuffer(dev, buf, macp->supIe, offset, macp->supIe[1]+2);
+        //memcpy(buf->data[offset], macp->supIe, macp->supIe[1]+2);
+        offset += (macp->supIe[1]+2);
+    }
+
+    return offset;
+}
+
+/* Leave an empty line below to remove warning message on some compiler */
diff --git a/drivers/staging/otus/wrap_usb.c b/drivers/staging/otus/wrap_usb.c
new file mode 100644
index 000000000000..c076e5645224
--- /dev/null
+++ b/drivers/staging/otus/wrap_usb.c
@@ -0,0 +1,191 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : wrap_usb.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for USB management        */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+extern void zfLnxInitUsbTxQ(zdev_t* dev);
+extern void zfLnxInitUsbRxQ(zdev_t* dev);
+extern u32_t zfLnxSubmitRegInUrb(zdev_t *dev);
+u32_t zfLnxUsbOut(zdev_t* dev, u8_t *hdr, u16_t hdrlen, u8_t *snap, u16_t snapLen,
+        u8_t *tail, u16_t tailLen, zbuf_t *buf, u16_t offset);
+u32_t zfLnxUsbWriteReg(zdev_t* dev, u32_t* cmd, u16_t cmdLen);
+
+void zfwUsbRegisterCallBack(zdev_t* dev, struct zfCbUsbFuncTbl *zfUsbFunc) {
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    macp->usbCbFunctions.zfcbUsbRecv = zfUsbFunc->zfcbUsbRecv;
+    macp->usbCbFunctions.zfcbUsbRegIn = zfUsbFunc->zfcbUsbRegIn;
+    macp->usbCbFunctions.zfcbUsbOutComplete = zfUsbFunc->zfcbUsbOutComplete;
+
+    return;
+}
+
+u32_t zfwUsbGetFreeTxQSize(zdev_t* dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u32_t        freeTxQSize;
+    unsigned long irqFlag;
+    //zmw_declare_for_critical_section();
+
+    //zmw_enter_critical_section(dev);
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    freeTxQSize = ZM_MAX_TX_BUF_NUM - macp->TxBufCnt;
+
+    //zmw_leave_critical_section(dev);
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+
+    return freeTxQSize;
+}
+
+u32_t zfwUsbGetMaxTxQSize(zdev_t* dev)
+{
+    return ZM_MAX_TX_BUF_NUM;
+}
+
+u32_t zfwUsbEnableIntEpt(zdev_t *dev, u8_t endpt)
+{
+    /* Initialize USB TxQ */
+    zfLnxInitUsbTxQ(dev);
+
+    /* Initialize USB RxQ */
+    zfLnxInitUsbRxQ(dev);
+
+    /* Initialize USB Register In URB */
+    //zfwUsbSubmitRegIn(dev);
+    /* Initialize USB Register In URB */
+    zfLnxSubmitRegInUrb(dev);
+
+    return 0;
+}
+
+int zfwUsbEnableRxEpt(zdev_t* dev, u8_t endpt)
+{
+    return 0;
+}
+
+u32_t zfwUsbSubmitControl(zdev_t* dev, u8_t req, u16_t value, u16_t index, void *data, u32_t size)
+{
+    int result = 0;
+    u32_t ret = 0;
+    struct usbdrv_private *macp = dev->ml_priv;
+    u8_t* buf;
+
+    if (size > 0)
+    {
+        buf = kmalloc(size, GFP_KERNEL);
+        memcpy(buf, (u8_t*)data, size);
+    }
+    else
+    {
+        buf = NULL;
+    }
+
+#if 0
+    printk(KERN_ERR "req = 0x%02x\n", req);
+    printk(KERN_ERR "value = 0x%04x\n", value);
+    printk(KERN_ERR "index = 0x%04x\n", index);
+    printk(KERN_ERR "data = 0x%lx\n", (u32_t) data);
+    printk(KERN_ERR "size = %ld\n", size);
+#endif
+
+    result = usb_control_msg(macp->udev, usb_sndctrlpipe(macp->udev, 0),
+            req, USB_DIR_OUT | 0x40, value, index, buf, size, HZ);
+
+    if (result < 0)
+    {
+        printk("zfwUsbSubmitControl() failed, result=0x%x\n", result);
+        ret = 1;
+    }
+    kfree(buf);
+
+    return ret;
+}
+
+void zfwUsbCmd(zdev_t* dev, u8_t endpt, u32_t* cmd, u16_t cmdLen)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u32_t ret;
+
+    //MPUsbCommand(dev, endpt, cmd, cmdLen);
+    ret = zfLnxUsbWriteReg(dev, cmd, cmdLen);
+
+    /* if zfLnxUsbWriteReg() return error, free and allocate urb, resend again */
+    if (ret != 0)
+    {
+        usb_free_urb(macp->RegOutUrb);
+        macp->RegOutUrb = usb_alloc_urb(0, GFP_ATOMIC);
+        ret = zfLnxUsbWriteReg(dev, cmd, cmdLen);
+    }
+}
+
+u32_t zfwUsbSend(zdev_t* dev, u8_t endpt, u8_t *hdr, u16_t hdrlen, u8_t *snap, u16_t snapLen,
+                u8_t *tail, u16_t tailLen, zbuf_t *buf, u16_t offset)
+{
+    u32_t status;
+
+#ifdef ZM_CONFIG_BIG_ENDIAN
+    u32_t ii = 0;
+    u16_t *pc = NULL;
+
+    pc = (u16_t *)hdr;
+    for(ii=0; ii<(hdrlen>>1); ii++)
+    {
+        pc[ii] = cpu_to_le16(pc[ii]);
+    }
+
+    pc = (u16_t *)snap;
+    for(ii=0; ii<(snapLen>>1); ii++)
+    {
+        pc[ii] = cpu_to_le16(pc[ii]);
+    }
+
+    pc = (u16_t *)tail;
+    for(ii=0; ii<(tailLen>>1); ii++)
+    {
+        pc[ii] = cpu_to_le16(pc[ii]);
+    }
+#endif
+
+    status = zfLnxUsbOut(dev, hdr, hdrlen, snap, snapLen, tail, tailLen, buf, offset);
+    if ( status == 0 )
+    {
+        return 0;
+    }
+    else
+    {
+        return 1;
+    }
+}
+
+/* Leave an empty line below to remove warning message on some compiler */
diff --git a/drivers/staging/otus/wwrap.c b/drivers/staging/otus/wwrap.c
new file mode 100644
index 000000000000..1bb5f596d6c3
--- /dev/null
+++ b/drivers/staging/otus/wwrap.c
@@ -0,0 +1,1134 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : wwrap.c                                               */
+/*  Abstract                                                            */
+/*      This module contains wrapper functions.                         */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      Platform dependent.                                             */
+/*                                                                      */
+
+/* Please include your header files here */
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+extern void zfiRecv80211(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+extern void zfCoreRecv(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+extern void zfIdlChkRsp(zdev_t* dev, u32_t* rsp, u16_t rspLen);
+extern void zfIdlRsp(zdev_t* dev, u32_t *rsp, u16_t rspLen);
+
+
+
+//extern struct zsWdsStruct wds[ZM_WDS_PORT_NUMBER];
+extern struct zsVapStruct vap[ZM_VAP_PORT_NUMBER];
+
+u32_t zfLnxUsbSubmitTxData(zdev_t* dev);
+u32_t zfLnxUsbIn(zdev_t* dev, urb_t *urb, zbuf_t *buf);
+u32_t zfLnxSubmitRegInUrb(zdev_t *dev);
+u32_t zfLnxUsbSubmitBulkUrb(urb_t *urb, struct usb_device *usb, u16_t epnum, u16_t direction,
+        void *transfer_buffer, int buffer_length, usb_complete_t complete, void *context);
+u32_t zfLnxUsbSubmitIntUrb(urb_t *urb, struct usb_device *usb, u16_t epnum, u16_t direction,
+        void *transfer_buffer, int buffer_length, usb_complete_t complete, void *context,
+        u32_t interval);
+
+u16_t zfLnxGetFreeTxUrb(zdev_t *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t idx;
+    unsigned long irqFlag;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    //idx = ((macp->TxUrbTail + 1) & (ZM_MAX_TX_URB_NUM - 1));
+
+    //if (idx != macp->TxUrbHead)
+    if (macp->TxUrbCnt != 0)
+    {
+        idx = macp->TxUrbTail;
+        macp->TxUrbTail = ((macp->TxUrbTail + 1) & (ZM_MAX_TX_URB_NUM - 1));
+        macp->TxUrbCnt--;
+    }
+    else
+    {
+        //printk(KERN_ERR "macp->TxUrbCnt: %d\n", macp->TxUrbCnt);
+        idx = 0xffff;
+    }
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+    return idx;
+}
+
+void zfLnxPutTxUrb(zdev_t *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t idx;
+    unsigned long irqFlag;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    idx = ((macp->TxUrbHead + 1) & (ZM_MAX_TX_URB_NUM - 1));
+
+    //if (idx != macp->TxUrbTail)
+    if (macp->TxUrbCnt < ZM_MAX_TX_URB_NUM)
+    {
+        macp->TxUrbHead = idx;
+        macp->TxUrbCnt++;
+    }
+    else
+    {
+        printk("UsbTxUrbQ inconsistent: TxUrbHead: %d, TxUrbTail: %d\n",
+                macp->TxUrbHead, macp->TxUrbTail);
+    }
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+}
+
+u16_t zfLnxCheckTxBufferCnt(zdev_t *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t TxBufCnt;
+    unsigned long irqFlag;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    TxBufCnt = macp->TxBufCnt;
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+    return TxBufCnt;
+}
+
+UsbTxQ_t *zfLnxGetUsbTxBuffer(zdev_t *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t idx;
+    UsbTxQ_t *TxQ;
+    unsigned long irqFlag;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    idx = ((macp->TxBufHead+1) & (ZM_MAX_TX_BUF_NUM - 1));
+
+    //if (idx != macp->TxBufTail)
+    if (macp->TxBufCnt > 0)
+    {
+        //printk("CWY - zfwGetUsbTxBuffer ,macp->TxBufCnt = %d\n", macp->TxBufCnt);
+        TxQ = (UsbTxQ_t *)&(macp->UsbTxBufQ[macp->TxBufHead]);
+        macp->TxBufHead = ((macp->TxBufHead+1) & (ZM_MAX_TX_BUF_NUM - 1));
+        macp->TxBufCnt--;
+    }
+    else
+    {
+        if (macp->TxBufHead != macp->TxBufTail)
+        {
+            printk(KERN_ERR "zfwGetUsbTxBuf UsbTxBufQ inconsistent: TxBufHead: %d, TxBufTail: %d\n",
+                    macp->TxBufHead, macp->TxBufTail);
+        }
+
+        spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+        return NULL;
+    }
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+    return TxQ;
+}
+
+u16_t zfLnxPutUsbTxBuffer(zdev_t *dev, u8_t *hdr, u16_t hdrlen,
+        u8_t *snap, u16_t snapLen, u8_t *tail, u16_t tailLen,
+        zbuf_t *buf, u16_t offset)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t idx;
+    UsbTxQ_t *TxQ;
+    unsigned long irqFlag;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    idx = ((macp->TxBufTail+1) & (ZM_MAX_TX_BUF_NUM - 1));
+
+    /* For Tx debug */
+    //zm_assert(macp->TxBufCnt >= 0); // deleted because of always true
+
+    //if (idx != macp->TxBufHead)
+    if (macp->TxBufCnt < ZM_MAX_TX_BUF_NUM)
+    {
+        //printk("CWY - zfwPutUsbTxBuffer ,macp->TxBufCnt = %d\n", macp->TxBufCnt);
+        TxQ = (UsbTxQ_t *)&(macp->UsbTxBufQ[macp->TxBufTail]);
+        memcpy(TxQ->hdr, hdr, hdrlen);
+        TxQ->hdrlen = hdrlen;
+        memcpy(TxQ->snap, snap, snapLen);
+        TxQ->snapLen = snapLen;
+        memcpy(TxQ->tail, tail, tailLen);
+        TxQ->tailLen = tailLen;
+        TxQ->buf = buf;
+        TxQ->offset = offset;
+
+        macp->TxBufTail = ((macp->TxBufTail+1) & (ZM_MAX_TX_BUF_NUM - 1));
+        macp->TxBufCnt++;
+    }
+    else
+    {
+        printk(KERN_ERR "zfLnxPutUsbTxBuffer UsbTxBufQ inconsistent: TxBufHead: %d, TxBufTail: %d, TxBufCnt: %d\n",
+            macp->TxBufHead, macp->TxBufTail, macp->TxBufCnt);
+        spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+        return 0xffff;
+    }
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+    return 0;
+}
+
+zbuf_t *zfLnxGetUsbRxBuffer(zdev_t *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    //u16_t idx;
+    zbuf_t *buf;
+    unsigned long irqFlag;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    //idx = ((macp->RxBufHead+1) & (ZM_MAX_RX_URB_NUM - 1));
+
+    //if (idx != macp->RxBufTail)
+    if (macp->RxBufCnt != 0)
+    {
+        buf = macp->UsbRxBufQ[macp->RxBufHead];
+        macp->RxBufHead = ((macp->RxBufHead+1) & (ZM_MAX_RX_URB_NUM - 1));
+        macp->RxBufCnt--;
+    }
+    else
+    {
+        printk("RxBufQ inconsistent: RxBufHead: %d, RxBufTail: %d\n",
+                macp->RxBufHead, macp->RxBufTail);
+        spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+        return NULL;
+    }
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+    return buf;
+}
+
+u32_t zfLnxPutUsbRxBuffer(zdev_t *dev, zbuf_t *buf)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t idx;
+    unsigned long irqFlag;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    idx = ((macp->RxBufTail+1) & (ZM_MAX_RX_URB_NUM - 1));
+
+    //if (idx != macp->RxBufHead)
+    if (macp->RxBufCnt != ZM_MAX_RX_URB_NUM)
+    {
+        macp->UsbRxBufQ[macp->RxBufTail] = buf;
+        macp->RxBufTail = idx;
+        macp->RxBufCnt++;
+    }
+    else
+    {
+        printk("RxBufQ inconsistent: RxBufHead: %d, RxBufTail: %d\n",
+                macp->RxBufHead, macp->RxBufTail);
+        spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+        return 0xffff;
+    }
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+    return 0;
+}
+
+void zfLnxUsbDataOut_callback(urb_t *urb)
+{
+    zdev_t* dev = urb->context;
+    //UsbTxQ_t *TxData;
+
+    /* Give the urb back */
+    zfLnxPutTxUrb(dev);
+
+    /* Check whether there is any pending buffer needed */
+    /* to be sent */
+    if (zfLnxCheckTxBufferCnt(dev) != 0)
+    {
+        //TxData = zfwGetUsbTxBuffer(dev);
+
+        //if (TxData == NULL)
+        //{
+        //    printk("Get a NULL buffer from zfwGetUsbTxBuffer\n");
+        //    return;
+        //}
+        //else
+        //{
+            zfLnxUsbSubmitTxData(dev);
+        //}
+    }
+}
+
+void zfLnxUsbDataIn_callback(urb_t *urb)
+{
+    zdev_t* dev = urb->context;
+    struct usbdrv_private *macp = dev->ml_priv;
+    zbuf_t *buf;
+    zbuf_t *new_buf;
+    int status;
+
+#if ZM_USB_STREAM_MODE == 1
+    static int remain_len = 0, check_pad = 0, check_len = 0;
+    int index = 0;
+    int chk_idx;
+    u16_t pkt_len;
+    u16_t pkt_tag;
+    u16_t ii;
+    zbuf_t *rxBufPool[8];
+    u16_t rxBufPoolIndex = 0;
+#endif
+
+    /* Check status for URB */
+    if (urb->status != 0){
+        printk("zfLnxUsbDataIn_callback() : status=0x%x\n", urb->status);
+        if ((urb->status != -ENOENT) && (urb->status != -ECONNRESET)
+            && (urb->status != -ESHUTDOWN))
+        {
+                if (urb->status == -EPIPE){
+                    //printk(KERN_ERR "nonzero read bulk status received: -EPIPE");
+                    status = -1;
+                }
+
+                if (urb->status == -EPROTO){
+                    //printk(KERN_ERR "nonzero read bulk status received: -EPROTO");
+                    status = -1;
+                }
+        }
+
+        //printk(KERN_ERR "urb->status: 0x%08x\n", urb->status);
+
+        /* Dequeue skb buffer */
+        buf = zfLnxGetUsbRxBuffer(dev);
+        dev_kfree_skb_any(buf);
+        #if 0
+        /* Enqueue skb buffer */
+        zfLnxPutUsbRxBuffer(dev, buf);
+
+        /* Submit a Rx urb */
+        zfLnxUsbIn(dev, urb, buf);
+        #endif
+        return;
+    }
+
+    if (urb->actual_length == 0)
+    {
+        printk(KERN_ERR "Get an URB whose length is zero");
+        status = -1;
+    }
+
+    /* Dequeue skb buffer */
+    buf = zfLnxGetUsbRxBuffer(dev);
+
+    //zfwBufSetSize(dev, buf, urb->actual_length);
+#ifdef NET_SKBUFF_DATA_USES_OFFSET
+    buf->tail = 0;
+    buf->len = 0;
+#else
+    buf->tail = buf->data;
+    buf->len = 0;
+#endif
+
+    if ((buf->tail + urb->actual_length) > buf->end)
+        BUG();
+
+    skb_put(buf, urb->actual_length);
+
+#if ZM_USB_STREAM_MODE == 1
+    if (remain_len != 0)
+    {
+        zbuf_t *remain_buf = macp->reamin_buf;
+
+        index = remain_len;
+        remain_len -= check_pad;
+
+        /*  Copy data */
+        memcpy(&(remain_buf->data[check_len]), buf->data, remain_len);
+        check_len += remain_len;
+        remain_len = 0;
+
+        rxBufPool[rxBufPoolIndex++] = remain_buf;
+    }
+
+    while(index < urb->actual_length)
+    {
+        pkt_len = buf->data[index] + (buf->data[index+1] << 8);
+        pkt_tag = buf->data[index+2] + (buf->data[index+3] << 8);
+
+        if (pkt_tag == 0x4e00)
+        {
+            int pad_len;
+
+            //printk("Get a packet, index: %d, pkt_len: 0x%04x\n", index, pkt_len);
+            #if 0
+            /* Dump data */
+            for (ii = index; ii < pkt_len+4;)
+            {
+                printk("%02x ", (buf->data[ii] & 0xff));
+
+                if ((++ii % 16) == 0)
+                    printk("\n");
+            }
+
+            printk("\n");
+            #endif
+
+            pad_len = 4 - (pkt_len & 0x3);
+
+            if(pad_len == 4)
+                pad_len = 0;
+
+            chk_idx = index;
+            index = index + 4 + pkt_len + pad_len;
+
+            if (index > ZM_MAX_RX_BUFFER_SIZE)
+            {
+                remain_len = index - ZM_MAX_RX_BUFFER_SIZE; // - pad_len;
+                check_len = ZM_MAX_RX_BUFFER_SIZE - chk_idx - 4;
+                check_pad = pad_len;
+
+                /* Allocate a skb buffer */
+                //new_buf = zfwBufAllocate(dev, ZM_MAX_RX_BUFFER_SIZE);
+                new_buf = dev_alloc_skb(ZM_MAX_RX_BUFFER_SIZE);
+
+                /* Set skb buffer length */
+            #ifdef NET_SKBUFF_DATA_USES_OFFSET
+                new_buf->tail = 0;
+                new_buf->len = 0;
+            #else
+                new_buf->tail = new_buf->data;
+                new_buf->len = 0;
+            #endif
+
+                skb_put(new_buf, pkt_len);
+
+                /* Copy the buffer */
+                memcpy(new_buf->data, &(buf->data[chk_idx+4]), check_len);
+
+                /* Record the buffer pointer */
+                macp->reamin_buf = new_buf;
+            }
+            else
+            {
+        #ifdef ZM_DONT_COPY_RX_BUFFER
+                if (rxBufPoolIndex == 0)
+                {
+                    new_buf = skb_clone(buf, GFP_ATOMIC);
+
+                    new_buf->data = &(buf->data[chk_idx+4]);
+                    new_buf->len = pkt_len;
+                }
+                else
+                {
+        #endif
+                /* Allocate a skb buffer */
+                new_buf = dev_alloc_skb(ZM_MAX_RX_BUFFER_SIZE);
+
+                /* Set skb buffer length */
+            #ifdef NET_SKBUFF_DATA_USES_OFFSET
+                new_buf->tail = 0;
+                new_buf->len = 0;
+            #else
+                new_buf->tail = new_buf->data;
+                new_buf->len = 0;
+            #endif
+
+                skb_put(new_buf, pkt_len);
+
+                /* Copy the buffer */
+                memcpy(new_buf->data, &(buf->data[chk_idx+4]), pkt_len);
+
+        #ifdef ZM_DONT_COPY_RX_BUFFER
+                }
+        #endif
+                rxBufPool[rxBufPoolIndex++] = new_buf;
+            }
+        }
+        else
+        {
+            printk(KERN_ERR "Can't find tag, pkt_len: 0x%04x, tag: 0x%04x\n", pkt_len, pkt_tag);
+
+            /* Free buffer */
+            dev_kfree_skb_any(buf);
+
+            /* Allocate a skb buffer */
+            new_buf = dev_alloc_skb(ZM_MAX_RX_BUFFER_SIZE);
+
+            /* Enqueue skb buffer */
+            zfLnxPutUsbRxBuffer(dev, new_buf);
+
+            /* Submit a Rx urb */
+            zfLnxUsbIn(dev, urb, new_buf);
+
+            return;
+        }
+    }
+
+    /* Free buffer */
+    dev_kfree_skb_any(buf);
+#endif
+
+    /* Allocate a skb buffer */
+    new_buf = dev_alloc_skb(ZM_MAX_RX_BUFFER_SIZE);
+
+    /* Enqueue skb buffer */
+    zfLnxPutUsbRxBuffer(dev, new_buf);
+
+    /* Submit a Rx urb */
+    zfLnxUsbIn(dev, urb, new_buf);
+
+#if ZM_USB_STREAM_MODE == 1
+    for(ii = 0; ii < rxBufPoolIndex; ii++)
+    {
+        macp->usbCbFunctions.zfcbUsbRecv(dev, rxBufPool[ii]);
+    }
+#else
+    /* pass data to upper layer */
+    macp->usbCbFunctions.zfcbUsbRecv(dev, buf);
+#endif
+}
+
+void zfLnxUsbRegOut_callback(urb_t *urb)
+{
+    //dev_t* dev = urb->context;
+
+    //printk(KERN_ERR "zfwUsbRegOut_callback\n");
+}
+
+void zfLnxUsbRegIn_callback(urb_t *urb)
+{
+    zdev_t* dev = urb->context;
+    u32_t rsp[64/4];
+    int status;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    /* Check status for URB */
+    if (urb->status != 0){
+        printk("zfLnxUsbRegIn_callback() : status=0x%x\n", urb->status);
+        if ((urb->status != -ENOENT) && (urb->status != -ECONNRESET)
+            && (urb->status != -ESHUTDOWN))
+        {
+                if (urb->status == -EPIPE){
+                    //printk(KERN_ERR "nonzero read bulk status received: -EPIPE");
+                    status = -1;
+                }
+
+                if (urb->status == -EPROTO){
+                    //printk(KERN_ERR "nonzero read bulk status received: -EPROTO");
+                    status = -1;
+                }
+        }
+
+        //printk(KERN_ERR "urb->status: 0x%08x\n", urb->status);
+        return;
+    }
+
+    if (urb->actual_length == 0)
+    {
+        printk(KERN_ERR "Get an URB whose length is zero");
+        status = -1;
+    }
+
+    /* Copy data into respone buffer */
+    memcpy(rsp, macp->regUsbReadBuf, urb->actual_length);
+
+    /* Notify to upper layer */
+    //zfIdlChkRsp(dev, rsp, (u16_t)urb->actual_length);
+    //zfiUsbRegIn(dev, rsp, (u16_t)urb->actual_length);
+    macp->usbCbFunctions.zfcbUsbRegIn(dev, rsp, (u16_t)urb->actual_length);
+
+    /* Issue another USB IN URB */
+    zfLnxSubmitRegInUrb(dev);
+}
+
+u32_t zfLnxSubmitRegInUrb(zdev_t *dev)
+{
+    u32_t ret;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    /* Submit a rx urb */
+    //ret = zfLnxUsbSubmitBulkUrb(macp->RegInUrb, macp->udev,
+    //        USB_REG_IN_PIPE, USB_DIR_IN, macp->regUsbReadBuf,
+    //        ZM_USB_REG_MAX_BUF_SIZE, zfLnxUsbRegIn_callback, dev);
+    //CWYang(-)
+    //if (ret != 0)
+    //    printk("zfwUsbSubmitBulkUrb fail, status: 0x%08x\n", (int)ret);
+
+    ret = zfLnxUsbSubmitIntUrb(macp->RegInUrb, macp->udev,
+            USB_REG_IN_PIPE, USB_DIR_IN, macp->regUsbReadBuf,
+            ZM_USB_REG_MAX_BUF_SIZE, zfLnxUsbRegIn_callback, dev, 1);
+
+    return ret;
+}
+
+u32_t zfLnxUsbSubmitTxData(zdev_t* dev)
+{
+    u32_t i;
+    u32_t ret;
+    u16_t freeTxUrb;
+    u8_t *puTxBuf = NULL;
+    UsbTxQ_t *TxData;
+    int len = 0;
+    struct usbdrv_private *macp = dev->ml_priv;
+#if ZM_USB_TX_STREAM_MODE == 1
+    u8_t               ii;
+    u16_t              offset = 0;
+    u16_t              usbTxAggCnt;
+    u16_t              *pUsbTxHdr;
+    UsbTxQ_t           *TxQPool[ZM_MAX_TX_AGGREGATE_NUM];
+#endif
+
+    /* First check whether there is a free URB */
+    freeTxUrb = zfLnxGetFreeTxUrb(dev);
+
+    /* If there is no any free Tx Urb */
+    if (freeTxUrb == 0xffff)
+    {
+        //printk(KERN_ERR "Can't get free Tx Urb\n");
+        //printk("CWY - Can't get free Tx Urb\n");
+        return 0xffff;
+    }
+
+#if ZM_USB_TX_STREAM_MODE == 1
+    usbTxAggCnt = zfLnxCheckTxBufferCnt(dev);
+
+    if (usbTxAggCnt >= ZM_MAX_TX_AGGREGATE_NUM)
+    {
+       usbTxAggCnt = ZM_MAX_TX_AGGREGATE_NUM;
+    }
+    else
+    {
+       usbTxAggCnt = 1;
+    }
+
+    //printk("usbTxAggCnt: %d\n", usbTxAggCnt);
+#endif
+
+#if ZM_USB_TX_STREAM_MODE == 1
+    for(ii = 0; ii < usbTxAggCnt; ii++)
+    {
+#endif
+    /* Dequeue the packet from UsbTxBufQ */
+    TxData = zfLnxGetUsbTxBuffer(dev);
+    if (TxData == NULL)
+    {
+        /* Give the urb back */
+        zfLnxPutTxUrb(dev);
+        return 0xffff;
+    }
+
+    /* Point to the freeTxUrb buffer */
+    puTxBuf = macp->txUsbBuf[freeTxUrb];
+
+#if ZM_USB_TX_STREAM_MODE == 1
+    puTxBuf += offset;
+    pUsbTxHdr = (u16_t *)puTxBuf;
+
+    /* Add the packet length and tag information */
+    *pUsbTxHdr++ = TxData->hdrlen + TxData->snapLen +
+             (TxData->buf->len - TxData->offset) +  TxData->tailLen;
+
+    *pUsbTxHdr++ = 0x697e;
+
+    puTxBuf += 4;
+#endif // #ifdef ZM_USB_TX_STREAM_MODE
+
+    /* Copy WLAN header and packet buffer into USB buffer */
+    for(i = 0; i < TxData->hdrlen; i++)
+    {
+        *puTxBuf++ = TxData->hdr[i];
+    }
+
+    /* Copy SNAP header */
+    for(i = 0; i < TxData->snapLen; i++)
+    {
+        *puTxBuf++ = TxData->snap[i];
+    }
+
+    /* Copy packet buffer */
+    for(i = 0; i < TxData->buf->len - TxData->offset; i++)
+    {
+    	//*puTxBuf++ = zmw_rx_buf_readb(dev, TxData->buf, i);
+    	*puTxBuf++ = *(u8_t*)((u8_t*)TxData->buf->data+i+TxData->offset);
+    }
+
+    /* Copy tail */
+    for(i = 0; i < TxData->tailLen; i++)
+    {
+        *puTxBuf++ = TxData->tail[i];
+    }
+
+    len = TxData->hdrlen+TxData->snapLen+TxData->buf->len+TxData->tailLen-TxData->offset;
+
+    #if 0
+    if (TxData->hdrlen != 0)
+    {
+        puTxBuf = macp->txUsbBuf[freeTxUrb];
+        for (i = 0; i < len; i++)
+        {
+            printk("%02x ", puTxBuf[i]);
+            if (i % 16 == 15)
+                printk("\n");
+        }
+        printk("\n");
+    }
+    #endif
+    #if 0
+    /* For debug purpose */
+    if(TxData->hdr[9] & 0x40)
+    {
+        int i;
+        u16_t ctrlLen = TxData->hdr[0] + (TxData->hdr[1] << 8);
+
+        if (ctrlLen != len + 4)
+        {
+        /* Dump control setting */
+        for(i = 0; i < 8; i++)
+        {
+            printk(KERN_ERR "0x%02x ", TxData->hdr[i]);
+        }
+        printk(KERN_ERR "\n");
+
+        printk(KERN_ERR "ctrLen: %d, hdrLen: %d, snapLen: %d\n", ctrlLen, TxData->hdrlen, TxData->snapLen);
+        printk(KERN_ERR "bufLen: %d, tailLen: %d, len: %d\n", TxData->buf->len, TxData->tailLen, len);
+        }
+    }
+    #endif
+
+#if ZM_USB_TX_STREAM_MODE == 1
+    // Add the Length and Tag
+    len += 4;
+
+    //printk("%d packet, length: %d\n", ii+1, len);
+
+    if (ii < (ZM_MAX_TX_AGGREGATE_NUM-1))
+    {
+        /* Pad the buffer to firmware descriptor boundary */
+        offset += (((len-1) / 4) + 1) * 4;
+    }
+
+    if (ii == (ZM_MAX_TX_AGGREGATE_NUM-1))
+    {
+        len += offset;
+    }
+
+    TxQPool[ii] = TxData;
+
+    //DbgPrint("%d packet, offset: %d\n", ii+1, pUsbTxTransfer->offset);
+
+    /* free packet */
+    //zfBufFree(dev, txData->buf);
+    }
+#endif
+    //printk("CWY - call zfwUsbSubmitBulkUrb(), len = 0x%d\n", len);
+    /* Submit a tx urb */
+    ret = zfLnxUsbSubmitBulkUrb(macp->WlanTxDataUrb[freeTxUrb], macp->udev,
+            USB_WLAN_TX_PIPE, USB_DIR_OUT, macp->txUsbBuf[freeTxUrb],
+            len, zfLnxUsbDataOut_callback, dev);
+    //CWYang(-)
+    //if (ret != 0)
+    //    printk("zfwUsbSubmitBulkUrb fail, status: 0x%08x\n", (int)ret);
+
+    /* free packet */
+    //dev_kfree_skb_any(TxData->buf);
+#if ZM_USB_TX_STREAM_MODE == 1
+    for(ii = 0; ii < usbTxAggCnt; ii++)
+        macp->usbCbFunctions.zfcbUsbOutComplete(dev, TxQPool[ii]->buf, 1, TxQPool[ii]->hdr);
+#else
+    macp->usbCbFunctions.zfcbUsbOutComplete(dev, TxData->buf, 1, TxData->hdr);
+#endif
+
+    return ret;
+}
+
+
+
+u32_t zfLnxUsbIn(zdev_t* dev, urb_t *urb, zbuf_t *buf)
+{
+    u32_t ret;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    /* Submit a rx urb */
+    ret = zfLnxUsbSubmitBulkUrb(urb, macp->udev, USB_WLAN_RX_PIPE,
+            USB_DIR_IN, buf->data, ZM_MAX_RX_BUFFER_SIZE,
+            zfLnxUsbDataIn_callback, dev);
+    //CWYang(-)
+    //if (ret != 0)
+    //    printk("zfwUsbSubmitBulkUrb fail, status: 0x%08x\n", (int)ret);
+
+    return ret;
+}
+
+u32_t zfLnxUsbWriteReg(zdev_t* dev, u32_t* cmd, u16_t cmdLen)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u32_t ret;
+
+#ifdef ZM_CONFIG_BIG_ENDIAN
+    int ii = 0;
+
+    for(ii=0; ii<(cmdLen>>2); ii++)
+	cmd[ii] = cpu_to_le32(cmd[ii]);
+#endif
+
+    memcpy(macp->regUsbWriteBuf, cmd, cmdLen);
+
+    /* Issue an USB Out transfer */
+    /* Submit a tx urb */
+    ret = zfLnxUsbSubmitIntUrb(macp->RegOutUrb, macp->udev,
+            USB_REG_OUT_PIPE, USB_DIR_OUT, macp->regUsbWriteBuf,
+            cmdLen, zfLnxUsbRegOut_callback, dev, 1);
+
+    return ret;
+}
+
+
+u32_t zfLnxUsbOut(zdev_t* dev, u8_t *hdr, u16_t hdrlen, u8_t *snap, u16_t snapLen,
+        u8_t *tail, u16_t tailLen, zbuf_t *buf, u16_t offset)
+{
+    u32_t ret;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    /* Check length of tail buffer */
+    //zm_assert((tailLen <= 16));
+
+    /* Enqueue the packet into UsbTxBufQ */
+    if (zfLnxPutUsbTxBuffer(dev, hdr, hdrlen, snap, snapLen, tail, tailLen, buf, offset) == 0xffff)
+    {
+        /* free packet */
+        //printk("CWY - zfwPutUsbTxBuffer Error, free packet\n");
+        //dev_kfree_skb_any(buf);
+        macp->usbCbFunctions.zfcbUsbOutComplete(dev, buf, 0, hdr);
+        return 0xffff;
+    }
+
+    //return 0;
+    //printk("CWY - call zfwUsbSubmitTxData()\n");
+    ret = zfLnxUsbSubmitTxData(dev);
+    return ret;
+}
+
+void zfLnxInitUsbTxQ(zdev_t* dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    printk(KERN_ERR "zfwInitUsbTxQ\n");
+
+    /* Zero memory for UsbTxBufQ */
+    memset(macp->UsbTxBufQ, 0, sizeof(UsbTxQ_t) * ZM_MAX_TX_URB_NUM);
+
+    macp->TxBufHead = 0;
+    macp->TxBufTail = 0;
+    macp->TxUrbHead = 0;
+    macp->TxUrbTail = 0;
+    macp->TxUrbCnt = ZM_MAX_TX_URB_NUM;
+}
+
+void zfLnxInitUsbRxQ(zdev_t* dev)
+{
+    u16_t i;
+    zbuf_t *buf;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    /* Zero memory for UsbRxBufQ */
+    memset(macp->UsbRxBufQ, 0, sizeof(zbuf_t *) * ZM_MAX_RX_URB_NUM);
+
+    macp->RxBufHead = 0;
+
+    for (i = 0; i < ZM_MAX_RX_URB_NUM; i++)
+    {
+        //buf = zfwBufAllocate(dev, ZM_MAX_RX_BUFFER_SIZE);
+        buf = dev_alloc_skb(ZM_MAX_RX_BUFFER_SIZE);
+        macp->UsbRxBufQ[i] = buf;
+    }
+
+    //macp->RxBufTail = ZM_MAX_RX_URB_NUM - 1;
+    macp->RxBufTail = 0;
+
+    /* Submit all Rx urbs */
+    for (i = 0; i < ZM_MAX_RX_URB_NUM; i++)
+    {
+        zfLnxPutUsbRxBuffer(dev, macp->UsbRxBufQ[i]);
+        zfLnxUsbIn(dev, macp->WlanRxDataUrb[i], macp->UsbRxBufQ[i]);
+    }
+}
+
+
+
+u32_t zfLnxUsbSubmitBulkUrb(urb_t *urb, struct usb_device *usb, u16_t epnum, u16_t direction,
+        void *transfer_buffer, int buffer_length, usb_complete_t complete, void *context)
+{
+    u32_t ret;
+
+    if(direction == USB_DIR_OUT)
+    {
+        usb_fill_bulk_urb(urb, usb, usb_sndbulkpipe(usb, epnum),
+                transfer_buffer, buffer_length, complete, context);
+
+        urb->transfer_flags |= URB_ZERO_PACKET;
+    }
+    else
+    {
+        usb_fill_bulk_urb(urb, usb, usb_rcvbulkpipe(usb, epnum),
+                transfer_buffer, buffer_length, complete, context);
+    }
+
+    if (epnum == 4)
+    {
+        if (urb->hcpriv)
+        {
+            //printk("CWY - urb->hcpriv set by unknown reason, reset it\n");
+            //urb->hcpriv = 0;
+        }
+    }
+
+    ret = usb_submit_urb(urb, GFP_ATOMIC);
+    if ((epnum == 4) & (ret != 0))
+    {
+        //printk("CWY - ret = %x\n", ret);
+    }
+    return ret;
+}
+
+u32_t zfLnxUsbSubmitIntUrb(urb_t *urb, struct usb_device *usb, u16_t epnum, u16_t direction,
+        void *transfer_buffer, int buffer_length, usb_complete_t complete, void *context,
+        u32_t interval)
+{
+    u32_t ret;
+
+    if(direction == USB_DIR_OUT)
+    {
+        usb_fill_int_urb(urb, usb, usb_sndbulkpipe(usb, epnum),
+                transfer_buffer, buffer_length, complete, context, interval);
+    }
+    else
+    {
+        usb_fill_int_urb(urb, usb, usb_rcvbulkpipe(usb, epnum),
+                transfer_buffer, buffer_length, complete, context, interval);
+    }
+
+    ret = usb_submit_urb(urb, GFP_ATOMIC);
+
+    return ret;
+}
+
+#ifdef ZM_ENABLE_CENC
+int zfLnxCencSendMsg(struct sock *netlink_sk, u_int8_t *msg, int len)
+{
+#define COMMTYPE_GROUP   8
+#define WAI_K_MSG        0x11
+
+	int ret = -1;
+	int size;
+	unsigned char *old_tail;
+	struct sk_buff *skb;
+	struct nlmsghdr *nlh;
+	char *pos = NULL;
+
+	size = NLMSG_SPACE(len);
+	skb = alloc_skb(size, GFP_ATOMIC);
+
+	if(skb == NULL)
+	{
+		printk("dev_alloc_skb failure \n");
+		goto out;
+	}
+	old_tail = skb->tail;
+
+	/*ÌîдÊý¾Ý±¨Ïà¹ØÐÅÏ¢*/
+	nlh = NLMSG_PUT(skb, 0, 0, WAI_K_MSG, size-sizeof(*nlh));
+	pos = NLMSG_DATA(nlh);
+	memset(pos, 0, len);
+
+	/*´«Êäµ½Óû§¿Õ¼äµÄÊý¾Ý*/
+	memcpy(pos, msg,  len);
+	/*¼ÆËã¾­¹ý×Ö½Ú¶ÔÆäºóµÄÊý¾Ýʵ¼Ê³¤¶È*/
+	nlh->nlmsg_len = skb->tail - old_tail;
+	NETLINK_CB(skb).dst_group = COMMTYPE_GROUP;
+	netlink_broadcast(netlink_sk, skb, 0, COMMTYPE_GROUP, GFP_ATOMIC);
+	ret = 0;
+out:
+	return ret;
+nlmsg_failure: /*NLMSG_PUT ʧ°Ü£¬Ôò³·ÏúÌ×½Ó×Ö»º´æ*/
+  	if(skb)
+    		kfree_skb(skb);
+	goto out;
+
+#undef COMMTYPE_GROUP
+#undef WAI_K_MSG
+}
+#endif //ZM_ENABLE_CENC
+
+/* Simply return 0xffff if VAP function is not supported */
+u16_t zfLnxGetVapId(zdev_t* dev)
+{
+    u16_t i;
+
+    for (i=0; i<ZM_VAP_PORT_NUMBER; i++)
+    {
+        if (vap[i].dev == dev)
+        {
+            return i;
+        }
+    }
+    return 0xffff;
+}
+
+u32_t zfwReadReg(zdev_t* dev, u32_t offset)
+{
+    return 0;
+}
+
+#ifndef INIT_WORK
+#define work_struct tq_struct
+
+#define schedule_work(a)  schedule_task(a)
+
+#define flush_scheduled_work  flush_scheduled_tasks
+#define INIT_WORK(_wq, _routine, _data)  INIT_TQUEUE(_wq, _routine, _data)
+#define PREPARE_WORK(_wq, _routine, _data)  PREPARE_TQUEUE(_wq, _routine, _data)
+#endif
+
+#define KEVENT_WATCHDOG        0x00000001
+
+u32_t smp_kevent_Lock = 0;
+
+void kevent(struct work_struct *work)
+{
+    struct usbdrv_private *macp =
+               container_of(work, struct usbdrv_private, kevent);
+    zdev_t *dev = macp->device;
+
+    if (macp == NULL)
+    {
+        return;
+    }
+
+    if (test_and_set_bit(0, (void *)&smp_kevent_Lock))
+    {
+        //schedule_work(&macp->kevent);
+        return;
+    }
+
+    down(&macp->ioctl_sem);
+
+    if (test_and_clear_bit(KEVENT_WATCHDOG, &macp->kevent_flags))
+    {
+    extern u16_t zfHpStartRecv(zdev_t *dev);
+        //zfiHwWatchDogReinit(dev);
+        printk(("\n ************ Hw watchDog occur!! ************** \n"));
+        zfiWlanSuspend(dev);
+        zfiWlanResume(dev,0);
+        zfHpStartRecv(dev);
+    }
+
+    clear_bit(0, (void *)&smp_kevent_Lock);
+    up(&macp->ioctl_sem);
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                 zfLnxCreateThread            */
+/*      Create a Thread                                                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      always 0                                                        */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Yuan-Gu Wei         Atheros Communications, INC.    2007.3      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfLnxCreateThread(zdev_t *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    /* Create Mutex and keventd */
+    INIT_WORK(&macp->kevent, kevent);
+    init_MUTEX(&macp->ioctl_sem);
+
+    return 0;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                 zfLnxSignalThread            */
+/*      Signal Thread with Flag                                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      flag : signal thread flag                                       */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Yuan-Gu Wei         Atheros Communications, INC.    2007.3      */
+/*                                                                      */
+/************************************************************************/
+void zfLnxSignalThread(zdev_t *dev, int flag)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if (macp == NULL)
+    {
+        printk("macp is NULL\n");
+        return;
+    }
+
+    if (0 && macp->kevent_ready != 1)
+    {
+        printk("Kevent not ready\n");
+        return;
+    }
+
+    set_bit(flag, &macp->kevent_flags);
+
+    if (!schedule_work(&macp->kevent))
+    {
+        //Fails is Normal
+        //printk(KERN_ERR "schedule_task failed, flag = %x\n", flag);
+    }
+}
+
+/* Notify wrapper todo redownload firmware and reinit procedure when */
+/* hardware watchdog occur : zfiHwWatchDogReinit() */
+void zfLnxWatchDogNotify(zdev_t* dev)
+{
+    zfLnxSignalThread(dev, KEVENT_WATCHDOG);
+}
+
+/* Query Durantion of Active Scan */
+void zfwGetActiveScanDur(zdev_t* dev, u8_t* Dur)
+{
+    *Dur = 30; // default 30 ms
+}
+
+void zfwGetShowZeroLengthSSID(zdev_t* dev, u8_t* Dur)
+{
+    *Dur = 0;
+}
+
diff --git a/drivers/staging/otus/zdcompat.h b/drivers/staging/otus/zdcompat.h
new file mode 100644
index 000000000000..8acf400fbc0b
--- /dev/null
+++ b/drivers/staging/otus/zdcompat.h
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : zdcompat.h                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains function defintion for compatibility.       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _ZDCOMPAT_H
+#define _ZDCOMPAT_H
+
+
+#ifndef DECLARE_TASKLET
+#define tasklet_schedule(a)   schedule_task(a)
+#endif
+
+#undef netdevice_t
+typedef struct net_device netdevice_t;
+
+#ifdef WIRELESS_EXT
+#if (WIRELESS_EXT < 13)
+struct iw_request_info
+{
+        __u16           cmd;            /* Wireless Extension command */
+        __u16           flags;          /* More to come ;-) */
+};
+#endif
+#endif
+
+#ifndef IRQ_NONE
+typedef void irqreturn_t;
+#define IRQ_NONE
+#define IRQ_HANDLED
+#define IRQ_RETVAL(x)
+#endif
+
+#ifndef in_atomic
+#define in_atomic()  0
+#endif
+
+#define USB_QUEUE_BULK 0
+
+
+#endif
diff --git a/drivers/staging/otus/zdusb.c b/drivers/staging/otus/zdusb.c
new file mode 100644
index 000000000000..78f1d2224fa1
--- /dev/null
+++ b/drivers/staging/otus/zdusb.c
@@ -0,0 +1,239 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : zdusb.c                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains plug and play handling for USB device driver*/
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#ifdef MODVERSIONS
+#include <linux/modversions.h>
+#endif
+
+#include <linux/module.h>
+#include <linux/usb.h>
+
+#include "usbdrv.h"
+#include "zdusb.h"
+
+int zfLnxAllocAllUrbs(struct usbdrv_private *macp);
+void zfLnxFreeAllUrbs(struct usbdrv_private *macp);
+void zfLnxUnlinkAllUrbs(struct usbdrv_private *macp);
+
+MODULE_AUTHOR("Atheros Communications");
+MODULE_DESCRIPTION("Atheros 802.11n Wireless LAN adapter");
+MODULE_LICENSE("Dual BSD/GPL");
+
+static const char driver_name[] = "Otus";
+
+/* table of devices that work with this driver */
+static struct usb_device_id zd1221_ids [] = {
+	{ USB_DEVICE(VENDOR_ATHR, PRODUCT_AR9170) },
+        { USB_DEVICE(VENDOR_DLINK, PRODUCT_DWA160A) },
+	{ USB_DEVICE(0x0846, 0x9010) },
+	{ }					/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, zd1221_ids);
+
+extern u8_t zfLnxInitSetup(struct net_device *dev, struct usbdrv_private *macp);
+extern int usbdrv_close(struct net_device *dev);
+extern u8_t zfLnxClearStructs(struct net_device *dev);
+extern int zfWdsClose(struct net_device *dev);
+extern int zfUnregisterWdsDev(struct net_device* parentDev, u16_t wdsId);
+extern int zfLnxVapClose(struct net_device *dev);
+extern int zfLnxUnregisterVapDev(struct net_device* parentDev, u16_t vapId);
+
+/* WDS */
+extern struct zsWdsStruct wds[ZM_WDS_PORT_NUMBER];
+
+/* VAP */
+extern struct zsVapStruct vap[ZM_VAP_PORT_NUMBER];
+
+static int zfLnxProbe(struct usb_interface *interface,
+	const struct usb_device_id *id)
+{
+    struct usb_device *dev = interface_to_usbdev(interface);
+
+    struct net_device *net = NULL;
+    struct usbdrv_private *macp = NULL;
+    int vendor_id, product_id;
+    int result = 0;
+
+    usb_get_dev(dev);
+
+    vendor_id = dev->descriptor.idVendor;
+    product_id = dev->descriptor.idProduct;
+
+#ifdef HMAC_DEBUG
+    printk(KERN_NOTICE "vendor_id = %04x\n", vendor_id);
+    printk(KERN_NOTICE "product_id = %04x\n", product_id);
+
+    if (dev->speed == USB_SPEED_HIGH)
+        printk(KERN_NOTICE "USB 2.0 Host\n");
+    else
+        printk(KERN_NOTICE "USB 1.1 Host\n");
+#endif
+
+    if (!(macp = kmalloc(sizeof(struct usbdrv_private), GFP_KERNEL)))
+    {
+        printk(KERN_ERR "out of memory allocating device structure\n");
+        result = -ENOMEM;
+        goto fail;
+    }
+
+    /* Zero the memory */
+    memset(macp, 0, sizeof(struct usbdrv_private));
+
+    net = alloc_etherdev(0);
+
+    if (net == NULL)
+    {
+        printk(KERN_ERR "zfLnxProbe: Not able to alloc etherdev struct\n");
+        result = -ENOMEM;
+        goto fail1;
+    }
+
+    strcpy(net->name, "ath%d");
+
+    net->ml_priv = macp;   //kernel 2.6
+    macp->udev = dev;
+    macp->device = net;
+
+    /* set up the endpoint information */
+    /* check out the endpoints */
+    macp->interface = interface;
+
+    //init_waitqueue_head(&macp->regSet_wait);
+    //init_waitqueue_head(&macp->iorwRsp_wait);
+    //init_waitqueue_head(&macp->term_wait);
+
+    if (!zfLnxAllocAllUrbs(macp))
+    {
+        result = -ENOMEM;
+        goto fail2;
+    }
+
+    if (!zfLnxInitSetup(net, macp))
+    {
+        result = -EIO;
+        goto fail3;
+    }
+    else
+    {
+        usb_set_intfdata(interface, macp);
+        SET_NETDEV_DEV(net, &interface->dev);
+
+        if (register_netdev(net) != 0)
+        {
+            usb_set_intfdata(interface, NULL);
+            goto fail3;
+        }
+    }
+
+    netif_carrier_off(net);
+    goto done;
+
+fail3:
+    zfLnxFreeAllUrbs(macp);
+fail2:
+    free_netdev(net);  //kernel 2.6
+fail1:
+    kfree(macp);
+
+fail:
+    usb_put_dev(dev);
+    macp = NULL;
+
+done:
+    return result;
+}
+
+static void zfLnxDisconnect(struct usb_interface *interface)
+{
+    struct usbdrv_private *macp = (struct usbdrv_private *) usb_get_intfdata(interface);
+
+    printk(KERN_DEBUG "zfLnxDisconnect\n");
+
+    if (!macp)
+    {
+        printk(KERN_ERR "unregistering non-existant device\n");
+        return;
+    }
+
+    if (macp->driver_isolated)
+    {
+        if (macp->device->flags & IFF_UP)
+            usbdrv_close(macp->device);
+    }
+
+#if 0
+    /* Close WDS */
+    //zfWdsClose(wds[0].dev);
+    /* Unregister WDS */
+    //zfUnregisterWdsDev(macp->device, 0);
+
+    /* Close VAP */
+    zfLnxVapClose(vap[0].dev);
+    /* Unregister VAP */
+    zfLnxUnregisterVapDev(macp->device, 0);
+#endif
+
+    zfLnxClearStructs(macp->device);
+
+    unregister_netdev(macp->device);
+
+    usb_put_dev(interface_to_usbdev(interface));
+
+    //printk(KERN_ERR "3. zfLnxUnlinkAllUrbs\n");
+    //zfLnxUnlinkAllUrbs(macp);
+
+    /* Free network interface */
+    free_netdev(macp->device);
+
+    zfLnxFreeAllUrbs(macp);
+    //zfLnxClearStructs(macp->device);
+    kfree(macp);
+    macp = NULL;
+
+    usb_set_intfdata(interface, NULL);
+}
+
+static struct usb_driver zd1221_driver = {
+	.name         = driver_name,
+	.probe        = zfLnxProbe,
+	.disconnect   = zfLnxDisconnect,
+	.id_table     = zd1221_ids,
+};
+
+int __init zfLnxIinit(void)
+{
+    printk(KERN_NOTICE "%s - version %s\n",  DRIVER_NAME, VERSIONID);
+    return usb_register(&zd1221_driver);
+}
+
+void __exit zfLnxExit(void)
+{
+    usb_deregister(&zd1221_driver);
+}
+
+module_init(zfLnxIinit);
+module_exit(zfLnxExit);
diff --git a/drivers/staging/otus/zdusb.h b/drivers/staging/otus/zdusb.h
new file mode 100644
index 000000000000..656dc212ade5
--- /dev/null
+++ b/drivers/staging/otus/zdusb.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : zdusb.h                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains definitions for USB device driver           */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _ZDUSB_H
+#define _ZDUSB_H
+
+#ifndef DRIVER_NAME
+#define DRIVER_NAME             "arusb"
+#endif
+
+#define VERSIONID               "0.0.0.999"
+
+/* Define these values to match your device */
+#define VENDOR_ATHR             0x0CF3  //Atheros
+#define PRODUCT_AR9170          0x9170
+
+#define VENDOR_DLINK            0x07D1  //Dlink
+#define PRODUCT_DWA160A         0x3C10
+
+#endif
diff --git a/drivers/staging/panel/Kconfig b/drivers/staging/panel/Kconfig
new file mode 100644
index 000000000000..c4b30f2a549b
--- /dev/null
+++ b/drivers/staging/panel/Kconfig
@@ -0,0 +1,278 @@
+config PANEL
+	tristate "Parallel port LCD/Keypad Panel support"
+	depends on PARPORT
+	---help---
+	  Say Y here if you have an HD44780 or KS-0074 LCD connected to your
+	  parallel port. This driver also features 4 and 6-key keypads. The LCD
+	  is accessible through the /dev/lcd char device (10, 156), and the
+	  keypad through /dev/keypad (10, 185). Both require misc device to be
+	  enabled. This code can either be compiled as a module, or linked into
+	  the kernel and started at boot. If you don't understand what all this
+	  is about, say N.
+
+config PANEL_PARPORT
+	int "Default parallel port number (0=LPT1)"
+	depends on PANEL
+	range 0 255
+	default "0"
+	---help---
+	  This is the index of the parallel port the panel is connected to. One
+	  driver instance only supports one parallel port, so if your keypad
+	  and LCD are connected to two separate ports, you have to start two
+	  modules with different arguments. Numbering starts with '0' for LPT1,
+	  and so on.
+
+config PANEL_PROFILE
+	int "Default panel profile (0-5, 0=custom)"
+	depends on PANEL
+	range 0 5
+	default "5"
+	---help---
+	  To ease configuration, the driver supports different configuration
+	  profiles for past and recent wirings. These profiles can also be
+	  used to define an approximative configuration, completed by a few
+	  other options. Here are the profiles :
+
+	    0 = custom (see further)
+	    1 = 2x16 parallel LCD, old keypad
+	    2 = 2x16 serial LCD (KS-0074), new keypad
+	    3 = 2x16 parallel LCD (Hantronix), no keypad
+	    4 = 2x16 parallel LCD (Nexcom NSA1045) with Nexcom's keypad
+	    5 = 2x40 parallel LCD (old one), with old keypad
+
+	  Custom configurations allow you to define how your display is
+	  wired to the parallel port, and how it works. This is only intended
+	  for experts.
+
+config PANEL_KEYPAD
+	depends on PANEL && PANEL_PROFILE="0"
+	int "Keypad type (0=none, 1=old 6 keys, 2=new 6 keys, 3=Nexcom 4 keys)"
+	range 0 4
+	default 0
+	---help---
+	  This enables and configures a keypad connected to the parallel port.
+	  The keys will be read from character device 10,185. Valid values are :
+
+	    0 : do not enable this driver
+	    1 : old 6 keys keypad
+	    2 : new 6 keys keypad, as used on the server at www.ant-computing.com
+	    3 : Nexcom NSA1045's 4 keys keypad
+
+	  New profiles can be described in the driver source. The driver also
+	  supports simultaneous keys pressed when the keypad supports them.
+
+config PANEL_LCD
+	depends on PANEL && PANEL_PROFILE="0"
+	int "LCD type (0=none, 1=custom, 2=old //, 3=ks0074, 4=hantronix, 5=Nexcom)"
+	range 0 5
+	default 0
+	---help---
+	   This enables and configures an LCD connected to the parallel port.
+	   The driver includes an interpreter for escape codes starting with
+	   '\e[L' which are specific to the LCD, and a few ANSI codes. The
+	   driver will be registered as character device 10,156, usually
+	   under the name '/dev/lcd'. There are a total of 6 supported types :
+
+	     0 : do not enable the driver
+	     1 : custom configuration and wiring (see further)
+	     2 : 2x16 & 2x40 parallel LCD (old wiring)
+	     3 : 2x16 serial LCD (KS-0074 based)
+	     4 : 2x16 parallel LCD (Hantronix wiring)
+	     5 : 2x16 parallel LCD (Nexcom wiring)
+
+	   When type '1' is specified, other options will appear to configure
+	   more precise aspects (wiring, dimensions, protocol, ...). Please note
+	   that those values changed from the 2.4 driver for better consistency.
+
+config PANEL_LCD_HEIGHT
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1"
+	int "Number of lines on the LCD (1-2)"
+	range 1 2
+	default 2
+	---help---
+	  This is the number of visible character lines on the LCD in custom profile.
+	  It can either be 1 or 2.
+
+config PANEL_LCD_WIDTH
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1"
+	int "Number of characters per line on the LCD (1-40)"
+	range 1 40
+	default 40
+	---help---
+	  This is the number of characters per line on the LCD in custom profile.
+	  Common values are 16,20,24,40.
+
+config PANEL_LCD_BWIDTH
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1"
+	int "Internal LCD line width (1-40, 40 by default)"
+	range 1 40
+	default 40
+	---help---
+	  Most LCDs use a standard controller which supports hardware lines of 40
+	  characters, although sometimes only 16, 20 or 24 of them are really wired
+	  to the terminal. This results in some non-visible but adressable characters,
+	  and is the case for most parallel LCDs. Other LCDs, and some serial ones,
+	  however, use the same line width internally as what is visible. The KS0074
+	  for example, uses 16 characters per line for 16 visible characters per line.
+
+	  This option lets you configure the value used by your LCD in 'custom' profile.
+	  If you don't know, put '40' here.
+
+config PANEL_LCD_HWIDTH
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1"
+	int "Hardware LCD line width (1-64, 64 by default)"
+	range 1 64
+	default 64
+	---help---
+	  Most LCDs use a single address bit to differentiate line 0 and line 1. Since
+	  some of them need to be able to address 40 chars with the lower bits, they
+	  often use the immediately superior power of 2, which is 64, to address the
+	  next line.
+
+	  If you don't know what your LCD uses, in doubt let 16 here for a 2x16, and
+	  64 here for a 2x40.
+
+config PANEL_LCD_CHARSET
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1"
+	int "LCD character set (0=normal, 1=KS0074)"
+	range 0 1
+	default 0
+	---help---
+	  Some controllers such as the KS0074 use a somewhat strange character set
+	  where many symbols are at unusual places. The driver knows how to map
+	  'standard' ASCII characters to the character sets used by these controllers.
+	  Valid values are :
+
+	     0 : normal (untranslated) character set
+	     1 : KS0074 character set
+
+	  If you don't know, use the normal one (0).
+
+config PANEL_LCD_PROTO
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1"
+	int "LCD communication mode (0=parallel 8 bits, 1=serial)"
+	range 0 1
+	default 0
+	---help---
+	  This driver now supports any serial or parallel LCD wired to a parallel
+	  port. But before assigning signals, the driver needs to know if it will
+	  be driving a serial LCD or a parallel one. Serial LCDs only use 2 wires
+	  (SDA/SCL), while parallel ones use 2 or 3 wires for the control signals
+	  (E, RS, sometimes RW), and 4 or 8 for the data. Use 0 here for a 8 bits
+	  parallel LCD, and 1 for a serial LCD.
+
+config PANEL_LCD_PIN_E
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1" && PANEL_LCD_PROTO="0"
+        int "Parallel port pin number & polarity connected to the LCD E signal (-17...17) "
+	range -17 17
+	default 14
+	---help---
+	  This describes the number of the parallel port pin to which the LCD 'E'
+	  signal has been connected. It can be :
+
+	          0 : no connection (eg: connected to ground)
+	      1..17 : directly connected to any of these pins on the DB25 plug
+	    -1..-17 : connected to the same pin through an inverter (eg: transistor).
+
+	  Default for the 'E' pin in custom profile is '14' (AUTOFEED).
+
+config PANEL_LCD_PIN_RS
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1" && PANEL_LCD_PROTO="0"
+        int "Parallel port pin number & polarity connected to the LCD RS signal (-17...17) "
+	range -17 17
+	default 17
+	---help---
+	  This describes the number of the parallel port pin to which the LCD 'RS'
+	  signal has been connected. It can be :
+
+	          0 : no connection (eg: connected to ground)
+	      1..17 : directly connected to any of these pins on the DB25 plug
+	    -1..-17 : connected to the same pin through an inverter (eg: transistor).
+
+	  Default for the 'RS' pin in custom profile is '17' (SELECT IN).
+
+config PANEL_LCD_PIN_RW
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1" && PANEL_LCD_PROTO="0"
+        int "Parallel port pin number & polarity connected to the LCD RW signal (-17...17) "
+	range -17 17
+	default 16
+	---help---
+	  This describes the number of the parallel port pin to which the LCD 'RW'
+	  signal has been connected. It can be :
+
+	          0 : no connection (eg: connected to ground)
+	      1..17 : directly connected to any of these pins on the DB25 plug
+	    -1..-17 : connected to the same pin through an inverter (eg: transistor).
+
+	  Default for the 'RW' pin in custom profile is '16' (INIT).
+
+config PANEL_LCD_PIN_SCL
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1" && PANEL_LCD_PROTO!="0"
+        int "Parallel port pin number & polarity connected to the LCD SCL signal (-17...17) "
+	range -17 17
+	default 1
+	---help---
+	  This describes the number of the parallel port pin to which the serial
+	  LCD 'SCL' signal has been connected. It can be :
+
+	          0 : no connection (eg: connected to ground)
+	      1..17 : directly connected to any of these pins on the DB25 plug
+	    -1..-17 : connected to the same pin through an inverter (eg: transistor).
+
+	  Default for the 'SCL' pin in custom profile is '1' (STROBE).
+
+config PANEL_LCD_PIN_SDA
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1" && PANEL_LCD_PROTO!="0"
+        int "Parallel port pin number & polarity connected to the LCD SDA signal (-17...17) "
+	range -17 17
+	default 2
+	---help---
+	  This describes the number of the parallel port pin to which the serial
+	  LCD 'SDA' signal has been connected. It can be :
+
+	          0 : no connection (eg: connected to ground)
+	      1..17 : directly connected to any of these pins on the DB25 plug
+	    -1..-17 : connected to the same pin through an inverter (eg: transistor).
+
+	  Default for the 'SDA' pin in custom profile is '2' (D0).
+
+config PANEL_LCD_PIN_BL
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1"
+        int "Parallel port pin number & polarity connected to the LCD backlight signal (-17...17) "
+	range -17 17
+	default 0
+	---help---
+	  This describes the number of the parallel port pin to which the LCD 'BL' signal
+          has been connected. It can be :
+
+	          0 : no connection (eg: connected to ground)
+	      1..17 : directly connected to any of these pins on the DB25 plug
+	    -1..-17 : connected to the same pin through an inverter (eg: transistor).
+
+	  Default for the 'BL' pin in custom profile is '0' (uncontrolled).
+
+config PANEL_CHANGE_MESSAGE
+	depends on PANEL
+	bool "Change LCD initialization message ?"
+	default "n"
+	---help---
+	  This allows you to replace the boot message indicating the kernel version
+	  and the driver version with a custom message. This is useful on appliances
+	  where a simple 'Starting system' message can be enough to stop a customer
+	  from worrying.
+
+	  If you say 'Y' here, you'll be able to choose a message yourself. Otherwise,
+	  say 'N' and keep the default message with the version.
+
+config PANEL_BOOT_MESSAGE
+	depends on PANEL && PANEL_CHANGE_MESSAGE="y"
+	string "New initialization message"
+	default ""
+	---help---
+	  This allows you to replace the boot message indicating the kernel version
+	  and the driver version with a custom message. This is useful on appliances
+	  where a simple 'Starting system' message can be enough to stop a customer
+	  from worrying.
+
+	  An empty message will only clear the display at driver init time. Any other
+	  printf()-formatted message is valid with newline and escape codes.
diff --git a/drivers/staging/panel/Makefile b/drivers/staging/panel/Makefile
new file mode 100644
index 000000000000..747c238b82f9
--- /dev/null
+++ b/drivers/staging/panel/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_PANEL)		+= panel.o
diff --git a/drivers/staging/panel/TODO b/drivers/staging/panel/TODO
new file mode 100644
index 000000000000..a4be749bcdfc
--- /dev/null
+++ b/drivers/staging/panel/TODO
@@ -0,0 +1,9 @@
+TODO:
+	- checkpatch.pl cleanups
+	- Lindent
+	- review major/minor usages
+	- review userspace api
+	- see if all of this could be easier done in userspace instead.
+
+Please send patches to Greg Kroah-Hartman <greg@kroah.com> and
+Willy Tarreau <willy@meta-x.org>
diff --git a/drivers/staging/panel/lcd-panel-cgram.txt b/drivers/staging/panel/lcd-panel-cgram.txt
new file mode 100644
index 000000000000..f9ceef4322a3
--- /dev/null
+++ b/drivers/staging/panel/lcd-panel-cgram.txt
@@ -0,0 +1,24 @@
+Some LCDs allow you to define up to 8 characters, mapped to ASCII
+characters 0 to 7. The escape code to define a new character is
+'\e[LG' followed by one digit from 0 to 7, representing the character
+number, and up to 8 couples of hex digits terminated by a semi-colon
+(';'). Each couple of digits represents a line, with 1-bits for each
+illuminated pixel with LSB on the right. Lines are numberred from the
+top of the character to the bottom. On a 5x7 matrix, only the 5 lower
+bits of the 7 first bytes are used for each character. If the string
+is incomplete, only complete lines will be redefined. Here are some
+examples :
+
+  printf "\e[LG0010101050D1F0C04;"  => 0 = [enter]
+  printf "\e[LG1040E1F0000000000;"  => 1 = [up]
+  printf "\e[LG2000000001F0E0400;"  => 2 = [down]
+  printf "\e[LG3040E1F001F0E0400;"  => 3 = [up-down]
+  printf "\e[LG40002060E1E0E0602;"  => 4 = [left]
+  printf "\e[LG500080C0E0F0E0C08;"  => 5 = [right]
+  printf "\e[LG60016051516141400;"  => 6 = "IP"
+
+  printf "\e[LG00103071F1F070301;"  => big speaker
+  printf "\e[LG00002061E1E060200;"  => small speaker
+
+Willy
+
diff --git a/drivers/staging/panel/panel.c b/drivers/staging/panel/panel.c
new file mode 100644
index 000000000000..5ffe269c2382
--- /dev/null
+++ b/drivers/staging/panel/panel.c
@@ -0,0 +1,2193 @@
+/*
+ * Front panel driver for Linux
+ * Copyright (C) 2000-2008, Willy Tarreau <w@1wt.eu>
+ *
+ * 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 code drives an LCD module (/dev/lcd), and a keypad (/dev/keypad)
+ * connected to a parallel printer port.
+ *
+ * The LCD module may either be an HD44780-like 8-bit parallel LCD, or a 1-bit
+ * serial module compatible with Samsung's KS0074. The pins may be connected in
+ * any combination, everything is programmable.
+ *
+ * The keypad consists in a matrix of push buttons connecting input pins to
+ * data output pins or to the ground. The combinations have to be hard-coded
+ * in the driver, though several profiles exist and adding new ones is easy.
+ *
+ * Several profiles are provided for commonly found LCD+keypad modules on the
+ * market, such as those found in Nexcom's appliances.
+ *
+ * FIXME:
+ *      - the initialization/deinitialization process is very dirty and should
+ *        be rewritten. It may even be buggy.
+ *
+ * TODO:
+ *	- document 24 keys keyboard (3 rows of 8 cols, 32 diodes + 2 inputs)
+ *      - make the LCD a part of a virtual screen of Vx*Vy
+ *	- make the inputs list smp-safe
+ *      - change the keyboard to a double mapping : signals -> key_id -> values
+ *        so that applications can change values without knowing signals
+ *
+ */
+
+#include <linux/module.h>
+
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/signal.h>
+#include <linux/sched.h>
+#include <linux/spinlock.h>
+#include <linux/smp_lock.h>
+#include <linux/interrupt.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/ioport.h>
+#include <linux/fcntl.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/ctype.h>
+#include <linux/parport.h>
+#include <linux/version.h>
+#include <linux/list.h>
+#include <linux/notifier.h>
+#include <linux/reboot.h>
+#include <linux/utsrelease.h>
+
+#include <linux/io.h>
+#include <asm/uaccess.h>
+#include <asm/system.h>
+
+#define LCD_MINOR		156
+#define KEYPAD_MINOR		185
+
+#define PANEL_VERSION		"0.9.5"
+
+#define LCD_MAXBYTES		256	/* max burst write */
+
+#define KEYPAD_BUFFER		64
+#define INPUT_POLL_TIME		(HZ/50)	/* poll the keyboard this every second */
+#define KEYPAD_REP_START	(10)	/* a key starts to repeat after this times INPUT_POLL_TIME */
+#define KEYPAD_REP_DELAY	(2)	/* a key repeats this times INPUT_POLL_TIME */
+
+#define FLASH_LIGHT_TEMPO	(200)	/* keep the light on this times INPUT_POLL_TIME for each flash */
+
+/* converts an r_str() input to an active high, bits string : 000BAOSE */
+#define PNL_PINPUT(a)		((((unsigned char)(a)) ^ 0x7F) >> 3)
+
+#define PNL_PBUSY		0x80	/* inverted input, active low */
+#define PNL_PACK		0x40	/* direct input, active low */
+#define PNL_POUTPA		0x20	/* direct input, active high */
+#define PNL_PSELECD		0x10	/* direct input, active high */
+#define PNL_PERRORP		0x08	/* direct input, active low */
+
+#define PNL_PBIDIR		0x20	/* bi-directional ports */
+#define PNL_PINTEN		0x10	/* high to read data in or-ed with data out */
+#define PNL_PSELECP		0x08	/* inverted output, active low */
+#define PNL_PINITP		0x04	/* direct output, active low */
+#define PNL_PAUTOLF		0x02	/* inverted output, active low */
+#define PNL_PSTROBE		0x01	/* inverted output */
+
+#define PNL_PD0			0x01
+#define PNL_PD1			0x02
+#define PNL_PD2			0x04
+#define PNL_PD3			0x08
+#define PNL_PD4			0x10
+#define PNL_PD5			0x20
+#define PNL_PD6			0x40
+#define PNL_PD7			0x80
+
+#define PIN_NONE		0
+#define PIN_STROBE		1
+#define PIN_D0			2
+#define PIN_D1			3
+#define PIN_D2			4
+#define PIN_D3			5
+#define PIN_D4			6
+#define PIN_D5			7
+#define PIN_D6			8
+#define PIN_D7			9
+#define PIN_AUTOLF		14
+#define PIN_INITP		16
+#define PIN_SELECP		17
+#define PIN_NOT_SET		127
+
+#define LCD_FLAG_S		0x0001
+#define LCD_FLAG_ID		0x0002
+#define LCD_FLAG_B		0x0004	/* blink on */
+#define LCD_FLAG_C		0x0008	/* cursor on */
+#define LCD_FLAG_D		0x0010	/* display on */
+#define LCD_FLAG_F		0x0020	/* large font mode */
+#define LCD_FLAG_N		0x0040	/* 2-rows mode */
+#define LCD_FLAG_L		0x0080	/* backlight enabled */
+
+#define LCD_ESCAPE_LEN		24	/* 24 chars max for an LCD escape command */
+#define LCD_ESCAPE_CHAR	27	/* use char 27 for escape command */
+
+/* macros to simplify use of the parallel port */
+#define r_ctr(x)        (parport_read_control((x)->port))
+#define r_dtr(x)        (parport_read_data((x)->port))
+#define r_str(x)        (parport_read_status((x)->port))
+#define w_ctr(x, y)     do { parport_write_control((x)->port, (y)); } while (0)
+#define w_dtr(x, y)     do { parport_write_data((x)->port, (y)); } while (0)
+
+/* this defines which bits are to be used and which ones to be ignored */
+static __u8 scan_mask_o;	/* logical or of the output bits involved in the scan matrix */
+static __u8 scan_mask_i;	/* logical or of the input bits involved in the scan matrix */
+
+typedef __u64 pmask_t;
+
+enum input_type {
+	INPUT_TYPE_STD,
+	INPUT_TYPE_KBD,
+};
+
+enum input_state {
+	INPUT_ST_LOW,
+	INPUT_ST_RISING,
+	INPUT_ST_HIGH,
+	INPUT_ST_FALLING,
+};
+
+struct logical_input {
+	struct list_head list;
+	pmask_t mask;
+	pmask_t value;
+	enum input_type type;
+	enum input_state state;
+	__u8 rise_time, fall_time;
+	__u8 rise_timer, fall_timer, high_timer;
+
+	union {
+		struct {	/* this structure is valid when type == INPUT_TYPE_STD */
+			void (*press_fct) (int);
+			void (*release_fct) (int);
+			int press_data;
+			int release_data;
+		} std;
+		struct {	/* this structure is valid when type == INPUT_TYPE_KBD */
+			/* strings can be full-length (ie. non null-terminated) */
+			char press_str[sizeof(void *) + sizeof(int)];
+			char repeat_str[sizeof(void *) + sizeof(int)];
+			char release_str[sizeof(void *) + sizeof(int)];
+		} kbd;
+	} u;
+};
+
+LIST_HEAD(logical_inputs);	/* list of all defined logical inputs */
+
+/* physical contacts history
+ * Physical contacts are a 45 bits string of 9 groups of 5 bits each.
+ * The 8 lower groups correspond to output bits 0 to 7, and the 9th group
+ * corresponds to the ground.
+ * Within each group, bits are stored in the same order as read on the port :
+ * BAPSE (busy=4, ack=3, paper empty=2, select=1, error=0).
+ * So, each __u64 (or pmask_t) is represented like this :
+ * 0000000000000000000BAPSEBAPSEBAPSEBAPSEBAPSEBAPSEBAPSEBAPSEBAPSE
+ * <-----unused------><gnd><d07><d06><d05><d04><d03><d02><d01><d00>
+ */
+static pmask_t phys_read;	/* what has just been read from the I/O ports */
+static pmask_t phys_read_prev;	/* previous phys_read */
+static pmask_t phys_curr;	/* stabilized phys_read (phys_read|phys_read_prev) */
+static pmask_t phys_prev;	/* previous phys_curr */
+static char inputs_stable;	/* 0 means that at least one logical signal needs be computed */
+
+/* these variables are specific to the keypad */
+static char keypad_buffer[KEYPAD_BUFFER];
+static int keypad_buflen;
+static int keypad_start;
+static char keypressed;
+static wait_queue_head_t keypad_read_wait;
+
+/* lcd-specific variables */
+static unsigned long int lcd_flags;	/* contains the LCD config state */
+static unsigned long int lcd_addr_x;	/* contains the LCD X offset */
+static unsigned long int lcd_addr_y;	/* contains the LCD Y offset */
+static char lcd_escape[LCD_ESCAPE_LEN + 1];	/* current escape sequence, 0 terminated */
+static int lcd_escape_len = -1;	/* not in escape state. >=0 = escape cmd len */
+
+/*
+ * Bit masks to convert LCD signals to parallel port outputs.
+ * _d_ are values for data port, _c_ are for control port.
+ * [0] = signal OFF, [1] = signal ON, [2] = mask
+ */
+#define BIT_CLR		0
+#define BIT_SET		1
+#define BIT_MSK		2
+#define BIT_STATES	3
+/*
+ * one entry for each bit on the LCD
+ */
+#define LCD_BIT_E	0
+#define LCD_BIT_RS	1
+#define LCD_BIT_RW	2
+#define LCD_BIT_BL	3
+#define LCD_BIT_CL	4
+#define LCD_BIT_DA	5
+#define LCD_BITS	6
+
+/*
+ * each bit can be either connected to a DATA or CTRL port
+ */
+#define LCD_PORT_C	0
+#define LCD_PORT_D	1
+#define LCD_PORTS	2
+
+static unsigned char lcd_bits[LCD_PORTS][LCD_BITS][BIT_STATES];
+
+/*
+ * LCD protocols
+ */
+#define LCD_PROTO_PARALLEL      0
+#define LCD_PROTO_SERIAL        1
+
+/*
+ * LCD character sets
+ */
+#define LCD_CHARSET_NORMAL      0
+#define LCD_CHARSET_KS0074      1
+
+/*
+ * LCD types
+ */
+#define LCD_TYPE_NONE		0
+#define LCD_TYPE_OLD		1
+#define LCD_TYPE_KS0074		2
+#define LCD_TYPE_HANTRONIX	3
+#define LCD_TYPE_NEXCOM		4
+#define LCD_TYPE_CUSTOM		5
+
+/*
+ * keypad types
+ */
+#define KEYPAD_TYPE_NONE	0
+#define KEYPAD_TYPE_OLD		1
+#define KEYPAD_TYPE_NEW		2
+#define KEYPAD_TYPE_NEXCOM	3
+
+/*
+ * panel profiles
+ */
+#define PANEL_PROFILE_CUSTOM	0
+#define PANEL_PROFILE_OLD	1
+#define PANEL_PROFILE_NEW	2
+#define PANEL_PROFILE_HANTRONIX	3
+#define PANEL_PROFILE_NEXCOM	4
+#define PANEL_PROFILE_LARGE	5
+
+/*
+ * Construct custom config from the kernel's configuration
+ */
+#define DEFAULT_PROFILE         PANEL_PROFILE_LARGE
+#define DEFAULT_PARPORT         0
+#define DEFAULT_LCD             LCD_TYPE_OLD
+#define DEFAULT_KEYPAD          KEYPAD_TYPE_OLD
+#define DEFAULT_LCD_WIDTH       40
+#define DEFAULT_LCD_BWIDTH      40
+#define DEFAULT_LCD_HWIDTH      64
+#define DEFAULT_LCD_HEIGHT      2
+#define DEFAULT_LCD_PROTO       LCD_PROTO_PARALLEL
+
+#define DEFAULT_LCD_PIN_E       PIN_AUTOLF
+#define DEFAULT_LCD_PIN_RS      PIN_SELECP
+#define DEFAULT_LCD_PIN_RW      PIN_INITP
+#define DEFAULT_LCD_PIN_SCL     PIN_STROBE
+#define DEFAULT_LCD_PIN_SDA     PIN_D0
+#define DEFAULT_LCD_PIN_BL      PIN_NOT_SET
+#define DEFAULT_LCD_CHARSET     LCD_CHARSET_NORMAL
+
+#ifdef CONFIG_PANEL_PROFILE
+#undef DEFAULT_PROFILE
+#define DEFAULT_PROFILE CONFIG_PANEL_PROFILE
+#endif
+
+#ifdef CONFIG_PANEL_PARPORT
+#undef DEFAULT_PARPORT
+#define DEFAULT_PARPORT CONFIG_PANEL_PARPORT
+#endif
+
+#if DEFAULT_PROFILE == 0	/* custom */
+#ifdef CONFIG_PANEL_KEYPAD
+#undef DEFAULT_KEYPAD
+#define DEFAULT_KEYPAD CONFIG_PANEL_KEYPAD
+#endif
+
+#ifdef CONFIG_PANEL_LCD
+#undef DEFAULT_LCD
+#define DEFAULT_LCD CONFIG_PANEL_LCD
+#endif
+
+#ifdef CONFIG_PANEL_LCD_WIDTH
+#undef DEFAULT_LCD_WIDTH
+#define DEFAULT_LCD_WIDTH CONFIG_PANEL_LCD_WIDTH
+#endif
+
+#ifdef CONFIG_PANEL_LCD_BWIDTH
+#undef DEFAULT_LCD_BWIDTH
+#define DEFAULT_LCD_BWIDTH CONFIG_PANEL_LCD_BWIDTH
+#endif
+
+#ifdef CONFIG_PANEL_LCD_HWIDTH
+#undef DEFAULT_LCD_HWIDTH
+#define DEFAULT_LCD_HWIDTH CONFIG_PANEL_LCD_HWIDTH
+#endif
+
+#ifdef CONFIG_PANEL_LCD_HEIGHT
+#undef DEFAULT_LCD_HEIGHT
+#define DEFAULT_LCD_HEIGHT CONFIG_PANEL_LCD_HEIGHT
+#endif
+
+#ifdef CONFIG_PANEL_LCD_PROTO
+#undef DEFAULT_LCD_PROTO
+#define DEFAULT_LCD_PROTO CONFIG_PANEL_LCD_PROTO
+#endif
+
+#ifdef CONFIG_PANEL_LCD_PIN_E
+#undef DEFAULT_LCD_PIN_E
+#define DEFAULT_LCD_PIN_E CONFIG_PANEL_LCD_PIN_E
+#endif
+
+#ifdef CONFIG_PANEL_LCD_PIN_RS
+#undef DEFAULT_LCD_PIN_RS
+#define DEFAULT_LCD_PIN_RS CONFIG_PANEL_LCD_PIN_RS
+#endif
+
+#ifdef CONFIG_PANEL_LCD_PIN_RW
+#undef DEFAULT_LCD_PIN_RW
+#define DEFAULT_LCD_PIN_RW CONFIG_PANEL_LCD_PIN_RW
+#endif
+
+#ifdef CONFIG_PANEL_LCD_PIN_SCL
+#undef DEFAULT_LCD_PIN_SCL
+#define DEFAULT_LCD_PIN_SCL CONFIG_PANEL_LCD_PIN_SCL
+#endif
+
+#ifdef CONFIG_PANEL_LCD_PIN_SDA
+#undef DEFAULT_LCD_PIN_SDA
+#define DEFAULT_LCD_PIN_SDA CONFIG_PANEL_LCD_PIN_SDA
+#endif
+
+#ifdef CONFIG_PANEL_LCD_PIN_BL
+#undef DEFAULT_LCD_PIN_BL
+#define DEFAULT_LCD_PIN_BL CONFIG_PANEL_LCD_PIN_BL
+#endif
+
+#ifdef CONFIG_PANEL_LCD_CHARSET
+#undef DEFAULT_LCD_CHARSET
+#define DEFAULT_LCD_CHARSET
+#endif
+
+#endif /* DEFAULT_PROFILE == 0 */
+
+/* global variables */
+static int keypad_open_cnt;	/* #times opened */
+static int lcd_open_cnt;	/* #times opened */
+static struct pardevice *pprt;
+
+static int lcd_initialized;
+static int keypad_initialized;
+
+static int light_tempo;
+
+static char lcd_must_clear;
+static char lcd_left_shift;
+static char init_in_progress;
+
+static void (*lcd_write_cmd) (int);
+static void (*lcd_write_data) (int);
+static void (*lcd_clear_fast) (void);
+
+static DEFINE_SPINLOCK(pprt_lock);
+static struct timer_list scan_timer;
+
+MODULE_DESCRIPTION("Generic parallel port LCD/Keypad driver");
+
+static int parport = -1;
+module_param(parport, int, 0000);
+MODULE_PARM_DESC(parport, "Parallel port index (0=lpt1, 1=lpt2, ...)");
+
+static int lcd_height = -1;
+module_param(lcd_height, int, 0000);
+MODULE_PARM_DESC(lcd_height, "Number of lines on the LCD");
+
+static int lcd_width = -1;
+module_param(lcd_width, int, 0000);
+MODULE_PARM_DESC(lcd_width, "Number of columns on the LCD");
+
+static int lcd_bwidth = -1;	/* internal buffer width (usually 40) */
+module_param(lcd_bwidth, int, 0000);
+MODULE_PARM_DESC(lcd_bwidth, "Internal LCD line width (40)");
+
+static int lcd_hwidth = -1;	/* hardware buffer width (usually 64) */
+module_param(lcd_hwidth, int, 0000);
+MODULE_PARM_DESC(lcd_hwidth, "LCD line hardware address (64)");
+
+static int lcd_enabled = -1;
+module_param(lcd_enabled, int, 0000);
+MODULE_PARM_DESC(lcd_enabled, "Deprecated option, use lcd_type instead");
+
+static int keypad_enabled = -1;
+module_param(keypad_enabled, int, 0000);
+MODULE_PARM_DESC(keypad_enabled, "Deprecated option, use keypad_type instead");
+
+static int lcd_type = -1;
+module_param(lcd_type, int, 0000);
+MODULE_PARM_DESC(lcd_type,
+		 "LCD type: 0=none, 1=old //, 2=serial ks0074, 3=hantronix //, 4=nexcom //, 5=compiled-in");
+
+static int lcd_proto = -1;
+module_param(lcd_proto, int, 0000);
+MODULE_PARM_DESC(lcd_proto, "LCD communication: 0=parallel (//), 1=serial");
+
+static int lcd_charset = -1;
+module_param(lcd_charset, int, 0000);
+MODULE_PARM_DESC(lcd_charset, "LCD character set: 0=standard, 1=KS0074");
+
+static int keypad_type = -1;
+module_param(keypad_type, int, 0000);
+MODULE_PARM_DESC(keypad_type,
+		 "Keypad type: 0=none, 1=old 6 keys, 2=new 6+1 keys, 3=nexcom 4 keys");
+
+static int profile = DEFAULT_PROFILE;
+module_param(profile, int, 0000);
+MODULE_PARM_DESC(profile,
+		 "1=16x2 old kp; 2=serial 16x2, new kp; 3=16x2 hantronix; 4=16x2 nexcom; default=40x2, old kp");
+
+/*
+ * These are the parallel port pins the LCD control signals are connected to.
+ * Set this to 0 if the signal is not used. Set it to its opposite value
+ * (negative) if the signal is negated. -MAXINT is used to indicate that the
+ * pin has not been explicitly specified.
+ *
+ * WARNING! no check will be performed about collisions with keypad !
+ */
+
+static int lcd_e_pin  = PIN_NOT_SET;
+module_param(lcd_e_pin, int, 0000);
+MODULE_PARM_DESC(lcd_e_pin,
+		 "# of the // port pin connected to LCD 'E' signal, with polarity (-17..17)");
+
+static int lcd_rs_pin = PIN_NOT_SET;
+module_param(lcd_rs_pin, int, 0000);
+MODULE_PARM_DESC(lcd_rs_pin,
+		 "# of the // port pin connected to LCD 'RS' signal, with polarity (-17..17)");
+
+static int lcd_rw_pin = PIN_NOT_SET;
+module_param(lcd_rw_pin, int, 0000);
+MODULE_PARM_DESC(lcd_rw_pin,
+		 "# of the // port pin connected to LCD 'RW' signal, with polarity (-17..17)");
+
+static int lcd_bl_pin = PIN_NOT_SET;
+module_param(lcd_bl_pin, int, 0000);
+MODULE_PARM_DESC(lcd_bl_pin,
+		 "# of the // port pin connected to LCD backlight, with polarity (-17..17)");
+
+static int lcd_da_pin = PIN_NOT_SET;
+module_param(lcd_da_pin, int, 0000);
+MODULE_PARM_DESC(lcd_da_pin,
+		 "# of the // port pin connected to serial LCD 'SDA' signal, with polarity (-17..17)");
+
+static int lcd_cl_pin = PIN_NOT_SET;
+module_param(lcd_cl_pin, int, 0000);
+MODULE_PARM_DESC(lcd_cl_pin,
+		 "# of the // port pin connected to serial LCD 'SCL' signal, with polarity (-17..17)");
+
+static unsigned char *lcd_char_conv;
+
+/* for some LCD drivers (ks0074) we need a charset conversion table. */
+static unsigned char lcd_char_conv_ks0074[256] = {
+	/*          0|8   1|9   2|A   3|B   4|C   5|D   6|E   7|F */
+	/* 0x00 */ 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+	/* 0x08 */ 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
+	/* 0x10 */ 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17,
+	/* 0x18 */ 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f,
+	/* 0x20 */ 0x20, 0x21, 0x22, 0x23, 0xa2, 0x25, 0x26, 0x27,
+	/* 0x28 */ 0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f,
+	/* 0x30 */ 0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37,
+	/* 0x38 */ 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f,
+	/* 0x40 */ 0xa0, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47,
+	/* 0x48 */ 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 0x4f,
+	/* 0x50 */ 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57,
+	/* 0x58 */ 0x58, 0x59, 0x5a, 0xfa, 0xfb, 0xfc, 0x1d, 0xc4,
+	/* 0x60 */ 0x96, 0x61, 0x62, 0x63, 0x64, 0x65, 0x66, 0x67,
+	/* 0x68 */ 0x68, 0x69, 0x6a, 0x6b, 0x6c, 0x6d, 0x6e, 0x6f,
+	/* 0x70 */ 0x70, 0x71, 0x72, 0x73, 0x74, 0x75, 0x76, 0x77,
+	/* 0x78 */ 0x78, 0x79, 0x7a, 0xfd, 0xfe, 0xff, 0xce, 0x20,
+	/* 0x80 */ 0x80, 0x81, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87,
+	/* 0x88 */ 0x88, 0x89, 0x8a, 0x8b, 0x8c, 0x8d, 0x8e, 0x8f,
+	/* 0x90 */ 0x90, 0x91, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97,
+	/* 0x98 */ 0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f,
+	/* 0xA0 */ 0x20, 0x40, 0xb1, 0xa1, 0x24, 0xa3, 0xfe, 0x5f,
+	/* 0xA8 */ 0x22, 0xc8, 0x61, 0x14, 0x97, 0x2d, 0xad, 0x96,
+	/* 0xB0 */ 0x80, 0x8c, 0x82, 0x83, 0x27, 0x8f, 0x86, 0xdd,
+	/* 0xB8 */ 0x2c, 0x81, 0x6f, 0x15, 0x8b, 0x8a, 0x84, 0x60,
+	/* 0xC0 */ 0xe2, 0xe2, 0xe2, 0x5b, 0x5b, 0xae, 0xbc, 0xa9,
+	/* 0xC8 */ 0xc5, 0xbf, 0xc6, 0xf1, 0xe3, 0xe3, 0xe3, 0xe3,
+	/* 0xD0 */ 0x44, 0x5d, 0xa8, 0xe4, 0xec, 0xec, 0x5c, 0x78,
+	/* 0xD8 */ 0xab, 0xa6, 0xe5, 0x5e, 0x5e, 0xe6, 0xaa, 0xbe,
+	/* 0xE0 */ 0x7f, 0xe7, 0xaf, 0x7b, 0x7b, 0xaf, 0xbd, 0xc8,
+	/* 0xE8 */ 0xa4, 0xa5, 0xc7, 0xf6, 0xa7, 0xe8, 0x69, 0x69,
+	/* 0xF0 */ 0xed, 0x7d, 0xa8, 0xe4, 0xec, 0x5c, 0x5c, 0x25,
+	/* 0xF8 */ 0xac, 0xa6, 0xea, 0xef, 0x7e, 0xeb, 0xb2, 0x79,
+};
+
+char old_keypad_profile[][4][9] = {
+	{"S0", "Left\n", "Left\n", ""},
+	{"S1", "Down\n", "Down\n", ""},
+	{"S2", "Up\n", "Up\n", ""},
+	{"S3", "Right\n", "Right\n", ""},
+	{"S4", "Esc\n", "Esc\n", ""},
+	{"S5", "Ret\n", "Ret\n", ""},
+	{"", "", "", ""}
+};
+
+/* signals, press, repeat, release */
+char new_keypad_profile[][4][9] = {
+	{"S0", "Left\n", "Left\n", ""},
+	{"S1", "Down\n", "Down\n", ""},
+	{"S2", "Up\n", "Up\n", ""},
+	{"S3", "Right\n", "Right\n", ""},
+	{"S4s5", "", "Esc\n", "Esc\n"},
+	{"s4S5", "", "Ret\n", "Ret\n"},
+	{"S4S5", "Help\n", "", ""},
+	/* add new signals above this line */
+	{"", "", "", ""}
+};
+
+/* signals, press, repeat, release */
+char nexcom_keypad_profile[][4][9] = {
+	{"a-p-e-", "Down\n", "Down\n", ""},
+	{"a-p-E-", "Ret\n", "Ret\n", ""},
+	{"a-P-E-", "Esc\n", "Esc\n", ""},
+	{"a-P-e-", "Up\n", "Up\n", ""},
+	/* add new signals above this line */
+	{"", "", "", ""}
+};
+
+static char (*keypad_profile)[4][9] = old_keypad_profile;
+
+/* FIXME: this should be converted to a bit array containing signals states */
+static struct {
+	unsigned char e;	/* parallel LCD E   (data latch on falling edge) */
+	unsigned char rs;	/* parallel LCD RS  (0 = cmd, 1 = data) */
+	unsigned char rw;	/* parallel LCD R/W (0 = W, 1 = R) */
+	unsigned char bl;	/* parallel LCD backlight (0 = off, 1 = on) */
+	unsigned char cl;	/* serial LCD clock (latch on rising edge) */
+	unsigned char da;	/* serial LCD data */
+} bits;
+
+static void init_scan_timer(void);
+
+/* sets data port bits according to current signals values */
+static int set_data_bits(void)
+{
+	int val, bit;
+
+	val = r_dtr(pprt);
+	for (bit = 0; bit < LCD_BITS; bit++)
+		val &= lcd_bits[LCD_PORT_D][bit][BIT_MSK];
+
+	val |= lcd_bits[LCD_PORT_D][LCD_BIT_E][bits.e]
+	    | lcd_bits[LCD_PORT_D][LCD_BIT_RS][bits.rs]
+	    | lcd_bits[LCD_PORT_D][LCD_BIT_RW][bits.rw]
+	    | lcd_bits[LCD_PORT_D][LCD_BIT_BL][bits.bl]
+	    | lcd_bits[LCD_PORT_D][LCD_BIT_CL][bits.cl]
+	    | lcd_bits[LCD_PORT_D][LCD_BIT_DA][bits.da];
+
+	w_dtr(pprt, val);
+	return val;
+}
+
+/* sets ctrl port bits according to current signals values */
+static int set_ctrl_bits(void)
+{
+	int val, bit;
+
+	val = r_ctr(pprt);
+	for (bit = 0; bit < LCD_BITS; bit++)
+		val &= lcd_bits[LCD_PORT_C][bit][BIT_MSK];
+
+	val |= lcd_bits[LCD_PORT_C][LCD_BIT_E][bits.e]
+	    | lcd_bits[LCD_PORT_C][LCD_BIT_RS][bits.rs]
+	    | lcd_bits[LCD_PORT_C][LCD_BIT_RW][bits.rw]
+	    | lcd_bits[LCD_PORT_C][LCD_BIT_BL][bits.bl]
+	    | lcd_bits[LCD_PORT_C][LCD_BIT_CL][bits.cl]
+	    | lcd_bits[LCD_PORT_C][LCD_BIT_DA][bits.da];
+
+	w_ctr(pprt, val);
+	return val;
+}
+
+/* sets ctrl & data port bits according to current signals values */
+static void set_bits(void)
+{
+	set_data_bits();
+	set_ctrl_bits();
+}
+
+/*
+ * Converts a parallel port pin (from -25 to 25) to data and control ports
+ * masks, and data and control port bits. The signal will be considered
+ * unconnected if it's on pin 0 or an invalid pin (<-25 or >25).
+ *
+ * Result will be used this way :
+ *   out(dport, in(dport) & d_val[2] | d_val[signal_state])
+ *   out(cport, in(cport) & c_val[2] | c_val[signal_state])
+ */
+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;
+
+	if (pin == 0)
+		return;
+
+	inv = (pin < 0);
+	if (inv)
+		pin = -pin;
+
+	d_bit = c_bit = 0;
+
+	switch (pin) {
+	case PIN_STROBE:	/* strobe, inverted */
+		c_bit = PNL_PSTROBE;
+		inv = !inv;
+		break;
+	case PIN_D0...PIN_D7:	/* D0 - D7 = 2 - 9 */
+		d_bit = 1 << (pin - 2);
+		break;
+	case PIN_AUTOLF:	/* autofeed, inverted */
+		c_bit = PNL_PAUTOLF;
+		inv = !inv;
+		break;
+	case PIN_INITP:	/* init, direct */
+		c_bit = PNL_PINITP;
+		break;
+	case PIN_SELECP:	/* select_in, inverted */
+		c_bit = PNL_PSELECP;
+		inv = !inv;
+		break;
+	default:		/* unknown pin, ignore */
+		break;
+	}
+
+	if (c_bit) {
+		c_val[2] &= ~c_bit;
+		c_val[!inv] = c_bit;
+	} else if (d_bit) {
+		d_val[2] &= ~d_bit;
+		d_val[!inv] = d_bit;
+	}
+}
+
+/* sleeps that many milliseconds with a reschedule */
+static void long_sleep(int ms)
+{
+
+	if (in_interrupt())
+		mdelay(ms);
+	else {
+		current->state = TASK_INTERRUPTIBLE;
+		schedule_timeout((ms * HZ + 999) / 1000);
+	}
+}
+
+/* send a serial byte to the LCD panel. The caller is responsible for locking if needed. */
+static void lcd_send_serial(int byte)
+{
+	int bit;
+
+	/* the data bit is set on D0, and the clock on STROBE.
+	 * LCD reads D0 on STROBE's rising edge.
+	 */
+	for (bit = 0; bit < 8; bit++) {
+		bits.cl = BIT_CLR;	/* CLK low */
+		set_bits();
+		bits.da = byte & 1;
+		set_bits();
+		udelay(2);	/* maintain the data during 2 us before CLK up */
+		bits.cl = BIT_SET;	/* CLK high */
+		set_bits();
+		udelay(1);	/* maintain the strobe during 1 us */
+		byte >>= 1;
+	}
+}
+
+/* turn the backlight on or off */
+static void lcd_backlight(int on)
+{
+	if (lcd_bl_pin == PIN_NONE)
+		return;
+
+	/* The backlight is activated by seting the AUTOFEED line to +5V  */
+	spin_lock(&pprt_lock);
+	bits.bl = on;
+	set_bits();
+	spin_unlock(&pprt_lock);
+}
+
+/* send a command to the LCD panel in serial mode */
+static void lcd_write_cmd_s(int cmd)
+{
+	spin_lock(&pprt_lock);
+	lcd_send_serial(0x1F);	/* R/W=W, RS=0 */
+	lcd_send_serial(cmd & 0x0F);
+	lcd_send_serial((cmd >> 4) & 0x0F);
+	udelay(40);		/* the shortest command takes at least 40 us */
+	spin_unlock(&pprt_lock);
+}
+
+/* send data to the LCD panel in serial mode */
+static void lcd_write_data_s(int data)
+{
+	spin_lock(&pprt_lock);
+	lcd_send_serial(0x5F);	/* R/W=W, RS=1 */
+	lcd_send_serial(data & 0x0F);
+	lcd_send_serial((data >> 4) & 0x0F);
+	udelay(40);		/* the shortest data takes at least 40 us */
+	spin_unlock(&pprt_lock);
+}
+
+/* send a command to the LCD panel in 8 bits parallel mode */
+static void lcd_write_cmd_p8(int cmd)
+{
+	spin_lock(&pprt_lock);
+	/* present the data to the data port */
+	w_dtr(pprt, cmd);
+	udelay(20);		/* maintain the data during 20 us before the strobe */
+
+	bits.e = BIT_SET;
+	bits.rs = BIT_CLR;
+	bits.rw = BIT_CLR;
+	set_ctrl_bits();
+
+	udelay(40);		/* maintain the strobe during 40 us */
+
+	bits.e = BIT_CLR;
+	set_ctrl_bits();
+
+	udelay(120);		/* the shortest command takes at least 120 us */
+	spin_unlock(&pprt_lock);
+}
+
+/* send data to the LCD panel in 8 bits parallel mode */
+static void lcd_write_data_p8(int data)
+{
+	spin_lock(&pprt_lock);
+	/* present the data to the data port */
+	w_dtr(pprt, data);
+	udelay(20);		/* maintain the data during 20 us before the strobe */
+
+	bits.e = BIT_SET;
+	bits.rs = BIT_SET;
+	bits.rw = BIT_CLR;
+	set_ctrl_bits();
+
+	udelay(40);		/* maintain the strobe during 40 us */
+
+	bits.e = BIT_CLR;
+	set_ctrl_bits();
+
+	udelay(45);		/* the shortest data takes at least 45 us */
+	spin_unlock(&pprt_lock);
+}
+
+static void lcd_gotoxy(void)
+{
+	lcd_write_cmd(0x80	/* set DDRAM address */
+		      | (lcd_addr_y ? lcd_hwidth : 0)
+		      /* we force the cursor to stay at the end of the line if it wants to go farther */
+		      | ((lcd_addr_x < lcd_bwidth) ? lcd_addr_x &
+			 (lcd_hwidth - 1) : lcd_bwidth - 1));
+}
+
+static void lcd_print(char c)
+{
+	if (lcd_addr_x < lcd_bwidth) {
+		if (lcd_char_conv != NULL)
+			c = lcd_char_conv[(unsigned char)c];
+		lcd_write_data(c);
+		lcd_addr_x++;
+	}
+	/* prevents the cursor from wrapping onto the next line */
+	if (lcd_addr_x == lcd_bwidth)
+		lcd_gotoxy();
+}
+
+/* fills the display with spaces and resets X/Y */
+static void lcd_clear_fast_s(void)
+{
+	int pos;
+	lcd_addr_x = lcd_addr_y = 0;
+	lcd_gotoxy();
+
+	spin_lock(&pprt_lock);
+	for (pos = 0; pos < lcd_height * lcd_hwidth; pos++) {
+		lcd_send_serial(0x5F);	/* R/W=W, RS=1 */
+		lcd_send_serial(' ' & 0x0F);
+		lcd_send_serial((' ' >> 4) & 0x0F);
+		udelay(40);	/* the shortest data takes at least 40 us */
+	}
+	spin_unlock(&pprt_lock);
+
+	lcd_addr_x = lcd_addr_y = 0;
+	lcd_gotoxy();
+}
+
+/* fills the display with spaces and resets X/Y */
+static void lcd_clear_fast_p8(void)
+{
+	int pos;
+	lcd_addr_x = lcd_addr_y = 0;
+	lcd_gotoxy();
+
+	spin_lock(&pprt_lock);
+	for (pos = 0; pos < lcd_height * lcd_hwidth; pos++) {
+		/* present the data to the data port */
+		w_dtr(pprt, ' ');
+		udelay(20);	/* maintain the data during 20 us before the strobe */
+
+		bits.e = BIT_SET;
+		bits.rs = BIT_SET;
+		bits.rw = BIT_CLR;
+		set_ctrl_bits();
+
+		udelay(40);	/* maintain the strobe during 40 us */
+
+		bits.e = BIT_CLR;
+		set_ctrl_bits();
+
+		udelay(45);	/* the shortest data takes at least 45 us */
+	}
+	spin_unlock(&pprt_lock);
+
+	lcd_addr_x = lcd_addr_y = 0;
+	lcd_gotoxy();
+}
+
+/* clears the display and resets X/Y */
+static void lcd_clear_display(void)
+{
+	lcd_write_cmd(0x01);	/* clear display */
+	lcd_addr_x = 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;
+
+	long_sleep(20);		/* wait 20 ms after power-up for the paranoid */
+
+	lcd_write_cmd(0x30);	/* 8bits, 1 line, small fonts */
+	long_sleep(10);
+	lcd_write_cmd(0x30);	/* 8bits, 1 line, small fonts */
+	long_sleep(10);
+	lcd_write_cmd(0x30);	/* 8bits, 1 line, small fonts */
+	long_sleep(10);
+
+	lcd_write_cmd(0x30	/* set font height and lines number */
+		      | ((lcd_flags & LCD_FLAG_F) ? 4 : 0)
+		      | ((lcd_flags & LCD_FLAG_N) ? 8 : 0)
+	    );
+	long_sleep(10);
+
+	lcd_write_cmd(0x08);	/* display off, cursor off, blink off */
+	long_sleep(10);
+
+	lcd_write_cmd(0x08	/* set display mode */
+		      | ((lcd_flags & LCD_FLAG_D) ? 4 : 0)
+		      | ((lcd_flags & LCD_FLAG_C) ? 2 : 0)
+		      | ((lcd_flags & LCD_FLAG_B) ? 1 : 0)
+	    );
+
+	lcd_backlight((lcd_flags & LCD_FLAG_L) ? 1 : 0);
+
+	long_sleep(10);
+
+	lcd_write_cmd(0x06);	/* entry mode set : increment, cursor shifting */
+
+	lcd_clear_display();
+}
+
+/*
+ * These are the file operation function for user access to /dev/lcd
+ * This function can also be called from inside the kernel, by
+ * setting file and ppos to NULL.
+ *
+ */
+
+static ssize_t lcd_write(struct file *file,
+			 const char *buf, size_t count, loff_t *ppos)
+{
+
+	const char *tmp = buf;
+	char c;
+
+	for (; count-- > 0; (ppos ? (*ppos)++ : 0), ++tmp) {
+		if (!in_interrupt() && (((count + 1) & 0x1f) == 0))
+			schedule();	/* let's be a little nice with other processes that need some CPU */
+
+		if (ppos == NULL && file == NULL)
+			c = *tmp;	/* let's not use get_user() from the kernel ! */
+		else 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 {
+			lcd_escape_len = -1;	/* aborts any previous escape sequence */
+
+			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) {
+					if (lcd_addr_x < lcd_bwidth)	/* check if we're not at the end of the line */
+						lcd_write_cmd(0x10);	/* back one char */
+					lcd_addr_x--;
+				}
+				lcd_write_data(' ');	/* replace with a space */
+				lcd_write_cmd(0x10);	/* back one char again */
+				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) {	/* minimal length for an escape command */
+			int processed = 0;	/* 1 means the command has been processed */
+
+			if (!strcmp(lcd_escape, "[2J")) {	/* Clear the display */
+				lcd_clear_fast();	/* clear display */
+				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')) {	/* LCD special codes */
+
+				char *esc = lcd_escape + 2;
+				int oldflags = lcd_flags;
+
+				/* check for display mode flags */
+				switch (*esc) {
+				case 'D':	/* Display ON */
+					lcd_flags |= LCD_FLAG_D;
+					processed = 1;
+					break;
+				case 'd':	/* Display OFF */
+					lcd_flags &= ~LCD_FLAG_D;
+					processed = 1;
+					break;
+				case 'C':	/* Cursor ON */
+					lcd_flags |= LCD_FLAG_C;
+					processed = 1;
+					break;
+				case 'c':	/* Cursor OFF */
+					lcd_flags &= ~LCD_FLAG_C;
+					processed = 1;
+					break;
+				case 'B':	/* Blink ON */
+					lcd_flags |= LCD_FLAG_B;
+					processed = 1;
+					break;
+				case 'b':	/* Blink OFF */
+					lcd_flags &= ~LCD_FLAG_B;
+					processed = 1;
+					break;
+				case '+':	/* Back light ON */
+					lcd_flags |= LCD_FLAG_L;
+					processed = 1;
+					break;
+				case '-':	/* Back light OFF */
+					lcd_flags &= ~LCD_FLAG_L;
+					processed = 1;
+					break;
+				case '*':	/* flash back light using the keypad timer */
+					if (scan_timer.function != NULL) {
+						if (light_tempo == 0
+						    && ((lcd_flags & LCD_FLAG_L)
+							== 0))
+							lcd_backlight(1);
+						light_tempo = FLASH_LIGHT_TEMPO;
+					}
+					processed = 1;
+					break;
+				case 'f':	/* Small Font */
+					lcd_flags &= ~LCD_FLAG_F;
+					processed = 1;
+					break;
+				case 'F':	/* Large Font */
+					lcd_flags |= LCD_FLAG_F;
+					processed = 1;
+					break;
+				case 'n':	/* One Line */
+					lcd_flags &= ~LCD_FLAG_N;
+					processed = 1;
+					break;
+				case 'N':	/* Two Lines */
+					lcd_flags |= LCD_FLAG_N;
+					break;
+
+				case 'l':	/* Shift Cursor Left */
+					if (lcd_addr_x > 0) {
+						if (lcd_addr_x < lcd_bwidth)
+							lcd_write_cmd(0x10);	/* back one char if not at end of line */
+						lcd_addr_x--;
+					}
+					processed = 1;
+					break;
+
+				case 'r':	/* shift cursor right */
+					if (lcd_addr_x < lcd_width) {
+						if (lcd_addr_x < (lcd_bwidth - 1))
+							lcd_write_cmd(0x14);	/* allow the cursor to pass the end of the line */
+						lcd_addr_x++;
+					}
+					processed = 1;
+					break;
+
+				case 'L':	/* shift display left */
+					lcd_left_shift++;
+					lcd_write_cmd(0x18);
+					processed = 1;
+					break;
+
+				case 'R':	/* shift display right */
+					lcd_left_shift--;
+					lcd_write_cmd(0x1C);
+					processed = 1;
+					break;
+
+				case 'k':{	/* kill end of line */
+						int x;
+						for (x = lcd_addr_x; x < lcd_bwidth; x++)
+							lcd_write_data(' ');
+						lcd_gotoxy();	/* restore cursor position */
+						processed = 1;
+						break;
+					}
+				case 'I':	/* reinitialize display */
+					lcd_init_display();
+					lcd_left_shift = 0;
+					processed = 1;
+					break;
+
+				case 'G':	/* Generator : LGcxxxxx...xx; */  {
+						/* must have <c> between '0' and '7', representing the numerical
+						 * ASCII code of the redefined character, and <xx...xx> a sequence
+						 * of 16 hex digits representing 8 bytes for each character. Most
+						 * LCDs will only use 5 lower bits of the 7 first bytes.
+						 */
+
+						unsigned char cgbytes[8];
+						unsigned char cgaddr;
+						int cgoffset;
+						int shift;
+						char value;
+						int addr;
+
+						if (strchr(esc, ';') == NULL)
+							break;
+
+						esc++;
+
+						cgaddr = *(esc++) - '0';
+						if (cgaddr > 7) {
+							processed = 1;
+							break;
+						}
+
+						cgoffset = 0;
+						shift = 0;
+						value = 0;
+						while (*esc && cgoffset < 8) {
+							shift ^= 4;
+							if (*esc >= '0' && *esc <= '9')
+								value |= (*esc - '0') << shift;
+							else if (*esc >= 'A' && *esc <= 'Z')
+								value |= (*esc - 'A' + 10) << shift;
+							else if (*esc >= 'a' && *esc <= 'z')
+								value |= (*esc - 'a' + 10) << shift;
+							else {
+								esc++;
+								continue;
+							}
+
+							if (shift == 0) {
+								cgbytes[cgoffset++] = value;
+								value = 0;
+							}
+
+							esc++;
+						}
+
+						lcd_write_cmd(0x40 | (cgaddr * 8));
+						for (addr = 0; addr < cgoffset; addr++)
+							lcd_write_data(cgbytes[addr]);
+
+						lcd_gotoxy();	/* ensures that we stop writing to CGRAM */
+						processed = 1;
+						break;
+					}
+				case 'x':	/* gotoxy : LxXXX[yYYY]; */
+				case 'y':	/* gotoxy : LyYYY[xXXX]; */
+					if (strchr(esc, ';') == NULL)
+						break;
+
+					while (*esc) {
+						if (*esc == 'x') {
+							esc++;
+							lcd_addr_x = 0;
+							while (isdigit(*esc)) {
+								lcd_addr_x =
+								    lcd_addr_x *
+								    10 + (*esc -
+									  '0');
+								esc++;
+							}
+						} else if (*esc == 'y') {
+							esc++;
+							lcd_addr_y = 0;
+							while (isdigit(*esc)) {
+								lcd_addr_y =
+								    lcd_addr_y *
+								    10 + (*esc -
+									  '0');
+								esc++;
+							}
+						} else
+							break;
+					}
+
+					lcd_gotoxy();
+					processed = 1;
+					break;
+				}	/* end of switch */
+
+				/* Check wether one flag was changed */
+				if (oldflags != lcd_flags) {
+					/* check wether one of B,C,D flags was changed */
+					if ((oldflags ^ lcd_flags) &
+					    (LCD_FLAG_B | LCD_FLAG_C | LCD_FLAG_D))
+						/* set display mode */
+						lcd_write_cmd(0x08 |
+							      ((lcd_flags & LCD_FLAG_D) ? 4 : 0) |
+							      ((lcd_flags & LCD_FLAG_C) ? 2 : 0) |
+							      ((lcd_flags & LCD_FLAG_B) ? 1 : 0));
+					/* check wether one of F,N flags was changed */
+					else if ((oldflags ^ lcd_flags) &
+						 (LCD_FLAG_F | LCD_FLAG_N))
+						lcd_write_cmd(0x30 |
+							      ((lcd_flags & LCD_FLAG_F) ? 4 : 0) |
+							      ((lcd_flags & LCD_FLAG_N) ? 8 : 0));
+					/* check wether L flag was changed */
+					else if ((oldflags ^ lcd_flags) &
+						 (LCD_FLAG_L)) {
+						if (lcd_flags & (LCD_FLAG_L))
+							lcd_backlight(1);
+						else if (light_tempo == 0)	/* switch off the light only when the tempo lighting is gone */
+							lcd_backlight(0);
+					}
+				}
+			}
+
+			/* 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 */
+	}
+
+	return tmp - buf;
+}
+
+static int lcd_open(struct inode *inode, struct file *file)
+{
+	if (lcd_open_cnt)
+		return -EBUSY;	/* open only once at a time */
+
+	if (file->f_mode & FMODE_READ)	/* device is write-only */
+		return -EPERM;
+
+	if (lcd_must_clear) {
+		lcd_clear_display();
+		lcd_must_clear = 0;
+	}
+	lcd_open_cnt++;
+	return 0;
+}
+
+static int lcd_release(struct inode *inode, struct file *file)
+{
+	lcd_open_cnt--;
+	return 0;
+}
+
+static struct file_operations lcd_fops = {
+	.write   = lcd_write,
+	.open    = lcd_open,
+	.release = lcd_release,
+};
+
+static struct miscdevice lcd_dev = {
+	LCD_MINOR,
+	"lcd",
+	&lcd_fops
+};
+
+/* public function usable from the kernel for any purpose */
+void panel_lcd_print(char *s)
+{
+	if (lcd_enabled && lcd_initialized)
+		lcd_write(NULL, s, strlen(s), NULL);
+}
+
+/* initialize the LCD driver */
+void lcd_init(void)
+{
+	switch (lcd_type) {
+	case LCD_TYPE_OLD:	/* parallel mode, 8 bits */
+		if (lcd_proto < 0)
+			lcd_proto = LCD_PROTO_PARALLEL;
+		if (lcd_charset < 0)
+			lcd_charset = LCD_CHARSET_NORMAL;
+		if (lcd_e_pin == PIN_NOT_SET)
+			lcd_e_pin = PIN_STROBE;
+		if (lcd_rs_pin == PIN_NOT_SET)
+			lcd_rs_pin = PIN_AUTOLF;
+
+		if (lcd_width < 0)
+			lcd_width = 40;
+		if (lcd_bwidth < 0)
+			lcd_bwidth = 40;
+		if (lcd_hwidth < 0)
+			lcd_hwidth = 64;
+		if (lcd_height < 0)
+			lcd_height = 2;
+		break;
+	case LCD_TYPE_KS0074:	/* serial mode, ks0074 */
+		if (lcd_proto < 0)
+			lcd_proto = LCD_PROTO_SERIAL;
+		if (lcd_charset < 0)
+			lcd_charset = LCD_CHARSET_KS0074;
+		if (lcd_bl_pin == PIN_NOT_SET)
+			lcd_bl_pin = PIN_AUTOLF;
+		if (lcd_cl_pin == PIN_NOT_SET)
+			lcd_cl_pin = PIN_STROBE;
+		if (lcd_da_pin == PIN_NOT_SET)
+			lcd_da_pin = PIN_D0;
+
+		if (lcd_width < 0)
+			lcd_width = 16;
+		if (lcd_bwidth < 0)
+			lcd_bwidth = 40;
+		if (lcd_hwidth < 0)
+			lcd_hwidth = 16;
+		if (lcd_height < 0)
+			lcd_height = 2;
+		break;
+	case LCD_TYPE_NEXCOM:	/* parallel mode, 8 bits, generic */
+		if (lcd_proto < 0)
+			lcd_proto = LCD_PROTO_PARALLEL;
+		if (lcd_charset < 0)
+			lcd_charset = LCD_CHARSET_NORMAL;
+		if (lcd_e_pin == PIN_NOT_SET)
+			lcd_e_pin = PIN_AUTOLF;
+		if (lcd_rs_pin == PIN_NOT_SET)
+			lcd_rs_pin = PIN_SELECP;
+		if (lcd_rw_pin == PIN_NOT_SET)
+			lcd_rw_pin = PIN_INITP;
+
+		if (lcd_width < 0)
+			lcd_width = 16;
+		if (lcd_bwidth < 0)
+			lcd_bwidth = 40;
+		if (lcd_hwidth < 0)
+			lcd_hwidth = 64;
+		if (lcd_height < 0)
+			lcd_height = 2;
+		break;
+	case LCD_TYPE_CUSTOM:	/* customer-defined */
+		if (lcd_proto < 0)
+			lcd_proto = DEFAULT_LCD_PROTO;
+		if (lcd_charset < 0)
+			lcd_charset = DEFAULT_LCD_CHARSET;
+		/* default geometry will be set later */
+		break;
+	case LCD_TYPE_HANTRONIX:	/* parallel mode, 8 bits, hantronix-like */
+	default:
+		if (lcd_proto < 0)
+			lcd_proto = LCD_PROTO_PARALLEL;
+		if (lcd_charset < 0)
+			lcd_charset = LCD_CHARSET_NORMAL;
+		if (lcd_e_pin == PIN_NOT_SET)
+			lcd_e_pin = PIN_STROBE;
+		if (lcd_rs_pin == PIN_NOT_SET)
+			lcd_rs_pin = PIN_SELECP;
+
+		if (lcd_width < 0)
+			lcd_width = 16;
+		if (lcd_bwidth < 0)
+			lcd_bwidth = 40;
+		if (lcd_hwidth < 0)
+			lcd_hwidth = 64;
+		if (lcd_height < 0)
+			lcd_height = 2;
+		break;
+	}
+
+	/* this is used to catch wrong and default values */
+	if (lcd_width <= 0)
+		lcd_width = DEFAULT_LCD_WIDTH;
+	if (lcd_bwidth <= 0)
+		lcd_bwidth = DEFAULT_LCD_BWIDTH;
+	if (lcd_hwidth <= 0)
+		lcd_hwidth = DEFAULT_LCD_HWIDTH;
+	if (lcd_height <= 0)
+		lcd_height = DEFAULT_LCD_HEIGHT;
+
+	if (lcd_proto == LCD_PROTO_SERIAL) {	/* SERIAL */
+		lcd_write_cmd = lcd_write_cmd_s;
+		lcd_write_data = lcd_write_data_s;
+		lcd_clear_fast = lcd_clear_fast_s;
+
+		if (lcd_cl_pin == PIN_NOT_SET)
+			lcd_cl_pin = DEFAULT_LCD_PIN_SCL;
+		if (lcd_da_pin == PIN_NOT_SET)
+			lcd_da_pin = DEFAULT_LCD_PIN_SDA;
+
+	} else {		/* PARALLEL */
+		lcd_write_cmd = lcd_write_cmd_p8;
+		lcd_write_data = lcd_write_data_p8;
+		lcd_clear_fast = lcd_clear_fast_p8;
+
+		if (lcd_e_pin == PIN_NOT_SET)
+			lcd_e_pin = DEFAULT_LCD_PIN_E;
+		if (lcd_rs_pin == PIN_NOT_SET)
+			lcd_rs_pin = DEFAULT_LCD_PIN_RS;
+		if (lcd_rw_pin == PIN_NOT_SET)
+			lcd_rw_pin = DEFAULT_LCD_PIN_RW;
+	}
+
+	if (lcd_bl_pin == PIN_NOT_SET)
+		lcd_bl_pin = DEFAULT_LCD_PIN_BL;
+
+	if (lcd_e_pin == PIN_NOT_SET)
+		lcd_e_pin = PIN_NONE;
+	if (lcd_rs_pin == PIN_NOT_SET)
+		lcd_rs_pin = PIN_NONE;
+	if (lcd_rw_pin == PIN_NOT_SET)
+		lcd_rw_pin = PIN_NONE;
+	if (lcd_bl_pin == PIN_NOT_SET)
+		lcd_bl_pin = PIN_NONE;
+	if (lcd_cl_pin == PIN_NOT_SET)
+		lcd_cl_pin = PIN_NONE;
+	if (lcd_da_pin == PIN_NOT_SET)
+		lcd_da_pin = PIN_NONE;
+
+	if (lcd_charset < 0)
+		lcd_charset = DEFAULT_LCD_CHARSET;
+
+	if (lcd_charset == LCD_CHARSET_KS0074)
+		lcd_char_conv = lcd_char_conv_ks0074;
+	else
+		lcd_char_conv = NULL;
+
+	if (lcd_bl_pin != PIN_NONE)
+		init_scan_timer();
+
+	pin_to_bits(lcd_e_pin, lcd_bits[LCD_PORT_D][LCD_BIT_E],
+		    lcd_bits[LCD_PORT_C][LCD_BIT_E]);
+	pin_to_bits(lcd_rs_pin, lcd_bits[LCD_PORT_D][LCD_BIT_RS],
+		    lcd_bits[LCD_PORT_C][LCD_BIT_RS]);
+	pin_to_bits(lcd_rw_pin, lcd_bits[LCD_PORT_D][LCD_BIT_RW],
+		    lcd_bits[LCD_PORT_C][LCD_BIT_RW]);
+	pin_to_bits(lcd_bl_pin, lcd_bits[LCD_PORT_D][LCD_BIT_BL],
+		    lcd_bits[LCD_PORT_C][LCD_BIT_BL]);
+	pin_to_bits(lcd_cl_pin, lcd_bits[LCD_PORT_D][LCD_BIT_CL],
+		    lcd_bits[LCD_PORT_C][LCD_BIT_CL]);
+	pin_to_bits(lcd_da_pin, lcd_bits[LCD_PORT_D][LCD_BIT_DA],
+		    lcd_bits[LCD_PORT_C][LCD_BIT_DA]);
+
+	/* before this line, we must NOT send anything to the display.
+	 * Since lcd_init_display() needs to write data, we have to
+	 * enable mark the LCD initialized just before.
+	 */
+	lcd_initialized = 1;
+	lcd_init_display();
+
+	/* display a short message */
+#ifdef CONFIG_PANEL_CHANGE_MESSAGE
+#ifdef CONFIG_PANEL_BOOT_MESSAGE
+	panel_lcd_print("\x1b[Lc\x1b[Lb\x1b[L*" CONFIG_PANEL_BOOT_MESSAGE);
+#endif
+#else
+	panel_lcd_print("\x1b[Lc\x1b[Lb\x1b[L*Linux-" UTS_RELEASE "\nPanel-"
+			PANEL_VERSION);
+#endif
+	lcd_addr_x = lcd_addr_y = 0;
+	lcd_must_clear = 1;	/* clear the display on the next device opening */
+	lcd_gotoxy();
+}
+
+/*
+ * These are the file operation function for user access to /dev/keypad
+ */
+
+static ssize_t keypad_read(struct file *file,
+			   char *buf, size_t count, loff_t *ppos)
+{
+
+	unsigned i = *ppos;
+	char *tmp = buf;
+
+	if (keypad_buflen == 0) {
+		if (file->f_flags & O_NONBLOCK)
+			return -EAGAIN;
+
+		interruptible_sleep_on(&keypad_read_wait);
+		if (signal_pending(current))
+			return -EINTR;
+	}
+
+	for (; count-- > 0 && (keypad_buflen > 0); ++i, ++tmp, --keypad_buflen) {
+		put_user(keypad_buffer[keypad_start], tmp);
+		keypad_start = (keypad_start + 1) % KEYPAD_BUFFER;
+	}
+	*ppos = i;
+
+	return tmp - buf;
+}
+
+static int keypad_open(struct inode *inode, struct file *file)
+{
+
+	if (keypad_open_cnt)
+		return -EBUSY;	/* open only once at a time */
+
+	if (file->f_mode & FMODE_WRITE)	/* device is read-only */
+		return -EPERM;
+
+	keypad_buflen = 0;	/* flush the buffer on opening */
+	keypad_open_cnt++;
+	return 0;
+}
+
+static int keypad_release(struct inode *inode, struct file *file)
+{
+	keypad_open_cnt--;
+	return 0;
+}
+
+static struct file_operations keypad_fops = {
+	.read    = keypad_read,		/* read */
+	.open    = keypad_open,		/* open */
+	.release = keypad_release,	/* close */
+};
+
+static struct miscdevice keypad_dev = {
+	KEYPAD_MINOR,
+	"keypad",
+	&keypad_fops
+};
+
+static void keypad_send_key(char *string, int max_len)
+{
+	if (init_in_progress)
+		return;
+
+	/* send the key to the device only if a process is attached to it. */
+	if (keypad_open_cnt > 0) {
+		while (max_len-- && keypad_buflen < KEYPAD_BUFFER && *string) {
+			keypad_buffer[(keypad_start + keypad_buflen++) %
+				      KEYPAD_BUFFER] = *string++;
+		}
+		wake_up_interruptible(&keypad_read_wait);
+	}
+}
+
+/* this function scans all the bits involving at least one logical signal, and puts the
+ * results in the bitfield "phys_read" (one bit per established contact), and sets
+ * "phys_read_prev" to "phys_read".
+ *
+ * Note: to debounce input signals, we will only consider as switched a signal which is
+ * stable across 2 measures. Signals which are different between two reads will be kept
+ * as they previously were in their logical form (phys_prev). A signal which has just
+ * switched will have a 1 in (phys_read ^ phys_read_prev).
+ */
+static void phys_scan_contacts(void)
+{
+	int bit, bitval;
+	char oldval;
+	char bitmask;
+	char gndmask;
+
+	phys_prev = phys_curr;
+	phys_read_prev = phys_read;
+	phys_read = 0;		/* flush all signals */
+
+	oldval = r_dtr(pprt) | scan_mask_o;	/* keep track of old value, with all outputs disabled */
+	w_dtr(pprt, oldval & ~scan_mask_o);	/* activate all keyboard outputs (active low) */
+	bitmask = PNL_PINPUT(r_str(pprt)) & scan_mask_i;	/* will have a 1 for each bit set to gnd */
+	w_dtr(pprt, oldval);	/* disable all matrix signals */
+
+	/* now that all outputs are cleared, the only active input bits are
+	 * directly connected to the ground
+	 */
+	gndmask = PNL_PINPUT(r_str(pprt)) & scan_mask_i;	/* 1 for each grounded input */
+
+	phys_read |= (pmask_t) gndmask << 40;	/* grounded inputs are signals 40-44 */
+
+	if (bitmask != gndmask) {
+		/* since clearing the outputs changed some inputs, we know that some
+		 * input signals are currently tied to some outputs. So we'll scan them.
+		 */
+		for (bit = 0; bit < 8; bit++) {
+			bitval = 1 << bit;
+
+			if (!(scan_mask_o & bitval))	/* skip unused bits */
+				continue;
+
+			w_dtr(pprt, oldval & ~bitval);	/* enable this output */
+			bitmask = PNL_PINPUT(r_str(pprt)) & ~gndmask;
+			phys_read |= (pmask_t) bitmask << (5 * bit);
+		}
+		w_dtr(pprt, oldval);	/* disable all outputs */
+	}
+	/* this is easy: use old bits when they are flapping, use new ones when stable */
+	phys_curr =
+	    (phys_prev & (phys_read ^ phys_read_prev)) | (phys_read &
+							  ~(phys_read ^
+							    phys_read_prev));
+}
+
+static void panel_process_inputs(void)
+{
+	struct list_head *item;
+	struct logical_input *input;
+
+#if 0
+	printk(KERN_DEBUG
+	       "entering panel_process_inputs with pp=%016Lx & pc=%016Lx\n",
+	       phys_prev, phys_curr);
+#endif
+
+	keypressed = 0;
+	inputs_stable = 1;
+	list_for_each(item, &logical_inputs) {
+		input = list_entry(item, struct logical_input, list);
+
+		switch (input->state) {
+		case INPUT_ST_LOW:
+			if ((phys_curr & input->mask) != input->value)
+				break;
+			/* if all needed ones were already set previously, this means that
+			 * this logical signal has been activated by the releasing of
+			 * another combined signal, so we don't want to match.
+			 * eg: AB -(release B)-> A -(release A)-> 0 : don't match A.
+			 */
+			if ((phys_prev & input->mask) == input->value)
+				break;
+			input->rise_timer = 0;
+			input->state = INPUT_ST_RISING;
+			/* no break here, fall through */
+		case INPUT_ST_RISING:
+			if ((phys_curr & input->mask) != input->value) {
+				input->state = INPUT_ST_LOW;
+				break;
+			}
+			if (input->rise_timer < input->rise_time) {
+				inputs_stable = 0;
+				input->rise_timer++;
+				break;
+			}
+			input->high_timer = 0;
+			input->state = INPUT_ST_HIGH;
+			/* no break here, fall through */
+		case INPUT_ST_HIGH:
+#if 0
+			/* FIXME:
+			 * this is an invalid test. It tries to catch transitions from single-key
+			 * to multiple-key, but doesn't take into account the contacts polarity.
+			 * The only solution to the problem is to parse keys from the most complex
+			 * to the simplest combinations, and mark them as 'caught' once a combination
+			 * matches, then unmatch it for all other ones.
+			 */
+
+			/* try to catch dangerous transitions cases :
+			 * someone adds a bit, so this signal was a false
+			 * positive resulting from a transition. We should invalidate
+			 * the signal immediately and not call the 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)) {
+				input->state = INPUT_ST_LOW;	/* invalidate */
+				break;
+			}
+#endif
+
+			if ((phys_curr & input->mask) == input->value) {
+				if ((input->type == INPUT_TYPE_STD)
+				    && (input->high_timer == 0)) {
+					input->high_timer++;
+					if (input->u.std.press_fct != NULL)
+						input->u.std.press_fct(input->u.
+								       std.
+								       press_data);
+				} else if (input->type == INPUT_TYPE_KBD) {
+					keypressed = 1;	/* will turn on the light */
+
+					if (input->high_timer == 0) {
+						if (input->u.kbd.press_str[0])
+							keypad_send_key(input->
+									u.kbd.
+									press_str,
+									sizeof
+									(input->
+									 u.kbd.
+									 press_str));
+					}
+
+					if (input->u.kbd.repeat_str[0]) {
+						if (input->high_timer >=
+						    KEYPAD_REP_START) {
+							input->high_timer -=
+							    KEYPAD_REP_DELAY;
+							keypad_send_key(input->
+									u.kbd.
+									repeat_str,
+									sizeof
+									(input->
+									 u.kbd.
+									 repeat_str));
+						}
+						inputs_stable = 0;	/* we will need to come back here soon */
+					}
+
+					if (input->high_timer < 255)
+						input->high_timer++;
+				}
+				break;
+			} else {
+				/* else signal falling down. Let's fall through. */
+				input->state = INPUT_ST_FALLING;
+				input->fall_timer = 0;
+			}
+			/* no break here, fall through */
+		case INPUT_ST_FALLING:
+#if 0
+			/* FIXME !!! same comment as above */
+			if (((phys_prev & input->mask) == input->value)
+			    && ((phys_curr & input->mask) > input->value)) {
+				input->state = INPUT_ST_LOW;	/* invalidate */
+				break;
+			}
+#endif
+
+			if ((phys_curr & input->mask) == input->value) {
+				if (input->type == INPUT_TYPE_KBD) {
+					keypressed = 1;	/* will turn on the light */
+
+					if (input->u.kbd.repeat_str[0]) {
+						if (input->high_timer >= KEYPAD_REP_START)
+							input->high_timer -= KEYPAD_REP_DELAY;
+						keypad_send_key(input->u.kbd.repeat_str,
+								sizeof(input->u.kbd.repeat_str));
+						inputs_stable = 0;	/* we will need to come back here soon */
+					}
+
+					if (input->high_timer < 255)
+						input->high_timer++;
+				}
+				input->state = INPUT_ST_HIGH;
+				break;
+			} else if (input->fall_timer >= input->fall_time) {
+				/* call release event */
+				if (input->type == INPUT_TYPE_STD) {
+					if (input->u.std.release_fct != NULL)
+						input->u.std.release_fct(input->u.std.release_data);
+
+				} else if (input->type == INPUT_TYPE_KBD) {
+					if (input->u.kbd.release_str[0])
+						keypad_send_key(input->u.kbd.release_str,
+								sizeof(input->u.kbd.release_str));
+				}
+
+				input->state = INPUT_ST_LOW;
+				break;
+			} else {
+				input->fall_timer++;
+				inputs_stable = 0;
+				break;
+			}
+		}
+	}
+}
+
+static void panel_scan_timer(void)
+{
+	if (keypad_enabled && keypad_initialized) {
+		if (spin_trylock(&pprt_lock)) {
+			phys_scan_contacts();
+			spin_unlock(&pprt_lock);	/* no need for the parport anymore */
+		}
+
+		if (!inputs_stable || phys_curr != phys_prev)
+			panel_process_inputs();
+	}
+
+	if (lcd_enabled && lcd_initialized) {
+		if (keypressed) {
+			if (light_tempo == 0 && ((lcd_flags & LCD_FLAG_L) == 0))
+				lcd_backlight(1);
+			light_tempo = FLASH_LIGHT_TEMPO;
+		} else if (light_tempo > 0) {
+			light_tempo--;
+			if (light_tempo == 0 && ((lcd_flags & LCD_FLAG_L) == 0))
+				lcd_backlight(0);
+		}
+	}
+
+	mod_timer(&scan_timer, jiffies + INPUT_POLL_TIME);
+}
+
+static void init_scan_timer(void)
+{
+	if (scan_timer.function != NULL)
+		return;		/* already started */
+
+	init_timer(&scan_timer);
+	scan_timer.expires = jiffies + INPUT_POLL_TIME;
+	scan_timer.data = 0;
+	scan_timer.function = (void *)&panel_scan_timer;
+	add_timer(&scan_timer);
+}
+
+/* converts a name of the form "({BbAaPpSsEe}{01234567-})*" to a series of bits.
+ * if <omask> or <imask> are non-null, they will be or'ed with the bits corresponding
+ * to out and in bits respectively.
+ * returns 1 if ok, 0 if error (in which case, nothing is written).
+ */
+static int input_name2mask(char *name, pmask_t *mask, pmask_t *value,
+			   char *imask, char *omask)
+{
+	static char sigtab[10] = "EeSsPpAaBb";
+	char im, om;
+	pmask_t m, v;
+
+	om = im = m = v = 0ULL;
+	while (*name) {
+		int in, out, bit, neg;
+		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 */
+		in >>= 1;
+		im |= (1 << in);
+
+		name++;
+		if (isdigit(*name)) {
+			out = *name - '0';
+			om |= (1 << out);
+		} else if (*name == '-')
+			out = 8;
+		else
+			return 0;	/* unknown bit name */
+
+		bit = (out * 5) + in;
+
+		m |= 1ULL << bit;
+		if (!neg)
+			v |= 1ULL << bit;
+		name++;
+	}
+	*mask = m;
+	*value = v;
+	if (imask)
+		*imask |= im;
+	if (omask)
+		*omask |= om;
+	return 1;
+}
+
+/* tries to bind a key to the signal name <name>. The key will send the
+ * strings <press>, <repeat>, <release> for these respective events.
+ * Returns the pointer to the new key if ok, NULL if the key could not be bound.
+ */
+static struct logical_input *panel_bind_key(char *name, char *press,
+					    char *repeat, char *release)
+{
+	struct logical_input *key;
+
+	key = kmalloc(sizeof(struct logical_input), GFP_KERNEL);
+	if (!key) {
+		printk(KERN_ERR "panel: not enough memory\n");
+		return NULL;
+	}
+	memset(key, 0, sizeof(struct logical_input));
+	if (!input_name2mask(name, &key->mask, &key->value, &scan_mask_i,
+			     &scan_mask_o))
+		return NULL;
+
+	key->type = INPUT_TYPE_KBD;
+	key->state = INPUT_ST_LOW;
+	key->rise_time = 1;
+	key->fall_time = 1;
+
+#if 0
+	printk(KERN_DEBUG "bind: <%s> : m=%016Lx v=%016Lx\n", name, key->mask,
+	       key->value);
+#endif
+	strncpy(key->u.kbd.press_str, press, sizeof(key->u.kbd.press_str));
+	strncpy(key->u.kbd.repeat_str, repeat, sizeof(key->u.kbd.repeat_str));
+	strncpy(key->u.kbd.release_str, release,
+		sizeof(key->u.kbd.release_str));
+	list_add(&key->list, &logical_inputs);
+	return key;
+}
+
+#if 0
+/* tries to bind a callback function to the signal name <name>. The function
+ * <press_fct> will be called with the <press_data> arg when the signal is
+ * activated, and so on for <release_fct>/<release_data>
+ * Returns the pointer to the new signal if ok, NULL if the signal could not be bound.
+ */
+static struct logical_input *panel_bind_callback(char *name,
+						 void (*press_fct) (int),
+						 int press_data,
+						 void (*release_fct) (int),
+						 int release_data)
+{
+	struct logical_input *callback;
+
+	callback = kmalloc(sizeof(struct logical_input), GFP_KERNEL);
+	if (!callback) {
+		printk(KERN_ERR "panel: not enough memory\n");
+		return NULL;
+	}
+	memset(callback, 0, sizeof(struct logical_input));
+	if (!input_name2mask(name, &callback->mask, &callback->value,
+			     &scan_mask_i, &scan_mask_o))
+		return NULL;
+
+	callback->type = INPUT_TYPE_STD;
+	callback->state = INPUT_ST_LOW;
+	callback->rise_time = 1;
+	callback->fall_time = 1;
+	callback->u.std.press_fct = press_fct;
+	callback->u.std.press_data = press_data;
+	callback->u.std.release_fct = release_fct;
+	callback->u.std.release_data = release_data;
+	list_add(&callback->list, &logical_inputs);
+	return callback;
+}
+#endif
+
+static void keypad_init(void)
+{
+	int keynum;
+	init_waitqueue_head(&keypad_read_wait);
+	keypad_buflen = 0;	/* flushes any eventual noisy keystroke */
+
+	/* Let's create all known keys */
+
+	for (keynum = 0; keypad_profile[keynum][0][0]; keynum++) {
+		panel_bind_key(keypad_profile[keynum][0],
+			       keypad_profile[keynum][1],
+			       keypad_profile[keynum][2],
+			       keypad_profile[keynum][3]);
+	}
+
+	init_scan_timer();
+	keypad_initialized = 1;
+}
+
+/**************************************************/
+/* device initialization                          */
+/**************************************************/
+
+static int panel_notify_sys(struct notifier_block *this, unsigned long code,
+			    void *unused)
+{
+	if (lcd_enabled && lcd_initialized) {
+		switch (code) {
+		case SYS_DOWN:
+			panel_lcd_print
+			    ("\x0cReloading\nSystem...\x1b[Lc\x1b[Lb\x1b[L+");
+			break;
+		case SYS_HALT:
+			panel_lcd_print
+			    ("\x0cSystem Halted.\x1b[Lc\x1b[Lb\x1b[L+");
+			break;
+		case SYS_POWER_OFF:
+			panel_lcd_print("\x0cPower off.\x1b[Lc\x1b[Lb\x1b[L+");
+			break;
+		default:
+			break;
+		}
+	}
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block panel_notifier = {
+	panel_notify_sys,
+	NULL,
+	0
+};
+
+static void panel_attach(struct parport *port)
+{
+	if (port->number != parport)
+		return;
+
+	if (pprt) {
+		printk(KERN_ERR
+		       "panel_attach(): port->number=%d parport=%d, already registered !\n",
+		       port->number, parport);
+		return;
+	}
+
+	pprt = parport_register_device(port, "panel", NULL, NULL,	/* pf, kf */
+				       NULL,
+				       /*PARPORT_DEV_EXCL */
+				       0, (void *)&pprt);
+
+	if (parport_claim(pprt)) {
+		printk(KERN_ERR
+		       "Panel: could not claim access to parport%d. Aborting.\n",
+		       parport);
+		return;
+	}
+
+	/* must init LCD first, just in case an IRQ from the keypad is generated at keypad init */
+	if (lcd_enabled) {
+		lcd_init();
+		misc_register(&lcd_dev);
+	}
+
+	if (keypad_enabled) {
+		keypad_init();
+		misc_register(&keypad_dev);
+	}
+}
+
+static void panel_detach(struct parport *port)
+{
+	if (port->number != parport)
+		return;
+
+	if (!pprt) {
+		printk(KERN_ERR
+		       "panel_detach(): port->number=%d parport=%d, nothing to unregister.\n",
+		       port->number, parport);
+		return;
+	}
+
+	if (keypad_enabled && keypad_initialized)
+		misc_deregister(&keypad_dev);
+
+	if (lcd_enabled && lcd_initialized)
+		misc_deregister(&lcd_dev);
+
+	parport_release(pprt);
+	parport_unregister_device(pprt);
+	pprt = NULL;
+}
+
+static struct parport_driver panel_driver = {
+	.name = "panel",
+	.attach = panel_attach,
+	.detach = panel_detach,
+};
+
+/* init function */
+int panel_init(void)
+{
+	/* for backwards compatibility */
+	if (keypad_type < 0)
+		keypad_type = keypad_enabled;
+
+	if (lcd_type < 0)
+		lcd_type = lcd_enabled;
+
+	if (parport < 0)
+		parport = DEFAULT_PARPORT;
+
+	/* take care of an eventual profile */
+	switch (profile) {
+	case PANEL_PROFILE_CUSTOM:	/* custom profile */
+		if (keypad_type < 0)
+			keypad_type = DEFAULT_KEYPAD;
+		if (lcd_type < 0)
+			lcd_type = DEFAULT_LCD;
+		break;
+	case PANEL_PROFILE_OLD:	/* 8 bits, 2*16, old keypad */
+		if (keypad_type < 0)
+			keypad_type = KEYPAD_TYPE_OLD;
+		if (lcd_type < 0)
+			lcd_type = LCD_TYPE_OLD;
+		if (lcd_width < 0)
+			lcd_width = 16;
+		if (lcd_hwidth < 0)
+			lcd_hwidth = 16;
+		break;
+	case PANEL_PROFILE_NEW:	/* serial, 2*16, new keypad */
+		if (keypad_type < 0)
+			keypad_type = KEYPAD_TYPE_NEW;
+		if (lcd_type < 0)
+			lcd_type = LCD_TYPE_KS0074;
+		break;
+	case PANEL_PROFILE_HANTRONIX:	/* 8 bits, 2*16 hantronix-like, no keypad */
+		if (keypad_type < 0)
+			keypad_type = KEYPAD_TYPE_NONE;
+		if (lcd_type < 0)
+			lcd_type = LCD_TYPE_HANTRONIX;
+		break;
+	case PANEL_PROFILE_NEXCOM:	/* generic 8 bits, 2*16, nexcom keypad, eg. Nexcom. */
+		if (keypad_type < 0)
+			keypad_type = KEYPAD_TYPE_NEXCOM;
+		if (lcd_type < 0)
+			lcd_type = LCD_TYPE_NEXCOM;
+		break;
+	case PANEL_PROFILE_LARGE:	/* 8 bits, 2*40, old keypad */
+		if (keypad_type < 0)
+			keypad_type = KEYPAD_TYPE_OLD;
+		if (lcd_type < 0)
+			lcd_type = LCD_TYPE_OLD;
+		break;
+	}
+
+	lcd_enabled = (lcd_type > 0);
+	keypad_enabled = (keypad_type > 0);
+
+	switch (keypad_type) {
+	case KEYPAD_TYPE_OLD:
+		keypad_profile = old_keypad_profile;
+		break;
+	case KEYPAD_TYPE_NEW:
+		keypad_profile = new_keypad_profile;
+		break;
+	case KEYPAD_TYPE_NEXCOM:
+		keypad_profile = nexcom_keypad_profile;
+		break;
+	default:
+		keypad_profile = NULL;
+		break;
+	}
+
+	/* tells various subsystems about the fact that we are initializing */
+	init_in_progress = 1;
+
+	if (parport_register_driver(&panel_driver)) {
+		printk(KERN_ERR
+		       "Panel: could not register with parport. Aborting.\n");
+		return -EIO;
+	}
+
+	if (!lcd_enabled && !keypad_enabled) {
+		/* no device enabled, let's release the parport */
+		if (pprt) {
+			parport_release(pprt);
+			parport_unregister_device(pprt);
+		}
+		parport_unregister_driver(&panel_driver);
+		printk(KERN_ERR "Panel driver version " PANEL_VERSION
+		       " disabled.\n");
+		return -ENODEV;
+	}
+
+	register_reboot_notifier(&panel_notifier);
+
+	if (pprt)
+		printk(KERN_INFO "Panel driver version " PANEL_VERSION
+		       " registered on parport%d (io=0x%lx).\n", parport,
+		       pprt->port->base);
+	else
+		printk(KERN_INFO "Panel driver version " PANEL_VERSION
+		       " not yet registered\n");
+	/* tells various subsystems about the fact that initialization is finished */
+	init_in_progress = 0;
+	return 0;
+}
+
+static int __init panel_init_module(void)
+{
+	return panel_init();
+}
+
+static void __exit panel_cleanup_module(void)
+{
+	unregister_reboot_notifier(&panel_notifier);
+
+	if (scan_timer.function != NULL)
+		del_timer(&scan_timer);
+
+	if (keypad_enabled)
+		misc_deregister(&keypad_dev);
+
+	if (lcd_enabled) {
+		panel_lcd_print("\x0cLCD driver " PANEL_VERSION
+				"\nunloaded.\x1b[Lc\x1b[Lb\x1b[L-");
+		misc_deregister(&lcd_dev);
+	}
+
+	/* TODO: free all input signals */
+
+	parport_release(pprt);
+	parport_unregister_device(pprt);
+	parport_unregister_driver(&panel_driver);
+}
+
+module_init(panel_init_module);
+module_exit(panel_cleanup_module);
+MODULE_AUTHOR("Willy Tarreau");
+MODULE_LICENSE("GPL");
+
+/*
+ * Local variables:
+ *  c-indent-level: 4
+ *  tab-width: 8
+ * End:
+ */
diff --git a/drivers/staging/poch/README b/drivers/staging/poch/README
index f65e979743ba..842afd464993 100644
--- a/drivers/staging/poch/README
+++ b/drivers/staging/poch/README
@@ -1,5 +1,12 @@
 TODO:
-	- fix transmit overflows
+	- Rx block size is limited to < 2048, hardware bug?
+	- Group size is limited to < page size, kernel alloc/mmap API issues
+	- fix/workaround cache issues in circular buffer header
+	- test whether Tx is transmitting data from provided buffers
+	- handle device unplug case
+	- handle temperature above threshold
+	- use bus address instead of physical address for DMA
+	- support for snapshot mode
 	- audit userspace interfaces
 	- get reserved major/minor if needed
 
diff --git a/drivers/staging/poch/poch.c b/drivers/staging/poch/poch.c
index 0e113f9a1581..ec343ef53a85 100644
--- a/drivers/staging/poch/poch.c
+++ b/drivers/staging/poch/poch.c
@@ -126,9 +126,11 @@
 #define FPGA_INT_TX_ACQ_DONE		(0x1 << 1)
 #define FPGA_INT_RX_ACQ_DONE		(0x1)
 
-#define FPGA_RX_ADC_CTL_REG		0x214
-#define FPGA_RX_ADC_CTL_CONT_CAP	(0x0)
-#define FPGA_RX_ADC_CTL_SNAP_CAP	(0x1)
+#define FPGA_RX_CTL_REG			0x214
+#define FPGA_RX_CTL_FIFO_FLUSH      	(0x1 << 9)
+#define FPGA_RX_CTL_SYNTH_DATA		(0x1 << 8)
+#define FPGA_RX_CTL_CONT_CAP		(0x0 << 1)
+#define FPGA_RX_CTL_SNAP_CAP		(0x1 << 1)
 
 #define FPGA_RX_ARM_REG			0x21C
 
@@ -299,6 +301,14 @@ static ssize_t show_direction(struct device *dev,
 }
 static DEVICE_ATTR(dir, S_IRUSR|S_IRGRP, show_direction, NULL);
 
+static unsigned long npages(unsigned long bytes)
+{
+	if (bytes % PAGE_SIZE == 0)
+		return bytes / PAGE_SIZE;
+	else
+		return (bytes / PAGE_SIZE) + 1;
+}
+
 static ssize_t show_mmap_size(struct device *dev,
 			      struct device_attribute *attr, char *buf)
 {
@@ -309,10 +319,8 @@ static ssize_t show_mmap_size(struct device *dev,
 	unsigned long header_pages;
 	unsigned long total_group_pages;
 
-	/* FIXME: We do not have to add 1, if group_size a multiple of
-	   PAGE_SIZE. */
-	group_pages = (channel->group_size / PAGE_SIZE) + 1;
-	header_pages = (channel->header_size / PAGE_SIZE) + 1;
+	group_pages = npages(channel->group_size);
+	header_pages = npages(channel->header_size);
 	total_group_pages = group_pages * channel->group_count;
 
 	mmap_size = (header_pages + total_group_pages) * PAGE_SIZE;
@@ -350,8 +358,8 @@ static int poch_channel_alloc_groups(struct channel_info *channel)
 	unsigned long group_pages;
 	unsigned long header_pages;
 
-	group_pages = (channel->group_size / PAGE_SIZE) + 1;
-	header_pages = (channel->header_size / PAGE_SIZE) + 1;
+	group_pages = npages(channel->group_size);
+	header_pages = npages(channel->header_size);
 
 	for (i = 0; i < channel->group_count; i++) {
 		struct poch_group_info *group;
@@ -384,18 +392,45 @@ static int poch_channel_alloc_groups(struct channel_info *channel)
 		group->user_offset =
 			(header_pages + (i * group_pages)) * PAGE_SIZE;
 
-		printk(KERN_INFO PFX "%ld: user_offset: 0x%lx dma: 0x%x\n", i,
-		       group->user_offset, group->dma_addr);
+		printk(KERN_INFO PFX "%ld: user_offset: 0x%lx\n", i,
+		       group->user_offset);
 	}
 
 	return 0;
 }
 
-static void channel_latch_attr(struct channel_info *channel)
+static int channel_latch_attr(struct channel_info *channel)
 {
 	channel->group_count = atomic_read(&channel->sys_group_count);
 	channel->group_size = atomic_read(&channel->sys_group_size);
 	channel->block_size = atomic_read(&channel->sys_block_size);
+
+	if (channel->group_count == 0) {
+		printk(KERN_ERR PFX "invalid group count %lu",
+		       channel->group_count);
+		return -EINVAL;
+	}
+
+	if (channel->group_size == 0 ||
+	    channel->group_size < channel->block_size) {
+		printk(KERN_ERR PFX "invalid group size %lu",
+		       channel->group_size);
+		return -EINVAL;
+	}
+
+	if (channel->block_size == 0 || (channel->block_size % 8) != 0) {
+		printk(KERN_ERR PFX "invalid block size %lu",
+		       channel->block_size);
+		return -EINVAL;
+	}
+
+	if (channel->group_size % channel->block_size != 0) {
+		printk(KERN_ERR PFX
+		       "group size should be multiple of block size");
+		return -EINVAL;
+	}
+
+	return 0;
 }
 
 /*
@@ -432,7 +467,10 @@ static void channel_dma_init(struct channel_info *channel)
 	}
 
 	printk(KERN_WARNING "block_size, group_size, group_count\n");
-	iowrite32(channel->block_size, fpga + block_size_reg);
+	/*
+	 * Block size is represented in no. of 64 bit transfers.
+	 */
+	iowrite32(channel->block_size / 8, fpga + block_size_reg);
 	iowrite32(channel->group_size / channel->block_size,
 		  fpga + block_count_reg);
 	iowrite32(channel->group_count, fpga + group_count_reg);
@@ -447,27 +485,30 @@ static void channel_dma_init(struct channel_info *channel)
 	/* The DMA address page register is shared between the RX and
 	 * TX channels, so acquire lock.
 	 */
-	spin_lock(channel->iomem_lock);
 	for (i = 0; i < channel->group_count; i++) {
 		page = i / 32;
 		group_in_page = i % 32;
 
 		group_reg = group_regs_base + (group_in_page * 4);
 
+		spin_lock(channel->iomem_lock);
 		iowrite32(page, fpga + FPGA_DMA_ADR_PAGE_REG);
 		iowrite32(channel->groups[i].dma_addr, fpga + group_reg);
+		spin_unlock(channel->iomem_lock);
 	}
+
 	for (i = 0; i < channel->group_count; i++) {
 		page = i / 32;
 		group_in_page = i % 32;
 
 		group_reg = group_regs_base + (group_in_page * 4);
 
+		spin_lock(channel->iomem_lock);
 		iowrite32(page, fpga + FPGA_DMA_ADR_PAGE_REG);
 		printk(KERN_INFO PFX "%ld: read dma_addr: 0x%x\n", i,
 		       ioread32(fpga + group_reg));
+		spin_unlock(channel->iomem_lock);
 	}
-	spin_unlock(channel->iomem_lock);
 
 }
 
@@ -538,7 +579,9 @@ static int poch_channel_init(struct channel_info *channel,
 
 	printk(KERN_WARNING "channel_latch_attr\n");
 
-	channel_latch_attr(channel);
+	ret = channel_latch_attr(channel);
+	if (ret != 0)
+		goto out;
 
 	channel->transfer = 0;
 
@@ -781,6 +824,11 @@ static int poch_open(struct inode *inode, struct file *filp)
 		iowrite32(FPGA_TX_CTL_FIFO_FLUSH
 			  | FPGA_TX_CTL_OUTPUT_CARDBUS,
 			  fpga + FPGA_TX_CTL_REG);
+	} else {
+		/* Flush RX FIFO and output data to cardbus. */
+		iowrite32(FPGA_RX_CTL_CONT_CAP
+			  | FPGA_RX_CTL_FIFO_FLUSH,
+			  fpga + FPGA_RX_CTL_REG);
 	}
 
 	atomic_inc(&channel->inited);
@@ -847,8 +895,8 @@ static int poch_mmap(struct file *filp, struct vm_area_struct *vma)
 		return -EINVAL;
 	}
 
-	group_pages = (channel->group_size / PAGE_SIZE) + 1;
-	header_pages = (channel->header_size / PAGE_SIZE) + 1;
+	group_pages = npages(channel->group_size);
+	header_pages = npages(channel->header_size);
 	total_group_pages = group_pages * channel->group_count;
 
 	size = vma->vm_end - vma->vm_start;
@@ -903,14 +951,7 @@ static int poch_channel_available(struct channel_info *channel)
 	spin_lock_irq(&channel->group_offsets_lock);
 
 	for (i = 0; i < channel->group_count; i++) {
-		if (channel->dir == CHANNEL_DIR_RX
-		    && channel->header->group_offsets[i] == -1) {
-			spin_unlock_irq(&channel->group_offsets_lock);
-			return 1;
-		}
-
-		if (channel->dir == CHANNEL_DIR_TX
-		    && channel->header->group_offsets[i] != -1) {
+		if (channel->header->group_offsets[i] != -1) {
 			spin_unlock_irq(&channel->group_offsets_lock);
 			return 1;
 		}
@@ -1058,10 +1099,7 @@ static void poch_irq_dma(struct channel_info *channel)
 
 	for (i = 0; i < groups_done; i++) {
 		j = (prev_transfer + i) % channel->group_count;
-		if (channel->dir == CHANNEL_DIR_RX)
-			group_offsets[j] = -1;
-		else
-			group_offsets[j] = groups[j].user_offset;
+		group_offsets[j] = groups[j].user_offset;
 	}
 
 	spin_unlock(&channel->group_offsets_lock);
@@ -1283,7 +1321,7 @@ static int __devinit poch_pci_probe(struct pci_dev *pdev,
 	}
 
 	ret = request_irq(pdev->irq, poch_irq_handler, IRQF_SHARED,
-			  dev->bus_id, poch_dev);
+			  dev_name(dev), poch_dev);
 	if (ret) {
 		dev_err(dev, "error requesting IRQ %u\n", pdev->irq);
 		ret = -ENOMEM;
@@ -1350,12 +1388,12 @@ static void poch_pci_remove(struct pci_dev *pdev)
 	unsigned int minor = MINOR(poch_dev->cdev.dev);
 	unsigned int id = minor / poch_dev->nchannels;
 
-	/* FIXME: unmap fpga_iomem and bridge_iomem */
-
 	poch_class_dev_unregister(poch_dev, id);
 	cdev_del(&poch_dev->cdev);
 	idr_remove(&poch_ids, id);
 	free_irq(pdev->irq, poch_dev);
+	iounmap(poch_dev->fpga_iomem);
+	iounmap(poch_dev->bridge_iomem);
 	uio_unregister_device(uio);
 	pci_release_regions(pdev);
 	pci_disable_device(pdev);
diff --git a/drivers/staging/rspiusb/Kconfig b/drivers/staging/rspiusb/Kconfig
new file mode 100644
index 000000000000..d225f6794d02
--- /dev/null
+++ b/drivers/staging/rspiusb/Kconfig
@@ -0,0 +1,6 @@
+config USB_RSPI
+	tristate "Princeton Instruments USB camera support"
+	default n
+	depends on USB && BROKEN
+	help
+	  This driver is for the Princeton Instruments USB camera device.
diff --git a/drivers/staging/rspiusb/Makefile b/drivers/staging/rspiusb/Makefile
new file mode 100644
index 000000000000..cc7aed92b0e3
--- /dev/null
+++ b/drivers/staging/rspiusb/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_USB_RSPI)		+= rspiusb.o
diff --git a/drivers/staging/rspiusb/TODO b/drivers/staging/rspiusb/TODO
new file mode 100644
index 000000000000..cd6336a9254d
--- /dev/null
+++ b/drivers/staging/rspiusb/TODO
@@ -0,0 +1,22 @@
+This driver is for the Princeton Instruments USB camera.
+
+It needs lots of work to get it into the main drivers/usb/ subdirectory:
+
+Any patches to do any of the following changes are greatly appreciated:
+
+	- make checkpatch.pl clean
+	- coding style fixups (typedefs, etc.)
+	- get it to build properly
+	- audit ioctls
+	- remove ioctls if possible
+	- assign proper minor number
+	- remove dbg() macro
+	- lots of general cleanups
+	- review locking
+
+Please send patches to:
+	Greg Kroah-Hartman <gregkh@suse.de>
+and CC:
+	Judd Montgomery <judd@jpilot.org>
+	Jeff Frontz <jeff.frontz@gmail.com>
+as they have this device and can test any needed changes.
diff --git a/drivers/staging/rspiusb/rspiusb.c b/drivers/staging/rspiusb/rspiusb.c
new file mode 100644
index 000000000000..ca281d6cbd7a
--- /dev/null
+++ b/drivers/staging/rspiusb/rspiusb.c
@@ -0,0 +1,887 @@
+/*
+ * rspiusb.c
+ *
+ * Copyright (C) 2005, 2006 Princeton Instruments
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/vmalloc.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/smp_lock.h>
+#include <linux/completion.h>
+#include <linux/scatterlist.h>
+#include <linux/usb.h>
+#include <linux/mm.h>
+#include <linux/pagemap.h>
+#include <linux/ioctl.h>
+#include "rspiusb.h"
+
+#ifdef CONFIG_USB_DEBUG
+static int debug = 1;
+#else
+static int debug;
+#endif
+/* Use our own dbg macro */
+#undef dbg
+#define dbg(format, arg...) do { if (debug) printk(KERN_DEBUG __FILE__ ": " format "\n" , ## arg); } while (0)
+
+/* Version Information */
+#define DRIVER_VERSION "V1.0.1"
+#define DRIVER_AUTHOR  "Princeton Instruments"
+#define DRIVER_DESC    "PI USB2.0 Device Driver for Linux"
+
+/* Define these values to match your devices */
+#define VENDOR_ID   0x0BD7
+#define ST133_PID   0xA010
+#define PIXIS_PID   0xA026
+
+/* Get a minor range for your devices from the usb maintainer */
+#ifdef CONFIG_USB_DYNAMIC_MINORS
+#define PIUSB_MINOR_BASE    0
+#else
+#define PIUSB_MINOR_BASE    192
+#endif
+
+/* prevent races between open() and disconnect() */
+static DECLARE_MUTEX(disconnect_sem);
+
+/* Structure to hold all of our device specific stuff */
+struct device_extension {
+	struct usb_device *udev;	/* save off the usb device pointer */
+	struct usb_interface *interface;	/* the interface for this device */
+	unsigned char minor;	/* the starting minor number for this device */
+	size_t bulk_in_size_returned;
+	int bulk_in_byte_trk;
+	struct urb ***PixelUrb;
+	int frameIdx;
+	int urbIdx;
+	unsigned int *maplist_numPagesMapped;
+	int open;		/* if the port is open or not */
+	int present;		/* if the device is not disconnected */
+	int userBufMapped;	/* has the user buffer been mapped? */
+	struct scatterlist **sgl;	/* scatter-gather list for user buffer */
+	unsigned int *sgEntries;
+	struct kref kref;
+	int gotPixelData;
+	int pendingWrite;
+	char **pendedPixelUrbs;
+	int iama;		/*PIXIS or ST133 */
+	int num_frames;		/* the number of frames that will fit in the user buffer */
+	int active_frame;
+	unsigned long frameSize;
+	struct semaphore sem;
+	//FX2 specific endpoints
+	unsigned int hEP[8];
+};
+#define to_pi_dev(d) container_of( d, struct device_extension, kref )
+
+static int MapUserBuffer(struct ioctl_struct *, struct device_extension *);
+static int UnMapUserBuffer(struct device_extension *);
+static int piusb_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+		       unsigned long arg);
+static int piusb_output(struct ioctl_struct *, unsigned char *, int, struct device_extension *);
+static struct usb_driver piusb_driver;
+
+/* table of devices that work with this driver */
+static struct usb_device_id pi_device_table[] = {
+	{USB_DEVICE(VENDOR_ID, ST133_PID)},
+	{USB_DEVICE(VENDOR_ID, PIXIS_PID)},
+	{0, }			/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, pi_device_table);
+
+static int lastErr = 0;
+static int errCnt = 0;
+
+static void piusb_delete(struct kref *kref)
+{
+	struct device_extension *pdx = to_pi_dev(kref);
+
+	dev_dbg(&pdx->udev->dev, "%s\n", __func__);
+	usb_put_dev(pdx->udev);
+	kfree(pdx);
+}
+
+static int piusb_open(struct inode *inode, struct file *file)
+{
+	struct device_extension *pdx = NULL;
+	struct usb_interface *interface;
+	int subminor;
+	int retval = 0;
+
+	dbg("Piusb_Open()");
+	subminor = iminor(inode);
+	interface = usb_find_interface(&piusb_driver, subminor);
+	if (!interface) {
+		printk(KERN_ERR "%s - error, can't find device for minor %d\n",
+		       __func__, subminor);
+		retval = -ENODEV;
+		goto exit_no_device;
+	}
+
+	pdx = usb_get_intfdata(interface);
+	if (!pdx) {
+		retval = -ENODEV;
+		goto exit_no_device;
+	}
+	dbg("Alternate Setting = %d", interface->num_altsetting);
+
+	pdx->frameIdx = pdx->urbIdx = 0;
+	pdx->gotPixelData = 0;
+	pdx->pendingWrite = 0;
+	pdx->frameSize = 0;
+	pdx->num_frames = 0;
+	pdx->active_frame = 0;
+	pdx->bulk_in_byte_trk = 0;
+	pdx->userBufMapped = 0;
+	pdx->pendedPixelUrbs = NULL;
+	pdx->sgEntries = NULL;
+	pdx->sgl = NULL;
+	pdx->maplist_numPagesMapped = NULL;
+	pdx->PixelUrb = NULL;
+	pdx->bulk_in_size_returned = 0;
+	/* increment our usage count for the device */
+	kref_get(&pdx->kref);
+	/* save our object in the file's private structure */
+	file->private_data = pdx;
+      exit_no_device:
+	return retval;
+}
+
+static int piusb_release(struct inode *inode, struct file *file)
+{
+	struct device_extension *pdx;
+	int retval = 0;
+
+	dbg("Piusb_Release()");
+	pdx = (struct device_extension *)file->private_data;
+	if (pdx == NULL) {
+		dbg("%s - object is NULL", __func__);
+		return -ENODEV;
+	}
+	/* decrement the count on our device */
+	kref_put(&pdx->kref, piusb_delete);
+	return retval;
+}
+
+/**
+ *	piusb_ioctl
+ */
+static int piusb_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+		       unsigned long arg)
+{
+	struct device_extension *pdx;
+	char dummyCtlBuf[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+	unsigned long devRB = 0;
+	int i = 0;
+	int err = 0;
+	int retval = 0;
+	struct ioctl_struct ctrl;
+	unsigned char *uBuf;
+	int numbytes = 0;
+	unsigned short controlData = 0;
+
+	pdx = (struct device_extension *)file->private_data;
+	/* verify that the device wasn't unplugged */
+	if (!pdx->present) {
+		dbg("No Device Present\n");
+		return -ENODEV;
+	}
+	/* fill in your device specific stuff here */
+	if (_IOC_DIR(cmd) & _IOC_READ)
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	else if (_IOC_DIR(cmd) & _IOC_WRITE)
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	if (err) {
+		dev_err(&pdx->udev->dev, "return with error = %d\n", err);
+		return -EFAULT;
+	}
+	switch (cmd) {
+	case PIUSB_GETVNDCMD:
+		if (copy_from_user
+		    (&ctrl, (void __user *)arg, sizeof(struct ioctl_struct)))
+			info("copy_from_user failed\n");
+		dbg("%s %x\n", "Get Vendor Command = ", ctrl.cmd);
+		retval =
+		    usb_control_msg(pdx->udev, usb_rcvctrlpipe(pdx->udev, 0),
+				    ctrl.cmd, USB_DIR_IN, 0, 0, &devRB,
+				    ctrl.numbytes, HZ * 10);
+		if (ctrl.cmd == 0xF1) {
+			dbg("FW Version returned from HW = %ld.%ld",
+			    (devRB >> 8), (devRB & 0xFF));
+		}
+		return devRB;
+	case PIUSB_SETVNDCMD:
+		if (copy_from_user
+		    (&ctrl, (void __user *)arg, sizeof(struct ioctl_struct)))
+			info("copy_from_user failed\n");
+//            dbg( "%s %x", "Set Vendor Command = ",ctrl.cmd );
+		controlData = ctrl.pData[0];
+		controlData |= (ctrl.pData[1] << 8);
+//            dbg( "%s %d", "Vendor Data =",controlData );
+		retval = usb_control_msg(pdx->udev, usb_sndctrlpipe(pdx->udev, 0), ctrl.cmd, (USB_DIR_OUT | USB_TYPE_VENDOR),	/* | USB_RECIP_ENDPOINT), */
+					 controlData,
+					 0,
+					 &dummyCtlBuf, ctrl.numbytes, HZ * 10);
+		return retval;
+		break;
+	case PIUSB_ISHIGHSPEED:
+		return ((pdx->udev->speed == USB_SPEED_HIGH) ? 1 : 0);
+		break;
+	case PIUSB_WRITEPIPE:
+		if (copy_from_user(&ctrl, (void __user *)arg, _IOC_SIZE(cmd)))
+			info("copy_from_user WRITE_DUMMY failed\n");
+		if (!access_ok(VERIFY_READ, ctrl.pData, ctrl.numbytes)) {
+			dbg("can't access pData");
+			return 0;
+		}
+		piusb_output(&ctrl, ctrl.pData /*uBuf */ , ctrl.numbytes, pdx);
+		return ctrl.numbytes;
+		break;
+	case PIUSB_USERBUFFER:
+		if (copy_from_user
+		    (&ctrl, (void __user *)arg, sizeof(struct ioctl_struct)))
+			info("copy_from_user failed\n");
+		return MapUserBuffer((struct ioctl_struct *) & ctrl, pdx);
+		break;
+	case PIUSB_UNMAP_USERBUFFER:
+		UnMapUserBuffer(pdx);
+		return 0;
+		break;
+	case PIUSB_READPIPE:
+		if (copy_from_user
+		    (&ctrl, (void __user *)arg, sizeof(struct ioctl_struct)))
+			info("copy_from_user failed\n");
+		switch (ctrl.endpoint) {
+		case 0:	//ST133 Pixel Data or PIXIS IO
+			if (pdx->iama == PIXIS_PID) {
+				unsigned int numToRead = 0;
+				unsigned int totalRead = 0;
+				uBuf = kmalloc(ctrl.numbytes, GFP_KERNEL);
+				if (!uBuf) {
+					dbg("Alloc for uBuf failed");
+					return 0;
+				}
+				numbytes = ctrl.numbytes;
+				numToRead = numbytes;
+				dbg("numbytes to read = %d", numbytes);
+				dbg("endpoint # %d", ctrl.endpoint);
+				if (copy_from_user(uBuf, ctrl.pData, numbytes))
+					dbg("copying ctrl.pData to dummyBuf failed");
+				do {
+					i = usb_bulk_msg(pdx->udev, pdx->hEP[ctrl.endpoint], (uBuf + totalRead), (numToRead > 64) ? 64 : numToRead, &numbytes, HZ * 10);	//EP0 can only handle 64 bytes at a time
+					if (i) {
+						dbg("CMD = %s, Address = 0x%02X", ((uBuf[3] == 0x02) ? "WRITE" : "READ"), uBuf[1]);
+						dbg("Number of bytes Attempted to read = %d", (int)ctrl.numbytes);
+						dbg("Blocking ReadI/O Failed with status %d", i);
+						kfree(uBuf);
+						return -1;
+					} else {
+						dbg("Pixis EP0 Read %d bytes",
+						    numbytes);
+						totalRead += numbytes;
+						numToRead -= numbytes;
+					}
+				}
+				while (numToRead);
+				memcpy(ctrl.pData, uBuf, totalRead);
+				dbg("Total Bytes Read from PIXIS EP0 = %d",
+				    totalRead);
+				ctrl.numbytes = totalRead;
+				if (copy_to_user
+				    ((struct ioctl_struct *) arg, &ctrl,
+				     sizeof(struct ioctl_struct)))
+					dbg("copy_to_user failed in IORB");
+				kfree(uBuf);
+				return ctrl.numbytes;
+			} else	//ST133 Pixel Data
+			{
+				if (!pdx->gotPixelData)
+					return 0;
+				else {
+					pdx->gotPixelData = 0;
+					ctrl.numbytes =
+					    pdx->bulk_in_size_returned;
+					pdx->bulk_in_size_returned -=
+					    pdx->frameSize;
+					for (i = 0; i < pdx->maplist_numPagesMapped[pdx->active_frame]; i++)
+						SetPageDirty(pdx->sgl[pdx->active_frame][i].page_link);
+					pdx->active_frame =
+					    ((pdx->active_frame +
+					      1) % pdx->num_frames);
+					return ctrl.numbytes;
+				}
+			}
+			break;
+		case 1:	//ST133IO
+		case 4:	//PIXIS IO
+			uBuf = kmalloc(ctrl.numbytes, GFP_KERNEL);
+			if (!uBuf) {
+				dbg("Alloc for uBuf failed");
+				return 0;
+			}
+			numbytes = ctrl.numbytes;
+//                                      dbg( "numbytes to read = %d", numbytes );
+			if (copy_from_user(uBuf, ctrl.pData, numbytes))
+				dbg("copying ctrl.pData to dummyBuf failed");
+			i = usb_bulk_msg(pdx->udev, pdx->hEP[ctrl.endpoint],
+					 uBuf, numbytes, &numbytes, HZ * 10);
+			if (i) {
+				dbg("Blocking ReadI/O Failed with status %d",
+				    i);
+				kfree(uBuf);
+				return -1;
+			} else {
+				ctrl.numbytes = numbytes;
+				memcpy(ctrl.pData, uBuf, numbytes);
+				if (copy_to_user
+				    ((struct ioctl_struct *) arg, &ctrl,
+				     sizeof(struct ioctl_struct)))
+					dbg("copy_to_user failed in IORB");
+				kfree(uBuf);
+				return ctrl.numbytes;
+			}
+			break;
+
+		case 2:	//PIXIS Ping
+		case 3:	//PIXIS Pong
+			if (!pdx->gotPixelData)
+				return 0;
+			else {
+				pdx->gotPixelData = 0;
+				ctrl.numbytes = pdx->bulk_in_size_returned;
+				pdx->bulk_in_size_returned -= pdx->frameSize;
+				for (i = 0;
+				     i <
+				     pdx->maplist_numPagesMapped[pdx->
+								 active_frame];
+				     i++)
+					SetPageDirty(pdx->sgl[pdx->active_frame][i].page_link);
+				pdx->active_frame =
+				    ((pdx->active_frame + 1) % pdx->num_frames);
+				return ctrl.numbytes;
+			}
+			break;
+		}
+		break;
+	case PIUSB_WHATCAMERA:
+		return pdx->iama;
+	case PIUSB_SETFRAMESIZE:
+		dbg("PIUSB_SETFRAMESIZE");
+		if (copy_from_user
+		    (&ctrl, (void __user *)arg, sizeof(struct ioctl_struct)))
+			info("copy_from_user failed\n");
+		pdx->frameSize = ctrl.numbytes;
+		pdx->num_frames = ctrl.numFrames;
+		if (!pdx->sgl)
+			pdx->sgl =
+			    kmalloc(sizeof(struct scatterlist *) *
+				    pdx->num_frames, GFP_KERNEL);
+		if (!pdx->sgEntries)
+			pdx->sgEntries =
+			    kmalloc(sizeof(unsigned int) * pdx->num_frames,
+				    GFP_KERNEL);
+		if (!pdx->PixelUrb)
+			pdx->PixelUrb =
+			    kmalloc(sizeof(struct urb **) * pdx->num_frames,
+				    GFP_KERNEL);
+		if (!pdx->maplist_numPagesMapped)
+			pdx->maplist_numPagesMapped =
+			    vmalloc(sizeof(unsigned int) * pdx->num_frames);
+		if (!pdx->pendedPixelUrbs)
+			pdx->pendedPixelUrbs =
+			    kmalloc(sizeof(char *) * pdx->num_frames,
+				    GFP_KERNEL);
+		return 0;
+	default:
+		dbg("%s\n", "No IOCTL found");
+		break;
+
+	}
+	/* return that we did not understand this ioctl call */
+	dbg("Returning -ENOTTY");
+	return -ENOTTY;
+}
+
+static void piusb_write_bulk_callback(struct urb *urb)
+{
+	struct device_extension *pdx = urb->context;
+	int status = urb->status;
+
+	/* sync/async unlink faults aren't errors */
+	if (status && !(status == -ENOENT || status == -ECONNRESET))
+		dev_dbg(&urb->dev->dev,
+			"%s - nonzero write bulk status received: %d",
+			__func__, status);
+
+	pdx->pendingWrite = 0;
+	usb_buffer_free(urb->dev, urb->transfer_buffer_length,
+			urb->transfer_buffer, urb->transfer_dma);
+}
+
+int piusb_output(struct ioctl_struct * io, unsigned char *uBuf, int len,
+		 struct device_extension *pdx)
+{
+	struct urb *urb = NULL;
+	int err = 0;
+	unsigned char *kbuf = NULL;
+
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (urb != NULL) {
+		kbuf =
+		    usb_buffer_alloc(pdx->udev, len, GFP_KERNEL,
+				     &urb->transfer_dma);
+		if (!kbuf) {
+			info("buffer_alloc failed\n");
+			return -ENOMEM;
+		}
+		memcpy(kbuf, uBuf, len);
+		usb_fill_bulk_urb(urb, pdx->udev, pdx->hEP[io->endpoint], kbuf,
+				  len, piusb_write_bulk_callback, pdx);
+		urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+		err = usb_submit_urb(urb, GFP_KERNEL);
+		if (err) {
+			dev_err(&pdx->udev->dev,
+				"WRITE ERROR:submit urb error = %d\n", err);
+		}
+		pdx->pendingWrite = 1;
+		usb_free_urb(urb);
+	}
+	return -EINPROGRESS;
+}
+
+static int UnMapUserBuffer(struct device_extension *pdx)
+{
+	int i = 0;
+	int k = 0;
+	unsigned int epAddr;
+	for (k = 0; k < pdx->num_frames; k++) {
+		dbg("Killing Urbs for Frame %d", k);
+		for (i = 0; i < pdx->sgEntries[k]; i++) {
+			usb_kill_urb(pdx->PixelUrb[k][i]);
+			usb_free_urb(pdx->PixelUrb[k][i]);
+			pdx->pendedPixelUrbs[k][i] = 0;
+		}
+		dbg("Urb error count = %d", errCnt);
+		errCnt = 0;
+		dbg("Urbs free'd and Killed for Frame %d", k);
+	}
+
+	for (k = 0; k < pdx->num_frames; k++) {
+		if (pdx->iama == PIXIS_PID)	//if so, which EP should we map this frame to
+		{
+			if (k % 2)	//check to see if this should use EP4(PONG)
+			{
+				epAddr = pdx->hEP[3];	//PONG, odd frames
+			} else {
+				epAddr = pdx->hEP[2];	//PING, even frames and zero
+			}
+		} else		//ST133 only has 1 endpoint for Pixel data transfer
+		{
+			epAddr = pdx->hEP[0];
+		}
+		usb_buffer_unmap_sg(pdx->udev, epAddr, pdx->sgl[k],
+				    pdx->maplist_numPagesMapped[k]);
+		for (i = 0; i < pdx->maplist_numPagesMapped[k]; i++) {
+			page_cache_release(pdx->sgl[k][i].page_link);
+		}
+		kfree(pdx->sgl[k]);
+		kfree(pdx->PixelUrb[k]);
+		kfree(pdx->pendedPixelUrbs[k]);
+		pdx->sgl[k] = NULL;
+		pdx->PixelUrb[k] = NULL;
+		pdx->pendedPixelUrbs[k] = NULL;
+	}
+	kfree(pdx->sgEntries);
+	vfree(pdx->maplist_numPagesMapped);
+	pdx->sgEntries = NULL;
+	pdx->maplist_numPagesMapped = NULL;
+	kfree(pdx->sgl);
+	kfree(pdx->pendedPixelUrbs);
+	kfree(pdx->PixelUrb);
+	pdx->sgl = NULL;
+	pdx->pendedPixelUrbs = NULL;
+	pdx->PixelUrb = NULL;
+	return 0;
+}
+
+static void piusb_readPIXEL_callback(struct urb *urb)
+{
+	int i = 0;
+	struct device_extension *pdx = urb->context;
+	int status = urb->status;
+
+	if (status && !(status == -ENOENT || status == -ECONNRESET)) {
+		dbg("%s - nonzero read bulk status received: %d", __func__,
+		    status);
+		dbg("Error in read EP2 callback");
+		dbg("FrameIndex = %d", pdx->frameIdx);
+		dbg("Bytes received before problem occurred = %d",
+		    pdx->bulk_in_byte_trk);
+		dbg("Urb Idx = %d", pdx->urbIdx);
+		pdx->pendedPixelUrbs[pdx->frameIdx][pdx->urbIdx] = 0;
+	} else {
+		pdx->bulk_in_byte_trk += urb->actual_length;
+		{
+			i = usb_submit_urb(urb, GFP_ATOMIC);	//resubmit the URB
+			if (i) {
+				errCnt++;
+				if (i != lastErr) {
+					dbg("submit urb in callback failed with error code %d", i);
+					lastErr = i;
+				}
+			} else {
+				pdx->urbIdx++;	//point to next URB when we callback
+				if (pdx->bulk_in_byte_trk >= pdx->frameSize) {
+					pdx->bulk_in_size_returned =
+					    pdx->bulk_in_byte_trk;
+					pdx->bulk_in_byte_trk = 0;
+					pdx->gotPixelData = 1;
+					pdx->frameIdx =
+					    ((pdx->frameIdx +
+					      1) % pdx->num_frames);
+					pdx->urbIdx = 0;
+				}
+			}
+		}
+	}
+}
+
+/* MapUserBuffer(
+	inputs:
+	struct ioctl_struct *io - structure containing user address, frame #, and size
+	struct device_extension *pdx - the PIUSB device extension
+	returns:
+	int - status of the task
+	Notes:
+	MapUserBuffer maps a buffer passed down through an ioctl.  The user buffer is Page Aligned by the app
+	and then passed down.  The function get_free_pages(...) does the actual mapping of the buffer from user space to
+	kernel space.  From there a scatterlist is created from all the pages.  The next function called is to usb_buffer_map_sg
+	which allocated DMA addresses for each page, even coalescing them if possible.  The DMA address is placed in the scatterlist
+	structure.  The function returns the number of DMA addresses.  This may or may not be equal to the number of pages that
+	the user buffer uses.  We then build an URB for each DMA address and then submit them.
+*/
+//int MapUserBuffer( unsigned long uaddr, unsigned long numbytes, unsigned long frameInfo, struct device_extension *pdx )
+static int MapUserBuffer(struct ioctl_struct *io, struct device_extension *pdx)
+{
+	unsigned long uaddr;
+	unsigned long numbytes;
+	int frameInfo;		//which frame we're mapping
+	unsigned int epAddr = 0;
+	unsigned long count = 0;
+	int i = 0;
+	int k = 0;
+	int err = 0;
+	struct page **maplist_p;
+	int numPagesRequired;
+	frameInfo = io->numFrames;
+	uaddr = (unsigned long)io->pData;
+	numbytes = io->numbytes;
+
+	if (pdx->iama == PIXIS_PID)	//if so, which EP should we map this frame to
+	{
+		if (frameInfo % 2)	//check to see if this should use EP4(PONG)
+		{
+			epAddr = pdx->hEP[3];	//PONG, odd frames
+		} else {
+			epAddr = pdx->hEP[2];	//PING, even frames and zero
+		}
+		dbg("Pixis Frame #%d: EP=%d", frameInfo,
+		    (epAddr == pdx->hEP[2]) ? 2 : 4);
+	} else			//ST133 only has 1 endpoint for Pixel data transfer
+	{
+		epAddr = pdx->hEP[0];
+		dbg("ST133 Frame #%d: EP=2", frameInfo);
+	}
+	count = numbytes;
+	dbg("UserAddress = 0x%08lX", uaddr);
+	dbg("numbytes = %d", (int)numbytes);
+	//number of pages to map the entire user space DMA buffer
+	numPagesRequired =
+	    ((uaddr & ~PAGE_MASK) + count + ~PAGE_MASK) >> PAGE_SHIFT;
+	dbg("Number of pages needed = %d", numPagesRequired);
+	maplist_p = vmalloc(numPagesRequired * sizeof(struct page));	//, GFP_ATOMIC);
+	if (!maplist_p) {
+		dbg("Can't Allocate Memory for maplist_p");
+		return -ENOMEM;
+	}
+	//map the user buffer to kernel memory
+	down_write(&current->mm->mmap_sem);
+	pdx->maplist_numPagesMapped[frameInfo] = get_user_pages(current, current->mm, (uaddr & PAGE_MASK), numPagesRequired, WRITE, 0,	//Don't Force
+								maplist_p,
+								NULL);
+	up_write(&current->mm->mmap_sem);
+	dbg("Number of pages mapped = %d",
+	    pdx->maplist_numPagesMapped[frameInfo]);
+	for (i = 0; i < pdx->maplist_numPagesMapped[frameInfo]; i++)
+		flush_dcache_page(maplist_p[i]);
+	if (!pdx->maplist_numPagesMapped[frameInfo]) {
+		dbg("get_user_pages() failed");
+		vfree(maplist_p);
+		return -ENOMEM;
+	}
+	//need to create a scatterlist that spans each frame that can fit into the mapped buffer
+	pdx->sgl[frameInfo] =
+	    kmalloc((pdx->maplist_numPagesMapped[frameInfo] *
+		     sizeof(struct scatterlist)), GFP_ATOMIC);
+	if (!pdx->sgl[frameInfo]) {
+		vfree(maplist_p);
+		dbg("can't allocate mem for sgl");
+		return -ENOMEM;
+	}
+	pdx->sgl[frameInfo][0].page_link = maplist_p[0];
+	pdx->sgl[frameInfo][0].offset = uaddr & ~PAGE_MASK;
+	if (pdx->maplist_numPagesMapped[frameInfo] > 1) {
+		pdx->sgl[frameInfo][0].length =
+		    PAGE_SIZE - pdx->sgl[frameInfo][0].offset;
+		count -= pdx->sgl[frameInfo][0].length;
+		for (k = 1; k < pdx->maplist_numPagesMapped[frameInfo]; k++) {
+			pdx->sgl[frameInfo][k].offset = 0;
+			pdx->sgl[frameInfo][k].page_link = maplist_p[k];
+			pdx->sgl[frameInfo][k].length =
+			    (count < PAGE_SIZE) ? count : PAGE_SIZE;
+			count -= PAGE_SIZE;	//example had PAGE_SIZE here;
+		}
+	} else {
+		pdx->sgl[frameInfo][0].length = count;
+	}
+	pdx->sgEntries[frameInfo] =
+	    usb_buffer_map_sg(pdx->udev, epAddr, pdx->sgl[frameInfo],
+			      pdx->maplist_numPagesMapped[frameInfo]);
+	dbg("number of sgEntries = %d", pdx->sgEntries[frameInfo]);
+	pdx->userBufMapped = 1;
+	vfree(maplist_p);
+	//Create and Send the URB's for each s/g entry
+	pdx->PixelUrb[frameInfo] =
+	    kmalloc(pdx->sgEntries[frameInfo] * sizeof(struct urb *),
+		    GFP_KERNEL);
+	if (!pdx->PixelUrb[frameInfo]) {
+		dbg("Can't Allocate Memory for Urb");
+		return -ENOMEM;
+	}
+	for (i = 0; i < pdx->sgEntries[frameInfo]; i++) {
+		pdx->PixelUrb[frameInfo][i] = usb_alloc_urb(0, GFP_KERNEL);	//0 because we're using BULK transfers
+		usb_fill_bulk_urb(pdx->PixelUrb[frameInfo][i],
+				  pdx->udev,
+				  epAddr,
+				  (dma_addr_t *) sg_dma_address(&pdx->
+								sgl[frameInfo]
+								[i]),
+				  sg_dma_len(&pdx->sgl[frameInfo][i]),
+				  piusb_readPIXEL_callback, (void *)pdx);
+		pdx->PixelUrb[frameInfo][i]->transfer_dma =
+		    sg_dma_address(&pdx->sgl[frameInfo][i]);
+		pdx->PixelUrb[frameInfo][i]->transfer_flags =
+		    URB_NO_TRANSFER_DMA_MAP | URB_NO_INTERRUPT;
+	}
+	pdx->PixelUrb[frameInfo][--i]->transfer_flags &= ~URB_NO_INTERRUPT;	//only interrupt when last URB completes
+	pdx->pendedPixelUrbs[frameInfo] =
+	    kmalloc((pdx->sgEntries[frameInfo] * sizeof(char)), GFP_KERNEL);
+	if (!pdx->pendedPixelUrbs[frameInfo])
+		dbg("Can't allocate Memory for pendedPixelUrbs");
+	for (i = 0; i < pdx->sgEntries[frameInfo]; i++) {
+		err = usb_submit_urb(pdx->PixelUrb[frameInfo][i], GFP_ATOMIC);
+		if (err) {
+			dbg("%s %d\n", "submit urb error =", err);
+			pdx->pendedPixelUrbs[frameInfo][i] = 0;
+			return err;
+		} else
+			pdx->pendedPixelUrbs[frameInfo][i] = 1;;
+	}
+	return 0;
+}
+
+static struct file_operations piusb_fops = {
+	.owner = THIS_MODULE,
+	.ioctl = piusb_ioctl,
+	.open = piusb_open,
+	.release = piusb_release,
+};
+
+static struct usb_class_driver piusb_class = {
+	.name = "usb/rspiusb%d",
+	.fops = &piusb_fops,
+	.minor_base = PIUSB_MINOR_BASE,
+};
+
+/**
+ *	piusb_probe
+ *
+ *	Called by the usb core when a new device is connected that it thinks
+ *	this driver might be interested in.
+ */
+static int piusb_probe(struct usb_interface *interface,
+		       const struct usb_device_id *id)
+{
+	struct device_extension *pdx = NULL;
+	struct usb_host_interface *iface_desc;
+	struct usb_endpoint_descriptor *endpoint;
+	int i;
+	int retval = -ENOMEM;
+
+	dev_dbg(&interface->dev, "%s - Looking for PI USB Hardware", __func__);
+
+	pdx = kzalloc(sizeof(struct device_extension), GFP_KERNEL);
+	if (pdx == NULL) {
+		dev_err(&interface->dev, "Out of memory\n");
+		goto error;
+	}
+	kref_init(&pdx->kref);
+	pdx->udev = usb_get_dev(interface_to_usbdev(interface));
+	pdx->interface = interface;
+	iface_desc = interface->cur_altsetting;
+
+	/* See if the device offered us matches what we can accept */
+	if ((pdx->udev->descriptor.idVendor != VENDOR_ID)
+	    || ((pdx->udev->descriptor.idProduct != PIXIS_PID)
+		&& (pdx->udev->descriptor.idProduct != ST133_PID))) {
+		return -ENODEV;
+	}
+	pdx->iama = pdx->udev->descriptor.idProduct;
+
+	if (debug) {
+		if (pdx->udev->descriptor.idProduct == PIXIS_PID)
+			dbg("PIUSB:Pixis Camera Found");
+		else
+			dbg("PIUSB:ST133 USB Controller Found");
+		if (pdx->udev->speed == USB_SPEED_HIGH)
+			dbg("Highspeed(USB2.0) Device Attached");
+		else
+			dbg("Lowspeed (USB1.1) Device Attached");
+
+		dbg("NumEndpoints in Configuration: %d",
+		    iface_desc->desc.bNumEndpoints);
+	}
+	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+		endpoint = &iface_desc->endpoint[i].desc;
+		if (debug) {
+			dbg("Endpoint[%d]->bDescriptorType = %d", i,
+			    endpoint->bDescriptorType);
+			dbg("Endpoint[%d]->bEndpointAddress = 0x%02X", i,
+			    endpoint->bEndpointAddress);
+			dbg("Endpoint[%d]->bbmAttributes = %d", i,
+			    endpoint->bmAttributes);
+			dbg("Endpoint[%d]->MaxPacketSize = %d\n", i,
+			    endpoint->wMaxPacketSize);
+		}
+		if ((endpoint->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) ==
+		    USB_ENDPOINT_XFER_BULK) {
+			if (endpoint->bEndpointAddress & USB_DIR_IN)
+				pdx->hEP[i] =
+				    usb_rcvbulkpipe(pdx->udev,
+						    endpoint->bEndpointAddress);
+			else
+				pdx->hEP[i] =
+				    usb_sndbulkpipe(pdx->udev,
+						    endpoint->bEndpointAddress);
+		}
+	}
+	usb_set_intfdata(interface, pdx);
+	retval = usb_register_dev(interface, &piusb_class);
+	if (retval) {
+		err("Not able to get a minor for this device.");
+		usb_set_intfdata(interface, NULL);
+		goto error;
+	}
+	pdx->present = 1;
+
+	/* we can register the device now, as it is ready */
+	pdx->minor = interface->minor;
+	/* let the user know what node this device is now attached to */
+	dbg("PI USB2.0 device now attached to piusb-%d", pdx->minor);
+	return 0;
+
+      error:
+	if (pdx)
+		kref_put(&pdx->kref, piusb_delete);
+	return retval;
+}
+
+/**
+ *	piusb_disconnect
+ *
+ *	Called by the usb core when the device is removed from the system.
+ *
+ *	This routine guarantees that the driver will not submit any more urbs
+ *	by clearing pdx->udev.  It is also supposed to terminate any currently
+ *	active urbs.  Unfortunately, usb_bulk_msg(), used in piusb_read(), does
+ *	not provide any way to do this.  But at least we can cancel an active
+ *	write.
+ */
+static void piusb_disconnect(struct usb_interface *interface)
+{
+	struct device_extension *pdx;
+	int minor = interface->minor;
+	lock_kernel();
+	pdx = usb_get_intfdata(interface);
+	usb_set_intfdata(interface, NULL);
+	/* give back our minor */
+	usb_deregister_dev(interface, &piusb_class);
+	unlock_kernel();
+	/* prevent device read, write and ioctl */
+	pdx->present = 0;
+	kref_put(&pdx->kref, piusb_delete);
+	dbg("PI USB2.0 device #%d now disconnected\n", minor);
+}
+
+static struct usb_driver piusb_driver = {
+	.name = "sub",
+	.probe = piusb_probe,
+	.disconnect = piusb_disconnect,
+	.id_table = pi_device_table,
+};
+
+/**
+ *	piusb_init
+ */
+static int __init piusb_init(void)
+{
+	int result;
+	/* register this driver with the USB subsystem */
+	result = usb_register(&piusb_driver);
+	if (result) {
+		printk(KERN_ERR KBUILD_MODNAME
+		       ": usb_register failed. Error number %d\n", result);
+		return result;
+	}
+	printk(KERN_INFO KBUILD_MODNAME ":%s: %s\n", DRIVER_DESC,
+	       DRIVER_VERSION);
+	return 0;
+}
+
+/**
+ *	piusb_exit
+ */
+static void __exit piusb_exit(void)
+{
+	/* deregister this driver with the USB subsystem */
+	usb_deregister(&piusb_driver);
+}
+
+module_init(piusb_init);
+module_exit(piusb_exit);
+
+/* Module parameters */
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug enabled or not");
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/rspiusb/rspiusb.h b/drivers/staging/rspiusb/rspiusb.h
new file mode 100644
index 000000000000..965cd2d8c194
--- /dev/null
+++ b/drivers/staging/rspiusb/rspiusb.h
@@ -0,0 +1,25 @@
+#ifndef __RSPIUSB_H
+#define __RSPIUSB_H
+
+#define PIUSB_MAGIC		'm'
+#define PIUSB_IOCTL_BASE	192
+#define PIUSB_GETVNDCMD		_IOR(PIUSB_MAGIC, PIUSB_IOCTL_BASE + 1, struct ioctl_struct)
+#define PIUSB_SETVNDCMD		_IOW(PIUSB_MAGIC, PIUSB_IOCTL_BASE + 2, struct ioctl_struct)
+#define PIUSB_WRITEPIPE		_IOW(PIUSB_MAGIC, PIUSB_IOCTL_BASE + 3, struct ioctl_struct)
+#define PIUSB_READPIPE		_IOR(PIUSB_MAGIC, PIUSB_IOCTL_BASE + 4, struct ioctl_struct)
+#define PIUSB_SETFRAMESIZE	_IOW(PIUSB_MAGIC, PIUSB_IOCTL_BASE + 5, struct ioctl_struct)
+#define PIUSB_WHATCAMERA	_IO(PIUSB_MAGIC,  PIUSB_IOCTL_BASE + 6)
+#define PIUSB_USERBUFFER	_IOW(PIUSB_MAGIC, PIUSB_IOCTL_BASE + 7, struct ioctl_struct)
+#define PIUSB_ISHIGHSPEED	_IO(PIUSB_MAGIC,  PIUSB_IOCTL_BASE + 8)
+#define PIUSB_UNMAP_USERBUFFER	_IOW(PIUSB_MAGIC, PIUSB_IOCTL_BASE + 9, struct ioctl_struct)
+
+struct ioctl_struct {
+	unsigned char cmd;
+	unsigned long numbytes;
+	unsigned char dir;	//1=out;0=in
+	int endpoint;
+	int numFrames;
+	unsigned char *pData;
+};
+
+#endif
diff --git a/drivers/staging/rt2860/2860_main_dev.c b/drivers/staging/rt2860/2860_main_dev.c
new file mode 100644
index 000000000000..08ca81f43cc8
--- /dev/null
+++ b/drivers/staging/rt2860/2860_main_dev.c
@@ -0,0 +1,1377 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    2870_main_dev.c
+
+    Abstract:
+    Create and register network interface.
+
+    Revision History:
+    Who         When            What
+    --------    ----------      ----------------------------------------------
+*/
+
+#include "rt_config.h"
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+// record whether the card in the card list is used in the card file
+extern UINT8  MC_CardUsed[];
+#endif // MULTIPLE_CARD_SUPPORT //
+
+
+extern INT __devinit rt28xx_probe(IN void *_dev_p, IN void *_dev_id_p,
+									IN UINT argc, OUT PRTMP_ADAPTER *ppAd);
+
+static void rx_done_tasklet(unsigned long data);
+static void mgmt_dma_done_tasklet(unsigned long data);
+static void ac0_dma_done_tasklet(unsigned long data);
+static void ac1_dma_done_tasklet(unsigned long data);
+static void ac2_dma_done_tasklet(unsigned long data);
+static void ac3_dma_done_tasklet(unsigned long data);
+static void hcca_dma_done_tasklet(unsigned long data);
+static void fifo_statistic_full_tasklet(unsigned long data);
+
+
+/*---------------------------------------------------------------------*/
+/* Symbol & Macro Definitions                                          */
+/*---------------------------------------------------------------------*/
+#define RT2860_INT_RX_DLY				(1<<0)		// bit 0
+#define RT2860_INT_TX_DLY				(1<<1)		// bit 1
+#define RT2860_INT_RX_DONE				(1<<2)		// bit 2
+#define RT2860_INT_AC0_DMA_DONE			(1<<3)		// bit 3
+#define RT2860_INT_AC1_DMA_DONE			(1<<4)		// bit 4
+#define RT2860_INT_AC2_DMA_DONE			(1<<5)		// bit 5
+#define RT2860_INT_AC3_DMA_DONE			(1<<6)		// bit 6
+#define RT2860_INT_HCCA_DMA_DONE		(1<<7)		// bit 7
+#define RT2860_INT_MGMT_DONE			(1<<8)		// bit 8
+
+#define INT_RX			RT2860_INT_RX_DONE
+
+#define INT_AC0_DLY		(RT2860_INT_AC0_DMA_DONE) //| RT2860_INT_TX_DLY)
+#define INT_AC1_DLY		(RT2860_INT_AC1_DMA_DONE) //| RT2860_INT_TX_DLY)
+#define INT_AC2_DLY		(RT2860_INT_AC2_DMA_DONE) //| RT2860_INT_TX_DLY)
+#define INT_AC3_DLY		(RT2860_INT_AC3_DMA_DONE) //| RT2860_INT_TX_DLY)
+#define INT_HCCA_DLY 	(RT2860_INT_HCCA_DMA_DONE) //| RT2860_INT_TX_DLY)
+#define INT_MGMT_DLY	RT2860_INT_MGMT_DONE
+
+/*---------------------------------------------------------------------*/
+/* Prototypes of Functions Used                                        */
+/*---------------------------------------------------------------------*/
+/* function declarations */
+static INT __devinit rt2860_init_one (struct pci_dev *pci_dev, const struct pci_device_id  *ent);
+static VOID __devexit rt2860_remove_one(struct pci_dev *pci_dev);
+static INT __devinit rt2860_probe(struct pci_dev *pci_dev, const struct pci_device_id  *ent);
+void init_thread_task(PRTMP_ADAPTER pAd);
+static void __exit rt2860_cleanup_module(void);
+static int __init rt2860_init_module(void);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#ifdef CONFIG_PM
+static int rt2860_suspend(struct pci_dev *pci_dev, pm_message_t state);
+static int rt2860_resume(struct pci_dev *pci_dev);
+#endif // CONFIG_PM //
+#endif
+
+
+//
+// Ralink PCI device table, include all supported chipsets
+//
+static struct pci_device_id rt2860_pci_tbl[] __devinitdata =
+{
+	{PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2860_PCI_DEVICE_ID)},		//RT28602.4G
+	{PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2860_PCIe_DEVICE_ID)},
+	{PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2760_PCI_DEVICE_ID)},
+	{PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2790_PCIe_DEVICE_ID)},
+	{PCI_DEVICE(VEN_AWT_PCI_VENDOR_ID, VEN_AWT_PCIe_DEVICE_ID)},
+    {0,}		// terminate list
+};
+
+MODULE_DEVICE_TABLE(pci, rt2860_pci_tbl);
+#ifdef CONFIG_STA_SUPPORT
+MODULE_LICENSE("GPL");
+#ifdef MODULE_VERSION
+MODULE_VERSION(STA_DRIVER_VERSION);
+#endif
+#endif // CONFIG_STA_SUPPORT //
+
+
+//
+// Our PCI driver structure
+//
+static struct pci_driver rt2860_driver =
+{
+    name:       "rt2860",
+    id_table:   rt2860_pci_tbl,
+    probe:      rt2860_init_one,
+#if LINUX_VERSION_CODE >= 0x20412
+    remove:     __devexit_p(rt2860_remove_one),
+#else
+    remove:     __devexit(rt2860_remove_one),
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#ifdef CONFIG_PM
+	suspend:	rt2860_suspend,
+	resume:		rt2860_resume,
+#endif
+#endif
+};
+
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#ifdef CONFIG_PM
+
+VOID RT2860RejectPendingPackets(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	// clear PS packets
+	// clear TxSw packets
+}
+
+static int rt2860_suspend(
+	struct pci_dev *pci_dev,
+	pm_message_t state)
+{
+	struct net_device *net_dev = pci_get_drvdata(pci_dev);
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER)NULL;
+	INT32 retval;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_suspend()\n"));
+
+	if (net_dev == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("net_dev == NULL!\n"));
+	}
+	else
+	{
+		pAd = net_dev->ml_priv;
+
+		/* we can not use IFF_UP because ra0 down but ra1 up */
+		/* and 1 suspend/resume function for 1 module, not for each interface */
+		/* so Linux will call suspend/resume function once */
+		if (VIRTUAL_IF_NUM(pAd) > 0)
+		{
+			// avoid users do suspend after interface is down
+
+			// stop interface
+			netif_carrier_off(net_dev);
+			netif_stop_queue(net_dev);
+
+			// mark device as removed from system and therefore no longer available
+			netif_device_detach(net_dev);
+
+			// mark halt flag
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+			// take down the device
+			rt28xx_close((PNET_DEV)net_dev);
+
+			RT_MOD_DEC_USE_COUNT();
+		}
+	}
+
+	// reference to http://vovo2000.com/type-lab/linux/kernel-api/linux-kernel-api.html
+	// enable device to generate PME# when suspended
+	// pci_choose_state(): Choose the power state of a PCI device to be suspended
+	retval = pci_enable_wake(pci_dev, pci_choose_state(pci_dev, state), 1);
+	// save the PCI configuration space of a device before suspending
+	pci_save_state(pci_dev);
+	// disable PCI device after use
+	pci_disable_device(pci_dev);
+
+	retval = pci_set_power_state(pci_dev, pci_choose_state(pci_dev, state));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_suspend()\n"));
+	return retval;
+}
+
+static int rt2860_resume(
+	struct pci_dev *pci_dev)
+{
+	struct net_device *net_dev = pci_get_drvdata(pci_dev);
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER)NULL;
+	INT32 retval;
+
+
+	// set the power state of a PCI device
+	// PCI has 4 power states, DO (normal) ~ D3(less power)
+	// in include/linux/pci.h, you can find that
+	// #define PCI_D0          ((pci_power_t __force) 0)
+	// #define PCI_D1          ((pci_power_t __force) 1)
+	// #define PCI_D2          ((pci_power_t __force) 2)
+	// #define PCI_D3hot       ((pci_power_t __force) 3)
+	// #define PCI_D3cold      ((pci_power_t __force) 4)
+	// #define PCI_UNKNOWN     ((pci_power_t __force) 5)
+	// #define PCI_POWER_ERROR ((pci_power_t __force) -1)
+	retval = pci_set_power_state(pci_dev, PCI_D0);
+
+	// restore the saved state of a PCI device
+	pci_restore_state(pci_dev);
+
+	// initialize device before it's used by a driver
+	if (pci_enable_device(pci_dev))
+	{
+		printk("pci enable fail!\n");
+		return 0;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_resume()\n"));
+
+	if (net_dev == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("net_dev == NULL!\n"));
+	}
+	else
+		pAd = net_dev->ml_priv;
+
+	if (pAd != NULL)
+	{
+		/* we can not use IFF_UP because ra0 down but ra1 up */
+		/* and 1 suspend/resume function for 1 module, not for each interface */
+		/* so Linux will call suspend/resume function once */
+		if (VIRTUAL_IF_NUM(pAd) > 0)
+		{
+			// mark device as attached from system and restart if needed
+			netif_device_attach(net_dev);
+
+			if (rt28xx_open((PNET_DEV)net_dev) != 0)
+			{
+				// open fail
+				DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_resume()\n"));
+				return 0;
+			}
+
+			// increase MODULE use count
+			RT_MOD_INC_USE_COUNT();
+
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+			netif_start_queue(net_dev);
+			netif_carrier_on(net_dev);
+			netif_wake_queue(net_dev);
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_resume()\n"));
+	return 0;
+}
+#endif // CONFIG_PM //
+#endif
+
+
+static INT __init rt2860_init_module(VOID)
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	return pci_register_driver(&rt2860_driver);
+#else
+    return pci_module_init(&rt2860_driver);
+#endif
+}
+
+
+//
+// Driver module unload function
+//
+static VOID __exit rt2860_cleanup_module(VOID)
+{
+    pci_unregister_driver(&rt2860_driver);
+}
+
+module_init(rt2860_init_module);
+module_exit(rt2860_cleanup_module);
+
+
+static INT __devinit rt2860_init_one (
+    IN  struct pci_dev              *pci_dev,
+    IN  const struct pci_device_id  *ent)
+{
+    INT rc;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_init_one\n"));
+
+    // wake up and enable device
+    if (pci_enable_device (pci_dev))
+    {
+        rc = -EIO;
+    }
+    else
+    {
+        rc = rt2860_probe(pci_dev, ent);
+    }
+
+    DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_init_one\n"));
+    return rc;
+}
+
+
+static VOID __devexit rt2860_remove_one(
+    IN  struct pci_dev  *pci_dev)
+{
+    struct net_device   *net_dev = pci_get_drvdata(pci_dev);
+    RTMP_ADAPTER        *pAd = net_dev->ml_priv;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_remove_one\n"));
+
+	if (pAd != NULL)
+	{
+#ifdef MULTIPLE_CARD_SUPPORT
+		if ((pAd->MC_RowID >= 0) && (pAd->MC_RowID <= MAX_NUM_OF_MULTIPLE_CARD))
+			MC_CardUsed[pAd->MC_RowID] = 0; // not clear MAC address
+#endif // MULTIPLE_CARD_SUPPORT //
+
+
+
+
+		// Unregister network device
+		unregister_netdev(net_dev);
+
+		// Unmap CSR base address
+		iounmap((char *)(net_dev->base_addr));
+
+		RTMPFreeAdapter(pAd);
+
+		// release memory region
+		release_mem_region(pci_resource_start(pci_dev, 0), pci_resource_len(pci_dev, 0));
+	}
+	else
+	{
+		// Unregister network device
+		unregister_netdev(net_dev);
+
+		// Unmap CSR base address
+		iounmap((char *)(net_dev->base_addr));
+
+		// release memory region
+		release_mem_region(pci_resource_start(pci_dev, 0), pci_resource_len(pci_dev, 0));
+	}
+
+	// Free pre-allocated net_device memory
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	free_netdev(net_dev);
+#else
+	kfree(net_dev);
+#endif
+}
+
+//
+// PCI device probe & initialization function
+//
+static INT __devinit   rt2860_probe(
+    IN  struct pci_dev              *pci_dev,
+    IN  const struct pci_device_id  *ent)
+{
+	PRTMP_ADAPTER pAd;
+    INT rv = 0;
+
+    rv = (INT)rt28xx_probe((void *)pci_dev, (void *)ent, 0, &pAd);
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE);
+	return rv;
+}
+
+
+void init_thread_task(IN PRTMP_ADAPTER pAd)
+{
+	POS_COOKIE pObj;
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	tasklet_init(&pObj->rx_done_task, rx_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->mgmt_dma_done_task, mgmt_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac0_dma_done_task, ac0_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac1_dma_done_task, ac1_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac2_dma_done_task, ac2_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac3_dma_done_task, ac3_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->hcca_dma_done_task, hcca_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->tbtt_task, tbtt_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->fifo_statistic_full_task, fifo_statistic_full_tasklet, (unsigned long)pAd);
+}
+
+void kill_thread_task(IN PRTMP_ADAPTER pAd)
+{
+	POS_COOKIE pObj;
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	tasklet_kill(&pObj->rx_done_task);
+	tasklet_kill(&pObj->mgmt_dma_done_task);
+	tasklet_kill(&pObj->ac0_dma_done_task);
+	tasklet_kill(&pObj->ac1_dma_done_task);
+	tasklet_kill(&pObj->ac2_dma_done_task);
+	tasklet_kill(&pObj->ac3_dma_done_task);
+	tasklet_kill(&pObj->hcca_dma_done_task);
+	tasklet_kill(&pObj->tbtt_task);
+	tasklet_kill(&pObj->fifo_statistic_full_task);
+}
+
+
+static void rt2860_int_enable(PRTMP_ADAPTER pAd, unsigned int mode)
+{
+	u32 regValue;
+
+	pAd->int_disable_mask &= ~(mode);
+	regValue = pAd->int_enable_reg & ~(pAd->int_disable_mask);
+	RTMP_IO_WRITE32(pAd, INT_MASK_CSR, regValue);     // 1:enable
+
+	if (regValue != 0)
+		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_ACTIVE);
+}
+
+
+static void rt2860_int_disable(PRTMP_ADAPTER pAd, unsigned int mode)
+{
+	u32 regValue;
+
+	pAd->int_disable_mask |= mode;
+	regValue = 	pAd->int_enable_reg & ~(pAd->int_disable_mask);
+	RTMP_IO_WRITE32(pAd, INT_MASK_CSR, regValue);     // 0: disable
+
+	if (regValue == 0)
+	{
+		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_ACTIVE);
+	}
+}
+
+static void mgmt_dma_done_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+    INT_SOURCE_CSR_STRUC	IntSource;
+	POS_COOKIE pObj;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	IntSource.word = 0;
+	IntSource.field.MgmtDmaDone = 1;
+	pAd->int_pending &= ~INT_MGMT_DLY;
+
+	RTMPHandleMgmtRingDmaDoneInterrupt(pAd);
+
+	// if you use RTMP_SEM_LOCK, sometimes kernel will hang up, no any
+	// bug report output
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+	/*
+	 * double check to avoid lose of interrupts
+	 */
+	if (pAd->int_pending & INT_MGMT_DLY)
+	{
+		tasklet_hi_schedule(&pObj->mgmt_dma_done_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable TxDataInt again */
+	rt2860_int_enable(pAd, INT_MGMT_DLY);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+}
+
+static void rx_done_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+	BOOLEAN	bReschedule = 0;
+	POS_COOKIE pObj;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	pAd->int_pending &= ~(INT_RX);
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		bReschedule = STARxDoneInterruptHandle(pAd, 0);
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+	/*
+	 * double check to avoid rotting packet
+	 */
+	if (pAd->int_pending & INT_RX || bReschedule)
+	{
+		tasklet_hi_schedule(&pObj->rx_done_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable RxINT again */
+	rt2860_int_enable(pAd, INT_RX);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+
+}
+
+void fifo_statistic_full_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+	POS_COOKIE pObj;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	pAd->int_pending &= ~(FifoStaFullInt);
+	NICUpdateFifoStaCounters(pAd);
+
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+	/*
+	 * double check to avoid rotting packet
+	 */
+	if (pAd->int_pending & FifoStaFullInt)
+	{
+		tasklet_hi_schedule(&pObj->fifo_statistic_full_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable RxINT again */
+
+	rt2860_int_enable(pAd, FifoStaFullInt);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+
+}
+
+static void hcca_dma_done_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+    INT_SOURCE_CSR_STRUC	IntSource;
+	POS_COOKIE pObj;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+
+	IntSource.word = 0;
+	IntSource.field.HccaDmaDone = 1;
+	pAd->int_pending &= ~INT_HCCA_DLY;
+
+	RTMPHandleTxRingDmaDoneInterrupt(pAd, IntSource);
+
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+	/*
+	 * double check to avoid lose of interrupts
+	 */
+	if (pAd->int_pending & INT_HCCA_DLY)
+	{
+		tasklet_hi_schedule(&pObj->hcca_dma_done_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable TxDataInt again */
+	rt2860_int_enable(pAd, INT_HCCA_DLY);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+}
+
+static void ac3_dma_done_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+    INT_SOURCE_CSR_STRUC	IntSource;
+	POS_COOKIE pObj;
+	BOOLEAN bReschedule = 0;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	IntSource.word = 0;
+	IntSource.field.Ac3DmaDone = 1;
+	pAd->int_pending &= ~INT_AC3_DLY;
+
+	bReschedule = RTMPHandleTxRingDmaDoneInterrupt(pAd, IntSource);
+
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+	/*
+	 * double check to avoid lose of interrupts
+	 */
+	if ((pAd->int_pending & INT_AC3_DLY) || bReschedule)
+	{
+		tasklet_hi_schedule(&pObj->ac3_dma_done_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable TxDataInt again */
+	rt2860_int_enable(pAd, INT_AC3_DLY);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+}
+
+static void ac2_dma_done_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+    INT_SOURCE_CSR_STRUC	IntSource;
+	POS_COOKIE pObj;
+	BOOLEAN bReschedule = 0;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	IntSource.word = 0;
+	IntSource.field.Ac2DmaDone = 1;
+	pAd->int_pending &= ~INT_AC2_DLY;
+
+	bReschedule = RTMPHandleTxRingDmaDoneInterrupt(pAd, IntSource);
+
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+
+	/*
+	 * double check to avoid lose of interrupts
+	 */
+	if ((pAd->int_pending & INT_AC2_DLY) || bReschedule)
+	{
+		tasklet_hi_schedule(&pObj->ac2_dma_done_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable TxDataInt again */
+	rt2860_int_enable(pAd, INT_AC2_DLY);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+}
+
+static void ac1_dma_done_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+    INT_SOURCE_CSR_STRUC	IntSource;
+	POS_COOKIE pObj;
+	BOOLEAN bReschedule = 0;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	IntSource.word = 0;
+	IntSource.field.Ac1DmaDone = 1;
+	pAd->int_pending &= ~INT_AC1_DLY;
+
+	bReschedule = RTMPHandleTxRingDmaDoneInterrupt(pAd, IntSource);
+
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+	/*
+	 * double check to avoid lose of interrupts
+	 */
+	if ((pAd->int_pending & INT_AC1_DLY) || bReschedule)
+	{
+		tasklet_hi_schedule(&pObj->ac1_dma_done_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable TxDataInt again */
+	rt2860_int_enable(pAd, INT_AC1_DLY);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+}
+
+static void ac0_dma_done_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+    INT_SOURCE_CSR_STRUC	IntSource;
+	POS_COOKIE pObj;
+	BOOLEAN bReschedule = 0;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	IntSource.word = 0;
+	IntSource.field.Ac0DmaDone = 1;
+	pAd->int_pending &= ~INT_AC0_DLY;
+
+	bReschedule = RTMPHandleTxRingDmaDoneInterrupt(pAd, IntSource);
+
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+	/*
+	 * double check to avoid lose of interrupts
+	 */
+	if ((pAd->int_pending & INT_AC0_DLY) || bReschedule)
+	{
+		tasklet_hi_schedule(&pObj->ac0_dma_done_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable TxDataInt again */
+	rt2860_int_enable(pAd, INT_AC0_DLY);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+}
+
+
+int print_int_count;
+
+IRQ_HANDLE_TYPE
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,19))
+rt2860_interrupt(int irq, void *dev_instance)
+#else
+rt2860_interrupt(int irq, void *dev_instance, struct pt_regs *regs)
+#endif
+{
+	struct net_device *net_dev = (struct net_device *) dev_instance;
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+	INT_SOURCE_CSR_STRUC	IntSource;
+	POS_COOKIE pObj;
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+
+	/* Note 03312008: we can not return here before
+		RTMP_IO_READ32(pAd, INT_SOURCE_CSR, &IntSource.word);
+		RTMP_IO_WRITE32(pAd, INT_SOURCE_CSR, IntSource.word);
+		Or kernel will panic after ifconfig ra0 down sometimes */
+
+
+	//
+	// Inital the Interrupt source.
+	//
+	IntSource.word = 0x00000000L;
+//	McuIntSource.word = 0x00000000L;
+
+	//
+	// Get the interrupt sources & saved to local variable
+	//
+	//RTMP_IO_READ32(pAd, where, &McuIntSource.word);
+	//RTMP_IO_WRITE32(pAd, , McuIntSource.word);
+
+	//
+	// Flag fOP_STATUS_DOZE On, means ASIC put to sleep, elase means ASICK WakeUp
+	// And at the same time, clock maybe turned off that say there is no DMA service.
+	// when ASIC get to sleep.
+	// To prevent system hang on power saving.
+	// We need to check it before handle the INT_SOURCE_CSR, ASIC must be wake up.
+	//
+	// RT2661 => when ASIC is sleeping, MAC register cannot be read and written.
+	// RT2860 => when ASIC is sleeping, MAC register can be read and written.
+
+	{
+		RTMP_IO_READ32(pAd, INT_SOURCE_CSR, &IntSource.word);
+		RTMP_IO_WRITE32(pAd, INT_SOURCE_CSR, IntSource.word); // write 1 to clear
+	}
+
+	// Do nothing if Reset in progress
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+		RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))
+	{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+        return  IRQ_HANDLED;
+#else
+        return;
+#endif
+	}
+
+	//
+	// Handle interrupt, walk through all bits
+	// Should start from highest priority interrupt
+	// The priority can be adjust by altering processing if statement
+	//
+
+    pAd->bPCIclkOff = FALSE;
+
+	// If required spinlock, each interrupt service routine has to acquire
+	// and release itself.
+	//
+
+	// Do nothing if NIC doesn't exist
+	if (IntSource.word == 0xffffffff)
+	{
+		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST | fRTMP_ADAPTER_HALT_IN_PROGRESS);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+        return  IRQ_HANDLED;
+#else
+        return;
+#endif
+	}
+
+	if (IntSource.word & TxCoherent)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (">>>TxCoherent<<<\n"));
+		RTMPHandleRxCoherentInterrupt(pAd);
+	}
+
+	if (IntSource.word & RxCoherent)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (">>>RxCoherent<<<\n"));
+		RTMPHandleRxCoherentInterrupt(pAd);
+	}
+
+	if (IntSource.word & FifoStaFullInt)
+	{
+#if 1
+		if ((pAd->int_disable_mask & FifoStaFullInt) == 0)
+		{
+			/* mask FifoStaFullInt */
+			rt2860_int_disable(pAd, FifoStaFullInt);
+			tasklet_hi_schedule(&pObj->fifo_statistic_full_task);
+		}
+		pAd->int_pending |= FifoStaFullInt;
+#else
+		NICUpdateFifoStaCounters(pAd);
+#endif
+	}
+
+	if (IntSource.word & INT_MGMT_DLY)
+	{
+		if ((pAd->int_disable_mask & INT_MGMT_DLY) ==0 )
+		{
+			rt2860_int_disable(pAd, INT_MGMT_DLY);
+			tasklet_hi_schedule(&pObj->mgmt_dma_done_task);
+		}
+		pAd->int_pending |= INT_MGMT_DLY ;
+	}
+
+	if (IntSource.word & INT_RX)
+	{
+		if ((pAd->int_disable_mask & INT_RX) == 0)
+		{
+			/* mask RxINT */
+			rt2860_int_disable(pAd, INT_RX);
+			tasklet_hi_schedule(&pObj->rx_done_task);
+		}
+		pAd->int_pending |= INT_RX;
+	}
+
+	if (IntSource.word & INT_HCCA_DLY)
+	{
+
+		if ((pAd->int_disable_mask & INT_HCCA_DLY) == 0)
+		{
+			/* mask TxDataInt */
+			rt2860_int_disable(pAd, INT_HCCA_DLY);
+			tasklet_hi_schedule(&pObj->hcca_dma_done_task);
+		}
+		pAd->int_pending |= INT_HCCA_DLY;
+	}
+
+	if (IntSource.word & INT_AC3_DLY)
+	{
+
+		if ((pAd->int_disable_mask & INT_AC3_DLY) == 0)
+		{
+			/* mask TxDataInt */
+			rt2860_int_disable(pAd, INT_AC3_DLY);
+			tasklet_hi_schedule(&pObj->ac3_dma_done_task);
+		}
+		pAd->int_pending |= INT_AC3_DLY;
+	}
+
+	if (IntSource.word & INT_AC2_DLY)
+	{
+
+		if ((pAd->int_disable_mask & INT_AC2_DLY) == 0)
+		{
+			/* mask TxDataInt */
+			rt2860_int_disable(pAd, INT_AC2_DLY);
+			tasklet_hi_schedule(&pObj->ac2_dma_done_task);
+		}
+		pAd->int_pending |= INT_AC2_DLY;
+	}
+
+	if (IntSource.word & INT_AC1_DLY)
+	{
+
+		pAd->int_pending |= INT_AC1_DLY;
+
+		if ((pAd->int_disable_mask & INT_AC1_DLY) == 0)
+		{
+			/* mask TxDataInt */
+			rt2860_int_disable(pAd, INT_AC1_DLY);
+			tasklet_hi_schedule(&pObj->ac1_dma_done_task);
+		}
+
+	}
+
+	if (IntSource.word & INT_AC0_DLY)
+	{
+		pAd->int_pending |= INT_AC0_DLY;
+
+		if ((pAd->int_disable_mask & INT_AC0_DLY) == 0)
+		{
+			/* mask TxDataInt */
+			rt2860_int_disable(pAd, INT_AC0_DLY);
+			tasklet_hi_schedule(&pObj->ac0_dma_done_task);
+		}
+
+	}
+
+    if (IntSource.word & PreTBTTInt)
+	{
+		RTMPHandlePreTBTTInterrupt(pAd);
+	}
+
+	if (IntSource.word & TBTTInt)
+	{
+		RTMPHandleTBTTInterrupt(pAd);
+	}
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (IntSource.word & AutoWakeupInt)
+			RTMPHandleTwakeupInterrupt(pAd);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+    return  IRQ_HANDLED;
+#endif
+
+}
+
+/*
+========================================================================
+Routine Description:
+    Check the chipset vendor/product ID.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+
+Return Value:
+    TRUE				Check ok
+	FALSE				Check fail
+
+Note:
+========================================================================
+*/
+BOOLEAN RT28XXChipsetCheck(
+	IN void *_dev_p)
+{
+	/* always TRUE */
+	return TRUE;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Init net device structure.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+    *net_dev			Point to the net device
+	*pAd				the raxx interface data pointer
+
+Return Value:
+    TRUE				Init ok
+	FALSE				Init fail
+
+Note:
+========================================================================
+*/
+BOOLEAN RT28XXNetDevInit(
+	IN void 				*_dev_p,
+	IN struct  net_device	*net_dev,
+	IN RTMP_ADAPTER 		*pAd)
+{
+	struct pci_dev *pci_dev = (struct pci_dev *)_dev_p;
+    const CHAR	*print_name;
+    ULONG	csr_addr;
+
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+    print_name = pci_dev ? pci_name(pci_dev) : "rt2860";
+#else
+    print_name = pci_dev ? pci_dev->slot_name : "rt2860";
+#endif // LINUX_VERSION_CODE //
+
+	net_dev->base_addr = 0;
+	net_dev->irq = 0;
+
+    if (pci_request_regions(pci_dev, print_name))
+        goto err_out_free_netdev;
+
+    // interrupt IRQ number
+    net_dev->irq = pci_dev->irq;
+
+    // map physical address to virtual address for accessing register
+    csr_addr = (unsigned long) ioremap(pci_resource_start(pci_dev, 0),
+										pci_resource_len(pci_dev, 0));
+
+    if (!csr_addr)
+    {
+        DBGPRINT(RT_DEBUG_ERROR,
+				("ioremap failed for device %s, region 0x%lX @ 0x%lX\n",
+				print_name, (ULONG)pci_resource_len(pci_dev, 0),
+				(ULONG)pci_resource_start(pci_dev, 0)));
+        goto err_out_free_res;
+    }
+
+    // Save CSR virtual address and irq to device structure
+    net_dev->base_addr = csr_addr;
+    pAd->CSRBaseAddress = (PUCHAR)net_dev->base_addr;
+
+    // Set DMA master
+    pci_set_master(pci_dev);
+
+    net_dev->priv_flags = INT_MAIN;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("%s: at 0x%lx, VA 0x%lx, IRQ %d. \n",
+        	net_dev->name, (ULONG)pci_resource_start(pci_dev, 0),
+			(ULONG)csr_addr, pci_dev->irq));
+	return TRUE;
+
+
+	/* --------------------------- ERROR HANDLE --------------------------- */
+err_out_free_res:
+    pci_release_regions(pci_dev);
+err_out_free_netdev:
+	/* free netdev in caller, not here */
+	return FALSE;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Init net device structure.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+	*pAd				the raxx interface data pointer
+
+Return Value:
+    TRUE				Config ok
+	FALSE				Config fail
+
+Note:
+========================================================================
+*/
+BOOLEAN RT28XXProbePostConfig(
+	IN void 				*_dev_p,
+	IN RTMP_ADAPTER 		*pAd,
+	IN INT32				argc)
+{
+	/* no use */
+	return TRUE;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Disable DMA.
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+	None
+
+Note:
+========================================================================
+*/
+VOID RT28XXDMADisable(
+	IN RTMP_ADAPTER 		*pAd)
+{
+	WPDMA_GLO_CFG_STRUC     GloCfg;
+
+
+	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+	GloCfg.word &= 0xff0;
+	GloCfg.field.EnTXWriteBackDDONE =1;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Enable DMA.
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+	None
+
+Note:
+========================================================================
+*/
+VOID RT28XXDMAEnable(
+	IN RTMP_ADAPTER 		*pAd)
+{
+	WPDMA_GLO_CFG_STRUC	GloCfg;
+	int i = 0;
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x4);
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0)  && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("==>  DMABusy\n"));
+		RTMPusecDelay(1000);
+		i++;
+	}while ( i <200);
+
+	RTMPusecDelay(50);
+
+	GloCfg.field.EnTXWriteBackDDONE = 1;
+	GloCfg.field.WPDMABurstSIZE = 2;
+	GloCfg.field.EnableRxDMA = 1;
+	GloCfg.field.EnableTxDMA = 1;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== WRITE DMA offset 0x208 = 0x%x\n", GloCfg.word));
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+}
+
+/*
+========================================================================
+Routine Description:
+    Write Beacon buffer to Asic.
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+	None
+
+Note:
+========================================================================
+*/
+VOID RT28xx_UpdateBeaconToAsic(
+	IN RTMP_ADAPTER		*pAd,
+	IN INT				apidx,
+	IN ULONG			FrameLen,
+	IN ULONG			UpdatePos)
+{
+	ULONG				CapInfoPos = 0;
+	UCHAR  			*ptr, *ptr_update, *ptr_capinfo;
+	UINT  			i;
+	BOOLEAN			bBcnReq = FALSE;
+	UCHAR			bcn_idx = 0;
+
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s() : No valid Interface be found.\n", __func__));
+		return;
+	}
+
+	if (bBcnReq == FALSE)
+	{
+		/* when the ra interface is down, do not send its beacon frame */
+		/* clear all zero */
+		for(i=0; i<TXWI_SIZE; i+=4)
+			RTMP_IO_WRITE32(pAd, pAd->BeaconOffset[bcn_idx] + i, 0x00);
+	}
+	else
+	{
+		ptr = (PUCHAR)&pAd->BeaconTxWI;
+#ifdef RT_BIG_ENDIAN
+		RTMPWIEndianChange(ptr, TYPE_TXWI);
+#endif
+		for (i=0; i<TXWI_SIZE; i+=4)  // 16-byte TXWI field
+		{
+			UINT32 longptr =  *ptr + (*(ptr+1)<<8) + (*(ptr+2)<<16) + (*(ptr+3)<<24);
+			RTMP_IO_WRITE32(pAd, pAd->BeaconOffset[bcn_idx] + i, longptr);
+			ptr += 4;
+		}
+
+		// Update CapabilityInfo in Beacon
+		for (i = CapInfoPos; i < (CapInfoPos+2); i++)
+		{
+			RTMP_IO_WRITE8(pAd, pAd->BeaconOffset[bcn_idx] + TXWI_SIZE + i, *ptr_capinfo);
+			ptr_capinfo ++;
+		}
+
+		if (FrameLen > UpdatePos)
+		{
+			for (i= UpdatePos; i< (FrameLen); i++)
+			{
+				RTMP_IO_WRITE8(pAd, pAd->BeaconOffset[bcn_idx] + TXWI_SIZE + i, *ptr_update);
+				ptr_update ++;
+			}
+		}
+
+	}
+
+}
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPInitPCIeLinkCtrlValue(
+	IN	PRTMP_ADAPTER	pAd)
+{
+}
+
+VOID RTMPFindHostPCIDev(
+    IN	PRTMP_ADAPTER	pAd)
+{
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+		Level = RESTORE_HALT : Restore PCI host and Ralink PCIe Link Control field to its default value.
+		Level = Other Value : Restore from dot11 power save or radio off status. And force PCI host Link Control fields to 0x1
+
+	========================================================================
+*/
+VOID RTMPPCIeLinkCtrlValueRestore(
+	IN	PRTMP_ADAPTER	pAd,
+	IN   UCHAR		Level)
+{
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+		Max : limit Host PCI and Ralink PCIe device's LINK CONTROL field's value.
+		Because now frequently set our device to mode 1 or mode 3 will cause problem.
+
+	========================================================================
+*/
+VOID RTMPPCIeLinkCtrlSetting(
+	IN	PRTMP_ADAPTER	pAd,
+	IN 	USHORT		Max)
+{
+}
+#endif // CONFIG_STA_SUPPORT //
+
+VOID rt2860_stop(struct net_device *net_dev)
+{
+    PRTMP_ADAPTER pAd = (PRTMP_ADAPTER)NULL;
+    if (net_dev == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("net_dev == NULL!\n"));
+	}
+	else
+		pAd = net_dev->ml_priv;
+
+	if (pAd != NULL)
+	{
+	    // stop interface
+		netif_carrier_off(net_dev);
+		netif_stop_queue(net_dev);
+
+		// mark device as removed from system and therefore no longer available
+		netif_device_detach(net_dev);
+
+		// mark halt flag
+		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+		// take down the device
+		rt28xx_close((PNET_DEV)net_dev);
+		RT_MOD_DEC_USE_COUNT();
+	}
+    return;
+}
+
+/*
+ * invaild or writeback cache
+ * and convert virtual address to physical address
+ */
+dma_addr_t linux_pci_map_single(void *handle, void *ptr, size_t size, int sd_idx, int direction)
+{
+	PRTMP_ADAPTER pAd;
+	POS_COOKIE pObj;
+
+	/*
+		------ Porting Information ------
+		> For Tx Alloc:
+			mgmt packets => sd_idx = 0
+			SwIdx: pAd->MgmtRing.TxCpuIdx
+			pTxD : pAd->MgmtRing.Cell[SwIdx].AllocVa;
+
+			data packets => sd_idx = 1
+	 		TxIdx : pAd->TxRing[pTxBlk->QueIdx].TxCpuIdx
+	 		QueIdx: pTxBlk->QueIdx
+	 		pTxD  : pAd->TxRing[pTxBlk->QueIdx].Cell[TxIdx].AllocVa;
+
+	 	> For Rx Alloc:
+	 		sd_idx = -1
+	*/
+
+	pAd = (PRTMP_ADAPTER)handle;
+	pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	if (sd_idx == 1)
+	{
+		PTX_BLK		pTxBlk;
+		pTxBlk = (PTX_BLK)ptr;
+		return pci_map_single(pObj->pci_dev, pTxBlk->pSrcBufData, pTxBlk->SrcBufLen, direction);
+	}
+	else
+	{
+		return pci_map_single(pObj->pci_dev, ptr, size, direction);
+	}
+
+}
+
+void linux_pci_unmap_single(void *handle, dma_addr_t dma_addr, size_t size, int direction)
+{
+	PRTMP_ADAPTER pAd;
+	POS_COOKIE pObj;
+
+	pAd=(PRTMP_ADAPTER)handle;
+	pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	pci_unmap_single(pObj->pci_dev, dma_addr, size, direction);
+
+}
+
diff --git a/drivers/staging/rt2860/Kconfig b/drivers/staging/rt2860/Kconfig
new file mode 100644
index 000000000000..7f44e5e72463
--- /dev/null
+++ b/drivers/staging/rt2860/Kconfig
@@ -0,0 +1,5 @@
+config RT2860
+	tristate "Ralink 2860 wireless support"
+	depends on PCI && X86 && WLAN_80211
+	---help---
+	  This is an experimental driver for the Ralink 2860 wireless chip.
diff --git a/drivers/staging/rt2860/Makefile b/drivers/staging/rt2860/Makefile
new file mode 100644
index 000000000000..6162212b588c
--- /dev/null
+++ b/drivers/staging/rt2860/Makefile
@@ -0,0 +1,43 @@
+obj-$(CONFIG_RT2860)	+= rt2860sta.o
+
+# TODO: all of these should be removed
+EXTRA_CFLAGS += -DLINUX -DAGGREGATION_SUPPORT -DPIGGYBACK_SUPPORT -DWMM_SUPPORT
+EXTRA_CFLAGS += -DRT2860
+EXTRA_CFLAGS += -DCONFIG_STA_SUPPORT
+EXTRA_CFLAGS += -DDBG
+EXTRA_CFLAGS += -DDOT11_N_SUPPORT
+EXTRA_CFLAGS += -DWPA_SUPPLICANT_SUPPORT
+EXTRA_CFLAGS += -DNATIVE_WPA_SUPPLICANT_SUPPORT
+
+rt2860sta-objs :=	\
+	common/md5.o		\
+	common/mlme.o		\
+	common/rtmp_wep.o	\
+	common/action.o		\
+	common/cmm_data.o	\
+	common/rtmp_init.o	\
+	common/rtmp_tkip.o	\
+	common/cmm_sync.o	\
+	common/eeprom.o		\
+	common/cmm_sanity.o	\
+	common/cmm_info.o	\
+	common/cmm_wpa.o	\
+	common/dfs.o		\
+	common/spectrum.o	\
+	sta/assoc.o		\
+	sta/aironet.o		\
+	sta/auth.o		\
+	sta/auth_rsp.o		\
+	sta/sync.o		\
+	sta/sanity.o		\
+	sta/rtmp_data.o		\
+	sta/connect.o		\
+	sta/wpa.o		\
+	rt_linux.o		\
+	rt_profile.o		\
+	rt_main_dev.o		\
+	sta_ioctl.o		\
+	common/ba_action.o	\
+	common/2860_rtmp_init.o	\
+	2860_main_dev.o		\
+	common/cmm_data_2860.o
diff --git a/drivers/staging/rt2860/TODO b/drivers/staging/rt2860/TODO
new file mode 100644
index 000000000000..2f70b0faca3d
--- /dev/null
+++ b/drivers/staging/rt2860/TODO
@@ -0,0 +1,17 @@
+I'm hesitant to add a TODO file here, as the wireless developers would
+really have people help them out on the "clean" rt2860 driver that can
+be found at the rt2860.sf.net site.
+
+But, if you wish to clean up this driver instead, here's a short list of
+things that need to be done to get it into a more mergable shape:
+
+TODO:
+	- checkpatch.pl clean
+	- sparse clean
+	- port to in-kernel 80211 stack
+	- remove reading from /etc/ config files
+	- review by the wireless developer community
+
+Please send any patches or complaints about this driver to Greg
+Kroah-Hartman <greg@kroah.com> and don't bother the upstream wireless
+kernel developers about it, they want nothing to do with it.
diff --git a/drivers/staging/rt2860/aironet.h b/drivers/staging/rt2860/aironet.h
new file mode 100644
index 000000000000..1e07b19b8cdc
--- /dev/null
+++ b/drivers/staging/rt2860/aironet.h
@@ -0,0 +1,210 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	aironet.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	Paul Lin	04-06-15		Initial
+*/
+
+#ifndef	__AIRONET_H__
+#define	__AIRONET_H__
+
+// Measurement Type definition
+#define	MSRN_TYPE_UNUSED				0
+#define	MSRN_TYPE_CHANNEL_LOAD_REQ		1
+#define	MSRN_TYPE_NOISE_HIST_REQ		2
+#define	MSRN_TYPE_BEACON_REQ			3
+#define	MSRN_TYPE_FRAME_REQ				4
+
+// Scan Mode in Beacon Request
+#define	MSRN_SCAN_MODE_PASSIVE			0
+#define	MSRN_SCAN_MODE_ACTIVE			1
+#define	MSRN_SCAN_MODE_BEACON_TABLE		2
+
+// PHY type definition for Aironet beacon report, CCX 2 table 36-9
+#define	PHY_FH							1
+#define	PHY_DSS							2
+#define	PHY_UNUSED						3
+#define	PHY_OFDM						4
+#define	PHY_HR_DSS						5
+#define	PHY_ERP							6
+
+// RPI table in dBm
+#define	RPI_0			0			//	Power <= -87
+#define	RPI_1			1			//	-87 < Power <= -82
+#define	RPI_2			2			//	-82 < Power <= -77
+#define	RPI_3			3			//	-77 < Power <= -72
+#define	RPI_4			4			//	-72 < Power <= -67
+#define	RPI_5			5			//	-67 < Power <= -62
+#define	RPI_6			6			//	-62 < Power <= -57
+#define	RPI_7			7			//	-57 < Power
+
+// Cisco Aironet IAPP definetions
+#define	AIRONET_IAPP_TYPE					0x32
+#define	AIRONET_IAPP_SUBTYPE_REQUEST		0x01
+#define	AIRONET_IAPP_SUBTYPE_REPORT			0x81
+
+// Measurement Request detail format
+typedef	struct	_MEASUREMENT_REQUEST	{
+	UCHAR	Channel;
+	UCHAR	ScanMode;			// Use only in beacon request, other requests did not use this field
+	USHORT	Duration;
+}	MEASUREMENT_REQUEST, *PMEASUREMENT_REQUEST;
+
+// Beacon Measurement Report
+// All these field might change to UCHAR, because we didn't do anything to these report.
+// We copy all these beacons and report to CCX 2 AP.
+typedef	struct	_BEACON_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	PhyType;			// Definiation is listed above table 36-9
+	UCHAR	RxPower;
+	UCHAR	BSSID[6];
+	UCHAR	ParentTSF[4];
+	UCHAR	TargetTSF[8];
+	USHORT	BeaconInterval;
+	USHORT	CapabilityInfo;
+}	BEACON_REPORT, *PBEACON_REPORT;
+
+// Frame Measurement Report (Optional)
+typedef	struct	_FRAME_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	TA;
+	UCHAR	BSSID[6];
+	UCHAR	RSSI;
+	UCHAR	Count;
+}	FRAME_REPORT, *PFRAME_REPORT;
+
+#pragma pack(1)
+// Channel Load Report
+typedef	struct	_CHANNEL_LOAD_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	CCABusy;
+}	CHANNEL_LOAD_REPORT, *PCHANNEL_LOAD_REPORT;
+#pragma pack()
+
+// Nosie Histogram Report
+typedef	struct	_NOISE_HIST_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	Density[8];
+}	NOISE_HIST_REPORT, *PNOISE_HIST_REPORT;
+
+// Radio Management Capability element
+typedef	struct	_RADIO_MANAGEMENT_CAPABILITY	{
+	UCHAR	Eid;				// TODO: Why the Eid is 1 byte, not normal 2 bytes???
+	UCHAR	Length;
+	UCHAR	AironetOui[3];		// AIronet OUI (00 40 96)
+	UCHAR	Type;				// Type / Version
+	USHORT	Status;				// swap16 required
+}	RADIO_MANAGEMENT_CAPABILITY, *PRADIO_MANAGEMENT_CAPABILITY;
+
+// Measurement Mode Bit definition
+typedef	struct	_MEASUREMENT_MODE	{
+	UCHAR	Rsvd:4;
+	UCHAR	Report:1;
+	UCHAR	NotUsed:1;
+	UCHAR	Enable:1;
+	UCHAR	Parallel:1;
+}	MEASUREMENT_MODE, *PMEASUREMENT_MODE;
+
+// Measurement Request element, This is little endian mode
+typedef	struct	_MEASUREMENT_REQUEST_ELEMENT	{
+	USHORT				Eid;
+	USHORT				Length;				// swap16 required
+	USHORT				Token;				// non-zero unique token
+	UCHAR				Mode;				// Measurement Mode
+	UCHAR				Type;				// Measurement type
+}	MEASUREMENT_REQUEST_ELEMENT, *PMEASUREMENT_REQUEST_ELEMENT;
+
+// Measurement Report element, This is little endian mode
+typedef	struct	_MEASUREMENT_REPORT_ELEMENT	{
+	USHORT				Eid;
+	USHORT				Length;				// swap16 required
+	USHORT				Token;				// non-zero unique token
+	UCHAR				Mode;				// Measurement Mode
+	UCHAR				Type;				// Measurement type
+}	MEASUREMENT_REPORT_ELEMENT, *PMEASUREMENT_REPORT_ELEMENT;
+
+// Cisco Aironet IAPP Frame Header, Network byte order used
+typedef	struct	_AIRONET_IAPP_HEADER {
+	UCHAR	CiscoSnapHeader[8];	// 8 bytes Cisco snap header
+	USHORT	Length;				// IAPP ID & length, remember to swap16 in LE system
+	UCHAR	Type;				// IAPP type
+	UCHAR	SubType;			// IAPP subtype
+	UCHAR	DA[6];				// Destination MAC address
+	UCHAR	SA[6];				// Source MAC address
+	USHORT	Token;				// Dialog token, no need to swap16 since it is for yoken usage only
+}	AIRONET_IAPP_HEADER, *PAIRONET_IAPP_HEADER;
+
+// Radio Measurement Request frame
+typedef	struct	_AIRONET_RM_REQUEST_FRAME	{
+    AIRONET_IAPP_HEADER	IAPP;			// Common header
+	UCHAR				Delay;			// Activation Delay
+	UCHAR				Offset;			// Measurement offset
+}	AIRONET_RM_REQUEST_FRAME, *PAIRONET_RM_REQUEST_FRAME;
+
+// Radio Measurement Report frame
+typedef	struct	_AIRONET_RM_REPORT_FRAME	{
+    AIRONET_IAPP_HEADER	IAPP;			// Common header
+}	AIRONET_RM_REPORT_FRAME, *PAIRONET_RM_REPORT_FRAME;
+
+// Saved element request actions which will saved in StaCfg.
+typedef	struct	_RM_REQUEST_ACTION	{
+	MEASUREMENT_REQUEST_ELEMENT	ReqElem;		// Saved request element
+	MEASUREMENT_REQUEST			Measurement;	// Saved measurement within the request element
+}	RM_REQUEST_ACTION, *PRM_REQUEST_ACTION;
+
+// CCX administration control
+typedef	union	_CCX_CONTROL	{
+	struct	{
+		UINT32		Enable:1;			// Enable CCX2
+		UINT32		LeapEnable:1;		// Enable LEAP at CCX2
+		UINT32		RMEnable:1;			// Radio Measurement Enable
+		UINT32		DCRMEnable:1;		// Non serving channel Radio Measurement enable
+		UINT32		QOSEnable:1;		// Enable QOS for CCX 2.0 support
+		UINT32		FastRoamEnable:1;	// Enable fast roaming
+		UINT32		Rsvd:2;				// Not used
+		UINT32		dBmToRoam:8;		// the condition to roam when receiving Rssi less than this value. It's negative value.
+		UINT32		TuLimit:16;			// Limit for different channel scan
+	}	field;
+	UINT32			word;
+}	CCX_CONTROL, *PCCX_CONTROL;
+
+#endif	// __AIRONET_H__
diff --git a/drivers/staging/rt2860/ap.h b/drivers/staging/rt2860/ap.h
new file mode 100644
index 000000000000..df6db2813121
--- /dev/null
+++ b/drivers/staging/rt2860/ap.h
@@ -0,0 +1,557 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    ap.h
+
+    Abstract:
+    Miniport generic portion header file
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Paul Lin    08-01-2002    created
+    James Tan   09-06-2002    modified (Revise NTCRegTable)
+    John Chang  12-22-2004    modified for RT2561/2661. merge with STA driver
+*/
+#ifndef __AP_H__
+#define __AP_H__
+
+
+
+// ========================= AP RTMP.h ================================
+
+
+
+// =============================================================
+//      Function Prototypes
+// =============================================================
+
+// ap_data.c
+
+BOOLEAN APBridgeToWirelessSta(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pHeader,
+    IN  UINT            HdrLen,
+    IN  PUCHAR          pData,
+    IN  UINT            DataLen,
+    IN  ULONG           fromwdsidx);
+
+BOOLEAN APHandleRxDoneInterrupt(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID	APSendPackets(
+	IN	NDIS_HANDLE		MiniportAdapterContext,
+	IN	PPNDIS_PACKET	ppPacketArray,
+	IN	UINT			NumberOfPackets);
+
+NDIS_STATUS APSendPacket(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PNDIS_PACKET    pPacket);
+
+
+NDIS_STATUS APHardTransmit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			QueIdx);
+
+VOID APRxEAPOLFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+NDIS_STATUS APCheckRxError(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PRT28XX_RXD_STRUC		pRxD,
+	IN	UCHAR			Wcid);
+
+BOOLEAN APCheckClass2Class3Error(
+    IN  PRTMP_ADAPTER   pAd,
+	IN ULONG Wcid,
+	IN  PHEADER_802_11  pHeader);
+
+VOID APHandleRxPsPoll(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pAddr,
+	IN	USHORT			Aid,
+    IN	BOOLEAN			isActive);
+
+VOID    RTMPDescriptorEndianChange(
+    IN  PUCHAR          pData,
+    IN  ULONG           DescriptorType);
+
+VOID    RTMPFrameEndianChange(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pData,
+    IN  ULONG           Dir,
+    IN  BOOLEAN         FromRxDoneInt);
+
+// ap_assoc.c
+
+VOID APAssocStateMachineInit(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  STATE_MACHINE *S,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID  APPeerAssocReqAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  APPeerReassocReqAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  APPeerDisassocReqAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MbssKickOutStas(
+	IN PRTMP_ADAPTER pAd,
+	IN INT apidx,
+	IN USHORT Reason);
+
+VOID APMlmeKickOutSta(
+    IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pStaAddr,
+	IN UCHAR Wcid,
+	IN USHORT Reason);
+
+VOID APMlmeDisassocReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID  APCls3errAction(
+    IN  PRTMP_ADAPTER   pAd,
+	IN 	ULONG Wcid,
+    IN	PHEADER_802_11	pHeader);
+
+
+USHORT APBuildAssociation(
+    IN PRTMP_ADAPTER pAd,
+    IN MAC_TABLE_ENTRY *pEntry,
+    IN USHORT CapabilityInfo,
+    IN UCHAR  MaxSupportedRateIn500Kbps,
+    IN UCHAR  *RSN,
+    IN UCHAR  *pRSNLen,
+    IN BOOLEAN bWmmCapable,
+    IN ULONG  RalinkIe,
+#ifdef DOT11N_DRAFT3
+    IN EXT_CAP_INFO_ELEMENT ExtCapInfo,
+#endif // DOT11N_DRAFT3 //
+	IN HT_CAPABILITY_IE		*pHtCapability,
+	IN UCHAR		 HtCapabilityLen,
+    OUT USHORT *pAid);
+
+// ap_auth.c
+
+void APAuthStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID APMlmeDeauthReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID APCls2errAction(
+    IN PRTMP_ADAPTER pAd,
+	IN 	ULONG Wcid,
+    IN	PHEADER_802_11	pHeader);
+
+// ap_authrsp.c
+
+VOID APAuthRspStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN PSTATE_MACHINE Sm,
+    IN STATE_MACHINE_FUNC Trans[]);
+
+VOID APPeerAuthAtAuthRspIdleAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerDeauthReqAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerAuthSimpleRspGenAndSend(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PHEADER_802_11 pHdr80211,
+    IN  USHORT Alg,
+    IN  USHORT Seq,
+    IN  USHORT StatusCode);
+
+// ap_connect.c
+
+BOOLEAN BeaconTransmitRequired(
+	IN PRTMP_ADAPTER	pAd,
+	IN INT				apidx);
+
+VOID APMakeBssBeacon(
+    IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx);
+
+VOID  APUpdateBeaconFrame(
+    IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx);
+
+VOID APMakeAllBssBeacon(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID  APUpdateAllBeaconFrame(
+    IN  PRTMP_ADAPTER   pAd);
+
+
+// ap_sync.c
+
+VOID APSyncStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID APScanTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID APInvalidStateWhenScan(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APScanTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerProbeReqAction(
+    IN  PRTMP_ADAPTER pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerBeaconAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID APMlmeScanReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerBeaconAtScanAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APScanCnclAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID ApSiteSurvey(
+	IN PRTMP_ADAPTER pAd);
+
+VOID SupportRate(
+	IN PUCHAR SupRate,
+	IN UCHAR SupRateLen,
+	IN PUCHAR ExtRate,
+	IN UCHAR ExtRateLen,
+	OUT PUCHAR *Rates,
+	OUT PUCHAR RatesLen,
+	OUT PUCHAR pMaxSupportRate);
+
+
+BOOLEAN ApScanRunning(
+	IN PRTMP_ADAPTER pAd);
+
+#ifdef DOT11N_DRAFT3
+VOID APOverlappingBSSScan(
+	IN RTMP_ADAPTER *pAd);
+#endif // DOT11N_DRAFT3 //
+
+// ap_wpa.c
+
+VOID APWpaStateMachineInit(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+// ap_mlme.c
+
+VOID APMlmePeriodicExec(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APMlmeSelectTxRateTable(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PUCHAR				*ppTable,
+	IN PUCHAR				pTableSize,
+	IN PUCHAR				pInitTxRateIdx);
+
+VOID APMlmeSetTxRate(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PRTMP_TX_RATE_SWITCH	pTxRate);
+
+VOID APMlmeDynamicTxRateSwitching(
+    IN PRTMP_ADAPTER pAd);
+
+VOID APQuickResponeForRateUpExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+BOOLEAN APMsgTypeSubst(
+    IN PRTMP_ADAPTER pAd,
+    IN PFRAME_802_11 pFrame,
+    OUT INT *Machine,
+    OUT INT *MsgType);
+
+VOID APQuickResponeForRateUpExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+
+VOID RTMPSetPiggyBack(
+	IN PRTMP_ADAPTER	pAd,
+	IN BOOLEAN			bPiggyBack);
+
+VOID APAsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID APAsicRxAntEvalTimeout(
+	IN PRTMP_ADAPTER	pAd);
+
+// ap.c
+
+VOID APSwitchChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN INT Channel);
+
+NDIS_STATUS APInitialize(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APShutdown(
+    IN PRTMP_ADAPTER    pAd);
+
+VOID APStartUp(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APStop(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APCleanupPsQueue(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PQUEUE_HEADER   pQueue);
+
+VOID MacTableReset(
+    IN  PRTMP_ADAPTER   pAd);
+
+MAC_TABLE_ENTRY *MacTableInsertEntry(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr,
+	IN	UCHAR			apidx,
+	IN BOOLEAN	CleanAll);
+
+BOOLEAN MacTableDeleteEntry(
+    IN  PRTMP_ADAPTER   pAd,
+	IN USHORT wcid,
+    IN  PUCHAR          pAddr);
+
+MAC_TABLE_ENTRY *MacTableLookup(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr);
+
+VOID MacTableMaintenance(
+    IN PRTMP_ADAPTER pAd);
+
+UINT32 MacTableAssocStaNumGet(
+	IN PRTMP_ADAPTER pAd);
+
+MAC_TABLE_ENTRY *APSsPsInquiry(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr,
+    OUT SST             *Sst,
+    OUT USHORT          *Aid,
+    OUT UCHAR           *PsMode,
+    OUT UCHAR           *Rate);
+
+BOOLEAN APPsIndicate(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr,
+	IN ULONG Wcid,
+    IN  UCHAR           Psm);
+
+VOID ApLogEvent(
+    IN PRTMP_ADAPTER    pAd,
+    IN PUCHAR           pAddr,
+    IN USHORT           Event);
+
+#ifdef DOT11_N_SUPPORT
+VOID APUpdateOperationMode(
+    IN PRTMP_ADAPTER pAd);
+#endif // DOT11_N_SUPPORT //
+
+VOID APUpdateCapabilityAndErpIe(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN ApCheckAccessControlList(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR         Apidx);
+
+VOID ApUpdateAccessControlList(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR         Apidx);
+
+VOID ApEnqueueNullFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR         TxRate,
+	IN UCHAR         PID,
+	IN UCHAR         apidx,
+    IN BOOLEAN       bQosNull,
+    IN BOOLEAN       bEOSP,
+    IN UCHAR         OldUP);
+
+VOID ApSendFrame(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PVOID           pBuffer,
+    IN  ULONG           Length,
+    IN  UCHAR           TxRate,
+    IN  UCHAR           PID);
+
+VOID ApEnqueueAckFrame(
+    IN PRTMP_ADAPTER pAd,
+    IN PUCHAR        pAddr,
+    IN UCHAR         TxRate,
+	IN UCHAR         apidx);
+
+UCHAR APAutoSelectChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN Optimal);
+
+// ap_sanity.c
+
+
+BOOLEAN PeerAssocReqCmmSanity(
+    IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN isRessoc,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pListenInterval,
+    OUT PUCHAR pApAddr,
+    OUT UCHAR *pSsidLen,
+    OUT char *Ssid,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *RSN,
+    OUT UCHAR *pRSNLen,
+    OUT BOOLEAN *pbWmmCapable,
+    OUT ULONG  *pRalinkIe,
+#ifdef DOT11N_DRAFT3
+    OUT EXT_CAP_INFO_ELEMENT	*pExtCapInfo,
+#endif // DOT11N_DRAFT3 //
+    OUT UCHAR		 *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability);
+
+BOOLEAN PeerDisassocReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *Reason);
+
+BOOLEAN PeerDeauthReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *Reason);
+
+BOOLEAN APPeerAuthSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+	OUT PUCHAR pAddr1,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *Alg,
+    OUT USHORT *Seq,
+    OUT USHORT *Status,
+    CHAR *ChlgText);
+
+BOOLEAN APPeerProbeReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT CHAR Ssid[],
+    OUT UCHAR *SsidLen);
+
+BOOLEAN APPeerBeaconAndProbeRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT PUCHAR pBssid,
+    OUT CHAR Ssid[],
+    OUT UCHAR *SsidLen,
+    OUT UCHAR *BssType,
+    OUT USHORT *BeaconPeriod,
+    OUT UCHAR *Channel,
+    OUT LARGE_INTEGER *Timestamp,
+    OUT USHORT *CapabilityInfo,
+    OUT UCHAR Rate[],
+    OUT UCHAR *RateLen,
+    OUT BOOLEAN *ExtendedRateIeExist,
+    OUT UCHAR *Erp);
+
+// ap_info.c
+
+#ifdef WIN_NDIS
+NDIS_STATUS APQueryInformation(
+    IN  NDIS_HANDLE MiniportAdapterContext,
+    IN  NDIS_OID    Oid,
+    IN  PVOID       pInformationBuffer,
+    IN  ULONG       InformationBufferLength,
+    OUT PULONG      pBytesWritten,
+    OUT PULONG      pBytesNeeded);
+
+NDIS_STATUS APSetInformation(
+    IN  NDIS_HANDLE MiniportAdapterContext,
+    IN  NDIS_OID    Oid,
+    IN  PVOID       pInformationBuffer,
+    IN  ULONG       InformationBufferLength,
+    OUT PULONG      pBytesRead,
+    OUT PULONG      pBytesNeeded);
+#endif
+
+
+// ================== end of AP RTMP.h ========================
+
+
+#endif  // __AP_H__
+
diff --git a/drivers/staging/rt2860/chlist.h b/drivers/staging/rt2860/chlist.h
new file mode 100644
index 000000000000..9e15b9daeb80
--- /dev/null
+++ b/drivers/staging/rt2860/chlist.h
@@ -0,0 +1,1296 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	chlist.c
+
+	Abstract:
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Fonchi Wu   2007-12-19    created
+*/
+
+#ifndef __CHLIST_H__
+#define __CHLIST_H__
+
+#include "rtmp_type.h"
+#include "rtmp_def.h"
+
+
+#define ODOR			0
+#define IDOR			1
+#define BOTH			2
+
+#define BAND_5G         0
+#define BAND_24G        1
+#define BAND_BOTH       2
+
+typedef struct _CH_DESP {
+	UCHAR FirstChannel;
+	UCHAR NumOfCh;
+	CHAR MaxTxPwr;			// dBm
+	UCHAR Geography;			// 0:out door, 1:in door, 2:both
+	BOOLEAN DfsReq;			// Dfs require, 0: No, 1: yes.
+} CH_DESP, *PCH_DESP;
+
+typedef struct _CH_REGION {
+	UCHAR CountReg[3];
+	UCHAR DfsType;			// 0: CE, 1: FCC, 2: JAP, 3:JAP_W53, JAP_W56
+	CH_DESP ChDesp[10];
+} CH_REGION, *PCH_REGION;
+
+static CH_REGION ChRegion[] =
+{
+		{	// Antigua and Berbuda
+			"AG",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Argentina
+			"AR",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Aruba
+			"AW",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Australia
+			"AU",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Austria
+			"AT",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, TRUE},		// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},		// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, TRUE},		// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Bahamas
+			"BS",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Barbados
+			"BB",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Bermuda
+			"BM",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Brazil
+			"BR",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 24, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Belgium
+			"BE",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  18, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  18, IDOR, FALSE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Bulgaria
+			"BG",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11, 30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Canada
+			"CA",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Cayman IsLands
+			"KY",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Chile
+			"CL",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  20, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  20, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// China
+			"CN",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149, 4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Colombia
+			"CO",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Costa Rica
+			"CR",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Cyprus
+			"CY",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, IDOR, TRUE},		// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, TRUE},		// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Czech_Republic
+			"CZ",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},		// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Denmark
+			"DK",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},		// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, TRUE},		// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Dominican Republic
+			"DO",
+			CE,
+			{
+				{ 1,   0,  20, BOTH, FALSE},	// 2.4 G, ch 0
+				{ 149, 4,  20, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Equador
+			"EC",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 100, 11,  27, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// El Salvador
+			"SV",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   30, BOTH, TRUE},	// 5G, ch 52~64
+				{ 149, 4,   36, BOTH, TRUE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Finland
+			"FI",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// France
+			"FR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Germany
+			"DE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Greece
+			"GR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Guam
+			"GU",
+			CE,
+			{
+				{ 1,   11,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,  4,   17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149,  5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Guatemala
+			"GT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Haiti
+			"HT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Honduras
+			"HN",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149,  4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Hong Kong
+			"HK",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Hungary
+			"HU",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Iceland
+			"IS",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// India
+			"IN",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149, 	4,  24, IDOR, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Indonesia
+			"ID",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149, 	4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Ireland
+			"IE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Israel
+			"IL",
+			CE,
+			{
+				{ 1,    3,  20, IDOR, FALSE},	// 2.4 G, ch 1~3
+				{ 4, 	6,  20, BOTH, FALSE},	// 2.4 G, ch 4~9
+				{ 10, 	4,  20, IDOR, FALSE},	// 2.4 G, ch 10~13
+				{ 0},							// end
+			}
+		},
+
+		{	// Italy
+			"IT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Japan
+			"JP",
+			JAP,
+			{
+				{ 1,   14,  20, BOTH, FALSE},	// 2.4 G, ch 1~14
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 0},							// end
+			}
+		},
+
+		{	// Jordan
+			"JO",
+			CE,
+			{
+				{ 1,   13,  20, IDOR, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 149, 	4,  23, IDOR, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Latvia
+			"LV",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Liechtenstein
+			"LI",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Lithuania
+			"LT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Luxemburg
+			"LU",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Malaysia
+			"MY",
+			CE,
+			{
+				{ 36, 	4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  5,  20, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Malta
+			"MT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Marocco
+			"MA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  24, IDOR, FALSE},	// 5G, ch 36~48
+				{ 0},							// end
+			}
+		},
+
+		{	// Mexico
+			"MX",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  5,  30, IDOR, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Netherlands
+			"NL",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// New Zealand
+			"NZ",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  24, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Norway
+			"NO",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  24, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Peru
+			"PE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149,  4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Portugal
+			"PT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Poland
+			"PL",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Romania
+			"RO",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Russia
+			"RU",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149,  4,  20, IDOR, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Saudi Arabia
+			"SA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  23, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Serbia_and_Montenegro
+			"CS",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 0},							// end
+			}
+		},
+
+		{	// Singapore
+			"SG",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  20, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Slovakia
+			"SK",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Slovenia
+			"SI",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// South Africa
+			"ZA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// South Korea
+			"KR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  20, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100,  8,  20, BOTH, FALSE},	// 5G, ch 100~128
+				{ 149,  4,  20, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Spain
+			"ES",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  17, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Sweden
+			"SE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Switzerland
+			"CH",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, TRUE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Taiwan
+			"TW",
+			CE,
+			{
+				{ 1,   11,  30, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 52,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Turkey
+			"TR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// UK
+			"GB",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Ukraine
+			"UA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 0},							// end
+			}
+		},
+
+		{	// United_Arab_Emirates
+			"AE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 0},							// end
+			}
+		},
+
+		{	// United_States
+			"US",
+			CE,
+			{
+				{ 1,   11,  30, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  17, IDOR, FALSE},	// 5G, ch 52~64
+				{ 52,   4,  24, BOTH, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 149,  5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Venezuela
+			"VE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 149,  4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Default
+			"",
+			CE,
+			{
+				{ 1,   11,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 52,   4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11,  20, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149,  5,  20, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+};
+
+static inline PCH_REGION GetChRegion(
+	IN PUCHAR CntryCode)
+{
+	INT loop = 0;
+	PCH_REGION pChRegion = NULL;
+
+	while (strcmp(ChRegion[loop].CountReg, "") != 0)
+	{
+		if (strncmp(ChRegion[loop].CountReg, CntryCode, 2) == 0)
+		{
+			pChRegion = &ChRegion[loop];
+			break;
+		}
+		loop++;
+	}
+
+	if (pChRegion == NULL)
+		pChRegion = &ChRegion[loop];
+	return pChRegion;
+}
+
+static inline VOID ChBandCheck(
+	IN UCHAR PhyMode,
+	OUT PUCHAR pChType)
+{
+	switch(PhyMode)
+	{
+		case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11AN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			*pChType = BAND_5G;
+			break;
+		case PHY_11ABG_MIXED:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11AGN_MIXED:
+		case PHY_11ABGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			*pChType = BAND_BOTH;
+			break;
+
+		default:
+			*pChType = BAND_24G;
+			break;
+	}
+}
+
+static inline UCHAR FillChList(
+	IN PRTMP_ADAPTER pAd,
+	IN PCH_DESP pChDesp,
+	IN UCHAR Offset,
+	IN UCHAR increment)
+{
+	INT i, j, l;
+	UCHAR channel;
+
+	j = Offset;
+	for (i = 0; i < pChDesp->NumOfCh; i++)
+	{
+		channel = pChDesp->FirstChannel + i * increment;
+		for (l=0; l<MAX_NUM_OF_CHANNELS; l++)
+		{
+			if (channel == pAd->TxPower[l].Channel)
+			{
+				pAd->ChannelList[j].Power = pAd->TxPower[l].Power;
+				pAd->ChannelList[j].Power2 = pAd->TxPower[l].Power2;
+				break;
+			}
+		}
+		if (l == MAX_NUM_OF_CHANNELS)
+			continue;
+
+		pAd->ChannelList[j].Channel = pChDesp->FirstChannel + i * increment;
+		pAd->ChannelList[j].MaxTxPwr = pChDesp->MaxTxPwr;
+		pAd->ChannelList[j].DfsReq = pChDesp->DfsReq;
+		j++;
+	}
+	pAd->ChannelListNum = j;
+
+	return j;
+}
+
+static inline VOID CreateChList(
+	IN PRTMP_ADAPTER pAd,
+	IN PCH_REGION pChRegion,
+	IN UCHAR Geography)
+{
+	INT i;
+	UCHAR offset = 0;
+	PCH_DESP pChDesp;
+	UCHAR ChType;
+	UCHAR increment;
+
+	if (pChRegion == NULL)
+		return;
+
+	ChBandCheck(pAd->CommonCfg.PhyMode, &ChType);
+
+	for (i=0; i<10; i++)
+	{
+		pChDesp = &pChRegion->ChDesp[i];
+		if (pChDesp->FirstChannel == 0)
+			break;
+
+		if (ChType == BAND_5G)
+		{
+			if (pChDesp->FirstChannel <= 14)
+				continue;
+		}
+		else if (ChType == BAND_24G)
+		{
+			if (pChDesp->FirstChannel > 14)
+				continue;
+		}
+
+		if ((pChDesp->Geography == BOTH)
+			|| (pChDesp->Geography == Geography))
+        {
+			if (pChDesp->FirstChannel > 14)
+                increment = 4;
+            else
+                increment = 1;
+			offset = FillChList(pAd, pChDesp, offset, increment);
+        }
+	}
+}
+
+static inline VOID BuildChannelListEx(
+	IN PRTMP_ADAPTER pAd)
+{
+	PCH_REGION pChReg;
+
+	pChReg = GetChRegion(pAd->CommonCfg.CountryCode);
+	CreateChList(pAd, pChReg, pAd->CommonCfg.Geography);
+}
+
+static inline VOID BuildBeaconChList(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pBuf,
+	OUT	PULONG pBufLen)
+{
+	INT i;
+	ULONG TmpLen;
+	PCH_REGION pChRegion;
+	PCH_DESP pChDesp;
+	UCHAR ChType;
+
+	pChRegion = GetChRegion(pAd->CommonCfg.CountryCode);
+
+	if (pChRegion == NULL)
+		return;
+
+	ChBandCheck(pAd->CommonCfg.PhyMode, &ChType);
+	*pBufLen = 0;
+
+	for (i=0; i<10; i++)
+	{
+		pChDesp = &pChRegion->ChDesp[i];
+		if (pChDesp->FirstChannel == 0)
+			break;
+
+		if (ChType == BAND_5G)
+		{
+			if (pChDesp->FirstChannel <= 14)
+				continue;
+		}
+		else if (ChType == BAND_24G)
+		{
+			if (pChDesp->FirstChannel > 14)
+				continue;
+		}
+
+		if ((pChDesp->Geography == BOTH)
+			|| (pChDesp->Geography == pAd->CommonCfg.Geography))
+		{
+			MakeOutgoingFrame(pBuf + *pBufLen,		&TmpLen,
+								1,                 	&pChDesp->FirstChannel,
+								1,                 	&pChDesp->NumOfCh,
+								1,                 	&pChDesp->MaxTxPwr,
+								END_OF_ARGS);
+			*pBufLen += TmpLen;
+		}
+	}
+}
+
+
+#ifdef DOT11_N_SUPPORT
+static inline BOOLEAN IsValidChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR channel)
+
+{
+	INT i;
+
+	for (i = 0; i < pAd->ChannelListNum; i++)
+	{
+		if (pAd->ChannelList[i].Channel == channel)
+			break;
+	}
+
+	if (i == pAd->ChannelListNum)
+		return FALSE;
+	else
+		return TRUE;
+}
+
+
+static inline UCHAR GetExtCh(
+	IN UCHAR Channel,
+	IN UCHAR Direction)
+{
+	CHAR ExtCh;
+
+	if (Direction == EXTCHA_ABOVE)
+		ExtCh = Channel + 4;
+	else
+		ExtCh = (Channel - 4) > 0 ? (Channel - 4) : 0;
+
+	return ExtCh;
+}
+
+
+static inline VOID N_ChannelCheck(
+	IN PRTMP_ADAPTER pAd)
+{
+	//UCHAR ChannelNum = pAd->ChannelListNum;
+	UCHAR Channel = pAd->CommonCfg.Channel;
+
+	if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED) && (pAd->CommonCfg.RegTransmitSetting.field.BW  == BW_40))
+	{
+		if (Channel > 14)
+		{
+			if ((Channel == 36) || (Channel == 44) || (Channel == 52) || (Channel == 60) || (Channel == 100) || (Channel == 108) ||
+			    (Channel == 116) || (Channel == 124) || (Channel == 132) || (Channel == 149) || (Channel == 157))
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+			}
+			else if ((Channel == 40) || (Channel == 48) || (Channel == 56) || (Channel == 64) || (Channel == 104) || (Channel == 112) ||
+					(Channel == 120) || (Channel == 128) || (Channel == 136) || (Channel == 153) || (Channel == 161))
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+			}
+			else
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+			}
+		}
+		else
+		{
+			do
+			{
+				UCHAR ExtCh;
+				UCHAR Dir = pAd->CommonCfg.RegTransmitSetting.field.EXTCHA;
+				ExtCh = GetExtCh(Channel, Dir);
+				if (IsValidChannel(pAd, ExtCh))
+					break;
+
+				Dir = (Dir == EXTCHA_ABOVE) ? EXTCHA_BELOW : EXTCHA_ABOVE;
+				ExtCh = GetExtCh(Channel, Dir);
+				if (IsValidChannel(pAd, ExtCh))
+				{
+					pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = Dir;
+					break;
+				}
+				pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+			} while(FALSE);
+
+			if (Channel == 14)
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+				//pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_NONE;	// We didn't set the ExtCh as NONE due to it'll set in RTMPSetHT()
+			}
+#if 0
+			switch (pAd->CommonCfg.CountryRegion  & 0x7f)
+			{
+				case REGION_0_BG_BAND:	// 1 -11
+				case REGION_1_BG_BAND:	// 1 - 13
+				case REGION_5_BG_BAND:	// 1 - 14
+					if (Channel <= 4)
+					{
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+					}
+					else if (Channel >= 8)
+					{
+						if ((ChannelNum - Channel) < 4)
+							pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+					}
+					break;
+
+				case REGION_2_BG_BAND:	// 10 - 11
+				case REGION_3_BG_BAND:	// 10 - 13
+				case REGION_4_BG_BAND:	// 14
+					pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+					break;
+
+				case REGION_6_BG_BAND:	// 3 - 9
+					if (Channel <= 5)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+					else if (Channel == 6)
+						pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+					else if (Channel >= 7)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+					break;
+
+				case REGION_7_BG_BAND:  // 5 - 13
+					if (Channel <= 8)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+					else if (Channel >= 10)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+					break;
+
+				default:	// Error. should never happen
+					break;
+			}
+#endif
+		}
+	}
+
+
+}
+
+
+static inline VOID N_SetCenCh(
+	IN PRTMP_ADAPTER pAd)
+{
+	if (pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40)
+	{
+		if (pAd->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_ABOVE)
+		{
+			pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel + 2;
+		}
+		else
+		{
+			if (pAd->CommonCfg.Channel == 14)
+				pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel - 1;
+			else
+				pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel - 2;
+		}
+	}
+	else
+	{
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+
+static inline UINT8 GetCuntryMaxTxPwr(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT8 channel)
+{
+	int i;
+	for (i = 0; i < pAd->ChannelListNum; i++)
+	{
+		if (pAd->ChannelList[i].Channel == channel)
+			break;
+	}
+
+	if (i == pAd->ChannelListNum)
+		return 0xff;
+	else
+		return pAd->ChannelList[i].MaxTxPwr;
+}
+#endif // __CHLIST_H__
+
diff --git a/drivers/staging/rt2860/common/2860_rtmp_init.c b/drivers/staging/rt2860/common/2860_rtmp_init.c
new file mode 100644
index 000000000000..546f304ec330
--- /dev/null
+++ b/drivers/staging/rt2860/common/2860_rtmp_init.c
@@ -0,0 +1,922 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	2860_rtmp_init.c
+
+	Abstract:
+	Miniport generic portion header file
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Paul Lin    2002-08-01    created
+    John Chang  2004-08-20    RT2561/2661 use scatter-gather scheme
+    Jan Lee  2006-09-15    RT2860. Change for 802.11n , EEPROM, Led, BA, HT.
+*/
+#include "../rt_config.h"
+
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Allocate DMA memory blocks for send, receive
+
+	Arguments:
+		Adapter		Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_RESOURCES
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	RTMPAllocTxRxRingMemory(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	NDIS_STATUS		Status = NDIS_STATUS_SUCCESS;
+	ULONG			RingBasePaHigh;
+	ULONG			RingBasePaLow;
+	PVOID			RingBaseVa;
+	INT				index, num;
+	PTXD_STRUC		pTxD;
+	PRXD_STRUC		pRxD;
+	ULONG			ErrorValue = 0;
+	PRTMP_TX_RING	pTxRing;
+	PRTMP_DMABUF	pDmaBuf;
+	PNDIS_PACKET	pPacket;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> RTMPAllocTxRxRingMemory\n"));
+	do
+	{
+		//
+		// Allocate all ring descriptors, include TxD, RxD, MgmtD.
+		// Although each size is different, to prevent cacheline and alignment
+		// issue, I intentional set them all to 64 bytes.
+		//
+		for (num=0; num<NUM_OF_TX_RING; num++)
+		{
+			ULONG  BufBasePaHigh;
+			ULONG  BufBasePaLow;
+			PVOID  BufBaseVa;
+
+			//
+			// Allocate Tx ring descriptor's memory (5 TX rings = 4 ACs + 1 HCCA)
+			//
+			pAd->TxDescRing[num].AllocSize = TX_RING_SIZE * TXD_SIZE;
+			RTMP_AllocateTxDescMemory(
+				pAd,
+				num,
+				pAd->TxDescRing[num].AllocSize,
+				FALSE,
+				&pAd->TxDescRing[num].AllocVa,
+				&pAd->TxDescRing[num].AllocPa);
+
+			if (pAd->TxDescRing[num].AllocVa == NULL)
+			{
+				ErrorValue = ERRLOG_OUT_OF_SHARED_MEMORY;
+				DBGPRINT_ERR(("Failed to allocate a big buffer\n"));
+				Status = NDIS_STATUS_RESOURCES;
+				break;
+			}
+
+			// Zero init this memory block
+			NdisZeroMemory(pAd->TxDescRing[num].AllocVa, pAd->TxDescRing[num].AllocSize);
+
+			// Save PA & VA for further operation
+			RingBasePaHigh = RTMP_GetPhysicalAddressHigh(pAd->TxDescRing[num].AllocPa);
+			RingBasePaLow  = RTMP_GetPhysicalAddressLow (pAd->TxDescRing[num].AllocPa);
+			RingBaseVa     = pAd->TxDescRing[num].AllocVa;
+
+			//
+			// Allocate all 1st TXBuf's memory for this TxRing
+			//
+			pAd->TxBufSpace[num].AllocSize = TX_RING_SIZE * TX_DMA_1ST_BUFFER_SIZE;
+			RTMP_AllocateFirstTxBuffer(
+				pAd,
+				num,
+				pAd->TxBufSpace[num].AllocSize,
+				FALSE,
+				&pAd->TxBufSpace[num].AllocVa,
+				&pAd->TxBufSpace[num].AllocPa);
+
+			if (pAd->TxBufSpace[num].AllocVa == NULL)
+			{
+				ErrorValue = ERRLOG_OUT_OF_SHARED_MEMORY;
+				DBGPRINT_ERR(("Failed to allocate a big buffer\n"));
+				Status = NDIS_STATUS_RESOURCES;
+				break;
+			}
+
+			// Zero init this memory block
+			NdisZeroMemory(pAd->TxBufSpace[num].AllocVa, pAd->TxBufSpace[num].AllocSize);
+
+			// Save PA & VA for further operation
+			BufBasePaHigh = RTMP_GetPhysicalAddressHigh(pAd->TxBufSpace[num].AllocPa);
+			BufBasePaLow  = RTMP_GetPhysicalAddressLow (pAd->TxBufSpace[num].AllocPa);
+			BufBaseVa     = pAd->TxBufSpace[num].AllocVa;
+
+			//
+			// Initialize Tx Ring Descriptor and associated buffer memory
+			//
+			pTxRing = &pAd->TxRing[num];
+			for (index = 0; index < TX_RING_SIZE; index++)
+			{
+				pTxRing->Cell[index].pNdisPacket = NULL;
+				pTxRing->Cell[index].pNextNdisPacket = NULL;
+				// Init Tx Ring Size, Va, Pa variables
+				pTxRing->Cell[index].AllocSize = TXD_SIZE;
+				pTxRing->Cell[index].AllocVa = RingBaseVa;
+				RTMP_SetPhysicalAddressHigh(pTxRing->Cell[index].AllocPa, RingBasePaHigh);
+				RTMP_SetPhysicalAddressLow (pTxRing->Cell[index].AllocPa, RingBasePaLow);
+
+				// Setup Tx Buffer size & address. only 802.11 header will store in this space
+				pDmaBuf = &pTxRing->Cell[index].DmaBuf;
+				pDmaBuf->AllocSize = TX_DMA_1ST_BUFFER_SIZE;
+				pDmaBuf->AllocVa = BufBaseVa;
+				RTMP_SetPhysicalAddressHigh(pDmaBuf->AllocPa, BufBasePaHigh);
+				RTMP_SetPhysicalAddressLow(pDmaBuf->AllocPa, BufBasePaLow);
+
+				// link the pre-allocated TxBuf to TXD
+				pTxD = (PTXD_STRUC) pTxRing->Cell[index].AllocVa;
+				pTxD->SDPtr0 = BufBasePaLow;
+				// advance to next ring descriptor address
+				pTxD->DMADONE = 1;
+#ifdef RT_BIG_ENDIAN
+				RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+				RingBasePaLow += TXD_SIZE;
+				RingBaseVa = (PUCHAR) RingBaseVa + TXD_SIZE;
+
+				// advance to next TxBuf address
+				BufBasePaLow += TX_DMA_1ST_BUFFER_SIZE;
+				BufBaseVa = (PUCHAR) BufBaseVa + TX_DMA_1ST_BUFFER_SIZE;
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("TxRing[%d]: total %d entry allocated\n", num, index));
+		}
+		if (Status == NDIS_STATUS_RESOURCES)
+			break;
+
+		//
+		// Allocate MGMT ring descriptor's memory except Tx ring which allocated eariler
+		//
+		pAd->MgmtDescRing.AllocSize = MGMT_RING_SIZE * TXD_SIZE;
+		RTMP_AllocateMgmtDescMemory(
+			pAd,
+			pAd->MgmtDescRing.AllocSize,
+			FALSE,
+			&pAd->MgmtDescRing.AllocVa,
+			&pAd->MgmtDescRing.AllocPa);
+
+		if (pAd->MgmtDescRing.AllocVa == NULL)
+		{
+			ErrorValue = ERRLOG_OUT_OF_SHARED_MEMORY;
+			DBGPRINT_ERR(("Failed to allocate a big buffer\n"));
+			Status = NDIS_STATUS_RESOURCES;
+			break;
+		}
+
+		// Zero init this memory block
+		NdisZeroMemory(pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocSize);
+
+		// Save PA & VA for further operation
+		RingBasePaHigh = RTMP_GetPhysicalAddressHigh(pAd->MgmtDescRing.AllocPa);
+		RingBasePaLow  = RTMP_GetPhysicalAddressLow (pAd->MgmtDescRing.AllocPa);
+		RingBaseVa     = pAd->MgmtDescRing.AllocVa;
+
+		//
+		// Initialize MGMT Ring and associated buffer memory
+		//
+		for (index = 0; index < MGMT_RING_SIZE; index++)
+		{
+			pAd->MgmtRing.Cell[index].pNdisPacket = NULL;
+			pAd->MgmtRing.Cell[index].pNextNdisPacket = NULL;
+			// Init MGMT Ring Size, Va, Pa variables
+			pAd->MgmtRing.Cell[index].AllocSize = TXD_SIZE;
+			pAd->MgmtRing.Cell[index].AllocVa = RingBaseVa;
+			RTMP_SetPhysicalAddressHigh(pAd->MgmtRing.Cell[index].AllocPa, RingBasePaHigh);
+			RTMP_SetPhysicalAddressLow (pAd->MgmtRing.Cell[index].AllocPa, RingBasePaLow);
+
+			// Offset to next ring descriptor address
+			RingBasePaLow += TXD_SIZE;
+			RingBaseVa = (PUCHAR) RingBaseVa + TXD_SIZE;
+
+			// link the pre-allocated TxBuf to TXD
+			pTxD = (PTXD_STRUC) pAd->MgmtRing.Cell[index].AllocVa;
+			pTxD->DMADONE = 1;
+
+#ifdef RT_BIG_ENDIAN
+			RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+			// no pre-allocated buffer required in MgmtRing for scatter-gather case
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("MGMT Ring: total %d entry allocated\n", index));
+
+		//
+		// Allocate RX ring descriptor's memory except Tx ring which allocated eariler
+		//
+		pAd->RxDescRing.AllocSize = RX_RING_SIZE * RXD_SIZE;
+		RTMP_AllocateRxDescMemory(
+			pAd,
+			pAd->RxDescRing.AllocSize,
+			FALSE,
+			&pAd->RxDescRing.AllocVa,
+			&pAd->RxDescRing.AllocPa);
+
+		if (pAd->RxDescRing.AllocVa == NULL)
+		{
+			ErrorValue = ERRLOG_OUT_OF_SHARED_MEMORY;
+			DBGPRINT_ERR(("Failed to allocate a big buffer\n"));
+			Status = NDIS_STATUS_RESOURCES;
+			break;
+		}
+
+		// Zero init this memory block
+		NdisZeroMemory(pAd->RxDescRing.AllocVa, pAd->RxDescRing.AllocSize);
+
+
+		printk("RX DESC %p  size = %ld\n", pAd->RxDescRing.AllocVa,
+					pAd->RxDescRing.AllocSize);
+
+		// Save PA & VA for further operation
+		RingBasePaHigh = RTMP_GetPhysicalAddressHigh(pAd->RxDescRing.AllocPa);
+		RingBasePaLow  = RTMP_GetPhysicalAddressLow (pAd->RxDescRing.AllocPa);
+		RingBaseVa     = pAd->RxDescRing.AllocVa;
+
+		//
+		// Initialize Rx Ring and associated buffer memory
+		//
+		for (index = 0; index < RX_RING_SIZE; index++)
+		{
+			// Init RX Ring Size, Va, Pa variables
+			pAd->RxRing.Cell[index].AllocSize = RXD_SIZE;
+			pAd->RxRing.Cell[index].AllocVa = RingBaseVa;
+			RTMP_SetPhysicalAddressHigh(pAd->RxRing.Cell[index].AllocPa, RingBasePaHigh);
+			RTMP_SetPhysicalAddressLow (pAd->RxRing.Cell[index].AllocPa, RingBasePaLow);
+
+			// Offset to next ring descriptor address
+			RingBasePaLow += RXD_SIZE;
+			RingBaseVa = (PUCHAR) RingBaseVa + RXD_SIZE;
+
+			// Setup Rx associated Buffer size & allocate share memory
+			pDmaBuf = &pAd->RxRing.Cell[index].DmaBuf;
+			pDmaBuf->AllocSize = RX_BUFFER_AGGRESIZE;
+			pPacket = RTMP_AllocateRxPacketBuffer(
+				pAd,
+				pDmaBuf->AllocSize,
+				FALSE,
+				&pDmaBuf->AllocVa,
+				&pDmaBuf->AllocPa);
+
+			/* keep allocated rx packet */
+			pAd->RxRing.Cell[index].pNdisPacket = pPacket;
+
+			// Error handling
+			if (pDmaBuf->AllocVa == NULL)
+			{
+				ErrorValue = ERRLOG_OUT_OF_SHARED_MEMORY;
+				DBGPRINT_ERR(("Failed to allocate RxRing's 1st buffer\n"));
+				Status = NDIS_STATUS_RESOURCES;
+				break;
+			}
+
+			// Zero init this memory block
+			NdisZeroMemory(pDmaBuf->AllocVa, pDmaBuf->AllocSize);
+
+			// Write RxD buffer address & allocated buffer length
+			pRxD = (PRXD_STRUC) pAd->RxRing.Cell[index].AllocVa;
+			pRxD->SDP0 = RTMP_GetPhysicalAddressLow(pDmaBuf->AllocPa);
+			pRxD->DDONE = 0;
+
+#ifdef RT_BIG_ENDIAN
+			RTMPDescriptorEndianChange((PUCHAR)pRxD, TYPE_RXD);
+#endif
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Rx Ring: total %d entry allocated\n", index));
+
+	}	while (FALSE);
+
+
+	NdisZeroMemory(&pAd->FragFrame, sizeof(FRAGMENT_FRAME));
+	pAd->FragFrame.pFragPacket =  RTMP_AllocateFragPacketBuffer(pAd, RX_BUFFER_NORMSIZE);
+
+	if (pAd->FragFrame.pFragPacket == NULL)
+	{
+		Status = NDIS_STATUS_RESOURCES;
+	}
+
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		// Log error inforamtion
+		NdisWriteErrorLogEntry(
+			pAd->AdapterHandle,
+			NDIS_ERROR_CODE_OUT_OF_RESOURCES,
+			1,
+			ErrorValue);
+	}
+
+	DBGPRINT_S(Status, ("<-- RTMPAllocTxRxRingMemory, Status=%x\n", Status));
+	return Status;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize transmit data structures
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Initialize all transmit releated private buffer, include those define
+		in RTMP_ADAPTER structure and all private data structures.
+
+	========================================================================
+*/
+VOID	NICInitTxRxRingAndBacklogQueue(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	//WPDMA_GLO_CFG_STRUC	GloCfg;
+	int i;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<--> NICInitTxRxRingAndBacklogQueue\n"));
+
+	// Initialize all transmit related software queues
+	InitializeQueueHeader(&pAd->TxSwQueue[QID_AC_BE]);
+	InitializeQueueHeader(&pAd->TxSwQueue[QID_AC_BK]);
+	InitializeQueueHeader(&pAd->TxSwQueue[QID_AC_VI]);
+	InitializeQueueHeader(&pAd->TxSwQueue[QID_AC_VO]);
+	InitializeQueueHeader(&pAd->TxSwQueue[QID_HCCA]);
+
+	// Init RX Ring index pointer
+	pAd->RxRing.RxSwReadIdx = 0;
+	pAd->RxRing.RxCpuIdx = RX_RING_SIZE - 1;
+
+	// Init TX rings index pointer
+		for (i=0; i<NUM_OF_TX_RING; i++)
+		{
+			pAd->TxRing[i].TxSwFreeIdx = 0;
+			pAd->TxRing[i].TxCpuIdx = 0;
+		}
+
+	// init MGMT ring index pointer
+	pAd->MgmtRing.TxSwFreeIdx = 0;
+	pAd->MgmtRing.TxCpuIdx = 0;
+
+	pAd->PrivateInfo.TxRingFullCnt       = 0;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Reset NIC Asics. Call after rest DMA. So reset TX_CTX_IDX to zero.
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		Reset NIC to initial state AS IS system boot up time.
+
+	========================================================================
+*/
+VOID	RTMPRingCleanUp(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			RingType)
+{
+	PTXD_STRUC		pTxD;
+	PRXD_STRUC		pRxD;
+	PQUEUE_ENTRY	pEntry;
+	PNDIS_PACKET	pPacket;
+	int				i;
+	PRTMP_TX_RING	pTxRing;
+	unsigned long	IrqFlags;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPRingCleanUp(RingIdx=%d, Pending-NDIS=%ld)\n", RingType, pAd->RalinkCounters.PendingNdisPacketCount));
+	switch (RingType)
+	{
+		case QID_AC_BK:
+		case QID_AC_BE:
+		case QID_AC_VI:
+		case QID_AC_VO:
+		case QID_HCCA:
+			RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
+			pTxRing = &pAd->TxRing[RingType];
+
+			// We have to clean all descriptors in case some error happened with reset
+			for (i=0; i<TX_RING_SIZE; i++) // We have to scan all TX ring
+			{
+				pTxD  = (PTXD_STRUC) pTxRing->Cell[i].AllocVa;
+
+				pPacket = (PNDIS_PACKET) pTxRing->Cell[i].pNdisPacket;
+				// release scatter-and-gather NDIS_PACKET
+				if (pPacket)
+				{
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+					pTxRing->Cell[i].pNdisPacket = NULL;
+				}
+
+				pPacket = (PNDIS_PACKET) pTxRing->Cell[i].pNextNdisPacket;
+				// release scatter-and-gather NDIS_PACKET
+				if (pPacket)
+				{
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+					pTxRing->Cell[i].pNextNdisPacket = NULL;
+				}
+			}
+
+			RTMP_IO_READ32(pAd, TX_DTX_IDX0 + RingType * 0x10, &pTxRing->TxDmaIdx);
+			pTxRing->TxSwFreeIdx = pTxRing->TxDmaIdx;
+			pTxRing->TxCpuIdx = pTxRing->TxDmaIdx;
+			RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + RingType * 0x10, pTxRing->TxCpuIdx);
+
+			RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+			RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
+			while (pAd->TxSwQueue[RingType].Head != NULL)
+			{
+				pEntry = RemoveHeadQueue(&pAd->TxSwQueue[RingType]);
+				pPacket = QUEUE_ENTRY_TO_PACKET(pEntry);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+				DBGPRINT(RT_DEBUG_TRACE,("Release 1 NDIS packet from s/w backlog queue\n"));
+			}
+			RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+			break;
+
+		case QID_MGMT:
+			// We have to clean all descriptors in case some error happened with reset
+			NdisAcquireSpinLock(&pAd->MgmtRingLock);
+
+			for (i=0; i<MGMT_RING_SIZE; i++)
+			{
+				pTxD  = (PTXD_STRUC) pAd->MgmtRing.Cell[i].AllocVa;
+
+				pPacket = (PNDIS_PACKET) pAd->MgmtRing.Cell[i].pNdisPacket;
+				// rlease scatter-and-gather NDIS_PACKET
+				if (pPacket)
+				{
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+				}
+				pAd->MgmtRing.Cell[i].pNdisPacket = NULL;
+
+				pPacket = (PNDIS_PACKET) pAd->MgmtRing.Cell[i].pNextNdisPacket;
+				// release scatter-and-gather NDIS_PACKET
+				if (pPacket)
+				{
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+				}
+				pAd->MgmtRing.Cell[i].pNextNdisPacket = NULL;
+
+			}
+
+			RTMP_IO_READ32(pAd, TX_MGMTDTX_IDX, &pAd->MgmtRing.TxDmaIdx);
+			pAd->MgmtRing.TxSwFreeIdx = pAd->MgmtRing.TxDmaIdx;
+			pAd->MgmtRing.TxCpuIdx = pAd->MgmtRing.TxDmaIdx;
+			RTMP_IO_WRITE32(pAd, TX_MGMTCTX_IDX, pAd->MgmtRing.TxCpuIdx);
+
+			NdisReleaseSpinLock(&pAd->MgmtRingLock);
+			pAd->RalinkCounters.MgmtRingFullCount = 0;
+			break;
+
+		case QID_RX:
+			// We have to clean all descriptors in case some error happened with reset
+			NdisAcquireSpinLock(&pAd->RxRingLock);
+
+			for (i=0; i<RX_RING_SIZE; i++)
+			{
+				pRxD  = (PRXD_STRUC) pAd->RxRing.Cell[i].AllocVa;
+				pRxD->DDONE = 0 ;
+			}
+
+			RTMP_IO_READ32(pAd, RX_DRX_IDX, &pAd->RxRing.RxDmaIdx);
+			pAd->RxRing.RxSwReadIdx = pAd->RxRing.RxDmaIdx;
+			pAd->RxRing.RxCpuIdx = ((pAd->RxRing.RxDmaIdx == 0) ? (RX_RING_SIZE-1) : (pAd->RxRing.RxDmaIdx-1));
+			RTMP_IO_WRITE32(pAd, RX_CRX_IDX, pAd->RxRing.RxCpuIdx);
+
+			NdisReleaseSpinLock(&pAd->RxRingLock);
+			break;
+
+		default:
+			break;
+	}
+}
+
+
+NDIS_STATUS AdapterBlockAllocateMemory(
+	IN PVOID	handle,
+	OUT	PVOID	*ppAd)
+{
+	PPCI_DEV pci_dev;
+	dma_addr_t	*phy_addr;
+	POS_COOKIE pObj = (POS_COOKIE) handle;
+
+	pci_dev = pObj->pci_dev;
+	phy_addr = &pObj->pAd_pa;
+
+	*ppAd = (PVOID)vmalloc(sizeof(RTMP_ADAPTER)); //pci_alloc_consistent(pci_dev, sizeof(RTMP_ADAPTER), phy_addr);
+
+	if (*ppAd)
+	{
+		NdisZeroMemory(*ppAd, sizeof(RTMP_ADAPTER));
+		((PRTMP_ADAPTER)*ppAd)->OS_Cookie = handle;
+		return (NDIS_STATUS_SUCCESS);
+	} else {
+		return (NDIS_STATUS_FAILURE);
+	}
+}
+
+
+void RTMP_AllocateTxDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	UINT	Index,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress)
+{
+	POS_COOKIE pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	*VirtualAddress = (PVOID)PCI_ALLOC_CONSISTENT(pObj->pci_dev,sizeof(char)*Length, PhysicalAddress);
+
+}
+
+void RTMP_AllocateMgmtDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress)
+{
+	POS_COOKIE pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	*VirtualAddress = (PVOID)PCI_ALLOC_CONSISTENT(pObj->pci_dev,sizeof(char)*Length, PhysicalAddress);
+
+}
+
+void RTMP_AllocateRxDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress)
+{
+	POS_COOKIE pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	*VirtualAddress = (PVOID)PCI_ALLOC_CONSISTENT(pObj->pci_dev,sizeof(char)*Length, PhysicalAddress);
+
+}
+
+void RTMP_FreeRxDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	PVOID	VirtualAddress,
+	IN	NDIS_PHYSICAL_ADDRESS PhysicalAddress)
+{
+	POS_COOKIE pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	PCI_FREE_CONSISTENT(pObj->pci_dev, Length, VirtualAddress, PhysicalAddress);
+}
+
+
+void RTMP_AllocateFirstTxBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	UINT	Index,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress)
+{
+	POS_COOKIE pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	*VirtualAddress = (PVOID)PCI_ALLOC_CONSISTENT(pObj->pci_dev,sizeof(char)*Length, PhysicalAddress);
+}
+
+/*
+ * FUNCTION: Allocate a common buffer for DMA
+ * ARGUMENTS:
+ *     AdapterHandle:  AdapterHandle
+ *     Length:  Number of bytes to allocate
+ *     Cached:  Whether or not the memory can be cached
+ *     VirtualAddress:  Pointer to memory is returned here
+ *     PhysicalAddress:  Physical address corresponding to virtual address
+ */
+
+void RTMP_AllocateSharedMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress)
+{
+	POS_COOKIE pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	*VirtualAddress = (PVOID)PCI_ALLOC_CONSISTENT(pObj->pci_dev,sizeof(char)*Length, PhysicalAddress);
+}
+
+VOID RTMPFreeTxRxRingMemory(
+    IN  PRTMP_ADAPTER   pAd)
+{
+	int index, num , j;
+	PRTMP_TX_RING pTxRing;
+	PTXD_STRUC	  pTxD;
+	PNDIS_PACKET  pPacket;
+	unsigned int  IrqFlags;
+
+	POS_COOKIE pObj =(POS_COOKIE) pAd->OS_Cookie;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> RTMPFreeTxRxRingMemory\n"));
+
+	// Free TxSwQueue Packet
+	for (index=0; index <NUM_OF_TX_RING; index++)
+	{
+		PQUEUE_ENTRY pEntry;
+		PNDIS_PACKET pPacket;
+		PQUEUE_HEADER   pQueue;
+
+		RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
+		pQueue = &pAd->TxSwQueue[index];
+		while (pQueue->Head)
+		{
+			pEntry = RemoveHeadQueue(pQueue);
+			pPacket = QUEUE_ENTRY_TO_PACKET(pEntry);
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		}
+		RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+	}
+
+	// Free Tx Ring Packet
+	for (index=0;index< NUM_OF_TX_RING;index++)
+	{
+		pTxRing = &pAd->TxRing[index];
+
+		for (j=0; j< TX_RING_SIZE; j++)
+		{
+			pTxD = (PTXD_STRUC) (pTxRing->Cell[j].AllocVa);
+			pPacket = pTxRing->Cell[j].pNdisPacket;
+
+			if (pPacket)
+			{
+            	PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNdisPacket as NULL after clear
+			pTxRing->Cell[j].pNdisPacket = NULL;
+
+			pPacket = pTxRing->Cell[j].pNextNdisPacket;
+
+			if (pPacket)
+			{
+            	PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNextNdisPacket as NULL after clear
+			pTxRing->Cell[pTxRing->TxSwFreeIdx].pNextNdisPacket = NULL;
+
+		}
+	}
+
+	for (index = RX_RING_SIZE - 1 ; index >= 0; index--)
+	{
+		if ((pAd->RxRing.Cell[index].DmaBuf.AllocVa) && (pAd->RxRing.Cell[index].pNdisPacket))
+		{
+			PCI_UNMAP_SINGLE(pObj->pci_dev, pAd->RxRing.Cell[index].DmaBuf.AllocPa, pAd->RxRing.Cell[index].DmaBuf.AllocSize, PCI_DMA_FROMDEVICE);
+			RELEASE_NDIS_PACKET(pAd, pAd->RxRing.Cell[index].pNdisPacket, NDIS_STATUS_SUCCESS);
+		}
+	}
+	NdisZeroMemory(pAd->RxRing.Cell, RX_RING_SIZE * sizeof(RTMP_DMACB));
+
+	if (pAd->RxDescRing.AllocVa)
+    {
+    	PCI_FREE_CONSISTENT(pObj->pci_dev, pAd->RxDescRing.AllocSize, pAd->RxDescRing.AllocVa, pAd->RxDescRing.AllocPa);
+    }
+    NdisZeroMemory(&pAd->RxDescRing, sizeof(RTMP_DMABUF));
+
+	if (pAd->MgmtDescRing.AllocVa)
+	{
+		PCI_FREE_CONSISTENT(pObj->pci_dev, pAd->MgmtDescRing.AllocSize,	pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocPa);
+	}
+	NdisZeroMemory(&pAd->MgmtDescRing, sizeof(RTMP_DMABUF));
+
+	for (num = 0; num < NUM_OF_TX_RING; num++)
+	{
+    	if (pAd->TxBufSpace[num].AllocVa)
+	    {
+	    	PCI_FREE_CONSISTENT(pObj->pci_dev, pAd->TxBufSpace[num].AllocSize, pAd->TxBufSpace[num].AllocVa, pAd->TxBufSpace[num].AllocPa);
+	    }
+	    NdisZeroMemory(&pAd->TxBufSpace[num], sizeof(RTMP_DMABUF));
+
+    	if (pAd->TxDescRing[num].AllocVa)
+	    {
+	    	PCI_FREE_CONSISTENT(pObj->pci_dev, pAd->TxDescRing[num].AllocSize, pAd->TxDescRing[num].AllocVa, pAd->TxDescRing[num].AllocPa);
+	    }
+	    NdisZeroMemory(&pAd->TxDescRing[num], sizeof(RTMP_DMABUF));
+	}
+
+	if (pAd->FragFrame.pFragPacket)
+		RELEASE_NDIS_PACKET(pAd, pAd->FragFrame.pFragPacket, NDIS_STATUS_SUCCESS);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- RTMPFreeTxRxRingMemory\n"));
+}
+
+
+/*
+ * FUNCTION: Allocate a packet buffer for DMA
+ * ARGUMENTS:
+ *     AdapterHandle:  AdapterHandle
+ *     Length:  Number of bytes to allocate
+ *     Cached:  Whether or not the memory can be cached
+ *     VirtualAddress:  Pointer to memory is returned here
+ *     PhysicalAddress:  Physical address corresponding to virtual address
+ * Notes:
+ *     Cached is ignored: always cached memory
+ */
+PNDIS_PACKET RTMP_AllocateRxPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress)
+{
+	PNDIS_PACKET pkt;
+
+	pkt = RTPKT_TO_OSPKT(DEV_ALLOC_SKB(Length));
+
+	if (pkt == NULL) {
+		DBGPRINT(RT_DEBUG_ERROR, ("can't allocate rx %ld size packet\n",Length));
+	}
+
+	if (pkt) {
+		RTMP_SET_PACKET_SOURCE(pkt, PKTSRC_NDIS);
+		*VirtualAddress = (PVOID) RTPKT_TO_OSPKT(pkt)->data;
+		*PhysicalAddress = PCI_MAP_SINGLE(pAd, *VirtualAddress, Length, -1, PCI_DMA_FROMDEVICE);
+	} else {
+		*VirtualAddress = (PVOID) NULL;
+		*PhysicalAddress = (NDIS_PHYSICAL_ADDRESS) NULL;
+	}
+
+	return (PNDIS_PACKET) pkt;
+}
+
+
+VOID Invalid_Remaining_Packet(
+	IN	PRTMP_ADAPTER pAd,
+	IN	 ULONG VirtualAddress)
+{
+	NDIS_PHYSICAL_ADDRESS PhysicalAddress;
+
+	PhysicalAddress = PCI_MAP_SINGLE(pAd, (void *)(VirtualAddress+1600), RX_BUFFER_NORMSIZE-1600, -1, PCI_DMA_FROMDEVICE);
+}
+
+PNDIS_PACKET GetPacketFromRxRing(
+	IN		PRTMP_ADAPTER	pAd,
+	OUT		PRT28XX_RXD_STRUC	pSaveRxD,
+	OUT		BOOLEAN			*pbReschedule,
+	IN OUT	UINT32			*pRxPending)
+{
+	PRXD_STRUC				pRxD;
+#ifdef RT_BIG_ENDIAN
+	PRXD_STRUC				pDestRxD;
+	RXD_STRUC				RxD;
+#endif
+	PNDIS_PACKET			pRxPacket = NULL;
+	PNDIS_PACKET			pNewPacket;
+	PVOID					AllocVa;
+	NDIS_PHYSICAL_ADDRESS	AllocPa;
+	BOOLEAN					bReschedule = FALSE;
+
+	RTMP_SEM_LOCK(&pAd->RxRingLock);
+
+	if (*pRxPending == 0)
+	{
+		// Get how may packets had been received
+		RTMP_IO_READ32(pAd, RX_DRX_IDX , &pAd->RxRing.RxDmaIdx);
+
+		if (pAd->RxRing.RxSwReadIdx == pAd->RxRing.RxDmaIdx)
+		{
+			// no more rx packets
+			bReschedule = FALSE;
+			goto done;
+		}
+
+		// get rx pending count
+		if (pAd->RxRing.RxDmaIdx > pAd->RxRing.RxSwReadIdx)
+			*pRxPending = pAd->RxRing.RxDmaIdx - pAd->RxRing.RxSwReadIdx;
+		else
+			*pRxPending	= pAd->RxRing.RxDmaIdx + RX_RING_SIZE - pAd->RxRing.RxSwReadIdx;
+
+	}
+
+#ifdef RT_BIG_ENDIAN
+	pDestRxD = (PRXD_STRUC) pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].AllocVa;
+	RxD = *pDestRxD;
+	pRxD = &RxD;
+	RTMPDescriptorEndianChange((PUCHAR)pRxD, TYPE_RXD);
+#else
+	// Point to Rx indexed rx ring descriptor
+	pRxD = (PRXD_STRUC) pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].AllocVa;
+#endif
+
+	if (pRxD->DDONE == 0)
+	{
+		*pRxPending = 0;
+		// DMAIndx had done but DDONE bit not ready
+		bReschedule = TRUE;
+		goto done;
+	}
+
+
+	// return rx descriptor
+	NdisMoveMemory(pSaveRxD, pRxD, RXD_SIZE);
+
+	pNewPacket = RTMP_AllocateRxPacketBuffer(pAd, RX_BUFFER_AGGRESIZE, FALSE, &AllocVa, &AllocPa);
+
+	if (pNewPacket)
+	{
+		// unmap the rx buffer
+		PCI_UNMAP_SINGLE(pAd, pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].DmaBuf.AllocPa,
+					 pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].DmaBuf.AllocSize, PCI_DMA_FROMDEVICE);
+		pRxPacket = pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].pNdisPacket;
+
+		pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].DmaBuf.AllocSize	= RX_BUFFER_AGGRESIZE;
+		pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].pNdisPacket		= (PNDIS_PACKET) pNewPacket;
+		pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].DmaBuf.AllocVa	= AllocVa;
+		pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].DmaBuf.AllocPa	= AllocPa;
+		/* update SDP0 to new buffer of rx packet */
+		pRxD->SDP0 = AllocPa;
+	}
+	else
+	{
+		//printk("No Rx Buffer\n");
+		pRxPacket = NULL;
+		bReschedule = TRUE;
+	}
+
+	pRxD->DDONE = 0;
+
+	// had handled one rx packet
+	*pRxPending = *pRxPending - 1;
+
+	// update rx descriptor and kick rx
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pRxD, TYPE_RXD);
+	WriteBackToDescriptor((PUCHAR)pDestRxD, (PUCHAR)pRxD, FALSE, TYPE_RXD);
+#endif
+	INC_RING_INDEX(pAd->RxRing.RxSwReadIdx, RX_RING_SIZE);
+
+	pAd->RxRing.RxCpuIdx = (pAd->RxRing.RxSwReadIdx == 0) ? (RX_RING_SIZE-1) : (pAd->RxRing.RxSwReadIdx-1);
+	RTMP_IO_WRITE32(pAd, RX_CRX_IDX, pAd->RxRing.RxCpuIdx);
+
+done:
+	RTMP_SEM_UNLOCK(&pAd->RxRingLock);
+	*pbReschedule = bReschedule;
+	return pRxPacket;
+}
+/* End of 2860_rtmp_init.c */
+
diff --git a/drivers/staging/rt2860/common/action.c b/drivers/staging/rt2860/common/action.c
new file mode 100644
index 000000000000..d6f530fb857f
--- /dev/null
+++ b/drivers/staging/rt2860/common/action.c
@@ -0,0 +1,1031 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+	action.c
+
+    Abstract:
+    Handle association related requests either from WSTA or from local MLME
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+	Jan Lee		2006	  	created for rt2860
+ */
+
+#include "../rt_config.h"
+#include "action.h"
+
+
+static VOID ReservedAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+/*
+    ==========================================================================
+    Description:
+        association state machine init, including state transition and timer init
+    Parameters:
+        S - pointer to the association state machine
+    Note:
+        The state machine looks like the following
+
+                                    ASSOC_IDLE
+        MT2_MLME_DISASSOC_REQ    mlme_disassoc_req_action
+        MT2_PEER_DISASSOC_REQ    peer_disassoc_action
+        MT2_PEER_ASSOC_REQ       drop
+        MT2_PEER_REASSOC_REQ     drop
+        MT2_CLS3ERR              cls3err_action
+    ==========================================================================
+ */
+VOID ActionStateMachineInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  STATE_MACHINE *S,
+    OUT STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(S, (STATE_MACHINE_FUNC *)Trans, MAX_ACT_STATE, MAX_ACT_MSG, (STATE_MACHINE_FUNC)Drop, ACT_IDLE, ACT_MACHINE_BASE);
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_SPECTRUM_CATE, (STATE_MACHINE_FUNC)PeerSpectrumAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_QOS_CATE, (STATE_MACHINE_FUNC)PeerQOSAction);
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_DLS_CATE, (STATE_MACHINE_FUNC)ReservedAction);
+#ifdef QOS_DLS_SUPPORT
+		StateMachineSetAction(S, ACT_IDLE, MT2_PEER_DLS_CATE, (STATE_MACHINE_FUNC)PeerDLSAction);
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_BA_CATE, (STATE_MACHINE_FUNC)PeerBAAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_HT_CATE, (STATE_MACHINE_FUNC)PeerHTAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_ADD_BA_CATE, (STATE_MACHINE_FUNC)MlmeADDBAAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_ORI_DELBA_CATE, (STATE_MACHINE_FUNC)MlmeDELBAAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_REC_DELBA_CATE, (STATE_MACHINE_FUNC)MlmeDELBAAction);
+#endif // DOT11_N_SUPPORT //
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_PUBLIC_CATE, (STATE_MACHINE_FUNC)PeerPublicAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_RM_CATE, (STATE_MACHINE_FUNC)PeerRMAction);
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_QOS_CATE, (STATE_MACHINE_FUNC)MlmeQOSAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_DLS_CATE, (STATE_MACHINE_FUNC)MlmeDLSAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_ACT_INVALID, (STATE_MACHINE_FUNC)MlmeInvalidAction);
+}
+
+#ifdef DOT11_N_SUPPORT
+VOID MlmeADDBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+
+{
+	MLME_ADDBA_REQ_STRUCT *pInfo;
+	UCHAR           Addr[6];
+	PUCHAR         pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG		Idx;
+	FRAME_ADDBA_REQ  Frame;
+	ULONG		FrameLen;
+	BA_ORI_ENTRY			*pBAEntry = NULL;
+
+	pInfo = (MLME_ADDBA_REQ_STRUCT *)Elem->Msg;
+	NdisZeroMemory(&Frame, sizeof(FRAME_ADDBA_REQ));
+
+	if(MlmeAddBAReqSanity(pAd, Elem->Msg, Elem->MsgLen, Addr))
+	{
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - MlmeADDBAAction() allocate memory failed \n"));
+			return;
+		}
+		// 1. find entry
+		Idx = pAd->MacTab.Content[pInfo->Wcid].BAOriWcidArray[pInfo->TID];
+		if (Idx == 0)
+		{
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeADDBAAction() can't find BAOriEntry \n"));
+			return;
+		}
+		else
+		{
+			pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (ADHOC_ON(pAd))
+				ActHeaderInit(pAd, &Frame.Hdr, pInfo->pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#ifdef QOS_DLS_SUPPORT
+			if (pAd->MacTab.Content[pInfo->Wcid].ValidAsDls)
+				ActHeaderInit(pAd, &Frame.Hdr, pInfo->pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#endif // QOS_DLS_SUPPORT //
+				ActHeaderInit(pAd, &Frame.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pInfo->pAddr);
+
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		Frame.Category = CATEGORY_BA;
+		Frame.Action = ADDBA_REQ;
+		Frame.BaParm.AMSDUSupported = 0;
+		Frame.BaParm.BAPolicy = IMMED_BA;
+		Frame.BaParm.TID = pInfo->TID;
+		Frame.BaParm.BufSize = pInfo->BaBufSize;
+		Frame.Token = pInfo->Token;
+		Frame.TimeOutValue = pInfo->TimeOutValue;
+		Frame.BaStartSeq.field.FragNum = 0;
+		Frame.BaStartSeq.field.StartSeq = pAd->MacTab.Content[pInfo->Wcid].TxSeq[pInfo->TID];
+
+		*(USHORT *)(&Frame.BaParm) = cpu2le16(*(USHORT *)(&Frame.BaParm));
+		Frame.TimeOutValue = cpu2le16(Frame.TimeOutValue);
+		Frame.BaStartSeq.word = cpu2le16(Frame.BaStartSeq.word);
+
+		MakeOutgoingFrame(pOutBuffer,		   &FrameLen,
+		              sizeof(FRAME_ADDBA_REQ), &Frame,
+		              END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("BA - Send ADDBA request. StartSeq = %x,  FrameLen = %ld. BufSize = %d\n", Frame.BaStartSeq.field.StartSeq, FrameLen, Frame.BaParm.BufSize));
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+        send DELBA and delete BaEntry if any
+    Parametrs:
+        Elem - MLME message MLME_DELBA_REQ_STRUCT
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDELBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_DELBA_REQ_STRUCT *pInfo;
+	PUCHAR         pOutBuffer = NULL;
+	PUCHAR		   pOutBuffer2 = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG		Idx;
+	FRAME_DELBA_REQ  Frame;
+	ULONG		FrameLen;
+	FRAME_BAR	FrameBar;
+
+	pInfo = (MLME_DELBA_REQ_STRUCT *)Elem->Msg;
+	// must send back DELBA
+	NdisZeroMemory(&Frame, sizeof(FRAME_DELBA_REQ));
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeDELBAAction(), Initiator(%d) \n", pInfo->Initiator));
+
+	if(MlmeDelBAReqSanity(pAd, Elem->Msg, Elem->MsgLen))
+	{
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeDELBAAction() allocate memory failed 1. \n"));
+			return;
+		}
+
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer2);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_ERROR, ("BA - MlmeDELBAAction() allocate memory failed 2. \n"));
+			return;
+		}
+
+		// SEND BAR (Send BAR to refresh peer reordering buffer.)
+		Idx = pAd->MacTab.Content[pInfo->Wcid].BAOriWcidArray[pInfo->TID];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			BarHeaderInit(pAd, &FrameBar, pAd->MacTab.Content[pInfo->Wcid].Addr, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+
+		FrameBar.StartingSeq.field.FragNum = 0; // make sure sequence not clear in DEL funciton.
+		FrameBar.StartingSeq.field.StartSeq = pAd->MacTab.Content[pInfo->Wcid].TxSeq[pInfo->TID]; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.TID = pInfo->TID; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.ACKPolicy = IMMED_BA; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.Compressed = 1; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.MTID = 0; // make sure sequence not clear in DEL funciton.
+
+		MakeOutgoingFrame(pOutBuffer2,				&FrameLen,
+					  sizeof(FRAME_BAR),	  &FrameBar,
+					  END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer2, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer2);
+		DBGPRINT(RT_DEBUG_TRACE,("BA - MlmeDELBAAction() . Send BAR to refresh peer reordering buffer \n"));
+
+		// SEND DELBA FRAME
+		FrameLen = 0;
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (ADHOC_ON(pAd))
+				ActHeaderInit(pAd, &Frame.Hdr, pAd->MacTab.Content[pInfo->Wcid].Addr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#ifdef QOS_DLS_SUPPORT
+			if (pAd->MacTab.Content[pInfo->Wcid].ValidAsDls)
+				ActHeaderInit(pAd, &Frame.Hdr, pAd->MacTab.Content[pInfo->Wcid].Addr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#endif // QOS_DLS_SUPPORT //
+				ActHeaderInit(pAd, &Frame.Hdr,  pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->MacTab.Content[pInfo->Wcid].Addr);
+		}
+#endif // CONFIG_STA_SUPPORT //
+		Frame.Category = CATEGORY_BA;
+		Frame.Action = DELBA;
+		Frame.DelbaParm.Initiator = pInfo->Initiator;
+		Frame.DelbaParm.TID = pInfo->TID;
+		Frame.ReasonCode = 39; // Time Out
+		*(USHORT *)(&Frame.DelbaParm) = cpu2le16(*(USHORT *)(&Frame.DelbaParm));
+		Frame.ReasonCode = cpu2le16(Frame.ReasonCode);
+
+		MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+		              sizeof(FRAME_DELBA_REQ),    &Frame,
+		              END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+		DBGPRINT(RT_DEBUG_TRACE, ("BA - MlmeDELBAAction() . 3 DELBA sent. Initiator(%d)\n", pInfo->Initiator));
+    	}
+}
+#endif // DOT11_N_SUPPORT //
+
+VOID MlmeQOSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+VOID MlmeDLSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+VOID MlmeInvalidAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	//PUCHAR		   pOutBuffer = NULL;
+	//Return the receiving frame except the MSB of category filed set to 1.  7.3.1.11
+}
+
+VOID PeerQOSAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+#ifdef QOS_DLS_SUPPORT
+VOID PeerDLSAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	switch(Action)
+	{
+		case ACTION_DLS_REQUEST:
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			PeerDlsReqAction(pAd, Elem);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+
+		case ACTION_DLS_RESPONSE:
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			PeerDlsRspAction(pAd, Elem);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+
+		case ACTION_DLS_TEARDOWN:
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			PeerDlsTearDownAction(pAd, Elem);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+	}
+}
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+VOID PeerBAAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	switch(Action)
+	{
+		case ADDBA_REQ:
+			PeerAddBAReqAction(pAd,Elem);
+			break;
+		case ADDBA_RESP:
+			PeerAddBARspAction(pAd,Elem);
+			break;
+		case DELBA:
+			PeerDelBAAction(pAd,Elem);
+			break;
+	}
+}
+
+
+#ifdef DOT11N_DRAFT3
+
+#ifdef CONFIG_STA_SUPPORT
+VOID StaPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Bss2040Coexist)
+{
+	BSS_2040_COEXIST_IE		BssCoexist;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+
+	BssCoexist.word = Bss2040Coexist;
+	// AP asks Station to return a 20/40 BSS Coexistence mgmt frame.  So we first starts a scan, then send back 20/40 BSS Coexistence mgmt frame
+	if ((BssCoexist.field.InfoReq == 1) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SCAN_2040)))
+	{
+		// Clear record first.  After scan , will update those bit and send back to transmiter.
+		pAd->CommonCfg.BSSCoexist2040.field.InfoReq = 1;
+		pAd->CommonCfg.BSSCoexist2040.field.Intolerant40 = 0;
+		pAd->CommonCfg.BSSCoexist2040.field.BSS20WidthReq = 0;
+		// Fill out stuff for scan request
+		ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_2040_BSS_COEXIST);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+	}
+}
+
+
+/*
+Description : Build Intolerant Channel Rerpot from Trigger event table.
+return : how many bytes copied.
+*/
+ULONG BuildIntolerantChannelRep(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    PUCHAR  pDest)
+{
+	ULONG			FrameLen = 0;
+	ULONG			ReadOffset = 0;
+	UCHAR			i;
+	UCHAR			LastRegClass = 0xff;
+	PUCHAR			pLen;
+
+	for ( i = 0;i < MAX_TRIGGER_EVENT;i++)
+	{
+		if (pAd->CommonCfg.TriggerEventTab.EventA[i].bValid == TRUE)
+		{
+			if (pAd->CommonCfg.TriggerEventTab.EventA[i].RegClass == LastRegClass)
+			{
+				*(pDest + ReadOffset) = (UCHAR)pAd->CommonCfg.TriggerEventTab.EventA[i].Channel;
+				*pLen++;
+				ReadOffset++;
+				FrameLen++;
+			}
+			else
+			{
+				*(pDest + ReadOffset) = IE_2040_BSS_INTOLERANT_REPORT;  // IE
+				*(pDest + ReadOffset + 1) = 2;	// Len = RegClass byte + channel byte.
+				pLen = pDest + ReadOffset + 1;
+				LastRegClass = pAd->CommonCfg.TriggerEventTab.EventA[i].RegClass;
+				*(pDest + ReadOffset + 2) = LastRegClass;	// Len = RegClass byte + channel byte.
+				*(pDest + ReadOffset + 3) = (UCHAR)pAd->CommonCfg.TriggerEventTab.EventA[i].Channel;
+				FrameLen += 4;
+				ReadOffset += 4;
+			}
+
+		}
+	}
+	return FrameLen;
+}
+
+
+/*
+Description : Send 20/40 BSS Coexistence Action frame If one trigger event is triggered.
+*/
+VOID Send2040CoexistAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS 	NStatus;
+	FRAME_ACTION_HDR	Frame;
+	ULONG			FrameLen;
+	ULONG			IntolerantChaRepLen;
+
+	IntolerantChaRepLen = 0;
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("ACT - Send2040CoexistAction() allocate memory failed \n"));
+		return;
+	}
+	ActHeaderInit(pAd, &Frame.Hdr, pAd->MacTab.Content[Wcid].Addr, pAd->CommonCfg.Bssid);
+	Frame.Category = CATEGORY_PUBLIC;
+	Frame.Action = ACTION_BSS_2040_COEXIST;
+
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+				  sizeof(FRAME_ACTION_HDR),	  &Frame,
+				  END_OF_ARGS);
+
+	*(pOutBuffer + FrameLen) = pAd->CommonCfg.BSSCoexist2040.word;
+	FrameLen++;
+
+	if (bAddIntolerantCha == TRUE)
+		IntolerantChaRepLen = BuildIntolerantChannelRep(pAd, pOutBuffer + FrameLen);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen + IntolerantChaRepLen);
+	DBGPRINT(RT_DEBUG_ERROR,("ACT - Send2040CoexistAction( BSSCoexist2040 = 0x%x )  \n", pAd->CommonCfg.BSSCoexist2040.word));
+
+}
+
+
+/*
+	==========================================================================
+	Description:
+	After scan, Update 20/40 BSS Coexistence IE and send out.
+	According to 802.11n D3.03 11.14.10
+
+	Parameters:
+	==========================================================================
+ */
+VOID Update2040CoexistFrameAndNotify(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha)
+{
+	BSS_2040_COEXIST_IE	OldValue;
+
+	OldValue.word = pAd->CommonCfg.BSSCoexist2040.word;
+	if ((pAd->CommonCfg.TriggerEventTab.EventANo > 0) || (pAd->CommonCfg.TriggerEventTab.EventBCountDown > 0))
+		pAd->CommonCfg.BSSCoexist2040.field.BSS20WidthReq = 1;
+
+	// Need to check !!!!
+	// How STA will set Intolerant40 if implementation dependent. Now we don't set this bit first.!!!!!
+	// So Only check BSS20WidthReq change.
+	if (OldValue.field.BSS20WidthReq != pAd->CommonCfg.BSSCoexist2040.field.BSS20WidthReq)
+	{
+		Send2040CoexistAction(pAd, Wcid, bAddIntolerantCha);
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+BOOLEAN ChannelSwitchSanityCheck(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  NewChannel,
+	IN    UCHAR  Secondary)
+{
+	UCHAR		i;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	if ((NewChannel > 7) && (Secondary == 1))
+		return FALSE;
+
+	if ((NewChannel < 5) && (Secondary == 3))
+		return FALSE;
+
+	// 0. Check if new channel is in the channellist.
+	for (i = 0;i < pAd->ChannelListNum;i++)
+	{
+		if (pAd->ChannelList[i].Channel == NewChannel)
+		{
+			break;
+		}
+	}
+
+	if (i == pAd->ChannelListNum)
+		return FALSE;
+
+	return TRUE;
+}
+
+
+VOID ChannelSwitchAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  NewChannel,
+	IN    UCHAR  Secondary)
+{
+	UCHAR		BBPValue = 0;
+	ULONG		MACValue;
+
+	DBGPRINT(RT_DEBUG_TRACE,("SPECTRUM - ChannelSwitchAction(NewChannel = %d , Secondary = %d)  \n", NewChannel, Secondary));
+
+	if (ChannelSwitchSanityCheck(pAd, Wcid, NewChannel, Secondary) == FALSE)
+		return;
+
+	// 1.  Switches to BW = 20.
+	if (Secondary == 0)
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+		BBPValue&= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x16);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x08);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x11);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+		pAd->CommonCfg.BBPCurrentBW = BW_20;
+		pAd->CommonCfg.Channel = NewChannel;
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel,FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+		pAd->MacTab.Content[Wcid].HTPhyMode.field.BW = 0;
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!20MHz   !!! \n" ));
+	}
+	// 1.  Switches to BW = 40 And Station supports BW = 40.
+	else if (((Secondary == 1) || (Secondary == 3)) && (pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth == 1))
+	{
+		pAd->CommonCfg.Channel = NewChannel;
+
+		if (Secondary == 1)
+		{
+			// Secondary above.
+			pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel + 2;
+			RTMP_IO_READ32(pAd, TX_BAND_CFG, &MACValue);
+			MACValue &= 0xfe;
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, MACValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue&= (~0x18);
+			BBPValue|= (0x10);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPValue);
+			BBPValue&= (~0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPValue);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Lower LINK UP !!! Control Channel at Below. Central = %d \n", pAd->CommonCfg.CentralChannel ));
+		}
+		else
+		{
+			// Secondary below.
+			pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel - 2;
+			RTMP_IO_READ32(pAd, TX_BAND_CFG, &MACValue);
+			MACValue &= 0xfe;
+			MACValue |= 0x1;
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, MACValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue&= (~0x18);
+			BBPValue|= (0x10);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPValue);
+			BBPValue&= (~0x20);
+			BBPValue|= (0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPValue);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Upper LINK UP !!! Control Channel at UpperCentral = %d \n", pAd->CommonCfg.CentralChannel ));
+		}
+		pAd->CommonCfg.BBPCurrentBW = BW_40;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+		pAd->MacTab.Content[Wcid].HTPhyMode.field.BW = 1;
+	}
+}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+VOID PeerPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+#ifdef DOT11N_DRAFT3
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+#endif // DOT11N_DRAFT3 //
+
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+#ifdef DOT11N_DRAFT3
+	switch(Action)
+	{
+		case ACTION_BSS_2040_COEXIST:	// Format defined in IEEE 7.4.7a.1 in 11n Draf3.03
+			{
+				//UCHAR	BssCoexist;
+				BSS_2040_COEXIST_ELEMENT		*pCoexistInfo;
+				BSS_2040_COEXIST_IE 			*pBssCoexistIe;
+				BSS_2040_INTOLERANT_CH_REPORT	*pIntolerantReport = NULL;
+
+				if (Elem->MsgLen <= (LENGTH_802_11 + sizeof(BSS_2040_COEXIST_ELEMENT)) )
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("ACTION - 20/40 BSS Coexistence Management Frame length too short! len = %ld!\n", Elem->MsgLen));
+					break;
+				}
+				DBGPRINT(RT_DEBUG_TRACE, ("ACTION - 20/40 BSS Coexistence Management action----> \n"));
+				hex_dump("CoexistenceMgmtFrame", Elem->Msg, Elem->MsgLen);
+
+
+				pCoexistInfo = (BSS_2040_COEXIST_ELEMENT *) &Elem->Msg[LENGTH_802_11+2];
+				//hex_dump("CoexistInfo", (PUCHAR)pCoexistInfo, sizeof(BSS_2040_COEXIST_ELEMENT));
+				if (Elem->MsgLen >= (LENGTH_802_11 + sizeof(BSS_2040_COEXIST_ELEMENT) + sizeof(BSS_2040_INTOLERANT_CH_REPORT)))
+				{
+					pIntolerantReport = (BSS_2040_INTOLERANT_CH_REPORT *)((PUCHAR)pCoexistInfo + sizeof(BSS_2040_COEXIST_ELEMENT));
+				}
+				//hex_dump("IntolerantReport ", (PUCHAR)pIntolerantReport, sizeof(BSS_2040_INTOLERANT_CH_REPORT));
+
+				pBssCoexistIe = (BSS_2040_COEXIST_IE *)(&pCoexistInfo->BssCoexistIe);
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					if (INFRA_ON(pAd))
+					{
+						StaPublicAction(pAd, pCoexistInfo);
+					}
+				}
+#endif // CONFIG_STA_SUPPORT //
+
+			}
+			break;
+	}
+
+#endif // DOT11N_DRAFT3 //
+
+}
+
+
+static VOID ReservedAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR Category;
+
+	if (Elem->MsgLen <= LENGTH_802_11)
+	{
+		return;
+	}
+
+	Category = Elem->Msg[LENGTH_802_11];
+	DBGPRINT(RT_DEBUG_TRACE,("Rcv reserved category(%d) Action Frame\n", Category));
+	hex_dump("Reserved Action Frame", &Elem->Msg[0], Elem->MsgLen);
+}
+
+VOID PeerRMAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	return;
+}
+
+#ifdef DOT11_N_SUPPORT
+static VOID respond_ht_information_exchange_action(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen;
+	FRAME_HT_INFO	HTINFOframe, *pFrame;
+	UCHAR   		*pAddr;
+
+
+	// 2. Always send back ADDBA Response
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);	 //Get an unused nonpaged memory
+
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ACTION - respond_ht_information_exchange_action() allocate memory failed \n"));
+		return;
+	}
+
+	// get RA
+	pFrame = (FRAME_HT_INFO *) &Elem->Msg[0];
+	pAddr = pFrame->Hdr.Addr2;
+
+	NdisZeroMemory(&HTINFOframe, sizeof(FRAME_HT_INFO));
+	// 2-1. Prepare ADDBA Response frame.
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (ADHOC_ON(pAd))
+			ActHeaderInit(pAd, &HTINFOframe.Hdr, pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+		else
+			ActHeaderInit(pAd, &HTINFOframe.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAddr);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	HTINFOframe.Category = CATEGORY_HT;
+	HTINFOframe.Action = HT_INFO_EXCHANGE;
+	HTINFOframe.HT_Info.Request = 0;
+	HTINFOframe.HT_Info.Forty_MHz_Intolerant = pAd->CommonCfg.HtCapability.HtCapInfo.Forty_Mhz_Intolerant;
+	HTINFOframe.HT_Info.STA_Channel_Width	 = pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth;
+
+	MakeOutgoingFrame(pOutBuffer,					&FrameLen,
+					  sizeof(FRAME_HT_INFO),	&HTINFOframe,
+					  END_OF_ARGS);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+
+#ifdef DOT11N_DRAFT3
+VOID SendNotifyBWActionFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR  Wcid,
+	IN UCHAR apidx)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS 	NStatus;
+	FRAME_ACTION_HDR	Frame;
+	ULONG			FrameLen;
+	PUCHAR			pAddr1;
+
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("ACT - SendNotifyBWAction() allocate memory failed \n"));
+		return;
+	}
+
+	if (Wcid == MCAST_WCID)
+		pAddr1 = &BROADCAST_ADDR[0];
+	else
+		pAddr1 = pAd->MacTab.Content[Wcid].Addr;
+	ActHeaderInit(pAd, &Frame.Hdr, pAddr1, pAd->ApCfg.MBSSID[apidx].Bssid, pAd->ApCfg.MBSSID[apidx].Bssid);
+
+	Frame.Category = CATEGORY_HT;
+	Frame.Action = NOTIFY_BW_ACTION;
+
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+				  sizeof(FRAME_ACTION_HDR),	  &Frame,
+				  END_OF_ARGS);
+
+	*(pOutBuffer + FrameLen) = pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth;
+	FrameLen++;
+
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	DBGPRINT(RT_DEBUG_TRACE,("ACT - SendNotifyBWAction(NotifyBW= %d)!\n", pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth));
+
+}
+#endif // DOT11N_DRAFT3 //
+
+
+VOID PeerHTAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+	switch(Action)
+	{
+		case NOTIFY_BW_ACTION:
+			DBGPRINT(RT_DEBUG_TRACE,("ACTION - HT Notify Channel bandwidth action----> \n"));
+#ifdef CONFIG_STA_SUPPORT
+			if(pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE)
+			{
+				// Note, this is to patch DIR-1353 AP. When the AP set to Wep, it will use legacy mode. But AP still keeps
+				// sending BW_Notify Action frame, and cause us to linkup and linkdown.
+				// In legacy mode, don't need to parse HT action frame.
+				DBGPRINT(RT_DEBUG_TRACE,("ACTION -Ignore HT Notify Channel BW when link as legacy mode. BW = %d---> \n",
+								Elem->Msg[LENGTH_802_11+2] ));
+				break;
+			}
+#endif // CONFIG_STA_SUPPORT //
+
+			if (Elem->Msg[LENGTH_802_11+2] == 0)	// 7.4.8.2. if value is 1, keep the same as supported channel bandwidth.
+				pAd->MacTab.Content[Elem->Wcid].HTPhyMode.field.BW = 0;
+
+			break;
+		case SMPS_ACTION:
+			// 7.3.1.25
+ 			DBGPRINT(RT_DEBUG_TRACE,("ACTION - SMPS action----> \n"));
+			if (((Elem->Msg[LENGTH_802_11+2]&0x1) == 0))
+			{
+				pAd->MacTab.Content[Elem->Wcid].MmpsMode = MMPS_ENABLE;
+			}
+			else if (((Elem->Msg[LENGTH_802_11+2]&0x2) == 0))
+			{
+				pAd->MacTab.Content[Elem->Wcid].MmpsMode = MMPS_STATIC;
+			}
+			else
+			{
+				pAd->MacTab.Content[Elem->Wcid].MmpsMode = MMPS_DYNAMIC;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,("Aid(%d) MIMO PS = %d\n", Elem->Wcid, pAd->MacTab.Content[Elem->Wcid].MmpsMode));
+			// rt2860c : add something for smps change.
+			break;
+
+		case SETPCO_ACTION:
+			break;
+		case MIMO_CHA_MEASURE_ACTION:
+			break;
+		case HT_INFO_EXCHANGE:
+			{
+				HT_INFORMATION_OCTET	*pHT_info;
+
+				pHT_info = (HT_INFORMATION_OCTET *) &Elem->Msg[LENGTH_802_11+2];
+    				// 7.4.8.10
+    				DBGPRINT(RT_DEBUG_TRACE,("ACTION - HT Information Exchange action----> \n"));
+    				if (pHT_info->Request)
+    				{
+    					respond_ht_information_exchange_action(pAd, Elem);
+    				}
+			}
+    			break;
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Retry sending ADDBA Reqest.
+
+	IRQL = DISPATCH_LEVEL
+
+	Parametrs:
+	p8023Header: if this is already 802.3 format, p8023Header is NULL
+
+	Return	: TRUE if put into rx reordering buffer, shouldn't indicaterxhere.
+				FALSE , then continue indicaterx at this moment.
+	==========================================================================
+ */
+VOID ORIBATimerTimeout(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	MAC_TABLE_ENTRY	*pEntry;
+	INT			i, total;
+	UCHAR			TID;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	total = pAd->MacTab.Size * NUM_OF_TID;
+
+	for (i = 1; ((i <MAX_LEN_OF_BA_ORI_TABLE) && (total > 0)) ; i++)
+	{
+		if  (pAd->BATable.BAOriEntry[i].ORI_BA_Status == Originator_Done)
+		{
+			pEntry = &pAd->MacTab.Content[pAd->BATable.BAOriEntry[i].Wcid];
+			TID = pAd->BATable.BAOriEntry[i].TID;
+
+			ASSERT(pAd->BATable.BAOriEntry[i].Wcid < MAX_LEN_OF_MAC_TABLE);
+		}
+		total --;
+	}
+}
+
+
+VOID SendRefreshBAR(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry)
+{
+	FRAME_BAR		FrameBar;
+	ULONG			FrameLen;
+	NDIS_STATUS 	NStatus;
+	PUCHAR			pOutBuffer = NULL;
+	USHORT			Sequence;
+	UCHAR			i, TID;
+	USHORT			idx;
+	BA_ORI_ENTRY	*pBAEntry;
+
+	for (i = 0; i <NUM_OF_TID; i++)
+	{
+		idx = pEntry->BAOriWcidArray[i];
+		if (idx == 0)
+		{
+			continue;
+		}
+		pBAEntry = &pAd->BATable.BAOriEntry[idx];
+
+		if  (pBAEntry->ORI_BA_Status == Originator_Done)
+		{
+			TID = pBAEntry->TID;
+
+			ASSERT(pBAEntry->Wcid < MAX_LEN_OF_MAC_TABLE);
+
+			NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+			if(NStatus != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeADDBAAction() allocate memory failed \n"));
+				return;
+			}
+
+			Sequence = pEntry->TxSeq[TID];
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				BarHeaderInit(pAd, &FrameBar, pEntry->Addr, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+
+			FrameBar.StartingSeq.field.FragNum = 0; // make sure sequence not clear in DEL function.
+			FrameBar.StartingSeq.field.StartSeq = Sequence; // make sure sequence not clear in DEL funciton.
+			FrameBar.BarControl.TID = TID; // make sure sequence not clear in DEL funciton.
+
+			MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+							  sizeof(FRAME_BAR),	  &FrameBar,
+							  END_OF_ARGS);
+			if (1)	// Now we always send BAR.
+			{
+				MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			}
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+VOID ActHeaderInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN OUT PHEADER_802_11 pHdr80211,
+    IN PUCHAR Addr1,
+    IN PUCHAR Addr2,
+    IN PUCHAR Addr3)
+{
+    NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+    pHdr80211->FC.Type = BTYPE_MGMT;
+    pHdr80211->FC.SubType = SUBTYPE_ACTION;
+
+    COPY_MAC_ADDR(pHdr80211->Addr1, Addr1);
+	COPY_MAC_ADDR(pHdr80211->Addr2, Addr2);
+    COPY_MAC_ADDR(pHdr80211->Addr3, Addr3);
+}
+
+VOID BarHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PFRAME_BAR pCntlBar,
+	IN PUCHAR pDA,
+	IN PUCHAR pSA)
+{
+	NdisZeroMemory(pCntlBar, sizeof(FRAME_BAR));
+	pCntlBar->FC.Type = BTYPE_CNTL;
+	pCntlBar->FC.SubType = SUBTYPE_BLOCK_ACK_REQ;
+   	pCntlBar->BarControl.MTID = 0;
+	pCntlBar->BarControl.Compressed = 1;
+	pCntlBar->BarControl.ACKPolicy = 0;
+
+
+	pCntlBar->Duration = 16 + RTMPCalcDuration(pAd, RATE_1, sizeof(FRAME_BA));
+
+	COPY_MAC_ADDR(pCntlBar->Addr1, pDA);
+	COPY_MAC_ADDR(pCntlBar->Addr2, pSA);
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Insert Category and action code into the action frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. category code of the frame.
+		4. action code of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID InsertActField(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 Category,
+	IN UINT8 ActCode)
+{
+	ULONG TempLen;
+
+	MakeOutgoingFrame(	pFrameBuf,		&TempLen,
+						1,				&Category,
+						1,				&ActCode,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
diff --git a/drivers/staging/rt2860/common/action.h b/drivers/staging/rt2860/common/action.h
new file mode 100644
index 000000000000..ce3877dce81b
--- /dev/null
+++ b/drivers/staging/rt2860/common/action.h
@@ -0,0 +1,68 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	aironet.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	Paul Lin	04-06-15		Initial
+*/
+
+#ifndef	__ACTION_H__
+#define	__ACTION_H__
+
+typedef struct PACKED __HT_INFO_OCTET
+{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	Reserved:5;
+	UCHAR 	STA_Channel_Width:1;
+	UCHAR	Forty_MHz_Intolerant:1;
+	UCHAR	Request:1;
+#else
+	UCHAR	Request:1;
+	UCHAR	Forty_MHz_Intolerant:1;
+	UCHAR 	STA_Channel_Width:1;
+	UCHAR	Reserved:5;
+#endif
+} HT_INFORMATION_OCTET;
+
+
+typedef struct PACKED __FRAME_HT_INFO
+{
+	HEADER_802_11   		Hdr;
+	UCHAR					Category;
+	UCHAR					Action;
+	HT_INFORMATION_OCTET	HT_Info;
+}   FRAME_HT_INFO, *PFRAME_HT_INFO;
+
+#endif /* __ACTION_H__ */
+
+
diff --git a/drivers/staging/rt2860/common/ba_action.c b/drivers/staging/rt2860/common/ba_action.c
new file mode 100644
index 000000000000..591d1e2158da
--- /dev/null
+++ b/drivers/staging/rt2860/common/ba_action.c
@@ -0,0 +1,1802 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifdef DOT11_N_SUPPORT
+
+#include "../rt_config.h"
+
+
+
+#define BA_ORI_INIT_SEQ		(pEntry->TxSeq[TID]) //1			// inital sequence number of BA session
+
+#define ORI_SESSION_MAX_RETRY	8
+#define ORI_BA_SESSION_TIMEOUT	(2000)	// ms
+#define REC_BA_SESSION_IDLE_TIMEOUT	(1000)	// ms
+
+#define REORDERING_PACKET_TIMEOUT		((100 * HZ)/1000)	// system ticks -- 100 ms
+#define MAX_REORDERING_PACKET_TIMEOUT	((3000 * HZ)/1000)	// system ticks -- 100 ms
+
+#define RESET_RCV_SEQ		(0xFFFF)
+
+static void ba_mpdu_blk_free(PRTMP_ADAPTER pAd, struct reordering_mpdu *mpdu_blk);
+
+
+BA_ORI_ENTRY *BATableAllocOriEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx);
+
+BA_REC_ENTRY *BATableAllocRecEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx);
+
+VOID BAOriSessionSetupTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID BARecSessionIdleTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+
+BUILD_TIMER_FUNCTION(BAOriSessionSetupTimeout);
+BUILD_TIMER_FUNCTION(BARecSessionIdleTimeout);
+
+#define ANNOUNCE_REORDERING_PACKET(_pAd, _mpdu_blk)	\
+			Announce_Reordering_Packet(_pAd, _mpdu_blk);
+
+VOID BA_MaxWinSizeReasign(
+	IN PRTMP_ADAPTER	pAd,
+	IN MAC_TABLE_ENTRY  *pEntryPeer,
+	OUT UCHAR			*pWinSize)
+{
+	UCHAR MaxSize;
+
+
+	if (pAd->MACVersion >= RALINK_2883_VERSION) // 3*3
+	{
+		if (pAd->MACVersion >= RALINK_3070_VERSION)
+		{
+			if (pEntryPeer->WepStatus != Ndis802_11EncryptionDisabled)
+				MaxSize = 7; // for non-open mode
+			else
+				MaxSize = 13;
+		}
+		else
+			MaxSize = 31;
+	}
+	else if (pAd->MACVersion >= RALINK_2880E_VERSION) // 2880 e
+	{
+		if (pEntryPeer->WepStatus != Ndis802_11EncryptionDisabled)
+			MaxSize = 7; // for non-open mode
+		else
+			MaxSize = 13;
+	}
+	else
+		MaxSize = 7;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ba> Win Size = %d, Max Size = %d\n",
+			*pWinSize, MaxSize));
+
+	if ((*pWinSize) > MaxSize)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ba> reassign max win size from %d to %d\n",
+				*pWinSize, MaxSize));
+
+		*pWinSize = MaxSize;
+	}
+}
+
+void Announce_Reordering_Packet(IN PRTMP_ADAPTER			pAd,
+								IN struct reordering_mpdu	*mpdu)
+{
+	PNDIS_PACKET    pPacket;
+
+	pPacket = mpdu->pPacket;
+
+	if (mpdu->bAMSDU)
+	{
+		ASSERT(0);
+		BA_Reorder_AMSDU_Annnounce(pAd, pPacket);
+	}
+	else
+	{
+		//
+		// pass this 802.3 packet to upper layer or forward this packet to WM directly
+		//
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pPacket, RTMP_GET_PACKET_IF(pPacket));
+#endif // CONFIG_STA_SUPPORT //
+	}
+}
+
+/*
+ * Insert a reordering mpdu into sorted linked list by sequence no.
+ */
+BOOLEAN ba_reordering_mpdu_insertsorted(struct reordering_list *list, struct reordering_mpdu *mpdu)
+{
+
+	struct reordering_mpdu **ppScan = &list->next;
+
+	while (*ppScan != NULL)
+	{
+		if (SEQ_SMALLER((*ppScan)->Sequence, mpdu->Sequence, MAXSEQ))
+		{
+			ppScan = &(*ppScan)->next;
+		}
+		else if ((*ppScan)->Sequence == mpdu->Sequence)
+		{
+			/* give up this duplicated frame */
+			return(FALSE);
+		}
+		else
+		{
+			/* find position */
+			break;
+		}
+	}
+
+	mpdu->next = *ppScan;
+	*ppScan = mpdu;
+	list->qlen++;
+	return TRUE;
+}
+
+
+/*
+ * caller lock critical section if necessary
+ */
+static inline void ba_enqueue(struct reordering_list *list, struct reordering_mpdu *mpdu_blk)
+{
+	list->qlen++;
+	mpdu_blk->next = list->next;
+	list->next = mpdu_blk;
+}
+
+/*
+ * caller lock critical section if necessary
+ */
+static inline struct reordering_mpdu * ba_dequeue(struct reordering_list *list)
+{
+	struct reordering_mpdu *mpdu_blk = NULL;
+
+	ASSERT(list);
+
+		if (list->qlen)
+		{
+			list->qlen--;
+			mpdu_blk = list->next;
+			if (mpdu_blk)
+			{
+				list->next = mpdu_blk->next;
+				mpdu_blk->next = NULL;
+			}
+		}
+	return mpdu_blk;
+}
+
+
+static inline struct reordering_mpdu  *ba_reordering_mpdu_dequeue(struct reordering_list *list)
+{
+	return(ba_dequeue(list));
+}
+
+
+static inline struct reordering_mpdu  *ba_reordering_mpdu_probe(struct reordering_list *list)
+	{
+	ASSERT(list);
+
+		return(list->next);
+	}
+
+
+/*
+ * free all resource for reordering mechanism
+ */
+void ba_reordering_resource_release(PRTMP_ADAPTER pAd)
+{
+	BA_TABLE        *Tab;
+	PBA_REC_ENTRY   pBAEntry;
+	struct reordering_mpdu *mpdu_blk;
+	int i;
+
+	Tab = &pAd->BATable;
+
+	/* I.  release all pending reordering packet */
+	NdisAcquireSpinLock(&pAd->BATabLock);
+	for (i = 0; i < MAX_LEN_OF_BA_REC_TABLE; i++)
+	{
+		pBAEntry = &Tab->BARecEntry[i];
+		if (pBAEntry->REC_BA_Status != Recipient_NONE)
+		{
+			while ((mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list)))
+			{
+				ASSERT(mpdu_blk->pPacket);
+				RELEASE_NDIS_PACKET(pAd, mpdu_blk->pPacket, NDIS_STATUS_FAILURE);
+				ba_mpdu_blk_free(pAd, mpdu_blk);
+			}
+		}
+	}
+	NdisReleaseSpinLock(&pAd->BATabLock);
+
+	ASSERT(pBAEntry->list.qlen == 0);
+	/* II. free memory of reordering mpdu table */
+	NdisAcquireSpinLock(&pAd->mpdu_blk_pool.lock);
+	os_free_mem(pAd, pAd->mpdu_blk_pool.mem);
+	NdisReleaseSpinLock(&pAd->mpdu_blk_pool.lock);
+}
+
+
+
+/*
+ * Allocate all resource for reordering mechanism
+ */
+BOOLEAN ba_reordering_resource_init(PRTMP_ADAPTER pAd, int num)
+{
+	int     i;
+	PUCHAR  mem;
+	struct reordering_mpdu *mpdu_blk;
+	struct reordering_list *freelist;
+
+	/* allocate spinlock */
+	NdisAllocateSpinLock(&pAd->mpdu_blk_pool.lock);
+
+	/* initialize freelist */
+	freelist = &pAd->mpdu_blk_pool.freelist;
+	freelist->next = NULL;
+	freelist->qlen = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Allocate %d memory for BA reordering\n", (UINT32)(num*sizeof(struct reordering_mpdu))));
+
+	/* allocate number of mpdu_blk memory */
+	os_alloc_mem(pAd, (PUCHAR *)&mem, (num*sizeof(struct reordering_mpdu)));
+
+	pAd->mpdu_blk_pool.mem = mem;
+
+	if (mem == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Can't Allocate Memory for BA Reordering\n"));
+		return(FALSE);
+	}
+
+	/* build mpdu_blk free list */
+	for (i=0; i<num; i++)
+	{
+		/* get mpdu_blk */
+		mpdu_blk = (struct reordering_mpdu *) mem;
+		/* initial mpdu_blk */
+		NdisZeroMemory(mpdu_blk, sizeof(struct reordering_mpdu));
+		/* next mpdu_blk */
+		mem += sizeof(struct reordering_mpdu);
+		/* insert mpdu_blk into freelist */
+		ba_enqueue(freelist, mpdu_blk);
+	}
+
+	return(TRUE);
+}
+
+//static int blk_count=0; // sample take off, no use
+
+static struct reordering_mpdu *ba_mpdu_blk_alloc(PRTMP_ADAPTER pAd)
+{
+	struct reordering_mpdu *mpdu_blk;
+
+	NdisAcquireSpinLock(&pAd->mpdu_blk_pool.lock);
+	mpdu_blk = ba_dequeue(&pAd->mpdu_blk_pool.freelist);
+	if (mpdu_blk)
+	{
+//		blk_count++;
+		/* reset mpdu_blk */
+		NdisZeroMemory(mpdu_blk, sizeof(struct reordering_mpdu));
+	}
+	NdisReleaseSpinLock(&pAd->mpdu_blk_pool.lock);
+	return mpdu_blk;
+}
+
+static void ba_mpdu_blk_free(PRTMP_ADAPTER pAd, struct reordering_mpdu *mpdu_blk)
+{
+	ASSERT(mpdu_blk);
+
+	NdisAcquireSpinLock(&pAd->mpdu_blk_pool.lock);
+//	blk_count--;
+	ba_enqueue(&pAd->mpdu_blk_pool.freelist, mpdu_blk);
+	NdisReleaseSpinLock(&pAd->mpdu_blk_pool.lock);
+}
+
+
+static USHORT ba_indicate_reordering_mpdus_in_order(
+												   IN PRTMP_ADAPTER    pAd,
+												   IN PBA_REC_ENTRY    pBAEntry,
+												   IN USHORT           StartSeq)
+{
+	struct reordering_mpdu *mpdu_blk;
+	USHORT  LastIndSeq = RESET_RCV_SEQ;
+
+	NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+
+	while ((mpdu_blk = ba_reordering_mpdu_probe(&pBAEntry->list)))
+		{
+			/* find in-order frame */
+		if (!SEQ_STEPONE(mpdu_blk->Sequence, StartSeq, MAXSEQ))
+			{
+				break;
+			}
+			/* dequeue in-order frame from reodering list */
+			mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list);
+			/* pass this frame up */
+		ANNOUNCE_REORDERING_PACKET(pAd, mpdu_blk);
+		/* move to next sequence */
+			StartSeq = mpdu_blk->Sequence;
+		LastIndSeq = StartSeq;
+		/* free mpdu_blk */
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+	}
+
+	NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+
+	/* update last indicated sequence */
+	return LastIndSeq;
+}
+
+static void ba_indicate_reordering_mpdus_le_seq(
+											   IN PRTMP_ADAPTER    pAd,
+											   IN PBA_REC_ENTRY    pBAEntry,
+											   IN USHORT           Sequence)
+{
+	struct reordering_mpdu *mpdu_blk;
+
+	NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+	while ((mpdu_blk = ba_reordering_mpdu_probe(&pBAEntry->list)))
+		{
+			/* find in-order frame */
+		if ((mpdu_blk->Sequence == Sequence) || SEQ_SMALLER(mpdu_blk->Sequence, Sequence, MAXSEQ))
+		{
+			/* dequeue in-order frame from reodering list */
+			mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list);
+			/* pass this frame up */
+			ANNOUNCE_REORDERING_PACKET(pAd, mpdu_blk);
+			/* free mpdu_blk */
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+		}
+		else
+			{
+				break;
+			}
+	}
+	NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+}
+
+
+static void ba_refresh_reordering_mpdus(
+									   IN PRTMP_ADAPTER    pAd,
+									   PBA_REC_ENTRY       pBAEntry)
+{
+	struct reordering_mpdu *mpdu_blk;
+
+	NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+
+			/* dequeue in-order frame from reodering list */
+	while ((mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list)))
+	{
+			/* pass this frame up */
+		ANNOUNCE_REORDERING_PACKET(pAd, mpdu_blk);
+
+		pBAEntry->LastIndSeq = mpdu_blk->Sequence;
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+
+		/* update last indicated sequence */
+	}
+	ASSERT(pBAEntry->list.qlen == 0);
+	pBAEntry->LastIndSeq = RESET_RCV_SEQ;
+	NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+}
+
+
+//static
+void ba_flush_reordering_timeout_mpdus(
+									IN PRTMP_ADAPTER    pAd,
+									IN PBA_REC_ENTRY    pBAEntry,
+									IN ULONG            Now32)
+
+{
+	USHORT Sequence;
+
+//	if ((RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+REORDERING_PACKET_TIMEOUT)) &&
+//		 (pBAEntry->list.qlen > ((pBAEntry->BAWinSize*7)/8))) //||
+//		(RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(10*REORDERING_PACKET_TIMEOUT))) &&
+//		 (pBAEntry->list.qlen > (pBAEntry->BAWinSize/8)))
+	if (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(MAX_REORDERING_PACKET_TIMEOUT/6)))
+		 &&(pBAEntry->list.qlen > 1)
+		)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("timeout[%d] (%08lx-%08lx = %d > %d): %x, flush all!\n ", pBAEntry->list.qlen, Now32, (pBAEntry->LastIndSeqAtTimer),
+			   (int)((long) Now32 - (long)(pBAEntry->LastIndSeqAtTimer)), MAX_REORDERING_PACKET_TIMEOUT,
+			   pBAEntry->LastIndSeq));
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+		pBAEntry->LastIndSeqAtTimer = Now32;
+	}
+	else
+	if (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(REORDERING_PACKET_TIMEOUT)))
+		&& (pBAEntry->list.qlen > 0)
+	   )
+		{
+//		printk("timeout[%d] (%lx-%lx = %d > %d): %x, ", pBAEntry->list.qlen, Now32, (pBAEntry->LastIndSeqAtTimer),
+//			   (int)((long) Now32 - (long)(pBAEntry->LastIndSeqAtTimer)), REORDERING_PACKET_TIMEOUT,
+//			   pBAEntry->LastIndSeq);
+    		//
+		// force LastIndSeq to shift to LastIndSeq+1
+    		//
+    		Sequence = (pBAEntry->LastIndSeq+1) & MAXSEQ;
+    		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, Sequence);
+    		pBAEntry->LastIndSeqAtTimer = Now32;
+			pBAEntry->LastIndSeq = Sequence;
+    		//
+    		// indicate in-order mpdus
+    		//
+    		Sequence = ba_indicate_reordering_mpdus_in_order(pAd, pBAEntry, Sequence);
+    		if (Sequence != RESET_RCV_SEQ)
+    		{
+    			pBAEntry->LastIndSeq = Sequence;
+    		}
+
+		//printk("%x, flush one!\n", pBAEntry->LastIndSeq);
+
+	}
+#if 0
+	else if (
+			 (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(MAX_REORDERING_PACKET_TIMEOUT))) &&
+			  (pBAEntry->list.qlen > 1))
+			)
+		{
+		DBGPRINT(RT_DEBUG_TRACE,("timeout[%d] (%lx-%lx = %d > %d): %x\n ", pBAEntry->list.qlen, Now32, (pBAEntry->LastIndSeqAtTimer),
+			   (int)((long) Now32 - (long)(pBAEntry->LastIndSeqAtTimer)), MAX_REORDERING_PACKET_TIMEOUT,
+			   pBAEntry->LastIndSeq));
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+			pBAEntry->LastIndSeqAtTimer = Now32;
+				}
+#endif
+}
+
+
+/*
+ * generate ADDBA request to
+ * set up BA agreement
+ */
+VOID BAOriSessionSetUp(
+					  IN PRTMP_ADAPTER    pAd,
+					  IN MAC_TABLE_ENTRY  *pEntry,
+					  IN UCHAR            TID,
+					  IN USHORT           TimeOut,
+					  IN ULONG            DelayTime,
+					  IN BOOLEAN          isForced)
+
+{
+	//MLME_ADDBA_REQ_STRUCT	AddbaReq;
+	BA_ORI_ENTRY            *pBAEntry = NULL;
+	USHORT                  Idx;
+	BOOLEAN                 Cancelled;
+
+	if ((pAd->CommonCfg.BACapability.field.AutoBA != TRUE)  &&  (isForced == FALSE))
+		return;
+
+	// if this entry is limited to use legacy tx mode, it doesn't generate BA.
+	if (RTMPStaFixedTxMode(pAd, pEntry) != FIXED_TXMODE_HT)
+		return;
+
+	if ((pEntry->BADeclineBitmap & (1<<TID)) && (isForced == FALSE))
+	{
+		// try again after 3 secs
+		DelayTime = 3000;
+//		printk("DeCline BA from Peer\n");
+//		return;
+	}
+
+
+	Idx = pEntry->BAOriWcidArray[TID];
+	if (Idx == 0)
+	{
+		// allocate a BA session
+		pBAEntry = BATableAllocOriEntry(pAd, &Idx);
+		if (pBAEntry == NULL)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("ADDBA - MlmeADDBAAction() allocate BA session failed \n"));
+			return;
+		}
+	}
+	else
+	{
+		pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+	}
+
+	if (pBAEntry->ORI_BA_Status >= Originator_WaitRes)
+	{
+		return;
+	}
+
+	pEntry->BAOriWcidArray[TID] = Idx;
+
+	// Initialize BA session
+	pBAEntry->ORI_BA_Status = Originator_WaitRes;
+	pBAEntry->Wcid = pEntry->Aid;
+	pBAEntry->BAWinSize = pAd->CommonCfg.BACapability.field.RxBAWinLimit;
+	pBAEntry->Sequence = BA_ORI_INIT_SEQ;
+	pBAEntry->Token = 1;	// (2008-01-21) Jan Lee recommends it - this token can't be 0
+	pBAEntry->TID = TID;
+	pBAEntry->TimeOutValue = TimeOut;
+	pBAEntry->pAdapter = pAd;
+
+	if (!(pEntry->TXBAbitmap & (1<<TID)))
+	{
+		RTMPInitTimer(pAd, &pBAEntry->ORIBATimer, GET_TIMER_FUNCTION(BAOriSessionSetupTimeout), pBAEntry, FALSE);
+	}
+	else
+		RTMPCancelTimer(&pBAEntry->ORIBATimer, &Cancelled);
+
+	// set timer to send ADDBA request
+	RTMPSetTimer(&pBAEntry->ORIBATimer, DelayTime);
+}
+
+VOID BAOriSessionAdd(
+			IN PRTMP_ADAPTER    pAd,
+					IN MAC_TABLE_ENTRY  *pEntry,
+			IN PFRAME_ADDBA_RSP pFrame)
+{
+	BA_ORI_ENTRY  *pBAEntry = NULL;
+	BOOLEAN       Cancelled;
+	UCHAR         TID;
+	USHORT        Idx;
+	PUCHAR          pOutBuffer2 = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG           FrameLen;
+	FRAME_BAR       FrameBar;
+
+	TID = pFrame->BaParm.TID;
+	Idx = pEntry->BAOriWcidArray[TID];
+	pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+
+	// Start fill in parameters.
+	if ((Idx !=0) && (pBAEntry->TID == TID) && (pBAEntry->ORI_BA_Status == Originator_WaitRes))
+	{
+		pBAEntry->BAWinSize = min(pBAEntry->BAWinSize, ((UCHAR)pFrame->BaParm.BufSize));
+		BA_MaxWinSizeReasign(pAd, pEntry, &pBAEntry->BAWinSize);
+
+		pBAEntry->TimeOutValue = pFrame->TimeOutValue;
+		pBAEntry->ORI_BA_Status = Originator_Done;
+		// reset sequence number
+		pBAEntry->Sequence = BA_ORI_INIT_SEQ;
+		// Set Bitmap flag.
+		pEntry->TXBAbitmap |= (1<<TID);
+				RTMPCancelTimer(&pBAEntry->ORIBATimer, &Cancelled);
+
+		pBAEntry->ORIBATimer.TimerValue = 0;	//pFrame->TimeOutValue;
+
+		DBGPRINT(RT_DEBUG_TRACE,("%s : TXBAbitmap = %x, BAWinSize = %d, TimeOut = %ld\n", __func__, pEntry->TXBAbitmap,
+								 pBAEntry->BAWinSize, pBAEntry->ORIBATimer.TimerValue));
+
+		// SEND BAR ;
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer2);  //Get an unused nonpaged memory
+		if (NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - BAOriSessionAdd() allocate memory failed \n"));
+			return;
+		}
+
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			BarHeaderInit(pAd, &FrameBar, pAd->MacTab.Content[pBAEntry->Wcid].Addr, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+
+		FrameBar.StartingSeq.field.FragNum = 0;	// make sure sequence not clear in DEL function.
+		FrameBar.StartingSeq.field.StartSeq = pBAEntry->Sequence; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.TID = pBAEntry->TID; // make sure sequence not clear in DEL funciton.
+		MakeOutgoingFrame(pOutBuffer2,              &FrameLen,
+						  sizeof(FRAME_BAR),      &FrameBar,
+					  END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer2, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer2);
+
+
+		if (pBAEntry->ORIBATimer.TimerValue)
+			RTMPSetTimer(&pBAEntry->ORIBATimer, pBAEntry->ORIBATimer.TimerValue); // in mSec
+	}
+}
+
+BOOLEAN BARecSessionAdd(
+					   IN PRTMP_ADAPTER    pAd,
+					   IN MAC_TABLE_ENTRY  *pEntry,
+					   IN PFRAME_ADDBA_REQ pFrame)
+{
+	BA_REC_ENTRY            *pBAEntry = NULL;
+	BOOLEAN                 Status = TRUE;
+	BOOLEAN                 Cancelled;
+	USHORT                  Idx;
+	UCHAR                   TID;
+	UCHAR                   BAWinSize;
+	//UINT32                  Value;
+	//UINT                    offset;
+
+
+	ASSERT(pEntry);
+
+	// find TID
+	TID = pFrame->BaParm.TID;
+
+	BAWinSize = min(((UCHAR)pFrame->BaParm.BufSize), (UCHAR)pAd->CommonCfg.BACapability.field.RxBAWinLimit);
+
+	// Intel patch
+	if (BAWinSize == 0)
+	{
+		BAWinSize = 64;
+	}
+
+	Idx = pEntry->BARecWcidArray[TID];
+
+
+	if (Idx == 0)
+	{
+		pBAEntry = BATableAllocRecEntry(pAd, &Idx);
+	}
+	else
+	{
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+		// flush all pending reordering mpdus
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s(%ld): Idx = %d, BAWinSize(req %d) = %d\n", __func__, pAd->BATable.numAsRecipient, Idx,
+							 pFrame->BaParm.BufSize, BAWinSize));
+
+	// Start fill in parameters.
+	if (pBAEntry != NULL)
+	{
+		ASSERT(pBAEntry->list.qlen == 0);
+
+		pBAEntry->REC_BA_Status = Recipient_HandleRes;
+		pBAEntry->BAWinSize = BAWinSize;
+		pBAEntry->Wcid = pEntry->Aid;
+		pBAEntry->TID = TID;
+		pBAEntry->TimeOutValue = pFrame->TimeOutValue;
+		pBAEntry->REC_BA_Status = Recipient_Accept;
+		// initial sequence number
+		pBAEntry->LastIndSeq = RESET_RCV_SEQ; //pFrame->BaStartSeq.field.StartSeq;
+
+		printk("Start Seq = %08x\n",  pFrame->BaStartSeq.field.StartSeq);
+
+		if (pEntry->RXBAbitmap & (1<<TID))
+		{
+			RTMPCancelTimer(&pBAEntry->RECBATimer, &Cancelled);
+		}
+		else
+		{
+			RTMPInitTimer(pAd, &pBAEntry->RECBATimer, GET_TIMER_FUNCTION(BARecSessionIdleTimeout), pBAEntry, TRUE);
+		}
+
+#if 0	// for debugging
+		RTMPSetTimer(&pBAEntry->RECBATimer, REC_BA_SESSION_IDLE_TIMEOUT);
+#endif
+
+		// Set Bitmap flag.
+		pEntry->RXBAbitmap |= (1<<TID);
+		pEntry->BARecWcidArray[TID] = Idx;
+
+		pEntry->BADeclineBitmap &= ~(1<<TID);
+
+		// Set BA session mask in WCID table.
+		RT28XX_ADD_BA_SESSION_TO_ASIC(pAd, pEntry->Aid, TID);
+
+		DBGPRINT(RT_DEBUG_TRACE,("MACEntry[%d]RXBAbitmap = 0x%x. BARecWcidArray=%d\n",
+				pEntry->Aid, pEntry->RXBAbitmap, pEntry->BARecWcidArray[TID]));
+	}
+	else
+	{
+		Status = FALSE;
+		DBGPRINT(RT_DEBUG_TRACE,("Can't Accept ADDBA for %02x:%02x:%02x:%02x:%02x:%02x TID = %d\n",
+				PRINT_MAC(pEntry->Addr), TID));
+	}
+	return(Status);
+}
+
+
+BA_REC_ENTRY *BATableAllocRecEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx)
+{
+	int             i;
+	BA_REC_ENTRY    *pBAEntry = NULL;
+
+
+	NdisAcquireSpinLock(&pAd->BATabLock);
+
+	if (pAd->BATable.numAsRecipient >= MAX_BARECI_SESSION)
+	{
+		printk("BA Recipeint Session (%ld) > %d\n", pAd->BATable.numAsRecipient,
+			MAX_BARECI_SESSION);
+		goto done;
+	}
+
+	// reserve idx 0 to identify BAWcidArray[TID] as empty
+	for (i=1; i < MAX_LEN_OF_BA_REC_TABLE; i++)
+	{
+		pBAEntry =&pAd->BATable.BARecEntry[i];
+		if ((pBAEntry->REC_BA_Status == Recipient_NONE))
+		{
+			// get one
+			pAd->BATable.numAsRecipient++;
+			pBAEntry->REC_BA_Status = Recipient_USED;
+			*Idx = i;
+			break;
+		}
+	}
+
+done:
+	NdisReleaseSpinLock(&pAd->BATabLock);
+	return pBAEntry;
+}
+
+BA_ORI_ENTRY *BATableAllocOriEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx)
+{
+	int             i;
+	BA_ORI_ENTRY    *pBAEntry = NULL;
+
+	NdisAcquireSpinLock(&pAd->BATabLock);
+
+	if (pAd->BATable.numAsOriginator >= (MAX_LEN_OF_BA_ORI_TABLE))
+	{
+		goto done;
+	}
+
+	// reserve idx 0 to identify BAWcidArray[TID] as empty
+	for (i=1; i<MAX_LEN_OF_BA_ORI_TABLE; i++)
+	{
+		pBAEntry =&pAd->BATable.BAOriEntry[i];
+		if ((pBAEntry->ORI_BA_Status == Originator_NONE))
+		{
+			// get one
+			pAd->BATable.numAsOriginator++;
+			pBAEntry->ORI_BA_Status = Originator_USED;
+			pBAEntry->pAdapter = pAd;
+			*Idx = i;
+			break;
+		}
+	}
+
+done:
+	NdisReleaseSpinLock(&pAd->BATabLock);
+	return pBAEntry;
+}
+
+
+VOID BATableFreeOriEntry(
+						IN  PRTMP_ADAPTER   pAd,
+						IN  ULONG           Idx)
+{
+	BA_ORI_ENTRY    *pBAEntry = NULL;
+	MAC_TABLE_ENTRY *pEntry;
+
+
+	if ((Idx == 0) || (Idx >= MAX_LEN_OF_BA_ORI_TABLE))
+		return;
+
+	pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+
+	if (pBAEntry->ORI_BA_Status != Originator_NONE)
+	{
+		pEntry = &pAd->MacTab.Content[pBAEntry->Wcid];
+		pEntry->BAOriWcidArray[pBAEntry->TID] = 0;
+
+
+		NdisAcquireSpinLock(&pAd->BATabLock);
+		if (pBAEntry->ORI_BA_Status == Originator_Done)
+		{
+		 	pEntry->TXBAbitmap &= (~(1<<(pBAEntry->TID) ));
+			DBGPRINT(RT_DEBUG_TRACE, ("BATableFreeOriEntry numAsOriginator= %ld\n", pAd->BATable.numAsRecipient));
+			// Erase Bitmap flag.
+		}
+
+		ASSERT(pAd->BATable.numAsOriginator != 0);
+
+		pAd->BATable.numAsOriginator -= 1;
+
+		pBAEntry->ORI_BA_Status = Originator_NONE;
+		pBAEntry->Token = 0;
+		NdisReleaseSpinLock(&pAd->BATabLock);
+	}
+}
+
+
+VOID BATableFreeRecEntry(
+						IN  PRTMP_ADAPTER   pAd,
+						IN  ULONG           Idx)
+{
+	BA_REC_ENTRY    *pBAEntry = NULL;
+	MAC_TABLE_ENTRY *pEntry;
+
+
+	if ((Idx == 0) || (Idx >= MAX_LEN_OF_BA_REC_TABLE))
+		return;
+
+	pBAEntry =&pAd->BATable.BARecEntry[Idx];
+
+	if (pBAEntry->REC_BA_Status != Recipient_NONE)
+	{
+		pEntry = &pAd->MacTab.Content[pBAEntry->Wcid];
+		pEntry->BARecWcidArray[pBAEntry->TID] = 0;
+
+		NdisAcquireSpinLock(&pAd->BATabLock);
+
+		ASSERT(pAd->BATable.numAsRecipient != 0);
+
+		pAd->BATable.numAsRecipient -= 1;
+
+		pBAEntry->REC_BA_Status = Recipient_NONE;
+		NdisReleaseSpinLock(&pAd->BATabLock);
+	}
+}
+
+
+VOID BAOriSessionTearDown(
+						 IN OUT  PRTMP_ADAPTER   pAd,
+						 IN      UCHAR           Wcid,
+						 IN      UCHAR           TID,
+						 IN      BOOLEAN         bPassive,
+						 IN      BOOLEAN         bForceSend)
+{
+	ULONG           Idx = 0;
+	BA_ORI_ENTRY    *pBAEntry;
+	BOOLEAN         Cancelled;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+	{
+		return;
+	}
+
+	//
+	// Locate corresponding BA Originator Entry in BA Table with the (pAddr,TID).
+	//
+	Idx = pAd->MacTab.Content[Wcid].BAOriWcidArray[TID];
+	if ((Idx == 0) || (Idx >= MAX_LEN_OF_BA_ORI_TABLE))
+	{
+		if (bForceSend == TRUE)
+		{
+			// force send specified TID DelBA
+			MLME_DELBA_REQ_STRUCT   DelbaReq;
+			MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+			NdisZeroMemory(&DelbaReq, sizeof(DelbaReq));
+			NdisZeroMemory(Elem, sizeof(MLME_QUEUE_ELEM));
+
+			COPY_MAC_ADDR(DelbaReq.Addr, pAd->MacTab.Content[Wcid].Addr);
+			DelbaReq.Wcid = Wcid;
+			DelbaReq.TID = TID;
+			DelbaReq.Initiator = ORIGINATOR;
+#if 1
+			Elem->MsgLen  = sizeof(DelbaReq);
+			NdisMoveMemory(Elem->Msg, &DelbaReq, sizeof(DelbaReq));
+			MlmeDELBAAction(pAd, Elem);
+			kfree(Elem);
+#else
+			MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ORI_DELBA_CATE, sizeof(MLME_DELBA_REQ_STRUCT), (PVOID)&DelbaReq);
+			RT28XX_MLME_HANDLER(pAd);
+#endif
+		}
+
+		return;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s===>Wcid=%d.TID=%d \n", __func__, Wcid, TID));
+
+	pBAEntry = &pAd->BATable.BAOriEntry[Idx];
+	DBGPRINT(RT_DEBUG_TRACE,("\t===>Idx = %ld, Wcid=%d.TID=%d, ORI_BA_Status = %d \n", Idx, Wcid, TID, pBAEntry->ORI_BA_Status));
+	//
+	// Prepare DelBA action frame and send to the peer.
+	//
+	if ((bPassive == FALSE) && (TID == pBAEntry->TID) && (pBAEntry->ORI_BA_Status == Originator_Done))
+	{
+		MLME_DELBA_REQ_STRUCT   DelbaReq;
+		MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+		NdisZeroMemory(&DelbaReq, sizeof(DelbaReq));
+		NdisZeroMemory(Elem, sizeof(MLME_QUEUE_ELEM));
+
+		COPY_MAC_ADDR(DelbaReq.Addr, pAd->MacTab.Content[Wcid].Addr);
+		DelbaReq.Wcid = Wcid;
+		DelbaReq.TID = pBAEntry->TID;
+		DelbaReq.Initiator = ORIGINATOR;
+#if 1
+		Elem->MsgLen  = sizeof(DelbaReq);
+		NdisMoveMemory(Elem->Msg, &DelbaReq, sizeof(DelbaReq));
+		MlmeDELBAAction(pAd, Elem);
+		kfree(Elem);
+#else
+		MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ORI_DELBA_CATE, sizeof(MLME_DELBA_REQ_STRUCT), (PVOID)&DelbaReq);
+		RT28XX_MLME_HANDLER(pAd);
+#endif
+	}
+	RTMPCancelTimer(&pBAEntry->ORIBATimer, &Cancelled);
+	BATableFreeOriEntry(pAd, Idx);
+
+	if (bPassive)
+	{
+		//BAOriSessionSetUp(pAd, &pAd->MacTab.Content[Wcid], TID, 0, 10000, TRUE);
+	}
+}
+
+VOID BARecSessionTearDown(
+						 IN OUT  PRTMP_ADAPTER   pAd,
+						 IN      UCHAR           Wcid,
+						 IN      UCHAR           TID,
+						 IN      BOOLEAN         bPassive)
+{
+	ULONG           Idx = 0;
+	BA_REC_ENTRY    *pBAEntry;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+	{
+		return;
+	}
+
+	//
+	//  Locate corresponding BA Originator Entry in BA Table with the (pAddr,TID).
+	//
+	Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+	if (Idx == 0)
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s===>Wcid=%d.TID=%d \n", __func__, Wcid, TID));
+
+
+	pBAEntry = &pAd->BATable.BARecEntry[Idx];
+	DBGPRINT(RT_DEBUG_TRACE,("\t===>Idx = %ld, Wcid=%d.TID=%d, REC_BA_Status = %d \n", Idx, Wcid, TID, pBAEntry->REC_BA_Status));
+	//
+	// Prepare DelBA action frame and send to the peer.
+	//
+	if ((TID == pBAEntry->TID) && (pBAEntry->REC_BA_Status == Recipient_Accept))
+	{
+		MLME_DELBA_REQ_STRUCT   DelbaReq;
+		BOOLEAN 				Cancelled;
+		MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+		//ULONG   offset;
+		//UINT32  VALUE;
+
+		RTMPCancelTimer(&pBAEntry->RECBATimer, &Cancelled);
+
+		//
+		// 1. Send DELBA Action Frame
+		//
+		if (bPassive == FALSE)
+		{
+			NdisZeroMemory(&DelbaReq, sizeof(DelbaReq));
+			NdisZeroMemory(Elem, sizeof(MLME_QUEUE_ELEM));
+
+			COPY_MAC_ADDR(DelbaReq.Addr, pAd->MacTab.Content[Wcid].Addr);
+			DelbaReq.Wcid = Wcid;
+			DelbaReq.TID = TID;
+			DelbaReq.Initiator = RECIPIENT;
+#if 1
+			Elem->MsgLen  = sizeof(DelbaReq);
+			NdisMoveMemory(Elem->Msg, &DelbaReq, sizeof(DelbaReq));
+			MlmeDELBAAction(pAd, Elem);
+			kfree(Elem);
+#else
+			MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ORI_DELBA_CATE, sizeof(MLME_DELBA_REQ_STRUCT), (PVOID)&DelbaReq);
+			RT28XX_MLME_HANDLER(pAd);
+#endif
+		}
+
+
+		//
+		// 2. Free resource of BA session
+		//
+		// flush all pending reordering mpdus
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+
+		NdisAcquireSpinLock(&pAd->BATabLock);
+
+		// Erase Bitmap flag.
+		pBAEntry->LastIndSeq = RESET_RCV_SEQ;
+		pBAEntry->BAWinSize = 0;
+		// Erase Bitmap flag at software mactable
+		pAd->MacTab.Content[Wcid].RXBAbitmap &= (~(1<<(pBAEntry->TID)));
+		pAd->MacTab.Content[Wcid].BARecWcidArray[TID] = 0;
+
+		RT28XX_DEL_BA_SESSION_FROM_ASIC(pAd, Wcid, TID);
+
+		NdisReleaseSpinLock(&pAd->BATabLock);
+
+	}
+
+	BATableFreeRecEntry(pAd, Idx);
+}
+
+VOID BASessionTearDownALL(
+						 IN OUT  PRTMP_ADAPTER pAd,
+						 IN      UCHAR Wcid)
+{
+	int i;
+
+	for (i=0; i<NUM_OF_TID; i++)
+	{
+		BAOriSessionTearDown(pAd, Wcid, i, FALSE, FALSE);
+		BARecSessionTearDown(pAd, Wcid, i, FALSE);
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Retry sending ADDBA Reqest.
+
+	IRQL = DISPATCH_LEVEL
+
+	Parametrs:
+	p8023Header: if this is already 802.3 format, p8023Header is NULL
+
+	Return	: TRUE if put into rx reordering buffer, shouldn't indicaterxhere.
+				FALSE , then continue indicaterx at this moment.
+	==========================================================================
+ */
+VOID BAOriSessionSetupTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+	BA_ORI_ENTRY    *pBAEntry = (BA_ORI_ENTRY *)FunctionContext;
+	MAC_TABLE_ENTRY *pEntry;
+	PRTMP_ADAPTER   pAd;
+
+	if (pBAEntry == NULL)
+		return;
+
+	pAd = pBAEntry->pAdapter;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Do nothing if monitor mode is on
+		if (MONITOR_ON(pAd))
+			return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RALINK_ATE
+	// Nothing to do in ATE mode.
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	pEntry = &pAd->MacTab.Content[pBAEntry->Wcid];
+
+	if ((pBAEntry->ORI_BA_Status == Originator_WaitRes) && (pBAEntry->Token < ORI_SESSION_MAX_RETRY))
+	{
+		MLME_ADDBA_REQ_STRUCT    AddbaReq;
+
+		NdisZeroMemory(&AddbaReq, sizeof(AddbaReq));
+		COPY_MAC_ADDR(AddbaReq.pAddr, pEntry->Addr);
+		AddbaReq.Wcid = (UCHAR)(pEntry->Aid);
+		AddbaReq.TID = pBAEntry->TID;
+		AddbaReq.BaBufSize = pAd->CommonCfg.BACapability.field.RxBAWinLimit;
+		AddbaReq.TimeOutValue = 0;
+		AddbaReq.Token = pBAEntry->Token;
+		MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ADD_BA_CATE, sizeof(MLME_ADDBA_REQ_STRUCT), (PVOID)&AddbaReq);
+		RT28XX_MLME_HANDLER(pAd);
+		DBGPRINT(RT_DEBUG_TRACE,("BA Ori Session Timeout(%d) : Send ADD BA again\n", pBAEntry->Token));
+
+		pBAEntry->Token++;
+		RTMPSetTimer(&pBAEntry->ORIBATimer, ORI_BA_SESSION_TIMEOUT);
+	}
+	else
+	{
+		BATableFreeOriEntry(pAd, pEntry->BAOriWcidArray[pBAEntry->TID]);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		Retry sending ADDBA Reqest.
+
+	IRQL = DISPATCH_LEVEL
+
+	Parametrs:
+	p8023Header: if this is already 802.3 format, p8023Header is NULL
+
+	Return	: TRUE if put into rx reordering buffer, shouldn't indicaterxhere.
+				FALSE , then continue indicaterx at this moment.
+	==========================================================================
+ */
+VOID BARecSessionIdleTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+
+	BA_REC_ENTRY    *pBAEntry = (BA_REC_ENTRY *)FunctionContext;
+	PRTMP_ADAPTER   pAd;
+	ULONG           Now32;
+
+	if (pBAEntry == NULL)
+		return;
+
+	if ((pBAEntry->REC_BA_Status == Recipient_Accept))
+	{
+		NdisGetSystemUpTime(&Now32);
+
+		if (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer + REC_BA_SESSION_IDLE_TIMEOUT)))
+		{
+			pAd = pBAEntry->pAdapter;
+			// flush all pending reordering mpdus
+			ba_refresh_reordering_mpdus(pAd, pBAEntry);
+			printk("%ld: REC BA session Timeout\n", Now32);
+		}
+	}
+}
+
+
+VOID PeerAddBAReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	//	7.4.4.1
+	//ULONG	Idx;
+	UCHAR   Status = 1;
+	UCHAR   pAddr[6];
+	FRAME_ADDBA_RSP ADDframe;
+	PUCHAR         pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	PFRAME_ADDBA_REQ  pAddreqFrame = NULL;
+	//UCHAR		BufSize;
+	ULONG       FrameLen;
+	PULONG      ptemp;
+	PMAC_TABLE_ENTRY	pMacEntry;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s ==> (Wcid = %d)\n", __func__, Elem->Wcid));
+
+	//hex_dump("AddBAReq", Elem->Msg, Elem->MsgLen);
+
+	//ADDBA Request from unknown peer, ignore this.
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+	pMacEntry = &pAd->MacTab.Content[Elem->Wcid];
+	DBGPRINT(RT_DEBUG_TRACE,("BA - PeerAddBAReqAction----> \n"));
+	ptemp = (PULONG)Elem->Msg;
+	//DBGPRINT_RAW(RT_DEBUG_EMU, ("%08x:: %08x:: %08x:: %08x:: %08x:: %08x:: %08x:: %08x:: %08x\n", *(ptemp), *(ptemp+1), *(ptemp+2), *(ptemp+3), *(ptemp+4), *(ptemp+5), *(ptemp+6), *(ptemp+7), *(ptemp+8)));
+
+	if (PeerAddBAReqActionSanity(pAd, Elem->Msg, Elem->MsgLen, pAddr))
+	{
+
+		if ((pAd->CommonCfg.bBADecline == FALSE) && IS_HT_STA(pMacEntry))
+		{
+			pAddreqFrame = (PFRAME_ADDBA_REQ)(&Elem->Msg[0]);
+			printk("Rcv Wcid(%d) AddBAReq\n", Elem->Wcid);
+			if (BARecSessionAdd(pAd, &pAd->MacTab.Content[Elem->Wcid], pAddreqFrame))
+				Status = 0;
+			else
+				Status = 38; // more parameters have invalid values
+		}
+		else
+		{
+			Status = 37; // the request has been declined.
+		}
+	}
+
+	if (pAd->MacTab.Content[Elem->Wcid].ValidAsCLI)
+		ASSERT(pAd->MacTab.Content[Elem->Wcid].Sst == SST_ASSOC);
+
+	pAddreqFrame = (PFRAME_ADDBA_REQ)(&Elem->Msg[0]);
+	// 2. Always send back ADDBA Response
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);	 //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ACTION - PeerBAAction() allocate memory failed \n"));
+		return;
+	}
+
+	NdisZeroMemory(&ADDframe, sizeof(FRAME_ADDBA_RSP));
+	// 2-1. Prepare ADDBA Response frame.
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (ADHOC_ON(pAd))
+			ActHeaderInit(pAd, &ADDframe.Hdr, pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+		else
+#ifdef QOS_DLS_SUPPORT
+		if (pAd->MacTab.Content[Elem->Wcid].ValidAsDls)
+			ActHeaderInit(pAd, &ADDframe.Hdr, pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+		else
+#endif // QOS_DLS_SUPPORT //
+			ActHeaderInit(pAd, &ADDframe.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAddr);
+	}
+#endif // CONFIG_STA_SUPPORT //
+	ADDframe.Category = CATEGORY_BA;
+	ADDframe.Action = ADDBA_RESP;
+	ADDframe.Token = pAddreqFrame->Token;
+	// What is the Status code??  need to check.
+	ADDframe.StatusCode = Status;
+	ADDframe.BaParm.BAPolicy = IMMED_BA;
+	ADDframe.BaParm.AMSDUSupported = 0;
+	ADDframe.BaParm.TID = pAddreqFrame->BaParm.TID;
+	ADDframe.BaParm.BufSize = min(((UCHAR)pAddreqFrame->BaParm.BufSize), (UCHAR)pAd->CommonCfg.BACapability.field.RxBAWinLimit);
+	if (ADDframe.BaParm.BufSize == 0)
+	{
+		ADDframe.BaParm.BufSize = 64;
+	}
+	ADDframe.TimeOutValue = 0; //pAddreqFrame->TimeOutValue;
+
+	*(USHORT *)(&ADDframe.BaParm) = cpu2le16(*(USHORT *)(&ADDframe.BaParm));
+	ADDframe.StatusCode = cpu2le16(ADDframe.StatusCode);
+	ADDframe.TimeOutValue = cpu2le16(ADDframe.TimeOutValue);
+
+	MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+					  sizeof(FRAME_ADDBA_RSP),  &ADDframe,
+			  END_OF_ARGS);
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s(%d): TID(%d), BufSize(%d) <== \n", __func__, Elem->Wcid, ADDframe.BaParm.TID,
+							  ADDframe.BaParm.BufSize));
+}
+
+
+VOID PeerAddBARspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	//UCHAR		Idx, i;
+	//PUCHAR		   pOutBuffer = NULL;
+	PFRAME_ADDBA_RSP    pFrame = NULL;
+	//PBA_ORI_ENTRY		pBAEntry;
+
+	//ADDBA Response from unknown peer, ignore this.
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s ==> Wcid(%d)\n", __func__, Elem->Wcid));
+
+	//hex_dump("PeerAddBARspAction()", Elem->Msg, Elem->MsgLen);
+
+	if (PeerAddBARspActionSanity(pAd, Elem->Msg, Elem->MsgLen))
+	{
+		pFrame = (PFRAME_ADDBA_RSP)(&Elem->Msg[0]);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("\t\t StatusCode = %d\n", pFrame->StatusCode));
+		switch (pFrame->StatusCode)
+		{
+			case 0:
+				// I want a BAsession with this peer as an originator.
+				BAOriSessionAdd(pAd, &pAd->MacTab.Content[Elem->Wcid], pFrame);
+				break;
+			default:
+				// check status == USED ???
+				BAOriSessionTearDown(pAd, Elem->Wcid, pFrame->BaParm.TID, TRUE, FALSE);
+				break;
+		}
+		// Rcv Decline StatusCode
+		if ((pFrame->StatusCode == 37)
+#ifdef CONFIG_STA_SUPPORT
+            || ((pAd->OpMode == OPMODE_STA) && STA_TGN_WIFI_ON(pAd) && (pFrame->StatusCode != 0))
+#endif // CONFIG_STA_SUPPORT //
+            )
+		{
+			pAd->MacTab.Content[Elem->Wcid].BADeclineBitmap |= 1<<pFrame->BaParm.TID;
+		}
+	}
+}
+
+VOID PeerDelBAAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	//UCHAR				Idx;
+	//PUCHAR				pOutBuffer = NULL;
+	PFRAME_DELBA_REQ    pDelFrame = NULL;
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s ==>\n", __func__));
+	//DELBA Request from unknown peer, ignore this.
+	if (PeerDelBAActionSanity(pAd, Elem->Wcid, Elem->Msg, Elem->MsgLen))
+	{
+		pDelFrame = (PFRAME_DELBA_REQ)(&Elem->Msg[0]);
+		if (pDelFrame->DelbaParm.Initiator == ORIGINATOR)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - PeerDelBAAction----> ORIGINATOR\n"));
+			BARecSessionTearDown(pAd, Elem->Wcid, pDelFrame->DelbaParm.TID, TRUE);
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - PeerDelBAAction----> RECIPIENT, Reason = %d\n",  pDelFrame->ReasonCode));
+			//hex_dump("DelBA Frame", pDelFrame, Elem->MsgLen);
+			BAOriSessionTearDown(pAd, Elem->Wcid, pDelFrame->DelbaParm.TID, TRUE, FALSE);
+		}
+	}
+}
+
+
+BOOLEAN CntlEnqueueForRecv(
+						  IN PRTMP_ADAPTER		pAd,
+						  IN ULONG				Wcid,
+						  IN ULONG				MsgLen,
+						  IN PFRAME_BA_REQ		pMsg)
+{
+	PFRAME_BA_REQ   pFrame = pMsg;
+	//PRTMP_REORDERBUF	pBuffer;
+	//PRTMP_REORDERBUF	pDmaBuf;
+	PBA_REC_ENTRY pBAEntry;
+	//BOOLEAN 	Result;
+	ULONG   Idx;
+	//UCHAR	NumRxPkt;
+	UCHAR	TID;//, i;
+
+	TID = (UCHAR)pFrame->BARControl.TID;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s(): BAR-Wcid(%ld), Tid (%d)\n", __func__, Wcid, TID));
+	//hex_dump("BAR", (PCHAR) pFrame, MsgLen);
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return FALSE;
+
+	// First check the size, it MUST not exceed the mlme queue size
+	if (MsgLen > MGMT_DMA_BUFFER_SIZE)
+	{
+		DBGPRINT_ERR(("CntlEnqueueForRecv: frame too large, size = %ld \n", MsgLen));
+		return FALSE;
+	}
+	else if (MsgLen != sizeof(FRAME_BA_REQ))
+	{
+		DBGPRINT_ERR(("CntlEnqueueForRecv: BlockAck Request frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+	else if (MsgLen != sizeof(FRAME_BA_REQ))
+	{
+		DBGPRINT_ERR(("CntlEnqueueForRecv: BlockAck Request frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+
+	if ((Wcid < MAX_LEN_OF_MAC_TABLE) && (TID < 8))
+		{
+		// if this receiving packet is from SA that is in our OriEntry. Since WCID <9 has direct mapping. no need search.
+		Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+		}
+		else
+		{
+		return FALSE;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BAR(%ld) : Tid (%d) - %04x:%04x\n", Wcid, TID, pFrame->BAStartingSeq.field.StartSeq, pBAEntry->LastIndSeq ));
+
+	if (SEQ_SMALLER(pBAEntry->LastIndSeq, pFrame->BAStartingSeq.field.StartSeq, MAXSEQ))
+	{
+		//printk("BAR Seq = %x, LastIndSeq = %x\n", pFrame->BAStartingSeq.field.StartSeq, pBAEntry->LastIndSeq);
+		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, pFrame->BAStartingSeq.field.StartSeq);
+		pBAEntry->LastIndSeq = (pFrame->BAStartingSeq.field.StartSeq == 0) ? MAXSEQ :(pFrame->BAStartingSeq.field.StartSeq -1);
+	}
+	//ba_refresh_reordering_mpdus(pAd, pBAEntry);
+	return TRUE;
+}
+
+/*
+Description : Send PSMP Action frame If PSMP mode switches.
+*/
+VOID SendPSMPAction(
+				   IN PRTMP_ADAPTER		pAd,
+				   IN UCHAR				Wcid,
+				   IN UCHAR				Psmp)
+{
+	PUCHAR          pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	//ULONG           Idx;
+	FRAME_PSMP_ACTION   Frame;
+	ULONG           FrameLen;
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);	 //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeADDBAAction() allocate memory failed \n"));
+		return;
+	}
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		ActHeaderInit(pAd, &Frame.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->MacTab.Content[Wcid].Addr);
+#endif // CONFIG_STA_SUPPORT //
+
+	Frame.Category = CATEGORY_HT;
+	Frame.Action = SMPS_ACTION;
+	switch (Psmp)
+	{
+		case MMPS_ENABLE:
+			Frame.Psmp = 0;
+			break;
+		case MMPS_DYNAMIC:
+			Frame.Psmp = 3;
+			break;
+		case MMPS_STATIC:
+			Frame.Psmp = 1;
+			break;
+	}
+	MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+					  sizeof(FRAME_PSMP_ACTION),      &Frame,
+					  END_OF_ARGS);
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+	DBGPRINT(RT_DEBUG_ERROR,("HT - SendPSMPAction( %d )  \n", Frame.Psmp));
+}
+
+
+#define RADIO_MEASUREMENT_REQUEST_ACTION	0
+
+typedef struct PACKED
+{
+	UCHAR	RegulatoryClass;
+	UCHAR	ChannelNumber;
+	USHORT	RandomInterval;
+	USHORT	MeasurementDuration;
+	UCHAR	MeasurementMode;
+	UCHAR   BSSID[MAC_ADDR_LEN];
+	UCHAR	ReportingCondition;
+	UCHAR	Threshold;
+	UCHAR   SSIDIE[2];			// 2 byte
+} BEACON_REQUEST;
+
+typedef struct PACKED
+{
+	UCHAR	ID;
+	UCHAR	Length;
+	UCHAR	Token;
+	UCHAR	RequestMode;
+	UCHAR	Type;
+} MEASUREMENT_REQ;
+
+
+
+
+void convert_reordering_packet_to_preAMSDU_or_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN  UCHAR			FromWhichBSSID)
+{
+	PNDIS_PACKET	pRxPkt;
+	UCHAR			Header802_3[LENGTH_802_3];
+
+	// 1. get 802.3 Header
+	// 2. remove LLC
+	// 		a. pointer pRxBlk->pData to payload
+	//      b. modify pRxBlk->DataSize
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(pRxBlk, Header802_3);
+#endif // CONFIG_STA_SUPPORT //
+
+	ASSERT(pRxBlk->pRxPacket);
+	pRxPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+
+	RTPKT_TO_OSPKT(pRxPkt)->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+	RTPKT_TO_OSPKT(pRxPkt)->data = pRxBlk->pData;
+	RTPKT_TO_OSPKT(pRxPkt)->len = pRxBlk->DataSize;
+	RTPKT_TO_OSPKT(pRxPkt)->tail = RTPKT_TO_OSPKT(pRxPkt)->data + RTPKT_TO_OSPKT(pRxPkt)->len;
+
+	//
+	// copy 802.3 header, if necessary
+	//
+	if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU))
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef LINUX
+			NdisMoveMemory(skb_push(pRxPkt, LENGTH_802_3), Header802_3, LENGTH_802_3);
+#endif
+#ifdef UCOS
+			NdisMoveMemory(net_pkt_push(pRxPkt, LENGTH_802_3), Header802_3, LENGTH_802_3);
+#endif
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+}
+
+
+#define INDICATE_LEGACY_OR_AMSDU(_pAd, _pRxBlk, _fromWhichBSSID)		\
+	do																	\
+	{																	\
+    	if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_AMSDU))						\
+    	{																\
+    		Indicate_AMSDU_Packet(_pAd, _pRxBlk, _fromWhichBSSID);		\
+    	}																\
+		else if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_EAP))					\
+		{																\
+			Indicate_EAPOL_Packet(_pAd, _pRxBlk, _fromWhichBSSID);		\
+		}																\
+    	else															\
+    	{																\
+    		Indicate_Legacy_Packet(_pAd, _pRxBlk, _fromWhichBSSID);		\
+    	}																\
+	} while (0);
+
+
+
+static VOID ba_enqueue_reordering_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PBA_REC_ENTRY	pBAEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	struct reordering_mpdu *mpdu_blk;
+	UINT16	Sequence = (UINT16) pRxBlk->pHeader->Sequence;
+
+	mpdu_blk = ba_mpdu_blk_alloc(pAd);
+	if (mpdu_blk != NULL)
+	{
+		// Write RxD buffer address & allocated buffer length
+		NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+
+		mpdu_blk->Sequence = Sequence;
+
+		mpdu_blk->bAMSDU = RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU);
+
+		convert_reordering_packet_to_preAMSDU_or_802_3_packet(pAd, pRxBlk, FromWhichBSSID);
+
+		STATS_INC_RX_PACKETS(pAd, FromWhichBSSID);
+
+        //
+		// it is necessary for reordering packet to record
+		// which BSS it come from
+		//
+		RTMP_SET_PACKET_IF(pRxBlk->pRxPacket, FromWhichBSSID);
+
+		mpdu_blk->pPacket = pRxBlk->pRxPacket;
+
+		if (ba_reordering_mpdu_insertsorted(&pBAEntry->list, mpdu_blk) == FALSE)
+		{
+			// had been already within reordering list
+			// don't indicate
+			RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_SUCCESS);
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+		}
+
+		ASSERT((0<= pBAEntry->list.qlen)  && (pBAEntry->list.qlen <= pBAEntry->BAWinSize));
+		NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+	}
+	else
+	{
+#if 0
+		DBGPRINT(RT_DEBUG_ERROR,  ("!!! (%d:%d) Can't allocate reordering mpdu blk\n",
+								   blk_count, pBAEntry->list.qlen));
+#else
+		DBGPRINT(RT_DEBUG_ERROR,  ("!!! (%d) Can't allocate reordering mpdu blk\n",
+								   pBAEntry->list.qlen));
+#endif
+		/*
+		 * flush all pending reordering mpdus
+		 * and receving mpdu to upper layer
+		 * make tcp/ip to take care reordering mechanism
+		 */
+		//ba_refresh_reordering_mpdus(pAd, pBAEntry);
+		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, Sequence);
+
+		pBAEntry->LastIndSeq = Sequence;
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Indicate this packet to upper layer or put it into reordering buffer
+
+	Parametrs:
+		pRxBlk         : carry necessary packet info 802.11 format
+		FromWhichBSSID : the packet received from which BSS
+
+	Return	:
+			  none
+
+	Note    :
+	          the packet queued into reordering buffer need to cover to 802.3 format
+			  or pre_AMSDU format
+	==========================================================================
+ */
+
+VOID Indicate_AMPDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	USHORT				Idx;
+	PBA_REC_ENTRY		pBAEntry = NULL;
+	UINT16				Sequence = pRxBlk->pHeader->Sequence;
+	ULONG				Now32;
+	UCHAR				Wcid = pRxBlk->pRxWI->WirelessCliID;
+	UCHAR				TID = pRxBlk->pRxWI->TID;
+
+
+	if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU) &&  (pRxBlk->DataSize > MAX_RX_PKT_LEN))
+	{
+#if 0 // sample take off, no use
+		static int err_size;
+
+		err_size++;
+		if (err_size > 20) {
+			 printk("AMPDU DataSize = %d\n", pRxBlk->DataSize);
+			 hex_dump("802.11 Header", (UCHAR *)pRxBlk->pHeader, 24);
+			 hex_dump("Payload", pRxBlk->pData, 64);
+			 err_size = 0;
+		}
+#endif
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+
+#if 0 // test
+	/* Rec BA Session had been torn down */
+	INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+	return;
+#endif
+
+	if (Wcid < MAX_LEN_OF_MAC_TABLE)
+	{
+		Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+		if (Idx == 0)
+		{
+			/* Rec BA Session had been torn down */
+			INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+			return;
+		}
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+	}
+	else
+	{
+		// impossible !!!
+		ASSERT(0);
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	ASSERT(pBAEntry);
+
+	// update last rx time
+	NdisGetSystemUpTime(&Now32);
+
+	pBAEntry->rcvSeq = Sequence;
+
+
+	ba_flush_reordering_timeout_mpdus(pAd, pBAEntry, Now32);
+	pBAEntry->LastIndSeqAtTimer = Now32;
+
+	//
+	// Reset Last Indicate Sequence
+	//
+	if (pBAEntry->LastIndSeq == RESET_RCV_SEQ)
+	{
+		ASSERT((pBAEntry->list.qlen == 0) && (pBAEntry->list.next == NULL));
+
+		// reset rcv sequence of BA session
+		pBAEntry->LastIndSeq = Sequence;
+		pBAEntry->LastIndSeqAtTimer = Now32;
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+		return;
+	}
+
+
+	//
+	// I. Check if in order.
+	//
+	if (SEQ_STEPONE(Sequence, pBAEntry->LastIndSeq, MAXSEQ))
+	{
+		USHORT  LastIndSeq;
+
+		pBAEntry->LastIndSeq = Sequence;
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+ 		LastIndSeq = ba_indicate_reordering_mpdus_in_order(pAd, pBAEntry, pBAEntry->LastIndSeq);
+		if (LastIndSeq != RESET_RCV_SEQ)
+		{
+			pBAEntry->LastIndSeq = LastIndSeq;
+		}
+		pBAEntry->LastIndSeqAtTimer = Now32;
+	}
+	//
+	// II. Drop Duplicated Packet
+	//
+	else if (Sequence == pBAEntry->LastIndSeq)
+	{
+
+		// drop and release packet
+		pBAEntry->nDropPacket++;
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	}
+	//
+	// III. Drop Old Received Packet
+	//
+	else if (SEQ_SMALLER(Sequence, pBAEntry->LastIndSeq, MAXSEQ))
+	{
+
+		// drop and release packet
+		pBAEntry->nDropPacket++;
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	}
+	//
+	// IV. Receive Sequence within Window Size
+	//
+	else if (SEQ_SMALLER(Sequence, (((pBAEntry->LastIndSeq+pBAEntry->BAWinSize+1)) & MAXSEQ), MAXSEQ))
+	{
+		ba_enqueue_reordering_packet(pAd, pBAEntry, pRxBlk, FromWhichBSSID);
+	}
+	//
+	// V. Receive seq surpasses Win(lastseq + nMSDU). So refresh all reorder buffer
+	//
+	else
+	{
+#if 0
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+#else
+		LONG WinStartSeq, TmpSeq;
+
+
+		TmpSeq = Sequence - (pBAEntry->BAWinSize) -1;
+		if (TmpSeq < 0)
+		{
+			TmpSeq = (MAXSEQ+1) + TmpSeq;
+		}
+		WinStartSeq = (TmpSeq+1) & MAXSEQ;
+		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, WinStartSeq);
+		pBAEntry->LastIndSeq = WinStartSeq; //TmpSeq;
+
+		pBAEntry->LastIndSeqAtTimer = Now32;
+
+		ba_enqueue_reordering_packet(pAd, pBAEntry, pRxBlk, FromWhichBSSID);
+
+		TmpSeq = ba_indicate_reordering_mpdus_in_order(pAd, pBAEntry, pBAEntry->LastIndSeq);
+		if (TmpSeq != RESET_RCV_SEQ)
+		{
+			pBAEntry->LastIndSeq = TmpSeq;
+		}
+#endif
+	}
+}
+
+#endif // DOT11_N_SUPPORT //
+
diff --git a/drivers/staging/rt2860/common/cmm_data.c b/drivers/staging/rt2860/common/cmm_data.c
new file mode 100644
index 000000000000..b67b9eba7229
--- /dev/null
+++ b/drivers/staging/rt2860/common/cmm_data.c
@@ -0,0 +1,3466 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+
+#include "../rt_config.h"
+
+#define MAX_TX_IN_TBTT		(16)
+
+
+UCHAR	SNAP_802_1H[] = {0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00};
+UCHAR	SNAP_BRIDGE_TUNNEL[] = {0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8};
+// Add Cisco Aironet SNAP heade for CCX2 support
+UCHAR	SNAP_AIRONET[] = {0xaa, 0xaa, 0x03, 0x00, 0x40, 0x96, 0x00, 0x00};
+UCHAR	CKIP_LLC_SNAP[] = {0xaa, 0xaa, 0x03, 0x00, 0x40, 0x96, 0x00, 0x02};
+UCHAR	EAPOL_LLC_SNAP[]= {0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00, 0x88, 0x8e};
+UCHAR	EAPOL[] = {0x88, 0x8e};
+UCHAR   TPID[] = {0x81, 0x00}; /* VLAN related */
+
+UCHAR	IPX[] = {0x81, 0x37};
+UCHAR	APPLE_TALK[] = {0x80, 0xf3};
+UCHAR	RateIdToPlcpSignal[12] = {
+	 0, /* RATE_1 */	1, /* RATE_2 */ 	2, /* RATE_5_5 */	3, /* RATE_11 */	// see BBP spec
+	11, /* RATE_6 */   15, /* RATE_9 */    10, /* RATE_12 */   14, /* RATE_18 */	// see IEEE802.11a-1999 p.14
+	 9, /* RATE_24 */  13, /* RATE_36 */	8, /* RATE_48 */   12  /* RATE_54 */ }; // see IEEE802.11a-1999 p.14
+
+UCHAR	 OfdmSignalToRateId[16] = {
+	RATE_54,  RATE_54,	RATE_54,  RATE_54,	// OFDM PLCP Signal = 0,  1,  2,  3 respectively
+	RATE_54,  RATE_54,	RATE_54,  RATE_54,	// OFDM PLCP Signal = 4,  5,  6,  7 respectively
+	RATE_48,  RATE_24,	RATE_12,  RATE_6,	// OFDM PLCP Signal = 8,  9,  10, 11 respectively
+	RATE_54,  RATE_36,	RATE_18,  RATE_9,	// OFDM PLCP Signal = 12, 13, 14, 15 respectively
+};
+
+UCHAR	 OfdmRateToRxwiMCS[12] = {
+	0,  0,	0,  0,
+	0,  1,	2,  3,	// OFDM rate 6,9,12,18 = rxwi mcs 0,1,2,3
+	4,  5,	6,  7,	// OFDM rate 24,36,48,54 = rxwi mcs 4,5,6,7
+};
+UCHAR	 RxwiMCSToOfdmRate[12] = {
+	RATE_6,  RATE_9,	RATE_12,  RATE_18,
+	RATE_24,  RATE_36,	RATE_48,  RATE_54,	// OFDM rate 6,9,12,18 = rxwi mcs 0,1,2,3
+	4,  5,	6,  7,	// OFDM rate 24,36,48,54 = rxwi mcs 4,5,6,7
+};
+
+char*   MCSToMbps[] = {"1Mbps","2Mbps","5.5Mbps","11Mbps","06Mbps","09Mbps","12Mbps","18Mbps","24Mbps","36Mbps","48Mbps","54Mbps","MM-0","MM-1","MM-2","MM-3","MM-4","MM-5","MM-6","MM-7","MM-8","MM-9","MM-10","MM-11","MM-12","MM-13","MM-14","MM-15","MM-32","ee1","ee2","ee3"};
+
+UCHAR default_cwmin[]={CW_MIN_IN_BITS, CW_MIN_IN_BITS, CW_MIN_IN_BITS-1, CW_MIN_IN_BITS-2};
+UCHAR default_sta_aifsn[]={3,7,2,2};
+
+UCHAR MapUserPriorityToAccessCategory[8] = {QID_AC_BE, QID_AC_BK, QID_AC_BK, QID_AC_BE, QID_AC_VI, QID_AC_VI, QID_AC_VO, QID_AC_VO};
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		API for MLME to transmit management frame to AP (BSS Mode)
+	or station (IBSS Mode)
+
+	Arguments:
+		pAd Pointer to our adapter
+		pData		Pointer to the outgoing 802.11 frame
+		Length		Size of outgoing management frame
+
+	Return Value:
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_PENDING
+		NDIS_STATUS_SUCCESS
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS MiniportMMRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	PUCHAR			pData,
+	IN	UINT			Length)
+{
+	PNDIS_PACKET	pPacket;
+	NDIS_STATUS  	Status = NDIS_STATUS_SUCCESS;
+	ULONG	 		FreeNum;
+#ifdef RT2860
+	unsigned long	IrqFlags = 0;
+#endif // RT2860 //
+	UCHAR			IrqState;
+	UCHAR			rtmpHwHdr[TXINFO_SIZE + TXWI_SIZE]; //RTMP_HW_HDR_LEN];
+
+	ASSERT(Length <= MGMT_DMA_BUFFER_SIZE);
+
+	QueIdx=3;
+
+	// 2860C use Tx Ring
+
+	IrqState = pAd->irq_disabled;
+#ifdef RT2860
+	if ((pAd->MACVersion == 0x28600100) && (!IrqState))
+		RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
+#endif // RT2860 //
+
+	do
+	{
+		// Reset is in progress, stop immediately
+		if ( RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+			 RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST)||
+			 !RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP))
+		{
+			Status = NDIS_STATUS_FAILURE;
+			break;
+		}
+
+		// Check Free priority queue
+		// Since we use PBF Queue2 for management frame.  Its corresponding DMA ring should be using TxRing.
+
+		// 2860C use Tx Ring
+		if (pAd->MACVersion == 0x28600100)
+		{
+			FreeNum = GET_TXRING_FREENO(pAd, QueIdx);
+		}
+		else
+		{
+			FreeNum = GET_MGMTRING_FREENO(pAd);
+		}
+
+		if ((FreeNum > 0))
+		{
+			// We need to reserve space for rtmp hardware header. i.e., TxWI for RT2860 and TxInfo+TxWI for RT2870
+			NdisZeroMemory(&rtmpHwHdr, (TXINFO_SIZE + TXWI_SIZE));
+			Status = RTMPAllocateNdisPacket(pAd, &pPacket, (PUCHAR)&rtmpHwHdr, (TXINFO_SIZE + TXWI_SIZE), pData, Length);
+			if (Status != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("MiniportMMRequest (error:: can't allocate NDIS PACKET)\n"));
+				break;
+			}
+
+			//pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+			//pAd->CommonCfg.MlmeRate = RATE_2;
+
+
+			Status = MlmeHardTransmit(pAd, QueIdx, pPacket);
+			if (Status != NDIS_STATUS_SUCCESS)
+				RTMPFreeNdisPacket(pAd, pPacket);
+		}
+		else
+		{
+			pAd->RalinkCounters.MgmtRingFullCount++;
+			DBGPRINT(RT_DEBUG_ERROR, ("Qidx(%d), not enough space in MgmtRing, MgmtRingFullCount=%ld!\n",
+										QueIdx, pAd->RalinkCounters.MgmtRingFullCount));
+		}
+
+	} while (FALSE);
+
+#ifdef RT2860
+	// 2860C use Tx Ring
+	if ((pAd->MACVersion == 0x28600100) && (!IrqState))
+		RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+#endif // RT2860 //
+
+	return Status;
+}
+
+
+#ifdef RT2860
+NDIS_STATUS MiniportMMRequestUnlock(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	PUCHAR			pData,
+	IN	UINT			Length)
+{
+	PNDIS_PACKET	pPacket;
+	NDIS_STATUS  Status = NDIS_STATUS_SUCCESS;
+	ULONG	 FreeNum;
+	TXWI_STRUC		TXWI;
+	ULONG	SW_TX_IDX;
+	PTXD_STRUC		pTxD;
+
+	QueIdx = 3;
+	ASSERT(Length <= MGMT_DMA_BUFFER_SIZE);
+
+	do
+	{
+		// Reset is in progress, stop immediately
+		if ( RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+			 RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST)||
+			 !RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP))
+		{
+			Status = NDIS_STATUS_FAILURE;
+			break;
+		}
+
+		// Check Free priority queue
+		// Since we use PBF Queue2 for management frame.  Its corresponding DMA ring should be using TxRing.
+		// 2860C use Tx Ring
+		if (pAd->MACVersion == 0x28600100)
+		{
+			FreeNum = GET_TXRING_FREENO(pAd, QueIdx);
+			SW_TX_IDX = pAd->TxRing[QueIdx].TxCpuIdx;
+			pTxD  = (PTXD_STRUC) pAd->TxRing[QueIdx].Cell[SW_TX_IDX].AllocVa;
+		}
+		else
+		{
+			FreeNum = GET_MGMTRING_FREENO(pAd);
+			SW_TX_IDX = pAd->MgmtRing.TxCpuIdx;
+			pTxD  = (PTXD_STRUC) pAd->MgmtRing.Cell[SW_TX_IDX].AllocVa;
+		}
+		if ((FreeNum > 0))
+		{
+			NdisZeroMemory(&TXWI, TXWI_SIZE);
+			Status = RTMPAllocateNdisPacket(pAd, &pPacket, (PUCHAR)&TXWI, TXWI_SIZE, pData, Length);
+			if (Status != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("MiniportMMRequest (error:: can't allocate NDIS PACKET)\n"));
+				break;
+			}
+
+			Status = MlmeHardTransmit(pAd, QueIdx, pPacket);
+			if (Status != NDIS_STATUS_SUCCESS)
+				RTMPFreeNdisPacket(pAd, pPacket);
+		}
+		else
+		{
+			pAd->RalinkCounters.MgmtRingFullCount++;
+			DBGPRINT(RT_DEBUG_ERROR, ("Qidx(%d), not enough space in MgmtRing\n", QueIdx));
+		}
+
+	} while (FALSE);
+
+
+	return Status;
+}
+#endif // RT2860 //
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware transmit function
+
+	Arguments:
+		pAd Pointer to our adapter
+		pBuffer 	Pointer to	memory of outgoing frame
+		Length		Size of outgoing management frame
+
+	Return Value:
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_PENDING
+		NDIS_STATUS_SUCCESS
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS MlmeHardTransmit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	PNDIS_PACKET	pPacket)
+{
+	if (pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE)
+	{
+		return NDIS_STATUS_FAILURE;
+	}
+
+#ifdef RT2860
+	if ( pAd->MACVersion == 0x28600100 )
+		return MlmeHardTransmitTxRing(pAd,QueIdx,pPacket);
+	else
+#endif // RT2860 //
+		return MlmeHardTransmitMgmtRing(pAd,QueIdx,pPacket);
+
+}
+
+
+#ifdef RT2860
+NDIS_STATUS MlmeHardTransmitTxRing(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	QueIdx,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	PTXD_STRUC		pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	PHEADER_802_11	pHeader_802_11;
+	BOOLEAN 		bAckRequired, bInsertTimestamp;
+	ULONG			SrcBufPA;
+	UCHAR			MlmeRate;
+	ULONG			SwIdx = pAd->TxRing[QueIdx].TxCpuIdx;
+	PTXWI_STRUC 	pFirstTxWI;
+	ULONG	 FreeNum;
+	MAC_TABLE_ENTRY	*pMacEntry = NULL;
+
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+
+	if (pSrcBufVA == NULL)
+	{
+		// The buffer shouldn't be NULL
+		return NDIS_STATUS_FAILURE;
+	}
+
+	// Make sure MGMT ring resource won't be used by other threads
+	//NdisAcquireSpinLock(&pAd->TxRingLock);
+
+	FreeNum = GET_TXRING_FREENO(pAd, QueIdx);
+
+	if (FreeNum == 0)
+	{
+		//NdisReleaseSpinLock(&pAd->TxRingLock);
+		return NDIS_STATUS_FAILURE;
+	}
+
+	SwIdx = pAd->TxRing[QueIdx].TxCpuIdx;
+
+#ifndef RT_BIG_ENDIAN
+	pTxD  = (PTXD_STRUC) pAd->TxRing[QueIdx].Cell[SwIdx].AllocVa;
+#else
+    pDestTxD  = (PTXD_STRUC)pAd->TxRing[QueIdx].Cell[SwIdx].AllocVa;
+    TxD = *pDestTxD;
+    pTxD = &TxD;
+    RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+
+	if (pAd->TxRing[QueIdx].Cell[SwIdx].pNdisPacket)
+	{
+		printk("MlmeHardTransmit Error\n");
+		return NDIS_STATUS_FAILURE;
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// outgoing frame always wakeup PHY to prevent frame lost
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+			AsicForceWakeup(pAd, TRUE);
+	}
+#endif // CONFIG_STA_SUPPORT //
+	pFirstTxWI	=(PTXWI_STRUC)pSrcBufVA;
+
+	pHeader_802_11 = (PHEADER_802_11) (pSrcBufVA + TXWI_SIZE);
+	if (pHeader_802_11->Addr1[0] & 0x01)
+	{
+		MlmeRate = pAd->CommonCfg.BasicMlmeRate;
+	}
+	else
+	{
+		MlmeRate = pAd->CommonCfg.MlmeRate;
+	}
+
+	if ((pHeader_802_11->FC.Type == BTYPE_DATA) &&
+		(pHeader_802_11->FC.SubType == SUBTYPE_QOS_NULL))
+	{
+		pMacEntry = MacTableLookup(pAd, pHeader_802_11->Addr1);
+	}
+
+	// Verify Mlme rate for a / g bands.
+	if ((pAd->LatchRfRegs.Channel > 14) && (MlmeRate < RATE_6)) // 11A band
+		MlmeRate = RATE_6;
+
+	//
+	// Should not be hard code to set PwrMgmt to 0 (PWR_ACTIVE)
+	// Snice it's been set to 0 while on MgtMacHeaderInit
+	// By the way this will cause frame to be send on PWR_SAVE failed.
+	//
+	//
+	// In WMM-UAPSD, mlme frame should be set psm as power saving but probe request frame
+#ifdef CONFIG_STA_SUPPORT
+    // Data-Null packets alse pass through MMRequest in RT2860, however, we hope control the psm bit to pass APSD
+	if (pHeader_802_11->FC.Type != BTYPE_DATA)
+    {
+    	if ((pHeader_802_11->FC.SubType == SUBTYPE_PROBE_REQ) || !(pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable))
+    	{
+    		pHeader_802_11->FC.PwrMgmt = PWR_ACTIVE;
+    	}
+    	else
+    	{
+    		pHeader_802_11->FC.PwrMgmt = pAd->CommonCfg.bAPSDForcePowerSave;
+    	}
+    }
+#endif // CONFIG_STA_SUPPORT //
+
+	bInsertTimestamp = FALSE;
+	if (pHeader_802_11->FC.Type == BTYPE_CNTL) // must be PS-POLL
+	{
+		bAckRequired = FALSE;
+	}
+	else // BTYPE_MGMT or BTYPE_DATA(must be NULL frame)
+	{
+		if (pHeader_802_11->Addr1[0] & 0x01) // MULTICAST, BROADCAST
+		{
+			bAckRequired = FALSE;
+			pHeader_802_11->Duration = 0;
+		}
+		else
+		{
+			bAckRequired = TRUE;
+			pHeader_802_11->Duration = RTMPCalcDuration(pAd, MlmeRate, 14);
+			if (pHeader_802_11->FC.SubType == SUBTYPE_PROBE_RSP)
+			{
+				bInsertTimestamp = TRUE;
+			}
+		}
+	}
+	pHeader_802_11->Sequence = pAd->Sequence++;
+	if (pAd->Sequence > 0xfff)
+		pAd->Sequence = 0;
+	// Before radar detection done, mgmt frame can not be sent but probe req
+	// Because we need to use probe req to trigger driver to send probe req in passive scan
+	if ((pHeader_802_11->FC.SubType != SUBTYPE_PROBE_REQ)
+		&& (pAd->CommonCfg.bIEEE80211H == 1)
+		&& (pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("MlmeHardTransmit --> radar detect not in normal mode !!!\n"));
+		return (NDIS_STATUS_FAILURE);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pHeader_802_11, DIR_WRITE, FALSE);
+#endif
+	//
+	// fill scatter-and-gather buffer list into TXD. Internally created NDIS PACKET
+	// should always has only one ohysical buffer, and the whole frame size equals
+	// to the first scatter buffer size
+	//
+
+	// Initialize TX Descriptor
+	// For inter-frame gap, the number is for this frame and next frame
+	// For MLME rate, we will fix as 2Mb to match other vendor's implement
+
+// management frame doesn't need encryption. so use RESERVED_WCID no matter u are sending to specific wcid or not.
+	// Only beacon use Nseq=TRUE. So here we use Nseq=FALSE.
+	if (pMacEntry == NULL)
+	{
+		RTMPWriteTxWI(pAd, pFirstTxWI, FALSE, FALSE, bInsertTimestamp, FALSE, bAckRequired, FALSE,
+		0, RESERVED_WCID, (SrcBufLen - TXWI_SIZE), PID_MGMT, 0,  (UCHAR)pAd->CommonCfg.MlmeTransmit.field.MCS, IFS_BACKOFF, FALSE, &pAd->CommonCfg.MlmeTransmit);
+	}
+	else
+	{
+		RTMPWriteTxWI(pAd, pFirstTxWI, FALSE, FALSE,
+					bInsertTimestamp, FALSE, bAckRequired, FALSE,
+					0, pMacEntry->Aid, (SrcBufLen - TXWI_SIZE),
+					pMacEntry->MaxHTPhyMode.field.MCS, 0,
+					(UCHAR)pMacEntry->MaxHTPhyMode.field.MCS,
+					IFS_BACKOFF, FALSE, &pMacEntry->MaxHTPhyMode);
+	}
+
+	pAd->TxRing[QueIdx].Cell[SwIdx].pNdisPacket = pPacket;
+	pAd->TxRing[QueIdx].Cell[SwIdx].pNextNdisPacket = NULL;
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)pFirstTxWI, TYPE_TXWI);
+#endif
+	SrcBufPA = PCI_MAP_SINGLE(pAd, pSrcBufVA, SrcBufLen, 0, PCI_DMA_TODEVICE);
+
+
+	RTMPWriteTxDescriptor(pAd, pTxD, TRUE, FIFO_EDCA);
+	pTxD->LastSec0 = 1;
+	pTxD->LastSec1 = 1;
+	pTxD->SDLen0 = SrcBufLen;
+	pTxD->SDLen1 = 0;
+	pTxD->SDPtr0 = SrcBufPA;
+	pTxD->DMADONE = 0;
+
+#ifdef RT_BIG_ENDIAN
+    RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+    WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif
+
+	pAd->RalinkCounters.KickTxCount++;
+	pAd->RalinkCounters.OneSecTxDoneCount++;
+
+   	// Increase TX_CTX_IDX, but write to register later.
+	INC_RING_INDEX(pAd->TxRing[QueIdx].TxCpuIdx, TX_RING_SIZE);
+
+	RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QueIdx*0x10,  pAd->TxRing[QueIdx].TxCpuIdx);
+
+	return NDIS_STATUS_SUCCESS;
+}
+#endif // RT2860 //
+
+
+NDIS_STATUS MlmeHardTransmitMgmtRing(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	QueIdx,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	PHEADER_802_11	pHeader_802_11;
+	BOOLEAN 		bAckRequired, bInsertTimestamp;
+	UCHAR			MlmeRate;
+	PTXWI_STRUC 	pFirstTxWI;
+	MAC_TABLE_ENTRY	*pMacEntry = NULL;
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+		RTMP_SEM_LOCK(&pAd->MgmtRingLock);
+
+
+	if (pSrcBufVA == NULL)
+	{
+		RTMP_SEM_UNLOCK(&pAd->MgmtRingLock);
+		return NDIS_STATUS_FAILURE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// outgoing frame always wakeup PHY to prevent frame lost
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+			AsicForceWakeup(pAd, TRUE);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	pFirstTxWI = (PTXWI_STRUC)(pSrcBufVA +  TXINFO_SIZE);
+	pHeader_802_11 = (PHEADER_802_11) (pSrcBufVA + TXINFO_SIZE + TXWI_SIZE); //TXWI_SIZE);
+
+	if (pHeader_802_11->Addr1[0] & 0x01)
+	{
+		MlmeRate = pAd->CommonCfg.BasicMlmeRate;
+	}
+	else
+	{
+		MlmeRate = pAd->CommonCfg.MlmeRate;
+	}
+
+	// Verify Mlme rate for a / g bands.
+	if ((pAd->LatchRfRegs.Channel > 14) && (MlmeRate < RATE_6)) // 11A band
+		MlmeRate = RATE_6;
+
+	if ((pHeader_802_11->FC.Type == BTYPE_DATA) &&
+		(pHeader_802_11->FC.SubType == SUBTYPE_QOS_NULL))
+	{
+		pMacEntry = MacTableLookup(pAd, pHeader_802_11->Addr1);
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Fixed W52 with Activity scan issue in ABG_MIXED and ABGN_MIXED mode.
+		if (pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED
+#ifdef DOT11_N_SUPPORT
+			|| pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED
+#endif // DOT11_N_SUPPORT //
+		)
+		{
+			if (pAd->LatchRfRegs.Channel > 14)
+				pAd->CommonCfg.MlmeTransmit.field.MODE = 1;
+			else
+				pAd->CommonCfg.MlmeTransmit.field.MODE = 0;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	//
+	// Should not be hard code to set PwrMgmt to 0 (PWR_ACTIVE)
+	// Snice it's been set to 0 while on MgtMacHeaderInit
+	// By the way this will cause frame to be send on PWR_SAVE failed.
+	//
+	// pHeader_802_11->FC.PwrMgmt = 0; // (pAd->StaCfg.Psm == PWR_SAVE);
+	//
+	// In WMM-UAPSD, mlme frame should be set psm as power saving but probe request frame
+#ifdef CONFIG_STA_SUPPORT
+    // Data-Null packets alse pass through MMRequest in RT2860, however, we hope control the psm bit to pass APSD
+	if ((pHeader_802_11->FC.Type != BTYPE_DATA) && (pHeader_802_11->FC.Type != BTYPE_CNTL))
+	{
+		if ((pAd->StaCfg.Psm == PWR_SAVE) &&
+			(pHeader_802_11->FC.SubType == SUBTYPE_ACTION))
+			pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+		else
+			pHeader_802_11->FC.PwrMgmt = PWR_ACTIVE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	bInsertTimestamp = FALSE;
+	if (pHeader_802_11->FC.Type == BTYPE_CNTL) // must be PS-POLL
+	{
+#ifdef CONFIG_STA_SUPPORT
+		//Set PM bit in ps-poll, to fix WLK 1.2  PowerSaveMode_ext failure issue.
+		if ((pAd->OpMode == OPMODE_STA) && (pHeader_802_11->FC.SubType == SUBTYPE_PS_POLL))
+		{
+			pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+		}
+#endif // CONFIG_STA_SUPPORT //
+		bAckRequired = FALSE;
+	}
+	else // BTYPE_MGMT or BTYPE_DATA(must be NULL frame)
+	{
+		if (pHeader_802_11->Addr1[0] & 0x01) // MULTICAST, BROADCAST
+		{
+			bAckRequired = FALSE;
+			pHeader_802_11->Duration = 0;
+		}
+		else
+		{
+			bAckRequired = TRUE;
+			pHeader_802_11->Duration = RTMPCalcDuration(pAd, MlmeRate, 14);
+			if (pHeader_802_11->FC.SubType == SUBTYPE_PROBE_RSP)
+			{
+				bInsertTimestamp = TRUE;
+			}
+		}
+	}
+
+	pHeader_802_11->Sequence = pAd->Sequence++;
+	if (pAd->Sequence >0xfff)
+		pAd->Sequence = 0;
+
+	// Before radar detection done, mgmt frame can not be sent but probe req
+	// Because we need to use probe req to trigger driver to send probe req in passive scan
+	if ((pHeader_802_11->FC.SubType != SUBTYPE_PROBE_REQ)
+		&& (pAd->CommonCfg.bIEEE80211H == 1)
+		&& (pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("MlmeHardTransmit --> radar detect not in normal mode !!!\n"));
+		RTMP_SEM_UNLOCK(&pAd->MgmtRingLock);
+		return (NDIS_STATUS_FAILURE);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pHeader_802_11, DIR_WRITE, FALSE);
+#endif
+
+	//
+	// fill scatter-and-gather buffer list into TXD. Internally created NDIS PACKET
+	// should always has only one ohysical buffer, and the whole frame size equals
+	// to the first scatter buffer size
+	//
+
+	// Initialize TX Descriptor
+	// For inter-frame gap, the number is for this frame and next frame
+	// For MLME rate, we will fix as 2Mb to match other vendor's implement
+
+// management frame doesn't need encryption. so use RESERVED_WCID no matter u are sending to specific wcid or not.
+	if (pMacEntry == NULL)
+	{
+		RTMPWriteTxWI(pAd, pFirstTxWI, FALSE, FALSE, bInsertTimestamp, FALSE, bAckRequired, FALSE,
+		0, RESERVED_WCID, (SrcBufLen - TXINFO_SIZE - TXWI_SIZE), PID_MGMT, 0,  (UCHAR)pAd->CommonCfg.MlmeTransmit.field.MCS, IFS_BACKOFF, FALSE, &pAd->CommonCfg.MlmeTransmit);
+	}
+	else
+	{
+		RTMPWriteTxWI(pAd, pFirstTxWI, FALSE, FALSE,
+					bInsertTimestamp, FALSE, bAckRequired, FALSE,
+					0, pMacEntry->Aid, (SrcBufLen - TXINFO_SIZE - TXWI_SIZE),
+					pMacEntry->MaxHTPhyMode.field.MCS, 0,
+					(UCHAR)pMacEntry->MaxHTPhyMode.field.MCS,
+					IFS_BACKOFF, FALSE, &pMacEntry->MaxHTPhyMode);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)pFirstTxWI, TYPE_TXWI);
+#endif
+
+	// Now do hardware-depened kick out.
+	HAL_KickOutMgmtTx(pAd, QueIdx, pPacket, pSrcBufVA, SrcBufLen);
+
+	// Make sure to release MGMT ring resource
+	RTMP_SEM_UNLOCK(&pAd->MgmtRingLock);
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+/********************************************************************************
+
+	New DeQueue Procedures.
+
+ ********************************************************************************/
+
+#define DEQUEUE_LOCK(lock, bIntContext, IrqFlags) 				\
+			do{													\
+				if (bIntContext == FALSE)						\
+				RTMP_IRQ_LOCK((lock), IrqFlags);		\
+			}while(0)
+
+#define DEQUEUE_UNLOCK(lock, bIntContext, IrqFlags)				\
+			do{													\
+				if (bIntContext == FALSE)						\
+					RTMP_IRQ_UNLOCK((lock), IrqFlags);	\
+			}while(0)
+
+/*
+	========================================================================
+	Tx Path design algorithm:
+		Basically, we divide the packets into four types, Broadcast/Multicast, 11N Rate(AMPDU, AMSDU, Normal), B/G Rate(ARALINK, Normal),
+		Specific Packet Type. Following show the classification rule and policy for each kinds of packets.
+				Classification Rule=>
+					Multicast: (*addr1 & 0x01) == 0x01
+					Specific : bDHCPFrame, bARPFrame, bEAPOLFrame, etc.
+					11N Rate : If peer support HT
+								(1).AMPDU  -- If TXBA is negotiated.
+								(2).AMSDU  -- If AMSDU is capable for both peer and ourself.
+											*). AMSDU can embedded in a AMPDU, but now we didn't support it.
+								(3).Normal -- Other packets which send as 11n rate.
+
+					B/G Rate : If peer is b/g only.
+								(1).ARALINK-- If both of peer/us supprot Ralink proprietary Aggregation and the TxRate is large than RATE_6
+								(2).Normal -- Other packets which send as b/g rate.
+					Fragment:
+								The packet must be unicast, NOT A-RALINK, NOT A-MSDU, NOT 11n, then can consider about fragment.
+
+				Classified Packet Handle Rule=>
+					Multicast:
+								No ACK, 		//pTxBlk->bAckRequired = FALSE;
+								No WMM, 		//pTxBlk->bWMM = FALSE;
+								No piggyback,   //pTxBlk->bPiggyBack = FALSE;
+								Force LowRate,  //pTxBlk->bForceLowRate = TRUE;
+					Specific :	Basically, for specific packet, we should handle it specifically, but now all specific packets are use
+									the same policy to handle it.
+								Force LowRate,  //pTxBlk->bForceLowRate = TRUE;
+
+					11N Rate :
+								No piggyback,	//pTxBlk->bPiggyBack = FALSE;
+
+								(1).AMSDU
+									pTxBlk->bWMM = TRUE;
+								(2).AMPDU
+									pTxBlk->bWMM = TRUE;
+								(3).Normal
+
+					B/G Rate :
+								(1).ARALINK
+
+								(2).Normal
+	========================================================================
+*/
+static UCHAR TxPktClassification(
+	IN RTMP_ADAPTER *pAd,
+	IN PNDIS_PACKET  pPacket)
+{
+	UCHAR			TxFrameType = TX_UNKOWN_FRAME;
+	UCHAR			Wcid;
+	MAC_TABLE_ENTRY	*pMacEntry = NULL;
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN			bHTRate = FALSE;
+#endif // DOT11_N_SUPPORT //
+
+	Wcid = RTMP_GET_PACKET_WCID(pPacket);
+	if (Wcid == MCAST_WCID)
+	{	// Handle for RA is Broadcast/Multicast Address.
+		return TX_MCAST_FRAME;
+	}
+
+	// Handle for unicast packets
+	pMacEntry = &pAd->MacTab.Content[Wcid];
+	if (RTMP_GET_PACKET_LOWRATE(pPacket))
+	{	// It's a specific packet need to force low rate, i.e., bDHCPFrame, bEAPOLFrame, bWAIFrame
+		TxFrameType = TX_LEGACY_FRAME;
+	}
+#ifdef DOT11_N_SUPPORT
+	else if (IS_HT_RATE(pMacEntry))
+	{	// it's a 11n capable packet
+
+		// Depends on HTPhyMode to check if the peer support the HTRate transmission.
+		// 	Currently didn't support A-MSDU embedded in A-MPDU
+		bHTRate = TRUE;
+		if (RTMP_GET_PACKET_MOREDATA(pPacket) || (pMacEntry->PsMode == PWR_SAVE))
+			TxFrameType = TX_LEGACY_FRAME;
+#ifdef UAPSD_AP_SUPPORT
+		else if (RTMP_GET_PACKET_EOSP(pPacket))
+			TxFrameType = TX_LEGACY_FRAME;
+#endif // UAPSD_AP_SUPPORT //
+		else if((pMacEntry->TXBAbitmap & (1<<(RTMP_GET_PACKET_UP(pPacket)))) != 0)
+			return TX_AMPDU_FRAME;
+		else if(CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_AMSDU_INUSED))
+			return TX_AMSDU_FRAME;
+		else
+			TxFrameType = TX_LEGACY_FRAME;
+	}
+#endif // DOT11_N_SUPPORT //
+	else
+	{	// it's a legacy b/g packet.
+		if ((CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_AGGREGATION_CAPABLE) && pAd->CommonCfg.bAggregationCapable) &&
+			(RTMP_GET_PACKET_TXRATE(pPacket) >= RATE_6) &&
+			(!(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) && CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_WMM_CAPABLE))))
+		{	// if peer support Ralink Aggregation, we use it.
+			TxFrameType = TX_RALINK_FRAME;
+		}
+		else
+		{
+			TxFrameType = TX_LEGACY_FRAME;
+		}
+	}
+
+	// Currently, our fragment only support when a unicast packet send as NOT-ARALINK, NOT-AMSDU and NOT-AMPDU.
+	if ((RTMP_GET_PACKET_FRAGMENTS(pPacket) > 1) && (TxFrameType == TX_LEGACY_FRAME))
+		TxFrameType = TX_FRAG_FRAME;
+
+	return TxFrameType;
+}
+
+
+BOOLEAN RTMP_FillTxBlkInfo(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK *pTxBlk)
+{
+	PACKET_INFO			PacketInfo;
+	PNDIS_PACKET		pPacket;
+	PMAC_TABLE_ENTRY	pMacEntry = NULL;
+
+	pPacket = pTxBlk->pPacket;
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pTxBlk->pSrcBufHeader, &pTxBlk->SrcBufLen);
+
+	pTxBlk->Wcid	 	 		= RTMP_GET_PACKET_WCID(pPacket);
+	pTxBlk->apidx		 		= RTMP_GET_PACKET_IF(pPacket);
+	pTxBlk->UserPriority 		= RTMP_GET_PACKET_UP(pPacket);
+	pTxBlk->FrameGap = IFS_HTTXOP;		// ASIC determine Frame Gap
+
+	if (RTMP_GET_PACKET_CLEAR_EAP_FRAME(pTxBlk->pPacket))
+		TX_BLK_SET_FLAG(pTxBlk, fTX_bClearEAPFrame);
+	else
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bClearEAPFrame);
+
+	// Default to clear this flag
+	TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bForceNonQoS);
+
+
+	if (pTxBlk->Wcid == MCAST_WCID)
+	{
+		pTxBlk->pMacEntry = NULL;
+		{
+#ifdef MCAST_RATE_SPECIFIC
+			PUCHAR pDA = GET_OS_PKT_DATAPTR(pPacket);
+			if (((*pDA & 0x01) == 0x01) && (*pDA != 0xff))
+				pTxBlk->pTransmit = &pAd->CommonCfg.MCastPhyMode;
+			else
+#endif // MCAST_RATE_SPECIFIC //
+				pTxBlk->pTransmit = &pAd->MacTab.Content[MCAST_WCID].HTPhyMode;
+		}
+
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bAckRequired);	// AckRequired = FALSE, when broadcast packet in Adhoc mode.
+		//TX_BLK_SET_FLAG(pTxBlk, fTX_bForceLowRate);
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bAllowFrag);
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bWMM);
+		if (RTMP_GET_PACKET_MOREDATA(pPacket))
+		{
+			TX_BLK_SET_FLAG(pTxBlk, fTX_bMoreData);
+		}
+
+	}
+	else
+	{
+		pTxBlk->pMacEntry = &pAd->MacTab.Content[pTxBlk->Wcid];
+		pTxBlk->pTransmit = &pTxBlk->pMacEntry->HTPhyMode;
+
+		pMacEntry = pTxBlk->pMacEntry;
+
+
+		// For all unicast packets, need Ack unless the Ack Policy is not set as NORMAL_ACK.
+		if (pAd->CommonCfg.AckPolicy[pTxBlk->QueIdx] != NORMAL_ACK)
+			TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bAckRequired);
+		else
+			TX_BLK_SET_FLAG(pTxBlk, fTX_bAckRequired);
+
+		{
+
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			{
+
+				// If support WMM, enable it.
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED))
+					TX_BLK_SET_FLAG(pTxBlk, fTX_bWMM);
+			}
+#endif // CONFIG_STA_SUPPORT //
+		}
+
+		if (pTxBlk->TxFrameType == TX_LEGACY_FRAME)
+		{
+			if ( (RTMP_GET_PACKET_LOWRATE(pPacket)) ||
+                ((pAd->OpMode == OPMODE_AP) && (pMacEntry->MaxHTPhyMode.field.MODE == MODE_CCK) && (pMacEntry->MaxHTPhyMode.field.MCS == RATE_1)))
+			{	// Specific packet, i.e., bDHCPFrame, bEAPOLFrame, bWAIFrame, need force low rate.
+				pTxBlk->pTransmit = &pAd->MacTab.Content[MCAST_WCID].HTPhyMode;
+#ifdef DOT11_N_SUPPORT
+				// Modify the WMM bit for ICV issue. If we have a packet with EOSP field need to set as 1, how to handle it???
+				if (IS_HT_STA(pTxBlk->pMacEntry) &&
+					(CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_RALINK_CHIPSET)) &&
+					((pAd->CommonCfg.bRdg == TRUE) && CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_RDG_CAPABLE)))
+				{
+					TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bWMM);
+					TX_BLK_SET_FLAG(pTxBlk, fTX_bForceNonQoS);
+				}
+#endif // DOT11_N_SUPPORT //
+			}
+
+#ifdef DOT11_N_SUPPORT
+			if ( (IS_HT_RATE(pMacEntry) == FALSE) &&
+				(CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_PIGGYBACK_CAPABLE)))
+			{	// Currently piggy-back only support when peer is operate in b/g mode.
+				TX_BLK_SET_FLAG(pTxBlk, fTX_bPiggyBack);
+			}
+#endif // DOT11_N_SUPPORT //
+
+			if (RTMP_GET_PACKET_MOREDATA(pPacket))
+			{
+				TX_BLK_SET_FLAG(pTxBlk, fTX_bMoreData);
+			}
+#ifdef UAPSD_AP_SUPPORT
+			if (RTMP_GET_PACKET_EOSP(pPacket))
+			{
+				TX_BLK_SET_FLAG(pTxBlk, fTX_bWMM_UAPSD_EOSP);
+			}
+#endif // UAPSD_AP_SUPPORT //
+		}
+		else if (pTxBlk->TxFrameType == TX_FRAG_FRAME)
+		{
+			TX_BLK_SET_FLAG(pTxBlk, fTX_bAllowFrag);
+		}
+
+		pMacEntry->DebugTxCount++;
+	}
+
+	return TRUE;
+}
+
+
+BOOLEAN CanDoAggregateTransmit(
+	IN RTMP_ADAPTER *pAd,
+	IN NDIS_PACKET *pPacket,
+	IN TX_BLK		*pTxBlk)
+{
+
+	//printk("Check if can do aggregation! TxFrameType=%d!\n", pTxBlk->TxFrameType);
+
+	if (RTMP_GET_PACKET_WCID(pPacket) == MCAST_WCID)
+		return FALSE;
+
+	if (RTMP_GET_PACKET_DHCP(pPacket) ||
+		RTMP_GET_PACKET_EAPOL(pPacket) ||
+		RTMP_GET_PACKET_WAI(pPacket))
+		return FALSE;
+
+	if ((pTxBlk->TxFrameType == TX_AMSDU_FRAME) &&
+		((pTxBlk->TotalFrameLen + GET_OS_PKT_LEN(pPacket))> (RX_BUFFER_AGGRESIZE - 100)))
+	{	// For AMSDU, allow the packets with total length < max-amsdu size
+		return FALSE;
+	}
+
+	if ((pTxBlk->TxFrameType == TX_RALINK_FRAME) &&
+		(pTxBlk->TxPacketList.Number == 2))
+	{	// For RALINK-Aggregation, allow two frames in one batch.
+		return FALSE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	if ((INFRA_ON(pAd)) && (pAd->OpMode == OPMODE_STA)) // must be unicast to AP
+		return TRUE;
+	else
+#endif // CONFIG_STA_SUPPORT //
+		return FALSE;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		To do the enqueue operation and extract the first item of waiting
+		list. If a number of available shared memory segments could meet
+		the request of extracted item, the extracted item will be fragmented
+		into shared memory segments.
+
+	Arguments:
+		pAd Pointer to our adapter
+		pQueue		Pointer to Waiting Queue
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPDeQueuePacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  BOOLEAN         bIntContext,
+	IN  UCHAR			QIdx, /* BulkOutPipeId */
+	IN  UCHAR           Max_Tx_Packets)
+{
+	PQUEUE_ENTRY    pEntry = NULL;
+	PNDIS_PACKET 	pPacket;
+	NDIS_STATUS     Status = NDIS_STATUS_SUCCESS;
+	UCHAR           Count=0;
+	PQUEUE_HEADER   pQueue;
+	ULONG           FreeNumber[NUM_OF_TX_RING];
+	UCHAR			QueIdx, sQIdx, eQIdx;
+	unsigned long	IrqFlags = 0;
+	BOOLEAN			hasTxDesc = FALSE;
+	TX_BLK			TxBlk;
+	TX_BLK			*pTxBlk;
+
+#ifdef DBG_DIAGNOSE
+	BOOLEAN			firstRound;
+	RtmpDiagStruct	*pDiagStruct = &pAd->DiagStruct;
+#endif
+
+
+	if (QIdx == NUM_OF_TX_RING)
+	{
+		sQIdx = 0;
+		eQIdx = 3;	// 4 ACs, start from 0.
+	}
+	else
+	{
+		sQIdx = eQIdx = QIdx;
+	}
+
+	for (QueIdx=sQIdx; QueIdx <= eQIdx; QueIdx++)
+	{
+		Count=0;
+
+		RT28XX_START_DEQUEUE(pAd, QueIdx, IrqFlags);
+
+#ifdef DBG_DIAGNOSE
+		firstRound = ((QueIdx == 0) ? TRUE : FALSE);
+#endif // DBG_DIAGNOSE //
+
+		while (1)
+		{
+			if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS |
+										fRTMP_ADAPTER_RADIO_OFF |
+										fRTMP_ADAPTER_RESET_IN_PROGRESS |
+										fRTMP_ADAPTER_HALT_IN_PROGRESS |
+										fRTMP_ADAPTER_NIC_NOT_EXIST))))
+			{
+				RT28XX_STOP_DEQUEUE(pAd, QueIdx, IrqFlags);
+				return;
+			}
+
+			if (Count >= Max_Tx_Packets)
+				break;
+
+			DEQUEUE_LOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+			if (&pAd->TxSwQueue[QueIdx] == NULL)
+			{
+#ifdef DBG_DIAGNOSE
+				if (firstRound == TRUE)
+					pDiagStruct->TxSWQueCnt[pDiagStruct->ArrayCurIdx][0]++;
+#endif // DBG_DIAGNOSE //
+				DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+				break;
+			}
+
+#ifdef RT2860
+			FreeNumber[QueIdx] = GET_TXRING_FREENO(pAd, QueIdx);
+
+#ifdef DBG_DIAGNOSE
+			if (firstRound == TRUE)
+			{
+				UCHAR	txDescNumLevel, txSwQNumLevel;
+
+				txDescNumLevel = (TX_RING_SIZE - FreeNumber[QueIdx]); // Number of occupied hw desc.
+				txDescNumLevel = ((txDescNumLevel <=15) ? txDescNumLevel : 15);
+				pDiagStruct->TxDescCnt[pDiagStruct->ArrayCurIdx][txDescNumLevel]++;
+
+				txSwQNumLevel = ((pAd->TxSwQueue[QueIdx].Number <=7) ? pAd->TxSwQueue[QueIdx].Number : 8);
+				pDiagStruct->TxSWQueCnt[pDiagStruct->ArrayCurIdx][txSwQNumLevel]++;
+
+				firstRound = FALSE;
+			}
+#endif // DBG_DIAGNOSE //
+
+			if (FreeNumber[QueIdx] <= 5)
+			{
+				// free Tx(QueIdx) resources
+				RTMPFreeTXDUponTxDmaDone(pAd, QueIdx);
+				FreeNumber[QueIdx] = GET_TXRING_FREENO(pAd, QueIdx);
+			}
+#endif // RT2860 //
+
+			// probe the Queue Head
+			pQueue = &pAd->TxSwQueue[QueIdx];
+			if ((pEntry = pQueue->Head) == NULL)
+			{
+				DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+				break;
+			}
+
+			pTxBlk = &TxBlk;
+			NdisZeroMemory((PUCHAR)pTxBlk, sizeof(TX_BLK));
+			pTxBlk->QueIdx = QueIdx;
+
+			pPacket = QUEUE_ENTRY_TO_PKT(pEntry);
+
+			// Early check to make sure we have enoguh Tx Resource.
+			hasTxDesc = RT28XX_HAS_ENOUGH_FREE_DESC(pAd, pTxBlk, FreeNumber[QueIdx], pPacket);
+			if (!hasTxDesc)
+			{
+				pAd->PrivateInfo.TxRingFullCnt++;
+
+				DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+
+				break;
+			}
+
+			pTxBlk->TxFrameType = TxPktClassification(pAd, pPacket);
+			pEntry = RemoveHeadQueue(pQueue);
+			pTxBlk->TotalFrameNum++;
+			pTxBlk->TotalFragNum += RTMP_GET_PACKET_FRAGMENTS(pPacket);	// The real fragment number maybe vary
+			pTxBlk->TotalFrameLen += GET_OS_PKT_LEN(pPacket);
+			pTxBlk->pPacket = pPacket;
+			InsertTailQueue(&pTxBlk->TxPacketList, PACKET_TO_QUEUE_ENTRY(pPacket));
+
+			if (pTxBlk->TxFrameType == TX_RALINK_FRAME || pTxBlk->TxFrameType == TX_AMSDU_FRAME)
+			{
+				// Enhance SW Aggregation Mechanism
+				if (NEED_QUEUE_BACK_FOR_AGG(pAd, QueIdx, FreeNumber[QueIdx], pTxBlk->TxFrameType))
+				{
+					InsertHeadQueue(pQueue, PACKET_TO_QUEUE_ENTRY(pPacket));
+					DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+					break;
+				}
+
+				do{
+					if((pEntry = pQueue->Head) == NULL)
+						break;
+
+					// For TX_AMSDU_FRAME/TX_RALINK_FRAME, Need to check if next pakcet can do aggregation.
+					pPacket = QUEUE_ENTRY_TO_PKT(pEntry);
+					FreeNumber[QueIdx] = GET_TXRING_FREENO(pAd, QueIdx);
+					hasTxDesc = RT28XX_HAS_ENOUGH_FREE_DESC(pAd, pTxBlk, FreeNumber[QueIdx], pPacket);
+					if ((hasTxDesc == FALSE) || (CanDoAggregateTransmit(pAd, pPacket, pTxBlk) == FALSE))
+						break;
+
+					//Remove the packet from the TxSwQueue and insert into pTxBlk
+					pEntry = RemoveHeadQueue(pQueue);
+					ASSERT(pEntry);
+					pPacket = QUEUE_ENTRY_TO_PKT(pEntry);
+					pTxBlk->TotalFrameNum++;
+					pTxBlk->TotalFragNum += RTMP_GET_PACKET_FRAGMENTS(pPacket);	// The real fragment number maybe vary
+					pTxBlk->TotalFrameLen += GET_OS_PKT_LEN(pPacket);
+					InsertTailQueue(&pTxBlk->TxPacketList, PACKET_TO_QUEUE_ENTRY(pPacket));
+				}while(1);
+
+				if (pTxBlk->TxPacketList.Number == 1)
+					pTxBlk->TxFrameType = TX_LEGACY_FRAME;
+			}
+
+
+			Count += pTxBlk->TxPacketList.Number;
+
+				// Do HardTransmit now.
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				Status = STAHardTransmit(pAd, pTxBlk, QueIdx);
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RT2860
+			DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+			// static rate also need NICUpdateFifoStaCounters() function.
+			//if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED))
+				NICUpdateFifoStaCounters(pAd);
+#endif // RT2860 //
+		}
+
+		RT28XX_STOP_DEQUEUE(pAd, QueIdx, IrqFlags);
+
+
+#ifdef BLOCK_NET_IF
+		if ((pAd->blockQueueTab[QueIdx].SwTxQueueBlockFlag == TRUE)
+			&& (pAd->TxSwQueue[QueIdx].Number < 1))
+		{
+			releaseNetIf(&pAd->blockQueueTab[QueIdx]);
+		}
+#endif // BLOCK_NET_IF //
+
+	}
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calculates the duration which is required to transmit out frames
+	with given size and specified rate.
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		Rate			Transmit rate
+		Size			Frame size in units of byte
+
+	Return Value:
+		Duration number in units of usec
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+USHORT	RTMPCalcDuration(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Rate,
+	IN	ULONG			Size)
+{
+	ULONG	Duration = 0;
+
+	if (Rate < RATE_FIRST_OFDM_RATE) // CCK
+	{
+		if ((Rate > RATE_1) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED))
+			Duration = 96;	// 72+24 preamble+plcp
+		else
+			Duration = 192; // 144+48 preamble+plcp
+
+		Duration += (USHORT)((Size << 4) / RateIdTo500Kbps[Rate]);
+		if ((Size << 4) % RateIdTo500Kbps[Rate])
+			Duration ++;
+	}
+	else if (Rate <= RATE_LAST_OFDM_RATE)// OFDM rates
+	{
+		Duration = 20 + 6;		// 16+4 preamble+plcp + Signal Extension
+		Duration += 4 * (USHORT)((11 + Size * 4) / RateIdTo500Kbps[Rate]);
+		if ((11 + Size * 4) % RateIdTo500Kbps[Rate])
+			Duration += 4;
+	}
+	else	//mimo rate
+	{
+		Duration = 20 + 6;		// 16+4 preamble+plcp + Signal Extension
+	}
+
+	return (USHORT)Duration;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calculates the duration which is required to transmit out frames
+	with given size and specified rate.
+
+	Arguments:
+		pTxWI		Pointer to head of each MPDU to HW.
+		Ack 		Setting for Ack requirement bit
+		Fragment	Setting for Fragment bit
+		RetryMode	Setting for retry mode
+		Ifs 		Setting for IFS gap
+		Rate		Setting for transmit rate
+		Service 	Setting for service
+		Length		Frame length
+		TxPreamble	Short or Long preamble when using CCK rates
+		QueIdx - 0-3, according to 802.11e/d4.4 June/2003
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+    See also : BASmartHardTransmit()    !!!
+
+	========================================================================
+*/
+VOID RTMPWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC 	pOutTxWI,
+	IN	BOOLEAN			FRAG,
+	IN	BOOLEAN			CFACK,
+	IN	BOOLEAN			InsTimestamp,
+	IN	BOOLEAN 		AMPDU,
+	IN	BOOLEAN 		Ack,
+	IN	BOOLEAN 		NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN	UCHAR 			PID,
+	IN	UCHAR			TID,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	*pTransmit)
+{
+	PMAC_TABLE_ENTRY	pMac = NULL;
+	TXWI_STRUC 		TxWI;
+	PTXWI_STRUC 	pTxWI;
+
+	if (WCID < MAX_LEN_OF_MAC_TABLE)
+		pMac = &pAd->MacTab.Content[WCID];
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+	NdisZeroMemory(&TxWI, TXWI_SIZE);
+	pTxWI = &TxWI;
+
+	pTxWI->FRAG= FRAG;
+
+	pTxWI->CFACK = CFACK;
+	pTxWI->TS= InsTimestamp;
+	pTxWI->AMPDU = AMPDU;
+	pTxWI->ACK = Ack;
+	pTxWI->txop= Txopmode;
+
+	pTxWI->NSEQ = NSeq;
+	// John tune the performace with Intel Client in 20 MHz performance
+#ifdef DOT11_N_SUPPORT
+	BASize = pAd->CommonCfg.TxBASize;
+
+	if( BASize >7 )
+		BASize =7;
+	pTxWI->BAWinSize = BASize;
+	pTxWI->ShortGI = pTransmit->field.ShortGI;
+	pTxWI->STBC = pTransmit->field.STBC;
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->WirelessCliID = WCID;
+	pTxWI->MPDUtotalByteCount = Length;
+	pTxWI->PacketId = PID;
+
+	// If CCK or OFDM, BW must be 20
+	pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+#ifdef DOT11N_DRAFT3
+	if (pTxWI->BW)
+		pTxWI->BW = (pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth == 0) ? (BW_20) : (pTransmit->field.BW);
+#endif // DOT11N_DRAFT3 //
+
+	pTxWI->MCS = pTransmit->field.MCS;
+	pTxWI->PHYMODE = pTransmit->field.MODE;
+	pTxWI->CFACK = CfAck;
+
+#ifdef DOT11_N_SUPPORT
+	if (pMac)
+	{
+		if (pAd->CommonCfg.bMIMOPSEnable)
+		{
+			if ((pMac->MmpsMode == MMPS_DYNAMIC) && (pTransmit->field.MCS > 7))
+			{
+				// Dynamic MIMO Power Save Mode
+				pTxWI->MIMOps = 1;
+			}
+			else if (pMac->MmpsMode == MMPS_STATIC)
+			{
+				// Static MIMO Power Save Mode
+				if (pTransmit->field.MODE >= MODE_HTMIX && pTransmit->field.MCS > 7)
+				{
+					pTxWI->MCS = 7;
+					pTxWI->MIMOps = 0;
+				}
+			}
+		}
+		//pTxWI->MIMOps = (pMac->PsMode == PWR_MMPS)? 1:0;
+		if (pMac->bIAmBadAtheros && (pMac->WepStatus != Ndis802_11WEPDisabled))
+		{
+			pTxWI->MpduDensity = 7;
+		}
+		else
+		{
+			pTxWI->MpduDensity = pMac->MpduDensity;
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->PacketId = pTxWI->MCS;
+	NdisMoveMemory(pOutTxWI, &TxWI, sizeof(TXWI_STRUC));
+}
+
+
+VOID RTMPWriteTxWI_Data(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk)
+{
+	HTTRANSMIT_SETTING	*pTransmit;
+	PMAC_TABLE_ENTRY	pMacEntry;
+#ifdef DOT11_N_SUPPORT
+	UCHAR				BASize;
+#endif // DOT11_N_SUPPORT //
+
+
+	ASSERT(pTxWI);
+
+	pTransmit = pTxBlk->pTransmit;
+	pMacEntry = pTxBlk->pMacEntry;
+
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+	NdisZeroMemory(pTxWI, TXWI_SIZE);
+
+	pTxWI->FRAG		= TX_BLK_TEST_FLAG(pTxBlk, fTX_bAllowFrag);
+	pTxWI->ACK		= TX_BLK_TEST_FLAG(pTxBlk, fTX_bAckRequired);
+	pTxWI->txop		= pTxBlk->FrameGap;
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+	if (pMacEntry &&
+		(pAd->StaCfg.BssType == BSS_INFRA) &&
+		(pMacEntry->ValidAsDls == TRUE))
+		pTxWI->WirelessCliID = BSSID_WCID;
+	else
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+		pTxWI->WirelessCliID		= pTxBlk->Wcid;
+
+	pTxWI->MPDUtotalByteCount	= pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+	pTxWI->CFACK				= TX_BLK_TEST_FLAG(pTxBlk, fTX_bPiggyBack);
+
+	// If CCK or OFDM, BW must be 20
+	pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	if (pTxWI->BW)
+		pTxWI->BW = (pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth == 0) ? (BW_20) : (pTransmit->field.BW);
+#endif // DOT11N_DRAFT3 //
+	pTxWI->AMPDU	= ((pTxBlk->TxFrameType == TX_AMPDU_FRAME) ? TRUE : FALSE);
+
+	// John tune the performace with Intel Client in 20 MHz performance
+	BASize = pAd->CommonCfg.TxBASize;
+	if((pTxBlk->TxFrameType == TX_AMPDU_FRAME) && (pMacEntry))
+	{
+		UCHAR		RABAOriIdx = 0;	//The RA's BA Originator table index.
+
+		RABAOriIdx = pTxBlk->pMacEntry->BAOriWcidArray[pTxBlk->UserPriority];
+		BASize = pAd->BATable.BAOriEntry[RABAOriIdx].BAWinSize;
+	}
+
+	pTxWI->TxBF = pTransmit->field.TxBF;
+	pTxWI->BAWinSize = BASize;
+	pTxWI->ShortGI = pTransmit->field.ShortGI;
+	pTxWI->STBC = pTransmit->field.STBC;
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->MCS = pTransmit->field.MCS;
+	pTxWI->PHYMODE = pTransmit->field.MODE;
+
+#ifdef DOT11_N_SUPPORT
+	if (pMacEntry)
+	{
+		if ((pMacEntry->MmpsMode == MMPS_DYNAMIC) && (pTransmit->field.MCS > 7))
+		{
+			// Dynamic MIMO Power Save Mode
+			pTxWI->MIMOps = 1;
+		}
+		else if (pMacEntry->MmpsMode == MMPS_STATIC)
+		{
+			// Static MIMO Power Save Mode
+			if (pTransmit->field.MODE >= MODE_HTMIX && pTransmit->field.MCS > 7)
+			{
+				pTxWI->MCS = 7;
+				pTxWI->MIMOps = 0;
+			}
+		}
+
+		if (pMacEntry->bIAmBadAtheros && (pMacEntry->WepStatus != Ndis802_11WEPDisabled))
+		{
+			pTxWI->MpduDensity = 7;
+		}
+		else
+		{
+			pTxWI->MpduDensity = pMacEntry->MpduDensity;
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DBG_DIAGNOSE
+		if (pTxBlk->QueIdx== 0)
+		{
+			pAd->DiagStruct.TxDataCnt[pAd->DiagStruct.ArrayCurIdx]++;
+			pAd->DiagStruct.TxMcsCnt[pAd->DiagStruct.ArrayCurIdx][pTxWI->MCS]++;
+		}
+#endif // DBG_DIAGNOSE //
+
+	// for rate adapation
+	pTxWI->PacketId = pTxWI->MCS;
+}
+
+
+VOID RTMPWriteTxWI_Cache(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk)
+{
+	PHTTRANSMIT_SETTING	pTransmit;
+	PMAC_TABLE_ENTRY	pMacEntry;
+
+	//
+	// update TXWI
+	//
+	pMacEntry = pTxBlk->pMacEntry;
+	pTransmit = pTxBlk->pTransmit;
+
+	if (pMacEntry->bAutoTxRateSwitch)
+	{
+		pTxWI->txop = IFS_HTTXOP;
+
+		// If CCK or OFDM, BW must be 20
+		pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+		pTxWI->ShortGI = pTransmit->field.ShortGI;
+		pTxWI->STBC = pTransmit->field.STBC;
+
+		pTxWI->MCS = pTransmit->field.MCS;
+		pTxWI->PHYMODE = pTransmit->field.MODE;
+
+		// set PID for TxRateSwitching
+		pTxWI->PacketId = pTransmit->field.MCS;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	pTxWI->AMPDU = ((pMacEntry->NoBADataCountDown == 0) ? TRUE: FALSE);
+	pTxWI->MIMOps = 0;
+
+#ifdef DOT11N_DRAFT3
+	if (pTxWI->BW)
+		pTxWI->BW = (pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth == 0) ? (BW_20) : (pTransmit->field.BW);
+#endif // DOT11N_DRAFT3 //
+
+	if (pAd->CommonCfg.bMIMOPSEnable)
+	{
+		// MIMO Power Save Mode
+		if ((pMacEntry->MmpsMode == MMPS_DYNAMIC) && (pTransmit->field.MCS > 7))
+		{
+			// Dynamic MIMO Power Save Mode
+			pTxWI->MIMOps = 1;
+		}
+		else if (pMacEntry->MmpsMode == MMPS_STATIC)
+		{
+			// Static MIMO Power Save Mode
+			if ((pTransmit->field.MODE >= MODE_HTMIX) && (pTransmit->field.MCS > 7))
+			{
+				pTxWI->MCS = 7;
+				pTxWI->MIMOps = 0;
+			}
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DBG_DIAGNOSE
+	if (pTxBlk->QueIdx== 0)
+	{
+		pAd->DiagStruct.TxDataCnt[pAd->DiagStruct.ArrayCurIdx]++;
+		pAd->DiagStruct.TxMcsCnt[pAd->DiagStruct.ArrayCurIdx][pTxWI->MCS]++;
+	}
+#endif // DBG_DIAGNOSE //
+
+	pTxWI->MPDUtotalByteCount = pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calculates the duration which is required to transmit out frames
+	with given size and specified rate.
+
+	Arguments:
+		pTxD		Pointer to transmit descriptor
+		Ack 		Setting for Ack requirement bit
+		Fragment	Setting for Fragment bit
+		RetryMode	Setting for retry mode
+		Ifs 		Setting for IFS gap
+		Rate		Setting for transmit rate
+		Service 	Setting for service
+		Length		Frame length
+		TxPreamble	Short or Long preamble when using CCK rates
+		QueIdx - 0-3, according to 802.11e/d4.4 June/2003
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID RTMPWriteTxDescriptor(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXD_STRUC		pTxD,
+	IN	BOOLEAN 		bWIV,
+	IN	UCHAR			QueueSEL)
+{
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+
+	pTxD->WIV	= (bWIV) ? 1: 0;
+	pTxD->QSEL= (QueueSEL);
+	if (pAd->bGenOneHCCA == TRUE)
+		pTxD->QSEL= FIFO_HCCA;
+	pTxD->DMADONE = 0;
+}
+
+
+// should be called only when -
+// 1. MEADIA_CONNECTED
+// 2. AGGREGATION_IN_USED
+// 3. Fragmentation not in used
+// 4. either no previous frame (pPrevAddr1=NULL) .OR. previoud frame is aggregatible
+BOOLEAN TxFrameIsAggregatible(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pPrevAddr1,
+	IN	PUCHAR			p8023hdr)
+{
+
+	// can't aggregate EAPOL (802.1x) frame
+	if ((p8023hdr[12] == 0x88) && (p8023hdr[13] == 0x8e))
+		return FALSE;
+
+	// can't aggregate multicast/broadcast frame
+	if (p8023hdr[0] & 0x01)
+		return FALSE;
+
+	if (INFRA_ON(pAd)) // must be unicast to AP
+		return TRUE;
+	else if ((pPrevAddr1 == NULL) || MAC_ADDR_EQUAL(pPrevAddr1, p8023hdr)) // unicast to same STA
+		return TRUE;
+	else
+		return FALSE;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+	   Check the MSDU Aggregation policy
+	1.HT aggregation is A-MSDU
+	2.legaacy rate aggregation is software aggregation by Ralink.
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN PeerIsAggreOn(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ULONG		   TxRate,
+	IN	PMAC_TABLE_ENTRY pMacEntry)
+{
+	ULONG	AFlags = (fCLIENT_STATUS_AMSDU_INUSED | fCLIENT_STATUS_AGGREGATION_CAPABLE);
+
+	if (pMacEntry != NULL && CLIENT_STATUS_TEST_FLAG(pMacEntry, AFlags))
+	{
+#ifdef DOT11_N_SUPPORT
+		if (pMacEntry->HTPhyMode.field.MODE >= MODE_HTMIX)
+		{
+			return TRUE;
+		}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef AGGREGATION_SUPPORT
+		if (TxRate >= RATE_6 && pAd->CommonCfg.bAggregationCapable && (!(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) && CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_WMM_CAPABLE))))
+		{	// legacy  Ralink Aggregation support
+			return TRUE;
+		}
+#endif // AGGREGATION_SUPPORT //
+	}
+
+	return FALSE;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Check and fine the packet waiting in SW queue with highest priority
+
+	Arguments:
+		pAd Pointer to our adapter
+
+	Return Value:
+		pQueue		Pointer to Waiting Queue
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+PQUEUE_HEADER	RTMPCheckTxSwQueue(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT PUCHAR			pQueIdx)
+{
+
+	ULONG	Number;
+
+	Number = pAd->TxSwQueue[QID_AC_BK].Number
+			 + pAd->TxSwQueue[QID_AC_BE].Number
+			 + pAd->TxSwQueue[QID_AC_VI].Number
+			 + pAd->TxSwQueue[QID_AC_VO].Number
+			 + pAd->TxSwQueue[QID_HCCA].Number;
+
+	if (pAd->TxSwQueue[QID_AC_VO].Head != NULL)
+	{
+		*pQueIdx = QID_AC_VO;
+		return (&pAd->TxSwQueue[QID_AC_VO]);
+	}
+	else if (pAd->TxSwQueue[QID_AC_VI].Head != NULL)
+	{
+		*pQueIdx = QID_AC_VI;
+		return (&pAd->TxSwQueue[QID_AC_VI]);
+	}
+	else if (pAd->TxSwQueue[QID_AC_BE].Head != NULL)
+	{
+		*pQueIdx = QID_AC_BE;
+		return (&pAd->TxSwQueue[QID_AC_BE]);
+	}
+	else if (pAd->TxSwQueue[QID_AC_BK].Head != NULL)
+	{
+		*pQueIdx = QID_AC_BK;
+		return (&pAd->TxSwQueue[QID_AC_BK]);
+	}
+	else if (pAd->TxSwQueue[QID_HCCA].Head != NULL)
+	{
+		*pQueIdx = QID_HCCA;
+		return (&pAd->TxSwQueue[QID_HCCA]);
+	}
+
+	// No packet pending in Tx Sw queue
+	*pQueIdx = QID_AC_BK;
+
+	return (NULL);
+}
+
+
+#ifdef RT2860
+BOOLEAN  RTMPFreeTXDUponTxDmaDone(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			QueIdx)
+{
+	PRTMP_TX_RING pTxRing;
+	PTXD_STRUC	  pTxD;
+#ifdef	RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+#endif
+	PNDIS_PACKET  pPacket;
+	UCHAR	FREE = 0;
+	TXD_STRUC	TxD, *pOriTxD;
+	//ULONG		IrqFlags;
+	BOOLEAN			bReschedule = FALSE;
+
+
+	ASSERT(QueIdx < NUM_OF_TX_RING);
+	pTxRing = &pAd->TxRing[QueIdx];
+
+	RTMP_IO_READ32(pAd, TX_DTX_IDX0 + QueIdx * RINGREG_DIFF, &pTxRing->TxDmaIdx);
+	while (pTxRing->TxSwFreeIdx != pTxRing->TxDmaIdx)
+	{
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+		PHEADER_802_11	pHeader80211;
+
+		if ((ATE_ON(pAd)) && (pAd->ate.bQATxStart == TRUE))
+		{
+			if (pAd->ate.QID == QueIdx)
+			{
+				pAd->ate.TxDoneCount++;
+				//pAd->ate.Repeat++;
+				pAd->RalinkCounters.KickTxCount++;
+
+				/* always use QID_AC_BE and FIFO_EDCA */
+				ASSERT(pAd->ate.QID == 0);
+				pAd->ate.TxAc0++;
+
+				FREE++;
+#ifndef RT_BIG_ENDIAN
+				pTxD = (PTXD_STRUC) (pTxRing->Cell[pTxRing->TxSwFreeIdx].AllocVa);
+				pOriTxD = pTxD;
+		        NdisMoveMemory(&TxD, pTxD, sizeof(TXD_STRUC));
+				pTxD = &TxD;
+#else
+		        pDestTxD = (PTXD_STRUC) (pTxRing->Cell[pTxRing->TxSwFreeIdx].AllocVa);
+		        pOriTxD = pDestTxD ;
+		        TxD = *pDestTxD;
+		        pTxD = &TxD;
+		        RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+				pTxD->DMADONE = 0;
+
+				pHeader80211 = pTxRing->Cell[pTxRing->TxSwFreeIdx].DmaBuf.AllocVa + sizeof(TXWI_STRUC);
+#ifdef RT_BIG_ENDIAN
+				RTMPFrameEndianChange(pAd, (PUCHAR)pHeader80211, DIR_READ, FALSE);
+#endif
+				pHeader80211->Sequence = ++pAd->ate.seq;
+#ifdef RT_BIG_ENDIAN
+				RTMPFrameEndianChange(pAd, (PUCHAR)pHeader80211, DIR_WRITE, FALSE);
+#endif
+
+				if  ((pAd->ate.bQATxStart == TRUE) && (pAd->ate.Mode & ATE_TXFRAME) && (pAd->ate.TxDoneCount < pAd->ate.TxCount))
+				{
+					pAd->RalinkCounters.TransmittedByteCount +=  (pTxD->SDLen1 + pTxD->SDLen0);
+					pAd->RalinkCounters.OneSecDmaDoneCount[QueIdx] ++;
+					INC_RING_INDEX(pTxRing->TxSwFreeIdx, TX_RING_SIZE);
+					/* get tx_tdx_idx again */
+					RTMP_IO_READ32(pAd, TX_DTX_IDX0 + QueIdx * RINGREG_DIFF ,  &pTxRing->TxDmaIdx);
+					goto kick_out;
+				}
+				else if ((pAd->ate.TxStatus == 1)/* or (pAd->ate.bQATxStart == TRUE) ??? */ && (pAd->ate.TxDoneCount == pAd->ate.TxCount))//<========================PETER
+				{
+					DBGPRINT(RT_DEBUG_TRACE,("all Tx is done\n"));
+					// Tx status enters idle mode.
+					pAd->ate.TxStatus = 0;
+				}
+				else if (!(pAd->ate.Mode & ATE_TXFRAME))
+				{
+					/* not complete sending yet, but someone press the Stop TX botton. */
+					DBGPRINT(RT_DEBUG_ERROR,("not complete sending yet, but someone pressed the Stop TX bottom\n"));
+					DBGPRINT(RT_DEBUG_ERROR,("pAd->ate.Mode = 0x%02x\n", pAd->ate.Mode));
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_OFF,("pTxRing->TxSwFreeIdx = %d\n", pTxRing->TxSwFreeIdx));
+  				}
+#ifndef RT_BIG_ENDIAN
+	        	NdisMoveMemory(pOriTxD, pTxD, sizeof(TXD_STRUC));
+#else
+        		RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+        		*pDestTxD = TxD;
+#endif // RT_BIG_ENDIAN //
+
+				INC_RING_INDEX(pTxRing->TxSwFreeIdx, TX_RING_SIZE);
+				continue;
+			}
+		}
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+		// static rate also need NICUpdateFifoStaCounters() function.
+		//if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED))
+			NICUpdateFifoStaCounters(pAd);
+
+		/* Note : If (pAd->ate.bQATxStart == TRUE), we will never reach here. */
+		FREE++;
+#ifndef RT_BIG_ENDIAN
+                pTxD = (PTXD_STRUC) (pTxRing->Cell[pTxRing->TxSwFreeIdx].AllocVa);
+		pOriTxD = pTxD;
+                NdisMoveMemory(&TxD, pTxD, sizeof(TXD_STRUC));
+		pTxD = &TxD;
+#else
+        pDestTxD = (PTXD_STRUC) (pTxRing->Cell[pTxRing->TxSwFreeIdx].AllocVa);
+        pOriTxD = pDestTxD ;
+        TxD = *pDestTxD;
+        pTxD = &TxD;
+        RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+
+		pTxD->DMADONE = 0;
+
+
+#ifdef RALINK_ATE
+		/* Execution of this block is not allowed when ATE is running. */
+		if (!(ATE_ON(pAd)))
+#endif // RALINK_ATE //
+/*====================================================================*/
+		{
+			pPacket = pTxRing->Cell[pTxRing->TxSwFreeIdx].pNdisPacket;
+			if (pPacket)
+			{
+#ifdef CONFIG_5VT_ENHANCE
+				if (RTMP_GET_PACKET_5VT(pPacket))
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, 16, PCI_DMA_TODEVICE);
+				else
+#endif // CONFIG_5VT_ENHANCE //
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNdisPacket as NULL after clear
+			pTxRing->Cell[pTxRing->TxSwFreeIdx].pNdisPacket = NULL;
+
+			pPacket = pTxRing->Cell[pTxRing->TxSwFreeIdx].pNextNdisPacket;
+
+			ASSERT(pPacket == NULL);
+			if (pPacket)
+			{
+#ifdef CONFIG_5VT_ENHANCE
+				if (RTMP_GET_PACKET_5VT(pPacket))
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, 16, PCI_DMA_TODEVICE);
+				else
+#endif // CONFIG_5VT_ENHANCE //
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNextNdisPacket as NULL after clear
+			pTxRing->Cell[pTxRing->TxSwFreeIdx].pNextNdisPacket = NULL;
+		}
+/*====================================================================*/
+
+		pAd->RalinkCounters.TransmittedByteCount +=  (pTxD->SDLen1 + pTxD->SDLen0);
+		pAd->RalinkCounters.OneSecDmaDoneCount[QueIdx] ++;
+		INC_RING_INDEX(pTxRing->TxSwFreeIdx, TX_RING_SIZE);
+		/* get tx_tdx_idx again */
+		RTMP_IO_READ32(pAd, TX_DTX_IDX0 + QueIdx * RINGREG_DIFF ,  &pTxRing->TxDmaIdx);
+#ifdef RT_BIG_ENDIAN
+        RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+        *pDestTxD = TxD;
+#else
+        NdisMoveMemory(pOriTxD, pTxD, sizeof(TXD_STRUC));
+#endif
+
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+kick_out:
+#endif // RALINK_28xx_QA //
+
+		//
+		// ATE_TXCONT mode also need to send some normal frames, so let it in.
+		// ATE_STOP must be changed not to be 0xff
+		// to prevent it from running into this block.
+		//
+		if ((pAd->ate.Mode & ATE_TXFRAME) && (pAd->ate.QID == QueIdx))
+		{
+			// TxDoneCount++ has been done if QA is used.
+			if (pAd->ate.bQATxStart == FALSE)
+			{
+				pAd->ate.TxDoneCount++;
+			}
+			if (((pAd->ate.TxCount - pAd->ate.TxDoneCount + 1) >= TX_RING_SIZE))
+			{
+				/* Note : We increase TxCpuIdx here, not TxSwFreeIdx ! */
+				INC_RING_INDEX(pAd->TxRing[QueIdx].TxCpuIdx, TX_RING_SIZE);
+#ifndef RT_BIG_ENDIAN//<==========================PETER
+				pTxD = (PTXD_STRUC) (pTxRing->Cell[pAd->TxRing[QueIdx].TxCpuIdx].AllocVa);
+				pOriTxD = pTxD;
+		        NdisMoveMemory(&TxD, pTxD, sizeof(TXD_STRUC));
+				pTxD = &TxD;
+#else
+		        pDestTxD = (PTXD_STRUC) (pTxRing->Cell[pAd->TxRing[QueIdx].TxCpuIdx].AllocVa);
+		        pOriTxD = pDestTxD ;
+		        TxD = *pDestTxD;
+		        pTxD = &TxD;
+		        RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+				pTxD->DMADONE = 0;
+#ifndef RT_BIG_ENDIAN//<==========================PETER
+        		NdisMoveMemory(pOriTxD, pTxD, sizeof(TXD_STRUC));
+#else
+        		RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+        		*pDestTxD = TxD;
+#endif
+				// kick Tx-Ring.
+				RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QueIdx * RINGREG_DIFF, pAd->TxRing[QueIdx].TxCpuIdx);
+				pAd->RalinkCounters.KickTxCount++;
+			}
+		}
+#endif // RALINK_ATE //
+	}
+
+
+	return  bReschedule;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process TX Rings DMA Done interrupt, running in DPC level
+
+	Arguments:
+		Adapter 	Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+BOOLEAN	RTMPHandleTxRingDmaDoneInterrupt(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	INT_SOURCE_CSR_STRUC TxRingBitmap)
+{
+    unsigned long	IrqFlags;
+	BOOLEAN			bReschedule = FALSE;
+
+	// Make sure Tx ring resource won't be used by other threads
+
+	RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
+
+	if (TxRingBitmap.field.Ac0DmaDone)
+		bReschedule = RTMPFreeTXDUponTxDmaDone(pAd, QID_AC_BE);
+
+	if (TxRingBitmap.field.HccaDmaDone)
+		bReschedule |= RTMPFreeTXDUponTxDmaDone(pAd, QID_HCCA);
+
+	if (TxRingBitmap.field.Ac3DmaDone)
+		bReschedule |= RTMPFreeTXDUponTxDmaDone(pAd, QID_AC_VO);
+
+	if (TxRingBitmap.field.Ac2DmaDone)
+		bReschedule |= RTMPFreeTXDUponTxDmaDone(pAd, QID_AC_VI);
+
+	if (TxRingBitmap.field.Ac1DmaDone)
+		bReschedule |= RTMPFreeTXDUponTxDmaDone(pAd, QID_AC_BK);
+
+	// Make sure to release Tx ring resource
+	RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+
+	// Dequeue outgoing frames from TxSwQueue[] and process it
+	RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+
+	return  bReschedule;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process MGMT ring DMA done interrupt, running in DPC level
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPHandleMgmtRingDmaDoneInterrupt(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PTXD_STRUC	 pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	PNDIS_PACKET pPacket;
+	UCHAR	FREE = 0;
+	PRTMP_MGMT_RING pMgmtRing = &pAd->MgmtRing;
+
+	NdisAcquireSpinLock(&pAd->MgmtRingLock);
+
+	RTMP_IO_READ32(pAd, TX_MGMTDTX_IDX, &pMgmtRing->TxDmaIdx);
+	while (pMgmtRing->TxSwFreeIdx!= pMgmtRing->TxDmaIdx)
+	{
+		FREE++;
+#ifdef RT_BIG_ENDIAN
+        pDestTxD = (PTXD_STRUC) (pMgmtRing->Cell[pAd->MgmtRing.TxSwFreeIdx].AllocVa);
+        TxD = *pDestTxD;
+        pTxD = &TxD;
+		RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#else
+		pTxD = (PTXD_STRUC) (pMgmtRing->Cell[pAd->MgmtRing.TxSwFreeIdx].AllocVa);
+#endif
+		pTxD->DMADONE = 0;
+		pPacket = pMgmtRing->Cell[pMgmtRing->TxSwFreeIdx].pNdisPacket;
+
+
+		if (pPacket)
+		{
+			PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+		}
+		pMgmtRing->Cell[pMgmtRing->TxSwFreeIdx].pNdisPacket = NULL;
+
+		pPacket = pMgmtRing->Cell[pMgmtRing->TxSwFreeIdx].pNextNdisPacket;
+		if (pPacket)
+		{
+			PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+		}
+		pMgmtRing->Cell[pMgmtRing->TxSwFreeIdx].pNextNdisPacket = NULL;
+		INC_RING_INDEX(pMgmtRing->TxSwFreeIdx, MGMT_RING_SIZE);
+
+#ifdef RT_BIG_ENDIAN
+        RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+        WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, TRUE, TYPE_TXD);
+#endif
+	}
+	NdisReleaseSpinLock(&pAd->MgmtRingLock);
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+	Arguments:
+		Adapter 	Pointer to our adapter. Dequeue all power safe delayed braodcast frames after beacon.
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPHandleTBTTInterrupt(
+	IN PRTMP_ADAPTER pAd)
+{
+	{
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+		{
+		}
+	}
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+	Arguments:
+		Adapter 	Pointer to our adapter. Rewrite beacon content before next send-out.
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPHandlePreTBTTInterrupt(
+	IN PRTMP_ADAPTER pAd)
+{
+	{
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("RTMPHandlePreTBTTInterrupt...\n"));
+		}
+	}
+
+
+}
+
+VOID	RTMPHandleRxCoherentInterrupt(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	WPDMA_GLO_CFG_STRUC	GloCfg;
+
+	if (pAd == NULL)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("====> pAd is NULL, return.\n"));
+		return;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> RTMPHandleRxCoherentInterrupt \n"));
+
+	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG , &GloCfg.word);
+
+	GloCfg.field.EnTXWriteBackDDONE = 0;
+	GloCfg.field.EnableRxDMA = 0;
+	GloCfg.field.EnableTxDMA = 0;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+	RTMPRingCleanUp(pAd, QID_AC_BE);
+	RTMPRingCleanUp(pAd, QID_AC_BK);
+	RTMPRingCleanUp(pAd, QID_AC_VI);
+	RTMPRingCleanUp(pAd, QID_AC_VO);
+	RTMPRingCleanUp(pAd, QID_HCCA);
+	RTMPRingCleanUp(pAd, QID_MGMT);
+	RTMPRingCleanUp(pAd, QID_RX);
+
+	RTMPEnableRxTx(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== RTMPHandleRxCoherentInterrupt \n"));
+}
+
+
+VOID DBGPRINT_TX_RING(
+	IN PRTMP_ADAPTER  pAd,
+	IN UCHAR          QueIdx)
+{
+	UINT32		Ac0Base;
+	UINT32		Ac0HwIdx = 0, Ac0SwIdx = 0, AC0freeIdx;
+	int			i;
+	PULONG	ptemp;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("=====================================================\n "  ));
+	switch (QueIdx)
+	{
+		case QID_AC_BE:
+			RTMP_IO_READ32(pAd, TX_BASE_PTR0, &Ac0Base);
+			RTMP_IO_READ32(pAd, TX_CTX_IDX0, &Ac0SwIdx);
+			RTMP_IO_READ32(pAd, TX_DTX_IDX0, &Ac0HwIdx);
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("All QID_AC_BE DESCRIPTOR  \n "  ));
+			for (i=0;i<TX_RING_SIZE;i++)
+			{
+				ptemp= (PULONG)pAd->TxRing[QID_AC_BE].Cell[i].AllocVa;
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("[%02d]  %08lx: %08lx: %08lx: %08lx\n " , i, *ptemp,*(ptemp+1),*(ptemp+2),*(ptemp+3)));
+			}
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("  \n "  ));
+			break;
+		case QID_AC_BK:
+			RTMP_IO_READ32(pAd, TX_BASE_PTR1, &Ac0Base);
+			RTMP_IO_READ32(pAd, TX_CTX_IDX1, &Ac0SwIdx);
+			RTMP_IO_READ32(pAd, TX_DTX_IDX1, &Ac0HwIdx);
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("All QID_AC_BK DESCRIPTOR  \n "  ));
+			for (i=0;i<TX_RING_SIZE;i++)
+			{
+				ptemp= (PULONG)pAd->TxRing[QID_AC_BK].Cell[i].AllocVa;
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("[%02d]  %08lx: %08lx: %08lx: %08lx\n " , i, *ptemp,*(ptemp+1),*(ptemp+2),*(ptemp+3)));
+			}
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("  \n "  ));
+			break;
+		case QID_AC_VI:
+			RTMP_IO_READ32(pAd, TX_BASE_PTR2, &Ac0Base);
+			RTMP_IO_READ32(pAd, TX_CTX_IDX2, &Ac0SwIdx);
+			RTMP_IO_READ32(pAd, TX_DTX_IDX2, &Ac0HwIdx);
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("All QID_AC_VI DESCRIPTOR \n "  ));
+			for (i=0;i<TX_RING_SIZE;i++)
+			{
+				ptemp= (PULONG)pAd->TxRing[QID_AC_VI].Cell[i].AllocVa;
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("[%02d]  %08lx: %08lx: %08lx: %08lx\n " , i, *ptemp,*(ptemp+1),*(ptemp+2),*(ptemp+3)));
+			}
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("  \n "  ));
+			break;
+		case QID_AC_VO:
+			RTMP_IO_READ32(pAd, TX_BASE_PTR3, &Ac0Base);
+			RTMP_IO_READ32(pAd, TX_CTX_IDX3, &Ac0SwIdx);
+			RTMP_IO_READ32(pAd, TX_DTX_IDX3, &Ac0HwIdx);
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("All QID_AC_VO DESCRIPTOR \n "  ));
+			for (i=0;i<TX_RING_SIZE;i++)
+			{
+				ptemp= (PULONG)pAd->TxRing[QID_AC_VO].Cell[i].AllocVa;
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("[%02d]  %08lx: %08lx: %08lx: %08lx\n " , i, *ptemp,*(ptemp+1),*(ptemp+2),*(ptemp+3)));
+			}
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("  \n "  ));
+			break;
+		case QID_MGMT:
+			RTMP_IO_READ32(pAd, TX_BASE_PTR5, &Ac0Base);
+			RTMP_IO_READ32(pAd, TX_CTX_IDX5, &Ac0SwIdx);
+			RTMP_IO_READ32(pAd, TX_DTX_IDX5, &Ac0HwIdx);
+			DBGPRINT_RAW(RT_DEBUG_TRACE, (" All QID_MGMT  DESCRIPTOR \n "  ));
+			for (i=0;i<MGMT_RING_SIZE;i++)
+			{
+				ptemp= (PULONG)pAd->MgmtRing.Cell[i].AllocVa;
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("[%02d]  %08lx: %08lx: %08lx: %08lx\n " , i, *ptemp,*(ptemp+1),*(ptemp+2),*(ptemp+3)));
+			}
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("  \n "  ));
+			break;
+
+		default:
+			DBGPRINT_ERR(("DBGPRINT_TX_RING(Ring %d) not supported\n", QueIdx));
+			break;
+	}
+	AC0freeIdx = pAd->TxRing[QueIdx].TxSwFreeIdx;
+
+	DBGPRINT(RT_DEBUG_TRACE,("TxRing%d, TX_DTX_IDX=%d, TX_CTX_IDX=%d\n", QueIdx, Ac0HwIdx, Ac0SwIdx));
+	DBGPRINT_RAW(RT_DEBUG_TRACE,(" 	TxSwFreeIdx[%d]", AC0freeIdx));
+	DBGPRINT_RAW(RT_DEBUG_TRACE,("	pending-NDIS=%ld\n", pAd->RalinkCounters.PendingNdisPacketCount));
+
+
+}
+
+
+VOID DBGPRINT_RX_RING(
+	IN PRTMP_ADAPTER  pAd)
+{
+	UINT32		Ac0Base;
+	UINT32		Ac0HwIdx = 0, Ac0SwIdx = 0, AC0freeIdx;
+	int			i;
+	UINT32	*ptemp;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("=====================================================\n "  ));
+	RTMP_IO_READ32(pAd, RX_BASE_PTR, &Ac0Base);
+	RTMP_IO_READ32(pAd, RX_CRX_IDX, &Ac0SwIdx);
+	RTMP_IO_READ32(pAd, RX_DRX_IDX, &Ac0HwIdx);
+	AC0freeIdx = pAd->RxRing.RxSwReadIdx;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("All RX DSP  \n "  ));
+	for (i=0;i<RX_RING_SIZE;i++)
+	{
+		ptemp = (UINT32 *)pAd->RxRing.Cell[i].AllocVa;
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("[%02d]  %08x: %08x: %08x: %08x\n " , i, *ptemp,*(ptemp+1),*(ptemp+2),*(ptemp+3)));
+	}
+	DBGPRINT(RT_DEBUG_TRACE,("RxRing, RX_DRX_IDX=%d, RX_CRX_IDX=%d \n", Ac0HwIdx, Ac0SwIdx));
+	DBGPRINT_RAW(RT_DEBUG_TRACE,(" 	RxSwReadIdx [%d]=", AC0freeIdx));
+	DBGPRINT_RAW(RT_DEBUG_TRACE,("	pending-NDIS=%ld\n", pAd->RalinkCounters.PendingNdisPacketCount));
+}
+#endif // RT2860 //
+
+/*
+	========================================================================
+
+	Routine Description:
+		Suspend MSDU transmission
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPSuspendMsduTransmission(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE,("SCANNING, suspend MSDU transmission ...\n"));
+
+
+	//
+	// Before BSS_SCAN_IN_PROGRESS, we need to keep Current R66 value and
+	// use Lowbound as R66 value on ScanNextChannel(...)
+	//
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &pAd->BbpTuning.R66CurrentValue);
+
+	// set BBP_R66 to 0x30/0x40 when scanning (AsicSwitchChannel will set R66 according to channel when scanning)
+	RTMPSetAGCInitValue(pAd, BW_20);
+
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Resume MSDU transmission
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPResumeMsduTransmission(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE,("SCAN done, resume MSDU transmission ...\n"));
+
+
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, pAd->BbpTuning.R66CurrentValue);
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+	RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+}
+
+
+UINT deaggregate_AMSDU_announce(
+	IN	PRTMP_ADAPTER	pAd,
+	PNDIS_PACKET		pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize)
+{
+	USHORT 			PayloadSize;
+	USHORT 			SubFrameSize;
+	PHEADER_802_3 	pAMSDUsubheader;
+	UINT			nMSDU;
+    UCHAR			Header802_3[14];
+
+	PUCHAR			pPayload, pDA, pSA, pRemovedLLCSNAP;
+	PNDIS_PACKET	pClonePacket;
+
+
+
+	nMSDU = 0;
+
+	while (DataSize > LENGTH_802_3)
+	{
+
+		nMSDU++;
+
+		pAMSDUsubheader = (PHEADER_802_3)pData;
+		PayloadSize = pAMSDUsubheader->Octet[1] + (pAMSDUsubheader->Octet[0]<<8);
+		SubFrameSize = PayloadSize + LENGTH_802_3;
+
+
+		if ((DataSize < SubFrameSize) || (PayloadSize > 1518 ))
+		{
+			break;
+		}
+
+		pPayload = pData + LENGTH_802_3;
+		pDA = pData;
+		pSA = pData + MAC_ADDR_LEN;
+
+		// convert to 802.3 header
+        CONVERT_TO_802_3(Header802_3, pDA, pSA, pPayload, PayloadSize, pRemovedLLCSNAP);
+
+#ifdef CONFIG_STA_SUPPORT
+		if ((Header802_3[12] == 0x88) && (Header802_3[13] == 0x8E) )
+		{
+		    // avoid local heap overflow, use dyanamic allocation
+		   MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+		   memmove(Elem->Msg+(LENGTH_802_11 + LENGTH_802_1_H), pPayload, PayloadSize);
+		   Elem->MsgLen = LENGTH_802_11 + LENGTH_802_1_H + PayloadSize;
+		   WpaEAPOLKeyAction(pAd, Elem);
+		   kfree(Elem);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+	        	if (pRemovedLLCSNAP)
+	        	{
+	    			pPayload -= LENGTH_802_3;
+	    			PayloadSize += LENGTH_802_3;
+	    			NdisMoveMemory(pPayload, &Header802_3[0], LENGTH_802_3);
+	        	}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		pClonePacket = ClonePacket(pAd, pPacket, pPayload, PayloadSize);
+		if (pClonePacket)
+		{
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pClonePacket, RTMP_GET_PACKET_IF(pPacket));
+#endif // CONFIG_STA_SUPPORT //
+		}
+
+
+		// A-MSDU has padding to multiple of 4 including subframe header.
+		// align SubFrameSize up to multiple of 4
+		SubFrameSize = (SubFrameSize+3)&(~0x3);
+
+
+		if (SubFrameSize > 1528 || SubFrameSize < 32)
+		{
+			break;
+		}
+
+		if (DataSize > SubFrameSize)
+		{
+			pData += SubFrameSize;
+			DataSize -= SubFrameSize;
+		}
+		else
+		{
+			// end of A-MSDU
+			DataSize = 0;
+		}
+	}
+
+	// finally release original rx packet
+	RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+
+	return nMSDU;
+}
+
+
+UINT BA_Reorder_AMSDU_Annnounce(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PUCHAR			pData;
+	USHORT			DataSize;
+	UINT			nMSDU = 0;
+
+	pData = (PUCHAR) GET_OS_PKT_DATAPTR(pPacket);
+	DataSize = (USHORT) GET_OS_PKT_LEN(pPacket);
+
+	nMSDU = deaggregate_AMSDU_announce(pAd, pPacket, pData, DataSize);
+
+	return nMSDU;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Look up the MAC address in the MAC table. Return NULL if not found.
+	Return:
+		pEntry - pointer to the MAC entry; NULL is not found
+	==========================================================================
+*/
+MAC_TABLE_ENTRY *MacTableLookup(
+	IN PRTMP_ADAPTER pAd,
+	PUCHAR pAddr)
+{
+	ULONG HashIdx;
+	MAC_TABLE_ENTRY *pEntry = NULL;
+
+	HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+	pEntry = pAd->MacTab.Hash[HashIdx];
+
+	while (pEntry && (pEntry->ValidAsCLI || pEntry->ValidAsWDS || pEntry->ValidAsApCli || pEntry->ValidAsMesh))
+	{
+		if (MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
+		{
+			break;
+		}
+		else
+			pEntry = pEntry->pNext;
+	}
+
+	return pEntry;
+}
+
+MAC_TABLE_ENTRY *MacTableInsertEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR			pAddr,
+	IN	UCHAR			apidx,
+	IN BOOLEAN	CleanAll)
+{
+	UCHAR HashIdx;
+	int i, FirstWcid;
+	MAC_TABLE_ENTRY *pEntry = NULL, *pCurrEntry;
+
+	// if FULL, return
+	if (pAd->MacTab.Size >= MAX_LEN_OF_MAC_TABLE)
+		return NULL;
+
+	FirstWcid = 1;
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	if (pAd->StaCfg.BssType == BSS_INFRA)
+		FirstWcid = 2;
+#endif // CONFIG_STA_SUPPORT //
+
+	// allocate one MAC entry
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+	for (i = FirstWcid; i< MAX_LEN_OF_MAC_TABLE; i++)   // skip entry#0 so that "entry index == AID" for fast lookup
+	{
+		// pick up the first available vacancy
+		if ((pAd->MacTab.Content[i].ValidAsCLI == FALSE) &&
+			(pAd->MacTab.Content[i].ValidAsWDS == FALSE) &&
+			(pAd->MacTab.Content[i].ValidAsApCli== FALSE) &&
+			(pAd->MacTab.Content[i].ValidAsMesh == FALSE)
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			&& (pAd->MacTab.Content[i].ValidAsDls == FALSE)
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+			)
+		{
+			pEntry = &pAd->MacTab.Content[i];
+			if (CleanAll == TRUE)
+			{
+				pEntry->MaxSupportedRate = RATE_11;
+				pEntry->CurrTxRate = RATE_11;
+				NdisZeroMemory(pEntry, sizeof(MAC_TABLE_ENTRY));
+				pEntry->PairwiseKey.KeyLen = 0;
+				pEntry->PairwiseKey.CipherAlg = CIPHER_NONE;
+			}
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			if (apidx >= MIN_NET_DEVICE_FOR_DLS)
+			{
+				pEntry->ValidAsCLI = FALSE;
+				pEntry->ValidAsWDS = FALSE;
+				pEntry->ValidAsApCli = FALSE;
+				pEntry->ValidAsMesh = FALSE;
+				pEntry->ValidAsDls = TRUE;
+				pEntry->isCached = FALSE;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+			{
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					pEntry->ValidAsCLI = TRUE;
+					pEntry->ValidAsWDS = FALSE;
+					pEntry->ValidAsApCli = FALSE;
+					pEntry->ValidAsMesh = FALSE;
+					pEntry->ValidAsDls = FALSE;
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+
+			pEntry->bIAmBadAtheros = FALSE;
+			pEntry->pAd = pAd;
+			pEntry->CMTimerRunning = FALSE;
+			pEntry->EnqueueEapolStartTimerRunning = EAPOL_START_DISABLE;
+			pEntry->RSNIE_Len = 0;
+			NdisZeroMemory(pEntry->R_Counter, sizeof(pEntry->R_Counter));
+			pEntry->ReTryCounter = PEER_MSG1_RETRY_TIMER_CTR;
+
+			if (pEntry->ValidAsMesh)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_MESH);
+			else if (pEntry->ValidAsApCli)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_APCLI);
+			else if (pEntry->ValidAsWDS)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_WDS);
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			else if (pEntry->ValidAsDls)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_DLS);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+			else
+				pEntry->apidx = apidx;
+
+			{
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					pEntry->AuthMode = pAd->StaCfg.AuthMode;
+					pEntry->WepStatus = pAd->StaCfg.WepStatus;
+					pEntry->PrivacyFilter = Ndis802_11PrivFilterAcceptAll;
+#ifdef RT2860
+					AsicRemovePairwiseKeyEntry(pAd, pEntry->apidx, (UCHAR)i);
+#endif // RT2860 //
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+
+			pEntry->GTKState = REKEY_NEGOTIATING;
+			pEntry->PairwiseKey.KeyLen = 0;
+			pEntry->PairwiseKey.CipherAlg = CIPHER_NONE;
+#ifdef CONFIG_STA_SUPPORT
+			if ((pAd->OpMode == OPMODE_STA) &&
+				(pAd->StaCfg.BssType == BSS_ADHOC))
+				pEntry->PortSecured = WPA_802_1X_PORT_SECURED;
+			else
+#ifdef QOS_DLS_SUPPORT
+			if (pEntry->ValidAsDls == TRUE)
+				pEntry->PortSecured = WPA_802_1X_PORT_SECURED;
+			else
+#endif //QOS_DLS_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+			pEntry->PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			pEntry->PMKID_CacheIdx = ENTRY_NOT_FOUND;
+			COPY_MAC_ADDR(pEntry->Addr, pAddr);
+			pEntry->Sst = SST_NOT_AUTH;
+			pEntry->AuthState = AS_NOT_AUTH;
+			pEntry->Aid = (USHORT)i;  //0;
+			pEntry->CapabilityInfo = 0;
+			pEntry->PsMode = PWR_ACTIVE;
+			pEntry->PsQIdleCount = 0;
+			pEntry->NoDataIdleCount = 0;
+			pEntry->ContinueTxFailCnt = 0;
+			InitializeQueueHeader(&pEntry->PsQueue);
+
+
+			pAd->MacTab.Size ++;
+			// Add this entry into ASIC RX WCID search table
+			RT28XX_STA_ENTRY_ADD(pAd, pEntry);
+
+
+
+			DBGPRINT(RT_DEBUG_TRACE, ("MacTableInsertEntry - allocate entry #%d, Total= %d\n",i, pAd->MacTab.Size));
+			break;
+		}
+	}
+
+	// add this MAC entry into HASH table
+	if (pEntry)
+	{
+		HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+		if (pAd->MacTab.Hash[HashIdx] == NULL)
+		{
+			pAd->MacTab.Hash[HashIdx] = pEntry;
+		}
+		else
+		{
+			pCurrEntry = pAd->MacTab.Hash[HashIdx];
+			while (pCurrEntry->pNext != NULL)
+				pCurrEntry = pCurrEntry->pNext;
+			pCurrEntry->pNext = pEntry;
+		}
+	}
+
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+	return pEntry;
+}
+
+/*
+	==========================================================================
+	Description:
+		Delete a specified client from MAC table
+	==========================================================================
+ */
+BOOLEAN MacTableDeleteEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT wcid,
+	IN PUCHAR pAddr)
+{
+	USHORT HashIdx;
+	MAC_TABLE_ENTRY *pEntry, *pPrevEntry, *pProbeEntry;
+	BOOLEAN Cancelled;
+
+	if (wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+
+	HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+	pEntry = &pAd->MacTab.Content[wcid];
+
+	if (pEntry && (pEntry->ValidAsCLI || pEntry->ValidAsApCli || pEntry->ValidAsWDS || pEntry->ValidAsMesh
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+ 		|| pEntry->ValidAsDls
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+		))
+	{
+		if (MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
+		{
+
+			// Delete this entry from ASIC on-chip WCID Table
+			RT28XX_STA_ENTRY_MAC_RESET(pAd, wcid);
+
+#ifdef DOT11_N_SUPPORT
+			// free resources of BA
+			BASessionTearDownALL(pAd, pEntry->Aid);
+#endif // DOT11_N_SUPPORT //
+
+
+			pPrevEntry = NULL;
+			pProbeEntry = pAd->MacTab.Hash[HashIdx];
+			ASSERT(pProbeEntry);
+
+			// update Hash list
+			do
+			{
+				if (pProbeEntry == pEntry)
+				{
+					if (pPrevEntry == NULL)
+					{
+						pAd->MacTab.Hash[HashIdx] = pEntry->pNext;
+					}
+					else
+					{
+						pPrevEntry->pNext = pEntry->pNext;
+					}
+					break;
+				}
+
+				pPrevEntry = pProbeEntry;
+				pProbeEntry = pProbeEntry->pNext;
+			} while (pProbeEntry);
+
+			// not found !!!
+			ASSERT(pProbeEntry != NULL);
+
+			RT28XX_STA_ENTRY_KEY_DEL(pAd, BSS0, wcid);
+
+
+		if (pEntry->EnqueueEapolStartTimerRunning != EAPOL_START_DISABLE)
+		{
+			RTMPCancelTimer(&pEntry->EnqueueStartForPSKTimer, &Cancelled);
+			pEntry->EnqueueEapolStartTimerRunning = EAPOL_START_DISABLE;
+		}
+
+
+   			NdisZeroMemory(pEntry, sizeof(MAC_TABLE_ENTRY));
+			pAd->MacTab.Size --;
+			DBGPRINT(RT_DEBUG_TRACE, ("MacTableDeleteEntry1 - Total= %d\n", pAd->MacTab.Size));
+		}
+		else
+		{
+			printk("\n%s: Impossible Wcid = %d !!!!!\n", __func__, wcid);
+		}
+	}
+
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+	//Reset operating mode when no Sta.
+	if (pAd->MacTab.Size == 0)
+	{
+#ifdef DOT11_N_SUPPORT
+		pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode = 0;
+#endif // DOT11_N_SUPPORT //
+		AsicUpdateProtect(pAd, 0 /*pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode*/, (ALLN_SETPROTECT), TRUE, 0 /*pAd->MacTab.fAnyStationNonGF*/);
+	}
+
+	return TRUE;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		This routine reset the entire MAC table. All packets pending in
+		the power-saving queues are freed here.
+	==========================================================================
+ */
+VOID MacTableReset(
+	IN  PRTMP_ADAPTER  pAd)
+{
+	int         i;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("MacTableReset\n"));
+	//NdisAcquireSpinLock(&pAd->MacTabLock);
+
+	for (i=1; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+#ifdef RT2860
+		RT28XX_STA_ENTRY_MAC_RESET(pAd, i);
+#endif // RT2860 //
+		if (pAd->MacTab.Content[i].ValidAsCLI == TRUE)
+	   {
+
+#ifdef DOT11_N_SUPPORT
+			// free resources of BA
+			BASessionTearDownALL(pAd, i);
+#endif // DOT11_N_SUPPORT //
+
+			pAd->MacTab.Content[i].ValidAsCLI = FALSE;
+
+
+
+
+			//AsicDelWcidTab(pAd, i);
+		}
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID AssocParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_ASSOC_REQ_STRUCT *AssocReq,
+	IN PUCHAR                     pAddr,
+	IN USHORT                     CapabilityInfo,
+	IN ULONG                      Timeout,
+	IN USHORT                     ListenIntv)
+{
+	COPY_MAC_ADDR(AssocReq->Addr, pAddr);
+	// Add mask to support 802.11b mode only
+	AssocReq->CapabilityInfo = CapabilityInfo & SUPPORTED_CAPABILITY_INFO; // not cf-pollable, not cf-poll-request
+	AssocReq->Timeout = Timeout;
+	AssocReq->ListenIntv = ListenIntv;
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID DisassocParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_DISASSOC_REQ_STRUCT *DisassocReq,
+	IN PUCHAR pAddr,
+	IN USHORT Reason)
+{
+	COPY_MAC_ADDR(DisassocReq->Addr, pAddr);
+	DisassocReq->Reason = Reason;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Check the out going frame, if this is an DHCP or ARP datagram
+	will be duplicate another frame at low data rate transmit.
+
+	Arguments:
+		pAd 		Pointer to our adapter
+		pPacket 	Pointer to outgoing Ndis frame
+
+	Return Value:
+		TRUE		To be duplicate at Low data rate transmit. (1mb)
+		FALSE		Do nothing.
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+		MAC header + IP Header + UDP Header
+		  14 Bytes	  20 Bytes
+
+		UDP Header
+		00|01|02|03|04|05|06|07|08|09|10|11|12|13|14|15|
+						Source Port
+		16|17|18|19|20|21|22|23|24|25|26|27|28|29|30|31|
+					Destination Port
+
+		port 0x43 means Bootstrap Protocol, server.
+		Port 0x44 means Bootstrap Protocol, client.
+
+	========================================================================
+*/
+
+BOOLEAN RTMPCheckDHCPFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	ULONG			NumberOfBytesRead = 0;
+	ULONG			CurrentOffset = 0;
+	PVOID			pVirtualAddress = NULL;
+	UINT			NdisBufferLength;
+	PUCHAR			pSrc;
+	USHORT			Protocol;
+	UCHAR			ByteOffset36 = 0;
+	UCHAR			ByteOffset38 = 0;
+	BOOLEAN 		ReadFirstParm = TRUE;
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, (PUCHAR *)&pVirtualAddress, &NdisBufferLength);
+
+	NumberOfBytesRead += NdisBufferLength;
+	pSrc = (PUCHAR) pVirtualAddress;
+	Protocol = *(pSrc + 12) * 256 + *(pSrc + 13);
+
+	//
+	// Check DHCP & BOOTP protocol
+	//
+	while (NumberOfBytesRead <= PacketInfo.TotalPacketLength)
+	{
+		if ((NumberOfBytesRead >= 35) && (ReadFirstParm == TRUE))
+		{
+			CurrentOffset = 35 - (NumberOfBytesRead - NdisBufferLength);
+			ByteOffset36 = *(pSrc + CurrentOffset);
+			ReadFirstParm = FALSE;
+		}
+
+		if (NumberOfBytesRead >= 37)
+		{
+			CurrentOffset = 37 - (NumberOfBytesRead - NdisBufferLength);
+			ByteOffset38 = *(pSrc + CurrentOffset);
+			//End of Read
+			break;
+		}
+		return FALSE;
+	}
+
+	// Check for DHCP & BOOTP protocol
+	if ((ByteOffset36 != 0x44) || (ByteOffset38 != 0x43))
+		{
+		//
+		// 2054 (hex 0806) for ARP datagrams
+		// if this packet is not ARP datagrams, then do nothing
+		// ARP datagrams will also be duplicate at 1mb broadcast frames
+		//
+		if (Protocol != 0x0806 )
+			return FALSE;
+		}
+
+	return TRUE;
+}
+
+
+BOOLEAN RTMPCheckEtherType(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	USHORT	TypeLen;
+	UCHAR	Byte0, Byte1;
+	PUCHAR	pSrcBuf;
+	UINT32	pktLen;
+	UINT16 	srcPort, dstPort;
+	BOOLEAN	status = TRUE;
+
+
+	pSrcBuf = GET_OS_PKT_DATAPTR(pPacket);
+	pktLen = GET_OS_PKT_LEN(pPacket);
+
+	ASSERT(pSrcBuf);
+
+	RTMP_SET_PACKET_SPECIFIC(pPacket, 0);
+
+	// get Ethernet protocol field
+	TypeLen = (pSrcBuf[12] << 8) + pSrcBuf[13];
+
+	pSrcBuf += LENGTH_802_3;	// Skip the Ethernet Header.
+
+	if (TypeLen <= 1500)
+	{	// 802.3, 802.3 LLC
+		/*
+			DestMAC(6) + SrcMAC(6) + Lenght(2) +
+			DSAP(1) + SSAP(1) + Control(1) +
+			if the DSAP = 0xAA, SSAP=0xAA, Contorl = 0x03, it has a 5-bytes SNAP header.
+				=> + SNAP (5, OriginationID(3) + etherType(2))
+		*/
+		if (pSrcBuf[0] == 0xAA && pSrcBuf[1] == 0xAA && pSrcBuf[2] == 0x03)
+		{
+			Sniff2BytesFromNdisBuffer(pSrcBuf, 6, &Byte0, &Byte1);
+			RTMP_SET_PACKET_LLCSNAP(pPacket, 1);
+			TypeLen = (USHORT)((Byte0 << 8) + Byte1);
+			pSrcBuf += 8; // Skip this LLC/SNAP header
+		}
+		else
+		{
+			//It just has 3-byte LLC header, maybe a legacy ether type frame. we didn't handle it.
+		}
+	}
+
+	// If it's a VLAN packet, get the real Type/Length field.
+	if (TypeLen == 0x8100)
+	{
+		/* 0x8100 means VLAN packets */
+
+		/* Dest. MAC Address (6-bytes) +
+		   Source MAC Address (6-bytes) +
+		   Length/Type = 802.1Q Tag Type (2-byte) +
+		   Tag Control Information (2-bytes) +
+		   Length / Type (2-bytes) +
+		   data payload (0-n bytes) +
+		   Pad (0-p bytes) +
+		   Frame Check Sequence (4-bytes) */
+
+		RTMP_SET_PACKET_VLAN(pPacket, 1);
+		Sniff2BytesFromNdisBuffer(pSrcBuf, 2, &Byte0, &Byte1);
+		TypeLen = (USHORT)((Byte0 << 8) + Byte1);
+
+		pSrcBuf += 4; // Skip the VLAN Header.
+	}
+
+	switch (TypeLen)
+	{
+		case 0x0800:
+			{
+				ASSERT((pktLen > 34));
+				if (*(pSrcBuf + 9) == 0x11)
+				{	// udp packet
+					ASSERT((pktLen > 34));	// 14 for ethernet header, 20 for IP header
+
+					pSrcBuf += 20;	// Skip the IP header
+					srcPort = OS_NTOHS(*((UINT16 *)pSrcBuf));
+					dstPort = OS_NTOHS(*((UINT16 *)(pSrcBuf +2)));
+
+					if ((srcPort==0x44 && dstPort==0x43) || (srcPort==0x43 && dstPort==0x44))
+					{	//It's a BOOTP/DHCP packet
+						RTMP_SET_PACKET_DHCP(pPacket, 1);
+					}
+				}
+			}
+			break;
+		case 0x0806:
+			{
+				//ARP Packet.
+				RTMP_SET_PACKET_DHCP(pPacket, 1);
+			}
+			break;
+		case 0x888e:
+			{
+				// EAPOL Packet.
+				RTMP_SET_PACKET_EAPOL(pPacket, 1);
+			}
+			break;
+		default:
+			status = FALSE;
+			break;
+	}
+
+	return status;
+
+}
+
+
+
+VOID Update_Rssi_Sample(
+	IN PRTMP_ADAPTER	pAd,
+	IN RSSI_SAMPLE		*pRssi,
+	IN PRXWI_STRUC		pRxWI)
+		{
+	CHAR	rssi0 = pRxWI->RSSI0;
+	CHAR	rssi1 = pRxWI->RSSI1;
+	CHAR	rssi2 = pRxWI->RSSI2;
+
+	if (rssi0 != 0)
+	{
+		pRssi->LastRssi0	= ConvertToRssi(pAd, (CHAR)rssi0, RSSI_0);
+		pRssi->AvgRssi0X8	= (pRssi->AvgRssi0X8 - pRssi->AvgRssi0) + pRssi->LastRssi0;
+		pRssi->AvgRssi0	= pRssi->AvgRssi0X8 >> 3;
+	}
+
+	if (rssi1 != 0)
+	{
+		pRssi->LastRssi1	= ConvertToRssi(pAd, (CHAR)rssi1, RSSI_1);
+		pRssi->AvgRssi1X8	= (pRssi->AvgRssi1X8 - pRssi->AvgRssi1) + pRssi->LastRssi1;
+		pRssi->AvgRssi1	= pRssi->AvgRssi1X8 >> 3;
+	}
+
+	if (rssi2 != 0)
+	{
+		pRssi->LastRssi2	= ConvertToRssi(pAd, (CHAR)rssi2, RSSI_2);
+		pRssi->AvgRssi2X8  = (pRssi->AvgRssi2X8 - pRssi->AvgRssi2) + pRssi->LastRssi2;
+		pRssi->AvgRssi2 = pRssi->AvgRssi2X8 >> 3;
+	}
+}
+
+
+
+// Normal legacy Rx packet indication
+VOID Indicate_Legacy_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+	UCHAR			Header802_3[LENGTH_802_3];
+
+	// 1. get 802.3 Header
+	// 2. remove LLC
+	// 		a. pointer pRxBlk->pData to payload
+	//      b. modify pRxBlk->DataSize
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(pRxBlk, Header802_3);
+#endif // CONFIG_STA_SUPPORT //
+
+	if (pRxBlk->DataSize > MAX_RX_PKT_LEN)
+	{
+
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+
+	STATS_INC_RX_PACKETS(pAd, FromWhichBSSID);
+
+
+	wlan_802_11_to_802_3_packet(pAd, pRxBlk, Header802_3, FromWhichBSSID);
+
+	//
+	// pass this 802.3 packet to upper layer or forward this packet to WM directly
+	//
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pRxPacket, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+
+// Normal, AMPDU or AMSDU
+VOID CmmRxnonRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+#ifdef DOT11_N_SUPPORT
+	if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMPDU) && (pAd->CommonCfg.bDisableReordering == 0))
+	{
+		Indicate_AMPDU_Packet(pAd, pRxBlk, FromWhichBSSID);
+	}
+	else
+#endif // DOT11_N_SUPPORT //
+	{
+#ifdef DOT11_N_SUPPORT
+		if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU))
+		{
+			// handle A-MSDU
+			Indicate_AMSDU_Packet(pAd, pRxBlk, FromWhichBSSID);
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
+		}
+	}
+}
+
+
+VOID CmmRxRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	UCHAR			Header802_3[LENGTH_802_3];
+	UINT16			Msdu2Size;
+	UINT16 			Payload1Size, Payload2Size;
+	PUCHAR 			pData2;
+	PNDIS_PACKET	pPacket2 = NULL;
+
+
+
+	Msdu2Size = *(pRxBlk->pData) + (*(pRxBlk->pData+1) << 8);
+
+	if ((Msdu2Size <= 1536) && (Msdu2Size < pRxBlk->DataSize))
+	{
+		/* skip two byte MSDU2 len */
+		pRxBlk->pData += 2;
+		pRxBlk->DataSize -= 2;
+	}
+	else
+	{
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	// get 802.3 Header and  remove LLC
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(pRxBlk, Header802_3);
+#endif // CONFIG_STA_SUPPORT //
+
+
+	ASSERT(pRxBlk->pRxPacket);
+
+	// Ralink Aggregation frame
+	pAd->RalinkCounters.OneSecRxAggregationCount ++;
+	Payload1Size = pRxBlk->DataSize - Msdu2Size;
+	Payload2Size = Msdu2Size - LENGTH_802_3;
+
+	pData2 = pRxBlk->pData + Payload1Size + LENGTH_802_3;
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		pPacket2 = duplicate_pkt(pAd, (pData2-LENGTH_802_3), LENGTH_802_3, pData2, Payload2Size, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+
+	if (!pPacket2)
+	{
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	// update payload size of 1st packet
+	pRxBlk->DataSize = Payload1Size;
+	wlan_802_11_to_802_3_packet(pAd, pRxBlk, Header802_3, FromWhichBSSID);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pRxBlk->pRxPacket, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+
+	if (pPacket2)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pPacket2, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+	}
+}
+
+
+#define RESET_FRAGFRAME(_fragFrame) \
+	{								\
+		_fragFrame.RxSize = 0;		\
+		_fragFrame.Sequence = 0;	\
+		_fragFrame.LastFrag = 0;	\
+		_fragFrame.Flags = 0;		\
+	}
+
+
+PNDIS_PACKET RTMPDeFragmentDataFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+	UCHAR			*pData = pRxBlk->pData;
+	USHORT			DataSize = pRxBlk->DataSize;
+	PNDIS_PACKET	pRetPacket = NULL;
+	UCHAR			*pFragBuffer = NULL;
+	BOOLEAN 		bReassDone = FALSE;
+	UCHAR			HeaderRoom = 0;
+
+
+	ASSERT(pHeader);
+
+	HeaderRoom = pData - (UCHAR *)pHeader;
+
+	// Re-assemble the fragmented packets
+	if (pHeader->Frag == 0)		// Frag. Number is 0 : First frag or only one pkt
+	{
+		// the first pkt of fragment, record it.
+		if (pHeader->FC.MoreFrag)
+		{
+			ASSERT(pAd->FragFrame.pFragPacket);
+			pFragBuffer = GET_OS_PKT_DATAPTR(pAd->FragFrame.pFragPacket);
+			pAd->FragFrame.RxSize   = DataSize + HeaderRoom;
+			NdisMoveMemory(pFragBuffer,	 pHeader, pAd->FragFrame.RxSize);
+			pAd->FragFrame.Sequence = pHeader->Sequence;
+			pAd->FragFrame.LastFrag = pHeader->Frag;	   // Should be 0
+			ASSERT(pAd->FragFrame.LastFrag == 0);
+			goto done;	// end of processing this frame
+		}
+	}
+	else	//Middle & End of fragment
+	{
+		if ((pHeader->Sequence != pAd->FragFrame.Sequence) ||
+			(pHeader->Frag != (pAd->FragFrame.LastFrag + 1)))
+		{
+			// Fragment is not the same sequence or out of fragment number order
+			// Reset Fragment control blk
+			RESET_FRAGFRAME(pAd->FragFrame);
+			DBGPRINT(RT_DEBUG_ERROR, ("Fragment is not the same sequence or out of fragment number order.\n"));
+			goto done; // give up this frame
+		}
+		else if ((pAd->FragFrame.RxSize + DataSize) > MAX_FRAME_SIZE)
+		{
+			// Fragment frame is too large, it exeeds the maximum frame size.
+			// Reset Fragment control blk
+			RESET_FRAGFRAME(pAd->FragFrame);
+			DBGPRINT(RT_DEBUG_ERROR, ("Fragment frame is too large, it exeeds the maximum frame size.\n"));
+			goto done; // give up this frame
+		}
+
+        //
+		// Broadcom AP(BCM94704AGR) will send out LLC in fragment's packet, LLC only can accpet at first fragment.
+		// In this case, we will dropt it.
+		//
+		if (NdisEqualMemory(pData, SNAP_802_1H, sizeof(SNAP_802_1H)))
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Find another LLC at Middle or End fragment(SN=%d, Frag=%d)\n", pHeader->Sequence, pHeader->Frag));
+			goto done; // give up this frame
+		}
+
+		pFragBuffer = GET_OS_PKT_DATAPTR(pAd->FragFrame.pFragPacket);
+
+		// concatenate this fragment into the re-assembly buffer
+		NdisMoveMemory((pFragBuffer + pAd->FragFrame.RxSize), pData, DataSize);
+		pAd->FragFrame.RxSize  += DataSize;
+		pAd->FragFrame.LastFrag = pHeader->Frag;	   // Update fragment number
+
+		// Last fragment
+		if (pHeader->FC.MoreFrag == FALSE)
+		{
+			bReassDone = TRUE;
+		}
+	}
+
+done:
+	// always release rx fragmented packet
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+
+	// return defragmented packet if packet is reassembled completely
+	// otherwise return NULL
+	if (bReassDone)
+	{
+		PNDIS_PACKET pNewFragPacket;
+
+		// allocate a new packet buffer for fragment
+		pNewFragPacket = RTMP_AllocateFragPacketBuffer(pAd, RX_BUFFER_NORMSIZE);
+		if (pNewFragPacket)
+		{
+			// update RxBlk
+			pRetPacket = pAd->FragFrame.pFragPacket;
+			pAd->FragFrame.pFragPacket = pNewFragPacket;
+			pRxBlk->pHeader = (PHEADER_802_11) GET_OS_PKT_DATAPTR(pRetPacket);
+			pRxBlk->pData = (UCHAR *)pRxBlk->pHeader + HeaderRoom;
+			pRxBlk->DataSize = pAd->FragFrame.RxSize - HeaderRoom;
+			pRxBlk->pRxPacket = pRetPacket;
+		}
+		else
+		{
+			RESET_FRAGFRAME(pAd->FragFrame);
+		}
+	}
+
+	return pRetPacket;
+}
+
+
+VOID Indicate_AMSDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	UINT			nMSDU;
+
+	update_os_packet_info(pAd, pRxBlk, FromWhichBSSID);
+	RTMP_SET_PACKET_IF(pRxBlk->pRxPacket, FromWhichBSSID);
+	nMSDU = deaggregate_AMSDU_announce(pAd, pRxBlk->pRxPacket, pRxBlk->pData, pRxBlk->DataSize);
+}
+
+VOID Indicate_EAPOL_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	MAC_TABLE_ENTRY *pEntry = NULL;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pEntry = &pAd->MacTab.Content[BSSID_WCID];
+		STARxEAPOLFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
+		return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	if (pEntry == NULL)
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("Indicate_EAPOL_Packet: drop and release the invalid packet.\n"));
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+}
+
+#define BCN_TBTT_OFFSET		64	//defer 64 us
+VOID ReSyncBeaconTime(
+	IN  PRTMP_ADAPTER   pAd)
+{
+
+	UINT32  Offset;
+
+
+	Offset = (pAd->TbttTickCount) % (BCN_TBTT_OFFSET);
+
+	pAd->TbttTickCount++;
+
+	//
+	// The updated BeaconInterval Value will affect Beacon Interval after two TBTT
+	// beacasue the original BeaconInterval had been loaded into next TBTT_TIMER
+	//
+	if (Offset == (BCN_TBTT_OFFSET-2))
+	{
+		BCN_TIME_CFG_STRUC csr;
+		RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+		csr.field.BeaconInterval = (pAd->CommonCfg.BeaconPeriod << 4) - 1 ;	// ASIC register in units of 1/16 TU = 64us
+		RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+	}
+	else
+	{
+		if (Offset == (BCN_TBTT_OFFSET-1))
+		{
+			BCN_TIME_CFG_STRUC csr;
+
+			RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+			csr.field.BeaconInterval = (pAd->CommonCfg.BeaconPeriod) << 4; // ASIC register in units of 1/16 TU
+			RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+		}
+	}
+}
+
diff --git a/drivers/staging/rt2860/common/cmm_data_2860.c b/drivers/staging/rt2860/common/cmm_data_2860.c
new file mode 100644
index 000000000000..419e50c3fc4c
--- /dev/null
+++ b/drivers/staging/rt2860/common/cmm_data_2860.c
@@ -0,0 +1,1240 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+
+/*
+   All functions in this file must be PCI-depended, or you should out your function
+	in other files.
+
+*/
+#include "../rt_config.h"
+
+extern RTMP_RF_REGS RF2850RegTable[];
+extern UCHAR	NUM_OF_2850_CHNL;
+
+USHORT RtmpPCI_WriteTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber)
+{
+
+	UCHAR			*pDMAHeaderBufVA;
+	USHORT			TxIdx, RetTxIdx;
+	PTXD_STRUC		pTxD;
+	UINT32			BufBasePaLow;
+	PRTMP_TX_RING	pTxRing;
+	USHORT			hwHeaderLen;
+
+	//
+	// get Tx Ring Resource
+	//
+	pTxRing = &pAd->TxRing[pTxBlk->QueIdx];
+	TxIdx = pAd->TxRing[pTxBlk->QueIdx].TxCpuIdx;
+	pDMAHeaderBufVA = (PUCHAR) pTxRing->Cell[TxIdx].DmaBuf.AllocVa;
+	BufBasePaLow = RTMP_GetPhysicalAddressLow(pTxRing->Cell[TxIdx].DmaBuf.AllocPa);
+
+	// copy TXINFO + TXWI + WLAN Header + LLC into DMA Header Buffer
+	if (pTxBlk->TxFrameType == TX_AMSDU_FRAME)
+	{
+		hwHeaderLen = pTxBlk->MpduHeaderLen - LENGTH_AMSDU_SUBFRAMEHEAD + pTxBlk->HdrPadLen + LENGTH_AMSDU_SUBFRAMEHEAD;
+	}
+	else
+	{
+		hwHeaderLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+	}
+	NdisMoveMemory(pDMAHeaderBufVA, pTxBlk->HeaderBuf, TXINFO_SIZE + TXWI_SIZE + hwHeaderLen);
+
+	pTxRing->Cell[TxIdx].pNdisPacket = pTxBlk->pPacket;
+	pTxRing->Cell[TxIdx].pNextNdisPacket = NULL;
+
+	//
+	// build Tx Descriptor
+	//
+
+	pTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+	NdisZeroMemory(pTxD, TXD_SIZE);
+
+	pTxD->SDPtr0 = BufBasePaLow;
+	pTxD->SDLen0 = TXINFO_SIZE + TXWI_SIZE + hwHeaderLen; // include padding
+	pTxD->SDPtr1 = PCI_MAP_SINGLE(pAd, pTxBlk, 0, 1, PCI_DMA_TODEVICE);;
+	pTxD->SDLen1 = pTxBlk->SrcBufLen;
+	pTxD->LastSec0 = 0;
+	pTxD->LastSec1 = (bIsLast) ? 1 : 0;
+
+	RTMPWriteTxDescriptor(pAd, pTxD, FALSE, FIFO_EDCA);
+
+	RetTxIdx = TxIdx;
+	//
+	// Update Tx index
+	//
+	INC_RING_INDEX(TxIdx, TX_RING_SIZE);
+	pTxRing->TxCpuIdx = TxIdx;
+
+	*FreeNumber -= 1;
+
+	return RetTxIdx;
+}
+
+
+USHORT RtmpPCI_WriteSingleTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber)
+{
+
+	UCHAR			*pDMAHeaderBufVA;
+	USHORT			TxIdx, RetTxIdx;
+	PTXD_STRUC		pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	UINT32			BufBasePaLow;
+	PRTMP_TX_RING	pTxRing;
+	USHORT			hwHeaderLen;
+
+	//
+	// get Tx Ring Resource
+	//
+	pTxRing = &pAd->TxRing[pTxBlk->QueIdx];
+	TxIdx = pAd->TxRing[pTxBlk->QueIdx].TxCpuIdx;
+	pDMAHeaderBufVA = (PUCHAR) pTxRing->Cell[TxIdx].DmaBuf.AllocVa;
+	BufBasePaLow = RTMP_GetPhysicalAddressLow(pTxRing->Cell[TxIdx].DmaBuf.AllocPa);
+
+	// copy TXINFO + TXWI + WLAN Header + LLC into DMA Header Buffer
+	hwHeaderLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+
+	NdisMoveMemory(pDMAHeaderBufVA, pTxBlk->HeaderBuf, TXINFO_SIZE + TXWI_SIZE + hwHeaderLen);
+
+	pTxRing->Cell[TxIdx].pNdisPacket = pTxBlk->pPacket;
+	pTxRing->Cell[TxIdx].pNextNdisPacket = NULL;
+
+	//
+	// build Tx Descriptor
+	//
+#ifndef RT_BIG_ENDIAN
+	pTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+#else
+	pDestTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+	TxD = *pDestTxD;
+	pTxD = &TxD;
+#endif
+	NdisZeroMemory(pTxD, TXD_SIZE);
+
+	pTxD->SDPtr0 = BufBasePaLow;
+	pTxD->SDLen0 = TXINFO_SIZE + TXWI_SIZE + hwHeaderLen; // include padding
+	pTxD->SDPtr1 = PCI_MAP_SINGLE(pAd, pTxBlk, 0, 1, PCI_DMA_TODEVICE);;
+	pTxD->SDLen1 = pTxBlk->SrcBufLen;
+	pTxD->LastSec0 = 0;
+	pTxD->LastSec1 = (bIsLast) ? 1 : 0;
+
+	RTMPWriteTxDescriptor(pAd, pTxD, FALSE, FIFO_EDCA);
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)(pDMAHeaderBufVA + TXINFO_SIZE), TYPE_TXWI);
+	RTMPFrameEndianChange(pAd, (PUCHAR)(pDMAHeaderBufVA + TXINFO_SIZE + TXWI_SIZE), DIR_WRITE, FALSE);
+	RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+    WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif // RT_BIG_ENDIAN //
+
+	RetTxIdx = TxIdx;
+	//
+	// Update Tx index
+	//
+	INC_RING_INDEX(TxIdx, TX_RING_SIZE);
+	pTxRing->TxCpuIdx = TxIdx;
+
+	*FreeNumber -= 1;
+
+	return RetTxIdx;
+}
+
+
+USHORT RtmpPCI_WriteMultiTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			frameNum,
+	OUT	USHORT			*FreeNumber)
+{
+	BOOLEAN bIsLast;
+	UCHAR			*pDMAHeaderBufVA;
+	USHORT			TxIdx, RetTxIdx;
+	PTXD_STRUC		pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	UINT32			BufBasePaLow;
+	PRTMP_TX_RING	pTxRing;
+	USHORT			hwHdrLen;
+	UINT32			firstDMALen;
+
+	bIsLast = ((frameNum == (pTxBlk->TotalFrameNum - 1)) ? 1 : 0);
+
+	//
+	// get Tx Ring Resource
+	//
+	pTxRing = &pAd->TxRing[pTxBlk->QueIdx];
+	TxIdx = pAd->TxRing[pTxBlk->QueIdx].TxCpuIdx;
+	pDMAHeaderBufVA = (PUCHAR) pTxRing->Cell[TxIdx].DmaBuf.AllocVa;
+	BufBasePaLow = RTMP_GetPhysicalAddressLow(pTxRing->Cell[TxIdx].DmaBuf.AllocPa);
+
+	if (frameNum == 0)
+	{
+		// copy TXINFO + TXWI + WLAN Header + LLC into DMA Header Buffer
+		if (pTxBlk->TxFrameType == TX_AMSDU_FRAME)
+			//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen-LENGTH_AMSDU_SUBFRAMEHEAD, 4)+LENGTH_AMSDU_SUBFRAMEHEAD;
+			hwHdrLen = pTxBlk->MpduHeaderLen - LENGTH_AMSDU_SUBFRAMEHEAD + pTxBlk->HdrPadLen + LENGTH_AMSDU_SUBFRAMEHEAD;
+		else if (pTxBlk->TxFrameType == TX_RALINK_FRAME)
+			//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen-LENGTH_ARALINK_HEADER_FIELD, 4)+LENGTH_ARALINK_HEADER_FIELD;
+			hwHdrLen = pTxBlk->MpduHeaderLen - LENGTH_ARALINK_HEADER_FIELD + pTxBlk->HdrPadLen + LENGTH_ARALINK_HEADER_FIELD;
+		else
+			//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen, 4);
+			hwHdrLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+
+		firstDMALen = TXINFO_SIZE + TXWI_SIZE + hwHdrLen;
+	}
+	else
+	{
+		firstDMALen = pTxBlk->MpduHeaderLen;
+	}
+
+	NdisMoveMemory(pDMAHeaderBufVA, pTxBlk->HeaderBuf, firstDMALen);
+
+	pTxRing->Cell[TxIdx].pNdisPacket = pTxBlk->pPacket;
+	pTxRing->Cell[TxIdx].pNextNdisPacket = NULL;
+
+	//
+	// build Tx Descriptor
+	//
+#ifndef RT_BIG_ENDIAN
+	pTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+#else
+	pDestTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+	TxD = *pDestTxD;
+	pTxD = &TxD;
+#endif
+	NdisZeroMemory(pTxD, TXD_SIZE);
+
+	pTxD->SDPtr0 = BufBasePaLow;
+	pTxD->SDLen0 = firstDMALen; // include padding
+	pTxD->SDPtr1 = PCI_MAP_SINGLE(pAd, pTxBlk, 0, 1, PCI_DMA_TODEVICE);;
+	pTxD->SDLen1 = pTxBlk->SrcBufLen;
+	pTxD->LastSec0 = 0;
+	pTxD->LastSec1 = (bIsLast) ? 1 : 0;
+
+	RTMPWriteTxDescriptor(pAd, pTxD, FALSE, FIFO_EDCA);
+
+#ifdef RT_BIG_ENDIAN
+	if (frameNum == 0)
+		RTMPFrameEndianChange(pAd, (PUCHAR)(pDMAHeaderBufVA+ TXINFO_SIZE + TXWI_SIZE), DIR_WRITE, FALSE);
+
+	if (frameNum != 0)
+		RTMPWIEndianChange((PUCHAR)(pDMAHeaderBufVA + TXINFO_SIZE), TYPE_TXWI);
+
+	RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+	WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif // RT_BIG_ENDIAN //
+
+	RetTxIdx = TxIdx;
+	//
+	// Update Tx index
+	//
+	INC_RING_INDEX(TxIdx, TX_RING_SIZE);
+	pTxRing->TxCpuIdx = TxIdx;
+
+	*FreeNumber -= 1;
+
+	return RetTxIdx;
+
+}
+
+
+VOID RtmpPCI_FinalWriteTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	USHORT			totalMPDUSize,
+	IN	USHORT			FirstTxIdx)
+{
+
+	PTXWI_STRUC		pTxWI;
+	PRTMP_TX_RING	pTxRing;
+
+	//
+	// get Tx Ring Resource
+	//
+	pTxRing = &pAd->TxRing[pTxBlk->QueIdx];
+	pTxWI = (PTXWI_STRUC) pTxRing->Cell[FirstTxIdx].DmaBuf.AllocVa;
+	pTxWI->MPDUtotalByteCount = totalMPDUSize;
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+#endif // RT_BIG_ENDIAN //
+
+}
+
+
+VOID RtmpPCIDataLastTxIdx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	USHORT			LastTxIdx)
+{
+	PTXD_STRUC		pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	PRTMP_TX_RING	pTxRing;
+
+	//
+	// get Tx Ring Resource
+	//
+	pTxRing = &pAd->TxRing[QueIdx];
+
+	//
+	// build Tx Descriptor
+	//
+#ifndef RT_BIG_ENDIAN
+	pTxD = (PTXD_STRUC) pTxRing->Cell[LastTxIdx].AllocVa;
+#else
+	pDestTxD = (PTXD_STRUC) pTxRing->Cell[LastTxIdx].AllocVa;
+	TxD = *pDestTxD;
+	pTxD = &TxD;
+#endif
+
+	pTxD->LastSec1 = 1;
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+    WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif // RT_BIG_ENDIAN //
+
+}
+
+
+USHORT	RtmpPCI_WriteFragTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			fragNum,
+	OUT	USHORT			*FreeNumber)
+{
+	UCHAR			*pDMAHeaderBufVA;
+	USHORT			TxIdx, RetTxIdx;
+	PTXD_STRUC		pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	UINT32			BufBasePaLow;
+	PRTMP_TX_RING	pTxRing;
+	USHORT			hwHeaderLen;
+	UINT32			firstDMALen;
+
+	//
+	// Get Tx Ring Resource
+	//
+	pTxRing = &pAd->TxRing[pTxBlk->QueIdx];
+	TxIdx = pAd->TxRing[pTxBlk->QueIdx].TxCpuIdx;
+	pDMAHeaderBufVA = (PUCHAR) pTxRing->Cell[TxIdx].DmaBuf.AllocVa;
+	BufBasePaLow = RTMP_GetPhysicalAddressLow(pTxRing->Cell[TxIdx].DmaBuf.AllocPa);
+
+	//
+	// Copy TXINFO + TXWI + WLAN Header + LLC into DMA Header Buffer
+	//
+	hwHeaderLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+
+	firstDMALen = TXINFO_SIZE + TXWI_SIZE + hwHeaderLen;
+	NdisMoveMemory(pDMAHeaderBufVA, pTxBlk->HeaderBuf, firstDMALen);
+
+
+	//
+	// Build Tx Descriptor
+	//
+#ifndef RT_BIG_ENDIAN
+	pTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+#else
+	pDestTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+	TxD = *pDestTxD;
+	pTxD = &TxD;
+#endif
+	NdisZeroMemory(pTxD, TXD_SIZE);
+
+	if (fragNum == pTxBlk->TotalFragNum)
+	{
+		pTxRing->Cell[TxIdx].pNdisPacket = pTxBlk->pPacket;
+		pTxRing->Cell[TxIdx].pNextNdisPacket = NULL;
+	}
+
+	pTxD->SDPtr0 = BufBasePaLow;
+	pTxD->SDLen0 = firstDMALen; // include padding
+	pTxD->SDPtr1 = PCI_MAP_SINGLE(pAd, pTxBlk, 0, 1, PCI_DMA_TODEVICE);
+	pTxD->SDLen1 = pTxBlk->SrcBufLen;
+	pTxD->LastSec0 = 0;
+	pTxD->LastSec1 = 1;
+
+	RTMPWriteTxDescriptor(pAd, pTxD, FALSE, FIFO_EDCA);
+
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)(pDMAHeaderBufVA + TXINFO_SIZE), TYPE_TXWI);
+	RTMPFrameEndianChange(pAd, (PUCHAR)(pDMAHeaderBufVA + TXINFO_SIZE + TXWI_SIZE), DIR_WRITE, FALSE);
+	RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+    WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif // RT_BIG_ENDIAN //
+
+	RetTxIdx = TxIdx;
+	pTxBlk->Priv += pTxBlk->SrcBufLen;
+
+	//
+	// Update Tx index
+	//
+	INC_RING_INDEX(TxIdx, TX_RING_SIZE);
+	pTxRing->TxCpuIdx = TxIdx;
+
+	*FreeNumber -= 1;
+
+	return RetTxIdx;
+
+}
+
+/*
+	Must be run in Interrupt context
+	This function handle PCI specific TxDesc and cpu index update and kick the packet out.
+ */
+int RtmpPCIMgmtKickOut(
+	IN RTMP_ADAPTER 	*pAd,
+	IN UCHAR 			QueIdx,
+	IN PNDIS_PACKET		pPacket,
+	IN PUCHAR			pSrcBufVA,
+	IN UINT 			SrcBufLen)
+{
+	PTXD_STRUC		pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	ULONG			SwIdx = pAd->MgmtRing.TxCpuIdx;
+
+#ifdef RT_BIG_ENDIAN
+    pDestTxD  = (PTXD_STRUC)pAd->MgmtRing.Cell[SwIdx].AllocVa;
+    TxD = *pDestTxD;
+    pTxD = &TxD;
+    RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#else
+	pTxD  = (PTXD_STRUC) pAd->MgmtRing.Cell[SwIdx].AllocVa;
+#endif
+
+	pAd->MgmtRing.Cell[SwIdx].pNdisPacket = pPacket;
+	pAd->MgmtRing.Cell[SwIdx].pNextNdisPacket = NULL;
+
+	RTMPWriteTxDescriptor(pAd, pTxD, TRUE, FIFO_MGMT);
+	pTxD->LastSec0 = 1;
+	pTxD->LastSec1 = 1;
+	pTxD->DMADONE = 0;
+	pTxD->SDLen1 = 0;
+	pTxD->SDPtr0 = PCI_MAP_SINGLE(pAd, pSrcBufVA, SrcBufLen, 0, PCI_DMA_TODEVICE);;
+	pTxD->SDLen0 = SrcBufLen;
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+    WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif
+
+	pAd->RalinkCounters.KickTxCount++;
+	pAd->RalinkCounters.OneSecTxDoneCount++;
+
+	// Increase TX_CTX_IDX, but write to register later.
+	INC_RING_INDEX(pAd->MgmtRing.TxCpuIdx, MGMT_RING_SIZE);
+
+	RTMP_IO_WRITE32(pAd, TX_MGMTCTX_IDX,  pAd->MgmtRing.TxCpuIdx);
+
+	return 0;
+}
+
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	========================================================================
+
+	Routine Description:
+		Check Rx descriptor, return NDIS_STATUS_FAILURE if any error dound
+
+	Arguments:
+		pRxD		Pointer to the Rx descriptor
+
+	Return Value:
+		NDIS_STATUS_SUCCESS 	No err
+		NDIS_STATUS_FAILURE 	Error
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS RTMPCheckRxError(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	PHEADER_802_11		pHeader,
+	IN	PRXWI_STRUC 		pRxWI,
+	IN  PRT28XX_RXD_STRUC 	pRxD)
+{
+	PCIPHER_KEY pWpaKey;
+	INT dBm;
+
+	// Phy errors & CRC errors
+	if (/*(pRxD->PhyErr) ||*/ (pRxD->Crc))
+	{
+		// Check RSSI for Noise Hist statistic collection.
+		dBm = (INT) (pRxWI->RSSI0) - pAd->BbpRssiToDbmDelta;
+		if (dBm <= -87)
+			pAd->StaCfg.RPIDensity[0] += 1;
+		else if (dBm <= -82)
+			pAd->StaCfg.RPIDensity[1] += 1;
+		else if (dBm <= -77)
+			pAd->StaCfg.RPIDensity[2] += 1;
+		else if (dBm <= -72)
+			pAd->StaCfg.RPIDensity[3] += 1;
+		else if (dBm <= -67)
+			pAd->StaCfg.RPIDensity[4] += 1;
+		else if (dBm <= -62)
+			pAd->StaCfg.RPIDensity[5] += 1;
+		else if (dBm <= -57)
+			pAd->StaCfg.RPIDensity[6] += 1;
+		else if (dBm > -57)
+			pAd->StaCfg.RPIDensity[7] += 1;
+
+		return(NDIS_STATUS_FAILURE);
+	}
+
+	// Add Rx size to channel load counter, we should ignore error counts
+	pAd->StaCfg.CLBusyBytes += (pRxD->SDL0 + 14);
+
+	// Drop ToDs promiscous frame, it is opened due to CCX 2 channel load statistics
+	if (pHeader != NULL)
+	{
+		if (pHeader->FC.ToDs)
+		{
+			return(NDIS_STATUS_FAILURE);
+		}
+	}
+
+	// Drop not U2M frames, cant's drop here because we will drop beacon in this case
+	// I am kind of doubting the U2M bit operation
+	// if (pRxD->U2M == 0)
+	//	return(NDIS_STATUS_FAILURE);
+
+	// drop decyption fail frame
+	if (pRxD->CipherErr)
+	{
+		if (pRxD->CipherErr == 2)
+			{DBGPRINT_RAW(RT_DEBUG_TRACE,("pRxD ERROR: ICV ok but MICErr "));}
+		else if (pRxD->CipherErr == 1)
+			{DBGPRINT_RAW(RT_DEBUG_TRACE,("pRxD ERROR: ICV Err "));}
+		else if (pRxD->CipherErr == 3)
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("pRxD ERROR: Key not valid "));
+
+        if (((pRxD->CipherErr & 1) == 1) && pAd->CommonCfg.bWirelessEvent && INFRA_ON(pAd))
+            RTMPSendWirelessEvent(pAd, IW_ICV_ERROR_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+		DBGPRINT_RAW(RT_DEBUG_TRACE,(" %d (len=%d, Mcast=%d, MyBss=%d, Wcid=%d, KeyId=%d)\n",
+			pRxD->CipherErr,
+			pRxD->SDL0,
+			pRxD->Mcast | pRxD->Bcast,
+			pRxD->MyBss,
+			pRxWI->WirelessCliID,
+			pRxWI->KeyIndex));
+
+		//
+		// MIC Error
+		//
+		if (pRxD->CipherErr == 2)
+		{
+			pWpaKey = &pAd->SharedKey[BSS0][pRxWI->KeyIndex];
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.WpaSupplicantUP)
+                WpaSendMicFailureToWpaSupplicant(pAd,
+                                   (pWpaKey->Type == PAIRWISEKEY) ? TRUE:FALSE);
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+			    RTMPReportMicError(pAd, pWpaKey);
+
+            if (((pRxD->CipherErr & 2) == 2) && pAd->CommonCfg.bWirelessEvent && INFRA_ON(pAd))
+                RTMPSendWirelessEvent(pAd, IW_MIC_ERROR_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("Rx MIC Value error\n"));
+		}
+
+		if (pHeader == NULL)
+			return(NDIS_STATUS_SUCCESS);
+
+		return(NDIS_STATUS_FAILURE);
+	}
+
+	return(NDIS_STATUS_SUCCESS);
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine sends command to firmware and turn our chip to power save mode.
+		Both RadioOff and .11 power save function needs to call this routine.
+	Input:
+		Level = GUIRADIO_OFF  : GUI Radio Off mode
+		Level = DOT11POWERSAVE  : 802.11 power save mode
+		Level = RTMP_HALT  : When Disable device.
+
+	==========================================================================
+ */
+VOID RT28xxPciAsicRadioOff(
+	IN PRTMP_ADAPTER    pAd,
+	IN UCHAR            Level,
+	IN USHORT           TbttNumToNextWakeUp)
+{
+	WPDMA_GLO_CFG_STRUC	DmaCfg;
+	UCHAR		i, tempBBP_R3 = 0;
+	BOOLEAN		brc = FALSE, Cancelled;
+    UINT32		TbTTTime = 0;
+	UINT32		PsPollTime = 0, MACValue;
+    ULONG		BeaconPeriodTime;
+    UINT32		RxDmaIdx, RxCpuIdx;
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicRadioOff ===> TxCpuIdx = %d, TxDmaIdx = %d. RxCpuIdx = %d, RxDmaIdx = %d.\n", pAd->TxRing[0].TxCpuIdx, pAd->TxRing[0].TxDmaIdx, pAd->RxRing.RxCpuIdx, pAd->RxRing.RxDmaIdx));
+
+    // Check Rx DMA busy status, if more than half is occupied, give up this radio off.
+	RTMP_IO_READ32(pAd, RX_DRX_IDX , &RxDmaIdx);
+	RTMP_IO_READ32(pAd, RX_CRX_IDX , &RxCpuIdx);
+	if ((RxDmaIdx > RxCpuIdx) && ((RxDmaIdx - RxCpuIdx) > RX_RING_SIZE/3))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("AsicRadioOff ===> return1. RxDmaIdx = %d ,  RxCpuIdx = %d. \n", RxDmaIdx, RxCpuIdx));
+		return;
+	}
+	else if ((RxCpuIdx >= RxDmaIdx) && ((RxCpuIdx - RxDmaIdx) < RX_RING_SIZE/3))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("AsicRadioOff ===> return2.  RxCpuIdx = %d. RxDmaIdx = %d ,  \n", RxCpuIdx, RxDmaIdx));
+		return;
+	}
+
+    // Once go into this function, disable tx because don't want too many packets in queue to prevent HW stops.
+	pAd->bPCIclkOffDisableTx = TRUE;
+
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+	{
+	    RTMPCancelTimer(&pAd->Mlme.RadioOnOffTimer,	&Cancelled);
+	    RTMPCancelTimer(&pAd->Mlme.PsPollTimer,	&Cancelled);
+
+	    if (Level == DOT11POWERSAVE)
+		{
+			RTMP_IO_READ32(pAd, TBTT_TIMER, &TbTTTime);
+			TbTTTime &= 0x1ffff;
+			// 00. check if need to do sleep in this DTIM period.   If next beacon will arrive within 30ms , ...doesn't necessarily sleep.
+			// TbTTTime uint = 64us, LEAD_TIME unit = 1024us, PsPollTime unit = 1ms
+	        if  (((64*TbTTTime) <((LEAD_TIME*1024) + 40000)) && (TbttNumToNextWakeUp == 0))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("TbTTTime = 0x%x , give up this sleep. \n", TbTTTime));
+	            OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+	            pAd->bPCIclkOffDisableTx = FALSE;
+				return;
+			}
+			else
+			{
+				PsPollTime = (64*TbTTTime- LEAD_TIME*1024)/1000;
+				PsPollTime -= 3;
+
+	            BeaconPeriodTime = pAd->CommonCfg.BeaconPeriod*102/100;
+				if (TbttNumToNextWakeUp > 0)
+					PsPollTime += ((TbttNumToNextWakeUp -1) * BeaconPeriodTime);
+
+	            pAd->Mlme.bPsPollTimerRunning = TRUE;
+				RTMPSetTimer(&pAd->Mlme.PsPollTimer, PsPollTime);
+			}
+		}
+	}
+
+    // 0. Disable Tx DMA.
+	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
+	DmaCfg.field.EnableTxDMA = 0;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, DmaCfg.word);
+
+	// 1. Wait DMA not busy
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
+		if ((DmaCfg.field.TxDMABusy == 0) && (DmaCfg.field.RxDMABusy == 0))
+			break;
+		RTMPusecDelay(20);
+		i++;
+	}while(i < 50);
+
+	if (i >= 50)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("DMA keeps busy.  return on RT28xxPciAsicRadioOff ()\n"));
+		pAd->bPCIclkOffDisableTx = FALSE;
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
+		DmaCfg.field.EnableTxDMA = 1;
+		RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, DmaCfg.word);
+		return;
+	}
+
+    RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_IDLE_RADIO_OFF);
+
+    // Set to 1R.
+    tempBBP_R3 = (pAd->StaCfg.BBPR3 & 0xE7);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, tempBBP_R3);
+
+	// In Radio Off, we turn off RF clk, So now need to call ASICSwitchChannel again.
+	if (INFRA_ON(pAd) && (pAd->CommonCfg.CentralChannel != pAd->CommonCfg.Channel)
+		&& (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+	{
+		// Must using 40MHz.
+		AsicTurnOffRFClk(pAd, pAd->CommonCfg.CentralChannel);
+	}
+	else
+	{
+		// Must using 20MHz.
+		AsicTurnOffRFClk(pAd, pAd->CommonCfg.Channel);
+	}
+
+    // When PCI clock is off, don't want to service interrupt.
+	RTMP_IO_WRITE32(pAd, INT_MASK_CSR, AutoWakeupInt);
+
+    RTMP_IO_WRITE32(pAd, RX_CRX_IDX, pAd->RxRing.RxCpuIdx);
+	// Disable MAC Rx
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL , &MACValue);
+	MACValue &= 0xf7;
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL , MACValue);
+
+	//  2. Send Sleep command
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_STATUS, 0xffffffff);
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CID, 0xffffffff);
+	AsicSendCommandToMcu(pAd, 0x30, PowerSafeCID, 0xff, 0x00);   // send POWER-SAVE command to MCU. Timeout unit:40us.
+	//  2-1. Wait command success
+	// Status = 1 : success, Status = 2, already sleep, Status = 3, Maybe MAC is busy so can't finish this task.
+	brc = AsicCheckCommanOk(pAd, PowerSafeCID);
+
+    if (brc == FALSE)
+    {
+        // try again
+    	AsicSendCommandToMcu(pAd, 0x30, PowerSafeCID, 0xff, 0x00);   // send POWER-SAVE command to MCU. Timeout unit:40us.
+    	//RTMPusecDelay(200);
+    	brc = AsicCheckCommanOk(pAd, PowerSafeCID);
+    }
+
+	//  3. After 0x30 command is ok, send radio off command. lowbyte = 0 for power safe.
+	// If 0x30 command is not ok this time, we can ignore 0x35 command. It will make sure not cause firmware'r problem.
+	if ((Level == DOT11POWERSAVE) && (brc == TRUE))
+	{
+		AsicSendCommandToMcu(pAd, 0x35, PowerRadioOffCID, 0, 0x00);	// lowbyte = 0 means to do power safe, NOT turn off radio.
+	 	//  3-1. Wait command success
+	 	AsicCheckCommanOk(pAd, PowerRadioOffCID);
+	}
+	else if (brc == TRUE)
+	{
+		AsicSendCommandToMcu(pAd, 0x35, PowerRadioOffCID, 1, 0x00);	// lowbyte = 0 means to do power safe, NOT turn off radio.
+	 	//  3-1. Wait command success
+	 	AsicCheckCommanOk(pAd, PowerRadioOffCID);
+	}
+
+    // Wait DMA not busy
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
+		if (DmaCfg.field.RxDMABusy == 0)
+			break;
+		RTMPusecDelay(20);
+		i++;
+	}while(i < 50);
+
+	if (i >= 50)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("DMA Rx keeps busy.  on RT28xxPciAsicRadioOff ()\n"));
+	}
+	// disable DMA Rx.
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
+		DmaCfg.field.EnableRxDMA = 0;
+		RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, DmaCfg.word);
+	}
+
+	if (Level == DOT11POWERSAVE)
+	{
+		AUTO_WAKEUP_STRUC	AutoWakeupCfg;
+		//RTMPSetTimer(&pAd->Mlme.PsPollTimer, 90);
+
+		// we have decided to SLEEP, so at least do it for a BEACON period.
+		if (TbttNumToNextWakeUp == 0)
+			TbttNumToNextWakeUp = 1;
+
+		AutoWakeupCfg.word = 0;
+		RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+
+		// 1. Set auto wake up timer.
+		AutoWakeupCfg.field.NumofSleepingTbtt = TbttNumToNextWakeUp - 1;
+		AutoWakeupCfg.field.EnableAutoWakeup = 1;
+		AutoWakeupCfg.field.AutoLeadTime = LEAD_TIME;
+		RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+	}
+
+	//  4-1. If it's to disable our device. Need to restore PCI Configuration Space to its original value.
+	if (Level == RTMP_HALT)
+	{
+		if ((brc == TRUE) && (i < 50))
+			RTMPPCIeLinkCtrlSetting(pAd, 1);
+	}
+	//  4. Set PCI configuration Space Link Comtrol fields.  Only Radio Off needs to call this function
+	else
+	{
+		if ((brc == TRUE) && (i < 50))
+			RTMPPCIeLinkCtrlSetting(pAd, 3);
+	}
+
+    pAd->bPCIclkOffDisableTx = FALSE;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		This routine sends command to firmware and turn our chip to wake up mode from power save mode.
+		Both RadioOn and .11 power save function needs to call this routine.
+	Input:
+		Level = GUIRADIO_OFF : call this function is from Radio Off to Radio On.  Need to restore PCI host value.
+		Level = other value : normal wake up function.
+
+	==========================================================================
+ */
+BOOLEAN RT28xxPciAsicRadioOn(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR     Level)
+{
+    WPDMA_GLO_CFG_STRUC	DmaCfg;
+	BOOLEAN				Cancelled, brv = TRUE;
+    UINT32			    MACValue;
+
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+	{
+	    pAd->Mlme.bPsPollTimerRunning = FALSE;
+		RTMPCancelTimer(&pAd->Mlme.PsPollTimer,	&Cancelled);
+		if ((Level == GUIRADIO_OFF) || (Level == GUI_IDLE_POWER_SAVE))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("RT28xxPciAsicRadioOn ()\n"));
+			// 1. Set PCI Link Control in Configuration Space.
+			RTMPPCIeLinkCtrlValueRestore(pAd, RESTORE_WAKEUP);
+			RTMPusecDelay(6000);
+		}
+	}
+
+    pAd->bPCIclkOff = FALSE;
+
+	// 2. Send wake up command.
+	AsicSendCommandToMcu(pAd, 0x31, PowerWakeCID, 0x00, 0x00);
+
+	// 2-1. wait command ok.
+	brv = AsicCheckCommanOk(pAd, PowerWakeCID);
+    if (brv)
+    {
+    	//RTMP_IO_WRITE32(pAd, INT_MASK_CSR, (DELAYINTMASK|RxINT));
+    	NICEnableInterrupt(pAd);
+
+    	// 3. Enable Tx DMA.
+    	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
+    	DmaCfg.field.EnableTxDMA = 1;
+        DmaCfg.field.EnableRxDMA = 1;
+    	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, DmaCfg.word);
+
+        // Eable MAC Rx
+    	RTMP_IO_READ32(pAd, MAC_SYS_CTRL , &MACValue);
+    	MACValue |= 0x8;
+    	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL , MACValue);
+
+    	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_IDLE_RADIO_OFF);
+    	if (Level == GUI_IDLE_POWER_SAVE)
+    	{
+    		// In Radio Off, we turn off RF clk, So now need to call ASICSwitchChannel again.
+    		if (INFRA_ON(pAd) && (pAd->CommonCfg.CentralChannel != pAd->CommonCfg.Channel)
+    			&& (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+    		{
+    			// Must using 40MHz.
+    			AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+    			AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+    		}
+    		else
+    		{
+    			// Must using 20MHz.
+    			AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+    			AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+    		}
+    	}
+        return TRUE;
+    }
+    else
+        return FALSE;
+}
+
+VOID RT28xxPciStaAsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN       bFromTx)
+{
+    AUTO_WAKEUP_STRUC	AutoWakeupCfg;
+
+    if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+        return;
+
+    if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WAKEUP_NOW))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("waking up now!\n"));
+        return;
+    }
+
+    OPSTATUS_SET_FLAG(pAd, fOP_STATUS_WAKEUP_NOW);
+
+    if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+    {
+        // Support PCIe Advance Power Save
+    	if (bFromTx == TRUE)
+    	{
+            pAd->Mlme.bPsPollTimerRunning = FALSE;
+    		RTMPPCIeLinkCtrlValueRestore(pAd, RESTORE_WAKEUP);
+    		RTMPusecDelay(3000);
+            DBGPRINT(RT_DEBUG_TRACE, ("=======AsicForceWakeup===bFromTx\n"));
+    	}
+
+		AutoWakeupCfg.word = 0;
+		RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+
+        if (RT28xxPciAsicRadioOn(pAd, DOT11POWERSAVE))
+        {
+            // In Radio Off, we turn off RF clk, So now need to call ASICSwitchChannel again.
+        	if (INFRA_ON(pAd) && (pAd->CommonCfg.CentralChannel != pAd->CommonCfg.Channel)
+        		&& (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+        	{
+        		// Must using 40MHz.
+        		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+        		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+        	}
+        	else
+        	{
+        		// Must using 20MHz.
+        		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+        		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+        	}
+        }
+    }
+    else
+    {
+        // PCI, 2860-PCIe
+        AsicSendCommandToMcu(pAd, 0x31, 0xff, 0x00, 0x00);
+		AutoWakeupCfg.word = 0;
+		RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+    }
+
+    OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+    OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_WAKEUP_NOW);
+    DBGPRINT(RT_DEBUG_TRACE, ("<=======RT28xxPciStaAsicForceWakeup\n"));
+}
+
+VOID RT28xxPciStaAsicSleepThenAutoWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TbttNumToNextWakeUp)
+{
+    if (pAd->StaCfg.bRadio == FALSE)
+	{
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+		return;
+	}
+    if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+    {
+    	ULONG	Now = 0;
+        if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WAKEUP_NOW))
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("waking up now!\n"));
+            OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+            return;
+        }
+
+		NdisGetSystemUpTime(&Now);
+		// If last send NULL fram time is too close to this receiving beacon (within 8ms), don't go to sleep for this DTM.
+		// Because Some AP can't queuing outgoing frames immediately.
+		if (((pAd->Mlme.LastSendNULLpsmTime + 8) >= Now) && (pAd->Mlme.LastSendNULLpsmTime <= Now))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("Now = %lu, LastSendNULLpsmTime=%lu :  RxCountSinceLastNULL = %lu. \n", Now, pAd->Mlme.LastSendNULLpsmTime, pAd->RalinkCounters.RxCountSinceLastNULL));
+			return;
+		}
+		else if ((pAd->RalinkCounters.RxCountSinceLastNULL > 0) && ((pAd->Mlme.LastSendNULLpsmTime + pAd->CommonCfg.BeaconPeriod) >= Now))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("Now = %lu, LastSendNULLpsmTime=%lu: RxCountSinceLastNULL = %lu > 0 \n", Now, pAd->Mlme.LastSendNULLpsmTime,  pAd->RalinkCounters.RxCountSinceLastNULL));
+			return;
+		}
+
+        RT28xxPciAsicRadioOff(pAd, DOT11POWERSAVE, TbttNumToNextWakeUp);
+    }
+    else
+    {
+        AUTO_WAKEUP_STRUC	AutoWakeupCfg;
+        // we have decided to SLEEP, so at least do it for a BEACON period.
+        if (TbttNumToNextWakeUp == 0)
+            TbttNumToNextWakeUp = 1;
+
+        AutoWakeupCfg.word = 0;
+        RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+        AutoWakeupCfg.field.NumofSleepingTbtt = TbttNumToNextWakeUp - 1;
+        AutoWakeupCfg.field.EnableAutoWakeup = 1;
+        AutoWakeupCfg.field.AutoLeadTime = 5;
+        RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+        AsicSendCommandToMcu(pAd, 0x30, 0xff, 0xff, 0x00);   // send POWER-SAVE command to MCU. Timeout 40us.
+        DBGPRINT(RT_DEBUG_TRACE, ("<-- %s, TbttNumToNextWakeUp=%d \n", __func__, TbttNumToNextWakeUp));
+    }
+    OPSTATUS_SET_FLAG(pAd, fOP_STATUS_DOZE);
+}
+
+VOID PsPollWakeExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+	unsigned long flags;
+
+    DBGPRINT(RT_DEBUG_TRACE,("-->PsPollWakeExec \n"));
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+    if (pAd->Mlme.bPsPollTimerRunning)
+    {
+	    RTMPPCIeLinkCtrlValueRestore(pAd, RESTORE_WAKEUP);
+    }
+    pAd->Mlme.bPsPollTimerRunning = FALSE;
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+}
+
+VOID  RadioOnExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+	WPDMA_GLO_CFG_STRUC	DmaCfg;
+	BOOLEAN				Cancelled;
+
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("-->RadioOnExec() return on fOP_STATUS_DOZE == TRUE; \n"));
+		RTMPSetTimer(&pAd->Mlme.RadioOnOffTimer, 10);
+		return;
+	}
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("-->RadioOnExec() return on SCAN_IN_PROGRESS; \n"));
+		RTMPSetTimer(&pAd->Mlme.RadioOnOffTimer, 10);
+		return;
+	}
+    pAd->Mlme.bPsPollTimerRunning = FALSE;
+	RTMPCancelTimer(&pAd->Mlme.PsPollTimer,	&Cancelled);
+	if (pAd->StaCfg.bRadio == TRUE)
+	{
+		pAd->bPCIclkOff = FALSE;
+        RTMPRingCleanUp(pAd, QID_AC_BK);
+		RTMPRingCleanUp(pAd, QID_AC_BE);
+		RTMPRingCleanUp(pAd, QID_AC_VI);
+		RTMPRingCleanUp(pAd, QID_AC_VO);
+		RTMPRingCleanUp(pAd, QID_HCCA);
+		RTMPRingCleanUp(pAd, QID_MGMT);
+		RTMPRingCleanUp(pAd, QID_RX);
+
+		// 2. Send wake up command.
+		AsicSendCommandToMcu(pAd, 0x31, PowerWakeCID, 0x00, 0x02);
+		// 2-1. wait command ok.
+		AsicCheckCommanOk(pAd, PowerWakeCID);
+
+		// When PCI clock is off, don't want to service interrupt. So when back to clock on, enable interrupt.
+		NICEnableInterrupt(pAd);
+
+		// 3. Enable Tx DMA.
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
+		DmaCfg.field.EnableTxDMA = 1;
+		RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, DmaCfg.word);
+
+		// In Radio Off, we turn off RF clk, So now need to call ASICSwitchChannel again.
+		if (INFRA_ON(pAd) && (pAd->CommonCfg.CentralChannel != pAd->CommonCfg.Channel)
+			&& (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+		{
+			// Must using 40MHz.
+			AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+		}
+		else
+		{
+			// Must using 20MHz.
+			AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+		}
+
+		// Clear Radio off flag
+		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+		// Set LED
+		RTMPSetLED(pAd, LED_RADIO_ON);
+
+        if (pAd->StaCfg.Psm == PWR_ACTIVE)
+        {
+    		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, pAd->StaCfg.BBPR3);
+        }
+	}
+	else
+	{
+		RT28xxPciAsicRadioOff(pAd, GUIRADIO_OFF, 0);
+	}
+}
+
+#endif // CONFIG_STA_SUPPORT //
+
+VOID RT28xxPciMlmeRadioOn(
+	IN PRTMP_ADAPTER pAd)
+{
+    if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+   		return;
+
+    DBGPRINT(RT_DEBUG_TRACE,("%s===>\n", __func__));
+
+    if ((pAd->OpMode == OPMODE_AP) ||
+        ((pAd->OpMode == OPMODE_STA) && (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))))
+    {
+    	NICResetFromError(pAd);
+
+    	RTMPRingCleanUp(pAd, QID_AC_BK);
+    	RTMPRingCleanUp(pAd, QID_AC_BE);
+    	RTMPRingCleanUp(pAd, QID_AC_VI);
+    	RTMPRingCleanUp(pAd, QID_AC_VO);
+    	RTMPRingCleanUp(pAd, QID_HCCA);
+    	RTMPRingCleanUp(pAd, QID_MGMT);
+    	RTMPRingCleanUp(pAd, QID_RX);
+
+    	// Enable Tx/Rx
+    	RTMPEnableRxTx(pAd);
+
+    	// Clear Radio off flag
+    	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+	    // Set LED
+	    RTMPSetLED(pAd, LED_RADIO_ON);
+    }
+
+#ifdef CONFIG_STA_SUPPORT
+    if ((pAd->OpMode == OPMODE_STA) &&
+        (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE)))
+    {
+        BOOLEAN		Cancelled;
+
+    	RTMPPCIeLinkCtrlValueRestore(pAd, RESTORE_WAKEUP);
+
+        pAd->Mlme.bPsPollTimerRunning = FALSE;
+    	RTMPCancelTimer(&pAd->Mlme.PsPollTimer,	&Cancelled);
+    	RTMPCancelTimer(&pAd->Mlme.RadioOnOffTimer,	&Cancelled);
+    	RTMPSetTimer(&pAd->Mlme.RadioOnOffTimer, 10);
+    }
+#endif // CONFIG_STA_SUPPORT //
+}
+
+VOID RT28xxPciMlmeRadioOFF(
+	IN PRTMP_ADAPTER pAd)
+{
+    WPDMA_GLO_CFG_STRUC	GloCfg;
+	UINT32	i;
+
+    if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+    	return;
+
+    DBGPRINT(RT_DEBUG_TRACE,("%s===>\n", __func__));
+
+	// Set LED
+	RTMPSetLED(pAd, LED_RADIO_OFF);
+	// Set Radio off flag
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+    {
+    	BOOLEAN		Cancelled;
+    	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+    	{
+			RTMPCancelTimer(&pAd->MlmeAux.ScanTimer, &Cancelled);
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+    	}
+
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+        {
+            BOOLEAN Cancelled;
+            pAd->Mlme.bPsPollTimerRunning = FALSE;
+            RTMPCancelTimer(&pAd->Mlme.PsPollTimer,	&Cancelled);
+	        RTMPCancelTimer(&pAd->Mlme.RadioOnOffTimer,	&Cancelled);
+        }
+
+        // Link down first if any association exists
+        if (INFRA_ON(pAd) || ADHOC_ON(pAd))
+            LinkDown(pAd, FALSE);
+        RTMPusecDelay(10000);
+        //==========================================
+        // Clean up old bss table
+        BssTableInit(&pAd->ScanTab);
+    }
+#endif // CONFIG_STA_SUPPORT //
+
+	// Disable Tx/Rx DMA
+	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);	   // disable DMA
+	GloCfg.field.EnableTxDMA = 0;
+	GloCfg.field.EnableRxDMA = 0;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);	   // abort all TX rings
+
+
+	// MAC_SYS_CTRL => value = 0x0 => 40mA
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0);
+
+	// PWR_PIN_CFG => value = 0x0 => 40mA
+	RTMP_IO_WRITE32(pAd, PWR_PIN_CFG, 0);
+
+	// TX_PIN_CFG => value = 0x0 => 20mA
+	RTMP_IO_WRITE32(pAd, TX_PIN_CFG, 0);
+
+	if (pAd->CommonCfg.BBPCurrentBW == BW_40)
+	{
+		// Must using 40MHz.
+		AsicTurnOffRFClk(pAd, pAd->CommonCfg.CentralChannel);
+	}
+	else
+	{
+		// Must using 20MHz.
+		AsicTurnOffRFClk(pAd, pAd->CommonCfg.Channel);
+	}
+
+	// Waiting for DMA idle
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0) && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		RTMPusecDelay(1000);
+	}while (i++ < 100);
+}
diff --git a/drivers/staging/rt2860/common/cmm_info.c b/drivers/staging/rt2860/common/cmm_info.c
new file mode 100644
index 000000000000..dd92ac6eaed2
--- /dev/null
+++ b/drivers/staging/rt2860/common/cmm_info.c
@@ -0,0 +1,3417 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+
+#include "../rt_config.h"
+
+INT	Show_SSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_TxBurst_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_TxPreamble_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_BGProtection_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_RTSThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_FragThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+#ifdef DOT11_N_SUPPORT
+INT	Show_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // DOT11_N_SUPPORT //
+
+INT	Show_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_CountryCode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+#ifdef AGGREGATION_SUPPORT
+INT	Show_PktAggregate_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // AGGREGATION_SUPPORT //
+
+#ifdef WMM_SUPPORT
+INT	Show_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // WMM_SUPPORT //
+
+INT	Show_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+#ifdef CONFIG_STA_SUPPORT
+INT	Show_NetworkType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // CONFIG_STA_SUPPORT //
+
+INT	Show_AuthMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_EncrypType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_DefaultKeyID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_WPAPSK_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+static struct {
+	CHAR *name;
+	INT (*show_proc)(PRTMP_ADAPTER pAdapter, PUCHAR arg);
+} *PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC, RTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC[] = {
+	{"SSID",					Show_SSID_Proc},
+	{"WirelessMode",			Show_WirelessMode_Proc},
+	{"TxBurst",					Show_TxBurst_Proc},
+	{"TxPreamble",				Show_TxPreamble_Proc},
+	{"TxPower",					Show_TxPower_Proc},
+	{"Channel",					Show_Channel_Proc},
+	{"BGProtection",			Show_BGProtection_Proc},
+	{"RTSThreshold",			Show_RTSThreshold_Proc},
+	{"FragThreshold",			Show_FragThreshold_Proc},
+#ifdef DOT11_N_SUPPORT
+	{"HtBw",					Show_HtBw_Proc},
+	{"HtMcs",					Show_HtMcs_Proc},
+	{"HtGi",					Show_HtGi_Proc},
+	{"HtOpMode",				Show_HtOpMode_Proc},
+	{"HtExtcha",				Show_HtExtcha_Proc},
+	{"HtMpduDensity",			Show_HtMpduDensity_Proc},
+	{"HtBaWinSize",		        Show_HtBaWinSize_Proc},
+	{"HtRdg",		        	Show_HtRdg_Proc},
+	{"HtAmsdu",		        	Show_HtAmsdu_Proc},
+	{"HtAutoBa",		        Show_HtAutoBa_Proc},
+#endif // DOT11_N_SUPPORT //
+	{"CountryRegion",			Show_CountryRegion_Proc},
+	{"CountryRegionABand",		Show_CountryRegionABand_Proc},
+	{"CountryCode",				Show_CountryCode_Proc},
+#ifdef AGGREGATION_SUPPORT
+	{"PktAggregate",			Show_PktAggregate_Proc},
+#endif
+
+#ifdef WMM_SUPPORT
+	{"WmmCapable",				Show_WmmCapable_Proc},
+#endif
+	{"IEEE80211H",				Show_IEEE80211H_Proc},
+#ifdef CONFIG_STA_SUPPORT
+    {"NetworkType",				Show_NetworkType_Proc},
+#endif // CONFIG_STA_SUPPORT //
+	{"AuthMode",				Show_AuthMode_Proc},
+	{"EncrypType",				Show_EncrypType_Proc},
+	{"DefaultKeyID",			Show_DefaultKeyID_Proc},
+	{"Key1",					Show_Key1_Proc},
+	{"Key2",					Show_Key2_Proc},
+	{"Key3",					Show_Key3_Proc},
+	{"Key4",					Show_Key4_Proc},
+	{"WPAPSK",					Show_WPAPSK_Proc},
+	{NULL, NULL}
+};
+
+/*
+    ==========================================================================
+    Description:
+        Get Driver version.
+
+    Return:
+    ==========================================================================
+*/
+INT Set_DriverVersion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		DBGPRINT(RT_DEBUG_TRACE, ("Driver version-%s\n", STA_DRIVER_VERSION));
+#endif // CONFIG_STA_SUPPORT //
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Country Region.
+        This command will not work, if the field of CountryRegion in eeprom is programmed.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG region;
+
+	region = simple_strtol(arg, 0, 10);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	return -EOPNOTSUPP;
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// Country can be set only when EEPROM not programmed
+	if (pAd->CommonCfg.CountryRegion & 0x80)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegion_Proc::parameter of CountryRegion in eeprom is programmed \n"));
+		return FALSE;
+	}
+
+	if((region >= 0) && (region <= REGION_MAXIMUM_BG_BAND))
+	{
+		pAd->CommonCfg.CountryRegion = (UCHAR) region;
+	}
+	else if (region == REGION_31_BG_BAND)
+	{
+		pAd->CommonCfg.CountryRegion = (UCHAR) region;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegion_Proc::parameters out of range\n"));
+		return FALSE;
+	}
+
+	// if set country region, driver needs to be reset
+	BuildChannelList(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_CountryRegion_Proc::(CountryRegion=%d)\n", pAd->CommonCfg.CountryRegion));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Country Region for A band.
+        This command will not work, if the field of CountryRegion in eeprom is programmed.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG region;
+
+	region = simple_strtol(arg, 0, 10);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	return -EOPNOTSUPP;
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// Country can be set only when EEPROM not programmed
+	if (pAd->CommonCfg.CountryRegionForABand & 0x80)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegionABand_Proc::parameter of CountryRegion in eeprom is programmed \n"));
+		return FALSE;
+	}
+
+	if((region >= 0) && (region <= REGION_MAXIMUM_A_BAND))
+	{
+		pAd->CommonCfg.CountryRegionForABand = (UCHAR) region;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegionABand_Proc::parameters out of range\n"));
+		return FALSE;
+	}
+
+	// if set country region, driver needs to be reset
+	BuildChannelList(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_CountryRegionABand_Proc::(CountryRegion=%d)\n", pAd->CommonCfg.CountryRegionForABand));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Wireless Mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG	WirelessMode;
+	INT		success = TRUE;
+
+	WirelessMode = simple_strtol(arg, 0, 10);
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		INT MaxPhyMode = PHY_11G;
+
+#ifdef DOT11_N_SUPPORT
+		MaxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+		if (WirelessMode <= MaxPhyMode)
+		{
+			RTMPSetPhyMode(pAd, WirelessMode);
+#ifdef DOT11_N_SUPPORT
+			if (WirelessMode >= PHY_11ABGN_MIXED)
+			{
+				pAd->CommonCfg.BACapability.field.AutoBA = TRUE;
+				pAd->CommonCfg.REGBACapability.field.AutoBA = TRUE;
+			}
+			else
+			{
+				pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+				pAd->CommonCfg.REGBACapability.field.AutoBA = FALSE;
+			}
+#endif // DOT11_N_SUPPORT //
+			// Set AdhocMode rates
+			if (pAd->StaCfg.BssType == BSS_ADHOC)
+			{
+				MlmeUpdateTxRates(pAd, FALSE, 0);
+				MakeIbssBeacon(pAd);           // re-build BEACON frame
+				AsicEnableIbssSync(pAd);       // copy to on-chip memory
+			}
+		}
+		else
+		{
+			success = FALSE;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// it is needed to set SSID to take effect
+	if (success == TRUE)
+	{
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+		DBGPRINT(RT_DEBUG_TRACE, ("Set_WirelessMode_Proc::(=%ld)\n", WirelessMode));
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_WirelessMode_Proc::parameters out of range\n"));
+	}
+
+	return success;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Channel
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+ 	INT		success = TRUE;
+	UCHAR	Channel;
+
+	Channel = (UCHAR) simple_strtol(arg, 0, 10);
+
+	// check if this channel is valid
+	if (ChannelSanity(pAd, Channel) == TRUE)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			pAd->CommonCfg.Channel = Channel;
+
+			if (MONITOR_ON(pAd))
+			{
+#ifdef DOT11_N_SUPPORT
+				N_ChannelCheck(pAd);
+				if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+					pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40)
+				{
+					N_SetCenCh(pAd);
+					AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+					DBGPRINT(RT_DEBUG_TRACE, ("BW_40, control_channel(%d), CentralChannel(%d) \n",
+								pAd->CommonCfg.Channel, pAd->CommonCfg.CentralChannel));
+				}
+				else
+#endif // DOT11_N_SUPPORT //
+				{
+					AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+					DBGPRINT(RT_DEBUG_TRACE, ("BW_20, Channel(%d)\n", pAd->CommonCfg.Channel));
+				}
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+		success = TRUE;
+	}
+	else
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			success = FALSE;
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+	if (success == TRUE)
+		DBGPRINT(RT_DEBUG_TRACE, ("Set_Channel_Proc::(Channel=%d)\n", pAd->CommonCfg.Channel));
+
+	return success;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Short Slot Time Enable or Disable
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ShortSlot_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG ShortSlot;
+
+	ShortSlot = simple_strtol(arg, 0, 10);
+
+	if (ShortSlot == 1)
+		pAd->CommonCfg.bUseShortSlotTime = TRUE;
+	else if (ShortSlot == 0)
+		pAd->CommonCfg.bUseShortSlotTime = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_ShortSlot_Proc::(ShortSlot=%d)\n", pAd->CommonCfg.bUseShortSlotTime));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Tx power
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG TxPower;
+	INT   success = FALSE;
+
+	TxPower = (ULONG) simple_strtol(arg, 0, 10);
+	if (TxPower <= 100)
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			pAd->CommonCfg.TxPowerDefault = TxPower;
+			pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+		}
+#endif // CONFIG_STA_SUPPORT //
+		success = TRUE;
+	}
+	else
+		success = FALSE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_TxPower_Proc::(TxPowerPercentage=%ld)\n", pAd->CommonCfg.TxPowerPercentage));
+
+	return success;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set 11B/11G Protection
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_BGProtection_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	switch (simple_strtol(arg, 0, 10))
+	{
+		case 0: //AUTO
+			pAd->CommonCfg.UseBGProtection = 0;
+			break;
+		case 1: //Always On
+			pAd->CommonCfg.UseBGProtection = 1;
+			break;
+		case 2: //Always OFF
+			pAd->CommonCfg.UseBGProtection = 2;
+			break;
+		default:  //Invalid argument
+			return FALSE;
+	}
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_BGProtection_Proc::(BGProtection=%ld)\n", pAd->CommonCfg.UseBGProtection));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set TxPreamble
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_TxPreamble_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	RT_802_11_PREAMBLE	Preamble;
+
+	Preamble = simple_strtol(arg, 0, 10);
+
+
+	switch (Preamble)
+	{
+		case Rt802_11PreambleShort:
+			pAd->CommonCfg.TxPreamble = Preamble;
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				MlmeSetTxPreamble(pAd, Rt802_11PreambleShort);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+		case Rt802_11PreambleLong:
+#ifdef CONFIG_STA_SUPPORT
+		case Rt802_11PreambleAuto:
+			// if user wants AUTO, initialize to LONG here, then change according to AP's
+			// capability upon association.
+#endif // CONFIG_STA_SUPPORT //
+			pAd->CommonCfg.TxPreamble = Preamble;
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				MlmeSetTxPreamble(pAd, Rt802_11PreambleLong);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+		default: //Invalid argument
+			return FALSE;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_TxPreamble_Proc::(TxPreamble=%ld)\n", pAd->CommonCfg.TxPreamble));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set RTS Threshold
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_RTSThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	 NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+
+	RtsThresh = simple_strtol(arg, 0, 10);
+
+	if((RtsThresh > 0) && (RtsThresh <= MAX_RTS_THRESHOLD))
+		pAd->CommonCfg.RtsThreshold  = (USHORT)RtsThresh;
+#ifdef CONFIG_STA_SUPPORT
+	else if (RtsThresh == 0)
+		pAd->CommonCfg.RtsThreshold = MAX_RTS_THRESHOLD;
+#endif // CONFIG_STA_SUPPORT //
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_RTSThreshold_Proc::(RTSThreshold=%d)\n", pAd->CommonCfg.RtsThreshold));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Fragment Threshold
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_FragThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	 NDIS_802_11_FRAGMENTATION_THRESHOLD     FragThresh;
+
+	FragThresh = simple_strtol(arg, 0, 10);
+
+	if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+	{
+		//Illegal FragThresh so we set it to default
+		pAd->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+	}
+	else if (FragThresh % 2 == 1)
+	{
+		// The length of each fragment shall always be an even number of octets, except for the last fragment
+		// of an MSDU or MMPDU, which may be either an even or an odd number of octets.
+		pAd->CommonCfg.FragmentThreshold = (USHORT)(FragThresh - 1);
+	}
+	else
+	{
+		pAd->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->CommonCfg.FragmentThreshold == MAX_FRAG_THRESHOLD)
+			pAd->CommonCfg.bUseZeroToDisableFragment = TRUE;
+		else
+			pAd->CommonCfg.bUseZeroToDisableFragment = FALSE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_FragThreshold_Proc::(FragThreshold=%d)\n", pAd->CommonCfg.FragmentThreshold));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set TxBurst
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_TxBurst_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG TxBurst;
+
+	TxBurst = simple_strtol(arg, 0, 10);
+	if (TxBurst == 1)
+		pAd->CommonCfg.bEnableTxBurst = TRUE;
+	else if (TxBurst == 0)
+		pAd->CommonCfg.bEnableTxBurst = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_TxBurst_Proc::(TxBurst=%d)\n", pAd->CommonCfg.bEnableTxBurst));
+
+	return TRUE;
+}
+
+#ifdef AGGREGATION_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set TxBurst
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_PktAggregate_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG aggre;
+
+	aggre = simple_strtol(arg, 0, 10);
+
+	if (aggre == 1)
+		pAd->CommonCfg.bAggregationCapable = TRUE;
+	else if (aggre == 0)
+		pAd->CommonCfg.bAggregationCapable = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_PktAggregate_Proc::(AGGRE=%d)\n", pAd->CommonCfg.bAggregationCapable));
+
+	return TRUE;
+}
+#endif
+
+/*
+    ==========================================================================
+    Description:
+        Set IEEE80211H.
+        This parameter is 1 when needs radar detection, otherwise 0
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    ULONG ieee80211h;
+
+	ieee80211h = simple_strtol(arg, 0, 10);
+
+	if (ieee80211h == 1)
+		pAd->CommonCfg.bIEEE80211H = TRUE;
+	else if (ieee80211h == 0)
+		pAd->CommonCfg.bIEEE80211H = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_IEEE80211H_Proc::(IEEE80211H=%d)\n", pAd->CommonCfg.bIEEE80211H));
+
+	return TRUE;
+}
+
+
+#ifdef DBG
+/*
+    ==========================================================================
+    Description:
+        For Debug information
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_Debug_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("==> Set_Debug_Proc *******************\n"));
+
+    if(simple_strtol(arg, 0, 10) <= RT_DEBUG_LOUD)
+        RTDebugLevel = simple_strtol(arg, 0, 10);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== Set_Debug_Proc(RTDebugLevel = %ld)\n", RTDebugLevel));
+
+	return TRUE;
+}
+#endif
+
+INT	Show_DescInfo_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+#ifdef RT2860
+	INT i, QueIdx=0;
+	PRT28XX_RXD_STRUC pRxD;
+    PTXD_STRUC pTxD;
+	PRTMP_TX_RING	pTxRing = &pAd->TxRing[QueIdx];
+	PRTMP_MGMT_RING	pMgmtRing = &pAd->MgmtRing;
+	PRTMP_RX_RING	pRxRing = &pAd->RxRing;
+
+	for(i=0;i<TX_RING_SIZE;i++)
+	{
+	    pTxD = (PTXD_STRUC) pTxRing->Cell[i].AllocVa;
+	    printk("Desc #%d\n",i);
+	    hex_dump("Tx Descriptor", (char *)pTxD, 16);
+	    printk("pTxD->DMADONE = %x\n", pTxD->DMADONE);
+	}
+	printk("---------------------------------------------------\n");
+	for(i=0;i<MGMT_RING_SIZE;i++)
+	{
+	    pTxD = (PTXD_STRUC) pMgmtRing->Cell[i].AllocVa;
+	    printk("Desc #%d\n",i);
+	    hex_dump("Mgmt Descriptor", (char *)pTxD, 16);
+	    printk("pMgmt->DMADONE = %x\n", pTxD->DMADONE);
+	}
+	printk("---------------------------------------------------\n");
+	for(i=0;i<RX_RING_SIZE;i++)
+	{
+	    pRxD = (PRT28XX_RXD_STRUC) pRxRing->Cell[i].AllocVa;
+	    printk("Desc #%d\n",i);
+	    hex_dump("Rx Descriptor", (char *)pRxD, 16);
+		printk("pRxD->DDONE = %x\n", pRxD->DDONE);
+	}
+#endif // RT2860 //
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Reset statistics counter
+
+    Arguments:
+        pAdapter            Pointer to our adapter
+        arg
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ResetStatCounter_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("==>Set_ResetStatCounter_Proc\n"));
+
+	// add the most up-to-date h/w raw counters into software counters
+	NICUpdateRawCounters(pAd);
+
+	NdisZeroMemory(&pAd->WlanCounters, sizeof(COUNTER_802_11));
+	NdisZeroMemory(&pAd->Counters8023, sizeof(COUNTER_802_3));
+	NdisZeroMemory(&pAd->RalinkCounters, sizeof(COUNTER_RALINK));
+
+	return TRUE;
+}
+
+BOOLEAN RTMPCheckStrPrintAble(
+    IN  CHAR *pInPutStr,
+    IN  UCHAR strLen)
+{
+    UCHAR i=0;
+
+    for (i=0; i<strLen; i++)
+    {
+        if ((pInPutStr[i] < 0x21) ||
+            (pInPutStr[i] > 0x7E))
+            return FALSE;
+    }
+
+    return TRUE;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Remove WPA Key process
+
+	Arguments:
+		pAd 					Pointer to our adapter
+		pBuf							Pointer to the where the key stored
+
+	Return Value:
+		NDIS_SUCCESS					Add key successfully
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+#ifdef CONFIG_STA_SUPPORT
+VOID    RTMPSetDesiredRates(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  LONG            Rates)
+{
+    NDIS_802_11_RATES aryRates;
+
+    memset(&aryRates, 0x00, sizeof(NDIS_802_11_RATES));
+    switch (pAdapter->CommonCfg.PhyMode)
+    {
+        case PHY_11A: // A only
+            switch (Rates)
+            {
+                case 6000000: //6M
+                    aryRates[0] = 0x0c; // 6M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_0;
+                    break;
+                case 9000000: //9M
+                    aryRates[0] = 0x12; // 9M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_1;
+                    break;
+                case 12000000: //12M
+                    aryRates[0] = 0x18; // 12M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_2;
+                    break;
+                case 18000000: //18M
+                    aryRates[0] = 0x24; // 18M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_3;
+                    break;
+                case 24000000: //24M
+                    aryRates[0] = 0x30; // 24M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_4;
+                    break;
+                case 36000000: //36M
+                    aryRates[0] = 0x48; // 36M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_5;
+                    break;
+                case 48000000: //48M
+                    aryRates[0] = 0x60; // 48M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_6;
+                    break;
+                case 54000000: //54M
+                    aryRates[0] = 0x6c; // 54M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_7;
+                    break;
+                case -1: //Auto
+                default:
+                    aryRates[0] = 0x6c; // 54Mbps
+                    aryRates[1] = 0x60; // 48Mbps
+                    aryRates[2] = 0x48; // 36Mbps
+                    aryRates[3] = 0x30; // 24Mbps
+                    aryRates[4] = 0x24; // 18M
+                    aryRates[5] = 0x18; // 12M
+                    aryRates[6] = 0x12; // 9M
+                    aryRates[7] = 0x0c; // 6M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+                    break;
+            }
+            break;
+        case PHY_11BG_MIXED: // B/G Mixed
+        case PHY_11B: // B only
+        case PHY_11ABG_MIXED: // A/B/G Mixed
+        default:
+            switch (Rates)
+            {
+                case 1000000: //1M
+                    aryRates[0] = 0x02;
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_0;
+                    break;
+                case 2000000: //2M
+                    aryRates[0] = 0x04;
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_1;
+                    break;
+                case 5000000: //5.5M
+                    aryRates[0] = 0x0b; // 5.5M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_2;
+                    break;
+                case 11000000: //11M
+                    aryRates[0] = 0x16; // 11M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_3;
+                    break;
+                case 6000000: //6M
+                    aryRates[0] = 0x0c; // 6M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_0;
+                    break;
+                case 9000000: //9M
+                    aryRates[0] = 0x12; // 9M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_1;
+                    break;
+                case 12000000: //12M
+                    aryRates[0] = 0x18; // 12M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_2;
+                    break;
+                case 18000000: //18M
+                    aryRates[0] = 0x24; // 18M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_3;
+                    break;
+                case 24000000: //24M
+                    aryRates[0] = 0x30; // 24M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_4;
+                    break;
+                case 36000000: //36M
+                    aryRates[0] = 0x48; // 36M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_5;
+                    break;
+                case 48000000: //48M
+                    aryRates[0] = 0x60; // 48M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_6;
+                    break;
+                case 54000000: //54M
+                    aryRates[0] = 0x6c; // 54M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_7;
+                    break;
+                case -1: //Auto
+                default:
+                    if (pAdapter->CommonCfg.PhyMode == PHY_11B)
+                    { //B Only
+                        aryRates[0] = 0x16; // 11Mbps
+                        aryRates[1] = 0x0b; // 5.5Mbps
+                        aryRates[2] = 0x04; // 2Mbps
+                        aryRates[3] = 0x02; // 1Mbps
+                    }
+                    else
+                    { //(B/G) Mixed or (A/B/G) Mixed
+                        aryRates[0] = 0x6c; // 54Mbps
+                        aryRates[1] = 0x60; // 48Mbps
+                        aryRates[2] = 0x48; // 36Mbps
+                        aryRates[3] = 0x30; // 24Mbps
+                        aryRates[4] = 0x16; // 11Mbps
+                        aryRates[5] = 0x0b; // 5.5Mbps
+                        aryRates[6] = 0x04; // 2Mbps
+                        aryRates[7] = 0x02; // 1Mbps
+                    }
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+                    break;
+            }
+            break;
+    }
+
+    NdisZeroMemory(pAdapter->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+    NdisMoveMemory(pAdapter->CommonCfg.DesireRate, &aryRates, sizeof(NDIS_802_11_RATES));
+    DBGPRINT(RT_DEBUG_TRACE, (" RTMPSetDesiredRates (%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x)\n",
+        pAdapter->CommonCfg.DesireRate[0],pAdapter->CommonCfg.DesireRate[1],
+        pAdapter->CommonCfg.DesireRate[2],pAdapter->CommonCfg.DesireRate[3],
+        pAdapter->CommonCfg.DesireRate[4],pAdapter->CommonCfg.DesireRate[5],
+        pAdapter->CommonCfg.DesireRate[6],pAdapter->CommonCfg.DesireRate[7] ));
+    // Changing DesiredRate may affect the MAX TX rate we used to TX frames out
+    MlmeUpdateTxRates(pAdapter, FALSE, 0);
+}
+
+NDIS_STATUS RTMPWPARemoveKeyProc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PVOID			pBuf)
+{
+	PNDIS_802_11_REMOVE_KEY pKey;
+	ULONG					KeyIdx;
+	NDIS_STATUS 			Status = NDIS_STATUS_FAILURE;
+	BOOLEAN 	bTxKey; 		// Set the key as transmit key
+	BOOLEAN 	bPairwise;		// Indicate the key is pairwise key
+	BOOLEAN 	bKeyRSC;		// indicate the receive  SC set by KeyRSC value.
+								// Otherwise, it will set by the NIC.
+	BOOLEAN 	bAuthenticator; // indicate key is set by authenticator.
+	INT 		i;
+
+	DBGPRINT(RT_DEBUG_TRACE,("---> RTMPWPARemoveKeyProc\n"));
+
+	pKey = (PNDIS_802_11_REMOVE_KEY) pBuf;
+	KeyIdx = pKey->KeyIndex & 0xff;
+	// Bit 31 of Add-key, Tx Key
+	bTxKey		   = (pKey->KeyIndex & 0x80000000) ? TRUE : FALSE;
+	// Bit 30 of Add-key PairwiseKey
+	bPairwise	   = (pKey->KeyIndex & 0x40000000) ? TRUE : FALSE;
+	// Bit 29 of Add-key KeyRSC
+	bKeyRSC 	   = (pKey->KeyIndex & 0x20000000) ? TRUE : FALSE;
+	// Bit 28 of Add-key Authenticator
+	bAuthenticator = (pKey->KeyIndex & 0x10000000) ? TRUE : FALSE;
+
+	// 1. If bTx is TRUE, return failure information
+	if (bTxKey == TRUE)
+		return(NDIS_STATUS_INVALID_DATA);
+
+	// 2. Check Pairwise Key
+	if (bPairwise)
+	{
+		// a. If BSSID is broadcast, remove all pairwise keys.
+		// b. If not broadcast, remove the pairwise specified by BSSID
+		for (i = 0; i < SHARE_KEY_NUM; i++)
+		{
+			if (MAC_ADDR_EQUAL(pAd->SharedKey[BSS0][i].BssId, pKey->BSSID))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("RTMPWPARemoveKeyProc(KeyIdx=%d)\n", i));
+				pAd->SharedKey[BSS0][i].KeyLen = 0;
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_NONE;
+				AsicRemoveSharedKeyEntry(pAd, BSS0, (UCHAR)i);
+				Status = NDIS_STATUS_SUCCESS;
+				break;
+			}
+		}
+	}
+	// 3. Group Key
+	else
+	{
+		// a. If BSSID is broadcast, remove all group keys indexed
+		// b. If BSSID matched, delete the group key indexed.
+		DBGPRINT(RT_DEBUG_TRACE,("RTMPWPARemoveKeyProc(KeyIdx=%ld)\n", KeyIdx));
+		pAd->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+		pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+		AsicRemoveSharedKeyEntry(pAd, BSS0, (UCHAR)KeyIdx);
+		Status = NDIS_STATUS_SUCCESS;
+	}
+
+	return (Status);
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	========================================================================
+
+	Routine Description:
+		Remove All WPA Keys
+
+	Arguments:
+		pAd 					Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPWPARemoveAllKeys(
+	IN	PRTMP_ADAPTER	pAd)
+{
+
+	UCHAR 	i;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPWPARemoveAllKeys(AuthMode=%d, WepStatus=%d)\n", pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus));
+
+	// For WEP/CKIP, there is no need to remove it, since WinXP won't set it again after
+	// Link up. And it will be replaced if user changed it.
+	if (pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+		return;
+
+	// For WPA-None, there is no need to remove it, since WinXP won't set it again after
+	// Link up. And it will be replaced if user changed it.
+	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		return;
+
+	// set BSSID wcid entry of the Pair-wise Key table as no-security mode
+	AsicRemovePairwiseKeyEntry(pAd, BSS0, BSSID_WCID);
+
+	// set all shared key mode as no-security.
+	for (i = 0; i < SHARE_KEY_NUM; i++)
+    {
+		DBGPRINT(RT_DEBUG_TRACE,("remove %s key #%d\n", CipherName[pAd->SharedKey[BSS0][i].CipherAlg], i));
+		NdisZeroMemory(&pAd->SharedKey[BSS0][i], sizeof(CIPHER_KEY));
+
+		AsicRemoveSharedKeyEntry(pAd, BSS0, i);
+	}
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	========================================================================
+	Routine Description:
+		Change NIC PHY mode. Re-association may be necessary. possible settings
+		include - PHY_11B, PHY_11BG_MIXED, PHY_11A, and PHY_11ABG_MIXED
+
+	Arguments:
+		pAd - Pointer to our adapter
+		phymode  -
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPSetPhyMode(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ULONG phymode)
+{
+	INT i;
+	// the selected phymode must be supported by the RF IC encoded in E2PROM
+
+	// if no change, do nothing
+	/* bug fix
+	if (pAd->CommonCfg.PhyMode == phymode)
+		return;
+    */
+	pAd->CommonCfg.PhyMode = (UCHAR)phymode;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPSetPhyMode : PhyMode=%d, channel=%d \n", pAd->CommonCfg.PhyMode, pAd->CommonCfg.Channel));
+#ifdef EXT_BUILD_CHANNEL_LIST
+	BuildChannelListEx(pAd);
+#else
+	BuildChannelList(pAd);
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// sanity check user setting
+	for (i = 0; i < pAd->ChannelListNum; i++)
+	{
+		if (pAd->CommonCfg.Channel == pAd->ChannelList[i].Channel)
+			break;
+	}
+
+	if (i == pAd->ChannelListNum)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			pAd->CommonCfg.Channel = FirstChannel(pAd);
+#endif // CONFIG_STA_SUPPORT //
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSetPhyMode: channel is out of range, use first channel=%d \n", pAd->CommonCfg.Channel));
+	}
+
+	NdisZeroMemory(pAd->CommonCfg.SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+	NdisZeroMemory(pAd->CommonCfg.ExtRate, MAX_LEN_OF_SUPPORTED_RATES);
+	NdisZeroMemory(pAd->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+	switch (phymode) {
+		case PHY_11B:
+			pAd->CommonCfg.SupRate[0]  = 0x82;	  // 1 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[1]  = 0x84;	  // 2 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[2]  = 0x8B;	  // 5.5 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[3]  = 0x96;	  // 11 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRateLen  = 4;
+			pAd->CommonCfg.ExtRateLen  = 0;
+			pAd->CommonCfg.DesireRate[0]  = 2;	   // 1 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[1]  = 4;	   // 2 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[2]  = 11;    // 5.5 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[3]  = 22;    // 11 mbps, in units of 0.5 Mbps
+			//pAd->CommonCfg.HTPhyMode.field.MODE = MODE_CCK; // This MODE is only FYI. not use
+			break;
+
+		case PHY_11G:
+		case PHY_11BG_MIXED:
+		case PHY_11ABG_MIXED:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11N_2_4G:
+		case PHY_11ABGN_MIXED:
+		case PHY_11BGN_MIXED:
+		case PHY_11GN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			pAd->CommonCfg.SupRate[0]  = 0x82;	  // 1 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[1]  = 0x84;	  // 2 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[2]  = 0x8B;	  // 5.5 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[3]  = 0x96;	  // 11 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[4]  = 0x12;	  // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[5]  = 0x24;	  // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[6]  = 0x48;	  // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[7]  = 0x6c;	  // 54 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRateLen  = 8;
+			pAd->CommonCfg.ExtRate[0]  = 0x0C;	  // 6 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRate[1]  = 0x18;	  // 12 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRate[2]  = 0x30;	  // 24 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRate[3]  = 0x60;	  // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRateLen  = 4;
+			pAd->CommonCfg.DesireRate[0]  = 2;	   // 1 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[1]  = 4;	   // 2 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[2]  = 11;    // 5.5 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[3]  = 22;    // 11 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[4]  = 12;    // 6 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[5]  = 18;    // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[6]  = 24;    // 12 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[7]  = 36;    // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[8]  = 48;    // 24 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[9]  = 72;    // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[10] = 96;    // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[11] = 108;   // 54 mbps, in units of 0.5 Mbps
+			break;
+
+		case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11AN_MIXED:
+		case PHY_11AGN_MIXED:
+		case PHY_11N_5G:
+#endif // DOT11_N_SUPPORT //
+			pAd->CommonCfg.SupRate[0]  = 0x8C;	  // 6 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[1]  = 0x12;	  // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[2]  = 0x98;	  // 12 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[3]  = 0x24;	  // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[4]  = 0xb0;	  // 24 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[5]  = 0x48;	  // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[6]  = 0x60;	  // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[7]  = 0x6c;	  // 54 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRateLen  = 8;
+			pAd->CommonCfg.ExtRateLen  = 0;
+			pAd->CommonCfg.DesireRate[0]  = 12;    // 6 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[1]  = 18;    // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[2]  = 24;    // 12 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[3]  = 36;    // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[4]  = 48;    // 24 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[5]  = 72;    // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[6]  = 96;    // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[7]  = 108;   // 54 mbps, in units of 0.5 Mbps
+			//pAd->CommonCfg.HTPhyMode.field.MODE = MODE_OFDM; // This MODE is only FYI. not use
+			break;
+
+		default:
+			break;
+	}
+
+
+	pAd->CommonCfg.BandState = UNKNOWN_BAND;
+}
+
+
+#ifdef DOT11_N_SUPPORT
+/*
+	========================================================================
+	Routine Description:
+		Caller ensures we has 802.11n support.
+		Calls at setting HT from AP/STASetinformation
+
+	Arguments:
+		pAd - Pointer to our adapter
+		phymode  -
+
+	========================================================================
+*/
+VOID	RTMPSetHT(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	OID_SET_HT_PHYMODE *pHTPhyMode)
+{
+	//ULONG	*pmcs;
+	UINT32	Value = 0;
+	UCHAR	BBPValue = 0;
+	UCHAR	BBP3Value = 0;
+	UCHAR	RxStream = pAd->CommonCfg.RxStream;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetHT : HT_mode(%d), ExtOffset(%d), MCS(%d), BW(%d), STBC(%d), SHORTGI(%d)\n",
+										pHTPhyMode->HtMode, pHTPhyMode->ExtOffset,
+										pHTPhyMode->MCS, pHTPhyMode->BW,
+										pHTPhyMode->STBC, pHTPhyMode->SHORTGI));
+
+	// Don't zero supportedHyPhy structure.
+	RTMPZeroMemory(&pAd->CommonCfg.HtCapability, sizeof(pAd->CommonCfg.HtCapability));
+	RTMPZeroMemory(&pAd->CommonCfg.AddHTInfo, sizeof(pAd->CommonCfg.AddHTInfo));
+	RTMPZeroMemory(&pAd->CommonCfg.NewExtChanOffset, sizeof(pAd->CommonCfg.NewExtChanOffset));
+	RTMPZeroMemory(&pAd->CommonCfg.DesiredHtPhy, sizeof(pAd->CommonCfg.DesiredHtPhy));
+
+   	if (pAd->CommonCfg.bRdg)
+	{
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.PlusHTC = 1;
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.RDGSupport = 1;
+	}
+	else
+	{
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.PlusHTC = 0;
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.RDGSupport = 0;
+	}
+
+	pAd->CommonCfg.HtCapability.HtCapParm.MaxRAmpduFactor = 3;
+	pAd->CommonCfg.DesiredHtPhy.MaxRAmpduFactor = 3;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetHT : RxBAWinLimit = %d\n", pAd->CommonCfg.BACapability.field.RxBAWinLimit));
+
+	// Mimo power save, A-MSDU size,
+	pAd->CommonCfg.DesiredHtPhy.AmsduEnable = (USHORT)pAd->CommonCfg.BACapability.field.AmsduEnable;
+	pAd->CommonCfg.DesiredHtPhy.AmsduSize = (UCHAR)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.DesiredHtPhy.MimoPs = (UCHAR)pAd->CommonCfg.BACapability.field.MMPSmode;
+	pAd->CommonCfg.DesiredHtPhy.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+
+	pAd->CommonCfg.HtCapability.HtCapInfo.AMsduSize = (USHORT)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.HtCapability.HtCapInfo.MimoPs = (USHORT)pAd->CommonCfg.BACapability.field.MMPSmode;
+	pAd->CommonCfg.HtCapability.HtCapParm.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetHT : AMsduSize = %d, MimoPs = %d, MpduDensity = %d, MaxRAmpduFactor = %d\n",
+													pAd->CommonCfg.DesiredHtPhy.AmsduSize,
+													pAd->CommonCfg.DesiredHtPhy.MimoPs,
+													pAd->CommonCfg.DesiredHtPhy.MpduDensity,
+													pAd->CommonCfg.DesiredHtPhy.MaxRAmpduFactor));
+
+	if(pHTPhyMode->HtMode == HTMODE_GF)
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.GF = 1;
+		pAd->CommonCfg.DesiredHtPhy.GF = 1;
+	}
+	else
+		pAd->CommonCfg.DesiredHtPhy.GF = 0;
+
+	// Decide Rx MCSSet
+	switch (RxStream)
+	{
+		case 1:
+			pAd->CommonCfg.HtCapability.MCSSet[0] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[1] =  0x00;
+			break;
+
+		case 2:
+			pAd->CommonCfg.HtCapability.MCSSet[0] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[1] =  0xff;
+			break;
+
+		case 3: // 3*3
+			pAd->CommonCfg.HtCapability.MCSSet[0] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[1] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[2] =  0xff;
+			break;
+	}
+
+	if (pAd->CommonCfg.bForty_Mhz_Intolerant && (pAd->CommonCfg.Channel <= 14) && (pHTPhyMode->BW == BW_40) )
+	{
+		pHTPhyMode->BW = BW_20;
+		pAd->CommonCfg.HtCapability.HtCapInfo.Forty_Mhz_Intolerant = 1;
+	}
+
+	if(pHTPhyMode->BW == BW_40)
+	{
+		pAd->CommonCfg.HtCapability.MCSSet[4] = 0x1; // MCS 32
+		pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth = 1;
+		if (pAd->CommonCfg.Channel <= 14)
+			pAd->CommonCfg.HtCapability.HtCapInfo.CCKmodein40 = 1;
+
+		pAd->CommonCfg.DesiredHtPhy.ChannelWidth = 1;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth = 1;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset = (pHTPhyMode->ExtOffset == EXTCHA_BELOW)? (EXTCHA_BELOW): EXTCHA_ABOVE;
+		// Set Regsiter for extension channel position.
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBP3Value);
+		if ((pHTPhyMode->ExtOffset == EXTCHA_BELOW))
+		{
+			Value |= 0x1;
+			BBP3Value |= (0x20);
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+		}
+		else if ((pHTPhyMode->ExtOffset == EXTCHA_ABOVE))
+		{
+			Value &= 0xfe;
+			BBP3Value &= (~0x20);
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+		}
+
+		// Turn on BBP 40MHz mode now only as AP .
+		// Sta can turn on BBP 40MHz after connection with 40MHz AP. Sta only broadcast 40MHz capability before connection.
+		if ((pAd->OpMode == OPMODE_AP) || INFRA_ON(pAd) || ADHOC_ON(pAd)
+			)
+		{
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue &= (~0x18);
+			BBPValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBP3Value);
+			pAd->CommonCfg.BBPCurrentBW = BW_40;
+		}
+	}
+	else
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth = 0;
+		pAd->CommonCfg.DesiredHtPhy.ChannelWidth = 0;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth = 0;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset = EXTCHA_NONE;
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+		// Turn on BBP 20MHz mode by request here.
+		{
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue &= (~0x18);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			pAd->CommonCfg.BBPCurrentBW = BW_20;
+		}
+	}
+
+	if(pHTPhyMode->STBC == STBC_USE)
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.TxSTBC = 1;
+		pAd->CommonCfg.DesiredHtPhy.TxSTBC = 1;
+		pAd->CommonCfg.HtCapability.HtCapInfo.RxSTBC = 1;
+		pAd->CommonCfg.DesiredHtPhy.RxSTBC = 1;
+	}
+	else
+	{
+		pAd->CommonCfg.DesiredHtPhy.TxSTBC = 0;
+		pAd->CommonCfg.DesiredHtPhy.RxSTBC = 0;
+	}
+
+
+	if(pHTPhyMode->SHORTGI == GI_400)
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor20 = 1;
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor40 = 1;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor20 = 1;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor40 = 1;
+	}
+	else
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor20 = 0;
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor40 = 0;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor20 = 0;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor40 = 0;
+	}
+
+	// We support link adaptation for unsolicit MCS feedback, set to 2.
+	pAd->CommonCfg.HtCapability.ExtHtCapInfo.MCSFeedback = MCSFBK_NONE; //MCSFBK_UNSOLICIT;
+	pAd->CommonCfg.AddHTInfo.ControlChan = pAd->CommonCfg.Channel;
+	// 1, the extension channel above the control channel.
+
+	// EDCA parameters used for AP's own transmission
+	if (pAd->CommonCfg.APEdcaParm.bValid == FALSE)
+	{
+		pAd->CommonCfg.APEdcaParm.bValid = TRUE;
+		pAd->CommonCfg.APEdcaParm.Aifsn[0] = 3;
+		pAd->CommonCfg.APEdcaParm.Aifsn[1] = 7;
+		pAd->CommonCfg.APEdcaParm.Aifsn[2] = 1;
+		pAd->CommonCfg.APEdcaParm.Aifsn[3] = 1;
+
+		pAd->CommonCfg.APEdcaParm.Cwmin[0] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[1] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[2] = 3;
+		pAd->CommonCfg.APEdcaParm.Cwmin[3] = 2;
+
+		pAd->CommonCfg.APEdcaParm.Cwmax[0] = 6;
+		pAd->CommonCfg.APEdcaParm.Cwmax[1] = 10;
+		pAd->CommonCfg.APEdcaParm.Cwmax[2] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmax[3] = 3;
+
+		pAd->CommonCfg.APEdcaParm.Txop[0]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[1]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[2]  = 94;
+		pAd->CommonCfg.APEdcaParm.Txop[3]  = 47;
+	}
+	AsicSetEdcaParm(pAd, &pAd->CommonCfg.APEdcaParm);
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		RTMPSetIndividualHT(pAd, 0);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Caller ensures we has 802.11n support.
+		Calls at setting HT from AP/STASetinformation
+
+	Arguments:
+		pAd - Pointer to our adapter
+		phymode  -
+
+	========================================================================
+*/
+VOID	RTMPSetIndividualHT(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	UCHAR				apidx)
+{
+	PRT_HT_PHY_INFO		pDesired_ht_phy = NULL;
+	UCHAR	TxStream = pAd->CommonCfg.TxStream;
+	UCHAR	DesiredMcs	= MCS_AUTO;
+
+	do
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			pDesired_ht_phy = &pAd->StaCfg.DesiredHtPhyInfo;
+			DesiredMcs = pAd->StaCfg.DesiredTransmitSetting.field.MCS;
+			//pAd->StaCfg.bAutoTxRateSwitch = (DesiredMcs == MCS_AUTO) ? TRUE : FALSE;
+				break;
+		}
+#endif // CONFIG_STA_SUPPORT //
+	} while (FALSE);
+
+	if (pDesired_ht_phy == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSetIndividualHT: invalid apidx(%d)\n", apidx));
+		return;
+	}
+	RTMPZeroMemory(pDesired_ht_phy, sizeof(RT_HT_PHY_INFO));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetIndividualHT : Desired MCS = %d\n", DesiredMcs));
+	// Check the validity of MCS
+	if ((TxStream == 1) && ((DesiredMcs >= MCS_8) && (DesiredMcs <= MCS_15)))
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("RTMPSetIndividualHT: MCS(%d) is invalid in 1S, reset it as MCS_7\n", DesiredMcs));
+		DesiredMcs = MCS_7;
+	}
+
+	if ((pAd->CommonCfg.DesiredHtPhy.ChannelWidth == BW_20) && (DesiredMcs == MCS_32))
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("RTMPSetIndividualHT: MCS_32 is only supported in 40-MHz, reset it as MCS_0\n"));
+		DesiredMcs = MCS_0;
+	}
+
+	pDesired_ht_phy->bHtEnable = TRUE;
+
+	// Decide desired Tx MCS
+	switch (TxStream)
+	{
+		case 1:
+			if (DesiredMcs == MCS_AUTO)
+			{
+				pDesired_ht_phy->MCSSet[0]= 0xff;
+				pDesired_ht_phy->MCSSet[1]= 0x00;
+			}
+			else if (DesiredMcs <= MCS_7)
+			{
+				pDesired_ht_phy->MCSSet[0]= 1<<DesiredMcs;
+				pDesired_ht_phy->MCSSet[1]= 0x00;
+			}
+			break;
+
+		case 2:
+			if (DesiredMcs == MCS_AUTO)
+			{
+				pDesired_ht_phy->MCSSet[0]= 0xff;
+				pDesired_ht_phy->MCSSet[1]= 0xff;
+			}
+			else if (DesiredMcs <= MCS_15)
+			{
+				ULONG mode;
+
+				mode = DesiredMcs / 8;
+				if (mode < 2)
+					pDesired_ht_phy->MCSSet[mode] = (1 << (DesiredMcs - mode * 8));
+			}
+			break;
+
+		case 3: // 3*3
+			if (DesiredMcs == MCS_AUTO)
+			{
+				/* MCS0 ~ MCS23, 3 bytes */
+				pDesired_ht_phy->MCSSet[0]= 0xff;
+				pDesired_ht_phy->MCSSet[1]= 0xff;
+				pDesired_ht_phy->MCSSet[2]= 0xff;
+			}
+			else if (DesiredMcs <= MCS_23)
+			{
+				ULONG mode;
+
+				mode = DesiredMcs / 8;
+				if (mode < 3)
+					pDesired_ht_phy->MCSSet[mode] = (1 << (DesiredMcs - mode * 8));
+			}
+			break;
+	}
+
+	if(pAd->CommonCfg.DesiredHtPhy.ChannelWidth == BW_40)
+	{
+		if (DesiredMcs == MCS_AUTO || DesiredMcs == MCS_32)
+			pDesired_ht_phy->MCSSet[4] = 0x1;
+	}
+
+	// update HT Rate setting
+    if (pAd->OpMode == OPMODE_STA)
+        MlmeUpdateHtTxRates(pAd, BSS0);
+    else
+	    MlmeUpdateHtTxRates(pAd, apidx);
+}
+
+
+/*
+	========================================================================
+	Routine Description:
+		Update HT IE from our capability.
+
+	Arguments:
+		Send all HT IE in beacon/probe rsp/assoc rsp/action frame.
+
+
+	========================================================================
+*/
+VOID	RTMPUpdateHTIE(
+	IN	RT_HT_CAPABILITY	*pRtHt,
+	IN		UCHAR				*pMcsSet,
+	OUT		HT_CAPABILITY_IE *pHtCapability,
+	OUT		ADD_HT_INFO_IE		*pAddHtInfo)
+{
+	RTMPZeroMemory(pHtCapability, sizeof(HT_CAPABILITY_IE));
+	RTMPZeroMemory(pAddHtInfo, sizeof(ADD_HT_INFO_IE));
+
+		pHtCapability->HtCapInfo.ChannelWidth = pRtHt->ChannelWidth;
+		pHtCapability->HtCapInfo.MimoPs = pRtHt->MimoPs;
+		pHtCapability->HtCapInfo.GF = pRtHt->GF;
+		pHtCapability->HtCapInfo.ShortGIfor20 = pRtHt->ShortGIfor20;
+		pHtCapability->HtCapInfo.ShortGIfor40 = pRtHt->ShortGIfor40;
+		pHtCapability->HtCapInfo.TxSTBC = pRtHt->TxSTBC;
+		pHtCapability->HtCapInfo.RxSTBC = pRtHt->RxSTBC;
+		pHtCapability->HtCapInfo.AMsduSize = pRtHt->AmsduSize;
+		pHtCapability->HtCapParm.MaxRAmpduFactor = pRtHt->MaxRAmpduFactor;
+		pHtCapability->HtCapParm.MpduDensity = pRtHt->MpduDensity;
+
+		pAddHtInfo->AddHtInfo.ExtChanOffset = pRtHt->ExtChanOffset ;
+		pAddHtInfo->AddHtInfo.RecomWidth = pRtHt->RecomWidth;
+		pAddHtInfo->AddHtInfo2.OperaionMode = pRtHt->OperaionMode;
+		pAddHtInfo->AddHtInfo2.NonGfPresent = pRtHt->NonGfPresent;
+		RTMPMoveMemory(pAddHtInfo->MCSSet, /*pRtHt->MCSSet*/pMcsSet, 4); // rt2860 only support MCS max=32, no need to copy all 16 uchar.
+
+        DBGPRINT(RT_DEBUG_TRACE,("RTMPUpdateHTIE <== \n"));
+}
+#endif // DOT11_N_SUPPORT //
+
+/*
+	========================================================================
+	Description:
+		Add Client security information into ASIC WCID table and IVEIV table.
+    Return:
+	========================================================================
+*/
+VOID	RTMPAddWcidAttributeEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BssIdx,
+	IN 	UCHAR		 	KeyIdx,
+	IN 	UCHAR		 	CipherAlg,
+	IN 	MAC_TABLE_ENTRY *pEntry)
+{
+	UINT32		WCIDAttri = 0;
+	USHORT		offset;
+	UCHAR		IVEIV = 0;
+	USHORT		Wcid = 0;
+
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (BssIdx > BSS0)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("RTMPAddWcidAttributeEntry: The BSS-index(%d) is out of range for Infra link. \n", BssIdx));
+				return;
+			}
+
+			// 1.	In ADHOC mode, the AID is wcid number. And NO mesh link exists.
+			// 2.	In Infra mode, the AID:1 MUST be wcid of infra STA.
+			//					   the AID:2~ assign to mesh link entry.
+			if (pEntry && ADHOC_ON(pAd))
+				Wcid = pEntry->Aid;
+			else if (pEntry && INFRA_ON(pAd))
+			{
+#ifdef QOS_DLS_SUPPORT
+				if (pEntry->ValidAsDls == TRUE)
+					Wcid = pEntry->Aid;
+				else
+#endif // QOS_DLS_SUPPORT //
+				Wcid = BSSID_WCID;
+			}
+			else
+				Wcid = MCAST_WCID;
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// Update WCID attribute table
+	offset = MAC_WCID_ATTRIBUTE_BASE + (Wcid * HW_WCID_ATTRI_SIZE);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pEntry && pEntry->ValidAsMesh)
+			WCIDAttri = (CipherAlg<<1) | PAIRWISEKEYTABLE;
+#ifdef QOS_DLS_SUPPORT
+		else if ((pEntry) && (pEntry->ValidAsDls) &&
+					((CipherAlg == CIPHER_TKIP) ||
+				 	(CipherAlg == CIPHER_TKIP_NO_MIC) ||
+					(CipherAlg == CIPHER_AES) ||
+				 	(CipherAlg == CIPHER_NONE)))
+			WCIDAttri = (CipherAlg<<1) | PAIRWISEKEYTABLE;
+#endif // QOS_DLS_SUPPORT //
+		else
+			WCIDAttri = (CipherAlg<<1) | SHAREDKEYTABLE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+
+
+	// Update IV/EIV table
+	offset = MAC_IVEIV_TABLE_BASE + (Wcid * HW_IVEIV_ENTRY_SIZE);
+
+	// WPA mode
+	if ((CipherAlg == CIPHER_TKIP) || (CipherAlg == CIPHER_TKIP_NO_MIC) || (CipherAlg == CIPHER_AES))
+	{
+		// Eiv bit on. keyid always is 0 for pairwise key
+		IVEIV = (KeyIdx <<6) | 0x20;
+	}
+	else
+	{
+		// WEP KeyIdx is default tx key.
+		IVEIV = (KeyIdx << 6);
+	}
+
+	// For key index and ext IV bit, so only need to update the position(offset+3).
+#ifdef RT2860
+	RTMP_IO_WRITE8(pAd, offset+3, IVEIV);
+#endif // RT2860 //
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPAddWcidAttributeEntry: WCID #%d, KeyIndex #%d, Alg=%s\n",Wcid, KeyIdx, CipherName[CipherAlg]));
+	DBGPRINT(RT_DEBUG_TRACE,("	WCIDAttri = 0x%x \n",  WCIDAttri));
+
+}
+
+/*
+    ==========================================================================
+    Description:
+        Parse encryption type
+Arguments:
+    pAdapter                    Pointer to our adapter
+    wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+    ==========================================================================
+*/
+CHAR *GetEncryptType(CHAR enc)
+{
+    if(enc == Ndis802_11WEPDisabled)
+        return "NONE";
+    if(enc == Ndis802_11WEPEnabled)
+    	return "WEP";
+    if(enc == Ndis802_11Encryption2Enabled)
+    	return "TKIP";
+    if(enc == Ndis802_11Encryption3Enabled)
+    	return "AES";
+	if(enc == Ndis802_11Encryption4Enabled)
+    	return "TKIPAES";
+    else
+    	return "UNKNOW";
+}
+
+CHAR *GetAuthMode(CHAR auth)
+{
+    if(auth == Ndis802_11AuthModeOpen)
+    	return "OPEN";
+    if(auth == Ndis802_11AuthModeShared)
+    	return "SHARED";
+	if(auth == Ndis802_11AuthModeAutoSwitch)
+    	return "AUTOWEP";
+    if(auth == Ndis802_11AuthModeWPA)
+    	return "WPA";
+    if(auth == Ndis802_11AuthModeWPAPSK)
+    	return "WPAPSK";
+    if(auth == Ndis802_11AuthModeWPANone)
+    	return "WPANONE";
+    if(auth == Ndis802_11AuthModeWPA2)
+    	return "WPA2";
+    if(auth == Ndis802_11AuthModeWPA2PSK)
+    	return "WPA2PSK";
+	if(auth == Ndis802_11AuthModeWPA1WPA2)
+    	return "WPA1WPA2";
+	if(auth == Ndis802_11AuthModeWPA1PSKWPA2PSK)
+    	return "WPA1PSKWPA2PSK";
+
+    	return "UNKNOW";
+}
+
+#if 1 //#ifndef UCOS
+/*
+    ==========================================================================
+    Description:
+        Get site survey results
+	Arguments:
+	    pAdapter                    Pointer to our adapter
+	    wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+        		1.) UI needs to wait 4 seconds after issue a site survey command
+        		2.) iwpriv ra0 get_site_survey
+        		3.) UI needs to prepare at least 4096bytes to get the results
+    ==========================================================================
+*/
+#define	LINE_LEN	(4+33+20+8+10+9+7+3)	// Channel+SSID+Bssid+WepStatus+AuthMode+Signal+WiressMode+NetworkType
+VOID RTMPIoctlGetSiteSurvey(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR		*msg;
+	INT 		i=0;
+	INT			WaitCnt;
+	INT 		Status=0;
+	CHAR		Ssid[MAX_LEN_OF_SSID +1];
+    INT         Rssi = 0, max_len = LINE_LEN;
+	UINT        Rssi_Quality = 0;
+	NDIS_802_11_NETWORK_TYPE    wireless_mode;
+
+	os_alloc_mem(NULL, (PUCHAR *)&msg, sizeof(CHAR)*((MAX_LEN_OF_BSS_TABLE)*max_len));
+
+	if (msg == NULL)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPIoctlGetSiteSurvey - msg memory alloc fail.\n"));
+		return;
+	}
+
+	memset(msg, 0 ,(MAX_LEN_OF_BSS_TABLE)*max_len );
+	memset(Ssid, 0 ,(MAX_LEN_OF_SSID +1));
+	sprintf(msg,"%s","\n");
+	sprintf(msg+strlen(msg),"%-4s%-33s%-20s%-8s%-10s%-9s%-7s%-3s\n",
+	    "Ch", "SSID", "BSSID", "Enc", "Auth", "Siganl(%)", "W-Mode", " NT");
+
+	WaitCnt = 0;
+#ifdef CONFIG_STA_SUPPORT
+	pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+	while ((ScanRunning(pAdapter) == TRUE) && (WaitCnt++ < 200))
+		OS_WAIT(500);
+#endif // CONFIG_STA_SUPPORT //
+
+	for(i=0; i<pAdapter->ScanTab.BssNr ;i++)
+	{
+		if( pAdapter->ScanTab.BssEntry[i].Channel==0)
+			break;
+
+		if((strlen(msg)+max_len ) >= IW_SCAN_MAX_DATA)
+			break;
+
+		//Channel
+		sprintf(msg+strlen(msg),"%-4d", pAdapter->ScanTab.BssEntry[i].Channel);
+		//SSID
+		memcpy(Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+		Ssid[pAdapter->ScanTab.BssEntry[i].SsidLen] = '\0';
+		sprintf(msg+strlen(msg),"%-33s", Ssid);
+		//BSSID
+		sprintf(msg+strlen(msg),"%02x:%02x:%02x:%02x:%02x:%02x   ",
+			pAdapter->ScanTab.BssEntry[i].Bssid[0],
+			pAdapter->ScanTab.BssEntry[i].Bssid[1],
+			pAdapter->ScanTab.BssEntry[i].Bssid[2],
+			pAdapter->ScanTab.BssEntry[i].Bssid[3],
+			pAdapter->ScanTab.BssEntry[i].Bssid[4],
+			pAdapter->ScanTab.BssEntry[i].Bssid[5]);
+		//Encryption Type
+		sprintf(msg+strlen(msg),"%-8s",GetEncryptType(pAdapter->ScanTab.BssEntry[i].WepStatus));
+		//Authentication Mode
+		if (pAdapter->ScanTab.BssEntry[i].WepStatus == Ndis802_11WEPEnabled)
+			sprintf(msg+strlen(msg),"%-10s", "UNKNOW");
+		else
+			sprintf(msg+strlen(msg),"%-10s",GetAuthMode(pAdapter->ScanTab.BssEntry[i].AuthMode));
+		// Rssi
+		Rssi = (INT)pAdapter->ScanTab.BssEntry[i].Rssi;
+		if (Rssi >= -50)
+			Rssi_Quality = 100;
+		else if (Rssi >= -80)    // between -50 ~ -80dbm
+			Rssi_Quality = (UINT)(24 + ((Rssi + 80) * 26)/10);
+		else if (Rssi >= -90)   // between -80 ~ -90dbm
+			Rssi_Quality = (UINT)(((Rssi + 90) * 26)/10);
+		else    // < -84 dbm
+			Rssi_Quality = 0;
+		sprintf(msg+strlen(msg),"%-9d", Rssi_Quality);
+		// Wireless Mode
+		wireless_mode = NetworkTypeInUseSanity(&pAdapter->ScanTab.BssEntry[i]);
+		if (wireless_mode == Ndis802_11FH ||
+			wireless_mode == Ndis802_11DS)
+			sprintf(msg+strlen(msg),"%-7s", "11b");
+		else if (wireless_mode == Ndis802_11OFDM5)
+			sprintf(msg+strlen(msg),"%-7s", "11a");
+		else if (wireless_mode == Ndis802_11OFDM5_N)
+			sprintf(msg+strlen(msg),"%-7s", "11a/n");
+		else if (wireless_mode == Ndis802_11OFDM24)
+			sprintf(msg+strlen(msg),"%-7s", "11b/g");
+		else if (wireless_mode == Ndis802_11OFDM24_N)
+			sprintf(msg+strlen(msg),"%-7s", "11b/g/n");
+		else
+			sprintf(msg+strlen(msg),"%-7s", "unknow");
+		//Network Type
+		if (pAdapter->ScanTab.BssEntry[i].BssType == BSS_ADHOC)
+			sprintf(msg+strlen(msg),"%-3s", " Ad");
+		else
+			sprintf(msg+strlen(msg),"%-3s", " In");
+
+        sprintf(msg+strlen(msg),"\n");
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+#endif // CONFIG_STA_SUPPORT //
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPIoctlGetSiteSurvey - wrq->u.data.length = %d\n", wrq->u.data.length));
+	os_free_mem(NULL, (PUCHAR)msg);
+}
+
+
+#define	MAC_LINE_LEN	(14+4+4+10+10+10+6+6)	// Addr+aid+psm+datatime+rxbyte+txbyte+current tx rate+last tx rate
+VOID RTMPIoctlGetMacTable(
+	IN PRTMP_ADAPTER pAd,
+	IN struct iwreq *wrq)
+{
+	INT i;
+	RT_802_11_MAC_TABLE MacTab;
+	char *msg;
+
+	MacTab.Num = 0;
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		if (pAd->MacTab.Content[i].ValidAsCLI && (pAd->MacTab.Content[i].Sst == SST_ASSOC))
+		{
+			COPY_MAC_ADDR(MacTab.Entry[MacTab.Num].Addr, &pAd->MacTab.Content[i].Addr);
+			MacTab.Entry[MacTab.Num].Aid = (UCHAR)pAd->MacTab.Content[i].Aid;
+			MacTab.Entry[MacTab.Num].Psm = pAd->MacTab.Content[i].PsMode;
+#ifdef DOT11_N_SUPPORT
+			MacTab.Entry[MacTab.Num].MimoPs = pAd->MacTab.Content[i].MmpsMode;
+#endif // DOT11_N_SUPPORT //
+
+			// Fill in RSSI per entry
+			MacTab.Entry[MacTab.Num].AvgRssi0 = pAd->MacTab.Content[i].RssiSample.AvgRssi0;
+			MacTab.Entry[MacTab.Num].AvgRssi1 = pAd->MacTab.Content[i].RssiSample.AvgRssi1;
+			MacTab.Entry[MacTab.Num].AvgRssi2 = pAd->MacTab.Content[i].RssiSample.AvgRssi2;
+
+			// the connected time per entry
+			MacTab.Entry[MacTab.Num].ConnectedTime = pAd->MacTab.Content[i].StaConnectTime;
+			MacTab.Entry[MacTab.Num].TxRate.field.MCS = pAd->MacTab.Content[i].HTPhyMode.field.MCS;
+			MacTab.Entry[MacTab.Num].TxRate.field.BW = pAd->MacTab.Content[i].HTPhyMode.field.BW;
+			MacTab.Entry[MacTab.Num].TxRate.field.ShortGI = pAd->MacTab.Content[i].HTPhyMode.field.ShortGI;
+			MacTab.Entry[MacTab.Num].TxRate.field.STBC = pAd->MacTab.Content[i].HTPhyMode.field.STBC;
+			MacTab.Entry[MacTab.Num].TxRate.field.rsv = pAd->MacTab.Content[i].HTPhyMode.field.rsv;
+			MacTab.Entry[MacTab.Num].TxRate.field.MODE = pAd->MacTab.Content[i].HTPhyMode.field.MODE;
+			MacTab.Entry[MacTab.Num].TxRate.word = pAd->MacTab.Content[i].HTPhyMode.word;
+
+			MacTab.Num += 1;
+		}
+	}
+	wrq->u.data.length = sizeof(RT_802_11_MAC_TABLE);
+	if (copy_to_user(wrq->u.data.pointer, &MacTab, wrq->u.data.length))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s: copy_to_user() fail\n", __func__));
+	}
+
+	msg = (CHAR *) kmalloc(sizeof(CHAR)*(MAX_LEN_OF_MAC_TABLE*MAC_LINE_LEN), MEM_ALLOC_FLAG);
+	memset(msg, 0 ,MAX_LEN_OF_MAC_TABLE*MAC_LINE_LEN );
+	sprintf(msg,"%s","\n");
+	sprintf(msg+strlen(msg),"%-14s%-4s%-4s%-10s%-10s%-10s%-6s%-6s\n",
+		"MAC", "AID", "PSM", "LDT", "RxB", "TxB","CTxR", "LTxR");
+
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+		if (pEntry->ValidAsCLI && (pEntry->Sst == SST_ASSOC))
+		{
+			if((strlen(msg)+MAC_LINE_LEN ) >= (MAX_LEN_OF_MAC_TABLE*MAC_LINE_LEN) )
+				break;
+			sprintf(msg+strlen(msg),"%02x%02x%02x%02x%02x%02x  ",
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+			sprintf(msg+strlen(msg),"%-4d", (int)pEntry->Aid);
+			sprintf(msg+strlen(msg),"%-4d", (int)pEntry->PsMode);
+			sprintf(msg+strlen(msg),"%-10d",0/*pAd->MacTab.Content[i].HSCounter.LastDataPacketTime*/); // ToDo
+			sprintf(msg+strlen(msg),"%-10d",0/*pAd->MacTab.Content[i].HSCounter.TotalRxByteCount*/); // ToDo
+			sprintf(msg+strlen(msg),"%-10d",0/*pAd->MacTab.Content[i].HSCounter.TotalTxByteCount*/); // ToDo
+			sprintf(msg+strlen(msg),"%-6d",RateIdToMbps[pAd->MacTab.Content[i].CurrTxRate]);
+			sprintf(msg+strlen(msg),"%-6d\n",0/*RateIdToMbps[pAd->MacTab.Content[i].LastTxRate]*/); // ToDo
+		}
+	}
+	// for compatible with old API just do the printk to console
+	//wrq->u.data.length = strlen(msg);
+	//if (copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s", msg));
+	}
+
+	kfree(msg);
+}
+#endif // UCOS //
+
+#ifdef DOT11_N_SUPPORT
+INT	Set_BASetup_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], tid;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+/*
+	The BASetup inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the tid value.
+*/
+	//printk("\n%s\n", arg);
+
+	if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and tid value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		tid = simple_strtol((token+1), 0, 10);
+		if (tid > 15)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+		printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x\n", mac[0], mac[1],
+				mac[2], mac[3], mac[4], mac[5], tid);
+
+	    pEntry = MacTableLookup(pAd, mac);
+
+    	if (pEntry) {
+        	printk("\nSetup BA Session: Tid = %d\n", tid);
+	        BAOriSessionSetUp(pAd, pEntry, tid, 0, 100, TRUE);
+    	}
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_BADecline_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG bBADecline;
+
+	bBADecline = simple_strtol(arg, 0, 10);
+
+	if (bBADecline == 0)
+	{
+		pAd->CommonCfg.bBADecline = FALSE;
+	}
+	else if (bBADecline == 1)
+	{
+		pAd->CommonCfg.bBADecline = TRUE;
+	}
+	else
+	{
+		return FALSE; //Invalid argument
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_BADecline_Proc::(BADecline=%d)\n", pAd->CommonCfg.bBADecline));
+
+	return TRUE;
+}
+
+INT	Set_BAOriTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], tid;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+/*
+	The BAOriTearDown inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the tid value.
+*/
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and tid value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		tid = simple_strtol((token+1), 0, 10);
+		if (tid > NUM_OF_TID)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+	    printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x", mac[0], mac[1],
+	           mac[2], mac[3], mac[4], mac[5], tid);
+
+	    pEntry = MacTableLookup(pAd, mac);
+
+	    if (pEntry) {
+	        printk("\nTear down Ori BA Session: Tid = %d\n", tid);
+        BAOriSessionTearDown(pAd, pEntry->Aid, tid, FALSE, TRUE);
+	    }
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_BARecTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], tid;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+    //printk("\n%s\n", arg);
+/*
+	The BARecTearDown inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the tid value.
+*/
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and tid value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		tid = simple_strtol((token+1), 0, 10);
+		if (tid > NUM_OF_TID)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+		printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x", mac[0], mac[1],
+		       mac[2], mac[3], mac[4], mac[5], tid);
+
+		pEntry = MacTableLookup(pAd, mac);
+
+		if (pEntry) {
+		    printk("\nTear down Rec BA Session: Tid = %d\n", tid);
+		    BARecSessionTearDown(pAd, pEntry->Aid, tid, FALSE);
+		}
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG HtBw;
+
+	HtBw = simple_strtol(arg, 0, 10);
+	if (HtBw == BW_40)
+		pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_40;
+	else if (HtBw == BW_20)
+		pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+	else
+		return FALSE;  //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtBw_Proc::(HtBw=%d)\n", pAd->CommonCfg.RegTransmitSetting.field.BW));
+
+	return TRUE;
+}
+
+INT	Set_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG HtMcs, Mcs_tmp;
+#ifdef CONFIG_STA_SUPPORT
+    BOOLEAN bAutoRate = FALSE;
+#endif // CONFIG_STA_SUPPORT //
+
+	Mcs_tmp = simple_strtol(arg, 0, 10);
+
+	if (Mcs_tmp <= 15 || Mcs_tmp == 32)
+		HtMcs = Mcs_tmp;
+	else
+		HtMcs = MCS_AUTO;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = HtMcs;
+		pAd->StaCfg.bAutoTxRateSwitch = (HtMcs == MCS_AUTO) ? TRUE:FALSE;
+		DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMcs_Proc::(HtMcs=%d, bAutoTxRateSwitch = %d)\n",
+						pAd->StaCfg.DesiredTransmitSetting.field.MCS, pAd->StaCfg.bAutoTxRateSwitch));
+
+		if ((pAd->CommonCfg.PhyMode < PHY_11ABGN_MIXED) ||
+			(pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE < MODE_HTMIX))
+		{
+	        if ((pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO) &&
+				(HtMcs >= 0 && HtMcs <= 3) &&
+	            (pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode == FIXED_TXMODE_CCK))
+			{
+				RTMPSetDesiredRates(pAd, (LONG) (RateIdToMbps[HtMcs] * 1000000));
+			}
+	        else if ((pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO) &&
+					(HtMcs >= 0 && HtMcs <= 7) &&
+	            	(pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode == FIXED_TXMODE_OFDM))
+			{
+				RTMPSetDesiredRates(pAd, (LONG) (RateIdToMbps[HtMcs+4] * 1000000));
+			}
+			else
+				bAutoRate = TRUE;
+
+			if (bAutoRate)
+			{
+	            pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+				RTMPSetDesiredRates(pAd, -1);
+			}
+	        DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMcs_Proc::(FixedTxMode=%d)\n",pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode));
+		}
+        if (ADHOC_ON(pAd))
+            return TRUE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	SetCommonHT(pAd);
+
+	return TRUE;
+}
+
+INT	Set_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG HtGi;
+
+	HtGi = simple_strtol(arg, 0, 10);
+
+	if ( HtGi == GI_400)
+		pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_400;
+	else if ( HtGi == GI_800 )
+		pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_800;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtGi_Proc::(ShortGI=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.ShortGI));
+
+	return TRUE;
+}
+
+
+INT	Set_HtTxBASize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR Size;
+
+	Size = simple_strtol(arg, 0, 10);
+
+	if (Size <=0 || Size >=64)
+	{
+		Size = 8;
+	}
+	pAd->CommonCfg.TxBASize = Size-1;
+	DBGPRINT(RT_DEBUG_ERROR, ("Set_HtTxBASize ::(TxBASize= %d)\n", Size));
+
+	return TRUE;
+}
+
+
+INT	Set_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == HTMODE_GF)
+		pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_GF;
+	else if ( Value == HTMODE_MM )
+		pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_MM;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtOpMode_Proc::(HtOpMode=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.HTMODE));
+
+	return TRUE;
+
+}
+
+INT	Set_HtStbc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == STBC_USE)
+		pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_USE;
+	else if ( Value == STBC_NONE )
+		pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_NONE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_Stbc_Proc::(HtStbc=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.STBC));
+
+	return TRUE;
+}
+
+INT	Set_HtHtc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->HTCEnable = FALSE;
+	else if ( Value ==1 )
+        	pAd->HTCEnable = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtHtc_Proc::(HtHtc=%d)\n",pAd->HTCEnable));
+
+	return TRUE;
+}
+
+INT	Set_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == 0)
+		pAd->CommonCfg.RegTransmitSetting.field.EXTCHA  = EXTCHA_BELOW;
+	else if ( Value ==1 )
+        pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtExtcha_Proc::(HtExtcha=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.EXTCHA));
+
+	return TRUE;
+}
+
+INT	Set_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value <=7 && Value >= 0)
+		pAd->CommonCfg.BACapability.field.MpduDensity = Value;
+	else
+		pAd->CommonCfg.BACapability.field.MpduDensity = 4;
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMpduDensity_Proc::(HtMpduDensity=%d)\n",pAd->CommonCfg.BACapability.field.MpduDensity));
+
+	return TRUE;
+}
+
+INT	Set_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+
+	if (Value >=1 && Value <= 64)
+	{
+		pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = Value;
+		pAd->CommonCfg.BACapability.field.RxBAWinLimit = Value;
+	}
+	else
+	{
+        pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = 64;
+		pAd->CommonCfg.BACapability.field.RxBAWinLimit = 64;
+	}
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtBaWinSize_Proc::(HtBaWinSize=%d)\n",pAd->CommonCfg.BACapability.field.RxBAWinLimit));
+
+	return TRUE;
+}
+
+INT	Set_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == 0)
+		pAd->CommonCfg.bRdg = FALSE;
+	else if ( Value ==1 )
+	{
+		pAd->HTCEnable = TRUE;
+        	pAd->CommonCfg.bRdg = TRUE;
+	}
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtRdg_Proc::(HtRdg=%d)\n",pAd->CommonCfg.bRdg));
+
+	return TRUE;
+}
+
+INT	Set_HtLinkAdapt_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->bLinkAdapt = FALSE;
+	else if ( Value ==1 )
+	{
+			pAd->HTCEnable = TRUE;
+			pAd->bLinkAdapt = TRUE;
+	}
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtLinkAdapt_Proc::(HtLinkAdapt=%d)\n",pAd->bLinkAdapt));
+
+	return TRUE;
+}
+
+INT	Set_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.BACapability.field.AmsduEnable = FALSE;
+	else if ( Value == 1 )
+        pAd->CommonCfg.BACapability.field.AmsduEnable = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtAmsdu_Proc::(HtAmsdu=%d)\n",pAd->CommonCfg.BACapability.field.AmsduEnable));
+
+	return TRUE;
+}
+
+INT	Set_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+    else if (Value == 1)
+		pAd->CommonCfg.BACapability.field.AutoBA = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+    pAd->CommonCfg.REGBACapability.field.AutoBA = pAd->CommonCfg.BACapability.field.AutoBA;
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtAutoBa_Proc::(HtAutoBa=%d)\n",pAd->CommonCfg.BACapability.field.AutoBA));
+
+	return TRUE;
+
+}
+
+INT	Set_HtProtect_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.bHTProtect = FALSE;
+    else if (Value == 1)
+		pAd->CommonCfg.bHTProtect = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtProtect_Proc::(HtProtect=%d)\n",pAd->CommonCfg.bHTProtect));
+
+	return TRUE;
+}
+
+INT	Set_SendPSMPAction_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], mode;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+    //printk("\n%s\n", arg);
+/*
+	The BARecTearDown inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the mode value.
+*/
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and mode value in decimal format.
+		return FALSE;
+
+   	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		mode = simple_strtol((token+1), 0, 10);
+		if (mode > MMPS_ENABLE)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+		printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x", mac[0], mac[1],
+		       mac[2], mac[3], mac[4], mac[5], mode);
+
+		pEntry = MacTableLookup(pAd, mac);
+
+		if (pEntry) {
+		    printk("\nSendPSMPAction MIPS mode = %d\n", mode);
+		    SendPSMPAction(pAd, pEntry->Aid, mode);
+		}
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+
+}
+
+INT	Set_HtMIMOPSmode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value <=3 && Value >= 0)
+		pAd->CommonCfg.BACapability.field.MMPSmode = Value;
+	else
+		pAd->CommonCfg.BACapability.field.MMPSmode = 3;
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMIMOPSmode_Proc::(MIMOPS mode=%d)\n",pAd->CommonCfg.BACapability.field.MMPSmode));
+
+	return TRUE;
+}
+
+
+INT	Set_ForceShortGI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->WIFItestbed.bShortGI = FALSE;
+	else if (Value == 1)
+		pAd->WIFItestbed.bShortGI = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_ForceShortGI_Proc::(ForceShortGI=%d)\n", pAd->WIFItestbed.bShortGI));
+
+	return TRUE;
+}
+
+
+
+INT	Set_ForceGF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->WIFItestbed.bGreenField = FALSE;
+	else if (Value == 1)
+		pAd->WIFItestbed.bGreenField = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_ForceGF_Proc::(ForceGF=%d)\n", pAd->WIFItestbed.bGreenField));
+
+	return TRUE;
+}
+
+INT	Set_HtMimoPs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.bMIMOPSEnable = FALSE;
+	else if (Value == 1)
+		pAd->CommonCfg.bMIMOPSEnable = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMimoPs_Proc::(HtMimoPs=%d)\n",pAd->CommonCfg.bMIMOPSEnable));
+
+	return TRUE;
+}
+#endif // DOT11_N_SUPPORT //
+
+
+#ifdef DOT11_N_SUPPORT
+INT	SetCommonHT(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	OID_SET_HT_PHYMODE		SetHT;
+
+	if (pAd->CommonCfg.PhyMode < PHY_11ABGN_MIXED)
+		return FALSE;
+
+	SetHT.PhyMode = pAd->CommonCfg.PhyMode;
+	SetHT.TransmitNo = ((UCHAR)pAd->Antenna.field.TxPath);
+	SetHT.HtMode = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.HTMODE;
+	SetHT.ExtOffset = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.EXTCHA;
+	SetHT.MCS = MCS_AUTO;
+	SetHT.BW = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.BW;
+	SetHT.STBC = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.STBC;
+	SetHT.SHORTGI = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.ShortGI;
+
+	RTMPSetHT(pAd, &SetHT);
+
+	return TRUE;
+}
+#endif // DOT11_N_SUPPORT //
+
+INT	Set_FixedTxMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR	fix_tx_mode = FIXED_TXMODE_HT;
+
+	if (strcmp(arg, "OFDM") == 0 || strcmp(arg, "ofdm") == 0)
+	{
+		fix_tx_mode = FIXED_TXMODE_OFDM;
+	}
+	else if (strcmp(arg, "CCK") == 0 || strcmp(arg, "cck") == 0)
+	{
+        fix_tx_mode = FIXED_TXMODE_CCK;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode = fix_tx_mode;
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_FixedTxMode_Proc::(FixedTxMode=%d)\n", fix_tx_mode));
+
+	return TRUE;
+}
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+INT	Set_OpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+#ifdef RT2860
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+#endif // RT2860 //
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Can not switch operate mode on interface up !! \n"));
+		return FALSE;
+	}
+
+	if (Value == 0)
+		pAd->OpMode = OPMODE_STA;
+	else if (Value == 1)
+		pAd->OpMode = OPMODE_AP;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_OpMode_Proc::(OpMode=%s)\n", pAd->OpMode == 1 ? "AP Mode" : "STA Mode"));
+
+	return TRUE;
+}
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+
+/////////////////////////////////////////////////////////////////////////
+PCHAR   RTMPGetRalinkAuthModeStr(
+    IN  NDIS_802_11_AUTHENTICATION_MODE authMode)
+{
+	switch(authMode)
+	{
+		case Ndis802_11AuthModeOpen:
+			return "OPEN";
+        default:
+		case Ndis802_11AuthModeWPAPSK:
+			return "WPAPSK";
+		case Ndis802_11AuthModeShared:
+			return "SHARED";
+		case Ndis802_11AuthModeWPA:
+			return "WPA";
+		case Ndis802_11AuthModeWPA2:
+			return "WPA2";
+		case Ndis802_11AuthModeWPA2PSK:
+			return "WPA2PSK";
+        case Ndis802_11AuthModeWPA1PSKWPA2PSK:
+			return "WPAPSKWPA2PSK";
+        case Ndis802_11AuthModeWPA1WPA2:
+			return "WPA1WPA2";
+		case Ndis802_11AuthModeWPANone:
+			return "WPANONE";
+	}
+}
+
+PCHAR   RTMPGetRalinkEncryModeStr(
+    IN  USHORT encryMode)
+{
+	switch(encryMode)
+	{
+	    default:
+		case Ndis802_11WEPDisabled:
+			return "NONE";
+		case Ndis802_11WEPEnabled:
+			return "WEP";
+		case Ndis802_11Encryption2Enabled:
+			return "TKIP";
+		case Ndis802_11Encryption3Enabled:
+			return "AES";
+        case Ndis802_11Encryption4Enabled:
+			return "TKIPAES";
+	}
+}
+
+INT RTMPShowCfgValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pName,
+	IN	PUCHAR			pBuf)
+{
+	INT	Status = 0;
+
+	for (PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC = RTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC++)
+	{
+		if (!strcmp(pName, PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name))
+		{
+			if(PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->show_proc(pAd, pBuf))
+				Status = -EINVAL;
+			break;  //Exit for loop.
+		}
+	}
+
+	if(PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name == NULL)
+	{
+		sprintf(pBuf, "\n");
+		for (PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC = RTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC++)
+			sprintf(pBuf, "%s%s\n", pBuf, PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name);
+	}
+
+	return Status;
+}
+
+INT	Show_SSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		sprintf(pBuf, "\t%s", pAd->CommonCfg.Ssid);
+#endif // CONFIG_STA_SUPPORT //
+	return 0;
+}
+
+INT	Show_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.PhyMode)
+	{
+		case PHY_11BG_MIXED:
+			sprintf(pBuf, "\t11B/G");
+			break;
+		case PHY_11B:
+			sprintf(pBuf, "\t11B");
+			break;
+		case PHY_11A:
+			sprintf(pBuf, "\t11A");
+			break;
+		case PHY_11ABG_MIXED:
+			sprintf(pBuf, "\t11A/B/G");
+			break;
+		case PHY_11G:
+			sprintf(pBuf, "\t11G");
+			break;
+#ifdef DOT11_N_SUPPORT
+		case PHY_11ABGN_MIXED:
+			sprintf(pBuf, "\t11A/B/G/N");
+			break;
+		case PHY_11N_2_4G:
+			sprintf(pBuf, "\t11N only with 2.4G");
+			break;
+		case PHY_11GN_MIXED:
+			sprintf(pBuf, "\t11G/N");
+			break;
+		case PHY_11AN_MIXED:
+			sprintf(pBuf, "\t11A/N");
+			break;
+		case PHY_11BGN_MIXED:
+			sprintf(pBuf, "\t11B/G/N");
+			break;
+		case PHY_11AGN_MIXED:
+			sprintf(pBuf, "\t11A/G/N");
+			break;
+		case PHY_11N_5G:
+			sprintf(pBuf, "\t11N only with 5G");
+			break;
+#endif // DOT11_N_SUPPORT //
+		default:
+			sprintf(pBuf, "\tUnknow Value(%d)", pAd->CommonCfg.PhyMode);
+			break;
+	}
+	return 0;
+}
+
+
+INT	Show_TxBurst_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bEnableTxBurst ? "TRUE":"FALSE");
+	return 0;
+}
+
+INT	Show_TxPreamble_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.TxPreamble)
+	{
+		case Rt802_11PreambleShort:
+			sprintf(pBuf, "\tShort");
+			break;
+		case Rt802_11PreambleLong:
+			sprintf(pBuf, "\tLong");
+			break;
+		case Rt802_11PreambleAuto:
+			sprintf(pBuf, "\tAuto");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%lu)", pAd->CommonCfg.TxPreamble);
+			break;
+	}
+
+	return 0;
+}
+
+INT	Show_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%lu", pAd->CommonCfg.TxPowerPercentage);
+	return 0;
+}
+
+INT	Show_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%d", pAd->CommonCfg.Channel);
+	return 0;
+}
+
+INT	Show_BGProtection_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.UseBGProtection)
+	{
+		case 1: //Always On
+			sprintf(pBuf, "\tON");
+			break;
+		case 2: //Always OFF
+			sprintf(pBuf, "\tOFF");
+			break;
+		case 0: //AUTO
+			sprintf(pBuf, "\tAuto");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%lu)", pAd->CommonCfg.UseBGProtection);
+			break;
+	}
+	return 0;
+}
+
+INT	Show_RTSThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.RtsThreshold);
+	return 0;
+}
+
+INT	Show_FragThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.FragmentThreshold);
+	return 0;
+}
+
+#ifdef DOT11_N_SUPPORT
+INT	Show_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	if (pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40)
+	{
+		sprintf(pBuf, "\t40 MHz");
+	}
+	else
+	{
+        sprintf(pBuf, "\t20 MHz");
+	}
+	return 0;
+}
+
+INT	Show_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		sprintf(pBuf, "\t%u", pAd->StaCfg.DesiredTransmitSetting.field.MCS);
+#endif // CONFIG_STA_SUPPORT //
+	return 0;
+}
+
+INT	Show_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.RegTransmitSetting.field.ShortGI)
+	{
+		case GI_400:
+			sprintf(pBuf, "\tGI_400");
+			break;
+		case GI_800:
+			sprintf(pBuf, "\tGI_800");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%u)", pAd->CommonCfg.RegTransmitSetting.field.ShortGI);
+			break;
+	}
+	return 0;
+}
+
+INT	Show_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.RegTransmitSetting.field.HTMODE)
+	{
+		case HTMODE_GF:
+			sprintf(pBuf, "\tGF");
+			break;
+		case HTMODE_MM:
+			sprintf(pBuf, "\tMM");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%u)", pAd->CommonCfg.RegTransmitSetting.field.HTMODE);
+			break;
+	}
+	return 0;
+}
+
+INT	Show_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.RegTransmitSetting.field.EXTCHA)
+	{
+		case EXTCHA_BELOW:
+			sprintf(pBuf, "\tBelow");
+			break;
+		case EXTCHA_ABOVE:
+			sprintf(pBuf, "\tAbove");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%u)", pAd->CommonCfg.RegTransmitSetting.field.EXTCHA);
+			break;
+	}
+	return 0;
+}
+
+
+INT	Show_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.BACapability.field.MpduDensity);
+	return 0;
+}
+
+INT	Show_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.BACapability.field.RxBAWinLimit);
+	return 0;
+}
+
+INT	Show_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bRdg ? "TRUE":"FALSE");
+	return 0;
+}
+
+INT	Show_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.BACapability.field.AmsduEnable ? "TRUE":"FALSE");
+	return 0;
+}
+
+INT	Show_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.BACapability.field.AutoBA ? "TRUE":"FALSE");
+	return 0;
+}
+#endif // DOT11_N_SUPPORT //
+
+INT	Show_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%d", pAd->CommonCfg.CountryRegion);
+	return 0;
+}
+
+INT	Show_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%d", pAd->CommonCfg.CountryRegionForABand);
+	return 0;
+}
+
+INT	Show_CountryCode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.CountryCode);
+	return 0;
+}
+
+#ifdef AGGREGATION_SUPPORT
+INT	Show_PktAggregate_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bAggregationCapable ? "TRUE":"FALSE");
+	return 0;
+}
+#endif // AGGREGATION_SUPPORT //
+
+#ifdef WMM_SUPPORT
+INT	Show_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		sprintf(pBuf, "\t%s", pAd->CommonCfg.bWmmCapable ? "TRUE":"FALSE");
+#endif // CONFIG_STA_SUPPORT //
+
+	return 0;
+}
+#endif // WMM_SUPPORT //
+
+INT	Show_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bIEEE80211H ? "TRUE":"FALSE");
+	return 0;
+}
+
+#ifdef CONFIG_STA_SUPPORT
+INT	Show_NetworkType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->StaCfg.BssType)
+	{
+		case BSS_ADHOC:
+			sprintf(pBuf, "\tAdhoc");
+			break;
+		case BSS_INFRA:
+			sprintf(pBuf, "\tInfra");
+			break;
+		case BSS_ANY:
+			sprintf(pBuf, "\tAny");
+			break;
+		case BSS_MONITOR:
+			sprintf(pBuf, "\tMonitor");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%d)", pAd->StaCfg.BssType);
+			break;
+	}
+	return 0;
+}
+#endif // CONFIG_STA_SUPPORT //
+
+INT	Show_AuthMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	NDIS_802_11_AUTHENTICATION_MODE	AuthMode = Ndis802_11AuthModeOpen;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		AuthMode = pAd->StaCfg.AuthMode;
+#endif // CONFIG_STA_SUPPORT //
+
+	if ((AuthMode >= Ndis802_11AuthModeOpen) &&
+		(AuthMode <= Ndis802_11AuthModeWPA1PSKWPA2PSK))
+		sprintf(pBuf, "\t%s", RTMPGetRalinkAuthModeStr(AuthMode));
+	else
+		sprintf(pBuf, "\tUnknow Value(%d)", AuthMode);
+
+	return 0;
+}
+
+INT	Show_EncrypType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	NDIS_802_11_WEP_STATUS	WepStatus = Ndis802_11WEPDisabled;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		WepStatus = pAd->StaCfg.WepStatus;
+#endif // CONFIG_STA_SUPPORT //
+
+	if ((WepStatus >= Ndis802_11WEPEnabled) &&
+		(WepStatus <= Ndis802_11Encryption4KeyAbsent))
+		sprintf(pBuf, "\t%s", RTMPGetRalinkEncryModeStr(WepStatus));
+	else
+		sprintf(pBuf, "\tUnknow Value(%d)", WepStatus);
+
+	return 0;
+}
+
+INT	Show_DefaultKeyID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	UCHAR DefaultKeyId = 0;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		DefaultKeyId = pAd->StaCfg.DefaultKeyId;
+#endif // CONFIG_STA_SUPPORT //
+
+	sprintf(pBuf, "\t%d", DefaultKeyId);
+
+	return 0;
+}
+
+INT	Show_WepKey_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  INT				KeyIdx,
+	OUT	PUCHAR			pBuf)
+{
+	UCHAR   Key[16] = {0}, KeyLength = 0;
+	INT		index = BSS0;
+
+	KeyLength = pAd->SharedKey[index][KeyIdx].KeyLen;
+	NdisMoveMemory(Key, pAd->SharedKey[index][KeyIdx].Key, KeyLength);
+
+	//check key string is ASCII or not
+    if (RTMPCheckStrPrintAble(Key, KeyLength))
+        sprintf(pBuf, "\t%s", Key);
+    else
+    {
+        int idx;
+        sprintf(pBuf, "\t");
+        for (idx = 0; idx < KeyLength; idx++)
+            sprintf(pBuf+strlen(pBuf), "%02X", Key[idx]);
+    }
+	return 0;
+}
+
+INT	Show_Key1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 0, pBuf);
+	return 0;
+}
+
+INT	Show_Key2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 1, pBuf);
+	return 0;
+}
+
+INT	Show_Key3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 2, pBuf);
+	return 0;
+}
+
+INT	Show_Key4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 3, pBuf);
+	return 0;
+}
+
+INT	Show_WPAPSK_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	INT 	idx;
+	UCHAR	PMK[32] = {0};
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		NdisMoveMemory(PMK, pAd->StaCfg.PMK, 32);
+#endif // CONFIG_STA_SUPPORT //
+
+    sprintf(pBuf, "\tPMK = ");
+    for (idx = 0; idx < 32; idx++)
+        sprintf(pBuf+strlen(pBuf), "%02X", PMK[idx]);
+
+	return 0;
+}
+
diff --git a/drivers/staging/rt2860/common/cmm_sanity.c b/drivers/staging/rt2860/common/cmm_sanity.c
new file mode 100644
index 000000000000..b0f070d43135
--- /dev/null
+++ b/drivers/staging/rt2860/common/cmm_sanity.c
@@ -0,0 +1,1633 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sanity.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang  2004-09-01      add WMM support
+*/
+#include "../rt_config.h"
+
+
+extern UCHAR	CISCO_OUI[];
+
+extern UCHAR	WPA_OUI[];
+extern UCHAR	RSN_OUI[];
+extern UCHAR	WME_INFO_ELEM[];
+extern UCHAR	WME_PARM_ELEM[];
+extern UCHAR	Ccx2QosInfo[];
+extern UCHAR	RALINK_OUI[];
+extern UCHAR	BROADCOM_OUI[];
+extern UCHAR    WPS_OUI[];
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN MlmeAddBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2)
+{
+    PMLME_ADDBA_REQ_STRUCT   pInfo;
+
+    pInfo = (MLME_ADDBA_REQ_STRUCT *)Msg;
+
+    if ((MsgLen != sizeof(MLME_ADDBA_REQ_STRUCT)))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - message lenght not correct.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->Wcid >= MAX_LEN_OF_MAC_TABLE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - The peer Mac is not associated yet.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->pAddr[0]&0x01) == 0x01)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - broadcast address not support BA\n"));
+        return FALSE;
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN MlmeDelBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen)
+{
+	MLME_DELBA_REQ_STRUCT *pInfo;
+	pInfo = (MLME_DELBA_REQ_STRUCT *)Msg;
+
+    if ((MsgLen != sizeof(MLME_DELBA_REQ_STRUCT)))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - message lenght not correct.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->Wcid >= MAX_LEN_OF_MAC_TABLE))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - The peer Mac is not associated yet.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->TID & 0xf0))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - The peer TID is incorrect.\n"));
+        return FALSE;
+    }
+
+	if (NdisEqualMemory(pAd->MacTab.Content[pInfo->Wcid].Addr, pInfo->Addr, MAC_ADDR_LEN) == 0)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - the peer addr dosen't exist.\n"));
+        return FALSE;
+    }
+
+    return TRUE;
+}
+
+BOOLEAN PeerAddBAReqActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen,
+	OUT PUCHAR pAddr2)
+{
+	PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+	PFRAME_ADDBA_REQ pAddFrame;
+	pAddFrame = (PFRAME_ADDBA_REQ)(pMsg);
+	if (MsgLen < (sizeof(FRAME_ADDBA_REQ)))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Request frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+	// we support immediate BA.
+	*(USHORT *)(&pAddFrame->BaParm) = cpu2le16(*(USHORT *)(&pAddFrame->BaParm));
+	pAddFrame->TimeOutValue = cpu2le16(pAddFrame->TimeOutValue);
+	pAddFrame->BaStartSeq.word = cpu2le16(pAddFrame->BaStartSeq.word);
+
+	if (pAddFrame->BaParm.BAPolicy != IMMED_BA)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Request Ba Policy[%d] not support\n", pAddFrame->BaParm.BAPolicy));
+		DBGPRINT(RT_DEBUG_ERROR,("ADDBA Request. tid=%x, Bufsize=%x, AMSDUSupported=%x \n", pAddFrame->BaParm.TID, pAddFrame->BaParm.BufSize, pAddFrame->BaParm.AMSDUSupported));
+		return FALSE;
+	}
+
+	// we support immediate BA.
+	if (pAddFrame->BaParm.TID &0xfff0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Request incorrect TID = %d\n", pAddFrame->BaParm.TID));
+		return FALSE;
+	}
+	COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+	return TRUE;
+}
+
+BOOLEAN PeerAddBARspActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen)
+{
+	PFRAME_ADDBA_RSP pAddFrame;
+
+	pAddFrame = (PFRAME_ADDBA_RSP)(pMsg);
+	if (MsgLen < (sizeof(FRAME_ADDBA_RSP)))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBARspActionSanity: ADDBA Response frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+	// we support immediate BA.
+	*(USHORT *)(&pAddFrame->BaParm) = cpu2le16(*(USHORT *)(&pAddFrame->BaParm));
+	pAddFrame->StatusCode = cpu2le16(pAddFrame->StatusCode);
+	pAddFrame->TimeOutValue = cpu2le16(pAddFrame->TimeOutValue);
+
+	if (pAddFrame->BaParm.BAPolicy != IMMED_BA)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Response Ba Policy[%d] not support\n", pAddFrame->BaParm.BAPolicy));
+		return FALSE;
+	}
+
+	// we support immediate BA.
+	if (pAddFrame->BaParm.TID &0xfff0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBARspActionSanity: ADDBA Response incorrect TID = %d\n", pAddFrame->BaParm.TID));
+		return FALSE;
+	}
+	return TRUE;
+
+}
+
+BOOLEAN PeerDelBAActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR Wcid,
+    IN VOID *pMsg,
+    IN ULONG MsgLen )
+{
+	//PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+	PFRAME_DELBA_REQ  pDelFrame;
+	if (MsgLen != (sizeof(FRAME_DELBA_REQ)))
+		return FALSE;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	pDelFrame = (PFRAME_DELBA_REQ)(pMsg);
+
+	*(USHORT *)(&pDelFrame->DelbaParm) = cpu2le16(*(USHORT *)(&pDelFrame->DelbaParm));
+	pDelFrame->ReasonCode = cpu2le16(pDelFrame->ReasonCode);
+
+	if (pDelFrame->DelbaParm.TID &0xfff0)
+		return FALSE;
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerBeaconAndProbeRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    IN UCHAR  MsgChannel,
+    OUT PUCHAR pAddr2,
+    OUT PUCHAR pBssid,
+    OUT CHAR Ssid[],
+    OUT UCHAR *pSsidLen,
+    OUT UCHAR *pBssType,
+    OUT USHORT *pBeaconPeriod,
+    OUT UCHAR *pChannel,
+    OUT UCHAR *pNewChannel,
+    OUT LARGE_INTEGER *pTimestamp,
+    OUT CF_PARM *pCfParm,
+    OUT USHORT *pAtimWin,
+    OUT USHORT *pCapabilityInfo,
+    OUT UCHAR *pErp,
+    OUT UCHAR *pDtimCount,
+    OUT UCHAR *pDtimPeriod,
+    OUT UCHAR *pBcastFlag,
+    OUT UCHAR *pMessageToMe,
+    OUT UCHAR SupRate[],
+    OUT UCHAR *pSupRateLen,
+    OUT UCHAR ExtRate[],
+    OUT UCHAR *pExtRateLen,
+    OUT	UCHAR *pCkipFlag,
+    OUT	UCHAR *pAironetCellPowerLimit,
+    OUT PEDCA_PARM       pEdcaParm,
+    OUT PQBSS_LOAD_PARM  pQbssLoad,
+    OUT PQOS_CAPABILITY_PARM pQosCapability,
+    OUT ULONG *pRalinkIe,
+    OUT UCHAR		 *pHtCapabilityLen,
+#ifdef CONFIG_STA_SUPPORT
+    OUT UCHAR		 *pPreNHtCapabilityLen,
+#endif // CONFIG_STA_SUPPORT //
+    OUT HT_CAPABILITY_IE *pHtCapability,
+	OUT UCHAR		 *AddHtInfoLen,
+	OUT ADD_HT_INFO_IE *AddHtInfo,
+	OUT UCHAR *NewExtChannelOffset,		// Ht extension channel offset(above or below)
+    OUT USHORT *LengthVIE,
+    OUT	PNDIS_802_11_VARIABLE_IEs pVIE)
+{
+    CHAR				*Ptr;
+#ifdef CONFIG_STA_SUPPORT
+	CHAR 				TimLen;
+#endif // CONFIG_STA_SUPPORT //
+    PFRAME_802_11		pFrame;
+    PEID_STRUCT         pEid;
+    UCHAR				SubType;
+    UCHAR				Sanity;
+    //UCHAR				ECWMin, ECWMax;
+    //MAC_CSR9_STRUC		Csr9;
+    ULONG				Length = 0;
+
+	// For some 11a AP which didn't have DS_IE, we use two conditions to decide the channel
+	//	1. If the AP is 11n enabled, then check the control channel.
+	//	2. If the AP didn't have any info about channel, use the channel we received this frame as the channel. (May inaccuracy!!)
+	UCHAR			CtrlChannel = 0;
+
+    // Add for 3 necessary EID field check
+    Sanity = 0;
+
+    *pAtimWin = 0;
+    *pErp = 0;
+    *pDtimCount = 0;
+    *pDtimPeriod = 0;
+    *pBcastFlag = 0;
+    *pMessageToMe = 0;
+    *pExtRateLen = 0;
+    *pCkipFlag = 0;			        // Default of CkipFlag is 0
+    *pAironetCellPowerLimit = 0xFF;  // Default of AironetCellPowerLimit is 0xFF
+    *LengthVIE = 0;					// Set the length of VIE to init value 0
+    *pHtCapabilityLen = 0;					// Set the length of VIE to init value 0
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->OpMode == OPMODE_STA)
+		*pPreNHtCapabilityLen = 0;					// Set the length of VIE to init value 0
+#endif // CONFIG_STA_SUPPORT //
+    *AddHtInfoLen = 0;					// Set the length of VIE to init value 0
+    *pRalinkIe = 0;
+    *pNewChannel = 0;
+    *NewExtChannelOffset = 0xff;	//Default 0xff means no such IE
+    pCfParm->bValid = FALSE;        // default: no IE_CF found
+    pQbssLoad->bValid = FALSE;      // default: no IE_QBSS_LOAD found
+    pEdcaParm->bValid = FALSE;      // default: no IE_EDCA_PARAMETER found
+    pQosCapability->bValid = FALSE; // default: no IE_QOS_CAPABILITY found
+
+    pFrame = (PFRAME_802_11)Msg;
+
+    // get subtype from header
+    SubType = (UCHAR)pFrame->Hdr.FC.SubType;
+
+    // get Addr2 and BSSID from header
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    COPY_MAC_ADDR(pBssid, pFrame->Hdr.Addr3);
+
+    Ptr = pFrame->Octet;
+    Length += LENGTH_802_11;
+
+    // get timestamp from payload and advance the pointer
+    NdisMoveMemory(pTimestamp, Ptr, TIMESTAMP_LEN);
+
+	pTimestamp->u.LowPart = cpu2le32(pTimestamp->u.LowPart);
+	pTimestamp->u.HighPart = cpu2le32(pTimestamp->u.HighPart);
+
+    Ptr += TIMESTAMP_LEN;
+    Length += TIMESTAMP_LEN;
+
+    // get beacon interval from payload and advance the pointer
+    NdisMoveMemory(pBeaconPeriod, Ptr, 2);
+    Ptr += 2;
+    Length += 2;
+
+    // get capability info from payload and advance the pointer
+    NdisMoveMemory(pCapabilityInfo, Ptr, 2);
+    Ptr += 2;
+    Length += 2;
+
+    if (CAP_IS_ESS_ON(*pCapabilityInfo))
+        *pBssType = BSS_INFRA;
+    else
+        *pBssType = BSS_ADHOC;
+
+    pEid = (PEID_STRUCT) Ptr;
+
+    // get variable fields from payload and advance the pointer
+    while ((Length + 2 + pEid->Len) <= MsgLen)
+    {
+        //
+        // Secure copy VIE to VarIE[MAX_VIE_LEN] didn't overflow.
+        //
+        if ((*LengthVIE + pEid->Len + 2) >= MAX_VIE_LEN)
+        {
+            DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - Variable IEs out of resource [len(=%d) > MAX_VIE_LEN(=%d)]\n",
+                    (*LengthVIE + pEid->Len + 2), MAX_VIE_LEN));
+            break;
+        }
+
+        switch(pEid->Eid)
+        {
+            case IE_SSID:
+                // Already has one SSID EID in this beacon, ignore the second one
+                if (Sanity & 0x1)
+                    break;
+                if(pEid->Len <= MAX_LEN_OF_SSID)
+                {
+                    NdisMoveMemory(Ssid, pEid->Octet, pEid->Len);
+                    *pSsidLen = pEid->Len;
+                    Sanity |= 0x1;
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_SSID (len=%d)\n",pEid->Len));
+                    return FALSE;
+                }
+                break;
+
+            case IE_SUPP_RATES:
+                if(pEid->Len <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    Sanity |= 0x2;
+                    NdisMoveMemory(SupRate, pEid->Octet, pEid->Len);
+                    *pSupRateLen = pEid->Len;
+
+                    // TODO: 2004-09-14 not a good design here, cause it exclude extra rates
+                    // from ScanTab. We should report as is. And filter out unsupported
+                    // rates in MlmeAux.
+                    // Check against the supported rates
+                    // RTMPCheckRates(pAd, SupRate, pSupRateLen);
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_SUPP_RATES (len=%d)\n",pEid->Len));
+                    return FALSE;
+                }
+                break;
+
+            case IE_HT_CAP:
+			if (pEid->Len >= SIZE_HT_CAP_IE)  //Note: allow extension.!!
+			{
+				NdisMoveMemory(pHtCapability, pEid->Octet, sizeof(HT_CAPABILITY_IE));
+				*pHtCapabilityLen = SIZE_HT_CAP_IE;	// Nnow we only support 26 bytes.
+
+				*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+				*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					*pPreNHtCapabilityLen = 0;	// Nnow we only support 26 bytes.
+
+					Ptr = (PUCHAR) pVIE;
+					NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+					*LengthVIE += (pEid->Len + 2);
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - wrong IE_HT_CAP. pEid->Len = %d\n", pEid->Len));
+			}
+
+		break;
+            case IE_ADD_HT:
+			if (pEid->Len >= sizeof(ADD_HT_INFO_IE))
+			{
+				// This IE allows extension, but we can ignore extra bytes beyond our knowledge , so only
+				// copy first sizeof(ADD_HT_INFO_IE)
+				NdisMoveMemory(AddHtInfo, pEid->Octet, sizeof(ADD_HT_INFO_IE));
+				*AddHtInfoLen = SIZE_ADD_HT_INFO_IE;
+
+				CtrlChannel = AddHtInfo->ControlChan;
+
+				*(USHORT *)(&AddHtInfo->AddHtInfo2) = cpu2le16(*(USHORT *)(&AddHtInfo->AddHtInfo2));
+				*(USHORT *)(&AddHtInfo->AddHtInfo3) = cpu2le16(*(USHORT *)(&AddHtInfo->AddHtInfo3));
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+			                Ptr = (PUCHAR) pVIE;
+			                NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+			                *LengthVIE += (pEid->Len + 2);
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - wrong IE_ADD_HT. \n"));
+			}
+
+		break;
+            case IE_SECONDARY_CH_OFFSET:
+			if (pEid->Len == 1)
+			{
+				*NewExtChannelOffset = pEid->Octet[0];
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - wrong IE_SECONDARY_CH_OFFSET. \n"));
+			}
+
+		break;
+            case IE_FH_PARM:
+                DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity(IE_FH_PARM) \n"));
+                break;
+
+            case IE_DS_PARM:
+                if(pEid->Len == 1)
+                {
+                    *pChannel = *pEid->Octet;
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						if (ChannelSanity(pAd, *pChannel) == 0)
+						{
+
+							return FALSE;
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+                    Sanity |= 0x4;
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_DS_PARM (len=%d)\n",pEid->Len));
+                    return FALSE;
+                }
+                break;
+
+            case IE_CF_PARM:
+                if(pEid->Len == 6)
+                {
+                    pCfParm->bValid = TRUE;
+                    pCfParm->CfpCount = pEid->Octet[0];
+                    pCfParm->CfpPeriod = pEid->Octet[1];
+                    pCfParm->CfpMaxDuration = pEid->Octet[2] + 256 * pEid->Octet[3];
+                    pCfParm->CfpDurRemaining = pEid->Octet[4] + 256 * pEid->Octet[5];
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_CF_PARM\n"));
+                    return FALSE;
+                }
+                break;
+
+            case IE_IBSS_PARM:
+                if(pEid->Len == 2)
+                {
+                    NdisMoveMemory(pAtimWin, pEid->Octet, pEid->Len);
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_IBSS_PARM\n"));
+                    return FALSE;
+                }
+                break;
+
+#ifdef CONFIG_STA_SUPPORT
+            case IE_TIM:
+                if(INFRA_ON(pAd) && SubType == SUBTYPE_BEACON)
+                {
+                    GetTimBit((PUCHAR)pEid, pAd->StaActive.Aid, &TimLen, pBcastFlag, pDtimCount, pDtimPeriod, pMessageToMe);
+                }
+                break;
+#endif // CONFIG_STA_SUPPORT //
+            case IE_CHANNEL_SWITCH_ANNOUNCEMENT:
+                if(pEid->Len == 3)
+                {
+                	*pNewChannel = pEid->Octet[1];	//extract new channel number
+                }
+                break;
+
+            // New for WPA
+            // CCX v2 has the same IE, we need to parse that too
+            // Wifi WMM use the same IE vale, need to parse that too
+            // case IE_WPA:
+            case IE_VENDOR_SPECIFIC:
+                // Check the OUI version, filter out non-standard usage
+                if (NdisEqualMemory(pEid->Octet, RALINK_OUI, 3) && (pEid->Len == 7))
+                {
+                    //*pRalinkIe = pEid->Octet[3];
+                    if (pEid->Octet[3] != 0)
+        				*pRalinkIe = pEid->Octet[3];
+        			else
+        				*pRalinkIe = 0xf0000000; // Set to non-zero value (can't set bit0-2) to represent this is Ralink Chip. So at linkup, we will set ralinkchip flag.
+                }
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+		// This HT IE is before IEEE draft set HT IE value.2006-09-28 by Jan.
+
+                // Other vendors had production before IE_HT_CAP value is assigned. To backward support those old-firmware AP,
+                // Check broadcom-defiend pre-802.11nD1.0 OUI for HT related IE, including HT Capatilities IE and HT Information IE
+                else if ((*pHtCapabilityLen == 0) && NdisEqualMemory(pEid->Octet, PRE_N_HT_OUI, 3) && (pEid->Len >= 4) && (pAd->OpMode == OPMODE_STA))
+                {
+                    if ((pEid->Octet[3] == OUI_PREN_HT_CAP) && (pEid->Len >= 30) && (*pHtCapabilityLen == 0))
+                    {
+                        NdisMoveMemory(pHtCapability, &pEid->Octet[4], sizeof(HT_CAPABILITY_IE));
+                        *pPreNHtCapabilityLen = SIZE_HT_CAP_IE;
+                    }
+
+                    if ((pEid->Octet[3] == OUI_PREN_ADD_HT) && (pEid->Len >= 26))
+                    {
+                        NdisMoveMemory(AddHtInfo, &pEid->Octet[4], sizeof(ADD_HT_INFO_IE));
+                        *AddHtInfoLen = SIZE_ADD_HT_INFO_IE;
+                    }
+                }
+#endif // DOT11_N_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+                else if (NdisEqualMemory(pEid->Octet, WPA_OUI, 4))
+                {
+                    // Copy to pVIE which will report to microsoft bssid list.
+                    Ptr = (PUCHAR) pVIE;
+                    NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+                    *LengthVIE += (pEid->Len + 2);
+                }
+                else if (NdisEqualMemory(pEid->Octet, WME_PARM_ELEM, 6) && (pEid->Len == 24))
+                {
+                    PUCHAR ptr;
+                    int i;
+
+                    // parsing EDCA parameters
+                    pEdcaParm->bValid          = TRUE;
+                    pEdcaParm->bQAck           = FALSE; // pEid->Octet[0] & 0x10;
+                    pEdcaParm->bQueueRequest   = FALSE; // pEid->Octet[0] & 0x20;
+                    pEdcaParm->bTxopRequest    = FALSE; // pEid->Octet[0] & 0x40;
+                    pEdcaParm->EdcaUpdateCount = pEid->Octet[6] & 0x0f;
+                    pEdcaParm->bAPSDCapable    = (pEid->Octet[6] & 0x80) ? 1 : 0;
+                    ptr = &pEid->Octet[8];
+                    for (i=0; i<4; i++)
+                    {
+                        UCHAR aci = (*ptr & 0x60) >> 5; // b5~6 is AC INDEX
+                        pEdcaParm->bACM[aci]  = (((*ptr) & 0x10) == 0x10);   // b5 is ACM
+                        pEdcaParm->Aifsn[aci] = (*ptr) & 0x0f;               // b0~3 is AIFSN
+                        pEdcaParm->Cwmin[aci] = *(ptr+1) & 0x0f;             // b0~4 is Cwmin
+                        pEdcaParm->Cwmax[aci] = *(ptr+1) >> 4;               // b5~8 is Cwmax
+                        pEdcaParm->Txop[aci]  = *(ptr+2) + 256 * (*(ptr+3)); // in unit of 32-us
+                        ptr += 4; // point to next AC
+                    }
+                }
+                else if (NdisEqualMemory(pEid->Octet, WME_INFO_ELEM, 6) && (pEid->Len == 7))
+                {
+                    // parsing EDCA parameters
+                    pEdcaParm->bValid          = TRUE;
+                    pEdcaParm->bQAck           = FALSE; // pEid->Octet[0] & 0x10;
+                    pEdcaParm->bQueueRequest   = FALSE; // pEid->Octet[0] & 0x20;
+                    pEdcaParm->bTxopRequest    = FALSE; // pEid->Octet[0] & 0x40;
+                    pEdcaParm->EdcaUpdateCount = pEid->Octet[6] & 0x0f;
+                    pEdcaParm->bAPSDCapable    = (pEid->Octet[6] & 0x80) ? 1 : 0;
+
+                    // use default EDCA parameter
+                    pEdcaParm->bACM[QID_AC_BE]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_BE] = 3;
+                    pEdcaParm->Cwmin[QID_AC_BE] = CW_MIN_IN_BITS;
+                    pEdcaParm->Cwmax[QID_AC_BE] = CW_MAX_IN_BITS;
+                    pEdcaParm->Txop[QID_AC_BE]  = 0;
+
+                    pEdcaParm->bACM[QID_AC_BK]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_BK] = 7;
+                    pEdcaParm->Cwmin[QID_AC_BK] = CW_MIN_IN_BITS;
+                    pEdcaParm->Cwmax[QID_AC_BK] = CW_MAX_IN_BITS;
+                    pEdcaParm->Txop[QID_AC_BK]  = 0;
+
+                    pEdcaParm->bACM[QID_AC_VI]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_VI] = 2;
+                    pEdcaParm->Cwmin[QID_AC_VI] = CW_MIN_IN_BITS-1;
+                    pEdcaParm->Cwmax[QID_AC_VI] = CW_MAX_IN_BITS;
+                    pEdcaParm->Txop[QID_AC_VI]  = 96;   // AC_VI: 96*32us ~= 3ms
+
+                    pEdcaParm->bACM[QID_AC_VO]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_VO] = 2;
+                    pEdcaParm->Cwmin[QID_AC_VO] = CW_MIN_IN_BITS-2;
+                    pEdcaParm->Cwmax[QID_AC_VO] = CW_MAX_IN_BITS-1;
+                    pEdcaParm->Txop[QID_AC_VO]  = 48;   // AC_VO: 48*32us ~= 1.5ms
+                }
+                break;
+
+            case IE_EXT_SUPP_RATES:
+                if (pEid->Len <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(ExtRate, pEid->Octet, pEid->Len);
+                    *pExtRateLen = pEid->Len;
+
+                    // TODO: 2004-09-14 not a good design here, cause it exclude extra rates
+                    // from ScanTab. We should report as is. And filter out unsupported
+                    // rates in MlmeAux.
+                    // Check against the supported rates
+                    // RTMPCheckRates(pAd, ExtRate, pExtRateLen);
+                }
+                break;
+
+            case IE_ERP:
+                if (pEid->Len == 1)
+                {
+                    *pErp = (UCHAR)pEid->Octet[0];
+                }
+                break;
+
+            case IE_AIRONET_CKIP:
+                // 0. Check Aironet IE length, it must be larger or equal to 28
+                // Cisco AP350 used length as 28
+                // Cisco AP12XX used length as 30
+                if (pEid->Len < (CKIP_NEGOTIATION_LENGTH - 2))
+                    break;
+
+                // 1. Copy CKIP flag byte to buffer for process
+                *pCkipFlag = *(pEid->Octet + 8);
+                break;
+
+            case IE_AP_TX_POWER:
+                // AP Control of Client Transmit Power
+                //0. Check Aironet IE length, it must be 6
+                if (pEid->Len != 0x06)
+                    break;
+
+                // Get cell power limit in dBm
+                if (NdisEqualMemory(pEid->Octet, CISCO_OUI, 3) == 1)
+                    *pAironetCellPowerLimit = *(pEid->Octet + 4);
+                break;
+
+            // WPA2 & 802.11i RSN
+            case IE_RSN:
+                // There is no OUI for version anymore, check the group cipher OUI before copying
+                if (RTMPEqualMemory(pEid->Octet + 2, RSN_OUI, 3))
+                {
+                    // Copy to pVIE which will report to microsoft bssid list.
+                    Ptr = (PUCHAR) pVIE;
+                    NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+                    *LengthVIE += (pEid->Len + 2);
+                }
+                break;
+#ifdef CONFIG_STA_SUPPORT
+#ifdef EXT_BUILD_CHANNEL_LIST
+			case IE_COUNTRY:
+				Ptr = (PUCHAR) pVIE;
+                NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+                *LengthVIE += (pEid->Len + 2);
+				break;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+
+            default:
+                break;
+        }
+
+        Length = Length + 2 + pEid->Len;  // Eid[1] + Len[1]+ content[Len]
+        pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+    }
+
+    // For some 11a AP. it did not have the channel EID, patch here
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		UCHAR LatchRfChannel = MsgChannel;
+		if ((pAd->LatchRfRegs.Channel > 14) && ((Sanity & 0x4) == 0))
+		{
+			if (CtrlChannel != 0)
+				*pChannel = CtrlChannel;
+			else
+				*pChannel = LatchRfChannel;
+			Sanity |= 0x4;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	if (Sanity != 0x7)
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - missing field, Sanity=0x%02x\n", Sanity));
+		return FALSE;
+	}
+	else
+	{
+		return TRUE;
+	}
+
+}
+
+#ifdef DOT11N_DRAFT3
+/*
+	==========================================================================
+	Description:
+		MLME message sanity check for some IE addressed  in 802.11n d3.03.
+	Return:
+		TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+BOOLEAN PeerBeaconAndProbeRspSanity2(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *Msg,
+	IN ULONG MsgLen,
+	OUT UCHAR 	*RegClass)
+{
+	CHAR				*Ptr;
+	PFRAME_802_11		pFrame;
+	PEID_STRUCT			pEid;
+	ULONG				Length = 0;
+
+	pFrame = (PFRAME_802_11)Msg;
+
+	*RegClass = 0;
+	Ptr = pFrame->Octet;
+	Length += LENGTH_802_11;
+
+	// get timestamp from payload and advance the pointer
+	Ptr += TIMESTAMP_LEN;
+	Length += TIMESTAMP_LEN;
+
+	// get beacon interval from payload and advance the pointer
+	Ptr += 2;
+	Length += 2;
+
+	// get capability info from payload and advance the pointer
+	Ptr += 2;
+	Length += 2;
+
+	pEid = (PEID_STRUCT) Ptr;
+
+	// get variable fields from payload and advance the pointer
+	while ((Length + 2 + pEid->Len) <= MsgLen)
+	{
+		switch(pEid->Eid)
+		{
+			case IE_SUPP_REG_CLASS:
+				if(pEid->Len > 0)
+				{
+					*RegClass = *pEid->Octet;
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_SSID (len=%d)\n",pEid->Len));
+					return FALSE;
+				}
+				break;
+		}
+
+		Length = Length + 2 + pEid->Len;  // Eid[1] + Len[1]+ content[Len]
+		pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+	}
+
+	return TRUE;
+
+}
+#endif // DOT11N_DRAFT3 //
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN MlmeScanReqSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *Msg,
+	IN ULONG MsgLen,
+	OUT UCHAR *pBssType,
+	OUT CHAR Ssid[],
+	OUT UCHAR *pSsidLen,
+	OUT UCHAR *pScanType)
+{
+	MLME_SCAN_REQ_STRUCT *Info;
+
+	Info = (MLME_SCAN_REQ_STRUCT *)(Msg);
+	*pBssType = Info->BssType;
+	*pSsidLen = Info->SsidLen;
+	NdisMoveMemory(Ssid, Info->Ssid, *pSsidLen);
+	*pScanType = Info->ScanType;
+
+	if ((*pBssType == BSS_INFRA || *pBssType == BSS_ADHOC || *pBssType == BSS_ANY)
+		&& (*pScanType == SCAN_ACTIVE || *pScanType == SCAN_PASSIVE
+#ifdef CONFIG_STA_SUPPORT
+		|| *pScanType == SCAN_CISCO_PASSIVE || *pScanType == SCAN_CISCO_ACTIVE
+		|| *pScanType == SCAN_CISCO_CHANNEL_LOAD || *pScanType == SCAN_CISCO_NOISE
+#endif // CONFIG_STA_SUPPORT //
+		))
+	{
+		return TRUE;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeScanReqSanity fail - wrong BssType or ScanType\n"));
+		return FALSE;
+	}
+}
+
+// IRQL = DISPATCH_LEVEL
+UCHAR ChannelSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR channel)
+{
+    int i;
+
+    for (i = 0; i < pAd->ChannelListNum; i ++)
+    {
+        if (channel == pAd->ChannelList[i].Channel)
+            return 1;
+    }
+    return 0;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerDeauthSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pReason)
+{
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    NdisMoveMemory(pReason, &pFrame->Octet[0], 2);
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerAuthSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr,
+    OUT USHORT *pAlg,
+    OUT USHORT *pSeq,
+    OUT USHORT *pStatus,
+    CHAR *pChlgText)
+{
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr,   pFrame->Hdr.Addr2);
+    NdisMoveMemory(pAlg,    &pFrame->Octet[0], 2);
+    NdisMoveMemory(pSeq,    &pFrame->Octet[2], 2);
+    NdisMoveMemory(pStatus, &pFrame->Octet[4], 2);
+
+    if ((*pAlg == Ndis802_11AuthModeOpen)
+#ifdef LEAP_SUPPORT
+      || (*pAlg == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+      )
+    {
+        if (*pSeq == 1 || *pSeq == 2)
+        {
+            return TRUE;
+        }
+        else
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("PeerAuthSanity fail - wrong Seg#\n"));
+            return FALSE;
+        }
+    }
+    else if (*pAlg == Ndis802_11AuthModeShared)
+    {
+        if (*pSeq == 1 || *pSeq == 4)
+        {
+            return TRUE;
+        }
+        else if (*pSeq == 2 || *pSeq == 3)
+        {
+            NdisMoveMemory(pChlgText, &pFrame->Octet[8], CIPHER_TEXT_LEN);
+            return TRUE;
+        }
+        else
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("PeerAuthSanity fail - wrong Seg#\n"));
+            return FALSE;
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerAuthSanity fail - wrong algorithm\n"));
+        return FALSE;
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN MlmeAuthReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr,
+    OUT ULONG *pTimeout,
+    OUT USHORT *pAlg)
+{
+    MLME_AUTH_REQ_STRUCT *pInfo;
+
+    pInfo  = (MLME_AUTH_REQ_STRUCT *)Msg;
+    COPY_MAC_ADDR(pAddr, pInfo->Addr);
+    *pTimeout = pInfo->Timeout;
+    *pAlg = pInfo->Alg;
+
+    if (((*pAlg == Ndis802_11AuthModeShared) ||(*pAlg == Ndis802_11AuthModeOpen)
+#ifdef LEAP_SUPPORT
+     || (*pAlg == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+     	) &&
+        ((*pAddr & 0x01) == 0))
+    {
+        return TRUE;
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAuthReqSanity fail - wrong algorithm\n"));
+        return FALSE;
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN MlmeAssocReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pApAddr,
+    OUT USHORT *pCapabilityInfo,
+    OUT ULONG *pTimeout,
+    OUT USHORT *pListenIntv)
+{
+    MLME_ASSOC_REQ_STRUCT *pInfo;
+
+    pInfo = (MLME_ASSOC_REQ_STRUCT *)Msg;
+    *pTimeout = pInfo->Timeout;                             // timeout
+    COPY_MAC_ADDR(pApAddr, pInfo->Addr);                   // AP address
+    *pCapabilityInfo = pInfo->CapabilityInfo;               // capability info
+    *pListenIntv = pInfo->ListenIntv;
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerDisassocSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pReason)
+{
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    NdisMoveMemory(pReason, &pFrame->Octet[0], 2);
+
+    return TRUE;
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Sanity check NetworkType (11b, 11g or 11a)
+
+	Arguments:
+		pBss - Pointer to BSS table.
+
+	Return Value:
+        Ndis802_11DS .......(11b)
+        Ndis802_11OFDM24....(11g)
+        Ndis802_11OFDM5.....(11a)
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+NDIS_802_11_NETWORK_TYPE NetworkTypeInUseSanity(
+    IN PBSS_ENTRY pBss)
+{
+	NDIS_802_11_NETWORK_TYPE	NetWorkType;
+	UCHAR						rate, i;
+
+	NetWorkType = Ndis802_11DS;
+
+	if (pBss->Channel <= 14)
+	{
+		//
+		// First check support Rate.
+		//
+		for (i = 0; i < pBss->SupRateLen; i++)
+		{
+			rate = pBss->SupRate[i] & 0x7f; // Mask out basic rate set bit
+			if ((rate == 2) || (rate == 4) || (rate == 11) || (rate == 22))
+			{
+				continue;
+			}
+			else
+			{
+				//
+				// Otherwise (even rate > 108) means Ndis802_11OFDM24
+				//
+				NetWorkType = Ndis802_11OFDM24;
+				break;
+			}
+		}
+
+		//
+		// Second check Extend Rate.
+		//
+		if (NetWorkType != Ndis802_11OFDM24)
+		{
+			for (i = 0; i < pBss->ExtRateLen; i++)
+			{
+				rate = pBss->SupRate[i] & 0x7f; // Mask out basic rate set bit
+				if ((rate == 2) || (rate == 4) || (rate == 11) || (rate == 22))
+				{
+					continue;
+				}
+				else
+				{
+					//
+					// Otherwise (even rate > 108) means Ndis802_11OFDM24
+					//
+					NetWorkType = Ndis802_11OFDM24;
+					break;
+				}
+			}
+		}
+	}
+	else
+	{
+		NetWorkType = Ndis802_11OFDM5;
+	}
+
+    if (pBss->HtCapabilityLen != 0)
+    {
+        if (NetWorkType == Ndis802_11OFDM5)
+            NetWorkType = Ndis802_11OFDM5_N;
+        else
+            NetWorkType = Ndis802_11OFDM24_N;
+    }
+
+	return NetWorkType;
+}
+
+/*
+    ==========================================================================
+    Description:
+        WPA message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN PeerWpaMessageSanity(
+    IN 	PRTMP_ADAPTER 		pAd,
+    IN 	PEAPOL_PACKET 		pMsg,
+    IN 	ULONG 				MsgLen,
+    IN 	UCHAR				MsgType,
+    IN 	MAC_TABLE_ENTRY  	*pEntry)
+{
+	UCHAR			mic[LEN_KEY_DESC_MIC], digest[80], KEYDATA[MAX_LEN_OF_RSNIE];
+	BOOLEAN			bReplayDiff = FALSE;
+	BOOLEAN			bWPA2 = FALSE;
+	KEY_INFO		EapolKeyInfo;
+	UCHAR			GroupKeyIndex = 0;
+
+
+	NdisZeroMemory(mic, sizeof(mic));
+	NdisZeroMemory(digest, sizeof(digest));
+	NdisZeroMemory(KEYDATA, sizeof(KEYDATA));
+	NdisZeroMemory((PUCHAR)&EapolKeyInfo, sizeof(EapolKeyInfo));
+
+	NdisMoveMemory((PUCHAR)&EapolKeyInfo, (PUCHAR)&pMsg->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT *)&EapolKeyInfo) = cpu2le16(*((USHORT *)&EapolKeyInfo));
+
+	// Choose WPA2 or not
+	if ((pEntry->AuthMode == Ndis802_11AuthModeWPA2) || (pEntry->AuthMode == Ndis802_11AuthModeWPA2PSK))
+		bWPA2 = TRUE;
+
+	// 0. Check MsgType
+	if ((MsgType > EAPOL_GROUP_MSG_2) || (MsgType < EAPOL_PAIR_MSG_1))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("The message type is invalid(%d)! \n", MsgType));
+		return FALSE;
+	}
+
+	// 1. Replay counter check
+ 	if (MsgType == EAPOL_PAIR_MSG_1 || MsgType == EAPOL_PAIR_MSG_3 || MsgType == EAPOL_GROUP_MSG_1)	// For supplicant
+    {
+    	// First validate replay counter, only accept message with larger replay counter.
+		// Let equal pass, some AP start with all zero replay counter
+		UCHAR	ZeroReplay[LEN_KEY_DESC_REPLAY];
+
+        NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);
+		if ((RTMPCompareMemory(pMsg->KeyDesc.ReplayCounter, pEntry->R_Counter, LEN_KEY_DESC_REPLAY) != 1) &&
+			(RTMPCompareMemory(pMsg->KeyDesc.ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
+    	{
+			bReplayDiff = TRUE;
+    	}
+ 	}
+	else if (MsgType == EAPOL_PAIR_MSG_2 || MsgType == EAPOL_PAIR_MSG_4 || MsgType == EAPOL_GROUP_MSG_2)	// For authenticator
+	{
+		// check Replay Counter coresponds to MSG from authenticator, otherwise discard
+    	if (!NdisEqualMemory(pMsg->KeyDesc.ReplayCounter, pEntry->R_Counter, LEN_KEY_DESC_REPLAY))
+    	{
+			bReplayDiff = TRUE;
+    	}
+	}
+
+	// Replay Counter different condition
+	if (bReplayDiff)
+	{
+		// send wireless event - for replay counter different
+		if (pAd->CommonCfg.bWirelessEvent)
+			RTMPSendWirelessEvent(pAd, IW_REPLAY_COUNTER_DIFF_EVENT_FLAG, pEntry->Addr, pEntry->apidx, 0);
+
+		if (MsgType < EAPOL_GROUP_MSG_1)
+		{
+           	DBGPRINT(RT_DEBUG_ERROR, ("Replay Counter Different in pairwise msg %d of 4-way handshake!\n", MsgType));
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Replay Counter Different in group msg %d of 2-way handshake!\n", (MsgType - EAPOL_PAIR_MSG_4)));
+		}
+
+		hex_dump("Receive replay counter ", pMsg->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+		hex_dump("Current replay counter ", pEntry->R_Counter, LEN_KEY_DESC_REPLAY);
+        return FALSE;
+	}
+
+	// 2. Verify MIC except Pairwise Msg1
+	if (MsgType != EAPOL_PAIR_MSG_1)
+	{
+		UCHAR			rcvd_mic[LEN_KEY_DESC_MIC];
+
+		// Record the received MIC for check later
+		NdisMoveMemory(rcvd_mic, pMsg->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+		NdisZeroMemory(pMsg->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+
+        if (pEntry->WepStatus == Ndis802_11Encryption2Enabled)	// TKIP
+        {
+            hmac_md5(pEntry->PTK, LEN_EAP_MICK, (PUCHAR)pMsg, MsgLen, mic);
+        }
+        else if (pEntry->WepStatus == Ndis802_11Encryption3Enabled)	// AES
+        {
+            HMAC_SHA1((PUCHAR)pMsg, MsgLen, pEntry->PTK, LEN_EAP_MICK, digest);
+            NdisMoveMemory(mic, digest, LEN_KEY_DESC_MIC);
+        }
+
+        if (!NdisEqualMemory(rcvd_mic, mic, LEN_KEY_DESC_MIC))
+        {
+			// send wireless event - for MIC different
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_MIC_DIFF_EVENT_FLAG, pEntry->Addr, pEntry->apidx, 0);
+
+			if (MsgType < EAPOL_GROUP_MSG_1)
+			{
+            	DBGPRINT(RT_DEBUG_ERROR, ("MIC Different in pairwise msg %d of 4-way handshake!\n", MsgType));
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("MIC Different in group msg %d of 2-way handshake!\n", (MsgType - EAPOL_PAIR_MSG_4)));
+			}
+
+			hex_dump("Received MIC", rcvd_mic, LEN_KEY_DESC_MIC);
+			hex_dump("Desired  MIC", mic, LEN_KEY_DESC_MIC);
+
+			return FALSE;
+        }
+	}
+
+	// Extract the context of the Key Data field if it exist
+	// The field in pairwise_msg_2_WPA1(WPA2) & pairwise_msg_3_WPA1 is un-encrypted.
+	// The field in group_msg_1_WPA1(WPA2) & pairwise_msg_3_WPA2 is encrypted.
+	if (pMsg->KeyDesc.KeyDataLen[1] > 0)
+	{
+		// Decrypt this field
+		if ((MsgType == EAPOL_PAIR_MSG_3 && bWPA2) || (MsgType == EAPOL_GROUP_MSG_1))
+		{
+			if(pEntry->WepStatus == Ndis802_11Encryption3Enabled)
+			{
+				// AES
+				AES_GTK_KEY_UNWRAP(&pEntry->PTK[16], KEYDATA, pMsg->KeyDesc.KeyDataLen[1],pMsg->KeyDesc.KeyData);
+			}
+			else
+			{
+				INT 	i;
+				UCHAR   Key[32];
+				// Decrypt TKIP GTK
+				// Construct 32 bytes RC4 Key
+				NdisMoveMemory(Key, pMsg->KeyDesc.KeyIv, 16);
+				NdisMoveMemory(&Key[16], &pEntry->PTK[16], 16);
+				ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
+				//discard first 256 bytes
+				for(i = 0; i < 256; i++)
+					ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
+				// Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
+				ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pMsg->KeyDesc.KeyData, pMsg->KeyDesc.KeyDataLen[1]);
+			}
+
+			if (!bWPA2 && (MsgType == EAPOL_GROUP_MSG_1))
+				GroupKeyIndex = EapolKeyInfo.KeyIndex;
+
+		}
+		else if ((MsgType == EAPOL_PAIR_MSG_2) || (MsgType == EAPOL_PAIR_MSG_3 && !bWPA2))
+		{
+			NdisMoveMemory(KEYDATA, pMsg->KeyDesc.KeyData, pMsg->KeyDesc.KeyDataLen[1]);
+		}
+		else
+		{
+
+			return TRUE;
+		}
+
+		// Parse Key Data field to
+		// 1. verify RSN IE for pairwise_msg_2_WPA1(WPA2) ,pairwise_msg_3_WPA1(WPA2)
+		// 2. verify KDE format for pairwise_msg_3_WPA2, group_msg_1_WPA2
+		// 3. update shared key for pairwise_msg_3_WPA2, group_msg_1_WPA1(WPA2)
+		if (!RTMPParseEapolKeyData(pAd, KEYDATA, pMsg->KeyDesc.KeyDataLen[1], GroupKeyIndex, MsgType, bWPA2, pEntry))
+		{
+			return FALSE;
+		}
+	}
+
+	return TRUE;
+
+}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+BOOLEAN MlmeDlsReqSanity(
+	IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PRT_802_11_DLS *pDLS,
+    OUT PUSHORT pReason)
+{
+	MLME_DLS_REQ_STRUCT *pInfo;
+
+    pInfo = (MLME_DLS_REQ_STRUCT *)Msg;
+
+	*pDLS = pInfo->pDLS;
+	*pReason = pInfo->Reason;
+
+	return TRUE;
+}
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef QOS_DLS_SUPPORT
+BOOLEAN PeerDlsReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pDlsTimeout,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+	OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability)
+{
+	CHAR            *Ptr;
+    PFRAME_802_11	Fr = (PFRAME_802_11)Msg;
+	PEID_STRUCT  eid_ptr;
+
+    // to prevent caller from using garbage output value
+    *pCapabilityInfo	= 0;
+    *pDlsTimeout	= 0;
+	*pHtCapabilityLen = 0;
+
+    Ptr = Fr->Octet;
+
+	// offset to destination MAC address (Category and Action field)
+    Ptr += 2;
+
+    // get DA from payload and advance the pointer
+    NdisMoveMemory(pDA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get SA from payload and advance the pointer
+    NdisMoveMemory(pSA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get capability info from payload and advance the pointer
+    NdisMoveMemory(pCapabilityInfo, Ptr, 2);
+    Ptr += 2;
+
+    // get capability info from payload and advance the pointer
+    NdisMoveMemory(pDlsTimeout, Ptr, 2);
+    Ptr += 2;
+
+	// Category and Action field + DA + SA + capability + Timeout
+	eid_ptr = (PEID_STRUCT) &Fr->Octet[18];
+
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((UCHAR*)Fr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_SUPP_RATES:
+                if ((eid_ptr->Len <= MAX_LEN_OF_SUPPORTED_RATES) && (eid_ptr->Len > 0))
+                {
+                    NdisMoveMemory(Rates, eid_ptr->Octet, eid_ptr->Len);
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - IE_SUPP_RATES., Len=%d. Rates[0]=%x\n",eid_ptr->Len, Rates[0]));
+                    DBGPRINT(RT_DEBUG_TRACE, ("Rates[1]=%x %x %x %x %x %x %x\n", Rates[1], Rates[2], Rates[3], Rates[4], Rates[5], Rates[6], Rates[7]));
+                    *pRatesLen = eid_ptr->Len;
+                }
+                else
+                {
+                    *pRatesLen = 8;
+					Rates[0] = 0x82;
+					Rates[1] = 0x84;
+					Rates[2] = 0x8b;
+					Rates[3] = 0x96;
+					Rates[4] = 0x12;
+					Rates[5] = 0x24;
+					Rates[6] = 0x48;
+					Rates[7] = 0x6c;
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - wrong IE_SUPP_RATES., Len=%d\n",eid_ptr->Len));
+                }
+				break;
+
+			case IE_EXT_SUPP_RATES:
+                if (eid_ptr->Len + *pRatesLen <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, eid_ptr->Len);
+                    *pRatesLen = (*pRatesLen) + eid_ptr->Len;
+                }
+                else
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, MAX_LEN_OF_SUPPORTED_RATES - (*pRatesLen));
+                    *pRatesLen = MAX_LEN_OF_SUPPORTED_RATES;
+                }
+				break;
+
+			case IE_HT_CAP:
+				if (eid_ptr->Len >= sizeof(HT_CAPABILITY_IE))
+				{
+					NdisMoveMemory(pHtCapability, eid_ptr->Octet, sizeof(HT_CAPABILITY_IE));
+
+					*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+					*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+					*pHtCapabilityLen = sizeof(HT_CAPABILITY_IE);
+
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - IE_HT_CAP\n"));
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - wrong IE_HT_CAP.eid_ptr->Len = %d\n", eid_ptr->Len));
+				}
+				break;
+
+			default:
+				break;
+		}
+
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+    return TRUE;
+}
+
+BOOLEAN PeerDlsRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pStatus,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability)
+{
+    CHAR            *Ptr;
+    PFRAME_802_11	Fr = (PFRAME_802_11)Msg;
+	PEID_STRUCT  eid_ptr;
+
+    // to prevent caller from using garbage output value
+    *pStatus		= 0;
+    *pCapabilityInfo	= 0;
+	*pHtCapabilityLen = 0;
+
+    Ptr = Fr->Octet;
+
+	// offset to destination MAC address (Category and Action field)
+    Ptr += 2;
+
+	// get status code from payload and advance the pointer
+    NdisMoveMemory(pStatus, Ptr, 2);
+    Ptr += 2;
+
+    // get DA from payload and advance the pointer
+    NdisMoveMemory(pDA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get SA from payload and advance the pointer
+    NdisMoveMemory(pSA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+	if (pStatus == 0)
+	{
+	    // get capability info from payload and advance the pointer
+	    NdisMoveMemory(pCapabilityInfo, Ptr, 2);
+	    Ptr += 2;
+	}
+
+	// Category and Action field + status code + DA + SA + capability
+	eid_ptr = (PEID_STRUCT) &Fr->Octet[18];
+
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((UCHAR*)Fr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_SUPP_RATES:
+                if ((eid_ptr->Len <= MAX_LEN_OF_SUPPORTED_RATES) && (eid_ptr->Len > 0))
+                {
+                    NdisMoveMemory(Rates, eid_ptr->Octet, eid_ptr->Len);
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - IE_SUPP_RATES., Len=%d. Rates[0]=%x\n",eid_ptr->Len, Rates[0]));
+                    DBGPRINT(RT_DEBUG_TRACE, ("Rates[1]=%x %x %x %x %x %x %x\n", Rates[1], Rates[2], Rates[3], Rates[4], Rates[5], Rates[6], Rates[7]));
+                    *pRatesLen = eid_ptr->Len;
+                }
+                else
+                {
+                    *pRatesLen = 8;
+					Rates[0] = 0x82;
+					Rates[1] = 0x84;
+					Rates[2] = 0x8b;
+					Rates[3] = 0x96;
+					Rates[4] = 0x12;
+					Rates[5] = 0x24;
+					Rates[6] = 0x48;
+					Rates[7] = 0x6c;
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - wrong IE_SUPP_RATES., Len=%d\n",eid_ptr->Len));
+                }
+				break;
+
+			case IE_EXT_SUPP_RATES:
+                if (eid_ptr->Len + *pRatesLen <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, eid_ptr->Len);
+                    *pRatesLen = (*pRatesLen) + eid_ptr->Len;
+                }
+                else
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, MAX_LEN_OF_SUPPORTED_RATES - (*pRatesLen));
+                    *pRatesLen = MAX_LEN_OF_SUPPORTED_RATES;
+                }
+				break;
+
+			case IE_HT_CAP:
+				if (eid_ptr->Len >= sizeof(HT_CAPABILITY_IE))
+				{
+					NdisMoveMemory(pHtCapability, eid_ptr->Octet, sizeof(HT_CAPABILITY_IE));
+
+					*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+					*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+					*pHtCapabilityLen = sizeof(HT_CAPABILITY_IE);
+
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - IE_HT_CAP\n"));
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - wrong IE_HT_CAP.eid_ptr->Len = %d\n", eid_ptr->Len));
+				}
+				break;
+
+			default:
+				break;
+		}
+
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+    return TRUE;
+}
+
+BOOLEAN PeerDlsTearDownSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pReason)
+{
+    CHAR            *Ptr;
+    PFRAME_802_11	Fr = (PFRAME_802_11)Msg;
+
+    // to prevent caller from using garbage output value
+    *pReason	= 0;
+
+    Ptr = Fr->Octet;
+
+	// offset to destination MAC address (Category and Action field)
+    Ptr += 2;
+
+    // get DA from payload and advance the pointer
+    NdisMoveMemory(pDA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get SA from payload and advance the pointer
+    NdisMoveMemory(pSA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+	// get reason code from payload and advance the pointer
+    NdisMoveMemory(pReason, Ptr, 2);
+    Ptr += 2;
+
+    return TRUE;
+}
+#endif // QOS_DLS_SUPPORT //
+
diff --git a/drivers/staging/rt2860/common/cmm_sync.c b/drivers/staging/rt2860/common/cmm_sync.c
new file mode 100644
index 000000000000..40e4109118e0
--- /dev/null
+++ b/drivers/staging/rt2860/common/cmm_sync.c
@@ -0,0 +1,702 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sync.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2004-09-01      modified for rt2561/2661
+*/
+#include "../rt_config.h"
+
+// 2.4 Ghz channel plan index in the TxPower arrays.
+#define	BG_BAND_REGION_0_START	0			// 1,2,3,4,5,6,7,8,9,10,11
+#define	BG_BAND_REGION_0_SIZE	11
+#define	BG_BAND_REGION_1_START	0			// 1,2,3,4,5,6,7,8,9,10,11,12,13
+#define	BG_BAND_REGION_1_SIZE	13
+#define	BG_BAND_REGION_2_START	9			// 10,11
+#define	BG_BAND_REGION_2_SIZE	2
+#define	BG_BAND_REGION_3_START	9			// 10,11,12,13
+#define	BG_BAND_REGION_3_SIZE	4
+#define	BG_BAND_REGION_4_START	13			// 14
+#define	BG_BAND_REGION_4_SIZE	1
+#define	BG_BAND_REGION_5_START	0			// 1,2,3,4,5,6,7,8,9,10,11,12,13,14
+#define	BG_BAND_REGION_5_SIZE	14
+#define	BG_BAND_REGION_6_START	2			// 3,4,5,6,7,8,9
+#define	BG_BAND_REGION_6_SIZE	7
+#define	BG_BAND_REGION_7_START	4			// 5,6,7,8,9,10,11,12,13
+#define	BG_BAND_REGION_7_SIZE	9
+#define	BG_BAND_REGION_31_START	0			// 1,2,3,4,5,6,7,8,9,10,11,12,13,14
+#define	BG_BAND_REGION_31_SIZE	14
+
+// 5 Ghz channel plan index in the TxPower arrays.
+UCHAR A_BAND_REGION_0_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_1_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+UCHAR A_BAND_REGION_2_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64};
+UCHAR A_BAND_REGION_3_CHANNEL_LIST[]={52, 56, 60, 64, 149, 153, 157, 161};
+UCHAR A_BAND_REGION_4_CHANNEL_LIST[]={149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_5_CHANNEL_LIST[]={149, 153, 157, 161};
+UCHAR A_BAND_REGION_6_CHANNEL_LIST[]={36, 40, 44, 48};
+UCHAR A_BAND_REGION_7_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_8_CHANNEL_LIST[]={52, 56, 60, 64};
+UCHAR A_BAND_REGION_9_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_10_CHANNEL_LIST[]={36, 40, 44, 48, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_11_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 149, 153, 157, 161};
+
+//BaSizeArray follows the 802.11n definition as MaxRxFactor.  2^(13+factor) bytes. When factor =0, it's about Ba buffer size =8.
+UCHAR BaSizeArray[4] = {8,16,32,64};
+
+/*
+	==========================================================================
+	Description:
+		Update StaCfg->ChannelList[] according to 1) Country Region 2) RF IC type,
+		and 3) PHY-mode user selected.
+		The outcome is used by driver when doing site survey.
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID BuildChannelList(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR i, j, index=0, num=0;
+	PUCHAR	pChannelList = NULL;
+
+	NdisZeroMemory(pAd->ChannelList, MAX_NUM_OF_CHANNELS * sizeof(CHANNEL_TX_POWER));
+
+	// if not 11a-only mode, channel list starts from 2.4Ghz band
+	if ((pAd->CommonCfg.PhyMode != PHY_11A)
+#ifdef DOT11_N_SUPPORT
+		&& (pAd->CommonCfg.PhyMode != PHY_11AN_MIXED) && (pAd->CommonCfg.PhyMode != PHY_11N_5G)
+#endif // DOT11_N_SUPPORT //
+	)
+	{
+		switch (pAd->CommonCfg.CountryRegion  & 0x7f)
+		{
+			case REGION_0_BG_BAND:	// 1 -11
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_0_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_0_SIZE);
+				index += BG_BAND_REGION_0_SIZE;
+				break;
+			case REGION_1_BG_BAND:	// 1 - 13
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_1_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_1_SIZE);
+				index += BG_BAND_REGION_1_SIZE;
+				break;
+			case REGION_2_BG_BAND:	// 10 - 11
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_2_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_2_SIZE);
+				index += BG_BAND_REGION_2_SIZE;
+				break;
+			case REGION_3_BG_BAND:	// 10 - 13
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_3_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_3_SIZE);
+				index += BG_BAND_REGION_3_SIZE;
+				break;
+			case REGION_4_BG_BAND:	// 14
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_4_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_4_SIZE);
+				index += BG_BAND_REGION_4_SIZE;
+				break;
+			case REGION_5_BG_BAND:	// 1 - 14
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_5_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_5_SIZE);
+				index += BG_BAND_REGION_5_SIZE;
+				break;
+			case REGION_6_BG_BAND:	// 3 - 9
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_6_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_6_SIZE);
+				index += BG_BAND_REGION_6_SIZE;
+				break;
+			case REGION_7_BG_BAND:  // 5 - 13
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_7_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_7_SIZE);
+				index += BG_BAND_REGION_7_SIZE;
+				break;
+			case REGION_31_BG_BAND:	// 1 - 14
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_31_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_31_SIZE);
+				index += BG_BAND_REGION_31_SIZE;
+				break;
+			default:            // Error. should never happen
+				break;
+		}
+		for (i=0; i<index; i++)
+			pAd->ChannelList[i].MaxTxPwr = 20;
+	}
+
+	if ((pAd->CommonCfg.PhyMode == PHY_11A) || (pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED)
+#ifdef DOT11_N_SUPPORT
+		|| (pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED) || (pAd->CommonCfg.PhyMode == PHY_11AN_MIXED)
+		|| (pAd->CommonCfg.PhyMode == PHY_11AGN_MIXED) || (pAd->CommonCfg.PhyMode == PHY_11N_5G)
+#endif // DOT11_N_SUPPORT //
+	)
+	{
+		switch (pAd->CommonCfg.CountryRegionForABand & 0x7f)
+		{
+			case REGION_0_A_BAND:
+				num = sizeof(A_BAND_REGION_0_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_0_CHANNEL_LIST;
+				break;
+			case REGION_1_A_BAND:
+				num = sizeof(A_BAND_REGION_1_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_1_CHANNEL_LIST;
+				break;
+			case REGION_2_A_BAND:
+				num = sizeof(A_BAND_REGION_2_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_2_CHANNEL_LIST;
+				break;
+			case REGION_3_A_BAND:
+				num = sizeof(A_BAND_REGION_3_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_3_CHANNEL_LIST;
+				break;
+			case REGION_4_A_BAND:
+				num = sizeof(A_BAND_REGION_4_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_4_CHANNEL_LIST;
+				break;
+			case REGION_5_A_BAND:
+				num = sizeof(A_BAND_REGION_5_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_5_CHANNEL_LIST;
+				break;
+			case REGION_6_A_BAND:
+				num = sizeof(A_BAND_REGION_6_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_6_CHANNEL_LIST;
+				break;
+			case REGION_7_A_BAND:
+				num = sizeof(A_BAND_REGION_7_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_7_CHANNEL_LIST;
+				break;
+			case REGION_8_A_BAND:
+				num = sizeof(A_BAND_REGION_8_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_8_CHANNEL_LIST;
+				break;
+			case REGION_9_A_BAND:
+				num = sizeof(A_BAND_REGION_9_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_9_CHANNEL_LIST;
+				break;
+
+			case REGION_10_A_BAND:
+				num = sizeof(A_BAND_REGION_10_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_10_CHANNEL_LIST;
+				break;
+
+			case REGION_11_A_BAND:
+				num = sizeof(A_BAND_REGION_11_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_11_CHANNEL_LIST;
+				break;
+
+			default:            // Error. should never happen
+				DBGPRINT(RT_DEBUG_WARN,("countryregion=%d not support", pAd->CommonCfg.CountryRegionForABand));
+				break;
+		}
+
+		if (num != 0)
+		{
+			UCHAR RadarCh[15]={52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+			for (i=0; i<num; i++)
+			{
+				for (j=0; j<MAX_NUM_OF_CHANNELS; j++)
+				{
+					if (pChannelList[i] == pAd->TxPower[j].Channel)
+						NdisMoveMemory(&pAd->ChannelList[index+i], &pAd->TxPower[j], sizeof(CHANNEL_TX_POWER));
+					}
+				for (j=0; j<15; j++)
+				{
+					if (pChannelList[i] == RadarCh[j])
+						pAd->ChannelList[index+i].DfsReq = TRUE;
+				}
+				pAd->ChannelList[index+i].MaxTxPwr = 20;
+			}
+			index += num;
+		}
+	}
+
+	pAd->ChannelListNum = index;
+	DBGPRINT(RT_DEBUG_TRACE,("country code=%d/%d, RFIC=%d, PHY mode=%d, support %d channels\n",
+		pAd->CommonCfg.CountryRegion, pAd->CommonCfg.CountryRegionForABand, pAd->RfIcType, pAd->CommonCfg.PhyMode, pAd->ChannelListNum));
+#ifdef DBG
+	for (i=0;i<pAd->ChannelListNum;i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE,("BuildChannel # %d :: Pwr0 = %d, Pwr1 =%d, \n ", pAd->ChannelList[i].Channel, pAd->ChannelList[i].Power, pAd->ChannelList[i].Power2));
+	}
+#endif
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine return the first channel number according to the country
+		code selection and RF IC selection (signal band or dual band). It is called
+		whenever driver need to start a site survey of all supported channels.
+	Return:
+		ch - the first channel number of current country code setting
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+UCHAR FirstChannel(
+	IN PRTMP_ADAPTER pAd)
+{
+	return pAd->ChannelList[0].Channel;
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine returns the next channel number. This routine is called
+		during driver need to start a site survey of all supported channels.
+	Return:
+		next_channel - the next channel number valid in current country code setting.
+	Note:
+		return 0 if no more next channel
+	==========================================================================
+ */
+UCHAR NextChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR channel)
+{
+	int i;
+	UCHAR next_channel = 0;
+
+	for (i = 0; i < (pAd->ChannelListNum - 1); i++)
+		if (channel == pAd->ChannelList[i].Channel)
+		{
+			next_channel = pAd->ChannelList[i+1].Channel;
+			break;
+	}
+	return next_channel;
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine is for Cisco Compatible Extensions 2.X
+		Spec31. AP Control of Client Transmit Power
+	Return:
+		None
+	Note:
+	   Required by Aironet dBm(mW)
+		   0dBm(1mW),   1dBm(5mW), 13dBm(20mW), 15dBm(30mW),
+		  17dBm(50mw), 20dBm(100mW)
+
+	   We supported
+		   3dBm(Lowest), 6dBm(10%), 9dBm(25%), 12dBm(50%),
+		  14dBm(75%),   15dBm(100%)
+
+		The client station's actual transmit power shall be within +/- 5dB of
+		the minimum value or next lower value.
+	==========================================================================
+ */
+VOID ChangeToCellPowerLimit(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         AironetCellPowerLimit)
+{
+	//valud 0xFF means that hasn't found power limit information
+	//from the AP's Beacon/Probe response.
+	if (AironetCellPowerLimit == 0xFF)
+		return;
+
+	if (AironetCellPowerLimit < 6) //Used Lowest Power Percentage.
+		pAd->CommonCfg.TxPowerPercentage = 6;
+	else if (AironetCellPowerLimit < 9)
+		pAd->CommonCfg.TxPowerPercentage = 10;
+	else if (AironetCellPowerLimit < 12)
+		pAd->CommonCfg.TxPowerPercentage = 25;
+	else if (AironetCellPowerLimit < 14)
+		pAd->CommonCfg.TxPowerPercentage = 50;
+	else if (AironetCellPowerLimit < 15)
+		pAd->CommonCfg.TxPowerPercentage = 75;
+	else
+		pAd->CommonCfg.TxPowerPercentage = 100; //else used maximum
+
+	if (pAd->CommonCfg.TxPowerPercentage > pAd->CommonCfg.TxPowerDefault)
+		pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+
+}
+
+CHAR	ConvertToRssi(
+	IN PRTMP_ADAPTER pAd,
+	IN	CHAR			Rssi,
+	IN  UCHAR   RssiNumber)
+{
+	UCHAR	RssiOffset, LNAGain;
+
+	// Rssi equals to zero should be an invalid value
+	if (Rssi == 0)
+		return -99;
+
+	LNAGain = GET_LNA_GAIN(pAd);
+    if (pAd->LatchRfRegs.Channel > 14)
+    {
+        if (RssiNumber == 0)
+			RssiOffset = pAd->ARssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->ARssiOffset1;
+		else
+			RssiOffset = pAd->ARssiOffset2;
+    }
+    else
+    {
+        if (RssiNumber == 0)
+			RssiOffset = pAd->BGRssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->BGRssiOffset1;
+		else
+			RssiOffset = pAd->BGRssiOffset2;
+    }
+
+    return (-12 - RssiOffset - LNAGain - Rssi);
+}
+
+/*
+	==========================================================================
+	Description:
+		Scan next channel
+	==========================================================================
+ */
+VOID ScanNextChannel(
+	IN PRTMP_ADAPTER pAd)
+{
+	HEADER_802_11   Hdr80211;
+	PUCHAR          pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG           FrameLen = 0;
+	UCHAR           SsidLen = 0, ScanType = pAd->MlmeAux.ScanType, BBPValue = 0;
+#ifdef CONFIG_STA_SUPPORT
+	USHORT          Status;
+	PHEADER_802_11  pHdr80211;
+#endif // CONFIG_STA_SUPPORT //
+	UINT			ScanTimeIn5gChannel = SHORT_CHANNEL_TIME;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (MONITOR_ON(pAd))
+			return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RALINK_ATE
+	// Nothing to do in ATE mode.
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	if (pAd->MlmeAux.Channel == 0)
+	{
+		if ((pAd->CommonCfg.BBPCurrentBW == BW_40)
+#ifdef CONFIG_STA_SUPPORT
+			&& (INFRA_ON(pAd)
+				|| (pAd->OpMode == OPMODE_AP))
+#endif // CONFIG_STA_SUPPORT //
+			)
+		{
+			AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue &= (~0x18);
+			BBPValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - End of SCAN, restore to 40MHz channel %d, Total BSS[%02d]\n",pAd->CommonCfg.CentralChannel, pAd->ScanTab.BssNr));
+		}
+		else
+		{
+			AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - End of SCAN, restore to channel %d, Total BSS[%02d]\n",pAd->CommonCfg.Channel, pAd->ScanTab.BssNr));
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			//
+			// To prevent data lost.
+			// Send an NULL data with turned PSM bit on to current associated AP before SCAN progress.
+			// Now, we need to send an NULL data with turned PSM bit off to AP, when scan progress done
+			//
+			if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) && (INFRA_ON(pAd)))
+			{
+				NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);
+				if (NStatus	== NDIS_STATUS_SUCCESS)
+				{
+					pHdr80211 = (PHEADER_802_11) pOutBuffer;
+					MgtMacHeaderInit(pAd, pHdr80211, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+					pHdr80211->Duration = 0;
+					pHdr80211->FC.Type = BTYPE_DATA;
+					pHdr80211->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
+
+					// Send using priority queue
+					MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+					DBGPRINT(RT_DEBUG_TRACE, ("MlmeScanReqAction -- Send PSM Data frame\n"));
+					MlmeFreeMemory(pAd, pOutBuffer);
+					RTMPusecDelay(5000);
+				}
+			}
+
+			pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+			Status = MLME_SUCCESS;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+
+		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+	}
+	else
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+		// BBP and RF are not accessible in PS mode, we has to wake them up first
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+			AsicForceWakeup(pAd, TRUE);
+
+			// leave PSM during scanning. otherwise we may lost ProbeRsp & BEACON
+			if (pAd->StaCfg.Psm == PWR_SAVE)
+				MlmeSetPsmBit(pAd, PWR_ACTIVE);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		AsicSwitchChannel(pAd, pAd->MlmeAux.Channel, TRUE);
+		AsicLockChannel(pAd, pAd->MlmeAux.Channel);
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (pAd->MlmeAux.Channel > 14)
+			{
+				if ((pAd->CommonCfg.bIEEE80211H == 1) && RadarChannelCheck(pAd, pAd->MlmeAux.Channel))
+				{
+					ScanType = SCAN_PASSIVE;
+					ScanTimeIn5gChannel = MIN_CHANNEL_TIME;
+				}
+			}
+
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+			// carrier detection
+			if (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+			{
+				ScanType = SCAN_PASSIVE;
+				ScanTimeIn5gChannel = MIN_CHANNEL_TIME;
+			}
+#endif // CARRIER_DETECTION_SUPPORT //
+		}
+
+#endif // CONFIG_STA_SUPPORT //
+
+		//Global country domain(ch1-11:active scan, ch12-14 passive scan)
+		if ((pAd->MlmeAux.Channel <= 14) && (pAd->MlmeAux.Channel >= 12) && ((pAd->CommonCfg.CountryRegion & 0x7f) == REGION_31_BG_BAND))
+		{
+			ScanType = SCAN_PASSIVE;
+		}
+
+		// We need to shorten active scan time in order for WZC connect issue
+		// Chnage the channel scan time for CISCO stuff based on its IAPP announcement
+		if (ScanType == FAST_SCAN_ACTIVE)
+			RTMPSetTimer(&pAd->MlmeAux.ScanTimer, FAST_ACTIVE_SCAN_TIME);
+#ifdef CONFIG_STA_SUPPORT
+		else if (((ScanType == SCAN_CISCO_ACTIVE) ||
+				(ScanType == SCAN_CISCO_PASSIVE) ||
+				(ScanType == SCAN_CISCO_CHANNEL_LOAD) ||
+				(ScanType == SCAN_CISCO_NOISE)) && (pAd->OpMode == OPMODE_STA))
+		{
+			if (pAd->StaCfg.CCXScanTime < 25)
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, pAd->StaCfg.CCXScanTime * 2);
+			else
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, pAd->StaCfg.CCXScanTime);
+		}
+#endif // CONFIG_STA_SUPPORT //
+		else // must be SCAN_PASSIVE or SCAN_ACTIVE
+		{
+			if ((pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED)
+#ifdef DOT11_N_SUPPORT
+				|| (pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED) || (pAd->CommonCfg.PhyMode == PHY_11AGN_MIXED)
+#endif // DOT11_N_SUPPORT //
+			)
+			{
+				if (pAd->MlmeAux.Channel > 14)
+					RTMPSetTimer(&pAd->MlmeAux.ScanTimer, ScanTimeIn5gChannel);
+				else
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, MIN_CHANNEL_TIME);
+			}
+			else
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, MAX_CHANNEL_TIME);
+		}
+
+		if ((ScanType == SCAN_ACTIVE) || (ScanType == FAST_SCAN_ACTIVE) ||
+			(ScanType == SCAN_CISCO_ACTIVE))
+		{
+			NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("SYNC - ScanNextChannel() allocate memory fail\n"));
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+					Status = MLME_FAIL_NO_RESOURCE;
+					MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+				}
+#endif // CONFIG_STA_SUPPORT //
+
+				return;
+			}
+
+			// There is no need to send broadcast probe request if active scan is in effect.
+			if ((ScanType == SCAN_ACTIVE) || (ScanType == FAST_SCAN_ACTIVE)
+				)
+				SsidLen = pAd->MlmeAux.SsidLen;
+			else
+				SsidLen = 0;
+
+			MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, BROADCAST_ADDR, BROADCAST_ADDR);
+			MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+							  sizeof(HEADER_802_11),    &Hdr80211,
+							  1,                        &SsidIe,
+							  1,                        &SsidLen,
+							  SsidLen,			        pAd->MlmeAux.Ssid,
+							  1,                        &SupRateIe,
+							  1,                        &pAd->CommonCfg.SupRateLen,
+							  pAd->CommonCfg.SupRateLen,  pAd->CommonCfg.SupRate,
+							  END_OF_ARGS);
+
+			if (pAd->CommonCfg.ExtRateLen)
+			{
+				ULONG Tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &Tmp,
+								  1,                                &ExtRateIe,
+								  1,                                &pAd->CommonCfg.ExtRateLen,
+								  pAd->CommonCfg.ExtRateLen,          pAd->CommonCfg.ExtRate,
+								  END_OF_ARGS);
+				FrameLen += Tmp;
+			}
+
+#ifdef DOT11_N_SUPPORT
+			if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+			{
+				ULONG	Tmp;
+				UCHAR	HtLen;
+				UCHAR	BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+#ifdef RT_BIG_ENDIAN
+				HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+				if (pAd->bBroadComHT == TRUE)
+				{
+					HtLen = pAd->MlmeAux.HtCapabilityLen + 4;
+#ifdef RT_BIG_ENDIAN
+					NdisMoveMemory(&HtCapabilityTmp, &pAd->MlmeAux.HtCapability, SIZE_HT_CAP_IE);
+					*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+					*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &WpaIe,
+									1,                                &HtLen,
+									4,                                &BROADCOM[0],
+									pAd->MlmeAux.HtCapabilityLen,     &HtCapabilityTmp,
+									END_OF_ARGS);
+#else
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &WpaIe,
+									1,                                &HtLen,
+									4,                                &BROADCOM[0],
+									pAd->MlmeAux.HtCapabilityLen,     &pAd->MlmeAux.HtCapability,
+									END_OF_ARGS);
+#endif // RT_BIG_ENDIAN //
+				}
+				else
+				{
+					HtLen = pAd->MlmeAux.HtCapabilityLen;
+#ifdef RT_BIG_ENDIAN
+					NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, SIZE_HT_CAP_IE);
+					*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+					*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &HtCapIe,
+									1,                                &HtLen,
+									HtLen,                            &HtCapabilityTmp,
+									END_OF_ARGS);
+#else
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &HtCapIe,
+									1,                                &HtLen,
+									HtLen,                            &pAd->CommonCfg.HtCapability,
+									END_OF_ARGS);
+#endif // RT_BIG_ENDIAN //
+				}
+				FrameLen += Tmp;
+
+#ifdef DOT11N_DRAFT3
+				if (pAd->CommonCfg.BACapability.field.b2040CoexistScanSup == 1)
+				{
+					ULONG		Tmp;
+					HtLen = 1;
+					MakeOutgoingFrame(pOutBuffer + FrameLen,            &Tmp,
+									  1,					&ExtHtCapIe,
+									  1,					&HtLen,
+									  1,          			&pAd->CommonCfg.BSSCoexist2040.word,
+									  END_OF_ARGS);
+
+					FrameLen += Tmp;
+				}
+#endif // DOT11N_DRAFT3 //
+			}
+#endif // DOT11_N_SUPPORT //
+
+
+			MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+
+		// For SCAN_CISCO_PASSIVE, do nothing and silently wait for beacon or other probe reponse
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			pAd->Mlme.SyncMachine.CurrState = SCAN_LISTEN;
+#endif // CONFIG_STA_SUPPORT //
+
+	}
+}
+
+VOID MgtProbReqMacHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PHEADER_802_11 pHdr80211,
+	IN UCHAR SubType,
+	IN UCHAR ToDs,
+	IN PUCHAR pDA,
+	IN PUCHAR pBssid)
+{
+	NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+
+	pHdr80211->FC.Type = BTYPE_MGMT;
+	pHdr80211->FC.SubType = SubType;
+	if (SubType == SUBTYPE_ACK)
+		pHdr80211->FC.Type = BTYPE_CNTL;
+	pHdr80211->FC.ToDs = ToDs;
+	COPY_MAC_ADDR(pHdr80211->Addr1, pDA);
+	COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pHdr80211->Addr3, pBssid);
+}
+
+
diff --git a/drivers/staging/rt2860/common/cmm_wpa.c b/drivers/staging/rt2860/common/cmm_wpa.c
new file mode 100644
index 000000000000..81c332ac2524
--- /dev/null
+++ b/drivers/staging/rt2860/common/cmm_wpa.c
@@ -0,0 +1,1606 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	wpa.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Jan	Lee		03-07-22		Initial
+	Paul Lin	03-11-28		Modify for supplicant
+*/
+#include "../rt_config.h"
+// WPA OUI
+UCHAR		OUI_WPA_NONE_AKM[4]		= {0x00, 0x50, 0xF2, 0x00};
+UCHAR       OUI_WPA_VERSION[4]      = {0x00, 0x50, 0xF2, 0x01};
+UCHAR       OUI_WPA_TKIP[4]     = {0x00, 0x50, 0xF2, 0x02};
+UCHAR       OUI_WPA_CCMP[4]     = {0x00, 0x50, 0xF2, 0x04};
+UCHAR       OUI_WPA_8021X_AKM[4]	= {0x00, 0x50, 0xF2, 0x01};
+UCHAR       OUI_WPA_PSK_AKM[4]      = {0x00, 0x50, 0xF2, 0x02};
+// WPA2 OUI
+UCHAR       OUI_WPA2_WEP40[4]   = {0x00, 0x0F, 0xAC, 0x01};
+UCHAR       OUI_WPA2_TKIP[4]        = {0x00, 0x0F, 0xAC, 0x02};
+UCHAR       OUI_WPA2_CCMP[4]        = {0x00, 0x0F, 0xAC, 0x04};
+UCHAR       OUI_WPA2_8021X_AKM[4]   = {0x00, 0x0F, 0xAC, 0x01};
+UCHAR       OUI_WPA2_PSK_AKM[4]   	= {0x00, 0x0F, 0xAC, 0x02};
+// MSA OUI
+UCHAR   	OUI_MSA_8021X_AKM[4]    = {0x00, 0x0F, 0xAC, 0x05};		// Not yet final - IEEE 802.11s-D1.06
+UCHAR   	OUI_MSA_PSK_AKM[4]   	= {0x00, 0x0F, 0xAC, 0x06};		// Not yet final - IEEE 802.11s-D1.06
+
+/*
+	========================================================================
+
+	Routine Description:
+		The pseudo-random function(PRF) that hashes various inputs to
+		derive a pseudo-random value. To add liveness to the pseudo-random
+		value, a nonce should be one of the inputs.
+
+		It is used to generate PTK, GTK or some specific random value.
+
+	Arguments:
+		UCHAR	*key,		-	the key material for HMAC_SHA1 use
+		INT		key_len		-	the length of key
+		UCHAR	*prefix		-	a prefix label
+		INT		prefix_len	-	the length of the label
+		UCHAR	*data		-	a specific data with variable length
+		INT		data_len	-	the length of a specific data
+		INT		len			-	the output lenght
+
+	Return Value:
+		UCHAR	*output		-	the calculated result
+
+	Note:
+		802.11i-2004	Annex H.3
+
+	========================================================================
+*/
+VOID	PRF(
+	IN	UCHAR	*key,
+	IN	INT		key_len,
+	IN	UCHAR	*prefix,
+	IN	INT		prefix_len,
+	IN	UCHAR	*data,
+	IN	INT		data_len,
+	OUT	UCHAR	*output,
+	IN	INT		len)
+{
+	INT		i;
+    UCHAR   *input;
+	INT		currentindex = 0;
+	INT		total_len;
+
+	// Allocate memory for input
+	os_alloc_mem(NULL, (PUCHAR *)&input, 1024);
+
+    if (input == NULL)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("!!!PRF: no memory!!!\n"));
+        return;
+    }
+
+	// Generate concatenation input
+	NdisMoveMemory(input, prefix, prefix_len);
+
+	// Concatenate a single octet containing 0
+	input[prefix_len] =	0;
+
+	// Concatenate specific data
+	NdisMoveMemory(&input[prefix_len + 1], data, data_len);
+	total_len =	prefix_len + 1 + data_len;
+
+	// Concatenate a single octet containing 0
+	// This octet shall be update later
+	input[total_len] = 0;
+	total_len++;
+
+	// Iterate to calculate the result by hmac-sha-1
+	// Then concatenate to last result
+	for	(i = 0;	i <	(len + 19) / 20; i++)
+	{
+		HMAC_SHA1(input, total_len,	key, key_len, &output[currentindex]);
+		currentindex +=	20;
+
+		// update the last octet
+		input[total_len - 1]++;
+	}
+    os_free_mem(NULL, input);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		It utilizes PRF-384 or PRF-512 to derive session-specific keys from a PMK.
+		It shall be called by 4-way handshake processing.
+
+	Arguments:
+		pAd 	-	pointer to our pAdapter context
+		PMK		-	pointer to PMK
+		ANonce	-	pointer to ANonce
+		AA		-	pointer to Authenticator Address
+		SNonce	-	pointer to SNonce
+		SA		-	pointer to Supplicant Address
+		len		-	indicate the length of PTK (octet)
+
+	Return Value:
+		Output		pointer to the PTK
+
+	Note:
+		Refer to IEEE 802.11i-2004 8.5.1.2
+
+	========================================================================
+*/
+VOID WpaCountPTK(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	*PMK,
+	IN	UCHAR	*ANonce,
+	IN	UCHAR	*AA,
+	IN	UCHAR	*SNonce,
+	IN	UCHAR	*SA,
+	OUT	UCHAR	*output,
+	IN	UINT	len)
+{
+	UCHAR	concatenation[76];
+	UINT	CurrPos = 0;
+	UCHAR	temp[32];
+	UCHAR	Prefix[] = {'P', 'a', 'i', 'r', 'w', 'i', 's', 'e', ' ', 'k', 'e', 'y', ' ',
+						'e', 'x', 'p', 'a', 'n', 's', 'i', 'o', 'n'};
+
+	// initiate the concatenation input
+	NdisZeroMemory(temp, sizeof(temp));
+	NdisZeroMemory(concatenation, 76);
+
+	// Get smaller address
+	if (RTMPCompareMemory(SA, AA, 6) == 1)
+		NdisMoveMemory(concatenation, AA, 6);
+	else
+		NdisMoveMemory(concatenation, SA, 6);
+	CurrPos += 6;
+
+	// Get larger address
+	if (RTMPCompareMemory(SA, AA, 6) == 1)
+		NdisMoveMemory(&concatenation[CurrPos], SA, 6);
+	else
+		NdisMoveMemory(&concatenation[CurrPos], AA, 6);
+
+	// store the larger mac address for backward compatible of
+	// ralink proprietary STA-key issue
+	NdisMoveMemory(temp, &concatenation[CurrPos], MAC_ADDR_LEN);
+	CurrPos += 6;
+
+	// Get smaller Nonce
+	if (RTMPCompareMemory(ANonce, SNonce, 32) == 0)
+		NdisMoveMemory(&concatenation[CurrPos], temp, 32);	// patch for ralink proprietary STA-key issue
+	else if (RTMPCompareMemory(ANonce, SNonce, 32) == 1)
+		NdisMoveMemory(&concatenation[CurrPos], SNonce, 32);
+	else
+		NdisMoveMemory(&concatenation[CurrPos], ANonce, 32);
+	CurrPos += 32;
+
+	// Get larger Nonce
+	if (RTMPCompareMemory(ANonce, SNonce, 32) == 0)
+		NdisMoveMemory(&concatenation[CurrPos], temp, 32);	// patch for ralink proprietary STA-key issue
+	else if (RTMPCompareMemory(ANonce, SNonce, 32) == 1)
+		NdisMoveMemory(&concatenation[CurrPos], ANonce, 32);
+	else
+		NdisMoveMemory(&concatenation[CurrPos], SNonce, 32);
+	CurrPos += 32;
+
+	hex_dump("concatenation=", concatenation, 76);
+
+	// Use PRF to generate PTK
+	PRF(PMK, LEN_MASTER_KEY, Prefix, 22, concatenation, 76, output, len);
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Generate random number by software.
+
+	Arguments:
+		pAd		-	pointer to our pAdapter context
+		macAddr	-	pointer to local MAC address
+
+	Return Value:
+
+	Note:
+		802.1ii-2004  Annex H.5
+
+	========================================================================
+*/
+VOID	GenRandom(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			*macAddr,
+	OUT	UCHAR			*random)
+{
+	INT		i, curr;
+	UCHAR	local[80], KeyCounter[32];
+	UCHAR	result[80];
+	ULONG	CurrentTime;
+	UCHAR	prefix[] = {'I', 'n', 'i', 't', ' ', 'C', 'o', 'u', 'n', 't', 'e', 'r'};
+
+	// Zero the related information
+	NdisZeroMemory(result, 80);
+	NdisZeroMemory(local, 80);
+	NdisZeroMemory(KeyCounter, 32);
+
+	for	(i = 0;	i <	32;	i++)
+	{
+		// copy the local MAC address
+		COPY_MAC_ADDR(local, macAddr);
+		curr =	MAC_ADDR_LEN;
+
+		// concatenate the current time
+		NdisGetSystemUpTime(&CurrentTime);
+		NdisMoveMemory(&local[curr],  &CurrentTime,	sizeof(CurrentTime));
+		curr +=	sizeof(CurrentTime);
+
+		// concatenate the last result
+		NdisMoveMemory(&local[curr],  result, 32);
+		curr +=	32;
+
+		// concatenate a variable
+		NdisMoveMemory(&local[curr],  &i,  2);
+		curr +=	2;
+
+		// calculate the result
+		PRF(KeyCounter, 32, prefix,12, local, curr, result, 32);
+	}
+
+	NdisMoveMemory(random, result,	32);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build cipher suite in RSN-IE.
+		It only shall be called by RTMPMakeRSNIE.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	ElementID	-	indicate the WPA1 or WPA2
+    	WepStatus	-	indicate the encryption type
+		bMixCipher	-	a boolean to indicate the pairwise cipher and group
+						cipher are the same or not
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+static VOID RTMPInsertRsnIeCipher(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			ElementID,
+	IN	UINT			WepStatus,
+	IN	BOOLEAN			bMixCipher,
+	IN	UCHAR			FlexibleCipher,
+	OUT	PUCHAR			pRsnIe,
+	OUT	UCHAR			*rsn_len)
+{
+	UCHAR	PairwiseCnt;
+
+	*rsn_len = 0;
+
+	// decide WPA2 or WPA1
+	if (ElementID == Wpa2Ie)
+	{
+		RSNIE2	*pRsnie_cipher = (RSNIE2*)pRsnIe;
+
+		// Assign the verson as 1
+		pRsnie_cipher->version = 1;
+
+        switch (WepStatus)
+        {
+        	// TKIP mode
+            case Ndis802_11Encryption2Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_TKIP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_TKIP, 4);
+                *rsn_len = sizeof(RSNIE2);
+                break;
+
+			// AES mode
+            case Ndis802_11Encryption3Enabled:
+				if (bMixCipher)
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_TKIP, 4);
+				else
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_CCMP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_CCMP, 4);
+                *rsn_len = sizeof(RSNIE2);
+                break;
+
+			// TKIP-AES mix mode
+            case Ndis802_11Encryption4Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_TKIP, 4);
+
+				PairwiseCnt = 1;
+				// Insert WPA2 TKIP as the first pairwise cipher
+				if (MIX_CIPHER_WPA2_TKIP_ON(FlexibleCipher))
+				{
+                	NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_TKIP, 4);
+					// Insert WPA2 AES as the secondary pairwise cipher
+					if (MIX_CIPHER_WPA2_AES_ON(FlexibleCipher))
+					{
+                		NdisMoveMemory(pRsnie_cipher->ucast[0].oui + 4, OUI_WPA2_CCMP, 4);
+						PairwiseCnt = 2;
+					}
+				}
+				else
+				{
+					// Insert WPA2 AES as the first pairwise cipher
+					NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_CCMP, 4);
+				}
+
+                pRsnie_cipher->ucount = PairwiseCnt;
+                *rsn_len = sizeof(RSNIE2) + (4 * (PairwiseCnt - 1));
+                break;
+        }
+
+		// swap for big-endian platform
+		pRsnie_cipher->version = cpu2le16(pRsnie_cipher->version);
+	    pRsnie_cipher->ucount = cpu2le16(pRsnie_cipher->ucount);
+	}
+	else
+	{
+		RSNIE	*pRsnie_cipher = (RSNIE*)pRsnIe;
+
+		// Assign OUI and version
+		NdisMoveMemory(pRsnie_cipher->oui, OUI_WPA_VERSION, 4);
+        pRsnie_cipher->version = 1;
+
+		switch (WepStatus)
+		{
+			// TKIP mode
+            case Ndis802_11Encryption2Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_TKIP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_TKIP, 4);
+                *rsn_len = sizeof(RSNIE);
+                break;
+
+			// AES mode
+            case Ndis802_11Encryption3Enabled:
+				if (bMixCipher)
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_TKIP, 4);
+				else
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_CCMP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_CCMP, 4);
+                *rsn_len = sizeof(RSNIE);
+                break;
+
+			// TKIP-AES mix mode
+            case Ndis802_11Encryption4Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_TKIP, 4);
+
+				PairwiseCnt = 1;
+				// Insert WPA TKIP as the first pairwise cipher
+				if (MIX_CIPHER_WPA_TKIP_ON(FlexibleCipher))
+				{
+                	NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_TKIP, 4);
+					// Insert WPA AES as the secondary pairwise cipher
+					if (MIX_CIPHER_WPA_AES_ON(FlexibleCipher))
+					{
+                		NdisMoveMemory(pRsnie_cipher->ucast[0].oui + 4, OUI_WPA_CCMP, 4);
+						PairwiseCnt = 2;
+					}
+				}
+				else
+				{
+					// Insert WPA AES as the first pairwise cipher
+					NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_CCMP, 4);
+				}
+
+                pRsnie_cipher->ucount = PairwiseCnt;
+                *rsn_len = sizeof(RSNIE) + (4 * (PairwiseCnt - 1));
+                break;
+        }
+
+		// swap for big-endian platform
+		pRsnie_cipher->version = cpu2le16(pRsnie_cipher->version);
+	    pRsnie_cipher->ucount = cpu2le16(pRsnie_cipher->ucount);
+	}
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build AKM suite in RSN-IE.
+		It only shall be called by RTMPMakeRSNIE.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	ElementID	-	indicate the WPA1 or WPA2
+    	AuthMode	-	indicate the authentication mode
+		apidx		-	indicate the interface index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+static VOID RTMPInsertRsnIeAKM(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			ElementID,
+	IN	UINT			AuthMode,
+	IN	UCHAR			apidx,
+	OUT	PUCHAR			pRsnIe,
+	OUT	UCHAR			*rsn_len)
+{
+	RSNIE_AUTH		*pRsnie_auth;
+
+	pRsnie_auth = (RSNIE_AUTH*)(pRsnIe + (*rsn_len));
+
+	// decide WPA2 or WPA1
+	if (ElementID == Wpa2Ie)
+	{
+		switch (AuthMode)
+        {
+            case Ndis802_11AuthModeWPA2:
+            case Ndis802_11AuthModeWPA1WPA2:
+                pRsnie_auth->acount = 1;
+                	NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA2_8021X_AKM, 4);
+                break;
+
+            case Ndis802_11AuthModeWPA2PSK:
+            case Ndis802_11AuthModeWPA1PSKWPA2PSK:
+                pRsnie_auth->acount = 1;
+                	NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA2_PSK_AKM, 4);
+                break;
+        }
+	}
+	else
+	{
+		switch (AuthMode)
+        {
+            case Ndis802_11AuthModeWPA:
+            case Ndis802_11AuthModeWPA1WPA2:
+                pRsnie_auth->acount = 1;
+                NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA_8021X_AKM, 4);
+                break;
+
+            case Ndis802_11AuthModeWPAPSK:
+            case Ndis802_11AuthModeWPA1PSKWPA2PSK:
+                pRsnie_auth->acount = 1;
+                NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA_PSK_AKM, 4);
+                break;
+
+			case Ndis802_11AuthModeWPANone:
+                pRsnie_auth->acount = 1;
+                NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA_NONE_AKM, 4);
+                break;
+        }
+	}
+
+	pRsnie_auth->acount = cpu2le16(pRsnie_auth->acount);
+
+	(*rsn_len) += sizeof(RSNIE_AUTH);	// update current RSNIE length
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build capability in RSN-IE.
+		It only shall be called by RTMPMakeRSNIE.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	ElementID	-	indicate the WPA1 or WPA2
+		apidx		-	indicate the interface index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+static VOID RTMPInsertRsnIeCap(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			ElementID,
+	IN	UCHAR			apidx,
+	OUT	PUCHAR			pRsnIe,
+	OUT	UCHAR			*rsn_len)
+{
+	RSN_CAPABILITIES    *pRSN_Cap;
+
+	// it could be ignored in WPA1 mode
+	if (ElementID == WpaIe)
+		return;
+
+	pRSN_Cap = (RSN_CAPABILITIES*)(pRsnIe + (*rsn_len));
+
+
+	pRSN_Cap->word = cpu2le16(pRSN_Cap->word);
+
+	(*rsn_len) += sizeof(RSN_CAPABILITIES);	// update current RSNIE length
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build RSN IE context. It is not included element-ID and length.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	AuthMode	-	indicate the authentication mode
+    	WepStatus	-	indicate the encryption type
+		apidx		-	indicate the interface index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPMakeRSNIE(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  UINT            AuthMode,
+    IN  UINT            WepStatus,
+	IN	UCHAR			apidx)
+{
+	PUCHAR		pRsnIe = NULL;			// primary RSNIE
+	UCHAR 		*rsnielen_cur_p = 0;	// the length of the primary RSNIE
+	UCHAR		*rsnielen_ex_cur_p = 0;	// the length of the secondary RSNIE
+	UCHAR		PrimaryRsnie;
+	BOOLEAN		bMixCipher = FALSE;	// indicate the pairwise and group cipher are different
+	UCHAR		p_offset;
+	WPA_MIX_PAIR_CIPHER		FlexibleCipher = MIX_CIPHER_NOTUSE;	// it provide the more flexible cipher combination in WPA-WPA2 and TKIPAES mode
+
+	rsnielen_cur_p = NULL;
+	rsnielen_ex_cur_p = NULL;
+
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef WPA_SUPPLICANT_SUPPORT
+			if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+			{
+				if (AuthMode < Ndis802_11AuthModeWPA)
+					return;
+			}
+			else
+#endif // WPA_SUPPLICANT_SUPPORT //
+			{
+				// Support WPAPSK or WPA2PSK in STA-Infra mode
+				// Support WPANone in STA-Adhoc mode
+				if ((AuthMode != Ndis802_11AuthModeWPAPSK) &&
+					(AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+					(AuthMode != Ndis802_11AuthModeWPANone)
+					)
+					return;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,("==> RTMPMakeRSNIE(STA)\n"));
+
+			// Zero RSNIE context
+			pAd->StaCfg.RSNIE_Len = 0;
+			NdisZeroMemory(pAd->StaCfg.RSN_IE, MAX_LEN_OF_RSNIE);
+
+			// Pointer to RSNIE
+			rsnielen_cur_p = &pAd->StaCfg.RSNIE_Len;
+			pRsnIe = pAd->StaCfg.RSN_IE;
+
+			bMixCipher = pAd->StaCfg.bMixCipher;
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// indicate primary RSNIE as WPA or WPA2
+	if ((AuthMode == Ndis802_11AuthModeWPA) ||
+		(AuthMode == Ndis802_11AuthModeWPAPSK) ||
+		(AuthMode == Ndis802_11AuthModeWPANone) ||
+		(AuthMode == Ndis802_11AuthModeWPA1WPA2) ||
+		(AuthMode == Ndis802_11AuthModeWPA1PSKWPA2PSK))
+		PrimaryRsnie = WpaIe;
+	else
+		PrimaryRsnie = Wpa2Ie;
+
+	{
+		// Build the primary RSNIE
+		// 1. insert cipher suite
+		RTMPInsertRsnIeCipher(pAd, PrimaryRsnie, WepStatus, bMixCipher, FlexibleCipher, pRsnIe, &p_offset);
+
+		// 2. insert AKM
+		RTMPInsertRsnIeAKM(pAd, PrimaryRsnie, AuthMode, apidx, pRsnIe, &p_offset);
+
+		// 3. insert capability
+		RTMPInsertRsnIeCap(pAd, PrimaryRsnie, apidx, pRsnIe, &p_offset);
+	}
+
+	// 4. update the RSNIE length
+	*rsnielen_cur_p = p_offset;
+
+	hex_dump("The primary RSNIE", pRsnIe, (*rsnielen_cur_p));
+
+
+}
+
+/*
+    ==========================================================================
+    Description:
+		Check whether the received frame is EAP frame.
+
+	Arguments:
+		pAd				-	pointer to our pAdapter context
+		pEntry			-	pointer to active entry
+		pData			-	the received frame
+		DataByteCount 	-	the received frame's length
+		FromWhichBSSID	-	indicate the interface index
+
+    Return:
+         TRUE 			-	This frame is EAP frame
+         FALSE 			-	otherwise
+    ==========================================================================
+*/
+BOOLEAN RTMPCheckWPAframe(
+    IN PRTMP_ADAPTER    pAd,
+    IN PMAC_TABLE_ENTRY	pEntry,
+    IN PUCHAR           pData,
+    IN ULONG            DataByteCount,
+	IN UCHAR			FromWhichBSSID)
+{
+	ULONG	Body_len;
+	BOOLEAN Cancelled;
+
+
+    if(DataByteCount < (LENGTH_802_1_H + LENGTH_EAPOL_H))
+        return FALSE;
+
+
+	// Skip LLC header
+    if (NdisEqualMemory(SNAP_802_1H, pData, 6) ||
+        // Cisco 1200 AP may send packet with SNAP_BRIDGE_TUNNEL
+        NdisEqualMemory(SNAP_BRIDGE_TUNNEL, pData, 6))
+    {
+        pData += 6;
+    }
+	// Skip 2-bytes EAPoL type
+    if (NdisEqualMemory(EAPOL, pData, 2))
+    {
+        pData += 2;
+    }
+    else
+        return FALSE;
+
+    switch (*(pData+1))
+    {
+        case EAPPacket:
+			Body_len = (*(pData+2)<<8) | (*(pData+3));
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAP-Packet frame, TYPE = 0, Length = %ld\n", Body_len));
+            break;
+        case EAPOLStart:
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL-Start frame, TYPE = 1 \n"));
+			if (pEntry->EnqueueEapolStartTimerRunning != EAPOL_START_DISABLE)
+            {
+            	DBGPRINT(RT_DEBUG_TRACE, ("Cancel the EnqueueEapolStartTimerRunning \n"));
+                RTMPCancelTimer(&pEntry->EnqueueStartForPSKTimer, &Cancelled);
+                pEntry->EnqueueEapolStartTimerRunning = EAPOL_START_DISABLE;
+            }
+            break;
+        case EAPOLLogoff:
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOLLogoff frame, TYPE = 2 \n"));
+            break;
+        case EAPOLKey:
+			Body_len = (*(pData+2)<<8) | (*(pData+3));
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL-Key frame, TYPE = 3, Length = %ld\n", Body_len));
+            break;
+        case EAPOLASFAlert:
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOLASFAlert frame, TYPE = 4 \n"));
+            break;
+        default:
+            return FALSE;
+
+    }
+    return TRUE;
+}
+
+
+/*
+    ==========================================================================
+    Description:
+        ENCRYPT AES GTK before sending in EAPOL frame.
+        AES GTK length = 128 bit,  so fix blocks for aes-key-wrap as 2 in this function.
+        This function references to RFC 3394 for aes key wrap algorithm.
+    Return:
+    ==========================================================================
+*/
+VOID AES_GTK_KEY_WRAP(
+    IN UCHAR    *key,
+    IN UCHAR    *plaintext,
+    IN UCHAR    p_len,
+    OUT UCHAR   *ciphertext)
+{
+    UCHAR       A[8], BIN[16], BOUT[16];
+    UCHAR       R[512];
+    INT         num_blocks = p_len/8;   // unit:64bits
+    INT         i, j;
+    aes_context aesctx;
+    UCHAR       xor;
+
+    rtmp_aes_set_key(&aesctx, key, 128);
+
+    // Init IA
+    for (i = 0; i < 8; i++)
+        A[i] = 0xa6;
+
+    //Input plaintext
+    for (i = 0; i < num_blocks; i++)
+    {
+        for (j = 0 ; j < 8; j++)
+            R[8 * (i + 1) + j] = plaintext[8 * i + j];
+    }
+
+    // Key Mix
+    for (j = 0; j < 6; j++)
+    {
+        for(i = 1; i <= num_blocks; i++)
+        {
+            //phase 1
+            NdisMoveMemory(BIN, A, 8);
+            NdisMoveMemory(&BIN[8], &R[8 * i], 8);
+            rtmp_aes_encrypt(&aesctx, BIN, BOUT);
+
+            NdisMoveMemory(A, &BOUT[0], 8);
+            xor = num_blocks * j + i;
+            A[7] = BOUT[7] ^ xor;
+            NdisMoveMemory(&R[8 * i], &BOUT[8], 8);
+        }
+    }
+
+    // Output ciphertext
+    NdisMoveMemory(ciphertext, A, 8);
+
+    for (i = 1; i <= num_blocks; i++)
+    {
+        for (j = 0 ; j < 8; j++)
+            ciphertext[8 * i + j] = R[8 * i + j];
+    }
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Misc function to decrypt AES body
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+		This function references to	RFC	3394 for aes key unwrap algorithm.
+
+	========================================================================
+*/
+VOID	AES_GTK_KEY_UNWRAP(
+	IN	UCHAR	*key,
+	OUT	UCHAR	*plaintext,
+	IN	UCHAR    c_len,
+	IN	UCHAR	*ciphertext)
+
+{
+	UCHAR       A[8], BIN[16], BOUT[16];
+	UCHAR       xor;
+	INT         i, j;
+	aes_context aesctx;
+	UCHAR       *R;
+	INT         num_blocks = c_len/8;	// unit:64bits
+
+
+	os_alloc_mem(NULL, (PUCHAR *)&R, 512);
+
+	if (R == NULL)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("!!!AES_GTK_KEY_UNWRAP: no memory!!!\n"));
+        return;
+    } /* End of if */
+
+	// Initialize
+	NdisMoveMemory(A, ciphertext, 8);
+	//Input plaintext
+	for(i = 0; i < (c_len-8); i++)
+	{
+		R[ i] = ciphertext[i + 8];
+	}
+
+	rtmp_aes_set_key(&aesctx, key, 128);
+
+	for(j = 5; j >= 0; j--)
+	{
+		for(i = (num_blocks-1); i > 0; i--)
+		{
+			xor = (num_blocks -1 )* j + i;
+			NdisMoveMemory(BIN, A, 8);
+			BIN[7] = A[7] ^ xor;
+			NdisMoveMemory(&BIN[8], &R[(i-1)*8], 8);
+			rtmp_aes_decrypt(&aesctx, BIN, BOUT);
+			NdisMoveMemory(A, &BOUT[0], 8);
+			NdisMoveMemory(&R[(i-1)*8], &BOUT[8], 8);
+		}
+	}
+
+	// OUTPUT
+	for(i = 0; i < c_len; i++)
+	{
+		plaintext[i] = R[i];
+	}
+
+
+	os_free_mem(NULL, R);
+}
+
+/*
+    ==========================================================================
+    Description:
+		Report the EAP message type
+
+	Arguments:
+		msg		-	EAPOL_PAIR_MSG_1
+					EAPOL_PAIR_MSG_2
+					EAPOL_PAIR_MSG_3
+					EAPOL_PAIR_MSG_4
+					EAPOL_GROUP_MSG_1
+					EAPOL_GROUP_MSG_2
+
+    Return:
+         message type string
+
+    ==========================================================================
+*/
+CHAR *GetEapolMsgType(CHAR msg)
+{
+    if(msg == EAPOL_PAIR_MSG_1)
+        return "Pairwise Message 1";
+    else if(msg == EAPOL_PAIR_MSG_2)
+        return "Pairwise Message 2";
+	else if(msg == EAPOL_PAIR_MSG_3)
+        return "Pairwise Message 3";
+	else if(msg == EAPOL_PAIR_MSG_4)
+        return "Pairwise Message 4";
+	else if(msg == EAPOL_GROUP_MSG_1)
+        return "Group Message 1";
+	else if(msg == EAPOL_GROUP_MSG_2)
+        return "Group Message 2";
+    else
+    	return "Invalid Message";
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Check Sanity RSN IE of EAPoL message
+
+    Arguments:
+
+    Return Value:
+
+
+    ========================================================================
+*/
+BOOLEAN RTMPCheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	OUT	UCHAR			*Offset)
+{
+	PUCHAR              pVIE;
+	UCHAR               len;
+	PEID_STRUCT         pEid;
+	BOOLEAN				result = FALSE;
+
+	pVIE = pData;
+	len	 = DataLen;
+	*Offset = 0;
+
+	while (len > sizeof(RSNIE2))
+	{
+		pEid = (PEID_STRUCT) pVIE;
+		// WPA RSN IE
+		if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4)))
+		{
+			if ((pEntry->AuthMode == Ndis802_11AuthModeWPA || pEntry->AuthMode == Ndis802_11AuthModeWPAPSK) &&
+				(NdisEqualMemory(pVIE, pEntry->RSN_IE, pEntry->RSNIE_Len)) &&
+				(pEntry->RSNIE_Len == (pEid->Len + 2)))
+			{
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		// WPA2 RSN IE
+		else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3)))
+		{
+			if ((pEntry->AuthMode == Ndis802_11AuthModeWPA2 || pEntry->AuthMode == Ndis802_11AuthModeWPA2PSK) &&
+				(NdisEqualMemory(pVIE, pEntry->RSN_IE, pEntry->RSNIE_Len)) &&
+				(pEntry->RSNIE_Len == (pEid->Len + 2))/* ToDo-AlbertY for mesh*/)
+			{
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		else
+		{
+			break;
+		}
+
+		pVIE += (pEid->Len + 2);
+		len  -= (pEid->Len + 2);
+	}
+
+
+	return result;
+
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Parse KEYDATA field.  KEYDATA[] May contain 2 RSN IE and optionally GTK.
+    GTK  is encaptulated in KDE format at  p.83 802.11i D10
+
+    Arguments:
+
+    Return Value:
+
+    Note:
+        802.11i D10
+
+    ========================================================================
+*/
+BOOLEAN RTMPParseEapolKeyData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKeyData,
+	IN  UCHAR           KeyDataLen,
+	IN	UCHAR			GroupKeyIndex,
+	IN	UCHAR			MsgType,
+	IN	BOOLEAN			bWPA2,
+	IN  MAC_TABLE_ENTRY *pEntry)
+{
+    PKDE_ENCAP          pKDE = NULL;
+    PUCHAR              pMyKeyData = pKeyData;
+    UCHAR               KeyDataLength = KeyDataLen;
+    UCHAR               GTKLEN = 0;
+	UCHAR				DefaultIdx = 0;
+	UCHAR				skip_offset;
+
+	// Verify The RSN IE contained in pairewise_msg_2 && pairewise_msg_3 and skip it
+	if (MsgType == EAPOL_PAIR_MSG_2 || MsgType == EAPOL_PAIR_MSG_3)
+    {
+		// Check RSN IE whether it is WPA2/WPA2PSK
+		if (!RTMPCheckRSNIE(pAd, pKeyData, KeyDataLen, pEntry, &skip_offset))
+		{
+			// send wireless event - for RSN IE different
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_RSNIE_DIFF_EVENT_FLAG, pEntry->Addr, pEntry->apidx, 0);
+
+        	DBGPRINT(RT_DEBUG_ERROR, ("RSN_IE Different in msg %d of 4-way handshake!\n", MsgType));
+			hex_dump("Receive RSN_IE ", pKeyData, KeyDataLen);
+			hex_dump("Desired RSN_IE ", pEntry->RSN_IE, pEntry->RSNIE_Len);
+
+			return FALSE;
+    	}
+    	else
+		{
+			if (bWPA2 && MsgType == EAPOL_PAIR_MSG_3)
+			{
+				// skip RSN IE
+				pMyKeyData += skip_offset;
+				KeyDataLength -= skip_offset;
+				DBGPRINT(RT_DEBUG_TRACE, ("RTMPParseEapolKeyData ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", skip_offset));
+			}
+			else
+				return TRUE;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPParseEapolKeyData ==> KeyDataLength %d without RSN_IE \n", KeyDataLength));
+
+	// Parse EKD format in pairwise_msg_3_WPA2 && group_msg_1_WPA2
+	if (bWPA2 && (MsgType == EAPOL_PAIR_MSG_3 || MsgType == EAPOL_GROUP_MSG_1))
+	{
+		if (KeyDataLength >= 8)	// KDE format exclude GTK length
+    	{
+        	pKDE = (PKDE_ENCAP) pMyKeyData;
+
+
+			DefaultIdx = pKDE->GTKEncap.Kid;
+
+			// Sanity check - KED length
+			if (KeyDataLength < (pKDE->Len + 2))
+    		{
+        		DBGPRINT(RT_DEBUG_ERROR, ("ERROR: The len from KDE is too short \n"));
+        		return FALSE;
+    		}
+
+			// Get GTK length - refer to IEEE 802.11i-2004 p.82
+			GTKLEN = pKDE->Len -6;
+			if (GTKLEN < LEN_AES_KEY)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key length is too short (%d) \n", GTKLEN));
+        		return FALSE;
+			}
+
+    	}
+		else
+    	{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR: KDE format length is too short \n"));
+	        return FALSE;
+    	}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("GTK in KDE format ,DefaultKeyID=%d, KeyLen=%d \n", DefaultIdx, GTKLEN));
+		// skip it
+		pMyKeyData += 8;
+		KeyDataLength -= 8;
+
+	}
+	else if (!bWPA2 && MsgType == EAPOL_GROUP_MSG_1)
+	{
+		DefaultIdx = GroupKeyIndex;
+		DBGPRINT(RT_DEBUG_TRACE, ("GTK DefaultKeyID=%d \n", DefaultIdx));
+	}
+
+	// Sanity check - shared key index must be 1 ~ 3
+	if (DefaultIdx < 1 || DefaultIdx > 3)
+    {
+     	DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key index(%d) is invalid in %s %s \n", DefaultIdx, ((bWPA2) ? "WPA2" : "WPA"), GetEapolMsgType(MsgType)));
+        return FALSE;
+    }
+
+
+#ifdef CONFIG_STA_SUPPORT
+	// Todo
+#endif // CONFIG_STA_SUPPORT //
+
+	return TRUE;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Construct EAPoL message for WPA handshaking
+		Its format is below,
+
+		+--------------------+
+		| Protocol Version	 |  1 octet
+		+--------------------+
+		| Protocol Type		 |	1 octet
+		+--------------------+
+		| Body Length		 |  2 octets
+		+--------------------+
+		| Descriptor Type	 |	1 octet
+		+--------------------+
+		| Key Information    |	2 octets
+		+--------------------+
+		| Key Length	     |  1 octet
+		+--------------------+
+		| Key Repaly Counter |	8 octets
+		+--------------------+
+		| Key Nonce		     |  32 octets
+		+--------------------+
+		| Key IV			 |  16 octets
+		+--------------------+
+		| Key RSC			 |  8 octets
+		+--------------------+
+		| Key ID or Reserved |	8 octets
+		+--------------------+
+		| Key MIC			 |	16 octets
+		+--------------------+
+		| Key Data Length	 |	2 octets
+		+--------------------+
+		| Key Data			 |	n octets
+		+--------------------+
+
+
+	Arguments:
+		pAd			Pointer	to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ConstructEapolMsg(
+	IN 	PRTMP_ADAPTER    	pAd,
+    IN 	UCHAR				AuthMode,
+    IN 	UCHAR				WepStatus,
+    IN 	UCHAR				GroupKeyWepStatus,
+    IN 	UCHAR				MsgType,
+    IN	UCHAR				DefaultKeyIdx,
+    IN 	UCHAR				*ReplayCounter,
+	IN 	UCHAR				*KeyNonce,
+	IN	UCHAR				*TxRSC,
+	IN	UCHAR				*PTK,
+	IN	UCHAR				*GTK,
+	IN	UCHAR				*RSNIE,
+	IN	UCHAR				RSNIE_Len,
+    OUT PEAPOL_PACKET       pMsg)
+{
+	BOOLEAN	bWPA2 = FALSE;
+
+	// Choose WPA2 or not
+	if ((AuthMode == Ndis802_11AuthModeWPA2) || (AuthMode == Ndis802_11AuthModeWPA2PSK))
+		bWPA2 = TRUE;
+
+    // Init Packet and Fill header
+    pMsg->ProVer = EAPOL_VER;
+    pMsg->ProType = EAPOLKey;
+
+	// Default 95 bytes, the EAPoL-Key descriptor exclude Key-data field
+	pMsg->Body_Len[1] = LEN_EAPOL_KEY_MSG;
+
+	// Fill in EAPoL descriptor
+	if (bWPA2)
+		pMsg->KeyDesc.Type = WPA2_KEY_DESC;
+	else
+		pMsg->KeyDesc.Type = WPA1_KEY_DESC;
+
+	// Fill in Key information, refer to IEEE Std 802.11i-2004 page 78
+	// When either the pairwise or the group cipher is AES, the DESC_TYPE_AES(2) shall be used.
+	pMsg->KeyDesc.KeyInfo.KeyDescVer =
+        	(((WepStatus == Ndis802_11Encryption3Enabled) || (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)) ? (DESC_TYPE_AES) : (DESC_TYPE_TKIP));
+
+	// Specify Key Type as Group(0) or Pairwise(1)
+	if (MsgType >= EAPOL_GROUP_MSG_1)
+		pMsg->KeyDesc.KeyInfo.KeyType = GROUPKEY;
+	else
+		pMsg->KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// Specify Key Index, only group_msg1_WPA1
+	if (!bWPA2 && (MsgType >= EAPOL_GROUP_MSG_1))
+		pMsg->KeyDesc.KeyInfo.KeyIndex = DefaultKeyIdx;
+
+	if (MsgType == EAPOL_PAIR_MSG_3)
+		pMsg->KeyDesc.KeyInfo.Install = 1;
+
+	if ((MsgType == EAPOL_PAIR_MSG_1) || (MsgType == EAPOL_PAIR_MSG_3) || (MsgType == EAPOL_GROUP_MSG_1))
+		pMsg->KeyDesc.KeyInfo.KeyAck = 1;
+
+	if (MsgType != EAPOL_PAIR_MSG_1)
+		pMsg->KeyDesc.KeyInfo.KeyMic = 1;
+
+	if ((bWPA2 && (MsgType >= EAPOL_PAIR_MSG_3)) || (!bWPA2 && (MsgType >= EAPOL_GROUP_MSG_1)))
+    {
+       	pMsg->KeyDesc.KeyInfo.Secure = 1;
+    }
+
+	if (bWPA2 && ((MsgType == EAPOL_PAIR_MSG_3) || (MsgType == EAPOL_GROUP_MSG_1)))
+    {
+        pMsg->KeyDesc.KeyInfo.EKD_DL = 1;
+    }
+
+	// key Information element has done.
+	*(USHORT *)(&pMsg->KeyDesc.KeyInfo) = cpu2le16(*(USHORT *)(&pMsg->KeyDesc.KeyInfo));
+
+	// Fill in Key Length
+	{
+		if (MsgType >= EAPOL_GROUP_MSG_1)
+		{
+			// the length of group key cipher
+			pMsg->KeyDesc.KeyLength[1] = ((GroupKeyWepStatus == Ndis802_11Encryption2Enabled) ? TKIP_GTK_LENGTH : LEN_AES_KEY);
+		}
+		else
+		{
+			// the length of pairwise key cipher
+			pMsg->KeyDesc.KeyLength[1] = ((WepStatus == Ndis802_11Encryption2Enabled) ? LEN_TKIP_KEY : LEN_AES_KEY);
+		}
+	}
+
+ 	// Fill in replay counter
+    NdisMoveMemory(pMsg->KeyDesc.ReplayCounter, ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Fill Key Nonce field
+	// ANonce : pairwise_msg1 & pairwise_msg3
+	// SNonce : pairwise_msg2
+	// GNonce : group_msg1_wpa1
+	if ((MsgType <= EAPOL_PAIR_MSG_3) || ((!bWPA2 && (MsgType == EAPOL_GROUP_MSG_1))))
+    	NdisMoveMemory(pMsg->KeyDesc.KeyNonce, KeyNonce, LEN_KEY_DESC_NONCE);
+
+	// Fill key IV - WPA2 as 0, WPA1 as random
+	if (!bWPA2 && (MsgType == EAPOL_GROUP_MSG_1))
+	{
+		// Suggest IV be random number plus some number,
+		NdisMoveMemory(pMsg->KeyDesc.KeyIv, &KeyNonce[16], LEN_KEY_DESC_IV);
+        pMsg->KeyDesc.KeyIv[15] += 2;
+	}
+
+    // Fill Key RSC field
+    // It contains the RSC for the GTK being installed.
+	if ((MsgType == EAPOL_PAIR_MSG_3 && bWPA2) || (MsgType == EAPOL_GROUP_MSG_1))
+	{
+        NdisMoveMemory(pMsg->KeyDesc.KeyRsc, TxRSC, 6);
+	}
+
+	// Clear Key MIC field for MIC calculation later
+    NdisZeroMemory(pMsg->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+
+	ConstructEapolKeyData(pAd,
+						  AuthMode,
+						  WepStatus,
+						  GroupKeyWepStatus,
+						  MsgType,
+						  DefaultKeyIdx,
+						  bWPA2,
+						  PTK,
+						  GTK,
+						  RSNIE,
+						  RSNIE_Len,
+						  pMsg);
+
+	// Calculate MIC and fill in KeyMic Field except Pairwise Msg 1.
+	if (MsgType != EAPOL_PAIR_MSG_1)
+	{
+		CalculateMIC(pAd, WepStatus, PTK, pMsg);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> ConstructEapolMsg for %s %s\n", ((bWPA2) ? "WPA2" : "WPA"), GetEapolMsgType(MsgType)));
+	DBGPRINT(RT_DEBUG_TRACE, ("	     Body length = %d \n", pMsg->Body_Len[1]));
+	DBGPRINT(RT_DEBUG_TRACE, ("	     Key length  = %d \n", pMsg->KeyDesc.KeyLength[1]));
+
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Construct the Key Data field of EAPoL message
+
+	Arguments:
+		pAd			Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ConstructEapolKeyData(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			AuthMode,
+	IN	UCHAR			WepStatus,
+	IN	UCHAR			GroupKeyWepStatus,
+	IN 	UCHAR			MsgType,
+	IN	UCHAR			DefaultKeyIdx,
+	IN	BOOLEAN			bWPA2Capable,
+	IN	UCHAR			*PTK,
+	IN	UCHAR			*GTK,
+	IN	UCHAR			*RSNIE,
+	IN	UCHAR			RSNIE_LEN,
+	OUT PEAPOL_PACKET   pMsg)
+{
+	UCHAR		*mpool, *Key_Data, *Rc4GTK;
+	UCHAR       ekey[(LEN_KEY_DESC_IV+LEN_EAP_EK)];
+	UCHAR		data_offset;
+
+
+	if (MsgType == EAPOL_PAIR_MSG_1 || MsgType == EAPOL_PAIR_MSG_4 || MsgType == EAPOL_GROUP_MSG_2)
+		return;
+
+	// allocate memory pool
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 1500);
+
+    if (mpool == NULL)
+		return;
+
+	/* Rc4GTK Len = 512 */
+	Rc4GTK = (UCHAR *) ROUND_UP(mpool, 4);
+	/* Key_Data Len = 512 */
+	Key_Data = (UCHAR *) ROUND_UP(Rc4GTK + 512, 4);
+
+	NdisZeroMemory(Key_Data, 512);
+	pMsg->KeyDesc.KeyDataLen[1] = 0;
+	data_offset = 0;
+
+	// Encapsulate RSNIE in pairwise_msg2 & pairwise_msg3
+	if (RSNIE_LEN && ((MsgType == EAPOL_PAIR_MSG_2) || (MsgType == EAPOL_PAIR_MSG_3)))
+	{
+		if (bWPA2Capable)
+			Key_Data[data_offset + 0] = IE_WPA2;
+		else
+			Key_Data[data_offset + 0] = IE_WPA;
+
+        Key_Data[data_offset + 1] = RSNIE_LEN;
+		NdisMoveMemory(&Key_Data[data_offset + 2], RSNIE, RSNIE_LEN);
+		data_offset += (2 + RSNIE_LEN);
+	}
+
+	// Encapsulate KDE format in pairwise_msg3_WPA2 & group_msg1_WPA2
+	if (bWPA2Capable && ((MsgType == EAPOL_PAIR_MSG_3) || (MsgType == EAPOL_GROUP_MSG_1)))
+	{
+		// Key Data Encapsulation (KDE) format - 802.11i-2004  Figure-43w and Table-20h
+        Key_Data[data_offset + 0] = 0xDD;
+
+		if (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)
+		{
+			Key_Data[data_offset + 1] = 0x16;// 4+2+16(OUI+DataType+DataField)
+		}
+		else
+		{
+			Key_Data[data_offset + 1] = 0x26;// 4+2+32(OUI+DataType+DataField)
+		}
+
+        Key_Data[data_offset + 2] = 0x00;
+        Key_Data[data_offset + 3] = 0x0F;
+        Key_Data[data_offset + 4] = 0xAC;
+        Key_Data[data_offset + 5] = 0x01;
+
+		// GTK KDE format - 802.11i-2004  Figure-43x
+        Key_Data[data_offset + 6] = (DefaultKeyIdx & 0x03);
+        Key_Data[data_offset + 7] = 0x00;	// Reserved Byte
+
+		data_offset += 8;
+	}
+
+
+	// Encapsulate GTK and encrypt the key-data field with KEK.
+	// Only for pairwise_msg3_WPA2 and group_msg1
+	if ((MsgType == EAPOL_PAIR_MSG_3 && bWPA2Capable) || (MsgType == EAPOL_GROUP_MSG_1))
+	{
+		// Fill in GTK
+		if (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)
+		{
+			NdisMoveMemory(&Key_Data[data_offset], GTK, LEN_AES_KEY);
+			data_offset += LEN_AES_KEY;
+		}
+		else
+		{
+			NdisMoveMemory(&Key_Data[data_offset], GTK, TKIP_GTK_LENGTH);
+			data_offset += TKIP_GTK_LENGTH;
+		}
+
+		// Still dont know why, but if not append will occur "GTK not include in MSG3"
+		// Patch for compatibility between zero config and funk
+		if (MsgType == EAPOL_PAIR_MSG_3 && bWPA2Capable)
+		{
+			if (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)
+			{
+				Key_Data[data_offset + 0] = 0xDD;
+				Key_Data[data_offset + 1] = 0;
+				data_offset += 2;
+			}
+			else
+			{
+				Key_Data[data_offset + 0] = 0xDD;
+				Key_Data[data_offset + 1] = 0;
+				Key_Data[data_offset + 2] = 0;
+				Key_Data[data_offset + 3] = 0;
+				Key_Data[data_offset + 4] = 0;
+				Key_Data[data_offset + 5] = 0;
+				data_offset += 6;
+			}
+		}
+
+		// Encrypt the data material in key data field
+		if (WepStatus == Ndis802_11Encryption3Enabled)
+		{
+			AES_GTK_KEY_WRAP(&PTK[16], Key_Data, data_offset, Rc4GTK);
+            // AES wrap function will grow 8 bytes in length
+            data_offset += 8;
+		}
+		else
+		{
+			// PREPARE Encrypted  "Key DATA" field.  (Encrypt GTK with RC4, usinf PTK[16]->[31] as Key, IV-field as IV)
+			// put TxTsc in Key RSC field
+			pAd->PrivateInfo.FCSCRC32 = PPPINITFCS32;   //Init crc32.
+
+			// ekey is the contanetion of IV-field, and PTK[16]->PTK[31]
+			NdisMoveMemory(ekey, pMsg->KeyDesc.KeyIv, LEN_KEY_DESC_IV);
+			NdisMoveMemory(&ekey[LEN_KEY_DESC_IV], &PTK[16], LEN_EAP_EK);
+			ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, ekey, sizeof(ekey));  //INIT SBOX, KEYLEN+3(IV)
+			pAd->PrivateInfo.FCSCRC32 = RTMP_CALC_FCS32(pAd->PrivateInfo.FCSCRC32, Key_Data, data_offset);
+			WPAARCFOUR_ENCRYPT(&pAd->PrivateInfo.WEPCONTEXT, Rc4GTK, Key_Data, data_offset);
+		}
+
+		NdisMoveMemory(pMsg->KeyDesc.KeyData, Rc4GTK, data_offset);
+	}
+	else
+	{
+		NdisMoveMemory(pMsg->KeyDesc.KeyData, Key_Data, data_offset);
+	}
+
+	// set key data length field and total length
+	pMsg->KeyDesc.KeyDataLen[1] = data_offset;
+    pMsg->Body_Len[1] += data_offset;
+
+	os_free_mem(pAd, mpool);
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calcaulate MIC. It is used during 4-ways handsharking.
+
+	Arguments:
+		pAd				-	pointer to our pAdapter context
+    	PeerWepStatus	-	indicate the encryption type
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	CalculateMIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			PeerWepStatus,
+	IN	UCHAR			*PTK,
+	OUT PEAPOL_PACKET   pMsg)
+{
+    UCHAR   *OutBuffer;
+	ULONG	FrameLen = 0;
+	UCHAR	mic[LEN_KEY_DESC_MIC];
+	UCHAR	digest[80];
+
+	// allocate memory for MIC calculation
+	os_alloc_mem(pAd, (PUCHAR *)&OutBuffer, 512);
+
+    if (OutBuffer == NULL)
+    {
+		DBGPRINT(RT_DEBUG_ERROR, ("!!!CalculateMIC: no memory!!!\n"));
+		return;
+    }
+
+	// make a frame for calculating MIC.
+    MakeOutgoingFrame(OutBuffer,            	&FrameLen,
+                      pMsg->Body_Len[1] + 4,  	pMsg,
+                      END_OF_ARGS);
+
+	NdisZeroMemory(mic, sizeof(mic));
+
+	// Calculate MIC
+    if (PeerWepStatus == Ndis802_11Encryption3Enabled)
+ 	{
+		HMAC_SHA1(OutBuffer,  FrameLen, PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(PTK,  LEN_EAP_MICK, OutBuffer, FrameLen, mic);
+	}
+
+	// store the calculated MIC
+	NdisMoveMemory(pMsg->KeyDesc.KeyMic, mic, LEN_KEY_DESC_MIC);
+
+	os_free_mem(pAd, OutBuffer);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Some received frames can't decrypt by Asic, so decrypt them by software.
+
+	Arguments:
+		pAd				-	pointer to our pAdapter context
+    	PeerWepStatus	-	indicate the encryption type
+
+	Return Value:
+		NDIS_STATUS_SUCCESS		-	decryption successful
+		NDIS_STATUS_FAILURE		-	decryption failure
+
+	========================================================================
+*/
+NDIS_STATUS	RTMPSoftDecryptBroadCastData(
+	IN	PRTMP_ADAPTER					pAd,
+	IN	RX_BLK							*pRxBlk,
+	IN  NDIS_802_11_ENCRYPTION_STATUS 	GroupCipher,
+	IN  PCIPHER_KEY						pShard_key)
+{
+	PRXWI_STRUC			pRxWI = pRxBlk->pRxWI;
+
+
+
+	// handle WEP decryption
+	if (GroupCipher == Ndis802_11Encryption1Enabled)
+    {
+		if (RTMPSoftDecryptWEP(pAd, pRxBlk->pData, pRxWI->MPDUtotalByteCount, pShard_key))
+		{
+
+			//Minus IV[4] & ICV[4]
+			pRxWI->MPDUtotalByteCount -= 8;
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR : Software decrypt WEP data fails.\n"));
+			// give up this frame
+			return NDIS_STATUS_FAILURE;
+		}
+	}
+	// handle TKIP decryption
+	else if (GroupCipher == Ndis802_11Encryption2Enabled)
+	{
+		if (RTMPSoftDecryptTKIP(pAd, pRxBlk->pData, pRxWI->MPDUtotalByteCount, 0, pShard_key))
+		{
+
+			//Minus 8 bytes MIC, 8 bytes IV/EIV, 4 bytes ICV
+			pRxWI->MPDUtotalByteCount -= 20;
+		}
+        else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR : RTMPSoftDecryptTKIP Failed\n"));
+			// give up this frame
+			return NDIS_STATUS_FAILURE;
+        }
+	}
+	// handle AES decryption
+	else if (GroupCipher == Ndis802_11Encryption3Enabled)
+	{
+		if (RTMPSoftDecryptAES(pAd, pRxBlk->pData, pRxWI->MPDUtotalByteCount , pShard_key))
+		{
+
+			//8 bytes MIC, 8 bytes IV/EIV (CCMP Header)
+			pRxWI->MPDUtotalByteCount -= 16;
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR : RTMPSoftDecryptAES Failed\n"));
+			// give up this frame
+			return NDIS_STATUS_FAILURE;
+		}
+	}
+	else
+	{
+		// give up this frame
+		return NDIS_STATUS_FAILURE;
+	}
+
+	return NDIS_STATUS_SUCCESS;
+
+}
+
diff --git a/drivers/staging/rt2860/common/dfs.c b/drivers/staging/rt2860/common/dfs.c
new file mode 100644
index 000000000000..b09bba5cb206
--- /dev/null
+++ b/drivers/staging/rt2860/common/dfs.c
@@ -0,0 +1,453 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    ap_dfs.c
+
+    Abstract:
+    Support DFS function.
+
+    Revision History:
+    Who       When            What
+    --------  ----------      ----------------------------------------------
+    Fonchi    03-12-2007      created
+*/
+
+#include "../rt_config.h"
+
+typedef struct _RADAR_DURATION_TABLE
+{
+	ULONG RDDurRegion;
+	ULONG RadarSignalDuration;
+	ULONG Tolerance;
+} RADAR_DURATION_TABLE, *PRADAR_DURATION_TABLE;
+
+
+static UCHAR RdIdleTimeTable[MAX_RD_REGION][4] =
+{
+	{9, 250, 250, 250},		// CE
+	{4, 250, 250, 250},		// FCC
+	{4, 250, 250, 250},		// JAP
+	{15, 250, 250, 250},	// JAP_W53
+	{4, 250, 250, 250}		// JAP_W56
+};
+
+/*
+	========================================================================
+
+	Routine Description:
+		Bbp Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+
+	========================================================================
+*/
+VOID BbpRadarDetectionStart(
+	IN PRTMP_ADAPTER pAd)
+{
+	UINT8 RadarPeriod;
+
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 114, 0x02);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 121, 0x20);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 122, 0x00);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 123, 0x08/*0x80*/);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 124, 0x28);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 125, 0xff);
+
+#if 0
+	// toggle Rx enable bit for radar detection.
+	// it's Andy's recommand.
+	{
+		UINT32 Value;
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+	Value |= (0x1 << 3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	Value &= ~(0x1 << 3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	}
+#endif
+	RadarPeriod = ((UINT)RdIdleTimeTable[pAd->CommonCfg.RadarDetect.RDDurRegion][0] + (UINT)pAd->CommonCfg.RadarDetect.DfsSessionTime) < 250 ?
+			(RdIdleTimeTable[pAd->CommonCfg.RadarDetect.RDDurRegion][0] + pAd->CommonCfg.RadarDetect.DfsSessionTime) : 250;
+
+	RTMP_IO_WRITE8(pAd, 0x7020, 0x1d);
+	RTMP_IO_WRITE8(pAd, 0x7021, 0x40);
+
+	RadarDetectionStart(pAd, 0, RadarPeriod);
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Bbp Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+
+	========================================================================
+*/
+VOID BbpRadarDetectionStop(
+	IN PRTMP_ADAPTER pAd)
+{
+	RTMP_IO_WRITE8(pAd, 0x7020, 0x1d);
+	RTMP_IO_WRITE8(pAd, 0x7021, 0x60);
+
+	RadarDetectionStop(pAd);
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+
+	========================================================================
+*/
+VOID RadarDetectionStart(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN CTSProtect,
+	IN UINT8 CTSPeriod)
+{
+	UINT8 DfsActiveTime = (pAd->CommonCfg.RadarDetect.DfsSessionTime & 0x1f);
+	UINT8 CtsProtect = (CTSProtect == 1) ? 0x02 : 0x01; // CTS protect.
+
+	if (CTSProtect != 0)
+	{
+		switch(pAd->CommonCfg.RadarDetect.RDDurRegion)
+		{
+		case FCC:
+		case JAP_W56:
+			CtsProtect = 0x03;
+			break;
+
+		case CE:
+		case JAP_W53:
+		default:
+			CtsProtect = 0x02;
+			break;
+		}
+	}
+	else
+		CtsProtect = 0x01;
+
+
+	// send start-RD with CTS protection command to MCU
+	// highbyte [7]		reserve
+	// highbyte [6:5]	0x: stop Carrier/Radar detection
+	// highbyte [10]:	Start Carrier/Radar detection without CTS protection, 11: Start Carrier/Radar detection with CTS protection
+	// highbyte [4:0]	Radar/carrier detection duration. In 1ms.
+
+	// lowbyte [7:0]	Radar/carrier detection period, in 1ms.
+	AsicSendCommandToMcu(pAd, 0x60, 0xff, CTSPeriod, DfsActiveTime | (CtsProtect << 5));
+	//AsicSendCommandToMcu(pAd, 0x63, 0xff, 10, 0);
+
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		TRUE	Found radar signal
+		FALSE	Not found radar signal
+
+	========================================================================
+*/
+VOID RadarDetectionStop(
+	IN PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE,("RadarDetectionStop.\n"));
+	AsicSendCommandToMcu(pAd, 0x60, 0xff, 0x00, 0x00);	// send start-RD with CTS protection command to MCU
+
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Radar channel check routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		TRUE	need to do radar detect
+		FALSE	need not to do radar detect
+
+	========================================================================
+*/
+BOOLEAN RadarChannelCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			Ch)
+{
+#if 1
+	INT		i;
+	BOOLEAN result = FALSE;
+
+	for (i=0; i<pAd->ChannelListNum; i++)
+	{
+		if (Ch == pAd->ChannelList[i].Channel)
+		{
+			result = pAd->ChannelList[i].DfsReq;
+			break;
+		}
+	}
+
+	return result;
+#else
+	INT		i;
+	UCHAR	Channel[15]={52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+
+	for (i=0; i<15; i++)
+	{
+		if (Ch == Channel[i])
+		{
+			break;
+		}
+	}
+
+	if (i != 15)
+		return TRUE;
+	else
+		return FALSE;
+#endif
+}
+
+ULONG JapRadarType(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG		i;
+	const UCHAR	Channel[15]={52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+
+	if (pAd->CommonCfg.RadarDetect.RDDurRegion != JAP)
+	{
+		return pAd->CommonCfg.RadarDetect.RDDurRegion;
+	}
+
+	for (i=0; i<15; i++)
+	{
+		if (pAd->CommonCfg.Channel == Channel[i])
+		{
+			break;
+		}
+	}
+
+	if (i < 4)
+		return JAP_W53;
+	else if (i < 15)
+		return JAP_W56;
+	else
+		return JAP; // W52
+
+}
+
+ULONG RTMPBbpReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd)
+{
+	UINT8 byteValue = 0;
+	ULONG result;
+
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R115, &byteValue);
+
+	result = 0;
+	switch (byteValue)
+	{
+	case 1: // radar signal detected by pulse mode.
+	case 2: // radar signal detected by width mode.
+		result = RTMPReadRadarDuration(pAd);
+		break;
+
+	case 0: // No radar signal.
+	default:
+
+		result = 0;
+		break;
+	}
+
+	return result;
+}
+
+ULONG RTMPReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd)
+{
+	ULONG result = 0;
+
+#ifdef DFS_SUPPORT
+	UINT8 duration1 = 0, duration2 = 0, duration3 = 0;
+
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R116, &duration1);
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R117, &duration2);
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R118, &duration3);
+	result = (duration1 << 16) + (duration2 << 8) + duration3;
+#endif // DFS_SUPPORT //
+
+	return result;
+
+}
+
+VOID RTMPCleanRadarDuration(
+	IN PRTMP_ADAPTER	pAd)
+{
+	return;
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Radar wave detection. The API should be invoke each second.
+
+    Arguments:
+        pAd         - Adapter pointer
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID ApRadarDetectPeriodic(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT	i;
+
+	pAd->CommonCfg.RadarDetect.InServiceMonitorCount++;
+
+	for (i=0; i<pAd->ChannelListNum; i++)
+	{
+		if (pAd->ChannelList[i].RemainingTimeForUse > 0)
+		{
+			pAd->ChannelList[i].RemainingTimeForUse --;
+			if ((pAd->Mlme.PeriodicRound%5) == 0)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("RadarDetectPeriodic - ch=%d, RemainingTimeForUse=%d\n", pAd->ChannelList[i].Channel, pAd->ChannelList[i].RemainingTimeForUse));
+			}
+		}
+	}
+
+	//radar detect
+	if ((pAd->CommonCfg.Channel > 14)
+		&& (pAd->CommonCfg.bIEEE80211H == 1)
+		&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+	{
+		RadarDetectPeriodic(pAd);
+	}
+
+	return;
+}
+
+// Periodic Radar detection, switch channel will occur in RTMPHandleTBTTInterrupt()
+// Before switch channel, driver needs doing channel switch announcement.
+VOID RadarDetectPeriodic(
+	IN PRTMP_ADAPTER	pAd)
+{
+	// need to check channel availability, after switch channel
+	if (pAd->CommonCfg.RadarDetect.RDMode != RD_SILENCE_MODE)
+			return;
+
+	// channel availability check time is 60sec, use 65 for assurance
+	if (pAd->CommonCfg.RadarDetect.RDCount++ > pAd->CommonCfg.RadarDetect.ChMovingTime)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("Not found radar signal, start send beacon and radar detection in service monitor\n\n"));
+			BbpRadarDetectionStop(pAd);
+		AsicEnableBssSync(pAd);
+		pAd->CommonCfg.RadarDetect.RDMode = RD_NORMAL_MODE;
+
+
+		return;
+	}
+
+	return;
+}
+
+
+/*
+    ==========================================================================
+    Description:
+		change channel moving time for DFS testing.
+
+	Arguments:
+	    pAdapter                    Pointer to our adapter
+	    wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 set ChMovTime=[value]
+    ==========================================================================
+*/
+INT Set_ChMovingTime_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg)
+{
+	UINT8 Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	pAd->CommonCfg.RadarDetect.ChMovingTime = Value;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s:: %d\n", __func__,
+		pAd->CommonCfg.RadarDetect.ChMovingTime));
+
+	return TRUE;
+}
+
+INT Set_LongPulseRadarTh_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg)
+{
+	UINT8 Value;
+
+	Value = simple_strtol(arg, 0, 10) > 10 ? 10 : simple_strtol(arg, 0, 10);
+
+	pAd->CommonCfg.RadarDetect.LongPulseRadarTh = Value;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s:: %d\n", __func__,
+		pAd->CommonCfg.RadarDetect.LongPulseRadarTh));
+
+	return TRUE;
+}
+
+
diff --git a/drivers/staging/rt2860/common/eeprom.c b/drivers/staging/rt2860/common/eeprom.c
new file mode 100644
index 000000000000..bed2d666629c
--- /dev/null
+++ b/drivers/staging/rt2860/common/eeprom.c
@@ -0,0 +1,244 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	eeprom.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#include "../rt_config.h"
+
+// IRQL = PASSIVE_LEVEL
+VOID RaiseClock(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  UINT32 *x)
+{
+    *x = *x | EESK;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, *x);
+    RTMPusecDelay(1);				// Max frequency = 1MHz in Spec. definition
+}
+
+// IRQL = PASSIVE_LEVEL
+VOID LowerClock(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  UINT32 *x)
+{
+    *x = *x & ~EESK;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, *x);
+    RTMPusecDelay(1);
+}
+
+// IRQL = PASSIVE_LEVEL
+USHORT ShiftInBits(
+    IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32		x,i;
+	USHORT      data=0;
+
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+    x &= ~( EEDO | EEDI);
+
+    for(i=0; i<16; i++)
+    {
+        data = data << 1;
+        RaiseClock(pAd, &x);
+
+        RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+        x &= ~(EEDI);
+        if(x & EEDO)
+            data |= 1;
+
+        LowerClock(pAd, &x);
+    }
+
+    return data;
+}
+
+// IRQL = PASSIVE_LEVEL
+VOID ShiftOutBits(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  USHORT data,
+    IN  USHORT count)
+{
+    UINT32       x,mask;
+
+    mask = 0x01 << (count - 1);
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+    x &= ~(EEDO | EEDI);
+
+    do
+    {
+        x &= ~EEDI;
+        if(data & mask)		x |= EEDI;
+
+        RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+        RaiseClock(pAd, &x);
+        LowerClock(pAd, &x);
+
+        mask = mask >> 1;
+    } while(mask);
+
+    x &= ~EEDI;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+}
+
+// IRQL = PASSIVE_LEVEL
+VOID EEpromCleanup(
+    IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32 x;
+
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+    x &= ~(EECS | EEDI);
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+    RaiseClock(pAd, &x);
+    LowerClock(pAd, &x);
+}
+
+VOID EWEN(
+	IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32	x;
+
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode and six pulse in that order
+    ShiftOutBits(pAd, EEPROM_EWEN_OPCODE, 5);
+    ShiftOutBits(pAd, 0, 6);
+
+    EEpromCleanup(pAd);
+}
+
+VOID EWDS(
+	IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32	x;
+
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode and six pulse in that order
+    ShiftOutBits(pAd, EEPROM_EWDS_OPCODE, 5);
+    ShiftOutBits(pAd, 0, 6);
+
+    EEpromCleanup(pAd);
+}
+
+// IRQL = PASSIVE_LEVEL
+USHORT RTMP_EEPROM_READ16(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  USHORT Offset)
+{
+    UINT32		x;
+    USHORT		data;
+
+    Offset /= 2;
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode and register number in that order
+    ShiftOutBits(pAd, EEPROM_READ_OPCODE, 3);
+    ShiftOutBits(pAd, Offset, pAd->EEPROMAddressNum);
+
+    // Now read the data (16 bits) in from the selected EEPROM word
+    data = ShiftInBits(pAd);
+
+    EEpromCleanup(pAd);
+
+    return data;
+}	//ReadEEprom
+
+VOID RTMP_EEPROM_WRITE16(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  USHORT Offset,
+    IN  USHORT Data)
+{
+    UINT32 x;
+
+	Offset /= 2;
+
+	EWEN(pAd);
+
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode ,register number and data in that order
+    ShiftOutBits(pAd, EEPROM_WRITE_OPCODE, 3);
+    ShiftOutBits(pAd, Offset, pAd->EEPROMAddressNum);
+	ShiftOutBits(pAd, Data, 16);		// 16-bit access
+
+    // read DO status
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+	EEpromCleanup(pAd);
+
+	RTMPusecDelay(10000);	//delay for twp(MAX)=10ms
+
+	EWDS(pAd);
+
+    EEpromCleanup(pAd);
+}
+
diff --git a/drivers/staging/rt2860/common/firmware.h b/drivers/staging/rt2860/common/firmware.h
new file mode 100644
index 000000000000..e72996f42c07
--- /dev/null
+++ b/drivers/staging/rt2860/common/firmware.h
@@ -0,0 +1,558 @@
+/*
+ Copyright (c) 2007, Ralink Technology Corporation
+ All rights reserved.
+
+ Redistribution.  Redistribution and use in binary form, without
+ modification, are permitted provided that the following conditions are
+ met:
+
+ 	* Redistributions must reproduce the above copyright notice and the
+ 	following disclaimer in the documentation and/or other materials
+ 	provided with the distribution.
+ 	* Neither the name of Ralink Technology Corporation nor the names of its
+ 	suppliers may be used to endorse or promote products derived from this
+ 	software without specific prior written permission.
+ 	* No reverse engineering, decompilation, or disassembly of this software
+ 	is permitted.
+
+ Limited patent license. Ralink Technology Corporation grants a world-wide,
+ royalty-free, non-exclusive license under patents it now or hereafter
+ owns or controls to make, have made, use, import, offer to sell and
+ sell ("Utilize") this software, but solely to the extent that any
+ such patent is necessary to Utilize the software alone, or in
+ combination with an operating system licensed under an approved Open
+ Source license as listed by the Open Source Initiative at
+ http://opensource.org/licenses.  The patent license shall not apply to
+ any other combinations which include this software.  No hardware per
+ se is licensed hereunder.
+
+ DISCLAIMER.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
+ BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+ FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+ TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
+ USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
+ DAMAGE.
+*/
+/* AUTO GEN PLEASE DO NOT MODIFY IT */
+/* AUTO GEN PLEASE DO NOT MODIFY IT */
+
+
+UCHAR FirmwareImage [] = {
+0x02, 0x03, 0x5e, 0x02, 0x02, 0xb1, 0x22, 0x22, 0xff, 0xff, 0xff, 0x02, 0x01, 0x82, 0xff, 0xff,
+0xff, 0xff, 0xff, 0x02, 0x00, 0x1e, 0xff, 0xff, 0xff, 0xff, 0xff, 0x02, 0x01, 0x33, 0xc0, 0xe0,
+0xc0, 0xf0, 0xc0, 0x83, 0xc0, 0x82, 0xc0, 0xd0, 0x75, 0xd0, 0x18, 0xc2, 0xaf, 0x30, 0x45, 0x03,
+0x12, 0x10, 0x09, 0x90, 0x04, 0x16, 0xe0, 0x30, 0xe3, 0x03, 0x74, 0x08, 0xf0, 0x90, 0x04, 0x14,
+0xe0, 0x20, 0xe7, 0x03, 0x02, 0x00, 0xcb, 0x74, 0x80, 0xf0, 0x90, 0x70, 0x12, 0xe0, 0xf5, 0x2f,
+0x90, 0x04, 0x04, 0xe0, 0x24, 0xcf, 0x60, 0x30, 0x14, 0x60, 0x42, 0x24, 0xe2, 0x60, 0x47, 0x14,
+0x60, 0x55, 0x24, 0x21, 0x70, 0x60, 0xe5, 0x55, 0x24, 0xfe, 0x60, 0x07, 0x14, 0x60, 0x08, 0x24,
+0x02, 0x70, 0x08, 0x7d, 0x01, 0x80, 0x28, 0x7d, 0x02, 0x80, 0x24, 0x90, 0x70, 0x10, 0xe0, 0xf5,
+0x50, 0x85, 0x2f, 0x40, 0xd2, 0x01, 0x80, 0x3e, 0xe5, 0x55, 0x64, 0x03, 0x60, 0x04, 0xe5, 0x55,
+0x70, 0x04, 0x7d, 0x02, 0x80, 0x09, 0x85, 0x2f, 0x41, 0xd2, 0x02, 0x80, 0x29, 0xad, 0x55, 0xaf,
+0x2f, 0x12, 0x02, 0x8d, 0x80, 0x20, 0x90, 0x70, 0x10, 0xe0, 0xf5, 0x47, 0x90, 0x70, 0x11, 0xe0,
+0xf5, 0x44, 0x12, 0x10, 0x25, 0x80, 0x06, 0x90, 0x70, 0x10, 0xe0, 0xf5, 0x45, 0xe4, 0xfd, 0xaf,
+0x2f, 0x12, 0x02, 0x8d, 0xd2, 0x04, 0x90, 0x70, 0x13, 0xe4, 0xf0, 0xd2, 0xaf, 0xd0, 0xd0, 0xd0,
+0x82, 0xd0, 0x83, 0xd0, 0xf0, 0xd0, 0xe0, 0x32, 0x90, 0x70, 0x2a, 0xe0, 0x30, 0xe1, 0x53, 0xc2,
+0xaf, 0x90, 0x70, 0x28, 0xe0, 0x90, 0x10, 0x1c, 0xf0, 0x90, 0x70, 0x29, 0xe0, 0x90, 0x10, 0x1d,
+0xf0, 0x90, 0x70, 0x2a, 0xe0, 0x90, 0x10, 0x1e, 0xf0, 0x90, 0x10, 0x1c, 0xe0, 0xf5, 0x30, 0x90,
+0x10, 0x1e, 0xe0, 0x20, 0xe1, 0xf3, 0x90, 0x10, 0x1c, 0xe0, 0x90, 0x70, 0x28, 0xf0, 0x90, 0x10,
+0x1d, 0xe0, 0x90, 0x70, 0x29, 0xf0, 0x90, 0x10, 0x1e, 0xe0, 0x90, 0x70, 0x2a, 0xf0, 0x30, 0x4a,
+0x0d, 0x90, 0x70, 0x24, 0xe0, 0x44, 0x01, 0xf0, 0x90, 0x02, 0x2c, 0x74, 0xff, 0xf0, 0xc2, 0x05,
+0xd2, 0xaf, 0x22, 0xc0, 0xe0, 0xc0, 0xf0, 0xc0, 0x83, 0xc0, 0x82, 0xc0, 0xd0, 0xe8, 0xc0, 0xe0,
+0xe9, 0xc0, 0xe0, 0xea, 0xc0, 0xe0, 0xeb, 0xc0, 0xe0, 0xec, 0xc0, 0xe0, 0xed, 0xc0, 0xe0, 0xee,
+0xc0, 0xe0, 0xef, 0xc0, 0xe0, 0xc2, 0xaf, 0x30, 0x45, 0x03, 0x12, 0x10, 0x12, 0xd2, 0xaf, 0xd0,
+0xe0, 0xff, 0xd0, 0xe0, 0xfe, 0xd0, 0xe0, 0xfd, 0xd0, 0xe0, 0xfc, 0xd0, 0xe0, 0xfb, 0xd0, 0xe0,
+0xfa, 0xd0, 0xe0, 0xf9, 0xd0, 0xe0, 0xf8, 0xd0, 0xd0, 0xd0, 0x82, 0xd0, 0x83, 0xd0, 0xf0, 0xd0,
+0xe0, 0x32, 0xc0, 0xe0, 0xc0, 0xf0, 0xc0, 0x83, 0xc0, 0x82, 0xc0, 0xd0, 0x75, 0xd0, 0x10, 0xc2,
+0xaf, 0x30, 0x45, 0x03, 0x12, 0x10, 0x0c, 0x30, 0x58, 0x0a, 0xe5, 0x54, 0x60, 0x04, 0x15, 0x54,
+0x80, 0x02, 0xc2, 0x58, 0x30, 0x59, 0x0a, 0xe5, 0x50, 0x60, 0x04, 0x15, 0x50, 0x80, 0x02, 0xc2,
+0x59, 0xd5, 0x53, 0x07, 0x30, 0x60, 0x04, 0x15, 0x46, 0xd2, 0x04, 0x30, 0x45, 0x03, 0x12, 0x10,
+0x0f, 0xc2, 0x8d, 0xd2, 0xaf, 0xd0, 0xd0, 0xd0, 0x82, 0xd0, 0x83, 0xd0, 0xf0, 0xd0, 0xe0, 0x32,
+0x12, 0x02, 0xd3, 0x30, 0x45, 0x03, 0x12, 0x10, 0x03, 0x30, 0x01, 0x06, 0x20, 0x09, 0x03, 0x12,
+0x10, 0x1c, 0x30, 0x02, 0x06, 0x20, 0x0a, 0x03, 0x12, 0x10, 0x1f, 0x30, 0x03, 0x06, 0x20, 0x0b,
+0x03, 0x12, 0x10, 0x1f, 0x30, 0x04, 0x06, 0x20, 0x0c, 0x03, 0x12, 0x10, 0x22, 0x20, 0x13, 0x09,
+0x20, 0x11, 0x06, 0xe5, 0x2b, 0x45, 0x2c, 0x60, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0xa9, 0x12,
+0x02, 0xec, 0x80, 0xbf, 0xc2, 0x43, 0xd2, 0x45, 0xe4, 0xf5, 0x20, 0xf5, 0x21, 0xf5, 0x53, 0xf5,
+0x46, 0xf5, 0x2b, 0xf5, 0x2c, 0xc2, 0x42, 0xf5, 0x51, 0xf5, 0x52, 0xf5, 0x55, 0x90, 0x04, 0x18,
+0x74, 0x80, 0xf0, 0x90, 0x04, 0x1a, 0x74, 0x08, 0xf0, 0xc2, 0x19, 0xc2, 0x18, 0xc2, 0x1a, 0x22,
+0xc8, 0xef, 0xc8, 0xe6, 0xfa, 0x08, 0xe6, 0x4a, 0x60, 0x0c, 0xc8, 0xef, 0xc8, 0x08, 0xe6, 0x16,
+0x18, 0x70, 0x01, 0x16, 0xc3, 0x22, 0xed, 0x24, 0xff, 0xfd, 0xec, 0x34, 0xff, 0xc8, 0xef, 0xc8,
+0xf6, 0x08, 0xc6, 0xed, 0xc6, 0xd3, 0x22, 0xd0, 0x83, 0xd0, 0x82, 0xf8, 0xe4, 0x93, 0x70, 0x12,
+0x74, 0x01, 0x93, 0x70, 0x0d, 0xa3, 0xa3, 0x93, 0xf8, 0x74, 0x01, 0x93, 0xf5, 0x82, 0x88, 0x83,
+0xe4, 0x73, 0x74, 0x02, 0x93, 0x68, 0x60, 0xef, 0xa3, 0xa3, 0xa3, 0x80, 0xdf, 0xef, 0xf4, 0x60,
+0x1f, 0xe4, 0xfe, 0x12, 0x03, 0x6a, 0xe0, 0xb4, 0xff, 0x12, 0x12, 0x03, 0x6a, 0xef, 0xf0, 0x74,
+0x1c, 0x2e, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xed, 0xf0, 0x22, 0x0e, 0xbe, 0x04, 0xe3,
+0x22, 0xc0, 0xe0, 0xc0, 0xf0, 0xc0, 0x83, 0xc0, 0x82, 0xc0, 0xd0, 0x75, 0xd0, 0x08, 0xc2, 0xaf,
+0x30, 0x45, 0x03, 0x12, 0x10, 0x06, 0xd2, 0xaf, 0xd0, 0xd0, 0xd0, 0x82, 0xd0, 0x83, 0xd0, 0xf0,
+0xd0, 0xe0, 0x32, 0xc2, 0xaf, 0x12, 0x00, 0x06, 0x12, 0x02, 0x14, 0x12, 0x03, 0x1c, 0xe4, 0xf5,
+0x22, 0xf5, 0x47, 0x90, 0x04, 0x00, 0x74, 0x80, 0xf0, 0xd2, 0xaf, 0x22, 0x30, 0x45, 0x03, 0x12,
+0x10, 0x15, 0xe5, 0x20, 0x70, 0x03, 0x20, 0x10, 0x03, 0x30, 0x11, 0x03, 0x43, 0x87, 0x01, 0x22,
+0xc0, 0x2a, 0x74, 0x03, 0xc0, 0xe0, 0xc0, 0x82, 0xc0, 0x83, 0x75, 0x2a, 0x0a, 0x22, 0xc0, 0x2a,
+0x74, 0x03, 0xc0, 0xe0, 0xc0, 0x82, 0xc0, 0x83, 0x75, 0x2a, 0x18, 0x22, 0x75, 0x89, 0x02, 0xe4,
+0xf5, 0x8c, 0xf5, 0x8a, 0xf5, 0x88, 0xf5, 0xb8, 0xf5, 0xe8, 0x75, 0x90, 0x18, 0xd2, 0x8c, 0x75,
+0xa8, 0x05, 0x22, 0xce, 0xef, 0xce, 0xee, 0x60, 0x08, 0x7f, 0xff, 0x12, 0x03, 0x80, 0x1e, 0x80,
+0xf5, 0x22, 0xc8, 0xef, 0xc8, 0xe6, 0x60, 0x03, 0x16, 0xc3, 0x22, 0xed, 0x14, 0xf6, 0xd3, 0x22,
+0xc8, 0xef, 0xc8, 0xe6, 0x60, 0x06, 0x16, 0xe6, 0x24, 0xff, 0xb3, 0x22, 0xc3, 0x22, 0x78, 0x7f,
+0xe4, 0xf6, 0xd8, 0xfd, 0x75, 0x81, 0x5f, 0x02, 0x01, 0xd0, 0x74, 0x14, 0x2e, 0xf5, 0x82, 0xe4,
+0x34, 0x70, 0xf5, 0x83, 0x22, 0xef, 0x90, 0x03, 0x7e, 0x93, 0x90, 0x03, 0x00, 0x73, 0x0a, 0x18,
+0xef, 0x60, 0x03, 0x1f, 0x80, 0xfa, 0x22, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0x02, 0x10, 0x28, 0x02, 0x10, 0x3b, 0x02, 0x10, 0x3c, 0x02, 0x12, 0xb8, 0x02,
+0x12, 0xb9, 0x02, 0x13, 0x3e, 0x02, 0x13, 0x3f, 0xc3, 0x22, 0xff, 0xff, 0x02, 0x16, 0x56, 0x02,
+0x17, 0x6b, 0x02, 0x14, 0x2a, 0x02, 0x13, 0x40, 0x30, 0x05, 0x06, 0x20, 0x0d, 0x03, 0x12, 0x00,
+0xd8, 0x30, 0x06, 0x06, 0x20, 0x0e, 0x03, 0x12, 0x18, 0x5e, 0x22, 0x22, 0x90, 0x04, 0x14, 0xe0,
+0x20, 0xe7, 0x03, 0x02, 0x12, 0xb7, 0x90, 0x70, 0x12, 0xe0, 0xf5, 0x56, 0x90, 0x04, 0x04, 0xe0,
+0x12, 0x02, 0x67, 0x11, 0x4e, 0x30, 0x11, 0x25, 0x31, 0x10, 0x87, 0x33, 0x10, 0xaa, 0x34, 0x10,
+0xc3, 0x35, 0x11, 0x57, 0x50, 0x11, 0x7b, 0x51, 0x11, 0x84, 0x52, 0x11, 0x84, 0x53, 0x11, 0x84,
+0x54, 0x11, 0xc5, 0x55, 0x11, 0xdc, 0x70, 0x12, 0x07, 0x71, 0x12, 0x34, 0x72, 0x12, 0x5e, 0x80,
+0x12, 0x81, 0x83, 0x00, 0x00, 0x12, 0xb7, 0x75, 0x24, 0x05, 0x75, 0x25, 0xdc, 0x90, 0x70, 0x9f,
+0x74, 0x12, 0xf0, 0xd2, 0x18, 0xd2, 0x61, 0x75, 0x35, 0x0d, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5,
+0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0xb7, 0x02, 0x12, 0xaa, 0xc2, 0x18, 0x90, 0x01, 0x14, 0xe0,
+0x54, 0xfd, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0xb7,
+0x02, 0x12, 0xaa, 0xe5, 0x55, 0x64, 0x02, 0x70, 0x37, 0x90, 0x70, 0x10, 0xe0, 0x60, 0x08, 0x90,
+0x01, 0x0d, 0x74, 0x09, 0xf0, 0x80, 0x25, 0xe5, 0x34, 0x14, 0x60, 0x0a, 0x14, 0x60, 0x0f, 0x14,
+0x60, 0x14, 0x24, 0x03, 0x70, 0x16, 0x90, 0x01, 0x0d, 0x74, 0x08, 0xf0, 0x80, 0x0e, 0x90, 0x01,
+0x0d, 0x74, 0x0b, 0xf0, 0x80, 0x06, 0x90, 0x01, 0x0d, 0x74, 0x1b, 0xf0, 0x7d, 0x01, 0x80, 0x02,
+0x7d, 0x02, 0xaf, 0x56, 0x12, 0x02, 0x8d, 0x90, 0x70, 0x11, 0xe0, 0x24, 0xff, 0x92, 0x1b, 0x90,
+0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02,
+0x12, 0xb7, 0x02, 0x12, 0xaa, 0x20, 0x02, 0x03, 0x30, 0x03, 0x1d, 0x7d, 0x02, 0xaf, 0x56, 0x12,
+0x02, 0x8d, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4,
+0x70, 0x03, 0x02, 0x12, 0xb7, 0x02, 0x12, 0xaa, 0x85, 0x56, 0x41, 0xd2, 0x02, 0x22, 0x90, 0x70,
+0x11, 0xe0, 0x24, 0xff, 0x92, 0x1b, 0x22, 0x90, 0x70, 0x10, 0xe0, 0x54, 0x7f, 0x64, 0x02, 0x60,
+0x03, 0x02, 0x12, 0xb7, 0x90, 0x70, 0x11, 0xe0, 0x64, 0x08, 0x60, 0x08, 0xe0, 0x64, 0x20, 0x60,
+0x03, 0x02, 0x12, 0xb7, 0x75, 0x4e, 0x03, 0x75, 0x4f, 0x20, 0x22, 0x90, 0x70, 0x11, 0xe0, 0x24,
+0xff, 0x92, 0x47, 0x22, 0x90, 0x04, 0x04, 0xe0, 0x25, 0xe0, 0x24, 0x5d, 0xf5, 0x57, 0x90, 0x70,
+0x10, 0xe0, 0xff, 0x74, 0x47, 0x25, 0x57, 0xf8, 0xc6, 0xef, 0xc6, 0x90, 0x70, 0x11, 0xe0, 0xff,
+0x74, 0x48, 0x25, 0x57, 0xf8, 0xc6, 0xef, 0xc6, 0xe4, 0xfd, 0xaf, 0x56, 0x12, 0x02, 0x8d, 0x90,
+0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02,
+0x12, 0xb7, 0x02, 0x12, 0xaa, 0xe5, 0x47, 0xb4, 0x07, 0x08, 0x90, 0x70, 0x11, 0xe0, 0x54, 0x07,
+0xf5, 0x26, 0xe4, 0xfd, 0xaf, 0x56, 0x12, 0x02, 0x8d, 0xd2, 0x04, 0x22, 0x90, 0x70, 0x10, 0xe0,
+0xfe, 0x90, 0x70, 0x11, 0xe0, 0xfd, 0xed, 0xf8, 0xe6, 0xf5, 0x57, 0xfd, 0xaf, 0x56, 0x12, 0x02,
+0x8d, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70,
+0x03, 0x02, 0x12, 0xb7, 0x02, 0x12, 0xaa, 0x90, 0x70, 0x10, 0xe0, 0xfe, 0x90, 0x70, 0x11, 0xe0,
+0xfd, 0xed, 0xf5, 0x82, 0x8e, 0x83, 0xe0, 0xf5, 0x57, 0xfd, 0xaf, 0x56, 0x12, 0x02, 0x8d, 0x90,
+0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02,
+0x12, 0xb7, 0x80, 0x76, 0xe4, 0xf5, 0x4e, 0xf5, 0x4f, 0x75, 0x26, 0xff, 0xc2, 0x19, 0xc2, 0x18,
+0xc2, 0x1a, 0x75, 0x34, 0xff, 0xad, 0x57, 0xaf, 0x56, 0x12, 0x02, 0x8d, 0x90, 0x04, 0x14, 0x74,
+0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x60, 0x5b, 0x80, 0x4c, 0x90, 0x70,
+0x10, 0xe0, 0x24, 0xff, 0x92, 0x4a, 0xd2, 0x05, 0xad, 0x57, 0xaf, 0x56, 0x12, 0x02, 0x8d, 0x90,
+0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x60, 0x38, 0x80,
+0x29, 0x90, 0x70, 0x10, 0xe0, 0xf5, 0x34, 0xd3, 0x94, 0x00, 0x40, 0x07, 0x90, 0x01, 0x0d, 0xe0,
+0x54, 0xfb, 0xf0, 0xad, 0x57, 0xaf, 0x56, 0x12, 0x02, 0x8d, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0,
+0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x60, 0x0d, 0x90, 0x70, 0x25, 0xe0, 0x44, 0x01,
+0xf0, 0x90, 0x02, 0x2c, 0x74, 0xff, 0xf0, 0x22, 0x22, 0xe5, 0x53, 0x60, 0x03, 0x02, 0x13, 0x3d,
+0xe5, 0x4f, 0x45, 0x4e, 0x60, 0x08, 0xe5, 0x4f, 0x15, 0x4f, 0x70, 0x02, 0x15, 0x4e, 0xa2, 0x19,
+0xe4, 0x33, 0x90, 0x70, 0x90, 0xf0, 0xa2, 0x18, 0xe4, 0x33, 0xa3, 0xf0, 0x30, 0x19, 0x4d, 0x90,
+0x70, 0x98, 0x74, 0x23, 0xf0, 0xa3, 0xe5, 0x25, 0xf0, 0xe5, 0x24, 0xa3, 0xf0, 0x7f, 0x35, 0x7d,
+0x32, 0x12, 0x03, 0x42, 0x50, 0x09, 0x90, 0x10, 0x04, 0xe0, 0x54, 0xf7, 0xf0, 0xd2, 0x06, 0xe5,
+0x35, 0xd3, 0x94, 0x10, 0x40, 0x1e, 0x30, 0x1a, 0x1b, 0xc2, 0x1a, 0xa2, 0x18, 0x92, 0x19, 0x20,
+0x19, 0x12, 0x90, 0x04, 0x09, 0xe0, 0x54, 0xdd, 0xf0, 0x90, 0x10, 0x04, 0xe0, 0x44, 0x08, 0xf0,
+0xc2, 0x61, 0xd2, 0x03, 0xe5, 0x35, 0xb4, 0x0b, 0x14, 0xd2, 0x03, 0x22, 0xe4, 0xf5, 0x35, 0xa2,
+0x18, 0x92, 0x19, 0x30, 0x19, 0x07, 0x90, 0x04, 0x09, 0xe0, 0x44, 0x22, 0xf0, 0x22, 0x22, 0x22,
+0xc2, 0x4b, 0xc2, 0x4c, 0xe5, 0x44, 0x12, 0x02, 0x67, 0x13, 0x62, 0x00, 0x13, 0xf5, 0x04, 0x13,
+0xf1, 0x08, 0x13, 0xcc, 0x10, 0x13, 0x76, 0x20, 0x13, 0x96, 0x60, 0x13, 0xa7, 0xa0, 0x00, 0x00,
+0x13, 0xf7, 0x85, 0x48, 0x43, 0x85, 0x4a, 0x42, 0x85, 0x4c, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x60,
+0x03, 0x02, 0x13, 0xf7, 0x80, 0x1b, 0xe5, 0x48, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4a, 0xc4,
+0x54, 0x0f, 0xf5, 0x42, 0xe5, 0x4c, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x70,
+0x66, 0x53, 0x43, 0x0f, 0x80, 0x61, 0x85, 0x49, 0x43, 0x85, 0x4b, 0x42, 0x85, 0x4d, 0x5e, 0xe5,
+0x47, 0x64, 0x06, 0x70, 0x52, 0x80, 0x1b, 0xe5, 0x49, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4b,
+0xc4, 0x54, 0x0f, 0xf5, 0x42, 0xe5, 0x4d, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06,
+0x70, 0x35, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x10, 0xf5, 0x43, 0x80, 0x2b, 0xe5, 0x47, 0xb4, 0x04,
+0x06, 0x53, 0x5e, 0xfb, 0x75, 0x42, 0x09, 0xe5, 0x47, 0xb4, 0x05, 0x06, 0x43, 0x5e, 0x04, 0x75,
+0x42, 0x09, 0xe5, 0x47, 0xb4, 0x06, 0x10, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x30, 0xf5, 0x43, 0x80,
+0x06, 0xd2, 0x4b, 0x80, 0x02, 0xd2, 0x4c, 0xe4, 0xf5, 0x38, 0xe5, 0x42, 0xc4, 0x54, 0xf0, 0xff,
+0xe5, 0x43, 0x54, 0x0f, 0x4f, 0xf5, 0x5f, 0x90, 0x70, 0x44, 0xf0, 0xa3, 0xe5, 0x5e, 0xf0, 0xa3,
+0xe5, 0x4a, 0xf0, 0xa3, 0xe5, 0x48, 0xf0, 0xa3, 0xe5, 0x4c, 0xf0, 0xa3, 0xe5, 0x44, 0xf0, 0xa3,
+0xe5, 0x42, 0xf0, 0xa3, 0xe5, 0x43, 0xf0, 0xd2, 0x60, 0x22, 0xe5, 0x47, 0x60, 0x10, 0x24, 0xc0,
+0x70, 0x03, 0x12, 0x16, 0x36, 0x12, 0x14, 0x3f, 0xc2, 0xaf, 0xc2, 0x04, 0xd2, 0xaf, 0x22, 0xc2,
+0xaf, 0x90, 0x04, 0x14, 0xe0, 0x54, 0x0e, 0x60, 0x04, 0xd2, 0x1c, 0x80, 0x08, 0xe5, 0x4e, 0x45,
+0x4f, 0x24, 0xff, 0x92, 0x1c, 0xd2, 0xaf, 0x90, 0x04, 0x14, 0xe0, 0xa2, 0xe4, 0x92, 0x1d, 0x74,
+0x1e, 0xf0, 0xe5, 0x5f, 0x54, 0x0f, 0xf5, 0x2d, 0xe5, 0x38, 0x70, 0x13, 0x30, 0x1c, 0x05, 0xe5,
+0x5f, 0x20, 0xe5, 0x0b, 0x30, 0x1d, 0x19, 0xe5, 0x5f, 0x54, 0x30, 0xff, 0xbf, 0x30, 0x11, 0xe5,
+0x38, 0x70, 0x05, 0x75, 0x38, 0x0c, 0x80, 0x02, 0x15, 0x38, 0xd2, 0x6c, 0xd2, 0x6d, 0x80, 0x0f,
+0xe5, 0x5f, 0x30, 0xe6, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x04, 0xd2, 0x6c, 0xc2, 0x6d, 0xe5,
+0x47, 0x64, 0x03, 0x70, 0x21, 0x30, 0x4b, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x18, 0xe5, 0x38,
+0x70, 0x03, 0x30, 0x4c, 0x11, 0xc2, 0x4c, 0xe5, 0x38, 0x70, 0x05, 0x75, 0x38, 0x07, 0x80, 0x02,
+0x15, 0x38, 0xd2, 0x6c, 0xd2, 0x6d, 0x90, 0x70, 0x46, 0xe5, 0x2d, 0xf0, 0x20, 0x69, 0x07, 0xe5,
+0x5e, 0x20, 0xe0, 0x02, 0xb2, 0x68, 0x20, 0x6b, 0x07, 0xe5, 0x5e, 0x20, 0xe1, 0x02, 0xb2, 0x6a,
+0x20, 0x6d, 0x07, 0xe5, 0x5e, 0x20, 0xe2, 0x02, 0xb2, 0x6c, 0x90, 0x70, 0x47, 0xe5, 0x2d, 0xf0,
+0x75, 0x2e, 0x40, 0x20, 0x69, 0x04, 0xa2, 0x68, 0x80, 0x15, 0x30, 0x68, 0x06, 0xe5, 0x46, 0xa2,
+0xe3, 0x80, 0x0c, 0xe5, 0x46, 0x54, 0xf0, 0xff, 0xbf, 0xf0, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92,
+0x73, 0x92, 0x72, 0x20, 0x6b, 0x04, 0xa2, 0x6a, 0x80, 0x15, 0x30, 0x6a, 0x06, 0xe5, 0x46, 0xa2,
+0xe3, 0x80, 0x0c, 0xe5, 0x46, 0x54, 0xf0, 0xff, 0xbf, 0xf0, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92,
+0x75, 0x92, 0x74, 0x20, 0x6d, 0x04, 0xa2, 0x6c, 0x80, 0x15, 0x30, 0x6c, 0x06, 0xe5, 0x46, 0xa2,
+0xe3, 0x80, 0x0c, 0xe5, 0x46, 0x54, 0xf0, 0xff, 0xbf, 0xf0, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92,
+0x71, 0x92, 0x70, 0x90, 0x10, 0x2f, 0xe5, 0x2e, 0xf0, 0xe5, 0x47, 0x64, 0x06, 0x70, 0x46, 0x90,
+0x02, 0x29, 0xe0, 0x54, 0xfe, 0xf0, 0xe5, 0x43, 0xc4, 0x54, 0x0f, 0x14, 0x60, 0x14, 0x24, 0xfe,
+0x60, 0x1f, 0x24, 0x03, 0x60, 0x03, 0x02, 0x16, 0x35, 0x90, 0x02, 0x28, 0xe0, 0x30, 0x47, 0x0d,
+0x80, 0x07, 0x90, 0x02, 0x28, 0xe0, 0x20, 0x47, 0x04, 0x54, 0xfe, 0xf0, 0x22, 0x44, 0x01, 0xf0,
+0x22, 0xe5, 0x46, 0x30, 0xe3, 0x04, 0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0x90, 0x02, 0x28, 0xe0,
+0x54, 0xfe, 0x4f, 0xf0, 0x22, 0xe5, 0x47, 0x64, 0x07, 0x60, 0x03, 0x02, 0x16, 0x35, 0xf5, 0x27,
+0x90, 0x02, 0x29, 0xe0, 0x54, 0xfc, 0xf0, 0xe5, 0x26, 0x14, 0x60, 0x26, 0x14, 0x60, 0x2e, 0x14,
+0x60, 0x36, 0x24, 0x03, 0x70, 0x5f, 0xe5, 0x46, 0x13, 0x13, 0x13, 0x54, 0x1f, 0x75, 0xf0, 0x03,
+0x84, 0xaf, 0xf0, 0x20, 0x47, 0x04, 0x7e, 0x01, 0x80, 0x02, 0x7e, 0x00, 0xef, 0x6e, 0x24, 0xff,
+0x80, 0x02, 0xa2, 0x47, 0x92, 0x39, 0xa2, 0x47, 0xb3, 0x92, 0x38, 0x80, 0x3f, 0xe5, 0x46, 0x30,
+0xe3, 0x03, 0xd3, 0x80, 0x27, 0xc3, 0x80, 0x24, 0xe5, 0x46, 0x30, 0xe3, 0x0d, 0x54, 0x70, 0xc3,
+0x94, 0x60, 0x50, 0x06, 0x7e, 0x00, 0x7f, 0x01, 0x80, 0x04, 0x7e, 0x00, 0x7f, 0x00, 0x20, 0x47,
+0x04, 0x7d, 0x01, 0x80, 0x02, 0x7d, 0x00, 0xef, 0x6d, 0x4e, 0x24, 0xff, 0x92, 0x38, 0xa2, 0x47,
+0xb3, 0x92, 0x39, 0x80, 0x07, 0xa2, 0x47, 0xb3, 0x92, 0x38, 0x92, 0x39, 0x90, 0x02, 0x28, 0xe0,
+0x54, 0xfc, 0x45, 0x27, 0xf0, 0x22, 0xe4, 0x90, 0x02, 0x29, 0xf0, 0x30, 0x47, 0x04, 0xaf, 0x45,
+0x80, 0x04, 0xe5, 0x45, 0xf4, 0xff, 0x90, 0x02, 0x28, 0xef, 0xf0, 0x22, 0x8f, 0x50, 0xd2, 0x59,
+0x22, 0x8f, 0x54, 0xd2, 0x58, 0x22, 0xe4, 0xf5, 0x30, 0xc2, 0xaf, 0xe5, 0x51, 0x14, 0x60, 0x4a,
+0x14, 0x60, 0x6a, 0x24, 0x02, 0x60, 0x03, 0x02, 0x17, 0x4c, 0xd2, 0x59, 0x75, 0x55, 0x01, 0x20,
+0x19, 0x1c, 0x90, 0x02, 0x08, 0xe0, 0x54, 0xfe, 0xf0, 0xe0, 0x20, 0xe1, 0x23, 0x90, 0x04, 0x34,
+0xe0, 0xb4, 0x02, 0x1c, 0xa3, 0xe0, 0xb4, 0x02, 0x17, 0xa3, 0xe0, 0xb4, 0x02, 0x12, 0x7f, 0x20,
+0x12, 0x16, 0x4c, 0x90, 0x10, 0x04, 0xe0, 0x54, 0xf3, 0xf0, 0x75, 0x51, 0x01, 0x02, 0x17, 0x4c,
+0xe5, 0x50, 0x70, 0x06, 0x75, 0x30, 0x03, 0x02, 0x17, 0x4c, 0x90, 0x12, 0x00, 0xe0, 0x54, 0x03,
+0x70, 0x15, 0x7f, 0x20, 0x12, 0x16, 0x4c, 0x20, 0x19, 0x07, 0x90, 0x02, 0x08, 0xe0, 0x54, 0xfb,
+0xf0, 0x75, 0x51, 0x02, 0x02, 0x17, 0x4c, 0xe5, 0x50, 0x70, 0x02, 0x80, 0x7a, 0x20, 0x19, 0x0f,
+0x90, 0x02, 0x08, 0xe0, 0x20, 0xe3, 0x6c, 0x90, 0x04, 0x37, 0xe0, 0x64, 0x22, 0x70, 0x64, 0x90,
+0x12, 0x04, 0x74, 0x0a, 0xf0, 0x30, 0x1b, 0x11, 0x90, 0x13, 0x28, 0xe0, 0x54, 0xf0, 0xf0, 0xa3,
+0xe0, 0x54, 0xf0, 0xf0, 0xa3, 0xe0, 0x54, 0xfa, 0xf0, 0x20, 0x19, 0x07, 0x90, 0x04, 0x01, 0xe0,
+0x44, 0x10, 0xf0, 0xe5, 0x34, 0xf4, 0x90, 0x04, 0x01, 0x60, 0x06, 0xe0, 0x54, 0xfb, 0xf0, 0x80,
+0x04, 0xe0, 0x54, 0xf9, 0xf0, 0x20, 0x19, 0x07, 0x90, 0x12, 0x04, 0xe0, 0x44, 0x04, 0xf0, 0xe5,
+0x34, 0xf4, 0x60, 0x14, 0x90, 0x01, 0x0d, 0xe0, 0xf5, 0x33, 0xe5, 0x34, 0xd3, 0x94, 0x02, 0x40,
+0x07, 0x90, 0x12, 0x04, 0xe0, 0x54, 0xfd, 0xf0, 0x75, 0x30, 0x01, 0x75, 0x55, 0x02, 0xe4, 0xf5,
+0x51, 0x80, 0x09, 0xe5, 0x50, 0x70, 0x05, 0x75, 0x30, 0x03, 0xf5, 0x51, 0xe5, 0x30, 0x60, 0x18,
+0xc2, 0x01, 0xe4, 0xf5, 0x51, 0xc2, 0x59, 0x20, 0x19, 0x0e, 0xad, 0x30, 0xaf, 0x40, 0x12, 0x18,
+0x2a, 0xe5, 0x30, 0xb4, 0x03, 0x02, 0xd2, 0x03, 0xd2, 0xaf, 0x22, 0xc2, 0xaf, 0x30, 0x01, 0x0e,
+0xe4, 0xf5, 0x51, 0xc2, 0x59, 0xc2, 0x01, 0x7d, 0x02, 0xaf, 0x40, 0x12, 0x18, 0x2a, 0xe5, 0x52,
+0x14, 0x60, 0x55, 0x14, 0x60, 0x2f, 0x24, 0x02, 0x60, 0x03, 0x02, 0x18, 0x27, 0xe5, 0x34, 0xf4,
+0x60, 0x23, 0xe5, 0x34, 0xd3, 0x94, 0x02, 0x40, 0x16, 0x90, 0x12, 0x04, 0xe0, 0x44, 0x02, 0xf0,
+0x90, 0x01, 0x0d, 0xe0, 0x20, 0xe3, 0x03, 0x02, 0x18, 0x27, 0x7f, 0x50, 0x12, 0x16, 0x51, 0x75,
+0x52, 0x02, 0x75, 0x55, 0x03, 0xe5, 0x34, 0xf4, 0x60, 0x0a, 0xe5, 0x54, 0x70, 0x69, 0x90, 0x01,
+0x0d, 0xe5, 0x33, 0xf0, 0x90, 0x12, 0x04, 0xe0, 0x54, 0xfb, 0xf0, 0x7f, 0x20, 0x12, 0x16, 0x51,
+0x75, 0x52, 0x01, 0x75, 0x55, 0x03, 0x80, 0x4f, 0xe5, 0x54, 0x70, 0x4b, 0x90, 0x04, 0x01, 0xe0,
+0x44, 0x0e, 0xf0, 0x20, 0x19, 0x04, 0xe0, 0x54, 0xef, 0xf0, 0x90, 0x13, 0x28, 0xe0, 0x44, 0x0f,
+0xf0, 0xa3, 0xe0, 0x44, 0x0f, 0xf0, 0xa3, 0xe0, 0x44, 0x05, 0xf0, 0x90, 0x12, 0x04, 0x74, 0x03,
+0xf0, 0x20, 0x19, 0x07, 0x90, 0x02, 0x08, 0xe0, 0x44, 0x05, 0xf0, 0x90, 0x10, 0x04, 0xe0, 0x44,
+0x0c, 0xf0, 0xe4, 0xf5, 0x52, 0xf5, 0x55, 0x30, 0x02, 0x0b, 0xc2, 0x02, 0x7d, 0x01, 0xaf, 0x41,
+0x12, 0x18, 0x2a, 0x80, 0x02, 0xc2, 0x03, 0xd2, 0xaf, 0x22, 0xef, 0xf4, 0x60, 0x2d, 0xe4, 0xfe,
+0x74, 0x14, 0x2e, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xe0, 0xb4, 0xff, 0x19, 0x74, 0x14,
+0x2e, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xef, 0xf0, 0x74, 0x1c, 0x2e, 0xf5, 0x82, 0xe4,
+0x34, 0x70, 0xf5, 0x83, 0xed, 0xf0, 0x22, 0x0e, 0xbe, 0x04, 0xd5, 0x22, 0x22, 0x22, 0x20, 0x19,
+0x03, 0x02, 0x19, 0x0f, 0x90, 0x70, 0x80, 0xe0, 0x04, 0xf0, 0x90, 0x04, 0x37, 0xe0, 0x30, 0xe5,
+0x03, 0x02, 0x19, 0x0b, 0x90, 0x04, 0x28, 0xe0, 0xf5, 0x31, 0xa3, 0xe0, 0xf5, 0x30, 0xf5, 0x32,
+0xe4, 0xf5, 0x37, 0x90, 0x70, 0x81, 0xe0, 0x04, 0xf0, 0x90, 0x70, 0x82, 0xe0, 0x04, 0xf0, 0xe5,
+0x32, 0x75, 0xf0, 0x80, 0xa4, 0x24, 0x00, 0xff, 0xe5, 0xf0, 0x34, 0x80, 0xfe, 0xe5, 0x30, 0x65,
+0x32, 0x70, 0x05, 0xfc, 0x7d, 0x18, 0x80, 0x04, 0x7c, 0x00, 0x7d, 0x00, 0xef, 0x2d, 0xff, 0xee,
+0x3c, 0xfe, 0x12, 0x19, 0x10, 0x50, 0x25, 0x90, 0x70, 0x83, 0xe0, 0x04, 0xf0, 0x90, 0x01, 0x14,
+0xe0, 0x44, 0x02, 0xf0, 0xe0, 0x30, 0xe1, 0x06, 0x90, 0x70, 0x92, 0x74, 0x45, 0xf0, 0x90, 0x70,
+0x93, 0xe0, 0x04, 0xf0, 0x90, 0x04, 0x01, 0xe0, 0x90, 0x70, 0x94, 0xf0, 0xe5, 0x32, 0x65, 0x31,
+0x60, 0x10, 0xe4, 0x25, 0x32, 0xff, 0xe4, 0x34, 0x80, 0x8f, 0x82, 0xf5, 0x83, 0xe0, 0xf5, 0x32,
+0x80, 0x97, 0x90, 0x04, 0x10, 0x74, 0x01, 0xf0, 0x90, 0x04, 0x28, 0xe5, 0x31, 0xf0, 0xa3, 0xe5,
+0x30, 0xf0, 0x90, 0x04, 0x11, 0x74, 0x01, 0xf0, 0x02, 0x18, 0x6a, 0xc2, 0x06, 0xd2, 0x1a, 0x22,
+0x90, 0x70, 0x84, 0xe5, 0x37, 0xf0, 0xc3, 0x94, 0x06, 0x50, 0x19, 0x8f, 0x82, 0x8e, 0x83, 0xe0,
+0xb4, 0xff, 0x07, 0x05, 0x37, 0xe4, 0xf5, 0x36, 0x80, 0x59, 0xe4, 0xf5, 0x37, 0x8f, 0x82, 0x8e,
+0x83, 0xf0, 0x80, 0x4f, 0xe5, 0x36, 0x75, 0xf0, 0x06, 0x84, 0x74, 0x08, 0x25, 0xf0, 0xf5, 0x82,
+0xe4, 0x34, 0x10, 0xf5, 0x83, 0xe0, 0xfd, 0x8f, 0x82, 0x8e, 0x83, 0xe0, 0xfc, 0x6d, 0x70, 0x30,
+0x90, 0x70, 0x88, 0xe0, 0x04, 0xf0, 0xa3, 0xe0, 0xfd, 0xd3, 0x95, 0x37, 0x40, 0x02, 0x80, 0x02,
+0xad, 0x37, 0x90, 0x70, 0x89, 0xed, 0xf0, 0x05, 0x37, 0x05, 0x36, 0xe5, 0x36, 0x75, 0xf0, 0x06,
+0x84, 0x74, 0x8a, 0x25, 0xf0, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xec, 0xf0, 0x80, 0x03,
+0xe4, 0xf5, 0x37, 0x0f, 0xbf, 0x00, 0x01, 0x0e, 0xef, 0x54, 0x7f, 0x60, 0x0a, 0xe5, 0x37, 0xc3,
+0x94, 0x4e, 0x50, 0x03, 0x02, 0x19, 0x10, 0xe5, 0x37, 0xb4, 0x4e, 0x03, 0xd3, 0x80, 0x01, 0xc3,
+0x22, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0b, 0x94, 0xeb, } ;
diff --git a/drivers/staging/rt2860/common/md5.c b/drivers/staging/rt2860/common/md5.c
new file mode 100644
index 000000000000..774776b4b8c3
--- /dev/null
+++ b/drivers/staging/rt2860/common/md5.c
@@ -0,0 +1,1427 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    md5.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	jan			10-28-03		Initial
+	Rita    	11-23-04		Modify MD5 and SHA-1
+	Rita		10-14-05		Modify SHA-1 in big-endian platform
+ */
+#include "../rt_config.h"
+
+/**
+ * md5_mac:
+ * @key: pointer to	the	key	used for MAC generation
+ * @key_len: length	of the key in bytes
+ * @data: pointer to the data area for which the MAC is	generated
+ * @data_len: length of	the	data in	bytes
+ * @mac: pointer to	the	buffer holding space for the MAC; the buffer should
+ * have	space for 128-bit (16 bytes) MD5 hash value
+ *
+ * md5_mac() determines	the	message	authentication code	by using secure	hash
+ * MD5(key | data |	key).
+ */
+void md5_mac(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac)
+{
+	MD5_CTX	context;
+
+	MD5Init(&context);
+	MD5Update(&context,	key, key_len);
+	MD5Update(&context,	data, data_len);
+	MD5Update(&context,	key, key_len);
+	MD5Final(mac, &context);
+}
+
+/**
+ * hmac_md5:
+ * @key: pointer to	the	key	used for MAC generation
+ * @key_len: length	of the key in bytes
+ * @data: pointer to the data area for which the MAC is	generated
+ * @data_len: length of	the	data in	bytes
+ * @mac: pointer to	the	buffer holding space for the MAC; the buffer should
+ * have	space for 128-bit (16 bytes) MD5 hash value
+ *
+ * hmac_md5() determines the message authentication	code using HMAC-MD5.
+ * This	implementation is based	on the sample code presented in	RFC	2104.
+ */
+void hmac_md5(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac)
+{
+	MD5_CTX	context;
+    u8 k_ipad[65]; /* inner padding - key XORd with ipad */
+    u8 k_opad[65]; /* outer padding - key XORd with opad */
+    u8 tk[16];
+	int	i;
+
+	//assert(key != NULL && data != NULL && mac != NULL);
+
+	/* if key is longer	than 64	bytes reset	it to key =	MD5(key) */
+	if (key_len	> 64) {
+		MD5_CTX	ttcontext;
+
+		MD5Init(&ttcontext);
+		MD5Update(&ttcontext, key, key_len);
+		MD5Final(tk, &ttcontext);
+		//key=(PUCHAR)ttcontext.buf;
+		key	= tk;
+		key_len	= 16;
+	}
+
+	/* the HMAC_MD5	transform looks	like:
+	 *
+	 * MD5(K XOR opad, MD5(K XOR ipad, text))
+	 *
+	 * where K is an n byte	key
+	 * ipad	is the byte	0x36 repeated 64 times
+	 * opad	is the byte	0x5c repeated 64 times
+	 * and text	is the data	being protected	*/
+
+	/* start out by	storing	key	in pads	*/
+	NdisZeroMemory(k_ipad, sizeof(k_ipad));
+	NdisZeroMemory(k_opad,	sizeof(k_opad));
+	//assert(key_len < sizeof(k_ipad));
+	NdisMoveMemory(k_ipad, key,	key_len);
+	NdisMoveMemory(k_opad, key,	key_len);
+
+	/* XOR key with	ipad and opad values */
+	for	(i = 0;	i <	64;	i++) {
+		k_ipad[i] ^= 0x36;
+		k_opad[i] ^= 0x5c;
+	}
+
+	/* perform inner MD5 */
+	MD5Init(&context);					 /*	init context for 1st pass */
+	MD5Update(&context,	k_ipad,	64);	 /*	start with inner pad */
+	MD5Update(&context,	data, data_len); /*	then text of datagram */
+	MD5Final(mac, &context);			 /*	finish up 1st pass */
+
+	/* perform outer MD5 */
+	MD5Init(&context);					 /*	init context for 2nd pass */
+	MD5Update(&context,	k_opad,	64);	 /*	start with outer pad */
+	MD5Update(&context,	mac, 16);		 /*	then results of	1st	hash */
+	MD5Final(mac, &context);			 /*	finish up 2nd pass */
+}
+
+#ifndef RT_BIG_ENDIAN
+#define byteReverse(buf, len)   /* Nothing */
+#else
+void byteReverse(unsigned char *buf, unsigned longs);
+void byteReverse(unsigned char *buf, unsigned longs)
+{
+    do {
+        *(UINT32 *)buf = SWAP32(*(UINT32 *)buf);
+        buf += 4;
+    } while (--longs);
+}
+#endif
+
+
+/* ==========================  MD5 implementation =========================== */
+// four base functions for MD5
+#define MD5_F1(x, y, z) (((x) & (y)) | ((~x) & (z)))
+#define MD5_F2(x, y, z) (((x) & (z)) | ((y) & (~z)))
+#define MD5_F3(x, y, z) ((x) ^ (y) ^ (z))
+#define MD5_F4(x, y, z) ((y) ^ ((x) | (~z)))
+#define CYCLIC_LEFT_SHIFT(w, s) (((w) << (s)) | ((w) >> (32-(s))))
+
+#define	MD5Step(f, w, x, y,	z, data, t, s)	\
+	( w	+= f(x,	y, z) +	data + t,  w = (CYCLIC_LEFT_SHIFT(w, s)) & 0xffffffff, w +=	x )
+
+
+/*
+ *  Function Description:
+ *      Initiate MD5 Context satisfied in RFC 1321
+ *
+ *  Arguments:
+ *      pCtx        Pointer	to MD5 context
+ *
+ *  Return Value:
+ *      None
+ */
+VOID MD5Init(MD5_CTX *pCtx)
+{
+    pCtx->Buf[0]=0x67452301;
+    pCtx->Buf[1]=0xefcdab89;
+    pCtx->Buf[2]=0x98badcfe;
+    pCtx->Buf[3]=0x10325476;
+
+    pCtx->LenInBitCount[0]=0;
+    pCtx->LenInBitCount[1]=0;
+}
+
+
+/*
+ *  Function Description:
+ *      Update MD5 Context, allow of an arrary of octets as the next portion
+ *      of the message
+ *
+ *  Arguments:
+ *      pCtx		Pointer	to MD5 context
+ * 	    pData       Pointer to input data
+ *      LenInBytes  The length of input data (unit: byte)
+ *
+ *  Return Value:
+ *      None
+ *
+ *  Note:
+ *      Called after MD5Init or MD5Update(itself)
+ */
+VOID MD5Update(MD5_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes)
+{
+
+    UINT32 TfTimes;
+    UINT32 temp;
+	unsigned int i;
+
+    temp = pCtx->LenInBitCount[0];
+
+    pCtx->LenInBitCount[0] = (UINT32) (pCtx->LenInBitCount[0] + (LenInBytes << 3));
+
+    if (pCtx->LenInBitCount[0] < temp)
+        pCtx->LenInBitCount[1]++;   //carry in
+
+    pCtx->LenInBitCount[1] += LenInBytes >> 29;
+
+    // mod 64 bytes
+    temp = (temp >> 3) & 0x3f;
+
+    // process lacks of 64-byte data
+    if (temp)
+    {
+        UCHAR *pAds = (UCHAR *) pCtx->Input + temp;
+
+        if ((temp+LenInBytes) < 64)
+        {
+            NdisMoveMemory(pAds, (UCHAR *)pData, LenInBytes);
+            return;
+        }
+
+        NdisMoveMemory(pAds, (UCHAR *)pData, 64-temp);
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+        pData += 64-temp;
+        LenInBytes -= 64-temp;
+    } // end of if (temp)
+
+
+    TfTimes = (LenInBytes >> 6);
+
+    for (i=TfTimes; i>0; i--)
+    {
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, 64);
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+        pData += 64;
+        LenInBytes -= 64;
+    } // end of for
+
+    // buffering lacks of 64-byte data
+    if(LenInBytes)
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, LenInBytes);
+
+}
+
+
+/*
+ *  Function Description:
+ *      Append padding bits and length of original message in the tail
+ *      The message digest has to be completed in the end
+ *
+ *  Arguments:
+ *      Digest		Output of Digest-Message for MD5
+ *  	pCtx        Pointer	to MD5 context
+ *
+ *  Return Value:
+ *      None
+ *
+ *  Note:
+ *      Called after MD5Update
+ */
+VOID MD5Final(UCHAR Digest[16], MD5_CTX *pCtx)
+{
+    UCHAR Remainder;
+    UCHAR PadLenInBytes;
+    UCHAR *pAppend=0;
+    unsigned int i;
+
+    Remainder = (UCHAR)((pCtx->LenInBitCount[0] >> 3) & 0x3f);
+
+    PadLenInBytes = (Remainder < 56) ? (56-Remainder) : (120-Remainder);
+
+    pAppend = (UCHAR *)pCtx->Input + Remainder;
+
+    // padding bits without crossing block(64-byte based) boundary
+    if (Remainder < 56)
+    {
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, PadLenInBytes);
+
+		// add data-length field, from low to high
+       	for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[0] >> (i << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[1] >> (i << 3)) & 0xff);
+      	}
+
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of if
+
+    // padding bits with crossing block(64-byte based) boundary
+    else
+    {
+        // the first block ===
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, (64-Remainder-1));
+        PadLenInBytes -= (64 - Remainder - 1);
+
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+
+        // the second block ===
+        NdisZeroMemory((UCHAR *)pCtx->Input, PadLenInBytes);
+
+        // add data-length field
+        for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[0] >> (i << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[1] >> (i << 3)) & 0xff);
+      	}
+
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of else
+
+
+    NdisMoveMemory((UCHAR *)Digest, (UINT32 *)pCtx->Buf, 16); // output
+    byteReverse((UCHAR *)Digest, 4);
+    NdisZeroMemory(pCtx, sizeof(pCtx)); // memory free
+}
+
+
+/*
+ *  Function Description:
+ *      The central algorithm of MD5, consists of four rounds and sixteen
+ *  	steps per round
+ *
+ *  Arguments:
+ *      Buf     Buffers of four states (output: 16 bytes)
+ * 	    Mes     Input data (input: 64 bytes)
+ *
+ *  Return Value:
+ *      None
+ *
+ *  Note:
+ *      Called by MD5Update or MD5Final
+ */
+VOID MD5Transform(UINT32 Buf[4], UINT32 Mes[16])
+{
+    UINT32 Reg[4], Temp;
+	unsigned int i;
+
+    static UCHAR LShiftVal[16] =
+    {
+        7, 12, 17, 22,
+		5, 9 , 14, 20,
+		4, 11, 16, 23,
+ 		6, 10, 15, 21,
+ 	};
+
+
+	// [equal to 4294967296*abs(sin(index))]
+    static UINT32 MD5Table[64] =
+	{
+		0xd76aa478,	0xe8c7b756,	0x242070db,	0xc1bdceee,
+		0xf57c0faf,	0x4787c62a,	0xa8304613, 0xfd469501,
+		0x698098d8,	0x8b44f7af,	0xffff5bb1,	0x895cd7be,
+    	0x6b901122,	0xfd987193,	0xa679438e,	0x49b40821,
+
+    	0xf61e2562,	0xc040b340,	0x265e5a51,	0xe9b6c7aa,
+    	0xd62f105d,	0x02441453,	0xd8a1e681,	0xe7d3fbc8,
+    	0x21e1cde6,	0xc33707d6,	0xf4d50d87,	0x455a14ed,
+    	0xa9e3e905,	0xfcefa3f8,	0x676f02d9,	0x8d2a4c8a,
+
+    	0xfffa3942,	0x8771f681,	0x6d9d6122,	0xfde5380c,
+    	0xa4beea44,	0x4bdecfa9,	0xf6bb4b60,	0xbebfbc70,
+    	0x289b7ec6,	0xeaa127fa,	0xd4ef3085,	0x04881d05,
+    	0xd9d4d039,	0xe6db99e5,	0x1fa27cf8,	0xc4ac5665,
+
+    	0xf4292244,	0x432aff97,	0xab9423a7,	0xfc93a039,
+   		0x655b59c3,	0x8f0ccc92,	0xffeff47d,	0x85845dd1,
+    	0x6fa87e4f,	0xfe2ce6e0,	0xa3014314,	0x4e0811a1,
+    	0xf7537e82,	0xbd3af235,	0x2ad7d2bb,	0xeb86d391
+	};
+
+
+    for (i=0; i<4; i++)
+        Reg[i]=Buf[i];
+
+
+    // 64 steps in MD5 algorithm
+    for (i=0; i<16; i++)
+    {
+        MD5Step(MD5_F1, Reg[0], Reg[1], Reg[2], Reg[3], Mes[i],
+                MD5Table[i], LShiftVal[i & 0x3]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+    for (i=16; i<32; i++)
+    {
+        MD5Step(MD5_F2, Reg[0], Reg[1], Reg[2], Reg[3], Mes[(5*(i & 0xf)+1) & 0xf],
+                MD5Table[i], LShiftVal[(0x1 << 2)+(i & 0x3)]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+    for (i=32; i<48; i++)
+    {
+        MD5Step(MD5_F3, Reg[0], Reg[1], Reg[2], Reg[3], Mes[(3*(i & 0xf)+5) & 0xf],
+                MD5Table[i], LShiftVal[(0x1 << 3)+(i & 0x3)]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+    for (i=48; i<64; i++)
+    {
+        MD5Step(MD5_F4, Reg[0], Reg[1], Reg[2], Reg[3], Mes[(7*(i & 0xf)) & 0xf],
+                MD5Table[i], LShiftVal[(0x3 << 2)+(i & 0x3)]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+
+
+    // (temporary)output
+    for (i=0; i<4; i++)
+        Buf[i] += Reg[i];
+
+}
+
+
+
+/* =========================  SHA-1 implementation ========================== */
+// four base functions for SHA-1
+#define SHA1_F1(b, c, d)    (((b) & (c)) | ((~b) & (d)))
+#define SHA1_F2(b, c, d)    ((b) ^ (c) ^ (d))
+#define SHA1_F3(b, c, d)    (((b) & (c)) | ((b) & (d)) | ((c) & (d)))
+
+
+#define SHA1Step(f, a, b, c, d, e, w, k)    \
+    ( e	+= ( f(b, c, d) + w + k + CYCLIC_LEFT_SHIFT(a, 5)) & 0xffffffff, \
+      b = CYCLIC_LEFT_SHIFT(b, 30) )
+
+//Initiate SHA-1 Context satisfied in RFC 3174
+VOID SHAInit(SHA_CTX *pCtx)
+{
+    pCtx->Buf[0]=0x67452301;
+    pCtx->Buf[1]=0xefcdab89;
+    pCtx->Buf[2]=0x98badcfe;
+    pCtx->Buf[3]=0x10325476;
+    pCtx->Buf[4]=0xc3d2e1f0;
+
+    pCtx->LenInBitCount[0]=0;
+    pCtx->LenInBitCount[1]=0;
+}
+
+/*
+ *  Function Description:
+ *      Update SHA-1 Context, allow of an arrary of octets as the next
+ *      portion of the message
+ *
+ *  Arguments:
+ *      pCtx		Pointer	to SHA-1 context
+ * 	    pData       Pointer to input data
+ *      LenInBytes  The length of input data (unit: byte)
+ *
+ *  Return Value:
+ *      error       indicate more than pow(2,64) bits of data
+ *
+ *  Note:
+ *      Called after SHAInit or SHAUpdate(itself)
+ */
+UCHAR SHAUpdate(SHA_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes)
+{
+    UINT32 TfTimes;
+    UINT32 temp1,temp2;
+	unsigned int i;
+	UCHAR err=1;
+
+    temp1 = pCtx->LenInBitCount[0];
+    temp2 = pCtx->LenInBitCount[1];
+
+    pCtx->LenInBitCount[0] = (UINT32) (pCtx->LenInBitCount[0] + (LenInBytes << 3));
+    if (pCtx->LenInBitCount[0] < temp1)
+        pCtx->LenInBitCount[1]++;   //carry in
+
+
+    pCtx->LenInBitCount[1] = (UINT32) (pCtx->LenInBitCount[1] +(LenInBytes >> 29));
+    if (pCtx->LenInBitCount[1] < temp2)
+        return (err);   //check total length of original data
+
+
+    // mod 64 bytes
+    temp1 = (temp1 >> 3) & 0x3f;
+
+    // process lacks of 64-byte data
+    if (temp1)
+    {
+        UCHAR *pAds = (UCHAR *) pCtx->Input + temp1;
+
+        if ((temp1+LenInBytes) < 64)
+        {
+            NdisMoveMemory(pAds, (UCHAR *)pData, LenInBytes);
+            return (0);
+        }
+
+        NdisMoveMemory(pAds, (UCHAR *)pData, 64-temp1);
+        byteReverse((UCHAR *)pCtx->Input, 16);
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+        pData += 64-temp1;
+        LenInBytes -= 64-temp1;
+    } // end of if (temp1)
+
+
+    TfTimes = (LenInBytes >> 6);
+
+    for (i=TfTimes; i>0; i--)
+    {
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, 64);
+        byteReverse((UCHAR *)pCtx->Input, 16);
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+        pData += 64;
+        LenInBytes -= 64;
+    } // end of for
+
+    // buffering lacks of 64-byte data
+    if(LenInBytes)
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, LenInBytes);
+
+	return (0);
+
+}
+
+// Append padding bits and length of original message in the tail
+// The message digest has to be completed in the end
+VOID SHAFinal(SHA_CTX *pCtx, UCHAR Digest[20])
+{
+    UCHAR Remainder;
+    UCHAR PadLenInBytes;
+    UCHAR *pAppend=0;
+    unsigned int i;
+
+    Remainder = (UCHAR)((pCtx->LenInBitCount[0] >> 3) & 0x3f);
+
+    pAppend = (UCHAR *)pCtx->Input + Remainder;
+
+    PadLenInBytes = (Remainder < 56) ? (56-Remainder) : (120-Remainder);
+
+    // padding bits without crossing block(64-byte based) boundary
+    if (Remainder < 56)
+    {
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, PadLenInBytes);
+
+		// add data-length field, from high to low
+        for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[1] >> ((3-i) << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[0] >> ((3-i) << 3)) & 0xff);
+      	}
+
+        byteReverse((UCHAR *)pCtx->Input, 16);
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 14);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of if
+
+    // padding bits with crossing block(64-byte based) boundary
+    else
+    {
+        // the first block ===
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, (64-Remainder-1));
+        PadLenInBytes -= (64 - Remainder - 1);
+
+        byteReverse((UCHAR *)pCtx->Input, 16);
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+
+        // the second block ===
+        NdisZeroMemory((UCHAR *)pCtx->Input, PadLenInBytes);
+
+		// add data-length field
+		for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[1] >> ((3-i) << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[0] >> ((3-i) << 3)) & 0xff);
+      	}
+
+        byteReverse((UCHAR *)pCtx->Input, 16);
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of else
+
+
+    //Output, bytereverse
+    for (i=0; i<20; i++)
+    {
+        Digest [i] = (UCHAR)(pCtx->Buf[i>>2] >> 8*(3-(i & 0x3)));
+    }
+
+    NdisZeroMemory(pCtx, sizeof(pCtx)); // memory free
+}
+
+
+// The central algorithm of SHA-1, consists of four rounds and
+// twenty steps per round
+VOID SHATransform(UINT32 Buf[5], UINT32 Mes[20])
+{
+    UINT32 Reg[5],Temp;
+	unsigned int i;
+    UINT32 W[80];
+
+    static UINT32 SHA1Table[4] = { 0x5a827999, 0x6ed9eba1,
+                                  0x8f1bbcdc, 0xca62c1d6 };
+
+    Reg[0]=Buf[0];
+	Reg[1]=Buf[1];
+	Reg[2]=Buf[2];
+	Reg[3]=Buf[3];
+	Reg[4]=Buf[4];
+
+    //the first octet of a word is stored in the 0th element, bytereverse
+	for(i = 0; i < 16; i++)
+    {
+    	W[i]  = (Mes[i] >> 24) & 0xff;
+        W[i] |= (Mes[i] >> 8 ) & 0xff00;
+        W[i] |= (Mes[i] << 8 ) & 0xff0000;
+        W[i] |= (Mes[i] << 24) & 0xff000000;
+    }
+
+
+    for	(i = 0; i < 64; i++)
+	    W[16+i] = CYCLIC_LEFT_SHIFT(W[i] ^ W[2+i] ^ W[8+i] ^ W[13+i], 1);
+
+
+    // 80 steps in SHA-1 algorithm
+    for (i=0; i<80; i++)
+    {
+        if (i<20)
+            SHA1Step(SHA1_F1, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                     W[i], SHA1Table[0]);
+
+        else if (i>=20 && i<40)
+            SHA1Step(SHA1_F2, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                     W[i], SHA1Table[1]);
+
+		else if (i>=40 && i<60)
+            SHA1Step(SHA1_F3, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                      W[i], SHA1Table[2]);
+
+        else
+            SHA1Step(SHA1_F2, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                     W[i], SHA1Table[3]);
+
+
+       // one-word right shift
+		Temp   = Reg[4];
+        Reg[4] = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+
+    } // end of for-loop
+
+
+    // (temporary)output
+    for (i=0; i<5; i++)
+        Buf[i] += Reg[i];
+
+}
+
+
+/* =========================  AES En/Decryption ========================== */
+
+/* forward S-box */
+static uint32 FSb[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
+};
+
+/* forward table */
+#define	FT \
+\
+	V(C6,63,63,A5),	V(F8,7C,7C,84),	V(EE,77,77,99),	V(F6,7B,7B,8D),	\
+	V(FF,F2,F2,0D),	V(D6,6B,6B,BD),	V(DE,6F,6F,B1),	V(91,C5,C5,54),	\
+	V(60,30,30,50),	V(02,01,01,03),	V(CE,67,67,A9),	V(56,2B,2B,7D),	\
+	V(E7,FE,FE,19),	V(B5,D7,D7,62),	V(4D,AB,AB,E6),	V(EC,76,76,9A),	\
+	V(8F,CA,CA,45),	V(1F,82,82,9D),	V(89,C9,C9,40),	V(FA,7D,7D,87),	\
+	V(EF,FA,FA,15),	V(B2,59,59,EB),	V(8E,47,47,C9),	V(FB,F0,F0,0B),	\
+	V(41,AD,AD,EC),	V(B3,D4,D4,67),	V(5F,A2,A2,FD),	V(45,AF,AF,EA),	\
+	V(23,9C,9C,BF),	V(53,A4,A4,F7),	V(E4,72,72,96),	V(9B,C0,C0,5B),	\
+	V(75,B7,B7,C2),	V(E1,FD,FD,1C),	V(3D,93,93,AE),	V(4C,26,26,6A),	\
+	V(6C,36,36,5A),	V(7E,3F,3F,41),	V(F5,F7,F7,02),	V(83,CC,CC,4F),	\
+	V(68,34,34,5C),	V(51,A5,A5,F4),	V(D1,E5,E5,34),	V(F9,F1,F1,08),	\
+	V(E2,71,71,93),	V(AB,D8,D8,73),	V(62,31,31,53),	V(2A,15,15,3F),	\
+	V(08,04,04,0C),	V(95,C7,C7,52),	V(46,23,23,65),	V(9D,C3,C3,5E),	\
+	V(30,18,18,28),	V(37,96,96,A1),	V(0A,05,05,0F),	V(2F,9A,9A,B5),	\
+	V(0E,07,07,09),	V(24,12,12,36),	V(1B,80,80,9B),	V(DF,E2,E2,3D),	\
+	V(CD,EB,EB,26),	V(4E,27,27,69),	V(7F,B2,B2,CD),	V(EA,75,75,9F),	\
+	V(12,09,09,1B),	V(1D,83,83,9E),	V(58,2C,2C,74),	V(34,1A,1A,2E),	\
+	V(36,1B,1B,2D),	V(DC,6E,6E,B2),	V(B4,5A,5A,EE),	V(5B,A0,A0,FB),	\
+	V(A4,52,52,F6),	V(76,3B,3B,4D),	V(B7,D6,D6,61),	V(7D,B3,B3,CE),	\
+	V(52,29,29,7B),	V(DD,E3,E3,3E),	V(5E,2F,2F,71),	V(13,84,84,97),	\
+	V(A6,53,53,F5),	V(B9,D1,D1,68),	V(00,00,00,00),	V(C1,ED,ED,2C),	\
+	V(40,20,20,60),	V(E3,FC,FC,1F),	V(79,B1,B1,C8),	V(B6,5B,5B,ED),	\
+	V(D4,6A,6A,BE),	V(8D,CB,CB,46),	V(67,BE,BE,D9),	V(72,39,39,4B),	\
+	V(94,4A,4A,DE),	V(98,4C,4C,D4),	V(B0,58,58,E8),	V(85,CF,CF,4A),	\
+	V(BB,D0,D0,6B),	V(C5,EF,EF,2A),	V(4F,AA,AA,E5),	V(ED,FB,FB,16),	\
+	V(86,43,43,C5),	V(9A,4D,4D,D7),	V(66,33,33,55),	V(11,85,85,94),	\
+	V(8A,45,45,CF),	V(E9,F9,F9,10),	V(04,02,02,06),	V(FE,7F,7F,81),	\
+	V(A0,50,50,F0),	V(78,3C,3C,44),	V(25,9F,9F,BA),	V(4B,A8,A8,E3),	\
+	V(A2,51,51,F3),	V(5D,A3,A3,FE),	V(80,40,40,C0),	V(05,8F,8F,8A),	\
+	V(3F,92,92,AD),	V(21,9D,9D,BC),	V(70,38,38,48),	V(F1,F5,F5,04),	\
+	V(63,BC,BC,DF),	V(77,B6,B6,C1),	V(AF,DA,DA,75),	V(42,21,21,63),	\
+	V(20,10,10,30),	V(E5,FF,FF,1A),	V(FD,F3,F3,0E),	V(BF,D2,D2,6D),	\
+	V(81,CD,CD,4C),	V(18,0C,0C,14),	V(26,13,13,35),	V(C3,EC,EC,2F),	\
+	V(BE,5F,5F,E1),	V(35,97,97,A2),	V(88,44,44,CC),	V(2E,17,17,39),	\
+	V(93,C4,C4,57),	V(55,A7,A7,F2),	V(FC,7E,7E,82),	V(7A,3D,3D,47),	\
+	V(C8,64,64,AC),	V(BA,5D,5D,E7),	V(32,19,19,2B),	V(E6,73,73,95),	\
+	V(C0,60,60,A0),	V(19,81,81,98),	V(9E,4F,4F,D1),	V(A3,DC,DC,7F),	\
+	V(44,22,22,66),	V(54,2A,2A,7E),	V(3B,90,90,AB),	V(0B,88,88,83),	\
+	V(8C,46,46,CA),	V(C7,EE,EE,29),	V(6B,B8,B8,D3),	V(28,14,14,3C),	\
+	V(A7,DE,DE,79),	V(BC,5E,5E,E2),	V(16,0B,0B,1D),	V(AD,DB,DB,76),	\
+	V(DB,E0,E0,3B),	V(64,32,32,56),	V(74,3A,3A,4E),	V(14,0A,0A,1E),	\
+	V(92,49,49,DB),	V(0C,06,06,0A),	V(48,24,24,6C),	V(B8,5C,5C,E4),	\
+	V(9F,C2,C2,5D),	V(BD,D3,D3,6E),	V(43,AC,AC,EF),	V(C4,62,62,A6),	\
+	V(39,91,91,A8),	V(31,95,95,A4),	V(D3,E4,E4,37),	V(F2,79,79,8B),	\
+	V(D5,E7,E7,32),	V(8B,C8,C8,43),	V(6E,37,37,59),	V(DA,6D,6D,B7),	\
+	V(01,8D,8D,8C),	V(B1,D5,D5,64),	V(9C,4E,4E,D2),	V(49,A9,A9,E0),	\
+	V(D8,6C,6C,B4),	V(AC,56,56,FA),	V(F3,F4,F4,07),	V(CF,EA,EA,25),	\
+	V(CA,65,65,AF),	V(F4,7A,7A,8E),	V(47,AE,AE,E9),	V(10,08,08,18),	\
+	V(6F,BA,BA,D5),	V(F0,78,78,88),	V(4A,25,25,6F),	V(5C,2E,2E,72),	\
+	V(38,1C,1C,24),	V(57,A6,A6,F1),	V(73,B4,B4,C7),	V(97,C6,C6,51),	\
+	V(CB,E8,E8,23),	V(A1,DD,DD,7C),	V(E8,74,74,9C),	V(3E,1F,1F,21),	\
+	V(96,4B,4B,DD),	V(61,BD,BD,DC),	V(0D,8B,8B,86),	V(0F,8A,8A,85),	\
+	V(E0,70,70,90),	V(7C,3E,3E,42),	V(71,B5,B5,C4),	V(CC,66,66,AA),	\
+	V(90,48,48,D8),	V(06,03,03,05),	V(F7,F6,F6,01),	V(1C,0E,0E,12),	\
+	V(C2,61,61,A3),	V(6A,35,35,5F),	V(AE,57,57,F9),	V(69,B9,B9,D0),	\
+	V(17,86,86,91),	V(99,C1,C1,58),	V(3A,1D,1D,27),	V(27,9E,9E,B9),	\
+	V(D9,E1,E1,38),	V(EB,F8,F8,13),	V(2B,98,98,B3),	V(22,11,11,33),	\
+	V(D2,69,69,BB),	V(A9,D9,D9,70),	V(07,8E,8E,89),	V(33,94,94,A7),	\
+	V(2D,9B,9B,B6),	V(3C,1E,1E,22),	V(15,87,87,92),	V(C9,E9,E9,20),	\
+	V(87,CE,CE,49),	V(AA,55,55,FF),	V(50,28,28,78),	V(A5,DF,DF,7A),	\
+	V(03,8C,8C,8F),	V(59,A1,A1,F8),	V(09,89,89,80),	V(1A,0D,0D,17),	\
+	V(65,BF,BF,DA),	V(D7,E6,E6,31),	V(84,42,42,C6),	V(D0,68,68,B8),	\
+	V(82,41,41,C3),	V(29,99,99,B0),	V(5A,2D,2D,77),	V(1E,0F,0F,11),	\
+	V(7B,B0,B0,CB),	V(A8,54,54,FC),	V(6D,BB,BB,D6),	V(2C,16,16,3A)
+
+#define	V(a,b,c,d) 0x##a##b##c##d
+static uint32 FT0[256] = { FT };
+#undef V
+
+#define	V(a,b,c,d) 0x##d##a##b##c
+static uint32 FT1[256] = { FT };
+#undef V
+
+#define	V(a,b,c,d) 0x##c##d##a##b
+static uint32 FT2[256] = { FT };
+#undef V
+
+#define	V(a,b,c,d) 0x##b##c##d##a
+static uint32 FT3[256] = { FT };
+#undef V
+
+#undef FT
+
+/* reverse S-box */
+
+static uint32 RSb[256] =
+{
+	0x52, 0x09,	0x6A, 0xD5,	0x30, 0x36,	0xA5, 0x38,
+	0xBF, 0x40,	0xA3, 0x9E,	0x81, 0xF3,	0xD7, 0xFB,
+	0x7C, 0xE3,	0x39, 0x82,	0x9B, 0x2F,	0xFF, 0x87,
+	0x34, 0x8E,	0x43, 0x44,	0xC4, 0xDE,	0xE9, 0xCB,
+	0x54, 0x7B,	0x94, 0x32,	0xA6, 0xC2,	0x23, 0x3D,
+	0xEE, 0x4C,	0x95, 0x0B,	0x42, 0xFA,	0xC3, 0x4E,
+	0x08, 0x2E,	0xA1, 0x66,	0x28, 0xD9,	0x24, 0xB2,
+	0x76, 0x5B,	0xA2, 0x49,	0x6D, 0x8B,	0xD1, 0x25,
+	0x72, 0xF8,	0xF6, 0x64,	0x86, 0x68,	0x98, 0x16,
+	0xD4, 0xA4,	0x5C, 0xCC,	0x5D, 0x65,	0xB6, 0x92,
+	0x6C, 0x70,	0x48, 0x50,	0xFD, 0xED,	0xB9, 0xDA,
+	0x5E, 0x15,	0x46, 0x57,	0xA7, 0x8D,	0x9D, 0x84,
+	0x90, 0xD8,	0xAB, 0x00,	0x8C, 0xBC,	0xD3, 0x0A,
+	0xF7, 0xE4,	0x58, 0x05,	0xB8, 0xB3,	0x45, 0x06,
+	0xD0, 0x2C,	0x1E, 0x8F,	0xCA, 0x3F,	0x0F, 0x02,
+	0xC1, 0xAF,	0xBD, 0x03,	0x01, 0x13,	0x8A, 0x6B,
+	0x3A, 0x91,	0x11, 0x41,	0x4F, 0x67,	0xDC, 0xEA,
+	0x97, 0xF2,	0xCF, 0xCE,	0xF0, 0xB4,	0xE6, 0x73,
+	0x96, 0xAC,	0x74, 0x22,	0xE7, 0xAD,	0x35, 0x85,
+	0xE2, 0xF9,	0x37, 0xE8,	0x1C, 0x75,	0xDF, 0x6E,
+	0x47, 0xF1,	0x1A, 0x71,	0x1D, 0x29,	0xC5, 0x89,
+	0x6F, 0xB7,	0x62, 0x0E,	0xAA, 0x18,	0xBE, 0x1B,
+	0xFC, 0x56,	0x3E, 0x4B,	0xC6, 0xD2,	0x79, 0x20,
+	0x9A, 0xDB,	0xC0, 0xFE,	0x78, 0xCD,	0x5A, 0xF4,
+	0x1F, 0xDD,	0xA8, 0x33,	0x88, 0x07,	0xC7, 0x31,
+	0xB1, 0x12,	0x10, 0x59,	0x27, 0x80,	0xEC, 0x5F,
+	0x60, 0x51,	0x7F, 0xA9,	0x19, 0xB5,	0x4A, 0x0D,
+	0x2D, 0xE5,	0x7A, 0x9F,	0x93, 0xC9,	0x9C, 0xEF,
+	0xA0, 0xE0,	0x3B, 0x4D,	0xAE, 0x2A,	0xF5, 0xB0,
+	0xC8, 0xEB,	0xBB, 0x3C,	0x83, 0x53,	0x99, 0x61,
+	0x17, 0x2B,	0x04, 0x7E,	0xBA, 0x77,	0xD6, 0x26,
+	0xE1, 0x69,	0x14, 0x63,	0x55, 0x21,	0x0C, 0x7D
+};
+
+/* reverse table */
+
+#define	RT \
+\
+	V(51,F4,A7,50),	V(7E,41,65,53),	V(1A,17,A4,C3),	V(3A,27,5E,96),	\
+	V(3B,AB,6B,CB),	V(1F,9D,45,F1),	V(AC,FA,58,AB),	V(4B,E3,03,93),	\
+	V(20,30,FA,55),	V(AD,76,6D,F6),	V(88,CC,76,91),	V(F5,02,4C,25),	\
+	V(4F,E5,D7,FC),	V(C5,2A,CB,D7),	V(26,35,44,80),	V(B5,62,A3,8F),	\
+	V(DE,B1,5A,49),	V(25,BA,1B,67),	V(45,EA,0E,98),	V(5D,FE,C0,E1),	\
+	V(C3,2F,75,02),	V(81,4C,F0,12),	V(8D,46,97,A3),	V(6B,D3,F9,C6),	\
+	V(03,8F,5F,E7),	V(15,92,9C,95),	V(BF,6D,7A,EB),	V(95,52,59,DA),	\
+	V(D4,BE,83,2D),	V(58,74,21,D3),	V(49,E0,69,29),	V(8E,C9,C8,44),	\
+	V(75,C2,89,6A),	V(F4,8E,79,78),	V(99,58,3E,6B),	V(27,B9,71,DD),	\
+	V(BE,E1,4F,B6),	V(F0,88,AD,17),	V(C9,20,AC,66),	V(7D,CE,3A,B4),	\
+	V(63,DF,4A,18),	V(E5,1A,31,82),	V(97,51,33,60),	V(62,53,7F,45),	\
+	V(B1,64,77,E0),	V(BB,6B,AE,84),	V(FE,81,A0,1C),	V(F9,08,2B,94),	\
+	V(70,48,68,58),	V(8F,45,FD,19),	V(94,DE,6C,87),	V(52,7B,F8,B7),	\
+	V(AB,73,D3,23),	V(72,4B,02,E2),	V(E3,1F,8F,57),	V(66,55,AB,2A),	\
+	V(B2,EB,28,07),	V(2F,B5,C2,03),	V(86,C5,7B,9A),	V(D3,37,08,A5),	\
+	V(30,28,87,F2),	V(23,BF,A5,B2),	V(02,03,6A,BA),	V(ED,16,82,5C),	\
+	V(8A,CF,1C,2B),	V(A7,79,B4,92),	V(F3,07,F2,F0),	V(4E,69,E2,A1),	\
+	V(65,DA,F4,CD),	V(06,05,BE,D5),	V(D1,34,62,1F),	V(C4,A6,FE,8A),	\
+	V(34,2E,53,9D),	V(A2,F3,55,A0),	V(05,8A,E1,32),	V(A4,F6,EB,75),	\
+	V(0B,83,EC,39),	V(40,60,EF,AA),	V(5E,71,9F,06),	V(BD,6E,10,51),	\
+	V(3E,21,8A,F9),	V(96,DD,06,3D),	V(DD,3E,05,AE),	V(4D,E6,BD,46),	\
+	V(91,54,8D,B5),	V(71,C4,5D,05),	V(04,06,D4,6F),	V(60,50,15,FF),	\
+	V(19,98,FB,24),	V(D6,BD,E9,97),	V(89,40,43,CC),	V(67,D9,9E,77),	\
+	V(B0,E8,42,BD),	V(07,89,8B,88),	V(E7,19,5B,38),	V(79,C8,EE,DB),	\
+	V(A1,7C,0A,47),	V(7C,42,0F,E9),	V(F8,84,1E,C9),	V(00,00,00,00),	\
+	V(09,80,86,83),	V(32,2B,ED,48),	V(1E,11,70,AC),	V(6C,5A,72,4E),	\
+	V(FD,0E,FF,FB),	V(0F,85,38,56),	V(3D,AE,D5,1E),	V(36,2D,39,27),	\
+	V(0A,0F,D9,64),	V(68,5C,A6,21),	V(9B,5B,54,D1),	V(24,36,2E,3A),	\
+	V(0C,0A,67,B1),	V(93,57,E7,0F),	V(B4,EE,96,D2),	V(1B,9B,91,9E),	\
+	V(80,C0,C5,4F),	V(61,DC,20,A2),	V(5A,77,4B,69),	V(1C,12,1A,16),	\
+	V(E2,93,BA,0A),	V(C0,A0,2A,E5),	V(3C,22,E0,43),	V(12,1B,17,1D),	\
+	V(0E,09,0D,0B),	V(F2,8B,C7,AD),	V(2D,B6,A8,B9),	V(14,1E,A9,C8),	\
+	V(57,F1,19,85),	V(AF,75,07,4C),	V(EE,99,DD,BB),	V(A3,7F,60,FD),	\
+	V(F7,01,26,9F),	V(5C,72,F5,BC),	V(44,66,3B,C5),	V(5B,FB,7E,34),	\
+	V(8B,43,29,76),	V(CB,23,C6,DC),	V(B6,ED,FC,68),	V(B8,E4,F1,63),	\
+	V(D7,31,DC,CA),	V(42,63,85,10),	V(13,97,22,40),	V(84,C6,11,20),	\
+	V(85,4A,24,7D),	V(D2,BB,3D,F8),	V(AE,F9,32,11),	V(C7,29,A1,6D),	\
+	V(1D,9E,2F,4B),	V(DC,B2,30,F3),	V(0D,86,52,EC),	V(77,C1,E3,D0),	\
+	V(2B,B3,16,6C),	V(A9,70,B9,99),	V(11,94,48,FA),	V(47,E9,64,22),	\
+	V(A8,FC,8C,C4),	V(A0,F0,3F,1A),	V(56,7D,2C,D8),	V(22,33,90,EF),	\
+	V(87,49,4E,C7),	V(D9,38,D1,C1),	V(8C,CA,A2,FE),	V(98,D4,0B,36),	\
+	V(A6,F5,81,CF),	V(A5,7A,DE,28),	V(DA,B7,8E,26),	V(3F,AD,BF,A4),	\
+	V(2C,3A,9D,E4),	V(50,78,92,0D),	V(6A,5F,CC,9B),	V(54,7E,46,62),	\
+	V(F6,8D,13,C2),	V(90,D8,B8,E8),	V(2E,39,F7,5E),	V(82,C3,AF,F5),	\
+	V(9F,5D,80,BE),	V(69,D0,93,7C),	V(6F,D5,2D,A9),	V(CF,25,12,B3),	\
+	V(C8,AC,99,3B),	V(10,18,7D,A7),	V(E8,9C,63,6E),	V(DB,3B,BB,7B),	\
+	V(CD,26,78,09),	V(6E,59,18,F4),	V(EC,9A,B7,01),	V(83,4F,9A,A8),	\
+	V(E6,95,6E,65),	V(AA,FF,E6,7E),	V(21,BC,CF,08),	V(EF,15,E8,E6),	\
+	V(BA,E7,9B,D9),	V(4A,6F,36,CE),	V(EA,9F,09,D4),	V(29,B0,7C,D6),	\
+	V(31,A4,B2,AF),	V(2A,3F,23,31),	V(C6,A5,94,30),	V(35,A2,66,C0),	\
+	V(74,4E,BC,37),	V(FC,82,CA,A6),	V(E0,90,D0,B0),	V(33,A7,D8,15),	\
+	V(F1,04,98,4A),	V(41,EC,DA,F7),	V(7F,CD,50,0E),	V(17,91,F6,2F),	\
+	V(76,4D,D6,8D),	V(43,EF,B0,4D),	V(CC,AA,4D,54),	V(E4,96,04,DF),	\
+	V(9E,D1,B5,E3),	V(4C,6A,88,1B),	V(C1,2C,1F,B8),	V(46,65,51,7F),	\
+	V(9D,5E,EA,04),	V(01,8C,35,5D),	V(FA,87,74,73),	V(FB,0B,41,2E),	\
+	V(B3,67,1D,5A),	V(92,DB,D2,52),	V(E9,10,56,33),	V(6D,D6,47,13),	\
+	V(9A,D7,61,8C),	V(37,A1,0C,7A),	V(59,F8,14,8E),	V(EB,13,3C,89),	\
+	V(CE,A9,27,EE),	V(B7,61,C9,35),	V(E1,1C,E5,ED),	V(7A,47,B1,3C),	\
+	V(9C,D2,DF,59),	V(55,F2,73,3F),	V(18,14,CE,79),	V(73,C7,37,BF),	\
+	V(53,F7,CD,EA),	V(5F,FD,AA,5B),	V(DF,3D,6F,14),	V(78,44,DB,86),	\
+	V(CA,AF,F3,81),	V(B9,68,C4,3E),	V(38,24,34,2C),	V(C2,A3,40,5F),	\
+	V(16,1D,C3,72),	V(BC,E2,25,0C),	V(28,3C,49,8B),	V(FF,0D,95,41),	\
+	V(39,A8,01,71),	V(08,0C,B3,DE),	V(D8,B4,E4,9C),	V(64,56,C1,90),	\
+	V(7B,CB,84,61),	V(D5,32,B6,70),	V(48,6C,5C,74),	V(D0,B8,57,42)
+
+#define	V(a,b,c,d) 0x##a##b##c##d
+static uint32 RT0[256] = { RT };
+#undef V
+
+#define	V(a,b,c,d) 0x##d##a##b##c
+static uint32 RT1[256] = { RT };
+#undef V
+
+#define	V(a,b,c,d) 0x##c##d##a##b
+static uint32 RT2[256] = { RT };
+#undef V
+
+#define	V(a,b,c,d) 0x##b##c##d##a
+static uint32 RT3[256] = { RT };
+#undef V
+
+#undef RT
+
+/* round constants */
+
+static uint32 RCON[10] =
+{
+	0x01000000,	0x02000000,	0x04000000,	0x08000000,
+	0x10000000,	0x20000000,	0x40000000,	0x80000000,
+	0x1B000000,	0x36000000
+};
+
+/* key schedule	tables */
+
+static int KT_init = 1;
+
+static uint32 KT0[256];
+static uint32 KT1[256];
+static uint32 KT2[256];
+static uint32 KT3[256];
+
+/* platform-independant	32-bit integer manipulation	macros */
+
+#define	GET_UINT32(n,b,i)						\
+{												\
+	(n)	= (	(uint32) (b)[(i)	] << 24	)		\
+		| (	(uint32) (b)[(i) + 1] << 16	)		\
+		| (	(uint32) (b)[(i) + 2] <<  8	)		\
+		| (	(uint32) (b)[(i) + 3]		);		\
+}
+
+#define	PUT_UINT32(n,b,i)						\
+{												\
+	(b)[(i)	   ] = (uint8) ( (n) >>	24 );		\
+	(b)[(i)	+ 1] = (uint8) ( (n) >>	16 );		\
+	(b)[(i)	+ 2] = (uint8) ( (n) >>	 8 );		\
+	(b)[(i)	+ 3] = (uint8) ( (n)	   );		\
+}
+
+/* AES key scheduling routine */
+
+int	rtmp_aes_set_key( aes_context *ctx, uint8 *key, int nbits )
+{
+	int	i;
+	uint32 *RK,	*SK;
+
+	switch(	nbits )
+	{
+		case 128: ctx->nr =	10;	break;
+		case 192: ctx->nr =	12;	break;
+		case 256: ctx->nr =	14;	break;
+		default	: return( 1	);
+	}
+
+	RK = ctx->erk;
+
+	for( i = 0;	i <	(nbits >> 5); i++ )
+	{
+		GET_UINT32(	RK[i], key,	i *	4 );
+	}
+
+	/* setup encryption	round keys */
+
+	switch(	nbits )
+	{
+	case 128:
+
+		for( i = 0;	i <	10;	i++, RK	+= 4 )
+		{
+			RK[4]  = RK[0] ^ RCON[i] ^
+						( FSb[ (uint8) ( RK[3] >> 16 ) ] <<	24 ) ^
+						( FSb[ (uint8) ( RK[3] >>  8 ) ] <<	16 ) ^
+						( FSb[ (uint8) ( RK[3]		 ) ] <<	 8 ) ^
+						( FSb[ (uint8) ( RK[3] >> 24 ) ]	   );
+
+			RK[5]  = RK[1] ^ RK[4];
+			RK[6]  = RK[2] ^ RK[5];
+			RK[7]  = RK[3] ^ RK[6];
+		}
+		break;
+
+	case 192:
+
+		for( i = 0;	i <	8; i++,	RK += 6	)
+		{
+			RK[6]  = RK[0] ^ RCON[i] ^
+						( FSb[ (uint8) ( RK[5] >> 16 ) ] <<	24 ) ^
+						( FSb[ (uint8) ( RK[5] >>  8 ) ] <<	16 ) ^
+						( FSb[ (uint8) ( RK[5]		 ) ] <<	 8 ) ^
+						( FSb[ (uint8) ( RK[5] >> 24 ) ]	   );
+
+			RK[7]  = RK[1] ^ RK[6];
+			RK[8]  = RK[2] ^ RK[7];
+			RK[9]  = RK[3] ^ RK[8];
+			RK[10] = RK[4] ^ RK[9];
+			RK[11] = RK[5] ^ RK[10];
+		}
+		break;
+
+	case 256:
+
+		for( i = 0;	i <	7; i++,	RK += 8	)
+		{
+			RK[8]  = RK[0] ^ RCON[i] ^
+						( FSb[ (uint8) ( RK[7] >> 16 ) ] <<	24 ) ^
+						( FSb[ (uint8) ( RK[7] >>  8 ) ] <<	16 ) ^
+						( FSb[ (uint8) ( RK[7]		 ) ] <<	 8 ) ^
+						( FSb[ (uint8) ( RK[7] >> 24 ) ]	   );
+
+			RK[9]  = RK[1] ^ RK[8];
+			RK[10] = RK[2] ^ RK[9];
+			RK[11] = RK[3] ^ RK[10];
+
+			RK[12] = RK[4] ^
+						( FSb[ (uint8) ( RK[11]	>> 24 )	] << 24	) ^
+						( FSb[ (uint8) ( RK[11]	>> 16 )	] << 16	) ^
+						( FSb[ (uint8) ( RK[11]	>>	8 )	] <<  8	) ^
+						( FSb[ (uint8) ( RK[11]		  )	]		);
+
+			RK[13] = RK[5] ^ RK[12];
+			RK[14] = RK[6] ^ RK[13];
+			RK[15] = RK[7] ^ RK[14];
+		}
+		break;
+	}
+
+	/* setup decryption	round keys */
+
+	if(	KT_init	)
+	{
+		for( i = 0;	i <	256; i++ )
+		{
+			KT0[i] = RT0[ FSb[i] ];
+			KT1[i] = RT1[ FSb[i] ];
+			KT2[i] = RT2[ FSb[i] ];
+			KT3[i] = RT3[ FSb[i] ];
+		}
+
+		KT_init	= 0;
+	}
+
+	SK = ctx->drk;
+
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+
+	for( i = 1;	i <	ctx->nr; i++ )
+	{
+		RK -= 8;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+	}
+
+	RK -= 8;
+
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+
+	return(	0 );
+}
+
+/* AES 128-bit block encryption	routine	*/
+
+void rtmp_aes_encrypt(aes_context *ctx, uint8 input[16],	uint8 output[16] )
+{
+	uint32 *RK,	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3;
+
+	RK = ctx->erk;
+	GET_UINT32(	X0,	input,	0 ); X0	^= RK[0];
+	GET_UINT32(	X1,	input,	4 ); X1	^= RK[1];
+	GET_UINT32(	X2,	input,	8 ); X2	^= RK[2];
+	GET_UINT32(	X3,	input, 12 ); X3	^= RK[3];
+
+#define	AES_FROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3)		\
+{												\
+	RK += 4;									\
+												\
+	X0 = RK[0] ^ FT0[ (uint8) (	Y0 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y1 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y2 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y3		 ) ];	\
+												\
+	X1 = RK[1] ^ FT0[ (uint8) (	Y1 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y2 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y3 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y0		 ) ];	\
+												\
+	X2 = RK[2] ^ FT0[ (uint8) (	Y2 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y3 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y0 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y1		 ) ];	\
+												\
+	X3 = RK[3] ^ FT0[ (uint8) (	Y3 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y0 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y1 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y2		 ) ];	\
+}
+
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 1 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 2 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 3 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 4 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 5 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 6 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 7 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 8 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 9 */
+
+	if(	ctx->nr	> 10 )
+	{
+		AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 10	*/
+		AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 11	*/
+	}
+
+	if(	ctx->nr	> 12 )
+	{
+		AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 12	*/
+		AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 13	*/
+	}
+
+	/* last	round */
+
+	RK += 4;
+
+	X0 = RK[0] ^ ( FSb[	(uint8)	( Y0 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y1 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y2 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y3	   ) ]		 );
+
+	X1 = RK[1] ^ ( FSb[	(uint8)	( Y1 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y2 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y3 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y0	   ) ]		 );
+
+	X2 = RK[2] ^ ( FSb[	(uint8)	( Y2 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y3 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y0 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y1	   ) ]		 );
+
+	X3 = RK[3] ^ ( FSb[	(uint8)	( Y3 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y0 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y1 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y2	   ) ]		 );
+
+	PUT_UINT32(	X0,	output,	 0 );
+	PUT_UINT32(	X1,	output,	 4 );
+	PUT_UINT32(	X2,	output,	 8 );
+	PUT_UINT32(	X3,	output,	12 );
+}
+
+/* AES 128-bit block decryption	routine	*/
+
+void rtmp_aes_decrypt( aes_context *ctx,	uint8 input[16], uint8 output[16] )
+{
+	uint32 *RK,	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3;
+
+	RK = ctx->drk;
+
+	GET_UINT32(	X0,	input,	0 ); X0	^= RK[0];
+	GET_UINT32(	X1,	input,	4 ); X1	^= RK[1];
+	GET_UINT32(	X2,	input,	8 ); X2	^= RK[2];
+	GET_UINT32(	X3,	input, 12 ); X3	^= RK[3];
+
+#define	AES_RROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3)		\
+{												\
+	RK += 4;									\
+												\
+	X0 = RK[0] ^ RT0[ (uint8) (	Y0 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y3 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y2 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y1		 ) ];	\
+												\
+	X1 = RK[1] ^ RT0[ (uint8) (	Y1 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y0 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y3 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y2		 ) ];	\
+												\
+	X2 = RK[2] ^ RT0[ (uint8) (	Y2 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y1 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y0 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y3		 ) ];	\
+												\
+	X3 = RK[3] ^ RT0[ (uint8) (	Y3 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y2 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y1 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y0		 ) ];	\
+}
+
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 1 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 2 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 3 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 4 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 5 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 6 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 7 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 8 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 9 */
+
+	if(	ctx->nr	> 10 )
+	{
+		AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 10	*/
+		AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 11	*/
+	}
+
+	if(	ctx->nr	> 12 )
+	{
+		AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 12	*/
+		AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 13	*/
+	}
+
+	/* last	round */
+
+	RK += 4;
+
+	X0 = RK[0] ^ ( RSb[	(uint8)	( Y0 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y3 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y2 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y1	   ) ]		 );
+
+	X1 = RK[1] ^ ( RSb[	(uint8)	( Y1 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y0 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y3 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y2	   ) ]		 );
+
+	X2 = RK[2] ^ ( RSb[	(uint8)	( Y2 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y1 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y0 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y3	   ) ]		 );
+
+	X3 = RK[3] ^ ( RSb[	(uint8)	( Y3 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y2 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y1 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y0	   ) ]		 );
+
+	PUT_UINT32(	X0,	output,	 0 );
+	PUT_UINT32(	X1,	output,	 4 );
+	PUT_UINT32(	X2,	output,	 8 );
+	PUT_UINT32(	X3,	output,	12 );
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		SHA1 function
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	HMAC_SHA1(
+	IN	UCHAR	*text,
+	IN	UINT	text_len,
+	IN	UCHAR	*key,
+	IN	UINT	key_len,
+	IN	UCHAR	*digest)
+{
+	SHA_CTX	context;
+	UCHAR	k_ipad[65]; /* inner padding - key XORd with ipad	*/
+	UCHAR	k_opad[65]; /* outer padding - key XORd with opad	*/
+	INT		i;
+
+	// if key is longer	than 64	bytes reset	it to key=SHA1(key)
+	if (key_len	> 64)
+	{
+		SHA_CTX		 tctx;
+		SHAInit(&tctx);
+		SHAUpdate(&tctx, key, key_len);
+		SHAFinal(&tctx,	key);
+		key_len	= 20;
+	}
+	NdisZeroMemory(k_ipad, sizeof(k_ipad));
+	NdisZeroMemory(k_opad, sizeof(k_opad));
+	NdisMoveMemory(k_ipad, key,	key_len);
+	NdisMoveMemory(k_opad, key,	key_len);
+
+	// XOR key with	ipad and opad values
+	for	(i = 0;	i <	64;	i++)
+	{
+		k_ipad[i] ^= 0x36;
+		k_opad[i] ^= 0x5c;
+	}
+
+	// perform inner SHA1
+	SHAInit(&context); 						/* init context for 1st pass */
+	SHAUpdate(&context,	k_ipad,	64);		/*	start with inner pad */
+	SHAUpdate(&context,	text, text_len);	/*	then text of datagram */
+	SHAFinal(&context, digest);				/* finish up 1st pass */
+
+	//perform outer	SHA1
+	SHAInit(&context);					/* init context for 2nd pass */
+	SHAUpdate(&context,	k_opad,	64);	/*	start with outer pad */
+	SHAUpdate(&context,	digest,	20);	/*	then results of	1st	hash */
+	SHAFinal(&context, digest);			/* finish up 2nd pass */
+
+}
+
+/*
+* F(P, S, c, i) = U1 xor U2 xor ... Uc
+* U1 = PRF(P, S || Int(i))
+* U2 = PRF(P, U1)
+* Uc = PRF(P, Uc-1)
+*/
+
+void F(char *password, unsigned char *ssid, int ssidlength, int iterations, int count, unsigned char *output)
+{
+    unsigned char digest[36], digest1[SHA_DIGEST_LEN];
+    int i, j;
+
+    /* U1 = PRF(P, S || int(i)) */
+    memcpy(digest, ssid, ssidlength);
+    digest[ssidlength] = (unsigned char)((count>>24) & 0xff);
+    digest[ssidlength+1] = (unsigned char)((count>>16) & 0xff);
+    digest[ssidlength+2] = (unsigned char)((count>>8) & 0xff);
+    digest[ssidlength+3] = (unsigned char)(count & 0xff);
+    HMAC_SHA1(digest, ssidlength+4, (unsigned char*) password, (int) strlen(password), digest1); // for WPA update
+
+    /* output = U1 */
+    memcpy(output, digest1, SHA_DIGEST_LEN);
+
+    for (i = 1; i < iterations; i++)
+    {
+        /* Un = PRF(P, Un-1) */
+        HMAC_SHA1(digest1, SHA_DIGEST_LEN, (unsigned char*) password, (int) strlen(password), digest); // for WPA update
+        memcpy(digest1, digest, SHA_DIGEST_LEN);
+
+        /* output = output xor Un */
+        for (j = 0; j < SHA_DIGEST_LEN; j++)
+        {
+            output[j] ^= digest[j];
+        }
+    }
+}
+/*
+* password - ascii string up to 63 characters in length
+* ssid - octet string up to 32 octets
+* ssidlength - length of ssid in octets
+* output must be 40 octets in length and outputs 256 bits of key
+*/
+int PasswordHash(char *password, unsigned char *ssid, int ssidlength, unsigned char *output)
+{
+    if ((strlen(password) > 63) || (ssidlength > 32))
+        return 0;
+
+    F(password, ssid, ssidlength, 4096, 1, output);
+    F(password, ssid, ssidlength, 4096, 2, &output[SHA_DIGEST_LEN]);
+    return 1;
+}
+
+
diff --git a/drivers/staging/rt2860/common/mlme.c b/drivers/staging/rt2860/common/mlme.c
new file mode 100644
index 000000000000..229747095aec
--- /dev/null
+++ b/drivers/staging/rt2860/common/mlme.c
@@ -0,0 +1,8667 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	mlme.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2004-08-25		Modify from RT2500 code base
+	John Chang	2004-09-06		modified for RT2600
+*/
+
+#include "../rt_config.h"
+#include <stdarg.h>
+
+UCHAR	CISCO_OUI[] = {0x00, 0x40, 0x96};
+
+UCHAR	WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
+UCHAR	RSN_OUI[] = {0x00, 0x0f, 0xac};
+UCHAR	WAPI_OUI[] = {0x00, 0x14, 0x72};
+UCHAR   WME_INFO_ELEM[]  = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+UCHAR   WME_PARM_ELEM[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+UCHAR	Ccx2QosInfo[] = {0x00, 0x40, 0x96, 0x04};
+UCHAR   RALINK_OUI[]  = {0x00, 0x0c, 0x43};
+UCHAR   BROADCOM_OUI[]  = {0x00, 0x90, 0x4c};
+UCHAR   WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+UCHAR	PRE_N_HT_OUI[]	= {0x00, 0x90, 0x4c};
+#endif // DOT11_N_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+UCHAR RateSwitchTable[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x11, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+    0x04, 0x21,  0, 30, 50,
+    0x05, 0x21,  1, 20, 50,
+    0x06, 0x21,  2, 20, 50,
+    0x07, 0x21,  3, 15, 50,
+    0x08, 0x21,  4, 15, 30,
+    0x09, 0x21,  5, 10, 25,
+    0x0a, 0x21,  6,  8, 25,
+    0x0b, 0x21,  7,  8, 25,
+    0x0c, 0x20, 12,  15, 30,
+    0x0d, 0x20, 13,  8, 20,
+    0x0e, 0x20, 14,  8, 20,
+    0x0f, 0x20, 15,  8, 25,
+    0x10, 0x22, 15,  8, 25,
+    0x11, 0x00,  0,  0,  0,
+    0x12, 0x00,  0,  0,  0,
+    0x13, 0x00,  0,  0,  0,
+    0x14, 0x00,  0,  0,  0,
+    0x15, 0x00,  0,  0,  0,
+    0x16, 0x00,  0,  0,  0,
+    0x17, 0x00,  0,  0,  0,
+    0x18, 0x00,  0,  0,  0,
+    0x19, 0x00,  0,  0,  0,
+    0x1a, 0x00,  0,  0,  0,
+    0x1b, 0x00,  0,  0,  0,
+    0x1c, 0x00,  0,  0,  0,
+    0x1d, 0x00,  0,  0,  0,
+    0x1e, 0x00,  0,  0,  0,
+    0x1f, 0x00,  0,  0,  0,
+};
+
+UCHAR RateSwitchTable11B[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x04, 0x03,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+};
+
+UCHAR RateSwitchTable11BG[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+    0x04, 0x10,  2, 20, 35,
+    0x05, 0x10,  3, 16, 35,
+    0x06, 0x10,  4, 10, 25,
+    0x07, 0x10,  5, 16, 25,
+    0x08, 0x10,  6, 10, 25,
+    0x09, 0x10,  7, 10, 13,
+};
+
+UCHAR RateSwitchTable11G[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x08, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x10,  0, 20, 101,
+    0x01, 0x10,  1, 20, 35,
+    0x02, 0x10,  2, 20, 35,
+    0x03, 0x10,  3, 16, 35,
+    0x04, 0x10,  4, 10, 25,
+    0x05, 0x10,  5, 16, 25,
+    0x06, 0x10,  6, 10, 25,
+    0x07, 0x10,  7, 10, 13,
+};
+
+#ifdef DOT11_N_SUPPORT
+UCHAR RateSwitchTable11N1S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x09, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 10, 25,
+    0x06, 0x21,  6,  8, 14,
+    0x07, 0x21,  7,  8, 14,
+    0x08, 0x23,  7,  8, 14,
+};
+
+UCHAR RateSwitchTable11N2S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,      // Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x20, 12,  15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11N3S[] = {
+// Item No.	Mode	Curr-MCS	TrainUp	TrainDown	// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,      // Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x20, 12,  15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11N2SForABand[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0b, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x20, 12,  15, 30,
+    0x07, 0x20, 13,  8, 20,
+    0x08, 0x20, 14,  8, 20,
+    0x09, 0x20, 15,  8, 25,
+    0x0a, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11N3SForABand[] = { // 3*3
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0b, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x20, 12,  15, 30,
+    0x07, 0x20, 13,  8, 20,
+    0x08, 0x20, 14,  8, 20,
+    0x09, 0x20, 15,  8, 25,
+    0x0a, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN1S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0d, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+    0x04, 0x21,  0, 30,101,	//50
+    0x05, 0x21,  1, 20, 50,
+    0x06, 0x21,  2, 20, 50,
+    0x07, 0x21,  3, 15, 50,
+    0x08, 0x21,  4, 15, 30,
+    0x09, 0x21,  5, 10, 25,
+    0x0a, 0x21,  6,  8, 14,
+    0x0b, 0x21,  7,  8, 14,
+	0x0c, 0x23,  7,  8, 14,
+};
+
+UCHAR RateSwitchTable11BGN2S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x20, 12, 15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN3S[] = { // 3*3
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 20, 50,
+    0x04, 0x21,  4, 15, 50,
+    0x05, 0x20, 20, 15, 30,
+    0x06, 0x20, 21,  8, 20,
+    0x07, 0x20, 22,  8, 20,
+    0x08, 0x20, 23,  8, 25,
+    0x09, 0x22, 23,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN2SForABand[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0b, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x20, 12, 15, 30,
+    0x07, 0x20, 13,  8, 20,
+    0x08, 0x20, 14,  8, 20,
+    0x09, 0x20, 15,  8, 25,
+    0x0a, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN3SForABand[] = { // 3*3
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0c, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x21, 12, 15, 30,
+    0x07, 0x20, 20, 15, 30,
+    0x08, 0x20, 21,  8, 20,
+    0x09, 0x20, 22,  8, 20,
+    0x0a, 0x20, 23,  8, 25,
+    0x0b, 0x22, 23,  8, 25,
+};
+#endif // DOT11_N_SUPPORT //
+
+PUCHAR ReasonString[] = {
+	/* 0  */	 "Reserved",
+	/* 1  */	 "Unspecified Reason",
+	/* 2  */	 "Previous Auth no longer valid",
+	/* 3  */	 "STA is leaving / has left",
+	/* 4  */	 "DIS-ASSOC due to inactivity",
+	/* 5  */	 "AP unable to hanle all associations",
+	/* 6  */	 "class 2 error",
+	/* 7  */	 "class 3 error",
+	/* 8  */	 "STA is leaving / has left",
+	/* 9  */	 "require auth before assoc/re-assoc",
+	/* 10 */	 "Reserved",
+	/* 11 */	 "Reserved",
+	/* 12 */	 "Reserved",
+	/* 13 */	 "invalid IE",
+	/* 14 */	 "MIC error",
+	/* 15 */	 "4-way handshake timeout",
+	/* 16 */	 "2-way (group key) handshake timeout",
+	/* 17 */	 "4-way handshake IE diff among AssosReq/Rsp/Beacon",
+	/* 18 */
+};
+
+extern UCHAR	 OfdmRateToRxwiMCS[];
+// since RT61 has better RX sensibility, we have to limit TX ACK rate not to exceed our normal data TX rate.
+// otherwise the WLAN peer may not be able to receive the ACK thus downgrade its data TX rate
+ULONG BasicRateMask[12]				= {0xfffff001 /* 1-Mbps */, 0xfffff003 /* 2 Mbps */, 0xfffff007 /* 5.5 */, 0xfffff00f /* 11 */,
+									  0xfffff01f /* 6 */	 , 0xfffff03f /* 9 */	  , 0xfffff07f /* 12 */ , 0xfffff0ff /* 18 */,
+									  0xfffff1ff /* 24 */	 , 0xfffff3ff /* 36 */	  , 0xfffff7ff /* 48 */ , 0xffffffff /* 54 */};
+
+UCHAR MULTICAST_ADDR[MAC_ADDR_LEN] = {0x1,  0x00, 0x00, 0x00, 0x00, 0x00};
+UCHAR BROADCAST_ADDR[MAC_ADDR_LEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+UCHAR ZERO_MAC_ADDR[MAC_ADDR_LEN]  = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+// e.g. RssiSafeLevelForTxRate[RATE_36]" means if the current RSSI is greater than
+//		this value, then it's quaranteed capable of operating in 36 mbps TX rate in
+//		clean environment.
+//								  TxRate: 1   2   5.5	11	 6	  9    12	18	 24   36   48	54	 72  100
+CHAR RssiSafeLevelForTxRate[] ={  -92, -91, -90, -87, -88, -86, -85, -83, -81, -78, -72, -71, -40, -40 };
+
+UCHAR  RateIdToMbps[]	 = { 1, 2, 5, 11, 6, 9, 12, 18, 24, 36, 48, 54, 72, 100};
+USHORT RateIdTo500Kbps[] = { 2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108, 144, 200};
+
+UCHAR  SsidIe	 = IE_SSID;
+UCHAR  SupRateIe = IE_SUPP_RATES;
+UCHAR  ExtRateIe = IE_EXT_SUPP_RATES;
+#ifdef DOT11_N_SUPPORT
+UCHAR  HtCapIe = IE_HT_CAP;
+UCHAR  AddHtInfoIe = IE_ADD_HT;
+UCHAR  NewExtChanIe = IE_SECONDARY_CH_OFFSET;
+#ifdef DOT11N_DRAFT3
+UCHAR  ExtHtCapIe = IE_EXT_CAPABILITY;
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+UCHAR  ErpIe	 = IE_ERP;
+UCHAR  DsIe 	 = IE_DS_PARM;
+UCHAR  TimIe	 = IE_TIM;
+UCHAR  WpaIe	 = IE_WPA;
+UCHAR  Wpa2Ie	 = IE_WPA2;
+UCHAR  IbssIe	 = IE_IBSS_PARM;
+UCHAR  Ccx2Ie	 = IE_CCX_V2;
+
+extern UCHAR	WPA_OUI[];
+
+UCHAR	SES_OUI[] = {0x00, 0x90, 0x4c};
+
+UCHAR	ZeroSsid[32] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+
+// Reset the RFIC setting to new series
+RTMP_RF_REGS RF2850RegTable[] = {
+//		ch	 R1 		 R2 		 R3(TX0~4=0) R4
+		{1,  0x98402ecc, 0x984c0786, 0x9816b455, 0x9800510b},
+		{2,  0x98402ecc, 0x984c0786, 0x98168a55, 0x9800519f},
+		{3,  0x98402ecc, 0x984c078a, 0x98168a55, 0x9800518b},
+		{4,  0x98402ecc, 0x984c078a, 0x98168a55, 0x9800519f},
+		{5,  0x98402ecc, 0x984c078e, 0x98168a55, 0x9800518b},
+		{6,  0x98402ecc, 0x984c078e, 0x98168a55, 0x9800519f},
+		{7,  0x98402ecc, 0x984c0792, 0x98168a55, 0x9800518b},
+		{8,  0x98402ecc, 0x984c0792, 0x98168a55, 0x9800519f},
+		{9,  0x98402ecc, 0x984c0796, 0x98168a55, 0x9800518b},
+		{10, 0x98402ecc, 0x984c0796, 0x98168a55, 0x9800519f},
+		{11, 0x98402ecc, 0x984c079a, 0x98168a55, 0x9800518b},
+		{12, 0x98402ecc, 0x984c079a, 0x98168a55, 0x9800519f},
+		{13, 0x98402ecc, 0x984c079e, 0x98168a55, 0x9800518b},
+		{14, 0x98402ecc, 0x984c07a2, 0x98168a55, 0x98005193},
+
+		// 802.11 UNI / HyperLan 2
+		{36, 0x98402ecc, 0x984c099a, 0x98158a55, 0x980ed1a3},
+		{38, 0x98402ecc, 0x984c099e, 0x98158a55, 0x980ed193},
+		{40, 0x98402ec8, 0x984c0682, 0x98158a55, 0x980ed183},
+		{44, 0x98402ec8, 0x984c0682, 0x98158a55, 0x980ed1a3},
+		{46, 0x98402ec8, 0x984c0686, 0x98158a55, 0x980ed18b},
+		{48, 0x98402ec8, 0x984c0686, 0x98158a55, 0x980ed19b},
+		{52, 0x98402ec8, 0x984c068a, 0x98158a55, 0x980ed193},
+		{54, 0x98402ec8, 0x984c068a, 0x98158a55, 0x980ed1a3},
+		{56, 0x98402ec8, 0x984c068e, 0x98158a55, 0x980ed18b},
+		{60, 0x98402ec8, 0x984c0692, 0x98158a55, 0x980ed183},
+		{62, 0x98402ec8, 0x984c0692, 0x98158a55, 0x980ed193},
+		{64, 0x98402ec8, 0x984c0692, 0x98158a55, 0x980ed1a3}, // Plugfest#4, Day4, change RFR3 left4th 9->5.
+
+		// 802.11 HyperLan 2
+		{100, 0x98402ec8, 0x984c06b2, 0x98178a55, 0x980ed783},
+
+		// 2008.04.30 modified
+		// The system team has AN to improve the EVM value
+		// for channel 102 to 108 for the RT2850/RT2750 dual band solution.
+		{102, 0x98402ec8, 0x985c06b2, 0x98578a55, 0x980ed793},
+		{104, 0x98402ec8, 0x985c06b2, 0x98578a55, 0x980ed1a3},
+		{108, 0x98402ecc, 0x985c0a32, 0x98578a55, 0x980ed193},
+
+		{110, 0x98402ecc, 0x984c0a36, 0x98178a55, 0x980ed183},
+		{112, 0x98402ecc, 0x984c0a36, 0x98178a55, 0x980ed19b},
+		{116, 0x98402ecc, 0x984c0a3a, 0x98178a55, 0x980ed1a3},
+		{118, 0x98402ecc, 0x984c0a3e, 0x98178a55, 0x980ed193},
+		{120, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed183},
+		{124, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed193},
+		{126, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed15b}, // 0x980ed1bb->0x980ed15b required by Rory 20070927
+		{128, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed1a3},
+		{132, 0x98402ec4, 0x984c0386, 0x98178a55, 0x980ed18b},
+		{134, 0x98402ec4, 0x984c0386, 0x98178a55, 0x980ed193},
+		{136, 0x98402ec4, 0x984c0386, 0x98178a55, 0x980ed19b},
+		{140, 0x98402ec4, 0x984c038a, 0x98178a55, 0x980ed183},
+
+		// 802.11 UNII
+		{149, 0x98402ec4, 0x984c038a, 0x98178a55, 0x980ed1a7},
+		{151, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed187},
+		{153, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed18f},
+		{157, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed19f},
+		{159, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed1a7},
+		{161, 0x98402ec4, 0x984c0392, 0x98178a55, 0x980ed187},
+		{165, 0x98402ec4, 0x984c0392, 0x98178a55, 0x980ed197},
+
+		// Japan
+		{184, 0x95002ccc, 0x9500491e, 0x9509be55, 0x950c0a0b},
+		{188, 0x95002ccc, 0x95004922, 0x9509be55, 0x950c0a13},
+		{192, 0x95002ccc, 0x95004926, 0x9509be55, 0x950c0a1b},
+		{196, 0x95002ccc, 0x9500492a, 0x9509be55, 0x950c0a23},
+		{208, 0x95002ccc, 0x9500493a, 0x9509be55, 0x950c0a13},
+		{212, 0x95002ccc, 0x9500493e, 0x9509be55, 0x950c0a1b},
+		{216, 0x95002ccc, 0x95004982, 0x9509be55, 0x950c0a23},
+
+		// still lack of MMAC(Japan) ch 34,38,42,46
+};
+UCHAR	NUM_OF_2850_CHNL = (sizeof(RF2850RegTable) / sizeof(RTMP_RF_REGS));
+
+FREQUENCY_ITEM FreqItems3020[] =
+{
+	/**************************************************/
+	// ISM : 2.4 to 2.483 GHz                         //
+	/**************************************************/
+	// 11g
+	/**************************************************/
+	//-CH---N-------R---K-----------
+	{1,    241,  2,  2},
+	{2,    241,	 2,  7},
+	{3,    242,	 2,  2},
+	{4,    242,	 2,  7},
+	{5,    243,	 2,  2},
+	{6,    243,	 2,  7},
+	{7,    244,	 2,  2},
+	{8,    244,	 2,  7},
+	{9,    245,	 2,  2},
+	{10,   245,	 2,  7},
+	{11,   246,	 2,  2},
+	{12,   246,	 2,  7},
+	{13,   247,	 2,  2},
+	{14,   248,	 2,  4},
+};
+#define	NUM_OF_3020_CHNL	(sizeof(FreqItems3020) / sizeof(FREQUENCY_ITEM))
+
+/*
+	==========================================================================
+	Description:
+		initialize the MLME task and its data structure (queue, spinlock,
+		timer, state machines).
+
+	IRQL = PASSIVE_LEVEL
+
+	Return:
+		always return NDIS_STATUS_SUCCESS
+
+	==========================================================================
+*/
+NDIS_STATUS MlmeInit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NDIS_STATUS Status = NDIS_STATUS_SUCCESS;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> MLME Initialize\n"));
+
+	do
+	{
+		Status = MlmeQueueInit(&pAd->Mlme.Queue);
+		if(Status != NDIS_STATUS_SUCCESS)
+			break;
+
+		pAd->Mlme.bRunning = FALSE;
+		NdisAllocateSpinLock(&pAd->Mlme.TaskLock);
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			BssTableInit(&pAd->ScanTab);
+
+			// init STA state machines
+			AssocStateMachineInit(pAd, &pAd->Mlme.AssocMachine, pAd->Mlme.AssocFunc);
+			AuthStateMachineInit(pAd, &pAd->Mlme.AuthMachine, pAd->Mlme.AuthFunc);
+			AuthRspStateMachineInit(pAd, &pAd->Mlme.AuthRspMachine, pAd->Mlme.AuthRspFunc);
+			SyncStateMachineInit(pAd, &pAd->Mlme.SyncMachine, pAd->Mlme.SyncFunc);
+			WpaPskStateMachineInit(pAd, &pAd->Mlme.WpaPskMachine, pAd->Mlme.WpaPskFunc);
+			AironetStateMachineInit(pAd, &pAd->Mlme.AironetMachine, pAd->Mlme.AironetFunc);
+
+#ifdef QOS_DLS_SUPPORT
+			DlsStateMachineInit(pAd, &pAd->Mlme.DlsMachine, pAd->Mlme.DlsFunc);
+#endif // QOS_DLS_SUPPORT //
+
+
+			// Since we are using switch/case to implement it, the init is different from the above
+			// state machine init
+			MlmeCntlInit(pAd, &pAd->Mlme.CntlMachine, NULL);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+		ActionStateMachineInit(pAd, &pAd->Mlme.ActMachine, pAd->Mlme.ActFunc);
+
+		// Init mlme periodic timer
+		RTMPInitTimer(pAd, &pAd->Mlme.PeriodicTimer, GET_TIMER_FUNCTION(MlmePeriodicExec), pAd, TRUE);
+
+		// Set mlme periodic timer
+		RTMPSetTimer(&pAd->Mlme.PeriodicTimer, MLME_TASK_EXEC_INTV);
+
+		// software-based RX Antenna diversity
+		RTMPInitTimer(pAd, &pAd->Mlme.RxAntEvalTimer, GET_TIMER_FUNCTION(AsicRxAntEvalTimeout), pAd, FALSE);
+
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef RT2860
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+	        if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+	        {
+	            // only PCIe cards need these two timers
+	    		RTMPInitTimer(pAd, &pAd->Mlme.PsPollTimer, GET_TIMER_FUNCTION(PsPollWakeExec), pAd, FALSE);
+	    		RTMPInitTimer(pAd, &pAd->Mlme.RadioOnOffTimer, GET_TIMER_FUNCTION(RadioOnExec), pAd, FALSE);
+	        }
+		}
+#endif // RT2860 //
+#endif // CONFIG_STA_SUPPORT //
+
+	} while (FALSE);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- MLME Initialize\n"));
+
+	return Status;
+}
+
+/*
+	==========================================================================
+	Description:
+		main loop of the MLME
+	Pre:
+		Mlme has to be initialized, and there are something inside the queue
+	Note:
+		This function is invoked from MPSetInformation and MPReceive;
+		This task guarantee only one MlmeHandler will run.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeHandler(
+	IN PRTMP_ADAPTER pAd)
+{
+	MLME_QUEUE_ELEM 	   *Elem = NULL;
+#ifdef APCLI_SUPPORT
+	SHORT apcliIfIndex;
+#endif
+
+	// Only accept MLME and Frame from peer side, no other (control/data) frame should
+	// get into this state machine
+
+	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
+	if(pAd->Mlme.bRunning)
+	{
+		NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+		return;
+	}
+	else
+	{
+		pAd->Mlme.bRunning = TRUE;
+	}
+	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+
+	while (!MlmeQueueEmpty(&pAd->Mlme.Queue))
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS) ||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS) ||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("Device Halted or Removed or MlmeRest, exit MlmeHandler! (queue num = %ld)\n", pAd->Mlme.Queue.Num));
+			break;
+		}
+
+#ifdef RALINK_ATE
+		if(ATE_ON(pAd))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now in MlmeHandler\n"));
+			break;
+		}
+#endif // RALINK_ATE //
+
+		//From message type, determine which state machine I should drive
+		if (MlmeDequeue(&pAd->Mlme.Queue, &Elem))
+		{
+
+			// if dequeue success
+			switch (Elem->Machine)
+			{
+				// STA state machines
+#ifdef	CONFIG_STA_SUPPORT
+				case ASSOC_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AssocMachine, Elem);
+					break;
+				case AUTH_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AuthMachine, Elem);
+					break;
+				case AUTH_RSP_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AuthRspMachine, Elem);
+					break;
+				case SYNC_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.SyncMachine, Elem);
+					break;
+				case MLME_CNTL_STATE_MACHINE:
+					MlmeCntlMachinePerformAction(pAd, &pAd->Mlme.CntlMachine, Elem);
+					break;
+				case WPA_PSK_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.WpaPskMachine, Elem);
+					break;
+#ifdef LEAP_SUPPORT
+				case LEAP_STATE_MACHINE:
+					LeapMachinePerformAction(pAd, &pAd->Mlme.LeapMachine, Elem);
+					break;
+#endif
+				case AIRONET_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AironetMachine, Elem);
+					break;
+
+#ifdef QOS_DLS_SUPPORT
+				case DLS_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.DlsMachine, Elem);
+					break;
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+				case ACTION_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.ActMachine, Elem);
+					break;
+
+
+
+
+				default:
+					DBGPRINT(RT_DEBUG_TRACE, ("ERROR: Illegal machine %ld in MlmeHandler()\n", Elem->Machine));
+					break;
+			} // end of switch
+
+			// free MLME element
+			Elem->Occupied = FALSE;
+			Elem->MsgLen = 0;
+
+		}
+		else {
+			DBGPRINT_ERR(("MlmeHandler: MlmeQueue empty\n"));
+		}
+	}
+
+	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
+	pAd->Mlme.bRunning = FALSE;
+	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+}
+
+/*
+	==========================================================================
+	Description:
+		Destructor of MLME (Destroy queue, state machine, spin lock and timer)
+	Parameters:
+		Adapter - NIC Adapter pointer
+	Post:
+		The MLME task will no longer work properly
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeHalt(
+	IN PRTMP_ADAPTER pAd)
+{
+	BOOLEAN 	  Cancelled;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeHalt\n"));
+
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		// disable BEACON generation and other BEACON related hardware timers
+		AsicDisableSync(pAd);
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef QOS_DLS_SUPPORT
+		UCHAR		i;
+#endif // QOS_DLS_SUPPORT //
+		// Cancel pending timers
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,	&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,		&Cancelled);
+#ifdef RT2860
+	    if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+	    {
+	   	    RTMPCancelTimer(&pAd->Mlme.PsPollTimer,		&Cancelled);
+		    RTMPCancelTimer(&pAd->Mlme.RadioOnOffTimer,		&Cancelled);
+		}
+#endif // RT2860 //
+
+#ifdef QOS_DLS_SUPPORT
+		for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &Cancelled);
+		}
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMPCancelTimer(&pAd->Mlme.PeriodicTimer,		&Cancelled);
+	RTMPCancelTimer(&pAd->Mlme.RxAntEvalTimer,		&Cancelled);
+
+
+
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		// Set LED
+		RTMPSetLED(pAd, LED_HALT);
+        RTMPSetSignalLED(pAd, -100);	// Force signal strength Led to be turned off, firmware is not done it.
+	}
+
+	RTMPusecDelay(5000);    //  5 msec to gurantee Ant Diversity timer canceled
+
+	MlmeQueueDestroy(&pAd->Mlme.Queue);
+	NdisFreeSpinLock(&pAd->Mlme.TaskLock);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== MlmeHalt\n"));
+}
+
+VOID MlmeResetRalinkCounters(
+	IN  PRTMP_ADAPTER   pAd)
+{
+	pAd->RalinkCounters.LastOneSecRxOkDataCnt = pAd->RalinkCounters.OneSecRxOkDataCnt;
+	// clear all OneSecxxx counters.
+	pAd->RalinkCounters.OneSecBeaconSentCnt = 0;
+	pAd->RalinkCounters.OneSecFalseCCACnt = 0;
+	pAd->RalinkCounters.OneSecRxFcsErrCnt = 0;
+	pAd->RalinkCounters.OneSecRxOkCnt = 0;
+	pAd->RalinkCounters.OneSecTxFailCount = 0;
+	pAd->RalinkCounters.OneSecTxNoRetryOkCount = 0;
+	pAd->RalinkCounters.OneSecTxRetryOkCount = 0;
+	pAd->RalinkCounters.OneSecRxOkDataCnt = 0;
+
+	// TODO: for debug only. to be removed
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_BE] = 0;
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_BK] = 0;
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_VI] = 0;
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_VO] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_BE] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_BK] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_VI] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_VO] = 0;
+	pAd->RalinkCounters.OneSecTxDoneCount = 0;
+	pAd->RalinkCounters.OneSecRxCount = 0;
+	pAd->RalinkCounters.OneSecTxAggregationCount = 0;
+	pAd->RalinkCounters.OneSecRxAggregationCount = 0;
+
+	return;
+}
+
+unsigned long rx_AMSDU;
+unsigned long rx_Total;
+
+/*
+	==========================================================================
+	Description:
+		This routine is executed periodically to -
+		1. Decide if it's a right time to turn on PwrMgmt bit of all
+		   outgoiing frames
+		2. Calculate ChannelQuality based on statistics of the last
+		   period, so that TX rate won't toggling very frequently between a
+		   successful TX and a failed TX.
+		3. If the calculated ChannelQuality indicated current connection not
+		   healthy, then a ROAMing attempt is tried here.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+#define ADHOC_BEACON_LOST_TIME		(8*OS_HZ)  // 8 sec
+VOID MlmePeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	ULONG			TxTotalCnt;
+	PRTMP_ADAPTER	pAd = (RTMP_ADAPTER *)FunctionContext;
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef RT2860
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+	    // If Hardware controlled Radio enabled, we have to check GPIO pin2 every 2 second.
+		// Move code to here, because following code will return when radio is off
+		if ((pAd->Mlme.PeriodicRound % (MLME_TASK_EXEC_MULTIPLE * 2) == 0) && (pAd->StaCfg.bHardwareRadio == TRUE) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(pAd->bPCIclkOff == FALSE))
+		{
+			UINT32				data = 0;
+
+			// Read GPIO pin2 as Hardware controlled radio state
+			RTMP_IO_READ32(pAd, GPIO_CTRL_CFG, &data);
+			if (data & 0x04)
+			{
+				pAd->StaCfg.bHwRadio = TRUE;
+			}
+			else
+			{
+				pAd->StaCfg.bHwRadio = FALSE;
+			}
+			if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+			{
+				pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+				if (pAd->StaCfg.bRadio == TRUE)
+				{
+					MlmeRadioOn(pAd);
+					// Update extra information
+					pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+				}
+				else
+				{
+					MlmeRadioOff(pAd);
+					// Update extra information
+					pAd->ExtraInfo = HW_RADIO_OFF;
+				}
+			}
+		}
+	}
+#endif // RT2860 //
+#endif // CONFIG_STA_SUPPORT //
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_RADIO_OFF |
+								fRTMP_ADAPTER_RADIO_MEASUREMENT |
+								fRTMP_ADAPTER_RESET_IN_PROGRESS))))
+		return;
+
+	RT28XX_MLME_PRE_SANITY_CHECK(pAd);
+
+#ifdef RALINK_ATE
+	/* Do not show RSSI until "Normal 1 second Mlme PeriodicExec". */
+	if (ATE_ON(pAd))
+	{
+		if (pAd->Mlme.PeriodicRound % MLME_TASK_EXEC_MULTIPLE != (MLME_TASK_EXEC_MULTIPLE - 1))
+	{
+			pAd->Mlme.PeriodicRound ++;
+			return;
+		}
+	}
+#endif // RALINK_ATE //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Do nothing if monitor mode is on
+		if (MONITOR_ON(pAd))
+			return;
+
+		if (pAd->Mlme.PeriodicRound & 0x1)
+		{
+			// This is the fix for wifi 11n extension channel overlapping test case.  for 2860D
+			if (((pAd->MACVersion & 0xffff) == 0x0101) &&
+				(STA_TGN_WIFI_ON(pAd)) &&
+				(pAd->CommonCfg.IOTestParm.bToggle == FALSE))
+
+				{
+					RTMP_IO_WRITE32(pAd, TXOP_CTRL_CFG, 0x24Bf);
+					pAd->CommonCfg.IOTestParm.bToggle = TRUE;
+				}
+				else if ((STA_TGN_WIFI_ON(pAd)) &&
+						((pAd->MACVersion & 0xffff) == 0x0101))
+				{
+					RTMP_IO_WRITE32(pAd, TXOP_CTRL_CFG, 0x243f);
+					pAd->CommonCfg.IOTestParm.bToggle = FALSE;
+				}
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	pAd->bUpdateBcnCntDone = FALSE;
+
+//	RECBATimerTimeout(SystemSpecific1,FunctionContext,SystemSpecific2,SystemSpecific3);
+	pAd->Mlme.PeriodicRound ++;
+
+	// execute every 500ms
+	if ((pAd->Mlme.PeriodicRound % 5 == 0) && RTMPAutoRateSwitchCheck(pAd)/*(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED))*/)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		// perform dynamic tx rate switching based on past TX history
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if ((OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+					)
+				&& (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE)))
+				MlmeDynamicTxRateSwitching(pAd);
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// Normal 1 second Mlme PeriodicExec.
+	if (pAd->Mlme.PeriodicRound %MLME_TASK_EXEC_MULTIPLE == 0)
+	{
+                pAd->Mlme.OneSecPeriodicRound ++;
+
+#ifdef RALINK_ATE
+    	if (ATE_ON(pAd))
+    	{
+			/* request from Baron : move this routine from later to here */
+			/* for showing Rx error count in ATE RXFRAME */
+            NICUpdateRawCounters(pAd);
+			if (pAd->ate.bRxFer == 1)
+			{
+				pAd->ate.RxTotalCnt += pAd->ate.RxCntPerSec;
+			    ate_print(KERN_EMERG "MlmePeriodicExec: Rx packet cnt = %d/%d\n", pAd->ate.RxCntPerSec, pAd->ate.RxTotalCnt);
+				pAd->ate.RxCntPerSec = 0;
+
+				if (pAd->ate.RxAntennaSel == 0)
+					ate_print(KERN_EMERG "MlmePeriodicExec: Rx AvgRssi0=%d, AvgRssi1=%d, AvgRssi2=%d\n\n",
+						pAd->ate.AvgRssi0, pAd->ate.AvgRssi1, pAd->ate.AvgRssi2);
+				else
+					ate_print(KERN_EMERG "MlmePeriodicExec: Rx AvgRssi=%d\n\n", pAd->ate.AvgRssi0);
+			}
+			MlmeResetRalinkCounters(pAd);
+			return;
+    	}
+#endif // RALINK_ATE //
+
+
+		if (rx_Total)
+		{
+
+			// reset counters
+			rx_AMSDU = 0;
+			rx_Total = 0;
+		}
+
+		// Media status changed, report to NDIS
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_CHANGE))
+		{
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_CHANGE);
+			if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+			{
+				pAd->IndicateMediaState = NdisMediaStateConnected;
+				RTMP_IndicateMediaState(pAd);
+
+			}
+			else
+			{
+				pAd->IndicateMediaState = NdisMediaStateDisconnected;
+				RTMP_IndicateMediaState(pAd);
+			}
+		}
+
+		NdisGetSystemUpTime(&pAd->Mlme.Now32);
+
+		// add the most up-to-date h/w raw counters into software variable, so that
+		// the dynamic tuning mechanism below are based on most up-to-date information
+		NICUpdateRawCounters(pAd);
+
+
+#ifdef DOT11_N_SUPPORT
+   		// Need statistics after read counter. So put after NICUpdateRawCounters
+		ORIBATimerTimeout(pAd);
+#endif // DOT11_N_SUPPORT //
+
+
+		// The time period for checking antenna is according to traffic
+		if (pAd->Mlme.bEnableAutoAntennaCheck)
+		{
+			TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+							 pAd->RalinkCounters.OneSecTxRetryOkCount +
+							 pAd->RalinkCounters.OneSecTxFailCount;
+
+			if (TxTotalCnt > 50)
+			{
+				if (pAd->Mlme.OneSecPeriodicRound % 10 == 0)
+				{
+					AsicEvaluateRxAnt(pAd);
+				}
+			}
+			else
+			{
+				if (pAd->Mlme.OneSecPeriodicRound % 3 == 0)
+				{
+					AsicEvaluateRxAnt(pAd);
+				}
+			}
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			STAMlmePeriodicExec(pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+		MlmeResetRalinkCounters(pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef RT2860
+			if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST) && (pAd->bPCIclkOff == FALSE))
+#endif // RT2860 //
+			{
+				// When Adhoc beacon is enabled and RTS/CTS is enabled, there is a chance that hardware MAC FSM will run into a deadlock
+				// and sending CTS-to-self over and over.
+				// Software Patch Solution:
+				// 1. Polling debug state register 0x10F4 every one second.
+				// 2. If in 0x10F4 the ((bit29==1) && (bit7==1)) OR ((bit29==1) && (bit5==1)), it means the deadlock has occurred.
+				// 3. If the deadlock occurred, reset MAC/BBP by setting 0x1004 to 0x0001 for a while then setting it back to 0x000C again.
+
+				UINT32	MacReg = 0;
+
+				RTMP_IO_READ32(pAd, 0x10F4, &MacReg);
+				if (((MacReg & 0x20000000) && (MacReg & 0x80)) || ((MacReg & 0x20000000) && (MacReg & 0x20)))
+				{
+					RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x1);
+					RTMPusecDelay(1);
+					RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0xC);
+
+					DBGPRINT(RT_DEBUG_WARN,("Warning, MAC specific condition occurs \n"));
+				}
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		RT28XX_MLME_HANDLER(pAd);
+	}
+
+
+	pAd->bUpdateBcnCntDone = FALSE;
+}
+
+#ifdef CONFIG_STA_SUPPORT
+VOID STAMlmePeriodicExec(
+	PRTMP_ADAPTER pAd)
+{
+	ULONG			    TxTotalCnt;
+
+//
+// We return here in ATE mode, because the statistics
+// that ATE needs are not collected via this routine.
+//
+#ifdef RALINK_ATE
+	// It is supposed that we will never reach here in ATE mode.
+	ASSERT(!(ATE_ON(pAd)));
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    if (pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_DISABLE)
+#endif // WPA_SUPPLICANT_SUPPORT //
+    {
+    	// WPA MIC error should block association attempt for 60 seconds
+    	if (pAd->StaCfg.bBlockAssoc && (pAd->StaCfg.LastMicErrorTime + (60 * OS_HZ) < pAd->Mlme.Now32))
+    		pAd->StaCfg.bBlockAssoc = FALSE;
+    }
+
+    if ((pAd->PreMediaState != pAd->IndicateMediaState) && (pAd->CommonCfg.bWirelessEvent))
+	{
+		if (pAd->IndicateMediaState == NdisMediaStateConnected)
+		{
+			RTMPSendWirelessEvent(pAd, IW_STA_LINKUP_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+		pAd->PreMediaState = pAd->IndicateMediaState;
+	}
+
+
+
+
+   	AsicStaBbpTuning(pAd);
+
+	TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+					 pAd->RalinkCounters.OneSecTxRetryOkCount +
+					 pAd->RalinkCounters.OneSecTxFailCount;
+
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		// update channel quality for Roaming and UI LinkQuality display
+		MlmeCalculateChannelQuality(pAd, pAd->Mlme.Now32);
+	}
+
+	// must be AFTER MlmeDynamicTxRateSwitching() because it needs to know if
+	// Radio is currently in noisy environment
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		AsicAdjustTxPower(pAd);
+
+	if (INFRA_ON(pAd))
+	{
+#ifdef QOS_DLS_SUPPORT
+		// Check DLS time out, then tear down those session
+		RTMPCheckDLSTimeOut(pAd);
+#endif // QOS_DLS_SUPPORT //
+
+		// Is PSM bit consistent with user power management policy?
+		// This is the only place that will set PSM bit ON.
+		if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+		MlmeCheckPsmChange(pAd, pAd->Mlme.Now32);
+
+		pAd->RalinkCounters.LastOneSecTotalTxCount = TxTotalCnt;
+
+		if ((pAd->StaCfg.LastBeaconRxTime + 1*OS_HZ < pAd->Mlme.Now32) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) &&
+			((TxTotalCnt + pAd->RalinkCounters.OneSecRxOkCnt < 600)))
+		{
+			RTMPSetAGCInitValue(pAd, BW_20);
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - No BEACON. restore R66 to the low bound(%d) \n", (0x2E + GET_LNA_GAIN(pAd))));
+		}
+
+        {
+    		if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable)
+    		{
+    		    // When APSD is enabled, the period changes as 20 sec
+    			if ((pAd->Mlme.OneSecPeriodicRound % 20) == 8)
+    				RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+    		}
+    		else
+    		{
+    		    // Send out a NULL frame every 10 sec to inform AP that STA is still alive (Avoid being age out)
+    			if ((pAd->Mlme.OneSecPeriodicRound % 10) == 8)
+                {
+                    if (pAd->CommonCfg.bWmmCapable)
+    					RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+                    else
+						RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, FALSE);
+                }
+    		}
+        }
+
+		if (CQI_IS_DEAD(pAd->Mlme.ChannelQuality))
+			{
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - No BEACON. Dead CQI. Auto Recovery attempt #%ld\n", pAd->RalinkCounters.BadCQIAutoRecoveryCount));
+			pAd->StaCfg.CCXAdjacentAPReportFlag = TRUE;
+			pAd->StaCfg.CCXAdjacentAPLinkDownTime = pAd->StaCfg.LastBeaconRxTime;
+
+			// Lost AP, send disconnect & link down event
+			LinkDown(pAd, FALSE);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.WpaSupplicantUP)
+			{
+                union iwreq_data    wrqu;
+                //send disassociate event to wpa_supplicant
+                memset(&wrqu, 0, sizeof(wrqu));
+                wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+                wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+			MlmeAutoReconnectLastSSID(pAd);
+		}
+		else if (CQI_IS_BAD(pAd->Mlme.ChannelQuality))
+		{
+			pAd->RalinkCounters.BadCQIAutoRecoveryCount ++;
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Bad CQI. Auto Recovery attempt #%ld\n", pAd->RalinkCounters.BadCQIAutoRecoveryCount));
+			MlmeAutoReconnectLastSSID(pAd);
+		}
+
+		// Add auto seamless roaming
+		if (pAd->StaCfg.bFastRoaming)
+		{
+			SHORT	dBmToRoam = (SHORT)pAd->StaCfg.dBmToRoam;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("Rssi=%d, dBmToRoam=%d\n", RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2), (CHAR)dBmToRoam));
+
+			if (RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2) <= (CHAR)dBmToRoam)
+			{
+				MlmeCheckForFastRoaming(pAd, pAd->Mlme.Now32);
+			}
+		}
+	}
+	else if (ADHOC_ON(pAd))
+	{
+		// 2003-04-17 john. this is a patch that driver forces a BEACON out if ASIC fails
+		// the "TX BEACON competition" for the entire past 1 sec.
+		// So that even when ASIC's BEACONgen engine been blocked
+		// by peer's BEACON due to slower system clock, this STA still can send out
+		// minimum BEACON to tell the peer I'm alive.
+		// drawback is that this BEACON won't be well aligned at TBTT boundary.
+		// EnqueueBeaconFrame(pAd);			  // software send BEACON
+
+		// if all 11b peers leave this BSS more than 5 seconds, update Tx rate,
+		// restore outgoing BEACON to support B/G-mixed mode
+		if ((pAd->CommonCfg.Channel <= 14)			   &&
+			(pAd->CommonCfg.MaxTxRate <= RATE_11)	   &&
+			(pAd->CommonCfg.MaxDesiredRate > RATE_11)  &&
+			((pAd->StaCfg.Last11bBeaconRxTime + 5*OS_HZ) < pAd->Mlme.Now32))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - last 11B peer left, update Tx rates\n"));
+			NdisMoveMemory(pAd->StaActive.SupRate, pAd->CommonCfg.SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+			pAd->StaActive.SupRateLen = pAd->CommonCfg.SupRateLen;
+			MlmeUpdateTxRates(pAd, FALSE, 0);
+			MakeIbssBeacon(pAd);		// re-build BEACON frame
+			AsicEnableIbssSync(pAd);	// copy to on-chip memory
+			pAd->StaCfg.AdhocBOnlyJoined = FALSE;
+		}
+
+#ifdef DOT11_N_SUPPORT
+		if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+		{
+			if ((pAd->StaCfg.AdhocBGJoined) &&
+				((pAd->StaCfg.Last11gBeaconRxTime + 5 * OS_HZ) < pAd->Mlme.Now32))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - last 11G peer left\n"));
+				pAd->StaCfg.AdhocBGJoined = FALSE;
+			}
+
+			if ((pAd->StaCfg.Adhoc20NJoined) &&
+				((pAd->StaCfg.Last20NBeaconRxTime + 5 * OS_HZ) < pAd->Mlme.Now32))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - last 20MHz N peer left\n"));
+				pAd->StaCfg.Adhoc20NJoined = FALSE;
+			}
+		}
+#endif // DOT11_N_SUPPORT //
+
+		//radar detect
+		if ((pAd->CommonCfg.Channel > 14)
+			&& (pAd->CommonCfg.bIEEE80211H == 1)
+			&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+		{
+			RadarDetectPeriodic(pAd);
+		}
+
+		// If all peers leave, and this STA becomes the last one in this IBSS, then change MediaState
+		// to DISCONNECTED. But still holding this IBSS (i.e. sending BEACON) so that other STAs can
+		// join later.
+		if ((pAd->StaCfg.LastBeaconRxTime + ADHOC_BEACON_LOST_TIME < pAd->Mlme.Now32) &&
+			OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+		{
+			MLME_START_REQ_STRUCT     StartReq;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - excessive BEACON lost, last STA in this IBSS, MediaState=Disconnected\n"));
+			LinkDown(pAd, FALSE);
+
+			StartParmFill(pAd, &StartReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_START_REQ, sizeof(MLME_START_REQ_STRUCT), &StartReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_START;
+		}
+	}
+	else // no INFRA nor ADHOC connection
+	{
+
+		if (pAd->StaCfg.bScanReqIsFromWebUI &&
+            ((pAd->StaCfg.LastScanTime + 30 * OS_HZ) > pAd->Mlme.Now32))
+			goto SKIP_AUTO_SCAN_CONN;
+        else
+            pAd->StaCfg.bScanReqIsFromWebUI = FALSE;
+
+		if ((pAd->StaCfg.bAutoReconnect == TRUE)
+			&& RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP)
+			&& (MlmeValidateSSID(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen) == TRUE))
+		{
+			if ((pAd->ScanTab.BssNr==0) && (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE))
+			{
+				MLME_SCAN_REQ_STRUCT	   ScanReq;
+
+				if ((pAd->StaCfg.LastScanTime + 10 * OS_HZ) < pAd->Mlme.Now32)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("STAMlmePeriodicExec():CNTL - ScanTab.BssNr==0, start a new ACTIVE scan SSID[%s]\n", pAd->MlmeAux.AutoReconnectSsid));
+					ScanParmFill(pAd, &ScanReq, pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen, BSS_ANY, SCAN_ACTIVE);
+					MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+					pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+					// Reset Missed scan number
+					pAd->StaCfg.LastScanTime = pAd->Mlme.Now32;
+				}
+				else if (pAd->StaCfg.BssType == BSS_ADHOC)	// Quit the forever scan when in a very clean room
+					MlmeAutoReconnectLastSSID(pAd);
+			}
+			else if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+			{
+				if ((pAd->Mlme.OneSecPeriodicRound % 7) == 0)
+				{
+					MlmeAutoScan(pAd);
+					pAd->StaCfg.LastScanTime = pAd->Mlme.Now32;
+				}
+				else
+				{
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+					if (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+					{
+						if ((pAd->Mlme.OneSecPeriodicRound % 5) == 1)
+							MlmeAutoReconnectLastSSID(pAd);
+					}
+					else
+#endif // CARRIER_DETECTION_SUPPORT //
+						MlmeAutoReconnectLastSSID(pAd);
+				}
+			}
+		}
+	}
+
+SKIP_AUTO_SCAN_CONN:
+
+#ifdef DOT11_N_SUPPORT
+    if ((pAd->MacTab.Content[BSSID_WCID].TXBAbitmap !=0) && (pAd->MacTab.fAnyBASession == FALSE))
+	{
+		pAd->MacTab.fAnyBASession = TRUE;
+		AsicUpdateProtect(pAd, HT_FORCERTSCTS,  ALLN_SETPROTECT, FALSE, FALSE);
+	}
+	else if ((pAd->MacTab.Content[BSSID_WCID].TXBAbitmap ==0) && (pAd->MacTab.fAnyBASession == TRUE))
+	{
+		pAd->MacTab.fAnyBASession = FALSE;
+		AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode,  ALLN_SETPROTECT, FALSE, FALSE);
+	}
+#endif // DOT11_N_SUPPORT //
+
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SCAN_2040))
+		TriEventCounterMaintenance(pAd);
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+	return;
+}
+
+// Link down report
+VOID LinkDownExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	pAd->IndicateMediaState = NdisMediaStateDisconnected;
+	RTMP_IndicateMediaState(pAd);
+    pAd->ExtraInfo = GENERAL_LINK_DOWN;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeAutoScan(
+	IN PRTMP_ADAPTER pAd)
+{
+	// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+	if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Driver auto scan\n"));
+		MlmeEnqueue(pAd,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_BSSID_LIST_SCAN,
+					0,
+					NULL);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeAutoReconnectLastSSID(
+	IN PRTMP_ADAPTER pAd)
+{
+
+
+	// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+	if ((pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE) &&
+		(MlmeValidateSSID(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen) == TRUE))
+	{
+		NDIS_802_11_SSID OidSsid;
+		OidSsid.SsidLength = pAd->MlmeAux.AutoReconnectSsidLen;
+		NdisMoveMemory(OidSsid.Ssid, pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Driver auto reconnect to last OID_802_11_SSID setting - %s, len - %d\n", pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen));
+		MlmeEnqueue(pAd,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_SSID,
+					sizeof(NDIS_802_11_SSID),
+					&OidSsid);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	==========================================================================
+	Validate SSID for connection try and rescan purpose
+	Valid SSID will have visible chars only.
+	The valid length is from 0 to 32.
+	IRQL = DISPATCH_LEVEL
+	==========================================================================
+ */
+BOOLEAN MlmeValidateSSID(
+	IN PUCHAR	pSsid,
+	IN UCHAR	SsidLen)
+{
+	int	index;
+
+	if (SsidLen > MAX_LEN_OF_SSID)
+		return (FALSE);
+
+	// Check each character value
+	for (index = 0; index < SsidLen; index++)
+	{
+		if (pSsid[index] < 0x20)
+			return (FALSE);
+	}
+
+	// All checked
+	return (TRUE);
+}
+
+VOID MlmeSelectTxRateTable(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PUCHAR				*ppTable,
+	IN PUCHAR				pTableSize,
+	IN PUCHAR				pInitTxRateIdx)
+{
+	do
+	{
+		// decide the rate table for tuning
+		if (pAd->CommonCfg.TxRateTableSize > 0)
+		{
+			*ppTable = RateSwitchTable;
+			*pTableSize = RateSwitchTable[0];
+			*pInitTxRateIdx = RateSwitchTable[1];
+
+			break;
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		if ((pAd->OpMode == OPMODE_STA) && ADHOC_ON(pAd))
+		{
+#ifdef DOT11_N_SUPPORT
+			if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED) &&
+				!pAd->StaCfg.AdhocBOnlyJoined &&
+				!pAd->StaCfg.AdhocBGJoined &&
+				(pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) &&
+				((pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0x00) || (pAd->Antenna.field.TxPath == 1)))
+			{// 11N 1S Adhoc
+				*ppTable = RateSwitchTable11N1S;
+				*pTableSize = RateSwitchTable11N1S[0];
+				*pInitTxRateIdx = RateSwitchTable11N1S[1];
+
+			}
+			else if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED) &&
+					!pAd->StaCfg.AdhocBOnlyJoined &&
+					!pAd->StaCfg.AdhocBGJoined &&
+					(pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) &&
+					(pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0xff) &&
+					(pAd->Antenna.field.TxPath == 2))
+			{// 11N 2S Adhoc
+				if (pAd->LatchRfRegs.Channel <= 14)
+				{
+					*ppTable = RateSwitchTable11N2S;
+					*pTableSize = RateSwitchTable11N2S[0];
+					*pInitTxRateIdx = RateSwitchTable11N2S[1];
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11N2SForABand;
+					*pTableSize = RateSwitchTable11N2SForABand[0];
+					*pInitTxRateIdx = RateSwitchTable11N2SForABand[1];
+				}
+
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+				if (pAd->CommonCfg.PhyMode == PHY_11B)
+			{
+				*ppTable = RateSwitchTable11B;
+				*pTableSize = RateSwitchTable11B[0];
+				*pInitTxRateIdx = RateSwitchTable11B[1];
+
+			}
+	        else if((pAd->LatchRfRegs.Channel <= 14) && (pAd->StaCfg.AdhocBOnlyJoined == TRUE))
+			{
+				// USe B Table when Only b-only Station in my IBSS .
+				*ppTable = RateSwitchTable11B;
+				*pTableSize = RateSwitchTable11B[0];
+				*pInitTxRateIdx = RateSwitchTable11B[1];
+
+			}
+			else if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				*ppTable = RateSwitchTable11BG;
+				*pTableSize = RateSwitchTable11BG[0];
+				*pInitTxRateIdx = RateSwitchTable11BG[1];
+
+			}
+			else
+			{
+				*ppTable = RateSwitchTable11G;
+				*pTableSize = RateSwitchTable11G[0];
+				*pInitTxRateIdx = RateSwitchTable11G[1];
+
+			}
+			break;
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+		if ((pEntry->RateLen == 12) && (pEntry->HTCapability.MCSSet[0] == 0xff) &&
+			((pEntry->HTCapability.MCSSet[1] == 0x00) || (pAd->CommonCfg.TxStream == 1)))
+		{// 11BGN 1S AP
+			*ppTable = RateSwitchTable11BGN1S;
+			*pTableSize = RateSwitchTable11BGN1S[0];
+			*pInitTxRateIdx = RateSwitchTable11BGN1S[1];
+
+			break;
+		}
+
+		if ((pEntry->RateLen == 12) && (pEntry->HTCapability.MCSSet[0] == 0xff) &&
+			(pEntry->HTCapability.MCSSet[1] == 0xff) && (pAd->CommonCfg.TxStream == 2))
+		{// 11BGN 2S AP
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				*ppTable = RateSwitchTable11BGN2S;
+				*pTableSize = RateSwitchTable11BGN2S[0];
+				*pInitTxRateIdx = RateSwitchTable11BGN2S[1];
+
+			}
+			else
+			{
+				*ppTable = RateSwitchTable11BGN2SForABand;
+				*pTableSize = RateSwitchTable11BGN2SForABand[0];
+				*pInitTxRateIdx = RateSwitchTable11BGN2SForABand[1];
+
+			}
+			break;
+		}
+
+		if ((pEntry->HTCapability.MCSSet[0] == 0xff) && ((pEntry->HTCapability.MCSSet[1] == 0x00) || (pAd->CommonCfg.TxStream == 1)))
+		{// 11N 1S AP
+			*ppTable = RateSwitchTable11N1S;
+			*pTableSize = RateSwitchTable11N1S[0];
+			*pInitTxRateIdx = RateSwitchTable11N1S[1];
+
+			break;
+		}
+
+		if ((pEntry->HTCapability.MCSSet[0] == 0xff) && (pEntry->HTCapability.MCSSet[1] == 0xff) && (pAd->CommonCfg.TxStream == 2))
+		{// 11N 2S AP
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+			*ppTable = RateSwitchTable11N2S;
+			*pTableSize = RateSwitchTable11N2S[0];
+			*pInitTxRateIdx = RateSwitchTable11N2S[1];
+            }
+			else
+			{
+				*ppTable = RateSwitchTable11N2SForABand;
+				*pTableSize = RateSwitchTable11N2SForABand[0];
+				*pInitTxRateIdx = RateSwitchTable11N2SForABand[1];
+			}
+
+			break;
+		}
+#endif // DOT11_N_SUPPORT //
+		//else if ((pAd->StaActive.SupRateLen == 4) && (pAd->StaActive.ExtRateLen == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+		if ((pEntry->RateLen == 4)
+#ifdef DOT11_N_SUPPORT
+			&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+			)
+		{// B only AP
+			*ppTable = RateSwitchTable11B;
+			*pTableSize = RateSwitchTable11B[0];
+			*pInitTxRateIdx = RateSwitchTable11B[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen > 8) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+		if ((pEntry->RateLen > 8)
+#ifdef DOT11_N_SUPPORT
+			&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+			)
+		{// B/G  mixed AP
+			*ppTable = RateSwitchTable11BG;
+			*pTableSize = RateSwitchTable11BG[0];
+			*pInitTxRateIdx = RateSwitchTable11BG[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen == 8) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+		if ((pEntry->RateLen == 8)
+#ifdef DOT11_N_SUPPORT
+			&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+			)
+		{// G only AP
+			*ppTable = RateSwitchTable11G;
+			*pTableSize = RateSwitchTable11G[0];
+			*pInitTxRateIdx = RateSwitchTable11G[1];
+
+			break;
+		}
+#ifdef DOT11_N_SUPPORT
+#endif // DOT11_N_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef DOT11_N_SUPPORT
+			//else if ((pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+			if ((pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0))
+#endif // DOT11_N_SUPPORT //
+			{	// Legacy mode
+				if (pAd->CommonCfg.MaxTxRate <= RATE_11)
+				{
+					*ppTable = RateSwitchTable11B;
+					*pTableSize = RateSwitchTable11B[0];
+					*pInitTxRateIdx = RateSwitchTable11B[1];
+				}
+				else if ((pAd->CommonCfg.MaxTxRate > RATE_11) && (pAd->CommonCfg.MinTxRate > RATE_11))
+				{
+					*ppTable = RateSwitchTable11G;
+					*pTableSize = RateSwitchTable11G[0];
+					*pInitTxRateIdx = RateSwitchTable11G[1];
+
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11BG;
+					*pTableSize = RateSwitchTable11BG[0];
+					*pInitTxRateIdx = RateSwitchTable11BG[1];
+				}
+				break;
+			}
+#ifdef DOT11_N_SUPPORT
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				if (pAd->CommonCfg.TxStream == 1)
+				{
+					*ppTable = RateSwitchTable11N1S;
+					*pTableSize = RateSwitchTable11N1S[0];
+					*pInitTxRateIdx = RateSwitchTable11N1S[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 1S AP \n"));
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11N2S;
+					*pTableSize = RateSwitchTable11N2S[0];
+					*pInitTxRateIdx = RateSwitchTable11N2S[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 2S AP \n"));
+				}
+			}
+			else
+			{
+				if (pAd->CommonCfg.TxStream == 1)
+				{
+					*ppTable = RateSwitchTable11N1S;
+					*pTableSize = RateSwitchTable11N1S[0];
+					*pInitTxRateIdx = RateSwitchTable11N1S[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 1S AP \n"));
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11N2SForABand;
+					*pTableSize = RateSwitchTable11N2SForABand[0];
+					*pInitTxRateIdx = RateSwitchTable11N2SForABand[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 2S AP \n"));
+				}
+			}
+#endif // DOT11_N_SUPPORT //
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode (SupRateLen=%d, ExtRateLen=%d, MCSSet[0]=0x%x, MCSSet[1]=0x%x)\n",
+				pAd->StaActive.SupRateLen, pAd->StaActive.ExtRateLen, pAd->StaActive.SupportedPhyInfo.MCSSet[0], pAd->StaActive.SupportedPhyInfo.MCSSet[1]));
+		}
+#endif // CONFIG_STA_SUPPORT //
+	} while(FALSE);
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	==========================================================================
+	Description:
+		This routine checks if there're other APs out there capable for
+		roaming. Caller should call this routine only when Link up in INFRA mode
+		and channel quality is below CQI_GOOD_THRESHOLD.
+
+	IRQL = DISPATCH_LEVEL
+
+	Output:
+	==========================================================================
+ */
+VOID MlmeCheckForRoaming(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG	Now32)
+{
+	USHORT	   i;
+	BSS_TABLE  *pRoamTab = &pAd->MlmeAux.RoamTab;
+	BSS_ENTRY  *pBss;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeCheckForRoaming\n"));
+	// put all roaming candidates into RoamTab, and sort in RSSI order
+	BssTableInit(pRoamTab);
+	for (i = 0; i < pAd->ScanTab.BssNr; i++)
+	{
+		pBss = &pAd->ScanTab.BssEntry[i];
+
+		if ((pBss->LastBeaconRxTime + BEACON_LOST_TIME) < Now32)
+			continue;	 // AP disappear
+		if (pBss->Rssi <= RSSI_THRESHOLD_FOR_ROAMING)
+			continue;	 // RSSI too weak. forget it.
+		if (MAC_ADDR_EQUAL(pBss->Bssid, pAd->CommonCfg.Bssid))
+			continue;	 // skip current AP
+		if (pBss->Rssi < (pAd->StaCfg.RssiSample.LastRssi0 + RSSI_DELTA))
+			continue;	 // only AP with stronger RSSI is eligible for roaming
+
+		// AP passing all above rules is put into roaming candidate table
+		NdisMoveMemory(&pRoamTab->BssEntry[pRoamTab->BssNr], pBss, sizeof(BSS_ENTRY));
+		pRoamTab->BssNr += 1;
+	}
+
+	if (pRoamTab->BssNr > 0)
+	{
+		// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+		if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+		{
+			pAd->RalinkCounters.PoorCQIRoamingCount ++;
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Roaming attempt #%ld\n", pAd->RalinkCounters.PoorCQIRoamingCount));
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_MLME_ROAMING_REQ, 0, NULL);
+			RT28XX_MLME_HANDLER(pAd);
+		}
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("<== MlmeCheckForRoaming(# of candidate= %d)\n",pRoamTab->BssNr));
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine checks if there're other APs out there capable for
+		roaming. Caller should call this routine only when link up in INFRA mode
+		and channel quality is below CQI_GOOD_THRESHOLD.
+
+	IRQL = DISPATCH_LEVEL
+
+	Output:
+	==========================================================================
+ */
+VOID MlmeCheckForFastRoaming(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ULONG			Now)
+{
+	USHORT		i;
+	BSS_TABLE	*pRoamTab = &pAd->MlmeAux.RoamTab;
+	BSS_ENTRY	*pBss;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeCheckForFastRoaming\n"));
+	// put all roaming candidates into RoamTab, and sort in RSSI order
+	BssTableInit(pRoamTab);
+	for (i = 0; i < pAd->ScanTab.BssNr; i++)
+	{
+		pBss = &pAd->ScanTab.BssEntry[i];
+
+        if ((pBss->Rssi <= -50) && (pBss->Channel == pAd->CommonCfg.Channel))
+			continue;	 // RSSI too weak. forget it.
+		if (MAC_ADDR_EQUAL(pBss->Bssid, pAd->CommonCfg.Bssid))
+			continue;	 // skip current AP
+		if (!SSID_EQUAL(pBss->Ssid, pBss->SsidLen, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen))
+			continue;	 // skip different SSID
+        if (pBss->Rssi < (RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2) + RSSI_DELTA))
+			continue;	 // skip AP without better RSSI
+
+        DBGPRINT(RT_DEBUG_TRACE, ("LastRssi0 = %d, pBss->Rssi = %d\n", RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2), pBss->Rssi));
+		// AP passing all above rules is put into roaming candidate table
+		NdisMoveMemory(&pRoamTab->BssEntry[pRoamTab->BssNr], pBss, sizeof(BSS_ENTRY));
+		pRoamTab->BssNr += 1;
+	}
+
+	if (pRoamTab->BssNr > 0)
+	{
+		// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+		if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+		{
+			pAd->RalinkCounters.PoorCQIRoamingCount ++;
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Roaming attempt #%ld\n", pAd->RalinkCounters.PoorCQIRoamingCount));
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_MLME_ROAMING_REQ, 0, NULL);
+			RT28XX_MLME_HANDLER(pAd);
+		}
+	}
+	// Maybe site survey required
+	else
+	{
+		if ((pAd->StaCfg.LastScanTime + 10 * 1000) < Now)
+		{
+			// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Roaming, No eligable entry, try new scan!\n"));
+			pAd->StaCfg.ScanCnt = 2;
+			pAd->StaCfg.LastScanTime = Now;
+			MlmeAutoScan(pAd);
+		}
+	}
+
+    DBGPRINT(RT_DEBUG_TRACE, ("<== MlmeCheckForFastRoaming (BssNr=%d)\n", pRoamTab->BssNr));
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine calculates TxPER, RxPER of the past N-sec period. And
+		according to the calculation result, ChannelQuality is calculated here
+		to decide if current AP is still doing the job.
+
+		If ChannelQuality is not good, a ROAMing attempt may be tried later.
+	Output:
+		StaCfg.ChannelQuality - 0..100
+
+	IRQL = DISPATCH_LEVEL
+
+	NOTE: This routine decide channle quality based on RX CRC error ratio.
+		Caller should make sure a function call to NICUpdateRawCounters(pAd)
+		is performed right before this routine, so that this routine can decide
+		channel quality based on the most up-to-date information
+	==========================================================================
+ */
+VOID MlmeCalculateChannelQuality(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Now32)
+{
+	ULONG TxOkCnt, TxCnt, TxPER, TxPRR;
+	ULONG RxCnt, RxPER;
+	UCHAR NorRssi;
+	CHAR  MaxRssi;
+	ULONG BeaconLostTime = BEACON_LOST_TIME;
+
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+	// longer beacon lost time when carrier detection enabled
+	if (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+	{
+		BeaconLostTime = BEACON_LOST_TIME + BEACON_LOST_TIME/2;
+	}
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	MaxRssi = RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2);
+
+	//
+	// calculate TX packet error ratio and TX retry ratio - if too few TX samples, skip TX related statistics
+	//
+	TxOkCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount + pAd->RalinkCounters.OneSecTxRetryOkCount;
+	TxCnt = TxOkCnt + pAd->RalinkCounters.OneSecTxFailCount;
+	if (TxCnt < 5)
+	{
+		TxPER = 0;
+		TxPRR = 0;
+	}
+	else
+	{
+		TxPER = (pAd->RalinkCounters.OneSecTxFailCount * 100) / TxCnt;
+		TxPRR = ((TxCnt - pAd->RalinkCounters.OneSecTxNoRetryOkCount) * 100) / TxCnt;
+	}
+
+	//
+	// calculate RX PER - don't take RxPER into consideration if too few sample
+	//
+	RxCnt = pAd->RalinkCounters.OneSecRxOkCnt + pAd->RalinkCounters.OneSecRxFcsErrCnt;
+	if (RxCnt < 5)
+		RxPER = 0;
+	else
+		RxPER = (pAd->RalinkCounters.OneSecRxFcsErrCnt * 100) / RxCnt;
+
+	//
+	// decide ChannelQuality based on: 1)last BEACON received time, 2)last RSSI, 3)TxPER, and 4)RxPER
+	//
+	if (INFRA_ON(pAd) &&
+		(pAd->RalinkCounters.OneSecTxNoRetryOkCount < 2) && // no heavy traffic
+		(pAd->StaCfg.LastBeaconRxTime + BeaconLostTime < Now32))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("BEACON lost > %ld msec with TxOkCnt=%ld -> CQI=0\n", BeaconLostTime, TxOkCnt));
+		pAd->Mlme.ChannelQuality = 0;
+	}
+	else
+	{
+		// Normalize Rssi
+		if (MaxRssi > -40)
+			NorRssi = 100;
+		else if (MaxRssi < -90)
+			NorRssi = 0;
+		else
+			NorRssi = (MaxRssi + 90) * 2;
+
+		// ChannelQuality = W1*RSSI + W2*TxPRR + W3*RxPER	 (RSSI 0..100), (TxPER 100..0), (RxPER 100..0)
+		pAd->Mlme.ChannelQuality = (RSSI_WEIGHTING * NorRssi +
+								   TX_WEIGHTING * (100 - TxPRR) +
+								   RX_WEIGHTING* (100 - RxPER)) / 100;
+		if (pAd->Mlme.ChannelQuality >= 100)
+			pAd->Mlme.ChannelQuality = 100;
+	}
+
+}
+
+VOID MlmeSetTxRate(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PRTMP_TX_RATE_SWITCH	pTxRate)
+{
+	UCHAR	MaxMode = MODE_OFDM;
+
+#ifdef DOT11_N_SUPPORT
+	MaxMode = MODE_HTGREENFIELD;
+
+	if (pTxRate->STBC && (pAd->StaCfg.MaxHTPhyMode.field.STBC) && (pAd->Antenna.field.TxPath == 2))
+		pAd->StaCfg.HTPhyMode.field.STBC = STBC_USE;
+	else
+#endif // DOT11_N_SUPPORT //
+		pAd->StaCfg.HTPhyMode.field.STBC = STBC_NONE;
+
+	if (pTxRate->CurrMCS < MCS_AUTO)
+		pAd->StaCfg.HTPhyMode.field.MCS = pTxRate->CurrMCS;
+
+	if (pAd->StaCfg.HTPhyMode.field.MCS > 7)
+		pAd->StaCfg.HTPhyMode.field.STBC = STBC_NONE;
+
+   	if (ADHOC_ON(pAd))
+	{
+		// If peer adhoc is b-only mode, we can't send 11g rate.
+		pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+		pEntry->HTPhyMode.field.STBC	= STBC_NONE;
+
+		//
+		// For Adhoc MODE_CCK, driver will use AdhocBOnlyJoined flag to roll back to B only if necessary
+		//
+		pEntry->HTPhyMode.field.MODE	= pTxRate->Mode;
+		pEntry->HTPhyMode.field.ShortGI	= pAd->StaCfg.HTPhyMode.field.ShortGI;
+		pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+
+		// Patch speed error in status page
+		pAd->StaCfg.HTPhyMode.field.MODE = pEntry->HTPhyMode.field.MODE;
+	}
+	else
+	{
+		if (pTxRate->Mode <= MaxMode)
+			pAd->StaCfg.HTPhyMode.field.MODE = pTxRate->Mode;
+
+#ifdef DOT11_N_SUPPORT
+		if (pTxRate->ShortGI && (pAd->StaCfg.MaxHTPhyMode.field.ShortGI))
+			pAd->StaCfg.HTPhyMode.field.ShortGI = GI_400;
+		else
+#endif // DOT11_N_SUPPORT //
+			pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+
+#ifdef DOT11_N_SUPPORT
+		// Reexam each bandwidth's SGI support.
+		if (pAd->StaCfg.HTPhyMode.field.ShortGI == GI_400)
+		{
+			if ((pEntry->HTPhyMode.field.BW == BW_20) && (!CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE)))
+				pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+			if ((pEntry->HTPhyMode.field.BW == BW_40) && (!CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE)))
+				pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+		}
+
+		// Turn RTS/CTS rate to 6Mbps.
+		if ((pEntry->HTPhyMode.field.MCS == 0) && (pAd->StaCfg.HTPhyMode.field.MCS != 0))
+		{
+			pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+			if (pAd->MacTab.fAnyBASession)
+			{
+				AsicUpdateProtect(pAd, HT_FORCERTSCTS, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+			else
+			{
+				AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+		}
+		else if ((pEntry->HTPhyMode.field.MCS == 8) && (pAd->StaCfg.HTPhyMode.field.MCS != 8))
+		{
+			pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+			if (pAd->MacTab.fAnyBASession)
+			{
+				AsicUpdateProtect(pAd, HT_FORCERTSCTS, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+			else
+			{
+				AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+		}
+		else if ((pEntry->HTPhyMode.field.MCS != 0) && (pAd->StaCfg.HTPhyMode.field.MCS == 0))
+		{
+			AsicUpdateProtect(pAd, HT_RTSCTS_6M, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+
+		}
+		else if ((pEntry->HTPhyMode.field.MCS != 8) && (pAd->StaCfg.HTPhyMode.field.MCS == 8))
+		{
+			AsicUpdateProtect(pAd, HT_RTSCTS_6M, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+		}
+#endif // DOT11_N_SUPPORT //
+
+		pEntry->HTPhyMode.field.STBC	= pAd->StaCfg.HTPhyMode.field.STBC;
+		pEntry->HTPhyMode.field.ShortGI	= pAd->StaCfg.HTPhyMode.field.ShortGI;
+		pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+		pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+#ifdef DOT11_N_SUPPORT
+		if ((pAd->StaCfg.MaxHTPhyMode.field.MODE == MODE_HTGREENFIELD) &&
+		    pAd->WIFItestbed.bGreenField)
+		    pEntry->HTPhyMode.field.MODE = MODE_HTGREENFIELD;
+#endif // DOT11_N_SUPPORT //
+	}
+
+	pAd->LastTxRate = (USHORT)(pEntry->HTPhyMode.word);
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine calculates the acumulated TxPER of eaxh TxRate. And
+		according to the calculation result, change CommonCfg.TxRate which
+		is the stable TX Rate we expect the Radio situation could sustained.
+
+		CommonCfg.TxRate will change dynamically within {RATE_1/RATE_6, MaxTxRate}
+	Output:
+		CommonCfg.TxRate -
+
+	IRQL = DISPATCH_LEVEL
+
+	NOTE:
+		call this routine every second
+	==========================================================================
+ */
+VOID MlmeDynamicTxRateSwitching(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR					UpRateIdx = 0, DownRateIdx = 0, CurrRateIdx;
+	ULONG					i, AccuTxTotalCnt = 0, TxTotalCnt;
+	ULONG					TxErrorRatio = 0;
+	BOOLEAN					bTxRateChanged, bUpgradeQuality = FALSE;
+	PRTMP_TX_RATE_SWITCH	pCurrTxRate, pNextTxRate = NULL;
+	PUCHAR					pTable;
+	UCHAR					TableSize = 0;
+	UCHAR					InitTxRateIdx = 0, TrainUp, TrainDown;
+	CHAR					Rssi, RssiOffset = 0;
+	TX_STA_CNT1_STRUC		StaTx1;
+	TX_STA_CNT0_STRUC		TxStaCnt0;
+	ULONG					TxRetransmit = 0, TxSuccess = 0, TxFailCount = 0;
+	MAC_TABLE_ENTRY			*pEntry;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		return;
+	}
+#endif // RALINK_ATE //
+
+	/*if (pAd->Antenna.field.RxPath > 1)
+		Rssi = (pAd->StaCfg.RssiSample.AvgRssi0 + pAd->StaCfg.RssiSample.AvgRssi1) >> 1;
+	else
+		Rssi = pAd->StaCfg.RssiSample.AvgRssi0;*/
+
+	//
+	// walk through MAC table, see if need to change AP's TX rate toward each entry
+	//
+   	for (i = 1; i < MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		pEntry = &pAd->MacTab.Content[i];
+
+		// check if this entry need to switch rate automatically
+		if (RTMPCheckEntryEnableAutoRateSwitch(pAd, pEntry) == FALSE)
+			continue;
+
+		if ((pAd->MacTab.Size == 1) || (pEntry->ValidAsDls))
+		{
+			Rssi = RTMPMaxRssi(pAd, (CHAR)pAd->StaCfg.RssiSample.AvgRssi0, (CHAR)pAd->StaCfg.RssiSample.AvgRssi1, (CHAR)pAd->StaCfg.RssiSample.AvgRssi2);
+
+			// Update statistic counter
+			RTMP_IO_READ32(pAd, TX_STA_CNT0, &TxStaCnt0.word);
+			RTMP_IO_READ32(pAd, TX_STA_CNT1, &StaTx1.word);
+			pAd->bUpdateBcnCntDone = TRUE;
+			TxRetransmit = StaTx1.field.TxRetransmit;
+			TxSuccess = StaTx1.field.TxSuccess;
+			TxFailCount = TxStaCnt0.field.TxFailCount;
+			TxTotalCnt = TxRetransmit + TxSuccess + TxFailCount;
+
+			pAd->RalinkCounters.OneSecTxRetryOkCount += StaTx1.field.TxRetransmit;
+			pAd->RalinkCounters.OneSecTxNoRetryOkCount += StaTx1.field.TxSuccess;
+			pAd->RalinkCounters.OneSecTxFailCount += TxStaCnt0.field.TxFailCount;
+			pAd->WlanCounters.TransmittedFragmentCount.u.LowPart += StaTx1.field.TxSuccess;
+			pAd->WlanCounters.RetryCount.u.LowPart += StaTx1.field.TxRetransmit;
+			pAd->WlanCounters.FailedCount.u.LowPart += TxStaCnt0.field.TxFailCount;
+
+			// if no traffic in the past 1-sec period, don't change TX rate,
+			// but clear all bad history. because the bad history may affect the next
+			// Chariot throughput test
+			AccuTxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((TxRetransmit + TxFailCount) * 100) / TxTotalCnt;
+		}
+		else
+		{
+			Rssi = RTMPMaxRssi(pAd, (CHAR)pEntry->RssiSample.AvgRssi0, (CHAR)pEntry->RssiSample.AvgRssi1, (CHAR)pEntry->RssiSample.AvgRssi2);
+
+			TxTotalCnt = pEntry->OneSecTxNoRetryOkCount +
+				 pEntry->OneSecTxRetryOkCount +
+				 pEntry->OneSecTxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((pEntry->OneSecTxRetryOkCount + pEntry->OneSecTxFailCount) * 100) / TxTotalCnt;
+		}
+
+		CurrRateIdx = pEntry->CurrTxRateIndex;
+
+		MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &InitTxRateIdx);
+
+		if (CurrRateIdx >= TableSize)
+		{
+			CurrRateIdx = TableSize - 1;
+		}
+
+		// When switch from Fixed rate -> auto rate, the REAL TX rate might be different from pAd->CommonCfg.TxRateIndex.
+		// So need to sync here.
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(CurrRateIdx+1)*5];
+		if ((pEntry->HTPhyMode.field.MCS != pCurrTxRate->CurrMCS)
+			//&& (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+			)
+		{
+
+			// Need to sync Real Tx rate and our record.
+			// Then return for next DRS.
+			pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(InitTxRateIdx+1)*5];
+			pEntry->CurrTxRateIndex = InitTxRateIdx;
+			MlmeSetTxRate(pAd, pEntry, pCurrTxRate);
+
+			// reset all OneSecTx counters
+			RESET_ONE_SEC_TX_CNT(pEntry);
+			continue;
+		}
+
+		// decide the next upgrade rate and downgrade rate, if any
+		if ((CurrRateIdx > 0) && (CurrRateIdx < (TableSize - 1)))
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx -1;
+		}
+		else if (CurrRateIdx == 0)
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx;
+		}
+		else if (CurrRateIdx == (TableSize - 1))
+		{
+			UpRateIdx = CurrRateIdx;
+			DownRateIdx = CurrRateIdx - 1;
+		}
+
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(CurrRateIdx+1)*5];
+
+#ifdef DOT11_N_SUPPORT
+		if ((Rssi > -65) && (pCurrTxRate->Mode >= MODE_HTMIX))
+		{
+			TrainUp		= (pCurrTxRate->TrainUp + (pCurrTxRate->TrainUp >> 1));
+			TrainDown	= (pCurrTxRate->TrainDown + (pCurrTxRate->TrainDown >> 1));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			TrainUp		= pCurrTxRate->TrainUp;
+			TrainDown	= pCurrTxRate->TrainDown;
+		}
+
+		//pAd->DrsCounters.LastTimeTxRateChangeAction = pAd->DrsCounters.LastSecTxRateChangeAction;
+
+		//
+		// Keep the last time TxRateChangeAction status.
+		//
+		pEntry->LastTimeTxRateChangeAction = pEntry->LastSecTxRateChangeAction;
+
+
+
+		//
+		// CASE 1. when TX samples are fewer than 15, then decide TX rate solely on RSSI
+		//         (criteria copied from RT2500 for Netopia case)
+		//
+		if (TxTotalCnt <= 15)
+		{
+			CHAR	idx = 0;
+			UCHAR	TxRateIdx;
+			//UCHAR	MCS0 = 0, MCS1 = 0, MCS2 = 0, MCS3 = 0, MCS4 = 0, MCS7 = 0, MCS12 = 0, MCS13 = 0, MCS14 = 0, MCS15 = 0;
+			UCHAR	MCS0 = 0, MCS1 = 0, MCS2 = 0, MCS3 = 0, MCS4 = 0,  MCS5 =0, MCS6 = 0, MCS7 = 0;
+	        UCHAR	MCS12 = 0, MCS13 = 0, MCS14 = 0, MCS15 = 0;
+			UCHAR	MCS20 = 0, MCS21 = 0, MCS22 = 0, MCS23 = 0; // 3*3
+
+			// check the existence and index of each needed MCS
+			while (idx < pTable[0])
+			{
+				pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(idx+1)*5];
+
+				if (pCurrTxRate->CurrMCS == MCS_0)
+				{
+					MCS0 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_1)
+				{
+					MCS1 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_2)
+				{
+					MCS2 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_3)
+				{
+					MCS3 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_4)
+				{
+					MCS4 = idx;
+				}
+	            else if (pCurrTxRate->CurrMCS == MCS_5)
+	            {
+	                MCS5 = idx;
+	            }
+	            else if (pCurrTxRate->CurrMCS == MCS_6)
+	            {
+	                MCS6 = idx;
+	            }
+				//else if (pCurrTxRate->CurrMCS == MCS_7)
+				else if ((pCurrTxRate->CurrMCS == MCS_7) && (pCurrTxRate->ShortGI == GI_800))	// prevent the highest MCS using short GI when 1T and low throughput
+				{
+					MCS7 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_12)
+				{
+					MCS12 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_13)
+				{
+					MCS13 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_14)
+				{
+					MCS14 = idx;
+				}
+				//else if ((pCurrTxRate->CurrMCS == MCS_15)/* && (pCurrTxRate->ShortGI == GI_800)*/)	//we hope to use ShortGI as initial rate
+				else if ((pCurrTxRate->CurrMCS == MCS_15) && (pCurrTxRate->ShortGI == GI_800))	//we hope to use ShortGI as initial rate, however Atheros's chip has bugs when short GI
+				{
+					MCS15 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_20) // 3*3
+				{
+					MCS20 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_21)
+				{
+					MCS21 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_22)
+				{
+					MCS22 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_23)
+				{
+					MCS23 = idx;
+				}
+				idx ++;
+			}
+
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				if (pAd->NicConfig2.field.ExternalLNAForG)
+				{
+					RssiOffset = 2;
+				}
+				else
+				{
+					RssiOffset = 5;
+				}
+			}
+			else
+			{
+				if (pAd->NicConfig2.field.ExternalLNAForA)
+				{
+					RssiOffset = 5;
+				}
+				else
+				{
+					RssiOffset = 8;
+				}
+			}
+#ifdef DOT11_N_SUPPORT
+			/*if (MCS15)*/
+			if ((pTable == RateSwitchTable11BGN3S) ||
+				(pTable == RateSwitchTable11N3S) ||
+				(pTable == RateSwitchTable))
+			{// N mode with 3 stream // 3*3
+				if (MCS23 && (Rssi >= -70))
+					TxRateIdx = MCS15;
+				else if (MCS22 && (Rssi >= -72))
+					TxRateIdx = MCS14;
+        	    else if (MCS21 && (Rssi >= -76))
+					TxRateIdx = MCS13;
+				else if (MCS20 && (Rssi >= -78))
+					TxRateIdx = MCS12;
+			else if (MCS4 && (Rssi >= -82))
+				TxRateIdx = MCS4;
+			else if (MCS3 && (Rssi >= -84))
+				TxRateIdx = MCS3;
+			else if (MCS2 && (Rssi >= -86))
+				TxRateIdx = MCS2;
+			else if (MCS1 && (Rssi >= -88))
+				TxRateIdx = MCS1;
+			else
+				TxRateIdx = MCS0;
+		}
+		else if ((pTable == RateSwitchTable11BGN2S) || (pTable == RateSwitchTable11BGN2SForABand) ||(pTable == RateSwitchTable11N2S) ||(pTable == RateSwitchTable11N2SForABand)) // 3*3
+			{// N mode with 2 stream
+				if (MCS15 && (Rssi >= (-70+RssiOffset)))
+					TxRateIdx = MCS15;
+				else if (MCS14 && (Rssi >= (-72+RssiOffset)))
+					TxRateIdx = MCS14;
+				else if (MCS13 && (Rssi >= (-76+RssiOffset)))
+					TxRateIdx = MCS13;
+				else if (MCS12 && (Rssi >= (-78+RssiOffset)))
+					TxRateIdx = MCS12;
+				else if (MCS4 && (Rssi >= (-82+RssiOffset)))
+					TxRateIdx = MCS4;
+				else if (MCS3 && (Rssi >= (-84+RssiOffset)))
+					TxRateIdx = MCS3;
+				else if (MCS2 && (Rssi >= (-86+RssiOffset)))
+					TxRateIdx = MCS2;
+				else if (MCS1 && (Rssi >= (-88+RssiOffset)))
+					TxRateIdx = MCS1;
+				else
+					TxRateIdx = MCS0;
+			}
+			else if ((pTable == RateSwitchTable11BGN1S) || (pTable == RateSwitchTable11N1S))
+			{// N mode with 1 stream
+				if (MCS7 && (Rssi > (-72+RssiOffset)))
+					TxRateIdx = MCS7;
+				else if (MCS6 && (Rssi > (-74+RssiOffset)))
+					TxRateIdx = MCS6;
+				else if (MCS5 && (Rssi > (-77+RssiOffset)))
+					TxRateIdx = MCS5;
+				else if (MCS4 && (Rssi > (-79+RssiOffset)))
+					TxRateIdx = MCS4;
+				else if (MCS3 && (Rssi > (-81+RssiOffset)))
+					TxRateIdx = MCS3;
+				else if (MCS2 && (Rssi > (-83+RssiOffset)))
+					TxRateIdx = MCS2;
+				else if (MCS1 && (Rssi > (-86+RssiOffset)))
+					TxRateIdx = MCS1;
+				else
+					TxRateIdx = MCS0;
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+			{// Legacy mode
+				if (MCS7 && (Rssi > -70))
+					TxRateIdx = MCS7;
+				else if (MCS6 && (Rssi > -74))
+					TxRateIdx = MCS6;
+				else if (MCS5 && (Rssi > -78))
+					TxRateIdx = MCS5;
+				else if (MCS4 && (Rssi > -82))
+					TxRateIdx = MCS4;
+				else if (MCS4 == 0)	// for B-only mode
+					TxRateIdx = MCS3;
+				else if (MCS3 && (Rssi > -85))
+					TxRateIdx = MCS3;
+				else if (MCS2 && (Rssi > -87))
+					TxRateIdx = MCS2;
+				else if (MCS1 && (Rssi > -90))
+					TxRateIdx = MCS1;
+				else
+					TxRateIdx = MCS0;
+			}
+
+			{
+				pEntry->CurrTxRateIndex = TxRateIdx;
+				pNextTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(pEntry->CurrTxRateIndex+1)*5];
+				MlmeSetTxRate(pAd, pEntry, pNextTxRate);
+			}
+
+			NdisZeroMemory(pEntry->TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pEntry->PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+			pEntry->fLastSecAccordingRSSI = TRUE;
+			// reset all OneSecTx counters
+			RESET_ONE_SEC_TX_CNT(pEntry);
+
+			continue;
+		}
+
+		if (pEntry->fLastSecAccordingRSSI == TRUE)
+		{
+			pEntry->fLastSecAccordingRSSI = FALSE;
+			pEntry->LastSecTxRateChangeAction = 0;
+			// reset all OneSecTx counters
+			RESET_ONE_SEC_TX_CNT(pEntry);
+
+			continue;
+		}
+
+		do
+		{
+			BOOLEAN	bTrainUpDown = FALSE;
+
+			pEntry->CurrTxRateStableTime ++;
+
+			// downgrade TX quality if PER >= Rate-Down threshold
+			if (TxErrorRatio >= TrainDown)
+			{
+				bTrainUpDown = TRUE;
+				pEntry->TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+			}
+			// upgrade TX quality if PER <= Rate-Up threshold
+			else if (TxErrorRatio <= TrainUp)
+			{
+				bTrainUpDown = TRUE;
+				bUpgradeQuality = TRUE;
+				if (pEntry->TxQuality[CurrRateIdx])
+					pEntry->TxQuality[CurrRateIdx] --;  // quality very good in CurrRate
+
+				if (pEntry->TxRateUpPenalty)
+					pEntry->TxRateUpPenalty --;
+				else if (pEntry->TxQuality[UpRateIdx])
+					pEntry->TxQuality[UpRateIdx] --;    // may improve next UP rate's quality
+			}
+
+			pEntry->PER[CurrRateIdx] = (UCHAR)TxErrorRatio;
+
+			if (bTrainUpDown)
+			{
+				// perform DRS - consider TxRate Down first, then rate up.
+				if ((CurrRateIdx != DownRateIdx) && (pEntry->TxQuality[CurrRateIdx] >= DRS_TX_QUALITY_WORST_BOUND))
+				{
+					pEntry->CurrTxRateIndex = DownRateIdx;
+				}
+				else if ((CurrRateIdx != UpRateIdx) && (pEntry->TxQuality[UpRateIdx] <= 0))
+				{
+					pEntry->CurrTxRateIndex = UpRateIdx;
+				}
+			}
+		} while (FALSE);
+
+		// if rate-up happen, clear all bad history of all TX rates
+		if (pEntry->CurrTxRateIndex > CurrRateIdx)
+		{
+			pEntry->CurrTxRateStableTime = 0;
+			pEntry->TxRateUpPenalty = 0;
+			pEntry->LastSecTxRateChangeAction = 1; // rate UP
+			NdisZeroMemory(pEntry->TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pEntry->PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+
+			//
+			// For TxRate fast train up
+			//
+			if (!pAd->StaCfg.StaQuickResponeForRateUpTimerRunning)
+			{
+				RTMPSetTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, 100);
+
+				pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = TRUE;
+			}
+			bTxRateChanged = TRUE;
+		}
+		// if rate-down happen, only clear DownRate's bad history
+		else if (pEntry->CurrTxRateIndex < CurrRateIdx)
+		{
+			pEntry->CurrTxRateStableTime = 0;
+			pEntry->TxRateUpPenalty = 0;           // no penalty
+			pEntry->LastSecTxRateChangeAction = 2; // rate DOWN
+			pEntry->TxQuality[pEntry->CurrTxRateIndex] = 0;
+			pEntry->PER[pEntry->CurrTxRateIndex] = 0;
+
+			//
+			// For TxRate fast train down
+			//
+			if (!pAd->StaCfg.StaQuickResponeForRateUpTimerRunning)
+			{
+				RTMPSetTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, 100);
+
+				pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = TRUE;
+			}
+			bTxRateChanged = TRUE;
+		}
+		else
+		{
+			pEntry->LastSecTxRateChangeAction = 0; // rate no change
+			bTxRateChanged = FALSE;
+		}
+
+		pEntry->LastTxOkCount = TxSuccess;
+
+		// reset all OneSecTx counters
+		RESET_ONE_SEC_TX_CNT(pEntry);
+
+		pNextTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(pEntry->CurrTxRateIndex+1)*5];
+		if (bTxRateChanged && pNextTxRate)
+		{
+			MlmeSetTxRate(pAd, pEntry, pNextTxRate);
+		}
+	}
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Station side, Auto TxRate faster train up timer call back function.
+
+	Arguments:
+		SystemSpecific1			- Not used.
+		FunctionContext			- Pointer to our Adapter context.
+		SystemSpecific2			- Not used.
+		SystemSpecific3			- Not used.
+
+	Return Value:
+		None
+
+	========================================================================
+*/
+VOID StaQuickResponeForRateUpExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	PRTMP_ADAPTER			pAd = (PRTMP_ADAPTER)FunctionContext;
+	UCHAR					UpRateIdx = 0, DownRateIdx = 0, CurrRateIdx = 0;
+	ULONG					TxTotalCnt;
+	ULONG					TxErrorRatio = 0;
+	BOOLEAN					bTxRateChanged = TRUE; //, bUpgradeQuality = FALSE;
+	PRTMP_TX_RATE_SWITCH	pCurrTxRate, pNextTxRate = NULL;
+	PUCHAR					pTable;
+	UCHAR					TableSize = 0;
+	UCHAR					InitTxRateIdx = 0, TrainUp, TrainDown;
+	TX_STA_CNT1_STRUC		StaTx1;
+	TX_STA_CNT0_STRUC		TxStaCnt0;
+	CHAR					Rssi, ratio;
+	ULONG					TxRetransmit = 0, TxSuccess = 0, TxFailCount = 0;
+	MAC_TABLE_ENTRY			*pEntry;
+	ULONG					i;
+
+	pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = FALSE;
+
+    //
+    // walk through MAC table, see if need to change AP's TX rate toward each entry
+    //
+	for (i = 1; i < MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		pEntry = &pAd->MacTab.Content[i];
+
+		// check if this entry need to switch rate automatically
+		if (RTMPCheckEntryEnableAutoRateSwitch(pAd, pEntry) == FALSE)
+			continue;
+
+		//Rssi = RTMPMaxRssi(pAd, (CHAR)pAd->StaCfg.AvgRssi0, (CHAR)pAd->StaCfg.AvgRssi1, (CHAR)pAd->StaCfg.AvgRssi2);
+	    if (pAd->Antenna.field.TxPath > 1)
+			Rssi = (pAd->StaCfg.RssiSample.AvgRssi0 + pAd->StaCfg.RssiSample.AvgRssi1) >> 1;
+		else
+			Rssi = pAd->StaCfg.RssiSample.AvgRssi0;
+
+		CurrRateIdx = pAd->CommonCfg.TxRateIndex;
+
+			MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &InitTxRateIdx);
+
+		// decide the next upgrade rate and downgrade rate, if any
+		if ((CurrRateIdx > 0) && (CurrRateIdx < (TableSize - 1)))
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx -1;
+		}
+		else if (CurrRateIdx == 0)
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx;
+		}
+		else if (CurrRateIdx == (TableSize - 1))
+		{
+			UpRateIdx = CurrRateIdx;
+			DownRateIdx = CurrRateIdx - 1;
+		}
+
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(CurrRateIdx+1)*5];
+
+#ifdef DOT11_N_SUPPORT
+		if ((Rssi > -65) && (pCurrTxRate->Mode >= MODE_HTMIX))
+		{
+			TrainUp		= (pCurrTxRate->TrainUp + (pCurrTxRate->TrainUp >> 1));
+			TrainDown	= (pCurrTxRate->TrainDown + (pCurrTxRate->TrainDown >> 1));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			TrainUp		= pCurrTxRate->TrainUp;
+			TrainDown	= pCurrTxRate->TrainDown;
+		}
+
+		if (pAd->MacTab.Size == 1)
+		{
+			// Update statistic counter
+			RTMP_IO_READ32(pAd, TX_STA_CNT0, &TxStaCnt0.word);
+			RTMP_IO_READ32(pAd, TX_STA_CNT1, &StaTx1.word);
+
+			TxRetransmit = StaTx1.field.TxRetransmit;
+			TxSuccess = StaTx1.field.TxSuccess;
+			TxFailCount = TxStaCnt0.field.TxFailCount;
+			TxTotalCnt = TxRetransmit + TxSuccess + TxFailCount;
+
+			pAd->RalinkCounters.OneSecTxRetryOkCount += StaTx1.field.TxRetransmit;
+			pAd->RalinkCounters.OneSecTxNoRetryOkCount += StaTx1.field.TxSuccess;
+			pAd->RalinkCounters.OneSecTxFailCount += TxStaCnt0.field.TxFailCount;
+			pAd->WlanCounters.TransmittedFragmentCount.u.LowPart += StaTx1.field.TxSuccess;
+			pAd->WlanCounters.RetryCount.u.LowPart += StaTx1.field.TxRetransmit;
+			pAd->WlanCounters.FailedCount.u.LowPart += TxStaCnt0.field.TxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((TxRetransmit + TxFailCount) * 100) / TxTotalCnt;
+		}
+		else
+		{
+			TxTotalCnt = pEntry->OneSecTxNoRetryOkCount +
+				 pEntry->OneSecTxRetryOkCount +
+				 pEntry->OneSecTxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((pEntry->OneSecTxRetryOkCount + pEntry->OneSecTxFailCount) * 100) / TxTotalCnt;
+		}
+
+
+		//
+		// CASE 1. when TX samples are fewer than 15, then decide TX rate solely on RSSI
+		//         (criteria copied from RT2500 for Netopia case)
+		//
+		if (TxTotalCnt <= 12)
+		{
+			NdisZeroMemory(pAd->DrsCounters.TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pAd->DrsCounters.PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+
+			if ((pAd->DrsCounters.LastSecTxRateChangeAction == 1) && (CurrRateIdx != DownRateIdx))
+			{
+				pAd->CommonCfg.TxRateIndex = DownRateIdx;
+				pAd->DrsCounters.TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+			}
+			else if ((pAd->DrsCounters.LastSecTxRateChangeAction == 2) && (CurrRateIdx != UpRateIdx))
+			{
+				pAd->CommonCfg.TxRateIndex = UpRateIdx;
+			}
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("QuickDRS: TxTotalCnt <= 15, train back to original rate \n"));
+			return;
+		}
+
+		do
+		{
+			ULONG OneSecTxNoRetryOKRationCount;
+
+			if (pAd->DrsCounters.LastTimeTxRateChangeAction == 0)
+				ratio = 5;
+			else
+				ratio = 4;
+
+			// downgrade TX quality if PER >= Rate-Down threshold
+			if (TxErrorRatio >= TrainDown)
+			{
+				pAd->DrsCounters.TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+			}
+
+			pAd->DrsCounters.PER[CurrRateIdx] = (UCHAR)TxErrorRatio;
+
+			OneSecTxNoRetryOKRationCount = (TxSuccess * ratio);
+
+			// perform DRS - consider TxRate Down first, then rate up.
+			if ((pAd->DrsCounters.LastSecTxRateChangeAction == 1) && (CurrRateIdx != DownRateIdx))
+			{
+				if ((pAd->DrsCounters.LastTxOkCount + 2) >= OneSecTxNoRetryOKRationCount)
+				{
+					pAd->CommonCfg.TxRateIndex = DownRateIdx;
+					pAd->DrsCounters.TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+
+				}
+
+			}
+			else if ((pAd->DrsCounters.LastSecTxRateChangeAction == 2) && (CurrRateIdx != UpRateIdx))
+			{
+				if ((TxErrorRatio >= 50) || (TxErrorRatio >= TrainDown))
+				{
+
+				}
+				else if ((pAd->DrsCounters.LastTxOkCount + 2) >= OneSecTxNoRetryOKRationCount)
+				{
+					pAd->CommonCfg.TxRateIndex = UpRateIdx;
+				}
+			}
+		}while (FALSE);
+
+		// if rate-up happen, clear all bad history of all TX rates
+		if (pAd->CommonCfg.TxRateIndex > CurrRateIdx)
+		{
+			pAd->DrsCounters.TxRateUpPenalty = 0;
+			NdisZeroMemory(pAd->DrsCounters.TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pAd->DrsCounters.PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+		}
+		// if rate-down happen, only clear DownRate's bad history
+		else if (pAd->CommonCfg.TxRateIndex < CurrRateIdx)
+		{
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("QuickDRS: --TX rate from %d to %d \n", CurrRateIdx, pAd->CommonCfg.TxRateIndex));
+
+			pAd->DrsCounters.TxRateUpPenalty = 0;           // no penalty
+			pAd->DrsCounters.TxQuality[pAd->CommonCfg.TxRateIndex] = 0;
+			pAd->DrsCounters.PER[pAd->CommonCfg.TxRateIndex] = 0;
+		}
+		else
+		{
+			bTxRateChanged = FALSE;
+		}
+
+		pNextTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(pAd->CommonCfg.TxRateIndex+1)*5];
+		if (bTxRateChanged && pNextTxRate)
+		{
+			MlmeSetTxRate(pAd, pEntry, pNextTxRate);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine is executed periodically inside MlmePeriodicExec() after
+		association with an AP.
+		It checks if StaCfg.Psm is consistent with user policy (recorded in
+		StaCfg.WindowsPowerMode). If not, enforce user policy. However,
+		there're some conditions to consider:
+		1. we don't support power-saving in ADHOC mode, so Psm=PWR_ACTIVE all
+		   the time when Mibss==TRUE
+		2. When link up in INFRA mode, Psm should not be switch to PWR_SAVE
+		   if outgoing traffic available in TxRing or MgmtRing.
+	Output:
+		1. change pAd->StaCfg.Psm to PWR_SAVE or leave it untouched
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeCheckPsmChange(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG	Now32)
+{
+	ULONG	PowerMode;
+
+	// condition -
+	// 1. Psm maybe ON only happen in INFRASTRUCTURE mode
+	// 2. user wants either MAX_PSP or FAST_PSP
+	// 3. but current psm is not in PWR_SAVE
+	// 4. CNTL state machine is not doing SCANning
+	// 5. no TX SUCCESS event for the past 1-sec period
+#ifdef NDIS51_MINIPORT
+	if (pAd->StaCfg.WindowsPowerProfile == NdisPowerProfileBattery)
+		PowerMode = pAd->StaCfg.WindowsBatteryPowerMode;
+	else
+#endif
+		PowerMode = pAd->StaCfg.WindowsPowerMode;
+
+	if (INFRA_ON(pAd) &&
+		(PowerMode != Ndis802_11PowerModeCAM) &&
+		(pAd->StaCfg.Psm == PWR_ACTIVE) &&
+		(pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE))
+	{
+		NdisGetSystemUpTime(&pAd->Mlme.LastSendNULLpsmTime);
+		pAd->RalinkCounters.RxCountSinceLastNULL = 0;
+		MlmeSetPsmBit(pAd, PWR_SAVE);
+		if (!(pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable))
+		{
+			RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, FALSE);
+		}
+		else
+		{
+			RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+		}
+	}
+}
+
+// IRQL = PASSIVE_LEVEL
+// IRQL = DISPATCH_LEVEL
+VOID MlmeSetPsmBit(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT psm)
+{
+	AUTO_RSP_CFG_STRUC csr4;
+
+	pAd->StaCfg.Psm = psm;
+	RTMP_IO_READ32(pAd, AUTO_RSP_CFG, &csr4.word);
+	csr4.field.AckCtsPsmBit = (psm == PWR_SAVE)? 1:0;
+	RTMP_IO_WRITE32(pAd, AUTO_RSP_CFG, csr4.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("MlmeSetPsmBit = %d\n", psm));
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeSetTxPreamble(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TxPreamble)
+{
+	AUTO_RSP_CFG_STRUC csr4;
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	//TxPreamble = Rt802_11PreambleLong;
+
+	RTMP_IO_READ32(pAd, AUTO_RSP_CFG, &csr4.word);
+	if (TxPreamble == Rt802_11PreambleLong)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeSetTxPreamble (= LONG PREAMBLE)\n"));
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+		csr4.field.AutoResponderPreamble = 0;
+	}
+	else
+	{
+		// NOTE: 1Mbps should always use long preamble
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeSetTxPreamble (= SHORT PREAMBLE)\n"));
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+		csr4.field.AutoResponderPreamble = 1;
+	}
+
+	RTMP_IO_WRITE32(pAd, AUTO_RSP_CFG, csr4.word);
+}
+
+/*
+    ==========================================================================
+    Description:
+        Update basic rate bitmap
+    ==========================================================================
+ */
+
+VOID UpdateBasicRateBitmap(
+    IN  PRTMP_ADAPTER   pAdapter)
+{
+    INT  i, j;
+                  /* 1  2  5.5, 11,  6,  9, 12, 18, 24, 36, 48,  54 */
+    UCHAR rate[] = { 2, 4,  11, 22, 12, 18, 24, 36, 48, 72, 96, 108 };
+    UCHAR *sup_p = pAdapter->CommonCfg.SupRate;
+    UCHAR *ext_p = pAdapter->CommonCfg.ExtRate;
+    ULONG bitmap = pAdapter->CommonCfg.BasicRateBitmap;
+
+
+    /* if A mode, always use fix BasicRateBitMap */
+    //if (pAdapter->CommonCfg.Channel == PHY_11A)
+	if (pAdapter->CommonCfg.Channel > 14)
+        pAdapter->CommonCfg.BasicRateBitmap = 0x150; /* 6, 12, 24M */
+    /* End of if */
+
+    if (pAdapter->CommonCfg.BasicRateBitmap > 4095)
+    {
+        /* (2 ^ MAX_LEN_OF_SUPPORTED_RATES) -1 */
+        return;
+    } /* End of if */
+
+    for(i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+    {
+        sup_p[i] &= 0x7f;
+        ext_p[i] &= 0x7f;
+    } /* End of for */
+
+    for(i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+    {
+        if (bitmap & (1 << i))
+        {
+            for(j=0; j<MAX_LEN_OF_SUPPORTED_RATES; j++)
+            {
+                if (sup_p[j] == rate[i])
+                    sup_p[j] |= 0x80;
+                /* End of if */
+            } /* End of for */
+
+            for(j=0; j<MAX_LEN_OF_SUPPORTED_RATES; j++)
+            {
+                if (ext_p[j] == rate[i])
+                    ext_p[j] |= 0x80;
+                /* End of if */
+            } /* End of for */
+        } /* End of if */
+    } /* End of for */
+} /* End of UpdateBasicRateBitmap */
+
+// IRQL = PASSIVE_LEVEL
+// IRQL = DISPATCH_LEVEL
+// bLinkUp is to identify the inital link speed.
+// TRUE indicates the rate update at linkup, we should not try to set the rate at 54Mbps.
+VOID MlmeUpdateTxRates(
+	IN PRTMP_ADAPTER 		pAd,
+	IN 	BOOLEAN		 		bLinkUp,
+	IN	UCHAR				apidx)
+{
+	int i, num;
+	UCHAR Rate = RATE_6, MaxDesire = RATE_1, MaxSupport = RATE_1;
+	UCHAR MinSupport = RATE_54;
+	ULONG BasicRateBitmap = 0;
+	UCHAR CurrBasicRate = RATE_1;
+	UCHAR *pSupRate, SupRateLen, *pExtRate, ExtRateLen;
+	PHTTRANSMIT_SETTING		pHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMaxHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMinHtPhy = NULL;
+	BOOLEAN 				*auto_rate_cur_p;
+	UCHAR					HtMcs = MCS_AUTO;
+
+	// find max desired rate
+	UpdateBasicRateBitmap(pAd);
+
+	num = 0;
+	auto_rate_cur_p = NULL;
+	for (i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+	{
+		switch (pAd->CommonCfg.DesireRate[i] & 0x7f)
+		{
+			case 2:  Rate = RATE_1;   num++;   break;
+			case 4:  Rate = RATE_2;   num++;   break;
+			case 11: Rate = RATE_5_5; num++;   break;
+			case 22: Rate = RATE_11;  num++;   break;
+			case 12: Rate = RATE_6;   num++;   break;
+			case 18: Rate = RATE_9;   num++;   break;
+			case 24: Rate = RATE_12;  num++;   break;
+			case 36: Rate = RATE_18;  num++;   break;
+			case 48: Rate = RATE_24;  num++;   break;
+			case 72: Rate = RATE_36;  num++;   break;
+			case 96: Rate = RATE_48;  num++;   break;
+			case 108: Rate = RATE_54; num++;   break;
+			//default: Rate = RATE_1;   break;
+		}
+		if (MaxDesire < Rate)  MaxDesire = Rate;
+	}
+
+//===========================================================================
+//===========================================================================
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pHtPhy 		= &pAd->StaCfg.HTPhyMode;
+		pMaxHtPhy	= &pAd->StaCfg.MaxHTPhyMode;
+		pMinHtPhy	= &pAd->StaCfg.MinHTPhyMode;
+
+		auto_rate_cur_p = &pAd->StaCfg.bAutoTxRateSwitch;
+		HtMcs 		= pAd->StaCfg.DesiredTransmitSetting.field.MCS;
+
+		if ((pAd->StaCfg.BssType == BSS_ADHOC) &&
+			(pAd->CommonCfg.PhyMode == PHY_11B) &&
+			(MaxDesire > RATE_11))
+		{
+			MaxDesire = RATE_11;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	pAd->CommonCfg.MaxDesiredRate = MaxDesire;
+	pMinHtPhy->word = 0;
+	pMaxHtPhy->word = 0;
+	pHtPhy->word = 0;
+
+	// Auto rate switching is enabled only if more than one DESIRED RATES are
+	// specified; otherwise disabled
+	if (num <= 1)
+	{
+		*auto_rate_cur_p = FALSE;
+	}
+	else
+	{
+		*auto_rate_cur_p = TRUE;
+	}
+
+#if 1
+	if (HtMcs != MCS_AUTO)
+	{
+		*auto_rate_cur_p = FALSE;
+	}
+	else
+	{
+		*auto_rate_cur_p = TRUE;
+	}
+#endif
+
+#ifdef CONFIG_STA_SUPPORT
+	if ((ADHOC_ON(pAd) || INFRA_ON(pAd)) && (pAd->OpMode == OPMODE_STA))
+	{
+		pSupRate = &pAd->StaActive.SupRate[0];
+		pExtRate = &pAd->StaActive.ExtRate[0];
+		SupRateLen = pAd->StaActive.SupRateLen;
+		ExtRateLen = pAd->StaActive.ExtRateLen;
+	}
+	else
+#endif // CONFIG_STA_SUPPORT //
+	{
+		pSupRate = &pAd->CommonCfg.SupRate[0];
+		pExtRate = &pAd->CommonCfg.ExtRate[0];
+		SupRateLen = pAd->CommonCfg.SupRateLen;
+		ExtRateLen = pAd->CommonCfg.ExtRateLen;
+	}
+
+	// find max supported rate
+	for (i=0; i<SupRateLen; i++)
+	{
+		switch (pSupRate[i] & 0x7f)
+		{
+			case 2:   Rate = RATE_1;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0001;	 break;
+			case 4:   Rate = RATE_2;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0002;	 break;
+			case 11:  Rate = RATE_5_5;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0004;	 break;
+			case 22:  Rate = RATE_11;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0008;	 break;
+			case 12:  Rate = RATE_6;	/*if (pSupRate[i] & 0x80)*/  BasicRateBitmap |= 0x0010;  break;
+			case 18:  Rate = RATE_9;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0020;	 break;
+			case 24:  Rate = RATE_12;	/*if (pSupRate[i] & 0x80)*/  BasicRateBitmap |= 0x0040;  break;
+			case 36:  Rate = RATE_18;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0080;	 break;
+			case 48:  Rate = RATE_24;	/*if (pSupRate[i] & 0x80)*/  BasicRateBitmap |= 0x0100;  break;
+			case 72:  Rate = RATE_36;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0200;	 break;
+			case 96:  Rate = RATE_48;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0400;	 break;
+			case 108: Rate = RATE_54;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0800;	 break;
+			default:  Rate = RATE_1;	break;
+		}
+		if (MaxSupport < Rate)	MaxSupport = Rate;
+
+		if (MinSupport > Rate) MinSupport = Rate;
+	}
+
+	for (i=0; i<ExtRateLen; i++)
+	{
+		switch (pExtRate[i] & 0x7f)
+		{
+			case 2:   Rate = RATE_1;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0001;	 break;
+			case 4:   Rate = RATE_2;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0002;	 break;
+			case 11:  Rate = RATE_5_5;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0004;	 break;
+			case 22:  Rate = RATE_11;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0008;	 break;
+			case 12:  Rate = RATE_6;	/*if (pExtRate[i] & 0x80)*/  BasicRateBitmap |= 0x0010;  break;
+			case 18:  Rate = RATE_9;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0020;	 break;
+			case 24:  Rate = RATE_12;	/*if (pExtRate[i] & 0x80)*/  BasicRateBitmap |= 0x0040;  break;
+			case 36:  Rate = RATE_18;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0080;	 break;
+			case 48:  Rate = RATE_24;	/*if (pExtRate[i] & 0x80)*/  BasicRateBitmap |= 0x0100;  break;
+			case 72:  Rate = RATE_36;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0200;	 break;
+			case 96:  Rate = RATE_48;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0400;	 break;
+			case 108: Rate = RATE_54;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0800;	 break;
+			default:  Rate = RATE_1;	break;
+		}
+		if (MaxSupport < Rate)	MaxSupport = Rate;
+
+		if (MinSupport > Rate) MinSupport = Rate;
+	}
+
+	RTMP_IO_WRITE32(pAd, LEGACY_BASIC_RATE, BasicRateBitmap);
+
+	// calculate the exptected ACK rate for each TX rate. This info is used to caculate
+	// the DURATION field of outgoing uniicast DATA/MGMT frame
+	for (i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+	{
+		if (BasicRateBitmap & (0x01 << i))
+			CurrBasicRate = (UCHAR)i;
+		pAd->CommonCfg.ExpectedACKRate[i] = CurrBasicRate;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateTxRates[MaxSupport = %d] = MaxDesire %d Mbps\n", RateIdToMbps[MaxSupport], RateIdToMbps[MaxDesire]));
+	// max tx rate = min {max desire rate, max supported rate}
+	if (MaxSupport < MaxDesire)
+		pAd->CommonCfg.MaxTxRate = MaxSupport;
+	else
+		pAd->CommonCfg.MaxTxRate = MaxDesire;
+
+	pAd->CommonCfg.MinTxRate = MinSupport;
+	if (*auto_rate_cur_p)
+	{
+		short dbm = 0;
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			dbm = pAd->StaCfg.RssiSample.AvgRssi0 - pAd->BbpRssiToDbmDelta;
+#endif // CONFIG_STA_SUPPORT //
+		if (bLinkUp == TRUE)
+			pAd->CommonCfg.TxRate = RATE_24;
+		else
+			pAd->CommonCfg.TxRate = pAd->CommonCfg.MaxTxRate;
+
+		if (dbm < -75)
+			pAd->CommonCfg.TxRate = RATE_11;
+		else if (dbm < -70)
+			pAd->CommonCfg.TxRate = RATE_24;
+
+		// should never exceed MaxTxRate (consider 11B-only mode)
+		if (pAd->CommonCfg.TxRate > pAd->CommonCfg.MaxTxRate)
+			pAd->CommonCfg.TxRate = pAd->CommonCfg.MaxTxRate;
+
+		pAd->CommonCfg.TxRateIndex = 0;
+	}
+	else
+	{
+		pAd->CommonCfg.TxRate = pAd->CommonCfg.MaxTxRate;
+		pHtPhy->field.MCS	= (pAd->CommonCfg.MaxTxRate > 3) ? (pAd->CommonCfg.MaxTxRate - 4) : pAd->CommonCfg.MaxTxRate;
+		pHtPhy->field.MODE	= (pAd->CommonCfg.MaxTxRate > 3) ? MODE_OFDM : MODE_CCK;
+
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.STBC	= pHtPhy->field.STBC;
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.ShortGI	= pHtPhy->field.ShortGI;
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MCS		= pHtPhy->field.MCS;
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE	= pHtPhy->field.MODE;
+	}
+
+	if (pAd->CommonCfg.TxRate <= RATE_11)
+	{
+		pMaxHtPhy->field.MODE = MODE_CCK;
+		pMaxHtPhy->field.MCS = pAd->CommonCfg.TxRate;
+		pMinHtPhy->field.MCS = pAd->CommonCfg.MinTxRate;
+	}
+	else
+	{
+		pMaxHtPhy->field.MODE = MODE_OFDM;
+		pMaxHtPhy->field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.TxRate];
+		if (pAd->CommonCfg.MinTxRate >= RATE_6 && (pAd->CommonCfg.MinTxRate <= RATE_54))
+			{pMinHtPhy->field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MinTxRate];}
+		else
+			{pMinHtPhy->field.MCS = pAd->CommonCfg.MinTxRate;}
+	}
+
+	pHtPhy->word = (pMaxHtPhy->word);
+	if (bLinkUp && (pAd->OpMode == OPMODE_STA))
+	{
+			pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word = pHtPhy->word;
+			pAd->MacTab.Content[BSSID_WCID].MaxHTPhyMode.word = pMaxHtPhy->word;
+			pAd->MacTab.Content[BSSID_WCID].MinHTPhyMode.word = pMinHtPhy->word;
+	}
+	else
+	{
+		switch (pAd->CommonCfg.PhyMode)
+		{
+			case PHY_11BG_MIXED:
+			case PHY_11B:
+#ifdef DOT11_N_SUPPORT
+			case PHY_11BGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+				pAd->CommonCfg.MlmeRate = RATE_1;
+				pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+				pAd->CommonCfg.MlmeTransmit.field.MCS = RATE_1;
+				pAd->CommonCfg.RtsRate = RATE_11;
+				break;
+			case PHY_11G:
+			case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+			case PHY_11AGN_MIXED:
+			case PHY_11GN_MIXED:
+			case PHY_11N_2_4G:
+			case PHY_11AN_MIXED:
+			case PHY_11N_5G:
+#endif // DOT11_N_SUPPORT //
+				pAd->CommonCfg.MlmeRate = RATE_6;
+				pAd->CommonCfg.RtsRate = RATE_6;
+				pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+				pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+				break;
+			case PHY_11ABG_MIXED:
+#ifdef DOT11_N_SUPPORT
+			case PHY_11ABGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+				if (pAd->CommonCfg.Channel <= 14)
+				{
+					pAd->CommonCfg.MlmeRate = RATE_1;
+					pAd->CommonCfg.RtsRate = RATE_1;
+					pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+					pAd->CommonCfg.MlmeTransmit.field.MCS = RATE_1;
+				}
+				else
+				{
+					pAd->CommonCfg.MlmeRate = RATE_6;
+					pAd->CommonCfg.RtsRate = RATE_6;
+					pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+					pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+				}
+				break;
+			default: // error
+				pAd->CommonCfg.MlmeRate = RATE_6;
+                        	pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+				pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+				pAd->CommonCfg.RtsRate = RATE_1;
+				break;
+		}
+		//
+		// Keep Basic Mlme Rate.
+		//
+		pAd->MacTab.Content[MCAST_WCID].HTPhyMode.word = pAd->CommonCfg.MlmeTransmit.word;
+		if (pAd->CommonCfg.MlmeTransmit.field.MODE == MODE_OFDM)
+			pAd->MacTab.Content[MCAST_WCID].HTPhyMode.field.MCS = OfdmRateToRxwiMCS[RATE_24];
+		else
+			pAd->MacTab.Content[MCAST_WCID].HTPhyMode.field.MCS = RATE_1;
+		pAd->CommonCfg.BasicMlmeRate = pAd->CommonCfg.MlmeRate;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (MaxDesire=%d, MaxSupport=%d, MaxTxRate=%d, MinRate=%d, Rate Switching =%d)\n",
+			 RateIdToMbps[MaxDesire], RateIdToMbps[MaxSupport], RateIdToMbps[pAd->CommonCfg.MaxTxRate], RateIdToMbps[pAd->CommonCfg.MinTxRate],
+			 /*OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED)*/*auto_rate_cur_p));
+	DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (TxRate=%d, RtsRate=%d, BasicRateBitmap=0x%04lx)\n",
+			 RateIdToMbps[pAd->CommonCfg.TxRate], RateIdToMbps[pAd->CommonCfg.RtsRate], BasicRateBitmap));
+	DBGPRINT(RT_DEBUG_TRACE, ("MlmeUpdateTxRates (MlmeTransmit=0x%x, MinHTPhyMode=%x, MaxHTPhyMode=0x%x, HTPhyMode=0x%x)\n",
+			 pAd->CommonCfg.MlmeTransmit.word, pAd->MacTab.Content[BSSID_WCID].MinHTPhyMode.word ,pAd->MacTab.Content[BSSID_WCID].MaxHTPhyMode.word ,pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word ));
+}
+
+#ifdef DOT11_N_SUPPORT
+/*
+	==========================================================================
+	Description:
+		This function update HT Rate setting.
+		Input Wcid value is valid for 2 case :
+		1. it's used for Station in infra mode that copy AP rate to Mactable.
+		2. OR Station 	in adhoc mode to copy peer's HT rate to Mactable.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeUpdateHtTxRates(
+	IN PRTMP_ADAPTER 		pAd,
+	IN	UCHAR				apidx)
+{
+	UCHAR	StbcMcs; //j, StbcMcs, bitmask;
+	CHAR 	i; // 3*3
+	RT_HT_CAPABILITY 	*pRtHtCap = NULL;
+	RT_HT_PHY_INFO		*pActiveHtPhy = NULL;
+	ULONG		BasicMCS;
+	UCHAR j, bitmask;
+	PRT_HT_PHY_INFO			pDesireHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMaxHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMinHtPhy = NULL;
+	BOOLEAN 				*auto_rate_cur_p;
+
+	DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateHtTxRates===> \n"));
+
+	auto_rate_cur_p = NULL;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pDesireHtPhy	= &pAd->StaCfg.DesiredHtPhyInfo;
+		pActiveHtPhy	= &pAd->StaCfg.DesiredHtPhyInfo;
+		pHtPhy 		= &pAd->StaCfg.HTPhyMode;
+		pMaxHtPhy	= &pAd->StaCfg.MaxHTPhyMode;
+		pMinHtPhy	= &pAd->StaCfg.MinHTPhyMode;
+
+		auto_rate_cur_p = &pAd->StaCfg.bAutoTxRateSwitch;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+	if ((ADHOC_ON(pAd) || INFRA_ON(pAd)) && (pAd->OpMode == OPMODE_STA))
+	{
+		if (pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE)
+			return;
+
+		pRtHtCap = &pAd->StaActive.SupportedHtPhy;
+		pActiveHtPhy = &pAd->StaActive.SupportedPhyInfo;
+		StbcMcs = (UCHAR)pAd->MlmeAux.AddHtInfo.AddHtInfo3.StbcMcs;
+		BasicMCS =pAd->MlmeAux.AddHtInfo.MCSSet[0]+(pAd->MlmeAux.AddHtInfo.MCSSet[1]<<8)+(StbcMcs<<16);
+		if ((pAd->CommonCfg.DesiredHtPhy.TxSTBC) && (pRtHtCap->RxSTBC) && (pAd->Antenna.field.TxPath == 2))
+			pMaxHtPhy->field.STBC = STBC_USE;
+		else
+			pMaxHtPhy->field.STBC = STBC_NONE;
+	}
+	else
+#endif // CONFIG_STA_SUPPORT //
+	{
+		if (pDesireHtPhy->bHtEnable == FALSE)
+			return;
+
+		pRtHtCap = &pAd->CommonCfg.DesiredHtPhy;
+		StbcMcs = (UCHAR)pAd->CommonCfg.AddHTInfo.AddHtInfo3.StbcMcs;
+		BasicMCS = pAd->CommonCfg.AddHTInfo.MCSSet[0]+(pAd->CommonCfg.AddHTInfo.MCSSet[1]<<8)+(StbcMcs<<16);
+		if ((pAd->CommonCfg.DesiredHtPhy.TxSTBC) && (pRtHtCap->RxSTBC) && (pAd->Antenna.field.TxPath == 2))
+			pMaxHtPhy->field.STBC = STBC_USE;
+		else
+			pMaxHtPhy->field.STBC = STBC_NONE;
+	}
+
+	// Decide MAX ht rate.
+	if ((pRtHtCap->GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+		pMaxHtPhy->field.MODE = MODE_HTGREENFIELD;
+	else
+		pMaxHtPhy->field.MODE = MODE_HTMIX;
+
+    if ((pAd->CommonCfg.DesiredHtPhy.ChannelWidth) && (pRtHtCap->ChannelWidth))
+		pMaxHtPhy->field.BW = BW_40;
+	else
+		pMaxHtPhy->field.BW = BW_20;
+
+    if (pMaxHtPhy->field.BW == BW_20)
+		pMaxHtPhy->field.ShortGI = (pAd->CommonCfg.DesiredHtPhy.ShortGIfor20 & pRtHtCap->ShortGIfor20);
+	else
+		pMaxHtPhy->field.ShortGI = (pAd->CommonCfg.DesiredHtPhy.ShortGIfor40 & pRtHtCap->ShortGIfor40);
+
+	for (i=23; i>=0; i--) // 3*3
+	{
+		j = i/8;
+		bitmask = (1<<(i-(j*8)));
+
+		if ((pActiveHtPhy->MCSSet[j] & bitmask) && (pDesireHtPhy->MCSSet[j] & bitmask))
+		{
+			pMaxHtPhy->field.MCS = i;
+			break;
+		}
+
+		if (i==0)
+			break;
+	}
+
+	// Copy MIN ht rate.  rt2860???
+	pMinHtPhy->field.BW = BW_20;
+	pMinHtPhy->field.MCS = 0;
+	pMinHtPhy->field.STBC = 0;
+	pMinHtPhy->field.ShortGI = 0;
+	//If STA assigns fixed rate. update to fixed here.
+#ifdef CONFIG_STA_SUPPORT
+	if ( (pAd->OpMode == OPMODE_STA) && (pDesireHtPhy->MCSSet[0] != 0xff))
+	{
+		if (pDesireHtPhy->MCSSet[4] != 0)
+		{
+			pMaxHtPhy->field.MCS = 32;
+			pMinHtPhy->field.MCS = 32;
+			DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateHtTxRates<=== Use Fixed MCS = %d\n",pMinHtPhy->field.MCS));
+		}
+
+		for (i=23; (CHAR)i >= 0; i--) // 3*3
+		{
+			j = i/8;
+			bitmask = (1<<(i-(j*8)));
+			if ( (pDesireHtPhy->MCSSet[j] & bitmask) && (pActiveHtPhy->MCSSet[j] & bitmask))
+			{
+				pMaxHtPhy->field.MCS = i;
+				pMinHtPhy->field.MCS = i;
+				break;
+			}
+			if (i==0)
+				break;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+	// Decide ht rate
+	pHtPhy->field.STBC = pMaxHtPhy->field.STBC;
+	pHtPhy->field.BW = pMaxHtPhy->field.BW;
+	pHtPhy->field.MODE = pMaxHtPhy->field.MODE;
+	pHtPhy->field.MCS = pMaxHtPhy->field.MCS;
+	pHtPhy->field.ShortGI = pMaxHtPhy->field.ShortGI;
+
+	// use default now. rt2860
+	if (pDesireHtPhy->MCSSet[0] != 0xff)
+		*auto_rate_cur_p = FALSE;
+	else
+		*auto_rate_cur_p = TRUE;
+
+	DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateHtTxRates<---.AMsduSize = %d  \n", pAd->CommonCfg.DesiredHtPhy.AmsduSize ));
+	DBGPRINT(RT_DEBUG_TRACE,("TX: MCS[0] = %x (choose %d), BW = %d, ShortGI = %d, MODE = %d,  \n", pActiveHtPhy->MCSSet[0],pHtPhy->field.MCS,
+		pHtPhy->field.BW, pHtPhy->field.ShortGI, pHtPhy->field.MODE));
+	DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateHtTxRates<=== \n"));
+}
+#endif // DOT11_N_SUPPORT //
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeRadioOff(
+	IN PRTMP_ADAPTER pAd)
+{
+	RT28XX_MLME_RADIO_OFF(pAd);
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeRadioOn(
+	IN PRTMP_ADAPTER pAd)
+{
+	RT28XX_MLME_RADIO_ON(pAd);
+}
+
+// ===========================================================================================
+// bss_table.c
+// ===========================================================================================
+
+
+/*! \brief initialize BSS table
+ *	\param p_tab pointer to the table
+ *	\return none
+ *	\pre
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID BssTableInit(
+	IN BSS_TABLE *Tab)
+{
+	int i;
+
+	Tab->BssNr = 0;
+    Tab->BssOverlapNr = 0;
+	for (i = 0; i < MAX_LEN_OF_BSS_TABLE; i++)
+	{
+		NdisZeroMemory(&Tab->BssEntry[i], sizeof(BSS_ENTRY));
+		Tab->BssEntry[i].Rssi = -127;	// initial the rssi as a minimum value
+	}
+}
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableInit(
+	IN PRTMP_ADAPTER pAd,
+    IN BA_TABLE *Tab)
+{
+	int i;
+
+	Tab->numAsOriginator = 0;
+	Tab->numAsRecipient = 0;
+	NdisAllocateSpinLock(&pAd->BATabLock);
+	for (i = 0; i < MAX_LEN_OF_BA_REC_TABLE; i++)
+	{
+		Tab->BARecEntry[i].REC_BA_Status = Recipient_NONE;
+		NdisAllocateSpinLock(&(Tab->BARecEntry[i].RxReRingLock));
+	}
+	for (i = 0; i < MAX_LEN_OF_BA_ORI_TABLE; i++)
+	{
+		Tab->BAOriEntry[i].ORI_BA_Status = Originator_NONE;
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+/*! \brief search the BSS table by SSID
+ *	\param p_tab pointer to the bss table
+ *	\param ssid SSID string
+ *	\return index of the table, BSS_NOT_FOUND if not in the table
+ *	\pre
+ *	\post
+ *	\note search by sequential search
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+ULONG BssTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR	 pBssid,
+	IN UCHAR	 Channel)
+{
+	UCHAR i;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		//
+		// Some AP that support A/B/G mode that may used the same BSSID on 11A and 11B/G.
+		// We should distinguish this case.
+		//
+		if ((((Tab->BssEntry[i].Channel <= 14) && (Channel <= 14)) ||
+			 ((Tab->BssEntry[i].Channel > 14) && (Channel > 14))) &&
+			MAC_ADDR_EQUAL(Tab->BssEntry[i].Bssid, pBssid))
+		{
+			return i;
+		}
+	}
+	return (ULONG)BSS_NOT_FOUND;
+}
+
+ULONG BssSsidTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR	 pBssid,
+	IN PUCHAR	 pSsid,
+	IN UCHAR	 SsidLen,
+	IN UCHAR	 Channel)
+{
+	UCHAR i;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		//
+		// Some AP that support A/B/G mode that may used the same BSSID on 11A and 11B/G.
+		// We should distinguish this case.
+		//
+		if ((((Tab->BssEntry[i].Channel <= 14) && (Channel <= 14)) ||
+			 ((Tab->BssEntry[i].Channel > 14) && (Channel > 14))) &&
+			MAC_ADDR_EQUAL(Tab->BssEntry[i].Bssid, pBssid) &&
+			SSID_EQUAL(pSsid, SsidLen, Tab->BssEntry[i].Ssid, Tab->BssEntry[i].SsidLen))
+		{
+			return i;
+		}
+	}
+	return (ULONG)BSS_NOT_FOUND;
+}
+
+ULONG BssTableSearchWithSSID(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR	 Bssid,
+	IN PUCHAR	 pSsid,
+	IN UCHAR	 SsidLen,
+	IN UCHAR	 Channel)
+{
+	UCHAR i;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		if ((((Tab->BssEntry[i].Channel <= 14) && (Channel <= 14)) ||
+			((Tab->BssEntry[i].Channel > 14) && (Channel > 14))) &&
+			MAC_ADDR_EQUAL(&(Tab->BssEntry[i].Bssid), Bssid) &&
+			(SSID_EQUAL(pSsid, SsidLen, Tab->BssEntry[i].Ssid, Tab->BssEntry[i].SsidLen) ||
+			(NdisEqualMemory(pSsid, ZeroSsid, SsidLen)) ||
+			(NdisEqualMemory(Tab->BssEntry[i].Ssid, ZeroSsid, Tab->BssEntry[i].SsidLen))))
+		{
+			return i;
+		}
+	}
+	return (ULONG)BSS_NOT_FOUND;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID BssTableDeleteEntry(
+	IN OUT	BSS_TABLE *Tab,
+	IN		PUCHAR	  pBssid,
+	IN		UCHAR	  Channel)
+{
+	UCHAR i, j;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		if ((Tab->BssEntry[i].Channel == Channel) &&
+			(MAC_ADDR_EQUAL(Tab->BssEntry[i].Bssid, pBssid)))
+		{
+			for (j = i; j < Tab->BssNr - 1; j++)
+			{
+				NdisMoveMemory(&(Tab->BssEntry[j]), &(Tab->BssEntry[j + 1]), sizeof(BSS_ENTRY));
+			}
+			NdisZeroMemory(&(Tab->BssEntry[Tab->BssNr - 1]), sizeof(BSS_ENTRY));
+			Tab->BssNr -= 1;
+			return;
+		}
+	}
+}
+
+#ifdef DOT11_N_SUPPORT
+/*
+	========================================================================
+	Routine Description:
+		Delete the Originator Entry in BAtable. Or decrease numAs Originator by 1 if needed.
+
+	Arguments:
+	// IRQL = DISPATCH_LEVEL
+	========================================================================
+*/
+VOID BATableDeleteORIEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		BA_ORI_ENTRY	*pBAORIEntry)
+{
+
+	if (pBAORIEntry->ORI_BA_Status != Originator_NONE)
+	{
+		NdisAcquireSpinLock(&pAd->BATabLock);
+		if (pBAORIEntry->ORI_BA_Status == Originator_Done)
+		{
+			pAd->BATable.numAsOriginator -= 1;
+			DBGPRINT(RT_DEBUG_TRACE, ("BATableDeleteORIEntry numAsOriginator= %ld\n", pAd->BATable.numAsRecipient));
+			// Erase Bitmap flag.
+		}
+		pAd->MacTab.Content[pBAORIEntry->Wcid].TXBAbitmap &= (~(1<<(pBAORIEntry->TID) ));	// If STA mode,  erase flag here
+		pAd->MacTab.Content[pBAORIEntry->Wcid].BAOriWcidArray[pBAORIEntry->TID] = 0;	// If STA mode,  erase flag here
+		pBAORIEntry->ORI_BA_Status = Originator_NONE;
+		pBAORIEntry->Token = 1;
+		// Not clear Sequence here.
+		NdisReleaseSpinLock(&pAd->BATabLock);
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+/*! \brief
+ *	\param
+ *	\return
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID BssEntrySet(
+	IN PRTMP_ADAPTER	pAd,
+	OUT BSS_ENTRY *pBss,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN PCF_PARM pCfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR Channel,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE)
+{
+	COPY_MAC_ADDR(pBss->Bssid, pBssid);
+	// Default Hidden SSID to be TRUE, it will be turned to FALSE after coping SSID
+	pBss->Hidden = 1;
+	if (SsidLen > 0)
+	{
+		// For hidden SSID AP, it might send beacon with SSID len equal to 0
+		// Or send beacon /probe response with SSID len matching real SSID length,
+		// but SSID is all zero. such as "00-00-00-00" with length 4.
+		// We have to prevent this case overwrite correct table
+		if (NdisEqualMemory(Ssid, ZeroSsid, SsidLen) == 0)
+		{
+		    NdisZeroMemory(pBss->Ssid, MAX_LEN_OF_SSID);
+			NdisMoveMemory(pBss->Ssid, Ssid, SsidLen);
+			pBss->SsidLen = SsidLen;
+			pBss->Hidden = 0;
+		}
+	}
+	else
+		pBss->SsidLen = 0;
+	pBss->BssType = BssType;
+	pBss->BeaconPeriod = BeaconPeriod;
+	if (BssType == BSS_INFRA)
+	{
+		if (pCfParm->bValid)
+		{
+			pBss->CfpCount = pCfParm->CfpCount;
+			pBss->CfpPeriod = pCfParm->CfpPeriod;
+			pBss->CfpMaxDuration = pCfParm->CfpMaxDuration;
+			pBss->CfpDurRemaining = pCfParm->CfpDurRemaining;
+		}
+	}
+	else
+	{
+		pBss->AtimWin = AtimWin;
+	}
+
+	pBss->CapabilityInfo = CapabilityInfo;
+	// The privacy bit indicate security is ON, it maight be WEP, TKIP or AES
+	// Combine with AuthMode, they will decide the connection methods.
+	pBss->Privacy = CAP_IS_PRIVACY_ON(pBss->CapabilityInfo);
+	ASSERT(SupRateLen <= MAX_LEN_OF_SUPPORTED_RATES);
+	if (SupRateLen <= MAX_LEN_OF_SUPPORTED_RATES)
+		NdisMoveMemory(pBss->SupRate, SupRate, SupRateLen);
+	else
+		NdisMoveMemory(pBss->SupRate, SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+	pBss->SupRateLen = SupRateLen;
+	ASSERT(ExtRateLen <= MAX_LEN_OF_SUPPORTED_RATES);
+	NdisMoveMemory(pBss->ExtRate, ExtRate, ExtRateLen);
+	NdisMoveMemory(&pBss->HtCapability, pHtCapability, HtCapabilityLen);
+	NdisMoveMemory(&pBss->AddHtInfo, pAddHtInfo, AddHtInfoLen);
+	pBss->NewExtChanOffset = NewExtChanOffset;
+	pBss->ExtRateLen = ExtRateLen;
+	pBss->Channel = Channel;
+	pBss->CentralChannel = Channel;
+	pBss->Rssi = Rssi;
+	// Update CkipFlag. if not exists, the value is 0x0
+	pBss->CkipFlag = CkipFlag;
+
+	// New for microsoft Fixed IEs
+	NdisMoveMemory(pBss->FixIEs.Timestamp, &TimeStamp, 8);
+	pBss->FixIEs.BeaconInterval = BeaconPeriod;
+	pBss->FixIEs.Capabilities = CapabilityInfo;
+
+	// New for microsoft Variable IEs
+	if (LengthVIE != 0)
+	{
+		pBss->VarIELen = LengthVIE;
+		NdisMoveMemory(pBss->VarIEs, pVIE, pBss->VarIELen);
+	}
+	else
+	{
+		pBss->VarIELen = 0;
+	}
+
+	pBss->AddHtInfoLen = 0;
+	pBss->HtCapabilityLen = 0;
+#ifdef DOT11_N_SUPPORT
+	if (HtCapabilityLen> 0)
+	{
+		pBss->HtCapabilityLen = HtCapabilityLen;
+		NdisMoveMemory(&pBss->HtCapability, pHtCapability, HtCapabilityLen);
+		if (AddHtInfoLen > 0)
+		{
+			pBss->AddHtInfoLen = AddHtInfoLen;
+			NdisMoveMemory(&pBss->AddHtInfo, pAddHtInfo, AddHtInfoLen);
+
+	 			if ((pAddHtInfo->ControlChan > 2)&& (pAddHtInfo->AddHtInfo.ExtChanOffset == EXTCHA_BELOW) && (pHtCapability->HtCapInfo.ChannelWidth == BW_40))
+	 			{
+	 				pBss->CentralChannel = pAddHtInfo->ControlChan - 2;
+	 			}
+	 			else if ((pAddHtInfo->AddHtInfo.ExtChanOffset == EXTCHA_ABOVE) && (pHtCapability->HtCapInfo.ChannelWidth == BW_40))
+				{
+		 				pBss->CentralChannel = pAddHtInfo->ControlChan + 2;
+				}
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	BssCipherParse(pBss);
+
+	// new for QOS
+	if (pEdcaParm)
+		NdisMoveMemory(&pBss->EdcaParm, pEdcaParm, sizeof(EDCA_PARM));
+	else
+		pBss->EdcaParm.bValid = FALSE;
+	if (pQosCapability)
+		NdisMoveMemory(&pBss->QosCapability, pQosCapability, sizeof(QOS_CAPABILITY_PARM));
+	else
+		pBss->QosCapability.bValid = FALSE;
+	if (pQbssLoad)
+		NdisMoveMemory(&pBss->QbssLoad, pQbssLoad, sizeof(QBSS_LOAD_PARM));
+	else
+		pBss->QbssLoad.bValid = FALSE;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		PEID_STRUCT     pEid;
+		USHORT          Length = 0;
+
+
+		NdisZeroMemory(&pBss->WpaIE.IE[0], MAX_CUSTOM_LEN);
+		NdisZeroMemory(&pBss->RsnIE.IE[0], MAX_CUSTOM_LEN);
+#ifdef EXT_BUILD_CHANNEL_LIST
+		NdisZeroMemory(&pBss->CountryString[0], 3);
+		pBss->bHasCountryIE = FALSE;
+#endif // EXT_BUILD_CHANNEL_LIST //
+		pEid = (PEID_STRUCT) pVIE;
+		while ((Length + 2 + (USHORT)pEid->Len) <= LengthVIE)
+		{
+			switch(pEid->Eid)
+			{
+				case IE_WPA:
+					if (NdisEqualMemory(pEid->Octet, WPA_OUI, 4))
+					{
+						if ((pEid->Len + 2) > MAX_CUSTOM_LEN)
+						{
+							pBss->WpaIE.IELen = 0;
+							break;
+						}
+						pBss->WpaIE.IELen = pEid->Len + 2;
+						NdisMoveMemory(pBss->WpaIE.IE, pEid, pBss->WpaIE.IELen);
+					}
+					break;
+                case IE_RSN:
+                    if (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3))
+					{
+						if ((pEid->Len + 2) > MAX_CUSTOM_LEN)
+						{
+							pBss->RsnIE.IELen = 0;
+							break;
+						}
+						pBss->RsnIE.IELen = pEid->Len + 2;
+						NdisMoveMemory(pBss->RsnIE.IE, pEid, pBss->RsnIE.IELen);
+			}
+				break;
+#ifdef EXT_BUILD_CHANNEL_LIST
+				case IE_COUNTRY:
+					NdisMoveMemory(&pBss->CountryString[0], pEid->Octet, 3);
+					pBss->bHasCountryIE = TRUE;
+					break;
+#endif // EXT_BUILD_CHANNEL_LIST //
+            }
+			Length = Length + 2 + (USHORT)pEid->Len;  // Eid[1] + Len[1]+ content[Len]
+			pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+}
+
+/*!
+ *	\brief insert an entry into the bss table
+ *	\param p_tab The BSS table
+ *	\param Bssid BSSID
+ *	\param ssid SSID
+ *	\param ssid_len Length of SSID
+ *	\param bss_type
+ *	\param beacon_period
+ *	\param timestamp
+ *	\param p_cf
+ *	\param atim_win
+ *	\param cap
+ *	\param rates
+ *	\param rates_len
+ *	\param channel_idx
+ *	\return none
+ *	\pre
+ *	\post
+ *	\note If SSID is identical, the old entry will be replaced by the new one
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+ULONG BssTableSetEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT BSS_TABLE *Tab,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN CF_PARM *CfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR ChannelNo,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE)
+{
+	ULONG	Idx;
+
+	Idx = BssTableSearchWithSSID(Tab, pBssid,  Ssid, SsidLen, ChannelNo);
+	if (Idx == BSS_NOT_FOUND)
+	{
+		if (Tab->BssNr >= MAX_LEN_OF_BSS_TABLE)
+	    {
+			//
+			// It may happen when BSS Table was full.
+			// The desired AP will not be added into BSS Table
+			// In this case, if we found the desired AP then overwrite BSS Table.
+			//
+			if(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+			{
+				if (MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, pBssid) ||
+					SSID_EQUAL(pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen, Ssid, SsidLen))
+				{
+					Idx = Tab->BssOverlapNr;
+					BssEntrySet(pAd, &Tab->BssEntry[Idx], pBssid, Ssid, SsidLen, BssType, BeaconPeriod, CfParm, AtimWin,
+						CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,pHtCapability, pAddHtInfo,HtCapabilityLen, AddHtInfoLen,
+						NewExtChanOffset, ChannelNo, Rssi, TimeStamp, CkipFlag, pEdcaParm, pQosCapability, pQbssLoad, LengthVIE, pVIE);
+                    Tab->BssOverlapNr = (Tab->BssOverlapNr++) % MAX_LEN_OF_BSS_TABLE;
+				}
+				return Idx;
+			}
+			else
+			{
+			return BSS_NOT_FOUND;
+			}
+		}
+		Idx = Tab->BssNr;
+		BssEntrySet(pAd, &Tab->BssEntry[Idx], pBssid, Ssid, SsidLen, BssType, BeaconPeriod, CfParm, AtimWin,
+					CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,pHtCapability, pAddHtInfo,HtCapabilityLen, AddHtInfoLen,
+					NewExtChanOffset, ChannelNo, Rssi, TimeStamp, CkipFlag, pEdcaParm, pQosCapability, pQbssLoad, LengthVIE, pVIE);
+		Tab->BssNr++;
+	}
+	else
+	{
+		BssEntrySet(pAd, &Tab->BssEntry[Idx], pBssid, Ssid, SsidLen, BssType, BeaconPeriod,CfParm, AtimWin,
+					CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,pHtCapability, pAddHtInfo,HtCapabilityLen, AddHtInfoLen,
+					NewExtChanOffset, ChannelNo, Rssi, TimeStamp, CkipFlag, pEdcaParm, pQosCapability, pQbssLoad, LengthVIE, pVIE);
+	}
+
+	return Idx;
+}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+VOID  TriEventInit(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR		i;
+
+	for (i = 0;i < MAX_TRIGGER_EVENT;i++)
+		pAd->CommonCfg.TriggerEventTab.EventA[i].bValid = FALSE;
+
+	pAd->CommonCfg.TriggerEventTab.EventANo = 0;
+	pAd->CommonCfg.TriggerEventTab.EventBCountDown = 0;
+}
+
+ULONG TriEventTableSetEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT TRIGGER_EVENT_TAB *Tab,
+	IN PUCHAR pBssid,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			RegClass,
+	IN UCHAR ChannelNo)
+{
+	// Event A
+	if (HtCapabilityLen == 0)
+	{
+		if (Tab->EventANo < MAX_TRIGGER_EVENT)
+		{
+			RTMPMoveMemory(Tab->EventA[Tab->EventANo].BSSID, pBssid, 6);
+			Tab->EventA[Tab->EventANo].bValid = TRUE;
+			Tab->EventA[Tab->EventANo].Channel = ChannelNo;
+			Tab->EventA[Tab->EventANo].CDCounter = pAd->CommonCfg.Dot11BssWidthChanTranDelay;
+			if (RegClass != 0)
+			{
+				// Beacon has Regulatory class IE. So use beacon's
+				Tab->EventA[Tab->EventANo].RegClass = RegClass;
+			}
+			else
+			{
+				// Use Station's Regulatory class instead.
+				if (pAd->StaActive.SupportedHtPhy.bHtEnable == TRUE)
+				{
+					if (pAd->CommonCfg.CentralChannel > pAd->CommonCfg.Channel)
+					{
+						Tab->EventA[Tab->EventANo].RegClass = 32;
+					}
+					else if (pAd->CommonCfg.CentralChannel < pAd->CommonCfg.Channel)
+						Tab->EventA[Tab->EventANo].RegClass = 33;
+				}
+				else
+					Tab->EventA[Tab->EventANo].RegClass = ??;
+
+			}
+
+			Tab->EventANo ++;
+		}
+	}
+	else if (pHtCapability->HtCapInfo.Intolerant40)
+	{
+		Tab->EventBCountDown = pAd->CommonCfg.Dot11BssWidthChanTranDelay;
+	}
+
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Trigger Event table Maintainence called once every second.
+
+	Arguments:
+	// IRQL = DISPATCH_LEVEL
+	========================================================================
+*/
+VOID TriEventCounterMaintenance(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR		i;
+	BOOLEAN			bNotify = FALSE;
+	for (i = 0;i < MAX_TRIGGER_EVENT;i++)
+	{
+		if (pAd->CommonCfg.TriggerEventTab.EventA[i].bValid && (pAd->CommonCfg.TriggerEventTab.EventA[i].CDCounter > 0))
+		{
+			pAd->CommonCfg.TriggerEventTab.EventA[i].CDCounter--;
+			if (pAd->CommonCfg.TriggerEventTab.EventA[i].CDCounter == 0)
+			{
+				pAd->CommonCfg.TriggerEventTab.EventA[i].bValid = FALSE;
+				pAd->CommonCfg.TriggerEventTab.EventANo --;
+				// Need to send 20/40 Coexistence Notify frame if has status change.
+				bNotify = TRUE;
+			}
+		}
+	}
+	if (pAd->CommonCfg.TriggerEventTab.EventBCountDown > 0)
+	{
+		pAd->CommonCfg.TriggerEventTab.EventBCountDown--;
+		if (pAd->CommonCfg.TriggerEventTab.EventBCountDown == 0)
+			bNotify = TRUE;
+	}
+
+	if (bNotify == TRUE)
+		Update2040CoexistFrameAndNotify(pAd, BSSID_WCID, TRUE);
+}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+// IRQL = DISPATCH_LEVEL
+VOID BssTableSsidSort(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT BSS_TABLE *OutTab,
+	IN	CHAR Ssid[],
+	IN	UCHAR SsidLen)
+{
+	INT i;
+	BssTableInit(OutTab);
+
+	for (i = 0; i < pAd->ScanTab.BssNr; i++)
+	{
+		BSS_ENTRY *pInBss = &pAd->ScanTab.BssEntry[i];
+		BOOLEAN	bIsHiddenApIncluded = FALSE;
+
+		if (((pAd->CommonCfg.bIEEE80211H == 1) &&
+            (pAd->MlmeAux.Channel > 14) &&
+             RadarChannelCheck(pAd, pInBss->Channel))
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+             || (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+#endif // CARRIER_DETECTION_SUPPORT //
+            )
+		{
+			if (pInBss->Hidden)
+				bIsHiddenApIncluded = TRUE;
+		}
+
+		if ((pInBss->BssType == pAd->StaCfg.BssType) &&
+			(SSID_EQUAL(Ssid, SsidLen, pInBss->Ssid, pInBss->SsidLen) || bIsHiddenApIncluded))
+		{
+			BSS_ENTRY *pOutBss = &OutTab->BssEntry[OutTab->BssNr];
+
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+			// If no Country IE exists no Connection will be established when IEEE80211dClientMode is strict.
+			if ((pAd->StaCfg.IEEE80211dClientMode == Rt802_11_D_Strict) &&
+				(pInBss->bHasCountryIE == FALSE))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("StaCfg.IEEE80211dClientMode == Rt802_11_D_Strict, but this AP doesn't have country IE.\n"));
+				continue;
+			}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef DOT11_N_SUPPORT
+			// 2.4G/5G N only mode
+			if ((pInBss->HtCapabilityLen == 0) &&
+				((pAd->CommonCfg.PhyMode == PHY_11N_2_4G) || (pAd->CommonCfg.PhyMode == PHY_11N_5G)))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("STA is in N-only Mode, this AP don't have Ht capability in Beacon.\n"));
+				continue;
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// New for WPA2
+			// Check the Authmode first
+			if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+			{
+				// Check AuthMode and AuthModeAux for matching, in case AP support dual-mode
+				if ((pAd->StaCfg.AuthMode != pInBss->AuthMode) && (pAd->StaCfg.AuthMode != pInBss->AuthModeAux))
+					// None matched
+					continue;
+
+				// Check cipher suite, AP must have more secured cipher than station setting
+				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA.GroupCipher)
+							continue;
+
+					// check group cipher
+					if (pAd->StaCfg.WepStatus < pInBss->WPA.GroupCipher)
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipherAux))
+						continue;
+				}
+				else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA2.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA2.GroupCipher)
+							continue;
+
+					// check group cipher
+					if (pAd->StaCfg.WepStatus < pInBss->WPA2.GroupCipher)
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipherAux))
+						continue;
+				}
+			}
+			// Bss Type matched, SSID matched.
+			// We will check wepstatus for qualification Bss
+			else if (pAd->StaCfg.WepStatus != pInBss->WepStatus)
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("StaCfg.WepStatus=%d, while pInBss->WepStatus=%d\n", pAd->StaCfg.WepStatus, pInBss->WepStatus));
+				//
+				// For the SESv2 case, we will not qualify WepStatus.
+				//
+				if (!pInBss->bSES)
+					continue;
+			}
+
+			// Since the AP is using hidden SSID, and we are trying to connect to ANY
+			// It definitely will fail. So, skip it.
+			// CCX also require not even try to connect it!!
+			if (SsidLen == 0)
+				continue;
+
+#ifdef DOT11_N_SUPPORT
+			// If both station and AP use 40MHz, still need to check if the 40MHZ band's legality in my country region
+			// If this 40MHz wideband is not allowed in my country list, use bandwidth 20MHZ instead,
+			if ((pInBss->CentralChannel != pInBss->Channel) &&
+				(pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40))
+			{
+				if (RTMPCheckChannel(pAd, pInBss->CentralChannel, pInBss->Channel) == FALSE)
+				{
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_20;
+					SetCommonHT(pAd);
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_40;
+				}
+				else
+				{
+					if (pAd->CommonCfg.DesiredHtPhy.ChannelWidth == BAND_WIDTH_20)
+					{
+						SetCommonHT(pAd);
+					}
+				}
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// copy matching BSS from InTab to OutTab
+			NdisMoveMemory(pOutBss, pInBss, sizeof(BSS_ENTRY));
+
+			OutTab->BssNr++;
+		}
+		else if ((pInBss->BssType == pAd->StaCfg.BssType) && (SsidLen == 0))
+		{
+			BSS_ENTRY *pOutBss = &OutTab->BssEntry[OutTab->BssNr];
+
+
+#ifdef DOT11_N_SUPPORT
+			// 2.4G/5G N only mode
+			if ((pInBss->HtCapabilityLen == 0) &&
+				((pAd->CommonCfg.PhyMode == PHY_11N_2_4G) || (pAd->CommonCfg.PhyMode == PHY_11N_5G)))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("STA is in N-only Mode, this AP don't have Ht capability in Beacon.\n"));
+				continue;
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// New for WPA2
+			// Check the Authmode first
+			if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+			{
+				// Check AuthMode and AuthModeAux for matching, in case AP support dual-mode
+				if ((pAd->StaCfg.AuthMode != pInBss->AuthMode) && (pAd->StaCfg.AuthMode != pInBss->AuthModeAux))
+					// None matched
+					continue;
+
+				// Check cipher suite, AP must have more secured cipher than station setting
+				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA.GroupCipher)
+							continue;
+
+					// check group cipher
+					if (pAd->StaCfg.WepStatus < pInBss->WPA.GroupCipher)
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipherAux))
+						continue;
+				}
+				else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA2.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA2.GroupCipher)
+							continue;
+
+					// check group cipher
+					if (pAd->StaCfg.WepStatus < pInBss->WPA2.GroupCipher)
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipherAux))
+						continue;
+				}
+			}
+			// Bss Type matched, SSID matched.
+			// We will check wepstatus for qualification Bss
+			else if (pAd->StaCfg.WepStatus != pInBss->WepStatus)
+					continue;
+
+#ifdef DOT11_N_SUPPORT
+			// If both station and AP use 40MHz, still need to check if the 40MHZ band's legality in my country region
+			// If this 40MHz wideband is not allowed in my country list, use bandwidth 20MHZ instead,
+			if ((pInBss->CentralChannel != pInBss->Channel) &&
+				(pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40))
+			{
+				if (RTMPCheckChannel(pAd, pInBss->CentralChannel, pInBss->Channel) == FALSE)
+				{
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_20;
+					SetCommonHT(pAd);
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_40;
+				}
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// copy matching BSS from InTab to OutTab
+			NdisMoveMemory(pOutBss, pInBss, sizeof(BSS_ENTRY));
+
+			OutTab->BssNr++;
+		}
+
+		if (OutTab->BssNr >= MAX_LEN_OF_BSS_TABLE)
+			break;
+	}
+
+	BssTableSortByRssi(OutTab);
+}
+
+
+// IRQL = DISPATCH_LEVEL
+VOID BssTableSortByRssi(
+	IN OUT BSS_TABLE *OutTab)
+{
+	INT 	  i, j;
+	BSS_ENTRY TmpBss;
+
+	for (i = 0; i < OutTab->BssNr - 1; i++)
+	{
+		for (j = i+1; j < OutTab->BssNr; j++)
+		{
+			if (OutTab->BssEntry[j].Rssi > OutTab->BssEntry[i].Rssi)
+			{
+				NdisMoveMemory(&TmpBss, &OutTab->BssEntry[j], sizeof(BSS_ENTRY));
+				NdisMoveMemory(&OutTab->BssEntry[j], &OutTab->BssEntry[i], sizeof(BSS_ENTRY));
+				NdisMoveMemory(&OutTab->BssEntry[i], &TmpBss, sizeof(BSS_ENTRY));
+			}
+		}
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+VOID BssCipherParse(
+	IN OUT	PBSS_ENTRY	pBss)
+{
+	PEID_STRUCT 		 pEid;
+	PUCHAR				pTmp;
+	PRSN_IE_HEADER_STRUCT			pRsnHeader;
+	PCIPHER_SUITE_STRUCT			pCipher;
+	PAKM_SUITE_STRUCT				pAKM;
+	USHORT							Count;
+	INT								Length;
+	NDIS_802_11_ENCRYPTION_STATUS	TmpCipher;
+
+	//
+	// WepStatus will be reset later, if AP announce TKIP or AES on the beacon frame.
+	//
+	if (pBss->Privacy)
+	{
+		pBss->WepStatus 	= Ndis802_11WEPEnabled;
+	}
+	else
+	{
+		pBss->WepStatus 	= Ndis802_11WEPDisabled;
+	}
+	// Set default to disable & open authentication before parsing variable IE
+	pBss->AuthMode		= Ndis802_11AuthModeOpen;
+	pBss->AuthModeAux	= Ndis802_11AuthModeOpen;
+
+	// Init WPA setting
+	pBss->WPA.PairCipher	= Ndis802_11WEPDisabled;
+	pBss->WPA.PairCipherAux = Ndis802_11WEPDisabled;
+	pBss->WPA.GroupCipher	= Ndis802_11WEPDisabled;
+	pBss->WPA.RsnCapability = 0;
+	pBss->WPA.bMixMode		= FALSE;
+
+	// Init WPA2 setting
+	pBss->WPA2.PairCipher	 = Ndis802_11WEPDisabled;
+	pBss->WPA2.PairCipherAux = Ndis802_11WEPDisabled;
+	pBss->WPA2.GroupCipher	 = Ndis802_11WEPDisabled;
+	pBss->WPA2.RsnCapability = 0;
+	pBss->WPA2.bMixMode 	 = FALSE;
+
+
+	Length = (INT) pBss->VarIELen;
+
+	while (Length > 0)
+	{
+		// Parse cipher suite base on WPA1 & WPA2, they should be parsed differently
+		pTmp = ((PUCHAR) pBss->VarIEs) + pBss->VarIELen - Length;
+		pEid = (PEID_STRUCT) pTmp;
+		switch (pEid->Eid)
+		{
+			case IE_WPA:
+				//Parse Cisco IE_WPA (LEAP, CCKM, etc.)
+				if ( NdisEqualMemory((pTmp+8), CISCO_OUI, 3))
+				{
+					pTmp   += 11;
+					switch (*pTmp)
+					{
+						case 1:
+						case 5:	// Although WEP is not allowed in WPA related auth mode, we parse it anyway
+							pBss->WepStatus = Ndis802_11Encryption1Enabled;
+							pBss->WPA.PairCipher = Ndis802_11Encryption1Enabled;
+							pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 2:
+							pBss->WepStatus = Ndis802_11Encryption2Enabled;
+							pBss->WPA.PairCipher = Ndis802_11Encryption1Enabled;
+							pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 4:
+							pBss->WepStatus = Ndis802_11Encryption3Enabled;
+							pBss->WPA.PairCipher = Ndis802_11Encryption1Enabled;
+							pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+							break;
+						default:
+							break;
+					}
+
+					// if Cisco IE_WPA, break
+					break;
+				}
+				else if (NdisEqualMemory(pEid->Octet, SES_OUI, 3) && (pEid->Len == 7))
+				{
+					pBss->bSES = TRUE;
+					break;
+				}
+				else if (NdisEqualMemory(pEid->Octet, WPA_OUI, 4) != 1)
+				{
+					// if unsupported vendor specific IE
+					break;
+				}
+				// Skip OUI, version, and multicast suite
+				// This part should be improved in the future when AP supported multiple cipher suite.
+				// For now, it's OK since almost all APs have fixed cipher suite supported.
+				// pTmp = (PUCHAR) pEid->Octet;
+				pTmp   += 11;
+
+				// Cipher Suite Selectors from Spec P802.11i/D3.2 P26.
+				//	Value	   Meaning
+				//	0			None
+				//	1			WEP-40
+				//	2			Tkip
+				//	3			WRAP
+				//	4			AES
+				//	5			WEP-104
+				// Parse group cipher
+				switch (*pTmp)
+				{
+					case 1:
+					case 5:	// Although WEP is not allowed in WPA related auth mode, we parse it anyway
+						pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+						break;
+					case 2:
+						pBss->WPA.GroupCipher = Ndis802_11Encryption2Enabled;
+						break;
+					case 4:
+						pBss->WPA.GroupCipher = Ndis802_11Encryption3Enabled;
+						break;
+					default:
+						break;
+				}
+				// number of unicast suite
+				pTmp   += 1;
+
+				// skip all unicast cipher suites
+				//Count = *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+
+				// Parsing all unicast cipher suite
+				while (Count > 0)
+				{
+					// Skip OUI
+					pTmp += 3;
+					TmpCipher = Ndis802_11WEPDisabled;
+					switch (*pTmp)
+					{
+						case 1:
+						case 5: // Although WEP is not allowed in WPA related auth mode, we parse it anyway
+							TmpCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 2:
+							TmpCipher = Ndis802_11Encryption2Enabled;
+							break;
+						case 4:
+							TmpCipher = Ndis802_11Encryption3Enabled;
+							break;
+						default:
+							break;
+					}
+					if (TmpCipher > pBss->WPA.PairCipher)
+					{
+						// Move the lower cipher suite to PairCipherAux
+						pBss->WPA.PairCipherAux = pBss->WPA.PairCipher;
+						pBss->WPA.PairCipher	= TmpCipher;
+					}
+					else
+					{
+						pBss->WPA.PairCipherAux = TmpCipher;
+					}
+					pTmp++;
+					Count--;
+				}
+
+				// 4. get AKM suite counts
+				//Count	= *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+				pTmp   += 3;
+
+				switch (*pTmp)
+				{
+					case 1:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPA;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPA;
+						break;
+					case 2:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPAPSK;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPAPSK;
+						break;
+					default:
+						break;
+				}
+				pTmp   += 1;
+
+				// Fixed for WPA-None
+				if (pBss->BssType == BSS_ADHOC)
+				{
+					pBss->AuthMode	  = Ndis802_11AuthModeWPANone;
+					pBss->AuthModeAux = Ndis802_11AuthModeWPANone;
+					pBss->WepStatus   = pBss->WPA.GroupCipher;
+					// Patched bugs for old driver
+					if (pBss->WPA.PairCipherAux == Ndis802_11WEPDisabled)
+						pBss->WPA.PairCipherAux = pBss->WPA.GroupCipher;
+				}
+				else
+					pBss->WepStatus   = pBss->WPA.PairCipher;
+
+				// Check the Pair & Group, if different, turn on mixed mode flag
+				if (pBss->WPA.GroupCipher != pBss->WPA.PairCipher)
+					pBss->WPA.bMixMode = TRUE;
+
+				break;
+
+			case IE_RSN:
+				pRsnHeader = (PRSN_IE_HEADER_STRUCT) pTmp;
+
+				// 0. Version must be 1
+				if (le2cpu16(pRsnHeader->Version) != 1)
+					break;
+				pTmp   += sizeof(RSN_IE_HEADER_STRUCT);
+
+				// 1. Check group cipher
+				pCipher = (PCIPHER_SUITE_STRUCT) pTmp;
+				if (!RTMPEqualMemory(pTmp, RSN_OUI, 3))
+					break;
+
+				// Parse group cipher
+				switch (pCipher->Type)
+				{
+					case 1:
+					case 5:	// Although WEP is not allowed in WPA related auth mode, we parse it anyway
+						pBss->WPA2.GroupCipher = Ndis802_11Encryption1Enabled;
+						break;
+					case 2:
+						pBss->WPA2.GroupCipher = Ndis802_11Encryption2Enabled;
+						break;
+					case 4:
+						pBss->WPA2.GroupCipher = Ndis802_11Encryption3Enabled;
+						break;
+					default:
+						break;
+				}
+				// set to correct offset for next parsing
+				pTmp   += sizeof(CIPHER_SUITE_STRUCT);
+
+				// 2. Get pairwise cipher counts
+				//Count = *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+
+				// 3. Get pairwise cipher
+				// Parsing all unicast cipher suite
+				while (Count > 0)
+				{
+					// Skip OUI
+					pCipher = (PCIPHER_SUITE_STRUCT) pTmp;
+					TmpCipher = Ndis802_11WEPDisabled;
+					switch (pCipher->Type)
+					{
+						case 1:
+						case 5: // Although WEP is not allowed in WPA related auth mode, we parse it anyway
+							TmpCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 2:
+							TmpCipher = Ndis802_11Encryption2Enabled;
+							break;
+						case 4:
+							TmpCipher = Ndis802_11Encryption3Enabled;
+							break;
+						default:
+							break;
+					}
+					if (TmpCipher > pBss->WPA2.PairCipher)
+					{
+						// Move the lower cipher suite to PairCipherAux
+						pBss->WPA2.PairCipherAux = pBss->WPA2.PairCipher;
+						pBss->WPA2.PairCipher	 = TmpCipher;
+					}
+					else
+					{
+						pBss->WPA2.PairCipherAux = TmpCipher;
+					}
+					pTmp += sizeof(CIPHER_SUITE_STRUCT);
+					Count--;
+				}
+
+				// 4. get AKM suite counts
+				//Count	= *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+
+				// 5. Get AKM ciphers
+				pAKM = (PAKM_SUITE_STRUCT) pTmp;
+				if (!RTMPEqualMemory(pTmp, RSN_OUI, 3))
+					break;
+
+				switch (pAKM->Type)
+				{
+					case 1:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPA2;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPA2;
+						break;
+					case 2:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPA2PSK;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPA2PSK;
+						break;
+					default:
+						break;
+				}
+				pTmp   += (Count * sizeof(AKM_SUITE_STRUCT));
+
+				// Fixed for WPA-None
+				if (pBss->BssType == BSS_ADHOC)
+				{
+					pBss->AuthMode = Ndis802_11AuthModeWPANone;
+					pBss->AuthModeAux = Ndis802_11AuthModeWPANone;
+					pBss->WPA.PairCipherAux = pBss->WPA2.PairCipherAux;
+					pBss->WPA.GroupCipher	= pBss->WPA2.GroupCipher;
+					pBss->WepStatus 		= pBss->WPA.GroupCipher;
+					// Patched bugs for old driver
+					if (pBss->WPA.PairCipherAux == Ndis802_11WEPDisabled)
+						pBss->WPA.PairCipherAux = pBss->WPA.GroupCipher;
+				}
+				pBss->WepStatus   = pBss->WPA2.PairCipher;
+
+				// 6. Get RSN capability
+				//pBss->WPA2.RsnCapability = *(PUSHORT) pTmp;
+				pBss->WPA2.RsnCapability = (pTmp[1]<<8) + pTmp[0];
+				pTmp += sizeof(USHORT);
+
+				// Check the Pair & Group, if different, turn on mixed mode flag
+				if (pBss->WPA2.GroupCipher != pBss->WPA2.PairCipher)
+					pBss->WPA2.bMixMode = TRUE;
+
+				break;
+			default:
+				break;
+		}
+		Length -= (pEid->Len + 2);
+	}
+}
+
+// ===========================================================================================
+// mac_table.c
+// ===========================================================================================
+
+/*! \brief generates a random mac address value for IBSS BSSID
+ *	\param Addr the bssid location
+ *	\return none
+ *	\pre
+ *	\post
+ */
+VOID MacAddrRandomBssid(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pAddr)
+{
+	INT i;
+
+	for (i = 0; i < MAC_ADDR_LEN; i++)
+	{
+		pAddr[i] = RandomByte(pAd);
+	}
+
+	pAddr[0] = (pAddr[0] & 0xfe) | 0x02;  // the first 2 bits must be 01xxxxxxxx
+}
+
+/*! \brief init the management mac frame header
+ *	\param p_hdr mac header
+ *	\param subtype subtype of the frame
+ *	\param p_ds destination address, don't care if it is a broadcast address
+ *	\return none
+ *	\pre the station has the following information in the pAd->StaCfg
+ *	 - bssid
+ *	 - station address
+ *	\post
+ *	\note this function initializes the following field
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID MgtMacHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PHEADER_802_11 pHdr80211,
+	IN UCHAR SubType,
+	IN UCHAR ToDs,
+	IN PUCHAR pDA,
+	IN PUCHAR pBssid)
+{
+	NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+
+	pHdr80211->FC.Type = BTYPE_MGMT;
+	pHdr80211->FC.SubType = SubType;
+	pHdr80211->FC.ToDs = ToDs;
+	COPY_MAC_ADDR(pHdr80211->Addr1, pDA);
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+	COPY_MAC_ADDR(pHdr80211->Addr3, pBssid);
+}
+
+// ===========================================================================================
+// mem_mgmt.c
+// ===========================================================================================
+
+/*!***************************************************************************
+ * This routine build an outgoing frame, and fill all information specified
+ * in argument list to the frame body. The actual frame size is the summation
+ * of all arguments.
+ * input params:
+ *		Buffer - pointer to a pre-allocated memory segment
+ *		args - a list of <int arg_size, arg> pairs.
+ *		NOTE NOTE NOTE!!!! the last argument must be NULL, otherwise this
+ *						   function will FAIL!!!
+ * return:
+ *		Size of the buffer
+ * usage:
+ *		MakeOutgoingFrame(Buffer, output_length, 2, &fc, 2, &dur, 6, p_addr1, 6,p_addr2, END_OF_ARGS);
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ ****************************************************************************/
+ULONG MakeOutgoingFrame(
+	OUT CHAR *Buffer,
+	OUT ULONG *FrameLen, ...)
+{
+	CHAR   *p;
+	int 	leng;
+	ULONG	TotLeng;
+	va_list Args;
+
+	// calculates the total length
+	TotLeng = 0;
+	va_start(Args, FrameLen);
+	do
+	{
+		leng = va_arg(Args, int);
+		if (leng == END_OF_ARGS)
+		{
+			break;
+		}
+		p = va_arg(Args, PVOID);
+		NdisMoveMemory(&Buffer[TotLeng], p, leng);
+		TotLeng = TotLeng + leng;
+	} while(TRUE);
+
+	va_end(Args); /* clean up */
+	*FrameLen = TotLeng;
+	return TotLeng;
+}
+
+// ===========================================================================================
+// mlme_queue.c
+// ===========================================================================================
+
+/*! \brief	Initialize The MLME Queue, used by MLME Functions
+ *	\param	*Queue	   The MLME Queue
+ *	\return Always	   Return NDIS_STATE_SUCCESS in this implementation
+ *	\pre
+ *	\post
+ *	\note	Because this is done only once (at the init stage), no need to be locked
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+NDIS_STATUS MlmeQueueInit(
+	IN MLME_QUEUE *Queue)
+{
+	INT i;
+
+	NdisAllocateSpinLock(&Queue->Lock);
+
+	Queue->Num	= 0;
+	Queue->Head = 0;
+	Queue->Tail = 0;
+
+	for (i = 0; i < MAX_LEN_OF_MLME_QUEUE; i++)
+	{
+		Queue->Entry[i].Occupied = FALSE;
+		Queue->Entry[i].MsgLen = 0;
+		NdisZeroMemory(Queue->Entry[i].Msg, MGMT_DMA_BUFFER_SIZE);
+	}
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*! \brief	 Enqueue a message for other threads, if they want to send messages to MLME thread
+ *	\param	*Queue	  The MLME Queue
+ *	\param	 Machine  The State Machine Id
+ *	\param	 MsgType  The Message Type
+ *	\param	 MsgLen   The Message length
+ *	\param	*Msg	  The message pointer
+ *	\return  TRUE if enqueue is successful, FALSE if the queue is full
+ *	\pre
+ *	\post
+ *	\note	 The message has to be initialized
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeEnqueue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN ULONG Machine,
+	IN ULONG MsgType,
+	IN ULONG MsgLen,
+	IN VOID *Msg)
+{
+	INT Tail;
+	MLME_QUEUE	*Queue = (MLME_QUEUE *)&pAd->Mlme.Queue;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return FALSE;
+
+	// First check the size, it MUST not exceed the mlme queue size
+	if (MsgLen > MGMT_DMA_BUFFER_SIZE)
+	{
+		DBGPRINT_ERR(("MlmeEnqueue: msg too large, size = %ld \n", MsgLen));
+		return FALSE;
+	}
+
+	if (MlmeQueueFull(Queue))
+	{
+		return FALSE;
+	}
+
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Tail = Queue->Tail;
+	Queue->Tail++;
+	Queue->Num++;
+	if (Queue->Tail == MAX_LEN_OF_MLME_QUEUE)
+	{
+		Queue->Tail = 0;
+	}
+
+	Queue->Entry[Tail].Wcid = RESERVED_WCID;
+	Queue->Entry[Tail].Occupied = TRUE;
+	Queue->Entry[Tail].Machine = Machine;
+	Queue->Entry[Tail].MsgType = MsgType;
+	Queue->Entry[Tail].MsgLen  = MsgLen;
+
+	if (Msg != NULL)
+	{
+		NdisMoveMemory(Queue->Entry[Tail].Msg, Msg, MsgLen);
+	}
+
+	NdisReleaseSpinLock(&(Queue->Lock));
+	return TRUE;
+}
+
+/*! \brief	 This function is used when Recv gets a MLME message
+ *	\param	*Queue			 The MLME Queue
+ *	\param	 TimeStampHigh	 The upper 32 bit of timestamp
+ *	\param	 TimeStampLow	 The lower 32 bit of timestamp
+ *	\param	 Rssi			 The receiving RSSI strength
+ *	\param	 MsgLen 		 The length of the message
+ *	\param	*Msg			 The message pointer
+ *	\return  TRUE if everything ok, FALSE otherwise (like Queue Full)
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeEnqueueForRecv(
+	IN	PRTMP_ADAPTER	pAd,
+	IN ULONG Wcid,
+	IN ULONG TimeStampHigh,
+	IN ULONG TimeStampLow,
+	IN UCHAR Rssi0,
+	IN UCHAR Rssi1,
+	IN UCHAR Rssi2,
+	IN ULONG MsgLen,
+	IN VOID *Msg,
+	IN UCHAR Signal)
+{
+	INT 		 Tail, Machine;
+	PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+	INT		 MsgType;
+	MLME_QUEUE	*Queue = (MLME_QUEUE *)&pAd->Mlme.Queue;
+
+#ifdef RALINK_ATE
+	/* Nothing to do in ATE mode */
+	if(ATE_ON(pAd))
+		return FALSE;
+#endif // RALINK_ATE //
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		DBGPRINT_ERR(("MlmeEnqueueForRecv: fRTMP_ADAPTER_HALT_IN_PROGRESS\n"));
+		return FALSE;
+	}
+
+	// First check the size, it MUST not exceed the mlme queue size
+	if (MsgLen > MGMT_DMA_BUFFER_SIZE)
+	{
+		DBGPRINT_ERR(("MlmeEnqueueForRecv: frame too large, size = %ld \n", MsgLen));
+		return FALSE;
+	}
+
+	if (MlmeQueueFull(Queue))
+	{
+		return FALSE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (!MsgTypeSubst(pAd, pFrame, &Machine, &MsgType))
+		{
+			DBGPRINT_ERR(("MlmeEnqueueForRecv: un-recongnized mgmt->subtype=%d\n",pFrame->Hdr.FC.SubType));
+			return FALSE;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// OK, we got all the informations, it is time to put things into queue
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Tail = Queue->Tail;
+	Queue->Tail++;
+	Queue->Num++;
+	if (Queue->Tail == MAX_LEN_OF_MLME_QUEUE)
+	{
+		Queue->Tail = 0;
+	}
+	Queue->Entry[Tail].Occupied = TRUE;
+	Queue->Entry[Tail].Machine = Machine;
+	Queue->Entry[Tail].MsgType = MsgType;
+	Queue->Entry[Tail].MsgLen  = MsgLen;
+	Queue->Entry[Tail].TimeStamp.u.LowPart = TimeStampLow;
+	Queue->Entry[Tail].TimeStamp.u.HighPart = TimeStampHigh;
+	Queue->Entry[Tail].Rssi0 = Rssi0;
+	Queue->Entry[Tail].Rssi1 = Rssi1;
+	Queue->Entry[Tail].Rssi2 = Rssi2;
+	Queue->Entry[Tail].Signal = Signal;
+	Queue->Entry[Tail].Wcid = (UCHAR)Wcid;
+
+	Queue->Entry[Tail].Channel = pAd->LatchRfRegs.Channel;
+
+	if (Msg != NULL)
+	{
+		NdisMoveMemory(Queue->Entry[Tail].Msg, Msg, MsgLen);
+	}
+
+	NdisReleaseSpinLock(&(Queue->Lock));
+
+	RT28XX_MLME_HANDLER(pAd);
+
+	return TRUE;
+}
+
+
+/*! \brief	 Dequeue a message from the MLME Queue
+ *	\param	*Queue	  The MLME Queue
+ *	\param	*Elem	  The message dequeued from MLME Queue
+ *	\return  TRUE if the Elem contains something, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeDequeue(
+	IN MLME_QUEUE *Queue,
+	OUT MLME_QUEUE_ELEM **Elem)
+{
+	NdisAcquireSpinLock(&(Queue->Lock));
+	*Elem = &(Queue->Entry[Queue->Head]);
+	Queue->Num--;
+	Queue->Head++;
+	if (Queue->Head == MAX_LEN_OF_MLME_QUEUE)
+	{
+		Queue->Head = 0;
+	}
+	NdisReleaseSpinLock(&(Queue->Lock));
+	return TRUE;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID	MlmeRestartStateMachine(
+	IN	PRTMP_ADAPTER	pAd)
+{
+#ifdef RT2860
+	MLME_QUEUE_ELEM		*Elem = NULL;
+#endif // RT2860 //
+#ifdef CONFIG_STA_SUPPORT
+	BOOLEAN				Cancelled;
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("MlmeRestartStateMachine \n"));
+
+#ifdef RT2860
+	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
+	if(pAd->Mlme.bRunning)
+	{
+		NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+		return;
+	}
+	else
+	{
+		pAd->Mlme.bRunning = TRUE;
+	}
+	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+
+	// Remove all Mlme queues elements
+	while (!MlmeQueueEmpty(&pAd->Mlme.Queue))
+	{
+		//From message type, determine which state machine I should drive
+		if (MlmeDequeue(&pAd->Mlme.Queue, &Elem))
+		{
+			// free MLME element
+			Elem->Occupied = FALSE;
+			Elem->MsgLen = 0;
+
+		}
+		else {
+			DBGPRINT_ERR(("MlmeRestartStateMachine: MlmeQueue empty\n"));
+		}
+	}
+#endif // RT2860 //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef QOS_DLS_SUPPORT
+		UCHAR i;
+#endif // QOS_DLS_SUPPORT //
+		// Cancel all timer events
+		// Be careful to cancel new added timer
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,	  &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,  &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,	   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,	   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,	   &Cancelled);
+
+#ifdef QOS_DLS_SUPPORT
+		for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &Cancelled);
+		}
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Change back to original channel in case of doing scan
+	AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+	// Resume MSDU which is turned off durning scan
+	RTMPResumeMsduTransmission(pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Set all state machines back IDLE
+		pAd->Mlme.CntlMachine.CurrState    = CNTL_IDLE;
+		pAd->Mlme.AssocMachine.CurrState   = ASSOC_IDLE;
+		pAd->Mlme.AuthMachine.CurrState    = AUTH_REQ_IDLE;
+		pAd->Mlme.AuthRspMachine.CurrState = AUTH_RSP_IDLE;
+		pAd->Mlme.SyncMachine.CurrState    = SYNC_IDLE;
+		pAd->Mlme.ActMachine.CurrState    = ACT_IDLE;
+#ifdef QOS_DLS_SUPPORT
+		pAd->Mlme.DlsMachine.CurrState    = DLS_IDLE;
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RT2860
+	// Remove running state
+	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
+	pAd->Mlme.bRunning = FALSE;
+	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+#endif // RT2860 //
+}
+
+/*! \brief	test if the MLME Queue is empty
+ *	\param	*Queue	  The MLME Queue
+ *	\return TRUE if the Queue is empty, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeQueueEmpty(
+	IN MLME_QUEUE *Queue)
+{
+	BOOLEAN Ans;
+
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Ans = (Queue->Num == 0);
+	NdisReleaseSpinLock(&(Queue->Lock));
+
+	return Ans;
+}
+
+/*! \brief	 test if the MLME Queue is full
+ *	\param	 *Queue 	 The MLME Queue
+ *	\return  TRUE if the Queue is empty, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeQueueFull(
+	IN MLME_QUEUE *Queue)
+{
+	BOOLEAN Ans;
+
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Ans = (Queue->Num == MAX_LEN_OF_MLME_QUEUE || Queue->Entry[Queue->Tail].Occupied);
+	NdisReleaseSpinLock(&(Queue->Lock));
+
+	return Ans;
+}
+
+/*! \brief	 The destructor of MLME Queue
+ *	\param
+ *	\return
+ *	\pre
+ *	\post
+ *	\note	Clear Mlme Queue, Set Queue->Num to Zero.
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+VOID MlmeQueueDestroy(
+	IN MLME_QUEUE *pQueue)
+{
+	NdisAcquireSpinLock(&(pQueue->Lock));
+	pQueue->Num  = 0;
+	pQueue->Head = 0;
+	pQueue->Tail = 0;
+	NdisReleaseSpinLock(&(pQueue->Lock));
+	NdisFreeSpinLock(&(pQueue->Lock));
+}
+
+/*! \brief	 To substitute the message type if the message is coming from external
+ *	\param	pFrame		   The frame received
+ *	\param	*Machine	   The state machine
+ *	\param	*MsgType	   the message type for the state machine
+ *	\return TRUE if the substitution is successful, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+#ifdef CONFIG_STA_SUPPORT
+BOOLEAN MsgTypeSubst(
+	IN PRTMP_ADAPTER  pAd,
+	IN PFRAME_802_11 pFrame,
+	OUT INT *Machine,
+	OUT INT *MsgType)
+{
+	USHORT	Seq;
+	UCHAR	EAPType;
+	PUCHAR	pData;
+
+	// Pointer to start of data frames including SNAP header
+	pData = (PUCHAR) pFrame + LENGTH_802_11;
+
+	// The only data type will pass to this function is EAPOL frame
+	if (pFrame->Hdr.FC.Type == BTYPE_DATA)
+	{
+		if (NdisEqualMemory(SNAP_AIRONET, pData, LENGTH_802_1_H))
+		{
+			// Cisco Aironet SNAP header
+			*Machine = AIRONET_STATE_MACHINE;
+			*MsgType = MT2_AIRONET_MSG;
+			return (TRUE);
+		}
+#ifdef LEAP_SUPPORT
+		if ( pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP ) //LEAP
+		{
+			// LEAP frames
+			*Machine = LEAP_STATE_MACHINE;
+			EAPType = *((UCHAR*)pFrame + LENGTH_802_11 + LENGTH_802_1_H + 1);
+			return (LeapMsgTypeSubst(EAPType, MsgType));
+		}
+		else
+#endif // LEAP_SUPPORT //
+		{
+			*Machine = WPA_PSK_STATE_MACHINE;
+			EAPType = *((UCHAR*)pFrame + LENGTH_802_11 + LENGTH_802_1_H + 1);
+			return(WpaMsgTypeSubst(EAPType, MsgType));
+		}
+	}
+
+	switch (pFrame->Hdr.FC.SubType)
+	{
+		case SUBTYPE_ASSOC_REQ:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_ASSOC_REQ;
+			break;
+		case SUBTYPE_ASSOC_RSP:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_ASSOC_RSP;
+			break;
+		case SUBTYPE_REASSOC_REQ:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_REASSOC_REQ;
+			break;
+		case SUBTYPE_REASSOC_RSP:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_REASSOC_RSP;
+			break;
+		case SUBTYPE_PROBE_REQ:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_PROBE_REQ;
+			break;
+		case SUBTYPE_PROBE_RSP:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_PROBE_RSP;
+			break;
+		case SUBTYPE_BEACON:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_BEACON;
+			break;
+		case SUBTYPE_ATIM:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_ATIM;
+			break;
+		case SUBTYPE_DISASSOC:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_DISASSOC_REQ;
+			break;
+		case SUBTYPE_AUTH:
+			// get the sequence number from payload 24 Mac Header + 2 bytes algorithm
+			NdisMoveMemory(&Seq, &pFrame->Octet[2], sizeof(USHORT));
+			if (Seq == 1 || Seq == 3)
+			{
+				*Machine = AUTH_RSP_STATE_MACHINE;
+				*MsgType = MT2_PEER_AUTH_ODD;
+			}
+			else if (Seq == 2 || Seq == 4)
+			{
+				*Machine = AUTH_STATE_MACHINE;
+				*MsgType = MT2_PEER_AUTH_EVEN;
+			}
+			else
+			{
+				return FALSE;
+			}
+			break;
+		case SUBTYPE_DEAUTH:
+			*Machine = AUTH_RSP_STATE_MACHINE;
+			*MsgType = MT2_PEER_DEAUTH;
+			break;
+		case SUBTYPE_ACTION:
+			*Machine = ACTION_STATE_MACHINE;
+			//  Sometimes Sta will return with category bytes with MSB = 1, if they receive catogory out of their support
+			if ((pFrame->Octet[0]&0x7F) > MAX_PEER_CATE_MSG)
+			{
+				*MsgType = MT2_ACT_INVALID;
+			}
+			else
+			{
+				*MsgType = (pFrame->Octet[0]&0x7F);
+			}
+			break;
+		default:
+			return FALSE;
+			break;
+	}
+
+	return TRUE;
+}
+#endif // CONFIG_STA_SUPPORT //
+
+// ===========================================================================================
+// state_machine.c
+// ===========================================================================================
+
+/*! \brief Initialize the state machine.
+ *	\param *S			pointer to the state machine
+ *	\param	Trans		State machine transition function
+ *	\param	StNr		number of states
+ *	\param	MsgNr		number of messages
+ *	\param	DefFunc 	default function, when there is invalid state/message combination
+ *	\param	InitState	initial state of the state machine
+ *	\param	Base		StateMachine base, internal use only
+ *	\pre p_sm should be a legal pointer
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+VOID StateMachineInit(
+	IN STATE_MACHINE *S,
+	IN STATE_MACHINE_FUNC Trans[],
+	IN ULONG StNr,
+	IN ULONG MsgNr,
+	IN STATE_MACHINE_FUNC DefFunc,
+	IN ULONG InitState,
+	IN ULONG Base)
+{
+	ULONG i, j;
+
+	// set number of states and messages
+	S->NrState = StNr;
+	S->NrMsg   = MsgNr;
+	S->Base    = Base;
+
+	S->TransFunc  = Trans;
+
+	// init all state transition to default function
+	for (i = 0; i < StNr; i++)
+	{
+		for (j = 0; j < MsgNr; j++)
+		{
+			S->TransFunc[i * MsgNr + j] = DefFunc;
+		}
+	}
+
+	// set the starting state
+	S->CurrState = InitState;
+}
+
+/*! \brief This function fills in the function pointer into the cell in the state machine
+ *	\param *S	pointer to the state machine
+ *	\param St	state
+ *	\param Msg	incoming message
+ *	\param f	the function to be executed when (state, message) combination occurs at the state machine
+ *	\pre *S should be a legal pointer to the state machine, st, msg, should be all within the range, Base should be set in the initial state
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+VOID StateMachineSetAction(
+	IN STATE_MACHINE *S,
+	IN ULONG St,
+	IN ULONG Msg,
+	IN STATE_MACHINE_FUNC Func)
+{
+	ULONG MsgIdx;
+
+	MsgIdx = Msg - S->Base;
+
+	if (St < S->NrState && MsgIdx < S->NrMsg)
+	{
+		// boundary checking before setting the action
+		S->TransFunc[St * S->NrMsg + MsgIdx] = Func;
+	}
+}
+
+/*! \brief	 This function does the state transition
+ *	\param	 *Adapter the NIC adapter pointer
+ *	\param	 *S 	  the state machine
+ *	\param	 *Elem	  the message to be executed
+ *	\return   None
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID StateMachinePerformAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN STATE_MACHINE *S,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	(*(S->TransFunc[S->CurrState * S->NrMsg + Elem->MsgType - S->Base]))(pAd, Elem);
+}
+
+/*
+	==========================================================================
+	Description:
+		The drop function, when machine executes this, the message is simply
+		ignored. This function does nothing, the message is freed in
+		StateMachinePerformAction()
+	==========================================================================
+ */
+VOID Drop(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+// ===========================================================================================
+// lfsr.c
+// ===========================================================================================
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID LfsrInit(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Seed)
+{
+	if (Seed == 0)
+		pAd->Mlme.ShiftReg = 1;
+	else
+		pAd->Mlme.ShiftReg = Seed;
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+UCHAR RandomByte(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG i;
+	UCHAR R, Result;
+
+	R = 0;
+
+	if (pAd->Mlme.ShiftReg == 0)
+	NdisGetSystemUpTime((ULONG *)&pAd->Mlme.ShiftReg);
+
+	for (i = 0; i < 8; i++)
+	{
+		if (pAd->Mlme.ShiftReg & 0x00000001)
+		{
+			pAd->Mlme.ShiftReg = ((pAd->Mlme.ShiftReg ^ LFSR_MASK) >> 1) | 0x80000000;
+			Result = 1;
+		}
+		else
+		{
+			pAd->Mlme.ShiftReg = pAd->Mlme.ShiftReg >> 1;
+			Result = 0;
+		}
+		R = (R << 1) | Result;
+	}
+
+	return R;
+}
+
+VOID AsicUpdateAutoFallBackTable(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pRateTable)
+{
+	UCHAR					i;
+	HT_FBK_CFG0_STRUC		HtCfg0;
+	HT_FBK_CFG1_STRUC		HtCfg1;
+	LG_FBK_CFG0_STRUC		LgCfg0;
+	LG_FBK_CFG1_STRUC		LgCfg1;
+	PRTMP_TX_RATE_SWITCH	pCurrTxRate, pNextTxRate;
+
+	// set to initial value
+	HtCfg0.word = 0x65432100;
+	HtCfg1.word = 0xedcba988;
+	LgCfg0.word = 0xedcba988;
+	LgCfg1.word = 0x00002100;
+
+	pNextTxRate = (PRTMP_TX_RATE_SWITCH)pRateTable+1;
+	for (i = 1; i < *((PUCHAR) pRateTable); i++)
+	{
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH)pRateTable+1+i;
+		switch (pCurrTxRate->Mode)
+		{
+			case 0:		//CCK
+				break;
+			case 1:		//OFDM
+				{
+					switch(pCurrTxRate->CurrMCS)
+					{
+						case 0:
+							LgCfg0.field.OFDMMCS0FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 1:
+							LgCfg0.field.OFDMMCS1FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 2:
+							LgCfg0.field.OFDMMCS2FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 3:
+							LgCfg0.field.OFDMMCS3FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 4:
+							LgCfg0.field.OFDMMCS4FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 5:
+							LgCfg0.field.OFDMMCS5FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 6:
+							LgCfg0.field.OFDMMCS6FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 7:
+							LgCfg0.field.OFDMMCS7FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+					}
+				}
+				break;
+#ifdef DOT11_N_SUPPORT
+			case 2:		//HT-MIX
+			case 3:		//HT-GF
+				{
+					if ((pNextTxRate->Mode >= MODE_HTMIX) && (pCurrTxRate->CurrMCS != pNextTxRate->CurrMCS))
+					{
+						switch(pCurrTxRate->CurrMCS)
+						{
+							case 0:
+								HtCfg0.field.HTMCS0FBK = pNextTxRate->CurrMCS;
+								break;
+							case 1:
+								HtCfg0.field.HTMCS1FBK = pNextTxRate->CurrMCS;
+								break;
+							case 2:
+								HtCfg0.field.HTMCS2FBK = pNextTxRate->CurrMCS;
+								break;
+							case 3:
+								HtCfg0.field.HTMCS3FBK = pNextTxRate->CurrMCS;
+								break;
+							case 4:
+								HtCfg0.field.HTMCS4FBK = pNextTxRate->CurrMCS;
+								break;
+							case 5:
+								HtCfg0.field.HTMCS5FBK = pNextTxRate->CurrMCS;
+								break;
+							case 6:
+								HtCfg0.field.HTMCS6FBK = pNextTxRate->CurrMCS;
+								break;
+							case 7:
+								HtCfg0.field.HTMCS7FBK = pNextTxRate->CurrMCS;
+								break;
+							case 8:
+								HtCfg1.field.HTMCS8FBK = pNextTxRate->CurrMCS;
+								break;
+							case 9:
+								HtCfg1.field.HTMCS9FBK = pNextTxRate->CurrMCS;
+								break;
+							case 10:
+								HtCfg1.field.HTMCS10FBK = pNextTxRate->CurrMCS;
+								break;
+							case 11:
+								HtCfg1.field.HTMCS11FBK = pNextTxRate->CurrMCS;
+								break;
+							case 12:
+								HtCfg1.field.HTMCS12FBK = pNextTxRate->CurrMCS;
+								break;
+							case 13:
+								HtCfg1.field.HTMCS13FBK = pNextTxRate->CurrMCS;
+								break;
+							case 14:
+								HtCfg1.field.HTMCS14FBK = pNextTxRate->CurrMCS;
+								break;
+							case 15:
+								HtCfg1.field.HTMCS15FBK = pNextTxRate->CurrMCS;
+								break;
+							default:
+								DBGPRINT(RT_DEBUG_ERROR, ("AsicUpdateAutoFallBackTable: not support CurrMCS=%d\n", pCurrTxRate->CurrMCS));
+						}
+					}
+				}
+				break;
+#endif // DOT11_N_SUPPORT //
+		}
+
+		pNextTxRate = pCurrTxRate;
+	}
+
+	RTMP_IO_WRITE32(pAd, HT_FBK_CFG0, HtCfg0.word);
+	RTMP_IO_WRITE32(pAd, HT_FBK_CFG1, HtCfg1.word);
+	RTMP_IO_WRITE32(pAd, LG_FBK_CFG0, LgCfg0.word);
+	RTMP_IO_WRITE32(pAd, LG_FBK_CFG1, LgCfg1.word);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set MAC register value according operation mode.
+		OperationMode AND bNonGFExist are for MM and GF Proteciton.
+		If MM or GF mask is not set, those passing argument doesn't not take effect.
+
+		Operation mode meaning:
+		= 0 : Pure HT, no preotection.
+		= 0x01; there may be non-HT devices in both the control and extension channel, protection is optional in BSS.
+		= 0x10: No Transmission in 40M is protected.
+		= 0x11: Transmission in both 40M and 20M shall be protected
+		if (bNonGFExist)
+			we should choose not to use GF. But still set correct ASIC registers.
+	========================================================================
+*/
+VOID 	AsicUpdateProtect(
+	IN		PRTMP_ADAPTER	pAd,
+	IN 		USHORT			OperationMode,
+	IN 		UCHAR			SetMask,
+	IN		BOOLEAN			bDisableBGProtect,
+	IN		BOOLEAN			bNonGFExist)
+{
+	PROT_CFG_STRUC	ProtCfg, ProtCfg4;
+	UINT32 Protect[6];
+	USHORT			offset;
+	UCHAR			i;
+	UINT32 MacReg = 0;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+#ifdef DOT11_N_SUPPORT
+	if (!(pAd->CommonCfg.bHTProtect) && (OperationMode != 8))
+	{
+		return;
+	}
+
+	if (pAd->BATable.numAsOriginator)
+	{
+		//
+		// enable the RTS/CTS to avoid channel collision
+		//
+		SetMask = ALLN_SETPROTECT;
+		OperationMode = 8;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	// Config ASIC RTS threshold register
+	RTMP_IO_READ32(pAd, TX_RTS_CFG, &MacReg);
+	MacReg &= 0xFF0000FF;
+#if 0
+	MacReg |= (pAd->CommonCfg.RtsThreshold << 8);
+#else
+	// If the user want disable RtsThreshold and enbale Amsdu/Ralink-Aggregation, set the RtsThreshold as 4096
+        if ((
+#ifdef DOT11_N_SUPPORT
+			(pAd->CommonCfg.BACapability.field.AmsduEnable) ||
+#endif // DOT11_N_SUPPORT //
+			(pAd->CommonCfg.bAggregationCapable == TRUE))
+            && pAd->CommonCfg.RtsThreshold == MAX_RTS_THRESHOLD)
+        {
+			MacReg |= (0x1000 << 8);
+        }
+        else
+        {
+			MacReg |= (pAd->CommonCfg.RtsThreshold << 8);
+        }
+#endif
+
+	RTMP_IO_WRITE32(pAd, TX_RTS_CFG, MacReg);
+
+	// Initial common protection settings
+	RTMPZeroMemory(Protect, sizeof(Protect));
+	ProtCfg4.word = 0;
+	ProtCfg.word = 0;
+	ProtCfg.field.TxopAllowGF40 = 1;
+	ProtCfg.field.TxopAllowGF20 = 1;
+	ProtCfg.field.TxopAllowMM40 = 1;
+	ProtCfg.field.TxopAllowMM20 = 1;
+	ProtCfg.field.TxopAllowOfdm = 1;
+	ProtCfg.field.TxopAllowCck = 1;
+	ProtCfg.field.RTSThEn = 1;
+	ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+
+	// update PHY mode and rate
+	if (pAd->CommonCfg.Channel > 14)
+		ProtCfg.field.ProtectRate = 0x4000;
+	ProtCfg.field.ProtectRate |= pAd->CommonCfg.RtsRate;
+
+	// Handle legacy(B/G) protection
+	if (bDisableBGProtect)
+	{
+		//ProtCfg.field.ProtectRate = pAd->CommonCfg.RtsRate;
+		ProtCfg.field.ProtectCtrl = 0;
+		Protect[0] = ProtCfg.word;
+		Protect[1] = ProtCfg.word;
+	}
+	else
+	{
+		//ProtCfg.field.ProtectRate = pAd->CommonCfg.RtsRate;
+		ProtCfg.field.ProtectCtrl = 0;			// CCK do not need to be protected
+		Protect[0] = ProtCfg.word;
+		ProtCfg.field.ProtectCtrl = ASIC_CTS;	// OFDM needs using CCK to protect
+		Protect[1] = ProtCfg.word;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	// Decide HT frame protection.
+	if ((SetMask & ALLN_SETPROTECT) != 0)
+	{
+		switch(OperationMode)
+		{
+			case 0x0:
+				// NO PROTECT
+				// 1.All STAs in the BSS are 20/40 MHz HT
+				// 2. in ai 20/40MHz BSS
+				// 3. all STAs are 20MHz in a 20MHz BSS
+				// Pure HT. no protection.
+
+				// MM20_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 010111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+				Protect[2] = 0x01744004;
+
+				// MM40_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 111111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+				Protect[3] = 0x03f44084;
+
+				// CF20_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 010111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+				Protect[4] = 0x01744004;
+
+				// CF40_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 111111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+				Protect[5] = 0x03f44084;
+
+				if (bNonGFExist)
+				{
+					// PROT_NAV(19:18)  -- 01 (Short NAV protectiion)
+					// PROT_CTRL(17:16) -- 01 (RTS/CTS)
+					Protect[4] = 0x01754004;
+					Protect[5] = 0x03f54084;
+				}
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = FALSE;
+				break;
+
+ 			case 1:
+				// This is "HT non-member protection mode."
+				// If there may be non-HT STAs my BSS
+				ProtCfg.word = 0x01744004;	// PROT_CTRL(17:16) : 0 (None)
+				ProtCfg4.word = 0x03f44084; // duplicaet legacy 24M. BW set 1.
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED))
+				{
+					ProtCfg.word = 0x01740003;	//ERP use Protection bit is set, use protection rate at Clause 18..
+					ProtCfg4.word = 0x03f40003; // Don't duplicate RTS/CTS in CCK mode. 0x03f40083;
+				}
+				//Assign Protection method for 20&40 MHz packets
+				ProtCfg.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+				ProtCfg4.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg4.field.ProtectNav = ASIC_SHORTNAV;
+				Protect[2] = ProtCfg.word;
+				Protect[3] = ProtCfg4.word;
+				Protect[4] = ProtCfg.word;
+				Protect[5] = ProtCfg4.word;
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = TRUE;
+				break;
+
+			case 2:
+				// If only HT STAs are in BSS. at least one is 20MHz. Only protect 40MHz packets
+				ProtCfg.word = 0x01744004;  // PROT_CTRL(17:16) : 0 (None)
+				ProtCfg4.word = 0x03f44084; // duplicaet legacy 24M. BW set 1.
+
+				//Assign Protection method for 40MHz packets
+				ProtCfg4.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg4.field.ProtectNav = ASIC_SHORTNAV;
+				Protect[2] = ProtCfg.word;
+				Protect[3] = ProtCfg4.word;
+				if (bNonGFExist)
+				{
+					ProtCfg.field.ProtectCtrl = ASIC_RTS;
+					ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+				}
+				Protect[4] = ProtCfg.word;
+				Protect[5] = ProtCfg4.word;
+
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = FALSE;
+				break;
+
+			case 3:
+				// HT mixed mode.	 PROTECT ALL!
+				// Assign Rate
+				ProtCfg.word = 0x01744004;	//duplicaet legacy 24M. BW set 1.
+				ProtCfg4.word = 0x03f44084;
+				// both 20MHz and 40MHz are protected. Whether use RTS or CTS-to-self depends on the
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED))
+				{
+					ProtCfg.word = 0x01740003;	//ERP use Protection bit is set, use protection rate at Clause 18..
+					ProtCfg4.word = 0x03f40003; // Don't duplicate RTS/CTS in CCK mode. 0x03f40083
+				}
+				//Assign Protection method for 20&40 MHz packets
+				ProtCfg.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+				ProtCfg4.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg4.field.ProtectNav = ASIC_SHORTNAV;
+				Protect[2] = ProtCfg.word;
+				Protect[3] = ProtCfg4.word;
+				Protect[4] = ProtCfg.word;
+				Protect[5] = ProtCfg4.word;
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = TRUE;
+				break;
+
+			case 8:
+				// Special on for Atheros problem n chip.
+				Protect[2] = 0x01754004;
+				Protect[3] = 0x03f54084;
+				Protect[4] = 0x01754004;
+				Protect[5] = 0x03f54084;
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = TRUE;
+				break;
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	offset = CCK_PROT_CFG;
+	for (i = 0;i < 6;i++)
+	{
+		if ((SetMask & (1<< i)))
+		{
+			RTMP_IO_WRITE32(pAd, offset + i*4, Protect[i]);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSwitchChannel(
+					  IN PRTMP_ADAPTER pAd,
+	IN	UCHAR			Channel,
+	IN	BOOLEAN			bScan)
+{
+	ULONG			R2 = 0, R3 = DEFAULT_RF_TX_POWER, R4 = 0;
+	CHAR    TxPwer = 0, TxPwer2 = DEFAULT_RF_TX_POWER; //Bbp94 = BBPR94_DEFAULT, TxPwer2 = DEFAULT_RF_TX_POWER;
+	UCHAR	index;
+	UINT32 	Value = 0; //BbpReg, Value;
+	RTMP_RF_REGS *RFRegTable;
+
+	// Search Tx power value
+	for (index = 0; index < pAd->ChannelListNum; index++)
+	{
+		if (Channel == pAd->ChannelList[index].Channel)
+		{
+			TxPwer = pAd->ChannelList[index].Power;
+			TxPwer2 = pAd->ChannelList[index].Power2;
+			break;
+		}
+	}
+
+	if (index == MAX_NUM_OF_CHANNELS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("AsicSwitchChannel: Cant find the Channel#%d \n", Channel));
+	}
+
+	{
+		RFRegTable = RF2850RegTable;
+
+		switch (pAd->RfIcType)
+		{
+			case RFIC_2820:
+			case RFIC_2850:
+			case RFIC_2720:
+			case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R2 = RFRegTable[index].R2;
+					if (pAd->Antenna.field.TxPath == 1)
+					{
+						R2 |= 0x4000;	// If TXpath is 1, bit 14 = 1;
+					}
+
+					if (pAd->Antenna.field.RxPath == 2)
+					{
+						R2 |= 0x40;	// write 1 to off Rxpath.
+					}
+					else if (pAd->Antenna.field.RxPath == 1)
+					{
+						R2 |= 0x20040;	// write 1 to off RxPath
+					}
+
+					if (Channel > 14)
+					{
+						// initialize R3, R4
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff);
+						R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->RfFreqOffset << 15);
+
+						// 5G band power range: 0xF9~0X0F, TX0 Reg3 bit9/TX1 Reg4 bit6="0" means the TX power reduce 7dB
+						// R3
+						if ((TxPwer >= -7) && (TxPwer < 0))
+						{
+							TxPwer = (7+TxPwer);
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10);
+							DBGPRINT(RT_DEBUG_ERROR, ("AsicSwitchChannel: TxPwer=%d \n", TxPwer));
+						}
+						else
+						{
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10) | (1 << 9);
+						}
+
+						// R4
+						if ((TxPwer2 >= -7) && (TxPwer2 < 0))
+						{
+							TxPwer2 = (7+TxPwer2);
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7);
+							DBGPRINT(RT_DEBUG_ERROR, ("AsicSwitchChannel: TxPwer2=%d \n", TxPwer2));
+						}
+						else
+						{
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7) | (1 << 6);
+						}
+					}
+					else
+					{
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff) | (TxPwer << 9); // set TX power0
+					R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->RfFreqOffset << 15) | (TxPwer2 <<6);// Set freq Offset & TxPwr1
+					}
+
+					// Based on BBP current mode before changing RF channel.
+					if (!bScan && (pAd->CommonCfg.BBPCurrentBW == BW_40))
+					{
+						R4 |=0x200000;
+					}
+
+					// Update variables
+					pAd->LatchRfRegs.Channel = Channel;
+					pAd->LatchRfRegs.R1 = RFRegTable[index].R1;
+					pAd->LatchRfRegs.R2 = R2;
+					pAd->LatchRfRegs.R3 = R3;
+					pAd->LatchRfRegs.R4 = R4;
+
+					// Set RF value 1's set R3[bit2] = [0]
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+					RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+					RTMPusecDelay(200);
+
+					// Set RF value 2's set R3[bit2] = [1]
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+					RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 | 0x04));
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+					RTMPusecDelay(200);
+
+					// Set RF value 3's set R3[bit2] = [0]
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+					RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+					break;
+				}
+			}
+			break;
+
+			default:
+			break;
+		}
+	}
+
+	// Change BBP setting during siwtch from a->g, g->a
+	if (Channel <= 14)
+	{
+	    ULONG	TxPinCfg = 0x00050F0A;//Gary 2007/08/09 0x050A0A
+
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0);//(0x44 - GET_LNA_GAIN(pAd)));	// According the Rory's suggestion to solve the middle range issue.
+		//RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x62);
+
+		// Rx High power VGA offset for LNA select
+	    if (pAd->NicConfig2.field.ExternalLNAForG)
+	    {
+	        RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x62);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x46);
+	    }
+	    else
+	    {
+	        RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x84);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x50);
+	    }
+
+		// 5G band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x04);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+		}
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+		}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+	else
+	{
+	    ULONG	TxPinCfg = 0x00050F05;//Gary 2007/8/9 0x050505
+
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0);//(0x44 - GET_LNA_GAIN(pAd)));   // According the Rory's suggestion to solve the middle range issue.
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0xF2);
+
+		// Rx High power VGA offset for LNA select
+		if (pAd->NicConfig2.field.ExternalLNAForA)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x46);
+		}
+		else
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x50);
+		}
+
+		// 5G band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x02);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+	}
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+	}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+
+    // R66 should be set according to Channel and use 20MHz when scanning
+	//RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, (0x2E + GET_LNA_GAIN(pAd)));
+	if (bScan)
+		RTMPSetAGCInitValue(pAd, BW_20);
+	else
+		RTMPSetAGCInitValue(pAd, pAd->CommonCfg.BBPCurrentBW);
+
+	//
+	// On 11A, We should delay and wait RF/BBP to be stable
+	// and the appropriate time should be 1000 micro seconds
+	// 2005/06/05 - On 11G, We also need this delay time. Otherwise it's difficult to pass the WHQL.
+	//
+	RTMPusecDelay(1000);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, Pwr0=%lu, Pwr1=%lu, %dT) to , R1=0x%08lx, R2=0x%08lx, R3=0x%08lx, R4=0x%08lx\n",
+							  Channel,
+							  pAd->RfIcType,
+							  (R3 & 0x00003e00) >> 9,
+							  (R4 & 0x000007c0) >> 6,
+							  pAd->Antenna.field.TxPath,
+							  pAd->LatchRfRegs.R1,
+							  pAd->LatchRfRegs.R2,
+							  pAd->LatchRfRegs.R3,
+							  pAd->LatchRfRegs.R4));
+}
+
+/*
+	==========================================================================
+	Description:
+		This function is required for 2421 only, and should not be used during
+		site survey. It's only required after NIC decided to stay at a channel
+		for a longer period.
+		When this function is called, it's always after AsicSwitchChannel().
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicLockChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Channel)
+{
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID	AsicAntennaSelect(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Channel)
+{
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Antenna miscellaneous setting.
+
+	Arguments:
+		pAd						Pointer to our adapter
+		BandState				Indicate current Band State.
+
+	Return Value:
+		None
+
+	IRQL <= DISPATCH_LEVEL
+
+	Note:
+		1.) Frame End type control
+			only valid for G only (RF_2527 & RF_2529)
+			0: means DPDT, set BBP R4 bit 5 to 1
+			1: means SPDT, set BBP R4 bit 5 to 0
+
+
+	========================================================================
+*/
+VOID	AsicAntennaSetting(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ABGBAND_STATE	BandState)
+{
+}
+
+VOID AsicRfTuningExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+}
+
+/*
+	==========================================================================
+	Description:
+		Gives CCK TX rate 2 more dB TX power.
+		This routine works only in LINK UP in INFRASTRUCTURE mode.
+
+		calculate desired Tx power in RF R3.Tx0~5,	should consider -
+		0. if current radio is a noisy environment (pAd->DrsCounters.fNoisyEnvironment)
+		1. TxPowerPercentage
+		2. auto calibration based on TSSI feedback
+		3. extra 2 db for CCK
+		4. -10 db upon very-short distance (AvgRSSI >= -40db) to AP
+
+	NOTE: Since this routine requires the value of (pAd->DrsCounters.fNoisyEnvironment),
+		it should be called AFTER MlmeDynamicTxRatSwitching()
+	==========================================================================
+ */
+VOID AsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT			i, j;
+	CHAR		DeltaPwr = 0;
+	BOOLEAN		bAutoTxAgc = FALSE;
+	UCHAR		TssiRef, *pTssiMinusBoundary, *pTssiPlusBoundary, TxAgcStep;
+	UCHAR		BbpR1 = 0, BbpR49 = 0, idx;
+	PCHAR		pTxAgcCompensate;
+	ULONG		TxPwr[5];
+	CHAR		Value;
+
+	if (pAd->CommonCfg.BBPCurrentBW == BW_40)
+	{
+		if (pAd->CommonCfg.CentralChannel > 14)
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgGBand[4];
+		}
+	}
+	else
+	{
+		if (pAd->CommonCfg.Channel > 14)
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgGBand[4];
+		}
+	}
+
+	// TX power compensation for temperature variation based on TSSI. try every 4 second
+	if (pAd->Mlme.OneSecPeriodicRound % 4 == 0)
+	{
+		if (pAd->CommonCfg.Channel <= 14)
+		{
+			/* bg channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			TssiRef            = pAd->TssiRefG;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryG[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryG[0];
+			TxAgcStep          = pAd->TxAgcStepG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			/* a channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			TssiRef            = pAd->TssiRefA;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryA[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryA[0];
+			TxAgcStep          = pAd->TxAgcStepA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+		{
+			/* BbpR1 is unsigned char */
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R49, &BbpR49);
+
+			/* (p) TssiPlusBoundaryG[0] = 0 = (m) TssiMinusBoundaryG[0] */
+			/* compensate: +4     +3   +2   +1    0   -1   -2   -3   -4 * steps */
+			/* step value is defined in pAd->TxAgcStepG for tx power value */
+
+			/* [4]+1+[4]   p4     p3   p2   p1   o1   m1   m2   m3   m4 */
+			/* ex:         0x00 0x15 0x25 0x45 0x88 0xA0 0xB5 0xD0 0xF0
+			   above value are examined in mass factory production */
+			/*             [4]    [3]  [2]  [1]  [0]  [1]  [2]  [3]  [4] */
+
+			/* plus (+) is 0x00 ~ 0x45, minus (-) is 0xa0 ~ 0xf0 */
+			/* if value is between p1 ~ o1 or o1 ~ s1, no need to adjust tx power */
+			/* if value is 0xa5, tx power will be -= TxAgcStep*(2-1) */
+
+			if (BbpR49 > pTssiMinusBoundary[1])
+			{
+				// Reading is larger than the reference value
+				// check for how large we need to decrease the Tx power
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 <= pTssiMinusBoundary[idx])  // Found the range
+						break;
+				}
+				// The index is the step we should decrease, idx = 0 means there is nothing to compensate
+				*pTxAgcCompensate = -(TxAgcStep * (idx-1));
+
+				DeltaPwr += (*pTxAgcCompensate);
+				DBGPRINT(RT_DEBUG_TRACE, ("-- Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = -%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else if (BbpR49 < pTssiPlusBoundary[1])
+			{
+				// Reading is smaller than the reference value
+				// check for how large we need to increase the Tx power
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 >= pTssiPlusBoundary[idx])   // Found the range
+						break;
+				}
+				// The index is the step we should increase, idx = 0 means there is nothing to compensate
+				*pTxAgcCompensate = TxAgcStep * (idx-1);
+				DeltaPwr += (*pTxAgcCompensate);
+				DBGPRINT(RT_DEBUG_TRACE, ("++ Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else
+			{
+				*pTxAgcCompensate = 0;
+				DBGPRINT(RT_DEBUG_TRACE, ("   Tx Power, BBP R49=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, 0));
+			}
+		}
+	}
+	else
+	{
+		if (pAd->CommonCfg.Channel <= 14)
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+			DeltaPwr += (*pTxAgcCompensate);
+	}
+
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpR1);
+	BbpR1 &= 0xFC;
+
+#ifdef SINGLE_SKU
+	// Handle regulatory max tx power constrain
+	do
+	{
+		UCHAR    TxPwrInEEPROM = 0xFF, CountryTxPwr = 0xFF, criterion;
+		UCHAR    AdjustMaxTxPwr[40];
+
+		if (pAd->CommonCfg.Channel > 14) // 5G band
+			TxPwrInEEPROM = ((pAd->CommonCfg.DefineMaxTxPwr & 0xFF00) >> 8);
+		else // 2.4G band
+			TxPwrInEEPROM = (pAd->CommonCfg.DefineMaxTxPwr & 0x00FF);
+		CountryTxPwr = GetCuntryMaxTxPwr(pAd, pAd->CommonCfg.Channel);
+
+		// error handling, range check
+		if ((TxPwrInEEPROM > 0x50) || (CountryTxPwr > 0x50))
+		{
+			DBGPRINT(RT_DEBUG_ERROR,("AsicAdjustTxPower - Invalid max tx power (=0x%02x), CountryTxPwr=%d\n", TxPwrInEEPROM, CountryTxPwr));
+			break;
+		}
+
+		criterion = *((PUCHAR)TxPwr + 2) & 0xF;        // FAE use OFDM 6M as criterion
+
+		DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (criterion=%d, TxPwrInEEPROM=%d, CountryTxPwr=%d)\n", criterion, TxPwrInEEPROM, CountryTxPwr));
+
+		// Adjust max tx power according to the relationship of tx power in E2PROM
+		for (i=0; i<5; i++)
+		{
+			// CCK will have 4dBm larger than OFDM
+			// Therefore, we should separate to parse the tx power field
+			if (i == 0)
+			{
+				for (j=0; j<8; j++)
+				{
+					Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F);
+
+					if (j < 4)
+					{
+						// CCK will have 4dBm larger than OFDM
+						AdjustMaxTxPwr[i*8+j] = TxPwrInEEPROM + (Value - criterion) + 4;
+					}
+					else
+					{
+						AdjustMaxTxPwr[i*8+j] = TxPwrInEEPROM + (Value - criterion);
+					}
+					DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+				}
+			}
+			else
+			{
+				for (j=0; j<8; j++)
+				{
+					Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F);
+
+					AdjustMaxTxPwr[i*8+j] = TxPwrInEEPROM + (Value - criterion);
+					DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+				}
+			}
+		}
+
+		// Adjust tx power according to the relationship
+		for (i=0; i<5; i++)
+		{
+			if (TxPwr[i] != 0xffffffff)
+			{
+				for (j=0; j<8; j++)
+				{
+					Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F);
+
+					// The system tx power is larger than the regulatory, the power should be restrain
+					if (AdjustMaxTxPwr[i*8+j] > CountryTxPwr)
+					{
+						// decrease to zero and don't need to take care BBPR1
+						if ((Value - (AdjustMaxTxPwr[i*8+j] - CountryTxPwr)) > 0)
+							Value -= (AdjustMaxTxPwr[i*8+j] - CountryTxPwr);
+						else
+							Value = 0;
+
+						DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+					}
+					else
+						DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d, no change)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+
+						TxPwr[i] = (TxPwr[i] & ~(0x0000000F << j*4)) | (Value << j*4);
+				}
+			}
+		}
+	} while (FALSE);
+#endif // SINGLE_SKU //
+
+	/* calculate delta power based on the percentage specified from UI */
+	// E2PROM setting is calibrated for maximum TX power (i.e. 100%)
+	// We lower TX power here according to the percentage specified from UI
+	if (pAd->CommonCfg.TxPowerPercentage == 0xffffffff)       // AUTO TX POWER control
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 90)  // 91 ~ 100% & AUTO, treat as 100% in terms of mW
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 60)  // 61 ~ 90%, treat as 75% in terms of mW		// DeltaPwr -= 1;
+	{
+		DeltaPwr -= 1;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 30)  // 31 ~ 60%, treat as 50% in terms of mW		// DeltaPwr -= 3;
+	{
+		DeltaPwr -= 3;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 15)  // 16 ~ 30%, treat as 25% in terms of mW		// DeltaPwr -= 6;
+	{
+		BbpR1 |= 0x01;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 9)   // 10 ~ 15%, treat as 12.5% in terms of mW		// DeltaPwr -= 9;
+	{
+		BbpR1 |= 0x01;
+		DeltaPwr -= 3;
+	}
+	else                                           // 0 ~ 9 %, treat as MIN(~3%) in terms of mW		// DeltaPwr -= 12;
+	{
+		BbpR1 |= 0x02;
+	}
+
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpR1);
+
+	/* reset different new tx power for different TX rate */
+	for(i=0; i<5; i++)
+	{
+		if (TxPwr[i] != 0xffffffff)
+		{
+			for (j=0; j<8; j++)
+			{
+				Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F); /* 0 ~ 15 */
+
+				if ((Value + DeltaPwr) < 0)
+				{
+					Value = 0; /* min */
+				}
+				else if ((Value + DeltaPwr) > 0xF)
+				{
+					Value = 0xF; /* max */
+				}
+				else
+				{
+					Value += DeltaPwr; /* temperature compensation */
+				}
+
+				/* fill new value to CSR offset */
+				TxPwr[i] = (TxPwr[i] & ~(0x0000000F << j*4)) | (Value << j*4);
+			}
+
+			/* write tx power value to CSR */
+			/* TX_PWR_CFG_0 (8 tx rate) for	TX power for OFDM 12M/18M
+											TX power for OFDM 6M/9M
+											TX power for CCK5.5M/11M
+											TX power for CCK1M/2M */
+			/* TX_PWR_CFG_1 ~ TX_PWR_CFG_4 */
+			RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, TxPwr[i]);
+		}
+	}
+
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	==========================================================================
+	Description:
+		put PHY to sleep here, and set next wakeup timer. PHY doesn't not wakeup
+		automatically. Instead, MCU will issue a TwakeUpInterrupt to host after
+		the wakeup timer timeout. Driver has to issue a separate command to wake
+		PHY up.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSleepThenAutoWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TbttNumToNextWakeUp)
+{
+    RT28XX_STA_SLEEP_THEN_AUTO_WAKEUP(pAd, TbttNumToNextWakeUp);
+}
+
+/*
+	==========================================================================
+	Description:
+		AsicForceWakeup() is used whenever manual wakeup is required
+		AsicForceSleep() should only be used when not in INFRA BSS. When
+		in INFRA BSS, we should use AsicSleepThenAutoWakeup() instead.
+	==========================================================================
+ */
+VOID AsicForceSleep(
+	IN PRTMP_ADAPTER pAd)
+{
+
+}
+
+/*
+	==========================================================================
+	Description:
+		AsicForceWakeup() is used whenever Twakeup timer (set via AsicSleepThenAutoWakeup)
+		expired.
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+	==========================================================================
+ */
+VOID AsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN    bFromTx)
+{
+    DBGPRINT(RT_DEBUG_TRACE, ("--> AsicForceWakeup \n"));
+    RT28XX_STA_FORCE_WAKEUP(pAd, bFromTx);
+}
+#endif // CONFIG_STA_SUPPORT //
+/*
+	==========================================================================
+	Description:
+		Set My BSSID
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSetBssid(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pBssid)
+{
+	ULONG		  Addr4;
+	DBGPRINT(RT_DEBUG_TRACE, ("==============> AsicSetBssid %x:%x:%x:%x:%x:%x\n",
+		pBssid[0],pBssid[1],pBssid[2],pBssid[3], pBssid[4],pBssid[5]));
+
+	Addr4 = (ULONG)(pBssid[0])		 |
+			(ULONG)(pBssid[1] << 8)  |
+			(ULONG)(pBssid[2] << 16) |
+			(ULONG)(pBssid[3] << 24);
+	RTMP_IO_WRITE32(pAd, MAC_BSSID_DW0, Addr4);
+
+	Addr4 = 0;
+	// always one BSSID in STA mode
+	Addr4 = (ULONG)(pBssid[4]) | (ULONG)(pBssid[5] << 8);
+
+	RTMP_IO_WRITE32(pAd, MAC_BSSID_DW1, Addr4);
+}
+
+VOID AsicSetMcastWC(
+	IN PRTMP_ADAPTER pAd)
+{
+	MAC_TABLE_ENTRY *pEntry = &pAd->MacTab.Content[MCAST_WCID];
+	USHORT		offset;
+
+	pEntry->Sst        = SST_ASSOC;
+	pEntry->Aid        = MCAST_WCID;	// Softap supports 1 BSSID and use WCID=0 as multicast Wcid index
+	pEntry->PsMode     = PWR_ACTIVE;
+	pEntry->CurrTxRate = pAd->CommonCfg.MlmeRate;
+	offset = MAC_WCID_BASE + BSS0Mcast_WCID * HW_WCID_ENTRY_SIZE;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicDelWcidTab(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	Wcid)
+{
+	ULONG		  Addr0 = 0x0, Addr1 = 0x0;
+	ULONG 		offset;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicDelWcidTab==>Wcid = 0x%x\n",Wcid));
+	offset = MAC_WCID_BASE + Wcid * HW_WCID_ENTRY_SIZE;
+	RTMP_IO_WRITE32(pAd, offset, Addr0);
+	offset += 4;
+	RTMP_IO_WRITE32(pAd, offset, Addr1);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicEnableRDG(
+	IN PRTMP_ADAPTER pAd)
+{
+	TX_LINK_CFG_STRUC	TxLinkCfg;
+	UINT32				Data = 0;
+
+	RTMP_IO_READ32(pAd, TX_LINK_CFG, &TxLinkCfg.word);
+	TxLinkCfg.field.TxRDGEn = 1;
+	RTMP_IO_WRITE32(pAd, TX_LINK_CFG, TxLinkCfg.word);
+
+	RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+	Data  &= 0xFFFFFF00;
+	Data  |= 0x80;
+	RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+
+	//OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicDisableRDG(
+	IN PRTMP_ADAPTER pAd)
+{
+	TX_LINK_CFG_STRUC	TxLinkCfg;
+	UINT32				Data = 0;
+
+
+	RTMP_IO_READ32(pAd, TX_LINK_CFG, &TxLinkCfg.word);
+	TxLinkCfg.field.TxRDGEn = 0;
+	RTMP_IO_WRITE32(pAd, TX_LINK_CFG, TxLinkCfg.word);
+
+	RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+
+	Data  &= 0xFFFFFF00;
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_DYNAMIC_BE_TXOP_ACTIVE)
+#ifdef DOT11_N_SUPPORT
+		&& (pAd->MacTab.fAnyStationMIMOPSDynamic == FALSE)
+#endif // DOT11_N_SUPPORT //
+	)
+	{
+		// For CWC test, change txop from 0x30 to 0x20 in TxBurst mode
+		if (pAd->CommonCfg.bEnableTxBurst)
+			Data |= 0x20;
+	}
+	RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicDisableSync(
+	IN PRTMP_ADAPTER pAd)
+{
+	BCN_TIME_CFG_STRUC csr;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->Disable TSF synchronization\n"));
+
+	// 2003-12-20 disable TSF and TBTT while NIC in power-saving have side effect
+	//			  that NIC will never wakes up because TSF stops and no more
+	//			  TBTT interrupts
+	pAd->TbttTickCount = 0;
+	RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+	csr.field.bBeaconGen = 0;
+	csr.field.bTBTTEnable = 0;
+	csr.field.TsfSyncMode = 0;
+	csr.field.bTsfTicking = 0;
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicEnableBssSync(
+	IN PRTMP_ADAPTER pAd)
+{
+	BCN_TIME_CFG_STRUC csr;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->AsicEnableBssSync(INFRA mode)\n"));
+
+	RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		csr.field.BeaconInterval = pAd->CommonCfg.BeaconPeriod << 4; // ASIC register in units of 1/16 TU
+		csr.field.bTsfTicking = 1;
+		csr.field.TsfSyncMode = 1; // sync TSF in INFRASTRUCTURE mode
+		csr.field.bBeaconGen  = 0; // do NOT generate BEACON
+		csr.field.bTBTTEnable = 1;
+	}
+#endif // CONFIG_STA_SUPPORT //
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+}
+
+/*
+	==========================================================================
+	Description:
+	Note:
+		BEACON frame in shared memory should be built ok before this routine
+		can be called. Otherwise, a garbage frame maybe transmitted out every
+		Beacon period.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicEnableIbssSync(
+	IN PRTMP_ADAPTER pAd)
+{
+	BCN_TIME_CFG_STRUC csr9;
+	PUCHAR			ptr;
+	UINT i;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->AsicEnableIbssSync(ADHOC mode. MPDUtotalByteCount = %d)\n", pAd->BeaconTxWI.MPDUtotalByteCount));
+
+	RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr9.word);
+	csr9.field.bBeaconGen = 0;
+	csr9.field.bTBTTEnable = 0;
+	csr9.field.bTsfTicking = 0;
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr9.word);
+
+#ifdef RT2860
+	// move BEACON TXD and frame content to on-chip memory
+	ptr = (PUCHAR)&pAd->BeaconTxWI;
+	for (i=0; i<TXWI_SIZE; i+=4)  // 16-byte TXWI field
+	{
+		UINT32 longptr =  *ptr + (*(ptr+1)<<8) + (*(ptr+2)<<16) + (*(ptr+3)<<24);
+		RTMP_IO_WRITE32(pAd, HW_BEACON_BASE0 + i, longptr);
+		ptr += 4;
+	}
+
+	// start right after the 16-byte TXWI field
+	ptr = pAd->BeaconBuf;
+	for (i=0; i< pAd->BeaconTxWI.MPDUtotalByteCount; i+=4)
+	{
+		UINT32 longptr =  *ptr + (*(ptr+1)<<8) + (*(ptr+2)<<16) + (*(ptr+3)<<24);
+		RTMP_IO_WRITE32(pAd, HW_BEACON_BASE0 + TXWI_SIZE + i, longptr);
+		ptr +=4;
+	}
+#endif // RT2860 //
+
+	// start sending BEACON
+	csr9.field.BeaconInterval = pAd->CommonCfg.BeaconPeriod << 4; // ASIC register in units of 1/16 TU
+	csr9.field.bTsfTicking = 1;
+	csr9.field.TsfSyncMode = 2; // sync TSF in IBSS mode
+	csr9.field.bTBTTEnable = 1;
+	csr9.field.bBeaconGen = 1;
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr9.word);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSetEdcaParm(
+	IN PRTMP_ADAPTER pAd,
+	IN PEDCA_PARM	 pEdcaParm)
+{
+	EDCA_AC_CFG_STRUC   Ac0Cfg, Ac1Cfg, Ac2Cfg, Ac3Cfg;
+	AC_TXOP_CSR0_STRUC csr0;
+	AC_TXOP_CSR1_STRUC csr1;
+	AIFSN_CSR_STRUC    AifsnCsr;
+	CWMIN_CSR_STRUC    CwminCsr;
+	CWMAX_CSR_STRUC    CwmaxCsr;
+	int i;
+
+	Ac0Cfg.word = 0;
+	Ac1Cfg.word = 0;
+	Ac2Cfg.word = 0;
+	Ac3Cfg.word = 0;
+	if ((pEdcaParm == NULL) || (pEdcaParm->bValid == FALSE))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("AsicSetEdcaParm\n"));
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_WMM_INUSED);
+		for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+		{
+			if (pAd->MacTab.Content[i].ValidAsCLI || pAd->MacTab.Content[i].ValidAsApCli)
+				CLIENT_STATUS_CLEAR_FLAG(&pAd->MacTab.Content[i], fCLIENT_STATUS_WMM_CAPABLE);
+		}
+
+		//========================================================
+		//      MAC Register has a copy .
+		//========================================================
+		if( pAd->CommonCfg.bEnableTxBurst )
+		{
+			// For CWC test, change txop from 0x30 to 0x20 in TxBurst mode
+			Ac0Cfg.field.AcTxop = 0x20; // Suggest by John for TxBurst in HT Mode
+		}
+		else
+			Ac0Cfg.field.AcTxop = 0;	// QID_AC_BE
+		Ac0Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac0Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac0Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Ac0Cfg.word);
+
+		Ac1Cfg.field.AcTxop = 0;	// QID_AC_BK
+		Ac1Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac1Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac1Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC1_CFG, Ac1Cfg.word);
+
+		if (pAd->CommonCfg.PhyMode == PHY_11B)
+		{
+			Ac2Cfg.field.AcTxop = 192;	// AC_VI: 192*32us ~= 6ms
+			Ac3Cfg.field.AcTxop = 96;	// AC_VO: 96*32us  ~= 3ms
+		}
+		else
+		{
+			Ac2Cfg.field.AcTxop = 96;	// AC_VI: 96*32us ~= 3ms
+			Ac3Cfg.field.AcTxop = 48;	// AC_VO: 48*32us ~= 1.5ms
+		}
+		Ac2Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac2Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac2Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC2_CFG, Ac2Cfg.word);
+		Ac3Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac3Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac3Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC3_CFG, Ac3Cfg.word);
+
+		//========================================================
+		//      DMA Register has a copy too.
+		//========================================================
+		csr0.field.Ac0Txop = 0;		// QID_AC_BE
+		csr0.field.Ac1Txop = 0;		// QID_AC_BK
+		RTMP_IO_WRITE32(pAd, WMM_TXOP0_CFG, csr0.word);
+		if (pAd->CommonCfg.PhyMode == PHY_11B)
+		{
+			csr1.field.Ac2Txop = 192;		// AC_VI: 192*32us ~= 6ms
+			csr1.field.Ac3Txop = 96;		// AC_VO: 96*32us  ~= 3ms
+		}
+		else
+		{
+			csr1.field.Ac2Txop = 96;		// AC_VI: 96*32us ~= 3ms
+			csr1.field.Ac3Txop = 48;		// AC_VO: 48*32us ~= 1.5ms
+		}
+		RTMP_IO_WRITE32(pAd, WMM_TXOP1_CFG, csr1.word);
+
+		CwminCsr.word = 0;
+		CwminCsr.field.Cwmin0 = CW_MIN_IN_BITS;
+		CwminCsr.field.Cwmin1 = CW_MIN_IN_BITS;
+		CwminCsr.field.Cwmin2 = CW_MIN_IN_BITS;
+		CwminCsr.field.Cwmin3 = CW_MIN_IN_BITS;
+		RTMP_IO_WRITE32(pAd, WMM_CWMIN_CFG, CwminCsr.word);
+
+		CwmaxCsr.word = 0;
+		CwmaxCsr.field.Cwmax0 = CW_MAX_IN_BITS;
+		CwmaxCsr.field.Cwmax1 = CW_MAX_IN_BITS;
+		CwmaxCsr.field.Cwmax2 = CW_MAX_IN_BITS;
+		CwmaxCsr.field.Cwmax3 = CW_MAX_IN_BITS;
+		RTMP_IO_WRITE32(pAd, WMM_CWMAX_CFG, CwmaxCsr.word);
+
+		RTMP_IO_WRITE32(pAd, WMM_AIFSN_CFG, 0x00002222);
+
+		NdisZeroMemory(&pAd->CommonCfg.APEdcaParm, sizeof(EDCA_PARM));
+	}
+	else
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_WMM_INUSED);
+		//========================================================
+		//      MAC Register has a copy.
+		//========================================================
+		//
+		// Modify Cwmin/Cwmax/Txop on queue[QID_AC_VI], Recommend by Jerry 2005/07/27
+		// To degrade our VIDO Queue's throughput for WiFi WMM S3T07 Issue.
+		//
+		//pEdcaParm->Txop[QID_AC_VI] = pEdcaParm->Txop[QID_AC_VI] * 7 / 10; // rt2860c need this
+
+		Ac0Cfg.field.AcTxop =  pEdcaParm->Txop[QID_AC_BE];
+		Ac0Cfg.field.Cwmin= pEdcaParm->Cwmin[QID_AC_BE];
+		Ac0Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_BE];
+		Ac0Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_BE]; //+1;
+
+		Ac1Cfg.field.AcTxop =  pEdcaParm->Txop[QID_AC_BK];
+		Ac1Cfg.field.Cwmin = pEdcaParm->Cwmin[QID_AC_BK]; //+2;
+		Ac1Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_BK];
+		Ac1Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_BK]; //+1;
+
+		Ac2Cfg.field.AcTxop = (pEdcaParm->Txop[QID_AC_VI] * 6) / 10;
+		Ac2Cfg.field.Cwmin = pEdcaParm->Cwmin[QID_AC_VI];
+		Ac2Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_VI];
+		Ac2Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_VI];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			// Tuning for Wi-Fi WMM S06
+			if (pAd->CommonCfg.bWiFiTest &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+				Ac2Cfg.field.Aifsn -= 1;
+
+			// Tuning for TGn Wi-Fi 5.2.32
+			// STA TestBed changes in this item: conexant legacy sta ==> broadcom 11n sta
+			if (STA_TGN_WIFI_ON(pAd) &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+			{
+				Ac0Cfg.field.Aifsn = 3;
+				Ac2Cfg.field.AcTxop = 5;
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		Ac3Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_VO];
+		Ac3Cfg.field.Cwmin = pEdcaParm->Cwmin[QID_AC_VO];
+		Ac3Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_VO];
+		Ac3Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_VO];
+
+//#ifdef WIFI_TEST
+		if (pAd->CommonCfg.bWiFiTest)
+		{
+			if (Ac3Cfg.field.AcTxop == 102)
+			{
+			Ac0Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_BE] ? pEdcaParm->Txop[QID_AC_BE] : 10;
+				Ac0Cfg.field.Aifsn  = pEdcaParm->Aifsn[QID_AC_BE]-1; /* AIFSN must >= 1 */
+			Ac1Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_BK];
+				Ac1Cfg.field.Aifsn  = pEdcaParm->Aifsn[QID_AC_BK];
+			Ac2Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_VI];
+			} /* End of if */
+		}
+//#endif // WIFI_TEST //
+
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Ac0Cfg.word);
+		RTMP_IO_WRITE32(pAd, EDCA_AC1_CFG, Ac1Cfg.word);
+		RTMP_IO_WRITE32(pAd, EDCA_AC2_CFG, Ac2Cfg.word);
+		RTMP_IO_WRITE32(pAd, EDCA_AC3_CFG, Ac3Cfg.word);
+
+
+		//========================================================
+		//      DMA Register has a copy too.
+		//========================================================
+		csr0.field.Ac0Txop = Ac0Cfg.field.AcTxop;
+		csr0.field.Ac1Txop = Ac1Cfg.field.AcTxop;
+		RTMP_IO_WRITE32(pAd, WMM_TXOP0_CFG, csr0.word);
+
+		csr1.field.Ac2Txop = Ac2Cfg.field.AcTxop;
+		csr1.field.Ac3Txop = Ac3Cfg.field.AcTxop;
+		RTMP_IO_WRITE32(pAd, WMM_TXOP1_CFG, csr1.word);
+
+		CwminCsr.word = 0;
+		CwminCsr.field.Cwmin0 = pEdcaParm->Cwmin[QID_AC_BE];
+		CwminCsr.field.Cwmin1 = pEdcaParm->Cwmin[QID_AC_BK];
+		CwminCsr.field.Cwmin2 = pEdcaParm->Cwmin[QID_AC_VI];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			CwminCsr.field.Cwmin3 = pEdcaParm->Cwmin[QID_AC_VO] - 1; //for TGn wifi test
+#endif // CONFIG_STA_SUPPORT //
+		RTMP_IO_WRITE32(pAd, WMM_CWMIN_CFG, CwminCsr.word);
+
+		CwmaxCsr.word = 0;
+		CwmaxCsr.field.Cwmax0 = pEdcaParm->Cwmax[QID_AC_BE];
+		CwmaxCsr.field.Cwmax1 = pEdcaParm->Cwmax[QID_AC_BK];
+		CwmaxCsr.field.Cwmax2 = pEdcaParm->Cwmax[QID_AC_VI];
+		CwmaxCsr.field.Cwmax3 = pEdcaParm->Cwmax[QID_AC_VO];
+		RTMP_IO_WRITE32(pAd, WMM_CWMAX_CFG, CwmaxCsr.word);
+
+		AifsnCsr.word = 0;
+		AifsnCsr.field.Aifsn0 = Ac0Cfg.field.Aifsn; //pEdcaParm->Aifsn[QID_AC_BE];
+		AifsnCsr.field.Aifsn1 = Ac1Cfg.field.Aifsn; //pEdcaParm->Aifsn[QID_AC_BK];
+		AifsnCsr.field.Aifsn2 = Ac2Cfg.field.Aifsn; //pEdcaParm->Aifsn[QID_AC_VI];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			// Tuning for Wi-Fi WMM S06
+			if (pAd->CommonCfg.bWiFiTest &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+				AifsnCsr.field.Aifsn2 = Ac2Cfg.field.Aifsn - 4;
+
+			// Tuning for TGn Wi-Fi 5.2.32
+			// STA TestBed changes in this item: conexant legacy sta ==> broadcom 11n sta
+			if (STA_TGN_WIFI_ON(pAd) &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+			{
+				AifsnCsr.field.Aifsn0 = 3;
+				AifsnCsr.field.Aifsn2 = 7;
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			AifsnCsr.field.Aifsn3 = Ac3Cfg.field.Aifsn - 1; //pEdcaParm->Aifsn[QID_AC_VO]; //for TGn wifi test
+#endif // CONFIG_STA_SUPPORT //
+		RTMP_IO_WRITE32(pAd, WMM_AIFSN_CFG, AifsnCsr.word);
+
+		NdisMoveMemory(&pAd->CommonCfg.APEdcaParm, pEdcaParm, sizeof(EDCA_PARM));
+		if (!ADHOC_ON(pAd))
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("EDCA [#%d]: AIFSN CWmin CWmax  TXOP(us)  ACM\n", pEdcaParm->EdcaUpdateCount));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_BE      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[0],
+									 pEdcaParm->Cwmin[0],
+									 pEdcaParm->Cwmax[0],
+									 pEdcaParm->Txop[0]<<5,
+									 pEdcaParm->bACM[0]));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_BK      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[1],
+									 pEdcaParm->Cwmin[1],
+									 pEdcaParm->Cwmax[1],
+									 pEdcaParm->Txop[1]<<5,
+									 pEdcaParm->bACM[1]));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_VI      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[2],
+									 pEdcaParm->Cwmin[2],
+									 pEdcaParm->Cwmax[2],
+									 pEdcaParm->Txop[2]<<5,
+									 pEdcaParm->bACM[2]));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_VO      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[3],
+									 pEdcaParm->Cwmin[3],
+									 pEdcaParm->Cwmax[3],
+									 pEdcaParm->Txop[3]<<5,
+									 pEdcaParm->bACM[3]));
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID 	AsicSetSlotTime(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN bUseShortSlotTime)
+{
+	ULONG	SlotTime;
+	UINT32	RegValue = 0;
+
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->CommonCfg.Channel > 14)
+		bUseShortSlotTime = TRUE;
+#endif // CONFIG_STA_SUPPORT //
+
+	if (bUseShortSlotTime)
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_SHORT_SLOT_INUSED);
+	else
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_SLOT_INUSED);
+
+	SlotTime = (bUseShortSlotTime)? 9 : 20;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// force using short SLOT time for FAE to demo performance when TxBurst is ON
+		if (((pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED)))
+#ifdef DOT11_N_SUPPORT
+			|| ((pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE) && (pAd->CommonCfg.BACapability.field.Policy == BA_NOTUSE))
+#endif // DOT11_N_SUPPORT //
+			)
+		{
+			// In this case, we will think it is doing Wi-Fi test
+			// And we will not set to short slot when bEnableTxBurst is TRUE.
+		}
+		else if (pAd->CommonCfg.bEnableTxBurst)
+			SlotTime = 9;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	//
+	// For some reasons, always set it to short slot time.
+	//
+	// ToDo: Should consider capability with 11B
+	//
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->StaCfg.BssType == BSS_ADHOC)
+			SlotTime = 20;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_IO_READ32(pAd, BKOFF_SLOT_CFG, &RegValue);
+	RegValue = RegValue & 0xFFFFFF00;
+
+	RegValue |= SlotTime;
+
+	RTMP_IO_WRITE32(pAd, BKOFF_SLOT_CFG, RegValue);
+}
+
+/*
+	========================================================================
+	Description:
+		Add Shared key information into ASIC.
+		Update shared key, TxMic and RxMic to Asic Shared key table
+		Update its cipherAlg to Asic Shared key Mode.
+
+    Return:
+	========================================================================
+*/
+VOID AsicAddSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 BssIndex,
+	IN UCHAR		 KeyIdx,
+	IN UCHAR		 CipherAlg,
+	IN PUCHAR		 pKey,
+	IN PUCHAR		 pTxMic,
+	IN PUCHAR		 pRxMic)
+{
+	ULONG offset; //, csr0;
+	SHAREDKEY_MODE_STRUC csr1;
+#ifdef RT2860
+	INT   i;
+#endif // RT2860 //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicAddSharedKeyEntry BssIndex=%d, KeyIdx=%d\n", BssIndex,KeyIdx));
+//============================================================================================
+
+	DBGPRINT(RT_DEBUG_TRACE,("AsicAddSharedKeyEntry: %s key #%d\n", CipherName[CipherAlg], BssIndex*4 + KeyIdx));
+	DBGPRINT_RAW(RT_DEBUG_TRACE, (" 	Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pKey[0],pKey[1],pKey[2],pKey[3],pKey[4],pKey[5],pKey[6],pKey[7],pKey[8],pKey[9],pKey[10],pKey[11],pKey[12],pKey[13],pKey[14],pKey[15]));
+	if (pRxMic)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, (" 	Rx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pRxMic[0],pRxMic[1],pRxMic[2],pRxMic[3],pRxMic[4],pRxMic[5],pRxMic[6],pRxMic[7]));
+	}
+	if (pTxMic)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, (" 	Tx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pTxMic[0],pTxMic[1],pTxMic[2],pTxMic[3],pTxMic[4],pTxMic[5],pTxMic[6],pTxMic[7]));
+	}
+//============================================================================================
+	//
+	// fill key material - key + TX MIC + RX MIC
+	//
+#ifdef RT2860
+	offset = SHARED_KEY_TABLE_BASE + (4*BssIndex + KeyIdx)*HW_KEY_ENTRY_SIZE;
+	for (i=0; i<MAX_LEN_OF_SHARE_KEY; i++)
+	{
+		RTMP_IO_WRITE8(pAd, offset + i, pKey[i]);
+	}
+
+	offset += MAX_LEN_OF_SHARE_KEY;
+	if (pTxMic)
+	{
+		for (i=0; i<8; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset + i, pTxMic[i]);
+		}
+	}
+
+	offset += 8;
+	if (pRxMic)
+	{
+		for (i=0; i<8; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset + i, pRxMic[i]);
+		}
+	}
+#endif // RT2860 //
+
+
+	//
+	// Update cipher algorithm. WSTA always use BSS0
+	//
+	RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), &csr1.word);
+	DBGPRINT(RT_DEBUG_TRACE,("Read: SHARED_KEY_MODE_BASE at this Bss[%d] KeyIdx[%d]= 0x%x \n", BssIndex,KeyIdx, csr1.word));
+	if ((BssIndex%2) == 0)
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss0Key0CipherAlg = CipherAlg;
+		else if (KeyIdx == 1)
+			csr1.field.Bss0Key1CipherAlg = CipherAlg;
+		else if (KeyIdx == 2)
+			csr1.field.Bss0Key2CipherAlg = CipherAlg;
+		else
+			csr1.field.Bss0Key3CipherAlg = CipherAlg;
+	}
+	else
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss1Key0CipherAlg = CipherAlg;
+		else if (KeyIdx == 1)
+			csr1.field.Bss1Key1CipherAlg = CipherAlg;
+		else if (KeyIdx == 2)
+			csr1.field.Bss1Key2CipherAlg = CipherAlg;
+		else
+			csr1.field.Bss1Key3CipherAlg = CipherAlg;
+	}
+	DBGPRINT(RT_DEBUG_TRACE,("Write: SHARED_KEY_MODE_BASE at this Bss[%d] = 0x%x \n", BssIndex, csr1.word));
+	RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), csr1.word);
+
+}
+
+//	IRQL = DISPATCH_LEVEL
+VOID AsicRemoveSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 BssIndex,
+	IN UCHAR		 KeyIdx)
+{
+	//ULONG SecCsr0;
+	SHAREDKEY_MODE_STRUC csr1;
+
+	DBGPRINT(RT_DEBUG_TRACE,("AsicRemoveSharedKeyEntry: #%d \n", BssIndex*4 + KeyIdx));
+
+	RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), &csr1.word);
+	if ((BssIndex%2) == 0)
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss0Key0CipherAlg = 0;
+		else if (KeyIdx == 1)
+			csr1.field.Bss0Key1CipherAlg = 0;
+		else if (KeyIdx == 2)
+			csr1.field.Bss0Key2CipherAlg = 0;
+		else
+			csr1.field.Bss0Key3CipherAlg = 0;
+	}
+	else
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss1Key0CipherAlg = 0;
+		else if (KeyIdx == 1)
+			csr1.field.Bss1Key1CipherAlg = 0;
+		else if (KeyIdx == 2)
+			csr1.field.Bss1Key2CipherAlg = 0;
+		else
+			csr1.field.Bss1Key3CipherAlg = 0;
+	}
+	DBGPRINT(RT_DEBUG_TRACE,("Write: SHARED_KEY_MODE_BASE at this Bss[%d] = 0x%x \n", BssIndex, csr1.word));
+	RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), csr1.word);
+	ASSERT(BssIndex < 4);
+	ASSERT(KeyIdx < 4);
+
+}
+
+
+VOID AsicUpdateWCIDAttribute(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR        CipherAlg,
+	IN BOOLEAN		bUsePairewiseKeyTable)
+{
+	ULONG   WCIDAttri = 0, offset;
+
+	//
+	// Update WCID attribute.
+	// Only TxKey could update WCID attribute.
+	//
+	offset = MAC_WCID_ATTRIBUTE_BASE + (WCID * HW_WCID_ATTRI_SIZE);
+	WCIDAttri = (BssIndex << 4) | (CipherAlg << 1) | (bUsePairewiseKeyTable);
+	RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+}
+
+VOID AsicUpdateWCIDIVEIV(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN ULONG        uIV,
+	IN ULONG        uEIV)
+{
+	ULONG	offset;
+
+	offset = MAC_IVEIV_TABLE_BASE + (WCID * HW_IVEIV_ENTRY_SIZE);
+
+	RTMP_IO_WRITE32(pAd, offset, uIV);
+	RTMP_IO_WRITE32(pAd, offset + 4, uEIV);
+}
+
+VOID AsicUpdateRxWCIDTable(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN PUCHAR        pAddr)
+{
+	ULONG offset;
+	ULONG Addr;
+
+	offset = MAC_WCID_BASE + (WCID * HW_WCID_ENTRY_SIZE);
+	Addr = pAddr[0] + (pAddr[1] << 8) +(pAddr[2] << 16) +(pAddr[3] << 24);
+	RTMP_IO_WRITE32(pAd, offset, Addr);
+	Addr = pAddr[4] + (pAddr[5] << 8);
+	RTMP_IO_WRITE32(pAd, offset + 4, Addr);
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Set Cipher Key, Cipher algorithm, IV/EIV to Asic
+
+    Arguments:
+        pAd                     Pointer to our adapter
+        WCID                    WCID Entry number.
+        BssIndex                BSSID index, station or none multiple BSSID support
+                                this value should be 0.
+        KeyIdx                  This KeyIdx will set to IV's KeyID if bTxKey enabled
+        pCipherKey              Pointer to Cipher Key.
+        bUsePairewiseKeyTable   TRUE means saved the key in SharedKey table,
+                                otherwise PairewiseKey table
+        bTxKey                  This is the transmit key if enabled.
+
+    Return Value:
+        None
+
+    Note:
+        This routine will set the relative key stuff to Asic including WCID attribute,
+        Cipher Key, Cipher algorithm and IV/EIV.
+
+        IV/EIV will be update if this CipherKey is the transmission key because
+        ASIC will base on IV's KeyID value to select Cipher Key.
+
+        If bTxKey sets to FALSE, this is not the TX key, but it could be
+        RX key
+
+    	For AP mode bTxKey must be always set to TRUE.
+    ========================================================================
+*/
+VOID AsicAddKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR		KeyIdx,
+	IN PCIPHER_KEY	pCipherKey,
+	IN BOOLEAN		bUsePairewiseKeyTable,
+	IN BOOLEAN		bTxKey)
+{
+	ULONG	offset;
+	UCHAR	IV4 = 0;
+	PUCHAR		pKey = pCipherKey->Key;
+	PUCHAR		pTxMic = pCipherKey->TxMic;
+	PUCHAR		pRxMic = pCipherKey->RxMic;
+	PUCHAR		pTxtsc = pCipherKey->TxTsc;
+	UCHAR		CipherAlg = pCipherKey->CipherAlg;
+	SHAREDKEY_MODE_STRUC csr1;
+#ifdef RT2860
+	UCHAR		i;
+#endif // RT2860 //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> AsicAddKeyEntry\n"));
+	//
+	// 1.) decide key table offset
+	//
+	if (bUsePairewiseKeyTable)
+		offset = PAIRWISE_KEY_TABLE_BASE + (WCID * HW_KEY_ENTRY_SIZE);
+	else
+		offset = SHARED_KEY_TABLE_BASE + (4 * BssIndex + KeyIdx) * HW_KEY_ENTRY_SIZE;
+
+	//
+	// 2.) Set Key to Asic
+	//
+	//for (i = 0; i < KeyLen; i++)
+#ifdef RT2860
+	for (i = 0; i < MAX_LEN_OF_PEER_KEY; i++)
+	{
+		RTMP_IO_WRITE8(pAd, offset + i, pKey[i]);
+	}
+	offset += MAX_LEN_OF_PEER_KEY;
+
+	//
+	// 3.) Set MIC key if available
+	//
+	if (pTxMic)
+	{
+		for (i = 0; i < 8; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset + i, pTxMic[i]);
+		}
+	}
+	offset += LEN_TKIP_TXMICK;
+
+	if (pRxMic)
+	{
+		for (i = 0; i < 8; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset + i, pRxMic[i]);
+		}
+	}
+#endif // RT2860 //
+
+
+	//
+	// 4.) Modify IV/EIV if needs
+	//     This will force Asic to use this key ID by setting IV.
+	//
+	if (bTxKey)
+	{
+#ifdef RT2860
+		offset = MAC_IVEIV_TABLE_BASE + (WCID * HW_IVEIV_ENTRY_SIZE);
+		//
+		// Write IV
+		//
+		RTMP_IO_WRITE8(pAd, offset, pTxtsc[1]);
+		RTMP_IO_WRITE8(pAd, offset + 1, ((pTxtsc[1] | 0x20) & 0x7f));
+		RTMP_IO_WRITE8(pAd, offset + 2, pTxtsc[0]);
+
+		IV4 = (KeyIdx << 6);
+		if ((CipherAlg == CIPHER_TKIP) || (CipherAlg == CIPHER_TKIP_NO_MIC) ||(CipherAlg == CIPHER_AES))
+			IV4 |= 0x20;  // turn on extension bit means EIV existence
+
+		RTMP_IO_WRITE8(pAd, offset + 3, IV4);
+
+		//
+		// Write EIV
+		//
+		offset += 4;
+		for (i = 0; i < 4; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset + i, pTxtsc[i + 2]);
+		}
+#endif // RT2860 //
+
+		AsicUpdateWCIDAttribute(pAd, WCID, BssIndex, CipherAlg, bUsePairewiseKeyTable);
+	}
+
+	if (!bUsePairewiseKeyTable)
+	{
+		//
+		// Only update the shared key security mode
+		//
+		RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE + 4 * (BssIndex / 2), &csr1.word);
+		if ((BssIndex % 2) == 0)
+		{
+			if (KeyIdx == 0)
+				csr1.field.Bss0Key0CipherAlg = CipherAlg;
+			else if (KeyIdx == 1)
+				csr1.field.Bss0Key1CipherAlg = CipherAlg;
+			else if (KeyIdx == 2)
+				csr1.field.Bss0Key2CipherAlg = CipherAlg;
+			else
+				csr1.field.Bss0Key3CipherAlg = CipherAlg;
+		}
+		else
+		{
+			if (KeyIdx == 0)
+				csr1.field.Bss1Key0CipherAlg = CipherAlg;
+			else if (KeyIdx == 1)
+				csr1.field.Bss1Key1CipherAlg = CipherAlg;
+			else if (KeyIdx == 2)
+				csr1.field.Bss1Key2CipherAlg = CipherAlg;
+			else
+				csr1.field.Bss1Key3CipherAlg = CipherAlg;
+		}
+		RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE + 4 * (BssIndex / 2), csr1.word);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== AsicAddKeyEntry\n"));
+}
+
+
+/*
+	========================================================================
+	Description:
+		Add Pair-wise key material into ASIC.
+		Update pairwise key, TxMic and RxMic to Asic Pair-wise key table
+
+    Return:
+	========================================================================
+*/
+VOID AsicAddPairwiseKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR		WCID,
+	IN CIPHER_KEY		 *pCipherKey)
+{
+	INT i;
+	ULONG 		offset;
+	PUCHAR		 pKey = pCipherKey->Key;
+	PUCHAR		 pTxMic = pCipherKey->TxMic;
+	PUCHAR		 pRxMic = pCipherKey->RxMic;
+#ifdef DBG
+	UCHAR		CipherAlg = pCipherKey->CipherAlg;
+#endif // DBG //
+
+	// EKEY
+	offset = PAIRWISE_KEY_TABLE_BASE + (WCID * HW_KEY_ENTRY_SIZE);
+#ifdef RT2860
+	for (i=0; i<MAX_LEN_OF_PEER_KEY; i++)
+	{
+		RTMP_IO_WRITE8(pAd, offset + i, pKey[i]);
+	}
+#endif // RT2860 //
+	for (i=0; i<MAX_LEN_OF_PEER_KEY; i+=4)
+	{
+		UINT32 Value;
+		RTMP_IO_READ32(pAd, offset + i, &Value);
+	}
+
+	offset += MAX_LEN_OF_PEER_KEY;
+
+	//  MIC KEY
+	if (pTxMic)
+	{
+#ifdef RT2860
+		for (i=0; i<8; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset+i, pTxMic[i]);
+		}
+#endif // RT2860 //
+	}
+	offset += 8;
+	if (pRxMic)
+	{
+#ifdef RT2860
+		for (i=0; i<8; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset+i, pRxMic[i]);
+		}
+#endif // RT2860 //
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("AsicAddPairwiseKeyEntry: WCID #%d Alg=%s\n",WCID, CipherName[CipherAlg]));
+	DBGPRINT(RT_DEBUG_TRACE,("	Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pKey[0],pKey[1],pKey[2],pKey[3],pKey[4],pKey[5],pKey[6],pKey[7],pKey[8],pKey[9],pKey[10],pKey[11],pKey[12],pKey[13],pKey[14],pKey[15]));
+	if (pRxMic)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("	Rx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pRxMic[0],pRxMic[1],pRxMic[2],pRxMic[3],pRxMic[4],pRxMic[5],pRxMic[6],pRxMic[7]));
+	}
+	if (pTxMic)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("	Tx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pTxMic[0],pTxMic[1],pTxMic[2],pTxMic[3],pTxMic[4],pTxMic[5],pTxMic[6],pTxMic[7]));
+	}
+}
+/*
+	========================================================================
+	Description:
+		Remove Pair-wise key material from ASIC.
+
+    Return:
+	========================================================================
+*/
+VOID AsicRemovePairwiseKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 BssIdx,
+	IN UCHAR		 Wcid)
+{
+	ULONG		WCIDAttri;
+	USHORT		offset;
+
+	// re-set the entry's WCID attribute as OPEN-NONE.
+	offset = MAC_WCID_ATTRIBUTE_BASE + (Wcid * HW_WCID_ATTRI_SIZE);
+	WCIDAttri = (BssIdx<<4) | PAIRWISEKEYTABLE;
+	RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+}
+
+BOOLEAN AsicSendCommandToMcu(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 Command,
+	IN UCHAR		 Token,
+	IN UCHAR		 Arg0,
+	IN UCHAR		 Arg1)
+{
+	HOST_CMD_CSR_STRUC	H2MCmd;
+	H2M_MAILBOX_STRUC	H2MMailbox;
+	ULONG				i = 0;
+#ifdef RT2860
+#ifdef RALINK_ATE
+	static UINT32 j = 0;
+#endif // RALINK_ATE //
+#endif // RT2860 //
+	do
+	{
+		RTMP_IO_READ32(pAd, H2M_MAILBOX_CSR, &H2MMailbox.word);
+		if (H2MMailbox.field.Owner == 0)
+			break;
+
+		RTMPusecDelay(2);
+	} while(i++ < 100);
+
+	if (i >= 100)
+	{
+#ifdef RT2860
+#ifdef RALINK_ATE
+		if (pAd->ate.bFWLoading == TRUE)
+		{
+			/* reloading firmware when received iwpriv cmd "ATE=ATESTOP" */
+			if (j > 0)
+			{
+				if (j % 64 != 0)
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("#"));
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("\n"));
+				}
+				++j;
+			}
+			else if (j == 0)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("Loading firmware. Please wait for a moment...\n"));
+				++j;
+			}
+		}
+		else
+#endif // RALINK_ATE //
+#endif // RT2860 //
+		{
+		DBGPRINT_ERR(("H2M_MAILBOX still hold by MCU. command fail\n"));
+		}
+		return FALSE;
+	}
+
+#ifdef RT2860
+#ifdef RALINK_ATE
+	else if (pAd->ate.bFWLoading == TRUE)
+	{
+		/* reloading of firmware is completed */
+		pAd->ate.bFWLoading = FALSE;
+		DBGPRINT(RT_DEBUG_ERROR, ("\n"));
+		j = 0;
+	}
+#endif // RALINK_ATE //
+#endif // RT2860 //
+
+	H2MMailbox.field.Owner	  = 1;	   // pass ownership to MCU
+	H2MMailbox.field.CmdToken = Token;
+	H2MMailbox.field.HighByte = Arg1;
+	H2MMailbox.field.LowByte  = Arg0;
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CSR, H2MMailbox.word);
+
+	H2MCmd.word 			  = 0;
+	H2MCmd.field.HostCommand  = Command;
+	RTMP_IO_WRITE32(pAd, HOST_CMD_CSR, H2MCmd.word);
+
+	if (Command != 0x80)
+	{
+	}
+
+	return TRUE;
+}
+
+#ifdef RT2860
+BOOLEAN AsicCheckCommanOk(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 Command)
+{
+	UINT32	CmdStatus = 0, CID = 0, i;
+	UINT32	ThisCIDMask = 0;
+
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, H2M_MAILBOX_CID, &CID);
+		// Find where the command is. Because this is randomly specified by firmware.
+		if ((CID & CID0MASK) == Command)
+		{
+			ThisCIDMask = CID0MASK;
+			break;
+		}
+		else if ((((CID & CID1MASK)>>8) & 0xff) == Command)
+		{
+			ThisCIDMask = CID1MASK;
+			break;
+		}
+		else if ((((CID & CID2MASK)>>16) & 0xff) == Command)
+		{
+			ThisCIDMask = CID2MASK;
+			break;
+		}
+		else if ((((CID & CID3MASK)>>24) & 0xff) == Command)
+		{
+			ThisCIDMask = CID3MASK;
+			break;
+		}
+
+		RTMPusecDelay(100);
+		i++;
+	}while (i < 200);
+
+	// Get CommandStatus Value
+	RTMP_IO_READ32(pAd, H2M_MAILBOX_STATUS, &CmdStatus);
+
+	// This command's status is at the same position as command. So AND command position's bitmask to read status.
+	if (i < 200)
+	{
+		// If Status is 1, the comamnd is success.
+		if (((CmdStatus & ThisCIDMask) == 0x1) || ((CmdStatus & ThisCIDMask) == 0x100)
+			|| ((CmdStatus & ThisCIDMask) == 0x10000) || ((CmdStatus & ThisCIDMask) == 0x1000000))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("--> AsicCheckCommanOk CID = 0x%x, CmdStatus= 0x%x \n", CID, CmdStatus));
+			RTMP_IO_WRITE32(pAd, H2M_MAILBOX_STATUS, 0xffffffff);
+			RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CID, 0xffffffff);
+			return TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("--> AsicCheckCommanFail1 CID = 0x%x, CmdStatus= 0x%x \n", CID, CmdStatus));
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("--> AsicCheckCommanFail2 Timeout Command = %d, CmdStatus= 0x%x \n", Command, CmdStatus));
+	}
+	// Clear Command and Status.
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_STATUS, 0xffffffff);
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CID, 0xffffffff);
+
+	return FALSE;
+}
+#endif // RT2860 //
+
+/*
+	========================================================================
+
+	Routine Description:
+		Verify the support rate for different PHY type
+
+	Arguments:
+		pAd 				Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPCheckRates(
+	IN		PRTMP_ADAPTER	pAd,
+	IN OUT	UCHAR			SupRate[],
+	IN OUT	UCHAR			*SupRateLen)
+{
+	UCHAR	RateIdx, i, j;
+	UCHAR	NewRate[12], NewRateLen;
+
+	NewRateLen = 0;
+
+	if (pAd->CommonCfg.PhyMode == PHY_11B)
+		RateIdx = 4;
+	else
+		RateIdx = 12;
+
+	// Check for support rates exclude basic rate bit
+	for (i = 0; i < *SupRateLen; i++)
+		for (j = 0; j < RateIdx; j++)
+			if ((SupRate[i] & 0x7f) == RateIdTo500Kbps[j])
+				NewRate[NewRateLen++] = SupRate[i];
+
+	*SupRateLen = NewRateLen;
+	NdisMoveMemory(SupRate, NewRate, NewRateLen);
+}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+BOOLEAN RTMPCheckChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		CentralChannel,
+	IN UCHAR		Channel)
+{
+	UCHAR		k;
+	UCHAR		UpperChannel = 0, LowerChannel = 0;
+	UCHAR		NoEffectChannelinList = 0;
+
+	// Find upper and lower channel according to 40MHz current operation.
+	if (CentralChannel < Channel)
+	{
+		UpperChannel = Channel;
+		if (CentralChannel > 2)
+			LowerChannel = CentralChannel - 2;
+		else
+			return FALSE;
+	}
+	else if (CentralChannel > Channel)
+	{
+		UpperChannel = CentralChannel + 2;
+		LowerChannel = Channel;
+	}
+
+	for (k = 0;k < pAd->ChannelListNum;k++)
+	{
+		if (pAd->ChannelList[k].Channel == UpperChannel)
+		{
+			NoEffectChannelinList ++;
+		}
+		if (pAd->ChannelList[k].Channel == LowerChannel)
+		{
+			NoEffectChannelinList ++;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("Total Channel in Channel List = [%d]\n", NoEffectChannelinList));
+	if (NoEffectChannelinList == 2)
+		return TRUE;
+	else
+		return FALSE;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Verify the support rate for HT phy type
+
+	Arguments:
+		pAd 				Pointer to our adapter
+
+	Return Value:
+		FALSE if pAd->CommonCfg.SupportedHtPhy doesn't accept the pHtCapability.  (AP Mode)
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+BOOLEAN 	RTMPCheckHt(
+	IN	PRTMP_ADAPTER			pAd,
+	IN	UCHAR					Wcid,
+	IN 	HT_CAPABILITY_IE		*pHtCapability,
+	IN 	ADD_HT_INFO_IE			*pAddHtInfo)
+{
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	// If use AMSDU, set flag.
+	if (pAd->CommonCfg.DesiredHtPhy.AmsduEnable)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_AMSDU_INUSED);
+	// Save Peer Capability
+	if (pHtCapability->HtCapInfo.ShortGIfor20)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_SGI20_CAPABLE);
+	if (pHtCapability->HtCapInfo.ShortGIfor40)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_SGI40_CAPABLE);
+	if (pHtCapability->HtCapInfo.TxSTBC)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_TxSTBC_CAPABLE);
+	if (pHtCapability->HtCapInfo.RxSTBC)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_RxSTBC_CAPABLE);
+	if (pAd->CommonCfg.bRdg && pHtCapability->ExtHtCapInfo.RDGSupport)
+	{
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_RDG_CAPABLE);
+	}
+
+	if (Wcid < MAX_LEN_OF_MAC_TABLE)
+	{
+		pAd->MacTab.Content[Wcid].MpduDensity = pHtCapability->HtCapParm.MpduDensity;
+	}
+
+	// Will check ChannelWidth for MCSSet[4] below
+	pAd->MlmeAux.HtCapability.MCSSet[4] = 0x1;
+    switch (pAd->CommonCfg.RxStream)
+	{
+		case 1:
+			pAd->MlmeAux.HtCapability.MCSSet[0] = 0xff;
+			pAd->MlmeAux.HtCapability.MCSSet[1] = 0x00;
+            pAd->MlmeAux.HtCapability.MCSSet[2] = 0x00;
+            pAd->MlmeAux.HtCapability.MCSSet[3] = 0x00;
+			break;
+		case 2:
+			pAd->MlmeAux.HtCapability.MCSSet[0] = 0xff;
+			pAd->MlmeAux.HtCapability.MCSSet[1] = 0xff;
+            pAd->MlmeAux.HtCapability.MCSSet[2] = 0x00;
+            pAd->MlmeAux.HtCapability.MCSSet[3] = 0x00;
+			break;
+		case 3:
+			pAd->MlmeAux.HtCapability.MCSSet[0] = 0xff;
+			pAd->MlmeAux.HtCapability.MCSSet[1] = 0xff;
+            pAd->MlmeAux.HtCapability.MCSSet[2] = 0xff;
+            pAd->MlmeAux.HtCapability.MCSSet[3] = 0x00;
+			break;
+	}
+
+	pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth = pAddHtInfo->AddHtInfo.RecomWidth & pAd->CommonCfg.DesiredHtPhy.ChannelWidth;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("RTMPCheckHt:: HtCapInfo.ChannelWidth=%d, RecomWidth=%d, DesiredHtPhy.ChannelWidth=%d, BW40MAvailForA/G=%d/%d, PhyMode=%d \n",
+		pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth, pAddHtInfo->AddHtInfo.RecomWidth, pAd->CommonCfg.DesiredHtPhy.ChannelWidth,
+		pAd->NicConfig2.field.BW40MAvailForA, pAd->NicConfig2.field.BW40MAvailForG, pAd->CommonCfg.PhyMode));
+
+	pAd->MlmeAux.HtCapability.HtCapInfo.GF =  pHtCapability->HtCapInfo.GF &pAd->CommonCfg.DesiredHtPhy.GF;
+
+	// Send Assoc Req with my HT capability.
+	pAd->MlmeAux.HtCapability.HtCapInfo.AMsduSize =  pAd->CommonCfg.DesiredHtPhy.AmsduSize;
+	pAd->MlmeAux.HtCapability.HtCapInfo.MimoPs =  pAd->CommonCfg.DesiredHtPhy.MimoPs;
+	pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor20 =  (pAd->CommonCfg.DesiredHtPhy.ShortGIfor20) & (pHtCapability->HtCapInfo.ShortGIfor20);
+	pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor40 =  (pAd->CommonCfg.DesiredHtPhy.ShortGIfor40) & (pHtCapability->HtCapInfo.ShortGIfor40);
+	pAd->MlmeAux.HtCapability.HtCapInfo.TxSTBC =  (pAd->CommonCfg.DesiredHtPhy.TxSTBC)&(pHtCapability->HtCapInfo.RxSTBC);
+	pAd->MlmeAux.HtCapability.HtCapInfo.RxSTBC =  (pAd->CommonCfg.DesiredHtPhy.RxSTBC)&(pHtCapability->HtCapInfo.TxSTBC);
+	pAd->MlmeAux.HtCapability.HtCapParm.MaxRAmpduFactor = pAd->CommonCfg.DesiredHtPhy.MaxRAmpduFactor;
+    pAd->MlmeAux.HtCapability.HtCapParm.MpduDensity = pAd->CommonCfg.HtCapability.HtCapParm.MpduDensity;
+	pAd->MlmeAux.HtCapability.ExtHtCapInfo.PlusHTC = pHtCapability->ExtHtCapInfo.PlusHTC;
+	pAd->MacTab.Content[Wcid].HTCapability.ExtHtCapInfo.PlusHTC = pHtCapability->ExtHtCapInfo.PlusHTC;
+	if (pAd->CommonCfg.bRdg)
+	{
+		pAd->MlmeAux.HtCapability.ExtHtCapInfo.RDGSupport = pHtCapability->ExtHtCapInfo.RDGSupport;
+        pAd->MlmeAux.HtCapability.ExtHtCapInfo.PlusHTC = 1;
+	}
+
+    if (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_20)
+        pAd->MlmeAux.HtCapability.MCSSet[4] = 0x0;  // BW20 can't transmit MCS32
+
+	COPY_AP_HTSETTINGS_FROM_BEACON(pAd, pHtCapability);
+	return TRUE;
+}
+#endif // DOT11_N_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	========================================================================
+
+	Routine Description:
+		Verify the support rate for different PHY type
+
+	Arguments:
+		pAd 				Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID RTMPUpdateMlmeRate(
+	IN PRTMP_ADAPTER	pAd)
+{
+	UCHAR	MinimumRate;
+	UCHAR	ProperMlmeRate; //= RATE_54;
+	UCHAR	i, j, RateIdx = 12; //1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54
+	BOOLEAN	bMatch = FALSE;
+
+	switch (pAd->CommonCfg.PhyMode)
+	{
+		case PHY_11B:
+			ProperMlmeRate = RATE_11;
+			MinimumRate = RATE_1;
+			break;
+		case PHY_11BG_MIXED:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11ABGN_MIXED:
+		case PHY_11BGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			if ((pAd->MlmeAux.SupRateLen == 4) &&
+				(pAd->MlmeAux.ExtRateLen == 0))
+				// B only AP
+				ProperMlmeRate = RATE_11;
+			else
+				ProperMlmeRate = RATE_24;
+
+			if (pAd->MlmeAux.Channel <= 14)
+				MinimumRate = RATE_1;
+			else
+				MinimumRate = RATE_6;
+			break;
+		case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11N_2_4G:	// rt2860 need to check mlmerate for 802.11n
+		case PHY_11GN_MIXED:
+		case PHY_11AGN_MIXED:
+		case PHY_11AN_MIXED:
+		case PHY_11N_5G:
+#endif // DOT11_N_SUPPORT //
+			ProperMlmeRate = RATE_24;
+			MinimumRate = RATE_6;
+			break;
+		case PHY_11ABG_MIXED:
+			ProperMlmeRate = RATE_24;
+			if (pAd->MlmeAux.Channel <= 14)
+			   MinimumRate = RATE_1;
+			else
+				MinimumRate = RATE_6;
+			break;
+		default: // error
+			ProperMlmeRate = RATE_1;
+			MinimumRate = RATE_1;
+			break;
+	}
+
+	for (i = 0; i < pAd->MlmeAux.SupRateLen; i++)
+	{
+		for (j = 0; j < RateIdx; j++)
+		{
+			if ((pAd->MlmeAux.SupRate[i] & 0x7f) == RateIdTo500Kbps[j])
+			{
+				if (j == ProperMlmeRate)
+				{
+					bMatch = TRUE;
+					break;
+				}
+			}
+		}
+
+		if (bMatch)
+			break;
+	}
+
+	if (bMatch == FALSE)
+	{
+		for (i = 0; i < pAd->MlmeAux.ExtRateLen; i++)
+		{
+			for (j = 0; j < RateIdx; j++)
+			{
+				if ((pAd->MlmeAux.ExtRate[i] & 0x7f) == RateIdTo500Kbps[j])
+				{
+					if (j == ProperMlmeRate)
+					{
+						bMatch = TRUE;
+						break;
+					}
+				}
+			}
+
+			if (bMatch)
+				break;
+		}
+	}
+
+	if (bMatch == FALSE)
+	{
+		ProperMlmeRate = MinimumRate;
+	}
+
+	pAd->CommonCfg.MlmeRate = MinimumRate;
+	pAd->CommonCfg.RtsRate = ProperMlmeRate;
+	if (pAd->CommonCfg.MlmeRate >= RATE_6)
+	{
+		pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+		pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MODE = MODE_OFDM;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+	}
+	else
+	{
+		pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+		pAd->CommonCfg.MlmeTransmit.field.MCS = pAd->CommonCfg.MlmeRate;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MODE = MODE_CCK;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MCS = pAd->CommonCfg.MlmeRate;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPUpdateMlmeRate ==>   MlmeTransmit = 0x%x  \n" , pAd->CommonCfg.MlmeTransmit.word));
+}
+
+CHAR RTMPMaxRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN CHAR				Rssi0,
+	IN CHAR				Rssi1,
+	IN CHAR				Rssi2)
+{
+	CHAR	larger = -127;
+
+	if ((pAd->Antenna.field.RxPath == 1) && (Rssi0 != 0))
+	{
+		larger = Rssi0;
+	}
+
+	if ((pAd->Antenna.field.RxPath >= 2) && (Rssi1 != 0))
+	{
+		larger = max(Rssi0, Rssi1);
+	}
+
+	if ((pAd->Antenna.field.RxPath == 3) && (Rssi2 != 0))
+	{
+		larger = max(larger, Rssi2);
+	}
+
+	if (larger == -127)
+		larger = 0;
+
+	return larger;
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Periodic evaluate antenna link status
+
+    Arguments:
+        pAd         - Adapter pointer
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID AsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd)
+{
+	UCHAR	BBPR3 = 0;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS	|
+								fRTMP_ADAPTER_HALT_IN_PROGRESS	|
+								fRTMP_ADAPTER_RADIO_OFF			|
+								fRTMP_ADAPTER_NIC_NOT_EXIST		|
+								fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			return;
+
+		if (pAd->StaCfg.Psm == PWR_SAVE)
+			return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
+	BBPR3 &= (~0x18);
+	if(pAd->Antenna.field.RxPath == 3)
+	{
+		BBPR3 |= (0x10);
+	}
+	else if(pAd->Antenna.field.RxPath == 2)
+	{
+		BBPR3 |= (0x8);
+	}
+	else if(pAd->Antenna.field.RxPath == 1)
+	{
+		BBPR3 |= (0x0);
+	}
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPR3);
+#ifdef CONFIG_STA_SUPPORT
+#ifdef RT2860
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+    	pAd->StaCfg.BBPR3 = BBPR3;
+#endif // RT2860 //
+#endif // CONFIG_STA_SUPPORT //
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+		)
+	{
+		ULONG	TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+								pAd->RalinkCounters.OneSecTxRetryOkCount +
+								pAd->RalinkCounters.OneSecTxFailCount;
+
+		if (TxTotalCnt > 50)
+		{
+			RTMPSetTimer(&pAd->Mlme.RxAntEvalTimer, 20);
+			pAd->Mlme.bLowThroughput = FALSE;
+		}
+		else
+		{
+			RTMPSetTimer(&pAd->Mlme.RxAntEvalTimer, 300);
+			pAd->Mlme.bLowThroughput = TRUE;
+		}
+	}
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        After evaluation, check antenna link status
+
+    Arguments:
+        pAd         - Adapter pointer
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID AsicRxAntEvalTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER	*pAd = (RTMP_ADAPTER *)FunctionContext;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR			BBPR3 = 0;
+	CHAR			larger = -127, rssi0, rssi1, rssi2;
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)	||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)		||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF)			||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+			return;
+
+		if (pAd->StaCfg.Psm == PWR_SAVE)
+			return;
+
+
+		// if the traffic is low, use average rssi as the criteria
+		if (pAd->Mlme.bLowThroughput == TRUE)
+		{
+			rssi0 = pAd->StaCfg.RssiSample.LastRssi0;
+			rssi1 = pAd->StaCfg.RssiSample.LastRssi1;
+			rssi2 = pAd->StaCfg.RssiSample.LastRssi2;
+		}
+		else
+		{
+			rssi0 = pAd->StaCfg.RssiSample.AvgRssi0;
+			rssi1 = pAd->StaCfg.RssiSample.AvgRssi1;
+			rssi2 = pAd->StaCfg.RssiSample.AvgRssi2;
+		}
+
+		if(pAd->Antenna.field.RxPath == 3)
+		{
+			larger = max(rssi0, rssi1);
+
+			if (larger > (rssi2 + 20))
+				pAd->Mlme.RealRxPath = 2;
+			else
+				pAd->Mlme.RealRxPath = 3;
+		}
+		else if(pAd->Antenna.field.RxPath == 2)
+		{
+			if (rssi0 > (rssi1 + 20))
+				pAd->Mlme.RealRxPath = 1;
+			else
+				pAd->Mlme.RealRxPath = 2;
+		}
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
+		BBPR3 &= (~0x18);
+		if(pAd->Mlme.RealRxPath == 3)
+		{
+			BBPR3 |= (0x10);
+		}
+		else if(pAd->Mlme.RealRxPath == 2)
+		{
+			BBPR3 |= (0x8);
+		}
+		else if(pAd->Mlme.RealRxPath == 1)
+		{
+			BBPR3 |= (0x0);
+		}
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPR3);
+#ifdef RT2860
+    pAd->StaCfg.BBPR3 = BBPR3;
+#endif // RT2860 //
+	}
+
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+
+
+VOID APSDPeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+		return;
+
+	pAd->CommonCfg.TriggerTimerCount++;
+
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Set/reset MAC registers according to bPiggyBack parameter
+
+    Arguments:
+        pAd         - Adapter pointer
+        bPiggyBack  - Enable / Disable Piggy-Back
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID RTMPSetPiggyBack(
+    IN PRTMP_ADAPTER    pAd,
+    IN BOOLEAN          bPiggyBack)
+{
+	TX_LINK_CFG_STRUC  TxLinkCfg;
+
+	RTMP_IO_READ32(pAd, TX_LINK_CFG, &TxLinkCfg.word);
+
+	TxLinkCfg.field.TxCFAckEn = bPiggyBack;
+	RTMP_IO_WRITE32(pAd, TX_LINK_CFG, TxLinkCfg.word);
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        check if this entry need to switch rate automatically
+
+    Arguments:
+        pAd
+        pEntry
+
+    Return Value:
+        TURE
+        FALSE
+
+    ========================================================================
+*/
+BOOLEAN RTMPCheckEntryEnableAutoRateSwitch(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry)
+{
+	BOOLEAN		result = TRUE;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// only associated STA counts
+		if (pEntry && (pEntry->ValidAsCLI) && (pEntry->Sst == SST_ASSOC))
+		{
+			result = pAd->StaCfg.bAutoTxRateSwitch;
+		}
+		else
+			result = FALSE;
+
+#ifdef QOS_DLS_SUPPORT
+		if (pEntry && (pEntry->ValidAsDls))
+			result = pAd->StaCfg.bAutoTxRateSwitch;
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+	return result;
+}
+
+
+BOOLEAN RTMPAutoRateSwitchCheck(
+	IN PRTMP_ADAPTER    pAd)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->StaCfg.bAutoTxRateSwitch)
+			return TRUE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+	return FALSE;
+}
+
+
+/*
+    ========================================================================
+    Routine Description:
+        check if this entry need to fix tx legacy rate
+
+    Arguments:
+        pAd
+        pEntry
+
+    Return Value:
+        TURE
+        FALSE
+
+    ========================================================================
+*/
+UCHAR RTMPStaFixedTxMode(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry)
+{
+	UCHAR	tx_mode = FIXED_TXMODE_HT;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		tx_mode = (UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	return tx_mode;
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Overwrite HT Tx Mode by Fixed Legency Tx Mode, if specified.
+
+    Arguments:
+        pAd
+        pEntry
+
+    Return Value:
+        TURE
+        FALSE
+
+    ========================================================================
+*/
+VOID RTMPUpdateLegacyTxSetting(
+		UCHAR				fixed_tx_mode,
+		PMAC_TABLE_ENTRY	pEntry)
+{
+	HTTRANSMIT_SETTING TransmitSetting;
+
+	if (fixed_tx_mode == FIXED_TXMODE_HT)
+		return;
+
+	TransmitSetting.word = 0;
+
+	TransmitSetting.field.MODE = pEntry->HTPhyMode.field.MODE;
+	TransmitSetting.field.MCS = pEntry->HTPhyMode.field.MCS;
+
+	if (fixed_tx_mode == FIXED_TXMODE_CCK)
+	{
+		TransmitSetting.field.MODE = MODE_CCK;
+		// CCK mode allow MCS 0~3
+		if (TransmitSetting.field.MCS > MCS_3)
+			TransmitSetting.field.MCS = MCS_3;
+	}
+	else
+	{
+		TransmitSetting.field.MODE = MODE_OFDM;
+		// OFDM mode allow MCS 0~7
+		if (TransmitSetting.field.MCS > MCS_7)
+			TransmitSetting.field.MCS = MCS_7;
+	}
+
+	if (pEntry->HTPhyMode.field.MODE >= TransmitSetting.field.MODE)
+	{
+		pEntry->HTPhyMode.word = TransmitSetting.word;
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPUpdateLegacyTxSetting : wcid-%d, MODE=%s, MCS=%d \n",
+				pEntry->Aid, GetPhyMode(pEntry->HTPhyMode.field.MODE), pEntry->HTPhyMode.field.MCS));
+	}
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	==========================================================================
+	Description:
+		dynamic tune BBP R66 to find a balance between sensibility and
+		noise isolation
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicStaBbpTuning(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR	OrigR66Value = 0, R66;//, R66UpperBound = 0x30, R66LowerBound = 0x30;
+	CHAR	Rssi;
+
+	// 2860C did not support Fase CCA, therefore can't tune
+	if (pAd->MACVersion == 0x28600100)
+		return;
+
+	//
+	// work as a STA
+	//
+	if (pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE)  // no R66 tuning when SCANNING
+		return;
+
+	if ((pAd->OpMode == OPMODE_STA)
+		&& (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+			)
+		&& !(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+#ifdef RT2860
+		&& (pAd->bPCIclkOff == FALSE)
+#endif // RT2860 //
+		)
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &OrigR66Value);
+		R66 = OrigR66Value;
+
+		if (pAd->Antenna.field.RxPath > 1)
+			Rssi = (pAd->StaCfg.RssiSample.AvgRssi0 + pAd->StaCfg.RssiSample.AvgRssi1) >> 1;
+		else
+			Rssi = pAd->StaCfg.RssiSample.AvgRssi0;
+
+		if (pAd->LatchRfRegs.Channel <= 14)
+		{	//BG band
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = (0x2E + GET_LNA_GAIN(pAd)) + 0x10;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x2E + GET_LNA_GAIN(pAd);
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+			}
+		}
+		else
+		{	//A band
+			if (pAd->CommonCfg.BBPCurrentBW == BW_20)
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = 0x32 + (GET_LNA_GAIN(pAd)*5)/3 + 0x10;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x32 + (GET_LNA_GAIN(pAd)*5)/3;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+			}
+			else
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = 0x3A + (GET_LNA_GAIN(pAd)*5)/3 + 0x10;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x3A + (GET_LNA_GAIN(pAd)*5)/3;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+			}
+		}
+
+
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+VOID RTMPSetAGCInitValue(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			BandWidth)
+{
+	UCHAR	R66 = 0x30;
+
+	if (pAd->LatchRfRegs.Channel <= 14)
+	{	// BG band
+		R66 = 0x2E + GET_LNA_GAIN(pAd);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+	}
+	else
+	{	//A band
+		if (BandWidth == BW_20)
+		{
+			R66 = (UCHAR)(0x32 + (GET_LNA_GAIN(pAd)*5)/3);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+#ifdef DOT11_N_SUPPORT
+		else
+		{
+			R66 = (UCHAR)(0x3A + (GET_LNA_GAIN(pAd)*5)/3);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+#endif // DOT11_N_SUPPORT //
+	}
+
+}
+
+VOID AsicTurnOffRFClk(
+	IN PRTMP_ADAPTER pAd,
+	IN	UCHAR		Channel)
+{
+
+	// RF R2 bit 18 = 0
+	UINT32			R1 = 0, R2 = 0, R3 = 0;
+	UCHAR			index;
+	RTMP_RF_REGS	*RFRegTable;
+
+	RFRegTable = RF2850RegTable;
+
+	switch (pAd->RfIcType)
+	{
+		case RFIC_2820:
+		case RFIC_2850:
+		case RFIC_2720:
+		case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R1 = RFRegTable[index].R1 & 0xffffdfff;
+					R2 = RFRegTable[index].R2 & 0xfffbffff;
+					R3 = RFRegTable[index].R3 & 0xfff3ffff;
+
+					RTMP_RF_IO_WRITE32(pAd, R1);
+					RTMP_RF_IO_WRITE32(pAd, R2);
+
+					// Program R1b13 to 1, R3/b18,19 to 0, R2b18 to 0.
+					// Set RF R2 bit18=0, R3 bit[18:19]=0
+					//if (pAd->StaCfg.bRadio == FALSE)
+					if (1)
+					{
+						RTMP_RF_IO_WRITE32(pAd, R3);
+
+						DBGPRINT(RT_DEBUG_TRACE, ("AsicTurnOffRFClk#%d(RF=%d, ) , R2=0x%08x,  R3 = 0x%08x \n",
+							Channel, pAd->RfIcType, R2, R3));
+					}
+					else
+						DBGPRINT(RT_DEBUG_TRACE, ("AsicTurnOffRFClk#%d(RF=%d, ) , R2=0x%08x \n",
+							Channel, pAd->RfIcType, R2));
+					break;
+				}
+			}
+			break;
+
+		default:
+			break;
+	}
+}
+
+
+VOID AsicTurnOnRFClk(
+	IN PRTMP_ADAPTER pAd,
+	IN	UCHAR			Channel)
+{
+
+	// RF R2 bit 18 = 0
+	UINT32			R1 = 0, R2 = 0, R3 = 0;
+	UCHAR			index;
+	RTMP_RF_REGS	*RFRegTable;
+
+	RFRegTable = RF2850RegTable;
+
+	switch (pAd->RfIcType)
+	{
+		case RFIC_2820:
+		case RFIC_2850:
+		case RFIC_2720:
+		case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R3 = pAd->LatchRfRegs.R3;
+					R3 &= 0xfff3ffff;
+					R3 |= 0x00080000;
+					RTMP_RF_IO_WRITE32(pAd, R3);
+
+					R1 = RFRegTable[index].R1;
+					RTMP_RF_IO_WRITE32(pAd, R1);
+
+					R2 = RFRegTable[index].R2;
+					if (pAd->Antenna.field.TxPath == 1)
+					{
+						R2 |= 0x4000;	// If TXpath is 1, bit 14 = 1;
+					}
+
+					if (pAd->Antenna.field.RxPath == 2)
+					{
+						R2 |= 0x40;	// write 1 to off Rxpath.
+					}
+					else if (pAd->Antenna.field.RxPath == 1)
+					{
+						R2 |= 0x20040;	// write 1 to off RxPath
+					}
+					RTMP_RF_IO_WRITE32(pAd, R2);
+
+					break;
+				}
+			}
+			break;
+
+		default:
+			break;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicTurnOnRFClk#%d(RF=%d, ) , R2=0x%08x\n",
+		Channel,
+		pAd->RfIcType,
+		R2));
+}
+
diff --git a/drivers/staging/rt2860/common/netif_block.c b/drivers/staging/rt2860/common/netif_block.c
new file mode 100644
index 000000000000..d3f7d087e7f0
--- /dev/null
+++ b/drivers/staging/rt2860/common/netif_block.c
@@ -0,0 +1,144 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "../rt_config.h"
+#include "netif_block.h"
+
+static NETIF_ENTRY freeNetIfEntryPool[FREE_NETIF_POOL_SIZE];
+static LIST_HEADER freeNetIfEntryList;
+
+void initblockQueueTab(
+	IN PRTMP_ADAPTER pAd)
+{
+	int i;
+
+	initList(&freeNetIfEntryList);
+	for (i = 0; i < FREE_NETIF_POOL_SIZE; i++)
+		insertTailList(&freeNetIfEntryList, (PLIST_ENTRY)&freeNetIfEntryPool[i]);
+
+	for (i=0; i < NUM_OF_TX_RING; i++)
+		initList(&pAd->blockQueueTab[i].NetIfList);
+
+	return;
+}
+
+BOOLEAN blockNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry,
+	IN PNET_DEV pNetDev)
+{
+	PNETIF_ENTRY pNetIfEntry = NULL;
+
+	if ((pNetIfEntry = (PNETIF_ENTRY)removeHeadList(&freeNetIfEntryList)) != NULL)
+	{
+		netif_stop_queue(pNetDev);
+		pNetIfEntry->pNetDev = pNetDev;
+		insertTailList(&pBlockQueueEntry->NetIfList, (PLIST_ENTRY)pNetIfEntry);
+
+		pBlockQueueEntry->SwTxQueueBlockFlag = TRUE;
+		DBGPRINT(RT_DEBUG_TRACE, ("netif_stop_queue(%s)\n", pNetDev->name));
+	}
+	else
+		return FALSE;
+
+	return TRUE;
+}
+
+VOID releaseNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry)
+{
+	PNETIF_ENTRY pNetIfEntry = NULL;
+	PLIST_HEADER pNetIfList = &pBlockQueueEntry->NetIfList;
+
+	while((pNetIfEntry = (PNETIF_ENTRY)removeHeadList(pNetIfList)) !=  NULL)
+	{
+		PNET_DEV pNetDev = pNetIfEntry->pNetDev;
+		netif_wake_queue(pNetDev);
+		insertTailList(&freeNetIfEntryList, (PLIST_ENTRY)pNetIfEntry);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("netif_wake_queue(%s)\n", pNetDev->name));
+	}
+	pBlockQueueEntry->SwTxQueueBlockFlag = FALSE;
+	return;
+}
+
+
+VOID StopNetIfQueue(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR QueIdx,
+	IN PNDIS_PACKET pPacket)
+{
+	PNET_DEV NetDev = NULL;
+	UCHAR IfIdx = 0;
+	BOOLEAN valid = FALSE;
+
+#ifdef APCLI_SUPPORT
+	if (RTMP_GET_PACKET_NET_DEVICE(pPacket) >= MIN_NET_DEVICE_FOR_APCLI)
+	{
+		IfIdx = (RTMP_GET_PACKET_NET_DEVICE(pPacket) - MIN_NET_DEVICE_FOR_APCLI) % MAX_APCLI_NUM;
+		NetDev = pAd->ApCfg.ApCliTab[IfIdx].dev;
+	}
+	else
+#endif // APCLI_SUPPORT //
+#ifdef WDS_SUPPORT
+	if (RTMP_GET_PACKET_NET_DEVICE(pPacket) >= MIN_NET_DEVICE_FOR_WDS)
+	{
+		IfIdx = (RTMP_GET_PACKET_NET_DEVICE(pPacket) - MIN_NET_DEVICE_FOR_WDS) % MAX_WDS_ENTRY;
+		NetDev = pAd->WdsTab.WdsEntry[IfIdx].dev;
+	}
+	else
+#endif // WDS_SUPPORT //
+	{
+#ifdef MBSS_SUPPORT
+		if (pAd->OpMode == OPMODE_AP)
+		{
+			IfIdx = (RTMP_GET_PACKET_NET_DEVICE(pPacket) - MIN_NET_DEVICE_FOR_MBSSID) % MAX_MBSSID_NUM;
+			NetDev = pAd->ApCfg.MBSSID[IfIdx].MSSIDDev;
+		}
+		else
+		{
+			IfIdx = MAIN_MBSSID;
+			NetDev = pAd->net_dev;
+		}
+#else
+		IfIdx = MAIN_MBSSID;
+		NetDev = pAd->net_dev;
+#endif
+	}
+
+	// WMM support 4 software queues.
+	// One software queue full doesn't mean device have no capbility to transmit packet.
+	// So disable block Net-If queue function while WMM enable.
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		valid = (pAd->CommonCfg.bWmmCapable == TRUE) ? FALSE : TRUE;
+#endif // CONFIG_STA_SUPPORT //
+
+	if (valid)
+		blockNetIf(&pAd->blockQueueTab[QueIdx], NetDev);
+	return;
+}
+
diff --git a/drivers/staging/rt2860/common/netif_block.h b/drivers/staging/rt2860/common/netif_block.h
new file mode 100644
index 000000000000..6e5151c41095
--- /dev/null
+++ b/drivers/staging/rt2860/common/netif_block.h
@@ -0,0 +1,58 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __NET_IF_BLOCK_H__
+#define __NET_IF_BLOCK_H__
+
+//#include <linux/device.h>
+#include "link_list.h"
+#include "rtmp.h"
+
+#define FREE_NETIF_POOL_SIZE 32
+
+typedef struct _NETIF_ENTRY
+{
+	struct _NETIF_ENTRY *pNext;
+	PNET_DEV pNetDev;
+} NETIF_ENTRY, *PNETIF_ENTRY;
+
+void initblockQueueTab(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN blockNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry,
+	IN PNET_DEV pNetDev);
+
+VOID releaseNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry);
+
+VOID StopNetIfQueue(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR QueIdx,
+	IN PNDIS_PACKET pPacket);
+#endif // __NET_IF_BLOCK_H__
+
diff --git a/drivers/staging/rt2860/common/rtmp_init.c b/drivers/staging/rt2860/common/rtmp_init.c
new file mode 100644
index 000000000000..563f2c5bb856
--- /dev/null
+++ b/drivers/staging/rt2860/common/rtmp_init.c
@@ -0,0 +1,3744 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_init.c
+
+	Abstract:
+	Miniport generic portion header file
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Paul Lin    2002-08-01    created
+    John Chang  2004-08-20    RT2561/2661 use scatter-gather scheme
+    Jan Lee  2006-09-15    RT2860. Change for 802.11n , EEPROM, Led, BA, HT.
+*/
+#include "../rt_config.h"
+#include 	"firmware.h"
+#include <linux/bitrev.h>
+
+UCHAR    BIT8[] = {0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80};
+ULONG    BIT32[] = {0x00000001, 0x00000002, 0x00000004, 0x00000008,
+					0x00000010, 0x00000020, 0x00000040, 0x00000080,
+					0x00000100, 0x00000200, 0x00000400, 0x00000800,
+					0x00001000, 0x00002000, 0x00004000, 0x00008000,
+					0x00010000, 0x00020000, 0x00040000, 0x00080000,
+					0x00100000, 0x00200000, 0x00400000, 0x00800000,
+					0x01000000, 0x02000000, 0x04000000, 0x08000000,
+					0x10000000, 0x20000000, 0x40000000, 0x80000000};
+
+char*   CipherName[] = {"none","wep64","wep128","TKIP","AES","CKIP64","CKIP128"};
+
+const unsigned short ccitt_16Table[] = {
+	0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
+	0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
+	0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
+	0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
+	0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
+	0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
+	0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
+	0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
+	0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
+	0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
+	0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
+	0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
+	0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
+	0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
+	0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
+	0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
+	0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
+	0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
+	0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
+	0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
+	0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
+	0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+	0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
+	0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
+	0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
+	0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
+	0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
+	0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
+	0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
+	0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
+	0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
+	0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
+};
+#define ByteCRC16(v, crc) \
+	(unsigned short)((crc << 8) ^  ccitt_16Table[((crc >> 8) ^ (v)) & 255])
+
+//
+// BBP register initialization set
+//
+REG_PAIR   BBPRegTable[] = {
+	{BBP_R65,		0x2C},		// fix rssi issue
+	{BBP_R66,		0x38},	// Also set this default value to pAd->BbpTuning.R66CurrentValue at initial
+	{BBP_R69,		0x12},
+	{BBP_R70,		0xa},	// BBP_R70 will change to 0x8 in ApStartUp and LinkUp for rt2860C, otherwise value is 0xa
+	{BBP_R73,		0x10},
+	{BBP_R81,		0x37},
+	{BBP_R82,		0x62},
+	{BBP_R83,		0x6A},
+	{BBP_R84,		0x99},	// 0x19 is for rt2860E and after. This is for extension channel overlapping IOT. 0x99 is for rt2860D and before
+	{BBP_R86,		0x00},	// middle range issue, Rory @2008-01-28
+	{BBP_R91,		0x04},	// middle range issue, Rory @2008-01-28
+	{BBP_R92,		0x00},	// middle range issue, Rory @2008-01-28
+	{BBP_R103,  	0x00}, 	// near range high-power issue, requested from Gary @2008-0528
+	{BBP_R105,		0x05},	// 0x05 is for rt2860E to turn on FEQ control. It is safe for rt2860D and before, because Bit 7:2 are reserved in rt2860D and before.
+};
+#define	NUM_BBP_REG_PARMS	(sizeof(BBPRegTable) / sizeof(REG_PAIR))
+
+//
+// RF register initialization set
+//
+
+//
+// ASIC register initialization sets
+//
+
+RTMP_REG_PAIR	MACRegTable[] =	{
+#if defined(HW_BEACON_OFFSET) && (HW_BEACON_OFFSET == 0x200)
+	{BCN_OFFSET0,			0xf8f0e8e0}, /* 0x3800(e0), 0x3A00(e8), 0x3C00(f0), 0x3E00(f8), 512B for each beacon */
+	{BCN_OFFSET1,			0x6f77d0c8}, /* 0x3200(c8), 0x3400(d0), 0x1DC0(77), 0x1BC0(6f), 512B for each beacon */
+#elif defined(HW_BEACON_OFFSET) && (HW_BEACON_OFFSET == 0x100)
+	{BCN_OFFSET0,			0xece8e4e0}, /* 0x3800, 0x3A00, 0x3C00, 0x3E00, 512B for each beacon */
+	{BCN_OFFSET1,			0xfcf8f4f0}, /* 0x3800, 0x3A00, 0x3C00, 0x3E00, 512B for each beacon */
+#else
+    #error You must re-calculate new value for BCN_OFFSET0 & BCN_OFFSET1 in MACRegTable[]!!!
+#endif // HW_BEACON_OFFSET //
+
+	{LEGACY_BASIC_RATE,		0x0000013f}, //  Basic rate set bitmap
+	{HT_BASIC_RATE,		0x00008003}, // Basic HT rate set , 20M, MCS=3, MM. Format is the same as in TXWI.
+	{MAC_SYS_CTRL,		0x00}, // 0x1004, , default Disable RX
+	{RX_FILTR_CFG,		0x17f97}, //0x1400  , RX filter control,
+	{BKOFF_SLOT_CFG,	0x209}, // default set short slot time, CC_DELAY_TIME should be 2
+	{TX_SW_CFG0,		0x0}, 		// Gary,2008-05-21 for CWC test
+	{TX_SW_CFG1,		0x80606}, // Gary,2006-08-23
+	{TX_LINK_CFG,		0x1020},		// Gary,2006-08-23
+	{TX_TIMEOUT_CFG,	0x000a2090},	// CCK has some problem. So increase timieout value. 2006-10-09// MArvek RT , Modify for 2860E ,2007-08-01
+	{MAX_LEN_CFG,		MAX_AGGREGATION_SIZE | 0x00001000},	// 0x3018, MAX frame length. Max PSDU = 16kbytes.
+	{LED_CFG,		0x7f031e46}, // Gary, 2006-08-23
+	{PBF_MAX_PCNT,			0x1F3FBF9F}, 	//0x1F3f7f9f},		//Jan, 2006/04/20
+	{TX_RTY_CFG,			0x47d01f0f},	// Jan, 2006/11/16, Set TxWI->ACK =0 in Probe Rsp Modify for 2860E ,2007-08-03
+	{AUTO_RSP_CFG,			0x00000013},	// Initial Auto_Responder, because QA will turn off Auto-Responder
+	{CCK_PROT_CFG,			0x05740003 /*0x01740003*/},	// Initial Auto_Responder, because QA will turn off Auto-Responder. And RTS threshold is enabled.
+	{OFDM_PROT_CFG,			0x05740003 /*0x01740003*/},	// Initial Auto_Responder, because QA will turn off Auto-Responder. And RTS threshold is enabled.
+	{GF20_PROT_CFG,			0x01744004},    // set 19:18 --> Short NAV for MIMO PS
+	{GF40_PROT_CFG,			0x03F44084},
+	{MM20_PROT_CFG,			0x01744004},
+#ifdef RT2860
+	{MM40_PROT_CFG,			0x03F54084},
+#endif // RT2860 //
+	{TXOP_CTRL_CFG,			0x0000583f, /*0x0000243f*/ /*0x000024bf*/},	//Extension channel backoff.
+	{TX_RTS_CFG,			0x00092b20},
+	{EXP_ACK_TIME,			0x002400ca},	// default value
+	{TXOP_HLDR_ET, 			0x00000002},
+
+	/* Jerry comments 2008/01/16: we use SIFS = 10us in CCK defaultly, but it seems that 10us
+		is too small for INTEL 2200bg card, so in MBSS mode, the delta time between beacon0
+		and beacon1 is SIFS (10us), so if INTEL 2200bg card connects to BSS0, the ping
+		will always lost. So we change the SIFS of CCK from 10us to 16us. */
+	{XIFS_TIME_CFG,			0x33a41010},
+	{PWR_PIN_CFG,			0x00000003},	// patch for 2880-E
+};
+
+
+#ifdef CONFIG_STA_SUPPORT
+RTMP_REG_PAIR	STAMACRegTable[] =	{
+	{WMM_AIFSN_CFG,		0x00002273},
+	{WMM_CWMIN_CFG,	0x00002344},
+	{WMM_CWMAX_CFG,	0x000034aa},
+};
+#endif // CONFIG_STA_SUPPORT //
+
+#define	NUM_MAC_REG_PARMS		(sizeof(MACRegTable) / sizeof(RTMP_REG_PAIR))
+#ifdef CONFIG_STA_SUPPORT
+#define	NUM_STA_MAC_REG_PARMS	(sizeof(STAMACRegTable) / sizeof(RTMP_REG_PAIR))
+#endif // CONFIG_STA_SUPPORT //
+
+
+// New 8k byte firmware size for RT3071/RT3072
+#define FIRMWAREIMAGE_MAX_LENGTH	0x2000
+#define FIRMWAREIMAGE_LENGTH		(sizeof (FirmwareImage) / sizeof(UCHAR))
+#define FIRMWARE_MAJOR_VERSION	0
+
+#define FIRMWAREIMAGEV1_LENGTH	0x1000
+#define FIRMWAREIMAGEV2_LENGTH	0x1000
+
+#ifdef RT2860
+#define FIRMWARE_MINOR_VERSION	2
+#endif // RT2860 //
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Allocate RTMP_ADAPTER data block and do some initialization
+
+	Arguments:
+		Adapter		Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	RTMPAllocAdapterBlock(
+	IN  PVOID	handle,
+	OUT	PRTMP_ADAPTER	*ppAdapter)
+{
+	PRTMP_ADAPTER	pAd;
+	NDIS_STATUS		Status;
+	INT 			index;
+	UCHAR			*pBeaconBuf = NULL;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> RTMPAllocAdapterBlock\n"));
+
+	*ppAdapter = NULL;
+
+	do
+	{
+		// Allocate RTMP_ADAPTER memory block
+		pBeaconBuf = kmalloc(MAX_BEACON_SIZE, MEM_ALLOC_FLAG);
+		if (pBeaconBuf == NULL)
+		{
+			Status = NDIS_STATUS_FAILURE;
+			DBGPRINT_ERR(("Failed to allocate memory - BeaconBuf!\n"));
+			break;
+		}
+
+		Status = AdapterBlockAllocateMemory(handle, (PVOID *)&pAd);
+		if (Status != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT_ERR(("Failed to allocate memory - ADAPTER\n"));
+			break;
+		}
+		pAd->BeaconBuf = pBeaconBuf;
+		printk("\n\n=== pAd = %p, size = %d ===\n\n", pAd, (UINT32)sizeof(RTMP_ADAPTER));
+
+
+		// Init spin locks
+		NdisAllocateSpinLock(&pAd->MgmtRingLock);
+#ifdef RT2860
+		NdisAllocateSpinLock(&pAd->RxRingLock);
+#endif // RT2860 //
+
+		for (index =0 ; index < NUM_OF_TX_RING; index++)
+		{
+			NdisAllocateSpinLock(&pAd->TxSwQueueLock[index]);
+			NdisAllocateSpinLock(&pAd->DeQueueLock[index]);
+			pAd->DeQueueRunning[index] = FALSE;
+		}
+
+		NdisAllocateSpinLock(&pAd->irq_lock);
+
+	} while (FALSE);
+
+	if ((Status != NDIS_STATUS_SUCCESS) && (pBeaconBuf))
+		kfree(pBeaconBuf);
+
+	*ppAdapter = pAd;
+
+	DBGPRINT_S(Status, ("<-- RTMPAllocAdapterBlock, Status=%x\n", Status));
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read initial Tx power per MCS and BW from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPReadTxPwrPerRate(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	ULONG		data, Adata, Gdata;
+	USHORT		i, value, value2;
+	INT			Apwrdelta, Gpwrdelta;
+	UCHAR		t1,t2,t3,t4;
+	BOOLEAN		bValid, bApwrdeltaMinus = TRUE, bGpwrdeltaMinus = TRUE;
+
+	//
+	// Get power delta for 20MHz and 40MHz.
+	//
+	DBGPRINT(RT_DEBUG_TRACE, ("Txpower per Rate\n"));
+	RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_DELTA, value2);
+	Apwrdelta = 0;
+	Gpwrdelta = 0;
+
+	if ((value2 & 0xff) != 0xff)
+	{
+		if ((value2 & 0x80))
+			Gpwrdelta = (value2&0xf);
+
+		if ((value2 & 0x40))
+			bGpwrdeltaMinus = FALSE;
+		else
+			bGpwrdeltaMinus = TRUE;
+	}
+	if ((value2 & 0xff00) != 0xff00)
+	{
+		if ((value2 & 0x8000))
+			Apwrdelta = ((value2&0xf00)>>8);
+
+		if ((value2 & 0x4000))
+			bApwrdeltaMinus = FALSE;
+		else
+			bApwrdeltaMinus = TRUE;
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("Gpwrdelta = %x, Apwrdelta = %x .\n", Gpwrdelta, Apwrdelta));
+
+	//
+	// Get Txpower per MCS for 20MHz in 2.4G.
+	//
+	for (i=0; i<5; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_2_4G + i*4, value);
+		data = value;
+		if (bApwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Apwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Apwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Apwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Apwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Apwrdelta)
+				t1 = (value&0xf)-(Apwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Apwrdelta)
+				t2 = ((value&0xf0)>>4)-(Apwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Apwrdelta)
+				t3 = ((value&0xf00)>>8)-(Apwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Apwrdelta)
+				t4 = ((value&0xf000)>>12)-(Apwrdelta);
+			else
+				t4 = 0;
+		}
+		Adata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+		if (bGpwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Gpwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Gpwrdelta)
+				t1 = (value&0xf)-(Gpwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Gpwrdelta)
+				t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Gpwrdelta)
+				t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Gpwrdelta)
+				t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+			else
+				t4 = 0;
+		}
+		Gdata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_2_4G + i*4 + 2, value);
+		if (bApwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Apwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Apwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Apwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Apwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Apwrdelta)
+				t1 = (value&0xf)-(Apwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Apwrdelta)
+				t2 = ((value&0xf0)>>4)-(Apwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Apwrdelta)
+				t3 = ((value&0xf00)>>8)-(Apwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Apwrdelta)
+				t4 = ((value&0xf000)>>12)-(Apwrdelta);
+			else
+				t4 = 0;
+		}
+		Adata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+		if (bGpwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Gpwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Gpwrdelta)
+				t1 = (value&0xf)-(Gpwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Gpwrdelta)
+				t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Gpwrdelta)
+				t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Gpwrdelta)
+				t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+			else
+				t4 = 0;
+		}
+		Gdata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+		data |= (value<<16);
+
+		pAd->Tx20MPwrCfgABand[i] = pAd->Tx40MPwrCfgABand[i] = Adata;
+		pAd->Tx20MPwrCfgGBand[i] = pAd->Tx40MPwrCfgGBand[i] = Gdata;
+
+		if (data != 0xffffffff)
+			RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, data);
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("20MHz BW, 2.4G band-%lx,  Adata = %lx,  Gdata = %lx \n", data, Adata, Gdata));
+	}
+
+	//
+	// Check this block is valid for 40MHz in 2.4G. If invalid, use parameter for 20MHz in 2.4G
+	//
+	bValid = TRUE;
+	for (i=0; i<6; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_2_4G + 2 + i*2, value);
+		if (((value & 0x00FF) == 0x00FF) || ((value & 0xFF00) == 0xFF00))
+		{
+			bValid = FALSE;
+			break;
+		}
+	}
+
+	//
+	// Get Txpower per MCS for 40MHz in 2.4G.
+	//
+	if (bValid)
+	{
+		for (i=0; i<4; i++)
+		{
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_2_4G + i*4, value);
+			if (bGpwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Gpwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Gpwrdelta)
+					t1 = (value&0xf)-(Gpwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Gpwrdelta)
+					t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Gpwrdelta)
+					t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Gpwrdelta)
+					t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+				else
+					t4 = 0;
+			}
+			Gdata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_2_4G + i*4 + 2, value);
+			if (bGpwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Gpwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Gpwrdelta)
+					t1 = (value&0xf)-(Gpwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Gpwrdelta)
+					t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Gpwrdelta)
+					t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Gpwrdelta)
+					t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+				else
+					t4 = 0;
+			}
+			Gdata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+
+			if (i == 0)
+				pAd->Tx40MPwrCfgGBand[i+1] = (pAd->Tx40MPwrCfgGBand[i+1] & 0x0000FFFF) | (Gdata & 0xFFFF0000);
+			else
+				pAd->Tx40MPwrCfgGBand[i+1] = Gdata;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("40MHz BW, 2.4G band, Gdata = %lx \n", Gdata));
+		}
+	}
+
+	//
+	// Check this block is valid for 20MHz in 5G. If invalid, use parameter for 20MHz in 2.4G
+	//
+	bValid = TRUE;
+	for (i=0; i<8; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_5G + 2 + i*2, value);
+		if (((value & 0x00FF) == 0x00FF) || ((value & 0xFF00) == 0xFF00))
+		{
+			bValid = FALSE;
+			break;
+		}
+	}
+
+	//
+	// Get Txpower per MCS for 20MHz in 5G.
+	//
+	if (bValid)
+	{
+		for (i=0; i<5; i++)
+		{
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_5G + i*4, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_5G + i*4 + 2, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+
+			if (i == 0)
+				pAd->Tx20MPwrCfgABand[i] = (pAd->Tx20MPwrCfgABand[i] & 0x0000FFFF) | (Adata & 0xFFFF0000);
+			else
+				pAd->Tx20MPwrCfgABand[i] = Adata;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("20MHz BW, 5GHz band, Adata = %lx \n", Adata));
+		}
+	}
+
+	//
+	// Check this block is valid for 40MHz in 5G. If invalid, use parameter for 20MHz in 2.4G
+	//
+	bValid = TRUE;
+	for (i=0; i<6; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_5G + 2 + i*2, value);
+		if (((value & 0x00FF) == 0x00FF) || ((value & 0xFF00) == 0xFF00))
+		{
+			bValid = FALSE;
+			break;
+		}
+	}
+
+	//
+	// Get Txpower per MCS for 40MHz in 5G.
+	//
+	if (bValid)
+	{
+		for (i=0; i<4; i++)
+		{
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_5G + i*4, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_5G + i*4 + 2, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+
+			if (i == 0)
+				pAd->Tx40MPwrCfgABand[i+1] = (pAd->Tx40MPwrCfgABand[i+1] & 0x0000FFFF) | (Adata & 0xFFFF0000);
+			else
+				pAd->Tx40MPwrCfgABand[i+1] = Adata;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("40MHz BW, 5GHz band, Adata = %lx \n", Adata));
+		}
+	}
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read initial channel power parameters from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPReadChannelPwr(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR				i, choffset;
+	EEPROM_TX_PWR_STRUC	    Power;
+	EEPROM_TX_PWR_STRUC	    Power2;
+
+	// Read Tx power value for all channels
+	// Value from 1 - 0x7f. Default value is 24.
+	// Power value : 2.4G 0x00 (0) ~ 0x1F (31)
+	//             : 5.5G 0xF9 (-7) ~ 0x0F (15)
+
+	// 0. 11b/g, ch1 - ch 14
+	for (i = 0; i < 7; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_G_TX_PWR_OFFSET + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_G_TX2_PWR_OFFSET + i * 2, Power2.word);
+		pAd->TxPower[i * 2].Channel = i * 2 + 1;
+		pAd->TxPower[i * 2 + 1].Channel = i * 2 + 2;
+
+		if ((Power.field.Byte0 > 31) || (Power.field.Byte0 < 0))
+			pAd->TxPower[i * 2].Power = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 > 31) || (Power.field.Byte1 < 0))
+			pAd->TxPower[i * 2 + 1].Power = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2 + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 > 31) || (Power2.field.Byte0 < 0))
+			pAd->TxPower[i * 2].Power2 = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 > 31) || (Power2.field.Byte1 < 0))
+			pAd->TxPower[i * 2 + 1].Power2 = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2 + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 1. U-NII lower/middle band: 36, 38, 40; 44, 46, 48; 52, 54, 56; 60, 62, 64 (including central frequency in BW 40MHz)
+	// 1.1 Fill up channel
+	choffset = 14;
+	for (i = 0; i < 4; i++)
+	{
+		pAd->TxPower[3 * i + choffset + 0].Channel	= 36 + i * 8 + 0;
+		pAd->TxPower[3 * i + choffset + 0].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 0].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 1].Channel	= 36 + i * 8 + 2;
+		pAd->TxPower[3 * i + choffset + 1].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 1].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 2].Channel	= 36 + i * 8 + 4;
+		pAd->TxPower[3 * i + choffset + 2].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 2].Power2	= DEFAULT_RF_TX_POWER;
+	}
+
+	// 1.2 Fill up power
+	for (i = 0; i < 6; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + i * 2, Power2.word);
+
+		if ((Power.field.Byte0 < 16) && (Power.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 16) && (Power.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 16) && (Power2.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 16) && (Power2.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 2. HipperLAN 2 100, 102 ,104; 108, 110, 112; 116, 118, 120; 124, 126, 128; 132, 134, 136; 140 (including central frequency in BW 40MHz)
+	// 2.1 Fill up channel
+	choffset = 14 + 12;
+	for (i = 0; i < 5; i++)
+	{
+		pAd->TxPower[3 * i + choffset + 0].Channel	= 100 + i * 8 + 0;
+		pAd->TxPower[3 * i + choffset + 0].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 0].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 1].Channel	= 100 + i * 8 + 2;
+		pAd->TxPower[3 * i + choffset + 1].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 1].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 2].Channel	= 100 + i * 8 + 4;
+		pAd->TxPower[3 * i + choffset + 2].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 2].Power2	= DEFAULT_RF_TX_POWER;
+	}
+	pAd->TxPower[3 * 5 + choffset + 0].Channel		= 140;
+	pAd->TxPower[3 * 5 + choffset + 0].Power		= DEFAULT_RF_TX_POWER;
+	pAd->TxPower[3 * 5 + choffset + 0].Power2		= DEFAULT_RF_TX_POWER;
+
+	// 2.2 Fill up power
+	for (i = 0; i < 8; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2, Power2.word);
+
+		if ((Power.field.Byte0 < 16) && (Power.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 16) && (Power.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 16) && (Power2.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 16) && (Power2.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 3. U-NII upper band: 149, 151, 153; 157, 159, 161; 165 (including central frequency in BW 40MHz)
+	// 3.1 Fill up channel
+	choffset = 14 + 12 + 16;
+	for (i = 0; i < 2; i++)
+	{
+		pAd->TxPower[3 * i + choffset + 0].Channel	= 149 + i * 8 + 0;
+		pAd->TxPower[3 * i + choffset + 0].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 0].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 1].Channel	= 149 + i * 8 + 2;
+		pAd->TxPower[3 * i + choffset + 1].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 1].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 2].Channel	= 149 + i * 8 + 4;
+		pAd->TxPower[3 * i + choffset + 2].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 2].Power2	= DEFAULT_RF_TX_POWER;
+	}
+	pAd->TxPower[3 * 2 + choffset + 0].Channel		= 165;
+	pAd->TxPower[3 * 2 + choffset + 0].Power		= DEFAULT_RF_TX_POWER;
+	pAd->TxPower[3 * 2 + choffset + 0].Power2		= DEFAULT_RF_TX_POWER;
+
+	// 3.2 Fill up power
+	for (i = 0; i < 4; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2, Power2.word);
+
+		if ((Power.field.Byte0 < 16) && (Power.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 16) && (Power.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 16) && (Power2.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 16) && (Power2.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 4. Print and Debug
+	choffset = 14 + 12 + 16 + 7;
+
+
+#if 0
+	// Init the 802.11j channel number for TX channel power
+	// 0. 20MHz
+	for (i = 0; i < 3; i++)
+	{
+		pAd->TxPower11J[i].Channel = 8 + i * 4;
+		pAd->TxPower11J[i].BW = BW_20;
+	}
+
+	for (i = 0; i < 4; i++)
+	{
+		pAd->TxPower11J[i + 3].Channel = 34 + i * 4;
+		pAd->TxPower11J[i + 3].BW = BW_20;
+	}
+
+	for (i = 0; i < 4; i++)
+	{
+		pAd->TxPower11J[i + 7].Channel = 184 + i * 4;
+		pAd->TxPower11J[i + 7].BW = BW_20;
+	}
+
+	// 0. 10MHz
+	for (i = 0; i < 2; i++)
+	{
+		pAd->TxPower11J[i + 11].Channel = 7 + i;
+		pAd->TxPower11J[i + 11].BW = BW_10;
+	}
+	pAd->TxPower11J[13].Channel = 11;
+	pAd->TxPower11J[13].BW = BW_10;
+
+	for (i = 0; i < 3; i++)
+	{
+		pAd->TxPower11J[i + 14].Channel = 183 + i;
+		pAd->TxPower11J[i + 14].BW= BW_10;
+	}
+
+	for (i = 0; i < 3; i++)
+	{
+		pAd->TxPower11J[i + 17].Channel = 187 + i;
+		pAd->TxPower11J[i + 17].BW = BW_10;
+	}
+	for (i = 0; i < 10; i++)
+	{
+		Power.word = RTMP_EEPROM_READ16(pAd, EEPROM_Japan_TX_PWR_OFFSET + i * 2);
+		Power2.word = RTMP_EEPROM_READ16(pAd, EEPROM_Japan_TX2_PWR_OFFSET + i * 2);
+
+		if ((Power.field.Byte0 < 36) && (Power.field.Byte0 > -6))
+			pAd->TxPower11J[i * 2].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 36) && (Power.field.Byte1 > -6))
+			pAd->TxPower11J[i * 2 + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 36) && (Power2.field.Byte0 > -6))
+			pAd->TxPower11J[i * 2].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 36) && (Power2.field.Byte1 > -6))
+			pAd->TxPower11J[i * 2 + 1].Power2 = Power2.field.Byte1;
+	}
+#endif
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read the following from the registry
+		1. All the parameters
+		2. NetworkAddres
+
+	Arguments:
+		Adapter						Pointer to our adapter
+		WrapperConfigurationContext	For use by NdisOpenConfiguration
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_RESOURCES
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	NICReadRegParameters(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	NDIS_HANDLE			WrapperConfigurationContext
+	)
+{
+	NDIS_STATUS						Status = NDIS_STATUS_SUCCESS;
+	DBGPRINT_S(Status, ("<-- NICReadRegParameters, Status=%x\n", Status));
+	return Status;
+}
+
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read initial parameters from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	NICReadEEPROMParameters(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			mac_addr)
+{
+	UINT32			data = 0;
+	USHORT			i, value, value2;
+	UCHAR			TmpPhy;
+	EEPROM_TX_PWR_STRUC	    Power;
+	EEPROM_VERSION_STRUC    Version;
+	EEPROM_ANTENNA_STRUC	Antenna;
+	EEPROM_NIC_CONFIG2_STRUC    NicConfig2;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICReadEEPROMParameters\n"));
+
+	// Init EEPROM Address Number, before access EEPROM; if 93c46, EEPROMAddressNum=6, else if 93c66, EEPROMAddressNum=8
+	RTMP_IO_READ32(pAd, E2PROM_CSR, &data);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> E2PROM_CSR = 0x%x\n", data));
+
+	if((data & 0x30) == 0)
+		pAd->EEPROMAddressNum = 6;		// 93C46
+	else if((data & 0x30) == 0x10)
+		pAd->EEPROMAddressNum = 8;     // 93C66
+	else
+		pAd->EEPROMAddressNum = 8;     // 93C86
+	DBGPRINT(RT_DEBUG_TRACE, ("--> EEPROMAddressNum = %d\n", pAd->EEPROMAddressNum ));
+
+	// RT2860 MAC no longer auto load MAC address from E2PROM. Driver has to intialize
+	// MAC address registers according to E2PROM setting
+	if (mac_addr == NULL ||
+		strlen(mac_addr) != 17 ||
+		mac_addr[2] != ':'  || mac_addr[5] != ':'  || mac_addr[8] != ':' ||
+		mac_addr[11] != ':' || mac_addr[14] != ':')
+	{
+		USHORT  Addr01,Addr23,Addr45 ;
+
+		RT28xx_EEPROM_READ16(pAd, 0x04, Addr01);
+		RT28xx_EEPROM_READ16(pAd, 0x06, Addr23);
+		RT28xx_EEPROM_READ16(pAd, 0x08, Addr45);
+
+		pAd->PermanentAddress[0] = (UCHAR)(Addr01 & 0xff);
+		pAd->PermanentAddress[1] = (UCHAR)(Addr01 >> 8);
+		pAd->PermanentAddress[2] = (UCHAR)(Addr23 & 0xff);
+		pAd->PermanentAddress[3] = (UCHAR)(Addr23 >> 8);
+		pAd->PermanentAddress[4] = (UCHAR)(Addr45 & 0xff);
+		pAd->PermanentAddress[5] = (UCHAR)(Addr45 >> 8);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Initialize MAC Address from E2PROM \n"));
+	}
+	else
+	{
+		INT		j;
+		PUCHAR	macptr;
+
+		macptr = mac_addr;
+
+		for (j=0; j<MAC_ADDR_LEN; j++)
+		{
+			AtoH(macptr, &pAd->PermanentAddress[j], 1);
+			macptr=macptr+3;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Initialize MAC Address from module parameter \n"));
+	}
+
+
+	{
+		//more conveninet to test mbssid, so ap's bssid &0xf1
+		if (pAd->PermanentAddress[0] == 0xff)
+			pAd->PermanentAddress[0] = RandomByte(pAd)&0xf8;
+
+		//if (pAd->PermanentAddress[5] == 0xff)
+		//	pAd->PermanentAddress[5] = RandomByte(pAd)&0xf8;
+
+		DBGPRINT_RAW(RT_DEBUG_TRACE,("E2PROM MAC: =%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pAd->PermanentAddress[0], pAd->PermanentAddress[1],
+			pAd->PermanentAddress[2], pAd->PermanentAddress[3],
+			pAd->PermanentAddress[4], pAd->PermanentAddress[5]));
+		if (pAd->bLocalAdminMAC == FALSE)
+		{
+			MAC_DW0_STRUC csr2;
+			MAC_DW1_STRUC csr3;
+			COPY_MAC_ADDR(pAd->CurrentAddress, pAd->PermanentAddress);
+			csr2.field.Byte0 = pAd->CurrentAddress[0];
+			csr2.field.Byte1 = pAd->CurrentAddress[1];
+			csr2.field.Byte2 = pAd->CurrentAddress[2];
+			csr2.field.Byte3 = pAd->CurrentAddress[3];
+			RTMP_IO_WRITE32(pAd, MAC_ADDR_DW0, csr2.word);
+			csr3.word = 0;
+			csr3.field.Byte4 = pAd->CurrentAddress[4];
+			csr3.field.Byte5 = pAd->CurrentAddress[5];
+			csr3.field.U2MeMask = 0xff;
+			RTMP_IO_WRITE32(pAd, MAC_ADDR_DW1, csr3.word);
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("E2PROM MAC: =%02x:%02x:%02x:%02x:%02x:%02x\n",
+				pAd->PermanentAddress[0], pAd->PermanentAddress[1],
+				pAd->PermanentAddress[2], pAd->PermanentAddress[3],
+				pAd->PermanentAddress[4], pAd->PermanentAddress[5]));
+		}
+	}
+
+	// if not return early. cause fail at emulation.
+	// Init the channel number for TX channel power
+	RTMPReadChannelPwr(pAd);
+
+	// if E2PROM version mismatch with driver's expectation, then skip
+	// all subsequent E2RPOM retieval and set a system error bit to notify GUI
+	RT28xx_EEPROM_READ16(pAd, EEPROM_VERSION_OFFSET, Version.word);
+	pAd->EepromVersion = Version.field.Version + Version.field.FaeReleaseNumber * 256;
+	DBGPRINT(RT_DEBUG_TRACE, ("E2PROM: Version = %d, FAE release #%d\n", Version.field.Version, Version.field.FaeReleaseNumber));
+
+	if (Version.field.Version > VALID_EEPROM_VERSION)
+	{
+		DBGPRINT_ERR(("E2PROM: WRONG VERSION 0x%x, should be %d\n",Version.field.Version, VALID_EEPROM_VERSION));
+		/*pAd->SystemErrorBitmap |= 0x00000001;
+
+		// hard-code default value when no proper E2PROM installed
+		pAd->bAutoTxAgcA = FALSE;
+		pAd->bAutoTxAgcG = FALSE;
+
+		// Default the channel power
+		for (i = 0; i < MAX_NUM_OF_CHANNELS; i++)
+			pAd->TxPower[i].Power = DEFAULT_RF_TX_POWER;
+
+		// Default the channel power
+		for (i = 0; i < MAX_NUM_OF_11JCHANNELS; i++)
+			pAd->TxPower11J[i].Power = DEFAULT_RF_TX_POWER;
+
+		for(i = 0; i < NUM_EEPROM_BBP_PARMS; i++)
+			pAd->EEPROMDefaultValue[i] = 0xffff;
+		return;  */
+	}
+
+	// Read BBP default value from EEPROM and store to array(EEPROMDefaultValue) in pAd
+	RT28xx_EEPROM_READ16(pAd, EEPROM_NIC1_OFFSET, value);
+	pAd->EEPROMDefaultValue[0] = value;
+
+	RT28xx_EEPROM_READ16(pAd, EEPROM_NIC2_OFFSET, value);
+	pAd->EEPROMDefaultValue[1] = value;
+
+	RT28xx_EEPROM_READ16(pAd, 0x38, value);	// Country Region
+	pAd->EEPROMDefaultValue[2] = value;
+
+	for(i = 0; i < 8; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_BBP_BASE_OFFSET + i*2, value);
+		pAd->EEPROMDefaultValue[i+3] = value;
+	}
+
+	// We have to parse NIC configuration 0 at here.
+	// If TSSI did not have preloaded value, it should reset the TxAutoAgc to false
+	// Therefore, we have to read TxAutoAgc control beforehand.
+	// Read Tx AGC control bit
+	Antenna.word = pAd->EEPROMDefaultValue[0];
+	if (Antenna.word == 0xFFFF)
+	{
+		Antenna.word = 0;
+		Antenna.field.RfIcType = RFIC_2820;
+		Antenna.field.TxPath = 1;
+		Antenna.field.RxPath = 2;
+		DBGPRINT(RT_DEBUG_WARN, ("E2PROM error, hard code as 0x%04x\n", Antenna.word));
+	}
+
+	// Choose the desired Tx&Rx stream.
+	if ((pAd->CommonCfg.TxStream == 0) || (pAd->CommonCfg.TxStream > Antenna.field.TxPath))
+		pAd->CommonCfg.TxStream = Antenna.field.TxPath;
+
+	if ((pAd->CommonCfg.RxStream == 0) || (pAd->CommonCfg.RxStream > Antenna.field.RxPath))
+	{
+		pAd->CommonCfg.RxStream = Antenna.field.RxPath;
+
+		if ((pAd->MACVersion < RALINK_2883_VERSION) &&
+			(pAd->CommonCfg.RxStream > 2))
+		{
+			// only 2 Rx streams for RT2860 series
+			pAd->CommonCfg.RxStream = 2;
+		}
+	}
+
+	// 3*3
+	// read value from EEPROM and set them to CSR174 ~ 177 in chain0 ~ chain2
+	// yet implement
+	for(i=0; i<3; i++)
+	{
+	}
+
+	NicConfig2.word = pAd->EEPROMDefaultValue[1];
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		NicConfig2.word = 0;
+		if ((NicConfig2.word & 0x00ff) == 0xff)
+		{
+			NicConfig2.word &= 0xff00;
+		}
+
+		if ((NicConfig2.word >> 8) == 0xff)
+		{
+			NicConfig2.word &= 0x00ff;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	if (NicConfig2.field.DynamicTxAgcControl == 1)
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = TRUE;
+	else
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = FALSE;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("NICReadEEPROMParameters: RxPath = %d, TxPath = %d\n", Antenna.field.RxPath, Antenna.field.TxPath));
+
+	// Save the antenna for future use
+	pAd->Antenna.word = Antenna.word;
+
+	//
+	// Reset PhyMode if we don't support 802.11a
+	// Only RFIC_2850 & RFIC_2750 support 802.11a
+	//
+	if ((Antenna.field.RfIcType != RFIC_2850) && (Antenna.field.RfIcType != RFIC_2750))
+	{
+		if ((pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED) ||
+			(pAd->CommonCfg.PhyMode == PHY_11A))
+			pAd->CommonCfg.PhyMode = PHY_11BG_MIXED;
+#ifdef DOT11_N_SUPPORT
+		else if ((pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED)	||
+				 (pAd->CommonCfg.PhyMode == PHY_11AN_MIXED) 	||
+				 (pAd->CommonCfg.PhyMode == PHY_11AGN_MIXED) 	||
+				 (pAd->CommonCfg.PhyMode == PHY_11N_5G))
+			pAd->CommonCfg.PhyMode = PHY_11BGN_MIXED;
+#endif // DOT11_N_SUPPORT //
+	}
+
+	// Read TSSI reference and TSSI boundary for temperature compensation. This is ugly
+	// 0. 11b/g
+	{
+		/* these are tempature reference value (0x00 ~ 0xFE)
+		   ex: 0x00 0x15 0x25 0x45 0x88 0xA0 0xB5 0xD0 0xF0
+		   TssiPlusBoundaryG [4] [3] [2] [1] [0] (smaller) +
+		   TssiMinusBoundaryG[0] [1] [2] [3] [4] (larger) */
+		RT28xx_EEPROM_READ16(pAd, 0x6E, Power.word);
+		pAd->TssiMinusBoundaryG[4] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryG[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x70, Power.word);
+		pAd->TssiMinusBoundaryG[2] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryG[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x72, Power.word);
+		pAd->TssiRefG   = Power.field.Byte0; /* reference value [0] */
+		pAd->TssiPlusBoundaryG[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x74, Power.word);
+		pAd->TssiPlusBoundaryG[2] = Power.field.Byte0;
+		pAd->TssiPlusBoundaryG[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x76, Power.word);
+		pAd->TssiPlusBoundaryG[4] = Power.field.Byte0;
+		pAd->TxAgcStepG = Power.field.Byte1;
+		pAd->TxAgcCompensateG = 0;
+		pAd->TssiMinusBoundaryG[0] = pAd->TssiRefG;
+		pAd->TssiPlusBoundaryG[0]  = pAd->TssiRefG;
+
+		// Disable TxAgc if the based value is not right
+		if (pAd->TssiRefG == 0xff)
+			pAd->bAutoTxAgcG = FALSE;
+
+		DBGPRINT(RT_DEBUG_TRACE,("E2PROM: G Tssi[-4 .. +4] = %d %d %d %d - %d -%d %d %d %d, step=%d, tuning=%d\n",
+			pAd->TssiMinusBoundaryG[4], pAd->TssiMinusBoundaryG[3], pAd->TssiMinusBoundaryG[2], pAd->TssiMinusBoundaryG[1],
+			pAd->TssiRefG,
+			pAd->TssiPlusBoundaryG[1], pAd->TssiPlusBoundaryG[2], pAd->TssiPlusBoundaryG[3], pAd->TssiPlusBoundaryG[4],
+			pAd->TxAgcStepG, pAd->bAutoTxAgcG));
+	}
+	// 1. 11a
+	{
+		RT28xx_EEPROM_READ16(pAd, 0xD4, Power.word);
+		pAd->TssiMinusBoundaryA[4] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryA[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xD6, Power.word);
+		pAd->TssiMinusBoundaryA[2] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryA[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xD8, Power.word);
+		pAd->TssiRefA   = Power.field.Byte0;
+		pAd->TssiPlusBoundaryA[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xDA, Power.word);
+		pAd->TssiPlusBoundaryA[2] = Power.field.Byte0;
+		pAd->TssiPlusBoundaryA[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xDC, Power.word);
+		pAd->TssiPlusBoundaryA[4] = Power.field.Byte0;
+		pAd->TxAgcStepA = Power.field.Byte1;
+		pAd->TxAgcCompensateA = 0;
+		pAd->TssiMinusBoundaryA[0] = pAd->TssiRefA;
+		pAd->TssiPlusBoundaryA[0]  = pAd->TssiRefA;
+
+		// Disable TxAgc if the based value is not right
+		if (pAd->TssiRefA == 0xff)
+			pAd->bAutoTxAgcA = FALSE;
+
+		DBGPRINT(RT_DEBUG_TRACE,("E2PROM: A Tssi[-4 .. +4] = %d %d %d %d - %d -%d %d %d %d, step=%d, tuning=%d\n",
+			pAd->TssiMinusBoundaryA[4], pAd->TssiMinusBoundaryA[3], pAd->TssiMinusBoundaryA[2], pAd->TssiMinusBoundaryA[1],
+			pAd->TssiRefA,
+			pAd->TssiPlusBoundaryA[1], pAd->TssiPlusBoundaryA[2], pAd->TssiPlusBoundaryA[3], pAd->TssiPlusBoundaryA[4],
+			pAd->TxAgcStepA, pAd->bAutoTxAgcA));
+	}
+	pAd->BbpRssiToDbmDelta = 0x0;
+
+	// Read frequency offset setting for RF
+	RT28xx_EEPROM_READ16(pAd, EEPROM_FREQ_OFFSET, value);
+	if ((value & 0x00FF) != 0x00FF)
+		pAd->RfFreqOffset = (ULONG) (value & 0x00FF);
+	else
+		pAd->RfFreqOffset = 0;
+	DBGPRINT(RT_DEBUG_TRACE, ("E2PROM: RF FreqOffset=0x%lx \n", pAd->RfFreqOffset));
+
+	//CountryRegion byte offset (38h)
+	value = pAd->EEPROMDefaultValue[2] >> 8;		// 2.4G band
+	value2 = pAd->EEPROMDefaultValue[2] & 0x00FF;	// 5G band
+
+	if ((value <= REGION_MAXIMUM_BG_BAND) && (value2 <= REGION_MAXIMUM_A_BAND))
+	{
+		pAd->CommonCfg.CountryRegion = ((UCHAR) value) | 0x80;
+		pAd->CommonCfg.CountryRegionForABand = ((UCHAR) value2) | 0x80;
+		TmpPhy = pAd->CommonCfg.PhyMode;
+		pAd->CommonCfg.PhyMode = 0xff;
+		RTMPSetPhyMode(pAd, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+	}
+
+	//
+	// Get RSSI Offset on EEPROM 0x9Ah & 0x9Ch.
+	// The valid value are (-10 ~ 10)
+	//
+	RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_BG_OFFSET, value);
+	pAd->BGRssiOffset0 = value & 0x00ff;
+	pAd->BGRssiOffset1 = (value >> 8);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_BG_OFFSET+2, value);
+	pAd->BGRssiOffset2 = value & 0x00ff;
+	pAd->ALNAGain1 = (value >> 8);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LNA_OFFSET, value);
+	pAd->BLNAGain = value & 0x00ff;
+	pAd->ALNAGain0 = (value >> 8);
+
+	// Validate 11b/g RSSI_0 offset.
+	if ((pAd->BGRssiOffset0 < -10) || (pAd->BGRssiOffset0 > 10))
+		pAd->BGRssiOffset0 = 0;
+
+	// Validate 11b/g RSSI_1 offset.
+	if ((pAd->BGRssiOffset1 < -10) || (pAd->BGRssiOffset1 > 10))
+		pAd->BGRssiOffset1 = 0;
+
+	// Validate 11b/g RSSI_2 offset.
+	if ((pAd->BGRssiOffset2 < -10) || (pAd->BGRssiOffset2 > 10))
+		pAd->BGRssiOffset2 = 0;
+
+	RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_A_OFFSET, value);
+	pAd->ARssiOffset0 = value & 0x00ff;
+	pAd->ARssiOffset1 = (value >> 8);
+	RT28xx_EEPROM_READ16(pAd, (EEPROM_RSSI_A_OFFSET+2), value);
+	pAd->ARssiOffset2 = value & 0x00ff;
+	pAd->ALNAGain2 = (value >> 8);
+
+	if (((UCHAR)pAd->ALNAGain1 == 0xFF) || (pAd->ALNAGain1 == 0x00))
+		pAd->ALNAGain1 = pAd->ALNAGain0;
+	if (((UCHAR)pAd->ALNAGain2 == 0xFF) || (pAd->ALNAGain2 == 0x00))
+		pAd->ALNAGain2 = pAd->ALNAGain0;
+
+	// Validate 11a RSSI_0 offset.
+	if ((pAd->ARssiOffset0 < -10) || (pAd->ARssiOffset0 > 10))
+		pAd->ARssiOffset0 = 0;
+
+	// Validate 11a RSSI_1 offset.
+	if ((pAd->ARssiOffset1 < -10) || (pAd->ARssiOffset1 > 10))
+		pAd->ARssiOffset1 = 0;
+
+	//Validate 11a RSSI_2 offset.
+	if ((pAd->ARssiOffset2 < -10) || (pAd->ARssiOffset2 > 10))
+		pAd->ARssiOffset2 = 0;
+
+	//
+	// Get LED Setting.
+	//
+	RT28xx_EEPROM_READ16(pAd, 0x3a, value);
+	pAd->LedCntl.word = (value&0xff00) >> 8;
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LED1_OFFSET, value);
+	pAd->Led1 = value;
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LED2_OFFSET, value);
+	pAd->Led2 = value;
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LED3_OFFSET, value);
+	pAd->Led3 = value;
+
+	RTMPReadTxPwrPerRate(pAd);
+
+#ifdef SINGLE_SKU
+	//pAd->CommonCfg.DefineMaxTxPwr = RTMP_EEPROM_READ16(pAd, EEPROM_DEFINE_MAX_TXPWR);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_DEFINE_MAX_TXPWR, pAd->CommonCfg.DefineMaxTxPwr);
+#endif // SINGLE_SKU //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICReadEEPROMParameters\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set default value from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	NICInitAsicFromEEPROM(
+	IN	PRTMP_ADAPTER	pAd)
+{
+#ifdef CONFIG_STA_SUPPORT
+	UINT32					data = 0;
+	UCHAR	BBPR1 = 0;
+#endif // CONFIG_STA_SUPPORT //
+	USHORT					i;
+	EEPROM_ANTENNA_STRUC	Antenna;
+	EEPROM_NIC_CONFIG2_STRUC    NicConfig2;
+	UCHAR	BBPR3 = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitAsicFromEEPROM\n"));
+	for(i = 3; i < NUM_EEPROM_BBP_PARMS; i++)
+	{
+		UCHAR BbpRegIdx, BbpValue;
+
+		if ((pAd->EEPROMDefaultValue[i] != 0xFFFF) && (pAd->EEPROMDefaultValue[i] != 0))
+		{
+			BbpRegIdx = (UCHAR)(pAd->EEPROMDefaultValue[i] >> 8);
+			BbpValue  = (UCHAR)(pAd->EEPROMDefaultValue[i] & 0xff);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BbpRegIdx, BbpValue);
+		}
+	}
+
+	Antenna.word = pAd->Antenna.word;
+	pAd->Mlme.RealRxPath = (UCHAR) Antenna.field.RxPath;
+	pAd->RfIcType = (UCHAR) Antenna.field.RfIcType;
+
+	NicConfig2.word = pAd->EEPROMDefaultValue[1];
+
+
+	// Save the antenna for future use
+	pAd->NicConfig2.word = NicConfig2.word;
+
+	//
+	// Send LED Setting to MCU.
+	//
+	if (pAd->LedCntl.word == 0xFF)
+	{
+		pAd->LedCntl.word = 0x01;
+		pAd->Led1 = 0x5555;
+		pAd->Led2 = 0x2221;
+
+#ifdef RT2860
+		pAd->Led3 = 0xA9F8;
+#endif // RT2860 //
+	}
+
+	AsicSendCommandToMcu(pAd, 0x52, 0xff, (UCHAR)pAd->Led1, (UCHAR)(pAd->Led1 >> 8));
+	AsicSendCommandToMcu(pAd, 0x53, 0xff, (UCHAR)pAd->Led2, (UCHAR)(pAd->Led2 >> 8));
+	AsicSendCommandToMcu(pAd, 0x54, 0xff, (UCHAR)pAd->Led3, (UCHAR)(pAd->Led3 >> 8));
+    pAd->LedIndicatorStregth = 0xFF;
+    RTMPSetSignalLED(pAd, -100);	// Force signal strength Led to be turned off, before link up
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Read Hardware controlled Radio state enable bit
+		if (NicConfig2.field.HardwareRadioControl == 1)
+		{
+			pAd->StaCfg.bHardwareRadio = TRUE;
+
+			// Read GPIO pin2 as Hardware controlled radio state
+			RTMP_IO_READ32(pAd, GPIO_CTRL_CFG, &data);
+			if ((data & 0x04) == 0)
+			{
+				pAd->StaCfg.bHwRadio = FALSE;
+				pAd->StaCfg.bRadio = FALSE;
+				RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+			}
+		}
+		else
+			pAd->StaCfg.bHardwareRadio = FALSE;
+
+		if (pAd->StaCfg.bRadio == FALSE)
+		{
+			RTMPSetLED(pAd, LED_RADIO_OFF);
+		}
+		else
+		{
+			RTMPSetLED(pAd, LED_RADIO_ON);
+#ifdef RT2860
+			AsicSendCommandToMcu(pAd, 0x30, 0xff, 0xff, 0x02);
+			AsicSendCommandToMcu(pAd, 0x31, PowerWakeCID, 0x00, 0x00);
+			// 2-1. wait command ok.
+			AsicCheckCommanOk(pAd, PowerWakeCID);
+#endif // RT2860 //
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Turn off patching for cardbus controller
+	if (NicConfig2.field.CardbusAcceleration == 1)
+	{
+	}
+
+	if (NicConfig2.field.DynamicTxAgcControl == 1)
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = TRUE;
+	else
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = FALSE;
+	//
+	// Since BBP has been progamed, to make sure BBP setting will be
+	// upate inside of AsicAntennaSelect, so reset to UNKNOWN_BAND!!
+	//
+	pAd->CommonCfg.BandState = UNKNOWN_BAND;
+
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
+	BBPR3 &= (~0x18);
+	if(pAd->Antenna.field.RxPath == 3)
+	{
+		BBPR3 |= (0x10);
+	}
+	else if(pAd->Antenna.field.RxPath == 2)
+	{
+		BBPR3 |= (0x8);
+	}
+	else if(pAd->Antenna.field.RxPath == 1)
+	{
+		BBPR3 |= (0x0);
+	}
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPR3);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Handle the difference when 1T
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BBPR1);
+		if(pAd->Antenna.field.TxPath == 1)
+		{
+		BBPR1 &= (~0x18);
+		}
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BBPR1);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Use Hw Radio Control Pin=%d; if used Pin=%d;\n", pAd->CommonCfg.bHardwareRadio, pAd->CommonCfg.bHardwareRadio));
+	}
+#endif // CONFIG_STA_SUPPORT //
+	DBGPRINT(RT_DEBUG_TRACE, ("TxPath = %d, RxPath = %d, RFIC=%d, Polar+LED mode=%x\n", pAd->Antenna.field.TxPath, pAd->Antenna.field.RxPath, pAd->RfIcType, pAd->LedCntl.word));
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitAsicFromEEPROM\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize NIC hardware
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	NICInitializeAdapter(
+	IN	PRTMP_ADAPTER	pAd,
+	IN   BOOLEAN    bHardReset)
+{
+	NDIS_STATUS     Status = NDIS_STATUS_SUCCESS;
+	WPDMA_GLO_CFG_STRUC	GloCfg;
+#ifdef RT2860
+	UINT32			Value;
+	DELAY_INT_CFG_STRUC	IntCfg;
+#endif // RT2860 //
+	ULONG	i =0, j=0;
+	AC_TXOP_CSR0_STRUC	csr0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitializeAdapter\n"));
+
+	// 3. Set DMA global configuration except TX_DMA_EN and RX_DMA_EN bits:
+retry:
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0)  && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		RTMPusecDelay(1000);
+		i++;
+	}while ( i<100);
+	DBGPRINT(RT_DEBUG_TRACE, ("<== DMA offset 0x208 = 0x%x\n", GloCfg.word));
+	GloCfg.word &= 0xff0;
+	GloCfg.field.EnTXWriteBackDDONE =1;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+	// Record HW Beacon offset
+	pAd->BeaconOffset[0] = HW_BEACON_BASE0;
+	pAd->BeaconOffset[1] = HW_BEACON_BASE1;
+	pAd->BeaconOffset[2] = HW_BEACON_BASE2;
+	pAd->BeaconOffset[3] = HW_BEACON_BASE3;
+	pAd->BeaconOffset[4] = HW_BEACON_BASE4;
+	pAd->BeaconOffset[5] = HW_BEACON_BASE5;
+	pAd->BeaconOffset[6] = HW_BEACON_BASE6;
+	pAd->BeaconOffset[7] = HW_BEACON_BASE7;
+
+	//
+	// write all shared Ring's base address into ASIC
+	//
+
+	// asic simulation sequence put this ahead before loading firmware.
+	// pbf hardware reset
+#ifdef RT2860
+	RTMP_IO_WRITE32(pAd, WPDMA_RST_IDX, 0x1003f);	// 0x10000 for reset rx, 0x3f resets all 6 tx rings.
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0xe1f);
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0xe00);
+#endif // RT2860 //
+
+	// Initialze ASIC for TX & Rx operation
+	if (NICInitializeAsic(pAd , bHardReset) != NDIS_STATUS_SUCCESS)
+	{
+		if (j++ == 0)
+		{
+			NICLoadFirmware(pAd);
+			goto retry;
+		}
+		return NDIS_STATUS_FAILURE;
+	}
+
+
+#ifdef RT2860
+	// Write AC_BK base address register
+	Value = RTMP_GetPhysicalAddressLow(pAd->TxRing[QID_AC_BK].Cell[0].AllocPa);
+	RTMP_IO_WRITE32(pAd, TX_BASE_PTR1, Value);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> TX_BASE_PTR1 : 0x%x\n", Value));
+
+	// Write AC_BE base address register
+	Value = RTMP_GetPhysicalAddressLow(pAd->TxRing[QID_AC_BE].Cell[0].AllocPa);
+	RTMP_IO_WRITE32(pAd, TX_BASE_PTR0, Value);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> TX_BASE_PTR0 : 0x%x\n", Value));
+
+	// Write AC_VI base address register
+	Value = RTMP_GetPhysicalAddressLow(pAd->TxRing[QID_AC_VI].Cell[0].AllocPa);
+	RTMP_IO_WRITE32(pAd, TX_BASE_PTR2, Value);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> TX_BASE_PTR2 : 0x%x\n", Value));
+
+	// Write AC_VO base address register
+	Value = RTMP_GetPhysicalAddressLow(pAd->TxRing[QID_AC_VO].Cell[0].AllocPa);
+	RTMP_IO_WRITE32(pAd, TX_BASE_PTR3, Value);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> TX_BASE_PTR3 : 0x%x\n", Value));
+
+	// Write HCCA base address register
+	  Value = RTMP_GetPhysicalAddressLow(pAd->TxRing[QID_HCCA].Cell[0].AllocPa);
+	  RTMP_IO_WRITE32(pAd, TX_BASE_PTR4, Value);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> TX_BASE_PTR4 : 0x%x\n", Value));
+
+	// Write MGMT_BASE_CSR register
+	Value = RTMP_GetPhysicalAddressLow(pAd->MgmtRing.Cell[0].AllocPa);
+	RTMP_IO_WRITE32(pAd, TX_BASE_PTR5, Value);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> TX_BASE_PTR5 : 0x%x\n", Value));
+
+	// Write RX_BASE_CSR register
+	Value = RTMP_GetPhysicalAddressLow(pAd->RxRing.Cell[0].AllocPa);
+	RTMP_IO_WRITE32(pAd, RX_BASE_PTR, Value);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> RX_BASE_PTR : 0x%x\n", Value));
+
+	// Init RX Ring index pointer
+	pAd->RxRing.RxSwReadIdx = 0;
+	pAd->RxRing.RxCpuIdx = RX_RING_SIZE-1;
+	RTMP_IO_WRITE32(pAd, RX_CRX_IDX, pAd->RxRing.RxCpuIdx);
+
+	// Init TX rings index pointer
+	{
+		for (i=0; i<NUM_OF_TX_RING; i++)
+		{
+			pAd->TxRing[i].TxSwFreeIdx = 0;
+			pAd->TxRing[i].TxCpuIdx = 0;
+			RTMP_IO_WRITE32(pAd, (TX_CTX_IDX0 + i * 0x10) ,  pAd->TxRing[i].TxCpuIdx);
+		}
+	}
+
+	// init MGMT ring index pointer
+	pAd->MgmtRing.TxSwFreeIdx = 0;
+	pAd->MgmtRing.TxCpuIdx = 0;
+	RTMP_IO_WRITE32(pAd, TX_MGMTCTX_IDX,  pAd->MgmtRing.TxCpuIdx);
+
+	//
+	// set each Ring's SIZE  into ASIC. Descriptor Size is fixed by design.
+	//
+
+	// Write TX_RING_CSR0 register
+	Value = TX_RING_SIZE;
+	RTMP_IO_WRITE32(pAd, TX_MAX_CNT0, Value);
+	RTMP_IO_WRITE32(pAd, TX_MAX_CNT1, Value);
+	RTMP_IO_WRITE32(pAd, TX_MAX_CNT2, Value);
+	RTMP_IO_WRITE32(pAd, TX_MAX_CNT3, Value);
+	RTMP_IO_WRITE32(pAd, TX_MAX_CNT4, Value);
+	Value = MGMT_RING_SIZE;
+	RTMP_IO_WRITE32(pAd, TX_MGMTMAX_CNT, Value);
+
+	// Write RX_RING_CSR register
+	Value = RX_RING_SIZE;
+	RTMP_IO_WRITE32(pAd, RX_MAX_CNT, Value);
+#endif // RT2860 //
+
+
+	// WMM parameter
+	csr0.word = 0;
+	RTMP_IO_WRITE32(pAd, WMM_TXOP0_CFG, csr0.word);
+	if (pAd->CommonCfg.PhyMode == PHY_11B)
+	{
+		csr0.field.Ac0Txop = 192;	// AC_VI: 192*32us ~= 6ms
+		csr0.field.Ac1Txop = 96;	// AC_VO: 96*32us  ~= 3ms
+	}
+	else
+	{
+		csr0.field.Ac0Txop = 96;	// AC_VI: 96*32us ~= 3ms
+		csr0.field.Ac1Txop = 48;	// AC_VO: 48*32us ~= 1.5ms
+	}
+	RTMP_IO_WRITE32(pAd, WMM_TXOP1_CFG, csr0.word);
+
+
+#ifdef RT2860
+	// 3. Set DMA global configuration except TX_DMA_EN and RX_DMA_EN bits:
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0)  && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		RTMPusecDelay(1000);
+		i++;
+	}while ( i < 100);
+
+	GloCfg.word &= 0xff0;
+	GloCfg.field.EnTXWriteBackDDONE =1;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+	IntCfg.word = 0;
+	RTMP_IO_WRITE32(pAd, DELAY_INT_CFG, IntCfg.word);
+#endif // RT2860 //
+
+
+	// reset action
+	// Load firmware
+	//  Status = NICLoadFirmware(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitializeAdapter\n"));
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize ASIC
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	NICInitializeAsic(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  BOOLEAN		bHardReset)
+{
+	ULONG			Index = 0;
+	UCHAR			R0 = 0xff;
+	UINT32			MacCsr12 = 0, Counter = 0;
+	USHORT			KeyIdx;
+	INT				i,apidx;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitializeAsic\n"));
+
+#ifdef RT2860
+	if (bHardReset == TRUE)
+	{
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x3);
+	}
+	else
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x1);
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x0);
+	// Initialize MAC register to default value
+	for (Index = 0; Index < NUM_MAC_REG_PARMS; Index++)
+	{
+		RTMP_IO_WRITE32(pAd, MACRegTable[Index].Register, MACRegTable[Index].Value);
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		for (Index = 0; Index < NUM_STA_MAC_REG_PARMS; Index++)
+		{
+			RTMP_IO_WRITE32(pAd, STAMACRegTable[Index].Register, STAMACRegTable[Index].Value);
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+#endif // RT2860 //
+
+
+	//
+	// Before program BBP, we need to wait BBP/RF get wake up.
+	//
+	Index = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, MAC_STATUS_CFG, &MacCsr12);
+
+		if ((MacCsr12 & 0x03) == 0)	// if BB.RF is stable
+			break;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Check MAC_STATUS_CFG  = Busy = %x\n", MacCsr12));
+		RTMPusecDelay(1000);
+	} while (Index++ < 100);
+
+    // The commands to firmware should be after these commands, these commands will init firmware
+	// PCI and USB are not the same because PCI driver needs to wait for PCI bus ready
+	RTMP_IO_WRITE32(pAd, H2M_BBP_AGENT, 0);	// initialize BBP R/W access agent
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CSR, 0);
+	RTMPusecDelay(1000);
+
+	// Read BBP register, make sure BBP is up and running before write new data
+	Index = 0;
+	do
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R0, &R0);
+		DBGPRINT(RT_DEBUG_TRACE, ("BBP version = %x\n", R0));
+	} while ((++Index < 20) && ((R0 == 0xff) || (R0 == 0x00)));
+	//ASSERT(Index < 20); //this will cause BSOD on Check-build driver
+
+	if ((R0 == 0xff) || (R0 == 0x00))
+		return NDIS_STATUS_FAILURE;
+
+	// Initialize BBP register to default value
+	for (Index = 0; Index < NUM_BBP_REG_PARMS; Index++)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBPRegTable[Index].Register, BBPRegTable[Index].Value);
+	}
+
+	// for rt2860E and after, init BBP_R84 with 0x19. This is for extension channel overlapping IOT.
+	if ((pAd->MACVersion&0xffff) != 0x0101)
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R84, 0x19);
+
+
+	if (pAd->MACVersion == 0x28600100)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x16);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x12);
+    }
+
+	if (pAd->MACVersion >= RALINK_2880E_VERSION && pAd->MACVersion < RALINK_3070_VERSION) // 3*3
+	{
+		// enlarge MAX_LEN_CFG
+		UINT32 csr;
+		RTMP_IO_READ32(pAd, MAX_LEN_CFG, &csr);
+		csr &= 0xFFF;
+		csr |= 0x2000;
+		RTMP_IO_WRITE32(pAd, MAX_LEN_CFG, csr);
+	}
+
+
+	// Add radio off control
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->StaCfg.bRadio == FALSE)
+		{
+//			RTMP_IO_WRITE32(pAd, PWR_PIN_CFG, 0x00001818);
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+			DBGPRINT(RT_DEBUG_TRACE, ("Set Radio Off\n"));
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Clear raw counters
+	RTMP_IO_READ32(pAd, RX_STA_CNT0, &Counter);
+	RTMP_IO_READ32(pAd, RX_STA_CNT1, &Counter);
+	RTMP_IO_READ32(pAd, RX_STA_CNT2, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT0, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT1, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT2, &Counter);
+
+	// ASIC will keep garbage value after boot
+	// Clear all seared key table when initial
+	// This routine can be ignored in radio-ON/OFF operation.
+	if (bHardReset)
+	{
+		for (KeyIdx = 0; KeyIdx < 4; KeyIdx++)
+		{
+			RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE + 4*KeyIdx, 0);
+		}
+
+		// Clear all pairwise key table when initial
+		for (KeyIdx = 0; KeyIdx < 256; KeyIdx++)
+		{
+			RTMP_IO_WRITE32(pAd, MAC_WCID_ATTRIBUTE_BASE + (KeyIdx * HW_WCID_ATTRI_SIZE), 1);
+		}
+	}
+
+
+	// It isn't necessary to clear this space when not hard reset.
+	if (bHardReset == TRUE)
+	{
+		// clear all on-chip BEACON frame space
+		for (apidx = 0; apidx < HW_BEACON_MAX_COUNT; apidx++)
+		{
+			for (i = 0; i < HW_BEACON_OFFSET>>2; i+=4)
+				RTMP_IO_WRITE32(pAd, pAd->BeaconOffset[apidx] + i, 0x00);
+		}
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// for rt2860E and after, init TXOP_CTRL_CFG with 0x583f. This is for extension channel overlapping IOT.
+		if ((pAd->MACVersion&0xffff) != 0x0101)
+			RTMP_IO_WRITE32(pAd, TXOP_CTRL_CFG, 0x583f);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitializeAsic\n"));
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Reset NIC Asics
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Reset NIC to initial state AS IS system boot up time.
+
+	========================================================================
+*/
+VOID	NICIssueReset(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UINT32	Value = 0;
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICIssueReset\n"));
+
+	// Disable Rx, register value supposed will remain after reset
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+	Value &= (0xfffffff3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+	// Issue reset and clear from reset state
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x03); // 2004-09-17 change from 0x01
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x00);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICIssueReset\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Check ASIC registers and find any reason the system might hang
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+BOOLEAN	NICCheckForHang(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	return (FALSE);
+}
+
+VOID NICUpdateFifoStaCounters(
+	IN PRTMP_ADAPTER pAd)
+{
+	TX_STA_FIFO_STRUC	StaFifo;
+	MAC_TABLE_ENTRY		*pEntry;
+	UCHAR				i = 0;
+	UCHAR			pid = 0, wcid = 0;
+	CHAR				reTry;
+	UCHAR				succMCS;
+
+#ifdef RALINK_ATE
+	/* Nothing to do in ATE mode */
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+		do
+		{
+			RTMP_IO_READ32(pAd, TX_STA_FIFO, &StaFifo.word);
+
+			if (StaFifo.field.bValid == 0)
+				break;
+
+			wcid = (UCHAR)StaFifo.field.wcid;
+
+
+		/* ignore NoACK and MGMT frame use 0xFF as WCID */
+			if ((StaFifo.field.TxAckRequired == 0) || (wcid >= MAX_LEN_OF_MAC_TABLE))
+			{
+				i++;
+				continue;
+			}
+
+			/* PID store Tx MCS Rate */
+			pid = (UCHAR)StaFifo.field.PidType;
+
+			pEntry = &pAd->MacTab.Content[wcid];
+
+			pEntry->DebugFIFOCount++;
+
+#ifdef DOT11_N_SUPPORT
+			if (StaFifo.field.TxBF) // 3*3
+				pEntry->TxBFCount++;
+#endif // DOT11_N_SUPPORT //
+
+#ifdef UAPSD_AP_SUPPORT
+			UAPSD_SP_AUE_Handle(pAd, pEntry, StaFifo.field.TxSuccess);
+#endif // UAPSD_AP_SUPPORT //
+
+			if (!StaFifo.field.TxSuccess)
+			{
+				pEntry->FIFOCount++;
+				pEntry->OneSecTxFailCount++;
+
+				if (pEntry->FIFOCount >= 1)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("#"));
+#if 0
+					SendRefreshBAR(pAd, pEntry);
+					pEntry->NoBADataCountDown = 64;
+#else
+#ifdef DOT11_N_SUPPORT
+					pEntry->NoBADataCountDown = 64;
+#endif // DOT11_N_SUPPORT //
+
+					if(pEntry->PsMode == PWR_ACTIVE)
+					{
+#ifdef DOT11_N_SUPPORT
+						int tid;
+						for (tid=0; tid<NUM_OF_TID; tid++)
+						{
+							BAOriSessionTearDown(pAd, pEntry->Aid,  tid, FALSE, FALSE);
+						}
+#endif // DOT11_N_SUPPORT //
+
+						// Update the continuous transmission counter except PS mode
+						pEntry->ContinueTxFailCnt++;
+					}
+					else
+					{
+						// Clear the FIFOCount when sta in Power Save mode. Basically we assume
+						//     this tx error happened due to sta just go to sleep.
+						pEntry->FIFOCount = 0;
+						pEntry->ContinueTxFailCnt = 0;
+					}
+#endif
+					//pEntry->FIFOCount = 0;
+				}
+				//pEntry->bSendBAR = TRUE;
+			}
+			else
+			{
+#ifdef DOT11_N_SUPPORT
+				if ((pEntry->PsMode != PWR_SAVE) && (pEntry->NoBADataCountDown > 0))
+				{
+					pEntry->NoBADataCountDown--;
+					if (pEntry->NoBADataCountDown==0)
+					{
+						DBGPRINT(RT_DEBUG_TRACE, ("@\n"));
+					}
+				}
+#endif // DOT11_N_SUPPORT //
+				pEntry->FIFOCount = 0;
+				pEntry->OneSecTxNoRetryOkCount++;
+				// update NoDataIdleCount when sucessful send packet to STA.
+				pEntry->NoDataIdleCount = 0;
+				pEntry->ContinueTxFailCnt = 0;
+			}
+
+			succMCS = StaFifo.field.SuccessRate & 0x7F;
+
+			reTry = pid - succMCS;
+
+			if (StaFifo.field.TxSuccess)
+			{
+				pEntry->TXMCSExpected[pid]++;
+				if (pid == succMCS)
+				{
+					pEntry->TXMCSSuccessful[pid]++;
+				}
+				else
+				{
+					pEntry->TXMCSAutoFallBack[pid][succMCS]++;
+				}
+			}
+			else
+			{
+				pEntry->TXMCSFailed[pid]++;
+			}
+
+			if (reTry > 0)
+			{
+				if ((pid >= 12) && succMCS <=7)
+				{
+					reTry -= 4;
+				}
+				pEntry->OneSecTxRetryOkCount += reTry;
+			}
+
+			i++;
+			// ASIC store 16 stack
+		} while ( i < (2*TX_RING_SIZE) );
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read statistical counters from hardware registers and record them
+		in software variables for later on query
+
+	Arguments:
+		pAd					Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID NICUpdateRawCounters(
+	IN PRTMP_ADAPTER pAd)
+{
+	UINT32	OldValue;
+	RX_STA_CNT0_STRUC	 RxStaCnt0;
+	RX_STA_CNT1_STRUC   RxStaCnt1;
+	RX_STA_CNT2_STRUC   RxStaCnt2;
+	TX_STA_CNT0_STRUC 	 TxStaCnt0;
+	TX_STA_CNT1_STRUC	 StaTx1;
+	TX_STA_CNT2_STRUC	 StaTx2;
+	TX_AGG_CNT_STRUC	TxAggCnt;
+	TX_AGG_CNT0_STRUC	TxAggCnt0;
+	TX_AGG_CNT1_STRUC	TxAggCnt1;
+	TX_AGG_CNT2_STRUC	TxAggCnt2;
+	TX_AGG_CNT3_STRUC	TxAggCnt3;
+	TX_AGG_CNT4_STRUC	TxAggCnt4;
+	TX_AGG_CNT5_STRUC	TxAggCnt5;
+	TX_AGG_CNT6_STRUC	TxAggCnt6;
+	TX_AGG_CNT7_STRUC	TxAggCnt7;
+
+	RTMP_IO_READ32(pAd, RX_STA_CNT0, &RxStaCnt0.word);
+	RTMP_IO_READ32(pAd, RX_STA_CNT2, &RxStaCnt2.word);
+
+	{
+		RTMP_IO_READ32(pAd, RX_STA_CNT1, &RxStaCnt1.word);
+	    // Update RX PLCP error counter
+	    pAd->PrivateInfo.PhyRxErrCnt += RxStaCnt1.field.PlcpErr;
+		// Update False CCA counter
+		pAd->RalinkCounters.OneSecFalseCCACnt += RxStaCnt1.field.FalseCca;
+	}
+
+	// Update FCS counters
+	OldValue= pAd->WlanCounters.FCSErrorCount.u.LowPart;
+	pAd->WlanCounters.FCSErrorCount.u.LowPart += (RxStaCnt0.field.CrcErr); // >> 7);
+	if (pAd->WlanCounters.FCSErrorCount.u.LowPart < OldValue)
+		pAd->WlanCounters.FCSErrorCount.u.HighPart++;
+
+	// Add FCS error count to private counters
+	pAd->RalinkCounters.OneSecRxFcsErrCnt += RxStaCnt0.field.CrcErr;
+	OldValue = pAd->RalinkCounters.RealFcsErrCount.u.LowPart;
+	pAd->RalinkCounters.RealFcsErrCount.u.LowPart += RxStaCnt0.field.CrcErr;
+	if (pAd->RalinkCounters.RealFcsErrCount.u.LowPart < OldValue)
+		pAd->RalinkCounters.RealFcsErrCount.u.HighPart++;
+
+	// Update Duplicate Rcv check
+	pAd->RalinkCounters.DuplicateRcv += RxStaCnt2.field.RxDupliCount;
+	pAd->WlanCounters.FrameDuplicateCount.u.LowPart += RxStaCnt2.field.RxDupliCount;
+	// Update RX Overflow counter
+	pAd->Counters8023.RxNoBuffer += (RxStaCnt2.field.RxFifoOverflowCount);
+
+	if (!pAd->bUpdateBcnCntDone)
+	{
+	// Update BEACON sent count
+	RTMP_IO_READ32(pAd, TX_STA_CNT0, &TxStaCnt0.word);
+	RTMP_IO_READ32(pAd, TX_STA_CNT1, &StaTx1.word);
+	RTMP_IO_READ32(pAd, TX_STA_CNT2, &StaTx2.word);
+	pAd->RalinkCounters.OneSecBeaconSentCnt += TxStaCnt0.field.TxBeaconCount;
+	pAd->RalinkCounters.OneSecTxRetryOkCount += StaTx1.field.TxRetransmit;
+	pAd->RalinkCounters.OneSecTxNoRetryOkCount += StaTx1.field.TxSuccess;
+	pAd->RalinkCounters.OneSecTxFailCount += TxStaCnt0.field.TxFailCount;
+	pAd->WlanCounters.TransmittedFragmentCount.u.LowPart += StaTx1.field.TxSuccess;
+	pAd->WlanCounters.RetryCount.u.LowPart += StaTx1.field.TxRetransmit;
+	pAd->WlanCounters.FailedCount.u.LowPart += TxStaCnt0.field.TxFailCount;
+	}
+
+	{
+		RTMP_IO_READ32(pAd, TX_AGG_CNT, &TxAggCnt.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT0, &TxAggCnt0.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT1, &TxAggCnt1.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT2, &TxAggCnt2.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT3, &TxAggCnt3.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT4, &TxAggCnt4.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT5, &TxAggCnt5.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT6, &TxAggCnt6.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT7, &TxAggCnt7.word);
+		pAd->RalinkCounters.TxAggCount += TxAggCnt.field.AggTxCount;
+		pAd->RalinkCounters.TxNonAggCount += TxAggCnt.field.NonAggTxCount;
+		pAd->RalinkCounters.TxAgg1MPDUCount += TxAggCnt0.field.AggSize1Count;
+		pAd->RalinkCounters.TxAgg2MPDUCount += TxAggCnt0.field.AggSize2Count;
+
+		pAd->RalinkCounters.TxAgg3MPDUCount += TxAggCnt1.field.AggSize3Count;
+		pAd->RalinkCounters.TxAgg4MPDUCount += TxAggCnt1.field.AggSize4Count;
+		pAd->RalinkCounters.TxAgg5MPDUCount += TxAggCnt2.field.AggSize5Count;
+		pAd->RalinkCounters.TxAgg6MPDUCount += TxAggCnt2.field.AggSize6Count;
+
+		pAd->RalinkCounters.TxAgg7MPDUCount += TxAggCnt3.field.AggSize7Count;
+		pAd->RalinkCounters.TxAgg8MPDUCount += TxAggCnt3.field.AggSize8Count;
+		pAd->RalinkCounters.TxAgg9MPDUCount += TxAggCnt4.field.AggSize9Count;
+		pAd->RalinkCounters.TxAgg10MPDUCount += TxAggCnt4.field.AggSize10Count;
+
+		pAd->RalinkCounters.TxAgg11MPDUCount += TxAggCnt5.field.AggSize11Count;
+		pAd->RalinkCounters.TxAgg12MPDUCount += TxAggCnt5.field.AggSize12Count;
+		pAd->RalinkCounters.TxAgg13MPDUCount += TxAggCnt6.field.AggSize13Count;
+		pAd->RalinkCounters.TxAgg14MPDUCount += TxAggCnt6.field.AggSize14Count;
+
+		pAd->RalinkCounters.TxAgg15MPDUCount += TxAggCnt7.field.AggSize15Count;
+		pAd->RalinkCounters.TxAgg16MPDUCount += TxAggCnt7.field.AggSize16Count;
+
+		// Calculate the transmitted A-MPDU count
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += TxAggCnt0.field.AggSize1Count;
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt0.field.AggSize2Count / 2);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt1.field.AggSize3Count / 3);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt1.field.AggSize4Count / 4);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt2.field.AggSize5Count / 5);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt2.field.AggSize6Count / 6);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt3.field.AggSize7Count / 7);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt3.field.AggSize8Count / 8);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt4.field.AggSize9Count / 9);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt4.field.AggSize10Count / 10);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt5.field.AggSize11Count / 11);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt5.field.AggSize12Count / 12);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt6.field.AggSize13Count / 13);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt6.field.AggSize14Count / 14);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt7.field.AggSize15Count / 15);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt7.field.AggSize16Count / 16);
+	}
+
+#ifdef DBG_DIAGNOSE
+	{
+		RtmpDiagStruct	*pDiag;
+		COUNTER_RALINK	*pRalinkCounters;
+		UCHAR			ArrayCurIdx, i;
+
+		pDiag = &pAd->DiagStruct;
+		pRalinkCounters = &pAd->RalinkCounters;
+		ArrayCurIdx = pDiag->ArrayCurIdx;
+
+		if (pDiag->inited == 0)
+		{
+			NdisZeroMemory(pDiag, sizeof(struct _RtmpDiagStrcut_));
+			pDiag->ArrayStartIdx = pDiag->ArrayCurIdx = 0;
+			pDiag->inited = 1;
+		}
+		else
+		{
+			// Tx
+			pDiag->TxFailCnt[ArrayCurIdx] = TxStaCnt0.field.TxFailCount;
+			pDiag->TxAggCnt[ArrayCurIdx] = TxAggCnt.field.AggTxCount;
+			pDiag->TxNonAggCnt[ArrayCurIdx] = TxAggCnt.field.NonAggTxCount;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][0] = TxAggCnt0.field.AggSize1Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][1] = TxAggCnt0.field.AggSize2Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][2] = TxAggCnt1.field.AggSize3Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][3] = TxAggCnt1.field.AggSize4Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][4] = TxAggCnt2.field.AggSize5Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][5] = TxAggCnt2.field.AggSize6Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][6] = TxAggCnt3.field.AggSize7Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][7] = TxAggCnt3.field.AggSize8Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][8] = TxAggCnt4.field.AggSize9Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][9] = TxAggCnt4.field.AggSize10Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][10] = TxAggCnt5.field.AggSize11Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][11] = TxAggCnt5.field.AggSize12Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][12] = TxAggCnt6.field.AggSize13Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][13] = TxAggCnt6.field.AggSize14Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][14] = TxAggCnt7.field.AggSize15Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][15] = TxAggCnt7.field.AggSize16Count;
+
+			pDiag->RxCrcErrCnt[ArrayCurIdx] = RxStaCnt0.field.CrcErr;
+
+			INC_RING_INDEX(pDiag->ArrayCurIdx,  DIAGNOSE_TIME);
+			ArrayCurIdx = pDiag->ArrayCurIdx;
+			for (i =0; i < 9; i++)
+			{
+				pDiag->TxDescCnt[ArrayCurIdx][i]= 0;
+				pDiag->TxSWQueCnt[ArrayCurIdx][i] =0;
+				pDiag->TxMcsCnt[ArrayCurIdx][i] = 0;
+				pDiag->RxMcsCnt[ArrayCurIdx][i] = 0;
+			}
+			pDiag->TxDataCnt[ArrayCurIdx] = 0;
+			pDiag->TxFailCnt[ArrayCurIdx] = 0;
+			pDiag->RxDataCnt[ArrayCurIdx] = 0;
+			pDiag->RxCrcErrCnt[ArrayCurIdx]  = 0;
+			for (i = 9; i < 24; i++) // 3*3
+			{
+				pDiag->TxDescCnt[ArrayCurIdx][i] = 0;
+				pDiag->TxMcsCnt[ArrayCurIdx][i] = 0;
+				pDiag->RxMcsCnt[ArrayCurIdx][i] = 0;
+}
+
+			if (pDiag->ArrayCurIdx == pDiag->ArrayStartIdx)
+				INC_RING_INDEX(pDiag->ArrayStartIdx,  DIAGNOSE_TIME);
+		}
+
+	}
+#endif // DBG_DIAGNOSE //
+
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Reset NIC from error
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Reset NIC from error state
+
+	========================================================================
+*/
+VOID	NICResetFromError(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	// Reset BBP (according to alex, reset ASIC will force reset BBP
+	// Therefore, skip the reset BBP
+	// RTMP_IO_WRITE32(pAd, MAC_CSR1, 0x2);
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x1);
+	// Remove ASIC from reset state
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x0);
+
+	NICInitializeAdapter(pAd, FALSE);
+	NICInitAsicFromEEPROM(pAd);
+
+	// Switch to current channel, since during reset process, the connection should remains on.
+	AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		erase 8051 firmware image in MAC ASIC
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID NICEraseFirmware(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG i;
+
+	for(i=0; i<MAX_FIRMWARE_IMAGE_SIZE; i+=4)
+		RTMP_IO_WRITE32(pAd, FIRMWARE_IMAGE_BASE + i, 0);
+
+}/* End of NICEraseFirmware */
+
+/*
+	========================================================================
+
+	Routine Description:
+		Load 8051 firmware RT2561.BIN file into MAC ASIC
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS         firmware image load ok
+		NDIS_STATUS_FAILURE         image not found
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+NDIS_STATUS NICLoadFirmware(
+	IN PRTMP_ADAPTER pAd)
+{
+#ifdef BIN_IN_FILE
+#define NICLF_DEFAULT_USE()	\
+	flg_default_firm_use = TRUE; \
+	printk("%s - Use default firmware!\n", __func__);
+
+	NDIS_STATUS		Status = NDIS_STATUS_SUCCESS;
+	PUCHAR			src;
+	struct file		*srcf;
+	INT 			retval, orgfsuid, orgfsgid, i;
+   	mm_segment_t	orgfs;
+	PUCHAR			pFirmwareImage;
+	UINT			FileLength = 0;
+	UINT32			MacReg;
+	ULONG			Index;
+	ULONG			firm;
+	BOOLEAN			flg_default_firm_use = FALSE;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> %s\n", __func__));
+
+	/* init */
+	pFirmwareImage = NULL;
+	src = RTMP_FIRMWARE_FILE_NAME;
+
+	/* save uid and gid used for filesystem access.
+	   set user and group to 0 (root) */
+	orgfsuid = current->fsuid;
+	orgfsgid = current->fsgid;
+	current->fsuid = current->fsgid = 0;
+    orgfs = get_fs();
+    set_fs(KERNEL_DS);
+
+	pAd->FirmwareVersion = (FIRMWARE_MAJOR_VERSION << 8) + \
+						   FIRMWARE_MINOR_VERSION;
+
+
+	/* allocate firmware buffer */
+    pFirmwareImage = kmalloc(MAX_FIRMWARE_IMAGE_SIZE, MEM_ALLOC_FLAG);
+    if (pFirmwareImage == NULL)
+	{
+		/* allocate fail, use default firmware array in firmware.h */
+		printk("%s - Allocate memory fail!\n", __func__);
+		NICLF_DEFAULT_USE();
+    }
+	else
+	{
+		/* allocate ok! zero the firmware buffer */
+		memset(pFirmwareImage, 0x00, MAX_FIRMWARE_IMAGE_SIZE);
+	} /* End of if */
+
+
+	/* if ok, read firmware file from *.bin file */
+	if (flg_default_firm_use == FALSE)
+	{
+		do
+		{
+			/* open the bin file */
+			srcf = filp_open(src, O_RDONLY, 0);
+
+			if (IS_ERR(srcf))
+			{
+				printk("%s - Error %ld opening %s\n",
+					   __func__, -PTR_ERR(srcf), src);
+				NICLF_DEFAULT_USE();
+				break;
+			} /* End of if */
+
+			/* the object must have a read method */
+			if ((srcf->f_op == NULL) || (srcf->f_op->read == NULL))
+			{
+				printk("%s - %s does not have a write method\n", __func__, src);
+				NICLF_DEFAULT_USE();
+				break;
+			} /* End of if */
+
+			/* read the firmware from the file *.bin */
+			FileLength = srcf->f_op->read(srcf,
+										  pFirmwareImage,
+										  MAX_FIRMWARE_IMAGE_SIZE,
+										  &srcf->f_pos);
+
+			if (FileLength != MAX_FIRMWARE_IMAGE_SIZE)
+			{
+				printk("%s: error file length (=%d) in RT2860AP.BIN\n",
+					   __func__, FileLength);
+				NICLF_DEFAULT_USE();
+				break;
+			}
+			else
+			{
+				PUCHAR ptr = pFirmwareImage;
+				USHORT crc = 0xffff;
+
+
+				/* calculate firmware CRC */
+				for(i=0; i<(MAX_FIRMWARE_IMAGE_SIZE-2); i++, ptr++)
+					crc = ByteCRC16(bitrev8(*ptr), crc);
+				/* End of for */
+
+				if ((pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-2] != \
+								(UCHAR)bitrev8((UCHAR)(crc>>8))) ||
+					(pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-1] != \
+								(UCHAR)bitrev8((UCHAR)crc)))
+				{
+					/* CRC fail */
+					printk("%s: CRC = 0x%02x 0x%02x "
+						   "error, should be 0x%02x 0x%02x\n",
+						   __func__,
+						   pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-2],
+						   pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-1],
+						   (UCHAR)(crc>>8), (UCHAR)(crc));
+					NICLF_DEFAULT_USE();
+					break;
+				}
+				else
+				{
+					/* firmware is ok */
+					pAd->FirmwareVersion = \
+						(pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-4] << 8) +
+						pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-3];
+
+					/* check if firmware version of the file is too old */
+					if ((pAd->FirmwareVersion) < \
+											((FIRMWARE_MAJOR_VERSION << 8) +
+									  	 	 FIRMWARE_MINOR_VERSION))
+					{
+						printk("%s: firmware version too old!\n", __func__);
+						NICLF_DEFAULT_USE();
+						break;
+					} /* End of if */
+				} /* End of if */
+
+				DBGPRINT(RT_DEBUG_TRACE,
+						 ("NICLoadFirmware: CRC ok, ver=%d.%d\n",
+						  pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-4],
+						  pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-3]));
+			} /* End of if (FileLength == MAX_FIRMWARE_IMAGE_SIZE) */
+			break;
+		} while(TRUE);
+
+		/* close firmware file */
+		if (IS_ERR(srcf))
+			;
+		else
+		{
+			retval = filp_close(srcf, NULL);
+			if (retval)
+			{
+				DBGPRINT(RT_DEBUG_ERROR,
+						 ("--> Error %d closing %s\n", -retval, src));
+			} /* End of if */
+		} /* End of if */
+	} /* End of if */
+
+
+	/* write firmware to ASIC */
+	if (flg_default_firm_use == TRUE)
+	{
+		/* use default fimeware, free allocated buffer */
+		if (pFirmwareImage != NULL)
+			kfree(pFirmwareImage);
+		/* End of if */
+
+		/* use default *.bin array */
+		pFirmwareImage = FirmwareImage;
+		FileLength = sizeof(FirmwareImage);
+	} /* End of if */
+
+	/* enable Host program ram write selection */
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x10000);
+
+	for(i=0; i<FileLength; i+=4)
+	{
+		firm = pFirmwareImage[i] +
+			   (pFirmwareImage[i+3] << 24) +
+			   (pFirmwareImage[i+2] << 16) +
+			   (pFirmwareImage[i+1] << 8);
+
+		RTMP_IO_WRITE32(pAd, FIRMWARE_IMAGE_BASE + i, firm);
+	} /* End of for */
+
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x00000);
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x00001);
+
+	/* initialize BBP R/W access agent */
+	RTMP_IO_WRITE32(pAd, H2M_BBP_AGENT, 0);
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CSR, 0);
+
+	if (flg_default_firm_use == FALSE)
+	{
+		/* use file firmware, free allocated buffer */
+		if (pFirmwareImage != NULL)
+			kfree(pFirmwareImage);
+		/* End of if */
+	} /* End of if */
+
+	set_fs(orgfs);
+	current->fsuid = orgfsuid;
+	current->fsgid = orgfsgid;
+#else
+
+	NDIS_STATUS		Status = NDIS_STATUS_SUCCESS;
+	PUCHAR			pFirmwareImage;
+	ULONG			FileLength, Index;
+	//ULONG			firm;
+	UINT32			MacReg = 0;
+
+	pFirmwareImage = FirmwareImage;
+	FileLength = sizeof(FirmwareImage);
+	RT28XX_WRITE_FIRMWARE(pAd, pFirmwareImage, FileLength);
+#endif
+
+	/* check if MCU is ready */
+	Index = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, PBF_SYS_CTRL, &MacReg);
+
+		if (MacReg & 0x80)
+			break;
+
+		RTMPusecDelay(1000);
+	} while (Index++ < 1000);
+
+    if (Index >= 1000)
+	{
+		Status = NDIS_STATUS_FAILURE;
+		DBGPRINT(RT_DEBUG_ERROR, ("NICLoadFirmware: MCU is not ready\n\n\n"));
+	} /* End of if */
+
+    DBGPRINT(RT_DEBUG_TRACE,
+			 ("<=== %s (status=%d)\n", __func__, Status));
+    return Status;
+} /* End of NICLoadFirmware */
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Load Tx rate switching parameters
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS         firmware image load ok
+		NDIS_STATUS_FAILURE         image not found
+
+	IRQL = PASSIVE_LEVEL
+
+	Rate Table Format:
+		1. (B0: Valid Item number) (B1:Initial item from zero)
+		2. Item Number(Dec)      Mode(Hex)     Current MCS(Dec)    TrainUp(Dec)    TrainDown(Dec)
+
+	========================================================================
+*/
+NDIS_STATUS NICLoadRateSwitchingParams(
+	IN PRTMP_ADAPTER pAd)
+{
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		if  pSrc1 all zero with length Length, return 0.
+		If not all zero, return 1
+
+	Arguments:
+		pSrc1
+
+	Return Value:
+		1:			not all zero
+		0:			all zero
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+ULONG	RTMPNotAllZero(
+	IN	PVOID	pSrc1,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem1;
+	ULONG	Index = 0;
+
+	pMem1 = (PUCHAR) pSrc1;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		if (pMem1[Index] != 0x0)
+		{
+			break;
+		}
+	}
+
+	if (Index == Length)
+	{
+		return (0);
+	}
+	else
+	{
+		return (1);
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Compare two memory block
+
+	Arguments:
+		pSrc1		Pointer to first memory address
+		pSrc2		Pointer to second memory address
+
+	Return Value:
+		0:			memory is equal
+		1:			pSrc1 memory is larger
+		2:			pSrc2 memory is larger
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+ULONG	RTMPCompareMemory(
+	IN	PVOID	pSrc1,
+	IN	PVOID	pSrc2,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem1;
+	PUCHAR	pMem2;
+	ULONG	Index = 0;
+
+	pMem1 = (PUCHAR) pSrc1;
+	pMem2 = (PUCHAR) pSrc2;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		if (pMem1[Index] > pMem2[Index])
+			return (1);
+		else if (pMem1[Index] < pMem2[Index])
+			return (2);
+	}
+
+	// Equal
+	return (0);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Zero out memory block
+
+	Arguments:
+		pSrc1		Pointer to memory address
+		Length		Size
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPZeroMemory(
+	IN	PVOID	pSrc,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem;
+	ULONG	Index = 0;
+
+	pMem = (PUCHAR) pSrc;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		pMem[Index] = 0x00;
+	}
+}
+
+VOID	RTMPFillMemory(
+	IN	PVOID	pSrc,
+	IN	ULONG	Length,
+	IN	UCHAR	Fill)
+{
+	PUCHAR	pMem;
+	ULONG	Index = 0;
+
+	pMem = (PUCHAR) pSrc;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		pMem[Index] = Fill;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Copy data from memory block 1 to memory block 2
+
+	Arguments:
+		pDest		Pointer to destination memory address
+		pSrc		Pointer to source memory address
+		Length		Copy size
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPMoveMemory(
+	OUT	PVOID	pDest,
+	IN	PVOID	pSrc,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem1;
+	PUCHAR	pMem2;
+	UINT	Index;
+
+	ASSERT((Length==0) || (pDest && pSrc));
+
+	pMem1 = (PUCHAR) pDest;
+	pMem2 = (PUCHAR) pSrc;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		pMem1[Index] = pMem2[Index];
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize port configuration structure
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	UserCfgInit(
+	IN	PRTMP_ADAPTER pAd)
+{
+    UINT key_index, bss_index;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> UserCfgInit\n"));
+
+	//
+	//  part I. intialize common configuration
+	//
+
+	for(key_index=0; key_index<SHARE_KEY_NUM; key_index++)
+	{
+		for(bss_index = 0; bss_index < MAX_MBSSID_NUM; bss_index++)
+		{
+			pAd->SharedKey[bss_index][key_index].KeyLen = 0;
+			pAd->SharedKey[bss_index][key_index].CipherAlg = CIPHER_NONE;
+		}
+	}
+
+	pAd->Antenna.word = 0;
+	pAd->CommonCfg.BBPCurrentBW = BW_20;
+
+	pAd->LedCntl.word = 0;
+#ifdef RT2860
+	pAd->LedIndicatorStregth = 0;
+	pAd->RLnkCtrlOffset = 0;
+	pAd->HostLnkCtrlOffset = 0;
+#endif // RT2860 //
+
+	pAd->bAutoTxAgcA = FALSE;			// Default is OFF
+	pAd->bAutoTxAgcG = FALSE;			// Default is OFF
+	pAd->RfIcType = RFIC_2820;
+
+	// Init timer for reset complete event
+	pAd->CommonCfg.CentralChannel = 1;
+	pAd->bForcePrintTX = FALSE;
+	pAd->bForcePrintRX = FALSE;
+	pAd->bStaFifoTest = FALSE;
+	pAd->bProtectionTest = FALSE;
+	pAd->bHCCATest = FALSE;
+	pAd->bGenOneHCCA = FALSE;
+	pAd->CommonCfg.Dsifs = 10;      // in units of usec
+	pAd->CommonCfg.TxPower = 100; //mW
+	pAd->CommonCfg.TxPowerPercentage = 0xffffffff; // AUTO
+	pAd->CommonCfg.TxPowerDefault = 0xffffffff; // AUTO
+	pAd->CommonCfg.TxPreamble = Rt802_11PreambleAuto; // use Long preamble on TX by defaut
+	pAd->CommonCfg.bUseZeroToDisableFragment = FALSE;
+	pAd->CommonCfg.RtsThreshold = 2347;
+	pAd->CommonCfg.FragmentThreshold = 2346;
+	pAd->CommonCfg.UseBGProtection = 0;    // 0: AUTO
+	pAd->CommonCfg.bEnableTxBurst = TRUE; //0;
+	pAd->CommonCfg.PhyMode = 0xff;     // unknown
+	pAd->CommonCfg.BandState = UNKNOWN_BAND;
+	pAd->CommonCfg.RadarDetect.CSPeriod = 10;
+	pAd->CommonCfg.RadarDetect.CSCount = 0;
+	pAd->CommonCfg.RadarDetect.RDMode = RD_NORMAL_MODE;
+	pAd->CommonCfg.RadarDetect.ChMovingTime = 65;
+	pAd->CommonCfg.RadarDetect.LongPulseRadarTh = 3;
+	pAd->CommonCfg.bAPSDCapable = FALSE;
+	pAd->CommonCfg.bNeedSendTriggerFrame = FALSE;
+	pAd->CommonCfg.TriggerTimerCount = 0;
+	pAd->CommonCfg.bAPSDForcePowerSave = FALSE;
+	pAd->CommonCfg.bCountryFlag = FALSE;
+	pAd->CommonCfg.TxStream = 0;
+	pAd->CommonCfg.RxStream = 0;
+
+	NdisZeroMemory(&pAd->BeaconTxWI, sizeof(pAd->BeaconTxWI));
+
+#ifdef DOT11_N_SUPPORT
+	NdisZeroMemory(&pAd->CommonCfg.HtCapability, sizeof(pAd->CommonCfg.HtCapability));
+	pAd->HTCEnable = FALSE;
+	pAd->bBroadComHT = FALSE;
+	pAd->CommonCfg.bRdg = FALSE;
+
+#ifdef DOT11N_DRAFT3
+	pAd->CommonCfg.Dot11OBssScanPassiveDwell = dot11OBSSScanPassiveDwell;	// Unit : TU. 5~1000
+	pAd->CommonCfg.Dot11OBssScanActiveDwell = dot11OBSSScanActiveDwell;	// Unit : TU. 10~1000
+	pAd->CommonCfg.Dot11BssWidthTriggerScanInt = dot11BSSWidthTriggerScanInterval;	// Unit : Second
+	pAd->CommonCfg.Dot11OBssScanPassiveTotalPerChannel = dot11OBSSScanPassiveTotalPerChannel;	// Unit : TU. 200~10000
+	pAd->CommonCfg.Dot11OBssScanActiveTotalPerChannel = dot11OBSSScanActiveTotalPerChannel;	// Unit : TU. 20~10000
+	pAd->CommonCfg.Dot11BssWidthChanTranDelayFactor = dot11BSSWidthChannelTransactionDelayFactor;
+	pAd->CommonCfg.Dot11OBssScanActivityThre = dot11BSSScanActivityThreshold;	// Unit : percentage
+	pAd->CommonCfg.Dot11BssWidthChanTranDelay = (pAd->CommonCfg.Dot11BssWidthTriggerScanInt * pAd->CommonCfg.Dot11BssWidthChanTranDelayFactor);
+#endif  // DOT11N_DRAFT3 //
+
+	NdisZeroMemory(&pAd->CommonCfg.AddHTInfo, sizeof(pAd->CommonCfg.AddHTInfo));
+	pAd->CommonCfg.BACapability.field.MMPSmode = MMPS_ENABLE;
+	pAd->CommonCfg.BACapability.field.MpduDensity = 0;
+	pAd->CommonCfg.BACapability.field.Policy = IMMED_BA;
+	pAd->CommonCfg.BACapability.field.RxBAWinLimit = 64; //32;
+	pAd->CommonCfg.BACapability.field.TxBAWinLimit = 64; //32;
+	DBGPRINT(RT_DEBUG_TRACE, ("--> UserCfgInit. BACapability = 0x%x\n", pAd->CommonCfg.BACapability.word));
+
+	pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+	BATableInit(pAd, &pAd->BATable);
+
+	pAd->CommonCfg.bExtChannelSwitchAnnouncement = 1;
+	pAd->CommonCfg.bHTProtect = 1;
+	pAd->CommonCfg.bMIMOPSEnable = TRUE;
+	pAd->CommonCfg.bBADecline = FALSE;
+	pAd->CommonCfg.bDisableReordering = FALSE;
+
+	pAd->CommonCfg.TxBASize = 7;
+
+	pAd->CommonCfg.REGBACapability.word = pAd->CommonCfg.BACapability.word;
+#endif // DOT11_N_SUPPORT //
+
+	//pAd->CommonCfg.HTPhyMode.field.BW = BW_20;
+	//pAd->CommonCfg.HTPhyMode.field.MCS = MCS_AUTO;
+	//pAd->CommonCfg.HTPhyMode.field.ShortGI = GI_800;
+	//pAd->CommonCfg.HTPhyMode.field.STBC = STBC_NONE;
+	pAd->CommonCfg.TxRate = RATE_6;
+
+	pAd->CommonCfg.MlmeTransmit.field.MCS = MCS_RATE_6;
+	pAd->CommonCfg.MlmeTransmit.field.BW = BW_20;
+	pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+
+	pAd->CommonCfg.BeaconPeriod = 100;     // in mSec
+
+	//
+	// part II. intialize STA specific configuration
+	//
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		RX_FILTER_SET_FLAG(pAd, fRX_FILTER_ACCEPT_DIRECT);
+		RX_FILTER_CLEAR_FLAG(pAd, fRX_FILTER_ACCEPT_MULTICAST);
+		RX_FILTER_SET_FLAG(pAd, fRX_FILTER_ACCEPT_BROADCAST);
+		RX_FILTER_SET_FLAG(pAd, fRX_FILTER_ACCEPT_ALL_MULTICAST);
+
+		pAd->StaCfg.Psm = PWR_ACTIVE;
+
+		pAd->StaCfg.OrigWepStatus = Ndis802_11EncryptionDisabled;
+		pAd->StaCfg.PairCipher = Ndis802_11EncryptionDisabled;
+		pAd->StaCfg.GroupCipher = Ndis802_11EncryptionDisabled;
+		pAd->StaCfg.bMixCipher = FALSE;
+		pAd->StaCfg.DefaultKeyId = 0;
+
+		// 802.1x port control
+		pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilter8021xWEP;
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+		pAd->StaCfg.LastMicErrorTime = 0;
+		pAd->StaCfg.MicErrCnt        = 0;
+		pAd->StaCfg.bBlockAssoc      = FALSE;
+		pAd->StaCfg.WpaState         = SS_NOTUSE;
+
+		pAd->CommonCfg.NdisRadioStateOff = FALSE;		// New to support microsoft disable radio with OID command
+
+		pAd->StaCfg.RssiTrigger = 0;
+		NdisZeroMemory(&pAd->StaCfg.RssiSample, sizeof(RSSI_SAMPLE));
+		pAd->StaCfg.RssiTriggerMode = RSSI_TRIGGERED_UPON_BELOW_THRESHOLD;
+		pAd->StaCfg.AtimWin = 0;
+		pAd->StaCfg.DefaultListenCount = 3;//default listen count;
+		pAd->StaCfg.BssType = BSS_INFRA;  // BSS_INFRA or BSS_ADHOC or BSS_MONITOR
+		pAd->StaCfg.bScanReqIsFromWebUI = FALSE;
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_WAKEUP_NOW);
+
+		pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+	}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	pAd->StaCfg.IEEE80211dClientMode = Rt802_11_D_None;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+
+	// global variables mXXXX used in MAC protocol state machines
+	OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_INFRA_ON);
+
+	// PHY specification
+	pAd->CommonCfg.PhyMode = PHY_11BG_MIXED;		// default PHY mode
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);  // CCK use LONG preamble
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// user desired power mode
+		pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+		pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+		pAd->StaCfg.bWindowsACCAMEnable = FALSE;
+
+#ifdef LEAP_SUPPORT
+		// CCX v1.0 releated init value
+		RTMPInitTimer(pAd, &pAd->StaCfg.LeapAuthTimer, GET_TIMER_FUNCTION(LeapAuthTimeout), pAd, FALSE);
+		pAd->StaCfg.LeapAuthMode = CISCO_AuthModeLEAPNone;
+		pAd->StaCfg.bCkipOn = FALSE;
+#endif // LEAP_SUPPORT //
+
+		RTMPInitTimer(pAd, &pAd->StaCfg.StaQuickResponeForRateUpTimer, GET_TIMER_FUNCTION(StaQuickResponeForRateUpExec), pAd, FALSE);
+		pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = FALSE;
+
+		// Patch for Ndtest
+		pAd->StaCfg.ScanCnt = 0;
+
+		// CCX 2.0 control flag init
+		pAd->StaCfg.CCXEnable = FALSE;
+		pAd->StaCfg.CCXReqType = MSRN_TYPE_UNUSED;
+		pAd->StaCfg.CCXQosECWMin	= 4;
+		pAd->StaCfg.CCXQosECWMax	= 10;
+
+		pAd->StaCfg.bHwRadio  = TRUE; // Default Hardware Radio status is On
+		pAd->StaCfg.bSwRadio  = TRUE; // Default Software Radio status is On
+		pAd->StaCfg.bRadio    = TRUE; // bHwRadio && bSwRadio
+		pAd->StaCfg.bHardwareRadio = FALSE;		// Default is OFF
+		pAd->StaCfg.bShowHiddenSSID = FALSE;		// Default no show
+
+		// Nitro mode control
+		pAd->StaCfg.bAutoReconnect = TRUE;
+
+		// Save the init time as last scan time, the system should do scan after 2 seconds.
+		// This patch is for driver wake up from standby mode, system will do scan right away.
+		pAd->StaCfg.LastScanTime = 0;
+		NdisZeroMemory(pAd->nickname, IW_ESSID_MAX_SIZE+1);
+		sprintf(pAd->nickname, "%s", STA_NIC_DEVICE_NAME);
+		RTMPInitTimer(pAd, &pAd->StaCfg.WpaDisassocAndBlockAssocTimer, GET_TIMER_FUNCTION(WpaDisassocApAndBlockAssoc), pAd, FALSE);
+#ifdef WPA_SUPPLICANT_SUPPORT
+		pAd->StaCfg.IEEE8021X = FALSE;
+		pAd->StaCfg.IEEE8021x_required_keys = FALSE;
+		pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+		pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE;
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Default for extra information is not valid
+	pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+
+	// Default Config change flag
+	pAd->bConfigChanged = FALSE;
+
+	//
+	// part III. AP configurations
+	//
+
+
+	//
+	// part IV. others
+	//
+	// dynamic BBP R66:sensibity tuning to overcome background noise
+	pAd->BbpTuning.bEnable                = TRUE;
+	pAd->BbpTuning.FalseCcaLowerThreshold = 100;
+	pAd->BbpTuning.FalseCcaUpperThreshold = 512;
+	pAd->BbpTuning.R66Delta               = 4;
+	pAd->Mlme.bEnableAutoAntennaCheck = TRUE;
+
+	//
+	// Also initial R66CurrentValue, RTUSBResumeMsduTransmission might use this value.
+	// if not initial this value, the default value will be 0.
+	//
+	pAd->BbpTuning.R66CurrentValue = 0x38;
+
+	pAd->Bbp94 = BBPR94_DEFAULT;
+	pAd->BbpForCCK = FALSE;
+
+	// initialize MAC table and allocate spin lock
+	NdisZeroMemory(&pAd->MacTab, sizeof(MAC_TABLE));
+	InitializeQueueHeader(&pAd->MacTab.McastPsQueue);
+	NdisAllocateSpinLock(&pAd->MacTabLock);
+
+#ifdef RALINK_ATE
+	NdisZeroMemory(&pAd->ate, sizeof(ATE_INFO));
+	pAd->ate.Mode = ATE_STOP;
+	pAd->ate.TxCount = 200;/* to exceed TX_RING_SIZE ... */
+	pAd->ate.TxLength = 1024;
+	pAd->ate.TxWI.ShortGI = 0;// LONG GI : 800 ns
+	pAd->ate.TxWI.PHYMODE = MODE_CCK;
+	pAd->ate.TxWI.MCS = 3;
+	pAd->ate.TxWI.BW = BW_20;
+	pAd->ate.Channel = 1;
+	pAd->ate.QID = QID_AC_BE;
+	pAd->ate.Addr1[0] = 0x00;
+	pAd->ate.Addr1[1] = 0x11;
+	pAd->ate.Addr1[2] = 0x22;
+	pAd->ate.Addr1[3] = 0xAA;
+	pAd->ate.Addr1[4] = 0xBB;
+	pAd->ate.Addr1[5] = 0xCC;
+	NdisMoveMemory(pAd->ate.Addr2, pAd->ate.Addr1, ETH_LENGTH_OF_ADDRESS);
+	NdisMoveMemory(pAd->ate.Addr3, pAd->ate.Addr1, ETH_LENGTH_OF_ADDRESS);
+	pAd->ate.bRxFer = 0;
+	pAd->ate.bQATxStart = FALSE;
+	pAd->ate.bQARxStart = FALSE;
+#ifdef RT2860
+	pAd->ate.bFWLoading = FALSE;
+#endif // RT2860 //
+#ifdef RALINK_28xx_QA
+	//pAd->ate.Repeat = 0;
+	pAd->ate.TxStatus = 0;
+	pAd->ate.AtePid = THREAD_PID_INIT_VALUE;
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+
+	pAd->CommonCfg.bWiFiTest = FALSE;
+#ifdef RT2860
+    pAd->bPCIclkOff = FALSE;
+#endif // RT2860 //
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- UserCfgInit\n"));
+}
+
+// IRQL = PASSIVE_LEVEL
+UCHAR BtoH(char ch)
+{
+	if (ch >= '0' && ch <= '9') return (ch - '0');        // Handle numerals
+	if (ch >= 'A' && ch <= 'F') return (ch - 'A' + 0xA);  // Handle capitol hex digits
+	if (ch >= 'a' && ch <= 'f') return (ch - 'a' + 0xA);  // Handle small hex digits
+	return(255);
+}
+
+//
+//  FUNCTION: AtoH(char *, UCHAR *, int)
+//
+//  PURPOSE:  Converts ascii string to network order hex
+//
+//  PARAMETERS:
+//    src    - pointer to input ascii string
+//    dest   - pointer to output hex
+//    destlen - size of dest
+//
+//  COMMENTS:
+//
+//    2 ascii bytes make a hex byte so must put 1st ascii byte of pair
+//    into upper nibble and 2nd ascii byte of pair into lower nibble.
+//
+// IRQL = PASSIVE_LEVEL
+
+void AtoH(char * src, UCHAR * dest, int destlen)
+{
+	char * srcptr;
+	PUCHAR destTemp;
+
+	srcptr = src;
+	destTemp = (PUCHAR) dest;
+
+	while(destlen--)
+	{
+		*destTemp = BtoH(*srcptr++) << 4;    // Put 1st ascii byte in upper nibble.
+		*destTemp += BtoH(*srcptr++);      // Add 2nd ascii byte to above.
+		destTemp++;
+	}
+}
+
+VOID	RTMPPatchMacBbpBug(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	ULONG	Index;
+
+	// Initialize BBP register to default value
+	for (Index = 0; Index < NUM_BBP_REG_PARMS; Index++)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBPRegTable[Index].Register, (UCHAR)BBPRegTable[Index].Value);
+	}
+
+	// Initialize RF register to default value
+	AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+	// Re-init BBP register from EEPROM value
+	NICInitAsicFromEEPROM(pAd);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init timer objects
+
+	Arguments:
+		pAd			Pointer to our adapter
+		pTimer				Timer structure
+		pTimerFunc			Function to execute when timer expired
+		Repeat				Ture for period timer
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitTimer(
+	IN	PRTMP_ADAPTER			pAd,
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	PVOID					pTimerFunc,
+	IN	PVOID					pData,
+	IN	BOOLEAN					Repeat)
+{
+	//
+	// Set Valid to TRUE for later used.
+	// It will crash if we cancel a timer or set a timer
+	// that we haven't initialize before.
+	//
+	pTimer->Valid      = TRUE;
+
+	pTimer->PeriodicType = Repeat;
+	pTimer->State      = FALSE;
+	pTimer->cookie = (ULONG) pData;
+
+
+	RTMP_OS_Init_Timer(pAd,	&pTimer->TimerObj,	pTimerFunc, (PVOID) pTimer);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init timer objects
+
+	Arguments:
+		pTimer				Timer structure
+		Value				Timer value in milliseconds
+
+	Return Value:
+		None
+
+	Note:
+		To use this routine, must call RTMPInitTimer before.
+
+	========================================================================
+*/
+VOID	RTMPSetTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	ULONG					Value)
+{
+	if (pTimer->Valid)
+	{
+		pTimer->TimerValue = Value;
+		pTimer->State      = FALSE;
+		if (pTimer->PeriodicType == TRUE)
+		{
+			pTimer->Repeat = TRUE;
+			RTMP_SetPeriodicTimer(&pTimer->TimerObj, Value);
+		}
+		else
+		{
+			pTimer->Repeat = FALSE;
+			RTMP_OS_Add_Timer(&pTimer->TimerObj, Value);
+		}
+	}
+	else
+	{
+		DBGPRINT_ERR(("RTMPSetTimer failed, Timer hasn't been initialize!\n"));
+	}
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init timer objects
+
+	Arguments:
+		pTimer				Timer structure
+		Value				Timer value in milliseconds
+
+	Return Value:
+		None
+
+	Note:
+		To use this routine, must call RTMPInitTimer before.
+
+	========================================================================
+*/
+VOID	RTMPModTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	ULONG					Value)
+{
+	BOOLEAN	Cancel;
+
+	if (pTimer->Valid)
+	{
+		pTimer->TimerValue = Value;
+		pTimer->State      = FALSE;
+		if (pTimer->PeriodicType == TRUE)
+		{
+			RTMPCancelTimer(pTimer, &Cancel);
+			RTMPSetTimer(pTimer, Value);
+		}
+		else
+		{
+			RTMP_OS_Mod_Timer(&pTimer->TimerObj, Value);
+		}
+	}
+	else
+	{
+		DBGPRINT_ERR(("RTMPModTimer failed, Timer hasn't been initialize!\n"));
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Cancel timer objects
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		1.) To use this routine, must call RTMPInitTimer before.
+		2.) Reset NIC to initial state AS IS system boot up time.
+
+	========================================================================
+*/
+VOID	RTMPCancelTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	OUT	BOOLEAN					*pCancelled)
+{
+	if (pTimer->Valid)
+	{
+		if (pTimer->State == FALSE)
+			pTimer->Repeat = FALSE;
+			RTMP_OS_Del_Timer(&pTimer->TimerObj, pCancelled);
+
+		if (*pCancelled == TRUE)
+			pTimer->State = TRUE;
+
+	}
+	else
+	{
+		//
+		// NdisMCancelTimer just canced the timer and not mean release the timer.
+		// And don't set the "Valid" to False. So that we can use this timer again.
+		//
+		DBGPRINT_ERR(("RTMPCancelTimer failed, Timer hasn't been initialize!\n"));
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set LED Status
+
+	Arguments:
+		pAd						Pointer to our adapter
+		Status					LED Status
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPSetLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN UCHAR			Status)
+{
+	//ULONG			data;
+	UCHAR			HighByte = 0;
+	UCHAR			LowByte;
+
+// In ATE mode of RT2860 AP/STA, we have erased 8051 firmware.
+// So LED mode is not supported when ATE is running.
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	LowByte = pAd->LedCntl.field.LedMode&0x7f;
+	switch (Status)
+	{
+		case LED_LINK_DOWN:
+			HighByte = 0x20;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			pAd->LedIndicatorStregth = 0;
+			break;
+		case LED_LINK_UP:
+			if (pAd->CommonCfg.Channel > 14)
+				HighByte = 0xa0;
+			else
+				HighByte = 0x60;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_RADIO_ON:
+			HighByte = 0x20;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_HALT:
+			LowByte = 0; // Driver sets MAC register and MAC controls LED
+		case LED_RADIO_OFF:
+			HighByte = 0;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+        case LED_WPS:
+			HighByte = 0x10;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_ON_SITE_SURVEY:
+			HighByte = 0x08;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_POWER_UP:
+			HighByte = 0x04;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		default:
+			DBGPRINT(RT_DEBUG_WARN, ("RTMPSetLED::Unknown Status %d\n", Status));
+			break;
+	}
+
+    //
+	// Keep LED status for LED SiteSurvey mode.
+	// After SiteSurvey, we will set the LED mode to previous status.
+	//
+	if ((Status != LED_ON_SITE_SURVEY) && (Status != LED_POWER_UP))
+		pAd->LedStatus = Status;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetLED::Mode=%d,HighByte=0x%02x,LowByte=0x%02x\n", pAd->LedCntl.field.LedMode, HighByte, LowByte));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set LED Signal Stregth
+
+	Arguments:
+		pAd						Pointer to our adapter
+		Dbm						Signal Stregth
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Can be run on any IRQL level.
+
+		According to Microsoft Zero Config Wireless Signal Stregth definition as belows.
+		<= -90  No Signal
+		<= -81  Very Low
+		<= -71  Low
+		<= -67  Good
+		<= -57  Very Good
+		 > -57  Excellent
+	========================================================================
+*/
+VOID RTMPSetSignalLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN NDIS_802_11_RSSI Dbm)
+{
+	UCHAR		nLed = 0;
+
+	//
+	// if not Signal Stregth, then do nothing.
+	//
+	if (pAd->LedCntl.field.LedMode != LED_MODE_SIGNAL_STREGTH)
+	{
+		return;
+	}
+
+	if (Dbm <= -90)
+		nLed = 0;
+	else if (Dbm <= -81)
+		nLed = 1;
+	else if (Dbm <= -71)
+		nLed = 3;
+	else if (Dbm <= -67)
+		nLed = 7;
+	else if (Dbm <= -57)
+		nLed = 15;
+	else
+		nLed = 31;
+
+	//
+	// Update Signal Stregth to firmware if changed.
+	//
+	if (pAd->LedIndicatorStregth != nLed)
+	{
+		AsicSendCommandToMcu(pAd, 0x51, 0xff, nLed, pAd->LedCntl.field.Polarity);
+		pAd->LedIndicatorStregth = nLed;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Enable RX
+
+	Arguments:
+		pAd						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL <= DISPATCH_LEVEL
+
+	Note:
+		Before Enable RX, make sure you have enabled Interrupt.
+	========================================================================
+*/
+VOID RTMPEnableRxTx(
+	IN PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("==> RTMPEnableRxTx\n"));
+
+	// Enable Rx DMA.
+	RT28XXDMAEnable(pAd);
+
+	// enable RX of MAC block
+	if (pAd->OpMode == OPMODE_AP)
+	{
+		UINT32 rx_filter_flag = APNORMAL;
+
+
+		RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, rx_filter_flag);     // enable RX of DMA block
+	}
+	else
+	{
+		RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL);     // Staion not drop control frame will fail WiFi Certification.
+	}
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0xc);
+	DBGPRINT(RT_DEBUG_TRACE, ("<== RTMPEnableRxTx\n"));
+}
+
+
diff --git a/drivers/staging/rt2860/common/rtmp_tkip.c b/drivers/staging/rt2860/common/rtmp_tkip.c
new file mode 100644
index 000000000000..a87ea3a5d3ea
--- /dev/null
+++ b/drivers/staging/rt2860/common/rtmp_tkip.c
@@ -0,0 +1,1607 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_tkip.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Paul Wu		02-25-02		Initial
+*/
+
+#include "../rt_config.h"
+
+// Rotation functions on 32 bit values
+#define ROL32( A, n ) \
+	( ((A) << (n)) | ( ((A)>>(32-(n))) & ( (1UL << (n)) - 1 ) ) )
+#define ROR32( A, n ) ROL32( (A), 32-(n) )
+
+UINT Tkip_Sbox_Lower[256] =
+{
+	0xA5,0x84,0x99,0x8D,0x0D,0xBD,0xB1,0x54,
+	0x50,0x03,0xA9,0x7D,0x19,0x62,0xE6,0x9A,
+	0x45,0x9D,0x40,0x87,0x15,0xEB,0xC9,0x0B,
+	0xEC,0x67,0xFD,0xEA,0xBF,0xF7,0x96,0x5B,
+	0xC2,0x1C,0xAE,0x6A,0x5A,0x41,0x02,0x4F,
+	0x5C,0xF4,0x34,0x08,0x93,0x73,0x53,0x3F,
+	0x0C,0x52,0x65,0x5E,0x28,0xA1,0x0F,0xB5,
+	0x09,0x36,0x9B,0x3D,0x26,0x69,0xCD,0x9F,
+	0x1B,0x9E,0x74,0x2E,0x2D,0xB2,0xEE,0xFB,
+	0xF6,0x4D,0x61,0xCE,0x7B,0x3E,0x71,0x97,
+	0xF5,0x68,0x00,0x2C,0x60,0x1F,0xC8,0xED,
+	0xBE,0x46,0xD9,0x4B,0xDE,0xD4,0xE8,0x4A,
+	0x6B,0x2A,0xE5,0x16,0xC5,0xD7,0x55,0x94,
+	0xCF,0x10,0x06,0x81,0xF0,0x44,0xBA,0xE3,
+	0xF3,0xFE,0xC0,0x8A,0xAD,0xBC,0x48,0x04,
+	0xDF,0xC1,0x75,0x63,0x30,0x1A,0x0E,0x6D,
+	0x4C,0x14,0x35,0x2F,0xE1,0xA2,0xCC,0x39,
+	0x57,0xF2,0x82,0x47,0xAC,0xE7,0x2B,0x95,
+	0xA0,0x98,0xD1,0x7F,0x66,0x7E,0xAB,0x83,
+	0xCA,0x29,0xD3,0x3C,0x79,0xE2,0x1D,0x76,
+	0x3B,0x56,0x4E,0x1E,0xDB,0x0A,0x6C,0xE4,
+	0x5D,0x6E,0xEF,0xA6,0xA8,0xA4,0x37,0x8B,
+	0x32,0x43,0x59,0xB7,0x8C,0x64,0xD2,0xE0,
+	0xB4,0xFA,0x07,0x25,0xAF,0x8E,0xE9,0x18,
+	0xD5,0x88,0x6F,0x72,0x24,0xF1,0xC7,0x51,
+	0x23,0x7C,0x9C,0x21,0xDD,0xDC,0x86,0x85,
+	0x90,0x42,0xC4,0xAA,0xD8,0x05,0x01,0x12,
+	0xA3,0x5F,0xF9,0xD0,0x91,0x58,0x27,0xB9,
+	0x38,0x13,0xB3,0x33,0xBB,0x70,0x89,0xA7,
+	0xB6,0x22,0x92,0x20,0x49,0xFF,0x78,0x7A,
+	0x8F,0xF8,0x80,0x17,0xDA,0x31,0xC6,0xB8,
+	0xC3,0xB0,0x77,0x11,0xCB,0xFC,0xD6,0x3A
+};
+
+UINT Tkip_Sbox_Upper[256] =
+{
+	0xC6,0xF8,0xEE,0xF6,0xFF,0xD6,0xDE,0x91,
+	0x60,0x02,0xCE,0x56,0xE7,0xB5,0x4D,0xEC,
+	0x8F,0x1F,0x89,0xFA,0xEF,0xB2,0x8E,0xFB,
+	0x41,0xB3,0x5F,0x45,0x23,0x53,0xE4,0x9B,
+	0x75,0xE1,0x3D,0x4C,0x6C,0x7E,0xF5,0x83,
+	0x68,0x51,0xD1,0xF9,0xE2,0xAB,0x62,0x2A,
+	0x08,0x95,0x46,0x9D,0x30,0x37,0x0A,0x2F,
+	0x0E,0x24,0x1B,0xDF,0xCD,0x4E,0x7F,0xEA,
+	0x12,0x1D,0x58,0x34,0x36,0xDC,0xB4,0x5B,
+	0xA4,0x76,0xB7,0x7D,0x52,0xDD,0x5E,0x13,
+	0xA6,0xB9,0x00,0xC1,0x40,0xE3,0x79,0xB6,
+	0xD4,0x8D,0x67,0x72,0x94,0x98,0xB0,0x85,
+	0xBB,0xC5,0x4F,0xED,0x86,0x9A,0x66,0x11,
+	0x8A,0xE9,0x04,0xFE,0xA0,0x78,0x25,0x4B,
+	0xA2,0x5D,0x80,0x05,0x3F,0x21,0x70,0xF1,
+	0x63,0x77,0xAF,0x42,0x20,0xE5,0xFD,0xBF,
+	0x81,0x18,0x26,0xC3,0xBE,0x35,0x88,0x2E,
+	0x93,0x55,0xFC,0x7A,0xC8,0xBA,0x32,0xE6,
+	0xC0,0x19,0x9E,0xA3,0x44,0x54,0x3B,0x0B,
+	0x8C,0xC7,0x6B,0x28,0xA7,0xBC,0x16,0xAD,
+	0xDB,0x64,0x74,0x14,0x92,0x0C,0x48,0xB8,
+	0x9F,0xBD,0x43,0xC4,0x39,0x31,0xD3,0xF2,
+	0xD5,0x8B,0x6E,0xDA,0x01,0xB1,0x9C,0x49,
+	0xD8,0xAC,0xF3,0xCF,0xCA,0xF4,0x47,0x10,
+	0x6F,0xF0,0x4A,0x5C,0x38,0x57,0x73,0x97,
+	0xCB,0xA1,0xE8,0x3E,0x96,0x61,0x0D,0x0F,
+	0xE0,0x7C,0x71,0xCC,0x90,0x06,0xF7,0x1C,
+	0xC2,0x6A,0xAE,0x69,0x17,0x99,0x3A,0x27,
+	0xD9,0xEB,0x2B,0x22,0xD2,0xA9,0x07,0x33,
+	0x2D,0x3C,0x15,0xC9,0x87,0xAA,0x50,0xA5,
+	0x03,0x59,0x09,0x1A,0x65,0xD7,0x84,0xD0,
+	0x82,0x29,0x5A,0x1E,0x7B,0xA8,0x6D,0x2C
+};
+
+/*****************************/
+/******** SBOX Table *********/
+/*****************************/
+
+UCHAR SboxTable[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
+};
+
+VOID xor_32(
+	IN  PUCHAR              a,
+	IN  PUCHAR              b,
+	OUT PUCHAR              out);
+
+VOID xor_128(
+	IN  PUCHAR              a,
+	IN  PUCHAR              b,
+	OUT PUCHAR              out);
+
+VOID next_key(
+	IN  PUCHAR              key,
+	IN  INT                 round);
+
+VOID byte_sub(
+	IN  PUCHAR              in,
+	OUT PUCHAR              out);
+
+VOID shift_row(
+	IN  PUCHAR              in,
+	OUT PUCHAR              out);
+
+VOID mix_column(
+	IN  PUCHAR              in,
+	OUT PUCHAR              out);
+
+UCHAR RTMPCkipSbox(
+	IN  UCHAR               a);
+//
+// Expanded IV for TKIP function.
+//
+typedef	struct	PACKED _IV_CONTROL_
+{
+	union PACKED
+	{
+		struct PACKED
+		{
+			UCHAR		rc0;
+			UCHAR		rc1;
+			UCHAR		rc2;
+
+			union PACKED
+			{
+				struct PACKED
+				{
+#ifdef RT_BIG_ENDIAN
+					UCHAR	KeyID:2;
+					UCHAR	ExtIV:1;
+					UCHAR	Rsvd:5;
+#else
+					UCHAR	Rsvd:5;
+					UCHAR	ExtIV:1;
+					UCHAR	KeyID:2;
+#endif
+				}	field;
+				UCHAR		Byte;
+			}	CONTROL;
+		}	field;
+
+		ULONG	word;
+	}	IV16;
+
+	ULONG	IV32;
+}	TKIP_IV, *PTKIP_IV;
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Convert from UCHAR[] to ULONG in a portable way
+
+	Arguments:
+      pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+ULONG	RTMPTkipGetUInt32(
+	IN	PUCHAR	pMICKey)
+{
+	ULONG	res = 0;
+	INT		i;
+
+	for (i = 0; i < 4; i++)
+	{
+		res |= (*pMICKey++) << (8 * i);
+	}
+
+	return res;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Convert from ULONG to UCHAR[] in a portable way
+
+	Arguments:
+      pDst			pointer to destination for convert ULONG to UCHAR[]
+      val			the value for convert
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPTkipPutUInt32(
+	IN OUT	PUCHAR		pDst,
+	IN		ULONG		val)
+{
+	INT i;
+
+	for(i = 0; i < 4; i++)
+	{
+		*pDst++ = (UCHAR) (val & 0xff);
+		val >>= 8;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Set the MIC Key.
+
+	Arguments:
+      pAd		Pointer to our adapter
+      pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPTkipSetMICKey(
+	IN	PTKIP_KEY_INFO	pTkip,
+	IN	PUCHAR			pMICKey)
+{
+	// Set the key
+	pTkip->K0 = RTMPTkipGetUInt32(pMICKey);
+	pTkip->K1 = RTMPTkipGetUInt32(pMICKey + 4);
+	// and reset the message
+	pTkip->L = pTkip->K0;
+	pTkip->R = pTkip->K1;
+	pTkip->nBytesInM = 0;
+	pTkip->M = 0;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Calculate the MIC Value.
+
+	Arguments:
+      pAd		Pointer to our adapter
+      uChar			Append this uChar
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPTkipAppendByte(
+	IN	PTKIP_KEY_INFO	pTkip,
+	IN	UCHAR 			uChar)
+{
+	// Append the byte to our word-sized buffer
+	pTkip->M |= (uChar << (8* pTkip->nBytesInM));
+	pTkip->nBytesInM++;
+	// Process the word if it is full.
+	if( pTkip->nBytesInM >= 4 )
+	{
+		pTkip->L ^= pTkip->M;
+		pTkip->R ^= ROL32( pTkip->L, 17 );
+		pTkip->L += pTkip->R;
+		pTkip->R ^= ((pTkip->L & 0xff00ff00) >> 8) | ((pTkip->L & 0x00ff00ff) << 8);
+		pTkip->L += pTkip->R;
+		pTkip->R ^= ROL32( pTkip->L, 3 );
+		pTkip->L += pTkip->R;
+		pTkip->R ^= ROR32( pTkip->L, 2 );
+		pTkip->L += pTkip->R;
+		// Clear the buffer
+		pTkip->M = 0;
+		pTkip->nBytesInM = 0;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Calculate the MIC Value.
+
+	Arguments:
+      pAd		Pointer to our adapter
+      pSrc			Pointer to source data for Calculate MIC Value
+      Len			Indicate the length of the source data
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPTkipAppend(
+	IN	PTKIP_KEY_INFO	pTkip,
+	IN	PUCHAR			pSrc,
+	IN	UINT			nBytes)
+{
+	// This is simple
+	while(nBytes > 0)
+	{
+		RTMPTkipAppendByte(pTkip, *pSrc++);
+		nBytes--;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Get the MIC Value.
+
+	Arguments:
+      pAd		Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		the MIC Value is store in pAd->PrivateInfo.MIC
+	========================================================================
+*/
+VOID	RTMPTkipGetMIC(
+	IN	PTKIP_KEY_INFO	pTkip)
+{
+	// Append the minimum padding
+	RTMPTkipAppendByte(pTkip, 0x5a );
+	RTMPTkipAppendByte(pTkip, 0 );
+	RTMPTkipAppendByte(pTkip, 0 );
+	RTMPTkipAppendByte(pTkip, 0 );
+	RTMPTkipAppendByte(pTkip, 0 );
+	// and then zeroes until the length is a multiple of 4
+	while( pTkip->nBytesInM != 0 )
+	{
+		RTMPTkipAppendByte(pTkip, 0 );
+	}
+	// The appendByte function has already computed the result.
+	RTMPTkipPutUInt32(pTkip->MIC, pTkip->L);
+	RTMPTkipPutUInt32(pTkip->MIC + 4, pTkip->R);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Init Tkip function.
+
+	Arguments:
+      pAd		Pointer to our adapter
+		pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
+		KeyId		TK Key ID
+		pTA			Pointer to transmitter address
+		pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitTkipEngine(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pKey,
+	IN	UCHAR			KeyId,
+	IN	PUCHAR			pTA,
+	IN	PUCHAR			pMICKey,
+	IN	PUCHAR			pTSC,
+	OUT	PULONG			pIV16,
+	OUT	PULONG			pIV32)
+{
+	TKIP_IV	tkipIv;
+
+	// Prepare 8 bytes TKIP encapsulation for MPDU
+	NdisZeroMemory(&tkipIv, sizeof(TKIP_IV));
+	tkipIv.IV16.field.rc0 = *(pTSC + 1);
+	tkipIv.IV16.field.rc1 = (tkipIv.IV16.field.rc0 | 0x20) & 0x7f;
+	tkipIv.IV16.field.rc2 = *pTSC;
+	tkipIv.IV16.field.CONTROL.field.ExtIV = 1;  // 0: non-extended IV, 1: an extended IV
+	tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;
+	NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4);   // Copy IV
+
+	*pIV16 = tkipIv.IV16.word;
+	*pIV32 = tkipIv.IV32;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Init MIC Value calculation function which include set MIC key &
+		calculate first 16 bytes (DA + SA + priority +  0)
+
+	Arguments:
+      pAd		Pointer to our adapter
+		pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
+		pDA			Pointer to DA address
+		pSA			Pointer to SA address
+		pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitMICEngine(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pKey,
+	IN	PUCHAR			pDA,
+	IN	PUCHAR			pSA,
+	IN  UCHAR           UserPriority,
+	IN	PUCHAR			pMICKey)
+{
+	ULONG Priority = UserPriority;
+
+	// Init MIC value calculation
+	RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
+	// DA
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
+	// SA
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
+	// Priority + 3 bytes of 0
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Compare MIC value of received MSDU
+
+	Arguments:
+		pAd	Pointer to our adapter
+		pSrc        Pointer to the received Plain text data
+		pDA			Pointer to DA address
+		pSA			Pointer to SA address
+		pMICKey		pointer to MIC Key
+		Len         the length of the received plain text data exclude MIC value
+
+	Return Value:
+		TRUE        MIC value matched
+		FALSE       MIC value mismatched
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN	RTMPTkipCompareMICValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pSrc,
+	IN	PUCHAR			pDA,
+	IN	PUCHAR			pSA,
+	IN	PUCHAR			pMICKey,
+	IN	UCHAR			UserPriority,
+	IN	UINT			Len)
+{
+	UCHAR	OldMic[8];
+	ULONG	Priority = UserPriority;
+
+	// Init MIC value calculation
+	RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
+	// DA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
+	// SA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
+	// Priority + 3 bytes of 0
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
+
+	// Calculate MIC value from plain text data
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
+
+	// Get MIC valude from received frame
+	NdisMoveMemory(OldMic, pSrc + Len, 8);
+
+	// Get MIC value from decrypted plain data
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
+
+	// Move MIC value from MSDU, this steps should move to data path.
+	// Since the MIC value might cross MPDUs.
+	if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n"));  //MIC error.
+
+
+		return (FALSE);
+	}
+	return (TRUE);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Compare MIC value of received MSDU
+
+	Arguments:
+		pAd	Pointer to our adapter
+		pLLC		LLC header
+		pSrc        Pointer to the received Plain text data
+		pDA			Pointer to DA address
+		pSA			Pointer to SA address
+		pMICKey		pointer to MIC Key
+		Len         the length of the received plain text data exclude MIC value
+
+	Return Value:
+		TRUE        MIC value matched
+		FALSE       MIC value mismatched
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN	RTMPTkipCompareMICValueWithLLC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pLLC,
+	IN	PUCHAR			pSrc,
+	IN	PUCHAR			pDA,
+	IN	PUCHAR			pSA,
+	IN	PUCHAR			pMICKey,
+	IN	UINT			Len)
+{
+	UCHAR	OldMic[8];
+	ULONG	Priority = 0;
+
+	// Init MIC value calculation
+	RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
+	// DA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
+	// SA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
+	// Priority + 3 bytes of 0
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
+
+	// Start with LLC header
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pLLC, 8);
+
+	// Calculate MIC value from plain text data
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
+
+	// Get MIC valude from received frame
+	NdisMoveMemory(OldMic, pSrc + Len, 8);
+
+	// Get MIC value from decrypted plain data
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
+
+	// Move MIC value from MSDU, this steps should move to data path.
+	// Since the MIC value might cross MPDUs.
+	if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValueWithLLC(): TKIP MIC Error !\n"));  //MIC error.
+
+
+		return (FALSE);
+	}
+	return (TRUE);
+}
+/*
+	========================================================================
+
+	Routine	Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware transmit function
+
+	Arguments:
+		pAd		Pointer	to our adapter
+		PNDIS_PACKET	Pointer to Ndis Packet for MIC calculation
+		pEncap			Pointer to LLC encap data
+		LenEncap		Total encap length, might be 0 which indicates no encap
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPCalculateMICValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			pEncap,
+	IN	PCIPHER_KEY		pKey,
+	IN	UCHAR			apidx)
+{
+	PACKET_INFO		PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	PUCHAR			pSrc;
+    UCHAR           UserPriority;
+	UCHAR			vlan_offset = 0;
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+
+	UserPriority = RTMP_GET_PACKET_UP(pPacket);
+	pSrc = pSrcBufVA;
+
+	// determine if this is a vlan packet
+	if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
+		vlan_offset = 4;
+
+	{
+		RTMPInitMICEngine(
+			pAd,
+			pKey->Key,
+			pSrc,
+			pSrc + 6,
+			UserPriority,
+			pKey->TxMic);
+	}
+
+
+	if (pEncap != NULL)
+	{
+		// LLC encapsulation
+		RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
+		// Protocol Type
+		RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset, 2);
+	}
+	SrcBufLen -= (14 + vlan_offset);
+	pSrc += (14 + vlan_offset);
+	do
+	{
+		if (SrcBufLen > 0)
+		{
+			RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
+		}
+
+		break;	// No need handle next packet
+
+	}	while (TRUE);		// End of copying payload
+
+	// Compute the final MIC Value
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
+}
+
+
+/************************************************************/
+/* tkip_sbox()																*/
+/* Returns a 16 bit value from a 64K entry table. The Table */
+/* is synthesized from two 256 entry byte wide tables.		*/
+/************************************************************/
+
+UINT tkip_sbox(UINT index)
+{
+	UINT index_low;
+	UINT index_high;
+	UINT left, right;
+
+	index_low = (index % 256);
+	index_high = ((index >> 8) % 256);
+
+	left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256);
+	right = Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256);
+
+	return (left ^ right);
+}
+
+UINT rotr1(UINT a)
+{
+	unsigned int b;
+
+	if ((a & 0x01) == 0x01)
+	{
+		b = (a >> 1) | 0x8000;
+	}
+	else
+	{
+		b = (a >> 1) & 0x7fff;
+	}
+	b = b % 65536;
+	return b;
+}
+
+VOID RTMPTkipMixKey(
+	UCHAR *key,
+	UCHAR *ta,
+	ULONG pnl, /* Least significant 16 bits of PN */
+	ULONG pnh, /* Most significant 32 bits of PN */
+	UCHAR *rc4key,
+	UINT *p1k)
+{
+
+	UINT tsc0;
+	UINT tsc1;
+	UINT tsc2;
+
+	UINT ppk0;
+	UINT ppk1;
+	UINT ppk2;
+	UINT ppk3;
+	UINT ppk4;
+	UINT ppk5;
+
+	INT i;
+	INT j;
+
+	tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
+	tsc1 = (unsigned int)(pnh % 65536);
+	tsc2 = (unsigned int)(pnl % 65536); /* lsb */
+
+	/* Phase 1, step 1 */
+	p1k[0] = tsc1;
+	p1k[1] = tsc0;
+	p1k[2] = (UINT)(ta[0] + (ta[1]*256));
+	p1k[3] = (UINT)(ta[2] + (ta[3]*256));
+	p1k[4] = (UINT)(ta[4] + (ta[5]*256));
+
+	/* Phase 1, step 2 */
+	for (i=0; i<8; i++)
+	{
+		j = 2*(i & 1);
+		p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536;
+		p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536;
+		p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536;
+		p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536;
+		p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536;
+		p1k[4] = (p1k[4] + i) % 65536;
+	}
+
+	/* Phase 2, Step 1 */
+	ppk0 = p1k[0];
+	ppk1 = p1k[1];
+	ppk2 = p1k[2];
+	ppk3 = p1k[3];
+	ppk4 = p1k[4];
+	ppk5 = (p1k[4] + tsc2) % 65536;
+
+	/* Phase2, Step 2 */
+	ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536);
+	ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536);
+	ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536);
+	ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536);
+	ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536);
+	ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536);
+
+	ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12]));
+	ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14]));
+	ppk2 = ppk2 + rotr1(ppk1);
+	ppk3 = ppk3 + rotr1(ppk2);
+	ppk4 = ppk4 + rotr1(ppk3);
+	ppk5 = ppk5 + rotr1(ppk4);
+
+	/* Phase 2, Step 3 */
+    /* Phase 2, Step 3 */
+
+	tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
+	tsc1 = (unsigned int)(pnh % 65536);
+	tsc2 = (unsigned int)(pnl % 65536); /* lsb */
+
+	rc4key[0] = (tsc2 >> 8) % 256;
+	rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
+	rc4key[2] = tsc2 % 256;
+	rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256;
+
+	rc4key[4] = ppk0 % 256;
+	rc4key[5] = (ppk0 >> 8) % 256;
+
+	rc4key[6] = ppk1 % 256;
+	rc4key[7] = (ppk1 >> 8) % 256;
+
+	rc4key[8] = ppk2 % 256;
+	rc4key[9] = (ppk2 >> 8) % 256;
+
+	rc4key[10] = ppk3 % 256;
+	rc4key[11] = (ppk3 >> 8) % 256;
+
+	rc4key[12] = ppk4 % 256;
+	rc4key[13] = (ppk4 >> 8) % 256;
+
+	rc4key[14] = ppk5 % 256;
+	rc4key[15] = (ppk5 >> 8) % 256;
+}
+
+
+/************************************************/
+/* construct_mic_header1()                      */
+/* Builds the first MIC header block from       */
+/* header fields.                               */
+/************************************************/
+
+void construct_mic_header1(
+	unsigned char *mic_header1,
+	int header_length,
+	unsigned char *mpdu)
+{
+	mic_header1[0] = (unsigned char)((header_length - 2) / 256);
+	mic_header1[1] = (unsigned char)((header_length - 2) % 256);
+	mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
+	mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
+	mic_header1[4] = mpdu[4];       /* A1 */
+	mic_header1[5] = mpdu[5];
+	mic_header1[6] = mpdu[6];
+	mic_header1[7] = mpdu[7];
+	mic_header1[8] = mpdu[8];
+	mic_header1[9] = mpdu[9];
+	mic_header1[10] = mpdu[10];     /* A2 */
+	mic_header1[11] = mpdu[11];
+	mic_header1[12] = mpdu[12];
+	mic_header1[13] = mpdu[13];
+	mic_header1[14] = mpdu[14];
+	mic_header1[15] = mpdu[15];
+}
+
+/************************************************/
+/* construct_mic_header2()                      */
+/* Builds the last MIC header block from        */
+/* header fields.                               */
+/************************************************/
+
+void construct_mic_header2(
+	unsigned char *mic_header2,
+	unsigned char *mpdu,
+	int a4_exists,
+	int qc_exists)
+{
+	int i;
+
+	for (i = 0; i<16; i++) mic_header2[i]=0x00;
+
+	mic_header2[0] = mpdu[16];    /* A3 */
+	mic_header2[1] = mpdu[17];
+	mic_header2[2] = mpdu[18];
+	mic_header2[3] = mpdu[19];
+	mic_header2[4] = mpdu[20];
+	mic_header2[5] = mpdu[21];
+
+	// In Sequence Control field, mute sequence numer bits (12-bit)
+	mic_header2[6] = mpdu[22] & 0x0f;   /* SC */
+	mic_header2[7] = 0x00; /* mpdu[23]; */
+
+	if ((!qc_exists) & a4_exists)
+	{
+		for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
+
+	}
+
+	if (qc_exists && (!a4_exists))
+	{
+		mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
+		mic_header2[9] = mpdu[25] & 0x00;
+	}
+
+	if (qc_exists && a4_exists)
+	{
+		for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
+
+		mic_header2[14] = mpdu[30] & 0x0f;
+		mic_header2[15] = mpdu[31] & 0x00;
+	}
+}
+
+
+/************************************************/
+/* construct_mic_iv()                           */
+/* Builds the MIC IV from header fields and PN  */
+/************************************************/
+
+void construct_mic_iv(
+	unsigned char *mic_iv,
+	int qc_exists,
+	int a4_exists,
+	unsigned char *mpdu,
+	unsigned int payload_length,
+	unsigned char *pn_vector)
+{
+	int i;
+
+	mic_iv[0] = 0x59;
+	if (qc_exists && a4_exists)
+		mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC           */
+	if (qc_exists && !a4_exists)
+		mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4    */
+	if (!qc_exists)
+		mic_iv[1] = 0x00;
+	for (i = 2; i < 8; i++)
+		mic_iv[i] = mpdu[i + 8];                    /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
+#ifdef CONSISTENT_PN_ORDER
+		for (i = 8; i < 14; i++)
+			mic_iv[i] = pn_vector[i - 8];           /* mic_iv[8:13] = PN[0:5] */
+#else
+		for (i = 8; i < 14; i++)
+			mic_iv[i] = pn_vector[13 - i];          /* mic_iv[8:13] = PN[5:0] */
+#endif
+	i = (payload_length / 256);
+	i = (payload_length % 256);
+	mic_iv[14] = (unsigned char) (payload_length / 256);
+	mic_iv[15] = (unsigned char) (payload_length % 256);
+
+}
+
+
+
+/************************************/
+/* bitwise_xor()                    */
+/* A 128 bit, bitwise exclusive or  */
+/************************************/
+
+void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out)
+{
+	int i;
+	for (i=0; i<16; i++)
+	{
+		out[i] = ina[i] ^ inb[i];
+	}
+}
+
+
+void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
+{
+	int round;
+	int i;
+	unsigned char intermediatea[16];
+	unsigned char intermediateb[16];
+	unsigned char round_key[16];
+
+	for(i=0; i<16; i++) round_key[i] = key[i];
+
+	for (round = 0; round < 11; round++)
+	{
+		if (round == 0)
+		{
+			xor_128(round_key, data, ciphertext);
+			next_key(round_key, round);
+		}
+		else if (round == 10)
+		{
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			xor_128(intermediateb, round_key, ciphertext);
+		}
+		else    /* 1 - 9 */
+		{
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			mix_column(&intermediateb[0], &intermediatea[0]);
+			mix_column(&intermediateb[4], &intermediatea[4]);
+			mix_column(&intermediateb[8], &intermediatea[8]);
+			mix_column(&intermediateb[12], &intermediatea[12]);
+			xor_128(intermediatea, round_key, ciphertext);
+			next_key(round_key, round);
+		}
+	}
+
+}
+
+void construct_ctr_preload(
+	unsigned char *ctr_preload,
+	int a4_exists,
+	int qc_exists,
+	unsigned char *mpdu,
+	unsigned char *pn_vector,
+	int c)
+{
+
+	int i = 0;
+	for (i=0; i<16; i++) ctr_preload[i] = 0x00;
+	i = 0;
+
+	ctr_preload[0] = 0x01;                                  /* flag */
+	if (qc_exists && a4_exists) ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control  */
+	if (qc_exists && !a4_exists) ctr_preload[1] = mpdu[24] & 0x0f;
+
+	for (i = 2; i < 8; i++)
+		ctr_preload[i] = mpdu[i + 8];                       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
+#ifdef CONSISTENT_PN_ORDER
+	  for (i = 8; i < 14; i++)
+			ctr_preload[i] =    pn_vector[i - 8];           /* ctr_preload[8:13] = PN[0:5] */
+#else
+	  for (i = 8; i < 14; i++)
+			ctr_preload[i] =    pn_vector[13 - i];          /* ctr_preload[8:13] = PN[5:0] */
+#endif
+	ctr_preload[14] =  (unsigned char) (c / 256); // Ctr
+	ctr_preload[15] =  (unsigned char) (c % 256);
+
+}
+
+
+//
+// TRUE: Success!
+// FALSE: Decrypt Error!
+//
+BOOLEAN RTMPSoftDecryptTKIP(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN UCHAR    UserPriority,
+	IN PCIPHER_KEY	pWpaKey)
+{
+	UCHAR			KeyID;
+	UINT			HeaderLen;
+    UCHAR			fc0;
+	UCHAR			fc1;
+	USHORT			fc;
+	UINT			frame_type;
+	UINT			frame_subtype;
+    UINT			from_ds;
+    UINT			to_ds;
+	INT				a4_exists;
+	INT				qc_exists;
+	USHORT			duration;
+	USHORT			seq_control;
+	USHORT			qos_control;
+	UCHAR			TA[MAC_ADDR_LEN];
+	UCHAR			DA[MAC_ADDR_LEN];
+	UCHAR			SA[MAC_ADDR_LEN];
+	UCHAR			RC4Key[16];
+	UINT			p1k[5]; //for mix_key;
+	ULONG			pnl;/* Least significant 16 bits of PN */
+	ULONG			pnh;/* Most significant 32 bits of PN */
+	UINT			num_blocks;
+	UINT			payload_remainder;
+	ARCFOURCONTEXT 	ArcFourContext;
+	UINT			crc32 = 0;
+	UINT			trailfcs = 0;
+	UCHAR			MIC[8];
+	UCHAR			TrailMIC[8];
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+	fc0 = *pData;
+	fc1 = *(pData + 1);
+
+	fc = *((PUSHORT)pData);
+
+	frame_type = ((fc0 >> 2) & 0x03);
+	frame_subtype = ((fc0 >> 4) & 0x0f);
+
+    from_ds = (fc1 & 0x2) >> 1;
+    to_ds = (fc1 & 0x1);
+
+    a4_exists = (from_ds & to_ds);
+    qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
+                  (frame_subtype == 0x09) ||   /* Likely to change.    */
+                  (frame_subtype == 0x0a) ||
+                  (frame_subtype == 0x0b)
+                 );
+
+	HeaderLen = 24;
+	if (a4_exists)
+		HeaderLen += 6;
+
+	KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
+	KeyID = KeyID >> 6;
+
+	if (pWpaKey[KeyID].KeyLen == 0)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID));
+		return FALSE;
+	}
+
+	duration = *((PUSHORT)(pData+2));
+
+	seq_control = *((PUSHORT)(pData+22));
+
+	if (qc_exists)
+	{
+		if (a4_exists)
+		{
+			qos_control = *((PUSHORT)(pData+30));
+		}
+		else
+		{
+			qos_control = *((PUSHORT)(pData+24));
+		}
+	}
+
+	if (to_ds == 0 && from_ds == 1)
+	{
+		NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
+		NdisMoveMemory(SA, pData+16, MAC_ADDR_LEN);
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);  //BSSID
+	}
+	else if (to_ds == 0 && from_ds == 0 )
+	{
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
+		NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
+	}
+	else if (to_ds == 1 && from_ds == 0)
+	{
+		NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
+	}
+	else if (to_ds == 1 && from_ds == 1)
+	{
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
+		NdisMoveMemory(SA, pData+22, MAC_ADDR_LEN);
+	}
+
+	num_blocks = (DataByteCnt - 16) / 16;
+	payload_remainder = (DataByteCnt - 16) % 16;
+
+	pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
+	pnh = *((PULONG)(pData + HeaderLen + 4));
+	pnh = cpu2le32(pnh);
+	RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
+
+	ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
+
+	ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
+	NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
+	crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4);  //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
+	crc32 ^= 0xffffffff;             /* complement */
+
+    if(crc32 != cpu2le32(trailfcs))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP, WEP Data ICV Error !\n"));	 //ICV error.
+
+		return (FALSE);
+	}
+
+	NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
+	RTMPInitMICEngine(pAd, pWpaKey[KeyID].Key, DA, SA, UserPriority, pWpaKey[KeyID].RxMic);
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
+	NdisMoveMemory(MIC, pAd->PrivateInfo.Tx.MIC, 8);
+
+	if (!NdisEqualMemory(MIC, TrailMIC, 8))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptTKIP, WEP Data MIC Error !\n"));	 //MIC error.
+		return (FALSE);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+	return TRUE;
+}
+
+
+
+
+BOOLEAN RTMPSoftDecryptAES(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN PCIPHER_KEY	pWpaKey)
+{
+	UCHAR			KeyID;
+	UINT			HeaderLen;
+	UCHAR			PN[6];
+	UINT			payload_len;
+	UINT			num_blocks;
+	UINT			payload_remainder;
+	USHORT			fc;
+	UCHAR			fc0;
+	UCHAR			fc1;
+	UINT			frame_type;
+	UINT			frame_subtype;
+	UINT			from_ds;
+	UINT			to_ds;
+	INT				a4_exists;
+	INT				qc_exists;
+	UCHAR			aes_out[16];
+	int 			payload_index;
+	UINT 			i;
+	UCHAR 			ctr_preload[16];
+	UCHAR 			chain_buffer[16];
+	UCHAR 			padded_buffer[16];
+	UCHAR 			mic_iv[16];
+	UCHAR 			mic_header1[16];
+	UCHAR 			mic_header2[16];
+	UCHAR			MIC[8];
+	UCHAR			TrailMIC[8];
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+	fc0 = *pData;
+	fc1 = *(pData + 1);
+
+	fc = *((PUSHORT)pData);
+
+	frame_type = ((fc0 >> 2) & 0x03);
+	frame_subtype = ((fc0 >> 4) & 0x0f);
+
+	from_ds = (fc1 & 0x2) >> 1;
+	to_ds = (fc1 & 0x1);
+
+	a4_exists = (from_ds & to_ds);
+	qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
+				  (frame_subtype == 0x09) ||   /* Likely to change.    */
+				  (frame_subtype == 0x0a) ||
+				  (frame_subtype == 0x0b)
+				 );
+
+	HeaderLen = 24;
+	if (a4_exists)
+		HeaderLen += 6;
+
+	KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
+	KeyID = KeyID >> 6;
+
+	if (pWpaKey[KeyID].KeyLen == 0)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptAES failed!(KeyID[%d] Length can not be 0)\n", KeyID));
+		return FALSE;
+	}
+
+	PN[0] = *(pData+ HeaderLen);
+	PN[1] = *(pData+ HeaderLen + 1);
+	PN[2] = *(pData+ HeaderLen + 4);
+	PN[3] = *(pData+ HeaderLen + 5);
+	PN[4] = *(pData+ HeaderLen + 6);
+	PN[5] = *(pData+ HeaderLen + 7);
+
+	payload_len = DataByteCnt - HeaderLen - 8 - 8;	// 8 bytes for CCMP header , 8 bytes for MIC
+	payload_remainder = (payload_len) % 16;
+	num_blocks = (payload_len) / 16;
+
+
+
+	// Find start of payload
+	payload_index = HeaderLen + 8; //IV+EIV
+
+	for (i=0; i< num_blocks; i++)
+	{
+		construct_ctr_preload(ctr_preload,
+								a4_exists,
+								qc_exists,
+								pData,
+								PN,
+								i+1 );
+
+		aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
+
+		bitwise_xor(aes_out, pData + payload_index, chain_buffer);
+		NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
+		payload_index += 16;
+	}
+
+	//
+	// If there is a short final block, then pad it
+	// encrypt it and copy the unpadded part back
+	//
+	if (payload_remainder > 0)
+	{
+		construct_ctr_preload(ctr_preload,
+								a4_exists,
+								qc_exists,
+								pData,
+								PN,
+								num_blocks + 1);
+
+		NdisZeroMemory(padded_buffer, 16);
+		NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
+
+		aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
+
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
+		payload_index += payload_remainder;
+	}
+
+	//
+	// Descrypt the MIC
+	//
+	construct_ctr_preload(ctr_preload,
+							a4_exists,
+							qc_exists,
+							pData,
+							PN,
+							0);
+	NdisZeroMemory(padded_buffer, 16);
+	NdisMoveMemory(padded_buffer, pData + payload_index, 8);
+
+	aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
+
+	bitwise_xor(aes_out, padded_buffer, chain_buffer);
+
+	NdisMoveMemory(TrailMIC, chain_buffer, 8);
+
+	//
+	// Calculate MIC
+	//
+
+	//Force the protected frame bit on
+	*(pData + 1) = *(pData + 1) | 0x40;
+
+	// Find start of payload
+	// Because the CCMP header has been removed
+	payload_index = HeaderLen;
+
+	construct_mic_iv(
+					mic_iv,
+					qc_exists,
+					a4_exists,
+					pData,
+					payload_len,
+					PN);
+
+	construct_mic_header1(
+						mic_header1,
+						HeaderLen,
+						pData);
+
+	construct_mic_header2(
+						mic_header2,
+						pData,
+						a4_exists,
+						qc_exists);
+
+	aes128k128d(pWpaKey[KeyID].Key, mic_iv, aes_out);
+	bitwise_xor(aes_out, mic_header1, chain_buffer);
+	aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+	bitwise_xor(aes_out, mic_header2, chain_buffer);
+	aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+
+	// iterate through each 16 byte payload block
+	for (i = 0; i < num_blocks; i++)
+	{
+		bitwise_xor(aes_out, pData + payload_index, chain_buffer);
+		payload_index += 16;
+		aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+	}
+
+	// Add on the final payload block if it needs padding
+	if (payload_remainder > 0)
+	{
+		NdisZeroMemory(padded_buffer, 16);
+		NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
+
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+	}
+	// aes_out contains padded mic, discard most significant
+	// 8 bytes to generate 64 bit MIC
+	for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];
+
+	if (!NdisEqualMemory(MIC, TrailMIC, 8))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptAES, MIC Error !\n"));	 //MIC error.
+		return FALSE;
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+	return TRUE;
+}
+
+/****************************************/
+/* aes128k128d()                        */
+/* Performs a 128 bit AES encrypt with  */
+/* 128 bit data.                        */
+/****************************************/
+VOID xor_128(
+	IN  PUCHAR  a,
+	IN  PUCHAR  b,
+	OUT PUCHAR  out)
+{
+	INT i;
+
+	for (i=0;i<16; i++)
+	{
+		out[i] = a[i] ^ b[i];
+	}
+}
+
+VOID next_key(
+	IN  PUCHAR  key,
+	IN  INT     round)
+{
+	UCHAR       rcon;
+	UCHAR       sbox_key[4];
+	UCHAR       rcon_table[12] =
+	{
+		0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
+		0x1b, 0x36, 0x36, 0x36
+	};
+
+	sbox_key[0] = RTMPCkipSbox(key[13]);
+	sbox_key[1] = RTMPCkipSbox(key[14]);
+	sbox_key[2] = RTMPCkipSbox(key[15]);
+	sbox_key[3] = RTMPCkipSbox(key[12]);
+
+	rcon = rcon_table[round];
+
+	xor_32(&key[0], sbox_key, &key[0]);
+	key[0] = key[0] ^ rcon;
+
+	xor_32(&key[4], &key[0], &key[4]);
+	xor_32(&key[8], &key[4], &key[8]);
+	xor_32(&key[12], &key[8], &key[12]);
+}
+
+VOID xor_32(
+	IN  PUCHAR  a,
+	IN  PUCHAR  b,
+	OUT PUCHAR  out)
+{
+	INT i;
+
+	for (i=0;i<4; i++)
+	{
+		out[i] = a[i] ^ b[i];
+	}
+}
+
+VOID byte_sub(
+	IN  PUCHAR  in,
+	OUT PUCHAR  out)
+{
+	INT i;
+
+	for (i=0; i< 16; i++)
+	{
+		out[i] = RTMPCkipSbox(in[i]);
+	}
+}
+
+UCHAR RTMPCkipSbox(
+	IN  UCHAR   a)
+{
+	return SboxTable[(int)a];
+}
+
+VOID shift_row(
+	IN  PUCHAR  in,
+	OUT PUCHAR  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];
+}
+
+VOID mix_column(
+	IN  PUCHAR  in,
+	OUT PUCHAR  out)
+{
+	INT         i;
+	UCHAR       add1b[4];
+	UCHAR       add1bf7[4];
+	UCHAR       rotl[4];
+	UCHAR       swap_halfs[4];
+	UCHAR       andf7[4];
+	UCHAR       rotr[4];
+	UCHAR       temp[4];
+	UCHAR       tempb[4];
+
+	for (i=0 ; i<4; i++)
+	{
+		if ((in[i] & 0x80)== 0x80)
+			add1b[i] = 0x1b;
+		else
+			add1b[i] = 0x00;
+	}
+
+	swap_halfs[0] = in[2];    /* Swap halfs */
+	swap_halfs[1] = in[3];
+	swap_halfs[2] = in[0];
+	swap_halfs[3] = in[1];
+
+	rotl[0] = in[3];        /* Rotate left 8 bits */
+	rotl[1] = in[0];
+	rotl[2] = in[1];
+	rotl[3] = in[2];
+
+	andf7[0] = in[0] & 0x7f;
+	andf7[1] = in[1] & 0x7f;
+	andf7[2] = in[2] & 0x7f;
+	andf7[3] = in[3] & 0x7f;
+
+	for (i = 3; i>0; i--)    /* logical shift left 1 bit */
+	{
+		andf7[i] = andf7[i] << 1;
+		if ((andf7[i-1] & 0x80) == 0x80)
+		{
+			andf7[i] = (andf7[i] | 0x01);
+		}
+	}
+	andf7[0] = andf7[0] << 1;
+	andf7[0] = andf7[0] & 0xfe;
+
+	xor_32(add1b, andf7, add1bf7);
+
+	xor_32(in, add1bf7, rotr);
+
+	temp[0] = rotr[0];         /* Rotate right 8 bits */
+	rotr[0] = rotr[1];
+	rotr[1] = rotr[2];
+	rotr[2] = rotr[3];
+	rotr[3] = temp[0];
+
+	xor_32(add1bf7, rotr, temp);
+	xor_32(swap_halfs, rotl,tempb);
+	xor_32(temp, tempb, out);
+}
+
diff --git a/drivers/staging/rt2860/common/rtmp_wep.c b/drivers/staging/rt2860/common/rtmp_wep.c
new file mode 100644
index 000000000000..ffe26c237950
--- /dev/null
+++ b/drivers/staging/rt2860/common/rtmp_wep.c
@@ -0,0 +1,499 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_wep.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Paul Wu		10-28-02		Initial
+*/
+
+#include "../rt_config.h"
+
+UINT FCSTAB_32[256] =
+{
+	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba,
+	0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
+	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
+	0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
+	0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de,
+	0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec,
+	0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
+	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
+	0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
+	0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940,
+	0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116,
+	0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
+	0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
+	0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
+	0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a,
+	0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818,
+	0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
+	0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
+	0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
+	0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c,
+	0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2,
+	0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
+	0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
+	0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
+	0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086,
+	0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4,
+	0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
+	0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
+	0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
+	0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8,
+	0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe,
+	0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
+	0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
+	0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
+	0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252,
+	0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60,
+	0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
+	0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
+	0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
+	0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04,
+	0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a,
+	0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
+	0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
+	0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
+	0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e,
+	0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c,
+	0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
+	0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
+	0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
+	0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0,
+	0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6,
+	0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
+	0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
+	0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
+};
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Init WEP function.
+
+	Arguments:
+      pAd		Pointer to our adapter
+		pKey        Pointer to the WEP KEY
+		KeyId		   WEP Key ID
+		KeyLen      the length of WEP KEY
+		pDest       Pointer to the destination which Encryption data will store in.
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitWepEngine(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pKey,
+	IN	UCHAR			KeyId,
+	IN	UCHAR			KeyLen,
+	IN OUT	PUCHAR		pDest)
+{
+	UINT i;
+	UCHAR   WEPKEY[] = {
+		//IV
+		0x00, 0x11, 0x22,
+		//WEP KEY
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xAA, 0xBB, 0xCC
+	};
+
+	pAd->PrivateInfo.FCSCRC32 = PPPINITFCS32;   //Init crc32.
+
+#ifdef CONFIG_STA_SUPPORT
+    if (pAd->StaCfg.bCkipOn && (pAd->StaCfg.CkipFlag & 0x10) && (pAd->OpMode == OPMODE_STA))
+    {
+        ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, pKey, KeyLen);  //INIT SBOX, KEYLEN+3(IV)
+        NdisMoveMemory(pDest, pKey, 3);  //Append Init Vector
+    }
+    else
+#endif // CONFIG_STA_SUPPORT //
+    {
+		NdisMoveMemory(WEPKEY + 3, pKey, KeyLen);
+
+        for(i = 0; i < 3; i++)
+			WEPKEY[i] = RandomByte(pAd);   //Call mlme RandomByte() function.
+		ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, WEPKEY, KeyLen + 3);  //INIT SBOX, KEYLEN+3(IV)
+
+		NdisMoveMemory(pDest, WEPKEY, 3);  //Append Init Vector
+    }
+	*(pDest+3) = (KeyId << 6);       //Append KEYID
+
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Encrypt transimitted data
+
+	Arguments:
+      pAd		Pointer to our adapter
+      pSrc        Pointer to the transimitted source data that will be encrypt
+      pDest       Pointer to the destination where entryption data will be store in.
+      Len			Indicate the length of the source data
+
+	Return Value:
+      None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPEncryptData(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pSrc,
+	IN	PUCHAR			pDest,
+	IN	UINT			Len)
+{
+	pAd->PrivateInfo.FCSCRC32 = RTMP_CALC_FCS32(pAd->PrivateInfo.FCSCRC32, pSrc, Len);
+	ARCFOUR_ENCRYPT(&pAd->PrivateInfo.WEPCONTEXT, pDest, pSrc, Len);
+}
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Decrypt received WEP data
+
+	Arguments:
+		pAdapter		Pointer to our adapter
+		pSrc        Pointer to the received data
+		Len         the length of the received data
+
+	Return Value:
+		TRUE        Decrypt WEP data success
+		FALSE       Decrypt WEP data failed
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN	RTMPSoftDecryptWEP(
+	IN PRTMP_ADAPTER 	pAd,
+	IN PUCHAR			pData,
+	IN ULONG			DataByteCnt,
+	IN PCIPHER_KEY		pGroupKey)
+{
+	UINT	trailfcs;
+	UINT    crc32;
+	UCHAR	KeyIdx;
+	UCHAR   WEPKEY[] = {
+		//IV
+		0x00, 0x11, 0x22,
+		//WEP KEY
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xAA, 0xBB, 0xCC
+	};
+	UCHAR 	*pPayload = (UCHAR *)pData + LENGTH_802_11;
+	ULONG	payload_len = DataByteCnt - LENGTH_802_11;
+
+	NdisMoveMemory(WEPKEY, pPayload, 3);    //Get WEP IV
+
+	KeyIdx = (*(pPayload + 3) & 0xc0) >> 6;
+	if (pGroupKey[KeyIdx].KeyLen == 0)
+		return (FALSE);
+
+	NdisMoveMemory(WEPKEY + 3, pGroupKey[KeyIdx].Key, pGroupKey[KeyIdx].KeyLen);
+	ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, WEPKEY, pGroupKey[KeyIdx].KeyLen + 3);
+	ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, pPayload, pPayload + 4, payload_len - 4);
+	NdisMoveMemory(&trailfcs, pPayload + payload_len - 8, 4);
+	crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pPayload, payload_len - 8);  //Skip last 4 bytes(FCS).
+	crc32 ^= 0xffffffff;             /* complement */
+
+    if(crc32 != cpu2le32(trailfcs))
+    {
+		DBGPRINT(RT_DEBUG_TRACE, ("! WEP Data CRC Error !\n"));	 //CRC error.
+		return (FALSE);
+	}
+	return (TRUE);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Encryption Algorithm "ARCFOUR" initialize
+
+	Arguments:
+	   Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pKey        Pointer to the WEP KEY
+		KeyLen      Indicate the length fo the WEP KEY
+
+	Return Value:
+	   None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	ARCFOUR_INIT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pKey,
+	IN	UINT			KeyLen)
+{
+	UCHAR	t, u;
+	UINT	keyindex;
+	UINT	stateindex;
+	PUCHAR	state;
+	UINT	counter;
+
+	state = Ctx->STATE;
+	Ctx->X = 0;
+	Ctx->Y = 0;
+	for (counter = 0; counter < 256; counter++)
+		state[counter] = (UCHAR)counter;
+	keyindex = 0;
+	stateindex = 0;
+	for (counter = 0; counter < 256; counter++)
+	{
+		t = state[counter];
+		stateindex = (stateindex + pKey[keyindex] + t) & 0xff;
+		u = state[stateindex];
+		state[stateindex] = t;
+		state[counter] = u;
+		if (++keyindex >= KeyLen)
+			keyindex = 0;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Get bytes from ARCFOUR CONTEXT (S-BOX)
+
+	Arguments:
+	   Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+
+	Return Value:
+	   UCHAR  - the value of the ARCFOUR CONTEXT (S-BOX)
+
+	Note:
+
+	========================================================================
+*/
+UCHAR	ARCFOUR_BYTE(
+	IN	PARCFOURCONTEXT		Ctx)
+{
+  UINT x;
+  UINT y;
+  UCHAR sx, sy;
+  PUCHAR state;
+
+  state = Ctx->STATE;
+  x = (Ctx->X + 1) & 0xff;
+  sx = state[x];
+  y = (sx + Ctx->Y) & 0xff;
+  sy = state[y];
+  Ctx->X = x;
+  Ctx->Y = y;
+  state[y] = sx;
+  state[x] = sy;
+
+  return(state[(sx + sy) & 0xff]);
+
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Decryption Algorithm
+
+	Arguments:
+		Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pDest			Pointer to the Destination
+		pSrc        Pointer to the Source data
+		Len         Indicate the length of the Source data
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ARCFOUR_DECRYPT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pDest,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len)
+{
+	UINT i;
+
+	for (i = 0; i < Len; i++)
+		pDest[i] = pSrc[i] ^ ARCFOUR_BYTE(Ctx);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Encryption Algorithm
+
+	Arguments:
+		Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pDest			Pointer to the Destination
+		pSrc        Pointer to the Source data
+		Len         Indicate the length of the Source dta
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	ARCFOUR_ENCRYPT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pDest,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len)
+{
+	UINT i;
+
+	for (i = 0; i < Len; i++)
+		pDest[i] = pSrc[i] ^ ARCFOUR_BYTE(Ctx);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Encryption Algorithm which conform to the special requirement to encrypt  GTK.
+
+	Arguments:
+		Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pDest			Pointer to the Destination
+		pSrc        Pointer to the Source data
+		Len         Indicate the length of the Source dta
+
+
+	========================================================================
+*/
+
+VOID	WPAARCFOUR_ENCRYPT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pDest,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len)
+{
+	UINT i;
+        //discard first 256 bytes
+	for (i = 0; i < 256; i++)
+            ARCFOUR_BYTE(Ctx);
+
+	for (i = 0; i < Len; i++)
+		pDest[i] = pSrc[i] ^ ARCFOUR_BYTE(Ctx);
+}
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Calculate a new FCS given the current FCS and the new data.
+
+	Arguments:
+		Fcs	      the original FCS value
+		Cp          pointer to the data which will be calculate the FCS
+		Len         the length of the data
+
+	Return Value:
+		UINT - FCS 32 bits
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+UINT	RTMP_CALC_FCS32(
+	IN	UINT	Fcs,
+	IN	PUCHAR	Cp,
+	IN	INT		Len)
+{
+	while (Len--)
+	   Fcs = (((Fcs) >> 8) ^ FCSTAB_32[((Fcs) ^ (*Cp++)) & 0xff]);
+
+	return (Fcs);
+}
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Get last FCS and encrypt it to the destination
+
+	Arguments:
+		pDest			Pointer to the Destination
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPSetICV(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR	pDest)
+{
+	pAd->PrivateInfo.FCSCRC32 ^= 0xffffffff;             /* complement */
+	pAd->PrivateInfo.FCSCRC32 = cpu2le32(pAd->PrivateInfo.FCSCRC32);
+
+	ARCFOUR_ENCRYPT(&pAd->PrivateInfo.WEPCONTEXT, pDest, (PUCHAR) &pAd->PrivateInfo.FCSCRC32, 4);
+}
+
diff --git a/drivers/staging/rt2860/common/spectrum.c b/drivers/staging/rt2860/common/spectrum.c
new file mode 100644
index 000000000000..0265a6d1df1a
--- /dev/null
+++ b/drivers/staging/rt2860/common/spectrum.c
@@ -0,0 +1,1877 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+
+    Module Name:
+	action.c
+
+    Abstract:
+    Handle association related requests either from WSTA or from local MLME
+
+    Revision History:
+    Who          When          What
+    ---------    ----------    ----------------------------------------------
+	Fonchi Wu    2008	  	   created for 802.11h
+ */
+
+#include "../rt_config.h"
+#include "action.h"
+
+VOID MeasureReqTabInit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisAllocateSpinLock(&pAd->CommonCfg.MeasureReqTabLock);
+
+	pAd->CommonCfg.pMeasureReqTab = kmalloc(sizeof(MEASURE_REQ_TAB), GFP_ATOMIC);
+	if (pAd->CommonCfg.pMeasureReqTab)
+		NdisZeroMemory(pAd->CommonCfg.pMeasureReqTab, sizeof(MEASURE_REQ_TAB));
+	else
+		DBGPRINT(RT_DEBUG_ERROR, ("%s Fail to alloc memory for pAd->CommonCfg.pMeasureReqTab.\n", __func__));
+
+	return;
+}
+
+VOID MeasureReqTabExit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisFreeSpinLock(pAd->CommonCfg.MeasureReqTabLock);
+
+	if (pAd->CommonCfg.pMeasureReqTab)
+		kfree(pAd->CommonCfg.pMeasureReqTab);
+	pAd->CommonCfg.pMeasureReqTab = NULL;
+
+	return;
+}
+
+static PMEASURE_REQ_ENTRY MeasureReqLookUp(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	UINT HashIdx;
+	PMEASURE_REQ_TAB pTab = pAd->CommonCfg.pMeasureReqTab;
+	PMEASURE_REQ_ENTRY pEntry = NULL;
+	PMEASURE_REQ_ENTRY pPrevEntry = NULL;
+
+	if (pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab doesn't exist.\n", __func__));
+		return NULL;
+	}
+
+	RTMP_SEM_LOCK(&pAd->CommonCfg.MeasureReqTabLock);
+
+	HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(DialogToken);
+	pEntry = pTab->Hash[HashIdx];
+
+	while (pEntry)
+	{
+		if (pEntry->DialogToken == DialogToken)
+			break;
+		else
+		{
+			pPrevEntry = pEntry;
+			pEntry = pEntry->pNext;
+		}
+	}
+
+	RTMP_SEM_UNLOCK(&pAd->CommonCfg.MeasureReqTabLock);
+
+	return pEntry;
+}
+
+static PMEASURE_REQ_ENTRY MeasureReqInsert(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	INT i;
+	ULONG HashIdx;
+	PMEASURE_REQ_TAB pTab = pAd->CommonCfg.pMeasureReqTab;
+	PMEASURE_REQ_ENTRY pEntry = NULL, pCurrEntry;
+	ULONG Now;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab doesn't exist.\n", __func__));
+		return NULL;
+	}
+
+	pEntry = MeasureReqLookUp(pAd, DialogToken);
+	if (pEntry == NULL)
+	{
+		RTMP_SEM_LOCK(&pAd->CommonCfg.MeasureReqTabLock);
+		for (i = 0; i < MAX_MEASURE_REQ_TAB_SIZE; i++)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry = &pTab->Content[i];
+
+			if ((pEntry->Valid == TRUE)
+				&& RTMP_TIME_AFTER((unsigned long)Now, (unsigned long)(pEntry->lastTime + MQ_REQ_AGE_OUT)))
+			{
+				PMEASURE_REQ_ENTRY pPrevEntry = NULL;
+				ULONG HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+				PMEASURE_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+				// update Hash list
+				do
+				{
+					if (pProbeEntry == pEntry)
+					{
+						if (pPrevEntry == NULL)
+						{
+							pTab->Hash[HashIdx] = pEntry->pNext;
+						}
+						else
+						{
+							pPrevEntry->pNext = pEntry->pNext;
+						}
+						break;
+					}
+
+					pPrevEntry = pProbeEntry;
+					pProbeEntry = pProbeEntry->pNext;
+				} while (pProbeEntry);
+
+				NdisZeroMemory(pEntry, sizeof(MEASURE_REQ_ENTRY));
+				pTab->Size--;
+
+				break;
+			}
+
+			if (pEntry->Valid == FALSE)
+				break;
+		}
+
+		if (i < MAX_MEASURE_REQ_TAB_SIZE)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry->lastTime = Now;
+			pEntry->Valid = TRUE;
+			pEntry->DialogToken = DialogToken;
+			pTab->Size++;
+		}
+		else
+		{
+			pEntry = NULL;
+			DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab tab full.\n", __func__));
+		}
+
+		// add this Neighbor entry into HASH table
+		if (pEntry)
+		{
+			HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(DialogToken);
+			if (pTab->Hash[HashIdx] == NULL)
+			{
+				pTab->Hash[HashIdx] = pEntry;
+			}
+			else
+			{
+				pCurrEntry = pTab->Hash[HashIdx];
+				while (pCurrEntry->pNext != NULL)
+					pCurrEntry = pCurrEntry->pNext;
+				pCurrEntry->pNext = pEntry;
+			}
+		}
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.MeasureReqTabLock);
+	}
+
+	return pEntry;
+}
+
+static VOID MeasureReqDelete(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	PMEASURE_REQ_TAB pTab = pAd->CommonCfg.pMeasureReqTab;
+	PMEASURE_REQ_ENTRY pEntry = NULL;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab doesn't exist.\n", __func__));
+		return;
+	}
+
+	// if empty, return
+	if (pTab->Size == 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("pMeasureReqTab empty.\n"));
+		return;
+	}
+
+	pEntry = MeasureReqLookUp(pAd, DialogToken);
+	if (pEntry != NULL)
+	{
+		PMEASURE_REQ_ENTRY pPrevEntry = NULL;
+		ULONG HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+		PMEASURE_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+		RTMP_SEM_LOCK(&pAd->CommonCfg.MeasureReqTabLock);
+		// update Hash list
+		do
+		{
+			if (pProbeEntry == pEntry)
+			{
+				if (pPrevEntry == NULL)
+				{
+					pTab->Hash[HashIdx] = pEntry->pNext;
+				}
+				else
+				{
+					pPrevEntry->pNext = pEntry->pNext;
+				}
+				break;
+			}
+
+			pPrevEntry = pProbeEntry;
+			pProbeEntry = pProbeEntry->pNext;
+		} while (pProbeEntry);
+
+		NdisZeroMemory(pEntry, sizeof(MEASURE_REQ_ENTRY));
+		pTab->Size--;
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.MeasureReqTabLock);
+	}
+
+	return;
+}
+
+VOID TpcReqTabInit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisAllocateSpinLock(&pAd->CommonCfg.TpcReqTabLock);
+
+	pAd->CommonCfg.pTpcReqTab = kmalloc(sizeof(TPC_REQ_TAB), GFP_ATOMIC);
+	if (pAd->CommonCfg.pTpcReqTab)
+		NdisZeroMemory(pAd->CommonCfg.pTpcReqTab, sizeof(TPC_REQ_TAB));
+	else
+		DBGPRINT(RT_DEBUG_ERROR, ("%s Fail to alloc memory for pAd->CommonCfg.pTpcReqTab.\n", __func__));
+
+	return;
+}
+
+VOID TpcReqTabExit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisFreeSpinLock(pAd->CommonCfg.TpcReqTabLock);
+
+	if (pAd->CommonCfg.pTpcReqTab)
+		kfree(pAd->CommonCfg.pTpcReqTab);
+	pAd->CommonCfg.pTpcReqTab = NULL;
+
+	return;
+}
+
+static PTPC_REQ_ENTRY TpcReqLookUp(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	UINT HashIdx;
+	PTPC_REQ_TAB pTab = pAd->CommonCfg.pTpcReqTab;
+	PTPC_REQ_ENTRY pEntry = NULL;
+	PTPC_REQ_ENTRY pPrevEntry = NULL;
+
+	if (pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab doesn't exist.\n", __func__));
+		return NULL;
+	}
+
+	RTMP_SEM_LOCK(&pAd->CommonCfg.TpcReqTabLock);
+
+	HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(DialogToken);
+	pEntry = pTab->Hash[HashIdx];
+
+	while (pEntry)
+	{
+		if (pEntry->DialogToken == DialogToken)
+			break;
+		else
+		{
+			pPrevEntry = pEntry;
+			pEntry = pEntry->pNext;
+		}
+	}
+
+	RTMP_SEM_UNLOCK(&pAd->CommonCfg.TpcReqTabLock);
+
+	return pEntry;
+}
+
+
+static PTPC_REQ_ENTRY TpcReqInsert(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	INT i;
+	ULONG HashIdx;
+	PTPC_REQ_TAB pTab = pAd->CommonCfg.pTpcReqTab;
+	PTPC_REQ_ENTRY pEntry = NULL, pCurrEntry;
+	ULONG Now;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab doesn't exist.\n", __func__));
+		return NULL;
+	}
+
+	pEntry = TpcReqLookUp(pAd, DialogToken);
+	if (pEntry == NULL)
+	{
+		RTMP_SEM_LOCK(&pAd->CommonCfg.TpcReqTabLock);
+		for (i = 0; i < MAX_TPC_REQ_TAB_SIZE; i++)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry = &pTab->Content[i];
+
+			if ((pEntry->Valid == TRUE)
+				&& RTMP_TIME_AFTER((unsigned long)Now, (unsigned long)(pEntry->lastTime + TPC_REQ_AGE_OUT)))
+			{
+				PTPC_REQ_ENTRY pPrevEntry = NULL;
+				ULONG HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+				PTPC_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+				// update Hash list
+				do
+				{
+					if (pProbeEntry == pEntry)
+					{
+						if (pPrevEntry == NULL)
+						{
+							pTab->Hash[HashIdx] = pEntry->pNext;
+						}
+						else
+						{
+							pPrevEntry->pNext = pEntry->pNext;
+						}
+						break;
+					}
+
+					pPrevEntry = pProbeEntry;
+					pProbeEntry = pProbeEntry->pNext;
+				} while (pProbeEntry);
+
+				NdisZeroMemory(pEntry, sizeof(TPC_REQ_ENTRY));
+				pTab->Size--;
+
+				break;
+			}
+
+			if (pEntry->Valid == FALSE)
+				break;
+		}
+
+		if (i < MAX_TPC_REQ_TAB_SIZE)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry->lastTime = Now;
+			pEntry->Valid = TRUE;
+			pEntry->DialogToken = DialogToken;
+			pTab->Size++;
+		}
+		else
+		{
+			pEntry = NULL;
+			DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab tab full.\n", __func__));
+		}
+
+		// add this Neighbor entry into HASH table
+		if (pEntry)
+		{
+			HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(DialogToken);
+			if (pTab->Hash[HashIdx] == NULL)
+			{
+				pTab->Hash[HashIdx] = pEntry;
+			}
+			else
+			{
+				pCurrEntry = pTab->Hash[HashIdx];
+				while (pCurrEntry->pNext != NULL)
+					pCurrEntry = pCurrEntry->pNext;
+				pCurrEntry->pNext = pEntry;
+			}
+		}
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.TpcReqTabLock);
+	}
+
+	return pEntry;
+}
+
+static VOID TpcReqDelete(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	PTPC_REQ_TAB pTab = pAd->CommonCfg.pTpcReqTab;
+	PTPC_REQ_ENTRY pEntry = NULL;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab doesn't exist.\n", __func__));
+		return;
+	}
+
+	// if empty, return
+	if (pTab->Size == 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("pTpcReqTab empty.\n"));
+		return;
+	}
+
+	pEntry = TpcReqLookUp(pAd, DialogToken);
+	if (pEntry != NULL)
+	{
+		PTPC_REQ_ENTRY pPrevEntry = NULL;
+		ULONG HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+		PTPC_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+		RTMP_SEM_LOCK(&pAd->CommonCfg.TpcReqTabLock);
+		// update Hash list
+		do
+		{
+			if (pProbeEntry == pEntry)
+			{
+				if (pPrevEntry == NULL)
+				{
+					pTab->Hash[HashIdx] = pEntry->pNext;
+				}
+				else
+				{
+					pPrevEntry->pNext = pEntry->pNext;
+				}
+				break;
+			}
+
+			pPrevEntry = pProbeEntry;
+			pProbeEntry = pProbeEntry->pNext;
+		} while (pProbeEntry);
+
+		NdisZeroMemory(pEntry, sizeof(TPC_REQ_ENTRY));
+		pTab->Size--;
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.TpcReqTabLock);
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Get Current TimeS tamp.
+
+	Parametrs:
+
+	Return	: Current Time Stamp.
+	==========================================================================
+ */
+static UINT64 GetCurrentTimeStamp(
+	IN PRTMP_ADAPTER pAd)
+{
+	// get current time stamp.
+	return 0;
+}
+
+/*
+	==========================================================================
+	Description:
+		Get Current Transmit Power.
+
+	Parametrs:
+
+	Return	: Current Time Stamp.
+	==========================================================================
+ */
+static UINT8 GetCurTxPwr(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT8 Wcid)
+{
+	return 16; /* 16 dBm */
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Dialog Token into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Dialog token.
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertDialogToken(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 DialogToken)
+{
+	ULONG TempLen;
+	MakeOutgoingFrame(pFrameBuf,	&TempLen,
+					1,				&DialogToken,
+					END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert TPC Request IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+
+	Return	: None.
+	==========================================================================
+ */
+ static VOID InsertTpcReqIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen)
+{
+	ULONG TempLen;
+	ULONG Len = 0;
+	UINT8 ElementID = IE_TPC_REQUEST;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert TPC Report IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Transmit Power.
+		4. Link Margin.
+
+	Return	: None.
+	==========================================================================
+ */
+ static VOID InsertTpcReportIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 TxPwr,
+	IN UINT8 LinkMargin)
+{
+	ULONG TempLen;
+	ULONG Len = sizeof(TPC_REPORT_INFO);
+	UINT8 ElementID = IE_TPC_REPORT;
+	TPC_REPORT_INFO TpcReportIE;
+
+	TpcReportIE.TxPwr = TxPwr;
+	TpcReportIE.LinkMargin = LinkMargin;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						Len,						&TpcReportIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Channel Switch Announcement IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. channel switch announcement mode.
+		4. new selected channel.
+		5. channel switch announcement count.
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertChSwAnnIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 ChSwMode,
+	IN UINT8 NewChannel,
+	IN UINT8 ChSwCnt)
+{
+	ULONG TempLen;
+	ULONG Len = sizeof(CH_SW_ANN_INFO);
+	UINT8 ElementID = IE_CHANNEL_SWITCH_ANNOUNCEMENT;
+	CH_SW_ANN_INFO ChSwAnnIE;
+
+	ChSwAnnIE.ChSwMode = ChSwMode;
+	ChSwAnnIE.Channel = NewChannel;
+	ChSwAnnIE.ChSwCnt = ChSwCnt;
+
+	MakeOutgoingFrame(pFrameBuf,				&TempLen,
+						1,						&ElementID,
+						1,						&Len,
+						Len,					&ChSwAnnIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Measure Request IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Measure Token.
+		4. Measure Request Mode.
+		5. Measure Request Type.
+		6. Measure Channel.
+		7. Measure Start time.
+		8. Measure Duration.
+
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertMeasureReqIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN PMEASURE_REQ_INFO pMeasureReqIE)
+{
+	ULONG TempLen;
+	UINT8 Len = sizeof(MEASURE_REQ_INFO);
+	UINT8 ElementID = IE_MEASUREMENT_REQUEST;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						Len,						pMeasureReqIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Measure Report IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Measure Token.
+		4. Measure Request Mode.
+		5. Measure Request Type.
+		6. Length of Report Infomation
+		7. Pointer of Report Infomation Buffer.
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertMeasureReportIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN PMEASURE_REPORT_INFO pMeasureReportIE,
+	IN UINT8 ReportLnfoLen,
+	IN PUINT8 pReportInfo)
+{
+	ULONG TempLen;
+	ULONG Len;
+	UINT8 ElementID = IE_MEASUREMENT_REPORT;
+
+	Len = sizeof(MEASURE_REPORT_INFO) + ReportLnfoLen;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						Len,						pMeasureReportIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	if ((ReportLnfoLen > 0) && (pReportInfo != NULL))
+	{
+		MakeOutgoingFrame(pFrameBuf + *pFrameLen,		&TempLen,
+							ReportLnfoLen,				pReportInfo,
+							END_OF_ARGS);
+
+		*pFrameLen = *pFrameLen + TempLen;
+	}
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 MeasureCh,
+	IN UINT16 MeasureDuration)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+	HEADER_802_11 ActHdr;
+	MEASURE_REQ_INFO MeasureReqIE;
+	UINT8 RmReqDailogToken = RandomByte(pAd);
+	UINT64 MeasureStartTime = GetCurrentTimeStamp(pAd);
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __func__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_MRQ);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, MeasureToken);
+
+	// prepare Measurement IE.
+	NdisZeroMemory(&MeasureReqIE, sizeof(MEASURE_REQ_INFO));
+	MeasureReqIE.Token = RmReqDailogToken;
+	MeasureReqIE.ReqMode.word = MeasureReqMode;
+	MeasureReqIE.ReqType = MeasureReqType;
+	MeasureReqIE.MeasureReq.ChNum = MeasureCh;
+	MeasureReqIE.MeasureReq.MeasureStartTime = cpu2le64(MeasureStartTime);
+	MeasureReqIE.MeasureReq.MeasureDuration = cpu2le16(MeasureDuration);
+	InsertMeasureReqIE(pAd, (pOutBuffer + FrameLen), &FrameLen, &MeasureReqIE);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 ReportInfoLen,
+	IN PUINT8 pReportInfo)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+	HEADER_802_11 ActHdr;
+	MEASURE_REPORT_INFO MeasureRepIE;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __func__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_MRP);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, DialogToken);
+
+	// prepare Measurement IE.
+	NdisZeroMemory(&MeasureRepIE, sizeof(MEASURE_REPORT_INFO));
+	MeasureRepIE.Token = MeasureToken;
+	MeasureRepIE.ReportMode.word = MeasureReqMode;
+	MeasureRepIE.ReportType = MeasureReqType;
+	InsertMeasureReportIE(pAd, (pOutBuffer + FrameLen), &FrameLen, &MeasureRepIE, ReportInfoLen, pReportInfo);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UCHAR DialogToken)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+
+	HEADER_802_11 ActHdr;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __func__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_TPCRQ);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, DialogToken);
+
+	// Insert TPC Request IE.
+	InsertTpcReqIE(pAd, (pOutBuffer + FrameLen), &FrameLen);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 TxPwr,
+	IN UINT8 LinkMargin)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+
+	HEADER_802_11 ActHdr;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __func__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_TPCRP);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, DialogToken);
+
+	// Insert TPC Request IE.
+	InsertTpcReportIE(pAd, (pOutBuffer + FrameLen), &FrameLen, TxPwr, LinkMargin);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare Channel Switch Announcement action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+		2. Channel switch announcement mode.
+		2. a New selected channel.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueChSwAnn(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 ChSwMode,
+	IN UINT8 NewCh)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+
+	HEADER_802_11 ActHdr;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __func__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_CHANNEL_SWITCH);
+
+	InsertChSwAnnIE(pAd, (pOutBuffer + FrameLen), &FrameLen, ChSwMode, NewCh, 0);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+static BOOLEAN DfsRequirementCheck(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT8 Channel)
+{
+	BOOLEAN Result = FALSE;
+	INT i;
+
+	do
+	{
+		// check DFS procedure is running.
+		// make sure DFS procedure won't start twice.
+		if (pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE)
+		{
+			Result = FALSE;
+			break;
+		}
+
+		// check the new channel carried from Channel Switch Announcemnet is valid.
+		for (i=0; i<pAd->ChannelListNum; i++)
+		{
+			if ((Channel == pAd->ChannelList[i].Channel)
+				&&(pAd->ChannelList[i].RemainingTimeForUse == 0))
+			{
+				// found radar signal in the channel. the channel can't use at least for 30 minutes.
+				pAd->ChannelList[i].RemainingTimeForUse = 1800;//30 min = 1800 sec
+				Result = TRUE;
+				break;
+			}
+		}
+	} while(FALSE);
+
+	return Result;
+}
+
+VOID NotifyChSwAnnToPeerAPs(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pRA,
+	IN PUCHAR pTA,
+	IN UINT8 ChSwMode,
+	IN UINT8 Channel)
+{
+#ifdef WDS_SUPPORT
+	if (!((pRA[0] & 0xff) == 0xff)) // is pRA a broadcase address.
+	{
+		INT i;
+		// info neighbor APs that Radar signal found throgh WDS link.
+		for (i = 0; i < MAX_WDS_ENTRY; i++)
+		{
+			if (ValidWdsEntry(pAd, i))
+			{
+				PUCHAR pDA = pAd->WdsTab.WdsEntry[i].PeerWdsAddr;
+
+				// DA equal to SA. have no necessary orignal AP which found Radar signal.
+				if (MAC_ADDR_EQUAL(pTA, pDA))
+					continue;
+
+				// send Channel Switch Action frame to info Neighbro APs.
+				EnqueueChSwAnn(pAd, pDA, ChSwMode, Channel);
+			}
+		}
+	}
+#endif // WDS_SUPPORT //
+}
+
+static VOID StartDFSProcedure(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Channel,
+	IN UINT8 ChSwMode)
+{
+	// start DFS procedure
+	pAd->CommonCfg.Channel = Channel;
+#ifdef DOT11_N_SUPPORT
+	N_ChannelCheck(pAd);
+#endif // DOT11_N_SUPPORT //
+	pAd->CommonCfg.RadarDetect.RDMode = RD_SWITCHING_MODE;
+	pAd->CommonCfg.RadarDetect.CSCount = 0;
+}
+
+/*
+	==========================================================================
+	Description:
+		Channel Switch Announcement action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Channel switch announcement infomation buffer.
+
+
+	Return	: None.
+	==========================================================================
+ */
+
+/*
+  Channel Switch Announcement IE.
+  +----+-----+-----------+------------+-----------+
+  | ID | Len |Ch Sw Mode | New Ch Num | Ch Sw Cnt |
+  +----+-----+-----------+------------+-----------+
+    1    1        1           1            1
+*/
+static BOOLEAN PeerChSwAnnSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PCH_SW_ANN_INFO pChSwAnnInfo)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+
+	// skip 802.11 header.
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pChSwAnnInfo == NULL)
+		return result;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_CHANNEL_SWITCH_ANNOUNCEMENT:
+				NdisMoveMemory(&pChSwAnnInfo->ChSwMode, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pChSwAnnInfo->Channel, eid_ptr->Octet + 1, 1);
+				NdisMoveMemory(&pChSwAnnInfo->ChSwCnt, eid_ptr->Octet + 2, 1);
+
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		Measurement request action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Measurement request infomation buffer.
+
+	Return	: None.
+	==========================================================================
+ */
+static BOOLEAN PeerMeasureReqSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken,
+	OUT PMEASURE_REQ_INFO pMeasureReqInfo)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+	PUCHAR ptr;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+
+	// skip 802.11 header.
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pMeasureReqInfo == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_MEASUREMENT_REQUEST:
+				NdisMoveMemory(&pMeasureReqInfo->Token, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pMeasureReqInfo->ReqMode.word, eid_ptr->Octet + 1, 1);
+				NdisMoveMemory(&pMeasureReqInfo->ReqType, eid_ptr->Octet + 2, 1);
+				ptr = eid_ptr->Octet + 3;
+				NdisMoveMemory(&pMeasureReqInfo->MeasureReq.ChNum, ptr, 1);
+				NdisMoveMemory(&MeasureStartTime, ptr + 1, 8);
+				pMeasureReqInfo->MeasureReq.MeasureStartTime = SWAP64(MeasureStartTime);
+				NdisMoveMemory(&MeasureDuration, ptr + 9, 2);
+				pMeasureReqInfo->MeasureReq.MeasureDuration = SWAP16(MeasureDuration);
+
+				result = TRUE;
+				break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		Measurement report action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Measurement report infomation buffer.
+		4. basic report infomation buffer.
+
+	Return	: None.
+	==========================================================================
+ */
+
+/*
+  Measurement Report IE.
+  +----+-----+-------+-------------+--------------+----------------+
+  | ID | Len | Token | Report Mode | Measure Type | Measure Report |
+  +----+-----+-------+-------------+--------------+----------------+
+    1     1      1          1             1            variable
+
+  Basic Report.
+  +--------+------------+----------+-----+
+  | Ch Num | Start Time | Duration | Map |
+  +--------+------------+----------+-----+
+      1          8           2        1
+
+  Map Field Bit Format.
+  +-----+---------------+---------------------+-------+------------+----------+
+  | Bss | OFDM Preamble | Unidentified signal | Radar | Unmeasured | Reserved |
+  +-----+---------------+---------------------+-------+------------+----------+
+     0          1                  2              3         4          5-7
+*/
+static BOOLEAN PeerMeasureReportSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken,
+	OUT PMEASURE_REPORT_INFO pMeasureReportInfo,
+	OUT PUINT8 pReportBuf)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+	PUCHAR ptr;
+
+	// skip 802.11 header.
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pMeasureReportInfo == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_MEASUREMENT_REPORT:
+				NdisMoveMemory(&pMeasureReportInfo->Token, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pMeasureReportInfo->ReportMode, eid_ptr->Octet + 1, 1);
+				NdisMoveMemory(&pMeasureReportInfo->ReportType, eid_ptr->Octet + 2, 1);
+				if (pMeasureReportInfo->ReportType == RM_BASIC)
+				{
+					PMEASURE_BASIC_REPORT pReport = (PMEASURE_BASIC_REPORT)pReportBuf;
+					ptr = eid_ptr->Octet + 3;
+					NdisMoveMemory(&pReport->ChNum, ptr, 1);
+					NdisMoveMemory(&pReport->MeasureStartTime, ptr + 1, 8);
+					NdisMoveMemory(&pReport->MeasureDuration, ptr + 9, 2);
+					NdisMoveMemory(&pReport->Map, ptr + 11, 1);
+
+				}
+				else if (pMeasureReportInfo->ReportType == RM_CCA)
+				{
+					PMEASURE_CCA_REPORT pReport = (PMEASURE_CCA_REPORT)pReportBuf;
+					ptr = eid_ptr->Octet + 3;
+					NdisMoveMemory(&pReport->ChNum, ptr, 1);
+					NdisMoveMemory(&pReport->MeasureStartTime, ptr + 1, 8);
+					NdisMoveMemory(&pReport->MeasureDuration, ptr + 9, 2);
+					NdisMoveMemory(&pReport->CCA_Busy_Fraction, ptr + 11, 1);
+
+				}
+				else if (pMeasureReportInfo->ReportType == RM_RPI_HISTOGRAM)
+				{
+					PMEASURE_RPI_REPORT pReport = (PMEASURE_RPI_REPORT)pReportBuf;
+					ptr = eid_ptr->Octet + 3;
+					NdisMoveMemory(&pReport->ChNum, ptr, 1);
+					NdisMoveMemory(&pReport->MeasureStartTime, ptr + 1, 8);
+					NdisMoveMemory(&pReport->MeasureDuration, ptr + 9, 2);
+					NdisMoveMemory(&pReport->RPI_Density, ptr + 11, 8);
+				}
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Request action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Dialog Token.
+
+	Return	: None.
+	==========================================================================
+ */
+static BOOLEAN PeerTpcReqSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pDialogToken == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_TPC_REQUEST:
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Report action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Dialog Token.
+		4. TPC Report IE.
+
+	Return	: None.
+	==========================================================================
+ */
+static BOOLEAN PeerTpcRepSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken,
+	OUT PTPC_REPORT_INFO pTpcRepInfo)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pDialogToken == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_TPC_REPORT:
+				NdisMoveMemory(&pTpcRepInfo->TxPwr, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pTpcRepInfo->LinkMargin, eid_ptr->Octet + 1, 1);
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		Channel Switch Announcement action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerChSwAnnAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	CH_SW_ANN_INFO ChSwAnnInfo;
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR index = 0, Channel = 0, NewChannel = 0;
+	ULONG Bssidx = 0;
+#endif // CONFIG_STA_SUPPORT //
+
+	NdisZeroMemory(&ChSwAnnInfo, sizeof(CH_SW_ANN_INFO));
+	if (! PeerChSwAnnSanity(pAd, Elem->Msg, Elem->MsgLen, &ChSwAnnInfo))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("Invalid Channel Switch Action Frame.\n"));
+		return;
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->OpMode == OPMODE_STA)
+	{
+		Bssidx = BssTableSearch(&pAd->ScanTab, pFr->Hdr.Addr3, pAd->CommonCfg.Channel);
+		if (Bssidx == BSS_NOT_FOUND)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("PeerChSwAnnAction - Bssidx is not found\n"));
+			return;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("\n****Bssidx is %d, Channel = %d\n", index, pAd->ScanTab.BssEntry[Bssidx].Channel));
+		hex_dump("SSID",pAd->ScanTab.BssEntry[Bssidx].Bssid ,6);
+
+		Channel = pAd->CommonCfg.Channel;
+		NewChannel = ChSwAnnInfo.Channel;
+
+		if ((pAd->CommonCfg.bIEEE80211H == 1) && (NewChannel != 0) && (Channel != NewChannel))
+		{
+			// Switching to channel 1 can prevent from rescanning the current channel immediately (by auto reconnection).
+			// In addition, clear the MLME queue and the scan table to discard the RX packets and previous scanning results.
+			AsicSwitchChannel(pAd, 1, FALSE);
+			AsicLockChannel(pAd, 1);
+		    LinkDown(pAd, FALSE);
+			MlmeQueueInit(&pAd->Mlme.Queue);
+			BssTableInit(&pAd->ScanTab);
+		    RTMPusecDelay(1000000);		// use delay to prevent STA do reassoc
+
+			// channel sanity check
+			for (index = 0 ; index < pAd->ChannelListNum; index++)
+			{
+				if (pAd->ChannelList[index].Channel == NewChannel)
+				{
+					pAd->ScanTab.BssEntry[Bssidx].Channel = NewChannel;
+					pAd->CommonCfg.Channel = NewChannel;
+					AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+					DBGPRINT(RT_DEBUG_TRACE, ("&&&&&&&&&&&&&&&&PeerChSwAnnAction - STA receive channel switch announcement IE (New Channel =%d)\n", NewChannel));
+					break;
+				}
+			}
+
+			if (index >= pAd->ChannelListNum)
+			{
+				DBGPRINT_ERR(("&&&&&&&&&&&&&&&&&&&&&&&&&&PeerChSwAnnAction(can not find New Channel=%d in ChannelList[%d]\n", pAd->CommonCfg.Channel, pAd->ChannelListNum));
+			}
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	return;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Measurement Request action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerMeasureReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+	UINT8 DialogToken;
+	MEASURE_REQ_INFO MeasureReqInfo;
+	MEASURE_REPORT_MODE ReportMode;
+
+	if(PeerMeasureReqSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken, &MeasureReqInfo))
+	{
+		ReportMode.word = 0;
+		ReportMode.field.Incapable = 1;
+		EnqueueMeasurementRep(pAd, pFr->Hdr.Addr2, DialogToken, MeasureReqInfo.Token, ReportMode.word, MeasureReqInfo.ReqType, 0, NULL);
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Measurement Report action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerMeasureReportAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MEASURE_REPORT_INFO MeasureReportInfo;
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+	UINT8 DialogToken;
+	PUINT8 pMeasureReportInfo;
+
+//	if (pAd->CommonCfg.bIEEE80211H != TRUE)
+//		return;
+
+	if ((pMeasureReportInfo = kmalloc(sizeof(MEASURE_RPI_REPORT), GFP_ATOMIC)) == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s unable to alloc memory for measure report buffer (size=%d).\n", __func__, sizeof(MEASURE_RPI_REPORT)));
+		return;
+	}
+
+	NdisZeroMemory(&MeasureReportInfo, sizeof(MEASURE_REPORT_INFO));
+	NdisZeroMemory(pMeasureReportInfo, sizeof(MEASURE_RPI_REPORT));
+	if (PeerMeasureReportSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken, &MeasureReportInfo, pMeasureReportInfo))
+	{
+		do {
+			PMEASURE_REQ_ENTRY pEntry = NULL;
+
+			// Not a autonomous measure report.
+			// check the dialog token field. drop it if the dialog token doesn't match.
+			if ((DialogToken != 0)
+				&& ((pEntry = MeasureReqLookUp(pAd, DialogToken)) == NULL))
+				break;
+
+			if (pEntry != NULL)
+				MeasureReqDelete(pAd, pEntry->DialogToken);
+
+			if (MeasureReportInfo.ReportType == RM_BASIC)
+			{
+				PMEASURE_BASIC_REPORT pBasicReport = (PMEASURE_BASIC_REPORT)pMeasureReportInfo;
+				if ((pBasicReport->Map.field.Radar)
+					&& (DfsRequirementCheck(pAd, pBasicReport->ChNum) == TRUE))
+				{
+					NotifyChSwAnnToPeerAPs(pAd, pFr->Hdr.Addr1, pFr->Hdr.Addr2, 1, pBasicReport->ChNum);
+					StartDFSProcedure(pAd, pBasicReport->ChNum, 1);
+				}
+			}
+		} while (FALSE);
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, ("Invalid Measurement Report Frame.\n"));
+
+	kfree(pMeasureReportInfo);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Request action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerTpcReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+	PUCHAR pFramePtr = pFr->Octet;
+	UINT8 DialogToken;
+	UINT8 TxPwr = GetCurTxPwr(pAd, Elem->Wcid);
+	UINT8 LinkMargin = 0;
+	CHAR RealRssi;
+
+	// link margin: Ratio of the received signal power to the minimum desired by the station (STA). The
+	//				STA may incorporate rate information and channel conditions, including interference, into its computation
+	//				of link margin.
+
+	RealRssi = RTMPMaxRssi(pAd, ConvertToRssi(pAd, Elem->Rssi0, RSSI_0),
+								ConvertToRssi(pAd, Elem->Rssi1, RSSI_1),
+								ConvertToRssi(pAd, Elem->Rssi2, RSSI_2));
+
+	// skip Category and action code.
+	pFramePtr += 2;
+
+	// Dialog token.
+	NdisMoveMemory(&DialogToken, pFramePtr, 1);
+
+	LinkMargin = (RealRssi / MIN_RCV_PWR);
+	if (PeerTpcReqSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken))
+		EnqueueTPCRep(pAd, pFr->Hdr.Addr2, DialogToken, TxPwr, LinkMargin);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Report action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerTpcRepAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UINT8 DialogToken;
+	TPC_REPORT_INFO TpcRepInfo;
+	PTPC_REQ_ENTRY pEntry = NULL;
+
+	NdisZeroMemory(&TpcRepInfo, sizeof(TPC_REPORT_INFO));
+	if (PeerTpcRepSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken, &TpcRepInfo))
+	{
+		if ((pEntry = TpcReqLookUp(pAd, DialogToken)) != NULL)
+		{
+			TpcReqDelete(pAd, pEntry->DialogToken);
+			DBGPRINT(RT_DEBUG_TRACE, ("%s: DialogToken=%x, TxPwr=%d, LinkMargin=%d\n",
+				__func__, DialogToken, TpcRepInfo.TxPwr, TpcRepInfo.LinkMargin));
+		}
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Spectrun action frames Handler such as channel switch annoucement,
+		measurement report, measurement request actions frames.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+VOID PeerSpectrumAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	if (pAd->CommonCfg.bIEEE80211H != TRUE)
+		return;
+
+	switch(Action)
+	{
+		case SPEC_MRQ:
+			// current rt2860 unable do such measure specified in Measurement Request.
+			// reject all measurement request.
+			PeerMeasureReqAction(pAd, Elem);
+			break;
+
+		case SPEC_MRP:
+			PeerMeasureReportAction(pAd, Elem);
+			break;
+
+		case SPEC_TPCRQ:
+			PeerTpcReqAction(pAd, Elem);
+			break;
+
+		case SPEC_TPCRP:
+			PeerTpcRepAction(pAd, Elem);
+			break;
+
+		case SPEC_CHANNEL_SWITCH:
+{
+#ifdef DOT11N_DRAFT3
+				SEC_CHA_OFFSET_IE	Secondary;
+				CHA_SWITCH_ANNOUNCE_IE	ChannelSwitch;
+
+				// 802.11h only has Channel Switch Announcement IE.
+				RTMPMoveMemory(&ChannelSwitch, &Elem->Msg[LENGTH_802_11+4], sizeof (CHA_SWITCH_ANNOUNCE_IE));
+
+				// 802.11n D3.03 adds secondary channel offset element in the end.
+				if (Elem->MsgLen ==  (LENGTH_802_11 + 2 + sizeof (CHA_SWITCH_ANNOUNCE_IE) + sizeof (SEC_CHA_OFFSET_IE)))
+				{
+					RTMPMoveMemory(&Secondary, &Elem->Msg[LENGTH_802_11+9], sizeof (SEC_CHA_OFFSET_IE));
+				}
+				else
+				{
+					Secondary.SecondaryChannelOffset = 0;
+				}
+
+				if ((Elem->Msg[LENGTH_802_11+2] == IE_CHANNEL_SWITCH_ANNOUNCEMENT) && (Elem->Msg[LENGTH_802_11+3] == 3))
+				{
+					ChannelSwitchAction(pAd, Elem->Wcid, ChannelSwitch.NewChannel, Secondary.SecondaryChannelOffset);
+				}
+#endif // DOT11N_DRAFT3 //
+}
+			PeerChSwAnnAction(pAd, Elem);
+			break;
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	Parametrs:
+
+	Return	: None.
+	==========================================================================
+ */
+INT Set_MeasureReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT Aid = 1;
+	UINT ArgIdx;
+	PUCHAR thisChar;
+
+	MEASURE_REQ_MODE MeasureReqMode;
+	UINT8 MeasureReqToken = RandomByte(pAd);
+	UINT8 MeasureReqType = RM_BASIC;
+	UINT8 MeasureCh = 1;
+
+	ArgIdx = 1;
+	while ((thisChar = strsep((char **)&arg, "-")) != NULL)
+	{
+		switch(ArgIdx)
+		{
+			case 1:	// Aid.
+				Aid = simple_strtol(thisChar, 0, 16);
+				break;
+
+			case 2: // Measurement Request Type.
+				MeasureReqType = simple_strtol(thisChar, 0, 16);
+				if (MeasureReqType > 3)
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("%s: unknow MeasureReqType(%d)\n", __func__, MeasureReqType));
+					return TRUE;
+				}
+				break;
+
+			case 3: // Measurement channel.
+				MeasureCh = simple_strtol(thisChar, 0, 16);
+				break;
+		}
+		ArgIdx++;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s::Aid = %d, MeasureReqType=%d MeasureCh=%d\n", __func__, Aid, MeasureReqType, MeasureCh));
+	if (!VALID_WCID(Aid))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: unknow sta of Aid(%d)\n", __func__, Aid));
+		return TRUE;
+	}
+
+	MeasureReqMode.word = 0;
+	MeasureReqMode.field.Enable = 1;
+
+	MeasureReqInsert(pAd, MeasureReqToken);
+
+	EnqueueMeasurementReq(pAd, pAd->MacTab.Content[Aid].Addr,
+		MeasureReqToken, MeasureReqMode.word, MeasureReqType, MeasureCh, 2000);
+
+	return TRUE;
+}
+
+INT Set_TpcReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT Aid;
+
+	UINT8 TpcReqToken = RandomByte(pAd);
+
+	Aid = simple_strtol(arg, 0, 16);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s::Aid = %d\n", __func__, Aid));
+	if (!VALID_WCID(Aid))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: unknow sta of Aid(%d)\n", __func__, Aid));
+		return TRUE;
+	}
+
+	TpcReqInsert(pAd, TpcReqToken);
+
+	EnqueueTPCReq(pAd, pAd->MacTab.Content[Aid].Addr, TpcReqToken);
+
+	return TRUE;
+}
+
diff --git a/drivers/staging/rt2860/config.mk b/drivers/staging/rt2860/config.mk
new file mode 100644
index 000000000000..f57d7bbffb24
--- /dev/null
+++ b/drivers/staging/rt2860/config.mk
@@ -0,0 +1,245 @@
+# Support ATE function
+HAS_ATE=n
+
+# Support 28xx QA ATE function
+HAS_28xx_QA=n
+
+# Support Wpa_Supplicant
+HAS_WPA_SUPPLICANT=n
+
+# Support Native WpaSupplicant for Network Maganger
+HAS_NATIVE_WPA_SUPPLICANT_SUPPORT=n
+
+#Support Net interface block while Tx-Sw queue full
+HAS_BLOCK_NET_IF=n
+
+#Support DFS function
+HAS_DFS_SUPPORT=n
+
+#Support Carrier-Sense function
+HAS_CS_SUPPORT=n
+
+#ifdef MULTI_CARD
+# Support for Multiple Cards
+HAS_MC_SUPPORT=n
+#endif // MULTI_CARD //
+
+#Support for IEEE802.11e DLS
+HAS_QOS_DLS_SUPPORT=n
+
+#Support for EXT_CHANNEL
+HAS_EXT_BUILD_CHANNEL_LIST=n
+
+#Support for Net-SNMP
+HAS_SNMP_SUPPORT=n
+
+#Support features of Single SKU.
+HAS_SINGLE_SKU_SUPPORT=n
+
+#Support features of 802.11n
+HAS_DOT11_N_SUPPORT=y
+
+
+#################################################
+
+CC := $(CROSS_COMPILE)gcc
+LD := $(CROSS_COMPILE)ld
+
+WFLAGS := -DAGGREGATION_SUPPORT -DPIGGYBACK_SUPPORT -DWMM_SUPPORT  -DLINUX -Wall -Wstrict-prototypes -Wno-trigraphs
+
+
+#################################################
+
+#ifdef CONFIG_STA_SUPPORT
+# config for STA mode
+
+ifeq ($(RT28xx_MODE),STA)
+WFLAGS += -DCONFIG_STA_SUPPORT -DDBG
+
+ifeq ($(HAS_WPA_SUPPLICANT),y)
+WFLAGS += -DWPA_SUPPLICANT_SUPPORT
+endif
+
+ifeq ($(HAS_NATIVE_WPA_SUPPLICANT_SUPPORT),y)
+WFLAGS += -DNATIVE_WPA_SUPPLICANT_SUPPORT
+endif
+
+ifeq ($(HAS_ATE),y)
+WFLAGS += -DRALINK_ATE
+ifeq ($(HAS_28xx_QA),y)
+WFLAGS += -DRALINK_28xx_QA
+endif
+endif
+
+ifeq ($(HAS_SNMP_SUPPORT),y)
+WFLAGS += -DSNMP_SUPPORT
+endif
+
+ifeq ($(HAS_QOS_DLS_SUPPORT),y)
+WFLAGS += -DQOS_DLS_SUPPORT
+endif
+
+ifeq ($(HAS_DOT11_N_SUPPORT),y)
+WFLAGS += -DDOT11_N_SUPPORT
+endif
+
+ifeq ($(HAS_CS_SUPPORT),y)
+WFLAGS += -DCARRIER_DETECTION_SUPPORT
+endif
+
+ifeq ($(HAS_SINGLE_SKU_SUPPORT),y)
+WFLAGS += -DSINGLE_SKU
+endif
+
+endif
+# endif of ifeq ($(RT28xx_MODE),STA)
+#endif // CONFIG_STA_SUPPORT //
+
+#################################################
+
+#################################################
+
+#
+# Common compiler flag
+#
+
+
+ifeq ($(HAS_EXT_BUILD_CHANNEL_LIST),y)
+WFLAGS += -DEXT_BUILD_CHANNEL_LIST
+endif
+
+ifeq ($(CHIPSET),2860)
+WFLAGS +=-DRT2860
+endif
+
+ifeq ($(CHIPSET),2870)
+WFLAGS +=-DRT2870
+endif
+
+ifeq ($(PLATFORM),5VT)
+#WFLAGS += -DCONFIG_5VT_ENHANCE
+endif
+
+ifeq ($(HAS_BLOCK_NET_IF),y)
+WFLAGS += -DBLOCK_NET_IF
+endif
+
+ifeq ($(HAS_DFS_SUPPORT),y)
+WFLAGS += -DDFS_SUPPORT
+endif
+
+#ifdef MULTI_CARD
+ifeq ($(HAS_MC_SUPPORT),y)
+WFLAGS += -DMULTIPLE_CARD_SUPPORT
+endif
+#endif // MULTI_CARD //
+
+ifeq ($(HAS_LLTD),y)
+WFLAGS += -DLLTD_SUPPORT
+endif
+
+ifeq ($(PLATFORM),IXP)
+WFLAGS += -DRT_BIG_ENDIAN
+endif
+
+ifeq ($(PLATFORM),IKANOS_V160)
+WFLAGS += -DRT_BIG_ENDIAN -DIKANOS_VX_1X0
+endif
+
+ifeq ($(PLATFORM),IKANOS_V180)
+WFLAGS += -DRT_BIG_ENDIAN -DIKANOS_VX_1X0
+endif
+
+ifeq ($(PLATFORM),INF_TWINPASS)
+WFLAGS += -DRT_BIG_ENDIAN -DINF_TWINPASS
+endif
+
+ifeq ($(PLATFORM),INF_DANUBE)
+WFLAGS += -DINF_DANUBE -DRT_BIG_ENDIAN
+endif
+
+ifeq ($(PLATFORM),CAVM_OCTEON)
+WFLAGS += -DRT_BIG_ENDIAN
+endif
+
+ifeq ($(PLATFORM),BRCM_6358)
+WFLAGS += -DRT_BIG_ENDIAN
+endif
+
+ifeq ($(PLATFORM),INF_AMAZON_SE)
+#WFLAGS += -DRT_BIG_ENDIAN -DINF_AMAZON_SE -DBG_FT_SUPPORT
+WFLAGS += -DRT_BIG_ENDIAN -DINF_AMAZON_SE
+endif
+
+#kernel build options for 2.4
+# move to Makefile outside LINUX_SRC := /opt/star/kernel/linux-2.4.27-star
+
+ifeq ($(PLATFORM),STAR)
+CFLAGS := -D__KERNEL__ -I$(LINUX_SRC)/include -I$(RT28xx_DIR)/include -Wall -Wstrict-prototypes -Wno-trigraphs -O2 -fno-strict-aliasing -fno-common -Uarm -fno-common -pipe -mapcs-32 -D__LINUX_ARM_ARCH__=4 -march=armv4  -mshort-load-bytes -msoft-float -Uarm -DMODULE -DMODVERSIONS -include $(LINUX_SRC)/include/linux/modversions.h $(WFLAGS)
+
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),SIGMA)
+CFLAGS := -D__KERNEL__ -I$(RT28xx_DIR)/include -I$(LINUX_SRC)/include -I$(LINUX_SRC)/include/asm/gcc -I$(LINUX_SRC)/include/asm-mips/mach-tango2 -I$(LINUX_SRC)/include/asm-mips/mach-tango2 -DEM86XX_CHIP=EM86XX_CHIPID_TANGO2 -DEM86XX_REVISION=6 -I$(LINUX_SRC)/include/asm-mips/mach-generic -I$(RT2860_DIR)/include -Wall -Wundef -Wstrict-prototypes -Wno-trigraphs -fno-strict-aliasing -fno-common -ffreestanding -O2     -fomit-frame-pointer -G 0 -mno-abicalls -fno-pic -pipe  -mabi=32 -march=mips32r2 -Wa,-32 -Wa,-march=mips32r2 -Wa,-mips32r2 -Wa,--trap -DMODULE $(WFLAGS)
+
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),SIGMA_8622)
+CFLAGS := -D__KERNEL__ -I$(CROSS_COMPILE_INCLUDE)/include -I$(LINUX_SRC)/include -I$(RT28xx_DIR)/include -Wall -Wstrict-prototypes -Wno-trigraphs -O2 -fno-strict-aliasing -fno-common -fno-common -pipe -fno-builtin -D__linux__ -DNO_MM -mapcs-32 -march=armv4 -mtune=arm7tdmi -msoft-float -DMODULE -mshort-load-bytes -nostdinc -iwithprefix -DMODULE $(WFLAGS)
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),5VT)
+CFLAGS := -D__KERNEL__ -I$(LINUX_SRC)/include -I$(RT28xx_DIR)/include -mlittle-endian -Wall -Wundef -Wstrict-prototypes -Wno-trigraphs -fno-strict-aliasing -fno-common -O3 -fno-omit-frame-pointer -fno-optimize-sibling-calls -fno-omit-frame-pointer -mapcs -mno-sched-prolog -mabi=apcs-gnu -mno-thumb-interwork -D__LINUX_ARM_ARCH__=5 -march=armv5te -mtune=arm926ej-s --param max-inline-insns-single=40000  -Uarm -Wdeclaration-after-statement -Wno-pointer-sign -DMODULE $(WFLAGS)
+
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),IKANOS_V160)
+CFLAGS := -D__KERNEL__ -I$(LINUX_SRC)/include -I$(LINUX_SRC)/include/asm/gcc -I$(LINUX_SRC)/include/asm-mips/mach-tango2 -I$(LINUX_SRC)/include/asm-mips/mach-tango2 -I$(LINUX_SRC)/include/asm-mips/mach-generic -I$(RT28xx_DIR)/include -Wall -Wundef -Wstrict-prototypes -Wno-trigraphs -fno-strict-aliasing -fno-common -ffreestanding -O2 -fomit-frame-pointer -G 0 -mno-abicalls -fno-pic -pipe -march=lx4189 -Wa, -DMODULE $(WFLAGS)
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),IKANOS_V180)
+CFLAGS := -D__KERNEL__ -I$(LINUX_SRC)/include -I$(LINUX_SRC)/include/asm/gcc -I$(LINUX_SRC)/include/asm-mips/mach-tango2 -I$(LINUX_SRC)/include/asm-mips/mach-tango2 -I$(LINUX_SRC)/include/asm-mips/mach-generic -I$(RT28xx_DIR)/include -Wall -Wundef -Wstrict-prototypes -Wno-trigraphs -fno-strict-aliasing -fno-common -ffreestanding -O2 -fomit-frame-pointer -G 0 -mno-abicalls -fno-pic -pipe -mips32r2 -Wa, -DMODULE $(WFLAGS)
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),INF_TWINPASS)
+CFLAGS := -D__KERNEL__ -DMODULE -I$(LINUX_SRC)/include -I$(RT28xx_DIR)/include -Wall -Wstrict-prototypes -Wno-trigraphs -O2 -fomit-frame-pointer -fno-strict-aliasing -fno-common -G 0 -mno-abicalls -fno-pic -march=4kc -mips32 -Wa,--trap -pipe -mlong-calls $(WFLAGS)
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),INF_DANUBE)
+CFLAGS := -I$(RT28xx_DIR)/include $(WFLAGS) -Wundef -fno-strict-aliasing -fno-common -ffreestanding -Os -fomit-frame-pointer -G 0 -mno-abicalls -fno-pic -pipe -msoft-float  -mabi=32 -march=mips32 -Wa,-32 -Wa,-march=mips32 -Wa,-mips32 -Wa,--trap -I$(LINUX_SRC)/include/asm-mips/mach-generic
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),BRCM_6358)
+CFLAGS := $(WFLAGS) -I$(RT28xx_DIR)/include -nostdinc -iwithprefix include -D__KERNEL__ -Wall -Wstrict-prototypes -Wno-trigraphs -fno-strict-aliasing -fno-common -I $(LINUX_SRC)/include/asm/gcc -G 0 -mno-abicalls -fno-pic -pipe  -finline-limit=100000 -mabi=32 -march=mips32 -Wa,-32 -Wa,-march=mips32 -Wa,-mips32 -Wa,--trap -I$(LINUX_SRC)/include/asm-mips/mach-bcm963xx -I$(LINUX_SRC)/include/asm-mips/mach-generic  -Os -fomit-frame-pointer -Wdeclaration-after-statement  -DMODULE -mlong-calls
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),PC)
+    ifneq (,$(findstring 2.4,$(LINUX_SRC)))
+	# Linux 2.4
+	CFLAGS := -D__KERNEL__ -I$(LINUX_SRC)/include -I$(RT28xx_DIR)/include -O2 -fomit-frame-pointer -fno-strict-aliasing -fno-common -pipe -mpreferred-stack-boundary=2 -march=i686 -DMODULE -DMODVERSIONS -include $(LINUX_SRC)/include/linux/modversions.h $(WFLAGS)
+	export CFLAGS
+    else
+	# Linux 2.6
+	EXTRA_CFLAGS := $(WFLAGS) -I$(RT28xx_DIR)/include
+    endif
+endif
+
+ifeq ($(PLATFORM),IXP)
+        EXTRA_CFLAGS := -v $(WFLAGS) -I$(RT28xx_DIR)/include -mbig-endian
+endif
+
+ifeq ($(PLATFORM),CAVM_OCTEON)
+	EXTRA_CFLAGS := $(WFLAGS) -I$(RT28xx_DIR)/include \
+				    -mabi=64 $(WFLAGS)
+export CFLAGS
+endif
+
diff --git a/drivers/staging/rt2860/dfs.h b/drivers/staging/rt2860/dfs.h
new file mode 100644
index 000000000000..752a6352d9dd
--- /dev/null
+++ b/drivers/staging/rt2860/dfs.h
@@ -0,0 +1,100 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    dfs.h
+
+    Abstract:
+    Support DFS function.
+
+    Revision History:
+    Who       When            What
+    --------  ----------      ----------------------------------------------
+    Fonchi    03-12-2007      created
+*/
+
+#define RADAR_PULSE 1
+#define RADAR_WIDTH 2
+
+#define WIDTH_RD_IDLE 0
+#define WIDTH_RD_CHECK 1
+
+
+VOID BbpRadarDetectionStart(
+	IN PRTMP_ADAPTER pAd);
+
+VOID BbpRadarDetectionStop(
+	IN PRTMP_ADAPTER pAd);
+
+VOID RadarDetectionStart(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN CTS_Protect,
+	IN UINT8 CTSPeriod);
+
+VOID RadarDetectionStop(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID RadarDetectPeriodic(
+	IN PRTMP_ADAPTER	pAd);
+
+
+BOOLEAN RadarChannelCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			Ch);
+
+ULONG JapRadarType(
+	IN PRTMP_ADAPTER pAd);
+
+ULONG RTMPBbpReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd);
+
+ULONG RTMPReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID RTMPCleanRadarDuration(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID RTMPPrepareRDCTSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA,
+	IN	ULONG			Duration,
+	IN  UCHAR           RTSRate,
+	IN  ULONG           CTSBaseAddr,
+	IN  UCHAR			FrameGap);
+
+VOID RTMPPrepareRadarDetectParams(
+	IN PRTMP_ADAPTER	pAd);
+
+
+INT Set_ChMovingTime_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg);
+
+INT Set_LongPulseRadarTh_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg);
+
+
diff --git a/drivers/staging/rt2860/leap.h b/drivers/staging/rt2860/leap.h
new file mode 100644
index 000000000000..6818c1ff4d73
--- /dev/null
+++ b/drivers/staging/rt2860/leap.h
@@ -0,0 +1,215 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	leap.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#ifndef __LEAP_H__
+#define __LEAP_H__
+
+// Messages for Associate state machine
+#define LEAP_MACHINE_BASE                   30
+
+#define LEAP_MSG_REQUEST_IDENTITY           31
+#define LEAP_MSG_REQUEST_LEAP               32
+#define LEAP_MSG_SUCCESS                    33
+#define LEAP_MSG_FAILED                     34
+#define LEAP_MSG_RESPONSE_LEAP              35
+#define LEAP_MSG_EAPOLKEY                   36
+#define LEAP_MSG_UNKNOWN                    37
+#define LEAP_MSG                            38
+//! assoc state-machine states
+#define LEAP_IDLE                           0
+#define LEAP_WAIT_IDENTITY_REQUEST          1
+#define LEAP_WAIT_CHANLLENGE_REQUEST        2
+#define LEAP_WAIT_SUCCESS                   3
+#define LEAP_WAIT_CHANLLENGE_RESPONSE       4
+#define LEAP_WAIT_EAPOLKEY                  5
+
+#define LEAP_REASON_INVALID_AUTH                    0x01
+#define LEAP_REASON_AUTH_TIMEOUT                    0x02
+#define LEAP_REASON_CHALLENGE_FROM_AP_FAILED        0x03
+#define LEAP_REASON_CHALLENGE_TO_AP_FAILED          0x04
+
+#define CISCO_AuthModeLEAP                          0x80
+#define CISCO_AuthModeLEAPNone                      0x00
+#define LEAP_AUTH_TIMEOUT                           30000
+#define LEAP_CHALLENGE_RESPONSE_LENGTH              24
+#define LEAP_CHALLENGE_REQUEST_LENGTH               8
+
+typedef struct _LEAP_EAPOL_HEADER_ {
+    UCHAR       Version;
+    UCHAR       Type;
+    UCHAR       Length[2];
+} LEAP_EAPOL_HEADER, *PLEAP_EAPOL_HEADER;
+
+typedef struct _LEAP_EAPOL_PACKET_ {
+    UCHAR       Code;
+    UCHAR       Identifier;
+    UCHAR       Length[2];
+    UCHAR       Type;
+} LEAP_EAPOL_PACKET, *PLEAP_EAPOL_PACKET;
+
+typedef struct _LEAP_EAP_CONTENTS_ {
+    UCHAR       Version;
+    UCHAR       Reserved;
+    UCHAR       Length;
+} LEAP_EAP_CONTENTS, *PLEAP_EAP_CONTENTS;
+
+/*** EAPOL key ***/
+typedef struct _EAPOL_KEY_HEADER_ {
+    UCHAR       Type;
+    UCHAR       Length[2];
+    UCHAR       Counter[8];
+    UCHAR       IV[16];
+    UCHAR       Index;
+    UCHAR       Signature[16];
+} EAPOL_KEY_HEADER, *PEAPOL_KEY_HEADER;
+
+BOOLEAN LeapMsgTypeSubst(
+    IN  UCHAR   EAPType,
+    OUT ULONG   *MsgType);
+
+VOID LeapMachinePerformAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN STATE_MACHINE    *S,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapMacHeaderInit(
+    IN  PRTMP_ADAPTER       pAd,
+    IN  OUT PHEADER_802_11  pHdr80211,
+    IN  UCHAR               wep,
+    IN  PUCHAR              pAddr3);
+
+VOID LeapStartAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapIdentityAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapPeerChallengeAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID HashPwd(
+    IN  PUCHAR  pwd,
+    IN  INT     pwdlen,
+    OUT PUCHAR  hash);
+
+VOID PeerChallengeResponse(
+    IN  PUCHAR  szChallenge,
+    IN  PUCHAR  smbPasswd,
+    OUT PUCHAR  szResponse);
+
+VOID ParityKey(
+    OUT PUCHAR  szOut,
+    IN  PUCHAR  szIn);
+
+VOID DesKey(
+    OUT ULONG   k[16][2],
+    IN  PUCHAR  key,
+    IN  INT     decrypt);
+
+VOID Des(
+    IN  ULONG   ks[16][2],
+    OUT UCHAR   block[8]);
+
+VOID DesEncrypt(
+    IN  PUCHAR  szClear,
+    IN  PUCHAR  szKey,
+    OUT PUCHAR  szOut);
+
+VOID LeapNetworkChallengeAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapNetworkChallengeResponse(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID HashpwdHash(
+    IN  PUCHAR  hash,
+    IN  PUCHAR  hashhash);
+
+VOID ProcessSessionKey(
+    OUT PUCHAR  SessionKey,
+    IN  PUCHAR  hash2,
+    IN  PUCHAR  ChallengeToRadius,
+    IN  PUCHAR  ChallengeResponseFromRadius,
+    IN  PUCHAR  ChallengeFromRadius,
+    IN  PUCHAR  ChallengeResponseToRadius);
+
+VOID LeapEapolKeyAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID RogueApTableInit(
+    IN ROGUEAP_TABLE    *Tab);
+
+ULONG RogueApTableSearch(
+    IN ROGUEAP_TABLE    *Tab,
+    IN PUCHAR           pAddr);
+
+VOID RogueApEntrySet(
+    IN  PRTMP_ADAPTER   pAd,
+    OUT ROGUEAP_ENTRY   *pRogueAp,
+    IN PUCHAR           pAddr,
+    IN UCHAR            FaileCode);
+
+ULONG RogueApTableSetEntry(
+    IN  PRTMP_ADAPTER   pAd,
+    OUT ROGUEAP_TABLE  *Tab,
+    IN PUCHAR           pAddr,
+    IN UCHAR            FaileCode);
+
+VOID RogueApTableDeleteEntry(
+    IN OUT ROGUEAP_TABLE *Tab,
+    IN PUCHAR          pAddr);
+
+VOID LeapAuthTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID LeapSendRogueAPReport(
+    IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN CCKMAssocRspSanity(
+    IN PRTMP_ADAPTER    pAd,
+    IN VOID             *Msg,
+    IN ULONG            MsgLen);
+
+#endif  // __LEAP_H__
diff --git a/drivers/staging/rt2860/link_list.h b/drivers/staging/rt2860/link_list.h
new file mode 100644
index 000000000000..f6521133fd5e
--- /dev/null
+++ b/drivers/staging/rt2860/link_list.h
@@ -0,0 +1,134 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __LINK_LIST_H__
+#define __LINK_LIST_H__
+
+typedef struct _LIST_ENTRY
+{
+	struct _LIST_ENTRY *pNext;
+} LIST_ENTRY, *PLIST_ENTRY;
+
+typedef struct _LIST_HEADR
+{
+	PLIST_ENTRY pHead;
+	PLIST_ENTRY pTail;
+	UCHAR size;
+} LIST_HEADER, *PLIST_HEADER;
+
+static inline VOID initList(
+	IN PLIST_HEADER pList)
+{
+	pList->pHead = pList->pTail = NULL;
+	pList->size = 0;
+	return;
+}
+
+static inline VOID insertTailList(
+	IN PLIST_HEADER pList,
+	IN PLIST_ENTRY pEntry)
+{
+	pEntry->pNext = NULL;
+	if (pList->pTail)
+		pList->pTail->pNext = pEntry;
+	else
+		pList->pHead = pEntry;
+	pList->pTail = pEntry;
+	pList->size++;
+
+	return;
+}
+
+static inline PLIST_ENTRY removeHeadList(
+	IN PLIST_HEADER pList)
+{
+	PLIST_ENTRY pNext;
+	PLIST_ENTRY pEntry;
+
+	pEntry = pList->pHead;
+	if (pList->pHead != NULL)
+	{
+		pNext = pList->pHead->pNext;
+		pList->pHead = pNext;
+		if (pNext == NULL)
+			pList->pTail = NULL;
+		pList->size--;
+	}
+	return pEntry;
+}
+
+static inline int getListSize(
+	IN PLIST_HEADER pList)
+{
+	return pList->size;
+}
+
+static inline PLIST_ENTRY delEntryList(
+	IN PLIST_HEADER pList,
+	IN PLIST_ENTRY pEntry)
+{
+	PLIST_ENTRY pCurEntry;
+	PLIST_ENTRY pPrvEntry;
+
+	if(pList->pHead == NULL)
+		return NULL;
+
+	if(pEntry == pList->pHead)
+	{
+		pCurEntry = pList->pHead;
+		pList->pHead = pCurEntry->pNext;
+
+		if(pList->pHead == NULL)
+			pList->pTail = NULL;
+
+		pList->size--;
+		return pCurEntry;
+	}
+
+	pPrvEntry = pList->pHead;
+	pCurEntry = pPrvEntry->pNext;
+	while(pCurEntry != NULL)
+	{
+		if (pEntry == pCurEntry)
+		{
+			pPrvEntry->pNext = pCurEntry->pNext;
+
+			if(pEntry == pList->pTail)
+				pList->pTail = pPrvEntry;
+
+			pList->size--;
+			break;
+		}
+		pPrvEntry = pCurEntry;
+		pCurEntry = pPrvEntry->pNext;
+	}
+
+	return pCurEntry;
+}
+
+#endif // ___LINK_LIST_H__ //
+
diff --git a/drivers/staging/rt2860/md4.h b/drivers/staging/rt2860/md4.h
new file mode 100644
index 000000000000..f1e5b526350a
--- /dev/null
+++ b/drivers/staging/rt2860/md4.h
@@ -0,0 +1,42 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __MD4_H__
+#define __MD4_H__
+
+/* MD4 context. */
+typedef	struct	_MD4_CTX_	{
+	ULONG	state[4];        /* state (ABCD) */
+	ULONG	count[2];        /* number of bits, modulo 2^64 (lsb first) */
+	UCHAR	buffer[64];      /* input buffer */
+}	MD4_CTX;
+
+VOID MD4Init (MD4_CTX *);
+VOID MD4Update (MD4_CTX *, PUCHAR, UINT);
+VOID MD4Final (UCHAR [16], MD4_CTX *);
+
+#endif //__MD4_H__
\ No newline at end of file
diff --git a/drivers/staging/rt2860/md5.h b/drivers/staging/rt2860/md5.h
new file mode 100644
index 000000000000..d85db12170d5
--- /dev/null
+++ b/drivers/staging/rt2860/md5.h
@@ -0,0 +1,107 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	md5.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	jan			10-28-03		Initial
+	Rita    	11-23-04		Modify MD5 and SHA-1
+*/
+
+#ifndef	uint8
+#define	uint8  unsigned	char
+#endif
+
+#ifndef	uint32
+#define	uint32 unsigned	long int
+#endif
+
+
+#ifndef	__MD5_H__
+#define	__MD5_H__
+
+#define MD5_MAC_LEN 16
+
+typedef struct _MD5_CTX {
+    UINT32   Buf[4];             // buffers of four states
+	UCHAR   Input[64];          // input message
+	UINT32   LenInBitCount[2];   // length counter for input message, 0 up to 64 bits
+}   MD5_CTX;
+
+VOID MD5Init(MD5_CTX *pCtx);
+VOID MD5Update(MD5_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes);
+VOID MD5Final(UCHAR Digest[16], MD5_CTX *pCtx);
+VOID MD5Transform(UINT32 Buf[4], UINT32 Mes[16]);
+
+void md5_mac(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac);
+void hmac_md5(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac);
+
+//
+// SHA context
+//
+typedef	struct _SHA_CTX
+{
+	UINT32   Buf[5];             // buffers of five states
+	UCHAR   Input[80];          // input message
+	UINT32   LenInBitCount[2];   // length counter for input message, 0 up to 64 bits
+
+}	SHA_CTX;
+
+VOID SHAInit(SHA_CTX *pCtx);
+UCHAR SHAUpdate(SHA_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes);
+VOID SHAFinal(SHA_CTX *pCtx, UCHAR Digest[20]);
+VOID SHATransform(UINT32 Buf[5], UINT32 Mes[20]);
+
+#define SHA_DIGEST_LEN 20
+#endif // __MD5_H__
+
+/******************************************************************************/
+#ifndef	_AES_H
+#define	_AES_H
+
+typedef	struct
+{
+	uint32 erk[64];		/* encryption round	keys */
+	uint32 drk[64];		/* decryption round	keys */
+	int	nr;				/* number of rounds	*/
+}
+aes_context;
+
+int	 rtmp_aes_set_key( aes_context *ctx,	uint8 *key,	int	nbits );
+void rtmp_aes_encrypt( aes_context *ctx,	uint8 input[16], uint8 output[16] );
+void rtmp_aes_decrypt( aes_context *ctx,	uint8 input[16], uint8 output[16] );
+
+void F(char *password, unsigned char *ssid, int ssidlength, int iterations, int count, unsigned char *output);
+int PasswordHash(char *password, unsigned char *ssid, int ssidlength, unsigned char *output);
+
+#endif /* aes.h	*/
+
diff --git a/drivers/staging/rt2860/mlme.h b/drivers/staging/rt2860/mlme.h
new file mode 100644
index 000000000000..5cb61652b668
--- /dev/null
+++ b/drivers/staging/rt2860/mlme.h
@@ -0,0 +1,1447 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	mlme.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2003-08-28		Created
+	John Chang  2004-09-06      modified for RT2600
+
+*/
+#ifndef __MLME_H__
+#define __MLME_H__
+
+// maximum supported capability information -
+// ESS, IBSS, Privacy, Short Preamble, Spectrum mgmt, Short Slot
+#define SUPPORTED_CAPABILITY_INFO   0x0533
+
+#define END_OF_ARGS                 -1
+#define LFSR_MASK                   0x80000057
+#define MLME_TASK_EXEC_INTV         100/*200*/       //
+#define LEAD_TIME                   5
+#define MLME_TASK_EXEC_MULTIPLE       10  /*5*/       // MLME_TASK_EXEC_MULTIPLE * MLME_TASK_EXEC_INTV = 1 sec
+#define REORDER_EXEC_INTV         	100       // 0.1 sec
+
+// The definition of Radar detection duration region
+#define CE		0
+#define FCC		1
+#define JAP		2
+#define JAP_W53	3
+#define JAP_W56	4
+#define MAX_RD_REGION 5
+
+#ifdef	NDIS51_MINIPORT
+#define BEACON_LOST_TIME            4000       // 2048 msec = 2 sec
+#else
+#define BEACON_LOST_TIME            4 * OS_HZ    // 2048 msec = 2 sec
+#endif
+
+#define DLS_TIMEOUT                 1200      // unit: msec
+#define AUTH_TIMEOUT                300       // unit: msec
+#define ASSOC_TIMEOUT               300       // unit: msec
+#define JOIN_TIMEOUT                2 * OS_HZ      // unit: msec
+#define SHORT_CHANNEL_TIME          90        // unit: msec
+#define MIN_CHANNEL_TIME            110        // unit: msec, for dual band scan
+#define MAX_CHANNEL_TIME            140       // unit: msec, for single band scan
+#define	FAST_ACTIVE_SCAN_TIME	    30 		  // Active scan waiting for probe response time
+#define CW_MIN_IN_BITS              4         // actual CwMin = 2^CW_MIN_IN_BITS - 1
+
+
+#ifdef CONFIG_STA_SUPPORT
+#ifndef CONFIG_AP_SUPPORT
+#define CW_MAX_IN_BITS              10        // actual CwMax = 2^CW_MAX_IN_BITS - 1
+#endif
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+extern UINT32 CW_MAX_IN_BITS;
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+// Note: RSSI_TO_DBM_OFFSET has been changed to variable for new RF (2004-0720).
+// SHould not refer to this constant anymore
+//#define RSSI_TO_DBM_OFFSET          120 // for RT2530 RSSI-115 = dBm
+#define RSSI_FOR_MID_TX_POWER       -55  // -55 db is considered mid-distance
+#define RSSI_FOR_LOW_TX_POWER       -45  // -45 db is considered very short distance and
+                                        // eligible to use a lower TX power
+#define RSSI_FOR_LOWEST_TX_POWER    -30
+//#define MID_TX_POWER_DELTA          0   // 0 db from full TX power upon mid-distance to AP
+#define LOW_TX_POWER_DELTA          6    // -3 db from full TX power upon very short distance. 1 grade is 0.5 db
+#define LOWEST_TX_POWER_DELTA       16   // -8 db from full TX power upon shortest distance. 1 grade is 0.5 db
+
+#define RSSI_TRIGGERED_UPON_BELOW_THRESHOLD     0
+#define RSSI_TRIGGERED_UPON_EXCCEED_THRESHOLD   1
+#define RSSI_THRESHOLD_FOR_ROAMING              25
+#define RSSI_DELTA                              5
+
+// Channel Quality Indication
+#define CQI_IS_GOOD(cqi)            ((cqi) >= 50)
+//#define CQI_IS_FAIR(cqi)          (((cqi) >= 20) && ((cqi) < 50))
+#define CQI_IS_POOR(cqi)            (cqi < 50)  //(((cqi) >= 5) && ((cqi) < 20))
+#define CQI_IS_BAD(cqi)             (cqi < 5)
+#define CQI_IS_DEAD(cqi)            (cqi == 0)
+
+// weighting factor to calculate Channel quality, total should be 100%
+#define RSSI_WEIGHTING                   50
+#define TX_WEIGHTING                     30
+#define RX_WEIGHTING                     20
+
+#define BSS_NOT_FOUND                    0xFFFFFFFF
+
+
+#ifdef CONFIG_STA_SUPPORT
+#define MAX_LEN_OF_MLME_QUEUE            40 //10
+#endif // CONFIG_STA_SUPPORT //
+
+#define SCAN_PASSIVE                     18		// scan with no probe request, only wait beacon and probe response
+#define SCAN_ACTIVE                      19		// scan with probe request, and wait beacon and probe response
+#define	SCAN_CISCO_PASSIVE				 20		// Single channel passive scan
+#define	SCAN_CISCO_ACTIVE				 21		// Single channel active scan
+#define	SCAN_CISCO_NOISE				 22		// Single channel passive scan for noise histogram collection
+#define	SCAN_CISCO_CHANNEL_LOAD			 23		// Single channel passive scan for channel load collection
+#define FAST_SCAN_ACTIVE                 24		// scan with probe request, and wait beacon and probe response
+
+#ifdef DOT11N_DRAFT3
+#define SCAN_2040_BSS_COEXIST                  26
+#endif // DOT11N_DRAFT3 //
+
+#define MAC_ADDR_IS_GROUP(Addr)       (((Addr[0]) & 0x01))
+#define MAC_ADDR_HASH(Addr)            (Addr[0] ^ Addr[1] ^ Addr[2] ^ Addr[3] ^ Addr[4] ^ Addr[5])
+#define MAC_ADDR_HASH_INDEX(Addr)      (MAC_ADDR_HASH(Addr) % HASH_TABLE_SIZE)
+#define TID_MAC_HASH(Addr,TID)            (TID^Addr[0] ^ Addr[1] ^ Addr[2] ^ Addr[3] ^ Addr[4] ^ Addr[5])
+#define TID_MAC_HASH_INDEX(Addr,TID)      (TID_MAC_HASH(Addr,TID) % HASH_TABLE_SIZE)
+
+// LED Control
+// assoiation ON. one LED ON. another blinking when TX, OFF when idle
+// no association, both LED off
+#define ASIC_LED_ACT_ON(pAd)        RTMP_IO_WRITE32(pAd, MAC_CSR14, 0x00031e46)
+#define ASIC_LED_ACT_OFF(pAd)       RTMP_IO_WRITE32(pAd, MAC_CSR14, 0x00001e46)
+
+// bit definition of the 2-byte pBEACON->Capability field
+#define CAP_IS_ESS_ON(x)                 (((x) & 0x0001) != 0)
+#define CAP_IS_IBSS_ON(x)                (((x) & 0x0002) != 0)
+#define CAP_IS_CF_POLLABLE_ON(x)         (((x) & 0x0004) != 0)
+#define CAP_IS_CF_POLL_REQ_ON(x)         (((x) & 0x0008) != 0)
+#define CAP_IS_PRIVACY_ON(x)             (((x) & 0x0010) != 0)
+#define CAP_IS_SHORT_PREAMBLE_ON(x)      (((x) & 0x0020) != 0)
+#define CAP_IS_PBCC_ON(x)                (((x) & 0x0040) != 0)
+#define CAP_IS_AGILITY_ON(x)             (((x) & 0x0080) != 0)
+#define CAP_IS_SPECTRUM_MGMT(x)          (((x) & 0x0100) != 0)  // 802.11e d9
+#define CAP_IS_QOS(x)                    (((x) & 0x0200) != 0)  // 802.11e d9
+#define CAP_IS_SHORT_SLOT(x)             (((x) & 0x0400) != 0)
+#define CAP_IS_APSD(x)                   (((x) & 0x0800) != 0)  // 802.11e d9
+#define CAP_IS_IMMED_BA(x)               (((x) & 0x1000) != 0)  // 802.11e d9
+#define CAP_IS_DSSS_OFDM(x)              (((x) & 0x2000) != 0)
+#define CAP_IS_DELAY_BA(x)               (((x) & 0x4000) != 0)  // 802.11e d9
+
+#define CAP_GENERATE(ess,ibss,priv,s_pre,s_slot,spectrum)  (((ess) ? 0x0001 : 0x0000) | ((ibss) ? 0x0002 : 0x0000) | ((priv) ? 0x0010 : 0x0000) | ((s_pre) ? 0x0020 : 0x0000) | ((s_slot) ? 0x0400 : 0x0000) | ((spectrum) ? 0x0100 : 0x0000))
+
+#define ERP_IS_NON_ERP_PRESENT(x)        (((x) & 0x01) != 0)    // 802.11g
+#define ERP_IS_USE_PROTECTION(x)         (((x) & 0x02) != 0)    // 802.11g
+#define ERP_IS_USE_BARKER_PREAMBLE(x)    (((x) & 0x04) != 0)    // 802.11g
+
+#define DRS_TX_QUALITY_WORST_BOUND       8// 3  // just test by gary
+#define DRS_PENALTY                      8
+
+#define BA_NOTUSE 	2
+//BA Policy subfiled value in ADDBA frame
+#define IMMED_BA 	1
+#define DELAY_BA	0
+
+// BA Initiator subfield in DELBA frame
+#define ORIGINATOR	1
+#define RECIPIENT	0
+
+// ADDBA Status Code
+#define ADDBA_RESULTCODE_SUCCESS					0
+#define ADDBA_RESULTCODE_REFUSED					37
+#define ADDBA_RESULTCODE_INVALID_PARAMETERS			38
+
+// DELBA Reason Code
+#define DELBA_REASONCODE_QSTA_LEAVING				36
+#define DELBA_REASONCODE_END_BA						37
+#define DELBA_REASONCODE_UNKNOWN_BA					38
+#define DELBA_REASONCODE_TIMEOUT					39
+
+// reset all OneSecTx counters
+#define RESET_ONE_SEC_TX_CNT(__pEntry) \
+if (((__pEntry)) != NULL) \
+{ \
+	(__pEntry)->OneSecTxRetryOkCount = 0; \
+	(__pEntry)->OneSecTxFailCount = 0; \
+	(__pEntry)->OneSecTxNoRetryOkCount = 0; \
+}
+
+//
+// 802.11 frame formats
+//
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	USHORT	LSIGTxopProSup:1;
+	USHORT	Forty_Mhz_Intolerant:1;
+	USHORT	PSMP:1;
+	USHORT	CCKmodein40:1;
+	USHORT	AMsduSize:1;
+	USHORT	DelayedBA:1;	//rt2860c not support
+	USHORT	RxSTBC:2;
+	USHORT	TxSTBC:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	ShortGIfor20:1;
+	USHORT	GF:1;	//green field
+	USHORT	MimoPs:2;//momi power safe
+	USHORT	ChannelWidth:1;
+	USHORT	AdvCoding:1;
+#else
+	USHORT	AdvCoding:1;
+	USHORT	ChannelWidth:1;
+	USHORT	MimoPs:2;//momi power safe
+	USHORT	GF:1;	//green field
+	USHORT	ShortGIfor20:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	TxSTBC:1;
+	USHORT	RxSTBC:2;
+	USHORT	DelayedBA:1;	//rt2860c not support
+	USHORT	AMsduSize:1;	// only support as zero
+	USHORT	CCKmodein40:1;
+	USHORT	PSMP:1;
+	USHORT	Forty_Mhz_Intolerant:1;
+	USHORT	LSIGTxopProSup:1;
+#endif	/* !RT_BIG_ENDIAN */
+} HT_CAP_INFO, *PHT_CAP_INFO;
+
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:3;//momi power safe
+	UCHAR	MpduDensity:3;
+	UCHAR	MaxRAmpduFactor:2;
+#else
+	UCHAR	MaxRAmpduFactor:2;
+	UCHAR	MpduDensity:3;
+	UCHAR	rsv:3;//momi power safe
+#endif /* !RT_BIG_ENDIAN */
+} HT_CAP_PARM, *PHT_CAP_PARM;
+
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+	UCHAR	MCSSet[10];
+	UCHAR	SupRate[2];  // unit : 1Mbps
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:3;
+	UCHAR	MpduDensity:1;
+	UCHAR	TxStream:2;
+	UCHAR	TxRxNotEqual:1;
+	UCHAR	TxMCSSetDefined:1;
+#else
+	UCHAR	TxMCSSetDefined:1;
+	UCHAR	TxRxNotEqual:1;
+	UCHAR	TxStream:2;
+	UCHAR	MpduDensity:1;
+	UCHAR	rsv:3;
+#endif // RT_BIG_ENDIAN //
+	UCHAR	rsv3[3];
+} HT_MCS_SET, *PHT_MCS_SET;
+
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv2:4;
+	USHORT	RDGSupport:1;	//reverse Direction Grant  support
+	USHORT	PlusHTC:1;	//+HTC control field support
+	USHORT	MCSFeedback:2;	//0:no MCS feedback, 2:unsolicited MCS feedback, 3:Full MCS feedback,  1:rsv.
+	USHORT	rsv:5;//momi power safe
+	USHORT	TranTime:2;
+	USHORT	Pco:1;
+#else
+	USHORT	Pco:1;
+	USHORT	TranTime:2;
+	USHORT	rsv:5;//momi power safe
+	USHORT	MCSFeedback:2;	//0:no MCS feedback, 2:unsolicited MCS feedback, 3:Full MCS feedback,  1:rsv.
+	USHORT	PlusHTC:1;	//+HTC control field support
+	USHORT	RDGSupport:1;	//reverse Direction Grant  support
+	USHORT	rsv2:4;
+#endif /* RT_BIG_ENDIAN */
+} EXT_HT_CAP_INFO, *PEXT_HT_CAP_INFO;
+
+//  HT Beamforming field in HT Cap IE .
+typedef struct PACKED _HT_BF_CAP{
+#ifdef RT_BIG_ENDIAN
+	ULONG	rsv:3;
+	ULONG	ChanEstimation:2;
+	ULONG	CSIRowBFSup:2;
+	ULONG	ComSteerBFAntSup:2;
+	ULONG	NoComSteerBFAntSup:2;
+	ULONG	CSIBFAntSup:2;
+	ULONG	MinGrouping:2;
+	ULONG	ExpComBF:2;
+	ULONG	ExpNoComBF:2;
+	ULONG	ExpCSIFbk:2;
+	ULONG	ExpComSteerCapable:1;
+	ULONG	ExpNoComSteerCapable:1;
+	ULONG	ExpCSICapable:1;
+	ULONG	Calibration:2;
+	ULONG	ImpTxBFCapable:1;
+	ULONG	TxNDPCapable:1;
+	ULONG	RxNDPCapable:1;
+	ULONG	TxSoundCapable:1;
+	ULONG	RxSoundCapable:1;
+	ULONG	TxBFRecCapable:1;
+#else
+	ULONG	TxBFRecCapable:1;
+	ULONG	RxSoundCapable:1;
+	ULONG	TxSoundCapable:1;
+	ULONG	RxNDPCapable:1;
+	ULONG	TxNDPCapable:1;
+	ULONG	ImpTxBFCapable:1;
+	ULONG	Calibration:2;
+	ULONG	ExpCSICapable:1;
+	ULONG	ExpNoComSteerCapable:1;
+	ULONG	ExpComSteerCapable:1;
+	ULONG	ExpCSIFbk:2;
+	ULONG	ExpNoComBF:2;
+	ULONG	ExpComBF:2;
+	ULONG	MinGrouping:2;
+	ULONG	CSIBFAntSup:2;
+	ULONG	NoComSteerBFAntSup:2;
+	ULONG	ComSteerBFAntSup:2;
+	ULONG	CSIRowBFSup:2;
+	ULONG	ChanEstimation:2;
+	ULONG	rsv:3;
+#endif // RT_BIG_ENDIAN //
+} HT_BF_CAP, *PHT_BF_CAP;
+
+//  HT antenna selection field in HT Cap IE .
+typedef struct PACKED _HT_AS_CAP{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:1;
+	UCHAR	TxSoundPPDU:1;
+	UCHAR	RxASel:1;
+	UCHAR	AntIndFbk:1;
+	UCHAR	ExpCSIFbk:1;
+	UCHAR	AntIndFbkTxASEL:1;
+	UCHAR	ExpCSIFbkTxASEL:1;
+	UCHAR	AntSelect:1;
+#else
+	UCHAR	AntSelect:1;
+	UCHAR	ExpCSIFbkTxASEL:1;
+	UCHAR	AntIndFbkTxASEL:1;
+	UCHAR	ExpCSIFbk:1;
+	UCHAR	AntIndFbk:1;
+	UCHAR	RxASel:1;
+	UCHAR	TxSoundPPDU:1;
+	UCHAR	rsv:1;
+#endif // RT_BIG_ENDIAN //
+} HT_AS_CAP, *PHT_AS_CAP;
+
+// Draft 1.0 set IE length 26, but is extensible..
+#define SIZE_HT_CAP_IE		26
+// The structure for HT Capability IE.
+typedef struct PACKED _HT_CAPABILITY_IE{
+	HT_CAP_INFO		HtCapInfo;
+	HT_CAP_PARM		HtCapParm;
+//	HT_MCS_SET		HtMCSSet;
+	UCHAR			MCSSet[16];
+	EXT_HT_CAP_INFO	ExtHtCapInfo;
+	HT_BF_CAP		TxBFCap;	// beamforming cap. rt2860c not support beamforming.
+	HT_AS_CAP		ASCap;	//antenna selection.
+} HT_CAPABILITY_IE, *PHT_CAPABILITY_IE;
+
+
+// 802.11n draft3 related structure definitions.
+// 7.3.2.60
+#define dot11OBSSScanPassiveDwell							20	// in TU. min amount of time that the STA continously scans each channel when performing an active OBSS scan.
+#define dot11OBSSScanActiveDwell							10	// in TU.min amount of time that the STA continously scans each channel when performing an passive OBSS scan.
+#define dot11BSSWidthTriggerScanInterval					300  // in sec. max interval between scan operations to be performed to detect BSS channel width trigger events.
+#define dot11OBSSScanPassiveTotalPerChannel					200	// in TU. min total amount of time that the STA scans each channel when performing a passive OBSS scan.
+#define dot11OBSSScanActiveTotalPerChannel					20	//in TU. min total amount of time that the STA scans each channel when performing a active OBSS scan
+#define dot11BSSWidthChannelTransactionDelayFactor			5	// min ratio between the delay time in performing a switch from 20MHz BSS to 20/40 BSS operation and the maxima
+																//	interval between overlapping BSS scan operations.
+#define dot11BSSScanActivityThreshold						25	// in %%, max total time that a STA may be active on the medium during a period of
+																//	(dot11BSSWidthChannelTransactionDelayFactor * dot11BSSWidthTriggerScanInterval) seconds without
+																//	being obligated to perform OBSS Scan operations. default is 25(== 0.25%)
+
+typedef struct PACKED _OVERLAP_BSS_SCAN_IE{
+	USHORT		ScanPassiveDwell;
+	USHORT		ScanActiveDwell;
+	USHORT		TriggerScanInt;				// Trigger scan interval
+	USHORT		PassiveTalPerChannel;		// passive total per channel
+	USHORT		ActiveTalPerChannel;		// active total per channel
+	USHORT		DelayFactor;				// BSS width channel transition delay factor
+	USHORT		ScanActThre;				// Scan Activity threshold
+}OVERLAP_BSS_SCAN_IE, *POVERLAP_BSS_SCAN_IE;
+
+
+//  7.3.2.56. 20/40 Coexistence element used in  Element ID = 72 = IE_2040_BSS_COEXIST
+typedef union PACKED _BSS_2040_COEXIST_IE{
+ struct PACKED {
+ #ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:5;
+	UCHAR	BSS20WidthReq:1;
+	UCHAR	Intolerant40:1;
+	UCHAR	InfoReq:1;
+ #else
+	UCHAR	InfoReq:1;
+	UCHAR	Intolerant40:1;			// Inter-BSS. set 1 when prohibits a receiving BSS from operating as a 20/40 Mhz BSS.
+	UCHAR	BSS20WidthReq:1;		// Intra-BSS set 1 when prohibits a receiving AP from operating its BSS as a 20/40MHz BSS.
+	UCHAR	rsv:5;
+#endif // RT_BIG_ENDIAN //
+    } field;
+ UCHAR   word;
+} BSS_2040_COEXIST_IE, *PBSS_2040_COEXIST_IE;
+
+
+typedef struct  _TRIGGER_EVENTA{
+	BOOLEAN			bValid;
+	UCHAR	BSSID[6];
+	UCHAR	RegClass;	// Regulatory Class
+	USHORT	Channel;
+	ULONG	CDCounter;   // Maintain a seperate count down counter for each Event A.
+} TRIGGER_EVENTA, *PTRIGGER_EVENTA;
+
+// 20/40 trigger event table
+// If one Event A delete or created, or if Event B is detected or not detected, STA should send 2040BSSCoexistence to AP.
+#define MAX_TRIGGER_EVENT		64
+typedef struct  _TRIGGER_EVENT_TAB{
+	UCHAR	EventANo;
+	TRIGGER_EVENTA	EventA[MAX_TRIGGER_EVENT];
+	ULONG			EventBCountDown;	// Count down counter for Event B.
+} TRIGGER_EVENT_TAB, *PTRIGGER_EVENT_TAB;
+
+// 7.3.27 20/40 Bss Coexistence Mgmt capability used in extended capabilities information IE( ID = 127 = IE_EXT_CAPABILITY).
+//	This is the first octet and was defined in 802.11n D3.03 and 802.11yD9.0
+typedef struct PACKED _EXT_CAP_INFO_ELEMENT{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv2:5;
+	UCHAR	ExtendChannelSwitch:1;
+	UCHAR	rsv:1;
+	UCHAR	BssCoexistMgmtSupport:1;
+#else
+	UCHAR	BssCoexistMgmtSupport:1;
+	UCHAR	rsv:1;
+	UCHAR	ExtendChannelSwitch:1;
+	UCHAR	rsv2:5;
+#endif // RT_BIG_ENDIAN //
+}EXT_CAP_INFO_ELEMENT, *PEXT_CAP_INFO_ELEMENT;
+
+
+// 802.11n 7.3.2.61
+typedef struct PACKED _BSS_2040_COEXIST_ELEMENT{
+	UCHAR					ElementID;		// ID = IE_2040_BSS_COEXIST = 72
+	UCHAR					Len;
+	BSS_2040_COEXIST_IE		BssCoexistIe;
+}BSS_2040_COEXIST_ELEMENT, *PBSS_2040_COEXIST_ELEMENT;
+
+
+//802.11n 7.3.2.59
+typedef struct PACKED _BSS_2040_INTOLERANT_CH_REPORT{
+	UCHAR				ElementID;		// ID = IE_2040_BSS_INTOLERANT_REPORT = 73
+	UCHAR				Len;
+	UCHAR				RegulatoryClass;
+	UCHAR				ChList[0];
+}BSS_2040_INTOLERANT_CH_REPORT, *PBSS_2040_INTOLERANT_CH_REPORT;
+
+
+// The structure for channel switch annoucement IE. This is in 802.11n D3.03
+typedef struct PACKED _CHA_SWITCH_ANNOUNCE_IE{
+	UCHAR			SwitchMode;	//channel switch mode
+	UCHAR			NewChannel;	//
+	UCHAR			SwitchCount;	//
+} CHA_SWITCH_ANNOUNCE_IE, *PCHA_SWITCH_ANNOUNCE_IE;
+
+
+// The structure for channel switch annoucement IE. This is in 802.11n D3.03
+typedef struct PACKED _SEC_CHA_OFFSET_IE{
+	UCHAR			SecondaryChannelOffset;	 // 1: Secondary above, 3: Secondary below, 0: no Secondary
+} SEC_CHA_OFFSET_IE, *PSEC_CHA_OFFSET_IE;
+
+
+// This structure is extracted from struct RT_HT_CAPABILITY
+typedef struct {
+	BOOLEAN			bHtEnable;	 // If we should use ht rate.
+	BOOLEAN			bPreNHt;	 // If we should use ht rate.
+	//Substract from HT Capability IE
+	UCHAR			MCSSet[16];	//only supoort MCS=0-15,32 ,
+} RT_HT_PHY_INFO, *PRT_HT_PHY_INFO;
+
+//This structure substracts ralink supports from all 802.11n-related features.
+//Features not listed here but contained in 802.11n spec are not supported in rt2860.
+typedef struct {
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv:5;
+	USHORT	AmsduSize:1;	// Max receiving A-MSDU size
+	USHORT	AmsduEnable:1;	// Enable to transmit A-MSDU. Suggest disable. We should use A-MPDU to gain best benifit of 802.11n
+	USHORT	RxSTBC:2;	// 2 bits
+	USHORT	TxSTBC:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	ShortGIfor20:1;
+	USHORT	GF:1;	//green field
+	USHORT	MimoPs:2;//mimo power safe MMPS_
+	USHORT	ChannelWidth:1;
+#else
+	USHORT	ChannelWidth:1;
+	USHORT	MimoPs:2;//mimo power safe MMPS_
+	USHORT	GF:1;	//green field
+	USHORT	ShortGIfor20:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	TxSTBC:1;
+	USHORT	RxSTBC:2;	// 2 bits
+	USHORT	AmsduEnable:1;	// Enable to transmit A-MSDU. Suggest disable. We should use A-MPDU to gain best benifit of 802.11n
+	USHORT	AmsduSize:1;	// Max receiving A-MSDU size
+	USHORT	rsv:5;
+#endif
+
+	//Substract from Addiont HT INFO IE
+#ifdef RT_BIG_ENDIAN
+	UCHAR	RecomWidth:1;
+	UCHAR	ExtChanOffset:2;	// Please not the difference with following 	UCHAR	NewExtChannelOffset; from 802.11n
+	UCHAR	MpduDensity:3;
+	UCHAR	MaxRAmpduFactor:2;
+#else
+	UCHAR	MaxRAmpduFactor:2;
+	UCHAR	MpduDensity:3;
+	UCHAR	ExtChanOffset:2;	// Please not the difference with following 	UCHAR	NewExtChannelOffset; from 802.11n
+	UCHAR	RecomWidth:1;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv2:11;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv3:1;
+	USHORT	NonGfPresent:1;
+	USHORT	OperaionMode:2;
+#else
+	USHORT	OperaionMode:2;
+	USHORT	NonGfPresent:1;
+	USHORT	rsv3:1;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv2:11;
+#endif
+
+	// New Extension Channel Offset IE
+	UCHAR	NewExtChannelOffset;
+	// Extension Capability IE = 127
+	UCHAR	BSSCoexist2040;
+} RT_HT_CAPABILITY, *PRT_HT_CAPABILITY;
+
+//   field in Addtional HT Information IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR	SerInterGranu:3;
+	UCHAR	S_PSMPSup:1;
+	UCHAR	RifsMode:1;
+	UCHAR	RecomWidth:1;
+	UCHAR	ExtChanOffset:2;
+#else
+	UCHAR	ExtChanOffset:2;
+	UCHAR	RecomWidth:1;
+	UCHAR	RifsMode:1;
+	UCHAR	S_PSMPSup:1;	 //Indicate support for scheduled PSMP
+	UCHAR	SerInterGranu:3;	 //service interval granularity
+#endif
+} ADD_HTINFO, *PADD_HTINFO;
+
+typedef struct PACKED{
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv2:11;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv:1;
+	USHORT	NonGfPresent:1;
+	USHORT	OperaionMode:2;
+#else
+	USHORT	OperaionMode:2;
+	USHORT	NonGfPresent:1;
+	USHORT	rsv:1;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv2:11;
+#endif
+} ADD_HTINFO2, *PADD_HTINFO2;
+
+
+// TODO: Need sync with spec about the definition of StbcMcs. In Draft 3.03, it's reserved.
+typedef struct PACKED{
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv:4;
+	USHORT	PcoPhase:1;
+	USHORT	PcoActive:1;
+	USHORT	LsigTxopProt:1;
+	USHORT	STBCBeacon:1;
+	USHORT	DualCTSProtect:1;
+	USHORT	DualBeacon:1;
+	USHORT	StbcMcs:6;
+#else
+	USHORT	StbcMcs:6;
+	USHORT	DualBeacon:1;
+	USHORT	DualCTSProtect:1;
+	USHORT	STBCBeacon:1;
+	USHORT	LsigTxopProt:1;	// L-SIG TXOP protection full support
+	USHORT	PcoActive:1;
+	USHORT	PcoPhase:1;
+	USHORT	rsv:4;
+#endif // RT_BIG_ENDIAN //
+} ADD_HTINFO3, *PADD_HTINFO3;
+
+#define SIZE_ADD_HT_INFO_IE		22
+typedef struct  PACKED{
+	UCHAR				ControlChan;
+	ADD_HTINFO			AddHtInfo;
+	ADD_HTINFO2			AddHtInfo2;
+	ADD_HTINFO3			AddHtInfo3;
+	UCHAR				MCSSet[16];		// Basic MCS set
+} ADD_HT_INFO_IE, *PADD_HT_INFO_IE;
+
+typedef struct  PACKED{
+	UCHAR				NewExtChanOffset;
+} NEW_EXT_CHAN_IE, *PNEW_EXT_CHAN_IE;
+
+
+// 4-byte HTC field.  maybe included in any frame except non-QOS data frame.  The Order bit must set 1.
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    UINT32		RDG:1;	//RDG / More PPDU
+    UINT32		ACConstraint:1;	//feedback request
+    UINT32		rsv:5;  //calibration sequence
+    UINT32		ZLFAnnouce:1;	// ZLF announcement
+    UINT32		CSISTEERING:2;	//CSI/ STEERING
+    UINT32		FBKReq:2;	//feedback request
+    UINT32		CalSeq:2;  //calibration sequence
+    UINT32		CalPos:2;	// calibration position
+    UINT32		MFBorASC:7;	//Link adaptation feedback containing recommended MCS. 0x7f for no feedback or not available
+    UINT32		MFS:3;	//SET to the received value of MRS. 0x111 for unsolicited MFB.
+    UINT32		MRSorASI:3;	// MRQ Sequence identifier. unchanged during entire procedure. 0x000-0x110.
+    UINT32		MRQ:1;	//MCS feedback. Request for a MCS feedback
+    UINT32		TRQ:1;	//sounding request
+    UINT32		MA:1;	//management action payload exist in (QoS Null+HTC)
+#else
+    UINT32		MA:1;	//management action payload exist in (QoS Null+HTC)
+    UINT32		TRQ:1;	//sounding request
+    UINT32		MRQ:1;	//MCS feedback. Request for a MCS feedback
+    UINT32		MRSorASI:3;	// MRQ Sequence identifier. unchanged during entire procedure. 0x000-0x110.
+    UINT32		MFS:3;	//SET to the received value of MRS. 0x111 for unsolicited MFB.
+    UINT32		MFBorASC:7;	//Link adaptation feedback containing recommended MCS. 0x7f for no feedback or not available
+    UINT32		CalPos:2;	// calibration position
+    UINT32		CalSeq:2;  //calibration sequence
+    UINT32		FBKReq:2;	//feedback request
+    UINT32		CSISTEERING:2;	//CSI/ STEERING
+    UINT32		ZLFAnnouce:1;	// ZLF announcement
+    UINT32		rsv:5;  //calibration sequence
+    UINT32		ACConstraint:1;	//feedback request
+    UINT32		RDG:1;	//RDG / More PPDU
+#endif /* !RT_BIG_ENDIAN */
+} HT_CONTROL, *PHT_CONTROL;
+
+// 2-byte QOS CONTROL field
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      Txop_QueueSize:8;
+    USHORT      AMsduPresent:1;
+    USHORT      AckPolicy:2;  //0: normal ACK 1:No ACK 2:scheduled under MTBA/PSMP  3: BA
+    USHORT      EOSP:1;
+    USHORT      TID:4;
+#else
+    USHORT      TID:4;
+    USHORT      EOSP:1;
+    USHORT      AckPolicy:2;  //0: normal ACK 1:No ACK 2:scheduled under MTBA/PSMP  3: BA
+    USHORT      AMsduPresent:1;
+    USHORT      Txop_QueueSize:8;
+#endif /* !RT_BIG_ENDIAN */
+} QOS_CONTROL, *PQOS_CONTROL;
+
+// 2-byte Frame control field
+typedef	struct	PACKED {
+#ifdef RT_BIG_ENDIAN
+	USHORT		Order:1;			// Strict order expected
+	USHORT		Wep:1;				// Wep data
+	USHORT		MoreData:1;			// More data bit
+	USHORT		PwrMgmt:1;			// Power management bit
+	USHORT		Retry:1;			// Retry status bit
+	USHORT		MoreFrag:1;			// More fragment bit
+	USHORT		FrDs:1;				// From DS indication
+	USHORT		ToDs:1;				// To DS indication
+	USHORT		SubType:4;			// MSDU subtype
+	USHORT		Type:2;				// MSDU type
+	USHORT		Ver:2;				// Protocol version
+#else
+	USHORT		Ver:2;				// Protocol version
+	USHORT		Type:2;				// MSDU type
+	USHORT		SubType:4;			// MSDU subtype
+	USHORT		ToDs:1;				// To DS indication
+	USHORT		FrDs:1;				// From DS indication
+	USHORT		MoreFrag:1;			// More fragment bit
+	USHORT		Retry:1;			// Retry status bit
+	USHORT		PwrMgmt:1;			// Power management bit
+	USHORT		MoreData:1;			// More data bit
+	USHORT		Wep:1;				// Wep data
+	USHORT		Order:1;			// Strict order expected
+#endif /* !RT_BIG_ENDIAN */
+} FRAME_CONTROL, *PFRAME_CONTROL;
+
+typedef	struct	PACKED _HEADER_802_11	{
+    FRAME_CONTROL   FC;
+    USHORT          Duration;
+    UCHAR           Addr1[MAC_ADDR_LEN];
+    UCHAR           Addr2[MAC_ADDR_LEN];
+	UCHAR			Addr3[MAC_ADDR_LEN];
+#ifdef RT_BIG_ENDIAN
+	USHORT			Sequence:12;
+	USHORT			Frag:4;
+#else
+	USHORT			Frag:4;
+	USHORT			Sequence:12;
+#endif /* !RT_BIG_ENDIAN */
+	UCHAR			Octet[0];
+}	HEADER_802_11, *PHEADER_802_11;
+
+typedef struct PACKED _FRAME_802_11 {
+    HEADER_802_11   Hdr;
+    UCHAR            Octet[1];
+}   FRAME_802_11, *PFRAME_802_11;
+
+// QoSNull embedding of management action. When HT Control MA field set to 1.
+typedef struct PACKED _MA_BODY {
+    UCHAR            Category;
+    UCHAR            Action;
+    UCHAR            Octet[1];
+}   MA_BODY, *PMA_BODY;
+
+typedef	struct	PACKED _HEADER_802_3	{
+    UCHAR           DAAddr1[MAC_ADDR_LEN];
+    UCHAR           SAAddr2[MAC_ADDR_LEN];
+    UCHAR           Octet[2];
+}	HEADER_802_3, *PHEADER_802_3;
+////Block ACK related format
+// 2-byte BA Parameter  field  in 	DELBA frames to terminate an already set up bA
+typedef struct PACKED{
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;	// value of TC os TS
+    USHORT      Initiator:1;	// 1: originator    0:recipient
+    USHORT      Rsv:11;	// always set to 0
+#else
+    USHORT      Rsv:11;	// always set to 0
+    USHORT      Initiator:1;	// 1: originator    0:recipient
+    USHORT      TID:4;	// value of TC os TS
+#endif /* !RT_BIG_ENDIAN */
+} DELBA_PARM, *PDELBA_PARM;
+
+// 2-byte BA Parameter Set field  in ADDBA frames to signal parm for setting up a BA
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      BufSize:10;	// number of buffe of size 2304 octetsr
+    USHORT      TID:4;	// value of TC os TS
+    USHORT      BAPolicy:1;	// 1: immediately BA    0:delayed BA
+    USHORT      AMSDUSupported:1;	// 0: not permitted		1: permitted
+#else
+    USHORT      AMSDUSupported:1;	// 0: not permitted		1: permitted
+    USHORT      BAPolicy:1;	// 1: immediately BA    0:delayed BA
+    USHORT      TID:4;	// value of TC os TS
+    USHORT      BufSize:10;	// number of buffe of size 2304 octetsr
+#endif /* !RT_BIG_ENDIAN */
+} BA_PARM, *PBA_PARM;
+
+// 2-byte BA Starting Seq CONTROL field
+typedef union PACKED {
+    struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      StartSeq:12;   // sequence number of the 1st MSDU for which this BAR is sent
+	USHORT      FragNum:4;	// always set to 0
+#else
+    USHORT      FragNum:4;	// always set to 0
+	USHORT      StartSeq:12;   // sequence number of the 1st MSDU for which this BAR is sent
+#endif /* RT_BIG_ENDIAN */
+    }   field;
+    USHORT           word;
+} BASEQ_CONTROL, *PBASEQ_CONTROL;
+
+//BAControl and BARControl are the same
+// 2-byte BA CONTROL field in BA frame
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;
+    USHORT      Rsv:9;
+    USHORT      Compressed:1;
+    USHORT      MTID:1;		//EWC V1.24
+    USHORT      ACKPolicy:1; // only related to N-Delayed BA. But not support in RT2860b. 0:NormalACK  1:No ACK
+#else
+    USHORT      ACKPolicy:1; // only related to N-Delayed BA. But not support in RT2860b. 0:NormalACK  1:No ACK
+    USHORT      MTID:1;		//EWC V1.24
+    USHORT      Compressed:1;
+    USHORT      Rsv:9;
+    USHORT      TID:4;
+#endif /* !RT_BIG_ENDIAN */
+} BA_CONTROL, *PBA_CONTROL;
+
+// 2-byte BAR CONTROL field in BAR frame
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;
+    USHORT      Rsv1:9;
+    USHORT      Compressed:1;
+    USHORT      MTID:1;		//if this bit1, use  FRAME_MTBA_REQ,  if 0, use FRAME_BA_REQ
+    USHORT      ACKPolicy:1;
+#else
+    USHORT      ACKPolicy:1; // 0:normal ack,  1:no ack.
+    USHORT      MTID:1;		//if this bit1, use  FRAME_MTBA_REQ,  if 0, use FRAME_BA_REQ
+    USHORT      Compressed:1;
+    USHORT      Rsv1:9;
+    USHORT      TID:4;
+#endif /* !RT_BIG_ENDIAN */
+} BAR_CONTROL, *PBAR_CONTROL;
+
+// BARControl in MTBAR frame
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      NumTID:4;
+    USHORT      Rsv1:9;
+    USHORT      Compressed:1;
+    USHORT      MTID:1;
+    USHORT      ACKPolicy:1;
+#else
+    USHORT      ACKPolicy:1;
+    USHORT      MTID:1;
+    USHORT      Compressed:1;
+    USHORT      Rsv1:9;
+    USHORT      NumTID:4;
+#endif /* !RT_BIG_ENDIAN */
+} MTBAR_CONTROL, *PMTBAR_CONTROL;
+
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;
+    USHORT      Rsv1:12;
+#else
+    USHORT      Rsv1:12;
+    USHORT      TID:4;
+#endif /* !RT_BIG_ENDIAN */
+} PER_TID_INFO, *PPER_TID_INFO;
+
+typedef struct {
+	PER_TID_INFO      PerTID;
+	BASEQ_CONTROL 	 BAStartingSeq;
+} EACH_TID, *PEACH_TID;
+
+
+typedef struct PACKED _PSPOLL_FRAME {
+    FRAME_CONTROL   FC;
+    USHORT          Aid;
+    UCHAR           Bssid[MAC_ADDR_LEN];
+    UCHAR           Ta[MAC_ADDR_LEN];
+}   PSPOLL_FRAME, *PPSPOLL_FRAME;
+
+typedef	struct	PACKED _RTS_FRAME	{
+    FRAME_CONTROL   FC;
+    USHORT          Duration;
+    UCHAR           Addr1[MAC_ADDR_LEN];
+    UCHAR           Addr2[MAC_ADDR_LEN];
+}RTS_FRAME, *PRTS_FRAME;
+
+// BAREQ AND MTBAREQ have the same subtype BAR, 802.11n BAR use compressed bitmap.
+typedef struct PACKED _FRAME_BA_REQ {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BAR_CONTROL  BARControl;
+	BASEQ_CONTROL 	 BAStartingSeq;
+}   FRAME_BA_REQ, *PFRAME_BA_REQ;
+
+typedef struct PACKED _FRAME_MTBA_REQ {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	MTBAR_CONTROL  MTBARControl;
+	PER_TID_INFO	PerTIDInfo;
+	BASEQ_CONTROL 	 BAStartingSeq;
+}   FRAME_MTBA_REQ, *PFRAME_MTBA_REQ;
+
+// Compressed format is mandantory in HT STA
+typedef struct PACKED _FRAME_MTBA {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BA_CONTROL  BAControl;
+	BASEQ_CONTROL 	 BAStartingSeq;
+	UCHAR		BitMap[8];
+}   FRAME_MTBA, *PFRAME_MTBA;
+
+typedef struct PACKED _FRAME_PSMP_ACTION {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Psmp;	// 7.3.1.25
+}   FRAME_PSMP_ACTION, *PFRAME_PSMP_ACTION;
+
+typedef struct PACKED _FRAME_ACTION_HDR {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+}   FRAME_ACTION_HDR, *PFRAME_ACTION_HDR;
+
+//Action Frame
+//Action Frame  Category:Spectrum,  Action:Channel Switch. 7.3.2.20
+typedef struct PACKED _CHAN_SWITCH_ANNOUNCE {
+	UCHAR					ElementID;	// ID = IE_CHANNEL_SWITCH_ANNOUNCEMENT = 37
+	UCHAR					Len;
+	CHA_SWITCH_ANNOUNCE_IE	CSAnnounceIe;
+}   CHAN_SWITCH_ANNOUNCE, *PCHAN_SWITCH_ANNOUNCE;
+
+
+//802.11n : 7.3.2.20a
+typedef struct PACKED _SECOND_CHAN_OFFSET {
+	UCHAR				ElementID;		// ID = IE_SECONDARY_CH_OFFSET = 62
+	UCHAR				Len;
+	SEC_CHA_OFFSET_IE	SecChOffsetIe;
+}   SECOND_CHAN_OFFSET, *PSECOND_CHAN_OFFSET;
+
+
+typedef struct PACKED _FRAME_SPETRUM_CS {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	CHAN_SWITCH_ANNOUNCE	CSAnnounce;
+	SECOND_CHAN_OFFSET		SecondChannel;
+}   FRAME_SPETRUM_CS, *PFRAME_SPETRUM_CS;
+
+
+typedef struct PACKED _FRAME_ADDBA_REQ {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Token;	// 1
+	BA_PARM		BaParm;	      //  2 - 10
+	USHORT		TimeOutValue;	// 0 - 0
+	BASEQ_CONTROL	BaStartSeq; // 0-0
+}   FRAME_ADDBA_REQ, *PFRAME_ADDBA_REQ;
+
+typedef struct PACKED _FRAME_ADDBA_RSP {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Token;
+	USHORT	StatusCode;
+	BA_PARM		BaParm; //0 - 2
+	USHORT		TimeOutValue;
+}   FRAME_ADDBA_RSP, *PFRAME_ADDBA_RSP;
+
+typedef struct PACKED _FRAME_DELBA_REQ {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	DELBA_PARM		DelbaParm;
+	USHORT	ReasonCode;
+}   FRAME_DELBA_REQ, *PFRAME_DELBA_REQ;
+
+
+//7.2.1.7
+typedef struct PACKED _FRAME_BAR {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BAR_CONTROL		BarControl;
+	BASEQ_CONTROL	StartingSeq;
+}   FRAME_BAR, *PFRAME_BAR;
+
+//7.2.1.7
+typedef struct PACKED _FRAME_BA {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BAR_CONTROL		BarControl;
+	BASEQ_CONTROL	StartingSeq;
+	UCHAR		bitmask[8];
+}   FRAME_BA, *PFRAME_BA;
+
+
+// Radio Measuement Request Frame Format
+typedef struct PACKED _FRAME_RM_REQ_ACTION {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Token;
+	USHORT	Repetition;
+	UCHAR   data[0];
+}   FRAME_RM_REQ_ACTION, *PFRAME_RM_REQ_ACTION;
+
+typedef struct PACKED {
+	UCHAR		ID;
+	UCHAR		Length;
+	UCHAR		ChannelSwitchMode;
+	UCHAR		NewRegClass;
+	UCHAR		NewChannelNum;
+	UCHAR		ChannelSwitchCount;
+} HT_EXT_CHANNEL_SWITCH_ANNOUNCEMENT_IE, *PHT_EXT_CHANNEL_SWITCH_ANNOUNCEMENT_IE;
+
+
+//
+// _Limit must be the 2**n - 1
+// _SEQ1 , _SEQ2 must be within 0 ~ _Limit
+//
+#define SEQ_STEPONE(_SEQ1, _SEQ2, _Limit)	((_SEQ1 == ((_SEQ2+1) & _Limit)))
+#define SEQ_SMALLER(_SEQ1, _SEQ2, _Limit)	(((_SEQ1-_SEQ2) & ((_Limit+1)>>1)))
+#define SEQ_LARGER(_SEQ1, _SEQ2, _Limit)	((_SEQ1 != _SEQ2) && !(((_SEQ1-_SEQ2) & ((_Limit+1)>>1))))
+#define SEQ_WITHIN_WIN(_SEQ1, _SEQ2, _WIN, _Limit) (SEQ_LARGER(_SEQ1, _SEQ2, _Limit) &&  \
+												SEQ_SMALLER(_SEQ1, ((_SEQ2+_WIN+1)&_Limit), _Limit))
+
+//
+// Contention-free parameter (without ID and Length)
+//
+typedef struct PACKED {
+    BOOLEAN     bValid;         // 1: variable contains valid value
+    UCHAR       CfpCount;
+    UCHAR       CfpPeriod;
+    USHORT      CfpMaxDuration;
+    USHORT      CfpDurRemaining;
+} CF_PARM, *PCF_PARM;
+
+typedef	struct	_CIPHER_SUITE	{
+	NDIS_802_11_ENCRYPTION_STATUS	PairCipher;		// Unicast cipher 1, this one has more secured cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS	PairCipherAux;	// Unicast cipher 2 if AP announce two unicast cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS	GroupCipher;	// Group cipher
+	USHORT							RsnCapability;	// RSN capability from beacon
+	BOOLEAN							bMixMode;		// Indicate Pair & Group cipher might be different
+}	CIPHER_SUITE, *PCIPHER_SUITE;
+
+// EDCA configuration from AP's BEACON/ProbeRsp
+typedef struct {
+    BOOLEAN     bValid;         // 1: variable contains valid value
+    BOOLEAN     bAdd;         // 1: variable contains valid value
+    BOOLEAN     bQAck;
+    BOOLEAN     bQueueRequest;
+    BOOLEAN     bTxopRequest;
+    BOOLEAN     bAPSDCapable;
+//  BOOLEAN     bMoreDataAck;
+    UCHAR       EdcaUpdateCount;
+    UCHAR       Aifsn[4];       // 0:AC_BK, 1:AC_BE, 2:AC_VI, 3:AC_VO
+    UCHAR       Cwmin[4];
+    UCHAR       Cwmax[4];
+    USHORT      Txop[4];      // in unit of 32-us
+    BOOLEAN     bACM[4];      // 1: Admission Control of AC_BK is mandattory
+} EDCA_PARM, *PEDCA_PARM;
+
+// QBSS LOAD information from QAP's BEACON/ProbeRsp
+typedef struct {
+    BOOLEAN     bValid;                     // 1: variable contains valid value
+    USHORT      StaNum;
+    UCHAR       ChannelUtilization;
+    USHORT      RemainingAdmissionControl;  // in unit of 32-us
+} QBSS_LOAD_PARM, *PQBSS_LOAD_PARM;
+
+// QBSS Info field in QSTA's assoc req
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR		Rsv2:1;
+	UCHAR		MaxSPLength:2;
+	UCHAR		Rsv1:1;
+	UCHAR		UAPSD_AC_BE:1;
+	UCHAR		UAPSD_AC_BK:1;
+	UCHAR		UAPSD_AC_VI:1;
+	UCHAR		UAPSD_AC_VO:1;
+#else
+    UCHAR		UAPSD_AC_VO:1;
+	UCHAR		UAPSD_AC_VI:1;
+	UCHAR		UAPSD_AC_BK:1;
+	UCHAR		UAPSD_AC_BE:1;
+	UCHAR		Rsv1:1;
+	UCHAR		MaxSPLength:2;
+	UCHAR		Rsv2:1;
+#endif /* !RT_BIG_ENDIAN */
+} QBSS_STA_INFO_PARM, *PQBSS_STA_INFO_PARM;
+
+// QBSS Info field in QAP's Beacon/ProbeRsp
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR		UAPSD:1;
+	UCHAR		Rsv:3;
+    UCHAR		ParamSetCount:4;
+#else
+    UCHAR		ParamSetCount:4;
+	UCHAR		Rsv:3;
+	UCHAR		UAPSD:1;
+#endif /* !RT_BIG_ENDIAN */
+} QBSS_AP_INFO_PARM, *PQBSS_AP_INFO_PARM;
+
+// QOS Capability reported in QAP's BEACON/ProbeRsp
+// QOS Capability sent out in QSTA's AssociateReq/ReAssociateReq
+typedef struct {
+    BOOLEAN     bValid;                     // 1: variable contains valid value
+    BOOLEAN     bQAck;
+    BOOLEAN     bQueueRequest;
+    BOOLEAN     bTxopRequest;
+//  BOOLEAN     bMoreDataAck;
+    UCHAR       EdcaUpdateCount;
+} QOS_CAPABILITY_PARM, *PQOS_CAPABILITY_PARM;
+
+#ifdef CONFIG_STA_SUPPORT
+typedef struct {
+    UCHAR       IELen;
+    UCHAR       IE[MAX_CUSTOM_LEN];
+} WPA_IE_;
+#endif // CONFIG_STA_SUPPORT //
+
+
+typedef struct {
+    UCHAR   Bssid[MAC_ADDR_LEN];
+    UCHAR   Channel;
+	UCHAR   CentralChannel;	//Store the wide-band central channel for 40MHz.  .used in 40MHz AP. Or this is the same as Channel.
+    UCHAR   BssType;
+    USHORT  AtimWin;
+    USHORT  BeaconPeriod;
+
+    UCHAR   SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+    UCHAR   SupRateLen;
+    UCHAR   ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+    UCHAR   ExtRateLen;
+	HT_CAPABILITY_IE HtCapability;
+	UCHAR			HtCapabilityLen;
+	ADD_HT_INFO_IE AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChanOffset;
+	CHAR    Rssi;
+    UCHAR   Privacy;			// Indicate security function ON/OFF. Don't mess up with auth mode.
+	UCHAR	Hidden;
+
+    USHORT  DtimPeriod;
+    USHORT  CapabilityInfo;
+
+    USHORT  CfpCount;
+    USHORT  CfpPeriod;
+    USHORT  CfpMaxDuration;
+    USHORT  CfpDurRemaining;
+    UCHAR   SsidLen;
+    CHAR    Ssid[MAX_LEN_OF_SSID];
+
+    ULONG   LastBeaconRxTime; // OS's timestamp
+
+	BOOLEAN	bSES;
+
+	// New for WPA2
+	CIPHER_SUITE					WPA;			// AP announced WPA cipher suite
+	CIPHER_SUITE					WPA2;			// AP announced WPA2 cipher suite
+
+	// New for microsoft WPA support
+	NDIS_802_11_FIXED_IEs	FixIEs;
+	NDIS_802_11_AUTHENTICATION_MODE	AuthModeAux;	// Addition mode for WPA2 / WPA capable AP
+	NDIS_802_11_AUTHENTICATION_MODE	AuthMode;
+	NDIS_802_11_WEP_STATUS	WepStatus;				// Unicast Encryption Algorithm extract from VAR_IE
+	USHORT					VarIELen;				// Length of next VIE include EID & Length
+	UCHAR					VarIEs[MAX_VIE_LEN];
+
+	// CCX Ckip information
+    UCHAR   CkipFlag;
+
+	// CCX 2 TSF
+	UCHAR	PTSF[4];		// Parent TSF
+	UCHAR	TTSF[8];		// Target TSF
+
+    // 802.11e d9, and WMM
+	EDCA_PARM           EdcaParm;
+	QOS_CAPABILITY_PARM QosCapability;
+	QBSS_LOAD_PARM      QbssLoad;
+#ifdef CONFIG_STA_SUPPORT
+    WPA_IE_     WpaIE;
+    WPA_IE_     RsnIE;
+#ifdef EXT_BUILD_CHANNEL_LIST
+	UCHAR		CountryString[3];
+	BOOLEAN		bHasCountryIE;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+} BSS_ENTRY, *PBSS_ENTRY;
+
+typedef struct {
+    UCHAR           BssNr;
+    UCHAR           BssOverlapNr;
+    BSS_ENTRY       BssEntry[MAX_LEN_OF_BSS_TABLE];
+} BSS_TABLE, *PBSS_TABLE;
+
+
+typedef struct _MLME_QUEUE_ELEM {
+    ULONG             Machine;
+    ULONG             MsgType;
+    ULONG             MsgLen;
+    UCHAR             Msg[MGMT_DMA_BUFFER_SIZE];
+    LARGE_INTEGER     TimeStamp;
+    UCHAR             Rssi0;
+    UCHAR             Rssi1;
+    UCHAR             Rssi2;
+    UCHAR             Signal;
+    UCHAR             Channel;
+    UCHAR             Wcid;
+    BOOLEAN           Occupied;
+} MLME_QUEUE_ELEM, *PMLME_QUEUE_ELEM;
+
+typedef struct _MLME_QUEUE {
+    ULONG             Num;
+    ULONG             Head;
+    ULONG             Tail;
+    NDIS_SPIN_LOCK   Lock;
+    MLME_QUEUE_ELEM  Entry[MAX_LEN_OF_MLME_QUEUE];
+} MLME_QUEUE, *PMLME_QUEUE;
+
+typedef VOID (*STATE_MACHINE_FUNC)(VOID *Adaptor, MLME_QUEUE_ELEM *Elem);
+
+typedef struct _STATE_MACHINE {
+    ULONG                           Base;
+    ULONG                           NrState;
+    ULONG                           NrMsg;
+    ULONG                           CurrState;
+    STATE_MACHINE_FUNC             *TransFunc;
+} STATE_MACHINE, *PSTATE_MACHINE;
+
+
+// MLME AUX data structure that hold temporarliy settings during a connection attempt.
+// Once this attemp succeeds, all settings will be copy to pAd->StaActive.
+// A connection attempt (user set OID, roaming, CCX fast roaming,..) consists of
+// several steps (JOIN, AUTH, ASSOC or REASSOC) and may fail at any step. We purposely
+// separate this under-trial settings away from pAd->StaActive so that once
+// this new attempt failed, driver can auto-recover back to the active settings.
+typedef struct _MLME_AUX {
+    UCHAR               BssType;
+    UCHAR               Ssid[MAX_LEN_OF_SSID];
+    UCHAR               SsidLen;
+    UCHAR               Bssid[MAC_ADDR_LEN];
+	UCHAR				AutoReconnectSsid[MAX_LEN_OF_SSID];
+	UCHAR				AutoReconnectSsidLen;
+    USHORT              Alg;
+    UCHAR               ScanType;
+    UCHAR               Channel;
+	UCHAR               CentralChannel;
+    USHORT              Aid;
+    USHORT              CapabilityInfo;
+    USHORT              BeaconPeriod;
+    USHORT              CfpMaxDuration;
+    USHORT              CfpPeriod;
+    USHORT              AtimWin;
+
+	// Copy supported rate from desired AP's beacon. We are trying to match
+	// AP's supported and extended rate settings.
+	UCHAR		        SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		        ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		        SupRateLen;
+	UCHAR		        ExtRateLen;
+	HT_CAPABILITY_IE		HtCapability;
+	UCHAR		        	HtCapabilityLen;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			NewExtChannelOffset;
+	//RT_HT_CAPABILITY	SupportedHtPhy;
+
+    // new for QOS
+    QOS_CAPABILITY_PARM APQosCapability;    // QOS capability of the current associated AP
+    EDCA_PARM           APEdcaParm;         // EDCA parameters of the current associated AP
+    QBSS_LOAD_PARM      APQbssLoad;         // QBSS load of the current associated AP
+
+    // new to keep Ralink specific feature
+    ULONG               APRalinkIe;
+
+    BSS_TABLE           SsidBssTab;     // AP list for the same SSID
+    BSS_TABLE           RoamTab;        // AP list eligible for roaming
+    ULONG               BssIdx;
+    ULONG               RoamIdx;
+
+	BOOLEAN				CurrReqIsFromNdis;
+
+    RALINK_TIMER_STRUCT BeaconTimer, ScanTimer;
+    RALINK_TIMER_STRUCT AuthTimer;
+    RALINK_TIMER_STRUCT AssocTimer, ReassocTimer, DisassocTimer;
+} MLME_AUX, *PMLME_AUX;
+
+typedef struct _MLME_ADDBA_REQ_STRUCT{
+	UCHAR   Wcid;	//
+	UCHAR   pAddr[MAC_ADDR_LEN];
+	UCHAR   BaBufSize;
+	USHORT	TimeOutValue;
+	UCHAR   TID;
+	UCHAR   Token;
+	USHORT	BaStartSeq;
+} MLME_ADDBA_REQ_STRUCT, *PMLME_ADDBA_REQ_STRUCT;
+
+
+typedef struct _MLME_DELBA_REQ_STRUCT{
+	UCHAR   Wcid;	//
+	UCHAR     Addr[MAC_ADDR_LEN];
+	UCHAR   TID;
+	UCHAR	Initiator;
+} MLME_DELBA_REQ_STRUCT, *PMLME_DELBA_REQ_STRUCT;
+
+// assoc struct is equal to reassoc
+typedef struct _MLME_ASSOC_REQ_STRUCT{
+    UCHAR     Addr[MAC_ADDR_LEN];
+    USHORT    CapabilityInfo;
+    USHORT    ListenIntv;
+    ULONG     Timeout;
+} MLME_ASSOC_REQ_STRUCT, *PMLME_ASSOC_REQ_STRUCT, MLME_REASSOC_REQ_STRUCT, *PMLME_REASSOC_REQ_STRUCT;
+
+typedef struct _MLME_DISASSOC_REQ_STRUCT{
+    UCHAR     Addr[MAC_ADDR_LEN];
+    USHORT    Reason;
+} MLME_DISASSOC_REQ_STRUCT, *PMLME_DISASSOC_REQ_STRUCT;
+
+typedef struct _MLME_AUTH_REQ_STRUCT {
+    UCHAR        Addr[MAC_ADDR_LEN];
+    USHORT       Alg;
+    ULONG        Timeout;
+} MLME_AUTH_REQ_STRUCT, *PMLME_AUTH_REQ_STRUCT;
+
+typedef struct _MLME_DEAUTH_REQ_STRUCT {
+    UCHAR        Addr[MAC_ADDR_LEN];
+    USHORT       Reason;
+} MLME_DEAUTH_REQ_STRUCT, *PMLME_DEAUTH_REQ_STRUCT;
+
+typedef struct {
+    ULONG      BssIdx;
+} MLME_JOIN_REQ_STRUCT;
+
+typedef struct _MLME_SCAN_REQ_STRUCT {
+    UCHAR      Bssid[MAC_ADDR_LEN];
+    UCHAR      BssType;
+    UCHAR      ScanType;
+    UCHAR      SsidLen;
+    CHAR       Ssid[MAX_LEN_OF_SSID];
+} MLME_SCAN_REQ_STRUCT, *PMLME_SCAN_REQ_STRUCT;
+
+typedef struct _MLME_START_REQ_STRUCT {
+    CHAR        Ssid[MAX_LEN_OF_SSID];
+    UCHAR       SsidLen;
+} MLME_START_REQ_STRUCT, *PMLME_START_REQ_STRUCT;
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+// structure for DLS
+typedef struct _RT_802_11_DLS {
+	USHORT						TimeOut;		// Use to time out while slience, unit: second , set by UI
+	USHORT						CountDownTimer;	// Use to time out while slience,unit: second , used by driver only
+	NDIS_802_11_MAC_ADDRESS		MacAddr;		// set by UI
+	UCHAR						Status;			// 0: none , 1: wait STAkey, 2: finish DLS setup , set by driver only
+	BOOLEAN						Valid;			// 1: valid , 0: invalid , set by UI, use to setup or tear down DLS link
+	RALINK_TIMER_STRUCT			Timer;			// Use to time out while handshake
+	USHORT						Sequence;
+	USHORT						MacTabMatchWCID;	// ASIC
+	BOOLEAN						bHTCap;
+	PVOID						pAd;
+} RT_802_11_DLS, *PRT_802_11_DLS;
+
+typedef struct _MLME_DLS_REQ_STRUCT {
+    PRT_802_11_DLS	pDLS;
+    USHORT			Reason;
+} MLME_DLS_REQ_STRUCT, *PMLME_DLS_REQ_STRUCT;
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+typedef struct PACKED {
+    UCHAR   Eid;
+    UCHAR   Len;
+    CHAR   Octet[1];
+} EID_STRUCT,*PEID_STRUCT, BEACON_EID_STRUCT, *PBEACON_EID_STRUCT;
+
+typedef struct PACKED _RTMP_TX_RATE_SWITCH
+{
+	UCHAR   ItemNo;
+#ifdef RT_BIG_ENDIAN
+	UCHAR	Rsv2:2;
+	UCHAR	Mode:2;
+	UCHAR	Rsv1:1;
+	UCHAR	BW:1;
+	UCHAR	ShortGI:1;
+	UCHAR	STBC:1;
+#else
+	UCHAR	STBC:1;
+	UCHAR	ShortGI:1;
+	UCHAR	BW:1;
+	UCHAR	Rsv1:1;
+	UCHAR	Mode:2;
+	UCHAR	Rsv2:2;
+#endif
+	UCHAR   CurrMCS;
+	UCHAR   TrainUp;
+	UCHAR   TrainDown;
+} RRTMP_TX_RATE_SWITCH, *PRTMP_TX_RATE_SWITCH;
+
+// ========================== AP mlme.h ===============================
+#define TBTT_PRELOAD_TIME       384        // usec. LomgPreamble + 24-byte at 1Mbps
+#define DEFAULT_DTIM_PERIOD     1
+
+#define MAC_TABLE_AGEOUT_TIME			300			// unit: sec
+#define MAC_TABLE_ASSOC_TIMEOUT			5			// unit: sec
+#define MAC_TABLE_FULL(Tab)				((Tab).size == MAX_LEN_OF_MAC_TABLE)
+
+// AP shall drop the sta if contine Tx fail count reach it.
+#define MAC_ENTRY_LIFE_CHECK_CNT		20			// packet cnt.
+
+// Value domain of pMacEntry->Sst
+typedef enum _Sst {
+    SST_NOT_AUTH,   // 0: equivalent to IEEE 802.11/1999 state 1
+    SST_AUTH,       // 1: equivalent to IEEE 802.11/1999 state 2
+    SST_ASSOC       // 2: equivalent to IEEE 802.11/1999 state 3
+} SST;
+
+// value domain of pMacEntry->AuthState
+typedef enum _AuthState {
+    AS_NOT_AUTH,
+    AS_AUTH_OPEN,       // STA has been authenticated using OPEN SYSTEM
+    AS_AUTH_KEY,        // STA has been authenticated using SHARED KEY
+    AS_AUTHENTICATING   // STA is waiting for AUTH seq#3 using SHARED KEY
+} AUTH_STATE;
+
+//for-wpa value domain of pMacEntry->WpaState  802.1i D3   p.114
+typedef enum _ApWpaState {
+    AS_NOTUSE,              // 0
+    AS_DISCONNECT,          // 1
+    AS_DISCONNECTED,        // 2
+    AS_INITIALIZE,          // 3
+    AS_AUTHENTICATION,      // 4
+    AS_AUTHENTICATION2,     // 5
+    AS_INITPMK,             // 6
+    AS_INITPSK,             // 7
+    AS_PTKSTART,            // 8
+    AS_PTKINIT_NEGOTIATING, // 9
+    AS_PTKINITDONE,         // 10
+    AS_UPDATEKEYS,          // 11
+    AS_INTEGRITY_FAILURE,   // 12
+    AS_KEYUPDATE,           // 13
+} AP_WPA_STATE;
+
+// for-wpa value domain of pMacEntry->WpaState  802.1i D3   p.114
+typedef enum _GTKState {
+    REKEY_NEGOTIATING,
+    REKEY_ESTABLISHED,
+    KEYERROR,
+} GTK_STATE;
+
+//  for-wpa  value domain of pMacEntry->WpaState  802.1i D3   p.114
+typedef enum _WpaGTKState {
+    SETKEYS,
+    SETKEYS_DONE,
+} WPA_GTK_STATE;
+// ====================== end of AP mlme.h ============================
+
+
+#endif	// MLME_H__
diff --git a/drivers/staging/rt2860/oid.h b/drivers/staging/rt2860/oid.h
new file mode 100644
index 000000000000..f2f91b607c47
--- /dev/null
+++ b/drivers/staging/rt2860/oid.h
@@ -0,0 +1,995 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	oid.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#ifndef _OID_H_
+#define _OID_H_
+
+
+#define TRUE				1
+#define FALSE				0
+//
+// IEEE 802.11 Structures and definitions
+//
+#define MAX_TX_POWER_LEVEL              100   /* mW */
+#define MAX_RSSI_TRIGGER                -10    /* dBm */
+#define MIN_RSSI_TRIGGER                -200   /* dBm */
+#define MAX_FRAG_THRESHOLD              2346  /* byte count */
+#define MIN_FRAG_THRESHOLD              256   /* byte count */
+#define MAX_RTS_THRESHOLD               2347  /* byte count */
+
+// new types for Media Specific Indications
+// Extension channel offset
+#define EXTCHA_NONE			0
+#define EXTCHA_ABOVE		0x1
+#define EXTCHA_BELOW		0x3
+
+// BW
+#define BAND_WIDTH_20		0
+#define BAND_WIDTH_40		1
+#define BAND_WIDTH_BOTH		2
+#define BAND_WIDTH_10		3	// 802.11j has 10MHz. This definition is for internal usage. doesn't fill in the IE or other field.
+// SHORTGI
+#define GAP_INTERVAL_400	1	// only support in HT mode
+#define GAP_INTERVAL_800	0
+#define GAP_INTERVAL_BOTH	2
+
+#define NdisMediaStateConnected			1
+#define NdisMediaStateDisconnected		0
+
+#define NDIS_802_11_LENGTH_SSID         32
+#define NDIS_802_11_LENGTH_RATES        8
+#define NDIS_802_11_LENGTH_RATES_EX     16
+#define MAC_ADDR_LENGTH                 6
+#define MAX_NUM_OF_CHS					49 // 14 channels @2.4G +  12@UNII + 4 @MMAC + 11 @HiperLAN2 + 7 @Japan + 1 as NULL terminationc
+#define MAX_NUMBER_OF_EVENT				10  // entry # in EVENT table
+#define MAX_NUMBER_OF_MAC				32 // if MAX_MBSSID_NUM is 8, this value can't be larger than 211
+#define MAX_NUMBER_OF_ACL				64
+#define MAX_LENGTH_OF_SUPPORT_RATES		12    // 1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54
+#define MAX_NUMBER_OF_DLS_ENTRY			4
+
+#ifndef UNDER_CE
+
+#define OID_GEN_MACHINE_NAME               0x0001021A
+
+#ifdef RALINK_ATE
+#define RT_QUERY_ATE_TXDONE_COUNT			0x0401
+#endif // RALINK_ATE //
+#define RT_QUERY_SIGNAL_CONTEXT				0x0402
+#define RT_SET_IAPP_PID                 	0x0404
+#define RT_SET_APD_PID						0x0405
+#define RT_SET_DEL_MAC_ENTRY				0x0406
+
+//
+// IEEE 802.11 OIDs
+//
+#define	OID_GET_SET_TOGGLE			0x8000
+
+#define	OID_802_11_NETWORK_TYPES_SUPPORTED			0x0103
+#define	OID_802_11_NETWORK_TYPE_IN_USE				0x0104
+#define	OID_802_11_RSSI_TRIGGER						0x0107
+#define	RT_OID_802_11_RSSI							0x0108 //rt2860	only , kathy
+#define	RT_OID_802_11_RSSI_1						0x0109 //rt2860	only , kathy
+#define	RT_OID_802_11_RSSI_2						0x010A //rt2860	only , kathy
+#define	OID_802_11_NUMBER_OF_ANTENNAS				0x010B
+#define	OID_802_11_RX_ANTENNA_SELECTED				0x010C
+#define	OID_802_11_TX_ANTENNA_SELECTED				0x010D
+#define	OID_802_11_SUPPORTED_RATES					0x010E
+#define	OID_802_11_ADD_WEP							0x0112
+#define	OID_802_11_REMOVE_WEP						0x0113
+#define	OID_802_11_DISASSOCIATE						0x0114
+#define	OID_802_11_PRIVACY_FILTER					0x0118
+#define	OID_802_11_ASSOCIATION_INFORMATION			0x011E
+#define	OID_802_11_TEST								0x011F
+#define	RT_OID_802_11_COUNTRY_REGION				0x0507
+#define	OID_802_11_BSSID_LIST_SCAN					0x0508
+#define	OID_802_11_SSID								0x0509
+#define	OID_802_11_BSSID							0x050A
+#define	RT_OID_802_11_RADIO							0x050B
+#define	RT_OID_802_11_PHY_MODE						0x050C
+#define	RT_OID_802_11_STA_CONFIG					0x050D
+#define	OID_802_11_DESIRED_RATES					0x050E
+#define	RT_OID_802_11_PREAMBLE						0x050F
+#define	OID_802_11_WEP_STATUS						0x0510
+#define	OID_802_11_AUTHENTICATION_MODE				0x0511
+#define	OID_802_11_INFRASTRUCTURE_MODE				0x0512
+#define	RT_OID_802_11_RESET_COUNTERS				0x0513
+#define	OID_802_11_RTS_THRESHOLD					0x0514
+#define	OID_802_11_FRAGMENTATION_THRESHOLD			0x0515
+#define	OID_802_11_POWER_MODE						0x0516
+#define	OID_802_11_TX_POWER_LEVEL					0x0517
+#define	RT_OID_802_11_ADD_WPA						0x0518
+#define	OID_802_11_REMOVE_KEY						0x0519
+#define	OID_802_11_ADD_KEY							0x0520
+#define	OID_802_11_CONFIGURATION					0x0521
+#define	OID_802_11_TX_PACKET_BURST					0x0522
+#define	RT_OID_802_11_QUERY_NOISE_LEVEL				0x0523
+#define	RT_OID_802_11_EXTRA_INFO					0x0524
+#ifdef	DBG
+#define	RT_OID_802_11_HARDWARE_REGISTER				0x0525
+#endif
+#define OID_802_11_ENCRYPTION_STATUS            OID_802_11_WEP_STATUS
+#define OID_802_11_DEAUTHENTICATION                 0x0526
+#define OID_802_11_DROP_UNENCRYPTED                 0x0527
+#define OID_802_11_MIC_FAILURE_REPORT_FRAME         0x0528
+
+// For 802.1x daemin using to require current driver configuration
+#define OID_802_11_RADIUS_QUERY_SETTING				0x0540
+
+#define	RT_OID_DEVICE_NAME							0x0607
+#define	RT_OID_VERSION_INFO							0x0608
+#define	OID_802_11_BSSID_LIST						0x0609
+#define	OID_802_3_CURRENT_ADDRESS					0x060A
+#define	OID_GEN_MEDIA_CONNECT_STATUS				0x060B
+#define	RT_OID_802_11_QUERY_LINK_STATUS				0x060C
+#define	OID_802_11_RSSI								0x060D
+#define	OID_802_11_STATISTICS						0x060E
+#define	OID_GEN_RCV_OK								0x060F
+#define	OID_GEN_RCV_NO_BUFFER						0x0610
+#define	RT_OID_802_11_QUERY_EEPROM_VERSION			0x0611
+#define	RT_OID_802_11_QUERY_FIRMWARE_VERSION		0x0612
+#define	RT_OID_802_11_QUERY_LAST_RX_RATE			0x0613
+#define	RT_OID_802_11_TX_POWER_LEVEL_1				0x0614
+#define	RT_OID_802_11_QUERY_PIDVID					0x0615
+//for WPA_SUPPLICANT_SUPPORT
+#define OID_SET_COUNTERMEASURES                     0x0616
+#define OID_802_11_SET_IEEE8021X                    0x0617
+#define OID_802_11_SET_IEEE8021X_REQUIRE_KEY        0x0618
+#define OID_802_11_PMKID                            0x0620
+#define RT_OID_WPA_SUPPLICANT_SUPPORT               0x0621
+#define RT_OID_WE_VERSION_COMPILED                  0x0622
+#define RT_OID_NEW_DRIVER                           0x0623
+
+
+//rt2860 , kathy
+#define	RT_OID_802_11_SNR_0							0x0630
+#define	RT_OID_802_11_SNR_1							0x0631
+#define	RT_OID_802_11_QUERY_LAST_TX_RATE			0x0632
+#define	RT_OID_802_11_QUERY_HT_PHYMODE				0x0633
+#define	RT_OID_802_11_SET_HT_PHYMODE				0x0634
+#define	OID_802_11_RELOAD_DEFAULTS					0x0635
+#define	RT_OID_802_11_QUERY_APSD_SETTING			0x0636
+#define	RT_OID_802_11_SET_APSD_SETTING				0x0637
+#define	RT_OID_802_11_QUERY_APSD_PSM				0x0638
+#define	RT_OID_802_11_SET_APSD_PSM					0x0639
+#define	RT_OID_802_11_QUERY_DLS						0x063A
+#define	RT_OID_802_11_SET_DLS						0x063B
+#define	RT_OID_802_11_QUERY_DLS_PARAM				0x063C
+#define	RT_OID_802_11_SET_DLS_PARAM					0x063D
+#define RT_OID_802_11_QUERY_WMM              		0x063E
+#define RT_OID_802_11_SET_WMM      					0x063F
+#define RT_OID_802_11_QUERY_IMME_BA_CAP				0x0640
+#define RT_OID_802_11_SET_IMME_BA_CAP				0x0641
+#define RT_OID_802_11_QUERY_BATABLE					0x0642
+#define RT_OID_802_11_ADD_IMME_BA					0x0643
+#define RT_OID_802_11_TEAR_IMME_BA					0x0644
+#define RT_OID_DRIVER_DEVICE_NAME                   0x0645
+#define RT_OID_802_11_QUERY_DAT_HT_PHYMODE          0x0646
+#define RT_OID_QUERY_MULTIPLE_CARD_SUPPORT          0x0647
+
+// Ralink defined OIDs
+// Dennis Lee move to platform specific
+
+#define	RT_OID_802_11_BSSID					  (OID_GET_SET_TOGGLE |	OID_802_11_BSSID)
+#define	RT_OID_802_11_SSID					  (OID_GET_SET_TOGGLE |	OID_802_11_SSID)
+#define	RT_OID_802_11_INFRASTRUCTURE_MODE	  (OID_GET_SET_TOGGLE |	OID_802_11_INFRASTRUCTURE_MODE)
+#define	RT_OID_802_11_ADD_WEP				  (OID_GET_SET_TOGGLE |	OID_802_11_ADD_WEP)
+#define	RT_OID_802_11_ADD_KEY				  (OID_GET_SET_TOGGLE |	OID_802_11_ADD_KEY)
+#define	RT_OID_802_11_REMOVE_WEP			  (OID_GET_SET_TOGGLE |	OID_802_11_REMOVE_WEP)
+#define	RT_OID_802_11_REMOVE_KEY			  (OID_GET_SET_TOGGLE |	OID_802_11_REMOVE_KEY)
+#define	RT_OID_802_11_DISASSOCIATE			  (OID_GET_SET_TOGGLE |	OID_802_11_DISASSOCIATE)
+#define	RT_OID_802_11_AUTHENTICATION_MODE	  (OID_GET_SET_TOGGLE |	OID_802_11_AUTHENTICATION_MODE)
+#define	RT_OID_802_11_PRIVACY_FILTER		  (OID_GET_SET_TOGGLE |	OID_802_11_PRIVACY_FILTER)
+#define	RT_OID_802_11_BSSID_LIST_SCAN		  (OID_GET_SET_TOGGLE |	OID_802_11_BSSID_LIST_SCAN)
+#define	RT_OID_802_11_WEP_STATUS			  (OID_GET_SET_TOGGLE |	OID_802_11_WEP_STATUS)
+#define	RT_OID_802_11_RELOAD_DEFAULTS		  (OID_GET_SET_TOGGLE |	OID_802_11_RELOAD_DEFAULTS)
+#define	RT_OID_802_11_NETWORK_TYPE_IN_USE	  (OID_GET_SET_TOGGLE |	OID_802_11_NETWORK_TYPE_IN_USE)
+#define	RT_OID_802_11_TX_POWER_LEVEL		  (OID_GET_SET_TOGGLE |	OID_802_11_TX_POWER_LEVEL)
+#define	RT_OID_802_11_RSSI_TRIGGER			  (OID_GET_SET_TOGGLE |	OID_802_11_RSSI_TRIGGER)
+#define	RT_OID_802_11_FRAGMENTATION_THRESHOLD (OID_GET_SET_TOGGLE |	OID_802_11_FRAGMENTATION_THRESHOLD)
+#define	RT_OID_802_11_RTS_THRESHOLD			  (OID_GET_SET_TOGGLE |	OID_802_11_RTS_THRESHOLD)
+#define	RT_OID_802_11_RX_ANTENNA_SELECTED	  (OID_GET_SET_TOGGLE |	OID_802_11_RX_ANTENNA_SELECTED)
+#define	RT_OID_802_11_TX_ANTENNA_SELECTED	  (OID_GET_SET_TOGGLE |	OID_802_11_TX_ANTENNA_SELECTED)
+#define	RT_OID_802_11_SUPPORTED_RATES		  (OID_GET_SET_TOGGLE |	OID_802_11_SUPPORTED_RATES)
+#define	RT_OID_802_11_DESIRED_RATES			  (OID_GET_SET_TOGGLE |	OID_802_11_DESIRED_RATES)
+#define	RT_OID_802_11_CONFIGURATION			  (OID_GET_SET_TOGGLE |	OID_802_11_CONFIGURATION)
+#define	RT_OID_802_11_POWER_MODE			  (OID_GET_SET_TOGGLE |	OID_802_11_POWER_MODE)
+
+typedef enum _NDIS_802_11_STATUS_TYPE
+{
+    Ndis802_11StatusType_Authentication,
+    Ndis802_11StatusType_MediaStreamMode,
+    Ndis802_11StatusType_PMKID_CandidateList,
+    Ndis802_11StatusTypeMax    // not a real type, defined as an upper bound
+} NDIS_802_11_STATUS_TYPE, *PNDIS_802_11_STATUS_TYPE;
+
+typedef UCHAR   NDIS_802_11_MAC_ADDRESS[6];
+
+typedef struct _NDIS_802_11_STATUS_INDICATION
+{
+    NDIS_802_11_STATUS_TYPE StatusType;
+} NDIS_802_11_STATUS_INDICATION, *PNDIS_802_11_STATUS_INDICATION;
+
+// 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
+
+typedef struct _NDIS_802_11_AUTHENTICATION_REQUEST
+{
+    ULONG Length;            // Length of structure
+    NDIS_802_11_MAC_ADDRESS Bssid;
+    ULONG Flags;
+} NDIS_802_11_AUTHENTICATION_REQUEST, *PNDIS_802_11_AUTHENTICATION_REQUEST;
+
+//Added new types for PMKID Candidate lists.
+typedef struct _PMKID_CANDIDATE {
+    NDIS_802_11_MAC_ADDRESS BSSID;
+    ULONG Flags;
+} PMKID_CANDIDATE, *PPMKID_CANDIDATE;
+
+typedef struct _NDIS_802_11_PMKID_CANDIDATE_LIST
+{
+    ULONG Version;       // Version of the structure
+    ULONG NumCandidates; // No. of pmkid candidates
+    PMKID_CANDIDATE CandidateList[1];
+} NDIS_802_11_PMKID_CANDIDATE_LIST, *PNDIS_802_11_PMKID_CANDIDATE_LIST;
+
+//Flags for PMKID Candidate list structure
+#define NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED	0x01
+
+// Added new types for OFDM 5G and 2.4G
+typedef enum _NDIS_802_11_NETWORK_TYPE
+{
+   Ndis802_11FH,
+   Ndis802_11DS,
+    Ndis802_11OFDM5,
+    Ndis802_11OFDM5_N,
+    Ndis802_11OFDM24,
+    Ndis802_11OFDM24_N,
+   Ndis802_11Automode,
+    Ndis802_11NetworkTypeMax    // not a real type, defined as an upper bound
+} NDIS_802_11_NETWORK_TYPE, *PNDIS_802_11_NETWORK_TYPE;
+
+typedef struct _NDIS_802_11_NETWORK_TYPE_LIST
+{
+    UINT                       NumberOfItems;  // in list below, at least 1
+   NDIS_802_11_NETWORK_TYPE    NetworkType [1];
+} NDIS_802_11_NETWORK_TYPE_LIST, *PNDIS_802_11_NETWORK_TYPE_LIST;
+
+typedef enum _NDIS_802_11_POWER_MODE
+{
+    Ndis802_11PowerModeCAM,
+    Ndis802_11PowerModeMAX_PSP,
+    Ndis802_11PowerModeFast_PSP,
+    Ndis802_11PowerModeLegacy_PSP,
+    Ndis802_11PowerModeMax      // not a real mode, defined as an upper bound
+} NDIS_802_11_POWER_MODE, *PNDIS_802_11_POWER_MODE;
+
+typedef ULONG   NDIS_802_11_TX_POWER_LEVEL; // in milliwatts
+
+//
+// Received Signal Strength Indication
+//
+typedef LONG    NDIS_802_11_RSSI;           // in dBm
+
+typedef struct _NDIS_802_11_CONFIGURATION_FH
+{
+   ULONG           Length;            // Length of structure
+   ULONG           HopPattern;        // As defined by 802.11, MSB set
+   ULONG           HopSet;            // to one if non-802.11
+   ULONG           DwellTime;         // units are Kusec
+} NDIS_802_11_CONFIGURATION_FH, *PNDIS_802_11_CONFIGURATION_FH;
+
+typedef struct _NDIS_802_11_CONFIGURATION
+{
+   ULONG                           Length;             // Length of structure
+   ULONG                           BeaconPeriod;       // units are Kusec
+   ULONG                           ATIMWindow;         // units are Kusec
+   ULONG                           DSConfig;           // Frequency, units are kHz
+   NDIS_802_11_CONFIGURATION_FH    FHConfig;
+} NDIS_802_11_CONFIGURATION, *PNDIS_802_11_CONFIGURATION;
+
+typedef struct _NDIS_802_11_STATISTICS
+{
+   ULONG           Length;             // Length of structure
+   LARGE_INTEGER   TransmittedFragmentCount;
+   LARGE_INTEGER   MulticastTransmittedFrameCount;
+   LARGE_INTEGER   FailedCount;
+   LARGE_INTEGER   RetryCount;
+   LARGE_INTEGER   MultipleRetryCount;
+   LARGE_INTEGER   RTSSuccessCount;
+   LARGE_INTEGER   RTSFailureCount;
+   LARGE_INTEGER   ACKFailureCount;
+   LARGE_INTEGER   FrameDuplicateCount;
+   LARGE_INTEGER   ReceivedFragmentCount;
+   LARGE_INTEGER   MulticastReceivedFrameCount;
+   LARGE_INTEGER   FCSErrorCount;
+   LARGE_INTEGER   TKIPLocalMICFailures;
+   LARGE_INTEGER   TKIPRemoteMICErrors;
+   LARGE_INTEGER   TKIPICVErrors;
+   LARGE_INTEGER   TKIPCounterMeasuresInvoked;
+   LARGE_INTEGER   TKIPReplays;
+   LARGE_INTEGER   CCMPFormatErrors;
+   LARGE_INTEGER   CCMPReplays;
+   LARGE_INTEGER   CCMPDecryptErrors;
+   LARGE_INTEGER   FourWayHandshakeFailures;
+} NDIS_802_11_STATISTICS, *PNDIS_802_11_STATISTICS;
+
+typedef  ULONG  NDIS_802_11_KEY_INDEX;
+typedef ULONGLONG   NDIS_802_11_KEY_RSC;
+
+#define MAX_RADIUS_SRV_NUM			2	  // 802.1x failover number
+
+typedef struct PACKED _RADIUS_SRV_INFO {
+	UINT32			radius_ip;
+	UINT32			radius_port;
+	UCHAR			radius_key[64];
+	UCHAR			radius_key_len;
+} RADIUS_SRV_INFO, *PRADIUS_SRV_INFO;
+
+typedef struct PACKED _RADIUS_KEY_INFO
+{
+	UCHAR			radius_srv_num;
+	RADIUS_SRV_INFO	radius_srv_info[MAX_RADIUS_SRV_NUM];
+	UCHAR			ieee8021xWEP;		 // dynamic WEP
+    UCHAR           key_index;
+    UCHAR           key_length;          // length of key in bytes
+    UCHAR           key_material[13];
+} RADIUS_KEY_INFO, *PRADIUS_KEY_INFO;
+
+// It's used by 802.1x daemon to require relative configuration
+typedef struct PACKED _RADIUS_CONF
+{
+    UINT32          Length;             // Length of this structure
+    UCHAR			mbss_num;			// indicate multiple BSS number
+	UINT32			own_ip_addr;
+	UINT32			retry_interval;
+	UINT32			session_timeout_interval;
+	UCHAR			EAPifname[IFNAMSIZ];
+	UCHAR			EAPifname_len;
+	UCHAR 			PreAuthifname[IFNAMSIZ];
+	UCHAR			PreAuthifname_len;
+	RADIUS_KEY_INFO	RadiusInfo[8/*MAX_MBSSID_NUM*/];
+} RADIUS_CONF, *PRADIUS_CONF;
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+// Key mapping keys require a BSSID
+typedef struct _NDIS_802_11_KEY
+{
+    UINT           Length;             // Length of this structure
+    UINT           KeyIndex;
+    UINT           KeyLength;          // length of key in bytes
+    NDIS_802_11_MAC_ADDRESS BSSID;
+    NDIS_802_11_KEY_RSC KeyRSC;
+    UCHAR           KeyMaterial[1];     // variable length depending on above field
+} NDIS_802_11_KEY, *PNDIS_802_11_KEY;
+#endif // CONFIG_STA_SUPPORT //
+
+typedef struct _NDIS_802_11_REMOVE_KEY
+{
+    UINT           Length;             // Length of this structure
+    UINT           KeyIndex;
+    NDIS_802_11_MAC_ADDRESS BSSID;
+} NDIS_802_11_REMOVE_KEY, *PNDIS_802_11_REMOVE_KEY;
+
+typedef struct _NDIS_802_11_WEP
+{
+   UINT     Length;        // Length of this structure
+   UINT     KeyIndex;           // 0 is the per-client key, 1-N are the
+                                        // global keys
+   UINT     KeyLength;     // length of key in bytes
+   UCHAR     KeyMaterial[1];// variable length depending on above field
+} NDIS_802_11_WEP, *PNDIS_802_11_WEP;
+
+
+typedef enum _NDIS_802_11_NETWORK_INFRASTRUCTURE
+{
+   Ndis802_11IBSS,
+   Ndis802_11Infrastructure,
+   Ndis802_11AutoUnknown,
+   Ndis802_11Monitor,
+   Ndis802_11InfrastructureMax     // Not a real value, defined as upper bound
+} NDIS_802_11_NETWORK_INFRASTRUCTURE, *PNDIS_802_11_NETWORK_INFRASTRUCTURE;
+
+// Add new authentication modes
+typedef enum _NDIS_802_11_AUTHENTICATION_MODE
+{
+   Ndis802_11AuthModeOpen,
+   Ndis802_11AuthModeShared,
+   Ndis802_11AuthModeAutoSwitch,
+    Ndis802_11AuthModeWPA,
+    Ndis802_11AuthModeWPAPSK,
+    Ndis802_11AuthModeWPANone,
+   Ndis802_11AuthModeWPA2,
+   Ndis802_11AuthModeWPA2PSK,
+   	Ndis802_11AuthModeWPA1WPA2,
+	Ndis802_11AuthModeWPA1PSKWPA2PSK,
+   Ndis802_11AuthModeMax           // Not a real mode, defined as upper bound
+} NDIS_802_11_AUTHENTICATION_MODE, *PNDIS_802_11_AUTHENTICATION_MODE;
+
+typedef UCHAR   NDIS_802_11_RATES[NDIS_802_11_LENGTH_RATES];        // Set of 8 data rates
+typedef UCHAR   NDIS_802_11_RATES_EX[NDIS_802_11_LENGTH_RATES_EX];  // Set of 16 data rates
+
+typedef struct PACKED _NDIS_802_11_SSID
+{
+    UINT   SsidLength;         // length of SSID field below, in bytes;
+                                // this can be zero.
+    UCHAR   Ssid[NDIS_802_11_LENGTH_SSID];           // SSID information field
+} NDIS_802_11_SSID, *PNDIS_802_11_SSID;
+
+
+typedef struct PACKED _NDIS_WLAN_BSSID
+{
+   ULONG                               Length;     // Length of this structure
+   NDIS_802_11_MAC_ADDRESS             MacAddress; // BSSID
+   UCHAR                               Reserved[2];
+   NDIS_802_11_SSID                    Ssid;       // SSID
+   ULONG                               Privacy;    // WEP encryption requirement
+   NDIS_802_11_RSSI                    Rssi;       // receive signal strength in dBm
+   NDIS_802_11_NETWORK_TYPE            NetworkTypeInUse;
+   NDIS_802_11_CONFIGURATION           Configuration;
+   NDIS_802_11_NETWORK_INFRASTRUCTURE  InfrastructureMode;
+   NDIS_802_11_RATES                   SupportedRates;
+} NDIS_WLAN_BSSID, *PNDIS_WLAN_BSSID;
+
+typedef struct PACKED _NDIS_802_11_BSSID_LIST
+{
+   UINT           NumberOfItems;      // in list below, at least 1
+   NDIS_WLAN_BSSID Bssid[1];
+} NDIS_802_11_BSSID_LIST, *PNDIS_802_11_BSSID_LIST;
+
+// Added Capabilities, IELength and IEs for each BSSID
+typedef struct PACKED _NDIS_WLAN_BSSID_EX
+{
+    ULONG                               Length;             // Length of this structure
+    NDIS_802_11_MAC_ADDRESS             MacAddress;         // BSSID
+    UCHAR                               Reserved[2];
+    NDIS_802_11_SSID                    Ssid;               // SSID
+    UINT                                Privacy;            // WEP encryption requirement
+    NDIS_802_11_RSSI                    Rssi;               // receive signal
+                                                            // strength in dBm
+    NDIS_802_11_NETWORK_TYPE            NetworkTypeInUse;
+    NDIS_802_11_CONFIGURATION           Configuration;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  InfrastructureMode;
+    NDIS_802_11_RATES_EX                SupportedRates;
+    ULONG                               IELength;
+    UCHAR                               IEs[1];
+} NDIS_WLAN_BSSID_EX, *PNDIS_WLAN_BSSID_EX;
+
+typedef struct PACKED _NDIS_802_11_BSSID_LIST_EX
+{
+    UINT                   NumberOfItems;      // in list below, at least 1
+    NDIS_WLAN_BSSID_EX      Bssid[1];
+} NDIS_802_11_BSSID_LIST_EX, *PNDIS_802_11_BSSID_LIST_EX;
+
+typedef struct PACKED _NDIS_802_11_FIXED_IEs
+{
+    UCHAR Timestamp[8];
+    USHORT BeaconInterval;
+    USHORT Capabilities;
+} NDIS_802_11_FIXED_IEs, *PNDIS_802_11_FIXED_IEs;
+
+typedef struct _NDIS_802_11_VARIABLE_IEs
+{
+    UCHAR ElementID;
+    UCHAR Length;    // Number of bytes in data field
+    UCHAR data[1];
+} NDIS_802_11_VARIABLE_IEs, *PNDIS_802_11_VARIABLE_IEs;
+
+typedef  ULONG   NDIS_802_11_FRAGMENTATION_THRESHOLD;
+
+typedef  ULONG   NDIS_802_11_RTS_THRESHOLD;
+
+typedef  ULONG   NDIS_802_11_ANTENNA;
+
+typedef enum _NDIS_802_11_PRIVACY_FILTER
+{
+   Ndis802_11PrivFilterAcceptAll,
+   Ndis802_11PrivFilter8021xWEP
+} NDIS_802_11_PRIVACY_FILTER, *PNDIS_802_11_PRIVACY_FILTER;
+
+// Added new encryption types
+// Also aliased typedef to new name
+typedef enum _NDIS_802_11_WEP_STATUS
+{
+   Ndis802_11WEPEnabled,
+    Ndis802_11Encryption1Enabled = Ndis802_11WEPEnabled,
+   Ndis802_11WEPDisabled,
+    Ndis802_11EncryptionDisabled = Ndis802_11WEPDisabled,
+   Ndis802_11WEPKeyAbsent,
+    Ndis802_11Encryption1KeyAbsent = Ndis802_11WEPKeyAbsent,
+   Ndis802_11WEPNotSupported,
+    Ndis802_11EncryptionNotSupported = Ndis802_11WEPNotSupported,
+    Ndis802_11Encryption2Enabled,
+    Ndis802_11Encryption2KeyAbsent,
+    Ndis802_11Encryption3Enabled,
+    Ndis802_11Encryption3KeyAbsent,
+    Ndis802_11Encryption4Enabled,	// TKIP or AES mix
+    Ndis802_11Encryption4KeyAbsent,
+} NDIS_802_11_WEP_STATUS, *PNDIS_802_11_WEP_STATUS,
+  NDIS_802_11_ENCRYPTION_STATUS, *PNDIS_802_11_ENCRYPTION_STATUS;
+
+typedef enum _NDIS_802_11_RELOAD_DEFAULTS
+{
+   Ndis802_11ReloadWEPKeys
+} NDIS_802_11_RELOAD_DEFAULTS, *PNDIS_802_11_RELOAD_DEFAULTS;
+
+#define NDIS_802_11_AI_REQFI_CAPABILITIES      1
+#define NDIS_802_11_AI_REQFI_LISTENINTERVAL    2
+#define NDIS_802_11_AI_REQFI_CURRENTAPADDRESS  4
+
+#define NDIS_802_11_AI_RESFI_CAPABILITIES      1
+#define NDIS_802_11_AI_RESFI_STATUSCODE        2
+#define NDIS_802_11_AI_RESFI_ASSOCIATIONID     4
+
+typedef struct _NDIS_802_11_AI_REQFI
+{
+    USHORT Capabilities;
+    USHORT ListenInterval;
+    NDIS_802_11_MAC_ADDRESS  CurrentAPAddress;
+} NDIS_802_11_AI_REQFI, *PNDIS_802_11_AI_REQFI;
+
+typedef struct _NDIS_802_11_AI_RESFI
+{
+    USHORT Capabilities;
+    USHORT StatusCode;
+    USHORT AssociationId;
+} NDIS_802_11_AI_RESFI, *PNDIS_802_11_AI_RESFI;
+
+typedef struct _NDIS_802_11_ASSOCIATION_INFORMATION
+{
+    ULONG                   Length;
+    USHORT                  AvailableRequestFixedIEs;
+    NDIS_802_11_AI_REQFI    RequestFixedIEs;
+    ULONG                   RequestIELength;
+    ULONG                   OffsetRequestIEs;
+    USHORT                  AvailableResponseFixedIEs;
+    NDIS_802_11_AI_RESFI    ResponseFixedIEs;
+    ULONG                   ResponseIELength;
+    ULONG                   OffsetResponseIEs;
+} NDIS_802_11_ASSOCIATION_INFORMATION, *PNDIS_802_11_ASSOCIATION_INFORMATION;
+
+typedef struct _NDIS_802_11_AUTHENTICATION_EVENT
+{
+    NDIS_802_11_STATUS_INDICATION       Status;
+    NDIS_802_11_AUTHENTICATION_REQUEST  Request[1];
+} NDIS_802_11_AUTHENTICATION_EVENT, *PNDIS_802_11_AUTHENTICATION_EVENT;
+
+// 802.11 Media stream constraints, associated with OID_802_11_MEDIA_STREAM_MODE
+typedef enum _NDIS_802_11_MEDIA_STREAM_MODE
+{
+    Ndis802_11MediaStreamOff,
+    Ndis802_11MediaStreamOn,
+} NDIS_802_11_MEDIA_STREAM_MODE, *PNDIS_802_11_MEDIA_STREAM_MODE;
+
+// PMKID Structures
+typedef UCHAR   NDIS_802_11_PMKID_VALUE[16];
+
+#ifdef CONFIG_STA_SUPPORT
+typedef struct _BSSID_INFO
+{
+    NDIS_802_11_MAC_ADDRESS BSSID;
+    NDIS_802_11_PMKID_VALUE PMKID;
+} BSSID_INFO, *PBSSID_INFO;
+
+typedef struct _NDIS_802_11_PMKID
+{
+    UINT    Length;
+    UINT    BSSIDInfoCount;
+    BSSID_INFO BSSIDInfo[1];
+} NDIS_802_11_PMKID, *PNDIS_802_11_PMKID;
+#endif // CONFIG_STA_SUPPORT //
+
+
+typedef struct _NDIS_802_11_AUTHENTICATION_ENCRYPTION
+{
+    NDIS_802_11_AUTHENTICATION_MODE AuthModeSupported;
+    NDIS_802_11_ENCRYPTION_STATUS EncryptStatusSupported;
+} NDIS_802_11_AUTHENTICATION_ENCRYPTION, *PNDIS_802_11_AUTHENTICATION_ENCRYPTION;
+
+typedef struct _NDIS_802_11_CAPABILITY
+{
+     ULONG Length;
+     ULONG Version;
+     ULONG NoOfPMKIDs;
+     ULONG NoOfAuthEncryptPairsSupported;
+     NDIS_802_11_AUTHENTICATION_ENCRYPTION AuthenticationEncryptionSupported[1];
+} NDIS_802_11_CAPABILITY, *PNDIS_802_11_CAPABILITY;
+
+//#endif //of WIN 2k
+#endif //UNDER_CE
+
+#if WIRELESS_EXT <= 11
+#ifndef SIOCDEVPRIVATE
+#define SIOCDEVPRIVATE                              0x8BE0
+#endif
+#define SIOCIWFIRSTPRIV								SIOCDEVPRIVATE
+#endif
+
+#ifdef CONFIG_STA_SUPPORT
+#define RTPRIV_IOCTL_SET							(SIOCIWFIRSTPRIV + 0x02)
+
+#ifdef DBG
+#define RTPRIV_IOCTL_BBP                            (SIOCIWFIRSTPRIV + 0x03)
+#define RTPRIV_IOCTL_MAC                            (SIOCIWFIRSTPRIV + 0x05)
+#define RTPRIV_IOCTL_E2P                            (SIOCIWFIRSTPRIV + 0x07)
+#endif
+
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+#define RTPRIV_IOCTL_ATE							(SIOCIWFIRSTPRIV + 0x08)
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#define RTPRIV_IOCTL_STATISTICS                     (SIOCIWFIRSTPRIV + 0x09)
+#define RTPRIV_IOCTL_ADD_PMKID_CACHE                (SIOCIWFIRSTPRIV + 0x0A)
+#define RTPRIV_IOCTL_RADIUS_DATA                    (SIOCIWFIRSTPRIV + 0x0C)
+#define RTPRIV_IOCTL_GSITESURVEY					(SIOCIWFIRSTPRIV + 0x0D)
+#define RT_PRIV_IOCTL								(SIOCIWFIRSTPRIV + 0x0E) // Sync. with RT61 (for wpa_supplicant)
+#define RTPRIV_IOCTL_GET_MAC_TABLE					(SIOCIWFIRSTPRIV + 0x0F)
+
+#define RTPRIV_IOCTL_SHOW							(SIOCIWFIRSTPRIV + 0x11)
+enum {
+    SHOW_CONN_STATUS = 4,
+    SHOW_DRVIER_VERION = 5,
+    SHOW_BA_INFO = 6,
+	SHOW_DESC_INFO = 7,
+    RAIO_OFF = 10,
+    RAIO_ON = 11,
+#ifdef QOS_DLS_SUPPORT
+	SHOW_DLS_ENTRY_INFO = 19,
+#endif // QOS_DLS_SUPPORT //
+	SHOW_CFG_VALUE = 20,
+};
+
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef SNMP_SUPPORT
+//SNMP ieee 802dot11, kathy , 2008_0220
+// dot11res(3)
+#define RT_OID_802_11_MANUFACTUREROUI			0x0700
+#define RT_OID_802_11_MANUFACTURERNAME			0x0701
+#define RT_OID_802_11_RESOURCETYPEIDNAME		0x0702
+
+// dot11smt(1)
+#define RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED	0x0703
+#define RT_OID_802_11_POWERMANAGEMENTMODE		0x0704
+#define OID_802_11_WEPDEFAULTKEYVALUE			0x0705 // read , write
+#define OID_802_11_WEPDEFAULTKEYID				0x0706
+#define RT_OID_802_11_WEPKEYMAPPINGLENGTH		0x0707
+#define OID_802_11_SHORTRETRYLIMIT				0x0708
+#define OID_802_11_LONGRETRYLIMIT				0x0709
+#define RT_OID_802_11_PRODUCTID					0x0710
+#define RT_OID_802_11_MANUFACTUREID				0x0711
+
+// //dot11Phy(4)
+#define OID_802_11_CURRENTCHANNEL				0x0712
+
+//dot11mac
+#define RT_OID_802_11_MAC_ADDRESS				0x0713
+#endif // SNMP_SUPPORT //
+
+#define OID_802_11_BUILD_CHANNEL_EX				0x0714
+#define OID_802_11_GET_CH_LIST					0x0715
+#define OID_802_11_GET_COUNTRY_CODE				0x0716
+#define OID_802_11_GET_CHANNEL_GEOGRAPHY		0x0717
+
+#ifdef LLTD_SUPPORT
+// for consistency with RT61
+#define RT_OID_GET_PHY_MODE                         0x761
+#endif // LLTD_SUPPORT //
+
+// New for MeetingHouse Api support
+#define OID_MH_802_1X_SUPPORTED               0xFFEDC100
+
+// MIMO Tx parameter, ShortGI, MCS, STBC, etc.  these are fields in TXWI. Don't change this definition!!!
+typedef union  _HTTRANSMIT_SETTING {
+#ifdef RT_BIG_ENDIAN
+	struct	{
+	USHORT		MODE:2;	// Use definition MODE_xxx.
+	USHORT		TxBF:1;
+	USHORT		rsv:2;
+	USHORT		STBC:2;	//SPACE
+	USHORT		ShortGI:1;
+	USHORT		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	USHORT   	MCS:7;                 // MCS
+	}	field;
+#else
+	struct	{
+	USHORT   	MCS:7;                 // MCS
+	USHORT		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	USHORT		ShortGI:1;
+	USHORT		STBC:2;	//SPACE
+	USHORT		rsv:2;
+	USHORT		TxBF:1;
+	USHORT		MODE:2;	// Use definition MODE_xxx.
+	}	field;
+#endif
+	USHORT		word;
+ } HTTRANSMIT_SETTING, *PHTTRANSMIT_SETTING;
+
+typedef enum _RT_802_11_PREAMBLE {
+    Rt802_11PreambleLong,
+    Rt802_11PreambleShort,
+    Rt802_11PreambleAuto
+} RT_802_11_PREAMBLE, *PRT_802_11_PREAMBLE;
+
+// Only for STA, need to sync with AP
+// 2005-03-08 match current RaConfig.
+typedef enum _RT_802_11_PHY_MODE {
+	PHY_11BG_MIXED = 0,
+	PHY_11B,
+	PHY_11A,
+	PHY_11ABG_MIXED,
+	PHY_11G,
+#ifdef DOT11_N_SUPPORT
+	PHY_11ABGN_MIXED,	// both band   5
+	PHY_11N_2_4G,		// 11n-only with 2.4G band   	6
+	PHY_11GN_MIXED,	// 2.4G band      7
+	PHY_11AN_MIXED,	// 5G  band       8
+	PHY_11BGN_MIXED,	// if check 802.11b.      9
+	PHY_11AGN_MIXED,	// if check 802.11b.      10
+	PHY_11N_5G,			// 11n-only with 5G band		11
+#endif // DOT11_N_SUPPORT //
+} RT_802_11_PHY_MODE;
+
+// put all proprietery for-query objects here to reduce # of Query_OID
+typedef struct _RT_802_11_LINK_STATUS {
+    ULONG   CurrTxRate;         // in units of 0.5Mbps
+    ULONG   ChannelQuality;     // 0..100 %
+    ULONG   TxByteCount;        // both ok and fail
+    ULONG   RxByteCount;        // both ok and fail
+    ULONG	CentralChannel;		// 40MHz central channel number
+} RT_802_11_LINK_STATUS, *PRT_802_11_LINK_STATUS;
+
+typedef struct _RT_802_11_EVENT_LOG {
+    LARGE_INTEGER   SystemTime;  // timestammp via NdisGetCurrentSystemTime()
+    UCHAR           Addr[MAC_ADDR_LENGTH];
+    USHORT          Event;       // EVENT_xxx
+} RT_802_11_EVENT_LOG, *PRT_802_11_EVENT_LOG;
+
+typedef struct _RT_802_11_EVENT_TABLE {
+    ULONG       Num;
+    ULONG       Rsv;     // to align Log[] at LARGE_INEGER boundary
+    RT_802_11_EVENT_LOG   Log[MAX_NUMBER_OF_EVENT];
+} RT_802_11_EVENT_TABLE, PRT_802_11_EVENT_TABLE;
+
+// MIMO Tx parameter, ShortGI, MCS, STBC, etc.  these are fields in TXWI. Don't change this definition!!!
+typedef union  _MACHTTRANSMIT_SETTING {
+	struct	{
+	USHORT   	MCS:7;                 // MCS
+	USHORT		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	USHORT		ShortGI:1;
+	USHORT		STBC:2;	//SPACE
+	USHORT		rsv:3;
+	USHORT		MODE:2;	// Use definition MODE_xxx.
+	}	field;
+	USHORT		word;
+ } MACHTTRANSMIT_SETTING, *PMACHTTRANSMIT_SETTING;
+
+typedef struct _RT_802_11_MAC_ENTRY {
+    UCHAR       Addr[MAC_ADDR_LENGTH];
+    UCHAR       Aid;
+    UCHAR       Psm;     // 0:PWR_ACTIVE, 1:PWR_SAVE
+    UCHAR		MimoPs;  // 0:MMPS_STATIC, 1:MMPS_DYNAMIC, 3:MMPS_Enabled
+    CHAR		AvgRssi0;
+	CHAR		AvgRssi1;
+	CHAR		AvgRssi2;
+	UINT32		ConnectedTime;
+    MACHTTRANSMIT_SETTING	TxRate;
+} RT_802_11_MAC_ENTRY, *PRT_802_11_MAC_ENTRY;
+
+typedef struct _RT_802_11_MAC_TABLE {
+    ULONG       Num;
+    RT_802_11_MAC_ENTRY Entry[MAX_NUMBER_OF_MAC];
+} RT_802_11_MAC_TABLE, *PRT_802_11_MAC_TABLE;
+
+// structure for query/set hardware register - MAC, BBP, RF register
+typedef struct _RT_802_11_HARDWARE_REGISTER {
+    ULONG   HardwareType;       // 0:MAC, 1:BBP, 2:RF register, 3:EEPROM
+    ULONG   Offset;             // Q/S register offset addr
+    ULONG   Data;               // R/W data buffer
+} RT_802_11_HARDWARE_REGISTER, *PRT_802_11_HARDWARE_REGISTER;
+
+typedef struct _RT_802_11_AP_CONFIG {
+    ULONG   EnableTxBurst;      // 0-disable, 1-enable
+    ULONG   EnableTurboRate;    // 0-disable, 1-enable 72/100mbps turbo rate
+    ULONG   IsolateInterStaTraffic;     // 0-disable, 1-enable isolation
+    ULONG   HideSsid;           // 0-disable, 1-enable hiding
+    ULONG   UseBGProtection;    // 0-AUTO, 1-always ON, 2-always OFF
+    ULONG   UseShortSlotTime;   // 0-no use, 1-use 9-us short slot time
+    ULONG   Rsv1;               // must be 0
+    ULONG   SystemErrorBitmap;  // ignore upon SET, return system error upon QUERY
+} RT_802_11_AP_CONFIG, *PRT_802_11_AP_CONFIG;
+
+// structure to query/set STA_CONFIG
+typedef struct _RT_802_11_STA_CONFIG {
+    ULONG   EnableTxBurst;      // 0-disable, 1-enable
+    ULONG   EnableTurboRate;    // 0-disable, 1-enable 72/100mbps turbo rate
+    ULONG   UseBGProtection;    // 0-AUTO, 1-always ON, 2-always OFF
+    ULONG   UseShortSlotTime;   // 0-no use, 1-use 9-us short slot time when applicable
+    ULONG   AdhocMode; 			// 0-11b rates only (WIFI spec), 1 - b/g mixed, 2 - g only
+    ULONG   HwRadioStatus;      // 0-OFF, 1-ON, default is 1, Read-Only
+    ULONG   Rsv1;               // must be 0
+    ULONG   SystemErrorBitmap;  // ignore upon SET, return system error upon QUERY
+} RT_802_11_STA_CONFIG, *PRT_802_11_STA_CONFIG;
+
+//
+//  For OID Query or Set about BA structure
+//
+typedef	struct	_OID_BACAP_STRUC	{
+		UCHAR		RxBAWinLimit;
+		UCHAR		TxBAWinLimit;
+		UCHAR		Policy;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use. other value invalid
+		UCHAR		MpduDensity;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use. other value invalid
+		UCHAR       	AmsduEnable;	//Enable AMSDU transmisstion
+		UCHAR       	AmsduSize;	// 0:3839, 1:7935 bytes. UINT  MSDUSizeToBytes[]	= { 3839, 7935};
+		UCHAR       	MMPSmode;	// MIMO power save more, 0:static, 1:dynamic, 2:rsv, 3:mimo enable
+		BOOLEAN		AutoBA;	// Auto BA will automatically
+} OID_BACAP_STRUC, *POID_BACAP_STRUC;
+
+typedef struct _RT_802_11_ACL_ENTRY {
+    UCHAR   Addr[MAC_ADDR_LENGTH];
+    USHORT  Rsv;
+} RT_802_11_ACL_ENTRY, *PRT_802_11_ACL_ENTRY;
+
+typedef struct PACKED _RT_802_11_ACL {
+    ULONG   Policy;             // 0-disable, 1-positive list, 2-negative list
+    ULONG   Num;
+    RT_802_11_ACL_ENTRY Entry[MAX_NUMBER_OF_ACL];
+} RT_802_11_ACL, *PRT_802_11_ACL;
+
+typedef struct _RT_802_11_WDS {
+    ULONG						Num;
+    NDIS_802_11_MAC_ADDRESS		Entry[24/*MAX_NUM_OF_WDS_LINK*/];
+	ULONG						KeyLength;
+	UCHAR						KeyMaterial[32];
+} RT_802_11_WDS, *PRT_802_11_WDS;
+
+typedef struct _RT_802_11_TX_RATES_ {
+    UCHAR       SupRateLen;
+    UCHAR       SupRate[MAX_LENGTH_OF_SUPPORT_RATES];
+    UCHAR       ExtRateLen;
+    UCHAR       ExtRate[MAX_LENGTH_OF_SUPPORT_RATES];
+} RT_802_11_TX_RATES, *PRT_802_11_TX_RATES;
+
+
+// Definition of extra information code
+#define	GENERAL_LINK_UP			0x0			// Link is Up
+#define	GENERAL_LINK_DOWN		0x1			// Link is Down
+#define	HW_RADIO_OFF			0x2			// Hardware radio off
+#define	SW_RADIO_OFF			0x3			// Software radio off
+#define	AUTH_FAIL				0x4			// Open authentication fail
+#define	AUTH_FAIL_KEYS			0x5			// Shared authentication fail
+#define	ASSOC_FAIL				0x6			// Association failed
+#define	EAP_MIC_FAILURE			0x7			// Deauthencation because MIC failure
+#define	EAP_4WAY_TIMEOUT		0x8			// Deauthencation on 4-way handshake timeout
+#define	EAP_GROUP_KEY_TIMEOUT	0x9			// Deauthencation on group key handshake timeout
+#define	EAP_SUCCESS				0xa			// EAP succeed
+#define	DETECT_RADAR_SIGNAL		0xb         // Radar signal occur in current channel
+#define EXTRA_INFO_MAX			0xb			// Indicate Last OID
+
+#define EXTRA_INFO_CLEAR		0xffffffff
+
+// This is OID setting structure. So only GF or MM as Mode. This is valid when our wirelss mode has 802.11n in use.
+typedef struct {
+	RT_802_11_PHY_MODE		PhyMode; 	//
+	UCHAR		TransmitNo;
+	UCHAR		HtMode; 	//HTMODE_GF or HTMODE_MM
+	UCHAR		ExtOffset;	//extension channel above or below
+	UCHAR		MCS;
+	UCHAR   	BW;
+	UCHAR		STBC;
+	UCHAR		SHORTGI;
+	UCHAR		rsv;
+} OID_SET_HT_PHYMODE, *POID_SET_HT_PHYMODE;
+
+#ifdef LLTD_SUPPORT
+typedef struct _RT_LLTD_ASSOICATION_ENTRY {
+    UCHAR           Addr[ETH_LENGTH_OF_ADDRESS];
+    unsigned short  MOR;        // maximum operational rate
+    UCHAR           phyMode;
+} RT_LLTD_ASSOICATION_ENTRY, *PRT_LLTD_ASSOICATION_ENTRY;
+
+typedef struct _RT_LLTD_ASSOICATION_TABLE {
+    unsigned int                Num;
+    RT_LLTD_ASSOICATION_ENTRY   Entry[MAX_NUMBER_OF_MAC];
+} RT_LLTD_ASSOICATION_TABLE, *PRT_LLTD_ASSOICATION_TABLE;
+#endif // LLTD_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+//rt2860, kathy 2007-0118
+// structure for DLS
+typedef struct _RT_802_11_DLS_UI {
+	USHORT						TimeOut;		// unit: second , set by UI
+	USHORT						CountDownTimer;	// unit: second , used by driver only
+	NDIS_802_11_MAC_ADDRESS		MacAddr;		// set by UI
+	UCHAR						Status;			// 0: none , 1: wait STAkey, 2: finish DLS setup , set by driver only
+	BOOLEAN						Valid;			// 1: valid , 0: invalid , set by UI, use to setup or tear down DLS link
+} RT_802_11_DLS_UI, *PRT_802_11_DLS_UI;
+
+typedef struct _RT_802_11_DLS_INFO {
+	RT_802_11_DLS_UI	Entry[MAX_NUMBER_OF_DLS_ENTRY];
+	UCHAR				num;
+} RT_802_11_DLS_INFO, *PRT_802_11_DLS_INFO;
+
+typedef enum _RT_802_11_DLS_MODE {
+    DLS_NONE,
+    DLS_WAIT_KEY,
+    DLS_FINISH
+} RT_802_11_DLS_MODE;
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+#define	RT_ASSOC_EVENT_FLAG                         0x0101
+#define	RT_DISASSOC_EVENT_FLAG                      0x0102
+#define	RT_REQIE_EVENT_FLAG                         0x0103
+#define	RT_RESPIE_EVENT_FLAG                        0x0104
+#define	RT_ASSOCINFO_EVENT_FLAG                     0x0105
+#define RT_PMKIDCAND_FLAG                           0x0106
+#define RT_INTERFACE_DOWN                           0x0107
+#define RT_INTERFACE_UP                             0x0108
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+#define MAX_CUSTOM_LEN 128
+
+#ifdef CONFIG_STA_SUPPORT
+typedef enum _RT_802_11_D_CLIENT_MODE
+{
+   Rt802_11_D_None,
+   Rt802_11_D_Flexible,
+   Rt802_11_D_Strict,
+} RT_802_11_D_CLIENT_MODE, *PRT_802_11_D_CLIENT_MODE;
+#endif // CONFIG_STA_SUPPORT //
+
+typedef struct _RT_CHANNEL_LIST_INFO
+{
+	UCHAR ChannelList[MAX_NUM_OF_CHS];   // list all supported channels for site survey
+	UCHAR ChannelListNum; // number of channel in ChannelList[]
+} RT_CHANNEL_LIST_INFO, *PRT_CHANNEL_LIST_INFO;
+
+
+#endif // _OID_H_
+
diff --git a/drivers/staging/rt2860/rt2860.h b/drivers/staging/rt2860/rt2860.h
new file mode 100644
index 000000000000..017201906628
--- /dev/null
+++ b/drivers/staging/rt2860/rt2860.h
@@ -0,0 +1,349 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __RT2860_H__
+#define __RT2860_H__
+
+#define RT28xx_CHIP_NAME	"RT2860"
+
+#define TXINFO_SIZE               0
+#define TXPADDING_SIZE      	  0
+
+/* ----------------- EEPROM Related MACRO ----------------- */
+#define RT28xx_EEPROM_READ16(pAd, offset, var)		\
+	var = RTMP_EEPROM_READ16(pAd, offset)
+
+#define RT28xx_EEPROM_WRITE16(pAd, offset, var)		\
+	RTMP_EEPROM_WRITE16(pAd, offset, var)
+
+/* ----------------- TASK/THREAD Related MACRO ----------------- */
+#define RT28XX_TASK_THREAD_INIT(pAd, Status)		\
+	init_thread_task(pAd); NICInitTxRxRingAndBacklogQueue(pAd);	\
+	Status = NDIS_STATUS_SUCCESS;
+
+/* function declarations */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#define IRQ_HANDLE_TYPE  irqreturn_t
+#else
+#define IRQ_HANDLE_TYPE  void
+#endif
+
+IRQ_HANDLE_TYPE
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,19))
+rt2860_interrupt(int irq, void *dev_instance);
+#else
+rt2860_interrupt(int irq, void *dev_instance, struct pt_regs *regs);
+#endif
+
+/* ----------------- Frimware Related MACRO ----------------- */
+#define RT28XX_WRITE_FIRMWARE(_pAd, _pFwImage, _FwLen)				\
+	do{																\
+		ULONG	_i, _firm;											\
+		RTMP_IO_WRITE32(_pAd, PBF_SYS_CTRL, 0x10000);				\
+																	\
+		for(_i=0; _i<_FwLen; _i+=4)									\
+		{															\
+			_firm = _pFwImage[_i] +									\
+			   (_pFwImage[_i+3] << 24) +							\
+			   (_pFwImage[_i+2] << 16) +							\
+			   (_pFwImage[_i+1] << 8);								\
+			RTMP_IO_WRITE32(_pAd, FIRMWARE_IMAGE_BASE + _i, _firm);	\
+		}															\
+		RTMP_IO_WRITE32(_pAd, PBF_SYS_CTRL, 0x00000);				\
+		RTMP_IO_WRITE32(_pAd, PBF_SYS_CTRL, 0x00001);				\
+																	\
+		/* initialize BBP R/W access agent */						\
+		RTMP_IO_WRITE32(_pAd, H2M_BBP_AGENT, 0);					\
+		RTMP_IO_WRITE32(_pAd, H2M_MAILBOX_CSR, 0);					\
+	}while(0)
+
+/* ----------------- TX Related MACRO ----------------- */
+#define RT28XX_START_DEQUEUE(pAd, QueIdx, irqFlags)		do{}while(0)
+#define RT28XX_STOP_DEQUEUE(pAd, QueIdx, irqFlags)		do{}while(0)
+
+
+#define RT28XX_HAS_ENOUGH_FREE_DESC(pAd, pTxBlk, freeNum, pPacket) \
+		((freeNum) >= (ULONG)(pTxBlk->TotalFragNum + RTMP_GET_PACKET_FRAGMENTS(pPacket) + 3)) /* rough estimate we will use 3 more descriptor. */
+#define RT28XX_RELEASE_DESC_RESOURCE(pAd, QueIdx)					\
+		do{}while(0)
+
+#define NEED_QUEUE_BACK_FOR_AGG(pAd, QueIdx, freeNum, _TxFrameType) \
+		(((freeNum != (TX_RING_SIZE-1)) && (pAd->TxSwQueue[QueIdx].Number == 0)) || (freeNum<3))
+		//(((freeNum) != (TX_RING_SIZE-1)) && (pAd->TxSwQueue[QueIdx].Number == 1 /*0*/))
+
+
+#define HAL_KickOutMgmtTx(_pAd, _QueIdx, _pPacket, _pSrcBufVA, _SrcBufLen)	\
+			RtmpPCIMgmtKickOut(_pAd, _QueIdx, _pPacket, _pSrcBufVA, _SrcBufLen)
+
+#define RTMP_PKT_TAIL_PADDING 0
+
+#define fRTMP_ADAPTER_NEED_STOP_TX	0
+
+#define HAL_WriteSubTxResource(pAd, pTxBlk, bIsLast, pFreeNumber)	\
+		/* RtmpPCI_WriteSubTxResource(pAd, pTxBlk, bIsLast, pFreeNumber)*/
+
+#define HAL_WriteTxResource(pAd, pTxBlk,bIsLast, pFreeNumber)	\
+			RtmpPCI_WriteSingleTxResource(pAd, pTxBlk, bIsLast, pFreeNumber)
+
+#define HAL_WriteFragTxResource(pAd, pTxBlk, fragNum, pFreeNumber) \
+			RtmpPCI_WriteFragTxResource(pAd, pTxBlk, fragNum, pFreeNumber)
+
+#define HAL_WriteMultiTxResource(pAd, pTxBlk,frameNum, pFreeNumber)	\
+			RtmpPCI_WriteMultiTxResource(pAd, pTxBlk, frameNum, pFreeNumber)
+
+#define HAL_FinalWriteTxResource(_pAd, _pTxBlk, _TotalMPDUSize, _FirstTxIdx)	\
+			RtmpPCI_FinalWriteTxResource(_pAd, _pTxBlk, _TotalMPDUSize, _FirstTxIdx)
+
+#define HAL_LastTxIdx(_pAd, _QueIdx,_LastTxIdx) \
+			/*RtmpPCIDataLastTxIdx(_pAd, _QueIdx,_LastTxIdx)*/
+
+#define HAL_KickOutTx(_pAd, _pTxBlk, _QueIdx)	\
+			RTMP_IO_WRITE32((_pAd), TX_CTX_IDX0+((_QueIdx)*0x10), (_pAd)->TxRing[(_QueIdx)].TxCpuIdx)
+/*			RtmpPCIDataKickOut(_pAd, _pTxBlk, _QueIdx)*/
+
+#define HAL_KickOutNullFrameTx(_pAd, _QueIdx, _pNullFrame, _frameLen)	\
+			MiniportMMRequest(_pAd, _QueIdx, _pNullFrame, _frameLen)
+
+#define GET_TXRING_FREENO(_pAd, _QueIdx) \
+	(_pAd->TxRing[_QueIdx].TxSwFreeIdx > _pAd->TxRing[_QueIdx].TxCpuIdx)	? \
+			(_pAd->TxRing[_QueIdx].TxSwFreeIdx - _pAd->TxRing[_QueIdx].TxCpuIdx - 1) \
+			 :	\
+			(_pAd->TxRing[_QueIdx].TxSwFreeIdx + TX_RING_SIZE - _pAd->TxRing[_QueIdx].TxCpuIdx - 1);
+
+
+#define GET_MGMTRING_FREENO(_pAd) \
+	(_pAd->MgmtRing.TxSwFreeIdx > _pAd->MgmtRing.TxCpuIdx)	? \
+			(_pAd->MgmtRing.TxSwFreeIdx - _pAd->MgmtRing.TxCpuIdx - 1) \
+			 :	\
+			(_pAd->MgmtRing.TxSwFreeIdx + MGMT_RING_SIZE - _pAd->MgmtRing.TxCpuIdx - 1);
+
+
+/* ----------------- RX Related MACRO ----------------- */
+
+// no use
+#define RT28XX_RCV_PKT_GET_INIT(pAd)
+#define RT28XX_RV_A_BUF_END
+//#define RT28XX_RV_ALL_BUF_END
+
+
+/* ----------------- ASIC Related MACRO ----------------- */
+// no use
+#define RT28XX_DMA_POST_WRITE(pAd)
+
+// reset MAC of a station entry to 0x000000000000
+#define RT28XX_STA_ENTRY_MAC_RESET(pAd, Wcid)						\
+	AsicDelWcidTab(pAd, Wcid);
+
+// add this entry into ASIC RX WCID search table
+#define RT28XX_STA_ENTRY_ADD(pAd, pEntry)							\
+	AsicUpdateRxWCIDTable(pAd, pEntry->Aid, pEntry->Addr);
+
+// remove Pair-wise key material from ASIC
+#define RT28XX_STA_ENTRY_KEY_DEL(pAd, BssIdx, Wcid)					\
+	AsicRemovePairwiseKeyEntry(pAd, BssIdx, (UCHAR)Wcid);
+
+// add Client security information into ASIC WCID table and IVEIV table
+#define RT28XX_STA_SECURITY_INFO_ADD(pAd, apidx, KeyID, pEntry)		\
+	RTMPAddWcidAttributeEntry(pAd, apidx, KeyID, 					\
+							pAd->SharedKey[apidx][KeyID].CipherAlg, pEntry);
+
+#define RT28XX_SECURITY_KEY_ADD(pAd, apidx, KeyID, pEntry)				\
+	{	/* update pairwise key information to ASIC Shared Key Table */	\
+		AsicAddSharedKeyEntry(pAd, apidx, KeyID,						\
+						  pAd->SharedKey[apidx][KeyID].CipherAlg,		\
+						  pAd->SharedKey[apidx][KeyID].Key,				\
+						  pAd->SharedKey[apidx][KeyID].TxMic,			\
+						  pAd->SharedKey[apidx][KeyID].RxMic);			\
+		/* update ASIC WCID attribute table and IVEIV table */			\
+		RTMPAddWcidAttributeEntry(pAd, apidx, KeyID,					\
+						  pAd->SharedKey[apidx][KeyID].CipherAlg,		\
+						  pEntry); }
+
+
+// Insert the BA bitmap to ASIC for the Wcid entry
+#define RT28XX_ADD_BA_SESSION_TO_ASIC(_pAd, _Aid, _TID)					\
+		do{																\
+			UINT32	_Value = 0, _Offset;									\
+			_Offset = MAC_WCID_BASE + (_Aid) * HW_WCID_ENTRY_SIZE + 4;	\
+			RTMP_IO_READ32((_pAd), _Offset, &_Value);					\
+			_Value |= (0x10000<<(_TID));								\
+			RTMP_IO_WRITE32((_pAd), _Offset, _Value);					\
+		}while(0)
+
+
+// Remove the BA bitmap from ASIC for the Wcid entry
+//		bitmap field starts at 0x10000 in ASIC WCID table
+#define RT28XX_DEL_BA_SESSION_FROM_ASIC(_pAd, _Wcid, _TID)				\
+		do{																\
+			UINT32	_Value = 0, _Offset;									\
+			_Offset = MAC_WCID_BASE + (_Wcid) * HW_WCID_ENTRY_SIZE + 4;	\
+			RTMP_IO_READ32((_pAd), _Offset, &_Value);					\
+			_Value &= (~(0x10000 << (_TID)));							\
+			RTMP_IO_WRITE32((_pAd), _Offset, _Value);			\
+		}while(0)
+
+
+/* ----------------- PCI/USB Related MACRO ----------------- */
+
+#define RT28XX_HANDLE_DEV_ASSIGN(handle, dev_p)				\
+	((POS_COOKIE)handle)->pci_dev = dev_p;
+
+// set driver data
+#define RT28XX_DRVDATA_SET(_a)			pci_set_drvdata(_a, net_dev);
+
+#define RT28XX_UNMAP()										\
+{	if (net_dev->base_addr)	{								\
+		iounmap((void *)(net_dev->base_addr));				\
+		release_mem_region(pci_resource_start(dev_p, 0),	\
+							pci_resource_len(dev_p, 0)); }	\
+	if (net_dev->irq) pci_release_regions(dev_p); }
+
+#ifdef PCI_MSI_SUPPORT
+#define RTMP_MSI_ENABLE(_pAd) \
+{ 	POS_COOKIE _pObj = (POS_COOKIE)(_pAd->OS_Cookie); \
+	(_pAd)->HaveMsi =	pci_enable_msi(_pObj->pci_dev) == 0 ? TRUE : FALSE; }
+
+#define RTMP_MSI_DISABLE(_pAd) \
+{ 	POS_COOKIE _pObj = (POS_COOKIE)(_pAd->OS_Cookie); \
+	if (_pAd->HaveMsi == TRUE) \
+		pci_disable_msi(_pObj->pci_dev); \
+	_pAd->HaveMsi = FALSE;	}
+#else
+#define RTMP_MSI_ENABLE(_pAd)
+#define RTMP_MSI_DISABLE(_pAd)
+#endif // PCI_MSI_SUPPORT //
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,24)
+#define SA_SHIRQ IRQF_SHARED
+#endif
+
+#define RT28XX_IRQ_REQUEST(net_dev)							\
+{	PRTMP_ADAPTER _pAd = (PRTMP_ADAPTER)((net_dev)->ml_priv);	\
+	POS_COOKIE _pObj = (POS_COOKIE)(_pAd->OS_Cookie);		\
+	RTMP_MSI_ENABLE(_pAd);									\
+	if ((retval = request_irq(_pObj->pci_dev->irq, 		\
+							rt2860_interrupt, SA_SHIRQ,		\
+							(net_dev)->name, (net_dev)))) {	\
+		printk("RT2860: request_irq  ERROR(%d)\n", retval);	\
+	return retval; } }
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#define RT28XX_IRQ_RELEASE(net_dev)								\
+{	PRTMP_ADAPTER _pAd = (PRTMP_ADAPTER)((net_dev)->ml_priv);		\
+	POS_COOKIE _pObj = (POS_COOKIE)(_pAd->OS_Cookie);			\
+	synchronize_irq(_pObj->pci_dev->irq);						\
+	free_irq(_pObj->pci_dev->irq, (net_dev));					\
+	RTMP_MSI_DISABLE(_pAd); }
+#else
+#define RT28XX_IRQ_RELEASE(net_dev)								\
+{	PRTMP_ADAPTER _pAd = (PRTMP_ADAPTER)((net_dev)->priv);		\
+	POS_COOKIE _pObj = (POS_COOKIE)(_pAd->OS_Cookie);			\
+	free_irq(_pObj->pci_dev->irq, (net_dev));					\
+	RTMP_MSI_DISABLE(_pAd); }
+#endif
+
+#define RT28XX_IRQ_INIT(pAd)										\
+	{	pAd->int_enable_reg = ((DELAYINTMASK) |						\
+							(RxINT|TxDataInt|TxMgmtInt)) & ~(0x03);	\
+		pAd->int_disable_mask = 0;									\
+		pAd->int_pending = 0; }
+
+#define RT28XX_IRQ_ENABLE(pAd)									\
+	{	/* clear garbage ints */								\
+		RTMP_IO_WRITE32(pAd, INT_SOURCE_CSR, 0xffffffff);		\
+		NICEnableInterrupt(pAd); }
+
+#define RT28XX_PUT_DEVICE(dev_p)
+
+
+/* ----------------- MLME Related MACRO ----------------- */
+#define RT28XX_MLME_HANDLER(pAd)			MlmeHandler(pAd)
+
+#define RT28XX_MLME_PRE_SANITY_CHECK(pAd)
+
+#define RT28XX_MLME_STA_QUICK_RSP_WAKE_UP(pAd)	\
+	RTMPSetTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, 100);
+
+#define RT28XX_MLME_RESET_STATE_MACHINE(pAd)	\
+		MlmeRestartStateMachine(pAd)
+
+#define RT28XX_HANDLE_COUNTER_MEASURE(_pAd, _pEntry)		\
+		HandleCounterMeasure(_pAd, _pEntry)
+
+/* ----------------- Power Save Related MACRO ----------------- */
+#define RT28XX_PS_POLL_ENQUEUE(pAd)				EnqueuePsPoll(pAd)
+
+//
+// Device ID & Vendor ID, these values should match EEPROM value
+//
+#define NIC2860_PCI_DEVICE_ID   0x0601
+#define NIC2860_PCIe_DEVICE_ID  0x0681
+#define NIC2760_PCI_DEVICE_ID   0x0701		// 1T/2R Cardbus ???
+#define NIC2790_PCIe_DEVICE_ID  0x0781	    // 1T/2R miniCard
+
+#define NIC_PCI_VENDOR_ID       0x1814
+
+#define VEN_AWT_PCIe_DEVICE_ID	0x1059
+#define VEN_AWT_PCI_VENDOR_ID	0x1A3B
+
+// For RTMPPCIePowerLinkCtrlRestore () function
+#define RESTORE_HALT		    1
+#define RESTORE_WAKEUP		    2
+#define RESTORE_CLOSE           3
+
+#define PowerSafeCID		1
+#define PowerRadioOffCID		2
+#define PowerWakeCID		3
+#define CID0MASK		0x000000ff
+#define CID1MASK		0x0000ff00
+#define CID2MASK		0x00ff0000
+#define CID3MASK		0xff000000
+
+#define PCI_REG_READ_WORD(pci_dev, offset, Configuration)   \
+    if (pci_read_config_word(pci_dev, offset, &reg16) == 0)     \
+        Configuration = le2cpu16(reg16);                        \
+    else                                                        \
+        Configuration = 0;
+
+#define PCI_REG_WIRTE_WORD(pci_dev, offset, Configuration)  \
+    reg16 = cpu2le16(Configuration);                        \
+    pci_write_config_word(pci_dev, offset, reg16);          \
+
+#define RT28XX_STA_FORCE_WAKEUP(pAd, bFromTx) \
+    RT28xxPciStaAsicForceWakeup(pAd, bFromTx);
+
+#define RT28XX_STA_SLEEP_THEN_AUTO_WAKEUP(pAd, TbttNumToNextWakeUp) \
+    RT28xxPciStaAsicSleepThenAutoWakeup(pAd, TbttNumToNextWakeUp);
+
+#define RT28XX_MLME_RADIO_ON(pAd) \
+    RT28xxPciMlmeRadioOn(pAd);
+
+#define RT28XX_MLME_RADIO_OFF(pAd) \
+    RT28xxPciMlmeRadioOFF(pAd);
+
+#endif //__RT2860_H__
+
diff --git a/drivers/staging/rt2860/rt28xx.h b/drivers/staging/rt2860/rt28xx.h
new file mode 100644
index 000000000000..ff23043e560e
--- /dev/null
+++ b/drivers/staging/rt2860/rt28xx.h
@@ -0,0 +1,2714 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rt28xx.h
+
+	Abstract:
+	RT28xx ASIC related definition & structures
+
+	Revision History:
+	Who			When		  What
+	--------	----------	  ----------------------------------------------
+       Jan Lee           Jan-3-2006     created for RT2860c
+*/
+
+#ifndef	__RT28XX_H__
+#define	__RT28XX_H__
+
+
+//
+// PCI registers - base address 0x0000
+//
+#define PCI_CFG			0x0000
+#define PCI_EECTRL			0x0004
+#define PCI_MCUCTRL			0x0008
+
+//
+// SCH/DMA registers - base address 0x0200
+//
+// INT_SOURCE_CSR: Interrupt source register. Write one to clear corresponding bit
+//
+#define DMA_CSR0      0x200
+#define INT_SOURCE_CSR      0x200
+#ifdef RT_BIG_ENDIAN
+typedef	union	_INT_SOURCE_CSR_STRUC	{
+	struct	{
+		UINT32       	:14;
+		UINT32       	TxCoherent:1;
+		UINT32       	RxCoherent:1;
+		UINT32       	GPTimer:1;
+		UINT32       	AutoWakeup:1;//bit14
+		UINT32       	TXFifoStatusInt:1;//FIFO Statistics is full, sw should read 0x171c
+		UINT32       	PreTBTT:1;
+		UINT32       	TBTTInt:1;
+		UINT32       	RxTxCoherent:1;
+		UINT32       	MCUCommandINT:1;
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	HccaDmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac1DmaDone:1;
+		UINT32		Ac0DmaDone:1;
+		UINT32		RxDone:1;
+		UINT32		TxDelayINT:1;	//delayed interrupt, not interrupt until several int or time limit hit
+		UINT32		RxDelayINT:1; //dealyed interrupt
+	}	field;
+	UINT32			word;
+}	INT_SOURCE_CSR_STRUC, *PINT_SOURCE_CSR_STRUC;
+#else
+typedef	union	_INT_SOURCE_CSR_STRUC	{
+	struct	{
+		UINT32		RxDelayINT:1;
+		UINT32		TxDelayINT:1;
+		UINT32		RxDone:1;
+		UINT32		Ac0DmaDone:1;//4
+		UINT32       	Ac1DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	HccaDmaDone:1; // bit7
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	MCUCommandINT:1;//bit 9
+		UINT32       	RxTxCoherent:1;
+		UINT32       	TBTTInt:1;
+		UINT32       	PreTBTT:1;
+		UINT32       	TXFifoStatusInt:1;//FIFO Statistics is full, sw should read 0x171c
+		UINT32       	AutoWakeup:1;//bit14
+		UINT32       	GPTimer:1;
+		UINT32       	RxCoherent:1;//bit16
+		UINT32       	TxCoherent:1;
+		UINT32       	:14;
+	}	field;
+	UINT32			word;
+} INT_SOURCE_CSR_STRUC, *PINT_SOURCE_CSR_STRUC;
+#endif
+
+//
+// INT_MASK_CSR:   Interrupt MASK register.   1: the interrupt is mask OFF
+//
+#define INT_MASK_CSR        0x204
+#ifdef RT_BIG_ENDIAN
+typedef	union	_INT_MASK_CSR_STRUC	{
+	struct	{
+		UINT32       	TxCoherent:1;
+		UINT32       	RxCoherent:1;
+		UINT32       	:20;
+		UINT32       	MCUCommandINT:1;
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	HccaDmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac1DmaDone:1;
+		UINT32		Ac0DmaDone:1;
+		UINT32		RxDone:1;
+		UINT32		TxDelay:1;
+		UINT32		RXDelay_INT_MSK:1;
+	}	field;
+	UINT32			word;
+}INT_MASK_CSR_STRUC, *PINT_MASK_CSR_STRUC;
+#else
+typedef	union	_INT_MASK_CSR_STRUC	{
+	struct	{
+		UINT32		RXDelay_INT_MSK:1;
+		UINT32		TxDelay:1;
+		UINT32		RxDone:1;
+		UINT32		Ac0DmaDone:1;
+		UINT32       	Ac1DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	HccaDmaDone:1;
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	MCUCommandINT:1;
+		UINT32       	:20;
+		UINT32       	RxCoherent:1;
+		UINT32       	TxCoherent:1;
+	}	field;
+	UINT32			word;
+} INT_MASK_CSR_STRUC, *PINT_MASK_CSR_STRUC;
+#endif
+#define WPDMA_GLO_CFG 	0x208
+#ifdef RT_BIG_ENDIAN
+typedef	union	_WPDMA_GLO_CFG_STRUC	{
+	struct	{
+		UINT32       	HDR_SEG_LEN:16;
+		UINT32       	RXHdrScater:8;
+		UINT32       	BigEndian:1;
+		UINT32       	EnTXWriteBackDDONE:1;
+		UINT32       	WPDMABurstSIZE:2;
+		UINT32		RxDMABusy:1;
+		UINT32		EnableRxDMA:1;
+		UINT32		TxDMABusy:1;
+		UINT32		EnableTxDMA:1;
+	}	field;
+	UINT32			word;
+}WPDMA_GLO_CFG_STRUC, *PWPDMA_GLO_CFG_STRUC;
+#else
+typedef	union	_WPDMA_GLO_CFG_STRUC	{
+	struct	{
+		UINT32		EnableTxDMA:1;
+		UINT32		TxDMABusy:1;
+		UINT32		EnableRxDMA:1;
+		UINT32		RxDMABusy:1;
+		UINT32       	WPDMABurstSIZE:2;
+		UINT32       	EnTXWriteBackDDONE:1;
+		UINT32       	BigEndian:1;
+		UINT32       	RXHdrScater:8;
+		UINT32       	HDR_SEG_LEN:16;
+	}	field;
+	UINT32			word;
+} WPDMA_GLO_CFG_STRUC, *PWPDMA_GLO_CFG_STRUC;
+#endif
+#define WPDMA_RST_IDX 	0x20c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_WPDMA_RST_IDX_STRUC	{
+	struct	{
+		UINT32       	:15;
+		UINT32       	RST_DRX_IDX0:1;
+		UINT32       	rsv:10;
+		UINT32       	RST_DTX_IDX5:1;
+		UINT32       	RST_DTX_IDX4:1;
+		UINT32		RST_DTX_IDX3:1;
+		UINT32		RST_DTX_IDX2:1;
+		UINT32		RST_DTX_IDX1:1;
+		UINT32		RST_DTX_IDX0:1;
+	}	field;
+	UINT32			word;
+}WPDMA_RST_IDX_STRUC, *PWPDMA_RST_IDX_STRUC;
+#else
+typedef	union	_WPDMA_RST_IDX_STRUC	{
+	struct	{
+		UINT32		RST_DTX_IDX0:1;
+		UINT32		RST_DTX_IDX1:1;
+		UINT32		RST_DTX_IDX2:1;
+		UINT32		RST_DTX_IDX3:1;
+		UINT32       	RST_DTX_IDX4:1;
+		UINT32       	RST_DTX_IDX5:1;
+		UINT32       	rsv:10;
+		UINT32       	RST_DRX_IDX0:1;
+		UINT32       	:15;
+	}	field;
+	UINT32			word;
+} WPDMA_RST_IDX_STRUC, *PWPDMA_RST_IDX_STRUC;
+#endif
+#define DELAY_INT_CFG  0x0210
+#ifdef RT_BIG_ENDIAN
+typedef	union	_DELAY_INT_CFG_STRUC	{
+	struct	{
+		UINT32       	TXDLY_INT_EN:1;
+		UINT32       	TXMAX_PINT:7;
+		UINT32       	TXMAX_PTIME:8;
+		UINT32       	RXDLY_INT_EN:1;
+		UINT32       	RXMAX_PINT:7;
+		UINT32		RXMAX_PTIME:8;
+	}	field;
+	UINT32			word;
+}DELAY_INT_CFG_STRUC, *PDELAY_INT_CFG_STRUC;
+#else
+typedef	union	_DELAY_INT_CFG_STRUC	{
+	struct	{
+		UINT32		RXMAX_PTIME:8;
+		UINT32       	RXMAX_PINT:7;
+		UINT32       	RXDLY_INT_EN:1;
+		UINT32       	TXMAX_PTIME:8;
+		UINT32       	TXMAX_PINT:7;
+		UINT32       	TXDLY_INT_EN:1;
+	}	field;
+	UINT32			word;
+} DELAY_INT_CFG_STRUC, *PDELAY_INT_CFG_STRUC;
+#endif
+#define WMM_AIFSN_CFG   0x0214
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AIFSN_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:16;
+	    UINT32   Aifsn3:4;       // for AC_VO
+	    UINT32   Aifsn2:4;       // for AC_VI
+	    UINT32   Aifsn1:4;       // for AC_BK
+	    UINT32   Aifsn0:4;       // for AC_BE
+	}	field;
+	UINT32			word;
+}	AIFSN_CSR_STRUC, *PAIFSN_CSR_STRUC;
+#else
+typedef	union	_AIFSN_CSR_STRUC	{
+	struct	{
+	    UINT32   Aifsn0:4;       // for AC_BE
+	    UINT32   Aifsn1:4;       // for AC_BK
+	    UINT32   Aifsn2:4;       // for AC_VI
+	    UINT32   Aifsn3:4;       // for AC_VO
+	    UINT32   Rsv:16;
+	}	field;
+	UINT32			word;
+}	AIFSN_CSR_STRUC, *PAIFSN_CSR_STRUC;
+#endif
+//
+// CWMIN_CSR: CWmin for each EDCA AC
+//
+#define WMM_CWMIN_CFG   0x0218
+#ifdef RT_BIG_ENDIAN
+typedef	union	_CWMIN_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:16;
+	    UINT32   Cwmin3:4;       // for AC_VO
+	    UINT32   Cwmin2:4;       // for AC_VI
+	    UINT32   Cwmin1:4;       // for AC_BK
+	    UINT32   Cwmin0:4;       // for AC_BE
+	}	field;
+	UINT32			word;
+}	CWMIN_CSR_STRUC, *PCWMIN_CSR_STRUC;
+#else
+typedef	union	_CWMIN_CSR_STRUC	{
+	struct	{
+	    UINT32   Cwmin0:4;       // for AC_BE
+	    UINT32   Cwmin1:4;       // for AC_BK
+	    UINT32   Cwmin2:4;       // for AC_VI
+	    UINT32   Cwmin3:4;       // for AC_VO
+	    UINT32   Rsv:16;
+	}	field;
+	UINT32			word;
+}	CWMIN_CSR_STRUC, *PCWMIN_CSR_STRUC;
+#endif
+
+//
+// CWMAX_CSR: CWmin for each EDCA AC
+//
+#define WMM_CWMAX_CFG   0x021c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_CWMAX_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:16;
+	    UINT32   Cwmax3:4;       // for AC_VO
+	    UINT32   Cwmax2:4;       // for AC_VI
+	    UINT32   Cwmax1:4;       // for AC_BK
+	    UINT32   Cwmax0:4;       // for AC_BE
+	}	field;
+	UINT32			word;
+}	CWMAX_CSR_STRUC, *PCWMAX_CSR_STRUC;
+#else
+typedef	union	_CWMAX_CSR_STRUC	{
+	struct	{
+	    UINT32   Cwmax0:4;       // for AC_BE
+	    UINT32   Cwmax1:4;       // for AC_BK
+	    UINT32   Cwmax2:4;       // for AC_VI
+	    UINT32   Cwmax3:4;       // for AC_VO
+	    UINT32   Rsv:16;
+	}	field;
+	UINT32			word;
+}	CWMAX_CSR_STRUC, *PCWMAX_CSR_STRUC;
+#endif
+
+
+//
+// AC_TXOP_CSR0: AC_BK/AC_BE TXOP register
+//
+#define WMM_TXOP0_CFG    0x0220
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AC_TXOP_CSR0_STRUC	{
+	struct	{
+	    USHORT  Ac1Txop;        // for AC_BE, in unit of 32us
+	    USHORT  Ac0Txop;        // for AC_BK, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR0_STRUC, *PAC_TXOP_CSR0_STRUC;
+#else
+typedef	union	_AC_TXOP_CSR0_STRUC	{
+	struct	{
+	    USHORT  Ac0Txop;        // for AC_BK, in unit of 32us
+	    USHORT  Ac1Txop;        // for AC_BE, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR0_STRUC, *PAC_TXOP_CSR0_STRUC;
+#endif
+
+//
+// AC_TXOP_CSR1: AC_VO/AC_VI TXOP register
+//
+#define WMM_TXOP1_CFG    0x0224
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AC_TXOP_CSR1_STRUC	{
+	struct	{
+	    USHORT  Ac3Txop;        // for AC_VO, in unit of 32us
+	    USHORT  Ac2Txop;        // for AC_VI, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR1_STRUC, *PAC_TXOP_CSR1_STRUC;
+#else
+typedef	union	_AC_TXOP_CSR1_STRUC	{
+	struct	{
+	    USHORT  Ac2Txop;        // for AC_VI, in unit of 32us
+	    USHORT  Ac3Txop;        // for AC_VO, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR1_STRUC, *PAC_TXOP_CSR1_STRUC;
+#endif
+#define RINGREG_DIFF			0x10
+#define GPIO_CTRL_CFG    0x0228	//MAC_CSR13
+#define MCU_CMD_CFG    0x022c
+#define TX_BASE_PTR0     0x0230	//AC_BK base address
+#define TX_MAX_CNT0      0x0234
+#define TX_CTX_IDX0       0x0238
+#define TX_DTX_IDX0      0x023c
+#define TX_BASE_PTR1     0x0240 	//AC_BE base address
+#define TX_MAX_CNT1      0x0244
+#define TX_CTX_IDX1       0x0248
+#define TX_DTX_IDX1      0x024c
+#define TX_BASE_PTR2     0x0250 	//AC_VI base address
+#define TX_MAX_CNT2      0x0254
+#define TX_CTX_IDX2       0x0258
+#define TX_DTX_IDX2      0x025c
+#define TX_BASE_PTR3     0x0260 	//AC_VO base address
+#define TX_MAX_CNT3      0x0264
+#define TX_CTX_IDX3       0x0268
+#define TX_DTX_IDX3      0x026c
+#define TX_BASE_PTR4     0x0270 	//HCCA base address
+#define TX_MAX_CNT4      0x0274
+#define TX_CTX_IDX4       0x0278
+#define TX_DTX_IDX4      0x027c
+#define TX_BASE_PTR5     0x0280 	//MGMT base address
+#define  TX_MAX_CNT5     0x0284
+#define TX_CTX_IDX5       0x0288
+#define TX_DTX_IDX5      0x028c
+#define TX_MGMTMAX_CNT      TX_MAX_CNT5
+#define TX_MGMTCTX_IDX       TX_CTX_IDX5
+#define TX_MGMTDTX_IDX      TX_DTX_IDX5
+#define RX_BASE_PTR     0x0290 	//RX base address
+#define RX_MAX_CNT      0x0294
+#define RX_CRX_IDX       0x0298
+#define RX_DRX_IDX      0x029c
+#define USB_DMA_CFG      0x02a0
+#ifdef RT_BIG_ENDIAN
+typedef	union	_USB_DMA_CFG_STRUC	{
+	struct	{
+	    UINT32  TxBusy:1;   	//USB DMA TX FSM busy . debug only
+	    UINT32  RxBusy:1;        //USB DMA RX FSM busy . debug only
+	    UINT32  EpoutValid:6;        //OUT endpoint data valid. debug only
+	    UINT32  TxBulkEn:1;        //Enable USB DMA Tx
+	    UINT32  RxBulkEn:1;        //Enable USB DMA Rx
+	    UINT32  RxBulkAggEn:1;        //Enable Rx Bulk Aggregation
+	    UINT32  TxopHalt:1;        //Halt TXOP count down when TX buffer is full.
+	    UINT32  TxClear:1;        //Clear USB DMA TX path
+	    UINT32  rsv:2;
+	    UINT32  phyclear:1;        		//phy watch dog enable. write 1
+	    UINT32  RxBulkAggLmt:8;        //Rx Bulk Aggregation Limit  in unit of 1024 bytes
+	    UINT32  RxBulkAggTOut:8;        //Rx Bulk Aggregation TimeOut  in unit of 33ns
+	}	field;
+	UINT32			word;
+}	USB_DMA_CFG_STRUC, *PUSB_DMA_CFG_STRUC;
+#else
+typedef	union	_USB_DMA_CFG_STRUC	{
+	struct	{
+	    UINT32  RxBulkAggTOut:8;        //Rx Bulk Aggregation TimeOut  in unit of 33ns
+	    UINT32  RxBulkAggLmt:8;        //Rx Bulk Aggregation Limit  in unit of 256 bytes
+	    UINT32  phyclear:1;        		//phy watch dog enable. write 1
+	    UINT32  rsv:2;
+	    UINT32  TxClear:1;        //Clear USB DMA TX path
+	    UINT32  TxopHalt:1;        //Halt TXOP count down when TX buffer is full.
+	    UINT32  RxBulkAggEn:1;        //Enable Rx Bulk Aggregation
+	    UINT32  RxBulkEn:1;        //Enable USB DMA Rx
+	    UINT32  TxBulkEn:1;        //Enable USB DMA Tx
+	    UINT32  EpoutValid:6;        //OUT endpoint data valid
+	    UINT32  RxBusy:1;        //USB DMA RX FSM busy
+	    UINT32  TxBusy:1;   	//USB DMA TX FSM busy
+	}	field;
+	UINT32			word;
+}	USB_DMA_CFG_STRUC, *PUSB_DMA_CFG_STRUC;
+#endif
+
+//
+//  3  PBF  registers
+//
+//
+// Most are for debug. Driver doesn't touch PBF register.
+#define 	PBF_SYS_CTRL 	 0x0400
+#define     PBF_CFG                 0x0408
+#define 	PBF_MAX_PCNT 	 0x040C
+#define 	PBF_CTRL	 	0x0410
+#define 	PBF_INT_STA	 0x0414
+#define 	PBF_INT_ENA	 0x0418
+#define 	TXRXQ_PCNT  	 0x0438
+#define 	PBF_DBG 	 	 0x043c
+#define     PBF_CAP_CTRL     0x0440
+
+//
+//  4  MAC  registers
+//
+//
+//  4.1 MAC SYSTEM  configuration registers (offset:0x1000)
+//
+#define MAC_CSR0            0x1000
+#ifdef RT_BIG_ENDIAN
+typedef	union	_ASIC_VER_ID_STRUC	{
+	struct	{
+	    USHORT  ASICVer;        // version : 2860
+	    USHORT  ASICRev;        // reversion  : 0
+	}	field;
+	UINT32			word;
+}	ASIC_VER_ID_STRUC, *PASIC_VER_ID_STRUC;
+#else
+typedef	union	_ASIC_VER_ID_STRUC	{
+	struct	{
+	    USHORT  ASICRev;        // reversion  : 0
+	    USHORT  ASICVer;        // version : 2860
+	}	field;
+	UINT32			word;
+}	ASIC_VER_ID_STRUC, *PASIC_VER_ID_STRUC;
+#endif
+#define MAC_SYS_CTRL            0x1004		//MAC_CSR1
+#define MAC_ADDR_DW0            		0x1008		// MAC ADDR DW0
+#define MAC_ADDR_DW1           		 0x100c		// MAC ADDR DW1
+//
+// MAC_CSR2: STA MAC register 0
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MAC_DW0_STRUC	{
+	struct	{
+		UCHAR		Byte3;		// MAC address byte 3
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte0;		// MAC address byte 0
+	}	field;
+	UINT32			word;
+}	MAC_DW0_STRUC, *PMAC_DW0_STRUC;
+#else
+typedef	union	_MAC_DW0_STRUC	{
+	struct	{
+		UCHAR		Byte0;		// MAC address byte 0
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte3;		// MAC address byte 3
+	}	field;
+	UINT32			word;
+}	MAC_DW0_STRUC, *PMAC_DW0_STRUC;
+#endif
+
+//
+// MAC_CSR3: STA MAC register 1
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MAC_DW1_STRUC	{
+	struct	{
+		UCHAR		Rsvd1;
+		UCHAR		U2MeMask;
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		Byte4;		// MAC address byte 4
+	}	field;
+	UINT32			word;
+}	MAC_DW1_STRUC, *PMAC_DW1_STRUC;
+#else
+typedef	union	_MAC_DW1_STRUC	{
+	struct	{
+		UCHAR		Byte4;		// MAC address byte 4
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		U2MeMask;
+		UCHAR		Rsvd1;
+	}	field;
+	UINT32			word;
+}	MAC_DW1_STRUC, *PMAC_DW1_STRUC;
+#endif
+
+#define MAC_BSSID_DW0            		0x1010		// MAC BSSID DW0
+#define MAC_BSSID_DW1            		0x1014		// MAC BSSID DW1
+
+//
+// MAC_CSR5: BSSID register 1
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MAC_CSR5_STRUC	{
+	struct	{
+		USHORT		Rsvd:11;
+		USHORT		MBssBcnNum:3;
+		USHORT		BssIdMode:2; // 0: one BSSID, 10: 4 BSSID,  01: 2 BSSID , 11: 8BSSID
+		UCHAR		Byte5;		 // BSSID byte 5
+		UCHAR		Byte4;		 // BSSID byte 4
+	}	field;
+	UINT32			word;
+}	MAC_CSR5_STRUC, *PMAC_CSR5_STRUC;
+#else
+typedef	union	_MAC_CSR5_STRUC	{
+	struct	{
+		UCHAR		Byte4;		 // BSSID byte 4
+		UCHAR		Byte5;		 // BSSID byte 5
+		USHORT      	BssIdMask:2; // 0: one BSSID, 10: 4 BSSID,  01: 2 BSSID , 11: 8BSSID
+		USHORT		MBssBcnNum:3;
+		USHORT		Rsvd:11;
+	}	field;
+	UINT32			word;
+}	MAC_CSR5_STRUC, *PMAC_CSR5_STRUC;
+#endif
+
+#define MAX_LEN_CFG              0x1018		// rt2860b max 16k bytes. bit12:13 Maximum PSDU length (power factor) 0:2^13, 1:2^14, 2:2^15, 3:2^16
+#define BBP_CSR_CFG            		0x101c		//
+//
+// BBP_CSR_CFG: BBP serial control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_BBP_CSR_CFG_STRUC	{
+	struct	{
+		UINT32		:12;
+		UINT32		BBP_RW_MODE:1;		// 0: use serial mode  1:parallel
+		UINT32		BBP_PAR_DUR:1;		    // 0: 4 MAC clock cycles  1: 8 MAC clock cycles
+		UINT32		Busy:1;				// 1: ASIC is busy execute BBP programming.
+		UINT32		fRead:1;		    // 0: Write	BBP, 1:	Read BBP
+		UINT32		RegNum:8;			// Selected	BBP	register
+		UINT32		Value:8;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	BBP_CSR_CFG_STRUC, *PBBP_CSR_CFG_STRUC;
+#else
+typedef	union	_BBP_CSR_CFG_STRUC	{
+	struct	{
+		UINT32		Value:8;			// Register	value to program into BBP
+		UINT32		RegNum:8;			// Selected	BBP	register
+		UINT32		fRead:1;		    // 0: Write	BBP, 1:	Read BBP
+		UINT32		Busy:1;				// 1: ASIC is busy execute BBP programming.
+		UINT32		BBP_PAR_DUR:1;		     // 0: 4 MAC clock cycles  1: 8 MAC clock cycles
+		UINT32		BBP_RW_MODE:1;		// 0: use serial mode  1:parallel
+		UINT32		:12;
+	}	field;
+	UINT32			word;
+}	BBP_CSR_CFG_STRUC, *PBBP_CSR_CFG_STRUC;
+#endif
+#define RF_CSR_CFG0            		0x1020
+//
+// RF_CSR_CFG: RF control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG0_STRUC	{
+	struct	{
+		UINT32		Busy:1;		    // 0: idle 1: 8busy
+		UINT32		Sel:1;				// 0:RF_LE0 activate  1:RF_LE1 activate
+		UINT32		StandbyMode:1;		    // 0: high when stand by 1:	low when standby
+		UINT32		bitwidth:5;			// Selected	BBP	register
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG0_STRUC, *PRF_CSR_CFG0_STRUC;
+#else
+typedef	union	_RF_CSR_CFG0_STRUC	{
+	struct	{
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+		UINT32		bitwidth:5;			// Selected	BBP	register
+		UINT32		StandbyMode:1;		    // 0: high when stand by 1:	low when standby
+		UINT32		Sel:1;				// 0:RF_LE0 activate  1:RF_LE1 activate
+		UINT32		Busy:1;		    // 0: idle 1: 8busy
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG0_STRUC, *PRF_CSR_CFG0_STRUC;
+#endif
+#define RF_CSR_CFG1           		0x1024
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG1_STRUC	{
+	struct	{
+		UINT32		rsv:7;		    // 0: idle 1: 8busy
+		UINT32		RFGap:5;			// Gap between BB_CONTROL_RF and RF_LE. 0: 3 system clock cycle (37.5usec) 1: 5 system clock cycle (62.5usec)
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG1_STRUC, *PRF_CSR_CFG1_STRUC;
+#else
+typedef	union	_RF_CSR_CFG1_STRUC	{
+	struct	{
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+		UINT32		RFGap:5;			// Gap between BB_CONTROL_RF and RF_LE. 0: 3 system clock cycle (37.5usec) 1: 5 system clock cycle (62.5usec)
+		UINT32		rsv:7;		    // 0: idle 1: 8busy
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG1_STRUC, *PRF_CSR_CFG1_STRUC;
+#endif
+#define RF_CSR_CFG2           		0x1028		//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG2_STRUC	{
+	struct	{
+		UINT32		rsv:8;		    // 0: idle 1: 8busy
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG2_STRUC, *PRF_CSR_CFG2_STRUC;
+#else
+typedef	union	_RF_CSR_CFG2_STRUC	{
+	struct	{
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+		UINT32		rsv:8;		    // 0: idle 1: 8busy
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG2_STRUC, *PRF_CSR_CFG2_STRUC;
+#endif
+#define LED_CFG           		0x102c		//  MAC_CSR14
+#ifdef RT_BIG_ENDIAN
+typedef	union	_LED_CFG_STRUC	{
+	struct	{
+		UINT32		:1;
+		UINT32		LedPolar:1;			// Led Polarity.  0: active low1: active high
+		UINT32		YLedMode:2;			// yellow Led Mode
+		UINT32		GLedMode:2;			// green Led Mode
+		UINT32		RLedMode:2;			// red Led Mode    0: off1: blinking upon TX2: periodic slow blinking3: always on
+		UINT32		rsv:2;
+		UINT32		SlowBlinkPeriod:6;			// slow blinking period. unit:1ms
+		UINT32		OffPeriod:8;			// blinking off period unit 1ms
+		UINT32		OnPeriod:8;			// blinking on period unit 1ms
+	}	field;
+	UINT32			word;
+}	LED_CFG_STRUC, *PLED_CFG_STRUC;
+#else
+typedef	union	_LED_CFG_STRUC	{
+	struct	{
+		UINT32		OnPeriod:8;			// blinking on period unit 1ms
+		UINT32		OffPeriod:8;			// blinking off period unit 1ms
+		UINT32		SlowBlinkPeriod:6;			// slow blinking period. unit:1ms
+		UINT32		rsv:2;
+		UINT32		RLedMode:2;			// red Led Mode    0: off1: blinking upon TX2: periodic slow blinking3: always on
+		UINT32		GLedMode:2;			// green Led Mode
+		UINT32		YLedMode:2;			// yellow Led Mode
+		UINT32		LedPolar:1;			// Led Polarity.  0: active low1: active high
+		UINT32		:1;
+	}	field;
+	UINT32			word;
+}	LED_CFG_STRUC, *PLED_CFG_STRUC;
+#endif
+//
+//  4.2 MAC TIMING  configuration registers (offset:0x1100)
+//
+#define XIFS_TIME_CFG             0x1100		 // MAC_CSR8  MAC_CSR9
+#ifdef RT_BIG_ENDIAN
+typedef	union	_IFS_SLOT_CFG_STRUC	{
+	struct	{
+	    UINT32  rsv:2;
+	    UINT32  BBRxendEnable:1;        //  reference RXEND signal to begin XIFS defer
+	    UINT32  EIFS:9;        //  unit 1us
+	    UINT32  OfdmXifsTime:4;        //OFDM SIFS. unit 1us. Applied after OFDM RX when MAC doesn't reference BBP signal BBRXEND
+	    UINT32  OfdmSifsTime:8;        //  unit 1us. Applied after OFDM RX/TX
+	    UINT32  CckmSifsTime:8;        //  unit 1us. Applied after CCK RX/TX
+	}	field;
+	UINT32			word;
+}	IFS_SLOT_CFG_STRUC, *PIFS_SLOT_CFG_STRUC;
+#else
+typedef	union	_IFS_SLOT_CFG_STRUC	{
+	struct	{
+	    UINT32  CckmSifsTime:8;        //  unit 1us. Applied after CCK RX/TX
+	    UINT32  OfdmSifsTime:8;        //  unit 1us. Applied after OFDM RX/TX
+	    UINT32  OfdmXifsTime:4;        //OFDM SIFS. unit 1us. Applied after OFDM RX when MAC doesn't reference BBP signal BBRXEND
+	    UINT32  EIFS:9;        //  unit 1us
+	    UINT32  BBRxendEnable:1;        //  reference RXEND signal to begin XIFS defer
+	    UINT32  rsv:2;
+	}	field;
+	UINT32			word;
+}	IFS_SLOT_CFG_STRUC, *PIFS_SLOT_CFG_STRUC;
+#endif
+
+#define BKOFF_SLOT_CFG             0x1104		 //  mac_csr9 last 8 bits
+#define NAV_TIME_CFG             0x1108		 // NAV  (MAC_CSR15)
+#define CH_TIME_CFG             0x110C		 	// Count as channel busy
+#define PBF_LIFE_TIMER             0x1110		 //TX/RX MPDU timestamp timer (free run)Unit: 1us
+#define BCN_TIME_CFG             0x1114		 // TXRX_CSR9
+
+#define BCN_OFFSET0				0x042C
+#define BCN_OFFSET1				0x0430
+
+//
+// BCN_TIME_CFG : Synchronization control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_BCN_TIME_CFG_STRUC	{
+	struct	{
+		UINT32		TxTimestampCompensate:8;
+        UINT32       :3;
+		UINT32		bBeaconGen:1;		// Enable beacon generator
+        UINT32       bTBTTEnable:1;
+		UINT32		TsfSyncMode:2;		// Enable TSF sync, 00: disable, 01: infra mode, 10: ad-hoc mode
+		UINT32		bTsfTicking:1;		// Enable TSF auto counting
+		UINT32       BeaconInterval:16;  // in unit of 1/16 TU
+	}	field;
+	UINT32			word;
+}	BCN_TIME_CFG_STRUC, *PBCN_TIME_CFG_STRUC;
+#else
+typedef	union	_BCN_TIME_CFG_STRUC	{
+	struct	{
+		UINT32       BeaconInterval:16;  // in unit of 1/16 TU
+		UINT32		bTsfTicking:1;		// Enable TSF auto counting
+		UINT32		TsfSyncMode:2;		// Enable TSF sync, 00: disable, 01: infra mode, 10: ad-hoc mode
+        UINT32       bTBTTEnable:1;
+		UINT32		bBeaconGen:1;		// Enable beacon generator
+        UINT32       :3;
+		UINT32		TxTimestampCompensate:8;
+	}	field;
+	UINT32			word;
+}	BCN_TIME_CFG_STRUC, *PBCN_TIME_CFG_STRUC;
+#endif
+#define TBTT_SYNC_CFG            0x1118  		// txrx_csr10
+#define TSF_TIMER_DW0             0x111C  		// Local TSF timer lsb 32 bits. Read-only
+#define TSF_TIMER_DW1             0x1120  		// msb 32 bits. Read-only.
+#define TBTT_TIMER             	0x1124  		// TImer remains till next TBTT. Read-only.  TXRX_CSR14
+#define INT_TIMER_CFG              	0x1128  		//
+#define INT_TIMER_EN             	0x112c  		//  GP-timer and pre-tbtt Int enable
+#define CH_IDLE_STA              	0x1130  		//  channel idle time
+#define CH_BUSY_STA              	0x1134  		//  channle busy time
+//
+//  4.2 MAC POWER  configuration registers (offset:0x1200)
+//
+#define MAC_STATUS_CFG             0x1200		 // old MAC_CSR12
+#define PWR_PIN_CFG             0x1204		 // old MAC_CSR12
+#define AUTO_WAKEUP_CFG             0x1208		 // old MAC_CSR10
+//
+// AUTO_WAKEUP_CFG: Manual power control / status register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AUTO_WAKEUP_STRUC	{
+	struct	{
+		UINT32		:16;
+		UINT32		EnableAutoWakeup:1;	// 0:sleep, 1:awake
+		UINT32       NumofSleepingTbtt:7;          // ForceWake has high privilege than PutToSleep when both set
+		UINT32       AutoLeadTime:8;
+	}	field;
+	UINT32			word;
+}	AUTO_WAKEUP_STRUC, *PAUTO_WAKEUP_STRUC;
+#else
+typedef	union	_AUTO_WAKEUP_STRUC	{
+	struct	{
+		UINT32       AutoLeadTime:8;
+		UINT32       NumofSleepingTbtt:7;          // ForceWake has high privilege than PutToSleep when both set
+		UINT32		EnableAutoWakeup:1;	// 0:sleep, 1:awake
+		UINT32		:16;
+	}	field;
+	UINT32			word;
+}	AUTO_WAKEUP_STRUC, *PAUTO_WAKEUP_STRUC;
+#endif
+//
+//  4.3 MAC TX  configuration registers (offset:0x1300)
+//
+
+#define EDCA_AC0_CFG	0x1300		//AC_TXOP_CSR0 0x3474
+#define EDCA_AC1_CFG	0x1304
+#define EDCA_AC2_CFG	0x1308
+#define EDCA_AC3_CFG	0x130c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EDCA_AC_CFG_STRUC	{
+	struct	{
+	    UINT32  :12;        //
+	    UINT32  Cwmax:4;        //unit power of 2
+	    UINT32  Cwmin:4;        //
+	    UINT32  Aifsn:4;        // # of slot time
+	    UINT32  AcTxop:8;        //  in unit of 32us
+	}	field;
+	UINT32			word;
+}	EDCA_AC_CFG_STRUC, *PEDCA_AC_CFG_STRUC;
+#else
+typedef	union	_EDCA_AC_CFG_STRUC	{
+	struct	{
+	    UINT32  AcTxop:8;        //  in unit of 32us
+	    UINT32  Aifsn:4;        // # of slot time
+	    UINT32  Cwmin:4;        //
+	    UINT32  Cwmax:4;        //unit power of 2
+	    UINT32  :12;       //
+	}	field;
+	UINT32			word;
+}	EDCA_AC_CFG_STRUC, *PEDCA_AC_CFG_STRUC;
+#endif
+
+#define EDCA_TID_AC_MAP	0x1310
+#define TX_PWR_CFG_0	0x1314
+#define TX_PWR_CFG_1	0x1318
+#define TX_PWR_CFG_2	0x131C
+#define TX_PWR_CFG_3	0x1320
+#define TX_PWR_CFG_4	0x1324
+#define TX_PIN_CFG		0x1328
+#define TX_BAND_CFG	0x132c		// 0x1 use upper 20MHz. 0 juse lower 20MHz
+#define TX_SW_CFG0		0x1330
+#define TX_SW_CFG1		0x1334
+#define TX_SW_CFG2		0x1338
+#define TXOP_THRES_CFG		0x133c
+#define TXOP_CTRL_CFG		0x1340
+#define TX_RTS_CFG		0x1344
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_RTS_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:7;
+	    UINT32       RtsFbkEn:1;    // enable rts rate fallback
+	    UINT32       RtsThres:16;    // unit:byte
+	    UINT32       AutoRtsRetryLimit:8;
+	}	field;
+	UINT32			word;
+}	TX_RTS_CFG_STRUC, *PTX_RTS_CFG_STRUC;
+#else
+typedef	union	_TX_RTS_CFG_STRUC	{
+	struct	{
+	    UINT32       AutoRtsRetryLimit:8;
+	    UINT32       RtsThres:16;    // unit:byte
+	    UINT32       RtsFbkEn:1;    // enable rts rate fallback
+	    UINT32       rsv:7;     // 1: HT non-STBC control frame enable
+	}	field;
+	UINT32			word;
+}	TX_RTS_CFG_STRUC, *PTX_RTS_CFG_STRUC;
+#endif
+#define TX_TIMEOUT_CFG	0x1348
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_TIMEOUT_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv2:8;
+	    UINT32       TxopTimeout:8;	//TXOP timeout value for TXOP truncation.  It is recommended that (SLOT_TIME) > (TX_OP_TIMEOUT) > (RX_ACK_TIMEOUT)
+	    UINT32       RxAckTimeout:8;	// unit:slot. Used for TX precedure
+	    UINT32       MpduLifeTime:4;    //  expiration time = 2^(9+MPDU LIFE TIME)  us
+	    UINT32       rsv:4;
+	}	field;
+	UINT32			word;
+}	TX_TIMEOUT_CFG_STRUC, *PTX_TIMEOUT_CFG_STRUC;
+#else
+typedef	union	_TX_TIMEOUT_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:4;
+	    UINT32       MpduLifeTime:4;    //  expiration time = 2^(9+MPDU LIFE TIME)  us
+	    UINT32       RxAckTimeout:8;	// unit:slot. Used for TX precedure
+	    UINT32       TxopTimeout:8;	//TXOP timeout value for TXOP truncation.  It is recommended that (SLOT_TIME) > (TX_OP_TIMEOUT) > (RX_ACK_TIMEOUT)
+	    UINT32       rsv2:8;     // 1: HT non-STBC control frame enable
+	}	field;
+	UINT32			word;
+}	TX_TIMEOUT_CFG_STRUC, *PTX_TIMEOUT_CFG_STRUC;
+#endif
+#define TX_RTY_CFG	0x134c
+#ifdef RT_BIG_ENDIAN
+typedef	union PACKED _TX_RTY_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:1;
+	    UINT32       TxautoFBEnable:1;    // Tx retry PHY rate auto fallback enable
+	    UINT32       AggRtyMode:1;	// Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       NonAggRtyMode:1;	// Non-Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       LongRtyThre:12;	// Long retry threshoold
+	    UINT32       LongRtyLimit:8;	//long retry limit
+	    UINT32       ShortRtyLimit:8;	//  short retry limit
+
+	}	field;
+	UINT32			word;
+}	TX_RTY_CFG_STRUC, *PTX_RTY_CFG_STRUC;
+#else
+typedef	union PACKED _TX_RTY_CFG_STRUC	{
+	struct	{
+	    UINT32       ShortRtyLimit:8;	//  short retry limit
+	    UINT32       LongRtyLimit:8;	//long retry limit
+	    UINT32       LongRtyThre:12;	// Long retry threshoold
+	    UINT32       NonAggRtyMode:1;	// Non-Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       AggRtyMode:1;	// Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       TxautoFBEnable:1;    // Tx retry PHY rate auto fallback enable
+	    UINT32       rsv:1;     // 1: HT non-STBC control frame enable
+	}	field;
+	UINT32			word;
+}	TX_RTY_CFG_STRUC, *PTX_RTY_CFG_STRUC;
+#endif
+#define TX_LINK_CFG	0x1350
+#ifdef RT_BIG_ENDIAN
+typedef	union	PACKED _TX_LINK_CFG_STRUC	{
+	struct PACKED {
+	    UINT32       RemotMFS:8;	//remote MCS feedback sequence number
+	    UINT32       RemotMFB:8;    //  remote MCS feedback
+	    UINT32       rsv:3;	//
+	    UINT32       TxCFAckEn:1;	//   Piggyback CF-ACK enable
+	    UINT32       TxRDGEn:1;	// RDG TX enable
+	    UINT32       TxMRQEn:1;	//  MCS request TX enable
+	    UINT32       RemoteUMFSEnable:1;	//  remote unsolicit  MFB enable.  0: not apply remote remote unsolicit (MFS=7)
+	    UINT32       MFBEnable:1;	//  TX apply remote MFB 1:enable
+	    UINT32       RemoteMFBLifeTime:8;	//remote MFB life time. unit : 32us
+	}	field;
+	UINT32			word;
+}	TX_LINK_CFG_STRUC, *PTX_LINK_CFG_STRUC;
+#else
+typedef	union	PACKED _TX_LINK_CFG_STRUC	{
+	struct PACKED {
+	    UINT32       RemoteMFBLifeTime:8;	//remote MFB life time. unit : 32us
+	    UINT32       MFBEnable:1;	//  TX apply remote MFB 1:enable
+	    UINT32       RemoteUMFSEnable:1;	//  remote unsolicit  MFB enable.  0: not apply remote remote unsolicit (MFS=7)
+	    UINT32       TxMRQEn:1;	//  MCS request TX enable
+	    UINT32       TxRDGEn:1;	// RDG TX enable
+	    UINT32       TxCFAckEn:1;	//   Piggyback CF-ACK enable
+	    UINT32       rsv:3;	//
+	    UINT32       RemotMFB:8;    //  remote MCS feedback
+	    UINT32       RemotMFS:8;	//remote MCS feedback sequence number
+	}	field;
+	UINT32			word;
+}	TX_LINK_CFG_STRUC, *PTX_LINK_CFG_STRUC;
+#endif
+#define HT_FBK_CFG0	0x1354
+#ifdef RT_BIG_ENDIAN
+typedef	union PACKED _HT_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       HTMCS7FBK:4;
+	    UINT32       HTMCS6FBK:4;
+	    UINT32       HTMCS5FBK:4;
+	    UINT32       HTMCS4FBK:4;
+	    UINT32       HTMCS3FBK:4;
+	    UINT32       HTMCS2FBK:4;
+	    UINT32       HTMCS1FBK:4;
+	    UINT32       HTMCS0FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG0_STRUC, *PHT_FBK_CFG0_STRUC;
+#else
+typedef	union PACKED _HT_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       HTMCS0FBK:4;
+	    UINT32       HTMCS1FBK:4;
+	    UINT32       HTMCS2FBK:4;
+	    UINT32       HTMCS3FBK:4;
+	    UINT32       HTMCS4FBK:4;
+	    UINT32       HTMCS5FBK:4;
+	    UINT32       HTMCS6FBK:4;
+	    UINT32       HTMCS7FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG0_STRUC, *PHT_FBK_CFG0_STRUC;
+#endif
+#define HT_FBK_CFG1	0x1358
+#ifdef RT_BIG_ENDIAN
+typedef	union	_HT_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       HTMCS15FBK:4;
+	    UINT32       HTMCS14FBK:4;
+	    UINT32       HTMCS13FBK:4;
+	    UINT32       HTMCS12FBK:4;
+	    UINT32       HTMCS11FBK:4;
+	    UINT32       HTMCS10FBK:4;
+	    UINT32       HTMCS9FBK:4;
+	    UINT32       HTMCS8FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG1_STRUC, *PHT_FBK_CFG1_STRUC;
+#else
+typedef	union	_HT_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       HTMCS8FBK:4;
+	    UINT32       HTMCS9FBK:4;
+	    UINT32       HTMCS10FBK:4;
+	    UINT32       HTMCS11FBK:4;
+	    UINT32       HTMCS12FBK:4;
+	    UINT32       HTMCS13FBK:4;
+	    UINT32       HTMCS14FBK:4;
+	    UINT32       HTMCS15FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG1_STRUC, *PHT_FBK_CFG1_STRUC;
+#endif
+#define LG_FBK_CFG0	0x135c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_LG_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       OFDMMCS7FBK:4;	//initial value is 6
+	    UINT32       OFDMMCS6FBK:4;	//initial value is 5
+	    UINT32       OFDMMCS5FBK:4;	//initial value is 4
+	    UINT32       OFDMMCS4FBK:4;	//initial value is 3
+	    UINT32       OFDMMCS3FBK:4;	//initial value is 2
+	    UINT32       OFDMMCS2FBK:4;	//initial value is 1
+	    UINT32       OFDMMCS1FBK:4;	//initial value is 0
+	    UINT32       OFDMMCS0FBK:4;	//initial value is 0
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG0_STRUC, *PLG_FBK_CFG0_STRUC;
+#else
+typedef	union	_LG_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       OFDMMCS0FBK:4;	//initial value is 0
+	    UINT32       OFDMMCS1FBK:4;	//initial value is 0
+	    UINT32       OFDMMCS2FBK:4;	//initial value is 1
+	    UINT32       OFDMMCS3FBK:4;	//initial value is 2
+	    UINT32       OFDMMCS4FBK:4;	//initial value is 3
+	    UINT32       OFDMMCS5FBK:4;	//initial value is 4
+	    UINT32       OFDMMCS6FBK:4;	//initial value is 5
+	    UINT32       OFDMMCS7FBK:4;	//initial value is 6
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG0_STRUC, *PLG_FBK_CFG0_STRUC;
+#endif
+#define LG_FBK_CFG1		0x1360
+#ifdef RT_BIG_ENDIAN
+typedef	union	_LG_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       rsv:16;
+	    UINT32       CCKMCS3FBK:4;	//initial value is 2
+	    UINT32       CCKMCS2FBK:4;	//initial value is 1
+	    UINT32       CCKMCS1FBK:4;	//initial value is 0
+	    UINT32       CCKMCS0FBK:4;	//initial value is 0
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG1_STRUC, *PLG_FBK_CFG1_STRUC;
+#else
+typedef	union	_LG_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       CCKMCS0FBK:4;	//initial value is 0
+	    UINT32       CCKMCS1FBK:4;	//initial value is 0
+	    UINT32       CCKMCS2FBK:4;	//initial value is 1
+	    UINT32       CCKMCS3FBK:4;	//initial value is 2
+	    UINT32       rsv:16;
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG1_STRUC, *PLG_FBK_CFG1_STRUC;
+#endif
+
+//=======================================================
+//================ Protection Paramater================================
+//=======================================================
+#define CCK_PROT_CFG	0x1364		//CCK Protection
+#define ASIC_SHORTNAV		1
+#define ASIC_LONGNAV		2
+#define ASIC_RTS		1
+#define ASIC_CTS		2
+#ifdef RT_BIG_ENDIAN
+typedef	union	_PROT_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:5;
+	    UINT32       RTSThEn:1;	//RTS threshold enable on CCK TX
+	    UINT32       TxopAllowGF40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowGF20:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowMM40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowMM20:1;	//CCK TXOP allowance. 0:disallow.
+	    UINT32       TxopAllowOfdm:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowCck:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       ProtectNav:2;	//TXOP protection type for CCK TX. 0:None, 1:ShortNAVprotect,  2:LongNAVProtect, 3:rsv
+	    UINT32       ProtectCtrl:2;	//Protection control frame type for CCK TX. 1:RTS/CTS, 2:CTS-to-self, 0:None, 3:rsv
+	    UINT32       ProtectRate:16;	//Protection control frame rate for CCK TX(RTS/CTS/CFEnd).
+	}	field;
+	UINT32			word;
+}	PROT_CFG_STRUC, *PPROT_CFG_STRUC;
+#else
+typedef	union	_PROT_CFG_STRUC	{
+	struct	{
+	    UINT32       ProtectRate:16;	//Protection control frame rate for CCK TX(RTS/CTS/CFEnd).
+	    UINT32       ProtectCtrl:2;	//Protection control frame type for CCK TX. 1:RTS/CTS, 2:CTS-to-self, 0:None, 3:rsv
+	    UINT32       ProtectNav:2;	//TXOP protection type for CCK TX. 0:None, 1:ShortNAVprotect,  2:LongNAVProtect, 3:rsv
+	    UINT32       TxopAllowCck:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowOfdm:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowMM20:1;	//CCK TXOP allowance. 0:disallow.
+	    UINT32       TxopAllowMM40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowGF20:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowGF40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       RTSThEn:1;	//RTS threshold enable on CCK TX
+	    UINT32       rsv:5;
+	}	field;
+	UINT32			word;
+}	PROT_CFG_STRUC, *PPROT_CFG_STRUC;
+#endif
+
+#define OFDM_PROT_CFG	0x1368		//OFDM Protection
+#define MM20_PROT_CFG	0x136C		//MM20 Protection
+#define MM40_PROT_CFG	0x1370		//MM40 Protection
+#define GF20_PROT_CFG	0x1374		//GF20 Protection
+#define GF40_PROT_CFG	0x1378		//GR40 Protection
+#define EXP_CTS_TIME	0x137C		//
+#define EXP_ACK_TIME	0x1380		//
+
+//
+//  4.4 MAC RX configuration registers (offset:0x1400)
+//
+#define RX_FILTR_CFG	0x1400			//TXRX_CSR0
+#define AUTO_RSP_CFG	0x1404			//TXRX_CSR4
+//
+// TXRX_CSR4: Auto-Responder/
+//
+#ifdef RT_BIG_ENDIAN
+typedef union _AUTO_RSP_CFG_STRUC {
+ struct {
+     UINT32        :24;
+     UINT32       AckCtsPsmBit:1;   // Power bit value in conrtrol frame
+     UINT32       DualCTSEn:1;   // Power bit value in conrtrol frame
+     UINT32       rsv:1;   // Power bit value in conrtrol frame
+     UINT32       AutoResponderPreamble:1;    // 0:long, 1:short preamble
+     UINT32       CTS40MRef:1;  // Response CTS 40MHz duplicate mode
+     UINT32       CTS40MMode:1;  // Response CTS 40MHz duplicate mode
+     UINT32       BACAckPolicyEnable:1;    // 0:long, 1:short preamble
+     UINT32       AutoResponderEnable:1;
+ } field;
+ UINT32   word;
+} AUTO_RSP_CFG_STRUC, *PAUTO_RSP_CFG_STRUC;
+#else
+typedef union _AUTO_RSP_CFG_STRUC {
+ struct {
+     UINT32       AutoResponderEnable:1;
+     UINT32       BACAckPolicyEnable:1;    // 0:long, 1:short preamble
+     UINT32       CTS40MMode:1;  // Response CTS 40MHz duplicate mode
+     UINT32       CTS40MRef:1;  // Response CTS 40MHz duplicate mode
+     UINT32       AutoResponderPreamble:1;    // 0:long, 1:short preamble
+     UINT32       rsv:1;   // Power bit value in conrtrol frame
+     UINT32       DualCTSEn:1;   // Power bit value in conrtrol frame
+     UINT32       AckCtsPsmBit:1;   // Power bit value in conrtrol frame
+     UINT32        :24;
+ } field;
+ UINT32   word;
+} AUTO_RSP_CFG_STRUC, *PAUTO_RSP_CFG_STRUC;
+#endif
+
+#define LEGACY_BASIC_RATE	0x1408	//  TXRX_CSR5           0x3054
+#define HT_BASIC_RATE		0x140c
+#define HT_CTRL_CFG		0x1410
+#define SIFS_COST_CFG		0x1414
+#define RX_PARSER_CFG		0x1418	//Set NAV for all received frames
+
+//
+//  4.5 MAC Security configuration (offset:0x1500)
+//
+#define TX_SEC_CNT0		0x1500		//
+#define RX_SEC_CNT0		0x1504		//
+#define CCMP_FC_MUTE		0x1508		//
+//
+//  4.6 HCCA/PSMP (offset:0x1600)
+//
+#define TXOP_HLDR_ADDR0		0x1600
+#define TXOP_HLDR_ADDR1		0x1604
+#define TXOP_HLDR_ET		0x1608
+#define QOS_CFPOLL_RA_DW0		0x160c
+#define QOS_CFPOLL_A1_DW1		0x1610
+#define QOS_CFPOLL_QC		0x1614
+//
+//  4.7 MAC Statistis registers (offset:0x1700)
+//
+#define RX_STA_CNT0		0x1700		//
+#define RX_STA_CNT1		0x1704		//
+#define RX_STA_CNT2		0x1708		//
+
+//
+// RX_STA_CNT0_STRUC: RX PLCP error count & RX CRC error count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  PhyErr;
+	    USHORT  CrcErr;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT0_STRUC, *PRX_STA_CNT0_STRUC;
+#else
+typedef	union	_RX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  CrcErr;
+	    USHORT  PhyErr;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT0_STRUC, *PRX_STA_CNT0_STRUC;
+#endif
+
+//
+// RX_STA_CNT1_STRUC: RX False CCA count & RX LONG frame count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  PlcpErr;
+	    USHORT  FalseCca;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT1_STRUC, *PRX_STA_CNT1_STRUC;
+#else
+typedef	union	_RX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  FalseCca;
+	    USHORT  PlcpErr;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT1_STRUC, *PRX_STA_CNT1_STRUC;
+#endif
+
+//
+// RX_STA_CNT2_STRUC:
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  RxFifoOverflowCount;
+	    USHORT  RxDupliCount;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT2_STRUC, *PRX_STA_CNT2_STRUC;
+#else
+typedef	union	_RX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  RxDupliCount;
+	    USHORT  RxFifoOverflowCount;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT2_STRUC, *PRX_STA_CNT2_STRUC;
+#endif
+#define TX_STA_CNT0		0x170C		//
+//
+// STA_CSR3: TX Beacon count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  TxBeaconCount;
+	    USHORT  TxFailCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT0_STRUC, *PTX_STA_CNT0_STRUC;
+#else
+typedef	union	_TX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  TxFailCount;
+	    USHORT  TxBeaconCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT0_STRUC, *PTX_STA_CNT0_STRUC;
+#endif
+#define TX_STA_CNT1		0x1710		//
+//
+// TX_STA_CNT1: TX tx count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  TxRetransmit;
+	    USHORT  TxSuccess;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT1_STRUC, *PTX_STA_CNT1_STRUC;
+#else
+typedef	union	_TX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  TxSuccess;
+	    USHORT  TxRetransmit;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT1_STRUC, *PTX_STA_CNT1_STRUC;
+#endif
+#define TX_STA_CNT2		0x1714		//
+//
+// TX_STA_CNT2: TX tx count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  TxUnderFlowCount;
+	    USHORT  TxZeroLenCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT2_STRUC, *PTX_STA_CNT2_STRUC;
+#else
+typedef	union	_TX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  TxZeroLenCount;
+	    USHORT  TxUnderFlowCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT2_STRUC, *PTX_STA_CNT2_STRUC;
+#endif
+#define TX_STA_FIFO		0x1718		//
+//
+// TX_STA_FIFO_STRUC: TX Result for specific PID status fifo register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union PACKED _TX_STA_FIFO_STRUC	{
+	struct	{
+		UINT32		Reserve:2;
+		UINT32		TxBF:1; // 3*3
+		UINT32		SuccessRate:13;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+//		UINT32		SuccessRate:16;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+		UINT32		wcid:8;		//wireless client index
+		UINT32       	TxAckRequired:1;    // ack required
+		UINT32       	TxAggre:1;    // Tx is aggregated
+		UINT32       	TxSuccess:1;   // Tx success. whether success or not
+		UINT32       	PidType:4;
+		UINT32       	bValid:1;   // 1:This register contains a valid TX result
+	}	field;
+	UINT32			word;
+}	TX_STA_FIFO_STRUC, *PTX_STA_FIFO_STRUC;
+#else
+typedef	union PACKED _TX_STA_FIFO_STRUC	{
+	struct	{
+		UINT32       	bValid:1;   // 1:This register contains a valid TX result
+		UINT32       	PidType:4;
+		UINT32       	TxSuccess:1;   // Tx No retry success
+		UINT32       	TxAggre:1;    // Tx Retry Success
+		UINT32       	TxAckRequired:1;    // Tx fail
+		UINT32		wcid:8;		//wireless client index
+//		UINT32		SuccessRate:16;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+		UINT32		SuccessRate:13;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+		UINT32		TxBF:1;
+		UINT32		Reserve:2;
+	}	field;
+	UINT32			word;
+}	TX_STA_FIFO_STRUC, *PTX_STA_FIFO_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT	0x171c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT_STRUC	{
+	struct	{
+	    USHORT  AggTxCount;
+	    USHORT  NonAggTxCount;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT_STRUC, *PTX_AGG_CNT_STRUC;
+#else
+typedef	union	_TX_AGG_CNT_STRUC	{
+	struct	{
+	    USHORT  NonAggTxCount;
+	    USHORT  AggTxCount;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT_STRUC, *PTX_AGG_CNT_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT0	0x1720
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT0_STRUC	{
+	struct	{
+	    USHORT  AggSize2Count;
+	    USHORT  AggSize1Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT0_STRUC, *PTX_AGG_CNT0_STRUC;
+#else
+typedef	union	_TX_AGG_CNT0_STRUC	{
+	struct	{
+	    USHORT  AggSize1Count;
+	    USHORT  AggSize2Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT0_STRUC, *PTX_AGG_CNT0_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT1	0x1724
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT1_STRUC	{
+	struct	{
+	    USHORT  AggSize4Count;
+	    USHORT  AggSize3Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT1_STRUC, *PTX_AGG_CNT1_STRUC;
+#else
+typedef	union	_TX_AGG_CNT1_STRUC	{
+	struct	{
+	    USHORT  AggSize3Count;
+	    USHORT  AggSize4Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT1_STRUC, *PTX_AGG_CNT1_STRUC;
+#endif
+#define TX_AGG_CNT2	0x1728
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT2_STRUC	{
+	struct	{
+	    USHORT  AggSize6Count;
+	    USHORT  AggSize5Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT2_STRUC, *PTX_AGG_CNT2_STRUC;
+#else
+typedef	union	_TX_AGG_CNT2_STRUC	{
+	struct	{
+	    USHORT  AggSize5Count;
+	    USHORT  AggSize6Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT2_STRUC, *PTX_AGG_CNT2_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT3	0x172c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT3_STRUC	{
+	struct	{
+	    USHORT  AggSize8Count;
+	    USHORT  AggSize7Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT3_STRUC, *PTX_AGG_CNT3_STRUC;
+#else
+typedef	union	_TX_AGG_CNT3_STRUC	{
+	struct	{
+	    USHORT  AggSize7Count;
+	    USHORT  AggSize8Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT3_STRUC, *PTX_AGG_CNT3_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT4	0x1730
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT4_STRUC	{
+	struct	{
+	    USHORT  AggSize10Count;
+	    USHORT  AggSize9Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT4_STRUC, *PTX_AGG_CNT4_STRUC;
+#else
+typedef	union	_TX_AGG_CNT4_STRUC	{
+	struct	{
+	    USHORT  AggSize9Count;
+	    USHORT  AggSize10Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT4_STRUC, *PTX_AGG_CNT4_STRUC;
+#endif
+#define TX_AGG_CNT5	0x1734
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT5_STRUC	{
+	struct	{
+	    USHORT  AggSize12Count;
+	    USHORT  AggSize11Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT5_STRUC, *PTX_AGG_CNT5_STRUC;
+#else
+typedef	union	_TX_AGG_CNT5_STRUC	{
+	struct	{
+	    USHORT  AggSize11Count;
+	    USHORT  AggSize12Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT5_STRUC, *PTX_AGG_CNT5_STRUC;
+#endif
+#define TX_AGG_CNT6		0x1738
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT6_STRUC	{
+	struct	{
+	    USHORT  AggSize14Count;
+	    USHORT  AggSize13Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT6_STRUC, *PTX_AGG_CNT6_STRUC;
+#else
+typedef	union	_TX_AGG_CNT6_STRUC	{
+	struct	{
+	    USHORT  AggSize13Count;
+	    USHORT  AggSize14Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT6_STRUC, *PTX_AGG_CNT6_STRUC;
+#endif
+#define TX_AGG_CNT7		0x173c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT7_STRUC	{
+	struct	{
+	    USHORT  AggSize16Count;
+	    USHORT  AggSize15Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT7_STRUC, *PTX_AGG_CNT7_STRUC;
+#else
+typedef	union	_TX_AGG_CNT7_STRUC	{
+	struct	{
+	    USHORT  AggSize15Count;
+	    USHORT  AggSize16Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT7_STRUC, *PTX_AGG_CNT7_STRUC;
+#endif
+#define MPDU_DENSITY_CNT		0x1740
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MPDU_DEN_CNT_STRUC	{
+	struct	{
+	    USHORT  RXZeroDelCount;	//RX zero length delimiter count
+	    USHORT  TXZeroDelCount;	//TX zero length delimiter count
+	}	field;
+	UINT32			word;
+}	MPDU_DEN_CNT_STRUC, *PMPDU_DEN_CNT_STRUC;
+#else
+typedef	union	_MPDU_DEN_CNT_STRUC	{
+	struct	{
+	    USHORT  TXZeroDelCount;	//TX zero length delimiter count
+	    USHORT  RXZeroDelCount;	//RX zero length delimiter count
+	}	field;
+	UINT32			word;
+}	MPDU_DEN_CNT_STRUC, *PMPDU_DEN_CNT_STRUC;
+#endif
+//
+// TXRX control registers - base address 0x3000
+//
+// rt2860b  UNKNOWN reg use R/O Reg Addr 0x77d0 first..
+#define TXRX_CSR1           0x77d0
+
+//
+// Security key table memory, base address = 0x1000
+//
+#define MAC_WCID_BASE		0x1800 //8-bytes(use only 6-bytes) * 256 entry =
+#define HW_WCID_ENTRY_SIZE   8
+#define PAIRWISE_KEY_TABLE_BASE     0x4000      // 32-byte * 256-entry =  -byte
+#define HW_KEY_ENTRY_SIZE           0x20
+#define PAIRWISE_IVEIV_TABLE_BASE     0x6000      // 8-byte * 256-entry =  -byte
+#define MAC_IVEIV_TABLE_BASE     0x6000      // 8-byte * 256-entry =  -byte
+#define HW_IVEIV_ENTRY_SIZE   8
+#define MAC_WCID_ATTRIBUTE_BASE     0x6800      // 4-byte * 256-entry =  -byte
+#define HW_WCID_ATTRI_SIZE   4
+#define WCID_RESERVED          		0x6bfc
+#define SHARED_KEY_TABLE_BASE       0x6c00      // 32-byte * 16-entry = 512-byte
+#define SHARED_KEY_MODE_BASE       0x7000      // 32-byte * 16-entry = 512-byte
+#define HW_SHARED_KEY_MODE_SIZE   4
+#define SHAREDKEYTABLE			0
+#define PAIRWISEKEYTABLE			1
+
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_SHAREDKEY_MODE_STRUC	{
+	struct	{
+		UINT32       :1;
+		UINT32       Bss1Key3CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key0CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key3CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key0CipherAlg:3;
+	}	field;
+	UINT32			word;
+}	SHAREDKEY_MODE_STRUC, *PSHAREDKEY_MODE_STRUC;
+#else
+typedef	union	_SHAREDKEY_MODE_STRUC	{
+	struct	{
+		UINT32       Bss0Key0CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key3CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key0CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key3CipherAlg:3;
+		UINT32       :1;
+	}	field;
+	UINT32			word;
+}	SHAREDKEY_MODE_STRUC, *PSHAREDKEY_MODE_STRUC;
+#endif
+// 64-entry for pairwise key table
+typedef struct _HW_WCID_ENTRY {  // 8-byte per entry
+    UCHAR   Address[6];
+    UCHAR   Rsv[2];
+} HW_WCID_ENTRY, PHW_WCID_ENTRY;
+
+
+
+//
+// Other on-chip shared memory space, base = 0x2000
+//
+
+// CIS space - base address = 0x2000
+#define HW_CIS_BASE             0x2000
+
+// Carrier-sense CTS frame base address. It's where mac stores carrier-sense frame for carrier-sense function.
+#define HW_CS_CTS_BASE			0x7700
+// DFS CTS frame base address. It's where mac stores CTS frame for DFS.
+#define HW_DFS_CTS_BASE			0x7780
+#define HW_CTS_FRAME_SIZE		0x80
+
+// 2004-11-08 john - since NULL frame won't be that long (256 byte). We steal 16 tail bytes
+// to save debugging settings
+#define HW_DEBUG_SETTING_BASE   0x77f0  // 0x77f0~0x77ff total 16 bytes
+#define HW_DEBUG_SETTING_BASE2   0x7770  // 0x77f0~0x77ff total 16 bytes
+
+// In order to support maximum 8 MBSS and its maximum length is 512 for each beacon
+// Three section discontinue memory segments will be used.
+// 1. The original region for BCN 0~3
+// 2. Extract memory from FCE table for BCN 4~5
+// 3. Extract memory from Pair-wise key table for BCN 6~7
+//	  It occupied those memory of wcid 238~253 for BCN 6
+//						      and wcid 222~237 for BCN 7
+#define HW_BEACON_MAX_SIZE      0x1000 /* unit: byte */
+#define HW_BEACON_BASE0         0x7800
+#define HW_BEACON_BASE1         0x7A00
+#define HW_BEACON_BASE2         0x7C00
+#define HW_BEACON_BASE3         0x7E00
+#define HW_BEACON_BASE4         0x7200
+#define HW_BEACON_BASE5         0x7400
+#define HW_BEACON_BASE6         0x5DC0
+#define HW_BEACON_BASE7         0x5BC0
+
+#define HW_BEACON_MAX_COUNT     8
+#define HW_BEACON_OFFSET		0x0200
+#define HW_BEACON_CONTENT_LEN	(HW_BEACON_OFFSET - TXWI_SIZE)
+
+// HOST-MCU shared memory - base address = 0x2100
+#define HOST_CMD_CSR		0x404
+#define H2M_MAILBOX_CSR         0x7010
+#define H2M_MAILBOX_CID         0x7014
+#define H2M_MAILBOX_STATUS      0x701c
+#define H2M_INT_SRC             0x7024
+#define H2M_BBP_AGENT           0x7028
+#define M2H_CMD_DONE_CSR        0x000c
+#define MCU_TXOP_ARRAY_BASE     0x000c   // TODO: to be provided by Albert
+#define MCU_TXOP_ENTRY_SIZE     32       // TODO: to be provided by Albert
+#define MAX_NUM_OF_TXOP_ENTRY   16       // TODO: must be same with 8051 firmware
+#define MCU_MBOX_VERSION        0x01     // TODO: to be confirmed by Albert
+#define MCU_MBOX_VERSION_OFFSET 5        // TODO: to be provided by Albert
+
+//
+// Host DMA registers - base address 0x200 .  TX0-3=EDCAQid0-3, TX4=HCCA, TX5=MGMT,
+//
+//
+//  DMA RING DESCRIPTOR
+//
+#define E2PROM_CSR          0x0004
+#define IO_CNTL_CSR         0x77d0
+
+#ifdef RT2860
+// 8051 firmware image for RT2860 - base address = 0x4000
+#define FIRMWARE_IMAGE_BASE     0x2000
+#define MAX_FIRMWARE_IMAGE_SIZE 0x2000    // 8kbyte
+#endif // RT2860 //
+
+
+// ================================================================
+// Tx /	Rx / Mgmt ring descriptor definition
+// ================================================================
+
+// the following PID values are used to mark outgoing frame type in TXD->PID so that
+// proper TX statistics can be collected based on these categories
+// b3-2 of PID field -
+#define PID_MGMT			0x05
+#define PID_BEACON			0x0c
+#define PID_DATA_NORMALUCAST	 	0x02
+#define PID_DATA_AMPDU	 	0x04
+#define PID_DATA_NO_ACK    	0x08
+#define PID_DATA_NOT_NORM_ACK	 	0x03
+// value domain of pTxD->HostQId (4-bit: 0~15)
+#define QID_AC_BK               1   // meet ACI definition in 802.11e
+#define QID_AC_BE               0   // meet ACI definition in 802.11e
+#define QID_AC_VI               2
+#define QID_AC_VO               3
+#define QID_HCCA                4
+#define NUM_OF_TX_RING          5
+#define QID_MGMT                13
+#define QID_RX                  14
+#define QID_OTHER               15
+
+
+// ------------------------------------------------------
+// BBP & RF	definition
+// ------------------------------------------------------
+#define	BUSY		                1
+#define	IDLE		                0
+
+#define	RF_R00					    0
+#define	RF_R01					    1
+#define	RF_R02					    2
+#define	RF_R03					    3
+#define	RF_R04					    4
+#define	RF_R05					    5
+#define	RF_R06					    6
+#define	RF_R07					    7
+#define	RF_R08					    8
+#define	RF_R09					    9
+#define	RF_R10					    10
+#define	RF_R11					    11
+#define	RF_R12					    12
+#define	RF_R13					    13
+#define	RF_R14					    14
+#define	RF_R15					    15
+#define	RF_R16					    16
+#define	RF_R17					    17
+#define	RF_R18					    18
+#define	RF_R19					    19
+#define	RF_R20					    20
+#define	RF_R21					    21
+#define	RF_R22					    22
+#define	RF_R23					    23
+#define	RF_R24					    24
+#define	RF_R25					    25
+#define	RF_R26					    26
+#define	RF_R27					    27
+#define	RF_R28					    28
+#define	RF_R29					    29
+#define	RF_R30					    30
+#define	RF_R31					    31
+
+#define	BBP_R0					    0  // version
+#define	BBP_R1				        1  // TSSI
+#define	BBP_R2          			2  // TX configure
+#define BBP_R3                      3
+#define BBP_R4                      4
+#define BBP_R5                      5
+#define BBP_R6                      6
+#define	BBP_R14			            14 // RX configure
+#define BBP_R16                     16
+#define BBP_R17                     17 // RX sensibility
+#define BBP_R18                     18
+#define BBP_R21                     21
+#define BBP_R22                     22
+#define BBP_R24                     24
+#define BBP_R25                     25
+#define BBP_R49                     49 //TSSI
+#define BBP_R50                     50
+#define BBP_R51                     51
+#define BBP_R52                     52
+#define BBP_R55                     55
+#define BBP_R62                     62 // Rx SQ0 Threshold HIGH
+#define BBP_R63                     63
+#define BBP_R64                     64
+#define BBP_R65                     65
+#define BBP_R66                     66
+#define BBP_R67                     67
+#define BBP_R68                     68
+#define BBP_R69                     69
+#define BBP_R70                     70 // Rx AGC SQ CCK Xcorr threshold
+#define BBP_R73                     73
+#define BBP_R75						75
+#define BBP_R77                     77
+#define BBP_R81                     81
+#define BBP_R82                     82
+#define BBP_R83                     83
+#define BBP_R84                     84
+#define BBP_R86						86
+#define BBP_R91						91
+#define BBP_R92						92
+#define BBP_R94                     94 // Tx Gain Control
+#define BBP_R103                    103
+#define BBP_R105                    105
+#define BBP_R113                    113
+#define BBP_R114                    114
+#define BBP_R115                    115
+#define BBP_R116                    116
+#define BBP_R117                    117
+#define BBP_R118                    118
+#define BBP_R119                    119
+#define BBP_R120                    120
+#define BBP_R121                    121
+#define BBP_R122                    122
+#define BBP_R123                    123
+
+
+#define BBPR94_DEFAULT              0x06 // Add 1 value will gain 1db
+
+#define RSSI_FOR_VERY_LOW_SENSIBILITY -35
+#define RSSI_FOR_LOW_SENSIBILITY      -58
+#define RSSI_FOR_MID_LOW_SENSIBILITY  -80
+#define RSSI_FOR_MID_SENSIBILITY      -90
+
+//-------------------------------------------------------------------------
+// EEPROM definition
+//-------------------------------------------------------------------------
+#define EEDO                        0x08
+#define EEDI                        0x04
+#define EECS                        0x02
+#define EESK                        0x01
+#define EERL                        0x80
+
+#define EEPROM_WRITE_OPCODE         0x05
+#define EEPROM_READ_OPCODE          0x06
+#define EEPROM_EWDS_OPCODE          0x10
+#define EEPROM_EWEN_OPCODE          0x13
+
+#define	NUM_EEPROM_BBP_PARMS		19			// Include NIC Config 0, 1, CR, TX ALC step, BBPs
+#define	NUM_EEPROM_TX_G_PARMS		7
+#define	EEPROM_NIC1_OFFSET          0x34		// The address is from NIC config 0, not BBP register ID
+#define	EEPROM_NIC2_OFFSET          0x36		// The address is from NIC config 0, not BBP register ID
+#define	EEPROM_BBP_BASE_OFFSET		0xf0		// The address is from NIC config 0, not BBP register ID
+#define	EEPROM_G_TX_PWR_OFFSET		0x52
+#define	EEPROM_G_TX2_PWR_OFFSET		0x60
+#define EEPROM_LED1_OFFSET			0x3c
+#define EEPROM_LED2_OFFSET			0x3e
+#define EEPROM_LED3_OFFSET			0x40
+#define EEPROM_LNA_OFFSET			0x44
+#define EEPROM_RSSI_BG_OFFSET		0x46
+#define EEPROM_RSSI_A_OFFSET		0x4a
+#define EEPROM_DEFINE_MAX_TXPWR		0x4e
+#define EEPROM_TXPOWER_BYRATE_20MHZ_2_4G	0xde	// 20MHZ 2.4G tx power.
+#define EEPROM_TXPOWER_BYRATE_40MHZ_2_4G	0xee	// 40MHZ 2.4G tx power.
+#define EEPROM_TXPOWER_BYRATE_20MHZ_5G		0xfa	// 20MHZ 5G tx power.
+#define EEPROM_TXPOWER_BYRATE_40MHZ_5G		0x10a	// 40MHZ 5G tx power.
+#define EEPROM_A_TX_PWR_OFFSET      0x78
+#define EEPROM_A_TX2_PWR_OFFSET      0xa6
+#define EEPROM_VERSION_OFFSET       0x02
+#define	EEPROM_FREQ_OFFSET			0x3a
+#define EEPROM_TXPOWER_BYRATE 	0xde	// 20MHZ power.
+#define EEPROM_TXPOWER_DELTA		0x50	// 20MHZ AND 40 MHZ use different power. This is delta in 40MHZ.
+#define VALID_EEPROM_VERSION        1
+
+// PairKeyMode definition
+#define PKMODE_NONE                 0
+#define PKMODE_WEP64                1
+#define PKMODE_WEP128               2
+#define PKMODE_TKIP                 3
+#define PKMODE_AES                  4
+#define PKMODE_CKIP64               5
+#define PKMODE_CKIP128              6
+#define PKMODE_TKIP_NO_MIC          7       // MIC appended by driver: not a valid value in hardware key table
+
+// =================================================================================
+// WCID  format
+// =================================================================================
+//7.1	WCID  ENTRY  format  : 8bytes
+typedef	struct	_WCID_ENTRY_STRUC {
+	UCHAR		RXBABitmap7;    // bit0 for TID8, bit7 for TID 15
+	UCHAR		RXBABitmap0;    // bit0 for TID0, bit7 for TID 7
+	UCHAR		MAC[6];	// 0 for shared key table.  1 for pairwise key table
+}	WCID_ENTRY_STRUC, *PWCID_ENTRY_STRUC;
+
+//8.1.1	SECURITY  KEY  format  : 8DW
+// 32-byte per entry, total 16-entry for shared key table, 64-entry for pairwise key table
+typedef struct _HW_KEY_ENTRY {          // 32-byte per entry
+    UCHAR   Key[16];
+    UCHAR   TxMic[8];
+    UCHAR   RxMic[8];
+} HW_KEY_ENTRY, *PHW_KEY_ENTRY;
+
+//8.1.2	IV/EIV  format  : 2DW
+
+//8.1.3	RX attribute entry format  : 1DW
+#ifdef RT_BIG_ENDIAN
+typedef	struct	_MAC_ATTRIBUTE_STRUC {
+	UINT32		rsv:22;
+	UINT32		RXWIUDF:3;
+	UINT32		BSSIDIdx:3; //multipleBSS index for the WCID
+	UINT32		PairKeyMode:3;
+	UINT32		KeyTab:1;	// 0 for shared key table.  1 for pairwise key table
+}	MAC_ATTRIBUTE_STRUC, *PMAC_ATTRIBUTE_STRUC;
+#else
+typedef	struct	_MAC_ATTRIBUTE_STRUC {
+	UINT32		KeyTab:1;	// 0 for shared key table.  1 for pairwise key table
+	UINT32		PairKeyMode:3;
+	UINT32		BSSIDIdx:3; //multipleBSS index for the WCID
+	UINT32		RXWIUDF:3;
+	UINT32		rsv:22;
+}	MAC_ATTRIBUTE_STRUC, *PMAC_ATTRIBUTE_STRUC;
+#endif
+
+
+// =================================================================================
+// TX / RX ring descriptor format
+// =================================================================================
+
+// the first 24-byte in TXD is called TXINFO and will be DMAed to MAC block through TXFIFO.
+// MAC block use this TXINFO to control the transmission behavior of this frame.
+#define FIFO_MGMT                 0
+#define FIFO_HCCA                 1
+#define FIFO_EDCA                 2
+
+//
+// TX descriptor format, Tx	ring, Mgmt Ring
+//
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _TXD_STRUC {
+	// Word 0
+	UINT32		SDPtr0;
+	// Word 1
+	UINT32		DMADONE:1;
+	UINT32		LastSec0:1;
+	UINT32		SDLen0:14;
+	UINT32		Burst:1;
+	UINT32		LastSec1:1;
+	UINT32		SDLen1:14;
+	// Word 2
+	UINT32		SDPtr1;
+	// Word 3
+	UINT32		ICO:1;
+	UINT32		UCO:1;
+	UINT32		TCO:1;
+	UINT32		rsv:2;
+	UINT32		QSEL:2;	// select on-chip FIFO ID for 2nd-stage output scheduler.0:MGMT, 1:HCCA 2:EDCA
+	UINT32		WIV:1;	// Wireless Info Valid. 1 if Driver already fill WI,  o if DMA needs to copy WI to correctposition
+	UINT32		rsv2:24;
+}	TXD_STRUC, *PTXD_STRUC;
+#else
+typedef	struct	PACKED _TXD_STRUC {
+	// Word	0
+	UINT32		SDPtr0;
+	// Word	1
+	UINT32		SDLen1:14;
+	UINT32		LastSec1:1;
+	UINT32		Burst:1;
+	UINT32		SDLen0:14;
+	UINT32		LastSec0:1;
+	UINT32		DMADONE:1;
+	//Word2
+	UINT32		SDPtr1;
+	//Word3
+	UINT32		rsv2:24;
+	UINT32		WIV:1;	// Wireless Info Valid. 1 if Driver already fill WI,  o if DMA needs to copy WI to correctposition
+	UINT32		QSEL:2;	// select on-chip FIFO ID for 2nd-stage output scheduler.0:MGMT, 1:HCCA 2:EDCA
+	UINT32		rsv:2;
+	UINT32		TCO:1;	//
+	UINT32		UCO:1;	//
+	UINT32		ICO:1;	//
+}	TXD_STRUC, *PTXD_STRUC;
+#endif
+
+
+//
+// TXD Wireless Information format for Tx ring and Mgmt Ring
+//
+//txop : for txop mode
+// 0:txop for the MPDU frame will be handles by ASIC by register
+// 1/2/3:the MPDU frame is send after PIFS/backoff/SIFS
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _TXWI_STRUC {
+	// Word 0
+	UINT32		PHYMODE:2;
+	UINT32		TxBF:1;	// 3*3
+	UINT32		rsv2:1;
+	UINT32		Ifs:1;	//
+	UINT32		STBC:2;	//channel bandwidth 20MHz or 40 MHz
+	UINT32		ShortGI:1;
+	UINT32		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	UINT32		MCS:7;
+
+	UINT32		rsv:6;
+	UINT32		txop:2;	//tx back off mode 0:HT TXOP rule , 1:PIFS TX ,2:Backoff, 3:sifs only when previous frame exchange is successful.
+	UINT32		MpduDensity:3;
+	UINT32		AMPDU:1;
+
+	UINT32		TS:1;
+	UINT32		CFACK:1;
+	UINT32		MIMOps:1;	// the remote peer is in dynamic MIMO-PS mode
+	UINT32		FRAG:1;		// 1 to inform TKIP engine this is a fragment.
+	// Word 1
+	UINT32		PacketId:4;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		WirelessCliID:8;
+	UINT32		BAWinSize:6;
+	UINT32		NSEQ:1;
+	UINT32		ACK:1;
+	// Word 2
+	UINT32		IV;
+	// Word 3
+	UINT32		EIV;
+}	TXWI_STRUC, *PTXWI_STRUC;
+#else
+typedef	struct	PACKED _TXWI_STRUC {
+	// Word	0
+	UINT32		FRAG:1;		// 1 to inform TKIP engine this is a fragment.
+	UINT32		MIMOps:1;	// the remote peer is in dynamic MIMO-PS mode
+	UINT32		CFACK:1;
+	UINT32		TS:1;
+
+	UINT32		AMPDU:1;
+	UINT32		MpduDensity:3;
+	UINT32		txop:2;	//FOR "THIS" frame. 0:HT TXOP rule , 1:PIFS TX ,2:Backoff, 3:sifs only when previous frame exchange is successful.
+	UINT32		rsv:6;
+
+	UINT32		MCS:7;
+	UINT32		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	UINT32		ShortGI:1;
+	UINT32		STBC:2;	// 1: STBC support MCS =0-7,   2,3 : RESERVE
+	UINT32		Ifs:1;	//
+	UINT32		rsv2:1;
+	UINT32		TxBF:1;	// 3*3
+	UINT32		PHYMODE:2;
+	// Word	1
+	UINT32		ACK:1;
+	UINT32		NSEQ:1;
+	UINT32		BAWinSize:6;
+	UINT32		WirelessCliID:8;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		PacketId:4;
+	//Word2
+	UINT32		IV;
+	//Word3
+	UINT32		EIV;
+}	TXWI_STRUC, *PTXWI_STRUC;
+#endif
+//
+// Rx descriptor format, Rx	Ring
+//
+#ifdef RT2860
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _RXD_STRUC	{
+	// Word 0
+	UINT32		SDP0;
+	// Word 1
+	UINT32		DDONE:1;
+	UINT32		LS0:1;
+	UINT32		SDL0:14;
+	UINT32		Rsv:2;
+	UINT32		SDL1:14;
+	// Word 2
+	UINT32		SDP1;
+	// Word 3
+	UINT32		Rsv1:13;
+	UINT32		PlcpRssil:1;// To be moved
+	UINT32		PlcpSignal:1;		// To be moved
+	UINT32		Decrypted:1;	// this frame is being decrypted.
+	UINT32		AMPDU:1;
+	UINT32		L2PAD:1;
+	UINT32		RSSI:1;
+	UINT32		HTC:1;
+	UINT32		AMSDU:1;		// rx with 802.3 header, not 802.11 header. obsolete.
+	UINT32		CipherErr:2;        // 0: decryption okay, 1:ICV error, 2:MIC error, 3:KEY not valid
+	UINT32		Crc:1;              // 1: CRC error
+	UINT32		MyBss:1;  	// 1: this frame belongs to the same BSSID
+	UINT32		Bcast:1;            // 1: this is a broadcast frame
+	UINT32		Mcast:1;            // 1: this is a multicast frame
+	UINT32		U2M:1;              // 1: this RX frame is unicast to me
+	UINT32		FRAG:1;
+	UINT32		NULLDATA:1;
+	UINT32		DATA:1;
+	UINT32		BA:1;
+
+}	RXD_STRUC, *PRXD_STRUC, RT28XX_RXD_STRUC, *PRT28XX_RXD_STRUC;
+#else
+typedef	struct	PACKED _RXD_STRUC	{
+	// Word	0
+	UINT32		SDP0;
+	// Word	1
+	UINT32		SDL1:14;
+	UINT32		Rsv:2;
+	UINT32		SDL0:14;
+	UINT32		LS0:1;
+	UINT32		DDONE:1;
+	// Word	2
+	UINT32		SDP1;
+	// Word	3
+	UINT32		BA:1;
+	UINT32		DATA:1;
+	UINT32		NULLDATA:1;
+	UINT32		FRAG:1;
+	UINT32		U2M:1;              // 1: this RX frame is unicast to me
+	UINT32		Mcast:1;            // 1: this is a multicast frame
+	UINT32		Bcast:1;            // 1: this is a broadcast frame
+	UINT32		MyBss:1;  	// 1: this frame belongs to the same BSSID
+	UINT32		Crc:1;              // 1: CRC error
+	UINT32		CipherErr:2;        // 0: decryption okay, 1:ICV error, 2:MIC error, 3:KEY not valid
+	UINT32		AMSDU:1;		// rx with 802.3 header, not 802.11 header.
+	UINT32		HTC:1;
+	UINT32		RSSI:1;
+	UINT32		L2PAD:1;
+	UINT32		AMPDU:1;
+	UINT32		Decrypted:1;	// this frame is being decrypted.
+	UINT32		PlcpSignal:1;		// To be moved
+	UINT32		PlcpRssil:1;// To be moved
+	UINT32		Rsv1:13;
+}	RXD_STRUC, *PRXD_STRUC, RT28XX_RXD_STRUC, *PRT28XX_RXD_STRUC;
+#endif
+#endif // RT2860 //
+//
+// RXWI wireless information format, in PBF. invisible in driver.
+//
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _RXWI_STRUC {
+	// Word 0
+	UINT32		TID:4;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		UDF:3;
+	UINT32		BSSID:3;
+	UINT32		KeyIndex:2;
+	UINT32		WirelessCliID:8;
+	// Word 1
+	UINT32		PHYMODE:2;              // 1: this RX frame is unicast to me
+	UINT32		rsv:3;
+	UINT32		STBC:2;
+	UINT32		ShortGI:1;
+	UINT32		BW:1;
+	UINT32		MCS:7;
+	UINT32		SEQUENCE:12;
+	UINT32		FRAG:4;
+	// Word 2
+	UINT32		rsv1:8;
+	UINT32		RSSI2:8;
+	UINT32		RSSI1:8;
+	UINT32		RSSI0:8;
+	// Word 3
+	UINT32		rsv2:16;
+	UINT32		SNR1:8;
+	UINT32		SNR0:8;
+}	RXWI_STRUC, *PRXWI_STRUC;
+#else
+typedef	struct	PACKED _RXWI_STRUC {
+	// Word	0
+	UINT32		WirelessCliID:8;
+	UINT32		KeyIndex:2;
+	UINT32		BSSID:3;
+	UINT32		UDF:3;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		TID:4;
+	// Word	1
+	UINT32		FRAG:4;
+	UINT32		SEQUENCE:12;
+	UINT32		MCS:7;
+	UINT32		BW:1;
+	UINT32		ShortGI:1;
+	UINT32		STBC:2;
+	UINT32		rsv:3;
+	UINT32		PHYMODE:2;              // 1: this RX frame is unicast to me
+	//Word2
+	UINT32		RSSI0:8;
+	UINT32		RSSI1:8;
+	UINT32		RSSI2:8;
+	UINT32		rsv1:8;
+	//Word3
+	UINT32		SNR0:8;
+	UINT32		SNR1:8;
+	UINT32		rsv2:16;
+}	RXWI_STRUC, *PRXWI_STRUC;
+#endif
+
+
+// =================================================================================
+// HOST-MCU communication data structure
+// =================================================================================
+
+//
+// H2M_MAILBOX_CSR: Host-to-MCU Mailbox
+//
+#ifdef RT_BIG_ENDIAN
+typedef union  _H2M_MAILBOX_STRUC {
+    struct {
+        UINT32       Owner:8;
+        UINT32       CmdToken:8;    // 0xff tells MCU not to report CmdDoneInt after excuting the command
+        UINT32       HighByte:8;
+        UINT32       LowByte:8;
+    }   field;
+    UINT32           word;
+} H2M_MAILBOX_STRUC, *PH2M_MAILBOX_STRUC;
+#else
+typedef union  _H2M_MAILBOX_STRUC {
+    struct {
+        UINT32       LowByte:8;
+        UINT32       HighByte:8;
+        UINT32       CmdToken:8;
+        UINT32       Owner:8;
+    }   field;
+    UINT32           word;
+} H2M_MAILBOX_STRUC, *PH2M_MAILBOX_STRUC;
+#endif
+
+//
+// M2H_CMD_DONE_CSR: MCU-to-Host command complete indication
+//
+#ifdef RT_BIG_ENDIAN
+typedef union _M2H_CMD_DONE_STRUC {
+    struct  {
+        UINT32       CmdToken3;
+        UINT32       CmdToken2;
+        UINT32       CmdToken1;
+        UINT32       CmdToken0;
+    } field;
+    UINT32           word;
+} M2H_CMD_DONE_STRUC, *PM2H_CMD_DONE_STRUC;
+#else
+typedef union _M2H_CMD_DONE_STRUC {
+    struct  {
+        UINT32       CmdToken0;
+        UINT32       CmdToken1;
+        UINT32       CmdToken2;
+        UINT32       CmdToken3;
+    } field;
+    UINT32           word;
+} M2H_CMD_DONE_STRUC, *PM2H_CMD_DONE_STRUC;
+#endif
+
+
+
+//
+// MCU_LEDCS: MCU LED Control Setting.
+//
+#ifdef RT_BIG_ENDIAN
+typedef union  _MCU_LEDCS_STRUC {
+	struct	{
+		UCHAR		Polarity:1;
+		UCHAR		LedMode:7;
+	} field;
+	UCHAR				word;
+} MCU_LEDCS_STRUC, *PMCU_LEDCS_STRUC;
+#else
+typedef union  _MCU_LEDCS_STRUC {
+	struct	{
+		UCHAR		LedMode:7;
+		UCHAR		Polarity:1;
+	} field;
+	UCHAR			word;
+} MCU_LEDCS_STRUC, *PMCU_LEDCS_STRUC;
+#endif
+// =================================================================================
+// Register format
+// =================================================================================
+
+
+
+//NAV_TIME_CFG :NAV
+#ifdef RT_BIG_ENDIAN
+typedef	union	_NAV_TIME_CFG_STRUC	{
+	struct	{
+		USHORT		rsv:6;
+		USHORT		ZeroSifs:1;               // Applied zero SIFS timer after OFDM RX 0: disable
+		USHORT		Eifs:9;               // in unit of 1-us
+		UCHAR       SlotTime;    // in unit of 1-us
+		UCHAR		Sifs;               // in unit of 1-us
+	}	field;
+	UINT32			word;
+}	NAV_TIME_CFG_STRUC, *PNAV_TIME_CFG_STRUC;
+#else
+typedef	union	_NAV_TIME_CFG_STRUC	{
+	struct	{
+		UCHAR		Sifs;               // in unit of 1-us
+		UCHAR       SlotTime;    // in unit of 1-us
+		USHORT		Eifs:9;               // in unit of 1-us
+		USHORT		ZeroSifs:1;               // Applied zero SIFS timer after OFDM RX 0: disable
+		USHORT		rsv:6;
+	}	field;
+	UINT32			word;
+}	NAV_TIME_CFG_STRUC, *PNAV_TIME_CFG_STRUC;
+#endif
+
+
+
+
+
+//
+// RX_FILTR_CFG:  /RX configuration register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	RX_FILTR_CFG_STRUC	{
+	struct	{
+		UINT32		:15;
+		UINT32       DropRsvCntlType:1;
+
+        	UINT32       	DropBAR:1;       //
+		UINT32		DropBA:1;		//
+		UINT32		DropPsPoll:1;		// Drop Ps-Poll
+		UINT32		DropRts:1;		// Drop Ps-Poll
+
+		UINT32		DropCts:1;		// Drop Ps-Poll
+		UINT32		DropAck:1;		// Drop Ps-Poll
+		UINT32		DropCFEnd:1;		// Drop Ps-Poll
+		UINT32		DropCFEndAck:1;		// Drop Ps-Poll
+
+		UINT32		DropDuplicate:1;		// Drop duplicate frame
+		UINT32		DropBcast:1;		// Drop broadcast frames
+		UINT32		DropMcast:1;		// Drop multicast frames
+		UINT32		DropVerErr:1;	    // Drop version error frame
+
+		UINT32		DropNotMyBSSID:1;			// Drop fram ToDs bit is true
+		UINT32		DropNotToMe:1;		// Drop not to me unicast frame
+		UINT32		DropPhyErr:1;		// Drop physical error
+		UINT32		DropCRCErr:1;		// Drop CRC error
+	}	field;
+	UINT32			word;
+}	RX_FILTR_CFG_STRUC, *PRX_FILTR_CFG_STRUC;
+#else
+typedef	union	_RX_FILTR_CFG_STRUC	{
+	struct	{
+		UINT32		DropCRCErr:1;		// Drop CRC error
+		UINT32		DropPhyErr:1;		// Drop physical error
+		UINT32		DropNotToMe:1;		// Drop not to me unicast frame
+		UINT32		DropNotMyBSSID:1;			// Drop fram ToDs bit is true
+
+		UINT32		DropVerErr:1;	    // Drop version error frame
+		UINT32		DropMcast:1;		// Drop multicast frames
+		UINT32		DropBcast:1;		// Drop broadcast frames
+		UINT32		DropDuplicate:1;		// Drop duplicate frame
+
+		UINT32		DropCFEndAck:1;		// Drop Ps-Poll
+		UINT32		DropCFEnd:1;		// Drop Ps-Poll
+		UINT32		DropAck:1;		// Drop Ps-Poll
+		UINT32		DropCts:1;		// Drop Ps-Poll
+
+		UINT32		DropRts:1;		// Drop Ps-Poll
+		UINT32		DropPsPoll:1;		// Drop Ps-Poll
+		UINT32		DropBA:1;		//
+        	UINT32       	DropBAR:1;       //
+
+		UINT32       	DropRsvCntlType:1;
+		UINT32		:15;
+	}	field;
+	UINT32			word;
+}	RX_FILTR_CFG_STRUC, *PRX_FILTR_CFG_STRUC;
+#endif
+
+
+
+
+//
+// PHY_CSR4: RF serial control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_PHY_CSR4_STRUC	{
+	struct	{
+		UINT32		Busy:1;				// 1: ASIC is busy execute RF programming.
+		UINT32		PLL_LD:1;			// RF PLL_LD status
+		UINT32		IFSelect:1;			// 1: select IF	to program,	0: select RF to	program
+		UINT32		NumberOfBits:5;		// Number of bits used in RFRegValue (I:20,	RFMD:22)
+		UINT32		RFRegValue:24;		// Register	value (include register	id)	serial out to RF/IF	chip.
+	}	field;
+	UINT32			word;
+}	PHY_CSR4_STRUC, *PPHY_CSR4_STRUC;
+#else
+typedef	union	_PHY_CSR4_STRUC	{
+	struct	{
+		UINT32		RFRegValue:24;		// Register	value (include register	id)	serial out to RF/IF	chip.
+		UINT32		NumberOfBits:5;		// Number of bits used in RFRegValue (I:20,	RFMD:22)
+		UINT32		IFSelect:1;			// 1: select IF	to program,	0: select RF to	program
+		UINT32		PLL_LD:1;			// RF PLL_LD status
+		UINT32		Busy:1;				// 1: ASIC is busy execute RF programming.
+	}	field;
+	UINT32			word;
+}	PHY_CSR4_STRUC, *PPHY_CSR4_STRUC;
+#endif
+
+
+//
+// SEC_CSR5: shared key table security mode register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_SEC_CSR5_STRUC	{
+	struct	{
+        UINT32       :1;
+        UINT32       Bss3Key3CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key0CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key3CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key0CipherAlg:3;
+	}	field;
+	UINT32			word;
+}	SEC_CSR5_STRUC, *PSEC_CSR5_STRUC;
+#else
+typedef	union	_SEC_CSR5_STRUC	{
+	struct	{
+        UINT32       Bss2Key0CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key3CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key0CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key3CipherAlg:3;
+        UINT32       :1;
+	}	field;
+	UINT32			word;
+}	SEC_CSR5_STRUC, *PSEC_CSR5_STRUC;
+#endif
+
+
+//
+// HOST_CMD_CSR: For HOST to interrupt embedded processor
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_HOST_CMD_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:24;
+	    UINT32   HostCommand:8;
+	}	field;
+	UINT32			word;
+}	HOST_CMD_CSR_STRUC, *PHOST_CMD_CSR_STRUC;
+#else
+typedef	union	_HOST_CMD_CSR_STRUC	{
+	struct	{
+	    UINT32   HostCommand:8;
+	    UINT32   Rsv:24;
+	}	field;
+	UINT32			word;
+}	HOST_CMD_CSR_STRUC, *PHOST_CMD_CSR_STRUC;
+#endif
+
+
+//
+// AIFSN_CSR: AIFSN for each EDCA AC
+//
+
+
+
+//
+// E2PROM_CSR: EEPROM control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_E2PROM_CSR_STRUC	{
+	struct	{
+		UINT32		Rsvd:25;
+		UINT32       LoadStatus:1;   // 1:loading, 0:done
+		UINT32		Type:1;			// 1: 93C46, 0:93C66
+		UINT32		EepromDO:1;
+		UINT32		EepromDI:1;
+		UINT32		EepromCS:1;
+		UINT32		EepromSK:1;
+		UINT32		Reload:1;		// Reload EEPROM content, write one to reload, self-cleared.
+	}	field;
+	UINT32			word;
+}	E2PROM_CSR_STRUC, *PE2PROM_CSR_STRUC;
+#else
+typedef	union	_E2PROM_CSR_STRUC	{
+	struct	{
+		UINT32		Reload:1;		// Reload EEPROM content, write one to reload, self-cleared.
+		UINT32		EepromSK:1;
+		UINT32		EepromCS:1;
+		UINT32		EepromDI:1;
+		UINT32		EepromDO:1;
+		UINT32		Type:1;			// 1: 93C46, 0:93C66
+		UINT32       LoadStatus:1;   // 1:loading, 0:done
+		UINT32		Rsvd:25;
+	}	field;
+	UINT32			word;
+}	E2PROM_CSR_STRUC, *PE2PROM_CSR_STRUC;
+#endif
+
+
+// -------------------------------------------------------------------
+//  E2PROM data layout
+// -------------------------------------------------------------------
+
+//
+// EEPROM antenna select format
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_ANTENNA_STRUC	{
+	struct	{
+		USHORT      Rsv:4;
+		USHORT      RfIcType:4;             // see E2PROM document
+		USHORT		TxPath:4;	// 1: 1T, 2: 2T
+		USHORT		RxPath:4;	// 1: 1R, 2: 2R, 3: 3R
+	}	field;
+	USHORT			word;
+}	EEPROM_ANTENNA_STRUC, *PEEPROM_ANTENNA_STRUC;
+#else
+typedef	union	_EEPROM_ANTENNA_STRUC	{
+	struct	{
+		USHORT		RxPath:4;	// 1: 1R, 2: 2R, 3: 3R
+		USHORT		TxPath:4;	// 1: 1T, 2: 2T
+		USHORT      RfIcType:4;             // see E2PROM document
+		USHORT      Rsv:4;
+	}	field;
+	USHORT			word;
+}	EEPROM_ANTENNA_STRUC, *PEEPROM_ANTENNA_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union _EEPROM_NIC_CINFIG2_STRUC	{
+	struct	{
+        USHORT		Rsv2:6;					// must be 0
+		USHORT		BW40MAvailForA:1;			// 0:enable, 1:disable
+		USHORT		BW40MAvailForG:1;			// 0:enable, 1:disable
+		USHORT		EnableWPSPBC:1;                 // WPS PBC Control bit
+		USHORT		BW40MSidebandForA:1;
+		USHORT		BW40MSidebandForG:1;
+		USHORT		CardbusAcceleration:1;	// !!! NOTE: 0 - enable, 1 - disable
+		USHORT		ExternalLNAForA:1;			// external LNA enable for 5G
+		USHORT		ExternalLNAForG:1;			// external LNA enable for 2.4G
+		USHORT		DynamicTxAgcControl:1;			//
+		USHORT		HardwareRadioControl:1;	// Whether RF is controlled by driver or HW. 1:enable hw control, 0:disable
+	}	field;
+	USHORT			word;
+}	EEPROM_NIC_CONFIG2_STRUC, *PEEPROM_NIC_CONFIG2_STRUC;
+#else
+typedef	union _EEPROM_NIC_CINFIG2_STRUC	{
+	struct {
+		USHORT		HardwareRadioControl:1;	// 1:enable, 0:disable
+		USHORT		DynamicTxAgcControl:1;			//
+		USHORT		ExternalLNAForG:1;				//
+		USHORT		ExternalLNAForA:1;			// external LNA enable for 2.4G
+		USHORT		CardbusAcceleration:1;	// !!! NOTE: 0 - enable, 1 - disable
+		USHORT		BW40MSidebandForG:1;
+		USHORT		BW40MSidebandForA:1;
+		USHORT		EnableWPSPBC:1;                 // WPS PBC Control bit
+		USHORT		BW40MAvailForG:1;			// 0:enable, 1:disable
+		USHORT		BW40MAvailForA:1;			// 0:enable, 1:disable
+		USHORT		Rsv2:6;                 // must be 0
+	}	field;
+	USHORT			word;
+}	EEPROM_NIC_CONFIG2_STRUC, *PEEPROM_NIC_CONFIG2_STRUC;
+#endif
+
+//
+// TX_PWR Value valid range 0xFA(-6) ~ 0x24(36)
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_TX_PWR_STRUC	{
+	struct	{
+		CHAR	Byte1;				// High Byte
+		CHAR	Byte0;				// Low Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_TX_PWR_STRUC, *PEEPROM_TX_PWR_STRUC;
+#else
+typedef	union	_EEPROM_TX_PWR_STRUC	{
+	struct	{
+		CHAR	Byte0;				// Low Byte
+		CHAR	Byte1;				// High Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_TX_PWR_STRUC, *PEEPROM_TX_PWR_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_VERSION_STRUC	{
+	struct	{
+		UCHAR	Version;			// High Byte
+		UCHAR	FaeReleaseNumber;	// Low Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_VERSION_STRUC, *PEEPROM_VERSION_STRUC;
+#else
+typedef	union	_EEPROM_VERSION_STRUC	{
+	struct	{
+		UCHAR	FaeReleaseNumber;	// Low Byte
+		UCHAR	Version;			// High Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_VERSION_STRUC, *PEEPROM_VERSION_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_LED_STRUC	{
+	struct	{
+		USHORT	Rsvd:3;				// Reserved
+		USHORT	LedMode:5;			// Led mode.
+		USHORT	PolarityGPIO_4:1;	// Polarity GPIO#4 setting.
+		USHORT	PolarityGPIO_3:1;	// Polarity GPIO#3 setting.
+		USHORT	PolarityGPIO_2:1;	// Polarity GPIO#2 setting.
+		USHORT	PolarityGPIO_1:1;	// Polarity GPIO#1 setting.
+		USHORT	PolarityGPIO_0:1;	// Polarity GPIO#0 setting.
+		USHORT	PolarityACT:1;		// Polarity ACT setting.
+		USHORT	PolarityRDY_A:1;		// Polarity RDY_A setting.
+		USHORT	PolarityRDY_G:1;		// Polarity RDY_G setting.
+	}	field;
+	USHORT	word;
+}	EEPROM_LED_STRUC, *PEEPROM_LED_STRUC;
+#else
+typedef	union	_EEPROM_LED_STRUC	{
+	struct	{
+		USHORT	PolarityRDY_G:1;		// Polarity RDY_G setting.
+		USHORT	PolarityRDY_A:1;		// Polarity RDY_A setting.
+		USHORT	PolarityACT:1;		// Polarity ACT setting.
+		USHORT	PolarityGPIO_0:1;	// Polarity GPIO#0 setting.
+		USHORT	PolarityGPIO_1:1;	// Polarity GPIO#1 setting.
+		USHORT	PolarityGPIO_2:1;	// Polarity GPIO#2 setting.
+		USHORT	PolarityGPIO_3:1;	// Polarity GPIO#3 setting.
+		USHORT	PolarityGPIO_4:1;	// Polarity GPIO#4 setting.
+		USHORT	LedMode:5;			// Led mode.
+		USHORT	Rsvd:3;				// Reserved
+	}	field;
+	USHORT	word;
+}	EEPROM_LED_STRUC, *PEEPROM_LED_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_TXPOWER_DELTA_STRUC	{
+	struct	{
+		UCHAR	TxPowerEnable:1;// Enable
+		UCHAR	Type:1;			// 1: plus the delta value, 0: minus the delta value
+		UCHAR	DeltaValue:6;	// Tx Power dalta value (MAX=4)
+	}	field;
+	UCHAR	value;
+}	EEPROM_TXPOWER_DELTA_STRUC, *PEEPROM_TXPOWER_DELTA_STRUC;
+#else
+typedef	union	_EEPROM_TXPOWER_DELTA_STRUC	{
+	struct	{
+		UCHAR	DeltaValue:6;	// Tx Power dalta value (MAX=4)
+		UCHAR	Type:1;			// 1: plus the delta value, 0: minus the delta value
+		UCHAR	TxPowerEnable:1;// Enable
+	}	field;
+	UCHAR	value;
+}	EEPROM_TXPOWER_DELTA_STRUC, *PEEPROM_TXPOWER_DELTA_STRUC;
+#endif
+
+//
+// QOS_CSR0: TXOP holder address0 register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_QOS_CSR0_STRUC	{
+	struct	{
+		UCHAR		Byte3;		// MAC address byte 3
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte0;		// MAC address byte 0
+	}	field;
+	UINT32			word;
+}	QOS_CSR0_STRUC, *PQOS_CSR0_STRUC;
+#else
+typedef	union	_QOS_CSR0_STRUC	{
+	struct	{
+		UCHAR		Byte0;		// MAC address byte 0
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte3;		// MAC address byte 3
+	}	field;
+	UINT32			word;
+}	QOS_CSR0_STRUC, *PQOS_CSR0_STRUC;
+#endif
+
+//
+// QOS_CSR1: TXOP holder address1 register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_QOS_CSR1_STRUC	{
+	struct	{
+		UCHAR		Rsvd1;
+		UCHAR		Rsvd0;
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		Byte4;		// MAC address byte 4
+	}	field;
+	UINT32			word;
+}	QOS_CSR1_STRUC, *PQOS_CSR1_STRUC;
+#else
+typedef	union	_QOS_CSR1_STRUC	{
+	struct	{
+		UCHAR		Byte4;		// MAC address byte 4
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		Rsvd0;
+		UCHAR		Rsvd1;
+	}	field;
+	UINT32			word;
+}	QOS_CSR1_STRUC, *PQOS_CSR1_STRUC;
+#endif
+
+#define	RF_CSR_CFG	0x500
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG_STRUC	{
+	struct	{
+		UINT	Rsvd1:14;				// Reserved
+		UINT	RF_CSR_KICK:1;			// kick RF register read/write
+		UINT	RF_CSR_WR:1;			// 0: read  1: write
+		UINT	Rsvd2:3;				// Reserved
+		UINT	TESTCSR_RFACC_REGNUM:5;	// RF register ID
+		UINT	RF_CSR_DATA:8;			// DATA
+	}	field;
+	UINT	word;
+}	RF_CSR_CFG_STRUC, *PRF_CSR_CFG_STRUC;
+#else
+typedef	union	_RF_CSR_CFG_STRUC	{
+	struct	{
+		UINT	RF_CSR_DATA:8;			// DATA
+		UINT	TESTCSR_RFACC_REGNUM:5;	// RF register ID
+		UINT	Rsvd2:3;				// Reserved
+		UINT	RF_CSR_WR:1;			// 0: read  1: write
+		UINT	RF_CSR_KICK:1;			// kick RF register read/write
+		UINT	Rsvd1:14;				// Reserved
+	}	field;
+	UINT	word;
+}	RF_CSR_CFG_STRUC, *PRF_CSR_CFG_STRUC;
+#endif
+
+#endif	// __RT28XX_H__
diff --git a/drivers/staging/rt2860/rt_ate.c b/drivers/staging/rt2860/rt_ate.c
new file mode 100644
index 000000000000..f3316ec0b156
--- /dev/null
+++ b/drivers/staging/rt2860/rt_ate.c
@@ -0,0 +1,6025 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "rt_config.h"
+
+#ifdef RALINK_ATE
+UCHAR TemplateFrame[24] = {0x08/* Data type */,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0xAA,0xBB,0x12,0x34,0x56,0x00,0x11,0x22,0xAA,0xBB,0xCC,0x00,0x00};	// 802.11 MAC Header, Type:Data, Length:24bytes
+extern RTMP_RF_REGS RF2850RegTable[];
+extern UCHAR NUM_OF_2850_CHNL;
+
+static CHAR CCKRateTable[] = {0, 1, 2, 3, 8, 9, 10, 11, -1}; /* CCK Mode. */
+static CHAR OFDMRateTable[] = {0, 1, 2, 3, 4, 5, 6, 7, -1}; /* OFDM Mode. */
+static CHAR HTMIXRateTable[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, -1}; /* HT Mix Mode. */
+
+static INT TxDmaBusy(
+	IN PRTMP_ADAPTER pAd);
+
+static INT RxDmaBusy(
+	IN PRTMP_ADAPTER pAd);
+
+static VOID RtmpDmaEnable(
+	IN PRTMP_ADAPTER pAd,
+	IN INT Enable);
+
+static VOID BbpSoftReset(
+	IN PRTMP_ADAPTER pAd);
+
+static VOID RtmpRfIoWrite(
+	IN PRTMP_ADAPTER pAd);
+
+static INT ATESetUpFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT32 TxIdx);
+
+static INT ATETxPwrHandler(
+	IN PRTMP_ADAPTER pAd,
+	IN char index);
+
+static INT ATECmdHandler(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+static int CheckMCSValid(
+	IN UCHAR Mode,
+	IN UCHAR Mcs);
+
+#ifdef RT2860
+static VOID ATEWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC 	pOutTxWI,
+	IN	BOOLEAN			FRAG,
+	IN	BOOLEAN			CFACK,
+	IN	BOOLEAN			InsTimestamp,
+	IN	BOOLEAN 		AMPDU,
+	IN	BOOLEAN 		Ack,
+	IN	BOOLEAN 		NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN	UCHAR 			PID,
+	IN	UCHAR			TID,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	*pTransmit);
+#endif // RT2860 //
+
+
+static VOID SetJapanFilter(
+	IN	PRTMP_ADAPTER	pAd);
+
+/*=========================end of prototype=========================*/
+
+#ifdef RT2860
+static INT TxDmaBusy(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT result;
+	WPDMA_GLO_CFG_STRUC GloCfg;
+
+	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);	// disable DMA
+	if (GloCfg.field.TxDMABusy)
+		result = 1;
+	else
+		result = 0;
+
+	return result;
+}
+
+static INT RxDmaBusy(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT result;
+	WPDMA_GLO_CFG_STRUC GloCfg;
+
+	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);	// disable DMA
+	if (GloCfg.field.RxDMABusy)
+		result = 1;
+	else
+		result = 0;
+
+	return result;
+}
+
+static VOID RtmpDmaEnable(
+	IN PRTMP_ADAPTER pAd,
+	IN INT Enable)
+{
+	BOOLEAN value;
+	ULONG WaitCnt;
+	WPDMA_GLO_CFG_STRUC GloCfg;
+
+	value = Enable > 0 ? 1 : 0;
+
+	// check DMA is in busy mode.
+	WaitCnt = 0;
+	while (TxDmaBusy(pAd) || RxDmaBusy(pAd))
+	{
+		RTMPusecDelay(10);
+		if (WaitCnt++ > 100)
+			break;
+	}
+
+	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);	// disable DMA
+	GloCfg.field.EnableTxDMA = value;
+	GloCfg.field.EnableRxDMA = value;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);	// abort all TX rings
+	RTMPusecDelay(5000);
+
+	return;
+}
+#endif // RT2860 //
+
+
+static VOID BbpSoftReset(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR BbpData = 0;
+
+	// Soft reset, set BBP R21 bit0=1->0
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R21, &BbpData);
+	BbpData |= 0x00000001; //set bit0=1
+	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R21, BbpData);
+
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R21, &BbpData);
+	BbpData &= ~(0x00000001); //set bit0=0
+	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R21, BbpData);
+
+	return;
+}
+
+static VOID RtmpRfIoWrite(
+	IN PRTMP_ADAPTER pAd)
+{
+	// Set RF value 1's set R3[bit2] = [0]
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+	RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+	RTMPusecDelay(200);
+
+	// Set RF value 2's set R3[bit2] = [1]
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+	RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 | 0x04));
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+	RTMPusecDelay(200);
+
+	// Set RF value 3's set R3[bit2] = [0]
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+	RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+	return;
+}
+
+static int CheckMCSValid(
+	UCHAR Mode,
+	UCHAR Mcs)
+{
+	int i;
+	PCHAR pRateTab;
+
+	switch(Mode)
+	{
+		case 0:
+			pRateTab = CCKRateTable;
+			break;
+		case 1:
+			pRateTab = OFDMRateTable;
+			break;
+		case 2:
+		case 3:
+			pRateTab = HTMIXRateTable;
+			break;
+		default:
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("unrecognizable Tx Mode %d\n", Mode));
+			return -1;
+			break;
+	}
+
+	i = 0;
+	while(pRateTab[i] != -1)
+	{
+		if (pRateTab[i] == Mcs)
+			return 0;
+		i++;
+	}
+
+	return -1;
+}
+
+#if 1
+static INT ATETxPwrHandler(
+	IN PRTMP_ADAPTER pAd,
+	IN char index)
+{
+	ULONG R;
+	CHAR TxPower;
+	UCHAR Bbp94 = 0;
+	BOOLEAN bPowerReduce = FALSE;
+
+#ifdef RALINK_28xx_QA
+	if ((pAd->ate.bQATxStart == TRUE) || (pAd->ate.bQARxStart == TRUE))
+	{
+		/* When QA is used for Tx, pAd->ate.TxPower0/1 and real tx power
+		** are not synchronized.
+		*/
+/*
+		pAd->ate.TxPower0 = pAd->LatchRfRegs.xxx;
+		pAd->ate.TxPower1 = pAd->LatchRfRegs.xxx;
+*/
+		return 0;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		TxPower = index == 0 ? pAd->ate.TxPower0 : pAd->ate.TxPower1;
+
+		if (pAd->ate.Channel <= 14)
+		{
+			if (TxPower > 31)
+			{
+				//
+				// R3, R4 can't large than 31 (0x24), 31 ~ 36 used by BBP 94
+				//
+				R = 31;
+				if (TxPower <= 36)
+					Bbp94 = BBPR94_DEFAULT + (UCHAR)(TxPower - 31);
+			}
+			else if (TxPower < 0)
+			{
+				//
+				// R3, R4 can't less than 0, -1 ~ -6 used by BBP 94
+				//
+				R = 0;
+				if (TxPower >= -6)
+					Bbp94 = BBPR94_DEFAULT + TxPower;
+			}
+			else
+			{
+				// 0 ~ 31
+				R = (ULONG) TxPower;
+				Bbp94 = BBPR94_DEFAULT;
+			}
+
+			ATEDBGPRINT(RT_DEBUG_TRACE, ("%s (TxPower=%d, R=%ld, BBP_R94=%d)\n", __func__, TxPower, R, Bbp94));
+		}
+		else// 5.5 GHz
+		{
+			if (TxPower > 15)
+			{
+				//
+				// R3, R4 can't large than 15 (0x0F)
+				//
+				R = 15;
+			}
+			else if (TxPower < 0)
+			{
+				//
+				// R3, R4 can't less than 0
+				//
+				// -1 ~ -7
+				ASSERT((TxPower >= -7));
+				R = (ULONG)(TxPower + 7);
+				bPowerReduce = TRUE;
+			}
+			else
+			{
+				// 0 ~ 15
+				R = (ULONG) TxPower;
+			}
+
+			ATEDBGPRINT(RT_DEBUG_TRACE, ("%s (TxPower=%d, R=%lu)\n", __func__, TxPower, R));
+		}
+
+		if (pAd->ate.Channel <= 14)
+		{
+			if (index == 0)
+			{
+				R = R << 9;		// shift TX power control to correct RF(R3) register bit position
+				R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+				pAd->LatchRfRegs.R3 = R;
+			}
+			else
+			{
+				R = R << 6;		// shift TX power control to correct RF(R4) register bit position
+				R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+				pAd->LatchRfRegs.R4 = R;
+			}
+		}
+		else// 5.5GHz
+		{
+			if (bPowerReduce == FALSE)
+			{
+				if (index == 0)
+				{
+					R = (R << 10) | (1 << 9);		// shift TX power control to correct RF(R3) register bit position
+					R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+					pAd->LatchRfRegs.R3 = R;
+				}
+				else
+				{
+					R = (R << 7) | (1 << 6);		// shift TX power control to correct RF(R4) register bit position
+					R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+					pAd->LatchRfRegs.R4 = R;
+				}
+			}
+			else
+			{
+				if (index == 0)
+				{
+					R = (R << 10);		// shift TX power control to correct RF(R3) register bit position
+					R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+
+					/* Clear bit 9 of R3 to reduce 7dB. */
+					pAd->LatchRfRegs.R3 = (R & (~(1 << 9)));
+				}
+				else
+				{
+					R = (R << 7);		// shift TX power control to correct RF(R4) register bit position
+					R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+
+					/* Clear bit 6 of R4 to reduce 7dB. */
+					pAd->LatchRfRegs.R4 = (R & (~(1 << 6)));
+				}
+			}
+		}
+
+		RtmpRfIoWrite(pAd);
+
+		return 0;
+	}
+}
+#else// 1 //
+static INT ATETxPwrHandler(
+	IN PRTMP_ADAPTER pAd,
+	IN char index)
+{
+	ULONG R;
+	CHAR TxPower;
+	UCHAR Bbp94 = 0;
+
+#ifdef RALINK_28xx_QA
+	if ((pAd->ate.bQATxStart == TRUE) || (pAd->ate.bQARxStart == TRUE))
+	{
+		// TODO: how to get current TxPower0/1 from pAd->LatchRfRegs ?
+		/* When QA is used for Tx, pAd->ate.TxPower0/1 and real tx power
+		** are not synchronized.
+		*/
+/*
+		pAd->ate.TxPower0 = pAd->LatchRfRegs.xxx;
+		pAd->ate.TxPower1 = pAd->LatchRfRegs.xxx;
+*/
+		return 0;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		TxPower = index == 0 ? pAd->ate.TxPower0 : pAd->ate.TxPower1;
+
+	if (TxPower > 31)
+	{
+		//
+		// R3, R4 can't large than 36 (0x24), 31 ~ 36 used by BBP 94
+		//
+		R = 31;
+		if (TxPower <= 36)
+			Bbp94 = BBPR94_DEFAULT + (UCHAR)(TxPower - 31);
+	}
+	else if (TxPower < 0)
+	{
+		//
+		// R3, R4 can't less than 0, -1 ~ -6 used by BBP 94
+		//
+		R = 0;
+		if (TxPower >= -6)
+			Bbp94 = BBPR94_DEFAULT + TxPower;
+	}
+	else
+	{
+		// 0 ~ 31
+		R = (ULONG) TxPower;
+		Bbp94 = BBPR94_DEFAULT;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("%s (TxPower=%d, R3=%ld, BBP_R94=%d)\n", __func__, TxPower, R, Bbp94));
+
+		if (pAd->ate.Channel <= 14)
+		{
+	if (index == 0)
+	{
+		R = R << 9;		// shift TX power control to correct RF(R3) register bit position
+		R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+		pAd->LatchRfRegs.R3 = R;
+	}
+	else
+	{
+		R = R << 6;		// shift TX power control to correct RF(R4) register bit position
+		R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+		pAd->LatchRfRegs.R4 = R;
+	}
+		}
+		else
+		{
+			if (index == 0)
+			{
+				R = (R << 10) | (1 << 9);		// shift TX power control to correct RF(R3) register bit position
+				R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+				pAd->LatchRfRegs.R3 = R;
+			}
+			else
+			{
+				R = (R << 7) | (1 << 6);		// shift TX power control to correct RF(R4) register bit position
+				R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+				pAd->LatchRfRegs.R4 = R;
+			}
+		}
+
+	RtmpRfIoWrite(pAd);
+
+	return 0;
+	}
+}
+#endif // 1 //
+/*
+    ==========================================================================
+    Description:
+        Set ATE operation mode to
+        0. ATESTART  = Start ATE Mode
+        1. ATESTOP   = Stop ATE Mode
+        2. TXCONT    = Continuous Transmit
+        3. TXCARR    = Transmit Carrier
+        4. TXFRAME   = Transmit Frames
+        5. RXFRAME   = Receive Frames
+#ifdef RALINK_28xx_QA
+        6. TXSTOP    = Stop Any Type of Transmition
+        7. RXSTOP    = Stop Receiving Frames
+#endif // RALINK_28xx_QA //
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+#ifdef RT2860
+static INT	ATECmdHandler(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32			Value = 0;
+	UCHAR			BbpData;
+	UINT32			MacData = 0;
+	PTXD_STRUC		pTxD;
+	INT				index;
+	UINT			i=0, atemode;
+	PRXD_STRUC		pRxD;
+	PRTMP_TX_RING 	pTxRing = &pAd->TxRing[QID_AC_BE];
+#ifndef UCOS
+	NDIS_STATUS		Status = NDIS_STATUS_SUCCESS;
+#endif // UCOS //
+#ifdef	RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("===> ATECmdHandler()\n"));
+
+	ATEAsicSwitchChannel(pAd);
+	AsicLockChannel(pAd, pAd->ate.Channel);
+
+	RTMPusecDelay(5000);
+
+	// read MAC_SYS_CTRL and backup MAC_SYS_CTRL value.
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+
+	// Default value in BBP R22 is 0x0.
+	BbpData = 0;
+
+	// clean bit4 to stop continuous Tx production test.
+	MacData &= 0xFFFFFFEF;
+
+	if (!strcmp(arg, "ATESTART")) 		//Enter ATE mode and set Tx/Rx Idle
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: ATESTART\n"));
+
+#ifndef UCOS
+		// check if we have removed the firmware
+		if (!(ATE_ON(pAd)))
+		{
+			NICEraseFirmware(pAd);
+		}
+#endif // !UCOS //
+		atemode = pAd->ate.Mode;
+		pAd->ate.Mode = ATE_START;
+//		pAd->ate.TxDoneCount = pAd->ate.TxCount;
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+		if (atemode & ATE_TXCARR)
+		{
+			// No Carrier Test set BBP R22 bit7=0, bit6=0, bit[5~0]=0x0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF00; //clear bit7, bit6, bit[5~0]
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		}
+		else if (atemode & ATE_TXCARRSUPP)
+		{
+			// No Cont. TX set BBP R22 bit7=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= ~(1 << 7); //set bit7=0
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+			// No Carrier Suppression set BBP R24 bit0=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R24, &BbpData);
+			BbpData &= 0xFFFFFFFE; //clear bit0
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R24, BbpData);
+		}
+		// We should free some resource which was allocated when ATE_TXFRAME , ATE_STOP, and ATE_TXCONT.
+		else if ((atemode & ATE_TXFRAME) || (atemode == ATE_STOP))
+		{
+			PRTMP_TX_RING pTxRing = &pAd->TxRing[QID_AC_BE];
+
+			if (atemode & ATE_TXCONT)
+			{
+				// No Cont. TX set BBP R22 bit7=0
+				ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+				BbpData &= ~(1 << 7); //set bit7=0
+				ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+			}
+			// Abort Tx, Rx DMA.
+			RtmpDmaEnable(pAd, 0);
+			for (i=0; i<TX_RING_SIZE; i++)
+			{
+				PNDIS_PACKET  pPacket;
+
+#ifndef RT_BIG_ENDIAN
+			    pTxD = (PTXD_STRUC)pAd->TxRing[QID_AC_BE].Cell[i].AllocVa;
+#else
+        		pDestTxD = (PTXD_STRUC)pAd->TxRing[QID_AC_BE].Cell[i].AllocVa;
+        		TxD = *pDestTxD;
+        		pTxD = &TxD;
+        		RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+				pTxD->DMADONE = 0;
+				pPacket = pTxRing->Cell[i].pNdisPacket;
+				if (pPacket)
+				{
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+				}
+				//Always assign pNdisPacket as NULL after clear
+				pTxRing->Cell[i].pNdisPacket = NULL;
+
+				pPacket = pTxRing->Cell[i].pNextNdisPacket;
+				if (pPacket)
+				{
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+				}
+				//Always assign pNextNdisPacket as NULL after clear
+				pTxRing->Cell[i].pNextNdisPacket = NULL;
+#ifdef RT_BIG_ENDIAN
+				RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+				WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif
+			}
+			// Start Tx, RX DMA
+			RtmpDmaEnable(pAd, 1);
+		}
+		// reset Rx statistics.
+		pAd->ate.LastSNR0 = 0;
+		pAd->ate.LastSNR1 = 0;
+		pAd->ate.LastRssi0 = 0;
+		pAd->ate.LastRssi1 = 0;
+		pAd->ate.LastRssi2 = 0;
+		pAd->ate.AvgRssi0 = 0;
+		pAd->ate.AvgRssi1 = 0;
+		pAd->ate.AvgRssi2 = 0;
+		pAd->ate.AvgRssi0X8 = 0;
+		pAd->ate.AvgRssi1X8 = 0;
+		pAd->ate.AvgRssi2X8 = 0;
+		pAd->ate.NumOfAvgRssiSample = 0;
+
+#ifdef RALINK_28xx_QA
+		// Tx frame
+		pAd->ate.bQATxStart = FALSE;
+		pAd->ate.bQARxStart = FALSE;
+		pAd->ate.seq = 0;
+
+		// counters
+		pAd->ate.U2M = 0;
+		pAd->ate.OtherData = 0;
+		pAd->ate.Beacon = 0;
+		pAd->ate.OtherCount = 0;
+		pAd->ate.TxAc0 = 0;
+		pAd->ate.TxAc1 = 0;
+		pAd->ate.TxAc2 = 0;
+		pAd->ate.TxAc3 = 0;
+		pAd->ate.TxHCCA = 0;
+		pAd->ate.TxMgmt = 0;
+		pAd->ate.RSSI0 = 0;
+		pAd->ate.RSSI1 = 0;
+		pAd->ate.RSSI2 = 0;
+		pAd->ate.SNR0 = 0;
+		pAd->ate.SNR1 = 0;
+
+		// control
+		pAd->ate.TxDoneCount = 0;
+		pAd->ate.TxStatus = 0; // task Tx status // 0 --> task is idle, 1 --> task is running
+#endif // RALINK_28xx_QA //
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+
+#ifdef CONFIG_STA_SUPPORT
+		//
+		// LinkDown() has "AsicDisableSync();" and "RTMP_BBP_IO_R/W8_BY_REG_ID();" inside.
+		//
+//      LinkDown(pAd, FALSE);
+//		AsicEnableBssSync(pAd);
+#ifndef UCOS
+		netif_stop_queue(pAd->net_dev);
+#endif // !UCOS //
+		//
+		// If we skip "LinkDown()", we should disable protection
+		// to prevent from sending out RTS or CTS-to-self.
+		//
+		ATEDisableAsicProtect(pAd);
+		RTMPStationStop(pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+		/* Disable Tx */
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		/* Disable Rx */
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	}
+	else if (!strcmp(arg, "ATESTOP"))
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: ATESTOP\n"));
+
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData); // recover the MAC_SYS_CTRL register back.
+
+		// Disable Tx, Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= (0xfffffff3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+#ifndef UCOS
+		pAd->ate.bFWLoading = TRUE;
+		Status = NICLoadFirmware(pAd);
+		if (Status != NDIS_STATUS_SUCCESS)
+		{
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("NICLoadFirmware failed, Status[=0x%08x]\n", Status));
+			return FALSE;
+		}
+#endif // !UCOS //
+		pAd->ate.Mode = ATE_STOP;
+
+
+#ifdef CONFIG_STA_SUPPORT
+		//
+		// Even the firmware has been loaded,
+		// we still could use ATE_BBP_IO_READ8_BY_REG_ID().
+		// But this is not suggested.
+		//
+		BbpSoftReset(pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+		NICDisableInterrupt(pAd);
+
+		NICInitializeAdapter(pAd, TRUE);
+
+
+		// Reinitialize Rx Ring before Rx DMA is enabled.
+		// The nightmare of >>>RxCoherent<<< was gone !
+		for (index = 0; index < RX_RING_SIZE; index++)
+		{
+			pRxD = (PRXD_STRUC) pAd->RxRing.Cell[index].AllocVa;
+			pRxD->DDONE = 0;
+		}
+
+		// We should read EEPROM for all cases.
+		NICReadEEPROMParameters(pAd, NULL);
+		NICInitAsicFromEEPROM(pAd);
+
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+		//
+		// Enable Interrupt
+		//
+
+		//
+		// These steps are only for APAutoSelectChannel().
+		//
+#if 0
+		//pAd->bStaFifoTest = TRUE;
+		pAd->int_enable_reg = ((DELAYINTMASK)  | (RxINT|TxDataInt|TxMgmtInt)) & ~(0x03);
+		pAd->int_disable_mask = 0;
+		pAd->int_pending = 0;
+#endif
+		RTMP_IO_WRITE32(pAd, INT_SOURCE_CSR, 0xffffffff);  // clear garbage interrupts
+		NICEnableInterrupt(pAd);
+
+
+/*=========================================================================*/
+		/* restore RX_FILTR_CFG */
+#ifdef CONFIG_STA_SUPPORT
+		/* restore RX_FILTR_CFG due to that QA maybe set it to 0x3 */
+		RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL);
+#endif // CONFIG_STA_SUPPORT //
+/*=========================================================================*/
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Enable Tx, Rx DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		// Enable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+
+#ifdef CONFIG_STA_SUPPORT
+		RTMPStationStart(pAd);
+#endif // CONFIG_STA_SUPPORT //
+#ifndef UCOS
+		netif_start_queue(pAd->net_dev);
+#endif // !UCOS //
+	}
+	else if (!strcmp(arg, "TXCARR"))	// Tx Carrier
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXCARR\n"));
+		pAd->ate.Mode |= ATE_TXCARR;
+
+		// QA has done the following steps if it is used.
+		if (pAd->ate.bQATxStart == FALSE)
+		{
+			// Soft reset BBP.
+			BbpSoftReset(pAd);
+
+			// Carrier Test set BBP R22 bit7=1, bit6=1, bit[5~0]=0x01
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF00; //clear bit7, bit6, bit[5~0]
+			BbpData |= 0x000000C1; //set bit7=1, bit6=1, bit[5~0]=0x01
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+			// set MAC_SYS_CTRL(0x1004) Continuous Tx Production Test (bit4) = 1
+			RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+			Value = Value | 0x00000010;
+			RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+		}
+	}
+	else if (!strcmp(arg, "TXCONT"))	// Tx Continue
+	{
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			/* set MAC_SYS_CTRL(0x1004) bit4(Continuous Tx Production Test)
+			   and bit2(MAC TX enable) back to zero. */
+			RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+			MacData &= 0xFFFFFFEB;
+			RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+			// set BBP R22 bit7=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF7F; //set bit7=0
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		}
+
+		/* for TxCont mode.
+		** Step 1: Send 50 packets first then wait for a moment.
+		** Step 2: Send more 50 packet then start continue mode.
+		*/
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXCONT\n"));
+		// Step 1: send 50 packets first.
+		pAd->ate.Mode |= ATE_TXCONT;
+		pAd->ate.TxCount = 50;
+                /* Do it after Tx/Rx DMA is aborted. */
+//		pAd->ate.TxDoneCount = 0;
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+		// Fix can't smooth kick
+		{
+			RTMP_IO_READ32(pAd, TX_DTX_IDX0 + QID_AC_BE * 0x10,  &pTxRing->TxDmaIdx);
+			pTxRing->TxSwFreeIdx = pTxRing->TxDmaIdx;
+			pTxRing->TxCpuIdx = pTxRing->TxDmaIdx;
+			RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QID_AC_BE * 0x10, pTxRing->TxCpuIdx);
+		}
+
+		pAd->ate.TxDoneCount = 0;
+
+		/* Only needed if we have to send some normal frames. */
+		SetJapanFilter(pAd);
+
+		for (i = 0; (i < TX_RING_SIZE-1) && (i < pAd->ate.TxCount); i++)
+		{
+			PNDIS_PACKET pPacket;
+			UINT32 TxIdx = pTxRing->TxCpuIdx;
+
+#ifndef RT_BIG_ENDIAN
+			pTxD = (PTXD_STRUC)pTxRing->Cell[TxIdx].AllocVa;
+#else
+			pDestTxD = (PTXD_STRUC)pTxRing->Cell[TxIdx].AllocVa;
+			TxD = *pDestTxD;
+			pTxD = &TxD;
+			RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+			// Clean current cell.
+			pPacket = pTxRing->Cell[TxIdx].pNdisPacket;
+			if (pPacket)
+			{
+				PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNdisPacket as NULL after clear
+			pTxRing->Cell[TxIdx].pNdisPacket = NULL;
+
+			pPacket = pTxRing->Cell[TxIdx].pNextNdisPacket;
+			if (pPacket)
+			{
+				PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNextNdisPacket as NULL after clear
+			pTxRing->Cell[TxIdx].pNextNdisPacket = NULL;
+
+#ifdef RT_BIG_ENDIAN
+			RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+			WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif
+
+			if (ATESetUpFrame(pAd, TxIdx) != 0)
+				break;
+
+			INC_RING_INDEX(pTxRing->TxCpuIdx, TX_RING_SIZE);
+		}
+
+                // Setup frame format.
+		ATESetUpFrame(pAd, pTxRing->TxCpuIdx);
+
+		// Start Tx, RX DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+#ifdef RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			pAd->ate.TxStatus = 1;
+			//pAd->ate.Repeat = 0;
+		}
+#endif // RALINK_28xx_QA //
+
+		// kick Tx-Ring.
+		RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QID_AC_BE * RINGREG_DIFF, pAd->TxRing[QID_AC_BE].TxCpuIdx);
+
+		RTMPusecDelay(5000);
+
+
+		// Step 2: send more 50 packets then start continue mode.
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+		// Cont. TX set BBP R22 bit7=1
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+		BbpData |= 0x00000080; //set bit7=1
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+		pAd->ate.TxCount = 50;
+
+		// Fix can't smooth kick
+		{
+			RTMP_IO_READ32(pAd, TX_DTX_IDX0 + QID_AC_BE * 0x10,  &pTxRing->TxDmaIdx);
+			pTxRing->TxSwFreeIdx = pTxRing->TxDmaIdx;
+			pTxRing->TxCpuIdx = pTxRing->TxDmaIdx;
+			RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QID_AC_BE * 0x10, pTxRing->TxCpuIdx);
+		}
+
+		pAd->ate.TxDoneCount = 0;
+
+		SetJapanFilter(pAd);
+
+		for (i = 0; (i < TX_RING_SIZE-1) && (i < pAd->ate.TxCount); i++)
+		{
+			PNDIS_PACKET pPacket;
+			UINT32 TxIdx = pTxRing->TxCpuIdx;
+
+#ifndef RT_BIG_ENDIAN
+			pTxD = (PTXD_STRUC)pTxRing->Cell[TxIdx].AllocVa;
+#else
+			pDestTxD = (PTXD_STRUC)pTxRing->Cell[TxIdx].AllocVa;
+			TxD = *pDestTxD;
+			pTxD = &TxD;
+			RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+			// clean current cell.
+			pPacket = pTxRing->Cell[TxIdx].pNdisPacket;
+			if (pPacket)
+			{
+				PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNdisPacket as NULL after clear
+			pTxRing->Cell[TxIdx].pNdisPacket = NULL;
+
+			pPacket = pTxRing->Cell[TxIdx].pNextNdisPacket;
+			if (pPacket)
+			{
+				PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNextNdisPacket as NULL after clear
+			pTxRing->Cell[TxIdx].pNextNdisPacket = NULL;
+
+#ifdef RT_BIG_ENDIAN
+			RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+			WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif
+
+			if (ATESetUpFrame(pAd, TxIdx) != 0)
+				break;
+
+			INC_RING_INDEX(pTxRing->TxCpuIdx, TX_RING_SIZE);
+		}
+
+		ATESetUpFrame(pAd, pTxRing->TxCpuIdx);
+
+		// Start Tx, RX DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+#ifdef RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			pAd->ate.TxStatus = 1;
+			//pAd->ate.Repeat = 0;
+		}
+#endif // RALINK_28xx_QA //
+
+		// kick Tx-Ring.
+		RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QID_AC_BE * RINGREG_DIFF, pAd->TxRing[QID_AC_BE].TxCpuIdx);
+
+		RTMPusecDelay(500);
+
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+		MacData |= 0x00000010;
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+	}
+	else if (!strcmp(arg, "TXFRAME")) // Tx Frames
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXFRAME(Count=%d)\n", pAd->ate.TxCount));
+		pAd->ate.Mode |= ATE_TXFRAME;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+		// Fix can't smooth kick
+		{
+			RTMP_IO_READ32(pAd, TX_DTX_IDX0 + QID_AC_BE * 0x10,  &pTxRing->TxDmaIdx);
+			pTxRing->TxSwFreeIdx = pTxRing->TxDmaIdx;
+			pTxRing->TxCpuIdx = pTxRing->TxDmaIdx;
+			RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QID_AC_BE * 0x10, pTxRing->TxCpuIdx);
+		}
+
+		pAd->ate.TxDoneCount = 0;
+
+		SetJapanFilter(pAd);
+
+		for (i = 0; (i < TX_RING_SIZE-1) && (i < pAd->ate.TxCount); i++)
+		{
+			PNDIS_PACKET pPacket;
+			UINT32 TxIdx = pTxRing->TxCpuIdx;
+
+#ifndef RT_BIG_ENDIAN
+			pTxD = (PTXD_STRUC)pTxRing->Cell[TxIdx].AllocVa;
+#else
+			pDestTxD = (PTXD_STRUC)pTxRing->Cell[TxIdx].AllocVa;
+			TxD = *pDestTxD;
+			pTxD = &TxD;
+			RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+			// Clean current cell.
+			pPacket = pTxRing->Cell[TxIdx].pNdisPacket;
+			if (pPacket)
+			{
+				PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNdisPacket as NULL after clear
+			pTxRing->Cell[TxIdx].pNdisPacket = NULL;
+
+			pPacket = pTxRing->Cell[TxIdx].pNextNdisPacket;
+			if (pPacket)
+			{
+				PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNextNdisPacket as NULL after clear
+			pTxRing->Cell[TxIdx].pNextNdisPacket = NULL;
+
+#ifdef RT_BIG_ENDIAN
+			RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+			WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif
+
+			if (ATESetUpFrame(pAd, TxIdx) != 0)
+				break;
+
+			INC_RING_INDEX(pTxRing->TxCpuIdx, TX_RING_SIZE);
+
+		}
+
+		ATESetUpFrame(pAd, pTxRing->TxCpuIdx);
+
+		// Start Tx, Rx DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+#ifdef RALINK_28xx_QA
+		// add this for LoopBack mode
+		if (pAd->ate.bQARxStart == FALSE)
+		{
+			// Disable Rx
+			RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+			Value &= ~(1 << 3);
+			RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+		}
+
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			pAd->ate.TxStatus = 1;
+			//pAd->ate.Repeat = 0;
+		}
+#else
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+#endif // RALINK_28xx_QA //
+
+		RTMP_IO_READ32(pAd, TX_DTX_IDX0 + QID_AC_BE * RINGREG_DIFF, &pAd->TxRing[QID_AC_BE].TxDmaIdx);
+		// kick Tx-Ring.
+		RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QID_AC_BE * RINGREG_DIFF, pAd->TxRing[QID_AC_BE].TxCpuIdx);
+
+		pAd->RalinkCounters.KickTxCount++;
+	}
+#ifdef RALINK_28xx_QA
+	else if (!strcmp(arg, "TXSTOP"))
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXSTOP\n"));
+		atemode = pAd->ate.Mode;
+		pAd->ate.Mode &= ATE_TXSTOP;
+		pAd->ate.bQATxStart = FALSE;
+//		pAd->ate.TxDoneCount = pAd->ate.TxCount;
+
+		if (atemode & ATE_TXCARR)
+		{
+			// No Carrier Test set BBP R22 bit7=0, bit6=0, bit[5~0]=0x0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF00; //clear bit7, bit6, bit[5~0]
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		}
+		else if (atemode & ATE_TXCARRSUPP)
+		{
+			// No Cont. TX set BBP R22 bit7=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= ~(1 << 7); //set bit7=0
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+			// No Carrier Suppression set BBP R24 bit0=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R24, &BbpData);
+			BbpData &= 0xFFFFFFFE; //clear bit0
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R24, BbpData);
+		}
+		// We should free some resource which allocate when ATE_TXFRAME , ATE_STOP, and ATE_TXCONT.
+		else if ((atemode & ATE_TXFRAME) || (atemode == ATE_STOP))
+		{
+
+			PRTMP_TX_RING pTxRing = &pAd->TxRing[QID_AC_BE];
+
+			if (atemode & ATE_TXCONT)
+			{
+				// No Cont. TX set BBP R22 bit7=0
+				ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+				BbpData &= ~(1 << 7); //set bit7=0
+				ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+			}
+
+			// Abort Tx, Rx DMA.
+			RtmpDmaEnable(pAd, 0);
+			for (i=0; i<TX_RING_SIZE; i++)
+			{
+				PNDIS_PACKET  pPacket;
+
+#ifndef RT_BIG_ENDIAN
+			    pTxD = (PTXD_STRUC)pAd->TxRing[QID_AC_BE].Cell[i].AllocVa;
+#else
+        		pDestTxD = (PTXD_STRUC)pAd->TxRing[QID_AC_BE].Cell[i].AllocVa;
+        		TxD = *pDestTxD;
+        		pTxD = &TxD;
+        		RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+				pTxD->DMADONE = 0;
+				pPacket = pTxRing->Cell[i].pNdisPacket;
+				if (pPacket)
+				{
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+				}
+				//Always assign pNdisPacket as NULL after clear
+				pTxRing->Cell[i].pNdisPacket = NULL;
+
+				pPacket = pTxRing->Cell[i].pNextNdisPacket;
+				if (pPacket)
+				{
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+				}
+				//Always assign pNextNdisPacket as NULL after clear
+				pTxRing->Cell[i].pNextNdisPacket = NULL;
+#ifdef RT_BIG_ENDIAN
+				RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+				WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif
+			}
+			// Enable Tx, Rx DMA
+			RtmpDmaEnable(pAd, 1);
+
+		}
+
+		// control
+//		pAd->ate.TxDoneCount = 0;
+		pAd->ate.TxStatus = 0; // task Tx status // 0 --> task is idle, 1 --> task is running
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+		// Disable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	}
+	else if (!strcmp(arg, "RXSTOP"))
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: RXSTOP\n"));
+		atemode = pAd->ate.Mode;
+		pAd->ate.Mode &= ATE_RXSTOP;
+		pAd->ate.bQARxStart = FALSE;
+//		pAd->ate.TxDoneCount = pAd->ate.TxCount;
+
+		if (atemode & ATE_TXCARR)
+		{
+			;
+		}
+		else if (atemode & ATE_TXCARRSUPP)
+		{
+			;
+		}
+
+		// We should free some resource which was allocated when ATE_TXFRAME , ATE_STOP, and ATE_TXCONT.
+		else if ((atemode & ATE_TXFRAME) || (atemode == ATE_STOP))
+		{
+			if (atemode & ATE_TXCONT)
+			{
+				;
+			}
+		}
+
+		// control
+//		pAd->ate.TxDoneCount = 0;
+//		pAd->ate.TxStatus = 0; // task Tx status // 0 --> task is idle, 1 --> task is running
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	}
+#endif // RALINK_28xx_QA //
+	else if (!strcmp(arg, "RXFRAME")) // Rx Frames
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: RXFRAME\n"));
+
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+		pAd->ate.Mode |= ATE_RXFRAME;
+
+		// Disable Tx of MAC block.
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Enable Rx of MAC block.
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: Invalid arg!\n"));
+		return FALSE;
+	}
+	RTMPusecDelay(5000);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("<=== ATECmdHandler()\n"));
+
+	return TRUE;
+}
+#endif // RT2860 //
+/*                                                           */
+/*                                                           */
+/*=======================End of RT2860=======================*/
+
+
+/*======================Start of RT2870======================*/
+/*                                                           */
+/*                                                           */
+
+
+INT	Set_ATE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	if (ATECmdHandler(pAd, arg))
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_Proc Success\n"));
+
+
+		return TRUE;
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_Proc Failed\n"));
+		return FALSE;
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE ADDR1=DA for TxFrame(AP  : To DS = 0 ; From DS = 1)
+        or
+        Set ATE ADDR3=DA for TxFrame(STA : To DS = 1 ; From DS = 0)
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_DA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR				*value;
+	INT					i;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+    for (i=0, value = rstrtok(arg, ":"); value; value = rstrtok(NULL, ":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+		AtoH(value, &pAd->ate.Addr3[i++], 1);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	if(i != 6)
+		return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_DA_Proc (DA = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAd->ate.Addr3[0],
+		pAd->ate.Addr3[1], pAd->ate.Addr3[2], pAd->ate.Addr3[3], pAd->ate.Addr3[4], pAd->ate.Addr3[5]));
+#endif // CONFIG_STA_SUPPORT //
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_DA_Proc Success\n"));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE ADDR3=SA for TxFrame(AP  : To DS = 0 ; From DS = 1)
+        or
+        Set ATE ADDR2=SA for TxFrame(STA : To DS = 1 ; From DS = 0)
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_SA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR				*value;
+	INT					i;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+    for (i=0, value = rstrtok(arg, ":"); value; value = rstrtok(NULL, ":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+		AtoH(value, &pAd->ate.Addr2[i++], 1);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	if(i != 6)
+		return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_SA_Proc (SA = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAd->ate.Addr2[0],
+		pAd->ate.Addr2[1], pAd->ate.Addr2[2], pAd->ate.Addr2[3], pAd->ate.Addr2[4], pAd->ate.Addr2[5]));
+#endif // CONFIG_STA_SUPPORT //
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_SA_Proc Success\n"));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE ADDR2=BSSID for TxFrame(AP  : To DS = 0 ; From DS = 1)
+        or
+        Set ATE ADDR1=BSSID for TxFrame(STA : To DS = 1 ; From DS = 0)
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_BSSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR				*value;
+	INT					i;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+    for (i=0, value = rstrtok(arg, ":"); value; value = rstrtok(NULL, ":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+		AtoH(value, &pAd->ate.Addr1[i++], 1);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	if(i != 6)
+		return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_BSSID_Proc (BSSID = %2X:%2X:%2X:%2X:%2X:%2X)\n",	pAd->ate.Addr1[0],
+		pAd->ate.Addr1[1], pAd->ate.Addr1[2], pAd->ate.Addr1[3], pAd->ate.Addr1[4], pAd->ate.Addr1[5]));
+#endif // CONFIG_STA_SUPPORT //
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_BSSID_Proc Success\n"));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Channel
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_CHANNEL_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR channel;
+
+	channel = simple_strtol(arg, 0, 10);
+
+	if ((channel < 1) || (channel > 216))// to allow A band channel : ((channel < 1) || (channel > 14))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_CHANNEL_Proc::Out of range, it should be in range of 1~14.\n"));
+		return FALSE;
+	}
+	pAd->ate.Channel = channel;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_CHANNEL_Proc (ATE Channel = %d)\n", pAd->ate.Channel));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_CHANNEL_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Power0
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_POWER0_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR TxPower;
+
+	TxPower = simple_strtol(arg, 0, 10);
+
+	if (pAd->ate.Channel <= 14)
+	{
+		if ((TxPower > 31) || (TxPower < 0))
+		{
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER0_Proc::Out of range (Value=%d)\n", TxPower));
+			return FALSE;
+		}
+	}
+	else// 5.5GHz
+	{
+		if ((TxPower > 15) || (TxPower < -7))
+		{
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER0_Proc::Out of range (Value=%d)\n", TxPower));
+			return FALSE;
+		}
+	}
+
+	pAd->ate.TxPower0 = TxPower;
+	ATETxPwrHandler(pAd, 0);
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_POWER0_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Power1
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_POWER1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR TxPower;
+
+	TxPower = simple_strtol(arg, 0, 10);
+
+	if (pAd->ate.Channel <= 14)
+	{
+	if ((TxPower > 31) || (TxPower < 0))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER1_Proc::Out of range (Value=%d)\n", TxPower));
+		return FALSE;
+	}
+	}
+	else
+	{
+		if ((TxPower > 15) || (TxPower < -7))
+		{
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER1_Proc::Out of range (Value=%d)\n", TxPower));
+			return FALSE;
+		}
+	}
+
+	pAd->ate.TxPower1 = TxPower;
+	ATETxPwrHandler(pAd, 1);
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_POWER1_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Antenna
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR value;
+
+	value = simple_strtol(arg, 0, 10);
+
+	if ((value > 2) || (value < 0))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_Antenna_Proc::Out of range (Value=%d)\n", value));
+		return FALSE;
+	}
+
+	pAd->ate.TxAntennaSel = value;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_Antenna_Proc (Antenna = %d)\n", pAd->ate.TxAntennaSel));
+	ATEDBGPRINT(RT_DEBUG_TRACE,("Ralink: Set_ATE_TX_Antenna_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Rx Antenna
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_RX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR value;
+
+	value = simple_strtol(arg, 0, 10);
+
+	if ((value > 3) || (value < 0))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_RX_Antenna_Proc::Out of range (Value=%d)\n", value));
+		return FALSE;
+	}
+
+	pAd->ate.RxAntennaSel = value;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_RX_Antenna_Proc (Antenna = %d)\n", pAd->ate.RxAntennaSel));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_RX_Antenna_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE RF frequence offset
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_FREQOFFSET_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR RFFreqOffset;
+	ULONG R4;
+
+	RFFreqOffset = simple_strtol(arg, 0, 10);
+
+	if(RFFreqOffset >= 64)
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_FREQOFFSET_Proc::Out of range, it should be in range of 0~63.\n"));
+		return FALSE;
+	}
+
+	pAd->ate.RFFreqOffset = RFFreqOffset;
+	R4 = pAd->ate.RFFreqOffset << 15;		// shift TX power control to correct RF register bit position
+	R4 |= (pAd->LatchRfRegs.R4 & ((~0x001f8000)));
+	pAd->LatchRfRegs.R4 = R4;
+
+	RtmpRfIoWrite(pAd);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_FREQOFFSET_Proc (RFFreqOffset = %d)\n", pAd->ate.RFFreqOffset));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_FREQOFFSET_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE RF BW
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_BW_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	int i;
+	UCHAR value = 0;
+	UCHAR BBPCurrentBW;
+
+	BBPCurrentBW = simple_strtol(arg, 0, 10);
+
+	if(BBPCurrentBW == 0)
+		pAd->ate.TxWI.BW = BW_20;
+	else
+		pAd->ate.TxWI.BW = BW_40;
+
+	if(pAd->ate.TxWI.BW == BW_20)
+	{
+		if(pAd->ate.Channel <= 14)
+		{
+ 		for (i=0; i<5; i++)
+ 		{
+				if (pAd->Tx20MPwrCfgGBand[i] != 0xffffffff)
+				{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx20MPwrCfgGBand[i]);
+					RTMPusecDelay(5000);
+				}
+			}
+		}
+		else
+		{
+			for (i=0; i<5; i++)
+			{
+				if (pAd->Tx20MPwrCfgABand[i] != 0xffffffff)
+ 			{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx20MPwrCfgABand[i]);
+ 				RTMPusecDelay(5000);
+ 			}
+ 		}
+		}
+
+		//Set BBP R4 bit[4:3]=0:0
+ 		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &value);
+ 		value &= (~0x18);
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, value);
+
+  		//Set BBP R66=0x3C
+ 		value = 0x3C;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, value);
+		//Set BBP R68=0x0B
+		//to improve Rx sensitivity.
+		value = 0x0B;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R68, value);
+		//Set BBP R69=0x16
+		value = 0x16;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, value);
+		//Set BBP R70=0x08
+		value = 0x08;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, value);
+		//Set BBP R73=0x11
+		value = 0x11;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, value);
+
+	    // If Channel=14, Bandwidth=20M and Mode=CCK, Set BBP R4 bit5=1
+	    // (Japan filter coefficients)
+	    // This segment of code will only works when ATETXMODE and ATECHANNEL
+	    // were set to MODE_CCK and 14 respectively before ATETXBW is set to 0.
+	    //=====================================================================
+		if (pAd->ate.Channel == 14)
+		{
+			int TxMode = pAd->ate.TxWI.PHYMODE;
+			if (TxMode == MODE_CCK)
+			{
+				// when Channel==14 && Mode==CCK && BandWidth==20M, BBP R4 bit5=1
+ 				ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &value);
+				value |= 0x20; //set bit5=1
+ 				ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, value);
+			}
+		}
+
+	    //=====================================================================
+		// If bandwidth != 40M, RF Reg4 bit 21 = 0.
+		pAd->LatchRfRegs.R4 &= ~0x00200000;
+		RtmpRfIoWrite(pAd);
+	}
+	else if(pAd->ate.TxWI.BW == BW_40)
+	{
+		if(pAd->ate.Channel <= 14)
+		{
+			for (i=0; i<5; i++)
+			{
+				if (pAd->Tx40MPwrCfgGBand[i] != 0xffffffff)
+				{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx40MPwrCfgGBand[i]);
+					RTMPusecDelay(5000);
+				}
+			}
+		}
+		else
+		{
+			for (i=0; i<5; i++)
+			{
+				if (pAd->Tx40MPwrCfgABand[i] != 0xffffffff)
+				{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx40MPwrCfgABand[i]);
+					RTMPusecDelay(5000);
+				}
+			}
+#ifdef DOT11_N_SUPPORT
+			if ((pAd->ate.TxWI.PHYMODE >= MODE_HTMIX) && (pAd->ate.TxWI.MCS == 7))
+			{
+    			value = 0x28;
+    			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R67, value);
+			}
+#endif // DOT11_N_SUPPORT //
+		}
+
+		//Set BBP R4 bit[4:3]=1:0
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &value);
+		value &= (~0x18);
+		value |= 0x10;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, value);
+
+		//Set BBP R66=0x3C
+		value = 0x3C;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, value);
+		//Set BBP R68=0x0C
+		//to improve Rx sensitivity.
+		value = 0x0C;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R68, value);
+		//Set BBP R69=0x1A
+		value = 0x1A;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, value);
+		//Set BBP R70=0x0A
+		value = 0x0A;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, value);
+		//Set BBP R73=0x16
+		value = 0x16;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, value);
+
+		// If bandwidth = 40M, set RF Reg4 bit 21 = 1.
+		pAd->LatchRfRegs.R4 |= 0x00200000;
+		RtmpRfIoWrite(pAd);
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_BW_Proc (BBPCurrentBW = %d)\n", pAd->ate.TxWI.BW));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_BW_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame length
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_LENGTH_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxLength = simple_strtol(arg, 0, 10);
+
+	if((pAd->ate.TxLength < 24) || (pAd->ate.TxLength > (MAX_FRAME_SIZE - 34/* == 2312 */)))
+	{
+		pAd->ate.TxLength = (MAX_FRAME_SIZE - 34/* == 2312 */);
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_LENGTH_Proc::Out of range, it should be in range of 24~%d.\n", (MAX_FRAME_SIZE - 34/* == 2312 */)));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_LENGTH_Proc (TxLength = %d)\n", pAd->ate.TxLength));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_LENGTH_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame count
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_COUNT_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxCount = simple_strtol(arg, 0, 10);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_COUNT_Proc (TxCount = %d)\n", pAd->ate.TxCount));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_COUNT_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame MCS
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_MCS_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR MCS;
+	int result;
+
+	MCS = simple_strtol(arg, 0, 10);
+	result = CheckMCSValid(pAd->ate.TxWI.PHYMODE, MCS);
+
+	if (result != -1)
+	{
+		pAd->ate.TxWI.MCS = (UCHAR)MCS;
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_MCS_Proc::Out of range, refer to rate table.\n"));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_MCS_Proc (MCS = %d)\n", pAd->ate.TxWI.MCS));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_MCS_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame Mode
+        0: MODE_CCK
+        1: MODE_OFDM
+        2: MODE_HTMIX
+        3: MODE_HTGREENFIELD
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_MODE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxWI.PHYMODE = simple_strtol(arg, 0, 10);
+
+	if(pAd->ate.TxWI.PHYMODE > 3)
+	{
+		pAd->ate.TxWI.PHYMODE = 0;
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_MODE_Proc::Out of range. it should be in range of 0~3\n"));
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("0: CCK, 1: OFDM, 2: HT_MIX, 3: HT_GREEN_FIELD.\n"));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_MODE_Proc (TxMode = %d)\n", pAd->ate.TxWI.PHYMODE));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_MODE_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame GI
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_GI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxWI.ShortGI = simple_strtol(arg, 0, 10);
+
+	if(pAd->ate.TxWI.ShortGI > 1)
+	{
+		pAd->ate.TxWI.ShortGI = 0;
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_GI_Proc::Out of range\n"));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_GI_Proc (GI = %d)\n", pAd->ate.TxWI.ShortGI));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_GI_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+    ==========================================================================
+ */
+INT	Set_ATE_RX_FER_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.bRxFer = simple_strtol(arg, 0, 10);
+
+	if (pAd->ate.bRxFer == 1)
+	{
+		pAd->ate.RxCntPerSec = 0;
+		pAd->ate.RxTotalCnt = 0;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_RX_FER_Proc (bRxFer = %d)\n", pAd->ate.bRxFer));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_RX_FER_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+INT Set_ATE_Read_RF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ate_print(KERN_EMERG "R1 = %lx\n", pAd->LatchRfRegs.R1);
+	ate_print(KERN_EMERG "R2 = %lx\n", pAd->LatchRfRegs.R2);
+	ate_print(KERN_EMERG "R3 = %lx\n", pAd->LatchRfRegs.R3);
+	ate_print(KERN_EMERG "R4 = %lx\n", pAd->LatchRfRegs.R4);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R1 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R2 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R3 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R4 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Load and Write EEPROM from a binary file prepared in advance.
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+#ifndef UCOS
+INT Set_ATE_Load_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	BOOLEAN		    ret = FALSE;
+	PUCHAR			src = EEPROM_BIN_FILE_NAME;
+	struct file		*srcf;
+	INT32 			retval, orgfsuid, orgfsgid;
+   	mm_segment_t	orgfs;
+	USHORT 			WriteEEPROM[(EEPROM_SIZE/2)];
+	UINT32			FileLength = 0;
+	UINT32 			value = simple_strtol(arg, 0, 10);
+
+	ATEDBGPRINT(RT_DEBUG_ERROR, ("===> %s (value=%d)\n\n", __func__, value));
+
+	if (value > 0)
+	{
+		/* zero the e2p buffer */
+		NdisZeroMemory((PUCHAR)WriteEEPROM, EEPROM_SIZE);
+
+		/* save uid and gid used for filesystem access.
+	    ** set user and group to 0 (root)
+	    */
+		orgfsuid = current->fsuid;
+		orgfsgid = current->fsgid;
+		/* as root */
+		current->fsuid = current->fsgid = 0;
+    	orgfs = get_fs();
+    	set_fs(KERNEL_DS);
+
+		do
+		{
+			/* open the bin file */
+			srcf = filp_open(src, O_RDONLY, 0);
+
+			if (IS_ERR(srcf))
+			{
+				ate_print("%s - Error %ld opening %s\n", __func__, -PTR_ERR(srcf), src);
+				break;
+			}
+
+			/* the object must have a read method */
+			if ((srcf->f_op == NULL) || (srcf->f_op->read == NULL))
+			{
+				ate_print("%s - %s does not have a read method\n", __func__, src);
+				break;
+			}
+
+			/* read the firmware from the file *.bin */
+			FileLength = srcf->f_op->read(srcf,
+										  (PUCHAR)WriteEEPROM,
+										  EEPROM_SIZE,
+										  &srcf->f_pos);
+
+			if (FileLength != EEPROM_SIZE)
+			{
+				ate_print("%s: error file length (=%d) in e2p.bin\n",
+					   __func__, FileLength);
+				break;
+			}
+			else
+			{
+				/* write the content of .bin file to EEPROM */
+				rt_ee_write_all(pAd, WriteEEPROM);
+				ret = TRUE;
+			}
+			break;
+		} while(TRUE);
+
+		/* close firmware file */
+		if (IS_ERR(srcf))
+		{
+				;
+		}
+		else
+		{
+			retval = filp_close(srcf, NULL);
+			if (retval)
+			{
+				ATEDBGPRINT(RT_DEBUG_ERROR, ("--> Error %d closing %s\n", -retval, src));
+
+			}
+		}
+
+		/* restore */
+		set_fs(orgfs);
+		current->fsuid = orgfsuid;
+		current->fsgid = orgfsgid;
+	}
+    ATEDBGPRINT(RT_DEBUG_ERROR, ("<=== %s (ret=%d)\n", __func__, ret));
+
+    return ret;
+
+}
+#else
+INT Set_ATE_Load_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT 			WriteEEPROM[(EEPROM_SIZE/2)];
+	struct iwreq	*wrq = (struct iwreq *)arg;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("===> %s (wrq->u.data.length = %d)\n\n", __func__, wrq->u.data.length));
+
+	if (wrq->u.data.length != EEPROM_SIZE)
+	{
+		ate_print("%s: error length (=%d) from host\n",
+			   __func__, wrq->u.data.length);
+		return FALSE;
+	}
+	else/* (wrq->u.data.length == EEPROM_SIZE) */
+	{
+		/* zero the e2p buffer */
+		NdisZeroMemory((PUCHAR)WriteEEPROM, EEPROM_SIZE);
+
+		/* fill the local buffer */
+		NdisMoveMemory((PUCHAR)WriteEEPROM, wrq->u.data.pointer, wrq->u.data.length);
+
+		do
+		{
+				/* write the content of .bin file to EEPROM */
+				rt_ee_write_all(pAd, WriteEEPROM);
+
+		} while(FALSE);
+		}
+
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("<=== %s\n", __func__));
+
+    return TRUE;
+
+}
+#endif // !UCOS //
+
+INT Set_ATE_Read_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT buffer[EEPROM_SIZE/2];
+	USHORT *p;
+	int i;
+
+	rt_ee_read_all(pAd, (USHORT *)buffer);
+	p = buffer;
+	for (i = 0; i < (EEPROM_SIZE/2); i++)
+	{
+		ate_print("%4.4x ", *p);
+		if (((i+1) % 16) == 0)
+			ate_print("\n");
+		p++;
+	}
+	return TRUE;
+}
+
+INT	Set_ATE_Show_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ate_print("Mode=%d\n", pAd->ate.Mode);
+	ate_print("TxPower0=%d\n", pAd->ate.TxPower0);
+	ate_print("TxPower1=%d\n", pAd->ate.TxPower1);
+	ate_print("TxAntennaSel=%d\n", pAd->ate.TxAntennaSel);
+	ate_print("RxAntennaSel=%d\n", pAd->ate.RxAntennaSel);
+	ate_print("BBPCurrentBW=%d\n", pAd->ate.TxWI.BW);
+	ate_print("GI=%d\n", pAd->ate.TxWI.ShortGI);
+	ate_print("MCS=%d\n", pAd->ate.TxWI.MCS);
+	ate_print("TxMode=%d\n", pAd->ate.TxWI.PHYMODE);
+	ate_print("Addr1=%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pAd->ate.Addr1[0], pAd->ate.Addr1[1], pAd->ate.Addr1[2], pAd->ate.Addr1[3], pAd->ate.Addr1[4], pAd->ate.Addr1[5]);
+	ate_print("Addr2=%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pAd->ate.Addr2[0], pAd->ate.Addr2[1], pAd->ate.Addr2[2], pAd->ate.Addr2[3], pAd->ate.Addr2[4], pAd->ate.Addr2[5]);
+	ate_print("Addr3=%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pAd->ate.Addr3[0], pAd->ate.Addr3[1], pAd->ate.Addr3[2], pAd->ate.Addr3[3], pAd->ate.Addr3[4], pAd->ate.Addr3[5]);
+	ate_print("Channel=%d\n", pAd->ate.Channel);
+	ate_print("TxLength=%d\n", pAd->ate.TxLength);
+	ate_print("TxCount=%u\n", pAd->ate.TxCount);
+	ate_print("RFFreqOffset=%d\n", pAd->ate.RFFreqOffset);
+	ate_print(KERN_EMERG "Set_ATE_Show_Proc Success\n");
+	return TRUE;
+}
+
+INT	Set_ATE_Help_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ate_print("ATE=ATESTART, ATESTOP, TXCONT, TXCARR, TXFRAME, RXFRAME\n");
+	ate_print("ATEDA\n");
+	ate_print("ATESA\n");
+	ate_print("ATEBSSID\n");
+	ate_print("ATECHANNEL, range:0~14(unless A band !)\n");
+	ate_print("ATETXPOW0, set power level of antenna 1.\n");
+	ate_print("ATETXPOW1, set power level of antenna 2.\n");
+	ate_print("ATETXANT, set TX antenna. 0:all, 1:antenna one, 2:antenna two.\n");
+	ate_print("ATERXANT, set RX antenna.0:all, 1:antenna one, 2:antenna two, 3:antenna three.\n");
+	ate_print("ATETXFREQOFFSET, set frequency offset, range 0~63\n");
+	ate_print("ATETXBW, set BandWidth, 0:20MHz, 1:40MHz.\n");
+	ate_print("ATETXLEN, set Frame length, range 24~%d\n", (MAX_FRAME_SIZE - 34/* == 2312 */));
+	ate_print("ATETXCNT, set how many frame going to transmit.\n");
+	ate_print("ATETXMCS, set MCS, reference to rate table.\n");
+	ate_print("ATETXMODE, set Mode 0:CCK, 1:OFDM, 2:HT-Mix, 3:GreenField, reference to rate table.\n");
+	ate_print("ATETXGI, set GI interval, 0:Long, 1:Short\n");
+	ate_print("ATERXFER, 0:disable Rx Frame error rate. 1:enable Rx Frame error rate.\n");
+	ate_print("ATERRF, show all RF registers.\n");
+	ate_print("ATEWRF1, set RF1 register.\n");
+	ate_print("ATEWRF2, set RF2 register.\n");
+	ate_print("ATEWRF3, set RF3 register.\n");
+	ate_print("ATEWRF4, set RF4 register.\n");
+	ate_print("ATELDE2P, load EEPROM from .bin file.\n");
+	ate_print("ATERE2P, display all EEPROM content.\n");
+	ate_print("ATESHOW, display all parameters of ATE.\n");
+	ate_print("ATEHELP, online help.\n");
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	AsicSwitchChannel() dedicated for ATE.
+
+    ==========================================================================
+*/
+VOID ATEAsicSwitchChannel(
+    IN PRTMP_ADAPTER pAd)
+{
+	UINT32 R2 = 0, R3 = DEFAULT_RF_TX_POWER, R4 = 0, Value = 0;
+	CHAR TxPwer = 0, TxPwer2 = 0;
+	UCHAR index, BbpValue = 0, R66 = 0x30;
+	RTMP_RF_REGS *RFRegTable;
+	UCHAR Channel;
+
+#ifdef RALINK_28xx_QA
+	if ((pAd->ate.bQATxStart == TRUE) || (pAd->ate.bQARxStart == TRUE))
+	{
+		if (pAd->ate.Channel != pAd->LatchRfRegs.Channel)
+		{
+			pAd->ate.Channel = pAd->LatchRfRegs.Channel;
+		}
+		return;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	Channel = pAd->ate.Channel;
+
+	// Select antenna
+	AsicAntennaSelect(pAd, Channel);
+
+	// fill Tx power value
+	TxPwer = pAd->ate.TxPower0;
+	TxPwer2 = pAd->ate.TxPower1;
+
+	RFRegTable = RF2850RegTable;
+
+	switch (pAd->RfIcType)
+	{
+		/* But only 2850 and 2750 support 5.5GHz band... */
+		case RFIC_2820:
+		case RFIC_2850:
+		case RFIC_2720:
+		case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R2 = RFRegTable[index].R2;
+					if (pAd->Antenna.field.TxPath == 1)
+					{
+						R2 |= 0x4000;	// If TXpath is 1, bit 14 = 1;
+					}
+
+					if (pAd->Antenna.field.RxPath == 2)
+					{
+						switch (pAd->ate.RxAntennaSel)
+						{
+							case 1:
+								R2 |= 0x20040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x00;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							case 2:
+								R2 |= 0x10040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x01;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							default:
+								R2 |= 0x40;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								/* Only enable two Antenna to receive. */
+								BbpValue |= 0x08;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+						}
+					}
+					else if (pAd->Antenna.field.RxPath == 1)
+					{
+						R2 |= 0x20040;	// write 1 to off RxPath
+					}
+
+					if (pAd->Antenna.field.TxPath == 2)
+					{
+						if (pAd->ate.TxAntennaSel == 1)
+						{
+							R2 |= 0x4000;	// If TX Antenna select is 1 , bit 14 = 1; Disable Ant 2
+							ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpValue);
+							BbpValue &= 0xE7;		//11100111B
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpValue);
+						}
+						else if (pAd->ate.TxAntennaSel == 2)
+						{
+							R2 |= 0x8000;	// If TX Antenna select is 2 , bit 15 = 1; Disable Ant 1
+							ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpValue);
+							BbpValue &= 0xE7;
+							BbpValue |= 0x08;
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpValue);
+						}
+						else
+						{
+							ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpValue);
+							BbpValue &= 0xE7;
+							BbpValue |= 0x10;
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpValue);
+						}
+					}
+					if (pAd->Antenna.field.RxPath == 3)
+					{
+						switch (pAd->ate.RxAntennaSel)
+						{
+							case 1:
+								R2 |= 0x20040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x00;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							case 2:
+								R2 |= 0x10040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x01;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							case 3:
+								R2 |= 0x30000;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x02;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							default:
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x10;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+						}
+					}
+
+					if (Channel > 14)
+					{
+						// initialize R3, R4
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff);
+						R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->ate.RFFreqOffset << 15);
+
+                        // According the Rory's suggestion to solve the middle range issue.
+						// 5.5G band power range: 0xF9~0X0F, TX0 Reg3 bit9/TX1 Reg4 bit6="0" means the TX power reduce 7dB
+						// R3
+						if ((TxPwer >= -7) && (TxPwer < 0))
+						{
+							TxPwer = (7+TxPwer);
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10);
+							ATEDBGPRINT(RT_DEBUG_TRACE, ("ATEAsicSwitchChannel: TxPwer=%d \n", TxPwer));
+						}
+						else
+						{
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10) | (1 << 9);
+						}
+
+						// R4
+						if ((TxPwer2 >= -7) && (TxPwer2 < 0))
+						{
+							TxPwer2 = (7+TxPwer2);
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7);
+							ATEDBGPRINT(RT_DEBUG_TRACE, ("ATEAsicSwitchChannel: TxPwer2=%d \n", TxPwer2));
+						}
+						else
+						{
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7) | (1 << 6);
+						}
+					}
+					else
+					{
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff) | (TxPwer << 9); // set TX power0
+						R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->ate.RFFreqOffset << 15) | (TxPwer2 <<6);// Set freq offset & TxPwr1
+					}
+
+					// Based on BBP current mode before changing RF channel.
+					if (pAd->ate.TxWI.BW == BW_40)
+					{
+						R4 |=0x200000;
+					}
+
+					// Update variables
+					pAd->LatchRfRegs.Channel = Channel;
+					pAd->LatchRfRegs.R1 = RFRegTable[index].R1;
+					pAd->LatchRfRegs.R2 = R2;
+					pAd->LatchRfRegs.R3 = R3;
+					pAd->LatchRfRegs.R4 = R4;
+
+					RtmpRfIoWrite(pAd);
+
+					break;
+				}
+			}
+			break;
+
+		default:
+			break;
+	}
+
+	// Change BBP setting during switch from a->g, g->a
+	if (Channel <= 14)
+	{
+	    ULONG	TxPinCfg = 0x00050F0A;// 2007.10.09 by Brian : 0x0005050A ==> 0x00050F0A
+
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+
+		/* For 1T/2R chip only... */
+	    if (pAd->NicConfig2.field.ExternalLNAForG)
+	    {
+	        ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x62);
+	    }
+	    else
+	    {
+	        ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x84);
+	    }
+
+        // According the Rory's suggestion to solve the middle range issue.
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R86, &BbpValue);
+		ASSERT((BbpValue == 0x00));
+		if ((BbpValue != 0x00))
+		{
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0x00);
+		}
+
+		// 5.5GHz band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x04);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R.
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+		}
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+		}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+	else
+	{
+	    ULONG	TxPinCfg = 0x00050F05;//2007.10.09 by Brian : 0x00050505 ==> 0x00050F05
+
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0xF2);
+
+        // According the Rory's suggestion to solve the middle range issue.
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R86, &BbpValue);
+		ASSERT((BbpValue == 0x00));
+		if ((BbpValue != 0x00))
+		{
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0x00);
+		}
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R91, &BbpValue);
+		ASSERT((BbpValue == 0x04));
+
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R92, &BbpValue);
+		ASSERT((BbpValue == 0x00));
+
+		// 5.5GHz band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x02);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R.
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+	    }
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+		}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+
+    // R66 should be set according to Channel and use 20MHz when scanning
+	if (Channel <= 14)
+	{
+		// BG band
+		R66 = 0x2E + GET_LNA_GAIN(pAd);
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+	}
+	else
+	{
+		// 5.5 GHz band
+		if (pAd->ate.TxWI.BW == BW_20)
+		{
+			R66 = (UCHAR)(0x32 + (GET_LNA_GAIN(pAd)*5)/3);
+    		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+		else
+		{
+			R66 = (UCHAR)(0x3A + (GET_LNA_GAIN(pAd)*5)/3);
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+	}
+
+	//
+	// On 11A, We should delay and wait RF/BBP to be stable
+	// and the appropriate time should be 1000 micro seconds
+	// 2005/06/05 - On 11G, We also need this delay time. Otherwise it's difficult to pass the WHQL.
+	//
+	RTMPusecDelay(1000);
+
+	if (Channel > 14)
+	{
+		// When 5.5GHz band the LSB of TxPwr will be used to reduced 7dB or not.
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, %dT) to , R1=0x%08lx, R2=0x%08lx, R3=0x%08lx, R4=0x%08lx\n",
+								  Channel,
+								  pAd->RfIcType,
+								  pAd->Antenna.field.TxPath,
+								  pAd->LatchRfRegs.R1,
+								  pAd->LatchRfRegs.R2,
+								  pAd->LatchRfRegs.R3,
+								  pAd->LatchRfRegs.R4));
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, Pwr0=%u, Pwr1=%u, %dT) to , R1=0x%08lx, R2=0x%08lx, R3=0x%08lx, R4=0x%08lx\n",
+								  Channel,
+								  pAd->RfIcType,
+								  (R3 & 0x00003e00) >> 9,
+								  (R4 & 0x000007c0) >> 6,
+								  pAd->Antenna.field.TxPath,
+								  pAd->LatchRfRegs.R1,
+								  pAd->LatchRfRegs.R2,
+								  pAd->LatchRfRegs.R3,
+								  pAd->LatchRfRegs.R4));
+    }
+}
+
+//
+// In fact, no one will call this routine so far !
+//
+/*
+	==========================================================================
+	Description:
+		Gives CCK TX rate 2 more dB TX power.
+		This routine works only in ATE mode.
+
+		calculate desired Tx power in RF R3.Tx0~5,	should consider -
+		0. if current radio is a noisy environment (pAd->DrsCounters.fNoisyEnvironment)
+		1. TxPowerPercentage
+		2. auto calibration based on TSSI feedback
+		3. extra 2 db for CCK
+		4. -10 db upon very-short distance (AvgRSSI >= -40db) to AP
+
+	NOTE: Since this routine requires the value of (pAd->DrsCounters.fNoisyEnvironment),
+		it should be called AFTER MlmeDynamicTxRateSwitching()
+	==========================================================================
+ */
+VOID ATEAsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT			i, j;
+	CHAR		DeltaPwr = 0;
+	BOOLEAN		bAutoTxAgc = FALSE;
+	UCHAR		TssiRef, *pTssiMinusBoundary, *pTssiPlusBoundary, TxAgcStep;
+	UCHAR		BbpR49 = 0, idx;
+	PCHAR		pTxAgcCompensate;
+	ULONG		TxPwr[5];
+	CHAR		Value;
+
+	/* no one calls this procedure so far */
+	if (pAd->ate.TxWI.BW == BW_40)
+	{
+		if (pAd->ate.Channel > 14)
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgGBand[4];
+		}
+	}
+	else
+	{
+		if (pAd->ate.Channel > 14)
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgGBand[4];
+		}
+	}
+
+	// TX power compensation for temperature variation based on TSSI.
+	// Do it per 4 seconds.
+	if (pAd->Mlme.OneSecPeriodicRound % 4 == 0)
+	{
+		if (pAd->ate.Channel <= 14)
+		{
+			/* bg channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			TssiRef            = pAd->TssiRefG;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryG[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryG[0];
+			TxAgcStep          = pAd->TxAgcStepG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			/* a channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			TssiRef            = pAd->TssiRefA;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryA[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryA[0];
+			TxAgcStep          = pAd->TxAgcStepA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+		{
+			/* BbpR49 is unsigned char */
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R49, &BbpR49);
+
+			/* (p) TssiPlusBoundaryG[0] = 0 = (m) TssiMinusBoundaryG[0] */
+			/* compensate: +4     +3   +2   +1    0   -1   -2   -3   -4 * steps */
+			/* step value is defined in pAd->TxAgcStepG for tx power value */
+
+			/* [4]+1+[4]   p4     p3   p2   p1   o1   m1   m2   m3   m4 */
+			/* ex:         0x00 0x15 0x25 0x45 0x88 0xA0 0xB5 0xD0 0xF0
+			   above value are examined in mass factory production */
+			/*             [4]    [3]  [2]  [1]  [0]  [1]  [2]  [3]  [4] */
+
+			/* plus is 0x10 ~ 0x40, minus is 0x60 ~ 0x90 */
+			/* if value is between p1 ~ o1 or o1 ~ s1, no need to adjust tx power */
+			/* if value is 0x65, tx power will be -= TxAgcStep*(2-1) */
+
+			if (BbpR49 > pTssiMinusBoundary[1])
+			{
+				// Reading is larger than the reference value.
+				// Check for how large we need to decrease the Tx power.
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 <= pTssiMinusBoundary[idx])  // Found the range
+						break;
+				}
+				// The index is the step we should decrease, idx = 0 means there is nothing to compensate
+//				if (R3 > (ULONG) (TxAgcStep * (idx-1)))
+					*pTxAgcCompensate = -(TxAgcStep * (idx-1));
+//				else
+//					*pTxAgcCompensate = -((UCHAR)R3);
+
+				DeltaPwr += (*pTxAgcCompensate);
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("-- Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = -%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else if (BbpR49 < pTssiPlusBoundary[1])
+			{
+				// Reading is smaller than the reference value
+				// check for how large we need to increase the Tx power
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 >= pTssiPlusBoundary[idx])   // Found the range
+						break;
+				}
+				// The index is the step we should increase, idx = 0 means there is nothing to compensate
+				*pTxAgcCompensate = TxAgcStep * (idx-1);
+				DeltaPwr += (*pTxAgcCompensate);
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("++ Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else
+			{
+				*pTxAgcCompensate = 0;
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("   Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, 0));
+			}
+		}
+	}
+	else
+	{
+		if (pAd->ate.Channel <= 14)
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+			DeltaPwr += (*pTxAgcCompensate);
+	}
+
+	/* calculate delta power based on the percentage specified from UI */
+	// E2PROM setting is calibrated for maximum TX power (i.e. 100%)
+	// We lower TX power here according to the percentage specified from UI
+	if (pAd->CommonCfg.TxPowerPercentage == 0xffffffff)       // AUTO TX POWER control
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 90)  // 91 ~ 100% & AUTO, treat as 100% in terms of mW
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 60)  // 61 ~ 90%, treat as 75% in terms of mW
+	{
+		DeltaPwr -= 1;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 30)  // 31 ~ 60%, treat as 50% in terms of mW
+	{
+		DeltaPwr -= 3;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 15)  // 16 ~ 30%, treat as 25% in terms of mW
+	{
+		DeltaPwr -= 6;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 9)   // 10 ~ 15%, treat as 12.5% in terms of mW
+	{
+		DeltaPwr -= 9;
+	}
+	else                                           // 0 ~ 9 %, treat as MIN(~3%) in terms of mW
+	{
+		DeltaPwr -= 12;
+	}
+
+	/* reset different new tx power for different TX rate */
+	for(i=0; i<5; i++)
+	{
+		if (TxPwr[i] != 0xffffffff)
+		{
+			for (j=0; j<8; j++)
+			{
+				Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F); /* 0 ~ 15 */
+
+				if ((Value + DeltaPwr) < 0)
+				{
+					Value = 0; /* min */
+				}
+				else if ((Value + DeltaPwr) > 0xF)
+				{
+					Value = 0xF; /* max */
+				}
+				else
+				{
+					Value += DeltaPwr; /* temperature compensation */
+				}
+
+				/* fill new value to CSR offset */
+				TxPwr[i] = (TxPwr[i] & ~(0x0000000F << j*4)) | (Value << j*4);
+			}
+
+			/* write tx power value to CSR */
+			/* TX_PWR_CFG_0 (8 tx rate) for	TX power for OFDM 12M/18M
+											TX power for OFDM 6M/9M
+											TX power for CCK5.5M/11M
+											TX power for CCK1M/2M */
+			/* TX_PWR_CFG_1 ~ TX_PWR_CFG_4 */
+			RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, TxPwr[i]);
+
+
+		}
+	}
+
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Write TxWI for ATE mode.
+
+	Return Value:
+		None
+	========================================================================
+*/
+#ifdef RT2860
+static VOID ATEWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC 	pOutTxWI,
+	IN	BOOLEAN			FRAG,
+	IN	BOOLEAN			CFACK,
+	IN	BOOLEAN			InsTimestamp,
+	IN	BOOLEAN 		AMPDU,
+	IN	BOOLEAN 		Ack,
+	IN	BOOLEAN 		NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN	UCHAR 			PID,
+	IN	UCHAR			TID,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	*pTransmit)
+{
+	TXWI_STRUC 		TxWI;
+	PTXWI_STRUC 	pTxWI;
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+	NdisZeroMemory(&TxWI, TXWI_SIZE);
+	pTxWI = &TxWI;
+
+	pTxWI->FRAG= FRAG;
+
+	pTxWI->CFACK = CFACK;
+	pTxWI->TS= InsTimestamp;
+	pTxWI->AMPDU = AMPDU;
+	pTxWI->ACK = Ack;
+	pTxWI->txop= Txopmode;
+
+	pTxWI->NSEQ = NSeq;
+	// John tune the performace with Intel Client in 20 MHz performance
+	if( BASize >7 )
+		BASize =7;
+
+	pTxWI->BAWinSize = BASize;
+	pTxWI->WirelessCliID = WCID;
+	pTxWI->MPDUtotalByteCount = Length;
+	pTxWI->PacketId = PID;
+
+	// If CCK or OFDM, BW must be 20
+	pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+	pTxWI->ShortGI = pTransmit->field.ShortGI;
+	pTxWI->STBC = pTransmit->field.STBC;
+
+	pTxWI->MCS = pTransmit->field.MCS;
+	pTxWI->PHYMODE = pTransmit->field.MODE;
+	pTxWI->CFACK = CfAck;
+	pTxWI->MIMOps = 0;
+	pTxWI->MpduDensity = 0;
+
+	pTxWI->PacketId = pTxWI->MCS;
+	NdisMoveMemory(pOutTxWI, &TxWI, sizeof(TXWI_STRUC));
+
+        return;
+}
+#endif // RT2860 //
+
+/*
+	========================================================================
+
+	Routine Description:
+		Disable protection for ATE.
+	========================================================================
+*/
+VOID ATEDisableAsicProtect(
+	IN		PRTMP_ADAPTER	pAd)
+{
+	PROT_CFG_STRUC	ProtCfg, ProtCfg4;
+	UINT32 Protect[6];
+	USHORT			offset;
+	UCHAR			i;
+	UINT32 MacReg = 0;
+
+	// Config ASIC RTS threshold register
+	RTMP_IO_READ32(pAd, TX_RTS_CFG, &MacReg);
+	MacReg &= 0xFF0000FF;
+	MacReg |= (pAd->CommonCfg.RtsThreshold << 8);
+	RTMP_IO_WRITE32(pAd, TX_RTS_CFG, MacReg);
+
+	// Initial common protection settings
+	RTMPZeroMemory(Protect, sizeof(Protect));
+	ProtCfg4.word = 0;
+	ProtCfg.word = 0;
+	ProtCfg.field.TxopAllowGF40 = 1;
+	ProtCfg.field.TxopAllowGF20 = 1;
+	ProtCfg.field.TxopAllowMM40 = 1;
+	ProtCfg.field.TxopAllowMM20 = 1;
+	ProtCfg.field.TxopAllowOfdm = 1;
+	ProtCfg.field.TxopAllowCck = 1;
+	ProtCfg.field.RTSThEn = 1;
+	ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+
+	// Handle legacy(B/G) protection
+	ProtCfg.field.ProtectRate = pAd->CommonCfg.RtsRate;
+	ProtCfg.field.ProtectCtrl = 0;
+	Protect[0] = ProtCfg.word;
+	Protect[1] = ProtCfg.word;
+
+	// NO PROTECT
+	// 1.All STAs in the BSS are 20/40 MHz HT
+	// 2. in ai 20/40MHz BSS
+	// 3. all STAs are 20MHz in a 20MHz BSS
+	// Pure HT. no protection.
+
+	// MM20_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 010111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+	Protect[2] = 0x01744004;
+
+	// MM40_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 111111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+	Protect[3] = 0x03f44084;
+
+	// CF20_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 010111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+	Protect[4] = 0x01744004;
+
+	// CF40_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 111111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+	Protect[5] = 0x03f44084;
+
+	pAd->CommonCfg.IOTestParm.bRTSLongProtOn = FALSE;
+
+	offset = CCK_PROT_CFG;
+	for (i = 0;i < 6;i++)
+		RTMP_IO_WRITE32(pAd, offset + i*4, Protect[i]);
+
+}
+
+
+/* There are two ways to convert Rssi */
+#if 1
+//
+// The way used with GET_LNA_GAIN().
+//
+CHAR ATEConvertToRssi(
+	IN PRTMP_ADAPTER pAd,
+	IN	CHAR	Rssi,
+	IN  UCHAR   RssiNumber)
+{
+	UCHAR	RssiOffset, LNAGain;
+
+	// Rssi equals to zero should be an invalid value
+	if (Rssi == 0)
+		return -99;
+
+	LNAGain = GET_LNA_GAIN(pAd);
+	if (pAd->LatchRfRegs.Channel > 14)
+	{
+		if (RssiNumber == 0)
+			RssiOffset = pAd->ARssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->ARssiOffset1;
+		else
+			RssiOffset = pAd->ARssiOffset2;
+	}
+	else
+	{
+		if (RssiNumber == 0)
+			RssiOffset = pAd->BGRssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->BGRssiOffset1;
+		else
+			RssiOffset = pAd->BGRssiOffset2;
+	}
+
+	return (-12 - RssiOffset - LNAGain - Rssi);
+}
+#else
+//
+// The way originally used in ATE of rt2860ap.
+//
+CHAR ATEConvertToRssi(
+	IN PRTMP_ADAPTER pAd,
+	IN	CHAR			Rssi,
+	IN  UCHAR   RssiNumber)
+{
+	UCHAR	RssiOffset, LNAGain;
+
+	// Rssi equals to zero should be an invalid value
+	if (Rssi == 0)
+		return -99;
+
+    if (pAd->LatchRfRegs.Channel > 14)
+    {
+        LNAGain = pAd->ALNAGain;
+        if (RssiNumber == 0)
+			RssiOffset = pAd->ARssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->ARssiOffset1;
+		else
+			RssiOffset = pAd->ARssiOffset2;
+    }
+    else
+    {
+        LNAGain = pAd->BLNAGain;
+        if (RssiNumber == 0)
+			RssiOffset = pAd->BGRssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->BGRssiOffset1;
+		else
+			RssiOffset = pAd->BGRssiOffset2;
+    }
+
+    return (-32 - RssiOffset + LNAGain - Rssi);
+}
+#endif /* end of #if 1 */
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set Japan filter coefficients if needed.
+	Note:
+		This routine should only be called when
+		entering TXFRAME mode or TXCONT mode.
+
+	========================================================================
+*/
+static VOID SetJapanFilter(
+	IN		PRTMP_ADAPTER	pAd)
+{
+	UCHAR			BbpData = 0;
+
+	//
+	// If Channel=14 and Bandwidth=20M and Mode=CCK, set BBP R4 bit5=1
+	// (Japan Tx filter coefficients)when (TXFRAME or TXCONT).
+	//
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BbpData);
+
+    if ((pAd->ate.TxWI.PHYMODE == MODE_CCK) && (pAd->ate.Channel == 14) && (pAd->ate.TxWI.BW == BW_20))
+    {
+        BbpData |= 0x20;    // turn on
+        ATEDBGPRINT(RT_DEBUG_TRACE, ("SetJapanFilter!!!\n"));
+    }
+    else
+    {
+		BbpData &= 0xdf;    // turn off
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ClearJapanFilter!!!\n"));
+    }
+
+	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BbpData);
+}
+
+VOID ATESampleRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN PRXWI_STRUC		pRxWI)
+{
+	/* There are two ways to collect RSSI. */
+#if 1
+	//pAd->LastRxRate = (USHORT)((pRxWI->MCS) + (pRxWI->BW <<7) + (pRxWI->ShortGI <<8)+ (pRxWI->PHYMODE <<14)) ;
+	if (pRxWI->RSSI0 != 0)
+	{
+		pAd->ate.LastRssi0	= ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI0, RSSI_0);
+		pAd->ate.AvgRssi0X8	= (pAd->ate.AvgRssi0X8 - pAd->ate.AvgRssi0) + pAd->ate.LastRssi0;
+		pAd->ate.AvgRssi0  	= pAd->ate.AvgRssi0X8 >> 3;
+	}
+	if (pRxWI->RSSI1 != 0)
+	{
+		pAd->ate.LastRssi1	= ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI1, RSSI_1);
+		pAd->ate.AvgRssi1X8	= (pAd->ate.AvgRssi1X8 - pAd->ate.AvgRssi1) + pAd->ate.LastRssi1;
+		pAd->ate.AvgRssi1	= pAd->ate.AvgRssi1X8 >> 3;
+	}
+	if (pRxWI->RSSI2 != 0)
+	{
+		pAd->ate.LastRssi2	= ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI2, RSSI_2);
+		pAd->ate.AvgRssi2X8	= (pAd->ate.AvgRssi2X8 - pAd->ate.AvgRssi2) + pAd->ate.LastRssi2;
+		pAd->ate.AvgRssi2	= pAd->ate.AvgRssi2X8 >> 3;
+	}
+
+	pAd->ate.LastSNR0 = (CHAR)(pRxWI->SNR0);// CHAR ==> UCHAR ?
+	pAd->ate.LastSNR1 = (CHAR)(pRxWI->SNR1);// CHAR ==> UCHAR ?
+
+	pAd->ate.NumOfAvgRssiSample ++;
+#else
+	pAd->ate.LastSNR0 = (CHAR)(pRxWI->SNR0);
+	pAd->ate.LastSNR1 = (CHAR)(pRxWI->SNR1);
+	pAd->ate.RxCntPerSec++;
+	pAd->ate.LastRssi0 = ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI0, RSSI_0);
+	pAd->ate.LastRssi1 = ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI1, RSSI_1);
+	pAd->ate.LastRssi2 = ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI2, RSSI_2);
+	pAd->ate.AvgRssi0X8 = (pAd->ate.AvgRssi0X8 - pAd->ate.AvgRssi0) + pAd->ate.LastRssi0;
+	pAd->ate.AvgRssi0 = pAd->ate.AvgRssi0X8 >> 3;
+	pAd->ate.AvgRssi1X8 = (pAd->ate.AvgRssi1X8 - pAd->ate.AvgRssi1) + pAd->ate.LastRssi1;
+	pAd->ate.AvgRssi1 = pAd->ate.AvgRssi1X8 >> 3;
+	pAd->ate.AvgRssi2X8 = (pAd->ate.AvgRssi2X8 - pAd->ate.AvgRssi2) + pAd->ate.LastRssi2;
+	pAd->ate.AvgRssi2 = pAd->ate.AvgRssi2X8 >> 3;
+	pAd->ate.NumOfAvgRssiSample ++;
+#endif
+}
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPStationStop(
+    IN  PRTMP_ADAPTER   pAd)
+{
+//	BOOLEAN       Cancelled;
+
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("==> RTMPStationStop\n"));
+
+#if 0
+	RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,      &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,    &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,   &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,       &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,     &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,       &Cancelled);
+#endif
+	// For rx statistics, we need to keep this timer running.
+//	RTMPCancelTimer(&pAd->Mlme.PeriodicTimer,      &Cancelled);
+
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("<== RTMPStationStop\n"));
+}
+
+VOID RTMPStationStart(
+    IN  PRTMP_ADAPTER   pAd)
+{
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("==> RTMPStationStart\n"));
+#ifdef RT2860
+	pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+	//
+	// We did not cancel this timer when entering ATE mode.
+	//
+//	RTMPSetTimer(&pAd->Mlme.PeriodicTimer, MLME_TASK_EXEC_INTV);
+#endif // RT2860 //
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("<== RTMPStationStart\n"));
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	==========================================================================
+	Description:
+		Setup Frame format.
+	NOTE:
+		This routine should only be used in ATE mode.
+	==========================================================================
+ */
+#ifdef RT2860
+static INT ATESetUpFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT32 TxIdx)
+{
+	UINT j;
+	PTXD_STRUC pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	PNDIS_PACKET pPacket;
+	PUCHAR pDest;
+	PVOID AllocVa;
+	NDIS_PHYSICAL_ADDRESS AllocPa;
+	HTTRANSMIT_SETTING	TxHTPhyMode;
+
+	PRTMP_TX_RING pTxRing = &pAd->TxRing[QID_AC_BE];
+	PTXWI_STRUC pTxWI = (PTXWI_STRUC) pTxRing->Cell[TxIdx].DmaBuf.AllocVa;
+	PUCHAR pDMAHeaderBufVA = (PUCHAR) pTxRing->Cell[TxIdx].DmaBuf.AllocVa;
+
+#ifdef RALINK_28xx_QA
+	PHEADER_802_11	pHeader80211;
+#endif // RALINK_28xx_QA //
+
+	if (pAd->ate.bQATxStart == TRUE)
+	{
+		// always use QID_AC_BE and FIFO_EDCA
+
+		// fill TxWI
+		TxHTPhyMode.field.BW = pAd->ate.TxWI.BW;
+		TxHTPhyMode.field.ShortGI = pAd->ate.TxWI.ShortGI;
+		TxHTPhyMode.field.STBC = 0;
+		TxHTPhyMode.field.MCS = pAd->ate.TxWI.MCS;
+		TxHTPhyMode.field.MODE = pAd->ate.TxWI.PHYMODE;
+		ATEWriteTxWI(pAd, pTxWI, pAd->ate.TxWI.FRAG, pAd->ate.TxWI.CFACK, pAd->ate.TxWI.TS,  pAd->ate.TxWI.AMPDU, pAd->ate.TxWI.ACK, pAd->ate.TxWI.NSEQ,
+			pAd->ate.TxWI.BAWinSize, 0, pAd->ate.TxWI.MPDUtotalByteCount, pAd->ate.TxWI.PacketId, 0, 0, pAd->ate.TxWI.txop/*IFS_HTTXOP*/, pAd->ate.TxWI.CFACK/*FALSE*/, &TxHTPhyMode);
+	}
+	else
+	{
+		TxHTPhyMode.field.BW = pAd->ate.TxWI.BW;
+		TxHTPhyMode.field.ShortGI = pAd->ate.TxWI.ShortGI;
+		TxHTPhyMode.field.STBC = 0;
+		TxHTPhyMode.field.MCS = pAd->ate.TxWI.MCS;
+		TxHTPhyMode.field.MODE = pAd->ate.TxWI.PHYMODE;
+		ATEWriteTxWI(pAd, pTxWI, FALSE, FALSE, FALSE,  FALSE, FALSE, FALSE,
+			4, 0, pAd->ate.TxLength, 0, 0, 0, IFS_HTTXOP, FALSE, &TxHTPhyMode);
+	}
+
+	// fill 802.11 header.
+#ifdef RALINK_28xx_QA
+	if (pAd->ate.bQATxStart == TRUE)
+	{
+		NdisMoveMemory(pDMAHeaderBufVA+TXWI_SIZE, pAd->ate.Header, pAd->ate.HLen);
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		NdisMoveMemory(pDMAHeaderBufVA+TXWI_SIZE, TemplateFrame, LENGTH_802_11);
+		NdisMoveMemory(pDMAHeaderBufVA+TXWI_SIZE+4, pAd->ate.Addr1, ETH_LENGTH_OF_ADDRESS);
+		NdisMoveMemory(pDMAHeaderBufVA+TXWI_SIZE+10, pAd->ate.Addr2, ETH_LENGTH_OF_ADDRESS);
+		NdisMoveMemory(pDMAHeaderBufVA+TXWI_SIZE+16, pAd->ate.Addr3, ETH_LENGTH_OF_ADDRESS);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (((PUCHAR)pDMAHeaderBufVA)+TXWI_SIZE), DIR_READ, FALSE);
+#endif // RT_BIG_ENDIAN //
+
+	/* alloc buffer for payload */
+#ifdef RALINK_28xx_QA
+	if (pAd->ate.bQATxStart == TRUE)
+	{
+		/* Why not use RTMP_AllocateTxPacketBuffer() instead of RTMP_AllocateRxPacketBuffer()? */
+		pPacket = RTMP_AllocateRxPacketBuffer(pAd, pAd->ate.DLen + 0x100, FALSE, &AllocVa, &AllocPa);
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		/* Why not use RTMP_AllocateTxPacketBuffer() instead of RTMP_AllocateRxPacketBuffer()? */
+		pPacket = RTMP_AllocateRxPacketBuffer(pAd, pAd->ate.TxLength, FALSE, &AllocVa, &AllocPa);
+	}
+
+	if (pPacket == NULL)
+	{
+		pAd->ate.TxCount = 0;
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("%s fail to alloc packet space.\n", __func__));
+		return -1;
+	}
+	pTxRing->Cell[TxIdx].pNextNdisPacket = pPacket;
+
+	pDest = (PUCHAR) AllocVa;
+
+#ifdef RALINK_28xx_QA
+	if (pAd->ate.bQATxStart == TRUE)
+	{
+		RTPKT_TO_OSPKT(pPacket)->len = pAd->ate.DLen;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		RTPKT_TO_OSPKT(pPacket)->len = pAd->ate.TxLength - LENGTH_802_11;
+	}
+
+	// Prepare frame payload
+#ifdef RALINK_28xx_QA
+	if (pAd->ate.bQATxStart == TRUE)
+	{
+		// copy pattern
+		if ((pAd->ate.PLen != 0))
+		{
+			int j;
+
+			for (j = 0; j < pAd->ate.DLen; j+=pAd->ate.PLen)
+			{
+				memcpy(RTPKT_TO_OSPKT(pPacket)->data + j, pAd->ate.Pattern, pAd->ate.PLen);
+			}
+		}
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		for(j = 0; j < RTPKT_TO_OSPKT(pPacket)->len; j++)
+			pDest[j] = 0xA5;
+	}
+
+	//
+	// build Tx Descriptor
+	//
+#ifndef RT_BIG_ENDIAN
+	pTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+#else
+    pDestTxD  = (PTXD_STRUC)pTxRing->Cell[TxIdx].AllocVa;
+    TxD = *pDestTxD;
+    pTxD = &TxD;
+#endif // !RT_BIG_ENDIAN //
+
+#ifdef RALINK_28xx_QA
+	if (pAd->ate.bQATxStart == TRUE)
+	{
+		// prepare TxD
+		NdisZeroMemory(pTxD, TXD_SIZE);
+		RTMPWriteTxDescriptor(pAd, pTxD, FALSE, FIFO_EDCA);
+		// build TX DESC
+		pTxD->SDPtr0 = RTMP_GetPhysicalAddressLow(pTxRing->Cell[TxIdx].DmaBuf.AllocPa);
+		pTxD->SDLen0 = TXWI_SIZE + pAd->ate.HLen;
+		pTxD->LastSec0 = 0;
+		pTxD->SDPtr1 = AllocPa;
+		pTxD->SDLen1 = RTPKT_TO_OSPKT(pPacket)->len;
+		pTxD->LastSec1 = 1;
+
+		pDest = (PUCHAR)pTxWI;
+		pDest += TXWI_SIZE;
+		pHeader80211 = (PHEADER_802_11)pDest;
+
+		// modify sequence number....
+		if (pAd->ate.TxDoneCount == 0)
+		{
+			pAd->ate.seq = pHeader80211->Sequence;
+		}
+		else
+			pHeader80211->Sequence = ++pAd->ate.seq;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		NdisZeroMemory(pTxD, TXD_SIZE);
+		RTMPWriteTxDescriptor(pAd, pTxD, FALSE, FIFO_EDCA);
+		// build TX DESC
+		pTxD->SDPtr0 = RTMP_GetPhysicalAddressLow (pTxRing->Cell[TxIdx].DmaBuf.AllocPa);
+		pTxD->SDLen0 = TXWI_SIZE + LENGTH_802_11;
+		pTxD->LastSec0 = 0;
+		pTxD->SDPtr1 = AllocPa;
+		pTxD->SDLen1 = RTPKT_TO_OSPKT(pPacket)->len;
+		pTxD->LastSec1 = 1;
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+	RTMPFrameEndianChange(pAd, (((PUCHAR)pDMAHeaderBufVA)+TXWI_SIZE), DIR_WRITE, FALSE);
+    RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+    WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif // RT_BIG_ENDIAN //
+	return 0;
+}
+/*                                                           */
+/*                                                           */
+/*=======================End of RT2860=======================*/
+#endif // RT2860 //
+
+
+VOID rt_ee_read_all(PRTMP_ADAPTER pAd, USHORT *Data)
+{
+	USHORT i;
+	USHORT value;
+
+	for (i = 0 ; i < EEPROM_SIZE/2 ; )
+	{
+		/* "value" is expecially for some compilers... */
+		RT28xx_EEPROM_READ16(pAd, i*2, value);
+		Data[i] = value;
+		i++;
+	}
+}
+
+VOID rt_ee_write_all(PRTMP_ADAPTER pAd, USHORT *Data)
+{
+	USHORT i;
+	USHORT value;
+
+	for (i = 0 ; i < EEPROM_SIZE/2 ; )
+	{
+		/* "value" is expecially for some compilers... */
+		value = Data[i];
+		RT28xx_EEPROM_WRITE16(pAd, i*2, value);
+		i ++;
+	}
+}
+#ifdef RALINK_28xx_QA
+VOID ATE_QA_Statistics(
+	IN PRTMP_ADAPTER			pAd,
+	IN PRXWI_STRUC				pRxWI,
+	IN PRT28XX_RXD_STRUC		pRxD,
+	IN PHEADER_802_11			pHeader)
+{
+	// update counter first
+	if (pHeader != NULL)
+	{
+		if (pHeader->FC.Type == BTYPE_DATA)
+		{
+			if (pRxD->U2M)
+				pAd->ate.U2M++;
+			else
+				pAd->ate.OtherData++;
+		}
+		else if (pHeader->FC.Type == BTYPE_MGMT)
+		{
+			if (pHeader->FC.SubType == SUBTYPE_BEACON)
+				pAd->ate.Beacon++;
+			else
+				pAd->ate.OtherCount++;
+		}
+		else if (pHeader->FC.Type == BTYPE_CNTL)
+		{
+			pAd->ate.OtherCount++;
+		}
+	}
+	pAd->ate.RSSI0 = pRxWI->RSSI0;
+	pAd->ate.RSSI1 = pRxWI->RSSI1;
+	pAd->ate.RSSI2 = pRxWI->RSSI2;
+	pAd->ate.SNR0 = pRxWI->SNR0;
+	pAd->ate.SNR1 = pRxWI->SNR1;
+}
+
+/* command id with Cmd Type == 0x0008(for 28xx)/0x0005(for iNIC) */
+#define RACFG_CMD_RF_WRITE_ALL		0x0000
+#define RACFG_CMD_E2PROM_READ16		0x0001
+#define RACFG_CMD_E2PROM_WRITE16	0x0002
+#define RACFG_CMD_E2PROM_READ_ALL	0x0003
+#define RACFG_CMD_E2PROM_WRITE_ALL	0x0004
+#define RACFG_CMD_IO_READ			0x0005
+#define RACFG_CMD_IO_WRITE			0x0006
+#define RACFG_CMD_IO_READ_BULK		0x0007
+#define RACFG_CMD_BBP_READ8			0x0008
+#define RACFG_CMD_BBP_WRITE8		0x0009
+#define RACFG_CMD_BBP_READ_ALL		0x000a
+#define RACFG_CMD_GET_COUNTER		0x000b
+#define RACFG_CMD_CLEAR_COUNTER		0x000c
+
+#define RACFG_CMD_RSV1				0x000d
+#define RACFG_CMD_RSV2				0x000e
+#define RACFG_CMD_RSV3				0x000f
+
+#define RACFG_CMD_TX_START			0x0010
+#define RACFG_CMD_GET_TX_STATUS		0x0011
+#define RACFG_CMD_TX_STOP			0x0012
+#define RACFG_CMD_RX_START			0x0013
+#define RACFG_CMD_RX_STOP			0x0014
+#define RACFG_CMD_GET_NOISE_LEVEL	0x0015
+
+#define RACFG_CMD_ATE_START			0x0080
+#define RACFG_CMD_ATE_STOP			0x0081
+
+#define RACFG_CMD_ATE_START_TX_CARRIER		0x0100
+#define RACFG_CMD_ATE_START_TX_CONT			0x0101
+#define RACFG_CMD_ATE_START_TX_FRAME		0x0102
+#define RACFG_CMD_ATE_SET_BW	            0x0103
+#define RACFG_CMD_ATE_SET_TX_POWER0	        0x0104
+#define RACFG_CMD_ATE_SET_TX_POWER1			0x0105
+#define RACFG_CMD_ATE_SET_FREQ_OFFSET		0x0106
+#define RACFG_CMD_ATE_GET_STATISTICS		0x0107
+#define RACFG_CMD_ATE_RESET_COUNTER			0x0108
+#define RACFG_CMD_ATE_SEL_TX_ANTENNA		0x0109
+#define RACFG_CMD_ATE_SEL_RX_ANTENNA		0x010a
+#define RACFG_CMD_ATE_SET_PREAMBLE			0x010b
+#define RACFG_CMD_ATE_SET_CHANNEL			0x010c
+#define RACFG_CMD_ATE_SET_ADDR1				0x010d
+#define RACFG_CMD_ATE_SET_ADDR2				0x010e
+#define RACFG_CMD_ATE_SET_ADDR3				0x010f
+#define RACFG_CMD_ATE_SET_RATE				0x0110
+#define RACFG_CMD_ATE_SET_TX_FRAME_LEN		0x0111
+#define RACFG_CMD_ATE_SET_TX_FRAME_COUNT	0x0112
+#define RACFG_CMD_ATE_START_RX_FRAME		0x0113
+#define RACFG_CMD_ATE_E2PROM_READ_BULK	0x0114
+#define RACFG_CMD_ATE_E2PROM_WRITE_BULK	0x0115
+#define RACFG_CMD_ATE_IO_WRITE_BULK		0x0116
+#define RACFG_CMD_ATE_BBP_READ_BULK		0x0117
+#define RACFG_CMD_ATE_BBP_WRITE_BULK	0x0118
+#define RACFG_CMD_ATE_RF_READ_BULK		0x0119
+#define RACFG_CMD_ATE_RF_WRITE_BULK		0x011a
+
+
+
+#define A2Hex(_X, _p) 				\
+{									\
+	UCHAR *p;						\
+	_X = 0;							\
+	p = _p;							\
+	while (((*p >= 'a') && (*p <= 'f')) || ((*p >= 'A') && (*p <= 'F')) || ((*p >= '0') && (*p <= '9')))		\
+	{												\
+		if ((*p >= 'a') && (*p <= 'f'))				\
+			_X = _X * 16 + *p - 87;					\
+		else if ((*p >= 'A') && (*p <= 'F'))		\
+			_X = _X * 16 + *p - 55;					\
+		else if ((*p >= '0') && (*p <= '9'))		\
+			_X = _X * 16 + *p - 48;					\
+		p++;										\
+	}												\
+}
+
+
+static VOID memcpy_exl(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len);
+static VOID memcpy_exs(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len);
+static VOID RTMP_IO_READ_BULK(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, UINT32 len);
+
+#ifdef UCOS
+int ate_copy_to_user(
+	IN PUCHAR payload,
+	IN PUCHAR msg,
+	IN INT    len)
+{
+	memmove(payload, msg, len);
+	return 0;
+}
+
+#undef	copy_to_user
+#define copy_to_user(x,y,z) ate_copy_to_user((PUCHAR)x, (PUCHAR)y, z)
+#endif // UCOS //
+
+#define	LEN_OF_ARG 16
+
+VOID RtmpDoAte(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	unsigned short Command_Id;
+	struct ate_racfghdr *pRaCfg;
+	INT	Status = NDIS_STATUS_SUCCESS;
+
+
+
+	if((pRaCfg = kmalloc(sizeof(struct ate_racfghdr), GFP_KERNEL)) == NULL)
+	{
+		Status = -EINVAL;
+		return;
+	}
+
+	NdisZeroMemory(pRaCfg, sizeof(struct ate_racfghdr));
+
+    if (copy_from_user((PUCHAR)pRaCfg, wrq->u.data.pointer, wrq->u.data.length))
+	{
+		Status = -EFAULT;
+		kfree(pRaCfg);
+		return;
+	}
+
+
+	Command_Id = ntohs(pRaCfg->command_id);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE,("\n%s: Command_Id = 0x%04x !\n", __func__, Command_Id));
+
+	switch (Command_Id)
+	{
+ 		// We will get this command when QA starts.
+		case RACFG_CMD_ATE_START:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START\n"));
+
+				// prepare feedback as soon as we can to avoid QA timeout.
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_TRACE, ("copy_to_user() fail in case RACFG_CMD_ATE_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START is done !\n"));
+				}
+				Set_ATE_Proc(pAdapter, "ATESTART");
+			}
+			break;
+
+ 		// We will get this command either QA is closed or ated is killed by user.
+		case RACFG_CMD_ATE_STOP:
+			{
+#ifndef UCOS
+				INT32 ret;
+#endif // !UCOS //
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_STOP\n"));
+
+				// Distinguish this command came from QA(via ated)
+				// or ate daemon according to the existence of pid in payload.
+				// No need to prepare feedback if this cmd came directly from ate daemon.
+				pRaCfg->length = ntohs(pRaCfg->length);
+
+				if (pRaCfg->length == sizeof(pAdapter->ate.AtePid))
+				{
+					// This command came from QA.
+					// Get the pid of ATE daemon.
+					memcpy((UCHAR *)&pAdapter->ate.AtePid,
+						(&pRaCfg->data[0]) - 2/* == &(pRaCfg->status) */,
+						sizeof(pAdapter->ate.AtePid));
+
+					// prepare feedback as soon as we can to avoid QA timeout.
+					pRaCfg->length = htons(2);
+					pRaCfg->status = htons(0);
+
+					wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+										+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+										+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+	            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+	            	{
+	            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_STOP\n"));
+	                    Status = -EFAULT;
+	            	}
+
+					//
+					// kill ATE daemon when leaving ATE mode.
+					// We must kill ATE daemon first before setting ATESTOP,
+					// or Microsoft will report sth. wrong.
+#ifndef UCOS
+					ret = KILL_THREAD_PID(pAdapter->ate.AtePid, SIGTERM, 1);
+					if (ret)
+					{
+						ATEDBGPRINT(RT_DEBUG_ERROR, ("%s: unable to signal thread\n", pAdapter->net_dev->name));
+					}
+#endif // !UCOS //
+				}
+
+				// AP might have in ATE_STOP mode due to cmd from QA.
+				if (ATE_ON(pAdapter))
+				{
+					// Someone has killed ate daemon while QA GUI is still open.
+					Set_ATE_Proc(pAdapter, "ATESTOP");
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_AP_START is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_RF_WRITE_ALL:
+			{
+				UINT32 R1, R2, R3, R4;
+				USHORT channel;
+
+				memcpy(&R1, pRaCfg->data-2, 4);
+				memcpy(&R2, pRaCfg->data+2, 4);
+				memcpy(&R3, pRaCfg->data+6, 4);
+				memcpy(&R4, pRaCfg->data+10, 4);
+				memcpy(&channel, pRaCfg->data+14, 2);
+
+				pAdapter->LatchRfRegs.R1 = ntohl(R1);
+				pAdapter->LatchRfRegs.R2 = ntohl(R2);
+				pAdapter->LatchRfRegs.R3 = ntohl(R3);
+				pAdapter->LatchRfRegs.R4 = ntohl(R4);
+				pAdapter->LatchRfRegs.Channel = ntohs(channel);
+
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R1);
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R2);
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R3);
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R4);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RF_WRITE_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RF_WRITE_ALL is done !\n"));
+				}
+			}
+            break;
+
+		case RACFG_CMD_E2PROM_READ16:
+			{
+				USHORT	offset, value, tmp;
+
+				offset = ntohs(pRaCfg->status);
+				/* "tmp" is expecially for some compilers... */
+				RT28xx_EEPROM_READ16(pAdapter, offset, tmp);
+				value = tmp;
+				value = htons(value);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("EEPROM Read offset = 0x%04x, value = 0x%04x\n", offset, value));
+
+				// prepare feedback
+				pRaCfg->length = htons(4);
+				pRaCfg->status = htons(0);
+				memcpy(pRaCfg->data, &value, 2);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("sizeof(struct ate_racfghdr) = %d\n", sizeof(struct ate_racfghdr)));
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_READ16\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_E2PROM_READ16 is done !\n"));
+				}
+           	}
+			break;
+
+		case RACFG_CMD_E2PROM_WRITE16:
+			{
+				USHORT	offset, value;
+
+				offset = ntohs(pRaCfg->status);
+				memcpy(&value, pRaCfg->data, 2);
+				value = ntohs(value);
+				RT28xx_EEPROM_WRITE16(pAdapter, offset, value);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_WRITE16\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_E2PROM_WRITE16 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_E2PROM_READ_ALL:
+			{
+				USHORT buffer[EEPROM_SIZE/2];
+
+				rt_ee_read_all(pAdapter,(USHORT *)buffer);
+				memcpy_exs(pAdapter, pRaCfg->data, (UCHAR *)buffer, EEPROM_SIZE);
+
+				// prepare feedback
+				pRaCfg->length = htons(2+EEPROM_SIZE);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_READ_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_E2PROM_READ_ALL is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_E2PROM_WRITE_ALL:
+			{
+				USHORT buffer[EEPROM_SIZE/2];
+
+				NdisZeroMemory((UCHAR *)buffer, EEPROM_SIZE);
+				memcpy_exs(pAdapter, (UCHAR *)buffer, (UCHAR *)&pRaCfg->status, EEPROM_SIZE);
+				rt_ee_write_all(pAdapter,(USHORT *)buffer);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_WRITE_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_ERROR, ("RACFG_CMD_E2PROM_WRITE_ALL is done !\n"));
+				}
+
+			}
+			break;
+
+		case RACFG_CMD_IO_READ:
+			{
+				UINT32	offset;
+				UINT32	value;
+
+				memcpy(&offset, &pRaCfg->status, 4);
+				offset = ntohl(offset);
+
+				// We do not need the base address.
+				// So just extract the offset out.
+				offset &= 0x0000FFFF;
+				RTMP_IO_READ32(pAdapter, offset, &value);
+				value = htonl(value);
+
+				// prepare feedback
+				pRaCfg->length = htons(6);
+				pRaCfg->status = htons(0);
+				memcpy(pRaCfg->data, &value, 4);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_IO_READ\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_IO_READ is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_IO_WRITE:
+			{
+				UINT32	offset, value;
+
+				memcpy(&offset, pRaCfg->data-2, 4);
+				memcpy(&value, pRaCfg->data+2, 4);
+
+				offset = ntohl(offset);
+
+				// We do not need the base address.
+				// So just extract out the offset.
+				offset &= 0x0000FFFF;
+				value = ntohl(value);
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_IO_WRITE: offset = %x, value = %x\n", offset, value));
+				RTMP_IO_WRITE32(pAdapter, offset, value);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_IO_WRITE\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_IO_WRITE is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_IO_READ_BULK:
+			{
+				UINT32	offset;
+				USHORT	len;
+
+				memcpy(&offset, &pRaCfg->status, 4);
+				offset = ntohl(offset);
+
+				// We do not need the base address.
+				// So just extract the offset.
+				offset &= 0x0000FFFF;
+				memcpy(&len, pRaCfg->data+2, 2);
+				len = ntohs(len);
+
+				if (len > 371)
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE,("len is too large, make it smaller\n"));
+					pRaCfg->length = htons(2);
+					pRaCfg->status = htons(1);
+					break;
+				}
+
+				RTMP_IO_READ_BULK(pAdapter, pRaCfg->data, (UCHAR *)offset, len*4);// unit in four bytes
+
+				// prepare feedback
+				pRaCfg->length = htons(2+len*4);// unit in four bytes
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_IO_READ_BULK\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_IO_READ_BULK is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_BBP_READ8:
+			{
+				USHORT	offset;
+				UCHAR	value;
+
+				value = 0;
+				offset = ntohs(pRaCfg->status);
+
+				if (ATE_ON(pAdapter))
+				{
+					ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, offset,  &value);
+				}
+				else
+				{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, offset,  &value);
+				}
+				// prepare feedback
+				pRaCfg->length = htons(3);
+				pRaCfg->status = htons(0);
+				pRaCfg->data[0] = value;
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("BBP value = %x\n", value));
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_BBP_READ8\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_BBP_READ8 is done !\n"));
+				}
+			}
+			break;
+		case RACFG_CMD_BBP_WRITE8:
+			{
+				USHORT	offset;
+				UCHAR	value;
+
+				offset = ntohs(pRaCfg->status);
+				memcpy(&value, pRaCfg->data, 1);
+
+				if (ATE_ON(pAdapter))
+				{
+					ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, offset,  value);
+				}
+				else
+				{
+					RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, offset,  value);
+				}
+
+				if ((offset == BBP_R1) || (offset == BBP_R3))
+				{
+					SyncTxRxConfig(pAdapter, offset, value);
+				}
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_BBP_WRITE8\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_BBP_WRITE8 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_BBP_READ_ALL:
+			{
+				USHORT j;
+
+				for (j = 0; j < 137; j++)
+				{
+					pRaCfg->data[j] = 0;
+
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j]);
+					}
+					else
+					{
+						RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j]);
+					}
+				}
+
+				// prepare feedback
+				pRaCfg->length = htons(2+137);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_BBP_READ_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_BBP_READ_ALL is done !\n"));
+				}
+			}
+
+			break;
+
+		case RACFG_CMD_ATE_E2PROM_READ_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT buffer[EEPROM_SIZE/2];
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			rt_ee_read_all(pAdapter,(USHORT *)buffer);
+			if (offset + len <= EEPROM_SIZE)
+				memcpy_exs(pAdapter, pRaCfg->data, (UCHAR *)buffer+offset, len);
+			else
+				ATEDBGPRINT(RT_DEBUG_ERROR, ("exceed EEPROM size\n"));
+
+			// prepare feedback
+			pRaCfg->length = htons(2+len);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_E2PROM_READ_BULK\n"));
+                Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_E2PROM_READ_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_E2PROM_WRITE_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT buffer[EEPROM_SIZE/2];
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			rt_ee_read_all(pAdapter,(USHORT *)buffer);
+			memcpy_exs(pAdapter, (UCHAR *)buffer + offset, (UCHAR *)pRaCfg->data + 2, len);
+			rt_ee_write_all(pAdapter,(USHORT *)buffer);
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_E2PROM_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_ERROR, ("RACFG_CMD_ATE_E2PROM_WRITE_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_IO_WRITE_BULK:
+		{
+			UINT32 offset, i, value;
+			USHORT len;
+
+			memcpy(&offset, &pRaCfg->status, 4);
+			offset = ntohl(offset);
+			memcpy(&len, pRaCfg->data+2, 2);
+			len = ntohs(len);
+
+			for (i = 0; i < len; i += 4)
+			{
+				memcpy_exl(pAdapter, (UCHAR *)&value, pRaCfg->data+4+i, 4);
+				printk("Write %x %x\n", offset + i, value);
+				RTMP_IO_WRITE32(pAdapter, (offset +i) & 0xffff, value);
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_IO_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_ERROR, ("RACFG_CMD_ATE_IO_WRITE_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_BBP_READ_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				pRaCfg->data[j - offset] = 0;
+
+				if (pAdapter->ate.Mode == ATE_STOP)
+				{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j - offset]);
+				}
+				else
+				{
+					ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j - offset]);
+				}
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2+len);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_BBP_READ_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_BBP_READ_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_BBP_WRITE_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+			UCHAR *value;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				value = pRaCfg->data + 2 + (j - offset);
+				if (pAdapter->ate.Mode == ATE_STOP)
+				{
+					RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, j,  *value);
+				}
+				else
+				{
+					ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, j,  *value);
+				}
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_BBP_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_BBP_WRITE_BULK is done !\n"));
+			}
+		}
+			break;
+
+#ifdef CONFIG_RALINK_RT3052
+		case RACFG_CMD_ATE_RF_READ_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				pRaCfg->data[j - offset] = 0;
+				RT30xxReadRFRegister(pAdapter, j,  &pRaCfg->data[j - offset]);
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2+len);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_RF_READ_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_RF_READ_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_RF_WRITE_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+			UCHAR *value;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				value = pRaCfg->data + 2 + (j - offset);
+				RT30xxWriteRFRegister(pAdapter, j,  *value);
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_RF_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_RF_WRITE_BULK is done !\n"));
+			}
+
+		}
+			break;
+#endif
+
+
+		case RACFG_CMD_GET_NOISE_LEVEL:
+			{
+				UCHAR	channel;
+				INT32   buffer[3][10];/* 3 : RxPath ; 10 : no. of per rssi samples */
+
+				channel = (ntohs(pRaCfg->status) & 0x00FF);
+				CalNoiseLevel(pAdapter, channel, buffer);
+				memcpy_exl(pAdapter, (UCHAR *)pRaCfg->data, (UCHAR *)&(buffer[0][0]), (sizeof(INT32)*3*10));
+
+				// prepare feedback
+				pRaCfg->length = htons(2 + (sizeof(INT32)*3*10));
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_GET_NOISE_LEVEL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_GET_NOISE_LEVEL is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_GET_COUNTER:
+			{
+				memcpy_exl(pAdapter, &pRaCfg->data[0], (UCHAR *)&pAdapter->ate.U2M, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[4], (UCHAR *)&pAdapter->ate.OtherData, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[8], (UCHAR *)&pAdapter->ate.Beacon, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[12], (UCHAR *)&pAdapter->ate.OtherCount, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[16], (UCHAR *)&pAdapter->ate.TxAc0, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[20], (UCHAR *)&pAdapter->ate.TxAc1, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[24], (UCHAR *)&pAdapter->ate.TxAc2, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[28], (UCHAR *)&pAdapter->ate.TxAc3, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[32], (UCHAR *)&pAdapter->ate.TxHCCA, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[36], (UCHAR *)&pAdapter->ate.TxMgmt, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[40], (UCHAR *)&pAdapter->ate.RSSI0, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[44], (UCHAR *)&pAdapter->ate.RSSI1, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[48], (UCHAR *)&pAdapter->ate.RSSI2, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[52], (UCHAR *)&pAdapter->ate.SNR0, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[56], (UCHAR *)&pAdapter->ate.SNR1, 4);
+
+				pRaCfg->length = htons(2+60);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_GET_COUNTER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_GET_COUNTER is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_CLEAR_COUNTER:
+			{
+				pAdapter->ate.U2M = 0;
+				pAdapter->ate.OtherData = 0;
+				pAdapter->ate.Beacon = 0;
+				pAdapter->ate.OtherCount = 0;
+				pAdapter->ate.TxAc0 = 0;
+				pAdapter->ate.TxAc1 = 0;
+				pAdapter->ate.TxAc2 = 0;
+				pAdapter->ate.TxAc3 = 0;
+				pAdapter->ate.TxHCCA = 0;
+				pAdapter->ate.TxMgmt = 0;
+				pAdapter->ate.TxDoneCount = 0;
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_CLEAR_COUNTER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_CLEAR_COUNTER is done !\n"));
+				}
+			}
+
+			break;
+
+		case RACFG_CMD_TX_START:
+			{
+				USHORT *p;
+				USHORT	err = 1;
+				UCHAR   Bbp22Value = 0, Bbp24Value = 0;
+
+				if ((pAdapter->ate.TxStatus != 0) && (pAdapter->ate.Mode & ATE_TXFRAME))
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE,("Ate Tx is already running, to run next Tx, you must stop it first\n"));
+					err = 2;
+					goto TX_START_ERROR;
+				}
+				else if ((pAdapter->ate.TxStatus != 0) && !(pAdapter->ate.Mode & ATE_TXFRAME))
+				{
+					int i = 0;
+
+					while ((i++ < 10) && (pAdapter->ate.TxStatus != 0))
+					{
+						RTMPusecDelay(5000);
+					}
+
+					// force it to stop
+					pAdapter->ate.TxStatus = 0;
+					pAdapter->ate.TxDoneCount = 0;
+					//pAdapter->ate.Repeat = 0;
+					pAdapter->ate.bQATxStart = FALSE;
+				}
+
+				// If pRaCfg->length == 0, this "RACFG_CMD_TX_START" is for Carrier test or Carrier Suppression.
+				if (ntohs(pRaCfg->length) != 0)
+				{
+					// Get frame info
+
+					NdisMoveMemory(&pAdapter->ate.TxWI, pRaCfg->data + 2, 16);
+#ifdef RT_BIG_ENDIAN
+					RTMPWIEndianChange((PUCHAR)&pAdapter->ate.TxWI, TYPE_TXWI);
+#endif // RT_BIG_ENDIAN //
+
+					NdisMoveMemory(&pAdapter->ate.TxCount, pRaCfg->data + 18, 4);
+					pAdapter->ate.TxCount = ntohl(pAdapter->ate.TxCount);
+
+					p = (USHORT *)(&pRaCfg->data[22]);
+					//p = pRaCfg->data + 22;
+					// always use QID_AC_BE
+					pAdapter->ate.QID = 0;
+					p = (USHORT *)(&pRaCfg->data[24]);
+					//p = pRaCfg->data + 24;
+					pAdapter->ate.HLen = ntohs(*p);
+
+					if (pAdapter->ate.HLen > 32)
+					{
+						ATEDBGPRINT(RT_DEBUG_ERROR,("pAdapter->ate.HLen > 32\n"));
+						err = 3;
+						goto TX_START_ERROR;
+					}
+
+					NdisMoveMemory(&pAdapter->ate.Header, pRaCfg->data + 26, pAdapter->ate.HLen);
+
+
+					pAdapter->ate.PLen = ntohs(pRaCfg->length) - (pAdapter->ate.HLen + 28);
+
+					if (pAdapter->ate.PLen > 32)
+					{
+						ATEDBGPRINT(RT_DEBUG_ERROR,("pAdapter->ate.PLen > 32\n"));
+						err = 4;
+						goto TX_START_ERROR;
+					}
+
+					NdisMoveMemory(&pAdapter->ate.Pattern, pRaCfg->data + 26 + pAdapter->ate.HLen, pAdapter->ate.PLen);
+					pAdapter->ate.DLen = pAdapter->ate.TxWI.MPDUtotalByteCount - pAdapter->ate.HLen;
+				}
+
+				ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R22, &Bbp22Value);
+
+				switch (Bbp22Value)
+				{
+					case BBP22_TXFRAME:
+						{
+							if (pAdapter->ate.TxCount == 0)
+							{
+#ifdef RT2860
+								pAdapter->ate.TxCount = 0xFFFFFFFF;
+#endif // RT2860 //
+							}
+							ATEDBGPRINT(RT_DEBUG_TRACE,("START TXFRAME\n"));
+							pAdapter->ate.bQATxStart = TRUE;
+							Set_ATE_Proc(pAdapter, "TXFRAME");
+						}
+						break;
+
+					case BBP22_TXCONT_OR_CARRSUPP:
+						{
+							ATEDBGPRINT(RT_DEBUG_TRACE,("BBP22_TXCONT_OR_CARRSUPP\n"));
+							ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, 24, &Bbp24Value);
+
+							switch (Bbp24Value)
+							{
+								case BBP24_TXCONT:
+									{
+										ATEDBGPRINT(RT_DEBUG_TRACE,("START TXCONT\n"));
+										pAdapter->ate.bQATxStart = TRUE;
+										Set_ATE_Proc(pAdapter, "TXCONT");
+									}
+									break;
+
+								case BBP24_CARRSUPP:
+									{
+										ATEDBGPRINT(RT_DEBUG_TRACE,("START TXCARRSUPP\n"));
+										pAdapter->ate.bQATxStart = TRUE;
+										pAdapter->ate.Mode |= ATE_TXCARRSUPP;
+									}
+									break;
+
+								default:
+									{
+										ATEDBGPRINT(RT_DEBUG_ERROR,("Unknown Start TX subtype !"));
+									}
+									break;
+							}
+						}
+						break;
+
+					case BBP22_TXCARR:
+						{
+							ATEDBGPRINT(RT_DEBUG_TRACE,("START TXCARR\n"));
+							pAdapter->ate.bQATxStart = TRUE;
+							Set_ATE_Proc(pAdapter, "TXCARR");
+						}
+						break;
+
+					default:
+						{
+							ATEDBGPRINT(RT_DEBUG_ERROR,("Unknown Start TX subtype !"));
+						}
+						break;
+				}
+
+				if (pAdapter->ate.bQATxStart == TRUE)
+				{
+					// prepare feedback
+					pRaCfg->length = htons(2);
+					pRaCfg->status = htons(0);
+
+					wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+										+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+										+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+	            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+	            	{
+	            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() was failed in case RACFG_CMD_TX_START\n"));
+	                    Status = -EFAULT;
+	            	}
+					else
+					{
+	                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_TX_START is done !\n"));
+					}
+					break;
+				}
+
+TX_START_ERROR:
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(err);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_TX_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("feedback of TX_START_ERROR is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_GET_TX_STATUS:
+			{
+				UINT32 count;
+
+				// prepare feedback
+				pRaCfg->length = htons(6);
+				pRaCfg->status = htons(0);
+				count = htonl(pAdapter->ate.TxDoneCount);
+				NdisMoveMemory(pRaCfg->data, &count, 4);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_GET_TX_STATUS\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_GET_TX_STATUS is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_TX_STOP:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_TX_STOP\n"));
+
+				Set_ATE_Proc(pAdapter, "TXSTOP");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_TRACE, ("copy_to_user() fail in case RACFG_CMD_TX_STOP\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_TX_STOP is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_RX_START:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_RX_START\n"));
+
+				pAdapter->ate.bQARxStart = TRUE;
+				Set_ATE_Proc(pAdapter, "RXFRAME");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RX_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RX_START is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_RX_STOP:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_RX_STOP\n"));
+
+				Set_ATE_Proc(pAdapter, "RXSTOP");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RX_STOP\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RX_STOP is done !\n"));
+				}
+			}
+			break;
+
+		/* The following cases are for new ATE GUI(not QA). */
+		/*==================================================*/
+		case RACFG_CMD_ATE_START_TX_CARRIER:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START_TX_CARRIER\n"));
+
+				Set_ATE_Proc(pAdapter, "TXCARR");
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_START_TX_CARRIER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START_TX_CARRIER is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_START_TX_CONT:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START_TX_CONT\n"));
+
+				Set_ATE_Proc(pAdapter, "TXCONT");
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_START_TX_CONT\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START_TX_CONT is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_START_TX_FRAME:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START_TX_FRAME\n"));
+
+				Set_ATE_Proc(pAdapter, "TXFRAME");
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_START_TX_FRAME\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START_TX_FRAME is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_BW:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_BW\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+
+				Set_ATE_TX_BW_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_BW\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_BW is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_POWER0:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_POWER0\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_POWER0_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_POWER0\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_POWER0 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_POWER1:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_POWER1\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_POWER1_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_POWER1\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_POWER1 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_FREQ_OFFSET:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_FREQ_OFFSET\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_FREQOFFSET_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_FREQ_OFFSET\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_FREQ_OFFSET is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_GET_STATISTICS:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_GET_STATISTICS\n"));
+
+				memcpy_exl(pAdapter, &pRaCfg->data[0], (UCHAR *)&pAdapter->ate.TxDoneCount, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[4], (UCHAR *)&pAdapter->WlanCounters.RetryCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[8], (UCHAR *)&pAdapter->WlanCounters.FailedCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[12], (UCHAR *)&pAdapter->WlanCounters.RTSSuccessCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[16], (UCHAR *)&pAdapter->WlanCounters.RTSFailureCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[20], (UCHAR *)&pAdapter->WlanCounters.ReceivedFragmentCount.QuadPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[24], (UCHAR *)&pAdapter->WlanCounters.FCSErrorCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[28], (UCHAR *)&pAdapter->Counters8023.RxNoBuffer, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[32], (UCHAR *)&pAdapter->WlanCounters.FrameDuplicateCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[36], (UCHAR *)&pAdapter->RalinkCounters.OneSecFalseCCACnt, 4);
+
+				if (pAdapter->ate.RxAntennaSel == 0)
+				{
+					INT32 RSSI0 = 0;
+					INT32 RSSI1 = 0;
+					INT32 RSSI2 = 0;
+
+					RSSI0 = (INT32)(pAdapter->ate.LastRssi0 - pAdapter->BbpRssiToDbmDelta);
+					RSSI1 = (INT32)(pAdapter->ate.LastRssi1 - pAdapter->BbpRssiToDbmDelta);
+					RSSI2 = (INT32)(pAdapter->ate.LastRssi2 - pAdapter->BbpRssiToDbmDelta);
+					memcpy_exl(pAdapter, &pRaCfg->data[40], (UCHAR *)&RSSI0, 4);
+					memcpy_exl(pAdapter, &pRaCfg->data[44], (UCHAR *)&RSSI1, 4);
+					memcpy_exl(pAdapter, &pRaCfg->data[48], (UCHAR *)&RSSI2, 4);
+					pRaCfg->length = htons(2+52);
+				}
+				else
+				{
+					INT32 RSSI0 = 0;
+
+					RSSI0 = (INT32)(pAdapter->ate.LastRssi0 - pAdapter->BbpRssiToDbmDelta);
+					memcpy_exl(pAdapter, &pRaCfg->data[40], (UCHAR *)&RSSI0, 4);
+					pRaCfg->length = htons(2+44);
+				}
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_GET_STATISTICS\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_GET_STATISTICS is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_RESET_COUNTER:
+			{
+				SHORT    value = 1;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_RESET_COUNTER\n"));
+
+				sprintf((PCHAR)str, "%d", value);
+				Set_ResetStatCounter_Proc(pAdapter, str);
+
+				pAdapter->ate.TxDoneCount = 0;
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_RESET_COUNTER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_RESET_COUNTER is done !\n"));
+				}
+			}
+
+			break;
+
+		case RACFG_CMD_ATE_SEL_TX_ANTENNA:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SEL_TX_ANTENNA\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_Antenna_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SEL_TX_ANTENNA\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SEL_TX_ANTENNA is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SEL_RX_ANTENNA:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SEL_RX_ANTENNA\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_RX_Antenna_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SEL_RX_ANTENNA\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SEL_RX_ANTENNA is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_PREAMBLE:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_PREAMBLE\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_MODE_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_PREAMBLE\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_PREAMBLE is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_CHANNEL:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_CHANNEL\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_CHANNEL_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_CHANNEL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_CHANNEL is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_ADDR1:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_ADDR1\n"));
+
+				// Addr is an array of UCHAR,
+				// so no need to perform endian swap.
+				memcpy(pAdapter->ate.Addr1, (PUCHAR)(pRaCfg->data - 2), MAC_ADDR_LEN);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_ADDR1\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_ADDR1 is done !\n (ADDR1 = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAdapter->ate.Addr1[0],
+						pAdapter->ate.Addr1[1], pAdapter->ate.Addr1[2], pAdapter->ate.Addr1[3], pAdapter->ate.Addr1[4], pAdapter->ate.Addr1[5]));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_ADDR2:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_ADDR2\n"));
+
+				// Addr is an array of UCHAR,
+				// so no need to perform endian swap.
+				memcpy(pAdapter->ate.Addr2, (PUCHAR)(pRaCfg->data - 2), MAC_ADDR_LEN);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_ADDR2\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_ADDR2 is done !\n (ADDR2 = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAdapter->ate.Addr2[0],
+						pAdapter->ate.Addr2[1], pAdapter->ate.Addr2[2], pAdapter->ate.Addr2[3], pAdapter->ate.Addr2[4], pAdapter->ate.Addr2[5]));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_ADDR3:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_ADDR3\n"));
+
+				// Addr is an array of UCHAR,
+				// so no need to perform endian swap.
+				memcpy(pAdapter->ate.Addr3, (PUCHAR)(pRaCfg->data - 2), MAC_ADDR_LEN);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_ADDR3\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_ADDR3 is done !\n (ADDR3 = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAdapter->ate.Addr3[0],
+						pAdapter->ate.Addr3[1], pAdapter->ate.Addr3[2], pAdapter->ate.Addr3[3], pAdapter->ate.Addr3[4], pAdapter->ate.Addr3[5]));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_RATE:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_RATE\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_MCS_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_RATE\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_RATE is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_FRAME_LEN:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_FRAME_LEN\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_LENGTH_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_FRAME_LEN\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_FRAME_LEN is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_FRAME_COUNT:
+			{
+				USHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_FRAME_COUNT\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+#ifdef RT2860
+				/* TX_FRAME_COUNT == 0 means tx infinitely */
+				if (value == 0)
+				{
+					/* Use TxCount = 0xFFFFFFFF to approximate the infinity. */
+					pAdapter->ate.TxCount = 0xFFFFFFFF;
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_COUNT_Proc (TxCount = %d)\n", pAdapter->ate.TxCount));
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_COUNT_Proc Success\n"));
+
+
+				}
+				else
+#endif // RT2860 //
+				{
+					sprintf((PCHAR)str, "%d", value);
+					Set_ATE_TX_COUNT_Proc(pAdapter, str);
+				}
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_FRAME_COUNT\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_FRAME_COUNT is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_START_RX_FRAME:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_RX_START\n"));
+
+				Set_ATE_Proc(pAdapter, "RXFRAME");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RX_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RX_START is done !\n"));
+				}
+			}
+			break;
+		default:
+			break;
+	}
+    ASSERT(pRaCfg != NULL);
+    if (pRaCfg != NULL)
+    {
+    kfree(pRaCfg);
+    }
+	return;
+}
+
+VOID BubbleSort(INT32 n, INT32 a[])
+{
+	INT32 k, j, temp;
+
+	for (k = n-1;  k>0;  k--)
+	{
+		for (j = 0; j<k; j++)
+		{
+			if(a[j] > a[j+1])
+			{
+				temp = a[j];
+				a[j]=a[j+1];
+				a[j+1]=temp;
+			}
+		}
+	}
+}
+
+VOID CalNoiseLevel(PRTMP_ADAPTER pAd, UCHAR channel, INT32 RSSI[3][10])
+{
+	INT32		RSSI0, RSSI1, RSSI2;
+ 	CHAR		Rssi0Offset, Rssi1Offset, Rssi2Offset;
+	UCHAR		BbpR50Rssi0 = 0, BbpR51Rssi1 = 0, BbpR52Rssi2 = 0;
+	UCHAR		Org_BBP66value = 0, Org_BBP69value = 0, Org_BBP70value = 0, data = 0;
+	USHORT		LNA_Gain = 0;
+	INT32       j = 0;
+	UCHAR		Org_Channel = pAd->ate.Channel;
+	USHORT	    GainValue = 0, OffsetValue = 0;
+
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &Org_BBP66value);
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R69, &Org_BBP69value);
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R70, &Org_BBP70value);
+
+	//**********************************************************************
+	// Read the value of LNA gain and Rssi offset
+	//**********************************************************************
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LNA_OFFSET, GainValue);
+
+	// for Noise Level
+	if (channel <= 14)
+	{
+		LNA_Gain = GainValue & 0x00FF;
+
+		RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_BG_OFFSET, OffsetValue);
+		Rssi0Offset = OffsetValue & 0x00FF;
+		Rssi1Offset = (OffsetValue & 0xFF00) >> 8;
+		RT28xx_EEPROM_READ16(pAd, (EEPROM_RSSI_BG_OFFSET + 2)/* 0x48 */, OffsetValue);
+		Rssi2Offset = OffsetValue & 0x00FF;
+	}
+	else
+	{
+		LNA_Gain = (GainValue & 0xFF00) >> 8;
+
+		RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_A_OFFSET, OffsetValue);
+		Rssi0Offset = OffsetValue & 0x00FF;
+		Rssi1Offset = (OffsetValue & 0xFF00) >> 8;
+		RT28xx_EEPROM_READ16(pAd, (EEPROM_RSSI_A_OFFSET + 2)/* 0x4C */, OffsetValue);
+		Rssi2Offset = OffsetValue & 0x00FF;
+	}
+	//**********************************************************************
+	{
+		pAd->ate.Channel = channel;
+		ATEAsicSwitchChannel(pAd);
+		mdelay(5);
+
+		data = 0x10;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, data);
+		data = 0x40;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, data);
+		data = 0x40;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, data);
+		mdelay(5);
+
+		// Start Rx
+		pAd->ate.bQARxStart = TRUE;
+		Set_ATE_Proc(pAd, "RXFRAME");
+
+		mdelay(5);
+
+		for (j = 0; j < 10; j++)
+		{
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R50, &BbpR50Rssi0);
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R51, &BbpR51Rssi1);
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R52, &BbpR52Rssi2);
+
+			mdelay(10);
+
+			// Calculate RSSI 0
+			if (BbpR50Rssi0 == 0)
+			{
+				RSSI0 = -100;
+			}
+			else
+			{
+				RSSI0 = (INT32)(-12 - BbpR50Rssi0 - LNA_Gain - Rssi0Offset);
+			}
+			RSSI[0][j] = RSSI0;
+
+			if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+			{
+				// Calculate RSSI 1
+				if (BbpR51Rssi1 == 0)
+				{
+					RSSI1 = -100;
+				}
+				else
+				{
+					RSSI1 = (INT32)(-12 - BbpR51Rssi1 - LNA_Gain - Rssi1Offset);
+				}
+				RSSI[1][j] = RSSI1;
+			}
+
+			if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+			{
+				// Calculate RSSI 2
+				if (BbpR52Rssi2 == 0)
+					RSSI2 = -100;
+				else
+					RSSI2 = (INT32)(-12 - BbpR52Rssi2 - LNA_Gain - Rssi2Offset);
+
+				RSSI[2][j] = RSSI2;
+			}
+		}
+
+		// Stop Rx
+		Set_ATE_Proc(pAd, "RXSTOP");
+
+		mdelay(5);
+
+#if 0// Debug Message................
+		ate_print("\n**********************************************************\n");
+		ate_print("Noise Level: Channel %d\n", channel);
+		ate_print("RSSI0 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+			RSSI[0][0], RSSI[0][1], RSSI[0][2],
+			RSSI[0][3], RSSI[0][4], RSSI[0][5],
+			RSSI[0][6], RSSI[0][7], RSSI[0][8],
+			RSSI[0][9]);
+		if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+		{
+			ate_print("RSSI1 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[1][0], RSSI[1][1], RSSI[1][2],
+				RSSI[1][3], RSSI[1][4], RSSI[1][5],
+				RSSI[1][6], RSSI[1][7], RSSI[1][8],
+				RSSI[1][9]);
+		}
+		if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+		{
+			ate_print("RSSI2 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[2][0], RSSI[2][1], RSSI[2][2],
+				RSSI[2][3], RSSI[2][4], RSSI[2][5],
+				RSSI[2][6], RSSI[2][7], RSSI[2][8],
+				RSSI[2][9]);
+		}
+#endif // 0 //
+		BubbleSort(10, RSSI[0]);	// 1R
+
+		if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+		{
+			BubbleSort(10, RSSI[1]);
+		}
+
+		if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+		{
+			BubbleSort(10, RSSI[2]);
+		}
+
+#if 0// Debug Message................
+		ate_print("\nAfter Sorting....Channel %d\n", channel);
+		ate_print("RSSI0 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+			RSSI[0][0], RSSI[0][1], RSSI[0][2],
+			RSSI[0][3], RSSI[0][4], RSSI[0][5],
+			RSSI[0][6], RSSI[0][7], RSSI[0][8],
+			RSSI[0][9]);
+		if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+		{
+			ate_print("RSSI1 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[1][0], RSSI[1][1], RSSI[1][2],
+				RSSI[1][3], RSSI[1][4], RSSI[1][5],
+				RSSI[1][6], RSSI[1][7], RSSI[1][8],
+				RSSI[1][9]);
+		}
+		if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+		{
+			ate_print("RSSI2 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[2][0], RSSI[2][1], RSSI[2][2],
+				RSSI[2][3], RSSI[2][4], RSSI[2][5],
+				RSSI[2][6], RSSI[2][7], RSSI[2][8],
+				RSSI[2][9]);
+		}
+		ate_print("**********************************************************\n");
+#endif // 0 //
+	}
+
+	pAd->ate.Channel = Org_Channel;
+	ATEAsicSwitchChannel(pAd);
+
+	// Restore original value
+    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, Org_BBP66value);
+    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, Org_BBP69value);
+    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, Org_BBP70value);
+
+	return;
+}
+
+BOOLEAN SyncTxRxConfig(PRTMP_ADAPTER pAd, USHORT offset, UCHAR value)
+{
+	UCHAR tmp = 0, bbp_data = 0;
+
+	if (ATE_ON(pAd))
+	{
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, offset, &bbp_data);
+	}
+	else
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, offset, &bbp_data);
+	}
+
+	/* confirm again */
+	ASSERT(bbp_data == value);
+
+	switch(offset)
+	{
+		case BBP_R1:
+			/* Need to sync. tx configuration with legacy ATE. */
+			tmp = (bbp_data & ((1 << 4) | (1 << 3))/* 0x18 */) >> 3;
+		    switch(tmp)
+		    {
+				/* The BBP R1 bit[4:3] = 2 :: Both DACs will be used by QA. */
+		        case 2:
+					/* All */
+					pAd->ate.TxAntennaSel = 0;
+		            break;
+				/* The BBP R1 bit[4:3] = 0 :: DAC 0 will be used by QA. */
+		        case 0:
+					/* Antenna one */
+					pAd->ate.TxAntennaSel = 1;
+		            break;
+				/* The BBP R1 bit[4:3] = 1 :: DAC 1 will be used by QA. */
+		        case 1:
+					/* Antenna two */
+					pAd->ate.TxAntennaSel = 2;
+		            break;
+		        default:
+		            DBGPRINT(RT_DEBUG_TRACE, ("%s -- Sth. wrong!  : return FALSE; \n", __func__));
+		            return FALSE;
+		    }
+			break;/* case BBP_R1 */
+
+		case BBP_R3:
+			/* Need to sync. rx configuration with legacy ATE. */
+			tmp = (bbp_data & ((1 << 1) | (1 << 0))/* 0x03 */);
+		    switch(tmp)
+		    {
+				/* The BBP R3 bit[1:0] = 3 :: All ADCs will be used by QA. */
+		        case 3:
+					/* All */
+					pAd->ate.RxAntennaSel = 0;
+		            break;
+				/* The BBP R3 bit[1:0] = 0 :: ADC 0 will be used by QA, */
+				/* unless the BBP R3 bit[4:3] = 2 */
+		        case 0:
+					/* Antenna one */
+					pAd->ate.RxAntennaSel = 1;
+					tmp = ((bbp_data & ((1 << 4) | (1 << 3))/* 0x03 */) >> 3);
+					if (tmp == 2)// 3R
+					{
+						/* Default : All ADCs will be used by QA */
+						pAd->ate.RxAntennaSel = 0;
+					}
+		            break;
+				/* The BBP R3 bit[1:0] = 1 :: ADC 1 will be used by QA. */
+		        case 1:
+					/* Antenna two */
+					pAd->ate.RxAntennaSel = 2;
+		            break;
+				/* The BBP R3 bit[1:0] = 2 :: ADC 2 will be used by QA. */
+		        case 2:
+					/* Antenna three */
+					pAd->ate.RxAntennaSel = 3;
+		            break;
+		        default:
+		            DBGPRINT(RT_DEBUG_ERROR, ("%s -- Impossible!  : return FALSE; \n", __func__));
+		            return FALSE;
+		    }
+			break;/* case BBP_R3 */
+
+        default:
+            DBGPRINT(RT_DEBUG_ERROR, ("%s -- Sth. wrong!  : return FALSE; \n", __func__));
+            return FALSE;
+
+	}
+	return TRUE;
+}
+
+static VOID memcpy_exl(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len)
+{
+	ULONG i, Value = 0;
+	ULONG *pDst, *pSrc;
+	UCHAR *p8;
+
+	p8 = src;
+	pDst = (ULONG *) dst;
+	pSrc = (ULONG *) src;
+
+	for (i = 0 ; i < (len/4); i++)
+	{
+		/* For alignment issue, we need a variable "Value". */
+		memmove(&Value, pSrc, 4);
+		Value = htonl(Value);
+		memmove(pDst, &Value, 4);
+		pDst++;
+		pSrc++;
+	}
+	if ((len % 4) != 0)
+	{
+		/* wish that it will never reach here */
+		memmove(&Value, pSrc, (len % 4));
+		Value = htonl(Value);
+		memmove(pDst, &Value, (len % 4));
+	}
+}
+
+static VOID memcpy_exs(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len)
+{
+	ULONG i;
+	UCHAR *pDst, *pSrc;
+
+	pDst = dst;
+	pSrc = src;
+
+	for (i = 0; i < (len/2); i++)
+	{
+		memmove(pDst, pSrc, 2);
+		*((USHORT *)pDst) = htons(*((USHORT *)pDst));
+		pDst+=2;
+		pSrc+=2;
+	}
+
+	if ((len % 2) != 0)
+	{
+		memmove(pDst, pSrc, 1);
+	}
+}
+
+static VOID RTMP_IO_READ_BULK(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, UINT32 len)
+{
+	UINT32 i, Value;
+	UINT32 *pDst, *pSrc;
+
+	pDst = (UINT32 *) dst;
+	pSrc = (UINT32 *) src;
+
+	for (i = 0 ; i < (len/4); i++)
+	{
+		RTMP_IO_READ32(pAd, (ULONG)pSrc, &Value);
+		Value = htonl(Value);
+		memmove(pDst, &Value, 4);
+		pDst++;
+		pSrc++;
+	}
+	return;
+}
+
+// TODO:
+#if 0
+/* These work only when RALINK_ATE is defined */
+INT Set_TxStart_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG value = simple_strtol(arg, 0, 10);
+	UCHAR buffer[26] = {0x88, 0x02, 0x2c, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x00, 0x55, 0x44, 0x33, 0x22, 0x11, 0xc0, 0x22, 0x00, 0x00};
+	POS_COOKIE pObj;
+
+	if (pAd->ate.TxStatus != 0)
+		return FALSE;
+
+	pAd->ate.TxInfo = 0x04000000;
+	bzero(&pAd->ate.TxWI, sizeof(TXWI_STRUC));
+	pAd->ate.TxWI.PHYMODE = 0;// MODE_CCK
+	pAd->ate.TxWI.MPDUtotalByteCount = 1226;
+	pAd->ate.TxWI.MCS = 3;
+	//pAd->ate.Mode = ATE_START;
+	pAd->ate.Mode |= ATE_TXFRAME;
+	pAd->ate.TxCount = value;
+	pAd->ate.QID = 0;
+	pAd->ate.HLen = 26;
+	pAd->ate.PLen = 0;
+	pAd->ate.DLen = 1200;
+	memcpy(pAd->ate.Header, buffer, 26);
+	pAd->ate.bQATxStart = TRUE;
+	//pObj = (POS_COOKIE) pAd->OS_Cookie;
+	//tasklet_hi_schedule(&pObj->AteTxTask);
+	return TRUE;
+}
+#endif  /* end of #if 0 */
+
+INT Set_TxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ATEDBGPRINT(RT_DEBUG_TRACE,("Set_TxStop_Proc\n"));
+
+	if (Set_ATE_Proc(pAd, "TXSTOP"))
+	{
+	return TRUE;
+}
+	else
+	{
+		return FALSE;
+	}
+}
+
+INT Set_RxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ATEDBGPRINT(RT_DEBUG_TRACE,("Set_RxStop_Proc\n"));
+
+	if (Set_ATE_Proc(pAd, "RXSTOP"))
+	{
+	return TRUE;
+}
+	else
+	{
+		return FALSE;
+	}
+}
+
+#if 0
+INT Set_EEWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT offset = 0, value;
+	PUCHAR p2 = arg;
+
+	while((*p2 != ':') && (*p2 != '\0'))
+	{
+		p2++;
+	}
+
+	if (*p2 == ':')
+	{
+		A2Hex(offset, arg);
+		A2Hex(value, p2+ 1);
+	}
+	else
+	{
+		A2Hex(value, arg);
+	}
+
+	if (offset >= EEPROM_SIZE)
+	{
+		ate_print("Offset can not exceed EEPROM_SIZE( == 0x%04x)\n", EEPROM_SIZE);
+		return FALSE;
+	}
+
+	RTMP_EEPROM_WRITE16(pAd, offset, value);
+
+	return TRUE;
+}
+
+INT Set_BBPRead_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR value = 0, offset;
+
+	A2Hex(offset, arg);
+
+	if (ATE_ON(pAd))
+	{
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, offset,  &value);
+	}
+	else
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, offset,  &value);
+	}
+
+	ate_print("%x\n", value);
+
+	return TRUE;
+}
+
+
+INT Set_BBPWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT offset = 0;
+	PUCHAR p2 = arg;
+	UCHAR value;
+
+	while((*p2 != ':') && (*p2 != '\0'))
+	{
+		p2++;
+	}
+
+	if (*p2 == ':')
+	{
+		A2Hex(offset, arg);
+		A2Hex(value, p2+ 1);
+	}
+	else
+	{
+		A2Hex(value, arg);
+	}
+
+	if (ATE_ON(pAd))
+	{
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, offset,  value);
+	}
+	else
+	{
+		RTNP_BBP_IO_WRITE8_BY_REG_ID(pAd, offset,  value);
+	}
+
+	return TRUE;
+}
+
+INT Set_RFWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	PUCHAR p2, p3, p4;
+	ULONG R1, R2, R3, R4;
+
+	p2 = arg;
+
+	while((*p2 != ':') && (*p2 != '\0'))
+	{
+		p2++;
+	}
+
+	if (*p2 != ':')
+		return FALSE;
+
+	p3 = p2 + 1;
+
+	while((*p3 != ':') && (*p3 != '\0'))
+	{
+		p3++;
+	}
+
+	if (*p3 != ':')
+		return FALSE;
+
+	p4 = p3 + 1;
+
+	while((*p4 != ':') && (*p4 != '\0'))
+	{
+		p4++;
+	}
+
+	if (*p4 != ':')
+		return FALSE;
+
+
+	A2Hex(R1, arg);
+	A2Hex(R2, p2 + 1);
+	A2Hex(R3, p3 + 1);
+	A2Hex(R4, p4 + 1);
+
+	RTMP_RF_IO_WRITE32(pAd, R1);
+	RTMP_RF_IO_WRITE32(pAd, R2);
+	RTMP_RF_IO_WRITE32(pAd, R3);
+	RTMP_RF_IO_WRITE32(pAd, R4);
+
+	return TRUE;
+}
+#endif  // end of #if 0 //
+#endif	// RALINK_28xx_QA //
+
+#endif	// RALINK_ATE //
+
diff --git a/drivers/staging/rt2860/rt_ate.h b/drivers/staging/rt2860/rt_ate.h
new file mode 100644
index 000000000000..48aa70d2f010
--- /dev/null
+++ b/drivers/staging/rt2860/rt_ate.h
@@ -0,0 +1,353 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __ATE_H__
+#define __ATE_H__
+
+#ifndef UCOS
+#define ate_print printk
+#define ATEDBGPRINT DBGPRINT
+#ifdef RT2860
+#define EEPROM_SIZE								0x200
+#ifdef CONFIG_STA_SUPPORT
+#define EEPROM_BIN_FILE_NAME  "/etc/Wireless/RT2860STA/e2p.bin"
+#endif // CONFIG_STA_SUPPORT //
+#endif // RT2860 //
+
+#else // !UCOS //
+#define fATE_LOAD_EEPROM						0x0C43
+#ifdef CONFIG_PRINTK
+extern INT ConsoleResponse(IN PUCHAR buff);
+extern int (*remote_display)(char *);
+extern void puts (const char *s);
+
+/* specificly defined to redirect and show ate-related messages to host. */
+/* Try to define ate_print as a macro. */
+#define ate_print(fmt, args...)                 \
+do{   int (*org_remote_display)(char *) = NULL;   \
+	org_remote_display = remote_display;\
+	/* Save original "remote_display" */\
+	remote_display = (int (*)(char *))ConsoleResponse;           \
+	printk(fmt, ## args);                       \
+	/* Restore the remote_display function pointer */        \
+	remote_display = org_remote_display; }while(0)
+
+#define ATEDBGPRINT(Level, Fmt)    	\
+{                                   \
+    if ((Level) <= RTDebugLevel)      \
+    {                               \
+        ate_print Fmt;					\
+    }                               \
+}
+#endif // CONFIG_PRINTK //
+#endif // !UCOS //
+
+#define ATE_ON(_p)              (((_p)->ate.Mode) != ATE_STOP)
+
+/* RT2880_iNIC will define "RT2860". */
+#ifdef RT2860
+#define ATE_BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)        \
+{                                                       \
+    BBP_CSR_CFG_STRUC  BbpCsr;                             \
+    int             i, k;                               \
+    for (i=0; i<MAX_BUSY_COUNT; i++)                    \
+    {                                                   \
+        RTMP_IO_READ32(_A, BBP_CSR_CFG, &BbpCsr.word);     \
+        if (BbpCsr.field.Busy == BUSY)                  \
+        {                                               \
+            continue;                                   \
+        }                                               \
+        BbpCsr.word = 0;                                \
+        BbpCsr.field.fRead = 1;                         \
+        BbpCsr.field.BBP_RW_MODE = 1;                         \
+        BbpCsr.field.Busy = 1;                          \
+        BbpCsr.field.RegNum = _I;                       \
+        RTMP_IO_WRITE32(_A, BBP_CSR_CFG, BbpCsr.word);     \
+        for (k=0; k<MAX_BUSY_COUNT; k++)                \
+        {                                               \
+            RTMP_IO_READ32(_A, BBP_CSR_CFG, &BbpCsr.word); \
+            if (BbpCsr.field.Busy == IDLE)              \
+                break;                                  \
+        }                                               \
+        if ((BbpCsr.field.Busy == IDLE) &&              \
+            (BbpCsr.field.RegNum == _I))                \
+        {                                               \
+            *(_pV) = (UCHAR)BbpCsr.field.Value;         \
+            break;                                      \
+        }                                               \
+    }                                                   \
+    if (BbpCsr.field.Busy == BUSY)                      \
+    {                                                   \
+        ATEDBGPRINT(RT_DEBUG_ERROR, ("BBP read R%d fail\n", _I));      \
+        *(_pV) = (_A)->BbpWriteLatch[_I];               \
+    }                                                   \
+}
+
+#define ATE_BBP_IO_WRITE8_BY_REG_ID(_A, _I, _V)        \
+{                                                       \
+    BBP_CSR_CFG_STRUC  BbpCsr;                             \
+    int             BusyCnt;                            \
+    for (BusyCnt=0; BusyCnt<MAX_BUSY_COUNT; BusyCnt++)  \
+    {                                                   \
+        RTMP_IO_READ32(_A, BBP_CSR_CFG, &BbpCsr.word);     \
+        if (BbpCsr.field.Busy == BUSY)                  \
+            continue;                                   \
+        BbpCsr.word = 0;                                \
+        BbpCsr.field.fRead = 0;                         \
+        BbpCsr.field.BBP_RW_MODE = 1;                         \
+        BbpCsr.field.Busy = 1;                          \
+        BbpCsr.field.Value = _V;                        \
+        BbpCsr.field.RegNum = _I;                       \
+        RTMP_IO_WRITE32(_A, BBP_CSR_CFG, BbpCsr.word);     \
+        (_A)->BbpWriteLatch[_I] = _V;                   \
+        break;                                          \
+    }                                                   \
+    if (BusyCnt == MAX_BUSY_COUNT)                      \
+    {                                                   \
+        ATEDBGPRINT(RT_DEBUG_ERROR, ("BBP write R%d fail\n", _I));     \
+    }                                                   \
+}
+#endif // RT2860 //
+
+/* RT2880_iNIC will define RT2860. */
+#ifdef RT2860
+#define EEPROM_SIZE								0x200
+/* iNIC has its own EEPROM_BIN_FILE_NAME */
+#ifndef UCOS
+#ifdef CONFIG_STA_SUPPORT
+#define EEPROM_BIN_FILE_NAME  "/etc/Wireless/RT2860STA/e2p.bin"
+#endif // CONFIG_STA_SUPPORT //
+#endif // !UCOS //
+#endif // RT2860 //
+
+
+
+VOID rt_ee_read_all(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT USHORT *Data);
+
+
+VOID rt_ee_write_all(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT *Data);
+
+INT Set_ATE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_DA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_SA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_BSSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_CHANNEL_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_POWER0_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_POWER1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_RX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_FREQOFFSET_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_BW_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_LENGTH_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_COUNT_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_MCS_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_MODE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_GI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+
+INT	Set_ATE_RX_FER_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Read_RF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Load_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Read_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_Show_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_Help_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+VOID ATE_QA_Statistics(
+	IN PRTMP_ADAPTER		pAd,
+	IN PRXWI_STRUC			pRxWI,
+	IN PRT28XX_RXD_STRUC    p28xxRxD,
+	IN PHEADER_802_11		pHeader);
+
+VOID RtmpDoAte(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID BubbleSort(
+	IN  INT32 n,
+	IN  INT32 a[]);
+
+VOID CalNoiseLevel(
+	IN  PRTMP_ADAPTER   pAdapter,
+	IN  UCHAR           channel,
+	OUT INT32           buffer[3][10]);
+
+BOOLEAN SyncTxRxConfig(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	USHORT			offset,
+	IN	UCHAR			value);
+
+#if 0
+INT Set_TxStart_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif  // 0 //
+
+INT Set_TxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_RxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#if 0
+INT Set_EERead_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_EEWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_BBPRead_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_BBPWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_RFWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // end of #if 0 //
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+VOID ATEAsicSwitchChannel(
+	IN PRTMP_ADAPTER pAd);
+
+VOID ATEAsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd);
+
+VOID ATEDisableAsicProtect(
+	IN		PRTMP_ADAPTER	pAd);
+
+CHAR ATEConvertToRssi(
+	IN PRTMP_ADAPTER  pAd,
+	IN CHAR				Rssi,
+	IN UCHAR    RssiNumber);
+
+VOID ATESampleRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN PRXWI_STRUC		pRxWI);
+
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPStationStop(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPStationStart(
+    IN  PRTMP_ADAPTER   pAd);
+#endif // CONFIG_STA_SUPPORT //
+#endif // __ATE_H__ //
diff --git a/drivers/staging/rt2860/rt_config.h b/drivers/staging/rt2860/rt_config.h
new file mode 100644
index 000000000000..7ee7a405b026
--- /dev/null
+++ b/drivers/staging/rt2860/rt_config.h
@@ -0,0 +1,101 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rt_config.h
+
+	Abstract:
+	Central header file to maintain all include files for all NDIS
+	miniport driver routines.
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Paul Lin    08-01-2002    created
+
+*/
+#ifndef	__RT_CONFIG_H__
+#define	__RT_CONFIG_H__
+
+#include    "rtmp_type.h"
+#ifdef UCOS
+#include "includes.h"
+#include <stdio.h>
+#include 	"rt_ucos.h"
+#endif
+
+#ifdef LINUX
+#include	"rt_linux.h"
+#endif
+#include    "rtmp_def.h"
+#include    "rt28xx.h"
+
+#ifdef RT2860
+#include	"rt2860.h"
+#endif // RT2860 //
+
+
+#include    "oid.h"
+#include    "mlme.h"
+#include    "wpa.h"
+#include    "md5.h"
+#include    "rtmp.h"
+#include	"ap.h"
+#include	"dfs.h"
+#include	"chlist.h"
+#include	"spectrum.h"
+
+#ifdef LEAP_SUPPORT
+#include    "leap.h"
+#endif // LEAP_SUPPORT //
+
+#ifdef BLOCK_NET_IF
+#include "netif_block.h"
+#endif // BLOCK_NET_IF //
+
+#ifdef IGMP_SNOOP_SUPPORT
+#include "igmp_snoop.h"
+#endif // IGMP_SNOOP_SUPPORT //
+
+#ifdef RALINK_ATE
+#include "rt_ate.h"
+#endif // RALINK_ATE //
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifndef WPA_SUPPLICANT_SUPPORT
+#error "Build for being controlled by NetworkManager or wext, please set HAS_WPA_SUPPLICANT=y and HAS_NATIVE_WPA_SUPPLICANT_SUPPORT=y"
+#endif // WPA_SUPPLICANT_SUPPORT //
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef IKANOS_VX_1X0
+#include	"vr_ikans.h"
+#endif // IKANOS_VX_1X0 //
+
+#endif	// __RT_CONFIG_H__
+
diff --git a/drivers/staging/rt2860/rt_linux.c b/drivers/staging/rt2860/rt_linux.c
new file mode 100644
index 000000000000..f14500931efb
--- /dev/null
+++ b/drivers/staging/rt2860/rt_linux.c
@@ -0,0 +1,1054 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "rt_config.h"
+
+ULONG	RTDebugLevel = RT_DEBUG_ERROR;
+
+BUILD_TIMER_FUNCTION(MlmePeriodicExec);
+BUILD_TIMER_FUNCTION(AsicRxAntEvalTimeout);
+BUILD_TIMER_FUNCTION(APSDPeriodicExec);
+BUILD_TIMER_FUNCTION(AsicRfTuningExec);
+
+
+#ifdef CONFIG_STA_SUPPORT
+BUILD_TIMER_FUNCTION(BeaconTimeout);
+BUILD_TIMER_FUNCTION(ScanTimeout);
+BUILD_TIMER_FUNCTION(AuthTimeout);
+BUILD_TIMER_FUNCTION(AssocTimeout);
+BUILD_TIMER_FUNCTION(ReassocTimeout);
+BUILD_TIMER_FUNCTION(DisassocTimeout);
+BUILD_TIMER_FUNCTION(LinkDownExec);
+#ifdef LEAP_SUPPORT
+BUILD_TIMER_FUNCTION(LeapAuthTimeout);
+#endif
+BUILD_TIMER_FUNCTION(StaQuickResponeForRateUpExec);
+BUILD_TIMER_FUNCTION(WpaDisassocApAndBlockAssoc);
+#ifdef RT2860
+BUILD_TIMER_FUNCTION(PsPollWakeExec);
+BUILD_TIMER_FUNCTION(RadioOnExec);
+#endif // RT2860 //
+#ifdef QOS_DLS_SUPPORT
+BUILD_TIMER_FUNCTION(DlsTimeoutAction);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+// for wireless system event message
+char const *pWirelessSysEventText[IW_SYS_EVENT_TYPE_NUM] = {
+	// system status event
+    "had associated successfully",							/* IW_ASSOC_EVENT_FLAG */
+    "had disassociated",									/* IW_DISASSOC_EVENT_FLAG */
+    "had deauthenticated",									/* IW_DEAUTH_EVENT_FLAG */
+    "had been aged-out and disassociated",					/* IW_AGEOUT_EVENT_FLAG */
+    "occurred CounterMeasures attack",						/* IW_COUNTER_MEASURES_EVENT_FLAG */
+    "occurred replay counter different in Key Handshaking",	/* IW_REPLAY_COUNTER_DIFF_EVENT_FLAG */
+    "occurred RSNIE different in Key Handshaking",			/* IW_RSNIE_DIFF_EVENT_FLAG */
+    "occurred MIC different in Key Handshaking",			/* IW_MIC_DIFF_EVENT_FLAG */
+    "occurred ICV error in RX",								/* IW_ICV_ERROR_EVENT_FLAG */
+    "occurred MIC error in RX",								/* IW_MIC_ERROR_EVENT_FLAG */
+	"Group Key Handshaking timeout",						/* IW_GROUP_HS_TIMEOUT_EVENT_FLAG */
+	"Pairwise Key Handshaking timeout",						/* IW_PAIRWISE_HS_TIMEOUT_EVENT_FLAG */
+	"RSN IE sanity check failure",							/* IW_RSNIE_SANITY_FAIL_EVENT_FLAG */
+	"set key done in WPA/WPAPSK",							/* IW_SET_KEY_DONE_WPA1_EVENT_FLAG */
+	"set key done in WPA2/WPA2PSK",                         /* IW_SET_KEY_DONE_WPA2_EVENT_FLAG */
+	"connects with our wireless client",                    /* IW_STA_LINKUP_EVENT_FLAG */
+	"disconnects with our wireless client",                 /* IW_STA_LINKDOWN_EVENT_FLAG */
+	"scan completed"										/* IW_SCAN_COMPLETED_EVENT_FLAG */
+	"scan terminate!! Busy!! Enqueue fail!!"				/* IW_SCAN_ENQUEUE_FAIL_EVENT_FLAG */
+	};
+
+// for wireless IDS_spoof_attack event message
+char const *pWirelessSpoofEventText[IW_SPOOF_EVENT_TYPE_NUM] = {
+    "detected conflict SSID",								/* IW_CONFLICT_SSID_EVENT_FLAG */
+    "detected spoofed association response",				/* IW_SPOOF_ASSOC_RESP_EVENT_FLAG */
+    "detected spoofed reassociation responses",				/* IW_SPOOF_REASSOC_RESP_EVENT_FLAG */
+    "detected spoofed probe response",						/* IW_SPOOF_PROBE_RESP_EVENT_FLAG */
+    "detected spoofed beacon",								/* IW_SPOOF_BEACON_EVENT_FLAG */
+    "detected spoofed disassociation",						/* IW_SPOOF_DISASSOC_EVENT_FLAG */
+    "detected spoofed authentication",						/* IW_SPOOF_AUTH_EVENT_FLAG */
+    "detected spoofed deauthentication",					/* IW_SPOOF_DEAUTH_EVENT_FLAG */
+    "detected spoofed unknown management frame",			/* IW_SPOOF_UNKNOWN_MGMT_EVENT_FLAG */
+	"detected replay attack"								/* IW_REPLAY_ATTACK_EVENT_FLAG */
+	};
+
+// for wireless IDS_flooding_attack event message
+char const *pWirelessFloodEventText[IW_FLOOD_EVENT_TYPE_NUM] = {
+	"detected authentication flooding",						/* IW_FLOOD_AUTH_EVENT_FLAG */
+    "detected association request flooding",				/* IW_FLOOD_ASSOC_REQ_EVENT_FLAG */
+    "detected reassociation request flooding",				/* IW_FLOOD_REASSOC_REQ_EVENT_FLAG */
+    "detected probe request flooding",						/* IW_FLOOD_PROBE_REQ_EVENT_FLAG */
+    "detected disassociation flooding",						/* IW_FLOOD_DISASSOC_EVENT_FLAG */
+    "detected deauthentication flooding",					/* IW_FLOOD_DEAUTH_EVENT_FLAG */
+    "detected 802.1x eap-request flooding"					/* IW_FLOOD_EAP_REQ_EVENT_FLAG */
+	};
+
+/* timeout -- ms */
+VOID RTMP_SetPeriodicTimer(
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	unsigned long timeout)
+{
+	timeout = ((timeout*HZ) / 1000);
+	pTimer->expires = jiffies + timeout;
+	add_timer(pTimer);
+}
+
+/* convert NdisMInitializeTimer --> RTMP_OS_Init_Timer */
+VOID RTMP_OS_Init_Timer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	TIMER_FUNCTION function,
+	IN	PVOID data)
+{
+	init_timer(pTimer);
+    pTimer->data = (unsigned long)data;
+    pTimer->function = function;
+}
+
+
+VOID RTMP_OS_Add_Timer(
+	IN	NDIS_MINIPORT_TIMER		*pTimer,
+	IN	unsigned long timeout)
+{
+	if (timer_pending(pTimer))
+		return;
+
+	timeout = ((timeout*HZ) / 1000);
+	pTimer->expires = jiffies + timeout;
+	add_timer(pTimer);
+}
+
+VOID RTMP_OS_Mod_Timer(
+	IN	NDIS_MINIPORT_TIMER		*pTimer,
+	IN	unsigned long timeout)
+{
+	timeout = ((timeout*HZ) / 1000);
+	mod_timer(pTimer, jiffies + timeout);
+}
+
+VOID RTMP_OS_Del_Timer(
+	IN	NDIS_MINIPORT_TIMER		*pTimer,
+	OUT	BOOLEAN					*pCancelled)
+{
+	if (timer_pending(pTimer))
+	{
+		*pCancelled = del_timer_sync(pTimer);
+	}
+	else
+	{
+		*pCancelled = TRUE;
+	}
+
+}
+
+VOID RTMP_OS_Release_Packet(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PQUEUE_ENTRY  pEntry)
+{
+	//RTMPFreeNdisPacket(pAd, (struct sk_buff *)pEntry);
+}
+
+// Unify all delay routine by using udelay
+VOID RTMPusecDelay(
+	IN	ULONG	usec)
+{
+	ULONG	i;
+
+	for (i = 0; i < (usec / 50); i++)
+		udelay(50);
+
+	if (usec % 50)
+		udelay(usec % 50);
+}
+
+void RTMP_GetCurrentSystemTime(LARGE_INTEGER *time)
+{
+	time->u.LowPart = jiffies;
+}
+
+// pAd MUST allow to be NULL
+NDIS_STATUS os_alloc_mem(
+	IN	PRTMP_ADAPTER pAd,
+	OUT	PUCHAR *mem,
+	IN	ULONG  size)
+{
+	*mem = (PUCHAR) kmalloc(size, GFP_ATOMIC);
+	if (*mem)
+		return (NDIS_STATUS_SUCCESS);
+	else
+		return (NDIS_STATUS_FAILURE);
+}
+
+// pAd MUST allow to be NULL
+NDIS_STATUS os_free_mem(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PUCHAR mem)
+{
+
+	ASSERT(mem);
+	kfree(mem);
+	return (NDIS_STATUS_SUCCESS);
+}
+
+
+PNDIS_PACKET RTMP_AllocateFragPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length)
+{
+	struct sk_buff *pkt;
+
+	pkt = dev_alloc_skb(Length);
+
+	if (pkt == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("can't allocate frag rx %ld size packet\n",Length));
+	}
+
+	if (pkt)
+	{
+		RTMP_SET_PACKET_SOURCE(OSPKT_TO_RTPKT(pkt), PKTSRC_NDIS);
+	}
+
+	return (PNDIS_PACKET) pkt;
+}
+
+
+PNDIS_PACKET RTMP_AllocateTxPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress)
+{
+	struct sk_buff *pkt;
+
+	pkt = dev_alloc_skb(Length);
+
+	if (pkt == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("can't allocate tx %ld size packet\n",Length));
+	}
+
+	if (pkt)
+	{
+		RTMP_SET_PACKET_SOURCE(OSPKT_TO_RTPKT(pkt), PKTSRC_NDIS);
+		*VirtualAddress = (PVOID) pkt->data;
+	}
+	else
+	{
+		*VirtualAddress = (PVOID) NULL;
+	}
+
+	return (PNDIS_PACKET) pkt;
+}
+
+
+VOID build_tx_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR	pFrame,
+	IN	ULONG	FrameLen)
+{
+
+	struct sk_buff	*pTxPkt;
+
+	ASSERT(pPacket);
+	pTxPkt = RTPKT_TO_OSPKT(pPacket);
+
+	NdisMoveMemory(skb_put(pTxPkt, FrameLen), pFrame, FrameLen);
+}
+
+VOID	RTMPFreeAdapter(
+	IN	PRTMP_ADAPTER	pAd)
+{
+    POS_COOKIE os_cookie;
+	int index;
+
+	os_cookie=(POS_COOKIE)pAd->OS_Cookie;
+
+	kfree(pAd->BeaconBuf);
+
+
+	NdisFreeSpinLock(&pAd->MgmtRingLock);
+
+#ifdef RT2860
+	NdisFreeSpinLock(&pAd->RxRingLock);
+#endif // RT2860 //
+
+	for (index =0 ; index < NUM_OF_TX_RING; index++)
+	{
+    	NdisFreeSpinLock(&pAd->TxSwQueueLock[index]);
+		NdisFreeSpinLock(&pAd->DeQueueLock[index]);
+		pAd->DeQueueRunning[index] = FALSE;
+	}
+
+	NdisFreeSpinLock(&pAd->irq_lock);
+
+	vfree(pAd); // pci_free_consistent(os_cookie->pci_dev,sizeof(RTMP_ADAPTER),pAd,os_cookie->pAd_pa);
+	kfree(os_cookie);
+}
+
+BOOLEAN OS_Need_Clone_Packet(void)
+{
+	return (FALSE);
+}
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		clone an input NDIS PACKET to another one. The new internally created NDIS PACKET
+		must have only one NDIS BUFFER
+		return - byte copied. 0 means can't create NDIS PACKET
+		NOTE: internally created NDIS_PACKET should be destroyed by RTMPFreeNdisPacket
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pInsAMSDUHdr	EWC A-MSDU format has extra 14-bytes header. if TRUE, insert this 14-byte hdr in front of MSDU.
+		*pSrcTotalLen			return total packet length. This lenght is calculated with 802.3 format packet.
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS RTMPCloneNdisPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	BOOLEAN			pInsAMSDUHdr,
+	IN	PNDIS_PACKET	pInPacket,
+	OUT PNDIS_PACKET   *ppOutPacket)
+{
+
+	struct sk_buff *pkt;
+
+	ASSERT(pInPacket);
+	ASSERT(ppOutPacket);
+
+	// 1. Allocate a packet
+	pkt = dev_alloc_skb(2048);
+
+	if (pkt == NULL)
+	{
+		return NDIS_STATUS_FAILURE;
+	}
+
+ 	skb_put(pkt, GET_OS_PKT_LEN(pInPacket));
+	NdisMoveMemory(pkt->data, GET_OS_PKT_DATAPTR(pInPacket), GET_OS_PKT_LEN(pInPacket));
+	*ppOutPacket = OSPKT_TO_RTPKT(pkt);
+
+
+	RTMP_SET_PACKET_SOURCE(OSPKT_TO_RTPKT(pkt), PKTSRC_NDIS);
+
+	printk("###Clone###\n");
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+// the allocated NDIS PACKET must be freed via RTMPFreeNdisPacket()
+NDIS_STATUS RTMPAllocateNdisPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT PNDIS_PACKET   *ppPacket,
+	IN	PUCHAR			pHeader,
+	IN	UINT			HeaderLen,
+	IN	PUCHAR			pData,
+	IN	UINT			DataLen)
+{
+	PNDIS_PACKET	pPacket;
+	ASSERT(pData);
+	ASSERT(DataLen);
+
+	// 1. Allocate a packet
+	pPacket = (PNDIS_PACKET *) dev_alloc_skb(HeaderLen + DataLen + TXPADDING_SIZE);
+	if (pPacket == NULL)
+ 	{
+		*ppPacket = NULL;
+#ifdef DEBUG
+		printk("RTMPAllocateNdisPacket Fail\n\n");
+#endif
+		return NDIS_STATUS_FAILURE;
+	}
+
+	// 2. clone the frame content
+	if (HeaderLen > 0)
+		NdisMoveMemory(GET_OS_PKT_DATAPTR(pPacket), pHeader, HeaderLen);
+	if (DataLen > 0)
+		NdisMoveMemory(GET_OS_PKT_DATAPTR(pPacket) + HeaderLen, pData, DataLen);
+
+	// 3. update length of packet
+ 	skb_put(GET_OS_PKT_TYPE(pPacket), HeaderLen+DataLen);
+
+	RTMP_SET_PACKET_SOURCE(pPacket, PKTSRC_NDIS);
+//	printk("%s : pPacket = %p, len = %d\n", __func__, pPacket, GET_OS_PKT_LEN(pPacket));
+	*ppPacket = pPacket;
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*
+  ========================================================================
+  Description:
+	This routine frees a miniport internally allocated NDIS_PACKET and its
+	corresponding NDIS_BUFFER and allocated memory.
+  ========================================================================
+*/
+VOID RTMPFreeNdisPacket(
+	IN PRTMP_ADAPTER pAd,
+	IN PNDIS_PACKET  pPacket)
+{
+	dev_kfree_skb_any(RTPKT_TO_OSPKT(pPacket));
+}
+
+
+// IRQL = DISPATCH_LEVEL
+// NOTE: we do have an assumption here, that Byte0 and Byte1 always reasid at the same
+//			 scatter gather buffer
+NDIS_STATUS Sniff2BytesFromNdisBuffer(
+	IN	PNDIS_BUFFER	pFirstBuffer,
+	IN	UCHAR			DesiredOffset,
+	OUT PUCHAR			pByte0,
+	OUT PUCHAR			pByte1)
+{
+    *pByte0 = *(PUCHAR)(pFirstBuffer + DesiredOffset);
+    *pByte1 = *(PUCHAR)(pFirstBuffer + DesiredOffset + 1);
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+void RTMP_QueryPacketInfo(
+	IN  PNDIS_PACKET pPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen)
+{
+	pPacketInfo->BufferCount = 1;
+	pPacketInfo->pFirstBuffer = GET_OS_PKT_DATAPTR(pPacket);
+	pPacketInfo->PhysicalBufferCount = 1;
+	pPacketInfo->TotalPacketLength = GET_OS_PKT_LEN(pPacket);
+
+	*pSrcBufVA = GET_OS_PKT_DATAPTR(pPacket);
+	*pSrcBufLen = GET_OS_PKT_LEN(pPacket);
+}
+
+void RTMP_QueryNextPacketInfo(
+	IN  PNDIS_PACKET *ppPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen)
+{
+	PNDIS_PACKET pPacket = NULL;
+
+	if (*ppPacket)
+		pPacket = GET_OS_PKT_NEXT(*ppPacket);
+
+	if (pPacket)
+	{
+		pPacketInfo->BufferCount = 1;
+		pPacketInfo->pFirstBuffer = GET_OS_PKT_DATAPTR(pPacket);
+		pPacketInfo->PhysicalBufferCount = 1;
+		pPacketInfo->TotalPacketLength = GET_OS_PKT_LEN(pPacket);
+
+		*pSrcBufVA = GET_OS_PKT_DATAPTR(pPacket);
+		*pSrcBufLen = GET_OS_PKT_LEN(pPacket);
+		*ppPacket = GET_OS_PKT_NEXT(pPacket);
+	}
+	else
+	{
+		pPacketInfo->BufferCount = 0;
+		pPacketInfo->pFirstBuffer = NULL;
+		pPacketInfo->PhysicalBufferCount = 0;
+		pPacketInfo->TotalPacketLength = 0;
+
+		*pSrcBufVA = NULL;
+		*pSrcBufLen = 0;
+		*ppPacket = NULL;
+	}
+}
+
+// not yet support MBSS
+PNET_DEV get_netdev_from_bssid(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			FromWhichBSSID)
+{
+    PNET_DEV dev_p = NULL;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		dev_p = pAd->net_dev;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	ASSERT(dev_p);
+	return dev_p; /* return one of MBSS */
+}
+
+PNDIS_PACKET DuplicatePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*skb;
+	PNDIS_PACKET	pRetPacket = NULL;
+	USHORT			DataSize;
+	UCHAR			*pData;
+
+	DataSize = (USHORT) GET_OS_PKT_LEN(pPacket);
+	pData = (PUCHAR) GET_OS_PKT_DATAPTR(pPacket);
+
+
+	skb = skb_clone(RTPKT_TO_OSPKT(pPacket), MEM_ALLOC_FLAG);
+	if (skb)
+	{
+		skb->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+		pRetPacket = OSPKT_TO_RTPKT(skb);
+	}
+
+	return pRetPacket;
+
+}
+
+PNDIS_PACKET duplicate_pkt(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+    IN  UINT            HdrLen,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN	UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*skb;
+	PNDIS_PACKET	pPacket = NULL;
+
+
+	if ((skb = __dev_alloc_skb(HdrLen + DataSize + 2, MEM_ALLOC_FLAG)) != NULL)
+	{
+		skb_reserve(skb, 2);
+		NdisMoveMemory(skb->tail, pHeader802_3, HdrLen);
+		skb_put(skb, HdrLen);
+		NdisMoveMemory(skb->tail, pData, DataSize);
+		skb_put(skb, DataSize);
+		skb->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+		pPacket = OSPKT_TO_RTPKT(skb);
+	}
+
+	return pPacket;
+}
+
+
+#define TKIP_TX_MIC_SIZE		8
+PNDIS_PACKET duplicate_pkt_with_TKIP_MIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	struct sk_buff	*skb, *newskb;
+
+
+	skb = RTPKT_TO_OSPKT(pPacket);
+	if (skb_tailroom(skb) < TKIP_TX_MIC_SIZE)
+	{
+		// alloc a new skb and copy the packet
+		newskb = skb_copy_expand(skb, skb_headroom(skb), TKIP_TX_MIC_SIZE, GFP_ATOMIC);
+		dev_kfree_skb_any(skb);
+		if (newskb == NULL)
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Extend Tx.MIC for packet failed!, dropping packet!\n"));
+			return NULL;
+		}
+		skb = newskb;
+	}
+
+	return OSPKT_TO_RTPKT(skb);
+}
+
+
+
+
+PNDIS_PACKET ClonePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize)
+{
+	struct sk_buff	*pRxPkt;
+	struct sk_buff	*pClonedPkt;
+
+	ASSERT(pPacket);
+	pRxPkt = RTPKT_TO_OSPKT(pPacket);
+
+	// clone the packet
+	pClonedPkt = skb_clone(pRxPkt, MEM_ALLOC_FLAG);
+
+	if (pClonedPkt)
+	{
+    	// set the correct dataptr and data len
+    	pClonedPkt->dev = pRxPkt->dev;
+    	pClonedPkt->data = pData;
+    	pClonedPkt->len = DataSize;
+    	pClonedPkt->tail = pClonedPkt->data + pClonedPkt->len;
+		ASSERT(DataSize < 1530);
+	}
+	return pClonedPkt;
+}
+
+//
+// change OS packet DataPtr and DataLen
+//
+void  update_os_packet_info(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN  UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*pOSPkt;
+
+	ASSERT(pRxBlk->pRxPacket);
+	pOSPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+
+	pOSPkt->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+	pOSPkt->data = pRxBlk->pData;
+	pOSPkt->len = pRxBlk->DataSize;
+	pOSPkt->tail = pOSPkt->data + pOSPkt->len;
+}
+
+
+void wlan_802_11_to_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	PUCHAR			pHeader802_3,
+	IN  UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*pOSPkt;
+
+	ASSERT(pRxBlk->pRxPacket);
+	ASSERT(pHeader802_3);
+
+	pOSPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+
+	pOSPkt->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+	pOSPkt->data = pRxBlk->pData;
+	pOSPkt->len = pRxBlk->DataSize;
+	pOSPkt->tail = pOSPkt->data + pOSPkt->len;
+
+	//
+	// copy 802.3 header
+	//
+	//
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		NdisMoveMemory(skb_push(pOSPkt, LENGTH_802_3), pHeader802_3, LENGTH_802_3);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+
+void announce_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+
+	struct sk_buff	*pRxPkt;
+
+	ASSERT(pPacket);
+
+	pRxPkt = RTPKT_TO_OSPKT(pPacket);
+
+    /* Push up the protocol stack */
+#ifdef IKANOS_VX_1X0
+	IKANOS_DataFrameRx(pAd, pRxPkt->dev, pRxPkt, pRxPkt->len);
+#else
+	pRxPkt->protocol = eth_type_trans(pRxPkt, pRxPkt->dev);
+
+	netif_rx(pRxPkt);
+#endif // IKANOS_VX_1X0 //
+}
+
+
+PRTMP_SCATTER_GATHER_LIST
+rt_get_sg_list_from_packet(PNDIS_PACKET pPacket, RTMP_SCATTER_GATHER_LIST *sg)
+{
+	sg->NumberOfElements = 1;
+	sg->Elements[0].Address =  GET_OS_PKT_DATAPTR(pPacket);
+	sg->Elements[0].Length = GET_OS_PKT_LEN(pPacket);
+	return (sg);
+}
+
+void hex_dump(char *str, unsigned char *pSrcBufVA, unsigned int SrcBufLen)
+{
+	unsigned char *pt;
+	int x;
+
+	if (RTDebugLevel < RT_DEBUG_TRACE)
+		return;
+
+	pt = pSrcBufVA;
+	printk("%s: %p, len = %d\n",str,  pSrcBufVA, SrcBufLen);
+	for (x=0; x<SrcBufLen; x++)
+	{
+		if (x % 16 == 0)
+			printk("0x%04x : ", x);
+		printk("%02x ", ((unsigned char)pt[x]));
+		if (x%16 == 15) printk("\n");
+	}
+	printk("\n");
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Send log message through wireless event
+
+		Support standard iw_event with IWEVCUSTOM. It is used below.
+
+		iwreq_data.data.flags is used to store event_flag that is defined by user.
+		iwreq_data.data.length is the length of the event log.
+
+		The format of the event log is composed of the entry's MAC address and
+		the desired log message (refer to pWirelessEventText).
+
+			ex: 11:22:33:44:55:66 has associated successfully
+
+		p.s. The requirement of Wireless Extension is v15 or newer.
+
+	========================================================================
+*/
+VOID RTMPSendWirelessEvent(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Event_flag,
+	IN	PUCHAR 			pAddr,
+	IN	UCHAR			BssIdx,
+	IN	CHAR			Rssi)
+{
+#if WIRELESS_EXT >= 15
+
+	union 	iwreq_data      wrqu;
+	PUCHAR 	pBuf = NULL, pBufPtr = NULL;
+	USHORT	event, type, BufLen;
+	UCHAR	event_table_len = 0;
+
+	type = Event_flag & 0xFF00;
+	event = Event_flag & 0x00FF;
+
+	switch (type)
+	{
+		case IW_SYS_EVENT_FLAG_START:
+			event_table_len = IW_SYS_EVENT_TYPE_NUM;
+			break;
+
+		case IW_SPOOF_EVENT_FLAG_START:
+			event_table_len = IW_SPOOF_EVENT_TYPE_NUM;
+			break;
+
+		case IW_FLOOD_EVENT_FLAG_START:
+			event_table_len = IW_FLOOD_EVENT_TYPE_NUM;
+			break;
+	}
+
+	if (event_table_len == 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s : The type(%0x02x) is not valid.\n", __func__, type));
+		return;
+	}
+
+	if (event >= event_table_len)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s : The event(%0x02x) is not valid.\n", __func__, event));
+		return;
+	}
+
+	//Allocate memory and copy the msg.
+	if((pBuf = kmalloc(IW_CUSTOM_MAX_LEN, GFP_ATOMIC)) != NULL)
+	{
+		//Prepare the payload
+		memset(pBuf, 0, IW_CUSTOM_MAX_LEN);
+
+		pBufPtr = pBuf;
+
+		if (pAddr)
+			pBufPtr += sprintf(pBufPtr, "(RT2860) STA(%02x:%02x:%02x:%02x:%02x:%02x) ", PRINT_MAC(pAddr));
+		else if (BssIdx < MAX_MBSSID_NUM)
+			pBufPtr += sprintf(pBufPtr, "(RT2860) BSS(ra%d) ", BssIdx);
+		else
+			pBufPtr += sprintf(pBufPtr, "(RT2860) ");
+
+		if (type == IW_SYS_EVENT_FLAG_START)
+			pBufPtr += sprintf(pBufPtr, "%s", pWirelessSysEventText[event]);
+		else if (type == IW_SPOOF_EVENT_FLAG_START)
+			pBufPtr += sprintf(pBufPtr, "%s (RSSI=%d)", pWirelessSpoofEventText[event], Rssi);
+		else if (type == IW_FLOOD_EVENT_FLAG_START)
+			pBufPtr += sprintf(pBufPtr, "%s", pWirelessFloodEventText[event]);
+		else
+			pBufPtr += sprintf(pBufPtr, "%s", "unknown event");
+
+		pBufPtr[pBufPtr - pBuf] = '\0';
+		BufLen = pBufPtr - pBuf;
+
+		memset(&wrqu, 0, sizeof(wrqu));
+	    wrqu.data.flags = Event_flag;
+		wrqu.data.length = BufLen;
+
+		//send wireless event
+	    wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, pBuf);
+
+		//DBGPRINT(RT_DEBUG_TRACE, ("%s : %s\n", __func__, pBuf));
+
+		kfree(pBuf);
+	}
+	else
+		DBGPRINT(RT_DEBUG_ERROR, ("%s : Can't allocate memory for wireless event.\n", __func__));
+#else
+	DBGPRINT(RT_DEBUG_ERROR, ("%s : The Wireless Extension MUST be v15 or newer.\n", __func__));
+#endif  /* WIRELESS_EXT >= 15 */
+}
+
+
+#ifdef CONFIG_STA_SUPPORT
+void send_monitor_packets(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+    struct sk_buff	*pOSPkt;
+    wlan_ng_prism2_header *ph;
+    int rate_index = 0;
+    USHORT header_len = 0;
+    UCHAR temp_header[40] = {0};
+
+    u_int32_t ralinkrate[256] = {2,4,11,22, 12,18,24,36,48,72,96,  108,   109, 110, 111, 112, 13, 26, 39, 52,78,104, 117, 130, 26, 52, 78,104, 156, 208, 234, 260, 27, 54,81,108,162, 216, 243, 270, // Last 38
+	54, 108, 162, 216, 324, 432, 486, 540,  14, 29, 43, 57, 87, 115, 130, 144, 29, 59,87,115, 173, 230,260, 288, 30, 60,90,120,180,240,270,300,60,120,180,240,360,480,540,600, 0,1,2,3,4,5,6,7,8,9,10,
+	11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80};
+
+
+    ASSERT(pRxBlk->pRxPacket);
+    if (pRxBlk->DataSize < 10)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("%s : Size is too small! (%d)\n", __func__, pRxBlk->DataSize));
+		goto err_free_sk_buff;
+    }
+
+    if (pRxBlk->DataSize + sizeof(wlan_ng_prism2_header) > RX_BUFFER_AGGRESIZE)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("%s : Size is too large! (%d)\n", __func__, pRxBlk->DataSize + sizeof(wlan_ng_prism2_header)));
+		goto err_free_sk_buff;
+    }
+
+    pOSPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+	pOSPkt->dev = get_netdev_from_bssid(pAd, BSS0);
+    if (pRxBlk->pHeader->FC.Type == BTYPE_DATA)
+    {
+        pRxBlk->DataSize -= LENGTH_802_11;
+        if ((pRxBlk->pHeader->FC.ToDs == 1) &&
+            (pRxBlk->pHeader->FC.FrDs == 1))
+            header_len = LENGTH_802_11_WITH_ADDR4;
+        else
+            header_len = LENGTH_802_11;
+
+        // QOS
+    	if (pRxBlk->pHeader->FC.SubType & 0x08)
+    	{
+    	    header_len += 2;
+    		// Data skip QOS contorl field
+    		pRxBlk->DataSize -=2;
+    	}
+
+    	// Order bit: A-Ralink or HTC+
+    	if (pRxBlk->pHeader->FC.Order)
+    	{
+    	    header_len += 4;
+			// Data skip HTC contorl field
+			pRxBlk->DataSize -= 4;
+    	}
+
+        // Copy Header
+        if (header_len <= 40)
+            NdisMoveMemory(temp_header, pRxBlk->pData, header_len);
+
+        // skip HW padding
+    	if (pRxBlk->RxD.L2PAD)
+    	    pRxBlk->pData += (header_len + 2);
+        else
+            pRxBlk->pData += header_len;
+    } //end if
+
+
+	if (pRxBlk->DataSize < pOSPkt->len) {
+        skb_trim(pOSPkt,pRxBlk->DataSize);
+    } else {
+        skb_put(pOSPkt,(pRxBlk->DataSize - pOSPkt->len));
+    } //end if
+
+    if ((pRxBlk->pData - pOSPkt->data) > 0) {
+	    skb_put(pOSPkt,(pRxBlk->pData - pOSPkt->data));
+	    skb_pull(pOSPkt,(pRxBlk->pData - pOSPkt->data));
+    } //end if
+
+    if (skb_headroom(pOSPkt) < (sizeof(wlan_ng_prism2_header)+ header_len)) {
+        if (pskb_expand_head(pOSPkt, (sizeof(wlan_ng_prism2_header) + header_len), 0, GFP_ATOMIC)) {
+	        DBGPRINT(RT_DEBUG_ERROR, ("%s : Reallocate header size of sk_buff fail!\n", __func__));
+			goto err_free_sk_buff;
+	    } //end if
+    } //end if
+
+    if (header_len > 0)
+        NdisMoveMemory(skb_push(pOSPkt, header_len), temp_header, header_len);
+
+    ph = (wlan_ng_prism2_header *) skb_push(pOSPkt, sizeof(wlan_ng_prism2_header));
+	NdisZeroMemory(ph, sizeof(wlan_ng_prism2_header));
+
+    ph->msgcode		    = DIDmsg_lnxind_wlansniffrm;
+	ph->msglen		    = sizeof(wlan_ng_prism2_header);
+	strcpy(ph->devname, pAd->net_dev->name);
+
+    ph->hosttime.did = DIDmsg_lnxind_wlansniffrm_hosttime;
+	ph->hosttime.status = 0;
+	ph->hosttime.len = 4;
+	ph->hosttime.data = jiffies;
+
+	ph->mactime.did = DIDmsg_lnxind_wlansniffrm_mactime;
+	ph->mactime.status = 0;
+	ph->mactime.len = 0;
+	ph->mactime.data = 0;
+
+    ph->istx.did = DIDmsg_lnxind_wlansniffrm_istx;
+	ph->istx.status = 0;
+	ph->istx.len = 0;
+	ph->istx.data = 0;
+
+    ph->channel.did = DIDmsg_lnxind_wlansniffrm_channel;
+	ph->channel.status = 0;
+	ph->channel.len = 4;
+
+    ph->channel.data = (u_int32_t)pAd->CommonCfg.Channel;
+
+    ph->rssi.did = DIDmsg_lnxind_wlansniffrm_rssi;
+	ph->rssi.status = 0;
+	ph->rssi.len = 4;
+    ph->rssi.data = (u_int32_t)RTMPMaxRssi(pAd, ConvertToRssi(pAd, pRxBlk->pRxWI->RSSI0, RSSI_0), ConvertToRssi(pAd, pRxBlk->pRxWI->RSSI1, RSSI_1), ConvertToRssi(pAd, pRxBlk->pRxWI->RSSI2, RSSI_2));;
+
+	ph->signal.did = DIDmsg_lnxind_wlansniffrm_signal;
+	ph->signal.status = 0;
+	ph->signal.len = 4;
+	ph->signal.data = 0; //rssi + noise;
+
+	ph->noise.did = DIDmsg_lnxind_wlansniffrm_noise;
+	ph->noise.status = 0;
+	ph->noise.len = 4;
+	ph->noise.data = 0;
+
+#ifdef DOT11_N_SUPPORT
+    if (pRxBlk->pRxWI->PHYMODE >= MODE_HTMIX)
+    {
+    	rate_index = 16 + ((UCHAR)pRxBlk->pRxWI->BW *16) + ((UCHAR)pRxBlk->pRxWI->ShortGI *32) + ((UCHAR)pRxBlk->pRxWI->MCS);
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+	if (pRxBlk->pRxWI->PHYMODE == MODE_OFDM)
+    	rate_index = (UCHAR)(pRxBlk->pRxWI->MCS) + 4;
+    else
+    	rate_index = (UCHAR)(pRxBlk->pRxWI->MCS);
+    if (rate_index < 0)
+        rate_index = 0;
+    if (rate_index > 255)
+        rate_index = 255;
+
+	ph->rate.did = DIDmsg_lnxind_wlansniffrm_rate;
+	ph->rate.status = 0;
+	ph->rate.len = 4;
+    ph->rate.data = ralinkrate[rate_index];
+
+	ph->frmlen.did = DIDmsg_lnxind_wlansniffrm_frmlen;
+    ph->frmlen.status = 0;
+	ph->frmlen.len = 4;
+	ph->frmlen.data	= (u_int32_t)pRxBlk->DataSize;
+
+
+    pOSPkt->pkt_type = PACKET_OTHERHOST;
+    pOSPkt->protocol = eth_type_trans(pOSPkt, pOSPkt->dev);
+    pOSPkt->ip_summed = CHECKSUM_NONE;
+    netif_rx(pOSPkt);
+
+    return;
+
+err_free_sk_buff:
+	RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	return;
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+void rtmp_os_thread_init(PUCHAR pThreadName, PVOID pNotify)
+{
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	daemonize(pThreadName /*"%s",pAd->net_dev->name*/);
+
+	allow_signal(SIGTERM);
+	allow_signal(SIGKILL);
+	current->flags |= PF_NOFREEZE;
+#else
+	unsigned long flags;
+
+	daemonize();
+	reparent_to_init();
+	strcpy(current->comm, pThreadName);
+
+	siginitsetinv(&current->blocked, sigmask(SIGTERM) | sigmask(SIGKILL));
+
+	/* Allow interception of SIGKILL only
+	 * Don't allow other signals to interrupt the transmission */
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,4,22)
+	spin_lock_irqsave(&current->sigmask_lock, flags);
+	flush_signals(current);
+	recalc_sigpending(current);
+	spin_unlock_irqrestore(&current->sigmask_lock, flags);
+#endif
+#endif
+
+    /* signal that we've started the thread */
+	complete(pNotify);
+
+}
+
+void RTMP_IndicateMediaState(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	if (pAd->CommonCfg.bWirelessEvent)
+	{
+		if (pAd->IndicateMediaState == NdisMediaStateConnected)
+		{
+			RTMPSendWirelessEvent(pAd, IW_STA_LINKUP_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+		else
+		{
+			RTMPSendWirelessEvent(pAd, IW_STA_LINKDOWN_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+	}
+}
+
diff --git a/drivers/staging/rt2860/rt_linux.h b/drivers/staging/rt2860/rt_linux.h
new file mode 100644
index 000000000000..0fd58f5109f2
--- /dev/null
+++ b/drivers/staging/rt2860/rt_linux.h
@@ -0,0 +1,926 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+/***********************************************************************/
+/*                                                                     */
+/*   Program:    rt_linux.c                                            */
+/*   Created:    4/21/2006 1:17:38 PM                                  */
+/*   Author:     Wu Xi-Kun                                             */
+/*   Comments:   `description`                                         */
+/*                                                                     */
+/*---------------------------------------------------------------------*/
+/*                                                                     */
+/* History:                                                            */
+/*    Revision 1.1 4/21/2006 1:17:38 PM  xsikun                        */
+/*    Initial revision                                                 */
+/*                                                                     */
+/***********************************************************************/
+
+#include "rtmp_type.h"
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/kernel.h>
+
+#include <linux/spinlock.h>
+#include <linux/init.h>
+#include <linux/string.h>
+#include <linux/timer.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/skbuff.h>
+#include <linux/ethtool.h>
+#include <linux/wireless.h>
+#include <linux/proc_fs.h>
+#include <linux/delay.h>
+#include <linux/if_arp.h>
+#include <linux/ctype.h>
+#include <linux/vmalloc.h>
+
+
+#include <linux/wireless.h>
+#include <net/iw_handler.h>
+
+// load firmware
+#define __KERNEL_SYSCALLS__
+#include <linux/unistd.h>
+#include <asm/uaccess.h>
+
+
+#define MEM_ALLOC_FLAG      (GFP_ATOMIC) //(GFP_DMA | GFP_ATOMIC)
+
+#ifndef IFNAMSIZ
+#define IFNAMSIZ 16
+#endif
+
+//#define CONFIG_CKIP_SUPPORT
+
+#undef __inline
+#define __inline	   static inline
+
+typedef int (*HARD_START_XMIT_FUNC)(struct sk_buff *skb, struct net_device *net_dev);
+
+// add by kathy
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef RT2860
+#define STA_PROFILE_PATH			"/etc/Wireless/RT2860STA/RT2860STA.dat"
+#define STA_RTMP_FIRMWARE_FILE_NAME "/etc/Wireless/RT2860STA/RT2860STA.bin"
+#define STA_NIC_DEVICE_NAME			"RT2860STA"
+#define STA_DRIVER_VERSION			"1.8.0.0"
+#ifdef MULTIPLE_CARD_SUPPORT
+#define CARD_INFO_PATH			"/etc/Wireless/RT2860STA/RT2860STACard.dat"
+#endif // MULTIPLE_CARD_SUPPORT //
+#endif // RT2860 //
+
+
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RT2860
+#ifndef PCI_DEVICE
+#define PCI_DEVICE(vend,dev) \
+	.vendor = (vend), .device = (dev), \
+	.subvendor = PCI_ANY_ID, .subdevice = PCI_ANY_ID
+#endif // PCI_DEVICE //
+#endif // RT2860 //
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+
+#define RTMP_TIME_AFTER(a,b)		\
+	(typecheck(unsigned long, (unsigned long)a) && \
+	 typecheck(unsigned long, (unsigned long)b) && \
+	 ((long)(b) - (long)(a) < 0))
+
+#define RTMP_TIME_AFTER_EQ(a,b)	\
+	(typecheck(unsigned long, (unsigned long)a) && \
+	 typecheck(unsigned long, (unsigned long)b) && \
+	 ((long)(a) - (long)(b) >= 0))
+#define RTMP_TIME_BEFORE(a,b)	RTMP_TIME_AFTER_EQ(b,a)
+#else
+#define RTMP_TIME_AFTER(a,b) time_after(a, b)
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#define RT_MOD_INC_USE_COUNT() \
+	if (!try_module_get(THIS_MODULE)) \
+	{ \
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: cannot reserve module\n", __func__)); \
+		return -1; \
+	}
+
+#define RT_MOD_DEC_USE_COUNT() module_put(THIS_MODULE);
+#else
+#define RT_MOD_INC_USE_COUNT()	MOD_INC_USE_COUNT;
+#define RT_MOD_DEC_USE_COUNT() MOD_DEC_USE_COUNT;
+#endif
+
+#define OS_HZ			HZ
+
+#define ETH_LENGTH_OF_ADDRESS	6
+
+#define IN
+#define OUT
+
+#define NDIS_STATUS                             INT
+#define NDIS_STATUS_SUCCESS                     0x00
+#define NDIS_STATUS_FAILURE                     0x01
+#define NDIS_STATUS_INVALID_DATA				0x02
+#define NDIS_STATUS_RESOURCES                   0x03
+
+#define MIN_NET_DEVICE_FOR_AID			0x00		//0x00~0x3f
+#define MIN_NET_DEVICE_FOR_MBSSID		0x00		//0x00,0x10,0x20,0x30
+#define MIN_NET_DEVICE_FOR_WDS			0x10		//0x40,0x50,0x60,0x70
+#define MIN_NET_DEVICE_FOR_APCLI		0x20
+#define MIN_NET_DEVICE_FOR_MESH			0x30
+#ifdef CONFIG_STA_SUPPORT
+#define MIN_NET_DEVICE_FOR_DLS			0x40
+#endif // CONFIG_STA_SUPPORT //
+
+
+#ifdef CONFIG_STA_SUPPORT
+#define NDIS_PACKET_TYPE_DIRECTED		0
+#define NDIS_PACKET_TYPE_MULTICAST		1
+#define NDIS_PACKET_TYPE_BROADCAST		2
+#define NDIS_PACKET_TYPE_ALL_MULTICAST	3
+#endif // CONFIG_STA_SUPPORT //
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+typedef	struct pid *	THREAD_PID;
+#define	THREAD_PID_INIT_VALUE	NULL
+#define	GET_PID(_v)	find_get_pid(_v)
+#define	GET_PID_NUMBER(_v)	pid_nr(_v)
+#define CHECK_PID_LEGALITY(_pid)	if (pid_nr(_pid) >= 0)
+#define KILL_THREAD_PID(_A, _B, _C)	kill_pid(_A, _B, _C)
+#else
+typedef	pid_t	THREAD_PID;
+#define	THREAD_PID_INIT_VALUE	-1
+#define	GET_PID(_v)	_v
+#define	GET_PID_NUMBER(_v)	_v
+#define CHECK_PID_LEGALITY(_pid)	if (_pid >= 0)
+#define KILL_THREAD_PID(_A, _B, _C)	kill_proc(_A, _B, _C)
+#endif
+
+struct os_lock  {
+	spinlock_t		lock;
+	unsigned long  	flags;
+};
+
+
+struct os_cookie {
+#ifdef RT2860
+	struct pci_dev 			*pci_dev;
+	struct pci_dev 			*parent_pci_dev;
+	dma_addr_t		  		pAd_pa;
+#endif // RT2860 //
+
+
+	struct tasklet_struct 	rx_done_task;
+	struct tasklet_struct 	mgmt_dma_done_task;
+	struct tasklet_struct 	ac0_dma_done_task;
+	struct tasklet_struct 	ac1_dma_done_task;
+	struct tasklet_struct 	ac2_dma_done_task;
+	struct tasklet_struct 	ac3_dma_done_task;
+	struct tasklet_struct 	hcca_dma_done_task;
+	struct tasklet_struct	tbtt_task;
+#ifdef RT2860
+	struct tasklet_struct	fifo_statistic_full_task;
+#endif // RT2860 //
+
+
+	unsigned long			apd_pid; //802.1x daemon pid
+	INT						ioctl_if_type;
+	INT 					ioctl_if;
+};
+
+typedef struct _VIRTUAL_ADAPTER
+{
+	struct net_device		*RtmpDev;
+	struct net_device		*VirtualDev;
+} VIRTUAL_ADAPTER, PVIRTUAL_ADAPTER;
+
+#undef  ASSERT
+#define ASSERT(x)                                                               \
+{                                                                               \
+    if (!(x))                                                                   \
+    {                                                                           \
+        printk(KERN_WARNING __FILE__ ":%d assert " #x "failed\n", __LINE__);    \
+    }                                                                           \
+}
+
+typedef struct os_cookie	* POS_COOKIE;
+typedef struct pci_dev 		* PPCI_DEV;
+typedef struct net_device	* PNET_DEV;
+typedef void				* PNDIS_PACKET;
+typedef char				NDIS_PACKET;
+typedef PNDIS_PACKET		* PPNDIS_PACKET;
+typedef	dma_addr_t			NDIS_PHYSICAL_ADDRESS;
+typedef	dma_addr_t			* PNDIS_PHYSICAL_ADDRESS;
+typedef spinlock_t			NDIS_SPIN_LOCK;
+typedef struct timer_list	NDIS_MINIPORT_TIMER;
+typedef void				* NDIS_HANDLE;
+typedef char 				* PNDIS_BUFFER;
+
+
+
+void hex_dump(char *str, unsigned char *pSrcBufVA, unsigned int SrcBufLen);
+
+dma_addr_t linux_pci_map_single(void *handle, void *ptr, size_t size, int sd_idx, int direction);
+void linux_pci_unmap_single(void *handle, dma_addr_t dma_addr, size_t size, int direction);
+
+
+////////////////////////////////////////
+// MOVE TO rtmp.h ?
+/////////////////////////////////////////
+#define PKTSRC_NDIS             0x7f
+#define PKTSRC_DRIVER           0x0f
+#define PRINT_MAC(addr)	\
+	addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]
+
+
+#define RT2860_PCI_DEVICE_ID		0x0601
+
+#ifdef RT2860
+#define PCI_MAP_SINGLE(_handle, _ptr, _size, _sd_idx, _dir) \
+	linux_pci_map_single(_handle, _ptr, _size, _sd_idx, _dir)
+
+#define PCI_UNMAP_SINGLE(_handle, _ptr, _size, _dir) \
+	linux_pci_unmap_single(_handle, _ptr, _size, _dir)
+
+#define PCI_ALLOC_CONSISTENT(_pci_dev, _size, _ptr) \
+	pci_alloc_consistent(_pci_dev, _size, _ptr)
+
+#define PCI_FREE_CONSISTENT(_pci_dev, _size, _virtual_addr, _physical_addr) \
+	pci_free_consistent(_pci_dev, _size, _virtual_addr, _physical_addr)
+
+#define DEV_ALLOC_SKB(_length) \
+	dev_alloc_skb(_length)
+#endif // RT2860 //
+
+
+
+#define BEACON_FRAME_DMA_CACHE_WBACK(_ptr, _size)	\
+	dma_cache_wback(_ptr, _size)
+
+
+//////////////////////////////////////////
+//
+//////////////////////////////////////////
+
+
+#define NdisMIndicateStatus(_w, _x, _y, _z)
+
+
+typedef struct timer_list	RTMP_OS_TIMER;
+
+
+
+typedef struct  _RALINK_TIMER_STRUCT    {
+    RTMP_OS_TIMER		TimerObj;       // Ndis Timer object
+	BOOLEAN				Valid;			// Set to True when call RTMPInitTimer
+    BOOLEAN             State;          // True if timer cancelled
+    BOOLEAN	      		PeriodicType;	// True if timer is periodic timer
+    BOOLEAN             Repeat;         // True if periodic timer
+    ULONG               TimerValue;     // Timer value in milliseconds
+	ULONG				cookie;			// os specific object
+}   RALINK_TIMER_STRUCT, *PRALINK_TIMER_STRUCT;
+
+
+
+
+//#define DBG	1
+
+//
+//  MACRO for debugging information
+//
+
+#ifdef DBG
+extern ULONG    RTDebugLevel;
+
+#define DBGPRINT_RAW(Level, Fmt)    \
+{                                   \
+    if (Level <= RTDebugLevel)      \
+    {                               \
+        printk Fmt;               \
+    }                               \
+}
+
+#define DBGPRINT(Level, Fmt)    DBGPRINT_RAW(Level, Fmt)
+
+
+#define DBGPRINT_ERR(Fmt)           \
+{                                   \
+    printk("ERROR!!! ");          \
+    printk Fmt;                  \
+}
+
+#define DBGPRINT_S(Status, Fmt)		\
+{									\
+	printk Fmt;					\
+}
+
+
+#else
+#define DBGPRINT(Level, Fmt)
+#define DBGPRINT_RAW(Level, Fmt)
+#define DBGPRINT_S(Status, Fmt)
+#define DBGPRINT_ERR(Fmt)
+#endif
+
+
+//
+//  spin_lock enhanced for Nested spin lock
+//
+#define NdisAllocateSpinLock(__lock)      \
+{                                       \
+    spin_lock_init((spinlock_t *)(__lock));               \
+}
+
+#define NdisFreeSpinLock(lock)          \
+{                                       \
+}
+
+
+#define RTMP_SEM_LOCK(__lock)					\
+{												\
+	spin_lock_bh((spinlock_t *)(__lock));				\
+}
+
+#define RTMP_SEM_UNLOCK(__lock)					\
+{												\
+	spin_unlock_bh((spinlock_t *)(__lock));				\
+}
+
+// sample, use semaphore lock to replace IRQ lock, 2007/11/15
+#define RTMP_IRQ_LOCK(__lock, __irqflags)			\
+{													\
+	__irqflags = 0;									\
+	spin_lock_bh((spinlock_t *)(__lock));			\
+	pAd->irq_disabled |= 1; \
+}
+
+#define RTMP_IRQ_UNLOCK(__lock, __irqflag)			\
+{													\
+	pAd->irq_disabled &= 0; \
+	spin_unlock_bh((spinlock_t *)(__lock));			\
+}
+
+#define RTMP_INT_LOCK(__lock, __irqflags)			\
+{													\
+	spin_lock_irqsave((spinlock_t *)__lock, __irqflags);	\
+}
+
+#define RTMP_INT_UNLOCK(__lock, __irqflag)			\
+{													\
+	spin_unlock_irqrestore((spinlock_t *)(__lock), ((unsigned long)__irqflag));	\
+}
+
+#ifdef RT2860
+#if defined(INF_TWINPASS) || defined(INF_DANUBE) || defined(IKANOS_VX_1X0)
+//Patch for ASIC turst read/write bug, needs to remove after metel fix
+#define RTMP_IO_READ32(_A, _R, _pV)									\
+{																	\
+    if ((_A)->bPCIclkOff == FALSE)                                      \
+    {                                                                   \
+	(*_pV = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0)));		\
+	(*_pV = readl((void *)((_A)->CSRBaseAddress + (_R))));			\
+	(*_pV = SWAP32(*((UINT32 *)(_pV))));                           \
+    }                                                                   \
+}
+#define RTMP_IO_READ8(_A, _R, _pV)									\
+{																	\
+	(*_pV = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0)));		\
+	(*_pV = readb((void *)((_A)->CSRBaseAddress + (_R))));			\
+}
+#define RTMP_IO_WRITE32(_A, _R, _V)									\
+{																	\
+    if ((_A)->bPCIclkOff == FALSE)                                      \
+    {                                                                   \
+	UINT32	_Val;													\
+	_Val = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0));		\
+	_Val = SWAP32(_V);												\
+	writel(_Val, (void *)((_A)->CSRBaseAddress + (_R)));			\
+    }                                                                   \
+}
+#define RTMP_IO_WRITE8(_A, _R, _V)									\
+{																	\
+	UINT	Val;													\
+	Val = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0));		\
+	writeb((_V), (PUCHAR)((_A)->CSRBaseAddress + (_R)));			\
+}
+#define RTMP_IO_WRITE16(_A, _R, _V)									\
+{																	\
+	UINT	Val;													\
+	Val = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0));		\
+	writew(SWAP16((_V)), (PUSHORT)((_A)->CSRBaseAddress + (_R)));	\
+}
+#else
+//Patch for ASIC turst read/write bug, needs to remove after metel fix
+#define RTMP_IO_READ32(_A, _R, _pV)								\
+{																\
+    if ((_A)->bPCIclkOff == FALSE)                                  \
+    {                                                               \
+		(*_pV = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0)));		\
+		(*_pV = readl((void *)((_A)->CSRBaseAddress + (_R))));			\
+    }                                                               \
+    else															\
+		*_pV = 0;													\
+}
+#define RTMP_IO_READ8(_A, _R, _pV)								\
+{																\
+	(*_pV = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0)));			\
+	(*_pV = readb((void *)((_A)->CSRBaseAddress + (_R))));				\
+}
+#define RTMP_IO_WRITE32(_A, _R, _V)												\
+{																				\
+    if ((_A)->bPCIclkOff == FALSE)                                  \
+    {                                                               \
+	UINT	Val;																\
+	Val = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0));			\
+	writel(_V, (void *)((_A)->CSRBaseAddress + (_R)));								\
+    }                                                               \
+}
+#if defined(BRCM_6358)
+#define RTMP_IO_WRITE8(_A, _R, _V)            \
+{                    \
+	ULONG Val;                \
+	UCHAR _i;                \
+	_i = (_R & 0x3);             \
+	Val = readl((void *)((_A)->CSRBaseAddress + (_R - _i)));   \
+	Val = Val & (~(0x000000ff << ((_i)*8)));         \
+	Val = Val | ((ULONG)_V << ((_i)*8));         \
+	writel((Val), (void *)((_A)->CSRBaseAddress + (_R - _i)));    \
+}
+#else
+#define RTMP_IO_WRITE8(_A, _R, _V)												\
+{																				\
+	UINT	Val;																\
+	Val = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0));			\
+	writeb((_V), (PUCHAR)((_A)->CSRBaseAddress + (_R)));		\
+}
+#endif
+#define RTMP_IO_WRITE16(_A, _R, _V)												\
+{																				\
+	UINT	Val;																\
+	Val = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0));			\
+	writew((_V), (PUSHORT)((_A)->CSRBaseAddress + (_R)));	\
+}
+#endif
+#endif // RT2860 //
+
+
+#ifndef wait_event_interruptible_timeout
+#define __wait_event_interruptible_timeout(wq, condition, ret) \
+do { \
+        wait_queue_t __wait; \
+        init_waitqueue_entry(&__wait, current); \
+        add_wait_queue(&wq, &__wait); \
+        for (;;) { \
+                set_current_state(TASK_INTERRUPTIBLE); \
+                if (condition) \
+                        break; \
+                if (!signal_pending(current)) { \
+                        ret = schedule_timeout(ret); \
+                        if (!ret) \
+                                break; \
+                        continue; \
+                } \
+                ret = -ERESTARTSYS; \
+                break; \
+        } \
+        current->state = TASK_RUNNING; \
+        remove_wait_queue(&wq, &__wait); \
+} while (0)
+
+#define wait_event_interruptible_timeout(wq, condition, timeout) \
+({ \
+        long __ret = timeout; \
+        if (!(condition)) \
+                __wait_event_interruptible_timeout(wq, condition, __ret); \
+        __ret; \
+})
+#endif
+#define ONE_TICK 1
+#define OS_WAIT(_time) \
+{	int _i; \
+	long _loop = ((_time)/(1000/OS_HZ)) > 0 ? ((_time)/(1000/OS_HZ)) : 1;\
+	wait_queue_head_t _wait; \
+	init_waitqueue_head(&_wait); \
+	for (_i=0; _i<(_loop); _i++) \
+		wait_event_interruptible_timeout(_wait, 0, ONE_TICK); }
+
+
+/* Modified by Wu Xi-Kun 4/21/2006 */
+typedef void (*TIMER_FUNCTION)(unsigned long);
+
+#define COPY_MAC_ADDR(Addr1, Addr2)             memcpy((Addr1), (Addr2), MAC_ADDR_LEN)
+
+#define MlmeAllocateMemory(_pAd, _ppVA) os_alloc_mem(_pAd, _ppVA, MGMT_DMA_BUFFER_SIZE)
+#define MlmeFreeMemory(_pAd, _pVA)     os_free_mem(_pAd, _pVA)
+
+#ifdef RT2860
+#define BUILD_TIMER_FUNCTION(_func)												\
+void linux_##_func(unsigned long data)											\
+{																				\
+	PRALINK_TIMER_STRUCT	pTimer = (PRALINK_TIMER_STRUCT) data;				\
+																				\
+	_func(NULL, (PVOID) pTimer->cookie, NULL, pTimer); 							\
+	if (pTimer->Repeat)															\
+		RTMP_OS_Add_Timer(&pTimer->TimerObj, pTimer->TimerValue);				\
+}
+#endif // RT2860 //
+
+
+
+#define DECLARE_TIMER_FUNCTION(_func)			\
+void linux_##_func(unsigned long data)
+
+#define GET_TIMER_FUNCTION(_func)				\
+		linux_##_func
+
+DECLARE_TIMER_FUNCTION(MlmePeriodicExec);
+DECLARE_TIMER_FUNCTION(MlmeRssiReportExec);
+DECLARE_TIMER_FUNCTION(AsicRxAntEvalTimeout);
+DECLARE_TIMER_FUNCTION(APSDPeriodicExec);
+DECLARE_TIMER_FUNCTION(AsicRfTuningExec);
+
+
+#ifdef CONFIG_STA_SUPPORT
+DECLARE_TIMER_FUNCTION(BeaconTimeout);
+DECLARE_TIMER_FUNCTION(ScanTimeout);
+DECLARE_TIMER_FUNCTION(AuthTimeout);
+DECLARE_TIMER_FUNCTION(AssocTimeout);
+DECLARE_TIMER_FUNCTION(ReassocTimeout);
+DECLARE_TIMER_FUNCTION(DisassocTimeout);
+DECLARE_TIMER_FUNCTION(LinkDownExec);
+#ifdef LEAP_SUPPORT
+DECLARE_TIMER_FUNCTION(LeapAuthTimeout);
+#endif
+DECLARE_TIMER_FUNCTION(StaQuickResponeForRateUpExec);
+DECLARE_TIMER_FUNCTION(WpaDisassocApAndBlockAssoc);
+DECLARE_TIMER_FUNCTION(PsPollWakeExec);
+DECLARE_TIMER_FUNCTION(RadioOnExec);
+
+#ifdef QOS_DLS_SUPPORT
+DECLARE_TIMER_FUNCTION(DlsTimeoutAction);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+void RTMP_GetCurrentSystemTime(LARGE_INTEGER *time);
+
+
+/*
+ * packet helper
+ * 	- convert internal rt packet to os packet or
+ *             os packet to rt packet
+ */
+#define RTPKT_TO_OSPKT(_p)		((struct sk_buff *)(_p))
+#define OSPKT_TO_RTPKT(_p)		((PNDIS_PACKET)(_p))
+
+#define GET_OS_PKT_DATAPTR(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->data)
+
+#define GET_OS_PKT_LEN(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->len)
+
+#define GET_OS_PKT_DATATAIL(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->tail)
+
+#define GET_OS_PKT_HEAD(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->head)
+
+#define GET_OS_PKT_END(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->end)
+
+#define GET_OS_PKT_NETDEV(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->dev)
+
+#define GET_OS_PKT_TYPE(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt))
+
+#define GET_OS_PKT_NEXT(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->next)
+
+
+#define OS_NTOHS(_Val) \
+		(ntohs(_Val))
+#define OS_HTONS(_Val) \
+		(htons(_Val))
+#define OS_NTOHL(_Val) \
+		(ntohl(_Val))
+#define OS_HTONL(_Val) \
+		(htonl(_Val))
+
+/* statistics counter */
+#define STATS_INC_RX_PACKETS(_pAd, _dev)
+#define STATS_INC_TX_PACKETS(_pAd, _dev)
+
+#define STATS_INC_RX_BYTESS(_pAd, _dev, len)
+#define STATS_INC_TX_BYTESS(_pAd, _dev, len)
+
+#define STATS_INC_RX_ERRORS(_pAd, _dev)
+#define STATS_INC_TX_ERRORS(_pAd, _dev)
+
+#define STATS_INC_RX_DROPPED(_pAd, _dev)
+#define STATS_INC_TX_DROPPED(_pAd, _dev)
+
+
+#define CB_OFF  10
+
+
+//   check DDK NDIS_PACKET data structure and find out only MiniportReservedEx[0..7] can be used by our driver without
+//   ambiguity. Fields after pPacket->MiniportReservedEx[8] may be used by other wrapper layer thus crashes the driver
+//
+
+// User Priority
+#define RTMP_SET_PACKET_UP(_p, _prio)			(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+0] = _prio)
+#define RTMP_GET_PACKET_UP(_p)					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+0])
+
+// Fragment #
+#define RTMP_SET_PACKET_FRAGMENTS(_p, _num)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+1] = _num)
+#define RTMP_GET_PACKET_FRAGMENTS(_p)			(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+1])
+
+// 0x0 ~0x7f: TX to AP's own BSS which has the specified AID. if AID>127, set bit 7 in RTMP_SET_PACKET_EMACTAB too.
+//(this value also as MAC(on-chip WCID) table index)
+// 0x80~0xff: TX to a WDS link. b0~6: WDS index
+#define RTMP_SET_PACKET_WCID(_p, _wdsidx)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+2] = _wdsidx)
+#define RTMP_GET_PACKET_WCID(_p)          		((UCHAR)(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+2]))
+
+// 0xff: PKTSRC_NDIS, others: local TX buffer index. This value affects how to a packet
+#define RTMP_SET_PACKET_SOURCE(_p, _pktsrc)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+3] = _pktsrc)
+#define RTMP_GET_PACKET_SOURCE(_p)       		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+3])
+
+// RTS/CTS-to-self protection method
+#define RTMP_SET_PACKET_RTS(_p, _num)      		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+4] = _num)
+#define RTMP_GET_PACKET_RTS(_p)          		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+4])
+// see RTMP_S(G)ET_PACKET_EMACTAB
+
+// TX rate index
+#define RTMP_SET_PACKET_TXRATE(_p, _rate)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+5] = _rate)
+#define RTMP_GET_PACKET_TXRATE(_p)		  		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+5])
+
+// From which Interface
+#define RTMP_SET_PACKET_IF(_p, _ifdx)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+6] = _ifdx)
+#define RTMP_GET_PACKET_IF(_p)		  		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+6])
+#define RTMP_SET_PACKET_NET_DEVICE_MBSSID(_p, _bss)		RTMP_SET_PACKET_IF((_p), (_bss))
+#define RTMP_SET_PACKET_NET_DEVICE_WDS(_p, _bss)		RTMP_SET_PACKET_IF((_p), ((_bss) + MIN_NET_DEVICE_FOR_WDS))
+#define RTMP_SET_PACKET_NET_DEVICE_APCLI(_p, _idx)   	RTMP_SET_PACKET_IF((_p), ((_idx) + MIN_NET_DEVICE_FOR_APCLI))
+#define RTMP_SET_PACKET_NET_DEVICE_MESH(_p, _idx)   	RTMP_SET_PACKET_IF((_p), ((_idx) + MIN_NET_DEVICE_FOR_MESH))
+#define RTMP_GET_PACKET_NET_DEVICE_MBSSID(_p)			RTMP_GET_PACKET_IF((_p))
+#define RTMP_GET_PACKET_NET_DEVICE(_p)					RTMP_GET_PACKET_IF((_p))
+
+#define RTMP_SET_PACKET_MOREDATA(_p, _morebit)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+7] = _morebit)
+#define RTMP_GET_PACKET_MOREDATA(_p)				(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+7])
+
+
+#if 0
+//#define RTMP_SET_PACKET_DHCP(_p, _flg)   	(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] = _flg)
+//#define RTMP_GET_PACKET_DHCP(_p)         	(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11])
+#else
+//
+//	Sepcific Pakcet Type definition
+//
+#define RTMP_PACKET_SPECIFIC_CB_OFFSET	11
+
+#define RTMP_PACKET_SPECIFIC_DHCP		0x01
+#define RTMP_PACKET_SPECIFIC_EAPOL		0x02
+#define RTMP_PACKET_SPECIFIC_IPV4		0x04
+#define RTMP_PACKET_SPECIFIC_WAI		0x08
+#define RTMP_PACKET_SPECIFIC_VLAN		0x10
+#define RTMP_PACKET_SPECIFIC_LLCSNAP	0x20
+
+//Specific
+#define RTMP_SET_PACKET_SPECIFIC(_p, _flg)	   	(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] = _flg)
+
+//DHCP
+#define RTMP_SET_PACKET_DHCP(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_DHCP);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_DHCP);	\
+			}while(0)
+#define RTMP_GET_PACKET_DHCP(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_DHCP)
+
+//EAPOL
+#define RTMP_SET_PACKET_EAPOL(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_EAPOL);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_EAPOL);	\
+			}while(0)
+#define RTMP_GET_PACKET_EAPOL(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_EAPOL)
+
+//WAI
+#define RTMP_SET_PACKET_WAI(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_WAI);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_WAI);	\
+			}while(0)
+#define RTMP_GET_PACKET_WAI(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_WAI)
+
+#define RTMP_GET_PACKET_LOWRATE(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & (RTMP_PACKET_SPECIFIC_EAPOL | RTMP_PACKET_SPECIFIC_DHCP | RTMP_PACKET_SPECIFIC_WAI))
+
+//VLAN
+#define RTMP_SET_PACKET_VLAN(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_VLAN);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_VLAN);	\
+			}while(0)
+#define RTMP_GET_PACKET_VLAN(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_VLAN)
+
+//LLC/SNAP
+#define RTMP_SET_PACKET_LLCSNAP(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_LLCSNAP);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_LLCSNAP);		\
+			}while(0)
+
+#define RTMP_GET_PACKET_LLCSNAP(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_LLCSNAP)
+
+// IP
+#define RTMP_SET_PACKET_IPV4(_p, _flg)														\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_IPV4);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_IPV4);	\
+			}while(0)
+
+#define RTMP_GET_PACKET_IPV4(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_IPV4)
+
+#endif
+
+
+// If this flag is set, it indicates that this EAPoL frame MUST be clear.
+#define RTMP_SET_PACKET_CLEAR_EAP_FRAME(_p, _flg)   (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+12] = _flg)
+#define RTMP_GET_PACKET_CLEAR_EAP_FRAME(_p)         (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+12])
+
+#define RTMP_SET_PACKET_5VT(_p, _flg)   (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+22] = _flg)
+#define RTMP_GET_PACKET_5VT(_p)         (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+22])
+
+#ifdef CONFIG_5VT_ENHANCE
+#define BRIDGE_TAG 0x35564252    // depends on 5VT define in br_input.c
+#endif
+
+
+#define NDIS_SET_PACKET_STATUS(_p, _status)
+
+
+#define GET_SG_LIST_FROM_PACKET(_p, _sc)	\
+    rt_get_sg_list_from_packet(_p, _sc)
+
+#define NdisMoveMemory(Destination, Source, Length) memmove(Destination, Source, Length)
+#define NdisZeroMemory(Destination, Length)         memset(Destination, 0, Length)
+#define NdisFillMemory(Destination, Length, Fill)   memset(Destination, Fill, Length)
+#define NdisEqualMemory(Source1, Source2, Length)   (!memcmp(Source1, Source2, Length))
+#define RTMPEqualMemory(Source1, Source2, Length)	(!memcmp(Source1, Source2, Length))
+
+
+#define RTMP_INC_REF(_A)		0
+#define RTMP_DEC_REF(_A)		0
+#define RTMP_GET_REF(_A)		0
+
+
+
+/*
+ * ULONG
+ * RTMP_GetPhysicalAddressLow(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress);
+ */
+#define RTMP_GetPhysicalAddressLow(PhysicalAddress)		(PhysicalAddress)
+
+/*
+ * ULONG
+ * RTMP_GetPhysicalAddressHigh(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress);
+ */
+#define RTMP_GetPhysicalAddressHigh(PhysicalAddress)		(0)
+
+/*
+ * VOID
+ * RTMP_SetPhysicalAddressLow(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress,
+ *   IN ULONG  Value);
+ */
+#define RTMP_SetPhysicalAddressLow(PhysicalAddress, Value)	\
+			PhysicalAddress = Value;
+
+/*
+ * VOID
+ * RTMP_SetPhysicalAddressHigh(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress,
+ *   IN ULONG  Value);
+ */
+#define RTMP_SetPhysicalAddressHigh(PhysicalAddress, Value)
+
+
+//CONTAINING_RECORD(pEntry, NDIS_PACKET, MiniportReservedEx);
+#define QUEUE_ENTRY_TO_PACKET(pEntry) \
+	(PNDIS_PACKET)(pEntry)
+
+#define PACKET_TO_QUEUE_ENTRY(pPacket) \
+	(PQUEUE_ENTRY)(pPacket)
+
+
+#ifndef CONTAINING_RECORD
+#define CONTAINING_RECORD(address, type, field)			\
+((type *)((PCHAR)(address) - offsetof(type, field)))
+#endif
+
+
+#define RELEASE_NDIS_PACKET(_pAd, _pPacket, _Status)                    \
+{                                                                       \
+        RTMPFreeNdisPacket(_pAd, _pPacket);                             \
+}
+
+
+#define SWITCH_PhyAB(_pAA, _pBB)    \
+{                                                                           \
+    ULONG	AABasePaHigh;                           \
+    ULONG	AABasePaLow;                           \
+    ULONG	BBBasePaHigh;                           \
+    ULONG	BBBasePaLow;                           \
+    BBBasePaHigh = RTMP_GetPhysicalAddressHigh(_pBB);                                                 \
+    BBBasePaLow = RTMP_GetPhysicalAddressLow(_pBB);                                                 \
+    AABasePaHigh = RTMP_GetPhysicalAddressHigh(_pAA);                                                 \
+    AABasePaLow = RTMP_GetPhysicalAddressLow(_pAA);                                                 \
+    RTMP_SetPhysicalAddressHigh(_pAA, BBBasePaHigh);                                                 \
+    RTMP_SetPhysicalAddressLow(_pAA, BBBasePaLow);                                                 \
+    RTMP_SetPhysicalAddressHigh(_pBB, AABasePaHigh);                                                 \
+    RTMP_SetPhysicalAddressLow(_pBB, AABasePaLow);                                                 \
+}
+
+
+#define NdisWriteErrorLogEntry(_a, _b, _c, _d)
+#define NdisMAllocateMapRegisters(_a, _b, _c, _d, _e)		NDIS_STATUS_SUCCESS
+
+
+#define NdisAcquireSpinLock		RTMP_SEM_LOCK
+#define NdisReleaseSpinLock		RTMP_SEM_UNLOCK
+
+static inline void NdisGetSystemUpTime(ULONG *time)
+{
+	*time = jiffies;
+}
+
+//pPacket = CONTAINING_RECORD(pEntry, NDIS_PACKET, MiniportReservedEx);
+#define QUEUE_ENTRY_TO_PKT(pEntry) \
+		((PNDIS_PACKET) (pEntry))
+
+int rt28xx_packet_xmit(struct sk_buff *skb);
+
+
+
+void rtmp_os_thread_init(PUCHAR pThreadName, PVOID pNotify);
+
+#ifdef RT2860
+#if !defined(PCI_CAP_ID_EXP)
+#define PCI_CAP_ID_EXP			    0x10
+#endif
+
+#if !defined(PCI_EXP_LNKCTL)
+#define PCI_EXP_LNKCTL			    0x10
+#endif
+
+#if !defined(PCI_CLASS_BRIDGE_PCI)
+#define PCI_CLASS_BRIDGE_PCI		0x0604
+#endif
+
+#define PCIBUS_INTEL_VENDOR         0x8086
+#endif // RT2860 //
+
+
diff --git a/drivers/staging/rt2860/rt_main_dev.c b/drivers/staging/rt2860/rt_main_dev.c
new file mode 100644
index 000000000000..3873c478f4ec
--- /dev/null
+++ b/drivers/staging/rt2860/rt_main_dev.c
@@ -0,0 +1,1686 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rt_main_dev.c
+
+    Abstract:
+    Create and register network interface.
+
+    Revision History:
+    Who         When            What
+    --------    ----------      ----------------------------------------------
+	Sample		Mar/21/07		Merge RT2870 and RT2860 drivers.
+*/
+
+#include "rt_config.h"
+
+#define FORTY_MHZ_INTOLERANT_INTERVAL	(60*1000) // 1 min
+
+#ifdef MULTIPLE_CARD_SUPPORT
+// record whether the card in the card list is used in the card file
+UINT8  MC_CardUsed[MAX_NUM_OF_MULTIPLE_CARD];
+// record used card mac address in the card list
+static UINT8  MC_CardMac[MAX_NUM_OF_MULTIPLE_CARD][6];
+#endif // MULTIPLE_CARD_SUPPORT //
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+UINT32 CW_MAX_IN_BITS;
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+/*---------------------------------------------------------------------*/
+/* Private Variables Used                                              */
+/*---------------------------------------------------------------------*/
+//static RALINK_TIMER_STRUCT     PeriodicTimer;
+
+char *mac = "";		   // default 00:00:00:00:00:00
+char *hostname = "";		   // default CMPC
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,12)
+MODULE_PARM (mac, "s");
+#else
+module_param (mac, charp, 0);
+#endif
+MODULE_PARM_DESC (mac, "rt28xx: wireless mac addr");
+
+
+/*---------------------------------------------------------------------*/
+/* Prototypes of Functions Used                                        */
+/*---------------------------------------------------------------------*/
+#ifdef DOT11_N_SUPPORT
+extern BOOLEAN ba_reordering_resource_init(PRTMP_ADAPTER pAd, int num);
+extern void ba_reordering_resource_release(PRTMP_ADAPTER pAd);
+#endif // DOT11_N_SUPPORT //
+extern NDIS_STATUS NICLoadRateSwitchingParams(IN PRTMP_ADAPTER pAd);
+
+#ifdef RT2860
+extern void init_thread_task(PRTMP_ADAPTER pAd);
+#endif // RT2860 //
+
+// public function prototype
+INT __devinit rt28xx_probe(IN void *_dev_p, IN void *_dev_id_p,
+							IN UINT argc, OUT PRTMP_ADAPTER *ppAd);
+
+// private function prototype
+static int rt28xx_init(IN struct net_device *net_dev);
+INT rt28xx_send_packets(IN struct sk_buff *skb_p, IN struct net_device *net_dev);
+
+#if LINUX_VERSION_CODE <= 0x20402	// Red Hat 7.1
+struct net_device *alloc_netdev(
+	int sizeof_priv,
+	const char *mask,
+	void (*setup)(struct net_device *));
+#endif // LINUX_VERSION_CODE //
+
+static void CfgInitHook(PRTMP_ADAPTER pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+extern	const struct iw_handler_def rt28xx_iw_handler_def;
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+extern	const struct iw_handler_def rt28xx_ap_iw_handler_def;
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+#if WIRELESS_EXT >= 12
+// This function will be called when query /proc
+struct iw_statistics *rt28xx_get_wireless_stats(
+    IN struct net_device *net_dev);
+#endif
+
+struct net_device_stats *RT28xx_get_ether_stats(
+    IN  struct net_device *net_dev);
+
+/*
+========================================================================
+Routine Description:
+    Close raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+	1. if open fail, kernel will not call the close function.
+	2. Free memory for
+		(1) Mlme Memory Handler:		MlmeHalt()
+		(2) TX & RX:					RTMPFreeTxRxRingMemory()
+		(3) BA Reordering: 				ba_reordering_resource_release()
+========================================================================
+*/
+int MainVirtualIF_close(IN struct net_device *net_dev)
+{
+    RTMP_ADAPTER *pAd = net_dev->ml_priv;
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+		return 0; // close ok
+
+	netif_carrier_off(pAd->net_dev);
+	netif_stop_queue(pAd->net_dev);
+
+
+	VIRTUAL_IF_DOWN(pAd);
+
+	RT_MOD_DEC_USE_COUNT();
+
+	return 0; // close ok
+}
+
+/*
+========================================================================
+Routine Description:
+    Open raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+	1. if open fail, kernel will not call the close function.
+	2. Free memory for
+		(1) Mlme Memory Handler:		MlmeHalt()
+		(2) TX & RX:					RTMPFreeTxRxRingMemory()
+		(3) BA Reordering: 				ba_reordering_resource_release()
+========================================================================
+*/
+int MainVirtualIF_open(IN struct net_device *net_dev)
+{
+    RTMP_ADAPTER *pAd = net_dev->ml_priv;
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+		return 0; // close ok
+
+	if (VIRTUAL_IF_UP(pAd) != 0)
+		return -1;
+
+	// increase MODULE use count
+	RT_MOD_INC_USE_COUNT();
+
+	netif_start_queue(net_dev);
+	netif_carrier_on(net_dev);
+	netif_wake_queue(net_dev);
+
+	return 0;
+}
+
+/*
+========================================================================
+Routine Description:
+    Close raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+	1. if open fail, kernel will not call the close function.
+	2. Free memory for
+		(1) Mlme Memory Handler:		MlmeHalt()
+		(2) TX & RX:					RTMPFreeTxRxRingMemory()
+		(3) BA Reordering: 				ba_reordering_resource_release()
+========================================================================
+*/
+int rt28xx_close(IN PNET_DEV dev)
+{
+	struct net_device * net_dev = (struct net_device *)dev;
+    RTMP_ADAPTER	*pAd = net_dev->ml_priv;
+	BOOLEAN 		Cancelled = FALSE;
+	UINT32			i = 0;
+
+
+    DBGPRINT(RT_DEBUG_TRACE, ("===> rt28xx_close\n"));
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+		return 0; // close ok
+
+
+#ifdef WDS_SUPPORT
+	WdsDown(pAd);
+#endif // WDS_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef RT2860
+		RTMPPCIeLinkCtrlValueRestore(pAd, RESTORE_CLOSE);
+#endif // RT2860 //
+
+		// If dirver doesn't wake up firmware here,
+		// NICLoadFirmware will hang forever when interface is up again.
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+        {
+		    AsicForceWakeup(pAd, TRUE);
+        }
+
+#ifdef QOS_DLS_SUPPORT
+		// send DLS-TEAR_DOWN message,
+		if (pAd->CommonCfg.bDLSCapable)
+		{
+			UCHAR i;
+
+			// tear down local dls table entry
+			for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+				{
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+					pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				}
+			}
+
+			// tear down peer dls table entry
+			for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+				{
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				}
+			}
+			RT28XX_MLME_HANDLER(pAd);
+		}
+#endif // QOS_DLS_SUPPORT //
+
+		if (INFRA_ON(pAd) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+		{
+			MLME_DISASSOC_REQ_STRUCT	DisReq;
+			MLME_QUEUE_ELEM *MsgElem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+			COPY_MAC_ADDR(DisReq.Addr, pAd->CommonCfg.Bssid);
+			DisReq.Reason =  REASON_DEAUTH_STA_LEAVING;
+
+			MsgElem->Machine = ASSOC_STATE_MACHINE;
+			MsgElem->MsgType = MT2_MLME_DISASSOC_REQ;
+			MsgElem->MsgLen = sizeof(MLME_DISASSOC_REQ_STRUCT);
+			NdisMoveMemory(MsgElem->Msg, &DisReq, sizeof(MLME_DISASSOC_REQ_STRUCT));
+
+			// Prevent to connect AP again in STAMlmePeriodicExec
+			pAd->MlmeAux.AutoReconnectSsidLen= 32;
+			NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+			MlmeDisassocReqAction(pAd, MsgElem);
+			kfree(MsgElem);
+
+			RTMPusecDelay(1000);
+		}
+
+
+#ifdef CCX_SUPPORT
+		RTMPCancelTimer(&pAd->StaCfg.LeapAuthTimer, &Cancelled);
+#endif
+
+		RTMPCancelTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, &Cancelled);
+		RTMPCancelTimer(&pAd->StaCfg.WpaDisassocAndBlockAssocTimer, &Cancelled);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+		{
+			union iwreq_data    wrqu;
+			// send wireless event to wpa_supplicant for infroming interface down.
+			memset(&wrqu, 0, sizeof(wrqu));
+			wrqu.data.flags = RT_INTERFACE_DOWN;
+			wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+		}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+		MlmeRadioOff(pAd);
+#ifdef RT2860
+		pAd->bPCIclkOff = FALSE;
+#endif // RT2860 //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+
+	for (i = 0 ; i < NUM_OF_TX_RING; i++)
+	{
+		while (pAd->DeQueueRunning[i] == TRUE)
+		{
+			printk("Waiting for TxQueue[%d] done..........\n", i);
+			RTMPusecDelay(1000);
+		}
+	}
+
+	// Stop Mlme state machine
+	MlmeHalt(pAd);
+
+	// Close kernel threads or tasklets
+	kill_thread_task(pAd);
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		MacTableReset(pAd);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+	MeasureReqTabExit(pAd);
+	TpcReqTabExit(pAd);
+
+
+#ifdef RT2860
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_ACTIVE))
+	{
+		NICDisableInterrupt(pAd);
+	}
+
+	// Disable Rx, register value supposed will remain after reset
+	NICIssueReset(pAd);
+
+	// Free IRQ
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+		// Deregister interrupt function
+		RT28XX_IRQ_RELEASE(net_dev)
+		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE);
+	}
+#endif // RT2860 //
+
+
+	// Free Ring or USB buffers
+	RTMPFreeTxRxRingMemory(pAd);
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+
+#ifdef DOT11_N_SUPPORT
+	// Free BA reorder resource
+	ba_reordering_resource_release(pAd);
+#endif // DOT11_N_SUPPORT //
+
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_START_UP);
+
+	return 0; // close ok
+} /* End of rt28xx_close */
+
+static int rt28xx_init(IN struct net_device *net_dev)
+{
+	PRTMP_ADAPTER 			pAd = (PRTMP_ADAPTER)net_dev->ml_priv;
+	UINT					index;
+	UCHAR					TmpPhy;
+	NDIS_STATUS				Status;
+	UINT32 		MacCsr0 = 0;
+
+
+#ifdef DOT11_N_SUPPORT
+	// Allocate BA Reordering memory
+	ba_reordering_resource_init(pAd, MAX_REORDERING_MPDU_NUM);
+#endif // DOT11_N_SUPPORT //
+
+	// Make sure MAC gets ready.
+	index = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, MAC_CSR0, &MacCsr0);
+		pAd->MACVersion = MacCsr0;
+
+		if ((pAd->MACVersion != 0x00) && (pAd->MACVersion != 0xFFFFFFFF))
+			break;
+
+		RTMPusecDelay(10);
+	} while (index++ < 100);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("MAC_CSR0  [ Ver:Rev=0x%08x]\n", pAd->MACVersion));
+
+	// Disable DMA
+	RT28XXDMADisable(pAd);
+
+
+	// Load 8051 firmware
+	Status = NICLoadFirmware(pAd);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("NICLoadFirmware failed, Status[=0x%08x]\n", Status));
+		goto err1;
+	}
+
+	NICLoadRateSwitchingParams(pAd);
+
+	// Disable interrupts here which is as soon as possible
+	// This statement should never be true. We might consider to remove it later
+#ifdef RT2860
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_ACTIVE))
+	{
+		NICDisableInterrupt(pAd);
+	}
+#endif // RT2860 //
+
+	Status = RTMPAllocTxRxRingMemory(pAd);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("RTMPAllocDMAMemory failed, Status[=0x%08x]\n", Status));
+		goto err1;
+	}
+
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE);
+
+	// initialize MLME
+	//
+
+	Status = MlmeInit(pAd);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("MlmeInit failed, Status[=0x%08x]\n", Status));
+		goto err2;
+	}
+
+	// Initialize pAd->StaCfg, pAd->ApCfg, pAd->CommonCfg to manufacture default
+	//
+	UserCfgInit(pAd);
+
+
+	RT28XX_TASK_THREAD_INIT(pAd, Status);
+	if (Status != NDIS_STATUS_SUCCESS)
+		goto err1;
+
+	CfgInitHook(pAd);
+
+
+#ifdef BLOCK_NET_IF
+	initblockQueueTab(pAd);
+#endif // BLOCK_NET_IF //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		NdisAllocateSpinLock(&pAd->MacTabLock);
+#endif // CONFIG_STA_SUPPORT //
+
+	MeasureReqTabInit(pAd);
+	TpcReqTabInit(pAd);
+
+	//
+	// Init the hardware, we need to init asic before read registry, otherwise mac register will be reset
+	//
+	Status = NICInitializeAdapter(pAd, TRUE);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("NICInitializeAdapter failed, Status[=0x%08x]\n", Status));
+		if (Status != NDIS_STATUS_SUCCESS)
+		goto err3;
+	}
+
+	// Read parameters from Config File
+	Status = RTMPReadParametersHook(pAd);
+
+	printk("1. Phy Mode = %d\n", pAd->CommonCfg.PhyMode);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("NICReadRegParameters failed, Status[=0x%08x]\n",Status));
+		goto err4;
+	}
+
+
+
+   	//Init Ba Capability parameters.
+#ifdef DOT11_N_SUPPORT
+	pAd->CommonCfg.DesiredHtPhy.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+	pAd->CommonCfg.DesiredHtPhy.AmsduEnable = (USHORT)pAd->CommonCfg.BACapability.field.AmsduEnable;
+	pAd->CommonCfg.DesiredHtPhy.AmsduSize = (USHORT)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.DesiredHtPhy.MimoPs = (USHORT)pAd->CommonCfg.BACapability.field.MMPSmode;
+	// UPdata to HT IE
+	pAd->CommonCfg.HtCapability.HtCapInfo.MimoPs = (USHORT)pAd->CommonCfg.BACapability.field.MMPSmode;
+	pAd->CommonCfg.HtCapability.HtCapInfo.AMsduSize = (USHORT)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.HtCapability.HtCapParm.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+#endif // DOT11_N_SUPPORT //
+
+	printk("2. Phy Mode = %d\n", pAd->CommonCfg.PhyMode);
+
+	// We should read EEPROM for all cases.  rt2860b
+	NICReadEEPROMParameters(pAd, mac);
+
+	printk("3. Phy Mode = %d\n", pAd->CommonCfg.PhyMode);
+
+	NICInitAsicFromEEPROM(pAd); //rt2860b
+
+	// Set PHY to appropriate mode
+	TmpPhy = pAd->CommonCfg.PhyMode;
+	pAd->CommonCfg.PhyMode = 0xff;
+	RTMPSetPhyMode(pAd, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+	SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+
+	// No valid channels.
+	if (pAd->ChannelListNum == 0)
+	{
+		printk("Wrong configuration. No valid channel found. Check \"ContryCode\" and \"ChannelGeography\" setting.\n");
+		goto err4;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	printk("MCS Set = %02x %02x %02x %02x %02x\n", pAd->CommonCfg.HtCapability.MCSSet[0],
+           pAd->CommonCfg.HtCapability.MCSSet[1], pAd->CommonCfg.HtCapability.MCSSet[2],
+           pAd->CommonCfg.HtCapability.MCSSet[3], pAd->CommonCfg.HtCapability.MCSSet[4]);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef IKANOS_VX_1X0
+	VR_IKANOS_FP_Init(pAd->ApCfg.BssidNum, pAd->PermanentAddress);
+#endif // IKANOS_VX_1X0 //
+
+		//
+	// Initialize RF register to default value
+	//
+	AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+	// 8051 firmware require the signal during booting time.
+	AsicSendCommandToMcu(pAd, 0x72, 0xFF, 0x00, 0x00);
+
+	if (pAd && (Status != NDIS_STATUS_SUCCESS))
+	{
+		//
+		// Undo everything if it failed
+		//
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+		{
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE);
+		}
+	}
+	else if (pAd)
+	{
+		// Microsoft HCT require driver send a disconnect event after driver initialization.
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_CHANGE);
+		DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event B!\n"));
+
+
+	}// end of else
+
+
+	DBGPRINT_S(Status, ("<==== RTMPInitialize, Status=%x\n", Status));
+
+	return TRUE;
+
+
+err4:
+err3:
+	MlmeHalt(pAd);
+err2:
+	RTMPFreeTxRxRingMemory(pAd);
+err1:
+
+#ifdef DOT11_N_SUPPORT
+	os_free_mem(pAd, pAd->mpdu_blk_pool.mem); // free BA pool
+#endif // DOT11_N_SUPPORT //
+	RT28XX_IRQ_RELEASE(net_dev);
+
+	// shall not set ml_priv to NULL here because the ml_priv didn't been free yet.
+	//net_dev->ml_priv = 0;
+#ifdef INF_AMAZON_SE
+err0:
+#endif // INF_AMAZON_SE //
+	printk("!!! %s Initialized fail !!!\n", RT28xx_CHIP_NAME);
+	return FALSE;
+} /* End of rt28xx_init */
+
+
+/*
+========================================================================
+Routine Description:
+    Open raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+========================================================================
+*/
+int rt28xx_open(IN PNET_DEV dev)
+{
+	struct net_device * net_dev = (struct net_device *)dev;
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+	int retval = 0;
+ 	POS_COOKIE pObj;
+
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -1;
+	}
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	if (pAd->OpMode == OPMODE_AP)
+	{
+		CW_MAX_IN_BITS = 6;
+	}
+	else if (pAd->OpMode == OPMODE_STA)
+	{
+		CW_MAX_IN_BITS = 10;
+	}
+
+#if WIRELESS_EXT >= 12
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		if (pAd->OpMode == OPMODE_AP)
+			net_dev->wireless_handlers = (struct iw_handler_def *) &rt28xx_ap_iw_handler_def;
+		else if (pAd->OpMode == OPMODE_STA)
+			net_dev->wireless_handlers = (struct iw_handler_def *) &rt28xx_iw_handler_def;
+	}
+#endif // WIRELESS_EXT >= 12 //
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef RT2860
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+    	// If dirver doesn't wake up firmware here,
+    	// NICLoadFirmware will hang forever when interface is up again.
+    	// RT2860 PCI
+    	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE) &&
+        	OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+    	{
+        	AUTO_WAKEUP_STRUC AutoWakeupCfg;
+			AsicForceWakeup(pAd, TRUE);
+        	AutoWakeupCfg.word = 0;
+	    	RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+        	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+    	}
+	}
+#endif // RT2860 //
+#endif // CONFIG_STA_SUPPORT //
+
+	// Init
+ 	pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	// reset Adapter flags
+	RTMP_CLEAR_FLAGS(pAd);
+
+	// Request interrupt service routine for PCI device
+	// register the interrupt routine with the os
+	RT28XX_IRQ_REQUEST(net_dev);
+
+
+	// Init BssTab & ChannelInfo tabbles for auto channel select.
+
+
+	// Chip & other init
+	if (rt28xx_init(net_dev) == FALSE)
+		goto err;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		NdisZeroMemory(pAd->StaCfg.dev_name, 16);
+		NdisMoveMemory(pAd->StaCfg.dev_name, net_dev->name, strlen(net_dev->name));
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Set up the Mac address
+	NdisMoveMemory(net_dev->dev_addr, (void *) pAd->CurrentAddress, 6);
+
+	// Init IRQ parameters
+	RT28XX_IRQ_INIT(pAd);
+
+	// Various AP function init
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+		{
+			union iwreq_data    wrqu;
+			// send wireless event to wpa_supplicant for infroming interface down.
+			memset(&wrqu, 0, sizeof(wrqu));
+			wrqu.data.flags = RT_INTERFACE_UP;
+			wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+		}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Enable Interrupt
+	RT28XX_IRQ_ENABLE(pAd);
+
+	// Now Enable RxTx
+	RTMPEnableRxTx(pAd);
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_START_UP);
+
+	{
+	UINT32 reg = 0;
+	RTMP_IO_READ32(pAd, 0x1300, &reg);  // clear garbage interrupts
+	printk("0x1300 = %08x\n", reg);
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef RT2860
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+        RTMPInitPCIeLinkCtrlValue(pAd);
+#endif // RT2860 //
+#endif // CONFIG_STA_SUPPORT //
+
+	return (retval);
+
+err:
+	return (-1);
+} /* End of rt28xx_open */
+
+
+/* Must not be called for mdev and apdev */
+static NDIS_STATUS rt_ieee80211_if_setup(struct net_device *dev, PRTMP_ADAPTER pAd)
+{
+	NDIS_STATUS Status;
+	INT     i=0;
+	CHAR    slot_name[IFNAMSIZ];
+	struct net_device   *device;
+
+
+	//ether_setup(dev);
+	dev->hard_start_xmit = rt28xx_send_packets;
+
+#ifdef IKANOS_VX_1X0
+	dev->hard_start_xmit = IKANOS_DataFramesTx;
+#endif // IKANOS_VX_1X0 //
+
+#ifdef CONFIG_STA_SUPPORT
+#if WIRELESS_EXT >= 12
+	if (pAd->OpMode == OPMODE_STA)
+	{
+		dev->wireless_handlers = &rt28xx_iw_handler_def;
+	}
+#endif //WIRELESS_EXT >= 12
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+#if WIRELESS_EXT >= 12
+	if (pAd->OpMode == OPMODE_AP)
+	{
+		dev->wireless_handlers = &rt28xx_ap_iw_handler_def;
+	}
+#endif //WIRELESS_EXT >= 12
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+#if WIRELESS_EXT < 21
+		dev->get_wireless_stats = rt28xx_get_wireless_stats;
+#endif
+	dev->get_stats = RT28xx_get_ether_stats;
+	dev->open = MainVirtualIF_open; //rt28xx_open;
+	dev->stop = MainVirtualIF_close; //rt28xx_close;
+	dev->priv_flags = INT_MAIN;
+	dev->do_ioctl = rt28xx_ioctl;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,24)
+    dev->validate_addr = NULL;
+#endif
+	// find available device name
+	for (i = 0; i < 8; i++)
+	{
+#ifdef MULTIPLE_CARD_SUPPORT
+		if (pAd->MC_RowID >= 0)
+			sprintf(slot_name, "ra%02d_%d", pAd->MC_RowID, i);
+		else
+#endif // MULTIPLE_CARD_SUPPORT //
+		sprintf(slot_name, "ra%d", i);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,24)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+        device = dev_get_by_name(dev_net(dev), slot_name);
+#else
+        device = dev_get_by_name(dev->nd_net, slot_name);
+#endif
+#else
+		device = dev_get_by_name(slot_name);
+#endif
+		if (device != NULL) dev_put(device);
+#else
+		for (device = dev_base; device != NULL; device = device->next)
+		{
+			if (strncmp(device->name, slot_name, 4) == 0)
+				break;
+		}
+#endif
+		if(device == NULL)
+			break;
+	}
+
+	if(i == 8)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("No available slot name\n"));
+		Status = NDIS_STATUS_FAILURE;
+	}
+	else
+	{
+#ifdef MULTIPLE_CARD_SUPPORT
+		if (pAd->MC_RowID >= 0)
+	        sprintf(dev->name, "ra%02d_%d", pAd->MC_RowID, i);
+		else
+#endif // MULTIPLE_CARD_SUPPORT //
+		sprintf(dev->name, "ra%d", i);
+		Status = NDIS_STATUS_SUCCESS;
+	}
+
+	return Status;
+
+}
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+/*
+========================================================================
+Routine Description:
+    Get card profile path.
+
+Arguments:
+    pAd
+
+Return Value:
+    TRUE		- Find a card profile
+	FALSE		- use default profile
+
+Note:
+========================================================================
+*/
+extern INT RTMPGetKeyParameter(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    IN  INT     destsize,
+    IN  PCHAR   buffer);
+
+BOOLEAN RTMP_CardInfoRead(
+	IN	PRTMP_ADAPTER pAd)
+{
+#define MC_SELECT_CARDID		0	/* use CARD ID (0 ~ 31) to identify different cards */
+#define MC_SELECT_MAC			1	/* use CARD MAC to identify different cards */
+#define MC_SELECT_CARDTYPE		2	/* use CARD type (abgn or bgn) to identify different cards */
+
+#define LETTER_CASE_TRANSLATE(txt_p, card_id)			\
+	{	UINT32 _len; char _char;						\
+		for(_len=0; _len<strlen(card_id); _len++) {		\
+			_char = *(txt_p + _len);					\
+			if (('A' <= _char) && (_char <= 'Z'))		\
+				*(txt_p+_len) = 'a'+(_char-'A');		\
+		} }
+
+	struct file *srcf;
+	INT retval, orgfsuid, orgfsgid;
+   	mm_segment_t orgfs;
+	CHAR *buffer, *tmpbuf, card_id_buf[30], RFIC_word[30];
+	BOOLEAN flg_match_ok = FALSE;
+	INT32 card_select_method;
+	INT32 card_free_id, card_nouse_id, card_same_mac_id, card_match_id;
+	EEPROM_ANTENNA_STRUC antenna;
+	USHORT addr01, addr23, addr45;
+	UINT8 mac[6];
+	UINT32 data, card_index;
+	UCHAR *start_ptr;
+
+
+	// init
+	buffer = kmalloc(MAX_INI_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if (buffer == NULL)
+        return FALSE;
+
+	tmpbuf = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if(tmpbuf == NULL)
+	{
+		kfree(buffer);
+        return NDIS_STATUS_FAILURE;
+	}
+
+	orgfsuid = current->fsuid;
+	orgfsgid = current->fsgid;
+	current->fsuid = current->fsgid = 0;
+    orgfs = get_fs();
+    set_fs(KERNEL_DS);
+
+	// get RF IC type
+	RTMP_IO_READ32(pAd, E2PROM_CSR, &data);
+
+	if ((data & 0x30) == 0)
+		pAd->EEPROMAddressNum = 6;	// 93C46
+	else if ((data & 0x30) == 0x10)
+		pAd->EEPROMAddressNum = 8;	// 93C66
+	else
+		pAd->EEPROMAddressNum = 8;	// 93C86
+
+	RT28xx_EEPROM_READ16(pAd, EEPROM_NIC1_OFFSET, antenna.word);
+
+	if ((antenna.field.RfIcType == RFIC_2850) ||
+		(antenna.field.RfIcType == RFIC_2750))
+	{
+		/* ABGN card */
+		strcpy(RFIC_word, "abgn");
+	}
+	else
+	{
+		/* BGN card */
+		strcpy(RFIC_word, "bgn");
+	}
+
+	// get MAC address
+	RT28xx_EEPROM_READ16(pAd, 0x04, addr01);
+	RT28xx_EEPROM_READ16(pAd, 0x06, addr23);
+	RT28xx_EEPROM_READ16(pAd, 0x08, addr45);
+
+	mac[0] = (UCHAR)(addr01 & 0xff);
+	mac[1] = (UCHAR)(addr01 >> 8);
+	mac[2] = (UCHAR)(addr23 & 0xff);
+	mac[3] = (UCHAR)(addr23 >> 8);
+	mac[4] = (UCHAR)(addr45 & 0xff);
+	mac[5] = (UCHAR)(addr45 >> 8);
+
+	// open card information file
+	srcf = filp_open(CARD_INFO_PATH, O_RDONLY, 0);
+	if (IS_ERR(srcf))
+	{
+		/* card information file does not exist */
+			DBGPRINT(RT_DEBUG_TRACE,
+				("--> Error %ld opening %s\n", -PTR_ERR(srcf), CARD_INFO_PATH));
+		return FALSE;
+	}
+
+	if (srcf->f_op && srcf->f_op->read)
+	{
+		/* card information file exists so reading the card information */
+		memset(buffer, 0x00, MAX_INI_BUFFER_SIZE);
+		retval = srcf->f_op->read(srcf, buffer, MAX_INI_BUFFER_SIZE, &srcf->f_pos);
+		if (retval < 0)
+		{
+			/* read fail */
+				DBGPRINT(RT_DEBUG_TRACE,
+					("--> Read %s error %d\n", CARD_INFO_PATH, -retval));
+		}
+		else
+		{
+			/* get card selection method */
+			memset(tmpbuf, 0x00, MAX_PARAM_BUFFER_SIZE);
+			card_select_method = MC_SELECT_CARDTYPE; // default
+
+			if (RTMPGetKeyParameter("SELECT", tmpbuf, 256, buffer))
+			{
+				if (strcmp(tmpbuf, "CARDID") == 0)
+					card_select_method = MC_SELECT_CARDID;
+				else if (strcmp(tmpbuf, "MAC") == 0)
+					card_select_method = MC_SELECT_MAC;
+				else if (strcmp(tmpbuf, "CARDTYPE") == 0)
+					card_select_method = MC_SELECT_CARDTYPE;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,
+					("MC> Card Selection = %d\n", card_select_method));
+
+			// init
+			card_free_id = -1;
+			card_nouse_id = -1;
+			card_same_mac_id = -1;
+			card_match_id = -1;
+
+			// search current card information records
+			for(card_index=0;
+				card_index<MAX_NUM_OF_MULTIPLE_CARD;
+				card_index++)
+			{
+				if ((*(UINT32 *)&MC_CardMac[card_index][0] == 0) &&
+					(*(UINT16 *)&MC_CardMac[card_index][4] == 0))
+				{
+					// MAC is all-0 so the entry is available
+					MC_CardUsed[card_index] = 0;
+
+					if (card_free_id < 0)
+						card_free_id = card_index; // 1st free entry
+				}
+				else
+				{
+					if (memcmp(MC_CardMac[card_index], mac, 6) == 0)
+					{
+						// we find the entry with same MAC
+						if (card_same_mac_id < 0)
+							card_same_mac_id = card_index; // 1st same entry
+					}
+					else
+					{
+						// MAC is not all-0 but used flag == 0
+						if ((MC_CardUsed[card_index] == 0) &&
+							(card_nouse_id < 0))
+						{
+							card_nouse_id = card_index; // 1st available entry
+						}
+					}
+				}
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,
+					("MC> Free = %d, Same = %d, NOUSE = %d\n",
+					card_free_id, card_same_mac_id, card_nouse_id));
+
+			if ((card_same_mac_id >= 0) &&
+				((card_select_method == MC_SELECT_CARDID) ||
+				(card_select_method == MC_SELECT_CARDTYPE)))
+			{
+				// same MAC entry is found
+				card_match_id = card_same_mac_id;
+
+				if (card_select_method == MC_SELECT_CARDTYPE)
+				{
+					// for CARDTYPE
+					sprintf(card_id_buf, "%02dCARDTYPE%s",
+							card_match_id, RFIC_word);
+
+					if ((start_ptr=rtstrstruncasecmp(buffer, card_id_buf)) != NULL)
+					{
+						// we found the card ID
+						LETTER_CASE_TRANSLATE(start_ptr, card_id_buf);
+					}
+				}
+			}
+			else
+			{
+				// the card is 1st plug-in, try to find the match card profile
+				switch(card_select_method)
+				{
+					case MC_SELECT_CARDID: // CARDID
+					default:
+						if (card_free_id >= 0)
+							card_match_id = card_free_id;
+						else
+							card_match_id = card_nouse_id;
+						break;
+
+					case MC_SELECT_MAC: // MAC
+						sprintf(card_id_buf, "MAC%02x:%02x:%02x:%02x:%02x:%02x",
+								mac[0], mac[1], mac[2],
+								mac[3], mac[4], mac[5]);
+
+						/* try to find the key word in the card file */
+						if ((start_ptr=rtstrstruncasecmp(buffer, card_id_buf)) != NULL)
+						{
+							LETTER_CASE_TRANSLATE(start_ptr, card_id_buf);
+
+							/* get the row ID (2 ASCII characters) */
+							start_ptr -= 2;
+							card_id_buf[0] = *(start_ptr);
+							card_id_buf[1] = *(start_ptr+1);
+							card_id_buf[2] = 0x00;
+
+							card_match_id = simple_strtol(card_id_buf, 0, 10);
+						}
+						break;
+
+					case MC_SELECT_CARDTYPE: // CARDTYPE
+						card_nouse_id = -1;
+
+						for(card_index=0;
+							card_index<MAX_NUM_OF_MULTIPLE_CARD;
+							card_index++)
+						{
+							sprintf(card_id_buf, "%02dCARDTYPE%s",
+									card_index, RFIC_word);
+
+							if ((start_ptr=rtstrstruncasecmp(buffer,
+														card_id_buf)) != NULL)
+							{
+								LETTER_CASE_TRANSLATE(start_ptr, card_id_buf);
+
+								if (MC_CardUsed[card_index] == 0)
+								{
+									/* current the card profile is not used */
+									if ((*(UINT32 *)&MC_CardMac[card_index][0] == 0) &&
+										(*(UINT16 *)&MC_CardMac[card_index][4] == 0))
+									{
+										// find it and no previous card use it
+										card_match_id = card_index;
+										break;
+									}
+									else
+									{
+										// ever a card use it
+										if (card_nouse_id < 0)
+											card_nouse_id = card_index;
+									}
+								}
+							}
+						}
+
+						// if not find a free one, use the available one
+						if (card_match_id < 0)
+							card_match_id = card_nouse_id;
+						break;
+				}
+			}
+
+			if (card_match_id >= 0)
+			{
+				// make up search keyword
+				switch(card_select_method)
+				{
+					case MC_SELECT_CARDID: // CARDID
+						sprintf(card_id_buf, "%02dCARDID", card_match_id);
+						break;
+
+					case MC_SELECT_MAC: // MAC
+						sprintf(card_id_buf,
+								"%02dmac%02x:%02x:%02x:%02x:%02x:%02x",
+								card_match_id,
+								mac[0], mac[1], mac[2],
+								mac[3], mac[4], mac[5]);
+						break;
+
+					case MC_SELECT_CARDTYPE: // CARDTYPE
+					default:
+						sprintf(card_id_buf, "%02dcardtype%s",
+								card_match_id, RFIC_word);
+						break;
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Search Keyword = %s\n", card_id_buf));
+
+				// read card file path
+				if (RTMPGetKeyParameter(card_id_buf, tmpbuf, 256, buffer))
+				{
+					if (strlen(tmpbuf) < sizeof(pAd->MC_FileName))
+					{
+						// backup card information
+						pAd->MC_RowID = card_match_id; /* base 0 */
+						MC_CardUsed[card_match_id] = 1;
+						memcpy(MC_CardMac[card_match_id], mac, sizeof(mac));
+
+						// backup card file path
+						NdisMoveMemory(pAd->MC_FileName, tmpbuf , strlen(tmpbuf));
+						pAd->MC_FileName[strlen(tmpbuf)] = '\0';
+						flg_match_ok = TRUE;
+
+						DBGPRINT(RT_DEBUG_TRACE,
+								("Card Profile Name = %s\n", pAd->MC_FileName));
+					}
+					else
+					{
+						DBGPRINT(RT_DEBUG_ERROR,
+								("Card Profile Name length too large!\n"));
+					}
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_ERROR,
+							("Can not find search key word in card.dat!\n"));
+				}
+
+				if ((flg_match_ok != TRUE) &&
+					(card_match_id < MAX_NUM_OF_MULTIPLE_CARD))
+				{
+					MC_CardUsed[card_match_id] = 0;
+					memset(MC_CardMac[card_match_id], 0, sizeof(mac));
+				}
+			} // if (card_match_id >= 0)
+		}
+	}
+
+	// close file
+	retval = filp_close(srcf, NULL);
+	set_fs(orgfs);
+	current->fsuid = orgfsuid;
+	current->fsgid = orgfsgid;
+	kfree(buffer);
+	kfree(tmpbuf);
+	return flg_match_ok;
+}
+#endif // MULTIPLE_CARD_SUPPORT //
+
+
+/*
+========================================================================
+Routine Description:
+    Probe RT28XX chipset.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+	_dev_id_p			Point to the PCI or USB device ID
+
+Return Value:
+    0					Probe OK
+	-ENODEV				Probe Fail
+
+Note:
+========================================================================
+*/
+INT __devinit   rt28xx_probe(
+    IN  void *_dev_p,
+    IN  void *_dev_id_p,
+	IN  UINT argc,
+	OUT PRTMP_ADAPTER *ppAd)
+{
+    struct  net_device	*net_dev;
+    PRTMP_ADAPTER       pAd = (PRTMP_ADAPTER) NULL;
+    INT                 status;
+	PVOID				handle;
+#ifdef RT2860
+	struct pci_dev *dev_p = (struct pci_dev *)_dev_p;
+#endif // RT2860 //
+
+
+#ifdef CONFIG_STA_SUPPORT
+    DBGPRINT(RT_DEBUG_TRACE, ("STA Driver version-%s\n", STA_DRIVER_VERSION));
+#endif // CONFIG_STA_SUPPORT //
+
+#if LINUX_VERSION_CODE <= 0x20402       // Red Hat 7.1
+    net_dev = alloc_netdev(sizeof(PRTMP_ADAPTER), "eth%d", ether_setup);
+#else
+    net_dev = alloc_etherdev(sizeof(PRTMP_ADAPTER));
+#endif
+    if (net_dev == NULL)
+    {
+        printk("alloc_netdev failed\n");
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,15)
+		module_put(THIS_MODULE);
+#endif //LINUX_VERSION_CODE < KERNEL_VERSION(2,6,15)
+#else
+		MOD_DEC_USE_COUNT;
+#endif
+        goto err_out;
+    }
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
+    SET_MODULE_OWNER(net_dev);
+#endif
+
+	netif_stop_queue(net_dev);
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+/* for supporting Network Manager */
+/* Set the sysfs physical device reference for the network logical device
+ * if set prior to registration will cause a symlink during initialization.
+ */
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0))
+    SET_NETDEV_DEV(net_dev, &(dev_p->dev));
+#endif
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+	// Allocate RTMP_ADAPTER miniport adapter structure
+	handle = kmalloc(sizeof(struct os_cookie), GFP_KERNEL);
+	RT28XX_HANDLE_DEV_ASSIGN(handle, dev_p);
+
+	status = RTMPAllocAdapterBlock(handle, &pAd);
+	if (status != NDIS_STATUS_SUCCESS)
+		goto err_out_free_netdev;
+
+	net_dev->ml_priv = (PVOID)pAd;
+    pAd->net_dev = net_dev; // must be before RT28XXNetDevInit()
+
+	RT28XXNetDevInit(_dev_p, net_dev, pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+    pAd->StaCfg.OriDevType = net_dev->type;
+#endif // CONFIG_STA_SUPPORT //
+
+	// Post config
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+	if (RT28XXProbePostConfig(_dev_p, pAd, argc) == FALSE)
+		goto err_out_unmap;
+#else
+	if (RT28XXProbePostConfig(_dev_p, pAd, 0) == FALSE)
+		goto err_out_unmap;
+#endif // LINUX_VERSION_CODE //
+
+#ifdef CONFIG_STA_SUPPORT
+	pAd->OpMode = OPMODE_STA;
+#endif // CONFIG_STA_SUPPORT //
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+	// find its profile path
+	pAd->MC_RowID = -1; // use default profile path
+	RTMP_CardInfoRead(pAd);
+
+	if (pAd->MC_RowID == -1)
+#ifdef CONFIG_STA_SUPPORT
+		strcpy(pAd->MC_FileName, STA_PROFILE_PATH);
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE,
+			("MC> ROW = %d, PATH = %s\n", pAd->MC_RowID, pAd->MC_FileName));
+#endif // MULTIPLE_CARD_SUPPORT //
+
+	// sample move
+	if (rt_ieee80211_if_setup(net_dev, pAd) != NDIS_STATUS_SUCCESS)
+		goto err_out_unmap;
+
+    // Register this device
+    status = register_netdev(net_dev);
+    if (status)
+        goto err_out_unmap;
+
+    // Set driver data
+	RT28XX_DRVDATA_SET(_dev_p);
+
+	*ppAd = pAd;
+    return 0; // probe ok
+
+
+	/* --------------------------- ERROR HANDLE --------------------------- */
+err_out_unmap:
+	RTMPFreeAdapter(pAd);
+	RT28XX_UNMAP();
+
+err_out_free_netdev:
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+    free_netdev(net_dev);
+#else
+	kfree(net_dev);
+#endif
+
+err_out:
+	RT28XX_PUT_DEVICE(dev_p);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+	return (LONG)NULL;
+#else
+    return -ENODEV; /* probe fail */
+#endif // LINUX_VERSION_CODE //
+} /* End of rt28xx_probe */
+
+
+/*
+========================================================================
+Routine Description:
+    The entry point for Linux kernel sent packet to our driver.
+
+Arguments:
+    sk_buff *skb		the pointer refer to a sk_buffer.
+
+Return Value:
+    0
+
+Note:
+	This function is the entry point of Tx Path for Os delivery packet to
+	our driver. You only can put OS-depened & STA/AP common handle procedures
+	in here.
+========================================================================
+*/
+int rt28xx_packet_xmit(struct sk_buff *skb)
+{
+	struct net_device *net_dev = skb->dev;
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+	int status = 0;
+	PNDIS_PACKET pPacket = (PNDIS_PACKET) skb;
+
+	/* RT2870STA does this in RTMPSendPackets() */
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_RESOURCES);
+		return 0;
+	}
+#endif // RALINK_ATE //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Drop send request since we are in monitor mode
+		if (MONITOR_ON(pAd))
+		{
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+			goto done;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+        // EapolStart size is 18
+	if (skb->len < 14)
+	{
+		//printk("bad packet size: %d\n", pkt->len);
+		hex_dump("bad packet", skb->data, skb->len);
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		goto done;
+	}
+
+	RTMP_SET_PACKET_5VT(pPacket, 0);
+#ifdef CONFIG_5VT_ENHANCE
+    if (*(int*)(skb->cb) == BRIDGE_TAG) {
+		RTMP_SET_PACKET_5VT(pPacket, 1);
+    }
+#endif
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+
+		STASendPackets((NDIS_HANDLE)pAd, (PPNDIS_PACKET) &pPacket, 1);
+	}
+
+#endif // CONFIG_STA_SUPPORT //
+
+	status = 0;
+done:
+
+	return status;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Send a packet to WLAN.
+
+Arguments:
+    skb_p           points to our adapter
+    dev_p           which WLAN network interface
+
+Return Value:
+    0: transmit successfully
+    otherwise: transmit fail
+
+Note:
+========================================================================
+*/
+INT rt28xx_send_packets(
+	IN struct sk_buff 		*skb_p,
+	IN struct net_device 	*net_dev)
+{
+    RTMP_ADAPTER *pAd = net_dev->ml_priv;
+	if (!(net_dev->flags & IFF_UP))
+	{
+		RELEASE_NDIS_PACKET(pAd, (PNDIS_PACKET)skb_p, NDIS_STATUS_FAILURE);
+		return 0;
+	}
+
+	NdisZeroMemory((PUCHAR)&skb_p->cb[CB_OFF], 15);
+	RTMP_SET_PACKET_NET_DEVICE_MBSSID(skb_p, MAIN_MBSSID);
+
+	return rt28xx_packet_xmit(skb_p);
+
+} /* End of MBSS_VirtualIF_PacketSend */
+
+
+
+
+#if LINUX_VERSION_CODE <= 0x20402	// Red Hat 7.1
+struct net_device *alloc_netdev(
+	int sizeof_priv,
+	const char *mask,
+	void (*setup)(struct net_device *))
+{
+    struct net_device	*dev;
+    INT					alloc_size;
+
+
+    /* ensure 32-byte alignment of the private area */
+    alloc_size = sizeof (*dev) + sizeof_priv + 31;
+
+    dev = (struct net_device *) kmalloc(alloc_size, GFP_KERNEL);
+    if (dev == NULL)
+    {
+        DBGPRINT(RT_DEBUG_ERROR,
+				("alloc_netdev: Unable to allocate device memory.\n"));
+        return NULL;
+    }
+
+    memset(dev, 0, alloc_size);
+
+    if (sizeof_priv)
+        dev->priv = (void *) (((long)(dev + 1) + 31) & ~31);
+
+    setup(dev);
+    strcpy(dev->name, mask);
+
+    return dev;
+}
+#endif // LINUX_VERSION_CODE //
+
+
+void CfgInitHook(PRTMP_ADAPTER pAd)
+{
+	pAd->bBroadComHT = TRUE;
+} /* End of CfgInitHook */
+
+
+#if WIRELESS_EXT >= 12
+// This function will be called when query /proc
+struct iw_statistics *rt28xx_get_wireless_stats(
+    IN struct net_device *net_dev)
+{
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("rt28xx_get_wireless_stats --->\n"));
+
+	pAd->iw_stats.status = 0; // Status - device dependent for now
+
+	// link quality
+	pAd->iw_stats.qual.qual = ((pAd->Mlme.ChannelQuality * 12)/10 + 10);
+	if(pAd->iw_stats.qual.qual > 100)
+		pAd->iw_stats.qual.qual = 100;
+
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->OpMode == OPMODE_STA)
+		pAd->iw_stats.qual.level = RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2);
+#endif // CONFIG_STA_SUPPORT //
+
+	pAd->iw_stats.qual.noise = pAd->BbpWriteLatch[66]; // noise level (dBm)
+
+	pAd->iw_stats.qual.noise += 256 - 143;
+	pAd->iw_stats.qual.updated = 1;     // Flags to know if updated
+#ifdef IW_QUAL_DBM
+	pAd->iw_stats.qual.updated |= IW_QUAL_DBM;	// Level + Noise are dBm
+#endif // IW_QUAL_DBM //
+
+	pAd->iw_stats.discard.nwid = 0;     // Rx : Wrong nwid/essid
+	pAd->iw_stats.miss.beacon = 0;      // Missed beacons/superframe
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<--- rt28xx_get_wireless_stats\n"));
+	return &pAd->iw_stats;
+} /* End of rt28xx_get_wireless_stats */
+#endif // WIRELESS_EXT //
+
+
+
+void tbtt_tasklet(unsigned long data)
+{
+#define MAX_TX_IN_TBTT		(16)
+
+}
+
+INT rt28xx_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT					cmd)
+{
+	VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+	RTMP_ADAPTER	*pAd = NULL;
+	INT				ret = 0;
+
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		pAd = net_dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = net_dev->ml_priv;
+		pAd = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		ret = rt28xx_sta_ioctl(net_dev, rq, cmd);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	return ret;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        return ethernet statistics counter
+
+    Arguments:
+        net_dev                     Pointer to net_device
+
+    Return Value:
+        net_device_stats*
+
+    Note:
+
+    ========================================================================
+*/
+struct net_device_stats *RT28xx_get_ether_stats(
+    IN  struct net_device *net_dev)
+{
+    RTMP_ADAPTER *pAd = NULL;
+
+	if (net_dev)
+		pAd = net_dev->ml_priv;
+
+	if (pAd)
+	{
+
+		pAd->stats.rx_packets = pAd->WlanCounters.ReceivedFragmentCount.QuadPart;
+		pAd->stats.tx_packets = pAd->WlanCounters.TransmittedFragmentCount.QuadPart;
+
+		pAd->stats.rx_bytes = pAd->RalinkCounters.ReceivedByteCount;
+		pAd->stats.tx_bytes = pAd->RalinkCounters.TransmittedByteCount;
+
+		pAd->stats.rx_errors = pAd->Counters8023.RxErrors;
+		pAd->stats.tx_errors = pAd->Counters8023.TxErrors;
+
+		pAd->stats.rx_dropped = 0;
+		pAd->stats.tx_dropped = 0;
+
+	    pAd->stats.multicast = pAd->WlanCounters.MulticastReceivedFrameCount.QuadPart;   // multicast packets received
+	    pAd->stats.collisions = pAd->Counters8023.OneCollision + pAd->Counters8023.MoreCollisions;  // Collision packets
+
+	    pAd->stats.rx_length_errors = 0;
+	    pAd->stats.rx_over_errors = pAd->Counters8023.RxNoBuffer;                   // receiver ring buff overflow
+	    pAd->stats.rx_crc_errors = 0;//pAd->WlanCounters.FCSErrorCount;     // recved pkt with crc error
+	    pAd->stats.rx_frame_errors = pAd->Counters8023.RcvAlignmentErrors;          // recv'd frame alignment error
+	    pAd->stats.rx_fifo_errors = pAd->Counters8023.RxNoBuffer;                   // recv'r fifo overrun
+	    pAd->stats.rx_missed_errors = 0;                                            // receiver missed packet
+
+	    // detailed tx_errors
+	    pAd->stats.tx_aborted_errors = 0;
+	    pAd->stats.tx_carrier_errors = 0;
+	    pAd->stats.tx_fifo_errors = 0;
+	    pAd->stats.tx_heartbeat_errors = 0;
+	    pAd->stats.tx_window_errors = 0;
+
+	    // for cslip etc
+	    pAd->stats.rx_compressed = 0;
+	    pAd->stats.tx_compressed = 0;
+
+		return &pAd->stats;
+	}
+	else
+    	return NULL;
+}
+
diff --git a/drivers/staging/rt2860/rt_profile.c b/drivers/staging/rt2860/rt_profile.c
new file mode 100644
index 000000000000..326a3cb52b9c
--- /dev/null
+++ b/drivers/staging/rt2860/rt_profile.c
@@ -0,0 +1,1981 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "rt_config.h"
+
+#ifdef DOT11_N_SUPPORT
+static void HTParametersHook(
+	IN	PRTMP_ADAPTER pAd,
+	IN	CHAR		  *pValueStr,
+	IN	CHAR		  *pInput);
+#endif // DOT11_N_SUPPORT //
+
+#define ETH_MAC_ADDR_STR_LEN 17  // in format of xx:xx:xx:xx:xx:xx
+
+// We assume the s1 is a sting, s2 is a memory space with 6 bytes. and content of s1 will be changed.
+BOOLEAN rtstrmactohex(char *s1, char *s2)
+{
+	int i = 0;
+	char *ptokS = s1, *ptokE = s1;
+
+	if (strlen(s1) != ETH_MAC_ADDR_STR_LEN)
+		return FALSE;
+
+	while((*ptokS) != '\0')
+	{
+		if((ptokE = strchr(ptokS, ':')) != NULL)
+			*ptokE++ = '\0';
+		if ((strlen(ptokS) != 2) || (!isxdigit(*ptokS)) || (!isxdigit(*(ptokS+1))))
+			break; // fail
+		AtoH(ptokS, &s2[i++], 1);
+		ptokS = ptokE;
+		if (i == 6)
+			break; // parsing finished
+	}
+
+	return ( i == 6 ? TRUE : FALSE);
+
+}
+
+
+// we assume the s1 and s2 both are strings.
+BOOLEAN rtstrcasecmp(char *s1, char *s2)
+{
+	char *p1 = s1, *p2 = s2;
+
+	if (strlen(s1) != strlen(s2))
+		return FALSE;
+
+	while(*p1 != '\0')
+	{
+		if((*p1 != *p2) && ((*p1 ^ *p2) != 0x20))
+			return FALSE;
+		p1++;
+		p2++;
+	}
+
+	return TRUE;
+}
+
+// we assume the s1 (buffer) and s2 (key) both are strings.
+char * rtstrstruncasecmp(char * s1, char * s2)
+{
+	INT l1, l2, i;
+	char temp1, temp2;
+
+	l2 = strlen(s2);
+	if (!l2)
+		return (char *) s1;
+
+	l1 = strlen(s1);
+
+	while (l1 >= l2)
+	{
+		l1--;
+
+		for(i=0; i<l2; i++)
+		{
+			temp1 = *(s1+i);
+			temp2 = *(s2+i);
+
+			if (('a' <= temp1) && (temp1 <= 'z'))
+				temp1 = 'A'+(temp1-'a');
+			if (('a' <= temp2) && (temp2 <= 'z'))
+				temp2 = 'A'+(temp2-'a');
+
+			if (temp1 != temp2)
+				break;
+		}
+
+		if (i == l2)
+			return (char *) s1;
+
+		s1++;
+	}
+
+	return NULL; // not found
+}
+
+//add by kathy
+
+ /**
+  * strstr - Find the first substring in a %NUL terminated string
+  * @s1: The string to be searched
+  * @s2: The string to search for
+  */
+char * rtstrstr(const char * s1,const char * s2)
+{
+	INT l1, l2;
+
+	l2 = strlen(s2);
+	if (!l2)
+		return (char *) s1;
+
+	l1 = strlen(s1);
+
+	while (l1 >= l2)
+	{
+		l1--;
+		if (!memcmp(s1,s2,l2))
+			return (char *) s1;
+		s1++;
+	}
+
+	return NULL;
+}
+
+/**
+ * rstrtok - Split a string into tokens
+ * @s: The string to be searched
+ * @ct: The characters to search for
+ * * WARNING: strtok is deprecated, use strsep instead. However strsep is not compatible with old architecture.
+ */
+char * __rstrtok;
+char * rstrtok(char * s,const char * ct)
+{
+	char *sbegin, *send;
+
+	sbegin  = s ? s : __rstrtok;
+	if (!sbegin)
+	{
+		return NULL;
+	}
+
+	sbegin += strspn(sbegin,ct);
+	if (*sbegin == '\0')
+	{
+		__rstrtok = NULL;
+		return( NULL );
+	}
+
+	send = strpbrk( sbegin, ct);
+	if (send && *send != '\0')
+		*send++ = '\0';
+
+	__rstrtok = send;
+
+	return (sbegin);
+}
+
+/**
+ * delimitcnt - return the count of a given delimiter in a given string.
+ * @s: The string to be searched.
+ * @ct: The delimiter to search for.
+ * Notice : We suppose the delimiter is a single-char string(for example : ";").
+ */
+INT delimitcnt(char * s,const char * ct)
+{
+	INT count = 0;
+	/* point to the beginning of the line */
+	const char *token = s;
+
+	for ( ;; )
+	{
+		token = strpbrk(token, ct); /* search for delimiters */
+
+        if ( token == NULL )
+		{
+			/* advanced to the terminating null character */
+			break;
+		}
+		/* skip the delimiter */
+	    ++token;
+
+		/*
+		 * Print the found text: use len with %.*s to specify field width.
+		 */
+
+		/* accumulate delimiter count */
+	    ++count;
+	}
+    return count;
+}
+
+/*
+  * converts the Internet host address from the standard numbers-and-dots notation
+  * into binary data.
+  * returns nonzero if the address is valid, zero if not.
+  */
+int rtinet_aton(const char *cp, unsigned int *addr)
+{
+	unsigned int 	val;
+	int         	base, n;
+	char        	c;
+	unsigned int    parts[4];
+	unsigned int    *pp = parts;
+
+	for (;;)
+    {
+         /*
+          * Collect number up to ``.''.
+          * Values are specified as for C:
+          *	0x=hex, 0=octal, other=decimal.
+          */
+         val = 0;
+         base = 10;
+         if (*cp == '0')
+         {
+             if (*++cp == 'x' || *cp == 'X')
+                 base = 16, cp++;
+             else
+                 base = 8;
+         }
+         while ((c = *cp) != '\0')
+         {
+             if (isdigit((unsigned char) c))
+             {
+                 val = (val * base) + (c - '0');
+                 cp++;
+                 continue;
+             }
+             if (base == 16 && isxdigit((unsigned char) c))
+             {
+                 val = (val << 4) +
+                     (c + 10 - (islower((unsigned char) c) ? 'a' : 'A'));
+                 cp++;
+                 continue;
+             }
+             break;
+         }
+         if (*cp == '.')
+         {
+             /*
+              * Internet format: a.b.c.d a.b.c   (with c treated as 16-bits)
+              * a.b     (with b treated as 24 bits)
+              */
+             if (pp >= parts + 3 || val > 0xff)
+                 return 0;
+             *pp++ = val, cp++;
+         }
+         else
+             break;
+     }
+
+     /*
+      * Check for trailing junk.
+      */
+     while (*cp)
+         if (!isspace((unsigned char) *cp++))
+             return 0;
+
+     /*
+      * Concoct the address according to the number of parts specified.
+      */
+     n = pp - parts + 1;
+     switch (n)
+     {
+
+         case 1:         /* a -- 32 bits */
+             break;
+
+         case 2:         /* a.b -- 8.24 bits */
+             if (val > 0xffffff)
+                 return 0;
+             val |= parts[0] << 24;
+             break;
+
+         case 3:         /* a.b.c -- 8.8.16 bits */
+             if (val > 0xffff)
+                 return 0;
+             val |= (parts[0] << 24) | (parts[1] << 16);
+             break;
+
+         case 4:         /* a.b.c.d -- 8.8.8.8 bits */
+             if (val > 0xff)
+                 return 0;
+             val |= (parts[0] << 24) | (parts[1] << 16) | (parts[2] << 8);
+             break;
+     }
+
+     *addr = htonl(val);
+     return 1;
+
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Find key section for Get key parameter.
+
+    Arguments:
+        buffer                      Pointer to the buffer to start find the key section
+        section                     the key of the secion to be find
+
+    Return Value:
+        NULL                        Fail
+        Others                      Success
+    ========================================================================
+*/
+PUCHAR  RTMPFindSection(
+    IN  PCHAR   buffer)
+{
+    CHAR temp_buf[32];
+    PUCHAR  ptr;
+
+    strcpy(temp_buf, "Default");
+
+    if((ptr = rtstrstr(buffer, temp_buf)) != NULL)
+            return (ptr+strlen("\n"));
+        else
+            return NULL;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Get key parameter.
+
+    Arguments:
+        key                         Pointer to key string
+        dest                        Pointer to destination
+        destsize                    The datasize of the destination
+        buffer                      Pointer to the buffer to start find the key
+
+    Return Value:
+        TRUE                        Success
+        FALSE                       Fail
+
+    Note:
+        This routine get the value with the matched key (case case-sensitive)
+    ========================================================================
+*/
+INT RTMPGetKeyParameter(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    IN  INT     destsize,
+    IN  PCHAR   buffer)
+{
+    UCHAR *temp_buf1 = NULL;
+    UCHAR *temp_buf2 = NULL;
+    CHAR *start_ptr;
+    CHAR *end_ptr;
+    CHAR *ptr;
+    CHAR *offset = 0;
+    INT  len;
+
+	//temp_buf1 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf1, MAX_PARAM_BUFFER_SIZE);
+
+	if(temp_buf1 == NULL)
+        return (FALSE);
+
+	//temp_buf2 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf2, MAX_PARAM_BUFFER_SIZE);
+	if(temp_buf2 == NULL)
+	{
+		os_free_mem(NULL, temp_buf1);
+        return (FALSE);
+	}
+
+    //find section
+    if((offset = RTMPFindSection(buffer)) == NULL)
+    {
+    	os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf1, "\n");
+    strcat(temp_buf1, key);
+    strcat(temp_buf1, "=");
+
+    //search key
+    if((start_ptr=rtstrstr(offset, temp_buf1))==NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    start_ptr+=strlen("\n");
+    if((end_ptr=rtstrstr(start_ptr, "\n"))==NULL)
+       end_ptr=start_ptr+strlen(start_ptr);
+
+    if (end_ptr<start_ptr)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    NdisMoveMemory(temp_buf2, start_ptr, end_ptr-start_ptr);
+    temp_buf2[end_ptr-start_ptr]='\0';
+    len = strlen(temp_buf2);
+    strcpy(temp_buf1, temp_buf2);
+    if((start_ptr=rtstrstr(temp_buf1, "=")) == NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf2, start_ptr+1);
+    ptr = temp_buf2;
+    //trim space or tab
+    while(*ptr != 0x00)
+    {
+        if( (*ptr == ' ') || (*ptr == '\t') )
+            ptr++;
+        else
+           break;
+    }
+
+    len = strlen(ptr);
+    memset(dest, 0x00, destsize);
+    strncpy(dest, ptr, len >= destsize ?  destsize: len);
+
+	os_free_mem(NULL, temp_buf1);
+    os_free_mem(NULL, temp_buf2);
+    return TRUE;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Get key parameter.
+
+    Arguments:
+        key                         Pointer to key string
+        dest                        Pointer to destination
+        destsize                    The datasize of the destination
+        buffer                      Pointer to the buffer to start find the key
+
+    Return Value:
+        TRUE                        Success
+        FALSE                       Fail
+
+    Note:
+        This routine get the value with the matched key (case case-sensitive).
+        It is called for parsing SSID and any key string.
+    ========================================================================
+*/
+INT RTMPGetCriticalParameter(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    IN  INT     destsize,
+    IN  PCHAR   buffer)
+{
+    UCHAR *temp_buf1 = NULL;
+    UCHAR *temp_buf2 = NULL;
+    CHAR *start_ptr;
+    CHAR *end_ptr;
+    CHAR *ptr;
+    CHAR *offset = 0;
+    INT  len;
+
+	//temp_buf1 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf1, MAX_PARAM_BUFFER_SIZE);
+
+	if(temp_buf1 == NULL)
+        return (FALSE);
+
+	//temp_buf2 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf2, MAX_PARAM_BUFFER_SIZE);
+	if(temp_buf2 == NULL)
+	{
+		os_free_mem(NULL, temp_buf1);
+        return (FALSE);
+	}
+
+    //find section
+    if((offset = RTMPFindSection(buffer)) == NULL)
+    {
+    	os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf1, "\n");
+    strcat(temp_buf1, key);
+    strcat(temp_buf1, "=");
+
+    //search key
+    if((start_ptr=rtstrstr(offset, temp_buf1))==NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    start_ptr+=strlen("\n");
+    if((end_ptr=rtstrstr(start_ptr, "\n"))==NULL)
+       end_ptr=start_ptr+strlen(start_ptr);
+
+    if (end_ptr<start_ptr)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    NdisMoveMemory(temp_buf2, start_ptr, end_ptr-start_ptr);
+    temp_buf2[end_ptr-start_ptr]='\0';
+    len = strlen(temp_buf2);
+    strcpy(temp_buf1, temp_buf2);
+    if((start_ptr=rtstrstr(temp_buf1, "=")) == NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf2, start_ptr+1);
+    ptr = temp_buf2;
+
+    //trim tab
+    /* We cannot trim space(' ') for SSID and key string. */
+    while(*ptr != 0x00)
+    {
+        //if( (*ptr == ' ') || (*ptr == '\t') )
+        if( (*ptr == '\t') )
+            ptr++;
+        else
+           break;
+    }
+
+    len = strlen(ptr);
+    memset(dest, 0x00, destsize);
+    strncpy(dest, ptr, len >= destsize ?  destsize: len);
+
+	os_free_mem(NULL, temp_buf1);
+    os_free_mem(NULL, temp_buf2);
+    return TRUE;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Get multiple key parameter.
+
+    Arguments:
+        key                         Pointer to key string
+        dest                        Pointer to destination
+        destsize                    The datasize of the destination
+        buffer                      Pointer to the buffer to start find the key
+
+    Return Value:
+        TRUE                        Success
+        FALSE                       Fail
+
+    Note:
+        This routine get the value with the matched key (case case-sensitive)
+    ========================================================================
+*/
+INT RTMPGetKeyParameterWithOffset(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    OUT	USHORT	*end_offset,
+    IN  INT     destsize,
+    IN  PCHAR   buffer,
+    IN	BOOLEAN	bTrimSpace)
+{
+    UCHAR *temp_buf1 = NULL;
+    UCHAR *temp_buf2 = NULL;
+    CHAR *start_ptr;
+    CHAR *end_ptr;
+    CHAR *ptr;
+    CHAR *offset = 0;
+    INT  len;
+
+	if (*end_offset >= MAX_INI_BUFFER_SIZE)
+		return (FALSE);
+
+	os_alloc_mem(NULL, &temp_buf1, MAX_PARAM_BUFFER_SIZE);
+
+	if(temp_buf1 == NULL)
+        return (FALSE);
+
+	os_alloc_mem(NULL, &temp_buf2, MAX_PARAM_BUFFER_SIZE);
+	if(temp_buf2 == NULL)
+	{
+		os_free_mem(NULL, temp_buf1);
+        return (FALSE);
+	}
+
+    //find section
+	if(*end_offset == 0)
+    {
+		if ((offset = RTMPFindSection(buffer)) == NULL)
+		{
+			os_free_mem(NULL, temp_buf1);
+	    	os_free_mem(NULL, temp_buf2);
+    	    return (FALSE);
+		}
+    }
+	else
+		offset = buffer + (*end_offset);
+
+    strcpy(temp_buf1, "\n");
+    strcat(temp_buf1, key);
+    strcat(temp_buf1, "=");
+
+    //search key
+    if((start_ptr=rtstrstr(offset, temp_buf1))==NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    start_ptr+=strlen("\n");
+    if((end_ptr=rtstrstr(start_ptr, "\n"))==NULL)
+       end_ptr=start_ptr+strlen(start_ptr);
+
+    if (end_ptr<start_ptr)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+	*end_offset = end_ptr - buffer;
+
+    NdisMoveMemory(temp_buf2, start_ptr, end_ptr-start_ptr);
+    temp_buf2[end_ptr-start_ptr]='\0';
+    len = strlen(temp_buf2);
+    strcpy(temp_buf1, temp_buf2);
+    if((start_ptr=rtstrstr(temp_buf1, "=")) == NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf2, start_ptr+1);
+    ptr = temp_buf2;
+    //trim space or tab
+    while(*ptr != 0x00)
+    {
+        if((bTrimSpace && (*ptr == ' ')) || (*ptr == '\t') )
+            ptr++;
+        else
+           break;
+    }
+
+    len = strlen(ptr);
+    memset(dest, 0x00, destsize);
+    strncpy(dest, ptr, len >= destsize ?  destsize: len);
+
+	os_free_mem(NULL, temp_buf1);
+    os_free_mem(NULL, temp_buf2);
+    return TRUE;
+}
+
+
+static int rtmp_parse_key_buffer_from_file(IN  PRTMP_ADAPTER pAd,IN  char *buffer,IN  ULONG KeyType,IN  INT BSSIdx,IN  INT KeyIdx)
+{
+	PUCHAR		keybuff;
+	INT			i = BSSIdx, idx = KeyIdx;
+	ULONG		KeyLen;
+	UCHAR		CipherAlg = CIPHER_WEP64;
+
+	keybuff = buffer;
+	KeyLen = strlen(keybuff);
+
+	if (KeyType == 1)
+	{//Ascii
+		if( (KeyLen == 5) || (KeyLen == 13))
+		{
+			pAd->SharedKey[i][idx].KeyLen = KeyLen;
+			NdisMoveMemory(pAd->SharedKey[i][idx].Key, keybuff, KeyLen);
+			if (KeyLen == 5)
+				CipherAlg = CIPHER_WEP64;
+			else
+				CipherAlg = CIPHER_WEP128;
+			pAd->SharedKey[i][idx].CipherAlg = CipherAlg;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("I/F(ra%d) Key%dStr=%s and type=%s\n", i, idx+1, keybuff, (KeyType == 0) ? "Hex":"Ascii"));
+			return 1;
+		}
+		else
+		{//Invalid key length
+			DBGPRINT(RT_DEBUG_ERROR, ("Key%dStr is Invalid key length! KeyLen = %ld!\n", idx+1, KeyLen));
+			return 0;
+		}
+	}
+	else
+	{//Hex type
+		if( (KeyLen == 10) || (KeyLen == 26))
+		{
+			pAd->SharedKey[i][idx].KeyLen = KeyLen / 2;
+			AtoH(keybuff, pAd->SharedKey[i][idx].Key, KeyLen / 2);
+			if (KeyLen == 10)
+				CipherAlg = CIPHER_WEP64;
+			else
+				CipherAlg = CIPHER_WEP128;
+			pAd->SharedKey[i][idx].CipherAlg = CipherAlg;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("I/F(ra%d) Key%dStr=%s and type=%s\n", i, idx+1, keybuff, (KeyType == 0) ? "Hex":"Ascii"));
+			return 1;
+		}
+		else
+		{//Invalid key length
+			DBGPRINT(RT_DEBUG_ERROR, ("I/F(ra%d) Key%dStr is Invalid key length! KeyLen = %ld!\n", i, idx+1, KeyLen));
+			return 0;
+		}
+	}
+}
+static void rtmp_read_key_parms_from_file(IN  PRTMP_ADAPTER pAd, char *tmpbuf, char *buffer)
+{
+	char		tok_str[16];
+	PUCHAR		macptr;
+	INT			i = 0, idx;
+	ULONG		KeyType[MAX_MBSSID_NUM];
+	ULONG		KeyIdx;
+
+	NdisZeroMemory(KeyType, MAX_MBSSID_NUM);
+
+	//DefaultKeyID
+	if(RTMPGetKeyParameter("DefaultKeyID", tmpbuf, 25, buffer))
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			KeyIdx = simple_strtol(tmpbuf, 0, 10);
+			if((KeyIdx >= 1 ) && (KeyIdx <= 4))
+				pAd->StaCfg.DefaultKeyId = (UCHAR) (KeyIdx - 1);
+			else
+				pAd->StaCfg.DefaultKeyId = 0;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("DefaultKeyID(0~3)=%d\n", pAd->StaCfg.DefaultKeyId));
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+	for (idx = 0; idx < 4; idx++)
+	{
+		sprintf(tok_str, "Key%dType", idx + 1);
+		//Key1Type
+		if (RTMPGetKeyParameter(tok_str, tmpbuf, 128, buffer))
+		{
+		    for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+		    {
+			    KeyType[i] = simple_strtol(macptr, 0, 10);
+		    }
+
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			{
+				sprintf(tok_str, "Key%dStr", idx + 1);
+				if (RTMPGetCriticalParameter(tok_str, tmpbuf, 128, buffer))
+				{
+					rtmp_parse_key_buffer_from_file(pAd, tmpbuf, KeyType[BSS0], BSS0, idx);
+				}
+			}
+#endif // CONFIG_STA_SUPPORT //
+		}
+	}
+}
+
+
+#ifdef CONFIG_STA_SUPPORT
+static void rtmp_read_sta_wmm_parms_from_file(IN  PRTMP_ADAPTER pAd, char *tmpbuf, char *buffer)
+{
+	PUCHAR					macptr;
+	INT						i=0;
+	BOOLEAN					bWmmEnable = FALSE;
+
+	//WmmCapable
+	if(RTMPGetKeyParameter("WmmCapable", tmpbuf, 32, buffer))
+	{
+		if(simple_strtol(tmpbuf, 0, 10) != 0) //Enable
+		{
+			pAd->CommonCfg.bWmmCapable = TRUE;
+			bWmmEnable = TRUE;
+		}
+		else //Disable
+		{
+			pAd->CommonCfg.bWmmCapable = FALSE;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("WmmCapable=%d\n", pAd->CommonCfg.bWmmCapable));
+	}
+
+#ifdef QOS_DLS_SUPPORT
+	//DLSCapable
+	if(RTMPGetKeyParameter("DLSCapable", tmpbuf, 32, buffer))
+	{
+		if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+		{
+			pAd->CommonCfg.bDLSCapable = TRUE;
+		}
+		else //Disable
+		{
+			pAd->CommonCfg.bDLSCapable = FALSE;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("bDLSCapable=%d\n", pAd->CommonCfg.bDLSCapable));
+	}
+#endif // QOS_DLS_SUPPORT //
+
+	//AckPolicy for AC_BK, AC_BE, AC_VI, AC_VO
+	if(RTMPGetKeyParameter("AckPolicy", tmpbuf, 32, buffer))
+	{
+		for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+		{
+			pAd->CommonCfg.AckPolicy[i] = (UCHAR)simple_strtol(macptr, 0, 10);
+
+			DBGPRINT(RT_DEBUG_TRACE, ("AckPolicy[%d]=%d\n", i, pAd->CommonCfg.AckPolicy[i]));
+		}
+	}
+
+	if (bWmmEnable)
+	{
+		//APSDCapable
+		if(RTMPGetKeyParameter("APSDCapable", tmpbuf, 10, buffer))
+		{
+			if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+				pAd->CommonCfg.bAPSDCapable = TRUE;
+			else
+				pAd->CommonCfg.bAPSDCapable = FALSE;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("APSDCapable=%d\n", pAd->CommonCfg.bAPSDCapable));
+		}
+
+		//APSDAC for AC_BE, AC_BK, AC_VI, AC_VO
+		if(RTMPGetKeyParameter("APSDAC", tmpbuf, 32, buffer))
+		{
+			BOOLEAN apsd_ac[4];
+
+			for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+			{
+				apsd_ac[i] = (BOOLEAN)simple_strtol(macptr, 0, 10);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("APSDAC%d  %d\n", i,  apsd_ac[i]));
+			}
+
+			pAd->CommonCfg.bAPSDAC_BE = apsd_ac[0];
+			pAd->CommonCfg.bAPSDAC_BK = apsd_ac[1];
+			pAd->CommonCfg.bAPSDAC_VI = apsd_ac[2];
+			pAd->CommonCfg.bAPSDAC_VO = apsd_ac[3];
+		}
+	}
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+NDIS_STATUS	RTMPReadParametersHook(
+	IN	PRTMP_ADAPTER pAd)
+{
+	PUCHAR					src = NULL;
+	struct file				*srcf;
+	INT 					retval, orgfsuid, orgfsgid;
+   	mm_segment_t			orgfs;
+	CHAR					*buffer;
+	CHAR					*tmpbuf;
+	ULONG					RtsThresh;
+	ULONG					FragThresh;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR	                keyMaterial[40];
+#endif // CONFIG_STA_SUPPORT //
+
+
+	PUCHAR					macptr;
+	INT						i = 0;
+
+	buffer = kmalloc(MAX_INI_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if(buffer == NULL)
+        return NDIS_STATUS_FAILURE;
+
+	tmpbuf = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if(tmpbuf == NULL)
+	{
+		kfree(buffer);
+        return NDIS_STATUS_FAILURE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		src = STA_PROFILE_PATH;
+#endif // CONFIG_STA_SUPPORT //
+#ifdef MULTIPLE_CARD_SUPPORT
+	src = pAd->MC_FileName;
+#endif // MULTIPLE_CARD_SUPPORT //
+
+	// Save uid and gid used for filesystem access.
+	// Set user and group to 0 (root)
+	orgfsuid = current_fsuid();
+	orgfsgid = current_fsgid();
+	/* Hm, can't really do this nicely anymore, so rely on these files
+	 * being set to the proper permission to read them... */
+	/* current->cred->fsuid = current->cred->fsgid = 0; */
+    orgfs = get_fs();
+    set_fs(KERNEL_DS);
+
+	if (src && *src)
+	{
+		srcf = filp_open(src, O_RDONLY, 0);
+		if (IS_ERR(srcf))
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("--> Error %ld opening %s\n", -PTR_ERR(srcf),src));
+		}
+		else
+		{
+			// The object must have a read method
+			if (srcf->f_op && srcf->f_op->read)
+			{
+				memset(buffer, 0x00, MAX_INI_BUFFER_SIZE);
+				retval=srcf->f_op->read(srcf, buffer, MAX_INI_BUFFER_SIZE, &srcf->f_pos);
+				if (retval < 0)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("--> Read %s error %d\n", src, -retval));
+				}
+				else
+				{
+					// set file parameter to portcfg
+					//CountryRegion
+					if(RTMPGetKeyParameter("CountryRegion", tmpbuf, 25, buffer))
+					{
+						pAd->CommonCfg.CountryRegion = (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("CountryRegion=%d\n", pAd->CommonCfg.CountryRegion));
+					}
+					//CountryRegionABand
+					if(RTMPGetKeyParameter("CountryRegionABand", tmpbuf, 25, buffer))
+					{
+						pAd->CommonCfg.CountryRegionForABand= (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("CountryRegionABand=%d\n", pAd->CommonCfg.CountryRegionForABand));
+					}
+					//CountryCode
+					if(RTMPGetKeyParameter("CountryCode", tmpbuf, 25, buffer))
+					{
+						NdisMoveMemory(pAd->CommonCfg.CountryCode, tmpbuf , 2);
+#ifdef CONFIG_STA_SUPPORT
+#ifdef EXT_BUILD_CHANNEL_LIST
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+							NdisMoveMemory(pAd->StaCfg.StaOriCountryCode, tmpbuf , 2);
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+						if (strlen(pAd->CommonCfg.CountryCode) != 0)
+						{
+							pAd->CommonCfg.bCountryFlag = TRUE;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("CountryCode=%s\n", pAd->CommonCfg.CountryCode));
+					}
+					//ChannelGeography
+					if(RTMPGetKeyParameter("ChannelGeography", tmpbuf, 25, buffer))
+					{
+						UCHAR Geography = (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						if (Geography <= BOTH)
+						{
+							pAd->CommonCfg.Geography = Geography;
+							pAd->CommonCfg.CountryCode[2] =
+								(pAd->CommonCfg.Geography == BOTH) ? ' ' : ((pAd->CommonCfg.Geography == IDOR) ? 'I' : 'O');
+#ifdef CONFIG_STA_SUPPORT
+#ifdef EXT_BUILD_CHANNEL_LIST
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+							pAd->StaCfg.StaOriGeography = pAd->CommonCfg.Geography;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+							DBGPRINT(RT_DEBUG_TRACE, ("ChannelGeography=%d\n", pAd->CommonCfg.Geography));
+						}
+					}
+					else
+					{
+						pAd->CommonCfg.Geography = BOTH;
+						pAd->CommonCfg.CountryCode[2] = ' ';
+					}
+
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						//SSID
+						if (RTMPGetCriticalParameter("SSID", tmpbuf, 256, buffer))
+						{
+							if (strlen(tmpbuf) <= 32)
+							{
+			 					pAd->CommonCfg.SsidLen = (UCHAR) strlen(tmpbuf);
+								NdisZeroMemory(pAd->CommonCfg.Ssid, NDIS_802_11_LENGTH_SSID);
+								NdisMoveMemory(pAd->CommonCfg.Ssid, tmpbuf, pAd->CommonCfg.SsidLen);
+								pAd->MlmeAux.AutoReconnectSsidLen = pAd->CommonCfg.SsidLen;
+								NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, NDIS_802_11_LENGTH_SSID);
+								NdisMoveMemory(pAd->MlmeAux.AutoReconnectSsid, tmpbuf, pAd->MlmeAux.AutoReconnectSsidLen);
+								pAd->MlmeAux.SsidLen = pAd->CommonCfg.SsidLen;
+								NdisZeroMemory(pAd->MlmeAux.Ssid, NDIS_802_11_LENGTH_SSID);
+								NdisMoveMemory(pAd->MlmeAux.Ssid, tmpbuf, pAd->MlmeAux.SsidLen);
+								DBGPRINT(RT_DEBUG_TRACE, ("%s::(SSID=%s)\n", __func__, tmpbuf));
+							}
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						//NetworkType
+						if (RTMPGetKeyParameter("NetworkType", tmpbuf, 25, buffer))
+						{
+							pAd->bConfigChanged = TRUE;
+							if (strcmp(tmpbuf, "Adhoc") == 0)
+								pAd->StaCfg.BssType = BSS_ADHOC;
+							else //Default Infrastructure mode
+								pAd->StaCfg.BssType = BSS_INFRA;
+							// Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+							pAd->StaCfg.WpaState = SS_NOTUSE;
+							DBGPRINT(RT_DEBUG_TRACE, ("%s::(NetworkType=%d)\n", __func__, pAd->StaCfg.BssType));
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+					//Channel
+					if(RTMPGetKeyParameter("Channel", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.Channel = (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("Channel=%d\n", pAd->CommonCfg.Channel));
+					}
+					//WirelessMode
+					if(RTMPGetKeyParameter("WirelessMode", tmpbuf, 10, buffer))
+					{
+						int value  = 0, maxPhyMode = PHY_11G;
+
+#ifdef DOT11_N_SUPPORT
+						maxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+						value = simple_strtol(tmpbuf, 0, 10);
+
+						if (value <= maxPhyMode)
+						{
+							pAd->CommonCfg.PhyMode = value;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("PhyMode=%d\n", pAd->CommonCfg.PhyMode));
+					}
+                    //BasicRate
+					if(RTMPGetKeyParameter("BasicRate", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.BasicRateBitmap = (ULONG) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("BasicRate=%ld\n", pAd->CommonCfg.BasicRateBitmap));
+					}
+					//BeaconPeriod
+					if(RTMPGetKeyParameter("BeaconPeriod", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.BeaconPeriod = (USHORT) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("BeaconPeriod=%d\n", pAd->CommonCfg.BeaconPeriod));
+					}
+                    //TxPower
+					if(RTMPGetKeyParameter("TxPower", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.TxPowerPercentage = (ULONG) simple_strtol(tmpbuf, 0, 10);
+#ifdef CONFIG_STA_SUPPORT
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+							pAd->CommonCfg.TxPowerDefault = pAd->CommonCfg.TxPowerPercentage;
+#endif // CONFIG_STA_SUPPORT //
+						DBGPRINT(RT_DEBUG_TRACE, ("TxPower=%ld\n", pAd->CommonCfg.TxPowerPercentage));
+					}
+					//BGProtection
+					if(RTMPGetKeyParameter("BGProtection", tmpbuf, 10, buffer))
+					{
+						switch (simple_strtol(tmpbuf, 0, 10))
+						{
+							case 1: //Always On
+								pAd->CommonCfg.UseBGProtection = 1;
+								break;
+							case 2: //Always OFF
+								pAd->CommonCfg.UseBGProtection = 2;
+								break;
+							case 0: //AUTO
+							default:
+								pAd->CommonCfg.UseBGProtection = 0;
+								break;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("BGProtection=%ld\n", pAd->CommonCfg.UseBGProtection));
+					}
+					//OLBCDetection
+					if(RTMPGetKeyParameter("DisableOLBC", tmpbuf, 10, buffer))
+					{
+						switch (simple_strtol(tmpbuf, 0, 10))
+						{
+							case 1: //disable OLBC Detection
+								pAd->CommonCfg.DisableOLBCDetect = 1;
+								break;
+							case 0: //enable OLBC Detection
+								pAd->CommonCfg.DisableOLBCDetect = 0;
+								break;
+							default:
+								pAd->CommonCfg.DisableOLBCDetect= 0;
+								break;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("OLBCDetection=%ld\n", pAd->CommonCfg.DisableOLBCDetect));
+					}
+					//TxPreamble
+					if(RTMPGetKeyParameter("TxPreamble", tmpbuf, 10, buffer))
+					{
+						switch (simple_strtol(tmpbuf, 0, 10))
+						{
+							case Rt802_11PreambleShort:
+								pAd->CommonCfg.TxPreamble = Rt802_11PreambleShort;
+								break;
+							case Rt802_11PreambleLong:
+							default:
+								pAd->CommonCfg.TxPreamble = Rt802_11PreambleLong;
+								break;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("TxPreamble=%ld\n", pAd->CommonCfg.TxPreamble));
+					}
+					//RTSThreshold
+					if(RTMPGetKeyParameter("RTSThreshold", tmpbuf, 10, buffer))
+					{
+						RtsThresh = simple_strtol(tmpbuf, 0, 10);
+						if( (RtsThresh >= 1) && (RtsThresh <= MAX_RTS_THRESHOLD) )
+							pAd->CommonCfg.RtsThreshold  = (USHORT)RtsThresh;
+						else
+							pAd->CommonCfg.RtsThreshold = MAX_RTS_THRESHOLD;
+
+						DBGPRINT(RT_DEBUG_TRACE, ("RTSThreshold=%d\n", pAd->CommonCfg.RtsThreshold));
+					}
+					//FragThreshold
+					if(RTMPGetKeyParameter("FragThreshold", tmpbuf, 10, buffer))
+					{
+						FragThresh = simple_strtol(tmpbuf, 0, 10);
+						pAd->CommonCfg.bUseZeroToDisableFragment = FALSE;
+
+						if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+						{ //illegal FragThresh so we set it to default
+							pAd->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+							pAd->CommonCfg.bUseZeroToDisableFragment = TRUE;
+						}
+						else if (FragThresh % 2 == 1)
+						{
+							// The length of each fragment shall always be an even number of octets, except for the last fragment
+							// of an MSDU or MMPDU, which may be either an even or an odd number of octets.
+							pAd->CommonCfg.FragmentThreshold = (USHORT)(FragThresh - 1);
+						}
+						else
+						{
+							pAd->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+						}
+						//pAd->CommonCfg.AllowFragSize = (pAd->CommonCfg.FragmentThreshold) - LENGTH_802_11 - LENGTH_CRC;
+						DBGPRINT(RT_DEBUG_TRACE, ("FragThreshold=%d\n", pAd->CommonCfg.FragmentThreshold));
+					}
+					//TxBurst
+					if(RTMPGetKeyParameter("TxBurst", tmpbuf, 10, buffer))
+					{
+						if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+							pAd->CommonCfg.bEnableTxBurst = TRUE;
+						else //Disable
+							pAd->CommonCfg.bEnableTxBurst = FALSE;
+						DBGPRINT(RT_DEBUG_TRACE, ("TxBurst=%d\n", pAd->CommonCfg.bEnableTxBurst));
+					}
+
+#ifdef AGGREGATION_SUPPORT
+					//PktAggregate
+					if(RTMPGetKeyParameter("PktAggregate", tmpbuf, 10, buffer))
+					{
+						if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+							pAd->CommonCfg.bAggregationCapable = TRUE;
+						else //Disable
+							pAd->CommonCfg.bAggregationCapable = FALSE;
+#ifdef PIGGYBACK_SUPPORT
+						pAd->CommonCfg.bPiggyBackCapable = pAd->CommonCfg.bAggregationCapable;
+#endif // PIGGYBACK_SUPPORT //
+						DBGPRINT(RT_DEBUG_TRACE, ("PktAggregate=%d\n", pAd->CommonCfg.bAggregationCapable));
+					}
+#else
+					pAd->CommonCfg.bAggregationCapable = FALSE;
+					pAd->CommonCfg.bPiggyBackCapable = FALSE;
+#endif // AGGREGATION_SUPPORT //
+
+					// WmmCapable
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						rtmp_read_sta_wmm_parms_from_file(pAd, tmpbuf, buffer);
+#endif // CONFIG_STA_SUPPORT //
+
+					//ShortSlot
+					if(RTMPGetKeyParameter("ShortSlot", tmpbuf, 10, buffer))
+					{
+						if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+							pAd->CommonCfg.bUseShortSlotTime = TRUE;
+						else //Disable
+							pAd->CommonCfg.bUseShortSlotTime = FALSE;
+
+						DBGPRINT(RT_DEBUG_TRACE, ("ShortSlot=%d\n", pAd->CommonCfg.bUseShortSlotTime));
+					}
+					//IEEE80211H
+					if(RTMPGetKeyParameter("IEEE80211H", tmpbuf, 10, buffer))
+					{
+					    for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+					    {
+    						if(simple_strtol(macptr, 0, 10) != 0)  //Enable
+    							pAd->CommonCfg.bIEEE80211H = TRUE;
+    						else //Disable
+    							pAd->CommonCfg.bIEEE80211H = FALSE;
+
+    						DBGPRINT(RT_DEBUG_TRACE, ("IEEE80211H=%d\n", pAd->CommonCfg.bIEEE80211H));
+					    }
+					}
+					//CSPeriod
+					if(RTMPGetKeyParameter("CSPeriod", tmpbuf, 10, buffer))
+					{
+					    if(simple_strtol(tmpbuf, 0, 10) != 0)
+							pAd->CommonCfg.RadarDetect.CSPeriod = simple_strtol(tmpbuf, 0, 10);
+						else
+							pAd->CommonCfg.RadarDetect.CSPeriod = 0;
+
+   						DBGPRINT(RT_DEBUG_TRACE, ("CSPeriod=%d\n", pAd->CommonCfg.RadarDetect.CSPeriod));
+					}
+
+					//RDRegion
+					if(RTMPGetKeyParameter("RDRegion", tmpbuf, 128, buffer))
+					{
+						if ((strncmp(tmpbuf, "JAP_W53", 7) == 0) || (strncmp(tmpbuf, "jap_w53", 7) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = JAP_W53;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 15;
+						}
+						else if ((strncmp(tmpbuf, "JAP_W56", 7) == 0) || (strncmp(tmpbuf, "jap_w56", 7) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = JAP_W56;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+						}
+						else if ((strncmp(tmpbuf, "JAP", 3) == 0) || (strncmp(tmpbuf, "jap", 3) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = JAP;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 5;
+						}
+						else  if ((strncmp(tmpbuf, "FCC", 3) == 0) || (strncmp(tmpbuf, "fcc", 3) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = FCC;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 5;
+						}
+						else if ((strncmp(tmpbuf, "CE", 2) == 0) || (strncmp(tmpbuf, "ce", 2) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = CE;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+						}
+						else
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = CE;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+						}
+
+						DBGPRINT(RT_DEBUG_TRACE, ("RDRegion=%d\n", pAd->CommonCfg.RadarDetect.RDDurRegion));
+					}
+					else
+					{
+						pAd->CommonCfg.RadarDetect.RDDurRegion = CE;
+						pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+					}
+
+					//WirelessEvent
+					if(RTMPGetKeyParameter("WirelessEvent", tmpbuf, 10, buffer))
+					{
+#if WIRELESS_EXT >= 15
+					    if(simple_strtol(tmpbuf, 0, 10) != 0)
+							pAd->CommonCfg.bWirelessEvent = simple_strtol(tmpbuf, 0, 10);
+						else
+							pAd->CommonCfg.bWirelessEvent = 0;	// disable
+#else
+						pAd->CommonCfg.bWirelessEvent = 0;	// disable
+#endif
+   						DBGPRINT(RT_DEBUG_TRACE, ("WirelessEvent=%d\n", pAd->CommonCfg.bWirelessEvent));
+					}
+					if(RTMPGetKeyParameter("WiFiTest", tmpbuf, 10, buffer))
+					{
+					    if(simple_strtol(tmpbuf, 0, 10) != 0)
+							pAd->CommonCfg.bWiFiTest= simple_strtol(tmpbuf, 0, 10);
+						else
+							pAd->CommonCfg.bWiFiTest = 0;	// disable
+
+   						DBGPRINT(RT_DEBUG_TRACE, ("WiFiTest=%d\n", pAd->CommonCfg.bWiFiTest));
+					}
+					//AuthMode
+					if(RTMPGetKeyParameter("AuthMode", tmpbuf, 128, buffer))
+					{
+#ifdef CONFIG_STA_SUPPORT
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						{
+							if ((strcmp(tmpbuf, "WEPAUTO") == 0) || (strcmp(tmpbuf, "wepauto") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeAutoSwitch;
+							else if ((strcmp(tmpbuf, "SHARED") == 0) || (strcmp(tmpbuf, "shared") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+							else if ((strcmp(tmpbuf, "WPAPSK") == 0) || (strcmp(tmpbuf, "wpapsk") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+							else if ((strcmp(tmpbuf, "WPANONE") == 0) || (strcmp(tmpbuf, "wpanone") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+							else if ((strcmp(tmpbuf, "WPA2PSK") == 0) || (strcmp(tmpbuf, "wpa2psk") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+#ifdef WPA_SUPPLICANT_SUPPORT
+							else if ((strcmp(tmpbuf, "WPA") == 0) || (strcmp(tmpbuf, "wpa") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+							else if ((strcmp(tmpbuf, "WPA2") == 0) || (strcmp(tmpbuf, "wpa2") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#endif // WPA_SUPPLICANT_SUPPORT //
+				                        else
+				                            pAd->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+
+				                        pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("%s::(EncrypType=%d)\n", __func__, pAd->StaCfg.WepStatus));
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+					//EncrypType
+					if(RTMPGetKeyParameter("EncrypType", tmpbuf, 128, buffer))
+					{
+
+#ifdef CONFIG_STA_SUPPORT
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						{
+							if ((strcmp(tmpbuf, "WEP") == 0) || (strcmp(tmpbuf, "wep") == 0))
+								pAd->StaCfg.WepStatus	= Ndis802_11WEPEnabled;
+							else if ((strcmp(tmpbuf, "TKIP") == 0) || (strcmp(tmpbuf, "tkip") == 0))
+								pAd->StaCfg.WepStatus	= Ndis802_11Encryption2Enabled;
+							else if ((strcmp(tmpbuf, "AES") == 0) || (strcmp(tmpbuf, "aes") == 0))
+								pAd->StaCfg.WepStatus	= Ndis802_11Encryption3Enabled;
+							else
+								pAd->StaCfg.WepStatus	= Ndis802_11WEPDisabled;
+
+							// Update all wepstatus related
+							pAd->StaCfg.PairCipher		= pAd->StaCfg.WepStatus;
+							pAd->StaCfg.GroupCipher 	= pAd->StaCfg.WepStatus;
+							pAd->StaCfg.OrigWepStatus 	= pAd->StaCfg.WepStatus;
+							pAd->StaCfg.bMixCipher 		= FALSE;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("%s::(EncrypType=%d)\n", __func__, pAd->StaCfg.WepStatus));
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						if(RTMPGetCriticalParameter("WPAPSK", tmpbuf, 512, buffer))
+						{
+							int     err=0;
+
+							tmpbuf[strlen(tmpbuf)] = '\0'; // make STA can process .$^& for WPAPSK input
+
+							if ((pAd->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+								(pAd->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+								(pAd->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+								)
+							{
+								err = 1;
+							}
+							else if ((strlen(tmpbuf) >= 8) && (strlen(tmpbuf) < 64))
+							{
+								PasswordHash((char *)tmpbuf, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen, keyMaterial);
+								NdisMoveMemory(pAd->StaCfg.PMK, keyMaterial, 32);
+
+							}
+							else if (strlen(tmpbuf) == 64)
+							{
+								AtoH(tmpbuf, keyMaterial, 32);
+								NdisMoveMemory(pAd->StaCfg.PMK, keyMaterial, 32);
+							}
+							else
+							{
+								err = 1;
+								DBGPRINT(RT_DEBUG_ERROR, ("%s::(WPAPSK key-string required 8 ~ 64 characters!)\n", __func__));
+							}
+
+							if (err == 0)
+	                        			{
+	                        				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+									(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+								{
+									// Start STA supplicant state machine
+									pAd->StaCfg.WpaState = SS_START;
+								}
+								else if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+								{
+									pAd->StaCfg.WpaState = SS_NOTUSE;
+								}
+
+								DBGPRINT(RT_DEBUG_TRACE, ("%s::(WPAPSK=%s)\n", __func__, tmpbuf));
+							}
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+
+					//DefaultKeyID, KeyType, KeyStr
+					rtmp_read_key_parms_from_file(pAd, tmpbuf, buffer);
+
+#ifdef DOT11_N_SUPPORT
+					HTParametersHook(pAd, tmpbuf, buffer);
+#endif // DOT11_N_SUPPORT //
+
+
+#ifdef CARRIER_DETECTION_SUPPORT
+						//CarrierDetect
+						if(RTMPGetKeyParameter("CarrierDetect", tmpbuf, 128, buffer))
+						{
+							if ((strncmp(tmpbuf, "0", 1) == 0))
+								pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+							else if ((strncmp(tmpbuf, "1", 1) == 0))
+								pAd->CommonCfg.CarrierDetect.Enable = TRUE;
+							else
+								pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("CarrierDetect.Enable=%d\n", pAd->CommonCfg.CarrierDetect.Enable));
+						}
+						else
+							pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						//PSMode
+						if (RTMPGetKeyParameter("PSMode", tmpbuf, 10, buffer))
+						{
+							if (pAd->StaCfg.BssType == BSS_INFRA)
+							{
+								if ((strcmp(tmpbuf, "MAX_PSP") == 0) || (strcmp(tmpbuf, "max_psp") == 0))
+								{
+									// do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+									// to exclude certain situations.
+									//	   MlmeSetPsm(pAd, PWR_SAVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeMAX_PSP;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeMAX_PSP;
+									pAd->StaCfg.DefaultListenCount = 5;
+								}
+								else if ((strcmp(tmpbuf, "Fast_PSP") == 0) || (strcmp(tmpbuf, "fast_psp") == 0)
+									|| (strcmp(tmpbuf, "FAST_PSP") == 0))
+								{
+									// do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+									// to exclude certain situations.
+									//	   MlmeSetPsmBit(pAd, PWR_SAVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeFast_PSP;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeFast_PSP;
+									pAd->StaCfg.DefaultListenCount = 3;
+								}
+								else if ((strcmp(tmpbuf, "Legacy_PSP") == 0) || (strcmp(tmpbuf, "legacy_psp") == 0)
+									|| (strcmp(tmpbuf, "LEGACY_PSP") == 0))
+								{
+									// do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+									// to exclude certain situations.
+									//	   MlmeSetPsmBit(pAd, PWR_SAVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeLegacy_PSP;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeLegacy_PSP;
+									pAd->StaCfg.DefaultListenCount = 3;
+								}
+								else
+								{ //Default Ndis802_11PowerModeCAM
+									// clear PSM bit immediately
+									MlmeSetPsmBit(pAd, PWR_ACTIVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+								}
+								DBGPRINT(RT_DEBUG_TRACE, ("PSMode=%ld\n", pAd->StaCfg.WindowsPowerMode));
+							}
+						}
+						// FastRoaming
+						if (RTMPGetKeyParameter("FastRoaming", tmpbuf, 32, buffer))
+						{
+							if (simple_strtol(tmpbuf, 0, 10) == 0)
+								pAd->StaCfg.bFastRoaming = FALSE;
+							else
+								pAd->StaCfg.bFastRoaming = TRUE;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("FastRoaming=%d\n", pAd->StaCfg.bFastRoaming));
+						}
+						// RoamThreshold
+						if (RTMPGetKeyParameter("RoamThreshold", tmpbuf, 32, buffer))
+						{
+							long lInfo = simple_strtol(tmpbuf, 0, 10);
+
+							if (lInfo > 90 || lInfo < 60)
+								pAd->StaCfg.dBmToRoam = -70;
+							else
+								pAd->StaCfg.dBmToRoam = (CHAR)(-1)*lInfo;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("RoamThreshold=%d  dBm\n", pAd->StaCfg.dBmToRoam));
+						}
+
+						if(RTMPGetKeyParameter("TGnWifiTest", tmpbuf, 10, buffer))
+						{
+							if(simple_strtol(tmpbuf, 0, 10) == 0)
+								pAd->StaCfg.bTGnWifiTest = FALSE;
+							else
+								pAd->StaCfg.bTGnWifiTest = TRUE;
+								DBGPRINT(RT_DEBUG_TRACE, ("TGnWifiTest=%d\n", pAd->StaCfg.bTGnWifiTest));
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+
+				}
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("--> %s does not have a write method\n", src));
+			}
+
+			retval=filp_close(srcf,NULL);
+
+			if (retval)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("--> Error %d closing %s\n", -retval, src));
+			}
+		}
+	}
+
+	set_fs(orgfs);
+
+#if 0
+	current->cred->fsuid = orgfsuid;
+	current->cred->fsgid = orgfsgid;
+#endif
+
+	kfree(buffer);
+	kfree(tmpbuf);
+
+	return (NDIS_STATUS_SUCCESS);
+}
+
+#ifdef DOT11_N_SUPPORT
+static void	HTParametersHook(
+	IN	PRTMP_ADAPTER pAd,
+	IN	CHAR		  *pValueStr,
+	IN	CHAR		  *pInput)
+{
+
+	INT Value;
+
+    if (RTMPGetKeyParameter("HT_PROTECT", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bHTProtect = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bHTProtect = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: Protection  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+    if (RTMPGetKeyParameter("HT_MIMOPSEnable", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bMIMOPSEnable = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bMIMOPSEnable = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: MIMOPSEnable  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+
+    if (RTMPGetKeyParameter("HT_MIMOPSMode", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value > MMPS_ENABLE)
+        {
+			pAd->CommonCfg.BACapability.field.MMPSmode = MMPS_ENABLE;
+        }
+        else
+        {
+            //TODO: add mimo power saving mechanism
+            pAd->CommonCfg.BACapability.field.MMPSmode = MMPS_ENABLE;
+			//pAd->CommonCfg.BACapability.field.MMPSmode = Value;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: MIMOPS Mode  = %d\n", Value));
+    }
+
+    if (RTMPGetKeyParameter("HT_BADecline", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bBADecline = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bBADecline = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: BA Decline  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+
+    if (RTMPGetKeyParameter("HT_DisableReordering", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bDisableReordering = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bDisableReordering = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: DisableReordering  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+    if (RTMPGetKeyParameter("HT_AutoBA", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.BACapability.field.AutoBA = TRUE;
+        }
+        pAd->CommonCfg.REGBACapability.field.AutoBA = pAd->CommonCfg.BACapability.field.AutoBA;
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: Auto BA  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+	// Tx_+HTC frame
+    if (RTMPGetKeyParameter("HT_HTC", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->HTCEnable = FALSE;
+		}
+		else
+		{
+            		pAd->HTCEnable = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Tx +HTC frame = %s\n", (Value==0) ? "Disable" : "Enable"));
+	}
+
+	// Enable HT Link Adaptation Control
+	if (RTMPGetKeyParameter("HT_LinkAdapt", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->bLinkAdapt = FALSE;
+		}
+		else
+		{
+			pAd->HTCEnable = TRUE;
+			pAd->bLinkAdapt = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Link Adaptation Control = %s\n", (Value==0) ? "Disable" : "Enable(+HTC)"));
+	}
+
+	// Reverse Direction Mechanism
+    if (RTMPGetKeyParameter("HT_RDG", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->CommonCfg.bRdg = FALSE;
+		}
+		else
+		{
+			pAd->HTCEnable = TRUE;
+            pAd->CommonCfg.bRdg = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: RDG = %s\n", (Value==0) ? "Disable" : "Enable(+HTC)"));
+	}
+
+
+
+
+	// Tx A-MSUD ?
+    if (RTMPGetKeyParameter("HT_AMSDU", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->CommonCfg.BACapability.field.AmsduEnable = FALSE;
+		}
+		else
+		{
+            pAd->CommonCfg.BACapability.field.AmsduEnable = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Tx A-MSDU = %s\n", (Value==0) ? "Disable" : "Enable"));
+	}
+
+	// MPDU Density
+    if (RTMPGetKeyParameter("HT_MpduDensity", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value <=7 && Value >= 0)
+		{
+			pAd->CommonCfg.BACapability.field.MpduDensity = Value;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: MPDU Density = %d\n", Value));
+		}
+		else
+		{
+			pAd->CommonCfg.BACapability.field.MpduDensity = 4;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: MPDU Density = %d (Default)\n", 4));
+		}
+	}
+
+	// Max Rx BA Window Size
+    if (RTMPGetKeyParameter("HT_BAWinSize", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value >=1 && Value <= 64)
+		{
+			pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = Value;
+			pAd->CommonCfg.BACapability.field.RxBAWinLimit = Value;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: BA Windw Size = %d\n", Value));
+		}
+		else
+		{
+            pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = 64;
+			pAd->CommonCfg.BACapability.field.RxBAWinLimit = 64;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: BA Windw Size = 64 (Defualt)\n"));
+		}
+
+	}
+
+	// Guard Interval
+	if (RTMPGetKeyParameter("HT_GI", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == GI_400)
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_400;
+		}
+		else
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_800;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Guard Interval = %s\n", (Value==GI_400) ? "400" : "800" ));
+	}
+
+	// HT Operation Mode : Mixed Mode , Green Field
+	if (RTMPGetKeyParameter("HT_OpMode", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == HTMODE_GF)
+		{
+
+			pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_GF;
+		}
+		else
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_MM;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Operate Mode = %s\n", (Value==HTMODE_GF) ? "Green Field" : "Mixed Mode" ));
+	}
+
+	// Fixed Tx mode : CCK, OFDM
+	if (RTMPGetKeyParameter("FixedTxMode", pValueStr, 25, pInput))
+	{
+		UCHAR	fix_tx_mode;
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			fix_tx_mode = FIXED_TXMODE_HT;
+
+			if (strcmp(pValueStr, "OFDM") == 0 || strcmp(pValueStr, "ofdm") == 0)
+			{
+				fix_tx_mode = FIXED_TXMODE_OFDM;
+			}
+			else if (strcmp(pValueStr, "CCK") == 0 || strcmp(pValueStr, "cck") == 0)
+			{
+		        fix_tx_mode = FIXED_TXMODE_CCK;
+			}
+			else if (strcmp(pValueStr, "HT") == 0 || strcmp(pValueStr, "ht") == 0)
+			{
+		        fix_tx_mode = FIXED_TXMODE_HT;
+		}
+		else
+		{
+				Value = simple_strtol(pValueStr, 0, 10);
+				// 1 : CCK
+				// 2 : OFDM
+				// otherwise : HT
+				if (Value == FIXED_TXMODE_CCK || Value == FIXED_TXMODE_OFDM)
+					fix_tx_mode = Value;
+				else
+					fix_tx_mode = FIXED_TXMODE_HT;
+		}
+
+			pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode = fix_tx_mode;
+			DBGPRINT(RT_DEBUG_TRACE, ("Fixed Tx Mode = %d\n", fix_tx_mode));
+
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+	// Channel Width
+	if (RTMPGetKeyParameter("HT_BW", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == BW_40)
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_40;
+		}
+		else
+		{
+            pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+		}
+
+#ifdef MCAST_RATE_SPECIFIC
+		pAd->CommonCfg.MCastPhyMode.field.BW = pAd->CommonCfg.RegTransmitSetting.field.BW;
+#endif // MCAST_RATE_SPECIFIC //
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Channel Width = %s\n", (Value==BW_40) ? "40 MHz" : "20 MHz" ));
+	}
+
+	if (RTMPGetKeyParameter("HT_EXTCHA", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == 0)
+		{
+
+			pAd->CommonCfg.RegTransmitSetting.field.EXTCHA  = EXTCHA_BELOW;
+		}
+		else
+		{
+            pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Ext Channel = %s\n", (Value==0) ? "BELOW" : "ABOVE" ));
+	}
+
+	// MSC
+	if (RTMPGetKeyParameter("HT_MCS", pValueStr, 50, pInput))
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			Value = simple_strtol(pValueStr, 0, 10);
+
+			if ((Value >= 0 && Value <= 23) || (Value == 32)) // 3*3
+		{
+				pAd->StaCfg.DesiredTransmitSetting.field.MCS  = Value;
+				pAd->StaCfg.bAutoTxRateSwitch = FALSE;
+				DBGPRINT(RT_DEBUG_TRACE, ("HT: MCS = %d\n", pAd->StaCfg.DesiredTransmitSetting.field.MCS));
+		}
+		else
+		{
+				pAd->StaCfg.DesiredTransmitSetting.field.MCS  = MCS_AUTO;
+				pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+				DBGPRINT(RT_DEBUG_TRACE, ("HT: MCS = AUTO\n"));
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// STBC
+    if (RTMPGetKeyParameter("HT_STBC", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == STBC_USE)
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_USE;
+		}
+		else
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_NONE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: STBC = %d\n", pAd->CommonCfg.RegTransmitSetting.field.STBC));
+	}
+
+	// 40_Mhz_Intolerant
+	if (RTMPGetKeyParameter("HT_40MHZ_INTOLERANT", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->CommonCfg.bForty_Mhz_Intolerant = FALSE;
+		}
+		else
+		{
+			pAd->CommonCfg.bForty_Mhz_Intolerant = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: 40MHZ INTOLERANT = %d\n", pAd->CommonCfg.bForty_Mhz_Intolerant));
+	}
+	//HT_TxStream
+	if(RTMPGetKeyParameter("HT_TxStream", pValueStr, 10, pInput))
+	{
+		switch (simple_strtol(pValueStr, 0, 10))
+		{
+			case 1:
+				pAd->CommonCfg.TxStream = 1;
+				break;
+			case 2:
+				pAd->CommonCfg.TxStream = 2;
+				break;
+			case 3: // 3*3
+			default:
+				pAd->CommonCfg.TxStream = 3;
+
+				if (pAd->MACVersion < RALINK_2883_VERSION)
+					pAd->CommonCfg.TxStream = 2; // only 2 tx streams for RT2860 series
+				break;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Tx Stream = %d\n", pAd->CommonCfg.TxStream));
+	}
+	//HT_RxStream
+	if(RTMPGetKeyParameter("HT_RxStream", pValueStr, 10, pInput))
+	{
+		switch (simple_strtol(pValueStr, 0, 10))
+		{
+			case 1:
+				pAd->CommonCfg.RxStream = 1;
+				break;
+			case 2:
+				pAd->CommonCfg.RxStream = 2;
+				break;
+			case 3:
+			default:
+				pAd->CommonCfg.RxStream = 3;
+
+				if (pAd->MACVersion < RALINK_2883_VERSION)
+					pAd->CommonCfg.RxStream = 2; // only 2 rx streams for RT2860 series
+				break;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Rx Stream = %d\n", pAd->CommonCfg.RxStream));
+	}
+
+}
+#endif // DOT11_N_SUPPORT //
+
diff --git a/drivers/staging/rt2860/rtmp.h b/drivers/staging/rt2860/rtmp.h
new file mode 100644
index 000000000000..411954206c28
--- /dev/null
+++ b/drivers/staging/rt2860/rtmp.h
@@ -0,0 +1,7177 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rtmp.h
+
+    Abstract:
+    Miniport generic portion header file
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Paul Lin    2002-08-01    created
+    James Tan   2002-09-06    modified (Revise NTCRegTable)
+    John Chang  2004-09-06    modified for RT2600
+*/
+#ifndef __RTMP_H__
+#define __RTMP_H__
+
+#include "link_list.h"
+#include "spectrum_def.h"
+
+
+#ifdef CONFIG_STA_SUPPORT
+#include "aironet.h"
+#endif // CONFIG_STA_SUPPORT //
+
+//#define DBG_DIAGNOSE		1
+
+#if defined(CONFIG_AP_SUPPORT) && defined(CONFIG_STA_SUPPORT)
+#define IF_DEV_CONFIG_OPMODE_ON_AP(_pAd)	if(_pAd->OpMode == OPMODE_AP)
+#define IF_DEV_CONFIG_OPMODE_ON_STA(_pAd)	if(_pAd->OpMode == OPMODE_STA)
+#else
+#define IF_DEV_CONFIG_OPMODE_ON_AP(_pAd)
+#define IF_DEV_CONFIG_OPMODE_ON_STA(_pAd)
+#endif
+
+#define VIRTUAL_IF_INC(__pAd) ((__pAd)->VirtualIfCnt++)
+#define VIRTUAL_IF_DEC(__pAd) ((__pAd)->VirtualIfCnt--)
+#define VIRTUAL_IF_NUM(__pAd) ((__pAd)->VirtualIfCnt)
+
+
+
+//
+//  NDIS Version definitions
+//
+#ifdef  NDIS50_MINIPORT
+#define RTMP_NDIS_MAJOR_VERSION     5
+#define RTMP_NDIS_MINOR_VERSION     0
+#endif
+
+#ifdef  NDIS51_MINIPORT
+#define RTMP_NDIS_MAJOR_VERSION     5
+#define RTMP_NDIS_MINOR_VERSION     1
+#endif
+
+extern  char    NIC_VENDOR_DESC[];
+extern  int     NIC_VENDOR_DESC_LEN;
+
+extern  unsigned char   SNAP_AIRONET[];
+extern  unsigned char   CipherSuiteCiscoCCKM[];
+extern  unsigned char   CipherSuiteCiscoCCKMLen;
+extern	unsigned char	CipherSuiteCiscoCCKM24[];
+extern	unsigned char	CipherSuiteCiscoCCKM24Len;
+extern  unsigned char   CipherSuiteCCXTkip[];
+extern  unsigned char   CipherSuiteCCXTkipLen;
+extern  unsigned char   CISCO_OUI[];
+extern  UCHAR	BaSizeArray[4];
+
+extern UCHAR BROADCAST_ADDR[MAC_ADDR_LEN];
+extern UCHAR MULTICAST_ADDR[MAC_ADDR_LEN];
+extern UCHAR ZERO_MAC_ADDR[MAC_ADDR_LEN];
+extern ULONG BIT32[32];
+extern UCHAR BIT8[8];
+extern char* CipherName[];
+extern char* MCSToMbps[];
+extern UCHAR	 RxwiMCSToOfdmRate[12];
+extern UCHAR SNAP_802_1H[6];
+extern UCHAR SNAP_BRIDGE_TUNNEL[6];
+extern UCHAR SNAP_AIRONET[8];
+extern UCHAR CKIP_LLC_SNAP[8];
+extern UCHAR EAPOL_LLC_SNAP[8];
+extern UCHAR EAPOL[2];
+extern UCHAR IPX[2];
+extern UCHAR APPLE_TALK[2];
+extern UCHAR RateIdToPlcpSignal[12]; // see IEEE802.11a-1999 p.14
+extern UCHAR	 OfdmRateToRxwiMCS[];
+extern UCHAR OfdmSignalToRateId[16] ;
+extern UCHAR default_cwmin[4];
+extern UCHAR default_cwmax[4];
+extern UCHAR default_sta_aifsn[4];
+extern UCHAR MapUserPriorityToAccessCategory[8];
+
+extern USHORT RateUpPER[];
+extern USHORT RateDownPER[];
+extern UCHAR  Phy11BNextRateDownward[];
+extern UCHAR  Phy11BNextRateUpward[];
+extern UCHAR  Phy11BGNextRateDownward[];
+extern UCHAR  Phy11BGNextRateUpward[];
+extern UCHAR  Phy11ANextRateDownward[];
+extern UCHAR  Phy11ANextRateUpward[];
+extern CHAR   RssiSafeLevelForTxRate[];
+extern UCHAR  RateIdToMbps[];
+extern USHORT RateIdTo500Kbps[];
+
+extern UCHAR  CipherSuiteWpaNoneTkip[];
+extern UCHAR  CipherSuiteWpaNoneTkipLen;
+
+extern UCHAR  CipherSuiteWpaNoneAes[];
+extern UCHAR  CipherSuiteWpaNoneAesLen;
+
+extern UCHAR  SsidIe;
+extern UCHAR  SupRateIe;
+extern UCHAR  ExtRateIe;
+
+#ifdef DOT11_N_SUPPORT
+extern UCHAR  HtCapIe;
+extern UCHAR  AddHtInfoIe;
+extern UCHAR  NewExtChanIe;
+#ifdef DOT11N_DRAFT3
+extern UCHAR  ExtHtCapIe;
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+extern UCHAR  ErpIe;
+extern UCHAR  DsIe;
+extern UCHAR  TimIe;
+extern UCHAR  WpaIe;
+extern UCHAR  Wpa2Ie;
+extern UCHAR  IbssIe;
+extern UCHAR  Ccx2Ie;
+
+extern UCHAR  WPA_OUI[];
+extern UCHAR  RSN_OUI[];
+extern UCHAR  WME_INFO_ELEM[];
+extern UCHAR  WME_PARM_ELEM[];
+extern UCHAR  Ccx2QosInfo[];
+extern UCHAR  Ccx2IeInfo[];
+extern UCHAR  RALINK_OUI[];
+extern UCHAR  PowerConstraintIE[];
+
+
+extern UCHAR  RateSwitchTable[];
+extern UCHAR  RateSwitchTable11B[];
+extern UCHAR  RateSwitchTable11G[];
+extern UCHAR  RateSwitchTable11BG[];
+
+#ifdef DOT11_N_SUPPORT
+extern UCHAR  RateSwitchTable11BGN1S[];
+extern UCHAR  RateSwitchTable11BGN2S[];
+extern UCHAR  RateSwitchTable11BGN2SForABand[];
+extern UCHAR  RateSwitchTable11N1S[];
+extern UCHAR  RateSwitchTable11N2S[];
+extern UCHAR  RateSwitchTable11N2SForABand[];
+
+#ifdef CONFIG_STA_SUPPORT
+extern UCHAR  PRE_N_HT_OUI[];
+#endif // CONFIG_STA_SUPPORT //
+#endif // DOT11_N_SUPPORT //
+
+#define	MAXSEQ		(0xFFF)
+
+#ifdef RALINK_ATE
+typedef	struct _ATE_INFO {
+	UCHAR	Mode;
+	CHAR	TxPower0;
+	CHAR	TxPower1;
+	CHAR    TxAntennaSel;
+	CHAR    RxAntennaSel;
+	TXWI_STRUC  TxWI; 	  // TXWI
+	USHORT	QID;
+	UCHAR	Addr1[MAC_ADDR_LEN];
+	UCHAR	Addr2[MAC_ADDR_LEN];
+	UCHAR	Addr3[MAC_ADDR_LEN];
+	UCHAR	Channel;
+	UINT32	TxLength;
+	UINT32	TxCount;
+	UINT32	TxDoneCount; // Tx DMA Done
+	UINT32	RFFreqOffset;
+	BOOLEAN	bRxFer;
+	BOOLEAN	bQATxStart; // Have compiled QA in and use it to ATE tx.
+	BOOLEAN	bQARxStart;	// Have compiled QA in and use it to ATE rx.
+#ifdef RT2860
+	BOOLEAN	bFWLoading;	// Reload firmware when ATE is done.
+#endif // RT2860 //
+	UINT32	RxTotalCnt;
+	UINT32	RxCntPerSec;
+
+	CHAR	LastSNR0;             // last received SNR
+	CHAR    LastSNR1;             // last received SNR for 2nd  antenna
+	CHAR    LastRssi0;            // last received RSSI
+	CHAR    LastRssi1;            // last received RSSI for 2nd  antenna
+	CHAR    LastRssi2;            // last received RSSI for 3rd  antenna
+	CHAR    AvgRssi0;             // last 8 frames' average RSSI
+	CHAR    AvgRssi1;             // last 8 frames' average RSSI
+	CHAR    AvgRssi2;             // last 8 frames' average RSSI
+	SHORT   AvgRssi0X8;           // sum of last 8 frames' RSSI
+	SHORT   AvgRssi1X8;           // sum of last 8 frames' RSSI
+	SHORT   AvgRssi2X8;           // sum of last 8 frames' RSSI
+
+	UINT32	NumOfAvgRssiSample;
+
+#ifdef RALINK_28xx_QA
+	// Tx frame
+	USHORT		HLen; // Header Length
+	USHORT		PLen; // Pattern Length
+	UCHAR 		Header[32]; // Header buffer
+	UCHAR		Pattern[32]; // Pattern buffer
+	USHORT		DLen; // Data Length
+	USHORT		seq;
+	UINT32		CID;
+	THREAD_PID 		AtePid;
+	// counters
+	UINT32		U2M;
+	UINT32		OtherData;
+	UINT32		Beacon;
+	UINT32		OtherCount;
+	UINT32		TxAc0;
+	UINT32		TxAc1;
+	UINT32		TxAc2;
+	UINT32		TxAc3;
+	UINT32		TxHCCA;
+	UINT32		TxMgmt;
+	UINT32		RSSI0;
+	UINT32		RSSI1;
+	UINT32		RSSI2;
+	UINT32		SNR0;
+	UINT32		SNR1;
+	// control
+	//UINT32		Repeat; // Tx Cpu count
+	UCHAR		TxStatus; // task Tx status // 0 --> task is idle, 1 --> task is running
+#endif // RALINK_28xx_QA //
+}	ATE_INFO, *PATE_INFO;
+
+#ifdef RALINK_28xx_QA
+struct ate_racfghdr {
+ 	UINT32		magic_no;
+	USHORT		command_type;
+	USHORT		command_id;
+	USHORT		length;
+	USHORT		sequence;
+	USHORT		status;
+	UCHAR		data[2046];
+}  __attribute__((packed));
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#ifdef DOT11_N_SUPPORT
+struct reordering_mpdu
+{
+	struct reordering_mpdu	*next;
+	PNDIS_PACKET			pPacket;		/* coverted to 802.3 frame */
+	int						Sequence;		/* sequence number of MPDU */
+	BOOLEAN					bAMSDU;
+};
+
+struct reordering_list
+{
+	struct reordering_mpdu *next;
+	int 	qlen;
+};
+
+struct reordering_mpdu_pool
+{
+	PVOID					mem;
+	NDIS_SPIN_LOCK			lock;
+	struct reordering_list 	freelist;
+};
+#endif // DOT11_N_SUPPORT //
+
+typedef struct 	_RSSI_SAMPLE {
+	CHAR			LastRssi0;             // last received RSSI
+	CHAR			LastRssi1;             // last received RSSI
+	CHAR			LastRssi2;             // last received RSSI
+	CHAR			AvgRssi0;
+	CHAR			AvgRssi1;
+	CHAR			AvgRssi2;
+	SHORT			AvgRssi0X8;
+	SHORT			AvgRssi1X8;
+	SHORT			AvgRssi2X8;
+} RSSI_SAMPLE;
+
+//
+//  Queue structure and macros
+//
+typedef struct  _QUEUE_ENTRY    {
+	struct _QUEUE_ENTRY     *Next;
+}   QUEUE_ENTRY, *PQUEUE_ENTRY;
+
+// Queue structure
+typedef struct  _QUEUE_HEADER   {
+	PQUEUE_ENTRY    Head;
+	PQUEUE_ENTRY    Tail;
+	ULONG           Number;
+}   QUEUE_HEADER, *PQUEUE_HEADER;
+
+#define InitializeQueueHeader(QueueHeader)              \
+{                                                       \
+	(QueueHeader)->Head = (QueueHeader)->Tail = NULL;   \
+	(QueueHeader)->Number = 0;                          \
+}
+
+#define RemoveHeadQueue(QueueHeader)                \
+(QueueHeader)->Head;                                \
+{                                                   \
+	PQUEUE_ENTRY pNext;                             \
+	if ((QueueHeader)->Head != NULL)				\
+	{												\
+		pNext = (QueueHeader)->Head->Next;          \
+		(QueueHeader)->Head = pNext;                \
+		if (pNext == NULL)                          \
+			(QueueHeader)->Tail = NULL;             \
+		(QueueHeader)->Number--;                    \
+	}												\
+}
+
+#define InsertHeadQueue(QueueHeader, QueueEntry)            \
+{                                                           \
+		((PQUEUE_ENTRY)QueueEntry)->Next = (QueueHeader)->Head; \
+		(QueueHeader)->Head = (PQUEUE_ENTRY)(QueueEntry);       \
+		if ((QueueHeader)->Tail == NULL)                        \
+			(QueueHeader)->Tail = (PQUEUE_ENTRY)(QueueEntry);   \
+		(QueueHeader)->Number++;                                \
+}
+
+#define InsertTailQueue(QueueHeader, QueueEntry)                \
+{                                                               \
+	((PQUEUE_ENTRY)QueueEntry)->Next = NULL;                    \
+	if ((QueueHeader)->Tail)                                    \
+		(QueueHeader)->Tail->Next = (PQUEUE_ENTRY)(QueueEntry); \
+	else                                                        \
+		(QueueHeader)->Head = (PQUEUE_ENTRY)(QueueEntry);       \
+	(QueueHeader)->Tail = (PQUEUE_ENTRY)(QueueEntry);           \
+	(QueueHeader)->Number++;                                    \
+}
+
+//
+//  Macros for flag and ref count operations
+//
+#define RTMP_SET_FLAG(_M, _F)       ((_M)->Flags |= (_F))
+#define RTMP_CLEAR_FLAG(_M, _F)     ((_M)->Flags &= ~(_F))
+#define RTMP_CLEAR_FLAGS(_M)        ((_M)->Flags = 0)
+#define RTMP_TEST_FLAG(_M, _F)      (((_M)->Flags & (_F)) != 0)
+#define RTMP_TEST_FLAGS(_M, _F)     (((_M)->Flags & (_F)) == (_F))
+
+#define OPSTATUS_SET_FLAG(_pAd, _F)     ((_pAd)->CommonCfg.OpStatusFlags |= (_F))
+#define OPSTATUS_CLEAR_FLAG(_pAd, _F)   ((_pAd)->CommonCfg.OpStatusFlags &= ~(_F))
+#define OPSTATUS_TEST_FLAG(_pAd, _F)    (((_pAd)->CommonCfg.OpStatusFlags & (_F)) != 0)
+
+#define CLIENT_STATUS_SET_FLAG(_pEntry,_F)      ((_pEntry)->ClientStatusFlags |= (_F))
+#define CLIENT_STATUS_CLEAR_FLAG(_pEntry,_F)    ((_pEntry)->ClientStatusFlags &= ~(_F))
+#define CLIENT_STATUS_TEST_FLAG(_pEntry,_F)     (((_pEntry)->ClientStatusFlags & (_F)) != 0)
+
+#define RX_FILTER_SET_FLAG(_pAd, _F)    ((_pAd)->CommonCfg.PacketFilter |= (_F))
+#define RX_FILTER_CLEAR_FLAG(_pAd, _F)  ((_pAd)->CommonCfg.PacketFilter &= ~(_F))
+#define RX_FILTER_TEST_FLAG(_pAd, _F)   (((_pAd)->CommonCfg.PacketFilter & (_F)) != 0)
+
+#ifdef CONFIG_STA_SUPPORT
+#define STA_NO_SECURITY_ON(_p)          (_p->StaCfg.WepStatus == Ndis802_11EncryptionDisabled)
+#define STA_WEP_ON(_p)                  (_p->StaCfg.WepStatus == Ndis802_11Encryption1Enabled)
+#define STA_TKIP_ON(_p)                 (_p->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+#define STA_AES_ON(_p)                  (_p->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+
+#define STA_TGN_WIFI_ON(_p)             (_p->StaCfg.bTGnWifiTest == TRUE)
+#endif // CONFIG_STA_SUPPORT //
+
+#define CKIP_KP_ON(_p)				((((_p)->StaCfg.CkipFlag) & 0x10) && ((_p)->StaCfg.bCkipCmicOn == TRUE))
+#define CKIP_CMIC_ON(_p)			((((_p)->StaCfg.CkipFlag) & 0x08) && ((_p)->StaCfg.bCkipCmicOn == TRUE))
+
+
+#define INC_RING_INDEX(_idx, _RingSize)    \
+{                                          \
+    (_idx) = (_idx+1) % (_RingSize);       \
+}
+
+#define IS_RT3070(_pAd)				(((_pAd)->MACVersion & 0xffff0000) == 0x30700000)
+
+#define RING_PACKET_INIT(_TxRing, _idx)    \
+{                                          \
+    _TxRing->Cell[_idx].pNdisPacket = NULL;                              \
+    _TxRing->Cell[_idx].pNextNdisPacket = NULL;                              \
+}
+
+#define TXDT_INIT(_TxD)    \
+{                                          \
+	NdisZeroMemory(_TxD, TXD_SIZE);	\
+	_TxD->DMADONE = 1;                              \
+}
+
+//Set last data segment
+#define RING_SET_LASTDS(_TxD, _IsSD0)    \
+{                                          \
+    if (_IsSD0) {_TxD->LastSec0 = 1;}     \
+    else {_TxD->LastSec1 = 1;}     \
+}
+
+// Increase TxTsc value for next transmission
+// TODO:
+// When i==6, means TSC has done one full cycle, do re-keying stuff follow specs
+// Should send a special event microsoft defined to request re-key
+#define INC_TX_TSC(_tsc)                                \
+{                                                       \
+    int i=0;                                            \
+    while (++_tsc[i] == 0x0)                            \
+    {                                                   \
+        i++;                                            \
+        if (i == 6)                                     \
+            break;                                      \
+    }                                                   \
+}
+
+#ifdef DOT11_N_SUPPORT
+// StaActive.SupportedHtPhy.MCSSet is copied from AP beacon.  Don't need to update here.
+#define COPY_HTSETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(_pAd)                                 \
+{                                                                                       \
+	_pAd->StaActive.SupportedHtPhy.ChannelWidth = _pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth;      \
+	_pAd->StaActive.SupportedHtPhy.MimoPs = _pAd->MlmeAux.HtCapability.HtCapInfo.MimoPs;      \
+	_pAd->StaActive.SupportedHtPhy.GF = _pAd->MlmeAux.HtCapability.HtCapInfo.GF;      \
+	_pAd->StaActive.SupportedHtPhy.ShortGIfor20 = _pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor20;      \
+	_pAd->StaActive.SupportedHtPhy.ShortGIfor40 = _pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor40;      \
+	_pAd->StaActive.SupportedHtPhy.TxSTBC = _pAd->MlmeAux.HtCapability.HtCapInfo.TxSTBC;      \
+	_pAd->StaActive.SupportedHtPhy.RxSTBC = _pAd->MlmeAux.HtCapability.HtCapInfo.RxSTBC;      \
+	_pAd->StaActive.SupportedHtPhy.ExtChanOffset = _pAd->MlmeAux.AddHtInfo.AddHtInfo.ExtChanOffset;      \
+	_pAd->StaActive.SupportedHtPhy.RecomWidth = _pAd->MlmeAux.AddHtInfo.AddHtInfo.RecomWidth;      \
+	_pAd->StaActive.SupportedHtPhy.OperaionMode = _pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode;      \
+	_pAd->StaActive.SupportedHtPhy.NonGfPresent = _pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent;      \
+	NdisMoveMemory((_pAd)->MacTab.Content[BSSID_WCID].HTCapability.MCSSet, (_pAd)->StaActive.SupportedPhyInfo.MCSSet, sizeof(UCHAR) * 16);\
+}
+
+#define COPY_AP_HTSETTINGS_FROM_BEACON(_pAd, _pHtCapability)                                 \
+{                                                                                       \
+	_pAd->MacTab.Content[BSSID_WCID].AMsduSize = (UCHAR)(_pHtCapability->HtCapInfo.AMsduSize);	\
+	_pAd->MacTab.Content[BSSID_WCID].MmpsMode= (UCHAR)(_pHtCapability->HtCapInfo.MimoPs);	\
+	_pAd->MacTab.Content[BSSID_WCID].MaxRAmpduFactor = (UCHAR)(_pHtCapability->HtCapParm.MaxRAmpduFactor);	\
+}
+#endif // DOT11_N_SUPPORT //
+
+//
+// MACRO for 32-bit PCI register read / write
+//
+// Usage : RTMP_IO_READ32(
+//              PRTMP_ADAPTER pAd,
+//              ULONG Register_Offset,
+//              PULONG  pValue)
+//
+//         RTMP_IO_WRITE32(
+//              PRTMP_ADAPTER pAd,
+//              ULONG Register_Offset,
+//              ULONG Value)
+//
+
+//
+// BBP & RF are using indirect access. Before write any value into it.
+// We have to make sure there is no outstanding command pending via checking busy bit.
+//
+#define MAX_BUSY_COUNT  100         // Number of retry before failing access BBP & RF indirect register
+//
+#ifdef RT2860
+#define RTMP_RF_IO_WRITE32(_A, _V)                  \
+{                                                   \
+    PHY_CSR4_STRUC  Value;                          \
+    ULONG           BusyCnt = 0;                    \
+    if ((_A)->bPCIclkOff) 	                \
+    {												\
+        return;										\
+    }                                               \
+    do {                                            \
+        RTMP_IO_READ32(_A, RF_CSR_CFG0, &Value.word);  \
+        if (Value.field.Busy == IDLE)               \
+            break;                                  \
+        BusyCnt++;                                  \
+    }   while (BusyCnt < MAX_BUSY_COUNT);           \
+    if (BusyCnt < MAX_BUSY_COUNT)                   \
+    {                                               \
+        RTMP_IO_WRITE32(_A, RF_CSR_CFG0, _V);          \
+    }                                               \
+}
+
+#define BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)        \
+{                                                       \
+    BBP_CSR_CFG_STRUC  BbpCsr;                             \
+    int             i, k;                               \
+    for (i=0; i<MAX_BUSY_COUNT; i++)                    \
+    {                                                   \
+        RTMP_IO_READ32(_A, BBP_CSR_CFG, &BbpCsr.word);     \
+        if (BbpCsr.field.Busy == BUSY)                  \
+        {                                               \
+            continue;                                   \
+        }                                               \
+        BbpCsr.word = 0;                                \
+        BbpCsr.field.fRead = 1;                         \
+        BbpCsr.field.BBP_RW_MODE = 1;                         \
+        BbpCsr.field.Busy = 1;                          \
+        BbpCsr.field.RegNum = _I;                       \
+        RTMP_IO_WRITE32(_A, BBP_CSR_CFG, BbpCsr.word);     \
+        for (k=0; k<MAX_BUSY_COUNT; k++)                \
+        {                                               \
+            RTMP_IO_READ32(_A, BBP_CSR_CFG, &BbpCsr.word); \
+            if (BbpCsr.field.Busy == IDLE)              \
+                break;                                  \
+        }                                               \
+        if ((BbpCsr.field.Busy == IDLE) &&              \
+            (BbpCsr.field.RegNum == _I))                \
+        {                                               \
+            *(_pV) = (UCHAR)BbpCsr.field.Value;         \
+            break;                                      \
+        }                                               \
+    }                                                   \
+    if (BbpCsr.field.Busy == BUSY)                      \
+    {                                                   \
+        DBGPRINT_ERR(("DFS BBP read R%d fail\n", _I));      \
+        *(_pV) = (_A)->BbpWriteLatch[_I];               \
+    }                                                   \
+}
+
+//#define RTMP_BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)    {}
+// Read BBP register by register's ID. Generate PER to test BA
+#define RTMP_BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)        \
+{                                                       \
+    BBP_CSR_CFG_STRUC  BbpCsr;                             \
+    int             i, k;                               \
+    if ((_A)->bPCIclkOff == FALSE)                     \
+    {                                                   \
+    for (i=0; i<MAX_BUSY_COUNT; i++)                    \
+    {                                                   \
+		RTMP_IO_READ32(_A, H2M_BBP_AGENT, &BbpCsr.word);				\
+        if (BbpCsr.field.Busy == BUSY)                  \
+        {                                               \
+            continue;                                   \
+        }                                               \
+        BbpCsr.word = 0;                                \
+        BbpCsr.field.fRead = 1;                         \
+        BbpCsr.field.BBP_RW_MODE = 1;                         \
+        BbpCsr.field.Busy = 1;                          \
+        BbpCsr.field.RegNum = _I;                       \
+		RTMP_IO_WRITE32(_A, H2M_BBP_AGENT, BbpCsr.word);				\
+		AsicSendCommandToMcu(_A, 0x80, 0xff, 0x0, 0x0);					\
+		RTMPusecDelay(1000);							\
+        for (k=0; k<MAX_BUSY_COUNT; k++)                \
+        {                                               \
+			RTMP_IO_READ32(_A, H2M_BBP_AGENT, &BbpCsr.word);			\
+            if (BbpCsr.field.Busy == IDLE)              \
+                break;                                  \
+        }                                               \
+        if ((BbpCsr.field.Busy == IDLE) &&              \
+            (BbpCsr.field.RegNum == _I))                \
+        {                                               \
+            *(_pV) = (UCHAR)BbpCsr.field.Value;         \
+            break;                                      \
+        }                                               \
+    }                                                   \
+    if (BbpCsr.field.Busy == BUSY)                      \
+    {                                                   \
+		DBGPRINT_ERR(("BBP read R%d=0x%x fail\n", _I, BbpCsr.word));	\
+        *(_pV) = (_A)->BbpWriteLatch[_I];               \
+		RTMP_IO_READ32(_A, H2M_BBP_AGENT, &BbpCsr.word);				\
+		BbpCsr.field.Busy = 0;                          \
+		RTMP_IO_WRITE32(_A, H2M_BBP_AGENT, BbpCsr.word);				\
+    }                                                   \
+    }                   \
+}
+
+#define BBP_IO_WRITE8_BY_REG_ID(_A, _I, _V)        \
+{                                                       \
+    BBP_CSR_CFG_STRUC  BbpCsr;                             \
+    int             BusyCnt;                            \
+    for (BusyCnt=0; BusyCnt<MAX_BUSY_COUNT; BusyCnt++)  \
+    {                                                   \
+        RTMP_IO_READ32(_A, BBP_CSR_CFG, &BbpCsr.word);     \
+        if (BbpCsr.field.Busy == BUSY)                  \
+            continue;                                   \
+        BbpCsr.word = 0;                                \
+        BbpCsr.field.fRead = 0;                         \
+        BbpCsr.field.BBP_RW_MODE = 1;                         \
+        BbpCsr.field.Busy = 1;                          \
+        BbpCsr.field.Value = _V;                        \
+        BbpCsr.field.RegNum = _I;                       \
+        RTMP_IO_WRITE32(_A, BBP_CSR_CFG, BbpCsr.word);     \
+        (_A)->BbpWriteLatch[_I] = _V;                   \
+        break;                                          \
+    }                                                   \
+    if (BusyCnt == MAX_BUSY_COUNT)                      \
+    {                                                   \
+        DBGPRINT_ERR(("BBP write R%d fail\n", _I));     \
+    }                                                   \
+}
+
+// Write BBP register by register's ID & value
+#define RTMP_BBP_IO_WRITE8_BY_REG_ID(_A, _I, _V)        \
+{                                                       \
+    BBP_CSR_CFG_STRUC  BbpCsr;                             \
+    int             BusyCnt;                            \
+    if ((_A)->bPCIclkOff == FALSE)                     \
+    {                                                   \
+    for (BusyCnt=0; BusyCnt<MAX_BUSY_COUNT; BusyCnt++)  \
+    {                                                   \
+		RTMP_IO_READ32(_A, H2M_BBP_AGENT, &BbpCsr.word);				\
+        if (BbpCsr.field.Busy == BUSY)                  \
+            continue;                                   \
+        BbpCsr.word = 0;                                \
+        BbpCsr.field.fRead = 0;                         \
+        BbpCsr.field.BBP_RW_MODE = 1;                         \
+        BbpCsr.field.Busy = 1;                          \
+        BbpCsr.field.Value = _V;                        \
+        BbpCsr.field.RegNum = _I;                       \
+		RTMP_IO_WRITE32(_A, H2M_BBP_AGENT, BbpCsr.word);				\
+		AsicSendCommandToMcu(_A, 0x80, 0xff, 0x0, 0x0);					\
+            if (_A->OpMode == OPMODE_AP)                    \
+		RTMPusecDelay(1000);							\
+        (_A)->BbpWriteLatch[_I] = _V;                   \
+        break;                                          \
+    }                                                   \
+    if (BusyCnt == MAX_BUSY_COUNT)                      \
+    {                                                   \
+		DBGPRINT_ERR(("BBP write R%d=0x%x fail\n", _I, BbpCsr.word));	\
+		RTMP_IO_READ32(_A, H2M_BBP_AGENT, &BbpCsr.word);				\
+		BbpCsr.field.Busy = 0;                          \
+		RTMP_IO_WRITE32(_A, H2M_BBP_AGENT, BbpCsr.word);				\
+    }                                                   \
+    }                                                   \
+}
+#endif // RT2860 //
+
+
+#define     MAP_CHANNEL_ID_TO_KHZ(ch, khz)  {               \
+                switch (ch)                                 \
+                {                                           \
+                    case 1:     khz = 2412000;   break;     \
+                    case 2:     khz = 2417000;   break;     \
+                    case 3:     khz = 2422000;   break;     \
+                    case 4:     khz = 2427000;   break;     \
+                    case 5:     khz = 2432000;   break;     \
+                    case 6:     khz = 2437000;   break;     \
+                    case 7:     khz = 2442000;   break;     \
+                    case 8:     khz = 2447000;   break;     \
+                    case 9:     khz = 2452000;   break;     \
+                    case 10:    khz = 2457000;   break;     \
+                    case 11:    khz = 2462000;   break;     \
+                    case 12:    khz = 2467000;   break;     \
+                    case 13:    khz = 2472000;   break;     \
+                    case 14:    khz = 2484000;   break;     \
+                    case 36:  /* UNII */  khz = 5180000;   break;     \
+                    case 40:  /* UNII */  khz = 5200000;   break;     \
+                    case 44:  /* UNII */  khz = 5220000;   break;     \
+                    case 48:  /* UNII */  khz = 5240000;   break;     \
+                    case 52:  /* UNII */  khz = 5260000;   break;     \
+                    case 56:  /* UNII */  khz = 5280000;   break;     \
+                    case 60:  /* UNII */  khz = 5300000;   break;     \
+                    case 64:  /* UNII */  khz = 5320000;   break;     \
+                    case 149: /* UNII */  khz = 5745000;   break;     \
+                    case 153: /* UNII */  khz = 5765000;   break;     \
+                    case 157: /* UNII */  khz = 5785000;   break;     \
+                    case 161: /* UNII */  khz = 5805000;   break;     \
+                    case 165: /* UNII */  khz = 5825000;   break;     \
+                    case 100: /* HiperLAN2 */  khz = 5500000;   break;     \
+                    case 104: /* HiperLAN2 */  khz = 5520000;   break;     \
+                    case 108: /* HiperLAN2 */  khz = 5540000;   break;     \
+                    case 112: /* HiperLAN2 */  khz = 5560000;   break;     \
+                    case 116: /* HiperLAN2 */  khz = 5580000;   break;     \
+                    case 120: /* HiperLAN2 */  khz = 5600000;   break;     \
+                    case 124: /* HiperLAN2 */  khz = 5620000;   break;     \
+                    case 128: /* HiperLAN2 */  khz = 5640000;   break;     \
+                    case 132: /* HiperLAN2 */  khz = 5660000;   break;     \
+                    case 136: /* HiperLAN2 */  khz = 5680000;   break;     \
+                    case 140: /* HiperLAN2 */  khz = 5700000;   break;     \
+                    case 34:  /* Japan MMAC */   khz = 5170000;   break;   \
+                    case 38:  /* Japan MMAC */   khz = 5190000;   break;   \
+                    case 42:  /* Japan MMAC */   khz = 5210000;   break;   \
+                    case 46:  /* Japan MMAC */   khz = 5230000;   break;   \
+                    case 184: /* Japan */   khz = 4920000;   break;   \
+                    case 188: /* Japan */   khz = 4940000;   break;   \
+                    case 192: /* Japan */   khz = 4960000;   break;   \
+                    case 196: /* Japan */   khz = 4980000;   break;   \
+                    case 208: /* Japan, means J08 */   khz = 5040000;   break;   \
+                    case 212: /* Japan, means J12 */   khz = 5060000;   break;   \
+                    case 216: /* Japan, means J16 */   khz = 5080000;   break;   \
+                    default:    khz = 2412000;   break;     \
+                }                                           \
+            }
+
+#define     MAP_KHZ_TO_CHANNEL_ID(khz, ch)  {               \
+                switch (khz)                                \
+                {                                           \
+                    case 2412000:    ch = 1;     break;     \
+                    case 2417000:    ch = 2;     break;     \
+                    case 2422000:    ch = 3;     break;     \
+                    case 2427000:    ch = 4;     break;     \
+                    case 2432000:    ch = 5;     break;     \
+                    case 2437000:    ch = 6;     break;     \
+                    case 2442000:    ch = 7;     break;     \
+                    case 2447000:    ch = 8;     break;     \
+                    case 2452000:    ch = 9;     break;     \
+                    case 2457000:    ch = 10;    break;     \
+                    case 2462000:    ch = 11;    break;     \
+                    case 2467000:    ch = 12;    break;     \
+                    case 2472000:    ch = 13;    break;     \
+                    case 2484000:    ch = 14;    break;     \
+                    case 5180000:    ch = 36;  /* UNII */  break;     \
+                    case 5200000:    ch = 40;  /* UNII */  break;     \
+                    case 5220000:    ch = 44;  /* UNII */  break;     \
+                    case 5240000:    ch = 48;  /* UNII */  break;     \
+                    case 5260000:    ch = 52;  /* UNII */  break;     \
+                    case 5280000:    ch = 56;  /* UNII */  break;     \
+                    case 5300000:    ch = 60;  /* UNII */  break;     \
+                    case 5320000:    ch = 64;  /* UNII */  break;     \
+                    case 5745000:    ch = 149; /* UNII */  break;     \
+                    case 5765000:    ch = 153; /* UNII */  break;     \
+                    case 5785000:    ch = 157; /* UNII */  break;     \
+                    case 5805000:    ch = 161; /* UNII */  break;     \
+                    case 5825000:    ch = 165; /* UNII */  break;     \
+                    case 5500000:    ch = 100; /* HiperLAN2 */  break;     \
+                    case 5520000:    ch = 104; /* HiperLAN2 */  break;     \
+                    case 5540000:    ch = 108; /* HiperLAN2 */  break;     \
+                    case 5560000:    ch = 112; /* HiperLAN2 */  break;     \
+                    case 5580000:    ch = 116; /* HiperLAN2 */  break;     \
+                    case 5600000:    ch = 120; /* HiperLAN2 */  break;     \
+                    case 5620000:    ch = 124; /* HiperLAN2 */  break;     \
+                    case 5640000:    ch = 128; /* HiperLAN2 */  break;     \
+                    case 5660000:    ch = 132; /* HiperLAN2 */  break;     \
+                    case 5680000:    ch = 136; /* HiperLAN2 */  break;     \
+                    case 5700000:    ch = 140; /* HiperLAN2 */  break;     \
+                    case 5170000:    ch = 34;  /* Japan MMAC */   break;   \
+                    case 5190000:    ch = 38;  /* Japan MMAC */   break;   \
+                    case 5210000:    ch = 42;  /* Japan MMAC */   break;   \
+                    case 5230000:    ch = 46;  /* Japan MMAC */   break;   \
+                    case 4920000:    ch = 184; /* Japan */  break;   \
+                    case 4940000:    ch = 188; /* Japan */  break;   \
+                    case 4960000:    ch = 192; /* Japan */  break;   \
+                    case 4980000:    ch = 196; /* Japan */  break;   \
+                    case 5040000:    ch = 208; /* Japan, means J08 */  break;   \
+                    case 5060000:    ch = 212; /* Japan, means J12 */  break;   \
+                    case 5080000:    ch = 216; /* Japan, means J16 */  break;   \
+                    default:         ch = 1;     break;     \
+                }                                           \
+            }
+
+//
+// Common fragment list structure -  Identical to the scatter gather frag list structure
+//
+#define NIC_MAX_PHYS_BUF_COUNT              8
+
+typedef struct _RTMP_SCATTER_GATHER_ELEMENT {
+    PVOID		Address;
+    ULONG		Length;
+    PULONG		Reserved;
+} RTMP_SCATTER_GATHER_ELEMENT, *PRTMP_SCATTER_GATHER_ELEMENT;
+
+
+typedef struct _RTMP_SCATTER_GATHER_LIST {
+    ULONG  NumberOfElements;
+    PULONG Reserved;
+    RTMP_SCATTER_GATHER_ELEMENT Elements[NIC_MAX_PHYS_BUF_COUNT];
+} RTMP_SCATTER_GATHER_LIST, *PRTMP_SCATTER_GATHER_LIST;
+
+//
+//  Some utility macros
+//
+#ifndef min
+#define min(_a, _b)     (((_a) < (_b)) ? (_a) : (_b))
+#endif
+
+#ifndef max
+#define max(_a, _b)     (((_a) > (_b)) ? (_a) : (_b))
+#endif
+
+#define GET_LNA_GAIN(_pAd)	((_pAd->LatchRfRegs.Channel <= 14) ? (_pAd->BLNAGain) : ((_pAd->LatchRfRegs.Channel <= 64) ? (_pAd->ALNAGain0) : ((_pAd->LatchRfRegs.Channel <= 128) ? (_pAd->ALNAGain1) : (_pAd->ALNAGain2))))
+
+#define INC_COUNTER64(Val)          (Val.QuadPart++)
+
+#define INFRA_ON(_p)                (OPSTATUS_TEST_FLAG(_p, fOP_STATUS_INFRA_ON))
+#define ADHOC_ON(_p)                (OPSTATUS_TEST_FLAG(_p, fOP_STATUS_ADHOC_ON))
+#define MONITOR_ON(_p)              (((_p)->StaCfg.BssType) == BSS_MONITOR)
+#define IDLE_ON(_p)                 (!INFRA_ON(_p) && !ADHOC_ON(_p))
+
+// Check LEAP & CCKM flags
+#define LEAP_ON(_p)                 (((_p)->StaCfg.LeapAuthMode) == CISCO_AuthModeLEAP)
+#define LEAP_CCKM_ON(_p)            ((((_p)->StaCfg.LeapAuthMode) == CISCO_AuthModeLEAP) && ((_p)->StaCfg.LeapAuthInfo.CCKM == TRUE))
+
+// if orginal Ethernet frame contains no LLC/SNAP, then an extra LLC/SNAP encap is required
+#define EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(_pBufVA, _pExtraLlcSnapEncap)		\
+{																\
+	if (((*(_pBufVA + 12) << 8) + *(_pBufVA + 13)) > 1500)		\
+	{															\
+		_pExtraLlcSnapEncap = SNAP_802_1H;						\
+		if (NdisEqualMemory(IPX, _pBufVA + 12, 2) || 			\
+			NdisEqualMemory(APPLE_TALK, _pBufVA + 12, 2))		\
+		{														\
+			_pExtraLlcSnapEncap = SNAP_BRIDGE_TUNNEL;			\
+		}														\
+	}															\
+	else														\
+	{															\
+		_pExtraLlcSnapEncap = NULL;								\
+	}															\
+}
+
+// New Define for new Tx Path.
+#define EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(_pBufVA, _pExtraLlcSnapEncap)	\
+{																\
+	if (((*(_pBufVA) << 8) + *(_pBufVA + 1)) > 1500)			\
+	{															\
+		_pExtraLlcSnapEncap = SNAP_802_1H;						\
+		if (NdisEqualMemory(IPX, _pBufVA, 2) || 				\
+			NdisEqualMemory(APPLE_TALK, _pBufVA, 2))			\
+		{														\
+			_pExtraLlcSnapEncap = SNAP_BRIDGE_TUNNEL;			\
+		}														\
+	}															\
+	else														\
+	{															\
+		_pExtraLlcSnapEncap = NULL;								\
+	}															\
+}
+
+
+#define MAKE_802_3_HEADER(_p, _pMac1, _pMac2, _pType)                   \
+{                                                                       \
+    NdisMoveMemory(_p, _pMac1, MAC_ADDR_LEN);                           \
+    NdisMoveMemory((_p + MAC_ADDR_LEN), _pMac2, MAC_ADDR_LEN);          \
+    NdisMoveMemory((_p + MAC_ADDR_LEN * 2), _pType, LENGTH_802_3_TYPE); \
+}
+
+// if pData has no LLC/SNAP (neither RFC1042 nor Bridge tunnel), keep it that way.
+// else if the received frame is LLC/SNAP-encaped IPX or APPLETALK, preserve the LLC/SNAP field
+// else remove the LLC/SNAP field from the result Ethernet frame
+// Patch for WHQL only, which did not turn on Netbios but use IPX within its payload
+// Note:
+//     _pData & _DataSize may be altered (remove 8-byte LLC/SNAP) by this MACRO
+//     _pRemovedLLCSNAP: pointer to removed LLC/SNAP; NULL is not removed
+#define CONVERT_TO_802_3(_p8023hdr, _pDA, _pSA, _pData, _DataSize, _pRemovedLLCSNAP)      \
+{                                                                       \
+    char LLC_Len[2];                                                    \
+                                                                        \
+    _pRemovedLLCSNAP = NULL;                                            \
+    if (NdisEqualMemory(SNAP_802_1H, _pData, 6)  ||                     \
+        NdisEqualMemory(SNAP_BRIDGE_TUNNEL, _pData, 6))                 \
+    {                                                                   \
+        PUCHAR pProto = _pData + 6;                                     \
+                                                                        \
+        if ((NdisEqualMemory(IPX, pProto, 2) || NdisEqualMemory(APPLE_TALK, pProto, 2)) &&  \
+            NdisEqualMemory(SNAP_802_1H, _pData, 6))                    \
+        {                                                               \
+            LLC_Len[0] = (UCHAR)(_DataSize / 256);                      \
+            LLC_Len[1] = (UCHAR)(_DataSize % 256);                      \
+            MAKE_802_3_HEADER(_p8023hdr, _pDA, _pSA, LLC_Len);          \
+        }                                                               \
+        else                                                            \
+        {                                                               \
+            MAKE_802_3_HEADER(_p8023hdr, _pDA, _pSA, pProto);           \
+            _pRemovedLLCSNAP = _pData;                                  \
+            _DataSize -= LENGTH_802_1_H;                                \
+            _pData += LENGTH_802_1_H;                                   \
+        }                                                               \
+    }                                                                   \
+    else                                                                \
+    {                                                                   \
+        LLC_Len[0] = (UCHAR)(_DataSize / 256);                          \
+        LLC_Len[1] = (UCHAR)(_DataSize % 256);                          \
+        MAKE_802_3_HEADER(_p8023hdr, _pDA, _pSA, LLC_Len);              \
+    }                                                                   \
+}
+
+#define SWITCH_AB( _pAA, _pBB)    \
+{                                                                           \
+    PVOID pCC;                                                          \
+    pCC = _pBB;                                                 \
+    _pBB = _pAA;                                                 \
+    _pAA = pCC;                                                 \
+}
+
+// Enqueue this frame to MLME engine
+// We need to enqueue the whole frame because MLME need to pass data type
+// information from 802.11 header
+#ifdef RT2860
+#define REPORT_MGMT_FRAME_TO_MLME(_pAd, Wcid, _pFrame, _FrameSize, _Rssi0, _Rssi1, _Rssi2, _PlcpSignal)        \
+{                                                                                       \
+    UINT32 High32TSF, Low32TSF;                                                          \
+    RTMP_IO_READ32(_pAd, TSF_TIMER_DW1, &High32TSF);                                       \
+    RTMP_IO_READ32(_pAd, TSF_TIMER_DW0, &Low32TSF);                                        \
+    MlmeEnqueueForRecv(_pAd, Wcid, High32TSF, Low32TSF, (UCHAR)_Rssi0, (UCHAR)_Rssi1,(UCHAR)_Rssi2,_FrameSize, _pFrame, (UCHAR)_PlcpSignal);   \
+}
+#endif // RT2860 //
+
+#define NDIS_QUERY_BUFFER(_NdisBuf, _ppVA, _pBufLen)                    \
+    NdisQueryBuffer(_NdisBuf, _ppVA, _pBufLen)
+
+#define MAC_ADDR_EQUAL(pAddr1,pAddr2)           RTMPEqualMemory((PVOID)(pAddr1), (PVOID)(pAddr2), MAC_ADDR_LEN)
+#define SSID_EQUAL(ssid1, len1, ssid2, len2)    ((len1==len2) && (RTMPEqualMemory(ssid1, ssid2, len1)))
+
+//
+// Check if it is Japan W53(ch52,56,60,64) channel.
+//
+#define JapanChannelCheck(channel)  ((channel == 52) || (channel == 56) || (channel == 60) || (channel == 64))
+
+#ifdef CONFIG_STA_SUPPORT
+#define STA_PORT_SECURED(_pAd) \
+{ \
+	_pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED; \
+	NdisAcquireSpinLock(&_pAd->MacTabLock); \
+	_pAd->MacTab.Content[BSSID_WCID].PortSecured = _pAd->StaCfg.PortSecured; \
+	NdisReleaseSpinLock(&_pAd->MacTabLock); \
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+//
+// Register set pair for initialzation register set definition
+//
+typedef struct  _RTMP_REG_PAIR
+{
+	ULONG   Register;
+	ULONG   Value;
+} RTMP_REG_PAIR, *PRTMP_REG_PAIR;
+
+typedef struct  _REG_PAIR
+{
+	UCHAR   Register;
+	UCHAR   Value;
+} REG_PAIR, *PREG_PAIR;
+
+//
+// Register set pair for initialzation register set definition
+//
+typedef struct  _RTMP_RF_REGS
+{
+	UCHAR   Channel;
+	ULONG   R1;
+	ULONG   R2;
+	ULONG   R3;
+	ULONG   R4;
+} RTMP_RF_REGS, *PRTMP_RF_REGS;
+
+typedef struct _FREQUENCY_ITEM {
+	UCHAR	Channel;
+	UCHAR	N;
+	UCHAR	R;
+	UCHAR	K;
+} FREQUENCY_ITEM, *PFREQUENCY_ITEM;
+
+//
+//  Data buffer for DMA operation, the buffer must be contiguous physical memory
+//  Both DMA to / from CPU use the same structure.
+//
+typedef struct  _RTMP_DMABUF
+{
+	ULONG                   AllocSize;
+	PVOID                   AllocVa;            // TxBuf virtual address
+	NDIS_PHYSICAL_ADDRESS   AllocPa;            // TxBuf physical address
+} RTMP_DMABUF, *PRTMP_DMABUF;
+
+
+typedef	union	_HEADER_802_11_SEQ{
+#ifdef RT_BIG_ENDIAN
+    struct {
+   	USHORT			Sequence:12;
+	USHORT			Frag:4;
+    }   field;
+#else
+    struct {
+	USHORT			Frag:4;
+	USHORT			Sequence:12;
+    }   field;
+#endif
+    USHORT           value;
+}	HEADER_802_11_SEQ, *PHEADER_802_11_SEQ;
+
+//
+//  Data buffer for DMA operation, the buffer must be contiguous physical memory
+//  Both DMA to / from CPU use the same structure.
+//
+typedef struct  _RTMP_REORDERBUF
+{
+	BOOLEAN			IsFull;
+	PVOID                   AllocVa;            // TxBuf virtual address
+	UCHAR			Header802_3[14];
+	HEADER_802_11_SEQ			Sequence;	//support compressed bitmap BA, so no consider fragment in BA
+	UCHAR 		DataOffset;
+	USHORT 		Datasize;
+	ULONG                   AllocSize;
+#ifdef RT2860
+	NDIS_PHYSICAL_ADDRESS   AllocPa;            // TxBuf physical address
+#endif // RT2860 //
+}   RTMP_REORDERBUF, *PRTMP_REORDERBUF;
+
+//
+// Control block (Descriptor) for all ring descriptor DMA operation, buffer must be
+// contiguous physical memory. NDIS_PACKET stored the binding Rx packet descriptor
+// which won't be released, driver has to wait until upper layer return the packet
+// before giveing up this rx ring descriptor to ASIC. NDIS_BUFFER is assocaited pair
+// to describe the packet buffer. For Tx, NDIS_PACKET stored the tx packet descriptor
+// which driver should ACK upper layer when the tx is physically done or failed.
+//
+typedef struct _RTMP_DMACB
+{
+	ULONG                   AllocSize;          // Control block size
+	PVOID                   AllocVa;            // Control block virtual address
+	NDIS_PHYSICAL_ADDRESS   AllocPa;            // Control block physical address
+	PNDIS_PACKET pNdisPacket;
+	PNDIS_PACKET pNextNdisPacket;
+
+	RTMP_DMABUF             DmaBuf;             // Associated DMA buffer structure
+} RTMP_DMACB, *PRTMP_DMACB;
+
+typedef struct _RTMP_TX_BUF
+{
+	PQUEUE_ENTRY    Next;
+	UCHAR           Index;
+	ULONG                   AllocSize;          // Control block size
+	PVOID                   AllocVa;            // Control block virtual address
+	NDIS_PHYSICAL_ADDRESS   AllocPa;            // Control block physical address
+} RTMP_TXBUF, *PRTMP_TXBUF;
+
+typedef struct _RTMP_RX_BUF
+{
+	BOOLEAN           InUse;
+	ULONG           	ByBaRecIndex;
+	RTMP_REORDERBUF	MAP_RXBuf[MAX_RX_REORDERBUF];
+} RTMP_RXBUF, *PRTMP_RXBUF;
+typedef struct _RTMP_TX_RING
+{
+	RTMP_DMACB  Cell[TX_RING_SIZE];
+	UINT32		TxCpuIdx;
+	UINT32		TxDmaIdx;
+	UINT32		TxSwFreeIdx; 	// software next free tx index
+} RTMP_TX_RING, *PRTMP_TX_RING;
+
+typedef struct _RTMP_RX_RING
+{
+	RTMP_DMACB  Cell[RX_RING_SIZE];
+	UINT32		RxCpuIdx;
+	UINT32		RxDmaIdx;
+	INT32		RxSwReadIdx; 	// software next read index
+} RTMP_RX_RING, *PRTMP_RX_RING;
+
+typedef struct _RTMP_MGMT_RING
+{
+	RTMP_DMACB  Cell[MGMT_RING_SIZE];
+	UINT32		TxCpuIdx;
+	UINT32		TxDmaIdx;
+	UINT32		TxSwFreeIdx; // software next free tx index
+} RTMP_MGMT_RING, *PRTMP_MGMT_RING;
+
+//
+//  Statistic counter structure
+//
+typedef struct _COUNTER_802_3
+{
+	// General Stats
+	ULONG       GoodTransmits;
+	ULONG       GoodReceives;
+	ULONG       TxErrors;
+	ULONG       RxErrors;
+	ULONG       RxNoBuffer;
+
+	// Ethernet Stats
+	ULONG       RcvAlignmentErrors;
+	ULONG       OneCollision;
+	ULONG       MoreCollisions;
+
+} COUNTER_802_3, *PCOUNTER_802_3;
+
+typedef struct _COUNTER_802_11 {
+	ULONG           Length;
+	LARGE_INTEGER   LastTransmittedFragmentCount;
+	LARGE_INTEGER   TransmittedFragmentCount;
+	LARGE_INTEGER   MulticastTransmittedFrameCount;
+	LARGE_INTEGER   FailedCount;
+	LARGE_INTEGER   RetryCount;
+	LARGE_INTEGER   MultipleRetryCount;
+	LARGE_INTEGER   RTSSuccessCount;
+	LARGE_INTEGER   RTSFailureCount;
+	LARGE_INTEGER   ACKFailureCount;
+	LARGE_INTEGER   FrameDuplicateCount;
+	LARGE_INTEGER   ReceivedFragmentCount;
+	LARGE_INTEGER   MulticastReceivedFrameCount;
+	LARGE_INTEGER   FCSErrorCount;
+} COUNTER_802_11, *PCOUNTER_802_11;
+
+typedef struct _COUNTER_RALINK {
+	ULONG           TransmittedByteCount;   // both successful and failure, used to calculate TX throughput
+	ULONG           ReceivedByteCount;      // both CRC okay and CRC error, used to calculate RX throughput
+	ULONG           BeenDisassociatedCount;
+	ULONG           BadCQIAutoRecoveryCount;
+	ULONG           PoorCQIRoamingCount;
+	ULONG           MgmtRingFullCount;
+	ULONG           RxCountSinceLastNULL;
+	ULONG           RxCount;
+	ULONG           RxRingErrCount;
+	ULONG           KickTxCount;
+	ULONG           TxRingErrCount;
+	LARGE_INTEGER   RealFcsErrCount;
+	ULONG           PendingNdisPacketCount;
+
+	ULONG           OneSecOsTxCount[NUM_OF_TX_RING];
+	ULONG           OneSecDmaDoneCount[NUM_OF_TX_RING];
+	UINT32          OneSecTxDoneCount;
+	ULONG           OneSecRxCount;
+	UINT32          OneSecTxAggregationCount;
+	UINT32          OneSecRxAggregationCount;
+
+	UINT32   		OneSecFrameDuplicateCount;
+
+
+	UINT32          OneSecTxNoRetryOkCount;
+	UINT32          OneSecTxRetryOkCount;
+	UINT32          OneSecTxFailCount;
+	UINT32          OneSecFalseCCACnt;      // CCA error count, for debug purpose, might move to global counter
+	UINT32          OneSecRxOkCnt;          // RX without error
+	UINT32          OneSecRxOkDataCnt;      // unicast-to-me DATA frame count
+	UINT32          OneSecRxFcsErrCnt;      // CRC error
+	UINT32          OneSecBeaconSentCnt;
+	UINT32          LastOneSecTotalTxCount; // OneSecTxNoRetryOkCount + OneSecTxRetryOkCount + OneSecTxFailCount
+	UINT32          LastOneSecRxOkDataCnt;  // OneSecRxOkDataCnt
+	ULONG		DuplicateRcv;
+	ULONG		TxAggCount;
+	ULONG		TxNonAggCount;
+	ULONG		TxAgg1MPDUCount;
+	ULONG		TxAgg2MPDUCount;
+	ULONG		TxAgg3MPDUCount;
+	ULONG		TxAgg4MPDUCount;
+	ULONG		TxAgg5MPDUCount;
+	ULONG		TxAgg6MPDUCount;
+	ULONG		TxAgg7MPDUCount;
+	ULONG		TxAgg8MPDUCount;
+	ULONG		TxAgg9MPDUCount;
+	ULONG		TxAgg10MPDUCount;
+	ULONG		TxAgg11MPDUCount;
+	ULONG		TxAgg12MPDUCount;
+	ULONG		TxAgg13MPDUCount;
+	ULONG		TxAgg14MPDUCount;
+	ULONG		TxAgg15MPDUCount;
+	ULONG		TxAgg16MPDUCount;
+
+	LARGE_INTEGER       TransmittedOctetsInAMSDU;
+	LARGE_INTEGER       TransmittedAMSDUCount;
+	LARGE_INTEGER       ReceivedOctesInAMSDUCount;
+	LARGE_INTEGER       ReceivedAMSDUCount;
+	LARGE_INTEGER       TransmittedAMPDUCount;
+	LARGE_INTEGER       TransmittedMPDUsInAMPDUCount;
+	LARGE_INTEGER       TransmittedOctetsInAMPDUCount;
+	LARGE_INTEGER       MPDUInReceivedAMPDUCount;
+} COUNTER_RALINK, *PCOUNTER_RALINK;
+
+typedef struct _PID_COUNTER {
+	ULONG           TxAckRequiredCount;      // CRC error
+	ULONG           TxAggreCount;
+	ULONG           TxSuccessCount; // OneSecTxNoRetryOkCount + OneSecTxRetryOkCount + OneSecTxFailCount
+	ULONG		LastSuccessRate;
+} PID_COUNTER, *PPID_COUNTER;
+
+typedef struct _COUNTER_DRS {
+	// to record the each TX rate's quality. 0 is best, the bigger the worse.
+	USHORT          TxQuality[MAX_STEP_OF_TX_RATE_SWITCH];
+	UCHAR           PER[MAX_STEP_OF_TX_RATE_SWITCH];
+	UCHAR           TxRateUpPenalty;      // extra # of second penalty due to last unstable condition
+	ULONG           CurrTxRateStableTime; // # of second in current TX rate
+	BOOLEAN         fNoisyEnvironment;
+	BOOLEAN         fLastSecAccordingRSSI;
+	UCHAR           LastSecTxRateChangeAction; // 0: no change, 1:rate UP, 2:rate down
+	UCHAR			LastTimeTxRateChangeAction; //Keep last time value of LastSecTxRateChangeAction
+	ULONG			LastTxOkCount;
+} COUNTER_DRS, *PCOUNTER_DRS;
+
+//
+//  Arcfour Structure Added by PaulWu
+//
+typedef struct  _ARCFOUR
+{
+	UINT            X;
+	UINT            Y;
+	UCHAR           STATE[256];
+} ARCFOURCONTEXT, *PARCFOURCONTEXT;
+
+// MIMO Tx parameter, ShortGI, MCS, STBC, etc.  these are fields in TXWI too. just copy to TXWI.
+typedef struct  _RECEIVE_SETTING {
+#ifdef RT_BIG_ENDIAN
+	USHORT		MIMO:1;
+	USHORT		OFDM:1;
+	USHORT		rsv:3;
+	USHORT		STBC:2;	//SPACE
+	USHORT		ShortGI:1;
+	USHORT		Mode:2;	//channel bandwidth 20MHz or 40 MHz
+	USHORT   	NumOfRX:2;                 // MIMO. WE HAVE 3R
+#else
+	USHORT   	NumOfRX:2;                 // MIMO. WE HAVE 3R
+	USHORT		Mode:2;	//channel bandwidth 20MHz or 40 MHz
+	USHORT		ShortGI:1;
+	USHORT		STBC:2;	//SPACE
+	USHORT		rsv:3;
+	USHORT		OFDM:1;
+	USHORT		MIMO:1;
+#endif
+ } RECEIVE_SETTING, *PRECEIVE_SETTING;
+
+// Shared key data structure
+typedef struct  _WEP_KEY {
+	UCHAR   KeyLen;                     // Key length for each key, 0: entry is invalid
+	UCHAR   Key[MAX_LEN_OF_KEY];        // right now we implement 4 keys, 128 bits max
+} WEP_KEY, *PWEP_KEY;
+
+typedef struct _CIPHER_KEY {
+	UCHAR   Key[16];            // right now we implement 4 keys, 128 bits max
+	UCHAR   RxMic[8];			// make alignment
+	UCHAR   TxMic[8];
+	UCHAR   TxTsc[6];           // 48bit TSC value
+	UCHAR   RxTsc[6];           // 48bit TSC value
+	UCHAR   CipherAlg;          // 0-none, 1:WEP64, 2:WEP128, 3:TKIP, 4:AES, 5:CKIP64, 6:CKIP128
+	UCHAR   KeyLen;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR   BssId[6];
+#endif // CONFIG_STA_SUPPORT //
+            // Key length for each key, 0: entry is invalid
+	UCHAR   Type;               // Indicate Pairwise/Group when reporting MIC error
+} CIPHER_KEY, *PCIPHER_KEY;
+
+typedef struct _BBP_TUNING_STRUCT {
+	BOOLEAN     Enable;
+	UCHAR       FalseCcaCountUpperBound;  // 100 per sec
+	UCHAR       FalseCcaCountLowerBound;  // 10 per sec
+	UCHAR       R17LowerBound;            // specified in E2PROM
+	UCHAR       R17UpperBound;            // 0x68 according to David Tung
+	UCHAR       CurrentR17Value;
+} BBP_TUNING, *PBBP_TUNING;
+
+typedef struct _SOFT_RX_ANT_DIVERSITY_STRUCT {
+	UCHAR     EvaluatePeriod;		 // 0:not evalute status, 1: evaluate status, 2: switching status
+	UCHAR     Pair1PrimaryRxAnt;     // 0:Ant-E1, 1:Ant-E2
+	UCHAR     Pair1SecondaryRxAnt;   // 0:Ant-E1, 1:Ant-E2
+	UCHAR     Pair2PrimaryRxAnt;     // 0:Ant-E3, 1:Ant-E4
+	UCHAR     Pair2SecondaryRxAnt;   // 0:Ant-E3, 1:Ant-E4
+	SHORT     Pair1AvgRssi[2];       // AvgRssi[0]:E1, AvgRssi[1]:E2
+	SHORT     Pair2AvgRssi[2];       // AvgRssi[0]:E3, AvgRssi[1]:E4
+	SHORT     Pair1LastAvgRssi;      //
+	SHORT     Pair2LastAvgRssi;      //
+	ULONG     RcvPktNumWhenEvaluate;
+	BOOLEAN   FirstPktArrivedWhenEvaluate;
+	RALINK_TIMER_STRUCT    RxAntDiversityTimer;
+} SOFT_RX_ANT_DIVERSITY, *PSOFT_RX_ANT_DIVERSITY;
+
+typedef struct _LEAP_AUTH_INFO {
+	BOOLEAN         Enabled;        //Ture: Enable LEAP Authentication
+	BOOLEAN         CCKM;           //Ture: Use Fast Reauthentication with CCKM
+	UCHAR           Reserve[2];
+	UCHAR           UserName[256];  //LEAP, User name
+	ULONG           UserNameLen;
+	UCHAR           Password[256];  //LEAP, User Password
+	ULONG           PasswordLen;
+} LEAP_AUTH_INFO, *PLEAP_AUTH_INFO;
+
+typedef struct {
+	UCHAR        Addr[MAC_ADDR_LEN];
+	UCHAR        ErrorCode[2];  //00 01-Invalid authentication type
+								//00 02-Authentication timeout
+								//00 03-Challenge from AP failed
+								//00 04-Challenge to AP failed
+	BOOLEAN      Reported;
+} ROGUEAP_ENTRY, *PROGUEAP_ENTRY;
+
+typedef struct {
+	UCHAR               RogueApNr;
+	ROGUEAP_ENTRY       RogueApEntry[MAX_LEN_OF_BSS_TABLE];
+} ROGUEAP_TABLE, *PROGUEAP_TABLE;
+
+typedef struct {
+	BOOLEAN     Enable;
+	UCHAR       Delta;
+	BOOLEAN     PlusSign;
+} CCK_TX_POWER_CALIBRATE, *PCCK_TX_POWER_CALIBRATE;
+
+//
+// Receive Tuple Cache Format
+//
+typedef struct  _TUPLE_CACHE    {
+	BOOLEAN         Valid;
+	UCHAR           MacAddress[MAC_ADDR_LEN];
+	USHORT          Sequence;
+	USHORT          Frag;
+} TUPLE_CACHE, *PTUPLE_CACHE;
+
+//
+// Fragment Frame structure
+//
+typedef struct  _FRAGMENT_FRAME {
+	PNDIS_PACKET    pFragPacket;
+	ULONG       RxSize;
+	USHORT      Sequence;
+	USHORT      LastFrag;
+	ULONG       Flags;          // Some extra frame information. bit 0: LLC presented
+} FRAGMENT_FRAME, *PFRAGMENT_FRAME;
+
+
+//
+// Packet information for NdisQueryPacket
+//
+typedef struct  _PACKET_INFO    {
+	UINT            PhysicalBufferCount;    // Physical breaks of buffer descripor chained
+	UINT            BufferCount ;           // Number of Buffer descriptor chained
+	UINT            TotalPacketLength ;     // Self explained
+	PNDIS_BUFFER    pFirstBuffer;           // Pointer to first buffer descriptor
+} PACKET_INFO, *PPACKET_INFO;
+
+//
+// Tkip Key structure which RC4 key & MIC calculation
+//
+typedef struct  _TKIP_KEY_INFO  {
+	UINT        nBytesInM;  // # bytes in M for MICKEY
+	ULONG       IV16;
+	ULONG       IV32;
+	ULONG       K0;         // for MICKEY Low
+	ULONG       K1;         // for MICKEY Hig
+	ULONG       L;          // Current state for MICKEY
+	ULONG       R;          // Current state for MICKEY
+	ULONG       M;          // Message accumulator for MICKEY
+	UCHAR       RC4KEY[16];
+	UCHAR       MIC[8];
+} TKIP_KEY_INFO, *PTKIP_KEY_INFO;
+
+//
+// Private / Misc data, counters for driver internal use
+//
+typedef struct  __PRIVATE_STRUC {
+	UINT       SystemResetCnt;         // System reset counter
+	UINT       TxRingFullCnt;          // Tx ring full occurrance number
+	UINT       PhyRxErrCnt;            // PHY Rx error count, for debug purpose, might move to global counter
+	// Variables for WEP encryption / decryption in rtmp_wep.c
+	UINT       FCSCRC32;
+	ARCFOURCONTEXT  WEPCONTEXT;
+	// Tkip stuff
+	TKIP_KEY_INFO   Tx;
+	TKIP_KEY_INFO   Rx;
+} PRIVATE_STRUC, *PPRIVATE_STRUC;
+
+// structure to tune BBP R66 (BBP TUNING)
+typedef struct _BBP_R66_TUNING {
+	BOOLEAN     bEnable;
+	USHORT      FalseCcaLowerThreshold;  // default 100
+	USHORT      FalseCcaUpperThreshold;  // default 512
+	UCHAR       R66Delta;
+	UCHAR       R66CurrentValue;
+	BOOLEAN		R66LowerUpperSelect; //Before LinkUp, Used LowerBound or UpperBound as R66 value.
+} BBP_R66_TUNING, *PBBP_R66_TUNING;
+
+// structure to store channel TX power
+typedef struct _CHANNEL_TX_POWER {
+	USHORT     RemainingTimeForUse;		//unit: sec
+	UCHAR      Channel;
+#ifdef DOT11N_DRAFT3
+	BOOLEAN       bEffectedChannel;	// For BW 40 operating in 2.4GHz , the "effected channel" is the channel that is covered in 40Mhz.
+#endif // DOT11N_DRAFT3 //
+	CHAR       Power;
+	CHAR       Power2;
+	UCHAR      MaxTxPwr;
+	UCHAR      DfsReq;
+} CHANNEL_TX_POWER, *PCHANNEL_TX_POWER;
+
+// structure to store 802.11j channel TX power
+typedef struct _CHANNEL_11J_TX_POWER {
+	UCHAR      Channel;
+	UCHAR      BW;	// BW_10 or BW_20
+	CHAR       Power;
+	CHAR       Power2;
+	USHORT     RemainingTimeForUse;		//unit: sec
+} CHANNEL_11J_TX_POWER, *PCHANNEL_11J_TX_POWER;
+
+typedef enum _ABGBAND_STATE_ {
+	UNKNOWN_BAND,
+	BG_BAND,
+	A_BAND,
+} ABGBAND_STATE;
+
+typedef struct _MLME_STRUCT {
+#ifdef CONFIG_STA_SUPPORT
+	// STA state machines
+	STATE_MACHINE           CntlMachine;
+	STATE_MACHINE           AssocMachine;
+	STATE_MACHINE           AuthMachine;
+	STATE_MACHINE           AuthRspMachine;
+	STATE_MACHINE           SyncMachine;
+	STATE_MACHINE           WpaPskMachine;
+	STATE_MACHINE           LeapMachine;
+	STATE_MACHINE           AironetMachine;
+	STATE_MACHINE_FUNC      AssocFunc[ASSOC_FUNC_SIZE];
+	STATE_MACHINE_FUNC      AuthFunc[AUTH_FUNC_SIZE];
+	STATE_MACHINE_FUNC      AuthRspFunc[AUTH_RSP_FUNC_SIZE];
+	STATE_MACHINE_FUNC      SyncFunc[SYNC_FUNC_SIZE];
+	STATE_MACHINE_FUNC      WpaPskFunc[WPA_PSK_FUNC_SIZE];
+	STATE_MACHINE_FUNC      AironetFunc[AIRONET_FUNC_SIZE];
+#endif // CONFIG_STA_SUPPORT //
+	STATE_MACHINE_FUNC      ActFunc[ACT_FUNC_SIZE];
+	// Action
+	STATE_MACHINE           ActMachine;
+
+
+#ifdef QOS_DLS_SUPPORT
+	STATE_MACHINE			DlsMachine;
+	STATE_MACHINE_FUNC      DlsFunc[DLS_FUNC_SIZE];
+#endif // QOS_DLS_SUPPORT //
+
+
+
+
+	ULONG                   ChannelQuality;  // 0..100, Channel Quality Indication for Roaming
+	ULONG                   Now32;           // latch the value of NdisGetSystemUpTime()
+	ULONG                   LastSendNULLpsmTime;
+
+	BOOLEAN                 bRunning;
+	NDIS_SPIN_LOCK          TaskLock;
+	MLME_QUEUE              Queue;
+
+	UINT                    ShiftReg;
+
+	RALINK_TIMER_STRUCT     PeriodicTimer;
+	RALINK_TIMER_STRUCT     APSDPeriodicTimer;
+	RALINK_TIMER_STRUCT     LinkDownTimer;
+	RALINK_TIMER_STRUCT     LinkUpTimer;
+#ifdef RT2860
+    UCHAR                   bPsPollTimerRunning;
+    RALINK_TIMER_STRUCT     PsPollTimer;
+	RALINK_TIMER_STRUCT     RadioOnOffTimer;
+#endif // RT2860 //
+	ULONG                   PeriodicRound;
+	ULONG                   OneSecPeriodicRound;
+
+	UCHAR					RealRxPath;
+	BOOLEAN					bLowThroughput;
+	BOOLEAN					bEnableAutoAntennaCheck;
+	RALINK_TIMER_STRUCT		RxAntEvalTimer;
+
+
+} MLME_STRUCT, *PMLME_STRUCT;
+
+// structure for radar detection and channel switch
+typedef struct _RADAR_DETECT_STRUCT {
+	UCHAR		CSCount;			//Channel switch counter
+	UCHAR		CSPeriod;			//Channel switch period (beacon count)
+	UCHAR		RDCount;			//Radar detection counter
+	UCHAR		RDMode;				//Radar Detection mode
+	UCHAR		RDDurRegion;		//Radar detection duration region
+	UCHAR		BBPR16;
+	UCHAR		BBPR17;
+	UCHAR		BBPR18;
+	UCHAR		BBPR21;
+	UCHAR		BBPR22;
+	UCHAR		BBPR64;
+	ULONG		InServiceMonitorCount; // unit: sec
+	UINT8		DfsSessionTime;
+	BOOLEAN		bFastDfs;
+	UINT8		ChMovingTime;
+	UINT8		LongPulseRadarTh;
+} RADAR_DETECT_STRUCT, *PRADAR_DETECT_STRUCT;
+
+#ifdef CARRIER_DETECTION_SUPPORT
+typedef enum CD_STATE_n
+{
+	CD_NORMAL,
+	CD_SILENCE,
+	CD_MAX_STATE
+} CD_STATE;
+
+typedef struct CARRIER_DETECTION_s
+{
+	BOOLEAN					Enable;
+	UINT8					CDSessionTime;
+	UINT8					CDPeriod;
+	CD_STATE				CD_State;
+} CARRIER_DETECTION, *PCARRIER_DETECTION;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+typedef enum _REC_BLOCKACK_STATUS
+{
+    Recipient_NONE=0,
+	Recipient_USED,
+	Recipient_HandleRes,
+    Recipient_Accept
+} REC_BLOCKACK_STATUS, *PREC_BLOCKACK_STATUS;
+
+typedef enum _ORI_BLOCKACK_STATUS
+{
+    Originator_NONE=0,
+	Originator_USED,
+    Originator_WaitRes,
+    Originator_Done
+} ORI_BLOCKACK_STATUS, *PORI_BLOCKACK_STATUS;
+
+#ifdef DOT11_N_SUPPORT
+typedef struct _BA_ORI_ENTRY{
+	UCHAR   Wcid;
+	UCHAR   TID;
+	UCHAR   BAWinSize;
+	UCHAR   Token;
+// Sequence is to fill every outgoing QoS DATA frame's sequence field in 802.11 header.
+	USHORT	Sequence;
+	USHORT	TimeOutValue;
+	ORI_BLOCKACK_STATUS  ORI_BA_Status;
+	RALINK_TIMER_STRUCT ORIBATimer;
+	PVOID	pAdapter;
+} BA_ORI_ENTRY, *PBA_ORI_ENTRY;
+
+typedef struct _BA_REC_ENTRY {
+	UCHAR   Wcid;
+	UCHAR   TID;
+	UCHAR   BAWinSize;	// 7.3.1.14. each buffer is capable of holding a max AMSDU or MSDU.
+	USHORT		LastIndSeq;
+	USHORT		TimeOutValue;
+	RALINK_TIMER_STRUCT RECBATimer;
+	ULONG		LastIndSeqAtTimer;
+	ULONG		nDropPacket;
+	ULONG		rcvSeq;
+	REC_BLOCKACK_STATUS  REC_BA_Status;
+	NDIS_SPIN_LOCK          RxReRingLock;                 // Rx Ring spinlock
+	PVOID	pAdapter;
+	struct reordering_list	list;
+} BA_REC_ENTRY, *PBA_REC_ENTRY;
+
+
+typedef struct {
+	ULONG		numAsRecipient;		// I am recipient of numAsRecipient clients. These client are in the BARecEntry[]
+	ULONG		numAsOriginator;	// I am originator of 	numAsOriginator clients. These clients are in the BAOriEntry[]
+	BA_ORI_ENTRY       BAOriEntry[MAX_LEN_OF_BA_ORI_TABLE];
+	BA_REC_ENTRY       BARecEntry[MAX_LEN_OF_BA_REC_TABLE];
+} BA_TABLE, *PBA_TABLE;
+
+//For QureyBATableOID use;
+typedef struct  PACKED _OID_BA_REC_ENTRY{
+	UCHAR   MACAddr[MAC_ADDR_LEN];
+	UCHAR   BaBitmap;   // if (BaBitmap&(1<<TID)), this session with{MACAddr, TID}exists, so read BufSize[TID] for BufferSize
+	UCHAR   rsv;
+	UCHAR   BufSize[8];
+	REC_BLOCKACK_STATUS	REC_BA_Status[8];
+} OID_BA_REC_ENTRY, *POID_BA_REC_ENTRY;
+
+//For QureyBATableOID use;
+typedef struct  PACKED _OID_BA_ORI_ENTRY{
+	UCHAR   MACAddr[MAC_ADDR_LEN];
+	UCHAR   BaBitmap;  // if (BaBitmap&(1<<TID)), this session with{MACAddr, TID}exists, so read BufSize[TID] for BufferSize, read ORI_BA_Status[TID] for status
+	UCHAR   rsv;
+	UCHAR   BufSize[8];
+	ORI_BLOCKACK_STATUS  ORI_BA_Status[8];
+} OID_BA_ORI_ENTRY, *POID_BA_ORI_ENTRY;
+
+typedef struct _QUERYBA_TABLE{
+	OID_BA_ORI_ENTRY       BAOriEntry[32];
+	OID_BA_REC_ENTRY       BARecEntry[32];
+	UCHAR   OriNum;// Number of below BAOriEntry
+	UCHAR   RecNum;// Number of below BARecEntry
+} QUERYBA_TABLE, *PQUERYBA_TABLE;
+
+typedef	union	_BACAP_STRUC	{
+#ifdef RT_BIG_ENDIAN
+	struct	{
+		UINT32     :4;
+		UINT32     b2040CoexistScanSup:1;		//As Sta, support do 2040 coexistence scan for AP. As Ap, support monitor trigger event to check if can use BW 40MHz.
+		UINT32     bHtAdhoc:1;			// adhoc can use ht rate.
+		UINT32     MMPSmode:2;	// MIMO power save more, 0:static, 1:dynamic, 2:rsv, 3:mimo enable
+		UINT32     AmsduSize:1;	// 0:3839, 1:7935 bytes. UINT  MSDUSizeToBytes[]	= { 3839, 7935};
+		UINT32     AmsduEnable:1;	//Enable AMSDU transmisstion
+		UINT32		MpduDensity:3;
+		UINT32		Policy:2;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use
+		UINT32		AutoBA:1;	// automatically BA
+		UINT32		TxBAWinLimit:8;
+		UINT32		RxBAWinLimit:8;
+	}	field;
+#else
+	struct	{
+		UINT32		RxBAWinLimit:8;
+		UINT32		TxBAWinLimit:8;
+		UINT32		AutoBA:1;	// automatically BA
+		UINT32		Policy:2;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use
+		UINT32		MpduDensity:3;
+		UINT32       	AmsduEnable:1;	//Enable AMSDU transmisstion
+		UINT32       	AmsduSize:1;	// 0:3839, 1:7935 bytes. UINT  MSDUSizeToBytes[]	= { 3839, 7935};
+		UINT32       	MMPSmode:2;	// MIMO power save more, 0:static, 1:dynamic, 2:rsv, 3:mimo enable
+		UINT32       	bHtAdhoc:1;			// adhoc can use ht rate.
+		UINT32       	b2040CoexistScanSup:1;		//As Sta, support do 2040 coexistence scan for AP. As Ap, support monitor trigger event to check if can use BW 40MHz.
+		UINT32       	:4;
+	}	field;
+#endif
+	UINT32			word;
+} BACAP_STRUC, *PBACAP_STRUC;
+#endif // DOT11_N_SUPPORT //
+
+//This structure is for all 802.11n card InterOptibilityTest action. Reset all Num every n second.  (Details see MLMEPeriodic)
+typedef	struct	_IOT_STRUC	{
+	UCHAR			Threshold[2];
+	UCHAR			ReorderTimeOutNum[MAX_LEN_OF_BA_REC_TABLE];	// compare with threshold[0]
+	UCHAR			RefreshNum[MAX_LEN_OF_BA_REC_TABLE];	// compare with threshold[1]
+	ULONG			OneSecInWindowCount;
+	ULONG			OneSecFrameDuplicateCount;
+	ULONG			OneSecOutWindowCount;
+	UCHAR			DelOriAct;
+	UCHAR			DelRecAct;
+	UCHAR			RTSShortProt;
+	UCHAR			RTSLongProt;
+	BOOLEAN			bRTSLongProtOn;
+#ifdef CONFIG_STA_SUPPORT
+	BOOLEAN			bLastAtheros;
+    BOOLEAN			bCurrentAtheros;
+    BOOLEAN         bNowAtherosBurstOn;
+	BOOLEAN			bNextDisableRxBA;
+    BOOLEAN			bToggle;
+#endif // CONFIG_STA_SUPPORT //
+} IOT_STRUC, *PIOT_STRUC;
+
+// This is the registry setting for 802.11n transmit setting.  Used in advanced page.
+typedef union _REG_TRANSMIT_SETTING {
+#ifdef RT_BIG_ENDIAN
+ struct {
+         UINT32  rsv:13;
+		 UINT32  EXTCHA:2;
+		 UINT32  HTMODE:1;
+		 UINT32  TRANSNO:2;
+		 UINT32  STBC:1; //SPACE
+		 UINT32  ShortGI:1;
+		 UINT32  BW:1; //channel bandwidth 20MHz or 40 MHz
+		 UINT32  TxBF:1; // 3*3
+		 UINT32  rsv0:10;
+    } field;
+#else
+ struct {
+		 UINT32  rsv0:10;
+		 UINT32  TxBF:1;
+         UINT32  BW:1; //channel bandwidth 20MHz or 40 MHz
+         UINT32  ShortGI:1;
+         UINT32  STBC:1; //SPACE
+         UINT32  TRANSNO:2;
+         UINT32  HTMODE:1;
+         UINT32  EXTCHA:2;
+         UINT32  rsv:13;
+    } field;
+#endif
+ UINT32   word;
+} REG_TRANSMIT_SETTING, *PREG_TRANSMIT_SETTING;
+
+typedef union  _DESIRED_TRANSMIT_SETTING {
+#ifdef RT_BIG_ENDIAN
+	struct	{
+			USHORT		rsv:3;
+			USHORT		FixedTxMode:2;			// If MCS isn't AUTO, fix rate in CCK, OFDM or HT mode.
+			USHORT		PhyMode:4;
+			USHORT   	MCS:7;                 // MCS
+	}	field;
+#else
+	struct	{
+			USHORT   	MCS:7;                 	// MCS
+			USHORT		PhyMode:4;
+			USHORT	 	FixedTxMode:2;			// If MCS isn't AUTO, fix rate in CCK, OFDM or HT mode.
+			USHORT		rsv:3;
+	}	field;
+#endif
+	USHORT		word;
+ } DESIRED_TRANSMIT_SETTING, *PDESIRED_TRANSMIT_SETTING;
+
+typedef struct {
+	BOOLEAN		IsRecipient;
+	UCHAR   MACAddr[MAC_ADDR_LEN];
+	UCHAR   TID;
+	UCHAR   nMSDU;
+	USHORT   TimeOut;
+	BOOLEAN bAllTid;  // If True, delete all TID for BA sessions with this MACaddr.
+} OID_ADD_BA_ENTRY, *POID_ADD_BA_ENTRY;
+
+//
+// Multiple SSID structure
+//
+#define WLAN_MAX_NUM_OF_TIM			((MAX_LEN_OF_MAC_TABLE >> 3) + 1) /* /8 + 1 */
+#define WLAN_CT_TIM_BCMC_OFFSET		0 /* unit: 32B */
+
+/* clear bcmc TIM bit */
+#define WLAN_MR_TIM_BCMC_CLEAR(apidx) \
+	pAd->ApCfg.MBSSID[apidx].TimBitmaps[WLAN_CT_TIM_BCMC_OFFSET] &= ~BIT8[0];
+
+/* set bcmc TIM bit */
+#define WLAN_MR_TIM_BCMC_SET(apidx) \
+	pAd->ApCfg.MBSSID[apidx].TimBitmaps[WLAN_CT_TIM_BCMC_OFFSET] |= BIT8[0];
+
+/* clear a station PS TIM bit */
+#define WLAN_MR_TIM_BIT_CLEAR(ad_p, apidx, wcid) \
+	{	UCHAR tim_offset = wcid >> 3; \
+		UCHAR bit_offset = wcid & 0x7; \
+		ad_p->ApCfg.MBSSID[apidx].TimBitmaps[tim_offset] &= (~BIT8[bit_offset]); }
+
+/* set a station PS TIM bit */
+#define WLAN_MR_TIM_BIT_SET(ad_p, apidx, wcid) \
+	{	UCHAR tim_offset = wcid >> 3; \
+		UCHAR bit_offset = wcid & 0x7; \
+		ad_p->ApCfg.MBSSID[apidx].TimBitmaps[tim_offset] |= BIT8[bit_offset]; }
+
+
+typedef struct _MULTISSID_STRUCT {
+	UCHAR								Bssid[MAC_ADDR_LEN];
+    UCHAR                               SsidLen;
+    CHAR                                Ssid[MAX_LEN_OF_SSID];
+    USHORT                              CapabilityInfo;
+
+    PNET_DEV                   			MSSIDDev;
+
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;
+	NDIS_802_11_WEP_STATUS              WepStatus;
+	NDIS_802_11_WEP_STATUS				GroupKeyWepStatus;
+	WPA_MIX_PAIR_CIPHER					WpaMixPairCipher;
+
+	ULONG								TxCount;
+	ULONG								RxCount;
+	ULONG								ReceivedByteCount;
+	ULONG								TransmittedByteCount;
+	ULONG								RxErrorCount;
+	ULONG								RxDropCount;
+
+	HTTRANSMIT_SETTING					HTPhyMode, MaxHTPhyMode, MinHTPhyMode;// For transmit phy setting in TXWI.
+	RT_HT_PHY_INFO						DesiredHtPhyInfo;
+	DESIRED_TRANSMIT_SETTING        	DesiredTransmitSetting; // Desired transmit setting. this is for reading registry setting only. not useful.
+	BOOLEAN								bAutoTxRateSwitch;
+
+	UCHAR                               DefaultKeyId;
+
+	UCHAR								TxRate;       // RATE_1, RATE_2, RATE_5_5, RATE_11, ...
+	UCHAR     							DesiredRates[MAX_LEN_OF_SUPPORTED_RATES];// OID_802_11_DESIRED_RATES
+	UCHAR								DesiredRatesIndex;
+	UCHAR     							MaxTxRate;            // RATE_1, RATE_2, RATE_5_5, RATE_11
+
+	UCHAR								TimBitmaps[WLAN_MAX_NUM_OF_TIM];
+
+    // WPA
+    UCHAR                               GMK[32];
+    UCHAR                               PMK[32];
+	UCHAR								GTK[32];
+    BOOLEAN                             IEEE8021X;
+    BOOLEAN                             PreAuth;
+    UCHAR                               GNonce[32];
+    UCHAR                               PortSecured;
+    NDIS_802_11_PRIVACY_FILTER          PrivacyFilter;
+    UCHAR                               BANClass3Data;
+    ULONG                               IsolateInterStaTraffic;
+
+    UCHAR                               RSNIE_Len[2];
+    UCHAR                               RSN_IE[2][MAX_LEN_OF_RSNIE];
+
+
+    UCHAR                   			TimIELocationInBeacon;
+    UCHAR                   			CapabilityInfoLocationInBeacon;
+    // outgoing BEACON frame buffer and corresponding TXWI
+	// PTXWI_STRUC                           BeaconTxWI; //
+    CHAR                                BeaconBuf[MAX_BEACON_SIZE]; // NOTE: BeaconBuf should be 4-byte aligned
+
+    BOOLEAN                             bHideSsid;
+	UINT16								StationKeepAliveTime; // unit: second
+
+    USHORT                              VLAN_VID;
+    USHORT                              VLAN_Priority;
+
+    RT_802_11_ACL						AccessControlList;
+
+	// EDCA Qos
+    BOOLEAN								bWmmCapable;	// 0:disable WMM, 1:enable WMM
+    BOOLEAN								bDLSCapable;	// 0:disable DLS, 1:enable DLS
+
+	UCHAR           					DlsPTK[64];		// Due to windows dirver count on meetinghouse to handle 4-way shake
+
+	// For 802.1x daemon setting per BSS
+	UCHAR								radius_srv_num;
+	RADIUS_SRV_INFO						radius_srv_info[MAX_RADIUS_SRV_NUM];
+
+#ifdef RTL865X_SOC
+	unsigned int						mylinkid;
+#endif
+
+
+	UINT32					RcvdConflictSsidCount;
+	UINT32					RcvdSpoofedAssocRespCount;
+	UINT32					RcvdSpoofedReassocRespCount;
+	UINT32					RcvdSpoofedProbeRespCount;
+	UINT32					RcvdSpoofedBeaconCount;
+	UINT32					RcvdSpoofedDisassocCount;
+	UINT32					RcvdSpoofedAuthCount;
+	UINT32					RcvdSpoofedDeauthCount;
+	UINT32					RcvdSpoofedUnknownMgmtCount;
+	UINT32					RcvdReplayAttackCount;
+
+	CHAR					RssiOfRcvdConflictSsid;
+	CHAR					RssiOfRcvdSpoofedAssocResp;
+	CHAR					RssiOfRcvdSpoofedReassocResp;
+	CHAR					RssiOfRcvdSpoofedProbeResp;
+	CHAR					RssiOfRcvdSpoofedBeacon;
+	CHAR					RssiOfRcvdSpoofedDisassoc;
+	CHAR					RssiOfRcvdSpoofedAuth;
+	CHAR					RssiOfRcvdSpoofedDeauth;
+	CHAR					RssiOfRcvdSpoofedUnknownMgmt;
+	CHAR					RssiOfRcvdReplayAttack;
+
+	BOOLEAN					bBcnSntReq;
+	UCHAR					BcnBufIdx;
+} MULTISSID_STRUCT, *PMULTISSID_STRUCT;
+
+
+
+#ifdef DOT11N_DRAFT3
+typedef enum _BSS2040COEXIST_FLAG{
+	BSS_2040_COEXIST_DISABLE = 0,
+	BSS_2040_COEXIST_TIMER_FIRED  = 1,
+	BSS_2040_COEXIST_INFO_SYNC = 2,
+	BSS_2040_COEXIST_INFO_NOTIFY = 4,
+}BSS2040COEXIST_FLAG;
+#endif // DOT11N_DRAFT3 //
+
+// configuration common to OPMODE_AP as well as OPMODE_STA
+typedef struct _COMMON_CONFIG {
+
+	BOOLEAN		bCountryFlag;
+	UCHAR		CountryCode[3];
+	UCHAR		Geography;
+	UCHAR       CountryRegion;      // Enum of country region, 0:FCC, 1:IC, 2:ETSI, 3:SPAIN, 4:France, 5:MKK, 6:MKK1, 7:Israel
+	UCHAR       CountryRegionForABand;	// Enum of country region for A band
+	UCHAR       PhyMode;            // PHY_11A, PHY_11B, PHY_11BG_MIXED, PHY_ABG_MIXED
+	USHORT      Dsifs;              // in units of usec
+	ULONG       PacketFilter;       // Packet filter for receiving
+
+	CHAR        Ssid[MAX_LEN_OF_SSID]; // NOT NULL-terminated
+	UCHAR       SsidLen;               // the actual ssid length in used
+	UCHAR       LastSsidLen;               // the actual ssid length in used
+	CHAR        LastSsid[MAX_LEN_OF_SSID]; // NOT NULL-terminated
+	UCHAR		LastBssid[MAC_ADDR_LEN];
+
+	UCHAR       Bssid[MAC_ADDR_LEN];
+	USHORT      BeaconPeriod;
+	UCHAR       Channel;
+	UCHAR       CentralChannel;    	// Central Channel when using 40MHz is indicating. not real channel.
+
+	UCHAR       SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       SupRateLen;
+	UCHAR       ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       ExtRateLen;
+	UCHAR       DesireRate[MAX_LEN_OF_SUPPORTED_RATES];      // OID_802_11_DESIRED_RATES
+	UCHAR       MaxDesiredRate;
+	UCHAR       ExpectedACKRate[MAX_LEN_OF_SUPPORTED_RATES];
+
+	ULONG       BasicRateBitmap;        // backup basic ratebitmap
+
+	BOOLEAN		bAPSDCapable;
+	BOOLEAN		bInServicePeriod;
+	BOOLEAN		bAPSDAC_BE;
+	BOOLEAN		bAPSDAC_BK;
+	BOOLEAN		bAPSDAC_VI;
+	BOOLEAN		bAPSDAC_VO;
+	BOOLEAN		bNeedSendTriggerFrame;
+	BOOLEAN		bAPSDForcePowerSave;	// Force power save mode, should only use in APSD-STAUT
+	ULONG		TriggerTimerCount;
+	UCHAR		MaxSPLength;
+	UCHAR		BBPCurrentBW;	// BW_10, 	BW_20, BW_40
+	REG_TRANSMIT_SETTING        RegTransmitSetting; //registry transmit setting. this is for reading registry setting only. not useful.
+	UCHAR       TxRate;                 // Same value to fill in TXD. TxRate is 6-bit
+	UCHAR       MaxTxRate;              // RATE_1, RATE_2, RATE_5_5, RATE_11
+	UCHAR       TxRateIndex;            // Tx rate index in RateSwitchTable
+	UCHAR       TxRateTableSize;        // Valid Tx rate table size in RateSwitchTable
+	UCHAR       MinTxRate;              // RATE_1, RATE_2, RATE_5_5, RATE_11
+	UCHAR       RtsRate;                // RATE_xxx
+	HTTRANSMIT_SETTING	MlmeTransmit;   // MGMT frame PHY rate setting when operatin at Ht rate.
+	UCHAR       MlmeRate;               // RATE_xxx, used to send MLME frames
+	UCHAR       BasicMlmeRate;          // Default Rate for sending MLME frames
+
+	USHORT      RtsThreshold;           // in unit of BYTE
+	USHORT      FragmentThreshold;      // in unit of BYTE
+
+	UCHAR       TxPower;                // in unit of mW
+	ULONG       TxPowerPercentage;      // 0~100 %
+	ULONG       TxPowerDefault;         // keep for TxPowerPercentage
+
+#ifdef DOT11_N_SUPPORT
+	BACAP_STRUC        BACapability; //   NO USE = 0XFF  ;  IMMED_BA =1  ;  DELAY_BA=0
+	BACAP_STRUC        REGBACapability; //   NO USE = 0XFF  ;  IMMED_BA =1  ;  DELAY_BA=0
+#endif // DOT11_N_SUPPORT //
+	IOT_STRUC		IOTestParm;	// 802.11n InterOpbility Test Parameter;
+	ULONG       TxPreamble;             // Rt802_11PreambleLong, Rt802_11PreambleShort, Rt802_11PreambleAuto
+	BOOLEAN     bUseZeroToDisableFragment;     // Microsoft use 0 as disable
+	ULONG       UseBGProtection;        // 0: auto, 1: always use, 2: always not use
+	BOOLEAN     bUseShortSlotTime;      // 0: disable, 1 - use short slot (9us)
+	BOOLEAN     bEnableTxBurst;         // 1: enble TX PACKET BURST, 0: disable TX PACKET BURST
+	BOOLEAN     bAggregationCapable;      // 1: enable TX aggregation when the peer supports it
+	BOOLEAN     bPiggyBackCapable;		// 1: enable TX piggy-back according MAC's version
+	BOOLEAN     bIEEE80211H;			// 1: enable IEEE802.11h spec.
+	ULONG		DisableOLBCDetect;		// 0: enable OLBC detect; 1 disable OLBC detect
+
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN				bRdg;
+#endif // DOT11_N_SUPPORT //
+	BOOLEAN             bWmmCapable;        // 0:disable WMM, 1:enable WMM
+	QOS_CAPABILITY_PARM APQosCapability;    // QOS capability of the current associated AP
+	EDCA_PARM           APEdcaParm;         // EDCA parameters of the current associated AP
+	QBSS_LOAD_PARM      APQbssLoad;         // QBSS load of the current associated AP
+	UCHAR               AckPolicy[4];       // ACK policy of the specified AC. see ACK_xxx
+#ifdef CONFIG_STA_SUPPORT
+	BOOLEAN				bDLSCapable;		// 0:disable DLS, 1:enable DLS
+#endif // CONFIG_STA_SUPPORT //
+	// a bitmap of BOOLEAN flags. each bit represent an operation status of a particular
+	// BOOLEAN control, either ON or OFF. These flags should always be accessed via
+	// OPSTATUS_TEST_FLAG(), OPSTATUS_SET_FLAG(), OP_STATUS_CLEAR_FLAG() macros.
+	// see fOP_STATUS_xxx in RTMP_DEF.C for detail bit definition
+	ULONG               OpStatusFlags;
+
+	BOOLEAN				NdisRadioStateOff; //For HCT 12.0, set this flag to TRUE instead of called MlmeRadioOff.
+	ABGBAND_STATE		BandState;		// For setting BBP used on B/G or A mode.
+
+	// IEEE802.11H--DFS.
+	RADAR_DETECT_STRUCT	RadarDetect;
+
+#ifdef CARRIER_DETECTION_SUPPORT
+	CARRIER_DETECTION		CarrierDetect;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	// HT
+	UCHAR			BASize;		// USer desired BAWindowSize. Should not exceed our max capability
+	//RT_HT_CAPABILITY	SupportedHtPhy;
+	RT_HT_CAPABILITY	DesiredHtPhy;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHTInfo;	// Useful as AP.
+	//This IE is used with channel switch announcement element when changing to a new 40MHz.
+	//This IE is included in channel switch ammouncement frames 7.4.1.5, beacons, probe Rsp.
+	NEW_EXT_CHAN_IE	NewExtChanOffset;	//7.3.2.20A, 1 if extension channel is above the control channel, 3 if below, 0 if not present
+
+#ifdef DOT11N_DRAFT3
+	UCHAR					Bss2040CoexistFlag;		// bit 0: bBssCoexistTimerRunning, bit 1: NeedSyncAddHtInfo.
+	RALINK_TIMER_STRUCT	Bss2040CoexistTimer;
+
+	//This IE is used for 20/40 BSS Coexistence.
+	BSS_2040_COEXIST_IE		BSS2040CoexistInfo;
+	// ====== 11n D3.0 =======================>
+	USHORT					Dot11OBssScanPassiveDwell;				// Unit : TU. 5~1000
+	USHORT					Dot11OBssScanActiveDwell;				// Unit : TU. 10~1000
+	USHORT					Dot11BssWidthTriggerScanInt;			// Unit : Second
+	USHORT					Dot11OBssScanPassiveTotalPerChannel;	// Unit : TU. 200~10000
+	USHORT					Dot11OBssScanActiveTotalPerChannel;	// Unit : TU. 20~10000
+	USHORT					Dot11BssWidthChanTranDelayFactor;
+	USHORT					Dot11OBssScanActivityThre;				// Unit : percentage
+
+	ULONG					Dot11BssWidthChanTranDelay;			// multiple of (Dot11BssWidthTriggerScanInt * Dot11BssWidthChanTranDelayFactor)
+	ULONG					CountDownCtr;	// CountDown Counter from (Dot11BssWidthTriggerScanInt * Dot11BssWidthChanTranDelayFactor)
+
+	NDIS_SPIN_LOCK          TriggerEventTabLock;
+	BSS_2040_COEXIST_IE		LastBSSCoexist2040;
+	BSS_2040_COEXIST_IE		BSSCoexist2040;
+	TRIGGER_EVENT_TAB		TriggerEventTab;
+	UCHAR					ChannelListIdx;
+	// <====== 11n D3.0 =======================
+	BOOLEAN					bOverlapScanning;
+#endif // DOT11N_DRAFT3 //
+
+    BOOLEAN                 bHTProtect;
+    BOOLEAN                 bMIMOPSEnable;
+    BOOLEAN					bBADecline;
+	BOOLEAN					bDisableReordering;
+	BOOLEAN					bForty_Mhz_Intolerant;
+	BOOLEAN					bExtChannelSwitchAnnouncement;
+	BOOLEAN					bRcvBSSWidthTriggerEvents;
+	ULONG					LastRcvBSSWidthTriggerEventsTime;
+
+	UCHAR					TxBASize;
+#endif // DOT11_N_SUPPORT //
+
+	// Enable wireless event
+	BOOLEAN				bWirelessEvent;
+	BOOLEAN				bWiFiTest;				// Enable this parameter for WiFi test
+
+	// Tx & Rx Stream number selection
+	UCHAR				TxStream;
+	UCHAR				RxStream;
+
+	// transmit phy mode, trasmit rate for Multicast.
+#ifdef MCAST_RATE_SPECIFIC
+	UCHAR				McastTransmitMcs;
+	UCHAR				McastTransmitPhyMode;
+#endif // MCAST_RATE_SPECIFIC //
+
+	BOOLEAN     		bHardwareRadio;     // Hardware controlled Radio enabled
+
+
+
+ 	NDIS_SPIN_LOCK			MeasureReqTabLock;
+	PMEASURE_REQ_TAB		pMeasureReqTab;
+
+	NDIS_SPIN_LOCK			TpcReqTabLock;
+	PTPC_REQ_TAB			pTpcReqTab;
+
+	// transmit phy mode, trasmit rate for Multicast.
+#ifdef MCAST_RATE_SPECIFIC
+	HTTRANSMIT_SETTING		MCastPhyMode;
+#endif // MCAST_RATE_SPECIFIC //
+
+#ifdef SINGLE_SKU
+	UINT16					DefineMaxTxPwr;
+#endif // SINGLE_SKU //
+
+
+} COMMON_CONFIG, *PCOMMON_CONFIG;
+
+
+#ifdef CONFIG_STA_SUPPORT
+/* Modified by Wu Xi-Kun 4/21/2006 */
+// STA configuration and status
+typedef struct _STA_ADMIN_CONFIG {
+	// GROUP 1 -
+	//   User configuration loaded from Registry, E2PROM or OID_xxx. These settings describe
+	//   the user intended configuration, but not necessary fully equal to the final
+	//   settings in ACTIVE BSS after negotiation/compromize with the BSS holder (either
+	//   AP or IBSS holder).
+	//   Once initialized, user configuration can only be changed via OID_xxx
+	UCHAR       BssType;              // BSS_INFRA or BSS_ADHOC
+	USHORT      AtimWin;          // used when starting a new IBSS
+
+	// GROUP 2 -
+	//   User configuration loaded from Registry, E2PROM or OID_xxx. These settings describe
+	//   the user intended configuration, and should be always applied to the final
+	//   settings in ACTIVE BSS without compromising with the BSS holder.
+	//   Once initialized, user configuration can only be changed via OID_xxx
+	UCHAR       RssiTrigger;
+	UCHAR       RssiTriggerMode;      // RSSI_TRIGGERED_UPON_BELOW_THRESHOLD or RSSI_TRIGGERED_UPON_EXCCEED_THRESHOLD
+	USHORT      DefaultListenCount;   // default listen count;
+	ULONG       WindowsPowerMode;           // Power mode for AC power
+	ULONG       WindowsBatteryPowerMode;    // Power mode for battery if exists
+	BOOLEAN     bWindowsACCAMEnable;        // Enable CAM power mode when AC on
+	BOOLEAN     bAutoReconnect;         // Set to TRUE when setting OID_802_11_SSID with no matching BSSID
+	ULONG       WindowsPowerProfile;    // Windows power profile, for NDIS5.1 PnP
+
+	// MIB:ieee802dot11.dot11smt(1).dot11StationConfigTable(1)
+	USHORT      Psm;                  // power management mode   (PWR_ACTIVE|PWR_SAVE)
+	USHORT      DisassocReason;
+	UCHAR       DisassocSta[MAC_ADDR_LEN];
+	USHORT      DeauthReason;
+	UCHAR       DeauthSta[MAC_ADDR_LEN];
+	USHORT      AuthFailReason;
+	UCHAR       AuthFailSta[MAC_ADDR_LEN];
+
+	NDIS_802_11_PRIVACY_FILTER          PrivacyFilter;  // PrivacyFilter enum for 802.1X
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;       // This should match to whatever microsoft defined
+	NDIS_802_11_WEP_STATUS              WepStatus;
+	NDIS_802_11_WEP_STATUS				OrigWepStatus;	// Original wep status set from OID
+
+	// Add to support different cipher suite for WPA2/WPA mode
+	NDIS_802_11_ENCRYPTION_STATUS		GroupCipher;		// Multicast cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS		PairCipher;			// Unicast cipher suite
+	BOOLEAN								bMixCipher;			// Indicate current Pair & Group use different cipher suites
+	USHORT								RsnCapability;
+
+	NDIS_802_11_WEP_STATUS              GroupKeyWepStatus;
+
+	UCHAR		PMK[32];                // WPA PSK mode PMK
+	UCHAR       PTK[64];                // WPA PSK mode PTK
+	UCHAR		GTK[32];				// GTK from authenticator
+	BSSID_INFO	SavedPMK[PMKID_NO];
+	UINT		SavedPMKNum;			// Saved PMKID number
+
+	UCHAR		DefaultKeyId;
+
+
+	// WPA 802.1x port control, WPA_802_1X_PORT_SECURED, WPA_802_1X_PORT_NOT_SECURED
+	UCHAR       PortSecured;
+
+	// For WPA countermeasures
+	ULONG       LastMicErrorTime;   // record last MIC error time
+	ULONG       MicErrCnt;          // Should be 0, 1, 2, then reset to zero (after disassoiciation).
+	BOOLEAN     bBlockAssoc;        // Block associate attempt for 60 seconds after counter measure occurred.
+	// For WPA-PSK supplicant state
+	WPA_STATE   WpaState;           // Default is SS_NOTUSE and handled by microsoft 802.1x
+	UCHAR       ReplayCounter[8];
+	UCHAR       ANonce[32];         // ANonce for WPA-PSK from aurhenticator
+	UCHAR       SNonce[32];         // SNonce for WPA-PSK
+
+	UCHAR       LastSNR0;             // last received BEACON's SNR
+	UCHAR       LastSNR1;            // last received BEACON's SNR for 2nd  antenna
+	RSSI_SAMPLE RssiSample;
+	ULONG       NumOfAvgRssiSample;
+
+	ULONG       LastBeaconRxTime;     // OS's timestamp of the last BEACON RX time
+	ULONG       Last11bBeaconRxTime;  // OS's timestamp of the last 11B BEACON RX time
+	ULONG		Last11gBeaconRxTime;	// OS's timestamp of the last 11G BEACON RX time
+	ULONG		Last20NBeaconRxTime;	// OS's timestamp of the last 20MHz N BEACON RX time
+
+	ULONG       LastScanTime;       // Record last scan time for issue BSSID_SCAN_LIST
+	ULONG       ScanCnt;            // Scan counts since most recent SSID, BSSID, SCAN OID request
+	BOOLEAN     bSwRadio;           // Software controlled Radio On/Off, TRUE: On
+	BOOLEAN     bHwRadio;           // Hardware controlled Radio On/Off, TRUE: On
+	BOOLEAN     bRadio;             // Radio state, And of Sw & Hw radio state
+	BOOLEAN     bHardwareRadio;     // Hardware controlled Radio enabled
+	BOOLEAN     bShowHiddenSSID;    // Show all known SSID in SSID list get operation
+
+    BOOLEAN		AdhocBOnlyJoined;	// Indicate Adhoc B Join.
+    BOOLEAN		AdhocBGJoined;		// Indicate Adhoc B/G Join.
+    BOOLEAN		Adhoc20NJoined;		// Indicate Adhoc 20MHz N Join.
+
+	// New for WPA, windows want us to to keep association information and
+	// Fixed IEs from last association response
+	NDIS_802_11_ASSOCIATION_INFORMATION     AssocInfo;
+	USHORT       ReqVarIELen;                // Length of next VIE include EID & Length
+	UCHAR       ReqVarIEs[MAX_VIE_LEN];		// The content saved here should be little-endian format.
+	USHORT       ResVarIELen;                // Length of next VIE include EID & Length
+	UCHAR       ResVarIEs[MAX_VIE_LEN];
+
+	UCHAR       RSNIE_Len;
+	UCHAR       RSN_IE[MAX_LEN_OF_RSNIE];	// The content saved here should be little-endian format.
+
+	// New variables used for CCX 1.0
+	BOOLEAN             bCkipOn;
+	BOOLEAN             bCkipCmicOn;
+	UCHAR               CkipFlag;
+	UCHAR               GIV[3];  //for CCX iv
+	UCHAR               RxSEQ[4];
+	UCHAR               TxSEQ[4];
+	UCHAR               CKIPMIC[4];
+	UCHAR               LeapAuthMode;
+	LEAP_AUTH_INFO      LeapAuthInfo;
+	UCHAR               HashPwd[16];
+	UCHAR               NetworkChallenge[8];
+	UCHAR               NetworkChallengeResponse[24];
+	UCHAR               PeerChallenge[8];
+
+	UCHAR               PeerChallengeResponse[24];
+	UCHAR               SessionKey[16]; //Network session keys (NSK)
+	RALINK_TIMER_STRUCT LeapAuthTimer;
+	ROGUEAP_TABLE       RogueApTab;   //Cisco CCX1 Rogue AP Detection
+
+	// New control flags for CCX
+	CCX_CONTROL         CCXControl;                 // Master administration state
+	BOOLEAN             CCXEnable;                  // Actual CCX state
+	UCHAR               CCXScanChannel;             // Selected channel for CCX beacon request
+	USHORT              CCXScanTime;                // Time out to wait for beacon and probe response
+	UCHAR               CCXReqType;                 // Current processing CCX request type
+	BSS_TABLE           CCXBssTab;                  // BSS Table
+	UCHAR               FrameReportBuf[2048];       // Buffer for creating frame report
+	USHORT              FrameReportLen;             // Current Frame report length
+	ULONG               CLBusyBytes;                // Save the total bytes received durning channel load scan time
+	USHORT              RPIDensity[8];              // Array for RPI density collection
+	// Start address of each BSS table within FrameReportBuf
+	// It's important to update the RxPower of the corresponding Bss
+	USHORT              BssReportOffset[MAX_LEN_OF_BSS_TABLE];
+	USHORT              BeaconToken;                // Token for beacon report
+	ULONG               LastBssIndex;               // Most current reported Bss index
+	RM_REQUEST_ACTION   MeasurementRequest[16];     // Saved measurement request
+	UCHAR               RMReqCnt;                   // Number of measurement request saved.
+	UCHAR               CurrentRMReqIdx;            // Number of measurement request saved.
+	BOOLEAN             ParallelReq;                // Parallel measurement, only one request performed,
+													// It must be the same channel with maximum duration
+	USHORT              ParallelDuration;           // Maximum duration for parallel measurement
+	UCHAR               ParallelChannel;            // Only one channel with parallel measurement
+	USHORT              IAPPToken;                  // IAPP dialog token
+	UCHAR               CCXQosECWMin;               // Cisco QOS ECWMin for AC 0
+	UCHAR               CCXQosECWMax;               // Cisco QOS ECWMax for AC 0
+	// Hack for channel load and noise histogram parameters
+	UCHAR               NHFactor;                   // Parameter for Noise histogram
+	UCHAR               CLFactor;                   // Parameter for channel load
+
+	UCHAR               KRK[16];        //Key Refresh Key.
+	UCHAR               BTK[32];        //Base Transient Key
+	BOOLEAN             CCKMLinkUpFlag;
+	ULONG               CCKMRN;    //(Re)Association request number.
+	LARGE_INTEGER       CCKMBeaconAtJoinTimeStamp;  //TSF timer for Re-assocaite to the new AP
+	UCHAR               AironetCellPowerLimit;      //in dBm
+	UCHAR               AironetIPAddress[4];        //eg. 192.168.1.1
+	BOOLEAN             CCXAdjacentAPReportFlag;    //flag for determining report Assoc Lost time
+	CHAR                CCXAdjacentAPSsid[MAX_LEN_OF_SSID]; //Adjacent AP's SSID report
+	UCHAR               CCXAdjacentAPSsidLen;               // the actual ssid length in used
+	UCHAR               CCXAdjacentAPBssid[MAC_ADDR_LEN];         //Adjacent AP's BSSID report
+	USHORT              CCXAdjacentAPChannel;
+	ULONG               CCXAdjacentAPLinkDownTime;  //for Spec S32.
+
+	RALINK_TIMER_STRUCT	StaQuickResponeForRateUpTimer;
+	BOOLEAN				StaQuickResponeForRateUpTimerRunning;
+
+	UCHAR           	DtimCount;      // 0.. DtimPeriod-1
+	UCHAR           	DtimPeriod;     // default = 3
+
+#ifdef QOS_DLS_SUPPORT
+	RT_802_11_DLS		DLSEntry[MAX_NUM_OF_DLS_ENTRY];
+	UCHAR				DlsReplayCounter[8];
+#endif // QOS_DLS_SUPPORT //
+	////////////////////////////////////////////////////////////////////////////////////////
+	// This is only for WHQL test.
+	BOOLEAN				WhqlTest;
+	////////////////////////////////////////////////////////////////////////////////////////
+
+    RALINK_TIMER_STRUCT WpaDisassocAndBlockAssocTimer;
+    // Fast Roaming
+	BOOLEAN		        bFastRoaming;       // 0:disable fast roaming, 1:enable fast roaming
+	CHAR		        dBmToRoam;          // the condition to roam when receiving Rssi less than this value. It's negative value.
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    BOOLEAN             IEEE8021X;
+    BOOLEAN             IEEE8021x_required_keys;
+    CIPHER_KEY	        DesireSharedKey[4];	// Record user desired WEP keys
+    UCHAR               DesireSharedKeyId;
+
+    // 0: driver ignores wpa_supplicant
+    // 1: wpa_supplicant initiates scanning and AP selection
+    // 2: driver takes care of scanning, AP selection, and IEEE 802.11 association parameters
+    UCHAR               WpaSupplicantUP;
+	UCHAR				WpaSupplicantScanCount;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+    CHAR                dev_name[16];
+    USHORT              OriDevType;
+
+    BOOLEAN             bTGnWifiTest;
+	BOOLEAN			    bScanReqIsFromWebUI;
+
+	HTTRANSMIT_SETTING				HTPhyMode, MaxHTPhyMode, MinHTPhyMode;// For transmit phy setting in TXWI.
+	DESIRED_TRANSMIT_SETTING       	DesiredTransmitSetting;
+	RT_HT_PHY_INFO					DesiredHtPhyInfo;
+	BOOLEAN							bAutoTxRateSwitch;
+
+#ifdef RT2860
+    UCHAR       BBPR3;
+#endif // RT2860 //
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	UCHAR				IEEE80211dClientMode;
+	UCHAR				StaOriCountryCode[3];
+	UCHAR				StaOriGeography;
+#endif // EXT_BUILD_CHANNEL_LIST //
+} STA_ADMIN_CONFIG, *PSTA_ADMIN_CONFIG;
+
+// This data structure keep the current active BSS/IBSS's configuration that this STA
+// had agreed upon joining the network. Which means these parameters are usually decided
+// by the BSS/IBSS creator instead of user configuration. Data in this data structurre
+// is valid only when either ADHOC_ON(pAd) or INFRA_ON(pAd) is TRUE.
+// Normally, after SCAN or failed roaming attempts, we need to recover back to
+// the current active settings.
+typedef struct _STA_ACTIVE_CONFIG {
+	USHORT      Aid;
+	USHORT      AtimWin;                // in kusec; IBSS parameter set element
+	USHORT      CapabilityInfo;
+	USHORT      CfpMaxDuration;
+	USHORT      CfpPeriod;
+
+	// Copy supported rate from desired AP's beacon. We are trying to match
+	// AP's supported and extended rate settings.
+	UCHAR       SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       SupRateLen;
+	UCHAR       ExtRateLen;
+	// Copy supported ht from desired AP's beacon. We are trying to match
+	RT_HT_PHY_INFO		SupportedPhyInfo;
+	RT_HT_CAPABILITY	SupportedHtPhy;
+} STA_ACTIVE_CONFIG, *PSTA_ACTIVE_CONFIG;
+#endif // CONFIG_STA_SUPPORT //
+
+// ----------- start of AP --------------------------
+// AUTH-RSP State Machine Aux data structure
+typedef struct _AP_MLME_AUX {
+	UCHAR               Addr[MAC_ADDR_LEN];
+	USHORT              Alg;
+	CHAR                Challenge[CIPHER_TEXT_LEN];
+} AP_MLME_AUX, *PAP_MLME_AUX;
+
+// structure to define WPA Group Key Rekey Interval
+typedef struct PACKED _RT_802_11_WPA_REKEY {
+	ULONG ReKeyMethod;          // mechanism for rekeying: 0:disable, 1: time-based, 2: packet-based
+	ULONG ReKeyInterval;        // time-based: seconds, packet-based: kilo-packets
+} RT_WPA_REKEY,*PRT_WPA_REKEY, RT_802_11_WPA_REKEY, *PRT_802_11_WPA_REKEY;
+
+typedef struct _MAC_TABLE_ENTRY {
+	//Choose 1 from ValidAsWDS and ValidAsCLI  to validize.
+	BOOLEAN		ValidAsCLI;		// Sta mode, set this TRUE after Linkup,too.
+	BOOLEAN		ValidAsWDS;	// This is WDS Entry. only for AP mode.
+	BOOLEAN		ValidAsApCli;   //This is a AP-Client entry, only for AP mode which enable AP-Client functions.
+	BOOLEAN		ValidAsMesh;
+	BOOLEAN		ValidAsDls;	// This is DLS Entry. only for STA mode.
+	BOOLEAN		isCached;
+	BOOLEAN		bIAmBadAtheros;	// Flag if this is Atheros chip that has IOT problem.  We need to turn on RTS/CTS protection.
+
+	UCHAR         	EnqueueEapolStartTimerRunning;  // Enqueue EAPoL-Start for triggering EAP SM
+	//jan for wpa
+	// record which entry revoke MIC Failure , if it leaves the BSS itself, AP won't update aMICFailTime MIB
+	UCHAR           CMTimerRunning;
+	UCHAR           apidx;			// MBSS number
+	UCHAR           RSNIE_Len;
+	UCHAR           RSN_IE[MAX_LEN_OF_RSNIE];
+	UCHAR           ANonce[LEN_KEY_DESC_NONCE];
+	UCHAR           R_Counter[LEN_KEY_DESC_REPLAY];
+	UCHAR           PTK[64];
+	UCHAR           ReTryCounter;
+	RALINK_TIMER_STRUCT                 RetryTimer;
+	RALINK_TIMER_STRUCT					EnqueueStartForPSKTimer;	// A timer which enqueue EAPoL-Start for triggering PSK SM
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;   // This should match to whatever microsoft defined
+	NDIS_802_11_WEP_STATUS              WepStatus;
+	AP_WPA_STATE    WpaState;
+	GTK_STATE       GTKState;
+	USHORT          PortSecured;
+	NDIS_802_11_PRIVACY_FILTER  PrivacyFilter;      // PrivacyFilter enum for 802.1X
+	CIPHER_KEY      PairwiseKey;
+	PVOID           pAd;
+    INT				PMKID_CacheIdx;
+    UCHAR			PMKID[LEN_PMKID];
+
+
+	UCHAR           Addr[MAC_ADDR_LEN];
+	UCHAR           PsMode;
+	SST             Sst;
+	AUTH_STATE      AuthState; // for SHARED KEY authentication state machine used only
+	BOOLEAN			IsReassocSta;	// Indicate whether this is a reassociation procedure
+	USHORT          Aid;
+	USHORT          CapabilityInfo;
+	UCHAR           LastRssi;
+	ULONG           NoDataIdleCount;
+	UINT16			StationKeepAliveCount; // unit: second
+	ULONG           PsQIdleCount;
+	QUEUE_HEADER    PsQueue;
+
+	UINT32			StaConnectTime;		// the live time of this station since associated with AP
+
+
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN			bSendBAR;
+	USHORT			NoBADataCountDown;
+
+	UINT32   		CachedBuf[16];		// UINT (4 bytes) for alignment
+	UINT			TxBFCount; // 3*3
+#endif // DOT11_N_SUPPORT //
+	UINT			FIFOCount;
+	UINT			DebugFIFOCount;
+	UINT			DebugTxCount;
+    BOOLEAN			bDlsInit;
+
+
+//====================================================
+//WDS entry needs these
+// rt2860 add this. if ValidAsWDS==TRUE, MatchWDSTabIdx is the index in WdsTab.MacTab
+	UINT			MatchWDSTabIdx;
+	UCHAR           MaxSupportedRate;
+	UCHAR           CurrTxRate;
+	UCHAR           CurrTxRateIndex;
+	// to record the each TX rate's quality. 0 is best, the bigger the worse.
+	USHORT          TxQuality[MAX_STEP_OF_TX_RATE_SWITCH];
+	UINT32			OneSecTxNoRetryOkCount;
+	UINT32          OneSecTxRetryOkCount;
+	UINT32          OneSecTxFailCount;
+	UINT32			ContinueTxFailCnt;
+	UINT32          CurrTxRateStableTime; // # of second in current TX rate
+	UCHAR           TxRateUpPenalty;      // extra # of second penalty due to last unstable condition
+//====================================================
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+	UINT			MatchDlsEntryIdx; // indicate the index in pAd->StaCfg.DLSEntry
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+	BOOLEAN         fNoisyEnvironment;
+	BOOLEAN			fLastSecAccordingRSSI;
+	UCHAR           LastSecTxRateChangeAction; // 0: no change, 1:rate UP, 2:rate down
+	CHAR			LastTimeTxRateChangeAction; //Keep last time value of LastSecTxRateChangeAction
+	ULONG			LastTxOkCount;
+	UCHAR           PER[MAX_STEP_OF_TX_RATE_SWITCH];
+
+	// a bitmap of BOOLEAN flags. each bit represent an operation status of a particular
+	// BOOLEAN control, either ON or OFF. These flags should always be accessed via
+	// CLIENT_STATUS_TEST_FLAG(), CLIENT_STATUS_SET_FLAG(), CLIENT_STATUS_CLEAR_FLAG() macros.
+	// see fOP_STATUS_xxx in RTMP_DEF.C for detail bit definition. fCLIENT_STATUS_AMSDU_INUSED
+	ULONG           ClientStatusFlags;
+
+	// TODO: Shall we move that to DOT11_N_SUPPORT???
+	HTTRANSMIT_SETTING	HTPhyMode, MaxHTPhyMode, MinHTPhyMode;// For transmit phy setting in TXWI.
+
+#ifdef DOT11_N_SUPPORT
+	// HT EWC MIMO-N used parameters
+	USHORT		RXBAbitmap;	// fill to on-chip  RXWI_BA_BITMASK in 8.1.3RX attribute entry format
+	USHORT		TXBAbitmap;	// This bitmap as originator, only keep in software used to mark AMPDU bit in TXWI
+	USHORT		TXAutoBAbitmap;
+	USHORT		BADeclineBitmap;
+	USHORT		BARecWcidArray[NUM_OF_TID];	// The mapping wcid of recipient session. if RXBAbitmap bit is masked
+	USHORT		BAOriWcidArray[NUM_OF_TID]; // The mapping wcid of originator session. if TXBAbitmap bit is masked
+	USHORT		BAOriSequence[NUM_OF_TID]; // The mapping wcid of originator session. if TXBAbitmap bit is masked
+
+	// 802.11n features.
+	UCHAR		MpduDensity;
+	UCHAR		MaxRAmpduFactor;
+	UCHAR		AMsduSize;
+	UCHAR		MmpsMode;	// MIMO power save more.
+
+	HT_CAPABILITY_IE		HTCapability;
+
+#ifdef DOT11N_DRAFT3
+	UCHAR		BSS2040CoexistenceMgmtSupport;
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+	BOOLEAN		bAutoTxRateSwitch;
+
+	UCHAR       RateLen;
+	struct _MAC_TABLE_ENTRY *pNext;
+    USHORT      TxSeq[NUM_OF_TID];
+	USHORT		NonQosDataSeq;
+
+	RSSI_SAMPLE	RssiSample;
+
+	UINT32			TXMCSExpected[16];
+	UINT32			TXMCSSuccessful[16];
+	UINT32			TXMCSFailed[16];
+	UINT32			TXMCSAutoFallBack[16][16];
+} MAC_TABLE_ENTRY, *PMAC_TABLE_ENTRY;
+
+typedef struct _MAC_TABLE {
+	USHORT			Size;
+	MAC_TABLE_ENTRY *Hash[HASH_TABLE_SIZE];
+	MAC_TABLE_ENTRY Content[MAX_LEN_OF_MAC_TABLE];
+	QUEUE_HEADER    McastPsQueue;
+	ULONG           PsQIdleCount;
+	BOOLEAN         fAnyStationInPsm;
+	BOOLEAN         fAnyStationBadAtheros;	// Check if any Station is atheros 802.11n Chip.  We need to use RTS/CTS with Atheros 802,.11n chip.
+	BOOLEAN			fAnyTxOPForceDisable;	// Check if it is necessary to disable BE TxOP
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN         fAnyStationIsLegacy;	// Check if I use legacy rate to transmit to my BSS Station/
+	BOOLEAN         fAnyStationNonGF;		// Check if any Station can't support GF.
+	BOOLEAN         fAnyStation20Only;		// Check if any Station can't support GF.
+	BOOLEAN			fAnyStationMIMOPSDynamic; // Check if any Station is MIMO Dynamic
+	BOOLEAN         fAnyBASession;   // Check if there is BA session.  Force turn on RTS/CTS
+#endif // DOT11_N_SUPPORT //
+} MAC_TABLE, *PMAC_TABLE;
+
+#ifdef DOT11_N_SUPPORT
+#define IS_HT_STA(_pMacEntry)	\
+	(_pMacEntry->MaxHTPhyMode.field.MODE >= MODE_HTMIX)
+
+#define IS_HT_RATE(_pMacEntry)	\
+	(_pMacEntry->HTPhyMode.field.MODE >= MODE_HTMIX)
+
+#define PEER_IS_HT_RATE(_pMacEntry)	\
+	(_pMacEntry->HTPhyMode.field.MODE >= MODE_HTMIX)
+#endif // DOT11_N_SUPPORT //
+
+typedef struct _WDS_ENTRY {
+	BOOLEAN         Valid;
+	UCHAR           Addr[MAC_ADDR_LEN];
+	ULONG           NoDataIdleCount;
+	struct _WDS_ENTRY *pNext;
+} WDS_ENTRY, *PWDS_ENTRY;
+
+typedef struct  _WDS_TABLE_ENTRY {
+	USHORT			Size;
+	UCHAR           WdsAddr[MAC_ADDR_LEN];
+	WDS_ENTRY       *Hash[HASH_TABLE_SIZE];
+	WDS_ENTRY       Content[MAX_LEN_OF_MAC_TABLE];
+	UCHAR           MaxSupportedRate;
+	UCHAR           CurrTxRate;
+	USHORT          TxQuality[MAX_LEN_OF_SUPPORTED_RATES];
+	USHORT          OneSecTxOkCount;
+	USHORT          OneSecTxRetryOkCount;
+	USHORT          OneSecTxFailCount;
+	ULONG           CurrTxRateStableTime; // # of second in current TX rate
+	UCHAR           TxRateUpPenalty;      // extra # of second penalty due to last unstable condition
+} WDS_TABLE_ENTRY, *PWDS_TABLE_ENTRY;
+
+typedef struct _RT_802_11_WDS_ENTRY {
+	PNET_DEV			dev;
+	UCHAR				Valid;
+	UCHAR				PhyMode;
+	UCHAR				PeerWdsAddr[MAC_ADDR_LEN];
+	UCHAR				MacTabMatchWCID;	// ASIC
+	NDIS_802_11_WEP_STATUS  WepStatus;
+	UCHAR					KeyIdx;
+	CIPHER_KEY          	WdsKey;
+	HTTRANSMIT_SETTING				HTPhyMode, MaxHTPhyMode, MinHTPhyMode;
+	RT_HT_PHY_INFO					DesiredHtPhyInfo;
+	BOOLEAN							bAutoTxRateSwitch;
+	DESIRED_TRANSMIT_SETTING       	DesiredTransmitSetting; // Desired transmit setting.
+} RT_802_11_WDS_ENTRY, *PRT_802_11_WDS_ENTRY;
+
+typedef struct _WDS_TABLE {
+	UCHAR               Mode;
+	ULONG               Size;
+	RT_802_11_WDS_ENTRY	WdsEntry[MAX_WDS_ENTRY];
+} WDS_TABLE, *PWDS_TABLE;
+
+typedef struct _APCLI_STRUCT {
+	PNET_DEV				dev;
+#ifdef RTL865X_SOC
+	unsigned int            mylinkid;
+#endif
+	BOOLEAN                 Enable;	// Set it as 1 if the apcli interface was configured to "1"  or by iwpriv cmd "ApCliEnable"
+	BOOLEAN                 Valid;	// Set it as 1 if the apcli interface associated success to remote AP.
+	UCHAR					MacTabWCID;	//WCID value, which point to the entry of ASIC Mac table.
+	UCHAR                   SsidLen;
+	CHAR                    Ssid[MAX_LEN_OF_SSID];
+
+	UCHAR                   CfgSsidLen;
+	CHAR                    CfgSsid[MAX_LEN_OF_SSID];
+	UCHAR                   CfgApCliBssid[ETH_LENGTH_OF_ADDRESS];
+	UCHAR                   CurrentAddress[ETH_LENGTH_OF_ADDRESS];
+
+	ULONG                   ApCliRcvBeaconTime;
+
+	ULONG                   CtrlCurrState;
+	ULONG                   SyncCurrState;
+	ULONG                   AuthCurrState;
+	ULONG                   AssocCurrState;
+	ULONG					WpaPskCurrState;
+
+	USHORT                  AuthReqCnt;
+	USHORT                  AssocReqCnt;
+
+	ULONG                   ClientStatusFlags;
+	UCHAR                   MpduDensity;
+
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;   // This should match to whatever microsoft defined
+	NDIS_802_11_WEP_STATUS              WepStatus;
+
+	// Add to support different cipher suite for WPA2/WPA mode
+	NDIS_802_11_ENCRYPTION_STATUS		GroupCipher;		// Multicast cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS		PairCipher;			// Unicast cipher suite
+	BOOLEAN								bMixCipher;			// Indicate current Pair & Group use different cipher suites
+	USHORT								RsnCapability;
+
+	UCHAR		PSK[100];				// reserve PSK key material
+	UCHAR       PSKLen;
+	UCHAR       PMK[32];                // WPA PSK mode PMK
+	UCHAR		GTK[32];				// GTK from authenticator
+
+	CIPHER_KEY      SharedKey[SHARE_KEY_NUM];
+	UCHAR           DefaultKeyId;
+
+	// store RSN_IE built by driver
+	UCHAR		RSN_IE[MAX_LEN_OF_RSNIE];  // The content saved here should be convert to little-endian format.
+	UCHAR		RSNIE_Len;
+
+	// For WPA countermeasures
+	ULONG       LastMicErrorTime;   // record last MIC error time
+	BOOLEAN                 bBlockAssoc; // Block associate attempt for 60 seconds after counter measure occurred.
+
+	// For WPA-PSK supplicant state
+	UCHAR       	SNonce[32];         // SNonce for WPA-PSK
+	UCHAR			GNonce[32];			// GNonce for WPA-PSK from authenticator
+
+#ifdef WSC_AP_SUPPORT
+	WSC_CTRL	           WscControl;
+#endif // WSC_AP_SUPPORT //
+
+	HTTRANSMIT_SETTING				HTPhyMode, MaxHTPhyMode, MinHTPhyMode;
+	RT_HT_PHY_INFO					DesiredHtPhyInfo;
+	BOOLEAN							bAutoTxRateSwitch;
+	DESIRED_TRANSMIT_SETTING       	DesiredTransmitSetting; // Desired transmit setting.
+} APCLI_STRUCT, *PAPCLI_STRUCT;
+
+// ----------- end of AP ----------------------------
+
+#ifdef BLOCK_NET_IF
+typedef struct _BLOCK_QUEUE_ENTRY
+{
+	BOOLEAN SwTxQueueBlockFlag;
+	LIST_HEADER NetIfList;
+} BLOCK_QUEUE_ENTRY, *PBLOCK_QUEUE_ENTRY;
+#endif // BLOCK_NET_IF //
+
+struct wificonf
+{
+	BOOLEAN	bShortGI;
+	BOOLEAN bGreenField;
+};
+
+
+
+
+typedef struct _INF_PCI_CONFIG
+{
+	PUCHAR                  CSRBaseAddress;     // PCI MMIO Base Address, all access will use
+}INF_PCI_CONFIG;
+
+typedef struct _INF_USB_CONFIG
+{
+	UINT                BulkInEpAddr;		// bulk-in endpoint address
+	UINT                BulkOutEpAddr[6];	// bulk-out endpoint address
+
+}INF_USB_CONFIG;
+
+#ifdef IKANOS_VX_1X0
+	typedef void (*IkanosWlanTxCbFuncP)(void *, void *);
+
+	struct IKANOS_TX_INFO
+	{
+		struct net_device *netdev;
+		IkanosWlanTxCbFuncP *fp;
+	};
+#endif // IKANOS_VX_1X0 //
+
+#ifdef NINTENDO_AP
+typedef struct _NINDO_CTRL_BLOCK {
+
+	RT_NINTENDO_TABLE	DS_TABLE;
+
+#ifdef CHIP25XX
+	spinlock_t			NINTENDO_TABLE_Lock;
+#else
+	NDIS_SPIN_LOCK		NINTENDO_TABLE_Lock;
+#endif // CHIP25XX //
+
+	UCHAR				NINTENDO_UP_BUFFER[512];
+	UCHAR				Local_KeyIdx;
+	CIPHER_KEY			Local_SharedKey;
+	UCHAR				Local_bHideSsid;
+	UCHAR				Local_AuthMode;
+	UCHAR				Local_WepStatus;
+	USHORT				Local_CapabilityInfo;
+} NINDO_CTRL_BLOCK;
+#endif // NINTENDO_AP //
+
+
+#ifdef DBG_DIAGNOSE
+#define DIAGNOSE_TIME	10   // 10 sec
+typedef struct _RtmpDiagStrcut_
+{	// Diagnosis Related element
+	unsigned char		inited;
+	unsigned char 	qIdx;
+	unsigned char 	ArrayStartIdx;
+	unsigned char		ArrayCurIdx;
+	// Tx Related Count
+	USHORT			TxDataCnt[DIAGNOSE_TIME];
+	USHORT			TxFailCnt[DIAGNOSE_TIME];
+	USHORT			TxDescCnt[DIAGNOSE_TIME][24]; // 3*3	// TxDesc queue length in scale of 0~14, >=15
+	USHORT			TxMcsCnt[DIAGNOSE_TIME][24]; // 3*3
+	USHORT			TxSWQueCnt[DIAGNOSE_TIME][9];		// TxSwQueue length in scale of 0, 1, 2, 3, 4, 5, 6, 7, >=8
+
+	USHORT			TxAggCnt[DIAGNOSE_TIME];
+	USHORT			TxNonAggCnt[DIAGNOSE_TIME];
+	USHORT			TxAMPDUCnt[DIAGNOSE_TIME][24]; // 3*3 // 10 sec, TxDMA APMDU Aggregation count in range from 0 to 15, in setp of 1.
+	USHORT			TxRalinkCnt[DIAGNOSE_TIME];			// TxRalink Aggregation Count in 1 sec scale.
+	USHORT			TxAMSDUCnt[DIAGNOSE_TIME];			// TxAMSUD Aggregation Count in 1 sec scale.
+
+	// Rx Related Count
+	USHORT			RxDataCnt[DIAGNOSE_TIME];			// Rx Total Data count.
+	USHORT			RxCrcErrCnt[DIAGNOSE_TIME];
+	USHORT			RxMcsCnt[DIAGNOSE_TIME][24]; // 3*3
+}RtmpDiagStruct;
+#endif // DBG_DIAGNOSE //
+
+
+//
+//  The miniport adapter structure
+//
+typedef struct _RTMP_ADAPTER
+{
+	PVOID					OS_Cookie;	// save specific structure relative to OS
+	PNET_DEV				net_dev;
+	ULONG					VirtualIfCnt;
+
+#ifdef RT2860
+    USHORT		            LnkCtrlBitMask;
+    USHORT		            RLnkCtrlConfiguration;
+    USHORT                  RLnkCtrlOffset;
+    USHORT		            HostLnkCtrlConfiguration;
+    USHORT                  HostLnkCtrlOffset;
+	USHORT		            PCIePowerSaveLevel;
+   	BOOLEAN					bPCIclkOff;						// flag that indicate if the PICE power status in Configuration SPace..
+	BOOLEAN					bPCIclkOffDisableTx;			//
+
+
+/*****************************************************************************************/
+/*      PCI related parameters                                                           */
+/*****************************************************************************************/
+	PUCHAR                  CSRBaseAddress;     // PCI MMIO Base Address, all access will use
+
+	UINT					int_enable_reg;
+	UINT					int_disable_mask;
+	UINT					int_pending;
+
+
+	RTMP_DMABUF             TxBufSpace[NUM_OF_TX_RING]; // Shared memory of all 1st pre-allocated TxBuf associated with each TXD
+	RTMP_DMABUF             RxDescRing;                 // Shared memory for RX descriptors
+	RTMP_DMABUF             TxDescRing[NUM_OF_TX_RING]; 	// Shared memory for Tx descriptors
+	RTMP_TX_RING            TxRing[NUM_OF_TX_RING];     	// AC0~4 + HCCA
+#endif // RT2860 //
+
+
+	NDIS_SPIN_LOCK          irq_lock;
+	UCHAR                   irq_disabled;
+
+
+
+/*****************************************************************************************/
+	/*      Both PCI/USB related parameters                                                  */
+/*****************************************************************************************/
+
+
+/*****************************************************************************************/
+/*      Tx related parameters                                                           */
+/*****************************************************************************************/
+	BOOLEAN                 DeQueueRunning[NUM_OF_TX_RING];  // for ensuring RTUSBDeQueuePacket get call once
+	NDIS_SPIN_LOCK          DeQueueLock[NUM_OF_TX_RING];
+
+
+	// resource for software backlog queues
+	QUEUE_HEADER            TxSwQueue[NUM_OF_TX_RING];  // 4 AC + 1 HCCA
+	NDIS_SPIN_LOCK          TxSwQueueLock[NUM_OF_TX_RING];	// TxSwQueue spinlock
+
+	RTMP_DMABUF             MgmtDescRing;               	// Shared memory for MGMT descriptors
+	RTMP_MGMT_RING          MgmtRing;
+	NDIS_SPIN_LOCK          MgmtRingLock;               	// Prio Ring spinlock
+
+
+/*****************************************************************************************/
+/*      Rx related parameters                                                           */
+/*****************************************************************************************/
+
+#ifdef RT2860
+	RTMP_RX_RING            RxRing;
+	NDIS_SPIN_LOCK          RxRingLock;                 // Rx Ring spinlock
+#endif // RT2860 //
+
+
+
+/*****************************************************************************************/
+/*      ASIC related parameters                                                          */
+/*****************************************************************************************/
+	UINT32               	MACVersion;      	// MAC version. Record rt2860C(0x28600100) or rt2860D (0x28600101)..
+
+	// ---------------------------
+	// E2PROM
+	// ---------------------------
+	ULONG                   EepromVersion;          // byte 0: version, byte 1: revision, byte 2~3: unused
+	UCHAR                   EEPROMAddressNum;       // 93c46=6  93c66=8
+	USHORT                  EEPROMDefaultValue[NUM_EEPROM_BBP_PARMS];
+	ULONG                   FirmwareVersion;        // byte 0: Minor version, byte 1: Major version, otherwise unused.
+
+	// ---------------------------
+	// BBP Control
+	// ---------------------------
+	UCHAR                   BbpWriteLatch[140];     // record last BBP register value written via BBP_IO_WRITE/BBP_IO_WRITE_VY_REG_ID
+	UCHAR                   BbpRssiToDbmDelta;
+	BBP_R66_TUNING          BbpTuning;
+
+	// ----------------------------
+	// RFIC control
+	// ----------------------------
+	UCHAR                   RfIcType;       // RFIC_xxx
+	ULONG                   RfFreqOffset;   // Frequency offset for channel switching
+	RTMP_RF_REGS            LatchRfRegs;    // latch th latest RF programming value since RF IC doesn't support READ
+
+	EEPROM_ANTENNA_STRUC    Antenna;                            // Since ANtenna definition is different for a & g. We need to save it for future reference.
+	EEPROM_NIC_CONFIG2_STRUC    NicConfig2;
+
+	// This soft Rx Antenna Diversity mechanism is used only when user set
+	// RX Antenna = DIVERSITY ON
+	SOFT_RX_ANT_DIVERSITY   RxAnt;
+
+	UCHAR                   RFProgSeq;
+	CHANNEL_TX_POWER        TxPower[MAX_NUM_OF_CHANNELS];       // Store Tx power value for all channels.
+	CHANNEL_TX_POWER        ChannelList[MAX_NUM_OF_CHANNELS];   // list all supported channels for site survey
+	CHANNEL_11J_TX_POWER    TxPower11J[MAX_NUM_OF_11JCHANNELS];       // 802.11j channel and bw
+	CHANNEL_11J_TX_POWER    ChannelList11J[MAX_NUM_OF_11JCHANNELS];   // list all supported channels for site survey
+
+	UCHAR                   ChannelListNum;                     // number of channel in ChannelList[]
+	UCHAR					Bbp94;
+	BOOLEAN					BbpForCCK;
+	ULONG		Tx20MPwrCfgABand[5];
+	ULONG		Tx20MPwrCfgGBand[5];
+	ULONG		Tx40MPwrCfgABand[5];
+	ULONG		Tx40MPwrCfgGBand[5];
+
+	BOOLEAN     bAutoTxAgcA;                // Enable driver auto Tx Agc control
+	UCHAR	    TssiRefA;					// Store Tssi reference value as 25 temperature.
+	UCHAR	    TssiPlusBoundaryA[5];		// Tssi boundary for increase Tx power to compensate.
+	UCHAR	    TssiMinusBoundaryA[5];		// Tssi boundary for decrease Tx power to compensate.
+	UCHAR	    TxAgcStepA;					// Store Tx TSSI delta increment / decrement value
+	CHAR		TxAgcCompensateA;			// Store the compensation (TxAgcStep * (idx-1))
+
+	BOOLEAN     bAutoTxAgcG;                // Enable driver auto Tx Agc control
+	UCHAR	    TssiRefG;					// Store Tssi reference value as 25 temperature.
+	UCHAR	    TssiPlusBoundaryG[5];		// Tssi boundary for increase Tx power to compensate.
+	UCHAR	    TssiMinusBoundaryG[5];		// Tssi boundary for decrease Tx power to compensate.
+	UCHAR	    TxAgcStepG;					// Store Tx TSSI delta increment / decrement value
+	CHAR		TxAgcCompensateG;			// Store the compensation (TxAgcStep * (idx-1))
+
+	//+++For RT2870, the parameteres is start from BGRssiOffset1 ~ BGRssiOffset3
+	CHAR		BGRssiOffset0;				// Store B/G RSSI#0 Offset value on EEPROM 0x46h
+	CHAR		BGRssiOffset1;				// Store B/G RSSI#1 Offset value
+	CHAR		BGRssiOffset2;				// Store B/G RSSI#2 Offset value
+	//---
+
+	//+++For RT2870, the parameteres is start from ARssiOffset1 ~ ARssiOffset3
+	CHAR		ARssiOffset0;				// Store A RSSI#0 Offset value on EEPROM 0x4Ah
+	CHAR		ARssiOffset1;				// Store A RSSI#1 Offset value
+	CHAR		ARssiOffset2;				// Store A RSSI#2 Offset value
+	//---
+
+	CHAR		BLNAGain;					// Store B/G external LNA#0 value on EEPROM 0x44h
+	CHAR		ALNAGain0;					// Store A external LNA#0 value for ch36~64
+	CHAR		ALNAGain1;					// Store A external LNA#1 value for ch100~128
+	CHAR		ALNAGain2;					// Store A external LNA#2 value for ch132~165
+
+	// ----------------------------
+	// LED control
+	// ----------------------------
+	MCU_LEDCS_STRUC		LedCntl;
+	USHORT				Led1;	// read from EEPROM 0x3c
+	USHORT				Led2;	// EEPROM 0x3e
+	USHORT				Led3;	// EEPROM 0x40
+	UCHAR				LedIndicatorStregth;
+	UCHAR				RssiSingalstrengthOffet;
+    BOOLEAN				bLedOnScanning;
+	UCHAR				LedStatus;
+
+/*****************************************************************************************/
+/*      802.11 related parameters                                                        */
+/*****************************************************************************************/
+	// outgoing BEACON frame buffer and corresponding TXD
+	TXWI_STRUC              	BeaconTxWI;
+	PUCHAR						BeaconBuf;
+	USHORT						BeaconOffset[HW_BEACON_MAX_COUNT];
+
+	// pre-build PS-POLL and NULL frame upon link up. for efficiency purpose.
+	PSPOLL_FRAME            	PsPollFrame;
+	HEADER_802_11           	NullFrame;
+
+//=========AP===========
+
+
+//=======STA===========
+#ifdef CONFIG_STA_SUPPORT
+/* Modified by Wu Xi-Kun 4/21/2006 */
+	// -----------------------------------------------
+	// STA specific configuration & operation status
+	// used only when pAd->OpMode == OPMODE_STA
+	// -----------------------------------------------
+	STA_ADMIN_CONFIG        StaCfg;           // user desired settings
+	STA_ACTIVE_CONFIG       StaActive;         // valid only when ADHOC_ON(pAd) || INFRA_ON(pAd)
+	CHAR                    nickname[IW_ESSID_MAX_SIZE+1]; // nickname, only used in the iwconfig i/f
+	NDIS_MEDIA_STATE        PreMediaState;
+#endif // CONFIG_STA_SUPPORT //
+
+//=======Common===========
+	// OP mode: either AP or STA
+	UCHAR                   OpMode;                     // OPMODE_STA, OPMODE_AP
+
+	NDIS_MEDIA_STATE        IndicateMediaState;			// Base on Indication state, default is NdisMediaStateDisConnected
+
+	// MAT related parameters
+
+	// configuration: read from Registry & E2PROM
+	BOOLEAN                 bLocalAdminMAC;             // Use user changed MAC
+	UCHAR                   PermanentAddress[MAC_ADDR_LEN];    // Factory default MAC address
+	UCHAR                   CurrentAddress[MAC_ADDR_LEN];      // User changed MAC address
+
+	// ------------------------------------------------------
+	// common configuration to both OPMODE_STA and OPMODE_AP
+	// ------------------------------------------------------
+	COMMON_CONFIG           CommonCfg;
+	MLME_STRUCT             Mlme;
+
+	// AP needs those vaiables for site survey feature.
+	MLME_AUX                MlmeAux;           // temporary settings used during MLME state machine
+	BSS_TABLE               ScanTab;           // store the latest SCAN result
+
+	//About MacTab, the sta driver will use #0 and #1 for multicast and AP.
+	MAC_TABLE                 MacTab;     // ASIC on-chip WCID entry table.  At TX, ASIC always use key according to this on-chip table.
+	NDIS_SPIN_LOCK          MacTabLock;
+
+#ifdef DOT11_N_SUPPORT
+	BA_TABLE			BATable;
+#endif // DOT11_N_SUPPORT //
+	NDIS_SPIN_LOCK          BATabLock;
+	RALINK_TIMER_STRUCT RECBATimer;
+
+	// encryption/decryption KEY tables
+	CIPHER_KEY              SharedKey[MAX_MBSSID_NUM][4]; // STA always use SharedKey[BSS0][0..3]
+
+		// RX re-assembly buffer for fragmentation
+	FRAGMENT_FRAME          FragFrame;                  // Frame storage for fragment frame
+
+	// various Counters
+	COUNTER_802_3           Counters8023;               // 802.3 counters
+	COUNTER_802_11          WlanCounters;               // 802.11 MIB counters
+	COUNTER_RALINK          RalinkCounters;             // Ralink propriety counters
+	COUNTER_DRS             DrsCounters;                // counters for Dynamic TX Rate Switching
+	PRIVATE_STRUC           PrivateInfo;                // Private information & counters
+
+	// flags, see fRTMP_ADAPTER_xxx flags
+	ULONG                   Flags;                      // Represent current device status
+
+	// current TX sequence #
+	USHORT                  Sequence;
+
+#ifdef UNDER_CE
+	NDIS_HANDLE             hGiISR;
+#endif
+
+
+	// Control disconnect / connect event generation
+	//+++Didn't used anymore
+	ULONG                   LinkDownTime;
+	//---
+	ULONG                   LastRxRate;
+	ULONG                   LastTxRate;
+	//+++Used only for Station
+	BOOLEAN                 bConfigChanged;         // Config Change flag for the same SSID setting
+	//---
+
+	ULONG                   ExtraInfo;              // Extra information for displaying status
+	ULONG                   SystemErrorBitmap;      // b0: E2PROM version error
+
+	//+++Didn't used anymore
+	ULONG                   MacIcVersion;           // MAC/BBP serial interface issue solved after ver.D
+	//---
+
+	// ---------------------------
+	// System event log
+	// ---------------------------
+	RT_802_11_EVENT_TABLE   EventTab;
+
+
+	BOOLEAN		HTCEnable;
+
+	/*****************************************************************************************/
+	/*      Statistic related parameters                                                     */
+	/*****************************************************************************************/
+
+	BOOLEAN						bUpdateBcnCntDone;
+	ULONG						watchDogMacDeadlock;	// prevent MAC/BBP into deadlock condition
+	// ----------------------------
+	// DEBUG paramerts
+	// ----------------------------
+	BOOLEAN		bBanAllBaSetup;
+	BOOLEAN		bPromiscuous;
+
+	// ----------------------------
+	// rt2860c emulation-use Parameters
+	// ----------------------------
+	ULONG		rtsaccu[30];
+	ULONG		ctsaccu[30];
+	ULONG		cfendaccu[30];
+	ULONG		bacontent[16];
+	ULONG		rxint[RX_RING_SIZE+1];
+	UCHAR		rcvba[60];
+	BOOLEAN		bLinkAdapt;
+	BOOLEAN		bForcePrintTX;
+	BOOLEAN		bForcePrintRX;
+	BOOLEAN		bDisablescanning;		//defined in RT2870 USB
+	BOOLEAN		bStaFifoTest;
+	BOOLEAN		bProtectionTest;
+	BOOLEAN		bHCCATest;
+	BOOLEAN		bGenOneHCCA;
+	BOOLEAN		bBroadComHT;
+	//+++Following add from RT2870 USB.
+	ULONG		BulkOutReq;
+	ULONG		BulkOutComplete;
+	ULONG		BulkOutCompleteOther;
+	ULONG		BulkOutCompleteCancel;	// seems not use now?
+	ULONG		BulkInReq;
+	ULONG		BulkInComplete;
+	ULONG		BulkInCompleteFail;
+	//---
+
+    struct wificonf			WIFItestbed;
+
+#ifdef RALINK_ATE
+	ATE_INFO				ate;
+#endif // RALINK_ATE //
+
+#ifdef DOT11_N_SUPPORT
+	struct reordering_mpdu_pool mpdu_blk_pool;
+#endif // DOT11_N_SUPPORT //
+
+	ULONG					OneSecondnonBEpackets;		// record non BE packets per second
+
+#if WIRELESS_EXT >= 12
+    struct iw_statistics    iw_stats;
+#endif
+
+	struct net_device_stats	stats;
+
+#ifdef BLOCK_NET_IF
+	BLOCK_QUEUE_ENTRY		blockQueueTab[NUM_OF_TX_RING];
+#endif // BLOCK_NET_IF //
+
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+	INT32					MC_RowID;
+	UCHAR					MC_FileName[256];
+#endif // MULTIPLE_CARD_SUPPORT //
+
+	ULONG					TbttTickCount;
+#ifdef PCI_MSI_SUPPORT
+	BOOLEAN					HaveMsi;
+#endif // PCI_MSI_SUPPORT //
+
+
+	UCHAR					is_on;
+
+#define TIME_BASE			(1000000/OS_HZ)
+#define TIME_ONE_SECOND		(1000000/TIME_BASE)
+	UCHAR					flg_be_adjust;
+	ULONG					be_adjust_last_time;
+
+#ifdef NINTENDO_AP
+	NINDO_CTRL_BLOCK		nindo_ctrl_block;
+#endif // NINTENDO_AP //
+
+
+#ifdef IKANOS_VX_1X0
+	struct IKANOS_TX_INFO	IkanosTxInfo;
+	struct IKANOS_TX_INFO	IkanosRxInfo[MAX_MBSSID_NUM + MAX_WDS_ENTRY + MAX_APCLI_NUM + MAX_MESH_NUM];
+#endif // IKANOS_VX_1X0 //
+
+
+#ifdef DBG_DIAGNOSE
+	RtmpDiagStruct	DiagStruct;
+#endif // DBG_DIAGNOSE //
+
+
+	UINT8					PM_FlgSuspend;
+} RTMP_ADAPTER, *PRTMP_ADAPTER;
+
+//
+// Cisco IAPP format
+//
+typedef struct  _CISCO_IAPP_CONTENT_
+{
+	USHORT     Length;        //IAPP Length
+	UCHAR      MessageType;      //IAPP type
+	UCHAR      FunctionCode;     //IAPP function type
+	UCHAR      DestinaionMAC[MAC_ADDR_LEN];
+	UCHAR      SourceMAC[MAC_ADDR_LEN];
+	USHORT     Tag;           //Tag(element IE) - Adjacent AP report
+	USHORT     TagLength;     //Length of element not including 4 byte header
+	UCHAR      OUI[4];           //0x00, 0x40, 0x96, 0x00
+	UCHAR      PreviousAP[MAC_ADDR_LEN];       //MAC Address of access point
+	USHORT     Channel;
+	USHORT     SsidLen;
+	UCHAR      Ssid[MAX_LEN_OF_SSID];
+	USHORT     Seconds;          //Seconds that the client has been disassociated.
+} CISCO_IAPP_CONTENT, *PCISCO_IAPP_CONTENT;
+
+#define DELAYINTMASK		0x0003fffb
+#define INTMASK				0x0003fffb
+#define IndMask				0x0003fffc
+#define RxINT				0x00000005	// Delayed Rx or indivi rx
+#define TxDataInt			0x000000fa	// Delayed Tx or indivi tx
+#define TxMgmtInt			0x00000102	// Delayed Tx or indivi tx
+#define TxCoherent			0x00020000	// tx coherent
+#define RxCoherent			0x00010000	// rx coherent
+#define McuCommand			0x00000200	// mcu
+#define PreTBTTInt			0x00001000	// Pre-TBTT interrupt
+#define TBTTInt				0x00000800		// TBTT interrupt
+#define GPTimeOutInt			0x00008000		// GPtimeout interrupt
+#define AutoWakeupInt		0x00004000		// AutoWakeupInt interrupt
+#define FifoStaFullInt			0x00002000	//  fifo statistics full interrupt
+
+
+typedef struct _RX_BLK_
+{
+	RT28XX_RXD_STRUC	RxD;
+	PRXWI_STRUC			pRxWI;
+	PHEADER_802_11		pHeader;
+	PNDIS_PACKET		pRxPacket;
+	UCHAR				*pData;
+	USHORT				DataSize;
+	USHORT				Flags;
+	UCHAR				UserPriority;	// for calculate TKIP MIC using
+} RX_BLK;
+
+
+#define RX_BLK_SET_FLAG(_pRxBlk, _flag)		(_pRxBlk->Flags |= _flag)
+#define RX_BLK_TEST_FLAG(_pRxBlk, _flag)	(_pRxBlk->Flags & _flag)
+#define RX_BLK_CLEAR_FLAG(_pRxBlk, _flag)	(_pRxBlk->Flags &= ~(_flag))
+
+
+#define fRX_WDS			0x0001
+#define fRX_AMSDU       0x0002
+#define fRX_ARALINK     0x0004
+#define fRX_HTC         0x0008
+#define fRX_PAD         0x0010
+#define fRX_AMPDU       0x0020
+#define fRX_QOS			0x0040
+#define fRX_INFRA		0x0080
+#define fRX_EAP			0x0100
+#define fRX_MESH		0x0200
+#define fRX_APCLI		0x0400
+#define fRX_DLS			0x0800
+#define fRX_WPI			0x1000
+
+#define LENGTH_AMSDU_SUBFRAMEHEAD	14
+#define LENGTH_ARALINK_SUBFRAMEHEAD	14
+#define LENGTH_ARALINK_HEADER_FIELD	 2
+
+#define TX_UNKOWN_FRAME			0x00
+#define TX_MCAST_FRAME			0x01
+#define TX_LEGACY_FRAME			0x02
+#define TX_AMPDU_FRAME			0x04
+#define TX_AMSDU_FRAME			0x08
+#define TX_RALINK_FRAME			0x10
+#define TX_FRAG_FRAME			0x20
+
+
+//	Currently the sizeof(TX_BLK) is 148 bytes.
+typedef struct _TX_BLK_
+{
+	UCHAR				QueIdx;
+	UCHAR				TxFrameType;				// Indicate the Transmission type of the all frames in one batch
+	UCHAR				TotalFrameNum;				// Total frame number want to send-out in one batch
+	USHORT				TotalFragNum;				// Total frame fragments required in one batch
+	USHORT				TotalFrameLen;				// Total length of all frames want to send-out in one batch
+
+	QUEUE_HEADER		TxPacketList;
+	MAC_TABLE_ENTRY		*pMacEntry;					// NULL: packet with 802.11 RA field is multicast/broadcast address
+	HTTRANSMIT_SETTING	*pTransmit;
+
+	// Following structure used for the characteristics of a specific packet.
+	PNDIS_PACKET		pPacket;
+	PUCHAR				pSrcBufHeader;				// Reference to the head of sk_buff->data
+	PUCHAR				pSrcBufData;				// Reference to the sk_buff->data, will changed depends on hanlding progresss
+	UINT				SrcBufLen;					// Length of packet payload which not including Layer 2 header
+	PUCHAR				pExtraLlcSnapEncap;			// NULL means no extra LLC/SNAP is required
+	UCHAR				HeaderBuf[80];				// TempBuffer for TX_INFO + TX_WI + 802.11 Header + padding + AMSDU SubHeader + LLC/SNAP
+	UCHAR				MpduHeaderLen;				// 802.11 header length NOT including the padding
+	UCHAR				HdrPadLen;					// recording Header Padding Length;
+	UCHAR				apidx;						// The interface associated to this packet
+	UCHAR				Wcid;						// The MAC entry associated to this packet
+	UCHAR				UserPriority;				// priority class of packet
+	UCHAR				FrameGap;					// what kind of IFS this packet use
+	UCHAR				MpduReqNum;					// number of fragments of this frame
+	UCHAR				TxRate;						// TODO: Obsoleted? Should change to MCS?
+	UCHAR				CipherAlg;					// cipher alogrithm
+	PCIPHER_KEY			pKey;
+
+
+
+	USHORT				Flags;						//See following definitions for detail.
+
+	//YOU SHOULD NOT TOUCH IT! Following parameters are used for hardware-depended layer.
+	ULONG				Priv;						// Hardware specific value saved in here.
+} TX_BLK, *PTX_BLK;
+
+
+#define fTX_bRtsRequired		0x0001	// Indicate if need send RTS frame for protection. Not used in RT2860/RT2870.
+#define fTX_bAckRequired       	0x0002	// the packet need ack response
+#define fTX_bPiggyBack     		0x0004	// Legacy device use Piggback or not
+#define fTX_bHTRate         	0x0008	// allow to use HT rate
+#define fTX_bForceNonQoS       	0x0010	// force to transmit frame without WMM-QoS in HT mode
+#define fTX_bAllowFrag       	0x0020	// allow to fragment the packet, A-MPDU, A-MSDU, A-Ralink is not allowed to fragment
+#define fTX_bMoreData			0x0040	// there are more data packets in PowerSave Queue
+#define fTX_bWMM				0x0080	// QOS Data
+
+#define fTX_bClearEAPFrame		0x0100
+
+#define TX_BLK_ASSIGN_FLAG(_pTxBlk, _flag, value)	\
+		do {										\
+			if (value) 								\
+				(_pTxBlk->Flags |= _flag) 			\
+			else 									\
+				(_pTxBlk->Flags &= ~(_flag))		\
+		}while(0)
+
+#define TX_BLK_SET_FLAG(_pTxBlk, _flag)		(_pTxBlk->Flags |= _flag)
+#define TX_BLK_TEST_FLAG(_pTxBlk, _flag)	(((_pTxBlk->Flags & _flag) == _flag) ? 1 : 0)
+#define TX_BLK_CLEAR_FLAG(_pTxBlk, _flag)	(_pTxBlk->Flags &= ~(_flag))
+
+
+
+
+
+//------------------------------------------------------------------------------------------
+
+
+#ifdef RT2860
+//
+// Enable & Disable NIC interrupt via writing interrupt mask register
+// Since it use ADAPTER structure, it have to be put after structure definition.
+//
+__inline    VOID    NICDisableInterrupt(
+    IN  PRTMP_ADAPTER   pAd)
+{
+	RTMP_IO_WRITE32(pAd, INT_MASK_CSR, 0x0);     // 0: disable
+	//RTMP_IO_WRITE32(pAd, PBF_INT_ENA, 0x0);	 	// 0x418 is for firmware . SW doesn't handle here.
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_ACTIVE);
+}
+
+__inline    VOID    NICEnableInterrupt(
+    IN  PRTMP_ADAPTER   pAd)
+{
+	//
+	// Flag "fOP_STATUS_DOZE" On, means ASIC put to sleep, else means ASIC WakeUp
+	// To prevent System hang, we should enalbe the interrupt when
+	// ASIC is already Wake Up.
+	//
+    // RT2661 => when ASIC is sleeping, MAC register cannot be read and written.
+	// RT2860 => when ASIC is sleeping, MAC register can be read and written.
+	//if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+	{
+		RTMP_IO_WRITE32(pAd, INT_MASK_CSR, pAd->int_enable_reg /*DELAYINTMASK*/);     // 1:enable
+	}
+	//else
+	//	DBGPRINT(RT_DEBUG_TRACE, ("fOP_STATUS_DOZE !\n"));
+
+	//RTMP_IO_WRITE32(pAd, PBF_INT_ENA, 0x00000030); // 1 : enable
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_ACTIVE);
+}
+#endif // RT2860 //
+
+#ifdef RT_BIG_ENDIAN
+static inline VOID	WriteBackToDescriptor(
+	IN  PUCHAR			Dest,
+ 	IN	PUCHAR			Src,
+    IN  BOOLEAN			DoEncrypt,
+	IN  ULONG           DescriptorType)
+{
+	UINT32 *p1, *p2;
+
+	p1 = ((UINT32 *)Dest);
+	p2 = ((UINT32 *)Src);
+
+	*p1 = *p2;
+	*(p1+2) = *(p2+2);
+	*(p1+3) = *(p2+3);
+	*(p1+1) = *(p2+1); // Word 1; this must be written back last
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Endian conversion of Tx/Rx descriptor .
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pData			Pointer to Tx/Rx descriptor
+		DescriptorType	Direction of the frame
+
+	Return Value:
+		None
+
+	Note:
+		Call this function when read or update descriptor
+	========================================================================
+*/
+static inline VOID	RTMPWIEndianChange(
+	IN	PUCHAR			pData,
+	IN	ULONG			DescriptorType)
+{
+	int size;
+	int i;
+
+	size = ((DescriptorType == TYPE_TXWI) ? TXWI_SIZE : RXWI_SIZE);
+
+	if(DescriptorType == TYPE_TXWI)
+	{
+		*((UINT32 *)(pData)) = SWAP32(*((UINT32 *)(pData)));		// Byte 0~3
+		*((UINT32 *)(pData + 4)) = SWAP32(*((UINT32 *)(pData+4)));	// Byte 4~7
+	}
+	else
+	{
+		for(i=0; i < size/4 ; i++)
+			*(((UINT32 *)pData) +i) = SWAP32(*(((UINT32 *)pData)+i));
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Endian conversion of Tx/Rx descriptor .
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pData			Pointer to Tx/Rx descriptor
+		DescriptorType	Direction of the frame
+
+	Return Value:
+		None
+
+	Note:
+		Call this function when read or update descriptor
+	========================================================================
+*/
+#ifdef RT2860
+static inline VOID	RTMPDescriptorEndianChange(
+	IN	PUCHAR			pData,
+	IN	ULONG			DescriptorType)
+{
+	*((UINT32 *)(pData)) = SWAP32(*((UINT32 *)(pData)));		// Byte 0~3
+	*((UINT32 *)(pData + 8)) = SWAP32(*((UINT32 *)(pData+8)));	// Byte 8~11
+	*((UINT32 *)(pData +12)) = SWAP32(*((UINT32 *)(pData + 12)));	// Byte 12~15
+	*((UINT32 *)(pData + 4)) = SWAP32(*((UINT32 *)(pData + 4)));				// Byte 4~7, this must be swapped last
+}
+#endif // RT2860 //
+
+/*
+	========================================================================
+
+	Routine Description:
+		Endian conversion of all kinds of 802.11 frames .
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pData			Pointer to the 802.11 frame structure
+		Dir 			Direction of the frame
+		FromRxDoneInt	Caller is from RxDone interrupt
+
+	Return Value:
+		None
+
+	Note:
+		Call this function when read or update buffer data
+	========================================================================
+*/
+static inline VOID	RTMPFrameEndianChange(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pData,
+	IN	ULONG			Dir,
+	IN	BOOLEAN 		FromRxDoneInt)
+{
+	PHEADER_802_11 pFrame;
+	PUCHAR	pMacHdr;
+
+	// swab 16 bit fields - Frame Control field
+	if(Dir == DIR_READ)
+	{
+		*(USHORT *)pData = SWAP16(*(USHORT *)pData);
+	}
+
+	pFrame = (PHEADER_802_11) pData;
+	pMacHdr = (PUCHAR) pFrame;
+
+	// swab 16 bit fields - Duration/ID field
+	*(USHORT *)(pMacHdr + 2) = SWAP16(*(USHORT *)(pMacHdr + 2));
+
+	// swab 16 bit fields - Sequence Control field
+	*(USHORT *)(pMacHdr + 22) = SWAP16(*(USHORT *)(pMacHdr + 22));
+
+	if(pFrame->FC.Type == BTYPE_MGMT)
+	{
+		switch(pFrame->FC.SubType)
+		{
+			case SUBTYPE_ASSOC_REQ:
+			case SUBTYPE_REASSOC_REQ:
+				// swab 16 bit fields - CapabilityInfo field
+				pMacHdr += sizeof(HEADER_802_11);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - Listen Interval field
+				pMacHdr += 2;
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+
+			case SUBTYPE_ASSOC_RSP:
+			case SUBTYPE_REASSOC_RSP:
+				// swab 16 bit fields - CapabilityInfo field
+				pMacHdr += sizeof(HEADER_802_11);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - Status Code field
+				pMacHdr += 2;
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - AID field
+				pMacHdr += 2;
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+
+			case SUBTYPE_AUTH:
+				// If from APHandleRxDoneInterrupt routine, it is still a encrypt format.
+				// The convertion is delayed to RTMPHandleDecryptionDoneInterrupt.
+				if(!FromRxDoneInt && pFrame->FC.Wep == 1)
+					break;
+				else
+				{
+					// swab 16 bit fields - Auth Alg No. field
+					pMacHdr += sizeof(HEADER_802_11);
+					*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+					// swab 16 bit fields - Auth Seq No. field
+					pMacHdr += 2;
+					*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+					// swab 16 bit fields - Status Code field
+					pMacHdr += 2;
+					*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				}
+				break;
+
+			case SUBTYPE_BEACON:
+			case SUBTYPE_PROBE_RSP:
+				// swab 16 bit fields - BeaconInterval field
+				pMacHdr += (sizeof(HEADER_802_11) + TIMESTAMP_LEN);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - CapabilityInfo field
+				pMacHdr += sizeof(USHORT);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+
+			case SUBTYPE_DEAUTH:
+			case SUBTYPE_DISASSOC:
+				// swab 16 bit fields - Reason code field
+				pMacHdr += sizeof(HEADER_802_11);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+		}
+	}
+	else if( pFrame->FC.Type == BTYPE_DATA )
+	{
+	}
+	else if(pFrame->FC.Type == BTYPE_CNTL)
+	{
+		switch(pFrame->FC.SubType)
+		{
+			case SUBTYPE_BLOCK_ACK_REQ:
+				{
+					PFRAME_BA_REQ pBAReq = (PFRAME_BA_REQ)pFrame;
+					*(USHORT *)(&pBAReq->BARControl) = SWAP16(*(USHORT *)(&pBAReq->BARControl));
+					pBAReq->BAStartingSeq.word = SWAP16(pBAReq->BAStartingSeq.word);
+				}
+				break;
+			case SUBTYPE_BLOCK_ACK:
+				// For Block Ack packet, the HT_CONTROL field is in the same offset with Addr3
+				*(UINT32 *)(&pFrame->Addr3[0]) = SWAP32(*(UINT32 *)(&pFrame->Addr3[0]));
+				break;
+
+			case SUBTYPE_ACK:
+				//For ACK packet, the HT_CONTROL field is in the same offset with Addr2
+				*(UINT32 *)(&pFrame->Addr2[0])=	SWAP32(*(UINT32 *)(&pFrame->Addr2[0]));
+				break;
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("Invalid Frame Type!!!\n"));
+	}
+
+	// swab 16 bit fields - Frame Control
+	if(Dir == DIR_WRITE)
+	{
+		*(USHORT *)pData = SWAP16(*(USHORT *)pData);
+	}
+}
+#endif // RT_BIG_ENDIAN //
+
+
+static inline VOID ConvertMulticastIP2MAC(
+	IN PUCHAR pIpAddr,
+	IN PUCHAR *ppMacAddr,
+	IN UINT16 ProtoType)
+{
+	if (pIpAddr == NULL)
+		return;
+
+	if (ppMacAddr == NULL || *ppMacAddr == NULL)
+		return;
+
+	switch (ProtoType)
+	{
+		case ETH_P_IPV6:
+//			memset(*ppMacAddr, 0, ETH_LENGTH_OF_ADDRESS);
+			*(*ppMacAddr) = 0x33;
+			*(*ppMacAddr + 1) = 0x33;
+			*(*ppMacAddr + 2) = pIpAddr[12];
+			*(*ppMacAddr + 3) = pIpAddr[13];
+			*(*ppMacAddr + 4) = pIpAddr[14];
+			*(*ppMacAddr + 5) = pIpAddr[15];
+			break;
+
+		case ETH_P_IP:
+		default:
+//			memset(*ppMacAddr, 0, ETH_LENGTH_OF_ADDRESS);
+			*(*ppMacAddr) = 0x01;
+			*(*ppMacAddr + 1) = 0x00;
+			*(*ppMacAddr + 2) = 0x5e;
+			*(*ppMacAddr + 3) = pIpAddr[1] & 0x7f;
+			*(*ppMacAddr + 4) = pIpAddr[2];
+			*(*ppMacAddr + 5) = pIpAddr[3];
+			break;
+	}
+
+	return;
+}
+
+BOOLEAN RTMPCheckForHang(
+	IN  NDIS_HANDLE MiniportAdapterContext
+	);
+
+VOID  RTMPHalt(
+	IN  NDIS_HANDLE MiniportAdapterContext
+	);
+
+//
+//  Private routines in rtmp_init.c
+//
+NDIS_STATUS RTMPAllocAdapterBlock(
+	IN PVOID			handle,
+	OUT PRTMP_ADAPTER   *ppAdapter
+	);
+
+NDIS_STATUS RTMPAllocTxRxRingMemory(
+	IN  PRTMP_ADAPTER   pAd
+	);
+
+NDIS_STATUS RTMPFindAdapter(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  NDIS_HANDLE     WrapperConfigurationContext
+	);
+
+NDIS_STATUS	RTMPReadParametersHook(
+	IN	PRTMP_ADAPTER pAd
+	);
+
+VOID RTMPFreeAdapter(
+	IN  PRTMP_ADAPTER   pAd
+	);
+
+NDIS_STATUS NICReadRegParameters(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  NDIS_HANDLE         WrapperConfigurationContext
+	);
+
+
+VOID NICReadEEPROMParameters(
+	IN  PRTMP_ADAPTER       pAd,
+	IN	PUCHAR				mac_addr);
+
+VOID NICInitAsicFromEEPROM(
+	IN  PRTMP_ADAPTER       pAd);
+
+VOID NICInitTxRxRingAndBacklogQueue(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS NICInitializeAdapter(
+	IN  PRTMP_ADAPTER   pAd,
+	IN   BOOLEAN    bHardReset);
+
+NDIS_STATUS NICInitializeAsic(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  BOOLEAN		bHardReset);
+
+VOID NICIssueReset(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPRingCleanUp(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           RingType);
+
+VOID RxTest(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS DbgSendPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket);
+
+VOID UserCfgInit(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID NICResetFromError(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID NICEraseFirmware(
+	IN PRTMP_ADAPTER pAd);
+
+NDIS_STATUS NICLoadFirmware(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS NICLoadRateSwitchingParams(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN NICCheckForHang(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID NICUpdateFifoStaCounters(
+	IN PRTMP_ADAPTER pAd);
+
+VOID NICUpdateRawCounters(
+	IN  PRTMP_ADAPTER   pAd);
+
+ULONG	RTMPNotAllZero(
+	IN	PVOID	pSrc1,
+	IN	ULONG	Length);
+
+VOID RTMPZeroMemory(
+	IN  PVOID   pSrc,
+	IN  ULONG   Length);
+
+ULONG RTMPCompareMemory(
+	IN  PVOID   pSrc1,
+	IN  PVOID   pSrc2,
+	IN  ULONG   Length);
+
+VOID RTMPMoveMemory(
+	OUT PVOID   pDest,
+	IN  PVOID   pSrc,
+	IN  ULONG   Length);
+
+VOID AtoH(
+	char	*src,
+	UCHAR	*dest,
+	int		destlen);
+
+UCHAR BtoH(
+	char ch);
+
+VOID RTMPPatchMacBbpBug(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPPatchCardBus(
+	IN	PRTMP_ADAPTER	pAdapter);
+
+VOID RTMPPatchRalinkCardBus(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	ULONG			Bus);
+
+ULONG RTMPReadCBConfig(
+	IN	ULONG	Bus,
+	IN	ULONG	Slot,
+	IN	ULONG	Func,
+	IN	ULONG	Offset);
+
+VOID RTMPWriteCBConfig(
+	IN	ULONG	Bus,
+	IN	ULONG	Slot,
+	IN	ULONG	Func,
+	IN	ULONG	Offset,
+	IN	ULONG	Value);
+
+VOID RTMPInitTimer(
+	IN  PRTMP_ADAPTER           pAd,
+	IN  PRALINK_TIMER_STRUCT    pTimer,
+	IN  PVOID                   pTimerFunc,
+	IN	PVOID					pData,
+	IN  BOOLEAN                 Repeat);
+
+VOID RTMPSetTimer(
+	IN  PRALINK_TIMER_STRUCT    pTimer,
+	IN  ULONG                   Value);
+
+
+VOID RTMPModTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	ULONG					Value);
+
+VOID RTMPCancelTimer(
+	IN  PRALINK_TIMER_STRUCT    pTimer,
+	OUT BOOLEAN                 *pCancelled);
+
+VOID RTMPSetLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN UCHAR			Status);
+
+VOID RTMPSetSignalLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN NDIS_802_11_RSSI Dbm);
+
+VOID RTMPEnableRxTx(
+	IN PRTMP_ADAPTER	pAd);
+
+//
+// prototype in action.c
+//
+VOID ActionStateMachineInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  STATE_MACHINE *S,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID MlmeADDBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeDELBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeDLSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeInvalidAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeQOSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef DOT11_N_SUPPORT
+VOID PeerAddBAReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAddBARspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDelBAAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+#endif // DOT11_N_SUPPORT //
+
+VOID SendPSMPAction(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			Wcid,
+	IN UCHAR			Psmp);
+
+
+#ifdef DOT11N_DRAFT3
+VOID SendBSS2040CoexistMgmtAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	Wcid,
+	IN	UCHAR	apidx,
+	IN	UCHAR	InfoReq);
+
+VOID SendNotifyBWActionFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR  Wcid,
+	IN UCHAR apidx);
+
+BOOLEAN ChannelSwitchSanityCheck(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  NewChannel,
+	IN    UCHAR  Secondary);
+
+VOID ChannelSwitchAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  Channel,
+	IN    UCHAR  Secondary);
+
+ULONG BuildIntolerantChannelRep(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    PUCHAR  pDest);
+
+VOID Update2040CoexistFrameAndNotify(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha);
+
+VOID Send2040CoexistAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha);
+#endif // DOT11N_DRAFT3 //
+
+VOID PeerRMAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID StaPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Bss2040Coexist);
+#endif // CONFIG_STA_SUPPORT //
+
+
+VOID PeerBSSTranAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef DOT11_N_SUPPORT
+VOID PeerHTAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+#endif // DOT11_N_SUPPORT //
+
+VOID PeerQOSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef QOS_DLS_SUPPORT
+VOID PeerDLSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+VOID DlsParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_DLS_REQ_STRUCT *pDlsReq,
+	IN PRT_802_11_DLS pDls,
+	IN USHORT reason);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+VOID RECBATimerTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID ORIBATimerTimeout(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID SendRefreshBAR(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry);
+#endif // DOT11_N_SUPPORT //
+
+VOID ActHeaderInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN OUT PHEADER_802_11 pHdr80211,
+    IN PUCHAR Addr1,
+    IN PUCHAR Addr2,
+    IN PUCHAR Addr3);
+
+VOID BarHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PFRAME_BAR pCntlBar,
+	IN PUCHAR pDA,
+	IN PUCHAR pSA);
+
+VOID InsertActField(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 Category,
+	IN UINT8 ActCode);
+
+BOOLEAN QosBADataParse(
+	IN PRTMP_ADAPTER	pAd,
+	IN BOOLEAN bAMSDU,
+	IN PUCHAR p8023Header,
+	IN UCHAR	WCID,
+	IN UCHAR	TID,
+	IN USHORT Sequence,
+	IN UCHAR DataOffset,
+	IN USHORT Datasize,
+	IN UINT   CurRxIndex);
+
+#ifdef DOT11_N_SUPPORT
+BOOLEAN CntlEnqueueForRecv(
+    IN	PRTMP_ADAPTER	pAd,
+	IN ULONG Wcid,
+    IN ULONG MsgLen,
+	IN PFRAME_BA_REQ pMsg);
+
+VOID BaAutoManSwitch(
+	IN	PRTMP_ADAPTER	pAd);
+#endif // DOT11_N_SUPPORT //
+
+VOID HTIOTCheck(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR     BatRecIdx);
+
+//
+// Private routines in rtmp_data.c
+//
+BOOLEAN RTMPHandleRxDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPHandleTxDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN RTMPHandleTxRingDmaDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  INT_SOURCE_CSR_STRUC TxRingBitmap);
+
+VOID RTMPHandleMgmtRingDmaDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPHandleTBTTInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPHandlePreTBTTInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+void RTMPHandleTwakeupInterrupt(
+	IN PRTMP_ADAPTER pAd);
+
+VOID	RTMPHandleRxCoherentInterrupt(
+	IN	PRTMP_ADAPTER	pAd);
+
+BOOLEAN TxFrameIsAggregatible(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pPrevAddr1,
+	IN  PUCHAR          p8023hdr);
+
+BOOLEAN PeerIsAggreOn(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  ULONG          TxRate,
+    IN  PMAC_TABLE_ENTRY pMacEntry);
+
+NDIS_STATUS Sniff2BytesFromNdisBuffer(
+	IN  PNDIS_BUFFER    pFirstBuffer,
+	IN  UCHAR           DesiredOffset,
+	OUT PUCHAR          pByte0,
+	OUT PUCHAR          pByte1);
+
+NDIS_STATUS STASendPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket);
+
+VOID STASendPackets(
+	IN  NDIS_HANDLE     MiniportAdapterContext,
+	IN  PPNDIS_PACKET   ppPacketArray,
+	IN  UINT            NumberOfPackets);
+
+VOID RTMPDeQueuePacket(
+	IN  PRTMP_ADAPTER   pAd,
+   	IN	BOOLEAN			bIntContext,
+	IN  UCHAR			QueIdx,
+	IN	UCHAR			Max_Tx_Packets);
+
+NDIS_STATUS	RTMPHardTransmit(
+	IN PRTMP_ADAPTER	pAd,
+	IN PNDIS_PACKET		pPacket,
+	IN  UCHAR			QueIdx,
+	OUT	PULONG			pFreeTXDLeft);
+
+NDIS_STATUS	STAHardTransmit(
+	IN PRTMP_ADAPTER	pAd,
+	IN TX_BLK			*pTxBlk,
+	IN  UCHAR			QueIdx);
+
+VOID STARxEAPOLFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+NDIS_STATUS RTMPFreeTXDRequest(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           RingType,
+	IN  UCHAR           NumberRequired,
+	IN 	PUCHAR          FreeNumberIs);
+
+NDIS_STATUS MlmeHardTransmit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR	QueIdx,
+	IN  PNDIS_PACKET    pPacket);
+
+NDIS_STATUS MlmeHardTransmitMgmtRing(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR	QueIdx,
+	IN  PNDIS_PACKET    pPacket);
+
+NDIS_STATUS MlmeHardTransmitTxRing(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR	QueIdx,
+	IN  PNDIS_PACKET    pPacket);
+
+USHORT  RTMPCalcDuration(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Rate,
+	IN  ULONG           Size);
+
+VOID RTMPWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC		pTxWI,
+	IN  BOOLEAN    		FRAG,
+	IN  BOOLEAN    		CFACK,
+	IN  BOOLEAN    		InsTimestamp,
+	IN	BOOLEAN			AMPDU,
+	IN	BOOLEAN			Ack,
+	IN	BOOLEAN			NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN  UCHAR      		PID,
+	IN	UCHAR			TID,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	*pTransmit);
+
+
+VOID RTMPWriteTxWI_Data(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk);
+
+
+VOID RTMPWriteTxWI_Cache(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk);
+
+VOID RTMPWriteTxDescriptor(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXD_STRUC		pTxD,
+	IN	BOOLEAN			bWIV,
+	IN	UCHAR			QSEL);
+
+VOID RTMPSuspendMsduTransmission(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPResumeMsduTransmission(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS MiniportMMRequest(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			QueIdx,
+	IN	PUCHAR			pData,
+	IN  UINT            Length);
+
+VOID RTMPSendNullFrame(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           TxRate,
+	IN	BOOLEAN			bQosNull);
+
+VOID RTMPSendDisassociationFrame(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTMPSendRTSFrame(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pDA,
+	IN	unsigned int	NextMpduSize,
+	IN  UCHAR           TxRate,
+	IN  UCHAR           RTSRate,
+	IN  USHORT          AckDuration,
+	IN  UCHAR           QueIdx,
+	IN  UCHAR			FrameGap);
+
+
+NDIS_STATUS RTMPApplyPacketFilter(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PRT28XX_RXD_STRUC      pRxD,
+	IN  PHEADER_802_11  pHeader);
+
+PQUEUE_HEADER   RTMPCheckTxSwQueue(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT UCHAR           *QueIdx);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPReportMicError(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PCIPHER_KEY     pWpaKey);
+
+VOID	WpaMicFailureReportFrame(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaDisassocApAndBlockAssoc(
+    IN  PVOID SystemSpecific1,
+    IN  PVOID FunctionContext,
+    IN  PVOID SystemSpecific2,
+    IN  PVOID SystemSpecific3);
+#endif // CONFIG_STA_SUPPORT //
+
+NDIS_STATUS RTMPCloneNdisPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	BOOLEAN    pInsAMSDUHdr,
+	IN  PNDIS_PACKET    pInPacket,
+	OUT PNDIS_PACKET   *ppOutPacket);
+
+NDIS_STATUS RTMPAllocateNdisPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    *pPacket,
+	IN  PUCHAR          pHeader,
+	IN  UINT            HeaderLen,
+	IN  PUCHAR          pData,
+	IN  UINT            DataLen);
+
+VOID RTMPFreeNdisPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket);
+
+BOOLEAN RTMPFreeTXDUponTxDmaDone(
+	IN PRTMP_ADAPTER    pAd,
+	IN UCHAR            QueIdx);
+
+BOOLEAN RTMPCheckDHCPFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+BOOLEAN RTMPCheckEtherType(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+VOID RTMPCckBbpTuning(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UINT			TxRate);
+
+//
+// Private routines in rtmp_wep.c
+//
+VOID RTMPInitWepEngine(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKey,
+	IN  UCHAR           KeyId,
+	IN  UCHAR           KeyLen,
+	IN  PUCHAR          pDest);
+
+VOID RTMPEncryptData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pSrc,
+	IN  PUCHAR          pDest,
+	IN  UINT            Len);
+
+BOOLEAN	RTMPDecryptData(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len,
+	IN	UINT			idx);
+
+BOOLEAN	RTMPSoftDecryptWEP(
+	IN PRTMP_ADAPTER 	pAd,
+	IN PUCHAR			pData,
+	IN ULONG			DataByteCnt,
+	IN PCIPHER_KEY		pGroupKey);
+
+VOID RTMPSetICV(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pDest);
+
+VOID ARCFOUR_INIT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pKey,
+	IN  UINT            KeyLen);
+
+UCHAR   ARCFOUR_BYTE(
+	IN  PARCFOURCONTEXT     Ctx);
+
+VOID ARCFOUR_DECRYPT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pDest,
+	IN  PUCHAR          pSrc,
+	IN  UINT            Len);
+
+VOID ARCFOUR_ENCRYPT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pDest,
+	IN  PUCHAR          pSrc,
+	IN  UINT            Len);
+
+VOID WPAARCFOUR_ENCRYPT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pDest,
+	IN  PUCHAR          pSrc,
+	IN  UINT            Len);
+
+UINT RTMP_CALC_FCS32(
+	IN  UINT   Fcs,
+	IN  PUCHAR  Cp,
+	IN  INT     Len);
+
+//
+// MLME routines
+//
+
+// Asic/RF/BBP related functions
+
+VOID AsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd);
+
+VOID 	AsicUpdateProtect(
+	IN		PRTMP_ADAPTER	pAd,
+	IN 		USHORT			OperaionMode,
+	IN 		UCHAR			SetMask,
+	IN		BOOLEAN			bDisableBGProtect,
+	IN		BOOLEAN			bNonGFExist);
+
+VOID AsicSwitchChannel(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			Channel,
+	IN	BOOLEAN			bScan);
+
+VOID AsicLockChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Channel) ;
+
+VOID AsicAntennaSelect(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Channel);
+
+VOID AsicAntennaSetting(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ABGBAND_STATE	BandState);
+
+VOID AsicRfTuningExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID AsicSleepThenAutoWakeup(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT TbttNumToNextWakeUp);
+
+VOID AsicForceSleep(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN    bFromTx);
+#endif // CONFIG_STA_SUPPORT //
+
+VOID AsicSetBssid(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pBssid);
+
+VOID AsicSetMcastWC(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicDelWcidTab(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	Wcid);
+
+VOID AsicEnableRDG(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicDisableRDG(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicDisableSync(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicEnableBssSync(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicEnableIbssSync(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicSetEdcaParm(
+	IN PRTMP_ADAPTER pAd,
+	IN PEDCA_PARM    pEdcaParm);
+
+VOID AsicSetSlotTime(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN bUseShortSlotTime);
+
+VOID AsicAddSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         BssIndex,
+	IN UCHAR         KeyIdx,
+	IN UCHAR         CipherAlg,
+	IN PUCHAR        pKey,
+	IN PUCHAR        pTxMic,
+	IN PUCHAR        pRxMic);
+
+VOID AsicRemoveSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         BssIndex,
+	IN UCHAR         KeyIdx);
+
+VOID AsicUpdateWCIDAttribute(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR        CipherAlg,
+	IN BOOLEAN		bUsePairewiseKeyTable);
+
+VOID AsicUpdateWCIDIVEIV(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN ULONG        uIV,
+	IN ULONG        uEIV);
+
+VOID AsicUpdateRxWCIDTable(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN PUCHAR        pAddr);
+
+VOID AsicAddKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR		KeyIdx,
+	IN PCIPHER_KEY	pCipherKey,
+	IN BOOLEAN		bUsePairewiseKeyTable,
+	IN BOOLEAN		bTxKey);
+
+VOID AsicAddPairwiseKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR		WCID,
+	IN CIPHER_KEY		 *pCipherKey);
+
+VOID AsicRemovePairwiseKeyEntry(
+	IN PRTMP_ADAPTER  pAd,
+	IN UCHAR		 BssIdx,
+	IN UCHAR		 Wcid);
+
+BOOLEAN AsicSendCommandToMcu(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         Command,
+	IN UCHAR         Token,
+	IN UCHAR         Arg0,
+	IN UCHAR         Arg1);
+
+#ifdef RT2860
+BOOLEAN AsicCheckCommanOk(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 Command);
+#endif // RT2860 //
+
+VOID MacAddrRandomBssid(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT PUCHAR pAddr);
+
+VOID MgtMacHeaderInit(
+	IN  PRTMP_ADAPTER     pAd,
+	IN OUT PHEADER_802_11 pHdr80211,
+	IN UCHAR SubType,
+	IN UCHAR ToDs,
+	IN PUCHAR pDA,
+	IN PUCHAR pBssid);
+
+VOID MlmeRadioOff(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MlmeRadioOn(
+	IN PRTMP_ADAPTER pAd);
+
+
+VOID BssTableInit(
+	IN BSS_TABLE *Tab);
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableInit(
+	IN PRTMP_ADAPTER pAd,
+    IN BA_TABLE *Tab);
+#endif // DOT11_N_SUPPORT //
+
+ULONG BssTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR pBssid,
+	IN UCHAR Channel);
+
+ULONG BssSsidTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR    pBssid,
+	IN PUCHAR    pSsid,
+	IN UCHAR     SsidLen,
+	IN UCHAR     Channel);
+
+ULONG BssTableSearchWithSSID(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR    Bssid,
+	IN PUCHAR    pSsid,
+	IN UCHAR     SsidLen,
+	IN UCHAR     Channel);
+
+VOID BssTableDeleteEntry(
+	IN OUT  PBSS_TABLE pTab,
+	IN      PUCHAR pBssid,
+	IN      UCHAR Channel);
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableDeleteORIEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		BA_ORI_ENTRY	*pBAORIEntry);
+
+VOID BATableDeleteRECEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		BA_REC_ENTRY	*pBARECEntry);
+
+VOID BATableTearORIEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		UCHAR TID,
+	IN		UCHAR Wcid,
+	IN		BOOLEAN bForceDelete,
+	IN		BOOLEAN ALL);
+
+VOID BATableTearRECEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		UCHAR TID,
+	IN		UCHAR WCID,
+	IN		BOOLEAN ALL);
+#endif // DOT11_N_SUPPORT //
+
+VOID  BssEntrySet(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT PBSS_ENTRY pBss,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN PCF_PARM CfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR Channel,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE);
+
+ULONG  BssTableSetEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT PBSS_TABLE pTab,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN CF_PARM *CfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR Channel,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE);
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableInsertEntry(
+    IN	PRTMP_ADAPTER	pAd,
+	IN USHORT Aid,
+    IN USHORT		TimeOutValue,
+	IN USHORT		StartingSeq,
+    IN UCHAR TID,
+	IN UCHAR BAWinSize,
+	IN UCHAR OriginatorStatus,
+    IN BOOLEAN IsRecipient);
+
+#ifdef DOT11N_DRAFT3
+VOID Bss2040CoexistTimeOut(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+
+VOID  TriEventInit(
+	IN	PRTMP_ADAPTER	pAd);
+
+ULONG TriEventTableSetEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT TRIGGER_EVENT_TAB *Tab,
+	IN PUCHAR pBssid,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			RegClass,
+	IN UCHAR ChannelNo);
+
+VOID TriEventCounterMaintenance(
+	IN	PRTMP_ADAPTER	pAd);
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+VOID BssTableSsidSort(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT BSS_TABLE *OutTab,
+	IN  CHAR Ssid[],
+	IN  UCHAR SsidLen);
+
+VOID  BssTableSortByRssi(
+	IN OUT BSS_TABLE *OutTab);
+
+VOID BssCipherParse(
+	IN OUT  PBSS_ENTRY  pBss);
+
+NDIS_STATUS  MlmeQueueInit(
+	IN MLME_QUEUE *Queue);
+
+VOID  MlmeQueueDestroy(
+	IN MLME_QUEUE *Queue);
+
+BOOLEAN MlmeEnqueue(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Machine,
+	IN ULONG MsgType,
+	IN ULONG MsgLen,
+	IN VOID *Msg);
+
+BOOLEAN MlmeEnqueueForRecv(
+	IN  PRTMP_ADAPTER   pAd,
+	IN ULONG Wcid,
+	IN ULONG TimeStampHigh,
+	IN ULONG TimeStampLow,
+	IN UCHAR Rssi0,
+	IN UCHAR Rssi1,
+	IN UCHAR Rssi2,
+	IN ULONG MsgLen,
+	IN PVOID Msg,
+	IN UCHAR Signal);
+
+
+BOOLEAN MlmeDequeue(
+	IN MLME_QUEUE *Queue,
+	OUT MLME_QUEUE_ELEM **Elem);
+
+VOID    MlmeRestartStateMachine(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN  MlmeQueueEmpty(
+	IN MLME_QUEUE *Queue);
+
+BOOLEAN  MlmeQueueFull(
+	IN MLME_QUEUE *Queue);
+
+BOOLEAN  MsgTypeSubst(
+	IN PRTMP_ADAPTER pAd,
+	IN PFRAME_802_11 pFrame,
+	OUT INT *Machine,
+	OUT INT *MsgType);
+
+VOID StateMachineInit(
+	IN STATE_MACHINE *Sm,
+	IN STATE_MACHINE_FUNC Trans[],
+	IN ULONG StNr,
+	IN ULONG MsgNr,
+	IN STATE_MACHINE_FUNC DefFunc,
+	IN ULONG InitState,
+	IN ULONG Base);
+
+VOID StateMachineSetAction(
+	IN STATE_MACHINE *S,
+	IN ULONG St,
+	ULONG Msg,
+	IN STATE_MACHINE_FUNC F);
+
+VOID StateMachinePerformAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN STATE_MACHINE *S,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID Drop(
+	IN  PRTMP_ADAPTER   pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID AssocStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *Sm,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID ReassocTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID AssocTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID DisassocTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+//----------------------------------------------
+VOID MlmeDisassocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeAssocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeReassocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeDisassocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAssocRspAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerReassocRspAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDisassocAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID DisassocTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID AssocTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  ReassocTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  Cls3errAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pAddr);
+
+VOID SwitchBetweenWepAndCkip(
+	IN PRTMP_ADAPTER pAd);
+
+VOID  InvalidStateWhenAssoc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  InvalidStateWhenReassoc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenDisassociate(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+
+VOID  ComposePsPoll(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID  ComposeNullFrame(
+	IN  PRTMP_ADAPTER pAd);
+
+VOID  AssocPostProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pAddr2,
+	IN  USHORT CapabilityInfo,
+	IN  USHORT Aid,
+	IN  UCHAR SupRate[],
+	IN  UCHAR SupRateLen,
+	IN  UCHAR ExtRate[],
+	IN  UCHAR ExtRateLen,
+	IN PEDCA_PARM pEdcaParm,
+	IN HT_CAPABILITY_IE		*pHtCapability,
+	IN  UCHAR HtCapabilityLen,
+	IN ADD_HT_INFO_IE		*pAddHtInfo);
+
+VOID AuthStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN PSTATE_MACHINE sm,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID AuthTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID MlmeAuthReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAuthRspAtSeq2Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAuthRspAtSeq4Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID AuthTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID Cls2errAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pAddr);
+
+VOID MlmeDeauthReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenAuth(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+//=============================================
+
+VOID AuthRspStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PSTATE_MACHINE Sm,
+	IN  STATE_MACHINE_FUNC Trans[]);
+
+VOID PeerDeauthAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAuthSimpleRspGenAndSend(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PHEADER_802_11  pHdr80211,
+	IN  USHORT Alg,
+	IN  USHORT Seq,
+	IN  USHORT Reason,
+	IN  USHORT Status);
+
+//
+// Private routines in dls.c
+//
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+void DlsStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID MlmeDlsReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDlsReqAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM	*Elem);
+
+VOID PeerDlsRspAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM	*Elem);
+
+VOID MlmeDlsTearDownAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDlsTearDownAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM	*Elem);
+
+VOID RTMPCheckDLSTimeOut(
+	IN PRTMP_ADAPTER	pAd);
+
+BOOLEAN RTMPRcvFrameDLSCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN PHEADER_802_11	pHeader,
+	IN ULONG			Len,
+	IN PRT28XX_RXD_STRUC	pRxD);
+
+INT	RTMPCheckDLSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA);
+
+VOID RTMPSendDLSTearDownFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA);
+
+NDIS_STATUS RTMPSendSTAKeyRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA);
+
+NDIS_STATUS RTMPSendSTAKeyHandShake(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA);
+
+VOID DlsTimeoutAction(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+BOOLEAN MlmeDlsReqSanity(
+	IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PRT_802_11_DLS *pDLS,
+    OUT PUSHORT pReason);
+
+INT Set_DlsEntryInfo_Display_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg);
+
+MAC_TABLE_ENTRY *MacTableInsertDlsEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR	pAddr,
+	IN  UINT	DlsEntryIdx);
+
+BOOLEAN MacTableDeleteDlsEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT wcid,
+	IN PUCHAR pAddr);
+
+MAC_TABLE_ENTRY *DlsEntryTableLookup(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount);
+
+MAC_TABLE_ENTRY *DlsEntryTableLookupByWcid(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	wcid,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount);
+
+INT	Set_DlsAddEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_DlsTearDownEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef QOS_DLS_SUPPORT
+BOOLEAN PeerDlsReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pDlsTimeout,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability);
+
+BOOLEAN PeerDlsRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pStatus,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability);
+
+BOOLEAN PeerDlsTearDownSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pReason);
+#endif // QOS_DLS_SUPPORT //
+
+//========================================
+
+VOID SyncStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *Sm,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID BeaconTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID ScanTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID MlmeScanReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenScan(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenJoin(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenStart(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeacon(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID EnqueueProbeRequest(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN ScanRunning(
+		IN PRTMP_ADAPTER pAd);
+//=========================================
+
+VOID MlmeCntlInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *S,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID MlmeCntlMachinePerformAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *S,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlIdleProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlOidScanProc(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlOidSsidProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM * Elem);
+
+VOID CntlOidRTBssidProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM * Elem);
+
+VOID CntlMlmeRoamingProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM * Elem);
+
+VOID CntlWaitDisassocProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitJoinProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitReassocProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitStartProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitAuthProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitAuthProc2(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitAssocProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+#ifdef QOS_DLS_SUPPORT
+VOID CntlOidDLSSetupProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+#endif // QOS_DLS_SUPPORT //
+
+VOID LinkUp(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR BssType);
+
+VOID LinkDown(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  BOOLEAN         IsReqFromAP);
+
+VOID IterateOnBssTab(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID IterateOnBssTab2(
+	IN  PRTMP_ADAPTER   pAd);;
+
+VOID JoinParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_JOIN_REQ_STRUCT *JoinReq,
+	IN  ULONG BssIdx);
+
+VOID AssocParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN OUT MLME_ASSOC_REQ_STRUCT *AssocReq,
+	IN  PUCHAR pAddr,
+	IN  USHORT CapabilityInfo,
+	IN  ULONG Timeout,
+	IN  USHORT ListenIntv);
+
+VOID ScanParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_SCAN_REQ_STRUCT *ScanReq,
+	IN  CHAR Ssid[],
+	IN  UCHAR SsidLen,
+	IN  UCHAR BssType,
+	IN  UCHAR ScanType);
+
+VOID DisassocParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_DISASSOC_REQ_STRUCT *DisassocReq,
+	IN  PUCHAR pAddr,
+	IN  USHORT Reason);
+
+VOID StartParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_START_REQ_STRUCT *StartReq,
+	IN  CHAR Ssid[],
+	IN  UCHAR SsidLen);
+
+VOID AuthParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_AUTH_REQ_STRUCT *AuthReq,
+	IN  PUCHAR pAddr,
+	IN  USHORT Alg);
+
+VOID EnqueuePsPoll(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID EnqueueBeaconFrame(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeJoinReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeScanReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeStartReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID ScanTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID BeaconTimeoutAtJoinAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeaconAtScanAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeaconAtJoinAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeacon(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerProbeReqAction(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID ScanNextChannel(
+	IN  PRTMP_ADAPTER   pAd);
+
+ULONG MakeIbssBeacon(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID CCXAdjacentAPReport(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN MlmeScanReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT UCHAR *BssType,
+	OUT CHAR ssid[],
+	OUT UCHAR *SsidLen,
+	OUT UCHAR *ScanType);
+
+BOOLEAN PeerBeaconAndProbeRspSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	IN  UCHAR MsgChannel,
+	OUT PUCHAR pAddr2,
+	OUT PUCHAR pBssid,
+	OUT CHAR Ssid[],
+	OUT UCHAR *pSsidLen,
+	OUT UCHAR *pBssType,
+	OUT USHORT *pBeaconPeriod,
+	OUT UCHAR *pChannel,
+	OUT UCHAR *pNewChannel,
+	OUT LARGE_INTEGER *pTimestamp,
+	OUT CF_PARM *pCfParm,
+	OUT USHORT *pAtimWin,
+	OUT USHORT *pCapabilityInfo,
+	OUT UCHAR *pErp,
+	OUT UCHAR *pDtimCount,
+	OUT UCHAR *pDtimPeriod,
+	OUT UCHAR *pBcastFlag,
+	OUT UCHAR *pMessageToMe,
+	OUT UCHAR SupRate[],
+	OUT UCHAR *pSupRateLen,
+	OUT UCHAR ExtRate[],
+	OUT UCHAR *pExtRateLen,
+	OUT	UCHAR *pCkipFlag,
+	OUT	UCHAR *pAironetCellPowerLimit,
+	OUT PEDCA_PARM       pEdcaParm,
+	OUT PQBSS_LOAD_PARM  pQbssLoad,
+	OUT PQOS_CAPABILITY_PARM pQosCapability,
+	OUT ULONG *pRalinkIe,
+	OUT UCHAR		 *pHtCapabilityLen,
+#ifdef CONFIG_STA_SUPPORT
+	OUT UCHAR		 *pPreNHtCapabilityLen,
+#endif // CONFIG_STA_SUPPORT //
+	OUT HT_CAPABILITY_IE *pHtCapability,
+	OUT UCHAR		 *AddHtInfoLen,
+	OUT ADD_HT_INFO_IE *AddHtInfo,
+	OUT UCHAR *NewExtChannel,
+	OUT USHORT *LengthVIE,
+	OUT PNDIS_802_11_VARIABLE_IEs pVIE);
+
+BOOLEAN PeerAddBAReqActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen,
+	OUT PUCHAR pAddr2);
+
+BOOLEAN PeerAddBARspActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen);
+
+BOOLEAN PeerDelBAActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR Wcid,
+    IN VOID *pMsg,
+    IN ULONG MsgLen);
+
+BOOLEAN MlmeAssocReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pApAddr,
+	OUT USHORT *CapabilityInfo,
+	OUT ULONG *Timeout,
+	OUT USHORT *ListenIntv);
+
+BOOLEAN MlmeAuthReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr,
+	OUT ULONG *Timeout,
+	OUT USHORT *Alg);
+
+BOOLEAN MlmeStartReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT CHAR Ssid[],
+	OUT UCHAR *Ssidlen);
+
+BOOLEAN PeerAuthSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr,
+	OUT USHORT *Alg,
+	OUT USHORT *Seq,
+	OUT USHORT *Status,
+	OUT CHAR ChlgText[]);
+
+BOOLEAN PeerAssocRspSanity(
+	IN  PRTMP_ADAPTER   pAd,
+    IN VOID *pMsg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT USHORT *pCapabilityInfo,
+	OUT USHORT *pStatus,
+	OUT USHORT *pAid,
+	OUT UCHAR SupRate[],
+	OUT UCHAR *pSupRateLen,
+	OUT UCHAR ExtRate[],
+	OUT UCHAR *pExtRateLen,
+    OUT HT_CAPABILITY_IE		*pHtCapability,
+    OUT ADD_HT_INFO_IE		*pAddHtInfo,	// AP might use this additional ht info IE
+    OUT UCHAR			*pHtCapabilityLen,
+    OUT UCHAR			*pAddHtInfoLen,
+    OUT UCHAR			*pNewExtChannelOffset,
+	OUT PEDCA_PARM pEdcaParm,
+	OUT UCHAR *pCkipFlag);
+
+BOOLEAN PeerDisassocSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT USHORT *Reason);
+
+BOOLEAN PeerWpaMessageSanity(
+    IN 	PRTMP_ADAPTER 		pAd,
+    IN 	PEAPOL_PACKET 		pMsg,
+    IN 	ULONG 				MsgLen,
+    IN 	UCHAR				MsgType,
+    IN 	MAC_TABLE_ENTRY  	*pEntry);
+
+BOOLEAN PeerDeauthSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT USHORT *Reason);
+
+BOOLEAN PeerProbeReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT CHAR Ssid[],
+	OUT UCHAR *pSsidLen);
+
+BOOLEAN GetTimBit(
+	IN  CHAR *Ptr,
+	IN  USHORT Aid,
+	OUT UCHAR *TimLen,
+	OUT UCHAR *BcastFlag,
+	OUT UCHAR *DtimCount,
+	OUT UCHAR *DtimPeriod,
+	OUT UCHAR *MessageToMe);
+
+UCHAR ChannelSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR channel);
+
+NDIS_802_11_NETWORK_TYPE NetworkTypeInUseSanity(
+	IN PBSS_ENTRY pBss);
+
+BOOLEAN MlmeDelBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen);
+
+BOOLEAN MlmeAddBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2);
+
+ULONG MakeOutgoingFrame(
+	OUT CHAR *Buffer,
+	OUT ULONG *Length, ...);
+
+VOID  LfsrInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG Seed);
+
+UCHAR RandomByte(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicUpdateAutoFallBackTable(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pTxRate);
+
+VOID  MlmePeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID LinkDownExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID LinkUpExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID STAMlmePeriodicExec(
+	PRTMP_ADAPTER pAd);
+
+VOID MlmeAutoScan(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MlmeAutoReconnectLastSSID(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN MlmeValidateSSID(
+	IN PUCHAR pSsid,
+	IN UCHAR  SsidLen);
+
+VOID MlmeCheckForRoaming(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG    Now32);
+
+VOID MlmeCheckForFastRoaming(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG           Now);
+
+VOID MlmeDynamicTxRateSwitching(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MlmeSetTxRate(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PRTMP_TX_RATE_SWITCH	pTxRate);
+
+VOID MlmeSelectTxRateTable(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PUCHAR				*ppTable,
+	IN PUCHAR				pTableSize,
+	IN PUCHAR				pInitTxRateIdx);
+
+VOID MlmeCalculateChannelQuality(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Now);
+
+VOID MlmeCheckPsmChange(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG    Now32);
+
+VOID MlmeSetPsmBit(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT psm);
+
+VOID MlmeSetTxPreamble(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TxPreamble);
+
+VOID UpdateBasicRateBitmap(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID MlmeUpdateTxRates(
+	IN PRTMP_ADAPTER 	pAd,
+	IN 	BOOLEAN		 	bLinkUp,
+	IN	UCHAR			apidx);
+
+#ifdef DOT11_N_SUPPORT
+VOID MlmeUpdateHtTxRates(
+	IN PRTMP_ADAPTER 		pAd,
+	IN	UCHAR				apidx);
+#endif // DOT11_N_SUPPORT //
+
+VOID    RTMPCheckRates(
+	IN      PRTMP_ADAPTER   pAd,
+	IN OUT  UCHAR           SupRate[],
+	IN OUT  UCHAR           *SupRateLen);
+
+#ifdef CONFIG_STA_SUPPORT
+BOOLEAN RTMPCheckChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		CentralChannel,
+	IN UCHAR		Channel);
+#endif // CONFIG_STA_SUPPORT //
+
+BOOLEAN 	RTMPCheckHt(
+	IN		PRTMP_ADAPTER	pAd,
+	IN		UCHAR	Wcid,
+	IN OUT	HT_CAPABILITY_IE			*pHtCapability,
+	IN OUT	ADD_HT_INFO_IE			*pAddHtInfo);
+
+VOID StaQuickResponeForRateUpExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID AsicBbpTuning1(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicBbpTuning2(
+	IN PRTMP_ADAPTER pAd);
+
+VOID RTMPUpdateMlmeRate(
+	IN PRTMP_ADAPTER	pAd);
+
+CHAR RTMPMaxRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN CHAR				Rssi0,
+	IN CHAR				Rssi1,
+	IN CHAR				Rssi2);
+
+VOID AsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID AsicRxAntEvalTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID APSDPeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+BOOLEAN RTMPCheckEntryEnableAutoRateSwitch(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry);
+
+UCHAR RTMPStaFixedTxMode(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry);
+
+VOID RTMPUpdateLegacyTxSetting(
+		UCHAR				fixed_tx_mode,
+		PMAC_TABLE_ENTRY	pEntry);
+
+BOOLEAN RTMPAutoRateSwitchCheck(
+	IN PRTMP_ADAPTER    pAd);
+
+NDIS_STATUS MlmeInit(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeHandler(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeHalt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeResetRalinkCounters(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID BuildChannelList(
+	IN PRTMP_ADAPTER pAd);
+
+UCHAR FirstChannel(
+	IN  PRTMP_ADAPTER   pAd);
+
+UCHAR NextChannel(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR channel);
+
+VOID ChangeToCellPowerLimit(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         AironetCellPowerLimit);
+
+VOID RaiseClock(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UINT32 *x);
+
+VOID LowerClock(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UINT32 *x);
+
+USHORT ShiftInBits(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID ShiftOutBits(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT data,
+	IN  USHORT count);
+
+VOID EEpromCleanup(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID EWDS(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID EWEN(
+	IN  PRTMP_ADAPTER   pAd);
+
+USHORT RTMP_EEPROM_READ16(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT Offset);
+
+VOID RTMP_EEPROM_WRITE16(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT Offset,
+	IN  USHORT Data);
+
+//
+// Prototypes of function definition in rtmp_tkip.c
+//
+VOID    RTMPInitTkipEngine(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pTKey,
+	IN  UCHAR           KeyId,
+	IN  PUCHAR          pTA,
+	IN  PUCHAR          pMICKey,
+	IN  PUCHAR          pTSC,
+	OUT PULONG          pIV16,
+	OUT PULONG          pIV32);
+
+VOID    RTMPInitMICEngine(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKey,
+	IN  PUCHAR          pDA,
+	IN  PUCHAR          pSA,
+	IN  UCHAR           UserPriority,
+	IN  PUCHAR          pMICKey);
+
+BOOLEAN RTMPTkipCompareMICValue(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pSrc,
+	IN  PUCHAR          pDA,
+	IN  PUCHAR          pSA,
+	IN  PUCHAR          pMICKey,
+	IN	UCHAR			UserPriority,
+	IN  UINT            Len);
+
+VOID    RTMPCalculateMICValue(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket,
+	IN  PUCHAR          pEncap,
+	IN  PCIPHER_KEY     pKey,
+	IN	UCHAR			apidx);
+
+BOOLEAN RTMPTkipCompareMICValueWithLLC(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pLLC,
+	IN  PUCHAR          pSrc,
+	IN  PUCHAR          pDA,
+	IN  PUCHAR          pSA,
+	IN  PUCHAR          pMICKey,
+	IN  UINT            Len);
+
+VOID    RTMPTkipAppendByte(
+	IN  PTKIP_KEY_INFO  pTkip,
+	IN  UCHAR           uChar);
+
+VOID    RTMPTkipAppend(
+	IN  PTKIP_KEY_INFO  pTkip,
+	IN  PUCHAR          pSrc,
+	IN  UINT            nBytes);
+
+VOID    RTMPTkipGetMIC(
+	IN  PTKIP_KEY_INFO  pTkip);
+
+BOOLEAN RTMPSoftDecryptTKIP(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN UCHAR    UserPriority,
+	IN PCIPHER_KEY	pWpaKey);
+
+BOOLEAN RTMPSoftDecryptAES(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN PCIPHER_KEY	pWpaKey);
+
+//
+// Prototypes of function definition in cmm_info.c
+//
+NDIS_STATUS RTMPWPARemoveKeyProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PVOID           pBuf);
+
+VOID    RTMPWPARemoveAllKeys(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN RTMPCheckStrPrintAble(
+    IN  CHAR *pInPutStr,
+    IN  UCHAR strLen);
+
+VOID    RTMPSetPhyMode(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG phymode);
+
+VOID	RTMPUpdateHTIE(
+	IN	RT_HT_CAPABILITY	*pRtHt,
+	IN		UCHAR				*pMcsSet,
+	OUT		HT_CAPABILITY_IE *pHtCapability,
+	OUT		ADD_HT_INFO_IE		*pAddHtInfo);
+
+VOID	RTMPAddWcidAttributeEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BssIdx,
+	IN 	UCHAR		 	KeyIdx,
+	IN 	UCHAR		 	CipherAlg,
+	IN 	MAC_TABLE_ENTRY *pEntry);
+
+CHAR *GetEncryptType(
+	CHAR enc);
+
+CHAR *GetAuthMode(
+	CHAR auth);
+
+VOID RTMPIoctlGetSiteSurvey(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlGetMacTable(
+	IN PRTMP_ADAPTER pAd,
+	IN struct iwreq *wrq);
+
+VOID RTMPIndicateWPA2Status(
+	IN  PRTMP_ADAPTER  pAdapter);
+
+VOID	RTMPOPModeSwitching(
+	IN	PRTMP_ADAPTER	pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID    RTMPAddBSSIDCipher(
+    IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR	Aid,
+    IN  PNDIS_802_11_KEY    pKey,
+    IN  UCHAR   CipherAlg);
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+VOID	RTMPSetHT(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	OID_SET_HT_PHYMODE *pHTPhyMode);
+
+VOID	RTMPSetIndividualHT(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	UCHAR				apidx);
+#endif // DOT11_N_SUPPORT //
+
+VOID RTMPSendWirelessEvent(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Event_flag,
+	IN	PUCHAR 			pAddr,
+	IN  UCHAR			BssIdx,
+	IN	CHAR			Rssi);
+
+VOID	NICUpdateCntlCounters(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PHEADER_802_11	pHeader,
+	IN    UCHAR			SubType,
+	IN	PRXWI_STRUC 	pRxWI);
+//
+// prototype in wpa.c
+//
+BOOLEAN WpaMsgTypeSubst(
+	IN  UCHAR   EAPType,
+	OUT INT		*MsgType);
+
+VOID WpaPskStateMachineInit(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  STATE_MACHINE       *S,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID WpaEAPOLKeyAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaPairMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaPairMsg3Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaGroupMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaMacHeaderInit(
+	IN      PRTMP_ADAPTER   pAd,
+	IN OUT  PHEADER_802_11  pHdr80211,
+	IN      UCHAR           wep,
+	IN      PUCHAR          pAddr1);
+
+VOID    Wpa2PairMsg1Action(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    Wpa2PairMsg3Action(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+BOOLEAN ParseKeyData(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pKeyData,
+    IN  UCHAR           KeyDataLen,
+	IN	UCHAR			bPairewise);
+
+VOID    RTMPToWirelessSta(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pHeader802_3,
+    IN  UINT            HdrLen,
+	IN  PUCHAR          pData,
+    IN  UINT            DataLen,
+    IN	BOOLEAN			is4wayFrame);
+
+VOID    HMAC_SHA1(
+	IN  UCHAR   *text,
+	IN  UINT    text_len,
+	IN  UCHAR   *key,
+	IN  UINT    key_len,
+	IN  UCHAR   *digest);
+
+VOID    PRF(
+	IN  UCHAR   *key,
+	IN  INT     key_len,
+	IN  UCHAR   *prefix,
+	IN  INT     prefix_len,
+	IN  UCHAR   *data,
+	IN  INT     data_len,
+	OUT UCHAR   *output,
+	IN  INT     len);
+
+VOID    CCKMPRF(
+	IN  UCHAR   *key,
+	IN  INT     key_len,
+	IN  UCHAR   *data,
+	IN  INT     data_len,
+	OUT UCHAR   *output,
+	IN  INT     len);
+
+VOID WpaCountPTK(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR   *PMK,
+	IN  UCHAR   *ANonce,
+	IN  UCHAR   *AA,
+	IN  UCHAR   *SNonce,
+	IN  UCHAR   *SA,
+	OUT UCHAR   *output,
+	IN  UINT    len);
+
+VOID    GenRandom(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			*macAddr,
+	OUT	UCHAR			*random);
+
+//
+// prototype in aironet.c
+//
+VOID    AironetStateMachineInit(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  STATE_MACHINE       *S,
+	OUT STATE_MACHINE_FUNC  Trans[]);
+
+VOID    AironetMsgAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    AironetRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    ChannelLoadRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    NoiseHistRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    BeaconRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    AironetReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    ChannelLoadReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    NoiseHistReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    AironetFinalReportAction(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID    BeaconReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    AironetAddBeaconReport(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  ULONG               Index,
+	IN  PMLME_QUEUE_ELEM    pElem);
+
+VOID    AironetCreateBeaconReportFromBssTable(
+	IN  PRTMP_ADAPTER       pAd);
+
+VOID    DBGPRINT_TX_RING(
+	IN PRTMP_ADAPTER  pAd,
+	IN UCHAR          QueIdx);
+
+VOID DBGPRINT_RX_RING(
+	IN PRTMP_ADAPTER  pAd);
+
+CHAR    ConvertToRssi(
+	IN PRTMP_ADAPTER  pAd,
+	IN CHAR				Rssi,
+	IN UCHAR    RssiNumber);
+
+
+#ifdef DOT11N_DRAFT3
+VOID BuildEffectedChannelList(
+	IN PRTMP_ADAPTER pAd);
+#endif // DOT11N_DRAFT3 //
+
+
+VOID APAsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd);
+
+
+VOID APAsicRxAntEvalTimeout(
+	IN PRTMP_ADAPTER	pAd);
+
+//
+// function prototype in cmm_wpa.c
+//
+BOOLEAN RTMPCheckWPAframe(
+	IN PRTMP_ADAPTER pAd,
+	IN PMAC_TABLE_ENTRY	pEntry,
+	IN PUCHAR 			pData,
+	IN ULONG 			DataByteCount,
+	IN UCHAR			FromWhichBSSID);
+
+VOID AES_GTK_KEY_UNWRAP(
+	IN  UCHAR   *key,
+	OUT UCHAR   *plaintext,
+	IN	UCHAR	c_len,
+	IN  UCHAR   *ciphertext);
+
+BOOLEAN RTMPCheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	OUT	UCHAR			*Offset);
+
+BOOLEAN RTMPParseEapolKeyData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKeyData,
+	IN  UCHAR           KeyDataLen,
+	IN	UCHAR			GroupKeyIndex,
+	IN	UCHAR			MsgType,
+	IN	BOOLEAN			bWPA2,
+	IN  MAC_TABLE_ENTRY *pEntry);
+
+VOID	ConstructEapolMsg(
+	IN 	PRTMP_ADAPTER    	pAd,
+    IN 	UCHAR				PeerAuthMode,
+    IN 	UCHAR				PeerWepStatus,
+    IN 	UCHAR				MyGroupKeyWepStatus,
+    IN 	UCHAR				MsgType,
+    IN	UCHAR				DefaultKeyIdx,
+    IN 	UCHAR				*ReplayCounter,
+	IN 	UCHAR				*KeyNonce,
+	IN	UCHAR				*TxRSC,
+	IN	UCHAR				*PTK,
+	IN	UCHAR				*GTK,
+	IN	UCHAR				*RSNIE,
+	IN	UCHAR				RSNIE_Len,
+    OUT PEAPOL_PACKET       pMsg);
+
+VOID	CalculateMIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			PeerWepStatus,
+	IN	UCHAR			*PTK,
+	OUT PEAPOL_PACKET   pMsg);
+
+NDIS_STATUS	RTMPSoftDecryptBroadCastData(
+	IN	PRTMP_ADAPTER					pAd,
+	IN	RX_BLK							*pRxBlk,
+	IN  NDIS_802_11_ENCRYPTION_STATUS 	GroupCipher,
+	IN  PCIPHER_KEY						pShard_key);
+
+VOID	ConstructEapolKeyData(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			PeerAuthMode,
+	IN	UCHAR			PeerWepStatus,
+	IN	UCHAR			GroupKeyWepStatus,
+	IN 	UCHAR			MsgType,
+	IN	UCHAR			DefaultKeyIdx,
+	IN	BOOLEAN			bWPA2Capable,
+	IN	UCHAR			*PTK,
+	IN	UCHAR			*GTK,
+	IN	UCHAR			*RSNIE,
+	IN	UCHAR			RSNIE_LEN,
+	OUT PEAPOL_PACKET   pMsg);
+
+VOID RTMPMakeRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UINT            AuthMode,
+	IN  UINT            WepStatus,
+	IN	UCHAR			apidx);
+
+//
+// function prototype in ap_wpa.c
+//
+
+BOOLEAN APWpaMsgTypeSubst(
+	IN UCHAR    EAPType,
+	OUT INT *MsgType) ;
+
+MAC_TABLE_ENTRY *PACInquiry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG           Wcid);
+
+BOOLEAN RTMPCheckMcast(
+	IN PRTMP_ADAPTER pAd,
+	IN PEID_STRUCT      eid_ptr,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+BOOLEAN RTMPCheckUcast(
+	IN PRTMP_ADAPTER pAd,
+	IN PEID_STRUCT      eid_ptr,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+BOOLEAN RTMPCheckAUTH(
+	IN PRTMP_ADAPTER pAd,
+	IN PEID_STRUCT      eid_ptr,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+VOID WPAStart4WayHS(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	IN	ULONG			TimeInterval);
+
+VOID WPAStart2WayGroupHS(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MAC_TABLE_ENTRY *pEntry);
+
+VOID APWpaEAPPacketAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLStartAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLLogoffAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLKeyAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLASFAlertAction(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  MLME_QUEUE_ELEM  *Elem);
+
+VOID HandleCounterMeasure(
+	IN PRTMP_ADAPTER pAd,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+VOID PeerPairMsg2Action(
+	IN PRTMP_ADAPTER pAd,
+	IN MAC_TABLE_ENTRY  *pEntry,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerPairMsg4Action(
+	IN PRTMP_ADAPTER pAd,
+	IN MAC_TABLE_ENTRY  *pEntry,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID CMTimerExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID WPARetryExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID EnqueueStartForPSKExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID RTMPHandleSTAKey(
+    IN PRTMP_ADAPTER    pAdapter,
+    IN MAC_TABLE_ENTRY  *pEntry,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID PeerGroupMsg2Action(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry,
+	IN  VOID             *Msg,
+	IN  UINT             MsgLen);
+
+VOID PairDisAssocAction(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry,
+	IN  USHORT           Reason);
+
+VOID MlmeDeAuthAction(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry,
+	IN  USHORT           Reason);
+
+VOID GREKEYPeriodicExec(
+	IN  PVOID   SystemSpecific1,
+	IN  PVOID   FunctionContext,
+	IN  PVOID   SystemSpecific2,
+	IN  PVOID   SystemSpecific3);
+
+VOID CountGTK(
+	IN  UCHAR   *PMK,
+	IN  UCHAR   *GNonce,
+	IN  UCHAR   *AA,
+	OUT UCHAR   *output,
+	IN  UINT    len);
+
+VOID    GetSmall(
+	IN  PVOID   pSrc1,
+	IN  PVOID   pSrc2,
+	OUT PUCHAR  out,
+	IN  ULONG   Length);
+
+VOID    GetLarge(
+	IN  PVOID   pSrc1,
+	IN  PVOID   pSrc2,
+	OUT PUCHAR  out,
+	IN  ULONG   Length);
+
+VOID APGenRandom(
+	IN PRTMP_ADAPTER pAd,
+	OUT UCHAR       *random);
+
+VOID AES_GTK_KEY_WRAP(
+	IN UCHAR *key,
+	IN UCHAR *plaintext,
+	IN UCHAR p_len,
+	OUT UCHAR *ciphertext);
+
+VOID    WpaSend(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          pPacket,
+    IN  ULONG           Len);
+
+VOID    APToWirelessSta(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	IN  PUCHAR          pHeader802_3,
+	IN  UINT            HdrLen,
+	IN  PUCHAR          pData,
+	IN  UINT            DataLen,
+    IN	BOOLEAN			bClearFrame);
+
+VOID RTMPAddPMKIDCache(
+	IN  PRTMP_ADAPTER   		pAd,
+	IN	INT						apidx,
+	IN	PUCHAR				pAddr,
+	IN	UCHAR					*PMKID,
+	IN	UCHAR					*PMK);
+
+INT RTMPSearchPMKIDCache(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx,
+	IN	PUCHAR		pAddr);
+
+VOID RTMPDeletePMKIDCache(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx,
+	IN  INT				idx);
+
+VOID RTMPMaintainPMKIDCache(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID	RTMPSendTriggerFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PVOID			pBuffer,
+	IN	ULONG			Length,
+	IN  UCHAR           TxRate,
+	IN	BOOLEAN			bQosNull);
+
+
+/* timeout -- ms */
+VOID RTMP_SetPeriodicTimer(
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	unsigned long timeout);
+
+VOID RTMP_OS_Init_Timer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	TIMER_FUNCTION function,
+	IN	PVOID data);
+
+VOID RTMP_OS_Add_Timer(
+	IN	NDIS_MINIPORT_TIMER	*pTimer,
+	IN	unsigned long timeout);
+
+VOID RTMP_OS_Mod_Timer(
+	IN	NDIS_MINIPORT_TIMER	*pTimer,
+	IN	unsigned long timeout);
+
+
+VOID RTMP_OS_Del_Timer(
+	IN	NDIS_MINIPORT_TIMER	*pTimer,
+	OUT	BOOLEAN				 *pCancelled);
+
+
+VOID RTMP_OS_Release_Packet(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PQUEUE_ENTRY  pEntry);
+
+VOID RTMPusecDelay(
+	IN	ULONG	usec);
+
+NDIS_STATUS os_alloc_mem(
+	IN	PRTMP_ADAPTER pAd,
+	OUT	PUCHAR *mem,
+	IN	ULONG  size);
+
+NDIS_STATUS os_free_mem(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PUCHAR mem);
+
+
+void RTMP_AllocateSharedMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+VOID RTMPFreeTxRxRingMemory(
+    IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS AdapterBlockAllocateMemory(
+	IN PVOID	handle,
+	OUT	PVOID	*ppAd);
+
+void RTMP_AllocateTxDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	UINT	Index,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+void RTMP_AllocateFirstTxBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	UINT	Index,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+void RTMP_AllocateMgmtDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+void RTMP_AllocateRxDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+PNDIS_PACKET RTMP_AllocateRxPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+PNDIS_PACKET RTMP_AllocateTxPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress);
+
+PNDIS_PACKET RTMP_AllocateFragPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length);
+
+void RTMP_QueryPacketInfo(
+	IN  PNDIS_PACKET pPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen);
+
+void RTMP_QueryNextPacketInfo(
+	IN  PNDIS_PACKET *ppPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen);
+
+
+BOOLEAN RTMP_FillTxBlkInfo(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK *pTxBlk);
+
+
+PRTMP_SCATTER_GATHER_LIST
+rt_get_sg_list_from_packet(PNDIS_PACKET pPacket, RTMP_SCATTER_GATHER_LIST *sg);
+
+
+ void announce_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+UINT BA_Reorder_AMSDU_Annnounce(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+UINT Handle_AMSDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN  UCHAR			FromWhichBSSID);
+
+
+void convert_802_11_to_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			p8023hdr,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN  UCHAR			FromWhichBSSID);
+
+
+PNET_DEV get_netdev_from_bssid(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			FromWhichBSSID);
+
+
+PNDIS_PACKET duplicate_pkt(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+    IN  UINT            HdrLen,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN	UCHAR			FromWhichBSSID);
+
+
+PNDIS_PACKET duplicate_pkt_with_TKIP_MIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pOldPkt);
+
+PNDIS_PACKET duplicate_pkt_with_VLAN(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+    IN  UINT            HdrLen,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN	UCHAR			FromWhichBSSID);
+
+PNDIS_PACKET duplicate_pkt_with_WPI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UINT32			ext_head_len,
+	IN	UINT32			ext_tail_len);
+
+UCHAR VLAN_8023_Header_Copy(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+	IN	UINT            HdrLen,
+	OUT PUCHAR			pData,
+	IN	UCHAR			FromWhichBSSID);
+
+#ifdef DOT11_N_SUPPORT
+void ba_flush_reordering_timeout_mpdus(
+	IN PRTMP_ADAPTER	pAd,
+	IN PBA_REC_ENTRY	pBAEntry,
+	IN ULONG			Now32);
+
+
+VOID BAOriSessionSetUp(
+			IN PRTMP_ADAPTER    pAd,
+			IN MAC_TABLE_ENTRY	*pEntry,
+			IN UCHAR			TID,
+			IN USHORT			TimeOut,
+			IN ULONG			DelayTime,
+			IN BOOLEAN		isForced);
+
+VOID BASessionTearDownALL(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		UCHAR Wcid);
+#endif // DOT11_N_SUPPORT //
+
+BOOLEAN OS_Need_Clone_Packet(void);
+
+
+VOID build_tx_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR	pFrame,
+	IN	ULONG	FrameLen);
+
+
+VOID BAOriSessionTearDown(
+	IN OUT	PRTMP_ADAPTER	pAd,
+	IN		UCHAR			Wcid,
+	IN		UCHAR			TID,
+	IN		BOOLEAN			bPassive,
+	IN		BOOLEAN			bForceSend);
+
+VOID BARecSessionTearDown(
+	IN OUT	PRTMP_ADAPTER	pAd,
+	IN		UCHAR			Wcid,
+	IN		UCHAR			TID,
+	IN		BOOLEAN			bPassive);
+
+BOOLEAN ba_reordering_resource_init(PRTMP_ADAPTER pAd, int num);
+void ba_reordering_resource_release(PRTMP_ADAPTER pAd);
+
+ULONG AutoChBssInsertEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR ChannelNo,
+	IN CHAR Rssi);
+
+void AutoChBssTableInit(
+	IN PRTMP_ADAPTER pAd);
+
+void ChannelInfoInit(
+	IN PRTMP_ADAPTER pAd);
+
+void AutoChBssTableDestroy(
+	IN PRTMP_ADAPTER pAd);
+
+void ChannelInfoDestroy(
+	IN PRTMP_ADAPTER pAd);
+
+UCHAR New_ApAutoSelectChannel(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN rtstrmactohex(
+	IN char *s1,
+	IN char *s2);
+
+BOOLEAN rtstrcasecmp(
+	IN char *s1,
+	IN char *s2);
+
+char *rtstrstruncasecmp(
+	IN char *s1,
+	IN char *s2);
+
+char    *rtstrstr(
+	IN	const char * s1,
+	IN	const char * s2);
+
+char *rstrtok(
+	IN char * s,
+	IN const char * ct);
+
+int rtinet_aton(
+	const char *cp,
+	unsigned int *addr);
+
+////////// common ioctl functions //////////
+INT Set_DriverVersion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ShortSlot_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_BGProtection_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_TxPreamble_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_RTSThreshold_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_FragThreshold_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_TxBurst_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+#ifdef AGGREGATION_SUPPORT
+INT	Set_PktAggregate_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+#endif
+
+INT	Set_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef DBG
+INT	Set_Debug_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif
+
+INT	Show_DescInfo_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ResetStatCounter_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef DOT11_N_SUPPORT
+INT	Set_BASetup_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_BADecline_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_BAOriTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_BARecTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtStbc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtHtc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtLinkAdapt_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtProtect_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMimoPs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+
+INT	Set_ForceShortGI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ForceGF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	SetCommonHT(
+	IN	PRTMP_ADAPTER	pAd);
+
+INT	Set_SendPSMPAction_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMIMOPSmode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+
+INT	Set_HtTxBASize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // DOT11_N_SUPPORT //
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+//Dls ,	kathy
+VOID RTMPSendDLSTearDownFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA);
+
+#ifdef DOT11_N_SUPPORT
+//Block ACK
+VOID QueryBATABLE(
+	IN  PRTMP_ADAPTER pAd,
+	OUT PQUERYBA_TABLE pBAT);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+INT	    WpaCheckEapCode(
+	IN  PRTMP_ADAPTER   	pAd,
+	IN  PUCHAR				pFrame,
+	IN  USHORT				FrameLen,
+	IN  USHORT				OffSet);
+
+VOID    WpaSendMicFailureToWpaSupplicant(
+    IN  PRTMP_ADAPTER       pAd,
+    IN  BOOLEAN             bUnicast);
+
+VOID    SendAssocIEsToWpaSupplicant(
+    IN  PRTMP_ADAPTER       pAd);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+int wext_notify_event_assoc(
+	IN  RTMP_ADAPTER *pAd);
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+#ifdef DOT11_N_SUPPORT
+VOID Handle_BSS_Width_Trigger_Events(
+	IN PRTMP_ADAPTER pAd);
+
+void build_ext_channel_switch_ie(
+	IN PRTMP_ADAPTER pAd,
+	IN HT_EXT_CHANNEL_SWITCH_ANNOUNCEMENT_IE *pIE);
+#endif // DOT11_N_SUPPORT //
+
+
+BOOLEAN APRxDoneInterruptHandle(
+	IN	PRTMP_ADAPTER	pAd);
+
+BOOLEAN STARxDoneInterruptHandle(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	BOOLEAN			argc);
+
+#ifdef DOT11_N_SUPPORT
+// AMPDU packet indication
+VOID Indicate_AMPDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+// AMSDU packet indication
+VOID Indicate_AMSDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+#endif // DOT11_N_SUPPORT //
+
+// Normal legacy Rx packet indication
+VOID Indicate_Legacy_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID Indicate_EAPOL_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+void  update_os_packet_info(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+void wlan_802_11_to_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	PUCHAR			pHeader802_3,
+	IN  UCHAR			FromWhichBSSID);
+
+UINT deaggregate_AMSDU_announce(
+	IN	PRTMP_ADAPTER	pAd,
+	PNDIS_PACKET		pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize);
+
+
+#ifdef CONFIG_STA_SUPPORT
+// remove LLC and get 802_3 Header
+#define  RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(_pRxBlk, _pHeader802_3)	\
+{																				\
+	PUCHAR _pRemovedLLCSNAP = NULL, _pDA, _pSA;                                 \
+																				\
+	if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_MESH))                                    \
+	{                                                                           \
+		_pDA = _pRxBlk->pHeader->Addr3;                                         \
+		_pSA = (PUCHAR)_pRxBlk->pHeader + sizeof(HEADER_802_11);                \
+	}                                                                           \
+	else                                                                        \
+	{                                                                           \
+		if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_INFRA))                              	\
+		{                                                                       \
+			_pDA = _pRxBlk->pHeader->Addr1;                                     \
+		if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_DLS))									\
+			_pSA = _pRxBlk->pHeader->Addr2;										\
+		else																	\
+			_pSA = _pRxBlk->pHeader->Addr3;                                     \
+		}                                                                       \
+		else                                                                    \
+		{                                                                       \
+			_pDA = _pRxBlk->pHeader->Addr1;                                     \
+			_pSA = _pRxBlk->pHeader->Addr2;                                     \
+		}                                                                       \
+	}                                                                           \
+																				\
+	CONVERT_TO_802_3(_pHeader802_3, _pDA, _pSA, _pRxBlk->pData, 				\
+		_pRxBlk->DataSize, _pRemovedLLCSNAP);                                   \
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+BOOLEAN APFowardWirelessStaToWirelessSta(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	ULONG			FromWhichBSSID);
+
+VOID Announce_or_Forward_802_3_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID Sta_Announce_or_Forward_802_3_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID);
+
+
+#ifdef CONFIG_STA_SUPPORT
+#define ANNOUNCE_OR_FORWARD_802_3_PACKET(_pAd, _pPacket, _FromWhichBSS)\
+			Sta_Announce_or_Forward_802_3_Packet(_pAd, _pPacket, _FromWhichBSS);
+			//announce_802_3_packet(_pAd, _pPacket);
+#endif // CONFIG_STA_SUPPORT //
+
+
+PNDIS_PACKET DuplicatePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID);
+
+
+PNDIS_PACKET ClonePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize);
+
+
+// Normal, AMPDU or AMSDU
+VOID CmmRxnonRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID CmmRxRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID Update_Rssi_Sample(
+	IN PRTMP_ADAPTER	pAd,
+	IN RSSI_SAMPLE		*pRssi,
+	IN PRXWI_STRUC		pRxWI);
+
+PNDIS_PACKET GetPacketFromRxRing(
+	IN		PRTMP_ADAPTER	pAd,
+	OUT		PRT28XX_RXD_STRUC		pSaveRxD,
+	OUT		BOOLEAN			*pbReschedule,
+	IN OUT	UINT32			*pRxPending);
+
+PNDIS_PACKET RTMPDeFragmentDataFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk);
+
+////////////////////////////////////////
+
+
+
+
+
+#ifdef SNMP_SUPPORT
+//for snmp , kathy
+typedef struct _DefaultKeyIdxValue
+{
+	UCHAR	KeyIdx;
+	UCHAR	Value[16];
+} DefaultKeyIdxValue, *PDefaultKeyIdxValue;
+#endif
+
+
+#ifdef CONFIG_STA_SUPPORT
+enum {
+	DIDmsg_lnxind_wlansniffrm		= 0x00000044,
+	DIDmsg_lnxind_wlansniffrm_hosttime	= 0x00010044,
+	DIDmsg_lnxind_wlansniffrm_mactime	= 0x00020044,
+	DIDmsg_lnxind_wlansniffrm_channel	= 0x00030044,
+	DIDmsg_lnxind_wlansniffrm_rssi		= 0x00040044,
+	DIDmsg_lnxind_wlansniffrm_sq		= 0x00050044,
+	DIDmsg_lnxind_wlansniffrm_signal	= 0x00060044,
+	DIDmsg_lnxind_wlansniffrm_noise		= 0x00070044,
+	DIDmsg_lnxind_wlansniffrm_rate		= 0x00080044,
+	DIDmsg_lnxind_wlansniffrm_istx		= 0x00090044,
+	DIDmsg_lnxind_wlansniffrm_frmlen	= 0x000A0044
+};
+enum {
+	P80211ENUM_msgitem_status_no_value	= 0x00
+};
+enum {
+	P80211ENUM_truth_false			= 0x00,
+	P80211ENUM_truth_true			= 0x01
+};
+
+/* Definition from madwifi */
+typedef struct {
+        UINT32 did;
+        UINT16 status;
+        UINT16 len;
+        UINT32 data;
+} p80211item_uint32_t;
+
+typedef struct {
+        UINT32 msgcode;
+        UINT32 msglen;
+#define WLAN_DEVNAMELEN_MAX 16
+        UINT8 devname[WLAN_DEVNAMELEN_MAX];
+        p80211item_uint32_t hosttime;
+        p80211item_uint32_t mactime;
+        p80211item_uint32_t channel;
+        p80211item_uint32_t rssi;
+        p80211item_uint32_t sq;
+        p80211item_uint32_t signal;
+        p80211item_uint32_t noise;
+        p80211item_uint32_t rate;
+        p80211item_uint32_t istx;
+        p80211item_uint32_t frmlen;
+} wlan_ng_prism2_header;
+
+/* The radio capture header precedes the 802.11 header. */
+typedef struct PACKED _ieee80211_radiotap_header {
+    UINT8	it_version;	/* Version 0. Only increases
+				 * for drastic changes,
+				 * introduction of compatible
+				 * new fields does not count.
+				 */
+    UINT8	it_pad;
+    UINT16     it_len;         /* length of the whole
+				 * header in bytes, including
+				 * it_version, it_pad,
+				 * it_len, and data fields.
+				 */
+    UINT32   it_present;	/* A bitmap telling which
+					 * fields are present. Set bit 31
+					 * (0x80000000) to extend the
+					 * bitmap by another 32 bits.
+					 * Additional extensions are made
+					 * by setting bit 31.
+					 */
+}ieee80211_radiotap_header ;
+
+enum ieee80211_radiotap_type {
+    IEEE80211_RADIOTAP_TSFT = 0,
+    IEEE80211_RADIOTAP_FLAGS = 1,
+    IEEE80211_RADIOTAP_RATE = 2,
+    IEEE80211_RADIOTAP_CHANNEL = 3,
+    IEEE80211_RADIOTAP_FHSS = 4,
+    IEEE80211_RADIOTAP_DBM_ANTSIGNAL = 5,
+    IEEE80211_RADIOTAP_DBM_ANTNOISE = 6,
+    IEEE80211_RADIOTAP_LOCK_QUALITY = 7,
+    IEEE80211_RADIOTAP_TX_ATTENUATION = 8,
+    IEEE80211_RADIOTAP_DB_TX_ATTENUATION = 9,
+    IEEE80211_RADIOTAP_DBM_TX_POWER = 10,
+    IEEE80211_RADIOTAP_ANTENNA = 11,
+    IEEE80211_RADIOTAP_DB_ANTSIGNAL = 12,
+    IEEE80211_RADIOTAP_DB_ANTNOISE = 13
+};
+
+#define WLAN_RADIOTAP_PRESENT (			\
+	(1 << IEEE80211_RADIOTAP_TSFT)	|	\
+	(1 << IEEE80211_RADIOTAP_FLAGS) |	\
+	(1 << IEEE80211_RADIOTAP_RATE)  | 	\
+	 0)
+
+typedef struct _wlan_radiotap_header {
+	ieee80211_radiotap_header wt_ihdr;
+	INT64 wt_tsft;
+	UINT8 wt_flags;
+	UINT8 wt_rate;
+} wlan_radiotap_header;
+/* Definition from madwifi */
+
+void send_monitor_packets(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk);
+
+#if WIRELESS_EXT >= 12
+// This function will be called when query /proc
+struct iw_statistics *rt28xx_get_wireless_stats(
+    IN struct net_device *net_dev);
+#endif
+
+VOID    RTMPSetDesiredRates(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  LONG            Rates);
+#endif // CONFIG_STA_SUPPORT //
+
+INT	Set_FixedTxMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+INT	Set_OpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+static inline char* GetPhyMode(
+	int Mode)
+{
+	switch(Mode)
+	{
+		case MODE_CCK:
+			return "CCK";
+
+		case MODE_OFDM:
+			return "OFDM";
+#ifdef DOT11_N_SUPPORT
+		case MODE_HTMIX:
+			return "HTMIX";
+
+		case MODE_HTGREENFIELD:
+			return "GREEN";
+#endif // DOT11_N_SUPPORT //
+		default:
+			return "N/A";
+	}
+}
+
+
+static inline char* GetBW(
+	int BW)
+{
+	switch(BW)
+	{
+		case BW_10:
+			return "10M";
+
+		case BW_20:
+			return "20M";
+#ifdef DOT11_N_SUPPORT
+		case BW_40:
+			return "40M";
+#endif // DOT11_N_SUPPORT //
+		default:
+			return "N/A";
+	}
+}
+
+
+VOID RT28xxThreadTerminate(
+	IN RTMP_ADAPTER *pAd);
+
+BOOLEAN RT28XXChipsetCheck(
+	IN void *_dev_p);
+
+BOOLEAN RT28XXNetDevInit(
+	IN void 				*_dev_p,
+	IN struct  net_device	*net_dev,
+	IN RTMP_ADAPTER 		*pAd);
+
+BOOLEAN RT28XXProbePostConfig(
+	IN void 				*_dev_p,
+	IN RTMP_ADAPTER 		*pAd,
+	IN INT32				argc);
+
+VOID RT28XXDMADisable(
+	IN RTMP_ADAPTER 		*pAd);
+
+VOID RT28XXDMAEnable(
+	IN RTMP_ADAPTER 		*pAd);
+
+VOID RT28xx_UpdateBeaconToAsic(
+	IN RTMP_ADAPTER * pAd,
+	IN INT apidx,
+	IN ULONG BeaconLen,
+	IN ULONG UpdatePos);
+
+INT rt28xx_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT			cmd);
+
+
+#ifdef CONFIG_STA_SUPPORT
+INT rt28xx_sta_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT			cmd);
+#endif // CONFIG_STA_SUPPORT //
+
+BOOLEAN RT28XXSecurityKeyAdd(
+	IN		PRTMP_ADAPTER		pAd,
+	IN		ULONG				apidx,
+	IN		ULONG				KeyIdx,
+	IN		MAC_TABLE_ENTRY 	*pEntry);
+
+////////////////////////////////////////
+PNDIS_PACKET GetPacketFromRxRing(
+	IN		PRTMP_ADAPTER	pAd,
+	OUT		PRT28XX_RXD_STRUC	pSaveRxD,
+	OUT		BOOLEAN			*pbReschedule,
+	IN OUT	UINT32			*pRxPending);
+
+
+void kill_thread_task(PRTMP_ADAPTER pAd);
+
+void tbtt_tasklet(unsigned long data);
+
+#ifdef RT2860
+//
+// Function Prototype in cmm_data_2860.c
+//
+USHORT RtmpPCI_WriteTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber);
+
+USHORT RtmpPCI_WriteSingleTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber);
+
+USHORT RtmpPCI_WriteMultiTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			frameNum,
+	OUT	USHORT			*FreeNumber);
+
+USHORT	RtmpPCI_WriteFragTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			fragNum,
+	OUT	USHORT			*FreeNumber);
+
+USHORT RtmpPCI_WriteSubTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber);
+
+VOID RtmpPCI_FinalWriteTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	USHORT			totalMPDUSize,
+	IN	USHORT			FirstTxIdx);
+
+VOID RtmpPCIDataLastTxIdx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	USHORT			LastTxIdx);
+
+VOID RtmpPCIDataKickOut(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			QueIdx);
+
+
+int RtmpPCIMgmtKickOut(
+	IN RTMP_ADAPTER 	*pAd,
+	IN UCHAR 			QueIdx,
+	IN PNDIS_PACKET		pPacket,
+	IN PUCHAR			pSrcBufVA,
+	IN UINT 			SrcBufLen);
+
+
+NDIS_STATUS RTMPCheckRxError(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PHEADER_802_11  pHeader,
+	IN	PRXWI_STRUC	pRxWI,
+	IN  PRT28XX_RXD_STRUC      pRxD);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPInitPCIeLinkCtrlValue(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTMPFindHostPCIDev(
+    IN	PRTMP_ADAPTER	pAd);
+
+VOID RTMPPCIeLinkCtrlValueRestore(
+	IN	PRTMP_ADAPTER	pAd,
+	IN   UCHAR		Level);
+
+VOID RTMPPCIeLinkCtrlSetting(
+	IN	PRTMP_ADAPTER	pAd,
+	IN 	USHORT		Max);
+
+VOID RT28xxPciAsicRadioOff(
+	IN PRTMP_ADAPTER    pAd,
+	IN UCHAR            Level,
+	IN USHORT           TbttNumToNextWakeUp);
+
+BOOLEAN RT28xxPciAsicRadioOn(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR     Level);
+
+VOID RT28xxPciStaAsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN       bFromTx);
+
+VOID RT28xxPciStaAsicSleepThenAutoWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TbttNumToNextWakeUp);
+
+VOID PsPollWakeExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID  RadioOnExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+#endif // CONFIG_STA_SUPPORT //
+
+VOID RT28xxPciMlmeRadioOn(
+	IN PRTMP_ADAPTER pAd);
+
+VOID RT28xxPciMlmeRadioOFF(
+	IN PRTMP_ADAPTER pAd);
+#endif // RT2860 //
+
+VOID AsicTurnOffRFClk(
+	IN PRTMP_ADAPTER    pAd,
+	IN	UCHAR           Channel);
+
+VOID AsicTurnOnRFClk(
+	IN PRTMP_ADAPTER 	pAd,
+	IN	UCHAR			Channel);
+
+
+////////////////////////////////////////
+
+VOID QBSS_LoadInit(
+ 	IN		RTMP_ADAPTER	*pAd);
+
+UINT32 QBSS_LoadElementAppend(
+ 	IN		RTMP_ADAPTER	*pAd,
+	OUT		UINT8			*buf_p);
+
+VOID QBSS_LoadUpdate(
+ 	IN		RTMP_ADAPTER	*pAd);
+
+///////////////////////////////////////
+INT RTMPShowCfgValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pName,
+	IN	PUCHAR			pBuf);
+
+PCHAR   RTMPGetRalinkAuthModeStr(
+    IN  NDIS_802_11_AUTHENTICATION_MODE authMode);
+
+PCHAR   RTMPGetRalinkEncryModeStr(
+    IN  USHORT encryMode);
+//////////////////////////////////////
+
+#ifdef CONFIG_STA_SUPPORT
+VOID AsicStaBbpTuning(
+	IN PRTMP_ADAPTER pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+void RTMP_IndicateMediaState(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID ReSyncBeaconTime(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPSetAGCInitValue(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			BandWidth);
+
+int rt28xx_close(IN PNET_DEV dev);
+int rt28xx_open(IN PNET_DEV dev);
+
+__inline INT VIRTUAL_IF_UP(PRTMP_ADAPTER pAd)
+{
+extern VOID MeshMakeBeacon(IN PRTMP_ADAPTER pAd, IN UCHAR idx);
+extern VOID MeshUpdateBeaconFrame(IN PRTMP_ADAPTER pAd, IN UCHAR idx);
+
+	if (VIRTUAL_IF_NUM(pAd) == 0)
+	{
+		if (rt28xx_open(pAd->net_dev) != 0)
+			return -1;
+	}
+	else
+	{
+	}
+	VIRTUAL_IF_INC(pAd);
+	return 0;
+}
+
+__inline VOID VIRTUAL_IF_DOWN(PRTMP_ADAPTER pAd)
+{
+	VIRTUAL_IF_DEC(pAd);
+	if (VIRTUAL_IF_NUM(pAd) == 0)
+		rt28xx_close(pAd->net_dev);
+	return;
+}
+
+
+#endif  // __RTMP_H__
+
diff --git a/drivers/staging/rt2860/rtmp_ckipmic.h b/drivers/staging/rt2860/rtmp_ckipmic.h
new file mode 100644
index 000000000000..a3d949a39d39
--- /dev/null
+++ b/drivers/staging/rt2860/rtmp_ckipmic.h
@@ -0,0 +1,113 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_ckipmic.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#ifndef	__RTMP_CKIPMIC_H__
+#define	__RTMP_CKIPMIC_H__
+
+typedef	struct	_MIC_CONTEXT	{
+	/* --- MMH context                            */
+	UCHAR		CK[16];				/* the key                                    */
+	UCHAR		coefficient[16];	/* current aes counter mode coefficients      */
+	ULONGLONG	accum;				/* accumulated mic, reduced to u32 in final() */
+	UINT		position;			/* current position (byte offset) in message  */
+	UCHAR		part[4];			/* for conversion of message to u32 for mmh   */
+}	MIC_CONTEXT, *PMIC_CONTEXT;
+
+VOID	CKIP_key_permute(
+	OUT	UCHAR	*PK,			/* output permuted key */
+	IN	UCHAR	*CK,			/* input CKIP key */
+	IN	UCHAR	toDsFromDs,		/* input toDs/FromDs bits */
+	IN	UCHAR	*piv);			/* input pointer to IV */
+
+VOID	RTMPCkipMicInit(
+	IN	PMIC_CONTEXT		pContext,
+	IN	PUCHAR				CK);
+
+VOID RTMPMicUpdate(
+    IN  PMIC_CONTEXT        pContext,
+    IN  PUCHAR              pOctets,
+    IN  INT                 len);
+
+ULONG RTMPMicGetCoefficient(
+    IN  PMIC_CONTEXT         pContext);
+
+VOID xor_128(
+    IN  PUCHAR              a,
+    IN  PUCHAR              b,
+    OUT PUCHAR              out);
+
+UCHAR RTMPCkipSbox(
+    IN  UCHAR               a);
+
+VOID xor_32(
+    IN  PUCHAR              a,
+    IN  PUCHAR              b,
+    OUT PUCHAR              out);
+
+VOID next_key(
+    IN  PUCHAR              key,
+    IN  INT                 round);
+
+VOID byte_sub(
+    IN  PUCHAR              in,
+    OUT PUCHAR              out);
+
+VOID shift_row(
+    IN  PUCHAR              in,
+    OUT PUCHAR              out);
+
+VOID mix_column(
+    IN  PUCHAR              in,
+    OUT PUCHAR              out);
+
+VOID RTMPAesEncrypt(
+    IN  PUCHAR              key,
+    IN  PUCHAR              data,
+    IN  PUCHAR              ciphertext);
+
+VOID RTMPMicFinal(
+    IN  PMIC_CONTEXT        pContext,
+    OUT UCHAR               digest[4]);
+
+VOID RTMPCkipInsertCMIC(
+    IN  PRTMP_ADAPTER   pAd,
+    OUT PUCHAR          pMIC,
+    IN  PUCHAR          p80211hdr,
+    IN  PNDIS_PACKET    pPacket,
+    IN  PCIPHER_KEY     pKey,
+    IN  PUCHAR          mic_snap);
+
+#endif //__RTMP_CKIPMIC_H__
diff --git a/drivers/staging/rt2860/rtmp_def.h b/drivers/staging/rt2860/rtmp_def.h
new file mode 100644
index 000000000000..be982147883f
--- /dev/null
+++ b/drivers/staging/rt2860/rtmp_def.h
@@ -0,0 +1,1588 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rtmp_def.h
+
+    Abstract:
+    Miniport related definition header
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Paul Lin    08-01-2002    created
+    John Chang  08-05-2003    add definition for 11g & other drafts
+*/
+#ifndef __RTMP_DEF_H__
+#define __RTMP_DEF_H__
+
+#include "oid.h"
+
+//
+//  Debug information verbosity: lower values indicate higher urgency
+//
+#define RT_DEBUG_OFF        0
+#define RT_DEBUG_ERROR      1
+#define RT_DEBUG_WARN       2
+#define RT_DEBUG_TRACE      3
+#define RT_DEBUG_INFO       4
+#define RT_DEBUG_LOUD       5
+
+#define NIC_TAG             ((ULONG)'0682')
+#define NIC_DBG_STRING      ("**RT28xx**")
+
+#ifdef SNMP_SUPPORT
+// for snmp
+// to get manufacturer OUI, kathy, 2008_0220
+#define ManufacturerOUI_LEN			3
+#define ManufacturerNAME			("Ralink Technology Company.")
+#define	ResourceTypeIdName			("Ralink_ID")
+#endif
+
+
+#define RALINK_2883_VERSION		((UINT32)0x28830300)
+#define RALINK_2880E_VERSION	((UINT32)0x28720200)
+#define RALINK_3070_VERSION		((UINT32)0x30700200)
+
+//
+// NDIS version in use by the NIC driver.
+// The high byte is the major version. The low byte is the minor version.
+//
+#ifdef  NDIS51_MINIPORT
+#define NIC_DRIVER_VERSION      0x0501
+#else
+#define NIC_DRIVER_VERSION      0x0500
+#endif
+
+//
+// NDIS media type, current is ethernet, change if native wireless supported
+//
+#define NIC_MEDIA_TYPE          NdisMedium802_3
+#define NIC_PCI_HDR_LENGTH      0xe2
+#define NIC_MAX_PACKET_SIZE     2304
+#define NIC_HEADER_SIZE         14
+#define MAX_MAP_REGISTERS_NEEDED 32
+#define MIN_MAP_REGISTERS_NEEDED 2   //Todo: should consider fragment issue.
+
+//
+// interface type, we use PCI
+//
+#define NIC_INTERFACE_TYPE      NdisInterfacePci
+#define NIC_INTERRUPT_MODE      NdisInterruptLevelSensitive
+
+//
+// buffer size passed in NdisMQueryAdapterResources
+// We should only need three adapter resources (IO, interrupt and memory),
+// Some devices get extra resources, so have room for 10 resources
+//                    UF_SIZE   (sizeof(NDIS_RESOURCE_LIST) + (10*sizeof(CM_PARTIAL_RESOURCE_DESCRIPTOR)))
+
+
+#define NIC_RESOURCE_B//
+// IO space length
+//
+#define NIC_MAP_IOSPACE_LENGTH  sizeof(CSR_STRUC)
+
+#define MAX_RX_PKT_LEN	1520
+
+//
+// Entry number for each DMA descriptor ring
+//
+
+#ifdef RT2860
+#define TX_RING_SIZE            64 //64
+#define MGMT_RING_SIZE          128
+#define RX_RING_SIZE            128 //64
+#define MAX_TX_PROCESS          TX_RING_SIZE //8
+#define MAX_DMA_DONE_PROCESS    TX_RING_SIZE
+#define MAX_TX_DONE_PROCESS     TX_RING_SIZE //8
+#define LOCAL_TXBUF_SIZE        2
+#endif // RT2860 //
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+// MC: Multple Cards
+#define MAX_NUM_OF_MULTIPLE_CARD		32
+#endif // MULTIPLE_CARD_SUPPORT //
+
+#define MAX_RX_PROCESS          128 //64 //32
+#define NUM_OF_LOCAL_TXBUF      2
+#define TXD_SIZE                16
+#define TXWI_SIZE               16
+#define RXD_SIZE               	16
+#define RXWI_SIZE             	16
+// TXINFO_SIZE + TXWI_SIZE + 802.11 Header Size + AMSDU sub frame header
+#define TX_DMA_1ST_BUFFER_SIZE  96    // only the 1st physical buffer is pre-allocated
+#define MGMT_DMA_BUFFER_SIZE    1536 //2048
+#define RX_BUFFER_AGGRESIZE     3840 //3904 //3968 //4096 //2048 //4096
+#define RX_BUFFER_NORMSIZE      3840 //3904 //3968 //4096 //2048 //4096
+#define TX_BUFFER_NORMSIZE		RX_BUFFER_NORMSIZE
+#define MAX_FRAME_SIZE          2346                    // Maximum 802.11 frame size
+#define MAX_AGGREGATION_SIZE    3840 //3904 //3968 //4096
+#define MAX_NUM_OF_TUPLE_CACHE  2
+#define MAX_MCAST_LIST_SIZE     32
+#define MAX_LEN_OF_VENDOR_DESC  64
+//#define MAX_SIZE_OF_MCAST_PSQ   (NUM_OF_LOCAL_TXBUF >> 2) // AP won't spend more than 1/4 of total buffers on M/BCAST PSQ
+#define MAX_SIZE_OF_MCAST_PSQ               32
+
+#define MAX_RX_PROCESS_CNT	(RX_RING_SIZE)
+
+
+#define MAX_PACKETS_IN_QUEUE				(512) //(512)    // to pass WMM A5-WPAPSK
+#define MAX_PACKETS_IN_MCAST_PS_QUEUE		32
+#define MAX_PACKETS_IN_PS_QUEUE				128	//32
+#define WMM_NUM_OF_AC                       4  /* AC0, AC1, AC2, and AC3 */
+
+
+
+// RxFilter
+#define STANORMAL	 0x17f97
+#define APNORMAL	 0x15f97
+//
+//  RTMP_ADAPTER flags
+//
+#define fRTMP_ADAPTER_MAP_REGISTER          0x00000001
+#define fRTMP_ADAPTER_INTERRUPT_IN_USE      0x00000002
+#define fRTMP_ADAPTER_HARDWARE_ERROR        0x00000004
+#define fRTMP_ADAPTER_SCATTER_GATHER        0x00000008
+#define fRTMP_ADAPTER_SEND_PACKET_ERROR     0x00000010
+#define fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS 0x00000020
+#define fRTMP_ADAPTER_HALT_IN_PROGRESS      0x00000040
+#define fRTMP_ADAPTER_RESET_IN_PROGRESS     0x00000080
+#define fRTMP_ADAPTER_NIC_NOT_EXIST         0x00000100
+#define fRTMP_ADAPTER_TX_RING_ALLOCATED     0x00000200
+#define fRTMP_ADAPTER_REMOVE_IN_PROGRESS    0x00000400
+#define fRTMP_ADAPTER_MIMORATE_INUSED       0x00000800
+#define fRTMP_ADAPTER_RX_RING_ALLOCATED     0x00001000
+#define fRTMP_ADAPTER_INTERRUPT_ACTIVE      0x00002000
+#define fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS  0x00004000
+#define	fRTMP_ADAPTER_REASSOC_IN_PROGRESS	0x00008000
+#define	fRTMP_ADAPTER_MEDIA_STATE_PENDING	0x00010000
+#define	fRTMP_ADAPTER_RADIO_OFF				0x00020000
+#define fRTMP_ADAPTER_BULKOUT_RESET			0x00040000
+#define	fRTMP_ADAPTER_BULKIN_RESET			0x00080000
+#define fRTMP_ADAPTER_RDG_ACTIVE			0x00100000
+#define fRTMP_ADAPTER_DYNAMIC_BE_TXOP_ACTIVE 0x00200000
+#define fRTMP_ADAPTER_SCAN_2040 			0x04000000
+#define	fRTMP_ADAPTER_RADIO_MEASUREMENT		0x08000000
+
+#define fRTMP_ADAPTER_START_UP         		0x10000000	//Devive already initialized and enabled Tx/Rx.
+#define fRTMP_ADAPTER_MEDIA_STATE_CHANGE    0x20000000
+#define fRTMP_ADAPTER_IDLE_RADIO_OFF        0x40000000
+
+//
+//  STA operation status flags
+//
+#define fOP_STATUS_INFRA_ON                 0x00000001
+#define fOP_STATUS_ADHOC_ON                 0x00000002
+#define fOP_STATUS_BG_PROTECTION_INUSED     0x00000004
+#define fOP_STATUS_SHORT_SLOT_INUSED        0x00000008
+#define fOP_STATUS_SHORT_PREAMBLE_INUSED    0x00000010
+#define fOP_STATUS_RECEIVE_DTIM             0x00000020
+#define fOP_STATUS_MEDIA_STATE_CONNECTED    0x00000080
+#define fOP_STATUS_WMM_INUSED               0x00000100
+#define fOP_STATUS_AGGREGATION_INUSED       0x00000200
+#define fOP_STATUS_DOZE                     0x00000400  // debug purpose
+#define fOP_STATUS_PIGGYBACK_INUSED         0x00000800  // piggy-back, and aggregation
+#define fOP_STATUS_APSD_INUSED				0x00001000
+#define fOP_STATUS_TX_AMSDU_INUSED			0x00002000
+#define fOP_STATUS_MAX_RETRY_ENABLED		0x00004000
+#define fOP_STATUS_WAKEUP_NOW               0x00008000
+#define fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE       0x00020000
+
+#ifdef DOT11N_DRAFT3
+#define fOP_STATUS_SCAN_2040               	    0x00040000
+#endif // DOT11N_DRAFT3 //
+
+#define CCKSETPROTECT		0x1
+#define OFDMSETPROTECT		0x2
+#define MM20SETPROTECT		0x4
+#define MM40SETPROTECT		0x8
+#define GF20SETPROTECT		0x10
+#define GR40SETPROTECT		0x20
+#define ALLN_SETPROTECT		(GR40SETPROTECT | GF20SETPROTECT | MM40SETPROTECT | MM20SETPROTECT)
+
+//
+//  AP's client table operation status flags
+//
+#define fCLIENT_STATUS_WMM_CAPABLE          0x00000001  // CLIENT can parse QOS DATA frame
+#define fCLIENT_STATUS_AGGREGATION_CAPABLE  0x00000002  // CLIENT can receive Ralink's proprietary TX aggregation frame
+#define fCLIENT_STATUS_PIGGYBACK_CAPABLE    0x00000004  // CLIENT support piggy-back
+#define fCLIENT_STATUS_AMSDU_INUSED			0x00000008
+#define fCLIENT_STATUS_SGI20_CAPABLE		0x00000010
+#define fCLIENT_STATUS_SGI40_CAPABLE		0x00000020
+#define fCLIENT_STATUS_TxSTBC_CAPABLE		0x00000040
+#define fCLIENT_STATUS_RxSTBC_CAPABLE		0x00000080
+#define fCLIENT_STATUS_HTC_CAPABLE			0x00000100
+#define fCLIENT_STATUS_RDG_CAPABLE			0x00000200
+#define fCLIENT_STATUS_MCSFEEDBACK_CAPABLE  0x00000400
+#define fCLIENT_STATUS_APSD_CAPABLE         0x00000800  /* UAPSD STATION */
+
+#ifdef DOT11N_DRAFT3
+#define fCLIENT_STATUS_BSSCOEXIST_CAPABLE	0x00001000
+#endif // DOT11N_DRAFT3 //
+
+#define fCLIENT_STATUS_RALINK_CHIPSET		0x00100000
+//
+//  STA configuration flags
+//
+
+// 802.11n Operating Mode Definition. 0-3 also used in ASICUPdateProtect switch case
+#define HT_NO_PROTECT	0
+#define HT_LEGACY_PROTECT	1
+#define HT_40_PROTECT	2
+#define HT_2040_PROTECT	3
+#define HT_RTSCTS_6M	7
+//following is our own definition in order to turn on our ASIC protection register in INFRASTRUCTURE.
+#define HT_ATHEROS	8	// rt2860c has problem with atheros chip. we need to turn on RTS/CTS .
+#define HT_FORCERTSCTS	9	// Force turn on RTS/CTS first. then go to evaluate if this force RTS is necessary.
+
+//
+// RX Packet Filter control flags. Apply on pAd->PacketFilter
+//
+#define fRX_FILTER_ACCEPT_DIRECT            NDIS_PACKET_TYPE_DIRECTED
+#define fRX_FILTER_ACCEPT_MULTICAST         NDIS_PACKET_TYPE_MULTICAST
+#define fRX_FILTER_ACCEPT_BROADCAST         NDIS_PACKET_TYPE_BROADCAST
+#define fRX_FILTER_ACCEPT_ALL_MULTICAST     NDIS_PACKET_TYPE_ALL_MULTICAST
+
+//
+// Error code section
+//
+// NDIS_ERROR_CODE_ADAPTER_NOT_FOUND
+#define ERRLOG_READ_PCI_SLOT_FAILED     0x00000101L
+#define ERRLOG_WRITE_PCI_SLOT_FAILED    0x00000102L
+#define ERRLOG_VENDOR_DEVICE_NOMATCH    0x00000103L
+
+// NDIS_ERROR_CODE_ADAPTER_DISABLED
+#define ERRLOG_BUS_MASTER_DISABLED      0x00000201L
+
+// NDIS_ERROR_CODE_UNSUPPORTED_CONFIGURATION
+#define ERRLOG_INVALID_SPEED_DUPLEX     0x00000301L
+#define ERRLOG_SET_SECONDARY_FAILED     0x00000302L
+
+// NDIS_ERROR_CODE_OUT_OF_RESOURCES
+#define ERRLOG_OUT_OF_MEMORY            0x00000401L
+#define ERRLOG_OUT_OF_SHARED_MEMORY     0x00000402L
+#define ERRLOG_OUT_OF_MAP_REGISTERS     0x00000403L
+#define ERRLOG_OUT_OF_BUFFER_POOL       0x00000404L
+#define ERRLOG_OUT_OF_NDIS_BUFFER       0x00000405L
+#define ERRLOG_OUT_OF_PACKET_POOL       0x00000406L
+#define ERRLOG_OUT_OF_NDIS_PACKET       0x00000407L
+#define ERRLOG_OUT_OF_LOOKASIDE_MEMORY  0x00000408L
+
+// NDIS_ERROR_CODE_HARDWARE_FAILURE
+#define ERRLOG_SELFTEST_FAILED          0x00000501L
+#define ERRLOG_INITIALIZE_ADAPTER       0x00000502L
+#define ERRLOG_REMOVE_MINIPORT          0x00000503L
+
+// NDIS_ERROR_CODE_RESOURCE_CONFLICT
+#define ERRLOG_MAP_IO_SPACE             0x00000601L
+#define ERRLOG_QUERY_ADAPTER_RESOURCES  0x00000602L
+#define ERRLOG_NO_IO_RESOURCE           0x00000603L
+#define ERRLOG_NO_INTERRUPT_RESOURCE    0x00000604L
+#define ERRLOG_NO_MEMORY_RESOURCE       0x00000605L
+
+
+// WDS definition
+#define	MAX_WDS_ENTRY               4
+#define WDS_PAIRWISE_KEY_OFFSET     60    // WDS links uses pairwise key#60 ~ 63 in ASIC pairwise key table
+
+#define	WDS_DISABLE_MODE            0
+#define	WDS_RESTRICT_MODE           1
+#define	WDS_BRIDGE_MODE             2
+#define	WDS_REPEATER_MODE           3
+#define	WDS_LAZY_MODE               4
+
+
+#define MAX_MESH_NUM				0
+
+#define MAX_APCLI_NUM				0
+#ifdef APCLI_SUPPORT
+#undef	MAX_APCLI_NUM
+#define MAX_APCLI_NUM				1
+#endif // APCLI_SUPPORT //
+
+#define MAX_MBSSID_NUM				1
+#ifdef MBSS_SUPPORT
+#undef	MAX_MBSSID_NUM
+#define MAX_MBSSID_NUM				(8 - MAX_MESH_NUM - MAX_APCLI_NUM)
+#endif // MBSS_SUPPORT //
+
+/* sanity check for apidx */
+#define MBSS_MR_APIDX_SANITY_CHECK(apidx) \
+    { if (apidx > MAX_MBSSID_NUM) { \
+          printk("%s> Error! apidx = %d > MAX_MBSSID_NUM!\n", __func__, apidx); \
+	  apidx = MAIN_MBSSID; } }
+
+#define VALID_WCID(_wcid)	((_wcid) > 0 && (_wcid) < MAX_LEN_OF_MAC_TABLE )
+
+#define MAIN_MBSSID                 0
+#define FIRST_MBSSID                1
+
+
+#define MAX_BEACON_SIZE				512
+// If the MAX_MBSSID_NUM is larger than 6,
+// it shall reserve some WCID space(wcid 222~253) for beacon frames.
+// -	these wcid 238~253 are reserved for beacon#6(ra6).
+// -	these wcid 222~237 are reserved for beacon#7(ra7).
+#if defined(MAX_MBSSID_NUM) && (MAX_MBSSID_NUM == 8)
+#define HW_RESERVED_WCID	222
+#elif defined(MAX_MBSSID_NUM) && (MAX_MBSSID_NUM == 7)
+#define HW_RESERVED_WCID	238
+#else
+#define HW_RESERVED_WCID	255
+#endif
+
+// Then dedicate wcid of DFS and Carrier-Sense.
+#define DFS_CTS_WCID 		(HW_RESERVED_WCID - 1)
+#define CS_CTS_WCID 		(HW_RESERVED_WCID - 2)
+#define LAST_SPECIFIC_WCID	(HW_RESERVED_WCID - 2)
+
+// If MAX_MBSSID_NUM is 8, the maximum available wcid for the associated STA is 211.
+// If MAX_MBSSID_NUM is 7, the maximum available wcid for the associated STA is 228.
+#define MAX_AVAILABLE_CLIENT_WCID	(LAST_SPECIFIC_WCID - MAX_MBSSID_NUM - 1)
+
+// TX need WCID to find Cipher Key
+// these wcid 212 ~ 219 are reserved for bc/mc packets if MAX_MBSSID_NUM is 8.
+#define GET_GroupKey_WCID(__wcid, __bssidx) \
+	{										\
+		__wcid = LAST_SPECIFIC_WCID - (MAX_MBSSID_NUM) + __bssidx;	\
+	}
+
+#define IsGroupKeyWCID(__wcid) (((__wcid) < LAST_SPECIFIC_WCID) && ((__wcid) >= (LAST_SPECIFIC_WCID - (MAX_MBSSID_NUM))))
+
+
+// definition to support multiple BSSID
+#define BSS0                            0
+#define BSS1                            1
+#define BSS2                            2
+#define BSS3                            3
+#define BSS4                            4
+#define BSS5                            5
+#define BSS6                            6
+#define BSS7                            7
+
+
+//============================================================
+// Length definitions
+#define PEER_KEY_NO                     2
+#define MAC_ADDR_LEN                    6
+#define TIMESTAMP_LEN                   8
+#define MAX_LEN_OF_SUPPORTED_RATES      MAX_LENGTH_OF_SUPPORT_RATES // 1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54
+#define MAX_LEN_OF_KEY                  32      // 32 octets == 256 bits, Redefine for WPA
+#define MAX_NUM_OF_CHANNELS             MAX_NUM_OF_CHS      // 14 channels @2.4G +  12@UNII + 4 @MMAC + 11 @HiperLAN2 + 7 @Japan + 1 as NULL termination
+#define MAX_NUM_OF_11JCHANNELS             20      // 14 channels @2.4G +  12@UNII + 4 @MMAC + 11 @HiperLAN2 + 7 @Japan + 1 as NULL termination
+#define MAX_LEN_OF_SSID                 32
+#define CIPHER_TEXT_LEN                 128
+#define HASH_TABLE_SIZE                 256
+#define MAX_VIE_LEN                     1024   // New for WPA cipher suite variable IE sizes.
+#define MAX_SUPPORT_MCS             32
+
+//============================================================
+// ASIC WCID Table definition.
+//============================================================
+#define BSSID_WCID		1	// in infra mode, always put bssid with this WCID
+#define MCAST_WCID	0x0
+#define BSS0Mcast_WCID	0x0
+#define BSS1Mcast_WCID	0xf8
+#define BSS2Mcast_WCID	0xf9
+#define BSS3Mcast_WCID	0xfa
+#define BSS4Mcast_WCID	0xfb
+#define BSS5Mcast_WCID	0xfc
+#define BSS6Mcast_WCID	0xfd
+#define BSS7Mcast_WCID	0xfe
+#define RESERVED_WCID		0xff
+
+#define MAX_NUM_OF_ACL_LIST				MAX_NUMBER_OF_ACL
+
+#define MAX_LEN_OF_MAC_TABLE            MAX_NUMBER_OF_MAC // if MAX_MBSSID_NUM is 8, this value can't be larger than 211
+
+#if MAX_LEN_OF_MAC_TABLE>MAX_AVAILABLE_CLIENT_WCID
+#error MAX_LEN_OF_MAC_TABLE can not be larger than MAX_AVAILABLE_CLIENT_WCID!!!!
+#endif
+
+#define MAX_NUM_OF_WDS_LINK_PERBSSID	            3
+#define MAX_NUM_OF_WDS_LINK	            (MAX_NUM_OF_WDS_LINK_PERBSSID*MAX_MBSSID_NUM)
+#define MAX_NUM_OF_EVENT                MAX_NUMBER_OF_EVENT
+#define WDS_LINK_START_WCID				(MAX_LEN_OF_MAC_TABLE-1)
+
+#define NUM_OF_TID			8
+#define MAX_AID_BA                    4
+#define MAX_LEN_OF_BA_REC_TABLE          ((NUM_OF_TID * MAX_LEN_OF_MAC_TABLE)/2)//   (NUM_OF_TID*MAX_AID_BA + 32)	 //Block ACK recipient
+#define MAX_LEN_OF_BA_ORI_TABLE          ((NUM_OF_TID * MAX_LEN_OF_MAC_TABLE)/2)//   (NUM_OF_TID*MAX_AID_BA + 32)   // Block ACK originator
+#define MAX_LEN_OF_BSS_TABLE             64
+#define MAX_REORDERING_MPDU_NUM			 512
+
+// key related definitions
+#define SHARE_KEY_NUM                   4
+#define MAX_LEN_OF_SHARE_KEY            16    // byte count
+#define MAX_LEN_OF_PEER_KEY             16    // byte count
+#define PAIRWISE_KEY_NUM                64    // in MAC ASIC pairwise key table
+#define GROUP_KEY_NUM                   4
+#define PMK_LEN                         32
+#define WDS_PAIRWISE_KEY_OFFSET         60    // WDS links uses pairwise key#60 ~ 63 in ASIC pairwise key table
+#define	PMKID_NO                        4     // Number of PMKID saved supported
+#define MAX_LEN_OF_MLME_BUFFER          2048
+
+// power status related definitions
+#define PWR_ACTIVE                      0
+#define PWR_SAVE                        1
+#define PWR_MMPS                        2			//MIMO power save
+
+// Auth and Assoc mode related definitions
+#define AUTH_MODE_OPEN                  0x00
+#define AUTH_MODE_KEY                   0x01
+
+// BSS Type definitions
+#define BSS_ADHOC                       0  // = Ndis802_11IBSS
+#define BSS_INFRA                       1  // = Ndis802_11Infrastructure
+#define BSS_ANY                         2  // = Ndis802_11AutoUnknown
+#define BSS_MONITOR			            3  // = Ndis802_11Monitor
+
+
+// Reason code definitions
+#define REASON_RESERVED                 0
+#define REASON_UNSPECIFY                1
+#define REASON_NO_LONGER_VALID          2
+#define REASON_DEAUTH_STA_LEAVING       3
+#define REASON_DISASSOC_INACTIVE        4
+#define REASON_DISASSPC_AP_UNABLE       5
+#define REASON_CLS2ERR                  6
+#define REASON_CLS3ERR                  7
+#define REASON_DISASSOC_STA_LEAVING     8
+#define REASON_STA_REQ_ASSOC_NOT_AUTH   9
+#define REASON_INVALID_IE               13
+#define REASON_MIC_FAILURE              14
+#define REASON_4_WAY_TIMEOUT            15
+#define REASON_GROUP_KEY_HS_TIMEOUT     16
+#define REASON_IE_DIFFERENT             17
+#define REASON_MCIPHER_NOT_VALID        18
+#define REASON_UCIPHER_NOT_VALID        19
+#define REASON_AKMP_NOT_VALID           20
+#define REASON_UNSUPPORT_RSNE_VER       21
+#define REASON_INVALID_RSNE_CAP         22
+#define REASON_8021X_AUTH_FAIL          23
+#define REASON_CIPHER_SUITE_REJECTED    24
+#define REASON_DECLINED                 37
+
+#define REASON_QOS_UNSPECIFY              32
+#define REASON_QOS_LACK_BANDWIDTH         33
+#define REASON_POOR_CHANNEL_CONDITION     34
+#define REASON_QOS_OUTSIDE_TXOP_LIMITION  35
+#define REASON_QOS_QSTA_LEAVING_QBSS      36
+#define REASON_QOS_UNWANTED_MECHANISM     37
+#define REASON_QOS_MECH_SETUP_REQUIRED    38
+#define REASON_QOS_REQUEST_TIMEOUT        39
+#define REASON_QOS_CIPHER_NOT_SUPPORT     45
+
+// Status code definitions
+#define MLME_SUCCESS                    0
+#define MLME_UNSPECIFY_FAIL             1
+#define MLME_CANNOT_SUPPORT_CAP         10
+#define MLME_REASSOC_DENY_ASSOC_EXIST   11
+#define MLME_ASSOC_DENY_OUT_SCOPE       12
+#define MLME_ALG_NOT_SUPPORT            13
+#define MLME_SEQ_NR_OUT_OF_SEQUENCE     14
+#define MLME_REJ_CHALLENGE_FAILURE      15
+#define MLME_REJ_TIMEOUT                  16
+#define MLME_ASSOC_REJ_UNABLE_HANDLE_STA  17
+#define MLME_ASSOC_REJ_DATA_RATE          18
+
+#define MLME_ASSOC_REJ_NO_EXT_RATE        22
+#define MLME_ASSOC_REJ_NO_EXT_RATE_PBCC   23
+#define MLME_ASSOC_REJ_NO_CCK_OFDM        24
+
+#define MLME_QOS_UNSPECIFY                32
+#define MLME_REQUEST_DECLINED             37
+#define MLME_REQUEST_WITH_INVALID_PARAM   38
+#define MLME_DLS_NOT_ALLOW_IN_QBSS        48
+#define MLME_DEST_STA_NOT_IN_QBSS         49
+#define MLME_DEST_STA_IS_NOT_A_QSTA       50
+
+#define MLME_INVALID_FORMAT             0x51
+#define MLME_FAIL_NO_RESOURCE           0x52
+#define MLME_STATE_MACHINE_REJECT       0x53
+#define MLME_MAC_TABLE_FAIL             0x54
+
+// IE code
+#define IE_SSID                         0
+#define IE_SUPP_RATES                   1
+#define IE_FH_PARM                      2
+#define IE_DS_PARM                      3
+#define IE_CF_PARM                      4
+#define IE_TIM                          5
+#define IE_IBSS_PARM                    6
+#define IE_COUNTRY                      7     // 802.11d
+#define IE_802_11D_REQUEST              10    // 802.11d
+#define IE_QBSS_LOAD                    11    // 802.11e d9
+#define IE_EDCA_PARAMETER               12    // 802.11e d9
+#define IE_TSPEC                        13    // 802.11e d9
+#define IE_TCLAS                        14    // 802.11e d9
+#define IE_SCHEDULE                     15    // 802.11e d9
+#define IE_CHALLENGE_TEXT               16
+#define IE_POWER_CONSTRAINT             32    // 802.11h d3.3
+#define IE_POWER_CAPABILITY             33    // 802.11h d3.3
+#define IE_TPC_REQUEST                  34    // 802.11h d3.3
+#define IE_TPC_REPORT                   35    // 802.11h d3.3
+#define IE_SUPP_CHANNELS                36    // 802.11h d3.3
+#define IE_CHANNEL_SWITCH_ANNOUNCEMENT  37    // 802.11h d3.3
+#define IE_MEASUREMENT_REQUEST          38    // 802.11h d3.3
+#define IE_MEASUREMENT_REPORT           39    // 802.11h d3.3
+#define IE_QUIET                        40    // 802.11h d3.3
+#define IE_IBSS_DFS                     41    // 802.11h d3.3
+#define IE_ERP                          42    // 802.11g
+#define IE_TS_DELAY                     43    // 802.11e d9
+#define IE_TCLAS_PROCESSING             44    // 802.11e d9
+#define IE_QOS_CAPABILITY               46    // 802.11e d6
+#define IE_HT_CAP                       45    // 802.11n d1. HT CAPABILITY. ELEMENT ID TBD
+#define IE_AP_CHANNEL_REPORT			51    // 802.11k d6
+#define IE_HT_CAP2                         52    // 802.11n d1. HT CAPABILITY. ELEMENT ID TBD
+#define IE_RSN                          48    // 802.11i d3.0
+#define IE_WPA2                         48    // WPA2
+#define IE_EXT_SUPP_RATES               50    // 802.11g
+#define IE_SUPP_REG_CLASS               59    // 802.11y. Supported regulatory classes.
+#define IE_EXT_CHANNEL_SWITCH_ANNOUNCEMENT	60	// 802.11n
+#define IE_ADD_HT                         61    // 802.11n d1. ADDITIONAL HT CAPABILITY. ELEMENT ID TBD
+#define IE_ADD_HT2                        53    // 802.11n d1. ADDITIONAL HT CAPABILITY. ELEMENT ID TBD
+
+
+// For 802.11n D3.03
+//#define IE_NEW_EXT_CHA_OFFSET             62    // 802.11n d1. New extension channel offset elemet
+#define IE_SECONDARY_CH_OFFSET		62	// 802.11n D3.03	Secondary Channel Offset element
+#define IE_2040_BSS_COEXIST               72    // 802.11n D3.0.3
+#define IE_2040_BSS_INTOLERANT_REPORT     73    // 802.11n D3.03
+#define IE_OVERLAPBSS_SCAN_PARM           74    // 802.11n D3.03
+#define IE_EXT_CAPABILITY                127   // 802.11n D3.03
+
+
+#define IE_WPA                          221   // WPA
+#define IE_VENDOR_SPECIFIC              221   // Wifi WMM (WME)
+
+#define OUI_BROADCOM_HT              51   //
+#define OUI_BROADCOM_HTADD              52   //
+#define OUI_PREN_HT_CAP              51   //
+#define OUI_PREN_ADD_HT              52   //
+
+// CCX information
+#define IE_AIRONET_CKIP                 133   // CCX1.0 ID 85H for CKIP
+#define IE_AP_TX_POWER                  150   // CCX 2.0 for AP transmit power
+#define IE_MEASUREMENT_CAPABILITY       221   // CCX 2.0
+#define IE_CCX_V2                       221
+#define IE_AIRONET_IPADDRESS            149   // CCX ID 95H for IP Address
+#define IE_AIRONET_CCKMREASSOC          156   // CCX ID 9CH for CCKM Reassociation Request element
+#define CKIP_NEGOTIATION_LENGTH         30
+#define AIRONET_IPADDRESS_LENGTH        10
+#define AIRONET_CCKMREASSOC_LENGTH      24
+
+// ========================================================
+// MLME state machine definition
+// ========================================================
+
+// STA MLME state mahcines
+#define ASSOC_STATE_MACHINE             1
+#define AUTH_STATE_MACHINE              2
+#define AUTH_RSP_STATE_MACHINE          3
+#define SYNC_STATE_MACHINE              4
+#define MLME_CNTL_STATE_MACHINE         5
+#define WPA_PSK_STATE_MACHINE           6
+#define LEAP_STATE_MACHINE              7
+#define AIRONET_STATE_MACHINE           8
+#define ACTION_STATE_MACHINE           9
+
+// AP MLME state machines
+#define AP_ASSOC_STATE_MACHINE          11
+#define AP_AUTH_STATE_MACHINE           12
+#define AP_AUTH_RSP_STATE_MACHINE       13
+#define AP_SYNC_STATE_MACHINE           14
+#define AP_CNTL_STATE_MACHINE           15
+#define AP_WPA_STATE_MACHINE            16
+
+#ifdef QOS_DLS_SUPPORT
+#define DLS_STATE_MACHINE               26
+#endif // QOS_DLS_SUPPORT //
+
+//
+// STA's CONTROL/CONNECT state machine: states, events, total function #
+//
+#define CNTL_IDLE                       0
+#define CNTL_WAIT_DISASSOC              1
+#define CNTL_WAIT_JOIN                  2
+#define CNTL_WAIT_REASSOC               3
+#define CNTL_WAIT_START                 4
+#define CNTL_WAIT_AUTH                  5
+#define CNTL_WAIT_ASSOC                 6
+#define CNTL_WAIT_AUTH2                 7
+#define CNTL_WAIT_OID_LIST_SCAN         8
+#define CNTL_WAIT_OID_DISASSOC          9
+
+#define MT2_ASSOC_CONF                  34
+#define MT2_AUTH_CONF                   35
+#define MT2_DEAUTH_CONF                 36
+#define MT2_DISASSOC_CONF               37
+#define MT2_REASSOC_CONF                38
+#define MT2_PWR_MGMT_CONF               39
+#define MT2_JOIN_CONF                   40
+#define MT2_SCAN_CONF                   41
+#define MT2_START_CONF                  42
+#define MT2_GET_CONF                    43
+#define MT2_SET_CONF                    44
+#define MT2_RESET_CONF                  45
+#define MT2_MLME_ROAMING_REQ            52
+
+#define CNTL_FUNC_SIZE                  1
+
+//
+// STA's ASSOC state machine: states, events, total function #
+//
+#define ASSOC_IDLE                      0
+#define ASSOC_WAIT_RSP                  1
+#define REASSOC_WAIT_RSP                2
+#define DISASSOC_WAIT_RSP               3
+#define MAX_ASSOC_STATE                 4
+
+#define ASSOC_MACHINE_BASE              0
+#define MT2_MLME_ASSOC_REQ              0
+#define MT2_MLME_REASSOC_REQ            1
+#define MT2_MLME_DISASSOC_REQ           2
+#define MT2_PEER_DISASSOC_REQ           3
+#define MT2_PEER_ASSOC_REQ              4
+#define MT2_PEER_ASSOC_RSP              5
+#define MT2_PEER_REASSOC_REQ            6
+#define MT2_PEER_REASSOC_RSP            7
+#define MT2_DISASSOC_TIMEOUT            8
+#define MT2_ASSOC_TIMEOUT               9
+#define MT2_REASSOC_TIMEOUT             10
+#define MAX_ASSOC_MSG                   11
+
+#define ASSOC_FUNC_SIZE                 (MAX_ASSOC_STATE * MAX_ASSOC_MSG)
+
+//
+// ACT state machine: states, events, total function #
+//
+#define ACT_IDLE                      0
+#define MAX_ACT_STATE                 1
+
+#define ACT_MACHINE_BASE              0
+
+//Those PEER_xx_CATE number is based on real Categary value in IEEE spec. Please don'es modify it by your self.
+//Category
+#define MT2_PEER_SPECTRUM_CATE              0
+#define MT2_PEER_QOS_CATE              1
+#define MT2_PEER_DLS_CATE             2
+#define MT2_PEER_BA_CATE             3
+#define MT2_PEER_PUBLIC_CATE             4
+#define MT2_PEER_RM_CATE             5
+#define MT2_PEER_HT_CATE             7	//	7.4.7
+#define MAX_PEER_CATE_MSG                   7
+#define MT2_MLME_ADD_BA_CATE             8
+#define MT2_MLME_ORI_DELBA_CATE             9
+#define MT2_MLME_REC_DELBA_CATE             10
+#define MT2_MLME_QOS_CATE              11
+#define MT2_MLME_DLS_CATE             12
+#define MT2_ACT_INVALID             13
+#define MAX_ACT_MSG                   14
+
+//Category field
+#define CATEGORY_SPECTRUM		0
+#define CATEGORY_QOS			1
+#define CATEGORY_DLS			2
+#define CATEGORY_BA			3
+#define CATEGORY_PUBLIC		4
+#define CATEGORY_RM			5
+#define CATEGORY_HT			7
+
+
+// DLS Action frame definition
+#define ACTION_DLS_REQUEST			0
+#define ACTION_DLS_RESPONSE			1
+#define ACTION_DLS_TEARDOWN			2
+
+//Spectrum  Action field value 802.11h 7.4.1
+#define SPEC_MRQ	0	// Request
+#define SPEC_MRP	1	//Report
+#define SPEC_TPCRQ	2
+#define SPEC_TPCRP	3
+#define SPEC_CHANNEL_SWITCH	4
+
+
+//BA  Action field value
+#define ADDBA_REQ	0
+#define ADDBA_RESP	1
+#define DELBA   2
+
+//Public's  Action field value in Public Category.  Some in 802.11y and some in 11n
+#define ACTION_BSS_2040_COEXIST				0	// 11n
+#define ACTION_DSE_ENABLEMENT					1	// 11y D9.0
+#define ACTION_DSE_DEENABLEMENT				2	// 11y D9.0
+#define ACTION_DSE_REG_LOCATION_ANNOUNCE	3	// 11y D9.0
+#define ACTION_EXT_CH_SWITCH_ANNOUNCE		4	// 11y D9.0
+#define ACTION_DSE_MEASUREMENT_REQ			5	// 11y D9.0
+#define ACTION_DSE_MEASUREMENT_REPORT		6	// 11y D9.0
+#define ACTION_MEASUREMENT_PILOT_ACTION		7  	// 11y D9.0
+#define ACTION_DSE_POWER_CONSTRAINT			8	// 11y D9.0
+
+
+//HT  Action field value
+#define NOTIFY_BW_ACTION				0
+#define SMPS_ACTION						1
+#define PSMP_ACTION   					2
+#define SETPCO_ACTION					3
+#define MIMO_CHA_MEASURE_ACTION			4
+#define MIMO_N_BEACONFORM				5
+#define MIMO_BEACONFORM					6
+#define ANTENNA_SELECT					7
+#define HT_INFO_EXCHANGE				8
+
+#define ACT_FUNC_SIZE                 (MAX_ACT_STATE * MAX_ACT_MSG)
+//
+// STA's AUTHENTICATION state machine: states, evvents, total function #
+//
+#define AUTH_REQ_IDLE                   0
+#define AUTH_WAIT_SEQ2                  1
+#define AUTH_WAIT_SEQ4                  2
+#define MAX_AUTH_STATE                  3
+
+#define AUTH_MACHINE_BASE               0
+#define MT2_MLME_AUTH_REQ               0
+#define MT2_PEER_AUTH_EVEN              1
+#define MT2_AUTH_TIMEOUT                2
+#define MAX_AUTH_MSG                    3
+
+#define AUTH_FUNC_SIZE                  (MAX_AUTH_STATE * MAX_AUTH_MSG)
+
+//
+// STA's AUTH_RSP state machine: states, events, total function #
+//
+#define AUTH_RSP_IDLE                   0
+#define AUTH_RSP_WAIT_CHAL              1
+#define MAX_AUTH_RSP_STATE              2
+
+#define AUTH_RSP_MACHINE_BASE           0
+#define MT2_AUTH_CHALLENGE_TIMEOUT      0
+#define MT2_PEER_AUTH_ODD               1
+#define MT2_PEER_DEAUTH                 2
+#define MAX_AUTH_RSP_MSG                3
+
+#define AUTH_RSP_FUNC_SIZE              (MAX_AUTH_RSP_STATE * MAX_AUTH_RSP_MSG)
+
+//
+// STA's SYNC state machine: states, events, total function #
+//
+#define SYNC_IDLE                       0  // merge NO_BSS,IBSS_IDLE,IBSS_ACTIVE and BSS in to 1 state
+#define JOIN_WAIT_BEACON                1
+#define SCAN_LISTEN                     2
+#define MAX_SYNC_STATE                  3
+
+#define SYNC_MACHINE_BASE               0
+#define MT2_MLME_SCAN_REQ               0
+#define MT2_MLME_JOIN_REQ               1
+#define MT2_MLME_START_REQ              2
+#define MT2_PEER_BEACON                 3
+#define MT2_PEER_PROBE_RSP              4
+#define MT2_PEER_ATIM                   5
+#define MT2_SCAN_TIMEOUT                6
+#define MT2_BEACON_TIMEOUT              7
+#define MT2_ATIM_TIMEOUT                8
+#define MT2_PEER_PROBE_REQ              9
+#define MAX_SYNC_MSG                    10
+
+#define SYNC_FUNC_SIZE                  (MAX_SYNC_STATE * MAX_SYNC_MSG)
+
+//Messages for the DLS state machine
+#define DLS_IDLE						0
+#define MAX_DLS_STATE					1
+
+#define DLS_MACHINE_BASE				0
+#define MT2_MLME_DLS_REQ			    0
+#define MT2_PEER_DLS_REQ			    1
+#define MT2_PEER_DLS_RSP			    2
+#define MT2_MLME_DLS_TEAR_DOWN		    3
+#define MT2_PEER_DLS_TEAR_DOWN		    4
+#define MAX_DLS_MSG				        5
+
+#define DLS_FUNC_SIZE					(MAX_DLS_STATE * MAX_DLS_MSG)
+
+//
+// STA's WPA-PSK State machine: states, events, total function #
+//
+#define WPA_PSK_IDLE					0
+#define MAX_WPA_PSK_STATE				1
+
+#define WPA_MACHINE_BASE                0
+#define MT2_EAPPacket                   0
+#define MT2_EAPOLStart                  1
+#define MT2_EAPOLLogoff                 2
+#define MT2_EAPOLKey                    3
+#define MT2_EAPOLASFAlert               4
+#define MAX_WPA_PSK_MSG                 5
+
+#define	WPA_PSK_FUNC_SIZE				(MAX_WPA_PSK_STATE * MAX_WPA_PSK_MSG)
+
+//
+// STA's CISCO-AIRONET State machine: states, events, total function #
+//
+#define AIRONET_IDLE					0
+#define	AIRONET_SCANNING				1
+#define MAX_AIRONET_STATE				2
+
+#define AIRONET_MACHINE_BASE		    0
+#define MT2_AIRONET_MSG				    0
+#define MT2_AIRONET_SCAN_REQ		    1
+#define MT2_AIRONET_SCAN_DONE		    2
+#define MAX_AIRONET_MSG				    3
+
+#define	AIRONET_FUNC_SIZE				(MAX_AIRONET_STATE * MAX_AIRONET_MSG)
+
+//
+// AP's CONTROL/CONNECT state machine: states, events, total function #
+//
+#define AP_CNTL_FUNC_SIZE               1
+
+//
+// AP's ASSOC state machine: states, events, total function #
+//
+#define AP_ASSOC_IDLE                   0
+#define AP_MAX_ASSOC_STATE              1
+
+#define AP_ASSOC_MACHINE_BASE           0
+#define APMT2_MLME_DISASSOC_REQ         0
+#define APMT2_PEER_DISASSOC_REQ         1
+#define APMT2_PEER_ASSOC_REQ            2
+#define APMT2_PEER_REASSOC_REQ          3
+#define APMT2_CLS3ERR                   4
+#define AP_MAX_ASSOC_MSG                5
+
+#define AP_ASSOC_FUNC_SIZE              (AP_MAX_ASSOC_STATE * AP_MAX_ASSOC_MSG)
+
+//
+// AP's AUTHENTICATION state machine: states, events, total function #
+//
+#define AP_AUTH_REQ_IDLE                0
+#define AP_MAX_AUTH_STATE               1
+
+#define AP_AUTH_MACHINE_BASE            0
+#define APMT2_MLME_DEAUTH_REQ           0
+#define APMT2_CLS2ERR                   1
+#define AP_MAX_AUTH_MSG                 2
+
+#define AP_AUTH_FUNC_SIZE               (AP_MAX_AUTH_STATE * AP_MAX_AUTH_MSG)
+
+//
+// AP's AUTH-RSP state machine: states, events, total function #
+//
+#define AP_AUTH_RSP_IDLE                0
+#define AP_MAX_AUTH_RSP_STATE           1
+
+#define AP_AUTH_RSP_MACHINE_BASE        0
+#define APMT2_AUTH_CHALLENGE_TIMEOUT    0
+#define APMT2_PEER_AUTH_ODD             1
+#define APMT2_PEER_DEAUTH               2
+#define AP_MAX_AUTH_RSP_MSG             3
+
+#define AP_AUTH_RSP_FUNC_SIZE           (AP_MAX_AUTH_RSP_STATE * AP_MAX_AUTH_RSP_MSG)
+
+//
+// AP's SYNC state machine: states, events, total function #
+//
+#define AP_SYNC_IDLE                    0
+#define AP_SCAN_LISTEN					1
+#define AP_MAX_SYNC_STATE               2
+
+#define AP_SYNC_MACHINE_BASE            0
+#define APMT2_PEER_PROBE_REQ            0
+#define APMT2_PEER_BEACON               1
+#define APMT2_MLME_SCAN_REQ				2
+#define APMT2_PEER_PROBE_RSP			3
+#define APMT2_SCAN_TIMEOUT				4
+#define APMT2_MLME_SCAN_CNCL			5
+#define AP_MAX_SYNC_MSG                 6
+
+#define AP_SYNC_FUNC_SIZE               (AP_MAX_SYNC_STATE * AP_MAX_SYNC_MSG)
+
+//
+// AP's WPA state machine: states, events, total function #
+//
+#define AP_WPA_PTK                      0
+#define AP_MAX_WPA_PTK_STATE            1
+
+#define AP_WPA_MACHINE_BASE             0
+#define APMT2_EAPPacket                 0
+#define APMT2_EAPOLStart                1
+#define APMT2_EAPOLLogoff               2
+#define APMT2_EAPOLKey                  3
+#define APMT2_EAPOLASFAlert             4
+#define AP_MAX_WPA_MSG                  5
+
+#define AP_WPA_FUNC_SIZE                (AP_MAX_WPA_PTK_STATE * AP_MAX_WPA_MSG)
+
+#ifdef APCLI_SUPPORT
+//ApCli authentication state machine
+#define APCLI_AUTH_REQ_IDLE                0
+#define APCLI_AUTH_WAIT_SEQ2               1
+#define APCLI_AUTH_WAIT_SEQ4               2
+#define APCLI_MAX_AUTH_STATE               3
+
+#define APCLI_AUTH_MACHINE_BASE            0
+#define APCLI_MT2_MLME_AUTH_REQ            0
+#define APCLI_MT2_MLME_DEAUTH_REQ          1
+#define APCLI_MT2_PEER_AUTH_EVEN           2
+#define APCLI_MT2_PEER_DEAUTH              3
+#define APCLI_MT2_AUTH_TIMEOUT             4
+#define APCLI_MAX_AUTH_MSG                 5
+
+#define APCLI_AUTH_FUNC_SIZE               (APCLI_MAX_AUTH_STATE * APCLI_MAX_AUTH_MSG)
+
+//ApCli association state machine
+#define APCLI_ASSOC_IDLE                   0
+#define APCLI_ASSOC_WAIT_RSP               1
+#define APCLI_MAX_ASSOC_STATE              2
+
+#define APCLI_ASSOC_MACHINE_BASE           0
+#define APCLI_MT2_MLME_ASSOC_REQ           0
+#define APCLI_MT2_MLME_DISASSOC_REQ        1
+#define APCLI_MT2_PEER_DISASSOC_REQ        2
+#define APCLI_MT2_PEER_ASSOC_RSP           3
+#define APCLI_MT2_ASSOC_TIMEOUT            4
+#define APCLI_MAX_ASSOC_MSG                5
+
+#define APCLI_ASSOC_FUNC_SIZE              (APCLI_MAX_ASSOC_STATE * APCLI_MAX_ASSOC_MSG)
+
+//ApCli sync state machine
+#define APCLI_SYNC_IDLE                   0  // merge NO_BSS,IBSS_IDLE,IBSS_ACTIVE and BSS in to 1 state
+#define APCLI_JOIN_WAIT_PROBE_RSP         1
+#define APCLI_MAX_SYNC_STATE              2
+
+#define APCLI_SYNC_MACHINE_BASE           0
+#define APCLI_MT2_MLME_PROBE_REQ          0
+#define APCLI_MT2_PEER_PROBE_RSP          1
+#define APCLI_MT2_PROBE_TIMEOUT           2
+#define APCLI_MAX_SYNC_MSG                3
+
+#define APCLI_SYNC_FUNC_SIZE              (APCLI_MAX_SYNC_STATE * APCLI_MAX_SYNC_MSG)
+
+//ApCli ctrl state machine
+#define APCLI_CTRL_DISCONNECTED           0  // merge NO_BSS,IBSS_IDLE,IBSS_ACTIVE and BSS in to 1 state
+#define APCLI_CTRL_PROBE                  1
+#define APCLI_CTRL_AUTH                   2
+#define APCLI_CTRL_AUTH_2                 3
+#define APCLI_CTRL_ASSOC                  4
+#define APCLI_CTRL_DEASSOC                5
+#define APCLI_CTRL_CONNECTED              6
+#define APCLI_MAX_CTRL_STATE              7
+
+#define APCLI_CTRL_MACHINE_BASE           0
+#define APCLI_CTRL_JOIN_REQ               0
+#define APCLI_CTRL_PROBE_RSP              1
+#define APCLI_CTRL_AUTH_RSP               2
+#define APCLI_CTRL_DISCONNECT_REQ         3
+#define APCLI_CTRL_PEER_DISCONNECT_REQ    4
+#define APCLI_CTRL_ASSOC_RSP              5
+#define APCLI_CTRL_DEASSOC_RSP            6
+#define APCLI_CTRL_JOIN_REQ_TIMEOUT       7
+#define APCLI_CTRL_AUTH_REQ_TIMEOUT       8
+#define APCLI_CTRL_ASSOC_REQ_TIMEOUT      9
+#define APCLI_MAX_CTRL_MSG                10
+
+#define APCLI_CTRL_FUNC_SIZE              (APCLI_MAX_CTRL_STATE * APCLI_MAX_CTRL_MSG)
+
+#endif	// APCLI_SUPPORT //
+
+
+// =============================================================================
+
+// value domain of 802.11 header FC.Tyte, which is b3..b2 of the 1st-byte of MAC header
+#define BTYPE_MGMT                  0
+#define BTYPE_CNTL                  1
+#define BTYPE_DATA                  2
+
+// value domain of 802.11 MGMT frame's FC.subtype, which is b7..4 of the 1st-byte of MAC header
+#define SUBTYPE_ASSOC_REQ           0
+#define SUBTYPE_ASSOC_RSP           1
+#define SUBTYPE_REASSOC_REQ         2
+#define SUBTYPE_REASSOC_RSP         3
+#define SUBTYPE_PROBE_REQ           4
+#define SUBTYPE_PROBE_RSP           5
+#define SUBTYPE_BEACON              8
+#define SUBTYPE_ATIM                9
+#define SUBTYPE_DISASSOC            10
+#define SUBTYPE_AUTH                11
+#define SUBTYPE_DEAUTH              12
+#define SUBTYPE_ACTION              13
+#define SUBTYPE_ACTION_NO_ACK              14
+
+// value domain of 802.11 CNTL frame's FC.subtype, which is b7..4 of the 1st-byte of MAC header
+#define SUBTYPE_WRAPPER       	7
+#define SUBTYPE_BLOCK_ACK_REQ       8
+#define SUBTYPE_BLOCK_ACK           9
+#define SUBTYPE_PS_POLL             10
+#define SUBTYPE_RTS                 11
+#define SUBTYPE_CTS                 12
+#define SUBTYPE_ACK                 13
+#define SUBTYPE_CFEND               14
+#define SUBTYPE_CFEND_CFACK         15
+
+// value domain of 802.11 DATA frame's FC.subtype, which is b7..4 of the 1st-byte of MAC header
+#define SUBTYPE_DATA                0
+#define SUBTYPE_DATA_CFACK          1
+#define SUBTYPE_DATA_CFPOLL         2
+#define SUBTYPE_DATA_CFACK_CFPOLL   3
+#define SUBTYPE_NULL_FUNC           4
+#define SUBTYPE_CFACK               5
+#define SUBTYPE_CFPOLL              6
+#define SUBTYPE_CFACK_CFPOLL        7
+#define SUBTYPE_QDATA               8
+#define SUBTYPE_QDATA_CFACK         9
+#define SUBTYPE_QDATA_CFPOLL        10
+#define SUBTYPE_QDATA_CFACK_CFPOLL  11
+#define SUBTYPE_QOS_NULL            12
+#define SUBTYPE_QOS_CFACK           13
+#define SUBTYPE_QOS_CFPOLL          14
+#define SUBTYPE_QOS_CFACK_CFPOLL    15
+
+// ACK policy of QOS Control field bit 6:5
+#define NORMAL_ACK                  0x00  // b6:5 = 00
+#define NO_ACK                      0x20  // b6:5 = 01
+#define NO_EXPLICIT_ACK             0x40  // b6:5 = 10
+#define BLOCK_ACK                   0x60  // b6:5 = 11
+
+//
+// rtmp_data.c use these definition
+//
+#define LENGTH_802_11               24
+#define LENGTH_802_11_AND_H         30
+#define LENGTH_802_11_CRC_H         34
+#define LENGTH_802_11_CRC           28
+#define LENGTH_802_11_WITH_ADDR4    30
+#define LENGTH_802_3                14
+#define LENGTH_802_3_TYPE           2
+#define LENGTH_802_1_H              8
+#define LENGTH_EAPOL_H              4
+#define LENGTH_WMMQOS_H				2
+#define LENGTH_CRC                  4
+#define MAX_SEQ_NUMBER              0x0fff
+#define LENGTH_802_3_NO_TYPE		12
+#define LENGTH_802_1Q				4 /* VLAN related */
+
+// STA_CSR4.field.TxResult
+#define TX_RESULT_SUCCESS           0
+#define TX_RESULT_ZERO_LENGTH       1
+#define TX_RESULT_UNDER_RUN         2
+#define TX_RESULT_OHY_ERROR         4
+#define TX_RESULT_RETRY_FAIL        6
+
+// All PHY rate summary in TXD
+// Preamble MODE in TxD
+#define MODE_CCK	0
+#define MODE_OFDM   1
+#ifdef DOT11_N_SUPPORT
+#define MODE_HTMIX	2
+#define MODE_HTGREENFIELD	3
+#endif // DOT11_N_SUPPORT //
+// MCS for CCK.  BW.SGI.STBC are reserved
+#define MCS_LONGP_RATE_1                      0	 // long preamble CCK 1Mbps
+#define MCS_LONGP_RATE_2                      1	// long preamble CCK 1Mbps
+#define MCS_LONGP_RATE_5_5                    2
+#define MCS_LONGP_RATE_11                     3
+#define MCS_SHORTP_RATE_1                      4	 // long preamble CCK 1Mbps. short is forbidden in 1Mbps
+#define MCS_SHORTP_RATE_2                      5	// short preamble CCK 2Mbps
+#define MCS_SHORTP_RATE_5_5                    6
+#define MCS_SHORTP_RATE_11                     7
+// To send duplicate legacy OFDM. set BW=BW_40.  SGI.STBC are reserved
+#define MCS_RATE_6                      0   // legacy OFDM
+#define MCS_RATE_9                      1   // OFDM
+#define MCS_RATE_12                     2   // OFDM
+#define MCS_RATE_18                     3   // OFDM
+#define MCS_RATE_24                     4  // OFDM
+#define MCS_RATE_36                     5   // OFDM
+#define MCS_RATE_48                     6  // OFDM
+#define MCS_RATE_54                     7 // OFDM
+// HT
+#define MCS_0		0	// 1S
+#define MCS_1		1
+#define MCS_2		2
+#define MCS_3		3
+#define MCS_4		4
+#define MCS_5		5
+#define MCS_6		6
+#define MCS_7		7
+#define MCS_8		8	// 2S
+#define MCS_9		9
+#define MCS_10		10
+#define MCS_11		11
+#define MCS_12		12
+#define MCS_13		13
+#define MCS_14		14
+#define MCS_15		15
+#define MCS_16		16	// 3*3
+#define MCS_17		17
+#define MCS_18		18
+#define MCS_19		19
+#define MCS_20		20
+#define MCS_21		21
+#define MCS_22		22
+#define MCS_23		23
+#define MCS_32		32
+#define MCS_AUTO		33
+
+#ifdef DOT11_N_SUPPORT
+// OID_HTPHYMODE
+// MODE
+#define HTMODE_MM	0
+#define HTMODE_GF	1
+#endif // DOT11_N_SUPPORT //
+
+// Fixed Tx MODE - HT, CCK or OFDM
+#define FIXED_TXMODE_HT		0
+#define FIXED_TXMODE_CCK	1
+#define FIXED_TXMODE_OFDM 	2
+// BW
+#define BW_20		BAND_WIDTH_20
+#define BW_40		BAND_WIDTH_40
+#define BW_BOTH		BAND_WIDTH_BOTH
+#define BW_10		BAND_WIDTH_10	// 802.11j has 10MHz. This definition is for internal usage. doesn't fill in the IE or other field.
+
+#ifdef DOT11_N_SUPPORT
+// SHORTGI
+#define GI_400		GAP_INTERVAL_400	// only support in HT mode
+#define GI_BOTH		GAP_INTERVAL_BOTH
+#endif // DOT11_N_SUPPORT //
+#define GI_800		GAP_INTERVAL_800
+// STBC
+#define STBC_NONE	0
+#ifdef DOT11_N_SUPPORT
+#define STBC_USE	1	// limited use in rt2860b phy
+#define RXSTBC_ONE	1	// rx support of one spatial stream
+#define RXSTBC_TWO	2	// rx support of 1 and 2 spatial stream
+#define RXSTBC_THR	3	// rx support of 1~3 spatial stream
+// MCS FEEDBACK
+#define MCSFBK_NONE	0  // not support mcs feedback /
+#define MCSFBK_RSV	1	// reserved
+#define MCSFBK_UNSOLICIT	2	// only support unsolict mcs feedback
+#define MCSFBK_MRQ	3	// response to both MRQ and unsolict mcs feedback
+
+// MIMO power safe
+#define	MMPS_STATIC	0
+#define	MMPS_DYNAMIC		1
+#define   MMPS_RSV		2
+#define MMPS_ENABLE		3
+
+
+// A-MSDU size
+#define	AMSDU_0	0
+#define	AMSDU_1		1
+
+#endif // DOT11_N_SUPPORT //
+
+// MCS use 7 bits
+#define TXRATEMIMO		0x80
+#define TXRATEMCS		0x7F
+#define TXRATEOFDM		0x7F
+#define RATE_1                      0
+#define RATE_2                      1
+#define RATE_5_5                    2
+#define RATE_11                     3
+#define RATE_6                      4   // OFDM
+#define RATE_9                      5   // OFDM
+#define RATE_12                     6   // OFDM
+#define RATE_18                     7   // OFDM
+#define RATE_24                     8   // OFDM
+#define RATE_36                     9   // OFDM
+#define RATE_48                     10  // OFDM
+#define RATE_54                     11  // OFDM
+#define RATE_FIRST_OFDM_RATE        RATE_6
+#define RATE_LAST_OFDM_RATE        	RATE_54
+#define RATE_6_5                    12  // HT mix
+#define RATE_13                     13  // HT mix
+#define RATE_19_5                   14  // HT mix
+#define RATE_26                     15  // HT mix
+#define RATE_39                     16  // HT mix
+#define RATE_52                     17  // HT mix
+#define RATE_58_5                   18  // HT mix
+#define RATE_65                     19  // HT mix
+#define RATE_78                     20  // HT mix
+#define RATE_104                    21  // HT mix
+#define RATE_117                    22  // HT mix
+#define RATE_130                    23  // HT mix
+//#define RATE_AUTO_SWITCH            255 // for StaCfg.FixedTxRate only
+#define HTRATE_0                      12
+#define RATE_FIRST_MM_RATE        HTRATE_0
+#define RATE_FIRST_HT_RATE        HTRATE_0
+#define RATE_LAST_HT_RATE        HTRATE_0
+
+// pTxWI->txop
+#define IFS_HTTXOP                 0	// The txop will be handles by ASIC.
+#define IFS_PIFS                    1
+#define IFS_SIFS                    2
+#define IFS_BACKOFF                 3
+
+// pTxD->RetryMode
+#define LONG_RETRY                  1
+#define SHORT_RETRY                 0
+
+// Country Region definition
+#define REGION_MINIMUM_BG_BAND            0
+#define REGION_0_BG_BAND                  0       // 1-11
+#define REGION_1_BG_BAND                  1       // 1-13
+#define REGION_2_BG_BAND                  2       // 10-11
+#define REGION_3_BG_BAND                  3       // 10-13
+#define REGION_4_BG_BAND                  4       // 14
+#define REGION_5_BG_BAND                  5       // 1-14
+#define REGION_6_BG_BAND                  6       // 3-9
+#define REGION_7_BG_BAND                  7       // 5-13
+#define REGION_31_BG_BAND                 31       // 5-13
+#define REGION_MAXIMUM_BG_BAND            7
+
+#define REGION_MINIMUM_A_BAND             0
+#define REGION_0_A_BAND                   0       // 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165
+#define REGION_1_A_BAND                   1       // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140
+#define REGION_2_A_BAND                   2       // 36, 40, 44, 48, 52, 56, 60, 64
+#define REGION_3_A_BAND                   3       // 52, 56, 60, 64, 149, 153, 157, 161
+#define REGION_4_A_BAND                   4       // 149, 153, 157, 161, 165
+#define REGION_5_A_BAND                   5       // 149, 153, 157, 161
+#define REGION_6_A_BAND                   6       // 36, 40, 44, 48
+#define REGION_7_A_BAND                   7       // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165
+#define REGION_8_A_BAND                   8       // 52, 56, 60, 64
+#define REGION_9_A_BAND                   9       // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165
+#define REGION_10_A_BAND                  10	  // 36, 40, 44, 48, 149, 153, 157, 161, 165
+#define REGION_11_A_BAND                  11	  // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 149, 153, 157, 161
+#define REGION_MAXIMUM_A_BAND             11
+
+// pTxD->CipherAlg
+#define CIPHER_NONE                 0
+#define CIPHER_WEP64                1
+#define CIPHER_WEP128               2
+#define CIPHER_TKIP                 3
+#define CIPHER_AES                  4
+#define CIPHER_CKIP64               5
+#define CIPHER_CKIP128              6
+#define CIPHER_TKIP_NO_MIC          7       // MIC appended by driver: not a valid value in hardware key table
+#define CIPHER_SMS4					8
+
+// value domain of pAd->RfIcType
+#define RFIC_2820                   1       // 2.4G 2T3R
+#define RFIC_2850                   2       // 2.4G/5G 2T3R
+#define RFIC_2720                   3       // 2.4G 1T2R
+#define RFIC_2750                   4       // 2.4G/5G 1T2R
+#define RFIC_3020                   5       // 2.4G 1T1R
+#define RFIC_2020                   6       // 2.4G B/G
+
+// LED Status.
+#define LED_LINK_DOWN               0
+#define LED_LINK_UP                 1
+#define LED_RADIO_OFF               2
+#define LED_RADIO_ON                3
+#define LED_HALT                    4
+#define LED_WPS                     5
+#define LED_ON_SITE_SURVEY          6
+#define LED_POWER_UP                7
+
+// value domain of pAd->LedCntl.LedMode and E2PROM
+#define LED_MODE_DEFAULT            0
+#define LED_MODE_TWO_LED			1
+#define LED_MODE_SIGNAL_STREGTH		8  // EEPROM define =8
+
+// RC4 init value, used fro WEP & TKIP
+#define PPPINITFCS32                0xffffffff   /* Initial FCS value */
+
+// value domain of pAd->StaCfg.PortSecured. 802.1X controlled port definition
+#define WPA_802_1X_PORT_SECURED     1
+#define WPA_802_1X_PORT_NOT_SECURED 2
+
+#define PAIRWISE_KEY                1
+#define GROUP_KEY                   2
+
+//definition of DRS
+#define MAX_STEP_OF_TX_RATE_SWITCH	32
+
+
+// pre-allocated free NDIS PACKET/BUFFER poll for internal usage
+#define MAX_NUM_OF_FREE_NDIS_PACKET 128
+
+//Block ACK
+#define MAX_TX_REORDERBUF   64
+#define MAX_RX_REORDERBUF   64
+#define DEFAULT_TX_TIMEOUT   30
+#define DEFAULT_RX_TIMEOUT   30
+
+// definition of Recipient or Originator
+#define I_RECIPIENT                  TRUE
+#define I_ORIGINATOR                   FALSE
+
+#define DEFAULT_BBP_TX_POWER        0
+#define DEFAULT_RF_TX_POWER         5
+
+#define MAX_INI_BUFFER_SIZE			4096
+#define MAX_PARAM_BUFFER_SIZE		(2048) // enough for ACL (18*64)
+											//18 : the length of Mac address acceptable format "01:02:03:04:05:06;")
+											//64 : MAX_NUM_OF_ACL_LIST
+// definition of pAd->OpMode
+#define OPMODE_STA                  0
+#define OPMODE_AP                   1
+//#define OPMODE_L3_BRG               2       // as AP and STA at the same time
+
+#ifdef RT_BIG_ENDIAN
+#define DIR_READ                    0
+#define DIR_WRITE                   1
+#define TYPE_TXD                    0
+#define TYPE_RXD                    1
+#define TYPE_TXINFO					0
+#define TYPE_RXINFO					1
+#define TYPE_TXWI					0
+#define TYPE_RXWI					1
+#endif
+
+// ========================= AP rtmp_def.h ===========================
+// value domain for pAd->EventTab.Log[].Event
+#define EVENT_RESET_ACCESS_POINT    0 // Log = "hh:mm:ss   Restart Access Point"
+#define EVENT_ASSOCIATED            1 // Log = "hh:mm:ss   STA 00:01:02:03:04:05 associated"
+#define EVENT_DISASSOCIATED         2 // Log = "hh:mm:ss   STA 00:01:02:03:04:05 left this BSS"
+#define EVENT_AGED_OUT              3 // Log = "hh:mm:ss   STA 00:01:02:03:04:05 was aged-out and removed from this BSS"
+#define EVENT_COUNTER_M             4
+#define EVENT_INVALID_PSK           5
+#define EVENT_MAX_EVENT_TYPE        6
+// ==== end of AP rtmp_def.h ============
+
+// definition RSSI Number
+#define RSSI_0					0
+#define RSSI_1					1
+#define RSSI_2					2
+
+// definition of radar detection
+#define RD_NORMAL_MODE				0	// Not found radar signal
+#define RD_SWITCHING_MODE			1	// Found radar signal, and doing channel switch
+#define RD_SILENCE_MODE				2	// After channel switch, need to be silence a while to ensure radar not found
+
+//Driver defined cid for mapping status and command.
+#define  SLEEPCID	0x11
+#define  WAKECID	0x22
+#define  QUERYPOWERCID	0x33
+#define  OWNERMCU	0x1
+#define  OWNERCPU	0x0
+
+// MBSSID definition
+#define ENTRY_NOT_FOUND             0xFF
+
+
+/* After Linux 2.6.9,
+ * VLAN module use Private (from user) interface flags (netdevice->priv_flags).
+ * #define IFF_802_1Q_VLAN 0x1         --    802.1Q VLAN device.  in if.h
+ * ref to ip_sabotage_out() [ out->priv_flags & IFF_802_1Q_VLAN ] in br_netfilter.c
+ *
+ * For this reason, we MUST use EVEN value in priv_flags
+ */
+#define INT_MAIN                    0x0100
+#define INT_MBSSID                  0x0200
+#define INT_WDS                     0x0300
+#define INT_APCLI                   0x0400
+#define INT_MESH                   	0x0500
+
+// Use bitmap to allow coexist of ATE_TXFRAME and ATE_RXFRAME(i.e.,to support LoopBack mode)
+#ifdef RALINK_ATE
+#define	ATE_START                   0x00   // Start ATE
+#define	ATE_STOP                    0x80   // Stop ATE
+#define	ATE_TXCONT                  0x05   // Continuous Transmit
+#define	ATE_TXCARR                  0x09   // Transmit Carrier
+#define	ATE_TXCARRSUPP              0x11   // Transmit Carrier Suppression
+#define	ATE_TXFRAME                 0x01   // Transmit Frames
+#define	ATE_RXFRAME                 0x02   // Receive Frames
+#ifdef RALINK_28xx_QA
+#define ATE_TXSTOP                  0xe2   // Stop Transmition(i.e., TXCONT, TXCARR, TXCARRSUPP, and TXFRAME)
+#define ATE_RXSTOP					0xfd   // Stop receiving Frames
+#define	BBP22_TXFRAME     			0x00   // Transmit Frames
+#define	BBP22_TXCONT_OR_CARRSUPP    0x80   // Continuous Transmit or Carrier Suppression
+#define	BBP22_TXCARR                0xc1   // Transmit Carrier
+#define	BBP24_TXCONT                0x00   // Continuous Transmit
+#define	BBP24_CARRSUPP              0x01   // Carrier Suppression
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+// WEP Key TYPE
+#define WEP_HEXADECIMAL_TYPE    0
+#define WEP_ASCII_TYPE          1
+
+
+
+// WIRELESS EVENTS definition
+/* Max number of char in custom event, refer to wireless_tools.28/wireless.20.h */
+#define IW_CUSTOM_MAX_LEN				  			255	/* In bytes */
+
+// For system event - start
+#define	IW_SYS_EVENT_FLAG_START                     0x0200
+#define	IW_ASSOC_EVENT_FLAG                         0x0200
+#define	IW_DISASSOC_EVENT_FLAG                      0x0201
+#define	IW_DEAUTH_EVENT_FLAG                      	0x0202
+#define	IW_AGEOUT_EVENT_FLAG                      	0x0203
+#define	IW_COUNTER_MEASURES_EVENT_FLAG              0x0204
+#define	IW_REPLAY_COUNTER_DIFF_EVENT_FLAG           0x0205
+#define	IW_RSNIE_DIFF_EVENT_FLAG           			0x0206
+#define	IW_MIC_DIFF_EVENT_FLAG           			0x0207
+#define IW_ICV_ERROR_EVENT_FLAG						0x0208
+#define IW_MIC_ERROR_EVENT_FLAG						0x0209
+#define IW_GROUP_HS_TIMEOUT_EVENT_FLAG				0x020A
+#define	IW_PAIRWISE_HS_TIMEOUT_EVENT_FLAG			0x020B
+#define IW_RSNIE_SANITY_FAIL_EVENT_FLAG				0x020C
+#define IW_SET_KEY_DONE_WPA1_EVENT_FLAG				0x020D
+#define IW_SET_KEY_DONE_WPA2_EVENT_FLAG				0x020E
+#define IW_STA_LINKUP_EVENT_FLAG					0x020F
+#define IW_STA_LINKDOWN_EVENT_FLAG					0x0210
+#define IW_SCAN_COMPLETED_EVENT_FLAG				0x0211
+#define IW_SCAN_ENQUEUE_FAIL_EVENT_FLAG				0x0212
+// if add new system event flag, please upadte the IW_SYS_EVENT_FLAG_END
+#define	IW_SYS_EVENT_FLAG_END                       0x0212
+#define	IW_SYS_EVENT_TYPE_NUM						(IW_SYS_EVENT_FLAG_END - IW_SYS_EVENT_FLAG_START + 1)
+// For system event - end
+
+// For spoof attack event - start
+#define	IW_SPOOF_EVENT_FLAG_START                   0x0300
+#define IW_CONFLICT_SSID_EVENT_FLAG					0x0300
+#define IW_SPOOF_ASSOC_RESP_EVENT_FLAG				0x0301
+#define IW_SPOOF_REASSOC_RESP_EVENT_FLAG			0x0302
+#define IW_SPOOF_PROBE_RESP_EVENT_FLAG				0x0303
+#define IW_SPOOF_BEACON_EVENT_FLAG					0x0304
+#define IW_SPOOF_DISASSOC_EVENT_FLAG				0x0305
+#define IW_SPOOF_AUTH_EVENT_FLAG					0x0306
+#define IW_SPOOF_DEAUTH_EVENT_FLAG					0x0307
+#define IW_SPOOF_UNKNOWN_MGMT_EVENT_FLAG			0x0308
+#define IW_REPLAY_ATTACK_EVENT_FLAG					0x0309
+// if add new spoof attack event flag, please upadte the IW_SPOOF_EVENT_FLAG_END
+#define	IW_SPOOF_EVENT_FLAG_END                     0x0309
+#define	IW_SPOOF_EVENT_TYPE_NUM						(IW_SPOOF_EVENT_FLAG_END - IW_SPOOF_EVENT_FLAG_START + 1)
+// For spoof attack event - end
+
+// For flooding attack event - start
+#define	IW_FLOOD_EVENT_FLAG_START                   0x0400
+#define IW_FLOOD_AUTH_EVENT_FLAG					0x0400
+#define IW_FLOOD_ASSOC_REQ_EVENT_FLAG				0x0401
+#define IW_FLOOD_REASSOC_REQ_EVENT_FLAG				0x0402
+#define IW_FLOOD_PROBE_REQ_EVENT_FLAG				0x0403
+#define IW_FLOOD_DISASSOC_EVENT_FLAG				0x0404
+#define IW_FLOOD_DEAUTH_EVENT_FLAG					0x0405
+#define IW_FLOOD_EAP_REQ_EVENT_FLAG					0x0406
+// if add new flooding attack event flag, please upadte the IW_FLOOD_EVENT_FLAG_END
+#define	IW_FLOOD_EVENT_FLAG_END                   	0x0406
+#define	IW_FLOOD_EVENT_TYPE_NUM						(IW_FLOOD_EVENT_FLAG_END - IW_FLOOD_EVENT_FLAG_START + 1)
+// For flooding attack - end
+
+// End - WIRELESS EVENTS definition
+
+#ifdef CONFIG_STA_SUPPORT
+// definition for DLS, kathy
+#define	MAX_NUM_OF_INIT_DLS_ENTRY   1
+#define	MAX_NUM_OF_DLS_ENTRY        MAX_NUMBER_OF_DLS_ENTRY
+
+//Block ACK , rt2860, kathy
+#define MAX_TX_REORDERBUF		64
+#define MAX_RX_REORDERBUF		64
+#define DEFAULT_TX_TIMEOUT		30
+#define DEFAULT_RX_TIMEOUT		30
+#ifndef CONFIG_AP_SUPPORT
+#define MAX_BARECI_SESSION		8
+#endif
+
+#ifndef IW_ESSID_MAX_SIZE
+/* Maximum size of the ESSID and pAd->nickname strings */
+#define IW_ESSID_MAX_SIZE   		32
+#endif
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef MCAST_RATE_SPECIFIC
+#define MCAST_DISABLE	0
+#define MCAST_CCK		1
+#define MCAST_OFDM		2
+#define MCAST_HTMIX		3
+#endif // MCAST_RATE_SPECIFIC //
+
+// For AsicRadioOff/AsicRadioOn function
+#define DOT11POWERSAVE		0
+#define GUIRADIO_OFF		1
+#define RTMP_HALT		    2
+#define GUI_IDLE_POWER_SAVE		3
+// --
+
+
+// definition for WpaSupport flag
+#define WPA_SUPPLICANT_DISABLE				0
+#define WPA_SUPPLICANT_ENABLE				1
+#define	WPA_SUPPLICANT_ENABLE_WITH_WEB_UI	2
+
+// Endian byte swapping codes
+#define SWAP16(x) \
+    ((UINT16)( \
+    (((UINT16)(x) & (UINT16) 0x00ffU) << 8) | \
+    (((UINT16)(x) & (UINT16) 0xff00U) >> 8) ))
+
+#define SWAP32(x) \
+    ((UINT32)( \
+    (((UINT32)(x) & (UINT32) 0x000000ffUL) << 24) | \
+    (((UINT32)(x) & (UINT32) 0x0000ff00UL) <<  8) | \
+    (((UINT32)(x) & (UINT32) 0x00ff0000UL) >>  8) | \
+    (((UINT32)(x) & (UINT32) 0xff000000UL) >> 24) ))
+
+#define SWAP64(x) \
+    ((UINT64)( \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x00000000000000ffULL) << 56) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x000000000000ff00ULL) << 40) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x0000000000ff0000ULL) << 24) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x00000000ff000000ULL) <<  8) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x000000ff00000000ULL) >>  8) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x0000ff0000000000ULL) >> 24) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x00ff000000000000ULL) >> 40) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0xff00000000000000ULL) >> 56) ))
+
+#ifdef RT_BIG_ENDIAN
+
+#define cpu2le64(x) SWAP64((x))
+#define le2cpu64(x) SWAP64((x))
+#define cpu2le32(x) SWAP32((x))
+#define le2cpu32(x) SWAP32((x))
+#define cpu2le16(x) SWAP16((x))
+#define le2cpu16(x) SWAP16((x))
+#define cpu2be64(x) ((UINT64)(x))
+#define be2cpu64(x) ((UINT64)(x))
+#define cpu2be32(x) ((UINT32)(x))
+#define be2cpu32(x) ((UINT32)(x))
+#define cpu2be16(x) ((UINT16)(x))
+#define be2cpu16(x) ((UINT16)(x))
+
+#else   // Little_Endian
+
+#define cpu2le64(x) ((UINT64)(x))
+#define le2cpu64(x) ((UINT64)(x))
+#define cpu2le32(x) ((UINT32)(x))
+#define le2cpu32(x) ((UINT32)(x))
+#define cpu2le16(x) ((UINT16)(x))
+#define le2cpu16(x) ((UINT16)(x))
+#define cpu2be64(x) SWAP64((x))
+#define be2cpu64(x) SWAP64((x))
+#define cpu2be32(x) SWAP32((x))
+#define be2cpu32(x) SWAP32((x))
+#define cpu2be16(x) SWAP16((x))
+#define be2cpu16(x) SWAP16((x))
+
+#endif  // RT_BIG_ENDIAN
+
+#endif  // __RTMP_DEF_H__
+
+
diff --git a/drivers/staging/rt2860/rtmp_type.h b/drivers/staging/rt2860/rtmp_type.h
new file mode 100644
index 000000000000..1fd7df1e1791
--- /dev/null
+++ b/drivers/staging/rt2860/rtmp_type.h
@@ -0,0 +1,94 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rtmp_type.h
+
+    Abstract:
+
+    Revision History:
+    Who         When            What
+    --------    ----------      ----------------------------------------------
+    Name        Date            Modification logs
+    Paul Lin    1-2-2004
+*/
+#ifndef __RTMP_TYPE_H__
+#define __RTMP_TYPE_H__
+
+#define PACKED  __attribute__ ((packed))
+
+// Put platform dependent declaration here
+// For example, linux type definition
+typedef unsigned char		UINT8;
+typedef unsigned short		UINT16;
+typedef unsigned int		UINT32;
+typedef unsigned long long	UINT64;
+typedef int					INT32;
+typedef long long 			INT64;
+
+typedef unsigned char *			PUINT8;
+typedef unsigned short *		PUINT16;
+typedef unsigned int *			PUINT32;
+typedef unsigned long long *	PUINT64;
+typedef int	*					PINT32;
+typedef long long * 			PINT64;
+
+typedef signed char			CHAR;
+typedef signed short		SHORT;
+typedef signed int			INT;
+typedef signed long			LONG;
+typedef signed long long	LONGLONG;
+
+
+typedef unsigned char		UCHAR;
+typedef unsigned short		USHORT;
+typedef unsigned int		UINT;
+typedef unsigned long		ULONG;
+typedef unsigned long long	ULONGLONG;
+
+typedef unsigned char		BOOLEAN;
+typedef void				VOID;
+
+typedef VOID *				PVOID;
+typedef CHAR *				PCHAR;
+typedef UCHAR * 			PUCHAR;
+typedef USHORT *			PUSHORT;
+typedef LONG *				PLONG;
+typedef ULONG *				PULONG;
+typedef UINT *				PUINT;
+
+typedef unsigned int	NDIS_MEDIA_STATE;
+
+typedef union _LARGE_INTEGER {
+    struct {
+        UINT LowPart;
+        INT32 HighPart;
+    } u;
+    INT64 QuadPart;
+} LARGE_INTEGER;
+
+#endif  // __RTMP_TYPE_H__
+
diff --git a/drivers/staging/rt2860/spectrum.h b/drivers/staging/rt2860/spectrum.h
new file mode 100644
index 000000000000..60f25dbdba9f
--- /dev/null
+++ b/drivers/staging/rt2860/spectrum.h
@@ -0,0 +1,322 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+
+#ifndef __SPECTRUM_H__
+#define __SPECTRUM_H__
+
+#include "rtmp_type.h"
+#include "spectrum_def.h"
+
+typedef struct PACKED _TPC_REPORT_INFO
+{
+	UINT8 TxPwr;
+	UINT8 LinkMargin;
+} TPC_REPORT_INFO, *PTPC_REPORT_INFO;
+
+typedef struct PACKED _CH_SW_ANN_INFO
+{
+	UINT8 ChSwMode;
+	UINT8 Channel;
+	UINT8 ChSwCnt;
+} CH_SW_ANN_INFO, *PCH_SW_ANN_INFO;
+
+typedef union PACKED _MEASURE_REQ_MODE
+{
+#ifdef RT_BIG_ENDIAN
+	struct PACKED
+	{
+		UINT8 Rev1:4;
+		UINT8 Report:1;
+		UINT8 Request:1;
+		UINT8 Enable:1;
+		UINT8 Rev0:1;
+	} field;
+#else
+	struct PACKED
+	{
+		UINT8 Rev0:1;
+		UINT8 Enable:1;
+		UINT8 Request:1;
+		UINT8 Report:1;
+		UINT8 Rev1:4;
+	} field;
+#endif // RT_BIG_ENDIAN //
+	UINT8 word;
+} MEASURE_REQ_MODE, *PMEASURE_REQ_MODE;
+
+typedef struct PACKED _MEASURE_REQ
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+} MEASURE_REQ, *PMEASURE_REQ;
+
+typedef struct PACKED _MEASURE_REQ_INFO
+{
+	UINT8 Token;
+	MEASURE_REQ_MODE ReqMode;
+	UINT8 ReqType;
+	MEASURE_REQ MeasureReq;
+} MEASURE_REQ_INFO, *PMEASURE_REQ_INFO;
+
+typedef union PACKED _MEASURE_BASIC_REPORT_MAP
+{
+#ifdef RT_BIG_ENDIAN
+	struct PACKED
+	{
+		UINT8 Rev:3;
+		UINT8 Unmeasure:1;
+		UINT8 Radar:1;
+		UINT8 UnidentifiedSignal:1;
+		UINT8 OfdmPreamble:1;
+		UINT8 BSS:1;
+	} field;
+#else
+	struct PACKED
+	{
+		UINT8 BSS:1;
+		UINT8 OfdmPreamble:1;
+		UINT8 UnidentifiedSignal:1;
+		UINT8 Radar:1;
+		UINT8 Unmeasure:1;
+		UINT8 Rev:3;
+	} field;
+#endif // RT_BIG_ENDIAN //
+	UINT8 word;
+} MEASURE_BASIC_REPORT_MAP, *PMEASURE_BASIC_REPORT_MAP;
+
+typedef struct PACKED _MEASURE_BASIC_REPORT
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+	MEASURE_BASIC_REPORT_MAP Map;
+} MEASURE_BASIC_REPORT, *PMEASURE_BASIC_REPORT;
+
+typedef struct PACKED _MEASURE_CCA_REPORT
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+	UINT8 CCA_Busy_Fraction;
+} MEASURE_CCA_REPORT, *PMEASURE_CCA_REPORT;
+
+typedef struct PACKED _MEASURE_RPI_REPORT
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+	UINT8 RPI_Density[8];
+} MEASURE_RPI_REPORT, *PMEASURE_RPI_REPORT;
+
+typedef union PACKED _MEASURE_REPORT_MODE
+{
+	struct PACKED
+	{
+#ifdef RT_BIG_ENDIAN
+		UINT8 Rev:5;
+		UINT8 Refused:1;
+		UINT8 Incapable:1;
+		UINT8 Late:1;
+#else
+		UINT8 Late:1;
+		UINT8 Incapable:1;
+		UINT8 Refused:1;
+		UINT8 Rev:5;
+#endif // RT_BIG_ENDIAN //
+	} field;
+	UINT8 word;
+} MEASURE_REPORT_MODE, *PMEASURE_REPORT_MODE;
+
+typedef struct PACKED _MEASURE_REPORT_INFO
+{
+	UINT8 Token;
+	MEASURE_REPORT_MODE ReportMode;
+	UINT8 ReportType;
+	UINT8 Octect[0];
+} MEASURE_REPORT_INFO, *PMEASURE_REPORT_INFO;
+
+typedef struct PACKED _QUIET_INFO
+{
+	UINT8 QuietCnt;
+	UINT8 QuietPeriod;
+	UINT8 QuietDuration;
+	UINT8 QuietOffset;
+} QUIET_INFO, *PQUIET_INFO;
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 MeasureCh,
+	IN UINT16 MeasureDuration);
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 ReportInfoLen,
+	IN PUINT8 pReportInfo);
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UCHAR DialogToken);
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 TxPwr,
+	IN UINT8 LinkMargin);
+
+/*
+	==========================================================================
+	Description:
+		Prepare Channel Switch Announcement action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+		2. Channel switch announcement mode.
+		2. a New selected channel.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueChSwAnn(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 ChSwMode,
+	IN UINT8 NewCh);
+
+/*
+	==========================================================================
+	Description:
+		Spectrun action frames Handler such as channel switch annoucement,
+		measurement report, measurement request actions frames.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+VOID PeerSpectrumAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+/*
+	==========================================================================
+	Description:
+
+	Parametrs:
+
+	Return	: None.
+	==========================================================================
+ */
+INT Set_MeasureReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_TpcReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+VOID MeasureReqTabInit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MeasureReqTabExit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID TpcReqTabInit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID TpcReqTabExit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID NotifyChSwAnnToPeerAPs(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pRA,
+	IN PUCHAR pTA,
+	IN UINT8 ChSwMode,
+	IN UINT8 Channel);
+#endif // __SPECTRUM_H__ //
+
diff --git a/drivers/staging/rt2860/spectrum_def.h b/drivers/staging/rt2860/spectrum_def.h
new file mode 100644
index 000000000000..4ca4817bba05
--- /dev/null
+++ b/drivers/staging/rt2860/spectrum_def.h
@@ -0,0 +1,95 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+	spectrum_def.h
+
+    Abstract:
+    Handle association related requests either from WSTA or from local MLME
+
+    Revision History:
+    Who          When          What
+    ---------    ----------    ----------------------------------------------
+	Fonchi Wu    2008	  	   created for 802.11h
+ */
+
+#ifndef __SPECTRUM_DEF_H__
+#define __SPECTRUM_DEF_H__
+
+#define MAX_MEASURE_REQ_TAB_SIZE		3
+#define MAX_HASH_MEASURE_REQ_TAB_SIZE	MAX_MEASURE_REQ_TAB_SIZE
+
+#define MAX_TPC_REQ_TAB_SIZE			3
+#define MAX_HASH_TPC_REQ_TAB_SIZE		MAX_TPC_REQ_TAB_SIZE
+
+#define MIN_RCV_PWR				100		/* Negative value ((dBm) */
+
+#define RM_TPC_REQ				0
+#define RM_MEASURE_REQ			1
+
+#define RM_BASIC				0
+#define RM_CCA					1
+#define RM_RPI_HISTOGRAM		2
+
+#define TPC_REQ_AGE_OUT			500		/* ms */
+#define MQ_REQ_AGE_OUT			500		/* ms */
+
+#define TPC_DIALOGTOKEN_HASH_INDEX(_DialogToken)	((_DialogToken) % MAX_HASH_TPC_REQ_TAB_SIZE)
+#define MQ_DIALOGTOKEN_HASH_INDEX(_DialogToken)		((_DialogToken) % MAX_MEASURE_REQ_TAB_SIZE)
+
+typedef struct _MEASURE_REQ_ENTRY
+{
+	struct _MEASURE_REQ_ENTRY *pNext;
+	ULONG lastTime;
+	BOOLEAN	Valid;
+	UINT8 DialogToken;
+	UINT8 MeasureDialogToken[3];	// 0:basic measure, 1: CCA measure, 2: RPI_Histogram measure.
+} MEASURE_REQ_ENTRY, *PMEASURE_REQ_ENTRY;
+
+typedef struct _MEASURE_REQ_TAB
+{
+	UCHAR Size;
+	PMEASURE_REQ_ENTRY Hash[MAX_HASH_MEASURE_REQ_TAB_SIZE];
+	MEASURE_REQ_ENTRY Content[MAX_MEASURE_REQ_TAB_SIZE];
+} MEASURE_REQ_TAB, *PMEASURE_REQ_TAB;
+
+typedef struct _TPC_REQ_ENTRY
+{
+	struct _TPC_REQ_ENTRY *pNext;
+	ULONG lastTime;
+	BOOLEAN Valid;
+	UINT8 DialogToken;
+} TPC_REQ_ENTRY, *PTPC_REQ_ENTRY;
+
+typedef struct _TPC_REQ_TAB
+{
+	UCHAR Size;
+	PTPC_REQ_ENTRY Hash[MAX_HASH_TPC_REQ_TAB_SIZE];
+	TPC_REQ_ENTRY Content[MAX_TPC_REQ_TAB_SIZE];
+} TPC_REQ_TAB, *PTPC_REQ_TAB;
+
+#endif // __SPECTRUM_DEF_H__ //
+
diff --git a/drivers/staging/rt2860/sta/aironet.c b/drivers/staging/rt2860/sta/aironet.c
new file mode 100644
index 000000000000..4af4a1906181
--- /dev/null
+++ b/drivers/staging/rt2860/sta/aironet.c
@@ -0,0 +1,1312 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	aironet.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Paul Lin	04-06-15		Initial
+*/
+#include "../rt_config.h"
+
+/*
+	==========================================================================
+	Description:
+		association	state machine init,	including state	transition and timer init
+	Parameters:
+		S -	pointer	to the association state machine
+	==========================================================================
+ */
+VOID	AironetStateMachineInit(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	STATE_MACHINE		*S,
+	OUT	STATE_MACHINE_FUNC	Trans[])
+{
+	StateMachineInit(S,	Trans, MAX_AIRONET_STATE, MAX_AIRONET_MSG, (STATE_MACHINE_FUNC)Drop, AIRONET_IDLE, AIRONET_MACHINE_BASE);
+	StateMachineSetAction(S, AIRONET_IDLE, MT2_AIRONET_MSG, (STATE_MACHINE_FUNC)AironetMsgAction);
+	StateMachineSetAction(S, AIRONET_IDLE, MT2_AIRONET_SCAN_REQ, (STATE_MACHINE_FUNC)AironetRequestAction);
+	StateMachineSetAction(S, AIRONET_SCANNING, MT2_AIRONET_SCAN_DONE, (STATE_MACHINE_FUNC)AironetReportAction);
+}
+
+/*
+	==========================================================================
+	Description:
+		This is	state machine function.
+		When receiving EAPOL packets which is  for 802.1x key management.
+		Use	both in	WPA, and WPAPSK	case.
+		In this	function, further dispatch to different	functions according	to the received	packet.	 3 categories are :
+		  1.  normal 4-way pairwisekey and 2-way groupkey handshake
+		  2.  MIC error	(Countermeasures attack)  report packet	from STA.
+		  3.  Request for pairwise/group key update	from STA
+	Return:
+	==========================================================================
+*/
+VOID	AironetMsgAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+{
+	USHORT							Length;
+	UCHAR							Index, i;
+	PUCHAR							pData;
+	PAIRONET_RM_REQUEST_FRAME		pRMReq;
+	PRM_REQUEST_ACTION				pReqElem;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-----> AironetMsgAction\n"));
+
+	// 0. Get Aironet IAPP header first
+	pRMReq = (PAIRONET_RM_REQUEST_FRAME) &Elem->Msg[LENGTH_802_11];
+	pData  = (PUCHAR) &Elem->Msg[LENGTH_802_11];
+
+	// 1. Change endian format form network to little endian
+	Length = be2cpu16(pRMReq->IAPP.Length);
+
+	// 2.0 Sanity check, this should only happen when CCX 2.0 support is enabled
+	if (pAd->StaCfg.CCXEnable != TRUE)
+		return;
+
+	// 2.1 Radio measurement must be on
+	if (pAd->StaCfg.CCXControl.field.RMEnable != 1)
+		return;
+
+	// 2.2. Debug print all bit information
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP ID & Length %d\n", Length));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Type %x\n", pRMReq->IAPP.Type));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP SubType %x\n", pRMReq->IAPP.SubType));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Dialog Token %x\n", pRMReq->IAPP.Token));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Activation Delay %x\n", pRMReq->Delay));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Measurement Offset %x\n", pRMReq->Offset));
+
+	// 3. Check IAPP frame type, it must be 0x32 for Cisco Aironet extension
+	if (pRMReq->IAPP.Type != AIRONET_IAPP_TYPE)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP type for Cisco Aironet extension\n"));
+		return;
+	}
+
+	// 4. Check IAPP frame subtype, it must be 0x01 for Cisco Aironet extension request.
+	//    Since we are acting as client only, we will disregards reply subtype.
+	if (pRMReq->IAPP.SubType != AIRONET_IAPP_SUBTYPE_REQUEST)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP subtype for Cisco Aironet extension\n"));
+		return;
+	}
+
+	// 5. Verify Destination MAC and Source MAC, both should be all zeros.
+	if (! MAC_ADDR_EQUAL(pRMReq->IAPP.DA, ZERO_MAC_ADDR))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP DA for Cisco Aironet extension, it's not Zero\n"));
+		return;
+	}
+
+	if (! MAC_ADDR_EQUAL(pRMReq->IAPP.SA, ZERO_MAC_ADDR))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP SA for Cisco Aironet extension, it's not Zero\n"));
+		return;
+	}
+
+	// 6. Reinit all report related fields
+	NdisZeroMemory(pAd->StaCfg.FrameReportBuf, 2048);
+	NdisZeroMemory(pAd->StaCfg.BssReportOffset, sizeof(USHORT) * MAX_LEN_OF_BSS_TABLE);
+	NdisZeroMemory(pAd->StaCfg.MeasurementRequest, sizeof(RM_REQUEST_ACTION) * 4);
+
+	// 7. Point to the start of first element report element
+	pAd->StaCfg.FrameReportLen   = LENGTH_802_11 + sizeof(AIRONET_IAPP_HEADER);
+	DBGPRINT(RT_DEBUG_TRACE, ("FR len = %d\n", pAd->StaCfg.FrameReportLen));
+	pAd->StaCfg.LastBssIndex     = 0xff;
+	pAd->StaCfg.RMReqCnt         = 0;
+	pAd->StaCfg.ParallelReq      = FALSE;
+	pAd->StaCfg.ParallelDuration = 0;
+	pAd->StaCfg.ParallelChannel  = 0;
+	pAd->StaCfg.IAPPToken        = pRMReq->IAPP.Token;
+	pAd->StaCfg.CurrentRMReqIdx  = 0;
+	pAd->StaCfg.CLBusyBytes      = 0;
+	// Reset the statistics
+	for (i = 0; i < 8; i++)
+		pAd->StaCfg.RPIDensity[i] = 0;
+
+	Index = 0;
+
+	// 8. Save dialog token for report
+	pAd->StaCfg.IAPPToken = pRMReq->IAPP.Token;
+
+	// Save Activation delay & measurement offset, Not really needed
+
+	// 9. Point to the first request element
+	pData += sizeof(AIRONET_RM_REQUEST_FRAME);
+	//    Length should exclude the CISCO Aironet SNAP header
+	Length -= (sizeof(AIRONET_RM_REQUEST_FRAME) - LENGTH_802_1_H);
+
+	// 10. Start Parsing the Measurement elements.
+	//    Be careful about multiple MR elements within one frames.
+	while (Length > 0)
+	{
+		pReqElem = (PRM_REQUEST_ACTION) pData;
+		switch (pReqElem->ReqElem.Eid)
+		{
+			case IE_MEASUREMENT_REQUEST:
+				// From the example, it seems we only need to support one request in one frame
+				// There is no multiple request in one frame.
+				// Besides, looks like we need to take care the measurement request only.
+				// The measurement request is always 4 bytes.
+
+				// Start parsing this type of request.
+				// 0. Eid is IE_MEASUREMENT_REQUEST
+				// 1. Length didn't include Eid and Length field, it always be 8.
+				// 2. Measurement Token, we nned to save it for the corresponding report.
+				// 3. Measurement Mode, Although there are definitions, but we din't see value other than
+				//    0 from test specs examples.
+				// 4. Measurement Type, this is what we need to do.
+				switch (pReqElem->ReqElem.Type)
+				{
+					case MSRN_TYPE_CHANNEL_LOAD_REQ:
+					case MSRN_TYPE_NOISE_HIST_REQ:
+					case MSRN_TYPE_BEACON_REQ:
+						// Check the Enable non-serving channel measurement control
+						if (pAd->StaCfg.CCXControl.field.DCRMEnable == 0)
+						{
+							// Check channel before enqueue the action
+							if (pReqElem->Measurement.Channel != pAd->CommonCfg.Channel)
+								break;
+						}
+						else
+						{
+							// If off channel measurement, check the TU duration limit
+							if (pReqElem->Measurement.Channel != pAd->CommonCfg.Channel)
+								if (pReqElem->Measurement.Duration > pAd->StaCfg.CCXControl.field.TuLimit)
+									break;
+						}
+
+						// Save requests and execute actions later
+						NdisMoveMemory(&pAd->StaCfg.MeasurementRequest[Index], pReqElem, sizeof(RM_REQUEST_ACTION));
+						Index += 1;
+						break;
+
+					case MSRN_TYPE_FRAME_REQ:
+						// Since it's option, we will support later
+						// FrameRequestAction(pAd, pData);
+						break;
+
+					default:
+						break;
+				}
+
+				// Point to next Measurement request
+				pData  += sizeof(RM_REQUEST_ACTION);
+				Length -= sizeof(RM_REQUEST_ACTION);
+				break;
+
+			// We accept request only, all others are dropped
+			case IE_MEASUREMENT_REPORT:
+			case IE_AP_TX_POWER:
+			case IE_MEASUREMENT_CAPABILITY:
+			default:
+				return;
+		}
+	}
+
+	// 11. Update some flags and index
+	pAd->StaCfg.RMReqCnt = Index;
+
+	if (Index)
+	{
+		MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_REQ, 0, NULL);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<----- AironetMsgAction\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+{
+	PRM_REQUEST_ACTION	pReq;
+
+	// 1. Point to next request element
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+	// 2. Parse measurement type and call appropriate functions
+	if (pReq->ReqElem.Type == MSRN_TYPE_CHANNEL_LOAD_REQ)
+		// Channel Load measurement request
+		ChannelLoadRequestAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_NOISE_HIST_REQ)
+		// Noise Histogram measurement request
+		NoiseHistRequestAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_BEACON_REQ)
+		// Beacon measurement request
+		BeaconRequestAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else
+		// Unknown. Do nothing and return, this should never happen
+		return;
+
+	// 3. Peek into the next request, if it's parallel, we will update the scan time to the largest one
+	if ((pAd->StaCfg.CurrentRMReqIdx + 1) < pAd->StaCfg.RMReqCnt)
+	{
+		pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx + 1];
+		// Check for parallel bit
+		if ((pReq->ReqElem.Mode & 0x01) && (pReq->Measurement.Channel == pAd->StaCfg.CCXScanChannel))
+		{
+			// Update parallel mode request information
+			pAd->StaCfg.ParallelReq = TRUE;
+			pAd->StaCfg.CCXScanTime = ((pReq->Measurement.Duration > pAd->StaCfg.CCXScanTime) ?
+			(pReq->Measurement.Duration) : (pAd->StaCfg.CCXScanTime));
+		}
+	}
+
+	// 4. Call RT28XX_MLME_HANDLER to execute the request mlme commands, Scan request is the only one used
+	RT28XX_MLME_HANDLER(pAd);
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare channel load report action, special scan operation added
+		to support
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		pData		Start from element ID
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ChannelLoadRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PRM_REQUEST_ACTION				pReq;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+	UCHAR							ZeroSsid[32];
+	NDIS_STATUS						NStatus;
+	PUCHAR							pOutBuffer = NULL;
+	PHEADER_802_11					pNullFrame;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadRequestAction ----->\n"));
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[Index];
+	NdisZeroMemory(ZeroSsid, 32);
+
+	// Prepare for special scan request
+	// The scan definition is different with our Active, Passive scan definition.
+	// For CCX2, Active means send out probe request with broadcast BSSID.
+	// Passive means no probe request sent, only listen to the beacons.
+	// The channel scanned is fixed as specified, no need to scan all channels.
+	// The scan wait time is specified in the request too.
+	// Passive scan Mode
+
+	// Control state machine is not idle, reject the request
+	if ((pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE) && (Index == 0))
+		return;
+
+	// Fill out stuff for scan request
+	ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_CHANNEL_LOAD);
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+	// Reset some internal control flags to make sure this scan works.
+	BssTableInit(&pAd->StaCfg.CCXBssTab);
+	pAd->StaCfg.ScanCnt        = 0;
+	pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+	pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Duration %d, Channel %d!\n", pReq->Measurement.Duration, pReq->Measurement.Channel));
+
+	// If it's non serving channel scan, send out a null frame with PSM bit on.
+	if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+	{
+		// Use MLME enqueue method
+		NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+		if (NStatus	!= NDIS_STATUS_SUCCESS)
+			return;
+
+		pNullFrame = (PHEADER_802_11) pOutBuffer;;
+		// Make the power save Null frame with PSM bit on
+		MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+		pNullFrame->Duration 	= 0;
+		pNullFrame->FC.Type 	= BTYPE_DATA;
+		pNullFrame->FC.PwrMgmt	= PWR_SAVE;
+
+		// Send using priority queue
+		MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+		MlmeFreeMemory(pAd, pOutBuffer);
+		DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+		RTMPusecDelay(5000);
+	}
+
+	pAd->StaCfg.CCXReqType     = MSRN_TYPE_CHANNEL_LOAD_REQ;
+	pAd->StaCfg.CLBusyBytes    = 0;
+	// Enable Rx with promiscuous reception
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, 0x1010);
+
+	// Set channel load measurement flag
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadRequestAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare noise histogram report action, special scan operation added
+		to support
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		pData		Start from element ID
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	NoiseHistRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PRM_REQUEST_ACTION				pReq;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+	UCHAR							ZeroSsid[32], i;
+	NDIS_STATUS						NStatus;
+	PUCHAR							pOutBuffer = NULL;
+	PHEADER_802_11					pNullFrame;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistRequestAction ----->\n"));
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[Index];
+	NdisZeroMemory(ZeroSsid, 32);
+
+	// Prepare for special scan request
+	// The scan definition is different with our Active, Passive scan definition.
+	// For CCX2, Active means send out probe request with broadcast BSSID.
+	// Passive means no probe request sent, only listen to the beacons.
+	// The channel scanned is fixed as specified, no need to scan all channels.
+	// The scan wait time is specified in the request too.
+	// Passive scan Mode
+
+	// Control state machine is not idle, reject the request
+	if ((pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE) && (Index == 0))
+		return;
+
+	// Fill out stuff for scan request
+	ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_NOISE);
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+	// Reset some internal control flags to make sure this scan works.
+	BssTableInit(&pAd->StaCfg.CCXBssTab);
+	pAd->StaCfg.ScanCnt        = 0;
+	pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+	pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+	pAd->StaCfg.CCXReqType     = MSRN_TYPE_NOISE_HIST_REQ;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Duration %d, Channel %d!\n", pReq->Measurement.Duration, pReq->Measurement.Channel));
+
+	// If it's non serving channel scan, send out a null frame with PSM bit on.
+	if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+	{
+		// Use MLME enqueue method
+		NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+		if (NStatus	!= NDIS_STATUS_SUCCESS)
+			return;
+
+		pNullFrame = (PHEADER_802_11) pOutBuffer;
+		// Make the power save Null frame with PSM bit on
+		MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+		pNullFrame->Duration 	= 0;
+		pNullFrame->FC.Type  	= BTYPE_DATA;
+		pNullFrame->FC.PwrMgmt	= PWR_SAVE;
+
+		// Send using priority queue
+		MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+		MlmeFreeMemory(pAd, pOutBuffer);
+		DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+		RTMPusecDelay(5000);
+	}
+
+	// Reset the statistics
+	for (i = 0; i < 8; i++)
+		pAd->StaCfg.RPIDensity[i] = 0;
+
+	// Enable Rx with promiscuous reception
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, 0x1010);
+
+	// Set channel load measurement flag
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistRequestAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare Beacon report action, special scan operation added
+		to support
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		pData		Start from element ID
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	BeaconRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PRM_REQUEST_ACTION				pReq;
+	NDIS_STATUS						NStatus;
+	PUCHAR							pOutBuffer = NULL;
+	PHEADER_802_11					pNullFrame;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+	UCHAR							ZeroSsid[32];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconRequestAction ----->\n"));
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[Index];
+	NdisZeroMemory(ZeroSsid, 32);
+
+	// Prepare for special scan request
+	// The scan definition is different with our Active, Passive scan definition.
+	// For CCX2, Active means send out probe request with broadcast BSSID.
+	// Passive means no probe request sent, only listen to the beacons.
+	// The channel scanned is fixed as specified, no need to scan all channels.
+	// The scan wait time is specified in the request too.
+	if (pReq->Measurement.ScanMode == MSRN_SCAN_MODE_PASSIVE)
+	{
+		// Passive scan Mode
+		DBGPRINT(RT_DEBUG_TRACE, ("Passive Scan Mode!\n"));
+
+		// Control state machine is not idle, reject the request
+		if ((pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE) && (Index == 0))
+			return;
+
+		// Fill out stuff for scan request
+		ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_PASSIVE);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+		// Reset some internal control flags to make sure this scan works.
+		BssTableInit(&pAd->StaCfg.CCXBssTab);
+		pAd->StaCfg.ScanCnt        = 0;
+		pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+		pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+		pAd->StaCfg.CCXReqType     = MSRN_TYPE_BEACON_REQ;
+		DBGPRINT(RT_DEBUG_TRACE, ("Duration %d!\n", pReq->Measurement.Duration));
+
+		// If it's non serving channel scan, send out a null frame with PSM bit on.
+		if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+		{
+			// Use MLME enqueue method
+			NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus	!= NDIS_STATUS_SUCCESS)
+				return;
+
+			pNullFrame = (PHEADER_802_11) pOutBuffer;
+			// Make the power save Null frame with PSM bit on
+			MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+			pNullFrame->Duration 	= 0;
+			pNullFrame->FC.Type     = BTYPE_DATA;
+			pNullFrame->FC.PwrMgmt  = PWR_SAVE;
+
+			// Send using priority queue
+			MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+			RTMPusecDelay(5000);
+		}
+
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+	}
+	else if (pReq->Measurement.ScanMode == MSRN_SCAN_MODE_ACTIVE)
+	{
+		// Active scan Mode
+		DBGPRINT(RT_DEBUG_TRACE, ("Active Scan Mode!\n"));
+
+		// Control state machine is not idle, reject the request
+		if (pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+			return;
+
+		// Fill out stuff for scan request
+		ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_ACTIVE);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+		// Reset some internal control flags to make sure this scan works.
+		BssTableInit(&pAd->StaCfg.CCXBssTab);
+		pAd->StaCfg.ScanCnt        = 0;
+		pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+		pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+		pAd->StaCfg.CCXReqType     = MSRN_TYPE_BEACON_REQ;
+		DBGPRINT(RT_DEBUG_TRACE, ("Duration %d!\n", pReq->Measurement.Duration));
+
+		// If it's non serving channel scan, send out a null frame with PSM bit on.
+		if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+		{
+			// Use MLME enqueue method
+			NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus	!= NDIS_STATUS_SUCCESS)
+				return;
+
+			pNullFrame = (PHEADER_802_11) pOutBuffer;
+			// Make the power save Null frame with PSM bit on
+			MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+			pNullFrame->Duration 	= 0;
+			pNullFrame->FC.Type     = BTYPE_DATA;
+			pNullFrame->FC.PwrMgmt  = PWR_SAVE;
+
+			// Send using priority queue
+			MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+			RTMPusecDelay(5000);
+		}
+
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+	}
+	else if (pReq->Measurement.ScanMode == MSRN_SCAN_MODE_BEACON_TABLE)
+	{
+		// Beacon report Mode, report all the APS in current bss table
+		DBGPRINT(RT_DEBUG_TRACE, ("Beacon Report Mode!\n"));
+
+		// Copy current BSS table to CCX table, we can omit this step later on.
+		NdisMoveMemory(&pAd->StaCfg.CCXBssTab, &pAd->ScanTab, sizeof(BSS_TABLE));
+
+		// Create beacon report from Bss table
+		AironetCreateBeaconReportFromBssTable(pAd);
+
+		// Set state to scanning
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+
+		// Enqueue report request
+		// Cisco scan request is finished, prepare beacon report
+		MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_DONE, 0, NULL);
+	}
+	else
+	{
+		// Wrong scan Mode
+		DBGPRINT(RT_DEBUG_TRACE, ("Wrong Scan Mode!\n"));
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconRequestAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+{
+	PRM_REQUEST_ACTION	pReq;
+	ULONG				Now32;
+
+    NdisGetSystemUpTime(&Now32);
+	pAd->StaCfg.LastBeaconRxTime = Now32;
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetReportAction ----->\n"));
+
+	// 1. Parse measurement type and call appropriate functions
+	if (pReq->ReqElem.Type == MSRN_TYPE_CHANNEL_LOAD_REQ)
+		// Channel Load measurement request
+		ChannelLoadReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_NOISE_HIST_REQ)
+		// Noise Histogram measurement request
+		NoiseHistReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_BEACON_REQ)
+		// Beacon measurement request
+		BeaconReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else
+		// Unknown. Do nothing and return
+		;
+
+	// 2. Point to the correct index of action element, start from 0
+	pAd->StaCfg.CurrentRMReqIdx++;
+
+	// 3. Check for parallel actions
+	if (pAd->StaCfg.ParallelReq == TRUE)
+	{
+		pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+		// Process next action right away
+		if (pReq->ReqElem.Type == MSRN_TYPE_CHANNEL_LOAD_REQ)
+			// Channel Load measurement request
+			ChannelLoadReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+		else if (pReq->ReqElem.Type == MSRN_TYPE_NOISE_HIST_REQ)
+			// Noise Histogram measurement request
+			NoiseHistReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+
+		pAd->StaCfg.ParallelReq = FALSE;
+		pAd->StaCfg.CurrentRMReqIdx++;
+	}
+
+	if (pAd->StaCfg.CurrentRMReqIdx >= pAd->StaCfg.RMReqCnt)
+	{
+		// 4. There is no more unprocessed measurement request, go for transmit this report
+		AironetFinalReportAction(pAd);
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+	}
+	else
+	{
+		pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+		if (pReq->Measurement.Channel != pAd->CommonCfg.Channel)
+		{
+			RTMPusecDelay(100000);
+		}
+
+		// 5. There are more requests to be measure
+		MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_REQ, 0, NULL);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetFinalReportAction(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PUCHAR					pDest;
+	PAIRONET_IAPP_HEADER	pIAPP;
+	PHEADER_802_11			pHeader;
+	UCHAR					AckRate = RATE_2;
+	USHORT					AckDuration = 0;
+	NDIS_STATUS				NStatus;
+	PUCHAR					pOutBuffer = NULL;
+	ULONG					FrameLen = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetFinalReportAction ----->\n"));
+
+	// 0. Set up the frame pointer, Frame was inited at the end of message action
+	pDest = &pAd->StaCfg.FrameReportBuf[LENGTH_802_11];
+
+	// 1. Update report IAPP fields
+	pIAPP = (PAIRONET_IAPP_HEADER) pDest;
+
+	// 2. Copy Cisco SNAP header
+	NdisMoveMemory(pIAPP->CiscoSnapHeader, SNAP_AIRONET, LENGTH_802_1_H);
+
+	// 3. network order for this 16bit length
+	pIAPP->Length  = cpu2be16(pAd->StaCfg.FrameReportLen - LENGTH_802_11 - LENGTH_802_1_H);
+
+	// 3.1 sanity check the report length, ignore it if there is nothing to report
+	if (be2cpu16(pIAPP->Length) <= 18)
+		return;
+
+	// 4. Type must be 0x32
+	pIAPP->Type    = AIRONET_IAPP_TYPE;
+
+	// 5. SubType for report must be 0x81
+	pIAPP->SubType = AIRONET_IAPP_SUBTYPE_REPORT;
+
+	// 6. DA is not used and must be zero, although the whole frame was cleared at the start of function
+	//    We will do it again here. We can use BSSID instead
+	COPY_MAC_ADDR(pIAPP->DA, pAd->CommonCfg.Bssid);
+
+	// 7. SA is the client reporting which must be our MAC
+	COPY_MAC_ADDR(pIAPP->SA, pAd->CurrentAddress);
+
+	// 8. Copy the saved dialog token
+	pIAPP->Token = pAd->StaCfg.IAPPToken;
+
+	// 9. Make the Report frame 802.11 header
+	//    Reuse function in wpa.c
+	pHeader = (PHEADER_802_11) pAd->StaCfg.FrameReportBuf;
+	pAd->Sequence ++;
+	WpaMacHeaderInit(pAd, pHeader, 0, pAd->CommonCfg.Bssid);
+
+	// ACK size	is 14 include CRC, and its rate	is based on real time information
+	AckRate     = pAd->CommonCfg.ExpectedACKRate[pAd->CommonCfg.MlmeRate];
+	AckDuration = RTMPCalcDuration(pAd, AckRate, 14);
+	pHeader->Duration = pAd->CommonCfg.Dsifs + AckDuration;
+
+	// Use MLME enqueue method
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus	!= NDIS_STATUS_SUCCESS)
+		return;
+
+	// 10. Prepare report frame with dynamic outbuffer. Just simply copy everything.
+	MakeOutgoingFrame(pOutBuffer,                       &FrameLen,
+	                  pAd->StaCfg.FrameReportLen, pAd->StaCfg.FrameReportBuf,
+		              END_OF_ARGS);
+
+	// 11. Send using priority queue
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	pAd->StaCfg.CCXReqType = MSRN_TYPE_UNUSED;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetFinalReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ChannelLoadReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PCHANNEL_LOAD_REPORT		pLoad;
+	PUCHAR						pDest;
+	UCHAR						CCABusyFraction;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadReportAction ----->\n"));
+
+	// Disable Rx with promiscuous reception, make it back to normal
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL); // Staion not drop control frame will fail WiFi Certification.
+
+	// 0. Setup pointer for processing beacon & probe response
+	pDest = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+	pReport = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+
+	// 1. Fill Measurement report element field.
+	pReport->Eid    = IE_MEASUREMENT_REPORT;
+	// Fixed Length at 9, not include Eid and length fields
+	pReport->Length = 9;
+	pReport->Token  = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Token;
+	pReport->Mode   = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Mode;
+	pReport->Type   = MSRN_TYPE_CHANNEL_LOAD_REQ;
+
+	// 2. Fill channel report measurement data
+	pDest += sizeof(MEASUREMENT_REPORT_ELEMENT);
+	pLoad  = (PCHANNEL_LOAD_REPORT) pDest;
+	pLoad->Channel  = pAd->StaCfg.MeasurementRequest[Index].Measurement.Channel;
+	pLoad->Spare    = 0;
+	pLoad->Duration = pAd->StaCfg.MeasurementRequest[Index].Measurement.Duration;
+
+	// 3. Calculate the CCA Busy Fraction
+	//    (Bytes + ACK size) * 8 / Tx speed * 255 / 1000 / measurement duration, use 24 us Tx speed
+	//     =  (Bytes + ACK) / 12 / duration
+	//     9 is the good value for pAd->StaCfg.CLFactor
+	// CCABusyFraction = (UCHAR) (pAd->StaCfg.CLBusyBytes / 9 / pLoad->Duration);
+	CCABusyFraction = (UCHAR) (pAd->StaCfg.CLBusyBytes / pAd->StaCfg.CLFactor / pLoad->Duration);
+	if (CCABusyFraction < 10)
+			CCABusyFraction = (UCHAR) (pAd->StaCfg.CLBusyBytes / 3 / pLoad->Duration) + 1;
+
+	pLoad->CCABusy = CCABusyFraction;
+	DBGPRINT(RT_DEBUG_TRACE, ("CLBusyByte %ld, Duration %d, Result, %d\n", pAd->StaCfg.CLBusyBytes, pLoad->Duration, CCABusyFraction));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen %d\n", pAd->StaCfg.FrameReportLen));
+	pAd->StaCfg.FrameReportLen += (sizeof(MEASUREMENT_REPORT_ELEMENT) + sizeof(CHANNEL_LOAD_REPORT));
+	DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen %d\n", pAd->StaCfg.FrameReportLen));
+
+	// 4. Clear channel load measurement flag
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	// 5. reset to idle state
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	NoiseHistReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PNOISE_HIST_REPORT			pNoise;
+	PUCHAR						pDest;
+	UCHAR						i,NoiseCnt;
+	USHORT						TotalRPICnt, TotalRPISum;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistReportAction ----->\n"));
+
+	// 0. Disable Rx with promiscuous reception, make it back to normal
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL); // Staion not drop control frame will fail WiFi Certification.
+	// 1. Setup pointer for processing beacon & probe response
+	pDest = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+	pReport = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+
+	// 2. Fill Measurement report element field.
+	pReport->Eid    = IE_MEASUREMENT_REPORT;
+	// Fixed Length at 16, not include Eid and length fields
+	pReport->Length = 16;
+	pReport->Token  = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Token;
+	pReport->Mode   = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Mode;
+	pReport->Type   = MSRN_TYPE_NOISE_HIST_REQ;
+
+	// 3. Fill noise histogram report measurement data
+	pDest += sizeof(MEASUREMENT_REPORT_ELEMENT);
+	pNoise  = (PNOISE_HIST_REPORT) pDest;
+	pNoise->Channel  = pAd->StaCfg.MeasurementRequest[Index].Measurement.Channel;
+	pNoise->Spare    = 0;
+	pNoise->Duration = pAd->StaCfg.MeasurementRequest[Index].Measurement.Duration;
+	// 4. Fill Noise histogram, the total RPI counts should be 0.4 * TU
+	//    We estimate 4000 normal packets received durning 10 seconds test.
+	//    Adjust it if required.
+	// 3 is a good value for pAd->StaCfg.NHFactor
+	// TotalRPICnt = pNoise->Duration * 3 / 10;
+	TotalRPICnt = pNoise->Duration * pAd->StaCfg.NHFactor / 10;
+	TotalRPISum = 0;
+
+	for (i = 0; i < 8; i++)
+	{
+		TotalRPISum += pAd->StaCfg.RPIDensity[i];
+		DBGPRINT(RT_DEBUG_TRACE, ("RPI %d Conuts %d\n", i, pAd->StaCfg.RPIDensity[i]));
+	}
+
+	// Double check if the counter is larger than our expectation.
+	// We will replace it with the total number plus a fraction.
+	if (TotalRPISum > TotalRPICnt)
+		TotalRPICnt = TotalRPISum + pNoise->Duration / 20;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Total RPI Conuts %d\n", TotalRPICnt));
+
+	// 5. Initialize noise count for the total summation of 0xff
+	NoiseCnt = 0;
+	for (i = 1; i < 8; i++)
+	{
+		pNoise->Density[i] = (UCHAR) (pAd->StaCfg.RPIDensity[i] * 255 / TotalRPICnt);
+		if ((pNoise->Density[i] == 0) && (pAd->StaCfg.RPIDensity[i] != 0))
+			pNoise->Density[i]++;
+		NoiseCnt += pNoise->Density[i];
+		DBGPRINT(RT_DEBUG_TRACE, ("Reported RPI[%d]  = 0x%02x\n", i, pNoise->Density[i]));
+	}
+
+	// 6. RPI[0] represents the rest of counts
+	pNoise->Density[0] = 0xff - NoiseCnt;
+	DBGPRINT(RT_DEBUG_TRACE, ("Reported RPI[0]  = 0x%02x\n", pNoise->Density[0]));
+
+	pAd->StaCfg.FrameReportLen += (sizeof(MEASUREMENT_REPORT_ELEMENT) + sizeof(NOISE_HIST_REPORT));
+
+	// 7. Clear channel load measurement flag
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	// 8. reset to idle state
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare Beacon report action,
+
+	Arguments:
+		pAd	Pointer	to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	BeaconReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconReportAction ----->\n"));
+
+	// Looks like we don't have anything thing need to do here.
+	// All measurement report already finished in AddBeaconReport
+	// The length is in the FrameReportLen
+
+	// reset Beacon index for next beacon request
+	pAd->StaCfg.LastBssIndex = 0xff;
+
+	// reset to idle state
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+		Index		Current BSSID in CCXBsstab entry index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetAddBeaconReport(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	ULONG				Index,
+	IN	PMLME_QUEUE_ELEM	pElem)
+{
+	PVOID						pMsg;
+	PUCHAR						pSrc, pDest;
+	UCHAR						ReqIdx;
+	ULONG						MsgLen;
+	USHORT						Length;
+	PFRAME_802_11				pFrame;
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PEID_STRUCT			        pEid;
+	PBEACON_REPORT				pBeaconReport;
+	PBSS_ENTRY					pBss;
+
+	// 0. Setup pointer for processing beacon & probe response
+	pMsg   = pElem->Msg;
+	MsgLen = pElem->MsgLen;
+	pFrame = (PFRAME_802_11) pMsg;
+	pSrc   = pFrame->Octet;				// Start from AP TSF
+	pBss   = (PBSS_ENTRY) &pAd->StaCfg.CCXBssTab.BssEntry[Index];
+	ReqIdx = pAd->StaCfg.CurrentRMReqIdx;
+
+	// 1 Check the Index, if we already create this entry, only update the average RSSI
+	if ((Index <= pAd->StaCfg.LastBssIndex) && (pAd->StaCfg.LastBssIndex != 0xff))
+	{
+		pDest  = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.BssReportOffset[Index]];
+		// Point to bss report information
+		pDest += sizeof(MEASUREMENT_REPORT_ELEMENT);
+		pBeaconReport = (PBEACON_REPORT) pDest;
+
+		// Update Rx power, in dBm
+		// Get the original RSSI readback from BBP
+		pBeaconReport->RxPower += pAd->BbpRssiToDbmDelta;
+		// Average the Rssi reading
+		pBeaconReport->RxPower  = (pBeaconReport->RxPower + pBss->Rssi) / 2;
+		// Get to dBm format
+		pBeaconReport->RxPower -= pAd->BbpRssiToDbmDelta;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Bssid %02x:%02x:%02x:%02x:%02x:%02x ",
+			pBss->Bssid[0], pBss->Bssid[1], pBss->Bssid[2],
+			pBss->Bssid[3], pBss->Bssid[4], pBss->Bssid[5]));
+		DBGPRINT(RT_DEBUG_TRACE, ("RxPower[%ld] Rssi %d, Avg Rssi %d\n", Index, (pBss->Rssi - pAd->BbpRssiToDbmDelta), pBeaconReport->RxPower - 256));
+		DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen = %d\n", pAd->StaCfg.BssReportOffset[Index]));
+
+		// Update other information here
+
+		// Done
+		return;
+	}
+
+	// 2. Update reported Index
+	pAd->StaCfg.LastBssIndex = Index;
+
+	// 3. Setup the buffer address for copying this BSSID into reporting frame
+	//    The offset should start after 802.11 header and report frame header.
+	pDest = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+
+	// 4. Save the start offset of each Bss in report frame
+	pAd->StaCfg.BssReportOffset[Index] = pAd->StaCfg.FrameReportLen;
+
+	// 5. Fill Measurement report fields
+	pReport = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+	pReport->Eid = IE_MEASUREMENT_REPORT;
+	pReport->Length = 0;
+	pReport->Token  = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Token;
+	pReport->Mode   = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Mode;
+	pReport->Type   = MSRN_TYPE_BEACON_REQ;
+	Length          = sizeof(MEASUREMENT_REPORT_ELEMENT);
+	pDest          += sizeof(MEASUREMENT_REPORT_ELEMENT);
+
+	// 6. Start thebeacon report format
+	pBeaconReport = (PBEACON_REPORT) pDest;
+	pDest        += sizeof(BEACON_REPORT);
+	Length       += sizeof(BEACON_REPORT);
+
+	// 7. Copy Channel number
+	pBeaconReport->Channel        = pBss->Channel;
+	pBeaconReport->Spare          = 0;
+	pBeaconReport->Duration       = pAd->StaCfg.MeasurementRequest[ReqIdx].Measurement.Duration;
+	pBeaconReport->PhyType        = ((pBss->SupRateLen+pBss->ExtRateLen > 4) ? PHY_ERP : PHY_DSS);
+	// 8. Rx power, in dBm
+	pBeaconReport->RxPower        = pBss->Rssi - pAd->BbpRssiToDbmDelta;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Bssid %02x:%02x:%02x:%02x:%02x:%02x ",
+		pBss->Bssid[0], pBss->Bssid[1], pBss->Bssid[2],
+		pBss->Bssid[3], pBss->Bssid[4], pBss->Bssid[5]));
+	DBGPRINT(RT_DEBUG_TRACE, ("RxPower[%ld], Rssi %d\n", Index, pBeaconReport->RxPower - 256));
+	DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen = %d\n", pAd->StaCfg.FrameReportLen));
+
+	pBeaconReport->BeaconInterval = pBss->BeaconPeriod;
+	COPY_MAC_ADDR(pBeaconReport->BSSID, pFrame->Hdr.Addr3);
+	NdisMoveMemory(pBeaconReport->ParentTSF, pSrc, 4);
+	NdisMoveMemory(pBeaconReport->TargetTSF, &pElem->TimeStamp.u.LowPart, 4);
+	NdisMoveMemory(&pBeaconReport->TargetTSF[4], &pElem->TimeStamp.u.HighPart, 4);
+
+	// 9. Skip the beacon frame and offset to start of capabilityinfo since we already processed capabilityinfo
+	pSrc += (TIMESTAMP_LEN + 2);
+	pBeaconReport->CapabilityInfo = *(USHORT *)pSrc;
+
+	// 10. Point to start of element ID
+	pSrc += 2;
+	pEid = (PEID_STRUCT) pSrc;
+
+	// 11. Start process all variable Eid oayload and add the appropriate to the frame report
+	while (((PUCHAR) pEid + pEid->Len + 1) < ((PUCHAR) pFrame + MsgLen))
+	{
+		// Only limited EID are required to report for CCX 2. It includes SSID, Supported rate,
+		// FH paramenter set, DS parameter set, CF parameter set, IBSS parameter set,
+		// TIM (report first 4 bytes only, radio measurement capability
+		switch (pEid->Eid)
+		{
+			case IE_SSID:
+			case IE_SUPP_RATES:
+			case IE_FH_PARM:
+			case IE_DS_PARM:
+			case IE_CF_PARM:
+			case IE_IBSS_PARM:
+				NdisMoveMemory(pDest, pEid, pEid->Len + 2);
+				pDest  += (pEid->Len + 2);
+				Length += (pEid->Len + 2);
+				break;
+
+			case IE_MEASUREMENT_CAPABILITY:
+				// Since this IE is duplicated with WPA security IE, we has to do sanity check before
+				// recognize it.
+				// 1. It also has fixed 6 bytes IE length.
+				if (pEid->Len != 6)
+					break;
+				// 2. Check the Cisco Aironet OUI
+				if (NdisEqualMemory(CISCO_OUI, (pSrc + 2), 3))
+				{
+					// Matched, this is what we want
+					NdisMoveMemory(pDest, pEid, pEid->Len + 2);
+					pDest  += (pEid->Len + 2);
+					Length += (pEid->Len + 2);
+				}
+				break;
+
+			case IE_TIM:
+				if (pEid->Len > 4)
+				{
+					// May truncate and report the first 4 bytes only, with the eid & len, total should be 6
+					NdisMoveMemory(pDest, pEid, 6);
+					pDest  += 6;
+					Length += 6;
+				}
+				else
+				{
+					NdisMoveMemory(pDest, pEid, pEid->Len + 2);
+					pDest  += (pEid->Len + 2);
+					Length += (pEid->Len + 2);
+				}
+				break;
+
+			default:
+				break;
+		}
+		// 12. Move to next element ID
+		pSrc += (2 + pEid->Len);
+		pEid = (PEID_STRUCT) pSrc;
+	}
+
+	// 13. Update the length in the header, not include EID and length
+	pReport->Length = Length - 4;
+
+	// 14. Update the frame report buffer data length
+	pAd->StaCfg.FrameReportLen += Length;
+	DBGPRINT(RT_DEBUG_TRACE, ("FR len = %d\n", pAd->StaCfg.FrameReportLen));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+		Index		Current BSSID in CCXBsstab entry index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetCreateBeaconReportFromBssTable(
+	IN	PRTMP_ADAPTER		pAd)
+{
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PBEACON_REPORT				pBeaconReport;
+	UCHAR						Index, ReqIdx;
+	USHORT						Length;
+	PUCHAR						pDest;
+	PBSS_ENTRY					pBss;
+
+	// 0. setup base pointer
+	ReqIdx = pAd->StaCfg.CurrentRMReqIdx;
+
+	for (Index = 0; Index < pAd->StaCfg.CCXBssTab.BssNr; Index++)
+	{
+		// 1. Setup the buffer address for copying this BSSID into reporting frame
+		//    The offset should start after 802.11 header and report frame header.
+		pDest  = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+		pBss   = (PBSS_ENTRY) &pAd->StaCfg.CCXBssTab.BssEntry[Index];
+		Length = 0;
+
+		// 2. Fill Measurement report fields
+		pReport         = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+		pReport->Eid    = IE_MEASUREMENT_REPORT;
+		pReport->Length = 0;
+		pReport->Token  = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Token;
+		pReport->Mode   = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Mode;
+		pReport->Type   = MSRN_TYPE_BEACON_REQ;
+		Length          = sizeof(MEASUREMENT_REPORT_ELEMENT);
+		pDest          += sizeof(MEASUREMENT_REPORT_ELEMENT);
+
+		// 3. Start the beacon report format
+		pBeaconReport = (PBEACON_REPORT) pDest;
+		pDest        += sizeof(BEACON_REPORT);
+		Length       += sizeof(BEACON_REPORT);
+
+		// 4. Copy Channel number
+		pBeaconReport->Channel        = pBss->Channel;
+		pBeaconReport->Spare          = 0;
+		pBeaconReport->Duration       = pAd->StaCfg.MeasurementRequest[ReqIdx].Measurement.Duration;
+		pBeaconReport->PhyType        = ((pBss->SupRateLen+pBss->ExtRateLen > 4) ? PHY_ERP : PHY_DSS);
+		pBeaconReport->RxPower        = pBss->Rssi - pAd->BbpRssiToDbmDelta;
+		pBeaconReport->BeaconInterval = pBss->BeaconPeriod;
+		pBeaconReport->CapabilityInfo = pBss->CapabilityInfo;
+		COPY_MAC_ADDR(pBeaconReport->BSSID, pBss->Bssid);
+		NdisMoveMemory(pBeaconReport->ParentTSF, pBss->PTSF, 4);
+		NdisMoveMemory(pBeaconReport->TargetTSF, pBss->TTSF, 8);
+
+		// 5. Create SSID
+		*pDest++ = 0x00;
+		*pDest++ = pBss->SsidLen;
+		NdisMoveMemory(pDest, pBss->Ssid, pBss->SsidLen);
+		pDest  += pBss->SsidLen;
+		Length += (2 + pBss->SsidLen);
+
+		// 6. Create SupportRates
+		*pDest++ = 0x01;
+		*pDest++ = pBss->SupRateLen;
+		NdisMoveMemory(pDest, pBss->SupRate, pBss->SupRateLen);
+		pDest  += pBss->SupRateLen;
+		Length += (2 + pBss->SupRateLen);
+
+		// 7. DS Parameter
+		*pDest++ = 0x03;
+		*pDest++ = 1;
+		*pDest++ = pBss->Channel;
+		Length  += 3;
+
+		// 8. IBSS parameter if presents
+		if (pBss->BssType == BSS_ADHOC)
+		{
+			*pDest++ = 0x06;
+			*pDest++ = 2;
+			*(PUSHORT) pDest = pBss->AtimWin;
+			pDest   += 2;
+			Length  += 4;
+		}
+
+		// 9. Update length field, not include EID and length
+		pReport->Length = Length - 4;
+
+		// 10. Update total frame size
+		pAd->StaCfg.FrameReportLen += Length;
+	}
+}
diff --git a/drivers/staging/rt2860/sta/assoc.c b/drivers/staging/rt2860/sta/assoc.c
new file mode 100644
index 000000000000..42db753eed70
--- /dev/null
+++ b/drivers/staging/rt2860/sta/assoc.c
@@ -0,0 +1,1826 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	assoc.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John		2004-9-3		porting from RT2500
+*/
+#include "../rt_config.h"
+
+UCHAR	CipherWpaTemplate[] = {
+		0xdd, 					// WPA IE
+		0x16,					// Length
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x01	// authentication
+		};
+
+UCHAR	CipherWpa2Template[] = {
+		0x30,					// RSN IE
+		0x14,					// Length
+		0x01, 0x00,				// Version
+		0x00, 0x0f, 0xac, 0x02,	// group cipher, TKIP
+		0x01, 0x00,				// number of pairwise
+		0x00, 0x0f, 0xac, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x0f, 0xac, 0x02,	// authentication
+		0x00, 0x00,				// RSN capability
+		};
+
+UCHAR	Ccx2IeInfo[] = { 0x00, 0x40, 0x96, 0x03, 0x02};
+
+/*
+	==========================================================================
+	Description:
+		association state machine init, including state transition and timer init
+	Parameters:
+		S - pointer to the association state machine
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID AssocStateMachineInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  STATE_MACHINE *S,
+	OUT STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(S, Trans, MAX_ASSOC_STATE, MAX_ASSOC_MSG, (STATE_MACHINE_FUNC)Drop, ASSOC_IDLE, ASSOC_MACHINE_BASE);
+
+	// first column
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)MlmeAssocReqAction);
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)MlmeReassocReqAction);
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)MlmeDisassocReqAction);
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+
+	// second column
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAssoc);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenReassoc);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenDisassociate);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_PEER_ASSOC_RSP, (STATE_MACHINE_FUNC)PeerAssocRspAction);
+	//
+	// Patch 3Com AP MOde:3CRWE454G72
+	// We send Assoc request frame to this AP, it always send Reassoc Rsp not Associate Rsp.
+	//
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_PEER_REASSOC_RSP, (STATE_MACHINE_FUNC)PeerAssocRspAction);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_ASSOC_TIMEOUT, (STATE_MACHINE_FUNC)AssocTimeoutAction);
+
+	// third column
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAssoc);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenReassoc);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenDisassociate);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_PEER_REASSOC_RSP, (STATE_MACHINE_FUNC)PeerReassocRspAction);
+	//
+	// Patch, AP doesn't send Reassociate Rsp frame to Station.
+	//
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_PEER_ASSOC_RSP, (STATE_MACHINE_FUNC)PeerReassocRspAction);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_REASSOC_TIMEOUT, (STATE_MACHINE_FUNC)ReassocTimeoutAction);
+
+	// fourth column
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAssoc);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenReassoc);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenDisassociate);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_DISASSOC_TIMEOUT, (STATE_MACHINE_FUNC)DisassocTimeoutAction);
+
+	// initialize the timer
+	RTMPInitTimer(pAd, &pAd->MlmeAux.AssocTimer, GET_TIMER_FUNCTION(AssocTimeout), pAd, FALSE);
+	RTMPInitTimer(pAd, &pAd->MlmeAux.ReassocTimer, GET_TIMER_FUNCTION(ReassocTimeout), pAd, FALSE);
+	RTMPInitTimer(pAd, &pAd->MlmeAux.DisassocTimer, GET_TIMER_FUNCTION(DisassocTimeout), pAd, FALSE);
+}
+
+/*
+	==========================================================================
+	Description:
+		Association timeout procedure. After association timeout, this function
+		will be called and it will put a message into the MLME queue
+	Parameters:
+		Standard timer parameters
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AssocTimeout(IN PVOID SystemSpecific1,
+				 IN PVOID FunctionContext,
+				 IN PVOID SystemSpecific2,
+				 IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_ASSOC_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		Reassociation timeout procedure. After reassociation timeout, this
+		function will be called and put a message into the MLME queue
+	Parameters:
+		Standard timer parameters
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID ReassocTimeout(IN PVOID SystemSpecific1,
+					IN PVOID FunctionContext,
+					IN PVOID SystemSpecific2,
+					IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_REASSOC_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		Disassociation timeout procedure. After disassociation timeout, this
+		function will be called and put a message into the MLME queue
+	Parameters:
+		Standard timer parameters
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID DisassocTimeout(IN PVOID SystemSpecific1,
+					IN PVOID FunctionContext,
+					IN PVOID SystemSpecific2,
+					IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_DISASSOC_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		mlme assoc req handling procedure
+	Parameters:
+		Adapter - Adapter pointer
+		Elem - MLME Queue Element
+	Pre:
+		the station has been authenticated and the following information is stored in the config
+			-# SSID
+			-# supported rates and their length
+			-# listen interval (Adapter->StaCfg.default_listen_count)
+			-# Transmit power  (Adapter->StaCfg.tx_power)
+	Post  :
+		-# An association request frame is generated and sent to the air
+		-# Association timer starts
+		-# Association state -> ASSOC_WAIT_RSP
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeAssocReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR			ApAddr[6];
+	HEADER_802_11	AssocHdr;
+	UCHAR			Ccx2Len = 5;
+	UCHAR			WmeIe[9] = {IE_VENDOR_SPECIFIC, 0x07, 0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
+	USHORT			ListenIntv;
+	ULONG			Timeout;
+	USHORT			CapabilityInfo;
+	BOOLEAN			TimerCancelled;
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	ULONG			tmp;
+	USHORT			VarIesOffset;
+	UCHAR			CkipFlag;
+	UCHAR			CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH];
+	UCHAR			AironetCkipIe = IE_AIRONET_CKIP;
+	UCHAR			AironetCkipLen = CKIP_NEGOTIATION_LENGTH;
+	UCHAR			AironetIPAddressIE = IE_AIRONET_IPADDRESS;
+	UCHAR			AironetIPAddressLen = AIRONET_IPADDRESS_LENGTH;
+	UCHAR			AironetIPAddressBuffer[AIRONET_IPADDRESS_LENGTH] = {0x00, 0x40, 0x96, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00};
+	USHORT			Status;
+
+	// Block all authentication request durning WPA block period
+	if (pAd->StaCfg.bBlockAssoc == TRUE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Block Assoc request durning WPA block period!\n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_STATE_MACHINE_REJECT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+	}
+	// check sanity first
+	else if (MlmeAssocReqSanity(pAd, Elem->Msg, Elem->MsgLen, ApAddr, &CapabilityInfo, &Timeout, &ListenIntv))
+	{
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer, &TimerCancelled);
+		COPY_MAC_ADDR(pAd->MlmeAux.Bssid, ApAddr);
+
+		// Get an unused nonpaged memory
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+		if (NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeAssocReqAction() allocate memory failed \n"));
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			Status = MLME_FAIL_NO_RESOURCE;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+			return;
+		}
+
+		// Add by James 03/06/27
+		pAd->StaCfg.AssocInfo.Length = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+		// Association don't need to report MAC address
+		pAd->StaCfg.AssocInfo.AvailableRequestFixedIEs =
+			NDIS_802_11_AI_REQFI_CAPABILITIES | NDIS_802_11_AI_REQFI_LISTENINTERVAL;
+		pAd->StaCfg.AssocInfo.RequestFixedIEs.Capabilities = CapabilityInfo;
+		pAd->StaCfg.AssocInfo.RequestFixedIEs.ListenInterval = ListenIntv;
+		// Only reassociate need this
+		//COPY_MAC_ADDR(pAd->StaCfg.AssocInfo.RequestFixedIEs.CurrentAPAddress, ApAddr);
+		pAd->StaCfg.AssocInfo.OffsetRequestIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+
+        NdisZeroMemory(pAd->StaCfg.ReqVarIEs, MAX_VIE_LEN);
+		// First add SSID
+		VarIesOffset = 0;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &SsidIe, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &pAd->MlmeAux.SsidLen, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+		VarIesOffset += pAd->MlmeAux.SsidLen;
+
+		// Second add Supported rates
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &SupRateIe, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &pAd->MlmeAux.SupRateLen, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, pAd->MlmeAux.SupRate, pAd->MlmeAux.SupRateLen);
+		VarIesOffset += pAd->MlmeAux.SupRateLen;
+		// End Add by James
+
+        if ((pAd->CommonCfg.Channel > 14) &&
+            (pAd->CommonCfg.bIEEE80211H == TRUE))
+            CapabilityInfo |= 0x0100;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Send ASSOC request...\n"));
+		MgtMacHeaderInit(pAd, &AssocHdr, SUBTYPE_ASSOC_REQ, 0, ApAddr, ApAddr);
+
+		// Build basic frame first
+		MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+						  sizeof(HEADER_802_11),	&AssocHdr,
+						  2,						&CapabilityInfo,
+						  2,						&ListenIntv,
+						  1,						&SsidIe,
+						  1,						&pAd->MlmeAux.SsidLen,
+						  pAd->MlmeAux.SsidLen, 	pAd->MlmeAux.Ssid,
+						  1,						&SupRateIe,
+						  1,						&pAd->MlmeAux.SupRateLen,
+						  pAd->MlmeAux.SupRateLen,  pAd->MlmeAux.SupRate,
+						  END_OF_ARGS);
+
+		if (pAd->MlmeAux.ExtRateLen != 0)
+		{
+			MakeOutgoingFrame(pOutBuffer + FrameLen,    &tmp,
+							  1,                        &ExtRateIe,
+							  1,                        &pAd->MlmeAux.ExtRateLen,
+							  pAd->MlmeAux.ExtRateLen,  pAd->MlmeAux.ExtRate,
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+#ifdef DOT11_N_SUPPORT
+		// HT
+		if ((pAd->MlmeAux.HtCapabilityLen > 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+		{
+			ULONG TmpLen;
+			UCHAR HtLen;
+			UCHAR BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+			if (pAd->StaActive.SupportedPhyInfo.bPreNHt == TRUE)
+			{
+				HtLen = SIZE_HT_CAP_IE + 4;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &WpaIe,
+							  1,                                &HtLen,
+							  4,                                &BROADCOM[0],
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+			}
+			else
+			{
+#ifdef RT_BIG_ENDIAN
+		        HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+
+#ifndef RT_BIG_ENDIAN
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &HtCapIe,
+							  1,                                &pAd->MlmeAux.HtCapabilityLen,
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+#else
+                NdisZeroMemory(&HtCapabilityTmp, sizeof(HT_CAPABILITY_IE));
+                NdisMoveMemory(&HtCapabilityTmp, &pAd->MlmeAux.HtCapability, pAd->MlmeAux.HtCapabilityLen);
+        		*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+        		*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+        		MakeOutgoingFrame(pOutBuffer + FrameLen,         &TmpLen,
+        							1,                           &HtCapIe,
+        							1,                           &pAd->MlmeAux.HtCapabilityLen,
+        							pAd->MlmeAux.HtCapabilityLen,&HtCapabilityTmp,
+        							END_OF_ARGS);
+#endif
+			}
+			FrameLen += TmpLen;
+		}
+#endif // DOT11_N_SUPPORT //
+
+		// add Ralink proprietary IE to inform AP this STA is going to use AGGREGATION or PIGGY-BACK+AGGREGATION
+		// Case I: (Aggregation + Piggy-Back)
+		// 1. user enable aggregation, AND
+		// 2. Mac support piggy-back
+		// 3. AP annouces it's PIGGY-BACK+AGGREGATION-capable in BEACON
+		// Case II: (Aggregation)
+		// 1. user enable aggregation, AND
+		// 2. AP annouces it's AGGREGATION-capable in BEACON
+		if (pAd->CommonCfg.bAggregationCapable)
+		{
+			if ((pAd->CommonCfg.bPiggyBackCapable) && ((pAd->MlmeAux.APRalinkIe & 0x00000003) == 3))
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x03, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+			else if (pAd->MlmeAux.APRalinkIe & 0x00000001)
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x01, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+		}
+		else
+		{
+			ULONG TmpLen;
+			UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x06, 0x00, 0x00, 0x00};
+			MakeOutgoingFrame(pOutBuffer+FrameLen,		 &TmpLen,
+							  9,						 RalinkIe,
+							  END_OF_ARGS);
+			FrameLen += TmpLen;
+		}
+
+		if (pAd->MlmeAux.APEdcaParm.bValid)
+		{
+			if (pAd->CommonCfg.bAPSDCapable && pAd->MlmeAux.APEdcaParm.bAPSDCapable)
+			{
+				QBSS_STA_INFO_PARM QosInfo;
+
+				NdisZeroMemory(&QosInfo, sizeof(QBSS_STA_INFO_PARM));
+				QosInfo.UAPSD_AC_BE = pAd->CommonCfg.bAPSDAC_BE;
+				QosInfo.UAPSD_AC_BK = pAd->CommonCfg.bAPSDAC_BK;
+				QosInfo.UAPSD_AC_VI = pAd->CommonCfg.bAPSDAC_VI;
+				QosInfo.UAPSD_AC_VO = pAd->CommonCfg.bAPSDAC_VO;
+				QosInfo.MaxSPLength = pAd->CommonCfg.MaxSPLength;
+				WmeIe[8] |= *(PUCHAR)&QosInfo;
+			}
+			else
+			{
+                // The Parameter Set Count is set to ¡§0¡¨ in the association request frames
+                // WmeIe[8] |= (pAd->MlmeAux.APEdcaParm.EdcaUpdateCount & 0x0f);
+			}
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen,    &tmp,
+							  9,                        &WmeIe[0],
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		//
+		// Let WPA(#221) Element ID on the end of this association frame.
+		// Otherwise some AP will fail on parsing Element ID and set status fail on Assoc Rsp.
+		// For example: Put Vendor Specific IE on the front of WPA IE.
+		// This happens on AP (Model No:Linksys WRK54G)
+		//
+		if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2)
+			)
+            )
+		{
+			UCHAR RSNIe = IE_WPA;
+
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+                (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2))
+			{
+				RSNIe = IE_WPA2;
+			}
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifdef SIOCSIWGENIE
+			if (pAd->StaCfg.WpaSupplicantUP != 1)
+#endif // SIOCSIWGENIE //
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+            	RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, BSS0);
+
+            // Check for WPA PMK cache list
+			if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2)
+			{
+			    INT     idx;
+                BOOLEAN FoundPMK = FALSE;
+				// Search chched PMKID, append it if existed
+				for (idx = 0; idx < PMKID_NO; idx++)
+				{
+					if (NdisEqualMemory(ApAddr, &pAd->StaCfg.SavedPMK[idx].BSSID, 6))
+					{
+						FoundPMK = TRUE;
+						break;
+					}
+				}
+
+				if (FoundPMK)
+				{
+					// Set PMK number
+					*(PUSHORT) &pAd->StaCfg.RSN_IE[pAd->StaCfg.RSNIE_Len] = 1;
+					NdisMoveMemory(&pAd->StaCfg.RSN_IE[pAd->StaCfg.RSNIE_Len + 2], &pAd->StaCfg.SavedPMK[idx].PMKID, 16);
+                    pAd->StaCfg.RSNIE_Len += 18;
+				}
+			}
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifdef SIOCSIWGENIE
+			if (pAd->StaCfg.WpaSupplicantUP == 1)
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,    		&tmp,
+		                        	pAd->StaCfg.RSNIE_Len,			pAd->StaCfg.RSN_IE,
+		                        	END_OF_ARGS);
+			}
+			else
+#endif
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,    		&tmp,
+				              		1,                              &RSNIe,
+		                        	1,                              &pAd->StaCfg.RSNIE_Len,
+		                        	pAd->StaCfg.RSNIE_Len,			pAd->StaCfg.RSN_IE,
+		                        	END_OF_ARGS);
+			}
+
+			FrameLen += tmp;
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifdef SIOCSIWGENIE
+			if (pAd->StaCfg.WpaSupplicantUP != 1)
+#endif
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+			{
+	            // Append Variable IE
+	            NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &RSNIe, 1);
+	            VarIesOffset += 1;
+	            NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &pAd->StaCfg.RSNIE_Len, 1);
+	            VarIesOffset += 1;
+			}
+			NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);
+			VarIesOffset += pAd->StaCfg.RSNIE_Len;
+
+			// Set Variable IEs Length
+			pAd->StaCfg.ReqVarIELen = VarIesOffset;
+		}
+
+		// We have update that at PeerBeaconAtJoinRequest()
+		CkipFlag = pAd->StaCfg.CkipFlag;
+		if (CkipFlag != 0)
+		{
+			NdisZeroMemory(CkipNegotiationBuffer, CKIP_NEGOTIATION_LENGTH);
+			CkipNegotiationBuffer[2] = 0x66;
+			// Make it try KP & MIC, since we have to follow the result from AssocRsp
+			CkipNegotiationBuffer[8] = 0x18;
+			CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH - 1] = 0x22;
+			CkipFlag = 0x18;
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, 	&tmp,
+						1,						  		&AironetCkipIe,
+						1,						  		&AironetCkipLen,
+						AironetCkipLen, 		  		CkipNegotiationBuffer,
+						END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		// Add CCX v2 request if CCX2 admin state is on
+		if (pAd->StaCfg.CCXControl.field.Enable == 1)
+		{
+
+			//
+			// Add AironetIPAddressIE for Cisco CCX 2.X
+			// Add CCX Version
+			//
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+						1,							&AironetIPAddressIE,
+						1,							&AironetIPAddressLen,
+						AironetIPAddressLen,		AironetIPAddressBuffer,
+						1,							&Ccx2Ie,
+						1,							&Ccx2Len,
+						Ccx2Len,				    Ccx2IeInfo,
+						END_OF_ARGS);
+			FrameLen += tmp;
+
+			//
+			// Add CipherSuite CCKM or LeapTkip if setting.
+			//
+#ifdef LEAP_SUPPORT
+			if (LEAP_CCKM_ON(pAd))
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+						CipherSuiteCiscoCCKMLen,		CipherSuiteCiscoCCKM,
+						END_OF_ARGS);
+				FrameLen += tmp;
+
+				// Third add RSN
+				NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, CipherSuiteCiscoCCKM, CipherSuiteCiscoCCKMLen); //Save CipherSuite
+				VarIesOffset += CipherSuiteCiscoCCKMLen;
+			}
+			else if ((pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP) && (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled))
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+						CipherSuiteCCXTkipLen,	    CipherSuiteCCXTkip,
+						END_OF_ARGS);
+				FrameLen += tmp;
+
+				// Third add RSN
+				NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, CipherSuiteCCXTkip, CipherSuiteCCXTkipLen);
+				VarIesOffset += CipherSuiteCCXTkipLen;
+			}
+#endif // LEAP_SUPPORT //
+
+			// Add by James 03/06/27
+			// Set Variable IEs Length
+			pAd->StaCfg.ReqVarIELen = VarIesOffset;
+			pAd->StaCfg.AssocInfo.RequestIELength = VarIesOffset;
+
+			// OffsetResponseIEs follow ReqVarIE
+			pAd->StaCfg.AssocInfo.OffsetResponseIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION) + pAd->StaCfg.ReqVarIELen;
+			// End Add by James
+		}
+
+
+		MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+
+		RTMPSetTimer(&pAd->MlmeAux.AssocTimer, Timeout);
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_WAIT_RSP;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeAssocReqAction() sanity check failed. BUG!!!!!! \n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+	}
+
+}
+
+/*
+	==========================================================================
+	Description:
+		mlme reassoc req handling procedure
+	Parameters:
+		Elem -
+	Pre:
+		-# SSID  (Adapter->StaCfg.ssid[])
+		-# BSSID (AP address, Adapter->StaCfg.bssid)
+		-# Supported rates (Adapter->StaCfg.supported_rates[])
+		-# Supported rates length (Adapter->StaCfg.supported_rates_len)
+		-# Tx power (Adapter->StaCfg.tx_power)
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeReassocReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR			ApAddr[6];
+	HEADER_802_11	ReassocHdr;
+	UCHAR			Ccx2Len = 5;
+	UCHAR			WmeIe[9] = {IE_VENDOR_SPECIFIC, 0x07, 0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
+	USHORT			CapabilityInfo, ListenIntv;
+	ULONG			Timeout;
+	ULONG			FrameLen = 0;
+	BOOLEAN			TimerCancelled;
+	NDIS_STATUS		NStatus;
+	ULONG			tmp;
+	PUCHAR			pOutBuffer = NULL;
+//CCX 2.X
+#ifdef LEAP_SUPPORT
+	UCHAR			CkipFlag;
+	UCHAR			CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH];
+	UCHAR			AironetCkipIe = IE_AIRONET_CKIP;
+	UCHAR			AironetCkipLen = CKIP_NEGOTIATION_LENGTH;
+	UCHAR			AironetIPAddressIE = IE_AIRONET_IPADDRESS;
+	UCHAR			AironetIPAddressLen = AIRONET_IPADDRESS_LENGTH;
+	UCHAR			AironetIPAddressBuffer[AIRONET_IPADDRESS_LENGTH] = {0x00, 0x40, 0x96, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00};
+	UCHAR			AironetCCKMReassocIE = IE_AIRONET_CCKMREASSOC;
+	UCHAR			AironetCCKMReassocLen = AIRONET_CCKMREASSOC_LENGTH;
+	UCHAR			AironetCCKMReassocBuffer[AIRONET_CCKMREASSOC_LENGTH];
+	UCHAR			AironetOUI[] = {0x00, 0x40, 0x96, 0x00};
+	UCHAR			MICMN[16];
+	UCHAR			CalcMicBuffer[80];
+	ULONG			CalcMicBufferLen = 0;
+#endif // LEAP_SUPPORT //
+	USHORT			Status;
+
+	// Block all authentication request durning WPA block period
+	if (pAd->StaCfg.bBlockAssoc == TRUE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Block ReAssoc request durning WPA block period!\n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_STATE_MACHINE_REJECT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+	}
+	// the parameters are the same as the association
+	else if(MlmeAssocReqSanity(pAd, Elem->Msg, Elem->MsgLen, ApAddr, &CapabilityInfo, &Timeout, &ListenIntv))
+	{
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer, &TimerCancelled);
+
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeReassocReqAction() allocate memory failed \n"));
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			Status = MLME_FAIL_NO_RESOURCE;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+			return;
+		}
+
+		COPY_MAC_ADDR(pAd->MlmeAux.Bssid, ApAddr);
+
+		// make frame, use bssid as the AP address??
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Send RE-ASSOC request...\n"));
+		MgtMacHeaderInit(pAd, &ReassocHdr, SUBTYPE_REASSOC_REQ, 0, ApAddr, ApAddr);
+		MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+						  sizeof(HEADER_802_11),    &ReassocHdr,
+						  2,                        &CapabilityInfo,
+						  2,                        &ListenIntv,
+						  MAC_ADDR_LEN,             ApAddr,
+						  1,                        &SsidIe,
+						  1,                        &pAd->MlmeAux.SsidLen,
+						  pAd->MlmeAux.SsidLen,     pAd->MlmeAux.Ssid,
+						  1,                        &SupRateIe,
+						  1,						&pAd->MlmeAux.SupRateLen,
+						  pAd->MlmeAux.SupRateLen,  pAd->MlmeAux.SupRate,
+						  END_OF_ARGS);
+
+		if (pAd->MlmeAux.ExtRateLen != 0)
+		{
+			MakeOutgoingFrame(pOutBuffer + FrameLen,        &tmp,
+							  1,                            &ExtRateIe,
+							  1,                            &pAd->MlmeAux.ExtRateLen,
+							  pAd->MlmeAux.ExtRateLen,	    pAd->MlmeAux.ExtRate,
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		if (pAd->MlmeAux.APEdcaParm.bValid)
+		{
+			if (pAd->CommonCfg.bAPSDCapable && pAd->MlmeAux.APEdcaParm.bAPSDCapable)
+			{
+				QBSS_STA_INFO_PARM QosInfo;
+
+				NdisZeroMemory(&QosInfo, sizeof(QBSS_STA_INFO_PARM));
+				QosInfo.UAPSD_AC_BE = pAd->CommonCfg.bAPSDAC_BE;
+				QosInfo.UAPSD_AC_BK = pAd->CommonCfg.bAPSDAC_BK;
+				QosInfo.UAPSD_AC_VI = pAd->CommonCfg.bAPSDAC_VI;
+				QosInfo.UAPSD_AC_VO = pAd->CommonCfg.bAPSDAC_VO;
+				QosInfo.MaxSPLength = pAd->CommonCfg.MaxSPLength;
+				WmeIe[8] |= *(PUCHAR)&QosInfo;
+			}
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen,    &tmp,
+							  9,                        &WmeIe[0],
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+#ifdef DOT11_N_SUPPORT
+		// HT
+		if ((pAd->MlmeAux.HtCapabilityLen > 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+		{
+			ULONG TmpLen;
+			UCHAR HtLen;
+			UCHAR BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+			if (pAd->StaActive.SupportedPhyInfo.bPreNHt == TRUE)
+			{
+				HtLen = SIZE_HT_CAP_IE + 4;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &WpaIe,
+							  1,                                &HtLen,
+							  4,                                &BROADCOM[0],
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+			}
+			else
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &HtCapIe,
+							  1,                                &pAd->MlmeAux.HtCapabilityLen,
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+			}
+			FrameLen += TmpLen;
+		}
+#endif // DOT11_N_SUPPORT //
+
+		// add Ralink proprietary IE to inform AP this STA is going to use AGGREGATION or PIGGY-BACK+AGGREGATION
+		// Case I: (Aggregation + Piggy-Back)
+		// 1. user enable aggregation, AND
+		// 2. Mac support piggy-back
+		// 3. AP annouces it's PIGGY-BACK+AGGREGATION-capable in BEACON
+		// Case II: (Aggregation)
+		// 1. user enable aggregation, AND
+		// 2. AP annouces it's AGGREGATION-capable in BEACON
+		if (pAd->CommonCfg.bAggregationCapable)
+		{
+			if ((pAd->CommonCfg.bPiggyBackCapable) && ((pAd->MlmeAux.APRalinkIe & 0x00000003) == 3))
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x03, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+			else if (pAd->MlmeAux.APRalinkIe & 0x00000001)
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x01, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+		}
+		else
+		{
+			ULONG TmpLen;
+			UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x04, 0x00, 0x00, 0x00};
+			MakeOutgoingFrame(pOutBuffer+FrameLen,		 &TmpLen,
+							  9,						 RalinkIe,
+							  END_OF_ARGS);
+			FrameLen += TmpLen;
+		}
+#ifdef LEAP_SUPPORT
+		if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+		{
+			CkipFlag = pAd->StaCfg.CkipFlag;	// We have update that at PeerBeaconAtJoinRequest()
+			if (CkipFlag != 0)
+			{
+				NdisZeroMemory(CkipNegotiationBuffer, CKIP_NEGOTIATION_LENGTH);
+				CkipNegotiationBuffer[2] = 0x66;
+				// Make it try KP & MIC, since we have to follow the result from AssocRsp
+				CkipNegotiationBuffer[8] = 0x18;
+				CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH - 1] = 0x22;
+
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &tmp,
+									1,                              &AironetCkipIe,
+									1,                              &AironetCkipLen,
+									AironetCkipLen,                 CkipNegotiationBuffer,
+									END_OF_ARGS);
+				FrameLen += tmp;
+			}
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+							1,                              &AironetIPAddressIE,
+							1,                              &AironetIPAddressLen,
+							AironetIPAddressLen,            AironetIPAddressBuffer,
+							END_OF_ARGS);
+			FrameLen += tmp;
+
+			//
+			// The RN is incremented before each reassociation request.
+			//
+			pAd->StaCfg.CCKMRN++;
+			//
+			// Calculate MIC = hmac-md5(krk, STA-ID|BSSID|RSNIE|TSF|RN);
+			//
+			COPY_MAC_ADDR(CalcMicBuffer, pAd->CurrentAddress);
+			CalcMicBufferLen = MAC_ADDR_LEN;
+			COPY_MAC_ADDR(CalcMicBuffer + CalcMicBufferLen, pAd->MlmeAux.Bssid);
+			CalcMicBufferLen += MAC_ADDR_LEN;
+			NdisMoveMemory(CalcMicBuffer + CalcMicBufferLen, CipherSuiteCiscoCCKM, CipherSuiteCiscoCCKMLen);
+			CalcMicBufferLen += CipherSuiteCiscoCCKMLen;
+			NdisMoveMemory(CalcMicBuffer + CalcMicBufferLen, (PUCHAR) &pAd->StaCfg.CCKMBeaconAtJoinTimeStamp, sizeof(pAd->StaCfg.CCKMBeaconAtJoinTimeStamp));
+			CalcMicBufferLen += sizeof(pAd->StaCfg.CCKMBeaconAtJoinTimeStamp);
+			NdisMoveMemory(CalcMicBuffer + CalcMicBufferLen, (PUCHAR)&pAd->StaCfg.CCKMRN, sizeof(pAd->StaCfg.CCKMRN));
+			CalcMicBufferLen += sizeof(pAd->StaCfg.CCKMRN);
+			hmac_md5(pAd->StaCfg.KRK, LEN_EAP_MICK, CalcMicBuffer, CalcMicBufferLen, MICMN);
+
+			//
+			// fill up CCKM reassociation request element
+			//
+			NdisMoveMemory(AironetCCKMReassocBuffer, AironetOUI, 4);
+			NdisMoveMemory(AironetCCKMReassocBuffer + 4, (PUCHAR)&pAd->StaCfg.CCKMBeaconAtJoinTimeStamp, 8);
+			NdisMoveMemory(AironetCCKMReassocBuffer + 12, (PUCHAR) &pAd->StaCfg.CCKMRN, 4);
+			NdisMoveMemory(AironetCCKMReassocBuffer +16, MICMN, 8);
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+							1,                      &AironetCCKMReassocIE,
+							1,                      &AironetCCKMReassocLen,
+							AironetCCKMReassocLen,  AironetCCKMReassocBuffer,
+							END_OF_ARGS);
+			FrameLen += tmp;
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+							CipherSuiteCiscoCCKMLen,CipherSuiteCiscoCCKM,
+							END_OF_ARGS);
+			FrameLen += tmp;
+		}
+#endif // LEAP_SUPPORT //
+
+		// Add CCX v2 request if CCX2 admin state is on
+		if (pAd->StaCfg.CCXControl.field.Enable == 1)
+		{
+			//
+			// Add CCX Version
+			//
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+						1,							&Ccx2Ie,
+						1,							&Ccx2Len,
+						Ccx2Len,				    Ccx2IeInfo,
+						END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+
+		RTMPSetTimer(&pAd->MlmeAux.ReassocTimer, Timeout); /* in mSec */
+		pAd->Mlme.AssocMachine.CurrState = REASSOC_WAIT_RSP;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeReassocReqAction() sanity check failed. BUG!!!! \n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		Upper layer issues disassoc request
+	Parameters:
+		Elem -
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeDisassocReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PMLME_DISASSOC_REQ_STRUCT pDisassocReq;
+	HEADER_802_11         DisassocHdr;
+	PHEADER_802_11        pDisassocHdr;
+	PUCHAR                pOutBuffer = NULL;
+	ULONG                 FrameLen = 0;
+	NDIS_STATUS           NStatus;
+	BOOLEAN               TimerCancelled;
+	ULONG                 Timeout = 0;
+	USHORT                Status;
+
+#ifdef QOS_DLS_SUPPORT
+	// send DLS-TEAR_DOWN message,
+	if (pAd->CommonCfg.bDLSCapable)
+	{
+		UCHAR i;
+
+		// tear down local dls table entry
+		for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+			{
+				RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			}
+		}
+
+		// tear down peer dls table entry
+		for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+			{
+				RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			}
+		}
+	}
+#endif // QOS_DLS_SUPPORT //
+
+	// skip sanity check
+	pDisassocReq = (PMLME_DISASSOC_REQ_STRUCT)(Elem->Msg);
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - MlmeDisassocReqAction() allocate memory failed\n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_FAIL_NO_RESOURCE;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DISASSOC_CONF, 2, &Status);
+		return;
+	}
+
+
+
+	RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer, &TimerCancelled);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Send DISASSOC request[BSSID::%02x:%02x:%02x:%02x:%02x:%02x (Reason=%d)\n",
+				pDisassocReq->Addr[0], pDisassocReq->Addr[1], pDisassocReq->Addr[2],
+				pDisassocReq->Addr[3], pDisassocReq->Addr[4], pDisassocReq->Addr[5], pDisassocReq->Reason));
+	MgtMacHeaderInit(pAd, &DisassocHdr, SUBTYPE_DISASSOC, 0, pDisassocReq->Addr, pDisassocReq->Addr);	// patch peap ttls switching issue
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+					  sizeof(HEADER_802_11),&DisassocHdr,
+					  2,                    &pDisassocReq->Reason,
+					  END_OF_ARGS);
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	// To patch Instance and Buffalo(N) AP
+	// Driver has to send deauth to Instance AP, but Buffalo(N) needs to send disassoc to reset Authenticator's state machine
+	// Therefore, we send both of them.
+	pDisassocHdr = (PHEADER_802_11)pOutBuffer;
+	pDisassocHdr->FC.SubType = SUBTYPE_DEAUTH;
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	pAd->StaCfg.DisassocReason = REASON_DISASSOC_STA_LEAVING;
+	COPY_MAC_ADDR(pAd->StaCfg.DisassocSta, pDisassocReq->Addr);
+
+	RTMPSetTimer(&pAd->MlmeAux.DisassocTimer, Timeout); /* in mSec */
+	pAd->Mlme.AssocMachine.CurrState = DISASSOC_WAIT_RSP;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+    if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+	{
+        union iwreq_data    wrqu;
+        //send disassociate event to wpa_supplicant
+        memset(&wrqu, 0, sizeof(wrqu));
+        wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+    }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+    {
+        union iwreq_data    wrqu;
+        memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+        wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+    }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+}
+
+/*
+	==========================================================================
+	Description:
+		peer sends assoc rsp back
+	Parameters:
+		Elme - MLME message containing the received frame
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerAssocRspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT        CapabilityInfo, Status, Aid;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES], SupRateLen;
+	UCHAR         ExtRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRateLen;
+	UCHAR         Addr2[MAC_ADDR_LEN];
+	BOOLEAN       TimerCancelled;
+	UCHAR         CkipFlag;
+	EDCA_PARM     EdcaParm;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+	if (PeerAssocRspSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &CapabilityInfo, &Status, &Aid, SupRate, &SupRateLen, ExtRate, &ExtRateLen,
+		&HtCapability,&AddHtInfo, &HtCapabilityLen,&AddHtInfoLen,&NewExtChannelOffset, &EdcaParm, &CkipFlag))
+	{
+		// The frame is for me ?
+		if(MAC_ADDR_EQUAL(Addr2, pAd->MlmeAux.Bssid))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspAction():ASSOC - receive ASSOC_RSP to me (status=%d)\n", Status));
+#ifdef DOT11_N_SUPPORT
+			DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspAction():MacTable [%d].AMsduSize = %d. ClientStatusFlags = 0x%lx \n",Elem->Wcid, pAd->MacTab.Content[BSSID_WCID].AMsduSize, pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+#endif // DOT11_N_SUPPORT //
+			RTMPCancelTimer(&pAd->MlmeAux.AssocTimer, &TimerCancelled);
+			if(Status == MLME_SUCCESS)
+			{
+				// go to procedure listed on page 376
+				AssocPostProc(pAd, Addr2, CapabilityInfo, Aid, SupRate, SupRateLen, ExtRate, ExtRateLen,
+					&EdcaParm, &HtCapability, HtCapabilityLen, &AddHtInfo);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+                if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+                {
+                    union iwreq_data    wrqu;
+
+                    SendAssocIEsToWpaSupplicant(pAd);
+                    memset(&wrqu, 0, sizeof(wrqu));
+                    wrqu.data.flags = RT_ASSOC_EVENT_FLAG;
+                    wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+                }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+                {
+                    union iwreq_data    wrqu;
+                    wext_notify_event_assoc(pAd);
+
+                    memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                    memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+                    wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+                }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+
+				pAd->StaCfg.CkipFlag = CkipFlag;
+				if (CkipFlag & 0x18)
+				{
+					NdisZeroMemory(pAd->StaCfg.TxSEQ, 4);
+					NdisZeroMemory(pAd->StaCfg.RxSEQ, 4);
+					NdisZeroMemory(pAd->StaCfg.CKIPMIC, 4);
+					pAd->StaCfg.GIV[0] = RandomByte(pAd);
+					pAd->StaCfg.GIV[1] = RandomByte(pAd);
+					pAd->StaCfg.GIV[2] = RandomByte(pAd);
+					pAd->StaCfg.bCkipOn = TRUE;
+					DBGPRINT(RT_DEBUG_TRACE, ("<CCX> pAd->StaCfg.CkipFlag = 0x%02x\n", pAd->StaCfg.CkipFlag));
+				}
+			}
+			else
+			{
+				// Faile on Association, we need to check the status code
+				// Is that a Rogue AP?
+#ifdef LEAP_SUPPORT
+				if ((pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP) && (Status == MLME_ALG_NOT_SUPPORT))
+				{ //Possibly Rogue AP
+					RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, pAd->MlmeAux.Bssid, LEAP_REASON_INVALID_AUTH);
+				}
+#endif // LEAP_SUPPORT //
+			}
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerAssocRspAction() sanity check fail\n"));
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		peer sends reassoc rsp
+	Parametrs:
+		Elem - MLME message cntaining the received frame
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerReassocRspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      CapabilityInfo;
+	USHORT      Status;
+	USHORT      Aid;
+	UCHAR       SupRate[MAX_LEN_OF_SUPPORTED_RATES], SupRateLen;
+	UCHAR       ExtRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRateLen;
+	UCHAR       Addr2[MAC_ADDR_LEN];
+	UCHAR       CkipFlag;
+	BOOLEAN     TimerCancelled;
+	EDCA_PARM   EdcaParm;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+	if(PeerAssocRspSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &CapabilityInfo, &Status, &Aid, SupRate, &SupRateLen, ExtRate, &ExtRateLen,
+								&HtCapability,	&AddHtInfo, &HtCapabilityLen, &AddHtInfoLen,&NewExtChannelOffset, &EdcaParm, &CkipFlag))
+	{
+		if(MAC_ADDR_EQUAL(Addr2, pAd->MlmeAux.Bssid)) // The frame is for me ?
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - receive REASSOC_RSP to me (status=%d)\n", Status));
+			RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer, &TimerCancelled);
+
+			if(Status == MLME_SUCCESS)
+			{
+				// go to procedure listed on page 376
+				AssocPostProc(pAd, Addr2, CapabilityInfo, Aid, SupRate, SupRateLen, ExtRate, ExtRateLen,
+					 &EdcaParm, &HtCapability, HtCapabilityLen, &AddHtInfo);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+                if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+                {
+                    union iwreq_data    wrqu;
+
+                    SendAssocIEsToWpaSupplicant(pAd);
+                    memset(&wrqu, 0, sizeof(wrqu));
+                    wrqu.data.flags = RT_ASSOC_EVENT_FLAG;
+                    wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+                }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+                {
+                    union iwreq_data    wrqu;
+                    wext_notify_event_assoc(pAd);
+
+                    memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                    memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+                    wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+                }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+			}
+
+			//
+			// Cisco Leap CCKM supported Re-association.
+			//
+#ifdef LEAP_SUPPORT
+			if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+			{
+				if (CCKMAssocRspSanity(pAd, Elem->Msg, Elem->MsgLen) == TRUE)
+				{
+					pAd->StaCfg.CkipFlag = CkipFlag;
+					if (CkipFlag & 0x18)
+					{
+						NdisZeroMemory(pAd->StaCfg.TxSEQ, 4);
+						NdisZeroMemory(pAd->StaCfg.RxSEQ, 4);
+						NdisZeroMemory(pAd->StaCfg.CKIPMIC, 4);
+						pAd->StaCfg.GIV[0] = RandomByte(pAd);
+						pAd->StaCfg.GIV[1] = RandomByte(pAd);
+						pAd->StaCfg.GIV[2] = RandomByte(pAd);
+						pAd->StaCfg.bCkipOn = TRUE;
+						DBGPRINT(RT_DEBUG_TRACE, ("<CCX> pAd->StaCfg.CkipFlag = 0x%02x\n", pAd->StaCfg.CkipFlag));
+					}
+
+					pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+					MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - CCKMAssocRspSanity() sanity check fail\n"));
+				}
+			}
+			else
+#endif // LEAP_SUPPORT //
+			{
+				// CkipFlag is no use for reassociate
+				pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+				MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+			}
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerReassocRspAction() sanity check fail\n"));
+	}
+
+}
+
+/*
+	==========================================================================
+	Description:
+		procedures on IEEE 802.11/1999 p.376
+	Parametrs:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AssocPostProc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pAddr2,
+	IN USHORT CapabilityInfo,
+	IN USHORT Aid,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN PEDCA_PARM pEdcaParm,
+	IN HT_CAPABILITY_IE		*pHtCapability,
+	IN UCHAR HtCapabilityLen,
+	IN ADD_HT_INFO_IE		*pAddHtInfo)	// AP might use this additional ht info IE
+{
+	ULONG Idx;
+
+	pAd->MlmeAux.BssType = BSS_INFRA;
+	COPY_MAC_ADDR(pAd->MlmeAux.Bssid, pAddr2);
+	pAd->MlmeAux.Aid = Aid;
+	pAd->MlmeAux.CapabilityInfo = CapabilityInfo & SUPPORTED_CAPABILITY_INFO;
+#ifdef DOT11_N_SUPPORT
+	// Some HT AP might lost WMM IE. We add WMM ourselves. beacuase HT requires QoS on.
+	if ((HtCapabilityLen > 0) && (pEdcaParm->bValid == FALSE))
+	{
+		pEdcaParm->bValid = TRUE;
+		pEdcaParm->Aifsn[0] = 3;
+		pEdcaParm->Aifsn[1] = 7;
+		pEdcaParm->Aifsn[2] = 2;
+		pEdcaParm->Aifsn[3] = 2;
+
+		pEdcaParm->Cwmin[0] = 4;
+		pEdcaParm->Cwmin[1] = 4;
+		pEdcaParm->Cwmin[2] = 3;
+		pEdcaParm->Cwmin[3] = 2;
+
+		pEdcaParm->Cwmax[0] = 10;
+		pEdcaParm->Cwmax[1] = 10;
+		pEdcaParm->Cwmax[2] = 4;
+		pEdcaParm->Cwmax[3] = 3;
+
+		pEdcaParm->Txop[0]  = 0;
+		pEdcaParm->Txop[1]  = 0;
+		pEdcaParm->Txop[2]  = 96;
+		pEdcaParm->Txop[3]  = 48;
+
+	}
+#endif // DOT11_N_SUPPORT //
+
+	NdisMoveMemory(&pAd->MlmeAux.APEdcaParm, pEdcaParm, sizeof(EDCA_PARM));
+
+	// filter out un-supported rates
+	pAd->MlmeAux.SupRateLen = SupRateLen;
+	NdisMoveMemory(pAd->MlmeAux.SupRate, SupRate, SupRateLen);
+	RTMPCheckRates(pAd, pAd->MlmeAux.SupRate, &pAd->MlmeAux.SupRateLen);
+
+	// filter out un-supported rates
+	pAd->MlmeAux.ExtRateLen = ExtRateLen;
+	NdisMoveMemory(pAd->MlmeAux.ExtRate, ExtRate, ExtRateLen);
+	RTMPCheckRates(pAd, pAd->MlmeAux.ExtRate, &pAd->MlmeAux.ExtRateLen);
+
+#ifdef DOT11_N_SUPPORT
+	if (HtCapabilityLen > 0)
+	{
+		RTMPCheckHt(pAd, BSSID_WCID, pHtCapability, pAddHtInfo);
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===>  AP.AMsduSize = %d. ClientStatusFlags = 0x%lx \n", pAd->MacTab.Content[BSSID_WCID].AMsduSize, pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===>    (Mmps=%d, AmsduSize=%d, )\n",
+		pAd->MacTab.Content[BSSID_WCID].MmpsMode, pAd->MacTab.Content[BSSID_WCID].AMsduSize));
+#endif // DOT11_N_SUPPORT //
+
+	// Set New WPA information
+	Idx = BssTableSearch(&pAd->ScanTab, pAddr2, pAd->MlmeAux.Channel);
+	if (Idx == BSS_NOT_FOUND)
+	{
+		DBGPRINT_ERR(("ASSOC - Can't find BSS after receiving Assoc response\n"));
+	}
+	else
+	{
+		// Init variable
+		pAd->MacTab.Content[BSSID_WCID].RSNIE_Len = 0;
+		NdisZeroMemory(pAd->MacTab.Content[BSSID_WCID].RSN_IE, MAX_LEN_OF_RSNIE);
+
+		// Store appropriate RSN_IE for WPA SM negotiation later
+		if ((pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) && (pAd->ScanTab.BssEntry[Idx].VarIELen != 0))
+		{
+			PUCHAR              pVIE;
+			USHORT              len;
+			PEID_STRUCT         pEid;
+
+			pVIE = pAd->ScanTab.BssEntry[Idx].VarIEs;
+			len	 = pAd->ScanTab.BssEntry[Idx].VarIELen;
+
+			while (len > 0)
+			{
+				pEid = (PEID_STRUCT) pVIE;
+				// For WPA/WPAPSK
+				if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4))
+					&& (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+				{
+					NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].RSN_IE, pVIE, (pEid->Len + 2));
+					pAd->MacTab.Content[BSSID_WCID].RSNIE_Len = (pEid->Len + 2);
+					DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===> Store RSN_IE for WPA SM negotiation \n"));
+				}
+				// For WPA2/WPA2PSK
+				else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3))
+					&& (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2 || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+				{
+					NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].RSN_IE, pVIE, (pEid->Len + 2));
+					pAd->MacTab.Content[BSSID_WCID].RSNIE_Len = (pEid->Len + 2);
+					DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===> Store RSN_IE for WPA2 SM negotiation \n"));
+				}
+
+				pVIE += (pEid->Len + 2);
+				len  -= (pEid->Len + 2);
+			}
+		}
+
+		if (pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == 0)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===> no RSN_IE \n"));
+		}
+		else
+		{
+			hex_dump("RSN_IE", pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		left part of IEEE 802.11/1999 p.374
+	Parameters:
+		Elem - MLME message containing the received frame
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerDisassocAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Addr2[MAC_ADDR_LEN];
+	USHORT        Reason;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerDisassocAction()\n"));
+	if(PeerDisassocSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Reason))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerDisassocAction() Reason = %d\n", Reason));
+		if (INFRA_ON(pAd) && MAC_ADDR_EQUAL(pAd->CommonCfg.Bssid, Addr2))
+		{
+
+			if (pAd->CommonCfg.bWirelessEvent)
+			{
+				RTMPSendWirelessEvent(pAd, IW_DISASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+			}
+
+
+#ifdef LEAP_SUPPORT
+			if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+			{
+				// Cisco_LEAP has start a timer
+				// We should cancel it if using LEAP
+				RTMPCancelTimer(&pAd->StaCfg.LeapAuthTimer, &TimerCancelled);
+				//Check is it mach the LEAP Authentication failed as possible a Rogue AP
+				//on it's PortSecured not equal to WPA_802_1X_PORT_SECURED while process the Association.
+				if ((pAd->Mlme.LeapMachine.CurrState != LEAP_IDLE) && (pAd->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED))
+				{
+					RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, Addr2, LEAP_REASON_AUTH_TIMEOUT);
+				}
+			}
+#endif	// LEAP_SUPPORT //
+			//
+			// Get Current System time and Turn on AdjacentAPReport
+			//
+			NdisGetSystemUpTime(&pAd->StaCfg.CCXAdjacentAPLinkDownTime);
+			pAd->StaCfg.CCXAdjacentAPReportFlag = TRUE;
+			LinkDown(pAd, TRUE);
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+			{
+                union iwreq_data    wrqu;
+                //send disassociate event to wpa_supplicant
+                memset(&wrqu, 0, sizeof(wrqu));
+                wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+                wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerDisassocAction() sanity check fail\n"));
+	}
+
+}
+
+/*
+	==========================================================================
+	Description:
+		what the state machine will do after assoc timeout
+	Parameters:
+		Elme -
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AssocTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - AssocTimeoutAction\n"));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_REJ_TIMEOUT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		what the state machine will do after reassoc timeout
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID ReassocTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - ReassocTimeoutAction\n"));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_REJ_TIMEOUT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		what the state machine will do after disassoc timeout
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID DisassocTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - DisassocTimeoutAction\n"));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_SUCCESS;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DISASSOC_CONF, 2, &Status);
+}
+
+VOID InvalidStateWhenAssoc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - InvalidStateWhenAssoc(state=%ld), reset ASSOC state machine\n",
+		pAd->Mlme.AssocMachine.CurrState));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+}
+
+VOID InvalidStateWhenReassoc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - InvalidStateWhenReassoc(state=%ld), reset ASSOC state machine\n",
+		pAd->Mlme.AssocMachine.CurrState));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+}
+
+VOID InvalidStateWhenDisassociate(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - InvalidStateWhenDisassoc(state=%ld), reset ASSOC state machine\n",
+		pAd->Mlme.AssocMachine.CurrState));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DISASSOC_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		right part of IEEE 802.11/1999 page 374
+	Note:
+		This event should never cause ASSOC state machine perform state
+		transition, and has no relationship with CNTL machine. So we separate
+		this routine as a service outside of ASSOC state transition table.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID Cls3errAction(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr)
+{
+	HEADER_802_11         DisassocHdr;
+	PHEADER_802_11        pDisassocHdr;
+	PUCHAR                pOutBuffer = NULL;
+	ULONG                 FrameLen = 0;
+	NDIS_STATUS           NStatus;
+	USHORT                Reason = REASON_CLS3ERR;
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Class 3 Error, Send DISASSOC frame\n"));
+	MgtMacHeaderInit(pAd, &DisassocHdr, SUBTYPE_DISASSOC, 0, pAddr, pAd->CommonCfg.Bssid);	// patch peap ttls switching issue
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+					  sizeof(HEADER_802_11),&DisassocHdr,
+					  2,                    &Reason,
+					  END_OF_ARGS);
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	// To patch Instance and Buffalo(N) AP
+	// Driver has to send deauth to Instance AP, but Buffalo(N) needs to send disassoc to reset Authenticator's state machine
+	// Therefore, we send both of them.
+	pDisassocHdr = (PHEADER_802_11)pOutBuffer;
+	pDisassocHdr->FC.SubType = SUBTYPE_DEAUTH;
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	pAd->StaCfg.DisassocReason = REASON_CLS3ERR;
+	COPY_MAC_ADDR(pAd->StaCfg.DisassocSta, pAddr);
+}
+
+ /*
+	 ==========================================================================
+	 Description:
+		 Switch between WEP and CKIP upon new association up.
+	 Parameters:
+
+	 IRQL = DISPATCH_LEVEL
+
+	 ==========================================================================
+  */
+VOID SwitchBetweenWepAndCkip(
+	IN PRTMP_ADAPTER pAd)
+{
+	int            i;
+	SHAREDKEY_MODE_STRUC  csr1;
+
+	// if KP is required. change the CipherAlg in hardware shard key table from WEP
+	// to CKIP. else remain as WEP
+	if (pAd->StaCfg.bCkipOn && (pAd->StaCfg.CkipFlag & 0x10))
+	{
+		// modify hardware key table so that MAC use correct algorithm to decrypt RX
+		RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE, &csr1.word);
+		if (csr1.field.Bss0Key0CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key0CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_CKIP128;
+
+		if (csr1.field.Bss0Key1CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key1CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_CKIP128;
+
+		if (csr1.field.Bss0Key2CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key2CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_CKIP128;
+
+		if (csr1.field.Bss0Key3CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key3CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_CKIP128;
+		RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE, csr1.word);
+		DBGPRINT(RT_DEBUG_TRACE, ("SwitchBetweenWepAndCkip: modify BSS0 cipher to %s\n", CipherName[csr1.field.Bss0Key0CipherAlg]));
+
+		// modify software key table so that driver can specify correct algorithm in TXD upon TX
+		for (i=0; i<SHARE_KEY_NUM; i++)
+		{
+			if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_WEP64)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_CKIP64;
+			else if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_WEP128)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_CKIP128;
+		}
+	}
+
+	// else if KP NOT inused. change the CipherAlg in hardware shard key table from CKIP
+	// to WEP.
+	else
+	{
+		// modify hardware key table so that MAC use correct algorithm to decrypt RX
+		RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE, &csr1.word);
+		if (csr1.field.Bss0Key0CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key0CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_WEP128;
+
+		if (csr1.field.Bss0Key1CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key1CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_WEP128;
+
+		if (csr1.field.Bss0Key2CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key2CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_WEP128;
+
+		if (csr1.field.Bss0Key3CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key3CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_WEP128;
+
+		// modify software key table so that driver can specify correct algorithm in TXD upon TX
+		for (i=0; i<SHARE_KEY_NUM; i++)
+		{
+			if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_CKIP64)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_WEP64;
+			else if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_CKIP128)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_WEP128;
+		}
+
+		//
+		// On WPA-NONE, must update CipherAlg.
+		// Because the OID_802_11_WEP_STATUS was been set after OID_802_11_ADD_KEY
+		// and CipherAlg will be CIPHER_NONE by Windows ZeroConfig.
+		// So we need to update CipherAlg after connect.
+		//
+		if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		{
+			for (i = 0; i < SHARE_KEY_NUM; i++)
+			{
+				if (pAd->SharedKey[BSS0][i].KeyLen != 0)
+				{
+					if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+					{
+						pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_TKIP;
+					}
+					else if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+					{
+						pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_AES;
+					}
+				}
+				else
+				{
+					pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_NONE;
+				}
+			}
+
+			csr1.field.Bss0Key0CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+			csr1.field.Bss0Key1CipherAlg = pAd->SharedKey[BSS0][1].CipherAlg;
+			csr1.field.Bss0Key2CipherAlg = pAd->SharedKey[BSS0][2].CipherAlg;
+			csr1.field.Bss0Key3CipherAlg = pAd->SharedKey[BSS0][3].CipherAlg;
+		}
+		RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE, csr1.word);
+		DBGPRINT(RT_DEBUG_TRACE, ("SwitchBetweenWepAndCkip: modify BSS0 cipher to %s\n", CipherName[csr1.field.Bss0Key0CipherAlg]));
+	}
+}
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+VOID    SendAssocIEsToWpaSupplicant(
+    IN  PRTMP_ADAPTER pAd)
+{
+    union iwreq_data    wrqu;
+    unsigned char custom[IW_CUSTOM_MAX] = {0};
+
+    if ((pAd->StaCfg.ReqVarIELen + 17) <= IW_CUSTOM_MAX)
+    {
+        sprintf(custom, "ASSOCINFO_ReqIEs=");
+	    NdisMoveMemory(custom+17, pAd->StaCfg.ReqVarIEs, pAd->StaCfg.ReqVarIELen);
+	    memset(&wrqu, 0, sizeof(wrqu));
+        wrqu.data.length = pAd->StaCfg.ReqVarIELen + 17;
+        wrqu.data.flags = RT_REQIE_EVENT_FLAG;
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, custom);
+
+        memset(&wrqu, 0, sizeof(wrqu));
+        wrqu.data.flags = RT_ASSOCINFO_EVENT_FLAG;
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+    }
+    else
+        DBGPRINT(RT_DEBUG_TRACE, ("pAd->StaCfg.ReqVarIELen + 17 > MAX_CUSTOM_LEN\n"));
+
+    return;
+}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+int wext_notify_event_assoc(
+	IN  RTMP_ADAPTER *pAd)
+{
+    union iwreq_data    wrqu;
+    char custom[IW_CUSTOM_MAX] = {0};
+
+#if WIRELESS_EXT > 17
+    if (pAd->StaCfg.ReqVarIELen <= IW_CUSTOM_MAX)
+    {
+        wrqu.data.length = pAd->StaCfg.ReqVarIELen;
+        memcpy(custom, pAd->StaCfg.ReqVarIEs, pAd->StaCfg.ReqVarIELen);
+        wireless_send_event(pAd->net_dev, IWEVASSOCREQIE, &wrqu, custom);
+    }
+    else
+        DBGPRINT(RT_DEBUG_TRACE, ("pAd->StaCfg.ReqVarIELen > MAX_CUSTOM_LEN\n"));
+#else
+    if (((pAd->StaCfg.ReqVarIELen*2) + 17) <= IW_CUSTOM_MAX)
+    {
+        UCHAR   idx;
+        wrqu.data.length = (pAd->StaCfg.ReqVarIELen*2) + 17;
+        sprintf(custom, "ASSOCINFO(ReqIEs=");
+        for (idx=0; idx<pAd->StaCfg.ReqVarIELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAd->StaCfg.ReqVarIEs[idx]);
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, custom);
+    }
+    else
+        DBGPRINT(RT_DEBUG_TRACE, ("(pAd->StaCfg.ReqVarIELen*2) + 17 > MAX_CUSTOM_LEN\n"));
+#endif
+
+	return 0;
+
+}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
diff --git a/drivers/staging/rt2860/sta/auth.c b/drivers/staging/rt2860/sta/auth.c
new file mode 100644
index 000000000000..73fb8d6ea76f
--- /dev/null
+++ b/drivers/staging/rt2860/sta/auth.c
@@ -0,0 +1,474 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	auth.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John		2004-9-3		porting from RT2500
+*/
+#include "../rt_config.h"
+
+/*
+    ==========================================================================
+    Description:
+        authenticate state machine init, including state transition and timer init
+    Parameters:
+        Sm - pointer to the auth state machine
+    Note:
+        The state machine looks like this
+
+                        AUTH_REQ_IDLE           AUTH_WAIT_SEQ2                   AUTH_WAIT_SEQ4
+    MT2_MLME_AUTH_REQ   mlme_auth_req_action    invalid_state_when_auth          invalid_state_when_auth
+    MT2_PEER_AUTH_EVEN  drop                    peer_auth_even_at_seq2_action    peer_auth_even_at_seq4_action
+    MT2_AUTH_TIMEOUT    Drop                    auth_timeout_action              auth_timeout_action
+
+	IRQL = PASSIVE_LEVEL
+
+    ==========================================================================
+ */
+
+void AuthStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[])
+{
+    StateMachineInit(Sm, Trans, MAX_AUTH_STATE, MAX_AUTH_MSG, (STATE_MACHINE_FUNC)Drop, AUTH_REQ_IDLE, AUTH_MACHINE_BASE);
+
+    // the first column
+    StateMachineSetAction(Sm, AUTH_REQ_IDLE, MT2_MLME_AUTH_REQ, (STATE_MACHINE_FUNC)MlmeAuthReqAction);
+
+    // the second column
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ2, MT2_MLME_AUTH_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAuth);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ2, MT2_PEER_AUTH_EVEN, (STATE_MACHINE_FUNC)PeerAuthRspAtSeq2Action);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ2, MT2_AUTH_TIMEOUT, (STATE_MACHINE_FUNC)AuthTimeoutAction);
+
+    // the third column
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ4, MT2_MLME_AUTH_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAuth);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ4, MT2_PEER_AUTH_EVEN, (STATE_MACHINE_FUNC)PeerAuthRspAtSeq4Action);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ4, MT2_AUTH_TIMEOUT, (STATE_MACHINE_FUNC)AuthTimeoutAction);
+
+	RTMPInitTimer(pAd, &pAd->MlmeAux.AuthTimer, GET_TIMER_FUNCTION(AuthTimeout), pAd, FALSE);
+}
+
+/*
+    ==========================================================================
+    Description:
+        function to be executed at timer thread when auth timer expires
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID AuthTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+    RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+    DBGPRINT(RT_DEBUG_TRACE,("AUTH - AuthTimeout\n"));
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	// send a de-auth to reset AP's state machine (Patch AP-Dir635)
+	if (pAd->Mlme.AuthMachine.CurrState == AUTH_WAIT_SEQ2)
+		Cls2errAction(pAd, pAd->MlmeAux.Bssid);
+
+
+    MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_AUTH_TIMEOUT, 0, NULL);
+    RT28XX_MLME_HANDLER(pAd);
+}
+
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeAuthReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    UCHAR              Addr[6];
+    USHORT             Alg, Seq, Status;
+    ULONG              Timeout;
+    HEADER_802_11      AuthHdr;
+    BOOLEAN            TimerCancelled;
+    NDIS_STATUS        NStatus;
+    PUCHAR             pOutBuffer = NULL;
+    ULONG              FrameLen = 0;
+
+	// Block all authentication request durning WPA block period
+	if (pAd->StaCfg.bBlockAssoc == TRUE)
+	{
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Block Auth request durning WPA block period!\n"));
+        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+        Status = MLME_STATE_MACHINE_REJECT;
+        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+	}
+    else if(MlmeAuthReqSanity(pAd, Elem->Msg, Elem->MsgLen, Addr, &Timeout, &Alg))
+    {
+        // reset timer
+        RTMPCancelTimer(&pAd->MlmeAux.AuthTimer, &TimerCancelled);
+        COPY_MAC_ADDR(pAd->MlmeAux.Bssid, Addr);
+        pAd->MlmeAux.Alg  = Alg;
+        Seq = 1;
+        Status = MLME_SUCCESS;
+
+        NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+        if(NStatus != NDIS_STATUS_SUCCESS)
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("AUTH - MlmeAuthReqAction(Alg:%d) allocate memory failed\n", Alg));
+            pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+            Status = MLME_FAIL_NO_RESOURCE;
+            MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+            return;
+        }
+
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Send AUTH request seq#1 (Alg=%d)...\n", Alg));
+        MgtMacHeaderInit(pAd, &AuthHdr, SUBTYPE_AUTH, 0, Addr, pAd->MlmeAux.Bssid);
+        MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+                          sizeof(HEADER_802_11),&AuthHdr,
+                          2,                    &Alg,
+                          2,                    &Seq,
+                          2,                    &Status,
+                          END_OF_ARGS);
+        MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+    	MlmeFreeMemory(pAd, pOutBuffer);
+
+        RTMPSetTimer(&pAd->MlmeAux.AuthTimer, Timeout);
+        pAd->Mlme.AuthMachine.CurrState = AUTH_WAIT_SEQ2;
+    }
+    else
+    {
+        DBGPRINT_ERR(("AUTH - MlmeAuthReqAction() sanity check failed\n"));
+        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+        Status = MLME_INVALID_FORMAT;
+        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerAuthRspAtSeq2Action(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    UCHAR         Addr2[MAC_ADDR_LEN];
+    USHORT        Seq, Status, RemoteStatus, Alg;
+    UCHAR         ChlgText[CIPHER_TEXT_LEN];
+    UCHAR         CyperChlgText[CIPHER_TEXT_LEN + 8 + 8];
+    UCHAR         Element[2];
+    HEADER_802_11 AuthHdr;
+    BOOLEAN       TimerCancelled;
+    PUCHAR        pOutBuffer = NULL;
+    NDIS_STATUS   NStatus;
+    ULONG         FrameLen = 0;
+    USHORT        Status2;
+
+    if (PeerAuthSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Alg, &Seq, &Status, ChlgText))
+    {
+        if (MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, Addr2) && Seq == 2)
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Receive AUTH_RSP seq#2 to me (Alg=%d, Status=%d)\n", Alg, Status));
+            RTMPCancelTimer(&pAd->MlmeAux.AuthTimer, &TimerCancelled);
+
+            if (Status == MLME_SUCCESS)
+            {
+                // Authentication Mode "LEAP" has allow for CCX 1.X
+                if ((pAd->MlmeAux.Alg == Ndis802_11AuthModeOpen)
+#ifdef LEAP_SUPPORT
+					|| (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+				)
+                {
+                    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+#ifdef LEAP_SUPPORT
+                    pAd->Mlme.LeapMachine.CurrState = LEAP_IDLE;
+#endif // LEAP_SUPPORT //
+                    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+                }
+                else
+                {
+                    // 2. shared key, need to be challenged
+                    Seq++;
+                    RemoteStatus = MLME_SUCCESS;
+
+					// Get an unused nonpaged memory
+                    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+                    if(NStatus != NDIS_STATUS_SUCCESS)
+                    {
+                        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - PeerAuthRspAtSeq2Action() allocate memory fail\n"));
+                        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+                        Status2 = MLME_FAIL_NO_RESOURCE;
+                        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status2);
+                        return;
+                    }
+
+                    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Send AUTH request seq#3...\n"));
+                    MgtMacHeaderInit(pAd, &AuthHdr, SUBTYPE_AUTH, 0, Addr2, pAd->MlmeAux.Bssid);
+                    AuthHdr.FC.Wep = 1;
+                    // Encrypt challenge text & auth information
+                    RTMPInitWepEngine(
+                    	pAd,
+                    	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+                    	pAd->StaCfg.DefaultKeyId,
+                    	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen,
+                    	CyperChlgText);
+
+					Alg = cpu2le16(*(USHORT *)&Alg);
+					Seq = cpu2le16(*(USHORT *)&Seq);
+					RemoteStatus= cpu2le16(*(USHORT *)&RemoteStatus);
+
+					RTMPEncryptData(pAd, (PUCHAR) &Alg, CyperChlgText + 4, 2);
+					RTMPEncryptData(pAd, (PUCHAR) &Seq, CyperChlgText + 6, 2);
+					RTMPEncryptData(pAd, (PUCHAR) &RemoteStatus, CyperChlgText + 8, 2);
+					Element[0] = 16;
+					Element[1] = 128;
+					RTMPEncryptData(pAd, Element, CyperChlgText + 10, 2);
+					RTMPEncryptData(pAd, ChlgText, CyperChlgText + 12, 128);
+					RTMPSetICV(pAd, CyperChlgText + 140);
+                    MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+                                      sizeof(HEADER_802_11),    &AuthHdr,
+                                      CIPHER_TEXT_LEN + 16,     CyperChlgText,
+                                      END_OF_ARGS);
+                    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+                	MlmeFreeMemory(pAd, pOutBuffer);
+
+                    RTMPSetTimer(&pAd->MlmeAux.AuthTimer, AUTH_TIMEOUT);
+                    pAd->Mlme.AuthMachine.CurrState = AUTH_WAIT_SEQ4;
+                }
+            }
+            else
+            {
+#ifdef LEAP_SUPPORT
+                if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+                {
+                    //Invalid Authentication possible rogue AP
+                    //Add this Ap to Rogue AP.
+                    RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, Addr2, LEAP_REASON_INVALID_AUTH);
+				}
+#endif // LEAP_SUPPORT //
+                pAd->StaCfg.AuthFailReason = Status;
+                COPY_MAC_ADDR(pAd->StaCfg.AuthFailSta, Addr2);
+                pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+                MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+            }
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - PeerAuthSanity() sanity check fail\n"));
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerAuthRspAtSeq4Action(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    UCHAR         Addr2[MAC_ADDR_LEN];
+    USHORT        Alg, Seq, Status;
+    CHAR          ChlgText[CIPHER_TEXT_LEN];
+    BOOLEAN       TimerCancelled;
+
+    if(PeerAuthSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Alg, &Seq, &Status, ChlgText))
+    {
+        if(MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, Addr2) && Seq == 4)
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Receive AUTH_RSP seq#4 to me\n"));
+            RTMPCancelTimer(&pAd->MlmeAux.AuthTimer, &TimerCancelled);
+
+            if (Status != MLME_SUCCESS)
+            {
+                pAd->StaCfg.AuthFailReason = Status;
+                COPY_MAC_ADDR(pAd->StaCfg.AuthFailSta, Addr2);
+            }
+
+            pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+            MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - PeerAuthRspAtSeq4Action() sanity check fail\n"));
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDeauthReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    MLME_DEAUTH_REQ_STRUCT *pInfo;
+    HEADER_802_11 DeauthHdr;
+    PUCHAR        pOutBuffer = NULL;
+    NDIS_STATUS   NStatus;
+    ULONG         FrameLen = 0;
+    USHORT        Status;
+
+    pInfo = (MLME_DEAUTH_REQ_STRUCT *)Elem->Msg;
+
+    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+    if (NStatus != NDIS_STATUS_SUCCESS)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - MlmeDeauthReqAction() allocate memory fail\n"));
+        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+        Status = MLME_FAIL_NO_RESOURCE;
+        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DEAUTH_CONF, 2, &Status);
+        return;
+    }
+
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Send DE-AUTH request (Reason=%d)...\n", pInfo->Reason));
+    MgtMacHeaderInit(pAd, &DeauthHdr, SUBTYPE_DEAUTH, 0, pInfo->Addr, pAd->MlmeAux.Bssid);
+    MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+                      sizeof(HEADER_802_11),&DeauthHdr,
+                      2,                    &pInfo->Reason,
+                      END_OF_ARGS);
+    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+    pAd->StaCfg.DeauthReason = pInfo->Reason;
+    COPY_MAC_ADDR(pAd->StaCfg.DeauthSta, pInfo->Addr);
+    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+    Status = MLME_SUCCESS;
+    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DEAUTH_CONF, 2, &Status);
+
+	// send wireless event - for deauthentication
+	if (pAd->CommonCfg.bWirelessEvent)
+		RTMPSendWirelessEvent(pAd, IW_DEAUTH_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID AuthTimeoutAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    USHORT Status;
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - AuthTimeoutAction\n"));
+    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+    Status = MLME_REJ_TIMEOUT;
+    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID InvalidStateWhenAuth(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    USHORT Status;
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - InvalidStateWhenAuth (state=%ld), reset AUTH state machine\n", pAd->Mlme.AuthMachine.CurrState));
+    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+    Status = MLME_STATE_MACHINE_REJECT;
+    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+}
+
+/*
+    ==========================================================================
+    Description:
+        Some STA/AP
+    Note:
+        This action should never trigger AUTH state transition, therefore we
+        separate it from AUTH state machine, and make it as a standalone service
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID Cls2errAction(
+    IN PRTMP_ADAPTER pAd,
+    IN PUCHAR pAddr)
+{
+    HEADER_802_11 DeauthHdr;
+    PUCHAR        pOutBuffer = NULL;
+    NDIS_STATUS   NStatus;
+    ULONG         FrameLen = 0;
+    USHORT        Reason = REASON_CLS2ERR;
+
+    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+    if (NStatus != NDIS_STATUS_SUCCESS)
+        return;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Class 2 error, Send DEAUTH frame...\n"));
+    MgtMacHeaderInit(pAd, &DeauthHdr, SUBTYPE_DEAUTH, 0, pAddr, pAd->MlmeAux.Bssid);
+    MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+                      sizeof(HEADER_802_11),&DeauthHdr,
+                      2,                    &Reason,
+                      END_OF_ARGS);
+    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+    pAd->StaCfg.DeauthReason = Reason;
+    COPY_MAC_ADDR(pAd->StaCfg.DeauthSta, pAddr);
+}
+
+
diff --git a/drivers/staging/rt2860/sta/auth_rsp.c b/drivers/staging/rt2860/sta/auth_rsp.c
new file mode 100644
index 000000000000..f7aa4b99cf56
--- /dev/null
+++ b/drivers/staging/rt2860/sta/auth_rsp.c
@@ -0,0 +1,167 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	auth_rsp.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John		2004-10-1		copy from RT2560
+*/
+#include "../rt_config.h"
+
+/*
+    ==========================================================================
+    Description:
+        authentication state machine init procedure
+    Parameters:
+        Sm - the state machine
+
+	IRQL = PASSIVE_LEVEL
+
+    ==========================================================================
+ */
+VOID AuthRspStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN PSTATE_MACHINE Sm,
+    IN STATE_MACHINE_FUNC Trans[])
+{
+    StateMachineInit(Sm, Trans, MAX_AUTH_RSP_STATE, MAX_AUTH_RSP_MSG, (STATE_MACHINE_FUNC)Drop, AUTH_RSP_IDLE, AUTH_RSP_MACHINE_BASE);
+
+    // column 1
+    StateMachineSetAction(Sm, AUTH_RSP_IDLE, MT2_PEER_DEAUTH, (STATE_MACHINE_FUNC)PeerDeauthAction);
+
+    // column 2
+    StateMachineSetAction(Sm, AUTH_RSP_WAIT_CHAL, MT2_PEER_DEAUTH, (STATE_MACHINE_FUNC)PeerDeauthAction);
+
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+*/
+VOID PeerAuthSimpleRspGenAndSend(
+    IN PRTMP_ADAPTER pAd,
+    IN PHEADER_802_11 pHdr80211,
+    IN USHORT Alg,
+    IN USHORT Seq,
+    IN USHORT Reason,
+    IN USHORT Status)
+{
+    HEADER_802_11     AuthHdr;
+    ULONG             FrameLen = 0;
+    PUCHAR            pOutBuffer = NULL;
+    NDIS_STATUS       NStatus;
+
+    if (Reason != MLME_SUCCESS)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("Peer AUTH fail...\n"));
+        return;
+    }
+
+	//Get an unused nonpaged memory
+    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+    if (NStatus != NDIS_STATUS_SUCCESS)
+        return;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Send AUTH response (seq#2)...\n"));
+    MgtMacHeaderInit(pAd, &AuthHdr, SUBTYPE_AUTH, 0, pHdr80211->Addr2, pAd->MlmeAux.Bssid);
+    MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+                      sizeof(HEADER_802_11),    &AuthHdr,
+                      2,                        &Alg,
+                      2,                        &Seq,
+                      2,                        &Reason,
+                      END_OF_ARGS);
+    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+*/
+VOID PeerDeauthAction(
+    IN PRTMP_ADAPTER pAd,
+    IN PMLME_QUEUE_ELEM Elem)
+{
+    UCHAR       Addr2[MAC_ADDR_LEN];
+    USHORT      Reason;
+
+    if (PeerDeauthSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Reason))
+    {
+        if (INFRA_ON(pAd) && MAC_ADDR_EQUAL(Addr2, pAd->CommonCfg.Bssid))
+        {
+            DBGPRINT(RT_DEBUG_TRACE,("AUTH_RSP - receive DE-AUTH from our AP (Reason=%d)\n", Reason));
+
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+
+			// send wireless event - for deauthentication
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_DEAUTH_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+            LinkDown(pAd, TRUE);
+
+            // Authentication Mode Cisco_LEAP has start a timer
+            // We should cancel it if using LEAP
+#ifdef LEAP_SUPPORT
+            if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+            {
+                RTMPCancelTimer(&pAd->StaCfg.LeapAuthTimer, &TimerCancelled);
+                //Check is it mach the LEAP Authentication failed as possible a Rogue AP
+                //on it's PortSecured not equal to WPA_802_1X_PORT_SECURED while process the Authenticaton.
+                if ((pAd->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED) && (pAd->Mlme.LeapMachine.CurrState != LEAP_IDLE))
+                {
+                    RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, Addr2, LEAP_REASON_AUTH_TIMEOUT);
+                }
+            }
+#endif // LEAP_SUPPORT //
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE,("AUTH_RSP - PeerDeauthAction() sanity check fail\n"));
+    }
+}
+
diff --git a/drivers/staging/rt2860/sta/connect.c b/drivers/staging/rt2860/sta/connect.c
new file mode 100644
index 000000000000..36f28f8b4aa4
--- /dev/null
+++ b/drivers/staging/rt2860/sta/connect.c
@@ -0,0 +1,2751 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	connect.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John			2004-08-08			Major modification from RT2560
+*/
+#include "../rt_config.h"
+
+UCHAR	CipherSuiteWpaNoneTkip[] = {
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x00	// authentication
+		};
+UCHAR	CipherSuiteWpaNoneTkipLen = (sizeof(CipherSuiteWpaNoneTkip) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteWpaNoneAes[] = {
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x04,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x04,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x00	// authentication
+		};
+UCHAR	CipherSuiteWpaNoneAesLen = (sizeof(CipherSuiteWpaNoneAes) / sizeof(UCHAR));
+
+// The following MACRO is called after 1. starting an new IBSS, 2. succesfully JOIN an IBSS,
+// or 3. succesfully ASSOCIATE to a BSS, 4. successfully RE_ASSOCIATE to a BSS
+// All settings successfuly negotiated furing MLME state machines become final settings
+// and are copied to pAd->StaActive
+#define COPY_SETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(_pAd)                                 \
+{                                                                                       \
+	(_pAd)->CommonCfg.SsidLen = (_pAd)->MlmeAux.SsidLen;                                \
+	NdisMoveMemory((_pAd)->CommonCfg.Ssid, (_pAd)->MlmeAux.Ssid, (_pAd)->MlmeAux.SsidLen); \
+	COPY_MAC_ADDR((_pAd)->CommonCfg.Bssid, (_pAd)->MlmeAux.Bssid);                      \
+	(_pAd)->CommonCfg.Channel = (_pAd)->MlmeAux.Channel;                                \
+	(_pAd)->CommonCfg.CentralChannel = (_pAd)->MlmeAux.CentralChannel;                  \
+	(_pAd)->StaActive.Aid = (_pAd)->MlmeAux.Aid;                                        \
+	(_pAd)->StaActive.AtimWin = (_pAd)->MlmeAux.AtimWin;                                \
+	(_pAd)->StaActive.CapabilityInfo = (_pAd)->MlmeAux.CapabilityInfo;                  \
+	(_pAd)->CommonCfg.BeaconPeriod = (_pAd)->MlmeAux.BeaconPeriod;                      \
+	(_pAd)->StaActive.CfpMaxDuration = (_pAd)->MlmeAux.CfpMaxDuration;                  \
+	(_pAd)->StaActive.CfpPeriod = (_pAd)->MlmeAux.CfpPeriod;                            \
+	(_pAd)->StaActive.SupRateLen = (_pAd)->MlmeAux.SupRateLen;                          \
+	NdisMoveMemory((_pAd)->StaActive.SupRate, (_pAd)->MlmeAux.SupRate, (_pAd)->MlmeAux.SupRateLen);\
+	(_pAd)->StaActive.ExtRateLen = (_pAd)->MlmeAux.ExtRateLen;                          \
+	NdisMoveMemory((_pAd)->StaActive.ExtRate, (_pAd)->MlmeAux.ExtRate, (_pAd)->MlmeAux.ExtRateLen);\
+	NdisMoveMemory(&(_pAd)->CommonCfg.APEdcaParm, &(_pAd)->MlmeAux.APEdcaParm, sizeof(EDCA_PARM));\
+	NdisMoveMemory(&(_pAd)->CommonCfg.APQosCapability, &(_pAd)->MlmeAux.APQosCapability, sizeof(QOS_CAPABILITY_PARM));\
+	NdisMoveMemory(&(_pAd)->CommonCfg.APQbssLoad, &(_pAd)->MlmeAux.APQbssLoad, sizeof(QBSS_LOAD_PARM));\
+	COPY_MAC_ADDR((_pAd)->MacTab.Content[BSSID_WCID].Addr, (_pAd)->MlmeAux.Bssid);      \
+	(_pAd)->MacTab.Content[BSSID_WCID].Aid = (_pAd)->MlmeAux.Aid;                       \
+	(_pAd)->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = (_pAd)->StaCfg.PairCipher;\
+	COPY_MAC_ADDR((_pAd)->MacTab.Content[BSSID_WCID].PairwiseKey.BssId, (_pAd)->MlmeAux.Bssid);\
+	(_pAd)->MacTab.Content[BSSID_WCID].RateLen = (_pAd)->StaActive.SupRateLen + (_pAd)->StaActive.ExtRateLen;\
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+*/
+VOID MlmeCntlInit(
+	IN PRTMP_ADAPTER pAd,
+	IN STATE_MACHINE *S,
+	OUT STATE_MACHINE_FUNC Trans[])
+{
+	// Control state machine differs from other state machines, the interface
+	// follows the standard interface
+	pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID MlmeCntlMachinePerformAction(
+	IN PRTMP_ADAPTER pAd,
+	IN STATE_MACHINE *S,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	switch(pAd->Mlme.CntlMachine.CurrState)
+	{
+		case CNTL_IDLE:
+			{
+				CntlIdleProc(pAd, Elem);
+			}
+			break;
+		case CNTL_WAIT_DISASSOC:
+			CntlWaitDisassocProc(pAd, Elem);
+			break;
+		case CNTL_WAIT_JOIN:
+			CntlWaitJoinProc(pAd, Elem);
+			break;
+
+		// CNTL_WAIT_REASSOC is the only state in CNTL machine that does
+		// not triggered directly or indirectly by "RTMPSetInformation(OID_xxx)".
+		// Therefore not protected by NDIS's "only one outstanding OID request"
+		// rule. Which means NDIS may SET OID in the middle of ROAMing attempts.
+		// Current approach is to block new SET request at RTMPSetInformation()
+		// when CntlMachine.CurrState is not CNTL_IDLE
+		case CNTL_WAIT_REASSOC:
+			CntlWaitReassocProc(pAd, Elem);
+			break;
+
+		case CNTL_WAIT_START:
+			CntlWaitStartProc(pAd, Elem);
+			break;
+		case CNTL_WAIT_AUTH:
+			CntlWaitAuthProc(pAd, Elem);
+			break;
+		case CNTL_WAIT_AUTH2:
+			CntlWaitAuthProc2(pAd, Elem);
+			break;
+		case CNTL_WAIT_ASSOC:
+			CntlWaitAssocProc(pAd, Elem);
+			break;
+
+		case CNTL_WAIT_OID_LIST_SCAN:
+			if(Elem->MsgType == MT2_SCAN_CONF)
+			{
+				// Resume TxRing after SCANING complete. We hope the out-of-service time
+				// won't be too long to let upper layer time-out the waiting frames
+				RTMPResumeMsduTransmission(pAd);
+				if (pAd->StaCfg.CCXReqType != MSRN_TYPE_UNUSED)
+				{
+					// Cisco scan request is finished, prepare beacon report
+					MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_DONE, 0, NULL);
+				}
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+
+                //
+				// Set LED status to previous status.
+				//
+				if (pAd->bLedOnScanning)
+				{
+					pAd->bLedOnScanning = FALSE;
+					RTMPSetLED(pAd, pAd->LedStatus);
+				}
+#ifdef DOT11N_DRAFT3
+				// AP sent a 2040Coexistence mgmt frame, then station perform a scan, and then send back the respone.
+				if (pAd->CommonCfg.BSSCoexist2040.field.InfoReq == 1)
+				{
+					Update2040CoexistFrameAndNotify(pAd, BSSID_WCID, TRUE);
+				}
+#endif // DOT11N_DRAFT3 //
+			}
+			break;
+
+		case CNTL_WAIT_OID_DISASSOC:
+			if (Elem->MsgType == MT2_DISASSOC_CONF)
+			{
+				LinkDown(pAd, FALSE);
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			}
+			break;
+		default:
+			DBGPRINT_ERR(("!ERROR! CNTL - Illegal message type(=%ld)", Elem->MsgType));
+			break;
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlIdleProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_DISASSOC_REQ_STRUCT   DisassocReq;
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+		return;
+
+	switch(Elem->MsgType)
+	{
+		case OID_802_11_SSID:
+			CntlOidSsidProc(pAd, Elem);
+			break;
+
+		case OID_802_11_BSSID:
+			CntlOidRTBssidProc(pAd,Elem);
+			break;
+
+		case OID_802_11_BSSID_LIST_SCAN:
+			CntlOidScanProc(pAd,Elem);
+			break;
+
+		case OID_802_11_DISASSOCIATE:
+#ifdef RALINK_ATE
+			if(ATE_ON(pAd))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_ENABLE_WITH_WEB_UI)
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+    			// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+    			// Since calling this indicate user don't want to connect to that SSID anymore.
+    			pAd->MlmeAux.AutoReconnectSsidLen= 32;
+    			NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen);
+            }
+			break;
+
+		case MT2_MLME_ROAMING_REQ:
+			CntlMlmeRoamingProc(pAd, Elem);
+			break;
+
+        case OID_802_11_MIC_FAILURE_REPORT_FRAME:
+            WpaMicFailureReportFrame(pAd, Elem);
+            break;
+
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_SET_DLS_PARAM:
+			CntlOidDLSSetupProc(pAd, Elem);
+			break;
+#endif // QOS_DLS_SUPPORT //
+
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Illegal message in CntlIdleProc(MsgType=%ld)\n",Elem->MsgType));
+			break;
+	}
+}
+
+VOID CntlOidScanProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_SCAN_REQ_STRUCT       ScanReq;
+	ULONG                      BssIdx = BSS_NOT_FOUND;
+	BSS_ENTRY                  CurrBss;
+
+#ifdef RALINK_ATE
+/* Disable scanning when ATE is running. */
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+
+	// record current BSS if network is connected.
+	// 2003-2-13 do not include current IBSS if this is the only STA in this IBSS.
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		BssIdx = BssSsidTableSearch(&pAd->ScanTab, pAd->CommonCfg.Bssid, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen, pAd->CommonCfg.Channel);
+		if (BssIdx != BSS_NOT_FOUND)
+		{
+			NdisMoveMemory(&CurrBss, &pAd->ScanTab.BssEntry[BssIdx], sizeof(BSS_ENTRY));
+		}
+	}
+
+	// clean up previous SCAN result, add current BSS back to table if any
+	BssTableInit(&pAd->ScanTab);
+	if (BssIdx != BSS_NOT_FOUND)
+	{
+		// DDK Note: If the NIC is associated with a particular BSSID and SSID
+		//    that are not contained in the list of BSSIDs generated by this scan, the
+		//    BSSID description of the currently associated BSSID and SSID should be
+		//    appended to the list of BSSIDs in the NIC's database.
+		// To ensure this, we append this BSS as the first entry in SCAN result
+		NdisMoveMemory(&pAd->ScanTab.BssEntry[0], &CurrBss, sizeof(BSS_ENTRY));
+		pAd->ScanTab.BssNr = 1;
+	}
+
+	ScanParmFill(pAd, &ScanReq, "", 0, BSS_ANY, SCAN_ACTIVE);
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ,
+		sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+}
+
+/*
+	==========================================================================
+	Description:
+		Before calling this routine, user desired SSID should already been
+		recorded in CommonCfg.Ssid[]
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlOidSsidProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM * Elem)
+{
+	PNDIS_802_11_SSID          pOidSsid = (NDIS_802_11_SSID *)Elem->Msg;
+	MLME_DISASSOC_REQ_STRUCT   DisassocReq;
+	ULONG					   Now;
+
+	// Step 1. record the desired user settings to MlmeAux
+	NdisZeroMemory(pAd->MlmeAux.Ssid, MAX_LEN_OF_SSID);
+	NdisMoveMemory(pAd->MlmeAux.Ssid, pOidSsid->Ssid, pOidSsid->SsidLength);
+	pAd->MlmeAux.SsidLen = (UCHAR)pOidSsid->SsidLength;
+	NdisZeroMemory(pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+	pAd->MlmeAux.BssType = pAd->StaCfg.BssType;
+
+
+	//
+	// Update Reconnect Ssid, that user desired to connect.
+	//
+	NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, MAX_LEN_OF_SSID);
+	NdisMoveMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+	pAd->MlmeAux.AutoReconnectSsidLen = pAd->MlmeAux.SsidLen;
+
+	// step 2. find all matching BSS in the lastest SCAN result (inBssTab)
+	//    & log them into MlmeAux.SsidBssTab for later-on iteration. Sort by RSSI order
+	BssTableSsidSort(pAd, &pAd->MlmeAux.SsidBssTab, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - %d BSS of %d BSS match the desire (%d)SSID - %s\n",
+			pAd->MlmeAux.SsidBssTab.BssNr, pAd->ScanTab.BssNr, pAd->MlmeAux.SsidLen, pAd->MlmeAux.Ssid));
+	NdisGetSystemUpTime(&Now);
+
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) &&
+		(pAd->CommonCfg.SsidLen == pAd->MlmeAux.SsidBssTab.BssEntry[0].SsidLen) &&
+		NdisEqualMemory(pAd->CommonCfg.Ssid, pAd->MlmeAux.SsidBssTab.BssEntry[0].Ssid, pAd->CommonCfg.SsidLen) &&
+		MAC_ADDR_EQUAL(pAd->CommonCfg.Bssid, pAd->MlmeAux.SsidBssTab.BssEntry[0].Bssid))
+	{
+		// Case 1. already connected with an AP who has the desired SSID
+		//         with highest RSSI
+
+		// Add checking Mode "LEAP" for CCX 1.0
+		if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+			 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+			 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+			 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+#ifdef LEAP_SUPPORT
+			 || (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+			 ) &&
+			(pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+		{
+			// case 1.1 For WPA, WPA-PSK, if the 1x port is not secured, we have to redo
+			//          connection process
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - disassociate with current AP...\n"));
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+						sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+		}
+		else if (pAd->bConfigChanged == TRUE)
+		{
+			// case 1.2 Important Config has changed, we have to reconnect to the same AP
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - disassociate with current AP Because config changed...\n"));
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+						sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+		}
+		else
+		{
+			// case 1.3. already connected to the SSID with highest RSSI.
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - already with this BSSID. ignore this SET_SSID request\n"));
+			//
+			// (HCT 12.1) 1c_wlan_mediaevents required
+			// media connect events are indicated when associating with the same AP
+			//
+			if (INFRA_ON(pAd))
+			{
+				//
+				// Since MediaState already is NdisMediaStateConnected
+				// We just indicate the connect event again to meet the WHQL required.
+				//
+				pAd->IndicateMediaState = NdisMediaStateConnected;
+				RTMP_IndicateMediaState(pAd);
+                pAd->ExtraInfo = GENERAL_LINK_UP;   // Update extra information to link is up
+			}
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+		}
+	}
+	else if (INFRA_ON(pAd))
+	{
+		//
+		// For RT61
+		// [88888] OID_802_11_SSID should have returned NDTEST_WEP_AP2(Returned: )
+		// RT61 may lost SSID, and not connect to NDTEST_WEP_AP2 and will connect to NDTEST_WEP_AP2 by Autoreconnect
+		// But media status is connected, so the SSID not report correctly.
+		//
+		if (!SSID_EQUAL(pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen))
+		{
+			//
+			// Different SSID means not Roaming case, so we let LinkDown() to Indicate a disconnect event.
+			//
+			pAd->MlmeAux.CurrReqIsFromNdis = TRUE;
+		}
+		// case 2. active INFRA association existent
+		//    roaming is done within miniport driver, nothing to do with configuration
+		//    utility. so upon a new SET(OID_802_11_SSID) is received, we just
+		//    disassociate with the current associated AP,
+		//    then perform a new association with this new SSID, no matter the
+		//    new/old SSID are the same or not.
+		DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - disassociate with current AP...\n"));
+		DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+		MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+					sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+	}
+	else
+	{
+		if (ADHOC_ON(pAd))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - drop current ADHOC\n"));
+			LinkDown(pAd, FALSE);
+			OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+			pAd->IndicateMediaState = NdisMediaStateDisconnected;
+			RTMP_IndicateMediaState(pAd);
+            pAd->ExtraInfo = GENERAL_LINK_DOWN;
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():NDIS_STATUS_MEDIA_DISCONNECT Event C!\n"));
+		}
+
+		if ((pAd->MlmeAux.SsidBssTab.BssNr == 0) &&
+			(pAd->StaCfg.bAutoReconnect == TRUE) &&
+			(pAd->MlmeAux.BssType == BSS_INFRA) &&
+			(MlmeValidateSSID(pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen) == TRUE)
+			)
+		{
+			MLME_SCAN_REQ_STRUCT       ScanReq;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - No matching BSS, start a new scan\n"));
+			ScanParmFill(pAd, &ScanReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen, BSS_ANY, SCAN_ACTIVE);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+			// Reset Missed scan number
+			pAd->StaCfg.LastScanTime = Now;
+		}
+		else
+		{
+			pAd->MlmeAux.BssIdx = 0;
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlOidRTBssidProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM * Elem)
+{
+	ULONG       BssIdx;
+	PUCHAR      pOidBssid = (PUCHAR)Elem->Msg;
+	MLME_DISASSOC_REQ_STRUCT    DisassocReq;
+	MLME_JOIN_REQ_STRUCT        JoinReq;
+
+#ifdef RALINK_ATE
+/* No need to perform this routine when ATE is running. */
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	// record user desired settings
+	COPY_MAC_ADDR(pAd->MlmeAux.Bssid, pOidBssid);
+	pAd->MlmeAux.BssType = pAd->StaCfg.BssType;
+
+	//
+	// Update Reconnect Ssid, that user desired to connect.
+	//
+	NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, MAX_LEN_OF_SSID);
+	pAd->MlmeAux.AutoReconnectSsidLen = pAd->MlmeAux.SsidLen;
+	NdisMoveMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+
+	// find the desired BSS in the latest SCAN result table
+	BssIdx = BssTableSearch(&pAd->ScanTab, pOidBssid, pAd->MlmeAux.Channel);
+	if (BssIdx == BSS_NOT_FOUND)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - BSSID not found. reply NDIS_STATUS_NOT_ACCEPTED\n"));
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+		return;
+	}
+
+	// copy the matched BSS entry from ScanTab to MlmeAux.SsidBssTab. Why?
+	// Because we need this entry to become the JOIN target in later on SYNC state machine
+	pAd->MlmeAux.BssIdx = 0;
+	pAd->MlmeAux.SsidBssTab.BssNr = 1;
+	NdisMoveMemory(&pAd->MlmeAux.SsidBssTab.BssEntry[0], &pAd->ScanTab.BssEntry[BssIdx], sizeof(BSS_ENTRY));
+
+	// 2002-11-26 skip the following checking. i.e. if user wants to re-connect to same AP
+	//   we just follow normal procedure. The reason of user doing this may because he/she changed
+	//   AP to another channel, but we still received BEACON from it thus don't claim Link Down.
+	//   Since user knows he's changed AP channel, he'll re-connect again. By skipping the following
+	//   checking, we'll disassociate then re-do normal association with this AP at the new channel.
+	// 2003-1-6 Re-enable this feature based on microsoft requirement which prefer not to re-do
+	//   connection when setting the same BSSID.
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) &&
+		MAC_ADDR_EQUAL(pAd->CommonCfg.Bssid, pOidBssid))
+	{
+		// already connected to the same BSSID, go back to idle state directly
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - already in this BSSID. ignore this SET_BSSID request\n"));
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+	}
+	else
+	{
+		if (INFRA_ON(pAd))
+		{
+			// disassoc from current AP first
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - disassociate with current AP ...\n"));
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+						sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+		}
+		else
+		{
+			if (ADHOC_ON(pAd))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - drop current ADHOC\n"));
+				LinkDown(pAd, FALSE);
+				OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+				pAd->IndicateMediaState = NdisMediaStateDisconnected;
+				RTMP_IndicateMediaState(pAd);
+                pAd->ExtraInfo = GENERAL_LINK_DOWN;
+				DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event C!\n"));
+			}
+
+			// Change the wepstatus to original wepstatus
+			pAd->StaCfg.WepStatus   = pAd->StaCfg.OrigWepStatus;
+			pAd->StaCfg.PairCipher  = pAd->StaCfg.OrigWepStatus;
+			pAd->StaCfg.GroupCipher = pAd->StaCfg.OrigWepStatus;
+
+			// Check cipher suite, AP must have more secured cipher than station setting
+			// Set the Pairwise and Group cipher to match the intended AP setting
+			// We can only connect to AP with less secured cipher setting
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+			{
+				pAd->StaCfg.GroupCipher = pAd->ScanTab.BssEntry[BssIdx].WPA.GroupCipher;
+
+				if (pAd->StaCfg.WepStatus == pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipher)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipher;
+				else if (pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipherAux != Ndis802_11WEPDisabled)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipherAux;
+				else	// There is no PairCipher Aux, downgrade our capability to TKIP
+					pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+			}
+			else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+			{
+				pAd->StaCfg.GroupCipher = pAd->ScanTab.BssEntry[BssIdx].WPA2.GroupCipher;
+
+				if (pAd->StaCfg.WepStatus == pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipher)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipher;
+				else if (pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipherAux != Ndis802_11WEPDisabled)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipherAux;
+				else	// There is no PairCipher Aux, downgrade our capability to TKIP
+					pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+
+				// RSN capability
+				pAd->StaCfg.RsnCapability = pAd->ScanTab.BssEntry[BssIdx].WPA2.RsnCapability;
+			}
+
+			// Set Mix cipher flag
+			pAd->StaCfg.bMixCipher = (pAd->StaCfg.PairCipher == pAd->StaCfg.GroupCipher) ? FALSE : TRUE;
+			if (pAd->StaCfg.bMixCipher == TRUE)
+			{
+				// If mix cipher, re-build RSNIE
+				RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, 0);
+			}
+			// No active association, join the BSS immediately
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - joining %02x:%02x:%02x:%02x:%02x:%02x ...\n",
+				pOidBssid[0],pOidBssid[1],pOidBssid[2],pOidBssid[3],pOidBssid[4],pOidBssid[5]));
+
+			JoinParmFill(pAd, &JoinReq, pAd->MlmeAux.BssIdx);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_JOIN_REQ, sizeof(MLME_JOIN_REQ_STRUCT), &JoinReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_JOIN;
+		}
+	}
+}
+
+// Roaming is the only external request triggering CNTL state machine
+// despite of other "SET OID" operation. All "SET OID" related oerations
+// happen in sequence, because no other SET OID will be sent to this device
+// until the the previous SET operation is complete (successful o failed).
+// So, how do we quarantee this ROAMING request won't corrupt other "SET OID"?
+// or been corrupted by other "SET OID"?
+//
+// IRQL = DISPATCH_LEVEL
+VOID CntlMlmeRoamingProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	// TODO:
+	// AP in different channel may show lower RSSI than actual value??
+	// should we add a weighting factor to compensate it?
+	DBGPRINT(RT_DEBUG_TRACE,("CNTL - Roaming in MlmeAux.RoamTab...\n"));
+
+	NdisMoveMemory(&pAd->MlmeAux.SsidBssTab, &pAd->MlmeAux.RoamTab, sizeof(pAd->MlmeAux.RoamTab));
+	pAd->MlmeAux.SsidBssTab.BssNr = pAd->MlmeAux.RoamTab.BssNr;
+
+	BssTableSortByRssi(&pAd->MlmeAux.SsidBssTab);
+	pAd->MlmeAux.BssIdx = 0;
+	IterateOnBssTab(pAd);
+}
+
+#ifdef QOS_DLS_SUPPORT
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlOidDLSSetupProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PRT_802_11_DLS		pDLS = (PRT_802_11_DLS)Elem->Msg;
+	MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+	INT					i;
+	USHORT				reason = REASON_UNSPECIFY;
+
+	DBGPRINT(RT_DEBUG_TRACE,("CNTL - (OID set %02x:%02x:%02x:%02x:%02x:%02x with Valid=%d, Status=%d, TimeOut=%d, CountDownTimer=%d)\n",
+		pDLS->MacAddr[0], pDLS->MacAddr[1], pDLS->MacAddr[2], pDLS->MacAddr[3], pDLS->MacAddr[4], pDLS->MacAddr[5],
+		pDLS->Valid, pDLS->Status, pDLS->TimeOut, pDLS->CountDownTimer));
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return;
+
+	// DLS will not be supported when Adhoc mode
+	if (INFRA_ON(pAd))
+	{
+		for (i = 0; i < MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			if (pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				(pDLS->TimeOut == pAd->StaCfg.DLSEntry[i].TimeOut) && MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 1. Same setting, just drop it
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - setting unchanged\n"));
+				break;
+			}
+			else if (!pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 2. Disable DLS link case, just tear down DLS link
+				reason = REASON_QOS_UNWANTED_MECHANISM;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - start tear down procedure\n"));
+				break;
+			}
+			else if ((i < MAX_NUM_OF_DLS_ENTRY) && pDLS->Valid && !pAd->StaCfg.DLSEntry[i].Valid)
+			{
+				// 3. Enable case, start DLS setup procedure
+				NdisMoveMemory(&pAd->StaCfg.DLSEntry[i], pDLS, sizeof(RT_802_11_DLS_UI));
+
+				//Update countdown timer
+				pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS setup case\n"));
+				break;
+			}
+			else if ((i < MAX_NUM_OF_DLS_ENTRY) && pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid &&
+				(pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) && !MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 4. update mac case, tear down old DLS and setup new DLS
+				reason = REASON_QOS_UNWANTED_MECHANISM;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				NdisMoveMemory(&pAd->StaCfg.DLSEntry[i], pDLS, sizeof(RT_802_11_DLS_UI));
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS tear down and restart case\n"));
+				break;
+			}
+			else if (pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid &&
+				MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr) && (pAd->StaCfg.DLSEntry[i].TimeOut != pDLS->TimeOut))
+			{
+				// 5. update timeout case, start DLS setup procedure (no tear down)
+				pAd->StaCfg.DLSEntry[i].TimeOut	= pDLS->TimeOut;
+				//Update countdown timer
+				pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS update timeout case\n"));
+				break;
+			}
+			else if (pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid &&
+				(pAd->StaCfg.DLSEntry[i].Status != DLS_FINISH) && MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 6. re-setup case, start DLS setup procedure (no tear down)
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS retry setup procedure\n"));
+				break;
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN,("CNTL - DLS not changed in entry - %d - Valid=%d, Status=%d, TimeOut=%d\n",
+					i, pAd->StaCfg.DLSEntry[i].Valid, pAd->StaCfg.DLSEntry[i].Status, pAd->StaCfg.DLSEntry[i].TimeOut));
+			}
+		}
+	}
+}
+#endif // QOS_DLS_SUPPORT //
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitDisassocProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_START_REQ_STRUCT     StartReq;
+
+	if (Elem->MsgType == MT2_DISASSOC_CONF)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Dis-associate successful\n"));
+
+	    if (pAd->CommonCfg.bWirelessEvent)
+		{
+			RTMPSendWirelessEvent(pAd, IW_DISASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+
+		LinkDown(pAd, FALSE);
+
+		// case 1. no matching BSS, and user wants ADHOC, so we just start a new one
+		if ((pAd->MlmeAux.SsidBssTab.BssNr==0) && (pAd->StaCfg.BssType == BSS_ADHOC))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - No matching BSS, start a new ADHOC (Ssid=%s)...\n",pAd->MlmeAux.Ssid));
+			StartParmFill(pAd, &StartReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_START_REQ, sizeof(MLME_START_REQ_STRUCT), &StartReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_START;
+		}
+		// case 2. try each matched BSS
+		else
+		{
+			pAd->MlmeAux.BssIdx = 0;
+
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitJoinProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT                      Reason;
+	MLME_AUTH_REQ_STRUCT        AuthReq;
+
+	if (Elem->MsgType == MT2_JOIN_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			// 1. joined an IBSS, we are pretty much done here
+			if (pAd->MlmeAux.BssType == BSS_ADHOC)
+			{
+			    //
+				// 5G bands rules of Japan:
+				// Ad hoc must be disabled in W53(ch52,56,60,64) channels.
+				//
+				if ( (pAd->CommonCfg.bIEEE80211H == 1) &&
+                      RadarChannelCheck(pAd, pAd->CommonCfg.Channel)
+				   )
+				{
+					pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+					DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Channel=%d, Join adhoc on W53(52,56,60,64) Channels are not accepted\n", pAd->CommonCfg.Channel));
+					return;
+				}
+
+				LinkUp(pAd, BSS_ADHOC);
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - join the IBSS = %02x:%02x:%02x:%02x:%02x:%02x ...\n",
+				pAd->CommonCfg.Bssid[0],pAd->CommonCfg.Bssid[1],pAd->CommonCfg.Bssid[2],
+				pAd->CommonCfg.Bssid[3],pAd->CommonCfg.Bssid[4],pAd->CommonCfg.Bssid[5]));
+
+                pAd->IndicateMediaState = NdisMediaStateConnected;
+                pAd->ExtraInfo = GENERAL_LINK_UP;
+			}
+			// 2. joined a new INFRA network, start from authentication
+			else
+			{
+#ifdef LEAP_SUPPORT
+				// Add AuthMode "LEAP" for CCX 1.X
+				if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+				{
+					AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, CISCO_AuthModeLEAP);
+				}
+				else
+#endif // LEAP_SUPPORT //
+				{
+					// either Ndis802_11AuthModeShared or Ndis802_11AuthModeAutoSwitch, try shared key first
+					if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeShared) ||
+						(pAd->StaCfg.AuthMode == Ndis802_11AuthModeAutoSwitch))
+					{
+						AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeShared);
+					}
+					else
+					{
+						AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeOpen);
+					}
+				}
+				MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_MLME_AUTH_REQ,
+							sizeof(MLME_AUTH_REQ_STRUCT), &AuthReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_AUTH;
+			}
+		}
+		else
+		{
+			// 3. failed, try next BSS
+			pAd->MlmeAux.BssIdx++;
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitStartProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      Result;
+
+	if (Elem->MsgType == MT2_START_CONF)
+	{
+		NdisMoveMemory(&Result, Elem->Msg, sizeof(USHORT));
+		if (Result == MLME_SUCCESS)
+		{
+		    //
+			// 5G bands rules of Japan:
+			// Ad hoc must be disabled in W53(ch52,56,60,64) channels.
+			//
+			if ( (pAd->CommonCfg.bIEEE80211H == 1) &&
+                  RadarChannelCheck(pAd, pAd->CommonCfg.Channel)
+			   )
+			{
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Channel=%d, Start adhoc on W53(52,56,60,64) Channels are not accepted\n", pAd->CommonCfg.Channel));
+				return;
+			}
+#ifdef DOT11_N_SUPPORT
+			if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+			{
+				N_ChannelCheck(pAd);
+				SetCommonHT(pAd);
+				NdisMoveMemory(&pAd->MlmeAux.AddHtInfo, &pAd->CommonCfg.AddHTInfo, sizeof(ADD_HT_INFO_IE));
+				RTMPCheckHt(pAd, BSSID_WCID, &pAd->CommonCfg.HtCapability, &pAd->CommonCfg.AddHTInfo);
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = TRUE;
+				NdisZeroMemory(&pAd->StaActive.SupportedPhyInfo.MCSSet[0], 16);
+				NdisMoveMemory(&pAd->StaActive.SupportedPhyInfo.MCSSet[0], &pAd->CommonCfg.HtCapability.MCSSet[0], 16);
+				COPY_HTSETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(pAd);
+
+				if ((pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth  == BW_40) &&
+					(pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset == EXTCHA_ABOVE))
+				{
+					pAd->MlmeAux.CentralChannel = pAd->CommonCfg.Channel + 2;
+				}
+				else if ((pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth  == BW_40) &&
+						 (pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset == EXTCHA_BELOW))
+				{
+					pAd->MlmeAux.CentralChannel = pAd->CommonCfg.Channel - 2;
+				}
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+			{
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+			}
+			LinkUp(pAd, BSS_ADHOC);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			// Before send beacon, driver need do radar detection
+			if ((pAd->CommonCfg.Channel > 14 )
+				&& (pAd->CommonCfg.bIEEE80211H == 1)
+				&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+			{
+				pAd->CommonCfg.RadarDetect.RDMode = RD_SILENCE_MODE;
+				pAd->CommonCfg.RadarDetect.RDCount = 0;
+#ifdef DFS_SUPPORT
+				BbpRadarDetectionStart(pAd);
+#endif // DFS_SUPPORT //
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - start a new IBSS = %02x:%02x:%02x:%02x:%02x:%02x ...\n",
+				pAd->CommonCfg.Bssid[0],pAd->CommonCfg.Bssid[1],pAd->CommonCfg.Bssid[2],
+				pAd->CommonCfg.Bssid[3],pAd->CommonCfg.Bssid[4],pAd->CommonCfg.Bssid[5]));
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Start IBSS fail. BUG!!!!!\n"));
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitAuthProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT                       Reason;
+	MLME_ASSOC_REQ_STRUCT        AssocReq;
+	MLME_AUTH_REQ_STRUCT         AuthReq;
+
+	if (Elem->MsgType == MT2_AUTH_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH OK\n"));
+			AssocParmFill(pAd, &AssocReq, pAd->MlmeAux.Bssid, pAd->MlmeAux.CapabilityInfo,
+						  ASSOC_TIMEOUT, pAd->StaCfg.DefaultListenCount);
+
+#ifdef LEAP_SUPPORT
+			//
+			// Cisco Leap CCKM supported Re-association.
+			//
+			if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+			{
+				//if CCKM is turn on , that's mean Fast Reauthentication
+				//Use CCKM Reassociation instead of normal association for Fast Roaming.
+				MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_REASSOC_REQ,
+							sizeof(MLME_ASSOC_REQ_STRUCT), &AssocReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_REASSOC;
+			}
+			else
+#endif // LEAP_SUPPORT //
+			{
+				MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_ASSOC_REQ,
+							sizeof(MLME_ASSOC_REQ_STRUCT), &AssocReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_ASSOC;
+			}
+		}
+		else
+		{
+			// This fail may because of the AP already keep us in its MAC table without
+			// ageing-out. The previous authentication attempt must have let it remove us.
+			// so try Authentication again may help. For D-Link DWL-900AP+ compatibility.
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH FAIL, try again...\n"));
+#ifdef LEAP_SUPPORT
+			//Add AuthMode "LEAP" for CCX 1.X
+			if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+			{
+				AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, CISCO_AuthModeLEAP);
+			}
+			else
+#endif // LEAP_SUPPORT //
+			{
+				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeShared) ||
+					(pAd->StaCfg.AuthMode == Ndis802_11AuthModeAutoSwitch))
+				{
+					// either Ndis802_11AuthModeShared or Ndis802_11AuthModeAutoSwitch, try shared key first
+					AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeShared);
+				}
+				else
+				{
+					AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeOpen);
+				}
+			}
+			MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_MLME_AUTH_REQ,
+						sizeof(MLME_AUTH_REQ_STRUCT), &AuthReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_AUTH2;
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitAuthProc2(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT                       Reason;
+	MLME_ASSOC_REQ_STRUCT        AssocReq;
+	MLME_AUTH_REQ_STRUCT         AuthReq;
+
+	if (Elem->MsgType == MT2_AUTH_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH OK\n"));
+			AssocParmFill(pAd, &AssocReq, pAd->MlmeAux.Bssid, pAd->MlmeAux.CapabilityInfo,
+						  ASSOC_TIMEOUT, pAd->StaCfg.DefaultListenCount);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_ASSOC_REQ,
+						sizeof(MLME_ASSOC_REQ_STRUCT), &AssocReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_ASSOC;
+		}
+		else
+		{
+#ifdef LEAP_SUPPORT
+			// Process LEAP first, since it use different control variable
+			// We don't want to affect other poven operation
+			if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+			{
+				// LEAP Auth not success, try next BSS
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - *LEAP* AUTH FAIL, give up; try next BSS\n"));
+				DBGPRINT(RT_DEBUG_TRACE, ("Total match BSSID [=%d]\n", pAd->MlmeAux.SsidBssTab.BssNr));
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+				pAd->MlmeAux.BssIdx++;
+				IterateOnBssTab(pAd);
+			}
+			else
+#endif // LEAP_SUPPORT //
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeAutoSwitch) &&
+				 (pAd->MlmeAux.Alg == Ndis802_11AuthModeShared))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH FAIL, try OPEN system...\n"));
+				AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeOpen);
+				MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_MLME_AUTH_REQ,
+							sizeof(MLME_AUTH_REQ_STRUCT), &AuthReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_AUTH2;
+			}
+			else
+			{
+				// not success, try next BSS
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH FAIL, give up; try next BSS\n"));
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE; //???????
+				pAd->MlmeAux.BssIdx++;
+				IterateOnBssTab(pAd);
+			}
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitAssocProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      Reason;
+
+	if (Elem->MsgType == MT2_ASSOC_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			LinkUp(pAd, BSS_INFRA);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Association successful on BSS #%ld\n",pAd->MlmeAux.BssIdx));
+
+			if (pAd->CommonCfg.bWirelessEvent)
+			{
+				RTMPSendWirelessEvent(pAd, IW_ASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+			}
+		}
+		else
+		{
+			// not success, try next BSS
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Association fails on BSS #%ld\n",pAd->MlmeAux.BssIdx));
+			pAd->MlmeAux.BssIdx++;
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitReassocProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      Result;
+
+	if (Elem->MsgType == MT2_REASSOC_CONF)
+	{
+		NdisMoveMemory(&Result, Elem->Msg, sizeof(USHORT));
+		if (Result == MLME_SUCCESS)
+		{
+			//
+			// NDIS requires a new Link UP indication but no Link Down for RE-ASSOC
+			//
+			LinkUp(pAd, BSS_INFRA);
+
+			// send wireless event - for association
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_ASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+
+#ifdef LEAP_SUPPORT
+			if (LEAP_CCKM_ON(pAd))
+			{
+				STA_PORT_SECURED(pAd);
+				pAd->StaCfg.WpaState = SS_FINISH;
+			}
+#endif // LEAP_SUPPORT //
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Re-assocition successful on BSS #%ld\n", pAd->MlmeAux.RoamIdx));
+		}
+		else
+		{
+			// reassoc failed, try to pick next BSS in the BSS Table
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Re-assocition fails on BSS #%ld\n", pAd->MlmeAux.RoamIdx));
+			pAd->MlmeAux.RoamIdx++;
+			IterateOnBssTab2(pAd);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID LinkUp(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR BssType)
+{
+	ULONG	Now;
+	UINT32	Data;
+	BOOLEAN	Cancelled;
+	UCHAR	Value = 0, idx;
+	MAC_TABLE_ENTRY *pEntry = NULL, *pCurrEntry;
+
+	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+
+	//
+	// ASSOC - DisassocTimeoutAction
+	// CNTL - Dis-associate successful
+	// !!! LINK DOWN !!!
+	// [88888] OID_802_11_SSID should have returned NDTEST_WEP_AP2(Returned: )
+	//
+	// To prevent DisassocTimeoutAction to call Link down after we link up,
+	// cancel the DisassocTimer no matter what it start or not.
+	//
+	RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,  &Cancelled);
+
+	COPY_SETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(pAd);
+
+#ifdef DOT11_N_SUPPORT
+	COPY_HTSETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(pAd);
+#endif // DOT11_N_SUPPORT //
+	// It's quite difficult to tell if a newly added KEY is WEP or CKIP until a new BSS
+	// is formed (either ASSOC/RE-ASSOC done or IBSS started. LinkUP should be a safe place
+	// to examine if cipher algorithm switching is required.
+	//rt2860b. Don't know why need this
+	SwitchBetweenWepAndCkip(pAd);
+
+#ifdef RT2860
+	// Before power save before link up function, We will force use 1R.
+	// So after link up, check Rx antenna # again.
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+	if(pAd->Antenna.field.RxPath == 3)
+	{
+		Value |= (0x10);
+	}
+	else if(pAd->Antenna.field.RxPath == 2)
+	{
+		Value |= (0x8);
+	}
+	else if(pAd->Antenna.field.RxPath == 1)
+	{
+		Value |= (0x0);
+	}
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+	pAd->StaCfg.BBPR3 = Value;
+#endif // RT2860 //
+
+	if (BssType == BSS_ADHOC)
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_INFRA_ON);
+
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+		// No carrier detection when adhoc
+		// CarrierDetectionStop(pAd);
+		pAd->CommonCfg.CarrierDetect.CD_State = CD_NORMAL;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!Adhoc LINK UP !!! \n" ));
+	}
+	else
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_INFRA_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!Infra LINK UP !!! \n" ));
+	}
+
+	// 3*3
+	// reset Tx beamforming bit
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+	Value &= (~0x01);
+	Value |= pAd->CommonCfg.RegTransmitSetting.field.TxBF;
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+#ifdef DOT11_N_SUPPORT
+	// Change to AP channel
+    if ((pAd->CommonCfg.CentralChannel > pAd->CommonCfg.Channel) && (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+	{
+		// Must using 40MHz.
+		pAd->CommonCfg.BBPCurrentBW = BW_40;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+		Value &= (~0x18);
+		Value |= 0x10;
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+		//  RX : control channel at lower
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+		Value &= (~0x20);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+#ifdef RT2860
+        pAd->StaCfg.BBPR3 = Value;
+#endif // RT2860 //
+
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Data);
+		Data &= 0xfffffffe;
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Data);
+
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x1A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x0A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x16);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Lower LINK UP !!! Control Channel at Below. Central = %d \n", pAd->CommonCfg.CentralChannel ));
+	}
+	else if ((pAd->CommonCfg.CentralChannel < pAd->CommonCfg.Channel) && (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+    {
+	    // Must using 40MHz.
+		pAd->CommonCfg.BBPCurrentBW = BW_40;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+	    AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+		Value &= (~0x18);
+		Value |= 0x10;
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Data);
+		Data |= 0x1;
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Data);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+	    Value |= (0x20);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+#ifdef RT2860
+        pAd->StaCfg.BBPR3 = Value;
+#endif // RT2860 //
+
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x1A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x0A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x16);
+			    DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+
+	    DBGPRINT(RT_DEBUG_TRACE, ("!!! 40MHz Upper LINK UP !!! Control Channel at UpperCentral = %d \n", pAd->CommonCfg.CentralChannel ));
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+    {
+	    pAd->CommonCfg.BBPCurrentBW = BW_20;
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+		Value &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Data);
+		Data &= 0xfffffffe;
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Data);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+		Value &= (~0x20);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+#ifdef RT2860
+        pAd->StaCfg.BBPR3 = Value;
+#endif // RT2860 //
+
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x16);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x08);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x11);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+
+	    DBGPRINT(RT_DEBUG_TRACE, ("!!! 20MHz LINK UP !!! \n" ));
+    }
+
+	RTMPSetAGCInitValue(pAd, pAd->CommonCfg.BBPCurrentBW);
+	//
+	// Save BBP_R66 value, it will be used in RTUSBResumeMsduTransmission
+	//
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &pAd->BbpTuning.R66CurrentValue);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !!! (BssType=%d, AID=%d, ssid=%s, Channel=%d, CentralChannel = %d)\n",
+		BssType, pAd->StaActive.Aid, pAd->CommonCfg.Ssid, pAd->CommonCfg.Channel, pAd->CommonCfg.CentralChannel));
+
+#ifdef DOT11_N_SUPPORT
+	DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !!! (Density =%d, )\n", pAd->MacTab.Content[BSSID_WCID].MpduDensity));
+#endif // DOT11_N_SUPPORT //
+
+		AsicSetBssid(pAd, pAd->CommonCfg.Bssid);
+
+	AsicSetSlotTime(pAd, TRUE);
+	AsicSetEdcaParm(pAd, &pAd->CommonCfg.APEdcaParm);
+
+	// Call this for RTS protectionfor legacy rate, we will always enable RTS threshold, but normally it will not hit
+	AsicUpdateProtect(pAd, 0, (OFDMSETPROTECT | CCKSETPROTECT), TRUE, FALSE);
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE))
+	{
+		// Update HT protectionfor based on AP's operating mode.
+    	if (pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1)
+    	{
+    		AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode,  ALLN_SETPROTECT, FALSE, TRUE);
+    	}
+    	else
+   			AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode,  ALLN_SETPROTECT, FALSE, FALSE);
+	}
+#endif // DOT11_N_SUPPORT //
+
+	NdisZeroMemory(&pAd->DrsCounters, sizeof(COUNTER_DRS));
+
+	NdisGetSystemUpTime(&Now);
+	pAd->StaCfg.LastBeaconRxTime = Now;   // last RX timestamp
+
+	if ((pAd->CommonCfg.TxPreamble != Rt802_11PreambleLong) &&
+		CAP_IS_SHORT_PREAMBLE_ON(pAd->StaActive.CapabilityInfo))
+	{
+		MlmeSetTxPreamble(pAd, Rt802_11PreambleShort);
+	}
+
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+
+	if (pAd->CommonCfg.RadarDetect.RDMode == RD_SILENCE_MODE)
+	{
+#ifdef DFS_SUPPORT
+		RadarDetectionStop(pAd);
+#endif // DFS_SUPPORT //
+	}
+	pAd->CommonCfg.RadarDetect.RDMode = RD_NORMAL_MODE;
+
+	if (BssType == BSS_ADHOC)
+	{
+		MakeIbssBeacon(pAd);
+		if ((pAd->CommonCfg.Channel > 14)
+			&& (pAd->CommonCfg.bIEEE80211H == 1)
+			&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+		{
+			; //Do nothing
+		}
+		else
+		{
+			AsicEnableIbssSync(pAd);
+		}
+
+		// In ad hoc mode, use MAC table from index 1.
+		// p.s ASIC use all 0xff as termination of WCID table search.To prevent it's 0xff-ff-ff-ff-ff-ff, Write 0 here.
+		RTMP_IO_WRITE32(pAd, MAC_WCID_BASE, 0x00);
+		RTMP_IO_WRITE32(pAd, 0x1808, 0x00);
+
+		// If WEP is enabled, add key material and cipherAlg into Asic
+		// Fill in Shared Key Table(offset: 0x6c00) and Shared Key Mode(offset: 0x7000)
+
+		if (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)
+		{
+			PUCHAR	Key;
+			UCHAR 	CipherAlg;
+
+			for (idx=0; idx < SHARE_KEY_NUM; idx++)
+        	{
+				CipherAlg = pAd->SharedKey[BSS0][idx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][idx].Key;
+
+				if (pAd->SharedKey[BSS0][idx].KeyLen > 0)
+				{
+					// Set key material and cipherAlg to Asic
+    				AsicAddSharedKeyEntry(pAd, BSS0, idx, CipherAlg, Key, NULL, NULL);
+
+                    if (idx == pAd->StaCfg.DefaultKeyId)
+					{
+						// Update WCID attribute table and IVEIV table for this group key table
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, NULL);
+					}
+				}
+
+
+			}
+		}
+		// If WPANone is enabled, add key material and cipherAlg into Asic
+		// Fill in Shared Key Table(offset: 0x6c00) and Shared Key Mode(offset: 0x7000)
+		else if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		{
+			pAd->StaCfg.DefaultKeyId = 0;	// always be zero
+
+            NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+							pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+			NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, pAd->StaCfg.PMK, LEN_TKIP_EK);
+
+            if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+            {
+    			NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PMK[16], LEN_TKIP_RXMICK);
+    			NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PMK[16], LEN_TKIP_TXMICK);
+            }
+
+			// Decide its ChiperAlg
+			if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+				pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+			else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+				pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+			else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Unknow Cipher (=%d), set Cipher to AES\n", pAd->StaCfg.PairCipher));
+				pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+            }
+
+			// Set key material and cipherAlg to Asic
+			AsicAddSharedKeyEntry(pAd,
+								  BSS0,
+								  0,
+								  pAd->SharedKey[BSS0][0].CipherAlg,
+								  pAd->SharedKey[BSS0][0].Key,
+								  pAd->SharedKey[BSS0][0].TxMic,
+								  pAd->SharedKey[BSS0][0].RxMic);
+
+            // Update WCID attribute table and IVEIV table for this group key table
+			RTMPAddWcidAttributeEntry(pAd, BSS0, 0, pAd->SharedKey[BSS0][0].CipherAlg, NULL);
+
+		}
+
+	}
+	else // BSS_INFRA
+	{
+		// Check the new SSID with last SSID
+		while (Cancelled == TRUE)
+		{
+			if (pAd->CommonCfg.LastSsidLen == pAd->CommonCfg.SsidLen)
+			{
+				if (RTMPCompareMemory(pAd->CommonCfg.LastSsid, pAd->CommonCfg.Ssid, pAd->CommonCfg.LastSsidLen) == 0)
+				{
+					// Link to the old one no linkdown is required.
+					break;
+				}
+			}
+			// Send link down event before set to link up
+			pAd->IndicateMediaState = NdisMediaStateDisconnected;
+			RTMP_IndicateMediaState(pAd);
+            pAd->ExtraInfo = GENERAL_LINK_DOWN;
+			DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event AA!\n"));
+			break;
+		}
+
+		//
+		// On WPA mode, Remove All Keys if not connect to the last BSSID
+		// Key will be set after 4-way handshake.
+		//
+		if ((pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA))
+		{
+			ULONG 		IV;
+
+			// Remove all WPA keys
+			RTMPWPARemoveAllKeys(pAd);
+			pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilter8021xWEP;
+
+			// Fixed connection failed with Range Maximizer - 515 AP (Marvell Chip) when security is WPAPSK/TKIP
+			// If IV related values are too large in GroupMsg2, AP would ignore this message.
+			IV = 0;
+			IV |= (pAd->StaCfg.DefaultKeyId << 30);
+			AsicUpdateWCIDIVEIV(pAd, BSSID_WCID, IV, 0);
+		}
+		// NOTE:
+		// the decision of using "short slot time" or not may change dynamically due to
+		// new STA association to the AP. so we have to decide that upon parsing BEACON, not here
+
+		// NOTE:
+		// the decision to use "RTC/CTS" or "CTS-to-self" protection or not may change dynamically
+		// due to new STA association to the AP. so we have to decide that upon parsing BEACON, not here
+
+		ComposePsPoll(pAd);
+		ComposeNullFrame(pAd);
+
+			AsicEnableBssSync(pAd);
+
+		// Add BSSID to WCID search table
+		AsicUpdateRxWCIDTable(pAd, BSSID_WCID, pAd->CommonCfg.Bssid);
+
+		NdisAcquireSpinLock(&pAd->MacTabLock);
+		// add this BSSID entry into HASH table
+		{
+			UCHAR HashIdx;
+
+			//pEntry = &pAd->MacTab.Content[BSSID_WCID];
+			HashIdx = MAC_ADDR_HASH_INDEX(pAd->CommonCfg.Bssid);
+			if (pAd->MacTab.Hash[HashIdx] == NULL)
+			{
+				pAd->MacTab.Hash[HashIdx] = pEntry;
+			}
+			else
+			{
+				pCurrEntry = pAd->MacTab.Hash[HashIdx];
+				while (pCurrEntry->pNext != NULL)
+					pCurrEntry = pCurrEntry->pNext;
+				pCurrEntry->pNext = pEntry;
+			}
+		}
+		NdisReleaseSpinLock(&pAd->MacTabLock);
+
+
+		// If WEP is enabled, add paiewise and shared key
+#ifdef WPA_SUPPLICANT_SUPPORT
+        if (((pAd->StaCfg.WpaSupplicantUP)&&
+             (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)&&
+             (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED)) ||
+            ((pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_DISABLE)&&
+              (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)))
+#else
+		if (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)
+#endif // WPA_SUPPLICANT_SUPPORT //
+		{
+			PUCHAR	Key;
+			UCHAR 	CipherAlg;
+
+			for (idx=0; idx < SHARE_KEY_NUM; idx++)
+        	{
+				CipherAlg = pAd->SharedKey[BSS0][idx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][idx].Key;
+
+				if (pAd->SharedKey[BSS0][idx].KeyLen > 0)
+				{
+					// Set key material and cipherAlg to Asic
+    				AsicAddSharedKeyEntry(pAd, BSS0, idx, CipherAlg, Key, NULL, NULL);
+
+					if (idx == pAd->StaCfg.DefaultKeyId)
+					{
+						// Assign group key info
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, NULL);
+
+						// Assign pairwise key info
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, pEntry);
+					}
+				}
+			}
+		}
+
+		// only INFRASTRUCTURE mode need to indicate connectivity immediately; ADHOC mode
+		// should wait until at least 2 active nodes in this BSSID.
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+
+        // For GUI ++
+		if (pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+		{
+			pAd->IndicateMediaState = NdisMediaStateConnected;
+			pAd->ExtraInfo = GENERAL_LINK_UP;
+		}
+        // --
+		RTMP_IndicateMediaState(pAd);
+
+		// Add BSSID in my MAC Table.
+        NdisAcquireSpinLock(&pAd->MacTabLock);
+		RTMPMoveMemory(pAd->MacTab.Content[BSSID_WCID].Addr, pAd->CommonCfg.Bssid, MAC_ADDR_LEN);
+		pAd->MacTab.Content[BSSID_WCID].Aid = BSSID_WCID;
+		pAd->MacTab.Content[BSSID_WCID].pAd = pAd;
+		pAd->MacTab.Content[BSSID_WCID].ValidAsCLI = TRUE;	//Although this is bssid..still set ValidAsCl
+		pAd->MacTab.Size = 1;	// infra mode always set MACtab size =1.
+		pAd->MacTab.Content[BSSID_WCID].Sst = SST_ASSOC;
+		pAd->MacTab.Content[BSSID_WCID].AuthState = SST_ASSOC;
+		pAd->MacTab.Content[BSSID_WCID].WepStatus = pAd->StaCfg.WepStatus;
+        NdisReleaseSpinLock(&pAd->MacTabLock);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !!!  ClientStatusFlags=%lx)\n",
+			pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+
+		MlmeUpdateTxRates(pAd, TRUE, BSS0);
+#ifdef DOT11_N_SUPPORT
+		MlmeUpdateHtTxRates(pAd, BSS0);
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !! (StaActive.bHtEnable =%d, )\n", pAd->StaActive.SupportedPhyInfo.bHtEnable));
+#endif // DOT11_N_SUPPORT //
+
+		//
+		// Report Adjacent AP report.
+		//
+#ifdef LEAP_SUPPORT
+		CCXAdjacentAPReport(pAd);
+#endif // LEAP_SUPPORT //
+
+		if (pAd->CommonCfg.bAggregationCapable)
+		{
+			if ((pAd->CommonCfg.bPiggyBackCapable) && (pAd->MlmeAux.APRalinkIe & 0x00000003) == 3)
+			{
+
+				OPSTATUS_SET_FLAG(pAd, fOP_STATUS_PIGGYBACK_INUSED);
+				OPSTATUS_SET_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+                RTMPSetPiggyBack(pAd, TRUE);
+				DBGPRINT(RT_DEBUG_TRACE, ("Turn on Piggy-Back\n"));
+			}
+			else if (pAd->MlmeAux.APRalinkIe & 0x00000001)
+			{
+				OPSTATUS_SET_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+			}
+		}
+
+		if (pAd->MlmeAux.APRalinkIe != 0x0)
+		{
+#ifdef DOT11_N_SUPPORT
+			if (CLIENT_STATUS_TEST_FLAG(&pAd->MacTab.Content[BSSID_WCID], fCLIENT_STATUS_RDG_CAPABLE))
+			{
+				AsicEnableRDG(pAd);
+			}
+#endif // DOT11_N_SUPPORT //
+			OPSTATUS_SET_FLAG(pAd, fCLIENT_STATUS_RALINK_CHIPSET);
+			CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[BSSID_WCID], fCLIENT_STATUS_RALINK_CHIPSET);
+		}
+		else
+		{
+			OPSTATUS_CLEAR_FLAG(pAd, fCLIENT_STATUS_RALINK_CHIPSET);
+			CLIENT_STATUS_CLEAR_FLAG(&pAd->MacTab.Content[BSSID_WCID], fCLIENT_STATUS_RALINK_CHIPSET);
+		}
+	}
+
+#ifdef DOT11_N_SUPPORT
+	DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_CONNECT Event B!.BACapability = %x. ClientStatusFlags = %lx\n", pAd->CommonCfg.BACapability.word, pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+#endif // DOT11_N_SUPPORT //
+
+	// Set LED
+	RTMPSetLED(pAd, LED_LINK_UP);
+
+	pAd->Mlme.PeriodicRound = 0;
+	pAd->Mlme.OneSecPeriodicRound = 0;
+	pAd->bConfigChanged = FALSE;        // Reset config flag
+	pAd->ExtraInfo = GENERAL_LINK_UP;   // Update extra information to link is up
+
+	// Set asic auto fall back
+	{
+		PUCHAR					pTable;
+		UCHAR					TableSize = 0;
+
+		MlmeSelectTxRateTable(pAd, &pAd->MacTab.Content[BSSID_WCID], &pTable, &TableSize, &pAd->CommonCfg.TxRateIndex);
+		AsicUpdateAutoFallBackTable(pAd, pTable);
+	}
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+    pEntry->HTPhyMode.word = pAd->StaCfg.HTPhyMode.word;
+    pEntry->MaxHTPhyMode.word = pAd->StaCfg.HTPhyMode.word;
+	if (pAd->StaCfg.bAutoTxRateSwitch == FALSE)
+	{
+		pEntry->bAutoTxRateSwitch = FALSE;
+#ifdef DOT11_N_SUPPORT
+		if (pEntry->HTPhyMode.field.MCS == 32)
+			pEntry->HTPhyMode.field.ShortGI = GI_800;
+
+		if ((pEntry->HTPhyMode.field.MCS > MCS_7) || (pEntry->HTPhyMode.field.MCS == 32))
+			pEntry->HTPhyMode.field.STBC = STBC_NONE;
+#endif // DOT11_N_SUPPORT //
+		// If the legacy mode is set, overwrite the transmit setting of this entry.
+		if (pEntry->HTPhyMode.field.MODE <= MODE_OFDM)
+			RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+	}
+	else
+		pEntry->bAutoTxRateSwitch = TRUE;
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+	//  Let Link Status Page display first initial rate.
+	pAd->LastTxRate = (USHORT)(pEntry->HTPhyMode.word);
+	// Select DAC according to HT or Legacy
+	if (pAd->StaActive.SupportedPhyInfo.MCSSet[0] != 0x00)
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &Value);
+		Value &= (~0x18);
+		if (pAd->Antenna.field.TxPath == 2)
+		{
+		    Value |= 0x10;
+		}
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, Value);
+	}
+	else
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &Value);
+		Value &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, Value);
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if (pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE)
+	{
+	}
+	else if (pEntry->MaxRAmpduFactor == 0)
+	{
+	    // If HT AP doesn't support MaxRAmpduFactor = 1, we need to set max PSDU to 0.
+	    // Because our Init value is 1 at MACRegTable.
+		RTMP_IO_WRITE32(pAd, MAX_LEN_CFG, 0x0fff);
+	}
+#endif // DOT11_N_SUPPORT //
+
+	// Patch for Marvel AP to gain high throughput
+	// Need to set as following,
+	// 1. Set txop in register-EDCA_AC0_CFG as 0x60
+	// 2. Set EnTXWriteBackDDONE in register-WPDMA_GLO_CFG as zero
+	// 3. PBF_MAX_PCNT as 0x1F3FBF9F
+	// 4. kick per two packets when dequeue
+	//
+	// Txop can only be modified when RDG is off, WMM is disable and TxBurst is enable
+	//
+	// if 1. Legacy AP WMM on,  or 2. 11n AP, AMPDU disable.  Force turn off burst no matter what bEnableTxBurst is.
+#ifdef DOT11_N_SUPPORT
+	if (((pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED)))
+		|| ((pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE) && (pAd->CommonCfg.BACapability.field.Policy == BA_NOTUSE)))
+	{
+		RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+		Data  &= 0xFFFFFF00;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+
+		RTMP_IO_WRITE32(pAd, PBF_MAX_PCNT, 0x1F3F7F9F);
+		DBGPRINT(RT_DEBUG_TRACE, ("Txburst 1\n"));
+	}
+	else
+#endif // DOT11_N_SUPPORT //
+	if (pAd->CommonCfg.bEnableTxBurst)
+	{
+		RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+		Data  &= 0xFFFFFF00;
+		Data  |= 0x60;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+		pAd->CommonCfg.IOTestParm.bNowAtherosBurstOn = TRUE;
+
+		RTMP_IO_WRITE32(pAd, PBF_MAX_PCNT, 0x1F3FBF9F);
+		DBGPRINT(RT_DEBUG_TRACE, ("Txburst 2\n"));
+	}
+	else
+	{
+		RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+		Data  &= 0xFFFFFF00;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+
+		RTMP_IO_WRITE32(pAd, PBF_MAX_PCNT, 0x1F3F7F9F);
+		DBGPRINT(RT_DEBUG_TRACE, ("Txburst 3\n"));
+	}
+
+#ifdef DOT11_N_SUPPORT
+	// Re-check to turn on TX burst or not.
+	if ((pAd->CommonCfg.IOTestParm.bLastAtheros == TRUE) && ((STA_WEP_ON(pAd))||(STA_TKIP_ON(pAd))))
+	{
+		pAd->CommonCfg.IOTestParm.bNextDisableRxBA = TRUE;
+		if (pAd->CommonCfg.bEnableTxBurst)
+		{
+		    UINT32 MACValue = 0;
+			// Force disable  TXOP value in this case. The same action in MLMEUpdateProtect too.
+			// I didn't change PBF_MAX_PCNT setting.
+			RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &MACValue);
+			MACValue  &= 0xFFFFFF00;
+			RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, MACValue);
+			pAd->CommonCfg.IOTestParm.bNowAtherosBurstOn = FALSE;
+		}
+	}
+	else
+	{
+		pAd->CommonCfg.IOTestParm.bNextDisableRxBA = FALSE;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	pAd->CommonCfg.IOTestParm.bLastAtheros = FALSE;
+	COPY_MAC_ADDR(pAd->CommonCfg.LastBssid, pAd->CommonCfg.Bssid);
+	DBGPRINT(RT_DEBUG_TRACE, ("!!!pAd->bNextDisableRxBA= %d \n", pAd->CommonCfg.IOTestParm.bNextDisableRxBA));
+	// BSSID add in one MAC entry too.  Because in Tx, ASIC need to check Cipher and IV/EIV, BAbitmap
+	// Pther information in MACTab.Content[BSSID_WCID] is not necessary for driver.
+	// Note: As STA, The MACTab.Content[BSSID_WCID]. PairwiseKey and Shared Key for BSS0 are the same.
+
+    if (pAd->StaCfg.WepStatus <= Ndis802_11WEPDisabled)
+    {
+        pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+		pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilterAcceptAll;
+	}
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+	pEntry->PortSecured = pAd->StaCfg.PortSecured;
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+    //
+	// Patch Atheros AP TX will breakdown issue.
+	// AP Model: DLink DWL-8200AP
+	//
+	if (INFRA_ON(pAd) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) && STA_TKIP_ON(pAd))
+	{
+		RTMP_IO_WRITE32(pAd, RX_PARSER_CFG, 0x01);
+	}
+	else
+	{
+		RTMP_IO_WRITE32(pAd, RX_PARSER_CFG, 0x00);
+	}
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	if ((pAd->CommonCfg.BACapability.field.b2040CoexistScanSup) && (pAd->CommonCfg.Channel <= 11))
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_SCAN_2040);
+		BuildEffectedChannelList(pAd);
+	}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+}
+
+/*
+	==========================================================================
+
+	Routine	Description:
+		Disconnect current BSSID
+
+	Arguments:
+		pAd				- Pointer to our adapter
+		IsReqFromAP		- Request from AP
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		We need more information to know it's this requst from AP.
+		If yes! we need to do extra handling, for example, remove the WPA key.
+		Otherwise on 4-way handshaking will faied, since the WPA key didn't be
+		remove while auto reconnect.
+		Disconnect request from AP, it means we will start afresh 4-way handshaking
+		on WPA mode.
+
+	==========================================================================
+*/
+VOID LinkDown(
+	IN PRTMP_ADAPTER pAd,
+	IN  BOOLEAN      IsReqFromAP)
+{
+	UCHAR			    i, ByteValue = 0;
+
+	// Do nothing if monitor mode is on
+	if (MONITOR_ON(pAd))
+		return;
+
+#ifdef RALINK_ATE
+	// Nothing to do in ATE mode.
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+    if (pAd->CommonCfg.bWirelessEvent)
+	{
+		RTMPSendWirelessEvent(pAd, IW_STA_LINKDOWN_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK DOWN !!!\n"));
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+
+#ifdef RT2860
+    if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+    {
+	    BOOLEAN Cancelled;
+        pAd->Mlme.bPsPollTimerRunning = FALSE;
+        RTMPCancelTimer(&pAd->Mlme.PsPollTimer,	&Cancelled);
+    }
+
+    if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+    {
+        AUTO_WAKEUP_STRUC AutoWakeupCfg;
+		AsicForceWakeup(pAd, TRUE);
+        AutoWakeupCfg.word = 0;
+	    RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+        OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+    }
+
+    pAd->bPCIclkOff = FALSE;
+#endif // RT2860 //
+	if (ADHOC_ON(pAd))		// Adhoc mode link down
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK DOWN 1!!!\n"));
+
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+		pAd->IndicateMediaState = NdisMediaStateDisconnected;
+		RTMP_IndicateMediaState(pAd);
+        pAd->ExtraInfo = GENERAL_LINK_DOWN;
+		BssTableDeleteEntry(&pAd->ScanTab, pAd->CommonCfg.Bssid, pAd->CommonCfg.Channel);
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! MacTab.Size=%d !!!\n", pAd->MacTab.Size));
+	}
+	else					// Infra structure mode
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK DOWN 2!!!\n"));
+
+#ifdef QOS_DLS_SUPPORT
+		// DLS tear down frame must be sent before link down
+		// send DLS-TEAR_DOWN message
+		if (pAd->CommonCfg.bDLSCapable)
+		{
+			// tear down local dls table entry
+			for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+				{
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				}
+			}
+
+			// tear down peer dls table entry
+			for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status ==  DLS_FINISH))
+				{
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				}
+			}
+		}
+#endif // QOS_DLS_SUPPORT //
+
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_INFRA_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+
+		// Saved last SSID for linkup comparison
+		pAd->CommonCfg.LastSsidLen = pAd->CommonCfg.SsidLen;
+		NdisMoveMemory(pAd->CommonCfg.LastSsid, pAd->CommonCfg.Ssid, pAd->CommonCfg.LastSsidLen);
+		COPY_MAC_ADDR(pAd->CommonCfg.LastBssid, pAd->CommonCfg.Bssid);
+		if (pAd->MlmeAux.CurrReqIsFromNdis == TRUE)
+		{
+			pAd->IndicateMediaState = NdisMediaStateDisconnected;
+			RTMP_IndicateMediaState(pAd);
+            pAd->ExtraInfo = GENERAL_LINK_DOWN;
+			DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event A!\n"));
+			pAd->MlmeAux.CurrReqIsFromNdis = FALSE;
+		}
+		else
+		{
+            //
+			// If disassociation request is from NDIS, then we don't need to delete BSSID from entry.
+			// Otherwise lost beacon or receive De-Authentication from AP,
+			// then we should delete BSSID from BssTable.
+			// If we don't delete from entry, roaming will fail.
+			//
+			BssTableDeleteEntry(&pAd->ScanTab, pAd->CommonCfg.Bssid, pAd->CommonCfg.Channel);
+		}
+
+		// restore back to -
+		//      1. long slot (20 us) or short slot (9 us) time
+		//      2. turn on/off RTS/CTS and/or CTS-to-self protection
+		//      3. short preamble
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED);
+
+		if (pAd->StaCfg.CCXAdjacentAPReportFlag == TRUE)
+		{
+			//
+			// Record current AP's information.
+			// for later used reporting Adjacent AP report.
+			//
+			pAd->StaCfg.CCXAdjacentAPChannel = pAd->CommonCfg.Channel;
+			pAd->StaCfg.CCXAdjacentAPSsidLen = pAd->CommonCfg.SsidLen;
+			NdisMoveMemory(pAd->StaCfg.CCXAdjacentAPSsid, pAd->CommonCfg.Ssid, pAd->StaCfg.CCXAdjacentAPSsidLen);
+			COPY_MAC_ADDR(pAd->StaCfg.CCXAdjacentAPBssid, pAd->CommonCfg.Bssid);
+		}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+		// Country IE of the AP will be evaluated and will be used.
+		if (pAd->StaCfg.IEEE80211dClientMode != Rt802_11_D_None)
+		{
+			NdisMoveMemory(&pAd->CommonCfg.CountryCode[0], &pAd->StaCfg.StaOriCountryCode[0], 2);
+			pAd->CommonCfg.Geography = pAd->StaCfg.StaOriGeography;
+			BuildChannelListEx(pAd);
+		}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	}
+
+	for (i=1; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		if (pAd->MacTab.Content[i].ValidAsCLI == TRUE)
+			MacTableDeleteEntry(pAd, pAd->MacTab.Content[i].Aid, pAd->MacTab.Content[i].Addr);
+	}
+
+	pAd->StaCfg.CCXQosECWMin	= 4;
+	pAd->StaCfg.CCXQosECWMax	= 10;
+
+	AsicSetSlotTime(pAd, TRUE); //FALSE);
+	AsicSetEdcaParm(pAd, NULL);
+
+	// Set LED
+	RTMPSetLED(pAd, LED_LINK_DOWN);
+    pAd->LedIndicatorStregth = 0xF0;
+    RTMPSetSignalLED(pAd, -100);	// Force signal strength Led to be turned off, firmware is not done it.
+
+		AsicDisableSync(pAd);
+
+	pAd->Mlme.PeriodicRound = 0;
+	pAd->Mlme.OneSecPeriodicRound = 0;
+
+	if (pAd->StaCfg.BssType == BSS_INFRA)
+	{
+		// Remove StaCfg Information after link down
+		NdisZeroMemory(pAd->CommonCfg.Bssid, MAC_ADDR_LEN);
+		NdisZeroMemory(pAd->CommonCfg.Ssid, MAX_LEN_OF_SSID);
+		pAd->CommonCfg.SsidLen = 0;
+	}
+#ifdef DOT11_N_SUPPORT
+	NdisZeroMemory(&pAd->MlmeAux.HtCapability, sizeof(HT_CAPABILITY_IE));
+	NdisZeroMemory(&pAd->MlmeAux.AddHtInfo, sizeof(ADD_HT_INFO_IE));
+	pAd->MlmeAux.HtCapabilityLen = 0;
+	pAd->MlmeAux.NewExtChannelOffset = 0xff;
+#endif // DOT11_N_SUPPORT //
+
+	// Reset WPA-PSK state. Only reset when supplicant enabled
+	if (pAd->StaCfg.WpaState != SS_NOTUSE)
+	{
+		pAd->StaCfg.WpaState = SS_START;
+		// Clear Replay counter
+		NdisZeroMemory(pAd->StaCfg.ReplayCounter, 8);
+
+#ifdef QOS_DLS_SUPPORT
+		if (pAd->CommonCfg.bDLSCapable)
+			NdisZeroMemory(pAd->StaCfg.DlsReplayCounter, 8);
+#endif // QOS_DLS_SUPPORT //
+	}
+
+
+	//
+	// if link down come from AP, we need to remove all WPA keys on WPA mode.
+	// otherwise will cause 4-way handshaking failed, since the WPA key not empty.
+	//
+	if ((IsReqFromAP) && (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA))
+	{
+		// Remove all WPA keys
+		RTMPWPARemoveAllKeys(pAd);
+	}
+
+	// 802.1x port control
+#ifdef WPA_SUPPLICANT_SUPPORT
+	// Prevent clear PortSecured here with static WEP
+	// NetworkManger set security policy first then set SSID to connect AP.
+	if (pAd->StaCfg.WpaSupplicantUP &&
+		(pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled) &&
+		(pAd->StaCfg.IEEE8021X == FALSE))
+	{
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+	}
+	else
+#endif // WPA_SUPPLICANT_SUPPORT //
+	{
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+		pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilter8021xWEP;
+	}
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+	pAd->MacTab.Content[BSSID_WCID].PortSecured = pAd->StaCfg.PortSecured;
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+	pAd->StaCfg.MicErrCnt = 0;
+
+	// Turn off Ckip control flag
+	pAd->StaCfg.bCkipOn = FALSE;
+	pAd->StaCfg.CCXEnable = FALSE;
+
+    pAd->IndicateMediaState = NdisMediaStateDisconnected;
+	// Update extra information to link is up
+	pAd->ExtraInfo = GENERAL_LINK_DOWN;
+
+    pAd->StaCfg.AdhocBOnlyJoined = FALSE;
+	pAd->StaCfg.AdhocBGJoined = FALSE;
+	pAd->StaCfg.Adhoc20NJoined = FALSE;
+    pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+
+	// Reset the Current AP's IP address
+	NdisZeroMemory(pAd->StaCfg.AironetIPAddress, 4);
+
+	// Clean association information
+	NdisZeroMemory(&pAd->StaCfg.AssocInfo, sizeof(NDIS_802_11_ASSOCIATION_INFORMATION));
+	pAd->StaCfg.AssocInfo.Length = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+	pAd->StaCfg.ReqVarIELen = 0;
+	pAd->StaCfg.ResVarIELen = 0;
+
+	//
+	// Reset RSSI value after link down
+	//
+	pAd->StaCfg.RssiSample.AvgRssi0 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi0X8 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi1 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi1X8 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi2 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi2X8 = 0;
+
+	// Restore MlmeRate
+	pAd->CommonCfg.MlmeRate = pAd->CommonCfg.BasicMlmeRate;
+	pAd->CommonCfg.RtsRate = pAd->CommonCfg.BasicMlmeRate;
+
+#ifdef DOT11_N_SUPPORT
+	//
+	// After Link down, reset piggy-back setting in ASIC. Disable RDG.
+	//
+	if (pAd->CommonCfg.BBPCurrentBW == BW_40)
+	{
+		pAd->CommonCfg.BBPCurrentBW = BW_20;
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &ByteValue);
+		ByteValue &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, ByteValue);
+	}
+#endif // DOT11_N_SUPPORT //
+	// Reset DAC
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &ByteValue);
+	ByteValue &= (~0x18);
+	if (pAd->Antenna.field.TxPath == 2)
+	{
+		ByteValue |= 0x10;
+	}
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, ByteValue);
+
+	RTMPSetPiggyBack(pAd,FALSE);
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_PIGGYBACK_INUSED);
+
+#ifdef DOT11_N_SUPPORT
+	pAd->CommonCfg.BACapability.word = pAd->CommonCfg.REGBACapability.word;
+#endif // DOT11_N_SUPPORT //
+
+	// Restore all settings in the following.
+	AsicUpdateProtect(pAd, 0, (ALLN_SETPROTECT|CCKSETPROTECT|OFDMSETPROTECT), TRUE, FALSE);
+	AsicDisableRDG(pAd);
+	pAd->CommonCfg.IOTestParm.bCurrentAtheros = FALSE;
+	pAd->CommonCfg.IOTestParm.bNowAtherosBurstOn = FALSE;
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SCAN_2040);
+	pAd->CommonCfg.BSSCoexist2040.word = 0;
+	TriEventInit(pAd);
+	for (i = 0; i < (pAd->ChannelListNum - 1); i++)
+	{
+		pAd->ChannelList[i].bEffectedChannel = FALSE;
+	}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+	RTMP_IO_WRITE32(pAd, MAX_LEN_CFG, 0x1fff);
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+	if (pAd->StaCfg.WpaSupplicantUP) {
+		union iwreq_data    wrqu;
+		//send disassociate event to wpa_supplicant
+		memset(&wrqu, 0, sizeof(wrqu));
+		wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+		wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+	}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+	{
+		union iwreq_data    wrqu;
+		memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+		wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+	}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID IterateOnBssTab(
+	IN PRTMP_ADAPTER pAd)
+{
+	MLME_START_REQ_STRUCT   StartReq;
+	MLME_JOIN_REQ_STRUCT    JoinReq;
+	ULONG                   BssIdx;
+
+	// Change the wepstatus to original wepstatus
+	pAd->StaCfg.WepStatus   = pAd->StaCfg.OrigWepStatus;
+	pAd->StaCfg.PairCipher  = pAd->StaCfg.OrigWepStatus;
+	pAd->StaCfg.GroupCipher = pAd->StaCfg.OrigWepStatus;
+
+	BssIdx = pAd->MlmeAux.BssIdx;
+	if (BssIdx < pAd->MlmeAux.SsidBssTab.BssNr)
+	{
+		// Check cipher suite, AP must have more secured cipher than station setting
+		// Set the Pairwise and Group cipher to match the intended AP setting
+		// We can only connect to AP with less secured cipher setting
+		if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+		{
+			pAd->StaCfg.GroupCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.GroupCipher;
+
+			if (pAd->StaCfg.WepStatus == pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipher)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipher;
+			else if (pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipherAux != Ndis802_11WEPDisabled)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipherAux;
+			else	// There is no PairCipher Aux, downgrade our capability to TKIP
+				pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+		}
+		else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+		{
+			pAd->StaCfg.GroupCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.GroupCipher;
+
+			if (pAd->StaCfg.WepStatus == pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipher)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipher;
+			else if (pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipherAux != Ndis802_11WEPDisabled)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipherAux;
+			else	// There is no PairCipher Aux, downgrade our capability to TKIP
+				pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+
+			// RSN capability
+			pAd->StaCfg.RsnCapability = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.RsnCapability;
+		}
+
+		// Set Mix cipher flag
+		pAd->StaCfg.bMixCipher = (pAd->StaCfg.PairCipher == pAd->StaCfg.GroupCipher) ? FALSE : TRUE;
+		if (pAd->StaCfg.bMixCipher == TRUE)
+		{
+			// If mix cipher, re-build RSNIE
+			RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, 0);
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - iterate BSS %ld of %d\n", BssIdx, pAd->MlmeAux.SsidBssTab.BssNr));
+		JoinParmFill(pAd, &JoinReq, BssIdx);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_JOIN_REQ, sizeof(MLME_JOIN_REQ_STRUCT),
+					&JoinReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_JOIN;
+	}
+	else if (pAd->StaCfg.BssType == BSS_ADHOC)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - All BSS fail; start a new ADHOC (Ssid=%s)...\n",pAd->MlmeAux.Ssid));
+		StartParmFill(pAd, &StartReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_START_REQ, sizeof(MLME_START_REQ_STRUCT), &StartReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_START;
+	}
+	else // no more BSS
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - All roaming failed, stay @ ch #%d\n", pAd->CommonCfg.Channel));
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+	}
+}
+
+// for re-association only
+// IRQL = DISPATCH_LEVEL
+VOID IterateOnBssTab2(
+	IN PRTMP_ADAPTER pAd)
+{
+	MLME_REASSOC_REQ_STRUCT ReassocReq;
+	ULONG                   BssIdx;
+	BSS_ENTRY               *pBss;
+
+	BssIdx = pAd->MlmeAux.RoamIdx;
+	pBss = &pAd->MlmeAux.RoamTab.BssEntry[BssIdx];
+
+	if (BssIdx < pAd->MlmeAux.RoamTab.BssNr)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - iterate BSS %ld of %d\n", BssIdx, pAd->MlmeAux.RoamTab.BssNr));
+
+		AsicSwitchChannel(pAd, pBss->Channel, FALSE);
+		AsicLockChannel(pAd, pBss->Channel);
+
+		// reassociate message has the same structure as associate message
+		AssocParmFill(pAd, &ReassocReq, pBss->Bssid, pBss->CapabilityInfo,
+					  ASSOC_TIMEOUT, pAd->StaCfg.DefaultListenCount);
+		MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_REASSOC_REQ,
+					sizeof(MLME_REASSOC_REQ_STRUCT), &ReassocReq);
+
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_REASSOC;
+	}
+	else // no more BSS
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - All fast roaming failed, back to ch #%d\n",pAd->CommonCfg.Channel));
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID JoinParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_JOIN_REQ_STRUCT *JoinReq,
+	IN ULONG BssIdx)
+{
+	JoinReq->BssIdx = BssIdx;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID ScanParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_SCAN_REQ_STRUCT *ScanReq,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN UCHAR ScanType)
+{
+    NdisZeroMemory(ScanReq->Ssid, MAX_LEN_OF_SSID);
+	ScanReq->SsidLen = SsidLen;
+	NdisMoveMemory(ScanReq->Ssid, Ssid, SsidLen);
+	ScanReq->BssType = BssType;
+	ScanReq->ScanType = ScanType;
+}
+
+#ifdef QOS_DLS_SUPPORT
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID DlsParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_DLS_REQ_STRUCT *pDlsReq,
+	IN PRT_802_11_DLS pDls,
+	IN USHORT reason)
+{
+	pDlsReq->pDLS = pDls;
+	pDlsReq->Reason = reason;
+}
+#endif // QOS_DLS_SUPPORT //
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID StartParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_START_REQ_STRUCT *StartReq,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen)
+{
+	ASSERT(SsidLen <= MAX_LEN_OF_SSID);
+	NdisMoveMemory(StartReq->Ssid, Ssid, SsidLen);
+	StartReq->SsidLen = SsidLen;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID AuthParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_AUTH_REQ_STRUCT *AuthReq,
+	IN PUCHAR pAddr,
+	IN USHORT Alg)
+{
+	COPY_MAC_ADDR(AuthReq->Addr, pAddr);
+	AuthReq->Alg = Alg;
+	AuthReq->Timeout = AUTH_TIMEOUT;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+#ifdef RT2860
+VOID ComposePsPoll(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisZeroMemory(&pAd->PsPollFrame, sizeof(PSPOLL_FRAME));
+	pAd->PsPollFrame.FC.Type = BTYPE_CNTL;
+	pAd->PsPollFrame.FC.SubType = SUBTYPE_PS_POLL;
+	pAd->PsPollFrame.Aid = pAd->StaActive.Aid | 0xC000;
+	COPY_MAC_ADDR(pAd->PsPollFrame.Bssid, pAd->CommonCfg.Bssid);
+	COPY_MAC_ADDR(pAd->PsPollFrame.Ta, pAd->CurrentAddress);
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID ComposeNullFrame(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisZeroMemory(&pAd->NullFrame, sizeof(HEADER_802_11));
+	pAd->NullFrame.FC.Type = BTYPE_DATA;
+	pAd->NullFrame.FC.SubType = SUBTYPE_NULL_FUNC;
+	pAd->NullFrame.FC.ToDs = 1;
+	COPY_MAC_ADDR(pAd->NullFrame.Addr1, pAd->CommonCfg.Bssid);
+	COPY_MAC_ADDR(pAd->NullFrame.Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pAd->NullFrame.Addr3, pAd->CommonCfg.Bssid);
+}
+#endif // RT2860 //
+
+
+
+
+/*
+	==========================================================================
+	Description:
+		Pre-build a BEACON frame in the shared memory
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+ULONG MakeIbssBeacon(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR         DsLen = 1, IbssLen = 2;
+	UCHAR         LocalErpIe[3] = {IE_ERP, 1, 0x04};
+	HEADER_802_11 BcnHdr;
+	USHORT        CapabilityInfo;
+	LARGE_INTEGER FakeTimestamp;
+	ULONG         FrameLen = 0;
+	PTXWI_STRUC	  pTxWI = &pAd->BeaconTxWI;
+	CHAR         *pBeaconFrame = pAd->BeaconBuf;
+	BOOLEAN       Privacy;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR         SupRateLen = 0;
+	UCHAR         ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR         ExtRateLen = 0;
+	UCHAR         RSNIe = IE_WPA;
+
+	if ((pAd->CommonCfg.PhyMode == PHY_11B) && (pAd->CommonCfg.Channel <= 14))
+	{
+		SupRate[0] = 0x82; // 1 mbps
+		SupRate[1] = 0x84; // 2 mbps
+		SupRate[2] = 0x8b; // 5.5 mbps
+		SupRate[3] = 0x96; // 11 mbps
+		SupRateLen = 4;
+		ExtRateLen = 0;
+	}
+	else if (pAd->CommonCfg.Channel > 14)
+	{
+		SupRate[0]  = 0x8C;    // 6 mbps, in units of 0.5 Mbps, basic rate
+		SupRate[1]  = 0x12;    // 9 mbps, in units of 0.5 Mbps
+		SupRate[2]  = 0x98;    // 12 mbps, in units of 0.5 Mbps, basic rate
+		SupRate[3]  = 0x24;    // 18 mbps, in units of 0.5 Mbps
+		SupRate[4]  = 0xb0;    // 24 mbps, in units of 0.5 Mbps, basic rate
+		SupRate[5]  = 0x48;    // 36 mbps, in units of 0.5 Mbps
+		SupRate[6]  = 0x60;    // 48 mbps, in units of 0.5 Mbps
+		SupRate[7]  = 0x6c;    // 54 mbps, in units of 0.5 Mbps
+		SupRateLen  = 8;
+		ExtRateLen  = 0;
+
+		//
+		// Also Update MlmeRate & RtsRate for G only & A only
+		//
+		pAd->CommonCfg.MlmeRate = RATE_6;
+		pAd->CommonCfg.RtsRate = RATE_6;
+		pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+		pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MODE = MODE_OFDM;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+	}
+	else
+	{
+		SupRate[0] = 0x82; // 1 mbps
+		SupRate[1] = 0x84; // 2 mbps
+		SupRate[2] = 0x8b; // 5.5 mbps
+		SupRate[3] = 0x96; // 11 mbps
+		SupRateLen = 4;
+
+		ExtRate[0]  = 0x0C;    // 6 mbps, in units of 0.5 Mbps,
+		ExtRate[1]  = 0x12;    // 9 mbps, in units of 0.5 Mbps
+		ExtRate[2]  = 0x18;    // 12 mbps, in units of 0.5 Mbps,
+		ExtRate[3]  = 0x24;    // 18 mbps, in units of 0.5 Mbps
+		ExtRate[4]  = 0x30;    // 24 mbps, in units of 0.5 Mbps,
+		ExtRate[5]  = 0x48;    // 36 mbps, in units of 0.5 Mbps
+		ExtRate[6]  = 0x60;    // 48 mbps, in units of 0.5 Mbps
+		ExtRate[7]  = 0x6c;    // 54 mbps, in units of 0.5 Mbps
+		ExtRateLen  = 8;
+	}
+
+	pAd->StaActive.SupRateLen = SupRateLen;
+	NdisMoveMemory(pAd->StaActive.SupRate, SupRate, SupRateLen);
+	pAd->StaActive.ExtRateLen = ExtRateLen;
+	NdisMoveMemory(pAd->StaActive.ExtRate, ExtRate, ExtRateLen);
+
+	// compose IBSS beacon frame
+	MgtMacHeaderInit(pAd, &BcnHdr, SUBTYPE_BEACON, 0, BROADCAST_ADDR, pAd->CommonCfg.Bssid);
+	Privacy = (pAd->StaCfg.WepStatus == Ndis802_11Encryption1Enabled) ||
+			  (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+			  (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled);
+	CapabilityInfo = CAP_GENERATE(0, 1, Privacy, (pAd->CommonCfg.TxPreamble == Rt802_11PreambleShort), 0, 0);
+
+	MakeOutgoingFrame(pBeaconFrame,                &FrameLen,
+					  sizeof(HEADER_802_11),           &BcnHdr,
+					  TIMESTAMP_LEN,                   &FakeTimestamp,
+					  2,                               &pAd->CommonCfg.BeaconPeriod,
+					  2,                               &CapabilityInfo,
+					  1,                               &SsidIe,
+					  1,                               &pAd->CommonCfg.SsidLen,
+					  pAd->CommonCfg.SsidLen,          pAd->CommonCfg.Ssid,
+					  1,                               &SupRateIe,
+					  1,                               &SupRateLen,
+					  SupRateLen,                      SupRate,
+					  1,                               &DsIe,
+					  1,                               &DsLen,
+					  1,                               &pAd->CommonCfg.Channel,
+					  1,                               &IbssIe,
+					  1,                               &IbssLen,
+					  2,                               &pAd->StaActive.AtimWin,
+					  END_OF_ARGS);
+
+	// add ERP_IE and EXT_RAE IE of in 802.11g
+	if (ExtRateLen)
+	{
+		ULONG	tmp;
+
+		MakeOutgoingFrame(pBeaconFrame + FrameLen,         &tmp,
+						  3,                               LocalErpIe,
+						  1,                               &ExtRateIe,
+						  1,                               &ExtRateLen,
+						  ExtRateLen,                      ExtRate,
+						  END_OF_ARGS);
+		FrameLen += tmp;
+	}
+
+	// If adhoc secruity is set for WPA-None, append the cipher suite IE
+	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+	{
+		ULONG tmp;
+        RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, BSS0);
+
+		MakeOutgoingFrame(pBeaconFrame + FrameLen,        	&tmp,
+						  1,                              	&RSNIe,
+						  1,                            	&pAd->StaCfg.RSNIE_Len,
+						  pAd->StaCfg.RSNIE_Len,      		pAd->StaCfg.RSN_IE,
+						  END_OF_ARGS);
+		FrameLen += tmp;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+	{
+		ULONG TmpLen;
+		UCHAR HtLen, HtLen1;
+
+#ifdef RT_BIG_ENDIAN
+		HT_CAPABILITY_IE HtCapabilityTmp;
+		ADD_HT_INFO_IE	addHTInfoTmp;
+		USHORT	b2lTmp, b2lTmp2;
+#endif
+
+		// add HT Capability IE
+		HtLen = sizeof(pAd->CommonCfg.HtCapability);
+		HtLen1 = sizeof(pAd->CommonCfg.AddHTInfo);
+#ifndef RT_BIG_ENDIAN
+		MakeOutgoingFrame(pBeaconFrame+FrameLen,	&TmpLen,
+						  1,						&HtCapIe,
+						  1,						&HtLen,
+						  HtLen,					&pAd->CommonCfg.HtCapability,
+						  1,						&AddHtInfoIe,
+						  1,						&HtLen1,
+						  HtLen1,					&pAd->CommonCfg.AddHTInfo,
+						  END_OF_ARGS);
+#else
+		NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, HtLen);
+		*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+		*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+		NdisMoveMemory(&addHTInfoTmp, &pAd->CommonCfg.AddHTInfo, HtLen1);
+		*(USHORT *)(&addHTInfoTmp.AddHtInfo2) = SWAP16(*(USHORT *)(&addHTInfoTmp.AddHtInfo2));
+		*(USHORT *)(&addHTInfoTmp.AddHtInfo3) = SWAP16(*(USHORT *)(&addHTInfoTmp.AddHtInfo3));
+
+		MakeOutgoingFrame(pBeaconFrame+FrameLen,	&TmpLen,
+						  1,						&HtCapIe,
+						  1,						&HtLen,
+						  HtLen,					&HtCapabilityTmp,
+						  1,						&AddHtInfoIe,
+						  1,						&HtLen1,
+						  HtLen1,					&addHTInfoTmp,
+						  END_OF_ARGS);
+#endif
+		FrameLen += TmpLen;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	//beacon use reserved WCID 0xff
+    if (pAd->CommonCfg.Channel > 14)
+    {
+	RTMPWriteTxWI(pAd, pTxWI, FALSE, FALSE,  TRUE, FALSE, FALSE, TRUE, 0, 0xff, FrameLen,
+		PID_MGMT, PID_BEACON, RATE_1, IFS_HTTXOP, FALSE, &pAd->CommonCfg.MlmeTransmit);
+    }
+    else
+    {
+        // Set to use 1Mbps for Adhoc beacon.
+		HTTRANSMIT_SETTING Transmit;
+        Transmit.word = 0;
+        RTMPWriteTxWI(pAd, pTxWI, FALSE, FALSE,  TRUE, FALSE, FALSE, TRUE, 0, 0xff, FrameLen,
+    		PID_MGMT, PID_BEACON, RATE_1, IFS_HTTXOP, FALSE, &Transmit);
+    }
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, pBeaconFrame, DIR_WRITE, FALSE);
+	RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+#endif
+
+    DBGPRINT(RT_DEBUG_TRACE, ("MakeIbssBeacon (len=%ld), SupRateLen=%d, ExtRateLen=%d, Channel=%d, PhyMode=%d\n",
+					FrameLen, SupRateLen, ExtRateLen, pAd->CommonCfg.Channel, pAd->CommonCfg.PhyMode));
+	return FrameLen;
+}
+
+
diff --git a/drivers/staging/rt2860/sta/dls.c b/drivers/staging/rt2860/sta/dls.c
new file mode 100644
index 000000000000..78fb28976fd7
--- /dev/null
+++ b/drivers/staging/rt2860/sta/dls.c
@@ -0,0 +1,2201 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    dls.c
+
+    Abstract:
+    Handle WMM-DLS state machine
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Rory Chen   02-14-2006
+	Arvin Tai	06-03-2008	  Modified for RT28xx
+ */
+
+#include "../rt_config.h"
+
+/*
+    ==========================================================================
+    Description:
+        dls state machine init, including state transition and timer init
+    Parameters:
+        Sm - pointer to the dls state machine
+    Note:
+        The state machine looks like this
+
+                            DLS_IDLE
+    MT2_MLME_DLS_REQUEST   MlmeDlsReqAction
+    MT2_PEER_DLS_REQUEST   PeerDlsReqAction
+    MT2_PEER_DLS_RESPONSE  PeerDlsRspAction
+    MT2_MLME_DLS_TEARDOWN  MlmeTearDownAction
+    MT2_PEER_DLS_TEARDOWN  PeerTearDownAction
+
+	IRQL = PASSIVE_LEVEL
+
+    ==========================================================================
+ */
+void DlsStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[])
+{
+	UCHAR	i;
+
+    StateMachineInit(Sm, (STATE_MACHINE_FUNC*)Trans, MAX_DLS_STATE, MAX_DLS_MSG, (STATE_MACHINE_FUNC)Drop, DLS_IDLE, DLS_MACHINE_BASE);
+
+    // the first column
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_MLME_DLS_REQ, (STATE_MACHINE_FUNC)MlmeDlsReqAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_PEER_DLS_REQ, (STATE_MACHINE_FUNC)PeerDlsReqAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_PEER_DLS_RSP, (STATE_MACHINE_FUNC)PeerDlsRspAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_MLME_DLS_TEAR_DOWN, (STATE_MACHINE_FUNC)MlmeDlsTearDownAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_PEER_DLS_TEAR_DOWN, (STATE_MACHINE_FUNC)PeerDlsTearDownAction);
+
+	for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		pAd->StaCfg.DLSEntry[i].pAd = pAd;
+		RTMPInitTimer(pAd, &pAd->StaCfg.DLSEntry[i].Timer, GET_TIMER_FUNCTION(DlsTimeoutAction), pAd, FALSE);
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDlsReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	HEADER_802_11	DlsReqHdr;
+	PRT_802_11_DLS	pDLS = NULL;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_REQUEST;
+	ULONG			tmp;
+	USHORT			reason;
+	ULONG			Timeout;
+	BOOLEAN			TimerCancelled;
+
+	if(!MlmeDlsReqSanity(pAd, Elem->Msg, Elem->MsgLen, &pDLS, &reason))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - MlmeDlsReqAction() \n"));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("DLS - MlmeDlsReqAction() allocate memory failed \n"));
+		return;
+	}
+
+	ActHeaderInit(pAd, &DlsReqHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+
+	// Build basic frame first
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+					sizeof(HEADER_802_11),		&DlsReqHdr,
+					1,							&Category,
+					1,							&Action,
+					6,							&pDLS->MacAddr,
+					6,							pAd->CurrentAddress,
+					2,							&pAd->StaActive.CapabilityInfo,
+					2,							&pDLS->TimeOut,
+					1,							&SupRateIe,
+					1,							&pAd->MlmeAux.SupRateLen,
+					pAd->MlmeAux.SupRateLen,	pAd->MlmeAux.SupRate,
+					END_OF_ARGS);
+
+	if (pAd->MlmeAux.ExtRateLen != 0)
+	{
+		MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+						  1,						&ExtRateIe,
+						  1,						&pAd->MlmeAux.ExtRateLen,
+						  pAd->MlmeAux.ExtRateLen,	pAd->MlmeAux.ExtRate,
+						  END_OF_ARGS);
+		FrameLen += tmp;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+	{
+		UCHAR HtLen;
+
+#ifdef RT_BIG_ENDIAN
+		HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+
+		// add HT Capability IE
+		HtLen = sizeof(HT_CAPABILITY_IE);
+#ifndef RT_BIG_ENDIAN
+		MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+							1,						&HtCapIe,
+							1,						&HtLen,
+							HtLen,					&pAd->CommonCfg.HtCapability,
+							END_OF_ARGS);
+#else
+		NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, HtLen);
+							*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+							*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+		MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+							1,						&HtCapIe,
+							1,						&HtLen,
+							HtLen,					&HtCapabilityTmp,
+							END_OF_ARGS);
+#endif
+		FrameLen = FrameLen + tmp;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	RTMPCancelTimer(&pDLS->Timer, &TimerCancelled);
+	Timeout = DLS_TIMEOUT;
+	RTMPSetTimer(&pDLS->Timer, Timeout);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerDlsReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	USHORT			StatusCode = MLME_SUCCESS;
+	HEADER_802_11	DlsRspHdr;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_RESPONSE;
+	ULONG			tmp;
+	USHORT			CapabilityInfo;
+	UCHAR			DA[MAC_ADDR_LEN], SA[MAC_ADDR_LEN];
+	USHORT			DLSTimeOut;
+	SHORT			i;
+	ULONG			Timeout;
+	BOOLEAN			TimerCancelled;
+	PRT_802_11_DLS	pDLS = NULL;
+	UCHAR			MaxSupportedRateIn500Kbps = 0;
+    UCHAR			SupportedRatesLen;
+    UCHAR			SupportedRates[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR			HtCapabilityLen;
+	HT_CAPABILITY_IE	HtCapability;
+
+	if (!PeerDlsReqSanity(pAd, Elem->Msg, Elem->MsgLen, DA, SA, &CapabilityInfo, &DLSTimeOut,
+							&SupportedRatesLen, &SupportedRates[0], &HtCapabilityLen, &HtCapability))
+		return;
+
+    // supported rates array may not be sorted. sort it and find the maximum rate
+    for (i = 0; i < SupportedRatesLen; i++)
+    {
+        if (MaxSupportedRateIn500Kbps < (SupportedRates[i] & 0x7f))
+            MaxSupportedRateIn500Kbps = SupportedRates[i] & 0x7f;
+    }
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsReqAction() from %02x:%02x:%02x:%02x:%02x:%02x\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsReqAction() allocate memory failed \n"));
+		return;
+	}
+
+	if (!INFRA_ON(pAd))
+	{
+		StatusCode = MLME_REQUEST_DECLINED;
+	}
+	else if (!pAd->CommonCfg.bWmmCapable)
+	{
+		StatusCode = MLME_DEST_STA_IS_NOT_A_QSTA;
+	}
+	else if (!pAd->CommonCfg.bDLSCapable)
+	{
+		StatusCode = MLME_REQUEST_DECLINED;
+	}
+	else
+	{
+		// find table to update parameters
+		for (i = (MAX_NUM_OF_DLS_ENTRY-1); i >= 0; i--)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+					pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+				else
+				{
+					RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+					pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+				}
+
+				pAd->StaCfg.DLSEntry[i].Sequence = 0;
+				pAd->StaCfg.DLSEntry[i].TimeOut = DLSTimeOut;
+				pAd->StaCfg.DLSEntry[i].CountDownTimer = DLSTimeOut;
+				if (HtCapabilityLen != 0)
+					pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+				else
+					pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+				pDLS = &pAd->StaCfg.DLSEntry[i];
+				break;
+			}
+		}
+
+		// can not find in table, create a new one
+		if (i < 0)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsReqAction() can not find same entry \n"));
+			for (i=(MAX_NUM_OF_DLS_ENTRY - 1); i >= MAX_NUM_OF_INIT_DLS_ENTRY; i--)
+			{
+				if (!pAd->StaCfg.DLSEntry[i].Valid)
+				{
+					MAC_TABLE_ENTRY *pEntry;
+					UCHAR MaxSupportedRate = RATE_11;
+
+					if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+					{
+						pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+					}
+					else
+					{
+						RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+						pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+					}
+
+					pAd->StaCfg.DLSEntry[i].Sequence = 0;
+					pAd->StaCfg.DLSEntry[i].Valid = TRUE;
+					pAd->StaCfg.DLSEntry[i].TimeOut = DLSTimeOut;
+					pAd->StaCfg.DLSEntry[i].CountDownTimer = DLSTimeOut;
+					NdisMoveMemory(pAd->StaCfg.DLSEntry[i].MacAddr, SA, MAC_ADDR_LEN);
+					if (HtCapabilityLen != 0)
+						pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+					else
+						pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+					pDLS = &pAd->StaCfg.DLSEntry[i];
+					pEntry = MacTableInsertDlsEntry(pAd, SA, i);
+
+					switch (MaxSupportedRateIn500Kbps)
+					{
+						case 108: MaxSupportedRate = RATE_54;   break;
+						case 96:  MaxSupportedRate = RATE_48;   break;
+						case 72:  MaxSupportedRate = RATE_36;   break;
+						case 48:  MaxSupportedRate = RATE_24;   break;
+						case 36:  MaxSupportedRate = RATE_18;   break;
+						case 24:  MaxSupportedRate = RATE_12;   break;
+						case 18:  MaxSupportedRate = RATE_9;    break;
+						case 12:  MaxSupportedRate = RATE_6;    break;
+						case 22:  MaxSupportedRate = RATE_11;   break;
+						case 11:  MaxSupportedRate = RATE_5_5;  break;
+						case 4:   MaxSupportedRate = RATE_2;    break;
+						case 2:   MaxSupportedRate = RATE_1;    break;
+						default:  MaxSupportedRate = RATE_11;   break;
+					}
+
+					pEntry->MaxSupportedRate = min(pAd->CommonCfg.MaxTxRate, MaxSupportedRate);
+
+					if (pEntry->MaxSupportedRate < RATE_FIRST_OFDM_RATE)
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MaxHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->MinHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MinHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->HTPhyMode.field.MODE = MODE_CCK;
+						pEntry->HTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MaxHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->MinHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MinHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					}
+
+					pEntry->MaxHTPhyMode.field.BW = BW_20;
+					pEntry->MinHTPhyMode.field.BW = BW_20;
+
+#ifdef DOT11_N_SUPPORT
+					pEntry->HTCapability.MCSSet[0] = 0;
+					pEntry->HTCapability.MCSSet[1] = 0;
+
+					// If this Entry supports 802.11n, upgrade to HT rate.
+					if ((HtCapabilityLen != 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+					{
+						UCHAR	j, bitmask; //k,bitmask;
+						CHAR    ii;
+
+						DBGPRINT(RT_DEBUG_OFF, ("DLS - PeerDlsReqAction() Receive Peer HT Capable STA from %02x:%02x:%02x:%02x:%02x:%02x\n",
+									SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+						if ((HtCapability.HtCapInfo.GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTGREENFIELD;
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+							pAd->MacTab.fAnyStationNonGF = TRUE;
+							pAd->CommonCfg.AddHTInfo.AddHtInfo2.NonGfPresent = 1;
+						}
+
+						if ((HtCapability.HtCapInfo.ChannelWidth) && (pAd->CommonCfg.DesiredHtPhy.ChannelWidth))
+						{
+							pEntry->MaxHTPhyMode.field.BW= BW_40;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor40)&(HtCapability.HtCapInfo.ShortGIfor40));
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.BW = BW_20;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor20)&(HtCapability.HtCapInfo.ShortGIfor20));
+							pAd->MacTab.fAnyStation20Only = TRUE;
+						}
+
+						// find max fixed rate
+						for (ii=15; ii>=0; ii--)
+						{
+							j = ii/8;
+							bitmask = (1<<(ii-(j*8)));
+							if ( (pAd->StaCfg.DesiredHtPhyInfo.MCSSet[j]&bitmask) && (HtCapability.MCSSet[j]&bitmask))
+							{
+								pEntry->MaxHTPhyMode.field.MCS = ii;
+								break;
+							}
+							if (ii==0)
+								break;
+						}
+
+
+						if (pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO)
+						{
+
+							printk("@@@ pAd->CommonCfg.RegTransmitSetting.field.MCS = %d\n",
+								pAd->StaCfg.DesiredTransmitSetting.field.MCS);
+							if (pAd->StaCfg.DesiredTransmitSetting.field.MCS == 32)
+							{
+								// Fix MCS as HT Duplicated Mode
+								pEntry->MaxHTPhyMode.field.BW = 1;
+								pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+								pEntry->MaxHTPhyMode.field.STBC = 0;
+								pEntry->MaxHTPhyMode.field.ShortGI = 0;
+								pEntry->MaxHTPhyMode.field.MCS = 32;
+							}
+							else if (pEntry->MaxHTPhyMode.field.MCS > pAd->StaCfg.HTPhyMode.field.MCS)
+							{
+								// STA supports fixed MCS
+								pEntry->MaxHTPhyMode.field.MCS = pAd->StaCfg.HTPhyMode.field.MCS;
+							}
+						}
+
+						pEntry->MaxHTPhyMode.field.STBC = (HtCapability.HtCapInfo.RxSTBC & (pAd->CommonCfg.DesiredHtPhy.TxSTBC));
+						pEntry->MpduDensity = HtCapability.HtCapParm.MpduDensity;
+						pEntry->MaxRAmpduFactor = HtCapability.HtCapParm.MaxRAmpduFactor;
+						pEntry->MmpsMode = (UCHAR)HtCapability.HtCapInfo.MimoPs;
+						pEntry->AMsduSize = (UCHAR)HtCapability.HtCapInfo.AMsduSize;
+						pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+
+						if (HtCapability.HtCapInfo.ShortGIfor20)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE);
+						if (HtCapability.HtCapInfo.ShortGIfor40)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE);
+						if (HtCapability.HtCapInfo.TxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_TxSTBC_CAPABLE);
+						if (HtCapability.HtCapInfo.RxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RxSTBC_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.PlusHTC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_HTC_CAPABLE);
+						if (pAd->CommonCfg.bRdg && HtCapability.ExtHtCapInfo.RDGSupport)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RDG_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.MCSFeedback == 0x03)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_MCSFEEDBACK_CAPABLE);
+
+						NdisMoveMemory(&pEntry->HTCapability, &HtCapability, sizeof(HT_CAPABILITY_IE));
+					}
+#endif // DOT11_N_SUPPORT //
+
+					pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+					pEntry->CurrTxRate = pEntry->MaxSupportedRate;
+					CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+
+					if (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+					{
+						PUCHAR pTable;
+						UCHAR TableSize = 0;
+
+						MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &pEntry->CurrTxRateIndex);
+						pEntry->bAutoTxRateSwitch = TRUE;
+					}
+					else
+					{
+						pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+						pEntry->HTPhyMode.field.MCS	= pAd->StaCfg.HTPhyMode.field.MCS;
+						pEntry->bAutoTxRateSwitch = FALSE;
+
+						RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+					}
+					pEntry->RateLen = SupportedRatesLen;
+
+					break;
+				}
+			}
+		}
+		StatusCode = MLME_SUCCESS;
+
+		// can not find in table, create a new one
+		if (i < 0)
+		{
+			StatusCode = MLME_QOS_UNSPECIFY;
+			DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsReqAction() DLSEntry table full(only can support %d DLS session) \n", MAX_NUM_OF_DLS_ENTRY - MAX_NUM_OF_INIT_DLS_ENTRY));
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsReqAction() use entry(%d) %02x:%02x:%02x:%02x:%02x:%02x\n",
+				i, SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+		}
+	}
+
+	ActHeaderInit(pAd, &DlsRspHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+
+	// Build basic frame first
+	if (StatusCode == MLME_SUCCESS)
+	{
+		MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+						sizeof(HEADER_802_11),		&DlsRspHdr,
+						1,							&Category,
+						1,							&Action,
+						2,							&StatusCode,
+						6,							SA,
+						6,							pAd->CurrentAddress,
+						2,							&pAd->StaActive.CapabilityInfo,
+						1,							&SupRateIe,
+						1,							&pAd->MlmeAux.SupRateLen,
+						pAd->MlmeAux.SupRateLen,	pAd->MlmeAux.SupRate,
+						END_OF_ARGS);
+
+		if (pAd->MlmeAux.ExtRateLen != 0)
+		{
+			MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+							  1,						&ExtRateIe,
+							  1,						&pAd->MlmeAux.ExtRateLen,
+							  pAd->MlmeAux.ExtRateLen, 	pAd->MlmeAux.ExtRate,
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+#ifdef DOT11_N_SUPPORT
+		if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+		{
+			UCHAR HtLen;
+
+#ifdef RT_BIG_ENDIAN
+			HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+
+			// add HT Capability IE
+			HtLen = sizeof(HT_CAPABILITY_IE);
+#ifndef RT_BIG_ENDIAN
+			MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+								1,						&HtCapIe,
+								1,						&HtLen,
+								HtLen,					&pAd->CommonCfg.HtCapability,
+								END_OF_ARGS);
+#else
+			NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, HtLen);
+								*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+								*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+								1,						&HtCapIe,
+								1,						&HtLen,
+								HtLen,					&HtCapabilityTmp,
+								END_OF_ARGS);
+#endif
+			FrameLen = FrameLen + tmp;
+		}
+#endif // DOT11_N_SUPPORT //
+
+		if (pDLS && (pDLS->Status != DLS_FINISH))
+		{
+			RTMPCancelTimer(&pDLS->Timer, &TimerCancelled);
+			Timeout = DLS_TIMEOUT;
+			RTMPSetTimer(&pDLS->Timer, Timeout);
+		}
+	}
+	else
+	{
+		MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+						sizeof(HEADER_802_11),		&DlsRspHdr,
+						1,							&Category,
+						1,							&Action,
+						2,							&StatusCode,
+						6,							SA,
+						6,							pAd->CurrentAddress,
+						END_OF_ARGS);
+	}
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerDlsRspAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT		CapabilityInfo;
+	UCHAR		DA[MAC_ADDR_LEN], SA[MAC_ADDR_LEN];
+	USHORT		StatusCode;
+	SHORT		i;
+	BOOLEAN		TimerCancelled;
+	UCHAR		MaxSupportedRateIn500Kbps = 0;
+    UCHAR		SupportedRatesLen;
+    UCHAR		SupportedRates[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		HtCapabilityLen;
+	HT_CAPABILITY_IE	HtCapability;
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return;
+
+	if (!INFRA_ON(pAd))
+		return;
+
+	if (!PeerDlsRspSanity(pAd, Elem->Msg, Elem->MsgLen, DA, SA, &CapabilityInfo, &StatusCode,
+							&SupportedRatesLen, &SupportedRates[0], &HtCapabilityLen, &HtCapability))
+		return;
+
+    // supported rates array may not be sorted. sort it and find the maximum rate
+    for (i=0; i<SupportedRatesLen; i++)
+    {
+        if (MaxSupportedRateIn500Kbps < (SupportedRates[i] & 0x7f))
+            MaxSupportedRateIn500Kbps = SupportedRates[i] & 0x7f;
+    }
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() from %02x:%02x:%02x:%02x:%02x:%02x with StatusCode=%d, CapabilityInfo=0x%x\n",
+		SA[0], SA[1], SA[2], SA[3], SA[4], SA[5], StatusCode, CapabilityInfo));
+
+	for (i = 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			if (StatusCode == MLME_SUCCESS)
+			{
+				MAC_TABLE_ENTRY *pEntry;
+				UCHAR MaxSupportedRate = RATE_11;
+
+				pEntry = MacTableInsertDlsEntry(pAd, SA, i);
+
+				switch (MaxSupportedRateIn500Kbps)
+				{
+					case 108: MaxSupportedRate = RATE_54;   break;
+					case 96:  MaxSupportedRate = RATE_48;   break;
+					case 72:  MaxSupportedRate = RATE_36;   break;
+					case 48:  MaxSupportedRate = RATE_24;   break;
+					case 36:  MaxSupportedRate = RATE_18;   break;
+					case 24:  MaxSupportedRate = RATE_12;   break;
+					case 18:  MaxSupportedRate = RATE_9;    break;
+					case 12:  MaxSupportedRate = RATE_6;    break;
+					case 22:  MaxSupportedRate = RATE_11;   break;
+					case 11:  MaxSupportedRate = RATE_5_5;  break;
+					case 4:   MaxSupportedRate = RATE_2;    break;
+					case 2:   MaxSupportedRate = RATE_1;    break;
+					default:  MaxSupportedRate = RATE_11;   break;
+				}
+
+				pEntry->MaxSupportedRate = min(pAd->CommonCfg.MaxTxRate, MaxSupportedRate);
+
+				if (pEntry->MaxSupportedRate < RATE_FIRST_OFDM_RATE)
+				{
+					pEntry->MaxHTPhyMode.field.MODE = MODE_CCK;
+					pEntry->MaxHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					pEntry->MinHTPhyMode.field.MODE = MODE_CCK;
+					pEntry->MinHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					pEntry->HTPhyMode.field.MODE = MODE_CCK;
+					pEntry->HTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+				}
+				else
+				{
+					pEntry->MaxHTPhyMode.field.MODE = MODE_OFDM;
+					pEntry->MaxHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					pEntry->MinHTPhyMode.field.MODE = MODE_OFDM;
+					pEntry->MinHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+					pEntry->HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+				}
+
+				pEntry->MaxHTPhyMode.field.BW = BW_20;
+				pEntry->MinHTPhyMode.field.BW = BW_20;
+
+#ifdef DOT11_N_SUPPORT
+				pEntry->HTCapability.MCSSet[0] = 0;
+				pEntry->HTCapability.MCSSet[1] = 0;
+
+				// If this Entry supports 802.11n, upgrade to HT rate.
+				if ((HtCapabilityLen != 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+				{
+					UCHAR	j, bitmask; //k,bitmask;
+					CHAR    ii;
+
+					DBGPRINT(RT_DEBUG_OFF, ("DLS - PeerDlsRspAction Receive Peer HT Capable STA from %02x:%02x:%02x:%02x:%02x:%02x\n",
+								SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+					if ((HtCapability.HtCapInfo.GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_HTGREENFIELD;
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+						pAd->MacTab.fAnyStationNonGF = TRUE;
+						pAd->CommonCfg.AddHTInfo.AddHtInfo2.NonGfPresent = 1;
+					}
+
+					if ((HtCapability.HtCapInfo.ChannelWidth) && (pAd->CommonCfg.DesiredHtPhy.ChannelWidth))
+					{
+						pEntry->MaxHTPhyMode.field.BW= BW_40;
+						pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor40)&(HtCapability.HtCapInfo.ShortGIfor40));
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.BW = BW_20;
+						pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor20)&(HtCapability.HtCapInfo.ShortGIfor20));
+						pAd->MacTab.fAnyStation20Only = TRUE;
+					}
+
+					// find max fixed rate
+					for (ii=15; ii>=0; ii--)
+					{
+						j = ii/8;
+						bitmask = (1<<(ii-(j*8)));
+						if ( (pAd->StaCfg.DesiredHtPhyInfo.MCSSet[j]&bitmask) && (HtCapability.MCSSet[j]&bitmask))
+						{
+							pEntry->MaxHTPhyMode.field.MCS = ii;
+							break;
+						}
+						if (ii==0)
+							break;
+					}
+
+					if (pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO)
+					{
+						if (pAd->StaCfg.DesiredTransmitSetting.field.MCS == 32)
+						{
+							// Fix MCS as HT Duplicated Mode
+							pEntry->MaxHTPhyMode.field.BW = 1;
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+							pEntry->MaxHTPhyMode.field.STBC = 0;
+							pEntry->MaxHTPhyMode.field.ShortGI = 0;
+							pEntry->MaxHTPhyMode.field.MCS = 32;
+						}
+						else if (pEntry->MaxHTPhyMode.field.MCS > pAd->StaCfg.HTPhyMode.field.MCS)
+						{
+							// STA supports fixed MCS
+							pEntry->MaxHTPhyMode.field.MCS = pAd->StaCfg.HTPhyMode.field.MCS;
+						}
+					}
+
+					pEntry->MaxHTPhyMode.field.STBC = (HtCapability.HtCapInfo.RxSTBC & (pAd->CommonCfg.DesiredHtPhy.TxSTBC));
+					pEntry->MpduDensity = HtCapability.HtCapParm.MpduDensity;
+					pEntry->MaxRAmpduFactor = HtCapability.HtCapParm.MaxRAmpduFactor;
+					pEntry->MmpsMode = (UCHAR)HtCapability.HtCapInfo.MimoPs;
+					pEntry->AMsduSize = (UCHAR)HtCapability.HtCapInfo.AMsduSize;
+					pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+
+					if (HtCapability.HtCapInfo.ShortGIfor20)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE);
+					if (HtCapability.HtCapInfo.ShortGIfor40)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE);
+					if (HtCapability.HtCapInfo.TxSTBC)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_TxSTBC_CAPABLE);
+					if (HtCapability.HtCapInfo.RxSTBC)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RxSTBC_CAPABLE);
+					if (HtCapability.ExtHtCapInfo.PlusHTC)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_HTC_CAPABLE);
+					if (pAd->CommonCfg.bRdg && HtCapability.ExtHtCapInfo.RDGSupport)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RDG_CAPABLE);
+					if (HtCapability.ExtHtCapInfo.MCSFeedback == 0x03)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_MCSFEEDBACK_CAPABLE);
+
+					NdisMoveMemory(&pEntry->HTCapability, &HtCapability, sizeof(HT_CAPABILITY_IE));
+				}
+#endif // DOT11_N_SUPPORT //
+				pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+				pEntry->CurrTxRate = pEntry->MaxSupportedRate;
+				CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+
+				if (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+				{
+					PUCHAR pTable;
+					UCHAR TableSize = 0;
+
+					MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &pEntry->CurrTxRateIndex);
+					pEntry->bAutoTxRateSwitch = TRUE;
+				}
+				else
+				{
+					pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+					pEntry->HTPhyMode.field.MCS	= pAd->StaCfg.HTPhyMode.field.MCS;
+					pEntry->bAutoTxRateSwitch = FALSE;
+
+					RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+				}
+				pEntry->RateLen = SupportedRatesLen;
+
+				if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+				{
+					// If support WPA or WPA2, start STAKey hand shake,
+					// If failed hand shake, just tear down peer DLS
+					if (RTMPSendSTAKeyRequest(pAd, pAd->StaCfg.DLSEntry[i].MacAddr) != NDIS_STATUS_SUCCESS)
+					{
+						MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+						USHORT				reason = REASON_QOS_CIPHER_NOT_SUPPORT;
+
+						DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+						MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+						pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+						pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+						DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed when call RTMPSendSTAKeyRequest \n"));
+					}
+					else
+					{
+						pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+						DBGPRINT(RT_DEBUG_TRACE,("DLS - waiting for STAKey handshake procedure\n"));
+					}
+				}
+				else
+				{
+					RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+					pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+					DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() from %02x:%02x:%02x:%02x:%02x:%02x Succeed with WEP or no security\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+				}
+
+				//initialize seq no for DLS frames.
+				pAd->StaCfg.DLSEntry[i].Sequence = 0;
+				if (HtCapabilityLen != 0)
+					pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+				else
+					pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+			}
+			else
+			{
+				// DLS setup procedure failed.
+				pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+				DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed with StatusCode=%d \n", StatusCode));
+			}
+		}
+	}
+
+	if (i >= MAX_NUM_OF_INIT_DLS_ENTRY)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() update timeout value \n"));
+		for (i=(MAX_NUM_OF_DLS_ENTRY-1); i>=MAX_NUM_OF_INIT_DLS_ENTRY; i--)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				if (StatusCode == MLME_SUCCESS)
+				{
+					MAC_TABLE_ENTRY *pEntry;
+					UCHAR MaxSupportedRate = RATE_11;
+
+					pEntry = MacTableInsertDlsEntry(pAd, SA, i);
+
+					switch (MaxSupportedRateIn500Kbps)
+					{
+						case 108: MaxSupportedRate = RATE_54;   break;
+						case 96:  MaxSupportedRate = RATE_48;   break;
+						case 72:  MaxSupportedRate = RATE_36;   break;
+						case 48:  MaxSupportedRate = RATE_24;   break;
+						case 36:  MaxSupportedRate = RATE_18;   break;
+						case 24:  MaxSupportedRate = RATE_12;   break;
+						case 18:  MaxSupportedRate = RATE_9;    break;
+						case 12:  MaxSupportedRate = RATE_6;    break;
+						case 22:  MaxSupportedRate = RATE_11;   break;
+						case 11:  MaxSupportedRate = RATE_5_5;  break;
+						case 4:   MaxSupportedRate = RATE_2;    break;
+						case 2:   MaxSupportedRate = RATE_1;    break;
+						default:  MaxSupportedRate = RATE_11;   break;
+					}
+
+					pEntry->MaxSupportedRate = min(pAd->CommonCfg.MaxTxRate, MaxSupportedRate);
+
+					if (pEntry->MaxSupportedRate < RATE_FIRST_OFDM_RATE)
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MaxHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->MinHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MinHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->HTPhyMode.field.MODE = MODE_CCK;
+						pEntry->HTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MaxHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->MinHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MinHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					}
+
+					pEntry->MaxHTPhyMode.field.BW = BW_20;
+					pEntry->MinHTPhyMode.field.BW = BW_20;
+
+#ifdef DOT11_N_SUPPORT
+					pEntry->HTCapability.MCSSet[0] = 0;
+					pEntry->HTCapability.MCSSet[1] = 0;
+
+					// If this Entry supports 802.11n, upgrade to HT rate.
+					if ((HtCapabilityLen != 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+					{
+						UCHAR	j, bitmask; //k,bitmask;
+						CHAR    ii;
+
+						DBGPRINT(RT_DEBUG_OFF, ("DLS - PeerDlsRspAction Receive Peer HT Capable STA from %02x:%02x:%02x:%02x:%02x:%02x\n",
+									SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+						if ((HtCapability.HtCapInfo.GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTGREENFIELD;
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+							pAd->MacTab.fAnyStationNonGF = TRUE;
+							pAd->CommonCfg.AddHTInfo.AddHtInfo2.NonGfPresent = 1;
+						}
+
+						if ((HtCapability.HtCapInfo.ChannelWidth) && (pAd->CommonCfg.DesiredHtPhy.ChannelWidth))
+						{
+							pEntry->MaxHTPhyMode.field.BW= BW_40;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor40)&(HtCapability.HtCapInfo.ShortGIfor40));
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.BW = BW_20;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor20)&(HtCapability.HtCapInfo.ShortGIfor20));
+							pAd->MacTab.fAnyStation20Only = TRUE;
+						}
+
+						// find max fixed rate
+						for (ii=15; ii>=0; ii--)
+						{
+							j = ii/8;
+							bitmask = (1<<(ii-(j*8)));
+							if ( (pAd->StaCfg.DesiredHtPhyInfo.MCSSet[j]&bitmask) && (HtCapability.MCSSet[j]&bitmask))
+							{
+								pEntry->MaxHTPhyMode.field.MCS = ii;
+								break;
+							}
+							if (ii==0)
+								break;
+						}
+
+						if (pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO)
+						{
+							printk("@@@ pAd->CommonCfg.RegTransmitSetting.field.MCS = %d\n",
+								pAd->StaCfg.DesiredTransmitSetting.field.MCS);
+							if (pAd->StaCfg.DesiredTransmitSetting.field.MCS == 32)
+							{
+								// Fix MCS as HT Duplicated Mode
+								pEntry->MaxHTPhyMode.field.BW = 1;
+								pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+								pEntry->MaxHTPhyMode.field.STBC = 0;
+								pEntry->MaxHTPhyMode.field.ShortGI = 0;
+								pEntry->MaxHTPhyMode.field.MCS = 32;
+							}
+							else if (pEntry->MaxHTPhyMode.field.MCS > pAd->StaCfg.HTPhyMode.field.MCS)
+							{
+								// STA supports fixed MCS
+								pEntry->MaxHTPhyMode.field.MCS = pAd->StaCfg.HTPhyMode.field.MCS;
+							}
+						}
+
+						pEntry->MaxHTPhyMode.field.STBC = (HtCapability.HtCapInfo.RxSTBC & (pAd->CommonCfg.DesiredHtPhy.TxSTBC));
+						pEntry->MpduDensity = HtCapability.HtCapParm.MpduDensity;
+						pEntry->MaxRAmpduFactor = HtCapability.HtCapParm.MaxRAmpduFactor;
+						pEntry->MmpsMode = (UCHAR)HtCapability.HtCapInfo.MimoPs;
+						pEntry->AMsduSize = (UCHAR)HtCapability.HtCapInfo.AMsduSize;
+						pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+
+						if (HtCapability.HtCapInfo.ShortGIfor20)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE);
+						if (HtCapability.HtCapInfo.ShortGIfor40)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE);
+						if (HtCapability.HtCapInfo.TxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_TxSTBC_CAPABLE);
+						if (HtCapability.HtCapInfo.RxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RxSTBC_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.PlusHTC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_HTC_CAPABLE);
+						if (pAd->CommonCfg.bRdg && HtCapability.ExtHtCapInfo.RDGSupport)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RDG_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.MCSFeedback == 0x03)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_MCSFEEDBACK_CAPABLE);
+
+						NdisMoveMemory(&pEntry->HTCapability, &HtCapability, sizeof(HT_CAPABILITY_IE));
+					}
+#endif // DOT11_N_SUPPORT //
+
+					pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+					pEntry->CurrTxRate = pEntry->MaxSupportedRate;
+					CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+
+					if (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+					{
+						PUCHAR pTable;
+						UCHAR TableSize = 0;
+
+						MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &pEntry->CurrTxRateIndex);
+						pEntry->bAutoTxRateSwitch = TRUE;
+					}
+					else
+					{
+						pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+						pEntry->HTPhyMode.field.MCS	= pAd->StaCfg.HTPhyMode.field.MCS;
+						pEntry->bAutoTxRateSwitch = FALSE;
+
+						RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+					}
+					pEntry->RateLen = SupportedRatesLen;
+
+					if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+					{
+						// If support WPA or WPA2, start STAKey hand shake,
+						// If failed hand shake, just tear down peer DLS
+						if (RTMPSendSTAKeyRequest(pAd, pAd->StaCfg.DLSEntry[i].MacAddr) != NDIS_STATUS_SUCCESS)
+						{
+							MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+							USHORT				reason = REASON_QOS_CIPHER_NOT_SUPPORT;
+
+							DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+							MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+							pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+							pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+							DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed when call RTMPSendSTAKeyRequest \n"));
+						}
+						else
+						{
+							pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+							DBGPRINT(RT_DEBUG_TRACE,("DLS - waiting for STAKey handshake procedure\n"));
+						}
+					}
+					else
+					{
+						RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+						pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+						DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() from %02x:%02x:%02x:%02x:%02x:%02x Succeed with WEP or no security\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+					}
+					pAd->StaCfg.DLSEntry[i].Sequence = 0;
+					if (HtCapabilityLen != 0)
+						pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+					else
+						pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+				}
+				else
+				{
+					// DLS setup procedure failed.
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+					RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+					DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed with StatusCode=%d \n", StatusCode));
+				}
+			}
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDlsTearDownAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_TEARDOWN;
+	USHORT			ReasonCode = REASON_QOS_UNSPECIFY;
+	HEADER_802_11	DlsTearDownHdr;
+	PRT_802_11_DLS	pDLS;
+	BOOLEAN			TimerCancelled;
+	UCHAR			i;
+
+	if(!MlmeDlsReqSanity(pAd, Elem->Msg, Elem->MsgLen, &pDLS, &ReasonCode))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - MlmeDlsTearDownAction() with ReasonCode=%d \n", ReasonCode));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("DLS - MlmeDlsTearDownAction() allocate memory failed \n"));
+		return;
+	}
+
+	ActHeaderInit(pAd, &DlsTearDownHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+
+	// Build basic frame first
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+					sizeof(HEADER_802_11),		&DlsTearDownHdr,
+					1,							&Category,
+					1,							&Action,
+					6,							&pDLS->MacAddr,
+					6,							pAd->CurrentAddress,
+					2,							&ReasonCode,
+					END_OF_ARGS);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+	RTMPCancelTimer(&pDLS->Timer, &TimerCancelled);
+
+	// Remove key in local dls table entry
+	for (i = 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	// clear peer dls table entry
+	for (i = MAX_NUM_OF_INIT_DLS_ENTRY; i < MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+			pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerDlsTearDownAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR			DA[MAC_ADDR_LEN], SA[MAC_ADDR_LEN];
+	USHORT			ReasonCode;
+	UINT			i;
+	BOOLEAN			TimerCancelled;
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return;
+
+	if (!INFRA_ON(pAd))
+		return;
+
+	if (!PeerDlsTearDownSanity(pAd, Elem->Msg, Elem->MsgLen, DA, SA, &ReasonCode))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsTearDownAction() from %02x:%02x:%02x:%02x:%02x:%02x with ReasonCode=%d\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5], ReasonCode));
+
+	// clear local dls table entry
+	for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+			pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+			//AsicDelWcidTab(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			//AsicRemovePairwiseKeyEntry(pAd, BSS0, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	// clear peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+			pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+			//AsicDelWcidTab(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			//AsicRemovePairwiseKeyEntry(pAd, BSS0, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID RTMPCheckDLSTimeOut(
+	IN PRTMP_ADAPTER	pAd)
+{
+	ULONG				i;
+	MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+	USHORT				reason = REASON_QOS_UNSPECIFY;
+
+	if (! pAd->CommonCfg.bDLSCapable)
+		return;
+
+	if (! INFRA_ON(pAd))
+		return;
+
+	// If timeout value is equaled to zero, it means always not be timeout.
+
+	// update local dls table entry
+	for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if ((pAd->StaCfg.DLSEntry[i].Valid) && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& (pAd->StaCfg.DLSEntry[i].TimeOut != 0))
+		{
+			pAd->StaCfg.DLSEntry[i].CountDownTimer --;
+
+			if (pAd->StaCfg.DLSEntry[i].CountDownTimer == 0)
+			{
+				reason = REASON_QOS_REQUEST_TIMEOUT;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+			}
+		}
+	}
+
+	// update peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if ((pAd->StaCfg.DLSEntry[i].Valid) && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& (pAd->StaCfg.DLSEntry[i].TimeOut != 0))
+		{
+			pAd->StaCfg.DLSEntry[i].CountDownTimer --;
+
+			if (pAd->StaCfg.DLSEntry[i].CountDownTimer == 0)
+			{
+				reason = REASON_QOS_REQUEST_TIMEOUT;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+			}
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN RTMPRcvFrameDLSCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN PHEADER_802_11	pHeader,
+	IN ULONG			Len,
+	IN PRT28XX_RXD_STRUC	pRxD)
+{
+	ULONG			i;
+	BOOLEAN			bFindEntry = FALSE;
+	BOOLEAN			bSTAKeyFrame = FALSE;
+	PEAPOL_PACKET	pEap;
+	PUCHAR			pProto, pAddr = NULL;
+	PUCHAR			pSTAKey = NULL;
+	UCHAR			ZeroReplay[LEN_KEY_DESC_REPLAY];
+	UCHAR			Mic[16], OldMic[16];
+	UCHAR			digest[80];
+	UCHAR			DlsPTK[80];
+	UCHAR			temp[64];
+	BOOLEAN			TimerCancelled;
+	CIPHER_KEY		PairwiseKey;
+
+
+	if (! pAd->CommonCfg.bDLSCapable)
+		return bSTAKeyFrame;
+
+	if (! INFRA_ON(pAd))
+		return bSTAKeyFrame;
+
+	if (! (pHeader->FC.SubType & 0x08))
+		return bSTAKeyFrame;
+
+	if (Len < LENGTH_802_11 + 6 + 2 + 2)
+		return bSTAKeyFrame;
+
+	pProto	= (PUCHAR)pHeader + LENGTH_802_11 + 2 + 6;	// QOS Control field , 0xAA 0xAA 0xAA 0x00 0x00 0x00
+	pAddr	= pHeader->Addr2;
+
+	// L2PAD bit on will pad 2 bytes at LLC
+	if (pRxD->L2PAD)
+	{
+		pProto += 2;
+	}
+
+	if (RTMPEqualMemory(EAPOL, pProto, 2) && (pAd->StaCfg.AuthMode >=  Ndis802_11AuthModeWPA))
+	{
+		pEap = (PEAPOL_PACKET) (pProto + 2);
+
+		DBGPRINT(RT_DEBUG_TRACE,("DLS - Sniff Len=%ld, DataLen=%d, KeyMic=%d, Install=%d, KeyAck=%d, Secure=%d, EKD_DL=%d, Error=%d, Request=%d\n", Len,
+			                                                             (LENGTH_802_11 + 6 + 2 + 2 + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE + 16),
+			                                                             pEap->KeyDesc.KeyInfo.KeyMic,
+			                                                             pEap->KeyDesc.KeyInfo.Install,
+			                                                             pEap->KeyDesc.KeyInfo.KeyAck,
+			                                                             pEap->KeyDesc.KeyInfo.Secure,
+			                                                             pEap->KeyDesc.KeyInfo.EKD_DL,
+			                                                             pEap->KeyDesc.KeyInfo.Error,
+			                                                             pEap->KeyDesc.KeyInfo.Request));
+
+		if ((Len >= (LENGTH_802_11 + 6 + 2 + 2 + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE + 16)) && pEap->KeyDesc.KeyInfo.KeyMic
+			&& pEap->KeyDesc.KeyInfo.Install && pEap->KeyDesc.KeyInfo.KeyAck && pEap->KeyDesc.KeyInfo.Secure
+			&& pEap->KeyDesc.KeyInfo.EKD_DL && !pEap->KeyDesc.KeyInfo.Error && !pEap->KeyDesc.KeyInfo.Request)
+		{
+			// First validate replay counter, only accept message with larger replay counter
+			// Let equal pass, some AP start with all zero replay counter
+			NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);
+			if ((RTMPCompareMemory(pEap->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1) &&
+				(RTMPCompareMemory(pEap->KeyDesc.ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
+				return bSTAKeyFrame;
+
+			//RTMPMoveMemory(pAd->StaCfg.ReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+			RTMPMoveMemory(pAd->StaCfg.DlsReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - Sniff replay counter (%02x-%02x-%02x-%02x-%02x-%02x-%02x-%02x) Len=%ld, KeyDataLen=%d\n",
+				pAd->StaCfg.ReplayCounter[0], pAd->StaCfg.ReplayCounter[1], pAd->StaCfg.ReplayCounter[2],
+				pAd->StaCfg.ReplayCounter[3], pAd->StaCfg.ReplayCounter[4],	pAd->StaCfg.ReplayCounter[5],
+				pAd->StaCfg.ReplayCounter[6], pAd->StaCfg.ReplayCounter[7], Len, pEap->KeyDesc.KeyData[1]));
+
+			// put these code segment to get the replay counter
+			if (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED)
+				return bSTAKeyFrame;
+
+			// Check MIC value
+			// Save the MIC and replace with zero
+			// use proprietary PTK
+			NdisZeroMemory(temp, 64);
+			NdisMoveMemory(temp, "IEEE802.11 WIRELESS ACCESS POINT", 32);
+			WpaCountPTK(pAd, temp, temp, pAd->CommonCfg.Bssid, temp, pAd->CurrentAddress, DlsPTK, LEN_PTK);
+
+			NdisMoveMemory(OldMic, pEap->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+			NdisZeroMemory(pEap->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+			if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+			{
+				// AES
+				HMAC_SHA1((PUCHAR) pEap, pEap->Body_Len[1] + 4, DlsPTK, LEN_EAP_MICK, digest);
+				NdisMoveMemory(Mic,	digest,	LEN_KEY_DESC_MIC);
+			}
+			else
+			{
+				hmac_md5(DlsPTK, LEN_EAP_MICK, (PUCHAR) pEap, pEap->Body_Len[1] + 4, Mic);
+			}
+
+			if (!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("MIC Different in Msg1 of STAKey handshake! \n"));
+				return bSTAKeyFrame;
+			}
+			else
+				DBGPRINT(RT_DEBUG_TRACE, ("MIC VALID in Msg1 of STAKey handshake! \n"));
+#if 1
+			if ((pEap->KeyDesc.KeyData[0] == 0xDD) && (pEap->KeyDesc.KeyData[2] == 0x00) && (pEap->KeyDesc.KeyData[3] == 0x0C)
+				&& (pEap->KeyDesc.KeyData[4] == 0x43) && (pEap->KeyDesc.KeyData[5] == 0x02))
+			{
+				pAddr			= pEap->KeyDesc.KeyData + 8;		// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2)
+				pSTAKey			= pEap->KeyDesc.KeyData + 14;	// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2), STAKey_Mac_Addr(6)
+
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 from %02x:%02x:%02x:%02x:%02x:%02x Len=%ld, KeyDataLen=%d\n",
+					pAddr[0], pAddr[1], pAddr[2], pAddr[3], pAddr[4], pAddr[5], Len, pEap->KeyDesc.KeyData[1]));
+
+				bSTAKeyFrame = TRUE;
+			}
+#else
+			if ((pEap->KeyDesc.KeyData[0] == 0xDD) && (pEap->KeyDesc.KeyData[2] == 0x00) && (pEap->KeyDesc.KeyData[3] == 0x0F)
+				&& (pEap->KeyDesc.KeyData[4] == 0xAC) && (pEap->KeyDesc.KeyData[5] == 0x02))
+			{
+				pAddr			= pEap->KeyDesc.KeyData + 8;		// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2)
+				pSTAKey			= pEap->KeyDesc.KeyData + 14;	// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2), STAKey_Mac_Addr(6)
+
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 from %02x:%02x:%02x:%02x:%02x:%02x Len=%d, KeyDataLen=%d\n",
+					pAddr[0], pAddr[1], pAddr[2], pAddr[3], pAddr[4], pAddr[5], Len, pEap->KeyDesc.KeyData[1]));
+
+				bSTAKeyFrame = TRUE;
+			}
+#endif
+
+		}
+		else if (Len >= (LENGTH_802_11 + 6 + 2 + 2 + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE))
+		{
+#if 0
+			RTMPMoveMemory(pAd->StaCfg.ReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+#endif
+			RTMPMoveMemory(pAd->StaCfg.DlsReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - Sniff replay counter 2(%02x-%02x-%02x-%02x-%02x-%02x-%02x-%02x) Len=%ld, KeyDataLen=%d\n",
+				pAd->StaCfg.ReplayCounter[0], pAd->StaCfg.ReplayCounter[1], pAd->StaCfg.ReplayCounter[2],
+				pAd->StaCfg.ReplayCounter[3], pAd->StaCfg.ReplayCounter[4],	pAd->StaCfg.ReplayCounter[5],
+				pAd->StaCfg.ReplayCounter[6], pAd->StaCfg.ReplayCounter[7], Len, pEap->KeyDesc.KeyData[1]));
+
+		}
+	}
+
+	// If timeout value is equaled to zero, it means always not be timeout.
+	// update local dls table entry
+	for (i= 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(pAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			if (bSTAKeyFrame)
+			{
+				PMAC_TABLE_ENTRY pEntry;
+
+				// STAKey frame, add pairwise key table
+				pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+				RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+
+				PairwiseKey.KeyLen = LEN_TKIP_EK;
+				NdisMoveMemory(PairwiseKey.Key, &pSTAKey[0], LEN_TKIP_EK);
+				NdisMoveMemory(PairwiseKey.TxMic, &pSTAKey[16], LEN_TKIP_RXMICK);
+				NdisMoveMemory(PairwiseKey.RxMic, &pSTAKey[24], LEN_TKIP_TXMICK);
+
+				PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg;
+
+				pEntry = DlsEntryTableLookup(pAd, pAd->StaCfg.DLSEntry[i].MacAddr, TRUE);
+				//AsicAddKeyEntry(pAd, (USHORT)(i + 2), BSS0, 0, &PairwiseKey, TRUE, TRUE);	// reserve 0 for multicast, 1 for unicast
+				//AsicUpdateRxWCIDTable(pAd, (USHORT)(i + 2), pAddr);
+				// Add Pair-wise key to Asic
+#ifdef RT2860
+            	AsicAddPairwiseKeyEntry(pAd,
+										pAd->StaCfg.DLSEntry[i].MacAddr,
+										(UCHAR)pAd->StaCfg.DLSEntry[i].MacTabMatchWCID,
+										&PairwiseKey);
+
+				RTMPAddWcidAttributeEntry(pAd,
+										  BSS0,
+										  0,
+										  PairwiseKey.CipherAlg,
+										  pEntry);
+
+#endif // RT2860 //
+				NdisMoveMemory(&pEntry->PairwiseKey, &PairwiseKey, sizeof(CIPHER_KEY));
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 (Peer STA MAC Address STAKey) \n"));
+
+				RTMPSendSTAKeyHandShake(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Finish STAKey handshake procedure (Initiator side)\n"));
+			}
+			else
+			{
+				// Data frame, update timeout value
+				if (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+				{
+					pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+					//AsicUpdateRxWCIDTable(pAd, (USHORT)(i + 2), pAddr);
+				}
+			}
+
+			bFindEntry = TRUE;
+		}
+	}
+
+	// update peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(pAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			if (bSTAKeyFrame)
+			{
+				PMAC_TABLE_ENTRY pEntry = NULL;
+
+				// STAKey frame, add pairwise key table, and send STAkey Msg-2
+				pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+				RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+
+				PairwiseKey.KeyLen = LEN_TKIP_EK;
+				NdisMoveMemory(PairwiseKey.Key, &pSTAKey[0], LEN_TKIP_EK);
+				NdisMoveMemory(PairwiseKey.TxMic, &pSTAKey[16], LEN_TKIP_RXMICK);
+				NdisMoveMemory(PairwiseKey.RxMic, &pSTAKey[24], LEN_TKIP_TXMICK);
+
+				PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg;
+
+				pEntry = DlsEntryTableLookup(pAd, pAd->StaCfg.DLSEntry[i].MacAddr, TRUE);
+				//AsicAddKeyEntry(pAd, (USHORT)(i + 2), BSS0, 0, &PairwiseKey, TRUE, TRUE);	// reserve 0 for multicast, 1 for unicast
+				//AsicUpdateRxWCIDTable(pAd, (USHORT)(i + 2), pAddr);
+				// Add Pair-wise key to Asic
+#ifdef RT2860
+            	AsicAddPairwiseKeyEntry(pAd,
+										pAd->StaCfg.DLSEntry[i].MacAddr,
+										(UCHAR)pAd->StaCfg.DLSEntry[i].MacTabMatchWCID,
+										&PairwiseKey);
+
+				RTMPAddWcidAttributeEntry(pAd,
+										  BSS0,
+										  0,
+										  PairwiseKey.CipherAlg,
+										  pEntry);
+#endif // RT2860 //
+				NdisMoveMemory(&pEntry->PairwiseKey, &PairwiseKey, sizeof(CIPHER_KEY));
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 (Initiator STA MAC Address STAKey)\n"));
+
+				// If support WPA or WPA2, start STAKey hand shake,
+				// If failed hand shake, just tear down peer DLS
+				if (RTMPSendSTAKeyHandShake(pAd, pAddr) != NDIS_STATUS_SUCCESS)
+				{
+					MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+					USHORT				reason = REASON_QOS_CIPHER_NOT_SUPPORT;
+
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+					pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+					DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+					MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE,("DLS - Finish STAKey handshake procedure (Peer side)\n"));
+				}
+			}
+			else
+			{
+				// Data frame, update timeout value
+				if (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+				{
+					pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+				}
+			}
+
+			bFindEntry = TRUE;
+		}
+	}
+
+
+	return bSTAKeyFrame;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Check if the frame can be sent through DLS direct link interface
+
+	Arguments:
+		pAd		Pointer	to adapter
+
+	Return Value:
+		DLS entry index
+
+	Note:
+
+	========================================================================
+*/
+INT	RTMPCheckDLSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA)
+{
+	INT rval = -1;
+	INT	i;
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return rval;
+
+	if (!INFRA_ON(pAd))
+		return rval;
+
+	do{
+		// check local dls table entry
+		for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				rval = i;
+				break;
+			}
+		}
+
+		// check peer dls table entry
+		for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				rval = i;
+				break;
+			}
+		}
+	} while (FALSE);
+
+	return rval;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID RTMPSendDLSTearDownFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	HEADER_802_11	DlsTearDownHdr;
+	ULONG			FrameLen = 0;
+	USHORT			Reason = REASON_QOS_QSTA_LEAVING_QBSS;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_TEARDOWN;
+	UCHAR			i = 0;
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+		RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Send DLS TearDown Frame \n"));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("ASSOC - RTMPSendDLSTearDownFrame() allocate memory failed \n"));
+		return;
+	}
+
+	ActHeaderInit(pAd, &DlsTearDownHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+					sizeof(HEADER_802_11),		&DlsTearDownHdr,
+					1,							&Category,
+					1,							&Action,
+					6,							pDA,
+					6,							pAd->CurrentAddress,
+					2,							&Reason,
+					END_OF_ARGS);
+
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	// Remove key in local dls table entry
+	for (i = 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	// Remove key in peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Send DLS TearDown Frame and remove key in (i=%d) \n", i));
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+NDIS_STATUS RTMPSendSTAKeyRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA)
+{
+	UCHAR				Header802_3[14];
+	NDIS_STATUS			NStatus;
+	ULONG				FrameLen = 0;
+	EAPOL_PACKET		Packet;
+	UCHAR				Mic[16];
+	UCHAR				digest[80];
+	PUCHAR				pOutBuffer = NULL;
+	PNDIS_PACKET		pNdisPacket;
+	UCHAR				temp[64];
+	UCHAR				DlsPTK[80];
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - RTMPSendSTAKeyRequest() to %02x:%02x:%02x:%02x:%02x:%02x\n", pDA[0], pDA[1], pDA[2], pDA[3], pDA[4], pDA[5]));
+
+	pAd->Sequence ++;
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer = EAPOL_VER;
+	Packet.ProType    = EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + 6 + MAC_ADDR_LEN;		// data field contain KDE andPeer MAC address
+
+	// STAKey Message is as EAPOL-Key(1,1,0,0,G/0,0,0, MIC, 0,Peer MAC KDE)
+	if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+    {
+        Packet.KeyDesc.Type = WPA1_KEY_DESC;
+    }
+    else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+    {
+        Packet.KeyDesc.Type = WPA2_KEY_DESC;
+    }
+
+	// Key descriptor version
+	Packet.KeyDesc.KeyInfo.KeyDescVer =
+		(((pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled) || (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)) ? (DESC_TYPE_AES) : (DESC_TYPE_TKIP));
+
+	Packet.KeyDesc.KeyInfo.KeyMic	= 1;
+	Packet.KeyDesc.KeyInfo.Secure	= 1;
+	Packet.KeyDesc.KeyInfo.Request	= 1;
+
+	Packet.KeyDesc.KeyDataLen[1]	= 12;
+
+	// use our own OUI to distinguish proprietary with standard.
+	Packet.KeyDesc.KeyData[0]		= 0xDD;
+	Packet.KeyDesc.KeyData[1]		= 0x0A;
+	Packet.KeyDesc.KeyData[2]		= 0x00;
+	Packet.KeyDesc.KeyData[3]		= 0x0C;
+	Packet.KeyDesc.KeyData[4]		= 0x43;
+	Packet.KeyDesc.KeyData[5]		= 0x03;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[6], pDA, MAC_ADDR_LEN);
+
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.DlsReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Allocate buffer for transmitting message
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+	if (NStatus	!= NDIS_STATUS_SUCCESS)
+		return NStatus;
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		              Packet.Body_Len[1] + 4,    &Packet,
+		              END_OF_ARGS);
+
+	// use proprietary PTK
+	NdisZeroMemory(temp, 64);
+	NdisMoveMemory(temp, "IEEE802.11 WIRELESS ACCESS POINT", 32);
+	WpaCountPTK(pAd, temp, temp, pAd->CommonCfg.Bssid, temp, pAd->CurrentAddress, DlsPTK, LEN_PTK);
+
+	// calculate MIC
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		NdisZeroMemory(digest,	sizeof(digest));
+		HMAC_SHA1(pOutBuffer, FrameLen, DlsPTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		NdisZeroMemory(Mic,	sizeof(Mic));
+		hmac_md5(DlsPTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+	}
+
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+	                  sizeof(Header802_3),	Header802_3,
+		              Packet.Body_Len[1] + 4,	&Packet,
+		              END_OF_ARGS);
+
+	NStatus = RTMPAllocateNdisPacket(pAd, &pNdisPacket, NULL, 0, pOutBuffer, FrameLen);
+	if (NStatus == NDIS_STATUS_SUCCESS)
+	{
+		RTMP_SET_PACKET_WCID(pNdisPacket, BSSID_WCID);
+		STASendPacket(pAd, pNdisPacket);
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSendSTAKeyRequest- Send STAKey request (NStatus=%x, FrameLen=%ld)\n", NStatus, FrameLen));
+
+	return NStatus;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+NDIS_STATUS RTMPSendSTAKeyHandShake(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA)
+{
+	UCHAR				Header802_3[14];
+	NDIS_STATUS			NStatus;
+	ULONG				FrameLen = 0;
+	EAPOL_PACKET		Packet;
+	UCHAR				Mic[16];
+	UCHAR				digest[80];
+	PUCHAR				pOutBuffer = NULL;
+	PNDIS_PACKET		pNdisPacket;
+	UCHAR				temp[64];
+	UCHAR				DlsPTK[80];			// Due to dirver can not get PTK, use proprietary PTK
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - RTMPSendSTAKeyHandShake() to %02x:%02x:%02x:%02x:%02x:%02x\n", pDA[0], pDA[1], pDA[2], pDA[3], pDA[4], pDA[5]));
+
+	pAd->Sequence ++;
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer = EAPOL_VER;
+	Packet.ProType    = EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + 6 + MAC_ADDR_LEN;		// data field contain KDE and Peer MAC address
+
+	// STAKey Message is as EAPOL-Key(1,1,0,0,G/0,0,0, MIC, 0,Peer MAC KDE)
+	if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+    {
+        Packet.KeyDesc.Type = WPA1_KEY_DESC;
+    }
+    else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+    {
+        Packet.KeyDesc.Type = WPA2_KEY_DESC;
+    }
+
+	// Key descriptor version
+	Packet.KeyDesc.KeyInfo.KeyDescVer =
+		(((pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled) || (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)) ? (DESC_TYPE_AES) : (DESC_TYPE_TKIP));
+
+	Packet.KeyDesc.KeyInfo.KeyMic	= 1;
+	Packet.KeyDesc.KeyInfo.Secure	= 1;
+
+	Packet.KeyDesc.KeyDataLen[1]	= 12;
+
+	// use our own OUI to distinguish proprietary with standard.
+	Packet.KeyDesc.KeyData[0]		= 0xDD;
+	Packet.KeyDesc.KeyData[1]		= 0x0A;
+	Packet.KeyDesc.KeyData[2]		= 0x00;
+	Packet.KeyDesc.KeyData[3]		= 0x0C;
+	Packet.KeyDesc.KeyData[4]		= 0x43;
+	Packet.KeyDesc.KeyData[5]		= 0x03;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[6], pDA, MAC_ADDR_LEN);
+
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.DlsReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Allocate buffer for transmitting message
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+	if (NStatus	!= NDIS_STATUS_SUCCESS)
+		return NStatus;
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		              Packet.Body_Len[1] + 4,    &Packet,
+		              END_OF_ARGS);
+
+	// use proprietary PTK
+	NdisZeroMemory(temp, 64);
+	NdisMoveMemory(temp, "IEEE802.11 WIRELESS ACCESS POINT", 32);
+	WpaCountPTK(pAd, temp, temp, pAd->CommonCfg.Bssid, temp, pAd->CurrentAddress, DlsPTK, LEN_PTK);
+
+	// calculate MIC
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		NdisZeroMemory(digest,	sizeof(digest));
+		HMAC_SHA1(pOutBuffer, FrameLen, DlsPTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		NdisZeroMemory(Mic,	sizeof(Mic));
+		hmac_md5(DlsPTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+	}
+
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+	                  sizeof(Header802_3),	Header802_3,
+		              Packet.Body_Len[1] + 4,	&Packet,
+		              END_OF_ARGS);
+
+	NStatus = RTMPAllocateNdisPacket(pAd, &pNdisPacket, NULL, 0, pOutBuffer, FrameLen);
+	if (NStatus == NDIS_STATUS_SUCCESS)
+	{
+		RTMP_SET_PACKET_WCID(pNdisPacket, BSSID_WCID);
+		STASendPacket(pAd, pNdisPacket);
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSendSTAKeyHandShake- Send STAKey Message-2 (NStatus=%x, FrameLen=%ld)\n", NStatus, FrameLen));
+
+	return NStatus;
+}
+
+VOID DlsTimeoutAction(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	MLME_DLS_REQ_STRUCT		MlmeDlsReq;
+	USHORT					reason;
+	PRT_802_11_DLS			pDLS = (PRT_802_11_DLS)FunctionContext;
+	PRTMP_ADAPTER			pAd = pDLS->pAd;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("DlsTimeout - Tear down DLS links (%02x:%02x:%02x:%02x:%02x:%02x)\n",
+		pDLS->MacAddr[0], pDLS->MacAddr[1], pDLS->MacAddr[2], pDLS->MacAddr[3], pDLS->MacAddr[4], pDLS->MacAddr[5]));
+
+	if ((pDLS) && (pDLS->Valid))
+	{
+		reason			= REASON_QOS_REQUEST_TIMEOUT;
+		pDLS->Valid		= FALSE;
+		pDLS->Status	= DLS_NONE;
+		DlsParmFill(pAd, &MlmeDlsReq, pDLS, reason);
+		MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+}
+
+/*
+================================================================
+Description : because DLS and CLI share the same WCID table in ASIC.
+Mesh entry also insert to pAd->MacTab.content[].  Such is marked as ValidAsDls = TRUE.
+Also fills the pairwise key.
+Because front MAX_AID_BA entries have direct mapping to BAEntry, which is only used as CLI. So we insert Dls
+from index MAX_AID_BA.
+================================================================
+*/
+MAC_TABLE_ENTRY *MacTableInsertDlsEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR	pAddr,
+	IN  UINT	DlsEntryIdx)
+{
+	PMAC_TABLE_ENTRY pEntry = NULL;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("====> MacTableInsertDlsEntry\n"));
+	// if FULL, return
+	if (pAd->MacTab.Size >= MAX_LEN_OF_MAC_TABLE)
+		return NULL;
+
+	do
+	{
+		if((pEntry = DlsEntryTableLookup(pAd, pAddr, TRUE)) != NULL)
+			break;
+
+		// allocate one MAC entry
+		pEntry = MacTableInsertEntry(pAd, pAddr, DlsEntryIdx + MIN_NET_DEVICE_FOR_DLS, TRUE);
+		if (pEntry)
+		{
+			pAd->StaCfg.DLSEntry[DlsEntryIdx].MacTabMatchWCID = pEntry->Aid;
+			pEntry->MatchDlsEntryIdx = DlsEntryIdx;
+			pEntry->AuthMode = pAd->StaCfg.AuthMode;
+			pEntry->WepStatus = pAd->StaCfg.WepStatus;
+			pEntry->PortSecured = WPA_802_1X_PORT_SECURED;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("MacTableInsertDlsEntry - allocate entry #%d, Total= %d\n",pEntry->Aid, pAd->MacTab.Size));
+
+			// If legacy WEP is used, set pair-wise cipherAlg into WCID attribute table for this entry
+			if ((pEntry->ValidAsDls) && (pEntry->WepStatus == Ndis802_11WEPEnabled))
+			{
+				UCHAR KeyIdx = 0;
+				UCHAR CipherAlg = 0;
+
+				KeyIdx	= pAd->StaCfg.DefaultKeyId;
+
+				CipherAlg 	= pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+
+				RTMPAddWcidAttributeEntry(pAd,
+											BSS0,
+											pAd->StaCfg.DefaultKeyId,
+											pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+											pEntry);
+			}
+
+			break;
+		}
+	} while(FALSE);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==== MacTableInsertDlsEntry\n"));
+
+	return pEntry;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Delete all Mesh Entry in pAd->MacTab
+	==========================================================================
+ */
+BOOLEAN MacTableDeleteDlsEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT wcid,
+	IN PUCHAR pAddr)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("====> MacTableDeleteDlsEntry\n"));
+
+	if (!VALID_WCID(wcid))
+		return FALSE;
+
+	MacTableDeleteEntry(pAd, wcid, pAddr);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==== MacTableDeleteDlsEntry\n"));
+
+	return TRUE;
+}
+
+MAC_TABLE_ENTRY *DlsEntryTableLookup(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount)
+{
+	ULONG HashIdx;
+	MAC_TABLE_ENTRY *pEntry = NULL;
+
+	RTMP_SEM_LOCK(&pAd->MacTabLock);
+	HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+	pEntry = pAd->MacTab.Hash[HashIdx];
+
+	while (pEntry)
+	{
+		if ((pEntry->ValidAsDls == TRUE)
+			&& MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
+		{
+			if(bResetIdelCount)
+				pEntry->NoDataIdleCount = 0;
+			break;
+		}
+		else
+			pEntry = pEntry->pNext;
+	}
+
+	RTMP_SEM_UNLOCK(&pAd->MacTabLock);
+	return pEntry;
+}
+
+MAC_TABLE_ENTRY *DlsEntryTableLookupByWcid(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	wcid,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount)
+{
+	ULONG DLsIndex;
+	PMAC_TABLE_ENTRY pCurEntry = NULL;
+	PMAC_TABLE_ENTRY pEntry = NULL;
+
+	if (!VALID_WCID(wcid))
+		return NULL;
+
+	RTMP_SEM_LOCK(&pAd->MacTabLock);
+
+	do
+	{
+		pCurEntry = &pAd->MacTab.Content[wcid];
+
+		DLsIndex = 0xff;
+		if ((pCurEntry) && (pCurEntry->ValidAsDls== TRUE))
+		{
+			DLsIndex = pCurEntry->MatchDlsEntryIdx;
+		}
+
+		if (DLsIndex == 0xff)
+			break;
+
+		if (MAC_ADDR_EQUAL(pCurEntry->Addr, pAddr))
+		{
+			if(bResetIdelCount)
+				pCurEntry->NoDataIdleCount = 0;
+			pEntry = pCurEntry;
+			break;
+		}
+	} while(FALSE);
+
+	RTMP_SEM_UNLOCK(&pAd->MacTabLock);
+
+	return pEntry;
+}
+
+INT Set_DlsEntryInfo_Display_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg)
+{
+	INT i;
+
+	printk("\n%-19s%-8s\n", "MAC", "TIMEOUT\n");
+	for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if ((pAd->StaCfg.DLSEntry[i].Valid) && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+		{
+			PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[pAd->StaCfg.DLSEntry[i].MacTabMatchWCID];
+
+			printk("%02x:%02x:%02x:%02x:%02x:%02x  ",
+				pAd->StaCfg.DLSEntry[i].MacAddr[0], pAd->StaCfg.DLSEntry[i].MacAddr[1], pAd->StaCfg.DLSEntry[i].MacAddr[2],
+				pAd->StaCfg.DLSEntry[i].MacAddr[3], pAd->StaCfg.DLSEntry[i].MacAddr[4], pAd->StaCfg.DLSEntry[i].MacAddr[5]);
+			printk("%-8d\n", pAd->StaCfg.DLSEntry[i].TimeOut);
+
+			printk("\n");
+			printk("\n%-19s%-4s%-4s%-4s%-4s%-8s%-7s%-7s%-7s%-10s%-6s%-6s%-6s%-6s\n",
+				   "MAC", "AID", "BSS", "PSM", "WMM", "MIMOPS", "RSSI0", "RSSI1", "RSSI2", "PhMd", "BW", "MCS", "SGI", "STBC");
+			printk("%02X:%02X:%02X:%02X:%02X:%02X  ",
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+			printk("%-4d", (int)pEntry->Aid);
+			printk("%-4d", (int)pEntry->apidx);
+			printk("%-4d", (int)pEntry->PsMode);
+			printk("%-4d", (int)CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE));
+			printk("%-8d", (int)pEntry->MmpsMode);
+			printk("%-7d", pEntry->RssiSample.AvgRssi0);
+			printk("%-7d", pEntry->RssiSample.AvgRssi1);
+			printk("%-7d", pEntry->RssiSample.AvgRssi2);
+			printk("%-10s", GetPhyMode(pEntry->HTPhyMode.field.MODE));
+			printk("%-6s", GetBW(pEntry->HTPhyMode.field.BW));
+			printk("%-6d", pEntry->HTPhyMode.field.MCS);
+			printk("%-6d", pEntry->HTPhyMode.field.ShortGI);
+			printk("%-6d", pEntry->HTPhyMode.field.STBC);
+			printk("%-10d, %d, %d%%\n", pEntry->DebugFIFOCount, pEntry->DebugTxCount,
+						(pEntry->DebugTxCount) ? ((pEntry->DebugTxCount-pEntry->DebugFIFOCount)*100/pEntry->DebugTxCount) : 0);
+			printk("\n");
+
+		}
+	}
+
+	return TRUE;
+}
+
+INT	Set_DlsAddEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR	mac[MAC_ADDR_LEN];
+	USHORT	Timeout;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    RT_802_11_DLS	Dls;
+
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and timeout value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		Timeout = simple_strtol((token+1), 0, 10);
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+	    printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%d", mac[0], mac[1],
+	           mac[2], mac[3], mac[4], mac[5], (int)Timeout);
+
+		NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+		Dls.TimeOut = Timeout;
+		COPY_MAC_ADDR(Dls.MacAddr, mac);
+		Dls.Valid = 1;
+
+		MlmeEnqueue(pAd,
+					MLME_CNTL_STATE_MACHINE,
+					RT_OID_802_11_SET_DLS_PARAM,
+					sizeof(RT_802_11_DLS),
+					&Dls);
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_DlsTearDownEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR			macAddr[MAC_ADDR_LEN];
+	CHAR			*value;
+	INT				i;
+	RT_802_11_DLS	Dls;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+	for (i=0, value = rstrtok(arg,":"); value; value = rstrtok(NULL,":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+		AtoH(value, &macAddr[i++], 2);
+	}
+
+	printk("\n%02x:%02x:%02x:%02x:%02x:%02x", macAddr[0], macAddr[1],
+	           macAddr[2], macAddr[3], macAddr[4], macAddr[5]);
+
+	NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+	COPY_MAC_ADDR(Dls.MacAddr, macAddr);
+	Dls.Valid = 0;
+
+	MlmeEnqueue(pAd,
+				MLME_CNTL_STATE_MACHINE,
+				RT_OID_802_11_SET_DLS_PARAM,
+				sizeof(RT_802_11_DLS),
+				&Dls);
+
+	return TRUE;
+}
+
diff --git a/drivers/staging/rt2860/sta/rtmp_data.c b/drivers/staging/rt2860/sta/rtmp_data.c
new file mode 100644
index 000000000000..36aff247cd95
--- /dev/null
+++ b/drivers/staging/rt2860/sta/rtmp_data.c
@@ -0,0 +1,2614 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_data.c
+
+	Abstract:
+	Data path subroutines
+
+	Revision History:
+	Who 		When			What
+	--------	----------		----------------------------------------------
+	John		      Aug/17/04		major modification for RT2561/2661
+	Jan Lee	      Mar/17/06		major modification for RT2860 New Ring Design
+*/
+#include "../rt_config.h"
+
+
+
+VOID STARxEAPOLFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	PRT28XX_RXD_STRUC	pRxD = &(pRxBlk->RxD);
+	PRXWI_STRUC		pRxWI = pRxBlk->pRxWI;
+	UCHAR			*pTmpBuf;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAd->StaCfg.WpaSupplicantUP)
+	{
+		// All EAPoL frames have to pass to upper layer (ex. WPA_SUPPLICANT daemon)
+		// TBD : process fragmented EAPol frames
+		{
+			// In 802.1x mode, if the received frame is EAP-SUCCESS packet, turn on the PortSecured variable
+			if ( pAd->StaCfg.IEEE8021X == TRUE &&
+				 (EAP_CODE_SUCCESS == WpaCheckEapCode(pAd, pRxBlk->pData, pRxBlk->DataSize, LENGTH_802_1_H)))
+			{
+				PUCHAR	Key;
+				UCHAR 	CipherAlg;
+				int     idx = 0;
+
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("Receive EAP-SUCCESS Packet\n"));
+				STA_PORT_SECURED(pAd);
+
+                if (pAd->StaCfg.IEEE8021x_required_keys == FALSE)
+                {
+                    idx = pAd->StaCfg.DesireSharedKeyId;
+                    CipherAlg = pAd->StaCfg.DesireSharedKey[idx].CipherAlg;
+					Key = pAd->StaCfg.DesireSharedKey[idx].Key;
+
+                    if (pAd->StaCfg.DesireSharedKey[idx].KeyLen > 0)
+    				{
+#ifdef RT2860
+						MAC_TABLE_ENTRY *pEntry = &pAd->MacTab.Content[BSSID_WCID];
+
+						// Set key material and cipherAlg to Asic
+						AsicAddSharedKeyEntry(pAd, BSS0, idx, CipherAlg, Key, NULL, NULL);
+
+						// Assign group key info
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, NULL);
+
+						// Assign pairwise key info
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, pEntry);
+
+                        pAd->IndicateMediaState = NdisMediaStateConnected;
+                        pAd->ExtraInfo = GENERAL_LINK_UP;
+#endif // RT2860 //
+						// For Preventing ShardKey Table is cleared by remove key procedure.
+    					pAd->SharedKey[BSS0][idx].CipherAlg = CipherAlg;
+						pAd->SharedKey[BSS0][idx].KeyLen = pAd->StaCfg.DesireSharedKey[idx].KeyLen;
+						NdisMoveMemory(pAd->SharedKey[BSS0][idx].Key,
+									   pAd->StaCfg.DesireSharedKey[idx].Key,
+									   pAd->StaCfg.DesireSharedKey[idx].KeyLen);
+    				}
+				}
+			}
+
+			Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
+			return;
+		}
+	}
+	else
+#endif // WPA_SUPPLICANT_SUPPORT //
+	{
+		// Special DATA frame that has to pass to MLME
+		//	 1. Cisco Aironet frames for CCX2. We need pass it to MLME for special process
+		//	 2. EAPOL handshaking frames when driver supplicant enabled, pass to MLME for special process
+		{
+			pTmpBuf = pRxBlk->pData - LENGTH_802_11;
+			NdisMoveMemory(pTmpBuf, pRxBlk->pHeader, LENGTH_802_11);
+			REPORT_MGMT_FRAME_TO_MLME(pAd, pRxWI->WirelessCliID, pTmpBuf, pRxBlk->DataSize + LENGTH_802_11, pRxWI->RSSI0, pRxWI->RSSI1, pRxWI->RSSI2, pRxD->PlcpSignal);
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("!!! report EAPOL/AIRONET DATA to MLME (len=%d) !!!\n", pRxBlk->DataSize));
+		}
+	}
+
+	RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	return;
+
+}
+
+VOID STARxDataFrameAnnounce(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+
+	// non-EAP frame
+	if (!RTMPCheckWPAframe(pAd, pEntry, pRxBlk->pData, pRxBlk->DataSize, FromWhichBSSID))
+	{
+		{
+			// drop all non-EAP DATA frame before
+			// this client's Port-Access-Control is secured
+			if (pRxBlk->pHeader->FC.Wep)
+			{
+				// unsupported cipher suite
+				if (pAd->StaCfg.WepStatus == Ndis802_11EncryptionDisabled)
+				{
+					// release packet
+					RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+					return;
+				}
+			}
+			else
+			{
+				// encryption in-use but receive a non-EAPOL clear text frame, drop it
+				if ((pAd->StaCfg.WepStatus != Ndis802_11EncryptionDisabled) &&
+					(pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+				{
+					// release packet
+					RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+					return;
+				}
+			}
+		}
+		RX_BLK_CLEAR_FLAG(pRxBlk, fRX_EAP);
+		if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_ARALINK))
+		{
+			// Normal legacy, AMPDU or AMSDU
+			CmmRxnonRalinkFrameIndicate(pAd, pRxBlk, FromWhichBSSID);
+
+		}
+		else
+		{
+			// ARALINK
+			CmmRxRalinkFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
+		}
+#ifdef QOS_DLS_SUPPORT
+		RX_BLK_CLEAR_FLAG(pRxBlk, fRX_DLS);
+#endif // QOS_DLS_SUPPORT //
+	}
+	else
+	{
+		RX_BLK_SET_FLAG(pRxBlk, fRX_EAP);
+#ifdef DOT11_N_SUPPORT
+		if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMPDU) && (pAd->CommonCfg.bDisableReordering == 0))
+		{
+			Indicate_AMPDU_Packet(pAd, pRxBlk, FromWhichBSSID);
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			// Determin the destination of the EAP frame
+			//  to WPA state machine or upper layer
+			STARxEAPOLFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
+		}
+	}
+}
+
+
+// For TKIP frame, calculate the MIC value
+BOOLEAN STACheckTkipMICValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk)
+{
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	UCHAR			*pData = pRxBlk->pData;
+	USHORT			DataSize = pRxBlk->DataSize;
+	UCHAR			UserPriority = pRxBlk->UserPriority;
+	PCIPHER_KEY		pWpaKey;
+	UCHAR			*pDA, *pSA;
+
+	pWpaKey = &pAd->SharedKey[BSS0][pRxBlk->pRxWI->KeyIndex];
+
+	pDA = pHeader->Addr1;
+	if (RX_BLK_TEST_FLAG(pRxBlk, fRX_INFRA))
+	{
+		pSA = pHeader->Addr3;
+	}
+	else
+	{
+		pSA = pHeader->Addr2;
+	}
+
+	if (RTMPTkipCompareMICValue(pAd,
+								pData,
+								pDA,
+								pSA,
+								pWpaKey->RxMic,
+								UserPriority,
+								DataSize) == FALSE)
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR,("Rx MIC Value error 2\n"));
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+		if (pAd->StaCfg.WpaSupplicantUP)
+		{
+			WpaSendMicFailureToWpaSupplicant(pAd, (pWpaKey->Type == PAIRWISEKEY) ? TRUE : FALSE);
+		}
+		else
+#endif // WPA_SUPPLICANT_SUPPORT //
+		{
+			RTMPReportMicError(pAd, pWpaKey);
+		}
+
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+
+//
+// All Rx routines use RX_BLK structure to hande rx events
+// It is very important to build pRxBlk attributes
+//  1. pHeader pointer to 802.11 Header
+//  2. pData pointer to payload including LLC (just skip Header)
+//  3. set payload size including LLC to DataSize
+//  4. set some flags with RX_BLK_SET_FLAG()
+//
+VOID STAHandleRxDataFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+	PRT28XX_RXD_STRUC				pRxD = &(pRxBlk->RxD);
+	PRXWI_STRUC						pRxWI = pRxBlk->pRxWI;
+	PHEADER_802_11					pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET					pRxPacket = pRxBlk->pRxPacket;
+	BOOLEAN 						bFragment = FALSE;
+	MAC_TABLE_ENTRY	    			*pEntry = NULL;
+	UCHAR							FromWhichBSSID = BSS0;
+	UCHAR                           UserPriority = 0;
+
+	{
+		// before LINK UP, all DATA frames are rejected
+		if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+#ifdef QOS_DLS_SUPPORT
+		//if ((pHeader->FC.FrDs == 0) && (pHeader->FC.ToDs == 0))
+		if (RTMPRcvFrameDLSCheck(pAd, pHeader, pRxWI->MPDUtotalByteCount, pRxD))
+		{
+			return;
+		}
+#endif // QOS_DLS_SUPPORT //
+
+		// Drop not my BSS frames
+		if (pRxD->MyBss == 0)
+		{
+			{
+				// release packet
+				RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+				return;
+			}
+		}
+
+		pAd->RalinkCounters.RxCountSinceLastNULL++;
+		if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable && (pHeader->FC.SubType & 0x08))
+		{
+			UCHAR *pData;
+			DBGPRINT(RT_DEBUG_TRACE,("bAPSDCapable\n"));
+
+			// Qos bit 4
+			pData = (PUCHAR)pHeader + LENGTH_802_11;
+			if ((*pData >> 4) & 0x01)
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("RxDone- Rcv EOSP frame, driver may fall into sleep\n"));
+				pAd->CommonCfg.bInServicePeriod = FALSE;
+
+				// Force driver to fall into sleep mode when rcv EOSP frame
+				if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+				{
+					USHORT  TbttNumToNextWakeUp;
+					USHORT  NextDtim = pAd->StaCfg.DtimPeriod;
+					ULONG   Now;
+
+					NdisGetSystemUpTime(&Now);
+					NextDtim -= (USHORT)(Now - pAd->StaCfg.LastBeaconRxTime)/pAd->CommonCfg.BeaconPeriod;
+
+					TbttNumToNextWakeUp = pAd->StaCfg.DefaultListenCount;
+					if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM) && (TbttNumToNextWakeUp > NextDtim))
+						TbttNumToNextWakeUp = NextDtim;
+
+					MlmeSetPsmBit(pAd, PWR_SAVE);
+					// if WMM-APSD is failed, try to disable following line
+					AsicSleepThenAutoWakeup(pAd, TbttNumToNextWakeUp);
+				}
+			}
+
+			if ((pHeader->FC.MoreData) && (pAd->CommonCfg.bInServicePeriod))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("Sending another trigger frame when More Data bit is set to 1\n"));
+			}
+		}
+
+		// Drop NULL, CF-ACK(no data), CF-POLL(no data), and CF-ACK+CF-POLL(no data) data frame
+		if ((pHeader->FC.SubType & 0x04)) // bit 2 : no DATA
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+	    // Drop not my BSS frame (we can not only check the MyBss bit in RxD)
+#ifdef QOS_DLS_SUPPORT
+	    if (!pAd->CommonCfg.bDLSCapable)
+	    {
+#endif // QOS_DLS_SUPPORT //
+		if (INFRA_ON(pAd))
+		{
+			// Infrastructure mode, check address 2 for BSSID
+			if (!RTMPEqualMemory(&pHeader->Addr2, &pAd->CommonCfg.Bssid, 6))
+			{
+				// Receive frame not my BSSID
+	            // release packet
+	            RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+				return;
+			}
+		}
+		else	// Ad-Hoc mode or Not associated
+		{
+			// Ad-Hoc mode, check address 3 for BSSID
+			if (!RTMPEqualMemory(&pHeader->Addr3, &pAd->CommonCfg.Bssid, 6))
+			{
+				// Receive frame not my BSSID
+	            // release packet
+	            RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+				return;
+			}
+		}
+#ifdef QOS_DLS_SUPPORT
+	    }
+#endif // QOS_DLS_SUPPORT //
+
+		//
+		// find pEntry
+		//
+		if (pRxWI->WirelessCliID < MAX_LEN_OF_MAC_TABLE)
+		{
+			pEntry = &pAd->MacTab.Content[pRxWI->WirelessCliID];
+		}
+		else
+		{
+			// 1. release packet if infra mode
+			// 2. new a pEntry if ad-hoc mode
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+		// infra or ad-hoc
+		if (INFRA_ON(pAd))
+		{
+			RX_BLK_SET_FLAG(pRxBlk, fRX_INFRA);
+#ifdef QOS_DLS_SUPPORT
+			if ((pHeader->FC.FrDs == 0) && (pHeader->FC.ToDs == 0))
+				RX_BLK_SET_FLAG(pRxBlk, fRX_DLS);
+			else
+#endif // QOS_DLS_SUPPORT //
+			ASSERT(pRxWI->WirelessCliID == BSSID_WCID);
+		}
+
+		// check Atheros Client
+		if ((pEntry->bIAmBadAtheros == FALSE) &&  (pRxD->AMPDU == 1) && (pHeader->FC.Retry ))
+		{
+			pEntry->bIAmBadAtheros = TRUE;
+			pAd->CommonCfg.IOTestParm.bCurrentAtheros = TRUE;
+			pAd->CommonCfg.IOTestParm.bLastAtheros = TRUE;
+			if (!STA_AES_ON(pAd))
+			{
+				AsicUpdateProtect(pAd, 8, ALLN_SETPROTECT, TRUE, FALSE);
+			}
+		}
+	}
+
+	pRxBlk->pData = (UCHAR *)pHeader;
+
+	//
+	// update RxBlk->pData, DataSize
+	// 802.11 Header, QOS, HTC, Hw Padding
+	//
+
+	// 1. skip 802.11 HEADER
+	{
+		pRxBlk->pData += LENGTH_802_11;
+		pRxBlk->DataSize -= LENGTH_802_11;
+	}
+
+	// 2. QOS
+	if (pHeader->FC.SubType & 0x08)
+	{
+		RX_BLK_SET_FLAG(pRxBlk, fRX_QOS);
+		UserPriority = *(pRxBlk->pData) & 0x0f;
+		// bit 7 in QoS Control field signals the HT A-MSDU format
+		if ((*pRxBlk->pData) & 0x80)
+		{
+			RX_BLK_SET_FLAG(pRxBlk, fRX_AMSDU);
+		}
+
+		// skip QOS contorl field
+		pRxBlk->pData += 2;
+		pRxBlk->DataSize -=2;
+	}
+	pRxBlk->UserPriority = UserPriority;
+
+	// 3. Order bit: A-Ralink or HTC+
+	if (pHeader->FC.Order)
+	{
+#ifdef AGGREGATION_SUPPORT
+		if ((pRxWI->PHYMODE <= MODE_OFDM) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED)))
+		{
+			RX_BLK_SET_FLAG(pRxBlk, fRX_ARALINK);
+		}
+		else
+#endif
+		{
+#ifdef DOT11_N_SUPPORT
+			RX_BLK_SET_FLAG(pRxBlk, fRX_HTC);
+			// skip HTC contorl field
+			pRxBlk->pData += 4;
+			pRxBlk->DataSize -= 4;
+#endif // DOT11_N_SUPPORT //
+		}
+	}
+
+	// 4. skip HW padding
+	if (pRxD->L2PAD)
+	{
+		// just move pData pointer
+		// because DataSize excluding HW padding
+		RX_BLK_SET_FLAG(pRxBlk, fRX_PAD);
+		pRxBlk->pData += 2;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if (pRxD->BA)
+	{
+		RX_BLK_SET_FLAG(pRxBlk, fRX_AMPDU);
+	}
+#endif // DOT11_N_SUPPORT //
+
+
+	//
+	// Case I  Process Broadcast & Multicast data frame
+	//
+	if (pRxD->Bcast || pRxD->Mcast)
+	{
+		INC_COUNTER64(pAd->WlanCounters.MulticastReceivedFrameCount);
+
+		// Drop Mcast/Bcast frame with fragment bit on
+		if (pHeader->FC.MoreFrag)
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+		// Filter out Bcast frame which AP relayed for us
+		if (pHeader->FC.FrDs && MAC_ADDR_EQUAL(pHeader->Addr3, pAd->CurrentAddress))
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+		Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
+		return;
+	}
+	else if (pRxD->U2M)
+	{
+		pAd->LastRxRate = (USHORT)((pRxWI->MCS) + (pRxWI->BW <<7) + (pRxWI->ShortGI <<8)+ (pRxWI->PHYMODE <<14)) ;
+
+
+#ifdef QOS_DLS_SUPPORT
+        if (RX_BLK_TEST_FLAG(pRxBlk, fRX_DLS))
+		{
+			MAC_TABLE_ENTRY *pDlsEntry = NULL;
+
+			pDlsEntry = DlsEntryTableLookupByWcid(pAd, pRxWI->WirelessCliID, pHeader->Addr2, TRUE);
+										                        if(pDlsEntry)
+			Update_Rssi_Sample(pAd, &pDlsEntry->RssiSample, pRxWI);
+		}
+		else
+#endif // QOS_DLS_SUPPORT //
+		if (ADHOC_ON(pAd))
+		{
+			pEntry = MacTableLookup(pAd, pHeader->Addr2);
+			if (pEntry)
+				Update_Rssi_Sample(pAd, &pEntry->RssiSample, pRxWI);
+		}
+
+
+		Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, pRxWI);
+
+		pAd->StaCfg.LastSNR0 = (UCHAR)(pRxWI->SNR0);
+		pAd->StaCfg.LastSNR1 = (UCHAR)(pRxWI->SNR1);
+
+		pAd->RalinkCounters.OneSecRxOkDataCnt++;
+
+
+    	if (!((pHeader->Frag == 0) && (pHeader->FC.MoreFrag == 0)))
+    	{
+    		// re-assemble the fragmented packets
+    		// return complete frame (pRxPacket) or NULL
+    		bFragment = TRUE;
+    		pRxPacket = RTMPDeFragmentDataFrame(pAd, pRxBlk);
+    	}
+
+    	if (pRxPacket)
+    	{
+			pEntry = &pAd->MacTab.Content[pRxWI->WirelessCliID];
+
+    		// process complete frame
+    		if (bFragment && (pRxD->Decrypted) && (pEntry->WepStatus == Ndis802_11Encryption2Enabled))
+    		{
+				// Minus MIC length
+				pRxBlk->DataSize -= 8;
+
+    			// For TKIP frame, calculate the MIC value
+    			if (STACheckTkipMICValue(pAd, pEntry, pRxBlk) == FALSE)
+    			{
+    				return;
+    			}
+    		}
+
+    		STARxDataFrameAnnounce(pAd, pEntry, pRxBlk, FromWhichBSSID);
+			return;
+    	}
+    	else
+    	{
+    		// just return
+    		// because RTMPDeFragmentDataFrame() will release rx packet,
+    		// if packet is fragmented
+    		return;
+    	}
+	}
+
+	ASSERT(0);
+	// release packet
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+}
+
+VOID STAHandleRxMgmtFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+	PRT28XX_RXD_STRUC	pRxD = &(pRxBlk->RxD);
+	PRXWI_STRUC		pRxWI = pRxBlk->pRxWI;
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+
+	do
+	{
+
+		// We should collect RSSI not only U2M data but also my beacon
+		if ((pHeader->FC.SubType == SUBTYPE_BEACON) && (MAC_ADDR_EQUAL(&pAd->CommonCfg.Bssid, &pHeader->Addr2)))
+		{
+			Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, pRxWI);
+
+			pAd->StaCfg.LastSNR0 = (UCHAR)(pRxWI->SNR0);
+			pAd->StaCfg.LastSNR1 = (UCHAR)(pRxWI->SNR1);
+		}
+
+		// First check the size, it MUST not exceed the mlme queue size
+		if (pRxWI->MPDUtotalByteCount > MGMT_DMA_BUFFER_SIZE)
+		{
+			DBGPRINT_ERR(("STAHandleRxMgmtFrame: frame too large, size = %d \n", pRxWI->MPDUtotalByteCount));
+			break;
+		}
+
+		REPORT_MGMT_FRAME_TO_MLME(pAd, pRxWI->WirelessCliID, pHeader, pRxWI->MPDUtotalByteCount,
+									pRxWI->RSSI0, pRxWI->RSSI1, pRxWI->RSSI2, pRxD->PlcpSignal);
+	} while (FALSE);
+
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_SUCCESS);
+}
+
+VOID STAHandleRxControlFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+#ifdef DOT11_N_SUPPORT
+	PRXWI_STRUC		pRxWI = pRxBlk->pRxWI;
+#endif // DOT11_N_SUPPORT //
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+
+	switch (pHeader->FC.SubType)
+	{
+		case SUBTYPE_BLOCK_ACK_REQ:
+#ifdef DOT11_N_SUPPORT
+			{
+				CntlEnqueueForRecv(pAd, pRxWI->WirelessCliID, (pRxWI->MPDUtotalByteCount), (PFRAME_BA_REQ)pHeader);
+			}
+			break;
+#endif // DOT11_N_SUPPORT //
+		case SUBTYPE_BLOCK_ACK:
+		case SUBTYPE_ACK:
+		default:
+			break;
+	}
+
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process RxDone interrupt, running in DPC level
+
+	Arguments:
+		pAd Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		This routine has to maintain Rx ring read pointer.
+		Need to consider QOS DATA format when converting to 802.3
+	========================================================================
+*/
+BOOLEAN STARxDoneInterruptHandle(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	BOOLEAN			argc)
+{
+	NDIS_STATUS			Status;
+	UINT32			RxProcessed, RxPending;
+	BOOLEAN			bReschedule = FALSE;
+	RT28XX_RXD_STRUC	*pRxD;
+	UCHAR			*pData;
+	PRXWI_STRUC		pRxWI;
+	PNDIS_PACKET	pRxPacket;
+	PHEADER_802_11	pHeader;
+	RX_BLK			RxCell;
+
+	RxProcessed = RxPending = 0;
+
+	// process whole rx ring
+	while (1)
+	{
+
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF |
+								fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST) ||
+			!RTMP_TEST_FLAG(pAd,fRTMP_ADAPTER_START_UP))
+		{
+			break;
+		}
+
+#ifdef RT2860
+		if (RxProcessed++ > MAX_RX_PROCESS_CNT)
+		{
+			// need to reschedule rx handle
+			bReschedule = TRUE;
+			break;
+		}
+#endif // RT2860 //
+
+		RxProcessed ++; // test
+
+		// 1. allocate a new data packet into rx ring to replace received packet
+		//    then processing the received packet
+		// 2. the callee must take charge of release of packet
+		// 3. As far as driver is concerned ,
+		//    the rx packet must
+		//      a. be indicated to upper layer or
+		//      b. be released if it is discarded
+		pRxPacket = GetPacketFromRxRing(pAd, &(RxCell.RxD), &bReschedule, &RxPending);
+		if (pRxPacket == NULL)
+		{
+			// no more packet to process
+			break;
+		}
+
+		// get rx ring descriptor
+		pRxD = &(RxCell.RxD);
+		// get rx data buffer
+		pData	= GET_OS_PKT_DATAPTR(pRxPacket);
+		pRxWI	= (PRXWI_STRUC) pData;
+		pHeader = (PHEADER_802_11) (pData+RXWI_SIZE) ;
+
+#ifdef RT_BIG_ENDIAN
+	    RTMPFrameEndianChange(pAd, (PUCHAR)pHeader, DIR_READ, TRUE);
+		RTMPWIEndianChange((PUCHAR)pRxWI, TYPE_RXWI);
+#endif
+
+		// build RxCell
+		RxCell.pRxWI = pRxWI;
+		RxCell.pHeader = pHeader;
+		RxCell.pRxPacket = pRxPacket;
+		RxCell.pData = (UCHAR *) pHeader;
+		RxCell.DataSize = pRxWI->MPDUtotalByteCount;
+		RxCell.Flags = 0;
+
+		// Increase Total receive byte counter after real data received no mater any error or not
+		pAd->RalinkCounters.ReceivedByteCount +=  pRxWI->MPDUtotalByteCount;
+		pAd->RalinkCounters.RxCount ++;
+
+		INC_COUNTER64(pAd->WlanCounters.ReceivedFragmentCount);
+
+		if (pRxWI->MPDUtotalByteCount < 14)
+			Status = NDIS_STATUS_FAILURE;
+
+        if (MONITOR_ON(pAd))
+		{
+            send_monitor_packets(pAd, &RxCell);
+			break;
+		}
+		/* RT2870 invokes STARxDoneInterruptHandle() in rtusb_bulk.c */
+#ifdef RALINK_ATE
+		if (ATE_ON(pAd))
+		{
+			pAd->ate.RxCntPerSec++;
+			ATESampleRssi(pAd, pRxWI);
+#ifdef RALINK_28xx_QA
+			if (pAd->ate.bQARxStart == TRUE)
+			{
+				/* (*pRxD) has been swapped in GetPacketFromRxRing() */
+				ATE_QA_Statistics(pAd, pRxWI, pRxD,	pHeader);
+			}
+#endif // RALINK_28xx_QA //
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_SUCCESS);
+			continue;
+		}
+#endif // RALINK_ATE //
+
+		// Check for all RxD errors
+		Status = RTMPCheckRxError(pAd, pHeader, pRxWI, pRxD);
+
+		// Handle the received frame
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			switch (pHeader->FC.Type)
+			{
+				// CASE I, receive a DATA frame
+				case BTYPE_DATA:
+				{
+					// process DATA frame
+					STAHandleRxDataFrame(pAd, &RxCell);
+				}
+				break;
+				// CASE II, receive a MGMT frame
+				case BTYPE_MGMT:
+				{
+					STAHandleRxMgmtFrame(pAd, &RxCell);
+				}
+				break;
+				// CASE III. receive a CNTL frame
+				case BTYPE_CNTL:
+				{
+					STAHandleRxControlFrame(pAd, &RxCell);
+				}
+				break;
+				// discard other type
+				default:
+					RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+					break;
+			}
+		}
+		else
+		{
+			pAd->Counters8023.RxErrors++;
+			// discard this frame
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+		}
+	}
+
+	return bReschedule;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPHandleTwakeupInterrupt(
+	IN PRTMP_ADAPTER pAd)
+{
+	AsicForceWakeup(pAd, FALSE);
+}
+
+/*
+========================================================================
+Routine Description:
+    Early checking and OS-depened parsing for Tx packet send to our STA driver.
+
+Arguments:
+    NDIS_HANDLE 	MiniportAdapterContext	Pointer refer to the device handle, i.e., the pAd.
+	PPNDIS_PACKET	ppPacketArray			The packet array need to do transmission.
+	UINT			NumberOfPackets			Number of packet in packet array.
+
+Return Value:
+	NONE
+
+Note:
+	This function do early checking and classification for send-out packet.
+	You only can put OS-depened & STA related code in here.
+========================================================================
+*/
+VOID STASendPackets(
+	IN	NDIS_HANDLE		MiniportAdapterContext,
+	IN	PPNDIS_PACKET	ppPacketArray,
+	IN	UINT			NumberOfPackets)
+{
+	UINT			Index;
+	PRTMP_ADAPTER	pAd = (PRTMP_ADAPTER) MiniportAdapterContext;
+	PNDIS_PACKET	pPacket;
+	BOOLEAN			allowToSend = FALSE;
+
+
+	for (Index = 0; Index < NumberOfPackets; Index++)
+	{
+		pPacket = ppPacketArray[Index];
+
+		do
+		{
+			if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+				RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS) ||
+				RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+			{
+				// Drop send request since hardware is in reset state
+					break;
+			}
+			else if (!INFRA_ON(pAd) && !ADHOC_ON(pAd))
+			{
+				// Drop send request since there are no physical connection yet
+					break;
+			}
+			else
+			{
+				// Record that orignal packet source is from NDIS layer,so that
+				// later on driver knows how to release this NDIS PACKET
+#ifdef QOS_DLS_SUPPORT
+				MAC_TABLE_ENTRY *pEntry;
+				PUCHAR pSrcBufVA = GET_OS_PKT_DATAPTR(pPacket);
+
+				pEntry = MacTableLookup(pAd, pSrcBufVA);
+				if (pEntry && (pEntry->ValidAsDls == TRUE))
+				{
+					RTMP_SET_PACKET_WCID(pPacket, pEntry->Aid);
+				}
+				else
+#endif // QOS_DLS_SUPPORT //
+				RTMP_SET_PACKET_WCID(pPacket, 0); // this field is useless when in STA mode
+				RTMP_SET_PACKET_SOURCE(pPacket, PKTSRC_NDIS);
+				NDIS_SET_PACKET_STATUS(pPacket, NDIS_STATUS_PENDING);
+				pAd->RalinkCounters.PendingNdisPacketCount++;
+
+				allowToSend = TRUE;
+			}
+		} while(FALSE);
+
+		if (allowToSend == TRUE)
+			STASendPacket(pAd, pPacket);
+		else
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+	}
+
+	// Dequeue outgoing frames from TxSwQueue[] and process it
+	RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+
+}
+
+
+/*
+========================================================================
+Routine Description:
+	This routine is used to do packet parsing and classification for Tx packet
+	to STA device, and it will en-queue packets to our TxSwQueue depends on AC
+	class.
+
+Arguments:
+	pAd    		Pointer to our adapter
+	pPacket 	Pointer to send packet
+
+Return Value:
+	NDIS_STATUS_SUCCESS			If succes to queue the packet into TxSwQueue.
+	NDIS_STATUS_FAILURE			If failed to do en-queue.
+
+Note:
+	You only can put OS-indepened & STA related code in here.
+========================================================================
+*/
+NDIS_STATUS STASendPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	UINT			AllowFragSize;
+	UCHAR			NumberOfFrag;
+	UCHAR			QueIdx, UserPriority;
+	MAC_TABLE_ENTRY *pEntry = NULL;
+	unsigned int 	IrqFlags;
+	UCHAR			FlgIsIP = 0;
+	UCHAR			Rate;
+
+	// Prepare packet information structure for buffer descriptor
+	// chained within a single NDIS packet.
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+
+	if (pSrcBufVA == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("STASendPacket --> pSrcBufVA == NULL !!!SrcBufLen=%x\n",SrcBufLen));
+		// Resourece is low, system did not allocate virtual address
+		// return NDIS_STATUS_FAILURE directly to upper layer
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return NDIS_STATUS_FAILURE;
+	}
+
+
+	if (SrcBufLen < 14)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("STASendPacket --> Ndis Packet buffer error !!!\n"));
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return (NDIS_STATUS_FAILURE);
+	}
+
+	// In HT rate adhoc mode, A-MPDU is often used. So need to lookup BA Table and MAC Entry.
+	// Note multicast packets in adhoc also use BSSID_WCID index.
+	{
+		if(INFRA_ON(pAd))
+		{
+#ifdef QOS_DLS_SUPPORT
+			USHORT	tmpWcid;
+
+			tmpWcid = RTMP_GET_PACKET_WCID(pPacket);
+			if (VALID_WCID(tmpWcid) &&
+				(pAd->MacTab.Content[tmpWcid].ValidAsDls== TRUE))
+			{
+				pEntry = &pAd->MacTab.Content[tmpWcid];
+				Rate = pAd->MacTab.Content[tmpWcid].CurrTxRate;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+			{
+			pEntry = &pAd->MacTab.Content[BSSID_WCID];
+			RTMP_SET_PACKET_WCID(pPacket, BSSID_WCID);
+			Rate = pAd->CommonCfg.TxRate;
+		}
+		}
+		else if (ADHOC_ON(pAd))
+		{
+			if (*pSrcBufVA & 0x01)
+			{
+				RTMP_SET_PACKET_WCID(pPacket, MCAST_WCID);
+				pEntry = &pAd->MacTab.Content[MCAST_WCID];
+			}
+			else
+			{
+				pEntry = MacTableLookup(pAd, pSrcBufVA);
+			}
+			Rate = pAd->CommonCfg.TxRate;
+		}
+	}
+
+	if (!pEntry)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("STASendPacket->Cannot find pEntry(%2x:%2x:%2x:%2x:%2x:%2x) in MacTab!\n", PRINT_MAC(pSrcBufVA)));
+		// Resourece is low, system did not allocate virtual address
+		// return NDIS_STATUS_FAILURE directly to upper layer
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return NDIS_STATUS_FAILURE;
+	}
+
+	if (ADHOC_ON(pAd)
+		)
+	{
+		RTMP_SET_PACKET_WCID(pPacket, (UCHAR)pEntry->Aid);
+	}
+
+	//
+	// Check the Ethernet Frame type of this packet, and set the RTMP_SET_PACKET_SPECIFIC flags.
+	//		Here we set the PACKET_SPECIFIC flags(LLC, VLAN, DHCP/ARP, EAPOL).
+	RTMPCheckEtherType(pAd, pPacket);
+
+
+
+	//
+	// WPA 802.1x secured port control - drop all non-802.1x frame before port secured
+	//
+	if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+		 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+		 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+		 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+#ifdef WPA_SUPPLICANT_SUPPORT
+		  || (pAd->StaCfg.IEEE8021X == TRUE)
+#endif // WPA_SUPPLICANT_SUPPORT //
+#ifdef LEAP_SUPPORT
+		  || (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+		  )
+		  && ((pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED) || (pAd->StaCfg.MicErrCnt >= 2))
+		  && (RTMP_GET_PACKET_EAPOL(pPacket)== FALSE)
+		  )
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("STASendPacket --> Drop packet before port secured !!!\n"));
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+
+		return (NDIS_STATUS_FAILURE);
+	}
+
+
+	// STEP 1. Decide number of fragments required to deliver this MSDU.
+	//	   The estimation here is not very accurate because difficult to
+	//	   take encryption overhead into consideration here. The result
+	//	   "NumberOfFrag" is then just used to pre-check if enough free
+	//	   TXD are available to hold this MSDU.
+
+
+	if (*pSrcBufVA & 0x01)	// fragmentation not allowed on multicast & broadcast
+		NumberOfFrag = 1;
+	else if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED))
+		NumberOfFrag = 1;	// Aggregation overwhelms fragmentation
+	else if (CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_AMSDU_INUSED))
+		NumberOfFrag = 1;	// Aggregation overwhelms fragmentation
+#ifdef DOT11_N_SUPPORT
+	else if ((pAd->StaCfg.HTPhyMode.field.MODE == MODE_HTMIX) || (pAd->StaCfg.HTPhyMode.field.MODE == MODE_HTGREENFIELD))
+		NumberOfFrag = 1;	// MIMO RATE overwhelms fragmentation
+#endif // DOT11_N_SUPPORT //
+	else
+	{
+		// The calculated "NumberOfFrag" is a rough estimation because of various
+		// encryption/encapsulation overhead not taken into consideration. This number is just
+		// used to make sure enough free TXD are available before fragmentation takes place.
+		// In case the actual required number of fragments of an NDIS packet
+		// excceeds "NumberOfFrag"caculated here and not enough free TXD available, the
+		// last fragment (i.e. last MPDU) will be dropped in RTMPHardTransmit() due to out of
+		// resource, and the NDIS packet will be indicated NDIS_STATUS_FAILURE. This should
+		// rarely happen and the penalty is just like a TX RETRY fail. Affordable.
+
+		AllowFragSize = (pAd->CommonCfg.FragmentThreshold) - LENGTH_802_11 - LENGTH_CRC;
+		NumberOfFrag = ((PacketInfo.TotalPacketLength - LENGTH_802_3 + LENGTH_802_1_H) / AllowFragSize) + 1;
+		// To get accurate number of fragmentation, Minus 1 if the size just match to allowable fragment size
+		if (((PacketInfo.TotalPacketLength - LENGTH_802_3 + LENGTH_802_1_H) % AllowFragSize) == 0)
+		{
+			NumberOfFrag--;
+		}
+	}
+
+	// Save fragment number to Ndis packet reserved field
+	RTMP_SET_PACKET_FRAGMENTS(pPacket, NumberOfFrag);
+
+
+	// STEP 2. Check the requirement of RTS:
+	//	   If multiple fragment required, RTS is required only for the first fragment
+	//	   if the fragment size large than RTS threshold
+	//     For RT28xx, Let ASIC send RTS/CTS
+	RTMP_SET_PACKET_RTS(pPacket, 0);
+	RTMP_SET_PACKET_TXRATE(pPacket, pAd->CommonCfg.TxRate);
+
+	//
+	// STEP 3. Traffic classification. outcome = <UserPriority, QueIdx>
+	//
+	UserPriority = 0;
+	QueIdx		 = QID_AC_BE;
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED))
+	{
+		USHORT Protocol;
+		UCHAR  LlcSnapLen = 0, Byte0, Byte1;
+		do
+		{
+			// get Ethernet protocol field
+			Protocol = (USHORT)((pSrcBufVA[12] << 8) + pSrcBufVA[13]);
+			if (Protocol <= 1500)
+			{
+				// get Ethernet protocol field from LLC/SNAP
+				if (Sniff2BytesFromNdisBuffer(PacketInfo.pFirstBuffer, LENGTH_802_3 + 6, &Byte0, &Byte1) != NDIS_STATUS_SUCCESS)
+					break;
+
+				Protocol = (USHORT)((Byte0 << 8) + Byte1);
+				LlcSnapLen = 8;
+			}
+
+			// always AC_BE for non-IP packet
+			if (Protocol != 0x0800)
+				break;
+
+			// get IP header
+			if (Sniff2BytesFromNdisBuffer(PacketInfo.pFirstBuffer, LENGTH_802_3 + LlcSnapLen, &Byte0, &Byte1) != NDIS_STATUS_SUCCESS)
+				break;
+
+			// return AC_BE if packet is not IPv4
+			if ((Byte0 & 0xf0) != 0x40)
+				break;
+
+			FlgIsIP = 1;
+			UserPriority = (Byte1 & 0xe0) >> 5;
+			QueIdx = MapUserPriorityToAccessCategory[UserPriority];
+
+			// TODO: have to check ACM bit. apply TSPEC if ACM is ON
+			// TODO: downgrade UP & QueIdx before passing ACM
+			if (pAd->CommonCfg.APEdcaParm.bACM[QueIdx])
+			{
+				UserPriority = 0;
+				QueIdx		 = QID_AC_BE;
+			}
+		} while (FALSE);
+	}
+
+	RTMP_SET_PACKET_UP(pPacket, UserPriority);
+
+
+
+	// Make sure SendTxWait queue resource won't be used by other threads
+	RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
+	if (pAd->TxSwQueue[QueIdx].Number >= MAX_PACKETS_IN_QUEUE)
+	{
+		RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+#ifdef BLOCK_NET_IF
+		StopNetIfQueue(pAd, QueIdx, pPacket);
+#endif // BLOCK_NET_IF //
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+
+		return NDIS_STATUS_FAILURE;
+	}
+	else
+	{
+		InsertTailQueue(&pAd->TxSwQueue[QueIdx], PACKET_TO_QUEUE_ENTRY(pPacket));
+	}
+	RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+
+#ifdef DOT11_N_SUPPORT
+    if ((pAd->CommonCfg.BACapability.field.AutoBA == TRUE)&&
+        (pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE))
+	{
+		if (((pEntry->TXBAbitmap & (1<<UserPriority)) == 0) &&
+            ((pEntry->BADeclineBitmap & (1<<UserPriority)) == 0) &&
+            (pEntry->PortSecured == WPA_802_1X_PORT_SECURED)
+			 // For IOT compatibility, if
+			 // 1. It is Ralink chip or
+			 // 2. It is OPEN or AES mode,
+			 // then BA session can be bulit.
+			 && ((pEntry->ValidAsCLI && pAd->MlmeAux.APRalinkIe != 0x0) ||
+			 	 (pEntry->WepStatus == Ndis802_11WEPDisabled || pEntry->WepStatus == Ndis802_11Encryption3Enabled))
+			)
+		{
+			BAOriSessionSetUp(pAd, pEntry, 0, 0, 10, FALSE);
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	pAd->RalinkCounters.OneSecOsTxCount[QueIdx]++; // TODO: for debug only. to be removed
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		This subroutine will scan through releative ring descriptor to find
+		out avaliable free ring descriptor and compare with request size.
+
+	Arguments:
+		pAd Pointer to our adapter
+		QueIdx		Selected TX Ring
+
+	Return Value:
+		NDIS_STATUS_FAILURE 	Not enough free descriptor
+		NDIS_STATUS_SUCCESS 	Enough free descriptor
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+#ifdef RT2860
+NDIS_STATUS RTMPFreeTXDRequest(
+	IN		PRTMP_ADAPTER	pAd,
+	IN		UCHAR			QueIdx,
+	IN		UCHAR			NumberRequired,
+	IN		PUCHAR			FreeNumberIs)
+{
+	ULONG		FreeNumber = 0;
+	NDIS_STATUS 	Status = NDIS_STATUS_FAILURE;
+
+	switch (QueIdx)
+	{
+		case QID_AC_BK:
+		case QID_AC_BE:
+		case QID_AC_VI:
+		case QID_AC_VO:
+		case QID_HCCA:
+			if (pAd->TxRing[QueIdx].TxSwFreeIdx > pAd->TxRing[QueIdx].TxCpuIdx)
+				FreeNumber = pAd->TxRing[QueIdx].TxSwFreeIdx - pAd->TxRing[QueIdx].TxCpuIdx - 1;
+			else
+				FreeNumber = pAd->TxRing[QueIdx].TxSwFreeIdx + TX_RING_SIZE - pAd->TxRing[QueIdx].TxCpuIdx - 1;
+
+			if (FreeNumber >= NumberRequired)
+				Status = NDIS_STATUS_SUCCESS;
+			break;
+
+		case QID_MGMT:
+			if (pAd->MgmtRing.TxSwFreeIdx > pAd->MgmtRing.TxCpuIdx)
+				FreeNumber = pAd->MgmtRing.TxSwFreeIdx - pAd->MgmtRing.TxCpuIdx - 1;
+			else
+				FreeNumber = pAd->MgmtRing.TxSwFreeIdx + MGMT_RING_SIZE - pAd->MgmtRing.TxCpuIdx - 1;
+
+			if (FreeNumber >= NumberRequired)
+				Status = NDIS_STATUS_SUCCESS;
+			break;
+
+		default:
+			DBGPRINT(RT_DEBUG_ERROR,("RTMPFreeTXDRequest::Invalid QueIdx(=%d)\n", QueIdx));
+			break;
+	}
+	*FreeNumberIs = (UCHAR)FreeNumber;
+
+	return (Status);
+}
+#endif // RT2860 //
+
+
+
+VOID RTMPSendDisassociationFrame(
+	IN	PRTMP_ADAPTER	pAd)
+{
+}
+
+VOID	RTMPSendNullFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			TxRate,
+	IN	BOOLEAN 		bQosNull)
+{
+	UCHAR	NullFrame[48];
+	ULONG	Length;
+	PHEADER_802_11	pHeader_802_11;
+
+
+#ifdef RALINK_ATE
+	if(ATE_ON(pAd))
+	{
+		return;
+	}
+#endif // RALINK_ATE //
+
+    // WPA 802.1x secured port control
+    if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+         (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+         (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+         (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+#ifdef WPA_SUPPLICANT_SUPPORT
+			  || (pAd->StaCfg.IEEE8021X == TRUE)
+#endif
+        ) &&
+       (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+	{
+		return;
+	}
+
+	NdisZeroMemory(NullFrame, 48);
+	Length = sizeof(HEADER_802_11);
+
+	pHeader_802_11 = (PHEADER_802_11) NullFrame;
+
+	pHeader_802_11->FC.Type = BTYPE_DATA;
+	pHeader_802_11->FC.SubType = SUBTYPE_NULL_FUNC;
+	pHeader_802_11->FC.ToDs = 1;
+	COPY_MAC_ADDR(pHeader_802_11->Addr1, pAd->CommonCfg.Bssid);
+	COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
+
+	if (pAd->CommonCfg.bAPSDForcePowerSave)
+	{
+		pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+	}
+	else
+	{
+		pHeader_802_11->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE) ? 1: 0;
+	}
+	pHeader_802_11->Duration = pAd->CommonCfg.Dsifs + RTMPCalcDuration(pAd, TxRate, 14);
+
+	pAd->Sequence++;
+	pHeader_802_11->Sequence = pAd->Sequence;
+
+	// Prepare QosNull function frame
+	if (bQosNull)
+	{
+		pHeader_802_11->FC.SubType = SUBTYPE_QOS_NULL;
+
+		// copy QOS control bytes
+		NullFrame[Length]	=  0;
+		NullFrame[Length+1] =  0;
+		Length += 2;// if pad with 2 bytes for alignment, APSD will fail
+	}
+
+	HAL_KickOutNullFrameTx(pAd, 0, NullFrame, Length);
+
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID	RTMPSendRTSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA,
+	IN	unsigned int	NextMpduSize,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			RTSRate,
+	IN	USHORT			AckDuration,
+	IN	UCHAR			QueIdx,
+	IN	UCHAR			FrameGap)
+{
+}
+
+
+
+// --------------------------------------------------------
+//  FIND ENCRYPT KEY AND DECIDE CIPHER ALGORITHM
+//		Find the WPA key, either Group or Pairwise Key
+//		LEAP + TKIP also use WPA key.
+// --------------------------------------------------------
+// Decide WEP bit and cipher suite to be used. Same cipher suite should be used for whole fragment burst
+// In Cisco CCX 2.0 Leap Authentication
+//		   WepStatus is Ndis802_11Encryption1Enabled but the key will use PairwiseKey
+//		   Instead of the SharedKey, SharedKey Length may be Zero.
+VOID STAFindCipherAlgorithm(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	NDIS_802_11_ENCRYPTION_STATUS	Cipher;				// To indicate cipher used for this packet
+	UCHAR							CipherAlg = CIPHER_NONE;		// cipher alogrithm
+	UCHAR							KeyIdx = 0xff;
+	PUCHAR							pSrcBufVA;
+	PCIPHER_KEY						pKey = NULL;
+
+	pSrcBufVA = GET_OS_PKT_DATAPTR(pTxBlk->pPacket);
+
+	{
+	    // Select Cipher
+	    if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd)))
+	        Cipher = pAd->StaCfg.GroupCipher; // Cipher for Multicast or Broadcast
+	    else
+	        Cipher = pAd->StaCfg.PairCipher; // Cipher for Unicast
+
+		if (RTMP_GET_PACKET_EAPOL(pTxBlk->pPacket))
+		{
+			ASSERT(pAd->SharedKey[BSS0][0].CipherAlg <= CIPHER_CKIP128);
+
+			// 4-way handshaking frame must be clear
+			if (!(TX_BLK_TEST_FLAG(pTxBlk, fTX_bClearEAPFrame)) && (pAd->SharedKey[BSS0][0].CipherAlg) &&
+				(pAd->SharedKey[BSS0][0].KeyLen))
+			{
+				CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+				KeyIdx = 0;
+			}
+		}
+		else if (Cipher == Ndis802_11Encryption1Enabled)
+		{
+#ifdef LEAP_SUPPORT
+			if (pAd->StaCfg.CkipFlag & 0x10) // Cisco CKIP KP is on
+			{
+				if (LEAP_CCKM_ON(pAd))
+				{
+					if (((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd))))
+						KeyIdx = 1;
+					else
+						KeyIdx = 0;
+				}
+				else
+					KeyIdx = pAd->StaCfg.DefaultKeyId;
+			}
+			else if (pAd->StaCfg.CkipFlag & 0x08) // only CKIP CMIC
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+			else if (LEAP_CCKM_ON(pAd))
+			{
+				if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd)))
+					KeyIdx = 1;
+				else
+					KeyIdx = 0;
+			}
+			else	// standard WEP64 or WEP128
+#endif // LEAP_SUPPORT //
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+		}
+		else if ((Cipher == Ndis802_11Encryption2Enabled) ||
+				 (Cipher == Ndis802_11Encryption3Enabled))
+		{
+			if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd))) // multicast
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+			else if (pAd->SharedKey[BSS0][0].KeyLen)
+				KeyIdx = 0;
+			else
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+		}
+
+		if (KeyIdx == 0xff)
+			CipherAlg = CIPHER_NONE;
+		else if ((Cipher == Ndis802_11EncryptionDisabled) || (pAd->SharedKey[BSS0][KeyIdx].KeyLen == 0))
+			CipherAlg = CIPHER_NONE;
+#ifdef WPA_SUPPLICANT_SUPPORT
+	    else if ( pAd->StaCfg.WpaSupplicantUP &&
+	             (Cipher == Ndis802_11Encryption1Enabled) &&
+	             (pAd->StaCfg.IEEE8021X == TRUE) &&
+	             (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+	        CipherAlg = CIPHER_NONE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+		else
+		{
+			//Header_802_11.FC.Wep = 1;
+			CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+			pKey = &pAd->SharedKey[BSS0][KeyIdx];
+		}
+	}
+
+	pTxBlk->CipherAlg = CipherAlg;
+	pTxBlk->pKey = pKey;
+}
+
+
+VOID STABuildCommon802_11Header(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  TX_BLK          *pTxBlk)
+{
+
+	HEADER_802_11	*pHeader_802_11;
+#ifdef QOS_DLS_SUPPORT
+	BOOLEAN	bDLSFrame = FALSE;
+	INT	DlsEntryIndex = 0;
+#endif // QOS_DLS_SUPPORT //
+
+	//
+	// MAKE A COMMON 802.11 HEADER
+	//
+
+	// normal wlan header size : 24 octets
+	pTxBlk->MpduHeaderLen = sizeof(HEADER_802_11);
+
+	pHeader_802_11 = (HEADER_802_11 *) &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+
+	NdisZeroMemory(pHeader_802_11, sizeof(HEADER_802_11));
+
+	pHeader_802_11->FC.FrDs = 0;
+	pHeader_802_11->FC.Type = BTYPE_DATA;
+	pHeader_802_11->FC.SubType = ((TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM)) ? SUBTYPE_QDATA : SUBTYPE_DATA);
+
+#ifdef QOS_DLS_SUPPORT
+	if (INFRA_ON(pAd))
+	{
+		// Check if the frame can be sent through DLS direct link interface
+		// If packet can be sent through DLS, then force aggregation disable. (Hard to determine peer STA's capability)
+		DlsEntryIndex = RTMPCheckDLSFrame(pAd, pTxBlk->pSrcBufHeader);
+		if (DlsEntryIndex >= 0)
+			bDLSFrame = TRUE;
+		else
+			bDLSFrame = FALSE;
+	}
+#endif // QOS_DLS_SUPPORT //
+
+    if (pTxBlk->pMacEntry)
+	{
+		if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bForceNonQoS))
+		{
+			pHeader_802_11->Sequence = pTxBlk->pMacEntry->NonQosDataSeq;
+			pTxBlk->pMacEntry->NonQosDataSeq = (pTxBlk->pMacEntry->NonQosDataSeq+1) & MAXSEQ;
+		}
+		else
+		{
+#ifdef QOS_DLS_SUPPORT
+			if (bDLSFrame)
+			{
+				pHeader_802_11->Sequence = pAd->StaCfg.DLSEntry[DlsEntryIndex].Sequence;
+				pAd->StaCfg.DLSEntry[DlsEntryIndex].Sequence = (pAd->StaCfg.DLSEntry[DlsEntryIndex].Sequence+1) & MAXSEQ;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+			{
+    	    pHeader_802_11->Sequence = pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority];
+    	    pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority] = (pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority]+1) & MAXSEQ;
+    	}
+	}
+	}
+	else
+	{
+		pHeader_802_11->Sequence = pAd->Sequence;
+		pAd->Sequence = (pAd->Sequence+1) & MAXSEQ; // next sequence
+	}
+
+	pHeader_802_11->Frag = 0;
+
+	pHeader_802_11->FC.MoreData = TX_BLK_TEST_FLAG(pTxBlk, fTX_bMoreData);
+
+	{
+		if (INFRA_ON(pAd))
+		{
+#ifdef QOS_DLS_SUPPORT
+			if (bDLSFrame)
+			{
+				COPY_MAC_ADDR(pHeader_802_11->Addr1, pTxBlk->pSrcBufHeader);
+				COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+				COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
+				pHeader_802_11->FC.ToDs = 0;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+			{
+			COPY_MAC_ADDR(pHeader_802_11->Addr1, pAd->CommonCfg.Bssid);
+			COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+			COPY_MAC_ADDR(pHeader_802_11->Addr3, pTxBlk->pSrcBufHeader);
+			pHeader_802_11->FC.ToDs = 1;
+		}
+		}
+		else if (ADHOC_ON(pAd))
+		{
+			COPY_MAC_ADDR(pHeader_802_11->Addr1, pTxBlk->pSrcBufHeader);
+			COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+			COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
+			pHeader_802_11->FC.ToDs = 0;
+		}
+	}
+
+	if (pTxBlk->CipherAlg != CIPHER_NONE)
+		pHeader_802_11->FC.Wep = 1;
+
+	// -----------------------------------------------------------------
+	// STEP 2. MAKE A COMMON 802.11 HEADER SHARED BY ENTIRE FRAGMENT BURST. Fill sequence later.
+	// -----------------------------------------------------------------
+	if (pAd->CommonCfg.bAPSDForcePowerSave)
+    	pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+	else
+    	pHeader_802_11->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
+}
+
+#ifdef DOT11_N_SUPPORT
+VOID STABuildCache802_11Header(
+	IN RTMP_ADAPTER		*pAd,
+	IN TX_BLK			*pTxBlk,
+	IN UCHAR			*pHeader)
+{
+	MAC_TABLE_ENTRY	*pMacEntry;
+	PHEADER_802_11	pHeader80211;
+
+	pHeader80211 = (PHEADER_802_11)pHeader;
+	pMacEntry = pTxBlk->pMacEntry;
+
+	//
+	// Update the cached 802.11 HEADER
+	//
+
+	// normal wlan header size : 24 octets
+	pTxBlk->MpduHeaderLen = sizeof(HEADER_802_11);
+
+	// More Bit
+	pHeader80211->FC.MoreData = TX_BLK_TEST_FLAG(pTxBlk, fTX_bMoreData);
+
+	// Sequence
+	pHeader80211->Sequence = pMacEntry->TxSeq[pTxBlk->UserPriority];
+    pMacEntry->TxSeq[pTxBlk->UserPriority] = (pMacEntry->TxSeq[pTxBlk->UserPriority]+1) & MAXSEQ;
+
+	{
+		// Check if the frame can be sent through DLS direct link interface
+		// If packet can be sent through DLS, then force aggregation disable. (Hard to determine peer STA's capability)
+#ifdef QOS_DLS_SUPPORT
+		BOOLEAN	bDLSFrame = FALSE;
+		INT	DlsEntryIndex = 0;
+
+		DlsEntryIndex = RTMPCheckDLSFrame(pAd, pTxBlk->pSrcBufHeader);
+		if (DlsEntryIndex >= 0)
+			bDLSFrame = TRUE;
+		else
+			bDLSFrame = FALSE;
+#endif // QOS_DLS_SUPPORT //
+
+		// The addr3 of normal packet send from DS is Dest Mac address.
+#ifdef QOS_DLS_SUPPORT
+		if (bDLSFrame)
+		{
+			COPY_MAC_ADDR(pHeader80211->Addr1, pTxBlk->pSrcBufHeader);
+			COPY_MAC_ADDR(pHeader80211->Addr3, pAd->CommonCfg.Bssid);
+			pHeader80211->FC.ToDs = 0;
+		}
+		else
+#endif // QOS_DLS_SUPPORT //
+		if (ADHOC_ON(pAd))
+			COPY_MAC_ADDR(pHeader80211->Addr3, pAd->CommonCfg.Bssid);
+		else
+			COPY_MAC_ADDR(pHeader80211->Addr3, pTxBlk->pSrcBufHeader);
+	}
+
+	// -----------------------------------------------------------------
+	// STEP 2. MAKE A COMMON 802.11 HEADER SHARED BY ENTIRE FRAGMENT BURST. Fill sequence later.
+	// -----------------------------------------------------------------
+	if (pAd->CommonCfg.bAPSDForcePowerSave)
+    	pHeader80211->FC.PwrMgmt = PWR_SAVE;
+	else
+    	pHeader80211->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
+}
+#endif // DOT11_N_SUPPORT //
+
+static inline PUCHAR STA_Build_ARalink_Frame_Header(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK		*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;
+	HEADER_802_11	*pHeader_802_11;
+	PNDIS_PACKET	pNextPacket;
+	UINT32			nextBufLen;
+	PQUEUE_ENTRY	pQEntry;
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+	// steal "order" bit to mark "aggregation"
+	pHeader_802_11->FC.Order = 1;
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
+	{
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+	}
+
+	// padding at front of LLC header. LLC header should at 4-bytes aligment.
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR)ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+	// For RA Aggregation,
+	// put the 2nd MSDU length(extra 2-byte field) after QOS_CONTROL in little endian format
+	pQEntry = pTxBlk->TxPacketList.Head;
+	pNextPacket = QUEUE_ENTRY_TO_PKT(pQEntry);
+	nextBufLen = GET_OS_PKT_LEN(pNextPacket);
+	if (RTMP_GET_PACKET_VLAN(pNextPacket))
+		nextBufLen -= LENGTH_802_1Q;
+
+	*pHeaderBufPtr = (UCHAR)nextBufLen & 0xff;
+	*(pHeaderBufPtr+1) = (UCHAR)(nextBufLen >> 8);
+
+	pHeaderBufPtr += 2;
+	pTxBlk->MpduHeaderLen += 2;
+
+	return pHeaderBufPtr;
+
+}
+
+#ifdef DOT11_N_SUPPORT
+static inline PUCHAR STA_Build_AMSDU_Frame_Header(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK		*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;//, pSaveBufPtr;
+	HEADER_802_11	*pHeader_802_11;
+
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	//
+	// build QOS Control bytes
+	//
+	*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+
+	//
+	// A-MSDU packet
+	//
+	*pHeaderBufPtr |= 0x80;
+
+	*(pHeaderBufPtr+1) = 0;
+	pHeaderBufPtr +=2;
+	pTxBlk->MpduHeaderLen += 2;
+
+	//pSaveBufPtr = pHeaderBufPtr;
+
+	//
+	// padding at front of LLC header
+	// LLC header should locate at 4-octets aligment
+	//
+	// @@@ MpduHeaderLen excluding padding @@@
+	//
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+	return pHeaderBufPtr;
+
+}
+
+
+VOID STA_AMPDU_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	HEADER_802_11	*pHeader_802_11;
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	MAC_TABLE_ENTRY	*pMacEntry;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+	ASSERT(pTxBlk);
+
+	while(pTxBlk->TxPacketList.Head)
+	{
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+		if ( RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+		{
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			continue;
+		}
+
+		bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+		pMacEntry = pTxBlk->pMacEntry;
+		if (pMacEntry->isCached)
+		{
+			// NOTE: Please make sure the size of pMacEntry->CachedBuf[] is smaller than pTxBlk->HeaderBuf[]!!!!
+			NdisMoveMemory((PUCHAR)&pTxBlk->HeaderBuf[TXINFO_SIZE], (PUCHAR)&pMacEntry->CachedBuf[0], TXWI_SIZE + sizeof(HEADER_802_11));
+			pHeaderBufPtr = (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE]);
+			STABuildCache802_11Header(pAd, pTxBlk, pHeaderBufPtr);
+		}
+		else
+		{
+			STAFindCipherAlgorithm(pAd, pTxBlk);
+			STABuildCommon802_11Header(pAd, pTxBlk);
+
+			pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+		}
+
+
+		pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+		// skip common header
+		pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+
+		//
+		// build HTC+
+		// HTC control filed following QoS field
+		//
+		if ((pAd->CommonCfg.bRdg == TRUE) && CLIENT_STATUS_TEST_FLAG(pTxBlk->pMacEntry, fCLIENT_STATUS_RDG_CAPABLE))
+		{
+			if (pMacEntry->isCached == FALSE)
+			{
+				// mark HTC bit
+				pHeader_802_11->FC.Order = 1;
+
+				NdisZeroMemory(pHeaderBufPtr, 4);
+				*(pHeaderBufPtr+3) |= 0x80;
+			}
+			pHeaderBufPtr += 4;
+			pTxBlk->MpduHeaderLen += 4;
+		}
+
+		//pTxBlk->MpduHeaderLen = pHeaderBufPtr - pTxBlk->HeaderBuf - TXWI_SIZE - TXINFO_SIZE;
+		ASSERT(pTxBlk->MpduHeaderLen >= 24);
+
+		// skip 802.3 header
+		pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+		pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+		// skip vlan tag
+		if (bVLANPkt)
+		{
+			pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+			pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+		}
+
+		//
+		// padding at front of LLC header
+		// LLC header should locate at 4-octets aligment
+		//
+		// @@@ MpduHeaderLen excluding padding @@@
+		//
+		pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+		pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+		pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+		{
+
+			//
+			// Insert LLC-SNAP encapsulation - 8 octets
+			//
+			EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
+			if (pTxBlk->pExtraLlcSnapEncap)
+			{
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+				pHeaderBufPtr += 6;
+				// get 2 octets (TypeofLen)
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+				pHeaderBufPtr += 2;
+				pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+			}
+
+		}
+
+		if (pMacEntry->isCached)
+		{
+            RTMPWriteTxWI_Cache(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+		}
+		else
+		{
+			RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+			NdisZeroMemory((PUCHAR)(&pMacEntry->CachedBuf[0]), sizeof(pMacEntry->CachedBuf));
+			NdisMoveMemory((PUCHAR)(&pMacEntry->CachedBuf[0]), (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), (pHeaderBufPtr - (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE])));
+			pMacEntry->isCached = TRUE;
+		}
+
+		// calculate Transmitted AMPDU count and ByteCount
+		{
+			pAd->RalinkCounters.TransmittedMPDUsInAMPDUCount.u.LowPart ++;
+			pAd->RalinkCounters.TransmittedOctetsInAMPDUCount.QuadPart += pTxBlk->SrcBufLen;
+		}
+
+		//FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
+
+		HAL_WriteTxResource(pAd, pTxBlk, TRUE, &FreeNumber);
+
+		//
+		// Kick out Tx
+		//
+		HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+	}
+
+}
+
+
+VOID STA_AMSDU_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	USHORT			subFramePayloadLen = 0;	// AMSDU Subframe length without AMSDU-Header / Padding.
+	USHORT			totalMPDUSize=0;
+	UCHAR			*subFrameHeader;
+	UCHAR			padding = 0;
+	USHORT			FirstTx = 0, LastTxIdx = 0;
+	BOOLEAN			bVLANPkt;
+	int 			frameNum = 0;
+	PQUEUE_ENTRY	pQEntry;
+
+
+	ASSERT(pTxBlk);
+
+	ASSERT((pTxBlk->TxPacketList.Number > 1));
+
+	while(pTxBlk->TxPacketList.Head)
+	{
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+		if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+		{
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			continue;
+		}
+
+		bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+		// skip 802.3 header
+		pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+		pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+		// skip vlan tag
+		if (bVLANPkt)
+		{
+			pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+			pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+		}
+
+		if (frameNum == 0)
+		{
+			pHeaderBufPtr = STA_Build_AMSDU_Frame_Header(pAd, pTxBlk);
+
+			// NOTE: TxWI->MPDUtotalByteCount will be updated after final frame was handled.
+			RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+		}
+		else
+		{
+			pHeaderBufPtr = &pTxBlk->HeaderBuf[0];
+			padding = ROUND_UP(LENGTH_AMSDU_SUBFRAMEHEAD + subFramePayloadLen, 4) - (LENGTH_AMSDU_SUBFRAMEHEAD + subFramePayloadLen);
+			NdisZeroMemory(pHeaderBufPtr, padding + LENGTH_AMSDU_SUBFRAMEHEAD);
+			pHeaderBufPtr += padding;
+			pTxBlk->MpduHeaderLen = padding;
+		}
+
+		//
+		// A-MSDU subframe
+		//   DA(6)+SA(6)+Length(2) + LLC/SNAP Encap
+		//
+		subFrameHeader = pHeaderBufPtr;
+		subFramePayloadLen = pTxBlk->SrcBufLen;
+
+		NdisMoveMemory(subFrameHeader, pTxBlk->pSrcBufHeader, 12);
+
+
+		pHeaderBufPtr += LENGTH_AMSDU_SUBFRAMEHEAD;
+		pTxBlk->MpduHeaderLen += LENGTH_AMSDU_SUBFRAMEHEAD;
+
+
+		//
+		// Insert LLC-SNAP encapsulation - 8 octets
+		//
+		EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
+
+		subFramePayloadLen = pTxBlk->SrcBufLen;
+
+		if (pTxBlk->pExtraLlcSnapEncap)
+		{
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+			pHeaderBufPtr += 6;
+			// get 2 octets (TypeofLen)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+			pHeaderBufPtr += 2;
+			pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+			subFramePayloadLen += LENGTH_802_1_H;
+		}
+
+		// update subFrame Length field
+		subFrameHeader[12] = (subFramePayloadLen & 0xFF00) >> 8;
+		subFrameHeader[13] = subFramePayloadLen & 0xFF;
+
+		totalMPDUSize += pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+
+		if (frameNum ==0)
+			FirstTx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+		else
+			LastTxIdx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+
+		frameNum++;
+
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+
+		// calculate Transmitted AMSDU Count and ByteCount
+		{
+			pAd->RalinkCounters.TransmittedAMSDUCount.u.LowPart ++;
+			pAd->RalinkCounters.TransmittedOctetsInAMSDU.QuadPart += totalMPDUSize;
+		}
+
+	}
+
+	HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, FirstTx);
+	HAL_LastTxIdx(pAd, pTxBlk->QueIdx, LastTxIdx);
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+}
+#endif // DOT11_N_SUPPORT //
+
+VOID STA_Legacy_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	HEADER_802_11	*pHeader_802_11;
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+	ASSERT(pTxBlk);
+
+
+	pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+	pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+	if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+	{
+		RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	if (pTxBlk->TxFrameType == TX_MCAST_FRAME)
+	{
+		INC_COUNTER64(pAd->WlanCounters.MulticastTransmittedFrameCount);
+	}
+
+	if (RTMP_GET_PACKET_RTS(pTxBlk->pPacket))
+		TX_BLK_SET_FLAG(pTxBlk, fTX_bRtsRequired);
+	else
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bRtsRequired);
+
+	bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+	if (pTxBlk->TxRate < pAd->CommonCfg.MinTxRate)
+		pTxBlk->TxRate = pAd->CommonCfg.MinTxRate;
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+
+	// skip 802.3 header
+	pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+	pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+	// skip vlan tag
+	if (bVLANPkt)
+	{
+		pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+		pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+	}
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
+	{
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+	}
+
+	// The remaining content of MPDU header should locate at 4-octets aligment
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+	{
+
+		//
+		// Insert LLC-SNAP encapsulation - 8 octets
+		//
+		//
+   		// if original Ethernet frame contains no LLC/SNAP,
+		// then an extra LLC/SNAP encap is required
+		//
+		EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(pTxBlk->pSrcBufHeader, pTxBlk->pExtraLlcSnapEncap);
+		if (pTxBlk->pExtraLlcSnapEncap)
+		{
+			UCHAR vlan_size;
+
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+			pHeaderBufPtr += 6;
+			// skip vlan tag
+			vlan_size =  (bVLANPkt) ? LENGTH_802_1Q : 0;
+			// get 2 octets (TypeofLen)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader+12+vlan_size, 2);
+			pHeaderBufPtr += 2;
+			pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+		}
+
+	}
+
+	//
+	// prepare for TXWI
+	// use Wcid as Key Index
+	//
+
+	RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+	//FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
+
+	HAL_WriteTxResource(pAd, pTxBlk, TRUE, &FreeNumber);
+
+	pAd->RalinkCounters.KickTxCount++;
+	pAd->RalinkCounters.OneSecTxDoneCount++;
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+}
+
+
+VOID STA_ARalink_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	USHORT			totalMPDUSize=0;
+	USHORT			FirstTx, LastTxIdx;
+	int 			frameNum = 0;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+
+	ASSERT(pTxBlk);
+
+	ASSERT((pTxBlk->TxPacketList.Number== 2));
+
+
+	FirstTx = LastTxIdx = 0;  // Is it ok init they as 0?
+	while(pTxBlk->TxPacketList.Head)
+	{
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+
+		if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+		{
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			continue;
+		}
+
+		bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+		// skip 802.3 header
+		pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+		pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+		// skip vlan tag
+		if (bVLANPkt)
+		{
+			pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+			pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+		}
+
+		if (frameNum == 0)
+		{	// For first frame, we need to create the 802.11 header + padding(optional) + RA-AGG-LEN + SNAP Header
+
+			pHeaderBufPtr = STA_Build_ARalink_Frame_Header(pAd, pTxBlk);
+
+			// It's ok write the TxWI here, because the TxWI->MPDUtotalByteCount
+			//	will be updated after final frame was handled.
+			RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+
+			//
+			// Insert LLC-SNAP encapsulation - 8 octets
+			//
+			EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
+
+			if (pTxBlk->pExtraLlcSnapEncap)
+			{
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+				pHeaderBufPtr += 6;
+				// get 2 octets (TypeofLen)
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+				pHeaderBufPtr += 2;
+				pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+			}
+		}
+		else
+		{	// For second aggregated frame, we need create the 802.3 header to headerBuf, because PCI will copy it to SDPtr0.
+
+			pHeaderBufPtr = &pTxBlk->HeaderBuf[0];
+			pTxBlk->MpduHeaderLen = 0;
+
+			// A-Ralink sub-sequent frame header is the same as 802.3 header.
+			//   DA(6)+SA(6)+FrameType(2)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader, 12);
+			pHeaderBufPtr += 12;
+			// get 2 octets (TypeofLen)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+			pHeaderBufPtr += 2;
+			pTxBlk->MpduHeaderLen = LENGTH_ARALINK_SUBFRAMEHEAD;
+		}
+
+		totalMPDUSize += pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+
+		//FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
+		if (frameNum ==0)
+			FirstTx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+		else
+			LastTxIdx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+
+		frameNum++;
+
+		pAd->RalinkCounters.OneSecTxAggregationCount++;
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+
+	}
+
+	HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, FirstTx);
+	HAL_LastTxIdx(pAd, pTxBlk->QueIdx, LastTxIdx);
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+
+}
+
+
+VOID STA_Fragment_Frame_Tx(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK		*pTxBlk)
+{
+	HEADER_802_11	*pHeader_802_11;
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	UCHAR 			fragNum = 0;
+	PACKET_INFO		PacketInfo;
+	USHORT			EncryptionOverhead = 0;
+	UINT32			FreeMpduSize, SrcRemainingBytes;
+	USHORT			AckDuration;
+	UINT 			NextMpduSize;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+
+	ASSERT(pTxBlk);
+
+	pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+	pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+	if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+	{
+		RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	ASSERT(TX_BLK_TEST_FLAG(pTxBlk, fTX_bAllowFrag));
+	bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+	if (pTxBlk->CipherAlg == CIPHER_TKIP)
+	{
+		pTxBlk->pPacket = duplicate_pkt_with_TKIP_MIC(pAd, pTxBlk->pPacket);
+		if (pTxBlk->pPacket == NULL)
+			return;
+		RTMP_QueryPacketInfo(pTxBlk->pPacket, &PacketInfo, &pTxBlk->pSrcBufHeader, &pTxBlk->SrcBufLen);
+	}
+
+	// skip 802.3 header
+	pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+	pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+
+	// skip vlan tag
+	if (bVLANPkt)
+	{
+		pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+		pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+	}
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *)pHeaderBufPtr;
+
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
+	{
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+	}
+
+	//
+	// padding at front of LLC header
+	// LLC header should locate at 4-octets aligment
+	//
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+
+
+	//
+	// Insert LLC-SNAP encapsulation - 8 octets
+	//
+	//
+   	// if original Ethernet frame contains no LLC/SNAP,
+	// then an extra LLC/SNAP encap is required
+	//
+	EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(pTxBlk->pSrcBufHeader, pTxBlk->pExtraLlcSnapEncap);
+	if (pTxBlk->pExtraLlcSnapEncap)
+	{
+		UCHAR vlan_size;
+
+		NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+		pHeaderBufPtr += 6;
+		// skip vlan tag
+		vlan_size =  (bVLANPkt) ? LENGTH_802_1Q : 0;
+		// get 2 octets (TypeofLen)
+		NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader+12+vlan_size, 2);
+		pHeaderBufPtr += 2;
+		pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+	}
+
+
+	// If TKIP is used and fragmentation is required. Driver has to
+	//	append TKIP MIC at tail of the scatter buffer
+	//	MAC ASIC will only perform IV/EIV/ICV insertion but no TKIP MIC
+	if (pTxBlk->CipherAlg == CIPHER_TKIP)
+	{
+
+		// NOTE: DON'T refer the skb->len directly after following copy. Becasue the length is not adjust
+		//			to correct lenght, refer to pTxBlk->SrcBufLen for the packet length in following progress.
+		NdisMoveMemory(pTxBlk->pSrcBufData + pTxBlk->SrcBufLen, &pAd->PrivateInfo.Tx.MIC[0], 8);
+		//skb_put((RTPKT_TO_OSPKT(pTxBlk->pPacket))->tail, 8);
+		pTxBlk->SrcBufLen += 8;
+		pTxBlk->TotalFrameLen += 8;
+		pTxBlk->CipherAlg = CIPHER_TKIP_NO_MIC;
+	}
+
+	//
+	// calcuate the overhead bytes that encryption algorithm may add. This
+	// affects the calculate of "duration" field
+	//
+	if ((pTxBlk->CipherAlg == CIPHER_WEP64) || (pTxBlk->CipherAlg == CIPHER_WEP128))
+		EncryptionOverhead = 8; //WEP: IV[4] + ICV[4];
+	else if (pTxBlk->CipherAlg == CIPHER_TKIP_NO_MIC)
+		EncryptionOverhead = 12;//TKIP: IV[4] + EIV[4] + ICV[4], MIC will be added to TotalPacketLength
+	else if (pTxBlk->CipherAlg == CIPHER_TKIP)
+		EncryptionOverhead = 20;//TKIP: IV[4] + EIV[4] + ICV[4] + MIC[8]
+	else if (pTxBlk->CipherAlg == CIPHER_AES)
+		EncryptionOverhead = 16;	// AES: IV[4] + EIV[4] + MIC[8]
+	else
+		EncryptionOverhead = 0;
+
+	// decide how much time an ACK/CTS frame will consume in the air
+	AckDuration = RTMPCalcDuration(pAd, pAd->CommonCfg.ExpectedACKRate[pTxBlk->TxRate], 14);
+
+	// Init the total payload length of this frame.
+	SrcRemainingBytes = pTxBlk->SrcBufLen;
+
+	pTxBlk->TotalFragNum = 0xff;
+
+	do {
+
+		FreeMpduSize = pAd->CommonCfg.FragmentThreshold - LENGTH_CRC;
+
+		FreeMpduSize -= pTxBlk->MpduHeaderLen;
+
+		if (SrcRemainingBytes <= FreeMpduSize)
+		{	// this is the last or only fragment
+
+			pTxBlk->SrcBufLen = SrcRemainingBytes;
+
+			pHeader_802_11->FC.MoreFrag = 0;
+			pHeader_802_11->Duration = pAd->CommonCfg.Dsifs + AckDuration;
+
+			// Indicate the lower layer that this's the last fragment.
+			pTxBlk->TotalFragNum = fragNum;
+		}
+		else
+		{	// more fragment is required
+
+			pTxBlk->SrcBufLen = FreeMpduSize;
+
+			NextMpduSize = min(((UINT)SrcRemainingBytes - pTxBlk->SrcBufLen), ((UINT)pAd->CommonCfg.FragmentThreshold));
+			pHeader_802_11->FC.MoreFrag = 1;
+			pHeader_802_11->Duration = (3 * pAd->CommonCfg.Dsifs) + (2 * AckDuration) + RTMPCalcDuration(pAd, pTxBlk->TxRate, NextMpduSize + EncryptionOverhead);
+		}
+
+		if (fragNum == 0)
+			pTxBlk->FrameGap = IFS_HTTXOP;
+		else
+			pTxBlk->FrameGap = IFS_SIFS;
+
+		RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+		HAL_WriteFragTxResource(pAd, pTxBlk, fragNum, &FreeNumber);
+
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+
+		// Update the frame number, remaining size of the NDIS packet payload.
+
+		// space for 802.11 header.
+		if (fragNum == 0 && pTxBlk->pExtraLlcSnapEncap)
+			pTxBlk->MpduHeaderLen -= LENGTH_802_1_H;
+
+		fragNum++;
+		SrcRemainingBytes -= pTxBlk->SrcBufLen;
+		pTxBlk->pSrcBufData += pTxBlk->SrcBufLen;
+
+		pHeader_802_11->Frag++;	 // increase Frag #
+
+	}while(SrcRemainingBytes > 0);
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+}
+
+
+#define RELEASE_FRAMES_OF_TXBLK(_pAd, _pTxBlk, _pQEntry, _Status) 										\
+		while(_pTxBlk->TxPacketList.Head)														\
+		{																						\
+			_pQEntry = RemoveHeadQueue(&_pTxBlk->TxPacketList);									\
+			RELEASE_NDIS_PACKET(_pAd, QUEUE_ENTRY_TO_PACKET(_pQEntry), _Status);	\
+		}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware encryption before really
+	sent out to air.
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		PNDIS_PACKET	Pointer to outgoing Ndis frame
+		NumberOfFrag	Number of fragment required
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS STAHardTransmit(
+	IN PRTMP_ADAPTER	pAd,
+	IN TX_BLK 			*pTxBlk,
+	IN	UCHAR			QueIdx)
+{
+	NDIS_PACKET		*pPacket;
+	PQUEUE_ENTRY	pQEntry;
+
+	// ---------------------------------------------
+	// STEP 0. DO SANITY CHECK AND SOME EARLY PREPARATION.
+	// ---------------------------------------------
+	//
+	ASSERT(pTxBlk->TxPacketList.Number);
+	if (pTxBlk->TxPacketList.Head == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("pTxBlk->TotalFrameNum == %ld!\n", pTxBlk->TxPacketList.Number));
+		return NDIS_STATUS_FAILURE;
+	}
+
+	pPacket = QUEUE_ENTRY_TO_PACKET(pTxBlk->TxPacketList.Head);
+
+#if 0 //def CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+		if ((pAd->CommonCfg.CarrierDetect.Enable == TRUE) && (isCarrierDetectExist(pAd) == TRUE))
+	{
+		DBGPRINT(RT_DEBUG_INFO,("STAHardTransmit --> radar detect not in normal mode !!!\n"));
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return (NDIS_STATUS_FAILURE);
+	}
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	// ------------------------------------------------------------------
+	// STEP 1. WAKE UP PHY
+	//		outgoing frame always wakeup PHY to prevent frame lost and
+	//		turn off PSM bit to improve performance
+	// ------------------------------------------------------------------
+	// not to change PSM bit, just send this frame out?
+	if ((pAd->StaCfg.Psm == PWR_SAVE) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+	{
+	    DBGPRINT_RAW(RT_DEBUG_TRACE, ("AsicForceWakeup At HardTx\n"));
+		AsicForceWakeup(pAd, TRUE);
+	}
+
+	// It should not change PSM bit, when APSD turn on.
+	if ((!(pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable) && (pAd->CommonCfg.bAPSDForcePowerSave == FALSE))
+		|| (RTMP_GET_PACKET_EAPOL(pTxBlk->pPacket))
+		|| (RTMP_GET_PACKET_WAI(pTxBlk->pPacket)))
+	{
+		if ((pAd->StaCfg.Psm == PWR_SAVE) &&
+            (pAd->StaCfg.WindowsPowerMode == Ndis802_11PowerModeFast_PSP))
+			MlmeSetPsmBit(pAd, PWR_ACTIVE);
+	}
+
+	switch (pTxBlk->TxFrameType)
+	{
+#ifdef DOT11_N_SUPPORT
+		case TX_AMPDU_FRAME:
+				STA_AMPDU_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_AMSDU_FRAME:
+				STA_AMSDU_Frame_Tx(pAd, pTxBlk);
+			break;
+#endif // DOT11_N_SUPPORT //
+		case TX_LEGACY_FRAME:
+				STA_Legacy_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_MCAST_FRAME:
+				STA_Legacy_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_RALINK_FRAME:
+				STA_ARalink_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_FRAG_FRAME:
+				STA_Fragment_Frame_Tx(pAd, pTxBlk);
+			break;
+		default:
+			{
+				// It should not happened!
+				DBGPRINT(RT_DEBUG_ERROR, ("Send a pacekt was not classified!! It should not happen!\n"));
+				while(pTxBlk->TxPacketList.Number)
+				{
+					pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+					pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+					if (pPacket)
+						RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+				}
+			}
+			break;
+	}
+
+	return (NDIS_STATUS_SUCCESS);
+
+}
+
+ULONG  HashBytesPolynomial(UCHAR *value, unsigned int len)
+{
+   unsigned char *word = value;
+   unsigned int ret = 0;
+   unsigned int i;
+
+   for(i=0; i < len; i++)
+   {
+	  int mod = i % 32;
+	  ret ^=(unsigned int) (word[i]) << mod;
+	  ret ^=(unsigned int) (word[i]) >> (32 - mod);
+   }
+   return ret;
+}
+
+VOID Sta_Announce_or_Forward_802_3_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID)
+{
+	if (TRUE
+		)
+	{
+		announce_802_3_packet(pAd, pPacket);
+	}
+	else
+	{
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+	}
+}
+
diff --git a/drivers/staging/rt2860/sta/sanity.c b/drivers/staging/rt2860/sta/sanity.c
new file mode 100644
index 000000000000..239872464bed
--- /dev/null
+++ b/drivers/staging/rt2860/sta/sanity.c
@@ -0,0 +1,420 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sanity.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang  2004-09-01      add WMM support
+*/
+#include "../rt_config.h"
+
+extern UCHAR	CISCO_OUI[];
+
+extern UCHAR	WPA_OUI[];
+extern UCHAR	RSN_OUI[];
+extern UCHAR	WME_INFO_ELEM[];
+extern UCHAR	WME_PARM_ELEM[];
+extern UCHAR	Ccx2QosInfo[];
+extern UCHAR	RALINK_OUI[];
+extern UCHAR	BROADCOM_OUI[];
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN MlmeStartReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT CHAR Ssid[],
+    OUT UCHAR *pSsidLen)
+{
+    MLME_START_REQ_STRUCT *Info;
+
+    Info = (MLME_START_REQ_STRUCT *)(Msg);
+
+    if (Info->SsidLen > MAX_LEN_OF_SSID)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeStartReqSanity fail - wrong SSID length\n"));
+        return FALSE;
+    }
+
+    *pSsidLen = Info->SsidLen;
+    NdisMoveMemory(Ssid, Info->Ssid, *pSsidLen);
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+    IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerAssocRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pStatus,
+    OUT USHORT *pAid,
+    OUT UCHAR SupRate[],
+    OUT UCHAR *pSupRateLen,
+    OUT UCHAR ExtRate[],
+    OUT UCHAR *pExtRateLen,
+    OUT HT_CAPABILITY_IE		*pHtCapability,
+    OUT ADD_HT_INFO_IE		*pAddHtInfo,	// AP might use this additional ht info IE
+    OUT UCHAR			*pHtCapabilityLen,
+    OUT UCHAR			*pAddHtInfoLen,
+    OUT UCHAR			*pNewExtChannelOffset,
+    OUT PEDCA_PARM pEdcaParm,
+    OUT UCHAR *pCkipFlag)
+{
+    CHAR          IeType, *Ptr;
+    PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+    PEID_STRUCT   pEid;
+    ULONG         Length = 0;
+
+	*pNewExtChannelOffset = 0xff;
+	*pHtCapabilityLen = 0;
+	*pAddHtInfoLen = 0;
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    Ptr = pFrame->Octet;
+    Length += LENGTH_802_11;
+
+    NdisMoveMemory(pCapabilityInfo, &pFrame->Octet[0], 2);
+    Length += 2;
+    NdisMoveMemory(pStatus,         &pFrame->Octet[2], 2);
+    Length += 2;
+    *pCkipFlag = 0;
+    *pExtRateLen = 0;
+    pEdcaParm->bValid = FALSE;
+
+    if (*pStatus != MLME_SUCCESS)
+        return TRUE;
+
+    NdisMoveMemory(pAid, &pFrame->Octet[4], 2);
+    Length += 2;
+
+    // Aid already swaped byte order in RTMPFrameEndianChange() for big endian platform
+    *pAid = (*pAid) & 0x3fff; // AID is low 14-bit
+
+    // -- get supported rates from payload and advance the pointer
+    IeType = pFrame->Octet[6];
+    *pSupRateLen = pFrame->Octet[7];
+    if ((IeType != IE_SUPP_RATES) || (*pSupRateLen > MAX_LEN_OF_SUPPORTED_RATES))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspSanity fail - wrong SupportedRates IE\n"));
+        return FALSE;
+    }
+    else
+        NdisMoveMemory(SupRate, &pFrame->Octet[8], *pSupRateLen);
+
+    Length = Length + 2 + *pSupRateLen;
+
+    // many AP implement proprietary IEs in non-standard order, we'd better
+    // tolerate mis-ordered IEs to get best compatibility
+    pEid = (PEID_STRUCT) &pFrame->Octet[8 + (*pSupRateLen)];
+
+    // get variable fields from payload and advance the pointer
+    while ((Length + 2 + pEid->Len) <= MsgLen)
+    {
+        switch (pEid->Eid)
+        {
+            case IE_EXT_SUPP_RATES:
+                if (pEid->Len <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(ExtRate, pEid->Octet, pEid->Len);
+                    *pExtRateLen = pEid->Len;
+                }
+                break;
+
+             case IE_HT_CAP:
+            case IE_HT_CAP2:
+			if (pEid->Len >= SIZE_HT_CAP_IE)  //Note: allow extension.!!
+			{
+				NdisMoveMemory(pHtCapability, pEid->Octet, SIZE_HT_CAP_IE);
+
+				*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+				*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+
+				*pHtCapabilityLen = SIZE_HT_CAP_IE;
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerAssocRspSanity - wrong IE_HT_CAP. \n"));
+			}
+
+		break;
+#ifdef DOT11_N_SUPPORT
+            case IE_ADD_HT:
+            case IE_ADD_HT2:
+			if (pEid->Len >= sizeof(ADD_HT_INFO_IE))
+			{
+				// This IE allows extension, but we can ignore extra bytes beyond our knowledge , so only
+				// copy first sizeof(ADD_HT_INFO_IE)
+				NdisMoveMemory(pAddHtInfo, pEid->Octet, sizeof(ADD_HT_INFO_IE));
+
+				*(USHORT *)(&pAddHtInfo->AddHtInfo2) = cpu2le16(*(USHORT *)(&pAddHtInfo->AddHtInfo2));
+				*(USHORT *)(&pAddHtInfo->AddHtInfo3) = cpu2le16(*(USHORT *)(&pAddHtInfo->AddHtInfo3));
+
+				*pAddHtInfoLen = SIZE_ADD_HT_INFO_IE;
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerAssocRspSanity - wrong IE_ADD_HT. \n"));
+			}
+
+		break;
+            case IE_SECONDARY_CH_OFFSET:
+			if (pEid->Len == 1)
+			{
+				*pNewExtChannelOffset = pEid->Octet[0];
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerAssocRspSanity - wrong IE_SECONDARY_CH_OFFSET. \n"));
+			}
+#endif // DOT11_N_SUPPORT //
+		break;
+            case IE_AIRONET_CKIP:
+                // 0. Check Aironet IE length, it must be larger or equal to 28
+                //    Cisco's AP VxWork version(will not be supported) used this IE length as 28
+                //    Cisco's AP IOS version used this IE length as 30
+                if (pEid->Len < (CKIP_NEGOTIATION_LENGTH - 2))
+                break;
+
+                // 1. Copy CKIP flag byte to buffer for process
+                *pCkipFlag = *(pEid->Octet + 8);
+                break;
+
+            case IE_AIRONET_IPADDRESS:
+                if (pEid->Len != 0x0A)
+                break;
+
+                // Get Cisco Aironet IP information
+                if (NdisEqualMemory(pEid->Octet, CISCO_OUI, 3) == 1)
+                    NdisMoveMemory(pAd->StaCfg.AironetIPAddress, pEid->Octet + 4, 4);
+                break;
+
+            // CCX2, WMM use the same IE value
+            // case IE_CCX_V2:
+            case IE_VENDOR_SPECIFIC:
+                // handle WME PARAMTER ELEMENT
+                if (NdisEqualMemory(pEid->Octet, WME_PARM_ELEM, 6) && (pEid->Len == 24))
+                {
+                    PUCHAR ptr;
+                    int i;
+
+                    // parsing EDCA parameters
+                    pEdcaParm->bValid          = TRUE;
+                    pEdcaParm->bQAck           = FALSE; // pEid->Octet[0] & 0x10;
+                    pEdcaParm->bQueueRequest   = FALSE; // pEid->Octet[0] & 0x20;
+                    pEdcaParm->bTxopRequest    = FALSE; // pEid->Octet[0] & 0x40;
+                    //pEdcaParm->bMoreDataAck    = FALSE; // pEid->Octet[0] & 0x80;
+                    pEdcaParm->EdcaUpdateCount = pEid->Octet[6] & 0x0f;
+                    pEdcaParm->bAPSDCapable    = (pEid->Octet[6] & 0x80) ? 1 : 0;
+                    ptr = &pEid->Octet[8];
+                    for (i=0; i<4; i++)
+                    {
+                        UCHAR aci = (*ptr & 0x60) >> 5; // b5~6 is AC INDEX
+                        pEdcaParm->bACM[aci]  = (((*ptr) & 0x10) == 0x10);   // b5 is ACM
+                        pEdcaParm->Aifsn[aci] = (*ptr) & 0x0f;               // b0~3 is AIFSN
+                        pEdcaParm->Cwmin[aci] = *(ptr+1) & 0x0f;             // b0~4 is Cwmin
+                        pEdcaParm->Cwmax[aci] = *(ptr+1) >> 4;               // b5~8 is Cwmax
+                        pEdcaParm->Txop[aci]  = *(ptr+2) + 256 * (*(ptr+3)); // in unit of 32-us
+                        ptr += 4; // point to next AC
+                    }
+                }
+
+                // handle CCX IE
+                else
+                {
+                    // 0. Check the size and CCX admin control
+                    if (pAd->StaCfg.CCXControl.field.Enable == 0)
+                        break;
+                    if (pEid->Len != 5)
+                        break;
+
+                    // Turn CCX2 if matched
+                    if (NdisEqualMemory(pEid->Octet, Ccx2IeInfo, 5) == 1)
+                        pAd->StaCfg.CCXEnable = TRUE;
+                    break;
+                }
+                break;
+
+            default:
+                DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspSanity - ignore unrecognized EID = %d\n", pEid->Eid));
+                break;
+        }
+
+        Length = Length + 2 + pEid->Len;
+        pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+    }
+
+    // Force CCX2 enable to TRUE for those AP didn't replay CCX v2 IE, we still force it to be on
+    if (pAd->StaCfg.CCXControl.field.Enable == 1)
+        pAd->StaCfg.CCXEnable = TRUE;
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerProbeReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT CHAR Ssid[],
+    OUT UCHAR *pSsidLen)
+{
+    UCHAR         Idx;
+    UCHAR	      RateLen;
+    CHAR          IeType;
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+
+    if ((pFrame->Octet[0] != IE_SSID) || (pFrame->Octet[1] > MAX_LEN_OF_SSID))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerProbeReqSanity fail - wrong SSID IE(Type=%d,Len=%d)\n",pFrame->Octet[0],pFrame->Octet[1]));
+        return FALSE;
+    }
+
+    *pSsidLen = pFrame->Octet[1];
+    NdisMoveMemory(Ssid, &pFrame->Octet[2], *pSsidLen);
+
+    Idx = *pSsidLen + 2;
+
+    // -- get supported rates from payload and advance the pointer
+    IeType = pFrame->Octet[Idx];
+    RateLen = pFrame->Octet[Idx + 1];
+    if (IeType != IE_SUPP_RATES)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerProbeReqSanity fail - wrong SupportRates IE(Type=%d,Len=%d)\n",pFrame->Octet[Idx],pFrame->Octet[Idx+1]));
+        return FALSE;
+    }
+    else
+    {
+        if ((pAd->CommonCfg.PhyMode == PHY_11G) && (RateLen < 8))
+            return (FALSE);
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN GetTimBit(
+    IN CHAR *Ptr,
+    IN USHORT Aid,
+    OUT UCHAR *TimLen,
+    OUT UCHAR *BcastFlag,
+    OUT UCHAR *DtimCount,
+    OUT UCHAR *DtimPeriod,
+    OUT UCHAR *MessageToMe)
+{
+    UCHAR          BitCntl, N1, N2, MyByte, MyBit;
+    CHAR          *IdxPtr;
+
+    IdxPtr = Ptr;
+
+    IdxPtr ++;
+    *TimLen = *IdxPtr;
+
+    // get DTIM Count from TIM element
+    IdxPtr ++;
+    *DtimCount = *IdxPtr;
+
+    // get DTIM Period from TIM element
+    IdxPtr++;
+    *DtimPeriod = *IdxPtr;
+
+    // get Bitmap Control from TIM element
+    IdxPtr++;
+    BitCntl = *IdxPtr;
+
+    if ((*DtimCount == 0) && (BitCntl & 0x01))
+        *BcastFlag = TRUE;
+    else
+        *BcastFlag = FALSE;
+
+    // Parse Partial Virtual Bitmap from TIM element
+    N1 = BitCntl & 0xfe;    // N1 is the first bitmap byte#
+    N2 = *TimLen - 4 + N1;  // N2 is the last bitmap byte#
+
+    if ((Aid < (N1 << 3)) || (Aid >= ((N2 + 1) << 3)))
+        *MessageToMe = FALSE;
+    else
+    {
+        MyByte = (Aid >> 3) - N1;                       // my byte position in the bitmap byte-stream
+        MyBit = Aid % 16 - ((MyByte & 0x01)? 8:0);
+
+        IdxPtr += (MyByte + 1);
+
+        //if (*IdxPtr)
+        //    DBGPRINT(RT_DEBUG_WARN, ("TIM bitmap = 0x%02x\n", *IdxPtr));
+
+        if (*IdxPtr & (0x01 << MyBit))
+            *MessageToMe = TRUE;
+        else
+            *MessageToMe = FALSE;
+    }
+
+    return TRUE;
+}
+
diff --git a/drivers/staging/rt2860/sta/sync.c b/drivers/staging/rt2860/sta/sync.c
new file mode 100644
index 000000000000..d196f85395ce
--- /dev/null
+++ b/drivers/staging/rt2860/sta/sync.c
@@ -0,0 +1,1959 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sync.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2004-09-01      modified for rt2561/2661
+	Jan Lee		2006-08-01      modified for rt2860 for 802.11n
+*/
+#include "../rt_config.h"
+
+#define AC0_DEF_TXOP		0
+#define AC1_DEF_TXOP		0
+#define AC2_DEF_TXOP		94
+#define AC3_DEF_TXOP		47
+
+VOID	AdhocTurnOnQos(
+	IN  PRTMP_ADAPTER pAd)
+{
+	// Turn on QOs if use HT rate.
+	if (pAd->CommonCfg.APEdcaParm.bValid == FALSE)
+	{
+		pAd->CommonCfg.APEdcaParm.bValid = TRUE;
+		pAd->CommonCfg.APEdcaParm.Aifsn[0] = 3;
+		pAd->CommonCfg.APEdcaParm.Aifsn[1] = 7;
+		pAd->CommonCfg.APEdcaParm.Aifsn[2] = 1;
+		pAd->CommonCfg.APEdcaParm.Aifsn[3] = 1;
+
+		pAd->CommonCfg.APEdcaParm.Cwmin[0] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[1] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[2] = 3;
+		pAd->CommonCfg.APEdcaParm.Cwmin[3] = 2;
+
+		pAd->CommonCfg.APEdcaParm.Cwmax[0] = 10;
+		pAd->CommonCfg.APEdcaParm.Cwmax[1] = 6;
+		pAd->CommonCfg.APEdcaParm.Cwmax[2] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmax[3] = 3;
+
+		pAd->CommonCfg.APEdcaParm.Txop[0]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[1]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[2]  = AC2_DEF_TXOP;
+		pAd->CommonCfg.APEdcaParm.Txop[3]  = AC3_DEF_TXOP;
+	}
+	AsicSetEdcaParm(pAd, &pAd->CommonCfg.APEdcaParm);
+}
+
+/*
+	==========================================================================
+	Description:
+		The sync state machine,
+	Parameters:
+		Sm - pointer to the state machine
+	Note:
+		the state machine looks like the following
+
+	==========================================================================
+ */
+VOID SyncStateMachineInit(
+	IN PRTMP_ADAPTER pAd,
+	IN STATE_MACHINE *Sm,
+	OUT STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(Sm, Trans, MAX_SYNC_STATE, MAX_SYNC_MSG, (STATE_MACHINE_FUNC)Drop, SYNC_IDLE, SYNC_MACHINE_BASE);
+
+	// column 1
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_MLME_SCAN_REQ, (STATE_MACHINE_FUNC)MlmeScanReqAction);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_MLME_JOIN_REQ, (STATE_MACHINE_FUNC)MlmeJoinReqAction);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_MLME_START_REQ, (STATE_MACHINE_FUNC)MlmeStartReqAction);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_PEER_BEACON, (STATE_MACHINE_FUNC)PeerBeacon);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_PEER_PROBE_REQ, (STATE_MACHINE_FUNC)PeerProbeReqAction);
+
+	//column 2
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_MLME_SCAN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenScan);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_MLME_JOIN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenJoin);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_MLME_START_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenStart);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_PEER_BEACON, (STATE_MACHINE_FUNC)PeerBeaconAtJoinAction);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_BEACON_TIMEOUT, (STATE_MACHINE_FUNC)BeaconTimeoutAtJoinAction);
+
+	// column 3
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_MLME_SCAN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenScan);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_MLME_JOIN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenJoin);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_MLME_START_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenStart);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_PEER_BEACON, (STATE_MACHINE_FUNC)PeerBeaconAtScanAction);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_PEER_PROBE_RSP, (STATE_MACHINE_FUNC)PeerBeaconAtScanAction);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_SCAN_TIMEOUT, (STATE_MACHINE_FUNC)ScanTimeoutAction);
+
+	// timer init
+	RTMPInitTimer(pAd, &pAd->MlmeAux.BeaconTimer, GET_TIMER_FUNCTION(BeaconTimeout), pAd, FALSE);
+	RTMPInitTimer(pAd, &pAd->MlmeAux.ScanTimer, GET_TIMER_FUNCTION(ScanTimeout), pAd, FALSE);
+}
+
+/*
+	==========================================================================
+	Description:
+		Beacon timeout handler, executed in timer thread
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID BeaconTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	DBGPRINT(RT_DEBUG_TRACE,("SYNC - BeaconTimeout\n"));
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))
+		return;
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->CommonCfg.BBPCurrentBW == BW_40)
+		)
+	{
+		UCHAR        BBPValue = 0;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+		BBPValue &= (~0x18);
+		BBPValue |= 0x10;
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - End of SCAN, restore to 40MHz channel %d, Total BSS[%02d]\n",pAd->CommonCfg.CentralChannel, pAd->ScanTab.BssNr));
+	}
+#endif // DOT11_N_SUPPORT //
+
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_BEACON_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		Scan timeout handler, executed in timer thread
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID ScanTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))
+		return;
+
+	if (MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_SCAN_TIMEOUT, 0, NULL))
+	{
+		RT28XX_MLME_HANDLER(pAd);
+	}
+	else
+	{
+		// To prevent SyncMachine.CurrState is SCAN_LISTEN forever.
+		pAd->MlmeAux.Channel = 0;
+		ScanNextChannel(pAd);
+		if (pAd->CommonCfg.bWirelessEvent)
+		{
+			RTMPSendWirelessEvent(pAd, IW_SCAN_ENQUEUE_FAIL_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		MLME SCAN req state machine procedure
+	==========================================================================
+ */
+VOID MlmeScanReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR          Ssid[MAX_LEN_OF_SSID], SsidLen, ScanType, BssType, BBPValue = 0;
+	BOOLEAN        TimerCancelled;
+	ULONG		   Now;
+	USHORT         Status;
+	PHEADER_802_11 pHdr80211;
+	PUCHAR         pOutBuffer = NULL;
+	NDIS_STATUS    NStatus;
+
+	// Check the total scan tries for one single OID command
+	// If this is the CCX 2.0 Case, skip that!
+	if ( !RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - MlmeScanReqAction before Startup\n"));
+		return;
+	}
+
+	// Increase the scan retry counters.
+	pAd->StaCfg.ScanCnt++;
+
+#ifdef RT2860
+    if ((OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE)) &&
+        (IDLE_ON(pAd)) &&
+		(pAd->StaCfg.bRadio == TRUE) &&
+		(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_IDLE_RADIO_OFF)))
+	{
+		RT28xxPciAsicRadioOn(pAd, GUI_IDLE_POWER_SAVE);
+	}
+#endif // RT2860 //
+
+	// first check the parameter sanity
+	if (MlmeScanReqSanity(pAd,
+						  Elem->Msg,
+						  Elem->MsgLen,
+						  &BssType,
+						  Ssid,
+						  &SsidLen,
+						  &ScanType))
+	{
+
+		// Check for channel load and noise hist request
+		// Suspend MSDU only at scan request, not the last two mentioned
+		if ((ScanType == SCAN_CISCO_NOISE) || (ScanType == SCAN_CISCO_CHANNEL_LOAD))
+		{
+			if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+				RTMPSuspendMsduTransmission(pAd);			// Suspend MSDU transmission here
+		}
+		else
+		{
+			// Suspend MSDU transmission here
+			RTMPSuspendMsduTransmission(pAd);
+		}
+
+		//
+		// To prevent data lost.
+		// Send an NULL data with turned PSM bit on to current associated AP before SCAN progress.
+		// And should send an NULL data with turned PSM bit off to AP, when scan progress done
+		//
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) && (INFRA_ON(pAd)))
+		{
+			NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);
+			if (NStatus	== NDIS_STATUS_SUCCESS)
+			{
+				pHdr80211 = (PHEADER_802_11) pOutBuffer;
+				MgtMacHeaderInit(pAd, pHdr80211, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+				pHdr80211->Duration = 0;
+				pHdr80211->FC.Type = BTYPE_DATA;
+				pHdr80211->FC.PwrMgmt = PWR_SAVE;
+
+				// Send using priority queue
+				MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+				DBGPRINT(RT_DEBUG_TRACE, ("MlmeScanReqAction -- Send PSM Data frame for off channel RM\n"));
+				MlmeFreeMemory(pAd, pOutBuffer);
+				RTMPusecDelay(5000);
+			}
+		}
+
+		NdisGetSystemUpTime(&Now);
+		pAd->StaCfg.LastScanTime = Now;
+		// reset all the timers
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer, &TimerCancelled);
+
+		// record desired BSS parameters
+		pAd->MlmeAux.BssType = BssType;
+		pAd->MlmeAux.ScanType = ScanType;
+		pAd->MlmeAux.SsidLen = SsidLen;
+        NdisZeroMemory(pAd->MlmeAux.Ssid, MAX_LEN_OF_SSID);
+		NdisMoveMemory(pAd->MlmeAux.Ssid, Ssid, SsidLen);
+
+		// start from the first channel
+		pAd->MlmeAux.Channel = FirstChannel(pAd);
+
+		// Change the scan channel when dealing with CCX beacon report
+		if ((ScanType == SCAN_CISCO_PASSIVE) || (ScanType == SCAN_CISCO_ACTIVE) ||
+			(ScanType == SCAN_CISCO_CHANNEL_LOAD) || (ScanType == SCAN_CISCO_NOISE))
+			pAd->MlmeAux.Channel = pAd->StaCfg.CCXScanChannel;
+
+		// Let BBP register at 20MHz to do scan
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+		BBPValue &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - BBP R4 to 20MHz.l\n"));
+		ScanNextChannel(pAd);
+	}
+	else
+	{
+		DBGPRINT_ERR(("SYNC - MlmeScanReqAction() sanity check fail\n"));
+		pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		MLME JOIN req state machine procedure
+	==========================================================================
+ */
+VOID MlmeJoinReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR        BBPValue = 0;
+	BSS_ENTRY    *pBss;
+	BOOLEAN       TimerCancelled;
+	HEADER_802_11 Hdr80211;
+	NDIS_STATUS   NStatus;
+	ULONG         FrameLen = 0;
+	PUCHAR        pOutBuffer = NULL;
+	PUCHAR        pSupRate = NULL;
+	UCHAR         SupRateLen;
+	PUCHAR        pExtRate = NULL;
+	UCHAR         ExtRateLen;
+	UCHAR         ASupRate[] = {0x8C, 0x12, 0x98, 0x24, 0xb0, 0x48, 0x60, 0x6C};
+	UCHAR         ASupRateLen = sizeof(ASupRate)/sizeof(UCHAR);
+	MLME_JOIN_REQ_STRUCT *pInfo = (MLME_JOIN_REQ_STRUCT *)(Elem->Msg);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - MlmeJoinReqAction(BSS #%ld)\n", pInfo->BssIdx));
+
+#ifdef RT2860
+    if ((OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE)) &&
+        (IDLE_ON(pAd)) &&
+		(pAd->StaCfg.bRadio == TRUE) &&
+		(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_IDLE_RADIO_OFF)))
+	{
+		RT28xxPciAsicRadioOn(pAd, GUI_IDLE_POWER_SAVE);
+	}
+#endif // RT2860 //
+
+	// reset all the timers
+	RTMPCancelTimer(&pAd->MlmeAux.ScanTimer, &TimerCancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+
+	pBss = &pAd->MlmeAux.SsidBssTab.BssEntry[pInfo->BssIdx];
+
+	// record the desired SSID & BSSID we're waiting for
+	COPY_MAC_ADDR(pAd->MlmeAux.Bssid, pBss->Bssid);
+
+	// If AP's SSID is not hidden, it is OK for updating ssid to MlmeAux again.
+	if (pBss->Hidden == 0)
+	{
+		NdisMoveMemory(pAd->MlmeAux.Ssid, pBss->Ssid, pBss->SsidLen);
+		pAd->MlmeAux.SsidLen = pBss->SsidLen;
+	}
+
+	pAd->MlmeAux.BssType = pBss->BssType;
+	pAd->MlmeAux.Channel = pBss->Channel;
+	pAd->MlmeAux.CentralChannel = pBss->CentralChannel;
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	// Country IE of the AP will be evaluated and will be used.
+	if ((pAd->StaCfg.IEEE80211dClientMode != Rt802_11_D_None) &&
+		(pBss->bHasCountryIE == TRUE))
+	{
+		NdisMoveMemory(&pAd->CommonCfg.CountryCode[0], &pBss->CountryString[0], 2);
+		if (pBss->CountryString[2] == 'I')
+			pAd->CommonCfg.Geography = IDOR;
+		else if (pBss->CountryString[2] == 'O')
+			pAd->CommonCfg.Geography = ODOR;
+		else
+			pAd->CommonCfg.Geography = BOTH;
+		BuildChannelListEx(pAd);
+	}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// Let BBP register at 20MHz to do scan
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+	BBPValue &= (~0x18);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - BBP R4 to 20MHz.l\n"));
+
+	// switch channel and waiting for beacon timer
+	AsicSwitchChannel(pAd, pAd->MlmeAux.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->MlmeAux.Channel);
+	RTMPSetTimer(&pAd->MlmeAux.BeaconTimer, JOIN_TIMEOUT);
+
+    do
+	{
+		if (((pAd->CommonCfg.bIEEE80211H == 1) &&
+            (pAd->MlmeAux.Channel > 14) &&
+             RadarChannelCheck(pAd, pAd->MlmeAux.Channel))
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+             || (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+#endif // CARRIER_DETECTION_SUPPORT //
+            )
+		{
+			//
+			// We can't send any Probe request frame to meet 802.11h.
+			//
+			if (pBss->Hidden == 0)
+				break;
+		}
+
+		//
+		// send probe request
+		//
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+		if (NStatus == NDIS_STATUS_SUCCESS)
+		{
+			if (pAd->MlmeAux.Channel <= 14)
+			{
+				pSupRate = pAd->CommonCfg.SupRate;
+				SupRateLen = pAd->CommonCfg.SupRateLen;
+				pExtRate = pAd->CommonCfg.ExtRate;
+				ExtRateLen = pAd->CommonCfg.ExtRateLen;
+			}
+			else
+			{
+				//
+				// Overwrite Support Rate, CCK rate are not allowed
+				//
+				pSupRate = ASupRate;
+				SupRateLen = ASupRateLen;
+				ExtRateLen = 0;
+			}
+
+			if (pAd->MlmeAux.BssType == BSS_INFRA)
+				MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, pAd->MlmeAux.Bssid, pAd->MlmeAux.Bssid);
+			else
+				MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, BROADCAST_ADDR, BROADCAST_ADDR);
+
+			MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+							  sizeof(HEADER_802_11),    &Hdr80211,
+							  1,                        &SsidIe,
+							  1,                        &pAd->MlmeAux.SsidLen,
+							  pAd->MlmeAux.SsidLen,	    pAd->MlmeAux.Ssid,
+							  1,                        &SupRateIe,
+							  1,                        &SupRateLen,
+							  SupRateLen,               pSupRate,
+							  END_OF_ARGS);
+
+			if (ExtRateLen)
+			{
+				ULONG Tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &Tmp,
+								  1,                                &ExtRateIe,
+								  1,                                &ExtRateLen,
+								  ExtRateLen,                       pExtRate,
+								  END_OF_ARGS);
+				FrameLen += Tmp;
+			}
+
+
+			MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+    } while (FALSE);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - Switch to ch %d, Wait BEACON from %02x:%02x:%02x:%02x:%02x:%02x\n",
+		pBss->Channel, pBss->Bssid[0], pBss->Bssid[1], pBss->Bssid[2], pBss->Bssid[3], pBss->Bssid[4], pBss->Bssid[5]));
+
+	pAd->Mlme.SyncMachine.CurrState = JOIN_WAIT_BEACON;
+}
+
+/*
+	==========================================================================
+	Description:
+		MLME START Request state machine procedure, starting an IBSS
+	==========================================================================
+ */
+VOID MlmeStartReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Ssid[MAX_LEN_OF_SSID], SsidLen;
+	BOOLEAN       TimerCancelled;
+
+	// New for WPA security suites
+	UCHAR						VarIE[MAX_VIE_LEN]; 	// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	LARGE_INTEGER				TimeStamp;
+	BOOLEAN Privacy;
+	USHORT Status;
+
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+	TimeStamp.u.LowPart  = 0;
+	TimeStamp.u.HighPart = 0;
+
+	if (MlmeStartReqSanity(pAd, Elem->Msg, Elem->MsgLen, Ssid, &SsidLen))
+	{
+		// reset all the timers
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer, &TimerCancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+
+		//
+		// Start a new IBSS. All IBSS parameters are decided now....
+		//
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeStartReqAction - Start a new IBSS. All IBSS parameters are decided now.... \n"));
+		pAd->MlmeAux.BssType           = BSS_ADHOC;
+		NdisMoveMemory(pAd->MlmeAux.Ssid, Ssid, SsidLen);
+		pAd->MlmeAux.SsidLen           = SsidLen;
+
+		// generate a radom number as BSSID
+		MacAddrRandomBssid(pAd, pAd->MlmeAux.Bssid);
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeStartReqAction - generate a radom number as BSSID \n"));
+
+		Privacy = (pAd->StaCfg.WepStatus == Ndis802_11Encryption1Enabled) ||
+				  (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+				  (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled);
+		pAd->MlmeAux.CapabilityInfo    = CAP_GENERATE(0,1,Privacy, (pAd->CommonCfg.TxPreamble == Rt802_11PreambleShort), 1, 0);
+		pAd->MlmeAux.BeaconPeriod      = pAd->CommonCfg.BeaconPeriod;
+		pAd->MlmeAux.AtimWin           = pAd->StaCfg.AtimWin;
+		pAd->MlmeAux.Channel           = pAd->CommonCfg.Channel;
+
+		pAd->CommonCfg.CentralChannel  = pAd->CommonCfg.Channel;
+		pAd->MlmeAux.CentralChannel    = pAd->CommonCfg.CentralChannel;
+
+		pAd->MlmeAux.SupRateLen= pAd->CommonCfg.SupRateLen;
+		NdisMoveMemory(pAd->MlmeAux.SupRate, pAd->CommonCfg.SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+		RTMPCheckRates(pAd, pAd->MlmeAux.SupRate, &pAd->MlmeAux.SupRateLen);
+		pAd->MlmeAux.ExtRateLen = pAd->CommonCfg.ExtRateLen;
+		NdisMoveMemory(pAd->MlmeAux.ExtRate, pAd->CommonCfg.ExtRate, MAX_LEN_OF_SUPPORTED_RATES);
+		RTMPCheckRates(pAd, pAd->MlmeAux.ExtRate, &pAd->MlmeAux.ExtRateLen);
+#ifdef DOT11_N_SUPPORT
+		if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+		{
+			RTMPUpdateHTIE(&pAd->CommonCfg.DesiredHtPhy, &pAd->StaCfg.DesiredHtPhyInfo.MCSSet[0], &pAd->MlmeAux.HtCapability, &pAd->MlmeAux.AddHtInfo);
+			pAd->MlmeAux.HtCapabilityLen = sizeof(HT_CAPABILITY_IE);
+			// Not turn pAd->StaActive.SupportedHtPhy.bHtEnable = TRUE here.
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC -pAd->StaActive.SupportedHtPhy.bHtEnable = TRUE\n"));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			pAd->MlmeAux.HtCapabilityLen = 0;
+			pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+		}
+		// temporarily not support QOS in IBSS
+		NdisZeroMemory(&pAd->MlmeAux.APEdcaParm, sizeof(EDCA_PARM));
+		NdisZeroMemory(&pAd->MlmeAux.APQbssLoad, sizeof(QBSS_LOAD_PARM));
+		NdisZeroMemory(&pAd->MlmeAux.APQosCapability, sizeof(QOS_CAPABILITY_PARM));
+
+		AsicSwitchChannel(pAd, pAd->MlmeAux.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->MlmeAux.Channel);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - MlmeStartReqAction(ch= %d,sup rates= %d, ext rates=%d)\n",
+			pAd->MlmeAux.Channel, pAd->MlmeAux.SupRateLen, pAd->MlmeAux.ExtRateLen));
+
+		pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+		Status = MLME_SUCCESS;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_START_CONF, 2, &Status);
+	}
+	else
+	{
+		DBGPRINT_ERR(("SYNC - MlmeStartReqAction() sanity check fail.\n"));
+		pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_START_CONF, 2, &Status);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		peer sends beacon back when scanning
+	==========================================================================
+ */
+VOID PeerBeaconAtScanAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR           Bssid[MAC_ADDR_LEN], Addr2[MAC_ADDR_LEN];
+	UCHAR           Ssid[MAX_LEN_OF_SSID], BssType, Channel, NewChannel,
+					SsidLen, DtimCount, DtimPeriod, BcastFlag, MessageToMe;
+	CF_PARM         CfParm;
+	USHORT          BeaconPeriod, AtimWin, CapabilityInfo;
+	PFRAME_802_11   pFrame;
+	LARGE_INTEGER   TimeStamp;
+	UCHAR           Erp;
+	UCHAR         	SupRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		  	SupRateLen, ExtRateLen;
+	USHORT 			LenVIE;
+	UCHAR			CkipFlag;
+	UCHAR			AironetCellPowerLimit;
+	EDCA_PARM       EdcaParm;
+	QBSS_LOAD_PARM  QbssLoad;
+	QOS_CAPABILITY_PARM QosCapability;
+	ULONG						RalinkIe;
+	UCHAR						VarIE[MAX_VIE_LEN];		// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen = 0, PreNHtCapabilityLen = 0;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+	pFrame = (PFRAME_802_11) Elem->Msg;
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+#ifdef DOT11_N_SUPPORT
+    RTMPZeroMemory(&HtCapability, sizeof(HtCapability));
+	RTMPZeroMemory(&AddHtInfo, sizeof(ADD_HT_INFO_IE));
+#endif // DOT11_N_SUPPORT //
+
+	if (PeerBeaconAndProbeRspSanity(pAd,
+								Elem->Msg,
+								Elem->MsgLen,
+								Elem->Channel,
+								Addr2,
+								Bssid,
+								Ssid,
+								&SsidLen,
+								&BssType,
+								&BeaconPeriod,
+								&Channel,
+								&NewChannel,
+								&TimeStamp,
+								&CfParm,
+								&AtimWin,
+								&CapabilityInfo,
+								&Erp,
+								&DtimCount,
+								&DtimPeriod,
+								&BcastFlag,
+								&MessageToMe,
+								SupRate,
+								&SupRateLen,
+								ExtRate,
+								&ExtRateLen,
+								&CkipFlag,
+								&AironetCellPowerLimit,
+								&EdcaParm,
+								&QbssLoad,
+								&QosCapability,
+								&RalinkIe,
+								&HtCapabilityLen,
+								&PreNHtCapabilityLen,
+								&HtCapability,
+								&AddHtInfoLen,
+								&AddHtInfo,
+								&NewExtChannelOffset,
+								&LenVIE,
+								pVIE))
+	{
+		ULONG Idx;
+		CHAR Rssi = 0;
+
+		Idx = BssTableSearch(&pAd->ScanTab, Bssid, Channel);
+		if (Idx != BSS_NOT_FOUND)
+			Rssi = pAd->ScanTab.BssEntry[Idx].Rssi;
+
+		Rssi = RTMPMaxRssi(pAd, ConvertToRssi(pAd, Elem->Rssi0, RSSI_0), ConvertToRssi(pAd, Elem->Rssi1, RSSI_1), ConvertToRssi(pAd, Elem->Rssi2, RSSI_2));
+
+
+#ifdef DOT11_N_SUPPORT
+		if ((HtCapabilityLen > 0) || (PreNHtCapabilityLen > 0))
+			HtCapabilityLen = SIZE_HT_CAP_IE;
+#endif // DOT11_N_SUPPORT //
+		if ((pAd->StaCfg.CCXReqType != MSRN_TYPE_UNUSED) && (Channel == pAd->StaCfg.CCXScanChannel))
+		{
+			Idx = BssTableSetEntry(pAd, &pAd->StaCfg.CCXBssTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+						 &CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen,ExtRate, ExtRateLen, &HtCapability,
+						 &AddHtInfo, HtCapabilityLen, AddHtInfoLen, NewExtChannelOffset, Channel, Rssi, TimeStamp, CkipFlag,
+						 &EdcaParm, &QosCapability, &QbssLoad, LenVIE, pVIE);
+			if (Idx != BSS_NOT_FOUND)
+			{
+				NdisMoveMemory(pAd->StaCfg.CCXBssTab.BssEntry[Idx].PTSF, &Elem->Msg[24], 4);
+				NdisMoveMemory(&pAd->StaCfg.CCXBssTab.BssEntry[Idx].TTSF[0], &Elem->TimeStamp.u.LowPart, 4);
+				NdisMoveMemory(&pAd->StaCfg.CCXBssTab.BssEntry[Idx].TTSF[4], &Elem->TimeStamp.u.LowPart, 4);
+				if (pAd->StaCfg.CCXReqType == MSRN_TYPE_BEACON_REQ)
+					AironetAddBeaconReport(pAd, Idx, Elem);
+			}
+		}
+		else
+		{
+			Idx = BssTableSetEntry(pAd, &pAd->ScanTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+						  &CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,  &HtCapability,
+						 &AddHtInfo, HtCapabilityLen, AddHtInfoLen, NewExtChannelOffset, Channel, Rssi, TimeStamp, CkipFlag,
+						 &EdcaParm, &QosCapability, &QbssLoad, LenVIE, pVIE);
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+			if (pAd->ChannelList[pAd->CommonCfg.ChannelListIdx].bEffectedChannel == TRUE)
+			{
+				UCHAR		RegClass;
+				PeerBeaconAndProbeRspSanity2(pAd, Elem->Msg, Elem->MsgLen, &RegClass);
+				TriEventTableSetEntry(pAd, &pAd->CommonCfg.TriggerEventTab, Bssid, &HtCapability, HtCapabilityLen, RegClass, Channel);
+			}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+			if (Idx != BSS_NOT_FOUND)
+			{
+				NdisMoveMemory(pAd->ScanTab.BssEntry[Idx].PTSF, &Elem->Msg[24], 4);
+				NdisMoveMemory(&pAd->ScanTab.BssEntry[Idx].TTSF[0], &Elem->TimeStamp.u.LowPart, 4);
+				NdisMoveMemory(&pAd->ScanTab.BssEntry[Idx].TTSF[4], &Elem->TimeStamp.u.LowPart, 4);
+			}
+		}
+	}
+	// sanity check fail, ignored
+}
+
+/*
+	==========================================================================
+	Description:
+		When waiting joining the (I)BSS, beacon received from external
+	==========================================================================
+ */
+VOID PeerBeaconAtJoinAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Bssid[MAC_ADDR_LEN], Addr2[MAC_ADDR_LEN];
+	UCHAR         Ssid[MAX_LEN_OF_SSID], SsidLen, BssType, Channel, MessageToMe,
+				  DtimCount, DtimPeriod, BcastFlag, NewChannel;
+	LARGE_INTEGER TimeStamp;
+	USHORT        BeaconPeriod, AtimWin, CapabilityInfo;
+	CF_PARM       Cf;
+	BOOLEAN       TimerCancelled;
+	UCHAR         Erp;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		  SupRateLen, ExtRateLen;
+	UCHAR         CkipFlag;
+	USHORT 		  LenVIE;
+	UCHAR		  AironetCellPowerLimit;
+	EDCA_PARM       EdcaParm;
+	QBSS_LOAD_PARM  QbssLoad;
+	QOS_CAPABILITY_PARM QosCapability;
+	USHORT        Status;
+	UCHAR						VarIE[MAX_VIE_LEN];		// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	ULONG           RalinkIe;
+	ULONG         Idx;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR				HtCapabilityLen = 0, PreNHtCapabilityLen = 0;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+#ifdef DOT11_N_SUPPORT
+	UCHAR			CentralChannel;
+#endif // DOT11_N_SUPPORT //
+
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+    RTMPZeroMemory(&HtCapability, sizeof(HtCapability));
+	RTMPZeroMemory(&AddHtInfo, sizeof(ADD_HT_INFO_IE));
+
+
+	if (PeerBeaconAndProbeRspSanity(pAd,
+								Elem->Msg,
+								Elem->MsgLen,
+								Elem->Channel,
+								Addr2,
+								Bssid,
+								Ssid,
+								&SsidLen,
+								&BssType,
+								&BeaconPeriod,
+								&Channel,
+								&NewChannel,
+								&TimeStamp,
+								&Cf,
+								&AtimWin,
+								&CapabilityInfo,
+								&Erp,
+								&DtimCount,
+								&DtimPeriod,
+								&BcastFlag,
+								&MessageToMe,
+								SupRate,
+								&SupRateLen,
+								ExtRate,
+								&ExtRateLen,
+								&CkipFlag,
+								&AironetCellPowerLimit,
+								&EdcaParm,
+								&QbssLoad,
+								&QosCapability,
+								&RalinkIe,
+								&HtCapabilityLen,
+								&PreNHtCapabilityLen,
+								&HtCapability,
+								&AddHtInfoLen,
+								&AddHtInfo,
+								&NewExtChannelOffset,
+								&LenVIE,
+								pVIE))
+	{
+		// Disqualify 11b only adhoc when we are in 11g only adhoc mode
+		if ((BssType == BSS_ADHOC) && (pAd->CommonCfg.PhyMode == PHY_11G) && ((SupRateLen+ExtRateLen)< 12))
+			return;
+
+		// BEACON from desired BSS/IBSS found. We should be able to decide most
+		// BSS parameters here.
+		// Q. But what happen if this JOIN doesn't conclude a successful ASSOCIATEION?
+		//    Do we need to receover back all parameters belonging to previous BSS?
+		// A. Should be not. There's no back-door recover to previous AP. It still need
+		//    a new JOIN-AUTH-ASSOC sequence.
+		if (MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, Bssid))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - receive desired BEACON at JoinWaitBeacon... Channel = %d\n", Channel));
+			RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+
+			// Update RSSI to prevent No signal display when cards first initialized
+			pAd->StaCfg.RssiSample.LastRssi0	= ConvertToRssi(pAd, Elem->Rssi0, RSSI_0);
+			pAd->StaCfg.RssiSample.LastRssi1	= ConvertToRssi(pAd, Elem->Rssi1, RSSI_1);
+			pAd->StaCfg.RssiSample.LastRssi2	= ConvertToRssi(pAd, Elem->Rssi2, RSSI_2);
+			pAd->StaCfg.RssiSample.AvgRssi0	= pAd->StaCfg.RssiSample.LastRssi0;
+			pAd->StaCfg.RssiSample.AvgRssi0X8	= pAd->StaCfg.RssiSample.AvgRssi0 << 3;
+			pAd->StaCfg.RssiSample.AvgRssi1	= pAd->StaCfg.RssiSample.LastRssi1;
+			pAd->StaCfg.RssiSample.AvgRssi1X8	= pAd->StaCfg.RssiSample.AvgRssi1 << 3;
+			pAd->StaCfg.RssiSample.AvgRssi2	= pAd->StaCfg.RssiSample.LastRssi2;
+			pAd->StaCfg.RssiSample.AvgRssi2X8	= pAd->StaCfg.RssiSample.AvgRssi2 << 3;
+
+			//
+			// We need to check if SSID only set to any, then we can record the current SSID.
+			// Otherwise will cause hidden SSID association failed.
+			//
+			if (pAd->MlmeAux.SsidLen == 0)
+			{
+				NdisMoveMemory(pAd->MlmeAux.Ssid, Ssid, SsidLen);
+				pAd->MlmeAux.SsidLen = SsidLen;
+			}
+			else
+			{
+				Idx = BssSsidTableSearch(&pAd->ScanTab, Bssid, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen, Channel);
+
+				if (Idx != BSS_NOT_FOUND)
+				{
+					//
+					// Multiple SSID case, used correct CapabilityInfo
+					//
+					CapabilityInfo = pAd->ScanTab.BssEntry[Idx].CapabilityInfo;
+				}
+			}
+			NdisMoveMemory(pAd->MlmeAux.Bssid, Bssid, MAC_ADDR_LEN);
+			pAd->MlmeAux.CapabilityInfo = CapabilityInfo & SUPPORTED_CAPABILITY_INFO;
+			pAd->MlmeAux.BssType = BssType;
+			pAd->MlmeAux.BeaconPeriod = BeaconPeriod;
+			pAd->MlmeAux.Channel = Channel;
+			pAd->MlmeAux.AtimWin = AtimWin;
+			pAd->MlmeAux.CfpPeriod = Cf.CfpPeriod;
+			pAd->MlmeAux.CfpMaxDuration = Cf.CfpMaxDuration;
+			pAd->MlmeAux.APRalinkIe = RalinkIe;
+
+			// Copy AP's supported rate to MlmeAux for creating assoication request
+			// Also filter out not supported rate
+			pAd->MlmeAux.SupRateLen = SupRateLen;
+			NdisMoveMemory(pAd->MlmeAux.SupRate, SupRate, SupRateLen);
+			RTMPCheckRates(pAd, pAd->MlmeAux.SupRate, &pAd->MlmeAux.SupRateLen);
+			pAd->MlmeAux.ExtRateLen = ExtRateLen;
+			NdisMoveMemory(pAd->MlmeAux.ExtRate, ExtRate, ExtRateLen);
+			RTMPCheckRates(pAd, pAd->MlmeAux.ExtRate, &pAd->MlmeAux.ExtRateLen);
+
+            NdisZeroMemory(pAd->StaActive.SupportedPhyInfo.MCSSet, 16);
+#ifdef DOT11_N_SUPPORT
+			pAd->MlmeAux.NewExtChannelOffset = NewExtChannelOffset;
+			pAd->MlmeAux.HtCapabilityLen = HtCapabilityLen;
+
+			// filter out un-supported ht rates
+			if (((HtCapabilityLen > 0) || (PreNHtCapabilityLen > 0)) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+			{
+				RTMPZeroMemory(&pAd->MlmeAux.HtCapability, SIZE_HT_CAP_IE);
+   				RTMPMoveMemory(&pAd->MlmeAux.AddHtInfo, &AddHtInfo, SIZE_ADD_HT_INFO_IE);
+
+				// StaActive.SupportedHtPhy.MCSSet stores Peer AP's 11n Rx capability
+				NdisMoveMemory(pAd->StaActive.SupportedPhyInfo.MCSSet, HtCapability.MCSSet, 16);
+				pAd->MlmeAux.NewExtChannelOffset = NewExtChannelOffset;
+				pAd->MlmeAux.HtCapabilityLen = SIZE_HT_CAP_IE;
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = TRUE;
+				if (PreNHtCapabilityLen > 0)
+					pAd->StaActive.SupportedPhyInfo.bPreNHt = TRUE;
+				RTMPCheckHt(pAd, BSSID_WCID, &HtCapability, &AddHtInfo);
+				// Copy AP Parameter to StaActive.  This is also in LinkUp.
+				DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAtJoinAction! (MpduDensity=%d, MaxRAmpduFactor=%d, BW=%d)\n",
+					pAd->StaActive.SupportedHtPhy.MpduDensity, pAd->StaActive.SupportedHtPhy.MaxRAmpduFactor, HtCapability.HtCapInfo.ChannelWidth));
+
+				if (AddHtInfoLen > 0)
+				{
+					CentralChannel = AddHtInfo.ControlChan;
+		 			// Check again the Bandwidth capability of this AP.
+		 			if ((AddHtInfo.ControlChan > 2)&& (AddHtInfo.AddHtInfo.ExtChanOffset == EXTCHA_BELOW) && (HtCapability.HtCapInfo.ChannelWidth == BW_40))
+		 			{
+		 				CentralChannel = AddHtInfo.ControlChan - 2;
+		 			}
+		 			else if ((AddHtInfo.AddHtInfo.ExtChanOffset == EXTCHA_ABOVE) && (HtCapability.HtCapInfo.ChannelWidth == BW_40))
+		 			{
+		 				CentralChannel = AddHtInfo.ControlChan + 2;
+		 			}
+
+					// Check Error .
+					if (pAd->MlmeAux.CentralChannel != CentralChannel)
+		 				DBGPRINT(RT_DEBUG_ERROR, ("PeerBeaconAtJoinAction HT===>Beacon Central Channel = %d, Control Channel = %d. Mlmeaux CentralChannel = %d\n", CentralChannel, AddHtInfo.ControlChan, pAd->MlmeAux.CentralChannel));
+
+		 			DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAtJoinAction HT===>Central Channel = %d, Control Channel = %d,  .\n", CentralChannel, AddHtInfo.ControlChan));
+
+				}
+
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+			{
+   				// To prevent error, let legacy AP must have same CentralChannel and Channel.
+				if ((HtCapabilityLen == 0) && (PreNHtCapabilityLen == 0))
+					pAd->MlmeAux.CentralChannel = pAd->MlmeAux.Channel;
+
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+				RTMPZeroMemory(&pAd->MlmeAux.HtCapability, SIZE_HT_CAP_IE);
+				RTMPZeroMemory(&pAd->MlmeAux.AddHtInfo, SIZE_ADD_HT_INFO_IE);
+			}
+
+			RTMPUpdateMlmeRate(pAd);
+
+			// copy QOS related information
+			if ((pAd->CommonCfg.bWmmCapable)
+#ifdef DOT11_N_SUPPORT
+				 || (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+#endif // DOT11_N_SUPPORT //
+				)
+			{
+				NdisMoveMemory(&pAd->MlmeAux.APEdcaParm, &EdcaParm, sizeof(EDCA_PARM));
+				NdisMoveMemory(&pAd->MlmeAux.APQbssLoad, &QbssLoad, sizeof(QBSS_LOAD_PARM));
+				NdisMoveMemory(&pAd->MlmeAux.APQosCapability, &QosCapability, sizeof(QOS_CAPABILITY_PARM));
+			}
+			else
+			{
+				NdisZeroMemory(&pAd->MlmeAux.APEdcaParm, sizeof(EDCA_PARM));
+				NdisZeroMemory(&pAd->MlmeAux.APQbssLoad, sizeof(QBSS_LOAD_PARM));
+				NdisZeroMemory(&pAd->MlmeAux.APQosCapability, sizeof(QOS_CAPABILITY_PARM));
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - after JOIN, SupRateLen=%d, ExtRateLen=%d\n",
+										pAd->MlmeAux.SupRateLen, pAd->MlmeAux.ExtRateLen));
+
+#ifdef LEAP_SUPPORT
+			// Update CkipFlag
+			pAd->StaCfg.CkipFlag = CkipFlag;
+
+			// Keep TimeStamp for Re-Association used.
+			if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+				pAd->StaCfg.CCKMBeaconAtJoinTimeStamp = TimeStamp;
+#endif // LEAP_SUPPORT //
+
+			if (AironetCellPowerLimit != 0xFF)
+			{
+				//We need to change our TxPower for CCX 2.0 AP Control of Client Transmit Power
+				ChangeToCellPowerLimit(pAd, AironetCellPowerLimit);
+			}
+			else  //Used the default TX Power Percentage.
+				pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+
+			pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+			Status = MLME_SUCCESS;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_JOIN_CONF, 2, &Status);
+		}
+		// not to me BEACON, ignored
+	}
+	// sanity check fail, ignore this frame
+}
+
+/*
+	==========================================================================
+	Description:
+		receive BEACON from peer
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerBeacon(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Bssid[MAC_ADDR_LEN], Addr2[MAC_ADDR_LEN];
+	CHAR          Ssid[MAX_LEN_OF_SSID];
+	CF_PARM       CfParm;
+	UCHAR         SsidLen, MessageToMe=0, BssType, Channel, NewChannel, index=0;
+	UCHAR         DtimCount=0, DtimPeriod=0, BcastFlag=0;
+	USHORT        CapabilityInfo, AtimWin, BeaconPeriod;
+	LARGE_INTEGER TimeStamp;
+	USHORT        TbttNumToNextWakeUp;
+	UCHAR         Erp;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		  SupRateLen, ExtRateLen;
+	UCHAR		  CkipFlag;
+	USHORT        LenVIE;
+	UCHAR		  AironetCellPowerLimit;
+	EDCA_PARM       EdcaParm;
+	QBSS_LOAD_PARM  QbssLoad;
+	QOS_CAPABILITY_PARM QosCapability;
+	ULONG           RalinkIe;
+	// New for WPA security suites
+	UCHAR						VarIE[MAX_VIE_LEN];		// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen, PreNHtCapabilityLen;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+
+#ifdef RALINK_ATE
+    if (ATE_ON(pAd))
+    {
+		return;
+    }
+#endif // RALINK_ATE //
+
+	if (!(INFRA_ON(pAd) || ADHOC_ON(pAd)
+		))
+		return;
+
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+    RTMPZeroMemory(&HtCapability, sizeof(HtCapability));
+	RTMPZeroMemory(&AddHtInfo, sizeof(ADD_HT_INFO_IE));
+
+	if (PeerBeaconAndProbeRspSanity(pAd,
+								Elem->Msg,
+								Elem->MsgLen,
+								Elem->Channel,
+								Addr2,
+								Bssid,
+								Ssid,
+								&SsidLen,
+								&BssType,
+								&BeaconPeriod,
+								&Channel,
+								&NewChannel,
+								&TimeStamp,
+								&CfParm,
+								&AtimWin,
+								&CapabilityInfo,
+								&Erp,
+								&DtimCount,
+								&DtimPeriod,
+								&BcastFlag,
+								&MessageToMe,
+								SupRate,
+								&SupRateLen,
+								ExtRate,
+								&ExtRateLen,
+								&CkipFlag,
+								&AironetCellPowerLimit,
+								&EdcaParm,
+								&QbssLoad,
+								&QosCapability,
+								&RalinkIe,
+								&HtCapabilityLen,
+								&PreNHtCapabilityLen,
+								&HtCapability,
+								&AddHtInfoLen,
+								&AddHtInfo,
+								&NewExtChannelOffset,
+								&LenVIE,
+								pVIE))
+	{
+		BOOLEAN is_my_bssid, is_my_ssid;
+		ULONG   Bssidx, Now;
+		BSS_ENTRY *pBss;
+		CHAR		RealRssi = RTMPMaxRssi(pAd, ConvertToRssi(pAd, Elem->Rssi0, RSSI_0), ConvertToRssi(pAd, Elem->Rssi1, RSSI_1), ConvertToRssi(pAd, Elem->Rssi2, RSSI_2));
+
+		is_my_bssid = MAC_ADDR_EQUAL(Bssid, pAd->CommonCfg.Bssid)? TRUE : FALSE;
+		is_my_ssid = SSID_EQUAL(Ssid, SsidLen, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen)? TRUE:FALSE;
+
+
+		// ignore BEACON not for my SSID
+		if ((! is_my_ssid) && (! is_my_bssid))
+			return;
+
+		// It means STA waits disassoc completely from this AP, ignores this beacon.
+		if (pAd->Mlme.CntlMachine.CurrState == CNTL_WAIT_DISASSOC)
+			return;
+
+#ifdef DOT11_N_SUPPORT
+		// Copy Control channel for this BSSID.
+		if (AddHtInfoLen != 0)
+			Channel = AddHtInfo.ControlChan;
+
+		if ((HtCapabilityLen > 0) || (PreNHtCapabilityLen > 0))
+			HtCapabilityLen = SIZE_HT_CAP_IE;
+#endif // DOT11_N_SUPPORT //
+
+		//
+		// Housekeeping "SsidBssTab" table for later-on ROAMing usage.
+		//
+		Bssidx = BssTableSearch(&pAd->ScanTab, Bssid, Channel);
+		if (Bssidx == BSS_NOT_FOUND)
+		{
+			// discover new AP of this network, create BSS entry
+			Bssidx = BssTableSetEntry(pAd, &pAd->ScanTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+						 &CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,
+						&HtCapability, &AddHtInfo,HtCapabilityLen,AddHtInfoLen,NewExtChannelOffset, Channel,
+						RealRssi, TimeStamp, CkipFlag, &EdcaParm, &QosCapability,
+						&QbssLoad, LenVIE, pVIE);
+			if (Bssidx == BSS_NOT_FOUND) // return if BSS table full
+				return;
+
+			NdisMoveMemory(pAd->ScanTab.BssEntry[Bssidx].PTSF, &Elem->Msg[24], 4);
+			NdisMoveMemory(&pAd->ScanTab.BssEntry[Bssidx].TTSF[0], &Elem->TimeStamp.u.LowPart, 4);
+			NdisMoveMemory(&pAd->ScanTab.BssEntry[Bssidx].TTSF[4], &Elem->TimeStamp.u.LowPart, 4);
+
+
+
+		}
+
+		if ((pAd->CommonCfg.bIEEE80211H == 1) && (NewChannel != 0) && (Channel != NewChannel))
+		{
+			// Switching to channel 1 can prevent from rescanning the current channel immediately (by auto reconnection).
+			// In addition, clear the MLME queue and the scan table to discard the RX packets and previous scanning results.
+			AsicSwitchChannel(pAd, 1, FALSE);
+			AsicLockChannel(pAd, 1);
+		    LinkDown(pAd, FALSE);
+			MlmeQueueInit(&pAd->Mlme.Queue);
+			BssTableInit(&pAd->ScanTab);
+		    RTMPusecDelay(1000000);		// use delay to prevent STA do reassoc
+
+			// channel sanity check
+			for (index = 0 ; index < pAd->ChannelListNum; index++)
+			{
+				if (pAd->ChannelList[index].Channel == NewChannel)
+				{
+					pAd->ScanTab.BssEntry[Bssidx].Channel = NewChannel;
+					pAd->CommonCfg.Channel = NewChannel;
+					AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerBeacon - STA receive channel switch announcement IE (New Channel =%d)\n", NewChannel));
+					break;
+				}
+			}
+
+			if (index >= pAd->ChannelListNum)
+			{
+				DBGPRINT_ERR(("PeerBeacon(can not find New Channel=%d in ChannelList[%d]\n", pAd->CommonCfg.Channel, pAd->ChannelListNum));
+			}
+		}
+
+		// if the ssid matched & bssid unmatched, we should select the bssid with large value.
+		// This might happened when two STA start at the same time
+		if ((! is_my_bssid) && ADHOC_ON(pAd))
+		{
+			INT	i;
+
+			// Add the safeguard against the mismatch of adhoc wep status
+			if (pAd->StaCfg.WepStatus != pAd->ScanTab.BssEntry[Bssidx].WepStatus)
+			{
+				return;
+			}
+
+			// collapse into the ADHOC network which has bigger BSSID value.
+			for (i = 0; i < 6; i++)
+			{
+				if (Bssid[i] > pAd->CommonCfg.Bssid[i])
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - merge to the IBSS with bigger BSSID=%02x:%02x:%02x:%02x:%02x:%02x\n",
+						Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+					AsicDisableSync(pAd);
+					COPY_MAC_ADDR(pAd->CommonCfg.Bssid, Bssid);
+					AsicSetBssid(pAd, pAd->CommonCfg.Bssid);
+					MakeIbssBeacon(pAd);        // re-build BEACON frame
+					AsicEnableIbssSync(pAd);    // copy BEACON frame to on-chip memory
+					is_my_bssid = TRUE;
+					break;
+				}
+				else if (Bssid[i] < pAd->CommonCfg.Bssid[i])
+					break;
+			}
+		}
+
+
+		NdisGetSystemUpTime(&Now);
+		pBss = &pAd->ScanTab.BssEntry[Bssidx];
+		pBss->Rssi = RealRssi;       // lastest RSSI
+		pBss->LastBeaconRxTime = Now;   // last RX timestamp
+
+		//
+		// BEACON from my BSSID - either IBSS or INFRA network
+		//
+		if (is_my_bssid)
+		{
+			RXWI_STRUC	RxWI;
+
+			pAd->StaCfg.DtimCount = DtimCount;
+			pAd->StaCfg.DtimPeriod = DtimPeriod;
+			pAd->StaCfg.LastBeaconRxTime = Now;
+
+
+			RxWI.RSSI0 = Elem->Rssi0;
+			RxWI.RSSI1 = Elem->Rssi1;
+			RxWI.RSSI2 = Elem->Rssi2;
+
+			Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, &RxWI);
+			if (AironetCellPowerLimit != 0xFF)
+			{
+				//
+				// We get the Cisco (ccx) "TxPower Limit" required
+				// Changed to appropriate TxPower Limit for Ciso Compatible Extensions
+				//
+				ChangeToCellPowerLimit(pAd, AironetCellPowerLimit);
+			}
+			else
+			{
+				//
+				// AironetCellPowerLimit equal to 0xFF means the Cisco (ccx) "TxPower Limit" not exist.
+				// Used the default TX Power Percentage, that set from UI.
+				//
+				pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+			}
+
+			// at least one 11b peer joined. downgrade the MaxTxRate to 11Mbps
+			// after last 11b peer left for several seconds, we'll auto switch back to 11G rate
+			// in MlmePeriodicExec()
+			if (ADHOC_ON(pAd) && (CAP_IS_IBSS_ON(CapabilityInfo)))
+			{
+				BOOLEAN	bRestart;
+                BOOLEAN	bnRestart;
+
+				bRestart = FALSE;
+                bnRestart = FALSE;
+
+				do
+				{
+					if ((SupRateLen+ExtRateLen <= 4) && (pAd->CommonCfg.MaxTxRate > RATE_11))
+					{
+						if (pAd->StaCfg.AdhocBOnlyJoined == FALSE)
+						{
+							DBGPRINT(RT_DEBUG_TRACE, ("SYNC - 11b peer joined. down-grade to 11b TX rates \n"));
+							bRestart = TRUE;
+							NdisMoveMemory(pAd->StaActive.SupRate, SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+							pAd->StaActive.SupRateLen = SupRateLen;
+							NdisMoveMemory(pAd->StaActive.ExtRate, ExtRate, MAX_LEN_OF_SUPPORTED_RATES);
+							pAd->StaActive.ExtRateLen = ExtRateLen;
+							pAd->StaCfg.AdhocBOnlyJoined = TRUE;
+							pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+							AsicSetEdcaParm(pAd, NULL);
+						}
+
+						// this timestamp is for MlmePeriodicExec() to check if all 11B peers have left
+						pAd->StaCfg.Last11bBeaconRxTime = Now;
+						break;
+					}
+#ifdef DOT11_N_SUPPORT
+					// Update Ht Phy.
+					if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+					{
+						if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) &&
+							!pAd->StaCfg.AdhocBGJoined &&
+							!pAd->StaCfg.AdhocBOnlyJoined)
+							AdhocTurnOnQos(pAd);
+
+						// Handle rate switch issue when Adhoc mode
+						if ((SupRateLen+ExtRateLen >= 8) && (HtCapability.MCSSet[0] == 0) && (HtCapability.MCSSet[1] == 0))
+						{
+							if (pAd->StaCfg.AdhocBGJoined == FALSE)
+							{
+								DBGPRINT(RT_DEBUG_TRACE, ("SYNC - 11g peer joined. down-grade to 11g TX rates \n"));
+								bRestart = TRUE;
+								NdisMoveMemory(pAd->StaActive.SupRate, SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+								pAd->StaActive.SupRateLen = SupRateLen;
+								NdisMoveMemory(pAd->StaActive.ExtRate, ExtRate, MAX_LEN_OF_SUPPORTED_RATES);
+								pAd->StaActive.ExtRateLen = ExtRateLen;
+								pAd->StaCfg.AdhocBGJoined = TRUE;
+								pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+								AsicSetEdcaParm(pAd, NULL);
+							}
+
+							// this timestamp is for MlmePeriodicExec() to check if all 11g peers have left
+							pAd->StaCfg.Last11gBeaconRxTime = Now;
+							break;
+						}
+						else if (!pAd->StaCfg.AdhocBGJoined &&
+								 !pAd->StaCfg.AdhocBOnlyJoined &&
+								 (pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40) &&
+								 (HtCapability.HtCapInfo.ChannelWidth == BW_20))
+						{
+							if (pAd->StaCfg.Adhoc20NJoined == FALSE)
+							{
+								pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+
+								pAd->StaCfg.Adhoc20NJoined = TRUE;
+								NdisMoveMemory(&pAd->MlmeAux.HtCapability, &HtCapability, SIZE_HT_CAP_IE);
+								if (AddHtInfoLen != 0)
+									NdisMoveMemory(&pAd->MlmeAux.AddHtInfo, &AddHtInfo, AddHtInfoLen);
+								NdisMoveMemory(pAd->StaActive.SupportedPhyInfo.MCSSet, HtCapability.MCSSet, 16);
+
+								RTMPCheckHt(pAd, Elem->Wcid, &pAd->MlmeAux.HtCapability, &pAd->MlmeAux.AddHtInfo);
+								COPY_HTSETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(pAd);
+								pAd->StaActive.SupportedPhyInfo.bHtEnable = TRUE;
+								bRestart = TRUE;
+								bnRestart = TRUE;
+							}
+							// this timestamp is for MlmePeriodicExec() to check if all 20MHz N peers have left
+							pAd->StaCfg.Last20NBeaconRxTime = Now;
+						}
+
+					}
+					else
+#endif // DOT11_N_SUPPORT //
+					{
+						RTMPZeroMemory(&pAd->MlmeAux.HtCapability, SIZE_HT_CAP_IE);
+						RTMPZeroMemory(&pAd->MlmeAux.AddHtInfo, SIZE_ADD_HT_INFO_IE);
+					}
+				}while (FALSE);
+
+				// If peer Adhoc is legacy mode, I don't need to call MlmeUpdateHtTxRates no matter I support HT or not
+				if ((bRestart == TRUE) && (bnRestart == FALSE))
+				{
+					MlmeUpdateTxRates(pAd, FALSE, 0);
+					MakeIbssBeacon(pAd);        // re-build BEACON frame
+					AsicEnableIbssSync(pAd);    // copy to on-chip memory
+				}
+#ifdef DOT11_N_SUPPORT
+				else if ((bRestart == TRUE) && (bnRestart == TRUE))
+				{
+					MlmeUpdateTxRates(pAd, FALSE, BSS0);
+					MlmeUpdateHtTxRates(pAd, BSS0);
+					MakeIbssBeacon(pAd);        // re-build BEACON frame
+					AsicEnableIbssSync(pAd);    // copy to on-chip memory
+				}
+#endif // DOT11_N_SUPPORT //
+
+				// At least another peer in this IBSS, declare MediaState as CONNECTED
+				if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				{
+					OPSTATUS_SET_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+
+					pAd->IndicateMediaState = NdisMediaStateConnected;
+					RTMP_IndicateMediaState(pAd);
+	                pAd->ExtraInfo = GENERAL_LINK_UP;
+					AsicSetBssid(pAd, pAd->CommonCfg.Bssid);
+
+					// 2003/03/12 - john
+					// Make sure this entry in "ScanTab" table, thus complies to Microsoft's policy that
+					// "site survey" result should always include the current connected network.
+					//
+					Bssidx = BssTableSearch(&pAd->ScanTab, Bssid, Channel);
+					if (Bssidx == BSS_NOT_FOUND)
+					{
+						Bssidx = BssTableSetEntry(pAd, &pAd->ScanTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+									&CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen, &HtCapability,
+									&AddHtInfo, HtCapabilityLen, AddHtInfoLen, NewExtChannelOffset, Channel, RealRssi, TimeStamp, 0,
+									&EdcaParm, &QosCapability, &QbssLoad, LenVIE, pVIE);
+					}
+					DBGPRINT(RT_DEBUG_TRACE, ("ADHOC  fOP_STATUS_MEDIA_STATE_CONNECTED.\n"));
+				}
+
+				// Ad-hoc mode is using MAC address as BA session. So we need to continuously find newly joined adhoc station by receiving beacon.
+				// To prevent always check this, we use wcid == RESERVED_WCID to recognize it as newly joined adhoc station.
+				if (ADHOC_ON(pAd) && (Elem->Wcid == RESERVED_WCID))
+				{
+					UCHAR	idx;
+					MAC_TABLE_ENTRY *pEntry;
+
+					// look up the existing table
+					pEntry = MacTableLookup(pAd, Addr2);
+					if (pEntry == NULL)
+					{
+						// Another adhoc joining, add to our MAC table.
+						pEntry = MacTableInsertEntry(pAd, Addr2, BSS0, FALSE);
+						if (pEntry)
+						{
+							pEntry->Sst = SST_ASSOC;
+							idx = pAd->StaCfg.DefaultKeyId;
+							// After InsertEntry, Write to ASIC on-chip table.
+							RT28XX_STA_SECURITY_INFO_ADD(pAd, BSS0, idx, pEntry);
+							DBGPRINT(RT_DEBUG_TRACE, ("ADHOC %x:%x:%x:%x:%x:%x  join in.Entry=%d\n", Addr2[0],Addr2[1],Addr2[2],Addr2[3],Addr2[4],Addr2[5], pEntry->Aid));
+
+							pEntry->HTPhyMode.word = pAd->StaCfg.HTPhyMode.word;
+	                        if (HtCapabilityLen <= 0)
+	                        {
+	                            pEntry->HTPhyMode.field.STBC = 0;
+	                            pEntry->HTPhyMode.field.BW = 0;
+	                            pEntry->HTPhyMode.field.ShortGI = 0;
+	                            if ((SupRateLen+ExtRateLen <= 4) && (pAd->CommonCfg.Channel <= 14))
+	        					{
+	        						pEntry->HTPhyMode.field.MODE = MODE_CCK;
+	        					}
+	        					else
+	        					{
+	        						pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+	        					}
+								MlmeUpdateTxRates(pAd, FALSE, 0);
+	                        }
+#ifdef DOT11_N_SUPPORT
+							else
+							{
+								MlmeUpdateTxRates(pAd, FALSE, 0);
+								MlmeUpdateHtTxRates(pAd, BSS0);
+							}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+	                        if (pAd->StaCfg.WpaSupplicantUP)
+	                        {
+	                            union iwreq_data    wrqu;
+
+	                            SendAssocIEsToWpaSupplicant(pAd);
+	                            memset(&wrqu, 0, sizeof(wrqu));
+	                            wrqu.data.flags = RT_ASSOC_EVENT_FLAG;
+	                            wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+	                        }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+	                        {
+	                            union iwreq_data    wrqu;
+	                            wext_notify_event_assoc(pAd);
+
+	                            memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+	                            memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+	                            wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+	                        }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+						}
+					}
+				}
+			}
+
+			if (INFRA_ON(pAd))
+			{
+				BOOLEAN bUseShortSlot, bUseBGProtection;
+
+				// decide to use/change to -
+				//      1. long slot (20 us) or short slot (9 us) time
+				//      2. turn on/off RTS/CTS and/or CTS-to-self protection
+				//      3. short preamble
+
+				//bUseShortSlot = pAd->CommonCfg.bUseShortSlotTime && CAP_IS_SHORT_SLOT(CapabilityInfo);
+				bUseShortSlot = CAP_IS_SHORT_SLOT(CapabilityInfo);
+				if (bUseShortSlot != OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SHORT_SLOT_INUSED))
+					AsicSetSlotTime(pAd, bUseShortSlot);
+
+				bUseBGProtection = (pAd->CommonCfg.UseBGProtection == 1) ||    // always use
+								   ((pAd->CommonCfg.UseBGProtection == 0) && ERP_IS_USE_PROTECTION(Erp));
+
+				if (pAd->CommonCfg.Channel > 14) // always no BG protection in A-band. falsely happened when switching A/G band to a dual-band AP
+					bUseBGProtection = FALSE;
+
+				if (bUseBGProtection != OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED))
+				{
+					if (bUseBGProtection)
+					{
+						OPSTATUS_SET_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED);
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, (OFDMSETPROTECT|CCKSETPROTECT|ALLN_SETPROTECT),FALSE,(pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1));
+					}
+					else
+					{
+						OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED);
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, (OFDMSETPROTECT|CCKSETPROTECT|ALLN_SETPROTECT),TRUE,(pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1));
+					}
+
+					DBGPRINT(RT_DEBUG_WARN, ("SYNC - AP changed B/G protection to %d\n", bUseBGProtection));
+				}
+
+#ifdef DOT11_N_SUPPORT
+				// check Ht protection mode. and adhere to the Non-GF device indication by AP.
+				if ((AddHtInfoLen != 0) &&
+					((AddHtInfo.AddHtInfo2.OperaionMode != pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode) ||
+					(AddHtInfo.AddHtInfo2.NonGfPresent != pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent)))
+				{
+					pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent = AddHtInfo.AddHtInfo2.NonGfPresent;
+					pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode = AddHtInfo.AddHtInfo2.OperaionMode;
+					if (pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1)
+				{
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, FALSE, TRUE);
+					}
+					else
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, FALSE, FALSE);
+
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - AP changed N OperaionMode to %d\n", pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode));
+				}
+#endif // DOT11_N_SUPPORT //
+
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED) &&
+					ERP_IS_USE_BARKER_PREAMBLE(Erp))
+				{
+					MlmeSetTxPreamble(pAd, Rt802_11PreambleLong);
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - AP forced to use LONG preamble\n"));
+				}
+
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED)    &&
+					(EdcaParm.bValid == TRUE)                          &&
+					(EdcaParm.EdcaUpdateCount != pAd->CommonCfg.APEdcaParm.EdcaUpdateCount))
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - AP change EDCA parameters(from %d to %d)\n",
+						pAd->CommonCfg.APEdcaParm.EdcaUpdateCount,
+						EdcaParm.EdcaUpdateCount));
+					AsicSetEdcaParm(pAd, &EdcaParm);
+				}
+
+				// copy QOS related information
+				NdisMoveMemory(&pAd->CommonCfg.APQbssLoad, &QbssLoad, sizeof(QBSS_LOAD_PARM));
+				NdisMoveMemory(&pAd->CommonCfg.APQosCapability, &QosCapability, sizeof(QOS_CAPABILITY_PARM));
+			}
+
+			// only INFRASTRUCTURE mode support power-saving feature
+			if ((INFRA_ON(pAd) && (pAd->StaCfg.Psm == PWR_SAVE)) || (pAd->CommonCfg.bAPSDForcePowerSave))
+			{
+				UCHAR FreeNumber;
+				//  1. AP has backlogged unicast-to-me frame, stay AWAKE, send PSPOLL
+				//  2. AP has backlogged broadcast/multicast frame and we want those frames, stay AWAKE
+				//  3. we have outgoing frames in TxRing or MgmtRing, better stay AWAKE
+				//  4. Psm change to PWR_SAVE, but AP not been informed yet, we better stay AWAKE
+				//  5. otherwise, put PHY back to sleep to save battery.
+				if (MessageToMe)
+				{
+#ifdef RT2860
+					if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, pAd->StaCfg.BBPR3);
+						// Turn clk to 80Mhz.
+					}
+#endif // RT2860 //
+					if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable &&
+						pAd->CommonCfg.bAPSDAC_BE && pAd->CommonCfg.bAPSDAC_BK && pAd->CommonCfg.bAPSDAC_VI && pAd->CommonCfg.bAPSDAC_VO)
+					{
+						pAd->CommonCfg.bNeedSendTriggerFrame = TRUE;
+					}
+					else
+						RT28XX_PS_POLL_ENQUEUE(pAd);
+				}
+				else if (BcastFlag && (DtimCount == 0) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM))
+				{
+#ifdef RT2860
+					if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, pAd->StaCfg.BBPR3);
+					}
+#endif // RT2860 //
+				}
+				else if ((pAd->TxSwQueue[QID_AC_BK].Number != 0)													||
+						(pAd->TxSwQueue[QID_AC_BE].Number != 0)														||
+						(pAd->TxSwQueue[QID_AC_VI].Number != 0)														||
+						(pAd->TxSwQueue[QID_AC_VO].Number != 0)														||
+						(RTMPFreeTXDRequest(pAd, QID_AC_BK, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_AC_BE, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_AC_VI, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_AC_VO, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_MGMT, MGMT_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS))
+				{
+					// TODO: consider scheduled HCCA. might not be proper to use traditional DTIM-based power-saving scheme
+					// can we cheat here (i.e. just check MGMT & AC_BE) for better performance?
+#ifdef RT2860
+					if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, pAd->StaCfg.BBPR3);
+					}
+#endif // RT2860 //
+				}
+				else
+				{
+					USHORT NextDtim = DtimCount;
+
+					if (NextDtim == 0)
+						NextDtim = DtimPeriod;
+
+					TbttNumToNextWakeUp = pAd->StaCfg.DefaultListenCount;
+					if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM) && (TbttNumToNextWakeUp > NextDtim))
+						TbttNumToNextWakeUp = NextDtim;
+
+					if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+					{
+						AsicSleepThenAutoWakeup(pAd, TbttNumToNextWakeUp);
+					}
+				}
+			}
+		}
+		// not my BSSID, ignore it
+	}
+	// sanity check fail, ignore this frame
+}
+
+/*
+	==========================================================================
+	Description:
+		Receive PROBE REQ from remote peer when operating in IBSS mode
+	==========================================================================
+ */
+VOID PeerProbeReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Addr2[MAC_ADDR_LEN];
+	CHAR          Ssid[MAX_LEN_OF_SSID];
+	UCHAR         SsidLen;
+#ifdef DOT11_N_SUPPORT
+	UCHAR		  HtLen, AddHtLen, NewExtLen;
+#endif // DOT11_N_SUPPORT //
+	HEADER_802_11 ProbeRspHdr;
+	NDIS_STATUS   NStatus;
+	PUCHAR        pOutBuffer = NULL;
+	ULONG         FrameLen = 0;
+	LARGE_INTEGER FakeTimestamp;
+	UCHAR         DsLen = 1, IbssLen = 2;
+	UCHAR         LocalErpIe[3] = {IE_ERP, 1, 0};
+	BOOLEAN       Privacy;
+	USHORT        CapabilityInfo;
+	UCHAR		  RSNIe = IE_WPA;
+
+	if (! ADHOC_ON(pAd))
+		return;
+
+	if (PeerProbeReqSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, Ssid, &SsidLen))
+	{
+		if ((SsidLen == 0) || SSID_EQUAL(Ssid, SsidLen, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen))
+		{
+			// allocate and send out ProbeRsp frame
+			NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus != NDIS_STATUS_SUCCESS)
+				return;
+
+			//pAd->StaCfg.AtimWin = 0;  // ??????
+
+			Privacy = (pAd->StaCfg.WepStatus == Ndis802_11Encryption1Enabled) ||
+					  (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+					  (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled);
+			CapabilityInfo = CAP_GENERATE(0, 1, Privacy, (pAd->CommonCfg.TxPreamble == Rt802_11PreambleShort), 0, 0);
+
+			MakeOutgoingFrame(pOutBuffer,                   &FrameLen,
+							  sizeof(HEADER_802_11),        &ProbeRspHdr,
+							  TIMESTAMP_LEN,                &FakeTimestamp,
+							  2,                            &pAd->CommonCfg.BeaconPeriod,
+							  2,                            &CapabilityInfo,
+							  1,                            &SsidIe,
+							  1,                            &pAd->CommonCfg.SsidLen,
+							  pAd->CommonCfg.SsidLen,       pAd->CommonCfg.Ssid,
+							  1,                            &SupRateIe,
+							  1,                            &pAd->StaActive.SupRateLen,
+							  pAd->StaActive.SupRateLen,    pAd->StaActive.SupRate,
+							  1,                            &DsIe,
+							  1,                            &DsLen,
+							  1,                            &pAd->CommonCfg.Channel,
+							  1,                            &IbssIe,
+							  1,                            &IbssLen,
+							  2,                            &pAd->StaActive.AtimWin,
+							  END_OF_ARGS);
+
+			if (pAd->StaActive.ExtRateLen)
+			{
+				ULONG tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,        &tmp,
+								  3,                            LocalErpIe,
+								  1,                            &ExtRateIe,
+								  1,                            &pAd->StaActive.ExtRateLen,
+								  pAd->StaActive.ExtRateLen,    &pAd->StaActive.ExtRate,
+								  END_OF_ARGS);
+				FrameLen += tmp;
+			}
+
+			// If adhoc secruity is set for WPA-None, append the cipher suite IE
+			if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+			{
+				ULONG tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,        	&tmp,
+						  			1,                              &RSNIe,
+						  			1,                            	&pAd->StaCfg.RSNIE_Len,
+						  			pAd->StaCfg.RSNIE_Len,      	pAd->StaCfg.RSN_IE,
+						  			END_OF_ARGS);
+				FrameLen += tmp;
+			}
+#ifdef DOT11_N_SUPPORT
+			if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+			{
+				ULONG TmpLen;
+				UCHAR	BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+				HtLen = sizeof(pAd->CommonCfg.HtCapability);
+				AddHtLen = sizeof(pAd->CommonCfg.AddHTInfo);
+				NewExtLen = 1;
+				//New extension channel offset IE is included in Beacon, Probe Rsp or channel Switch Announcement Frame
+				if (pAd->bBroadComHT == TRUE)
+				{
+					MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+								  1,                                &WpaIe,
+								  4,                                &BROADCOM[0],
+								 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+								  END_OF_ARGS);
+				}
+				else
+				{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+								  1,                                &HtCapIe,
+								  1,                                &HtLen,
+								 sizeof(HT_CAPABILITY_IE),          &pAd->CommonCfg.HtCapability,
+								  1,                                &AddHtInfoIe,
+								  1,                                &AddHtLen,
+								 sizeof(ADD_HT_INFO_IE),          &pAd->CommonCfg.AddHTInfo,
+								  1,                                &NewExtChanIe,
+								  1,                                &NewExtLen,
+								 sizeof(NEW_EXT_CHAN_IE),          &pAd->CommonCfg.NewExtChanOffset,
+								  END_OF_ARGS);
+				}
+				FrameLen += TmpLen;
+			}
+#endif // DOT11_N_SUPPORT //
+			MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+	}
+}
+
+VOID BeaconTimeoutAtJoinAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - BeaconTimeoutAtJoinAction\n"));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_REJ_TIMEOUT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_JOIN_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		Scan timeout procedure. basically add channel index by 1 and rescan
+	==========================================================================
+ */
+VOID ScanTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	pAd->MlmeAux.Channel = NextChannel(pAd, pAd->MlmeAux.Channel);
+
+	// Only one channel scanned for CISCO beacon request
+	if ((pAd->MlmeAux.ScanType == SCAN_CISCO_ACTIVE) ||
+		(pAd->MlmeAux.ScanType == SCAN_CISCO_PASSIVE) ||
+		(pAd->MlmeAux.ScanType == SCAN_CISCO_NOISE) ||
+		(pAd->MlmeAux.ScanType == SCAN_CISCO_CHANNEL_LOAD))
+		pAd->MlmeAux.Channel = 0;
+
+	// this routine will stop if pAd->MlmeAux.Channel == 0
+	ScanNextChannel(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID InvalidStateWhenScan(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("AYNC - InvalidStateWhenScan(state=%ld). Reset SYNC machine\n", pAd->Mlme.SyncMachine.CurrState));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID InvalidStateWhenJoin(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("InvalidStateWhenJoin(state=%ld). Reset SYNC machine\n", pAd->Mlme.SyncMachine.CurrState));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_JOIN_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID InvalidStateWhenStart(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("InvalidStateWhenStart(state=%ld). Reset SYNC machine\n", pAd->Mlme.SyncMachine.CurrState));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_START_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID EnqueuePsPoll(
+	IN PRTMP_ADAPTER pAd)
+{
+#ifdef RALINK_ATE
+    if (ATE_ON(pAd))
+    {
+		return;
+    }
+#endif // RALINK_ATE //
+
+
+	if (pAd->StaCfg.WindowsPowerMode == Ndis802_11PowerModeLegacy_PSP)
+    	pAd->PsPollFrame.FC.PwrMgmt = PWR_SAVE;
+	MiniportMMRequest(pAd, 0, (PUCHAR)&pAd->PsPollFrame, sizeof(PSPOLL_FRAME));
+}
+
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID EnqueueProbeRequest(
+	IN PRTMP_ADAPTER pAd)
+{
+	NDIS_STATUS     NState;
+	PUCHAR          pOutBuffer;
+	ULONG           FrameLen = 0;
+	HEADER_802_11   Hdr80211;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("force out a ProbeRequest ...\n"));
+
+	NState = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NState == NDIS_STATUS_SUCCESS)
+	{
+		MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, BROADCAST_ADDR, BROADCAST_ADDR);
+
+		// this ProbeRequest explicitly specify SSID to reduce unwanted ProbeResponse
+		MakeOutgoingFrame(pOutBuffer,                     &FrameLen,
+						  sizeof(HEADER_802_11),          &Hdr80211,
+						  1,                              &SsidIe,
+						  1,                              &pAd->CommonCfg.SsidLen,
+						  pAd->CommonCfg.SsidLen,		  pAd->CommonCfg.Ssid,
+						  1,                              &SupRateIe,
+						  1,                              &pAd->StaActive.SupRateLen,
+						  pAd->StaActive.SupRateLen,      pAd->StaActive.SupRate,
+						  END_OF_ARGS);
+		MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+	}
+
+}
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+VOID BuildEffectedChannelList(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR		EChannel[11];
+	UCHAR		i, j, k;
+	UCHAR		UpperChannel = 0, LowerChannel = 0;
+
+	RTMPZeroMemory(EChannel, 11);
+	i = 0;
+	// Find upper channel and lower channel.
+	if (pAd->CommonCfg.CentralChannel < pAd->CommonCfg.Channel)
+	{
+		UpperChannel = pAd->CommonCfg.Channel;
+		LowerChannel = pAd->CommonCfg.CentralChannel;
+	}
+	else if (pAd->CommonCfg.CentralChannel > pAd->CommonCfg.Channel)
+	{
+		UpperChannel = pAd->CommonCfg.CentralChannel;
+		LowerChannel = pAd->CommonCfg.Channel;
+	}
+	else
+	{
+		return;
+	}
+
+	// Record channels that is below lower channel..
+	if (LowerChannel > 1)
+	{
+		EChannel[0] = LowerChannel - 1;
+		i = 1;
+		if (LowerChannel > 2)
+		{
+			EChannel[1] = LowerChannel - 2;
+			i = 2;
+			if (LowerChannel > 3)
+			{
+				EChannel[2] = LowerChannel - 3;
+				i = 3;
+			}
+		}
+	}
+	// Record channels that is between  lower channel and upper channel.
+	for (k = LowerChannel;k < UpperChannel;k++)
+	{
+		EChannel[i] = k;
+		i++;
+	}
+	// Record channels that is above upper channel..
+	if (LowerChannel < 11)
+	{
+		EChannel[i] = UpperChannel + 1;
+		i++;
+		if (LowerChannel < 10)
+		{
+			EChannel[i] = LowerChannel + 2;
+			i++;
+			if (LowerChannel < 9)
+			{
+				EChannel[i] = LowerChannel + 3;
+				i++;
+			}
+		}
+	}
+	//
+	for (j = 0;j < i;j++)
+	{
+		for (k = 0;k < pAd->ChannelListNum;k++)
+		{
+			if (pAd->ChannelList[k].Channel == EChannel[j])
+			{
+				pAd->ChannelList[k].bEffectedChannel = TRUE;
+				DBGPRINT(RT_DEBUG_TRACE,(" EffectedChannel( =%d)\n", EChannel[j]));
+				break;
+			}
+		}
+	}
+}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+BOOLEAN ScanRunning(
+		IN PRTMP_ADAPTER pAd)
+{
+	return (pAd->Mlme.SyncMachine.CurrState == SCAN_LISTEN) ? TRUE : FALSE;
+}
+
diff --git a/drivers/staging/rt2860/sta/wpa.c b/drivers/staging/rt2860/sta/wpa.c
new file mode 100644
index 000000000000..774c6567ae53
--- /dev/null
+++ b/drivers/staging/rt2860/sta/wpa.c
@@ -0,0 +1,2086 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	wpa.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Jan	Lee		03-07-22		Initial
+	Paul Lin	03-11-28		Modify for supplicant
+*/
+#include "../rt_config.h"
+
+#define		WPARSNIE	0xdd
+#define		WPA2RSNIE	0x30
+
+//extern UCHAR BIT8[];
+UCHAR	CipherWpaPskTkip[] = {
+		0xDD, 0x16,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x02	// authentication
+		};
+UCHAR	CipherWpaPskTkipLen = (sizeof(CipherWpaPskTkip) / sizeof(UCHAR));
+
+UCHAR	CipherWpaPskAes[] = {
+		0xDD, 0x16, 			// RSN IE
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x04,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x04,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x02	// authentication
+		};
+UCHAR	CipherWpaPskAesLen = (sizeof(CipherWpaPskAes) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteCiscoCCKM[] = {
+		0xDD, 0x16,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01, // oui
+		0x01, 0x00,				// Version
+		0x00, 0x40, 0x96, 0x01, // Multicast
+		0x01, 0x00,				// Number of uicast
+		0x00, 0x40, 0x96, 0x01, // unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x40, 0x96, 0x00  // Authentication
+		};
+UCHAR	CipherSuiteCiscoCCKMLen = (sizeof(CipherSuiteCiscoCCKM) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteCiscoCCKM24[] = {
+		0xDD, 0x18,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01, // oui
+		0x01, 0x00,				// Version
+		0x00, 0x40, 0x96, 0x01, // Multicast
+		0x01, 0x00,				// Number of uicast
+		0x00, 0x40, 0x96, 0x01, // unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x40, 0x96, 0x00,
+		0x28, 0x00// Authentication
+		};
+
+UCHAR	CipherSuiteCiscoCCKM24Len = (sizeof(CipherSuiteCiscoCCKM24) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteCCXTkip[] = {
+		0xDD, 0x16,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x01	// authentication
+		};
+UCHAR	CipherSuiteCCXTkipLen = (sizeof(CipherSuiteCCXTkip) / sizeof(UCHAR));
+
+UCHAR	CCX_LLC_HDR[] = {0xAA, 0xAA, 0x03, 0x00, 0x40, 0x96, 0x00, 0x02};
+UCHAR	LLC_NORMAL[] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};
+
+UCHAR	EAPOL_FRAME[] = {0x88, 0x8E};
+
+BOOLEAN CheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	OUT	UCHAR			*Offset);
+
+void inc_byte_array(UCHAR *counter, int len);
+
+/*
+	========================================================================
+
+	Routine Description:
+		Classify WPA EAP message type
+
+	Arguments:
+		EAPType		Value of EAP message type
+		MsgType		Internal Message definition for MLME state machine
+
+	Return Value:
+		TRUE		Found appropriate message type
+		FALSE		No appropriate message type
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		All these constants are defined in wpa.h
+		For supplicant, there is only EAPOL Key message avaliable
+
+	========================================================================
+*/
+BOOLEAN	WpaMsgTypeSubst(
+	IN	UCHAR	EAPType,
+	OUT	INT		*MsgType)
+{
+	switch (EAPType)
+	{
+		case EAPPacket:
+			*MsgType = MT2_EAPPacket;
+			break;
+		case EAPOLStart:
+			*MsgType = MT2_EAPOLStart;
+			break;
+		case EAPOLLogoff:
+			*MsgType = MT2_EAPOLLogoff;
+			break;
+		case EAPOLKey:
+			*MsgType = MT2_EAPOLKey;
+			break;
+		case EAPOLASFAlert:
+			*MsgType = MT2_EAPOLASFAlert;
+			break;
+		default:
+			return FALSE;
+	}
+	return TRUE;
+}
+
+/*
+	==========================================================================
+	Description:
+		association	state machine init,	including state	transition and timer init
+	Parameters:
+		S -	pointer	to the association state machine
+	==========================================================================
+ */
+VOID WpaPskStateMachineInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	STATE_MACHINE *S,
+	OUT	STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(S,	Trans, MAX_WPA_PSK_STATE, MAX_WPA_PSK_MSG, (STATE_MACHINE_FUNC)Drop, WPA_PSK_IDLE, WPA_MACHINE_BASE);
+	StateMachineSetAction(S, WPA_PSK_IDLE, MT2_EAPOLKey, (STATE_MACHINE_FUNC)WpaEAPOLKeyAction);
+}
+
+/*
+	==========================================================================
+	Description:
+		This is	state machine function.
+		When receiving EAPOL packets which is  for 802.1x key management.
+		Use	both in	WPA, and WPAPSK	case.
+		In this	function, further dispatch to different	functions according	to the received	packet.	 3 categories are :
+		  1.  normal 4-way pairwisekey and 2-way groupkey handshake
+		  2.  MIC error	(Countermeasures attack)  report packet	from STA.
+		  3.  Request for pairwise/group key update	from STA
+	Return:
+	==========================================================================
+*/
+VOID WpaEAPOLKeyAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+
+{
+	INT             MsgType = EAPOL_MSG_INVALID;
+	PKEY_DESCRIPTER pKeyDesc;
+	PHEADER_802_11  pHeader; //red
+	UCHAR           ZeroReplay[LEN_KEY_DESC_REPLAY];
+	UCHAR EapolVr;
+	KEY_INFO		peerKeyInfo;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-----> WpaEAPOLKeyAction\n"));
+
+	// Get 802.11 header first
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Get EAPoL-Key Descriptor
+	pKeyDesc = (PKEY_DESCRIPTER) &Elem->Msg[(LENGTH_802_11 + LENGTH_802_1_H + LENGTH_EAPOL_H)];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pKeyDesc->KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT *)&peerKeyInfo) = cpu2le16(*((USHORT *)&peerKeyInfo));
+
+
+	// 1. Check EAPOL frame version and type
+	EapolVr	= (UCHAR) Elem->Msg[LENGTH_802_11+LENGTH_802_1_H];
+
+    if (((EapolVr != EAPOL_VER) && (EapolVr != EAPOL_VER2)) || ((pKeyDesc->Type != WPA1_KEY_DESC) && (pKeyDesc->Type != WPA2_KEY_DESC)))
+	{
+        DBGPRINT(RT_DEBUG_ERROR, ("Key descripter does not match with WPA rule\n"));
+			return;
+	}
+
+	// First validate replay counter, only accept message with larger replay counter
+	// Let equal pass, some AP start with all zero replay counter
+	NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);
+
+	if((RTMPCompareMemory(pKeyDesc->ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1) &&
+		(RTMPCompareMemory(pKeyDesc->ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("   ReplayCounter not match   \n"));
+		return;
+	}
+
+	// Process WPA2PSK frame
+	if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+	{
+		if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.EKD_DL == 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 0) &&
+			(peerKeyInfo.Secure == 0) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 1\n"));
+		} else if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.EKD_DL  == 1) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 1) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_3;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 3\n"));
+		} else if((peerKeyInfo.KeyType == GROUPKEY) &&
+			(peerKeyInfo.EKD_DL == 1) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 1) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_GROUP_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Group Message 1\n"));
+		}
+
+		// We will assume link is up (assoc suceess and port not secured).
+		// All state has to be able to process message from previous state
+		switch(pAd->StaCfg.WpaState)
+		{
+		case SS_START:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				Wpa2PairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			break;
+
+		case SS_WAIT_MSG_3:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				Wpa2PairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+				Wpa2PairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+			}
+			break;
+
+		case SS_WAIT_GROUP:		// When doing group key exchange
+		case SS_FINISH:			// This happened when update group key
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+			    // Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+				Wpa2PairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+			    // Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+				Wpa2PairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+			}
+			else if(MsgType == EAPOL_GROUP_MSG_1)
+			{
+				WpaGroupMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_FINISH;
+			}
+			break;
+
+		default:
+			break;
+		}
+	}
+	// Process WPAPSK Frame
+	// Classify message Type, either pairwise message 1, 3, or group message 1 for supplicant
+	else if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+	{
+		if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.KeyIndex == 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 0) &&
+			(peerKeyInfo.Secure == 0) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 1\n"));
+		}
+		else if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.KeyIndex == 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 0) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_3;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 3\n"));
+		}
+		else if((peerKeyInfo.KeyType == GROUPKEY) &&
+			(peerKeyInfo.KeyIndex != 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 1) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_GROUP_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Group Message 1\n"));
+		}
+
+		// We will assume link is up (assoc suceess and port not secured).
+		// All state has to be able to process message from previous state
+		switch(pAd->StaCfg.WpaState)
+		{
+		case SS_START:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				WpaPairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			break;
+
+		case SS_WAIT_MSG_3:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				WpaPairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+				WpaPairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+			}
+			break;
+
+		case SS_WAIT_GROUP:		// When doing group key exchange
+		case SS_FINISH:			// This happened when update group key
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				WpaPairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+				// Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+				WpaPairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+				// Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			}
+			else if(MsgType == EAPOL_GROUP_MSG_1)
+			{
+				WpaGroupMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_FINISH;
+			}
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<----- WpaEAPOLKeyAction\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process Pairwise key 4-way handshaking
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaPairMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem)
+{
+	PHEADER_802_11      pHeader;
+	UCHAR				*mpool, *PTK, *digest;
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	PEAPOL_PACKET       pMsg1;
+	EAPOL_PACKET        Packet;
+	UCHAR               Mic[16];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg1Action ----->\n"));
+
+	// allocate memory pool
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 256);
+
+	if (mpool == NULL)
+		return;
+
+	// PTK Len = 80.
+	PTK = (UCHAR *) ROUND_UP(mpool, 4);
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(PTK + 80, 4);
+
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Process message 1 from authenticator
+	pMsg1 = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	// 1. Save Replay counter, it will use to verify message 3 and construct message 2
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 2. Save ANonce
+	NdisMoveMemory(pAd->StaCfg.ANonce, pMsg1->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE);
+
+	// Generate random SNonce
+	GenRandom(pAd, pAd->CurrentAddress, pAd->StaCfg.SNonce);
+
+	// Calc PTK(ANonce, SNonce)
+	WpaCountPTK(pAd,
+		pAd->StaCfg.PMK,
+		pAd->StaCfg.ANonce,
+		pAd->CommonCfg.Bssid,
+		pAd->StaCfg.SNonce,
+		pAd->CurrentAddress,
+		PTK,
+		LEN_PTK);
+
+	// Save key to PTK entry
+	NdisMoveMemory(pAd->StaCfg.PTK, PTK, LEN_PTK);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero Message 2 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	//
+	// Message 2 as  EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
+	//
+	Packet.KeyDesc.Type = WPA1_KEY_DESC;
+	// 1. Key descriptor version and appropriate RSN IE
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
+	}
+	else	  // TKIP
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
+	}
+
+	// fill in Data Material and its length
+	Packet.KeyDesc.KeyData[0] = IE_WPA;
+	Packet.KeyDesc.KeyData[1] = pAd->StaCfg.RSNIE_Len;
+	Packet.KeyDesc.KeyDataLen[1] = pAd->StaCfg.RSNIE_Len + 2;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[2], pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);
+
+	// Update packet length after decide Key data payload
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + Packet.KeyDesc.KeyDataLen[1];
+
+	// Update Key length
+	Packet.KeyDesc.KeyLength[0] = pMsg1->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pMsg1->KeyDesc.KeyLength[1];
+	// 2. Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// 3. KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	//Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+
+	// 4. Fill SNonce
+	NdisMoveMemory(Packet.KeyDesc.KeyNonce, pAd->StaCfg.SNonce, LEN_KEY_DESC_NONCE);
+
+	// 5. Key Replay Count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Send EAPOL(0, 1, 0, 0, 0, P, 0, SNonce, MIC, RSN_IE)
+	// Out buffer for transmitting message 2
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		os_free_mem(pAd, mpool);
+		return;
+	}
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// 6. Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{	// AES
+
+		HMAC_SHA1(pOutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{	// TKIP
+		hmac_md5(PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+	//hex_dump("MIC", Mic, LEN_KEY_DESC_MIC);
+
+		MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     			&Header802_3,
+						Packet.Body_Len[1] + 4,    &Packet,
+						END_OF_ARGS);
+
+
+	// 5. Copy frame to Tx ring and send Msg 2 to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+	os_free_mem(pAd, (PUCHAR)mpool);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg1Action <-----\n"));
+}
+
+VOID Wpa2PairMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem)
+{
+	PHEADER_802_11      pHeader;
+	UCHAR				*mpool, *PTK, *digest;
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	PEAPOL_PACKET       pMsg1;
+	EAPOL_PACKET        Packet;
+	UCHAR               Mic[16];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg1Action ----->\n"));
+
+	// allocate memory pool
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 256);
+
+	if (mpool == NULL)
+		return;
+
+	// PTK Len = 80.
+	PTK = (UCHAR *) ROUND_UP(mpool, 4);
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(PTK + 80, 4);
+
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Process message 1 from authenticator
+		pMsg1 = (PEAPOL_PACKET)	&Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	// 1. Save Replay counter, it will use to verify message 3 and construct message 2
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 2. Save ANonce
+	NdisMoveMemory(pAd->StaCfg.ANonce, pMsg1->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE);
+
+	// Generate random SNonce
+	GenRandom(pAd, pAd->CurrentAddress, pAd->StaCfg.SNonce);
+
+	if(pMsg1->KeyDesc.KeyDataLen[1] > 0 )
+	{
+		// cached PMKID
+	}
+
+	// Calc PTK(ANonce, SNonce)
+	WpaCountPTK(pAd,
+		pAd->StaCfg.PMK,
+		pAd->StaCfg.ANonce,
+		pAd->CommonCfg.Bssid,
+		pAd->StaCfg.SNonce,
+		pAd->CurrentAddress,
+		PTK,
+		LEN_PTK);
+
+	// Save key to PTK entry
+	NdisMoveMemory(pAd->StaCfg.PTK, PTK, LEN_PTK);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message 2 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	//
+	// Message 2 as  EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
+	//
+	Packet.KeyDesc.Type = WPA2_KEY_DESC;
+
+	// 1. Key descriptor version and appropriate RSN IE
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
+	}
+	else	  // TKIP
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
+	}
+
+	// fill in Data Material and its length
+	Packet.KeyDesc.KeyData[0] = IE_WPA2;
+	Packet.KeyDesc.KeyData[1] = pAd->StaCfg.RSNIE_Len;
+	Packet.KeyDesc.KeyDataLen[1] = pAd->StaCfg.RSNIE_Len + 2;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[2], pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);
+
+	// Update packet length after decide Key data payload
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + Packet.KeyDesc.KeyDataLen[1];
+
+	// 2. Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// 3. KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = 0;
+	Packet.KeyDesc.KeyLength[1] = pMsg1->KeyDesc.KeyLength[1];
+
+	// 4. Fill SNonce
+	NdisMoveMemory(Packet.KeyDesc.KeyNonce, pAd->StaCfg.SNonce, LEN_KEY_DESC_NONCE);
+
+	// 5. Key Replay Count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Send EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
+	// Out buffer for transmitting message 2
+	MlmeAllocateMemory(pAd,  (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		os_free_mem(pAd, mpool);
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,        &FrameLen,
+		Packet.Body_Len[1] + 4, &Packet,
+		END_OF_ARGS);
+
+	// 6. Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1(pOutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+
+	// Make  Transmitting frame
+	MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// 5. Copy frame to Tx ring
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+	os_free_mem(pAd, mpool);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg1Action <-----\n"));
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process Pairwise key 4-way handshaking
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaPairMsg3Action(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+
+{
+	PHEADER_802_11      pHeader;
+	PUCHAR          	pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG           	FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	PEAPOL_PACKET       pMsg3;
+	UCHAR           	Mic[16], OldMic[16];
+	MAC_TABLE_ENTRY 	*pEntry = NULL;
+	UCHAR				skip_offset;
+	KEY_INFO			peerKeyInfo;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg3Action ----->\n"));
+
+	// Record 802.11 header & the received EAPOL packet Msg3
+	pHeader = (PHEADER_802_11) Elem->Msg;
+	pMsg3 = (PEAPOL_PACKET)	&Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pMsg3->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));
+
+
+	// 1. Verify cipher type match
+	if (pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer != 2))
+	{
+		return;
+	}
+	else if(pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
+	{
+		return;
+	}
+
+	// Verify RSN IE
+	//if (!RTMPEqualMemory(pMsg3->KeyDesc.KeyData, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len))
+	if (!CheckRSNIE(pAd, pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1], &skip_offset))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RSN_IE Different in Msg 3 of WPA1 4-way handshake!! \n"));
+		hex_dump("The original RSN_IE", pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len);
+		hex_dump("The received RSN_IE", pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1]);
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, ("RSN_IE VALID in Msg 3 of WPA1 4-way handshake!! \n"));
+
+
+	// 2. Check MIC value
+	// Save the MIC and replace with zero
+	NdisMoveMemory(OldMic, pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	NdisZeroMemory(pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		UCHAR digest[80];
+
+		HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else	// TKIP
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, Mic);
+	}
+
+	if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+
+	// 3. Check Replay Counter, it has to be larger than last one. No need to be exact one larger
+	if(RTMPCompareMemory(pMsg3->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
+		return;
+
+	// Update new replay counter
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 4. Double check ANonce
+	if(!NdisEqualMemory(pAd->StaCfg.ANonce, pMsg3->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE))
+		return;
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero Message 4 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;		// No data field
+
+	//
+	// Message 4 as  EAPOL-Key(0,1,0,0,0,P,0,0,MIC,0)
+	//
+	Packet.KeyDesc.Type = WPA1_KEY_DESC;
+
+	// Key descriptor version and appropriate RSN IE
+	Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = pMsg3->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pMsg3->KeyDesc.KeyLength[1];
+
+	// Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	// In Msg3,  KeyInfo.secure =0 if Group Key HS to come. 1 if no group key HS
+	// Station sends Msg4  KeyInfo.secure should be the same as that in Msg.3
+	Packet.KeyDesc.KeyInfo.Secure= peerKeyInfo.Secure;
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Key Replay count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Out buffer for transmitting message 4
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+		return;
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		UCHAR digest[80];
+
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+	// Update PTK
+	// Prepare pair-wise key information into shared key table
+	NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+	pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+
+	// Decide its ChiperAlg
+	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+	else
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+	// Update these related information to MAC_TABLE_ENTRY
+	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+	NdisMoveMemory(pEntry->PairwiseKey.Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(pEntry->PairwiseKey.RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pEntry->PairwiseKey.TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+	// Update pairwise key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  0,
+						  pAd->SharedKey[BSS0][0].CipherAlg,
+						  pAd->SharedKey[BSS0][0].Key,
+						  pAd->SharedKey[BSS0][0].TxMic,
+						  pAd->SharedKey[BSS0][0].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  0,
+							  pAd->SharedKey[BSS0][0].CipherAlg,
+							  pEntry);
+
+	// Make transmitting frame
+	MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// Copy frame to Tx ring and Send Message 4 to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg3Action <-----\n"));
+}
+
+VOID    Wpa2PairMsg3Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem)
+
+{
+	PHEADER_802_11      pHeader;
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	PEAPOL_PACKET       pMsg3;
+	UCHAR               Mic[16], OldMic[16];
+	UCHAR               *mpool, *KEYDATA, *digest;
+	UCHAR               Key[32];
+	MAC_TABLE_ENTRY 	*pEntry = NULL;
+	KEY_INFO			peerKeyInfo;
+
+	// allocate memory
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 1024);
+
+	if(mpool == NULL)
+		return;
+
+	// KEYDATA Len = 512.
+	KEYDATA = (UCHAR *) ROUND_UP(mpool, 4);
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(KEYDATA + 512, 4);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg3Action ----->\n"));
+
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Process message 3 frame.
+	pMsg3 = (PEAPOL_PACKET)	&Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pMsg3->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));
+
+	// 1. Verify cipher type match
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer!= 2))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else if(pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// 2. Check MIC value
+	// Save the MIC and replace with zero
+	NdisMoveMemory(OldMic, pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	NdisZeroMemory(pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, Mic);
+	}
+
+	if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+
+	// 3. Check Replay Counter, it has to be larger than last one. No need to be exact one larger
+	if(RTMPCompareMemory(pMsg3->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Update new replay counter
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 4. Double check ANonce
+	if(!NdisEqualMemory(pAd->StaCfg.ANonce, pMsg3->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Obtain GTK
+	// 5. Decrypt GTK from Key Data
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("EKD = %d\n", peerKeyInfo.EKD_DL));
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// Decrypt AES GTK
+		AES_GTK_KEY_UNWRAP(&pAd->StaCfg.PTK[16], KEYDATA, pMsg3->KeyDesc.KeyDataLen[1],pMsg3->KeyDesc.KeyData);
+	}
+	else	  // TKIP
+	{
+		INT i;
+		// Decrypt TKIP GTK
+		// Construct 32 bytes RC4 Key
+		NdisMoveMemory(Key, pMsg3->KeyDesc.KeyIv, 16);
+		NdisMoveMemory(&Key[16], &pAd->StaCfg.PTK[16], 16);
+		ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
+		//discard first 256 bytes
+		for(i = 0; i < 256; i++)
+			ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
+		// Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
+		ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1]);
+	}
+
+	if (!ParseKeyData(pAd, KEYDATA, pMsg3->KeyDesc.KeyDataLen[1], 1))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Update GTK to ASIC
+	// Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  pAd->StaCfg.DefaultKeyId,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  pAd->StaCfg.DefaultKeyId,
+							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+							  NULL);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message 4 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	Packet.Body_Len[1]  	= sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;		// No data field
+
+	//
+	// Message 4 as  EAPOL-Key(0,1,0,0,0,P,0,0,MIC,0)
+	//
+	Packet.KeyDesc.Type = WPA2_KEY_DESC;
+
+	// Key descriptor version and appropriate RSN IE
+	Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = pMsg3->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pMsg3->KeyDesc.KeyLength[1];
+
+	// Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+	Packet.KeyDesc.KeyInfo.Secure = 1;
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Key Replay count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Out buffer for transmitting message 4
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+	// Update PTK
+	// Prepare pair-wise key information into shared key table
+	NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+	pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+
+	// Decide its ChiperAlg
+	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+	else
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+	// Update these related information to MAC_TABLE_ENTRY
+	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+	NdisMoveMemory(&pEntry->PairwiseKey.Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(&pEntry->PairwiseKey.RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(&pEntry->PairwiseKey.TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+	// Update pairwise key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  0,
+						  pAd->SharedKey[BSS0][0].CipherAlg,
+						  pAd->SharedKey[BSS0][0].Key,
+						  pAd->SharedKey[BSS0][0].TxMic,
+						  pAd->SharedKey[BSS0][0].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  0,
+							  pAd->SharedKey[BSS0][0].CipherAlg,
+							  pEntry);
+
+	// Make  Transmitting frame
+	MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// Copy frame to Tx ring and Send Message 4 to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	// set 802.1x port control
+	STA_PORT_SECURED(pAd);
+
+    // Indicate Connected for GUI
+    pAd->IndicateMediaState = NdisMediaStateConnected;
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+	os_free_mem(pAd, (PUCHAR)mpool);
+
+
+	// send wireless event - for set key done WPA2
+	if (pAd->CommonCfg.bWirelessEvent)
+		RTMPSendWirelessEvent(pAd, IW_SET_KEY_DONE_WPA2_EVENT_FLAG, pEntry->Addr, BSS0, 0);
+
+	DBGPRINT(RT_DEBUG_ERROR, ("Wpa2PairMsg3Action <-----\n"));
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process Group key 2-way handshaking
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaGroupMsg1Action(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+
+{
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	PEAPOL_PACKET       pGroup;
+	UCHAR               *mpool, *digest, *KEYDATA;
+	UCHAR               Mic[16], OldMic[16];
+	UCHAR               GTK[32], Key[32];
+	KEY_INFO			peerKeyInfo;
+
+	// allocate memory
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 1024);
+
+	if(mpool == NULL)
+		return;
+
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(mpool, 4);
+	// KEYDATA Len = 512.
+	KEYDATA = (UCHAR *) ROUND_UP(digest + 80, 4);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaGroupMsg1Action ----->\n"));
+
+	// Process Group Message 1 frame. skip 802.11 header(24) & LLC_SNAP header(8)
+	pGroup = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pGroup->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));
+
+	// 0. Check cipher type match
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer != 2))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// 1. Verify Replay counter
+	//    Check Replay Counter, it has to be larger than last one. No need to be exact one larger
+	if(RTMPCompareMemory(pGroup->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Update new replay counter
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 2. Verify MIC is valid
+	// Save the MIC and replace with zero
+	NdisMoveMemory(OldMic, pGroup->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	NdisZeroMemory(pGroup->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{	// AES
+		HMAC_SHA1((PUCHAR) pGroup, pGroup->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{	// TKIP
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pGroup, pGroup->Body_Len[1] + 4, Mic);
+	}
+
+	if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in group msg 1 of 2-way handshake!!!!!!!!!! \n"));
+		MlmeFreeMemory(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in group msg 1 of 2-way handshake!!!!!!!!!! \n"));
+
+
+	// 3. Decrypt GTK from Key Data
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// Decrypt AES GTK
+		AES_GTK_KEY_UNWRAP(&pAd->StaCfg.PTK[16], KEYDATA,  pGroup->KeyDesc.KeyDataLen[1], pGroup->KeyDesc.KeyData);
+	}
+	else	// TKIP
+	{
+		INT i;
+
+		// Decrypt TKIP GTK
+		// Construct 32 bytes RC4 Key
+		NdisMoveMemory(Key, pGroup->KeyDesc.KeyIv, 16);
+		NdisMoveMemory(&Key[16], &pAd->StaCfg.PTK[16], 16);
+		ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
+		//discard first 256 bytes
+		for(i = 0; i < 256; i++)
+			ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
+		// Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
+		ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pGroup->KeyDesc.KeyData, pGroup->KeyDesc.KeyDataLen[1]);
+	}
+
+	// Process decrypted key data material
+	// Parse keyData to handle KDE format for WPA2PSK
+	if (peerKeyInfo.EKD_DL)
+	{
+		if (!ParseKeyData(pAd, KEYDATA, pGroup->KeyDesc.KeyDataLen[1], 0))
+		{
+			os_free_mem(pAd, (PUCHAR)mpool);
+			return;
+		}
+	}
+	else	// WPAPSK
+	{
+		// set key material, TxMic and RxMic for WPAPSK
+		NdisMoveMemory(GTK, KEYDATA, 32);
+		NdisMoveMemory(pAd->StaCfg.GTK, GTK, 32);
+		pAd->StaCfg.DefaultKeyId = peerKeyInfo.KeyIndex;
+
+		// Prepare pair-wise key information into shared key table
+		NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+		NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, GTK, LEN_TKIP_EK);
+		NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, &GTK[16], LEN_TKIP_RXMICK);
+		NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, &GTK[24], LEN_TKIP_TXMICK);
+
+		// Update Shared Key CipherAlg
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+		if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+		else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+
+    	//hex_dump("Group Key :", pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, LEN_TKIP_EK);
+	}
+
+	// Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  pAd->StaCfg.DefaultKeyId,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  pAd->StaCfg.DefaultKeyId,
+							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+							  NULL);
+
+	// set 802.1x port control
+	STA_PORT_SECURED(pAd);
+
+    // Indicate Connected for GUI
+    pAd->IndicateMediaState = NdisMediaStateConnected;
+
+	// init header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero Group message 1 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;		// No data field
+
+	//
+	// Group Message 2 as  EAPOL-Key(1,0,0,0,G,0,0,MIC,0)
+	//
+	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+	{
+		Packet.KeyDesc.Type = WPA2_KEY_DESC;
+	}
+	else
+	{
+		Packet.KeyDesc.Type = WPA1_KEY_DESC;
+	}
+
+	// Key descriptor version and appropriate RSN IE
+	Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = pGroup->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pGroup->KeyDesc.KeyLength[1];
+
+	// Key Index as G-Msg 1
+	if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+		Packet.KeyDesc.KeyInfo.KeyIndex = peerKeyInfo.KeyIndex;
+
+	// Key Type Group key
+	Packet.KeyDesc.KeyInfo.KeyType = GROUPKEY;
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	// Secure bit
+	Packet.KeyDesc.KeyInfo.Secure  = 1;
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Key Replay count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Out buffer for transmitting group message 2
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		MlmeFreeMemory(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+
+	MakeOutgoingFrame(pOutBuffer,       		&FrameLen,
+						LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// 5. Copy frame to Tx ring and prepare for encryption
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, FALSE);
+
+	// 6 Free allocated memory
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+	os_free_mem(pAd, (PUCHAR)mpool);
+
+	// send wireless event - for set key done WPA2
+	if (pAd->CommonCfg.bWirelessEvent)
+		RTMPSendWirelessEvent(pAd, IW_SET_KEY_DONE_WPA2_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaGroupMsg1Action <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init WPA MAC header
+
+	Arguments:
+		pAd	Pointer	to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaMacHeaderInit(
+	IN		PRTMP_ADAPTER	pAd,
+	IN OUT	PHEADER_802_11	pHdr80211,
+	IN		UCHAR			wep,
+	IN		PUCHAR		    pAddr1)
+{
+	NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+	pHdr80211->FC.Type	= BTYPE_DATA;
+	pHdr80211->FC.ToDs	= 1;
+	if (wep	== 1)
+		pHdr80211->FC.Wep = 1;
+
+	 //	Addr1: BSSID, Addr2: SA, Addr3:	DA
+	COPY_MAC_ADDR(pHdr80211->Addr1, pAddr1);
+	COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pHdr80211->Addr3, pAd->CommonCfg.Bssid);
+	pHdr80211->Sequence =	pAd->Sequence;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware encryption before really
+	sent out to air.
+
+	Arguments:
+		pAd		Pointer	to our adapter
+		PNDIS_PACKET	Pointer to outgoing Ndis frame
+		NumberOfFrag	Number of fragment required
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID    RTMPToWirelessSta(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pHeader802_3,
+    IN  UINT            HdrLen,
+	IN  PUCHAR          pData,
+    IN  UINT            DataLen,
+    IN	BOOLEAN			is4wayFrame)
+
+{
+	NDIS_STATUS     Status;
+	PNDIS_PACKET    pPacket;
+	UCHAR   Index;
+
+	do
+	{
+		// 1. build a NDIS packet and call RTMPSendPacket();
+		//    be careful about how/when to release this internal allocated NDIS PACKET buffer
+		Status = RTMPAllocateNdisPacket(pAd, &pPacket, pHeader802_3, HdrLen, pData, DataLen);
+		if (Status != NDIS_STATUS_SUCCESS)
+			break;
+
+		if (is4wayFrame)
+			RTMP_SET_PACKET_CLEAR_EAP_FRAME(pPacket, 1);
+		else
+			RTMP_SET_PACKET_CLEAR_EAP_FRAME(pPacket, 0);
+
+		// 2. send out the packet
+		Status = STASendPacket(pAd, pPacket);
+		if(Status == NDIS_STATUS_SUCCESS)
+		{
+			// Dequeue one frame from TxSwQueue0..3 queue and process it
+			// There are three place calling dequeue for TX ring.
+			// 1. Here, right after queueing the frame.
+			// 2. At the end of TxRingTxDone service routine.
+			// 3. Upon NDIS call RTMPSendPackets
+			if((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) &&
+				(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)))
+			{
+				for(Index = 0; Index < 5; Index ++)
+					if(pAd->TxSwQueue[Index].Number > 0)
+						RTMPDeQueuePacket(pAd, FALSE, Index, MAX_TX_PROCESS);
+			}
+		}
+	} while(FALSE);
+
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Check Sanity RSN IE form AP
+
+    Arguments:
+
+    Return Value:
+
+
+    ========================================================================
+*/
+BOOLEAN CheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	OUT	UCHAR			*Offset)
+{
+	PUCHAR              pVIE;
+	UCHAR               len;
+	PEID_STRUCT         pEid;
+	BOOLEAN				result = FALSE;
+
+	pVIE = pData;
+	len	 = DataLen;
+	*Offset = 0;
+
+	while (len > sizeof(RSNIE2))
+	{
+		pEid = (PEID_STRUCT) pVIE;
+		// WPA RSN IE
+		if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4)))
+		{
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) &&
+				(NdisEqualMemory(pVIE, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len)) &&
+				(pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == (pEid->Len + 2)))
+			{
+					DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> WPA/WPAPSK RSN IE matched in Msg 3, Length(%d) \n", (pEid->Len + 2)));
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		// WPA2 RSN IE
+		else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3)))
+		{
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2 || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) &&
+				(NdisEqualMemory(pVIE, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len)) &&
+				(pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == (pEid->Len + 2)))
+			{
+					DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", (pEid->Len + 2)));
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		else
+		{
+			break;
+		}
+
+		pVIE += (pEid->Len + 2);
+		len  -= (pEid->Len + 2);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> skip_offset(%d) \n", *Offset));
+
+	return result;
+
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Parse KEYDATA field.  KEYDATA[] May contain 2 RSN IE and optionally GTK.
+    GTK  is encaptulated in KDE format at  p.83 802.11i D10
+
+    Arguments:
+
+    Return Value:
+
+    Note:
+        802.11i D10
+
+    ========================================================================
+*/
+BOOLEAN ParseKeyData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKeyData,
+	IN  UCHAR           KeyDataLen,
+	IN	UCHAR			bPairewise)
+{
+    PKDE_ENCAP          pKDE = NULL;
+    PUCHAR              pMyKeyData = pKeyData;
+    UCHAR               KeyDataLength = KeyDataLen;
+    UCHAR               GTKLEN;
+	UCHAR				skip_offset;
+
+	// Verify The RSN IE contained in Pairewise-Msg 3 and skip it
+	if (bPairewise)
+    {
+		// Check RSN IE whether it is WPA2/WPA2PSK
+		if (!CheckRSNIE(pAd, pKeyData, KeyDataLen, &skip_offset))
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ParseKeyData ==> WPA2/WPA2PSK RSN IE mismatched \n"));
+			hex_dump("Get KEYDATA :", pKeyData, KeyDataLen);
+			return FALSE;
+    	}
+    	else
+		{
+			// skip RSN IE
+			pMyKeyData += skip_offset;
+			KeyDataLength -= skip_offset;
+
+			//DBGPRINT(RT_DEBUG_TRACE, ("ParseKeyData ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", skip_offset));
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("ParseKeyData ==> KeyDataLength %d without RSN_IE \n", KeyDataLength));
+
+	// Parse EKD format
+	if (KeyDataLength >= 8)
+    {
+        pKDE = (PKDE_ENCAP) pMyKeyData;
+    }
+	else
+    {
+		DBGPRINT(RT_DEBUG_ERROR, ("ERROR: KeyDataLength is too short \n"));
+        return FALSE;
+    }
+
+
+	// Sanity check - shared key index should not be 0
+	if (pKDE->GTKEncap.Kid == 0)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key index zero \n"));
+        return FALSE;
+    }
+
+	// Sanity check - KED length
+	if (KeyDataLength < (pKDE->Len + 2))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("ERROR: The len from KDE is too short \n"));
+        return FALSE;
+    }
+
+	// Get GTK length - refer to IEEE 802.11i-2004 p.82
+	GTKLEN = pKDE->Len -6;
+
+	if (GTKLEN < LEN_AES_KEY)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key length is too short (%d) \n", GTKLEN));
+        return FALSE;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, ("GTK Key with KDE formet got index=%d, len=%d \n", pKDE->GTKEncap.Kid, GTKLEN));
+
+	// Update GTK
+	// set key material, TxMic and RxMic for WPAPSK
+	NdisMoveMemory(pAd->StaCfg.GTK, pKDE->GTKEncap.GTK, 32);
+	pAd->StaCfg.DefaultKeyId = pKDE->GTKEncap.Kid;
+
+	// Update shared key table
+	NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, pKDE->GTKEncap.GTK, LEN_TKIP_EK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, &pKDE->GTKEncap.GTK[16], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, &pKDE->GTKEncap.GTK[24], LEN_TKIP_TXMICK);
+
+	// Update Shared Key CipherAlg
+	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+	if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+	else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+
+	return TRUE;
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Cisco CCKM PRF function
+
+	Arguments:
+		key				Cisco Base Transient Key (BTK)
+		key_len			The key length of the BTK
+		data			Ruquest Number(RN) + BSSID
+		data_len		The length of the data
+		output			Store for PTK(Pairwise transient keys)
+		len				The length of the output
+	Return Value:
+		None
+
+	Note:
+		802.1i	Annex F.9
+
+	========================================================================
+*/
+VOID CCKMPRF(
+	IN	UCHAR	*key,
+	IN	INT		key_len,
+	IN	UCHAR	*data,
+	IN	INT		data_len,
+	OUT	UCHAR	*output,
+	IN	INT		len)
+{
+	INT		i;
+	UCHAR	input[1024];
+	INT		currentindex = 0;
+	INT		total_len;
+
+	NdisMoveMemory(input, data, data_len);
+	total_len = data_len;
+	input[total_len] = 0;
+	total_len++;
+	for	(i = 0;	i <	(len + 19) / 20; i++)
+	{
+		HMAC_SHA1(input, total_len,	key, key_len, &output[currentindex]);
+		currentindex +=	20;
+		input[total_len - 1]++;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process MIC error indication and record MIC error timer.
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pWpaKey 		Pointer to the WPA key structure
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPReportMicError(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCIPHER_KEY 	pWpaKey)
+{
+	ULONG	Now;
+    UCHAR   unicastKey = (pWpaKey->Type == PAIRWISE_KEY ? 1:0);
+
+	// Record Last MIC error time and count
+	Now = jiffies;
+	if (pAd->StaCfg.MicErrCnt == 0)
+	{
+		pAd->StaCfg.MicErrCnt++;
+		pAd->StaCfg.LastMicErrorTime = Now;
+        NdisZeroMemory(pAd->StaCfg.ReplayCounter, 8);
+	}
+	else if (pAd->StaCfg.MicErrCnt == 1)
+	{
+		if ((pAd->StaCfg.LastMicErrorTime + (60 * OS_HZ)) < Now)
+		{
+			// Update Last MIC error time, this did not violate two MIC errors within 60 seconds
+			pAd->StaCfg.LastMicErrorTime = Now;
+		}
+		else
+		{
+
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_COUNTER_MEASURES_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+			pAd->StaCfg.LastMicErrorTime = Now;
+			// Violate MIC error counts, MIC countermeasures kicks in
+			pAd->StaCfg.MicErrCnt++;
+		}
+	}
+	else
+	{
+		// MIC error count >= 2
+		// This should not happen
+		;
+	}
+    MlmeEnqueue(pAd,
+				MLME_CNTL_STATE_MACHINE,
+				OID_802_11_MIC_FAILURE_REPORT_FRAME,
+				1,
+				&unicastKey);
+
+    if (pAd->StaCfg.MicErrCnt == 2)
+    {
+        RTMPSetTimer(&pAd->StaCfg.WpaDisassocAndBlockAssocTimer, 100);
+    }
+}
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#define	LENGTH_EAP_H    4
+// If the received frame is EAP-Packet ,find out its EAP-Code (Request(0x01), Response(0x02), Success(0x03), Failure(0x04)).
+INT	    WpaCheckEapCode(
+	IN  PRTMP_ADAPTER   		pAd,
+	IN  PUCHAR				pFrame,
+	IN  USHORT				FrameLen,
+	IN  USHORT				OffSet)
+{
+
+	PUCHAR	pData;
+	INT	result = 0;
+
+	if( FrameLen < OffSet + LENGTH_EAPOL_H + LENGTH_EAP_H )
+		return result;
+
+	pData = pFrame + OffSet; // skip offset bytes
+
+	if(*(pData+1) == EAPPacket) 	// 802.1x header - Packet Type
+	{
+		 result = *(pData+4);		// EAP header - Code
+	}
+
+	return result;
+}
+
+VOID    WpaSendMicFailureToWpaSupplicant(
+    IN  PRTMP_ADAPTER    pAd,
+    IN  BOOLEAN          bUnicast)
+{
+    union iwreq_data    wrqu;
+    char custom[IW_CUSTOM_MAX] = {0};
+
+    sprintf(custom, "MLME-MICHAELMICFAILURE.indication");
+    if (bUnicast)
+        sprintf(custom, "%s unicast", custom);
+    wrqu.data.length = strlen(custom);
+    wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, custom);
+
+    return;
+}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+VOID	WpaMicFailureReportFrame(
+	IN  PRTMP_ADAPTER   pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	UCHAR               Mic[16];
+    BOOLEAN             bUnicast;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaMicFailureReportFrame ----->\n"));
+
+    bUnicast = (Elem->Msg[0] == 1 ? TRUE:FALSE);
+	pAd->Sequence = ((pAd->Sequence) + 1) & (MAX_SEQ_NUMBER);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+
+	Packet.KeyDesc.Type = WPA1_KEY_DESC;
+
+    // Request field presented
+    Packet.KeyDesc.KeyInfo.Request = 1;
+
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
+	}
+	else	  // TKIP
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
+	}
+
+    Packet.KeyDesc.KeyInfo.KeyType = (bUnicast ? PAIRWISEKEY : GROUPKEY);
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+    // Error field presented
+	Packet.KeyDesc.KeyInfo.Error  = 1;
+
+	// Update packet length after decide Key data payload
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;
+
+	// Key Replay Count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
+    inc_byte_array(pAd->StaCfg.ReplayCounter, 8);
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+		              Packet.Body_Len[1] + 4,   &Packet,
+		              END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{	// AES
+        UCHAR digest[20] = {0};
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{	// TKIP
+		hmac_md5(pAd->StaCfg.PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+    MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+    	  			LENGTH_802_3,     			&Header802_3,
+    				Packet.Body_Len[1] + 4,     &Packet,
+    				END_OF_ARGS);
+
+	// opy frame to Tx ring and send MIC failure report frame to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, FALSE);
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaMicFailureReportFrame <-----\n"));
+}
+
+/** from wpa_supplicant
+ * inc_byte_array - Increment arbitrary length byte array by one
+ * @counter: Pointer to byte array
+ * @len: Length of the counter in bytes
+ *
+ * This function increments the last byte of the counter by one and continues
+ * rolling over to more significant bytes if the byte was incremented from
+ * 0xff to 0x00.
+ */
+void inc_byte_array(UCHAR *counter, int len)
+{
+	int pos = len - 1;
+	while (pos >= 0) {
+		counter[pos]++;
+		if (counter[pos] != 0)
+			break;
+		pos--;
+	}
+}
+
+VOID WpaDisassocApAndBlockAssoc(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+    RTMP_ADAPTER                *pAd = (PRTMP_ADAPTER)FunctionContext;
+    MLME_DISASSOC_REQ_STRUCT    DisassocReq;
+
+	// disassoc from current AP first
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPReportMicError - disassociate with current AP after sending second continuous EAPOL frame\n"));
+	DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_MIC_FAILURE);
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+	pAd->StaCfg.bBlockAssoc = TRUE;
+}
+
diff --git a/drivers/staging/rt2860/sta_ioctl.c b/drivers/staging/rt2860/sta_ioctl.c
new file mode 100644
index 000000000000..3ea2b2c4ab0e
--- /dev/null
+++ b/drivers/staging/rt2860/sta_ioctl.c
@@ -0,0 +1,6944 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    sta_ioctl.c
+
+    Abstract:
+    IOCTL related subroutines
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Rory Chen   01-03-2003    created
+	Rory Chen   02-14-2005    modify to support RT61
+*/
+
+#include	"rt_config.h"
+
+#ifdef DBG
+extern ULONG    RTDebugLevel;
+#endif
+
+#define NR_WEP_KEYS 				4
+#define WEP_SMALL_KEY_LEN 			(40/8)
+#define WEP_LARGE_KEY_LEN 			(104/8)
+
+#define GROUP_KEY_NO                4
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_A, _B, _C, _D, _E, _F)
+#else
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_B, _C, _D, _E, _F)
+#endif
+
+extern UCHAR    CipherWpa2Template[];
+extern UCHAR    CipherWpaPskTkip[];
+extern UCHAR    CipherWpaPskTkipLen;
+
+typedef struct PACKED _RT_VERSION_INFO{
+    UCHAR       DriverVersionW;
+    UCHAR       DriverVersionX;
+    UCHAR       DriverVersionY;
+    UCHAR       DriverVersionZ;
+    UINT        DriverBuildYear;
+    UINT        DriverBuildMonth;
+    UINT        DriverBuildDay;
+} RT_VERSION_INFO, *PRT_VERSION_INFO;
+
+struct iw_priv_args privtab[] = {
+{ RTPRIV_IOCTL_SET,
+  IW_PRIV_TYPE_CHAR | 1024, 0,
+  "set"},
+
+{ RTPRIV_IOCTL_SHOW, 0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+{ RTPRIV_IOCTL_SHOW, IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+/* --- sub-ioctls definitions --- */
+    { SHOW_CONN_STATUS,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "connStatus" },
+	{ SHOW_DRVIER_VERION,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "driverVer" },
+    { SHOW_BA_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "bainfo" },
+	{ SHOW_DESC_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "descinfo" },
+    { RAIO_OFF,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_off" },
+	{ RAIO_ON,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_on" },
+#ifdef QOS_DLS_SUPPORT
+	{ SHOW_DLS_ENTRY_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "dlsentryinfo" },
+#endif // QOS_DLS_SUPPORT //
+	{ SHOW_CFG_VALUE,
+	  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "show" },
+/* --- sub-ioctls relations --- */
+
+#ifdef DBG
+{ RTPRIV_IOCTL_BBP,
+  IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "bbp"},
+{ RTPRIV_IOCTL_MAC,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "mac"},
+{ RTPRIV_IOCTL_E2P,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "e2p"},
+#endif  /* DBG */
+
+{ RTPRIV_IOCTL_STATISTICS,
+  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "stat"},
+{ RTPRIV_IOCTL_GSITESURVEY,
+  0, IW_PRIV_TYPE_CHAR | 1024,
+  "get_site_survey"},
+};
+
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WMM_SUPPORT
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif
+
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+VOID RTMPIoctlBBP(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlE2PROM(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  struct iwreq    *wrq);
+#endif // DBG //
+
+
+NDIS_STATUS RTMPWPANoneAddKeyProc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN	PVOID			pBuf);
+
+INT Set_FragTest_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef DOT11_N_SUPPORT
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // DOT11_N_SUPPORT //
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // CARRIER_DETECTION_SUPPORT //
+
+static struct {
+	CHAR *name;
+	INT (*set_proc)(PRTMP_ADAPTER pAdapter, PUCHAR arg);
+} *PRTMP_PRIVATE_SET_PROC, RTMP_PRIVATE_SUPPORT_PROC[] = {
+	{"DriverVersion",				Set_DriverVersion_Proc},
+	{"CountryRegion",				Set_CountryRegion_Proc},
+	{"CountryRegionABand",			Set_CountryRegionABand_Proc},
+	{"SSID",						Set_SSID_Proc},
+	{"WirelessMode",				Set_WirelessMode_Proc},
+	{"TxBurst",					Set_TxBurst_Proc},
+	{"TxPreamble",				Set_TxPreamble_Proc},
+	{"TxPower",					Set_TxPower_Proc},
+	{"Channel",					Set_Channel_Proc},
+	{"BGProtection",				Set_BGProtection_Proc},
+	{"RTSThreshold",				Set_RTSThreshold_Proc},
+	{"FragThreshold",				Set_FragThreshold_Proc},
+#ifdef DOT11_N_SUPPORT
+	{"HtBw",		                Set_HtBw_Proc},
+	{"HtMcs",		                Set_HtMcs_Proc},
+	{"HtGi",		                Set_HtGi_Proc},
+	{"HtOpMode",		            Set_HtOpMode_Proc},
+	{"HtExtcha",		            Set_HtExtcha_Proc},
+	{"HtMpduDensity",		        Set_HtMpduDensity_Proc},
+	{"HtBaWinSize",		        	Set_HtBaWinSize_Proc},
+	{"HtRdg",		        		Set_HtRdg_Proc},
+	{"HtAmsdu",		        		Set_HtAmsdu_Proc},
+	{"HtAutoBa",		        	Set_HtAutoBa_Proc},
+	{"HtBaDecline",					Set_BADecline_Proc},
+	{"HtProtect",		        	Set_HtProtect_Proc},
+	{"HtMimoPs",		        	Set_HtMimoPs_Proc},
+#endif // DOT11_N_SUPPORT //
+
+#ifdef AGGREGATION_SUPPORT
+	{"PktAggregate",				Set_PktAggregate_Proc},
+#endif
+
+#ifdef WMM_SUPPORT
+	{"WmmCapable",					Set_WmmCapable_Proc},
+#endif
+	{"IEEE80211H",					Set_IEEE80211H_Proc},
+    {"NetworkType",                 Set_NetworkType_Proc},
+	{"AuthMode",					Set_AuthMode_Proc},
+	{"EncrypType",					Set_EncrypType_Proc},
+	{"DefaultKeyID",				Set_DefaultKeyID_Proc},
+	{"Key1",						Set_Key1_Proc},
+	{"Key2",						Set_Key2_Proc},
+	{"Key3",						Set_Key3_Proc},
+	{"Key4",						Set_Key4_Proc},
+	{"WPAPSK",						Set_WPAPSK_Proc},
+	{"ResetCounter",				Set_ResetStatCounter_Proc},
+	{"PSMode",                      Set_PSMode_Proc},
+#ifdef DBG
+	{"Debug",						Set_Debug_Proc},
+#endif
+
+#ifdef RALINK_ATE
+	{"ATE",							Set_ATE_Proc},
+	{"ATEDA",						Set_ATE_DA_Proc},
+	{"ATESA",						Set_ATE_SA_Proc},
+	{"ATEBSSID",					Set_ATE_BSSID_Proc},
+	{"ATECHANNEL",					Set_ATE_CHANNEL_Proc},
+	{"ATETXPOW0",					Set_ATE_TX_POWER0_Proc},
+	{"ATETXPOW1",					Set_ATE_TX_POWER1_Proc},
+	{"ATETXANT",					Set_ATE_TX_Antenna_Proc},
+	{"ATERXANT",					Set_ATE_RX_Antenna_Proc},
+	{"ATETXFREQOFFSET",				Set_ATE_TX_FREQOFFSET_Proc},
+	{"ATETXBW",						Set_ATE_TX_BW_Proc},
+	{"ATETXLEN",					Set_ATE_TX_LENGTH_Proc},
+	{"ATETXCNT",					Set_ATE_TX_COUNT_Proc},
+	{"ATETXMCS",					Set_ATE_TX_MCS_Proc},
+	{"ATETXMODE",					Set_ATE_TX_MODE_Proc},
+	{"ATETXGI",						Set_ATE_TX_GI_Proc},
+	{"ATERXFER",					Set_ATE_RX_FER_Proc},
+	{"ATERRF",						Set_ATE_Read_RF_Proc},
+	{"ATEWRF1",						Set_ATE_Write_RF1_Proc},
+	{"ATEWRF2",						Set_ATE_Write_RF2_Proc},
+	{"ATEWRF3",						Set_ATE_Write_RF3_Proc},
+	{"ATEWRF4",						Set_ATE_Write_RF4_Proc},
+	{"ATELDE2P",				    Set_ATE_Load_E2P_Proc},
+	{"ATERE2P",						Set_ATE_Read_E2P_Proc},
+	{"ATESHOW",						Set_ATE_Show_Proc},
+	{"ATEHELP",						Set_ATE_Help_Proc},
+
+#ifdef RALINK_28xx_QA
+	{"TxStop",						Set_TxStop_Proc},
+	{"RxStop",						Set_RxStop_Proc},
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    {"WpaSupport",                  Set_Wpa_Support},
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+	{"FixedTxMode",                 Set_FixedTxMode_Proc},
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	{"OpMode",						Set_OpMode_Proc},
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+#ifdef DOT11_N_SUPPORT
+    {"TGnWifiTest",                 Set_TGnWifiTest_Proc},
+    {"ForceGF",		        		Set_ForceGF_Proc},
+#endif // DOT11_N_SUPPORT //
+#ifdef QOS_DLS_SUPPORT
+	{"DlsAddEntry",					Set_DlsAddEntry_Proc},
+	{"DlsTearDownEntry",			Set_DlsTearDownEntry_Proc},
+#endif // QOS_DLS_SUPPORT //
+	{"LongRetry",	        		Set_LongRetryLimit_Proc},
+	{"ShortRetry",	        		Set_ShortRetryLimit_Proc},
+#ifdef EXT_BUILD_CHANNEL_LIST
+	{"11dClientMode",				Set_Ieee80211dClientMode_Proc},
+#endif // EXT_BUILD_CHANNEL_LIST //
+#ifdef CARRIER_DETECTION_SUPPORT
+	{"CarrierDetect",				Set_CarrierDetect_Proc},
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	{NULL,}
+};
+
+
+VOID RTMPAddKey(
+	IN	PRTMP_ADAPTER	    pAd,
+	IN	PNDIS_802_11_KEY    pKey)
+{
+	ULONG				KeyIdx;
+	MAC_TABLE_ENTRY  	*pEntry;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey ------>\n"));
+
+	if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+	{
+		if (pKey->KeyIndex & 0x80000000)
+		{
+		    if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+            {
+                NdisZeroMemory(pAd->StaCfg.PMK, 32);
+                NdisMoveMemory(pAd->StaCfg.PMK, pKey->KeyMaterial, pKey->KeyLength);
+                goto end;
+            }
+		    // Update PTK
+		    NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Decide its ChiperAlg
+        	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+        	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+        	else
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+            // Update these related information to MAC_TABLE_ENTRY
+        	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+            NdisMoveMemory(pEntry->PairwiseKey.Key, pAd->SharedKey[BSS0][0].Key, LEN_TKIP_EK);
+        	NdisMoveMemory(pEntry->PairwiseKey.RxMic, pAd->SharedKey[BSS0][0].RxMic, LEN_TKIP_RXMICK);
+        	NdisMoveMemory(pEntry->PairwiseKey.TxMic, pAd->SharedKey[BSS0][0].TxMic, LEN_TKIP_TXMICK);
+        	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+        	// Update pairwise key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  0,
+        						  pAd->SharedKey[BSS0][0].CipherAlg,
+        						  pAd->SharedKey[BSS0][0].Key,
+        						  pAd->SharedKey[BSS0][0].TxMic,
+        						  pAd->SharedKey[BSS0][0].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  0,
+        							  pAd->SharedKey[BSS0][0].CipherAlg,
+        							  pEntry);
+
+            if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+            {
+                // set 802.1x port control
+				STA_PORT_SECURED(pAd);
+
+                // Indicate Connected for GUI
+                pAd->IndicateMediaState = NdisMediaStateConnected;
+            }
+		}
+        else
+        {
+            // Update GTK
+            pAd->StaCfg.DefaultKeyId = (pKey->KeyIndex & 0xFF);
+            NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Update Shared Key CipherAlg
+    		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+    		if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+    		else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+
+            // Update group key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  pAd->StaCfg.DefaultKeyId,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  pAd->StaCfg.DefaultKeyId,
+        							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        							  NULL);
+
+            // set 802.1x port control
+			STA_PORT_SECURED(pAd);
+
+            // Indicate Connected for GUI
+            pAd->IndicateMediaState = NdisMediaStateConnected;
+        }
+	}
+	else	// dynamic WEP from wpa_supplicant
+	{
+		UCHAR	CipherAlg;
+    	PUCHAR	Key;
+
+		if(pKey->KeyLength == 32)
+			goto end;
+
+		KeyIdx = pKey->KeyIndex & 0x0fffffff;
+
+		if (KeyIdx < 4)
+		{
+			// it is a default shared key, for Pairwise key setting
+			if (pKey->KeyIndex & 0x80000000)
+			{
+				pEntry = MacTableLookup(pAd, pKey->BSSID);
+
+				if (pEntry)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey: Set Pair-wise Key\n"));
+
+					// set key material and key length
+ 					pEntry->PairwiseKey.KeyLen = (UCHAR)pKey->KeyLength;
+					NdisMoveMemory(pEntry->PairwiseKey.Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+					// set Cipher type
+					if (pKey->KeyLength == 5)
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP64;
+					else
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP128;
+
+					// Add Pair-wise key to Asic
+					AsicAddPairwiseKeyEntry(
+						pAd,
+						pEntry->Addr,
+						(UCHAR)pEntry->Aid,
+                		&pEntry->PairwiseKey);
+
+					// update WCID attribute table and IVEIV table for this entry
+					RTMPAddWcidAttributeEntry(
+						pAd,
+						BSS0,
+						KeyIdx, // The value may be not zero
+						pEntry->PairwiseKey.CipherAlg,
+						pEntry);
+
+				}
+			}
+			else
+            {
+				// Default key for tx (shared key)
+				pAd->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+
+				// set key material and key length
+				pAd->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(pAd->SharedKey[BSS0][KeyIdx].Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+				// Set Ciper type
+				if (pKey->KeyLength == 5)
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP64;
+				else
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP128;
+
+    			CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][KeyIdx].Key;
+
+				// Set Group key material to Asic
+    			AsicAddSharedKeyEntry(pAd, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+				// Update WCID attribute table and IVEIV table for this group key table
+				RTMPAddWcidAttributeEntry(pAd, BSS0, KeyIdx, CipherAlg, NULL);
+
+			}
+		}
+	}
+end:
+	return;
+}
+
+char * rtstrchr(const char * s, int c)
+{
+    for(; *s != (char) c; ++s)
+        if (*s == '\0')
+            return NULL;
+    return (char *) s;
+}
+
+/*
+This is required for LinEX2004/kernel2.6.7 to provide iwlist scanning function
+*/
+
+int
+rt_ioctl_giwname(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+//	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+#ifdef RT2860
+    strncpy(name, "RT2860 Wireless", IFNAMSIZ);
+#endif // RT2860 //
+	return 0;
+}
+
+int rt_ioctl_siwfreq(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_freq *freq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	int 	chan = -1;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+
+	if (freq->e > 1)
+		return -EINVAL;
+
+	if((freq->e == 0) && (freq->m <= 1000))
+		chan = freq->m;	// Setting by channel number
+	else
+		MAP_KHZ_TO_CHANNEL_ID( (freq->m /100) , chan); // Setting by frequency - search the table , like 2.412G, 2.422G,
+
+    if (ChannelSanity(pAdapter, chan) == TRUE)
+    {
+	pAdapter->CommonCfg.Channel = chan;
+	DBGPRINT(RT_DEBUG_ERROR, ("==>rt_ioctl_siwfreq::SIOCSIWFREQ[cmd=0x%x] (Channel=%d)\n", SIOCSIWFREQ, pAdapter->CommonCfg.Channel));
+    }
+    else
+        return -EINVAL;
+
+	return 0;
+}
+int rt_ioctl_giwfreq(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_freq *freq, char *extra)
+{
+    VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter = NULL;
+	UCHAR ch;
+	ULONG	m;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+		ch = pAdapter->CommonCfg.Channel;
+
+	DBGPRINT(RT_DEBUG_TRACE,("==>rt_ioctl_giwfreq  %d\n", ch));
+
+    MAP_CHANNEL_ID_TO_KHZ(ch, m);
+	freq->m = m * 100;
+	freq->e = 1;
+	return 0;
+}
+
+int rt_ioctl_siwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+    	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	switch (*mode)
+	{
+		case IW_MODE_ADHOC:
+			Set_NetworkType_Proc(pAdapter, "Adhoc");
+			break;
+		case IW_MODE_INFRA:
+			Set_NetworkType_Proc(pAdapter, "Infra");
+			break;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+        case IW_MODE_MONITOR:
+			Set_NetworkType_Proc(pAdapter, "Monitor");
+			break;
+#endif
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_siwmode::SIOCSIWMODE (unknown %d)\n", *mode));
+			return -EINVAL;
+	}
+
+	// Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+	pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+	return 0;
+}
+
+int rt_ioctl_giwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (ADHOC_ON(pAdapter))
+		*mode = IW_MODE_ADHOC;
+    else if (INFRA_ON(pAdapter))
+		*mode = IW_MODE_INFRA;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+    else if (MONITOR_ON(pAdapter))
+    {
+        *mode = IW_MODE_MONITOR;
+    }
+#endif
+    else
+        *mode = IW_MODE_AUTO;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==>rt_ioctl_giwmode(mode=%d)\n", *mode));
+	return 0;
+}
+
+int rt_ioctl_siwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+        	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	return 0;
+}
+
+int rt_ioctl_giwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	return 0;
+}
+
+int rt_ioctl_giwrange(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	struct iw_range *range = (struct iw_range *) extra;
+	u16 val;
+	int i;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===>rt_ioctl_giwrange\n"));
+	data->length = sizeof(struct iw_range);
+	memset(range, 0, sizeof(struct iw_range));
+
+	range->txpower_capa = IW_TXPOW_DBM;
+
+	if (INFRA_ON(pAdapter)||ADHOC_ON(pAdapter))
+	{
+		range->min_pmp = 1 * 1024;
+		range->max_pmp = 65535 * 1024;
+		range->min_pmt = 1 * 1024;
+		range->max_pmt = 1000 * 1024;
+		range->pmp_flags = IW_POWER_PERIOD;
+		range->pmt_flags = IW_POWER_TIMEOUT;
+		range->pm_capa = IW_POWER_PERIOD | IW_POWER_TIMEOUT |
+			IW_POWER_UNICAST_R | IW_POWER_ALL_R;
+	}
+
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 14;
+
+	range->retry_capa = IW_RETRY_LIMIT;
+	range->retry_flags = IW_RETRY_LIMIT;
+	range->min_retry = 0;
+	range->max_retry = 255;
+
+	range->num_channels =  pAdapter->ChannelListNum;
+
+	val = 0;
+	for (i = 1; i <= range->num_channels; i++)
+	{
+		u32 m;
+		range->freq[val].i = pAdapter->ChannelList[i-1].Channel;
+		MAP_CHANNEL_ID_TO_KHZ(pAdapter->ChannelList[i-1].Channel, m);
+		range->freq[val].m = m * 100; /* HZ */
+
+		range->freq[val].e = 1;
+		val++;
+		if (val == IW_MAX_FREQUENCIES)
+			break;
+	}
+	range->num_frequency = val;
+
+	range->max_qual.qual = 100; /* what is correct max? This was not
+					* documented exactly. At least
+					* 69 has been observed. */
+	range->max_qual.level = 0; /* dB */
+	range->max_qual.noise = 0; /* dB */
+
+	/* What would be suitable values for "average/typical" qual? */
+	range->avg_qual.qual = 20;
+	range->avg_qual.level = -60;
+	range->avg_qual.noise = -95;
+	range->sensitivity = 3;
+
+	range->max_encoding_tokens = NR_WEP_KEYS;
+	range->num_encoding_sizes = 2;
+	range->encoding_size[0] = 5;
+	range->encoding_size[1] = 13;
+
+	range->min_rts = 0;
+	range->max_rts = 2347;
+	range->min_frag = 256;
+	range->max_frag = 2346;
+
+#if WIRELESS_EXT > 17
+	/* IW_ENC_CAPA_* bit field */
+	range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
+					IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+#endif
+
+	return 0;
+}
+
+int rt_ioctl_siwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+    NDIS_802_11_MAC_ADDRESS Bssid;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+    {
+        RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+    }
+
+    // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+    // this request, because this request is initiated by NDIS.
+    pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+	// Prevent to connect AP again in STAMlmePeriodicExec
+	pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+    memset(Bssid, 0, MAC_ADDR_LEN);
+    memcpy(Bssid, ap_addr->sa_data, MAC_ADDR_LEN);
+    MlmeEnqueue(pAdapter,
+                MLME_CNTL_STATE_MACHINE,
+                OID_802_11_BSSID,
+                sizeof(NDIS_802_11_MAC_ADDRESS),
+                (VOID *)&Bssid);
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCSIWAP %02x:%02x:%02x:%02x:%02x:%02x\n",
+        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+
+	return 0;
+}
+
+int rt_ioctl_giwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+	{
+		ap_addr->sa_family = ARPHRD_ETHER;
+		memcpy(ap_addr->sa_data, &pAdapter->CommonCfg.Bssid, ETH_ALEN);
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    // Add for RT2870
+    else if (pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+    {
+        ap_addr->sa_family = ARPHRD_ETHER;
+        memcpy(ap_addr->sa_data, &pAdapter->MlmeAux.Bssid, ETH_ALEN);
+    }
+#endif // WPA_SUPPLICANT_SUPPORT //
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIWAP(=EMPTY)\n"));
+		return -ENOTCONN;
+	}
+
+	return 0;
+}
+
+/*
+ * Units are in db above the noise floor. That means the
+ * rssi values reported in the tx/rx descriptors in the
+ * driver are the SNR expressed in db.
+ *
+ * If you assume that the noise floor is -95, which is an
+ * excellent assumption 99.5 % of the time, then you can
+ * derive the absolute signal level (i.e. -95 + rssi).
+ * There are some other slight factors to take into account
+ * depending on whether the rssi measurement is from 11b,
+ * 11g, or 11a.   These differences are at most 2db and
+ * can be documented.
+ *
+ * NB: various calculations are based on the orinoco/wavelan
+ *     drivers for compatibility
+ */
+static void set_quality(PRTMP_ADAPTER pAdapter,
+                        struct iw_quality *iq,
+                        signed char rssi)
+{
+	__u8 ChannelQuality;
+
+	// Normalize Rssi
+	if (rssi >= -50)
+		ChannelQuality = 100;
+	else if (rssi >= -80) // between -50 ~ -80dbm
+		ChannelQuality = (__u8)(24 + ((rssi + 80) * 26)/10);
+	else if (rssi >= -90)   // between -80 ~ -90dbm
+        ChannelQuality = (__u8)((rssi + 90) * 26)/10;
+	else
+		ChannelQuality = 0;
+
+    iq->qual = (__u8)ChannelQuality;
+
+    iq->level = (__u8)(rssi);
+    iq->noise = (pAdapter->BbpWriteLatch[66] > pAdapter->BbpTuning.FalseCcaUpperThreshold) ? ((__u8)pAdapter->BbpTuning.FalseCcaUpperThreshold) : ((__u8) pAdapter->BbpWriteLatch[66]); 	// noise level (dBm)
+    iq->noise += 256 - 143;
+    iq->updated = pAdapter->iw_stats.qual.updated;
+}
+
+int rt_ioctl_iwaplist(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+ 	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	struct sockaddr addr[IW_MAX_AP];
+	struct iw_quality qual[IW_MAX_AP];
+	int i;
+
+   	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		data->length = 0;
+		return 0;
+        //return -ENETDOWN;
+	}
+
+	for (i = 0; i <IW_MAX_AP ; i++)
+	{
+		if (i >=  pAdapter->ScanTab.BssNr)
+			break;
+		addr[i].sa_family = ARPHRD_ETHER;
+			memcpy(addr[i].sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+		set_quality(pAdapter, &qual[i], pAdapter->ScanTab.BssEntry[i].Rssi);
+	}
+	data->length = i;
+	memcpy(extra, &addr, i*sizeof(addr[0]));
+	data->flags = 1;		/* signal quality present (sort of) */
+	memcpy(extra + i*sizeof(addr[0]), &qual, i*sizeof(qual[i]));
+
+	return 0;
+}
+
+#ifdef SIOCGIWSCAN
+int rt_ioctl_siwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	ULONG								Now;
+	int Status = NDIS_STATUS_SUCCESS;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		return -ENETDOWN;
+	}
+
+	if (MONITOR_ON(pAdapter))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+        return -EINVAL;
+    }
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount++;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+    pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		return 0;
+	do{
+		Now = jiffies;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+		if ((pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE) &&
+			(pAdapter->StaCfg.WpaSupplicantScanCount > 3))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! WpaSupplicantScanCount > 3\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+		if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+			((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+			(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)) &&
+			(pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+
+		if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+		{
+			RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+		}
+
+		// tell CNTL state machine to call NdisMSetInformationComplete() after completing
+		// this request, because this request is initiated by NDIS.
+		pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+		// Reset allowed scan retries
+		pAdapter->StaCfg.ScanCnt = 0;
+		pAdapter->StaCfg.LastScanTime = Now;
+
+		MlmeEnqueue(pAdapter,
+			MLME_CNTL_STATE_MACHINE,
+			OID_802_11_BSSID_LIST_SCAN,
+			0,
+			NULL);
+
+		Status = NDIS_STATUS_SUCCESS;
+		RT28XX_MLME_HANDLER(pAdapter);
+	}while(0);
+	return 0;
+}
+
+int rt_ioctl_giwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	int i=0;
+	char *current_ev = extra, *previous_ev = extra;
+	char *end_buf;
+	char *current_val, custom[MAX_CUSTOM_LEN] = {0};
+#ifndef IWEVGENIE
+	char idx;
+#endif // IWEVGENIE //
+	struct iw_event iwe;
+
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+    {
+		/*
+		 * Still scanning, indicate the caller should try again.
+		 */
+		return -EAGAIN;
+	}
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount = 0;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	if (pAdapter->ScanTab.BssNr == 0)
+	{
+		data->length = 0;
+		return 0;
+	}
+
+#if WIRELESS_EXT >= 17
+    if (data->length > 0)
+        end_buf = extra + data->length;
+    else
+        end_buf = extra + IW_SCAN_MAX_DATA;
+#else
+    end_buf = extra + IW_SCAN_MAX_DATA;
+#endif
+
+	for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+	{
+		if (current_ev >= end_buf)
+        {
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+        }
+
+		//MAC address
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWAP;
+		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+		memcpy(iwe.u.ap_addr.sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, ETH_ALEN);
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_ADDR_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//ESSID
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWESSID;
+		iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].SsidLen;
+		iwe.u.data.flags = 1;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_POINT(info, current_ev,end_buf, &iwe, pAdapter->ScanTab.BssEntry[i].Ssid);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Network Type
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWMODE;
+		if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11IBSS)
+		{
+			iwe.u.mode = IW_MODE_ADHOC;
+		}
+		else if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11Infrastructure)
+		{
+			iwe.u.mode = IW_MODE_INFRA;
+		}
+		else
+		{
+			iwe.u.mode = IW_MODE_AUTO;
+		}
+		iwe.len = IW_EV_UINT_LEN;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe,  IW_EV_UINT_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Channel and Frequency
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWFREQ;
+		if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		else
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		iwe.u.freq.e = 0;
+		iwe.u.freq.i = 0;
+
+		previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_FREQ_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+        //Add quality statistics
+        //================================
+        memset(&iwe, 0, sizeof(iwe));
+    	iwe.cmd = IWEVQUAL;
+    	iwe.u.qual.level = 0;
+    	iwe.u.qual.noise = 0;
+        set_quality(pAdapter, &iwe.u.qual, pAdapter->ScanTab.BssEntry[i].Rssi);
+    	current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Encyption key
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWENCODE;
+		if (CAP_IS_PRIVACY_ON (pAdapter->ScanTab.BssEntry[i].CapabilityInfo ))
+			iwe.u.data.flags =IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+		else
+			iwe.u.data.flags = IW_ENCODE_DISABLED;
+
+        previous_ev = current_ev;
+        current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf,&iwe, (char *)pAdapter->SharedKey[BSS0][(iwe.u.data.flags & IW_ENCODE_INDEX)-1].Key);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Bit Rate
+		//================================
+		if (pAdapter->ScanTab.BssEntry[i].SupRateLen)
+        {
+            UCHAR tmpRate = pAdapter->ScanTab.BssEntry[i].SupRate[pAdapter->ScanTab.BssEntry[i].SupRateLen-1];
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWRATE;
+    		current_val = current_ev + IW_EV_LCP_LEN;
+            if (tmpRate == 0x82)
+                iwe.u.bitrate.value =  1 * 1000000;
+            else if (tmpRate == 0x84)
+                iwe.u.bitrate.value =  2 * 1000000;
+            else if (tmpRate == 0x8B)
+                iwe.u.bitrate.value =  5.5 * 1000000;
+            else if (tmpRate == 0x96)
+                iwe.u.bitrate.value =  11 * 1000000;
+            else
+    		    iwe.u.bitrate.value =  (tmpRate/2) * 1000000;
+
+			iwe.u.bitrate.disabled = 0;
+			current_val = IWE_STREAM_ADD_VALUE(info, current_ev,
+				current_val, end_buf, &iwe,
+    			IW_EV_PARAM_LEN);
+
+        	if((current_val-current_ev)>IW_EV_LCP_LEN)
+            	current_ev = current_val;
+        	else
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+#ifdef IWEVGENIE
+		//WPA IE
+		if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+		{
+			memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].WpaIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].WpaIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].WpaIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+		}
+
+		//WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+        	memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].RsnIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].RsnIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].RsnIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#else
+        //WPA IE
+		//================================
+        if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "wpa_ie=", 7);
+            for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].WpaIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].WpaIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+        //WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "rsn_ie=", 7);
+			for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].RsnIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].RsnIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#endif // IWEVGENIE //
+	}
+
+	data->length = current_ev - extra;
+    pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+	DBGPRINT(RT_DEBUG_ERROR ,("===>rt_ioctl_giwscan. %d(%d) BSS returned, data->length = %d\n",i , pAdapter->ScanTab.BssNr, data->length));
+	return 0;
+}
+#endif
+
+int rt_ioctl_siwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (data->flags)
+	{
+		PCHAR	pSsidString = NULL;
+
+		// Includes null character.
+		if (data->length > (IW_ESSID_MAX_SIZE + 1))
+			return -E2BIG;
+
+		pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+		if (pSsidString)
+		{
+			NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+			NdisMoveMemory(pSsidString, essid, data->length);
+			if (Set_SSID_Proc(pAdapter, pSsidString) == FALSE)
+				return -EINVAL;
+		}
+		else
+			return -ENOMEM;
+	}
+	else
+	{
+		// ANY ssid
+		if (Set_SSID_Proc(pAdapter, "") == FALSE)
+			return -EINVAL;
+    }
+	return 0;
+}
+
+int rt_ioctl_giwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	data->flags = 1;
+    if (MONITOR_ON(pAdapter))
+    {
+        data->length  = 0;
+        return 0;
+    }
+
+	if (OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is connected\n"));
+		data->length = pAdapter->CommonCfg.SsidLen;
+		memcpy(essid, pAdapter->CommonCfg.Ssid, pAdapter->CommonCfg.SsidLen);
+	}
+	else
+	{//the ANY ssid was specified
+		data->length  = 0;
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is not connected, ess\n"));
+	}
+
+	return 0;
+
+}
+
+int rt_ioctl_siwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE ,("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (data->length > IW_ESSID_MAX_SIZE)
+		return -EINVAL;
+
+	memset(pAdapter->nickname, 0, IW_ESSID_MAX_SIZE + 1);
+	memcpy(pAdapter->nickname, nickname, data->length);
+
+
+	return 0;
+}
+
+int rt_ioctl_giwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (data->length > strlen(pAdapter->nickname) + 1)
+		data->length = strlen(pAdapter->nickname) + 1;
+	if (data->length > 0) {
+		memcpy(nickname, pAdapter->nickname, data->length-1);
+		nickname[data->length-1] = '\0';
+	}
+	return 0;
+}
+
+int rt_ioctl_siwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	u16 val;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (rts->disabled)
+		val = MAX_RTS_THRESHOLD;
+	else if (rts->value < 0 || rts->value > MAX_RTS_THRESHOLD)
+		return -EINVAL;
+	else if (rts->value == 0)
+	    val = MAX_RTS_THRESHOLD;
+	else
+		val = rts->value;
+
+	if (val != pAdapter->CommonCfg.RtsThreshold)
+		pAdapter->CommonCfg.RtsThreshold = val;
+
+	return 0;
+}
+
+int rt_ioctl_giwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	rts->value = pAdapter->CommonCfg.RtsThreshold;
+	rts->disabled = (rts->value == MAX_RTS_THRESHOLD);
+	rts->fixed = 1;
+
+	return 0;
+}
+
+int rt_ioctl_siwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	u16 val;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if (frag->disabled)
+		val = MAX_FRAG_THRESHOLD;
+	else if (frag->value >= MIN_FRAG_THRESHOLD || frag->value <= MAX_FRAG_THRESHOLD)
+        val = __cpu_to_le16(frag->value & ~0x1); /* even numbers only */
+	else if (frag->value == 0)
+	    val = MAX_FRAG_THRESHOLD;
+	else
+		return -EINVAL;
+
+	pAdapter->CommonCfg.FragmentThreshold = val;
+	return 0;
+}
+
+int rt_ioctl_giwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	frag->value = pAdapter->CommonCfg.FragmentThreshold;
+	frag->disabled = (frag->value == MAX_FRAG_THRESHOLD);
+	frag->fixed = 1;
+
+	return 0;
+}
+
+#define MAX_WEP_KEY_SIZE 13
+#define MIN_WEP_KEY_SIZE 5
+int rt_ioctl_siwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if ((erq->length == 0) &&
+        (erq->flags & IW_ENCODE_DISABLED))
+	{
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+	else if ((erq->length == 0) &&
+             (erq->flags & IW_ENCODE_RESTRICTED || erq->flags & IW_ENCODE_OPEN))
+	{
+		STA_PORT_SECURED(pAdapter);
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+		if (erq->flags & IW_ENCODE_RESTRICTED)
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    	else
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+
+    if (erq->length > 0)
+	{
+		int keyIdx = (erq->flags & IW_ENCODE_INDEX) - 1;
+		/* Check the size of the key */
+		if (erq->length > MAX_WEP_KEY_SIZE) {
+			return -EINVAL;
+		}
+		/* Check key index */
+		if ((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+        {
+            DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::Wrong keyIdx=%d! Using default key instead (%d)\n",
+                                        keyIdx, pAdapter->StaCfg.DefaultKeyId));
+
+            //Using default key
+			keyIdx = pAdapter->StaCfg.DefaultKeyId;
+        }
+
+        NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+
+		if (erq->length == MAX_WEP_KEY_SIZE)
+        {
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+		}
+		else if (erq->length == MIN_WEP_KEY_SIZE)
+        {
+            pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+		}
+		else
+			/* Disable the key */
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+
+		/* Check if the key is not marked as invalid */
+		if(!(erq->flags & IW_ENCODE_NOKEY)) {
+			/* Copy the key in the driver */
+			NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, extra, erq->length);
+        }
+	}
+    else
+			{
+		/* Do we want to just set the transmit key index ? */
+		int index = (erq->flags & IW_ENCODE_INDEX) - 1;
+		if ((index >= 0) && (index < 4))
+        {
+			pAdapter->StaCfg.DefaultKeyId = index;
+            }
+        else
+			/* Don't complain if only change the mode */
+			if(!erq->flags & IW_ENCODE_MODE) {
+				return -EINVAL;
+		}
+	}
+
+done:
+    DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::erq->flags=%x\n",erq->flags));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::AuthMode=%x\n",pAdapter->StaCfg.AuthMode));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::DefaultKeyId=%x, KeyLen = %d\n",pAdapter->StaCfg.DefaultKeyId , pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::WepStatus=%x\n",pAdapter->StaCfg.WepStatus));
+	return 0;
+}
+
+int
+rt_ioctl_giwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *key)
+{
+	int kid;
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	kid = erq->flags & IW_ENCODE_INDEX;
+	DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_giwencode %d\n", erq->flags & IW_ENCODE_INDEX));
+
+	if (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled)
+	{
+		erq->length = 0;
+		erq->flags = IW_ENCODE_DISABLED;
+	}
+	else if ((kid > 0) && (kid <=4))
+	{
+		// copy wep key
+		erq->flags = kid ;			/* NB: base 1 */
+		if (erq->length > pAdapter->SharedKey[BSS0][kid-1].KeyLen)
+			erq->length = pAdapter->SharedKey[BSS0][kid-1].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][kid-1].Key, erq->length);
+		//if ((kid == pAdapter->PortCfg.DefaultKeyId))
+		//erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+
+	}
+	else if (kid == 0)
+	{
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->length = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, erq->length);
+		// copy default key ID
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->flags = pAdapter->StaCfg.DefaultKeyId + 1;			/* NB: base 1 */
+		erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+	}
+
+	return 0;
+
+}
+
+static int
+rt_ioctl_setparam(struct net_device *dev, struct iw_request_info *info,
+			 void *w, char *extra)
+{
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter;
+	POS_COOKIE pObj;
+	char *this_char = extra;
+	char *value;
+	int  Status=0;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+	pObj = (POS_COOKIE) pAdapter->OS_Cookie;
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+			return -ENETDOWN;
+    	}
+
+	if (!*this_char)
+		return -EINVAL;
+
+	if ((value = rtstrchr(this_char, '=')) != NULL)
+	    *value++ = 0;
+
+	if (!value)
+	    return -EINVAL;
+
+	// reject setting nothing besides ANY ssid(ssidLen=0)
+    if (!*value && (strcmp(this_char, "SSID") != 0))
+        return -EINVAL;
+
+	for (PRTMP_PRIVATE_SET_PROC = RTMP_PRIVATE_SUPPORT_PROC; PRTMP_PRIVATE_SET_PROC->name; PRTMP_PRIVATE_SET_PROC++)
+	{
+	    if (strcmp(this_char, PRTMP_PRIVATE_SET_PROC->name) == 0)
+	    {
+	        if(!PRTMP_PRIVATE_SET_PROC->set_proc(pAdapter, value))
+	        {	//FALSE:Set private failed then return Invalid argument
+			    Status = -EINVAL;
+	        }
+		    break;	//Exit for loop.
+	    }
+	}
+
+	if(PRTMP_PRIVATE_SET_PROC->name == NULL)
+	{  //Not found argument
+	    Status = -EINVAL;
+	    DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_setparam:: (iwpriv) Not Support Set Command [%s=%s]\n", this_char, value));
+	}
+
+    return Status;
+}
+
+
+static int
+rt_private_get_statistics(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+	INT				Status = 0;
+    PRTMP_ADAPTER   pAd = dev->ml_priv;
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+    sprintf(extra, "\n\n");
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+	    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	    //sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart - (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+	}
+    sprintf(extra+strlen(extra), "Tx success after retry          = %ld\n", (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx fail to Rcv ACK after retry  = %ld\n", (ULONG)pAd->WlanCounters.FailedCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Success Rcv CTS             = %ld\n", (ULONG)pAd->WlanCounters.RTSSuccessCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Fail Rcv CTS                = %ld\n", (ULONG)pAd->WlanCounters.RTSFailureCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "Rx success                      = %ld\n", (ULONG)pAd->WlanCounters.ReceivedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx with CRC                     = %ld\n", (ULONG)pAd->WlanCounters.FCSErrorCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx drop due to out of resource  = %ld\n", (ULONG)pAd->Counters8023.RxNoBuffer);
+    sprintf(extra+strlen(extra), "Rx duplicate frame              = %ld\n", (ULONG)pAd->WlanCounters.FrameDuplicateCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "False CCA (one second)          = %ld\n", (ULONG)pAd->RalinkCounters.OneSecFalseCCACnt);
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		if (pAd->ate.RxAntennaSel == 0)
+		{
+    		sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->ate.LastRssi1 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->ate.LastRssi2 - pAd->BbpRssiToDbmDelta));
+		}
+		else
+		{
+    		sprintf(extra+strlen(extra), "RSSI                            = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+		}
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    	sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi0 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi1 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi2 - pAd->BbpRssiToDbmDelta));
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    sprintf(extra+strlen(extra), "WpaSupplicantUP                 = %d\n\n", pAd->StaCfg.WpaSupplicantUP);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+    DBGPRINT(RT_DEBUG_TRACE, ("<== rt_private_get_statistics, wrq->length = %d\n", wrq->length));
+
+    return Status;
+}
+
+#ifdef DOT11_N_SUPPORT
+void	getBaInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pOutBuf)
+{
+	INT i, j;
+	BA_ORI_ENTRY *pOriBAEntry;
+	BA_REC_ENTRY *pRecBAEntry;
+
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+		if (((pEntry->ValidAsCLI || pEntry->ValidAsApCli) && (pEntry->Sst == SST_ASSOC))
+			|| (pEntry->ValidAsWDS) || (pEntry->ValidAsMesh))
+		{
+			sprintf(pOutBuf, "%s\n%02X:%02X:%02X:%02X:%02X:%02X (Aid = %d) (AP) -\n",
+                pOutBuf,
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5], pEntry->Aid);
+
+			sprintf(pOutBuf, "%s[Recipient]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BARecWcidArray[j] != 0)
+				{
+					pRecBAEntry =&pAd->BATable.BARecEntry[pEntry->BARecWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, LastIndSeq=%d, ReorderingPkts=%d\n", pOutBuf, j, pRecBAEntry->BAWinSize, pRecBAEntry->LastIndSeq, pRecBAEntry->list.qlen);
+				}
+			}
+			sprintf(pOutBuf, "%s\n", pOutBuf);
+
+			sprintf(pOutBuf, "%s[Originator]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BAOriWcidArray[j] != 0)
+				{
+					pOriBAEntry =&pAd->BATable.BAOriEntry[pEntry->BAOriWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, StartSeq=%d, CurTxSeq=%d\n", pOutBuf, j, pOriBAEntry->BAWinSize, pOriBAEntry->Sequence, pEntry->TxSeq[j]);
+				}
+			}
+			sprintf(pOutBuf, "%s\n\n", pOutBuf);
+		}
+        if (strlen(pOutBuf) > (IW_PRIV_SIZE_MASK - 30))
+                break;
+	}
+
+	return;
+}
+#endif // DOT11_N_SUPPORT //
+
+static int
+rt_private_show(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+    INT				Status = 0;
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+    PRTMP_ADAPTER   pAd;
+	POS_COOKIE		pObj;
+    u32             subcmd = wrq->flags;
+
+	if (dev->priv_flags == INT_MAIN)
+		pAd = dev->ml_priv;
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		pAd = pVirtualAd->RtmpDev->ml_priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+    switch(subcmd)
+    {
+
+        case SHOW_CONN_STATUS:
+            if (MONITOR_ON(pAd))
+            {
+#ifdef DOT11_N_SUPPORT
+                if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                    pAd->CommonCfg.RegTransmitSetting.field.BW)
+                    sprintf(extra, "Monitor Mode(CentralChannel %d)\n", pAd->CommonCfg.CentralChannel);
+                else
+#endif // DOT11_N_SUPPORT //
+                    sprintf(extra, "Monitor Mode(Channel %d)\n", pAd->CommonCfg.Channel);
+            }
+            else
+            {
+                if (pAd->IndicateMediaState == NdisMediaStateConnected)
+            	{
+            	    if (INFRA_ON(pAd))
+                    {
+                    sprintf(extra, "Connected(AP: %s[%02X:%02X:%02X:%02X:%02X:%02X])\n",
+                                    pAd->CommonCfg.Ssid,
+                                    pAd->CommonCfg.Bssid[0],
+                                    pAd->CommonCfg.Bssid[1],
+                                    pAd->CommonCfg.Bssid[2],
+                                    pAd->CommonCfg.Bssid[3],
+                                    pAd->CommonCfg.Bssid[4],
+                                    pAd->CommonCfg.Bssid[5]);
+            		DBGPRINT(RT_DEBUG_TRACE ,("Ssid=%s ,Ssidlen = %d\n",pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen));
+            	}
+                    else if (ADHOC_ON(pAd))
+                        sprintf(extra, "Connected\n");
+            	}
+            	else
+            	{
+            	    sprintf(extra, "Disconnected\n");
+            		DBGPRINT(RT_DEBUG_TRACE ,("ConnStatus is not connected\n"));
+            	}
+            }
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case SHOW_DRVIER_VERION:
+            sprintf(extra, "Driver version-%s, %s %s\n", STA_DRIVER_VERSION, __DATE__, __TIME__ );
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#ifdef DOT11_N_SUPPORT
+        case SHOW_BA_INFO:
+            getBaInfo(pAd, extra);
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#endif // DOT11_N_SUPPORT //
+		case SHOW_DESC_INFO:
+			{
+				Show_DescInfo_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+        case RAIO_OFF:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = FALSE;
+            if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == FALSE)
+                {
+                    MlmeRadioOff(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = SW_RADIO_OFF;
+                }
+            }
+            sprintf(extra, "Radio Off\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case RAIO_ON:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = TRUE;
+            //if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == TRUE)
+                {
+                    MlmeRadioOn(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+                }
+            }
+            sprintf(extra, "Radio On\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case SHOW_DLS_ENTRY_INFO:
+			{
+				Set_DlsEntryInfo_Display_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+
+		case SHOW_CFG_VALUE:
+			{
+				Status = RTMPShowCfgValue(pAd, wrq->pointer, extra);
+				if (Status == 0)
+					wrq->length = strlen(extra) + 1; // 1: size of '\0'
+			}
+			break;
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("%s - unknow subcmd = %d\n", __func__, subcmd));
+            break;
+    }
+
+    return Status;
+}
+
+#ifdef SIOCSIWMLME
+int rt_ioctl_siwmlme(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+	struct iw_mlme *pMlme = (struct iw_mlme *)wrqu->data.pointer;
+	MLME_QUEUE_ELEM				MsgElem;
+	MLME_DISASSOC_REQ_STRUCT	DisAssocReq;
+	MLME_DEAUTH_REQ_STRUCT      DeAuthReq;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("====> %s\n", __func__));
+
+	if (pMlme == NULL)
+		return -EINVAL;
+
+	switch(pMlme->cmd)
+	{
+#ifdef IW_MLME_DEAUTH
+		case IW_MLME_DEAUTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DEAUTH\n", __func__));
+			COPY_MAC_ADDR(DeAuthReq.Addr, pAd->CommonCfg.Bssid);
+			DeAuthReq.Reason = pMlme->reason_code;
+			MsgElem.MsgLen = sizeof(MLME_DEAUTH_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DeAuthReq, sizeof(MLME_DEAUTH_REQ_STRUCT));
+			MlmeDeauthReqAction(pAd, &MsgElem);
+			if (INFRA_ON(pAd))
+			{
+			    LinkDown(pAd, FALSE);
+			    pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			}
+			break;
+#endif // IW_MLME_DEAUTH //
+#ifdef IW_MLME_DISASSOC
+		case IW_MLME_DISASSOC:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DISASSOC\n", __func__));
+			COPY_MAC_ADDR(DisAssocReq.Addr, pAd->CommonCfg.Bssid);
+			DisAssocReq.Reason =  pMlme->reason_code;
+
+			MsgElem.Machine = ASSOC_STATE_MACHINE;
+			MsgElem.MsgType = MT2_MLME_DISASSOC_REQ;
+			MsgElem.MsgLen = sizeof(MLME_DISASSOC_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DisAssocReq, sizeof(MLME_DISASSOC_REQ_STRUCT));
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+			MlmeDisassocReqAction(pAd, &MsgElem);
+			break;
+#endif // IW_MLME_DISASSOC //
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - Unknow Command\n", __func__));
+			break;
+	}
+
+	return 0;
+}
+#endif // SIOCSIWMLME //
+
+#if WIRELESS_EXT > 17
+int rt_ioctl_siwauth(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = dev->ml_priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+	switch (param->flags & IW_AUTH_INDEX) {
+    	case IW_AUTH_WPA_VERSION:
+            if (param->value == IW_AUTH_WPA_VERSION_WPA)
+            {
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+				if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+					pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+            }
+            else if (param->value == IW_AUTH_WPA_VERSION_WPA2)
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_PAIRWISE:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption2Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption3Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_PAIRWISE - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_GROUP:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_GROUP - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_KEY_MGMT:
+            if (param->value == IW_AUTH_KEY_MGMT_802_1X)
+            {
+                if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+                else if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+#ifdef WPA_SUPPLICANT_SUPPORT
+                else
+                    // WEP 1x
+                    pAdapter->StaCfg.IEEE8021X = TRUE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == 0)
+            {
+				STA_PORT_SECURED(pAdapter);
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_KEY_MGMT - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+            break;
+    	case IW_AUTH_PRIVACY_INVOKED:
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_PRIVACY_INVOKED - param->value = %d!\n", __func__, param->value));
+    		break;
+    	case IW_AUTH_DROP_UNENCRYPTED:
+            if (param->value != 0)
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			else
+			{
+				STA_PORT_SECURED(pAdapter);
+			}
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __func__, param->value));
+    		break;
+    	case IW_AUTH_80211_AUTH_ALG:
+			if (param->value & IW_AUTH_ALG_SHARED_KEY)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+			}
+            else if (param->value & IW_AUTH_ALG_OPEN_SYSTEM)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+			}
+            else
+				return -EINVAL;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_80211_AUTH_ALG - param->value = %d!\n", __func__, param->value));
+			break;
+    	case IW_AUTH_WPA_ENABLED:
+    		DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_ENABLED - Driver supports WPA!(param->value = %d)\n", __func__, param->value));
+    		break;
+    	default:
+    		return -EOPNOTSUPP;
+}
+
+	return 0;
+}
+
+int rt_ioctl_giwauth(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = dev->ml_priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+    }
+
+	switch (param->flags & IW_AUTH_INDEX) {
+	case IW_AUTH_DROP_UNENCRYPTED:
+        param->value = (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled) ? 0 : 1;
+		break;
+
+	case IW_AUTH_80211_AUTH_ALG:
+        param->value = (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared) ? IW_AUTH_ALG_SHARED_KEY : IW_AUTH_ALG_OPEN_SYSTEM;
+		break;
+
+	case IW_AUTH_WPA_ENABLED:
+		param->value = (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) ? 1 : 0;
+		break;
+
+	default:
+		return -EOPNOTSUPP;
+	}
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_giwauth::param->value = %d!\n", param->value));
+	return 0;
+}
+
+void fnSetCipherKey(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  INT             keyIdx,
+    IN  UCHAR           CipherAlg,
+    IN  BOOLEAN         bGTK,
+    IN  struct iw_encode_ext *ext)
+{
+    NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, LEN_TKIP_EK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].TxMic, ext->key + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].RxMic, ext->key + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CipherAlg;
+
+    // Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAdapter,
+						  BSS0,
+						  keyIdx,
+						  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+						  pAdapter->SharedKey[BSS0][keyIdx].Key,
+						  pAdapter->SharedKey[BSS0][keyIdx].TxMic,
+						  pAdapter->SharedKey[BSS0][keyIdx].RxMic);
+
+    if (bGTK)
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  NULL);
+    else
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  &pAdapter->MacTab.Content[BSSID_WCID]);
+}
+
+int rt_ioctl_siwencodeext(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+			{
+    PRTMP_ADAPTER   pAdapter = dev->ml_priv;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+    int keyIdx, alg = ext->alg;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if (encoding->flags & IW_ENCODE_DISABLED)
+	{
+        keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+        // set BSSID wcid entry of the Pair-wise Key table as no-security mode
+	    AsicRemovePairwiseKeyEntry(pAdapter, BSS0, BSSID_WCID);
+        pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+		pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_NONE;
+		AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)keyIdx);
+        NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+        DBGPRINT(RT_DEBUG_TRACE, ("%s::Remove all keys!(encoding->flags = %x)\n", __func__, encoding->flags));
+    }
+					else
+    {
+        // Get Key Index and convet to our own defined key index
+    	keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+    	if((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+    		return -EINVAL;
+
+        if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+        {
+            pAdapter->StaCfg.DefaultKeyId = keyIdx;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::DefaultKeyId = %d\n", __func__, pAdapter->StaCfg.DefaultKeyId));
+        }
+
+        switch (alg) {
+    		case IW_ENCODE_ALG_NONE:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_NONE\n", __func__));
+    			break;
+    		case IW_ENCODE_ALG_WEP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_WEP - ext->key_len = %d, keyIdx = %d\n", __func__, ext->key_len, keyIdx));
+    			if (ext->key_len == MAX_WEP_KEY_SIZE)
+                {
+        			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+				}
+        		else if (ext->key_len == MIN_WEP_KEY_SIZE)
+                {
+                    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+			}
+        		else
+                    return -EINVAL;
+
+                NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+			    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, ext->key_len);
+    			break;
+            case IW_ENCODE_ALG_TKIP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_TKIP - keyIdx = %d, ext->key_len = %d\n", __func__, keyIdx, ext->key_len));
+                if (ext->key_len == 32)
+                {
+                    if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, FALSE, ext);
+                        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                        {
+                            STA_PORT_SECURED(pAdapter);
+                        }
+		}
+                    else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, TRUE, ext);
+
+                        // set 802.1x port control
+            	        STA_PORT_SECURED(pAdapter);
+                    }
+                }
+                else
+                    return -EINVAL;
+                break;
+            case IW_ENCODE_ALG_CCMP:
+                if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+		{
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, FALSE, ext);
+                    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                    	STA_PORT_SECURED(pAdapter);
+                }
+                else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                {
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, TRUE, ext);
+
+                    // set 802.1x port control
+        	        STA_PORT_SECURED(pAdapter);
+                }
+                break;
+    		default:
+    			return -EINVAL;
+		}
+    }
+
+    return 0;
+}
+
+int
+rt_ioctl_giwencodeext(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER pAd = dev->ml_priv;
+	PCHAR pKey = NULL;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+	int idx, max_key_len;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_giwencodeext\n"));
+
+	max_key_len = encoding->length - sizeof(*ext);
+	if (max_key_len < 0)
+		return -EINVAL;
+
+	idx = encoding->flags & IW_ENCODE_INDEX;
+	if (idx)
+	{
+		if (idx < 1 || idx > 4)
+			return -EINVAL;
+		idx--;
+
+		if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+			(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled))
+		{
+			if (idx != pAd->StaCfg.DefaultKeyId)
+			{
+				ext->key_len = 0;
+				return 0;
+			}
+		}
+	}
+	else
+		idx = pAd->StaCfg.DefaultKeyId;
+
+	encoding->flags = idx + 1;
+	memset(ext, 0, sizeof(*ext));
+
+	ext->key_len = 0;
+	switch(pAd->StaCfg.WepStatus) {
+		case Ndis802_11WEPDisabled:
+			ext->alg = IW_ENCODE_ALG_NONE;
+			encoding->flags |= IW_ENCODE_DISABLED;
+			break;
+		case Ndis802_11WEPEnabled:
+			ext->alg = IW_ENCODE_ALG_WEP;
+			if (pAd->SharedKey[BSS0][idx].KeyLen > max_key_len)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = pAd->SharedKey[BSS0][idx].KeyLen;
+				pKey = &(pAd->SharedKey[BSS0][idx].Key[0]);
+			}
+			break;
+		case Ndis802_11Encryption2Enabled:
+		case Ndis802_11Encryption3Enabled:
+			if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+				ext->alg = IW_ENCODE_ALG_TKIP;
+			else
+				ext->alg = IW_ENCODE_ALG_CCMP;
+
+			if (max_key_len < 32)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = 32;
+				pKey = &pAd->StaCfg.PMK[0];
+			}
+			break;
+		default:
+			return -EINVAL;
+	}
+
+	if (ext->key_len && pKey)
+	{
+		encoding->flags |= IW_ENCODE_ENABLED;
+		memcpy(ext->key, pKey, ext->key_len);
+	}
+
+	return 0;
+}
+
+#ifdef SIOCSIWGENIE
+int rt_ioctl_siwgenie(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+
+	if (wrqu->data.length > MAX_LEN_OF_RSNIE ||
+	    (wrqu->data.length && extra == NULL))
+		return -EINVAL;
+
+	if (wrqu->data.length)
+	{
+		pAd->StaCfg.RSNIE_Len = wrqu->data.length;
+		NdisMoveMemory(&pAd->StaCfg.RSN_IE[0], extra, pAd->StaCfg.RSNIE_Len);
+	}
+	else
+	{
+		pAd->StaCfg.RSNIE_Len = 0;
+		NdisZeroMemory(&pAd->StaCfg.RSN_IE[0], MAX_LEN_OF_RSNIE);
+	}
+
+	return 0;
+}
+#endif // SIOCSIWGENIE //
+
+int rt_ioctl_giwgenie(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+
+	if ((pAd->StaCfg.RSNIE_Len == 0) ||
+		(pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA))
+	{
+		wrqu->data.length = 0;
+		return 0;
+	}
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifdef SIOCSIWGENIE
+	if (pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+	if (wrqu->data.length < pAd->StaCfg.RSNIE_Len)
+		return -E2BIG;
+
+	wrqu->data.length = pAd->StaCfg.RSNIE_Len;
+	memcpy(extra, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+	else
+#endif // SIOCSIWGENIE //
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+	{
+		UCHAR RSNIe = IE_WPA;
+
+		if (wrqu->data.length < (pAd->StaCfg.RSNIE_Len + 2)) // ID, Len
+			return -E2BIG;
+		wrqu->data.length = pAd->StaCfg.RSNIE_Len + 2;
+
+		if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2))
+			RSNIe = IE_RSN;
+
+		extra[0] = (char)RSNIe;
+		extra[1] = pAd->StaCfg.RSNIE_Len;
+		memcpy(extra+2, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+
+	return 0;
+}
+
+int rt_ioctl_siwpmksa(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+	struct iw_pmksa *pPmksa = (struct iw_pmksa *)wrqu->data.pointer;
+	INT	CachedIdx = 0, idx = 0;
+
+	if (pPmksa == NULL)
+		return -EINVAL;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_siwpmksa\n"));
+	switch(pPmksa->cmd)
+	{
+		case IW_PMKSA_FLUSH:
+			NdisZeroMemory(pAd->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_FLUSH\n"));
+			break;
+		case IW_PMKSA_REMOVE:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+		        {
+		        	NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN);
+					NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].PMKID, 16);
+					for (idx = CachedIdx; idx < (pAd->StaCfg.SavedPMKNum - 1); idx++)
+					{
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].BSSID[0], &pAd->StaCfg.SavedPMK[idx+1].BSSID[0], MAC_ADDR_LEN);
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].PMKID[0], &pAd->StaCfg.SavedPMK[idx+1].PMKID[0], 16);
+					}
+					pAd->StaCfg.SavedPMKNum--;
+			        break;
+		        }
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_REMOVE\n"));
+			break;
+		case IW_PMKSA_ADD:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+			        break;
+	        }
+
+	        // Found, replace it
+	        if (CachedIdx < PMKID_NO)
+	        {
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+		        pAd->StaCfg.SavedPMKNum++;
+	        }
+	        // Not found, replace the last one
+	        else
+	        {
+		        // Randomly replace one
+		        CachedIdx = (pPmksa->bssid.sa_data[5] % PMKID_NO);
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_ADD\n"));
+			break;
+		default:
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - Unknow Command!!\n"));
+			break;
+	}
+
+	return 0;
+}
+#endif // #if WIRELESS_EXT > 17
+
+#ifdef DBG
+static int
+rt_private_ioctl_bbp(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+			{
+	CHAR				*this_char;
+	CHAR				*value = NULL;
+	UCHAR				regBBP = 0;
+	UINT32				bbpId;
+	UINT32				bbpValue;
+	BOOLEAN				bIsPrintAllBBP = FALSE;
+	INT					Status = 0;
+    PRTMP_ADAPTER       pAdapter = dev->ml_priv;
+
+
+	memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	if (wrq->length > 1) //No parameters.
+				{
+		sprintf(extra, "\n");
+
+		//Parsing Read or Write
+		this_char = wrq->pointer;
+		DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s\n", this_char));
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s, value=%s\n", this_char, value));
+			if (sscanf(this_char, "%d", &(bbpId)) == 1)
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		else
+		{ //Write
+			if ((sscanf(this_char, "%d", &(bbpId)) == 1) && (sscanf(value, "%x", &(bbpValue)) == 1))
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+						//Read it back for showing
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					    RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+    					//Read it back for showing
+    					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		}
+	else
+		bIsPrintAllBBP = TRUE;
+
+next:
+	if (bIsPrintAllBBP)
+	{
+		memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+		sprintf(extra, "\n");
+		for (bbpId = 0; bbpId <= 136; bbpId++)
+		{
+		    if (strlen(extra) >= (IW_PRIV_SIZE_MASK - 10))
+                break;
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+			else
+#endif // RALINK_ATE //
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X    ", bbpId, bbpId*2, regBBP);
+			if (bbpId%5 == 4)
+				sprintf(extra+strlen(extra), "\n");
+		}
+
+        wrq->length = strlen(extra) + 1; // 1: size of '\0'
+        DBGPRINT(RT_DEBUG_TRACE, ("wrq->length = %d\n", wrq->length));
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==rt_private_ioctl_bbp\n\n"));
+
+    return Status;
+}
+#endif // DBG //
+
+int rt_ioctl_siwrate(struct net_device *dev,
+			struct iw_request_info *info,
+			union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = dev->ml_priv;
+    UINT32          rate = wrqu->bitrate.value, fixed = wrqu->bitrate.fixed;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(rate = %d, fixed = %d)\n", rate, fixed));
+    /* rate = -1 => auto rate
+       rate = X, fixed = 1 => (fixed rate X)
+    */
+    if (rate == -1)
+    {
+		//Auto Rate
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+		pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+		if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+		    (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+			RTMPSetDesiredRates(pAd, -1);
+
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+    }
+    else
+    {
+        if (fixed)
+        {
+        	pAd->StaCfg.bAutoTxRateSwitch = FALSE;
+            if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+                (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+                RTMPSetDesiredRates(pAd, rate);
+            else
+            {
+                pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+#ifdef DOT11_N_SUPPORT
+                SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(HtMcs=%d)\n",pAd->StaCfg.DesiredTransmitSetting.field.MCS));
+        }
+        else
+        {
+            // TODO: rate = X, fixed = 0 => (rates <= X)
+            return -EOPNOTSUPP;
+        }
+    }
+
+    return 0;
+}
+
+int rt_ioctl_giwrate(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = dev->ml_priv;
+    int rate_index = 0, rate_count = 0;
+    HTTRANSMIT_SETTING ht_setting;
+    __s32 ralinkrate[] =
+	{2,  4,   11,  22, // CCK
+	12, 18,   24,  36, 48, 72, 96, 108, // OFDM
+	13, 26,   39,  52,  78, 104, 117, 130, 26,  52,  78, 104, 156, 208, 234, 260, // 20MHz, 800ns GI, MCS: 0 ~ 15
+	39, 78,  117, 156, 234, 312, 351, 390,										  // 20MHz, 800ns GI, MCS: 16 ~ 23
+	27, 54,   81, 108, 162, 216, 243, 270, 54, 108, 162, 216, 324, 432, 486, 540, // 40MHz, 800ns GI, MCS: 0 ~ 15
+	81, 162, 243, 324, 486, 648, 729, 810,										  // 40MHz, 800ns GI, MCS: 16 ~ 23
+	14, 29,   43,  57,  87, 115, 130, 144, 29, 59,   87, 115, 173, 230, 260, 288, // 20MHz, 400ns GI, MCS: 0 ~ 15
+	43, 87,  130, 173, 260, 317, 390, 433,										  // 20MHz, 400ns GI, MCS: 16 ~ 23
+	30, 60,   90, 120, 180, 240, 270, 300, 60, 120, 180, 240, 360, 480, 540, 600, // 40MHz, 400ns GI, MCS: 0 ~ 15
+	90, 180, 270, 360, 540, 720, 810, 900};										  // 40MHz, 400ns GI, MCS: 16 ~ 23
+
+    rate_count = sizeof(ralinkrate)/sizeof(__s32);
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if ((pAd->StaCfg.bAutoTxRateSwitch == FALSE) &&
+        (INFRA_ON(pAd)) &&
+        ((pAd->CommonCfg.PhyMode <= PHY_11G) || (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM)))
+        ht_setting.word = pAd->StaCfg.HTPhyMode.word;
+    else
+        ht_setting.word = pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+
+#ifdef DOT11_N_SUPPORT
+    if (ht_setting.field.MODE >= MODE_HTMIX)
+    {
+    	rate_index = 12 + ((UCHAR)ht_setting.field.BW *24) + ((UCHAR)ht_setting.field.ShortGI *48) + ((UCHAR)ht_setting.field.MCS);
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+    if (ht_setting.field.MODE == MODE_OFDM)
+    	rate_index = (UCHAR)(ht_setting.field.MCS) + 4;
+    else if (ht_setting.field.MODE == MODE_CCK)
+    	rate_index = (UCHAR)(ht_setting.field.MCS);
+
+    if (rate_index < 0)
+        rate_index = 0;
+
+    if (rate_index > rate_count)
+        rate_index = rate_count;
+
+    wrqu->bitrate.value = ralinkrate[rate_index] * 500000;
+    wrqu->bitrate.disabled = 0;
+
+    return 0;
+}
+
+static const iw_handler rt_handler[] =
+{
+	(iw_handler) NULL,			            /* SIOCSIWCOMMIT */
+	(iw_handler) rt_ioctl_giwname,			/* SIOCGIWNAME   */
+	(iw_handler) NULL,			            /* SIOCSIWNWID   */
+	(iw_handler) NULL,			            /* SIOCGIWNWID   */
+	(iw_handler) rt_ioctl_siwfreq,		    /* SIOCSIWFREQ   */
+	(iw_handler) rt_ioctl_giwfreq,		    /* SIOCGIWFREQ   */
+	(iw_handler) rt_ioctl_siwmode,		    /* SIOCSIWMODE   */
+	(iw_handler) rt_ioctl_giwmode,		    /* SIOCGIWMODE   */
+	(iw_handler) NULL,		                /* SIOCSIWSENS   */
+	(iw_handler) NULL,		                /* SIOCGIWSENS   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWRANGE  */
+	(iw_handler) rt_ioctl_giwrange,		    /* SIOCGIWRANGE  */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWPRIV   */
+	(iw_handler) NULL /* kernel code */,    /* SIOCGIWPRIV   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWSTATS  */
+	(iw_handler) rt28xx_get_wireless_stats /* kernel code */,    /* SIOCGIWSTATS  */
+	(iw_handler) NULL,		                /* SIOCSIWSPY    */
+	(iw_handler) NULL,		                /* SIOCGIWSPY    */
+	(iw_handler) NULL,				        /* SIOCSIWTHRSPY */
+	(iw_handler) NULL,				        /* SIOCGIWTHRSPY */
+	(iw_handler) rt_ioctl_siwap,            /* SIOCSIWAP     */
+	(iw_handler) rt_ioctl_giwap,		    /* SIOCGIWAP     */
+#ifdef SIOCSIWMLME
+	(iw_handler) rt_ioctl_siwmlme,	        /* SIOCSIWMLME   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWMLME */
+#endif // SIOCSIWMLME //
+	(iw_handler) rt_ioctl_iwaplist,		    /* SIOCGIWAPLIST */
+#ifdef SIOCGIWSCAN
+	(iw_handler) rt_ioctl_siwscan,		    /* SIOCSIWSCAN   */
+	(iw_handler) rt_ioctl_giwscan,		    /* SIOCGIWSCAN   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWSCAN   */
+	(iw_handler) NULL,				        /* SIOCGIWSCAN   */
+#endif /* SIOCGIWSCAN */
+	(iw_handler) rt_ioctl_siwessid,		    /* SIOCSIWESSID  */
+	(iw_handler) rt_ioctl_giwessid,		    /* SIOCGIWESSID  */
+	(iw_handler) rt_ioctl_siwnickn,		    /* SIOCSIWNICKN  */
+	(iw_handler) rt_ioctl_giwnickn,		    /* SIOCGIWNICKN  */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) rt_ioctl_siwrate,          /* SIOCSIWRATE   */
+	(iw_handler) rt_ioctl_giwrate,          /* SIOCGIWRATE   */
+	(iw_handler) rt_ioctl_siwrts,		    /* SIOCSIWRTS    */
+	(iw_handler) rt_ioctl_giwrts,		    /* SIOCGIWRTS    */
+	(iw_handler) rt_ioctl_siwfrag,		    /* SIOCSIWFRAG   */
+	(iw_handler) rt_ioctl_giwfrag,		    /* SIOCGIWFRAG   */
+	(iw_handler) NULL,		                /* SIOCSIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCGIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCSIWRETRY  */
+	(iw_handler) NULL,		                /* SIOCGIWRETRY  */
+	(iw_handler) rt_ioctl_siwencode,		/* SIOCSIWENCODE */
+	(iw_handler) rt_ioctl_giwencode,		/* SIOCGIWENCODE */
+	(iw_handler) NULL,		                /* SIOCSIWPOWER  */
+	(iw_handler) NULL,		                /* SIOCGIWPOWER  */
+	(iw_handler) NULL,						/* -- hole -- */
+	(iw_handler) NULL,						/* -- hole -- */
+#if WIRELESS_EXT > 17
+    (iw_handler) rt_ioctl_siwgenie,         /* SIOCSIWGENIE  */
+	(iw_handler) rt_ioctl_giwgenie,         /* SIOCGIWGENIE  */
+	(iw_handler) rt_ioctl_siwauth,		    /* SIOCSIWAUTH   */
+	(iw_handler) rt_ioctl_giwauth,		    /* SIOCGIWAUTH   */
+	(iw_handler) rt_ioctl_siwencodeext,	    /* SIOCSIWENCODEEXT */
+	(iw_handler) rt_ioctl_giwencodeext,		/* SIOCGIWENCODEEXT */
+	(iw_handler) rt_ioctl_siwpmksa,         /* SIOCSIWPMKSA  */
+#endif
+};
+
+static const iw_handler rt_priv_handlers[] = {
+	(iw_handler) NULL, /* + 0x00 */
+	(iw_handler) NULL, /* + 0x01 */
+#ifndef CONFIG_AP_SUPPORT
+	(iw_handler) rt_ioctl_setparam, /* + 0x02 */
+#else
+	(iw_handler) NULL, /* + 0x02 */
+#endif // CONFIG_AP_SUPPORT //
+#ifdef DBG
+	(iw_handler) rt_private_ioctl_bbp, /* + 0x03 */
+#else
+	(iw_handler) NULL, /* + 0x03 */
+#endif
+	(iw_handler) NULL, /* + 0x04 */
+	(iw_handler) NULL, /* + 0x05 */
+	(iw_handler) NULL, /* + 0x06 */
+	(iw_handler) NULL, /* + 0x07 */
+	(iw_handler) NULL, /* + 0x08 */
+	(iw_handler) rt_private_get_statistics, /* + 0x09 */
+	(iw_handler) NULL, /* + 0x0A */
+	(iw_handler) NULL, /* + 0x0B */
+	(iw_handler) NULL, /* + 0x0C */
+	(iw_handler) NULL, /* + 0x0D */
+	(iw_handler) NULL, /* + 0x0E */
+	(iw_handler) NULL, /* + 0x0F */
+	(iw_handler) NULL, /* + 0x10 */
+	(iw_handler) rt_private_show, /* + 0x11 */
+    (iw_handler) NULL, /* + 0x12 */
+	(iw_handler) NULL, /* + 0x13 */
+	(iw_handler) NULL, /* + 0x15 */
+	(iw_handler) NULL, /* + 0x17 */
+	(iw_handler) NULL, /* + 0x18 */
+};
+
+const struct iw_handler_def rt28xx_iw_handler_def =
+{
+#define	N(a)	(sizeof (a) / sizeof (a[0]))
+	.standard	= (iw_handler *) rt_handler,
+	.num_standard	= sizeof(rt_handler) / sizeof(iw_handler),
+	.private	= (iw_handler *) rt_priv_handlers,
+	.num_private		= N(rt_priv_handlers),
+	.private_args	= (struct iw_priv_args *) privtab,
+	.num_private_args	= N(privtab),
+#if IW_HANDLER_VERSION >= 7
+    .get_wireless_stats = rt28xx_get_wireless_stats,
+#endif
+};
+
+INT RTMPSetInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_MAC_ADDRESS             Bssid;
+    RT_802_11_PHY_MODE                  PhyMode;
+    RT_802_11_STA_CONFIG                StaConfig;
+    NDIS_802_11_RATES                   aryRates;
+    RT_802_11_PREAMBLE                  Preamble;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode = Ndis802_11AuthModeMax;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    PNDIS_802_11_KEY                    pKey = NULL;
+    PNDIS_802_11_WEP			        pWepKey =NULL;
+    PNDIS_802_11_REMOVE_KEY             pRemoveKey = NULL;
+    NDIS_802_11_CONFIGURATION           Config, *pConfig = NULL;
+    NDIS_802_11_NETWORK_TYPE            NetType;
+    ULONG                               Now;
+    UINT                                KeyIdx = 0;
+    INT                                 Status = NDIS_STATUS_SUCCESS, MaxPhyMode = PHY_11G;
+    ULONG                               PowerTemp;
+    BOOLEAN                             RadioState;
+    BOOLEAN                             StateMachineTouched = FALSE;
+#ifdef DOT11_N_SUPPORT
+	OID_SET_HT_PHYMODE					HT_PhyMode;	//11n ,kathy
+#endif // DOT11_N_SUPPORT //
+#ifdef WPA_SUPPLICANT_SUPPORT
+    PNDIS_802_11_PMKID                  pPmkId = NULL;
+    BOOLEAN				                IEEE8021xState = FALSE;
+    BOOLEAN				                IEEE8021x_required_keys = FALSE;
+    UCHAR                               wpa_supplicant_enable = 0;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef SNMP_SUPPORT
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						ctmp;
+#endif // SNMP_SUPPORT //
+
+
+#ifdef DOT11_N_SUPPORT
+	MaxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-->RTMPSetInformation(),	0x%08x\n", cmd&0x7FFF));
+    switch(cmd & 0x7FFF) {
+        case RT_OID_802_11_COUNTRY_REGION:
+            if (wrq->u.data.length < sizeof(UCHAR))
+                Status = -EINVAL;
+			// Only avaliable when EEPROM not programming
+            else if (!(pAdapter->CommonCfg.CountryRegion & 0x80) && !(pAdapter->CommonCfg.CountryRegionForABand & 0x80))
+            {
+                ULONG   Country;
+                UCHAR	TmpPhy;
+
+				Status = copy_from_user(&Country, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->CommonCfg.CountryRegion = (UCHAR)(Country & 0x000000FF);
+				pAdapter->CommonCfg.CountryRegionForABand = (UCHAR)((Country >> 8) & 0x000000FF);
+                TmpPhy = pAdapter->CommonCfg.PhyMode;
+				pAdapter->CommonCfg.PhyMode = 0xff;
+				// Build all corresponding channel information
+				RTMPSetPhyMode(pAdapter, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+				SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_COUNTRY_REGION (A:%d  B/G:%d)\n", pAdapter->CommonCfg.CountryRegionForABand,
+				    pAdapter->CommonCfg.CountryRegion));
+            }
+            break;
+        case OID_802_11_BSSID_LIST_SCAN:
+ #ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            Now = jiffies;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID_LIST_SCAN, TxCnt = %d \n", pAdapter->RalinkCounters.LastOneSecTotalTxCount));
+
+            if (MONITOR_ON(pAdapter))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+                break;
+            }
+
+			//Benson add 20080527, when radio off, sta don't need to scan
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_RADIO_OFF))
+				break;
+
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			{
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is scanning now !!!\n"));
+				pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+				Status = NDIS_STATUS_SUCCESS;
+                break;
+            }
+
+			if (pAdapter->RalinkCounters.LastOneSecTotalTxCount > 100)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+            if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+				((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)) &&
+                (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+
+            if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+            {
+                RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+            }
+
+            // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+            // this request, because this request is initiated by NDIS.
+            pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+            // Reset allowed scan retries
+            pAdapter->StaCfg.ScanCnt = 0;
+            pAdapter->StaCfg.LastScanTime = Now;
+
+			pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+            RTMP_SET_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+            MlmeEnqueue(pAdapter,
+                        MLME_CNTL_STATE_MACHINE,
+                        OID_802_11_BSSID_LIST_SCAN,
+                        0,
+                        NULL);
+
+            Status = NDIS_STATUS_SUCCESS;
+            StateMachineTouched = TRUE;
+            break;
+        case OID_802_11_SSID:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_SSID))
+                Status = -EINVAL;
+            else
+            {
+            	PCHAR pSsidString = NULL;
+                Status = copy_from_user(&Ssid, wrq->u.data.pointer, wrq->u.data.length);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SSID (Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+                if (Ssid.SsidLength > MAX_LEN_OF_SSID)
+                    Status = -EINVAL;
+                else
+                {
+                	if (Ssid.SsidLength == 0)
+                	{
+                		Set_SSID_Proc(pAdapter, "");
+                	}
+					else
+                	{
+	                	pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+						if (pSsidString)
+						{
+							NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+							NdisMoveMemory(pSsidString, Ssid.Ssid, Ssid.SsidLength);
+	                		Set_SSID_Proc(pAdapter, pSsidString);
+							kfree(pSsidString);
+						}
+						else
+							Status = -ENOMEM;
+                	}
+                }
+            }
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            if (wrq->u.data.length != sizeof(NDIS_802_11_MAC_ADDRESS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Bssid, wrq->u.data.pointer, wrq->u.data.length);
+
+                // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+                // this request, because this request is initiated by NDIS.
+                pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+
+				// Prevent to connect AP again in STAMlmePeriodicExec
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+                // Reset allowed scan retries
+				pAdapter->StaCfg.ScanCnt = 0;
+
+                if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+                {
+                    RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                    DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+                }
+                MlmeEnqueue(pAdapter,
+                            MLME_CNTL_STATE_MACHINE,
+                            OID_802_11_BSSID,
+                            sizeof(NDIS_802_11_MAC_ADDRESS),
+                            (VOID *)&Bssid);
+                Status = NDIS_STATUS_SUCCESS;
+                StateMachineTouched = TRUE;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID %02x:%02x:%02x:%02x:%02x:%02x\n",
+                                        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+            }
+            break;
+        case RT_OID_802_11_RADIO:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RadioState, wrq->u.data.pointer, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RADIO (=%d)\n", RadioState));
+                if (pAdapter->StaCfg.bSwRadio != RadioState)
+                {
+                    pAdapter->StaCfg.bSwRadio = RadioState;
+                    if (pAdapter->StaCfg.bRadio != (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio))
+                    {
+                        pAdapter->StaCfg.bRadio = (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio);
+                        if (pAdapter->StaCfg.bRadio == TRUE)
+                        {
+                            MlmeRadioOn(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = EXTRA_INFO_CLEAR;
+                        }
+                        else
+                        {
+                            MlmeRadioOff(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = SW_RADIO_OFF;
+                        }
+                    }
+                }
+            }
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PHY_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				if (PhyMode <= MaxPhyMode)
+				{
+                	RTMPSetPhyMode(pAdapter, PhyMode);
+#ifdef DOT11_N_SUPPORT
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				}
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PHY_MODE (=%d)\n", PhyMode));
+            }
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            if (wrq->u.data.length != sizeof(RT_802_11_STA_CONFIG))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&StaConfig, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bEnableTxBurst = StaConfig.EnableTxBurst;
+                pAdapter->CommonCfg.UseBGProtection = StaConfig.UseBGProtection;
+                pAdapter->CommonCfg.bUseShortSlotTime = 1; // 2003-10-30 always SHORT SLOT capable
+                if ((pAdapter->CommonCfg.PhyMode != StaConfig.AdhocMode) &&
+					(StaConfig.AdhocMode <= MaxPhyMode))
+                {
+                    // allow dynamic change of "USE OFDM rate or not" in ADHOC mode
+                    // if setting changed, need to reset current TX rate as well as BEACON frame format
+                    if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                    {
+						pAdapter->CommonCfg.PhyMode = StaConfig.AdhocMode;
+                    	RTMPSetPhyMode(pAdapter, PhyMode);
+                        MlmeUpdateTxRates(pAdapter, FALSE, 0);
+                        MakeIbssBeacon(pAdapter);           // re-build BEACON frame
+                        AsicEnableIbssSync(pAdapter);   // copy to on-chip memory
+                    }
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_STA_CONFIG (Burst=%d, Protection=%ld,ShortSlot=%d\n",
+                                        pAdapter->CommonCfg.bEnableTxBurst,
+                                        pAdapter->CommonCfg.UseBGProtection,
+                                        pAdapter->CommonCfg.bUseShortSlotTime));
+            }
+            break;
+        case OID_802_11_DESIRED_RATES:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RATES))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&aryRates, wrq->u.data.pointer, wrq->u.data.length);
+                NdisZeroMemory(pAdapter->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+                NdisMoveMemory(pAdapter->CommonCfg.DesireRate, &aryRates, sizeof(NDIS_802_11_RATES));
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DESIRED_RATES (%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x)\n",
+                    pAdapter->CommonCfg.DesireRate[0],pAdapter->CommonCfg.DesireRate[1],
+                    pAdapter->CommonCfg.DesireRate[2],pAdapter->CommonCfg.DesireRate[3],
+                    pAdapter->CommonCfg.DesireRate[4],pAdapter->CommonCfg.DesireRate[5],
+                    pAdapter->CommonCfg.DesireRate[6],pAdapter->CommonCfg.DesireRate[7] ));
+                // Changing DesiredRate may affect the MAX TX rate we used to TX frames out
+                MlmeUpdateTxRates(pAdapter, FALSE, 0);
+            }
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PREAMBLE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Preamble, wrq->u.data.pointer, wrq->u.data.length);
+                if (Preamble == Rt802_11PreambleShort)
+                {
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleShort);
+                }
+                else if ((Preamble == Rt802_11PreambleLong) || (Preamble == Rt802_11PreambleAuto))
+                {
+                    // if user wants AUTO, initialize to LONG here, then change according to AP's
+                    // capability upon association.
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleLong);
+                }
+                else
+                {
+                    Status = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PREAMBLE (=%d)\n", Preamble));
+            }
+            break;
+        case OID_802_11_WEP_STATUS:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_WEP_STATUS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&WepStatus, wrq->u.data.pointer, wrq->u.data.length);
+                // Since TKIP, AES, WEP are all supported. It should not have any invalid setting
+                if (WepStatus <= Ndis802_11Encryption3KeyAbsent)
+                {
+                    if (pAdapter->StaCfg.WepStatus != WepStatus)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.WepStatus     = WepStatus;
+                    pAdapter->StaCfg.OrigWepStatus = WepStatus;
+                    pAdapter->StaCfg.PairCipher    = WepStatus;
+                	pAdapter->StaCfg.GroupCipher   = WepStatus;
+                }
+                else
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEP_STATUS (=%d)\n",WepStatus));
+            }
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_AUTHENTICATION_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&AuthMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (AuthMode > Ndis802_11AuthModeMax)
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                else
+                {
+                    if (pAdapter->StaCfg.AuthMode != AuthMode)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.AuthMode = AuthMode;
+                }
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_AUTHENTICATION_MODE (=%d) \n",pAdapter->StaCfg.AuthMode));
+            }
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_INFRASTRUCTURE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&BssType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (BssType == Ndis802_11IBSS)
+					Set_NetworkType_Proc(pAdapter, "Adhoc");
+				else if (BssType == Ndis802_11Infrastructure)
+					Set_NetworkType_Proc(pAdapter, "Infra");
+				else if (BssType == Ndis802_11Monitor)
+					Set_NetworkType_Proc(pAdapter, "Monitor");
+				else
+				{
+					Status  = -EINVAL;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_INFRASTRUCTURE_MODE (unknown)\n"));
+				}
+			}
+			break;
+	 case OID_802_11_REMOVE_WEP:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_WEP\n"));
+            if (wrq->u.data.length != sizeof(NDIS_802_11_KEY_INDEX))
+            {
+				Status = -EINVAL;
+            }
+            else
+            {
+				KeyIdx = *(NDIS_802_11_KEY_INDEX *) wrq->u.data.pointer;
+
+				if (KeyIdx & 0x80000000)
+				{
+					// Should never set default bit when remove key
+					Status = -EINVAL;
+				}
+				else
+				{
+					KeyIdx = KeyIdx & 0x0fffffff;
+					if (KeyIdx >= 4){
+						Status = -EINVAL;
+					}
+					else
+					{
+						pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+						pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+						AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+					}
+				}
+            }
+            break;
+        case RT_OID_802_11_RESET_COUNTERS:
+            NdisZeroMemory(&pAdapter->WlanCounters, sizeof(COUNTER_802_11));
+            NdisZeroMemory(&pAdapter->Counters8023, sizeof(COUNTER_802_3));
+            NdisZeroMemory(&pAdapter->RalinkCounters, sizeof(COUNTER_RALINK));
+            pAdapter->Counters8023.RxNoBuffer   = 0;
+			pAdapter->Counters8023.GoodReceives = 0;
+			pAdapter->Counters8023.RxNoBuffer   = 0;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RESET_COUNTERS \n"));
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RTS_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RtsThresh, wrq->u.data.pointer, wrq->u.data.length);
+                if (RtsThresh > MAX_RTS_THRESHOLD)
+                    Status  = -EINVAL;
+                else
+                    pAdapter->CommonCfg.RtsThreshold = (USHORT)RtsThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_RTS_THRESHOLD (=%ld)\n",RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_FRAGMENTATION_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&FragThresh, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bUseZeroToDisableFragment = FALSE;
+                if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+                {
+                    if (FragThresh == 0)
+                    {
+                        pAdapter->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+                        pAdapter->CommonCfg.bUseZeroToDisableFragment = TRUE;
+                    }
+                    else
+                        Status  = -EINVAL;
+                }
+                else
+                    pAdapter->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_FRAGMENTATION_THRESHOLD (=%ld) \n",FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_POWER_MODE))
+                Status = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PowerMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (PowerMode == Ndis802_11PowerModeCAM)
+                	Set_PSMode_Proc(pAdapter, "CAM");
+                else if (PowerMode == Ndis802_11PowerModeMAX_PSP)
+                	Set_PSMode_Proc(pAdapter, "Max_PSP");
+                else if (PowerMode == Ndis802_11PowerModeFast_PSP)
+					Set_PSMode_Proc(pAdapter, "Fast_PSP");
+                else if (PowerMode == Ndis802_11PowerModeLegacy_PSP)
+					Set_PSMode_Proc(pAdapter, "Legacy_PSP");
+                else
+                    Status = -EINVAL;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_POWER_MODE (=%d)\n",PowerMode));
+            break;
+         case RT_OID_802_11_TX_POWER_LEVEL_1:
+			if (wrq->u.data.length  < sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&PowerTemp, wrq->u.data.pointer, wrq->u.data.length);
+				if (PowerTemp > 100)
+					PowerTemp = 0xffffffff;  // AUTO
+				pAdapter->CommonCfg.TxPowerDefault = PowerTemp; //keep current setting.
+					pAdapter->CommonCfg.TxPowerPercentage = pAdapter->CommonCfg.TxPowerDefault;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			}
+	        break;
+		case OID_802_11_NETWORK_TYPE_IN_USE:
+			if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_TYPE))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&NetType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (NetType == Ndis802_11DS)
+					RTMPSetPhyMode(pAdapter, PHY_11B);
+				else if (NetType == Ndis802_11OFDM24)
+					RTMPSetPhyMode(pAdapter, PHY_11BG_MIXED);
+				else if (NetType == Ndis802_11OFDM5)
+					RTMPSetPhyMode(pAdapter, PHY_11A);
+				else
+					Status = -EINVAL;
+#ifdef DOT11_N_SUPPORT
+				if (Status == NDIS_STATUS_SUCCESS)
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_NETWORK_TYPE_IN_USE (=%d)\n",NetType));
+		    }
+			break;
+        // For WPA PSK PMK key
+        case RT_OID_802_11_ADD_WPA:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!!\n"));
+            }
+            else
+            {
+                if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone) )
+                {
+                    Status = -EOPNOTSUPP;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!! [AuthMode != WPAPSK/WPA2PSK/WPANONE]\n"));
+                }
+                else if ((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone) )     // Only for WPA PSK mode
+				{
+                    NdisMoveMemory(pAdapter->StaCfg.PMK, &pKey->KeyMaterial, pKey->KeyLength);
+                    // Use RaConfig as PSK agent.
+                    // Start STA supplicant state machine
+                    if (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+                        pAdapter->StaCfg.WpaState = SS_START;
+
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+                else
+                {
+                    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_REMOVE_KEY:
+            pRemoveKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pRemoveKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pRemoveKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pRemoveKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!\n"));
+            }
+            else
+            {
+                if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+                {
+                    RTMPWPARemoveKeyProc(pAdapter, pRemoveKey);
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Remove WPA Key!!\n"));
+                }
+                else
+                {
+                    KeyIdx = pRemoveKey->KeyIndex;
+
+                    if (KeyIdx & 0x80000000)
+                    {
+                        // Should never set default bit when remove key
+                        Status  = -EINVAL;
+                        DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(Should never set default bit when remove key)\n"));
+                    }
+                    else
+                    {
+                        KeyIdx = KeyIdx & 0x0fffffff;
+                        if (KeyIdx > 3)
+                        {
+                            Status  = -EINVAL;
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(KeyId[%d] out of range)\n", KeyIdx));
+                        }
+                        else
+                        {
+                            pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+                            pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+                            AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY (id=0x%x, Len=%d-byte)\n", pRemoveKey->KeyIndex, pRemoveKey->Length));
+                        }
+                    }
+                }
+            }
+            kfree(pRemoveKey);
+            break;
+        // New for WPA
+        case OID_802_11_ADD_KEY:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY, Failed!!\n"));
+            }
+            else
+            {
+                RTMPAddKey(pAdapter, pKey);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_CONFIGURATION:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_CONFIGURATION))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Config, wrq->u.data.pointer, wrq->u.data.length);
+                pConfig = &Config;
+
+                if ((pConfig->BeaconPeriod >= 20) && (pConfig->BeaconPeriod <=400))
+                     pAdapter->CommonCfg.BeaconPeriod = (USHORT) pConfig->BeaconPeriod;
+
+                pAdapter->StaActive.AtimWin = (USHORT) pConfig->ATIMWindow;
+                MAP_KHZ_TO_CHANNEL_ID(pConfig->DSConfig, pAdapter->CommonCfg.Channel);
+                //
+				// Save the channel on MlmeAux for CntlOidRTBssidProc used.
+				//
+				pAdapter->MlmeAux.Channel = pAdapter->CommonCfg.Channel;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CONFIGURATION (BeacnPeriod=%ld,AtimW=%ld,Ch=%d)\n",
+                    pConfig->BeaconPeriod, pConfig->ATIMWindow, pAdapter->CommonCfg.Channel));
+                // Config has changed
+                pAdapter->bConfigChanged = TRUE;
+            }
+            break;
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_HT_PHYMODE:
+			if (wrq->u.data.length	!= sizeof(OID_SET_HT_PHYMODE))
+				Status = -EINVAL;
+			else
+			{
+			    POID_SET_HT_PHYMODE	pHTPhyMode = &HT_PhyMode;
+
+				Status = copy_from_user(&HT_PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::pHTPhyMode	(PhyMode = %d,TransmitNo = %d, HtMode =	%d,	ExtOffset =	%d , MCS = %d, BW =	%d,	STBC = %d, SHORTGI = %d) \n",
+				pHTPhyMode->PhyMode, pHTPhyMode->TransmitNo,pHTPhyMode->HtMode,pHTPhyMode->ExtOffset,
+				pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC,	pHTPhyMode->SHORTGI));
+				if (pAdapter->CommonCfg.PhyMode	>= PHY_11ABGN_MIXED)
+					RTMPSetHT(pAdapter,	pHTPhyMode);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_HT_PHYMODE(MCS=%d,BW=%d,SGI=%d,STBC=%d)\n",
+				pAdapter->StaCfg.HTPhyMode.field.MCS, pAdapter->StaCfg.HTPhyMode.field.BW, pAdapter->StaCfg.HTPhyMode.field.ShortGI,
+				pAdapter->StaCfg.HTPhyMode.field.STBC));
+			break;
+#endif // DOT11_N_SUPPORT //
+		case RT_OID_802_11_SET_APSD_SETTING:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				ULONG apsd ;
+				Status = copy_from_user(&apsd, wrq->u.data.pointer,	wrq->u.data.length);
+
+				/*-------------------------------------------------------------------
+				|B31~B7	|	B6~B5	 |	 B4	 |	 B3	 |	B2	 |	B1	 |	   B0		|
+				---------------------------------------------------------------------
+				| Rsvd	| Max SP Len | AC_VO | AC_VI | AC_BK | AC_BE | APSD	Capable	|
+				---------------------------------------------------------------------*/
+				pAdapter->CommonCfg.bAPSDCapable = (apsd & 0x00000001) ? TRUE :	FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BE = ((apsd	& 0x00000002) >> 1)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BK = ((apsd	& 0x00000004) >> 2)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VI = ((apsd	& 0x00000008) >> 3)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VO = ((apsd	& 0x00000010) >> 4)	? TRUE : FALSE;
+				pAdapter->CommonCfg.MaxSPLength	= (UCHAR)((apsd	& 0x00000060) >> 5);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_SETTING (apsd=0x%lx, APSDCap=%d, [BE,BK,VI,VO]=[%d/%d/%d/%d],	MaxSPLen=%d)\n", apsd, pAdapter->CommonCfg.bAPSDCapable,
+					pAdapter->CommonCfg.bAPSDAC_BE,	pAdapter->CommonCfg.bAPSDAC_BK,	pAdapter->CommonCfg.bAPSDAC_VI,	pAdapter->CommonCfg.bAPSDAC_VO,	pAdapter->CommonCfg.MaxSPLength));
+			}
+			break;
+
+		case RT_OID_802_11_SET_APSD_PSM:
+			if (wrq->u.data.length	!= sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				// Driver needs	to notify AP when PSM changes
+				Status = copy_from_user(&pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.pointer, wrq->u.data.length);
+				if (pAdapter->CommonCfg.bAPSDForcePowerSave	!= pAdapter->StaCfg.Psm)
+				{
+					MlmeSetPsmBit(pAdapter,	pAdapter->CommonCfg.bAPSDForcePowerSave);
+					RTMPSendNullFrame(pAdapter,	pAdapter->CommonCfg.TxRate,	TRUE);
+				}
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_PSM (bAPSDForcePowerSave:%d)\n",	pAdapter->CommonCfg.bAPSDForcePowerSave));
+			}
+			break;
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_SET_DLS:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				BOOLEAN	oldvalue = pAdapter->CommonCfg.bDLSCapable;
+				Status = copy_from_user(&pAdapter->CommonCfg.bDLSCapable, wrq->u.data.pointer, wrq->u.data.length);
+				if (oldvalue &&	!pAdapter->CommonCfg.bDLSCapable)
+				{
+					int	i;
+					// tear	down local dls table entry
+					for	(i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+
+					// tear	down peer dls table	entry
+					for	(i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS (=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			}
+			break;
+
+		case RT_OID_802_11_SET_DLS_PARAM:
+			if (wrq->u.data.length	!= sizeof(RT_802_11_DLS_UI))
+				Status = -EINVAL;
+			else
+			{
+				RT_802_11_DLS	Dls;
+
+				NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+				RTMPMoveMemory(&Dls, wrq->u.data.pointer, sizeof(RT_802_11_DLS_UI));
+				MlmeEnqueue(pAdapter,
+							MLME_CNTL_STATE_MACHINE,
+							RT_OID_802_11_SET_DLS_PARAM,
+							sizeof(RT_802_11_DLS),
+							&Dls);
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS_PARAM \n"));
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+		case RT_OID_802_11_SET_WMM:
+			if (wrq->u.data.length	!= sizeof(BOOLEAN))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&pAdapter->CommonCfg.bWmmCapable, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_WMM (=%d)	\n", pAdapter->CommonCfg.bWmmCapable));
+			}
+			break;
+
+		case OID_802_11_DISASSOCIATE:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+			//
+			// Set NdisRadioStateOff to	TRUE, instead of called	MlmeRadioOff.
+			// Later on, NDIS_802_11_BSSID_LIST_EX->NumberOfItems should be	0
+			// when	query OID_802_11_BSSID_LIST.
+			//
+			// TRUE:  NumberOfItems	will set to	0.
+			// FALSE: NumberOfItems	no change.
+			//
+			pAdapter->CommonCfg.NdisRadioStateOff =	TRUE;
+			// Set to immediately send the media disconnect	event
+			pAdapter->MlmeAux.CurrReqIsFromNdis	= TRUE;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DISASSOCIATE	\n"));
+
+			if (INFRA_ON(pAdapter))
+			{
+				if (pAdapter->Mlme.CntlMachine.CurrState !=	CNTL_IDLE)
+				{
+					RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+					DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME	busy, reset	MLME state machine !!!\n"));
+				}
+
+				MlmeEnqueue(pAdapter,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_DISASSOCIATE,
+					0,
+					NULL);
+
+				StateMachineTouched	= TRUE;
+			}
+			break;
+
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_IMME_BA_CAP:
+				if (wrq->u.data.length != sizeof(OID_BACAP_STRUC))
+					Status = -EINVAL;
+				else
+				{
+					OID_BACAP_STRUC Orde ;
+					Status = copy_from_user(&Orde, wrq->u.data.pointer, wrq->u.data.length);
+					if (Orde.Policy > BA_NOTUSE)
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+					}
+					else if (Orde.Policy == BA_NOTUSE)
+					{
+						pAdapter->CommonCfg.BACapability.field.Policy = BA_NOTUSE;
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs= Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+					}
+					else
+					{
+                        pAdapter->CommonCfg.BACapability.field.AutoBA = Orde.AutoBA;
+						pAdapter->CommonCfg.BACapability.field.Policy = IMMED_BA; // we only support immediate BA.
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+
+						if (pAdapter->CommonCfg.BACapability.field.RxBAWinLimit > MAX_RX_REORDERBUF)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = MAX_RX_REORDERBUF;
+
+					}
+
+					pAdapter->CommonCfg.REGBACapability.word = pAdapter->CommonCfg.BACapability.word;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(Orde.AutoBA = %d) (Policy=%d)(ReBAWinLimit=%d)(TxBAWinLimit=%d)(AutoMode=%d)\n",Orde.AutoBA, pAdapter->CommonCfg.BACapability.field.Policy,
+						pAdapter->CommonCfg.BACapability.field.RxBAWinLimit,pAdapter->CommonCfg.BACapability.field.TxBAWinLimit, pAdapter->CommonCfg.BACapability.field.AutoBA));
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(MimoPs = %d)(AmsduEnable = %d) (AmsduSize=%d)(MpduDensity=%d)\n",pAdapter->CommonCfg.DesiredHtPhy.MimoPs, pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable,
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize, pAdapter->CommonCfg.DesiredHtPhy.MpduDensity));
+				}
+
+				break;
+		case RT_OID_802_11_ADD_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, (" Set :: RT_OID_802_11_ADD_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				UCHAR		        index;
+				OID_ADD_BA_ENTRY    BA;
+				MAC_TABLE_ENTRY     *pEntry;
+
+				Status = copy_from_user(&BA, wrq->u.data.pointer, wrq->u.data.length);
+				if (BA.TID > 15)
+				{
+					Status = NDIS_STATUS_INVALID_DATA;
+					break;
+				}
+				else
+				{
+					//BATableInsertEntry
+					//As ad-hoc mode, BA pair is not limited to only BSSID. so add via OID.
+					index = BA.TID;
+					// in ad hoc mode, when adding BA pair, we should insert this entry into MACEntry too
+					pEntry = MacTableLookup(pAdapter, BA.MACAddr);
+					if (!pEntry)
+					{
+						DBGPRINT(RT_DEBUG_TRACE, ("RT_OID_802_11_ADD_IMME_BA. break on no connection.----:%x:%x\n", BA.MACAddr[4], BA.MACAddr[5]));
+						break;
+					}
+					if (BA.IsRecipient == FALSE)
+					{
+					    if (pEntry->bIAmBadAtheros == TRUE)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = 0x10;
+
+						BAOriSessionSetUp(pAdapter, pEntry, index, 0, 100, TRUE);
+					}
+					else
+					{
+						//BATableInsertEntry(pAdapter, pEntry->Aid, BA.MACAddr, 0, 0xffff, BA.TID, BA.nMSDU, BA.IsRecipient);
+					}
+
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_IMME_BA. Rec = %d. Mac = %x:%x:%x:%x:%x:%x . \n",
+						BA.IsRecipient, BA.MACAddr[0], BA.MACAddr[1], BA.MACAddr[2], BA.MACAddr[2]
+						, BA.MACAddr[4], BA.MACAddr[5]));
+				}
+			}
+			break;
+
+		case RT_OID_802_11_TEAR_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				POID_ADD_BA_ENTRY	pBA;
+				MAC_TABLE_ENTRY *pEntry;
+
+				pBA = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+				if (pBA == NULL)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA kmalloc() can't allocate enough memory\n"));
+					Status = NDIS_STATUS_FAILURE;
+				}
+				else
+				{
+					Status = copy_from_user(pBA, wrq->u.data.pointer, wrq->u.data.length);
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA(TID=%d, bAllTid=%d)\n", pBA->TID, pBA->bAllTid));
+
+					if (!pBA->bAllTid && (pBA->TID > NUM_OF_TID))
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+						break;
+					}
+
+					if (pBA->IsRecipient == FALSE)
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						DBGPRINT(RT_DEBUG_TRACE, (" pBA->IsRecipient == FALSE\n"));
+						if (pEntry)
+						{
+							DBGPRINT(RT_DEBUG_TRACE, (" pBA->pEntry\n"));
+							BAOriSessionTearDown(pAdapter, pEntry->Aid, pBA->TID, FALSE, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					else
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						if (pEntry)
+						{
+							BARecSessionTearDown( pAdapter, (UCHAR)pEntry->Aid, pBA->TID, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					kfree(pBA);
+				}
+            }
+            break;
+#endif // DOT11_N_SUPPORT //
+
+        // For WPA_SUPPLICANT to set static wep key
+    	case OID_802_11_ADD_WEP:
+    	    pWepKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+    	    if(pWepKey == NULL)
+            {
+                Status = -ENOMEM;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed!!\n"));
+                break;
+            }
+            Status = copy_from_user(pWepKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (Status)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (length mismatch)!!\n"));
+            }
+            else
+            {
+		        KeyIdx = pWepKey->KeyIndex & 0x0fffffff;
+                // KeyIdx must be 0 ~ 3
+                if (KeyIdx > 4)
+    			{
+                    Status  = -EINVAL;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (KeyIdx must be smaller than 4)!!\n"));
+                }
+                else
+                {
+                    UCHAR CipherAlg = 0;
+                    PUCHAR Key;
+
+                    // set key material and key length
+                    NdisZeroMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, 16);
+                    pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                    NdisMoveMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+
+                    switch(pWepKey->KeyLength)
+                    {
+                        case 5:
+                            CipherAlg = CIPHER_WEP64;
+                            break;
+                        case 13:
+                            CipherAlg = CIPHER_WEP128;
+                            break;
+                        default:
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, only support CIPHER_WEP64(len:5) & CIPHER_WEP128(len:13)!!\n"));
+                            Status = -EINVAL;
+                            break;
+                    }
+                    pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CipherAlg;
+
+                    // Default key for tx (shared key)
+                    if (pWepKey->KeyIndex & 0x80000000)
+                    {
+#ifdef WPA_SUPPLICANT_SUPPORT
+                        // set key material and key length
+                        NdisZeroMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, 16);
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                        NdisMoveMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+                        pAdapter->StaCfg.DesireSharedKeyId = KeyIdx;
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].CipherAlg = CipherAlg;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                        pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+                    }
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    if (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED)
+#endif // WPA_SUPPLICANT_SUPPORT
+                    {
+                        Key = pAdapter->SharedKey[BSS0][KeyIdx].Key;
+
+                        // Set key material and cipherAlg to Asic
+        				AsicAddSharedKeyEntry(pAdapter, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+                        if (pWepKey->KeyIndex & 0x80000000)
+                        {
+                            PMAC_TABLE_ENTRY pEntry = &pAdapter->MacTab.Content[BSSID_WCID];
+                            // Assign group key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, NULL);
+    						// Assign pairwise key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, pEntry);
+                        }
+                    }
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP (id=0x%x, Len=%d-byte), %s\n", pWepKey->KeyIndex, pWepKey->KeyLength, (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED) ? "Port Secured":"Port NOT Secured"));
+				}
+            }
+            kfree(pWepKey);
+            break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+	    case OID_SET_COUNTERMEASURES:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.bBlockAssoc = TRUE;
+                else
+                    // WPA MIC error should block association attempt for 60 seconds
+                    pAdapter->StaCfg.bBlockAssoc = FALSE;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_SET_COUNTERMEASURES bBlockAssoc=%s\n", pAdapter->StaCfg.bBlockAssoc ? "TRUE":"FALSE"));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+			if (wrq->u.data.length != sizeof(UCHAR))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&wpa_supplicant_enable, wrq->u.data.pointer, wrq->u.data.length);
+    			pAdapter->StaCfg.WpaSupplicantUP = wpa_supplicant_enable;
+    			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+			}
+            break;
+        case OID_802_11_DEAUTHENTICATION:
+            if (wrq->u.data.length != sizeof(MLME_DEAUTH_REQ_STRUCT))
+                Status  = -EINVAL;
+            else
+            {
+                MLME_DEAUTH_REQ_STRUCT      *pInfo;
+				MLME_QUEUE_ELEM *MsgElem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+                pInfo = (MLME_DEAUTH_REQ_STRUCT *) MsgElem->Msg;
+                Status = copy_from_user(pInfo, wrq->u.data.pointer, wrq->u.data.length);
+                MlmeDeauthReqAction(pAdapter, MsgElem);
+				kfree(MsgElem);
+
+                if (INFRA_ON(pAdapter))
+                {
+                    LinkDown(pAdapter, FALSE);
+                    pAdapter->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DEAUTHENTICATION (Reason=%d)\n", pInfo->Reason));
+            }
+            break;
+        case OID_802_11_DROP_UNENCRYPTED:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                else
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				NdisAcquireSpinLock(&pAdapter->MacTabLock);
+				pAdapter->MacTab.Content[BSSID_WCID].PortSecured = pAdapter->StaCfg.PortSecured;
+				NdisReleaseSpinLock(&pAdapter->MacTabLock);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DROP_UNENCRYPTED (=%d)\n", enabled));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021xState, wrq->u.data.pointer, wrq->u.data.length);
+		        pAdapter->StaCfg.IEEE8021X = IEEE8021xState;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X (=%d)\n", IEEE8021xState));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X_REQUIRE_KEY:
+			if (wrq->u.data.length != sizeof(BOOLEAN))
+				 Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021x_required_keys, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->StaCfg.IEEE8021x_required_keys = IEEE8021x_required_keys;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X_REQUIRE_KEY (%d)\n", IEEE8021x_required_keys));
+			}
+			break;
+        case OID_802_11_PMKID:
+	        pPmkId = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+	        if(pPmkId == NULL) {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pPmkId, wrq->u.data.pointer, wrq->u.data.length);
+
+	        // check the PMKID information
+	        if (pPmkId->BSSIDInfoCount == 0)
+                NdisZeroMemory(pAdapter->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+	        else
+	        {
+		        PBSSID_INFO	pBssIdInfo;
+		        UINT		BssIdx;
+		        UINT		CachedIdx;
+
+		        for (BssIdx = 0; BssIdx < pPmkId->BSSIDInfoCount; BssIdx++)
+		        {
+			        // point to the indexed BSSID_INFO structure
+			        pBssIdInfo = (PBSSID_INFO) ((PUCHAR) pPmkId + 2 * sizeof(UINT) + BssIdx * sizeof(BSSID_INFO));
+			        // Find the entry in the saved data base.
+			        for (CachedIdx = 0; CachedIdx < pAdapter->StaCfg.SavedPMKNum; CachedIdx++)
+			        {
+				        // compare the BSSID
+				        if (NdisEqualMemory(pBssIdInfo->BSSID, pAdapter->StaCfg.SavedPMK[CachedIdx].BSSID, sizeof(NDIS_802_11_MAC_ADDRESS)))
+					        break;
+			        }
+
+			        // Found, replace it
+			        if (CachedIdx < PMKID_NO)
+			        {
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+				        pAdapter->StaCfg.SavedPMKNum++;
+			        }
+			        // Not found, replace the last one
+			        else
+			        {
+				        // Randomly replace one
+				        CachedIdx = (pBssIdInfo->BSSID[5] % PMKID_NO);
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+			        }
+		        }
+			}
+			if(pPmkId)
+				kfree(pPmkId);
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+#ifdef SNMP_SUPPORT
+		case OID_802_11_SHORTRETRYLIMIT:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ShortRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SHORTRETRYLIMIT (tx_rty_cfg.field.ShortRetryLimit=%d, ShortRetryLimit=%ld)\n", tx_rty_cfg.field.ShortRtyLimit, ShortRetryLimit));
+			}
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT \n"));
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&LongRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT (tx_rty_cfg.field.LongRetryLimit= %d,LongRetryLimit=%ld)\n", tx_rty_cfg.field.LongRtyLimit, LongRetryLimit));
+			}
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE\n"));
+			pKey = kmalloc(wrq->u.data.length, GFP_KERNEL);
+			Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+			//pKey = &WepKey;
+
+			if ( pKey->Length != wrq->u.data.length)
+			{
+				Status = -EINVAL;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE, Failed!!\n"));
+			}
+			KeyIdx = pKey->KeyIndex & 0x0fffffff;
+			DBGPRINT(RT_DEBUG_TRACE,("pKey->KeyIndex =%d, pKey->KeyLength=%d\n", pKey->KeyIndex, pKey->KeyLength));
+
+			// it is a shared key
+			if (KeyIdx > 4)
+				Status = -EINVAL;
+			else
+			{
+				pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, &pKey->KeyMaterial, pKey->KeyLength);
+				if (pKey->KeyIndex & 0x80000000)
+				{
+					// Default key for tx (shared key)
+					pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+				}
+				//RestartAPIsRequired = TRUE;
+			}
+			break;
+
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYID \n"));
+
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+				Status = copy_from_user(&pAdapter->StaCfg.DefaultKeyId, wrq->u.data.pointer, wrq->u.data.length);
+
+			break;
+
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CURRENTCHANNEL \n"));
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ctmp, wrq->u.data.pointer, wrq->u.data.length);
+				sprintf(&ctmp,"%d", ctmp);
+				Set_Channel_Proc(pAdapter, &ctmp);
+			}
+			break;
+#endif
+
+
+
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+
+
+    return Status;
+}
+
+INT RTMPQueryInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_BSSID_LIST_EX           *pBssidList = NULL;
+    PNDIS_WLAN_BSSID_EX                 pBss;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_CONFIGURATION           *pConfiguration = NULL;
+    RT_802_11_LINK_STATUS               *pLinkStatus = NULL;
+    RT_802_11_STA_CONFIG                *pStaConfig = NULL;
+    NDIS_802_11_STATISTICS              *pStatistics = NULL;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    RT_802_11_PREAMBLE                  PreamType;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_MEDIA_STATE                    MediaState;
+    ULONG                               BssBufSize, ulInfo=0, NetworkTypeList[4], apsd = 0;
+    USHORT                              BssLen = 0;
+    PUCHAR                              pBuf = NULL, pPtr;
+    INT                                 Status = NDIS_STATUS_SUCCESS;
+    UINT                                we_version_compiled;
+    UCHAR                               i, Padding = 0;
+    BOOLEAN                             RadioState;
+	UCHAR	driverVersion[8];
+    OID_SET_HT_PHYMODE			        *pHTPhyMode = NULL;
+
+
+#ifdef SNMP_SUPPORT
+	//for snmp, kathy
+	DefaultKeyIdxValue			*pKeyIdxValue;
+	INT							valueLen;
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						tmp[64];
+#endif //SNMP
+
+    switch(cmd)
+    {
+        case RT_OID_DEVICE_NAME:
+            wrq->u.data.length = sizeof(STA_NIC_DEVICE_NAME);
+            Status = copy_to_user(wrq->u.data.pointer, STA_NIC_DEVICE_NAME, wrq->u.data.length);
+            break;
+        case RT_OID_VERSION_INFO:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_VERSION_INFO \n"));
+			wrq->u.data.length = 8*sizeof(UCHAR);
+			sprintf(&driverVersion[0], "%s", STA_DRIVER_VERSION);
+			driverVersion[7] = '\0';
+			if (copy_to_user(wrq->u.data.pointer, &driverVersion, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+#ifdef RALINK_ATE
+		case RT_QUERY_ATE_TXDONE_COUNT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_QUERY_ATE_TXDONE_COUNT \n"));
+			wrq->u.data.length = sizeof(UINT32);
+			if (copy_to_user(wrq->u.data.pointer, &pAdapter->ate.TxDoneCount, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+			break;
+#endif // RALINK_ATE //
+        case OID_802_11_BSSID_LIST:
+            if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+            	/*
+            	 * Still scanning, indicate the caller should try again.
+            	 */
+            	DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (Still scanning)\n"));
+				return -EAGAIN;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (%d BSS returned)\n",pAdapter->ScanTab.BssNr));
+			pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+            // Claculate total buffer size required
+            BssBufSize = sizeof(ULONG);
+
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                // Align pointer to 4 bytes boundary.
+                //Padding = 4 - (pAdapter->ScanTab.BssEntry[i].VarIELen & 0x0003);
+                //if (Padding == 4)
+                //    Padding = 0;
+                BssBufSize += (sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+            }
+
+            // For safety issue, we add 256 bytes just in case
+            BssBufSize += 256;
+            // Allocate the same size as passed from higher layer
+            pBuf = kmalloc(BssBufSize, MEM_ALLOC_FLAG);
+            if(pBuf == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            // Init 802_11_BSSID_LIST_EX structure
+            NdisZeroMemory(pBuf, BssBufSize);
+            pBssidList = (PNDIS_802_11_BSSID_LIST_EX) pBuf;
+            pBssidList->NumberOfItems = pAdapter->ScanTab.BssNr;
+
+            // Calculate total buffer length
+            BssLen = 4; // Consist of NumberOfItems
+            // Point to start of NDIS_WLAN_BSSID_EX
+            // pPtr = pBuf + sizeof(ULONG);
+            pPtr = (PUCHAR) &pBssidList->Bssid[0];
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                pBss = (PNDIS_WLAN_BSSID_EX) pPtr;
+                NdisMoveMemory(&pBss->MacAddress, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+                if ((pAdapter->ScanTab.BssEntry[i].Hidden == 1) && (pAdapter->StaCfg.bShowHiddenSSID == FALSE))
+                {
+                    //
+					// We must return this SSID during 4way handshaking, otherwise Aegis will failed to parse WPA infomation
+					// and then failed to send EAPOl farame.
+					//
+					if ((pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) && (pAdapter->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED))
+					{
+						pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+						NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+					}
+					else
+                    	pBss->Ssid.SsidLength = 0;
+                }
+                else
+                {
+                    pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+                    NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+                }
+                pBss->Privacy = pAdapter->ScanTab.BssEntry[i].Privacy;
+                pBss->Rssi = pAdapter->ScanTab.BssEntry[i].Rssi - pAdapter->BbpRssiToDbmDelta;
+                pBss->NetworkTypeInUse = NetworkTypeInUseSanity(&pAdapter->ScanTab.BssEntry[i]);
+                pBss->Configuration.Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pBss->Configuration.BeaconPeriod = pAdapter->ScanTab.BssEntry[i].BeaconPeriod;
+                pBss->Configuration.ATIMWindow = pAdapter->ScanTab.BssEntry[i].AtimWin;
+
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->ScanTab.BssEntry[i].Channel, pBss->Configuration.DSConfig);
+
+                if (pAdapter->ScanTab.BssEntry[i].BssType == BSS_INFRA)
+                    pBss->InfrastructureMode = Ndis802_11Infrastructure;
+                else
+                    pBss->InfrastructureMode = Ndis802_11IBSS;
+
+                NdisMoveMemory(pBss->SupportedRates, pAdapter->ScanTab.BssEntry[i].SupRate, pAdapter->ScanTab.BssEntry[i].SupRateLen);
+                NdisMoveMemory(pBss->SupportedRates + pAdapter->ScanTab.BssEntry[i].SupRateLen,
+                               pAdapter->ScanTab.BssEntry[i].ExtRate,
+                               pAdapter->ScanTab.BssEntry[i].ExtRateLen);
+
+                if (pAdapter->ScanTab.BssEntry[i].VarIELen == 0)
+                {
+                    pBss->IELength = sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                }
+                else
+                {
+                    pBss->IELength = (ULONG)(sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    NdisMoveMemory(pBss->IEs + sizeof(NDIS_802_11_FIXED_IEs), pAdapter->ScanTab.BssEntry[i].VarIEs, pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr += pAdapter->ScanTab.BssEntry[i].VarIELen;
+                }
+                pBss->Length = (ULONG)(sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+
+#if WIRELESS_EXT < 17
+                if ((BssLen + pBss->Length) < wrq->u.data.length)
+                BssLen += pBss->Length;
+                else
+                {
+                    pBssidList->NumberOfItems = i;
+                    break;
+                }
+#else
+                BssLen += pBss->Length;
+#endif
+            }
+
+#if WIRELESS_EXT < 17
+            wrq->u.data.length = BssLen;
+#else
+            if (BssLen > wrq->u.data.length)
+            {
+                kfree(pBssidList);
+                return -E2BIG;
+            }
+            else
+                wrq->u.data.length = BssLen;
+#endif
+            Status = copy_to_user(wrq->u.data.pointer, pBssidList, BssLen);
+            kfree(pBssidList);
+            break;
+        case OID_802_3_CURRENT_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+            break;
+        case OID_GEN_MEDIA_CONNECT_STATUS:
+            if (pAdapter->IndicateMediaState == NdisMediaStateConnected)
+                MediaState = NdisMediaStateConnected;
+            else
+                MediaState = NdisMediaStateDisconnected;
+
+            wrq->u.data.length = sizeof(NDIS_MEDIA_STATE);
+            Status = copy_to_user(wrq->u.data.pointer, &MediaState, wrq->u.data.length);
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				Status = NDIS_STATUS_RESOURCES;
+				break;
+			}
+#endif // RALINK_ATE //
+            if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+            {
+                Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Bssid, sizeof(NDIS_802_11_MAC_ADDRESS));
+
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID(=EMPTY)\n"));
+                Status = -ENOTCONN;
+            }
+            break;
+        case OID_802_11_SSID:
+			NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+			NdisZeroMemory(Ssid.Ssid, MAX_LEN_OF_SSID);
+            Ssid.SsidLength = pAdapter->CommonCfg.SsidLen;
+			memcpy(Ssid.Ssid, pAdapter->CommonCfg.Ssid,	Ssid.SsidLength);
+            wrq->u.data.length = sizeof(NDIS_802_11_SSID);
+            Status = copy_to_user(wrq->u.data.pointer, &Ssid, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SSID (Len=%d, ssid=%s)\n", Ssid.SsidLength,Ssid.Ssid));
+            break;
+        case RT_OID_802_11_QUERY_LINK_STATUS:
+            pLinkStatus = (RT_802_11_LINK_STATUS *) kmalloc(sizeof(RT_802_11_LINK_STATUS), MEM_ALLOC_FLAG);
+            if (pLinkStatus)
+            {
+                pLinkStatus->CurrTxRate = RateIdTo500Kbps[pAdapter->CommonCfg.TxRate];   // unit : 500 kbps
+                pLinkStatus->ChannelQuality = pAdapter->Mlme.ChannelQuality;
+                pLinkStatus->RxByteCount = pAdapter->RalinkCounters.ReceivedByteCount;
+                pLinkStatus->TxByteCount = pAdapter->RalinkCounters.TransmittedByteCount;
+        		pLinkStatus->CentralChannel = pAdapter->CommonCfg.CentralChannel;
+                wrq->u.data.length = sizeof(RT_802_11_LINK_STATUS);
+                Status = copy_to_user(wrq->u.data.pointer, pLinkStatus, wrq->u.data.length);
+                kfree(pLinkStatus);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS\n"));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_CONFIGURATION:
+            pConfiguration = (NDIS_802_11_CONFIGURATION *) kmalloc(sizeof(NDIS_802_11_CONFIGURATION), MEM_ALLOC_FLAG);
+            if (pConfiguration)
+            {
+                pConfiguration->Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pConfiguration->BeaconPeriod = pAdapter->CommonCfg.BeaconPeriod;
+                pConfiguration->ATIMWindow = pAdapter->StaActive.AtimWin;
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->CommonCfg.Channel, pConfiguration->DSConfig);
+                wrq->u.data.length = sizeof(NDIS_802_11_CONFIGURATION);
+                Status = copy_to_user(wrq->u.data.pointer, pConfiguration, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(BeaconPeriod=%ld,AtimW=%ld,Channel=%d) \n",
+                                        pConfiguration->BeaconPeriod, pConfiguration->ATIMWindow, pAdapter->CommonCfg.Channel));
+				kfree(pConfiguration);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+		case RT_OID_802_11_SNR_0:
+			if ((pAdapter->StaCfg.LastSNR0 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR0) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_SNR_0(0x=%lx)\n", ulInfo));
+			}
+            else
+			    Status = -EFAULT;
+			break;
+		case RT_OID_802_11_SNR_1:
+			if ((pAdapter->Antenna.field.RxPath	> 1) &&
+                (pAdapter->StaCfg.LastSNR1 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR1) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(0x=%lx)\n",ulInfo));
+			}
+			else
+				Status = -EFAULT;
+            DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(pAdapter->StaCfg.LastSNR1=%d)\n",pAdapter->StaCfg.LastSNR1));
+			break;
+        case OID_802_11_RSSI_TRIGGER:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0 - pAdapter->BbpRssiToDbmDelta;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RSSI_TRIGGER(=%ld)\n", ulInfo));
+            break;
+		case OID_802_11_RSSI:
+        case RT_OID_802_11_RSSI:
+			ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+		case RT_OID_802_11_RSSI_1:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi1;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case RT_OID_802_11_RSSI_2:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi2;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case OID_802_11_STATISTICS:
+            pStatistics = (NDIS_802_11_STATISTICS *) kmalloc(sizeof(NDIS_802_11_STATISTICS), MEM_ALLOC_FLAG);
+            if (pStatistics)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS \n"));
+                // add the most up-to-date h/w raw counters into software counters
+			    NICUpdateRawCounters(pAdapter);
+
+                // Sanity check for calculation of sucessful count
+                if (pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart < pAdapter->WlanCounters.RetryCount.QuadPart)
+                    pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+
+                pStatistics->TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart;
+                pStatistics->MulticastTransmittedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastTransmittedFrameCount.QuadPart;
+                pStatistics->FailedCount.QuadPart = pAdapter->WlanCounters.FailedCount.QuadPart;
+                pStatistics->RetryCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+                pStatistics->MultipleRetryCount.QuadPart = pAdapter->WlanCounters.MultipleRetryCount.QuadPart;
+                pStatistics->RTSSuccessCount.QuadPart = pAdapter->WlanCounters.RTSSuccessCount.QuadPart;
+                pStatistics->RTSFailureCount.QuadPart = pAdapter->WlanCounters.RTSFailureCount.QuadPart;
+                pStatistics->ACKFailureCount.QuadPart = pAdapter->WlanCounters.ACKFailureCount.QuadPart;
+                pStatistics->FrameDuplicateCount.QuadPart = pAdapter->WlanCounters.FrameDuplicateCount.QuadPart;
+                pStatistics->ReceivedFragmentCount.QuadPart = pAdapter->WlanCounters.ReceivedFragmentCount.QuadPart;
+                pStatistics->MulticastReceivedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastReceivedFrameCount.QuadPart;
+#ifdef DBG
+                pStatistics->FCSErrorCount = pAdapter->RalinkCounters.RealFcsErrCount;
+#else
+                pStatistics->FCSErrorCount.QuadPart = pAdapter->WlanCounters.FCSErrorCount.QuadPart;
+                pStatistics->FrameDuplicateCount.u.LowPart = pAdapter->WlanCounters.FrameDuplicateCount.u.LowPart / 100;
+#endif
+                wrq->u.data.length = sizeof(NDIS_802_11_STATISTICS);
+                Status = copy_to_user(wrq->u.data.pointer, pStatistics, wrq->u.data.length);
+                kfree(pStatistics);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_GEN_RCV_OK:
+            ulInfo = pAdapter->Counters8023.GoodReceives;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case OID_GEN_RCV_NO_BUFFER:
+            ulInfo = pAdapter->Counters8023.RxNoBuffer;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            ulInfo = (ULONG)pAdapter->CommonCfg.PhyMode;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PHY_MODE (=%ld)\n", ulInfo));
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            pStaConfig = (RT_802_11_STA_CONFIG *) kmalloc(sizeof(RT_802_11_STA_CONFIG), MEM_ALLOC_FLAG);
+            if (pStaConfig)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG\n"));
+                pStaConfig->EnableTxBurst = pAdapter->CommonCfg.bEnableTxBurst;
+                pStaConfig->EnableTurboRate = 0;
+                pStaConfig->UseBGProtection = pAdapter->CommonCfg.UseBGProtection;
+                pStaConfig->UseShortSlotTime = pAdapter->CommonCfg.bUseShortSlotTime;
+                //pStaConfig->AdhocMode = pAdapter->StaCfg.AdhocMode;
+                pStaConfig->HwRadioStatus = (pAdapter->StaCfg.bHwRadio == TRUE) ? 1 : 0;
+                pStaConfig->Rsv1 = 0;
+                pStaConfig->SystemErrorBitmap = pAdapter->SystemErrorBitmap;
+                wrq->u.data.length = sizeof(RT_802_11_STA_CONFIG);
+                Status = copy_to_user(wrq->u.data.pointer, pStaConfig, wrq->u.data.length);
+                kfree(pStaConfig);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            RtsThresh = pAdapter->CommonCfg.RtsThreshold;
+            wrq->u.data.length = sizeof(RtsThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &RtsThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RTS_THRESHOLD(=%ld)\n", RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            FragThresh = pAdapter->CommonCfg.FragmentThreshold;
+            if (pAdapter->CommonCfg.bUseZeroToDisableFragment == TRUE)
+                FragThresh = 0;
+            wrq->u.data.length = sizeof(FragThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &FragThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_FRAGMENTATION_THRESHOLD(=%ld)\n", FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            PowerMode = pAdapter->StaCfg.WindowsPowerMode;
+            wrq->u.data.length = sizeof(PowerMode);
+            Status = copy_to_user(wrq->u.data.pointer, &PowerMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_POWER_MODE(=%d)\n", PowerMode));
+            break;
+        case RT_OID_802_11_RADIO:
+            RadioState = (BOOLEAN) pAdapter->StaCfg.bSwRadio;
+            wrq->u.data.length = sizeof(RadioState);
+            Status = copy_to_user(wrq->u.data.pointer, &RadioState, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_RADIO (=%d)\n", RadioState));
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                BssType = Ndis802_11IBSS;
+            else if (pAdapter->StaCfg.BssType == BSS_INFRA)
+                BssType = Ndis802_11Infrastructure;
+            else if (pAdapter->StaCfg.BssType == BSS_MONITOR)
+                BssType = Ndis802_11Monitor;
+            else
+                BssType = Ndis802_11AutoUnknown;
+
+            wrq->u.data.length = sizeof(BssType);
+            Status = copy_to_user(wrq->u.data.pointer, &BssType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_INFRASTRUCTURE_MODE(=%d)\n", BssType));
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            PreamType = pAdapter->CommonCfg.TxPreamble;
+            wrq->u.data.length = sizeof(PreamType);
+            Status = copy_to_user(wrq->u.data.pointer, &PreamType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PREAMBLE(=%d)\n", PreamType));
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            AuthMode = pAdapter->StaCfg.AuthMode;
+            wrq->u.data.length = sizeof(AuthMode);
+            Status = copy_to_user(wrq->u.data.pointer, &AuthMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_AUTHENTICATION_MODE(=%d)\n", AuthMode));
+            break;
+        case OID_802_11_WEP_STATUS:
+            WepStatus = pAdapter->StaCfg.WepStatus;
+            wrq->u.data.length = sizeof(WepStatus);
+            Status = copy_to_user(wrq->u.data.pointer, &WepStatus, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEP_STATUS(=%d)\n", WepStatus));
+            break;
+        case OID_802_11_TX_POWER_LEVEL:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPower, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_TX_POWER_LEVEL %x\n",pAdapter->CommonCfg.TxPower));
+			break;
+        case RT_OID_802_11_TX_POWER_LEVEL_1:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPowerPercentage, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			break;
+        case OID_802_11_NETWORK_TYPES_SUPPORTED:
+			if ((pAdapter->RfIcType	== RFIC_2850) || (pAdapter->RfIcType ==	RFIC_2750))
+			{
+				NetworkTypeList[0] = 3;                 // NumberOfItems = 3
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+				NetworkTypeList[3] = Ndis802_11OFDM5;   // NetworkType[3] = 11a
+                wrq->u.data.length = 16;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			else
+			{
+				NetworkTypeList[0] = 2;                 // NumberOfItems = 2
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+			    wrq->u.data.length = 12;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_NETWORK_TYPES_SUPPORTED\n"));
+				break;
+	    case OID_802_11_NETWORK_TYPE_IN_USE:
+            wrq->u.data.length = sizeof(ULONG);
+			if (pAdapter->CommonCfg.PhyMode == PHY_11A)
+				ulInfo = Ndis802_11OFDM5;
+			else if ((pAdapter->CommonCfg.PhyMode == PHY_11BG_MIXED) || (pAdapter->CommonCfg.PhyMode == PHY_11G))
+				ulInfo = Ndis802_11OFDM24;
+			else
+				ulInfo = Ndis802_11DS;
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+        case RT_OID_802_11_QUERY_LAST_RX_RATE:
+            ulInfo = (ULONG)pAdapter->LastRxRate;
+            wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_RX_RATE (=%ld)\n", ulInfo));
+			break;
+		case RT_OID_802_11_QUERY_LAST_TX_RATE:
+			//ulInfo = (ULONG)pAdapter->LastTxRate;
+			ulInfo = (ULONG)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_TX_RATE (=%lx)\n", ulInfo));
+			break;
+        case RT_OID_802_11_QUERY_EEPROM_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->EepromVersion, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_QUERY_FIRMWARE_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->FirmwareVersion, wrq->u.data.length);
+			break;
+	    case RT_OID_802_11_QUERY_NOISE_LEVEL:
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->BbpWriteLatch[66], wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_NOISE_LEVEL (=%d)\n", pAdapter->BbpWriteLatch[66]));
+			break;
+	    case RT_OID_802_11_EXTRA_INFO:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->ExtraInfo, wrq->u.data.length);
+	        DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_EXTRA_INFO (=%ld)\n", pAdapter->ExtraInfo));
+	        break;
+	    case RT_OID_WE_VERSION_COMPILED:
+	        wrq->u.data.length = sizeof(UINT);
+	        we_version_compiled = WIRELESS_EXT;
+	        Status = copy_to_user(wrq->u.data.pointer, &we_version_compiled, wrq->u.data.length);
+	        break;
+		case RT_OID_802_11_QUERY_APSD_SETTING:
+			apsd = (pAdapter->CommonCfg.bAPSDCapable | (pAdapter->CommonCfg.bAPSDAC_BE << 1) | (pAdapter->CommonCfg.bAPSDAC_BK << 2)
+				| (pAdapter->CommonCfg.bAPSDAC_VI << 3)	| (pAdapter->CommonCfg.bAPSDAC_VO << 4)	| (pAdapter->CommonCfg.MaxSPLength << 5));
+
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &apsd, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_SETTING (=0x%lx,APSDCap=%d,AC_BE=%d,AC_BK=%d,AC_VI=%d,AC_VO=%d,MAXSPLen=%d)\n",
+				apsd,pAdapter->CommonCfg.bAPSDCapable,pAdapter->CommonCfg.bAPSDAC_BE,pAdapter->CommonCfg.bAPSDAC_BK,pAdapter->CommonCfg.bAPSDAC_VI,pAdapter->CommonCfg.bAPSDAC_VO,pAdapter->CommonCfg.MaxSPLength));
+			break;
+		case RT_OID_802_11_QUERY_APSD_PSM:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_PSM (=%d)\n", pAdapter->CommonCfg.bAPSDForcePowerSave));
+			break;
+		case RT_OID_802_11_QUERY_WMM:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bWmmCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_WMM (=%d)\n",	pAdapter->CommonCfg.bWmmCapable));
+			break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+        case RT_OID_NEW_DRIVER:
+            {
+                UCHAR enabled = 1;
+    	        wrq->u.data.length = sizeof(UCHAR);
+    	        Status = copy_to_user(wrq->u.data.pointer, &enabled, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_NEW_DRIVER (=%d)\n", enabled));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+	        wrq->u.data.length = sizeof(UCHAR);
+	        Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.WpaSupplicantUP, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+        case RT_OID_DRIVER_DEVICE_NAME:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_DRIVER_DEVICE_NAME \n"));
+			wrq->u.data.length = 16;
+			if (copy_to_user(wrq->u.data.pointer, pAdapter->StaCfg.dev_name, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+            break;
+        case RT_OID_802_11_QUERY_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.STBC;
+
+    			pHTPhyMode->ExtOffset = ((pAdapter->CommonCfg.CentralChannel < pAdapter->CommonCfg.Channel) ? (EXTCHA_BELOW) : (EXTCHA_ABOVE));
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_COUNTRY_REGION:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_COUNTRY_REGION \n"));
+			wrq->u.data.length = sizeof(ulInfo);
+            ulInfo = pAdapter->CommonCfg.CountryRegionForABand;
+            ulInfo = (ulInfo << 8)|(pAdapter->CommonCfg.CountryRegion);
+			if (copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_QUERY_DAT_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.HTMODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->StaCfg.DesiredTransmitSetting.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.STBC;
+
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_QUERY_MULTIPLE_CARD_SUPPORT:
+			wrq->u.data.length = sizeof(UCHAR);
+            i = 0;
+#ifdef MULTIPLE_CARD_SUPPORT
+            i = 1;
+#endif // MULTIPLE_CARD_SUPPORT //
+			if (copy_to_user(wrq->u.data.pointer, &i, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_QUERY_MULTIPLE_CARD_SUPPORT(=%d) \n", i));
+            break;
+#ifdef SNMP_SUPPORT
+		case RT_OID_802_11_MAC_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREROUI:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREROUI \n"));
+			wrq->u.data.length = ManufacturerOUI_LEN;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTURERNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTURERNAME \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_RESOURCETYPEIDNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_RESOURCETYPEIDNAME \n"));
+			wrq->u.data.length = strlen(ResourceTypeIdName);
+			Status = copy_to_user(wrq->u.data.pointer, ResourceTypeIdName, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED \n"));
+			ulInfo = 1; // 1 is support wep else 2 is not support.
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_POWERMANAGEMENTMODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_POWERMANAGEMENTMODE \n"));
+			if (pAdapter->StaCfg.Psm == PSMP_ACTION)
+				ulInfo = 1; // 1 is power active else 2 is power save.
+			else
+				ulInfo = 2;
+
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEPDEFAULTKEYVALUE \n"));
+			//KeyIdxValue.KeyIdx = pAd->PortCfg.MBSSID[pAd->IoctlIF].DefaultKeyId;
+			pKeyIdxValue = wrq->u.data.pointer;
+			DBGPRINT(RT_DEBUG_TRACE,("KeyIdxValue.KeyIdx = %d, \n",pKeyIdxValue->KeyIdx));
+			valueLen = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+			NdisMoveMemory(pKeyIdxValue->Value,
+						   &pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key,
+						   valueLen);
+			pKeyIdxValue->Value[valueLen]='\0';
+
+			wrq->u.data.length = sizeof(DefaultKeyIdxValue);
+
+			Status = copy_to_user(wrq->u.data.pointer, pKeyIdxValue, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE,("DefaultKeyId = %d, total len = %d, str len=%d, KeyValue= %02x %02x %02x %02x \n", pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+			pAdapter->SharedKey[BSS0][0].Key[0],
+			pAdapter->SharedKey[BSS0][1].Key[0],
+			pAdapter->SharedKey[BSS0][2].Key[0],
+			pAdapter->SharedKey[BSS0][3].Key[0]));
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPDEFAULTKEYID \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("DefaultKeyId =%d \n", pAdapter->StaCfg.DefaultKeyId));
+			break;
+
+		case RT_OID_802_11_WEPKEYMAPPINGLENGTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPKEYMAPPINGLENGTH \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer,
+									&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+									wrq->u.data.length);
+			break;
+
+		case OID_802_11_SHORTRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SHORTRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			ShortRetryLimit = tx_rty_cfg.field.ShortRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("ShortRetryLimit =%ld,  tx_rty_cfg.field.ShortRetryLimit=%d\n", ShortRetryLimit, tx_rty_cfg.field.ShortRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &ShortRetryLimit, wrq->u.data.length);
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_LONGRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			LongRetryLimit = tx_rty_cfg.field.LongRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("LongRetryLimit =%ld,  tx_rty_cfg.field.LongRtyLimit=%d\n", LongRetryLimit, tx_rty_cfg.field.LongRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &LongRetryLimit, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRODUCTID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRODUCTID \n"));
+
+#ifdef RT2860
+			{
+
+				USHORT  device_id;
+				if (((POS_COOKIE)pAdapter->OS_Cookie)->pci_dev != NULL)
+			    	pci_read_config_word(((POS_COOKIE)pAdapter->OS_Cookie)->pci_dev, PCI_DEVICE_ID, &device_id);
+				else
+					DBGPRINT(RT_DEBUG_TRACE, (" pci_dev = NULL\n"));
+				sprintf(tmp, "%04x %04x\n", NIC_PCI_VENDOR_ID, device_id);
+			}
+#endif // RT2860 //
+			wrq->u.data.length = strlen(tmp);
+			Status = copy_to_user(wrq->u.data.pointer, tmp, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREID \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CURRENTCHANNEL \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			DBGPRINT(RT_DEBUG_TRACE, ("sizeof UCHAR=%d, channel=%d \n", sizeof(UCHAR), pAdapter->CommonCfg.Channel));
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Channel, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+#endif //SNMP_SUPPORT
+
+		case OID_802_11_BUILD_CHANNEL_EX:
+			{
+				UCHAR value;
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BUILD_CHANNEL_EX \n"));
+				wrq->u.data.length = sizeof(UCHAR);
+#ifdef EXT_BUILD_CHANNEL_LIST
+				DBGPRINT(RT_DEBUG_TRACE, ("Support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 1;
+#else
+				DBGPRINT(RT_DEBUG_TRACE, ("Doesn't support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 0;
+#endif // EXT_BUILD_CHANNEL_LIST //
+				Status = copy_to_user(wrq->u.data.pointer, &value, 1);
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			}
+			break;
+
+		case OID_802_11_GET_CH_LIST:
+			{
+				PRT_CHANNEL_LIST_INFO pChListBuf;
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CH_LIST \n"));
+				if (pAdapter->ChannelListNum == 0)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf = (RT_CHANNEL_LIST_INFO *) kmalloc(sizeof(RT_CHANNEL_LIST_INFO), MEM_ALLOC_FLAG);
+				if (pChListBuf == NULL)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf->ChannelListNum = pAdapter->ChannelListNum;
+				for (i = 0; i < pChListBuf->ChannelListNum; i++)
+					pChListBuf->ChannelList[i] = pAdapter->ChannelList[i].Channel;
+
+				wrq->u.data.length = sizeof(RT_CHANNEL_LIST_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pChListBuf, sizeof(RT_CHANNEL_LIST_INFO));
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+
+				if (pChListBuf)
+					kfree(pChListBuf);
+			}
+			break;
+
+		case OID_802_11_GET_COUNTRY_CODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_COUNTRY_CODE \n"));
+			wrq->u.data.length = 2;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.CountryCode, 2);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+		case OID_802_11_GET_CHANNEL_GEOGRAPHY:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CHANNEL_GEOGRAPHY \n"));
+			wrq->u.data.length = 1;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Geography, 1);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_QUERY_DLS:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bDLSCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS(=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			break;
+
+		case RT_OID_802_11_QUERY_DLS_PARAM:
+			{
+				PRT_802_11_DLS_INFO	pDlsInfo = kmalloc(sizeof(RT_802_11_DLS_INFO), GFP_ATOMIC);
+				if (pDlsInfo == NULL)
+					break;
+
+				for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+				{
+					RTMPMoveMemory(&pDlsInfo->Entry[i], &pAdapter->StaCfg.DLSEntry[i], sizeof(RT_802_11_DLS_UI));
+				}
+
+				pDlsInfo->num = MAX_NUM_OF_DLS_ENTRY;
+				wrq->u.data.length = sizeof(RT_802_11_DLS_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pDlsInfo, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS_PARAM\n"));
+
+				if (pDlsInfo)
+					kfree(pDlsInfo);
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+    return Status;
+}
+
+INT rt28xx_sta_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT					cmd)
+{
+	POS_COOKIE			pObj;
+	VIRTUAL_ADAPTER		*pVirtualAd = NULL;
+	RTMP_ADAPTER        *pAd = NULL;
+	struct iwreq        *wrq = (struct iwreq *) rq;
+	BOOLEAN				StateMachineTouched = FALSE;
+	INT					Status = NDIS_STATUS_SUCCESS;
+	USHORT				subcmd;
+
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		pAd = net_dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = net_dev->ml_priv;
+		pAd = pVirtualAd->RtmpDev->ml_priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	    if (wrq->u.data.pointer == NULL)
+	    {
+		    return Status;
+	    }
+
+	    if (strstr(wrq->u.data.pointer, "OpMode") == NULL)
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		    return -ENETDOWN;
+        }
+    }
+
+	{	// determine this ioctl command is comming from which interface.
+		pObj->ioctl_if_type = INT_MAIN;
+		pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	switch(cmd)
+	{
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+		case RTPRIV_IOCTL_ATE:
+			{
+				RtmpDoAte(pAd, wrq);
+			}
+			break;
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+        case SIOCGIFHWADDR:
+			DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIFHWADDR\n"));
+			memcpy(wrq->u.name, pAd->CurrentAddress, ETH_ALEN);
+			break;
+		case SIOCGIWNAME:
+        {
+        	char *name=&wrq->u.name[0];
+        	rt_ioctl_giwname(net_dev, NULL, name, NULL);
+			break;
+		}
+		case SIOCGIWESSID:  //Get ESSID
+        {
+        	struct iw_point *essid=&wrq->u.essid;
+        	rt_ioctl_giwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWESSID:  //Set ESSID
+        {
+        	struct iw_point	*essid=&wrq->u.essid;
+        	rt_ioctl_siwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWNWID:   // set network id (the cell)
+		case SIOCGIWNWID:   // get network id
+			Status = -EOPNOTSUPP;
+			break;
+		case SIOCSIWFREQ:   //set channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_siwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCGIWFREQ:   // get channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_giwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCSIWNICKN: //set node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_siwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWNICKN: //get node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_giwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWRATE:   //get default bit rate (bps)
+		    rt_ioctl_giwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+	    case SIOCSIWRATE:  //set default bit rate (bps)
+	        rt_ioctl_siwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+        case SIOCGIWRTS:  // get RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_giwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCSIWRTS:  //set RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_siwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCGIWFRAG:  //get fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_giwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCSIWFRAG:  //set fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_siwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCGIWENCODE:  //get encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_giwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+        case SIOCSIWENCODE:  //set encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_siwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+		case SIOCGIWAP:     //get access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_giwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+	    case SIOCSIWAP:  //set access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_siwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+		case SIOCGIWMODE:   //get operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_giwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCSIWMODE:   //set operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_siwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCGIWSENS:   //get sensitivity (dBm)
+		case SIOCSIWSENS:	//set sensitivity (dBm)
+		case SIOCGIWPOWER:  //get Power Management settings
+		case SIOCSIWPOWER:  //set Power Management settings
+		case SIOCGIWTXPOW:  //get transmit power (dBm)
+		case SIOCSIWTXPOW:  //set transmit power (dBm)
+		case SIOCGIWRANGE:	//Get range of parameters
+		case SIOCGIWRETRY:	//get retry limits and lifetime
+		case SIOCSIWRETRY:	//set retry limits and lifetime
+			Status = -EOPNOTSUPP;
+			break;
+		case RT_PRIV_IOCTL:
+			subcmd = wrq->u.data.flags;
+			if( subcmd & OID_GET_SET_TOGGLE)
+				Status = RTMPSetInformation(pAd, rq, subcmd);
+			else
+				Status = RTMPQueryInformation(pAd, rq, subcmd);
+			break;
+		case SIOCGIWPRIV:
+			if (wrq->u.data.pointer)
+			{
+				if ( access_ok(VERIFY_WRITE, wrq->u.data.pointer, sizeof(privtab)) != TRUE)
+					break;
+				wrq->u.data.length = sizeof(privtab) / sizeof(privtab[0]);
+				if (copy_to_user(wrq->u.data.pointer, privtab, sizeof(privtab)))
+					Status = -EFAULT;
+			}
+			break;
+		case RTPRIV_IOCTL_SET:
+			if(access_ok(VERIFY_READ, wrq->u.data.pointer, wrq->u.data.length) != TRUE)
+				break;
+			rt_ioctl_setparam(net_dev, NULL, NULL, wrq->u.data.pointer);
+			break;
+		case RTPRIV_IOCTL_GSITESURVEY:
+			RTMPIoctlGetSiteSurvey(pAd, wrq);
+		    break;
+#ifdef DBG
+		case RTPRIV_IOCTL_MAC:
+			RTMPIoctlMAC(pAd, wrq);
+			break;
+		case RTPRIV_IOCTL_E2P:
+			RTMPIoctlE2PROM(pAd, wrq);
+			break;
+#endif // DBG //
+        case SIOCETHTOOL:
+                break;
+		default:
+			DBGPRINT(RT_DEBUG_ERROR, ("IOCTL::unknown IOCTL's cmd = 0x%08x\n", cmd));
+			Status = -EOPNOTSUPP;
+			break;
+	}
+
+    if(StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAd);
+
+	return Status;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set SSID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    NDIS_802_11_SSID                    Ssid, *pSsid=NULL;
+    BOOLEAN                             StateMachineTouched = FALSE;
+    int                                 success = TRUE;
+
+    if( strlen(arg) <= MAX_LEN_OF_SSID)
+    {
+        NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+        if (strlen(arg) != 0)
+        {
+            NdisMoveMemory(Ssid.Ssid, arg, strlen(arg));
+            Ssid.SsidLength = strlen(arg);
+        }
+        else   //ANY ssid
+        {
+            Ssid.SsidLength = 0;
+		    memcpy(Ssid.Ssid, "", 0);
+			pAdapter->StaCfg.BssType = BSS_INFRA;
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+	        pAdapter->StaCfg.WepStatus  = Ndis802_11EncryptionDisabled;
+		}
+        pSsid = &Ssid;
+
+        if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+        {
+            RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+        }
+
+        pAdapter->MlmeAux.CurrReqIsFromNdis = TRUE;
+        pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+		pAdapter->bConfigChanged = TRUE;
+
+        MlmeEnqueue(pAdapter,
+                    MLME_CNTL_STATE_MACHINE,
+                    OID_802_11_SSID,
+                    sizeof(NDIS_802_11_SSID),
+                    (VOID *)pSsid);
+
+        StateMachineTouched = TRUE;
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_SSID_Proc::(Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+    }
+    else
+        success = FALSE;
+
+    if (StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAdapter);
+
+    return success;
+}
+
+#ifdef WMM_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WmmCapable Enable or Disable
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	BOOLEAN	bWmmCapable;
+
+	bWmmCapable = simple_strtol(arg, 0, 10);
+
+	if ((bWmmCapable == 1)
+		)
+		pAd->CommonCfg.bWmmCapable = TRUE;
+	else if (bWmmCapable == 0)
+		pAd->CommonCfg.bWmmCapable = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_WmmCapable_Proc::(bWmmCapable=%d)\n",
+		pAd->CommonCfg.bWmmCapable));
+
+	return TRUE;
+}
+#endif // WMM_SUPPORT //
+
+/*
+    ==========================================================================
+    Description:
+        Set Network Type(Infrastructure/Adhoc mode)
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UINT32	Value = 0;
+
+    if (strcmp(arg, "Adhoc") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_ADHOC)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (INFRA_ON(pAdapter))
+			{
+				//BOOLEAN Cancelled;
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event BB!\n"));
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_ADHOC;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(AD-HOC)\n"));
+	}
+    else if (strcmp(arg, "Infra") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_INFRA)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (ADHOC_ON(pAdapter))
+			{
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_INFRA;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(INFRA)\n"));
+
+        pAdapter->StaCfg.BssType = BSS_INFRA;
+	}
+    else if (strcmp(arg, "Monitor") == 0)
+    {
+		UCHAR	bbpValue = 0;
+		BCN_TIME_CFG_STRUC csr;
+		OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_INFRA_ON);
+        OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+		// disable all periodic state machine
+		pAdapter->StaCfg.bAutoReconnect = FALSE;
+		// reset all mlme state machine
+		RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+		DBGPRINT(RT_DEBUG_TRACE, ("fOP_STATUS_MEDIA_STATE_CONNECTED \n"));
+        if (pAdapter->CommonCfg.CentralChannel == 0)
+        {
+#ifdef DOT11_N_SUPPORT
+            if (pAdapter->CommonCfg.PhyMode == PHY_11AN_MIXED)
+                pAdapter->CommonCfg.CentralChannel = 36;
+            else
+#endif // DOT11_N_SUPPORT //
+                pAdapter->CommonCfg.CentralChannel = 6;
+        }
+#ifdef DOT11_N_SUPPORT
+        else
+            N_ChannelCheck(pAdapter);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_ABOVE)
+		{
+			// 40MHz ,control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			//  RX : control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue &= (~0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value &= 0xfffffffe;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel + 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_BELOW)
+		{
+			// 40MHz ,control channel at upper
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value |= 0x1;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue |= (0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel - 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			// 20MHz
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_20;
+			AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAdapter, pAdapter->CommonCfg.Channel);
+			DBGPRINT(RT_DEBUG_TRACE, ("BW_20, Channel(%d)\n", pAdapter->CommonCfg.Channel));
+		}
+		// Enable Rx with promiscuous reception
+		RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, 0x3);
+		// ASIC supporsts sniffer function with replacing RSSI with timestamp.
+		//RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+		//Value |= (0x80);
+		//RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+		// disable sync
+		RTMP_IO_READ32(pAdapter, BCN_TIME_CFG, &csr.word);
+		csr.field.bBeaconGen = 0;
+		csr.field.bTBTTEnable = 0;
+		csr.field.TsfSyncMode = 0;
+		RTMP_IO_WRITE32(pAdapter, BCN_TIME_CFG, csr.word);
+
+		pAdapter->StaCfg.BssType = BSS_MONITOR;
+        pAdapter->net_dev->type = ARPHRD_IEEE80211_PRISM; //ARPHRD_IEEE80211; // IEEE80211
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(MONITOR)\n"));
+    }
+
+    // Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_NetworkType_Proc::(NetworkType=%d)\n", pAdapter->StaCfg.BssType));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Authentication mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "WEPAUTO") == 0) || (strcmp(arg, "wepauto") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeAutoSwitch;
+    else if ((strcmp(arg, "OPEN") == 0) || (strcmp(arg, "open") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+    else if ((strcmp(arg, "SHARED") == 0) || (strcmp(arg, "shared") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    else if ((strcmp(arg, "WPAPSK") == 0) || (strcmp(arg, "wpapsk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+    else if ((strcmp(arg, "WPANONE") == 0) || (strcmp(arg, "wpanone") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+    else if ((strcmp(arg, "WPA2PSK") == 0) || (strcmp(arg, "wpa2psk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+#ifdef WPA_SUPPLICANT_SUPPORT
+    else if ((strcmp(arg, "WPA") == 0) || (strcmp(arg, "wpa") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+    else if ((strcmp(arg, "WPA2") == 0) || (strcmp(arg, "wpa2") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#endif // WPA_SUPPLICANT_SUPPORT //
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_AuthMode_Proc::(AuthMode=%d)\n", pAdapter->StaCfg.AuthMode));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Encryption Type
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "NONE") == 0) || (strcmp(arg, "none") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPDisabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPDisabled;
+    }
+    else if ((strcmp(arg, "WEP") == 0) || (strcmp(arg, "wep") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPEnabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPEnabled;
+    }
+    else if ((strcmp(arg, "TKIP") == 0) || (strcmp(arg, "tkip") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption2Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption2Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption2Enabled;
+    }
+    else if ((strcmp(arg, "AES") == 0) || (strcmp(arg, "aes") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption3Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption3Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption3Enabled;
+    }
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_EncrypType_Proc::(EncrypType=%d)\n", pAdapter->StaCfg.WepStatus));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Default Key ID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    ULONG                               KeyIdx;
+
+    KeyIdx = simple_strtol(arg, 0, 10);
+    if((KeyIdx >= 1 ) && (KeyIdx <= 4))
+        pAdapter->StaCfg.DefaultKeyId = (UCHAR) (KeyIdx - 1 );
+    else
+        return FALSE;  //Invalid argument
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_DefaultKeyID_Proc::(DefaultKeyID=%d)\n", pAdapter->StaCfg.DefaultKeyId));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY1
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+
+    pAdapter->SharedKey[BSS0][0].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              0,
+                              pAdapter->SharedKey[BSS0][0].CipherAlg,
+                              pAdapter->SharedKey[BSS0][0].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+
+    Description:
+        Set WEP KEY2
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][1].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              1,
+                              pAdapter->SharedKey[BSS0][1].CipherAlg,
+                              pAdapter->SharedKey[BSS0][1].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY3
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][2].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              2,
+                              pAdapter->SharedKey[BSS0][2].CipherAlg,
+                              pAdapter->SharedKey[BSS0][2].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY4
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][3].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              3,
+                              pAdapter->SharedKey[BSS0][3].CipherAlg,
+                              pAdapter->SharedKey[BSS0][3].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WPA PSK key
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UCHAR                   keyMaterial[40];
+
+    if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+        (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+	    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+		)
+        return TRUE;    // do nothing
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_WPAPSK_Proc::(WPAPSK=%s)\n", arg));
+
+    NdisZeroMemory(keyMaterial, 40);
+
+    if ((strlen(arg) < 8) || (strlen(arg) > 64))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("Set failed!!(WPAPSK=%s), WPAPSK key-string required 8 ~ 64 characters \n", arg));
+        return FALSE;
+    }
+
+    if (strlen(arg) == 64)
+    {
+        AtoH(arg, keyMaterial, 32);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+
+    }
+    else
+    {
+        PasswordHash((char *)arg, pAdapter->MlmeAux.Ssid, pAdapter->MlmeAux.SsidLen, keyMaterial);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+    }
+
+
+
+    if(pAdapter->StaCfg.BssType == BSS_ADHOC &&
+       pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+    {
+         pAdapter->StaCfg.WpaState = SS_NOTUSE;
+    }
+    else
+    {
+        // Start STA supplicant state machine
+        pAdapter->StaCfg.WpaState = SS_START;
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Power Saving mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (pAdapter->StaCfg.BssType == BSS_INFRA)
+    {
+        if ((strcmp(arg, "Max_PSP") == 0) ||
+			(strcmp(arg, "max_psp") == 0) ||
+			(strcmp(arg, "MAX_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeMAX_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeMAX_PSP;
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            pAdapter->StaCfg.DefaultListenCount = 5;
+
+        }
+        else if ((strcmp(arg, "Fast_PSP") == 0) ||
+				 (strcmp(arg, "fast_psp") == 0) ||
+                 (strcmp(arg, "FAST_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else if ((strcmp(arg, "Legacy_PSP") == 0) ||
+                 (strcmp(arg, "legacy_psp") == 0) ||
+                 (strcmp(arg, "LEGACY_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else
+        {
+            //Default Ndis802_11PowerModeCAM
+            // clear PSM bit immediately
+            MlmeSetPsmBit(pAdapter, PWR_ACTIVE);
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+        }
+
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_PSMode_Proc::(PSMode=%ld)\n", pAdapter->StaCfg.WindowsPowerMode));
+    }
+    else
+        return FALSE;
+
+
+    return TRUE;
+}
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WpaSupport flag.
+    Value:
+        0: Driver ignore wpa_supplicant.
+        1: wpa_supplicant initiates scanning and AP selection.
+        2: driver takes care of scanning, AP selection, and IEEE 802.11 association parameters.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+    if ( simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+    else if ( simple_strtol(arg, 0, 10) == 1)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE;
+    else if ( simple_strtol(arg, 0, 10) == 2)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE_WITH_WEB_UI;
+    else
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Wpa_Support::(WpaSupplicantUP=%d)\n", pAd->StaCfg.WpaSupplicantUP));
+
+    return TRUE;
+}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+/*
+    ==========================================================================
+    Description:
+        Read / Write MAC
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 mac 0        ==> read MAC where Addr=0x0
+               2.) iwpriv ra0 mac 0=12     ==> write MAC where Addr=0x0, value=12
+    ==========================================================================
+*/
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	ULONG				macAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	UINT32				macValue = 0;
+	INT					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+		//Parsing Read or Write
+	    this_char = arg;
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// Mac Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+				if (macAddr < 0xFFFF)
+				{
+					RTMP_IO_READ32(pAdapter, macAddr, &macValue);
+					DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%lx, MacValue=%x\n", macAddr, macValue));
+					sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr , macValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[8-k+j] = temp2[j];
+			}
+
+			while(k < 8)
+				temp2[7-k++]='0';
+			temp2[8]='\0';
+
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+
+				AtoH(temp2, temp, 4);
+				macValue = *temp*256*256*256 + temp[1]*256*256 + temp[2]*256 + temp[3];
+
+				// debug mode
+				if (macAddr == (HW_DEBUG_SETTING_BASE + 4))
+				{
+					// 0x2bf4: byte0 non-zero: enable R17 tuning, 0: disable R17 tuning
+                    if (macValue & 0x000000ff)
+                    {
+                        pAdapter->BbpTuning.bEnable = TRUE;
+                        DBGPRINT(RT_DEBUG_TRACE,("turn on R17 tuning\n"));
+                    }
+                    else
+                    {
+                        UCHAR R66;
+                        pAdapter->BbpTuning.bEnable = FALSE;
+                        R66 = 0x26 + GET_LNA_GAIN(pAdapter);
+#ifdef RALINK_ATE
+						if (ATE_ON(pAdapter))
+						{
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+						}
+						else
+#endif // RALINK_ATE //
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+                        DBGPRINT(RT_DEBUG_TRACE,("turn off R17 tuning, restore to 0x%02x\n", R66));
+                    }
+					return;
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%02lx, MacValue=0x%x\n", macAddr, macValue));
+
+				RTMP_IO_WRITE32(pAdapter, macAddr, macValue);
+				sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr, macValue);
+			}
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlMAC\n\n"));
+}
+
+/*
+    ==========================================================================
+    Description:
+        Read / Write E2PROM
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 e2p 0     	==> read E2PROM where Addr=0x0
+               2.) iwpriv ra0 e2p 0=1234    ==> write E2PROM where Addr=0x0, value=1234
+    ==========================================================================
+*/
+VOID RTMPIoctlE2PROM(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	USHORT				eepAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	USHORT				eepValue;
+	int					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+	    //Parsing Read or Write
+		this_char = arg;
+
+
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// E2PROM addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				eepAddr = *temp*256 + temp[1];
+				if (eepAddr < 0xFFFF)
+				{
+					RT28xx_EEPROM_READ16(pAdapter, eepAddr, eepValue);
+					sprintf(msg+strlen(msg), "[0x%04X]:0x%04X  ", eepAddr , eepValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[4-k+j] = temp2[j];
+			}
+
+			while(k < 4)
+				temp2[3-k++]='0';
+			temp2[4]='\0';
+
+			AtoH(this_char, temp, 2);
+			eepAddr = *temp*256 + temp[1];
+
+			AtoH(temp2, temp, 2);
+			eepValue = *temp*256 + temp[1];
+
+			RT28xx_EEPROM_WRITE16(pAdapter, eepAddr, eepValue);
+			sprintf(msg+strlen(msg), "[0x%02X]:%02X  ", eepAddr, eepValue);
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlE2PROM\n"));
+}
+#endif // DBG //
+
+
+
+
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.bTGnWifiTest = FALSE;
+    else
+        pAd->StaCfg.bTGnWifiTest = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_TGnWifiTest_Proc::(bTGnWifiTest=%d)\n", pAd->StaCfg.bTGnWifiTest));
+	return TRUE;
+}
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				LongRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_LongRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				ShortRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_ShortRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_None;
+    else if (simple_strtol(arg, 0, 10) == 1)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Flexible;
+    else if (simple_strtol(arg, 0, 10) == 2)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Strict;
+    else
+        return FALSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Ieee802dMode_Proc::(IEEEE0211dMode=%d)\n", pAdapter->StaCfg.IEEE80211dClientMode));
+    return TRUE;
+}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+    else
+        pAd->CommonCfg.CarrierDetect.Enable = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_CarrierDetect_Proc::(CarrierDetect.Enable=%d)\n", pAd->CommonCfg.CarrierDetect.Enable));
+	return TRUE;
+}
+#endif // CARRIER_DETECTION_SUPPORT //
+
diff --git a/drivers/staging/rt2860/wpa.h b/drivers/staging/rt2860/wpa.h
new file mode 100644
index 000000000000..88c7c8bf3fcd
--- /dev/null
+++ b/drivers/staging/rt2860/wpa.h
@@ -0,0 +1,356 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	wpa.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+
+#ifndef	__WPA_H__
+#define	__WPA_H__
+
+// EAPOL Key descripter frame format related length
+#define LEN_KEY_DESC_NONCE			32
+#define LEN_KEY_DESC_IV				16
+#define LEN_KEY_DESC_RSC			8
+#define LEN_KEY_DESC_ID				8
+#define LEN_KEY_DESC_REPLAY			8
+#define LEN_KEY_DESC_MIC			16
+
+// The length is the EAPoL-Key frame except key data field.
+// Please refer to 802.11i-2004 ,Figure 43u in p.78
+#define LEN_EAPOL_KEY_MSG			(sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE)
+
+// EAP Code Type.
+#define EAP_CODE_REQUEST	1
+#define EAP_CODE_RESPONSE	2
+#define EAP_CODE_SUCCESS    3
+#define EAP_CODE_FAILURE    4
+
+// EAPOL frame Protocol Version
+#define	EAPOL_VER					1
+#define	EAPOL_VER2					2
+
+// EAPOL-KEY Descriptor Type
+#define	WPA1_KEY_DESC				0xfe
+#define WPA2_KEY_DESC               0x02
+
+// Key Descriptor Version of Key Information
+#define	DESC_TYPE_TKIP				1
+#define	DESC_TYPE_AES				2
+#define DESC_TYPE_MESH				3
+
+#define LEN_MSG1_2WAY               0x7f
+#define MAX_LEN_OF_EAP_HS           256
+
+#define LEN_MASTER_KEY				32
+
+// EAPOL EK, MK
+#define LEN_EAP_EK					16
+#define LEN_EAP_MICK				16
+#define LEN_EAP_KEY					((LEN_EAP_EK)+(LEN_EAP_MICK))
+// TKIP key related
+#define LEN_PMKID					16
+#define LEN_TKIP_EK					16
+#define LEN_TKIP_RXMICK				8
+#define LEN_TKIP_TXMICK				8
+#define LEN_AES_EK					16
+#define LEN_AES_KEY					LEN_AES_EK
+#define LEN_TKIP_KEY				((LEN_TKIP_EK)+(LEN_TKIP_RXMICK)+(LEN_TKIP_TXMICK))
+#define TKIP_AP_TXMICK_OFFSET		((LEN_EAP_KEY)+(LEN_TKIP_EK))
+#define TKIP_AP_RXMICK_OFFSET		(TKIP_AP_TXMICK_OFFSET+LEN_TKIP_TXMICK)
+#define TKIP_GTK_LENGTH				((LEN_TKIP_EK)+(LEN_TKIP_RXMICK)+(LEN_TKIP_TXMICK))
+#define LEN_PTK						((LEN_EAP_KEY)+(LEN_TKIP_KEY))
+
+// RSN IE Length definition
+#define MAX_LEN_OF_RSNIE         	90
+#define MIN_LEN_OF_RSNIE         	8
+
+//EAP Packet Type
+#define	EAPPacket		0
+#define	EAPOLStart		1
+#define	EAPOLLogoff		2
+#define	EAPOLKey		3
+#define	EAPOLASFAlert	4
+#define	EAPTtypeMax		5
+
+#define	EAPOL_MSG_INVALID	0
+#define	EAPOL_PAIR_MSG_1	1
+#define	EAPOL_PAIR_MSG_2	2
+#define	EAPOL_PAIR_MSG_3	3
+#define	EAPOL_PAIR_MSG_4	4
+#define	EAPOL_GROUP_MSG_1	5
+#define	EAPOL_GROUP_MSG_2	6
+
+#define PAIRWISEKEY					1
+#define GROUPKEY					0
+
+// Retry timer counter initial value
+#define PEER_MSG1_RETRY_TIMER_CTR           0
+#define PEER_MSG3_RETRY_TIMER_CTR           10
+#define GROUP_MSG1_RETRY_TIMER_CTR          20
+
+
+#define EAPOL_START_DISABLE					0
+#define EAPOL_START_PSK						1
+#define EAPOL_START_1X						2
+
+#define MIX_CIPHER_WPA_TKIP_ON(x)       (((x) & 0x08) != 0)
+#define MIX_CIPHER_WPA_AES_ON(x)        (((x) & 0x04) != 0)
+#define MIX_CIPHER_WPA2_TKIP_ON(x)      (((x) & 0x02) != 0)
+#define MIX_CIPHER_WPA2_AES_ON(x)       (((x) & 0x01) != 0)
+
+#define ROUND_UP(__x, __y) \
+	(((ULONG)((__x)+((__y)-1))) & ((ULONG)~((__y)-1)))
+
+#define	ADD_ONE_To_64BIT_VAR(_V)		\
+{										\
+	UCHAR	cnt = LEN_KEY_DESC_REPLAY;	\
+	do									\
+	{									\
+		cnt--;							\
+		_V[cnt]++;						\
+		if (cnt == 0)					\
+			break;						\
+	}while (_V[cnt] == 0);				\
+}
+
+#define IS_WPA_CAPABILITY(a)       (((a) >= Ndis802_11AuthModeWPA) && ((a) <= Ndis802_11AuthModeWPA1PSKWPA2PSK))
+
+// EAPOL Key Information definition within Key descriptor format
+typedef	struct PACKED _KEY_INFO
+{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	KeyAck:1;
+    UCHAR	Install:1;
+    UCHAR	KeyIndex:2;
+    UCHAR	KeyType:1;
+    UCHAR	KeyDescVer:3;
+    UCHAR	Rsvd:3;
+    UCHAR	EKD_DL:1;		// EKD for AP; DL for STA
+    UCHAR	Request:1;
+    UCHAR	Error:1;
+    UCHAR	Secure:1;
+    UCHAR	KeyMic:1;
+#else
+	UCHAR	KeyMic:1;
+	UCHAR	Secure:1;
+	UCHAR	Error:1;
+	UCHAR	Request:1;
+	UCHAR	EKD_DL:1;       // EKD for AP; DL for STA
+	UCHAR	Rsvd:3;
+	UCHAR	KeyDescVer:3;
+	UCHAR	KeyType:1;
+	UCHAR	KeyIndex:2;
+	UCHAR	Install:1;
+	UCHAR	KeyAck:1;
+#endif
+}	KEY_INFO, *PKEY_INFO;
+
+// EAPOL Key descriptor format
+typedef	struct PACKED _KEY_DESCRIPTER
+{
+	UCHAR		Type;
+	KEY_INFO	KeyInfo;
+	UCHAR		KeyLength[2];
+	UCHAR		ReplayCounter[LEN_KEY_DESC_REPLAY];
+	UCHAR		KeyNonce[LEN_KEY_DESC_NONCE];
+	UCHAR		KeyIv[LEN_KEY_DESC_IV];
+	UCHAR		KeyRsc[LEN_KEY_DESC_RSC];
+	UCHAR		KeyId[LEN_KEY_DESC_ID];
+	UCHAR		KeyMic[LEN_KEY_DESC_MIC];
+	UCHAR		KeyDataLen[2];
+	UCHAR		KeyData[MAX_LEN_OF_RSNIE];
+}	KEY_DESCRIPTER, *PKEY_DESCRIPTER;
+
+typedef	struct PACKED _EAPOL_PACKET
+{
+	UCHAR	 			ProVer;
+	UCHAR	 			ProType;
+	UCHAR	 			Body_Len[2];
+	KEY_DESCRIPTER		KeyDesc;
+}	EAPOL_PACKET, *PEAPOL_PACKET;
+
+//802.11i D10 page 83
+typedef struct PACKED _GTK_ENCAP
+{
+#ifndef RT_BIG_ENDIAN
+    UCHAR               Kid:2;
+    UCHAR               tx:1;
+    UCHAR               rsv:5;
+    UCHAR               rsv1;
+#else
+    UCHAR               rsv:5;
+    UCHAR               tx:1;
+    UCHAR               Kid:2;
+    UCHAR               rsv1;
+#endif
+    UCHAR               GTK[TKIP_GTK_LENGTH];
+}   GTK_ENCAP, *PGTK_ENCAP;
+
+typedef struct PACKED _KDE_ENCAP
+{
+    UCHAR               Type;
+    UCHAR               Len;
+    UCHAR               OUI[3];
+    UCHAR               DataType;
+    GTK_ENCAP      GTKEncap;
+}   KDE_ENCAP, *PKDE_ENCAP;
+
+// For WPA1
+typedef struct PACKED _RSNIE {
+    UCHAR   oui[4];
+    USHORT  version;
+    UCHAR   mcast[4];
+    USHORT  ucount;
+    struct PACKED {
+        UCHAR oui[4];
+    }ucast[1];
+} RSNIE, *PRSNIE;
+
+// For WPA2
+typedef struct PACKED _RSNIE2 {
+    USHORT  version;
+    UCHAR   mcast[4];
+    USHORT  ucount;
+    struct PACKED {
+        UCHAR oui[4];
+    }ucast[1];
+} RSNIE2, *PRSNIE2;
+
+// AKM Suite
+typedef struct PACKED _RSNIE_AUTH {
+    USHORT acount;
+    struct PACKED {
+        UCHAR oui[4];
+    }auth[1];
+} RSNIE_AUTH,*PRSNIE_AUTH;
+
+typedef	union PACKED _RSN_CAPABILITIES	{
+	struct	PACKED {
+#ifdef RT_BIG_ENDIAN
+        USHORT		Rsvd:10;
+        USHORT		GTKSA_R_Counter:2;
+        USHORT		PTKSA_R_Counter:2;
+        USHORT		No_Pairwise:1;
+		USHORT		PreAuth:1;
+#else
+        USHORT		PreAuth:1;
+		USHORT		No_Pairwise:1;
+		USHORT		PTKSA_R_Counter:2;
+		USHORT		GTKSA_R_Counter:2;
+		USHORT		Rsvd:10;
+#endif
+	}	field;
+	USHORT			word;
+}	RSN_CAPABILITIES, *PRSN_CAPABILITIES;
+
+typedef struct PACKED _EAP_HDR {
+    UCHAR   ProVer;
+    UCHAR   ProType;
+    UCHAR   Body_Len[2];
+    UCHAR   code;
+    UCHAR   identifier;
+    UCHAR   length[2]; // including code and identifier, followed by length-2 octets of data
+} EAP_HDR, *PEAP_HDR;
+
+// For supplicant state machine states. 802.11i Draft 4.1, p. 97
+// We simplified it
+typedef	enum	_WpaState
+{
+	SS_NOTUSE,				// 0
+	SS_START,				// 1
+	SS_WAIT_MSG_3,			// 2
+	SS_WAIT_GROUP,			// 3
+	SS_FINISH,  			// 4
+	SS_KEYUPDATE,			// 5
+}	WPA_STATE;
+
+//
+//	The definition of the cipher combination
+//
+// 	 bit3	bit2  bit1   bit0
+//	+------------+------------+
+// 	|	  WPA	 |	   WPA2   |
+//	+------+-----+------+-----+
+//	| TKIP | AES | TKIP | AES |
+//	|	0  |  1  |   1  |  0  | -> 0x06
+//	|	0  |  1  |   1  |  1  | -> 0x07
+//	|	1  |  0  |   0  |  1  | -> 0x09
+//	|	1  |  0  |   1  |  1  | -> 0x0B
+//	|	1  |  1  |   0  |  1  | -> 0x0D
+//	|	1  |  1  |   1  |  0  | -> 0x0E
+//	|	1  |  1  |   1  |  1  |	-> 0x0F
+//	+------+-----+------+-----+
+//
+typedef	enum	_WpaMixPairCipher
+{
+	MIX_CIPHER_NOTUSE 			= 0x00,
+	WPA_NONE_WPA2_TKIPAES		= 0x03,		// WPA2-TKIPAES
+	WPA_AES_WPA2_TKIP 			= 0x06,
+	WPA_AES_WPA2_TKIPAES		= 0x07,
+	WPA_TKIP_WPA2_AES			= 0x09,
+	WPA_TKIP_WPA2_TKIPAES		= 0x0B,
+	WPA_TKIPAES_WPA2_NONE		= 0x0C,		// WPA-TKIPAES
+	WPA_TKIPAES_WPA2_AES		= 0x0D,
+	WPA_TKIPAES_WPA2_TKIP		= 0x0E,
+	WPA_TKIPAES_WPA2_TKIPAES	= 0x0F,
+}	WPA_MIX_PAIR_CIPHER;
+
+typedef struct PACKED _RSN_IE_HEADER_STRUCT	{
+	UCHAR		Eid;
+	UCHAR		Length;
+	USHORT		Version;	// Little endian format
+}	RSN_IE_HEADER_STRUCT, *PRSN_IE_HEADER_STRUCT;
+
+// Cipher suite selector types
+typedef struct PACKED _CIPHER_SUITE_STRUCT	{
+	UCHAR		Oui[3];
+	UCHAR		Type;
+}	CIPHER_SUITE_STRUCT, *PCIPHER_SUITE_STRUCT;
+
+// Authentication and Key Management suite selector
+typedef struct PACKED _AKM_SUITE_STRUCT	{
+	UCHAR		Oui[3];
+	UCHAR		Type;
+}	AKM_SUITE_STRUCT, *PAKM_SUITE_STRUCT;
+
+// RSN capability
+typedef struct	PACKED _RSN_CAPABILITY	{
+	USHORT		Rsv:10;
+	USHORT		GTKSAReplayCnt:2;
+	USHORT		PTKSAReplayCnt:2;
+	USHORT		NoPairwise:1;
+	USHORT		PreAuth:1;
+}	RSN_CAPABILITY, *PRSN_CAPABILITY;
+
+#endif
diff --git a/drivers/staging/rt2870/2870_main_dev.c b/drivers/staging/rt2870/2870_main_dev.c
new file mode 100644
index 000000000000..04c764d95d77
--- /dev/null
+++ b/drivers/staging/rt2870/2870_main_dev.c
@@ -0,0 +1,1612 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rtmp_main.c
+
+    Abstract:
+    main initialization routines
+
+    Revision History:
+    Who         When            What
+    --------    ----------      ----------------------------------------------
+    Name        Date            Modification logs
+    Jan Lee		01-10-2005	    modified
+	Sample		Jun/01/07		Merge RT2870 and RT2860 drivers.
+*/
+
+#include "rt_config.h"
+
+
+// Following information will be show when you run 'modinfo'
+// *** If you have a solution for the bug in current version of driver, please mail to me.
+// Otherwise post to forum in ralinktech's web site(www.ralinktech.com) and let all users help you. ***
+MODULE_AUTHOR("Paul Lin <paul_lin@ralinktech.com>");
+MODULE_DESCRIPTION("RT2870 Wireless Lan Linux Driver");
+#ifdef CONFIG_STA_SUPPORT
+MODULE_LICENSE("GPL");
+#ifdef MODULE_VERSION
+MODULE_VERSION(STA_DRIVER_VERSION);
+#endif
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef MULTIPLE_CARD_SUPPORT
+// record whether the card in the card list is used in the card file
+extern UINT8  MC_CardUsed[];
+#endif // MULTIPLE_CARD_SUPPORT //
+
+/* Kernel thread and vars, which handles packets that are completed. Only
+ * packets that have a "complete" function are sent here. This way, the
+ * completion is run out of kernel context, and doesn't block the rest of
+ * the stack. */
+
+extern INT __devinit rt28xx_probe(IN void *_dev_p, IN void *_dev_id_p,
+									IN UINT argc, OUT PRTMP_ADAPTER *ppAd);
+
+
+/* module table */
+struct usb_device_id    rtusb_usb_id[] = RT2870_USB_DEVICES;
+INT const               rtusb_usb_id_len = sizeof(rtusb_usb_id) / sizeof(struct usb_device_id);
+MODULE_DEVICE_TABLE(usb, rtusb_usb_id);
+
+#ifndef PF_NOFREEZE
+#define PF_NOFREEZE  0
+#endif
+
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+
+/**************************************************************************/
+/**************************************************************************/
+//tested for kernel 2.4 series
+/**************************************************************************/
+/**************************************************************************/
+static void *rtusb_probe(struct usb_device *dev, UINT interface,
+						const struct usb_device_id *id_table);
+static void rtusb_disconnect(struct usb_device *dev, void *ptr);
+
+struct usb_driver rtusb_driver = {
+		name:"rt2870",
+		probe:rtusb_probe,
+		disconnect:rtusb_disconnect,
+		id_table:rtusb_usb_id,
+	};
+
+#else
+
+#ifdef CONFIG_PM
+static int rt2870_suspend(struct usb_interface *intf, pm_message_t state);
+static int rt2870_resume(struct usb_interface *intf);
+#endif // CONFIG_PM //
+
+/**************************************************************************/
+/**************************************************************************/
+//tested for kernel 2.6series
+/**************************************************************************/
+/**************************************************************************/
+static int rtusb_probe (struct usb_interface *intf,
+						const struct usb_device_id *id);
+static void rtusb_disconnect(struct usb_interface *intf);
+
+struct usb_driver rtusb_driver = {
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,15)
+	.owner = THIS_MODULE,
+#endif
+	.name="rt2870",
+	.probe=rtusb_probe,
+	.disconnect=rtusb_disconnect,
+	.id_table=rtusb_usb_id,
+
+#ifdef CONFIG_PM
+	suspend:	rt2870_suspend,
+	resume:		rt2870_resume,
+#endif
+	};
+
+#ifdef CONFIG_PM
+
+VOID RT2860RejectPendingPackets(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	// clear PS packets
+	// clear TxSw packets
+}
+
+static int rt2870_suspend(
+	struct usb_interface *intf,
+	pm_message_t state)
+{
+	struct net_device *net_dev;
+	PRTMP_ADAPTER pAd = usb_get_intfdata(intf);
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> rt2870_suspend()\n"));
+	net_dev = pAd->net_dev;
+	netif_device_detach (net_dev);
+
+	pAd->PM_FlgSuspend = 1;
+	if (netif_running(net_dev)) {
+		RTUSBCancelPendingBulkInIRP(pAd);
+		RTUSBCancelPendingBulkOutIRP(pAd);
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2870_suspend()\n"));
+	return 0;
+}
+
+static int rt2870_resume(
+	struct usb_interface *intf)
+{
+	struct net_device *net_dev;
+	PRTMP_ADAPTER pAd = usb_get_intfdata(intf);
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> rt2870_resume()\n"));
+
+	pAd->PM_FlgSuspend = 0;
+	net_dev = pAd->net_dev;
+	netif_device_attach (net_dev);
+	netif_start_queue(net_dev);
+	netif_carrier_on(net_dev);
+	netif_wake_queue(net_dev);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2870_resume()\n"));
+	return 0;
+}
+#endif // CONFIG_PM //
+#endif // LINUX_VERSION_CODE //
+
+
+// Init driver module
+INT __init rtusb_init(void)
+{
+	printk("rtusb init --->\n");
+	return usb_register(&rtusb_driver);
+}
+
+// Deinit driver module
+VOID __exit rtusb_exit(void)
+{
+	usb_deregister(&rtusb_driver);
+	printk("<--- rtusb exit\n");
+}
+
+module_init(rtusb_init);
+module_exit(rtusb_exit);
+
+
+
+
+/*---------------------------------------------------------------------	*/
+/* function declarations												*/
+/*---------------------------------------------------------------------	*/
+
+/*
+========================================================================
+Routine Description:
+    MLME kernel thread.
+
+Arguments:
+	*Context			the pAd, driver control block pointer
+
+Return Value:
+    0					close the thread
+
+Note:
+========================================================================
+*/
+INT MlmeThread(
+	IN void *Context)
+{
+	PRTMP_ADAPTER	pAd = (PRTMP_ADAPTER)Context;
+	POS_COOKIE	pObj;
+	int status;
+
+	pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	rtmp_os_thread_init("rt2870MlmeThread", (PVOID)&(pAd->mlmeComplete));
+
+	while (pAd->mlme_kill == 0)
+	{
+		/* lock the device pointers */
+		//down(&(pAd->mlme_semaphore));
+		status = down_interruptible(&(pAd->mlme_semaphore));
+
+		/* lock the device pointers , need to check if required*/
+		//down(&(pAd->usbdev_semaphore));
+
+		if (!pAd->PM_FlgSuspend)
+		MlmeHandler(pAd);
+
+		/* unlock the device pointers */
+		//up(&(pAd->usbdev_semaphore));
+		if (status != 0)
+		{
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+			break;
+		}
+	}
+
+	/* notify the exit routine that we're actually exiting now
+	 *
+	 * complete()/wait_for_completion() is similar to up()/down(),
+	 * except that complete() is safe in the case where the structure
+	 * is getting deleted in a parallel mode of execution (i.e. just
+	 * after the down() -- that's necessary for the thread-shutdown
+	 * case.
+	 *
+	 * complete_and_exit() goes even further than this -- it is safe in
+	 * the case that the thread of the caller is going away (not just
+	 * the structure) -- this is necessary for the module-remove case.
+	 * This is important in preemption kernels, which transfer the flow
+	 * of execution immediately upon a complete().
+	 */
+	DBGPRINT(RT_DEBUG_TRACE,( "<---%s\n",__func__));
+
+	pObj->MLMEThr_pid = THREAD_PID_INIT_VALUE;
+
+	complete_and_exit (&pAd->mlmeComplete, 0);
+	return 0;
+
+}
+
+
+/*
+========================================================================
+Routine Description:
+    USB command kernel thread.
+
+Arguments:
+	*Context			the pAd, driver control block pointer
+
+Return Value:
+    0					close the thread
+
+Note:
+========================================================================
+*/
+INT RTUSBCmdThread(
+	IN void * Context)
+{
+	PRTMP_ADAPTER	pAd = (PRTMP_ADAPTER)Context;
+	POS_COOKIE		pObj;
+	int status;
+
+	pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	rtmp_os_thread_init("rt2870CmdThread", (PVOID)&(pAd->CmdQComplete));
+
+	NdisAcquireSpinLock(&pAd->CmdQLock);
+	pAd->CmdQ.CmdQState = RT2870_THREAD_RUNNING;
+	NdisReleaseSpinLock(&pAd->CmdQLock);
+
+	while (pAd->CmdQ.CmdQState == RT2870_THREAD_RUNNING)
+	{
+		/* lock the device pointers */
+		//down(&(pAd->RTUSBCmd_semaphore));
+		status = down_interruptible(&(pAd->RTUSBCmd_semaphore));
+
+		if (pAd->CmdQ.CmdQState == RT2870_THREAD_STOPED)
+			break;
+
+		if (status != 0)
+		{
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+			break;
+		}
+		/* lock the device pointers , need to check if required*/
+		//down(&(pAd->usbdev_semaphore));
+
+		if (!pAd->PM_FlgSuspend)
+		CMDHandler(pAd);
+
+		/* unlock the device pointers */
+		//up(&(pAd->usbdev_semaphore));
+	}
+
+	if (!pAd->PM_FlgSuspend)
+	{	// Clear the CmdQElements.
+		CmdQElmt	*pCmdQElmt = NULL;
+
+		NdisAcquireSpinLock(&pAd->CmdQLock);
+		pAd->CmdQ.CmdQState = RT2870_THREAD_STOPED;
+		while(pAd->CmdQ.size)
+		{
+			RTUSBDequeueCmd(&pAd->CmdQ, &pCmdQElmt);
+			if (pCmdQElmt)
+			{
+				if (pCmdQElmt->CmdFromNdis == TRUE)
+				{
+					if (pCmdQElmt->buffer != NULL)
+						NdisFreeMemory(pCmdQElmt->buffer, pCmdQElmt->bufferlength, 0);
+
+					NdisFreeMemory(pCmdQElmt, sizeof(CmdQElmt), 0);
+				}
+				else
+				{
+					if ((pCmdQElmt->buffer != NULL) && (pCmdQElmt->bufferlength != 0))
+						NdisFreeMemory(pCmdQElmt->buffer, pCmdQElmt->bufferlength, 0);
+		            {
+						NdisFreeMemory(pCmdQElmt, sizeof(CmdQElmt), 0);
+					}
+				}
+			}
+		}
+
+		NdisReleaseSpinLock(&pAd->CmdQLock);
+	}
+	/* notify the exit routine that we're actually exiting now
+	 *
+	 * complete()/wait_for_completion() is similar to up()/down(),
+	 * except that complete() is safe in the case where the structure
+	 * is getting deleted in a parallel mode of execution (i.e. just
+	 * after the down() -- that's necessary for the thread-shutdown
+	 * case.
+	 *
+	 * complete_and_exit() goes even further than this -- it is safe in
+	 * the case that the thread of the caller is going away (not just
+	 * the structure) -- this is necessary for the module-remove case.
+	 * This is important in preemption kernels, which transfer the flow
+	 * of execution immediately upon a complete().
+	 */
+	DBGPRINT(RT_DEBUG_TRACE,( "<---RTUSBCmdThread\n"));
+
+	pObj->RTUSBCmdThr_pid = THREAD_PID_INIT_VALUE;
+
+	complete_and_exit (&pAd->CmdQComplete, 0);
+	return 0;
+
+}
+
+
+static void RT2870_TimerQ_Handle(RTMP_ADAPTER *pAd)
+{
+	int status;
+	RALINK_TIMER_STRUCT	*pTimer;
+	RT2870_TIMER_ENTRY	*pEntry;
+	unsigned long	irqFlag;
+
+	while(!pAd->TimerFunc_kill)
+	{
+//		printk("waiting for event!\n");
+		pTimer = NULL;
+
+		status = down_interruptible(&(pAd->RTUSBTimer_semaphore));
+
+		if (pAd->TimerQ.status == RT2870_THREAD_STOPED)
+			break;
+
+		// event happened.
+		while(pAd->TimerQ.pQHead)
+		{
+			RTMP_IRQ_LOCK(&pAd->TimerQLock, irqFlag);
+			pEntry = pAd->TimerQ.pQHead;
+			if (pEntry)
+			{
+				pTimer = pEntry->pRaTimer;
+
+				// update pQHead
+				pAd->TimerQ.pQHead = pEntry->pNext;
+				if (pEntry == pAd->TimerQ.pQTail)
+					pAd->TimerQ.pQTail = NULL;
+
+				// return this queue entry to timerQFreeList.
+				pEntry->pNext = pAd->TimerQ.pQPollFreeList;
+				pAd->TimerQ.pQPollFreeList = pEntry;
+			}
+			RTMP_IRQ_UNLOCK(&pAd->TimerQLock, irqFlag);
+
+			if (pTimer)
+			{
+				if (pTimer->handle != NULL)
+				if (!pAd->PM_FlgSuspend)
+					pTimer->handle(NULL, (PVOID) pTimer->cookie, NULL, pTimer);
+				if ((pTimer->Repeat) && (pTimer->State == FALSE))
+					RTMP_OS_Add_Timer(&pTimer->TimerObj, pTimer->TimerValue);
+			}
+		}
+
+		if (status != 0)
+		{
+			pAd->TimerQ.status = RT2870_THREAD_STOPED;
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+			break;
+		}
+	}
+}
+
+
+INT TimerQThread(
+	IN OUT PVOID Context)
+{
+	PRTMP_ADAPTER	pAd;
+	POS_COOKIE	pObj;
+
+	pAd = (PRTMP_ADAPTER)Context;
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	rtmp_os_thread_init("rt2870TimerQHandle", (PVOID)&(pAd->TimerQComplete));
+
+	RT2870_TimerQ_Handle(pAd);
+
+	/* notify the exit routine that we're actually exiting now
+	 *
+	 * complete()/wait_for_completion() is similar to up()/down(),
+	 * except that complete() is safe in the case where the structure
+	 * is getting deleted in a parallel mode of execution (i.e. just
+	 * after the down() -- that's necessary for the thread-shutdown
+	 * case.
+	 *
+	 * complete_and_exit() goes even further than this -- it is safe in
+	 * the case that the thread of the caller is going away (not just
+	 * the structure) -- this is necessary for the module-remove case.
+	 * This is important in preemption kernels, which transfer the flow
+	 * of execution immediately upon a complete().
+	 */
+	DBGPRINT(RT_DEBUG_TRACE,( "<---%s\n",__func__));
+
+	pObj->TimerQThr_pid = THREAD_PID_INIT_VALUE;
+
+	complete_and_exit(&pAd->TimerQComplete, 0);
+	return 0;
+
+}
+
+
+RT2870_TIMER_ENTRY *RT2870_TimerQ_Insert(
+	IN RTMP_ADAPTER *pAd,
+	IN RALINK_TIMER_STRUCT *pTimer)
+{
+	RT2870_TIMER_ENTRY *pQNode = NULL, *pQTail;
+	unsigned long irqFlags;
+
+
+	RTMP_IRQ_LOCK(&pAd->TimerQLock, irqFlags);
+	if (pAd->TimerQ.status & RT2870_THREAD_CAN_DO_INSERT)
+	{
+		if(pAd->TimerQ.pQPollFreeList)
+		{
+			pQNode = pAd->TimerQ.pQPollFreeList;
+			pAd->TimerQ.pQPollFreeList = pQNode->pNext;
+
+			pQNode->pRaTimer = pTimer;
+			pQNode->pNext = NULL;
+
+			pQTail = pAd->TimerQ.pQTail;
+			if (pAd->TimerQ.pQTail != NULL)
+				pQTail->pNext = pQNode;
+			pAd->TimerQ.pQTail = pQNode;
+			if (pAd->TimerQ.pQHead == NULL)
+				pAd->TimerQ.pQHead = pQNode;
+		}
+		RTMP_IRQ_UNLOCK(&pAd->TimerQLock, irqFlags);
+
+		if (pQNode)
+			up(&pAd->RTUSBTimer_semaphore);
+			//wake_up(&timerWaitQ);
+	}
+	else
+	{
+		RTMP_IRQ_UNLOCK(&pAd->TimerQLock, irqFlags);
+	}
+	return pQNode;
+}
+
+
+BOOLEAN RT2870_TimerQ_Remove(
+	IN RTMP_ADAPTER *pAd,
+	IN RALINK_TIMER_STRUCT *pTimer)
+{
+	RT2870_TIMER_ENTRY *pNode, *pPrev = NULL;
+	unsigned long irqFlags;
+
+	RTMP_IRQ_LOCK(&pAd->TimerQLock, irqFlags);
+	if (pAd->TimerQ.status >= RT2870_THREAD_INITED)
+	{
+		pNode = pAd->TimerQ.pQHead;
+		while (pNode)
+		{
+			if (pNode->pRaTimer == pTimer)
+				break;
+			pPrev = pNode;
+			pNode = pNode->pNext;
+		}
+
+		// Now move it to freeList queue.
+		if (pNode)
+		{
+			if (pNode == pAd->TimerQ.pQHead)
+				pAd->TimerQ.pQHead = pNode->pNext;
+			if (pNode == pAd->TimerQ.pQTail)
+				pAd->TimerQ.pQTail = pPrev;
+			if (pPrev != NULL)
+				pPrev->pNext = pNode->pNext;
+
+			// return this queue entry to timerQFreeList.
+			pNode->pNext = pAd->TimerQ.pQPollFreeList;
+			pAd->TimerQ.pQPollFreeList = pNode;
+		}
+	}
+	RTMP_IRQ_UNLOCK(&pAd->TimerQLock, irqFlags);
+
+	return TRUE;
+}
+
+
+void RT2870_TimerQ_Exit(RTMP_ADAPTER *pAd)
+{
+	RT2870_TIMER_ENTRY *pTimerQ;
+	unsigned long irqFlags;
+
+	RTMP_IRQ_LOCK(&pAd->TimerQLock, irqFlags);
+	while (pAd->TimerQ.pQHead)
+	{
+		pTimerQ = pAd->TimerQ.pQHead;
+		pAd->TimerQ.pQHead = pTimerQ->pNext;
+		// remove the timeQ
+	}
+	pAd->TimerQ.pQPollFreeList = NULL;
+	os_free_mem(pAd, pAd->TimerQ.pTimerQPoll);
+	pAd->TimerQ.pQTail = NULL;
+	pAd->TimerQ.pQHead = NULL;
+	pAd->TimerQ.status = RT2870_THREAD_STOPED;
+	RTMP_IRQ_UNLOCK(&pAd->TimerQLock, irqFlags);
+
+}
+
+
+void RT2870_TimerQ_Init(RTMP_ADAPTER *pAd)
+{
+	int 	i;
+	RT2870_TIMER_ENTRY *pQNode, *pEntry;
+	unsigned long irqFlags;
+
+	NdisAllocateSpinLock(&pAd->TimerQLock);
+
+	RTMP_IRQ_LOCK(&pAd->TimerQLock, irqFlags);
+	NdisZeroMemory(&pAd->TimerQ, sizeof(pAd->TimerQ));
+	//InterlockedExchange(&pAd->TimerQ.count, 0);
+
+	/* Initialise the wait q head */
+	//init_waitqueue_head(&timerWaitQ);
+
+	os_alloc_mem(pAd, &pAd->TimerQ.pTimerQPoll, sizeof(RT2870_TIMER_ENTRY) * TIMER_QUEUE_SIZE_MAX);
+	if (pAd->TimerQ.pTimerQPoll)
+	{
+		pEntry = NULL;
+		pQNode = (RT2870_TIMER_ENTRY *)pAd->TimerQ.pTimerQPoll;
+		for (i = 0 ;i <TIMER_QUEUE_SIZE_MAX; i++)
+		{
+			pQNode->pNext = pEntry;
+			pEntry = pQNode;
+			pQNode++;
+		}
+		pAd->TimerQ.pQPollFreeList = pEntry;
+		pAd->TimerQ.pQHead = NULL;
+		pAd->TimerQ.pQTail = NULL;
+		pAd->TimerQ.status = RT2870_THREAD_INITED;
+	}
+	RTMP_IRQ_UNLOCK(&pAd->TimerQLock, irqFlags);
+}
+
+
+VOID RT2870_WatchDog(IN RTMP_ADAPTER *pAd)
+{
+	PHT_TX_CONTEXT		pHTTXContext;
+	int 					idx;
+	ULONG				irqFlags;
+	PURB		   		pUrb;
+	BOOLEAN				needDumpSeq = FALSE;
+	UINT32          	MACValue;
+
+
+	idx = 0;
+	RTMP_IO_READ32(pAd, TXRXQ_PCNT, &MACValue);
+	if ((MACValue & 0xff) !=0 )
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("TX QUEUE 0 Not EMPTY(Value=0x%0x). !!!!!!!!!!!!!!!\n", MACValue));
+		RTMP_IO_WRITE32(pAd, PBF_CFG, 0xf40012);
+		while((MACValue &0xff) != 0 && (idx++ < 10))
+		{
+		        RTMP_IO_READ32(pAd, TXRXQ_PCNT, &MACValue);
+		        NdisMSleep(1);
+		}
+		RTMP_IO_WRITE32(pAd, PBF_CFG, 0xf40006);
+	}
+
+	idx = 0;
+	if ((MACValue & 0xff00) !=0 )
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("TX QUEUE 1 Not EMPTY(Value=0x%0x). !!!!!!!!!!!!!!!\n", MACValue));
+		RTMP_IO_WRITE32(pAd, PBF_CFG, 0xf4000a);
+		while((MACValue &0xff00) != 0 && (idx++ < 10))
+		{
+			RTMP_IO_READ32(pAd, TXRXQ_PCNT, &MACValue);
+			NdisMSleep(1);
+		}
+		RTMP_IO_WRITE32(pAd, PBF_CFG, 0xf40006);
+	}
+
+
+	if (pAd->watchDogRxOverFlowCnt >= 2)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("Maybe the Rx Bulk-In hanged! Cancel the pending Rx bulks request!\n"));
+		if ((!RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+									fRTMP_ADAPTER_BULKIN_RESET |
+									fRTMP_ADAPTER_HALT_IN_PROGRESS |
+									fRTMP_ADAPTER_NIC_NOT_EXIST))))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("Call CMDTHREAD_RESET_BULK_IN to cancel the pending Rx Bulk!\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_IN, NULL, 0);
+			needDumpSeq = TRUE;
+		}
+		pAd->watchDogRxOverFlowCnt = 0;
+	}
+
+
+	for (idx = 0; idx < NUM_OF_TX_RING; idx++)
+	{
+		pUrb = NULL;
+
+		RTMP_IRQ_LOCK(&pAd->BulkOutLock[idx], irqFlags);
+		if ((pAd->BulkOutPending[idx] == TRUE) && pAd->watchDogTxPendingCnt)
+		{
+			pAd->watchDogTxPendingCnt[idx]++;
+
+			if ((pAd->watchDogTxPendingCnt[idx] > 2) &&
+				 (!RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS | fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST | fRTMP_ADAPTER_BULKOUT_RESET)))
+				)
+			{
+				// FIXME: Following code just support single bulk out. If you wanna support multiple bulk out. Modify it!
+				pHTTXContext = (PHT_TX_CONTEXT)(&pAd->TxContext[idx]);
+				if (pHTTXContext->IRPPending)
+				{	// Check TxContext.
+					pUrb = pHTTXContext->pUrb;
+				}
+				else if (idx == MGMTPIPEIDX)
+				{
+					PTX_CONTEXT pMLMEContext, pNULLContext, pPsPollContext;
+
+					//Check MgmtContext.
+					pMLMEContext = (PTX_CONTEXT)(pAd->MgmtRing.Cell[pAd->MgmtRing.TxDmaIdx].AllocVa);
+					pPsPollContext = (PTX_CONTEXT)(&pAd->PsPollContext);
+					pNULLContext = (PTX_CONTEXT)(&pAd->NullContext);
+
+					if (pMLMEContext->IRPPending)
+					{
+						ASSERT(pMLMEContext->IRPPending);
+						pUrb = pMLMEContext->pUrb;
+					}
+					else if (pNULLContext->IRPPending)
+					{
+						ASSERT(pNULLContext->IRPPending);
+						pUrb = pNULLContext->pUrb;
+					}
+					else if (pPsPollContext->IRPPending)
+					{
+						ASSERT(pPsPollContext->IRPPending);
+						pUrb = pPsPollContext->pUrb;
+					}
+				}
+
+				RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[idx], irqFlags);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Maybe the Tx Bulk-Out hanged! Cancel the pending Tx bulks request of idx(%d)!\n", idx));
+				if (pUrb)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("Unlink the pending URB!\n"));
+					// unlink it now
+					RTUSB_UNLINK_URB(pUrb);
+					// Sleep 200 microseconds to give cancellation time to work
+					RTMPusecDelay(200);
+					needDumpSeq = TRUE;
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("Unkonw bulkOut URB maybe hanged!!!!!!!!!!!!\n"));
+				}
+			}
+			else
+			{
+				RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[idx], irqFlags);
+			}
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[idx], irqFlags);
+		}
+	}
+
+#ifdef DOT11_N_SUPPORT
+	// For Sigma debug, dump the ba_reordering sequence.
+	if((needDumpSeq == TRUE) && (pAd->CommonCfg.bDisableReordering == 0))
+	{
+		USHORT				Idx;
+		PBA_REC_ENTRY		pBAEntry = NULL;
+		UCHAR				count = 0;
+		struct reordering_mpdu *mpdu_blk;
+
+		Idx = pAd->MacTab.Content[BSSID_WCID].BARecWcidArray[0];
+
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+		if((pBAEntry->list.qlen > 0) && (pBAEntry->list.next != NULL))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("NICUpdateRawCounters():The Queueing pkt in reordering buffer:\n"));
+			NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+			mpdu_blk = pBAEntry->list.next;
+			while (mpdu_blk)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("\t%d:Seq-%d, bAMSDU-%d!\n", count, mpdu_blk->Sequence, mpdu_blk->bAMSDU));
+				mpdu_blk = mpdu_blk->next;
+				count++;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE, ("\npBAEntry->LastIndSeq=%d!\n", pBAEntry->LastIndSeq));
+			NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+}
+
+/*
+========================================================================
+Routine Description:
+    Release allocated resources.
+
+Arguments:
+    *dev				Point to the PCI or USB device
+	pAd					driver control block pointer
+
+Return Value:
+    None
+
+Note:
+========================================================================
+*/
+static void _rtusb_disconnect(struct usb_device *dev, PRTMP_ADAPTER pAd)
+{
+	struct net_device	*net_dev = NULL;
+
+
+	DBGPRINT(RT_DEBUG_ERROR, ("rtusb_disconnect: unregister usbnet usb-%s-%s\n",
+				dev->bus->bus_name, dev->devpath));
+	if (!pAd)
+	{
+#ifdef MULTIPLE_CARD_SUPPORT
+		if ((pAd->MC_RowID >= 0) && (pAd->MC_RowID <= MAX_NUM_OF_MULTIPLE_CARD))
+			MC_CardUsed[pAd->MC_RowID] = 0; // not clear MAC address
+#endif // MULTIPLE_CARD_SUPPORT //
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+		while(MOD_IN_USE > 0)
+		{
+			MOD_DEC_USE_COUNT;
+		}
+#else
+		usb_put_dev(dev);
+#endif // LINUX_VERSION_CODE //
+
+		printk("rtusb_disconnect: pAd == NULL!\n");
+		return;
+	}
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST);
+
+
+
+	// for debug, wait to show some messages to /proc system
+	udelay(1);
+
+
+
+
+	net_dev = pAd->net_dev;
+	if (pAd->net_dev != NULL)
+	{
+		printk("rtusb_disconnect: unregister_netdev(), dev->name=%s!\n", net_dev->name);
+		unregister_netdev (pAd->net_dev);
+	}
+	udelay(1);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+#else
+	flush_scheduled_work();
+#endif // LINUX_VERSION_CODE //
+	udelay(1);
+
+	// free net_device memory
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+	kfree(net_dev);
+#else
+	free_netdev(net_dev);
+#endif // LINUX_VERSION_CODE //
+
+	// free adapter memory
+	RTMPFreeAdapter(pAd);
+
+	// release a use of the usb device structure
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+	while(MOD_IN_USE > 0)
+	{
+		MOD_DEC_USE_COUNT;
+	}
+#else
+	usb_put_dev(dev);
+#endif // LINUX_VERSION_CODE //
+	udelay(1);
+
+	DBGPRINT(RT_DEBUG_ERROR, (" RTUSB disconnect successfully\n"));
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Probe RT28XX chipset.
+
+Arguments:
+    *dev				Point to the PCI or USB device
+	interface
+	*id_table			Point to the PCI or USB device ID
+
+Return Value:
+    None
+
+Note:
+========================================================================
+*/
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+static void *rtusb_probe(struct usb_device *dev, UINT interface,
+						const struct usb_device_id *id)
+{
+	PRTMP_ADAPTER pAd;
+	rt28xx_probe((void *)dev, (void *)id, interface, &pAd);
+	return (void *)pAd;
+}
+
+//Disconnect function is called within exit routine
+static void rtusb_disconnect(struct usb_device *dev, void *ptr)
+{
+	_rtusb_disconnect(dev, ((PRTMP_ADAPTER)ptr));
+}
+
+#else	/* kernel 2.6 series */
+static int rtusb_probe (struct usb_interface *intf,
+						const struct usb_device_id *id)
+{
+	PRTMP_ADAPTER pAd;
+	return (int)rt28xx_probe((void *)intf, (void *)id, 0, &pAd);
+}
+
+
+static void rtusb_disconnect(struct usb_interface *intf)
+{
+	struct usb_device   *dev = interface_to_usbdev(intf);
+	PRTMP_ADAPTER       pAd;
+
+
+	pAd = usb_get_intfdata(intf);
+	usb_set_intfdata(intf, NULL);
+
+	_rtusb_disconnect(dev, pAd);
+}
+#endif // LINUX_VERSION_CODE //
+
+
+/*
+========================================================================
+Routine Description:
+    Close kernel threads.
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+    NONE
+
+Note:
+========================================================================
+*/
+VOID RT28xxThreadTerminate(
+	IN RTMP_ADAPTER *pAd)
+{
+	POS_COOKIE	pObj = (POS_COOKIE) pAd->OS_Cookie;
+	INT			ret;
+
+
+	// Sleep 50 milliseconds so pending io might finish normally
+	RTMPusecDelay(50000);
+
+	// We want to wait until all pending receives and sends to the
+	// device object. We cancel any
+	// irps. Wait until sends and receives have stopped.
+	RTUSBCancelPendingIRPs(pAd);
+
+	// Terminate Threads
+	CHECK_PID_LEGALITY(pObj->TimerQThr_pid)
+	{
+		POS_COOKIE pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+		printk("Terminate the TimerQThr_pid=%d!\n", GET_PID_NUMBER(pObj->TimerQThr_pid));
+		mb();
+		pAd->TimerFunc_kill = 1;
+		mb();
+		ret = KILL_THREAD_PID(pObj->TimerQThr_pid, SIGTERM, 1);
+		if (ret)
+		{
+			printk(KERN_WARNING "%s: unable to stop TimerQThread, pid=%d, ret=%d!\n",
+					pAd->net_dev->name, GET_PID_NUMBER(pObj->TimerQThr_pid), ret);
+		}
+		else
+		{
+			wait_for_completion(&pAd->TimerQComplete);
+			pObj->TimerQThr_pid = THREAD_PID_INIT_VALUE;
+		}
+	}
+
+	CHECK_PID_LEGALITY(pObj->MLMEThr_pid)
+	{
+		printk("Terminate the MLMEThr_pid=%d!\n", GET_PID_NUMBER(pObj->MLMEThr_pid));
+		mb();
+		pAd->mlme_kill = 1;
+		//RT28XX_MLME_HANDLER(pAd);
+		mb();
+		ret = KILL_THREAD_PID(pObj->MLMEThr_pid, SIGTERM, 1);
+		if (ret)
+		{
+			printk (KERN_WARNING "%s: unable to Mlme thread, pid=%d, ret=%d!\n",
+					pAd->net_dev->name, GET_PID_NUMBER(pObj->MLMEThr_pid), ret);
+		}
+		else
+		{
+			//wait_for_completion (&pAd->notify);
+			wait_for_completion (&pAd->mlmeComplete);
+			pObj->MLMEThr_pid = THREAD_PID_INIT_VALUE;
+		}
+	}
+
+	CHECK_PID_LEGALITY(pObj->RTUSBCmdThr_pid)
+	{
+		printk("Terminate the RTUSBCmdThr_pid=%d!\n", GET_PID_NUMBER(pObj->RTUSBCmdThr_pid));
+		mb();
+		NdisAcquireSpinLock(&pAd->CmdQLock);
+		pAd->CmdQ.CmdQState = RT2870_THREAD_STOPED;
+		NdisReleaseSpinLock(&pAd->CmdQLock);
+		mb();
+		//RTUSBCMDUp(pAd);
+		ret = KILL_THREAD_PID(pObj->RTUSBCmdThr_pid, SIGTERM, 1);
+		if (ret)
+		{
+			printk(KERN_WARNING "%s: unable to RTUSBCmd thread, pid=%d, ret=%d!\n",
+					pAd->net_dev->name, GET_PID_NUMBER(pObj->RTUSBCmdThr_pid), ret);
+		}
+		else
+		{
+			//wait_for_completion (&pAd->notify);
+			wait_for_completion (&pAd->CmdQComplete);
+			pObj->RTUSBCmdThr_pid = THREAD_PID_INIT_VALUE;
+	}
+	}
+
+
+	// Kill tasklets
+	pAd->mlme_kill = 0;
+	pAd->CmdQ.CmdQState = RT2870_THREAD_UNKNOWN;
+	pAd->TimerFunc_kill = 0;
+}
+
+
+void kill_thread_task(IN PRTMP_ADAPTER pAd)
+{
+	POS_COOKIE pObj;
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	tasklet_kill(&pObj->rx_done_task);
+	tasklet_kill(&pObj->mgmt_dma_done_task);
+	tasklet_kill(&pObj->ac0_dma_done_task);
+	tasklet_kill(&pObj->ac1_dma_done_task);
+	tasklet_kill(&pObj->ac2_dma_done_task);
+	tasklet_kill(&pObj->ac3_dma_done_task);
+	tasklet_kill(&pObj->hcca_dma_done_task);
+	tasklet_kill(&pObj->tbtt_task);
+
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Check the chipset vendor/product ID.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+
+Return Value:
+    TRUE				Check ok
+	FALSE				Check fail
+
+Note:
+========================================================================
+*/
+BOOLEAN RT28XXChipsetCheck(
+	IN void *_dev_p)
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+	struct usb_device *dev_p = (struct usb_device *)_dev_p;
+#else
+	struct usb_interface *intf = (struct usb_interface *)_dev_p;
+	struct usb_device *dev_p = interface_to_usbdev(intf);
+#endif // LINUX_VERSION_CODE //
+	UINT32 i;
+
+
+	for(i=0; i<rtusb_usb_id_len; i++)
+	{
+		if (dev_p->descriptor.idVendor == rtusb_usb_id[i].idVendor &&
+			dev_p->descriptor.idProduct == rtusb_usb_id[i].idProduct)
+		{
+			printk("rt2870: idVendor = 0x%x, idProduct = 0x%x\n",
+					dev_p->descriptor.idVendor, dev_p->descriptor.idProduct);
+			break;
+		}
+	}
+
+	if (i == rtusb_usb_id_len)
+	{
+		printk("rt2870: Error! Device Descriptor not matching!\n");
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Init net device structure.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+    *net_dev			Point to the net device
+	*pAd				the raxx interface data pointer
+
+Return Value:
+    TRUE				Init ok
+	FALSE				Init fail
+
+Note:
+========================================================================
+*/
+BOOLEAN RT28XXNetDevInit(
+	IN void 				*_dev_p,
+	IN struct  net_device	*net_dev,
+	IN RTMP_ADAPTER 		*pAd)
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+	struct usb_device *dev_p = (struct usb_device *)_dev_p;
+#else
+	struct usb_interface *intf = (struct usb_interface *)_dev_p;
+	struct usb_device *dev_p = interface_to_usbdev(intf);
+#endif // LINUX_VERSION_CODE //
+
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+	pAd->config = dev_p->config;
+#else
+	pAd->config = &dev_p->config->desc;
+#endif // LINUX_VERSION_CODE //
+	return TRUE;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Init net device structure.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+	*pAd				the raxx interface data pointer
+
+Return Value:
+    TRUE				Config ok
+	FALSE				Config fail
+
+Note:
+========================================================================
+*/
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+BOOLEAN RT28XXProbePostConfig(
+	IN void 				*_dev_p,
+	IN RTMP_ADAPTER 		*pAd,
+	IN INT32				interface)
+{
+	struct usb_device *dev_p = (struct usb_device *)_dev_p;
+	struct usb_interface *intf;
+	struct usb_interface_descriptor *iface_desc;
+	struct usb_endpoint_descriptor *endpoint;
+	ULONG BulkOutIdx;
+	UINT32 i;
+
+
+	/* get the active interface descriptor */
+	intf = &dev_p->actconfig->interface[interface];
+	iface_desc = &intf->altsetting[0];
+
+	/* get # of enpoints */
+	pAd->NumberOfPipes = iface_desc->bNumEndpoints;
+	DBGPRINT(RT_DEBUG_TRACE, ("NumEndpoints=%d\n", iface_desc->bNumEndpoints));
+
+	/* Configure Pipes */
+	endpoint = &iface_desc->endpoint[0];
+	BulkOutIdx = 0;
+
+	for(i=0; i<pAd->NumberOfPipes; i++)
+	{
+		if ((endpoint[i].bmAttributes == USB_ENDPOINT_XFER_BULK) &&
+			((endpoint[i].bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_IN))
+		{
+			pAd->BulkInEpAddr = endpoint[i].bEndpointAddress;
+			pAd->BulkInMaxPacketSize = endpoint[i].wMaxPacketSize;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("BULK IN MaximumPacketSize = %d\n", pAd->BulkInMaxPacketSize));
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("EP address = 0x%2x  \n", endpoint[i].bEndpointAddress));
+		}
+		else if ((endpoint[i].bmAttributes == USB_ENDPOINT_XFER_BULK) &&
+				((endpoint[i].bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT))
+		{
+			// There are 6 bulk out EP. EP6 highest priority.
+			// EP1-4 is EDCA.  EP5 is HCCA.
+			pAd->BulkOutEpAddr[BulkOutIdx++] = endpoint[i].bEndpointAddress;
+			pAd->BulkOutMaxPacketSize = endpoint[i].wMaxPacketSize;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("BULK OUT MaximumPacketSize = %d\n", pAd->BulkOutMaxPacketSize));
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("EP address = 0x%2x  \n", endpoint[i].bEndpointAddress));
+		}
+	}
+
+	if (!(pAd->BulkInEpAddr && pAd->BulkOutEpAddr[0]))
+	{
+		printk("Could not find both bulk-in and bulk-out endpoints\n");
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+#else
+BOOLEAN RT28XXProbePostConfig(
+	IN void 				*_dev_p,
+	IN RTMP_ADAPTER 		*pAd,
+	IN INT32				interface)
+{
+	struct usb_interface *intf = (struct usb_interface *)_dev_p;
+	struct usb_host_interface *iface_desc;
+	ULONG BulkOutIdx;
+	UINT32 i;
+
+
+	/* get the active interface descriptor */
+	iface_desc = intf->cur_altsetting;
+
+	/* get # of enpoints  */
+	pAd->NumberOfPipes = iface_desc->desc.bNumEndpoints;
+	DBGPRINT(RT_DEBUG_TRACE,
+			("NumEndpoints=%d\n", iface_desc->desc.bNumEndpoints));
+
+	/* Configure Pipes */
+	BulkOutIdx = 0;
+
+	for(i=0; i<pAd->NumberOfPipes; i++)
+	{
+		if ((iface_desc->endpoint[i].desc.bmAttributes ==
+				USB_ENDPOINT_XFER_BULK) &&
+			((iface_desc->endpoint[i].desc.bEndpointAddress &
+				USB_ENDPOINT_DIR_MASK) == USB_DIR_IN))
+		{
+			pAd->BulkInEpAddr = iface_desc->endpoint[i].desc.bEndpointAddress;
+			pAd->BulkInMaxPacketSize = iface_desc->endpoint[i].desc.wMaxPacketSize;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("BULK IN MaximumPacketSize = %d\n", pAd->BulkInMaxPacketSize));
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("EP address = 0x%2x\n", iface_desc->endpoint[i].desc.bEndpointAddress));
+		}
+		else if ((iface_desc->endpoint[i].desc.bmAttributes ==
+					USB_ENDPOINT_XFER_BULK) &&
+				((iface_desc->endpoint[i].desc.bEndpointAddress &
+					USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT))
+		{
+			// there are 6 bulk out EP. EP6 highest priority.
+			// EP1-4 is EDCA.  EP5 is HCCA.
+			pAd->BulkOutEpAddr[BulkOutIdx++] = iface_desc->endpoint[i].desc.bEndpointAddress;
+			pAd->BulkOutMaxPacketSize = iface_desc->endpoint[i].desc.wMaxPacketSize;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("BULK OUT MaximumPacketSize = %d\n", pAd->BulkOutMaxPacketSize));
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("EP address = 0x%2x  \n", iface_desc->endpoint[i].desc.bEndpointAddress));
+		}
+	}
+
+	if (!(pAd->BulkInEpAddr && pAd->BulkOutEpAddr[0]))
+	{
+		printk("%s: Could not find both bulk-in and bulk-out endpoints\n", __func__);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+#endif // LINUX_VERSION_CODE //
+
+
+/*
+========================================================================
+Routine Description:
+    Disable DMA.
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+	None
+
+Note:
+========================================================================
+*/
+VOID RT28XXDMADisable(
+	IN RTMP_ADAPTER 		*pAd)
+{
+	// no use
+}
+
+
+
+/*
+========================================================================
+Routine Description:
+    Enable DMA.
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+	None
+
+Note:
+========================================================================
+*/
+VOID RT28XXDMAEnable(
+	IN RTMP_ADAPTER 		*pAd)
+{
+	WPDMA_GLO_CFG_STRUC	GloCfg;
+	USB_DMA_CFG_STRUC	UsbCfg;
+	int					i = 0;
+
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x4);
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0)  && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("==>  DMABusy\n"));
+		RTMPusecDelay(1000);
+		i++;
+	}while ( i <200);
+
+
+	RTMPusecDelay(50);
+	GloCfg.field.EnTXWriteBackDDONE = 1;
+	GloCfg.field.EnableRxDMA = 1;
+	GloCfg.field.EnableTxDMA = 1;
+	DBGPRINT(RT_DEBUG_TRACE, ("<== WRITE DMA offset 0x208 = 0x%x\n", GloCfg.word));
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+	UsbCfg.word = 0;
+	UsbCfg.field.phyclear = 0;
+	/* usb version is 1.1,do not use bulk in aggregation */
+	if (pAd->BulkInMaxPacketSize == 512)
+			UsbCfg.field.RxBulkAggEn = 1;
+	/* for last packet, PBF might use more than limited, so minus 2 to prevent from error */
+	UsbCfg.field.RxBulkAggLmt = (MAX_RXBULK_SIZE /1024)-3;
+	UsbCfg.field.RxBulkAggTOut = 0x80; /* 2006-10-18 */
+	UsbCfg.field.RxBulkEn = 1;
+	UsbCfg.field.TxBulkEn = 1;
+
+	RTUSBWriteMACRegister(pAd, USB_DMA_CFG, UsbCfg.word);
+
+}
+
+/*
+========================================================================
+Routine Description:
+    Write Beacon buffer to Asic.
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+	None
+
+Note:
+========================================================================
+*/
+VOID RT28xx_UpdateBeaconToAsic(
+	IN RTMP_ADAPTER		*pAd,
+	IN INT				apidx,
+	IN ULONG			FrameLen,
+	IN ULONG			UpdatePos)
+{
+	PUCHAR        	pBeaconFrame = NULL;
+	UCHAR  			*ptr;
+	UINT  			i, padding;
+	BEACON_SYNC_STRUCT	*pBeaconSync = pAd->CommonCfg.pBeaconSync;
+	UINT32			longValue;
+	BOOLEAN			bBcnReq = FALSE;
+	UCHAR			bcn_idx = 0;
+
+
+	if (pBeaconFrame == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("pBeaconFrame is NULL!\n"));
+		return;
+	}
+
+	if (pBeaconSync == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("pBeaconSync is NULL!\n"));
+		return;
+	}
+
+	//if ((pAd->WdsTab.Mode == WDS_BRIDGE_MODE) ||
+	//	((pAd->ApCfg.MBSSID[apidx].MSSIDDev == NULL) || !(pAd->ApCfg.MBSSID[apidx].MSSIDDev->flags & IFF_UP))
+	//	)
+	if (bBcnReq == FALSE)
+	{
+		/* when the ra interface is down, do not send its beacon frame */
+		/* clear all zero */
+		for(i=0; i<TXWI_SIZE; i+=4) {
+			RTMP_IO_WRITE32(pAd, pAd->BeaconOffset[bcn_idx] + i, 0x00);
+		}
+		pBeaconSync->BeaconBitMap &= (~(BEACON_BITMAP_MASK & (1 << bcn_idx)));
+		NdisZeroMemory(pBeaconSync->BeaconTxWI[bcn_idx], TXWI_SIZE);
+	}
+	else
+	{
+		ptr = (PUCHAR)&pAd->BeaconTxWI;
+#ifdef RT_BIG_ENDIAN
+		RTMPWIEndianChange(ptr, TYPE_TXWI);
+#endif
+		if (NdisEqualMemory(pBeaconSync->BeaconTxWI[bcn_idx], &pAd->BeaconTxWI, TXWI_SIZE) == FALSE)
+		{	// If BeaconTxWI changed, we need to rewrite the TxWI for the Beacon frames.
+			pBeaconSync->BeaconBitMap &= (~(BEACON_BITMAP_MASK & (1 << bcn_idx)));
+			NdisMoveMemory(pBeaconSync->BeaconTxWI[bcn_idx], &pAd->BeaconTxWI, TXWI_SIZE);
+		}
+
+		if ((pBeaconSync->BeaconBitMap & (1 << bcn_idx)) != (1 << bcn_idx))
+		{
+			for (i=0; i<TXWI_SIZE; i+=4)  // 16-byte TXWI field
+			{
+				longValue =  *ptr + (*(ptr+1)<<8) + (*(ptr+2)<<16) + (*(ptr+3)<<24);
+				RTMP_IO_WRITE32(pAd, pAd->BeaconOffset[bcn_idx] + i, longValue);
+				ptr += 4;
+			}
+		}
+
+		ptr = pBeaconSync->BeaconBuf[bcn_idx];
+		padding = (FrameLen & 0x01);
+		NdisZeroMemory((PUCHAR)(pBeaconFrame + FrameLen), padding);
+		FrameLen += padding;
+		for (i = 0 ; i < FrameLen /*HW_BEACON_OFFSET*/; i += 2)
+		{
+			if (NdisEqualMemory(ptr, pBeaconFrame, 2) == FALSE)
+			{
+				NdisMoveMemory(ptr, pBeaconFrame, 2);
+				//shortValue = *ptr + (*(ptr+1)<<8);
+				//RTMP_IO_WRITE8(pAd, pAd->BeaconOffset[bcn_idx] + TXWI_SIZE + i, shortValue);
+				RTUSBMultiWrite(pAd, pAd->BeaconOffset[bcn_idx] + TXWI_SIZE + i, ptr, 2);
+			}
+			ptr +=2;
+			pBeaconFrame += 2;
+		}
+
+		pBeaconSync->BeaconBitMap |= (1 << bcn_idx);
+	}
+
+}
+
+
+VOID RT2870_BssBeaconStop(
+	IN RTMP_ADAPTER *pAd)
+{
+	BEACON_SYNC_STRUCT	*pBeaconSync;
+	int i, offset;
+	BOOLEAN	Cancelled = TRUE;
+
+	pBeaconSync = pAd->CommonCfg.pBeaconSync;
+	if (pBeaconSync && pBeaconSync->EnableBeacon)
+	{
+		INT NumOfBcn;
+
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			NumOfBcn = MAX_MESH_NUM;
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		RTMPCancelTimer(&pAd->CommonCfg.BeaconUpdateTimer, &Cancelled);
+
+		for(i=0; i<NumOfBcn; i++)
+		{
+			NdisZeroMemory(pBeaconSync->BeaconBuf[i], HW_BEACON_OFFSET);
+			NdisZeroMemory(pBeaconSync->BeaconTxWI[i], TXWI_SIZE);
+
+			for (offset=0; offset<HW_BEACON_OFFSET; offset+=4)
+				RTMP_IO_WRITE32(pAd, pAd->BeaconOffset[i] + offset, 0x00);
+
+			pBeaconSync->CapabilityInfoLocationInBeacon[i] = 0;
+			pBeaconSync->TimIELocationInBeacon[i] = 0;
+		}
+		pBeaconSync->BeaconBitMap = 0;
+		pBeaconSync->DtimBitOn = 0;
+	}
+}
+
+
+VOID RT2870_BssBeaconStart(
+	IN RTMP_ADAPTER *pAd)
+{
+	int apidx;
+	BEACON_SYNC_STRUCT	*pBeaconSync;
+//	LARGE_INTEGER 	tsfTime, deltaTime;
+
+	pBeaconSync = pAd->CommonCfg.pBeaconSync;
+	if (pBeaconSync && pBeaconSync->EnableBeacon)
+	{
+		INT NumOfBcn;
+
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			NumOfBcn = MAX_MESH_NUM;
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		for(apidx=0; apidx<NumOfBcn; apidx++)
+		{
+			UCHAR CapabilityInfoLocationInBeacon = 0;
+			UCHAR TimIELocationInBeacon = 0;
+
+			NdisZeroMemory(pBeaconSync->BeaconBuf[apidx], HW_BEACON_OFFSET);
+			pBeaconSync->CapabilityInfoLocationInBeacon[apidx] = CapabilityInfoLocationInBeacon;
+			pBeaconSync->TimIELocationInBeacon[apidx] = TimIELocationInBeacon;
+			NdisZeroMemory(pBeaconSync->BeaconTxWI[apidx], TXWI_SIZE);
+		}
+		pBeaconSync->BeaconBitMap = 0;
+		pBeaconSync->DtimBitOn = 0;
+		pAd->CommonCfg.BeaconUpdateTimer.Repeat = TRUE;
+
+		pAd->CommonCfg.BeaconAdjust = 0;
+		pAd->CommonCfg.BeaconFactor = 0xffffffff / (pAd->CommonCfg.BeaconPeriod << 10);
+		pAd->CommonCfg.BeaconRemain = (0xffffffff % (pAd->CommonCfg.BeaconPeriod << 10)) + 1;
+		printk("RT2870_BssBeaconStart:BeaconFactor=%d, BeaconRemain=%d!\n", pAd->CommonCfg.BeaconFactor, pAd->CommonCfg.BeaconRemain);
+		RTMPSetTimer(&pAd->CommonCfg.BeaconUpdateTimer, pAd->CommonCfg.BeaconPeriod);
+
+	}
+}
+
+
+VOID RT2870_BssBeaconInit(
+	IN RTMP_ADAPTER *pAd)
+{
+	BEACON_SYNC_STRUCT	*pBeaconSync;
+	int i;
+
+	NdisAllocMemory(pAd->CommonCfg.pBeaconSync, sizeof(BEACON_SYNC_STRUCT), MEM_ALLOC_FLAG);
+	if (pAd->CommonCfg.pBeaconSync)
+	{
+		pBeaconSync = pAd->CommonCfg.pBeaconSync;
+		NdisZeroMemory(pBeaconSync, sizeof(BEACON_SYNC_STRUCT));
+		for(i=0; i < HW_BEACON_MAX_COUNT; i++)
+		{
+			NdisZeroMemory(pBeaconSync->BeaconBuf[i], HW_BEACON_OFFSET);
+			pBeaconSync->CapabilityInfoLocationInBeacon[i] = 0;
+			pBeaconSync->TimIELocationInBeacon[i] = 0;
+			NdisZeroMemory(pBeaconSync->BeaconTxWI[i], TXWI_SIZE);
+		}
+		pBeaconSync->BeaconBitMap = 0;
+
+		//RTMPInitTimer(pAd, &pAd->CommonCfg.BeaconUpdateTimer, GET_TIMER_FUNCTION(BeaconUpdateExec), pAd, TRUE);
+		pBeaconSync->EnableBeacon = TRUE;
+	}
+}
+
+
+VOID RT2870_BssBeaconExit(
+	IN RTMP_ADAPTER *pAd)
+{
+	BEACON_SYNC_STRUCT	*pBeaconSync;
+	BOOLEAN	Cancelled = TRUE;
+	int i;
+
+	if (pAd->CommonCfg.pBeaconSync)
+	{
+		pBeaconSync = pAd->CommonCfg.pBeaconSync;
+		pBeaconSync->EnableBeacon = FALSE;
+		RTMPCancelTimer(&pAd->CommonCfg.BeaconUpdateTimer, &Cancelled);
+		pBeaconSync->BeaconBitMap = 0;
+
+		for(i=0; i<HW_BEACON_MAX_COUNT; i++)
+		{
+			NdisZeroMemory(pBeaconSync->BeaconBuf[i], HW_BEACON_OFFSET);
+			pBeaconSync->CapabilityInfoLocationInBeacon[i] = 0;
+			pBeaconSync->TimIELocationInBeacon[i] = 0;
+			NdisZeroMemory(pBeaconSync->BeaconTxWI[i], TXWI_SIZE);
+		}
+
+		NdisFreeMemory(pAd->CommonCfg.pBeaconSync, HW_BEACON_OFFSET * HW_BEACON_MAX_COUNT, 0);
+		pAd->CommonCfg.pBeaconSync = NULL;
+	}
+}
+
+VOID BeaconUpdateExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+	PRTMP_ADAPTER	pAd = (PRTMP_ADAPTER)FunctionContext;
+	LARGE_INTEGER	tsfTime_a;//, tsfTime_b, deltaTime_exp, deltaTime_ab;
+	UINT32			delta, remain, remain_low, remain_high;
+//	BOOLEAN			positive;
+
+	ReSyncBeaconTime(pAd);
+
+
+
+	RTMP_IO_READ32(pAd, TSF_TIMER_DW0, &tsfTime_a.u.LowPart);
+	RTMP_IO_READ32(pAd, TSF_TIMER_DW1, &tsfTime_a.u.HighPart);
+
+
+	//positive=getDeltaTime(tsfTime_a, expectedTime, &deltaTime_exp);
+	remain_high = pAd->CommonCfg.BeaconRemain * tsfTime_a.u.HighPart;
+	remain_low = tsfTime_a.u.LowPart % (pAd->CommonCfg.BeaconPeriod << 10);
+	remain = (remain_high + remain_low)%(pAd->CommonCfg.BeaconPeriod << 10);
+	delta = (pAd->CommonCfg.BeaconPeriod << 10) - remain;
+
+	pAd->CommonCfg.BeaconUpdateTimer.TimerValue = (delta >> 10) + 10;
+
+}
+
diff --git a/drivers/staging/rt2870/Kconfig b/drivers/staging/rt2870/Kconfig
new file mode 100644
index 000000000000..8398d9797e1d
--- /dev/null
+++ b/drivers/staging/rt2870/Kconfig
@@ -0,0 +1,6 @@
+config RT2870
+	tristate "Ralink 2870 wireless support"
+	depends on USB && X86 && WLAN_80211
+	---help---
+	  This is an experimental driver for the Ralink 2870 wireless chip.
+
diff --git a/drivers/staging/rt2870/Makefile b/drivers/staging/rt2870/Makefile
new file mode 100644
index 000000000000..1a015f408e69
--- /dev/null
+++ b/drivers/staging/rt2870/Makefile
@@ -0,0 +1,47 @@
+obj-$(CONFIG_RT2870)	+= rt2870sta.o
+
+# TODO: all of these should be removed
+EXTRA_CFLAGS += -DLINUX -DAGGREGATION_SUPPORT -DPIGGYBACK_SUPPORT -DWMM_SUPPORT
+EXTRA_CFLAGS += -DRT2870
+EXTRA_CFLAGS += -DCONFIG_STA_SUPPORT
+EXTRA_CFLAGS += -DDBG
+EXTRA_CFLAGS += -DDOT11_N_SUPPORT
+EXTRA_CFLAGS += -DWPA_SUPPLICANT_SUPPORT
+EXTRA_CFLAGS += -DNATIVE_WPA_SUPPLICANT_SUPPORT
+
+rt2870sta-objs :=		\
+	common/md5.o		\
+	common/mlme.o		\
+	common/rtmp_wep.o	\
+	common/action.o		\
+	common/cmm_data.o	\
+	common/rtmp_init.o	\
+	common/rtmp_tkip.o	\
+	common/cmm_sync.o	\
+	common/eeprom.o		\
+	common/cmm_sanity.o	\
+	common/cmm_info.o	\
+	common/cmm_wpa.o	\
+	common/dfs.o		\
+	common/spectrum.o	\
+	sta/assoc.o		\
+	sta/aironet.o		\
+	sta/auth.o		\
+	sta/auth_rsp.o		\
+	sta/sync.o		\
+	sta/sanity.o		\
+	sta/rtmp_data.o		\
+	sta/connect.o		\
+	sta/wpa.o		\
+	rt_linux.o		\
+	rt_profile.o		\
+	rt_main_dev.o		\
+	sta_ioctl.o		\
+	common/ba_action.o	\
+	2870_main_dev.o		\
+	common/2870_rtmp_init.o	\
+	common/rtusb_io.o	\
+	common/rtusb_bulk.o	\
+	common/rtusb_data.o	\
+	common/cmm_data_2870.o
+
diff --git a/drivers/staging/rt2870/TODO b/drivers/staging/rt2870/TODO
new file mode 100644
index 000000000000..eae1ac47d3f9
--- /dev/null
+++ b/drivers/staging/rt2870/TODO
@@ -0,0 +1,10 @@
+TODO:
+	- checkpatch.pl clean
+	- sparse clean
+	- port to in-kernel 80211 stack
+	- remove reading from /etc/ config files
+	- review by the wireless developer community
+
+Please send any patches or complaints about this driver to Greg
+Kroah-Hartman <greg@kroah.com> and don't bother the upstream wireless
+kernel developers about it, they want nothing to do with it.
diff --git a/drivers/staging/rt2870/aironet.h b/drivers/staging/rt2870/aironet.h
new file mode 100644
index 000000000000..1e07b19b8cdc
--- /dev/null
+++ b/drivers/staging/rt2870/aironet.h
@@ -0,0 +1,210 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	aironet.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	Paul Lin	04-06-15		Initial
+*/
+
+#ifndef	__AIRONET_H__
+#define	__AIRONET_H__
+
+// Measurement Type definition
+#define	MSRN_TYPE_UNUSED				0
+#define	MSRN_TYPE_CHANNEL_LOAD_REQ		1
+#define	MSRN_TYPE_NOISE_HIST_REQ		2
+#define	MSRN_TYPE_BEACON_REQ			3
+#define	MSRN_TYPE_FRAME_REQ				4
+
+// Scan Mode in Beacon Request
+#define	MSRN_SCAN_MODE_PASSIVE			0
+#define	MSRN_SCAN_MODE_ACTIVE			1
+#define	MSRN_SCAN_MODE_BEACON_TABLE		2
+
+// PHY type definition for Aironet beacon report, CCX 2 table 36-9
+#define	PHY_FH							1
+#define	PHY_DSS							2
+#define	PHY_UNUSED						3
+#define	PHY_OFDM						4
+#define	PHY_HR_DSS						5
+#define	PHY_ERP							6
+
+// RPI table in dBm
+#define	RPI_0			0			//	Power <= -87
+#define	RPI_1			1			//	-87 < Power <= -82
+#define	RPI_2			2			//	-82 < Power <= -77
+#define	RPI_3			3			//	-77 < Power <= -72
+#define	RPI_4			4			//	-72 < Power <= -67
+#define	RPI_5			5			//	-67 < Power <= -62
+#define	RPI_6			6			//	-62 < Power <= -57
+#define	RPI_7			7			//	-57 < Power
+
+// Cisco Aironet IAPP definetions
+#define	AIRONET_IAPP_TYPE					0x32
+#define	AIRONET_IAPP_SUBTYPE_REQUEST		0x01
+#define	AIRONET_IAPP_SUBTYPE_REPORT			0x81
+
+// Measurement Request detail format
+typedef	struct	_MEASUREMENT_REQUEST	{
+	UCHAR	Channel;
+	UCHAR	ScanMode;			// Use only in beacon request, other requests did not use this field
+	USHORT	Duration;
+}	MEASUREMENT_REQUEST, *PMEASUREMENT_REQUEST;
+
+// Beacon Measurement Report
+// All these field might change to UCHAR, because we didn't do anything to these report.
+// We copy all these beacons and report to CCX 2 AP.
+typedef	struct	_BEACON_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	PhyType;			// Definiation is listed above table 36-9
+	UCHAR	RxPower;
+	UCHAR	BSSID[6];
+	UCHAR	ParentTSF[4];
+	UCHAR	TargetTSF[8];
+	USHORT	BeaconInterval;
+	USHORT	CapabilityInfo;
+}	BEACON_REPORT, *PBEACON_REPORT;
+
+// Frame Measurement Report (Optional)
+typedef	struct	_FRAME_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	TA;
+	UCHAR	BSSID[6];
+	UCHAR	RSSI;
+	UCHAR	Count;
+}	FRAME_REPORT, *PFRAME_REPORT;
+
+#pragma pack(1)
+// Channel Load Report
+typedef	struct	_CHANNEL_LOAD_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	CCABusy;
+}	CHANNEL_LOAD_REPORT, *PCHANNEL_LOAD_REPORT;
+#pragma pack()
+
+// Nosie Histogram Report
+typedef	struct	_NOISE_HIST_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	Density[8];
+}	NOISE_HIST_REPORT, *PNOISE_HIST_REPORT;
+
+// Radio Management Capability element
+typedef	struct	_RADIO_MANAGEMENT_CAPABILITY	{
+	UCHAR	Eid;				// TODO: Why the Eid is 1 byte, not normal 2 bytes???
+	UCHAR	Length;
+	UCHAR	AironetOui[3];		// AIronet OUI (00 40 96)
+	UCHAR	Type;				// Type / Version
+	USHORT	Status;				// swap16 required
+}	RADIO_MANAGEMENT_CAPABILITY, *PRADIO_MANAGEMENT_CAPABILITY;
+
+// Measurement Mode Bit definition
+typedef	struct	_MEASUREMENT_MODE	{
+	UCHAR	Rsvd:4;
+	UCHAR	Report:1;
+	UCHAR	NotUsed:1;
+	UCHAR	Enable:1;
+	UCHAR	Parallel:1;
+}	MEASUREMENT_MODE, *PMEASUREMENT_MODE;
+
+// Measurement Request element, This is little endian mode
+typedef	struct	_MEASUREMENT_REQUEST_ELEMENT	{
+	USHORT				Eid;
+	USHORT				Length;				// swap16 required
+	USHORT				Token;				// non-zero unique token
+	UCHAR				Mode;				// Measurement Mode
+	UCHAR				Type;				// Measurement type
+}	MEASUREMENT_REQUEST_ELEMENT, *PMEASUREMENT_REQUEST_ELEMENT;
+
+// Measurement Report element, This is little endian mode
+typedef	struct	_MEASUREMENT_REPORT_ELEMENT	{
+	USHORT				Eid;
+	USHORT				Length;				// swap16 required
+	USHORT				Token;				// non-zero unique token
+	UCHAR				Mode;				// Measurement Mode
+	UCHAR				Type;				// Measurement type
+}	MEASUREMENT_REPORT_ELEMENT, *PMEASUREMENT_REPORT_ELEMENT;
+
+// Cisco Aironet IAPP Frame Header, Network byte order used
+typedef	struct	_AIRONET_IAPP_HEADER {
+	UCHAR	CiscoSnapHeader[8];	// 8 bytes Cisco snap header
+	USHORT	Length;				// IAPP ID & length, remember to swap16 in LE system
+	UCHAR	Type;				// IAPP type
+	UCHAR	SubType;			// IAPP subtype
+	UCHAR	DA[6];				// Destination MAC address
+	UCHAR	SA[6];				// Source MAC address
+	USHORT	Token;				// Dialog token, no need to swap16 since it is for yoken usage only
+}	AIRONET_IAPP_HEADER, *PAIRONET_IAPP_HEADER;
+
+// Radio Measurement Request frame
+typedef	struct	_AIRONET_RM_REQUEST_FRAME	{
+    AIRONET_IAPP_HEADER	IAPP;			// Common header
+	UCHAR				Delay;			// Activation Delay
+	UCHAR				Offset;			// Measurement offset
+}	AIRONET_RM_REQUEST_FRAME, *PAIRONET_RM_REQUEST_FRAME;
+
+// Radio Measurement Report frame
+typedef	struct	_AIRONET_RM_REPORT_FRAME	{
+    AIRONET_IAPP_HEADER	IAPP;			// Common header
+}	AIRONET_RM_REPORT_FRAME, *PAIRONET_RM_REPORT_FRAME;
+
+// Saved element request actions which will saved in StaCfg.
+typedef	struct	_RM_REQUEST_ACTION	{
+	MEASUREMENT_REQUEST_ELEMENT	ReqElem;		// Saved request element
+	MEASUREMENT_REQUEST			Measurement;	// Saved measurement within the request element
+}	RM_REQUEST_ACTION, *PRM_REQUEST_ACTION;
+
+// CCX administration control
+typedef	union	_CCX_CONTROL	{
+	struct	{
+		UINT32		Enable:1;			// Enable CCX2
+		UINT32		LeapEnable:1;		// Enable LEAP at CCX2
+		UINT32		RMEnable:1;			// Radio Measurement Enable
+		UINT32		DCRMEnable:1;		// Non serving channel Radio Measurement enable
+		UINT32		QOSEnable:1;		// Enable QOS for CCX 2.0 support
+		UINT32		FastRoamEnable:1;	// Enable fast roaming
+		UINT32		Rsvd:2;				// Not used
+		UINT32		dBmToRoam:8;		// the condition to roam when receiving Rssi less than this value. It's negative value.
+		UINT32		TuLimit:16;			// Limit for different channel scan
+	}	field;
+	UINT32			word;
+}	CCX_CONTROL, *PCCX_CONTROL;
+
+#endif	// __AIRONET_H__
diff --git a/drivers/staging/rt2870/ap.h b/drivers/staging/rt2870/ap.h
new file mode 100644
index 000000000000..0dc557516790
--- /dev/null
+++ b/drivers/staging/rt2870/ap.h
@@ -0,0 +1,562 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    ap.h
+
+    Abstract:
+    Miniport generic portion header file
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Paul Lin    08-01-2002    created
+    James Tan   09-06-2002    modified (Revise NTCRegTable)
+    John Chang  12-22-2004    modified for RT2561/2661. merge with STA driver
+*/
+#ifndef __AP_H__
+#define __AP_H__
+
+
+
+// ========================= AP RTMP.h ================================
+
+
+
+// =============================================================
+//      Function Prototypes
+// =============================================================
+
+// ap_data.c
+
+BOOLEAN APBridgeToWirelessSta(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pHeader,
+    IN  UINT            HdrLen,
+    IN  PUCHAR          pData,
+    IN  UINT            DataLen,
+    IN  ULONG           fromwdsidx);
+
+BOOLEAN APHandleRxDoneInterrupt(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID	APSendPackets(
+	IN	NDIS_HANDLE		MiniportAdapterContext,
+	IN	PPNDIS_PACKET	ppPacketArray,
+	IN	UINT			NumberOfPackets);
+
+NDIS_STATUS APSendPacket(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PNDIS_PACKET    pPacket);
+
+
+NDIS_STATUS APHardTransmit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			QueIdx);
+
+VOID APRxEAPOLFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+NDIS_STATUS APCheckRxError(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PRT28XX_RXD_STRUC		pRxD,
+	IN	UCHAR			Wcid);
+
+BOOLEAN APCheckClass2Class3Error(
+    IN  PRTMP_ADAPTER   pAd,
+	IN ULONG Wcid,
+	IN  PHEADER_802_11  pHeader);
+
+VOID APHandleRxPsPoll(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pAddr,
+	IN	USHORT			Aid,
+    IN	BOOLEAN			isActive);
+
+VOID    RTMPDescriptorEndianChange(
+    IN  PUCHAR          pData,
+    IN  ULONG           DescriptorType);
+
+VOID    RTMPFrameEndianChange(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pData,
+    IN  ULONG           Dir,
+    IN  BOOLEAN         FromRxDoneInt);
+
+// ap_assoc.c
+
+VOID APAssocStateMachineInit(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  STATE_MACHINE *S,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID  APPeerAssocReqAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  APPeerReassocReqAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  APPeerDisassocReqAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MbssKickOutStas(
+	IN PRTMP_ADAPTER pAd,
+	IN INT apidx,
+	IN USHORT Reason);
+
+VOID APMlmeKickOutSta(
+    IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pStaAddr,
+	IN UCHAR Wcid,
+	IN USHORT Reason);
+
+VOID APMlmeDisassocReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID  APCls3errAction(
+    IN  PRTMP_ADAPTER   pAd,
+	IN 	ULONG Wcid,
+    IN	PHEADER_802_11	pHeader);
+
+
+USHORT APBuildAssociation(
+    IN PRTMP_ADAPTER pAd,
+    IN MAC_TABLE_ENTRY *pEntry,
+    IN USHORT CapabilityInfo,
+    IN UCHAR  MaxSupportedRateIn500Kbps,
+    IN UCHAR  *RSN,
+    IN UCHAR  *pRSNLen,
+    IN BOOLEAN bWmmCapable,
+    IN ULONG  RalinkIe,
+#ifdef DOT11N_DRAFT3
+    IN EXT_CAP_INFO_ELEMENT ExtCapInfo,
+#endif // DOT11N_DRAFT3 //
+	IN HT_CAPABILITY_IE		*pHtCapability,
+	IN UCHAR		 HtCapabilityLen,
+    OUT USHORT *pAid);
+
+/*
+VOID	RTMPAddClientSec(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	BssIdx,
+	IN UCHAR		 KeyIdx,
+	IN UCHAR		 CipherAlg,
+	IN PUCHAR		 pKey,
+	IN PUCHAR		 pTxMic,
+	IN PUCHAR		 pRxMic,
+	IN MAC_TABLE_ENTRY *pEntry);
+*/
+
+// ap_auth.c
+
+void APAuthStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID APMlmeDeauthReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID APCls2errAction(
+    IN PRTMP_ADAPTER pAd,
+	IN 	ULONG Wcid,
+    IN	PHEADER_802_11	pHeader);
+
+// ap_authrsp.c
+
+VOID APAuthRspStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN PSTATE_MACHINE Sm,
+    IN STATE_MACHINE_FUNC Trans[]);
+
+VOID APPeerAuthAtAuthRspIdleAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerDeauthReqAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerAuthSimpleRspGenAndSend(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PHEADER_802_11 pHdr80211,
+    IN  USHORT Alg,
+    IN  USHORT Seq,
+    IN  USHORT StatusCode);
+
+// ap_connect.c
+
+BOOLEAN BeaconTransmitRequired(
+	IN PRTMP_ADAPTER	pAd,
+	IN INT				apidx);
+
+VOID APMakeBssBeacon(
+    IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx);
+
+VOID  APUpdateBeaconFrame(
+    IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx);
+
+VOID APMakeAllBssBeacon(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID  APUpdateAllBeaconFrame(
+    IN  PRTMP_ADAPTER   pAd);
+
+
+// ap_sync.c
+
+VOID APSyncStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID APScanTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID APInvalidStateWhenScan(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APScanTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerProbeReqAction(
+    IN  PRTMP_ADAPTER pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerBeaconAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID APMlmeScanReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerBeaconAtScanAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APScanCnclAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID ApSiteSurvey(
+	IN PRTMP_ADAPTER pAd);
+
+VOID SupportRate(
+	IN PUCHAR SupRate,
+	IN UCHAR SupRateLen,
+	IN PUCHAR ExtRate,
+	IN UCHAR ExtRateLen,
+	OUT PUCHAR *Rates,
+	OUT PUCHAR RatesLen,
+	OUT PUCHAR pMaxSupportRate);
+
+
+BOOLEAN ApScanRunning(
+	IN PRTMP_ADAPTER pAd);
+
+#ifdef DOT11N_DRAFT3
+VOID APOverlappingBSSScan(
+	IN RTMP_ADAPTER *pAd);
+#endif // DOT11N_DRAFT3 //
+
+// ap_wpa.c
+
+VOID APWpaStateMachineInit(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+// ap_mlme.c
+
+VOID APMlmePeriodicExec(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APMlmeSelectTxRateTable(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PUCHAR				*ppTable,
+	IN PUCHAR				pTableSize,
+	IN PUCHAR				pInitTxRateIdx);
+
+VOID APMlmeSetTxRate(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PRTMP_TX_RATE_SWITCH	pTxRate);
+
+VOID APMlmeDynamicTxRateSwitching(
+    IN PRTMP_ADAPTER pAd);
+
+VOID APQuickResponeForRateUpExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+BOOLEAN APMsgTypeSubst(
+    IN PRTMP_ADAPTER pAd,
+    IN PFRAME_802_11 pFrame,
+    OUT INT *Machine,
+    OUT INT *MsgType);
+
+VOID APQuickResponeForRateUpExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+#ifdef RT2870
+VOID BeaconUpdateExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+#endif // RT2870 //
+
+VOID RTMPSetPiggyBack(
+	IN PRTMP_ADAPTER	pAd,
+	IN BOOLEAN			bPiggyBack);
+
+VOID APAsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID APAsicRxAntEvalTimeout(
+	IN PRTMP_ADAPTER	pAd);
+
+// ap.c
+
+VOID APSwitchChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN INT Channel);
+
+NDIS_STATUS APInitialize(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APShutdown(
+    IN PRTMP_ADAPTER    pAd);
+
+VOID APStartUp(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APStop(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APCleanupPsQueue(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PQUEUE_HEADER   pQueue);
+
+VOID MacTableReset(
+    IN  PRTMP_ADAPTER   pAd);
+
+MAC_TABLE_ENTRY *MacTableInsertEntry(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr,
+	IN	UCHAR			apidx,
+	IN BOOLEAN	CleanAll);
+
+BOOLEAN MacTableDeleteEntry(
+    IN  PRTMP_ADAPTER   pAd,
+	IN USHORT wcid,
+    IN  PUCHAR          pAddr);
+
+MAC_TABLE_ENTRY *MacTableLookup(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr);
+
+VOID MacTableMaintenance(
+    IN PRTMP_ADAPTER pAd);
+
+UINT32 MacTableAssocStaNumGet(
+	IN PRTMP_ADAPTER pAd);
+
+MAC_TABLE_ENTRY *APSsPsInquiry(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr,
+    OUT SST             *Sst,
+    OUT USHORT          *Aid,
+    OUT UCHAR           *PsMode,
+    OUT UCHAR           *Rate);
+
+BOOLEAN APPsIndicate(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr,
+	IN ULONG Wcid,
+    IN  UCHAR           Psm);
+
+VOID ApLogEvent(
+    IN PRTMP_ADAPTER    pAd,
+    IN PUCHAR           pAddr,
+    IN USHORT           Event);
+
+#ifdef DOT11_N_SUPPORT
+VOID APUpdateOperationMode(
+    IN PRTMP_ADAPTER pAd);
+#endif // DOT11_N_SUPPORT //
+
+VOID APUpdateCapabilityAndErpIe(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN ApCheckAccessControlList(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR         Apidx);
+
+VOID ApUpdateAccessControlList(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR         Apidx);
+
+VOID ApEnqueueNullFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR         TxRate,
+	IN UCHAR         PID,
+	IN UCHAR         apidx,
+    IN BOOLEAN       bQosNull,
+    IN BOOLEAN       bEOSP,
+    IN UCHAR         OldUP);
+
+VOID ApSendFrame(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PVOID           pBuffer,
+    IN  ULONG           Length,
+    IN  UCHAR           TxRate,
+    IN  UCHAR           PID);
+
+VOID ApEnqueueAckFrame(
+    IN PRTMP_ADAPTER pAd,
+    IN PUCHAR        pAddr,
+    IN UCHAR         TxRate,
+	IN UCHAR         apidx);
+
+UCHAR APAutoSelectChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN Optimal);
+
+// ap_sanity.c
+
+
+BOOLEAN PeerAssocReqCmmSanity(
+    IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN isRessoc,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pListenInterval,
+    OUT PUCHAR pApAddr,
+    OUT UCHAR *pSsidLen,
+    OUT char *Ssid,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *RSN,
+    OUT UCHAR *pRSNLen,
+    OUT BOOLEAN *pbWmmCapable,
+#ifdef WSC_AP_SUPPORT
+    OUT BOOLEAN *pWscCapable,
+#endif // WSC_AP_SUPPORT //
+    OUT ULONG  *pRalinkIe,
+#ifdef DOT11N_DRAFT3
+    OUT EXT_CAP_INFO_ELEMENT	*pExtCapInfo,
+#endif // DOT11N_DRAFT3 //
+    OUT UCHAR		 *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability);
+
+BOOLEAN PeerDisassocReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *Reason);
+
+BOOLEAN PeerDeauthReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *Reason);
+
+BOOLEAN APPeerAuthSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+	OUT PUCHAR pAddr1,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *Alg,
+    OUT USHORT *Seq,
+    OUT USHORT *Status,
+    CHAR *ChlgText);
+
+BOOLEAN APPeerProbeReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT CHAR Ssid[],
+    OUT UCHAR *SsidLen);
+
+BOOLEAN APPeerBeaconAndProbeRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT PUCHAR pBssid,
+    OUT CHAR Ssid[],
+    OUT UCHAR *SsidLen,
+    OUT UCHAR *BssType,
+    OUT USHORT *BeaconPeriod,
+    OUT UCHAR *Channel,
+    OUT LARGE_INTEGER *Timestamp,
+    OUT USHORT *CapabilityInfo,
+    OUT UCHAR Rate[],
+    OUT UCHAR *RateLen,
+    OUT BOOLEAN *ExtendedRateIeExist,
+    OUT UCHAR *Erp);
+
+// ap_info.c
+
+
+
+// ================== end of AP RTMP.h ========================
+
+
+#endif  // __AP_H__
+
diff --git a/drivers/staging/rt2870/chlist.h b/drivers/staging/rt2870/chlist.h
new file mode 100644
index 000000000000..9e15b9daeb80
--- /dev/null
+++ b/drivers/staging/rt2870/chlist.h
@@ -0,0 +1,1296 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	chlist.c
+
+	Abstract:
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Fonchi Wu   2007-12-19    created
+*/
+
+#ifndef __CHLIST_H__
+#define __CHLIST_H__
+
+#include "rtmp_type.h"
+#include "rtmp_def.h"
+
+
+#define ODOR			0
+#define IDOR			1
+#define BOTH			2
+
+#define BAND_5G         0
+#define BAND_24G        1
+#define BAND_BOTH       2
+
+typedef struct _CH_DESP {
+	UCHAR FirstChannel;
+	UCHAR NumOfCh;
+	CHAR MaxTxPwr;			// dBm
+	UCHAR Geography;			// 0:out door, 1:in door, 2:both
+	BOOLEAN DfsReq;			// Dfs require, 0: No, 1: yes.
+} CH_DESP, *PCH_DESP;
+
+typedef struct _CH_REGION {
+	UCHAR CountReg[3];
+	UCHAR DfsType;			// 0: CE, 1: FCC, 2: JAP, 3:JAP_W53, JAP_W56
+	CH_DESP ChDesp[10];
+} CH_REGION, *PCH_REGION;
+
+static CH_REGION ChRegion[] =
+{
+		{	// Antigua and Berbuda
+			"AG",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Argentina
+			"AR",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Aruba
+			"AW",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Australia
+			"AU",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Austria
+			"AT",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, TRUE},		// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},		// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, TRUE},		// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Bahamas
+			"BS",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Barbados
+			"BB",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Bermuda
+			"BM",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Brazil
+			"BR",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 24, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Belgium
+			"BE",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  18, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  18, IDOR, FALSE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Bulgaria
+			"BG",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11, 30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Canada
+			"CA",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Cayman IsLands
+			"KY",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Chile
+			"CL",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  20, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  20, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// China
+			"CN",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149, 4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Colombia
+			"CO",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Costa Rica
+			"CR",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Cyprus
+			"CY",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, IDOR, TRUE},		// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, TRUE},		// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Czech_Republic
+			"CZ",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},		// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Denmark
+			"DK",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},		// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, TRUE},		// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Dominican Republic
+			"DO",
+			CE,
+			{
+				{ 1,   0,  20, BOTH, FALSE},	// 2.4 G, ch 0
+				{ 149, 4,  20, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Equador
+			"EC",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 100, 11,  27, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// El Salvador
+			"SV",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   30, BOTH, TRUE},	// 5G, ch 52~64
+				{ 149, 4,   36, BOTH, TRUE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Finland
+			"FI",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// France
+			"FR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Germany
+			"DE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Greece
+			"GR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Guam
+			"GU",
+			CE,
+			{
+				{ 1,   11,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,  4,   17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149,  5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Guatemala
+			"GT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Haiti
+			"HT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Honduras
+			"HN",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149,  4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Hong Kong
+			"HK",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Hungary
+			"HU",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Iceland
+			"IS",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// India
+			"IN",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149, 	4,  24, IDOR, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Indonesia
+			"ID",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149, 	4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Ireland
+			"IE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Israel
+			"IL",
+			CE,
+			{
+				{ 1,    3,  20, IDOR, FALSE},	// 2.4 G, ch 1~3
+				{ 4, 	6,  20, BOTH, FALSE},	// 2.4 G, ch 4~9
+				{ 10, 	4,  20, IDOR, FALSE},	// 2.4 G, ch 10~13
+				{ 0},							// end
+			}
+		},
+
+		{	// Italy
+			"IT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Japan
+			"JP",
+			JAP,
+			{
+				{ 1,   14,  20, BOTH, FALSE},	// 2.4 G, ch 1~14
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 0},							// end
+			}
+		},
+
+		{	// Jordan
+			"JO",
+			CE,
+			{
+				{ 1,   13,  20, IDOR, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 149, 	4,  23, IDOR, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Latvia
+			"LV",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Liechtenstein
+			"LI",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Lithuania
+			"LT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Luxemburg
+			"LU",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Malaysia
+			"MY",
+			CE,
+			{
+				{ 36, 	4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  5,  20, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Malta
+			"MT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Marocco
+			"MA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  24, IDOR, FALSE},	// 5G, ch 36~48
+				{ 0},							// end
+			}
+		},
+
+		{	// Mexico
+			"MX",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  5,  30, IDOR, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Netherlands
+			"NL",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// New Zealand
+			"NZ",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  24, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Norway
+			"NO",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  24, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Peru
+			"PE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149,  4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Portugal
+			"PT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Poland
+			"PL",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Romania
+			"RO",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Russia
+			"RU",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149,  4,  20, IDOR, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Saudi Arabia
+			"SA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  23, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Serbia_and_Montenegro
+			"CS",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 0},							// end
+			}
+		},
+
+		{	// Singapore
+			"SG",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  20, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Slovakia
+			"SK",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Slovenia
+			"SI",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// South Africa
+			"ZA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// South Korea
+			"KR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  20, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100,  8,  20, BOTH, FALSE},	// 5G, ch 100~128
+				{ 149,  4,  20, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Spain
+			"ES",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  17, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Sweden
+			"SE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Switzerland
+			"CH",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, TRUE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Taiwan
+			"TW",
+			CE,
+			{
+				{ 1,   11,  30, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 52,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Turkey
+			"TR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// UK
+			"GB",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Ukraine
+			"UA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 0},							// end
+			}
+		},
+
+		{	// United_Arab_Emirates
+			"AE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 0},							// end
+			}
+		},
+
+		{	// United_States
+			"US",
+			CE,
+			{
+				{ 1,   11,  30, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  17, IDOR, FALSE},	// 5G, ch 52~64
+				{ 52,   4,  24, BOTH, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 149,  5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Venezuela
+			"VE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 149,  4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Default
+			"",
+			CE,
+			{
+				{ 1,   11,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 52,   4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11,  20, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149,  5,  20, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+};
+
+static inline PCH_REGION GetChRegion(
+	IN PUCHAR CntryCode)
+{
+	INT loop = 0;
+	PCH_REGION pChRegion = NULL;
+
+	while (strcmp(ChRegion[loop].CountReg, "") != 0)
+	{
+		if (strncmp(ChRegion[loop].CountReg, CntryCode, 2) == 0)
+		{
+			pChRegion = &ChRegion[loop];
+			break;
+		}
+		loop++;
+	}
+
+	if (pChRegion == NULL)
+		pChRegion = &ChRegion[loop];
+	return pChRegion;
+}
+
+static inline VOID ChBandCheck(
+	IN UCHAR PhyMode,
+	OUT PUCHAR pChType)
+{
+	switch(PhyMode)
+	{
+		case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11AN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			*pChType = BAND_5G;
+			break;
+		case PHY_11ABG_MIXED:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11AGN_MIXED:
+		case PHY_11ABGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			*pChType = BAND_BOTH;
+			break;
+
+		default:
+			*pChType = BAND_24G;
+			break;
+	}
+}
+
+static inline UCHAR FillChList(
+	IN PRTMP_ADAPTER pAd,
+	IN PCH_DESP pChDesp,
+	IN UCHAR Offset,
+	IN UCHAR increment)
+{
+	INT i, j, l;
+	UCHAR channel;
+
+	j = Offset;
+	for (i = 0; i < pChDesp->NumOfCh; i++)
+	{
+		channel = pChDesp->FirstChannel + i * increment;
+		for (l=0; l<MAX_NUM_OF_CHANNELS; l++)
+		{
+			if (channel == pAd->TxPower[l].Channel)
+			{
+				pAd->ChannelList[j].Power = pAd->TxPower[l].Power;
+				pAd->ChannelList[j].Power2 = pAd->TxPower[l].Power2;
+				break;
+			}
+		}
+		if (l == MAX_NUM_OF_CHANNELS)
+			continue;
+
+		pAd->ChannelList[j].Channel = pChDesp->FirstChannel + i * increment;
+		pAd->ChannelList[j].MaxTxPwr = pChDesp->MaxTxPwr;
+		pAd->ChannelList[j].DfsReq = pChDesp->DfsReq;
+		j++;
+	}
+	pAd->ChannelListNum = j;
+
+	return j;
+}
+
+static inline VOID CreateChList(
+	IN PRTMP_ADAPTER pAd,
+	IN PCH_REGION pChRegion,
+	IN UCHAR Geography)
+{
+	INT i;
+	UCHAR offset = 0;
+	PCH_DESP pChDesp;
+	UCHAR ChType;
+	UCHAR increment;
+
+	if (pChRegion == NULL)
+		return;
+
+	ChBandCheck(pAd->CommonCfg.PhyMode, &ChType);
+
+	for (i=0; i<10; i++)
+	{
+		pChDesp = &pChRegion->ChDesp[i];
+		if (pChDesp->FirstChannel == 0)
+			break;
+
+		if (ChType == BAND_5G)
+		{
+			if (pChDesp->FirstChannel <= 14)
+				continue;
+		}
+		else if (ChType == BAND_24G)
+		{
+			if (pChDesp->FirstChannel > 14)
+				continue;
+		}
+
+		if ((pChDesp->Geography == BOTH)
+			|| (pChDesp->Geography == Geography))
+        {
+			if (pChDesp->FirstChannel > 14)
+                increment = 4;
+            else
+                increment = 1;
+			offset = FillChList(pAd, pChDesp, offset, increment);
+        }
+	}
+}
+
+static inline VOID BuildChannelListEx(
+	IN PRTMP_ADAPTER pAd)
+{
+	PCH_REGION pChReg;
+
+	pChReg = GetChRegion(pAd->CommonCfg.CountryCode);
+	CreateChList(pAd, pChReg, pAd->CommonCfg.Geography);
+}
+
+static inline VOID BuildBeaconChList(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pBuf,
+	OUT	PULONG pBufLen)
+{
+	INT i;
+	ULONG TmpLen;
+	PCH_REGION pChRegion;
+	PCH_DESP pChDesp;
+	UCHAR ChType;
+
+	pChRegion = GetChRegion(pAd->CommonCfg.CountryCode);
+
+	if (pChRegion == NULL)
+		return;
+
+	ChBandCheck(pAd->CommonCfg.PhyMode, &ChType);
+	*pBufLen = 0;
+
+	for (i=0; i<10; i++)
+	{
+		pChDesp = &pChRegion->ChDesp[i];
+		if (pChDesp->FirstChannel == 0)
+			break;
+
+		if (ChType == BAND_5G)
+		{
+			if (pChDesp->FirstChannel <= 14)
+				continue;
+		}
+		else if (ChType == BAND_24G)
+		{
+			if (pChDesp->FirstChannel > 14)
+				continue;
+		}
+
+		if ((pChDesp->Geography == BOTH)
+			|| (pChDesp->Geography == pAd->CommonCfg.Geography))
+		{
+			MakeOutgoingFrame(pBuf + *pBufLen,		&TmpLen,
+								1,                 	&pChDesp->FirstChannel,
+								1,                 	&pChDesp->NumOfCh,
+								1,                 	&pChDesp->MaxTxPwr,
+								END_OF_ARGS);
+			*pBufLen += TmpLen;
+		}
+	}
+}
+
+
+#ifdef DOT11_N_SUPPORT
+static inline BOOLEAN IsValidChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR channel)
+
+{
+	INT i;
+
+	for (i = 0; i < pAd->ChannelListNum; i++)
+	{
+		if (pAd->ChannelList[i].Channel == channel)
+			break;
+	}
+
+	if (i == pAd->ChannelListNum)
+		return FALSE;
+	else
+		return TRUE;
+}
+
+
+static inline UCHAR GetExtCh(
+	IN UCHAR Channel,
+	IN UCHAR Direction)
+{
+	CHAR ExtCh;
+
+	if (Direction == EXTCHA_ABOVE)
+		ExtCh = Channel + 4;
+	else
+		ExtCh = (Channel - 4) > 0 ? (Channel - 4) : 0;
+
+	return ExtCh;
+}
+
+
+static inline VOID N_ChannelCheck(
+	IN PRTMP_ADAPTER pAd)
+{
+	//UCHAR ChannelNum = pAd->ChannelListNum;
+	UCHAR Channel = pAd->CommonCfg.Channel;
+
+	if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED) && (pAd->CommonCfg.RegTransmitSetting.field.BW  == BW_40))
+	{
+		if (Channel > 14)
+		{
+			if ((Channel == 36) || (Channel == 44) || (Channel == 52) || (Channel == 60) || (Channel == 100) || (Channel == 108) ||
+			    (Channel == 116) || (Channel == 124) || (Channel == 132) || (Channel == 149) || (Channel == 157))
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+			}
+			else if ((Channel == 40) || (Channel == 48) || (Channel == 56) || (Channel == 64) || (Channel == 104) || (Channel == 112) ||
+					(Channel == 120) || (Channel == 128) || (Channel == 136) || (Channel == 153) || (Channel == 161))
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+			}
+			else
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+			}
+		}
+		else
+		{
+			do
+			{
+				UCHAR ExtCh;
+				UCHAR Dir = pAd->CommonCfg.RegTransmitSetting.field.EXTCHA;
+				ExtCh = GetExtCh(Channel, Dir);
+				if (IsValidChannel(pAd, ExtCh))
+					break;
+
+				Dir = (Dir == EXTCHA_ABOVE) ? EXTCHA_BELOW : EXTCHA_ABOVE;
+				ExtCh = GetExtCh(Channel, Dir);
+				if (IsValidChannel(pAd, ExtCh))
+				{
+					pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = Dir;
+					break;
+				}
+				pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+			} while(FALSE);
+
+			if (Channel == 14)
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+				//pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_NONE;	// We didn't set the ExtCh as NONE due to it'll set in RTMPSetHT()
+			}
+#if 0
+			switch (pAd->CommonCfg.CountryRegion  & 0x7f)
+			{
+				case REGION_0_BG_BAND:	// 1 -11
+				case REGION_1_BG_BAND:	// 1 - 13
+				case REGION_5_BG_BAND:	// 1 - 14
+					if (Channel <= 4)
+					{
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+					}
+					else if (Channel >= 8)
+					{
+						if ((ChannelNum - Channel) < 4)
+							pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+					}
+					break;
+
+				case REGION_2_BG_BAND:	// 10 - 11
+				case REGION_3_BG_BAND:	// 10 - 13
+				case REGION_4_BG_BAND:	// 14
+					pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+					break;
+
+				case REGION_6_BG_BAND:	// 3 - 9
+					if (Channel <= 5)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+					else if (Channel == 6)
+						pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+					else if (Channel >= 7)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+					break;
+
+				case REGION_7_BG_BAND:  // 5 - 13
+					if (Channel <= 8)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+					else if (Channel >= 10)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+					break;
+
+				default:	// Error. should never happen
+					break;
+			}
+#endif
+		}
+	}
+
+
+}
+
+
+static inline VOID N_SetCenCh(
+	IN PRTMP_ADAPTER pAd)
+{
+	if (pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40)
+	{
+		if (pAd->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_ABOVE)
+		{
+			pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel + 2;
+		}
+		else
+		{
+			if (pAd->CommonCfg.Channel == 14)
+				pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel - 1;
+			else
+				pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel - 2;
+		}
+	}
+	else
+	{
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+
+static inline UINT8 GetCuntryMaxTxPwr(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT8 channel)
+{
+	int i;
+	for (i = 0; i < pAd->ChannelListNum; i++)
+	{
+		if (pAd->ChannelList[i].Channel == channel)
+			break;
+	}
+
+	if (i == pAd->ChannelListNum)
+		return 0xff;
+	else
+		return pAd->ChannelList[i].MaxTxPwr;
+}
+#endif // __CHLIST_H__
+
diff --git a/drivers/staging/rt2870/common/2870_rtmp_init.c b/drivers/staging/rt2870/common/2870_rtmp_init.c
new file mode 100644
index 000000000000..9f5143b3922e
--- /dev/null
+++ b/drivers/staging/rt2870/common/2870_rtmp_init.c
@@ -0,0 +1,1778 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	2870_rtmp_init.c
+
+	Abstract:
+	Miniport generic portion header file
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Paul Lin    2002-08-01    created
+    John Chang  2004-08-20    RT2561/2661 use scatter-gather scheme
+    Jan Lee  	2006-09-15    RT2860. Change for 802.11n , EEPROM, Led, BA, HT.
+	Sample Lin	2007-05-31    Merge RT2860 and RT2870 drivers.
+*/
+
+#include "../rt_config.h"
+
+
+static void rx_done_tasklet(unsigned long data);
+static void rt2870_hcca_dma_done_tasklet(unsigned long data);
+static void rt2870_ac3_dma_done_tasklet(unsigned long data);
+static void rt2870_ac2_dma_done_tasklet(unsigned long data);
+static void rt2870_ac1_dma_done_tasklet(unsigned long data);
+static void rt2870_ac0_dma_done_tasklet(unsigned long data);
+static void rt2870_mgmt_dma_done_tasklet(unsigned long data);
+static void rt2870_null_frame_complete_tasklet(unsigned long data);
+static void rt2870_rts_frame_complete_tasklet(unsigned long data);
+static void rt2870_pspoll_frame_complete_tasklet(unsigned long data);
+static void rt2870_dataout_complete_tasklet(unsigned long data);
+
+
+/*
+========================================================================
+Routine Description:
+    Initialize receive data structures.
+
+Arguments:
+    pAd					Pointer to our adapter
+
+Return Value:
+	NDIS_STATUS_SUCCESS
+	NDIS_STATUS_RESOURCES
+
+Note:
+	Initialize all receive releated private buffer, include those define
+	in RTMP_ADAPTER structure and all private data structures. The mahor
+	work is to allocate buffer for each packet and chain buffer to
+	NDIS packet descriptor.
+========================================================================
+*/
+NDIS_STATUS	NICInitRecv(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR				i;
+	NDIS_STATUS			Status = NDIS_STATUS_SUCCESS;
+	POS_COOKIE			pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitRecv\n"));
+	pObj = pObj;
+
+	//InterlockedExchange(&pAd->PendingRx, 0);
+	pAd->PendingRx = 0;
+	pAd->NextRxBulkInReadIndex 	= 0;	// Next Rx Read index
+	pAd->NextRxBulkInIndex		= 0 ; //RX_RING_SIZE -1; // Rx Bulk pointer
+	pAd->NextRxBulkInPosition 	= 0;
+
+	for (i = 0; i < (RX_RING_SIZE); i++)
+	{
+		PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+
+		//Allocate URB
+		pRxContext->pUrb = RTUSB_ALLOC_URB(0);
+		if (pRxContext->pUrb == NULL)
+		{
+			Status = NDIS_STATUS_RESOURCES;
+			goto out1;
+		}
+
+		// Allocate transfer buffer
+		pRxContext->TransferBuffer = RTUSB_URB_ALLOC_BUFFER(pObj->pUsb_Dev, MAX_RXBULK_SIZE, &pRxContext->data_dma);
+		if (pRxContext->TransferBuffer == NULL)
+		{
+			Status = NDIS_STATUS_RESOURCES;
+			goto out1;
+		}
+
+		NdisZeroMemory(pRxContext->TransferBuffer, MAX_RXBULK_SIZE);
+
+		pRxContext->pAd	= pAd;
+		pRxContext->pIrp = NULL;
+		pRxContext->InUse		= FALSE;
+		pRxContext->IRPPending	= FALSE;
+		pRxContext->Readable	= FALSE;
+		//pRxContext->ReorderInUse = FALSE;
+		pRxContext->bRxHandling = FALSE;
+		pRxContext->BulkInOffset = 0;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitRecv\n"));
+	return Status;
+
+out1:
+	for (i = 0; i < (RX_RING_SIZE); i++)
+	{
+		PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+
+		if (NULL != pRxContext->TransferBuffer)
+		{
+			RTUSB_URB_FREE_BUFFER(pObj->pUsb_Dev, MAX_RXBULK_SIZE,
+								pRxContext->TransferBuffer, pRxContext->data_dma);
+			pRxContext->TransferBuffer = NULL;
+		}
+
+		if (NULL != pRxContext->pUrb)
+		{
+			RTUSB_UNLINK_URB(pRxContext->pUrb);
+			RTUSB_FREE_URB(pRxContext->pUrb);
+			pRxContext->pUrb = NULL;
+		}
+	}
+
+	return Status;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Initialize transmit data structures.
+
+Arguments:
+    pAd					Pointer to our adapter
+
+Return Value:
+	NDIS_STATUS_SUCCESS
+	NDIS_STATUS_RESOURCES
+
+Note:
+========================================================================
+*/
+NDIS_STATUS	NICInitTransmit(
+	IN	PRTMP_ADAPTER	pAd)
+{
+#define LM_USB_ALLOC(pObj, Context, TB_Type, BufferSize, Status, msg1, err1, msg2, err2)	\
+	Context->pUrb = RTUSB_ALLOC_URB(0);		\
+	if (Context->pUrb == NULL) {			\
+		DBGPRINT(RT_DEBUG_ERROR, msg1);		\
+		Status = NDIS_STATUS_RESOURCES;		\
+		goto err1; }						\
+											\
+	Context->TransferBuffer = 				\
+		(TB_Type)RTUSB_URB_ALLOC_BUFFER(pObj->pUsb_Dev, BufferSize, &Context->data_dma);	\
+	if (Context->TransferBuffer == NULL) {	\
+		DBGPRINT(RT_DEBUG_ERROR, msg2);		\
+		Status = NDIS_STATUS_RESOURCES;		\
+		goto err2; }
+
+#define LM_URB_FREE(pObj, Context, BufferSize)				\
+	if (NULL != Context->pUrb) {							\
+		RTUSB_UNLINK_URB(Context->pUrb);					\
+		RTUSB_FREE_URB(Context->pUrb);						\
+		Context->pUrb = NULL; }								\
+	if (NULL != Context->TransferBuffer) {				\
+		RTUSB_URB_FREE_BUFFER(pObj->pUsb_Dev, BufferSize,	\
+								Context->TransferBuffer,	\
+								Context->data_dma);			\
+		Context->TransferBuffer = NULL; }
+
+	UCHAR			i, acidx;
+	NDIS_STATUS     Status = NDIS_STATUS_SUCCESS;
+	PTX_CONTEXT		pNullContext   = &(pAd->NullContext);
+	PTX_CONTEXT		pPsPollContext = &(pAd->PsPollContext);
+	PTX_CONTEXT		pRTSContext    = &(pAd->RTSContext);
+	PTX_CONTEXT		pMLMEContext = NULL;
+//	PHT_TX_CONTEXT	pHTTXContext = NULL;
+	POS_COOKIE		pObj = (POS_COOKIE) pAd->OS_Cookie;
+	PVOID			RingBaseVa;
+//	RTMP_TX_RING	*pTxRing;
+	RTMP_MGMT_RING  *pMgmtRing;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitTransmit\n"));
+	pObj = pObj;
+
+	// Init 4 set of Tx parameters
+	for(acidx = 0; acidx < NUM_OF_TX_RING; acidx++)
+	{
+		// Initialize all Transmit releated queues
+		InitializeQueueHeader(&pAd->TxSwQueue[acidx]);
+
+		// Next Local tx ring pointer waiting for buck out
+		pAd->NextBulkOutIndex[acidx] = acidx;
+		pAd->BulkOutPending[acidx] = FALSE; // Buck Out control flag
+		//pAd->DataBulkDoneIdx[acidx] = 0;
+	}
+
+	//pAd->NextMLMEIndex	= 0;
+	//pAd->PushMgmtIndex	= 0;
+	//pAd->PopMgmtIndex	= 0;
+	//InterlockedExchange(&pAd->MgmtQueueSize, 0);
+	//InterlockedExchange(&pAd->TxCount, 0);
+
+	//pAd->PrioRingFirstIndex	= 0;
+	//pAd->PrioRingTxCnt		= 0;
+
+	do
+	{
+		//
+		// TX_RING_SIZE, 4 ACs
+		//
+		for(acidx=0; acidx<4; acidx++)
+		{
+			PHT_TX_CONTEXT	pHTTXContext = &(pAd->TxContext[acidx]);
+
+			NdisZeroMemory(pHTTXContext, sizeof(HT_TX_CONTEXT));
+			//Allocate URB
+			LM_USB_ALLOC(pObj, pHTTXContext, PHTTX_BUFFER, sizeof(HTTX_BUFFER), Status,
+							("<-- ERROR in Alloc TX TxContext[%d] urb!! \n", acidx),
+							done,
+							("<-- ERROR in Alloc TX TxContext[%d] HTTX_BUFFER !! \n", acidx),
+							out1);
+
+			NdisZeroMemory(pHTTXContext->TransferBuffer->Aggregation, 4);
+			pHTTXContext->pAd = pAd;
+			pHTTXContext->pIrp = NULL;
+			pHTTXContext->IRPPending = FALSE;
+			pHTTXContext->NextBulkOutPosition = 0;
+			pHTTXContext->ENextBulkOutPosition = 0;
+			pHTTXContext->CurWritePosition = 0;
+			pHTTXContext->CurWriteRealPos = 0;
+			pHTTXContext->BulkOutSize = 0;
+			pHTTXContext->BulkOutPipeId = acidx;
+			pHTTXContext->bRingEmpty = TRUE;
+			pHTTXContext->bCopySavePad = FALSE;
+
+			pAd->BulkOutPending[acidx] = FALSE;
+		}
+
+
+		//
+		// MGMT_RING_SIZE
+		//
+#if 0
+		for(i=0; i<MGMT_RING_SIZE; i++) // 8
+		{
+			PTX_CONTEXT	pMLMEContext = &(pAd->MLMEContext[i]);
+
+
+			NdisZeroMemory(pMLMEContext, sizeof(TX_CONTEXT));
+
+			//Allocate URB
+			LM_USB_ALLOC(pObj, pMLMEContext, PTX_BUFFER, sizeof(TX_BUFFER), Status,
+							("<-- ERROR in Alloc TX MLMEContext[%d] urb!! \n", i),
+							out2,
+							("<-- ERROR in Alloc TX MLMEContext[%d] TX_BUFFER !! \n", i),
+							out2);
+
+			pMLMEContext->pAd = pAd;
+			pMLMEContext->pIrp = NULL;
+			pMLMEContext->InUse = FALSE;
+			pMLMEContext->IRPPending = FALSE;
+		}
+#else
+		// Allocate MGMT ring descriptor's memory
+		pAd->MgmtDescRing.AllocSize = MGMT_RING_SIZE * sizeof(TX_CONTEXT);
+		RTMPAllocateMemory(&pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocSize);
+		if (pAd->MgmtDescRing.AllocVa == NULL)
+		{
+			DBGPRINT_ERR(("Failed to allocate a big buffer for MgmtDescRing!\n"));
+			Status = NDIS_STATUS_RESOURCES;
+			goto out1;
+		}
+		NdisZeroMemory(pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocSize);
+		RingBaseVa     = pAd->MgmtDescRing.AllocVa;
+
+		// Initialize MGMT Ring and associated buffer memory
+		pMgmtRing = &pAd->MgmtRing;
+		for (i = 0; i < MGMT_RING_SIZE; i++)
+		{
+			// link the pre-allocated Mgmt buffer to MgmtRing.Cell
+			pMgmtRing->Cell[i].AllocSize = sizeof(TX_CONTEXT);
+			pMgmtRing->Cell[i].AllocVa = RingBaseVa;
+			pMgmtRing->Cell[i].pNdisPacket = NULL;
+			pMgmtRing->Cell[i].pNextNdisPacket = NULL;
+
+			//Allocate URB for MLMEContext
+			pMLMEContext = (PTX_CONTEXT) pAd->MgmtRing.Cell[i].AllocVa;
+			pMLMEContext->pUrb = RTUSB_ALLOC_URB(0);
+			if (pMLMEContext->pUrb == NULL)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("<-- ERROR in Alloc TX MLMEContext[%d] urb!! \n", i));
+				Status = NDIS_STATUS_RESOURCES;
+				goto out2;
+			}
+			pMLMEContext->pAd = pAd;
+			pMLMEContext->pIrp = NULL;
+			pMLMEContext->TransferBuffer = NULL;
+			pMLMEContext->InUse = FALSE;
+			pMLMEContext->IRPPending = FALSE;
+			pMLMEContext->bWaitingBulkOut = FALSE;
+			pMLMEContext->BulkOutSize = 0;
+			pMLMEContext->SelfIdx = i;
+
+			// Offset to next ring descriptor address
+			RingBaseVa = (PUCHAR) RingBaseVa + sizeof(TX_CONTEXT);
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("MGMT Ring: total %d entry allocated\n", i));
+
+		//pAd->MgmtRing.TxSwFreeIdx = (MGMT_RING_SIZE - 1);
+		pAd->MgmtRing.TxSwFreeIdx = MGMT_RING_SIZE;
+		pAd->MgmtRing.TxCpuIdx = 0;
+		pAd->MgmtRing.TxDmaIdx = 0;
+#endif
+
+		//
+		// BEACON_RING_SIZE
+		//
+		for(i=0; i<BEACON_RING_SIZE; i++) // 2
+		{
+			PTX_CONTEXT	pBeaconContext = &(pAd->BeaconContext[i]);
+
+
+			NdisZeroMemory(pBeaconContext, sizeof(TX_CONTEXT));
+
+			//Allocate URB
+			LM_USB_ALLOC(pObj, pBeaconContext, PTX_BUFFER, sizeof(TX_BUFFER), Status,
+							("<-- ERROR in Alloc TX BeaconContext[%d] urb!! \n", i),
+							out2,
+							("<-- ERROR in Alloc TX BeaconContext[%d] TX_BUFFER !! \n", i),
+							out3);
+
+			pBeaconContext->pAd = pAd;
+			pBeaconContext->pIrp = NULL;
+			pBeaconContext->InUse = FALSE;
+			pBeaconContext->IRPPending = FALSE;
+		}
+
+		//
+		// NullContext
+		//
+		NdisZeroMemory(pNullContext, sizeof(TX_CONTEXT));
+
+		//Allocate URB
+		LM_USB_ALLOC(pObj, pNullContext, PTX_BUFFER, sizeof(TX_BUFFER), Status,
+						("<-- ERROR in Alloc TX NullContext urb!! \n"),
+						out3,
+						("<-- ERROR in Alloc TX NullContext TX_BUFFER !! \n"),
+						out4);
+
+		pNullContext->pAd = pAd;
+		pNullContext->pIrp = NULL;
+		pNullContext->InUse = FALSE;
+		pNullContext->IRPPending = FALSE;
+
+		//
+		// RTSContext
+		//
+		NdisZeroMemory(pRTSContext, sizeof(TX_CONTEXT));
+
+		//Allocate URB
+		LM_USB_ALLOC(pObj, pRTSContext, PTX_BUFFER, sizeof(TX_BUFFER), Status,
+						("<-- ERROR in Alloc TX RTSContext urb!! \n"),
+						out4,
+						("<-- ERROR in Alloc TX RTSContext TX_BUFFER !! \n"),
+						out5);
+
+		pRTSContext->pAd = pAd;
+		pRTSContext->pIrp = NULL;
+		pRTSContext->InUse = FALSE;
+		pRTSContext->IRPPending = FALSE;
+
+		//
+		// PsPollContext
+		//
+		//NdisZeroMemory(pPsPollContext, sizeof(TX_CONTEXT));
+		//Allocate URB
+		LM_USB_ALLOC(pObj, pPsPollContext, PTX_BUFFER, sizeof(TX_BUFFER), Status,
+						("<-- ERROR in Alloc TX PsPollContext urb!! \n"),
+						out5,
+						("<-- ERROR in Alloc TX PsPollContext TX_BUFFER !! \n"),
+						out6);
+
+		pPsPollContext->pAd = pAd;
+		pPsPollContext->pIrp = NULL;
+		pPsPollContext->InUse = FALSE;
+		pPsPollContext->IRPPending = FALSE;
+		pPsPollContext->bAggregatible = FALSE;
+		pPsPollContext->LastOne = TRUE;
+
+	}   while (FALSE);
+
+
+done:
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitTransmit\n"));
+
+	return Status;
+
+	/* --------------------------- ERROR HANDLE --------------------------- */
+out6:
+	LM_URB_FREE(pObj, pPsPollContext, sizeof(TX_BUFFER));
+
+out5:
+	LM_URB_FREE(pObj, pRTSContext, sizeof(TX_BUFFER));
+
+out4:
+	LM_URB_FREE(pObj, pNullContext, sizeof(TX_BUFFER));
+
+out3:
+	for(i=0; i<BEACON_RING_SIZE; i++)
+	{
+		PTX_CONTEXT	pBeaconContext = &(pAd->BeaconContext[i]);
+		if (pBeaconContext)
+			LM_URB_FREE(pObj, pBeaconContext, sizeof(TX_BUFFER));
+	}
+
+out2:
+	if (pAd->MgmtDescRing.AllocVa)
+	{
+		pMgmtRing = &pAd->MgmtRing;
+	for(i=0; i<MGMT_RING_SIZE; i++)
+	{
+		pMLMEContext = (PTX_CONTEXT) pAd->MgmtRing.Cell[i].AllocVa;
+		if (pMLMEContext)
+			LM_URB_FREE(pObj, pMLMEContext, sizeof(TX_BUFFER));
+	}
+		NdisFreeMemory(pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocSize, 0);
+		pAd->MgmtDescRing.AllocVa = NULL;
+	}
+
+out1:
+	for (acidx = 0; acidx < 4; acidx++)
+	{
+		PHT_TX_CONTEXT pTxContext = &(pAd->TxContext[acidx]);
+		if (pTxContext)
+			LM_URB_FREE(pObj, pTxContext, sizeof(HTTX_BUFFER));
+	}
+
+	// Here we didn't have any pre-allocated memory need to free.
+
+	return Status;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Allocate DMA memory blocks for send, receive.
+
+Arguments:
+    pAd					Pointer to our adapter
+
+Return Value:
+	NDIS_STATUS_SUCCESS
+	NDIS_STATUS_FAILURE
+	NDIS_STATUS_RESOURCES
+
+Note:
+========================================================================
+*/
+NDIS_STATUS	RTMPAllocTxRxRingMemory(
+	IN	PRTMP_ADAPTER	pAd)
+{
+//	COUNTER_802_11	pCounter = &pAd->WlanCounters;
+	NDIS_STATUS		Status;
+	INT				num;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> RTMPAllocTxRxRingMemory\n"));
+
+
+	do
+	{
+		// Init the CmdQ and CmdQLock
+		NdisAllocateSpinLock(&pAd->CmdQLock);
+		NdisAcquireSpinLock(&pAd->CmdQLock);
+		RTUSBInitializeCmdQ(&pAd->CmdQ);
+		NdisReleaseSpinLock(&pAd->CmdQLock);
+
+
+		NdisAllocateSpinLock(&pAd->MLMEBulkOutLock);
+		//NdisAllocateSpinLock(&pAd->MLMEWaitQueueLock);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[0]);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[1]);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[2]);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[3]);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[4]);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[5]);
+		NdisAllocateSpinLock(&pAd->BulkInLock);
+
+		for (num = 0; num < NUM_OF_TX_RING; num++)
+		{
+			NdisAllocateSpinLock(&pAd->TxContextQueueLock[num]);
+		}
+
+#ifdef RALINK_ATE
+		NdisAllocateSpinLock(&pAd->GenericLock);
+#endif // RALINK_ATE //
+
+//		NdisAllocateSpinLock(&pAd->MemLock);	// Not used in RT28XX
+
+//		NdisAllocateSpinLock(&pAd->MacTabLock); // init it in UserCfgInit()
+//		NdisAllocateSpinLock(&pAd->BATabLock); // init it in BATableInit()
+
+//		for(num=0; num<MAX_LEN_OF_BA_REC_TABLE; num++)
+//		{
+//			NdisAllocateSpinLock(&pAd->BATable.BARecEntry[num].RxReRingLock);
+//		}
+
+		//
+		// Init Mac Table
+		//
+//		MacTableInitialize(pAd);
+
+		//
+		// Init send data structures and related parameters
+		//
+		Status = NICInitTransmit(pAd);
+		if (Status != NDIS_STATUS_SUCCESS)
+			break;
+
+		//
+		// Init receive data structures and related parameters
+		//
+		Status = NICInitRecv(pAd);
+		if (Status != NDIS_STATUS_SUCCESS)
+			break;
+
+		pAd->PendingIoCount = 1;
+
+	} while (FALSE);
+
+	NdisZeroMemory(&pAd->FragFrame, sizeof(FRAGMENT_FRAME));
+	pAd->FragFrame.pFragPacket =  RTMP_AllocateFragPacketBuffer(pAd, RX_BUFFER_NORMSIZE);
+
+	if (pAd->FragFrame.pFragPacket == NULL)
+	{
+		Status = NDIS_STATUS_RESOURCES;
+	}
+
+	DBGPRINT_S(Status, ("<-- RTMPAllocTxRxRingMemory, Status=%x\n", Status));
+	return Status;
+}
+
+
+/*
+========================================================================
+Routine Description:
+	Calls USB_InterfaceStop and frees memory allocated for the URBs
+    calls NdisMDeregisterDevice and frees the memory
+    allocated in VNetInitialize for the Adapter Object
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+	None
+
+Note:
+========================================================================
+*/
+VOID	RTMPFreeTxRxRingMemory(
+	IN	PRTMP_ADAPTER	pAd)
+{
+#define LM_URB_FREE(pObj, Context, BufferSize)				\
+	if (NULL != Context->pUrb) {							\
+		RTUSB_UNLINK_URB(Context->pUrb);					\
+		RTUSB_FREE_URB(Context->pUrb);						\
+		Context->pUrb = NULL; }								\
+	if (NULL != Context->TransferBuffer) {					\
+		RTUSB_URB_FREE_BUFFER(pObj->pUsb_Dev, BufferSize,	\
+								Context->TransferBuffer,	\
+								Context->data_dma);			\
+		Context->TransferBuffer = NULL; }
+
+
+	UINT                i, acidx;
+	PTX_CONTEXT			pNullContext   = &pAd->NullContext;
+	PTX_CONTEXT			pPsPollContext = &pAd->PsPollContext;
+	PTX_CONTEXT			pRTSContext    = &pAd->RTSContext;
+//	PHT_TX_CONTEXT 		pHTTXContext;
+	//PRTMP_REORDERBUF	pReorderBuf;
+	POS_COOKIE			pObj = (POS_COOKIE) pAd->OS_Cookie;
+//	RTMP_TX_RING		*pTxRing;
+
+	DBGPRINT(RT_DEBUG_ERROR, ("---> RTMPFreeTxRxRingMemory\n"));
+	pObj = pObj;
+
+	// Free all resources for the RECEIVE buffer queue.
+	for(i=0; i<(RX_RING_SIZE); i++)
+	{
+		PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+		if (pRxContext)
+			LM_URB_FREE(pObj, pRxContext, MAX_RXBULK_SIZE);
+	}
+
+	// Free PsPoll frame resource
+	LM_URB_FREE(pObj, pPsPollContext, sizeof(TX_BUFFER));
+
+	// Free NULL frame resource
+	LM_URB_FREE(pObj, pNullContext, sizeof(TX_BUFFER));
+
+	// Free RTS frame resource
+	LM_URB_FREE(pObj, pRTSContext, sizeof(TX_BUFFER));
+
+
+	// Free beacon frame resource
+	for(i=0; i<BEACON_RING_SIZE; i++)
+	{
+		PTX_CONTEXT	pBeaconContext = &(pAd->BeaconContext[i]);
+		if (pBeaconContext)
+			LM_URB_FREE(pObj, pBeaconContext, sizeof(TX_BUFFER));
+	}
+
+
+	// Free mgmt frame resource
+	for(i = 0; i < MGMT_RING_SIZE; i++)
+	{
+		PTX_CONTEXT pMLMEContext = (PTX_CONTEXT)pAd->MgmtRing.Cell[i].AllocVa;
+		//LM_URB_FREE(pObj, pMLMEContext, sizeof(TX_BUFFER));
+		if (NULL != pAd->MgmtRing.Cell[i].pNdisPacket)
+		{
+			RTMPFreeNdisPacket(pAd, pAd->MgmtRing.Cell[i].pNdisPacket);
+			pAd->MgmtRing.Cell[i].pNdisPacket = NULL;
+			pMLMEContext->TransferBuffer = NULL;
+		}
+
+		if (pMLMEContext)
+		{
+			if (NULL != pMLMEContext->pUrb)
+			{
+				RTUSB_UNLINK_URB(pMLMEContext->pUrb);
+				RTUSB_FREE_URB(pMLMEContext->pUrb);
+				pMLMEContext->pUrb = NULL;
+			}
+		}
+	}
+	if (pAd->MgmtDescRing.AllocVa)
+		NdisFreeMemory(pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocSize, 0);
+
+
+	// Free Tx frame resource
+	for (acidx = 0; acidx < 4; acidx++)
+		{
+		PHT_TX_CONTEXT pHTTXContext = &(pAd->TxContext[acidx]);
+			if (pHTTXContext)
+				LM_URB_FREE(pObj, pHTTXContext, sizeof(HTTX_BUFFER));
+		}
+
+	if (pAd->FragFrame.pFragPacket)
+		RELEASE_NDIS_PACKET(pAd, pAd->FragFrame.pFragPacket, NDIS_STATUS_SUCCESS);
+
+	for(i=0; i<6; i++)
+	{
+		NdisFreeSpinLock(&pAd->BulkOutLock[i]);
+	}
+
+	NdisFreeSpinLock(&pAd->BulkInLock);
+	NdisFreeSpinLock(&pAd->MLMEBulkOutLock);
+
+	NdisFreeSpinLock(&pAd->CmdQLock);
+#ifdef RALINK_ATE
+	NdisFreeSpinLock(&pAd->GenericLock);
+#endif // RALINK_ATE //
+	// Clear all pending bulk-out request flags.
+	RTUSB_CLEAR_BULK_FLAG(pAd, 0xffffffff);
+
+//	NdisFreeSpinLock(&pAd->MacTabLock);
+
+//	for(i=0; i<MAX_LEN_OF_BA_REC_TABLE; i++)
+//	{
+//		NdisFreeSpinLock(&pAd->BATable.BARecEntry[i].RxReRingLock);
+//	}
+
+	DBGPRINT(RT_DEBUG_ERROR, ("<--- ReleaseAdapter\n"));
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Allocate memory for adapter control block.
+
+Arguments:
+    pAd					Pointer to our adapter
+
+Return Value:
+	NDIS_STATUS_SUCCESS
+	NDIS_STATUS_FAILURE
+	NDIS_STATUS_RESOURCES
+
+Note:
+========================================================================
+*/
+NDIS_STATUS AdapterBlockAllocateMemory(
+	IN PVOID	handle,
+	OUT	PVOID	*ppAd)
+{
+	PUSB_DEV	usb_dev;
+	POS_COOKIE	pObj = (POS_COOKIE) handle;
+
+
+	usb_dev = pObj->pUsb_Dev;
+
+	pObj->MLMEThr_pid		= THREAD_PID_INIT_VALUE;
+	pObj->RTUSBCmdThr_pid	= THREAD_PID_INIT_VALUE;
+
+	*ppAd = (PVOID)vmalloc(sizeof(RTMP_ADAPTER));
+
+	if (*ppAd)
+	{
+		NdisZeroMemory(*ppAd, sizeof(RTMP_ADAPTER));
+		((PRTMP_ADAPTER)*ppAd)->OS_Cookie = handle;
+		return (NDIS_STATUS_SUCCESS);
+	}
+	else
+	{
+		return (NDIS_STATUS_FAILURE);
+	}
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Create kernel threads & tasklets.
+
+Arguments:
+    *net_dev			Pointer to wireless net device interface
+
+Return Value:
+	NDIS_STATUS_SUCCESS
+	NDIS_STATUS_FAILURE
+
+Note:
+========================================================================
+*/
+NDIS_STATUS	 CreateThreads(
+	IN	struct net_device *net_dev)
+{
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+	POS_COOKIE pObj = (POS_COOKIE) pAd->OS_Cookie;
+	pid_t pid_number = -1;
+
+	//init_MUTEX(&(pAd->usbdev_semaphore));
+
+	init_MUTEX_LOCKED(&(pAd->mlme_semaphore));
+	init_completion (&pAd->mlmeComplete);
+
+	init_MUTEX_LOCKED(&(pAd->RTUSBCmd_semaphore));
+	init_completion (&pAd->CmdQComplete);
+
+	init_MUTEX_LOCKED(&(pAd->RTUSBTimer_semaphore));
+	init_completion (&pAd->TimerQComplete);
+
+	// Creat MLME Thread
+	pObj->MLMEThr_pid= THREAD_PID_INIT_VALUE;
+	pid_number = kernel_thread(MlmeThread, pAd, CLONE_VM);
+	if (pid_number < 0)
+	{
+		printk (KERN_WARNING "%s: unable to start Mlme thread\n",pAd->net_dev->name);
+		return NDIS_STATUS_FAILURE;
+	}
+	pObj->MLMEThr_pid = GET_PID(pid_number);
+	// Wait for the thread to start
+	wait_for_completion(&(pAd->mlmeComplete));
+
+	// Creat Command Thread
+	pObj->RTUSBCmdThr_pid= THREAD_PID_INIT_VALUE;
+	pid_number = kernel_thread(RTUSBCmdThread, pAd, CLONE_VM);
+	if (pid_number < 0)
+	{
+		printk (KERN_WARNING "%s: unable to start RTUSBCmd thread\n",pAd->net_dev->name);
+		return NDIS_STATUS_FAILURE;
+	}
+	pObj->RTUSBCmdThr_pid = GET_PID(pid_number);
+	wait_for_completion(&(pAd->CmdQComplete));
+
+	pObj->TimerQThr_pid= THREAD_PID_INIT_VALUE;
+	pid_number = kernel_thread(TimerQThread, pAd, CLONE_VM);
+	if (pid_number < 0)
+	{
+		printk (KERN_WARNING "%s: unable to start TimerQThread\n",pAd->net_dev->name);
+		return NDIS_STATUS_FAILURE;
+	}
+	pObj->TimerQThr_pid = GET_PID(pid_number);
+	// Wait for the thread to start
+	wait_for_completion(&(pAd->TimerQComplete));
+
+	// Create receive tasklet
+	tasklet_init(&pObj->rx_done_task, rx_done_tasklet, (ULONG)pAd);
+	tasklet_init(&pObj->mgmt_dma_done_task, rt2870_mgmt_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac0_dma_done_task, rt2870_ac0_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac1_dma_done_task, rt2870_ac1_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac2_dma_done_task, rt2870_ac2_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac3_dma_done_task, rt2870_ac3_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->hcca_dma_done_task, rt2870_hcca_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->tbtt_task, tbtt_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->null_frame_complete_task, rt2870_null_frame_complete_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->rts_frame_complete_task, rt2870_rts_frame_complete_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->pspoll_frame_complete_task, rt2870_pspoll_frame_complete_tasklet, (unsigned long)pAd);
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+========================================================================
+Routine Description:
+ 	As STA's BSSID is a WC too, it uses shared key table.
+ 	This function write correct unicast TX key to ASIC WCID.
+ 	And we still make a copy in our MacTab.Content[BSSID_WCID].PairwiseKey.
+	Caller guarantee TKIP/AES always has keyidx = 0. (pairwise key)
+	Caller guarantee WEP calls this function when set Txkey,  default key index=0~3.
+
+Arguments:
+	pAd 					Pointer to our adapter
+	pKey					Pointer to the where the key stored
+
+Return Value:
+	NDIS_SUCCESS			Add key successfully
+
+Note:
+========================================================================
+*/
+VOID	RTMPAddBSSIDCipher(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	UCHAR				Aid,
+	IN	PNDIS_802_11_KEY	pKey,
+	IN  UCHAR   			CipherAlg)
+{
+	PUCHAR		pTxMic, pRxMic;
+	BOOLEAN 	bKeyRSC, bAuthenticator; // indicate the receive SC set by KeyRSC value
+//	UCHAR		CipherAlg;
+	UCHAR		i;
+	ULONG		WCIDAttri;
+	USHORT	 	offset;
+	UCHAR		KeyIdx, IVEIV[8];
+	UINT32		Value;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddBSSIDCipher==> Aid = %d\n",Aid));
+
+	// Bit 29 of Add-key KeyRSC
+	bKeyRSC 	   = (pKey->KeyIndex & 0x20000000) ? TRUE : FALSE;
+
+	// Bit 28 of Add-key Authenticator
+	bAuthenticator = (pKey->KeyIndex & 0x10000000) ? TRUE : FALSE;
+	KeyIdx = (UCHAR)pKey->KeyIndex&0xff;
+
+	if (KeyIdx > 4)
+		return;
+
+
+	if (pAd->MacTab.Content[Aid].PairwiseKey.CipherAlg == CIPHER_TKIP)
+	{	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		{
+			// for WPA-None Tx, Rx MIC is the same
+			pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+			pRxMic = pTxMic;
+		}
+		else if (bAuthenticator == TRUE)
+		{
+			pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+			pRxMic = (PUCHAR) (&pKey->KeyMaterial) + 24;
+		}
+		else
+		{
+			pRxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+			pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 24;
+		}
+
+		offset = PAIRWISE_KEY_TABLE_BASE + (Aid * HW_KEY_ENTRY_SIZE) + 0x10;
+		for (i=0; i<8; )
+		{
+			Value = *(pTxMic+i);
+			Value += (*(pTxMic+i+1)<<8);
+			Value += (*(pTxMic+i+2)<<16);
+			Value += (*(pTxMic+i+3)<<24);
+			RTUSBWriteMACRegister(pAd, offset+i, Value);
+			i+=4;
+		}
+
+		offset = PAIRWISE_KEY_TABLE_BASE + (Aid * HW_KEY_ENTRY_SIZE) + 0x18;
+		for (i=0; i<8; )
+		{
+			Value = *(pRxMic+i);
+			Value += (*(pRxMic+i+1)<<8);
+			Value += (*(pRxMic+i+2)<<16);
+			Value += (*(pRxMic+i+3)<<24);
+			RTUSBWriteMACRegister(pAd, offset+i, Value);
+			i+=4;
+		}
+
+		// Only Key lenth equal to TKIP key have these
+		NdisMoveMemory(pAd->MacTab.Content[Aid].PairwiseKey.RxMic, pRxMic, 8);
+		NdisMoveMemory(pAd->MacTab.Content[Aid].PairwiseKey.TxMic, pTxMic, 8);
+
+		DBGPRINT(RT_DEBUG_TRACE,
+				("	TxMIC  = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x \n",
+				pTxMic[0],pTxMic[1],pTxMic[2],pTxMic[3],
+				pTxMic[4],pTxMic[5],pTxMic[6],pTxMic[7]));
+		DBGPRINT(RT_DEBUG_TRACE,
+				("	RxMIC  = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x \n",
+				pRxMic[0],pRxMic[1],pRxMic[2],pRxMic[3],
+				pRxMic[4],pRxMic[5],pRxMic[6],pRxMic[7]));
+	}
+
+	// 2. Record Security Key.
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.KeyLen= (UCHAR)pKey->KeyLength;
+	NdisMoveMemory(pAd->MacTab.Content[Aid].PairwiseKey.Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+	// 3. Check RxTsc. And used to init to ASIC IV.
+	if (bKeyRSC == TRUE)
+		NdisMoveMemory(pAd->MacTab.Content[Aid].PairwiseKey.RxTsc, &pKey->KeyRSC, 6);
+	else
+		NdisZeroMemory(pAd->MacTab.Content[Aid].PairwiseKey.RxTsc, 6);
+
+	// 4. Init TxTsc to one based on WiFi WPA specs
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[0] = 1;
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[1] = 0;
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[2] = 0;
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[3] = 0;
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[4] = 0;
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[5] = 0;
+
+	CipherAlg = pAd->MacTab.Content[Aid].PairwiseKey.CipherAlg;
+
+	offset = PAIRWISE_KEY_TABLE_BASE + (Aid * HW_KEY_ENTRY_SIZE);
+	RTUSBMultiWrite(pAd, (USHORT) offset, pKey->KeyMaterial,
+				((pKey->KeyLength == LEN_TKIP_KEY) ? 16 : (USHORT)pKey->KeyLength));
+
+	offset = SHARED_KEY_TABLE_BASE + (KeyIdx * HW_KEY_ENTRY_SIZE);
+	RTUSBMultiWrite(pAd, (USHORT) offset, pKey->KeyMaterial, (USHORT)pKey->KeyLength);
+
+	offset = PAIRWISE_IVEIV_TABLE_BASE + (Aid * HW_IVEIV_ENTRY_SIZE);
+	NdisZeroMemory(IVEIV, 8);
+
+	// IV/EIV
+	if ((CipherAlg == CIPHER_TKIP) ||
+		(CipherAlg == CIPHER_TKIP_NO_MIC) ||
+		(CipherAlg == CIPHER_AES))
+	{
+		IVEIV[3] = 0x20; // Eiv bit on. keyid always 0 for pairwise key
+	}
+	// default key idx needs to set.
+	// in TKIP/AES KeyIdx = 0 , WEP KeyIdx is default tx key.
+	else
+	{
+		IVEIV[3] |= (KeyIdx<< 6);
+	}
+	RTUSBMultiWrite(pAd, (USHORT) offset, IVEIV, 8);
+
+	// WCID Attribute UDF:3, BSSIdx:3, Alg:3, Keytable:1=PAIRWISE KEY, BSSIdx is 0
+	if ((CipherAlg == CIPHER_TKIP) ||
+		(CipherAlg == CIPHER_TKIP_NO_MIC) ||
+		(CipherAlg == CIPHER_AES))
+	{
+		WCIDAttri = (CipherAlg<<1)|SHAREDKEYTABLE;
+	}
+	else
+		WCIDAttri = (CipherAlg<<1)|SHAREDKEYTABLE;
+
+	offset = MAC_WCID_ATTRIBUTE_BASE + (Aid* HW_WCID_ATTRI_SIZE);
+	RTUSBWriteMACRegister(pAd, offset, WCIDAttri);
+	RTUSBReadMACRegister(pAd, offset, &Value);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BSSID_WCID : offset = %x, WCIDAttri = %lx\n",
+			offset, WCIDAttri));
+
+	// pAddr
+	// Add Bssid mac address at linkup. not here.  check!
+	/*offset = MAC_WCID_BASE + (BSSID_WCID * HW_WCID_ENTRY_SIZE);
+	*for (i=0; i<MAC_ADDR_LEN; i++)
+	{
+		RTMP_IO_WRITE8(pAd, offset+i, pKey->BSSID[i]);
+	}
+	*/
+
+	DBGPRINT(RT_DEBUG_ERROR, ("AddBSSIDasWCIDEntry: Alg=%s, KeyLength = %d\n",
+			CipherName[CipherAlg], pKey->KeyLength));
+	DBGPRINT(RT_DEBUG_TRACE, ("Key [idx=%x] [KeyLen = %d]\n",
+			pKey->KeyIndex, pKey->KeyLength));
+	for(i=0; i<pKey->KeyLength; i++)
+		DBGPRINT_RAW(RT_DEBUG_TRACE,(" %x:", pKey->KeyMaterial[i]));
+	DBGPRINT(RT_DEBUG_TRACE,("	 \n"));
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+========================================================================
+Routine Description:
+    Get a received packet.
+
+Arguments:
+	pAd					device control block
+	pSaveRxD			receive descriptor information
+	*pbReschedule		need reschedule flag
+	*pRxPending			pending received packet flag
+
+Return Value:
+    the recieved packet
+
+Note:
+========================================================================
+*/
+#define RT2870_RXDMALEN_FIELD_SIZE			4
+PNDIS_PACKET GetPacketFromRxRing(
+	IN		PRTMP_ADAPTER		pAd,
+	OUT		PRT28XX_RXD_STRUC	pSaveRxD,
+	OUT		BOOLEAN				*pbReschedule,
+	IN OUT	UINT32				*pRxPending)
+{
+	PRX_CONTEXT		pRxContext;
+	PNDIS_PACKET	pSkb;
+	PUCHAR			pData;
+	ULONG			ThisFrameLen;
+	ULONG			RxBufferLength;
+	PRXWI_STRUC		pRxWI;
+
+	pRxContext = &pAd->RxContext[pAd->NextRxBulkInReadIndex];
+	if ((pRxContext->Readable == FALSE) || (pRxContext->InUse == TRUE))
+		return NULL;
+
+	RxBufferLength = pRxContext->BulkInOffset - pAd->ReadPosition;
+	if (RxBufferLength < (RT2870_RXDMALEN_FIELD_SIZE + sizeof(RXWI_STRUC) + sizeof(RXINFO_STRUC)))
+	{
+		goto label_null;
+	}
+
+	pData = &pRxContext->TransferBuffer[pAd->ReadPosition]; /* 4KB */
+	// The RXDMA field is 4 bytes, now just use the first 2 bytes. The Length including the (RXWI + MSDU + Padding)
+	ThisFrameLen = *pData + (*(pData+1)<<8);
+    if (ThisFrameLen == 0)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("BIRIdx(%d): RXDMALen is zero.[%ld], BulkInBufLen = %ld)\n",
+								pAd->NextRxBulkInReadIndex, ThisFrameLen, pRxContext->BulkInOffset));
+		goto label_null;
+	}
+	if ((ThisFrameLen&0x3) != 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("BIRIdx(%d): RXDMALen not multiple of 4.[%ld], BulkInBufLen = %ld)\n",
+								pAd->NextRxBulkInReadIndex, ThisFrameLen, pRxContext->BulkInOffset));
+		goto label_null;
+	}
+
+	if ((ThisFrameLen + 8)> RxBufferLength)	// 8 for (RT2870_RXDMALEN_FIELD_SIZE + sizeof(RXINFO_STRUC))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("BIRIdx(%d):FrameLen(0x%lx) outranges. BulkInLen=0x%lx, remaining RxBufLen=0x%lx, ReadPos=0x%lx\n",
+						pAd->NextRxBulkInReadIndex, ThisFrameLen, pRxContext->BulkInOffset, RxBufferLength, pAd->ReadPosition));
+
+		// error frame. finish this loop
+		goto label_null;
+	}
+
+	// skip USB frame length field
+	pData += RT2870_RXDMALEN_FIELD_SIZE;
+	pRxWI = (PRXWI_STRUC)pData;
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange(pData, TYPE_RXWI);
+#endif // RT_BIG_ENDIAN //
+	if (pRxWI->MPDUtotalByteCount > ThisFrameLen)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s():pRxWIMPDUtotalByteCount(%d) large than RxDMALen(%ld)\n",
+									__func__, pRxWI->MPDUtotalByteCount, ThisFrameLen));
+		goto label_null;
+	}
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange(pData, TYPE_RXWI);
+#endif // RT_BIG_ENDIAN //
+
+	// allocate a rx packet
+	pSkb = dev_alloc_skb(ThisFrameLen);
+	if (pSkb == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("%s():Cannot Allocate sk buffer for this Bulk-In buffer!\n", __func__));
+		goto label_null;
+	}
+
+	// copy the rx packet
+	memcpy(skb_put(pSkb, ThisFrameLen), pData, ThisFrameLen);
+	RTPKT_TO_OSPKT(pSkb)->dev = get_netdev_from_bssid(pAd, BSS0);
+	RTMP_SET_PACKET_SOURCE(OSPKT_TO_RTPKT(pSkb), PKTSRC_NDIS);
+
+	// copy RxD
+	*pSaveRxD = *(PRXINFO_STRUC)(pData + ThisFrameLen);
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pSaveRxD, TYPE_RXINFO);
+#endif // RT_BIG_ENDIAN //
+
+	// update next packet read position.
+	pAd->ReadPosition += (ThisFrameLen + RT2870_RXDMALEN_FIELD_SIZE + RXINFO_SIZE);	// 8 for (RT2870_RXDMALEN_FIELD_SIZE + sizeof(RXINFO_STRUC))
+
+	return pSkb;
+
+label_null:
+
+	return NULL;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Handle received packets.
+
+Arguments:
+	data				- URB information pointer
+
+Return Value:
+    None
+
+Note:
+========================================================================
+*/
+static void rx_done_tasklet(unsigned long data)
+{
+	purbb_t 			pUrb;
+	PRX_CONTEXT			pRxContext;
+	PRTMP_ADAPTER		pAd;
+	NTSTATUS			Status;
+	unsigned int		IrqFlags;
+
+	pUrb		= (purbb_t)data;
+	pRxContext	= (PRX_CONTEXT)pUrb->context;
+	pAd 		= pRxContext->pAd;
+	Status = pUrb->status;
+
+
+	RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+	pRxContext->InUse = FALSE;
+	pRxContext->IRPPending = FALSE;
+	pRxContext->BulkInOffset += pUrb->actual_length;
+	//NdisInterlockedDecrement(&pAd->PendingRx);
+	pAd->PendingRx--;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		pAd->BulkInComplete++;
+		pAd->NextRxBulkInPosition = 0;
+		if (pRxContext->BulkInOffset)	// As jan's comment, it may bulk-in success but size is zero.
+		{
+			pRxContext->Readable = TRUE;
+			INC_RING_INDEX(pAd->NextRxBulkInIndex, RX_RING_SIZE);
+		}
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+	}
+	else	 // STATUS_OTHER
+	{
+		pAd->BulkInCompleteFail++;
+		// Still read this packet although it may comtain wrong bytes.
+		pRxContext->Readable = FALSE;
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+		// Parsing all packets. because after reset, the index will reset to all zero.
+		if ((!RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+									fRTMP_ADAPTER_BULKIN_RESET |
+									fRTMP_ADAPTER_HALT_IN_PROGRESS |
+									fRTMP_ADAPTER_NIC_NOT_EXIST))))
+		{
+
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk In Failed. Status=%d, BIIdx=0x%x, BIRIdx=0x%x, actual_length= 0x%x\n",
+							Status, pAd->NextRxBulkInIndex, pAd->NextRxBulkInReadIndex, pRxContext->pUrb->actual_length));
+
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_IN, NULL, 0);
+		}
+	}
+
+	ASSERT((pRxContext->InUse == pRxContext->IRPPending));
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+    {
+		// If the driver is in ATE mode and Rx frame is set into here.
+		if (pAd->ContinBulkIn == TRUE)
+		{
+			RTUSBBulkReceive(pAd);
+		}
+	}
+	else
+#endif // RALINK_ATE //
+	RTUSBBulkReceive(pAd);
+
+	return;
+
+}
+
+
+static void rt2870_mgmt_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER 	pAd;
+	PTX_CONTEXT		pMLMEContext;
+	int				index;
+	PNDIS_PACKET	pPacket;
+	purbb_t			pUrb;
+	NTSTATUS		Status;
+	unsigned long	IrqFlags;
+
+
+	pUrb			= (purbb_t)data;
+	pMLMEContext	= (PTX_CONTEXT)pUrb->context;
+	pAd 			= pMLMEContext->pAd;
+	Status			= pUrb->status;
+	index 			= pMLMEContext->SelfIdx;
+
+	ASSERT((pAd->MgmtRing.TxDmaIdx == index));
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+
+	if (Status != USB_ST_NOERROR)
+	{
+		//Bulk-Out fail status handle
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out MLME Failed, Status=%d!\n", Status));
+			// TODO: How to handle about the MLMEBulkOut failed issue. Need to resend the mgmt pkt?
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+		}
+	}
+
+	pAd->BulkOutPending[MGMTPIPEIDX] = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+	RTMP_IRQ_LOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+	// Reset MLME context flags
+	pMLMEContext->IRPPending = FALSE;
+	pMLMEContext->InUse = FALSE;
+	pMLMEContext->bWaitingBulkOut = FALSE;
+	pMLMEContext->BulkOutSize = 0;
+
+	pPacket = pAd->MgmtRing.Cell[index].pNdisPacket;
+	pAd->MgmtRing.Cell[index].pNdisPacket = NULL;
+
+	// Increase MgmtRing Index
+	INC_RING_INDEX(pAd->MgmtRing.TxDmaIdx, MGMT_RING_SIZE);
+	pAd->MgmtRing.TxSwFreeIdx++;
+	RTMP_IRQ_UNLOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+
+	// No-matter success or fail, we free the mgmt packet.
+	if (pPacket)
+		RTMPFreeNdisPacket(pAd, pPacket);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET) &&
+			((pAd->bulkResetPipeid & BULKOUT_MGMT_RESET_FLAG) == BULKOUT_MGMT_RESET_FLAG))
+		{	// For Mgmt Bulk-Out failed, ignore it now.
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{
+
+			// Always call Bulk routine, even reset bulk.
+			// The protectioon of rest bulk should be in BulkOut routine
+			if (pAd->MgmtRing.TxSwFreeIdx < MGMT_RING_SIZE /* pMLMEContext->bWaitingBulkOut == TRUE */)
+			{
+				RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME);
+			}
+				RTUSBKickBulkOut(pAd);
+			}
+		}
+
+}
+
+
+static void rt2870_hcca_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId = 4;
+	purbb_t				pUrb;
+
+
+	DBGPRINT_RAW(RT_DEBUG_ERROR, ("--->hcca_dma_done_tasklet\n"));
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+
+	rt2870_dataout_complete_tasklet((unsigned long)pUrb);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET))
+		{
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+			if ((pAd->TxSwQueue[BulkOutPipeId].Number > 0) &&
+				/*((pHTTXContext->CurWritePosition > (pHTTXContext->NextBulkOutPosition + 0x6000)) || (pHTTXContext->NextBulkOutPosition > pHTTXContext->CurWritePosition + 0x6000)) && */
+				(pAd->DeQueueRunning[BulkOutPipeId] == FALSE) &&
+				(pHTTXContext->bCurWriting == FALSE))
+			{
+				RTMPDeQueuePacket(pAd, FALSE, BulkOutPipeId, MAX_TX_PROCESS);
+			}
+
+			RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL);
+			RTUSBKickBulkOut(pAd);
+		}
+	}
+
+	DBGPRINT_RAW(RT_DEBUG_ERROR, ("<---hcca_dma_done_tasklet\n"));
+
+		return;
+}
+
+
+static void rt2870_ac3_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId = 3;
+	purbb_t				pUrb;
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+
+	rt2870_dataout_complete_tasklet((unsigned long)pUrb);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET))
+		{
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+			if ((pAd->TxSwQueue[BulkOutPipeId].Number > 0) &&
+				/*((pHTTXContext->CurWritePosition > (pHTTXContext->NextBulkOutPosition + 0x6000)) || (pHTTXContext->NextBulkOutPosition > pHTTXContext->CurWritePosition + 0x6000)) && */
+				(pAd->DeQueueRunning[BulkOutPipeId] == FALSE) &&
+				(pHTTXContext->bCurWriting == FALSE))
+			{
+				RTMPDeQueuePacket(pAd, FALSE, BulkOutPipeId, MAX_TX_PROCESS);
+			}
+
+			RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL<<3);
+			RTUSBKickBulkOut(pAd);
+		}
+	}
+
+
+		return;
+}
+
+
+static void rt2870_ac2_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId = 2;
+	purbb_t				pUrb;
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+
+	rt2870_dataout_complete_tasklet((unsigned long)pUrb);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET))
+		{
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+			if ((pAd->TxSwQueue[BulkOutPipeId].Number > 0) &&
+				/*((pHTTXContext->CurWritePosition > (pHTTXContext->NextBulkOutPosition + 0x6000)) || (pHTTXContext->NextBulkOutPosition > pHTTXContext->CurWritePosition + 0x6000)) && */
+				(pAd->DeQueueRunning[BulkOutPipeId] == FALSE) &&
+				(pHTTXContext->bCurWriting == FALSE))
+			{
+				RTMPDeQueuePacket(pAd, FALSE, BulkOutPipeId, MAX_TX_PROCESS);
+			}
+
+			RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL<<2);
+			RTUSBKickBulkOut(pAd);
+		}
+	}
+
+		return;
+}
+
+
+static void rt2870_ac1_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId = 1;
+	purbb_t				pUrb;
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+
+	rt2870_dataout_complete_tasklet((unsigned long)pUrb);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET))
+		{
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+			if ((pAd->TxSwQueue[BulkOutPipeId].Number > 0) &&
+				/*((pHTTXContext->CurWritePosition > (pHTTXContext->NextBulkOutPosition + 0x6000)) || (pHTTXContext->NextBulkOutPosition > pHTTXContext->CurWritePosition + 0x6000)) && */
+				(pAd->DeQueueRunning[BulkOutPipeId] == FALSE) &&
+				(pHTTXContext->bCurWriting == FALSE))
+			{
+				RTMPDeQueuePacket(pAd, FALSE, BulkOutPipeId, MAX_TX_PROCESS);
+			}
+
+			RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL<<1);
+			RTUSBKickBulkOut(pAd);
+		}
+	}
+
+
+	return;
+}
+
+
+static void rt2870_ac0_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId = 0;
+	purbb_t				pUrb;
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+
+	rt2870_dataout_complete_tasklet((unsigned long)pUrb);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET))
+		{
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+			if ((pAd->TxSwQueue[BulkOutPipeId].Number > 0) &&
+				/*  ((pHTTXContext->CurWritePosition > (pHTTXContext->NextBulkOutPosition + 0x6000)) || (pHTTXContext->NextBulkOutPosition > pHTTXContext->CurWritePosition + 0x6000)) && */
+				(pAd->DeQueueRunning[BulkOutPipeId] == FALSE) &&
+				(pHTTXContext->bCurWriting == FALSE))
+			{
+				RTMPDeQueuePacket(pAd, FALSE, BulkOutPipeId, MAX_TX_PROCESS);
+			}
+
+			RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL);
+			RTUSBKickBulkOut(pAd);
+		}
+	}
+
+
+	return;
+
+}
+
+
+static void rt2870_null_frame_complete_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER	pAd;
+	PTX_CONTEXT		pNullContext;
+	purbb_t			pUrb;
+	NTSTATUS		Status;
+	unsigned long	irqFlag;
+
+
+	pUrb			= (purbb_t)data;
+	pNullContext	= (PTX_CONTEXT)pUrb->context;
+	pAd 			= pNullContext->pAd;
+	Status 			= pUrb->status;
+
+	// Reset Null frame context flags
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], irqFlag);
+	pNullContext->IRPPending 	= FALSE;
+	pNullContext->InUse 		= FALSE;
+	pAd->BulkOutPending[0] = FALSE;
+	pAd->watchDogTxPendingCnt[0] = 0;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else	// STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out Null Frame Failed, ReasonCode=%d!\n", Status));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+			RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+		}
+	}
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+
+}
+
+
+static void rt2870_rts_frame_complete_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER	pAd;
+	PTX_CONTEXT		pRTSContext;
+	purbb_t			pUrb;
+	NTSTATUS		Status;
+	unsigned long	irqFlag;
+
+
+	pUrb		= (purbb_t)data;
+	pRTSContext	= (PTX_CONTEXT)pUrb->context;
+	pAd			= pRTSContext->pAd;
+	Status		= pUrb->status;
+
+	// Reset RTS frame context flags
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], irqFlag);
+	pRTSContext->IRPPending = FALSE;
+	pRTSContext->InUse		= FALSE;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else	// STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out RTS Frame Failed\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+			RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+		}
+	}
+
+	RTMP_SEM_LOCK(&pAd->BulkOutLock[pRTSContext->BulkOutPipeId]);
+	pAd->BulkOutPending[pRTSContext->BulkOutPipeId] = FALSE;
+	RTMP_SEM_UNLOCK(&pAd->BulkOutLock[pRTSContext->BulkOutPipeId]);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+
+}
+
+
+static void rt2870_pspoll_frame_complete_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER	pAd;
+	PTX_CONTEXT		pPsPollContext;
+	purbb_t			pUrb;
+	NTSTATUS		Status;
+
+
+	pUrb			= (purbb_t)data;
+	pPsPollContext	= (PTX_CONTEXT)pUrb->context;
+	pAd				= pPsPollContext->pAd;
+	Status			= pUrb->status;
+
+	// Reset PsPoll context flags
+	pPsPollContext->IRPPending	= FALSE;
+	pPsPollContext->InUse		= FALSE;
+	pAd->watchDogTxPendingCnt[0] = 0;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else // STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out PSPoll Failed\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+	}
+
+	RTMP_SEM_LOCK(&pAd->BulkOutLock[0]);
+	pAd->BulkOutPending[0] = FALSE;
+	RTMP_SEM_UNLOCK(&pAd->BulkOutLock[0]);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+
+}
+
+
+static void rt2870_dataout_complete_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	purbb_t				pUrb;
+	POS_COOKIE			pObj;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId;
+	NTSTATUS			Status;
+	unsigned long		IrqFlags;
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+	pObj 			= (POS_COOKIE) pAd->OS_Cookie;
+	Status			= pUrb->status;
+
+	// Store BulkOut PipeId
+	BulkOutPipeId = pHTTXContext->BulkOutPipeId;
+	pAd->BulkOutDataOneSecCount++;
+
+	//DBGPRINT(RT_DEBUG_LOUD, ("Done-B(%d):I=0x%lx, CWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d!\n", BulkOutPipeId, in_interrupt(), pHTTXContext->CurWritePosition,
+	//		pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad));
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+	pHTTXContext->IRPPending = FALSE;
+	pAd->watchDogTxPendingCnt[BulkOutPipeId] = 0;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		pAd->BulkOutComplete++;
+
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+		pAd->Counters8023.GoodTransmits++;
+		//RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+		FREE_HTTX_RING(pAd, BulkOutPipeId, pHTTXContext);
+		//RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+
+	}
+	else	// STATUS_OTHER
+	{
+		PUCHAR	pBuf;
+
+		pAd->BulkOutCompleteOther++;
+
+		pBuf = &pHTTXContext->TransferBuffer->field.WirelessPacket[pHTTXContext->NextBulkOutPosition];
+
+		if (!RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+									fRTMP_ADAPTER_HALT_IN_PROGRESS |
+									fRTMP_ADAPTER_NIC_NOT_EXIST |
+									fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = BulkOutPipeId;
+			pAd->bulkResetReq[BulkOutPipeId] = pAd->BulkOutReq;
+		}
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("BulkOutDataPacket failed: ReasonCode=%d!\n", Status));
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("\t>>BulkOut Req=0x%lx, Complete=0x%lx, Other=0x%lx\n", pAd->BulkOutReq, pAd->BulkOutComplete, pAd->BulkOutCompleteOther));
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("\t>>BulkOut Header:%x %x %x %x %x %x %x %x\n", pBuf[0], pBuf[1], pBuf[2], pBuf[3], pBuf[4], pBuf[5], pBuf[6], pBuf[7]));
+		//DBGPRINT_RAW(RT_DEBUG_ERROR, (">>BulkOutCompleteCancel=0x%x, BulkOutCompleteOther=0x%x\n", pAd->BulkOutCompleteCancel, pAd->BulkOutCompleteOther));
+
+	}
+
+	//
+	// bInUse = TRUE, means some process are filling TX data, after that must turn on bWaitingBulkOut
+	// bWaitingBulkOut = TRUE, means the TX data are waiting for bulk out.
+	//
+	//RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+	if ((pHTTXContext->ENextBulkOutPosition != pHTTXContext->CurWritePosition) &&
+		(pHTTXContext->ENextBulkOutPosition != (pHTTXContext->CurWritePosition+8)) &&
+		!RTUSB_TEST_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_FRAG << BulkOutPipeId)))
+	{
+		// Indicate There is data avaliable
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+	}
+	//RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protection of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+}
+
+/* End of 2870_rtmp_init.c */
diff --git a/drivers/staging/rt2870/common/action.c b/drivers/staging/rt2870/common/action.c
new file mode 100644
index 000000000000..3a48a7f5e2bc
--- /dev/null
+++ b/drivers/staging/rt2870/common/action.c
@@ -0,0 +1,1046 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+	action.c
+
+    Abstract:
+    Handle association related requests either from WSTA or from local MLME
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+	Jan Lee		2006	  	created for rt2860
+ */
+
+#include "../rt_config.h"
+#include "action.h"
+
+
+static VOID ReservedAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+/*
+    ==========================================================================
+    Description:
+        association state machine init, including state transition and timer init
+    Parameters:
+        S - pointer to the association state machine
+    Note:
+        The state machine looks like the following
+
+                                    ASSOC_IDLE
+        MT2_MLME_DISASSOC_REQ    mlme_disassoc_req_action
+        MT2_PEER_DISASSOC_REQ    peer_disassoc_action
+        MT2_PEER_ASSOC_REQ       drop
+        MT2_PEER_REASSOC_REQ     drop
+        MT2_CLS3ERR              cls3err_action
+    ==========================================================================
+ */
+VOID ActionStateMachineInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  STATE_MACHINE *S,
+    OUT STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(S, (STATE_MACHINE_FUNC *)Trans, MAX_ACT_STATE, MAX_ACT_MSG, (STATE_MACHINE_FUNC)Drop, ACT_IDLE, ACT_MACHINE_BASE);
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_SPECTRUM_CATE, (STATE_MACHINE_FUNC)PeerSpectrumAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_QOS_CATE, (STATE_MACHINE_FUNC)PeerQOSAction);
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_DLS_CATE, (STATE_MACHINE_FUNC)ReservedAction);
+#ifdef QOS_DLS_SUPPORT
+		StateMachineSetAction(S, ACT_IDLE, MT2_PEER_DLS_CATE, (STATE_MACHINE_FUNC)PeerDLSAction);
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_BA_CATE, (STATE_MACHINE_FUNC)PeerBAAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_HT_CATE, (STATE_MACHINE_FUNC)PeerHTAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_ADD_BA_CATE, (STATE_MACHINE_FUNC)MlmeADDBAAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_ORI_DELBA_CATE, (STATE_MACHINE_FUNC)MlmeDELBAAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_REC_DELBA_CATE, (STATE_MACHINE_FUNC)MlmeDELBAAction);
+#endif // DOT11_N_SUPPORT //
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_PUBLIC_CATE, (STATE_MACHINE_FUNC)PeerPublicAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_RM_CATE, (STATE_MACHINE_FUNC)PeerRMAction);
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_QOS_CATE, (STATE_MACHINE_FUNC)MlmeQOSAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_DLS_CATE, (STATE_MACHINE_FUNC)MlmeDLSAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_ACT_INVALID, (STATE_MACHINE_FUNC)MlmeInvalidAction);
+}
+
+#ifdef DOT11_N_SUPPORT
+VOID MlmeADDBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+
+{
+	MLME_ADDBA_REQ_STRUCT *pInfo;
+	UCHAR           Addr[6];
+	PUCHAR         pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG		Idx;
+	FRAME_ADDBA_REQ  Frame;
+	ULONG		FrameLen;
+	BA_ORI_ENTRY			*pBAEntry = NULL;
+
+	pInfo = (MLME_ADDBA_REQ_STRUCT *)Elem->Msg;
+	NdisZeroMemory(&Frame, sizeof(FRAME_ADDBA_REQ));
+
+	if(MlmeAddBAReqSanity(pAd, Elem->Msg, Elem->MsgLen, Addr))
+	{
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - MlmeADDBAAction() allocate memory failed \n"));
+			return;
+		}
+		// 1. find entry
+		Idx = pAd->MacTab.Content[pInfo->Wcid].BAOriWcidArray[pInfo->TID];
+		if (Idx == 0)
+		{
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeADDBAAction() can't find BAOriEntry \n"));
+			return;
+		}
+		else
+		{
+			pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (ADHOC_ON(pAd))
+				ActHeaderInit(pAd, &Frame.Hdr, pInfo->pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#ifdef QOS_DLS_SUPPORT
+			if (pAd->MacTab.Content[pInfo->Wcid].ValidAsDls)
+				ActHeaderInit(pAd, &Frame.Hdr, pInfo->pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#endif // QOS_DLS_SUPPORT //
+				ActHeaderInit(pAd, &Frame.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pInfo->pAddr);
+
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		Frame.Category = CATEGORY_BA;
+		Frame.Action = ADDBA_REQ;
+		Frame.BaParm.AMSDUSupported = 0;
+		Frame.BaParm.BAPolicy = IMMED_BA;
+		Frame.BaParm.TID = pInfo->TID;
+		Frame.BaParm.BufSize = pInfo->BaBufSize;
+		Frame.Token = pInfo->Token;
+		Frame.TimeOutValue = pInfo->TimeOutValue;
+		Frame.BaStartSeq.field.FragNum = 0;
+		Frame.BaStartSeq.field.StartSeq = pAd->MacTab.Content[pInfo->Wcid].TxSeq[pInfo->TID];
+
+		*(USHORT *)(&Frame.BaParm) = cpu2le16(*(USHORT *)(&Frame.BaParm));
+		Frame.TimeOutValue = cpu2le16(Frame.TimeOutValue);
+		Frame.BaStartSeq.word = cpu2le16(Frame.BaStartSeq.word);
+
+		MakeOutgoingFrame(pOutBuffer,		   &FrameLen,
+		              sizeof(FRAME_ADDBA_REQ), &Frame,
+		              END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("BA - Send ADDBA request. StartSeq = %x,  FrameLen = %ld. BufSize = %d\n", Frame.BaStartSeq.field.StartSeq, FrameLen, Frame.BaParm.BufSize));
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+        send DELBA and delete BaEntry if any
+    Parametrs:
+        Elem - MLME message MLME_DELBA_REQ_STRUCT
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDELBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_DELBA_REQ_STRUCT *pInfo;
+	PUCHAR         pOutBuffer = NULL;
+	PUCHAR		   pOutBuffer2 = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG		Idx;
+	FRAME_DELBA_REQ  Frame;
+	ULONG		FrameLen;
+	FRAME_BAR	FrameBar;
+
+	pInfo = (MLME_DELBA_REQ_STRUCT *)Elem->Msg;
+	// must send back DELBA
+	NdisZeroMemory(&Frame, sizeof(FRAME_DELBA_REQ));
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeDELBAAction(), Initiator(%d) \n", pInfo->Initiator));
+
+	if(MlmeDelBAReqSanity(pAd, Elem->Msg, Elem->MsgLen))
+	{
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeDELBAAction() allocate memory failed 1. \n"));
+			return;
+		}
+
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer2);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_ERROR, ("BA - MlmeDELBAAction() allocate memory failed 2. \n"));
+			return;
+		}
+
+		// SEND BAR (Send BAR to refresh peer reordering buffer.)
+		Idx = pAd->MacTab.Content[pInfo->Wcid].BAOriWcidArray[pInfo->TID];
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			BarHeaderInit(pAd, &FrameBar, pAd->MacTab.Content[pInfo->Wcid].Addr, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+
+		FrameBar.StartingSeq.field.FragNum = 0; // make sure sequence not clear in DEL funciton.
+		FrameBar.StartingSeq.field.StartSeq = pAd->MacTab.Content[pInfo->Wcid].TxSeq[pInfo->TID]; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.TID = pInfo->TID; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.ACKPolicy = IMMED_BA; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.Compressed = 1; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.MTID = 0; // make sure sequence not clear in DEL funciton.
+
+		MakeOutgoingFrame(pOutBuffer2,				&FrameLen,
+					  sizeof(FRAME_BAR),	  &FrameBar,
+					  END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer2, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer2);
+		DBGPRINT(RT_DEBUG_TRACE,("BA - MlmeDELBAAction() . Send BAR to refresh peer reordering buffer \n"));
+
+		// SEND DELBA FRAME
+		FrameLen = 0;
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (ADHOC_ON(pAd))
+				ActHeaderInit(pAd, &Frame.Hdr, pAd->MacTab.Content[pInfo->Wcid].Addr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#ifdef QOS_DLS_SUPPORT
+			if (pAd->MacTab.Content[pInfo->Wcid].ValidAsDls)
+				ActHeaderInit(pAd, &Frame.Hdr, pAd->MacTab.Content[pInfo->Wcid].Addr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#endif // QOS_DLS_SUPPORT //
+				ActHeaderInit(pAd, &Frame.Hdr,  pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->MacTab.Content[pInfo->Wcid].Addr);
+		}
+#endif // CONFIG_STA_SUPPORT //
+		Frame.Category = CATEGORY_BA;
+		Frame.Action = DELBA;
+		Frame.DelbaParm.Initiator = pInfo->Initiator;
+		Frame.DelbaParm.TID = pInfo->TID;
+		Frame.ReasonCode = 39; // Time Out
+		*(USHORT *)(&Frame.DelbaParm) = cpu2le16(*(USHORT *)(&Frame.DelbaParm));
+		Frame.ReasonCode = cpu2le16(Frame.ReasonCode);
+
+		MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+		              sizeof(FRAME_DELBA_REQ),    &Frame,
+		              END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+		DBGPRINT(RT_DEBUG_TRACE, ("BA - MlmeDELBAAction() . 3 DELBA sent. Initiator(%d)\n", pInfo->Initiator));
+    	}
+}
+#endif // DOT11_N_SUPPORT //
+
+VOID MlmeQOSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+VOID MlmeDLSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+VOID MlmeInvalidAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	//PUCHAR		   pOutBuffer = NULL;
+	//Return the receiving frame except the MSB of category filed set to 1.  7.3.1.11
+}
+
+VOID PeerQOSAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+#ifdef QOS_DLS_SUPPORT
+VOID PeerDLSAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	switch(Action)
+	{
+		case ACTION_DLS_REQUEST:
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			PeerDlsReqAction(pAd, Elem);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+
+		case ACTION_DLS_RESPONSE:
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			PeerDlsRspAction(pAd, Elem);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+
+		case ACTION_DLS_TEARDOWN:
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			PeerDlsTearDownAction(pAd, Elem);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+	}
+}
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+VOID PeerBAAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	switch(Action)
+	{
+		case ADDBA_REQ:
+			PeerAddBAReqAction(pAd,Elem);
+			break;
+		case ADDBA_RESP:
+			PeerAddBARspAction(pAd,Elem);
+			break;
+		case DELBA:
+			PeerDelBAAction(pAd,Elem);
+			break;
+	}
+}
+
+
+#ifdef DOT11N_DRAFT3
+
+#ifdef CONFIG_STA_SUPPORT
+VOID StaPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Bss2040Coexist)
+{
+	BSS_2040_COEXIST_IE		BssCoexist;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+
+	BssCoexist.word = Bss2040Coexist;
+	// AP asks Station to return a 20/40 BSS Coexistence mgmt frame.  So we first starts a scan, then send back 20/40 BSS Coexistence mgmt frame
+	if ((BssCoexist.field.InfoReq == 1) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SCAN_2040)))
+	{
+		// Clear record first.  After scan , will update those bit and send back to transmiter.
+		pAd->CommonCfg.BSSCoexist2040.field.InfoReq = 1;
+		pAd->CommonCfg.BSSCoexist2040.field.Intolerant40 = 0;
+		pAd->CommonCfg.BSSCoexist2040.field.BSS20WidthReq = 0;
+		// Fill out stuff for scan request
+		ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_2040_BSS_COEXIST);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+	}
+}
+
+
+/*
+Description : Build Intolerant Channel Rerpot from Trigger event table.
+return : how many bytes copied.
+*/
+ULONG BuildIntolerantChannelRep(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    PUCHAR  pDest)
+{
+	ULONG			FrameLen = 0;
+	ULONG			ReadOffset = 0;
+	UCHAR			i;
+	UCHAR			LastRegClass = 0xff;
+	PUCHAR			pLen;
+
+	for ( i = 0;i < MAX_TRIGGER_EVENT;i++)
+	{
+		if (pAd->CommonCfg.TriggerEventTab.EventA[i].bValid == TRUE)
+		{
+			if (pAd->CommonCfg.TriggerEventTab.EventA[i].RegClass == LastRegClass)
+			{
+				*(pDest + ReadOffset) = (UCHAR)pAd->CommonCfg.TriggerEventTab.EventA[i].Channel;
+				*pLen++;
+				ReadOffset++;
+				FrameLen++;
+			}
+			else
+			{
+				*(pDest + ReadOffset) = IE_2040_BSS_INTOLERANT_REPORT;  // IE
+				*(pDest + ReadOffset + 1) = 2;	// Len = RegClass byte + channel byte.
+				pLen = pDest + ReadOffset + 1;
+				LastRegClass = pAd->CommonCfg.TriggerEventTab.EventA[i].RegClass;
+				*(pDest + ReadOffset + 2) = LastRegClass;	// Len = RegClass byte + channel byte.
+				*(pDest + ReadOffset + 3) = (UCHAR)pAd->CommonCfg.TriggerEventTab.EventA[i].Channel;
+				FrameLen += 4;
+				ReadOffset += 4;
+			}
+
+		}
+	}
+	return FrameLen;
+}
+
+
+/*
+Description : Send 20/40 BSS Coexistence Action frame If one trigger event is triggered.
+*/
+VOID Send2040CoexistAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS 	NStatus;
+	FRAME_ACTION_HDR	Frame;
+	ULONG			FrameLen;
+	ULONG			IntolerantChaRepLen;
+
+	IntolerantChaRepLen = 0;
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("ACT - Send2040CoexistAction() allocate memory failed \n"));
+		return;
+	}
+	ActHeaderInit(pAd, &Frame.Hdr, pAd->MacTab.Content[Wcid].Addr, pAd->CommonCfg.Bssid);
+	Frame.Category = CATEGORY_PUBLIC;
+	Frame.Action = ACTION_BSS_2040_COEXIST;
+
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+				  sizeof(FRAME_ACTION_HDR),	  &Frame,
+				  END_OF_ARGS);
+
+	*(pOutBuffer + FrameLen) = pAd->CommonCfg.BSSCoexist2040.word;
+	FrameLen++;
+
+	if (bAddIntolerantCha == TRUE)
+		IntolerantChaRepLen = BuildIntolerantChannelRep(pAd, pOutBuffer + FrameLen);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen + IntolerantChaRepLen);
+	DBGPRINT(RT_DEBUG_ERROR,("ACT - Send2040CoexistAction( BSSCoexist2040 = 0x%x )  \n", pAd->CommonCfg.BSSCoexist2040.word));
+
+}
+
+
+/*
+	==========================================================================
+	Description:
+	After scan, Update 20/40 BSS Coexistence IE and send out.
+	According to 802.11n D3.03 11.14.10
+
+	Parameters:
+	==========================================================================
+ */
+VOID Update2040CoexistFrameAndNotify(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha)
+{
+	BSS_2040_COEXIST_IE	OldValue;
+
+	OldValue.word = pAd->CommonCfg.BSSCoexist2040.word;
+	if ((pAd->CommonCfg.TriggerEventTab.EventANo > 0) || (pAd->CommonCfg.TriggerEventTab.EventBCountDown > 0))
+		pAd->CommonCfg.BSSCoexist2040.field.BSS20WidthReq = 1;
+
+	// Need to check !!!!
+	// How STA will set Intolerant40 if implementation dependent. Now we don't set this bit first.!!!!!
+	// So Only check BSS20WidthReq change.
+	if (OldValue.field.BSS20WidthReq != pAd->CommonCfg.BSSCoexist2040.field.BSS20WidthReq)
+	{
+		Send2040CoexistAction(pAd, Wcid, bAddIntolerantCha);
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+BOOLEAN ChannelSwitchSanityCheck(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  NewChannel,
+	IN    UCHAR  Secondary)
+{
+	UCHAR		i;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	if ((NewChannel > 7) && (Secondary == 1))
+		return FALSE;
+
+	if ((NewChannel < 5) && (Secondary == 3))
+		return FALSE;
+
+	// 0. Check if new channel is in the channellist.
+	for (i = 0;i < pAd->ChannelListNum;i++)
+	{
+		if (pAd->ChannelList[i].Channel == NewChannel)
+		{
+			break;
+		}
+	}
+
+	if (i == pAd->ChannelListNum)
+		return FALSE;
+
+	return TRUE;
+}
+
+
+VOID ChannelSwitchAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  NewChannel,
+	IN    UCHAR  Secondary)
+{
+	UCHAR		BBPValue = 0;
+	ULONG		MACValue;
+
+	DBGPRINT(RT_DEBUG_TRACE,("SPECTRUM - ChannelSwitchAction(NewChannel = %d , Secondary = %d)  \n", NewChannel, Secondary));
+
+	if (ChannelSwitchSanityCheck(pAd, Wcid, NewChannel, Secondary) == FALSE)
+		return;
+
+	// 1.  Switches to BW = 20.
+	if (Secondary == 0)
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+		BBPValue&= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x16);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x08);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x11);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+		pAd->CommonCfg.BBPCurrentBW = BW_20;
+		pAd->CommonCfg.Channel = NewChannel;
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel,FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+		pAd->MacTab.Content[Wcid].HTPhyMode.field.BW = 0;
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!20MHz   !!! \n" ));
+	}
+	// 1.  Switches to BW = 40 And Station supports BW = 40.
+	else if (((Secondary == 1) || (Secondary == 3)) && (pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth == 1))
+	{
+		pAd->CommonCfg.Channel = NewChannel;
+
+		if (Secondary == 1)
+		{
+			// Secondary above.
+			pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel + 2;
+			RTMP_IO_READ32(pAd, TX_BAND_CFG, &MACValue);
+			MACValue &= 0xfe;
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, MACValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue&= (~0x18);
+			BBPValue|= (0x10);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPValue);
+			BBPValue&= (~0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPValue);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Lower LINK UP !!! Control Channel at Below. Central = %d \n", pAd->CommonCfg.CentralChannel ));
+		}
+		else
+		{
+			// Secondary below.
+			pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel - 2;
+			RTMP_IO_READ32(pAd, TX_BAND_CFG, &MACValue);
+			MACValue &= 0xfe;
+			MACValue |= 0x1;
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, MACValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue&= (~0x18);
+			BBPValue|= (0x10);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPValue);
+			BBPValue&= (~0x20);
+			BBPValue|= (0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPValue);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Upper LINK UP !!! Control Channel at UpperCentral = %d \n", pAd->CommonCfg.CentralChannel ));
+		}
+		pAd->CommonCfg.BBPCurrentBW = BW_40;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+		pAd->MacTab.Content[Wcid].HTPhyMode.field.BW = 1;
+	}
+}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+VOID PeerPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+#ifdef DOT11N_DRAFT3
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+#endif // DOT11N_DRAFT3 //
+
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+#ifdef DOT11N_DRAFT3
+	switch(Action)
+	{
+		case ACTION_BSS_2040_COEXIST:	// Format defined in IEEE 7.4.7a.1 in 11n Draf3.03
+			{
+				//UCHAR	BssCoexist;
+				BSS_2040_COEXIST_ELEMENT		*pCoexistInfo;
+				BSS_2040_COEXIST_IE 			*pBssCoexistIe;
+				BSS_2040_INTOLERANT_CH_REPORT	*pIntolerantReport = NULL;
+
+				if (Elem->MsgLen <= (LENGTH_802_11 + sizeof(BSS_2040_COEXIST_ELEMENT)) )
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("ACTION - 20/40 BSS Coexistence Management Frame length too short! len = %ld!\n", Elem->MsgLen));
+					break;
+				}
+				DBGPRINT(RT_DEBUG_TRACE, ("ACTION - 20/40 BSS Coexistence Management action----> \n"));
+				hex_dump("CoexistenceMgmtFrame", Elem->Msg, Elem->MsgLen);
+
+
+				pCoexistInfo = (BSS_2040_COEXIST_ELEMENT *) &Elem->Msg[LENGTH_802_11+2];
+				//hex_dump("CoexistInfo", (PUCHAR)pCoexistInfo, sizeof(BSS_2040_COEXIST_ELEMENT));
+				if (Elem->MsgLen >= (LENGTH_802_11 + sizeof(BSS_2040_COEXIST_ELEMENT) + sizeof(BSS_2040_INTOLERANT_CH_REPORT)))
+				{
+					pIntolerantReport = (BSS_2040_INTOLERANT_CH_REPORT *)((PUCHAR)pCoexistInfo + sizeof(BSS_2040_COEXIST_ELEMENT));
+				}
+				//hex_dump("IntolerantReport ", (PUCHAR)pIntolerantReport, sizeof(BSS_2040_INTOLERANT_CH_REPORT));
+
+				pBssCoexistIe = (BSS_2040_COEXIST_IE *)(&pCoexistInfo->BssCoexistIe);
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					if (INFRA_ON(pAd))
+					{
+						StaPublicAction(pAd, pCoexistInfo);
+					}
+				}
+#endif // CONFIG_STA_SUPPORT //
+
+			}
+			break;
+	}
+
+#endif // DOT11N_DRAFT3 //
+
+}
+
+
+static VOID ReservedAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR Category;
+
+	if (Elem->MsgLen <= LENGTH_802_11)
+	{
+		return;
+	}
+
+	Category = Elem->Msg[LENGTH_802_11];
+	DBGPRINT(RT_DEBUG_TRACE,("Rcv reserved category(%d) Action Frame\n", Category));
+	hex_dump("Reserved Action Frame", &Elem->Msg[0], Elem->MsgLen);
+}
+
+VOID PeerRMAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	return;
+}
+
+#ifdef DOT11_N_SUPPORT
+static VOID respond_ht_information_exchange_action(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen;
+	FRAME_HT_INFO	HTINFOframe, *pFrame;
+	UCHAR   		*pAddr;
+
+
+	// 2. Always send back ADDBA Response
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);	 //Get an unused nonpaged memory
+
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ACTION - respond_ht_information_exchange_action() allocate memory failed \n"));
+		return;
+	}
+
+	// get RA
+	pFrame = (FRAME_HT_INFO *) &Elem->Msg[0];
+	pAddr = pFrame->Hdr.Addr2;
+
+	NdisZeroMemory(&HTINFOframe, sizeof(FRAME_HT_INFO));
+	// 2-1. Prepare ADDBA Response frame.
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (ADHOC_ON(pAd))
+			ActHeaderInit(pAd, &HTINFOframe.Hdr, pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+		else
+			ActHeaderInit(pAd, &HTINFOframe.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAddr);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	HTINFOframe.Category = CATEGORY_HT;
+	HTINFOframe.Action = HT_INFO_EXCHANGE;
+	HTINFOframe.HT_Info.Request = 0;
+	HTINFOframe.HT_Info.Forty_MHz_Intolerant = pAd->CommonCfg.HtCapability.HtCapInfo.Forty_Mhz_Intolerant;
+	HTINFOframe.HT_Info.STA_Channel_Width	 = pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth;
+
+	MakeOutgoingFrame(pOutBuffer,					&FrameLen,
+					  sizeof(FRAME_HT_INFO),	&HTINFOframe,
+					  END_OF_ARGS);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+
+#ifdef DOT11N_DRAFT3
+VOID SendNotifyBWActionFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR  Wcid,
+	IN UCHAR apidx)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS 	NStatus;
+	FRAME_ACTION_HDR	Frame;
+	ULONG			FrameLen;
+	PUCHAR			pAddr1;
+
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("ACT - SendNotifyBWAction() allocate memory failed \n"));
+		return;
+	}
+
+	if (Wcid == MCAST_WCID)
+		pAddr1 = &BROADCAST_ADDR[0];
+	else
+		pAddr1 = pAd->MacTab.Content[Wcid].Addr;
+	ActHeaderInit(pAd, &Frame.Hdr, pAddr1, pAd->ApCfg.MBSSID[apidx].Bssid, pAd->ApCfg.MBSSID[apidx].Bssid);
+
+	Frame.Category = CATEGORY_HT;
+	Frame.Action = NOTIFY_BW_ACTION;
+
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+				  sizeof(FRAME_ACTION_HDR),	  &Frame,
+				  END_OF_ARGS);
+
+	*(pOutBuffer + FrameLen) = pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth;
+	FrameLen++;
+
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	DBGPRINT(RT_DEBUG_TRACE,("ACT - SendNotifyBWAction(NotifyBW= %d)!\n", pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth));
+
+}
+#endif // DOT11N_DRAFT3 //
+
+
+VOID PeerHTAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+	switch(Action)
+	{
+		case NOTIFY_BW_ACTION:
+			DBGPRINT(RT_DEBUG_TRACE,("ACTION - HT Notify Channel bandwidth action----> \n"));
+#ifdef CONFIG_STA_SUPPORT
+			if(pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE)
+			{
+				// Note, this is to patch DIR-1353 AP. When the AP set to Wep, it will use legacy mode. But AP still keeps
+				// sending BW_Notify Action frame, and cause us to linkup and linkdown.
+				// In legacy mode, don't need to parse HT action frame.
+				DBGPRINT(RT_DEBUG_TRACE,("ACTION -Ignore HT Notify Channel BW when link as legacy mode. BW = %d---> \n",
+								Elem->Msg[LENGTH_802_11+2] ));
+				break;
+			}
+#endif // CONFIG_STA_SUPPORT //
+
+			if (Elem->Msg[LENGTH_802_11+2] == 0)	// 7.4.8.2. if value is 1, keep the same as supported channel bandwidth.
+				pAd->MacTab.Content[Elem->Wcid].HTPhyMode.field.BW = 0;
+
+			break;
+
+		case SMPS_ACTION:
+			// 7.3.1.25
+ 			DBGPRINT(RT_DEBUG_TRACE,("ACTION - SMPS action----> \n"));
+			if (((Elem->Msg[LENGTH_802_11+2]&0x1) == 0))
+			{
+				pAd->MacTab.Content[Elem->Wcid].MmpsMode = MMPS_ENABLE;
+			}
+			else if (((Elem->Msg[LENGTH_802_11+2]&0x2) == 0))
+			{
+				pAd->MacTab.Content[Elem->Wcid].MmpsMode = MMPS_STATIC;
+			}
+			else
+			{
+				pAd->MacTab.Content[Elem->Wcid].MmpsMode = MMPS_DYNAMIC;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,("Aid(%d) MIMO PS = %d\n", Elem->Wcid, pAd->MacTab.Content[Elem->Wcid].MmpsMode));
+			// rt2860c : add something for smps change.
+			break;
+
+		case SETPCO_ACTION:
+			break;
+
+		case MIMO_CHA_MEASURE_ACTION:
+			break;
+
+		case HT_INFO_EXCHANGE:
+			{
+				HT_INFORMATION_OCTET	*pHT_info;
+
+				pHT_info = (HT_INFORMATION_OCTET *) &Elem->Msg[LENGTH_802_11+2];
+    				// 7.4.8.10
+    				DBGPRINT(RT_DEBUG_TRACE,("ACTION - HT Information Exchange action----> \n"));
+    				if (pHT_info->Request)
+    				{
+    					respond_ht_information_exchange_action(pAd, Elem);
+    				}
+			}
+    			break;
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Retry sending ADDBA Reqest.
+
+	IRQL = DISPATCH_LEVEL
+
+	Parametrs:
+	p8023Header: if this is already 802.3 format, p8023Header is NULL
+
+	Return	: TRUE if put into rx reordering buffer, shouldn't indicaterxhere.
+				FALSE , then continue indicaterx at this moment.
+	==========================================================================
+ */
+VOID ORIBATimerTimeout(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	MAC_TABLE_ENTRY	*pEntry;
+	INT			i, total;
+//	FRAME_BAR			FrameBar;
+//	ULONG			FrameLen;
+//	NDIS_STATUS 	NStatus;
+//	PUCHAR			pOutBuffer = NULL;
+//	USHORT			Sequence;
+	UCHAR			TID;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	total = pAd->MacTab.Size * NUM_OF_TID;
+
+	for (i = 1; ((i <MAX_LEN_OF_BA_ORI_TABLE) && (total > 0)) ; i++)
+	{
+		if  (pAd->BATable.BAOriEntry[i].ORI_BA_Status == Originator_Done)
+		{
+			pEntry = &pAd->MacTab.Content[pAd->BATable.BAOriEntry[i].Wcid];
+			TID = pAd->BATable.BAOriEntry[i].TID;
+
+			ASSERT(pAd->BATable.BAOriEntry[i].Wcid < MAX_LEN_OF_MAC_TABLE);
+		}
+		total --;
+	}
+}
+
+
+VOID SendRefreshBAR(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry)
+{
+	FRAME_BAR		FrameBar;
+	ULONG			FrameLen;
+	NDIS_STATUS 	NStatus;
+	PUCHAR			pOutBuffer = NULL;
+	USHORT			Sequence;
+	UCHAR			i, TID;
+	USHORT			idx;
+	BA_ORI_ENTRY	*pBAEntry;
+
+	for (i = 0; i <NUM_OF_TID; i++)
+	{
+		idx = pEntry->BAOriWcidArray[i];
+		if (idx == 0)
+		{
+			continue;
+		}
+		pBAEntry = &pAd->BATable.BAOriEntry[idx];
+
+		if  (pBAEntry->ORI_BA_Status == Originator_Done)
+		{
+			TID = pBAEntry->TID;
+
+			ASSERT(pBAEntry->Wcid < MAX_LEN_OF_MAC_TABLE);
+
+			NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+			if(NStatus != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeADDBAAction() allocate memory failed \n"));
+				return;
+			}
+
+			Sequence = pEntry->TxSeq[TID];
+
+
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				BarHeaderInit(pAd, &FrameBar, pEntry->Addr, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+
+			FrameBar.StartingSeq.field.FragNum = 0; // make sure sequence not clear in DEL function.
+			FrameBar.StartingSeq.field.StartSeq = Sequence; // make sure sequence not clear in DEL funciton.
+			FrameBar.BarControl.TID = TID; // make sure sequence not clear in DEL funciton.
+
+			MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+							  sizeof(FRAME_BAR),	  &FrameBar,
+							  END_OF_ARGS);
+			//if (!(CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_RALINK_CHIPSET)))
+			if (1)	// Now we always send BAR.
+			{
+				//MiniportMMRequestUnlock(pAd, 0, pOutBuffer, FrameLen);
+				MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			}
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+VOID ActHeaderInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN OUT PHEADER_802_11 pHdr80211,
+    IN PUCHAR Addr1,
+    IN PUCHAR Addr2,
+    IN PUCHAR Addr3)
+{
+    NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+    pHdr80211->FC.Type = BTYPE_MGMT;
+    pHdr80211->FC.SubType = SUBTYPE_ACTION;
+
+    COPY_MAC_ADDR(pHdr80211->Addr1, Addr1);
+	COPY_MAC_ADDR(pHdr80211->Addr2, Addr2);
+    COPY_MAC_ADDR(pHdr80211->Addr3, Addr3);
+}
+
+VOID BarHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PFRAME_BAR pCntlBar,
+	IN PUCHAR pDA,
+	IN PUCHAR pSA)
+{
+//	USHORT	Duration;
+
+	NdisZeroMemory(pCntlBar, sizeof(FRAME_BAR));
+	pCntlBar->FC.Type = BTYPE_CNTL;
+	pCntlBar->FC.SubType = SUBTYPE_BLOCK_ACK_REQ;
+   	pCntlBar->BarControl.MTID = 0;
+	pCntlBar->BarControl.Compressed = 1;
+	pCntlBar->BarControl.ACKPolicy = 0;
+
+
+	pCntlBar->Duration = 16 + RTMPCalcDuration(pAd, RATE_1, sizeof(FRAME_BA));
+
+	COPY_MAC_ADDR(pCntlBar->Addr1, pDA);
+	COPY_MAC_ADDR(pCntlBar->Addr2, pSA);
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Insert Category and action code into the action frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. category code of the frame.
+		4. action code of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID InsertActField(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 Category,
+	IN UINT8 ActCode)
+{
+	ULONG TempLen;
+
+	MakeOutgoingFrame(	pFrameBuf,		&TempLen,
+						1,				&Category,
+						1,				&ActCode,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
diff --git a/drivers/staging/rt2870/common/action.h b/drivers/staging/rt2870/common/action.h
new file mode 100644
index 000000000000..ce3877dce81b
--- /dev/null
+++ b/drivers/staging/rt2870/common/action.h
@@ -0,0 +1,68 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	aironet.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	Paul Lin	04-06-15		Initial
+*/
+
+#ifndef	__ACTION_H__
+#define	__ACTION_H__
+
+typedef struct PACKED __HT_INFO_OCTET
+{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	Reserved:5;
+	UCHAR 	STA_Channel_Width:1;
+	UCHAR	Forty_MHz_Intolerant:1;
+	UCHAR	Request:1;
+#else
+	UCHAR	Request:1;
+	UCHAR	Forty_MHz_Intolerant:1;
+	UCHAR 	STA_Channel_Width:1;
+	UCHAR	Reserved:5;
+#endif
+} HT_INFORMATION_OCTET;
+
+
+typedef struct PACKED __FRAME_HT_INFO
+{
+	HEADER_802_11   		Hdr;
+	UCHAR					Category;
+	UCHAR					Action;
+	HT_INFORMATION_OCTET	HT_Info;
+}   FRAME_HT_INFO, *PFRAME_HT_INFO;
+
+#endif /* __ACTION_H__ */
+
+
diff --git a/drivers/staging/rt2870/common/ba_action.c b/drivers/staging/rt2870/common/ba_action.c
new file mode 100644
index 000000000000..d9f738177e2b
--- /dev/null
+++ b/drivers/staging/rt2870/common/ba_action.c
@@ -0,0 +1,1798 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+
+#ifdef DOT11_N_SUPPORT
+
+#include "../rt_config.h"
+
+
+
+#define BA_ORI_INIT_SEQ		(pEntry->TxSeq[TID]) //1			// inital sequence number of BA session
+
+#define ORI_SESSION_MAX_RETRY	8
+#define ORI_BA_SESSION_TIMEOUT	(2000)	// ms
+#define REC_BA_SESSION_IDLE_TIMEOUT	(1000)	// ms
+
+#define REORDERING_PACKET_TIMEOUT		((100 * HZ)/1000)	// system ticks -- 100 ms
+#define MAX_REORDERING_PACKET_TIMEOUT	((3000 * HZ)/1000)	// system ticks -- 100 ms
+
+#define RESET_RCV_SEQ		(0xFFFF)
+
+static void ba_mpdu_blk_free(PRTMP_ADAPTER pAd, struct reordering_mpdu *mpdu_blk);
+
+
+BA_ORI_ENTRY *BATableAllocOriEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx);
+
+BA_REC_ENTRY *BATableAllocRecEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx);
+
+VOID BAOriSessionSetupTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID BARecSessionIdleTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+
+BUILD_TIMER_FUNCTION(BAOriSessionSetupTimeout);
+BUILD_TIMER_FUNCTION(BARecSessionIdleTimeout);
+
+#define ANNOUNCE_REORDERING_PACKET(_pAd, _mpdu_blk)	\
+			Announce_Reordering_Packet(_pAd, _mpdu_blk);
+
+VOID BA_MaxWinSizeReasign(
+	IN PRTMP_ADAPTER	pAd,
+	IN MAC_TABLE_ENTRY  *pEntryPeer,
+	OUT UCHAR			*pWinSize)
+{
+	UCHAR MaxSize;
+
+
+	if (pAd->MACVersion >= RALINK_2883_VERSION) // 3*3
+	{
+		if (pAd->MACVersion >= RALINK_3070_VERSION)
+		{
+			if (pEntryPeer->WepStatus != Ndis802_11EncryptionDisabled)
+				MaxSize = 7; // for non-open mode
+			else
+				MaxSize = 13;
+		}
+		else
+			MaxSize = 31;
+	}
+	else if (pAd->MACVersion >= RALINK_2880E_VERSION) // 2880 e
+	{
+		if (pEntryPeer->WepStatus != Ndis802_11EncryptionDisabled)
+			MaxSize = 7; // for non-open mode
+		else
+			MaxSize = 13;
+	}
+	else
+		MaxSize = 7;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ba> Win Size = %d, Max Size = %d\n",
+			*pWinSize, MaxSize));
+
+	if ((*pWinSize) > MaxSize)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ba> reassign max win size from %d to %d\n",
+				*pWinSize, MaxSize));
+
+		*pWinSize = MaxSize;
+	}
+}
+
+void Announce_Reordering_Packet(IN PRTMP_ADAPTER			pAd,
+								IN struct reordering_mpdu	*mpdu)
+{
+	PNDIS_PACKET    pPacket;
+
+	pPacket = mpdu->pPacket;
+
+	if (mpdu->bAMSDU)
+	{
+		ASSERT(0);
+		BA_Reorder_AMSDU_Annnounce(pAd, pPacket);
+	}
+	else
+	{
+		//
+		// pass this 802.3 packet to upper layer or forward this packet to WM directly
+		//
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pPacket, RTMP_GET_PACKET_IF(pPacket));
+#endif // CONFIG_STA_SUPPORT //
+	}
+}
+
+/*
+ * Insert a reordering mpdu into sorted linked list by sequence no.
+ */
+BOOLEAN ba_reordering_mpdu_insertsorted(struct reordering_list *list, struct reordering_mpdu *mpdu)
+{
+
+	struct reordering_mpdu **ppScan = &list->next;
+
+	while (*ppScan != NULL)
+	{
+		if (SEQ_SMALLER((*ppScan)->Sequence, mpdu->Sequence, MAXSEQ))
+		{
+			ppScan = &(*ppScan)->next;
+		}
+		else if ((*ppScan)->Sequence == mpdu->Sequence)
+		{
+			/* give up this duplicated frame */
+			return(FALSE);
+		}
+		else
+		{
+			/* find position */
+			break;
+		}
+	}
+
+	mpdu->next = *ppScan;
+	*ppScan = mpdu;
+	list->qlen++;
+	return TRUE;
+}
+
+
+/*
+ * caller lock critical section if necessary
+ */
+static inline void ba_enqueue(struct reordering_list *list, struct reordering_mpdu *mpdu_blk)
+{
+	list->qlen++;
+	mpdu_blk->next = list->next;
+	list->next = mpdu_blk;
+}
+
+/*
+ * caller lock critical section if necessary
+ */
+static inline struct reordering_mpdu * ba_dequeue(struct reordering_list *list)
+{
+	struct reordering_mpdu *mpdu_blk = NULL;
+
+	ASSERT(list);
+
+		if (list->qlen)
+		{
+			list->qlen--;
+			mpdu_blk = list->next;
+			if (mpdu_blk)
+			{
+				list->next = mpdu_blk->next;
+				mpdu_blk->next = NULL;
+			}
+		}
+	return mpdu_blk;
+}
+
+
+static inline struct reordering_mpdu  *ba_reordering_mpdu_dequeue(struct reordering_list *list)
+{
+	return(ba_dequeue(list));
+}
+
+
+static inline struct reordering_mpdu  *ba_reordering_mpdu_probe(struct reordering_list *list)
+	{
+	ASSERT(list);
+
+		return(list->next);
+	}
+
+
+/*
+ * free all resource for reordering mechanism
+ */
+void ba_reordering_resource_release(PRTMP_ADAPTER pAd)
+{
+	BA_TABLE        *Tab;
+	PBA_REC_ENTRY   pBAEntry;
+	struct reordering_mpdu *mpdu_blk;
+	int i;
+
+	Tab = &pAd->BATable;
+
+	/* I.  release all pending reordering packet */
+	NdisAcquireSpinLock(&pAd->BATabLock);
+	for (i = 0; i < MAX_LEN_OF_BA_REC_TABLE; i++)
+	{
+		pBAEntry = &Tab->BARecEntry[i];
+		if (pBAEntry->REC_BA_Status != Recipient_NONE)
+		{
+			while ((mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list)))
+			{
+				ASSERT(mpdu_blk->pPacket);
+				RELEASE_NDIS_PACKET(pAd, mpdu_blk->pPacket, NDIS_STATUS_FAILURE);
+				ba_mpdu_blk_free(pAd, mpdu_blk);
+			}
+		}
+	}
+	NdisReleaseSpinLock(&pAd->BATabLock);
+
+	ASSERT(pBAEntry->list.qlen == 0);
+	/* II. free memory of reordering mpdu table */
+	NdisAcquireSpinLock(&pAd->mpdu_blk_pool.lock);
+	os_free_mem(pAd, pAd->mpdu_blk_pool.mem);
+	NdisReleaseSpinLock(&pAd->mpdu_blk_pool.lock);
+}
+
+
+
+/*
+ * Allocate all resource for reordering mechanism
+ */
+BOOLEAN ba_reordering_resource_init(PRTMP_ADAPTER pAd, int num)
+{
+	int     i;
+	PUCHAR  mem;
+	struct reordering_mpdu *mpdu_blk;
+	struct reordering_list *freelist;
+
+	/* allocate spinlock */
+	NdisAllocateSpinLock(&pAd->mpdu_blk_pool.lock);
+
+	/* initialize freelist */
+	freelist = &pAd->mpdu_blk_pool.freelist;
+	freelist->next = NULL;
+	freelist->qlen = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Allocate %d memory for BA reordering\n", (UINT32)(num*sizeof(struct reordering_mpdu))));
+
+	/* allocate number of mpdu_blk memory */
+	os_alloc_mem(pAd, (PUCHAR *)&mem, (num*sizeof(struct reordering_mpdu)));
+
+	pAd->mpdu_blk_pool.mem = mem;
+
+	if (mem == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Can't Allocate Memory for BA Reordering\n"));
+		return(FALSE);
+	}
+
+	/* build mpdu_blk free list */
+	for (i=0; i<num; i++)
+	{
+		/* get mpdu_blk */
+		mpdu_blk = (struct reordering_mpdu *) mem;
+		/* initial mpdu_blk */
+		NdisZeroMemory(mpdu_blk, sizeof(struct reordering_mpdu));
+		/* next mpdu_blk */
+		mem += sizeof(struct reordering_mpdu);
+		/* insert mpdu_blk into freelist */
+		ba_enqueue(freelist, mpdu_blk);
+	}
+
+	return(TRUE);
+}
+
+//static int blk_count=0; // sample take off, no use
+
+static struct reordering_mpdu *ba_mpdu_blk_alloc(PRTMP_ADAPTER pAd)
+{
+	struct reordering_mpdu *mpdu_blk;
+
+	NdisAcquireSpinLock(&pAd->mpdu_blk_pool.lock);
+	mpdu_blk = ba_dequeue(&pAd->mpdu_blk_pool.freelist);
+	if (mpdu_blk)
+	{
+//		blk_count++;
+		/* reset mpdu_blk */
+		NdisZeroMemory(mpdu_blk, sizeof(struct reordering_mpdu));
+	}
+	NdisReleaseSpinLock(&pAd->mpdu_blk_pool.lock);
+	return mpdu_blk;
+}
+
+static void ba_mpdu_blk_free(PRTMP_ADAPTER pAd, struct reordering_mpdu *mpdu_blk)
+{
+	ASSERT(mpdu_blk);
+
+	NdisAcquireSpinLock(&pAd->mpdu_blk_pool.lock);
+//	blk_count--;
+	ba_enqueue(&pAd->mpdu_blk_pool.freelist, mpdu_blk);
+	NdisReleaseSpinLock(&pAd->mpdu_blk_pool.lock);
+}
+
+
+static USHORT ba_indicate_reordering_mpdus_in_order(
+												   IN PRTMP_ADAPTER    pAd,
+												   IN PBA_REC_ENTRY    pBAEntry,
+												   IN USHORT           StartSeq)
+{
+	struct reordering_mpdu *mpdu_blk;
+	USHORT  LastIndSeq = RESET_RCV_SEQ;
+
+	NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+
+	while ((mpdu_blk = ba_reordering_mpdu_probe(&pBAEntry->list)))
+		{
+			/* find in-order frame */
+		if (!SEQ_STEPONE(mpdu_blk->Sequence, StartSeq, MAXSEQ))
+			{
+				break;
+			}
+			/* dequeue in-order frame from reodering list */
+			mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list);
+			/* pass this frame up */
+		ANNOUNCE_REORDERING_PACKET(pAd, mpdu_blk);
+		/* move to next sequence */
+			StartSeq = mpdu_blk->Sequence;
+		LastIndSeq = StartSeq;
+		/* free mpdu_blk */
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+	}
+
+	NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+
+	/* update last indicated sequence */
+	return LastIndSeq;
+}
+
+static void ba_indicate_reordering_mpdus_le_seq(
+											   IN PRTMP_ADAPTER    pAd,
+											   IN PBA_REC_ENTRY    pBAEntry,
+											   IN USHORT           Sequence)
+{
+	struct reordering_mpdu *mpdu_blk;
+
+	NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+	while ((mpdu_blk = ba_reordering_mpdu_probe(&pBAEntry->list)))
+		{
+			/* find in-order frame */
+		if ((mpdu_blk->Sequence == Sequence) || SEQ_SMALLER(mpdu_blk->Sequence, Sequence, MAXSEQ))
+		{
+			/* dequeue in-order frame from reodering list */
+			mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list);
+			/* pass this frame up */
+			ANNOUNCE_REORDERING_PACKET(pAd, mpdu_blk);
+			/* free mpdu_blk */
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+		}
+		else
+			{
+				break;
+			}
+	}
+	NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+}
+
+
+static void ba_refresh_reordering_mpdus(
+									   IN PRTMP_ADAPTER    pAd,
+									   PBA_REC_ENTRY       pBAEntry)
+{
+	struct reordering_mpdu *mpdu_blk;
+
+	NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+
+			/* dequeue in-order frame from reodering list */
+	while ((mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list)))
+	{
+			/* pass this frame up */
+		ANNOUNCE_REORDERING_PACKET(pAd, mpdu_blk);
+
+		pBAEntry->LastIndSeq = mpdu_blk->Sequence;
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+
+		/* update last indicated sequence */
+	}
+	ASSERT(pBAEntry->list.qlen == 0);
+	pBAEntry->LastIndSeq = RESET_RCV_SEQ;
+	NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+}
+
+
+//static
+void ba_flush_reordering_timeout_mpdus(
+									IN PRTMP_ADAPTER    pAd,
+									IN PBA_REC_ENTRY    pBAEntry,
+									IN ULONG            Now32)
+
+{
+	USHORT Sequence;
+
+//	if ((RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+REORDERING_PACKET_TIMEOUT)) &&
+//		 (pBAEntry->list.qlen > ((pBAEntry->BAWinSize*7)/8))) //||
+//		(RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(10*REORDERING_PACKET_TIMEOUT))) &&
+//		 (pBAEntry->list.qlen > (pBAEntry->BAWinSize/8)))
+	if (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(MAX_REORDERING_PACKET_TIMEOUT/6)))
+		 &&(pBAEntry->list.qlen > 1)
+		)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("timeout[%d] (%08lx-%08lx = %d > %d): %x, flush all!\n ", pBAEntry->list.qlen, Now32, (pBAEntry->LastIndSeqAtTimer),
+			   (int)((long) Now32 - (long)(pBAEntry->LastIndSeqAtTimer)), MAX_REORDERING_PACKET_TIMEOUT,
+			   pBAEntry->LastIndSeq));
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+		pBAEntry->LastIndSeqAtTimer = Now32;
+	}
+	else
+	if (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(REORDERING_PACKET_TIMEOUT)))
+		&& (pBAEntry->list.qlen > 0)
+	   )
+		{
+    		//
+		// force LastIndSeq to shift to LastIndSeq+1
+    		//
+    		Sequence = (pBAEntry->LastIndSeq+1) & MAXSEQ;
+    		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, Sequence);
+    		pBAEntry->LastIndSeqAtTimer = Now32;
+			pBAEntry->LastIndSeq = Sequence;
+    		//
+    		// indicate in-order mpdus
+    		//
+    		Sequence = ba_indicate_reordering_mpdus_in_order(pAd, pBAEntry, Sequence);
+    		if (Sequence != RESET_RCV_SEQ)
+    		{
+    			pBAEntry->LastIndSeq = Sequence;
+    		}
+
+	}
+#if 0
+	else if (
+			 (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(MAX_REORDERING_PACKET_TIMEOUT))) &&
+			  (pBAEntry->list.qlen > 1))
+			)
+		{
+		DBGPRINT(RT_DEBUG_TRACE,("timeout[%d] (%lx-%lx = %d > %d): %x\n ", pBAEntry->list.qlen, Now32, (pBAEntry->LastIndSeqAtTimer),
+			   (int)((long) Now32 - (long)(pBAEntry->LastIndSeqAtTimer)), MAX_REORDERING_PACKET_TIMEOUT,
+			   pBAEntry->LastIndSeq));
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+			pBAEntry->LastIndSeqAtTimer = Now32;
+				}
+#endif
+}
+
+
+/*
+ * generate ADDBA request to
+ * set up BA agreement
+ */
+VOID BAOriSessionSetUp(
+					  IN PRTMP_ADAPTER    pAd,
+					  IN MAC_TABLE_ENTRY  *pEntry,
+					  IN UCHAR            TID,
+					  IN USHORT           TimeOut,
+					  IN ULONG            DelayTime,
+					  IN BOOLEAN          isForced)
+
+{
+	//MLME_ADDBA_REQ_STRUCT	AddbaReq;
+	BA_ORI_ENTRY            *pBAEntry = NULL;
+	USHORT                  Idx;
+	BOOLEAN                 Cancelled;
+
+	if ((pAd->CommonCfg.BACapability.field.AutoBA != TRUE)  &&  (isForced == FALSE))
+		return;
+
+	// if this entry is limited to use legacy tx mode, it doesn't generate BA.
+	if (RTMPStaFixedTxMode(pAd, pEntry) != FIXED_TXMODE_HT)
+		return;
+
+	if ((pEntry->BADeclineBitmap & (1<<TID)) && (isForced == FALSE))
+	{
+		// try again after 3 secs
+		DelayTime = 3000;
+//		printk("DeCline BA from Peer\n");
+//		return;
+	}
+
+
+	Idx = pEntry->BAOriWcidArray[TID];
+	if (Idx == 0)
+	{
+		// allocate a BA session
+		pBAEntry = BATableAllocOriEntry(pAd, &Idx);
+		if (pBAEntry == NULL)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("ADDBA - MlmeADDBAAction() allocate BA session failed \n"));
+			return;
+		}
+	}
+	else
+	{
+		pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+	}
+
+	if (pBAEntry->ORI_BA_Status >= Originator_WaitRes)
+	{
+		return;
+	}
+
+	pEntry->BAOriWcidArray[TID] = Idx;
+
+	// Initialize BA session
+	pBAEntry->ORI_BA_Status = Originator_WaitRes;
+	pBAEntry->Wcid = pEntry->Aid;
+	pBAEntry->BAWinSize = pAd->CommonCfg.BACapability.field.RxBAWinLimit;
+	pBAEntry->Sequence = BA_ORI_INIT_SEQ;
+	pBAEntry->Token = 1;	// (2008-01-21) Jan Lee recommends it - this token can't be 0
+	pBAEntry->TID = TID;
+	pBAEntry->TimeOutValue = TimeOut;
+	pBAEntry->pAdapter = pAd;
+
+	if (!(pEntry->TXBAbitmap & (1<<TID)))
+	{
+		RTMPInitTimer(pAd, &pBAEntry->ORIBATimer, GET_TIMER_FUNCTION(BAOriSessionSetupTimeout), pBAEntry, FALSE);
+	}
+	else
+		RTMPCancelTimer(&pBAEntry->ORIBATimer, &Cancelled);
+
+	// set timer to send ADDBA request
+	RTMPSetTimer(&pBAEntry->ORIBATimer, DelayTime);
+}
+
+VOID BAOriSessionAdd(
+			IN PRTMP_ADAPTER    pAd,
+					IN MAC_TABLE_ENTRY  *pEntry,
+			IN PFRAME_ADDBA_RSP pFrame)
+{
+	BA_ORI_ENTRY  *pBAEntry = NULL;
+	BOOLEAN       Cancelled;
+	UCHAR         TID;
+	USHORT        Idx;
+	PUCHAR          pOutBuffer2 = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG           FrameLen;
+	FRAME_BAR       FrameBar;
+
+	TID = pFrame->BaParm.TID;
+	Idx = pEntry->BAOriWcidArray[TID];
+	pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+
+	// Start fill in parameters.
+	if ((Idx !=0) && (pBAEntry->TID == TID) && (pBAEntry->ORI_BA_Status == Originator_WaitRes))
+	{
+		pBAEntry->BAWinSize = min(pBAEntry->BAWinSize, ((UCHAR)pFrame->BaParm.BufSize));
+		BA_MaxWinSizeReasign(pAd, pEntry, &pBAEntry->BAWinSize);
+
+		pBAEntry->TimeOutValue = pFrame->TimeOutValue;
+		pBAEntry->ORI_BA_Status = Originator_Done;
+		// reset sequence number
+		pBAEntry->Sequence = BA_ORI_INIT_SEQ;
+		// Set Bitmap flag.
+		pEntry->TXBAbitmap |= (1<<TID);
+				RTMPCancelTimer(&pBAEntry->ORIBATimer, &Cancelled);
+
+		pBAEntry->ORIBATimer.TimerValue = 0;	//pFrame->TimeOutValue;
+
+		DBGPRINT(RT_DEBUG_TRACE,("%s : TXBAbitmap = %x, BAWinSize = %d, TimeOut = %ld\n", __func__, pEntry->TXBAbitmap,
+								 pBAEntry->BAWinSize, pBAEntry->ORIBATimer.TimerValue));
+
+		// SEND BAR ;
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer2);  //Get an unused nonpaged memory
+		if (NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - BAOriSessionAdd() allocate memory failed \n"));
+			return;
+		}
+
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			BarHeaderInit(pAd, &FrameBar, pAd->MacTab.Content[pBAEntry->Wcid].Addr, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+
+		FrameBar.StartingSeq.field.FragNum = 0;	// make sure sequence not clear in DEL function.
+		FrameBar.StartingSeq.field.StartSeq = pBAEntry->Sequence; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.TID = pBAEntry->TID; // make sure sequence not clear in DEL funciton.
+		MakeOutgoingFrame(pOutBuffer2,              &FrameLen,
+						  sizeof(FRAME_BAR),      &FrameBar,
+					  END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer2, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer2);
+
+
+		if (pBAEntry->ORIBATimer.TimerValue)
+			RTMPSetTimer(&pBAEntry->ORIBATimer, pBAEntry->ORIBATimer.TimerValue); // in mSec
+	}
+}
+
+BOOLEAN BARecSessionAdd(
+					   IN PRTMP_ADAPTER    pAd,
+					   IN MAC_TABLE_ENTRY  *pEntry,
+					   IN PFRAME_ADDBA_REQ pFrame)
+{
+	BA_REC_ENTRY            *pBAEntry = NULL;
+	BOOLEAN                 Status = TRUE;
+	BOOLEAN                 Cancelled;
+	USHORT                  Idx;
+	UCHAR                   TID;
+	UCHAR                   BAWinSize;
+	//UINT32                  Value;
+	//UINT                    offset;
+
+
+	ASSERT(pEntry);
+
+	// find TID
+	TID = pFrame->BaParm.TID;
+
+	BAWinSize = min(((UCHAR)pFrame->BaParm.BufSize), (UCHAR)pAd->CommonCfg.BACapability.field.RxBAWinLimit);
+
+	// Intel patch
+	if (BAWinSize == 0)
+	{
+		BAWinSize = 64;
+	}
+
+	Idx = pEntry->BARecWcidArray[TID];
+
+
+	if (Idx == 0)
+	{
+		pBAEntry = BATableAllocRecEntry(pAd, &Idx);
+	}
+	else
+	{
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+		// flush all pending reordering mpdus
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s(%ld): Idx = %d, BAWinSize(req %d) = %d\n", __func__, pAd->BATable.numAsRecipient, Idx,
+							 pFrame->BaParm.BufSize, BAWinSize));
+
+	// Start fill in parameters.
+	if (pBAEntry != NULL)
+	{
+		ASSERT(pBAEntry->list.qlen == 0);
+
+		pBAEntry->REC_BA_Status = Recipient_HandleRes;
+		pBAEntry->BAWinSize = BAWinSize;
+		pBAEntry->Wcid = pEntry->Aid;
+		pBAEntry->TID = TID;
+		pBAEntry->TimeOutValue = pFrame->TimeOutValue;
+		pBAEntry->REC_BA_Status = Recipient_Accept;
+		// initial sequence number
+		pBAEntry->LastIndSeq = RESET_RCV_SEQ; //pFrame->BaStartSeq.field.StartSeq;
+
+		printk("Start Seq = %08x\n",  pFrame->BaStartSeq.field.StartSeq);
+
+		if (pEntry->RXBAbitmap & (1<<TID))
+		{
+			RTMPCancelTimer(&pBAEntry->RECBATimer, &Cancelled);
+		}
+		else
+		{
+			RTMPInitTimer(pAd, &pBAEntry->RECBATimer, GET_TIMER_FUNCTION(BARecSessionIdleTimeout), pBAEntry, TRUE);
+		}
+
+#if 0	// for debugging
+		RTMPSetTimer(&pBAEntry->RECBATimer, REC_BA_SESSION_IDLE_TIMEOUT);
+#endif
+
+		// Set Bitmap flag.
+		pEntry->RXBAbitmap |= (1<<TID);
+		pEntry->BARecWcidArray[TID] = Idx;
+
+		pEntry->BADeclineBitmap &= ~(1<<TID);
+
+		// Set BA session mask in WCID table.
+		RT28XX_ADD_BA_SESSION_TO_ASIC(pAd, pEntry->Aid, TID);
+
+		DBGPRINT(RT_DEBUG_TRACE,("MACEntry[%d]RXBAbitmap = 0x%x. BARecWcidArray=%d\n",
+				pEntry->Aid, pEntry->RXBAbitmap, pEntry->BARecWcidArray[TID]));
+	}
+	else
+	{
+		Status = FALSE;
+		DBGPRINT(RT_DEBUG_TRACE,("Can't Accept ADDBA for %02x:%02x:%02x:%02x:%02x:%02x TID = %d\n",
+				PRINT_MAC(pEntry->Addr), TID));
+	}
+	return(Status);
+}
+
+
+BA_REC_ENTRY *BATableAllocRecEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx)
+{
+	int             i;
+	BA_REC_ENTRY    *pBAEntry = NULL;
+
+
+	NdisAcquireSpinLock(&pAd->BATabLock);
+
+	if (pAd->BATable.numAsRecipient >= MAX_BARECI_SESSION)
+	{
+		printk("BA Recipeint Session (%ld) > %d\n", pAd->BATable.numAsRecipient,
+			MAX_BARECI_SESSION);
+		goto done;
+	}
+
+	// reserve idx 0 to identify BAWcidArray[TID] as empty
+	for (i=1; i < MAX_LEN_OF_BA_REC_TABLE; i++)
+	{
+		pBAEntry =&pAd->BATable.BARecEntry[i];
+		if ((pBAEntry->REC_BA_Status == Recipient_NONE))
+		{
+			// get one
+			pAd->BATable.numAsRecipient++;
+			pBAEntry->REC_BA_Status = Recipient_USED;
+			*Idx = i;
+			break;
+		}
+	}
+
+done:
+	NdisReleaseSpinLock(&pAd->BATabLock);
+	return pBAEntry;
+}
+
+BA_ORI_ENTRY *BATableAllocOriEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx)
+{
+	int             i;
+	BA_ORI_ENTRY    *pBAEntry = NULL;
+
+	NdisAcquireSpinLock(&pAd->BATabLock);
+
+	if (pAd->BATable.numAsOriginator >= (MAX_LEN_OF_BA_ORI_TABLE))
+	{
+		goto done;
+	}
+
+	// reserve idx 0 to identify BAWcidArray[TID] as empty
+	for (i=1; i<MAX_LEN_OF_BA_ORI_TABLE; i++)
+	{
+		pBAEntry =&pAd->BATable.BAOriEntry[i];
+		if ((pBAEntry->ORI_BA_Status == Originator_NONE))
+		{
+			// get one
+			pAd->BATable.numAsOriginator++;
+			pBAEntry->ORI_BA_Status = Originator_USED;
+			pBAEntry->pAdapter = pAd;
+			*Idx = i;
+			break;
+		}
+	}
+
+done:
+	NdisReleaseSpinLock(&pAd->BATabLock);
+	return pBAEntry;
+}
+
+
+VOID BATableFreeOriEntry(
+						IN  PRTMP_ADAPTER   pAd,
+						IN  ULONG           Idx)
+{
+	BA_ORI_ENTRY    *pBAEntry = NULL;
+	MAC_TABLE_ENTRY *pEntry;
+
+
+	if ((Idx == 0) || (Idx >= MAX_LEN_OF_BA_ORI_TABLE))
+		return;
+
+	pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+
+	if (pBAEntry->ORI_BA_Status != Originator_NONE)
+	{
+		pEntry = &pAd->MacTab.Content[pBAEntry->Wcid];
+		pEntry->BAOriWcidArray[pBAEntry->TID] = 0;
+
+
+		NdisAcquireSpinLock(&pAd->BATabLock);
+		if (pBAEntry->ORI_BA_Status == Originator_Done)
+		{
+		 	pEntry->TXBAbitmap &= (~(1<<(pBAEntry->TID) ));
+			DBGPRINT(RT_DEBUG_TRACE, ("BATableFreeOriEntry numAsOriginator= %ld\n", pAd->BATable.numAsRecipient));
+			// Erase Bitmap flag.
+		}
+
+		ASSERT(pAd->BATable.numAsOriginator != 0);
+
+		pAd->BATable.numAsOriginator -= 1;
+
+		pBAEntry->ORI_BA_Status = Originator_NONE;
+		pBAEntry->Token = 0;
+		NdisReleaseSpinLock(&pAd->BATabLock);
+	}
+}
+
+
+VOID BATableFreeRecEntry(
+						IN  PRTMP_ADAPTER   pAd,
+						IN  ULONG           Idx)
+{
+	BA_REC_ENTRY    *pBAEntry = NULL;
+	MAC_TABLE_ENTRY *pEntry;
+
+
+	if ((Idx == 0) || (Idx >= MAX_LEN_OF_BA_REC_TABLE))
+		return;
+
+	pBAEntry =&pAd->BATable.BARecEntry[Idx];
+
+	if (pBAEntry->REC_BA_Status != Recipient_NONE)
+	{
+		pEntry = &pAd->MacTab.Content[pBAEntry->Wcid];
+		pEntry->BARecWcidArray[pBAEntry->TID] = 0;
+
+		NdisAcquireSpinLock(&pAd->BATabLock);
+
+		ASSERT(pAd->BATable.numAsRecipient != 0);
+
+		pAd->BATable.numAsRecipient -= 1;
+
+		pBAEntry->REC_BA_Status = Recipient_NONE;
+		NdisReleaseSpinLock(&pAd->BATabLock);
+	}
+}
+
+
+VOID BAOriSessionTearDown(
+						 IN OUT  PRTMP_ADAPTER   pAd,
+						 IN      UCHAR           Wcid,
+						 IN      UCHAR           TID,
+						 IN      BOOLEAN         bPassive,
+						 IN      BOOLEAN         bForceSend)
+{
+	ULONG           Idx = 0;
+	BA_ORI_ENTRY    *pBAEntry;
+	BOOLEAN         Cancelled;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+	{
+		return;
+	}
+
+	//
+	// Locate corresponding BA Originator Entry in BA Table with the (pAddr,TID).
+	//
+	Idx = pAd->MacTab.Content[Wcid].BAOriWcidArray[TID];
+	if ((Idx == 0) || (Idx >= MAX_LEN_OF_BA_ORI_TABLE))
+	{
+		if (bForceSend == TRUE)
+		{
+			// force send specified TID DelBA
+			MLME_DELBA_REQ_STRUCT   DelbaReq;
+			MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+			NdisZeroMemory(&DelbaReq, sizeof(DelbaReq));
+			NdisZeroMemory(Elem, sizeof(MLME_QUEUE_ELEM));
+
+			COPY_MAC_ADDR(DelbaReq.Addr, pAd->MacTab.Content[Wcid].Addr);
+			DelbaReq.Wcid = Wcid;
+			DelbaReq.TID = TID;
+			DelbaReq.Initiator = ORIGINATOR;
+#if 1
+			Elem->MsgLen  = sizeof(DelbaReq);
+			NdisMoveMemory(Elem->Msg, &DelbaReq, sizeof(DelbaReq));
+			MlmeDELBAAction(pAd, Elem);
+			kfree(Elem);
+#else
+			MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ORI_DELBA_CATE, sizeof(MLME_DELBA_REQ_STRUCT), (PVOID)&DelbaReq);
+			RT28XX_MLME_HANDLER(pAd);
+#endif
+		}
+
+		return;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s===>Wcid=%d.TID=%d \n", __func__, Wcid, TID));
+
+	pBAEntry = &pAd->BATable.BAOriEntry[Idx];
+	DBGPRINT(RT_DEBUG_TRACE,("\t===>Idx = %ld, Wcid=%d.TID=%d, ORI_BA_Status = %d \n", Idx, Wcid, TID, pBAEntry->ORI_BA_Status));
+	//
+	// Prepare DelBA action frame and send to the peer.
+	//
+	if ((bPassive == FALSE) && (TID == pBAEntry->TID) && (pBAEntry->ORI_BA_Status == Originator_Done))
+	{
+		MLME_DELBA_REQ_STRUCT   DelbaReq;
+		MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+		NdisZeroMemory(&DelbaReq, sizeof(DelbaReq));
+		NdisZeroMemory(Elem, sizeof(MLME_QUEUE_ELEM));
+
+		COPY_MAC_ADDR(DelbaReq.Addr, pAd->MacTab.Content[Wcid].Addr);
+		DelbaReq.Wcid = Wcid;
+		DelbaReq.TID = pBAEntry->TID;
+		DelbaReq.Initiator = ORIGINATOR;
+#if 1
+		Elem->MsgLen  = sizeof(DelbaReq);
+		NdisMoveMemory(Elem->Msg, &DelbaReq, sizeof(DelbaReq));
+		MlmeDELBAAction(pAd, Elem);
+		kfree(Elem);
+#else
+		MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ORI_DELBA_CATE, sizeof(MLME_DELBA_REQ_STRUCT), (PVOID)&DelbaReq);
+		RT28XX_MLME_HANDLER(pAd);
+#endif
+	}
+	RTMPCancelTimer(&pBAEntry->ORIBATimer, &Cancelled);
+	BATableFreeOriEntry(pAd, Idx);
+
+	if (bPassive)
+	{
+		//BAOriSessionSetUp(pAd, &pAd->MacTab.Content[Wcid], TID, 0, 10000, TRUE);
+	}
+}
+
+VOID BARecSessionTearDown(
+						 IN OUT  PRTMP_ADAPTER   pAd,
+						 IN      UCHAR           Wcid,
+						 IN      UCHAR           TID,
+						 IN      BOOLEAN         bPassive)
+{
+	ULONG           Idx = 0;
+	BA_REC_ENTRY    *pBAEntry;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+	{
+		return;
+	}
+
+	//
+	//  Locate corresponding BA Originator Entry in BA Table with the (pAddr,TID).
+	//
+	Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+	if (Idx == 0)
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s===>Wcid=%d.TID=%d \n", __func__, Wcid, TID));
+
+
+	pBAEntry = &pAd->BATable.BARecEntry[Idx];
+	DBGPRINT(RT_DEBUG_TRACE,("\t===>Idx = %ld, Wcid=%d.TID=%d, REC_BA_Status = %d \n", Idx, Wcid, TID, pBAEntry->REC_BA_Status));
+	//
+	// Prepare DelBA action frame and send to the peer.
+	//
+	if ((TID == pBAEntry->TID) && (pBAEntry->REC_BA_Status == Recipient_Accept))
+	{
+		MLME_DELBA_REQ_STRUCT   DelbaReq;
+		BOOLEAN 				Cancelled;
+		MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+		//ULONG   offset;
+		//UINT32  VALUE;
+
+		RTMPCancelTimer(&pBAEntry->RECBATimer, &Cancelled);
+
+		//
+		// 1. Send DELBA Action Frame
+		//
+		if (bPassive == FALSE)
+		{
+			NdisZeroMemory(&DelbaReq, sizeof(DelbaReq));
+			NdisZeroMemory(Elem, sizeof(MLME_QUEUE_ELEM));
+
+			COPY_MAC_ADDR(DelbaReq.Addr, pAd->MacTab.Content[Wcid].Addr);
+			DelbaReq.Wcid = Wcid;
+			DelbaReq.TID = TID;
+			DelbaReq.Initiator = RECIPIENT;
+#if 1
+			Elem->MsgLen  = sizeof(DelbaReq);
+			NdisMoveMemory(Elem->Msg, &DelbaReq, sizeof(DelbaReq));
+			MlmeDELBAAction(pAd, Elem);
+			kfree(Elem);
+#else
+			MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ORI_DELBA_CATE, sizeof(MLME_DELBA_REQ_STRUCT), (PVOID)&DelbaReq);
+			RT28XX_MLME_HANDLER(pAd);
+#endif
+		}
+
+
+		//
+		// 2. Free resource of BA session
+		//
+		// flush all pending reordering mpdus
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+
+		NdisAcquireSpinLock(&pAd->BATabLock);
+
+		// Erase Bitmap flag.
+		pBAEntry->LastIndSeq = RESET_RCV_SEQ;
+		pBAEntry->BAWinSize = 0;
+		// Erase Bitmap flag at software mactable
+		pAd->MacTab.Content[Wcid].RXBAbitmap &= (~(1<<(pBAEntry->TID)));
+		pAd->MacTab.Content[Wcid].BARecWcidArray[TID] = 0;
+
+		RT28XX_DEL_BA_SESSION_FROM_ASIC(pAd, Wcid, TID);
+
+		NdisReleaseSpinLock(&pAd->BATabLock);
+
+	}
+
+	BATableFreeRecEntry(pAd, Idx);
+}
+
+VOID BASessionTearDownALL(
+						 IN OUT  PRTMP_ADAPTER pAd,
+						 IN      UCHAR Wcid)
+{
+	int i;
+
+	for (i=0; i<NUM_OF_TID; i++)
+	{
+		BAOriSessionTearDown(pAd, Wcid, i, FALSE, FALSE);
+		BARecSessionTearDown(pAd, Wcid, i, FALSE);
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Retry sending ADDBA Reqest.
+
+	IRQL = DISPATCH_LEVEL
+
+	Parametrs:
+	p8023Header: if this is already 802.3 format, p8023Header is NULL
+
+	Return	: TRUE if put into rx reordering buffer, shouldn't indicaterxhere.
+				FALSE , then continue indicaterx at this moment.
+	==========================================================================
+ */
+VOID BAOriSessionSetupTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+	BA_ORI_ENTRY    *pBAEntry = (BA_ORI_ENTRY *)FunctionContext;
+	MAC_TABLE_ENTRY *pEntry;
+	PRTMP_ADAPTER   pAd;
+
+	if (pBAEntry == NULL)
+		return;
+
+	pAd = pBAEntry->pAdapter;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Do nothing if monitor mode is on
+		if (MONITOR_ON(pAd))
+			return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RALINK_ATE
+	// Nothing to do in ATE mode.
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	pEntry = &pAd->MacTab.Content[pBAEntry->Wcid];
+
+	if ((pBAEntry->ORI_BA_Status == Originator_WaitRes) && (pBAEntry->Token < ORI_SESSION_MAX_RETRY))
+	{
+		MLME_ADDBA_REQ_STRUCT    AddbaReq;
+
+		NdisZeroMemory(&AddbaReq, sizeof(AddbaReq));
+		COPY_MAC_ADDR(AddbaReq.pAddr, pEntry->Addr);
+		AddbaReq.Wcid = (UCHAR)(pEntry->Aid);
+		AddbaReq.TID = pBAEntry->TID;
+		AddbaReq.BaBufSize = pAd->CommonCfg.BACapability.field.RxBAWinLimit;
+		AddbaReq.TimeOutValue = 0;
+		AddbaReq.Token = pBAEntry->Token;
+		MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ADD_BA_CATE, sizeof(MLME_ADDBA_REQ_STRUCT), (PVOID)&AddbaReq);
+		RT28XX_MLME_HANDLER(pAd);
+		DBGPRINT(RT_DEBUG_TRACE,("BA Ori Session Timeout(%d) : Send ADD BA again\n", pBAEntry->Token));
+
+		pBAEntry->Token++;
+		RTMPSetTimer(&pBAEntry->ORIBATimer, ORI_BA_SESSION_TIMEOUT);
+	}
+	else
+	{
+		BATableFreeOriEntry(pAd, pEntry->BAOriWcidArray[pBAEntry->TID]);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		Retry sending ADDBA Reqest.
+
+	IRQL = DISPATCH_LEVEL
+
+	Parametrs:
+	p8023Header: if this is already 802.3 format, p8023Header is NULL
+
+	Return	: TRUE if put into rx reordering buffer, shouldn't indicaterxhere.
+				FALSE , then continue indicaterx at this moment.
+	==========================================================================
+ */
+VOID BARecSessionIdleTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+
+	BA_REC_ENTRY    *pBAEntry = (BA_REC_ENTRY *)FunctionContext;
+	PRTMP_ADAPTER   pAd;
+	ULONG           Now32;
+
+	if (pBAEntry == NULL)
+		return;
+
+	if ((pBAEntry->REC_BA_Status == Recipient_Accept))
+	{
+		NdisGetSystemUpTime(&Now32);
+
+		if (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer + REC_BA_SESSION_IDLE_TIMEOUT)))
+		{
+			pAd = pBAEntry->pAdapter;
+			// flush all pending reordering mpdus
+			ba_refresh_reordering_mpdus(pAd, pBAEntry);
+			printk("%ld: REC BA session Timeout\n", Now32);
+		}
+	}
+}
+
+
+VOID PeerAddBAReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	//	7.4.4.1
+	//ULONG	Idx;
+	UCHAR   Status = 1;
+	UCHAR   pAddr[6];
+	FRAME_ADDBA_RSP ADDframe;
+	PUCHAR         pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	PFRAME_ADDBA_REQ  pAddreqFrame = NULL;
+	//UCHAR		BufSize;
+	ULONG       FrameLen;
+	PULONG      ptemp;
+	PMAC_TABLE_ENTRY	pMacEntry;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s ==> (Wcid = %d)\n", __func__, Elem->Wcid));
+
+	//hex_dump("AddBAReq", Elem->Msg, Elem->MsgLen);
+
+	//ADDBA Request from unknown peer, ignore this.
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+	pMacEntry = &pAd->MacTab.Content[Elem->Wcid];
+	DBGPRINT(RT_DEBUG_TRACE,("BA - PeerAddBAReqAction----> \n"));
+	ptemp = (PULONG)Elem->Msg;
+	//DBGPRINT_RAW(RT_DEBUG_EMU, ("%08x:: %08x:: %08x:: %08x:: %08x:: %08x:: %08x:: %08x:: %08x\n", *(ptemp), *(ptemp+1), *(ptemp+2), *(ptemp+3), *(ptemp+4), *(ptemp+5), *(ptemp+6), *(ptemp+7), *(ptemp+8)));
+
+	if (PeerAddBAReqActionSanity(pAd, Elem->Msg, Elem->MsgLen, pAddr))
+	{
+
+		if ((pAd->CommonCfg.bBADecline == FALSE) && IS_HT_STA(pMacEntry))
+		{
+			pAddreqFrame = (PFRAME_ADDBA_REQ)(&Elem->Msg[0]);
+			printk("Rcv Wcid(%d) AddBAReq\n", Elem->Wcid);
+			if (BARecSessionAdd(pAd, &pAd->MacTab.Content[Elem->Wcid], pAddreqFrame))
+				Status = 0;
+			else
+				Status = 38; // more parameters have invalid values
+		}
+		else
+		{
+			Status = 37; // the request has been declined.
+		}
+	}
+
+	if (pAd->MacTab.Content[Elem->Wcid].ValidAsCLI)
+		ASSERT(pAd->MacTab.Content[Elem->Wcid].Sst == SST_ASSOC);
+
+	pAddreqFrame = (PFRAME_ADDBA_REQ)(&Elem->Msg[0]);
+	// 2. Always send back ADDBA Response
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);	 //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ACTION - PeerBAAction() allocate memory failed \n"));
+		return;
+	}
+
+	NdisZeroMemory(&ADDframe, sizeof(FRAME_ADDBA_RSP));
+	// 2-1. Prepare ADDBA Response frame.
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (ADHOC_ON(pAd))
+			ActHeaderInit(pAd, &ADDframe.Hdr, pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+		else
+#ifdef QOS_DLS_SUPPORT
+		if (pAd->MacTab.Content[Elem->Wcid].ValidAsDls)
+			ActHeaderInit(pAd, &ADDframe.Hdr, pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+		else
+#endif // QOS_DLS_SUPPORT //
+			ActHeaderInit(pAd, &ADDframe.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAddr);
+	}
+#endif // CONFIG_STA_SUPPORT //
+	ADDframe.Category = CATEGORY_BA;
+	ADDframe.Action = ADDBA_RESP;
+	ADDframe.Token = pAddreqFrame->Token;
+	// What is the Status code??  need to check.
+	ADDframe.StatusCode = Status;
+	ADDframe.BaParm.BAPolicy = IMMED_BA;
+	ADDframe.BaParm.AMSDUSupported = 0;
+	ADDframe.BaParm.TID = pAddreqFrame->BaParm.TID;
+	ADDframe.BaParm.BufSize = min(((UCHAR)pAddreqFrame->BaParm.BufSize), (UCHAR)pAd->CommonCfg.BACapability.field.RxBAWinLimit);
+	if (ADDframe.BaParm.BufSize == 0)
+	{
+		ADDframe.BaParm.BufSize = 64;
+	}
+	ADDframe.TimeOutValue = 0; //pAddreqFrame->TimeOutValue;
+
+	*(USHORT *)(&ADDframe.BaParm) = cpu2le16(*(USHORT *)(&ADDframe.BaParm));
+	ADDframe.StatusCode = cpu2le16(ADDframe.StatusCode);
+	ADDframe.TimeOutValue = cpu2le16(ADDframe.TimeOutValue);
+
+	MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+					  sizeof(FRAME_ADDBA_RSP),  &ADDframe,
+			  END_OF_ARGS);
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s(%d): TID(%d), BufSize(%d) <== \n", __func__, Elem->Wcid, ADDframe.BaParm.TID,
+							  ADDframe.BaParm.BufSize));
+}
+
+
+VOID PeerAddBARspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	//UCHAR		Idx, i;
+	//PUCHAR		   pOutBuffer = NULL;
+	PFRAME_ADDBA_RSP    pFrame = NULL;
+	//PBA_ORI_ENTRY		pBAEntry;
+
+	//ADDBA Response from unknown peer, ignore this.
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s ==> Wcid(%d)\n", __func__, Elem->Wcid));
+
+	//hex_dump("PeerAddBARspAction()", Elem->Msg, Elem->MsgLen);
+
+	if (PeerAddBARspActionSanity(pAd, Elem->Msg, Elem->MsgLen))
+	{
+		pFrame = (PFRAME_ADDBA_RSP)(&Elem->Msg[0]);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("\t\t StatusCode = %d\n", pFrame->StatusCode));
+		switch (pFrame->StatusCode)
+		{
+			case 0:
+				// I want a BAsession with this peer as an originator.
+				BAOriSessionAdd(pAd, &pAd->MacTab.Content[Elem->Wcid], pFrame);
+				break;
+			default:
+				// check status == USED ???
+				BAOriSessionTearDown(pAd, Elem->Wcid, pFrame->BaParm.TID, TRUE, FALSE);
+				break;
+		}
+		// Rcv Decline StatusCode
+		if ((pFrame->StatusCode == 37)
+#ifdef CONFIG_STA_SUPPORT
+            || ((pAd->OpMode == OPMODE_STA) && STA_TGN_WIFI_ON(pAd) && (pFrame->StatusCode != 0))
+#endif // CONFIG_STA_SUPPORT //
+            )
+		{
+			pAd->MacTab.Content[Elem->Wcid].BADeclineBitmap |= 1<<pFrame->BaParm.TID;
+		}
+	}
+}
+
+VOID PeerDelBAAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	//UCHAR				Idx;
+	//PUCHAR				pOutBuffer = NULL;
+	PFRAME_DELBA_REQ    pDelFrame = NULL;
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s ==>\n", __func__));
+	//DELBA Request from unknown peer, ignore this.
+	if (PeerDelBAActionSanity(pAd, Elem->Wcid, Elem->Msg, Elem->MsgLen))
+	{
+		pDelFrame = (PFRAME_DELBA_REQ)(&Elem->Msg[0]);
+		if (pDelFrame->DelbaParm.Initiator == ORIGINATOR)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - PeerDelBAAction----> ORIGINATOR\n"));
+			BARecSessionTearDown(pAd, Elem->Wcid, pDelFrame->DelbaParm.TID, TRUE);
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - PeerDelBAAction----> RECIPIENT, Reason = %d\n",  pDelFrame->ReasonCode));
+			//hex_dump("DelBA Frame", pDelFrame, Elem->MsgLen);
+			BAOriSessionTearDown(pAd, Elem->Wcid, pDelFrame->DelbaParm.TID, TRUE, FALSE);
+		}
+	}
+}
+
+
+BOOLEAN CntlEnqueueForRecv(
+						  IN PRTMP_ADAPTER		pAd,
+						  IN ULONG				Wcid,
+						  IN ULONG				MsgLen,
+						  IN PFRAME_BA_REQ		pMsg)
+{
+	PFRAME_BA_REQ   pFrame = pMsg;
+	//PRTMP_REORDERBUF	pBuffer;
+	//PRTMP_REORDERBUF	pDmaBuf;
+	PBA_REC_ENTRY pBAEntry;
+	//BOOLEAN 	Result;
+	ULONG   Idx;
+	//UCHAR	NumRxPkt;
+	UCHAR	TID;//, i;
+
+	TID = (UCHAR)pFrame->BARControl.TID;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s(): BAR-Wcid(%ld), Tid (%d)\n", __func__, Wcid, TID));
+	//hex_dump("BAR", (PCHAR) pFrame, MsgLen);
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return FALSE;
+
+	// First check the size, it MUST not exceed the mlme queue size
+	if (MsgLen > MGMT_DMA_BUFFER_SIZE)
+	{
+		DBGPRINT_ERR(("CntlEnqueueForRecv: frame too large, size = %ld \n", MsgLen));
+		return FALSE;
+	}
+	else if (MsgLen != sizeof(FRAME_BA_REQ))
+	{
+		DBGPRINT_ERR(("CntlEnqueueForRecv: BlockAck Request frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+	else if (MsgLen != sizeof(FRAME_BA_REQ))
+	{
+		DBGPRINT_ERR(("CntlEnqueueForRecv: BlockAck Request frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+
+	if ((Wcid < MAX_LEN_OF_MAC_TABLE) && (TID < 8))
+		{
+		// if this receiving packet is from SA that is in our OriEntry. Since WCID <9 has direct mapping. no need search.
+		Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+		}
+		else
+		{
+		return FALSE;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BAR(%ld) : Tid (%d) - %04x:%04x\n", Wcid, TID, pFrame->BAStartingSeq.field.StartSeq, pBAEntry->LastIndSeq ));
+
+	if (SEQ_SMALLER(pBAEntry->LastIndSeq, pFrame->BAStartingSeq.field.StartSeq, MAXSEQ))
+	{
+		//printk("BAR Seq = %x, LastIndSeq = %x\n", pFrame->BAStartingSeq.field.StartSeq, pBAEntry->LastIndSeq);
+		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, pFrame->BAStartingSeq.field.StartSeq);
+		pBAEntry->LastIndSeq = (pFrame->BAStartingSeq.field.StartSeq == 0) ? MAXSEQ :(pFrame->BAStartingSeq.field.StartSeq -1);
+	}
+	//ba_refresh_reordering_mpdus(pAd, pBAEntry);
+	return TRUE;
+}
+
+/*
+Description : Send PSMP Action frame If PSMP mode switches.
+*/
+VOID SendPSMPAction(
+				   IN PRTMP_ADAPTER		pAd,
+				   IN UCHAR				Wcid,
+				   IN UCHAR				Psmp)
+{
+	PUCHAR          pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	//ULONG           Idx;
+	FRAME_PSMP_ACTION   Frame;
+	ULONG           FrameLen;
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);	 //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeADDBAAction() allocate memory failed \n"));
+		return;
+	}
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		ActHeaderInit(pAd, &Frame.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->MacTab.Content[Wcid].Addr);
+#endif // CONFIG_STA_SUPPORT //
+
+	Frame.Category = CATEGORY_HT;
+	Frame.Action = SMPS_ACTION;
+	switch (Psmp)
+	{
+		case MMPS_ENABLE:
+			Frame.Psmp = 0;
+			break;
+		case MMPS_DYNAMIC:
+			Frame.Psmp = 3;
+			break;
+		case MMPS_STATIC:
+			Frame.Psmp = 1;
+			break;
+	}
+	MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+					  sizeof(FRAME_PSMP_ACTION),      &Frame,
+					  END_OF_ARGS);
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+	DBGPRINT(RT_DEBUG_ERROR,("HT - SendPSMPAction( %d )  \n", Frame.Psmp));
+}
+
+
+#define RADIO_MEASUREMENT_REQUEST_ACTION	0
+
+typedef struct PACKED
+{
+	UCHAR	RegulatoryClass;
+	UCHAR	ChannelNumber;
+	USHORT	RandomInterval;
+	USHORT	MeasurementDuration;
+	UCHAR	MeasurementMode;
+	UCHAR   BSSID[MAC_ADDR_LEN];
+	UCHAR	ReportingCondition;
+	UCHAR	Threshold;
+	UCHAR   SSIDIE[2];			// 2 byte
+} BEACON_REQUEST;
+
+typedef struct PACKED
+{
+	UCHAR	ID;
+	UCHAR	Length;
+	UCHAR	Token;
+	UCHAR	RequestMode;
+	UCHAR	Type;
+} MEASUREMENT_REQ;
+
+
+
+
+void convert_reordering_packet_to_preAMSDU_or_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN  UCHAR			FromWhichBSSID)
+{
+	PNDIS_PACKET	pRxPkt;
+	UCHAR			Header802_3[LENGTH_802_3];
+
+	// 1. get 802.3 Header
+	// 2. remove LLC
+	// 		a. pointer pRxBlk->pData to payload
+	//      b. modify pRxBlk->DataSize
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(pRxBlk, Header802_3);
+#endif // CONFIG_STA_SUPPORT //
+
+	ASSERT(pRxBlk->pRxPacket);
+	pRxPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+
+	RTPKT_TO_OSPKT(pRxPkt)->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+	RTPKT_TO_OSPKT(pRxPkt)->data = pRxBlk->pData;
+	RTPKT_TO_OSPKT(pRxPkt)->len = pRxBlk->DataSize;
+	RTPKT_TO_OSPKT(pRxPkt)->tail = RTPKT_TO_OSPKT(pRxPkt)->data + RTPKT_TO_OSPKT(pRxPkt)->len;
+
+	//
+	// copy 802.3 header, if necessary
+	//
+	if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU))
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef LINUX
+			NdisMoveMemory(skb_push(pRxPkt, LENGTH_802_3), Header802_3, LENGTH_802_3);
+#endif
+#ifdef UCOS
+			NdisMoveMemory(net_pkt_push(pRxPkt, LENGTH_802_3), Header802_3, LENGTH_802_3);
+#endif
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+}
+
+
+#define INDICATE_LEGACY_OR_AMSDU(_pAd, _pRxBlk, _fromWhichBSSID)		\
+	do																	\
+	{																	\
+    	if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_AMSDU))						\
+    	{																\
+    		Indicate_AMSDU_Packet(_pAd, _pRxBlk, _fromWhichBSSID);		\
+    	}																\
+		else if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_EAP))					\
+		{																\
+			Indicate_EAPOL_Packet(_pAd, _pRxBlk, _fromWhichBSSID);		\
+		}																\
+    	else															\
+    	{																\
+    		Indicate_Legacy_Packet(_pAd, _pRxBlk, _fromWhichBSSID);		\
+    	}																\
+	} while (0);
+
+
+
+static VOID ba_enqueue_reordering_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PBA_REC_ENTRY	pBAEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	struct reordering_mpdu *mpdu_blk;
+	UINT16	Sequence = (UINT16) pRxBlk->pHeader->Sequence;
+
+	mpdu_blk = ba_mpdu_blk_alloc(pAd);
+	if (mpdu_blk != NULL)
+	{
+		// Write RxD buffer address & allocated buffer length
+		NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+
+		mpdu_blk->Sequence = Sequence;
+
+		mpdu_blk->bAMSDU = RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU);
+
+		convert_reordering_packet_to_preAMSDU_or_802_3_packet(pAd, pRxBlk, FromWhichBSSID);
+
+		STATS_INC_RX_PACKETS(pAd, FromWhichBSSID);
+
+        //
+		// it is necessary for reordering packet to record
+		// which BSS it come from
+		//
+		RTMP_SET_PACKET_IF(pRxBlk->pRxPacket, FromWhichBSSID);
+
+		mpdu_blk->pPacket = pRxBlk->pRxPacket;
+
+		if (ba_reordering_mpdu_insertsorted(&pBAEntry->list, mpdu_blk) == FALSE)
+		{
+			// had been already within reordering list
+			// don't indicate
+			RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_SUCCESS);
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+		}
+
+		ASSERT((0<= pBAEntry->list.qlen)  && (pBAEntry->list.qlen <= pBAEntry->BAWinSize));
+		NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+	}
+	else
+	{
+#if 0
+		DBGPRINT(RT_DEBUG_ERROR,  ("!!! (%d:%d) Can't allocate reordering mpdu blk\n",
+								   blk_count, pBAEntry->list.qlen));
+#else
+		DBGPRINT(RT_DEBUG_ERROR,  ("!!! (%d) Can't allocate reordering mpdu blk\n",
+								   pBAEntry->list.qlen));
+#endif
+		/*
+		 * flush all pending reordering mpdus
+		 * and receving mpdu to upper layer
+		 * make tcp/ip to take care reordering mechanism
+		 */
+		//ba_refresh_reordering_mpdus(pAd, pBAEntry);
+		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, Sequence);
+
+		pBAEntry->LastIndSeq = Sequence;
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Indicate this packet to upper layer or put it into reordering buffer
+
+	Parametrs:
+		pRxBlk         : carry necessary packet info 802.11 format
+		FromWhichBSSID : the packet received from which BSS
+
+	Return	:
+			  none
+
+	Note    :
+	          the packet queued into reordering buffer need to cover to 802.3 format
+			  or pre_AMSDU format
+	==========================================================================
+ */
+
+VOID Indicate_AMPDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	USHORT				Idx;
+	PBA_REC_ENTRY		pBAEntry = NULL;
+	UINT16				Sequence = pRxBlk->pHeader->Sequence;
+	ULONG				Now32;
+	UCHAR				Wcid = pRxBlk->pRxWI->WirelessCliID;
+	UCHAR				TID = pRxBlk->pRxWI->TID;
+
+
+	if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU) &&  (pRxBlk->DataSize > MAX_RX_PKT_LEN))
+	{
+#if 0 // sample take off, no use
+		static int err_size;
+
+		err_size++;
+		if (err_size > 20) {
+			 printk("AMPDU DataSize = %d\n", pRxBlk->DataSize);
+			 hex_dump("802.11 Header", (UCHAR *)pRxBlk->pHeader, 24);
+			 hex_dump("Payload", pRxBlk->pData, 64);
+			 err_size = 0;
+		}
+#endif
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+
+#if 0 // test
+	/* Rec BA Session had been torn down */
+	INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+	return;
+#endif
+
+	if (Wcid < MAX_LEN_OF_MAC_TABLE)
+	{
+		Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+		if (Idx == 0)
+		{
+			/* Rec BA Session had been torn down */
+			INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+			return;
+		}
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+	}
+	else
+	{
+		// impossible !!!
+		ASSERT(0);
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	ASSERT(pBAEntry);
+
+	// update last rx time
+	NdisGetSystemUpTime(&Now32);
+
+	pBAEntry->rcvSeq = Sequence;
+
+
+	ba_flush_reordering_timeout_mpdus(pAd, pBAEntry, Now32);
+	pBAEntry->LastIndSeqAtTimer = Now32;
+
+	//
+	// Reset Last Indicate Sequence
+	//
+	if (pBAEntry->LastIndSeq == RESET_RCV_SEQ)
+	{
+		ASSERT((pBAEntry->list.qlen == 0) && (pBAEntry->list.next == NULL));
+
+		// reset rcv sequence of BA session
+		pBAEntry->LastIndSeq = Sequence;
+		pBAEntry->LastIndSeqAtTimer = Now32;
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+		return;
+	}
+
+
+	//
+	// I. Check if in order.
+	//
+	if (SEQ_STEPONE(Sequence, pBAEntry->LastIndSeq, MAXSEQ))
+	{
+		USHORT  LastIndSeq;
+
+		pBAEntry->LastIndSeq = Sequence;
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+ 		LastIndSeq = ba_indicate_reordering_mpdus_in_order(pAd, pBAEntry, pBAEntry->LastIndSeq);
+		if (LastIndSeq != RESET_RCV_SEQ)
+		{
+			pBAEntry->LastIndSeq = LastIndSeq;
+		}
+		pBAEntry->LastIndSeqAtTimer = Now32;
+	}
+	//
+	// II. Drop Duplicated Packet
+	//
+	else if (Sequence == pBAEntry->LastIndSeq)
+	{
+
+		// drop and release packet
+		pBAEntry->nDropPacket++;
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	}
+	//
+	// III. Drop Old Received Packet
+	//
+	else if (SEQ_SMALLER(Sequence, pBAEntry->LastIndSeq, MAXSEQ))
+	{
+
+		// drop and release packet
+		pBAEntry->nDropPacket++;
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	}
+	//
+	// IV. Receive Sequence within Window Size
+	//
+	else if (SEQ_SMALLER(Sequence, (((pBAEntry->LastIndSeq+pBAEntry->BAWinSize+1)) & MAXSEQ), MAXSEQ))
+	{
+		ba_enqueue_reordering_packet(pAd, pBAEntry, pRxBlk, FromWhichBSSID);
+	}
+	//
+	// V. Receive seq surpasses Win(lastseq + nMSDU). So refresh all reorder buffer
+	//
+	else
+	{
+#if 0
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+#else
+		LONG WinStartSeq, TmpSeq;
+
+
+		TmpSeq = Sequence - (pBAEntry->BAWinSize) -1;
+		if (TmpSeq < 0)
+		{
+			TmpSeq = (MAXSEQ+1) + TmpSeq;
+		}
+		WinStartSeq = (TmpSeq+1) & MAXSEQ;
+		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, WinStartSeq);
+		pBAEntry->LastIndSeq = WinStartSeq; //TmpSeq;
+
+		pBAEntry->LastIndSeqAtTimer = Now32;
+
+		ba_enqueue_reordering_packet(pAd, pBAEntry, pRxBlk, FromWhichBSSID);
+
+		TmpSeq = ba_indicate_reordering_mpdus_in_order(pAd, pBAEntry, pBAEntry->LastIndSeq);
+		if (TmpSeq != RESET_RCV_SEQ)
+		{
+			pBAEntry->LastIndSeq = TmpSeq;
+		}
+#endif
+	}
+}
+
+#endif // DOT11_N_SUPPORT //
+
diff --git a/drivers/staging/rt2870/common/cmm_data.c b/drivers/staging/rt2870/common/cmm_data.c
new file mode 100644
index 000000000000..fd809abf6df0
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_data.c
@@ -0,0 +1,2734 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+
+#include "../rt_config.h"
+
+#define MAX_TX_IN_TBTT		(16)
+
+
+UCHAR	SNAP_802_1H[] = {0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00};
+UCHAR	SNAP_BRIDGE_TUNNEL[] = {0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8};
+// Add Cisco Aironet SNAP heade for CCX2 support
+UCHAR	SNAP_AIRONET[] = {0xaa, 0xaa, 0x03, 0x00, 0x40, 0x96, 0x00, 0x00};
+UCHAR	CKIP_LLC_SNAP[] = {0xaa, 0xaa, 0x03, 0x00, 0x40, 0x96, 0x00, 0x02};
+UCHAR	EAPOL_LLC_SNAP[]= {0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00, 0x88, 0x8e};
+UCHAR	EAPOL[] = {0x88, 0x8e};
+UCHAR   TPID[] = {0x81, 0x00}; /* VLAN related */
+
+UCHAR	IPX[] = {0x81, 0x37};
+UCHAR	APPLE_TALK[] = {0x80, 0xf3};
+UCHAR	RateIdToPlcpSignal[12] = {
+	 0, /* RATE_1 */	1, /* RATE_2 */ 	2, /* RATE_5_5 */	3, /* RATE_11 */	// see BBP spec
+	11, /* RATE_6 */   15, /* RATE_9 */    10, /* RATE_12 */   14, /* RATE_18 */	// see IEEE802.11a-1999 p.14
+	 9, /* RATE_24 */  13, /* RATE_36 */	8, /* RATE_48 */   12  /* RATE_54 */ }; // see IEEE802.11a-1999 p.14
+
+UCHAR	 OfdmSignalToRateId[16] = {
+	RATE_54,  RATE_54,	RATE_54,  RATE_54,	// OFDM PLCP Signal = 0,  1,  2,  3 respectively
+	RATE_54,  RATE_54,	RATE_54,  RATE_54,	// OFDM PLCP Signal = 4,  5,  6,  7 respectively
+	RATE_48,  RATE_24,	RATE_12,  RATE_6,	// OFDM PLCP Signal = 8,  9,  10, 11 respectively
+	RATE_54,  RATE_36,	RATE_18,  RATE_9,	// OFDM PLCP Signal = 12, 13, 14, 15 respectively
+};
+
+UCHAR	 OfdmRateToRxwiMCS[12] = {
+	0,  0,	0,  0,
+	0,  1,	2,  3,	// OFDM rate 6,9,12,18 = rxwi mcs 0,1,2,3
+	4,  5,	6,  7,	// OFDM rate 24,36,48,54 = rxwi mcs 4,5,6,7
+};
+UCHAR	 RxwiMCSToOfdmRate[12] = {
+	RATE_6,  RATE_9,	RATE_12,  RATE_18,
+	RATE_24,  RATE_36,	RATE_48,  RATE_54,	// OFDM rate 6,9,12,18 = rxwi mcs 0,1,2,3
+	4,  5,	6,  7,	// OFDM rate 24,36,48,54 = rxwi mcs 4,5,6,7
+};
+
+char*   MCSToMbps[] = {"1Mbps","2Mbps","5.5Mbps","11Mbps","06Mbps","09Mbps","12Mbps","18Mbps","24Mbps","36Mbps","48Mbps","54Mbps","MM-0","MM-1","MM-2","MM-3","MM-4","MM-5","MM-6","MM-7","MM-8","MM-9","MM-10","MM-11","MM-12","MM-13","MM-14","MM-15","MM-32","ee1","ee2","ee3"};
+
+UCHAR default_cwmin[]={CW_MIN_IN_BITS, CW_MIN_IN_BITS, CW_MIN_IN_BITS-1, CW_MIN_IN_BITS-2};
+//UCHAR default_cwmax[]={CW_MAX_IN_BITS, CW_MAX_IN_BITS, CW_MIN_IN_BITS, CW_MIN_IN_BITS-1};
+UCHAR default_sta_aifsn[]={3,7,2,2};
+
+UCHAR MapUserPriorityToAccessCategory[8] = {QID_AC_BE, QID_AC_BK, QID_AC_BK, QID_AC_BE, QID_AC_VI, QID_AC_VI, QID_AC_VO, QID_AC_VO};
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		API for MLME to transmit management frame to AP (BSS Mode)
+	or station (IBSS Mode)
+
+	Arguments:
+		pAd Pointer to our adapter
+		pData		Pointer to the outgoing 802.11 frame
+		Length		Size of outgoing management frame
+
+	Return Value:
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_PENDING
+		NDIS_STATUS_SUCCESS
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS MiniportMMRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	PUCHAR			pData,
+	IN	UINT			Length)
+{
+	PNDIS_PACKET	pPacket;
+	NDIS_STATUS  	Status = NDIS_STATUS_SUCCESS;
+	ULONG	 		FreeNum;
+	UCHAR			IrqState;
+	UCHAR			rtmpHwHdr[TXINFO_SIZE + TXWI_SIZE]; //RTMP_HW_HDR_LEN];
+
+	ASSERT(Length <= MGMT_DMA_BUFFER_SIZE);
+
+	QueIdx=3;
+
+	// 2860C use Tx Ring
+
+	IrqState = pAd->irq_disabled;
+
+	do
+	{
+		// Reset is in progress, stop immediately
+		if ( RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+			 RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST)||
+			 !RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP))
+		{
+			Status = NDIS_STATUS_FAILURE;
+			break;
+		}
+
+		// Check Free priority queue
+		// Since we use PBF Queue2 for management frame.  Its corresponding DMA ring should be using TxRing.
+
+		// 2860C use Tx Ring
+		if (pAd->MACVersion == 0x28600100)
+		{
+			FreeNum = GET_TXRING_FREENO(pAd, QueIdx);
+		}
+		else
+		{
+			FreeNum = GET_MGMTRING_FREENO(pAd);
+		}
+
+		if ((FreeNum > 0))
+		{
+			// We need to reserve space for rtmp hardware header. i.e., TxWI for RT2860 and TxInfo+TxWI for RT2870
+			NdisZeroMemory(&rtmpHwHdr, (TXINFO_SIZE + TXWI_SIZE));
+			Status = RTMPAllocateNdisPacket(pAd, &pPacket, (PUCHAR)&rtmpHwHdr, (TXINFO_SIZE + TXWI_SIZE), pData, Length);
+			if (Status != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("MiniportMMRequest (error:: can't allocate NDIS PACKET)\n"));
+				break;
+			}
+
+			//pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+			//pAd->CommonCfg.MlmeRate = RATE_2;
+
+
+			Status = MlmeHardTransmit(pAd, QueIdx, pPacket);
+			if (Status != NDIS_STATUS_SUCCESS)
+				RTMPFreeNdisPacket(pAd, pPacket);
+		}
+		else
+		{
+			pAd->RalinkCounters.MgmtRingFullCount++;
+			DBGPRINT(RT_DEBUG_ERROR, ("Qidx(%d), not enough space in MgmtRing, MgmtRingFullCount=%ld!\n",
+										QueIdx, pAd->RalinkCounters.MgmtRingFullCount));
+		}
+
+	} while (FALSE);
+
+
+	return Status;
+}
+
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware transmit function
+
+	Arguments:
+		pAd Pointer to our adapter
+		pBuffer 	Pointer to	memory of outgoing frame
+		Length		Size of outgoing management frame
+
+	Return Value:
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_PENDING
+		NDIS_STATUS_SUCCESS
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS MlmeHardTransmit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	PNDIS_PACKET	pPacket)
+{
+	if ((pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE)
+#ifdef CARRIER_DETECTION_SUPPORT
+#endif // CARRIER_DETECTION_SUPPORT //
+		)
+	{
+		return NDIS_STATUS_FAILURE;
+	}
+
+		return MlmeHardTransmitMgmtRing(pAd,QueIdx,pPacket);
+
+}
+
+
+
+NDIS_STATUS MlmeHardTransmitMgmtRing(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	QueIdx,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	PHEADER_802_11	pHeader_802_11;
+	BOOLEAN 		bAckRequired, bInsertTimestamp;
+	UCHAR			MlmeRate;
+	PTXWI_STRUC 	pFirstTxWI;
+	MAC_TABLE_ENTRY	*pMacEntry = NULL;
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+
+	// Make sure MGMT ring resource won't be used by other threads
+// sample, for IRQ LOCK -> SEM LOCK
+//	IrqState = pAd->irq_disabled;
+//	if (!IrqState)
+		RTMP_SEM_LOCK(&pAd->MgmtRingLock);
+
+
+	if (pSrcBufVA == NULL)
+	{
+		// The buffer shouldn't be NULL
+//		if (!IrqState)
+			RTMP_SEM_UNLOCK(&pAd->MgmtRingLock);
+		return NDIS_STATUS_FAILURE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// outgoing frame always wakeup PHY to prevent frame lost
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+			AsicForceWakeup(pAd, TRUE);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	pFirstTxWI = (PTXWI_STRUC)(pSrcBufVA +  TXINFO_SIZE);
+	pHeader_802_11 = (PHEADER_802_11) (pSrcBufVA + TXINFO_SIZE + TXWI_SIZE); //TXWI_SIZE);
+
+	if (pHeader_802_11->Addr1[0] & 0x01)
+	{
+		MlmeRate = pAd->CommonCfg.BasicMlmeRate;
+	}
+	else
+	{
+		MlmeRate = pAd->CommonCfg.MlmeRate;
+	}
+
+	// Verify Mlme rate for a / g bands.
+	if ((pAd->LatchRfRegs.Channel > 14) && (MlmeRate < RATE_6)) // 11A band
+		MlmeRate = RATE_6;
+
+	if ((pHeader_802_11->FC.Type == BTYPE_DATA) &&
+		(pHeader_802_11->FC.SubType == SUBTYPE_QOS_NULL))
+	{
+		pMacEntry = MacTableLookup(pAd, pHeader_802_11->Addr1);
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Fixed W52 with Activity scan issue in ABG_MIXED and ABGN_MIXED mode.
+		if (pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED
+#ifdef DOT11_N_SUPPORT
+			|| pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED
+#endif // DOT11_N_SUPPORT //
+		)
+		{
+			if (pAd->LatchRfRegs.Channel > 14)
+				pAd->CommonCfg.MlmeTransmit.field.MODE = 1;
+			else
+				pAd->CommonCfg.MlmeTransmit.field.MODE = 0;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	//
+	// Should not be hard code to set PwrMgmt to 0 (PWR_ACTIVE)
+	// Snice it's been set to 0 while on MgtMacHeaderInit
+	// By the way this will cause frame to be send on PWR_SAVE failed.
+	//
+	// pHeader_802_11->FC.PwrMgmt = 0; // (pAd->StaCfg.Psm == PWR_SAVE);
+	//
+	// In WMM-UAPSD, mlme frame should be set psm as power saving but probe request frame
+#ifdef CONFIG_STA_SUPPORT
+    // Data-Null packets alse pass through MMRequest in RT2860, however, we hope control the psm bit to pass APSD
+	if ((pHeader_802_11->FC.Type != BTYPE_DATA) && (pHeader_802_11->FC.Type != BTYPE_CNTL))
+	{
+		if ((pAd->StaCfg.Psm == PWR_SAVE) &&
+			(pHeader_802_11->FC.SubType == SUBTYPE_ACTION))
+			pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+		else
+			pHeader_802_11->FC.PwrMgmt = PWR_ACTIVE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	bInsertTimestamp = FALSE;
+	if (pHeader_802_11->FC.Type == BTYPE_CNTL) // must be PS-POLL
+	{
+#ifdef CONFIG_STA_SUPPORT
+		//Set PM bit in ps-poll, to fix WLK 1.2  PowerSaveMode_ext failure issue.
+		if ((pAd->OpMode == OPMODE_STA) && (pHeader_802_11->FC.SubType == SUBTYPE_PS_POLL))
+		{
+			pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+		}
+#endif // CONFIG_STA_SUPPORT //
+		bAckRequired = FALSE;
+	}
+	else // BTYPE_MGMT or BTYPE_DATA(must be NULL frame)
+	{
+		//pAd->Sequence++;
+		//pHeader_802_11->Sequence = pAd->Sequence;
+
+		if (pHeader_802_11->Addr1[0] & 0x01) // MULTICAST, BROADCAST
+		{
+			bAckRequired = FALSE;
+			pHeader_802_11->Duration = 0;
+		}
+		else
+		{
+			bAckRequired = TRUE;
+			pHeader_802_11->Duration = RTMPCalcDuration(pAd, MlmeRate, 14);
+			if (pHeader_802_11->FC.SubType == SUBTYPE_PROBE_RSP)
+			{
+				bInsertTimestamp = TRUE;
+			}
+		}
+	}
+
+	pHeader_802_11->Sequence = pAd->Sequence++;
+	if (pAd->Sequence >0xfff)
+		pAd->Sequence = 0;
+
+	// Before radar detection done, mgmt frame can not be sent but probe req
+	// Because we need to use probe req to trigger driver to send probe req in passive scan
+	if ((pHeader_802_11->FC.SubType != SUBTYPE_PROBE_REQ)
+		&& (pAd->CommonCfg.bIEEE80211H == 1)
+		&& (pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("MlmeHardTransmit --> radar detect not in normal mode !!!\n"));
+//		if (!IrqState)
+			RTMP_SEM_UNLOCK(&pAd->MgmtRingLock);
+		return (NDIS_STATUS_FAILURE);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pHeader_802_11, DIR_WRITE, FALSE);
+#endif
+
+	//
+	// fill scatter-and-gather buffer list into TXD. Internally created NDIS PACKET
+	// should always has only one ohysical buffer, and the whole frame size equals
+	// to the first scatter buffer size
+	//
+
+	// Initialize TX Descriptor
+	// For inter-frame gap, the number is for this frame and next frame
+	// For MLME rate, we will fix as 2Mb to match other vendor's implement
+//	pAd->CommonCfg.MlmeTransmit.field.MODE = 1;
+
+// management frame doesn't need encryption. so use RESERVED_WCID no matter u are sending to specific wcid or not.
+	if (pMacEntry == NULL)
+	{
+		RTMPWriteTxWI(pAd, pFirstTxWI, FALSE, FALSE, bInsertTimestamp, FALSE, bAckRequired, FALSE,
+		0, RESERVED_WCID, (SrcBufLen - TXINFO_SIZE - TXWI_SIZE), PID_MGMT, 0,  (UCHAR)pAd->CommonCfg.MlmeTransmit.field.MCS, IFS_BACKOFF, FALSE, &pAd->CommonCfg.MlmeTransmit);
+	}
+	else
+	{
+		RTMPWriteTxWI(pAd, pFirstTxWI, FALSE, FALSE,
+					bInsertTimestamp, FALSE, bAckRequired, FALSE,
+					0, pMacEntry->Aid, (SrcBufLen - TXINFO_SIZE - TXWI_SIZE),
+					pMacEntry->MaxHTPhyMode.field.MCS, 0,
+					(UCHAR)pMacEntry->MaxHTPhyMode.field.MCS,
+					IFS_BACKOFF, FALSE, &pMacEntry->MaxHTPhyMode);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)pFirstTxWI, TYPE_TXWI);
+#endif
+
+	// Now do hardware-depened kick out.
+	HAL_KickOutMgmtTx(pAd, QueIdx, pPacket, pSrcBufVA, SrcBufLen);
+
+	// Make sure to release MGMT ring resource
+//	if (!IrqState)
+		RTMP_SEM_UNLOCK(&pAd->MgmtRingLock);
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+/********************************************************************************
+
+	New DeQueue Procedures.
+
+ ********************************************************************************/
+
+#define DEQUEUE_LOCK(lock, bIntContext, IrqFlags) 				\
+			do{													\
+				if (bIntContext == FALSE)						\
+				RTMP_IRQ_LOCK((lock), IrqFlags);		\
+			}while(0)
+
+#define DEQUEUE_UNLOCK(lock, bIntContext, IrqFlags)				\
+			do{													\
+				if (bIntContext == FALSE)						\
+					RTMP_IRQ_UNLOCK((lock), IrqFlags);	\
+			}while(0)
+
+
+#if 0
+static VOID dumpTxBlk(TX_BLK *pTxBlk)
+{
+	NDIS_PACKET *pPacket;
+	int i, frameNum;
+	PQUEUE_ENTRY	pQEntry;
+
+	printk("Dump TX_BLK Structure:\n");
+	printk("\tTxFrameType=%d!\n", pTxBlk->TxFrameType);
+	printk("\tTotalFrameLen=%d\n", pTxBlk->TotalFrameLen);
+	printk("\tTotalFrameNum=%ld!\n", pTxBlk->TxPacketList.Number);
+	printk("\tTotalFragNum=%d!\n", pTxBlk->TotalFragNum);
+	printk("\tpPacketList=\n");
+
+	frameNum = pTxBlk->TxPacketList.Number;
+
+	for(i=0; i < frameNum; i++)
+	{	int j;
+		UCHAR	*pBuf;
+
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+		if (pPacket)
+		{
+			pBuf = GET_OS_PKT_DATAPTR(pPacket);
+			printk("\t\t[%d]:ptr=0x%x, Len=%d!\n", i, (UINT32)(GET_OS_PKT_DATAPTR(pPacket)), GET_OS_PKT_LEN(pPacket));
+			printk("\t\t");
+			for (j =0 ; j < GET_OS_PKT_LEN(pPacket); j++)
+			{
+				printk("%02x ", (pBuf[j] & 0xff));
+				if (j == 16)
+					break;
+			}
+			InsertTailQueue(&pTxBlk->TxPacketList, PACKET_TO_QUEUE_ENTRY(pPacket));
+		}
+	}
+	printk("\tWcid=%d!\n", pTxBlk->Wcid);
+	printk("\tapidx=%d!\n", pTxBlk->apidx);
+	printk("----EndOfDump\n");
+
+}
+#endif
+
+
+/*
+	========================================================================
+	Tx Path design algorithm:
+		Basically, we divide the packets into four types, Broadcast/Multicast, 11N Rate(AMPDU, AMSDU, Normal), B/G Rate(ARALINK, Normal),
+		Specific Packet Type. Following show the classification rule and policy for each kinds of packets.
+				Classification Rule=>
+					Multicast: (*addr1 & 0x01) == 0x01
+					Specific : bDHCPFrame, bARPFrame, bEAPOLFrame, etc.
+					11N Rate : If peer support HT
+								(1).AMPDU  -- If TXBA is negotiated.
+								(2).AMSDU  -- If AMSDU is capable for both peer and ourself.
+											*). AMSDU can embedded in a AMPDU, but now we didn't support it.
+								(3).Normal -- Other packets which send as 11n rate.
+
+					B/G Rate : If peer is b/g only.
+								(1).ARALINK-- If both of peer/us supprot Ralink proprietary Aggregation and the TxRate is large than RATE_6
+								(2).Normal -- Other packets which send as b/g rate.
+					Fragment:
+								The packet must be unicast, NOT A-RALINK, NOT A-MSDU, NOT 11n, then can consider about fragment.
+
+				Classified Packet Handle Rule=>
+					Multicast:
+								No ACK, 		//pTxBlk->bAckRequired = FALSE;
+								No WMM, 		//pTxBlk->bWMM = FALSE;
+								No piggyback,   //pTxBlk->bPiggyBack = FALSE;
+								Force LowRate,  //pTxBlk->bForceLowRate = TRUE;
+					Specific :	Basically, for specific packet, we should handle it specifically, but now all specific packets are use
+									the same policy to handle it.
+								Force LowRate,  //pTxBlk->bForceLowRate = TRUE;
+
+					11N Rate :
+								No piggyback,	//pTxBlk->bPiggyBack = FALSE;
+
+								(1).AMSDU
+									pTxBlk->bWMM = TRUE;
+								(2).AMPDU
+									pTxBlk->bWMM = TRUE;
+								(3).Normal
+
+					B/G Rate :
+								(1).ARALINK
+
+								(2).Normal
+	========================================================================
+*/
+static UCHAR TxPktClassification(
+	IN RTMP_ADAPTER *pAd,
+	IN PNDIS_PACKET  pPacket)
+{
+	UCHAR			TxFrameType = TX_UNKOWN_FRAME;
+	UCHAR			Wcid;
+	MAC_TABLE_ENTRY	*pMacEntry = NULL;
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN			bHTRate = FALSE;
+#endif // DOT11_N_SUPPORT //
+
+	Wcid = RTMP_GET_PACKET_WCID(pPacket);
+	if (Wcid == MCAST_WCID)
+	{	// Handle for RA is Broadcast/Multicast Address.
+		return TX_MCAST_FRAME;
+	}
+
+	// Handle for unicast packets
+	pMacEntry = &pAd->MacTab.Content[Wcid];
+	if (RTMP_GET_PACKET_LOWRATE(pPacket))
+	{	// It's a specific packet need to force low rate, i.e., bDHCPFrame, bEAPOLFrame, bWAIFrame
+		TxFrameType = TX_LEGACY_FRAME;
+	}
+#ifdef DOT11_N_SUPPORT
+	else if (IS_HT_RATE(pMacEntry))
+	{	// it's a 11n capable packet
+
+		// Depends on HTPhyMode to check if the peer support the HTRate transmission.
+		// 	Currently didn't support A-MSDU embedded in A-MPDU
+		bHTRate = TRUE;
+		if (RTMP_GET_PACKET_MOREDATA(pPacket) || (pMacEntry->PsMode == PWR_SAVE))
+			TxFrameType = TX_LEGACY_FRAME;
+#ifdef UAPSD_AP_SUPPORT
+		else if (RTMP_GET_PACKET_EOSP(pPacket))
+			TxFrameType = TX_LEGACY_FRAME;
+#endif // UAPSD_AP_SUPPORT //
+		else if((pMacEntry->TXBAbitmap & (1<<(RTMP_GET_PACKET_UP(pPacket)))) != 0)
+			return TX_AMPDU_FRAME;
+		else if(CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_AMSDU_INUSED))
+			return TX_AMSDU_FRAME;
+		else
+			TxFrameType = TX_LEGACY_FRAME;
+	}
+#endif // DOT11_N_SUPPORT //
+	else
+	{	// it's a legacy b/g packet.
+		if ((CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_AGGREGATION_CAPABLE) && pAd->CommonCfg.bAggregationCapable) &&
+			(RTMP_GET_PACKET_TXRATE(pPacket) >= RATE_6) &&
+			(!(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) && CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_WMM_CAPABLE))))
+		{	// if peer support Ralink Aggregation, we use it.
+			TxFrameType = TX_RALINK_FRAME;
+		}
+		else
+		{
+			TxFrameType = TX_LEGACY_FRAME;
+		}
+	}
+
+	// Currently, our fragment only support when a unicast packet send as NOT-ARALINK, NOT-AMSDU and NOT-AMPDU.
+	if ((RTMP_GET_PACKET_FRAGMENTS(pPacket) > 1) && (TxFrameType == TX_LEGACY_FRAME))
+		TxFrameType = TX_FRAG_FRAME;
+
+	return TxFrameType;
+}
+
+
+BOOLEAN RTMP_FillTxBlkInfo(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK *pTxBlk)
+{
+	PACKET_INFO			PacketInfo;
+	PNDIS_PACKET		pPacket;
+	PMAC_TABLE_ENTRY	pMacEntry = NULL;
+
+	pPacket = pTxBlk->pPacket;
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pTxBlk->pSrcBufHeader, &pTxBlk->SrcBufLen);
+
+	pTxBlk->Wcid	 	 		= RTMP_GET_PACKET_WCID(pPacket);
+	pTxBlk->apidx		 		= RTMP_GET_PACKET_IF(pPacket);
+	pTxBlk->UserPriority 		= RTMP_GET_PACKET_UP(pPacket);
+	pTxBlk->FrameGap = IFS_HTTXOP;		// ASIC determine Frame Gap
+
+	if (RTMP_GET_PACKET_CLEAR_EAP_FRAME(pTxBlk->pPacket))
+		TX_BLK_SET_FLAG(pTxBlk, fTX_bClearEAPFrame);
+	else
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bClearEAPFrame);
+
+	// Default to clear this flag
+	TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bForceNonQoS);
+
+
+	if (pTxBlk->Wcid == MCAST_WCID)
+	{
+		pTxBlk->pMacEntry = NULL;
+		{
+#ifdef MCAST_RATE_SPECIFIC
+			PUCHAR pDA = GET_OS_PKT_DATAPTR(pPacket);
+			if (((*pDA & 0x01) == 0x01) && (*pDA != 0xff))
+				pTxBlk->pTransmit = &pAd->CommonCfg.MCastPhyMode;
+			else
+#endif // MCAST_RATE_SPECIFIC //
+				pTxBlk->pTransmit = &pAd->MacTab.Content[MCAST_WCID].HTPhyMode;
+		}
+
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bAckRequired);	// AckRequired = FALSE, when broadcast packet in Adhoc mode.
+		//TX_BLK_SET_FLAG(pTxBlk, fTX_bForceLowRate);
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bAllowFrag);
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bWMM);
+		if (RTMP_GET_PACKET_MOREDATA(pPacket))
+		{
+			TX_BLK_SET_FLAG(pTxBlk, fTX_bMoreData);
+		}
+
+	}
+	else
+	{
+		pTxBlk->pMacEntry = &pAd->MacTab.Content[pTxBlk->Wcid];
+		pTxBlk->pTransmit = &pTxBlk->pMacEntry->HTPhyMode;
+
+		pMacEntry = pTxBlk->pMacEntry;
+
+
+		// For all unicast packets, need Ack unless the Ack Policy is not set as NORMAL_ACK.
+		if (pAd->CommonCfg.AckPolicy[pTxBlk->QueIdx] != NORMAL_ACK)
+			TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bAckRequired);
+		else
+			TX_BLK_SET_FLAG(pTxBlk, fTX_bAckRequired);
+
+		{
+
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			{
+
+				// If support WMM, enable it.
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) &&
+					CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_WMM_CAPABLE))
+					TX_BLK_SET_FLAG(pTxBlk, fTX_bWMM);
+			}
+#endif // CONFIG_STA_SUPPORT //
+		}
+
+		if (pTxBlk->TxFrameType == TX_LEGACY_FRAME)
+		{
+			if ( (RTMP_GET_PACKET_LOWRATE(pPacket)) ||
+                ((pAd->OpMode == OPMODE_AP) && (pMacEntry->MaxHTPhyMode.field.MODE == MODE_CCK) && (pMacEntry->MaxHTPhyMode.field.MCS == RATE_1)))
+			{	// Specific packet, i.e., bDHCPFrame, bEAPOLFrame, bWAIFrame, need force low rate.
+				pTxBlk->pTransmit = &pAd->MacTab.Content[MCAST_WCID].HTPhyMode;
+#ifdef DOT11_N_SUPPORT
+				// Modify the WMM bit for ICV issue. If we have a packet with EOSP field need to set as 1, how to handle it???
+				if (IS_HT_STA(pTxBlk->pMacEntry) &&
+					(CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_RALINK_CHIPSET)) &&
+					((pAd->CommonCfg.bRdg == TRUE) && CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_RDG_CAPABLE)))
+				{
+					TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bWMM);
+					TX_BLK_SET_FLAG(pTxBlk, fTX_bForceNonQoS);
+				}
+#endif // DOT11_N_SUPPORT //
+			}
+
+#ifdef DOT11_N_SUPPORT
+			if ( (IS_HT_RATE(pMacEntry) == FALSE) &&
+				(CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_PIGGYBACK_CAPABLE)))
+			{	// Currently piggy-back only support when peer is operate in b/g mode.
+				TX_BLK_SET_FLAG(pTxBlk, fTX_bPiggyBack);
+			}
+#endif // DOT11_N_SUPPORT //
+
+			if (RTMP_GET_PACKET_MOREDATA(pPacket))
+			{
+				TX_BLK_SET_FLAG(pTxBlk, fTX_bMoreData);
+			}
+#ifdef UAPSD_AP_SUPPORT
+			if (RTMP_GET_PACKET_EOSP(pPacket))
+			{
+				TX_BLK_SET_FLAG(pTxBlk, fTX_bWMM_UAPSD_EOSP);
+			}
+#endif // UAPSD_AP_SUPPORT //
+		}
+		else if (pTxBlk->TxFrameType == TX_FRAG_FRAME)
+		{
+			TX_BLK_SET_FLAG(pTxBlk, fTX_bAllowFrag);
+		}
+
+		pMacEntry->DebugTxCount++;
+	}
+
+	return TRUE;
+
+FillTxBlkErr:
+	return FALSE;
+}
+
+
+BOOLEAN CanDoAggregateTransmit(
+	IN RTMP_ADAPTER *pAd,
+	IN NDIS_PACKET *pPacket,
+	IN TX_BLK		*pTxBlk)
+{
+
+	//printk("Check if can do aggregation! TxFrameType=%d!\n", pTxBlk->TxFrameType);
+
+	if (RTMP_GET_PACKET_WCID(pPacket) == MCAST_WCID)
+		return FALSE;
+
+	if (RTMP_GET_PACKET_DHCP(pPacket) ||
+		RTMP_GET_PACKET_EAPOL(pPacket) ||
+		RTMP_GET_PACKET_WAI(pPacket))
+		return FALSE;
+
+	if ((pTxBlk->TxFrameType == TX_AMSDU_FRAME) &&
+		((pTxBlk->TotalFrameLen + GET_OS_PKT_LEN(pPacket))> (RX_BUFFER_AGGRESIZE - 100)))
+	{	// For AMSDU, allow the packets with total length < max-amsdu size
+		return FALSE;
+	}
+
+	if ((pTxBlk->TxFrameType == TX_RALINK_FRAME) &&
+		(pTxBlk->TxPacketList.Number == 2))
+	{	// For RALINK-Aggregation, allow two frames in one batch.
+		return FALSE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	if ((INFRA_ON(pAd)) && (pAd->OpMode == OPMODE_STA)) // must be unicast to AP
+		return TRUE;
+	else
+#endif // CONFIG_STA_SUPPORT //
+		return FALSE;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		To do the enqueue operation and extract the first item of waiting
+		list. If a number of available shared memory segments could meet
+		the request of extracted item, the extracted item will be fragmented
+		into shared memory segments.
+
+	Arguments:
+		pAd Pointer to our adapter
+		pQueue		Pointer to Waiting Queue
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPDeQueuePacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  BOOLEAN         bIntContext,
+	IN  UCHAR			QIdx, /* BulkOutPipeId */
+	IN  UCHAR           Max_Tx_Packets)
+{
+	PQUEUE_ENTRY    pEntry = NULL;
+	PNDIS_PACKET 	pPacket;
+	NDIS_STATUS     Status = NDIS_STATUS_SUCCESS;
+	UCHAR           Count=0;
+	PQUEUE_HEADER   pQueue;
+	ULONG           FreeNumber[NUM_OF_TX_RING];
+	UCHAR			QueIdx, sQIdx, eQIdx;
+	unsigned long	IrqFlags = 0;
+	BOOLEAN			hasTxDesc = FALSE;
+	TX_BLK			TxBlk;
+	TX_BLK			*pTxBlk;
+
+#ifdef DBG_DIAGNOSE
+	BOOLEAN			firstRound;
+	RtmpDiagStruct	*pDiagStruct = &pAd->DiagStruct;
+#endif
+
+
+	if (QIdx == NUM_OF_TX_RING)
+	{
+		sQIdx = 0;
+		eQIdx = 3;	// 4 ACs, start from 0.
+	}
+	else
+	{
+		sQIdx = eQIdx = QIdx;
+	}
+
+	for (QueIdx=sQIdx; QueIdx <= eQIdx; QueIdx++)
+	{
+		Count=0;
+
+		RT28XX_START_DEQUEUE(pAd, QueIdx, IrqFlags);
+
+#ifdef DBG_DIAGNOSE
+		firstRound = ((QueIdx == 0) ? TRUE : FALSE);
+#endif // DBG_DIAGNOSE //
+
+		while (1)
+		{
+			if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS |
+										fRTMP_ADAPTER_RADIO_OFF |
+										fRTMP_ADAPTER_RESET_IN_PROGRESS |
+										fRTMP_ADAPTER_HALT_IN_PROGRESS |
+										fRTMP_ADAPTER_NIC_NOT_EXIST))))
+			{
+				RT28XX_STOP_DEQUEUE(pAd, QueIdx, IrqFlags);
+				return;
+			}
+
+			if (Count >= Max_Tx_Packets)
+				break;
+
+			DEQUEUE_LOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+			if (&pAd->TxSwQueue[QueIdx] == NULL)
+			{
+#ifdef DBG_DIAGNOSE
+				if (firstRound == TRUE)
+					pDiagStruct->TxSWQueCnt[pDiagStruct->ArrayCurIdx][0]++;
+#endif // DBG_DIAGNOSE //
+				DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+				break;
+			}
+
+
+			// probe the Queue Head
+			pQueue = &pAd->TxSwQueue[QueIdx];
+			if ((pEntry = pQueue->Head) == NULL)
+			{
+				DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+				break;
+			}
+
+			pTxBlk = &TxBlk;
+			NdisZeroMemory((PUCHAR)pTxBlk, sizeof(TX_BLK));
+			//InitializeQueueHeader(&pTxBlk->TxPacketList);		// Didn't need it because we already memzero it.
+			pTxBlk->QueIdx = QueIdx;
+
+			pPacket = QUEUE_ENTRY_TO_PKT(pEntry);
+
+			// Early check to make sure we have enoguh Tx Resource.
+			hasTxDesc = RT28XX_HAS_ENOUGH_FREE_DESC(pAd, pTxBlk, FreeNumber[QueIdx], pPacket);
+			if (!hasTxDesc)
+			{
+				pAd->PrivateInfo.TxRingFullCnt++;
+
+				DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+
+				break;
+			}
+
+			pTxBlk->TxFrameType = TxPktClassification(pAd, pPacket);
+			pEntry = RemoveHeadQueue(pQueue);
+			pTxBlk->TotalFrameNum++;
+			pTxBlk->TotalFragNum += RTMP_GET_PACKET_FRAGMENTS(pPacket);	// The real fragment number maybe vary
+			pTxBlk->TotalFrameLen += GET_OS_PKT_LEN(pPacket);
+			pTxBlk->pPacket = pPacket;
+			InsertTailQueue(&pTxBlk->TxPacketList, PACKET_TO_QUEUE_ENTRY(pPacket));
+
+			if (pTxBlk->TxFrameType == TX_RALINK_FRAME || pTxBlk->TxFrameType == TX_AMSDU_FRAME)
+			{
+				// Enhance SW Aggregation Mechanism
+				if (NEED_QUEUE_BACK_FOR_AGG(pAd, QueIdx, FreeNumber[QueIdx], pTxBlk->TxFrameType))
+				{
+					InsertHeadQueue(pQueue, PACKET_TO_QUEUE_ENTRY(pPacket));
+					DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+					break;
+				}
+
+				do{
+					if((pEntry = pQueue->Head) == NULL)
+						break;
+
+					// For TX_AMSDU_FRAME/TX_RALINK_FRAME, Need to check if next pakcet can do aggregation.
+					pPacket = QUEUE_ENTRY_TO_PKT(pEntry);
+					FreeNumber[QueIdx] = GET_TXRING_FREENO(pAd, QueIdx);
+					hasTxDesc = RT28XX_HAS_ENOUGH_FREE_DESC(pAd, pTxBlk, FreeNumber[QueIdx], pPacket);
+					if ((hasTxDesc == FALSE) || (CanDoAggregateTransmit(pAd, pPacket, pTxBlk) == FALSE))
+						break;
+
+					//Remove the packet from the TxSwQueue and insert into pTxBlk
+					pEntry = RemoveHeadQueue(pQueue);
+					ASSERT(pEntry);
+					pPacket = QUEUE_ENTRY_TO_PKT(pEntry);
+					pTxBlk->TotalFrameNum++;
+					pTxBlk->TotalFragNum += RTMP_GET_PACKET_FRAGMENTS(pPacket);	// The real fragment number maybe vary
+					pTxBlk->TotalFrameLen += GET_OS_PKT_LEN(pPacket);
+					InsertTailQueue(&pTxBlk->TxPacketList, PACKET_TO_QUEUE_ENTRY(pPacket));
+				}while(1);
+
+				if (pTxBlk->TxPacketList.Number == 1)
+					pTxBlk->TxFrameType = TX_LEGACY_FRAME;
+			}
+
+#ifdef RT2870
+			DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+#endif // RT2870 //
+
+			Count += pTxBlk->TxPacketList.Number;
+
+				// Do HardTransmit now.
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				Status = STAHardTransmit(pAd, pTxBlk, QueIdx);
+#endif // CONFIG_STA_SUPPORT //
+
+
+#if 0	// We should not break if HardTransmit failed. Well, at least now we should not!
+			if (Status != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_TRACE /*RT_DEBUG_INFO*/,("RTMPHardTransmit return failed!!!\n"));
+				break;
+			}
+#endif
+		}
+
+		RT28XX_STOP_DEQUEUE(pAd, QueIdx, IrqFlags);
+
+#ifdef RT2870
+		if (!hasTxDesc)
+			RTUSBKickBulkOut(pAd);
+#endif // RT2870 //
+
+#ifdef BLOCK_NET_IF
+		if ((pAd->blockQueueTab[QueIdx].SwTxQueueBlockFlag == TRUE)
+			&& (pAd->TxSwQueue[QueIdx].Number < 1))
+		{
+			releaseNetIf(&pAd->blockQueueTab[QueIdx]);
+		}
+#endif // BLOCK_NET_IF //
+
+	}
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calculates the duration which is required to transmit out frames
+	with given size and specified rate.
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		Rate			Transmit rate
+		Size			Frame size in units of byte
+
+	Return Value:
+		Duration number in units of usec
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+USHORT	RTMPCalcDuration(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Rate,
+	IN	ULONG			Size)
+{
+	ULONG	Duration = 0;
+
+	if (Rate < RATE_FIRST_OFDM_RATE) // CCK
+	{
+		if ((Rate > RATE_1) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED))
+			Duration = 96;	// 72+24 preamble+plcp
+		else
+			Duration = 192; // 144+48 preamble+plcp
+
+		Duration += (USHORT)((Size << 4) / RateIdTo500Kbps[Rate]);
+		if ((Size << 4) % RateIdTo500Kbps[Rate])
+			Duration ++;
+	}
+	else if (Rate <= RATE_LAST_OFDM_RATE)// OFDM rates
+	{
+		Duration = 20 + 6;		// 16+4 preamble+plcp + Signal Extension
+		Duration += 4 * (USHORT)((11 + Size * 4) / RateIdTo500Kbps[Rate]);
+		if ((11 + Size * 4) % RateIdTo500Kbps[Rate])
+			Duration += 4;
+	}
+	else	//mimo rate
+	{
+		Duration = 20 + 6;		// 16+4 preamble+plcp + Signal Extension
+	}
+
+	return (USHORT)Duration;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calculates the duration which is required to transmit out frames
+	with given size and specified rate.
+
+	Arguments:
+		pTxWI		Pointer to head of each MPDU to HW.
+		Ack 		Setting for Ack requirement bit
+		Fragment	Setting for Fragment bit
+		RetryMode	Setting for retry mode
+		Ifs 		Setting for IFS gap
+		Rate		Setting for transmit rate
+		Service 	Setting for service
+		Length		Frame length
+		TxPreamble	Short or Long preamble when using CCK rates
+		QueIdx - 0-3, according to 802.11e/d4.4 June/2003
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+    See also : BASmartHardTransmit()    !!!
+
+	========================================================================
+*/
+VOID RTMPWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC 	pOutTxWI,
+	IN	BOOLEAN			FRAG,
+	IN	BOOLEAN			CFACK,
+	IN	BOOLEAN			InsTimestamp,
+	IN	BOOLEAN 		AMPDU,
+	IN	BOOLEAN 		Ack,
+	IN	BOOLEAN 		NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN	UCHAR 			PID,
+	IN	UCHAR			TID,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	*pTransmit)
+{
+	PMAC_TABLE_ENTRY	pMac = NULL;
+	TXWI_STRUC 		TxWI;
+	PTXWI_STRUC 	pTxWI;
+
+	if (WCID < MAX_LEN_OF_MAC_TABLE)
+		pMac = &pAd->MacTab.Content[WCID];
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+	NdisZeroMemory(&TxWI, TXWI_SIZE);
+	pTxWI = &TxWI;
+
+	pTxWI->FRAG= FRAG;
+
+	pTxWI->CFACK = CFACK;
+	pTxWI->TS= InsTimestamp;
+	pTxWI->AMPDU = AMPDU;
+	pTxWI->ACK = Ack;
+	pTxWI->txop= Txopmode;
+
+	pTxWI->NSEQ = NSeq;
+	// John tune the performace with Intel Client in 20 MHz performance
+#ifdef DOT11_N_SUPPORT
+	BASize = pAd->CommonCfg.TxBASize;
+
+	if( BASize >7 )
+		BASize =7;
+	pTxWI->BAWinSize = BASize;
+	pTxWI->ShortGI = pTransmit->field.ShortGI;
+	pTxWI->STBC = pTransmit->field.STBC;
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->WirelessCliID = WCID;
+	pTxWI->MPDUtotalByteCount = Length;
+	pTxWI->PacketId = PID;
+
+	// If CCK or OFDM, BW must be 20
+	pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+#ifdef DOT11N_DRAFT3
+	if (pTxWI->BW)
+		pTxWI->BW = (pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth == 0) ? (BW_20) : (pTransmit->field.BW);
+#endif // DOT11N_DRAFT3 //
+
+	pTxWI->MCS = pTransmit->field.MCS;
+	pTxWI->PHYMODE = pTransmit->field.MODE;
+	pTxWI->CFACK = CfAck;
+
+#ifdef DOT11_N_SUPPORT
+	if (pMac)
+	{
+		if (pAd->CommonCfg.bMIMOPSEnable)
+		{
+			if ((pMac->MmpsMode == MMPS_DYNAMIC) && (pTransmit->field.MCS > 7))
+			{
+				// Dynamic MIMO Power Save Mode
+				pTxWI->MIMOps = 1;
+			}
+			else if (pMac->MmpsMode == MMPS_STATIC)
+			{
+				// Static MIMO Power Save Mode
+				if (pTransmit->field.MODE >= MODE_HTMIX && pTransmit->field.MCS > 7)
+				{
+					pTxWI->MCS = 7;
+					pTxWI->MIMOps = 0;
+				}
+			}
+		}
+		//pTxWI->MIMOps = (pMac->PsMode == PWR_MMPS)? 1:0;
+		if (pMac->bIAmBadAtheros && (pMac->WepStatus != Ndis802_11WEPDisabled))
+		{
+			pTxWI->MpduDensity = 7;
+		}
+		else
+		{
+			pTxWI->MpduDensity = pMac->MpduDensity;
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->PacketId = pTxWI->MCS;
+	NdisMoveMemory(pOutTxWI, &TxWI, sizeof(TXWI_STRUC));
+}
+
+
+VOID RTMPWriteTxWI_Data(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk)
+{
+	HTTRANSMIT_SETTING	*pTransmit;
+	PMAC_TABLE_ENTRY	pMacEntry;
+#ifdef DOT11_N_SUPPORT
+	UCHAR				BASize;
+#endif // DOT11_N_SUPPORT //
+
+
+	ASSERT(pTxWI);
+
+	pTransmit = pTxBlk->pTransmit;
+	pMacEntry = pTxBlk->pMacEntry;
+
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+	NdisZeroMemory(pTxWI, TXWI_SIZE);
+
+	pTxWI->FRAG		= TX_BLK_TEST_FLAG(pTxBlk, fTX_bAllowFrag);
+	pTxWI->ACK		= TX_BLK_TEST_FLAG(pTxBlk, fTX_bAckRequired);
+	pTxWI->txop		= pTxBlk->FrameGap;
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+	if (pMacEntry &&
+		(pAd->StaCfg.BssType == BSS_INFRA) &&
+		(pMacEntry->ValidAsDls == TRUE))
+		pTxWI->WirelessCliID = BSSID_WCID;
+	else
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+		pTxWI->WirelessCliID		= pTxBlk->Wcid;
+
+	pTxWI->MPDUtotalByteCount	= pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+	pTxWI->CFACK				= TX_BLK_TEST_FLAG(pTxBlk, fTX_bPiggyBack);
+
+	// If CCK or OFDM, BW must be 20
+	pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	if (pTxWI->BW)
+		pTxWI->BW = (pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth == 0) ? (BW_20) : (pTransmit->field.BW);
+#endif // DOT11N_DRAFT3 //
+	pTxWI->AMPDU	= ((pTxBlk->TxFrameType == TX_AMPDU_FRAME) ? TRUE : FALSE);
+
+	// John tune the performace with Intel Client in 20 MHz performance
+	BASize = pAd->CommonCfg.TxBASize;
+	if((pTxBlk->TxFrameType == TX_AMPDU_FRAME) && (pMacEntry))
+	{
+		UCHAR		RABAOriIdx = 0;	//The RA's BA Originator table index.
+
+		RABAOriIdx = pTxBlk->pMacEntry->BAOriWcidArray[pTxBlk->UserPriority];
+		BASize = pAd->BATable.BAOriEntry[RABAOriIdx].BAWinSize;
+	}
+
+#if 0 // 3*3
+	if (BASize > 7)
+		BASize = 7;
+#endif
+
+	pTxWI->TxBF = pTransmit->field.TxBF;
+	pTxWI->BAWinSize = BASize;
+	pTxWI->ShortGI = pTransmit->field.ShortGI;
+	pTxWI->STBC = pTransmit->field.STBC;
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->MCS = pTransmit->field.MCS;
+	pTxWI->PHYMODE = pTransmit->field.MODE;
+
+#ifdef DOT11_N_SUPPORT
+	if (pMacEntry)
+	{
+		if ((pMacEntry->MmpsMode == MMPS_DYNAMIC) && (pTransmit->field.MCS > 7))
+		{
+			// Dynamic MIMO Power Save Mode
+			pTxWI->MIMOps = 1;
+		}
+		else if (pMacEntry->MmpsMode == MMPS_STATIC)
+		{
+			// Static MIMO Power Save Mode
+			if (pTransmit->field.MODE >= MODE_HTMIX && pTransmit->field.MCS > 7)
+			{
+				pTxWI->MCS = 7;
+				pTxWI->MIMOps = 0;
+			}
+		}
+
+		if (pMacEntry->bIAmBadAtheros && (pMacEntry->WepStatus != Ndis802_11WEPDisabled))
+		{
+			pTxWI->MpduDensity = 7;
+		}
+		else
+		{
+			pTxWI->MpduDensity = pMacEntry->MpduDensity;
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DBG_DIAGNOSE
+		if (pTxBlk->QueIdx== 0)
+		{
+			pAd->DiagStruct.TxDataCnt[pAd->DiagStruct.ArrayCurIdx]++;
+			pAd->DiagStruct.TxMcsCnt[pAd->DiagStruct.ArrayCurIdx][pTxWI->MCS]++;
+		}
+#endif // DBG_DIAGNOSE //
+
+	// for rate adapation
+	pTxWI->PacketId = pTxWI->MCS;
+}
+
+
+VOID RTMPWriteTxWI_Cache(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk)
+{
+	PHTTRANSMIT_SETTING	/*pTxHTPhyMode,*/ pTransmit;
+	PMAC_TABLE_ENTRY	pMacEntry;
+
+	//
+	// update TXWI
+	//
+	pMacEntry = pTxBlk->pMacEntry;
+	pTransmit = pTxBlk->pTransmit;
+
+	if (pMacEntry->bAutoTxRateSwitch)
+	{
+		pTxWI->txop = IFS_HTTXOP;
+
+		// If CCK or OFDM, BW must be 20
+		pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+		pTxWI->ShortGI = pTransmit->field.ShortGI;
+		pTxWI->STBC = pTransmit->field.STBC;
+
+		pTxWI->MCS = pTransmit->field.MCS;
+		pTxWI->PHYMODE = pTransmit->field.MODE;
+
+		// set PID for TxRateSwitching
+		pTxWI->PacketId = pTransmit->field.MCS;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	pTxWI->AMPDU = ((pMacEntry->NoBADataCountDown == 0) ? TRUE: FALSE);
+	pTxWI->MIMOps = 0;
+
+#ifdef DOT11N_DRAFT3
+	if (pTxWI->BW)
+		pTxWI->BW = (pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth == 0) ? (BW_20) : (pTransmit->field.BW);
+#endif // DOT11N_DRAFT3 //
+
+	if (pAd->CommonCfg.bMIMOPSEnable)
+	{
+		// MIMO Power Save Mode
+		if ((pMacEntry->MmpsMode == MMPS_DYNAMIC) && (pTransmit->field.MCS > 7))
+		{
+			// Dynamic MIMO Power Save Mode
+			pTxWI->MIMOps = 1;
+		}
+		else if (pMacEntry->MmpsMode == MMPS_STATIC)
+		{
+			// Static MIMO Power Save Mode
+			if ((pTransmit->field.MODE >= MODE_HTMIX) && (pTransmit->field.MCS > 7))
+			{
+				pTxWI->MCS = 7;
+				pTxWI->MIMOps = 0;
+			}
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DBG_DIAGNOSE
+	if (pTxBlk->QueIdx== 0)
+	{
+		pAd->DiagStruct.TxDataCnt[pAd->DiagStruct.ArrayCurIdx]++;
+		pAd->DiagStruct.TxMcsCnt[pAd->DiagStruct.ArrayCurIdx][pTxWI->MCS]++;
+	}
+#endif // DBG_DIAGNOSE //
+
+	pTxWI->MPDUtotalByteCount = pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calculates the duration which is required to transmit out frames
+	with given size and specified rate.
+
+	Arguments:
+		pTxD		Pointer to transmit descriptor
+		Ack 		Setting for Ack requirement bit
+		Fragment	Setting for Fragment bit
+		RetryMode	Setting for retry mode
+		Ifs 		Setting for IFS gap
+		Rate		Setting for transmit rate
+		Service 	Setting for service
+		Length		Frame length
+		TxPreamble	Short or Long preamble when using CCK rates
+		QueIdx - 0-3, according to 802.11e/d4.4 June/2003
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID RTMPWriteTxDescriptor(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXD_STRUC		pTxD,
+	IN	BOOLEAN 		bWIV,
+	IN	UCHAR			QueueSEL)
+{
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+
+	pTxD->WIV	= (bWIV) ? 1: 0;
+	pTxD->QSEL= (QueueSEL);
+	//RT2860c??  fixed using EDCA queue for test...  We doubt Queue1 has problem.  2006-09-26 Jan
+	//pTxD->QSEL= FIFO_EDCA;
+	if (pAd->bGenOneHCCA == TRUE)
+		pTxD->QSEL= FIFO_HCCA;
+	pTxD->DMADONE = 0;
+}
+
+
+// should be called only when -
+// 1. MEADIA_CONNECTED
+// 2. AGGREGATION_IN_USED
+// 3. Fragmentation not in used
+// 4. either no previous frame (pPrevAddr1=NULL) .OR. previoud frame is aggregatible
+BOOLEAN TxFrameIsAggregatible(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pPrevAddr1,
+	IN	PUCHAR			p8023hdr)
+{
+
+	// can't aggregate EAPOL (802.1x) frame
+	if ((p8023hdr[12] == 0x88) && (p8023hdr[13] == 0x8e))
+		return FALSE;
+
+	// can't aggregate multicast/broadcast frame
+	if (p8023hdr[0] & 0x01)
+		return FALSE;
+
+	if (INFRA_ON(pAd)) // must be unicast to AP
+		return TRUE;
+	else if ((pPrevAddr1 == NULL) || MAC_ADDR_EQUAL(pPrevAddr1, p8023hdr)) // unicast to same STA
+		return TRUE;
+	else
+		return FALSE;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+	   Check the MSDU Aggregation policy
+	1.HT aggregation is A-MSDU
+	2.legaacy rate aggregation is software aggregation by Ralink.
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN PeerIsAggreOn(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ULONG		   TxRate,
+	IN	PMAC_TABLE_ENTRY pMacEntry)
+{
+	ULONG	AFlags = (fCLIENT_STATUS_AMSDU_INUSED | fCLIENT_STATUS_AGGREGATION_CAPABLE);
+
+	if (pMacEntry != NULL && CLIENT_STATUS_TEST_FLAG(pMacEntry, AFlags))
+	{
+#ifdef DOT11_N_SUPPORT
+		if (pMacEntry->HTPhyMode.field.MODE >= MODE_HTMIX)
+		{
+			return TRUE;
+		}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef AGGREGATION_SUPPORT
+		if (TxRate >= RATE_6 && pAd->CommonCfg.bAggregationCapable && (!(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) && CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_WMM_CAPABLE))))
+		{	// legacy  Ralink Aggregation support
+			return TRUE;
+		}
+#endif // AGGREGATION_SUPPORT //
+	}
+
+	return FALSE;
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Check and fine the packet waiting in SW queue with highest priority
+
+	Arguments:
+		pAd Pointer to our adapter
+
+	Return Value:
+		pQueue		Pointer to Waiting Queue
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+PQUEUE_HEADER	RTMPCheckTxSwQueue(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT PUCHAR			pQueIdx)
+{
+
+	ULONG	Number;
+	// 2004-11-15 to be removed. test aggregation only
+//	if ((OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED)) && (*pNumber < 2))
+//		 return NULL;
+
+	Number = pAd->TxSwQueue[QID_AC_BK].Number
+			 + pAd->TxSwQueue[QID_AC_BE].Number
+			 + pAd->TxSwQueue[QID_AC_VI].Number
+			 + pAd->TxSwQueue[QID_AC_VO].Number
+			 + pAd->TxSwQueue[QID_HCCA].Number;
+
+	if (pAd->TxSwQueue[QID_AC_VO].Head != NULL)
+	{
+		*pQueIdx = QID_AC_VO;
+		return (&pAd->TxSwQueue[QID_AC_VO]);
+	}
+	else if (pAd->TxSwQueue[QID_AC_VI].Head != NULL)
+	{
+		*pQueIdx = QID_AC_VI;
+		return (&pAd->TxSwQueue[QID_AC_VI]);
+	}
+	else if (pAd->TxSwQueue[QID_AC_BE].Head != NULL)
+	{
+		*pQueIdx = QID_AC_BE;
+		return (&pAd->TxSwQueue[QID_AC_BE]);
+	}
+	else if (pAd->TxSwQueue[QID_AC_BK].Head != NULL)
+	{
+		*pQueIdx = QID_AC_BK;
+		return (&pAd->TxSwQueue[QID_AC_BK]);
+	}
+	else if (pAd->TxSwQueue[QID_HCCA].Head != NULL)
+	{
+		*pQueIdx = QID_HCCA;
+		return (&pAd->TxSwQueue[QID_HCCA]);
+	}
+
+	// No packet pending in Tx Sw queue
+	*pQueIdx = QID_AC_BK;
+
+	return (NULL);
+}
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Suspend MSDU transmission
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPSuspendMsduTransmission(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE,("SCANNING, suspend MSDU transmission ...\n"));
+
+
+	//
+	// Before BSS_SCAN_IN_PROGRESS, we need to keep Current R66 value and
+	// use Lowbound as R66 value on ScanNextChannel(...)
+	//
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &pAd->BbpTuning.R66CurrentValue);
+
+	// set BBP_R66 to 0x30/0x40 when scanning (AsicSwitchChannel will set R66 according to channel when scanning)
+	//RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, (0x26 + GET_LNA_GAIN(pAd)));
+	RTMPSetAGCInitValue(pAd, BW_20);
+
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+	//RTMP_IO_WRITE32(pAd, TX_CNTL_CSR, 0x000f0000);		// abort all TX rings
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Resume MSDU transmission
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPResumeMsduTransmission(
+	IN	PRTMP_ADAPTER	pAd)
+{
+//    UCHAR			IrqState;
+
+	DBGPRINT(RT_DEBUG_TRACE,("SCAN done, resume MSDU transmission ...\n"));
+
+
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, pAd->BbpTuning.R66CurrentValue);
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+// sample, for IRQ LOCK to SEM LOCK
+//    IrqState = pAd->irq_disabled;
+//	if (IrqState)
+//		RTMPDeQueuePacket(pAd, TRUE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+//    else
+	RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+}
+
+
+UINT deaggregate_AMSDU_announce(
+	IN	PRTMP_ADAPTER	pAd,
+	PNDIS_PACKET		pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize)
+{
+	USHORT 			PayloadSize;
+	USHORT 			SubFrameSize;
+	PHEADER_802_3 	pAMSDUsubheader;
+	UINT			nMSDU;
+    UCHAR			Header802_3[14];
+
+	PUCHAR			pPayload, pDA, pSA, pRemovedLLCSNAP;
+	PNDIS_PACKET	pClonePacket;
+
+
+
+	nMSDU = 0;
+
+	while (DataSize > LENGTH_802_3)
+	{
+
+		nMSDU++;
+
+		//hex_dump("subheader", pData, 64);
+		pAMSDUsubheader = (PHEADER_802_3)pData;
+		//pData += LENGTH_802_3;
+		PayloadSize = pAMSDUsubheader->Octet[1] + (pAMSDUsubheader->Octet[0]<<8);
+		SubFrameSize = PayloadSize + LENGTH_802_3;
+
+
+		if ((DataSize < SubFrameSize) || (PayloadSize > 1518 ))
+		{
+			break;
+		}
+
+		//printk("%d subframe: Size = %d\n",  nMSDU, PayloadSize);
+
+		pPayload = pData + LENGTH_802_3;
+		pDA = pData;
+		pSA = pData + MAC_ADDR_LEN;
+
+		// convert to 802.3 header
+        CONVERT_TO_802_3(Header802_3, pDA, pSA, pPayload, PayloadSize, pRemovedLLCSNAP);
+
+#ifdef CONFIG_STA_SUPPORT
+		if ((Header802_3[12] == 0x88) && (Header802_3[13] == 0x8E) )
+		{
+		    // avoid local heap overflow, use dyanamic allocation
+		   MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+		   memmove(Elem->Msg+(LENGTH_802_11 + LENGTH_802_1_H), pPayload, PayloadSize);
+		   Elem->MsgLen = LENGTH_802_11 + LENGTH_802_1_H + PayloadSize;
+		   WpaEAPOLKeyAction(pAd, Elem);
+		   kfree(Elem);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+	        	if (pRemovedLLCSNAP)
+	        	{
+	    			pPayload -= LENGTH_802_3;
+	    			PayloadSize += LENGTH_802_3;
+	    			NdisMoveMemory(pPayload, &Header802_3[0], LENGTH_802_3);
+	        	}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		pClonePacket = ClonePacket(pAd, pPacket, pPayload, PayloadSize);
+		if (pClonePacket)
+		{
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pClonePacket, RTMP_GET_PACKET_IF(pPacket));
+#endif // CONFIG_STA_SUPPORT //
+		}
+
+
+		// A-MSDU has padding to multiple of 4 including subframe header.
+		// align SubFrameSize up to multiple of 4
+		SubFrameSize = (SubFrameSize+3)&(~0x3);
+
+
+		if (SubFrameSize > 1528 || SubFrameSize < 32)
+		{
+			break;
+		}
+
+		if (DataSize > SubFrameSize)
+		{
+			pData += SubFrameSize;
+			DataSize -= SubFrameSize;
+		}
+		else
+		{
+			// end of A-MSDU
+			DataSize = 0;
+		}
+	}
+
+	// finally release original rx packet
+	RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+
+	return nMSDU;
+}
+
+
+UINT BA_Reorder_AMSDU_Annnounce(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PUCHAR			pData;
+	USHORT			DataSize;
+	UINT			nMSDU = 0;
+
+	pData = (PUCHAR) GET_OS_PKT_DATAPTR(pPacket);
+	DataSize = (USHORT) GET_OS_PKT_LEN(pPacket);
+
+	nMSDU = deaggregate_AMSDU_announce(pAd, pPacket, pData, DataSize);
+
+	return nMSDU;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Look up the MAC address in the MAC table. Return NULL if not found.
+	Return:
+		pEntry - pointer to the MAC entry; NULL is not found
+	==========================================================================
+*/
+MAC_TABLE_ENTRY *MacTableLookup(
+	IN PRTMP_ADAPTER pAd,
+	PUCHAR pAddr)
+{
+	ULONG HashIdx;
+	MAC_TABLE_ENTRY *pEntry = NULL;
+
+	HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+	pEntry = pAd->MacTab.Hash[HashIdx];
+
+	while (pEntry && (pEntry->ValidAsCLI || pEntry->ValidAsWDS || pEntry->ValidAsApCli || pEntry->ValidAsMesh))
+	{
+		if (MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
+		{
+			break;
+		}
+		else
+			pEntry = pEntry->pNext;
+	}
+
+	return pEntry;
+}
+
+MAC_TABLE_ENTRY *MacTableInsertEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR			pAddr,
+	IN	UCHAR			apidx,
+	IN BOOLEAN	CleanAll)
+{
+	UCHAR HashIdx;
+	int i, FirstWcid;
+	MAC_TABLE_ENTRY *pEntry = NULL, *pCurrEntry;
+
+	// if FULL, return
+	if (pAd->MacTab.Size >= MAX_LEN_OF_MAC_TABLE)
+		return NULL;
+
+	FirstWcid = 1;
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	if (pAd->StaCfg.BssType == BSS_INFRA)
+		FirstWcid = 2;
+#endif // CONFIG_STA_SUPPORT //
+
+	// allocate one MAC entry
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+	for (i = FirstWcid; i< MAX_LEN_OF_MAC_TABLE; i++)   // skip entry#0 so that "entry index == AID" for fast lookup
+	{
+		// pick up the first available vacancy
+		if ((pAd->MacTab.Content[i].ValidAsCLI == FALSE) &&
+			(pAd->MacTab.Content[i].ValidAsWDS == FALSE) &&
+			(pAd->MacTab.Content[i].ValidAsApCli== FALSE) &&
+			(pAd->MacTab.Content[i].ValidAsMesh == FALSE)
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			&& (pAd->MacTab.Content[i].ValidAsDls == FALSE)
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+			)
+		{
+			pEntry = &pAd->MacTab.Content[i];
+			if (CleanAll == TRUE)
+			{
+				pEntry->MaxSupportedRate = RATE_11;
+				pEntry->CurrTxRate = RATE_11;
+				NdisZeroMemory(pEntry, sizeof(MAC_TABLE_ENTRY));
+				pEntry->PairwiseKey.KeyLen = 0;
+				pEntry->PairwiseKey.CipherAlg = CIPHER_NONE;
+			}
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			if (apidx >= MIN_NET_DEVICE_FOR_DLS)
+			{
+				pEntry->ValidAsCLI = FALSE;
+				pEntry->ValidAsWDS = FALSE;
+				pEntry->ValidAsApCli = FALSE;
+				pEntry->ValidAsMesh = FALSE;
+				pEntry->ValidAsDls = TRUE;
+				pEntry->isCached = FALSE;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+			{
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					pEntry->ValidAsCLI = TRUE;
+					pEntry->ValidAsWDS = FALSE;
+					pEntry->ValidAsApCli = FALSE;
+					pEntry->ValidAsMesh = FALSE;
+					pEntry->ValidAsDls = FALSE;
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+
+			pEntry->bIAmBadAtheros = FALSE;
+			pEntry->pAd = pAd;
+			pEntry->CMTimerRunning = FALSE;
+			pEntry->EnqueueEapolStartTimerRunning = EAPOL_START_DISABLE;
+			pEntry->RSNIE_Len = 0;
+			NdisZeroMemory(pEntry->R_Counter, sizeof(pEntry->R_Counter));
+			pEntry->ReTryCounter = PEER_MSG1_RETRY_TIMER_CTR;
+
+			if (pEntry->ValidAsMesh)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_MESH);
+			else if (pEntry->ValidAsApCli)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_APCLI);
+			else if (pEntry->ValidAsWDS)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_WDS);
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			else if (pEntry->ValidAsDls)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_DLS);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+			else
+				pEntry->apidx = apidx;
+
+			{
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					pEntry->AuthMode = pAd->StaCfg.AuthMode;
+					pEntry->WepStatus = pAd->StaCfg.WepStatus;
+					pEntry->PrivacyFilter = Ndis802_11PrivFilterAcceptAll;
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+
+			pEntry->GTKState = REKEY_NEGOTIATING;
+			pEntry->PairwiseKey.KeyLen = 0;
+			pEntry->PairwiseKey.CipherAlg = CIPHER_NONE;
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			if (pEntry->ValidAsDls == TRUE)
+				pEntry->PortSecured = WPA_802_1X_PORT_SECURED;
+#endif //QOS_DLS_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+			pEntry->PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			pEntry->PMKID_CacheIdx = ENTRY_NOT_FOUND;
+			COPY_MAC_ADDR(pEntry->Addr, pAddr);
+			pEntry->Sst = SST_NOT_AUTH;
+			pEntry->AuthState = AS_NOT_AUTH;
+			pEntry->Aid = (USHORT)i;  //0;
+			pEntry->CapabilityInfo = 0;
+			pEntry->PsMode = PWR_ACTIVE;
+			pEntry->PsQIdleCount = 0;
+			pEntry->NoDataIdleCount = 0;
+			pEntry->ContinueTxFailCnt = 0;
+			InitializeQueueHeader(&pEntry->PsQueue);
+
+
+			pAd->MacTab.Size ++;
+			// Add this entry into ASIC RX WCID search table
+			RT28XX_STA_ENTRY_ADD(pAd, pEntry);
+
+			DBGPRINT(RT_DEBUG_TRACE, ("MacTableInsertEntry - allocate entry #%d, Total= %d\n",i, pAd->MacTab.Size));
+			break;
+		}
+	}
+
+	// add this MAC entry into HASH table
+	if (pEntry)
+	{
+		HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+		if (pAd->MacTab.Hash[HashIdx] == NULL)
+		{
+			pAd->MacTab.Hash[HashIdx] = pEntry;
+		}
+		else
+		{
+			pCurrEntry = pAd->MacTab.Hash[HashIdx];
+			while (pCurrEntry->pNext != NULL)
+				pCurrEntry = pCurrEntry->pNext;
+			pCurrEntry->pNext = pEntry;
+		}
+	}
+
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+	return pEntry;
+}
+
+/*
+	==========================================================================
+	Description:
+		Delete a specified client from MAC table
+	==========================================================================
+ */
+BOOLEAN MacTableDeleteEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT wcid,
+	IN PUCHAR pAddr)
+{
+	USHORT HashIdx;
+	MAC_TABLE_ENTRY *pEntry, *pPrevEntry, *pProbeEntry;
+	BOOLEAN Cancelled;
+	//USHORT	offset;	// unused variable
+	//UCHAR	j;			// unused variable
+
+	if (wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+
+	HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+	//pEntry = pAd->MacTab.Hash[HashIdx];
+	pEntry = &pAd->MacTab.Content[wcid];
+
+	if (pEntry && (pEntry->ValidAsCLI || pEntry->ValidAsApCli || pEntry->ValidAsWDS || pEntry->ValidAsMesh
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+ 		|| pEntry->ValidAsDls
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+		))
+	{
+		if (MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
+		{
+
+			// Delete this entry from ASIC on-chip WCID Table
+			RT28XX_STA_ENTRY_MAC_RESET(pAd, wcid);
+
+#ifdef DOT11_N_SUPPORT
+			// free resources of BA
+			BASessionTearDownALL(pAd, pEntry->Aid);
+#endif // DOT11_N_SUPPORT //
+
+
+			pPrevEntry = NULL;
+			pProbeEntry = pAd->MacTab.Hash[HashIdx];
+			ASSERT(pProbeEntry);
+
+			// update Hash list
+			do
+			{
+				if (pProbeEntry == pEntry)
+				{
+					if (pPrevEntry == NULL)
+					{
+						pAd->MacTab.Hash[HashIdx] = pEntry->pNext;
+					}
+					else
+					{
+						pPrevEntry->pNext = pEntry->pNext;
+					}
+					break;
+				}
+
+				pPrevEntry = pProbeEntry;
+				pProbeEntry = pProbeEntry->pNext;
+			} while (pProbeEntry);
+
+			// not found !!!
+			ASSERT(pProbeEntry != NULL);
+
+			RT28XX_STA_ENTRY_KEY_DEL(pAd, BSS0, wcid);
+
+
+		if (pEntry->EnqueueEapolStartTimerRunning != EAPOL_START_DISABLE)
+		{
+			RTMPCancelTimer(&pEntry->EnqueueStartForPSKTimer, &Cancelled);
+			pEntry->EnqueueEapolStartTimerRunning = EAPOL_START_DISABLE;
+		}
+
+
+   			NdisZeroMemory(pEntry, sizeof(MAC_TABLE_ENTRY));
+			pAd->MacTab.Size --;
+			DBGPRINT(RT_DEBUG_TRACE, ("MacTableDeleteEntry1 - Total= %d\n", pAd->MacTab.Size));
+		}
+		else
+		{
+			printk("\n%s: Impossible Wcid = %d !!!!!\n", __func__, wcid);
+		}
+	}
+
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+	//Reset operating mode when no Sta.
+	if (pAd->MacTab.Size == 0)
+	{
+#ifdef DOT11_N_SUPPORT
+		pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode = 0;
+#endif // DOT11_N_SUPPORT //
+		AsicUpdateProtect(pAd, 0 /*pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode*/, (ALLN_SETPROTECT), TRUE, 0 /*pAd->MacTab.fAnyStationNonGF*/);
+	}
+
+	return TRUE;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		This routine reset the entire MAC table. All packets pending in
+		the power-saving queues are freed here.
+	==========================================================================
+ */
+VOID MacTableReset(
+	IN  PRTMP_ADAPTER  pAd)
+{
+	int         i;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("MacTableReset\n"));
+	//NdisAcquireSpinLock(&pAd->MacTabLock);
+
+	for (i=1; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		if (pAd->MacTab.Content[i].ValidAsCLI == TRUE)
+	   {
+
+#ifdef DOT11_N_SUPPORT
+			// free resources of BA
+			BASessionTearDownALL(pAd, i);
+#endif // DOT11_N_SUPPORT //
+
+			pAd->MacTab.Content[i].ValidAsCLI = FALSE;
+
+
+
+#ifdef RT2870
+			NdisZeroMemory(pAd->MacTab.Content[i].Addr, 6);
+			RT28XX_STA_ENTRY_MAC_RESET(pAd, i);
+#endif // RT2870 //
+
+			//AsicDelWcidTab(pAd, i);
+		}
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID AssocParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_ASSOC_REQ_STRUCT *AssocReq,
+	IN PUCHAR                     pAddr,
+	IN USHORT                     CapabilityInfo,
+	IN ULONG                      Timeout,
+	IN USHORT                     ListenIntv)
+{
+	COPY_MAC_ADDR(AssocReq->Addr, pAddr);
+	// Add mask to support 802.11b mode only
+	AssocReq->CapabilityInfo = CapabilityInfo & SUPPORTED_CAPABILITY_INFO; // not cf-pollable, not cf-poll-request
+	AssocReq->Timeout = Timeout;
+	AssocReq->ListenIntv = ListenIntv;
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID DisassocParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_DISASSOC_REQ_STRUCT *DisassocReq,
+	IN PUCHAR pAddr,
+	IN USHORT Reason)
+{
+	COPY_MAC_ADDR(DisassocReq->Addr, pAddr);
+	DisassocReq->Reason = Reason;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Check the out going frame, if this is an DHCP or ARP datagram
+	will be duplicate another frame at low data rate transmit.
+
+	Arguments:
+		pAd 		Pointer to our adapter
+		pPacket 	Pointer to outgoing Ndis frame
+
+	Return Value:
+		TRUE		To be duplicate at Low data rate transmit. (1mb)
+		FALSE		Do nothing.
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+		MAC header + IP Header + UDP Header
+		  14 Bytes	  20 Bytes
+
+		UDP Header
+		00|01|02|03|04|05|06|07|08|09|10|11|12|13|14|15|
+						Source Port
+		16|17|18|19|20|21|22|23|24|25|26|27|28|29|30|31|
+					Destination Port
+
+		port 0x43 means Bootstrap Protocol, server.
+		Port 0x44 means Bootstrap Protocol, client.
+
+	========================================================================
+*/
+
+BOOLEAN RTMPCheckDHCPFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	ULONG			NumberOfBytesRead = 0;
+	ULONG			CurrentOffset = 0;
+	PVOID			pVirtualAddress = NULL;
+	UINT			NdisBufferLength;
+	PUCHAR			pSrc;
+	USHORT			Protocol;
+	UCHAR			ByteOffset36 = 0;
+	UCHAR			ByteOffset38 = 0;
+	BOOLEAN 		ReadFirstParm = TRUE;
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, (PUCHAR *)&pVirtualAddress, &NdisBufferLength);
+
+	NumberOfBytesRead += NdisBufferLength;
+	pSrc = (PUCHAR) pVirtualAddress;
+	Protocol = *(pSrc + 12) * 256 + *(pSrc + 13);
+
+	//
+	// Check DHCP & BOOTP protocol
+	//
+	while (NumberOfBytesRead <= PacketInfo.TotalPacketLength)
+	{
+		if ((NumberOfBytesRead >= 35) && (ReadFirstParm == TRUE))
+		{
+			CurrentOffset = 35 - (NumberOfBytesRead - NdisBufferLength);
+			ByteOffset36 = *(pSrc + CurrentOffset);
+			ReadFirstParm = FALSE;
+		}
+
+		if (NumberOfBytesRead >= 37)
+		{
+			CurrentOffset = 37 - (NumberOfBytesRead - NdisBufferLength);
+			ByteOffset38 = *(pSrc + CurrentOffset);
+			//End of Read
+			break;
+		}
+		return FALSE;
+	}
+
+	// Check for DHCP & BOOTP protocol
+	if ((ByteOffset36 != 0x44) || (ByteOffset38 != 0x43))
+		{
+		//
+		// 2054 (hex 0806) for ARP datagrams
+		// if this packet is not ARP datagrams, then do nothing
+		// ARP datagrams will also be duplicate at 1mb broadcast frames
+		//
+		if (Protocol != 0x0806 )
+			return FALSE;
+		}
+
+	return TRUE;
+}
+
+
+BOOLEAN RTMPCheckEtherType(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	USHORT	TypeLen;
+	UCHAR	Byte0, Byte1;
+	PUCHAR	pSrcBuf;
+	UINT32	pktLen;
+	UINT16 	srcPort, dstPort;
+	BOOLEAN	status = TRUE;
+
+
+	pSrcBuf = GET_OS_PKT_DATAPTR(pPacket);
+	pktLen = GET_OS_PKT_LEN(pPacket);
+
+	ASSERT(pSrcBuf);
+
+	RTMP_SET_PACKET_SPECIFIC(pPacket, 0);
+
+	// get Ethernet protocol field
+	TypeLen = (pSrcBuf[12] << 8) + pSrcBuf[13];
+
+	pSrcBuf += LENGTH_802_3;	// Skip the Ethernet Header.
+
+	if (TypeLen <= 1500)
+	{	// 802.3, 802.3 LLC
+		/*
+			DestMAC(6) + SrcMAC(6) + Lenght(2) +
+			DSAP(1) + SSAP(1) + Control(1) +
+			if the DSAP = 0xAA, SSAP=0xAA, Contorl = 0x03, it has a 5-bytes SNAP header.
+				=> + SNAP (5, OriginationID(3) + etherType(2))
+		*/
+		if (pSrcBuf[0] == 0xAA && pSrcBuf[1] == 0xAA && pSrcBuf[2] == 0x03)
+		{
+			Sniff2BytesFromNdisBuffer(pSrcBuf, 6, &Byte0, &Byte1);
+			RTMP_SET_PACKET_LLCSNAP(pPacket, 1);
+			TypeLen = (USHORT)((Byte0 << 8) + Byte1);
+			pSrcBuf += 8; // Skip this LLC/SNAP header
+		}
+		else
+		{
+			//It just has 3-byte LLC header, maybe a legacy ether type frame. we didn't handle it.
+		}
+	}
+
+	// If it's a VLAN packet, get the real Type/Length field.
+	if (TypeLen == 0x8100)
+	{
+		/* 0x8100 means VLAN packets */
+
+		/* Dest. MAC Address (6-bytes) +
+		   Source MAC Address (6-bytes) +
+		   Length/Type = 802.1Q Tag Type (2-byte) +
+		   Tag Control Information (2-bytes) +
+		   Length / Type (2-bytes) +
+		   data payload (0-n bytes) +
+		   Pad (0-p bytes) +
+		   Frame Check Sequence (4-bytes) */
+
+		RTMP_SET_PACKET_VLAN(pPacket, 1);
+		Sniff2BytesFromNdisBuffer(pSrcBuf, 2, &Byte0, &Byte1);
+		TypeLen = (USHORT)((Byte0 << 8) + Byte1);
+
+		pSrcBuf += 4; // Skip the VLAN Header.
+	}
+
+	switch (TypeLen)
+	{
+		case 0x0800:
+			{
+				ASSERT((pktLen > 34));
+				if (*(pSrcBuf + 9) == 0x11)
+				{	// udp packet
+					ASSERT((pktLen > 34));	// 14 for ethernet header, 20 for IP header
+
+					pSrcBuf += 20;	// Skip the IP header
+					srcPort = OS_NTOHS(*((UINT16 *)pSrcBuf));
+					dstPort = OS_NTOHS(*((UINT16 *)(pSrcBuf +2)));
+
+					if ((srcPort==0x44 && dstPort==0x43) || (srcPort==0x43 && dstPort==0x44))
+					{	//It's a BOOTP/DHCP packet
+						RTMP_SET_PACKET_DHCP(pPacket, 1);
+					}
+				}
+			}
+			break;
+		case 0x0806:
+			{
+				//ARP Packet.
+				RTMP_SET_PACKET_DHCP(pPacket, 1);
+			}
+			break;
+		case 0x888e:
+			{
+				// EAPOL Packet.
+				RTMP_SET_PACKET_EAPOL(pPacket, 1);
+			}
+			break;
+		default:
+			status = FALSE;
+			break;
+	}
+
+	return status;
+
+}
+
+
+
+VOID Update_Rssi_Sample(
+	IN PRTMP_ADAPTER	pAd,
+	IN RSSI_SAMPLE		*pRssi,
+	IN PRXWI_STRUC		pRxWI)
+		{
+	CHAR	rssi0 = pRxWI->RSSI0;
+	CHAR	rssi1 = pRxWI->RSSI1;
+	CHAR	rssi2 = pRxWI->RSSI2;
+
+	if (rssi0 != 0)
+	{
+		pRssi->LastRssi0	= ConvertToRssi(pAd, (CHAR)rssi0, RSSI_0);
+		pRssi->AvgRssi0X8	= (pRssi->AvgRssi0X8 - pRssi->AvgRssi0) + pRssi->LastRssi0;
+		pRssi->AvgRssi0	= pRssi->AvgRssi0X8 >> 3;
+	}
+
+	if (rssi1 != 0)
+	{
+		pRssi->LastRssi1	= ConvertToRssi(pAd, (CHAR)rssi1, RSSI_1);
+		pRssi->AvgRssi1X8	= (pRssi->AvgRssi1X8 - pRssi->AvgRssi1) + pRssi->LastRssi1;
+		pRssi->AvgRssi1	= pRssi->AvgRssi1X8 >> 3;
+	}
+
+	if (rssi2 != 0)
+	{
+		pRssi->LastRssi2	= ConvertToRssi(pAd, (CHAR)rssi2, RSSI_2);
+		pRssi->AvgRssi2X8  = (pRssi->AvgRssi2X8 - pRssi->AvgRssi2) + pRssi->LastRssi2;
+		pRssi->AvgRssi2 = pRssi->AvgRssi2X8 >> 3;
+	}
+}
+
+
+
+// Normal legacy Rx packet indication
+VOID Indicate_Legacy_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+	UCHAR			Header802_3[LENGTH_802_3];
+
+	// 1. get 802.3 Header
+	// 2. remove LLC
+	// 		a. pointer pRxBlk->pData to payload
+	//      b. modify pRxBlk->DataSize
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(pRxBlk, Header802_3);
+#endif // CONFIG_STA_SUPPORT //
+
+	if (pRxBlk->DataSize > MAX_RX_PKT_LEN)
+	{
+#if 0 // sample take off, for multiple card design
+		static int err_size;
+
+		err_size++;
+		if (err_size > 20)
+		{
+			 printk("Legacy DataSize = %d\n", pRxBlk->DataSize);
+			 hex_dump("802.3 Header", Header802_3, LENGTH_802_3);
+			 hex_dump("Payload", pRxBlk->pData, 64);
+			 err_size = 0;
+		}
+#endif
+
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+
+	STATS_INC_RX_PACKETS(pAd, FromWhichBSSID);
+
+#ifdef RT2870
+#ifdef DOT11_N_SUPPORT
+	if (pAd->CommonCfg.bDisableReordering == 0)
+	{
+		PBA_REC_ENTRY		pBAEntry;
+		ULONG				Now32;
+		UCHAR				Wcid = pRxBlk->pRxWI->WirelessCliID;
+		UCHAR				TID = pRxBlk->pRxWI->TID;
+		USHORT				Idx;
+
+#define REORDERING_PACKET_TIMEOUT		((100 * HZ)/1000)	// system ticks -- 100 ms
+
+		if (Wcid < MAX_LEN_OF_MAC_TABLE)
+		{
+			Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+			if (Idx != 0)
+			{
+				pBAEntry = &pAd->BATable.BARecEntry[Idx];
+				// update last rx time
+				NdisGetSystemUpTime(&Now32);
+				if ((pBAEntry->list.qlen > 0) &&
+					 RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(REORDERING_PACKET_TIMEOUT)))
+	   				)
+				{
+					printk("Indicate_Legacy_Packet():flush reordering_timeout_mpdus! RxWI->Flags=%d, pRxWI.TID=%d, RxD->AMPDU=%d!\n", pRxBlk->Flags, pRxBlk->pRxWI->TID, pRxBlk->RxD.AMPDU);
+					hex_dump("Dump the legacy Packet:", GET_OS_PKT_DATAPTR(pRxBlk->pRxPacket), 64);
+					ba_flush_reordering_timeout_mpdus(pAd, pBAEntry, Now32);
+				}
+			}
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+#endif // RT2870 //
+
+	wlan_802_11_to_802_3_packet(pAd, pRxBlk, Header802_3, FromWhichBSSID);
+
+	//
+	// pass this 802.3 packet to upper layer or forward this packet to WM directly
+	//
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pRxPacket, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+
+// Normal, AMPDU or AMSDU
+VOID CmmRxnonRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+#ifdef DOT11_N_SUPPORT
+	if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMPDU) && (pAd->CommonCfg.bDisableReordering == 0))
+	{
+		Indicate_AMPDU_Packet(pAd, pRxBlk, FromWhichBSSID);
+	}
+	else
+#endif // DOT11_N_SUPPORT //
+	{
+#ifdef DOT11_N_SUPPORT
+		if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU))
+		{
+			// handle A-MSDU
+			Indicate_AMSDU_Packet(pAd, pRxBlk, FromWhichBSSID);
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
+		}
+	}
+}
+
+
+VOID CmmRxRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	UCHAR			Header802_3[LENGTH_802_3];
+	UINT16			Msdu2Size;
+	UINT16 			Payload1Size, Payload2Size;
+	PUCHAR 			pData2;
+	PNDIS_PACKET	pPacket2 = NULL;
+
+
+
+	Msdu2Size = *(pRxBlk->pData) + (*(pRxBlk->pData+1) << 8);
+
+	if ((Msdu2Size <= 1536) && (Msdu2Size < pRxBlk->DataSize))
+	{
+		/* skip two byte MSDU2 len */
+		pRxBlk->pData += 2;
+		pRxBlk->DataSize -= 2;
+	}
+	else
+	{
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	// get 802.3 Header and  remove LLC
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(pRxBlk, Header802_3);
+#endif // CONFIG_STA_SUPPORT //
+
+
+	ASSERT(pRxBlk->pRxPacket);
+
+	// Ralink Aggregation frame
+	pAd->RalinkCounters.OneSecRxAggregationCount ++;
+	Payload1Size = pRxBlk->DataSize - Msdu2Size;
+	Payload2Size = Msdu2Size - LENGTH_802_3;
+
+	pData2 = pRxBlk->pData + Payload1Size + LENGTH_802_3;
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		pPacket2 = duplicate_pkt(pAd, (pData2-LENGTH_802_3), LENGTH_802_3, pData2, Payload2Size, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+
+	if (!pPacket2)
+	{
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	// update payload size of 1st packet
+	pRxBlk->DataSize = Payload1Size;
+	wlan_802_11_to_802_3_packet(pAd, pRxBlk, Header802_3, FromWhichBSSID);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pRxBlk->pRxPacket, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+
+	if (pPacket2)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pPacket2, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+	}
+}
+
+
+#define RESET_FRAGFRAME(_fragFrame) \
+	{								\
+		_fragFrame.RxSize = 0;		\
+		_fragFrame.Sequence = 0;	\
+		_fragFrame.LastFrag = 0;	\
+		_fragFrame.Flags = 0;		\
+	}
+
+
+PNDIS_PACKET RTMPDeFragmentDataFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+	UCHAR			*pData = pRxBlk->pData;
+	USHORT			DataSize = pRxBlk->DataSize;
+	PNDIS_PACKET	pRetPacket = NULL;
+	UCHAR			*pFragBuffer = NULL;
+	BOOLEAN 		bReassDone = FALSE;
+	UCHAR			HeaderRoom = 0;
+
+
+	ASSERT(pHeader);
+
+	HeaderRoom = pData - (UCHAR *)pHeader;
+
+	// Re-assemble the fragmented packets
+	if (pHeader->Frag == 0)		// Frag. Number is 0 : First frag or only one pkt
+	{
+		// the first pkt of fragment, record it.
+		if (pHeader->FC.MoreFrag)
+		{
+			ASSERT(pAd->FragFrame.pFragPacket);
+			pFragBuffer = GET_OS_PKT_DATAPTR(pAd->FragFrame.pFragPacket);
+			pAd->FragFrame.RxSize   = DataSize + HeaderRoom;
+			NdisMoveMemory(pFragBuffer,	 pHeader, pAd->FragFrame.RxSize);
+			pAd->FragFrame.Sequence = pHeader->Sequence;
+			pAd->FragFrame.LastFrag = pHeader->Frag;	   // Should be 0
+			ASSERT(pAd->FragFrame.LastFrag == 0);
+			goto done;	// end of processing this frame
+		}
+	}
+	else	//Middle & End of fragment
+	{
+		if ((pHeader->Sequence != pAd->FragFrame.Sequence) ||
+			(pHeader->Frag != (pAd->FragFrame.LastFrag + 1)))
+		{
+			// Fragment is not the same sequence or out of fragment number order
+			// Reset Fragment control blk
+			RESET_FRAGFRAME(pAd->FragFrame);
+			DBGPRINT(RT_DEBUG_ERROR, ("Fragment is not the same sequence or out of fragment number order.\n"));
+			goto done; // give up this frame
+		}
+		else if ((pAd->FragFrame.RxSize + DataSize) > MAX_FRAME_SIZE)
+		{
+			// Fragment frame is too large, it exeeds the maximum frame size.
+			// Reset Fragment control blk
+			RESET_FRAGFRAME(pAd->FragFrame);
+			DBGPRINT(RT_DEBUG_ERROR, ("Fragment frame is too large, it exeeds the maximum frame size.\n"));
+			goto done; // give up this frame
+		}
+
+        //
+		// Broadcom AP(BCM94704AGR) will send out LLC in fragment's packet, LLC only can accpet at first fragment.
+		// In this case, we will dropt it.
+		//
+		if (NdisEqualMemory(pData, SNAP_802_1H, sizeof(SNAP_802_1H)))
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Find another LLC at Middle or End fragment(SN=%d, Frag=%d)\n", pHeader->Sequence, pHeader->Frag));
+			goto done; // give up this frame
+		}
+
+		pFragBuffer = GET_OS_PKT_DATAPTR(pAd->FragFrame.pFragPacket);
+
+		// concatenate this fragment into the re-assembly buffer
+		NdisMoveMemory((pFragBuffer + pAd->FragFrame.RxSize), pData, DataSize);
+		pAd->FragFrame.RxSize  += DataSize;
+		pAd->FragFrame.LastFrag = pHeader->Frag;	   // Update fragment number
+
+		// Last fragment
+		if (pHeader->FC.MoreFrag == FALSE)
+		{
+			bReassDone = TRUE;
+		}
+	}
+
+done:
+	// always release rx fragmented packet
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+
+	// return defragmented packet if packet is reassembled completely
+	// otherwise return NULL
+	if (bReassDone)
+	{
+		PNDIS_PACKET pNewFragPacket;
+
+		// allocate a new packet buffer for fragment
+		pNewFragPacket = RTMP_AllocateFragPacketBuffer(pAd, RX_BUFFER_NORMSIZE);
+		if (pNewFragPacket)
+		{
+			// update RxBlk
+			pRetPacket = pAd->FragFrame.pFragPacket;
+			pAd->FragFrame.pFragPacket = pNewFragPacket;
+			pRxBlk->pHeader = (PHEADER_802_11) GET_OS_PKT_DATAPTR(pRetPacket);
+			pRxBlk->pData = (UCHAR *)pRxBlk->pHeader + HeaderRoom;
+			pRxBlk->DataSize = pAd->FragFrame.RxSize - HeaderRoom;
+			pRxBlk->pRxPacket = pRetPacket;
+		}
+		else
+		{
+			RESET_FRAGFRAME(pAd->FragFrame);
+		}
+	}
+
+	return pRetPacket;
+}
+
+
+VOID Indicate_AMSDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	UINT			nMSDU;
+
+	update_os_packet_info(pAd, pRxBlk, FromWhichBSSID);
+	RTMP_SET_PACKET_IF(pRxBlk->pRxPacket, FromWhichBSSID);
+	nMSDU = deaggregate_AMSDU_announce(pAd, pRxBlk->pRxPacket, pRxBlk->pData, pRxBlk->DataSize);
+}
+
+VOID Indicate_EAPOL_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	MAC_TABLE_ENTRY *pEntry = NULL;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pEntry = &pAd->MacTab.Content[BSSID_WCID];
+		STARxEAPOLFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
+		return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	if (pEntry == NULL)
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("Indicate_EAPOL_Packet: drop and release the invalid packet.\n"));
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+}
+
+#define BCN_TBTT_OFFSET		64	//defer 64 us
+VOID ReSyncBeaconTime(
+	IN  PRTMP_ADAPTER   pAd)
+{
+
+	UINT32  Offset;
+
+
+	Offset = (pAd->TbttTickCount) % (BCN_TBTT_OFFSET);
+
+	pAd->TbttTickCount++;
+
+	//
+	// The updated BeaconInterval Value will affect Beacon Interval after two TBTT
+	// beacasue the original BeaconInterval had been loaded into next TBTT_TIMER
+	//
+	if (Offset == (BCN_TBTT_OFFSET-2))
+	{
+		BCN_TIME_CFG_STRUC csr;
+		RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+		csr.field.BeaconInterval = (pAd->CommonCfg.BeaconPeriod << 4) - 1 ;	// ASIC register in units of 1/16 TU = 64us
+		RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+	}
+	else
+	{
+		if (Offset == (BCN_TBTT_OFFSET-1))
+		{
+			BCN_TIME_CFG_STRUC csr;
+
+			RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+			csr.field.BeaconInterval = (pAd->CommonCfg.BeaconPeriod) << 4; // ASIC register in units of 1/16 TU
+			RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+		}
+	}
+}
+
diff --git a/drivers/staging/rt2870/common/cmm_data_2870.c b/drivers/staging/rt2870/common/cmm_data_2870.c
new file mode 100644
index 000000000000..f77000f336a9
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_data_2870.c
@@ -0,0 +1,963 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+/*
+   All functions in this file must be USB-depended, or you should out your function
+	in other files.
+
+*/
+#include "../rt_config.h"
+
+
+/*
+	We can do copy the frame into pTxContext when match following conditions.
+		=>
+		=>
+		=>
+*/
+static inline NDIS_STATUS RtmpUSBCanDoWrite(
+	IN RTMP_ADAPTER		*pAd,
+	IN UCHAR			QueIdx,
+	IN HT_TX_CONTEXT 	*pHTTXContext)
+{
+	NDIS_STATUS	canWrite = NDIS_STATUS_RESOURCES;
+
+	if (((pHTTXContext->CurWritePosition) < pHTTXContext->NextBulkOutPosition) && (pHTTXContext->CurWritePosition + LOCAL_TXBUF_SIZE) > pHTTXContext->NextBulkOutPosition)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("RtmpUSBCanDoWrite c1!\n"));
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << QueIdx));
+	}
+	else if ((pHTTXContext->CurWritePosition == 8) && (pHTTXContext->NextBulkOutPosition < LOCAL_TXBUF_SIZE))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("RtmpUSBCanDoWrite c2!\n"));
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << QueIdx));
+	}
+	else if (pHTTXContext->bCurWriting == TRUE)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("RtmpUSBCanDoWrite c3!\n"));
+	}
+	else
+	{
+		canWrite = NDIS_STATUS_SUCCESS;
+	}
+
+
+	return canWrite;
+}
+
+
+USHORT RtmpUSB_WriteSubTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber)
+{
+
+	// Dummy function. Should be removed in the future.
+	return 0;
+
+}
+
+USHORT	RtmpUSB_WriteFragTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			fragNum,
+	OUT	USHORT			*FreeNumber)
+{
+	HT_TX_CONTEXT	*pHTTXContext;
+	USHORT			hwHdrLen;	// The hwHdrLen consist of 802.11 header length plus the header padding length.
+	UINT32			fillOffset;
+	TXINFO_STRUC	*pTxInfo;
+	TXWI_STRUC		*pTxWI;
+	PUCHAR			pWirelessPacket = NULL;
+	UCHAR			QueIdx;
+	NDIS_STATUS		Status;
+	unsigned long	IrqFlags;
+	UINT32			USBDMApktLen = 0, DMAHdrLen, padding;
+	BOOLEAN			TxQLastRound = FALSE;
+
+	//
+	// get Tx Ring Resource & Dma Buffer address
+	//
+	QueIdx = pTxBlk->QueIdx;
+	pHTTXContext  = &pAd->TxContext[QueIdx];
+
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+	pHTTXContext  = &pAd->TxContext[QueIdx];
+	fillOffset = pHTTXContext->CurWritePosition;
+
+	if(fragNum == 0)
+	{
+		// Check if we have enough space for this bulk-out batch.
+		Status = RtmpUSBCanDoWrite(pAd, QueIdx, pHTTXContext);
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			pHTTXContext->bCurWriting = TRUE;
+
+			// Reserve space for 8 bytes padding.
+			if ((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition))
+			{
+				pHTTXContext->ENextBulkOutPosition += 8;
+				pHTTXContext->CurWritePosition += 8;
+				fillOffset += 8;
+			}
+			pTxBlk->Priv = 0;
+			pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			return(Status);
+		}
+	}
+	else
+	{
+		// For sub-sequent frames of this bulk-out batch. Just copy it to our bulk-out buffer.
+		Status = ((pHTTXContext->bCurWriting == TRUE) ? NDIS_STATUS_SUCCESS : NDIS_STATUS_FAILURE);
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			fillOffset += pTxBlk->Priv;
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			return(Status);
+		}
+	}
+
+	NdisZeroMemory((PUCHAR)(&pTxBlk->HeaderBuf[0]), TXINFO_SIZE);
+	pTxInfo = (PTXINFO_STRUC)(&pTxBlk->HeaderBuf[0]);
+	pTxWI= (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]);
+
+	pWirelessPacket = &pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset];
+
+	// copy TXWI + WLAN Header + LLC into DMA Header Buffer
+	//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen, 4);
+	hwHdrLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+
+	// Build our URB for USBD
+	DMAHdrLen = TXWI_SIZE + hwHdrLen;
+	USBDMApktLen = DMAHdrLen + pTxBlk->SrcBufLen;
+	padding = (4 - (USBDMApktLen % 4)) & 0x03;	// round up to 4 byte alignment
+	USBDMApktLen += padding;
+
+	pTxBlk->Priv += (TXINFO_SIZE + USBDMApktLen);
+
+	// For TxInfo, the length of USBDMApktLen = TXWI_SIZE + 802.11 header + payload
+	RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(USBDMApktLen), FALSE, FIFO_EDCA, FALSE /*NextValid*/,  FALSE);
+
+	if (fragNum == pTxBlk->TotalFragNum)
+	{
+		pTxInfo->USBDMATxburst = 0;
+		if ((pHTTXContext->CurWritePosition + pTxBlk->Priv + 3906)> MAX_TXBULK_LIMIT)
+		{
+			pTxInfo->SwUseLastRound = 1;
+			TxQLastRound = TRUE;
+		}
+	}
+	else
+	{
+		pTxInfo->USBDMATxburst = 1;
+	}
+
+	NdisMoveMemory(pWirelessPacket, pTxBlk->HeaderBuf, TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)(pWirelessPacket + TXINFO_SIZE + TXWI_SIZE), DIR_WRITE, FALSE);
+#endif // RT_BIG_ENDIAN //
+	pWirelessPacket += (TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+	pHTTXContext->CurWriteRealPos += (TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+	NdisMoveMemory(pWirelessPacket, pTxBlk->pSrcBufData, pTxBlk->SrcBufLen);
+
+	//	Zero the last padding.
+	pWirelessPacket += pTxBlk->SrcBufLen;
+	NdisZeroMemory(pWirelessPacket, padding + 8);
+
+	if (fragNum == pTxBlk->TotalFragNum)
+	{
+		RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+		// Update the pHTTXContext->CurWritePosition. 3906 used to prevent the NextBulkOut is a A-RALINK/A-MSDU Frame.
+		pHTTXContext->CurWritePosition += pTxBlk->Priv;
+		if (TxQLastRound == TRUE)
+			pHTTXContext->CurWritePosition = 8;
+		pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+
+
+		// Finally, set bCurWriting as FALSE
+	pHTTXContext->bCurWriting = FALSE;
+
+		RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+		// succeed and release the skb buffer
+		RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_SUCCESS);
+	}
+
+
+	return(Status);
+
+}
+
+
+USHORT RtmpUSB_WriteSingleTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber)
+{
+	HT_TX_CONTEXT	*pHTTXContext;
+	USHORT			hwHdrLen;
+	UINT32			fillOffset;
+	TXINFO_STRUC	*pTxInfo;
+	TXWI_STRUC		*pTxWI;
+	PUCHAR			pWirelessPacket;
+	UCHAR			QueIdx;
+	unsigned long	IrqFlags;
+	NDIS_STATUS		Status;
+	UINT32			USBDMApktLen = 0, DMAHdrLen, padding;
+	BOOLEAN			bTxQLastRound = FALSE;
+
+	// For USB, didn't need PCI_MAP_SINGLE()
+	//SrcBufPA = PCI_MAP_SINGLE(pAd, (char *) pTxBlk->pSrcBufData, pTxBlk->SrcBufLen, PCI_DMA_TODEVICE);
+
+
+	//
+	// get Tx Ring Resource & Dma Buffer address
+	//
+	QueIdx = pTxBlk->QueIdx;
+
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+	pHTTXContext  = &pAd->TxContext[QueIdx];
+	fillOffset = pHTTXContext->CurWritePosition;
+
+
+
+	// Check ring full.
+	Status = RtmpUSBCanDoWrite(pAd, QueIdx, pHTTXContext);
+	if(Status == NDIS_STATUS_SUCCESS)
+	{
+		pHTTXContext->bCurWriting = TRUE;
+
+		pTxInfo = (PTXINFO_STRUC)(&pTxBlk->HeaderBuf[0]);
+		pTxWI= (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]);
+
+		// Reserve space for 8 bytes padding.
+		if ((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition))
+		{
+			pHTTXContext->ENextBulkOutPosition += 8;
+			pHTTXContext->CurWritePosition += 8;
+			fillOffset += 8;
+		}
+		pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+
+		pWirelessPacket = &pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset];
+
+		// copy TXWI + WLAN Header + LLC into DMA Header Buffer
+		//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen, 4);
+		hwHdrLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+
+		// Build our URB for USBD
+		DMAHdrLen = TXWI_SIZE + hwHdrLen;
+		USBDMApktLen = DMAHdrLen + pTxBlk->SrcBufLen;
+		padding = (4 - (USBDMApktLen % 4)) & 0x03;	// round up to 4 byte alignment
+		USBDMApktLen += padding;
+
+		pTxBlk->Priv = (TXINFO_SIZE + USBDMApktLen);
+
+		// For TxInfo, the length of USBDMApktLen = TXWI_SIZE + 802.11 header + payload
+		RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(USBDMApktLen), FALSE, FIFO_EDCA, FALSE /*NextValid*/,  FALSE);
+
+		if ((pHTTXContext->CurWritePosition + 3906 + pTxBlk->Priv) > MAX_TXBULK_LIMIT)
+		{
+			pTxInfo->SwUseLastRound = 1;
+			bTxQLastRound = TRUE;
+		}
+		NdisMoveMemory(pWirelessPacket, pTxBlk->HeaderBuf, TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+#ifdef RT_BIG_ENDIAN
+		RTMPFrameEndianChange(pAd, (PUCHAR)(pWirelessPacket + TXINFO_SIZE + TXWI_SIZE), DIR_WRITE, FALSE);
+#endif // RT_BIG_ENDIAN //
+		pWirelessPacket += (TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+
+		// We unlock it here to prevent the first 8 bytes maybe over-writed issue.
+		//	1. First we got CurWritePosition but the first 8 bytes still not write to the pTxcontext.
+		//	2. An interrupt break our routine and handle bulk-out complete.
+		//	3. In the bulk-out compllete, it need to do another bulk-out,
+		//			if the ENextBulkOutPosition is just the same as CurWritePosition, it will save the first 8 bytes from CurWritePosition,
+		//			but the payload still not copyed. the pTxContext->SavedPad[] will save as allzero. and set the bCopyPad = TRUE.
+		//	4. Interrupt complete.
+		//  5. Our interrupted routine go back and fill the first 8 bytes to pTxContext.
+		//	6. Next time when do bulk-out, it found the bCopyPad==TRUE and will copy the SavedPad[] to pTxContext->NextBulkOutPosition.
+		//		and the packet will wrong.
+		pHTTXContext->CurWriteRealPos += (TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+		RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+		NdisMoveMemory(pWirelessPacket, pTxBlk->pSrcBufData, pTxBlk->SrcBufLen);
+		pWirelessPacket += pTxBlk->SrcBufLen;
+		NdisZeroMemory(pWirelessPacket, padding + 8);
+
+		RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+		pHTTXContext->CurWritePosition += pTxBlk->Priv;
+		if (bTxQLastRound)
+			pHTTXContext->CurWritePosition = 8;
+		pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+
+	pHTTXContext->bCurWriting = FALSE;
+	}
+
+
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+
+	// succeed and release the skb buffer
+	RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_SUCCESS);
+
+	return(Status);
+
+}
+
+
+USHORT RtmpUSB_WriteMultiTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			frameNum,
+	OUT	USHORT			*FreeNumber)
+{
+	HT_TX_CONTEXT	*pHTTXContext;
+	USHORT			hwHdrLen;	// The hwHdrLen consist of 802.11 header length plus the header padding length.
+	UINT32			fillOffset;
+	TXINFO_STRUC	*pTxInfo;
+	TXWI_STRUC		*pTxWI;
+	PUCHAR			pWirelessPacket = NULL;
+	UCHAR			QueIdx;
+	NDIS_STATUS		Status;
+	unsigned long	IrqFlags;
+	//UINT32			USBDMApktLen = 0, DMAHdrLen, padding;
+
+	//
+	// get Tx Ring Resource & Dma Buffer address
+	//
+	QueIdx = pTxBlk->QueIdx;
+	pHTTXContext  = &pAd->TxContext[QueIdx];
+
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+	if(frameNum == 0)
+	{
+		// Check if we have enough space for this bulk-out batch.
+		Status = RtmpUSBCanDoWrite(pAd, QueIdx, pHTTXContext);
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			pHTTXContext->bCurWriting = TRUE;
+
+			pTxInfo = (PTXINFO_STRUC)(&pTxBlk->HeaderBuf[0]);
+			pTxWI= (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]);
+
+
+			// Reserve space for 8 bytes padding.
+			if ((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition))
+			{
+
+				pHTTXContext->CurWritePosition += 8;
+				pHTTXContext->ENextBulkOutPosition += 8;
+			}
+			fillOffset = pHTTXContext->CurWritePosition;
+			pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+
+			pWirelessPacket = &pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset];
+
+			//
+			// Copy TXINFO + TXWI + WLAN Header + LLC into DMA Header Buffer
+			//
+			if (pTxBlk->TxFrameType == TX_AMSDU_FRAME)
+				//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen-LENGTH_AMSDU_SUBFRAMEHEAD, 4)+LENGTH_AMSDU_SUBFRAMEHEAD;
+				hwHdrLen = pTxBlk->MpduHeaderLen-LENGTH_AMSDU_SUBFRAMEHEAD + pTxBlk->HdrPadLen + LENGTH_AMSDU_SUBFRAMEHEAD;
+			else if (pTxBlk->TxFrameType == TX_RALINK_FRAME)
+				//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen-LENGTH_ARALINK_HEADER_FIELD, 4)+LENGTH_ARALINK_HEADER_FIELD;
+				hwHdrLen = pTxBlk->MpduHeaderLen-LENGTH_ARALINK_HEADER_FIELD + pTxBlk->HdrPadLen + LENGTH_ARALINK_HEADER_FIELD;
+			else
+				//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen, 4);
+				hwHdrLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+
+			// Update the pTxBlk->Priv.
+			pTxBlk->Priv = TXINFO_SIZE + TXWI_SIZE + hwHdrLen;
+
+			//	pTxInfo->USBDMApktLen now just a temp value and will to correct latter.
+			RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(pTxBlk->Priv), FALSE, FIFO_EDCA, FALSE /*NextValid*/,  FALSE);
+
+			// Copy it.
+			NdisMoveMemory(pWirelessPacket, pTxBlk->HeaderBuf, pTxBlk->Priv);
+#ifdef RT_BIG_ENDIAN
+			RTMPFrameEndianChange(pAd, (PUCHAR)(pWirelessPacket+ TXINFO_SIZE + TXWI_SIZE), DIR_WRITE, FALSE);
+#endif // RT_BIG_ENDIAN //
+			pHTTXContext->CurWriteRealPos += pTxBlk->Priv;
+			pWirelessPacket += pTxBlk->Priv;
+		}
+	}
+	else
+	{	// For sub-sequent frames of this bulk-out batch. Just copy it to our bulk-out buffer.
+
+		Status = ((pHTTXContext->bCurWriting == TRUE) ? NDIS_STATUS_SUCCESS : NDIS_STATUS_FAILURE);
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			fillOffset =  (pHTTXContext->CurWritePosition + pTxBlk->Priv);
+			pWirelessPacket = &pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset];
+
+			//hwHdrLen = pTxBlk->MpduHeaderLen;
+			NdisMoveMemory(pWirelessPacket, pTxBlk->HeaderBuf, pTxBlk->MpduHeaderLen);
+			pWirelessPacket += (pTxBlk->MpduHeaderLen);
+			pTxBlk->Priv += pTxBlk->MpduHeaderLen;
+		}
+		else
+		{	// It should not happened now unless we are going to shutdown.
+			DBGPRINT(RT_DEBUG_ERROR, ("WriteMultiTxResource():bCurWriting is FALSE when handle sub-sequent frames.\n"));
+			Status = NDIS_STATUS_FAILURE;
+		}
+	}
+
+
+	// We unlock it here to prevent the first 8 bytes maybe over-write issue.
+	//	1. First we got CurWritePosition but the first 8 bytes still not write to the pTxContext.
+	//	2. An interrupt break our routine and handle bulk-out complete.
+	//	3. In the bulk-out compllete, it need to do another bulk-out,
+	//			if the ENextBulkOutPosition is just the same as CurWritePosition, it will save the first 8 bytes from CurWritePosition,
+	//			but the payload still not copyed. the pTxContext->SavedPad[] will save as allzero. and set the bCopyPad = TRUE.
+	//	4. Interrupt complete.
+	//  5. Our interrupted routine go back and fill the first 8 bytes to pTxContext.
+	//	6. Next time when do bulk-out, it found the bCopyPad==TRUE and will copy the SavedPad[] to pTxContext->NextBulkOutPosition.
+	//		and the packet will wrong.
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("WriteMultiTxResource: CWPos = %ld, NBOutPos = %ld.\n", pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition));
+		goto done;
+	}
+
+	// Copy the frame content into DMA buffer and update the pTxBlk->Priv
+	NdisMoveMemory(pWirelessPacket, pTxBlk->pSrcBufData, pTxBlk->SrcBufLen);
+	pWirelessPacket += pTxBlk->SrcBufLen;
+	pTxBlk->Priv += pTxBlk->SrcBufLen;
+
+done:
+	// Release the skb buffer here
+	RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_SUCCESS);
+
+	return(Status);
+
+}
+
+
+VOID RtmpUSB_FinalWriteTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	USHORT			totalMPDUSize,
+	IN	USHORT			TxIdx)
+{
+	UCHAR			QueIdx;
+	HT_TX_CONTEXT	*pHTTXContext;
+	UINT32			fillOffset;
+	TXINFO_STRUC	*pTxInfo;
+	TXWI_STRUC		*pTxWI;
+	UINT32			USBDMApktLen, padding;
+	unsigned long	IrqFlags;
+	PUCHAR			pWirelessPacket;
+
+	QueIdx = pTxBlk->QueIdx;
+	pHTTXContext  = &pAd->TxContext[QueIdx];
+
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+	if (pHTTXContext->bCurWriting == TRUE)
+	{
+		fillOffset = pHTTXContext->CurWritePosition;
+		if (((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition) || ((pHTTXContext->ENextBulkOutPosition-8) == pHTTXContext->CurWritePosition))
+			&& (pHTTXContext->bCopySavePad == TRUE))
+			pWirelessPacket = (PUCHAR)(&pHTTXContext->SavedPad[0]);
+		else
+			pWirelessPacket = (PUCHAR)(&pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset]);
+
+		//
+		// Update TxInfo->USBDMApktLen ,
+		//		the length = TXWI_SIZE + 802.11_hdr + 802.11_hdr_pad + payload_of_all_batch_frames + Bulk-Out-padding
+		//
+		pTxInfo = (PTXINFO_STRUC)(pWirelessPacket);
+
+		// Calculate the bulk-out padding
+		USBDMApktLen = pTxBlk->Priv - TXINFO_SIZE;
+		padding = (4 - (USBDMApktLen % 4)) & 0x03;	// round up to 4 byte alignment
+		USBDMApktLen += padding;
+
+		pTxInfo->USBDMATxPktLen = USBDMApktLen;
+
+		//
+		// Update TXWI->MPDUtotalByteCount ,
+		//		the length = 802.11 header + payload_of_all_batch_frames
+		pTxWI= (PTXWI_STRUC)(pWirelessPacket + TXINFO_SIZE);
+		pTxWI->MPDUtotalByteCount = totalMPDUSize;
+
+		//
+		// Update the pHTTXContext->CurWritePosition
+		//
+		pHTTXContext->CurWritePosition += (TXINFO_SIZE + USBDMApktLen);
+		if ((pHTTXContext->CurWritePosition + 3906)> MAX_TXBULK_LIMIT)
+		{	// Add 3906 for prevent the NextBulkOut packet size is a A-RALINK/A-MSDU Frame.
+			pHTTXContext->CurWritePosition = 8;
+			pTxInfo->SwUseLastRound = 1;
+		}
+		pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+
+
+		//
+		//	Zero the last padding.
+		//
+		pWirelessPacket = (&pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset + pTxBlk->Priv]);
+		NdisZeroMemory(pWirelessPacket, padding + 8);
+
+		// Finally, set bCurWriting as FALSE
+		pHTTXContext->bCurWriting = FALSE;
+
+	}
+	else
+	{	// It should not happened now unless we are going to shutdown.
+		DBGPRINT(RT_DEBUG_ERROR, ("FinalWriteTxResource():bCurWriting is FALSE when handle last frames.\n"));
+	}
+
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+}
+
+
+VOID RtmpUSBDataLastTxIdx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	USHORT			TxIdx)
+{
+	// DO nothing for USB.
+}
+
+
+/*
+	When can do bulk-out:
+		1. TxSwFreeIdx < TX_RING_SIZE;
+			It means has at least one Ring entity is ready for bulk-out, kick it out.
+		2. If TxSwFreeIdx == TX_RING_SIZE
+			Check if the CurWriting flag is FALSE, if it's FALSE, we can do kick out.
+
+*/
+VOID RtmpUSBDataKickOut(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			QueIdx)
+{
+	RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << QueIdx));
+	RTUSBKickBulkOut(pAd);
+
+}
+
+
+/*
+	Must be run in Interrupt context
+	This function handle RT2870 specific TxDesc and cpu index update and kick the packet out.
+ */
+int RtmpUSBMgmtKickOut(
+	IN RTMP_ADAPTER 	*pAd,
+	IN UCHAR 			QueIdx,
+	IN PNDIS_PACKET		pPacket,
+	IN PUCHAR			pSrcBufVA,
+	IN UINT 			SrcBufLen)
+{
+	PTXINFO_STRUC	pTxInfo;
+	ULONG			BulkOutSize;
+	UCHAR			padLen;
+	PUCHAR			pDest;
+	ULONG			SwIdx = pAd->MgmtRing.TxCpuIdx;
+	PTX_CONTEXT		pMLMEContext = (PTX_CONTEXT)pAd->MgmtRing.Cell[SwIdx].AllocVa;
+	unsigned long	IrqFlags;
+
+
+	pTxInfo = (PTXINFO_STRUC)(pSrcBufVA);
+
+	// Build our URB for USBD
+	BulkOutSize = SrcBufLen;
+	BulkOutSize = (BulkOutSize + 3) & (~3);
+	RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(BulkOutSize - TXINFO_SIZE), TRUE, EpToQueue[MGMTPIPEIDX], FALSE,  FALSE);
+
+	BulkOutSize += 4; // Always add 4 extra bytes at every packet.
+
+	// If BulkOutSize is multiple of BulkOutMaxPacketSize, add extra 4 bytes again.
+	if ((BulkOutSize % pAd->BulkOutMaxPacketSize) == 0)
+		BulkOutSize += 4;
+
+	padLen = BulkOutSize - SrcBufLen;
+	ASSERT((padLen <= RTMP_PKT_TAIL_PADDING));
+
+	// Now memzero all extra padding bytes.
+	pDest = (PUCHAR)(pSrcBufVA + SrcBufLen);
+	skb_put(GET_OS_PKT_TYPE(pPacket), padLen);
+	NdisZeroMemory(pDest, padLen);
+
+	RTMP_IRQ_LOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+
+	pAd->MgmtRing.Cell[pAd->MgmtRing.TxCpuIdx].pNdisPacket = pPacket;
+	pMLMEContext->TransferBuffer = (PTX_BUFFER)(GET_OS_PKT_DATAPTR(pPacket));
+
+	// Length in TxInfo should be 8 less than bulkout size.
+	pMLMEContext->BulkOutSize = BulkOutSize;
+	pMLMEContext->InUse = TRUE;
+	pMLMEContext->bWaitingBulkOut = TRUE;
+
+
+	//for debug
+	//hex_dump("RtmpUSBMgmtKickOut", &pMLMEContext->TransferBuffer->field.WirelessPacket[0], (pMLMEContext->BulkOutSize > 16 ? 16 : pMLMEContext->BulkOutSize));
+
+	//pAd->RalinkCounters.KickTxCount++;
+	//pAd->RalinkCounters.OneSecTxDoneCount++;
+
+	//if (pAd->MgmtRing.TxSwFreeIdx == MGMT_RING_SIZE)
+	//	needKickOut = TRUE;
+
+	// Decrease the TxSwFreeIdx and Increase the TX_CTX_IDX
+	pAd->MgmtRing.TxSwFreeIdx--;
+	INC_RING_INDEX(pAd->MgmtRing.TxCpuIdx, MGMT_RING_SIZE);
+
+	RTMP_IRQ_UNLOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+
+	RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME);
+	//if (needKickOut)
+	RTUSBKickBulkOut(pAd);
+
+	return 0;
+}
+
+
+VOID RtmpUSBNullFrameKickOut(
+	IN RTMP_ADAPTER *pAd,
+	IN UCHAR		QueIdx,
+	IN UCHAR		*pNullFrame,
+	IN UINT32		frameLen)
+{
+	if (pAd->NullContext.InUse == FALSE)
+	{
+		PTX_CONTEXT		pNullContext;
+		PTXINFO_STRUC	pTxInfo;
+		PTXWI_STRUC		pTxWI;
+		PUCHAR			pWirelessPkt;
+
+		pNullContext = &(pAd->NullContext);
+
+		// Set the in use bit
+		pNullContext->InUse = TRUE;
+		pWirelessPkt = (PUCHAR)&pNullContext->TransferBuffer->field.WirelessPacket[0];
+
+		RTMPZeroMemory(&pWirelessPkt[0], 100);
+		pTxInfo = (PTXINFO_STRUC)&pWirelessPkt[0];
+		RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(sizeof(HEADER_802_11)+TXWI_SIZE), TRUE, EpToQueue[MGMTPIPEIDX], FALSE,  FALSE);
+		pTxInfo->QSEL = FIFO_EDCA;
+		pTxWI = (PTXWI_STRUC)&pWirelessPkt[TXINFO_SIZE];
+		RTMPWriteTxWI(pAd, pTxWI,  FALSE, FALSE, FALSE, FALSE, TRUE, FALSE, 0, BSSID_WCID, (sizeof(HEADER_802_11)),
+			0, 0, (UCHAR)pAd->CommonCfg.MlmeTransmit.field.MCS, IFS_HTTXOP, FALSE, &pAd->CommonCfg.MlmeTransmit);
+#ifdef RT_BIG_ENDIAN
+		RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+#endif // RT_BIG_ENDIAN //
+
+		RTMPMoveMemory(&pWirelessPkt[TXWI_SIZE+TXINFO_SIZE], &pAd->NullFrame, sizeof(HEADER_802_11));
+#ifdef RT_BIG_ENDIAN
+		RTMPFrameEndianChange(pAd, (PUCHAR)&pWirelessPkt[TXINFO_SIZE + TXWI_SIZE], DIR_WRITE, FALSE);
+#endif // RT_BIG_ENDIAN //
+		pAd->NullContext.BulkOutSize =  TXINFO_SIZE + TXWI_SIZE + sizeof(pAd->NullFrame) + 4;
+
+		// Fill out frame length information for global Bulk out arbitor
+		//pNullContext->BulkOutSize = TransferBufferLength;
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - send NULL Frame @%d Mbps...\n", RateIdToMbps[pAd->CommonCfg.TxRate]));
+		RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NULL);
+
+		// Kick bulk out
+		RTUSBKickBulkOut(pAd);
+	}
+
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	========================================================================
+
+	Routine	Description:
+		Check Rx descriptor, return NDIS_STATUS_FAILURE if any error dound
+
+	Arguments:
+		pRxD		Pointer	to the Rx descriptor
+
+	Return Value:
+		NDIS_STATUS_SUCCESS		No err
+		NDIS_STATUS_FAILURE		Error
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	RTMPCheckRxError(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PHEADER_802_11	pHeader,
+	IN	PRXWI_STRUC	pRxWI,
+	IN	PRT28XX_RXD_STRUC	pRxINFO)
+{
+	PCIPHER_KEY pWpaKey;
+	INT	dBm;
+
+	if (pAd->bPromiscuous == TRUE)
+		return(NDIS_STATUS_SUCCESS);
+	if(pRxINFO == NULL)
+		return(NDIS_STATUS_FAILURE);
+
+	// Phy errors & CRC errors
+	if (pRxINFO->Crc)
+	{
+		// Check RSSI for Noise Hist statistic collection.
+		dBm = (INT) (pRxWI->RSSI0) - pAd->BbpRssiToDbmDelta;
+		if (dBm <= -87)
+			pAd->StaCfg.RPIDensity[0] += 1;
+		else if (dBm <= -82)
+			pAd->StaCfg.RPIDensity[1] += 1;
+		else if (dBm <= -77)
+			pAd->StaCfg.RPIDensity[2] += 1;
+		else if (dBm <= -72)
+			pAd->StaCfg.RPIDensity[3] += 1;
+		else if (dBm <= -67)
+			pAd->StaCfg.RPIDensity[4] += 1;
+		else if (dBm <= -62)
+			pAd->StaCfg.RPIDensity[5] += 1;
+		else if (dBm <= -57)
+			pAd->StaCfg.RPIDensity[6] += 1;
+		else if (dBm > -57)
+			pAd->StaCfg.RPIDensity[7] += 1;
+
+		return(NDIS_STATUS_FAILURE);
+	}
+
+	// Add Rx size to channel load counter, we should ignore error counts
+	pAd->StaCfg.CLBusyBytes += (pRxWI->MPDUtotalByteCount+ 14);
+
+	// Drop ToDs promiscous frame, it is opened due to CCX 2 channel load statistics
+	if (pHeader->FC.ToDs)
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Err;FC.ToDs\n"));
+		return NDIS_STATUS_FAILURE;
+	}
+
+	// Paul 04-03 for OFDM Rx length issue
+	if (pRxWI->MPDUtotalByteCount > MAX_AGGREGATION_SIZE)
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("received packet too long\n"));
+		return NDIS_STATUS_FAILURE;
+	}
+
+	// Drop not U2M frames, cant's drop here because we will drop beacon in this case
+	// I am kind of doubting the U2M bit operation
+	// if (pRxD->U2M == 0)
+	//	return(NDIS_STATUS_FAILURE);
+
+	// drop decyption fail frame
+	if (pRxINFO->Decrypted && pRxINFO->CipherErr)
+	{
+
+		//
+		// MIC Error
+		//
+		if ((pRxINFO->CipherErr == 2) && pRxINFO->MyBss)
+		{
+			pWpaKey = &pAd->SharedKey[BSS0][pRxWI->KeyIndex];
+			RTMPReportMicError(pAd, pWpaKey);
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("Rx MIC Value error\n"));
+		}
+
+		if (pRxINFO->Decrypted &&
+			(pAd->SharedKey[BSS0][pRxWI->KeyIndex].CipherAlg == CIPHER_AES) &&
+			(pHeader->Sequence == pAd->FragFrame.Sequence))
+		{
+			//
+			// Acceptable since the First FragFrame no CipherErr problem.
+			//
+			return(NDIS_STATUS_SUCCESS);
+		}
+
+		return(NDIS_STATUS_FAILURE);
+	}
+
+	return(NDIS_STATUS_SUCCESS);
+}
+
+VOID RT28xxUsbStaAsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN       bFromTx)
+{
+    AUTO_WAKEUP_STRUC	AutoWakeupCfg;
+
+	AutoWakeupCfg.word = 0;
+	RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+
+	AsicSendCommandToMcu(pAd, 0x31, 0xff, 0x00, 0x00);
+
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+}
+
+VOID RT28xxUsbStaAsicSleepThenAutoWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TbttNumToNextWakeUp)
+{
+	AUTO_WAKEUP_STRUC	AutoWakeupCfg;
+
+	// we have decided to SLEEP, so at least do it for a BEACON period.
+	if (TbttNumToNextWakeUp == 0)
+		TbttNumToNextWakeUp = 1;
+
+	AutoWakeupCfg.word = 0;
+	RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+
+	AutoWakeupCfg.field.NumofSleepingTbtt = TbttNumToNextWakeUp - 1;
+	AutoWakeupCfg.field.EnableAutoWakeup = 1;
+	AutoWakeupCfg.field.AutoLeadTime = 5;
+	RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+
+	AsicSendCommandToMcu(pAd, 0x30, 0xff, 0xff, 0x02);   // send POWER-SAVE command to MCU. Timeout 40us.
+
+	OPSTATUS_SET_FLAG(pAd, fOP_STATUS_DOZE);
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+VOID RT28xxUsbMlmeRadioOn(
+	IN PRTMP_ADAPTER pAd)
+{
+    DBGPRINT(RT_DEBUG_TRACE,("RT28xxUsbMlmeRadioOn()\n"));
+
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+		return;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+    	AsicSendCommandToMcu(pAd, 0x31, 0xff, 0x00, 0x00);
+		RTMPusecDelay(10000);
+	}
+#endif // CONFIG_STA_SUPPORT //
+	NICResetFromError(pAd);
+
+	// Enable Tx/Rx
+	RTMPEnableRxTx(pAd);
+
+	// Clear Radio off flag
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTUSBBulkReceive(pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+	// Set LED
+	RTMPSetLED(pAd, LED_RADIO_ON);
+}
+
+VOID RT28xxUsbMlmeRadioOFF(
+	IN PRTMP_ADAPTER pAd)
+{
+	WPDMA_GLO_CFG_STRUC	GloCfg;
+	UINT32	Value, i;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RT28xxUsbMlmeRadioOFF()\n"));
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+		return;
+
+	// Set LED
+	RTMPSetLED(pAd, LED_RADIO_OFF);
+	// Set Radio off flag
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Link down first if any association exists
+		if (INFRA_ON(pAd) || ADHOC_ON(pAd))
+			LinkDown(pAd, FALSE);
+		RTMPusecDelay(10000);
+
+		//==========================================
+		// Clean up old bss table
+		BssTableInit(&pAd->ScanTab);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+	// Disable MAC Tx/Rx
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+	Value &= (0xfffffff3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+	// MAC_SYS_CTRL => value = 0x0 => 40mA
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0);
+
+	// PWR_PIN_CFG => value = 0x0 => 40mA
+	RTMP_IO_WRITE32(pAd, PWR_PIN_CFG, 0);
+
+	// TX_PIN_CFG => value = 0x0 => 20mA
+	RTMP_IO_WRITE32(pAd, TX_PIN_CFG, 0);
+
+	if (pAd->CommonCfg.BBPCurrentBW == BW_40)
+	{
+		// Must using 40MHz.
+		AsicTurnOffRFClk(pAd, pAd->CommonCfg.CentralChannel);
+	}
+	else
+	{
+		// Must using 20MHz.
+		AsicTurnOffRFClk(pAd, pAd->CommonCfg.Channel);
+	}
+
+	// Waiting for DMA idle
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0) && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		RTMPusecDelay(1000);
+	}while (i++ < 100);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		AsicSendCommandToMcu(pAd, 0x30, 0xff, 0xff, 0x02);
+#endif // CONFIG_STA_SUPPORT //
+}
+
diff --git a/drivers/staging/rt2870/common/cmm_info.c b/drivers/staging/rt2870/common/cmm_info.c
new file mode 100644
index 000000000000..47a1b1a73f09
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_info.c
@@ -0,0 +1,3712 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+
+#include "../rt_config.h"
+
+INT	Show_SSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_TxBurst_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_TxPreamble_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_BGProtection_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_RTSThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_FragThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+#ifdef DOT11_N_SUPPORT
+INT	Show_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // DOT11_N_SUPPORT //
+
+INT	Show_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_CountryCode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+#ifdef AGGREGATION_SUPPORT
+INT	Show_PktAggregate_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // AGGREGATION_SUPPORT //
+
+#ifdef WMM_SUPPORT
+INT	Show_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // WMM_SUPPORT //
+
+INT	Show_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+#ifdef CONFIG_STA_SUPPORT
+INT	Show_NetworkType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // CONFIG_STA_SUPPORT //
+
+INT	Show_AuthMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_EncrypType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_DefaultKeyID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_WPAPSK_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+static struct {
+	CHAR *name;
+	INT (*show_proc)(PRTMP_ADAPTER pAdapter, PUCHAR arg);
+} *PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC, RTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC[] = {
+	{"SSID",					Show_SSID_Proc},
+	{"WirelessMode",			Show_WirelessMode_Proc},
+	{"TxBurst",					Show_TxBurst_Proc},
+	{"TxPreamble",				Show_TxPreamble_Proc},
+	{"TxPower",					Show_TxPower_Proc},
+	{"Channel",					Show_Channel_Proc},
+	{"BGProtection",			Show_BGProtection_Proc},
+	{"RTSThreshold",			Show_RTSThreshold_Proc},
+	{"FragThreshold",			Show_FragThreshold_Proc},
+#ifdef DOT11_N_SUPPORT
+	{"HtBw",					Show_HtBw_Proc},
+	{"HtMcs",					Show_HtMcs_Proc},
+	{"HtGi",					Show_HtGi_Proc},
+	{"HtOpMode",				Show_HtOpMode_Proc},
+	{"HtExtcha",				Show_HtExtcha_Proc},
+	{"HtMpduDensity",			Show_HtMpduDensity_Proc},
+	{"HtBaWinSize",		        Show_HtBaWinSize_Proc},
+	{"HtRdg",		        	Show_HtRdg_Proc},
+	{"HtAmsdu",		        	Show_HtAmsdu_Proc},
+	{"HtAutoBa",		        Show_HtAutoBa_Proc},
+#endif // DOT11_N_SUPPORT //
+	{"CountryRegion",			Show_CountryRegion_Proc},
+	{"CountryRegionABand",		Show_CountryRegionABand_Proc},
+	{"CountryCode",				Show_CountryCode_Proc},
+#ifdef AGGREGATION_SUPPORT
+	{"PktAggregate",			Show_PktAggregate_Proc},
+#endif
+
+#ifdef WMM_SUPPORT
+	{"WmmCapable",				Show_WmmCapable_Proc},
+#endif
+	{"IEEE80211H",				Show_IEEE80211H_Proc},
+#ifdef CONFIG_STA_SUPPORT
+    {"NetworkType",				Show_NetworkType_Proc},
+#endif // CONFIG_STA_SUPPORT //
+	{"AuthMode",				Show_AuthMode_Proc},
+	{"EncrypType",				Show_EncrypType_Proc},
+	{"DefaultKeyID",			Show_DefaultKeyID_Proc},
+	{"Key1",					Show_Key1_Proc},
+	{"Key2",					Show_Key2_Proc},
+	{"Key3",					Show_Key3_Proc},
+	{"Key4",					Show_Key4_Proc},
+	{"WPAPSK",					Show_WPAPSK_Proc},
+	{NULL, NULL}
+};
+
+/*
+    ==========================================================================
+    Description:
+        Get Driver version.
+
+    Return:
+    ==========================================================================
+*/
+INT Set_DriverVersion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		DBGPRINT(RT_DEBUG_TRACE, ("Driver version-%s\n", STA_DRIVER_VERSION));
+#endif // CONFIG_STA_SUPPORT //
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Country Region.
+        This command will not work, if the field of CountryRegion in eeprom is programmed.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG region;
+
+	region = simple_strtol(arg, 0, 10);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	return -EOPNOTSUPP;
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// Country can be set only when EEPROM not programmed
+	if (pAd->CommonCfg.CountryRegion & 0x80)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegion_Proc::parameter of CountryRegion in eeprom is programmed \n"));
+		return FALSE;
+	}
+
+	if((region >= 0) && (region <= REGION_MAXIMUM_BG_BAND))
+	{
+		pAd->CommonCfg.CountryRegion = (UCHAR) region;
+	}
+	else if (region == REGION_31_BG_BAND)
+	{
+		pAd->CommonCfg.CountryRegion = (UCHAR) region;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegion_Proc::parameters out of range\n"));
+		return FALSE;
+	}
+
+	// if set country region, driver needs to be reset
+	BuildChannelList(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_CountryRegion_Proc::(CountryRegion=%d)\n", pAd->CommonCfg.CountryRegion));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Country Region for A band.
+        This command will not work, if the field of CountryRegion in eeprom is programmed.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG region;
+
+	region = simple_strtol(arg, 0, 10);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	return -EOPNOTSUPP;
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// Country can be set only when EEPROM not programmed
+	if (pAd->CommonCfg.CountryRegionForABand & 0x80)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegionABand_Proc::parameter of CountryRegion in eeprom is programmed \n"));
+		return FALSE;
+	}
+
+	if((region >= 0) && (region <= REGION_MAXIMUM_A_BAND))
+	{
+		pAd->CommonCfg.CountryRegionForABand = (UCHAR) region;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegionABand_Proc::parameters out of range\n"));
+		return FALSE;
+	}
+
+	// if set country region, driver needs to be reset
+	BuildChannelList(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_CountryRegionABand_Proc::(CountryRegion=%d)\n", pAd->CommonCfg.CountryRegionForABand));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Wireless Mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG	WirelessMode;
+	INT		success = TRUE;
+
+	WirelessMode = simple_strtol(arg, 0, 10);
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		INT MaxPhyMode = PHY_11G;
+
+#ifdef DOT11_N_SUPPORT
+		MaxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+		if (WirelessMode <= MaxPhyMode)
+		{
+			RTMPSetPhyMode(pAd, WirelessMode);
+#ifdef DOT11_N_SUPPORT
+			if (WirelessMode >= PHY_11ABGN_MIXED)
+			{
+				pAd->CommonCfg.BACapability.field.AutoBA = TRUE;
+				pAd->CommonCfg.REGBACapability.field.AutoBA = TRUE;
+			}
+			else
+			{
+				pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+				pAd->CommonCfg.REGBACapability.field.AutoBA = FALSE;
+			}
+#endif // DOT11_N_SUPPORT //
+			// Set AdhocMode rates
+			if (pAd->StaCfg.BssType == BSS_ADHOC)
+			{
+				MlmeUpdateTxRates(pAd, FALSE, 0);
+				MakeIbssBeacon(pAd);           // re-build BEACON frame
+				AsicEnableIbssSync(pAd);       // copy to on-chip memory
+			}
+		}
+		else
+		{
+			success = FALSE;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// it is needed to set SSID to take effect
+	if (success == TRUE)
+	{
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+		DBGPRINT(RT_DEBUG_TRACE, ("Set_WirelessMode_Proc::(=%ld)\n", WirelessMode));
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_WirelessMode_Proc::parameters out of range\n"));
+	}
+
+	return success;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Channel
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+ 	INT		success = TRUE;
+	UCHAR	Channel;
+
+	Channel = (UCHAR) simple_strtol(arg, 0, 10);
+
+	// check if this channel is valid
+	if (ChannelSanity(pAd, Channel) == TRUE)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			pAd->CommonCfg.Channel = Channel;
+
+			if (MONITOR_ON(pAd))
+			{
+#ifdef DOT11_N_SUPPORT
+				N_ChannelCheck(pAd);
+				if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+					pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40)
+				{
+					N_SetCenCh(pAd);
+					AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+					DBGPRINT(RT_DEBUG_TRACE, ("BW_40, control_channel(%d), CentralChannel(%d) \n",
+								pAd->CommonCfg.Channel, pAd->CommonCfg.CentralChannel));
+				}
+				else
+#endif // DOT11_N_SUPPORT //
+				{
+					AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+					DBGPRINT(RT_DEBUG_TRACE, ("BW_20, Channel(%d)\n", pAd->CommonCfg.Channel));
+				}
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+		success = TRUE;
+	}
+	else
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			success = FALSE;
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+	if (success == TRUE)
+		DBGPRINT(RT_DEBUG_TRACE, ("Set_Channel_Proc::(Channel=%d)\n", pAd->CommonCfg.Channel));
+
+	return success;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Short Slot Time Enable or Disable
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ShortSlot_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG ShortSlot;
+
+	ShortSlot = simple_strtol(arg, 0, 10);
+
+	if (ShortSlot == 1)
+		pAd->CommonCfg.bUseShortSlotTime = TRUE;
+	else if (ShortSlot == 0)
+		pAd->CommonCfg.bUseShortSlotTime = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_ShortSlot_Proc::(ShortSlot=%d)\n", pAd->CommonCfg.bUseShortSlotTime));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Tx power
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG TxPower;
+	INT   success = FALSE;
+
+	TxPower = (ULONG) simple_strtol(arg, 0, 10);
+	if (TxPower <= 100)
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			pAd->CommonCfg.TxPowerDefault = TxPower;
+			pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+		}
+#endif // CONFIG_STA_SUPPORT //
+		success = TRUE;
+	}
+	else
+		success = FALSE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_TxPower_Proc::(TxPowerPercentage=%ld)\n", pAd->CommonCfg.TxPowerPercentage));
+
+	return success;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set 11B/11G Protection
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_BGProtection_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	switch (simple_strtol(arg, 0, 10))
+	{
+		case 0: //AUTO
+			pAd->CommonCfg.UseBGProtection = 0;
+			break;
+		case 1: //Always On
+			pAd->CommonCfg.UseBGProtection = 1;
+			break;
+		case 2: //Always OFF
+			pAd->CommonCfg.UseBGProtection = 2;
+			break;
+		default:  //Invalid argument
+			return FALSE;
+	}
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_BGProtection_Proc::(BGProtection=%ld)\n", pAd->CommonCfg.UseBGProtection));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set TxPreamble
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_TxPreamble_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	RT_802_11_PREAMBLE	Preamble;
+
+	Preamble = simple_strtol(arg, 0, 10);
+
+
+	switch (Preamble)
+	{
+		case Rt802_11PreambleShort:
+			pAd->CommonCfg.TxPreamble = Preamble;
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				MlmeSetTxPreamble(pAd, Rt802_11PreambleShort);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+		case Rt802_11PreambleLong:
+#ifdef CONFIG_STA_SUPPORT
+		case Rt802_11PreambleAuto:
+			// if user wants AUTO, initialize to LONG here, then change according to AP's
+			// capability upon association.
+#endif // CONFIG_STA_SUPPORT //
+			pAd->CommonCfg.TxPreamble = Preamble;
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				MlmeSetTxPreamble(pAd, Rt802_11PreambleLong);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+		default: //Invalid argument
+			return FALSE;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_TxPreamble_Proc::(TxPreamble=%ld)\n", pAd->CommonCfg.TxPreamble));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set RTS Threshold
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_RTSThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	 NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+
+	RtsThresh = simple_strtol(arg, 0, 10);
+
+	if((RtsThresh > 0) && (RtsThresh <= MAX_RTS_THRESHOLD))
+		pAd->CommonCfg.RtsThreshold  = (USHORT)RtsThresh;
+#ifdef CONFIG_STA_SUPPORT
+	else if (RtsThresh == 0)
+		pAd->CommonCfg.RtsThreshold = MAX_RTS_THRESHOLD;
+#endif // CONFIG_STA_SUPPORT //
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_RTSThreshold_Proc::(RTSThreshold=%d)\n", pAd->CommonCfg.RtsThreshold));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Fragment Threshold
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_FragThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	 NDIS_802_11_FRAGMENTATION_THRESHOLD     FragThresh;
+
+	FragThresh = simple_strtol(arg, 0, 10);
+
+	if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+	{
+		//Illegal FragThresh so we set it to default
+		pAd->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+	}
+	else if (FragThresh % 2 == 1)
+	{
+		// The length of each fragment shall always be an even number of octets, except for the last fragment
+		// of an MSDU or MMPDU, which may be either an even or an odd number of octets.
+		pAd->CommonCfg.FragmentThreshold = (USHORT)(FragThresh - 1);
+	}
+	else
+	{
+		pAd->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->CommonCfg.FragmentThreshold == MAX_FRAG_THRESHOLD)
+			pAd->CommonCfg.bUseZeroToDisableFragment = TRUE;
+		else
+			pAd->CommonCfg.bUseZeroToDisableFragment = FALSE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_FragThreshold_Proc::(FragThreshold=%d)\n", pAd->CommonCfg.FragmentThreshold));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set TxBurst
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_TxBurst_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG TxBurst;
+
+	TxBurst = simple_strtol(arg, 0, 10);
+	if (TxBurst == 1)
+		pAd->CommonCfg.bEnableTxBurst = TRUE;
+	else if (TxBurst == 0)
+		pAd->CommonCfg.bEnableTxBurst = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_TxBurst_Proc::(TxBurst=%d)\n", pAd->CommonCfg.bEnableTxBurst));
+
+	return TRUE;
+}
+
+#ifdef AGGREGATION_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set TxBurst
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_PktAggregate_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG aggre;
+
+	aggre = simple_strtol(arg, 0, 10);
+
+	if (aggre == 1)
+		pAd->CommonCfg.bAggregationCapable = TRUE;
+	else if (aggre == 0)
+		pAd->CommonCfg.bAggregationCapable = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_PktAggregate_Proc::(AGGRE=%d)\n", pAd->CommonCfg.bAggregationCapable));
+
+	return TRUE;
+}
+#endif
+
+/*
+    ==========================================================================
+    Description:
+        Set IEEE80211H.
+        This parameter is 1 when needs radar detection, otherwise 0
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    ULONG ieee80211h;
+
+	ieee80211h = simple_strtol(arg, 0, 10);
+
+	if (ieee80211h == 1)
+		pAd->CommonCfg.bIEEE80211H = TRUE;
+	else if (ieee80211h == 0)
+		pAd->CommonCfg.bIEEE80211H = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_IEEE80211H_Proc::(IEEE80211H=%d)\n", pAd->CommonCfg.bIEEE80211H));
+
+	return TRUE;
+}
+
+
+#ifdef DBG
+/*
+    ==========================================================================
+    Description:
+        For Debug information
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_Debug_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("==> Set_Debug_Proc *******************\n"));
+
+    if(simple_strtol(arg, 0, 10) <= RT_DEBUG_LOUD)
+        RTDebugLevel = simple_strtol(arg, 0, 10);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== Set_Debug_Proc(RTDebugLevel = %ld)\n", RTDebugLevel));
+
+	return TRUE;
+}
+#endif
+
+INT	Show_DescInfo_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Reset statistics counter
+
+    Arguments:
+        pAdapter            Pointer to our adapter
+        arg
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ResetStatCounter_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	//UCHAR           i;
+	//MAC_TABLE_ENTRY *pEntry;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==>Set_ResetStatCounter_Proc\n"));
+
+	// add the most up-to-date h/w raw counters into software counters
+	NICUpdateRawCounters(pAd);
+
+	NdisZeroMemory(&pAd->WlanCounters, sizeof(COUNTER_802_11));
+	NdisZeroMemory(&pAd->Counters8023, sizeof(COUNTER_802_3));
+	NdisZeroMemory(&pAd->RalinkCounters, sizeof(COUNTER_RALINK));
+
+	// Reset HotSpot counter
+#if 0 // ToDo.
+	for (i = 0; i < MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		pEntry = &pAd->MacTab.Content[i];
+
+		if ((pEntry->Valid == FALSE) || (pEntry->Sst != SST_ASSOC))
+			continue;
+
+		pEntry->HSCounter.LastDataPacketTime = 0;
+		pEntry->HSCounter.TotalRxByteCount= 0;
+		pEntry->HSCounter.TotalTxByteCount= 0;
+	}
+#endif
+
+
+	return TRUE;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Add WPA key process.
+		In Adhoc WPANONE, bPairwise = 0;  KeyIdx = 0;
+
+	Arguments:
+		pAd 					Pointer to our adapter
+		pBuf							Pointer to the where the key stored
+
+	Return Value:
+		NDIS_SUCCESS					Add key successfully
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+#if 0 // remove by AlbertY
+NDIS_STATUS RTMPWPAAddKeyProc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PVOID			pBuf)
+{
+	PNDIS_802_11_KEY	pKey;
+	ULONG				KeyIdx;
+//	NDIS_STATUS 		Status;
+//	ULONG 	offset;	// unused variable, snowpin 2006.07.13
+
+	PUCHAR		pTxMic, pRxMic;
+	BOOLEAN 	bTxKey; 		// Set the key as transmit key
+	BOOLEAN 	bPairwise;		// Indicate the key is pairwise key
+	BOOLEAN 	bKeyRSC;		// indicate the receive  SC set by KeyRSC value.
+								// Otherwise, it will set by the NIC.
+	BOOLEAN 	bAuthenticator; // indicate key is set by authenticator.
+	UCHAR		apidx = BSS0;
+
+	pKey = (PNDIS_802_11_KEY) pBuf;
+	KeyIdx = pKey->KeyIndex & 0xff;
+	// Bit 31 of Add-key, Tx Key
+	bTxKey		   = (pKey->KeyIndex & 0x80000000) ? TRUE : FALSE;
+	// Bit 30 of Add-key PairwiseKey
+	bPairwise	   = (pKey->KeyIndex & 0x40000000) ? TRUE : FALSE;
+	// Bit 29 of Add-key KeyRSC
+	bKeyRSC 	   = (pKey->KeyIndex & 0x20000000) ? TRUE : FALSE;
+	// Bit 28 of Add-key Authenticator
+	bAuthenticator = (pKey->KeyIndex & 0x10000000) ? TRUE : FALSE;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPWPAAddKeyProc==>pKey->KeyIndex = %x. bPairwise= %d\n", pKey->KeyIndex, bPairwise));
+	// 1. Check Group / Pairwise Key
+	if (bPairwise)	// Pairwise Key
+	{
+		// 1. KeyIdx must be 0, otherwise, return NDIS_STATUS_INVALID_DATA
+		if (KeyIdx != 0)
+			return(NDIS_STATUS_INVALID_DATA);
+
+		// 2. Check bTx, it must be true, otherwise, return NDIS_STATUS_INVALID_DATA
+		if (bTxKey == FALSE)
+			return(NDIS_STATUS_INVALID_DATA);
+
+		// 3. If BSSID is all 0xff, return NDIS_STATUS_INVALID_DATA
+		if (MAC_ADDR_EQUAL(pKey->BSSID, BROADCAST_ADDR))
+			return(NDIS_STATUS_INVALID_DATA);
+
+		// 3.1 Check Pairwise key length for TKIP key. For AES, it's always 128 bits
+		//if ((pAdapter->PortCfg.WepStatus == Ndis802_11Encryption2Enabled) && (pKey->KeyLength != LEN_TKIP_KEY))
+		if ((pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled) && (pKey->KeyLength != LEN_TKIP_KEY))
+			return(NDIS_STATUS_INVALID_DATA);
+
+		pAd->SharedKey[apidx][KeyIdx].Type = PAIRWISE_KEY;
+
+		if (pAd->ApCfg.MBSSID[apidx].AuthMode == Ndis802_11AuthModeWPA2)
+		{
+			// Send media specific event to start PMKID caching
+			RTMPIndicateWPA2Status(pAd);
+		}
+	}
+	else
+	{
+		// 1. Check BSSID, if not current BSSID or Bcast, return NDIS_STATUS_INVALID_DATA
+		if ((! MAC_ADDR_EQUAL(pKey->BSSID, BROADCAST_ADDR)) &&
+			(! MAC_ADDR_EQUAL(pKey->BSSID, pAd->ApCfg.MBSSID[apidx].Bssid)))
+			return(NDIS_STATUS_INVALID_DATA);
+
+		// 2. Check Key index for supported Group Key
+		if (KeyIdx >= GROUP_KEY_NUM)
+			return(NDIS_STATUS_INVALID_DATA);
+
+		// 3. Set as default Tx Key if bTxKey is TRUE
+		if (bTxKey == TRUE)
+			pAd->ApCfg.MBSSID[apidx].DefaultKeyId = (UCHAR) KeyIdx;
+
+		pAd->SharedKey[apidx][KeyIdx].Type = GROUP_KEY;
+	}
+
+	// 4. Select RxMic / TxMic based on Supp / Authenticator
+	if (pAd->ApCfg.MBSSID[apidx].AuthMode == Ndis802_11AuthModeWPANone)
+	{
+		// for WPA-None Tx, Rx MIC is the same
+		pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+		pRxMic = pTxMic;
+	}
+	else if (bAuthenticator == TRUE)
+	{
+		pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+		pRxMic = (PUCHAR) (&pKey->KeyMaterial) + 24;
+	}
+	else
+	{
+		pRxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+		pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 24;
+	}
+
+	// 6. Check RxTsc
+	if (bKeyRSC == TRUE)
+	{
+		NdisMoveMemory(pAd->SharedKey[apidx][KeyIdx].RxTsc, &pKey->KeyRSC, 6);
+		NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].PairwiseKey.RxTsc, &pKey->KeyRSC, 6);
+	}
+	else
+	{
+		NdisZeroMemory(pAd->SharedKey[apidx][KeyIdx].RxTsc, 6);
+	}
+
+	// 7. Copy information into Pairwise Key structure.
+	// pKey->KeyLength will include TxMic and RxMic, therefore, we use 16 bytes hardcoded.
+	pAd->SharedKey[apidx][KeyIdx].KeyLen = (UCHAR) pKey->KeyLength;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.KeyLen = (UCHAR)pKey->KeyLength;
+	NdisMoveMemory(pAd->SharedKey[BSS0][KeyIdx].Key, &pKey->KeyMaterial, 16);
+	NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].PairwiseKey.Key, &pKey->KeyMaterial, 16);
+	if (pKey->KeyLength == LEN_TKIP_KEY)
+	{
+		// Only Key lenth equal to TKIP key have these
+		NdisMoveMemory(pAd->SharedKey[apidx][KeyIdx].RxMic, pRxMic, 8);
+		NdisMoveMemory(pAd->SharedKey[apidx][KeyIdx].TxMic, pTxMic, 8);
+		NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].PairwiseKey.RxMic, pRxMic, 8);
+		NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxMic, pTxMic, 8);
+	}
+
+	COPY_MAC_ADDR(pAd->SharedKey[BSS0][KeyIdx].BssId, pKey->BSSID);
+
+	// Init TxTsc to one based on WiFi WPA specs
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[0] = 1;
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[1] = 0;
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[2] = 0;
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[3] = 0;
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[4] = 0;
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[5] = 0;
+	// 4. Init TxTsc to one based on WiFi WPA specs
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[0] = 1;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[1] = 0;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[2] = 0;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[3] = 0;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[4] = 0;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[5] = 0;
+
+	if (pAd->ApCfg.MBSSID[apidx].WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_AES;
+		pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_AES;
+	}
+	else if (pAd->ApCfg.MBSSID[apidx].WepStatus == Ndis802_11Encryption2Enabled)
+	{
+		pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_TKIP;
+		pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_TKIP;
+	}
+	else if (pAd->ApCfg.MBSSID[apidx].WepStatus == Ndis802_11Encryption1Enabled)
+	{
+		if (pAd->SharedKey[apidx][KeyIdx].KeyLen == 5)
+		{
+			pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_WEP64;
+			pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_WEP64;
+		}
+		else if (pAd->SharedKey[apidx][KeyIdx].KeyLen == 13)
+		{
+			pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_WEP128;
+			pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_WEP128;
+		}
+		else
+		{
+			pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_NONE;
+			pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_NONE;
+		}
+	}
+	else
+	{
+		pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_NONE;
+		pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_NONE;
+	}
+
+	if ((pAd->OpMode == OPMODE_STA))  // Pairwise Key. Add BSSID to WCTable
+	{
+		pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+		pAd->MacTab.Content[BSSID_WCID].PairwiseKey.KeyLen = pAd->SharedKey[BSS0][KeyIdx].KeyLen;
+	}
+
+	if ((pAd->ApCfg.MBSSID[apidx].AuthMode == Ndis802_11AuthModeWPA2) ||
+		(pAd->ApCfg.MBSSID[apidx].AuthMode == Ndis802_11AuthModeWPA2PSK))
+	{
+		//
+		// On WPA2, Update Group Key Cipher.
+		//
+		if (!bPairwise)
+		{
+			if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+				pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_AES;
+			else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+				pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_TKIP;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("pAd->SharedKey[%d][%d].CipherAlg = %d\n", apidx, KeyIdx, pAd->SharedKey[apidx][KeyIdx].CipherAlg));
+
+#if 0
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("%s Key #%d", CipherName[pAd->SharedKey[apidx][KeyIdx].CipherAlg],KeyIdx));
+	for (i = 0; i < 16; i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("%02x:", pAd->SharedKey[apidx][KeyIdx].Key[i]));
+	}
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("\n	  Rx MIC Key = "));
+	for (i = 0; i < 8; i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("%02x:", pAd->SharedKey[apidx][KeyIdx].RxMic[i]));
+	}
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("\n	  Tx MIC Key = "));
+	for (i = 0; i < 8; i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("%02x:", pAd->SharedKey[apidx][KeyIdx].TxMic[i]));
+	}
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("\n	  RxTSC = "));
+	for (i = 0; i < 6; i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("%02x:", pAd->SharedKey[apidx][KeyIdx].RxTsc[i]));
+	}
+#endif
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("\n pKey-> BSSID:%02x:%02x:%02x:%02x:%02x:%02x \n",
+		pKey->BSSID[0],pKey->BSSID[1],pKey->BSSID[2],pKey->BSSID[3],pKey->BSSID[4],pKey->BSSID[5]));
+
+	if ((bTxKey) && (pAd->OpMode == OPMODE_STA))  // Pairwise Key. Add BSSID to WCTable
+		RTMPAddBSSIDCipher(pAd, BSSID_WCID, pKey, pAd->SharedKey[BSS0][KeyIdx].CipherAlg);
+
+
+	// No matter pairwise key or what leyidx is, always has a copy at on-chip SharedKeytable.
+	AsicAddSharedKeyEntry(pAd,
+						  apidx,
+						  (UCHAR)KeyIdx,
+						  pAd->SharedKey[apidx][KeyIdx].CipherAlg,
+						  pAd->SharedKey[apidx][KeyIdx].Key,
+						  pAd->SharedKey[apidx][KeyIdx].TxMic,
+						  pAd->SharedKey[apidx][KeyIdx].RxMic);
+
+	// The WCID key specified in used at Tx. For STA, always use pairwise key.
+
+	// ad-hoc mode need to specify WAP Group key with WCID index=BSS0Mcast_WCID. Let's always set this key here.
+/*	if (bPairwise == FALSE)
+	{
+		offset = MAC_IVEIV_TABLE_BASE + (BSS0Mcast_WCID * HW_IVEIV_ENTRY_SIZE);
+		NdisZeroMemory(IVEIV, 8);
+		// 1. IV/EIV
+		// Specify key index to find shared key.
+		if ((pAd->SharedKey[BSS0][KeyIdx].CipherAlg==CIPHER_TKIP) ||
+			(pAd->SharedKey[BSS0][KeyIdx].CipherAlg==CIPHER_AES))
+		IVEIV[3] = 0x20;		// Eiv bit on. keyid always 0 for pairwise key
+		IVEIV[3] |= (KeyIdx<< 6);	// groupkey index is not 0
+		for (i=0; i<8; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset+i, IVEIV[i]);
+		}
+
+		// 2. WCID Attribute UDF:3, BSSIdx:3, Alg:3, Keytable:use share key, BSSIdx is 0
+		WCIDAttri = (pAd->SharedKey[BSS0][KeyIdx].CipherAlg<<1)|PAIRWISEKEYTABLE;
+		offset = MAC_WCID_ATTRIBUTE_BASE + (BSS0Mcast_WCID* HW_WCID_ATTRI_SIZE);
+		RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+
+	}
+
+*/
+
+   if (pAd->SharedKey[apidx][KeyIdx].Type == GROUP_KEY)
+	{
+		// 802.1x port control
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+		DBGPRINT(RT_DEBUG_TRACE,("!!WPA_802_1X_PORT_SECURED!!\n"));
+
+	}
+
+	return (NDIS_STATUS_SUCCESS);
+}
+#endif
+
+BOOLEAN RTMPCheckStrPrintAble(
+    IN  CHAR *pInPutStr,
+    IN  UCHAR strLen)
+{
+    UCHAR i=0;
+
+    for (i=0; i<strLen; i++)
+    {
+        if ((pInPutStr[i] < 0x21) ||
+            (pInPutStr[i] > 0x7E))
+            return FALSE;
+    }
+
+    return TRUE;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Remove WPA Key process
+
+	Arguments:
+		pAd 					Pointer to our adapter
+		pBuf							Pointer to the where the key stored
+
+	Return Value:
+		NDIS_SUCCESS					Add key successfully
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+#ifdef CONFIG_STA_SUPPORT
+VOID    RTMPSetDesiredRates(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  LONG            Rates)
+{
+    NDIS_802_11_RATES aryRates;
+
+    memset(&aryRates, 0x00, sizeof(NDIS_802_11_RATES));
+    switch (pAdapter->CommonCfg.PhyMode)
+    {
+        case PHY_11A: // A only
+            switch (Rates)
+            {
+                case 6000000: //6M
+                    aryRates[0] = 0x0c; // 6M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_0;
+                    break;
+                case 9000000: //9M
+                    aryRates[0] = 0x12; // 9M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_1;
+                    break;
+                case 12000000: //12M
+                    aryRates[0] = 0x18; // 12M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_2;
+                    break;
+                case 18000000: //18M
+                    aryRates[0] = 0x24; // 18M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_3;
+                    break;
+                case 24000000: //24M
+                    aryRates[0] = 0x30; // 24M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_4;
+                    break;
+                case 36000000: //36M
+                    aryRates[0] = 0x48; // 36M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_5;
+                    break;
+                case 48000000: //48M
+                    aryRates[0] = 0x60; // 48M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_6;
+                    break;
+                case 54000000: //54M
+                    aryRates[0] = 0x6c; // 54M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_7;
+                    break;
+                case -1: //Auto
+                default:
+                    aryRates[0] = 0x6c; // 54Mbps
+                    aryRates[1] = 0x60; // 48Mbps
+                    aryRates[2] = 0x48; // 36Mbps
+                    aryRates[3] = 0x30; // 24Mbps
+                    aryRates[4] = 0x24; // 18M
+                    aryRates[5] = 0x18; // 12M
+                    aryRates[6] = 0x12; // 9M
+                    aryRates[7] = 0x0c; // 6M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+                    break;
+            }
+            break;
+        case PHY_11BG_MIXED: // B/G Mixed
+        case PHY_11B: // B only
+        case PHY_11ABG_MIXED: // A/B/G Mixed
+        default:
+            switch (Rates)
+            {
+                case 1000000: //1M
+                    aryRates[0] = 0x02;
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_0;
+                    break;
+                case 2000000: //2M
+                    aryRates[0] = 0x04;
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_1;
+                    break;
+                case 5000000: //5.5M
+                    aryRates[0] = 0x0b; // 5.5M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_2;
+                    break;
+                case 11000000: //11M
+                    aryRates[0] = 0x16; // 11M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_3;
+                    break;
+                case 6000000: //6M
+                    aryRates[0] = 0x0c; // 6M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_0;
+                    break;
+                case 9000000: //9M
+                    aryRates[0] = 0x12; // 9M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_1;
+                    break;
+                case 12000000: //12M
+                    aryRates[0] = 0x18; // 12M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_2;
+                    break;
+                case 18000000: //18M
+                    aryRates[0] = 0x24; // 18M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_3;
+                    break;
+                case 24000000: //24M
+                    aryRates[0] = 0x30; // 24M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_4;
+                    break;
+                case 36000000: //36M
+                    aryRates[0] = 0x48; // 36M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_5;
+                    break;
+                case 48000000: //48M
+                    aryRates[0] = 0x60; // 48M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_6;
+                    break;
+                case 54000000: //54M
+                    aryRates[0] = 0x6c; // 54M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_7;
+                    break;
+                case -1: //Auto
+                default:
+                    if (pAdapter->CommonCfg.PhyMode == PHY_11B)
+                    { //B Only
+                        aryRates[0] = 0x16; // 11Mbps
+                        aryRates[1] = 0x0b; // 5.5Mbps
+                        aryRates[2] = 0x04; // 2Mbps
+                        aryRates[3] = 0x02; // 1Mbps
+                    }
+                    else
+                    { //(B/G) Mixed or (A/B/G) Mixed
+                        aryRates[0] = 0x6c; // 54Mbps
+                        aryRates[1] = 0x60; // 48Mbps
+                        aryRates[2] = 0x48; // 36Mbps
+                        aryRates[3] = 0x30; // 24Mbps
+                        aryRates[4] = 0x16; // 11Mbps
+                        aryRates[5] = 0x0b; // 5.5Mbps
+                        aryRates[6] = 0x04; // 2Mbps
+                        aryRates[7] = 0x02; // 1Mbps
+                    }
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+                    break;
+            }
+            break;
+    }
+
+    NdisZeroMemory(pAdapter->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+    NdisMoveMemory(pAdapter->CommonCfg.DesireRate, &aryRates, sizeof(NDIS_802_11_RATES));
+    DBGPRINT(RT_DEBUG_TRACE, (" RTMPSetDesiredRates (%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x)\n",
+        pAdapter->CommonCfg.DesireRate[0],pAdapter->CommonCfg.DesireRate[1],
+        pAdapter->CommonCfg.DesireRate[2],pAdapter->CommonCfg.DesireRate[3],
+        pAdapter->CommonCfg.DesireRate[4],pAdapter->CommonCfg.DesireRate[5],
+        pAdapter->CommonCfg.DesireRate[6],pAdapter->CommonCfg.DesireRate[7] ));
+    // Changing DesiredRate may affect the MAX TX rate we used to TX frames out
+    MlmeUpdateTxRates(pAdapter, FALSE, 0);
+}
+
+NDIS_STATUS RTMPWPARemoveKeyProc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PVOID			pBuf)
+{
+	PNDIS_802_11_REMOVE_KEY pKey;
+	ULONG					KeyIdx;
+	NDIS_STATUS 			Status = NDIS_STATUS_FAILURE;
+	BOOLEAN 	bTxKey; 		// Set the key as transmit key
+	BOOLEAN 	bPairwise;		// Indicate the key is pairwise key
+	BOOLEAN 	bKeyRSC;		// indicate the receive  SC set by KeyRSC value.
+								// Otherwise, it will set by the NIC.
+	BOOLEAN 	bAuthenticator; // indicate key is set by authenticator.
+	INT 		i;
+
+	DBGPRINT(RT_DEBUG_TRACE,("---> RTMPWPARemoveKeyProc\n"));
+
+	pKey = (PNDIS_802_11_REMOVE_KEY) pBuf;
+	KeyIdx = pKey->KeyIndex & 0xff;
+	// Bit 31 of Add-key, Tx Key
+	bTxKey		   = (pKey->KeyIndex & 0x80000000) ? TRUE : FALSE;
+	// Bit 30 of Add-key PairwiseKey
+	bPairwise	   = (pKey->KeyIndex & 0x40000000) ? TRUE : FALSE;
+	// Bit 29 of Add-key KeyRSC
+	bKeyRSC 	   = (pKey->KeyIndex & 0x20000000) ? TRUE : FALSE;
+	// Bit 28 of Add-key Authenticator
+	bAuthenticator = (pKey->KeyIndex & 0x10000000) ? TRUE : FALSE;
+
+	// 1. If bTx is TRUE, return failure information
+	if (bTxKey == TRUE)
+		return(NDIS_STATUS_INVALID_DATA);
+
+	// 2. Check Pairwise Key
+	if (bPairwise)
+	{
+		// a. If BSSID is broadcast, remove all pairwise keys.
+		// b. If not broadcast, remove the pairwise specified by BSSID
+		for (i = 0; i < SHARE_KEY_NUM; i++)
+		{
+			if (MAC_ADDR_EQUAL(pAd->SharedKey[BSS0][i].BssId, pKey->BSSID))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("RTMPWPARemoveKeyProc(KeyIdx=%d)\n", i));
+				pAd->SharedKey[BSS0][i].KeyLen = 0;
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_NONE;
+				AsicRemoveSharedKeyEntry(pAd, BSS0, (UCHAR)i);
+				Status = NDIS_STATUS_SUCCESS;
+				break;
+			}
+		}
+	}
+	// 3. Group Key
+	else
+	{
+		// a. If BSSID is broadcast, remove all group keys indexed
+		// b. If BSSID matched, delete the group key indexed.
+		DBGPRINT(RT_DEBUG_TRACE,("RTMPWPARemoveKeyProc(KeyIdx=%ld)\n", KeyIdx));
+		pAd->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+		pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+		AsicRemoveSharedKeyEntry(pAd, BSS0, (UCHAR)KeyIdx);
+		Status = NDIS_STATUS_SUCCESS;
+	}
+
+	return (Status);
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	========================================================================
+
+	Routine Description:
+		Remove All WPA Keys
+
+	Arguments:
+		pAd 					Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPWPARemoveAllKeys(
+	IN	PRTMP_ADAPTER	pAd)
+{
+
+	UCHAR 	i;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPWPARemoveAllKeys(AuthMode=%d, WepStatus=%d)\n", pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus));
+
+	// For WEP/CKIP, there is no need to remove it, since WinXP won't set it again after
+	// Link up. And it will be replaced if user changed it.
+	if (pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+		return;
+
+	// For WPA-None, there is no need to remove it, since WinXP won't set it again after
+	// Link up. And it will be replaced if user changed it.
+	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		return;
+
+	// set BSSID wcid entry of the Pair-wise Key table as no-security mode
+	AsicRemovePairwiseKeyEntry(pAd, BSS0, BSSID_WCID);
+
+	// set all shared key mode as no-security.
+	for (i = 0; i < SHARE_KEY_NUM; i++)
+    {
+		DBGPRINT(RT_DEBUG_TRACE,("remove %s key #%d\n", CipherName[pAd->SharedKey[BSS0][i].CipherAlg], i));
+		NdisZeroMemory(&pAd->SharedKey[BSS0][i], sizeof(CIPHER_KEY));
+
+		AsicRemoveSharedKeyEntry(pAd, BSS0, i);
+	}
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	========================================================================
+	Routine Description:
+		Change NIC PHY mode. Re-association may be necessary. possible settings
+		include - PHY_11B, PHY_11BG_MIXED, PHY_11A, and PHY_11ABG_MIXED
+
+	Arguments:
+		pAd - Pointer to our adapter
+		phymode  -
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPSetPhyMode(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ULONG phymode)
+{
+	INT i;
+	// the selected phymode must be supported by the RF IC encoded in E2PROM
+
+	// if no change, do nothing
+	/* bug fix
+	if (pAd->CommonCfg.PhyMode == phymode)
+		return;
+    */
+	pAd->CommonCfg.PhyMode = (UCHAR)phymode;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPSetPhyMode : PhyMode=%d, channel=%d \n", pAd->CommonCfg.PhyMode, pAd->CommonCfg.Channel));
+#ifdef EXT_BUILD_CHANNEL_LIST
+	BuildChannelListEx(pAd);
+#else
+	BuildChannelList(pAd);
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// sanity check user setting
+	for (i = 0; i < pAd->ChannelListNum; i++)
+	{
+		if (pAd->CommonCfg.Channel == pAd->ChannelList[i].Channel)
+			break;
+	}
+
+	if (i == pAd->ChannelListNum)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			pAd->CommonCfg.Channel = FirstChannel(pAd);
+#endif // CONFIG_STA_SUPPORT //
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSetPhyMode: channel is out of range, use first channel=%d \n", pAd->CommonCfg.Channel));
+	}
+
+	NdisZeroMemory(pAd->CommonCfg.SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+	NdisZeroMemory(pAd->CommonCfg.ExtRate, MAX_LEN_OF_SUPPORTED_RATES);
+	NdisZeroMemory(pAd->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+	switch (phymode) {
+		case PHY_11B:
+			pAd->CommonCfg.SupRate[0]  = 0x82;	  // 1 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[1]  = 0x84;	  // 2 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[2]  = 0x8B;	  // 5.5 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[3]  = 0x96;	  // 11 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRateLen  = 4;
+			pAd->CommonCfg.ExtRateLen  = 0;
+			pAd->CommonCfg.DesireRate[0]  = 2;	   // 1 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[1]  = 4;	   // 2 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[2]  = 11;    // 5.5 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[3]  = 22;    // 11 mbps, in units of 0.5 Mbps
+			//pAd->CommonCfg.HTPhyMode.field.MODE = MODE_CCK; // This MODE is only FYI. not use
+			break;
+
+		case PHY_11G:
+		case PHY_11BG_MIXED:
+		case PHY_11ABG_MIXED:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11N_2_4G:
+		case PHY_11ABGN_MIXED:
+		case PHY_11BGN_MIXED:
+		case PHY_11GN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			pAd->CommonCfg.SupRate[0]  = 0x82;	  // 1 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[1]  = 0x84;	  // 2 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[2]  = 0x8B;	  // 5.5 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[3]  = 0x96;	  // 11 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[4]  = 0x12;	  // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[5]  = 0x24;	  // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[6]  = 0x48;	  // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[7]  = 0x6c;	  // 54 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRateLen  = 8;
+			pAd->CommonCfg.ExtRate[0]  = 0x0C;	  // 6 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRate[1]  = 0x18;	  // 12 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRate[2]  = 0x30;	  // 24 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRate[3]  = 0x60;	  // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRateLen  = 4;
+			pAd->CommonCfg.DesireRate[0]  = 2;	   // 1 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[1]  = 4;	   // 2 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[2]  = 11;    // 5.5 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[3]  = 22;    // 11 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[4]  = 12;    // 6 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[5]  = 18;    // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[6]  = 24;    // 12 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[7]  = 36;    // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[8]  = 48;    // 24 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[9]  = 72;    // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[10] = 96;    // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[11] = 108;   // 54 mbps, in units of 0.5 Mbps
+			break;
+
+		case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11AN_MIXED:
+		case PHY_11AGN_MIXED:
+		case PHY_11N_5G:
+#endif // DOT11_N_SUPPORT //
+			pAd->CommonCfg.SupRate[0]  = 0x8C;	  // 6 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[1]  = 0x12;	  // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[2]  = 0x98;	  // 12 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[3]  = 0x24;	  // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[4]  = 0xb0;	  // 24 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[5]  = 0x48;	  // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[6]  = 0x60;	  // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[7]  = 0x6c;	  // 54 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRateLen  = 8;
+			pAd->CommonCfg.ExtRateLen  = 0;
+			pAd->CommonCfg.DesireRate[0]  = 12;    // 6 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[1]  = 18;    // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[2]  = 24;    // 12 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[3]  = 36;    // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[4]  = 48;    // 24 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[5]  = 72;    // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[6]  = 96;    // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[7]  = 108;   // 54 mbps, in units of 0.5 Mbps
+			//pAd->CommonCfg.HTPhyMode.field.MODE = MODE_OFDM; // This MODE is only FYI. not use
+			break;
+
+		default:
+			break;
+	}
+
+
+	pAd->CommonCfg.BandState = UNKNOWN_BAND;
+}
+
+
+#ifdef DOT11_N_SUPPORT
+/*
+	========================================================================
+	Routine Description:
+		Caller ensures we has 802.11n support.
+		Calls at setting HT from AP/STASetinformation
+
+	Arguments:
+		pAd - Pointer to our adapter
+		phymode  -
+
+	========================================================================
+*/
+VOID	RTMPSetHT(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	OID_SET_HT_PHYMODE *pHTPhyMode)
+{
+	//ULONG	*pmcs;
+	UINT32	Value = 0;
+	UCHAR	BBPValue = 0;
+	UCHAR	BBP3Value = 0;
+	UCHAR	RxStream = pAd->CommonCfg.RxStream;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetHT : HT_mode(%d), ExtOffset(%d), MCS(%d), BW(%d), STBC(%d), SHORTGI(%d)\n",
+										pHTPhyMode->HtMode, pHTPhyMode->ExtOffset,
+										pHTPhyMode->MCS, pHTPhyMode->BW,
+										pHTPhyMode->STBC, pHTPhyMode->SHORTGI));
+
+	// Don't zero supportedHyPhy structure.
+	RTMPZeroMemory(&pAd->CommonCfg.HtCapability, sizeof(pAd->CommonCfg.HtCapability));
+	RTMPZeroMemory(&pAd->CommonCfg.AddHTInfo, sizeof(pAd->CommonCfg.AddHTInfo));
+	RTMPZeroMemory(&pAd->CommonCfg.NewExtChanOffset, sizeof(pAd->CommonCfg.NewExtChanOffset));
+	RTMPZeroMemory(&pAd->CommonCfg.DesiredHtPhy, sizeof(pAd->CommonCfg.DesiredHtPhy));
+
+   	if (pAd->CommonCfg.bRdg)
+	{
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.PlusHTC = 1;
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.RDGSupport = 1;
+	}
+	else
+	{
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.PlusHTC = 0;
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.RDGSupport = 0;
+	}
+
+	pAd->CommonCfg.HtCapability.HtCapParm.MaxRAmpduFactor = 3;
+	pAd->CommonCfg.DesiredHtPhy.MaxRAmpduFactor = 3;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetHT : RxBAWinLimit = %d\n", pAd->CommonCfg.BACapability.field.RxBAWinLimit));
+
+	// Mimo power save, A-MSDU size,
+	pAd->CommonCfg.DesiredHtPhy.AmsduEnable = (USHORT)pAd->CommonCfg.BACapability.field.AmsduEnable;
+	pAd->CommonCfg.DesiredHtPhy.AmsduSize = (UCHAR)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.DesiredHtPhy.MimoPs = (UCHAR)pAd->CommonCfg.BACapability.field.MMPSmode;
+	pAd->CommonCfg.DesiredHtPhy.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+
+	pAd->CommonCfg.HtCapability.HtCapInfo.AMsduSize = (USHORT)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.HtCapability.HtCapInfo.MimoPs = (USHORT)pAd->CommonCfg.BACapability.field.MMPSmode;
+	pAd->CommonCfg.HtCapability.HtCapParm.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetHT : AMsduSize = %d, MimoPs = %d, MpduDensity = %d, MaxRAmpduFactor = %d\n",
+													pAd->CommonCfg.DesiredHtPhy.AmsduSize,
+													pAd->CommonCfg.DesiredHtPhy.MimoPs,
+													pAd->CommonCfg.DesiredHtPhy.MpduDensity,
+													pAd->CommonCfg.DesiredHtPhy.MaxRAmpduFactor));
+
+	if(pHTPhyMode->HtMode == HTMODE_GF)
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.GF = 1;
+		pAd->CommonCfg.DesiredHtPhy.GF = 1;
+	}
+	else
+		pAd->CommonCfg.DesiredHtPhy.GF = 0;
+
+	// Decide Rx MCSSet
+	switch (RxStream)
+	{
+		case 1:
+			pAd->CommonCfg.HtCapability.MCSSet[0] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[1] =  0x00;
+			break;
+
+		case 2:
+			pAd->CommonCfg.HtCapability.MCSSet[0] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[1] =  0xff;
+			break;
+
+		case 3: // 3*3
+			pAd->CommonCfg.HtCapability.MCSSet[0] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[1] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[2] =  0xff;
+			break;
+	}
+
+	if (pAd->CommonCfg.bForty_Mhz_Intolerant && (pAd->CommonCfg.Channel <= 14) && (pHTPhyMode->BW == BW_40) )
+	{
+		pHTPhyMode->BW = BW_20;
+		pAd->CommonCfg.HtCapability.HtCapInfo.Forty_Mhz_Intolerant = 1;
+	}
+
+	if(pHTPhyMode->BW == BW_40)
+	{
+		pAd->CommonCfg.HtCapability.MCSSet[4] = 0x1; // MCS 32
+		pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth = 1;
+		if (pAd->CommonCfg.Channel <= 14)
+			pAd->CommonCfg.HtCapability.HtCapInfo.CCKmodein40 = 1;
+
+		pAd->CommonCfg.DesiredHtPhy.ChannelWidth = 1;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth = 1;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset = (pHTPhyMode->ExtOffset == EXTCHA_BELOW)? (EXTCHA_BELOW): EXTCHA_ABOVE;
+		// Set Regsiter for extension channel position.
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBP3Value);
+		if ((pHTPhyMode->ExtOffset == EXTCHA_BELOW))
+		{
+			Value |= 0x1;
+			BBP3Value |= (0x20);
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+		}
+		else if ((pHTPhyMode->ExtOffset == EXTCHA_ABOVE))
+		{
+			Value &= 0xfe;
+			BBP3Value &= (~0x20);
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+		}
+
+		// Turn on BBP 40MHz mode now only as AP .
+		// Sta can turn on BBP 40MHz after connection with 40MHz AP. Sta only broadcast 40MHz capability before connection.
+		if ((pAd->OpMode == OPMODE_AP) || INFRA_ON(pAd) || ADHOC_ON(pAd)
+			)
+		{
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue &= (~0x18);
+			BBPValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBP3Value);
+			pAd->CommonCfg.BBPCurrentBW = BW_40;
+		}
+	}
+	else
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth = 0;
+		pAd->CommonCfg.DesiredHtPhy.ChannelWidth = 0;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth = 0;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset = EXTCHA_NONE;
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+		// Turn on BBP 20MHz mode by request here.
+		{
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue &= (~0x18);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			pAd->CommonCfg.BBPCurrentBW = BW_20;
+		}
+	}
+
+	if(pHTPhyMode->STBC == STBC_USE)
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.TxSTBC = 1;
+		pAd->CommonCfg.DesiredHtPhy.TxSTBC = 1;
+		pAd->CommonCfg.HtCapability.HtCapInfo.RxSTBC = 1;
+		pAd->CommonCfg.DesiredHtPhy.RxSTBC = 1;
+	}
+	else
+	{
+		pAd->CommonCfg.DesiredHtPhy.TxSTBC = 0;
+		pAd->CommonCfg.DesiredHtPhy.RxSTBC = 0;
+	}
+
+#ifdef RT2870
+	/* Frank recommend ,If not, Tx maybe block in high power. Rx has no problem*/
+	if(IS_RT3070(pAd) && ((pAd->RfIcType == RFIC_3020) || (pAd->RfIcType == RFIC_2020)))
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.TxSTBC = 0;
+		pAd->CommonCfg.DesiredHtPhy.TxSTBC = 0;
+	}
+#endif // RT2870 //
+
+	if(pHTPhyMode->SHORTGI == GI_400)
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor20 = 1;
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor40 = 1;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor20 = 1;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor40 = 1;
+	}
+	else
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor20 = 0;
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor40 = 0;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor20 = 0;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor40 = 0;
+	}
+
+	// We support link adaptation for unsolicit MCS feedback, set to 2.
+	pAd->CommonCfg.HtCapability.ExtHtCapInfo.MCSFeedback = MCSFBK_NONE; //MCSFBK_UNSOLICIT;
+	pAd->CommonCfg.AddHTInfo.ControlChan = pAd->CommonCfg.Channel;
+	// 1, the extension channel above the control channel.
+
+	// EDCA parameters used for AP's own transmission
+	if (pAd->CommonCfg.APEdcaParm.bValid == FALSE)
+	{
+		pAd->CommonCfg.APEdcaParm.bValid = TRUE;
+		pAd->CommonCfg.APEdcaParm.Aifsn[0] = 3;
+		pAd->CommonCfg.APEdcaParm.Aifsn[1] = 7;
+		pAd->CommonCfg.APEdcaParm.Aifsn[2] = 1;
+		pAd->CommonCfg.APEdcaParm.Aifsn[3] = 1;
+
+		pAd->CommonCfg.APEdcaParm.Cwmin[0] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[1] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[2] = 3;
+		pAd->CommonCfg.APEdcaParm.Cwmin[3] = 2;
+
+		pAd->CommonCfg.APEdcaParm.Cwmax[0] = 6;
+		pAd->CommonCfg.APEdcaParm.Cwmax[1] = 10;
+		pAd->CommonCfg.APEdcaParm.Cwmax[2] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmax[3] = 3;
+
+		pAd->CommonCfg.APEdcaParm.Txop[0]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[1]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[2]  = 94;
+		pAd->CommonCfg.APEdcaParm.Txop[3]  = 47;
+	}
+	AsicSetEdcaParm(pAd, &pAd->CommonCfg.APEdcaParm);
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		RTMPSetIndividualHT(pAd, 0);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Caller ensures we has 802.11n support.
+		Calls at setting HT from AP/STASetinformation
+
+	Arguments:
+		pAd - Pointer to our adapter
+		phymode  -
+
+	========================================================================
+*/
+VOID	RTMPSetIndividualHT(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	UCHAR				apidx)
+{
+	PRT_HT_PHY_INFO		pDesired_ht_phy = NULL;
+	UCHAR	TxStream = pAd->CommonCfg.TxStream;
+	UCHAR	DesiredMcs	= MCS_AUTO;
+
+	do
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			pDesired_ht_phy = &pAd->StaCfg.DesiredHtPhyInfo;
+			DesiredMcs = pAd->StaCfg.DesiredTransmitSetting.field.MCS;
+			//pAd->StaCfg.bAutoTxRateSwitch = (DesiredMcs == MCS_AUTO) ? TRUE : FALSE;
+				break;
+		}
+#endif // CONFIG_STA_SUPPORT //
+	} while (FALSE);
+
+	if (pDesired_ht_phy == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSetIndividualHT: invalid apidx(%d)\n", apidx));
+		return;
+	}
+	RTMPZeroMemory(pDesired_ht_phy, sizeof(RT_HT_PHY_INFO));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetIndividualHT : Desired MCS = %d\n", DesiredMcs));
+	// Check the validity of MCS
+	if ((TxStream == 1) && ((DesiredMcs >= MCS_8) && (DesiredMcs <= MCS_15)))
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("RTMPSetIndividualHT: MCS(%d) is invalid in 1S, reset it as MCS_7\n", DesiredMcs));
+		DesiredMcs = MCS_7;
+	}
+
+	if ((pAd->CommonCfg.DesiredHtPhy.ChannelWidth == BW_20) && (DesiredMcs == MCS_32))
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("RTMPSetIndividualHT: MCS_32 is only supported in 40-MHz, reset it as MCS_0\n"));
+		DesiredMcs = MCS_0;
+	}
+
+	pDesired_ht_phy->bHtEnable = TRUE;
+
+	// Decide desired Tx MCS
+	switch (TxStream)
+	{
+		case 1:
+			if (DesiredMcs == MCS_AUTO)
+			{
+				pDesired_ht_phy->MCSSet[0]= 0xff;
+				pDesired_ht_phy->MCSSet[1]= 0x00;
+			}
+			else if (DesiredMcs <= MCS_7)
+			{
+				pDesired_ht_phy->MCSSet[0]= 1<<DesiredMcs;
+				pDesired_ht_phy->MCSSet[1]= 0x00;
+			}
+			break;
+
+		case 2:
+			if (DesiredMcs == MCS_AUTO)
+			{
+				pDesired_ht_phy->MCSSet[0]= 0xff;
+				pDesired_ht_phy->MCSSet[1]= 0xff;
+			}
+			else if (DesiredMcs <= MCS_15)
+			{
+				ULONG mode;
+
+				mode = DesiredMcs / 8;
+				if (mode < 2)
+					pDesired_ht_phy->MCSSet[mode] = (1 << (DesiredMcs - mode * 8));
+			}
+			break;
+
+		case 3: // 3*3
+			if (DesiredMcs == MCS_AUTO)
+			{
+				/* MCS0 ~ MCS23, 3 bytes */
+				pDesired_ht_phy->MCSSet[0]= 0xff;
+				pDesired_ht_phy->MCSSet[1]= 0xff;
+				pDesired_ht_phy->MCSSet[2]= 0xff;
+			}
+			else if (DesiredMcs <= MCS_23)
+			{
+				ULONG mode;
+
+				mode = DesiredMcs / 8;
+				if (mode < 3)
+					pDesired_ht_phy->MCSSet[mode] = (1 << (DesiredMcs - mode * 8));
+			}
+			break;
+	}
+
+	if(pAd->CommonCfg.DesiredHtPhy.ChannelWidth == BW_40)
+	{
+		if (DesiredMcs == MCS_AUTO || DesiredMcs == MCS_32)
+			pDesired_ht_phy->MCSSet[4] = 0x1;
+	}
+
+	// update HT Rate setting
+    if (pAd->OpMode == OPMODE_STA)
+        MlmeUpdateHtTxRates(pAd, BSS0);
+    else
+	    MlmeUpdateHtTxRates(pAd, apidx);
+}
+
+
+/*
+	========================================================================
+	Routine Description:
+		Update HT IE from our capability.
+
+	Arguments:
+		Send all HT IE in beacon/probe rsp/assoc rsp/action frame.
+
+
+	========================================================================
+*/
+VOID	RTMPUpdateHTIE(
+	IN	RT_HT_CAPABILITY	*pRtHt,
+	IN		UCHAR				*pMcsSet,
+	OUT		HT_CAPABILITY_IE *pHtCapability,
+	OUT		ADD_HT_INFO_IE		*pAddHtInfo)
+{
+	RTMPZeroMemory(pHtCapability, sizeof(HT_CAPABILITY_IE));
+	RTMPZeroMemory(pAddHtInfo, sizeof(ADD_HT_INFO_IE));
+
+		pHtCapability->HtCapInfo.ChannelWidth = pRtHt->ChannelWidth;
+		pHtCapability->HtCapInfo.MimoPs = pRtHt->MimoPs;
+		pHtCapability->HtCapInfo.GF = pRtHt->GF;
+		pHtCapability->HtCapInfo.ShortGIfor20 = pRtHt->ShortGIfor20;
+		pHtCapability->HtCapInfo.ShortGIfor40 = pRtHt->ShortGIfor40;
+		pHtCapability->HtCapInfo.TxSTBC = pRtHt->TxSTBC;
+		pHtCapability->HtCapInfo.RxSTBC = pRtHt->RxSTBC;
+		pHtCapability->HtCapInfo.AMsduSize = pRtHt->AmsduSize;
+		pHtCapability->HtCapParm.MaxRAmpduFactor = pRtHt->MaxRAmpduFactor;
+		pHtCapability->HtCapParm.MpduDensity = pRtHt->MpduDensity;
+
+		pAddHtInfo->AddHtInfo.ExtChanOffset = pRtHt->ExtChanOffset ;
+		pAddHtInfo->AddHtInfo.RecomWidth = pRtHt->RecomWidth;
+		pAddHtInfo->AddHtInfo2.OperaionMode = pRtHt->OperaionMode;
+		pAddHtInfo->AddHtInfo2.NonGfPresent = pRtHt->NonGfPresent;
+		RTMPMoveMemory(pAddHtInfo->MCSSet, /*pRtHt->MCSSet*/pMcsSet, 4); // rt2860 only support MCS max=32, no need to copy all 16 uchar.
+
+        DBGPRINT(RT_DEBUG_TRACE,("RTMPUpdateHTIE <== \n"));
+}
+#endif // DOT11_N_SUPPORT //
+
+/*
+	========================================================================
+	Description:
+		Add Client security information into ASIC WCID table and IVEIV table.
+    Return:
+	========================================================================
+*/
+VOID	RTMPAddWcidAttributeEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BssIdx,
+	IN 	UCHAR		 	KeyIdx,
+	IN 	UCHAR		 	CipherAlg,
+	IN 	MAC_TABLE_ENTRY *pEntry)
+{
+	UINT32		WCIDAttri = 0;
+	USHORT		offset;
+	UCHAR		IVEIV = 0;
+	USHORT		Wcid = 0;
+
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (BssIdx > BSS0)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("RTMPAddWcidAttributeEntry: The BSS-index(%d) is out of range for Infra link. \n", BssIdx));
+				return;
+			}
+
+			// 1.	In ADHOC mode, the AID is wcid number. And NO mesh link exists.
+			// 2.	In Infra mode, the AID:1 MUST be wcid of infra STA.
+			//					   the AID:2~ assign to mesh link entry.
+			if (pEntry && ADHOC_ON(pAd))
+				Wcid = pEntry->Aid;
+			else if (pEntry && INFRA_ON(pAd))
+			{
+#ifdef QOS_DLS_SUPPORT
+				if (pEntry->ValidAsDls == TRUE)
+					Wcid = pEntry->Aid;
+				else
+#endif // QOS_DLS_SUPPORT //
+				Wcid = BSSID_WCID;
+			}
+			else
+				Wcid = MCAST_WCID;
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// Update WCID attribute table
+	offset = MAC_WCID_ATTRIBUTE_BASE + (Wcid * HW_WCID_ATTRI_SIZE);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pEntry && pEntry->ValidAsMesh)
+			WCIDAttri = (CipherAlg<<1) | PAIRWISEKEYTABLE;
+#ifdef QOS_DLS_SUPPORT
+		else if ((pEntry) && (pEntry->ValidAsDls) &&
+					((CipherAlg == CIPHER_TKIP) ||
+				 	(CipherAlg == CIPHER_TKIP_NO_MIC) ||
+					(CipherAlg == CIPHER_AES) ||
+				 	(CipherAlg == CIPHER_NONE)))
+			WCIDAttri = (CipherAlg<<1) | PAIRWISEKEYTABLE;
+#endif // QOS_DLS_SUPPORT //
+		else
+			WCIDAttri = (CipherAlg<<1) | SHAREDKEYTABLE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+
+
+	// Update IV/EIV table
+	offset = MAC_IVEIV_TABLE_BASE + (Wcid * HW_IVEIV_ENTRY_SIZE);
+
+	// WPA mode
+	if ((CipherAlg == CIPHER_TKIP) || (CipherAlg == CIPHER_TKIP_NO_MIC) || (CipherAlg == CIPHER_AES))
+	{
+		// Eiv bit on. keyid always is 0 for pairwise key
+		IVEIV = (KeyIdx <<6) | 0x20;
+	}
+	else
+	{
+		// WEP KeyIdx is default tx key.
+		IVEIV = (KeyIdx << 6);
+	}
+
+	// For key index and ext IV bit, so only need to update the position(offset+3).
+#ifdef RT2870
+	RTUSBMultiWrite_OneByte(pAd, offset+3, &IVEIV);
+#endif // RT2870 //
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPAddWcidAttributeEntry: WCID #%d, KeyIndex #%d, Alg=%s\n",Wcid, KeyIdx, CipherName[CipherAlg]));
+	DBGPRINT(RT_DEBUG_TRACE,("	WCIDAttri = 0x%x \n",  WCIDAttri));
+
+}
+
+/*
+    ==========================================================================
+    Description:
+        Parse encryption type
+Arguments:
+    pAdapter                    Pointer to our adapter
+    wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+    ==========================================================================
+*/
+CHAR *GetEncryptType(CHAR enc)
+{
+    if(enc == Ndis802_11WEPDisabled)
+        return "NONE";
+    if(enc == Ndis802_11WEPEnabled)
+    	return "WEP";
+    if(enc == Ndis802_11Encryption2Enabled)
+    	return "TKIP";
+    if(enc == Ndis802_11Encryption3Enabled)
+    	return "AES";
+	if(enc == Ndis802_11Encryption4Enabled)
+    	return "TKIPAES";
+    else
+    	return "UNKNOW";
+}
+
+CHAR *GetAuthMode(CHAR auth)
+{
+    if(auth == Ndis802_11AuthModeOpen)
+    	return "OPEN";
+    if(auth == Ndis802_11AuthModeShared)
+    	return "SHARED";
+	if(auth == Ndis802_11AuthModeAutoSwitch)
+    	return "AUTOWEP";
+    if(auth == Ndis802_11AuthModeWPA)
+    	return "WPA";
+    if(auth == Ndis802_11AuthModeWPAPSK)
+    	return "WPAPSK";
+    if(auth == Ndis802_11AuthModeWPANone)
+    	return "WPANONE";
+    if(auth == Ndis802_11AuthModeWPA2)
+    	return "WPA2";
+    if(auth == Ndis802_11AuthModeWPA2PSK)
+    	return "WPA2PSK";
+	if(auth == Ndis802_11AuthModeWPA1WPA2)
+    	return "WPA1WPA2";
+	if(auth == Ndis802_11AuthModeWPA1PSKWPA2PSK)
+    	return "WPA1PSKWPA2PSK";
+
+    	return "UNKNOW";
+}
+
+#if 1 //#ifndef UCOS
+/*
+    ==========================================================================
+    Description:
+        Get site survey results
+	Arguments:
+	    pAdapter                    Pointer to our adapter
+	    wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+        		1.) UI needs to wait 4 seconds after issue a site survey command
+        		2.) iwpriv ra0 get_site_survey
+        		3.) UI needs to prepare at least 4096bytes to get the results
+    ==========================================================================
+*/
+#define	LINE_LEN	(4+33+20+8+10+9+7+3)	// Channel+SSID+Bssid+WepStatus+AuthMode+Signal+WiressMode+NetworkType
+VOID RTMPIoctlGetSiteSurvey(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR		*msg;
+	INT 		i=0;
+	INT			WaitCnt;
+	INT 		Status=0;
+	CHAR		Ssid[MAX_LEN_OF_SSID +1];
+    INT         Rssi = 0, max_len = LINE_LEN;
+	UINT        Rssi_Quality = 0;
+	NDIS_802_11_NETWORK_TYPE    wireless_mode;
+
+	os_alloc_mem(NULL, (PUCHAR *)&msg, sizeof(CHAR)*((MAX_LEN_OF_BSS_TABLE)*max_len));
+
+	if (msg == NULL)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPIoctlGetSiteSurvey - msg memory alloc fail.\n"));
+		return;
+	}
+
+	memset(msg, 0 ,(MAX_LEN_OF_BSS_TABLE)*max_len );
+	memset(Ssid, 0 ,(MAX_LEN_OF_SSID +1));
+	sprintf(msg,"%s","\n");
+	sprintf(msg+strlen(msg),"%-4s%-33s%-20s%-8s%-10s%-9s%-7s%-3s\n",
+	    "Ch", "SSID", "BSSID", "Enc", "Auth", "Siganl(%)", "W-Mode", " NT");
+
+
+	WaitCnt = 0;
+#ifdef CONFIG_STA_SUPPORT
+	pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+	while ((ScanRunning(pAdapter) == TRUE) && (WaitCnt++ < 200))
+		OS_WAIT(500);
+#endif // CONFIG_STA_SUPPORT //
+
+	for(i=0; i<pAdapter->ScanTab.BssNr ;i++)
+	{
+		if( pAdapter->ScanTab.BssEntry[i].Channel==0)
+			break;
+
+		if((strlen(msg)+max_len ) >= IW_SCAN_MAX_DATA)
+			break;
+
+		//Channel
+		sprintf(msg+strlen(msg),"%-4d", pAdapter->ScanTab.BssEntry[i].Channel);
+		//SSID
+		memcpy(Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+		Ssid[pAdapter->ScanTab.BssEntry[i].SsidLen] = '\0';
+		sprintf(msg+strlen(msg),"%-33s", Ssid);
+		//BSSID
+		sprintf(msg+strlen(msg),"%02x:%02x:%02x:%02x:%02x:%02x   ",
+			pAdapter->ScanTab.BssEntry[i].Bssid[0],
+			pAdapter->ScanTab.BssEntry[i].Bssid[1],
+			pAdapter->ScanTab.BssEntry[i].Bssid[2],
+			pAdapter->ScanTab.BssEntry[i].Bssid[3],
+			pAdapter->ScanTab.BssEntry[i].Bssid[4],
+			pAdapter->ScanTab.BssEntry[i].Bssid[5]);
+		//Encryption Type
+		sprintf(msg+strlen(msg),"%-8s",GetEncryptType(pAdapter->ScanTab.BssEntry[i].WepStatus));
+		//Authentication Mode
+		if (pAdapter->ScanTab.BssEntry[i].WepStatus == Ndis802_11WEPEnabled)
+			sprintf(msg+strlen(msg),"%-10s", "UNKNOW");
+		else
+			sprintf(msg+strlen(msg),"%-10s",GetAuthMode(pAdapter->ScanTab.BssEntry[i].AuthMode));
+		// Rssi
+		Rssi = (INT)pAdapter->ScanTab.BssEntry[i].Rssi;
+		if (Rssi >= -50)
+			Rssi_Quality = 100;
+		else if (Rssi >= -80)    // between -50 ~ -80dbm
+			Rssi_Quality = (UINT)(24 + ((Rssi + 80) * 26)/10);
+		else if (Rssi >= -90)   // between -80 ~ -90dbm
+			Rssi_Quality = (UINT)(((Rssi + 90) * 26)/10);
+		else    // < -84 dbm
+			Rssi_Quality = 0;
+		sprintf(msg+strlen(msg),"%-9d", Rssi_Quality);
+		// Wireless Mode
+		wireless_mode = NetworkTypeInUseSanity(&pAdapter->ScanTab.BssEntry[i]);
+		if (wireless_mode == Ndis802_11FH ||
+			wireless_mode == Ndis802_11DS)
+			sprintf(msg+strlen(msg),"%-7s", "11b");
+		else if (wireless_mode == Ndis802_11OFDM5)
+			sprintf(msg+strlen(msg),"%-7s", "11a");
+		else if (wireless_mode == Ndis802_11OFDM5_N)
+			sprintf(msg+strlen(msg),"%-7s", "11a/n");
+		else if (wireless_mode == Ndis802_11OFDM24)
+			sprintf(msg+strlen(msg),"%-7s", "11b/g");
+		else if (wireless_mode == Ndis802_11OFDM24_N)
+			sprintf(msg+strlen(msg),"%-7s", "11b/g/n");
+		else
+			sprintf(msg+strlen(msg),"%-7s", "unknow");
+		//Network Type
+		if (pAdapter->ScanTab.BssEntry[i].BssType == BSS_ADHOC)
+			sprintf(msg+strlen(msg),"%-3s", " Ad");
+		else
+			sprintf(msg+strlen(msg),"%-3s", " In");
+
+        sprintf(msg+strlen(msg),"\n");
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+#endif // CONFIG_STA_SUPPORT //
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPIoctlGetSiteSurvey - wrq->u.data.length = %d\n", wrq->u.data.length));
+	os_free_mem(NULL, (PUCHAR)msg);
+}
+
+
+#define	MAC_LINE_LEN	(14+4+4+10+10+10+6+6)	// Addr+aid+psm+datatime+rxbyte+txbyte+current tx rate+last tx rate
+VOID RTMPIoctlGetMacTable(
+	IN PRTMP_ADAPTER pAd,
+	IN struct iwreq *wrq)
+{
+	INT i;
+	RT_802_11_MAC_TABLE MacTab;
+	char *msg;
+
+	MacTab.Num = 0;
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		if (pAd->MacTab.Content[i].ValidAsCLI && (pAd->MacTab.Content[i].Sst == SST_ASSOC))
+		{
+			COPY_MAC_ADDR(MacTab.Entry[MacTab.Num].Addr, &pAd->MacTab.Content[i].Addr);
+			MacTab.Entry[MacTab.Num].Aid = (UCHAR)pAd->MacTab.Content[i].Aid;
+			MacTab.Entry[MacTab.Num].Psm = pAd->MacTab.Content[i].PsMode;
+#ifdef DOT11_N_SUPPORT
+			MacTab.Entry[MacTab.Num].MimoPs = pAd->MacTab.Content[i].MmpsMode;
+#endif // DOT11_N_SUPPORT //
+
+			// Fill in RSSI per entry
+			MacTab.Entry[MacTab.Num].AvgRssi0 = pAd->MacTab.Content[i].RssiSample.AvgRssi0;
+			MacTab.Entry[MacTab.Num].AvgRssi1 = pAd->MacTab.Content[i].RssiSample.AvgRssi1;
+			MacTab.Entry[MacTab.Num].AvgRssi2 = pAd->MacTab.Content[i].RssiSample.AvgRssi2;
+
+			// the connected time per entry
+			MacTab.Entry[MacTab.Num].ConnectedTime = pAd->MacTab.Content[i].StaConnectTime;
+#if 0 // ToDo
+			MacTab.Entry[MacTab.Num].HSCounter.LastDataPacketTime = pAd->MacTab.Content[i].HSCounter.LastDataPacketTime;
+			MacTab.Entry[MacTab.Num].HSCounter.TotalRxByteCount = pAd->MacTab.Content[i].HSCounter.TotalRxByteCount;
+			MacTab.Entry[MacTab.Num].HSCounter.TotalTxByteCount = pAd->MacTab.Content[i].HSCounter.TotalTxByteCount;
+#endif
+			MacTab.Entry[MacTab.Num].TxRate.field.MCS = pAd->MacTab.Content[i].HTPhyMode.field.MCS;
+			MacTab.Entry[MacTab.Num].TxRate.field.BW = pAd->MacTab.Content[i].HTPhyMode.field.BW;
+			MacTab.Entry[MacTab.Num].TxRate.field.ShortGI = pAd->MacTab.Content[i].HTPhyMode.field.ShortGI;
+			MacTab.Entry[MacTab.Num].TxRate.field.STBC = pAd->MacTab.Content[i].HTPhyMode.field.STBC;
+			MacTab.Entry[MacTab.Num].TxRate.field.rsv = pAd->MacTab.Content[i].HTPhyMode.field.rsv;
+			MacTab.Entry[MacTab.Num].TxRate.field.MODE = pAd->MacTab.Content[i].HTPhyMode.field.MODE;
+			MacTab.Entry[MacTab.Num].TxRate.word = pAd->MacTab.Content[i].HTPhyMode.word;
+
+			MacTab.Num += 1;
+		}
+	}
+	wrq->u.data.length = sizeof(RT_802_11_MAC_TABLE);
+	if (copy_to_user(wrq->u.data.pointer, &MacTab, wrq->u.data.length))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s: copy_to_user() fail\n", __func__));
+	}
+
+	msg = (CHAR *) kmalloc(sizeof(CHAR)*(MAX_LEN_OF_MAC_TABLE*MAC_LINE_LEN), MEM_ALLOC_FLAG);
+	memset(msg, 0 ,MAX_LEN_OF_MAC_TABLE*MAC_LINE_LEN );
+	sprintf(msg,"%s","\n");
+	sprintf(msg+strlen(msg),"%-14s%-4s%-4s%-10s%-10s%-10s%-6s%-6s\n",
+		"MAC", "AID", "PSM", "LDT", "RxB", "TxB","CTxR", "LTxR");
+
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+		if (pEntry->ValidAsCLI && (pEntry->Sst == SST_ASSOC))
+		{
+			if((strlen(msg)+MAC_LINE_LEN ) >= (MAX_LEN_OF_MAC_TABLE*MAC_LINE_LEN) )
+				break;
+			sprintf(msg+strlen(msg),"%02x%02x%02x%02x%02x%02x  ",
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+			sprintf(msg+strlen(msg),"%-4d", (int)pEntry->Aid);
+			sprintf(msg+strlen(msg),"%-4d", (int)pEntry->PsMode);
+			sprintf(msg+strlen(msg),"%-10d",0/*pAd->MacTab.Content[i].HSCounter.LastDataPacketTime*/); // ToDo
+			sprintf(msg+strlen(msg),"%-10d",0/*pAd->MacTab.Content[i].HSCounter.TotalRxByteCount*/); // ToDo
+			sprintf(msg+strlen(msg),"%-10d",0/*pAd->MacTab.Content[i].HSCounter.TotalTxByteCount*/); // ToDo
+			sprintf(msg+strlen(msg),"%-6d",RateIdToMbps[pAd->MacTab.Content[i].CurrTxRate]);
+			sprintf(msg+strlen(msg),"%-6d\n",0/*RateIdToMbps[pAd->MacTab.Content[i].LastTxRate]*/); // ToDo
+		}
+	}
+	// for compatible with old API just do the printk to console
+	//wrq->u.data.length = strlen(msg);
+	//if (copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s", msg));
+	}
+
+	kfree(msg);
+}
+#endif // UCOS //
+
+#ifdef DOT11_N_SUPPORT
+INT	Set_BASetup_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], tid;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+/*
+	The BASetup inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the tid value.
+*/
+	//printk("\n%s\n", arg);
+
+	if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and tid value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		tid = simple_strtol((token+1), 0, 10);
+		if (tid > 15)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+		printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x\n", mac[0], mac[1],
+				mac[2], mac[3], mac[4], mac[5], tid);
+
+	    pEntry = MacTableLookup(pAd, mac);
+
+    	if (pEntry) {
+        	printk("\nSetup BA Session: Tid = %d\n", tid);
+	        BAOriSessionSetUp(pAd, pEntry, tid, 0, 100, TRUE);
+    	}
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_BADecline_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG bBADecline;
+
+	bBADecline = simple_strtol(arg, 0, 10);
+
+	if (bBADecline == 0)
+	{
+		pAd->CommonCfg.bBADecline = FALSE;
+	}
+	else if (bBADecline == 1)
+	{
+		pAd->CommonCfg.bBADecline = TRUE;
+	}
+	else
+	{
+		return FALSE; //Invalid argument
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_BADecline_Proc::(BADecline=%d)\n", pAd->CommonCfg.bBADecline));
+
+	return TRUE;
+}
+
+INT	Set_BAOriTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], tid;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+    //printk("\n%s\n", arg);
+/*
+	The BAOriTearDown inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the tid value.
+*/
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and tid value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		tid = simple_strtol((token+1), 0, 10);
+		if (tid > NUM_OF_TID)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+	    printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x", mac[0], mac[1],
+	           mac[2], mac[3], mac[4], mac[5], tid);
+
+	    pEntry = MacTableLookup(pAd, mac);
+
+	    if (pEntry) {
+	        printk("\nTear down Ori BA Session: Tid = %d\n", tid);
+        BAOriSessionTearDown(pAd, pEntry->Aid, tid, FALSE, TRUE);
+	    }
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_BARecTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], tid;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+    //printk("\n%s\n", arg);
+/*
+	The BARecTearDown inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the tid value.
+*/
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and tid value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		tid = simple_strtol((token+1), 0, 10);
+		if (tid > NUM_OF_TID)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+		printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x", mac[0], mac[1],
+		       mac[2], mac[3], mac[4], mac[5], tid);
+
+		pEntry = MacTableLookup(pAd, mac);
+
+		if (pEntry) {
+		    printk("\nTear down Rec BA Session: Tid = %d\n", tid);
+		    BARecSessionTearDown(pAd, pEntry->Aid, tid, FALSE);
+		}
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG HtBw;
+
+	HtBw = simple_strtol(arg, 0, 10);
+	if (HtBw == BW_40)
+		pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_40;
+	else if (HtBw == BW_20)
+		pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+	else
+		return FALSE;  //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtBw_Proc::(HtBw=%d)\n", pAd->CommonCfg.RegTransmitSetting.field.BW));
+
+	return TRUE;
+}
+
+INT	Set_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG HtMcs, Mcs_tmp;
+#ifdef CONFIG_STA_SUPPORT
+    BOOLEAN bAutoRate = FALSE;
+#endif // CONFIG_STA_SUPPORT //
+
+	Mcs_tmp = simple_strtol(arg, 0, 10);
+
+	if (Mcs_tmp <= 15 || Mcs_tmp == 32)
+		HtMcs = Mcs_tmp;
+	else
+		HtMcs = MCS_AUTO;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = HtMcs;
+		pAd->StaCfg.bAutoTxRateSwitch = (HtMcs == MCS_AUTO) ? TRUE:FALSE;
+		DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMcs_Proc::(HtMcs=%d, bAutoTxRateSwitch = %d)\n",
+						pAd->StaCfg.DesiredTransmitSetting.field.MCS, pAd->StaCfg.bAutoTxRateSwitch));
+
+		if ((pAd->CommonCfg.PhyMode < PHY_11ABGN_MIXED) ||
+			(pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE < MODE_HTMIX))
+		{
+	        if ((pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO) &&
+				(HtMcs >= 0 && HtMcs <= 3) &&
+	            (pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode == FIXED_TXMODE_CCK))
+			{
+				RTMPSetDesiredRates(pAd, (LONG) (RateIdToMbps[HtMcs] * 1000000));
+			}
+	        else if ((pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO) &&
+					(HtMcs >= 0 && HtMcs <= 7) &&
+	            	(pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode == FIXED_TXMODE_OFDM))
+			{
+				RTMPSetDesiredRates(pAd, (LONG) (RateIdToMbps[HtMcs+4] * 1000000));
+			}
+			else
+				bAutoRate = TRUE;
+
+			if (bAutoRate)
+			{
+	            pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+				RTMPSetDesiredRates(pAd, -1);
+			}
+	        DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMcs_Proc::(FixedTxMode=%d)\n",pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode));
+		}
+        if (ADHOC_ON(pAd))
+            return TRUE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	SetCommonHT(pAd);
+
+	return TRUE;
+}
+
+INT	Set_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG HtGi;
+
+	HtGi = simple_strtol(arg, 0, 10);
+
+	if ( HtGi == GI_400)
+		pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_400;
+	else if ( HtGi == GI_800 )
+		pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_800;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtGi_Proc::(ShortGI=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.ShortGI));
+
+	return TRUE;
+}
+
+
+INT	Set_HtTxBASize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR Size;
+
+	Size = simple_strtol(arg, 0, 10);
+
+	if (Size <=0 || Size >=64)
+	{
+		Size = 8;
+	}
+	pAd->CommonCfg.TxBASize = Size-1;
+	DBGPRINT(RT_DEBUG_ERROR, ("Set_HtTxBASize ::(TxBASize= %d)\n", Size));
+
+	return TRUE;
+}
+
+
+INT	Set_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == HTMODE_GF)
+		pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_GF;
+	else if ( Value == HTMODE_MM )
+		pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_MM;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtOpMode_Proc::(HtOpMode=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.HTMODE));
+
+	return TRUE;
+
+}
+
+INT	Set_HtStbc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == STBC_USE)
+		pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_USE;
+	else if ( Value == STBC_NONE )
+		pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_NONE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_Stbc_Proc::(HtStbc=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.STBC));
+
+	return TRUE;
+}
+
+INT	Set_HtHtc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->HTCEnable = FALSE;
+	else if ( Value ==1 )
+        	pAd->HTCEnable = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtHtc_Proc::(HtHtc=%d)\n",pAd->HTCEnable));
+
+	return TRUE;
+}
+
+INT	Set_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == 0)
+		pAd->CommonCfg.RegTransmitSetting.field.EXTCHA  = EXTCHA_BELOW;
+	else if ( Value ==1 )
+        pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtExtcha_Proc::(HtExtcha=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.EXTCHA));
+
+	return TRUE;
+}
+
+INT	Set_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value <=7 && Value >= 0)
+		pAd->CommonCfg.BACapability.field.MpduDensity = Value;
+	else
+		pAd->CommonCfg.BACapability.field.MpduDensity = 4;
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMpduDensity_Proc::(HtMpduDensity=%d)\n",pAd->CommonCfg.BACapability.field.MpduDensity));
+
+	return TRUE;
+}
+
+INT	Set_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+
+	if (Value >=1 && Value <= 64)
+	{
+		pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = Value;
+		pAd->CommonCfg.BACapability.field.RxBAWinLimit = Value;
+	}
+	else
+	{
+        pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = 64;
+		pAd->CommonCfg.BACapability.field.RxBAWinLimit = 64;
+	}
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtBaWinSize_Proc::(HtBaWinSize=%d)\n",pAd->CommonCfg.BACapability.field.RxBAWinLimit));
+
+	return TRUE;
+}
+
+INT	Set_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == 0)
+		pAd->CommonCfg.bRdg = FALSE;
+	else if ( Value ==1 )
+	{
+		pAd->HTCEnable = TRUE;
+        	pAd->CommonCfg.bRdg = TRUE;
+	}
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtRdg_Proc::(HtRdg=%d)\n",pAd->CommonCfg.bRdg));
+
+	return TRUE;
+}
+
+INT	Set_HtLinkAdapt_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->bLinkAdapt = FALSE;
+	else if ( Value ==1 )
+	{
+			pAd->HTCEnable = TRUE;
+			pAd->bLinkAdapt = TRUE;
+	}
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtLinkAdapt_Proc::(HtLinkAdapt=%d)\n",pAd->bLinkAdapt));
+
+	return TRUE;
+}
+
+INT	Set_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.BACapability.field.AmsduEnable = FALSE;
+	else if ( Value == 1 )
+        pAd->CommonCfg.BACapability.field.AmsduEnable = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtAmsdu_Proc::(HtAmsdu=%d)\n",pAd->CommonCfg.BACapability.field.AmsduEnable));
+
+	return TRUE;
+}
+
+INT	Set_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+    else if (Value == 1)
+		pAd->CommonCfg.BACapability.field.AutoBA = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+    pAd->CommonCfg.REGBACapability.field.AutoBA = pAd->CommonCfg.BACapability.field.AutoBA;
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtAutoBa_Proc::(HtAutoBa=%d)\n",pAd->CommonCfg.BACapability.field.AutoBA));
+
+	return TRUE;
+
+}
+
+INT	Set_HtProtect_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.bHTProtect = FALSE;
+    else if (Value == 1)
+		pAd->CommonCfg.bHTProtect = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtProtect_Proc::(HtProtect=%d)\n",pAd->CommonCfg.bHTProtect));
+
+	return TRUE;
+}
+
+INT	Set_SendPSMPAction_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], mode;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+    //printk("\n%s\n", arg);
+/*
+	The BARecTearDown inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the mode value.
+*/
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and mode value in decimal format.
+		return FALSE;
+
+   	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		mode = simple_strtol((token+1), 0, 10);
+		if (mode > MMPS_ENABLE)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+		printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x", mac[0], mac[1],
+		       mac[2], mac[3], mac[4], mac[5], mode);
+
+		pEntry = MacTableLookup(pAd, mac);
+
+		if (pEntry) {
+		    printk("\nSendPSMPAction MIPS mode = %d\n", mode);
+		    SendPSMPAction(pAd, pEntry->Aid, mode);
+		}
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+
+}
+
+INT	Set_HtMIMOPSmode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value <=3 && Value >= 0)
+		pAd->CommonCfg.BACapability.field.MMPSmode = Value;
+	else
+		pAd->CommonCfg.BACapability.field.MMPSmode = 3;
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMIMOPSmode_Proc::(MIMOPS mode=%d)\n",pAd->CommonCfg.BACapability.field.MMPSmode));
+
+	return TRUE;
+}
+
+
+INT	Set_ForceShortGI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->WIFItestbed.bShortGI = FALSE;
+	else if (Value == 1)
+		pAd->WIFItestbed.bShortGI = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_ForceShortGI_Proc::(ForceShortGI=%d)\n", pAd->WIFItestbed.bShortGI));
+
+	return TRUE;
+}
+
+
+
+INT	Set_ForceGF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->WIFItestbed.bGreenField = FALSE;
+	else if (Value == 1)
+		pAd->WIFItestbed.bGreenField = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_ForceGF_Proc::(ForceGF=%d)\n", pAd->WIFItestbed.bGreenField));
+
+	return TRUE;
+}
+
+INT	Set_HtMimoPs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.bMIMOPSEnable = FALSE;
+	else if (Value == 1)
+		pAd->CommonCfg.bMIMOPSEnable = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMimoPs_Proc::(HtMimoPs=%d)\n",pAd->CommonCfg.bMIMOPSEnable));
+
+	return TRUE;
+}
+#endif // DOT11_N_SUPPORT //
+
+
+#ifdef DOT11_N_SUPPORT
+INT	SetCommonHT(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	OID_SET_HT_PHYMODE		SetHT;
+
+	if (pAd->CommonCfg.PhyMode < PHY_11ABGN_MIXED)
+		return FALSE;
+
+	SetHT.PhyMode = pAd->CommonCfg.PhyMode;
+	SetHT.TransmitNo = ((UCHAR)pAd->Antenna.field.TxPath);
+	SetHT.HtMode = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.HTMODE;
+	SetHT.ExtOffset = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.EXTCHA;
+	SetHT.MCS = MCS_AUTO;
+	SetHT.BW = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.BW;
+	SetHT.STBC = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.STBC;
+	SetHT.SHORTGI = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.ShortGI;
+
+	RTMPSetHT(pAd, &SetHT);
+
+	return TRUE;
+}
+#endif // DOT11_N_SUPPORT //
+
+INT	Set_FixedTxMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR	fix_tx_mode = FIXED_TXMODE_HT;
+
+	if (strcmp(arg, "OFDM") == 0 || strcmp(arg, "ofdm") == 0)
+	{
+		fix_tx_mode = FIXED_TXMODE_OFDM;
+	}
+	else if (strcmp(arg, "CCK") == 0 || strcmp(arg, "cck") == 0)
+	{
+        fix_tx_mode = FIXED_TXMODE_CCK;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode = fix_tx_mode;
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_FixedTxMode_Proc::(FixedTxMode=%d)\n", fix_tx_mode));
+
+	return TRUE;
+}
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+INT	Set_OpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+#ifdef RT2870
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP))
+#endif // RT2870 //
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Can not switch operate mode on interface up !! \n"));
+		return FALSE;
+	}
+
+	if (Value == 0)
+		pAd->OpMode = OPMODE_STA;
+	else if (Value == 1)
+		pAd->OpMode = OPMODE_AP;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_OpMode_Proc::(OpMode=%s)\n", pAd->OpMode == 1 ? "AP Mode" : "STA Mode"));
+
+	return TRUE;
+}
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+
+/////////////////////////////////////////////////////////////////////////
+PCHAR   RTMPGetRalinkAuthModeStr(
+    IN  NDIS_802_11_AUTHENTICATION_MODE authMode)
+{
+	switch(authMode)
+	{
+		case Ndis802_11AuthModeOpen:
+			return "OPEN";
+		case Ndis802_11AuthModeWPAPSK:
+			return "WPAPSK";
+		case Ndis802_11AuthModeShared:
+			return "SHARED";
+		case Ndis802_11AuthModeWPA:
+			return "WPA";
+		case Ndis802_11AuthModeWPA2:
+			return "WPA2";
+		case Ndis802_11AuthModeWPA2PSK:
+			return "WPA2PSK";
+        case Ndis802_11AuthModeWPA1PSKWPA2PSK:
+			return "WPAPSKWPA2PSK";
+        case Ndis802_11AuthModeWPA1WPA2:
+			return "WPA1WPA2";
+		case Ndis802_11AuthModeWPANone:
+			return "WPANONE";
+		default:
+			return "UNKNOW";
+	}
+}
+
+PCHAR   RTMPGetRalinkEncryModeStr(
+    IN  USHORT encryMode)
+{
+	switch(encryMode)
+	{
+		case Ndis802_11WEPDisabled:
+			return "NONE";
+		case Ndis802_11WEPEnabled:
+			return "WEP";
+		case Ndis802_11Encryption2Enabled:
+			return "TKIP";
+		case Ndis802_11Encryption3Enabled:
+			return "AES";
+        case Ndis802_11Encryption4Enabled:
+			return "TKIPAES";
+		default:
+			return "UNKNOW";
+	}
+}
+
+INT RTMPShowCfgValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pName,
+	IN	PUCHAR			pBuf)
+{
+	INT	Status = 0;
+
+	for (PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC = RTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC++)
+	{
+		if (!strcmp(pName, PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name))
+		{
+			if(PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->show_proc(pAd, pBuf))
+				Status = -EINVAL;
+			break;  //Exit for loop.
+		}
+	}
+
+	if(PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name == NULL)
+	{
+		sprintf(pBuf, "\n");
+		for (PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC = RTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC++)
+			sprintf(pBuf, "%s%s\n", pBuf, PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name);
+	}
+
+	return Status;
+}
+
+INT	Show_SSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		sprintf(pBuf, "\t%s", pAd->CommonCfg.Ssid);
+#endif // CONFIG_STA_SUPPORT //
+	return 0;
+}
+
+INT	Show_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.PhyMode)
+	{
+		case PHY_11BG_MIXED:
+			sprintf(pBuf, "\t11B/G");
+			break;
+		case PHY_11B:
+			sprintf(pBuf, "\t11B");
+			break;
+		case PHY_11A:
+			sprintf(pBuf, "\t11A");
+			break;
+		case PHY_11ABG_MIXED:
+			sprintf(pBuf, "\t11A/B/G");
+			break;
+		case PHY_11G:
+			sprintf(pBuf, "\t11G");
+			break;
+#ifdef DOT11_N_SUPPORT
+		case PHY_11ABGN_MIXED:
+			sprintf(pBuf, "\t11A/B/G/N");
+			break;
+		case PHY_11N_2_4G:
+			sprintf(pBuf, "\t11N only with 2.4G");
+			break;
+		case PHY_11GN_MIXED:
+			sprintf(pBuf, "\t11G/N");
+			break;
+		case PHY_11AN_MIXED:
+			sprintf(pBuf, "\t11A/N");
+			break;
+		case PHY_11BGN_MIXED:
+			sprintf(pBuf, "\t11B/G/N");
+			break;
+		case PHY_11AGN_MIXED:
+			sprintf(pBuf, "\t11A/G/N");
+			break;
+		case PHY_11N_5G:
+			sprintf(pBuf, "\t11N only with 5G");
+			break;
+#endif // DOT11_N_SUPPORT //
+		default:
+			sprintf(pBuf, "\tUnknow Value(%d)", pAd->CommonCfg.PhyMode);
+			break;
+	}
+	return 0;
+}
+
+
+INT	Show_TxBurst_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bEnableTxBurst ? "TRUE":"FALSE");
+	return 0;
+}
+
+INT	Show_TxPreamble_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.TxPreamble)
+	{
+		case Rt802_11PreambleShort:
+			sprintf(pBuf, "\tShort");
+			break;
+		case Rt802_11PreambleLong:
+			sprintf(pBuf, "\tLong");
+			break;
+		case Rt802_11PreambleAuto:
+			sprintf(pBuf, "\tAuto");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%lu)", pAd->CommonCfg.TxPreamble);
+			break;
+	}
+
+	return 0;
+}
+
+INT	Show_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%lu", pAd->CommonCfg.TxPowerPercentage);
+	return 0;
+}
+
+INT	Show_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%d", pAd->CommonCfg.Channel);
+	return 0;
+}
+
+INT	Show_BGProtection_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.UseBGProtection)
+	{
+		case 1: //Always On
+			sprintf(pBuf, "\tON");
+			break;
+		case 2: //Always OFF
+			sprintf(pBuf, "\tOFF");
+			break;
+		case 0: //AUTO
+			sprintf(pBuf, "\tAuto");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%lu)", pAd->CommonCfg.UseBGProtection);
+			break;
+	}
+	return 0;
+}
+
+INT	Show_RTSThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.RtsThreshold);
+	return 0;
+}
+
+INT	Show_FragThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.FragmentThreshold);
+	return 0;
+}
+
+#ifdef DOT11_N_SUPPORT
+INT	Show_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	if (pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40)
+	{
+		sprintf(pBuf, "\t40 MHz");
+	}
+	else
+	{
+        sprintf(pBuf, "\t20 MHz");
+	}
+	return 0;
+}
+
+INT	Show_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		sprintf(pBuf, "\t%u", pAd->StaCfg.DesiredTransmitSetting.field.MCS);
+#endif // CONFIG_STA_SUPPORT //
+	return 0;
+}
+
+INT	Show_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.RegTransmitSetting.field.ShortGI)
+	{
+		case GI_400:
+			sprintf(pBuf, "\tGI_400");
+			break;
+		case GI_800:
+			sprintf(pBuf, "\tGI_800");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%u)", pAd->CommonCfg.RegTransmitSetting.field.ShortGI);
+			break;
+	}
+	return 0;
+}
+
+INT	Show_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.RegTransmitSetting.field.HTMODE)
+	{
+		case HTMODE_GF:
+			sprintf(pBuf, "\tGF");
+			break;
+		case HTMODE_MM:
+			sprintf(pBuf, "\tMM");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%u)", pAd->CommonCfg.RegTransmitSetting.field.HTMODE);
+			break;
+	}
+	return 0;
+}
+
+INT	Show_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.RegTransmitSetting.field.EXTCHA)
+	{
+		case EXTCHA_BELOW:
+			sprintf(pBuf, "\tBelow");
+			break;
+		case EXTCHA_ABOVE:
+			sprintf(pBuf, "\tAbove");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%u)", pAd->CommonCfg.RegTransmitSetting.field.EXTCHA);
+			break;
+	}
+	return 0;
+}
+
+
+INT	Show_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.BACapability.field.MpduDensity);
+	return 0;
+}
+
+INT	Show_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.BACapability.field.RxBAWinLimit);
+	return 0;
+}
+
+INT	Show_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bRdg ? "TRUE":"FALSE");
+	return 0;
+}
+
+INT	Show_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.BACapability.field.AmsduEnable ? "TRUE":"FALSE");
+	return 0;
+}
+
+INT	Show_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.BACapability.field.AutoBA ? "TRUE":"FALSE");
+	return 0;
+}
+#endif // DOT11_N_SUPPORT //
+
+INT	Show_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%d", pAd->CommonCfg.CountryRegion);
+	return 0;
+}
+
+INT	Show_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%d", pAd->CommonCfg.CountryRegionForABand);
+	return 0;
+}
+
+INT	Show_CountryCode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.CountryCode);
+	return 0;
+}
+
+#ifdef AGGREGATION_SUPPORT
+INT	Show_PktAggregate_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bAggregationCapable ? "TRUE":"FALSE");
+	return 0;
+}
+#endif // AGGREGATION_SUPPORT //
+
+#ifdef WMM_SUPPORT
+INT	Show_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		sprintf(pBuf, "\t%s", pAd->CommonCfg.bWmmCapable ? "TRUE":"FALSE");
+#endif // CONFIG_STA_SUPPORT //
+
+	return 0;
+}
+#endif // WMM_SUPPORT //
+
+INT	Show_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bIEEE80211H ? "TRUE":"FALSE");
+	return 0;
+}
+
+#ifdef CONFIG_STA_SUPPORT
+INT	Show_NetworkType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->StaCfg.BssType)
+	{
+		case BSS_ADHOC:
+			sprintf(pBuf, "\tAdhoc");
+			break;
+		case BSS_INFRA:
+			sprintf(pBuf, "\tInfra");
+			break;
+		case BSS_ANY:
+			sprintf(pBuf, "\tAny");
+			break;
+		case BSS_MONITOR:
+			sprintf(pBuf, "\tMonitor");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%d)", pAd->StaCfg.BssType);
+			break;
+	}
+	return 0;
+}
+#endif // CONFIG_STA_SUPPORT //
+
+INT	Show_AuthMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	NDIS_802_11_AUTHENTICATION_MODE	AuthMode = Ndis802_11AuthModeOpen;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		AuthMode = pAd->StaCfg.AuthMode;
+#endif // CONFIG_STA_SUPPORT //
+
+	if ((AuthMode >= Ndis802_11AuthModeOpen) &&
+		(AuthMode <= Ndis802_11AuthModeWPA1PSKWPA2PSK))
+		sprintf(pBuf, "\t%s", RTMPGetRalinkAuthModeStr(AuthMode));
+	else
+		sprintf(pBuf, "\tUnknow Value(%d)", AuthMode);
+
+	return 0;
+}
+
+INT	Show_EncrypType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	NDIS_802_11_WEP_STATUS	WepStatus = Ndis802_11WEPDisabled;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		WepStatus = pAd->StaCfg.WepStatus;
+#endif // CONFIG_STA_SUPPORT //
+
+	if ((WepStatus >= Ndis802_11WEPEnabled) &&
+		(WepStatus <= Ndis802_11Encryption4KeyAbsent))
+		sprintf(pBuf, "\t%s", RTMPGetRalinkEncryModeStr(WepStatus));
+	else
+		sprintf(pBuf, "\tUnknow Value(%d)", WepStatus);
+
+	return 0;
+}
+
+INT	Show_DefaultKeyID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	UCHAR DefaultKeyId = 0;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		DefaultKeyId = pAd->StaCfg.DefaultKeyId;
+#endif // CONFIG_STA_SUPPORT //
+
+	sprintf(pBuf, "\t%d", DefaultKeyId);
+
+	return 0;
+}
+
+INT	Show_WepKey_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  INT				KeyIdx,
+	OUT	PUCHAR			pBuf)
+{
+	UCHAR   Key[16] = {0}, KeyLength = 0;
+	INT		index = BSS0;
+
+	KeyLength = pAd->SharedKey[index][KeyIdx].KeyLen;
+	NdisMoveMemory(Key, pAd->SharedKey[index][KeyIdx].Key, KeyLength);
+
+	//check key string is ASCII or not
+    if (RTMPCheckStrPrintAble(Key, KeyLength))
+        sprintf(pBuf, "\t%s", Key);
+    else
+    {
+        int idx;
+        sprintf(pBuf, "\t");
+        for (idx = 0; idx < KeyLength; idx++)
+            sprintf(pBuf+strlen(pBuf), "%02X", Key[idx]);
+    }
+	return 0;
+}
+
+INT	Show_Key1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 0, pBuf);
+	return 0;
+}
+
+INT	Show_Key2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 1, pBuf);
+	return 0;
+}
+
+INT	Show_Key3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 2, pBuf);
+	return 0;
+}
+
+INT	Show_Key4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 3, pBuf);
+	return 0;
+}
+
+INT	Show_WPAPSK_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	INT 	idx;
+	UCHAR	PMK[32] = {0};
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		NdisMoveMemory(PMK, pAd->StaCfg.PMK, 32);
+#endif // CONFIG_STA_SUPPORT //
+
+    sprintf(pBuf, "\tPMK = ");
+    for (idx = 0; idx < 32; idx++)
+        sprintf(pBuf+strlen(pBuf), "%02X", PMK[idx]);
+
+	return 0;
+}
+
diff --git a/drivers/staging/rt2870/common/cmm_sanity.c b/drivers/staging/rt2870/common/cmm_sanity.c
new file mode 100644
index 000000000000..1e24320177bc
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_sanity.c
@@ -0,0 +1,1663 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sanity.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang  2004-09-01      add WMM support
+*/
+#include "../rt_config.h"
+
+
+extern UCHAR	CISCO_OUI[];
+
+extern UCHAR	WPA_OUI[];
+extern UCHAR	RSN_OUI[];
+extern UCHAR	WME_INFO_ELEM[];
+extern UCHAR	WME_PARM_ELEM[];
+extern UCHAR	Ccx2QosInfo[];
+extern UCHAR	RALINK_OUI[];
+extern UCHAR	BROADCOM_OUI[];
+extern UCHAR    WPS_OUI[];
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN MlmeAddBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2)
+{
+    PMLME_ADDBA_REQ_STRUCT   pInfo;
+
+    pInfo = (MLME_ADDBA_REQ_STRUCT *)Msg;
+
+    if ((MsgLen != sizeof(MLME_ADDBA_REQ_STRUCT)))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - message lenght not correct.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->Wcid >= MAX_LEN_OF_MAC_TABLE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - The peer Mac is not associated yet.\n"));
+        return FALSE;
+    }
+
+	/*
+    if ((pInfo->BaBufSize > MAX_RX_REORDERBUF) || (pInfo->BaBufSize < 2))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - Rx Reordering buffer too big or too small\n"));
+        return FALSE;
+    }
+	*/
+
+    if ((pInfo->pAddr[0]&0x01) == 0x01)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - broadcast address not support BA\n"));
+        return FALSE;
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN MlmeDelBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen)
+{
+	MLME_DELBA_REQ_STRUCT *pInfo;
+	pInfo = (MLME_DELBA_REQ_STRUCT *)Msg;
+
+    if ((MsgLen != sizeof(MLME_DELBA_REQ_STRUCT)))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - message lenght not correct.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->Wcid >= MAX_LEN_OF_MAC_TABLE))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - The peer Mac is not associated yet.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->TID & 0xf0))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - The peer TID is incorrect.\n"));
+        return FALSE;
+    }
+
+	if (NdisEqualMemory(pAd->MacTab.Content[pInfo->Wcid].Addr, pInfo->Addr, MAC_ADDR_LEN) == 0)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - the peer addr dosen't exist.\n"));
+        return FALSE;
+    }
+
+    return TRUE;
+}
+
+BOOLEAN PeerAddBAReqActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen,
+	OUT PUCHAR pAddr2)
+{
+	PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+	PFRAME_ADDBA_REQ pAddFrame;
+	pAddFrame = (PFRAME_ADDBA_REQ)(pMsg);
+	if (MsgLen < (sizeof(FRAME_ADDBA_REQ)))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Request frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+	// we support immediate BA.
+	*(USHORT *)(&pAddFrame->BaParm) = cpu2le16(*(USHORT *)(&pAddFrame->BaParm));
+	pAddFrame->TimeOutValue = cpu2le16(pAddFrame->TimeOutValue);
+	pAddFrame->BaStartSeq.word = cpu2le16(pAddFrame->BaStartSeq.word);
+
+	if (pAddFrame->BaParm.BAPolicy != IMMED_BA)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Request Ba Policy[%d] not support\n", pAddFrame->BaParm.BAPolicy));
+		DBGPRINT(RT_DEBUG_ERROR,("ADDBA Request. tid=%x, Bufsize=%x, AMSDUSupported=%x \n", pAddFrame->BaParm.TID, pAddFrame->BaParm.BufSize, pAddFrame->BaParm.AMSDUSupported));
+		return FALSE;
+	}
+
+	// we support immediate BA.
+	if (pAddFrame->BaParm.TID &0xfff0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Request incorrect TID = %d\n", pAddFrame->BaParm.TID));
+		return FALSE;
+	}
+	COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+	return TRUE;
+}
+
+BOOLEAN PeerAddBARspActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen)
+{
+	//PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+	PFRAME_ADDBA_RSP pAddFrame;
+
+	pAddFrame = (PFRAME_ADDBA_RSP)(pMsg);
+	if (MsgLen < (sizeof(FRAME_ADDBA_RSP)))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBARspActionSanity: ADDBA Response frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+	// we support immediate BA.
+	*(USHORT *)(&pAddFrame->BaParm) = cpu2le16(*(USHORT *)(&pAddFrame->BaParm));
+	pAddFrame->StatusCode = cpu2le16(pAddFrame->StatusCode);
+	pAddFrame->TimeOutValue = cpu2le16(pAddFrame->TimeOutValue);
+
+	if (pAddFrame->BaParm.BAPolicy != IMMED_BA)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Response Ba Policy[%d] not support\n", pAddFrame->BaParm.BAPolicy));
+		return FALSE;
+	}
+
+	// we support immediate BA.
+	if (pAddFrame->BaParm.TID &0xfff0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBARspActionSanity: ADDBA Response incorrect TID = %d\n", pAddFrame->BaParm.TID));
+		return FALSE;
+	}
+	return TRUE;
+
+}
+
+BOOLEAN PeerDelBAActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR Wcid,
+    IN VOID *pMsg,
+    IN ULONG MsgLen )
+{
+	//PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+	PFRAME_DELBA_REQ  pDelFrame;
+	if (MsgLen != (sizeof(FRAME_DELBA_REQ)))
+		return FALSE;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	pDelFrame = (PFRAME_DELBA_REQ)(pMsg);
+
+	*(USHORT *)(&pDelFrame->DelbaParm) = cpu2le16(*(USHORT *)(&pDelFrame->DelbaParm));
+	pDelFrame->ReasonCode = cpu2le16(pDelFrame->ReasonCode);
+
+	if (pDelFrame->DelbaParm.TID &0xfff0)
+		return FALSE;
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerBeaconAndProbeRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    IN UCHAR  MsgChannel,
+    OUT PUCHAR pAddr2,
+    OUT PUCHAR pBssid,
+    OUT CHAR Ssid[],
+    OUT UCHAR *pSsidLen,
+    OUT UCHAR *pBssType,
+    OUT USHORT *pBeaconPeriod,
+    OUT UCHAR *pChannel,
+    OUT UCHAR *pNewChannel,
+    OUT LARGE_INTEGER *pTimestamp,
+    OUT CF_PARM *pCfParm,
+    OUT USHORT *pAtimWin,
+    OUT USHORT *pCapabilityInfo,
+    OUT UCHAR *pErp,
+    OUT UCHAR *pDtimCount,
+    OUT UCHAR *pDtimPeriod,
+    OUT UCHAR *pBcastFlag,
+    OUT UCHAR *pMessageToMe,
+    OUT UCHAR SupRate[],
+    OUT UCHAR *pSupRateLen,
+    OUT UCHAR ExtRate[],
+    OUT UCHAR *pExtRateLen,
+    OUT	UCHAR *pCkipFlag,
+    OUT	UCHAR *pAironetCellPowerLimit,
+    OUT PEDCA_PARM       pEdcaParm,
+    OUT PQBSS_LOAD_PARM  pQbssLoad,
+    OUT PQOS_CAPABILITY_PARM pQosCapability,
+    OUT ULONG *pRalinkIe,
+    OUT UCHAR		 *pHtCapabilityLen,
+#ifdef CONFIG_STA_SUPPORT
+    OUT UCHAR		 *pPreNHtCapabilityLen,
+#endif // CONFIG_STA_SUPPORT //
+    OUT HT_CAPABILITY_IE *pHtCapability,
+	OUT UCHAR		 *AddHtInfoLen,
+	OUT ADD_HT_INFO_IE *AddHtInfo,
+	OUT UCHAR *NewExtChannelOffset,		// Ht extension channel offset(above or below)
+    OUT USHORT *LengthVIE,
+    OUT	PNDIS_802_11_VARIABLE_IEs pVIE)
+{
+    CHAR				*Ptr;
+#ifdef CONFIG_STA_SUPPORT
+	CHAR 				TimLen;
+#endif // CONFIG_STA_SUPPORT //
+    PFRAME_802_11		pFrame;
+    PEID_STRUCT         pEid;
+    UCHAR				SubType;
+    UCHAR				Sanity;
+    //UCHAR				ECWMin, ECWMax;
+    //MAC_CSR9_STRUC		Csr9;
+    ULONG				Length = 0;
+
+	// For some 11a AP which didn't have DS_IE, we use two conditions to decide the channel
+	//	1. If the AP is 11n enabled, then check the control channel.
+	//	2. If the AP didn't have any info about channel, use the channel we received this frame as the channel. (May inaccuracy!!)
+	UCHAR			CtrlChannel = 0;
+
+    // Add for 3 necessary EID field check
+    Sanity = 0;
+
+    *pAtimWin = 0;
+    *pErp = 0;
+    *pDtimCount = 0;
+    *pDtimPeriod = 0;
+    *pBcastFlag = 0;
+    *pMessageToMe = 0;
+    *pExtRateLen = 0;
+    *pCkipFlag = 0;			        // Default of CkipFlag is 0
+    *pAironetCellPowerLimit = 0xFF;  // Default of AironetCellPowerLimit is 0xFF
+    *LengthVIE = 0;					// Set the length of VIE to init value 0
+    *pHtCapabilityLen = 0;					// Set the length of VIE to init value 0
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->OpMode == OPMODE_STA)
+		*pPreNHtCapabilityLen = 0;					// Set the length of VIE to init value 0
+#endif // CONFIG_STA_SUPPORT //
+    *AddHtInfoLen = 0;					// Set the length of VIE to init value 0
+    *pRalinkIe = 0;
+    *pNewChannel = 0;
+    *NewExtChannelOffset = 0xff;	//Default 0xff means no such IE
+    pCfParm->bValid = FALSE;        // default: no IE_CF found
+    pQbssLoad->bValid = FALSE;      // default: no IE_QBSS_LOAD found
+    pEdcaParm->bValid = FALSE;      // default: no IE_EDCA_PARAMETER found
+    pQosCapability->bValid = FALSE; // default: no IE_QOS_CAPABILITY found
+
+    pFrame = (PFRAME_802_11)Msg;
+
+    // get subtype from header
+    SubType = (UCHAR)pFrame->Hdr.FC.SubType;
+
+    // get Addr2 and BSSID from header
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    COPY_MAC_ADDR(pBssid, pFrame->Hdr.Addr3);
+
+//	hex_dump("Beacon", Msg, MsgLen);
+
+    Ptr = pFrame->Octet;
+    Length += LENGTH_802_11;
+
+    // get timestamp from payload and advance the pointer
+    NdisMoveMemory(pTimestamp, Ptr, TIMESTAMP_LEN);
+
+	pTimestamp->u.LowPart = cpu2le32(pTimestamp->u.LowPart);
+	pTimestamp->u.HighPart = cpu2le32(pTimestamp->u.HighPart);
+
+    Ptr += TIMESTAMP_LEN;
+    Length += TIMESTAMP_LEN;
+
+    // get beacon interval from payload and advance the pointer
+    NdisMoveMemory(pBeaconPeriod, Ptr, 2);
+    Ptr += 2;
+    Length += 2;
+
+    // get capability info from payload and advance the pointer
+    NdisMoveMemory(pCapabilityInfo, Ptr, 2);
+    Ptr += 2;
+    Length += 2;
+
+    if (CAP_IS_ESS_ON(*pCapabilityInfo))
+        *pBssType = BSS_INFRA;
+    else
+        *pBssType = BSS_ADHOC;
+
+    pEid = (PEID_STRUCT) Ptr;
+
+    // get variable fields from payload and advance the pointer
+    while ((Length + 2 + pEid->Len) <= MsgLen)
+    {
+        //
+        // Secure copy VIE to VarIE[MAX_VIE_LEN] didn't overflow.
+        //
+        if ((*LengthVIE + pEid->Len + 2) >= MAX_VIE_LEN)
+        {
+            DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - Variable IEs out of resource [len(=%d) > MAX_VIE_LEN(=%d)]\n",
+                    (*LengthVIE + pEid->Len + 2), MAX_VIE_LEN));
+            break;
+        }
+
+        switch(pEid->Eid)
+        {
+            case IE_SSID:
+                // Already has one SSID EID in this beacon, ignore the second one
+                if (Sanity & 0x1)
+                    break;
+                if(pEid->Len <= MAX_LEN_OF_SSID)
+                {
+                    NdisMoveMemory(Ssid, pEid->Octet, pEid->Len);
+                    *pSsidLen = pEid->Len;
+                    Sanity |= 0x1;
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_SSID (len=%d)\n",pEid->Len));
+                    return FALSE;
+                }
+                break;
+
+            case IE_SUPP_RATES:
+                if(pEid->Len <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    Sanity |= 0x2;
+                    NdisMoveMemory(SupRate, pEid->Octet, pEid->Len);
+                    *pSupRateLen = pEid->Len;
+
+                    // TODO: 2004-09-14 not a good design here, cause it exclude extra rates
+                    // from ScanTab. We should report as is. And filter out unsupported
+                    // rates in MlmeAux.
+                    // Check against the supported rates
+                    // RTMPCheckRates(pAd, SupRate, pSupRateLen);
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_SUPP_RATES (len=%d)\n",pEid->Len));
+                    return FALSE;
+                }
+                break;
+
+            case IE_HT_CAP:
+			if (pEid->Len >= SIZE_HT_CAP_IE)  //Note: allow extension.!!
+			{
+				NdisMoveMemory(pHtCapability, pEid->Octet, sizeof(HT_CAPABILITY_IE));
+				*pHtCapabilityLen = SIZE_HT_CAP_IE;	// Nnow we only support 26 bytes.
+
+				*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+				*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					*pPreNHtCapabilityLen = 0;	// Nnow we only support 26 bytes.
+
+					Ptr = (PUCHAR) pVIE;
+					NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+					*LengthVIE += (pEid->Len + 2);
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - wrong IE_HT_CAP. pEid->Len = %d\n", pEid->Len));
+			}
+
+		break;
+            case IE_ADD_HT:
+			if (pEid->Len >= sizeof(ADD_HT_INFO_IE))
+			{
+				// This IE allows extension, but we can ignore extra bytes beyond our knowledge , so only
+				// copy first sizeof(ADD_HT_INFO_IE)
+				NdisMoveMemory(AddHtInfo, pEid->Octet, sizeof(ADD_HT_INFO_IE));
+				*AddHtInfoLen = SIZE_ADD_HT_INFO_IE;
+
+				CtrlChannel = AddHtInfo->ControlChan;
+
+				*(USHORT *)(&AddHtInfo->AddHtInfo2) = cpu2le16(*(USHORT *)(&AddHtInfo->AddHtInfo2));
+				*(USHORT *)(&AddHtInfo->AddHtInfo3) = cpu2le16(*(USHORT *)(&AddHtInfo->AddHtInfo3));
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+			                Ptr = (PUCHAR) pVIE;
+			                NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+			                *LengthVIE += (pEid->Len + 2);
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - wrong IE_ADD_HT. \n"));
+			}
+
+		break;
+            case IE_SECONDARY_CH_OFFSET:
+			if (pEid->Len == 1)
+			{
+				*NewExtChannelOffset = pEid->Octet[0];
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - wrong IE_SECONDARY_CH_OFFSET. \n"));
+			}
+
+		break;
+            case IE_FH_PARM:
+                DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity(IE_FH_PARM) \n"));
+                break;
+
+            case IE_DS_PARM:
+                if(pEid->Len == 1)
+                {
+                    *pChannel = *pEid->Octet;
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						if (ChannelSanity(pAd, *pChannel) == 0)
+						{
+
+							return FALSE;
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+                    Sanity |= 0x4;
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_DS_PARM (len=%d)\n",pEid->Len));
+                    return FALSE;
+                }
+                break;
+
+            case IE_CF_PARM:
+                if(pEid->Len == 6)
+                {
+                    pCfParm->bValid = TRUE;
+                    pCfParm->CfpCount = pEid->Octet[0];
+                    pCfParm->CfpPeriod = pEid->Octet[1];
+                    pCfParm->CfpMaxDuration = pEid->Octet[2] + 256 * pEid->Octet[3];
+                    pCfParm->CfpDurRemaining = pEid->Octet[4] + 256 * pEid->Octet[5];
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_CF_PARM\n"));
+                    return FALSE;
+                }
+                break;
+
+            case IE_IBSS_PARM:
+                if(pEid->Len == 2)
+                {
+                    NdisMoveMemory(pAtimWin, pEid->Octet, pEid->Len);
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_IBSS_PARM\n"));
+                    return FALSE;
+                }
+                break;
+
+#ifdef CONFIG_STA_SUPPORT
+            case IE_TIM:
+                if(INFRA_ON(pAd) && SubType == SUBTYPE_BEACON)
+                {
+                    GetTimBit((PUCHAR)pEid, pAd->StaActive.Aid, &TimLen, pBcastFlag, pDtimCount, pDtimPeriod, pMessageToMe);
+                }
+                break;
+#endif // CONFIG_STA_SUPPORT //
+            case IE_CHANNEL_SWITCH_ANNOUNCEMENT:
+                if(pEid->Len == 3)
+                {
+                	*pNewChannel = pEid->Octet[1];	//extract new channel number
+                }
+                break;
+
+            // New for WPA
+            // CCX v2 has the same IE, we need to parse that too
+            // Wifi WMM use the same IE vale, need to parse that too
+            // case IE_WPA:
+            case IE_VENDOR_SPECIFIC:
+                // Check Broadcom/Atheros 802.11n OUI version, for HT Capability IE.
+                // This HT IE is before IEEE draft set HT IE value.2006-09-28 by Jan.
+                /*if (NdisEqualMemory(pEid->Octet, BROADCOM_OUI, 3) && (pEid->Len >= 4))
+                {
+                	if ((pEid->Octet[3] == OUI_BROADCOM_HT) && (pEid->Len >= 30))
+            		{
+				{
+					NdisMoveMemory(pHtCapability, &pEid->Octet[4], sizeof(HT_CAPABILITY_IE));
+					*pHtCapabilityLen = SIZE_HT_CAP_IE;	// Nnow we only support 26 bytes.
+				}
+         		}
+                	if ((pEid->Octet[3] == OUI_BROADCOM_HT) && (pEid->Len >= 26))
+            		{
+				{
+					NdisMoveMemory(AddHtInfo, &pEid->Octet[4], sizeof(ADD_HT_INFO_IE));
+					*AddHtInfoLen = SIZE_ADD_HT_INFO_IE;	// Nnow we only support 26 bytes.
+				}
+         		}
+                }
+				*/
+                // Check the OUI version, filter out non-standard usage
+                if (NdisEqualMemory(pEid->Octet, RALINK_OUI, 3) && (pEid->Len == 7))
+                {
+                    //*pRalinkIe = pEid->Octet[3];
+                    if (pEid->Octet[3] != 0)
+        				*pRalinkIe = pEid->Octet[3];
+        			else
+        				*pRalinkIe = 0xf0000000; // Set to non-zero value (can't set bit0-2) to represent this is Ralink Chip. So at linkup, we will set ralinkchip flag.
+                }
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+		// This HT IE is before IEEE draft set HT IE value.2006-09-28 by Jan.
+
+                // Other vendors had production before IE_HT_CAP value is assigned. To backward support those old-firmware AP,
+                // Check broadcom-defiend pre-802.11nD1.0 OUI for HT related IE, including HT Capatilities IE and HT Information IE
+                else if ((*pHtCapabilityLen == 0) && NdisEqualMemory(pEid->Octet, PRE_N_HT_OUI, 3) && (pEid->Len >= 4) && (pAd->OpMode == OPMODE_STA))
+                {
+                    if ((pEid->Octet[3] == OUI_PREN_HT_CAP) && (pEid->Len >= 30) && (*pHtCapabilityLen == 0))
+                    {
+                        NdisMoveMemory(pHtCapability, &pEid->Octet[4], sizeof(HT_CAPABILITY_IE));
+                        *pPreNHtCapabilityLen = SIZE_HT_CAP_IE;
+                    }
+
+                    if ((pEid->Octet[3] == OUI_PREN_ADD_HT) && (pEid->Len >= 26))
+                    {
+                        NdisMoveMemory(AddHtInfo, &pEid->Octet[4], sizeof(ADD_HT_INFO_IE));
+                        *AddHtInfoLen = SIZE_ADD_HT_INFO_IE;
+                    }
+                }
+#endif // DOT11_N_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+                else if (NdisEqualMemory(pEid->Octet, WPA_OUI, 4))
+                {
+                    // Copy to pVIE which will report to microsoft bssid list.
+                    Ptr = (PUCHAR) pVIE;
+                    NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+                    *LengthVIE += (pEid->Len + 2);
+                }
+                else if (NdisEqualMemory(pEid->Octet, WME_PARM_ELEM, 6) && (pEid->Len == 24))
+                {
+                    PUCHAR ptr;
+                    int i;
+
+                    // parsing EDCA parameters
+                    pEdcaParm->bValid          = TRUE;
+                    pEdcaParm->bQAck           = FALSE; // pEid->Octet[0] & 0x10;
+                    pEdcaParm->bQueueRequest   = FALSE; // pEid->Octet[0] & 0x20;
+                    pEdcaParm->bTxopRequest    = FALSE; // pEid->Octet[0] & 0x40;
+                    pEdcaParm->EdcaUpdateCount = pEid->Octet[6] & 0x0f;
+                    pEdcaParm->bAPSDCapable    = (pEid->Octet[6] & 0x80) ? 1 : 0;
+                    ptr = &pEid->Octet[8];
+                    for (i=0; i<4; i++)
+                    {
+                        UCHAR aci = (*ptr & 0x60) >> 5; // b5~6 is AC INDEX
+                        pEdcaParm->bACM[aci]  = (((*ptr) & 0x10) == 0x10);   // b5 is ACM
+                        pEdcaParm->Aifsn[aci] = (*ptr) & 0x0f;               // b0~3 is AIFSN
+                        pEdcaParm->Cwmin[aci] = *(ptr+1) & 0x0f;             // b0~4 is Cwmin
+                        pEdcaParm->Cwmax[aci] = *(ptr+1) >> 4;               // b5~8 is Cwmax
+                        pEdcaParm->Txop[aci]  = *(ptr+2) + 256 * (*(ptr+3)); // in unit of 32-us
+                        ptr += 4; // point to next AC
+                    }
+                }
+                else if (NdisEqualMemory(pEid->Octet, WME_INFO_ELEM, 6) && (pEid->Len == 7))
+                {
+                    // parsing EDCA parameters
+                    pEdcaParm->bValid          = TRUE;
+                    pEdcaParm->bQAck           = FALSE; // pEid->Octet[0] & 0x10;
+                    pEdcaParm->bQueueRequest   = FALSE; // pEid->Octet[0] & 0x20;
+                    pEdcaParm->bTxopRequest    = FALSE; // pEid->Octet[0] & 0x40;
+                    pEdcaParm->EdcaUpdateCount = pEid->Octet[6] & 0x0f;
+                    pEdcaParm->bAPSDCapable    = (pEid->Octet[6] & 0x80) ? 1 : 0;
+
+                    // use default EDCA parameter
+                    pEdcaParm->bACM[QID_AC_BE]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_BE] = 3;
+                    pEdcaParm->Cwmin[QID_AC_BE] = CW_MIN_IN_BITS;
+                    pEdcaParm->Cwmax[QID_AC_BE] = CW_MAX_IN_BITS;
+                    pEdcaParm->Txop[QID_AC_BE]  = 0;
+
+                    pEdcaParm->bACM[QID_AC_BK]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_BK] = 7;
+                    pEdcaParm->Cwmin[QID_AC_BK] = CW_MIN_IN_BITS;
+                    pEdcaParm->Cwmax[QID_AC_BK] = CW_MAX_IN_BITS;
+                    pEdcaParm->Txop[QID_AC_BK]  = 0;
+
+                    pEdcaParm->bACM[QID_AC_VI]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_VI] = 2;
+                    pEdcaParm->Cwmin[QID_AC_VI] = CW_MIN_IN_BITS-1;
+                    pEdcaParm->Cwmax[QID_AC_VI] = CW_MAX_IN_BITS;
+                    pEdcaParm->Txop[QID_AC_VI]  = 96;   // AC_VI: 96*32us ~= 3ms
+
+                    pEdcaParm->bACM[QID_AC_VO]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_VO] = 2;
+                    pEdcaParm->Cwmin[QID_AC_VO] = CW_MIN_IN_BITS-2;
+                    pEdcaParm->Cwmax[QID_AC_VO] = CW_MAX_IN_BITS-1;
+                    pEdcaParm->Txop[QID_AC_VO]  = 48;   // AC_VO: 48*32us ~= 1.5ms
+                }
+                break;
+
+            case IE_EXT_SUPP_RATES:
+                if (pEid->Len <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(ExtRate, pEid->Octet, pEid->Len);
+                    *pExtRateLen = pEid->Len;
+
+                    // TODO: 2004-09-14 not a good design here, cause it exclude extra rates
+                    // from ScanTab. We should report as is. And filter out unsupported
+                    // rates in MlmeAux.
+                    // Check against the supported rates
+                    // RTMPCheckRates(pAd, ExtRate, pExtRateLen);
+                }
+                break;
+
+            case IE_ERP:
+                if (pEid->Len == 1)
+                {
+                    *pErp = (UCHAR)pEid->Octet[0];
+                }
+                break;
+
+            case IE_AIRONET_CKIP:
+                // 0. Check Aironet IE length, it must be larger or equal to 28
+                // Cisco AP350 used length as 28
+                // Cisco AP12XX used length as 30
+                if (pEid->Len < (CKIP_NEGOTIATION_LENGTH - 2))
+                    break;
+
+                // 1. Copy CKIP flag byte to buffer for process
+                *pCkipFlag = *(pEid->Octet + 8);
+                break;
+
+            case IE_AP_TX_POWER:
+                // AP Control of Client Transmit Power
+                //0. Check Aironet IE length, it must be 6
+                if (pEid->Len != 0x06)
+                    break;
+
+                // Get cell power limit in dBm
+                if (NdisEqualMemory(pEid->Octet, CISCO_OUI, 3) == 1)
+                    *pAironetCellPowerLimit = *(pEid->Octet + 4);
+                break;
+
+            // WPA2 & 802.11i RSN
+            case IE_RSN:
+                // There is no OUI for version anymore, check the group cipher OUI before copying
+                if (RTMPEqualMemory(pEid->Octet + 2, RSN_OUI, 3))
+                {
+                    // Copy to pVIE which will report to microsoft bssid list.
+                    Ptr = (PUCHAR) pVIE;
+                    NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+                    *LengthVIE += (pEid->Len + 2);
+                }
+                break;
+#ifdef CONFIG_STA_SUPPORT
+#ifdef EXT_BUILD_CHANNEL_LIST
+			case IE_COUNTRY:
+				Ptr = (PUCHAR) pVIE;
+                NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+                *LengthVIE += (pEid->Len + 2);
+				break;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+            default:
+                break;
+        }
+
+        Length = Length + 2 + pEid->Len;  // Eid[1] + Len[1]+ content[Len]
+        pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+    }
+
+    // For some 11a AP. it did not have the channel EID, patch here
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		UCHAR LatchRfChannel = MsgChannel;
+		if ((pAd->LatchRfRegs.Channel > 14) && ((Sanity & 0x4) == 0))
+		{
+			if (CtrlChannel != 0)
+				*pChannel = CtrlChannel;
+			else
+				*pChannel = LatchRfChannel;
+			Sanity |= 0x4;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	if (Sanity != 0x7)
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - missing field, Sanity=0x%02x\n", Sanity));
+		return FALSE;
+	}
+	else
+	{
+		return TRUE;
+	}
+
+}
+
+#ifdef DOT11N_DRAFT3
+/*
+	==========================================================================
+	Description:
+		MLME message sanity check for some IE addressed  in 802.11n d3.03.
+	Return:
+		TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+BOOLEAN PeerBeaconAndProbeRspSanity2(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *Msg,
+	IN ULONG MsgLen,
+	OUT UCHAR 	*RegClass)
+{
+	CHAR				*Ptr;
+	PFRAME_802_11		pFrame;
+	PEID_STRUCT			pEid;
+	ULONG				Length = 0;
+
+	pFrame = (PFRAME_802_11)Msg;
+
+	*RegClass = 0;
+	Ptr = pFrame->Octet;
+	Length += LENGTH_802_11;
+
+	// get timestamp from payload and advance the pointer
+	Ptr += TIMESTAMP_LEN;
+	Length += TIMESTAMP_LEN;
+
+	// get beacon interval from payload and advance the pointer
+	Ptr += 2;
+	Length += 2;
+
+	// get capability info from payload and advance the pointer
+	Ptr += 2;
+	Length += 2;
+
+	pEid = (PEID_STRUCT) Ptr;
+
+	// get variable fields from payload and advance the pointer
+	while ((Length + 2 + pEid->Len) <= MsgLen)
+	{
+		switch(pEid->Eid)
+		{
+			case IE_SUPP_REG_CLASS:
+				if(pEid->Len > 0)
+				{
+					*RegClass = *pEid->Octet;
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_SSID (len=%d)\n",pEid->Len));
+					return FALSE;
+				}
+				break;
+		}
+
+		Length = Length + 2 + pEid->Len;  // Eid[1] + Len[1]+ content[Len]
+		pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+	}
+
+	return TRUE;
+
+}
+#endif // DOT11N_DRAFT3 //
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN MlmeScanReqSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *Msg,
+	IN ULONG MsgLen,
+	OUT UCHAR *pBssType,
+	OUT CHAR Ssid[],
+	OUT UCHAR *pSsidLen,
+	OUT UCHAR *pScanType)
+{
+	MLME_SCAN_REQ_STRUCT *Info;
+
+	Info = (MLME_SCAN_REQ_STRUCT *)(Msg);
+	*pBssType = Info->BssType;
+	*pSsidLen = Info->SsidLen;
+	NdisMoveMemory(Ssid, Info->Ssid, *pSsidLen);
+	*pScanType = Info->ScanType;
+
+	if ((*pBssType == BSS_INFRA || *pBssType == BSS_ADHOC || *pBssType == BSS_ANY)
+		&& (*pScanType == SCAN_ACTIVE || *pScanType == SCAN_PASSIVE
+#ifdef CONFIG_STA_SUPPORT
+		|| *pScanType == SCAN_CISCO_PASSIVE || *pScanType == SCAN_CISCO_ACTIVE
+		|| *pScanType == SCAN_CISCO_CHANNEL_LOAD || *pScanType == SCAN_CISCO_NOISE
+#endif // CONFIG_STA_SUPPORT //
+		))
+	{
+		return TRUE;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeScanReqSanity fail - wrong BssType or ScanType\n"));
+		return FALSE;
+	}
+}
+
+// IRQL = DISPATCH_LEVEL
+UCHAR ChannelSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR channel)
+{
+    int i;
+
+    for (i = 0; i < pAd->ChannelListNum; i ++)
+    {
+        if (channel == pAd->ChannelList[i].Channel)
+            return 1;
+    }
+    return 0;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerDeauthSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pReason)
+{
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    NdisMoveMemory(pReason, &pFrame->Octet[0], 2);
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerAuthSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr,
+    OUT USHORT *pAlg,
+    OUT USHORT *pSeq,
+    OUT USHORT *pStatus,
+    CHAR *pChlgText)
+{
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr,   pFrame->Hdr.Addr2);
+    NdisMoveMemory(pAlg,    &pFrame->Octet[0], 2);
+    NdisMoveMemory(pSeq,    &pFrame->Octet[2], 2);
+    NdisMoveMemory(pStatus, &pFrame->Octet[4], 2);
+
+    if ((*pAlg == Ndis802_11AuthModeOpen)
+#ifdef LEAP_SUPPORT
+      || (*pAlg == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+      )
+    {
+        if (*pSeq == 1 || *pSeq == 2)
+        {
+            return TRUE;
+        }
+        else
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("PeerAuthSanity fail - wrong Seg#\n"));
+            return FALSE;
+        }
+    }
+    else if (*pAlg == Ndis802_11AuthModeShared)
+    {
+        if (*pSeq == 1 || *pSeq == 4)
+        {
+            return TRUE;
+        }
+        else if (*pSeq == 2 || *pSeq == 3)
+        {
+            NdisMoveMemory(pChlgText, &pFrame->Octet[8], CIPHER_TEXT_LEN);
+            return TRUE;
+        }
+        else
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("PeerAuthSanity fail - wrong Seg#\n"));
+            return FALSE;
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerAuthSanity fail - wrong algorithm\n"));
+        return FALSE;
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN MlmeAuthReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr,
+    OUT ULONG *pTimeout,
+    OUT USHORT *pAlg)
+{
+    MLME_AUTH_REQ_STRUCT *pInfo;
+
+    pInfo  = (MLME_AUTH_REQ_STRUCT *)Msg;
+    COPY_MAC_ADDR(pAddr, pInfo->Addr);
+    *pTimeout = pInfo->Timeout;
+    *pAlg = pInfo->Alg;
+
+    if (((*pAlg == Ndis802_11AuthModeShared) ||(*pAlg == Ndis802_11AuthModeOpen)
+#ifdef LEAP_SUPPORT
+     || (*pAlg == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+     	) &&
+        ((*pAddr & 0x01) == 0))
+    {
+        return TRUE;
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAuthReqSanity fail - wrong algorithm\n"));
+        return FALSE;
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN MlmeAssocReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pApAddr,
+    OUT USHORT *pCapabilityInfo,
+    OUT ULONG *pTimeout,
+    OUT USHORT *pListenIntv)
+{
+    MLME_ASSOC_REQ_STRUCT *pInfo;
+
+    pInfo = (MLME_ASSOC_REQ_STRUCT *)Msg;
+    *pTimeout = pInfo->Timeout;                             // timeout
+    COPY_MAC_ADDR(pApAddr, pInfo->Addr);                   // AP address
+    *pCapabilityInfo = pInfo->CapabilityInfo;               // capability info
+    *pListenIntv = pInfo->ListenIntv;
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerDisassocSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pReason)
+{
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    NdisMoveMemory(pReason, &pFrame->Octet[0], 2);
+
+    return TRUE;
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Sanity check NetworkType (11b, 11g or 11a)
+
+	Arguments:
+		pBss - Pointer to BSS table.
+
+	Return Value:
+        Ndis802_11DS .......(11b)
+        Ndis802_11OFDM24....(11g)
+        Ndis802_11OFDM5.....(11a)
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+NDIS_802_11_NETWORK_TYPE NetworkTypeInUseSanity(
+    IN PBSS_ENTRY pBss)
+{
+	NDIS_802_11_NETWORK_TYPE	NetWorkType;
+	UCHAR						rate, i;
+
+	NetWorkType = Ndis802_11DS;
+
+	if (pBss->Channel <= 14)
+	{
+		//
+		// First check support Rate.
+		//
+		for (i = 0; i < pBss->SupRateLen; i++)
+		{
+			rate = pBss->SupRate[i] & 0x7f; // Mask out basic rate set bit
+			if ((rate == 2) || (rate == 4) || (rate == 11) || (rate == 22))
+			{
+				continue;
+			}
+			else
+			{
+				//
+				// Otherwise (even rate > 108) means Ndis802_11OFDM24
+				//
+				NetWorkType = Ndis802_11OFDM24;
+				break;
+			}
+		}
+
+		//
+		// Second check Extend Rate.
+		//
+		if (NetWorkType != Ndis802_11OFDM24)
+		{
+			for (i = 0; i < pBss->ExtRateLen; i++)
+			{
+				rate = pBss->SupRate[i] & 0x7f; // Mask out basic rate set bit
+				if ((rate == 2) || (rate == 4) || (rate == 11) || (rate == 22))
+				{
+					continue;
+				}
+				else
+				{
+					//
+					// Otherwise (even rate > 108) means Ndis802_11OFDM24
+					//
+					NetWorkType = Ndis802_11OFDM24;
+					break;
+				}
+			}
+		}
+	}
+	else
+	{
+		NetWorkType = Ndis802_11OFDM5;
+	}
+
+    if (pBss->HtCapabilityLen != 0)
+    {
+        if (NetWorkType == Ndis802_11OFDM5)
+            NetWorkType = Ndis802_11OFDM5_N;
+        else
+            NetWorkType = Ndis802_11OFDM24_N;
+    }
+
+	return NetWorkType;
+}
+
+/*
+    ==========================================================================
+    Description:
+        WPA message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN PeerWpaMessageSanity(
+    IN 	PRTMP_ADAPTER 		pAd,
+    IN 	PEAPOL_PACKET 		pMsg,
+    IN 	ULONG 				MsgLen,
+    IN 	UCHAR				MsgType,
+    IN 	MAC_TABLE_ENTRY  	*pEntry)
+{
+	UCHAR			mic[LEN_KEY_DESC_MIC], digest[80], KEYDATA[MAX_LEN_OF_RSNIE];
+	BOOLEAN			bReplayDiff = FALSE;
+	BOOLEAN			bWPA2 = FALSE;
+	KEY_INFO		EapolKeyInfo;
+	UCHAR			GroupKeyIndex = 0;
+
+
+	NdisZeroMemory(mic, sizeof(mic));
+	NdisZeroMemory(digest, sizeof(digest));
+	NdisZeroMemory(KEYDATA, sizeof(KEYDATA));
+	NdisZeroMemory((PUCHAR)&EapolKeyInfo, sizeof(EapolKeyInfo));
+
+	NdisMoveMemory((PUCHAR)&EapolKeyInfo, (PUCHAR)&pMsg->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT *)&EapolKeyInfo) = cpu2le16(*((USHORT *)&EapolKeyInfo));
+
+	// Choose WPA2 or not
+	if ((pEntry->AuthMode == Ndis802_11AuthModeWPA2) || (pEntry->AuthMode == Ndis802_11AuthModeWPA2PSK))
+		bWPA2 = TRUE;
+
+	// 0. Check MsgType
+	if ((MsgType > EAPOL_GROUP_MSG_2) || (MsgType < EAPOL_PAIR_MSG_1))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("The message type is invalid(%d)! \n", MsgType));
+		return FALSE;
+	}
+
+	// 1. Replay counter check
+ 	if (MsgType == EAPOL_PAIR_MSG_1 || MsgType == EAPOL_PAIR_MSG_3 || MsgType == EAPOL_GROUP_MSG_1)	// For supplicant
+    {
+    	// First validate replay counter, only accept message with larger replay counter.
+		// Let equal pass, some AP start with all zero replay counter
+		UCHAR	ZeroReplay[LEN_KEY_DESC_REPLAY];
+
+        NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);
+		if ((RTMPCompareMemory(pMsg->KeyDesc.ReplayCounter, pEntry->R_Counter, LEN_KEY_DESC_REPLAY) != 1) &&
+			(RTMPCompareMemory(pMsg->KeyDesc.ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
+    	{
+			bReplayDiff = TRUE;
+    	}
+ 	}
+	else if (MsgType == EAPOL_PAIR_MSG_2 || MsgType == EAPOL_PAIR_MSG_4 || MsgType == EAPOL_GROUP_MSG_2)	// For authenticator
+	{
+		// check Replay Counter coresponds to MSG from authenticator, otherwise discard
+    	if (!NdisEqualMemory(pMsg->KeyDesc.ReplayCounter, pEntry->R_Counter, LEN_KEY_DESC_REPLAY))
+    	{
+			bReplayDiff = TRUE;
+    	}
+	}
+
+	// Replay Counter different condition
+	if (bReplayDiff)
+	{
+		// send wireless event - for replay counter different
+		if (pAd->CommonCfg.bWirelessEvent)
+			RTMPSendWirelessEvent(pAd, IW_REPLAY_COUNTER_DIFF_EVENT_FLAG, pEntry->Addr, pEntry->apidx, 0);
+
+		if (MsgType < EAPOL_GROUP_MSG_1)
+		{
+           	DBGPRINT(RT_DEBUG_ERROR, ("Replay Counter Different in pairwise msg %d of 4-way handshake!\n", MsgType));
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Replay Counter Different in group msg %d of 2-way handshake!\n", (MsgType - EAPOL_PAIR_MSG_4)));
+		}
+
+		hex_dump("Receive replay counter ", pMsg->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+		hex_dump("Current replay counter ", pEntry->R_Counter, LEN_KEY_DESC_REPLAY);
+        return FALSE;
+	}
+
+	// 2. Verify MIC except Pairwise Msg1
+	if (MsgType != EAPOL_PAIR_MSG_1)
+	{
+		UCHAR			rcvd_mic[LEN_KEY_DESC_MIC];
+
+		// Record the received MIC for check later
+		NdisMoveMemory(rcvd_mic, pMsg->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+		NdisZeroMemory(pMsg->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+
+        if (pEntry->WepStatus == Ndis802_11Encryption2Enabled)	// TKIP
+        {
+            hmac_md5(pEntry->PTK, LEN_EAP_MICK, (PUCHAR)pMsg, MsgLen, mic);
+        }
+        else if (pEntry->WepStatus == Ndis802_11Encryption3Enabled)	// AES
+        {
+            HMAC_SHA1((PUCHAR)pMsg, MsgLen, pEntry->PTK, LEN_EAP_MICK, digest);
+            NdisMoveMemory(mic, digest, LEN_KEY_DESC_MIC);
+        }
+
+        if (!NdisEqualMemory(rcvd_mic, mic, LEN_KEY_DESC_MIC))
+        {
+			// send wireless event - for MIC different
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_MIC_DIFF_EVENT_FLAG, pEntry->Addr, pEntry->apidx, 0);
+
+			if (MsgType < EAPOL_GROUP_MSG_1)
+			{
+            	DBGPRINT(RT_DEBUG_ERROR, ("MIC Different in pairwise msg %d of 4-way handshake!\n", MsgType));
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("MIC Different in group msg %d of 2-way handshake!\n", (MsgType - EAPOL_PAIR_MSG_4)));
+			}
+
+			hex_dump("Received MIC", rcvd_mic, LEN_KEY_DESC_MIC);
+			hex_dump("Desired  MIC", mic, LEN_KEY_DESC_MIC);
+
+			return FALSE;
+        }
+	}
+
+	// Extract the context of the Key Data field if it exist
+	// The field in pairwise_msg_2_WPA1(WPA2) & pairwise_msg_3_WPA1 is un-encrypted.
+	// The field in group_msg_1_WPA1(WPA2) & pairwise_msg_3_WPA2 is encrypted.
+	if (pMsg->KeyDesc.KeyDataLen[1] > 0)
+	{
+		// Decrypt this field
+		if ((MsgType == EAPOL_PAIR_MSG_3 && bWPA2) || (MsgType == EAPOL_GROUP_MSG_1))
+		{
+			if(pEntry->WepStatus == Ndis802_11Encryption3Enabled)
+			{
+				// AES
+				AES_GTK_KEY_UNWRAP(&pEntry->PTK[16], KEYDATA, pMsg->KeyDesc.KeyDataLen[1],pMsg->KeyDesc.KeyData);
+			}
+			else
+			{
+				INT 	i;
+				UCHAR   Key[32];
+				// Decrypt TKIP GTK
+				// Construct 32 bytes RC4 Key
+				NdisMoveMemory(Key, pMsg->KeyDesc.KeyIv, 16);
+				NdisMoveMemory(&Key[16], &pEntry->PTK[16], 16);
+				ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
+				//discard first 256 bytes
+				for(i = 0; i < 256; i++)
+					ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
+				// Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
+				ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pMsg->KeyDesc.KeyData, pMsg->KeyDesc.KeyDataLen[1]);
+			}
+
+			if (!bWPA2 && (MsgType == EAPOL_GROUP_MSG_1))
+				GroupKeyIndex = EapolKeyInfo.KeyIndex;
+
+		}
+		else if ((MsgType == EAPOL_PAIR_MSG_2) || (MsgType == EAPOL_PAIR_MSG_3 && !bWPA2))
+		{
+			NdisMoveMemory(KEYDATA, pMsg->KeyDesc.KeyData, pMsg->KeyDesc.KeyDataLen[1]);
+		}
+		else
+		{
+
+			return TRUE;
+		}
+
+		// Parse Key Data field to
+		// 1. verify RSN IE for pairwise_msg_2_WPA1(WPA2) ,pairwise_msg_3_WPA1(WPA2)
+		// 2. verify KDE format for pairwise_msg_3_WPA2, group_msg_1_WPA2
+		// 3. update shared key for pairwise_msg_3_WPA2, group_msg_1_WPA1(WPA2)
+		if (!RTMPParseEapolKeyData(pAd, KEYDATA, pMsg->KeyDesc.KeyDataLen[1], GroupKeyIndex, MsgType, bWPA2, pEntry))
+		{
+			return FALSE;
+		}
+	}
+
+	return TRUE;
+
+}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+BOOLEAN MlmeDlsReqSanity(
+	IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PRT_802_11_DLS *pDLS,
+    OUT PUSHORT pReason)
+{
+	MLME_DLS_REQ_STRUCT *pInfo;
+
+    pInfo = (MLME_DLS_REQ_STRUCT *)Msg;
+
+	*pDLS = pInfo->pDLS;
+	*pReason = pInfo->Reason;
+
+	return TRUE;
+}
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef QOS_DLS_SUPPORT
+BOOLEAN PeerDlsReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pDlsTimeout,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+	OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability)
+{
+	CHAR            *Ptr;
+    PFRAME_802_11	Fr = (PFRAME_802_11)Msg;
+	PEID_STRUCT  eid_ptr;
+
+    // to prevent caller from using garbage output value
+    *pCapabilityInfo	= 0;
+    *pDlsTimeout	= 0;
+	*pHtCapabilityLen = 0;
+
+    Ptr = Fr->Octet;
+
+	// offset to destination MAC address (Category and Action field)
+    Ptr += 2;
+
+    // get DA from payload and advance the pointer
+    NdisMoveMemory(pDA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get SA from payload and advance the pointer
+    NdisMoveMemory(pSA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get capability info from payload and advance the pointer
+    NdisMoveMemory(pCapabilityInfo, Ptr, 2);
+    Ptr += 2;
+
+    // get capability info from payload and advance the pointer
+    NdisMoveMemory(pDlsTimeout, Ptr, 2);
+    Ptr += 2;
+
+	// Category and Action field + DA + SA + capability + Timeout
+	eid_ptr = (PEID_STRUCT) &Fr->Octet[18];
+
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((UCHAR*)Fr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_SUPP_RATES:
+                if ((eid_ptr->Len <= MAX_LEN_OF_SUPPORTED_RATES) && (eid_ptr->Len > 0))
+                {
+                    NdisMoveMemory(Rates, eid_ptr->Octet, eid_ptr->Len);
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - IE_SUPP_RATES., Len=%d. Rates[0]=%x\n",eid_ptr->Len, Rates[0]));
+                    DBGPRINT(RT_DEBUG_TRACE, ("Rates[1]=%x %x %x %x %x %x %x\n", Rates[1], Rates[2], Rates[3], Rates[4], Rates[5], Rates[6], Rates[7]));
+                    *pRatesLen = eid_ptr->Len;
+                }
+                else
+                {
+                    *pRatesLen = 8;
+					Rates[0] = 0x82;
+					Rates[1] = 0x84;
+					Rates[2] = 0x8b;
+					Rates[3] = 0x96;
+					Rates[4] = 0x12;
+					Rates[5] = 0x24;
+					Rates[6] = 0x48;
+					Rates[7] = 0x6c;
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - wrong IE_SUPP_RATES., Len=%d\n",eid_ptr->Len));
+                }
+				break;
+
+			case IE_EXT_SUPP_RATES:
+                if (eid_ptr->Len + *pRatesLen <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, eid_ptr->Len);
+                    *pRatesLen = (*pRatesLen) + eid_ptr->Len;
+                }
+                else
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, MAX_LEN_OF_SUPPORTED_RATES - (*pRatesLen));
+                    *pRatesLen = MAX_LEN_OF_SUPPORTED_RATES;
+                }
+				break;
+
+			case IE_HT_CAP:
+				if (eid_ptr->Len >= sizeof(HT_CAPABILITY_IE))
+				{
+					NdisMoveMemory(pHtCapability, eid_ptr->Octet, sizeof(HT_CAPABILITY_IE));
+
+					*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+					*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+					*pHtCapabilityLen = sizeof(HT_CAPABILITY_IE);
+
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - IE_HT_CAP\n"));
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - wrong IE_HT_CAP.eid_ptr->Len = %d\n", eid_ptr->Len));
+				}
+				break;
+
+			default:
+				break;
+		}
+
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+    return TRUE;
+}
+
+BOOLEAN PeerDlsRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pStatus,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability)
+{
+    CHAR            *Ptr;
+    PFRAME_802_11	Fr = (PFRAME_802_11)Msg;
+	PEID_STRUCT  eid_ptr;
+
+    // to prevent caller from using garbage output value
+    *pStatus		= 0;
+    *pCapabilityInfo	= 0;
+	*pHtCapabilityLen = 0;
+
+    Ptr = Fr->Octet;
+
+	// offset to destination MAC address (Category and Action field)
+    Ptr += 2;
+
+	// get status code from payload and advance the pointer
+    NdisMoveMemory(pStatus, Ptr, 2);
+    Ptr += 2;
+
+    // get DA from payload and advance the pointer
+    NdisMoveMemory(pDA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get SA from payload and advance the pointer
+    NdisMoveMemory(pSA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+	if (pStatus == 0)
+	{
+	    // get capability info from payload and advance the pointer
+	    NdisMoveMemory(pCapabilityInfo, Ptr, 2);
+	    Ptr += 2;
+	}
+
+	// Category and Action field + status code + DA + SA + capability
+	eid_ptr = (PEID_STRUCT) &Fr->Octet[18];
+
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((UCHAR*)Fr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_SUPP_RATES:
+                if ((eid_ptr->Len <= MAX_LEN_OF_SUPPORTED_RATES) && (eid_ptr->Len > 0))
+                {
+                    NdisMoveMemory(Rates, eid_ptr->Octet, eid_ptr->Len);
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - IE_SUPP_RATES., Len=%d. Rates[0]=%x\n",eid_ptr->Len, Rates[0]));
+                    DBGPRINT(RT_DEBUG_TRACE, ("Rates[1]=%x %x %x %x %x %x %x\n", Rates[1], Rates[2], Rates[3], Rates[4], Rates[5], Rates[6], Rates[7]));
+                    *pRatesLen = eid_ptr->Len;
+                }
+                else
+                {
+                    *pRatesLen = 8;
+					Rates[0] = 0x82;
+					Rates[1] = 0x84;
+					Rates[2] = 0x8b;
+					Rates[3] = 0x96;
+					Rates[4] = 0x12;
+					Rates[5] = 0x24;
+					Rates[6] = 0x48;
+					Rates[7] = 0x6c;
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - wrong IE_SUPP_RATES., Len=%d\n",eid_ptr->Len));
+                }
+				break;
+
+			case IE_EXT_SUPP_RATES:
+                if (eid_ptr->Len + *pRatesLen <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, eid_ptr->Len);
+                    *pRatesLen = (*pRatesLen) + eid_ptr->Len;
+                }
+                else
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, MAX_LEN_OF_SUPPORTED_RATES - (*pRatesLen));
+                    *pRatesLen = MAX_LEN_OF_SUPPORTED_RATES;
+                }
+				break;
+
+			case IE_HT_CAP:
+				if (eid_ptr->Len >= sizeof(HT_CAPABILITY_IE))
+				{
+					NdisMoveMemory(pHtCapability, eid_ptr->Octet, sizeof(HT_CAPABILITY_IE));
+
+					*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+					*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+					*pHtCapabilityLen = sizeof(HT_CAPABILITY_IE);
+
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - IE_HT_CAP\n"));
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - wrong IE_HT_CAP.eid_ptr->Len = %d\n", eid_ptr->Len));
+				}
+				break;
+
+			default:
+				break;
+		}
+
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+    return TRUE;
+}
+
+BOOLEAN PeerDlsTearDownSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pReason)
+{
+    CHAR            *Ptr;
+    PFRAME_802_11	Fr = (PFRAME_802_11)Msg;
+
+    // to prevent caller from using garbage output value
+    *pReason	= 0;
+
+    Ptr = Fr->Octet;
+
+	// offset to destination MAC address (Category and Action field)
+    Ptr += 2;
+
+    // get DA from payload and advance the pointer
+    NdisMoveMemory(pDA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get SA from payload and advance the pointer
+    NdisMoveMemory(pSA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+	// get reason code from payload and advance the pointer
+    NdisMoveMemory(pReason, Ptr, 2);
+    Ptr += 2;
+
+    return TRUE;
+}
+#endif // QOS_DLS_SUPPORT //
+
diff --git a/drivers/staging/rt2870/common/cmm_sync.c b/drivers/staging/rt2870/common/cmm_sync.c
new file mode 100644
index 000000000000..2be7c77a384e
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_sync.c
@@ -0,0 +1,711 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sync.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2004-09-01      modified for rt2561/2661
+*/
+#include "../rt_config.h"
+
+// 2.4 Ghz channel plan index in the TxPower arrays.
+#define	BG_BAND_REGION_0_START	0			// 1,2,3,4,5,6,7,8,9,10,11
+#define	BG_BAND_REGION_0_SIZE	11
+#define	BG_BAND_REGION_1_START	0			// 1,2,3,4,5,6,7,8,9,10,11,12,13
+#define	BG_BAND_REGION_1_SIZE	13
+#define	BG_BAND_REGION_2_START	9			// 10,11
+#define	BG_BAND_REGION_2_SIZE	2
+#define	BG_BAND_REGION_3_START	9			// 10,11,12,13
+#define	BG_BAND_REGION_3_SIZE	4
+#define	BG_BAND_REGION_4_START	13			// 14
+#define	BG_BAND_REGION_4_SIZE	1
+#define	BG_BAND_REGION_5_START	0			// 1,2,3,4,5,6,7,8,9,10,11,12,13,14
+#define	BG_BAND_REGION_5_SIZE	14
+#define	BG_BAND_REGION_6_START	2			// 3,4,5,6,7,8,9
+#define	BG_BAND_REGION_6_SIZE	7
+#define	BG_BAND_REGION_7_START	4			// 5,6,7,8,9,10,11,12,13
+#define	BG_BAND_REGION_7_SIZE	9
+#define	BG_BAND_REGION_31_START	0			// 1,2,3,4,5,6,7,8,9,10,11,12,13,14
+#define	BG_BAND_REGION_31_SIZE	14
+
+// 5 Ghz channel plan index in the TxPower arrays.
+UCHAR A_BAND_REGION_0_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_1_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+UCHAR A_BAND_REGION_2_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64};
+UCHAR A_BAND_REGION_3_CHANNEL_LIST[]={52, 56, 60, 64, 149, 153, 157, 161};
+UCHAR A_BAND_REGION_4_CHANNEL_LIST[]={149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_5_CHANNEL_LIST[]={149, 153, 157, 161};
+UCHAR A_BAND_REGION_6_CHANNEL_LIST[]={36, 40, 44, 48};
+UCHAR A_BAND_REGION_7_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_8_CHANNEL_LIST[]={52, 56, 60, 64};
+UCHAR A_BAND_REGION_9_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_10_CHANNEL_LIST[]={36, 40, 44, 48, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_11_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 149, 153, 157, 161};
+
+//BaSizeArray follows the 802.11n definition as MaxRxFactor.  2^(13+factor) bytes. When factor =0, it's about Ba buffer size =8.
+UCHAR BaSizeArray[4] = {8,16,32,64};
+
+/*
+	==========================================================================
+	Description:
+		Update StaCfg->ChannelList[] according to 1) Country Region 2) RF IC type,
+		and 3) PHY-mode user selected.
+		The outcome is used by driver when doing site survey.
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID BuildChannelList(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR i, j, index=0, num=0;
+	PUCHAR	pChannelList = NULL;
+
+	NdisZeroMemory(pAd->ChannelList, MAX_NUM_OF_CHANNELS * sizeof(CHANNEL_TX_POWER));
+
+	// if not 11a-only mode, channel list starts from 2.4Ghz band
+	if ((pAd->CommonCfg.PhyMode != PHY_11A)
+#ifdef DOT11_N_SUPPORT
+		&& (pAd->CommonCfg.PhyMode != PHY_11AN_MIXED) && (pAd->CommonCfg.PhyMode != PHY_11N_5G)
+#endif // DOT11_N_SUPPORT //
+	)
+	{
+		switch (pAd->CommonCfg.CountryRegion  & 0x7f)
+		{
+			case REGION_0_BG_BAND:	// 1 -11
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_0_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_0_SIZE);
+				index += BG_BAND_REGION_0_SIZE;
+				break;
+			case REGION_1_BG_BAND:	// 1 - 13
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_1_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_1_SIZE);
+				index += BG_BAND_REGION_1_SIZE;
+				break;
+			case REGION_2_BG_BAND:	// 10 - 11
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_2_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_2_SIZE);
+				index += BG_BAND_REGION_2_SIZE;
+				break;
+			case REGION_3_BG_BAND:	// 10 - 13
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_3_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_3_SIZE);
+				index += BG_BAND_REGION_3_SIZE;
+				break;
+			case REGION_4_BG_BAND:	// 14
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_4_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_4_SIZE);
+				index += BG_BAND_REGION_4_SIZE;
+				break;
+			case REGION_5_BG_BAND:	// 1 - 14
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_5_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_5_SIZE);
+				index += BG_BAND_REGION_5_SIZE;
+				break;
+			case REGION_6_BG_BAND:	// 3 - 9
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_6_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_6_SIZE);
+				index += BG_BAND_REGION_6_SIZE;
+				break;
+			case REGION_7_BG_BAND:  // 5 - 13
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_7_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_7_SIZE);
+				index += BG_BAND_REGION_7_SIZE;
+				break;
+			case REGION_31_BG_BAND:	// 1 - 14
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_31_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_31_SIZE);
+				index += BG_BAND_REGION_31_SIZE;
+				break;
+			default:            // Error. should never happen
+				break;
+		}
+		for (i=0; i<index; i++)
+			pAd->ChannelList[i].MaxTxPwr = 20;
+	}
+
+	if ((pAd->CommonCfg.PhyMode == PHY_11A) || (pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED)
+#ifdef DOT11_N_SUPPORT
+		|| (pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED) || (pAd->CommonCfg.PhyMode == PHY_11AN_MIXED)
+		|| (pAd->CommonCfg.PhyMode == PHY_11AGN_MIXED) || (pAd->CommonCfg.PhyMode == PHY_11N_5G)
+#endif // DOT11_N_SUPPORT //
+	)
+	{
+		switch (pAd->CommonCfg.CountryRegionForABand & 0x7f)
+		{
+			case REGION_0_A_BAND:
+				num = sizeof(A_BAND_REGION_0_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_0_CHANNEL_LIST;
+				break;
+			case REGION_1_A_BAND:
+				num = sizeof(A_BAND_REGION_1_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_1_CHANNEL_LIST;
+				break;
+			case REGION_2_A_BAND:
+				num = sizeof(A_BAND_REGION_2_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_2_CHANNEL_LIST;
+				break;
+			case REGION_3_A_BAND:
+				num = sizeof(A_BAND_REGION_3_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_3_CHANNEL_LIST;
+				break;
+			case REGION_4_A_BAND:
+				num = sizeof(A_BAND_REGION_4_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_4_CHANNEL_LIST;
+				break;
+			case REGION_5_A_BAND:
+				num = sizeof(A_BAND_REGION_5_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_5_CHANNEL_LIST;
+				break;
+			case REGION_6_A_BAND:
+				num = sizeof(A_BAND_REGION_6_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_6_CHANNEL_LIST;
+				break;
+			case REGION_7_A_BAND:
+				num = sizeof(A_BAND_REGION_7_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_7_CHANNEL_LIST;
+				break;
+			case REGION_8_A_BAND:
+				num = sizeof(A_BAND_REGION_8_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_8_CHANNEL_LIST;
+				break;
+			case REGION_9_A_BAND:
+				num = sizeof(A_BAND_REGION_9_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_9_CHANNEL_LIST;
+				break;
+
+			case REGION_10_A_BAND:
+				num = sizeof(A_BAND_REGION_10_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_10_CHANNEL_LIST;
+				break;
+
+			case REGION_11_A_BAND:
+				num = sizeof(A_BAND_REGION_11_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_11_CHANNEL_LIST;
+				break;
+
+			default:            // Error. should never happen
+				DBGPRINT(RT_DEBUG_WARN,("countryregion=%d not support", pAd->CommonCfg.CountryRegionForABand));
+				break;
+		}
+
+		if (num != 0)
+		{
+			UCHAR RadarCh[15]={52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+			for (i=0; i<num; i++)
+			{
+				for (j=0; j<MAX_NUM_OF_CHANNELS; j++)
+				{
+					if (pChannelList[i] == pAd->TxPower[j].Channel)
+						NdisMoveMemory(&pAd->ChannelList[index+i], &pAd->TxPower[j], sizeof(CHANNEL_TX_POWER));
+					}
+				for (j=0; j<15; j++)
+				{
+					if (pChannelList[i] == RadarCh[j])
+						pAd->ChannelList[index+i].DfsReq = TRUE;
+				}
+				pAd->ChannelList[index+i].MaxTxPwr = 20;
+			}
+			index += num;
+		}
+	}
+
+	pAd->ChannelListNum = index;
+	DBGPRINT(RT_DEBUG_TRACE,("country code=%d/%d, RFIC=%d, PHY mode=%d, support %d channels\n",
+		pAd->CommonCfg.CountryRegion, pAd->CommonCfg.CountryRegionForABand, pAd->RfIcType, pAd->CommonCfg.PhyMode, pAd->ChannelListNum));
+#ifdef DBG
+	for (i=0;i<pAd->ChannelListNum;i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE,("BuildChannel # %d :: Pwr0 = %d, Pwr1 =%d, \n ", pAd->ChannelList[i].Channel, pAd->ChannelList[i].Power, pAd->ChannelList[i].Power2));
+	}
+#endif
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine return the first channel number according to the country
+		code selection and RF IC selection (signal band or dual band). It is called
+		whenever driver need to start a site survey of all supported channels.
+	Return:
+		ch - the first channel number of current country code setting
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+UCHAR FirstChannel(
+	IN PRTMP_ADAPTER pAd)
+{
+	return pAd->ChannelList[0].Channel;
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine returns the next channel number. This routine is called
+		during driver need to start a site survey of all supported channels.
+	Return:
+		next_channel - the next channel number valid in current country code setting.
+	Note:
+		return 0 if no more next channel
+	==========================================================================
+ */
+UCHAR NextChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR channel)
+{
+	int i;
+	UCHAR next_channel = 0;
+
+	for (i = 0; i < (pAd->ChannelListNum - 1); i++)
+		if (channel == pAd->ChannelList[i].Channel)
+		{
+			next_channel = pAd->ChannelList[i+1].Channel;
+			break;
+	}
+	return next_channel;
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine is for Cisco Compatible Extensions 2.X
+		Spec31. AP Control of Client Transmit Power
+	Return:
+		None
+	Note:
+	   Required by Aironet dBm(mW)
+		   0dBm(1mW),   1dBm(5mW), 13dBm(20mW), 15dBm(30mW),
+		  17dBm(50mw), 20dBm(100mW)
+
+	   We supported
+		   3dBm(Lowest), 6dBm(10%), 9dBm(25%), 12dBm(50%),
+		  14dBm(75%),   15dBm(100%)
+
+		The client station's actual transmit power shall be within +/- 5dB of
+		the minimum value or next lower value.
+	==========================================================================
+ */
+VOID ChangeToCellPowerLimit(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         AironetCellPowerLimit)
+{
+	//valud 0xFF means that hasn't found power limit information
+	//from the AP's Beacon/Probe response.
+	if (AironetCellPowerLimit == 0xFF)
+		return;
+
+	if (AironetCellPowerLimit < 6) //Used Lowest Power Percentage.
+		pAd->CommonCfg.TxPowerPercentage = 6;
+	else if (AironetCellPowerLimit < 9)
+		pAd->CommonCfg.TxPowerPercentage = 10;
+	else if (AironetCellPowerLimit < 12)
+		pAd->CommonCfg.TxPowerPercentage = 25;
+	else if (AironetCellPowerLimit < 14)
+		pAd->CommonCfg.TxPowerPercentage = 50;
+	else if (AironetCellPowerLimit < 15)
+		pAd->CommonCfg.TxPowerPercentage = 75;
+	else
+		pAd->CommonCfg.TxPowerPercentage = 100; //else used maximum
+
+	if (pAd->CommonCfg.TxPowerPercentage > pAd->CommonCfg.TxPowerDefault)
+		pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+
+}
+
+CHAR	ConvertToRssi(
+	IN PRTMP_ADAPTER pAd,
+	IN	CHAR			Rssi,
+	IN  UCHAR   RssiNumber)
+{
+	UCHAR	RssiOffset, LNAGain;
+
+	// Rssi equals to zero should be an invalid value
+	if (Rssi == 0)
+		return -99;
+
+	LNAGain = GET_LNA_GAIN(pAd);
+    if (pAd->LatchRfRegs.Channel > 14)
+    {
+        if (RssiNumber == 0)
+			RssiOffset = pAd->ARssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->ARssiOffset1;
+		else
+			RssiOffset = pAd->ARssiOffset2;
+    }
+    else
+    {
+        if (RssiNumber == 0)
+			RssiOffset = pAd->BGRssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->BGRssiOffset1;
+		else
+			RssiOffset = pAd->BGRssiOffset2;
+    }
+
+    return (-12 - RssiOffset - LNAGain - Rssi);
+}
+
+/*
+	==========================================================================
+	Description:
+		Scan next channel
+	==========================================================================
+ */
+VOID ScanNextChannel(
+	IN PRTMP_ADAPTER pAd)
+{
+	HEADER_802_11   Hdr80211;
+	PUCHAR          pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG           FrameLen = 0;
+	UCHAR           SsidLen = 0, ScanType = pAd->MlmeAux.ScanType, BBPValue = 0;
+#ifdef CONFIG_STA_SUPPORT
+	USHORT          Status;
+	PHEADER_802_11  pHdr80211;
+#endif // CONFIG_STA_SUPPORT //
+	UINT			ScanTimeIn5gChannel = SHORT_CHANNEL_TIME;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (MONITOR_ON(pAd))
+			return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RALINK_ATE
+	// Nothing to do in ATE mode.
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	if (pAd->MlmeAux.Channel == 0)
+	{
+		if ((pAd->CommonCfg.BBPCurrentBW == BW_40)
+#ifdef CONFIG_STA_SUPPORT
+			&& (INFRA_ON(pAd)
+				|| (pAd->OpMode == OPMODE_AP))
+#endif // CONFIG_STA_SUPPORT //
+			)
+		{
+			AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue &= (~0x18);
+			BBPValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - End of SCAN, restore to 40MHz channel %d, Total BSS[%02d]\n",pAd->CommonCfg.CentralChannel, pAd->ScanTab.BssNr));
+		}
+		else
+		{
+			AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - End of SCAN, restore to channel %d, Total BSS[%02d]\n",pAd->CommonCfg.Channel, pAd->ScanTab.BssNr));
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			//
+			// To prevent data lost.
+			// Send an NULL data with turned PSM bit on to current associated AP before SCAN progress.
+			// Now, we need to send an NULL data with turned PSM bit off to AP, when scan progress done
+			//
+			if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) && (INFRA_ON(pAd)))
+			{
+				NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);
+				if (NStatus	== NDIS_STATUS_SUCCESS)
+				{
+					pHdr80211 = (PHEADER_802_11) pOutBuffer;
+					MgtMacHeaderInit(pAd, pHdr80211, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+					pHdr80211->Duration = 0;
+					pHdr80211->FC.Type = BTYPE_DATA;
+					pHdr80211->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
+
+					// Send using priority queue
+					MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+					DBGPRINT(RT_DEBUG_TRACE, ("MlmeScanReqAction -- Send PSM Data frame\n"));
+					MlmeFreeMemory(pAd, pOutBuffer);
+					RTMPusecDelay(5000);
+				}
+			}
+
+			pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+			Status = MLME_SUCCESS;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+
+		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+	}
+#ifdef RT2870
+#ifdef CONFIG_STA_SUPPORT
+	else if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST) && (pAd->OpMode == OPMODE_STA))
+	{
+		pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+		MlmeCntlConfirm(pAd, MT2_SCAN_CONF, MLME_FAIL_NO_RESOURCE);
+	}
+#endif // CONFIG_STA_SUPPORT //
+#endif // RT2870 //
+	else
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+		// BBP and RF are not accessible in PS mode, we has to wake them up first
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+			AsicForceWakeup(pAd, TRUE);
+
+			// leave PSM during scanning. otherwise we may lost ProbeRsp & BEACON
+			if (pAd->StaCfg.Psm == PWR_SAVE)
+				MlmeSetPsmBit(pAd, PWR_ACTIVE);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		AsicSwitchChannel(pAd, pAd->MlmeAux.Channel, TRUE);
+		AsicLockChannel(pAd, pAd->MlmeAux.Channel);
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (pAd->MlmeAux.Channel > 14)
+			{
+				if ((pAd->CommonCfg.bIEEE80211H == 1) && RadarChannelCheck(pAd, pAd->MlmeAux.Channel))
+				{
+					ScanType = SCAN_PASSIVE;
+					ScanTimeIn5gChannel = MIN_CHANNEL_TIME;
+				}
+			}
+
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+			// carrier detection
+			if (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+			{
+				ScanType = SCAN_PASSIVE;
+				ScanTimeIn5gChannel = MIN_CHANNEL_TIME;
+			}
+#endif // CARRIER_DETECTION_SUPPORT //
+		}
+
+#endif // CONFIG_STA_SUPPORT //
+
+		//Global country domain(ch1-11:active scan, ch12-14 passive scan)
+		if ((pAd->MlmeAux.Channel <= 14) && (pAd->MlmeAux.Channel >= 12) && ((pAd->CommonCfg.CountryRegion & 0x7f) == REGION_31_BG_BAND))
+		{
+			ScanType = SCAN_PASSIVE;
+		}
+
+		// We need to shorten active scan time in order for WZC connect issue
+		// Chnage the channel scan time for CISCO stuff based on its IAPP announcement
+		if (ScanType == FAST_SCAN_ACTIVE)
+			RTMPSetTimer(&pAd->MlmeAux.ScanTimer, FAST_ACTIVE_SCAN_TIME);
+#ifdef CONFIG_STA_SUPPORT
+		else if (((ScanType == SCAN_CISCO_ACTIVE) ||
+				(ScanType == SCAN_CISCO_PASSIVE) ||
+				(ScanType == SCAN_CISCO_CHANNEL_LOAD) ||
+				(ScanType == SCAN_CISCO_NOISE)) && (pAd->OpMode == OPMODE_STA))
+		{
+			if (pAd->StaCfg.CCXScanTime < 25)
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, pAd->StaCfg.CCXScanTime * 2);
+			else
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, pAd->StaCfg.CCXScanTime);
+		}
+#endif // CONFIG_STA_SUPPORT //
+		else // must be SCAN_PASSIVE or SCAN_ACTIVE
+		{
+			if ((pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED)
+#ifdef DOT11_N_SUPPORT
+				|| (pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED) || (pAd->CommonCfg.PhyMode == PHY_11AGN_MIXED)
+#endif // DOT11_N_SUPPORT //
+			)
+			{
+				if (pAd->MlmeAux.Channel > 14)
+					RTMPSetTimer(&pAd->MlmeAux.ScanTimer, ScanTimeIn5gChannel);
+				else
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, MIN_CHANNEL_TIME);
+			}
+			else
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, MAX_CHANNEL_TIME);
+		}
+
+		if ((ScanType == SCAN_ACTIVE) || (ScanType == FAST_SCAN_ACTIVE) ||
+			(ScanType == SCAN_CISCO_ACTIVE))
+		{
+			NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("SYNC - ScanNextChannel() allocate memory fail\n"));
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+					Status = MLME_FAIL_NO_RESOURCE;
+					MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+				}
+#endif // CONFIG_STA_SUPPORT //
+
+				return;
+			}
+
+			// There is no need to send broadcast probe request if active scan is in effect.
+			if ((ScanType == SCAN_ACTIVE) || (ScanType == FAST_SCAN_ACTIVE)
+				)
+				SsidLen = pAd->MlmeAux.SsidLen;
+			else
+				SsidLen = 0;
+
+			MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, BROADCAST_ADDR, BROADCAST_ADDR);
+			MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+							  sizeof(HEADER_802_11),    &Hdr80211,
+							  1,                        &SsidIe,
+							  1,                        &SsidLen,
+							  SsidLen,			        pAd->MlmeAux.Ssid,
+							  1,                        &SupRateIe,
+							  1,                        &pAd->CommonCfg.SupRateLen,
+							  pAd->CommonCfg.SupRateLen,  pAd->CommonCfg.SupRate,
+							  END_OF_ARGS);
+
+			if (pAd->CommonCfg.ExtRateLen)
+			{
+				ULONG Tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &Tmp,
+								  1,                                &ExtRateIe,
+								  1,                                &pAd->CommonCfg.ExtRateLen,
+								  pAd->CommonCfg.ExtRateLen,          pAd->CommonCfg.ExtRate,
+								  END_OF_ARGS);
+				FrameLen += Tmp;
+			}
+
+#ifdef DOT11_N_SUPPORT
+			if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+			{
+				ULONG	Tmp;
+				UCHAR	HtLen;
+				UCHAR	BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+#ifdef RT_BIG_ENDIAN
+				HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+				if (pAd->bBroadComHT == TRUE)
+				{
+					HtLen = pAd->MlmeAux.HtCapabilityLen + 4;
+#ifdef RT_BIG_ENDIAN
+					NdisMoveMemory(&HtCapabilityTmp, &pAd->MlmeAux.HtCapability, SIZE_HT_CAP_IE);
+					*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+					*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &WpaIe,
+									1,                                &HtLen,
+									4,                                &BROADCOM[0],
+									pAd->MlmeAux.HtCapabilityLen,     &HtCapabilityTmp,
+									END_OF_ARGS);
+#else
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &WpaIe,
+									1,                                &HtLen,
+									4,                                &BROADCOM[0],
+									pAd->MlmeAux.HtCapabilityLen,     &pAd->MlmeAux.HtCapability,
+									END_OF_ARGS);
+#endif // RT_BIG_ENDIAN //
+				}
+				else
+				{
+					HtLen = pAd->MlmeAux.HtCapabilityLen;
+#ifdef RT_BIG_ENDIAN
+					NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, SIZE_HT_CAP_IE);
+					*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+					*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &HtCapIe,
+									1,                                &HtLen,
+									HtLen,                            &HtCapabilityTmp,
+									END_OF_ARGS);
+#else
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &HtCapIe,
+									1,                                &HtLen,
+									HtLen,                            &pAd->CommonCfg.HtCapability,
+									END_OF_ARGS);
+#endif // RT_BIG_ENDIAN //
+				}
+				FrameLen += Tmp;
+
+#ifdef DOT11N_DRAFT3
+				if (pAd->CommonCfg.BACapability.field.b2040CoexistScanSup == 1)
+				{
+					ULONG		Tmp;
+					HtLen = 1;
+					MakeOutgoingFrame(pOutBuffer + FrameLen,            &Tmp,
+									  1,					&ExtHtCapIe,
+									  1,					&HtLen,
+									  1,          			&pAd->CommonCfg.BSSCoexist2040.word,
+									  END_OF_ARGS);
+
+					FrameLen += Tmp;
+				}
+#endif // DOT11N_DRAFT3 //
+			}
+#endif // DOT11_N_SUPPORT //
+
+
+			MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+
+		// For SCAN_CISCO_PASSIVE, do nothing and silently wait for beacon or other probe reponse
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			pAd->Mlme.SyncMachine.CurrState = SCAN_LISTEN;
+#endif // CONFIG_STA_SUPPORT //
+
+	}
+}
+
+VOID MgtProbReqMacHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PHEADER_802_11 pHdr80211,
+	IN UCHAR SubType,
+	IN UCHAR ToDs,
+	IN PUCHAR pDA,
+	IN PUCHAR pBssid)
+{
+	NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+
+	pHdr80211->FC.Type = BTYPE_MGMT;
+	pHdr80211->FC.SubType = SubType;
+	if (SubType == SUBTYPE_ACK)
+		pHdr80211->FC.Type = BTYPE_CNTL;
+	pHdr80211->FC.ToDs = ToDs;
+	COPY_MAC_ADDR(pHdr80211->Addr1, pDA);
+	COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pHdr80211->Addr3, pBssid);
+}
+
+
diff --git a/drivers/staging/rt2870/common/cmm_wpa.c b/drivers/staging/rt2870/common/cmm_wpa.c
new file mode 100644
index 000000000000..d2c24bd49e75
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_wpa.c
@@ -0,0 +1,1654 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	wpa.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Jan	Lee		03-07-22		Initial
+	Paul Lin	03-11-28		Modify for supplicant
+*/
+#include "../rt_config.h"
+// WPA OUI
+UCHAR		OUI_WPA_NONE_AKM[4]		= {0x00, 0x50, 0xF2, 0x00};
+UCHAR       OUI_WPA_VERSION[4]      = {0x00, 0x50, 0xF2, 0x01};
+UCHAR       OUI_WPA_WEP40[4]      = {0x00, 0x50, 0xF2, 0x01};
+UCHAR       OUI_WPA_TKIP[4]     = {0x00, 0x50, 0xF2, 0x02};
+UCHAR       OUI_WPA_CCMP[4]     = {0x00, 0x50, 0xF2, 0x04};
+UCHAR       OUI_WPA_WEP104[4]      = {0x00, 0x50, 0xF2, 0x05};
+UCHAR       OUI_WPA_8021X_AKM[4]	= {0x00, 0x50, 0xF2, 0x01};
+UCHAR       OUI_WPA_PSK_AKM[4]      = {0x00, 0x50, 0xF2, 0x02};
+// WPA2 OUI
+UCHAR       OUI_WPA2_WEP40[4]   = {0x00, 0x0F, 0xAC, 0x01};
+UCHAR       OUI_WPA2_TKIP[4]        = {0x00, 0x0F, 0xAC, 0x02};
+UCHAR       OUI_WPA2_CCMP[4]        = {0x00, 0x0F, 0xAC, 0x04};
+UCHAR       OUI_WPA2_8021X_AKM[4]   = {0x00, 0x0F, 0xAC, 0x01};
+UCHAR       OUI_WPA2_PSK_AKM[4]   	= {0x00, 0x0F, 0xAC, 0x02};
+UCHAR       OUI_WPA2_WEP104[4]   = {0x00, 0x0F, 0xAC, 0x05};
+// MSA OUI
+UCHAR   	OUI_MSA_8021X_AKM[4]    = {0x00, 0x0F, 0xAC, 0x05};		// Not yet final - IEEE 802.11s-D1.06
+UCHAR   	OUI_MSA_PSK_AKM[4]   	= {0x00, 0x0F, 0xAC, 0x06};		// Not yet final - IEEE 802.11s-D1.06
+
+/*
+	========================================================================
+
+	Routine Description:
+		The pseudo-random function(PRF) that hashes various inputs to
+		derive a pseudo-random value. To add liveness to the pseudo-random
+		value, a nonce should be one of the inputs.
+
+		It is used to generate PTK, GTK or some specific random value.
+
+	Arguments:
+		UCHAR	*key,		-	the key material for HMAC_SHA1 use
+		INT		key_len		-	the length of key
+		UCHAR	*prefix		-	a prefix label
+		INT		prefix_len	-	the length of the label
+		UCHAR	*data		-	a specific data with variable length
+		INT		data_len	-	the length of a specific data
+		INT		len			-	the output lenght
+
+	Return Value:
+		UCHAR	*output		-	the calculated result
+
+	Note:
+		802.11i-2004	Annex H.3
+
+	========================================================================
+*/
+VOID	PRF(
+	IN	UCHAR	*key,
+	IN	INT		key_len,
+	IN	UCHAR	*prefix,
+	IN	INT		prefix_len,
+	IN	UCHAR	*data,
+	IN	INT		data_len,
+	OUT	UCHAR	*output,
+	IN	INT		len)
+{
+	INT		i;
+    UCHAR   *input;
+	INT		currentindex = 0;
+	INT		total_len;
+
+	// Allocate memory for input
+	os_alloc_mem(NULL, (PUCHAR *)&input, 1024);
+
+    if (input == NULL)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("!!!PRF: no memory!!!\n"));
+        return;
+    }
+
+	// Generate concatenation input
+	NdisMoveMemory(input, prefix, prefix_len);
+
+	// Concatenate a single octet containing 0
+	input[prefix_len] =	0;
+
+	// Concatenate specific data
+	NdisMoveMemory(&input[prefix_len + 1], data, data_len);
+	total_len =	prefix_len + 1 + data_len;
+
+	// Concatenate a single octet containing 0
+	// This octet shall be update later
+	input[total_len] = 0;
+	total_len++;
+
+	// Iterate to calculate the result by hmac-sha-1
+	// Then concatenate to last result
+	for	(i = 0;	i <	(len + 19) / 20; i++)
+	{
+		HMAC_SHA1(input, total_len,	key, key_len, &output[currentindex]);
+		currentindex +=	20;
+
+		// update the last octet
+		input[total_len - 1]++;
+	}
+    os_free_mem(NULL, input);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		It utilizes PRF-384 or PRF-512 to derive session-specific keys from a PMK.
+		It shall be called by 4-way handshake processing.
+
+	Arguments:
+		pAd 	-	pointer to our pAdapter context
+		PMK		-	pointer to PMK
+		ANonce	-	pointer to ANonce
+		AA		-	pointer to Authenticator Address
+		SNonce	-	pointer to SNonce
+		SA		-	pointer to Supplicant Address
+		len		-	indicate the length of PTK (octet)
+
+	Return Value:
+		Output		pointer to the PTK
+
+	Note:
+		Refer to IEEE 802.11i-2004 8.5.1.2
+
+	========================================================================
+*/
+VOID WpaCountPTK(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	*PMK,
+	IN	UCHAR	*ANonce,
+	IN	UCHAR	*AA,
+	IN	UCHAR	*SNonce,
+	IN	UCHAR	*SA,
+	OUT	UCHAR	*output,
+	IN	UINT	len)
+{
+	UCHAR	concatenation[76];
+	UINT	CurrPos = 0;
+	UCHAR	temp[32];
+	UCHAR	Prefix[] = {'P', 'a', 'i', 'r', 'w', 'i', 's', 'e', ' ', 'k', 'e', 'y', ' ',
+						'e', 'x', 'p', 'a', 'n', 's', 'i', 'o', 'n'};
+
+	// initiate the concatenation input
+	NdisZeroMemory(temp, sizeof(temp));
+	NdisZeroMemory(concatenation, 76);
+
+	// Get smaller address
+	if (RTMPCompareMemory(SA, AA, 6) == 1)
+		NdisMoveMemory(concatenation, AA, 6);
+	else
+		NdisMoveMemory(concatenation, SA, 6);
+	CurrPos += 6;
+
+	// Get larger address
+	if (RTMPCompareMemory(SA, AA, 6) == 1)
+		NdisMoveMemory(&concatenation[CurrPos], SA, 6);
+	else
+		NdisMoveMemory(&concatenation[CurrPos], AA, 6);
+
+	// store the larger mac address for backward compatible of
+	// ralink proprietary STA-key issue
+	NdisMoveMemory(temp, &concatenation[CurrPos], MAC_ADDR_LEN);
+	CurrPos += 6;
+
+	// Get smaller Nonce
+	if (RTMPCompareMemory(ANonce, SNonce, 32) == 0)
+		NdisMoveMemory(&concatenation[CurrPos], temp, 32);	// patch for ralink proprietary STA-key issue
+	else if (RTMPCompareMemory(ANonce, SNonce, 32) == 1)
+		NdisMoveMemory(&concatenation[CurrPos], SNonce, 32);
+	else
+		NdisMoveMemory(&concatenation[CurrPos], ANonce, 32);
+	CurrPos += 32;
+
+	// Get larger Nonce
+	if (RTMPCompareMemory(ANonce, SNonce, 32) == 0)
+		NdisMoveMemory(&concatenation[CurrPos], temp, 32);	// patch for ralink proprietary STA-key issue
+	else if (RTMPCompareMemory(ANonce, SNonce, 32) == 1)
+		NdisMoveMemory(&concatenation[CurrPos], ANonce, 32);
+	else
+		NdisMoveMemory(&concatenation[CurrPos], SNonce, 32);
+	CurrPos += 32;
+
+	hex_dump("concatenation=", concatenation, 76);
+
+	// Use PRF to generate PTK
+	PRF(PMK, LEN_MASTER_KEY, Prefix, 22, concatenation, 76, output, len);
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Generate random number by software.
+
+	Arguments:
+		pAd		-	pointer to our pAdapter context
+		macAddr	-	pointer to local MAC address
+
+	Return Value:
+
+	Note:
+		802.1ii-2004  Annex H.5
+
+	========================================================================
+*/
+VOID	GenRandom(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			*macAddr,
+	OUT	UCHAR			*random)
+{
+	INT		i, curr;
+	UCHAR	local[80], KeyCounter[32];
+	UCHAR	result[80];
+	ULONG	CurrentTime;
+	UCHAR	prefix[] = {'I', 'n', 'i', 't', ' ', 'C', 'o', 'u', 'n', 't', 'e', 'r'};
+
+	// Zero the related information
+	NdisZeroMemory(result, 80);
+	NdisZeroMemory(local, 80);
+	NdisZeroMemory(KeyCounter, 32);
+
+	for	(i = 0;	i <	32;	i++)
+	{
+		// copy the local MAC address
+		COPY_MAC_ADDR(local, macAddr);
+		curr =	MAC_ADDR_LEN;
+
+		// concatenate the current time
+		NdisGetSystemUpTime(&CurrentTime);
+		NdisMoveMemory(&local[curr],  &CurrentTime,	sizeof(CurrentTime));
+		curr +=	sizeof(CurrentTime);
+
+		// concatenate the last result
+		NdisMoveMemory(&local[curr],  result, 32);
+		curr +=	32;
+
+		// concatenate a variable
+		NdisMoveMemory(&local[curr],  &i,  2);
+		curr +=	2;
+
+		// calculate the result
+		PRF(KeyCounter, 32, prefix,12, local, curr, result, 32);
+	}
+
+	NdisMoveMemory(random, result,	32);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build cipher suite in RSN-IE.
+		It only shall be called by RTMPMakeRSNIE.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	ElementID	-	indicate the WPA1 or WPA2
+    	WepStatus	-	indicate the encryption type
+		bMixCipher	-	a boolean to indicate the pairwise cipher and group
+						cipher are the same or not
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+static VOID RTMPInsertRsnIeCipher(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			ElementID,
+	IN	UINT			WepStatus,
+	IN	BOOLEAN			bMixCipher,
+	IN	UCHAR			FlexibleCipher,
+	OUT	PUCHAR			pRsnIe,
+	OUT	UCHAR			*rsn_len)
+{
+	UCHAR	PairwiseCnt;
+
+	*rsn_len = 0;
+
+	// decide WPA2 or WPA1
+	if (ElementID == Wpa2Ie)
+	{
+		RSNIE2	*pRsnie_cipher = (RSNIE2*)pRsnIe;
+
+		// Assign the verson as 1
+		pRsnie_cipher->version = 1;
+
+        switch (WepStatus)
+        {
+        	// TKIP mode
+            case Ndis802_11Encryption2Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_TKIP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_TKIP, 4);
+                *rsn_len = sizeof(RSNIE2);
+                break;
+
+			// AES mode
+            case Ndis802_11Encryption3Enabled:
+				if (bMixCipher)
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_TKIP, 4);
+				else
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_CCMP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_CCMP, 4);
+                *rsn_len = sizeof(RSNIE2);
+                break;
+
+			// TKIP-AES mix mode
+            case Ndis802_11Encryption4Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_TKIP, 4);
+
+				PairwiseCnt = 1;
+				// Insert WPA2 TKIP as the first pairwise cipher
+				if (MIX_CIPHER_WPA2_TKIP_ON(FlexibleCipher))
+				{
+                	NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_TKIP, 4);
+					// Insert WPA2 AES as the secondary pairwise cipher
+					if (MIX_CIPHER_WPA2_AES_ON(FlexibleCipher))
+					{
+                		NdisMoveMemory(pRsnie_cipher->ucast[0].oui + 4, OUI_WPA2_CCMP, 4);
+						PairwiseCnt = 2;
+					}
+				}
+				else
+				{
+					// Insert WPA2 AES as the first pairwise cipher
+					NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_CCMP, 4);
+				}
+
+                pRsnie_cipher->ucount = PairwiseCnt;
+                *rsn_len = sizeof(RSNIE2) + (4 * (PairwiseCnt - 1));
+                break;
+        }
+
+#ifdef CONFIG_STA_SUPPORT
+		if ((pAd->OpMode == OPMODE_STA) &&
+			(pAd->StaCfg.GroupCipher != Ndis802_11Encryption2Enabled) &&
+			(pAd->StaCfg.GroupCipher != Ndis802_11Encryption3Enabled))
+		{
+			UINT GroupCipher = pAd->StaCfg.GroupCipher;
+			switch(GroupCipher)
+			{
+				case Ndis802_11GroupWEP40Enabled:
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_WEP40, 4);
+					break;
+				case Ndis802_11GroupWEP104Enabled:
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_WEP104, 4);
+					break;
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		// swap for big-endian platform
+		pRsnie_cipher->version = cpu2le16(pRsnie_cipher->version);
+	    pRsnie_cipher->ucount = cpu2le16(pRsnie_cipher->ucount);
+	}
+	else
+	{
+		RSNIE	*pRsnie_cipher = (RSNIE*)pRsnIe;
+
+		// Assign OUI and version
+		NdisMoveMemory(pRsnie_cipher->oui, OUI_WPA_VERSION, 4);
+        pRsnie_cipher->version = 1;
+
+		switch (WepStatus)
+		{
+			// TKIP mode
+            case Ndis802_11Encryption2Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_TKIP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_TKIP, 4);
+                *rsn_len = sizeof(RSNIE);
+                break;
+
+			// AES mode
+            case Ndis802_11Encryption3Enabled:
+				if (bMixCipher)
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_TKIP, 4);
+				else
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_CCMP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_CCMP, 4);
+                *rsn_len = sizeof(RSNIE);
+                break;
+
+			// TKIP-AES mix mode
+            case Ndis802_11Encryption4Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_TKIP, 4);
+
+				PairwiseCnt = 1;
+				// Insert WPA TKIP as the first pairwise cipher
+				if (MIX_CIPHER_WPA_TKIP_ON(FlexibleCipher))
+				{
+                	NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_TKIP, 4);
+					// Insert WPA AES as the secondary pairwise cipher
+					if (MIX_CIPHER_WPA_AES_ON(FlexibleCipher))
+					{
+                		NdisMoveMemory(pRsnie_cipher->ucast[0].oui + 4, OUI_WPA_CCMP, 4);
+						PairwiseCnt = 2;
+					}
+				}
+				else
+				{
+					// Insert WPA AES as the first pairwise cipher
+					NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_CCMP, 4);
+				}
+
+                pRsnie_cipher->ucount = PairwiseCnt;
+                *rsn_len = sizeof(RSNIE) + (4 * (PairwiseCnt - 1));
+                break;
+        }
+
+#ifdef CONFIG_STA_SUPPORT
+		if ((pAd->OpMode == OPMODE_STA) &&
+			(pAd->StaCfg.GroupCipher != Ndis802_11Encryption2Enabled) &&
+			(pAd->StaCfg.GroupCipher != Ndis802_11Encryption3Enabled))
+		{
+			UINT GroupCipher = pAd->StaCfg.GroupCipher;
+			switch(GroupCipher)
+			{
+				case Ndis802_11GroupWEP40Enabled:
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_WEP40, 4);
+					break;
+				case Ndis802_11GroupWEP104Enabled:
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_WEP104, 4);
+					break;
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		// swap for big-endian platform
+		pRsnie_cipher->version = cpu2le16(pRsnie_cipher->version);
+	    pRsnie_cipher->ucount = cpu2le16(pRsnie_cipher->ucount);
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build AKM suite in RSN-IE.
+		It only shall be called by RTMPMakeRSNIE.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	ElementID	-	indicate the WPA1 or WPA2
+    	AuthMode	-	indicate the authentication mode
+		apidx		-	indicate the interface index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+static VOID RTMPInsertRsnIeAKM(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			ElementID,
+	IN	UINT			AuthMode,
+	IN	UCHAR			apidx,
+	OUT	PUCHAR			pRsnIe,
+	OUT	UCHAR			*rsn_len)
+{
+	RSNIE_AUTH		*pRsnie_auth;
+
+	pRsnie_auth = (RSNIE_AUTH*)(pRsnIe + (*rsn_len));
+
+	// decide WPA2 or WPA1
+	if (ElementID == Wpa2Ie)
+	{
+		switch (AuthMode)
+        {
+            case Ndis802_11AuthModeWPA2:
+            case Ndis802_11AuthModeWPA1WPA2:
+                pRsnie_auth->acount = 1;
+                	NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA2_8021X_AKM, 4);
+                break;
+
+            case Ndis802_11AuthModeWPA2PSK:
+            case Ndis802_11AuthModeWPA1PSKWPA2PSK:
+                pRsnie_auth->acount = 1;
+                	NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA2_PSK_AKM, 4);
+                break;
+        }
+	}
+	else
+	{
+		switch (AuthMode)
+        {
+            case Ndis802_11AuthModeWPA:
+            case Ndis802_11AuthModeWPA1WPA2:
+                pRsnie_auth->acount = 1;
+                NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA_8021X_AKM, 4);
+                break;
+
+            case Ndis802_11AuthModeWPAPSK:
+            case Ndis802_11AuthModeWPA1PSKWPA2PSK:
+                pRsnie_auth->acount = 1;
+                NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA_PSK_AKM, 4);
+                break;
+
+			case Ndis802_11AuthModeWPANone:
+                pRsnie_auth->acount = 1;
+                NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA_NONE_AKM, 4);
+                break;
+        }
+	}
+
+	pRsnie_auth->acount = cpu2le16(pRsnie_auth->acount);
+
+	(*rsn_len) += sizeof(RSNIE_AUTH);	// update current RSNIE length
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build capability in RSN-IE.
+		It only shall be called by RTMPMakeRSNIE.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	ElementID	-	indicate the WPA1 or WPA2
+		apidx		-	indicate the interface index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+static VOID RTMPInsertRsnIeCap(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			ElementID,
+	IN	UCHAR			apidx,
+	OUT	PUCHAR			pRsnIe,
+	OUT	UCHAR			*rsn_len)
+{
+	RSN_CAPABILITIES    *pRSN_Cap;
+
+	// it could be ignored in WPA1 mode
+	if (ElementID == WpaIe)
+		return;
+
+	pRSN_Cap = (RSN_CAPABILITIES*)(pRsnIe + (*rsn_len));
+
+
+	pRSN_Cap->word = cpu2le16(pRSN_Cap->word);
+
+	(*rsn_len) += sizeof(RSN_CAPABILITIES);	// update current RSNIE length
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build RSN IE context. It is not included element-ID and length.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	AuthMode	-	indicate the authentication mode
+    	WepStatus	-	indicate the encryption type
+		apidx		-	indicate the interface index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPMakeRSNIE(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  UINT            AuthMode,
+    IN  UINT            WepStatus,
+	IN	UCHAR			apidx)
+{
+	PUCHAR		pRsnIe = NULL;			// primary RSNIE
+	UCHAR 		*rsnielen_cur_p = 0;	// the length of the primary RSNIE
+	UCHAR		*rsnielen_ex_cur_p = 0;	// the length of the secondary RSNIE
+	UCHAR		PrimaryRsnie;
+	BOOLEAN		bMixCipher = FALSE;	// indicate the pairwise and group cipher are different
+	UCHAR		p_offset;
+	WPA_MIX_PAIR_CIPHER		FlexibleCipher = MIX_CIPHER_NOTUSE;	// it provide the more flexible cipher combination in WPA-WPA2 and TKIPAES mode
+
+	rsnielen_cur_p = NULL;
+	rsnielen_ex_cur_p = NULL;
+
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef WPA_SUPPLICANT_SUPPORT
+			if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+			{
+				if (AuthMode < Ndis802_11AuthModeWPA)
+					return;
+			}
+			else
+#endif // WPA_SUPPLICANT_SUPPORT //
+			{
+				// Support WPAPSK or WPA2PSK in STA-Infra mode
+				// Support WPANone in STA-Adhoc mode
+				if ((AuthMode != Ndis802_11AuthModeWPAPSK) &&
+					(AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+					(AuthMode != Ndis802_11AuthModeWPANone)
+					)
+					return;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,("==> RTMPMakeRSNIE(STA)\n"));
+
+			// Zero RSNIE context
+			pAd->StaCfg.RSNIE_Len = 0;
+			NdisZeroMemory(pAd->StaCfg.RSN_IE, MAX_LEN_OF_RSNIE);
+
+			// Pointer to RSNIE
+			rsnielen_cur_p = &pAd->StaCfg.RSNIE_Len;
+			pRsnIe = pAd->StaCfg.RSN_IE;
+
+			bMixCipher = pAd->StaCfg.bMixCipher;
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// indicate primary RSNIE as WPA or WPA2
+	if ((AuthMode == Ndis802_11AuthModeWPA) ||
+		(AuthMode == Ndis802_11AuthModeWPAPSK) ||
+		(AuthMode == Ndis802_11AuthModeWPANone) ||
+		(AuthMode == Ndis802_11AuthModeWPA1WPA2) ||
+		(AuthMode == Ndis802_11AuthModeWPA1PSKWPA2PSK))
+		PrimaryRsnie = WpaIe;
+	else
+		PrimaryRsnie = Wpa2Ie;
+
+	{
+		// Build the primary RSNIE
+		// 1. insert cipher suite
+		RTMPInsertRsnIeCipher(pAd, PrimaryRsnie, WepStatus, bMixCipher, FlexibleCipher, pRsnIe, &p_offset);
+
+		// 2. insert AKM
+		RTMPInsertRsnIeAKM(pAd, PrimaryRsnie, AuthMode, apidx, pRsnIe, &p_offset);
+
+		// 3. insert capability
+		RTMPInsertRsnIeCap(pAd, PrimaryRsnie, apidx, pRsnIe, &p_offset);
+	}
+
+	// 4. update the RSNIE length
+	*rsnielen_cur_p = p_offset;
+
+	hex_dump("The primary RSNIE", pRsnIe, (*rsnielen_cur_p));
+
+
+}
+
+/*
+    ==========================================================================
+    Description:
+		Check whether the received frame is EAP frame.
+
+	Arguments:
+		pAd				-	pointer to our pAdapter context
+		pEntry			-	pointer to active entry
+		pData			-	the received frame
+		DataByteCount 	-	the received frame's length
+		FromWhichBSSID	-	indicate the interface index
+
+    Return:
+         TRUE 			-	This frame is EAP frame
+         FALSE 			-	otherwise
+    ==========================================================================
+*/
+BOOLEAN RTMPCheckWPAframe(
+    IN PRTMP_ADAPTER    pAd,
+    IN PMAC_TABLE_ENTRY	pEntry,
+    IN PUCHAR           pData,
+    IN ULONG            DataByteCount,
+	IN UCHAR			FromWhichBSSID)
+{
+	ULONG	Body_len;
+	BOOLEAN Cancelled;
+
+
+    if(DataByteCount < (LENGTH_802_1_H + LENGTH_EAPOL_H))
+        return FALSE;
+
+
+	// Skip LLC header
+    if (NdisEqualMemory(SNAP_802_1H, pData, 6) ||
+        // Cisco 1200 AP may send packet with SNAP_BRIDGE_TUNNEL
+        NdisEqualMemory(SNAP_BRIDGE_TUNNEL, pData, 6))
+    {
+        pData += 6;
+    }
+	// Skip 2-bytes EAPoL type
+    if (NdisEqualMemory(EAPOL, pData, 2))
+    {
+        pData += 2;
+    }
+    else
+        return FALSE;
+
+    switch (*(pData+1))
+    {
+        case EAPPacket:
+			Body_len = (*(pData+2)<<8) | (*(pData+3));
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAP-Packet frame, TYPE = 0, Length = %ld\n", Body_len));
+            break;
+        case EAPOLStart:
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL-Start frame, TYPE = 1 \n"));
+			if (pEntry->EnqueueEapolStartTimerRunning != EAPOL_START_DISABLE)
+            {
+            	DBGPRINT(RT_DEBUG_TRACE, ("Cancel the EnqueueEapolStartTimerRunning \n"));
+                RTMPCancelTimer(&pEntry->EnqueueStartForPSKTimer, &Cancelled);
+                pEntry->EnqueueEapolStartTimerRunning = EAPOL_START_DISABLE;
+            }
+            break;
+        case EAPOLLogoff:
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOLLogoff frame, TYPE = 2 \n"));
+            break;
+        case EAPOLKey:
+			Body_len = (*(pData+2)<<8) | (*(pData+3));
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL-Key frame, TYPE = 3, Length = %ld\n", Body_len));
+            break;
+        case EAPOLASFAlert:
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOLASFAlert frame, TYPE = 4 \n"));
+            break;
+        default:
+            return FALSE;
+
+    }
+    return TRUE;
+}
+
+
+/*
+    ==========================================================================
+    Description:
+        ENCRYPT AES GTK before sending in EAPOL frame.
+        AES GTK length = 128 bit,  so fix blocks for aes-key-wrap as 2 in this function.
+        This function references to RFC 3394 for aes key wrap algorithm.
+    Return:
+    ==========================================================================
+*/
+VOID AES_GTK_KEY_WRAP(
+    IN UCHAR    *key,
+    IN UCHAR    *plaintext,
+    IN UCHAR    p_len,
+    OUT UCHAR   *ciphertext)
+{
+    UCHAR       A[8], BIN[16], BOUT[16];
+    UCHAR       R[512];
+    INT         num_blocks = p_len/8;   // unit:64bits
+    INT         i, j;
+    aes_context aesctx;
+    UCHAR       xor;
+
+    rtmp_aes_set_key(&aesctx, key, 128);
+
+    // Init IA
+    for (i = 0; i < 8; i++)
+        A[i] = 0xa6;
+
+    //Input plaintext
+    for (i = 0; i < num_blocks; i++)
+    {
+        for (j = 0 ; j < 8; j++)
+            R[8 * (i + 1) + j] = plaintext[8 * i + j];
+    }
+
+    // Key Mix
+    for (j = 0; j < 6; j++)
+    {
+        for(i = 1; i <= num_blocks; i++)
+        {
+            //phase 1
+            NdisMoveMemory(BIN, A, 8);
+            NdisMoveMemory(&BIN[8], &R[8 * i], 8);
+            rtmp_aes_encrypt(&aesctx, BIN, BOUT);
+
+            NdisMoveMemory(A, &BOUT[0], 8);
+            xor = num_blocks * j + i;
+            A[7] = BOUT[7] ^ xor;
+            NdisMoveMemory(&R[8 * i], &BOUT[8], 8);
+        }
+    }
+
+    // Output ciphertext
+    NdisMoveMemory(ciphertext, A, 8);
+
+    for (i = 1; i <= num_blocks; i++)
+    {
+        for (j = 0 ; j < 8; j++)
+            ciphertext[8 * i + j] = R[8 * i + j];
+    }
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Misc function to decrypt AES body
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+		This function references to	RFC	3394 for aes key unwrap algorithm.
+
+	========================================================================
+*/
+VOID	AES_GTK_KEY_UNWRAP(
+	IN	UCHAR	*key,
+	OUT	UCHAR	*plaintext,
+	IN	UCHAR    c_len,
+	IN	UCHAR	*ciphertext)
+
+{
+	UCHAR       A[8], BIN[16], BOUT[16];
+	UCHAR       xor;
+	INT         i, j;
+	aes_context aesctx;
+	UCHAR       *R;
+	INT         num_blocks = c_len/8;	// unit:64bits
+
+
+	os_alloc_mem(NULL, (PUCHAR *)&R, 512);
+
+	if (R == NULL)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("!!!AES_GTK_KEY_UNWRAP: no memory!!!\n"));
+        return;
+    } /* End of if */
+
+	// Initialize
+	NdisMoveMemory(A, ciphertext, 8);
+	//Input plaintext
+	for(i = 0; i < (c_len-8); i++)
+	{
+		R[ i] = ciphertext[i + 8];
+	}
+
+	rtmp_aes_set_key(&aesctx, key, 128);
+
+	for(j = 5; j >= 0; j--)
+	{
+		for(i = (num_blocks-1); i > 0; i--)
+		{
+			xor = (num_blocks -1 )* j + i;
+			NdisMoveMemory(BIN, A, 8);
+			BIN[7] = A[7] ^ xor;
+			NdisMoveMemory(&BIN[8], &R[(i-1)*8], 8);
+			rtmp_aes_decrypt(&aesctx, BIN, BOUT);
+			NdisMoveMemory(A, &BOUT[0], 8);
+			NdisMoveMemory(&R[(i-1)*8], &BOUT[8], 8);
+		}
+	}
+
+	// OUTPUT
+	for(i = 0; i < c_len; i++)
+	{
+		plaintext[i] = R[i];
+	}
+
+
+	os_free_mem(NULL, R);
+}
+
+/*
+    ==========================================================================
+    Description:
+		Report the EAP message type
+
+	Arguments:
+		msg		-	EAPOL_PAIR_MSG_1
+					EAPOL_PAIR_MSG_2
+					EAPOL_PAIR_MSG_3
+					EAPOL_PAIR_MSG_4
+					EAPOL_GROUP_MSG_1
+					EAPOL_GROUP_MSG_2
+
+    Return:
+         message type string
+
+    ==========================================================================
+*/
+CHAR *GetEapolMsgType(CHAR msg)
+{
+    if(msg == EAPOL_PAIR_MSG_1)
+        return "Pairwise Message 1";
+    else if(msg == EAPOL_PAIR_MSG_2)
+        return "Pairwise Message 2";
+	else if(msg == EAPOL_PAIR_MSG_3)
+        return "Pairwise Message 3";
+	else if(msg == EAPOL_PAIR_MSG_4)
+        return "Pairwise Message 4";
+	else if(msg == EAPOL_GROUP_MSG_1)
+        return "Group Message 1";
+	else if(msg == EAPOL_GROUP_MSG_2)
+        return "Group Message 2";
+    else
+    	return "Invalid Message";
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Check Sanity RSN IE of EAPoL message
+
+    Arguments:
+
+    Return Value:
+
+
+    ========================================================================
+*/
+BOOLEAN RTMPCheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	OUT	UCHAR			*Offset)
+{
+	PUCHAR              pVIE;
+	UCHAR               len;
+	PEID_STRUCT         pEid;
+	BOOLEAN				result = FALSE;
+
+	pVIE = pData;
+	len	 = DataLen;
+	*Offset = 0;
+
+	while (len > sizeof(RSNIE2))
+	{
+		pEid = (PEID_STRUCT) pVIE;
+		// WPA RSN IE
+		if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4)))
+		{
+			if ((pEntry->AuthMode == Ndis802_11AuthModeWPA || pEntry->AuthMode == Ndis802_11AuthModeWPAPSK) &&
+				(NdisEqualMemory(pVIE, pEntry->RSN_IE, pEntry->RSNIE_Len)) &&
+				(pEntry->RSNIE_Len == (pEid->Len + 2)))
+			{
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		// WPA2 RSN IE
+		else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3)))
+		{
+			if ((pEntry->AuthMode == Ndis802_11AuthModeWPA2 || pEntry->AuthMode == Ndis802_11AuthModeWPA2PSK) &&
+				(NdisEqualMemory(pVIE, pEntry->RSN_IE, pEntry->RSNIE_Len)) &&
+				(pEntry->RSNIE_Len == (pEid->Len + 2))/* ToDo-AlbertY for mesh*/)
+			{
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		else
+		{
+			break;
+		}
+
+		pVIE += (pEid->Len + 2);
+		len  -= (pEid->Len + 2);
+	}
+
+
+	return result;
+
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Parse KEYDATA field.  KEYDATA[] May contain 2 RSN IE and optionally GTK.
+    GTK  is encaptulated in KDE format at  p.83 802.11i D10
+
+    Arguments:
+
+    Return Value:
+
+    Note:
+        802.11i D10
+
+    ========================================================================
+*/
+BOOLEAN RTMPParseEapolKeyData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKeyData,
+	IN  UCHAR           KeyDataLen,
+	IN	UCHAR			GroupKeyIndex,
+	IN	UCHAR			MsgType,
+	IN	BOOLEAN			bWPA2,
+	IN  MAC_TABLE_ENTRY *pEntry)
+{
+    PKDE_ENCAP          pKDE = NULL;
+    PUCHAR              pMyKeyData = pKeyData;
+    UCHAR               KeyDataLength = KeyDataLen;
+    UCHAR               GTKLEN = 0;
+	UCHAR				DefaultIdx = 0;
+	UCHAR				skip_offset;
+
+	// Verify The RSN IE contained in pairewise_msg_2 && pairewise_msg_3 and skip it
+	if (MsgType == EAPOL_PAIR_MSG_2 || MsgType == EAPOL_PAIR_MSG_3)
+    {
+		// Check RSN IE whether it is WPA2/WPA2PSK
+		if (!RTMPCheckRSNIE(pAd, pKeyData, KeyDataLen, pEntry, &skip_offset))
+		{
+			// send wireless event - for RSN IE different
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_RSNIE_DIFF_EVENT_FLAG, pEntry->Addr, pEntry->apidx, 0);
+
+        	DBGPRINT(RT_DEBUG_ERROR, ("RSN_IE Different in msg %d of 4-way handshake!\n", MsgType));
+			hex_dump("Receive RSN_IE ", pKeyData, KeyDataLen);
+			hex_dump("Desired RSN_IE ", pEntry->RSN_IE, pEntry->RSNIE_Len);
+
+			return FALSE;
+    	}
+    	else
+		{
+			if (bWPA2 && MsgType == EAPOL_PAIR_MSG_3)
+			{
+				// skip RSN IE
+				pMyKeyData += skip_offset;
+				KeyDataLength -= skip_offset;
+				DBGPRINT(RT_DEBUG_TRACE, ("RTMPParseEapolKeyData ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", skip_offset));
+			}
+			else
+				return TRUE;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPParseEapolKeyData ==> KeyDataLength %d without RSN_IE \n", KeyDataLength));
+
+	// Parse EKD format in pairwise_msg_3_WPA2 && group_msg_1_WPA2
+	if (bWPA2 && (MsgType == EAPOL_PAIR_MSG_3 || MsgType == EAPOL_GROUP_MSG_1))
+	{
+		if (KeyDataLength >= 8)	// KDE format exclude GTK length
+    	{
+        	pKDE = (PKDE_ENCAP) pMyKeyData;
+
+
+			DefaultIdx = pKDE->GTKEncap.Kid;
+
+			// Sanity check - KED length
+			if (KeyDataLength < (pKDE->Len + 2))
+    		{
+        		DBGPRINT(RT_DEBUG_ERROR, ("ERROR: The len from KDE is too short \n"));
+        		return FALSE;
+    		}
+
+			// Get GTK length - refer to IEEE 802.11i-2004 p.82
+			GTKLEN = pKDE->Len -6;
+			if (GTKLEN < LEN_AES_KEY)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key length is too short (%d) \n", GTKLEN));
+        		return FALSE;
+			}
+
+    	}
+		else
+    	{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR: KDE format length is too short \n"));
+	        return FALSE;
+    	}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("GTK in KDE format ,DefaultKeyID=%d, KeyLen=%d \n", DefaultIdx, GTKLEN));
+		// skip it
+		pMyKeyData += 8;
+		KeyDataLength -= 8;
+
+	}
+	else if (!bWPA2 && MsgType == EAPOL_GROUP_MSG_1)
+	{
+		DefaultIdx = GroupKeyIndex;
+		DBGPRINT(RT_DEBUG_TRACE, ("GTK DefaultKeyID=%d \n", DefaultIdx));
+	}
+
+	// Sanity check - shared key index must be 1 ~ 3
+	if (DefaultIdx < 1 || DefaultIdx > 3)
+    {
+     	DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key index(%d) is invalid in %s %s \n", DefaultIdx, ((bWPA2) ? "WPA2" : "WPA"), GetEapolMsgType(MsgType)));
+        return FALSE;
+    }
+
+
+#ifdef CONFIG_STA_SUPPORT
+	// Todo
+#endif // CONFIG_STA_SUPPORT //
+
+	return TRUE;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Construct EAPoL message for WPA handshaking
+		Its format is below,
+
+		+--------------------+
+		| Protocol Version	 |  1 octet
+		+--------------------+
+		| Protocol Type		 |	1 octet
+		+--------------------+
+		| Body Length		 |  2 octets
+		+--------------------+
+		| Descriptor Type	 |	1 octet
+		+--------------------+
+		| Key Information    |	2 octets
+		+--------------------+
+		| Key Length	     |  1 octet
+		+--------------------+
+		| Key Repaly Counter |	8 octets
+		+--------------------+
+		| Key Nonce		     |  32 octets
+		+--------------------+
+		| Key IV			 |  16 octets
+		+--------------------+
+		| Key RSC			 |  8 octets
+		+--------------------+
+		| Key ID or Reserved |	8 octets
+		+--------------------+
+		| Key MIC			 |	16 octets
+		+--------------------+
+		| Key Data Length	 |	2 octets
+		+--------------------+
+		| Key Data			 |	n octets
+		+--------------------+
+
+
+	Arguments:
+		pAd			Pointer	to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ConstructEapolMsg(
+	IN 	PRTMP_ADAPTER    	pAd,
+    IN 	UCHAR				AuthMode,
+    IN 	UCHAR				WepStatus,
+    IN 	UCHAR				GroupKeyWepStatus,
+    IN 	UCHAR				MsgType,
+    IN	UCHAR				DefaultKeyIdx,
+    IN 	UCHAR				*ReplayCounter,
+	IN 	UCHAR				*KeyNonce,
+	IN	UCHAR				*TxRSC,
+	IN	UCHAR				*PTK,
+	IN	UCHAR				*GTK,
+	IN	UCHAR				*RSNIE,
+	IN	UCHAR				RSNIE_Len,
+    OUT PEAPOL_PACKET       pMsg)
+{
+	BOOLEAN	bWPA2 = FALSE;
+
+	// Choose WPA2 or not
+	if ((AuthMode == Ndis802_11AuthModeWPA2) || (AuthMode == Ndis802_11AuthModeWPA2PSK))
+		bWPA2 = TRUE;
+
+    // Init Packet and Fill header
+    pMsg->ProVer = EAPOL_VER;
+    pMsg->ProType = EAPOLKey;
+
+	// Default 95 bytes, the EAPoL-Key descriptor exclude Key-data field
+	pMsg->Body_Len[1] = LEN_EAPOL_KEY_MSG;
+
+	// Fill in EAPoL descriptor
+	if (bWPA2)
+		pMsg->KeyDesc.Type = WPA2_KEY_DESC;
+	else
+		pMsg->KeyDesc.Type = WPA1_KEY_DESC;
+
+	// Fill in Key information, refer to IEEE Std 802.11i-2004 page 78
+	// When either the pairwise or the group cipher is AES, the DESC_TYPE_AES(2) shall be used.
+	pMsg->KeyDesc.KeyInfo.KeyDescVer =
+        	(((WepStatus == Ndis802_11Encryption3Enabled) || (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)) ? (DESC_TYPE_AES) : (DESC_TYPE_TKIP));
+
+	// Specify Key Type as Group(0) or Pairwise(1)
+	if (MsgType >= EAPOL_GROUP_MSG_1)
+		pMsg->KeyDesc.KeyInfo.KeyType = GROUPKEY;
+	else
+		pMsg->KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// Specify Key Index, only group_msg1_WPA1
+	if (!bWPA2 && (MsgType >= EAPOL_GROUP_MSG_1))
+		pMsg->KeyDesc.KeyInfo.KeyIndex = DefaultKeyIdx;
+
+	if (MsgType == EAPOL_PAIR_MSG_3)
+		pMsg->KeyDesc.KeyInfo.Install = 1;
+
+	if ((MsgType == EAPOL_PAIR_MSG_1) || (MsgType == EAPOL_PAIR_MSG_3) || (MsgType == EAPOL_GROUP_MSG_1))
+		pMsg->KeyDesc.KeyInfo.KeyAck = 1;
+
+	if (MsgType != EAPOL_PAIR_MSG_1)
+		pMsg->KeyDesc.KeyInfo.KeyMic = 1;
+
+	if ((bWPA2 && (MsgType >= EAPOL_PAIR_MSG_3)) || (!bWPA2 && (MsgType >= EAPOL_GROUP_MSG_1)))
+    {
+       	pMsg->KeyDesc.KeyInfo.Secure = 1;
+    }
+
+	if (bWPA2 && ((MsgType == EAPOL_PAIR_MSG_3) || (MsgType == EAPOL_GROUP_MSG_1)))
+    {
+        pMsg->KeyDesc.KeyInfo.EKD_DL = 1;
+    }
+
+	// key Information element has done.
+	*(USHORT *)(&pMsg->KeyDesc.KeyInfo) = cpu2le16(*(USHORT *)(&pMsg->KeyDesc.KeyInfo));
+
+	// Fill in Key Length
+#if 0
+	if (bWPA2)
+	{
+		// In WPA2 mode, the field indicates the length of pairwise key cipher,
+		// so only pairwise_msg_1 and pairwise_msg_3 need to fill.
+		if ((MsgType == EAPOL_PAIR_MSG_1) || (MsgType == EAPOL_PAIR_MSG_3))
+			pMsg->KeyDesc.KeyLength[1] = ((WepStatus == Ndis802_11Encryption2Enabled) ? LEN_TKIP_KEY : LEN_AES_KEY);
+	}
+	else if (!bWPA2)
+#endif
+	{
+		if (MsgType >= EAPOL_GROUP_MSG_1)
+		{
+			// the length of group key cipher
+			pMsg->KeyDesc.KeyLength[1] = ((GroupKeyWepStatus == Ndis802_11Encryption2Enabled) ? TKIP_GTK_LENGTH : LEN_AES_KEY);
+		}
+		else
+		{
+			// the length of pairwise key cipher
+			pMsg->KeyDesc.KeyLength[1] = ((WepStatus == Ndis802_11Encryption2Enabled) ? LEN_TKIP_KEY : LEN_AES_KEY);
+		}
+	}
+
+ 	// Fill in replay counter
+    NdisMoveMemory(pMsg->KeyDesc.ReplayCounter, ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Fill Key Nonce field
+	// ANonce : pairwise_msg1 & pairwise_msg3
+	// SNonce : pairwise_msg2
+	// GNonce : group_msg1_wpa1
+	if ((MsgType <= EAPOL_PAIR_MSG_3) || ((!bWPA2 && (MsgType == EAPOL_GROUP_MSG_1))))
+    	NdisMoveMemory(pMsg->KeyDesc.KeyNonce, KeyNonce, LEN_KEY_DESC_NONCE);
+
+	// Fill key IV - WPA2 as 0, WPA1 as random
+	if (!bWPA2 && (MsgType == EAPOL_GROUP_MSG_1))
+	{
+		// Suggest IV be random number plus some number,
+		NdisMoveMemory(pMsg->KeyDesc.KeyIv, &KeyNonce[16], LEN_KEY_DESC_IV);
+        pMsg->KeyDesc.KeyIv[15] += 2;
+	}
+
+    // Fill Key RSC field
+    // It contains the RSC for the GTK being installed.
+	if ((MsgType == EAPOL_PAIR_MSG_3 && bWPA2) || (MsgType == EAPOL_GROUP_MSG_1))
+	{
+        NdisMoveMemory(pMsg->KeyDesc.KeyRsc, TxRSC, 6);
+	}
+
+	// Clear Key MIC field for MIC calculation later
+    NdisZeroMemory(pMsg->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+
+	ConstructEapolKeyData(pAd,
+						  AuthMode,
+						  WepStatus,
+						  GroupKeyWepStatus,
+						  MsgType,
+						  DefaultKeyIdx,
+						  bWPA2,
+						  PTK,
+						  GTK,
+						  RSNIE,
+						  RSNIE_Len,
+						  pMsg);
+
+	// Calculate MIC and fill in KeyMic Field except Pairwise Msg 1.
+	if (MsgType != EAPOL_PAIR_MSG_1)
+	{
+		CalculateMIC(pAd, WepStatus, PTK, pMsg);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> ConstructEapolMsg for %s %s\n", ((bWPA2) ? "WPA2" : "WPA"), GetEapolMsgType(MsgType)));
+	DBGPRINT(RT_DEBUG_TRACE, ("	     Body length = %d \n", pMsg->Body_Len[1]));
+	DBGPRINT(RT_DEBUG_TRACE, ("	     Key length  = %d \n", pMsg->KeyDesc.KeyLength[1]));
+
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Construct the Key Data field of EAPoL message
+
+	Arguments:
+		pAd			Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ConstructEapolKeyData(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			AuthMode,
+	IN	UCHAR			WepStatus,
+	IN	UCHAR			GroupKeyWepStatus,
+	IN 	UCHAR			MsgType,
+	IN	UCHAR			DefaultKeyIdx,
+	IN	BOOLEAN			bWPA2Capable,
+	IN	UCHAR			*PTK,
+	IN	UCHAR			*GTK,
+	IN	UCHAR			*RSNIE,
+	IN	UCHAR			RSNIE_LEN,
+	OUT PEAPOL_PACKET   pMsg)
+{
+	UCHAR		*mpool, *Key_Data, *Rc4GTK;
+	UCHAR       ekey[(LEN_KEY_DESC_IV+LEN_EAP_EK)];
+	UCHAR		data_offset;
+
+
+	if (MsgType == EAPOL_PAIR_MSG_1 || MsgType == EAPOL_PAIR_MSG_4 || MsgType == EAPOL_GROUP_MSG_2)
+		return;
+
+	// allocate memory pool
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 1500);
+
+    if (mpool == NULL)
+		return;
+
+	/* Rc4GTK Len = 512 */
+	Rc4GTK = (UCHAR *) ROUND_UP(mpool, 4);
+	/* Key_Data Len = 512 */
+	Key_Data = (UCHAR *) ROUND_UP(Rc4GTK + 512, 4);
+
+	NdisZeroMemory(Key_Data, 512);
+	pMsg->KeyDesc.KeyDataLen[1] = 0;
+	data_offset = 0;
+
+	// Encapsulate RSNIE in pairwise_msg2 & pairwise_msg3
+	if (RSNIE_LEN && ((MsgType == EAPOL_PAIR_MSG_2) || (MsgType == EAPOL_PAIR_MSG_3)))
+	{
+		if (bWPA2Capable)
+			Key_Data[data_offset + 0] = IE_WPA2;
+		else
+			Key_Data[data_offset + 0] = IE_WPA;
+
+        Key_Data[data_offset + 1] = RSNIE_LEN;
+		NdisMoveMemory(&Key_Data[data_offset + 2], RSNIE, RSNIE_LEN);
+		data_offset += (2 + RSNIE_LEN);
+	}
+
+	// Encapsulate KDE format in pairwise_msg3_WPA2 & group_msg1_WPA2
+	if (bWPA2Capable && ((MsgType == EAPOL_PAIR_MSG_3) || (MsgType == EAPOL_GROUP_MSG_1)))
+	{
+		// Key Data Encapsulation (KDE) format - 802.11i-2004  Figure-43w and Table-20h
+        Key_Data[data_offset + 0] = 0xDD;
+
+		if (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)
+		{
+			Key_Data[data_offset + 1] = 0x16;// 4+2+16(OUI+DataType+DataField)
+		}
+		else
+		{
+			Key_Data[data_offset + 1] = 0x26;// 4+2+32(OUI+DataType+DataField)
+		}
+
+        Key_Data[data_offset + 2] = 0x00;
+        Key_Data[data_offset + 3] = 0x0F;
+        Key_Data[data_offset + 4] = 0xAC;
+        Key_Data[data_offset + 5] = 0x01;
+
+		// GTK KDE format - 802.11i-2004  Figure-43x
+        Key_Data[data_offset + 6] = (DefaultKeyIdx & 0x03);
+        Key_Data[data_offset + 7] = 0x00;	// Reserved Byte
+
+		data_offset += 8;
+	}
+
+
+	// Encapsulate GTK and encrypt the key-data field with KEK.
+	// Only for pairwise_msg3_WPA2 and group_msg1
+	if ((MsgType == EAPOL_PAIR_MSG_3 && bWPA2Capable) || (MsgType == EAPOL_GROUP_MSG_1))
+	{
+		// Fill in GTK
+		if (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)
+		{
+			NdisMoveMemory(&Key_Data[data_offset], GTK, LEN_AES_KEY);
+			data_offset += LEN_AES_KEY;
+		}
+		else
+		{
+			NdisMoveMemory(&Key_Data[data_offset], GTK, TKIP_GTK_LENGTH);
+			data_offset += TKIP_GTK_LENGTH;
+		}
+
+		// Still dont know why, but if not append will occur "GTK not include in MSG3"
+		// Patch for compatibility between zero config and funk
+		if (MsgType == EAPOL_PAIR_MSG_3 && bWPA2Capable)
+		{
+			if (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)
+			{
+				Key_Data[data_offset + 0] = 0xDD;
+				Key_Data[data_offset + 1] = 0;
+				data_offset += 2;
+			}
+			else
+			{
+				Key_Data[data_offset + 0] = 0xDD;
+				Key_Data[data_offset + 1] = 0;
+				Key_Data[data_offset + 2] = 0;
+				Key_Data[data_offset + 3] = 0;
+				Key_Data[data_offset + 4] = 0;
+				Key_Data[data_offset + 5] = 0;
+				data_offset += 6;
+			}
+		}
+
+		// Encrypt the data material in key data field
+		if (WepStatus == Ndis802_11Encryption3Enabled)
+		{
+			AES_GTK_KEY_WRAP(&PTK[16], Key_Data, data_offset, Rc4GTK);
+            // AES wrap function will grow 8 bytes in length
+            data_offset += 8;
+		}
+		else
+		{
+			// PREPARE Encrypted  "Key DATA" field.  (Encrypt GTK with RC4, usinf PTK[16]->[31] as Key, IV-field as IV)
+			// put TxTsc in Key RSC field
+			pAd->PrivateInfo.FCSCRC32 = PPPINITFCS32;   //Init crc32.
+
+			// ekey is the contanetion of IV-field, and PTK[16]->PTK[31]
+			NdisMoveMemory(ekey, pMsg->KeyDesc.KeyIv, LEN_KEY_DESC_IV);
+			NdisMoveMemory(&ekey[LEN_KEY_DESC_IV], &PTK[16], LEN_EAP_EK);
+			ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, ekey, sizeof(ekey));  //INIT SBOX, KEYLEN+3(IV)
+			pAd->PrivateInfo.FCSCRC32 = RTMP_CALC_FCS32(pAd->PrivateInfo.FCSCRC32, Key_Data, data_offset);
+			WPAARCFOUR_ENCRYPT(&pAd->PrivateInfo.WEPCONTEXT, Rc4GTK, Key_Data, data_offset);
+		}
+
+		NdisMoveMemory(pMsg->KeyDesc.KeyData, Rc4GTK, data_offset);
+	}
+	else
+	{
+		NdisMoveMemory(pMsg->KeyDesc.KeyData, Key_Data, data_offset);
+	}
+
+	// set key data length field and total length
+	pMsg->KeyDesc.KeyDataLen[1] = data_offset;
+    pMsg->Body_Len[1] += data_offset;
+
+	os_free_mem(pAd, mpool);
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calcaulate MIC. It is used during 4-ways handsharking.
+
+	Arguments:
+		pAd				-	pointer to our pAdapter context
+    	PeerWepStatus	-	indicate the encryption type
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	CalculateMIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			PeerWepStatus,
+	IN	UCHAR			*PTK,
+	OUT PEAPOL_PACKET   pMsg)
+{
+    UCHAR   *OutBuffer;
+	ULONG	FrameLen = 0;
+	UCHAR	mic[LEN_KEY_DESC_MIC];
+	UCHAR	digest[80];
+
+	// allocate memory for MIC calculation
+	os_alloc_mem(pAd, (PUCHAR *)&OutBuffer, 512);
+
+    if (OutBuffer == NULL)
+    {
+		DBGPRINT(RT_DEBUG_ERROR, ("!!!CalculateMIC: no memory!!!\n"));
+		return;
+    }
+
+	// make a frame for calculating MIC.
+    MakeOutgoingFrame(OutBuffer,            	&FrameLen,
+                      pMsg->Body_Len[1] + 4,  	pMsg,
+                      END_OF_ARGS);
+
+	NdisZeroMemory(mic, sizeof(mic));
+
+	// Calculate MIC
+    if (PeerWepStatus == Ndis802_11Encryption3Enabled)
+ 	{
+		HMAC_SHA1(OutBuffer,  FrameLen, PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(PTK,  LEN_EAP_MICK, OutBuffer, FrameLen, mic);
+	}
+
+	// store the calculated MIC
+	NdisMoveMemory(pMsg->KeyDesc.KeyMic, mic, LEN_KEY_DESC_MIC);
+
+	os_free_mem(pAd, OutBuffer);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Some received frames can't decrypt by Asic, so decrypt them by software.
+
+	Arguments:
+		pAd				-	pointer to our pAdapter context
+    	PeerWepStatus	-	indicate the encryption type
+
+	Return Value:
+		NDIS_STATUS_SUCCESS		-	decryption successful
+		NDIS_STATUS_FAILURE		-	decryption failure
+
+	========================================================================
+*/
+NDIS_STATUS	RTMPSoftDecryptBroadCastData(
+	IN	PRTMP_ADAPTER					pAd,
+	IN	RX_BLK							*pRxBlk,
+	IN  NDIS_802_11_ENCRYPTION_STATUS 	GroupCipher,
+	IN  PCIPHER_KEY						pShard_key)
+{
+	PRXWI_STRUC			pRxWI = pRxBlk->pRxWI;
+
+
+
+	// handle WEP decryption
+	if (GroupCipher == Ndis802_11Encryption1Enabled)
+    {
+		if (RTMPSoftDecryptWEP(pAd, pRxBlk->pData, pRxWI->MPDUtotalByteCount, pShard_key))
+		{
+
+			//Minus IV[4] & ICV[4]
+			pRxWI->MPDUtotalByteCount -= 8;
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR : Software decrypt WEP data fails.\n"));
+			// give up this frame
+			return NDIS_STATUS_FAILURE;
+		}
+	}
+	// handle TKIP decryption
+	else if (GroupCipher == Ndis802_11Encryption2Enabled)
+	{
+		if (RTMPSoftDecryptTKIP(pAd, pRxBlk->pData, pRxWI->MPDUtotalByteCount, 0, pShard_key))
+		{
+
+			//Minus 8 bytes MIC, 8 bytes IV/EIV, 4 bytes ICV
+			pRxWI->MPDUtotalByteCount -= 20;
+		}
+        else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR : RTMPSoftDecryptTKIP Failed\n"));
+			// give up this frame
+			return NDIS_STATUS_FAILURE;
+        }
+	}
+	// handle AES decryption
+	else if (GroupCipher == Ndis802_11Encryption3Enabled)
+	{
+		if (RTMPSoftDecryptAES(pAd, pRxBlk->pData, pRxWI->MPDUtotalByteCount , pShard_key))
+		{
+
+			//8 bytes MIC, 8 bytes IV/EIV (CCMP Header)
+			pRxWI->MPDUtotalByteCount -= 16;
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR : RTMPSoftDecryptAES Failed\n"));
+			// give up this frame
+			return NDIS_STATUS_FAILURE;
+		}
+	}
+	else
+	{
+		// give up this frame
+		return NDIS_STATUS_FAILURE;
+	}
+
+	return NDIS_STATUS_SUCCESS;
+
+}
+
diff --git a/drivers/staging/rt2870/common/dfs.c b/drivers/staging/rt2870/common/dfs.c
new file mode 100644
index 000000000000..b09bba5cb206
--- /dev/null
+++ b/drivers/staging/rt2870/common/dfs.c
@@ -0,0 +1,453 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    ap_dfs.c
+
+    Abstract:
+    Support DFS function.
+
+    Revision History:
+    Who       When            What
+    --------  ----------      ----------------------------------------------
+    Fonchi    03-12-2007      created
+*/
+
+#include "../rt_config.h"
+
+typedef struct _RADAR_DURATION_TABLE
+{
+	ULONG RDDurRegion;
+	ULONG RadarSignalDuration;
+	ULONG Tolerance;
+} RADAR_DURATION_TABLE, *PRADAR_DURATION_TABLE;
+
+
+static UCHAR RdIdleTimeTable[MAX_RD_REGION][4] =
+{
+	{9, 250, 250, 250},		// CE
+	{4, 250, 250, 250},		// FCC
+	{4, 250, 250, 250},		// JAP
+	{15, 250, 250, 250},	// JAP_W53
+	{4, 250, 250, 250}		// JAP_W56
+};
+
+/*
+	========================================================================
+
+	Routine Description:
+		Bbp Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+
+	========================================================================
+*/
+VOID BbpRadarDetectionStart(
+	IN PRTMP_ADAPTER pAd)
+{
+	UINT8 RadarPeriod;
+
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 114, 0x02);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 121, 0x20);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 122, 0x00);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 123, 0x08/*0x80*/);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 124, 0x28);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 125, 0xff);
+
+#if 0
+	// toggle Rx enable bit for radar detection.
+	// it's Andy's recommand.
+	{
+		UINT32 Value;
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+	Value |= (0x1 << 3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	Value &= ~(0x1 << 3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	}
+#endif
+	RadarPeriod = ((UINT)RdIdleTimeTable[pAd->CommonCfg.RadarDetect.RDDurRegion][0] + (UINT)pAd->CommonCfg.RadarDetect.DfsSessionTime) < 250 ?
+			(RdIdleTimeTable[pAd->CommonCfg.RadarDetect.RDDurRegion][0] + pAd->CommonCfg.RadarDetect.DfsSessionTime) : 250;
+
+	RTMP_IO_WRITE8(pAd, 0x7020, 0x1d);
+	RTMP_IO_WRITE8(pAd, 0x7021, 0x40);
+
+	RadarDetectionStart(pAd, 0, RadarPeriod);
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Bbp Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+
+	========================================================================
+*/
+VOID BbpRadarDetectionStop(
+	IN PRTMP_ADAPTER pAd)
+{
+	RTMP_IO_WRITE8(pAd, 0x7020, 0x1d);
+	RTMP_IO_WRITE8(pAd, 0x7021, 0x60);
+
+	RadarDetectionStop(pAd);
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+
+	========================================================================
+*/
+VOID RadarDetectionStart(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN CTSProtect,
+	IN UINT8 CTSPeriod)
+{
+	UINT8 DfsActiveTime = (pAd->CommonCfg.RadarDetect.DfsSessionTime & 0x1f);
+	UINT8 CtsProtect = (CTSProtect == 1) ? 0x02 : 0x01; // CTS protect.
+
+	if (CTSProtect != 0)
+	{
+		switch(pAd->CommonCfg.RadarDetect.RDDurRegion)
+		{
+		case FCC:
+		case JAP_W56:
+			CtsProtect = 0x03;
+			break;
+
+		case CE:
+		case JAP_W53:
+		default:
+			CtsProtect = 0x02;
+			break;
+		}
+	}
+	else
+		CtsProtect = 0x01;
+
+
+	// send start-RD with CTS protection command to MCU
+	// highbyte [7]		reserve
+	// highbyte [6:5]	0x: stop Carrier/Radar detection
+	// highbyte [10]:	Start Carrier/Radar detection without CTS protection, 11: Start Carrier/Radar detection with CTS protection
+	// highbyte [4:0]	Radar/carrier detection duration. In 1ms.
+
+	// lowbyte [7:0]	Radar/carrier detection period, in 1ms.
+	AsicSendCommandToMcu(pAd, 0x60, 0xff, CTSPeriod, DfsActiveTime | (CtsProtect << 5));
+	//AsicSendCommandToMcu(pAd, 0x63, 0xff, 10, 0);
+
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		TRUE	Found radar signal
+		FALSE	Not found radar signal
+
+	========================================================================
+*/
+VOID RadarDetectionStop(
+	IN PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE,("RadarDetectionStop.\n"));
+	AsicSendCommandToMcu(pAd, 0x60, 0xff, 0x00, 0x00);	// send start-RD with CTS protection command to MCU
+
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Radar channel check routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		TRUE	need to do radar detect
+		FALSE	need not to do radar detect
+
+	========================================================================
+*/
+BOOLEAN RadarChannelCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			Ch)
+{
+#if 1
+	INT		i;
+	BOOLEAN result = FALSE;
+
+	for (i=0; i<pAd->ChannelListNum; i++)
+	{
+		if (Ch == pAd->ChannelList[i].Channel)
+		{
+			result = pAd->ChannelList[i].DfsReq;
+			break;
+		}
+	}
+
+	return result;
+#else
+	INT		i;
+	UCHAR	Channel[15]={52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+
+	for (i=0; i<15; i++)
+	{
+		if (Ch == Channel[i])
+		{
+			break;
+		}
+	}
+
+	if (i != 15)
+		return TRUE;
+	else
+		return FALSE;
+#endif
+}
+
+ULONG JapRadarType(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG		i;
+	const UCHAR	Channel[15]={52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+
+	if (pAd->CommonCfg.RadarDetect.RDDurRegion != JAP)
+	{
+		return pAd->CommonCfg.RadarDetect.RDDurRegion;
+	}
+
+	for (i=0; i<15; i++)
+	{
+		if (pAd->CommonCfg.Channel == Channel[i])
+		{
+			break;
+		}
+	}
+
+	if (i < 4)
+		return JAP_W53;
+	else if (i < 15)
+		return JAP_W56;
+	else
+		return JAP; // W52
+
+}
+
+ULONG RTMPBbpReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd)
+{
+	UINT8 byteValue = 0;
+	ULONG result;
+
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R115, &byteValue);
+
+	result = 0;
+	switch (byteValue)
+	{
+	case 1: // radar signal detected by pulse mode.
+	case 2: // radar signal detected by width mode.
+		result = RTMPReadRadarDuration(pAd);
+		break;
+
+	case 0: // No radar signal.
+	default:
+
+		result = 0;
+		break;
+	}
+
+	return result;
+}
+
+ULONG RTMPReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd)
+{
+	ULONG result = 0;
+
+#ifdef DFS_SUPPORT
+	UINT8 duration1 = 0, duration2 = 0, duration3 = 0;
+
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R116, &duration1);
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R117, &duration2);
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R118, &duration3);
+	result = (duration1 << 16) + (duration2 << 8) + duration3;
+#endif // DFS_SUPPORT //
+
+	return result;
+
+}
+
+VOID RTMPCleanRadarDuration(
+	IN PRTMP_ADAPTER	pAd)
+{
+	return;
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Radar wave detection. The API should be invoke each second.
+
+    Arguments:
+        pAd         - Adapter pointer
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID ApRadarDetectPeriodic(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT	i;
+
+	pAd->CommonCfg.RadarDetect.InServiceMonitorCount++;
+
+	for (i=0; i<pAd->ChannelListNum; i++)
+	{
+		if (pAd->ChannelList[i].RemainingTimeForUse > 0)
+		{
+			pAd->ChannelList[i].RemainingTimeForUse --;
+			if ((pAd->Mlme.PeriodicRound%5) == 0)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("RadarDetectPeriodic - ch=%d, RemainingTimeForUse=%d\n", pAd->ChannelList[i].Channel, pAd->ChannelList[i].RemainingTimeForUse));
+			}
+		}
+	}
+
+	//radar detect
+	if ((pAd->CommonCfg.Channel > 14)
+		&& (pAd->CommonCfg.bIEEE80211H == 1)
+		&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+	{
+		RadarDetectPeriodic(pAd);
+	}
+
+	return;
+}
+
+// Periodic Radar detection, switch channel will occur in RTMPHandleTBTTInterrupt()
+// Before switch channel, driver needs doing channel switch announcement.
+VOID RadarDetectPeriodic(
+	IN PRTMP_ADAPTER	pAd)
+{
+	// need to check channel availability, after switch channel
+	if (pAd->CommonCfg.RadarDetect.RDMode != RD_SILENCE_MODE)
+			return;
+
+	// channel availability check time is 60sec, use 65 for assurance
+	if (pAd->CommonCfg.RadarDetect.RDCount++ > pAd->CommonCfg.RadarDetect.ChMovingTime)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("Not found radar signal, start send beacon and radar detection in service monitor\n\n"));
+			BbpRadarDetectionStop(pAd);
+		AsicEnableBssSync(pAd);
+		pAd->CommonCfg.RadarDetect.RDMode = RD_NORMAL_MODE;
+
+
+		return;
+	}
+
+	return;
+}
+
+
+/*
+    ==========================================================================
+    Description:
+		change channel moving time for DFS testing.
+
+	Arguments:
+	    pAdapter                    Pointer to our adapter
+	    wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 set ChMovTime=[value]
+    ==========================================================================
+*/
+INT Set_ChMovingTime_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg)
+{
+	UINT8 Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	pAd->CommonCfg.RadarDetect.ChMovingTime = Value;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s:: %d\n", __func__,
+		pAd->CommonCfg.RadarDetect.ChMovingTime));
+
+	return TRUE;
+}
+
+INT Set_LongPulseRadarTh_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg)
+{
+	UINT8 Value;
+
+	Value = simple_strtol(arg, 0, 10) > 10 ? 10 : simple_strtol(arg, 0, 10);
+
+	pAd->CommonCfg.RadarDetect.LongPulseRadarTh = Value;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s:: %d\n", __func__,
+		pAd->CommonCfg.RadarDetect.LongPulseRadarTh));
+
+	return TRUE;
+}
+
+
diff --git a/drivers/staging/rt2870/common/eeprom.c b/drivers/staging/rt2870/common/eeprom.c
new file mode 100644
index 000000000000..33f16ed9c491
--- /dev/null
+++ b/drivers/staging/rt2870/common/eeprom.c
@@ -0,0 +1,254 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	eeprom.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#include "../rt_config.h"
+
+#if 0
+#define EEPROM_SIZE								0x200
+#define NVRAM_OFFSET			0x30000
+#define RF_OFFSET				0x40000
+
+static UCHAR init_flag = 0;
+static PUCHAR nv_ee_start = 0;
+
+static UCHAR EeBuffer[EEPROM_SIZE];
+#endif
+// IRQL = PASSIVE_LEVEL
+VOID RaiseClock(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  UINT32 *x)
+{
+    *x = *x | EESK;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, *x);
+    RTMPusecDelay(1);				// Max frequency = 1MHz in Spec. definition
+}
+
+// IRQL = PASSIVE_LEVEL
+VOID LowerClock(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  UINT32 *x)
+{
+    *x = *x & ~EESK;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, *x);
+    RTMPusecDelay(1);
+}
+
+// IRQL = PASSIVE_LEVEL
+USHORT ShiftInBits(
+    IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32		x,i;
+	USHORT      data=0;
+
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+    x &= ~( EEDO | EEDI);
+
+    for(i=0; i<16; i++)
+    {
+        data = data << 1;
+        RaiseClock(pAd, &x);
+
+        RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+        x &= ~(EEDI);
+        if(x & EEDO)
+            data |= 1;
+
+        LowerClock(pAd, &x);
+    }
+
+    return data;
+}
+
+// IRQL = PASSIVE_LEVEL
+VOID ShiftOutBits(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  USHORT data,
+    IN  USHORT count)
+{
+    UINT32       x,mask;
+
+    mask = 0x01 << (count - 1);
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+    x &= ~(EEDO | EEDI);
+
+    do
+    {
+        x &= ~EEDI;
+        if(data & mask)		x |= EEDI;
+
+        RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+        RaiseClock(pAd, &x);
+        LowerClock(pAd, &x);
+
+        mask = mask >> 1;
+    } while(mask);
+
+    x &= ~EEDI;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+}
+
+// IRQL = PASSIVE_LEVEL
+VOID EEpromCleanup(
+    IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32 x;
+
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+    x &= ~(EECS | EEDI);
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+    RaiseClock(pAd, &x);
+    LowerClock(pAd, &x);
+}
+
+VOID EWEN(
+	IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32	x;
+
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode and six pulse in that order
+    ShiftOutBits(pAd, EEPROM_EWEN_OPCODE, 5);
+    ShiftOutBits(pAd, 0, 6);
+
+    EEpromCleanup(pAd);
+}
+
+VOID EWDS(
+	IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32	x;
+
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode and six pulse in that order
+    ShiftOutBits(pAd, EEPROM_EWDS_OPCODE, 5);
+    ShiftOutBits(pAd, 0, 6);
+
+    EEpromCleanup(pAd);
+}
+
+// IRQL = PASSIVE_LEVEL
+USHORT RTMP_EEPROM_READ16(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  USHORT Offset)
+{
+    UINT32		x;
+    USHORT		data;
+
+    Offset /= 2;
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode and register number in that order
+    ShiftOutBits(pAd, EEPROM_READ_OPCODE, 3);
+    ShiftOutBits(pAd, Offset, pAd->EEPROMAddressNum);
+
+    // Now read the data (16 bits) in from the selected EEPROM word
+    data = ShiftInBits(pAd);
+
+    EEpromCleanup(pAd);
+
+    return data;
+}	//ReadEEprom
+
+VOID RTMP_EEPROM_WRITE16(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  USHORT Offset,
+    IN  USHORT Data)
+{
+    UINT32 x;
+
+	Offset /= 2;
+
+	EWEN(pAd);
+
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode ,register number and data in that order
+    ShiftOutBits(pAd, EEPROM_WRITE_OPCODE, 3);
+    ShiftOutBits(pAd, Offset, pAd->EEPROMAddressNum);
+	ShiftOutBits(pAd, Data, 16);		// 16-bit access
+
+    // read DO status
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+	EEpromCleanup(pAd);
+
+	RTMPusecDelay(10000);	//delay for twp(MAX)=10ms
+
+	EWDS(pAd);
+
+    EEpromCleanup(pAd);
+}
+
diff --git a/drivers/staging/rt2870/common/firmware.h b/drivers/staging/rt2870/common/firmware.h
new file mode 100644
index 000000000000..a40669895ef5
--- /dev/null
+++ b/drivers/staging/rt2870/common/firmware.h
@@ -0,0 +1,558 @@
+/*
+ Copyright (c) 2007, Ralink Technology Corporation
+ All rights reserved.
+
+ Redistribution.  Redistribution and use in binary form, without
+ modification, are permitted provided that the following conditions are
+ met:
+
+ 	* Redistributions must reproduce the above copyright notice and the
+ 	following disclaimer in the documentation and/or other materials
+ 	provided with the distribution.
+ 	* Neither the name of Ralink Technology Corporation nor the names of its
+ 	suppliers may be used to endorse or promote products derived from this
+ 	software without specific prior written permission.
+ 	* No reverse engineering, decompilation, or disassembly of this software
+ 	is permitted.
+
+ Limited patent license. Ralink Technology Corporation grants a world-wide,
+ royalty-free, non-exclusive license under patents it now or hereafter
+ owns or controls to make, have made, use, import, offer to sell and
+ sell ("Utilize") this software, but solely to the extent that any
+ such patent is necessary to Utilize the software alone, or in
+ combination with an operating system licensed under an approved Open
+ Source license as listed by the Open Source Initiative at
+ http://opensource.org/licenses.  The patent license shall not apply to
+ any other combinations which include this software.  No hardware per
+ se is licensed hereunder.
+
+ DISCLAIMER.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
+ BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+ FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+ TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
+ USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
+ DAMAGE.
+*/
+/* AUTO GEN PLEASE DO NOT MODIFY IT */
+/* AUTO GEN PLEASE DO NOT MODIFY IT */
+
+
+UCHAR FirmwareImage [] = {
+0xff, 0xff, 0xff, 0x02, 0x10, 0x28, 0x02, 0x10, 0x32, 0x02, 0x10, 0x78, 0x02, 0x12, 0x79, 0x02,
+0x12, 0x7a, 0x02, 0x12, 0x99, 0x02, 0x12, 0x9e, 0x12, 0x12, 0x9a, 0x22, 0x02, 0x16, 0x36, 0x02,
+0x17, 0x0c, 0x02, 0x13, 0x89, 0x02, 0x12, 0x9f, 0x30, 0x05, 0x06, 0x20, 0x0d, 0x03, 0x12, 0x17,
+0xae, 0x22, 0x90, 0x01, 0x8c, 0xe0, 0x30, 0xe3, 0x1b, 0xe5, 0x4c, 0x30, 0xe0, 0x04, 0x7f, 0x40,
+0x80, 0x02, 0x7f, 0x00, 0x90, 0x10, 0x2f, 0xef, 0xf0, 0x90, 0x01, 0x8c, 0x74, 0x08, 0xf0, 0xe4,
+0x90, 0x01, 0xa7, 0xf0, 0x90, 0x01, 0x8c, 0xe0, 0x30, 0xe0, 0x1c, 0x90, 0x01, 0x80, 0xe0, 0xb4,
+0x02, 0x15, 0xa3, 0xe0, 0xb4, 0x01, 0x10, 0x90, 0x01, 0x84, 0xe0, 0xb4, 0x81, 0x09, 0x90, 0x01,
+0x8c, 0x74, 0x01, 0xf0, 0x12, 0x0d, 0xc8, 0x22, 0x90, 0x04, 0x14, 0xe0, 0x20, 0xe7, 0x03, 0x02,
+0x12, 0x6e, 0x90, 0x70, 0x12, 0xe0, 0xf5, 0x56, 0x90, 0x04, 0x04, 0xe0, 0x12, 0x0a, 0x9d, 0x10,
+0xb7, 0x31, 0x10, 0xe2, 0x50, 0x11, 0x08, 0x51, 0x11, 0x13, 0x52, 0x11, 0x13, 0x53, 0x11, 0x13,
+0x54, 0x11, 0x54, 0x55, 0x11, 0x79, 0x70, 0x11, 0xa4, 0x71, 0x11, 0xd2, 0x72, 0x12, 0x25, 0x73,
+0x12, 0x46, 0x80, 0x00, 0x00, 0x12, 0x6e, 0x20, 0x02, 0x03, 0x30, 0x03, 0x1d, 0x7d, 0x02, 0xaf,
+0x56, 0x12, 0x0b, 0x91, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5,
+0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0x6e, 0x02, 0x12, 0x67, 0x85, 0x56, 0x41, 0xd2, 0x02, 0x02,
+0x12, 0x6e, 0x90, 0x70, 0x10, 0xe0, 0x54, 0x7f, 0x64, 0x02, 0x60, 0x03, 0x02, 0x12, 0x6e, 0x90,
+0x70, 0x11, 0xe0, 0x64, 0x08, 0x60, 0x08, 0xe0, 0x64, 0x20, 0x60, 0x03, 0x02, 0x12, 0x6e, 0x75,
+0x4e, 0x03, 0x75, 0x4f, 0x20, 0x02, 0x12, 0x6e, 0x90, 0x70, 0x11, 0xe0, 0x24, 0xff, 0x92, 0x47,
+0x02, 0x12, 0x6e, 0x90, 0x04, 0x04, 0xe0, 0x25, 0xe0, 0x24, 0x5d, 0xf5, 0x57, 0x90, 0x70, 0x10,
+0xe0, 0xff, 0x74, 0x47, 0x25, 0x57, 0xf8, 0xc6, 0xef, 0xc6, 0x90, 0x70, 0x11, 0xe0, 0xff, 0x74,
+0x48, 0x25, 0x57, 0xf8, 0xc6, 0xef, 0xc6, 0xe4, 0xfd, 0xaf, 0x56, 0x12, 0x0b, 0x91, 0x90, 0x04,
+0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12,
+0x6e, 0x02, 0x12, 0x67, 0xe5, 0x47, 0x64, 0x07, 0x60, 0x0b, 0xe5, 0x47, 0x64, 0x08, 0x60, 0x05,
+0xe5, 0x47, 0xb4, 0x09, 0x08, 0x90, 0x70, 0x11, 0xe0, 0x54, 0x0f, 0xf5, 0x3a, 0xe4, 0xfd, 0xaf,
+0x56, 0x12, 0x0b, 0x91, 0xd2, 0x04, 0x02, 0x12, 0x6e, 0x90, 0x70, 0x10, 0xe0, 0xfe, 0x90, 0x70,
+0x11, 0xe0, 0xfd, 0xed, 0xf8, 0xe6, 0xf5, 0x57, 0xfd, 0xaf, 0x56, 0x12, 0x0b, 0x91, 0x90, 0x04,
+0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12,
+0x6e, 0x02, 0x12, 0x67, 0x90, 0x70, 0x10, 0xe0, 0xfe, 0x90, 0x70, 0x11, 0xe0, 0xfd, 0xed, 0xf5,
+0x82, 0x8e, 0x83, 0xe0, 0xf5, 0x57, 0xfd, 0xaf, 0x56, 0x12, 0x0b, 0x91, 0x90, 0x04, 0x14, 0x74,
+0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0x6e, 0x02,
+0x12, 0x67, 0x90, 0x10, 0x02, 0xe0, 0xb4, 0x70, 0x1e, 0xa3, 0xe0, 0xb4, 0x30, 0x19, 0x90, 0x05,
+0x08, 0xe0, 0x44, 0x01, 0xf0, 0xfd, 0x90, 0x05, 0x05, 0xe0, 0x54, 0xfb, 0xf0, 0x44, 0x04, 0xf0,
+0xed, 0x54, 0xfe, 0x90, 0x05, 0x08, 0xf0, 0xe4, 0xf5, 0x4e, 0xf5, 0x4f, 0x75, 0x3a, 0xff, 0xad,
+0x57, 0xaf, 0x56, 0x12, 0x0b, 0x91, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13,
+0xf0, 0xe5, 0x56, 0xf4, 0x60, 0x07, 0x90, 0x70, 0x25, 0xe0, 0x44, 0x01, 0xf0, 0x90, 0x70, 0x40,
+0xe5, 0x3a, 0xf0, 0x80, 0x49, 0x90, 0x70, 0x10, 0xe0, 0x24, 0xff, 0x92, 0x93, 0xe4, 0xfd, 0xaf,
+0x56, 0x12, 0x0b, 0x91, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5,
+0x56, 0xf4, 0x60, 0x2a, 0x80, 0x21, 0x90, 0x70, 0x10, 0xe0, 0x24, 0xff, 0x92, 0x4a, 0xd2, 0x05,
+0xad, 0x57, 0xaf, 0x56, 0x12, 0x0b, 0x91, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70,
+0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x60, 0x07, 0x90, 0x70, 0x25, 0xe0, 0x44, 0x01, 0xf0, 0x90, 0x70,
+0x42, 0xe5, 0x3a, 0xf0, 0xa3, 0x74, 0xab, 0xf0, 0x22, 0x22, 0xe5, 0x53, 0x70, 0x1a, 0x30, 0x60,
+0x09, 0xb2, 0x4d, 0x30, 0x4d, 0x04, 0x05, 0x46, 0xc2, 0x04, 0xe5, 0x4f, 0x45, 0x4e, 0x60, 0x08,
+0xe5, 0x4f, 0x15, 0x4f, 0x70, 0x02, 0x15, 0x4e, 0x22, 0x22, 0xc2, 0x42, 0xd3, 0x22, 0x22, 0xc2,
+0x4b, 0xc2, 0x4c, 0xe5, 0x44, 0x12, 0x0a, 0x9d, 0x12, 0xc1, 0x00, 0x13, 0x54, 0x04, 0x13, 0x50,
+0x08, 0x13, 0x2b, 0x10, 0x12, 0xd5, 0x20, 0x12, 0xf5, 0x60, 0x13, 0x06, 0xa0, 0x00, 0x00, 0x13,
+0x56, 0x85, 0x48, 0x43, 0x85, 0x4a, 0x42, 0x85, 0x4c, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x60, 0x03,
+0x02, 0x13, 0x56, 0x80, 0x1b, 0xe5, 0x48, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4a, 0xc4, 0x54,
+0x0f, 0xf5, 0x42, 0xe5, 0x4c, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x70, 0x66,
+0x53, 0x43, 0x0f, 0x80, 0x61, 0x85, 0x49, 0x43, 0x85, 0x4b, 0x42, 0x85, 0x4d, 0x5e, 0xe5, 0x47,
+0x64, 0x06, 0x70, 0x52, 0x80, 0x1b, 0xe5, 0x49, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4b, 0xc4,
+0x54, 0x0f, 0xf5, 0x42, 0xe5, 0x4d, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x70,
+0x35, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x10, 0xf5, 0x43, 0x80, 0x2b, 0xe5, 0x47, 0xb4, 0x04, 0x06,
+0x53, 0x5e, 0xfb, 0x75, 0x42, 0x09, 0xe5, 0x47, 0xb4, 0x05, 0x06, 0x43, 0x5e, 0x04, 0x75, 0x42,
+0x09, 0xe5, 0x47, 0xb4, 0x06, 0x10, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x30, 0xf5, 0x43, 0x80, 0x06,
+0xd2, 0x4b, 0x80, 0x02, 0xd2, 0x4c, 0xe4, 0xf5, 0x25, 0xe5, 0x42, 0xc4, 0x54, 0xf0, 0xff, 0xe5,
+0x43, 0x54, 0x0f, 0x4f, 0xf5, 0x5f, 0x90, 0x70, 0x44, 0xf0, 0xa3, 0xe5, 0x5e, 0xf0, 0xa3, 0xe5,
+0x4a, 0xf0, 0xa3, 0xe5, 0x48, 0xf0, 0xa3, 0xe5, 0x4c, 0xf0, 0xa3, 0xe5, 0x44, 0xf0, 0xa3, 0xe5,
+0x42, 0xf0, 0xa3, 0xe5, 0x43, 0xf0, 0xd2, 0x60, 0x22, 0xe5, 0x47, 0x60, 0x10, 0x24, 0xc0, 0x70,
+0x03, 0x12, 0x16, 0x16, 0x12, 0x13, 0x9e, 0xc2, 0xaf, 0xc2, 0x04, 0xd2, 0xaf, 0x22, 0xc2, 0xaf,
+0x90, 0x04, 0x14, 0xe0, 0x54, 0x0e, 0x60, 0x04, 0xd2, 0x18, 0x80, 0x08, 0xe5, 0x4e, 0x45, 0x4f,
+0x24, 0xff, 0x92, 0x18, 0xd2, 0xaf, 0x90, 0x04, 0x14, 0xe0, 0xa2, 0xe4, 0x92, 0x19, 0x74, 0x1e,
+0xf0, 0xe5, 0x5f, 0x54, 0x0f, 0xf5, 0x2d, 0xe5, 0x25, 0x70, 0x13, 0x30, 0x18, 0x05, 0xe5, 0x5f,
+0x20, 0xe5, 0x0b, 0x30, 0x19, 0x19, 0xe5, 0x5f, 0x54, 0x30, 0xff, 0xbf, 0x30, 0x11, 0xe5, 0x25,
+0x70, 0x05, 0x75, 0x25, 0x0c, 0x80, 0x02, 0x15, 0x25, 0xd2, 0x6c, 0xd2, 0x6d, 0x80, 0x0f, 0xe5,
+0x5f, 0x30, 0xe6, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x04, 0xd2, 0x6c, 0xc2, 0x6d, 0xe5, 0x47,
+0x64, 0x03, 0x70, 0x21, 0x30, 0x4b, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x18, 0xe5, 0x25, 0x70,
+0x03, 0x30, 0x4c, 0x11, 0xc2, 0x4c, 0xe5, 0x25, 0x70, 0x05, 0x75, 0x25, 0x07, 0x80, 0x02, 0x15,
+0x25, 0xd2, 0x6c, 0xd2, 0x6d, 0xe5, 0x47, 0xb4, 0x09, 0x14, 0xe5, 0x44, 0x20, 0xe3, 0x0b, 0xe5,
+0x3a, 0x64, 0x02, 0x60, 0x05, 0xe5, 0x3a, 0xb4, 0x03, 0x04, 0xc2, 0x6c, 0xd2, 0x6d, 0x90, 0x70,
+0x46, 0xe5, 0x2d, 0xf0, 0x20, 0x69, 0x07, 0xe5, 0x5e, 0x20, 0xe0, 0x02, 0xb2, 0x68, 0x20, 0x6b,
+0x07, 0xe5, 0x5e, 0x20, 0xe1, 0x02, 0xb2, 0x6a, 0x20, 0x6d, 0x07, 0xe5, 0x5e, 0x20, 0xe2, 0x02,
+0xb2, 0x6c, 0x90, 0x70, 0x47, 0xe5, 0x2d, 0xf0, 0x75, 0x2e, 0x40, 0x20, 0x69, 0x04, 0xa2, 0x68,
+0x80, 0x26, 0x30, 0x68, 0x06, 0xe5, 0x46, 0xa2, 0xe2, 0x80, 0x1d, 0xe5, 0x5e, 0x20, 0xe2, 0x04,
+0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0xe5, 0x46, 0x54, 0xf0, 0xfe, 0xbe, 0xf0, 0x04, 0x7e, 0x01,
+0x80, 0x02, 0x7e, 0x00, 0xee, 0x6f, 0x24, 0xff, 0x92, 0x73, 0x92, 0x72, 0x20, 0x6b, 0x04, 0xa2,
+0x6a, 0x80, 0x26, 0x30, 0x6a, 0x06, 0xe5, 0x46, 0xa2, 0xe2, 0x80, 0x1d, 0xe5, 0x5e, 0x20, 0xe0,
+0x04, 0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0xe5, 0x46, 0x54, 0xf0, 0xfe, 0xbe, 0xf0, 0x04, 0x7e,
+0x01, 0x80, 0x02, 0x7e, 0x00, 0xee, 0x6f, 0x24, 0xff, 0x92, 0x75, 0x92, 0x74, 0x20, 0x6d, 0x04,
+0xa2, 0x6c, 0x80, 0x26, 0x30, 0x6c, 0x06, 0xe5, 0x46, 0xa2, 0xe2, 0x80, 0x1d, 0xe5, 0x5e, 0x20,
+0xe1, 0x04, 0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0xe5, 0x46, 0x54, 0xf0, 0xfe, 0xbe, 0xf0, 0x04,
+0x7e, 0x01, 0x80, 0x02, 0x7e, 0x00, 0xee, 0x6f, 0x24, 0xff, 0x92, 0x71, 0x92, 0x70, 0x90, 0x10,
+0x2f, 0xe5, 0x2e, 0xf0, 0xe5, 0x47, 0x64, 0x06, 0x70, 0x4c, 0x90, 0x02, 0x29, 0xe0, 0x54, 0xfe,
+0xf0, 0xe5, 0x43, 0xc4, 0x54, 0x0f, 0x14, 0x60, 0x14, 0x24, 0xfe, 0x60, 0x23, 0x24, 0x03, 0x60,
+0x03, 0x02, 0x16, 0x05, 0x90, 0x02, 0x28, 0xe0, 0x30, 0x47, 0x0f, 0x80, 0x07, 0x90, 0x02, 0x28,
+0xe0, 0x20, 0x47, 0x06, 0x54, 0xfe, 0xf0, 0x02, 0x16, 0x05, 0x44, 0x01, 0xf0, 0x02, 0x16, 0x05,
+0xe5, 0x46, 0x30, 0xe2, 0x04, 0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0x90, 0x02, 0x28, 0xe0, 0x54,
+0xfe, 0x4f, 0xf0, 0x02, 0x16, 0x05, 0xe5, 0x47, 0x64, 0x07, 0x60, 0x0f, 0xe5, 0x47, 0x64, 0x08,
+0x60, 0x09, 0xe5, 0x47, 0x64, 0x09, 0x60, 0x03, 0x02, 0x16, 0x05, 0xe4, 0xf5, 0x27, 0x90, 0x02,
+0x29, 0xe0, 0x54, 0xfc, 0xf0, 0xe5, 0x3a, 0x14, 0x60, 0x2d, 0x14, 0x60, 0x2e, 0x14, 0x60, 0x36,
+0x24, 0xfc, 0x60, 0x5f, 0x24, 0xf9, 0x60, 0x1f, 0x24, 0x0e, 0x70, 0x69, 0xe5, 0x46, 0x13, 0x13,
+0x54, 0x3f, 0x75, 0xf0, 0x01, 0x84, 0xaf, 0xf0, 0x20, 0x47, 0x04, 0x7e, 0x01, 0x80, 0x02, 0x7e,
+0x00, 0xef, 0x6e, 0x24, 0xff, 0x80, 0x45, 0xa2, 0x47, 0x80, 0x41, 0xe5, 0x46, 0x30, 0xe2, 0x03,
+0xd3, 0x80, 0x27, 0xc3, 0x80, 0x24, 0xe5, 0x46, 0x30, 0xe2, 0x0d, 0x54, 0x38, 0xc3, 0x94, 0x30,
+0x50, 0x06, 0x7e, 0x00, 0x7f, 0x01, 0x80, 0x04, 0x7e, 0x00, 0x7f, 0x00, 0x20, 0x47, 0x04, 0x7d,
+0x01, 0x80, 0x02, 0x7d, 0x00, 0xef, 0x6d, 0x4e, 0x24, 0xff, 0x92, 0x38, 0xa2, 0x47, 0xb3, 0x92,
+0x39, 0x80, 0x19, 0xe5, 0x46, 0x30, 0xe2, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0x39, 0xa2, 0x47,
+0xb3, 0x92, 0x38, 0x80, 0x07, 0xa2, 0x47, 0xb3, 0x92, 0x38, 0x92, 0x39, 0x90, 0x02, 0x28, 0xe0,
+0x54, 0xfc, 0x45, 0x27, 0xf0, 0x90, 0x70, 0x9c, 0xe5, 0x3a, 0xf0, 0xa3, 0xe5, 0x47, 0xf0, 0x90,
+0x70, 0x41, 0xe5, 0x3a, 0xf0, 0x22, 0xe4, 0x90, 0x02, 0x29, 0xf0, 0x30, 0x47, 0x04, 0xaf, 0x45,
+0x80, 0x04, 0xe5, 0x45, 0xf4, 0xff, 0x90, 0x02, 0x28, 0xef, 0xf0, 0x22, 0x8f, 0x50, 0xd2, 0x59,
+0x22, 0x8f, 0x54, 0xd2, 0x58, 0x22, 0xe4, 0xf5, 0x62, 0xc2, 0xaf, 0xe5, 0x51, 0x14, 0x60, 0x46,
+0x14, 0x60, 0x62, 0x24, 0x02, 0x60, 0x03, 0x02, 0x16, 0xf0, 0xd2, 0x59, 0x75, 0x55, 0x01, 0x90,
+0x02, 0xa2, 0xe0, 0x54, 0x7f, 0xf0, 0xa3, 0xe0, 0x20, 0xe7, 0x22, 0x90, 0x04, 0x34, 0xe0, 0xb4,
+0x02, 0x1b, 0xa3, 0xe0, 0xb4, 0x02, 0x16, 0xa3, 0xe0, 0xb4, 0x02, 0x11, 0x7f, 0x20, 0x12, 0x16,
+0x2c, 0x90, 0x10, 0x04, 0xe0, 0x54, 0xf3, 0xf0, 0x75, 0x51, 0x01, 0x80, 0x73, 0xe5, 0x50, 0x70,
+0x05, 0x75, 0x62, 0x03, 0x80, 0x6a, 0x90, 0x12, 0x00, 0xe0, 0x54, 0x03, 0x70, 0x11, 0x7f, 0x20,
+0x12, 0x16, 0x2c, 0x90, 0x02, 0xa2, 0xe0, 0x54, 0xbf, 0xf0, 0x75, 0x51, 0x02, 0x80, 0x51, 0xe5,
+0x50, 0x70, 0x02, 0x80, 0x46, 0x90, 0x02, 0xa3, 0xe0, 0x20, 0xe6, 0x3b, 0x90, 0x04, 0x37, 0xe0,
+0x64, 0x22, 0x70, 0x33, 0x90, 0x01, 0x8a, 0x74, 0x7e, 0xf0, 0x90, 0x01, 0x96, 0xf0, 0x90, 0x12,
+0x04, 0x74, 0x0a, 0xf0, 0x90, 0x13, 0x28, 0xe0, 0x54, 0xf0, 0xf0, 0xa3, 0xe0, 0x54, 0xf0, 0xf0,
+0xa3, 0xe0, 0x54, 0xfa, 0xf0, 0x90, 0x04, 0x01, 0xe0, 0x54, 0xf9, 0xf0, 0x75, 0x62, 0x01, 0x75,
+0x55, 0x02, 0xe4, 0xf5, 0x51, 0x80, 0x09, 0xe5, 0x50, 0x70, 0x05, 0x75, 0x62, 0x03, 0xf5, 0x51,
+0xe5, 0x62, 0x60, 0x15, 0xc2, 0x01, 0xe4, 0xf5, 0x51, 0xc2, 0x59, 0xad, 0x62, 0xaf, 0x40, 0x12,
+0x17, 0x7a, 0xe5, 0x62, 0xb4, 0x03, 0x02, 0xd2, 0x03, 0xd2, 0xaf, 0x22, 0xc2, 0xaf, 0x30, 0x01,
+0x12, 0xe4, 0x90, 0x01, 0x96, 0xf0, 0xf5, 0x51, 0xc2, 0x59, 0xc2, 0x01, 0x7d, 0x02, 0xaf, 0x40,
+0x12, 0x17, 0x7a, 0xe5, 0x52, 0x14, 0x60, 0x09, 0x04, 0x70, 0x4c, 0x75, 0x52, 0x01, 0x75, 0x55,
+0x03, 0x90, 0x04, 0x01, 0xe0, 0x44, 0x0e, 0xf0, 0x90, 0x13, 0x28, 0xe0, 0x44, 0x0f, 0xf0, 0xa3,
+0xe0, 0x44, 0x0f, 0xf0, 0xa3, 0xe0, 0x44, 0x05, 0xf0, 0x90, 0x12, 0x04, 0x74, 0x03, 0xf0, 0x90,
+0x02, 0xa2, 0xe0, 0x44, 0xc0, 0xf0, 0x90, 0x10, 0x04, 0xe0, 0x44, 0x0c, 0xf0, 0xe4, 0xf5, 0x52,
+0xf5, 0x55, 0x30, 0x02, 0x0b, 0xc2, 0x02, 0x7d, 0x01, 0xaf, 0x41, 0x12, 0x17, 0x7a, 0x80, 0x02,
+0xc2, 0x03, 0xe4, 0x90, 0x01, 0x96, 0xf0, 0xd2, 0xaf, 0x22, 0xef, 0xf4, 0x60, 0x2d, 0xe4, 0xfe,
+0x74, 0x14, 0x2e, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xe0, 0xb4, 0xff, 0x19, 0x74, 0x14,
+0x2e, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xef, 0xf0, 0x74, 0x1c, 0x2e, 0xf5, 0x82, 0xe4,
+0x34, 0x70, 0xf5, 0x83, 0xed, 0xf0, 0x22, 0x0e, 0xbe, 0x04, 0xd5, 0x22, 0x22, 0x22, 0x90, 0x70,
+0x2a, 0xe0, 0x30, 0xe1, 0x4d, 0xc2, 0xaf, 0x90, 0x70, 0x28, 0xe0, 0x90, 0x10, 0x1c, 0xf0, 0x90,
+0x70, 0x29, 0xe0, 0x90, 0x10, 0x1d, 0xf0, 0x90, 0x70, 0x2a, 0xe0, 0x90, 0x10, 0x1e, 0xf0, 0x90,
+0x10, 0x1c, 0xe0, 0xf5, 0x62, 0x90, 0x10, 0x1e, 0xe0, 0x20, 0xe1, 0xf3, 0x90, 0x10, 0x1c, 0xe0,
+0x90, 0x70, 0x28, 0xf0, 0x90, 0x10, 0x1d, 0xe0, 0x90, 0x70, 0x29, 0xf0, 0x90, 0x10, 0x1e, 0xe0,
+0x90, 0x70, 0x2a, 0xf0, 0x30, 0x4a, 0x07, 0x90, 0x70, 0x24, 0xe0, 0x44, 0x01, 0xf0, 0xc2, 0x05,
+0xd2, 0xaf, 0x22, 0x22, 0x22, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a, 0x69, 0x77,
+0xff, 0xff, 0xff, 0x02, 0x10, 0x28, 0x02, 0x10, 0x32, 0x02, 0x10, 0x78, 0x02, 0x12, 0x25, 0x02,
+0x12, 0x26, 0x02, 0x12, 0x39, 0x02, 0x12, 0x3e, 0x12, 0x12, 0x3a, 0x22, 0x02, 0x15, 0x72, 0x02,
+0x16, 0x48, 0x02, 0x13, 0x29, 0x02, 0x12, 0x3f, 0x30, 0x05, 0x06, 0x20, 0x0d, 0x03, 0x12, 0x16,
+0xea, 0x22, 0x90, 0x01, 0x8c, 0xe0, 0x30, 0xe3, 0x1b, 0xe5, 0x4c, 0x30, 0xe0, 0x04, 0x7f, 0x40,
+0x80, 0x02, 0x7f, 0x00, 0x90, 0x10, 0x2f, 0xef, 0xf0, 0x90, 0x01, 0x8c, 0x74, 0x08, 0xf0, 0xe4,
+0x90, 0x01, 0xa7, 0xf0, 0x90, 0x01, 0x8c, 0xe0, 0x30, 0xe0, 0x1c, 0x90, 0x01, 0x80, 0xe0, 0xb4,
+0x02, 0x15, 0xa3, 0xe0, 0xb4, 0x01, 0x10, 0x90, 0x01, 0x84, 0xe0, 0xb4, 0x81, 0x09, 0x90, 0x01,
+0x8c, 0x74, 0x01, 0xf0, 0x12, 0x0d, 0xdd, 0x22, 0x90, 0x04, 0x14, 0xe0, 0x20, 0xe7, 0x03, 0x02,
+0x12, 0x1a, 0x90, 0x70, 0x12, 0xe0, 0xf5, 0x56, 0x90, 0x04, 0x04, 0xe0, 0x12, 0x0a, 0xb6, 0x10,
+0xb4, 0x31, 0x10, 0xdf, 0x50, 0x11, 0x05, 0x51, 0x11, 0x10, 0x52, 0x11, 0x10, 0x53, 0x11, 0x10,
+0x54, 0x11, 0x51, 0x55, 0x11, 0x70, 0x70, 0x11, 0x9a, 0x71, 0x11, 0xc4, 0x72, 0x11, 0xf2, 0x80,
+0x00, 0x00, 0x12, 0x1a, 0x20, 0x02, 0x03, 0x30, 0x03, 0x1d, 0x7d, 0x02, 0xaf, 0x56, 0x12, 0x0b,
+0xaa, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70,
+0x03, 0x02, 0x12, 0x1a, 0x02, 0x12, 0x13, 0x85, 0x56, 0x41, 0xd2, 0x02, 0x02, 0x12, 0x1a, 0x90,
+0x70, 0x10, 0xe0, 0x54, 0x7f, 0x64, 0x02, 0x60, 0x03, 0x02, 0x12, 0x1a, 0x90, 0x70, 0x11, 0xe0,
+0x64, 0x08, 0x60, 0x08, 0xe0, 0x64, 0x20, 0x60, 0x03, 0x02, 0x12, 0x1a, 0x75, 0x4e, 0x03, 0x75,
+0x4f, 0x20, 0x02, 0x12, 0x1a, 0x90, 0x70, 0x11, 0xe0, 0x24, 0xff, 0x92, 0x47, 0x02, 0x12, 0x1a,
+0x90, 0x04, 0x04, 0xe0, 0x25, 0xe0, 0x24, 0x5d, 0xf5, 0x57, 0x90, 0x70, 0x10, 0xe0, 0xff, 0x74,
+0x47, 0x25, 0x57, 0xf8, 0xc6, 0xef, 0xc6, 0x90, 0x70, 0x11, 0xe0, 0xff, 0x74, 0x48, 0x25, 0x57,
+0xf8, 0xc6, 0xef, 0xc6, 0xe4, 0xfd, 0xaf, 0x56, 0x12, 0x0b, 0xaa, 0x90, 0x04, 0x14, 0x74, 0x80,
+0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0x1a, 0x02, 0x12,
+0x13, 0xe5, 0x47, 0x64, 0x07, 0x60, 0x05, 0xe5, 0x47, 0xb4, 0x08, 0x08, 0x90, 0x70, 0x11, 0xe0,
+0x54, 0x07, 0xf5, 0x3a, 0xe4, 0xfd, 0xaf, 0x56, 0x12, 0x0b, 0xaa, 0xd2, 0x04, 0x02, 0x12, 0x1a,
+0x90, 0x70, 0x10, 0xe0, 0xfe, 0x90, 0x70, 0x11, 0xe0, 0xfd, 0xed, 0xf8, 0xe6, 0xf5, 0x57, 0xfd,
+0xaf, 0x56, 0x12, 0x0b, 0xaa, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0,
+0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0x1a, 0x80, 0x79, 0x90, 0x70, 0x10, 0xe0, 0xfe, 0x90,
+0x70, 0x11, 0xe0, 0xfd, 0xed, 0xf5, 0x82, 0x8e, 0x83, 0xe0, 0xf5, 0x57, 0xfd, 0xaf, 0x56, 0x12,
+0x0b, 0xaa, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4,
+0x60, 0x58, 0x80, 0x4f, 0xe4, 0xf5, 0x4e, 0xf5, 0x4f, 0x75, 0x3a, 0xff, 0xad, 0x57, 0xaf, 0x56,
+0x12, 0x0b, 0xaa, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56,
+0xf4, 0x60, 0x07, 0x90, 0x70, 0x25, 0xe0, 0x44, 0x01, 0xf0, 0x90, 0x70, 0x40, 0xe5, 0x3a, 0xf0,
+0x80, 0x28, 0x90, 0x70, 0x10, 0xe0, 0x24, 0xff, 0x92, 0x4a, 0xd2, 0x05, 0xad, 0x57, 0xaf, 0x56,
+0x12, 0x0b, 0xaa, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56,
+0xf4, 0x60, 0x07, 0x90, 0x70, 0x25, 0xe0, 0x44, 0x01, 0xf0, 0x90, 0x70, 0x42, 0xe5, 0x3a, 0xf0,
+0xa3, 0x74, 0xab, 0xf0, 0x22, 0x22, 0xe5, 0x53, 0x70, 0x0e, 0xe5, 0x4f, 0x45, 0x4e, 0x60, 0x08,
+0xe5, 0x4f, 0x15, 0x4f, 0x70, 0x02, 0x15, 0x4e, 0x22, 0x22, 0xc2, 0x42, 0xd3, 0x22, 0x22, 0xc2,
+0x4b, 0xc2, 0x4c, 0xe5, 0x44, 0x12, 0x0a, 0xb6, 0x12, 0x61, 0x00, 0x12, 0xf4, 0x04, 0x12, 0xf0,
+0x08, 0x12, 0xcb, 0x10, 0x12, 0x75, 0x20, 0x12, 0x95, 0x60, 0x12, 0xa6, 0xa0, 0x00, 0x00, 0x12,
+0xf6, 0x85, 0x48, 0x43, 0x85, 0x4a, 0x42, 0x85, 0x4c, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x60, 0x03,
+0x02, 0x12, 0xf6, 0x80, 0x1b, 0xe5, 0x48, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4a, 0xc4, 0x54,
+0x0f, 0xf5, 0x42, 0xe5, 0x4c, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x70, 0x66,
+0x53, 0x43, 0x0f, 0x80, 0x61, 0x85, 0x49, 0x43, 0x85, 0x4b, 0x42, 0x85, 0x4d, 0x5e, 0xe5, 0x47,
+0x64, 0x06, 0x70, 0x52, 0x80, 0x1b, 0xe5, 0x49, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4b, 0xc4,
+0x54, 0x0f, 0xf5, 0x42, 0xe5, 0x4d, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x70,
+0x35, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x10, 0xf5, 0x43, 0x80, 0x2b, 0xe5, 0x47, 0xb4, 0x04, 0x06,
+0x53, 0x5e, 0xfb, 0x75, 0x42, 0x09, 0xe5, 0x47, 0xb4, 0x05, 0x06, 0x43, 0x5e, 0x04, 0x75, 0x42,
+0x09, 0xe5, 0x47, 0xb4, 0x06, 0x10, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x30, 0xf5, 0x43, 0x80, 0x06,
+0xd2, 0x4b, 0x80, 0x02, 0xd2, 0x4c, 0xe4, 0xf5, 0x25, 0xe5, 0x42, 0xc4, 0x54, 0xf0, 0xff, 0xe5,
+0x43, 0x54, 0x0f, 0x4f, 0xf5, 0x5f, 0x90, 0x70, 0x44, 0xf0, 0xa3, 0xe5, 0x5e, 0xf0, 0xa3, 0xe5,
+0x4a, 0xf0, 0xa3, 0xe5, 0x48, 0xf0, 0xa3, 0xe5, 0x4c, 0xf0, 0xa3, 0xe5, 0x44, 0xf0, 0xa3, 0xe5,
+0x42, 0xf0, 0xa3, 0xe5, 0x43, 0xf0, 0xd2, 0x60, 0x22, 0xe5, 0x47, 0x60, 0x10, 0x24, 0xc0, 0x70,
+0x03, 0x12, 0x15, 0x52, 0x12, 0x13, 0x3e, 0xc2, 0xaf, 0xc2, 0x04, 0xd2, 0xaf, 0x22, 0xc2, 0xaf,
+0x90, 0x04, 0x14, 0xe0, 0x54, 0x0e, 0x60, 0x04, 0xd2, 0x18, 0x80, 0x08, 0xe5, 0x4e, 0x45, 0x4f,
+0x24, 0xff, 0x92, 0x18, 0xd2, 0xaf, 0x90, 0x04, 0x14, 0xe0, 0xa2, 0xe4, 0x92, 0x19, 0x74, 0x1e,
+0xf0, 0xe5, 0x5f, 0x54, 0x0f, 0xf5, 0x2d, 0xe5, 0x25, 0x70, 0x13, 0x30, 0x18, 0x05, 0xe5, 0x5f,
+0x20, 0xe5, 0x0b, 0x30, 0x19, 0x19, 0xe5, 0x5f, 0x54, 0x30, 0xff, 0xbf, 0x30, 0x11, 0xe5, 0x25,
+0x70, 0x05, 0x75, 0x25, 0x0c, 0x80, 0x02, 0x15, 0x25, 0xd2, 0x6c, 0xd2, 0x6d, 0x80, 0x0f, 0xe5,
+0x5f, 0x30, 0xe6, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x04, 0xd2, 0x6c, 0xc2, 0x6d, 0xe5, 0x47,
+0x64, 0x03, 0x70, 0x21, 0x30, 0x4b, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x18, 0xe5, 0x25, 0x70,
+0x03, 0x30, 0x4c, 0x11, 0xc2, 0x4c, 0xe5, 0x25, 0x70, 0x05, 0x75, 0x25, 0x07, 0x80, 0x02, 0x15,
+0x25, 0xd2, 0x6c, 0xd2, 0x6d, 0x90, 0x70, 0x46, 0xe5, 0x2d, 0xf0, 0x20, 0x69, 0x07, 0xe5, 0x5e,
+0x20, 0xe0, 0x02, 0xb2, 0x68, 0x20, 0x6b, 0x07, 0xe5, 0x5e, 0x20, 0xe1, 0x02, 0xb2, 0x6a, 0x20,
+0x6d, 0x07, 0xe5, 0x5e, 0x20, 0xe2, 0x02, 0xb2, 0x6c, 0x90, 0x70, 0x47, 0xe5, 0x2d, 0xf0, 0x75,
+0x2e, 0x40, 0x20, 0x69, 0x04, 0xa2, 0x68, 0x80, 0x15, 0x30, 0x68, 0x06, 0xe5, 0x46, 0xa2, 0xe3,
+0x80, 0x0c, 0xe5, 0x46, 0x54, 0xf0, 0xff, 0xbf, 0xf0, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0x73,
+0x92, 0x72, 0x20, 0x6b, 0x04, 0xa2, 0x6a, 0x80, 0x15, 0x30, 0x6a, 0x06, 0xe5, 0x46, 0xa2, 0xe3,
+0x80, 0x0c, 0xe5, 0x46, 0x54, 0xf0, 0xff, 0xbf, 0xf0, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0x75,
+0x92, 0x74, 0x20, 0x6d, 0x04, 0xa2, 0x6c, 0x80, 0x15, 0x30, 0x6c, 0x06, 0xe5, 0x46, 0xa2, 0xe3,
+0x80, 0x0c, 0xe5, 0x46, 0x54, 0xf0, 0xff, 0xbf, 0xf0, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0x71,
+0x92, 0x70, 0x90, 0x10, 0x2f, 0xe5, 0x2e, 0xf0, 0xe5, 0x47, 0x64, 0x06, 0x70, 0x4c, 0x90, 0x02,
+0x29, 0xe0, 0x54, 0xfe, 0xf0, 0xe5, 0x43, 0xc4, 0x54, 0x0f, 0x14, 0x60, 0x14, 0x24, 0xfe, 0x60,
+0x23, 0x24, 0x03, 0x60, 0x03, 0x02, 0x15, 0x41, 0x90, 0x02, 0x28, 0xe0, 0x30, 0x47, 0x0f, 0x80,
+0x07, 0x90, 0x02, 0x28, 0xe0, 0x20, 0x47, 0x06, 0x54, 0xfe, 0xf0, 0x02, 0x15, 0x41, 0x44, 0x01,
+0xf0, 0x02, 0x15, 0x41, 0xe5, 0x46, 0x30, 0xe3, 0x04, 0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0x90,
+0x02, 0x28, 0xe0, 0x54, 0xfe, 0x4f, 0xf0, 0x02, 0x15, 0x41, 0xe5, 0x47, 0x64, 0x07, 0x60, 0x09,
+0xe5, 0x47, 0x64, 0x08, 0x60, 0x03, 0x02, 0x15, 0x41, 0xe4, 0xf5, 0x27, 0x90, 0x02, 0x29, 0xe0,
+0x54, 0xfc, 0xf0, 0xe5, 0x3a, 0x14, 0x60, 0x26, 0x14, 0x60, 0x2e, 0x14, 0x60, 0x36, 0x24, 0x03,
+0x70, 0x5f, 0xe5, 0x46, 0x13, 0x13, 0x13, 0x54, 0x1f, 0x75, 0xf0, 0x03, 0x84, 0xaf, 0xf0, 0x20,
+0x47, 0x04, 0x7e, 0x01, 0x80, 0x02, 0x7e, 0x00, 0xef, 0x6e, 0x24, 0xff, 0x80, 0x02, 0xa2, 0x47,
+0x92, 0x39, 0xa2, 0x47, 0xb3, 0x92, 0x38, 0x80, 0x3f, 0xe5, 0x46, 0x30, 0xe3, 0x03, 0xd3, 0x80,
+0x27, 0xc3, 0x80, 0x24, 0xe5, 0x46, 0x30, 0xe3, 0x0d, 0x54, 0x70, 0xc3, 0x94, 0x60, 0x50, 0x06,
+0x7e, 0x00, 0x7f, 0x01, 0x80, 0x04, 0x7e, 0x00, 0x7f, 0x00, 0x20, 0x47, 0x04, 0x7d, 0x01, 0x80,
+0x02, 0x7d, 0x00, 0xef, 0x6d, 0x4e, 0x24, 0xff, 0x92, 0x38, 0xa2, 0x47, 0xb3, 0x92, 0x39, 0x80,
+0x07, 0xa2, 0x47, 0xb3, 0x92, 0x38, 0x92, 0x39, 0x90, 0x02, 0x28, 0xe0, 0x54, 0xfc, 0x45, 0x27,
+0xf0, 0x90, 0x70, 0x9c, 0xe5, 0x3a, 0xf0, 0xa3, 0xe5, 0x47, 0xf0, 0x90, 0x70, 0x41, 0xe5, 0x3a,
+0xf0, 0x22, 0xe4, 0x90, 0x02, 0x29, 0xf0, 0x30, 0x47, 0x04, 0xaf, 0x45, 0x80, 0x04, 0xe5, 0x45,
+0xf4, 0xff, 0x90, 0x02, 0x28, 0xef, 0xf0, 0x22, 0x8f, 0x50, 0xd2, 0x59, 0x22, 0x8f, 0x54, 0xd2,
+0x58, 0x22, 0xe4, 0xf5, 0x62, 0xc2, 0xaf, 0xe5, 0x51, 0x14, 0x60, 0x46, 0x14, 0x60, 0x62, 0x24,
+0x02, 0x60, 0x03, 0x02, 0x16, 0x2c, 0xd2, 0x59, 0x75, 0x55, 0x01, 0x90, 0x02, 0xa2, 0xe0, 0x54,
+0x7f, 0xf0, 0xa3, 0xe0, 0x20, 0xe7, 0x22, 0x90, 0x04, 0x34, 0xe0, 0xb4, 0x02, 0x1b, 0xa3, 0xe0,
+0xb4, 0x02, 0x16, 0xa3, 0xe0, 0xb4, 0x02, 0x11, 0x7f, 0x20, 0x12, 0x15, 0x68, 0x90, 0x10, 0x04,
+0xe0, 0x54, 0xf3, 0xf0, 0x75, 0x51, 0x01, 0x80, 0x73, 0xe5, 0x50, 0x70, 0x05, 0x75, 0x62, 0x03,
+0x80, 0x6a, 0x90, 0x12, 0x00, 0xe0, 0x54, 0x03, 0x70, 0x11, 0x7f, 0x20, 0x12, 0x15, 0x68, 0x90,
+0x02, 0xa2, 0xe0, 0x54, 0xbf, 0xf0, 0x75, 0x51, 0x02, 0x80, 0x51, 0xe5, 0x50, 0x70, 0x02, 0x80,
+0x46, 0x90, 0x02, 0xa3, 0xe0, 0x20, 0xe6, 0x3b, 0x90, 0x04, 0x37, 0xe0, 0x64, 0x22, 0x70, 0x33,
+0x90, 0x01, 0x8a, 0x74, 0x7e, 0xf0, 0x90, 0x01, 0x96, 0xf0, 0x90, 0x12, 0x04, 0x74, 0x0a, 0xf0,
+0x90, 0x13, 0x28, 0xe0, 0x54, 0xf0, 0xf0, 0xa3, 0xe0, 0x54, 0xf0, 0xf0, 0xa3, 0xe0, 0x54, 0xfa,
+0xf0, 0x90, 0x04, 0x01, 0xe0, 0x54, 0xf9, 0xf0, 0x75, 0x62, 0x01, 0x75, 0x55, 0x02, 0xe4, 0xf5,
+0x51, 0x80, 0x09, 0xe5, 0x50, 0x70, 0x05, 0x75, 0x62, 0x03, 0xf5, 0x51, 0xe5, 0x62, 0x60, 0x15,
+0xc2, 0x01, 0xe4, 0xf5, 0x51, 0xc2, 0x59, 0xad, 0x62, 0xaf, 0x40, 0x12, 0x16, 0xb6, 0xe5, 0x62,
+0xb4, 0x03, 0x02, 0xd2, 0x03, 0xd2, 0xaf, 0x22, 0xc2, 0xaf, 0x30, 0x01, 0x12, 0xe4, 0x90, 0x01,
+0x96, 0xf0, 0xf5, 0x51, 0xc2, 0x59, 0xc2, 0x01, 0x7d, 0x02, 0xaf, 0x40, 0x12, 0x16, 0xb6, 0xe5,
+0x52, 0x14, 0x60, 0x09, 0x04, 0x70, 0x4c, 0x75, 0x52, 0x01, 0x75, 0x55, 0x03, 0x90, 0x04, 0x01,
+0xe0, 0x44, 0x0e, 0xf0, 0x90, 0x13, 0x28, 0xe0, 0x44, 0x0f, 0xf0, 0xa3, 0xe0, 0x44, 0x0f, 0xf0,
+0xa3, 0xe0, 0x44, 0x05, 0xf0, 0x90, 0x12, 0x04, 0x74, 0x03, 0xf0, 0x90, 0x02, 0xa2, 0xe0, 0x44,
+0xc0, 0xf0, 0x90, 0x10, 0x04, 0xe0, 0x44, 0x0c, 0xf0, 0xe4, 0xf5, 0x52, 0xf5, 0x55, 0x30, 0x02,
+0x0b, 0xc2, 0x02, 0x7d, 0x01, 0xaf, 0x41, 0x12, 0x16, 0xb6, 0x80, 0x02, 0xc2, 0x03, 0xe4, 0x90,
+0x01, 0x96, 0xf0, 0xd2, 0xaf, 0x22, 0xef, 0xf4, 0x60, 0x2d, 0xe4, 0xfe, 0x74, 0x14, 0x2e, 0xf5,
+0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xe0, 0xb4, 0xff, 0x19, 0x74, 0x14, 0x2e, 0xf5, 0x82, 0xe4,
+0x34, 0x70, 0xf5, 0x83, 0xef, 0xf0, 0x74, 0x1c, 0x2e, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83,
+0xed, 0xf0, 0x22, 0x0e, 0xbe, 0x04, 0xd5, 0x22, 0x22, 0x22, 0x90, 0x70, 0x2a, 0xe0, 0x30, 0xe1,
+0x4d, 0xc2, 0xaf, 0x90, 0x70, 0x28, 0xe0, 0x90, 0x10, 0x1c, 0xf0, 0x90, 0x70, 0x29, 0xe0, 0x90,
+0x10, 0x1d, 0xf0, 0x90, 0x70, 0x2a, 0xe0, 0x90, 0x10, 0x1e, 0xf0, 0x90, 0x10, 0x1c, 0xe0, 0xf5,
+0x62, 0x90, 0x10, 0x1e, 0xe0, 0x20, 0xe1, 0xf3, 0x90, 0x10, 0x1c, 0xe0, 0x90, 0x70, 0x28, 0xf0,
+0x90, 0x10, 0x1d, 0xe0, 0x90, 0x70, 0x29, 0xf0, 0x90, 0x10, 0x1e, 0xe0, 0x90, 0x70, 0x2a, 0xf0,
+0x30, 0x4a, 0x07, 0x90, 0x70, 0x24, 0xe0, 0x44, 0x01, 0xf0, 0xc2, 0x05, 0xd2, 0xaf, 0x22, 0x22,
+0x22, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xe9, 0x00, } ;
diff --git a/drivers/staging/rt2870/common/md5.c b/drivers/staging/rt2870/common/md5.c
new file mode 100644
index 000000000000..774776b4b8c3
--- /dev/null
+++ b/drivers/staging/rt2870/common/md5.c
@@ -0,0 +1,1427 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    md5.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	jan			10-28-03		Initial
+	Rita    	11-23-04		Modify MD5 and SHA-1
+	Rita		10-14-05		Modify SHA-1 in big-endian platform
+ */
+#include "../rt_config.h"
+
+/**
+ * md5_mac:
+ * @key: pointer to	the	key	used for MAC generation
+ * @key_len: length	of the key in bytes
+ * @data: pointer to the data area for which the MAC is	generated
+ * @data_len: length of	the	data in	bytes
+ * @mac: pointer to	the	buffer holding space for the MAC; the buffer should
+ * have	space for 128-bit (16 bytes) MD5 hash value
+ *
+ * md5_mac() determines	the	message	authentication code	by using secure	hash
+ * MD5(key | data |	key).
+ */
+void md5_mac(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac)
+{
+	MD5_CTX	context;
+
+	MD5Init(&context);
+	MD5Update(&context,	key, key_len);
+	MD5Update(&context,	data, data_len);
+	MD5Update(&context,	key, key_len);
+	MD5Final(mac, &context);
+}
+
+/**
+ * hmac_md5:
+ * @key: pointer to	the	key	used for MAC generation
+ * @key_len: length	of the key in bytes
+ * @data: pointer to the data area for which the MAC is	generated
+ * @data_len: length of	the	data in	bytes
+ * @mac: pointer to	the	buffer holding space for the MAC; the buffer should
+ * have	space for 128-bit (16 bytes) MD5 hash value
+ *
+ * hmac_md5() determines the message authentication	code using HMAC-MD5.
+ * This	implementation is based	on the sample code presented in	RFC	2104.
+ */
+void hmac_md5(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac)
+{
+	MD5_CTX	context;
+    u8 k_ipad[65]; /* inner padding - key XORd with ipad */
+    u8 k_opad[65]; /* outer padding - key XORd with opad */
+    u8 tk[16];
+	int	i;
+
+	//assert(key != NULL && data != NULL && mac != NULL);
+
+	/* if key is longer	than 64	bytes reset	it to key =	MD5(key) */
+	if (key_len	> 64) {
+		MD5_CTX	ttcontext;
+
+		MD5Init(&ttcontext);
+		MD5Update(&ttcontext, key, key_len);
+		MD5Final(tk, &ttcontext);
+		//key=(PUCHAR)ttcontext.buf;
+		key	= tk;
+		key_len	= 16;
+	}
+
+	/* the HMAC_MD5	transform looks	like:
+	 *
+	 * MD5(K XOR opad, MD5(K XOR ipad, text))
+	 *
+	 * where K is an n byte	key
+	 * ipad	is the byte	0x36 repeated 64 times
+	 * opad	is the byte	0x5c repeated 64 times
+	 * and text	is the data	being protected	*/
+
+	/* start out by	storing	key	in pads	*/
+	NdisZeroMemory(k_ipad, sizeof(k_ipad));
+	NdisZeroMemory(k_opad,	sizeof(k_opad));
+	//assert(key_len < sizeof(k_ipad));
+	NdisMoveMemory(k_ipad, key,	key_len);
+	NdisMoveMemory(k_opad, key,	key_len);
+
+	/* XOR key with	ipad and opad values */
+	for	(i = 0;	i <	64;	i++) {
+		k_ipad[i] ^= 0x36;
+		k_opad[i] ^= 0x5c;
+	}
+
+	/* perform inner MD5 */
+	MD5Init(&context);					 /*	init context for 1st pass */
+	MD5Update(&context,	k_ipad,	64);	 /*	start with inner pad */
+	MD5Update(&context,	data, data_len); /*	then text of datagram */
+	MD5Final(mac, &context);			 /*	finish up 1st pass */
+
+	/* perform outer MD5 */
+	MD5Init(&context);					 /*	init context for 2nd pass */
+	MD5Update(&context,	k_opad,	64);	 /*	start with outer pad */
+	MD5Update(&context,	mac, 16);		 /*	then results of	1st	hash */
+	MD5Final(mac, &context);			 /*	finish up 2nd pass */
+}
+
+#ifndef RT_BIG_ENDIAN
+#define byteReverse(buf, len)   /* Nothing */
+#else
+void byteReverse(unsigned char *buf, unsigned longs);
+void byteReverse(unsigned char *buf, unsigned longs)
+{
+    do {
+        *(UINT32 *)buf = SWAP32(*(UINT32 *)buf);
+        buf += 4;
+    } while (--longs);
+}
+#endif
+
+
+/* ==========================  MD5 implementation =========================== */
+// four base functions for MD5
+#define MD5_F1(x, y, z) (((x) & (y)) | ((~x) & (z)))
+#define MD5_F2(x, y, z) (((x) & (z)) | ((y) & (~z)))
+#define MD5_F3(x, y, z) ((x) ^ (y) ^ (z))
+#define MD5_F4(x, y, z) ((y) ^ ((x) | (~z)))
+#define CYCLIC_LEFT_SHIFT(w, s) (((w) << (s)) | ((w) >> (32-(s))))
+
+#define	MD5Step(f, w, x, y,	z, data, t, s)	\
+	( w	+= f(x,	y, z) +	data + t,  w = (CYCLIC_LEFT_SHIFT(w, s)) & 0xffffffff, w +=	x )
+
+
+/*
+ *  Function Description:
+ *      Initiate MD5 Context satisfied in RFC 1321
+ *
+ *  Arguments:
+ *      pCtx        Pointer	to MD5 context
+ *
+ *  Return Value:
+ *      None
+ */
+VOID MD5Init(MD5_CTX *pCtx)
+{
+    pCtx->Buf[0]=0x67452301;
+    pCtx->Buf[1]=0xefcdab89;
+    pCtx->Buf[2]=0x98badcfe;
+    pCtx->Buf[3]=0x10325476;
+
+    pCtx->LenInBitCount[0]=0;
+    pCtx->LenInBitCount[1]=0;
+}
+
+
+/*
+ *  Function Description:
+ *      Update MD5 Context, allow of an arrary of octets as the next portion
+ *      of the message
+ *
+ *  Arguments:
+ *      pCtx		Pointer	to MD5 context
+ * 	    pData       Pointer to input data
+ *      LenInBytes  The length of input data (unit: byte)
+ *
+ *  Return Value:
+ *      None
+ *
+ *  Note:
+ *      Called after MD5Init or MD5Update(itself)
+ */
+VOID MD5Update(MD5_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes)
+{
+
+    UINT32 TfTimes;
+    UINT32 temp;
+	unsigned int i;
+
+    temp = pCtx->LenInBitCount[0];
+
+    pCtx->LenInBitCount[0] = (UINT32) (pCtx->LenInBitCount[0] + (LenInBytes << 3));
+
+    if (pCtx->LenInBitCount[0] < temp)
+        pCtx->LenInBitCount[1]++;   //carry in
+
+    pCtx->LenInBitCount[1] += LenInBytes >> 29;
+
+    // mod 64 bytes
+    temp = (temp >> 3) & 0x3f;
+
+    // process lacks of 64-byte data
+    if (temp)
+    {
+        UCHAR *pAds = (UCHAR *) pCtx->Input + temp;
+
+        if ((temp+LenInBytes) < 64)
+        {
+            NdisMoveMemory(pAds, (UCHAR *)pData, LenInBytes);
+            return;
+        }
+
+        NdisMoveMemory(pAds, (UCHAR *)pData, 64-temp);
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+        pData += 64-temp;
+        LenInBytes -= 64-temp;
+    } // end of if (temp)
+
+
+    TfTimes = (LenInBytes >> 6);
+
+    for (i=TfTimes; i>0; i--)
+    {
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, 64);
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+        pData += 64;
+        LenInBytes -= 64;
+    } // end of for
+
+    // buffering lacks of 64-byte data
+    if(LenInBytes)
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, LenInBytes);
+
+}
+
+
+/*
+ *  Function Description:
+ *      Append padding bits and length of original message in the tail
+ *      The message digest has to be completed in the end
+ *
+ *  Arguments:
+ *      Digest		Output of Digest-Message for MD5
+ *  	pCtx        Pointer	to MD5 context
+ *
+ *  Return Value:
+ *      None
+ *
+ *  Note:
+ *      Called after MD5Update
+ */
+VOID MD5Final(UCHAR Digest[16], MD5_CTX *pCtx)
+{
+    UCHAR Remainder;
+    UCHAR PadLenInBytes;
+    UCHAR *pAppend=0;
+    unsigned int i;
+
+    Remainder = (UCHAR)((pCtx->LenInBitCount[0] >> 3) & 0x3f);
+
+    PadLenInBytes = (Remainder < 56) ? (56-Remainder) : (120-Remainder);
+
+    pAppend = (UCHAR *)pCtx->Input + Remainder;
+
+    // padding bits without crossing block(64-byte based) boundary
+    if (Remainder < 56)
+    {
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, PadLenInBytes);
+
+		// add data-length field, from low to high
+       	for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[0] >> (i << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[1] >> (i << 3)) & 0xff);
+      	}
+
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of if
+
+    // padding bits with crossing block(64-byte based) boundary
+    else
+    {
+        // the first block ===
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, (64-Remainder-1));
+        PadLenInBytes -= (64 - Remainder - 1);
+
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+
+        // the second block ===
+        NdisZeroMemory((UCHAR *)pCtx->Input, PadLenInBytes);
+
+        // add data-length field
+        for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[0] >> (i << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[1] >> (i << 3)) & 0xff);
+      	}
+
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of else
+
+
+    NdisMoveMemory((UCHAR *)Digest, (UINT32 *)pCtx->Buf, 16); // output
+    byteReverse((UCHAR *)Digest, 4);
+    NdisZeroMemory(pCtx, sizeof(pCtx)); // memory free
+}
+
+
+/*
+ *  Function Description:
+ *      The central algorithm of MD5, consists of four rounds and sixteen
+ *  	steps per round
+ *
+ *  Arguments:
+ *      Buf     Buffers of four states (output: 16 bytes)
+ * 	    Mes     Input data (input: 64 bytes)
+ *
+ *  Return Value:
+ *      None
+ *
+ *  Note:
+ *      Called by MD5Update or MD5Final
+ */
+VOID MD5Transform(UINT32 Buf[4], UINT32 Mes[16])
+{
+    UINT32 Reg[4], Temp;
+	unsigned int i;
+
+    static UCHAR LShiftVal[16] =
+    {
+        7, 12, 17, 22,
+		5, 9 , 14, 20,
+		4, 11, 16, 23,
+ 		6, 10, 15, 21,
+ 	};
+
+
+	// [equal to 4294967296*abs(sin(index))]
+    static UINT32 MD5Table[64] =
+	{
+		0xd76aa478,	0xe8c7b756,	0x242070db,	0xc1bdceee,
+		0xf57c0faf,	0x4787c62a,	0xa8304613, 0xfd469501,
+		0x698098d8,	0x8b44f7af,	0xffff5bb1,	0x895cd7be,
+    	0x6b901122,	0xfd987193,	0xa679438e,	0x49b40821,
+
+    	0xf61e2562,	0xc040b340,	0x265e5a51,	0xe9b6c7aa,
+    	0xd62f105d,	0x02441453,	0xd8a1e681,	0xe7d3fbc8,
+    	0x21e1cde6,	0xc33707d6,	0xf4d50d87,	0x455a14ed,
+    	0xa9e3e905,	0xfcefa3f8,	0x676f02d9,	0x8d2a4c8a,
+
+    	0xfffa3942,	0x8771f681,	0x6d9d6122,	0xfde5380c,
+    	0xa4beea44,	0x4bdecfa9,	0xf6bb4b60,	0xbebfbc70,
+    	0x289b7ec6,	0xeaa127fa,	0xd4ef3085,	0x04881d05,
+    	0xd9d4d039,	0xe6db99e5,	0x1fa27cf8,	0xc4ac5665,
+
+    	0xf4292244,	0x432aff97,	0xab9423a7,	0xfc93a039,
+   		0x655b59c3,	0x8f0ccc92,	0xffeff47d,	0x85845dd1,
+    	0x6fa87e4f,	0xfe2ce6e0,	0xa3014314,	0x4e0811a1,
+    	0xf7537e82,	0xbd3af235,	0x2ad7d2bb,	0xeb86d391
+	};
+
+
+    for (i=0; i<4; i++)
+        Reg[i]=Buf[i];
+
+
+    // 64 steps in MD5 algorithm
+    for (i=0; i<16; i++)
+    {
+        MD5Step(MD5_F1, Reg[0], Reg[1], Reg[2], Reg[3], Mes[i],
+                MD5Table[i], LShiftVal[i & 0x3]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+    for (i=16; i<32; i++)
+    {
+        MD5Step(MD5_F2, Reg[0], Reg[1], Reg[2], Reg[3], Mes[(5*(i & 0xf)+1) & 0xf],
+                MD5Table[i], LShiftVal[(0x1 << 2)+(i & 0x3)]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+    for (i=32; i<48; i++)
+    {
+        MD5Step(MD5_F3, Reg[0], Reg[1], Reg[2], Reg[3], Mes[(3*(i & 0xf)+5) & 0xf],
+                MD5Table[i], LShiftVal[(0x1 << 3)+(i & 0x3)]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+    for (i=48; i<64; i++)
+    {
+        MD5Step(MD5_F4, Reg[0], Reg[1], Reg[2], Reg[3], Mes[(7*(i & 0xf)) & 0xf],
+                MD5Table[i], LShiftVal[(0x3 << 2)+(i & 0x3)]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+
+
+    // (temporary)output
+    for (i=0; i<4; i++)
+        Buf[i] += Reg[i];
+
+}
+
+
+
+/* =========================  SHA-1 implementation ========================== */
+// four base functions for SHA-1
+#define SHA1_F1(b, c, d)    (((b) & (c)) | ((~b) & (d)))
+#define SHA1_F2(b, c, d)    ((b) ^ (c) ^ (d))
+#define SHA1_F3(b, c, d)    (((b) & (c)) | ((b) & (d)) | ((c) & (d)))
+
+
+#define SHA1Step(f, a, b, c, d, e, w, k)    \
+    ( e	+= ( f(b, c, d) + w + k + CYCLIC_LEFT_SHIFT(a, 5)) & 0xffffffff, \
+      b = CYCLIC_LEFT_SHIFT(b, 30) )
+
+//Initiate SHA-1 Context satisfied in RFC 3174
+VOID SHAInit(SHA_CTX *pCtx)
+{
+    pCtx->Buf[0]=0x67452301;
+    pCtx->Buf[1]=0xefcdab89;
+    pCtx->Buf[2]=0x98badcfe;
+    pCtx->Buf[3]=0x10325476;
+    pCtx->Buf[4]=0xc3d2e1f0;
+
+    pCtx->LenInBitCount[0]=0;
+    pCtx->LenInBitCount[1]=0;
+}
+
+/*
+ *  Function Description:
+ *      Update SHA-1 Context, allow of an arrary of octets as the next
+ *      portion of the message
+ *
+ *  Arguments:
+ *      pCtx		Pointer	to SHA-1 context
+ * 	    pData       Pointer to input data
+ *      LenInBytes  The length of input data (unit: byte)
+ *
+ *  Return Value:
+ *      error       indicate more than pow(2,64) bits of data
+ *
+ *  Note:
+ *      Called after SHAInit or SHAUpdate(itself)
+ */
+UCHAR SHAUpdate(SHA_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes)
+{
+    UINT32 TfTimes;
+    UINT32 temp1,temp2;
+	unsigned int i;
+	UCHAR err=1;
+
+    temp1 = pCtx->LenInBitCount[0];
+    temp2 = pCtx->LenInBitCount[1];
+
+    pCtx->LenInBitCount[0] = (UINT32) (pCtx->LenInBitCount[0] + (LenInBytes << 3));
+    if (pCtx->LenInBitCount[0] < temp1)
+        pCtx->LenInBitCount[1]++;   //carry in
+
+
+    pCtx->LenInBitCount[1] = (UINT32) (pCtx->LenInBitCount[1] +(LenInBytes >> 29));
+    if (pCtx->LenInBitCount[1] < temp2)
+        return (err);   //check total length of original data
+
+
+    // mod 64 bytes
+    temp1 = (temp1 >> 3) & 0x3f;
+
+    // process lacks of 64-byte data
+    if (temp1)
+    {
+        UCHAR *pAds = (UCHAR *) pCtx->Input + temp1;
+
+        if ((temp1+LenInBytes) < 64)
+        {
+            NdisMoveMemory(pAds, (UCHAR *)pData, LenInBytes);
+            return (0);
+        }
+
+        NdisMoveMemory(pAds, (UCHAR *)pData, 64-temp1);
+        byteReverse((UCHAR *)pCtx->Input, 16);
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+        pData += 64-temp1;
+        LenInBytes -= 64-temp1;
+    } // end of if (temp1)
+
+
+    TfTimes = (LenInBytes >> 6);
+
+    for (i=TfTimes; i>0; i--)
+    {
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, 64);
+        byteReverse((UCHAR *)pCtx->Input, 16);
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+        pData += 64;
+        LenInBytes -= 64;
+    } // end of for
+
+    // buffering lacks of 64-byte data
+    if(LenInBytes)
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, LenInBytes);
+
+	return (0);
+
+}
+
+// Append padding bits and length of original message in the tail
+// The message digest has to be completed in the end
+VOID SHAFinal(SHA_CTX *pCtx, UCHAR Digest[20])
+{
+    UCHAR Remainder;
+    UCHAR PadLenInBytes;
+    UCHAR *pAppend=0;
+    unsigned int i;
+
+    Remainder = (UCHAR)((pCtx->LenInBitCount[0] >> 3) & 0x3f);
+
+    pAppend = (UCHAR *)pCtx->Input + Remainder;
+
+    PadLenInBytes = (Remainder < 56) ? (56-Remainder) : (120-Remainder);
+
+    // padding bits without crossing block(64-byte based) boundary
+    if (Remainder < 56)
+    {
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, PadLenInBytes);
+
+		// add data-length field, from high to low
+        for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[1] >> ((3-i) << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[0] >> ((3-i) << 3)) & 0xff);
+      	}
+
+        byteReverse((UCHAR *)pCtx->Input, 16);
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 14);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of if
+
+    // padding bits with crossing block(64-byte based) boundary
+    else
+    {
+        // the first block ===
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, (64-Remainder-1));
+        PadLenInBytes -= (64 - Remainder - 1);
+
+        byteReverse((UCHAR *)pCtx->Input, 16);
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+
+        // the second block ===
+        NdisZeroMemory((UCHAR *)pCtx->Input, PadLenInBytes);
+
+		// add data-length field
+		for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[1] >> ((3-i) << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[0] >> ((3-i) << 3)) & 0xff);
+      	}
+
+        byteReverse((UCHAR *)pCtx->Input, 16);
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of else
+
+
+    //Output, bytereverse
+    for (i=0; i<20; i++)
+    {
+        Digest [i] = (UCHAR)(pCtx->Buf[i>>2] >> 8*(3-(i & 0x3)));
+    }
+
+    NdisZeroMemory(pCtx, sizeof(pCtx)); // memory free
+}
+
+
+// The central algorithm of SHA-1, consists of four rounds and
+// twenty steps per round
+VOID SHATransform(UINT32 Buf[5], UINT32 Mes[20])
+{
+    UINT32 Reg[5],Temp;
+	unsigned int i;
+    UINT32 W[80];
+
+    static UINT32 SHA1Table[4] = { 0x5a827999, 0x6ed9eba1,
+                                  0x8f1bbcdc, 0xca62c1d6 };
+
+    Reg[0]=Buf[0];
+	Reg[1]=Buf[1];
+	Reg[2]=Buf[2];
+	Reg[3]=Buf[3];
+	Reg[4]=Buf[4];
+
+    //the first octet of a word is stored in the 0th element, bytereverse
+	for(i = 0; i < 16; i++)
+    {
+    	W[i]  = (Mes[i] >> 24) & 0xff;
+        W[i] |= (Mes[i] >> 8 ) & 0xff00;
+        W[i] |= (Mes[i] << 8 ) & 0xff0000;
+        W[i] |= (Mes[i] << 24) & 0xff000000;
+    }
+
+
+    for	(i = 0; i < 64; i++)
+	    W[16+i] = CYCLIC_LEFT_SHIFT(W[i] ^ W[2+i] ^ W[8+i] ^ W[13+i], 1);
+
+
+    // 80 steps in SHA-1 algorithm
+    for (i=0; i<80; i++)
+    {
+        if (i<20)
+            SHA1Step(SHA1_F1, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                     W[i], SHA1Table[0]);
+
+        else if (i>=20 && i<40)
+            SHA1Step(SHA1_F2, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                     W[i], SHA1Table[1]);
+
+		else if (i>=40 && i<60)
+            SHA1Step(SHA1_F3, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                      W[i], SHA1Table[2]);
+
+        else
+            SHA1Step(SHA1_F2, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                     W[i], SHA1Table[3]);
+
+
+       // one-word right shift
+		Temp   = Reg[4];
+        Reg[4] = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+
+    } // end of for-loop
+
+
+    // (temporary)output
+    for (i=0; i<5; i++)
+        Buf[i] += Reg[i];
+
+}
+
+
+/* =========================  AES En/Decryption ========================== */
+
+/* forward S-box */
+static uint32 FSb[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
+};
+
+/* forward table */
+#define	FT \
+\
+	V(C6,63,63,A5),	V(F8,7C,7C,84),	V(EE,77,77,99),	V(F6,7B,7B,8D),	\
+	V(FF,F2,F2,0D),	V(D6,6B,6B,BD),	V(DE,6F,6F,B1),	V(91,C5,C5,54),	\
+	V(60,30,30,50),	V(02,01,01,03),	V(CE,67,67,A9),	V(56,2B,2B,7D),	\
+	V(E7,FE,FE,19),	V(B5,D7,D7,62),	V(4D,AB,AB,E6),	V(EC,76,76,9A),	\
+	V(8F,CA,CA,45),	V(1F,82,82,9D),	V(89,C9,C9,40),	V(FA,7D,7D,87),	\
+	V(EF,FA,FA,15),	V(B2,59,59,EB),	V(8E,47,47,C9),	V(FB,F0,F0,0B),	\
+	V(41,AD,AD,EC),	V(B3,D4,D4,67),	V(5F,A2,A2,FD),	V(45,AF,AF,EA),	\
+	V(23,9C,9C,BF),	V(53,A4,A4,F7),	V(E4,72,72,96),	V(9B,C0,C0,5B),	\
+	V(75,B7,B7,C2),	V(E1,FD,FD,1C),	V(3D,93,93,AE),	V(4C,26,26,6A),	\
+	V(6C,36,36,5A),	V(7E,3F,3F,41),	V(F5,F7,F7,02),	V(83,CC,CC,4F),	\
+	V(68,34,34,5C),	V(51,A5,A5,F4),	V(D1,E5,E5,34),	V(F9,F1,F1,08),	\
+	V(E2,71,71,93),	V(AB,D8,D8,73),	V(62,31,31,53),	V(2A,15,15,3F),	\
+	V(08,04,04,0C),	V(95,C7,C7,52),	V(46,23,23,65),	V(9D,C3,C3,5E),	\
+	V(30,18,18,28),	V(37,96,96,A1),	V(0A,05,05,0F),	V(2F,9A,9A,B5),	\
+	V(0E,07,07,09),	V(24,12,12,36),	V(1B,80,80,9B),	V(DF,E2,E2,3D),	\
+	V(CD,EB,EB,26),	V(4E,27,27,69),	V(7F,B2,B2,CD),	V(EA,75,75,9F),	\
+	V(12,09,09,1B),	V(1D,83,83,9E),	V(58,2C,2C,74),	V(34,1A,1A,2E),	\
+	V(36,1B,1B,2D),	V(DC,6E,6E,B2),	V(B4,5A,5A,EE),	V(5B,A0,A0,FB),	\
+	V(A4,52,52,F6),	V(76,3B,3B,4D),	V(B7,D6,D6,61),	V(7D,B3,B3,CE),	\
+	V(52,29,29,7B),	V(DD,E3,E3,3E),	V(5E,2F,2F,71),	V(13,84,84,97),	\
+	V(A6,53,53,F5),	V(B9,D1,D1,68),	V(00,00,00,00),	V(C1,ED,ED,2C),	\
+	V(40,20,20,60),	V(E3,FC,FC,1F),	V(79,B1,B1,C8),	V(B6,5B,5B,ED),	\
+	V(D4,6A,6A,BE),	V(8D,CB,CB,46),	V(67,BE,BE,D9),	V(72,39,39,4B),	\
+	V(94,4A,4A,DE),	V(98,4C,4C,D4),	V(B0,58,58,E8),	V(85,CF,CF,4A),	\
+	V(BB,D0,D0,6B),	V(C5,EF,EF,2A),	V(4F,AA,AA,E5),	V(ED,FB,FB,16),	\
+	V(86,43,43,C5),	V(9A,4D,4D,D7),	V(66,33,33,55),	V(11,85,85,94),	\
+	V(8A,45,45,CF),	V(E9,F9,F9,10),	V(04,02,02,06),	V(FE,7F,7F,81),	\
+	V(A0,50,50,F0),	V(78,3C,3C,44),	V(25,9F,9F,BA),	V(4B,A8,A8,E3),	\
+	V(A2,51,51,F3),	V(5D,A3,A3,FE),	V(80,40,40,C0),	V(05,8F,8F,8A),	\
+	V(3F,92,92,AD),	V(21,9D,9D,BC),	V(70,38,38,48),	V(F1,F5,F5,04),	\
+	V(63,BC,BC,DF),	V(77,B6,B6,C1),	V(AF,DA,DA,75),	V(42,21,21,63),	\
+	V(20,10,10,30),	V(E5,FF,FF,1A),	V(FD,F3,F3,0E),	V(BF,D2,D2,6D),	\
+	V(81,CD,CD,4C),	V(18,0C,0C,14),	V(26,13,13,35),	V(C3,EC,EC,2F),	\
+	V(BE,5F,5F,E1),	V(35,97,97,A2),	V(88,44,44,CC),	V(2E,17,17,39),	\
+	V(93,C4,C4,57),	V(55,A7,A7,F2),	V(FC,7E,7E,82),	V(7A,3D,3D,47),	\
+	V(C8,64,64,AC),	V(BA,5D,5D,E7),	V(32,19,19,2B),	V(E6,73,73,95),	\
+	V(C0,60,60,A0),	V(19,81,81,98),	V(9E,4F,4F,D1),	V(A3,DC,DC,7F),	\
+	V(44,22,22,66),	V(54,2A,2A,7E),	V(3B,90,90,AB),	V(0B,88,88,83),	\
+	V(8C,46,46,CA),	V(C7,EE,EE,29),	V(6B,B8,B8,D3),	V(28,14,14,3C),	\
+	V(A7,DE,DE,79),	V(BC,5E,5E,E2),	V(16,0B,0B,1D),	V(AD,DB,DB,76),	\
+	V(DB,E0,E0,3B),	V(64,32,32,56),	V(74,3A,3A,4E),	V(14,0A,0A,1E),	\
+	V(92,49,49,DB),	V(0C,06,06,0A),	V(48,24,24,6C),	V(B8,5C,5C,E4),	\
+	V(9F,C2,C2,5D),	V(BD,D3,D3,6E),	V(43,AC,AC,EF),	V(C4,62,62,A6),	\
+	V(39,91,91,A8),	V(31,95,95,A4),	V(D3,E4,E4,37),	V(F2,79,79,8B),	\
+	V(D5,E7,E7,32),	V(8B,C8,C8,43),	V(6E,37,37,59),	V(DA,6D,6D,B7),	\
+	V(01,8D,8D,8C),	V(B1,D5,D5,64),	V(9C,4E,4E,D2),	V(49,A9,A9,E0),	\
+	V(D8,6C,6C,B4),	V(AC,56,56,FA),	V(F3,F4,F4,07),	V(CF,EA,EA,25),	\
+	V(CA,65,65,AF),	V(F4,7A,7A,8E),	V(47,AE,AE,E9),	V(10,08,08,18),	\
+	V(6F,BA,BA,D5),	V(F0,78,78,88),	V(4A,25,25,6F),	V(5C,2E,2E,72),	\
+	V(38,1C,1C,24),	V(57,A6,A6,F1),	V(73,B4,B4,C7),	V(97,C6,C6,51),	\
+	V(CB,E8,E8,23),	V(A1,DD,DD,7C),	V(E8,74,74,9C),	V(3E,1F,1F,21),	\
+	V(96,4B,4B,DD),	V(61,BD,BD,DC),	V(0D,8B,8B,86),	V(0F,8A,8A,85),	\
+	V(E0,70,70,90),	V(7C,3E,3E,42),	V(71,B5,B5,C4),	V(CC,66,66,AA),	\
+	V(90,48,48,D8),	V(06,03,03,05),	V(F7,F6,F6,01),	V(1C,0E,0E,12),	\
+	V(C2,61,61,A3),	V(6A,35,35,5F),	V(AE,57,57,F9),	V(69,B9,B9,D0),	\
+	V(17,86,86,91),	V(99,C1,C1,58),	V(3A,1D,1D,27),	V(27,9E,9E,B9),	\
+	V(D9,E1,E1,38),	V(EB,F8,F8,13),	V(2B,98,98,B3),	V(22,11,11,33),	\
+	V(D2,69,69,BB),	V(A9,D9,D9,70),	V(07,8E,8E,89),	V(33,94,94,A7),	\
+	V(2D,9B,9B,B6),	V(3C,1E,1E,22),	V(15,87,87,92),	V(C9,E9,E9,20),	\
+	V(87,CE,CE,49),	V(AA,55,55,FF),	V(50,28,28,78),	V(A5,DF,DF,7A),	\
+	V(03,8C,8C,8F),	V(59,A1,A1,F8),	V(09,89,89,80),	V(1A,0D,0D,17),	\
+	V(65,BF,BF,DA),	V(D7,E6,E6,31),	V(84,42,42,C6),	V(D0,68,68,B8),	\
+	V(82,41,41,C3),	V(29,99,99,B0),	V(5A,2D,2D,77),	V(1E,0F,0F,11),	\
+	V(7B,B0,B0,CB),	V(A8,54,54,FC),	V(6D,BB,BB,D6),	V(2C,16,16,3A)
+
+#define	V(a,b,c,d) 0x##a##b##c##d
+static uint32 FT0[256] = { FT };
+#undef V
+
+#define	V(a,b,c,d) 0x##d##a##b##c
+static uint32 FT1[256] = { FT };
+#undef V
+
+#define	V(a,b,c,d) 0x##c##d##a##b
+static uint32 FT2[256] = { FT };
+#undef V
+
+#define	V(a,b,c,d) 0x##b##c##d##a
+static uint32 FT3[256] = { FT };
+#undef V
+
+#undef FT
+
+/* reverse S-box */
+
+static uint32 RSb[256] =
+{
+	0x52, 0x09,	0x6A, 0xD5,	0x30, 0x36,	0xA5, 0x38,
+	0xBF, 0x40,	0xA3, 0x9E,	0x81, 0xF3,	0xD7, 0xFB,
+	0x7C, 0xE3,	0x39, 0x82,	0x9B, 0x2F,	0xFF, 0x87,
+	0x34, 0x8E,	0x43, 0x44,	0xC4, 0xDE,	0xE9, 0xCB,
+	0x54, 0x7B,	0x94, 0x32,	0xA6, 0xC2,	0x23, 0x3D,
+	0xEE, 0x4C,	0x95, 0x0B,	0x42, 0xFA,	0xC3, 0x4E,
+	0x08, 0x2E,	0xA1, 0x66,	0x28, 0xD9,	0x24, 0xB2,
+	0x76, 0x5B,	0xA2, 0x49,	0x6D, 0x8B,	0xD1, 0x25,
+	0x72, 0xF8,	0xF6, 0x64,	0x86, 0x68,	0x98, 0x16,
+	0xD4, 0xA4,	0x5C, 0xCC,	0x5D, 0x65,	0xB6, 0x92,
+	0x6C, 0x70,	0x48, 0x50,	0xFD, 0xED,	0xB9, 0xDA,
+	0x5E, 0x15,	0x46, 0x57,	0xA7, 0x8D,	0x9D, 0x84,
+	0x90, 0xD8,	0xAB, 0x00,	0x8C, 0xBC,	0xD3, 0x0A,
+	0xF7, 0xE4,	0x58, 0x05,	0xB8, 0xB3,	0x45, 0x06,
+	0xD0, 0x2C,	0x1E, 0x8F,	0xCA, 0x3F,	0x0F, 0x02,
+	0xC1, 0xAF,	0xBD, 0x03,	0x01, 0x13,	0x8A, 0x6B,
+	0x3A, 0x91,	0x11, 0x41,	0x4F, 0x67,	0xDC, 0xEA,
+	0x97, 0xF2,	0xCF, 0xCE,	0xF0, 0xB4,	0xE6, 0x73,
+	0x96, 0xAC,	0x74, 0x22,	0xE7, 0xAD,	0x35, 0x85,
+	0xE2, 0xF9,	0x37, 0xE8,	0x1C, 0x75,	0xDF, 0x6E,
+	0x47, 0xF1,	0x1A, 0x71,	0x1D, 0x29,	0xC5, 0x89,
+	0x6F, 0xB7,	0x62, 0x0E,	0xAA, 0x18,	0xBE, 0x1B,
+	0xFC, 0x56,	0x3E, 0x4B,	0xC6, 0xD2,	0x79, 0x20,
+	0x9A, 0xDB,	0xC0, 0xFE,	0x78, 0xCD,	0x5A, 0xF4,
+	0x1F, 0xDD,	0xA8, 0x33,	0x88, 0x07,	0xC7, 0x31,
+	0xB1, 0x12,	0x10, 0x59,	0x27, 0x80,	0xEC, 0x5F,
+	0x60, 0x51,	0x7F, 0xA9,	0x19, 0xB5,	0x4A, 0x0D,
+	0x2D, 0xE5,	0x7A, 0x9F,	0x93, 0xC9,	0x9C, 0xEF,
+	0xA0, 0xE0,	0x3B, 0x4D,	0xAE, 0x2A,	0xF5, 0xB0,
+	0xC8, 0xEB,	0xBB, 0x3C,	0x83, 0x53,	0x99, 0x61,
+	0x17, 0x2B,	0x04, 0x7E,	0xBA, 0x77,	0xD6, 0x26,
+	0xE1, 0x69,	0x14, 0x63,	0x55, 0x21,	0x0C, 0x7D
+};
+
+/* reverse table */
+
+#define	RT \
+\
+	V(51,F4,A7,50),	V(7E,41,65,53),	V(1A,17,A4,C3),	V(3A,27,5E,96),	\
+	V(3B,AB,6B,CB),	V(1F,9D,45,F1),	V(AC,FA,58,AB),	V(4B,E3,03,93),	\
+	V(20,30,FA,55),	V(AD,76,6D,F6),	V(88,CC,76,91),	V(F5,02,4C,25),	\
+	V(4F,E5,D7,FC),	V(C5,2A,CB,D7),	V(26,35,44,80),	V(B5,62,A3,8F),	\
+	V(DE,B1,5A,49),	V(25,BA,1B,67),	V(45,EA,0E,98),	V(5D,FE,C0,E1),	\
+	V(C3,2F,75,02),	V(81,4C,F0,12),	V(8D,46,97,A3),	V(6B,D3,F9,C6),	\
+	V(03,8F,5F,E7),	V(15,92,9C,95),	V(BF,6D,7A,EB),	V(95,52,59,DA),	\
+	V(D4,BE,83,2D),	V(58,74,21,D3),	V(49,E0,69,29),	V(8E,C9,C8,44),	\
+	V(75,C2,89,6A),	V(F4,8E,79,78),	V(99,58,3E,6B),	V(27,B9,71,DD),	\
+	V(BE,E1,4F,B6),	V(F0,88,AD,17),	V(C9,20,AC,66),	V(7D,CE,3A,B4),	\
+	V(63,DF,4A,18),	V(E5,1A,31,82),	V(97,51,33,60),	V(62,53,7F,45),	\
+	V(B1,64,77,E0),	V(BB,6B,AE,84),	V(FE,81,A0,1C),	V(F9,08,2B,94),	\
+	V(70,48,68,58),	V(8F,45,FD,19),	V(94,DE,6C,87),	V(52,7B,F8,B7),	\
+	V(AB,73,D3,23),	V(72,4B,02,E2),	V(E3,1F,8F,57),	V(66,55,AB,2A),	\
+	V(B2,EB,28,07),	V(2F,B5,C2,03),	V(86,C5,7B,9A),	V(D3,37,08,A5),	\
+	V(30,28,87,F2),	V(23,BF,A5,B2),	V(02,03,6A,BA),	V(ED,16,82,5C),	\
+	V(8A,CF,1C,2B),	V(A7,79,B4,92),	V(F3,07,F2,F0),	V(4E,69,E2,A1),	\
+	V(65,DA,F4,CD),	V(06,05,BE,D5),	V(D1,34,62,1F),	V(C4,A6,FE,8A),	\
+	V(34,2E,53,9D),	V(A2,F3,55,A0),	V(05,8A,E1,32),	V(A4,F6,EB,75),	\
+	V(0B,83,EC,39),	V(40,60,EF,AA),	V(5E,71,9F,06),	V(BD,6E,10,51),	\
+	V(3E,21,8A,F9),	V(96,DD,06,3D),	V(DD,3E,05,AE),	V(4D,E6,BD,46),	\
+	V(91,54,8D,B5),	V(71,C4,5D,05),	V(04,06,D4,6F),	V(60,50,15,FF),	\
+	V(19,98,FB,24),	V(D6,BD,E9,97),	V(89,40,43,CC),	V(67,D9,9E,77),	\
+	V(B0,E8,42,BD),	V(07,89,8B,88),	V(E7,19,5B,38),	V(79,C8,EE,DB),	\
+	V(A1,7C,0A,47),	V(7C,42,0F,E9),	V(F8,84,1E,C9),	V(00,00,00,00),	\
+	V(09,80,86,83),	V(32,2B,ED,48),	V(1E,11,70,AC),	V(6C,5A,72,4E),	\
+	V(FD,0E,FF,FB),	V(0F,85,38,56),	V(3D,AE,D5,1E),	V(36,2D,39,27),	\
+	V(0A,0F,D9,64),	V(68,5C,A6,21),	V(9B,5B,54,D1),	V(24,36,2E,3A),	\
+	V(0C,0A,67,B1),	V(93,57,E7,0F),	V(B4,EE,96,D2),	V(1B,9B,91,9E),	\
+	V(80,C0,C5,4F),	V(61,DC,20,A2),	V(5A,77,4B,69),	V(1C,12,1A,16),	\
+	V(E2,93,BA,0A),	V(C0,A0,2A,E5),	V(3C,22,E0,43),	V(12,1B,17,1D),	\
+	V(0E,09,0D,0B),	V(F2,8B,C7,AD),	V(2D,B6,A8,B9),	V(14,1E,A9,C8),	\
+	V(57,F1,19,85),	V(AF,75,07,4C),	V(EE,99,DD,BB),	V(A3,7F,60,FD),	\
+	V(F7,01,26,9F),	V(5C,72,F5,BC),	V(44,66,3B,C5),	V(5B,FB,7E,34),	\
+	V(8B,43,29,76),	V(CB,23,C6,DC),	V(B6,ED,FC,68),	V(B8,E4,F1,63),	\
+	V(D7,31,DC,CA),	V(42,63,85,10),	V(13,97,22,40),	V(84,C6,11,20),	\
+	V(85,4A,24,7D),	V(D2,BB,3D,F8),	V(AE,F9,32,11),	V(C7,29,A1,6D),	\
+	V(1D,9E,2F,4B),	V(DC,B2,30,F3),	V(0D,86,52,EC),	V(77,C1,E3,D0),	\
+	V(2B,B3,16,6C),	V(A9,70,B9,99),	V(11,94,48,FA),	V(47,E9,64,22),	\
+	V(A8,FC,8C,C4),	V(A0,F0,3F,1A),	V(56,7D,2C,D8),	V(22,33,90,EF),	\
+	V(87,49,4E,C7),	V(D9,38,D1,C1),	V(8C,CA,A2,FE),	V(98,D4,0B,36),	\
+	V(A6,F5,81,CF),	V(A5,7A,DE,28),	V(DA,B7,8E,26),	V(3F,AD,BF,A4),	\
+	V(2C,3A,9D,E4),	V(50,78,92,0D),	V(6A,5F,CC,9B),	V(54,7E,46,62),	\
+	V(F6,8D,13,C2),	V(90,D8,B8,E8),	V(2E,39,F7,5E),	V(82,C3,AF,F5),	\
+	V(9F,5D,80,BE),	V(69,D0,93,7C),	V(6F,D5,2D,A9),	V(CF,25,12,B3),	\
+	V(C8,AC,99,3B),	V(10,18,7D,A7),	V(E8,9C,63,6E),	V(DB,3B,BB,7B),	\
+	V(CD,26,78,09),	V(6E,59,18,F4),	V(EC,9A,B7,01),	V(83,4F,9A,A8),	\
+	V(E6,95,6E,65),	V(AA,FF,E6,7E),	V(21,BC,CF,08),	V(EF,15,E8,E6),	\
+	V(BA,E7,9B,D9),	V(4A,6F,36,CE),	V(EA,9F,09,D4),	V(29,B0,7C,D6),	\
+	V(31,A4,B2,AF),	V(2A,3F,23,31),	V(C6,A5,94,30),	V(35,A2,66,C0),	\
+	V(74,4E,BC,37),	V(FC,82,CA,A6),	V(E0,90,D0,B0),	V(33,A7,D8,15),	\
+	V(F1,04,98,4A),	V(41,EC,DA,F7),	V(7F,CD,50,0E),	V(17,91,F6,2F),	\
+	V(76,4D,D6,8D),	V(43,EF,B0,4D),	V(CC,AA,4D,54),	V(E4,96,04,DF),	\
+	V(9E,D1,B5,E3),	V(4C,6A,88,1B),	V(C1,2C,1F,B8),	V(46,65,51,7F),	\
+	V(9D,5E,EA,04),	V(01,8C,35,5D),	V(FA,87,74,73),	V(FB,0B,41,2E),	\
+	V(B3,67,1D,5A),	V(92,DB,D2,52),	V(E9,10,56,33),	V(6D,D6,47,13),	\
+	V(9A,D7,61,8C),	V(37,A1,0C,7A),	V(59,F8,14,8E),	V(EB,13,3C,89),	\
+	V(CE,A9,27,EE),	V(B7,61,C9,35),	V(E1,1C,E5,ED),	V(7A,47,B1,3C),	\
+	V(9C,D2,DF,59),	V(55,F2,73,3F),	V(18,14,CE,79),	V(73,C7,37,BF),	\
+	V(53,F7,CD,EA),	V(5F,FD,AA,5B),	V(DF,3D,6F,14),	V(78,44,DB,86),	\
+	V(CA,AF,F3,81),	V(B9,68,C4,3E),	V(38,24,34,2C),	V(C2,A3,40,5F),	\
+	V(16,1D,C3,72),	V(BC,E2,25,0C),	V(28,3C,49,8B),	V(FF,0D,95,41),	\
+	V(39,A8,01,71),	V(08,0C,B3,DE),	V(D8,B4,E4,9C),	V(64,56,C1,90),	\
+	V(7B,CB,84,61),	V(D5,32,B6,70),	V(48,6C,5C,74),	V(D0,B8,57,42)
+
+#define	V(a,b,c,d) 0x##a##b##c##d
+static uint32 RT0[256] = { RT };
+#undef V
+
+#define	V(a,b,c,d) 0x##d##a##b##c
+static uint32 RT1[256] = { RT };
+#undef V
+
+#define	V(a,b,c,d) 0x##c##d##a##b
+static uint32 RT2[256] = { RT };
+#undef V
+
+#define	V(a,b,c,d) 0x##b##c##d##a
+static uint32 RT3[256] = { RT };
+#undef V
+
+#undef RT
+
+/* round constants */
+
+static uint32 RCON[10] =
+{
+	0x01000000,	0x02000000,	0x04000000,	0x08000000,
+	0x10000000,	0x20000000,	0x40000000,	0x80000000,
+	0x1B000000,	0x36000000
+};
+
+/* key schedule	tables */
+
+static int KT_init = 1;
+
+static uint32 KT0[256];
+static uint32 KT1[256];
+static uint32 KT2[256];
+static uint32 KT3[256];
+
+/* platform-independant	32-bit integer manipulation	macros */
+
+#define	GET_UINT32(n,b,i)						\
+{												\
+	(n)	= (	(uint32) (b)[(i)	] << 24	)		\
+		| (	(uint32) (b)[(i) + 1] << 16	)		\
+		| (	(uint32) (b)[(i) + 2] <<  8	)		\
+		| (	(uint32) (b)[(i) + 3]		);		\
+}
+
+#define	PUT_UINT32(n,b,i)						\
+{												\
+	(b)[(i)	   ] = (uint8) ( (n) >>	24 );		\
+	(b)[(i)	+ 1] = (uint8) ( (n) >>	16 );		\
+	(b)[(i)	+ 2] = (uint8) ( (n) >>	 8 );		\
+	(b)[(i)	+ 3] = (uint8) ( (n)	   );		\
+}
+
+/* AES key scheduling routine */
+
+int	rtmp_aes_set_key( aes_context *ctx, uint8 *key, int nbits )
+{
+	int	i;
+	uint32 *RK,	*SK;
+
+	switch(	nbits )
+	{
+		case 128: ctx->nr =	10;	break;
+		case 192: ctx->nr =	12;	break;
+		case 256: ctx->nr =	14;	break;
+		default	: return( 1	);
+	}
+
+	RK = ctx->erk;
+
+	for( i = 0;	i <	(nbits >> 5); i++ )
+	{
+		GET_UINT32(	RK[i], key,	i *	4 );
+	}
+
+	/* setup encryption	round keys */
+
+	switch(	nbits )
+	{
+	case 128:
+
+		for( i = 0;	i <	10;	i++, RK	+= 4 )
+		{
+			RK[4]  = RK[0] ^ RCON[i] ^
+						( FSb[ (uint8) ( RK[3] >> 16 ) ] <<	24 ) ^
+						( FSb[ (uint8) ( RK[3] >>  8 ) ] <<	16 ) ^
+						( FSb[ (uint8) ( RK[3]		 ) ] <<	 8 ) ^
+						( FSb[ (uint8) ( RK[3] >> 24 ) ]	   );
+
+			RK[5]  = RK[1] ^ RK[4];
+			RK[6]  = RK[2] ^ RK[5];
+			RK[7]  = RK[3] ^ RK[6];
+		}
+		break;
+
+	case 192:
+
+		for( i = 0;	i <	8; i++,	RK += 6	)
+		{
+			RK[6]  = RK[0] ^ RCON[i] ^
+						( FSb[ (uint8) ( RK[5] >> 16 ) ] <<	24 ) ^
+						( FSb[ (uint8) ( RK[5] >>  8 ) ] <<	16 ) ^
+						( FSb[ (uint8) ( RK[5]		 ) ] <<	 8 ) ^
+						( FSb[ (uint8) ( RK[5] >> 24 ) ]	   );
+
+			RK[7]  = RK[1] ^ RK[6];
+			RK[8]  = RK[2] ^ RK[7];
+			RK[9]  = RK[3] ^ RK[8];
+			RK[10] = RK[4] ^ RK[9];
+			RK[11] = RK[5] ^ RK[10];
+		}
+		break;
+
+	case 256:
+
+		for( i = 0;	i <	7; i++,	RK += 8	)
+		{
+			RK[8]  = RK[0] ^ RCON[i] ^
+						( FSb[ (uint8) ( RK[7] >> 16 ) ] <<	24 ) ^
+						( FSb[ (uint8) ( RK[7] >>  8 ) ] <<	16 ) ^
+						( FSb[ (uint8) ( RK[7]		 ) ] <<	 8 ) ^
+						( FSb[ (uint8) ( RK[7] >> 24 ) ]	   );
+
+			RK[9]  = RK[1] ^ RK[8];
+			RK[10] = RK[2] ^ RK[9];
+			RK[11] = RK[3] ^ RK[10];
+
+			RK[12] = RK[4] ^
+						( FSb[ (uint8) ( RK[11]	>> 24 )	] << 24	) ^
+						( FSb[ (uint8) ( RK[11]	>> 16 )	] << 16	) ^
+						( FSb[ (uint8) ( RK[11]	>>	8 )	] <<  8	) ^
+						( FSb[ (uint8) ( RK[11]		  )	]		);
+
+			RK[13] = RK[5] ^ RK[12];
+			RK[14] = RK[6] ^ RK[13];
+			RK[15] = RK[7] ^ RK[14];
+		}
+		break;
+	}
+
+	/* setup decryption	round keys */
+
+	if(	KT_init	)
+	{
+		for( i = 0;	i <	256; i++ )
+		{
+			KT0[i] = RT0[ FSb[i] ];
+			KT1[i] = RT1[ FSb[i] ];
+			KT2[i] = RT2[ FSb[i] ];
+			KT3[i] = RT3[ FSb[i] ];
+		}
+
+		KT_init	= 0;
+	}
+
+	SK = ctx->drk;
+
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+
+	for( i = 1;	i <	ctx->nr; i++ )
+	{
+		RK -= 8;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+	}
+
+	RK -= 8;
+
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+
+	return(	0 );
+}
+
+/* AES 128-bit block encryption	routine	*/
+
+void rtmp_aes_encrypt(aes_context *ctx, uint8 input[16],	uint8 output[16] )
+{
+	uint32 *RK,	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3;
+
+	RK = ctx->erk;
+	GET_UINT32(	X0,	input,	0 ); X0	^= RK[0];
+	GET_UINT32(	X1,	input,	4 ); X1	^= RK[1];
+	GET_UINT32(	X2,	input,	8 ); X2	^= RK[2];
+	GET_UINT32(	X3,	input, 12 ); X3	^= RK[3];
+
+#define	AES_FROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3)		\
+{												\
+	RK += 4;									\
+												\
+	X0 = RK[0] ^ FT0[ (uint8) (	Y0 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y1 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y2 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y3		 ) ];	\
+												\
+	X1 = RK[1] ^ FT0[ (uint8) (	Y1 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y2 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y3 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y0		 ) ];	\
+												\
+	X2 = RK[2] ^ FT0[ (uint8) (	Y2 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y3 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y0 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y1		 ) ];	\
+												\
+	X3 = RK[3] ^ FT0[ (uint8) (	Y3 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y0 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y1 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y2		 ) ];	\
+}
+
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 1 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 2 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 3 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 4 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 5 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 6 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 7 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 8 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 9 */
+
+	if(	ctx->nr	> 10 )
+	{
+		AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 10	*/
+		AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 11	*/
+	}
+
+	if(	ctx->nr	> 12 )
+	{
+		AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 12	*/
+		AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 13	*/
+	}
+
+	/* last	round */
+
+	RK += 4;
+
+	X0 = RK[0] ^ ( FSb[	(uint8)	( Y0 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y1 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y2 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y3	   ) ]		 );
+
+	X1 = RK[1] ^ ( FSb[	(uint8)	( Y1 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y2 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y3 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y0	   ) ]		 );
+
+	X2 = RK[2] ^ ( FSb[	(uint8)	( Y2 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y3 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y0 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y1	   ) ]		 );
+
+	X3 = RK[3] ^ ( FSb[	(uint8)	( Y3 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y0 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y1 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y2	   ) ]		 );
+
+	PUT_UINT32(	X0,	output,	 0 );
+	PUT_UINT32(	X1,	output,	 4 );
+	PUT_UINT32(	X2,	output,	 8 );
+	PUT_UINT32(	X3,	output,	12 );
+}
+
+/* AES 128-bit block decryption	routine	*/
+
+void rtmp_aes_decrypt( aes_context *ctx,	uint8 input[16], uint8 output[16] )
+{
+	uint32 *RK,	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3;
+
+	RK = ctx->drk;
+
+	GET_UINT32(	X0,	input,	0 ); X0	^= RK[0];
+	GET_UINT32(	X1,	input,	4 ); X1	^= RK[1];
+	GET_UINT32(	X2,	input,	8 ); X2	^= RK[2];
+	GET_UINT32(	X3,	input, 12 ); X3	^= RK[3];
+
+#define	AES_RROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3)		\
+{												\
+	RK += 4;									\
+												\
+	X0 = RK[0] ^ RT0[ (uint8) (	Y0 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y3 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y2 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y1		 ) ];	\
+												\
+	X1 = RK[1] ^ RT0[ (uint8) (	Y1 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y0 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y3 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y2		 ) ];	\
+												\
+	X2 = RK[2] ^ RT0[ (uint8) (	Y2 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y1 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y0 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y3		 ) ];	\
+												\
+	X3 = RK[3] ^ RT0[ (uint8) (	Y3 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y2 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y1 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y0		 ) ];	\
+}
+
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 1 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 2 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 3 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 4 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 5 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 6 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 7 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 8 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 9 */
+
+	if(	ctx->nr	> 10 )
+	{
+		AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 10	*/
+		AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 11	*/
+	}
+
+	if(	ctx->nr	> 12 )
+	{
+		AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 12	*/
+		AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 13	*/
+	}
+
+	/* last	round */
+
+	RK += 4;
+
+	X0 = RK[0] ^ ( RSb[	(uint8)	( Y0 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y3 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y2 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y1	   ) ]		 );
+
+	X1 = RK[1] ^ ( RSb[	(uint8)	( Y1 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y0 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y3 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y2	   ) ]		 );
+
+	X2 = RK[2] ^ ( RSb[	(uint8)	( Y2 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y1 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y0 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y3	   ) ]		 );
+
+	X3 = RK[3] ^ ( RSb[	(uint8)	( Y3 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y2 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y1 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y0	   ) ]		 );
+
+	PUT_UINT32(	X0,	output,	 0 );
+	PUT_UINT32(	X1,	output,	 4 );
+	PUT_UINT32(	X2,	output,	 8 );
+	PUT_UINT32(	X3,	output,	12 );
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		SHA1 function
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	HMAC_SHA1(
+	IN	UCHAR	*text,
+	IN	UINT	text_len,
+	IN	UCHAR	*key,
+	IN	UINT	key_len,
+	IN	UCHAR	*digest)
+{
+	SHA_CTX	context;
+	UCHAR	k_ipad[65]; /* inner padding - key XORd with ipad	*/
+	UCHAR	k_opad[65]; /* outer padding - key XORd with opad	*/
+	INT		i;
+
+	// if key is longer	than 64	bytes reset	it to key=SHA1(key)
+	if (key_len	> 64)
+	{
+		SHA_CTX		 tctx;
+		SHAInit(&tctx);
+		SHAUpdate(&tctx, key, key_len);
+		SHAFinal(&tctx,	key);
+		key_len	= 20;
+	}
+	NdisZeroMemory(k_ipad, sizeof(k_ipad));
+	NdisZeroMemory(k_opad, sizeof(k_opad));
+	NdisMoveMemory(k_ipad, key,	key_len);
+	NdisMoveMemory(k_opad, key,	key_len);
+
+	// XOR key with	ipad and opad values
+	for	(i = 0;	i <	64;	i++)
+	{
+		k_ipad[i] ^= 0x36;
+		k_opad[i] ^= 0x5c;
+	}
+
+	// perform inner SHA1
+	SHAInit(&context); 						/* init context for 1st pass */
+	SHAUpdate(&context,	k_ipad,	64);		/*	start with inner pad */
+	SHAUpdate(&context,	text, text_len);	/*	then text of datagram */
+	SHAFinal(&context, digest);				/* finish up 1st pass */
+
+	//perform outer	SHA1
+	SHAInit(&context);					/* init context for 2nd pass */
+	SHAUpdate(&context,	k_opad,	64);	/*	start with outer pad */
+	SHAUpdate(&context,	digest,	20);	/*	then results of	1st	hash */
+	SHAFinal(&context, digest);			/* finish up 2nd pass */
+
+}
+
+/*
+* F(P, S, c, i) = U1 xor U2 xor ... Uc
+* U1 = PRF(P, S || Int(i))
+* U2 = PRF(P, U1)
+* Uc = PRF(P, Uc-1)
+*/
+
+void F(char *password, unsigned char *ssid, int ssidlength, int iterations, int count, unsigned char *output)
+{
+    unsigned char digest[36], digest1[SHA_DIGEST_LEN];
+    int i, j;
+
+    /* U1 = PRF(P, S || int(i)) */
+    memcpy(digest, ssid, ssidlength);
+    digest[ssidlength] = (unsigned char)((count>>24) & 0xff);
+    digest[ssidlength+1] = (unsigned char)((count>>16) & 0xff);
+    digest[ssidlength+2] = (unsigned char)((count>>8) & 0xff);
+    digest[ssidlength+3] = (unsigned char)(count & 0xff);
+    HMAC_SHA1(digest, ssidlength+4, (unsigned char*) password, (int) strlen(password), digest1); // for WPA update
+
+    /* output = U1 */
+    memcpy(output, digest1, SHA_DIGEST_LEN);
+
+    for (i = 1; i < iterations; i++)
+    {
+        /* Un = PRF(P, Un-1) */
+        HMAC_SHA1(digest1, SHA_DIGEST_LEN, (unsigned char*) password, (int) strlen(password), digest); // for WPA update
+        memcpy(digest1, digest, SHA_DIGEST_LEN);
+
+        /* output = output xor Un */
+        for (j = 0; j < SHA_DIGEST_LEN; j++)
+        {
+            output[j] ^= digest[j];
+        }
+    }
+}
+/*
+* password - ascii string up to 63 characters in length
+* ssid - octet string up to 32 octets
+* ssidlength - length of ssid in octets
+* output must be 40 octets in length and outputs 256 bits of key
+*/
+int PasswordHash(char *password, unsigned char *ssid, int ssidlength, unsigned char *output)
+{
+    if ((strlen(password) > 63) || (ssidlength > 32))
+        return 0;
+
+    F(password, ssid, ssidlength, 4096, 1, output);
+    F(password, ssid, ssidlength, 4096, 2, &output[SHA_DIGEST_LEN]);
+    return 1;
+}
+
+
diff --git a/drivers/staging/rt2870/common/mlme.c b/drivers/staging/rt2870/common/mlme.c
new file mode 100644
index 000000000000..8a82cee8bf26
--- /dev/null
+++ b/drivers/staging/rt2870/common/mlme.c
@@ -0,0 +1,8609 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	mlme.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2004-08-25		Modify from RT2500 code base
+	John Chang	2004-09-06		modified for RT2600
+*/
+
+#include "../rt_config.h"
+#include <stdarg.h>
+
+UCHAR	CISCO_OUI[] = {0x00, 0x40, 0x96};
+
+UCHAR	WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
+UCHAR	RSN_OUI[] = {0x00, 0x0f, 0xac};
+UCHAR	WAPI_OUI[] = {0x00, 0x14, 0x72};
+UCHAR   WME_INFO_ELEM[]  = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+UCHAR   WME_PARM_ELEM[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+UCHAR	Ccx2QosInfo[] = {0x00, 0x40, 0x96, 0x04};
+UCHAR   RALINK_OUI[]  = {0x00, 0x0c, 0x43};
+UCHAR   BROADCOM_OUI[]  = {0x00, 0x90, 0x4c};
+UCHAR   WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+UCHAR	PRE_N_HT_OUI[]	= {0x00, 0x90, 0x4c};
+#endif // DOT11_N_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+UCHAR RateSwitchTable[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x11, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+    0x04, 0x21,  0, 30, 50,
+    0x05, 0x21,  1, 20, 50,
+    0x06, 0x21,  2, 20, 50,
+    0x07, 0x21,  3, 15, 50,
+    0x08, 0x21,  4, 15, 30,
+    0x09, 0x21,  5, 10, 25,
+    0x0a, 0x21,  6,  8, 25,
+    0x0b, 0x21,  7,  8, 25,
+    0x0c, 0x20, 12,  15, 30,
+    0x0d, 0x20, 13,  8, 20,
+    0x0e, 0x20, 14,  8, 20,
+    0x0f, 0x20, 15,  8, 25,
+    0x10, 0x22, 15,  8, 25,
+    0x11, 0x00,  0,  0,  0,
+    0x12, 0x00,  0,  0,  0,
+    0x13, 0x00,  0,  0,  0,
+    0x14, 0x00,  0,  0,  0,
+    0x15, 0x00,  0,  0,  0,
+    0x16, 0x00,  0,  0,  0,
+    0x17, 0x00,  0,  0,  0,
+    0x18, 0x00,  0,  0,  0,
+    0x19, 0x00,  0,  0,  0,
+    0x1a, 0x00,  0,  0,  0,
+    0x1b, 0x00,  0,  0,  0,
+    0x1c, 0x00,  0,  0,  0,
+    0x1d, 0x00,  0,  0,  0,
+    0x1e, 0x00,  0,  0,  0,
+    0x1f, 0x00,  0,  0,  0,
+};
+
+UCHAR RateSwitchTable11B[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x04, 0x03,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+};
+
+UCHAR RateSwitchTable11BG[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+    0x04, 0x10,  2, 20, 35,
+    0x05, 0x10,  3, 16, 35,
+    0x06, 0x10,  4, 10, 25,
+    0x07, 0x10,  5, 16, 25,
+    0x08, 0x10,  6, 10, 25,
+    0x09, 0x10,  7, 10, 13,
+};
+
+UCHAR RateSwitchTable11G[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x08, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x10,  0, 20, 101,
+    0x01, 0x10,  1, 20, 35,
+    0x02, 0x10,  2, 20, 35,
+    0x03, 0x10,  3, 16, 35,
+    0x04, 0x10,  4, 10, 25,
+    0x05, 0x10,  5, 16, 25,
+    0x06, 0x10,  6, 10, 25,
+    0x07, 0x10,  7, 10, 13,
+};
+
+#ifdef DOT11_N_SUPPORT
+UCHAR RateSwitchTable11N1S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x09, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 10, 25,
+    0x06, 0x21,  6,  8, 14,
+    0x07, 0x21,  7,  8, 14,
+    0x08, 0x23,  7,  8, 14,
+};
+
+UCHAR RateSwitchTable11N2S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,      // Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x20, 12,  15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11N3S[] = {
+// Item No.	Mode	Curr-MCS	TrainUp	TrainDown	// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,      // Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x20, 12,  15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11N2SForABand[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0b, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x20, 12,  15, 30,
+    0x07, 0x20, 13,  8, 20,
+    0x08, 0x20, 14,  8, 20,
+    0x09, 0x20, 15,  8, 25,
+    0x0a, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11N3SForABand[] = { // 3*3
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0b, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x20, 12,  15, 30,
+    0x07, 0x20, 13,  8, 20,
+    0x08, 0x20, 14,  8, 20,
+    0x09, 0x20, 15,  8, 25,
+    0x0a, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN1S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0d, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+    0x04, 0x21,  0, 30,101,	//50
+    0x05, 0x21,  1, 20, 50,
+    0x06, 0x21,  2, 20, 50,
+    0x07, 0x21,  3, 15, 50,
+    0x08, 0x21,  4, 15, 30,
+    0x09, 0x21,  5, 10, 25,
+    0x0a, 0x21,  6,  8, 14,
+    0x0b, 0x21,  7,  8, 14,
+	0x0c, 0x23,  7,  8, 14,
+};
+
+UCHAR RateSwitchTable11BGN2S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x20, 12, 15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN3S[] = { // 3*3
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 20, 50,
+    0x04, 0x21,  4, 15, 50,
+#if 1
+    0x05, 0x20, 20, 15, 30,
+    0x06, 0x20, 21,  8, 20,
+    0x07, 0x20, 22,  8, 20,
+    0x08, 0x20, 23,  8, 25,
+    0x09, 0x22, 23,  8, 25,
+#else // for RT2860 2*3 test
+    0x05, 0x20, 12, 15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+#endif
+};
+
+UCHAR RateSwitchTable11BGN2SForABand[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0b, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x20, 12, 15, 30,
+    0x07, 0x20, 13,  8, 20,
+    0x08, 0x20, 14,  8, 20,
+    0x09, 0x20, 15,  8, 25,
+    0x0a, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN3SForABand[] = { // 3*3
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0c, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x21, 12, 15, 30,
+    0x07, 0x20, 20, 15, 30,
+    0x08, 0x20, 21,  8, 20,
+    0x09, 0x20, 22,  8, 20,
+    0x0a, 0x20, 23,  8, 25,
+    0x0b, 0x22, 23,  8, 25,
+};
+#endif // DOT11_N_SUPPORT //
+
+PUCHAR ReasonString[] = {
+	/* 0  */	 "Reserved",
+	/* 1  */	 "Unspecified Reason",
+	/* 2  */	 "Previous Auth no longer valid",
+	/* 3  */	 "STA is leaving / has left",
+	/* 4  */	 "DIS-ASSOC due to inactivity",
+	/* 5  */	 "AP unable to hanle all associations",
+	/* 6  */	 "class 2 error",
+	/* 7  */	 "class 3 error",
+	/* 8  */	 "STA is leaving / has left",
+	/* 9  */	 "require auth before assoc/re-assoc",
+	/* 10 */	 "Reserved",
+	/* 11 */	 "Reserved",
+	/* 12 */	 "Reserved",
+	/* 13 */	 "invalid IE",
+	/* 14 */	 "MIC error",
+	/* 15 */	 "4-way handshake timeout",
+	/* 16 */	 "2-way (group key) handshake timeout",
+	/* 17 */	 "4-way handshake IE diff among AssosReq/Rsp/Beacon",
+	/* 18 */
+};
+
+extern UCHAR	 OfdmRateToRxwiMCS[];
+// since RT61 has better RX sensibility, we have to limit TX ACK rate not to exceed our normal data TX rate.
+// otherwise the WLAN peer may not be able to receive the ACK thus downgrade its data TX rate
+ULONG BasicRateMask[12]				= {0xfffff001 /* 1-Mbps */, 0xfffff003 /* 2 Mbps */, 0xfffff007 /* 5.5 */, 0xfffff00f /* 11 */,
+									  0xfffff01f /* 6 */	 , 0xfffff03f /* 9 */	  , 0xfffff07f /* 12 */ , 0xfffff0ff /* 18 */,
+									  0xfffff1ff /* 24 */	 , 0xfffff3ff /* 36 */	  , 0xfffff7ff /* 48 */ , 0xffffffff /* 54 */};
+
+UCHAR MULTICAST_ADDR[MAC_ADDR_LEN] = {0x1,  0x00, 0x00, 0x00, 0x00, 0x00};
+UCHAR BROADCAST_ADDR[MAC_ADDR_LEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+UCHAR ZERO_MAC_ADDR[MAC_ADDR_LEN]  = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+// e.g. RssiSafeLevelForTxRate[RATE_36]" means if the current RSSI is greater than
+//		this value, then it's quaranteed capable of operating in 36 mbps TX rate in
+//		clean environment.
+//								  TxRate: 1   2   5.5	11	 6	  9    12	18	 24   36   48	54	 72  100
+CHAR RssiSafeLevelForTxRate[] ={  -92, -91, -90, -87, -88, -86, -85, -83, -81, -78, -72, -71, -40, -40 };
+
+UCHAR  RateIdToMbps[]	 = { 1, 2, 5, 11, 6, 9, 12, 18, 24, 36, 48, 54, 72, 100};
+USHORT RateIdTo500Kbps[] = { 2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108, 144, 200};
+
+UCHAR  SsidIe	 = IE_SSID;
+UCHAR  SupRateIe = IE_SUPP_RATES;
+UCHAR  ExtRateIe = IE_EXT_SUPP_RATES;
+#ifdef DOT11_N_SUPPORT
+UCHAR  HtCapIe = IE_HT_CAP;
+UCHAR  AddHtInfoIe = IE_ADD_HT;
+UCHAR  NewExtChanIe = IE_SECONDARY_CH_OFFSET;
+#ifdef DOT11N_DRAFT3
+UCHAR  ExtHtCapIe = IE_EXT_CAPABILITY;
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+UCHAR  ErpIe	 = IE_ERP;
+UCHAR  DsIe 	 = IE_DS_PARM;
+UCHAR  TimIe	 = IE_TIM;
+UCHAR  WpaIe	 = IE_WPA;
+UCHAR  Wpa2Ie	 = IE_WPA2;
+UCHAR  IbssIe	 = IE_IBSS_PARM;
+UCHAR  Ccx2Ie	 = IE_CCX_V2;
+UCHAR  WapiIe	 = IE_WAPI;
+
+extern UCHAR	WPA_OUI[];
+
+UCHAR	SES_OUI[] = {0x00, 0x90, 0x4c};
+
+UCHAR	ZeroSsid[32] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+
+// Reset the RFIC setting to new series
+RTMP_RF_REGS RF2850RegTable[] = {
+//		ch	 R1 		 R2 		 R3(TX0~4=0) R4
+		{1,  0x98402ecc, 0x984c0786, 0x9816b455, 0x9800510b},
+		{2,  0x98402ecc, 0x984c0786, 0x98168a55, 0x9800519f},
+		{3,  0x98402ecc, 0x984c078a, 0x98168a55, 0x9800518b},
+		{4,  0x98402ecc, 0x984c078a, 0x98168a55, 0x9800519f},
+		{5,  0x98402ecc, 0x984c078e, 0x98168a55, 0x9800518b},
+		{6,  0x98402ecc, 0x984c078e, 0x98168a55, 0x9800519f},
+		{7,  0x98402ecc, 0x984c0792, 0x98168a55, 0x9800518b},
+		{8,  0x98402ecc, 0x984c0792, 0x98168a55, 0x9800519f},
+		{9,  0x98402ecc, 0x984c0796, 0x98168a55, 0x9800518b},
+		{10, 0x98402ecc, 0x984c0796, 0x98168a55, 0x9800519f},
+		{11, 0x98402ecc, 0x984c079a, 0x98168a55, 0x9800518b},
+		{12, 0x98402ecc, 0x984c079a, 0x98168a55, 0x9800519f},
+		{13, 0x98402ecc, 0x984c079e, 0x98168a55, 0x9800518b},
+		{14, 0x98402ecc, 0x984c07a2, 0x98168a55, 0x98005193},
+
+		// 802.11 UNI / HyperLan 2
+		{36, 0x98402ecc, 0x984c099a, 0x98158a55, 0x980ed1a3},
+		{38, 0x98402ecc, 0x984c099e, 0x98158a55, 0x980ed193},
+		{40, 0x98402ec8, 0x984c0682, 0x98158a55, 0x980ed183},
+		{44, 0x98402ec8, 0x984c0682, 0x98158a55, 0x980ed1a3},
+		{46, 0x98402ec8, 0x984c0686, 0x98158a55, 0x980ed18b},
+		{48, 0x98402ec8, 0x984c0686, 0x98158a55, 0x980ed19b},
+		{52, 0x98402ec8, 0x984c068a, 0x98158a55, 0x980ed193},
+		{54, 0x98402ec8, 0x984c068a, 0x98158a55, 0x980ed1a3},
+		{56, 0x98402ec8, 0x984c068e, 0x98158a55, 0x980ed18b},
+		{60, 0x98402ec8, 0x984c0692, 0x98158a55, 0x980ed183},
+		{62, 0x98402ec8, 0x984c0692, 0x98158a55, 0x980ed193},
+		{64, 0x98402ec8, 0x984c0692, 0x98158a55, 0x980ed1a3}, // Plugfest#4, Day4, change RFR3 left4th 9->5.
+
+		// 802.11 HyperLan 2
+		{100, 0x98402ec8, 0x984c06b2, 0x98178a55, 0x980ed783},
+
+		// 2008.04.30 modified
+		// The system team has AN to improve the EVM value
+		// for channel 102 to 108 for the RT2850/RT2750 dual band solution.
+		{102, 0x98402ec8, 0x985c06b2, 0x98578a55, 0x980ed793},
+		{104, 0x98402ec8, 0x985c06b2, 0x98578a55, 0x980ed1a3},
+		{108, 0x98402ecc, 0x985c0a32, 0x98578a55, 0x980ed193},
+
+		{110, 0x98402ecc, 0x984c0a36, 0x98178a55, 0x980ed183},
+		{112, 0x98402ecc, 0x984c0a36, 0x98178a55, 0x980ed19b},
+		{116, 0x98402ecc, 0x984c0a3a, 0x98178a55, 0x980ed1a3},
+		{118, 0x98402ecc, 0x984c0a3e, 0x98178a55, 0x980ed193},
+		{120, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed183},
+		{124, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed193},
+		{126, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed15b}, // 0x980ed1bb->0x980ed15b required by Rory 20070927
+		{128, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed1a3},
+		{132, 0x98402ec4, 0x984c0386, 0x98178a55, 0x980ed18b},
+		{134, 0x98402ec4, 0x984c0386, 0x98178a55, 0x980ed193},
+		{136, 0x98402ec4, 0x984c0386, 0x98178a55, 0x980ed19b},
+		{140, 0x98402ec4, 0x984c038a, 0x98178a55, 0x980ed183},
+
+		// 802.11 UNII
+		{149, 0x98402ec4, 0x984c038a, 0x98178a55, 0x980ed1a7},
+		{151, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed187},
+		{153, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed18f},
+		{157, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed19f},
+		{159, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed1a7},
+		{161, 0x98402ec4, 0x984c0392, 0x98178a55, 0x980ed187},
+		{165, 0x98402ec4, 0x984c0392, 0x98178a55, 0x980ed197},
+
+		// Japan
+		{184, 0x95002ccc, 0x9500491e, 0x9509be55, 0x950c0a0b},
+		{188, 0x95002ccc, 0x95004922, 0x9509be55, 0x950c0a13},
+		{192, 0x95002ccc, 0x95004926, 0x9509be55, 0x950c0a1b},
+		{196, 0x95002ccc, 0x9500492a, 0x9509be55, 0x950c0a23},
+		{208, 0x95002ccc, 0x9500493a, 0x9509be55, 0x950c0a13},
+		{212, 0x95002ccc, 0x9500493e, 0x9509be55, 0x950c0a1b},
+		{216, 0x95002ccc, 0x95004982, 0x9509be55, 0x950c0a23},
+
+		// still lack of MMAC(Japan) ch 34,38,42,46
+};
+UCHAR	NUM_OF_2850_CHNL = (sizeof(RF2850RegTable) / sizeof(RTMP_RF_REGS));
+
+FREQUENCY_ITEM FreqItems3020[] =
+{
+	/**************************************************/
+	// ISM : 2.4 to 2.483 GHz                         //
+	/**************************************************/
+	// 11g
+	/**************************************************/
+	//-CH---N-------R---K-----------
+	{1,    241,  2,  2},
+	{2,    241,	 2,  7},
+	{3,    242,	 2,  2},
+	{4,    242,	 2,  7},
+	{5,    243,	 2,  2},
+	{6,    243,	 2,  7},
+	{7,    244,	 2,  2},
+	{8,    244,	 2,  7},
+	{9,    245,	 2,  2},
+	{10,   245,	 2,  7},
+	{11,   246,	 2,  2},
+	{12,   246,	 2,  7},
+	{13,   247,	 2,  2},
+	{14,   248,	 2,  4},
+};
+#define	NUM_OF_3020_CHNL	(sizeof(FreqItems3020) / sizeof(FREQUENCY_ITEM))
+
+/*
+	==========================================================================
+	Description:
+		initialize the MLME task and its data structure (queue, spinlock,
+		timer, state machines).
+
+	IRQL = PASSIVE_LEVEL
+
+	Return:
+		always return NDIS_STATUS_SUCCESS
+
+	==========================================================================
+*/
+NDIS_STATUS MlmeInit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NDIS_STATUS Status = NDIS_STATUS_SUCCESS;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> MLME Initialize\n"));
+
+	do
+	{
+		Status = MlmeQueueInit(&pAd->Mlme.Queue);
+		if(Status != NDIS_STATUS_SUCCESS)
+			break;
+
+		pAd->Mlme.bRunning = FALSE;
+		NdisAllocateSpinLock(&pAd->Mlme.TaskLock);
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			BssTableInit(&pAd->ScanTab);
+
+			// init STA state machines
+			AssocStateMachineInit(pAd, &pAd->Mlme.AssocMachine, pAd->Mlme.AssocFunc);
+			AuthStateMachineInit(pAd, &pAd->Mlme.AuthMachine, pAd->Mlme.AuthFunc);
+			AuthRspStateMachineInit(pAd, &pAd->Mlme.AuthRspMachine, pAd->Mlme.AuthRspFunc);
+			SyncStateMachineInit(pAd, &pAd->Mlme.SyncMachine, pAd->Mlme.SyncFunc);
+			WpaPskStateMachineInit(pAd, &pAd->Mlme.WpaPskMachine, pAd->Mlme.WpaPskFunc);
+			AironetStateMachineInit(pAd, &pAd->Mlme.AironetMachine, pAd->Mlme.AironetFunc);
+
+#ifdef QOS_DLS_SUPPORT
+			DlsStateMachineInit(pAd, &pAd->Mlme.DlsMachine, pAd->Mlme.DlsFunc);
+#endif // QOS_DLS_SUPPORT //
+
+
+			// Since we are using switch/case to implement it, the init is different from the above
+			// state machine init
+			MlmeCntlInit(pAd, &pAd->Mlme.CntlMachine, NULL);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+		ActionStateMachineInit(pAd, &pAd->Mlme.ActMachine, pAd->Mlme.ActFunc);
+
+		// Init mlme periodic timer
+		RTMPInitTimer(pAd, &pAd->Mlme.PeriodicTimer, GET_TIMER_FUNCTION(MlmePeriodicExec), pAd, TRUE);
+
+		// Set mlme periodic timer
+		RTMPSetTimer(&pAd->Mlme.PeriodicTimer, MLME_TASK_EXEC_INTV);
+
+		// software-based RX Antenna diversity
+		RTMPInitTimer(pAd, &pAd->Mlme.RxAntEvalTimer, GET_TIMER_FUNCTION(AsicRxAntEvalTimeout), pAd, FALSE);
+
+	} while (FALSE);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- MLME Initialize\n"));
+
+	return Status;
+}
+
+/*
+	==========================================================================
+	Description:
+		main loop of the MLME
+	Pre:
+		Mlme has to be initialized, and there are something inside the queue
+	Note:
+		This function is invoked from MPSetInformation and MPReceive;
+		This task guarantee only one MlmeHandler will run.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeHandler(
+	IN PRTMP_ADAPTER pAd)
+{
+	MLME_QUEUE_ELEM 	   *Elem = NULL;
+#ifdef APCLI_SUPPORT
+	SHORT apcliIfIndex;
+#endif
+
+	// Only accept MLME and Frame from peer side, no other (control/data) frame should
+	// get into this state machine
+
+	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
+	if(pAd->Mlme.bRunning)
+	{
+		NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+		return;
+	}
+	else
+	{
+		pAd->Mlme.bRunning = TRUE;
+	}
+	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+
+	while (!MlmeQueueEmpty(&pAd->Mlme.Queue))
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS) ||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS) ||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("Device Halted or Removed or MlmeRest, exit MlmeHandler! (queue num = %ld)\n", pAd->Mlme.Queue.Num));
+			break;
+		}
+
+#ifdef RALINK_ATE
+		if(ATE_ON(pAd))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now in MlmeHandler\n"));
+			break;
+		}
+#endif // RALINK_ATE //
+
+		//From message type, determine which state machine I should drive
+		if (MlmeDequeue(&pAd->Mlme.Queue, &Elem))
+		{
+#ifdef RT2870
+			if (Elem->MsgType == MT2_RESET_CONF)
+			{
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("!!! reset MLME state machine !!!\n"));
+				MlmeRestartStateMachine(pAd);
+				Elem->Occupied = FALSE;
+				Elem->MsgLen = 0;
+				continue;
+			}
+#endif // RT2870 //
+
+			// if dequeue success
+			switch (Elem->Machine)
+			{
+				// STA state machines
+#ifdef	CONFIG_STA_SUPPORT
+				case ASSOC_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AssocMachine, Elem);
+					break;
+				case AUTH_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AuthMachine, Elem);
+					break;
+				case AUTH_RSP_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AuthRspMachine, Elem);
+					break;
+				case SYNC_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.SyncMachine, Elem);
+					break;
+				case MLME_CNTL_STATE_MACHINE:
+					MlmeCntlMachinePerformAction(pAd, &pAd->Mlme.CntlMachine, Elem);
+					break;
+				case WPA_PSK_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.WpaPskMachine, Elem);
+					break;
+#ifdef LEAP_SUPPORT
+				case LEAP_STATE_MACHINE:
+					LeapMachinePerformAction(pAd, &pAd->Mlme.LeapMachine, Elem);
+					break;
+#endif
+				case AIRONET_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AironetMachine, Elem);
+					break;
+
+#ifdef QOS_DLS_SUPPORT
+				case DLS_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.DlsMachine, Elem);
+					break;
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+				case ACTION_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.ActMachine, Elem);
+					break;
+
+
+
+
+				default:
+					DBGPRINT(RT_DEBUG_TRACE, ("ERROR: Illegal machine %ld in MlmeHandler()\n", Elem->Machine));
+					break;
+			} // end of switch
+
+			// free MLME element
+			Elem->Occupied = FALSE;
+			Elem->MsgLen = 0;
+
+		}
+		else {
+			DBGPRINT_ERR(("MlmeHandler: MlmeQueue empty\n"));
+		}
+	}
+
+	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
+	pAd->Mlme.bRunning = FALSE;
+	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+}
+
+/*
+	==========================================================================
+	Description:
+		Destructor of MLME (Destroy queue, state machine, spin lock and timer)
+	Parameters:
+		Adapter - NIC Adapter pointer
+	Post:
+		The MLME task will no longer work properly
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeHalt(
+	IN PRTMP_ADAPTER pAd)
+{
+	BOOLEAN 	  Cancelled;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeHalt\n"));
+
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		// disable BEACON generation and other BEACON related hardware timers
+		AsicDisableSync(pAd);
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef QOS_DLS_SUPPORT
+		UCHAR		i;
+#endif // QOS_DLS_SUPPORT //
+		// Cancel pending timers
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,	&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,		&Cancelled);
+
+#ifdef QOS_DLS_SUPPORT
+		for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &Cancelled);
+		}
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMPCancelTimer(&pAd->Mlme.PeriodicTimer,		&Cancelled);
+	RTMPCancelTimer(&pAd->Mlme.RxAntEvalTimer,		&Cancelled);
+
+
+
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		// Set LED
+		RTMPSetLED(pAd, LED_HALT);
+        RTMPSetSignalLED(pAd, -100);	// Force signal strength Led to be turned off, firmware is not done it.
+#ifdef RT2870
+        {
+            LED_CFG_STRUC LedCfg;
+            RTMP_IO_READ32(pAd, LED_CFG, &LedCfg.word);
+            LedCfg.field.LedPolar = 0;
+            LedCfg.field.RLedMode = 0;
+            LedCfg.field.GLedMode = 0;
+            LedCfg.field.YLedMode = 0;
+            RTMP_IO_WRITE32(pAd, LED_CFG, LedCfg.word);
+        }
+#endif // RT2870 //
+	}
+
+	RTMPusecDelay(5000);    //  5 msec to gurantee Ant Diversity timer canceled
+
+	MlmeQueueDestroy(&pAd->Mlme.Queue);
+	NdisFreeSpinLock(&pAd->Mlme.TaskLock);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== MlmeHalt\n"));
+}
+
+VOID MlmeResetRalinkCounters(
+	IN  PRTMP_ADAPTER   pAd)
+{
+	pAd->RalinkCounters.LastOneSecRxOkDataCnt = pAd->RalinkCounters.OneSecRxOkDataCnt;
+	// clear all OneSecxxx counters.
+	pAd->RalinkCounters.OneSecBeaconSentCnt = 0;
+	pAd->RalinkCounters.OneSecFalseCCACnt = 0;
+	pAd->RalinkCounters.OneSecRxFcsErrCnt = 0;
+	pAd->RalinkCounters.OneSecRxOkCnt = 0;
+	pAd->RalinkCounters.OneSecTxFailCount = 0;
+	pAd->RalinkCounters.OneSecTxNoRetryOkCount = 0;
+	pAd->RalinkCounters.OneSecTxRetryOkCount = 0;
+	pAd->RalinkCounters.OneSecRxOkDataCnt = 0;
+
+	// TODO: for debug only. to be removed
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_BE] = 0;
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_BK] = 0;
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_VI] = 0;
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_VO] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_BE] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_BK] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_VI] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_VO] = 0;
+	pAd->RalinkCounters.OneSecTxDoneCount = 0;
+	pAd->RalinkCounters.OneSecRxCount = 0;
+	pAd->RalinkCounters.OneSecTxAggregationCount = 0;
+	pAd->RalinkCounters.OneSecRxAggregationCount = 0;
+
+	return;
+}
+
+unsigned long rx_AMSDU;
+unsigned long rx_Total;
+
+/*
+	==========================================================================
+	Description:
+		This routine is executed periodically to -
+		1. Decide if it's a right time to turn on PwrMgmt bit of all
+		   outgoiing frames
+		2. Calculate ChannelQuality based on statistics of the last
+		   period, so that TX rate won't toggling very frequently between a
+		   successful TX and a failed TX.
+		3. If the calculated ChannelQuality indicated current connection not
+		   healthy, then a ROAMing attempt is tried here.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+#define ADHOC_BEACON_LOST_TIME		(8*OS_HZ)  // 8 sec
+VOID MlmePeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	ULONG			TxTotalCnt;
+	PRTMP_ADAPTER	pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_RADIO_OFF |
+								fRTMP_ADAPTER_RADIO_MEASUREMENT |
+								fRTMP_ADAPTER_RESET_IN_PROGRESS))))
+		return;
+
+	RT28XX_MLME_PRE_SANITY_CHECK(pAd);
+
+#ifdef RALINK_ATE
+	/* Do not show RSSI until "Normal 1 second Mlme PeriodicExec". */
+	if (ATE_ON(pAd))
+	{
+		if (pAd->Mlme.PeriodicRound % MLME_TASK_EXEC_MULTIPLE != (MLME_TASK_EXEC_MULTIPLE - 1))
+	{
+			pAd->Mlme.PeriodicRound ++;
+			return;
+		}
+	}
+#endif // RALINK_ATE //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Do nothing if monitor mode is on
+		if (MONITOR_ON(pAd))
+			return;
+
+		if (pAd->Mlme.PeriodicRound & 0x1)
+		{
+			// This is the fix for wifi 11n extension channel overlapping test case.  for 2860D
+			if (((pAd->MACVersion & 0xffff) == 0x0101) &&
+				(STA_TGN_WIFI_ON(pAd)) &&
+				(pAd->CommonCfg.IOTestParm.bToggle == FALSE))
+
+				{
+					RTMP_IO_WRITE32(pAd, TXOP_CTRL_CFG, 0x24Bf);
+					pAd->CommonCfg.IOTestParm.bToggle = TRUE;
+				}
+				else if ((STA_TGN_WIFI_ON(pAd)) &&
+						((pAd->MACVersion & 0xffff) == 0x0101))
+				{
+					RTMP_IO_WRITE32(pAd, TXOP_CTRL_CFG, 0x243f);
+					pAd->CommonCfg.IOTestParm.bToggle = FALSE;
+				}
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	pAd->bUpdateBcnCntDone = FALSE;
+
+//	RECBATimerTimeout(SystemSpecific1,FunctionContext,SystemSpecific2,SystemSpecific3);
+	pAd->Mlme.PeriodicRound ++;
+
+	// execute every 500ms
+	if ((pAd->Mlme.PeriodicRound % 5 == 0) && RTMPAutoRateSwitchCheck(pAd)/*(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED))*/)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		// perform dynamic tx rate switching based on past TX history
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if ((OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+					)
+				&& (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE)))
+				MlmeDynamicTxRateSwitching(pAd);
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// Normal 1 second Mlme PeriodicExec.
+	if (pAd->Mlme.PeriodicRound %MLME_TASK_EXEC_MULTIPLE == 0)
+	{
+                pAd->Mlme.OneSecPeriodicRound ++;
+
+#ifdef RALINK_ATE
+    	if (ATE_ON(pAd))
+    	{
+			/* request from Baron : move this routine from later to here */
+			/* for showing Rx error count in ATE RXFRAME */
+            NICUpdateRawCounters(pAd);
+			if (pAd->ate.bRxFer == 1)
+			{
+				pAd->ate.RxTotalCnt += pAd->ate.RxCntPerSec;
+			    ate_print(KERN_EMERG "MlmePeriodicExec: Rx packet cnt = %d/%d\n", pAd->ate.RxCntPerSec, pAd->ate.RxTotalCnt);
+				pAd->ate.RxCntPerSec = 0;
+
+				if (pAd->ate.RxAntennaSel == 0)
+					ate_print(KERN_EMERG "MlmePeriodicExec: Rx AvgRssi0=%d, AvgRssi1=%d, AvgRssi2=%d\n\n",
+						pAd->ate.AvgRssi0, pAd->ate.AvgRssi1, pAd->ate.AvgRssi2);
+				else
+					ate_print(KERN_EMERG "MlmePeriodicExec: Rx AvgRssi=%d\n\n", pAd->ate.AvgRssi0);
+			}
+			MlmeResetRalinkCounters(pAd);
+			return;
+    	}
+#endif // RALINK_ATE //
+
+
+		if (rx_Total)
+		{
+
+			// reset counters
+			rx_AMSDU = 0;
+			rx_Total = 0;
+		}
+
+		//ORIBATimerTimeout(pAd);
+
+		// Media status changed, report to NDIS
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_CHANGE))
+		{
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_CHANGE);
+			if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+			{
+				pAd->IndicateMediaState = NdisMediaStateConnected;
+				RTMP_IndicateMediaState(pAd);
+
+			}
+			else
+			{
+				pAd->IndicateMediaState = NdisMediaStateDisconnected;
+				RTMP_IndicateMediaState(pAd);
+			}
+		}
+
+		NdisGetSystemUpTime(&pAd->Mlme.Now32);
+
+		// add the most up-to-date h/w raw counters into software variable, so that
+		// the dynamic tuning mechanism below are based on most up-to-date information
+		NICUpdateRawCounters(pAd);
+
+#ifdef RT2870
+		RT2870_WatchDog(pAd);
+#endif // RT2870 //
+
+#ifdef DOT11_N_SUPPORT
+   		// Need statistics after read counter. So put after NICUpdateRawCounters
+		ORIBATimerTimeout(pAd);
+#endif // DOT11_N_SUPPORT //
+
+		// if MGMT RING is full more than twice within 1 second, we consider there's
+		// a hardware problem stucking the TX path. In this case, try a hardware reset
+		// to recover the system
+	//	if (pAd->RalinkCounters.MgmtRingFullCount >= 2)
+	//		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HARDWARE_ERROR);
+	//	else
+	//		pAd->RalinkCounters.MgmtRingFullCount = 0;
+
+		// The time period for checking antenna is according to traffic
+		if (pAd->Mlme.bEnableAutoAntennaCheck)
+		{
+			TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+							 pAd->RalinkCounters.OneSecTxRetryOkCount +
+							 pAd->RalinkCounters.OneSecTxFailCount;
+
+			if (TxTotalCnt > 50)
+			{
+				if (pAd->Mlme.OneSecPeriodicRound % 10 == 0)
+				{
+					AsicEvaluateRxAnt(pAd);
+				}
+			}
+			else
+			{
+				if (pAd->Mlme.OneSecPeriodicRound % 3 == 0)
+				{
+					AsicEvaluateRxAnt(pAd);
+				}
+			}
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			STAMlmePeriodicExec(pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+		MlmeResetRalinkCounters(pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			{
+				// When Adhoc beacon is enabled and RTS/CTS is enabled, there is a chance that hardware MAC FSM will run into a deadlock
+				// and sending CTS-to-self over and over.
+				// Software Patch Solution:
+				// 1. Polling debug state register 0x10F4 every one second.
+				// 2. If in 0x10F4 the ((bit29==1) && (bit7==1)) OR ((bit29==1) && (bit5==1)), it means the deadlock has occurred.
+				// 3. If the deadlock occurred, reset MAC/BBP by setting 0x1004 to 0x0001 for a while then setting it back to 0x000C again.
+
+				UINT32	MacReg = 0;
+
+				RTMP_IO_READ32(pAd, 0x10F4, &MacReg);
+				if (((MacReg & 0x20000000) && (MacReg & 0x80)) || ((MacReg & 0x20000000) && (MacReg & 0x20)))
+				{
+					RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x1);
+					RTMPusecDelay(1);
+					RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0xC);
+
+					DBGPRINT(RT_DEBUG_WARN,("Warning, MAC specific condition occurs \n"));
+				}
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		RT28XX_MLME_HANDLER(pAd);
+	}
+
+
+	pAd->bUpdateBcnCntDone = FALSE;
+}
+
+#ifdef CONFIG_STA_SUPPORT
+VOID STAMlmePeriodicExec(
+	PRTMP_ADAPTER pAd)
+{
+	ULONG	TxTotalCnt;
+	int 	i;
+
+//
+// We return here in ATE mode, because the statistics
+// that ATE needs are not collected via this routine.
+//
+#ifdef RALINK_ATE
+	// It is supposed that we will never reach here in ATE mode.
+	ASSERT(!(ATE_ON(pAd)));
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    if (pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_DISABLE)
+#endif // WPA_SUPPLICANT_SUPPORT //
+    {
+    	// WPA MIC error should block association attempt for 60 seconds
+    	if (pAd->StaCfg.bBlockAssoc && (pAd->StaCfg.LastMicErrorTime + (60 * OS_HZ) < pAd->Mlme.Now32))
+    		pAd->StaCfg.bBlockAssoc = FALSE;
+    }
+
+    if ((pAd->PreMediaState != pAd->IndicateMediaState) && (pAd->CommonCfg.bWirelessEvent))
+	{
+		if (pAd->IndicateMediaState == NdisMediaStateConnected)
+		{
+			RTMPSendWirelessEvent(pAd, IW_STA_LINKUP_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+		pAd->PreMediaState = pAd->IndicateMediaState;
+	}
+
+
+
+
+   	AsicStaBbpTuning(pAd);
+
+	TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+					 pAd->RalinkCounters.OneSecTxRetryOkCount +
+					 pAd->RalinkCounters.OneSecTxFailCount;
+
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		// update channel quality for Roaming and UI LinkQuality display
+		MlmeCalculateChannelQuality(pAd, pAd->Mlme.Now32);
+	}
+
+	// must be AFTER MlmeDynamicTxRateSwitching() because it needs to know if
+	// Radio is currently in noisy environment
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		AsicAdjustTxPower(pAd);
+
+	if (INFRA_ON(pAd))
+	{
+#ifdef QOS_DLS_SUPPORT
+		// Check DLS time out, then tear down those session
+		RTMPCheckDLSTimeOut(pAd);
+#endif // QOS_DLS_SUPPORT //
+
+		// Is PSM bit consistent with user power management policy?
+		// This is the only place that will set PSM bit ON.
+		if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+		MlmeCheckPsmChange(pAd, pAd->Mlme.Now32);
+
+		pAd->RalinkCounters.LastOneSecTotalTxCount = TxTotalCnt;
+
+		if ((pAd->StaCfg.LastBeaconRxTime + 1*OS_HZ < pAd->Mlme.Now32) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) &&
+			((TxTotalCnt + pAd->RalinkCounters.OneSecRxOkCnt < 600)))
+		{
+			RTMPSetAGCInitValue(pAd, BW_20);
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - No BEACON. restore R66 to the low bound(%d) \n", (0x2E + GET_LNA_GAIN(pAd))));
+		}
+
+        //if ((pAd->RalinkCounters.OneSecTxNoRetryOkCount == 0) &&
+        //    (pAd->RalinkCounters.OneSecTxRetryOkCount == 0))
+        {
+    		if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable)
+    		{
+    		    // When APSD is enabled, the period changes as 20 sec
+    			if ((pAd->Mlme.OneSecPeriodicRound % 20) == 8)
+    				RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+    		}
+    		else
+    		{
+    		    // Send out a NULL frame every 10 sec to inform AP that STA is still alive (Avoid being age out)
+    			if ((pAd->Mlme.OneSecPeriodicRound % 10) == 8)
+                {
+                    if (pAd->CommonCfg.bWmmCapable)
+    					RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+                    else
+						RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, FALSE);
+                }
+    		}
+        }
+
+		if (CQI_IS_DEAD(pAd->Mlme.ChannelQuality))
+			{
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - No BEACON. Dead CQI. Auto Recovery attempt #%ld\n", pAd->RalinkCounters.BadCQIAutoRecoveryCount));
+			pAd->StaCfg.CCXAdjacentAPReportFlag = TRUE;
+			pAd->StaCfg.CCXAdjacentAPLinkDownTime = pAd->StaCfg.LastBeaconRxTime;
+
+			// Lost AP, send disconnect & link down event
+			LinkDown(pAd, FALSE);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.WpaSupplicantUP)
+			{
+                union iwreq_data    wrqu;
+                //send disassociate event to wpa_supplicant
+                memset(&wrqu, 0, sizeof(wrqu));
+                wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+                wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+			// RTMPPatchMacBbpBug(pAd);
+			MlmeAutoReconnectLastSSID(pAd);
+		}
+		else if (CQI_IS_BAD(pAd->Mlme.ChannelQuality))
+		{
+			pAd->RalinkCounters.BadCQIAutoRecoveryCount ++;
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Bad CQI. Auto Recovery attempt #%ld\n", pAd->RalinkCounters.BadCQIAutoRecoveryCount));
+			MlmeAutoReconnectLastSSID(pAd);
+		}
+
+		// Add auto seamless roaming
+		if (pAd->StaCfg.bFastRoaming)
+		{
+			SHORT	dBmToRoam = (SHORT)pAd->StaCfg.dBmToRoam;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("Rssi=%d, dBmToRoam=%d\n", RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2), (CHAR)dBmToRoam));
+
+			if (RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2) <= (CHAR)dBmToRoam)
+			{
+				MlmeCheckForFastRoaming(pAd, pAd->Mlme.Now32);
+			}
+		}
+	}
+	else if (ADHOC_ON(pAd))
+	{
+		//radar detect
+		if ((pAd->CommonCfg.Channel > 14)
+			&& (pAd->CommonCfg.bIEEE80211H == 1)
+			&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+		{
+			RadarDetectPeriodic(pAd);
+		}
+
+		// If all peers leave, and this STA becomes the last one in this IBSS, then change MediaState
+		// to DISCONNECTED. But still holding this IBSS (i.e. sending BEACON) so that other STAs can
+		// join later.
+		if ((pAd->StaCfg.LastBeaconRxTime + ADHOC_BEACON_LOST_TIME < pAd->Mlme.Now32) &&
+			OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+		{
+			MLME_START_REQ_STRUCT     StartReq;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - excessive BEACON lost, last STA in this IBSS, MediaState=Disconnected\n"));
+			LinkDown(pAd, FALSE);
+
+			StartParmFill(pAd, &StartReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_START_REQ, sizeof(MLME_START_REQ_STRUCT), &StartReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_START;
+		}
+
+		for (i = 1; i < MAX_LEN_OF_MAC_TABLE; i++)
+		{
+			MAC_TABLE_ENTRY *pEntry = &pAd->MacTab.Content[i];
+
+			if (pEntry->ValidAsCLI == FALSE)
+				continue;
+
+			if (pEntry->LastBeaconRxTime + ADHOC_BEACON_LOST_TIME < pAd->Mlme.Now32)
+				MacTableDeleteEntry(pAd, pEntry->Aid, pEntry->Addr);
+		}
+	}
+	else // no INFRA nor ADHOC connection
+	{
+
+		if (pAd->StaCfg.bScanReqIsFromWebUI &&
+            ((pAd->StaCfg.LastScanTime + 30 * OS_HZ) > pAd->Mlme.Now32))
+			goto SKIP_AUTO_SCAN_CONN;
+        else
+            pAd->StaCfg.bScanReqIsFromWebUI = FALSE;
+
+		if ((pAd->StaCfg.bAutoReconnect == TRUE)
+			&& RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP)
+			&& (MlmeValidateSSID(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen) == TRUE))
+		{
+			if ((pAd->ScanTab.BssNr==0) && (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE))
+			{
+				MLME_SCAN_REQ_STRUCT	   ScanReq;
+
+				if ((pAd->StaCfg.LastScanTime + 10 * OS_HZ) < pAd->Mlme.Now32)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("STAMlmePeriodicExec():CNTL - ScanTab.BssNr==0, start a new ACTIVE scan SSID[%s]\n", pAd->MlmeAux.AutoReconnectSsid));
+					ScanParmFill(pAd, &ScanReq, pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen, BSS_ANY, SCAN_ACTIVE);
+					MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+					pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+					// Reset Missed scan number
+					pAd->StaCfg.LastScanTime = pAd->Mlme.Now32;
+				}
+				else if (pAd->StaCfg.BssType == BSS_ADHOC)	// Quit the forever scan when in a very clean room
+					MlmeAutoReconnectLastSSID(pAd);
+			}
+			else if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+			{
+				if ((pAd->Mlme.OneSecPeriodicRound % 7) == 0)
+				{
+					MlmeAutoScan(pAd);
+					pAd->StaCfg.LastScanTime = pAd->Mlme.Now32;
+				}
+				else
+				{
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+					if (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+					{
+						if ((pAd->Mlme.OneSecPeriodicRound % 5) == 1)
+							MlmeAutoReconnectLastSSID(pAd);
+					}
+					else
+#endif // CARRIER_DETECTION_SUPPORT //
+						MlmeAutoReconnectLastSSID(pAd);
+				}
+			}
+		}
+	}
+
+SKIP_AUTO_SCAN_CONN:
+
+#ifdef DOT11_N_SUPPORT
+    if ((pAd->MacTab.Content[BSSID_WCID].TXBAbitmap !=0) && (pAd->MacTab.fAnyBASession == FALSE))
+	{
+		pAd->MacTab.fAnyBASession = TRUE;
+		AsicUpdateProtect(pAd, HT_FORCERTSCTS,  ALLN_SETPROTECT, FALSE, FALSE);
+	}
+	else if ((pAd->MacTab.Content[BSSID_WCID].TXBAbitmap ==0) && (pAd->MacTab.fAnyBASession == TRUE))
+	{
+		pAd->MacTab.fAnyBASession = FALSE;
+		AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode,  ALLN_SETPROTECT, FALSE, FALSE);
+	}
+#endif // DOT11_N_SUPPORT //
+
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SCAN_2040))
+		TriEventCounterMaintenance(pAd);
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+	return;
+}
+
+// Link down report
+VOID LinkDownExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	pAd->IndicateMediaState = NdisMediaStateDisconnected;
+	RTMP_IndicateMediaState(pAd);
+    pAd->ExtraInfo = GENERAL_LINK_DOWN;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeAutoScan(
+	IN PRTMP_ADAPTER pAd)
+{
+	// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+	if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Driver auto scan\n"));
+		MlmeEnqueue(pAd,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_BSSID_LIST_SCAN,
+					0,
+					NULL);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeAutoReconnectLastSSID(
+	IN PRTMP_ADAPTER pAd)
+{
+
+
+	// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+	if ((pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE) &&
+		(MlmeValidateSSID(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen) == TRUE))
+	{
+		NDIS_802_11_SSID OidSsid;
+		OidSsid.SsidLength = pAd->MlmeAux.AutoReconnectSsidLen;
+		NdisMoveMemory(OidSsid.Ssid, pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Driver auto reconnect to last OID_802_11_SSID setting - %s, len - %d\n", pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen));
+		MlmeEnqueue(pAd,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_SSID,
+					sizeof(NDIS_802_11_SSID),
+					&OidSsid);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	==========================================================================
+	Validate SSID for connection try and rescan purpose
+	Valid SSID will have visible chars only.
+	The valid length is from 0 to 32.
+	IRQL = DISPATCH_LEVEL
+	==========================================================================
+ */
+BOOLEAN MlmeValidateSSID(
+	IN PUCHAR	pSsid,
+	IN UCHAR	SsidLen)
+{
+	int	index;
+
+	if (SsidLen > MAX_LEN_OF_SSID)
+		return (FALSE);
+
+	// Check each character value
+	for (index = 0; index < SsidLen; index++)
+	{
+		if (pSsid[index] < 0x20)
+			return (FALSE);
+	}
+
+	// All checked
+	return (TRUE);
+}
+
+VOID MlmeSelectTxRateTable(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PUCHAR				*ppTable,
+	IN PUCHAR				pTableSize,
+	IN PUCHAR				pInitTxRateIdx)
+{
+	do
+	{
+		// decide the rate table for tuning
+		if (pAd->CommonCfg.TxRateTableSize > 0)
+		{
+			*ppTable = RateSwitchTable;
+			*pTableSize = RateSwitchTable[0];
+			*pInitTxRateIdx = RateSwitchTable[1];
+
+			break;
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		if ((pAd->OpMode == OPMODE_STA) && ADHOC_ON(pAd))
+		{
+#ifdef DOT11_N_SUPPORT
+			if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED) &&
+				(pEntry->HTCapability.MCSSet[0] == 0xff) &&
+				((pEntry->HTCapability.MCSSet[1] == 0x00) || (pAd->Antenna.field.TxPath == 1)))
+			{// 11N 1S Adhoc
+				*ppTable = RateSwitchTable11N1S;
+				*pTableSize = RateSwitchTable11N1S[0];
+				*pInitTxRateIdx = RateSwitchTable11N1S[1];
+
+			}
+			else if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED) &&
+					(pEntry->HTCapability.MCSSet[0] == 0xff) &&
+					(pEntry->HTCapability.MCSSet[1] == 0xff) &&
+					(pAd->Antenna.field.TxPath == 2))
+			{// 11N 2S Adhoc
+				if (pAd->LatchRfRegs.Channel <= 14)
+				{
+					*ppTable = RateSwitchTable11N2S;
+					*pTableSize = RateSwitchTable11N2S[0];
+					*pInitTxRateIdx = RateSwitchTable11N2S[1];
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11N2SForABand;
+					*pTableSize = RateSwitchTable11N2SForABand[0];
+					*pInitTxRateIdx = RateSwitchTable11N2SForABand[1];
+				}
+
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+				if ((pEntry->RateLen == 4)
+#ifdef DOT11_N_SUPPORT
+					&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+					)
+			{
+				*ppTable = RateSwitchTable11B;
+				*pTableSize = RateSwitchTable11B[0];
+				*pInitTxRateIdx = RateSwitchTable11B[1];
+
+			}
+			else if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				*ppTable = RateSwitchTable11BG;
+				*pTableSize = RateSwitchTable11BG[0];
+				*pInitTxRateIdx = RateSwitchTable11BG[1];
+
+			}
+			else
+			{
+				*ppTable = RateSwitchTable11G;
+				*pTableSize = RateSwitchTable11G[0];
+				*pInitTxRateIdx = RateSwitchTable11G[1];
+
+			}
+			break;
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+		//if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen == 12) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) &&
+		//	((pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0x00) || (pAd->Antenna.field.TxPath == 1)))
+		if ((pEntry->RateLen == 12) && (pEntry->HTCapability.MCSSet[0] == 0xff) &&
+			((pEntry->HTCapability.MCSSet[1] == 0x00) || (pAd->CommonCfg.TxStream == 1)))
+		{// 11BGN 1S AP
+			*ppTable = RateSwitchTable11BGN1S;
+			*pTableSize = RateSwitchTable11BGN1S[0];
+			*pInitTxRateIdx = RateSwitchTable11BGN1S[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen == 12) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) &&
+		//	(pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0xff) && (pAd->Antenna.field.TxPath == 2))
+		if ((pEntry->RateLen == 12) && (pEntry->HTCapability.MCSSet[0] == 0xff) &&
+			(pEntry->HTCapability.MCSSet[1] == 0xff) && (pAd->CommonCfg.TxStream == 2))
+		{// 11BGN 2S AP
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				*ppTable = RateSwitchTable11BGN2S;
+				*pTableSize = RateSwitchTable11BGN2S[0];
+				*pInitTxRateIdx = RateSwitchTable11BGN2S[1];
+
+			}
+			else
+			{
+				*ppTable = RateSwitchTable11BGN2SForABand;
+				*pTableSize = RateSwitchTable11BGN2SForABand[0];
+				*pInitTxRateIdx = RateSwitchTable11BGN2SForABand[1];
+
+			}
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) && ((pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0x00) || (pAd->Antenna.field.TxPath == 1)))
+		if ((pEntry->HTCapability.MCSSet[0] == 0xff) && ((pEntry->HTCapability.MCSSet[1] == 0x00) || (pAd->CommonCfg.TxStream == 1)))
+		{// 11N 1S AP
+			*ppTable = RateSwitchTable11N1S;
+			*pTableSize = RateSwitchTable11N1S[0];
+			*pInitTxRateIdx = RateSwitchTable11N1S[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0xff) && (pAd->Antenna.field.TxPath == 2))
+		if ((pEntry->HTCapability.MCSSet[0] == 0xff) && (pEntry->HTCapability.MCSSet[1] == 0xff) && (pAd->CommonCfg.TxStream == 2))
+		{// 11N 2S AP
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+			*ppTable = RateSwitchTable11N2S;
+			*pTableSize = RateSwitchTable11N2S[0];
+			*pInitTxRateIdx = RateSwitchTable11N2S[1];
+            }
+			else
+			{
+				*ppTable = RateSwitchTable11N2SForABand;
+				*pTableSize = RateSwitchTable11N2SForABand[0];
+				*pInitTxRateIdx = RateSwitchTable11N2SForABand[1];
+			}
+
+			break;
+		}
+#endif // DOT11_N_SUPPORT //
+		//else if ((pAd->StaActive.SupRateLen == 4) && (pAd->StaActive.ExtRateLen == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+		if ((pEntry->RateLen == 4)
+#ifdef DOT11_N_SUPPORT
+			&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+			)
+		{// B only AP
+			*ppTable = RateSwitchTable11B;
+			*pTableSize = RateSwitchTable11B[0];
+			*pInitTxRateIdx = RateSwitchTable11B[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen > 8) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+		if ((pEntry->RateLen > 8)
+#ifdef DOT11_N_SUPPORT
+			&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+			)
+		{// B/G  mixed AP
+			*ppTable = RateSwitchTable11BG;
+			*pTableSize = RateSwitchTable11BG[0];
+			*pInitTxRateIdx = RateSwitchTable11BG[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen == 8) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+		if ((pEntry->RateLen == 8)
+#ifdef DOT11_N_SUPPORT
+			&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+			)
+		{// G only AP
+			*ppTable = RateSwitchTable11G;
+			*pTableSize = RateSwitchTable11G[0];
+			*pInitTxRateIdx = RateSwitchTable11G[1];
+
+			break;
+		}
+#ifdef DOT11_N_SUPPORT
+#endif // DOT11_N_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef DOT11_N_SUPPORT
+			//else if ((pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+			if ((pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0))
+#endif // DOT11_N_SUPPORT //
+			{	// Legacy mode
+				if (pAd->CommonCfg.MaxTxRate <= RATE_11)
+				{
+					*ppTable = RateSwitchTable11B;
+					*pTableSize = RateSwitchTable11B[0];
+					*pInitTxRateIdx = RateSwitchTable11B[1];
+				}
+				else if ((pAd->CommonCfg.MaxTxRate > RATE_11) && (pAd->CommonCfg.MinTxRate > RATE_11))
+				{
+					*ppTable = RateSwitchTable11G;
+					*pTableSize = RateSwitchTable11G[0];
+					*pInitTxRateIdx = RateSwitchTable11G[1];
+
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11BG;
+					*pTableSize = RateSwitchTable11BG[0];
+					*pInitTxRateIdx = RateSwitchTable11BG[1];
+				}
+				break;
+			}
+#ifdef DOT11_N_SUPPORT
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				if (pAd->CommonCfg.TxStream == 1)
+				{
+					*ppTable = RateSwitchTable11N1S;
+					*pTableSize = RateSwitchTable11N1S[0];
+					*pInitTxRateIdx = RateSwitchTable11N1S[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 1S AP \n"));
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11N2S;
+					*pTableSize = RateSwitchTable11N2S[0];
+					*pInitTxRateIdx = RateSwitchTable11N2S[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 2S AP \n"));
+				}
+			}
+			else
+			{
+				if (pAd->CommonCfg.TxStream == 1)
+				{
+					*ppTable = RateSwitchTable11N1S;
+					*pTableSize = RateSwitchTable11N1S[0];
+					*pInitTxRateIdx = RateSwitchTable11N1S[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 1S AP \n"));
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11N2SForABand;
+					*pTableSize = RateSwitchTable11N2SForABand[0];
+					*pInitTxRateIdx = RateSwitchTable11N2SForABand[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 2S AP \n"));
+				}
+			}
+#endif // DOT11_N_SUPPORT //
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode (SupRateLen=%d, ExtRateLen=%d, MCSSet[0]=0x%x, MCSSet[1]=0x%x)\n",
+				pAd->StaActive.SupRateLen, pAd->StaActive.ExtRateLen, pAd->StaActive.SupportedPhyInfo.MCSSet[0], pAd->StaActive.SupportedPhyInfo.MCSSet[1]));
+		}
+#endif // CONFIG_STA_SUPPORT //
+	} while(FALSE);
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	==========================================================================
+	Description:
+		This routine checks if there're other APs out there capable for
+		roaming. Caller should call this routine only when Link up in INFRA mode
+		and channel quality is below CQI_GOOD_THRESHOLD.
+
+	IRQL = DISPATCH_LEVEL
+
+	Output:
+	==========================================================================
+ */
+VOID MlmeCheckForRoaming(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG	Now32)
+{
+	USHORT	   i;
+	BSS_TABLE  *pRoamTab = &pAd->MlmeAux.RoamTab;
+	BSS_ENTRY  *pBss;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeCheckForRoaming\n"));
+	// put all roaming candidates into RoamTab, and sort in RSSI order
+	BssTableInit(pRoamTab);
+	for (i = 0; i < pAd->ScanTab.BssNr; i++)
+	{
+		pBss = &pAd->ScanTab.BssEntry[i];
+
+		if ((pBss->LastBeaconRxTime + BEACON_LOST_TIME) < Now32)
+			continue;	 // AP disappear
+		if (pBss->Rssi <= RSSI_THRESHOLD_FOR_ROAMING)
+			continue;	 // RSSI too weak. forget it.
+		if (MAC_ADDR_EQUAL(pBss->Bssid, pAd->CommonCfg.Bssid))
+			continue;	 // skip current AP
+		if (pBss->Rssi < (pAd->StaCfg.RssiSample.LastRssi0 + RSSI_DELTA))
+			continue;	 // only AP with stronger RSSI is eligible for roaming
+
+		// AP passing all above rules is put into roaming candidate table
+		NdisMoveMemory(&pRoamTab->BssEntry[pRoamTab->BssNr], pBss, sizeof(BSS_ENTRY));
+		pRoamTab->BssNr += 1;
+	}
+
+	if (pRoamTab->BssNr > 0)
+	{
+		// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+		if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+		{
+			pAd->RalinkCounters.PoorCQIRoamingCount ++;
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Roaming attempt #%ld\n", pAd->RalinkCounters.PoorCQIRoamingCount));
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_MLME_ROAMING_REQ, 0, NULL);
+			RT28XX_MLME_HANDLER(pAd);
+		}
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("<== MlmeCheckForRoaming(# of candidate= %d)\n",pRoamTab->BssNr));
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine checks if there're other APs out there capable for
+		roaming. Caller should call this routine only when link up in INFRA mode
+		and channel quality is below CQI_GOOD_THRESHOLD.
+
+	IRQL = DISPATCH_LEVEL
+
+	Output:
+	==========================================================================
+ */
+VOID MlmeCheckForFastRoaming(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ULONG			Now)
+{
+	USHORT		i;
+	BSS_TABLE	*pRoamTab = &pAd->MlmeAux.RoamTab;
+	BSS_ENTRY	*pBss;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeCheckForFastRoaming\n"));
+	// put all roaming candidates into RoamTab, and sort in RSSI order
+	BssTableInit(pRoamTab);
+	for (i = 0; i < pAd->ScanTab.BssNr; i++)
+	{
+		pBss = &pAd->ScanTab.BssEntry[i];
+
+        if ((pBss->Rssi <= -50) && (pBss->Channel == pAd->CommonCfg.Channel))
+			continue;	 // RSSI too weak. forget it.
+		if (MAC_ADDR_EQUAL(pBss->Bssid, pAd->CommonCfg.Bssid))
+			continue;	 // skip current AP
+		if (!SSID_EQUAL(pBss->Ssid, pBss->SsidLen, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen))
+			continue;	 // skip different SSID
+        if (pBss->Rssi < (RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2) + RSSI_DELTA))
+			continue;	 // skip AP without better RSSI
+
+        DBGPRINT(RT_DEBUG_TRACE, ("LastRssi0 = %d, pBss->Rssi = %d\n", RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2), pBss->Rssi));
+		// AP passing all above rules is put into roaming candidate table
+		NdisMoveMemory(&pRoamTab->BssEntry[pRoamTab->BssNr], pBss, sizeof(BSS_ENTRY));
+		pRoamTab->BssNr += 1;
+	}
+
+	if (pRoamTab->BssNr > 0)
+	{
+		// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+		if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+		{
+			pAd->RalinkCounters.PoorCQIRoamingCount ++;
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Roaming attempt #%ld\n", pAd->RalinkCounters.PoorCQIRoamingCount));
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_MLME_ROAMING_REQ, 0, NULL);
+			RT28XX_MLME_HANDLER(pAd);
+		}
+	}
+	// Maybe site survey required
+	else
+	{
+		if ((pAd->StaCfg.LastScanTime + 10 * 1000) < Now)
+		{
+			// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Roaming, No eligable entry, try new scan!\n"));
+			pAd->StaCfg.ScanCnt = 2;
+			pAd->StaCfg.LastScanTime = Now;
+			MlmeAutoScan(pAd);
+		}
+	}
+
+    DBGPRINT(RT_DEBUG_TRACE, ("<== MlmeCheckForFastRoaming (BssNr=%d)\n", pRoamTab->BssNr));
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine calculates TxPER, RxPER of the past N-sec period. And
+		according to the calculation result, ChannelQuality is calculated here
+		to decide if current AP is still doing the job.
+
+		If ChannelQuality is not good, a ROAMing attempt may be tried later.
+	Output:
+		StaCfg.ChannelQuality - 0..100
+
+	IRQL = DISPATCH_LEVEL
+
+	NOTE: This routine decide channle quality based on RX CRC error ratio.
+		Caller should make sure a function call to NICUpdateRawCounters(pAd)
+		is performed right before this routine, so that this routine can decide
+		channel quality based on the most up-to-date information
+	==========================================================================
+ */
+VOID MlmeCalculateChannelQuality(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Now32)
+{
+	ULONG TxOkCnt, TxCnt, TxPER, TxPRR;
+	ULONG RxCnt, RxPER;
+	UCHAR NorRssi;
+	CHAR  MaxRssi;
+	ULONG BeaconLostTime = BEACON_LOST_TIME;
+
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+	// longer beacon lost time when carrier detection enabled
+	if (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+	{
+		BeaconLostTime = BEACON_LOST_TIME + BEACON_LOST_TIME/2;
+	}
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	MaxRssi = RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2);
+
+	//
+	// calculate TX packet error ratio and TX retry ratio - if too few TX samples, skip TX related statistics
+	//
+	TxOkCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount + pAd->RalinkCounters.OneSecTxRetryOkCount;
+	TxCnt = TxOkCnt + pAd->RalinkCounters.OneSecTxFailCount;
+	if (TxCnt < 5)
+	{
+		TxPER = 0;
+		TxPRR = 0;
+	}
+	else
+	{
+		TxPER = (pAd->RalinkCounters.OneSecTxFailCount * 100) / TxCnt;
+		TxPRR = ((TxCnt - pAd->RalinkCounters.OneSecTxNoRetryOkCount) * 100) / TxCnt;
+	}
+
+	//
+	// calculate RX PER - don't take RxPER into consideration if too few sample
+	//
+	RxCnt = pAd->RalinkCounters.OneSecRxOkCnt + pAd->RalinkCounters.OneSecRxFcsErrCnt;
+	if (RxCnt < 5)
+		RxPER = 0;
+	else
+		RxPER = (pAd->RalinkCounters.OneSecRxFcsErrCnt * 100) / RxCnt;
+
+	//
+	// decide ChannelQuality based on: 1)last BEACON received time, 2)last RSSI, 3)TxPER, and 4)RxPER
+	//
+	if (INFRA_ON(pAd) &&
+		(pAd->RalinkCounters.OneSecTxNoRetryOkCount < 2) && // no heavy traffic
+		(pAd->StaCfg.LastBeaconRxTime + BeaconLostTime < Now32))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("BEACON lost > %ld msec with TxOkCnt=%ld -> CQI=0\n", BeaconLostTime, TxOkCnt));
+		pAd->Mlme.ChannelQuality = 0;
+	}
+	else
+	{
+		// Normalize Rssi
+		if (MaxRssi > -40)
+			NorRssi = 100;
+		else if (MaxRssi < -90)
+			NorRssi = 0;
+		else
+			NorRssi = (MaxRssi + 90) * 2;
+
+		// ChannelQuality = W1*RSSI + W2*TxPRR + W3*RxPER	 (RSSI 0..100), (TxPER 100..0), (RxPER 100..0)
+		pAd->Mlme.ChannelQuality = (RSSI_WEIGHTING * NorRssi +
+								   TX_WEIGHTING * (100 - TxPRR) +
+								   RX_WEIGHTING* (100 - RxPER)) / 100;
+		if (pAd->Mlme.ChannelQuality >= 100)
+			pAd->Mlme.ChannelQuality = 100;
+	}
+
+}
+
+VOID MlmeSetTxRate(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PRTMP_TX_RATE_SWITCH	pTxRate)
+{
+	UCHAR	MaxMode = MODE_OFDM;
+
+#ifdef DOT11_N_SUPPORT
+	MaxMode = MODE_HTGREENFIELD;
+
+	if (pTxRate->STBC && (pAd->StaCfg.MaxHTPhyMode.field.STBC) && (pAd->Antenna.field.TxPath == 2))
+		pAd->StaCfg.HTPhyMode.field.STBC = STBC_USE;
+	else
+#endif // DOT11_N_SUPPORT //
+		pAd->StaCfg.HTPhyMode.field.STBC = STBC_NONE;
+
+	if (pTxRate->CurrMCS < MCS_AUTO)
+		pAd->StaCfg.HTPhyMode.field.MCS = pTxRate->CurrMCS;
+
+	if (pAd->StaCfg.HTPhyMode.field.MCS > 7)
+		pAd->StaCfg.HTPhyMode.field.STBC = STBC_NONE;
+
+   	if (ADHOC_ON(pAd))
+	{
+		// If peer adhoc is b-only mode, we can't send 11g rate.
+		pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+		pEntry->HTPhyMode.field.STBC	= STBC_NONE;
+
+		//
+		// For Adhoc MODE_CCK, driver will use AdhocBOnlyJoined flag to roll back to B only if necessary
+		//
+		pEntry->HTPhyMode.field.MODE	= pTxRate->Mode;
+		pEntry->HTPhyMode.field.ShortGI	= pAd->StaCfg.HTPhyMode.field.ShortGI;
+		pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+
+		// Patch speed error in status page
+		pAd->StaCfg.HTPhyMode.field.MODE = pEntry->HTPhyMode.field.MODE;
+	}
+	else
+	{
+		if (pTxRate->Mode <= MaxMode)
+			pAd->StaCfg.HTPhyMode.field.MODE = pTxRate->Mode;
+
+#ifdef DOT11_N_SUPPORT
+		if (pTxRate->ShortGI && (pAd->StaCfg.MaxHTPhyMode.field.ShortGI))
+			pAd->StaCfg.HTPhyMode.field.ShortGI = GI_400;
+		else
+#endif // DOT11_N_SUPPORT //
+			pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+
+#ifdef DOT11_N_SUPPORT
+		// Reexam each bandwidth's SGI support.
+		if (pAd->StaCfg.HTPhyMode.field.ShortGI == GI_400)
+		{
+			if ((pEntry->HTPhyMode.field.BW == BW_20) && (!CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE)))
+				pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+			if ((pEntry->HTPhyMode.field.BW == BW_40) && (!CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE)))
+				pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+		}
+
+		// Turn RTS/CTS rate to 6Mbps.
+		if ((pEntry->HTPhyMode.field.MCS == 0) && (pAd->StaCfg.HTPhyMode.field.MCS != 0))
+		{
+			pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+			if (pAd->MacTab.fAnyBASession)
+			{
+				AsicUpdateProtect(pAd, HT_FORCERTSCTS, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+			else
+			{
+				AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+		}
+		else if ((pEntry->HTPhyMode.field.MCS == 8) && (pAd->StaCfg.HTPhyMode.field.MCS != 8))
+		{
+			pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+			if (pAd->MacTab.fAnyBASession)
+			{
+				AsicUpdateProtect(pAd, HT_FORCERTSCTS, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+			else
+			{
+				AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+		}
+		else if ((pEntry->HTPhyMode.field.MCS != 0) && (pAd->StaCfg.HTPhyMode.field.MCS == 0))
+		{
+			AsicUpdateProtect(pAd, HT_RTSCTS_6M, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+
+		}
+		else if ((pEntry->HTPhyMode.field.MCS != 8) && (pAd->StaCfg.HTPhyMode.field.MCS == 8))
+		{
+			AsicUpdateProtect(pAd, HT_RTSCTS_6M, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+		}
+#endif // DOT11_N_SUPPORT //
+
+		pEntry->HTPhyMode.field.STBC	= pAd->StaCfg.HTPhyMode.field.STBC;
+		pEntry->HTPhyMode.field.ShortGI	= pAd->StaCfg.HTPhyMode.field.ShortGI;
+		pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+		pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+#ifdef DOT11_N_SUPPORT
+		if ((pAd->StaCfg.MaxHTPhyMode.field.MODE == MODE_HTGREENFIELD) &&
+		    pAd->WIFItestbed.bGreenField)
+		    pEntry->HTPhyMode.field.MODE = MODE_HTGREENFIELD;
+#endif // DOT11_N_SUPPORT //
+	}
+
+	pAd->LastTxRate = (USHORT)(pEntry->HTPhyMode.word);
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine calculates the acumulated TxPER of eaxh TxRate. And
+		according to the calculation result, change CommonCfg.TxRate which
+		is the stable TX Rate we expect the Radio situation could sustained.
+
+		CommonCfg.TxRate will change dynamically within {RATE_1/RATE_6, MaxTxRate}
+	Output:
+		CommonCfg.TxRate -
+
+	IRQL = DISPATCH_LEVEL
+
+	NOTE:
+		call this routine every second
+	==========================================================================
+ */
+VOID MlmeDynamicTxRateSwitching(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR					UpRateIdx = 0, DownRateIdx = 0, CurrRateIdx;
+	ULONG					i, AccuTxTotalCnt = 0, TxTotalCnt;
+	ULONG					TxErrorRatio = 0;
+	BOOLEAN					bTxRateChanged, bUpgradeQuality = FALSE;
+	PRTMP_TX_RATE_SWITCH	pCurrTxRate, pNextTxRate = NULL;
+	PUCHAR					pTable;
+	UCHAR					TableSize = 0;
+	UCHAR					InitTxRateIdx = 0, TrainUp, TrainDown;
+	CHAR					Rssi, RssiOffset = 0;
+	TX_STA_CNT1_STRUC		StaTx1;
+	TX_STA_CNT0_STRUC		TxStaCnt0;
+	ULONG					TxRetransmit = 0, TxSuccess = 0, TxFailCount = 0;
+	MAC_TABLE_ENTRY			*pEntry;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		return;
+	}
+#endif // RALINK_ATE //
+
+	//
+	// walk through MAC table, see if need to change AP's TX rate toward each entry
+	//
+   	for (i = 1; i < MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		pEntry = &pAd->MacTab.Content[i];
+
+		// check if this entry need to switch rate automatically
+		if (RTMPCheckEntryEnableAutoRateSwitch(pAd, pEntry) == FALSE)
+			continue;
+
+		if ((pAd->MacTab.Size == 1) || (pEntry->ValidAsDls))
+		{
+			Rssi = RTMPMaxRssi(pAd,
+							   pAd->StaCfg.RssiSample.AvgRssi0,
+							   pAd->StaCfg.RssiSample.AvgRssi1,
+							   pAd->StaCfg.RssiSample.AvgRssi2);
+
+			// Update statistic counter
+			RTMP_IO_READ32(pAd, TX_STA_CNT0, &TxStaCnt0.word);
+			RTMP_IO_READ32(pAd, TX_STA_CNT1, &StaTx1.word);
+			pAd->bUpdateBcnCntDone = TRUE;
+			TxRetransmit = StaTx1.field.TxRetransmit;
+			TxSuccess = StaTx1.field.TxSuccess;
+			TxFailCount = TxStaCnt0.field.TxFailCount;
+			TxTotalCnt = TxRetransmit + TxSuccess + TxFailCount;
+
+			pAd->RalinkCounters.OneSecTxRetryOkCount += StaTx1.field.TxRetransmit;
+			pAd->RalinkCounters.OneSecTxNoRetryOkCount += StaTx1.field.TxSuccess;
+			pAd->RalinkCounters.OneSecTxFailCount += TxStaCnt0.field.TxFailCount;
+			pAd->WlanCounters.TransmittedFragmentCount.u.LowPart += StaTx1.field.TxSuccess;
+			pAd->WlanCounters.RetryCount.u.LowPart += StaTx1.field.TxRetransmit;
+			pAd->WlanCounters.FailedCount.u.LowPart += TxStaCnt0.field.TxFailCount;
+
+			// if no traffic in the past 1-sec period, don't change TX rate,
+			// but clear all bad history. because the bad history may affect the next
+			// Chariot throughput test
+			AccuTxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((TxRetransmit + TxFailCount) * 100) / TxTotalCnt;
+		}
+		else
+		{
+			if (INFRA_ON(pAd) && (i == 1))
+				Rssi = RTMPMaxRssi(pAd,
+								   pAd->StaCfg.RssiSample.AvgRssi0,
+								   pAd->StaCfg.RssiSample.AvgRssi1,
+								   pAd->StaCfg.RssiSample.AvgRssi2);
+			else
+				Rssi = RTMPMaxRssi(pAd,
+								   pEntry->RssiSample.AvgRssi0,
+								   pEntry->RssiSample.AvgRssi1,
+								   pEntry->RssiSample.AvgRssi2);
+
+			TxTotalCnt = pEntry->OneSecTxNoRetryOkCount +
+				 pEntry->OneSecTxRetryOkCount +
+				 pEntry->OneSecTxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((pEntry->OneSecTxRetryOkCount + pEntry->OneSecTxFailCount) * 100) / TxTotalCnt;
+		}
+
+		CurrRateIdx = pEntry->CurrTxRateIndex;
+
+		MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &InitTxRateIdx);
+
+		if (CurrRateIdx >= TableSize)
+		{
+			CurrRateIdx = TableSize - 1;
+		}
+
+		// When switch from Fixed rate -> auto rate, the REAL TX rate might be different from pAd->CommonCfg.TxRateIndex.
+		// So need to sync here.
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(CurrRateIdx+1)*5];
+		if ((pEntry->HTPhyMode.field.MCS != pCurrTxRate->CurrMCS)
+			//&& (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+			)
+		{
+
+			// Need to sync Real Tx rate and our record.
+			// Then return for next DRS.
+			pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(InitTxRateIdx+1)*5];
+			pEntry->CurrTxRateIndex = InitTxRateIdx;
+			MlmeSetTxRate(pAd, pEntry, pCurrTxRate);
+
+			// reset all OneSecTx counters
+			RESET_ONE_SEC_TX_CNT(pEntry);
+			continue;
+		}
+
+		// decide the next upgrade rate and downgrade rate, if any
+		if ((CurrRateIdx > 0) && (CurrRateIdx < (TableSize - 1)))
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx -1;
+		}
+		else if (CurrRateIdx == 0)
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx;
+		}
+		else if (CurrRateIdx == (TableSize - 1))
+		{
+			UpRateIdx = CurrRateIdx;
+			DownRateIdx = CurrRateIdx - 1;
+		}
+
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(CurrRateIdx+1)*5];
+
+#ifdef DOT11_N_SUPPORT
+		if ((Rssi > -65) && (pCurrTxRate->Mode >= MODE_HTMIX))
+		{
+			TrainUp		= (pCurrTxRate->TrainUp + (pCurrTxRate->TrainUp >> 1));
+			TrainDown	= (pCurrTxRate->TrainDown + (pCurrTxRate->TrainDown >> 1));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			TrainUp		= pCurrTxRate->TrainUp;
+			TrainDown	= pCurrTxRate->TrainDown;
+		}
+
+		//pAd->DrsCounters.LastTimeTxRateChangeAction = pAd->DrsCounters.LastSecTxRateChangeAction;
+
+		//
+		// Keep the last time TxRateChangeAction status.
+		//
+		pEntry->LastTimeTxRateChangeAction = pEntry->LastSecTxRateChangeAction;
+
+
+
+		//
+		// CASE 1. when TX samples are fewer than 15, then decide TX rate solely on RSSI
+		//         (criteria copied from RT2500 for Netopia case)
+		//
+		if (TxTotalCnt <= 15)
+		{
+			CHAR	idx = 0;
+			UCHAR	TxRateIdx;
+			//UCHAR	MCS0 = 0, MCS1 = 0, MCS2 = 0, MCS3 = 0, MCS4 = 0, MCS7 = 0, MCS12 = 0, MCS13 = 0, MCS14 = 0, MCS15 = 0;
+			UCHAR	MCS0 = 0, MCS1 = 0, MCS2 = 0, MCS3 = 0, MCS4 = 0,  MCS5 =0, MCS6 = 0, MCS7 = 0;
+	        UCHAR	MCS12 = 0, MCS13 = 0, MCS14 = 0, MCS15 = 0;
+			UCHAR	MCS20 = 0, MCS21 = 0, MCS22 = 0, MCS23 = 0; // 3*3
+
+			// check the existence and index of each needed MCS
+			while (idx < pTable[0])
+			{
+				pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(idx+1)*5];
+
+				if (pCurrTxRate->CurrMCS == MCS_0)
+				{
+					MCS0 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_1)
+				{
+					MCS1 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_2)
+				{
+					MCS2 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_3)
+				{
+					MCS3 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_4)
+				{
+					MCS4 = idx;
+				}
+	            else if (pCurrTxRate->CurrMCS == MCS_5)
+	            {
+	                MCS5 = idx;
+	            }
+	            else if (pCurrTxRate->CurrMCS == MCS_6)
+	            {
+	                MCS6 = idx;
+	            }
+				//else if (pCurrTxRate->CurrMCS == MCS_7)
+				else if ((pCurrTxRate->CurrMCS == MCS_7) && (pCurrTxRate->ShortGI == GI_800))	// prevent the highest MCS using short GI when 1T and low throughput
+				{
+					MCS7 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_12)
+				{
+					MCS12 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_13)
+				{
+					MCS13 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_14)
+				{
+					MCS14 = idx;
+				}
+				//else if ((pCurrTxRate->CurrMCS == MCS_15)/* && (pCurrTxRate->ShortGI == GI_800)*/)	//we hope to use ShortGI as initial rate
+				else if ((pCurrTxRate->CurrMCS == MCS_15) && (pCurrTxRate->ShortGI == GI_800))	//we hope to use ShortGI as initial rate, however Atheros's chip has bugs when short GI
+				{
+					MCS15 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_20) // 3*3
+				{
+					MCS20 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_21)
+				{
+					MCS21 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_22)
+				{
+					MCS22 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_23)
+				{
+					MCS23 = idx;
+				}
+				idx ++;
+			}
+
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				if (pAd->NicConfig2.field.ExternalLNAForG)
+				{
+					RssiOffset = 2;
+				}
+				else
+				{
+					RssiOffset = 5;
+				}
+			}
+			else
+			{
+				if (pAd->NicConfig2.field.ExternalLNAForA)
+				{
+					RssiOffset = 5;
+				}
+				else
+				{
+					RssiOffset = 8;
+				}
+			}
+#ifdef DOT11_N_SUPPORT
+			/*if (MCS15)*/
+			if ((pTable == RateSwitchTable11BGN3S) ||
+				(pTable == RateSwitchTable11N3S) ||
+				(pTable == RateSwitchTable))
+			{// N mode with 3 stream // 3*3
+				if (MCS23 && (Rssi >= -70))
+					TxRateIdx = MCS15;
+				else if (MCS22 && (Rssi >= -72))
+					TxRateIdx = MCS14;
+        	    else if (MCS21 && (Rssi >= -76))
+					TxRateIdx = MCS13;
+				else if (MCS20 && (Rssi >= -78))
+					TxRateIdx = MCS12;
+			else if (MCS4 && (Rssi >= -82))
+				TxRateIdx = MCS4;
+			else if (MCS3 && (Rssi >= -84))
+				TxRateIdx = MCS3;
+			else if (MCS2 && (Rssi >= -86))
+				TxRateIdx = MCS2;
+			else if (MCS1 && (Rssi >= -88))
+				TxRateIdx = MCS1;
+			else
+				TxRateIdx = MCS0;
+		}
+//		else if ((pTable == RateSwitchTable11BGN2S) || (pTable == RateSwitchTable11BGN2SForABand) ||(pTable == RateSwitchTable11N2S) ||(pTable == RateSwitchTable11N2SForABand) || (pTable == RateSwitchTable))
+		else if ((pTable == RateSwitchTable11BGN2S) || (pTable == RateSwitchTable11BGN2SForABand) ||(pTable == RateSwitchTable11N2S) ||(pTable == RateSwitchTable11N2SForABand)) // 3*3
+			{// N mode with 2 stream
+				if (MCS15 && (Rssi >= (-70+RssiOffset)))
+					TxRateIdx = MCS15;
+				else if (MCS14 && (Rssi >= (-72+RssiOffset)))
+					TxRateIdx = MCS14;
+				else if (MCS13 && (Rssi >= (-76+RssiOffset)))
+					TxRateIdx = MCS13;
+				else if (MCS12 && (Rssi >= (-78+RssiOffset)))
+					TxRateIdx = MCS12;
+				else if (MCS4 && (Rssi >= (-82+RssiOffset)))
+					TxRateIdx = MCS4;
+				else if (MCS3 && (Rssi >= (-84+RssiOffset)))
+					TxRateIdx = MCS3;
+				else if (MCS2 && (Rssi >= (-86+RssiOffset)))
+					TxRateIdx = MCS2;
+				else if (MCS1 && (Rssi >= (-88+RssiOffset)))
+					TxRateIdx = MCS1;
+				else
+					TxRateIdx = MCS0;
+			}
+			else if ((pTable == RateSwitchTable11BGN1S) || (pTable == RateSwitchTable11N1S))
+			{// N mode with 1 stream
+				if (MCS7 && (Rssi > (-72+RssiOffset)))
+					TxRateIdx = MCS7;
+				else if (MCS6 && (Rssi > (-74+RssiOffset)))
+					TxRateIdx = MCS6;
+				else if (MCS5 && (Rssi > (-77+RssiOffset)))
+					TxRateIdx = MCS5;
+				else if (MCS4 && (Rssi > (-79+RssiOffset)))
+					TxRateIdx = MCS4;
+				else if (MCS3 && (Rssi > (-81+RssiOffset)))
+					TxRateIdx = MCS3;
+				else if (MCS2 && (Rssi > (-83+RssiOffset)))
+					TxRateIdx = MCS2;
+				else if (MCS1 && (Rssi > (-86+RssiOffset)))
+					TxRateIdx = MCS1;
+				else
+					TxRateIdx = MCS0;
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+			{// Legacy mode
+				if (MCS7 && (Rssi > -70))
+					TxRateIdx = MCS7;
+				else if (MCS6 && (Rssi > -74))
+					TxRateIdx = MCS6;
+				else if (MCS5 && (Rssi > -78))
+					TxRateIdx = MCS5;
+				else if (MCS4 && (Rssi > -82))
+					TxRateIdx = MCS4;
+				else if (MCS4 == 0)	// for B-only mode
+					TxRateIdx = MCS3;
+				else if (MCS3 && (Rssi > -85))
+					TxRateIdx = MCS3;
+				else if (MCS2 && (Rssi > -87))
+					TxRateIdx = MCS2;
+				else if (MCS1 && (Rssi > -90))
+					TxRateIdx = MCS1;
+				else
+					TxRateIdx = MCS0;
+			}
+
+	//		if (TxRateIdx != pAd->CommonCfg.TxRateIndex)
+			{
+				pEntry->CurrTxRateIndex = TxRateIdx;
+				pNextTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(pEntry->CurrTxRateIndex+1)*5];
+				MlmeSetTxRate(pAd, pEntry, pNextTxRate);
+			}
+
+			NdisZeroMemory(pEntry->TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pEntry->PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+			pEntry->fLastSecAccordingRSSI = TRUE;
+			// reset all OneSecTx counters
+			RESET_ONE_SEC_TX_CNT(pEntry);
+
+			continue;
+		}
+
+		if (pEntry->fLastSecAccordingRSSI == TRUE)
+		{
+			pEntry->fLastSecAccordingRSSI = FALSE;
+			pEntry->LastSecTxRateChangeAction = 0;
+			// reset all OneSecTx counters
+			RESET_ONE_SEC_TX_CNT(pEntry);
+
+			continue;
+		}
+
+		do
+		{
+			BOOLEAN	bTrainUpDown = FALSE;
+
+			pEntry->CurrTxRateStableTime ++;
+
+			// downgrade TX quality if PER >= Rate-Down threshold
+			if (TxErrorRatio >= TrainDown)
+			{
+				bTrainUpDown = TRUE;
+				pEntry->TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+			}
+			// upgrade TX quality if PER <= Rate-Up threshold
+			else if (TxErrorRatio <= TrainUp)
+			{
+				bTrainUpDown = TRUE;
+				bUpgradeQuality = TRUE;
+				if (pEntry->TxQuality[CurrRateIdx])
+					pEntry->TxQuality[CurrRateIdx] --;  // quality very good in CurrRate
+
+				if (pEntry->TxRateUpPenalty)
+					pEntry->TxRateUpPenalty --;
+				else if (pEntry->TxQuality[UpRateIdx])
+					pEntry->TxQuality[UpRateIdx] --;    // may improve next UP rate's quality
+			}
+
+			pEntry->PER[CurrRateIdx] = (UCHAR)TxErrorRatio;
+
+			if (bTrainUpDown)
+			{
+				// perform DRS - consider TxRate Down first, then rate up.
+				if ((CurrRateIdx != DownRateIdx) && (pEntry->TxQuality[CurrRateIdx] >= DRS_TX_QUALITY_WORST_BOUND))
+				{
+					pEntry->CurrTxRateIndex = DownRateIdx;
+				}
+				else if ((CurrRateIdx != UpRateIdx) && (pEntry->TxQuality[UpRateIdx] <= 0))
+				{
+					pEntry->CurrTxRateIndex = UpRateIdx;
+				}
+			}
+		} while (FALSE);
+
+		// if rate-up happen, clear all bad history of all TX rates
+		if (pEntry->CurrTxRateIndex > CurrRateIdx)
+		{
+			pEntry->CurrTxRateStableTime = 0;
+			pEntry->TxRateUpPenalty = 0;
+			pEntry->LastSecTxRateChangeAction = 1; // rate UP
+			NdisZeroMemory(pEntry->TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pEntry->PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+
+			//
+			// For TxRate fast train up
+			//
+			if (!pAd->StaCfg.StaQuickResponeForRateUpTimerRunning)
+			{
+				RTMPSetTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, 100);
+
+				pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = TRUE;
+			}
+			bTxRateChanged = TRUE;
+		}
+		// if rate-down happen, only clear DownRate's bad history
+		else if (pEntry->CurrTxRateIndex < CurrRateIdx)
+		{
+			pEntry->CurrTxRateStableTime = 0;
+			pEntry->TxRateUpPenalty = 0;           // no penalty
+			pEntry->LastSecTxRateChangeAction = 2; // rate DOWN
+			pEntry->TxQuality[pEntry->CurrTxRateIndex] = 0;
+			pEntry->PER[pEntry->CurrTxRateIndex] = 0;
+
+			//
+			// For TxRate fast train down
+			//
+			if (!pAd->StaCfg.StaQuickResponeForRateUpTimerRunning)
+			{
+				RTMPSetTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, 100);
+
+				pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = TRUE;
+			}
+			bTxRateChanged = TRUE;
+		}
+		else
+		{
+			pEntry->LastSecTxRateChangeAction = 0; // rate no change
+			bTxRateChanged = FALSE;
+		}
+
+		pEntry->LastTxOkCount = TxSuccess;
+
+		// reset all OneSecTx counters
+		RESET_ONE_SEC_TX_CNT(pEntry);
+
+		pNextTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(pEntry->CurrTxRateIndex+1)*5];
+		if (bTxRateChanged && pNextTxRate)
+		{
+			MlmeSetTxRate(pAd, pEntry, pNextTxRate);
+		}
+	}
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Station side, Auto TxRate faster train up timer call back function.
+
+	Arguments:
+		SystemSpecific1			- Not used.
+		FunctionContext			- Pointer to our Adapter context.
+		SystemSpecific2			- Not used.
+		SystemSpecific3			- Not used.
+
+	Return Value:
+		None
+
+	========================================================================
+*/
+VOID StaQuickResponeForRateUpExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	PRTMP_ADAPTER			pAd = (PRTMP_ADAPTER)FunctionContext;
+	UCHAR					UpRateIdx = 0, DownRateIdx = 0, CurrRateIdx = 0;
+	ULONG					TxTotalCnt;
+	ULONG					TxErrorRatio = 0;
+	BOOLEAN					bTxRateChanged; //, bUpgradeQuality = FALSE;
+	PRTMP_TX_RATE_SWITCH	pCurrTxRate, pNextTxRate = NULL;
+	PUCHAR					pTable;
+	UCHAR					TableSize = 0;
+	UCHAR					InitTxRateIdx = 0, TrainUp, TrainDown;
+	TX_STA_CNT1_STRUC		StaTx1;
+	TX_STA_CNT0_STRUC		TxStaCnt0;
+	CHAR					Rssi, ratio;
+	ULONG					TxRetransmit = 0, TxSuccess = 0, TxFailCount = 0;
+	MAC_TABLE_ENTRY			*pEntry;
+	ULONG					i;
+
+	pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = FALSE;
+
+    //
+    // walk through MAC table, see if need to change AP's TX rate toward each entry
+    //
+	for (i = 1; i < MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		pEntry = &pAd->MacTab.Content[i];
+
+		// check if this entry need to switch rate automatically
+		if (RTMPCheckEntryEnableAutoRateSwitch(pAd, pEntry) == FALSE)
+			continue;
+
+		if (INFRA_ON(pAd) && (i == 1))
+			Rssi = RTMPMaxRssi(pAd,
+							   pAd->StaCfg.RssiSample.AvgRssi0,
+							   pAd->StaCfg.RssiSample.AvgRssi1,
+							   pAd->StaCfg.RssiSample.AvgRssi2);
+		else
+			Rssi = RTMPMaxRssi(pAd,
+							   pEntry->RssiSample.AvgRssi0,
+							   pEntry->RssiSample.AvgRssi1,
+							   pEntry->RssiSample.AvgRssi2);
+
+		CurrRateIdx = pAd->CommonCfg.TxRateIndex;
+
+			MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &InitTxRateIdx);
+
+		// decide the next upgrade rate and downgrade rate, if any
+		if ((CurrRateIdx > 0) && (CurrRateIdx < (TableSize - 1)))
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx -1;
+		}
+		else if (CurrRateIdx == 0)
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx;
+		}
+		else if (CurrRateIdx == (TableSize - 1))
+		{
+			UpRateIdx = CurrRateIdx;
+			DownRateIdx = CurrRateIdx - 1;
+		}
+
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(CurrRateIdx+1)*5];
+
+#ifdef DOT11_N_SUPPORT
+		if ((Rssi > -65) && (pCurrTxRate->Mode >= MODE_HTMIX))
+		{
+			TrainUp		= (pCurrTxRate->TrainUp + (pCurrTxRate->TrainUp >> 1));
+			TrainDown	= (pCurrTxRate->TrainDown + (pCurrTxRate->TrainDown >> 1));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			TrainUp		= pCurrTxRate->TrainUp;
+			TrainDown	= pCurrTxRate->TrainDown;
+		}
+
+		if (pAd->MacTab.Size == 1)
+		{
+			// Update statistic counter
+			RTMP_IO_READ32(pAd, TX_STA_CNT0, &TxStaCnt0.word);
+			RTMP_IO_READ32(pAd, TX_STA_CNT1, &StaTx1.word);
+
+			TxRetransmit = StaTx1.field.TxRetransmit;
+			TxSuccess = StaTx1.field.TxSuccess;
+			TxFailCount = TxStaCnt0.field.TxFailCount;
+			TxTotalCnt = TxRetransmit + TxSuccess + TxFailCount;
+
+			pAd->RalinkCounters.OneSecTxRetryOkCount += StaTx1.field.TxRetransmit;
+			pAd->RalinkCounters.OneSecTxNoRetryOkCount += StaTx1.field.TxSuccess;
+			pAd->RalinkCounters.OneSecTxFailCount += TxStaCnt0.field.TxFailCount;
+			pAd->WlanCounters.TransmittedFragmentCount.u.LowPart += StaTx1.field.TxSuccess;
+			pAd->WlanCounters.RetryCount.u.LowPart += StaTx1.field.TxRetransmit;
+			pAd->WlanCounters.FailedCount.u.LowPart += TxStaCnt0.field.TxFailCount;
+
+#if 0 // test by Gary.
+			// if no traffic in the past 1-sec period, don't change TX rate,
+			// but clear all bad history. because the bad history may affect the next
+			// Chariot throughput test
+			TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxFailCount;
+#endif
+			if (TxTotalCnt)
+				TxErrorRatio = ((TxRetransmit + TxFailCount) * 100) / TxTotalCnt;
+		}
+		else
+		{
+			TxTotalCnt = pEntry->OneSecTxNoRetryOkCount +
+				 pEntry->OneSecTxRetryOkCount +
+				 pEntry->OneSecTxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((pEntry->OneSecTxRetryOkCount + pEntry->OneSecTxFailCount) * 100) / TxTotalCnt;
+		}
+
+
+		//
+		// CASE 1. when TX samples are fewer than 15, then decide TX rate solely on RSSI
+		//         (criteria copied from RT2500 for Netopia case)
+		//
+		if (TxTotalCnt <= 12)
+		{
+			NdisZeroMemory(pAd->DrsCounters.TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pAd->DrsCounters.PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+
+			if ((pAd->DrsCounters.LastSecTxRateChangeAction == 1) && (CurrRateIdx != DownRateIdx))
+			{
+				pAd->CommonCfg.TxRateIndex = DownRateIdx;
+				pAd->DrsCounters.TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+			}
+			else if ((pAd->DrsCounters.LastSecTxRateChangeAction == 2) && (CurrRateIdx != UpRateIdx))
+			{
+				pAd->CommonCfg.TxRateIndex = UpRateIdx;
+			}
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("QuickDRS: TxTotalCnt <= 15, train back to original rate \n"));
+			return;
+		}
+
+		do
+		{
+			ULONG OneSecTxNoRetryOKRationCount;
+
+			if (pAd->DrsCounters.LastTimeTxRateChangeAction == 0)
+				ratio = 5;
+			else
+				ratio = 4;
+
+			// downgrade TX quality if PER >= Rate-Down threshold
+			if (TxErrorRatio >= TrainDown)
+			{
+				pAd->DrsCounters.TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+			}
+
+			pAd->DrsCounters.PER[CurrRateIdx] = (UCHAR)TxErrorRatio;
+
+			OneSecTxNoRetryOKRationCount = (TxSuccess * ratio);
+
+			// perform DRS - consider TxRate Down first, then rate up.
+			if ((pAd->DrsCounters.LastSecTxRateChangeAction == 1) && (CurrRateIdx != DownRateIdx))
+			{
+				if ((pAd->DrsCounters.LastTxOkCount + 2) >= OneSecTxNoRetryOKRationCount)
+				{
+					pAd->CommonCfg.TxRateIndex = DownRateIdx;
+					pAd->DrsCounters.TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+
+				}
+
+			}
+			else if ((pAd->DrsCounters.LastSecTxRateChangeAction == 2) && (CurrRateIdx != UpRateIdx))
+			{
+				if ((TxErrorRatio >= 50) || (TxErrorRatio >= TrainDown))
+				{
+
+				}
+				else if ((pAd->DrsCounters.LastTxOkCount + 2) >= OneSecTxNoRetryOKRationCount)
+				{
+					pAd->CommonCfg.TxRateIndex = UpRateIdx;
+				}
+			}
+		}while (FALSE);
+
+		// if rate-up happen, clear all bad history of all TX rates
+		if (pAd->CommonCfg.TxRateIndex > CurrRateIdx)
+		{
+			pAd->DrsCounters.TxRateUpPenalty = 0;
+			NdisZeroMemory(pAd->DrsCounters.TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pAd->DrsCounters.PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+			bTxRateChanged = TRUE;
+		}
+		// if rate-down happen, only clear DownRate's bad history
+		else if (pAd->CommonCfg.TxRateIndex < CurrRateIdx)
+		{
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("QuickDRS: --TX rate from %d to %d \n", CurrRateIdx, pAd->CommonCfg.TxRateIndex));
+
+			pAd->DrsCounters.TxRateUpPenalty = 0;           // no penalty
+			pAd->DrsCounters.TxQuality[pAd->CommonCfg.TxRateIndex] = 0;
+			pAd->DrsCounters.PER[pAd->CommonCfg.TxRateIndex] = 0;
+			bTxRateChanged = TRUE;
+		}
+		else
+		{
+			bTxRateChanged = FALSE;
+		}
+
+		pNextTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(pAd->CommonCfg.TxRateIndex+1)*5];
+		if (bTxRateChanged && pNextTxRate)
+		{
+			MlmeSetTxRate(pAd, pEntry, pNextTxRate);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine is executed periodically inside MlmePeriodicExec() after
+		association with an AP.
+		It checks if StaCfg.Psm is consistent with user policy (recorded in
+		StaCfg.WindowsPowerMode). If not, enforce user policy. However,
+		there're some conditions to consider:
+		1. we don't support power-saving in ADHOC mode, so Psm=PWR_ACTIVE all
+		   the time when Mibss==TRUE
+		2. When link up in INFRA mode, Psm should not be switch to PWR_SAVE
+		   if outgoing traffic available in TxRing or MgmtRing.
+	Output:
+		1. change pAd->StaCfg.Psm to PWR_SAVE or leave it untouched
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeCheckPsmChange(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG	Now32)
+{
+	ULONG	PowerMode;
+
+	// condition -
+	// 1. Psm maybe ON only happen in INFRASTRUCTURE mode
+	// 2. user wants either MAX_PSP or FAST_PSP
+	// 3. but current psm is not in PWR_SAVE
+	// 4. CNTL state machine is not doing SCANning
+	// 5. no TX SUCCESS event for the past 1-sec period
+#ifdef NDIS51_MINIPORT
+	if (pAd->StaCfg.WindowsPowerProfile == NdisPowerProfileBattery)
+		PowerMode = pAd->StaCfg.WindowsBatteryPowerMode;
+	else
+#endif
+		PowerMode = pAd->StaCfg.WindowsPowerMode;
+
+	if (INFRA_ON(pAd) &&
+		(PowerMode != Ndis802_11PowerModeCAM) &&
+		(pAd->StaCfg.Psm == PWR_ACTIVE) &&
+//		(! RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		(pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE) /*&&
+		(pAd->RalinkCounters.OneSecTxNoRetryOkCount == 0) &&
+		(pAd->RalinkCounters.OneSecTxRetryOkCount == 0)*/)
+	{
+		NdisGetSystemUpTime(&pAd->Mlme.LastSendNULLpsmTime);
+		pAd->RalinkCounters.RxCountSinceLastNULL = 0;
+		MlmeSetPsmBit(pAd, PWR_SAVE);
+		if (!(pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable))
+		{
+			RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, FALSE);
+		}
+		else
+		{
+			RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+		}
+	}
+}
+
+// IRQL = PASSIVE_LEVEL
+// IRQL = DISPATCH_LEVEL
+VOID MlmeSetPsmBit(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT psm)
+{
+	AUTO_RSP_CFG_STRUC csr4;
+
+	pAd->StaCfg.Psm = psm;
+	RTMP_IO_READ32(pAd, AUTO_RSP_CFG, &csr4.word);
+	csr4.field.AckCtsPsmBit = (psm == PWR_SAVE)? 1:0;
+	RTMP_IO_WRITE32(pAd, AUTO_RSP_CFG, csr4.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("MlmeSetPsmBit = %d\n", psm));
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeSetTxPreamble(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TxPreamble)
+{
+	AUTO_RSP_CFG_STRUC csr4;
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	//TxPreamble = Rt802_11PreambleLong;
+
+	RTMP_IO_READ32(pAd, AUTO_RSP_CFG, &csr4.word);
+	if (TxPreamble == Rt802_11PreambleLong)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeSetTxPreamble (= LONG PREAMBLE)\n"));
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+		csr4.field.AutoResponderPreamble = 0;
+	}
+	else
+	{
+		// NOTE: 1Mbps should always use long preamble
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeSetTxPreamble (= SHORT PREAMBLE)\n"));
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+		csr4.field.AutoResponderPreamble = 1;
+	}
+
+	RTMP_IO_WRITE32(pAd, AUTO_RSP_CFG, csr4.word);
+}
+
+/*
+    ==========================================================================
+    Description:
+        Update basic rate bitmap
+    ==========================================================================
+ */
+
+VOID UpdateBasicRateBitmap(
+    IN  PRTMP_ADAPTER   pAdapter)
+{
+    INT  i, j;
+                  /* 1  2  5.5, 11,  6,  9, 12, 18, 24, 36, 48,  54 */
+    UCHAR rate[] = { 2, 4,  11, 22, 12, 18, 24, 36, 48, 72, 96, 108 };
+    UCHAR *sup_p = pAdapter->CommonCfg.SupRate;
+    UCHAR *ext_p = pAdapter->CommonCfg.ExtRate;
+    ULONG bitmap = pAdapter->CommonCfg.BasicRateBitmap;
+
+
+    /* if A mode, always use fix BasicRateBitMap */
+    //if (pAdapter->CommonCfg.Channel == PHY_11A)
+	if (pAdapter->CommonCfg.Channel > 14)
+        pAdapter->CommonCfg.BasicRateBitmap = 0x150; /* 6, 12, 24M */
+    /* End of if */
+
+    if (pAdapter->CommonCfg.BasicRateBitmap > 4095)
+    {
+        /* (2 ^ MAX_LEN_OF_SUPPORTED_RATES) -1 */
+        return;
+    } /* End of if */
+
+    for(i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+    {
+        sup_p[i] &= 0x7f;
+        ext_p[i] &= 0x7f;
+    } /* End of for */
+
+    for(i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+    {
+        if (bitmap & (1 << i))
+        {
+            for(j=0; j<MAX_LEN_OF_SUPPORTED_RATES; j++)
+            {
+                if (sup_p[j] == rate[i])
+                    sup_p[j] |= 0x80;
+                /* End of if */
+            } /* End of for */
+
+            for(j=0; j<MAX_LEN_OF_SUPPORTED_RATES; j++)
+            {
+                if (ext_p[j] == rate[i])
+                    ext_p[j] |= 0x80;
+                /* End of if */
+            } /* End of for */
+        } /* End of if */
+    } /* End of for */
+} /* End of UpdateBasicRateBitmap */
+
+// IRQL = PASSIVE_LEVEL
+// IRQL = DISPATCH_LEVEL
+// bLinkUp is to identify the inital link speed.
+// TRUE indicates the rate update at linkup, we should not try to set the rate at 54Mbps.
+VOID MlmeUpdateTxRates(
+	IN PRTMP_ADAPTER 		pAd,
+	IN 	BOOLEAN		 		bLinkUp,
+	IN	UCHAR				apidx)
+{
+	int i, num;
+	UCHAR Rate = RATE_6, MaxDesire = RATE_1, MaxSupport = RATE_1;
+	UCHAR MinSupport = RATE_54;
+	ULONG BasicRateBitmap = 0;
+	UCHAR CurrBasicRate = RATE_1;
+	UCHAR *pSupRate, SupRateLen, *pExtRate, ExtRateLen;
+	PHTTRANSMIT_SETTING		pHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMaxHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMinHtPhy = NULL;
+	BOOLEAN 				*auto_rate_cur_p;
+	UCHAR					HtMcs = MCS_AUTO;
+
+	// find max desired rate
+	UpdateBasicRateBitmap(pAd);
+
+	num = 0;
+	auto_rate_cur_p = NULL;
+	for (i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+	{
+		switch (pAd->CommonCfg.DesireRate[i] & 0x7f)
+		{
+			case 2:  Rate = RATE_1;   num++;   break;
+			case 4:  Rate = RATE_2;   num++;   break;
+			case 11: Rate = RATE_5_5; num++;   break;
+			case 22: Rate = RATE_11;  num++;   break;
+			case 12: Rate = RATE_6;   num++;   break;
+			case 18: Rate = RATE_9;   num++;   break;
+			case 24: Rate = RATE_12;  num++;   break;
+			case 36: Rate = RATE_18;  num++;   break;
+			case 48: Rate = RATE_24;  num++;   break;
+			case 72: Rate = RATE_36;  num++;   break;
+			case 96: Rate = RATE_48;  num++;   break;
+			case 108: Rate = RATE_54; num++;   break;
+			//default: Rate = RATE_1;   break;
+		}
+		if (MaxDesire < Rate)  MaxDesire = Rate;
+	}
+
+//===========================================================================
+//===========================================================================
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pHtPhy 		= &pAd->StaCfg.HTPhyMode;
+		pMaxHtPhy	= &pAd->StaCfg.MaxHTPhyMode;
+		pMinHtPhy	= &pAd->StaCfg.MinHTPhyMode;
+
+		auto_rate_cur_p = &pAd->StaCfg.bAutoTxRateSwitch;
+		HtMcs 		= pAd->StaCfg.DesiredTransmitSetting.field.MCS;
+
+		if ((pAd->StaCfg.BssType == BSS_ADHOC) &&
+			(pAd->CommonCfg.PhyMode == PHY_11B) &&
+			(MaxDesire > RATE_11))
+		{
+			MaxDesire = RATE_11;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	pAd->CommonCfg.MaxDesiredRate = MaxDesire;
+	pMinHtPhy->word = 0;
+	pMaxHtPhy->word = 0;
+	pHtPhy->word = 0;
+
+	// Auto rate switching is enabled only if more than one DESIRED RATES are
+	// specified; otherwise disabled
+	if (num <= 1)
+	{
+		//OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED);
+		//pAd->CommonCfg.bAutoTxRateSwitch	= FALSE;
+		*auto_rate_cur_p = FALSE;
+	}
+	else
+	{
+		//OPSTATUS_SET_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED);
+		//pAd->CommonCfg.bAutoTxRateSwitch	= TRUE;
+		*auto_rate_cur_p = TRUE;
+	}
+
+#if 1
+	if (HtMcs != MCS_AUTO)
+	{
+		//OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED);
+		//pAd->CommonCfg.bAutoTxRateSwitch	= FALSE;
+		*auto_rate_cur_p = FALSE;
+	}
+	else
+	{
+		//OPSTATUS_SET_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED);
+		//pAd->CommonCfg.bAutoTxRateSwitch	= TRUE;
+		*auto_rate_cur_p = TRUE;
+	}
+#endif
+
+#ifdef CONFIG_STA_SUPPORT
+	if ((ADHOC_ON(pAd) || INFRA_ON(pAd)) && (pAd->OpMode == OPMODE_STA))
+	{
+		pSupRate = &pAd->StaActive.SupRate[0];
+		pExtRate = &pAd->StaActive.ExtRate[0];
+		SupRateLen = pAd->StaActive.SupRateLen;
+		ExtRateLen = pAd->StaActive.ExtRateLen;
+	}
+	else
+#endif // CONFIG_STA_SUPPORT //
+	{
+		pSupRate = &pAd->CommonCfg.SupRate[0];
+		pExtRate = &pAd->CommonCfg.ExtRate[0];
+		SupRateLen = pAd->CommonCfg.SupRateLen;
+		ExtRateLen = pAd->CommonCfg.ExtRateLen;
+	}
+
+	// find max supported rate
+	for (i=0; i<SupRateLen; i++)
+	{
+		switch (pSupRate[i] & 0x7f)
+		{
+			case 2:   Rate = RATE_1;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0001;	 break;
+			case 4:   Rate = RATE_2;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0002;	 break;
+			case 11:  Rate = RATE_5_5;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0004;	 break;
+			case 22:  Rate = RATE_11;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0008;	 break;
+			case 12:  Rate = RATE_6;	/*if (pSupRate[i] & 0x80)*/  BasicRateBitmap |= 0x0010;  break;
+			case 18:  Rate = RATE_9;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0020;	 break;
+			case 24:  Rate = RATE_12;	/*if (pSupRate[i] & 0x80)*/  BasicRateBitmap |= 0x0040;  break;
+			case 36:  Rate = RATE_18;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0080;	 break;
+			case 48:  Rate = RATE_24;	/*if (pSupRate[i] & 0x80)*/  BasicRateBitmap |= 0x0100;  break;
+			case 72:  Rate = RATE_36;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0200;	 break;
+			case 96:  Rate = RATE_48;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0400;	 break;
+			case 108: Rate = RATE_54;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0800;	 break;
+			default:  Rate = RATE_1;	break;
+		}
+		if (MaxSupport < Rate)	MaxSupport = Rate;
+
+		if (MinSupport > Rate) MinSupport = Rate;
+	}
+
+	for (i=0; i<ExtRateLen; i++)
+	{
+		switch (pExtRate[i] & 0x7f)
+		{
+			case 2:   Rate = RATE_1;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0001;	 break;
+			case 4:   Rate = RATE_2;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0002;	 break;
+			case 11:  Rate = RATE_5_5;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0004;	 break;
+			case 22:  Rate = RATE_11;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0008;	 break;
+			case 12:  Rate = RATE_6;	/*if (pExtRate[i] & 0x80)*/  BasicRateBitmap |= 0x0010;  break;
+			case 18:  Rate = RATE_9;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0020;	 break;
+			case 24:  Rate = RATE_12;	/*if (pExtRate[i] & 0x80)*/  BasicRateBitmap |= 0x0040;  break;
+			case 36:  Rate = RATE_18;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0080;	 break;
+			case 48:  Rate = RATE_24;	/*if (pExtRate[i] & 0x80)*/  BasicRateBitmap |= 0x0100;  break;
+			case 72:  Rate = RATE_36;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0200;	 break;
+			case 96:  Rate = RATE_48;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0400;	 break;
+			case 108: Rate = RATE_54;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0800;	 break;
+			default:  Rate = RATE_1;	break;
+		}
+		if (MaxSupport < Rate)	MaxSupport = Rate;
+
+		if (MinSupport > Rate) MinSupport = Rate;
+	}
+
+	RTMP_IO_WRITE32(pAd, LEGACY_BASIC_RATE, BasicRateBitmap);
+
+	// bug fix
+	// pAd->CommonCfg.BasicRateBitmap = BasicRateBitmap;
+
+	// calculate the exptected ACK rate for each TX rate. This info is used to caculate
+	// the DURATION field of outgoing uniicast DATA/MGMT frame
+	for (i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+	{
+		if (BasicRateBitmap & (0x01 << i))
+			CurrBasicRate = (UCHAR)i;
+		pAd->CommonCfg.ExpectedACKRate[i] = CurrBasicRate;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateTxRates[MaxSupport = %d] = MaxDesire %d Mbps\n", RateIdToMbps[MaxSupport], RateIdToMbps[MaxDesire]));
+	// max tx rate = min {max desire rate, max supported rate}
+	if (MaxSupport < MaxDesire)
+		pAd->CommonCfg.MaxTxRate = MaxSupport;
+	else
+		pAd->CommonCfg.MaxTxRate = MaxDesire;
+
+	pAd->CommonCfg.MinTxRate = MinSupport;
+	// 2003-07-31 john - 2500 doesn't have good sensitivity at high OFDM rates. to increase the success
+	// ratio of initial DHCP packet exchange, TX rate starts from a lower rate depending
+	// on average RSSI
+	//	 1. RSSI >= -70db, start at 54 Mbps (short distance)
+	//	 2. -70 > RSSI >= -75, start at 24 Mbps (mid distance)
+	//	 3. -75 > RSSI, start at 11 Mbps (long distance)
+	//if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED)/* &&
+	//	OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)*/)
+	if (*auto_rate_cur_p)
+	{
+		short dbm = 0;
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			dbm = pAd->StaCfg.RssiSample.AvgRssi0 - pAd->BbpRssiToDbmDelta;
+#endif // CONFIG_STA_SUPPORT //
+		if (bLinkUp == TRUE)
+			pAd->CommonCfg.TxRate = RATE_24;
+		else
+			pAd->CommonCfg.TxRate = pAd->CommonCfg.MaxTxRate;
+
+		if (dbm < -75)
+			pAd->CommonCfg.TxRate = RATE_11;
+		else if (dbm < -70)
+			pAd->CommonCfg.TxRate = RATE_24;
+
+		// should never exceed MaxTxRate (consider 11B-only mode)
+		if (pAd->CommonCfg.TxRate > pAd->CommonCfg.MaxTxRate)
+			pAd->CommonCfg.TxRate = pAd->CommonCfg.MaxTxRate;
+
+		pAd->CommonCfg.TxRateIndex = 0;
+	}
+	else
+	{
+		pAd->CommonCfg.TxRate = pAd->CommonCfg.MaxTxRate;
+		pHtPhy->field.MCS	= (pAd->CommonCfg.MaxTxRate > 3) ? (pAd->CommonCfg.MaxTxRate - 4) : pAd->CommonCfg.MaxTxRate;
+		pHtPhy->field.MODE	= (pAd->CommonCfg.MaxTxRate > 3) ? MODE_OFDM : MODE_CCK;
+
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.STBC	= pHtPhy->field.STBC;
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.ShortGI	= pHtPhy->field.ShortGI;
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MCS		= pHtPhy->field.MCS;
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE	= pHtPhy->field.MODE;
+	}
+
+	if (pAd->CommonCfg.TxRate <= RATE_11)
+	{
+		pMaxHtPhy->field.MODE = MODE_CCK;
+		pMaxHtPhy->field.MCS = pAd->CommonCfg.TxRate;
+		pMinHtPhy->field.MCS = pAd->CommonCfg.MinTxRate;
+	}
+	else
+	{
+		pMaxHtPhy->field.MODE = MODE_OFDM;
+		pMaxHtPhy->field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.TxRate];
+		if (pAd->CommonCfg.MinTxRate >= RATE_6 && (pAd->CommonCfg.MinTxRate <= RATE_54))
+			{pMinHtPhy->field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MinTxRate];}
+		else
+			{pMinHtPhy->field.MCS = pAd->CommonCfg.MinTxRate;}
+	}
+
+	pHtPhy->word = (pMaxHtPhy->word);
+	if (bLinkUp && (pAd->OpMode == OPMODE_STA))
+	{
+			pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word = pHtPhy->word;
+			pAd->MacTab.Content[BSSID_WCID].MaxHTPhyMode.word = pMaxHtPhy->word;
+			pAd->MacTab.Content[BSSID_WCID].MinHTPhyMode.word = pMinHtPhy->word;
+	}
+	else
+	{
+		switch (pAd->CommonCfg.PhyMode)
+		{
+			case PHY_11BG_MIXED:
+			case PHY_11B:
+#ifdef DOT11_N_SUPPORT
+			case PHY_11BGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+				pAd->CommonCfg.MlmeRate = RATE_1;
+				pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+				pAd->CommonCfg.MlmeTransmit.field.MCS = RATE_1;
+
+//#ifdef	WIFI_TEST
+				pAd->CommonCfg.RtsRate = RATE_11;
+//#else
+//				pAd->CommonCfg.RtsRate = RATE_1;
+//#endif
+				break;
+			case PHY_11G:
+			case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+			case PHY_11AGN_MIXED:
+			case PHY_11GN_MIXED:
+			case PHY_11N_2_4G:
+			case PHY_11AN_MIXED:
+			case PHY_11N_5G:
+#endif // DOT11_N_SUPPORT //
+				pAd->CommonCfg.MlmeRate = RATE_6;
+				pAd->CommonCfg.RtsRate = RATE_6;
+				pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+				pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+				break;
+			case PHY_11ABG_MIXED:
+#ifdef DOT11_N_SUPPORT
+			case PHY_11ABGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+				if (pAd->CommonCfg.Channel <= 14)
+				{
+					pAd->CommonCfg.MlmeRate = RATE_1;
+					pAd->CommonCfg.RtsRate = RATE_1;
+					pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+					pAd->CommonCfg.MlmeTransmit.field.MCS = RATE_1;
+				}
+				else
+				{
+					pAd->CommonCfg.MlmeRate = RATE_6;
+					pAd->CommonCfg.RtsRate = RATE_6;
+					pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+					pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+				}
+				break;
+			default: // error
+				pAd->CommonCfg.MlmeRate = RATE_6;
+                        	pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+				pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+				pAd->CommonCfg.RtsRate = RATE_1;
+				break;
+		}
+		//
+		// Keep Basic Mlme Rate.
+		//
+		pAd->MacTab.Content[MCAST_WCID].HTPhyMode.word = pAd->CommonCfg.MlmeTransmit.word;
+		if (pAd->CommonCfg.MlmeTransmit.field.MODE == MODE_OFDM)
+			pAd->MacTab.Content[MCAST_WCID].HTPhyMode.field.MCS = OfdmRateToRxwiMCS[RATE_24];
+		else
+			pAd->MacTab.Content[MCAST_WCID].HTPhyMode.field.MCS = RATE_1;
+		pAd->CommonCfg.BasicMlmeRate = pAd->CommonCfg.MlmeRate;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (MaxDesire=%d, MaxSupport=%d, MaxTxRate=%d, MinRate=%d, Rate Switching =%d)\n",
+			 RateIdToMbps[MaxDesire], RateIdToMbps[MaxSupport], RateIdToMbps[pAd->CommonCfg.MaxTxRate], RateIdToMbps[pAd->CommonCfg.MinTxRate],
+			 /*OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED)*/*auto_rate_cur_p));
+	DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (TxRate=%d, RtsRate=%d, BasicRateBitmap=0x%04lx)\n",
+			 RateIdToMbps[pAd->CommonCfg.TxRate], RateIdToMbps[pAd->CommonCfg.RtsRate], BasicRateBitmap));
+	DBGPRINT(RT_DEBUG_TRACE, ("MlmeUpdateTxRates (MlmeTransmit=0x%x, MinHTPhyMode=%x, MaxHTPhyMode=0x%x, HTPhyMode=0x%x)\n",
+			 pAd->CommonCfg.MlmeTransmit.word, pAd->MacTab.Content[BSSID_WCID].MinHTPhyMode.word ,pAd->MacTab.Content[BSSID_WCID].MaxHTPhyMode.word ,pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word ));
+}
+
+#ifdef DOT11_N_SUPPORT
+/*
+	==========================================================================
+	Description:
+		This function update HT Rate setting.
+		Input Wcid value is valid for 2 case :
+		1. it's used for Station in infra mode that copy AP rate to Mactable.
+		2. OR Station 	in adhoc mode to copy peer's HT rate to Mactable.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeUpdateHtTxRates(
+	IN PRTMP_ADAPTER 		pAd,
+	IN	UCHAR				apidx)
+{
+	UCHAR	StbcMcs; //j, StbcMcs, bitmask;
+	CHAR 	i; // 3*3
+	RT_HT_CAPABILITY 	*pRtHtCap = NULL;
+	RT_HT_PHY_INFO		*pActiveHtPhy = NULL;
+	ULONG		BasicMCS;
+	UCHAR j, bitmask;
+	PRT_HT_PHY_INFO			pDesireHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMaxHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMinHtPhy = NULL;
+	BOOLEAN 				*auto_rate_cur_p;
+
+	DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateHtTxRates===> \n"));
+
+	auto_rate_cur_p = NULL;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pDesireHtPhy	= &pAd->StaCfg.DesiredHtPhyInfo;
+		pActiveHtPhy	= &pAd->StaCfg.DesiredHtPhyInfo;
+		pHtPhy 		= &pAd->StaCfg.HTPhyMode;
+		pMaxHtPhy	= &pAd->StaCfg.MaxHTPhyMode;
+		pMinHtPhy	= &pAd->StaCfg.MinHTPhyMode;
+
+		auto_rate_cur_p = &pAd->StaCfg.bAutoTxRateSwitch;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+	if ((ADHOC_ON(pAd) || INFRA_ON(pAd)) && (pAd->OpMode == OPMODE_STA))
+	{
+		if (pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE)
+			return;
+
+		pRtHtCap = &pAd->StaActive.SupportedHtPhy;
+		pActiveHtPhy = &pAd->StaActive.SupportedPhyInfo;
+		StbcMcs = (UCHAR)pAd->MlmeAux.AddHtInfo.AddHtInfo3.StbcMcs;
+		BasicMCS =pAd->MlmeAux.AddHtInfo.MCSSet[0]+(pAd->MlmeAux.AddHtInfo.MCSSet[1]<<8)+(StbcMcs<<16);
+		if ((pAd->CommonCfg.DesiredHtPhy.TxSTBC) && (pRtHtCap->RxSTBC) && (pAd->Antenna.field.TxPath == 2))
+			pMaxHtPhy->field.STBC = STBC_USE;
+		else
+			pMaxHtPhy->field.STBC = STBC_NONE;
+	}
+	else
+#endif // CONFIG_STA_SUPPORT //
+	{
+		if (pDesireHtPhy->bHtEnable == FALSE)
+			return;
+
+		pRtHtCap = &pAd->CommonCfg.DesiredHtPhy;
+		StbcMcs = (UCHAR)pAd->CommonCfg.AddHTInfo.AddHtInfo3.StbcMcs;
+		BasicMCS = pAd->CommonCfg.AddHTInfo.MCSSet[0]+(pAd->CommonCfg.AddHTInfo.MCSSet[1]<<8)+(StbcMcs<<16);
+		if ((pAd->CommonCfg.DesiredHtPhy.TxSTBC) && (pRtHtCap->RxSTBC) && (pAd->Antenna.field.TxPath == 2))
+			pMaxHtPhy->field.STBC = STBC_USE;
+		else
+			pMaxHtPhy->field.STBC = STBC_NONE;
+	}
+
+	// Decide MAX ht rate.
+	if ((pRtHtCap->GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+		pMaxHtPhy->field.MODE = MODE_HTGREENFIELD;
+	else
+		pMaxHtPhy->field.MODE = MODE_HTMIX;
+
+    if ((pAd->CommonCfg.DesiredHtPhy.ChannelWidth) && (pRtHtCap->ChannelWidth))
+		pMaxHtPhy->field.BW = BW_40;
+	else
+		pMaxHtPhy->field.BW = BW_20;
+
+    if (pMaxHtPhy->field.BW == BW_20)
+		pMaxHtPhy->field.ShortGI = (pAd->CommonCfg.DesiredHtPhy.ShortGIfor20 & pRtHtCap->ShortGIfor20);
+	else
+		pMaxHtPhy->field.ShortGI = (pAd->CommonCfg.DesiredHtPhy.ShortGIfor40 & pRtHtCap->ShortGIfor40);
+
+	for (i=23; i>=0; i--) // 3*3
+	{
+		j = i/8;
+		bitmask = (1<<(i-(j*8)));
+
+		if ((pActiveHtPhy->MCSSet[j] & bitmask) && (pDesireHtPhy->MCSSet[j] & bitmask))
+		{
+			pMaxHtPhy->field.MCS = i;
+			break;
+		}
+
+		if (i==0)
+			break;
+	}
+
+	// Copy MIN ht rate.  rt2860???
+	pMinHtPhy->field.BW = BW_20;
+	pMinHtPhy->field.MCS = 0;
+	pMinHtPhy->field.STBC = 0;
+	pMinHtPhy->field.ShortGI = 0;
+	//If STA assigns fixed rate. update to fixed here.
+#ifdef CONFIG_STA_SUPPORT
+	if ( (pAd->OpMode == OPMODE_STA) && (pDesireHtPhy->MCSSet[0] != 0xff))
+	{
+		if (pDesireHtPhy->MCSSet[4] != 0)
+		{
+			pMaxHtPhy->field.MCS = 32;
+			pMinHtPhy->field.MCS = 32;
+			DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateHtTxRates<=== Use Fixed MCS = %d\n",pMinHtPhy->field.MCS));
+		}
+
+		for (i=23; (CHAR)i >= 0; i--) // 3*3
+		{
+			j = i/8;
+			bitmask = (1<<(i-(j*8)));
+			if ( (pDesireHtPhy->MCSSet[j] & bitmask) && (pActiveHtPhy->MCSSet[j] & bitmask))
+			{
+				pMaxHtPhy->field.MCS = i;
+				pMinHtPhy->field.MCS = i;
+				break;
+			}
+			if (i==0)
+				break;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+	// Decide ht rate
+	pHtPhy->field.STBC = pMaxHtPhy->field.STBC;
+	pHtPhy->field.BW = pMaxHtPhy->field.BW;
+	pHtPhy->field.MODE = pMaxHtPhy->field.MODE;
+	pHtPhy->field.MCS = pMaxHtPhy->field.MCS;
+	pHtPhy->field.ShortGI = pMaxHtPhy->field.ShortGI;
+
+	// use default now. rt2860
+	if (pDesireHtPhy->MCSSet[0] != 0xff)
+		*auto_rate_cur_p = FALSE;
+	else
+		*auto_rate_cur_p = TRUE;
+
+	DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateHtTxRates<---.AMsduSize = %d  \n", pAd->CommonCfg.DesiredHtPhy.AmsduSize ));
+	DBGPRINT(RT_DEBUG_TRACE,("TX: MCS[0] = %x (choose %d), BW = %d, ShortGI = %d, MODE = %d,  \n", pActiveHtPhy->MCSSet[0],pHtPhy->field.MCS,
+		pHtPhy->field.BW, pHtPhy->field.ShortGI, pHtPhy->field.MODE));
+	DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateHtTxRates<=== \n"));
+}
+#endif // DOT11_N_SUPPORT //
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeRadioOff(
+	IN PRTMP_ADAPTER pAd)
+{
+	RT28XX_MLME_RADIO_OFF(pAd);
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeRadioOn(
+	IN PRTMP_ADAPTER pAd)
+{
+	RT28XX_MLME_RADIO_ON(pAd);
+}
+
+// ===========================================================================================
+// bss_table.c
+// ===========================================================================================
+
+
+/*! \brief initialize BSS table
+ *	\param p_tab pointer to the table
+ *	\return none
+ *	\pre
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID BssTableInit(
+	IN BSS_TABLE *Tab)
+{
+	int i;
+
+	Tab->BssNr = 0;
+    Tab->BssOverlapNr = 0;
+	for (i = 0; i < MAX_LEN_OF_BSS_TABLE; i++)
+	{
+		NdisZeroMemory(&Tab->BssEntry[i], sizeof(BSS_ENTRY));
+		Tab->BssEntry[i].Rssi = -127;	// initial the rssi as a minimum value
+	}
+}
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableInit(
+	IN PRTMP_ADAPTER pAd,
+    IN BA_TABLE *Tab)
+{
+	int i;
+
+	Tab->numAsOriginator = 0;
+	Tab->numAsRecipient = 0;
+	NdisAllocateSpinLock(&pAd->BATabLock);
+	for (i = 0; i < MAX_LEN_OF_BA_REC_TABLE; i++)
+	{
+		Tab->BARecEntry[i].REC_BA_Status = Recipient_NONE;
+		NdisAllocateSpinLock(&(Tab->BARecEntry[i].RxReRingLock));
+	}
+	for (i = 0; i < MAX_LEN_OF_BA_ORI_TABLE; i++)
+	{
+		Tab->BAOriEntry[i].ORI_BA_Status = Originator_NONE;
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+/*! \brief search the BSS table by SSID
+ *	\param p_tab pointer to the bss table
+ *	\param ssid SSID string
+ *	\return index of the table, BSS_NOT_FOUND if not in the table
+ *	\pre
+ *	\post
+ *	\note search by sequential search
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+ULONG BssTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR	 pBssid,
+	IN UCHAR	 Channel)
+{
+	UCHAR i;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		//
+		// Some AP that support A/B/G mode that may used the same BSSID on 11A and 11B/G.
+		// We should distinguish this case.
+		//
+		if ((((Tab->BssEntry[i].Channel <= 14) && (Channel <= 14)) ||
+			 ((Tab->BssEntry[i].Channel > 14) && (Channel > 14))) &&
+			MAC_ADDR_EQUAL(Tab->BssEntry[i].Bssid, pBssid))
+		{
+			return i;
+		}
+	}
+	return (ULONG)BSS_NOT_FOUND;
+}
+
+ULONG BssSsidTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR	 pBssid,
+	IN PUCHAR	 pSsid,
+	IN UCHAR	 SsidLen,
+	IN UCHAR	 Channel)
+{
+	UCHAR i;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		//
+		// Some AP that support A/B/G mode that may used the same BSSID on 11A and 11B/G.
+		// We should distinguish this case.
+		//
+		if ((((Tab->BssEntry[i].Channel <= 14) && (Channel <= 14)) ||
+			 ((Tab->BssEntry[i].Channel > 14) && (Channel > 14))) &&
+			MAC_ADDR_EQUAL(Tab->BssEntry[i].Bssid, pBssid) &&
+			SSID_EQUAL(pSsid, SsidLen, Tab->BssEntry[i].Ssid, Tab->BssEntry[i].SsidLen))
+		{
+			return i;
+		}
+	}
+	return (ULONG)BSS_NOT_FOUND;
+}
+
+ULONG BssTableSearchWithSSID(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR	 Bssid,
+	IN PUCHAR	 pSsid,
+	IN UCHAR	 SsidLen,
+	IN UCHAR	 Channel)
+{
+	UCHAR i;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		if ((((Tab->BssEntry[i].Channel <= 14) && (Channel <= 14)) ||
+			((Tab->BssEntry[i].Channel > 14) && (Channel > 14))) &&
+			MAC_ADDR_EQUAL(&(Tab->BssEntry[i].Bssid), Bssid) &&
+			(SSID_EQUAL(pSsid, SsidLen, Tab->BssEntry[i].Ssid, Tab->BssEntry[i].SsidLen) ||
+			(NdisEqualMemory(pSsid, ZeroSsid, SsidLen)) ||
+			(NdisEqualMemory(Tab->BssEntry[i].Ssid, ZeroSsid, Tab->BssEntry[i].SsidLen))))
+		{
+			return i;
+		}
+	}
+	return (ULONG)BSS_NOT_FOUND;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID BssTableDeleteEntry(
+	IN OUT	BSS_TABLE *Tab,
+	IN		PUCHAR	  pBssid,
+	IN		UCHAR	  Channel)
+{
+	UCHAR i, j;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		if ((Tab->BssEntry[i].Channel == Channel) &&
+			(MAC_ADDR_EQUAL(Tab->BssEntry[i].Bssid, pBssid)))
+		{
+			for (j = i; j < Tab->BssNr - 1; j++)
+			{
+				NdisMoveMemory(&(Tab->BssEntry[j]), &(Tab->BssEntry[j + 1]), sizeof(BSS_ENTRY));
+			}
+			NdisZeroMemory(&(Tab->BssEntry[Tab->BssNr - 1]), sizeof(BSS_ENTRY));
+			Tab->BssNr -= 1;
+			return;
+		}
+	}
+}
+
+#ifdef DOT11_N_SUPPORT
+/*
+	========================================================================
+	Routine Description:
+		Delete the Originator Entry in BAtable. Or decrease numAs Originator by 1 if needed.
+
+	Arguments:
+	// IRQL = DISPATCH_LEVEL
+	========================================================================
+*/
+VOID BATableDeleteORIEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		BA_ORI_ENTRY	*pBAORIEntry)
+{
+
+	if (pBAORIEntry->ORI_BA_Status != Originator_NONE)
+	{
+		NdisAcquireSpinLock(&pAd->BATabLock);
+		if (pBAORIEntry->ORI_BA_Status == Originator_Done)
+		{
+			pAd->BATable.numAsOriginator -= 1;
+			DBGPRINT(RT_DEBUG_TRACE, ("BATableDeleteORIEntry numAsOriginator= %ld\n", pAd->BATable.numAsRecipient));
+			// Erase Bitmap flag.
+		}
+		pAd->MacTab.Content[pBAORIEntry->Wcid].TXBAbitmap &= (~(1<<(pBAORIEntry->TID) ));	// If STA mode,  erase flag here
+		pAd->MacTab.Content[pBAORIEntry->Wcid].BAOriWcidArray[pBAORIEntry->TID] = 0;	// If STA mode,  erase flag here
+		pBAORIEntry->ORI_BA_Status = Originator_NONE;
+		pBAORIEntry->Token = 1;
+		// Not clear Sequence here.
+		NdisReleaseSpinLock(&pAd->BATabLock);
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+/*! \brief
+ *	\param
+ *	\return
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID BssEntrySet(
+	IN PRTMP_ADAPTER	pAd,
+	OUT BSS_ENTRY *pBss,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN PCF_PARM pCfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR Channel,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE)
+{
+	COPY_MAC_ADDR(pBss->Bssid, pBssid);
+	// Default Hidden SSID to be TRUE, it will be turned to FALSE after coping SSID
+	pBss->Hidden = 1;
+	if (SsidLen > 0)
+	{
+		// For hidden SSID AP, it might send beacon with SSID len equal to 0
+		// Or send beacon /probe response with SSID len matching real SSID length,
+		// but SSID is all zero. such as "00-00-00-00" with length 4.
+		// We have to prevent this case overwrite correct table
+		if (NdisEqualMemory(Ssid, ZeroSsid, SsidLen) == 0)
+		{
+		    NdisZeroMemory(pBss->Ssid, MAX_LEN_OF_SSID);
+			NdisMoveMemory(pBss->Ssid, Ssid, SsidLen);
+			pBss->SsidLen = SsidLen;
+			pBss->Hidden = 0;
+		}
+	}
+	else
+		pBss->SsidLen = 0;
+	pBss->BssType = BssType;
+	pBss->BeaconPeriod = BeaconPeriod;
+	if (BssType == BSS_INFRA)
+	{
+		if (pCfParm->bValid)
+		{
+			pBss->CfpCount = pCfParm->CfpCount;
+			pBss->CfpPeriod = pCfParm->CfpPeriod;
+			pBss->CfpMaxDuration = pCfParm->CfpMaxDuration;
+			pBss->CfpDurRemaining = pCfParm->CfpDurRemaining;
+		}
+	}
+	else
+	{
+		pBss->AtimWin = AtimWin;
+	}
+
+	pBss->CapabilityInfo = CapabilityInfo;
+	// The privacy bit indicate security is ON, it maight be WEP, TKIP or AES
+	// Combine with AuthMode, they will decide the connection methods.
+	pBss->Privacy = CAP_IS_PRIVACY_ON(pBss->CapabilityInfo);
+	ASSERT(SupRateLen <= MAX_LEN_OF_SUPPORTED_RATES);
+	if (SupRateLen <= MAX_LEN_OF_SUPPORTED_RATES)
+		NdisMoveMemory(pBss->SupRate, SupRate, SupRateLen);
+	else
+		NdisMoveMemory(pBss->SupRate, SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+	pBss->SupRateLen = SupRateLen;
+	ASSERT(ExtRateLen <= MAX_LEN_OF_SUPPORTED_RATES);
+	NdisMoveMemory(pBss->ExtRate, ExtRate, ExtRateLen);
+	NdisMoveMemory(&pBss->HtCapability, pHtCapability, HtCapabilityLen);
+	NdisMoveMemory(&pBss->AddHtInfo, pAddHtInfo, AddHtInfoLen);
+	pBss->NewExtChanOffset = NewExtChanOffset;
+	pBss->ExtRateLen = ExtRateLen;
+	pBss->Channel = Channel;
+	pBss->CentralChannel = Channel;
+	pBss->Rssi = Rssi;
+	// Update CkipFlag. if not exists, the value is 0x0
+	pBss->CkipFlag = CkipFlag;
+
+	// New for microsoft Fixed IEs
+	NdisMoveMemory(pBss->FixIEs.Timestamp, &TimeStamp, 8);
+	pBss->FixIEs.BeaconInterval = BeaconPeriod;
+	pBss->FixIEs.Capabilities = CapabilityInfo;
+
+	// New for microsoft Variable IEs
+	if (LengthVIE != 0)
+	{
+		pBss->VarIELen = LengthVIE;
+		NdisMoveMemory(pBss->VarIEs, pVIE, pBss->VarIELen);
+	}
+	else
+	{
+		pBss->VarIELen = 0;
+	}
+
+	pBss->AddHtInfoLen = 0;
+	pBss->HtCapabilityLen = 0;
+#ifdef DOT11_N_SUPPORT
+	if (HtCapabilityLen> 0)
+	{
+		pBss->HtCapabilityLen = HtCapabilityLen;
+		NdisMoveMemory(&pBss->HtCapability, pHtCapability, HtCapabilityLen);
+		if (AddHtInfoLen > 0)
+		{
+			pBss->AddHtInfoLen = AddHtInfoLen;
+			NdisMoveMemory(&pBss->AddHtInfo, pAddHtInfo, AddHtInfoLen);
+
+	 			if ((pAddHtInfo->ControlChan > 2)&& (pAddHtInfo->AddHtInfo.ExtChanOffset == EXTCHA_BELOW) && (pHtCapability->HtCapInfo.ChannelWidth == BW_40))
+	 			{
+	 				pBss->CentralChannel = pAddHtInfo->ControlChan - 2;
+	 			}
+	 			else if ((pAddHtInfo->AddHtInfo.ExtChanOffset == EXTCHA_ABOVE) && (pHtCapability->HtCapInfo.ChannelWidth == BW_40))
+				{
+		 				pBss->CentralChannel = pAddHtInfo->ControlChan + 2;
+				}
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	BssCipherParse(pBss);
+
+	// new for QOS
+	if (pEdcaParm)
+		NdisMoveMemory(&pBss->EdcaParm, pEdcaParm, sizeof(EDCA_PARM));
+	else
+		pBss->EdcaParm.bValid = FALSE;
+	if (pQosCapability)
+		NdisMoveMemory(&pBss->QosCapability, pQosCapability, sizeof(QOS_CAPABILITY_PARM));
+	else
+		pBss->QosCapability.bValid = FALSE;
+	if (pQbssLoad)
+		NdisMoveMemory(&pBss->QbssLoad, pQbssLoad, sizeof(QBSS_LOAD_PARM));
+	else
+		pBss->QbssLoad.bValid = FALSE;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		PEID_STRUCT     pEid;
+		USHORT          Length = 0;
+
+
+		NdisZeroMemory(&pBss->WpaIE.IE[0], MAX_CUSTOM_LEN);
+		NdisZeroMemory(&pBss->RsnIE.IE[0], MAX_CUSTOM_LEN);
+#ifdef EXT_BUILD_CHANNEL_LIST
+		NdisZeroMemory(&pBss->CountryString[0], 3);
+		pBss->bHasCountryIE = FALSE;
+#endif // EXT_BUILD_CHANNEL_LIST //
+		pEid = (PEID_STRUCT) pVIE;
+		while ((Length + 2 + (USHORT)pEid->Len) <= LengthVIE)
+		{
+			switch(pEid->Eid)
+			{
+				case IE_WPA:
+					if (NdisEqualMemory(pEid->Octet, WPA_OUI, 4))
+					{
+						if ((pEid->Len + 2) > MAX_CUSTOM_LEN)
+						{
+							pBss->WpaIE.IELen = 0;
+							break;
+						}
+						pBss->WpaIE.IELen = pEid->Len + 2;
+						NdisMoveMemory(pBss->WpaIE.IE, pEid, pBss->WpaIE.IELen);
+					}
+					break;
+                case IE_RSN:
+                    if (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3))
+					{
+						if ((pEid->Len + 2) > MAX_CUSTOM_LEN)
+						{
+							pBss->RsnIE.IELen = 0;
+							break;
+						}
+						pBss->RsnIE.IELen = pEid->Len + 2;
+						NdisMoveMemory(pBss->RsnIE.IE, pEid, pBss->RsnIE.IELen);
+			}
+				break;
+#ifdef EXT_BUILD_CHANNEL_LIST
+				case IE_COUNTRY:
+					NdisMoveMemory(&pBss->CountryString[0], pEid->Octet, 3);
+					pBss->bHasCountryIE = TRUE;
+					break;
+#endif // EXT_BUILD_CHANNEL_LIST //
+            }
+			Length = Length + 2 + (USHORT)pEid->Len;  // Eid[1] + Len[1]+ content[Len]
+			pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+}
+
+/*!
+ *	\brief insert an entry into the bss table
+ *	\param p_tab The BSS table
+ *	\param Bssid BSSID
+ *	\param ssid SSID
+ *	\param ssid_len Length of SSID
+ *	\param bss_type
+ *	\param beacon_period
+ *	\param timestamp
+ *	\param p_cf
+ *	\param atim_win
+ *	\param cap
+ *	\param rates
+ *	\param rates_len
+ *	\param channel_idx
+ *	\return none
+ *	\pre
+ *	\post
+ *	\note If SSID is identical, the old entry will be replaced by the new one
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+ULONG BssTableSetEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT BSS_TABLE *Tab,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN CF_PARM *CfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR ChannelNo,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE)
+{
+	ULONG	Idx;
+
+	Idx = BssTableSearchWithSSID(Tab, pBssid,  Ssid, SsidLen, ChannelNo);
+	if (Idx == BSS_NOT_FOUND)
+	{
+		if (Tab->BssNr >= MAX_LEN_OF_BSS_TABLE)
+	    {
+			//
+			// It may happen when BSS Table was full.
+			// The desired AP will not be added into BSS Table
+			// In this case, if we found the desired AP then overwrite BSS Table.
+			//
+			if(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+			{
+				if (MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, pBssid) ||
+					SSID_EQUAL(pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen, Ssid, SsidLen))
+				{
+					Idx = Tab->BssOverlapNr;
+					BssEntrySet(pAd, &Tab->BssEntry[Idx], pBssid, Ssid, SsidLen, BssType, BeaconPeriod, CfParm, AtimWin,
+						CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,pHtCapability, pAddHtInfo,HtCapabilityLen, AddHtInfoLen,
+						NewExtChanOffset, ChannelNo, Rssi, TimeStamp, CkipFlag, pEdcaParm, pQosCapability, pQbssLoad, LengthVIE, pVIE);
+                    Tab->BssOverlapNr = (Tab->BssOverlapNr++) % MAX_LEN_OF_BSS_TABLE;
+				}
+				return Idx;
+			}
+			else
+			{
+			return BSS_NOT_FOUND;
+			}
+		}
+		Idx = Tab->BssNr;
+		BssEntrySet(pAd, &Tab->BssEntry[Idx], pBssid, Ssid, SsidLen, BssType, BeaconPeriod, CfParm, AtimWin,
+					CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,pHtCapability, pAddHtInfo,HtCapabilityLen, AddHtInfoLen,
+					NewExtChanOffset, ChannelNo, Rssi, TimeStamp, CkipFlag, pEdcaParm, pQosCapability, pQbssLoad, LengthVIE, pVIE);
+		Tab->BssNr++;
+	}
+	else
+	{
+		BssEntrySet(pAd, &Tab->BssEntry[Idx], pBssid, Ssid, SsidLen, BssType, BeaconPeriod,CfParm, AtimWin,
+					CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,pHtCapability, pAddHtInfo,HtCapabilityLen, AddHtInfoLen,
+					NewExtChanOffset, ChannelNo, Rssi, TimeStamp, CkipFlag, pEdcaParm, pQosCapability, pQbssLoad, LengthVIE, pVIE);
+	}
+
+	return Idx;
+}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+VOID  TriEventInit(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR		i;
+
+	for (i = 0;i < MAX_TRIGGER_EVENT;i++)
+		pAd->CommonCfg.TriggerEventTab.EventA[i].bValid = FALSE;
+
+	pAd->CommonCfg.TriggerEventTab.EventANo = 0;
+	pAd->CommonCfg.TriggerEventTab.EventBCountDown = 0;
+}
+
+ULONG TriEventTableSetEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT TRIGGER_EVENT_TAB *Tab,
+	IN PUCHAR pBssid,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			RegClass,
+	IN UCHAR ChannelNo)
+{
+	// Event A
+	if (HtCapabilityLen == 0)
+	{
+		if (Tab->EventANo < MAX_TRIGGER_EVENT)
+		{
+			RTMPMoveMemory(Tab->EventA[Tab->EventANo].BSSID, pBssid, 6);
+			Tab->EventA[Tab->EventANo].bValid = TRUE;
+			Tab->EventA[Tab->EventANo].Channel = ChannelNo;
+			Tab->EventA[Tab->EventANo].CDCounter = pAd->CommonCfg.Dot11BssWidthChanTranDelay;
+			if (RegClass != 0)
+			{
+				// Beacon has Regulatory class IE. So use beacon's
+				Tab->EventA[Tab->EventANo].RegClass = RegClass;
+			}
+			else
+			{
+				// Use Station's Regulatory class instead.
+				if (pAd->StaActive.SupportedHtPhy.bHtEnable == TRUE)
+				{
+					if (pAd->CommonCfg.CentralChannel > pAd->CommonCfg.Channel)
+					{
+						Tab->EventA[Tab->EventANo].RegClass = 32;
+					}
+					else if (pAd->CommonCfg.CentralChannel < pAd->CommonCfg.Channel)
+						Tab->EventA[Tab->EventANo].RegClass = 33;
+				}
+				else
+					Tab->EventA[Tab->EventANo].RegClass = ??;
+
+			}
+
+			Tab->EventANo ++;
+		}
+	}
+	else if (pHtCapability->HtCapInfo.Intolerant40)
+	{
+		Tab->EventBCountDown = pAd->CommonCfg.Dot11BssWidthChanTranDelay;
+	}
+
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Trigger Event table Maintainence called once every second.
+
+	Arguments:
+	// IRQL = DISPATCH_LEVEL
+	========================================================================
+*/
+VOID TriEventCounterMaintenance(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR		i;
+	BOOLEAN			bNotify = FALSE;
+	for (i = 0;i < MAX_TRIGGER_EVENT;i++)
+	{
+		if (pAd->CommonCfg.TriggerEventTab.EventA[i].bValid && (pAd->CommonCfg.TriggerEventTab.EventA[i].CDCounter > 0))
+		{
+			pAd->CommonCfg.TriggerEventTab.EventA[i].CDCounter--;
+			if (pAd->CommonCfg.TriggerEventTab.EventA[i].CDCounter == 0)
+			{
+				pAd->CommonCfg.TriggerEventTab.EventA[i].bValid = FALSE;
+				pAd->CommonCfg.TriggerEventTab.EventANo --;
+				// Need to send 20/40 Coexistence Notify frame if has status change.
+				bNotify = TRUE;
+			}
+		}
+	}
+	if (pAd->CommonCfg.TriggerEventTab.EventBCountDown > 0)
+	{
+		pAd->CommonCfg.TriggerEventTab.EventBCountDown--;
+		if (pAd->CommonCfg.TriggerEventTab.EventBCountDown == 0)
+			bNotify = TRUE;
+	}
+
+	if (bNotify == TRUE)
+		Update2040CoexistFrameAndNotify(pAd, BSSID_WCID, TRUE);
+}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+// IRQL = DISPATCH_LEVEL
+VOID BssTableSsidSort(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT BSS_TABLE *OutTab,
+	IN	CHAR Ssid[],
+	IN	UCHAR SsidLen)
+{
+	INT i;
+	BssTableInit(OutTab);
+
+	for (i = 0; i < pAd->ScanTab.BssNr; i++)
+	{
+		BSS_ENTRY *pInBss = &pAd->ScanTab.BssEntry[i];
+		BOOLEAN	bIsHiddenApIncluded = FALSE;
+
+		if (((pAd->CommonCfg.bIEEE80211H == 1) &&
+            (pAd->MlmeAux.Channel > 14) &&
+             RadarChannelCheck(pAd, pInBss->Channel))
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+             || (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+#endif // CARRIER_DETECTION_SUPPORT //
+            )
+		{
+			if (pInBss->Hidden)
+				bIsHiddenApIncluded = TRUE;
+		}
+
+		if ((pInBss->BssType == pAd->StaCfg.BssType) &&
+			(SSID_EQUAL(Ssid, SsidLen, pInBss->Ssid, pInBss->SsidLen) || bIsHiddenApIncluded))
+		{
+			BSS_ENTRY *pOutBss = &OutTab->BssEntry[OutTab->BssNr];
+
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+			// If no Country IE exists no Connection will be established when IEEE80211dClientMode is strict.
+			if ((pAd->StaCfg.IEEE80211dClientMode == Rt802_11_D_Strict) &&
+				(pInBss->bHasCountryIE == FALSE))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("StaCfg.IEEE80211dClientMode == Rt802_11_D_Strict, but this AP doesn't have country IE.\n"));
+				continue;
+			}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef DOT11_N_SUPPORT
+			// 2.4G/5G N only mode
+			if ((pInBss->HtCapabilityLen == 0) &&
+				((pAd->CommonCfg.PhyMode == PHY_11N_2_4G) || (pAd->CommonCfg.PhyMode == PHY_11N_5G)))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("STA is in N-only Mode, this AP don't have Ht capability in Beacon.\n"));
+				continue;
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// New for WPA2
+			// Check the Authmode first
+			if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+			{
+				// Check AuthMode and AuthModeAux for matching, in case AP support dual-mode
+				if ((pAd->StaCfg.AuthMode != pInBss->AuthMode) && (pAd->StaCfg.AuthMode != pInBss->AuthModeAux))
+					// None matched
+					continue;
+
+				// Check cipher suite, AP must have more secured cipher than station setting
+				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA.GroupCipher)
+							continue;
+
+					// check group cipher
+					if ((pAd->StaCfg.WepStatus < pInBss->WPA.GroupCipher) &&
+						(pInBss->WPA.GroupCipher != Ndis802_11GroupWEP40Enabled) &&
+						(pInBss->WPA.GroupCipher != Ndis802_11GroupWEP104Enabled))
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipherAux))
+						continue;
+				}
+				else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA2.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA2.GroupCipher)
+							continue;
+
+					// check group cipher
+					if ((pAd->StaCfg.WepStatus < pInBss->WPA.GroupCipher) &&
+						(pInBss->WPA2.GroupCipher != Ndis802_11GroupWEP40Enabled) &&
+						(pInBss->WPA2.GroupCipher != Ndis802_11GroupWEP104Enabled))
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipherAux))
+						continue;
+				}
+			}
+			// Bss Type matched, SSID matched.
+			// We will check wepstatus for qualification Bss
+			else if (pAd->StaCfg.WepStatus != pInBss->WepStatus)
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("StaCfg.WepStatus=%d, while pInBss->WepStatus=%d\n", pAd->StaCfg.WepStatus, pInBss->WepStatus));
+				//
+				// For the SESv2 case, we will not qualify WepStatus.
+				//
+				if (!pInBss->bSES)
+					continue;
+			}
+
+			// Since the AP is using hidden SSID, and we are trying to connect to ANY
+			// It definitely will fail. So, skip it.
+			// CCX also require not even try to connect it!!
+			if (SsidLen == 0)
+				continue;
+
+#ifdef DOT11_N_SUPPORT
+			// If both station and AP use 40MHz, still need to check if the 40MHZ band's legality in my country region
+			// If this 40MHz wideband is not allowed in my country list, use bandwidth 20MHZ instead,
+			if ((pInBss->CentralChannel != pInBss->Channel) &&
+				(pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40))
+			{
+				if (RTMPCheckChannel(pAd, pInBss->CentralChannel, pInBss->Channel) == FALSE)
+				{
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_20;
+					SetCommonHT(pAd);
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_40;
+				}
+				else
+				{
+					if (pAd->CommonCfg.DesiredHtPhy.ChannelWidth == BAND_WIDTH_20)
+					{
+						SetCommonHT(pAd);
+					}
+				}
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// copy matching BSS from InTab to OutTab
+			NdisMoveMemory(pOutBss, pInBss, sizeof(BSS_ENTRY));
+
+			OutTab->BssNr++;
+		}
+		else if ((pInBss->BssType == pAd->StaCfg.BssType) && (SsidLen == 0))
+		{
+			BSS_ENTRY *pOutBss = &OutTab->BssEntry[OutTab->BssNr];
+
+
+#ifdef DOT11_N_SUPPORT
+			// 2.4G/5G N only mode
+			if ((pInBss->HtCapabilityLen == 0) &&
+				((pAd->CommonCfg.PhyMode == PHY_11N_2_4G) || (pAd->CommonCfg.PhyMode == PHY_11N_5G)))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("STA is in N-only Mode, this AP don't have Ht capability in Beacon.\n"));
+				continue;
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// New for WPA2
+			// Check the Authmode first
+			if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+			{
+				// Check AuthMode and AuthModeAux for matching, in case AP support dual-mode
+				if ((pAd->StaCfg.AuthMode != pInBss->AuthMode) && (pAd->StaCfg.AuthMode != pInBss->AuthModeAux))
+					// None matched
+					continue;
+
+				// Check cipher suite, AP must have more secured cipher than station setting
+				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA.GroupCipher)
+							continue;
+
+					// check group cipher
+					if (pAd->StaCfg.WepStatus < pInBss->WPA.GroupCipher)
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipherAux))
+						continue;
+				}
+				else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA2.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA2.GroupCipher)
+							continue;
+
+					// check group cipher
+					if (pAd->StaCfg.WepStatus < pInBss->WPA2.GroupCipher)
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipherAux))
+						continue;
+				}
+			}
+			// Bss Type matched, SSID matched.
+			// We will check wepstatus for qualification Bss
+			else if (pAd->StaCfg.WepStatus != pInBss->WepStatus)
+					continue;
+
+#ifdef DOT11_N_SUPPORT
+			// If both station and AP use 40MHz, still need to check if the 40MHZ band's legality in my country region
+			// If this 40MHz wideband is not allowed in my country list, use bandwidth 20MHZ instead,
+			if ((pInBss->CentralChannel != pInBss->Channel) &&
+				(pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40))
+			{
+				if (RTMPCheckChannel(pAd, pInBss->CentralChannel, pInBss->Channel) == FALSE)
+				{
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_20;
+					SetCommonHT(pAd);
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_40;
+				}
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// copy matching BSS from InTab to OutTab
+			NdisMoveMemory(pOutBss, pInBss, sizeof(BSS_ENTRY));
+
+			OutTab->BssNr++;
+		}
+
+		if (OutTab->BssNr >= MAX_LEN_OF_BSS_TABLE)
+			break;
+	}
+
+	BssTableSortByRssi(OutTab);
+}
+
+
+// IRQL = DISPATCH_LEVEL
+VOID BssTableSortByRssi(
+	IN OUT BSS_TABLE *OutTab)
+{
+	INT 	  i, j;
+	BSS_ENTRY TmpBss;
+
+	for (i = 0; i < OutTab->BssNr - 1; i++)
+	{
+		for (j = i+1; j < OutTab->BssNr; j++)
+		{
+			if (OutTab->BssEntry[j].Rssi > OutTab->BssEntry[i].Rssi)
+			{
+				NdisMoveMemory(&TmpBss, &OutTab->BssEntry[j], sizeof(BSS_ENTRY));
+				NdisMoveMemory(&OutTab->BssEntry[j], &OutTab->BssEntry[i], sizeof(BSS_ENTRY));
+				NdisMoveMemory(&OutTab->BssEntry[i], &TmpBss, sizeof(BSS_ENTRY));
+			}
+		}
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+VOID BssCipherParse(
+	IN OUT	PBSS_ENTRY	pBss)
+{
+	PEID_STRUCT 		 pEid;
+	PUCHAR				pTmp;
+	PRSN_IE_HEADER_STRUCT			pRsnHeader;
+	PCIPHER_SUITE_STRUCT			pCipher;
+	PAKM_SUITE_STRUCT				pAKM;
+	USHORT							Count;
+	INT								Length;
+	NDIS_802_11_ENCRYPTION_STATUS	TmpCipher;
+
+	//
+	// WepStatus will be reset later, if AP announce TKIP or AES on the beacon frame.
+	//
+	if (pBss->Privacy)
+	{
+		pBss->WepStatus 	= Ndis802_11WEPEnabled;
+	}
+	else
+	{
+		pBss->WepStatus 	= Ndis802_11WEPDisabled;
+	}
+	// Set default to disable & open authentication before parsing variable IE
+	pBss->AuthMode		= Ndis802_11AuthModeOpen;
+	pBss->AuthModeAux	= Ndis802_11AuthModeOpen;
+
+	// Init WPA setting
+	pBss->WPA.PairCipher	= Ndis802_11WEPDisabled;
+	pBss->WPA.PairCipherAux = Ndis802_11WEPDisabled;
+	pBss->WPA.GroupCipher	= Ndis802_11WEPDisabled;
+	pBss->WPA.RsnCapability = 0;
+	pBss->WPA.bMixMode		= FALSE;
+
+	// Init WPA2 setting
+	pBss->WPA2.PairCipher	 = Ndis802_11WEPDisabled;
+	pBss->WPA2.PairCipherAux = Ndis802_11WEPDisabled;
+	pBss->WPA2.GroupCipher	 = Ndis802_11WEPDisabled;
+	pBss->WPA2.RsnCapability = 0;
+	pBss->WPA2.bMixMode 	 = FALSE;
+
+
+	Length = (INT) pBss->VarIELen;
+
+	while (Length > 0)
+	{
+		// Parse cipher suite base on WPA1 & WPA2, they should be parsed differently
+		pTmp = ((PUCHAR) pBss->VarIEs) + pBss->VarIELen - Length;
+		pEid = (PEID_STRUCT) pTmp;
+		switch (pEid->Eid)
+		{
+			case IE_WPA:
+				//Parse Cisco IE_WPA (LEAP, CCKM, etc.)
+				if ( NdisEqualMemory((pTmp+8), CISCO_OUI, 3))
+				{
+					pTmp   += 11;
+					switch (*pTmp)
+					{
+						case 1:
+						case 5:	// Although WEP is not allowed in WPA related auth mode, we parse it anyway
+							pBss->WepStatus = Ndis802_11Encryption1Enabled;
+							pBss->WPA.PairCipher = Ndis802_11Encryption1Enabled;
+							pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 2:
+							pBss->WepStatus = Ndis802_11Encryption2Enabled;
+							pBss->WPA.PairCipher = Ndis802_11Encryption1Enabled;
+							pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 4:
+							pBss->WepStatus = Ndis802_11Encryption3Enabled;
+							pBss->WPA.PairCipher = Ndis802_11Encryption1Enabled;
+							pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+							break;
+						default:
+							break;
+					}
+
+					// if Cisco IE_WPA, break
+					break;
+				}
+				else if (NdisEqualMemory(pEid->Octet, SES_OUI, 3) && (pEid->Len == 7))
+				{
+					pBss->bSES = TRUE;
+					break;
+				}
+				else if (NdisEqualMemory(pEid->Octet, WPA_OUI, 4) != 1)
+				{
+					// if unsupported vendor specific IE
+					break;
+				}
+				// Skip OUI, version, and multicast suite
+				// This part should be improved in the future when AP supported multiple cipher suite.
+				// For now, it's OK since almost all APs have fixed cipher suite supported.
+				// pTmp = (PUCHAR) pEid->Octet;
+				pTmp   += 11;
+
+				// Cipher Suite Selectors from Spec P802.11i/D3.2 P26.
+				//	Value	   Meaning
+				//	0			None
+				//	1			WEP-40
+				//	2			Tkip
+				//	3			WRAP
+				//	4			AES
+				//	5			WEP-104
+				// Parse group cipher
+				switch (*pTmp)
+				{
+					case 1:
+						pBss->WPA.GroupCipher = Ndis802_11GroupWEP40Enabled;
+						break;
+					case 5:
+						pBss->WPA.GroupCipher = Ndis802_11GroupWEP104Enabled;
+						break;
+					case 2:
+						pBss->WPA.GroupCipher = Ndis802_11Encryption2Enabled;
+						break;
+					case 4:
+						pBss->WPA.GroupCipher = Ndis802_11Encryption3Enabled;
+						break;
+					default:
+						break;
+				}
+				// number of unicast suite
+				pTmp   += 1;
+
+				// skip all unicast cipher suites
+				//Count = *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+
+				// Parsing all unicast cipher suite
+				while (Count > 0)
+				{
+					// Skip OUI
+					pTmp += 3;
+					TmpCipher = Ndis802_11WEPDisabled;
+					switch (*pTmp)
+					{
+						case 1:
+						case 5: // Although WEP is not allowed in WPA related auth mode, we parse it anyway
+							TmpCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 2:
+							TmpCipher = Ndis802_11Encryption2Enabled;
+							break;
+						case 4:
+							TmpCipher = Ndis802_11Encryption3Enabled;
+							break;
+						default:
+							break;
+					}
+					if (TmpCipher > pBss->WPA.PairCipher)
+					{
+						// Move the lower cipher suite to PairCipherAux
+						pBss->WPA.PairCipherAux = pBss->WPA.PairCipher;
+						pBss->WPA.PairCipher	= TmpCipher;
+					}
+					else
+					{
+						pBss->WPA.PairCipherAux = TmpCipher;
+					}
+					pTmp++;
+					Count--;
+				}
+
+				// 4. get AKM suite counts
+				//Count	= *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+				pTmp   += 3;
+
+				switch (*pTmp)
+				{
+					case 1:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPA;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPA;
+						break;
+					case 2:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPAPSK;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPAPSK;
+						break;
+					default:
+						break;
+				}
+				pTmp   += 1;
+
+				// Fixed for WPA-None
+				if (pBss->BssType == BSS_ADHOC)
+				{
+					pBss->AuthMode	  = Ndis802_11AuthModeWPANone;
+					pBss->AuthModeAux = Ndis802_11AuthModeWPANone;
+					pBss->WepStatus   = pBss->WPA.GroupCipher;
+					// Patched bugs for old driver
+					if (pBss->WPA.PairCipherAux == Ndis802_11WEPDisabled)
+						pBss->WPA.PairCipherAux = pBss->WPA.GroupCipher;
+				}
+				else
+					pBss->WepStatus   = pBss->WPA.PairCipher;
+
+				// Check the Pair & Group, if different, turn on mixed mode flag
+				if (pBss->WPA.GroupCipher != pBss->WPA.PairCipher)
+					pBss->WPA.bMixMode = TRUE;
+
+				break;
+
+			case IE_RSN:
+				pRsnHeader = (PRSN_IE_HEADER_STRUCT) pTmp;
+
+				// 0. Version must be 1
+				if (le2cpu16(pRsnHeader->Version) != 1)
+					break;
+				pTmp   += sizeof(RSN_IE_HEADER_STRUCT);
+
+				// 1. Check group cipher
+				pCipher = (PCIPHER_SUITE_STRUCT) pTmp;
+				if (!RTMPEqualMemory(pTmp, RSN_OUI, 3))
+					break;
+
+				// Parse group cipher
+				switch (pCipher->Type)
+				{
+					case 1:
+						pBss->WPA2.GroupCipher = Ndis802_11GroupWEP40Enabled;
+						break;
+					case 5:
+						pBss->WPA2.GroupCipher = Ndis802_11GroupWEP104Enabled;
+						break;
+					case 2:
+						pBss->WPA2.GroupCipher = Ndis802_11Encryption2Enabled;
+						break;
+					case 4:
+						pBss->WPA2.GroupCipher = Ndis802_11Encryption3Enabled;
+						break;
+					default:
+						break;
+				}
+				// set to correct offset for next parsing
+				pTmp   += sizeof(CIPHER_SUITE_STRUCT);
+
+				// 2. Get pairwise cipher counts
+				//Count = *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+
+				// 3. Get pairwise cipher
+				// Parsing all unicast cipher suite
+				while (Count > 0)
+				{
+					// Skip OUI
+					pCipher = (PCIPHER_SUITE_STRUCT) pTmp;
+					TmpCipher = Ndis802_11WEPDisabled;
+					switch (pCipher->Type)
+					{
+						case 1:
+						case 5: // Although WEP is not allowed in WPA related auth mode, we parse it anyway
+							TmpCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 2:
+							TmpCipher = Ndis802_11Encryption2Enabled;
+							break;
+						case 4:
+							TmpCipher = Ndis802_11Encryption3Enabled;
+							break;
+						default:
+							break;
+					}
+					if (TmpCipher > pBss->WPA2.PairCipher)
+					{
+						// Move the lower cipher suite to PairCipherAux
+						pBss->WPA2.PairCipherAux = pBss->WPA2.PairCipher;
+						pBss->WPA2.PairCipher	 = TmpCipher;
+					}
+					else
+					{
+						pBss->WPA2.PairCipherAux = TmpCipher;
+					}
+					pTmp += sizeof(CIPHER_SUITE_STRUCT);
+					Count--;
+				}
+
+				// 4. get AKM suite counts
+				//Count	= *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+
+				// 5. Get AKM ciphers
+				pAKM = (PAKM_SUITE_STRUCT) pTmp;
+				if (!RTMPEqualMemory(pTmp, RSN_OUI, 3))
+					break;
+
+				switch (pAKM->Type)
+				{
+					case 1:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPA2;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPA2;
+						break;
+					case 2:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPA2PSK;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPA2PSK;
+						break;
+					default:
+						break;
+				}
+				pTmp   += (Count * sizeof(AKM_SUITE_STRUCT));
+
+				// Fixed for WPA-None
+				if (pBss->BssType == BSS_ADHOC)
+				{
+					pBss->AuthMode = Ndis802_11AuthModeWPANone;
+					pBss->AuthModeAux = Ndis802_11AuthModeWPANone;
+					pBss->WPA.PairCipherAux = pBss->WPA2.PairCipherAux;
+					pBss->WPA.GroupCipher	= pBss->WPA2.GroupCipher;
+					pBss->WepStatus 		= pBss->WPA.GroupCipher;
+					// Patched bugs for old driver
+					if (pBss->WPA.PairCipherAux == Ndis802_11WEPDisabled)
+						pBss->WPA.PairCipherAux = pBss->WPA.GroupCipher;
+				}
+				pBss->WepStatus   = pBss->WPA2.PairCipher;
+
+				// 6. Get RSN capability
+				//pBss->WPA2.RsnCapability = *(PUSHORT) pTmp;
+				pBss->WPA2.RsnCapability = (pTmp[1]<<8) + pTmp[0];
+				pTmp += sizeof(USHORT);
+
+				// Check the Pair & Group, if different, turn on mixed mode flag
+				if (pBss->WPA2.GroupCipher != pBss->WPA2.PairCipher)
+					pBss->WPA2.bMixMode = TRUE;
+
+				break;
+			default:
+				break;
+		}
+		Length -= (pEid->Len + 2);
+	}
+}
+
+// ===========================================================================================
+// mac_table.c
+// ===========================================================================================
+
+/*! \brief generates a random mac address value for IBSS BSSID
+ *	\param Addr the bssid location
+ *	\return none
+ *	\pre
+ *	\post
+ */
+VOID MacAddrRandomBssid(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pAddr)
+{
+	INT i;
+
+	for (i = 0; i < MAC_ADDR_LEN; i++)
+	{
+		pAddr[i] = RandomByte(pAd);
+	}
+
+	pAddr[0] = (pAddr[0] & 0xfe) | 0x02;  // the first 2 bits must be 01xxxxxxxx
+}
+
+/*! \brief init the management mac frame header
+ *	\param p_hdr mac header
+ *	\param subtype subtype of the frame
+ *	\param p_ds destination address, don't care if it is a broadcast address
+ *	\return none
+ *	\pre the station has the following information in the pAd->StaCfg
+ *	 - bssid
+ *	 - station address
+ *	\post
+ *	\note this function initializes the following field
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID MgtMacHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PHEADER_802_11 pHdr80211,
+	IN UCHAR SubType,
+	IN UCHAR ToDs,
+	IN PUCHAR pDA,
+	IN PUCHAR pBssid)
+{
+	NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+
+	pHdr80211->FC.Type = BTYPE_MGMT;
+	pHdr80211->FC.SubType = SubType;
+//	if (SubType == SUBTYPE_ACK)	// sample, no use, it will conflict with ACTION frame sub type
+//		pHdr80211->FC.Type = BTYPE_CNTL;
+	pHdr80211->FC.ToDs = ToDs;
+	COPY_MAC_ADDR(pHdr80211->Addr1, pDA);
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+	COPY_MAC_ADDR(pHdr80211->Addr3, pBssid);
+}
+
+// ===========================================================================================
+// mem_mgmt.c
+// ===========================================================================================
+
+/*!***************************************************************************
+ * This routine build an outgoing frame, and fill all information specified
+ * in argument list to the frame body. The actual frame size is the summation
+ * of all arguments.
+ * input params:
+ *		Buffer - pointer to a pre-allocated memory segment
+ *		args - a list of <int arg_size, arg> pairs.
+ *		NOTE NOTE NOTE!!!! the last argument must be NULL, otherwise this
+ *						   function will FAIL!!!
+ * return:
+ *		Size of the buffer
+ * usage:
+ *		MakeOutgoingFrame(Buffer, output_length, 2, &fc, 2, &dur, 6, p_addr1, 6,p_addr2, END_OF_ARGS);
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ ****************************************************************************/
+ULONG MakeOutgoingFrame(
+	OUT CHAR *Buffer,
+	OUT ULONG *FrameLen, ...)
+{
+	CHAR   *p;
+	int 	leng;
+	ULONG	TotLeng;
+	va_list Args;
+
+	// calculates the total length
+	TotLeng = 0;
+	va_start(Args, FrameLen);
+	do
+	{
+		leng = va_arg(Args, int);
+		if (leng == END_OF_ARGS)
+		{
+			break;
+		}
+		p = va_arg(Args, PVOID);
+		NdisMoveMemory(&Buffer[TotLeng], p, leng);
+		TotLeng = TotLeng + leng;
+	} while(TRUE);
+
+	va_end(Args); /* clean up */
+	*FrameLen = TotLeng;
+	return TotLeng;
+}
+
+// ===========================================================================================
+// mlme_queue.c
+// ===========================================================================================
+
+/*! \brief	Initialize The MLME Queue, used by MLME Functions
+ *	\param	*Queue	   The MLME Queue
+ *	\return Always	   Return NDIS_STATE_SUCCESS in this implementation
+ *	\pre
+ *	\post
+ *	\note	Because this is done only once (at the init stage), no need to be locked
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+NDIS_STATUS MlmeQueueInit(
+	IN MLME_QUEUE *Queue)
+{
+	INT i;
+
+	NdisAllocateSpinLock(&Queue->Lock);
+
+	Queue->Num	= 0;
+	Queue->Head = 0;
+	Queue->Tail = 0;
+
+	for (i = 0; i < MAX_LEN_OF_MLME_QUEUE; i++)
+	{
+		Queue->Entry[i].Occupied = FALSE;
+		Queue->Entry[i].MsgLen = 0;
+		NdisZeroMemory(Queue->Entry[i].Msg, MGMT_DMA_BUFFER_SIZE);
+	}
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*! \brief	 Enqueue a message for other threads, if they want to send messages to MLME thread
+ *	\param	*Queue	  The MLME Queue
+ *	\param	 Machine  The State Machine Id
+ *	\param	 MsgType  The Message Type
+ *	\param	 MsgLen   The Message length
+ *	\param	*Msg	  The message pointer
+ *	\return  TRUE if enqueue is successful, FALSE if the queue is full
+ *	\pre
+ *	\post
+ *	\note	 The message has to be initialized
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeEnqueue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN ULONG Machine,
+	IN ULONG MsgType,
+	IN ULONG MsgLen,
+	IN VOID *Msg)
+{
+	INT Tail;
+	MLME_QUEUE	*Queue = (MLME_QUEUE *)&pAd->Mlme.Queue;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return FALSE;
+
+	// First check the size, it MUST not exceed the mlme queue size
+	if (MsgLen > MGMT_DMA_BUFFER_SIZE)
+	{
+		DBGPRINT_ERR(("MlmeEnqueue: msg too large, size = %ld \n", MsgLen));
+		return FALSE;
+	}
+
+	if (MlmeQueueFull(Queue))
+	{
+		return FALSE;
+	}
+
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Tail = Queue->Tail;
+	Queue->Tail++;
+	Queue->Num++;
+	if (Queue->Tail == MAX_LEN_OF_MLME_QUEUE)
+	{
+		Queue->Tail = 0;
+	}
+
+	Queue->Entry[Tail].Wcid = RESERVED_WCID;
+	Queue->Entry[Tail].Occupied = TRUE;
+	Queue->Entry[Tail].Machine = Machine;
+	Queue->Entry[Tail].MsgType = MsgType;
+	Queue->Entry[Tail].MsgLen  = MsgLen;
+
+	if (Msg != NULL)
+	{
+		NdisMoveMemory(Queue->Entry[Tail].Msg, Msg, MsgLen);
+	}
+
+	NdisReleaseSpinLock(&(Queue->Lock));
+	return TRUE;
+}
+
+/*! \brief	 This function is used when Recv gets a MLME message
+ *	\param	*Queue			 The MLME Queue
+ *	\param	 TimeStampHigh	 The upper 32 bit of timestamp
+ *	\param	 TimeStampLow	 The lower 32 bit of timestamp
+ *	\param	 Rssi			 The receiving RSSI strength
+ *	\param	 MsgLen 		 The length of the message
+ *	\param	*Msg			 The message pointer
+ *	\return  TRUE if everything ok, FALSE otherwise (like Queue Full)
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeEnqueueForRecv(
+	IN	PRTMP_ADAPTER	pAd,
+	IN ULONG Wcid,
+	IN ULONG TimeStampHigh,
+	IN ULONG TimeStampLow,
+	IN UCHAR Rssi0,
+	IN UCHAR Rssi1,
+	IN UCHAR Rssi2,
+	IN ULONG MsgLen,
+	IN VOID *Msg,
+	IN UCHAR Signal)
+{
+	INT 		 Tail, Machine;
+	PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+	INT		 MsgType;
+	MLME_QUEUE	*Queue = (MLME_QUEUE *)&pAd->Mlme.Queue;
+
+#ifdef RALINK_ATE
+	/* Nothing to do in ATE mode */
+	if(ATE_ON(pAd))
+		return FALSE;
+#endif // RALINK_ATE //
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		DBGPRINT_ERR(("MlmeEnqueueForRecv: fRTMP_ADAPTER_HALT_IN_PROGRESS\n"));
+		return FALSE;
+	}
+
+	// First check the size, it MUST not exceed the mlme queue size
+	if (MsgLen > MGMT_DMA_BUFFER_SIZE)
+	{
+		DBGPRINT_ERR(("MlmeEnqueueForRecv: frame too large, size = %ld \n", MsgLen));
+		return FALSE;
+	}
+
+	if (MlmeQueueFull(Queue))
+	{
+		return FALSE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (!MsgTypeSubst(pAd, pFrame, &Machine, &MsgType))
+		{
+			DBGPRINT_ERR(("MlmeEnqueueForRecv: un-recongnized mgmt->subtype=%d\n",pFrame->Hdr.FC.SubType));
+			return FALSE;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// OK, we got all the informations, it is time to put things into queue
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Tail = Queue->Tail;
+	Queue->Tail++;
+	Queue->Num++;
+	if (Queue->Tail == MAX_LEN_OF_MLME_QUEUE)
+	{
+		Queue->Tail = 0;
+	}
+	Queue->Entry[Tail].Occupied = TRUE;
+	Queue->Entry[Tail].Machine = Machine;
+	Queue->Entry[Tail].MsgType = MsgType;
+	Queue->Entry[Tail].MsgLen  = MsgLen;
+	Queue->Entry[Tail].TimeStamp.u.LowPart = TimeStampLow;
+	Queue->Entry[Tail].TimeStamp.u.HighPart = TimeStampHigh;
+	Queue->Entry[Tail].Rssi0 = Rssi0;
+	Queue->Entry[Tail].Rssi1 = Rssi1;
+	Queue->Entry[Tail].Rssi2 = Rssi2;
+	Queue->Entry[Tail].Signal = Signal;
+	Queue->Entry[Tail].Wcid = (UCHAR)Wcid;
+
+	Queue->Entry[Tail].Channel = pAd->LatchRfRegs.Channel;
+
+	if (Msg != NULL)
+	{
+		NdisMoveMemory(Queue->Entry[Tail].Msg, Msg, MsgLen);
+	}
+
+	NdisReleaseSpinLock(&(Queue->Lock));
+
+	RT28XX_MLME_HANDLER(pAd);
+
+	return TRUE;
+}
+
+
+/*! \brief	 Dequeue a message from the MLME Queue
+ *	\param	*Queue	  The MLME Queue
+ *	\param	*Elem	  The message dequeued from MLME Queue
+ *	\return  TRUE if the Elem contains something, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeDequeue(
+	IN MLME_QUEUE *Queue,
+	OUT MLME_QUEUE_ELEM **Elem)
+{
+	NdisAcquireSpinLock(&(Queue->Lock));
+	*Elem = &(Queue->Entry[Queue->Head]);
+	Queue->Num--;
+	Queue->Head++;
+	if (Queue->Head == MAX_LEN_OF_MLME_QUEUE)
+	{
+		Queue->Head = 0;
+	}
+	NdisReleaseSpinLock(&(Queue->Lock));
+	return TRUE;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID	MlmeRestartStateMachine(
+	IN	PRTMP_ADAPTER	pAd)
+{
+#ifdef CONFIG_STA_SUPPORT
+	BOOLEAN				Cancelled;
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("MlmeRestartStateMachine \n"));
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef QOS_DLS_SUPPORT
+		UCHAR i;
+#endif // QOS_DLS_SUPPORT //
+		// Cancel all timer events
+		// Be careful to cancel new added timer
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,	  &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,  &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,	   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,	   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,	   &Cancelled);
+
+#ifdef QOS_DLS_SUPPORT
+		for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &Cancelled);
+		}
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Change back to original channel in case of doing scan
+	AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+	// Resume MSDU which is turned off durning scan
+	RTMPResumeMsduTransmission(pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Set all state machines back IDLE
+		pAd->Mlme.CntlMachine.CurrState    = CNTL_IDLE;
+		pAd->Mlme.AssocMachine.CurrState   = ASSOC_IDLE;
+		pAd->Mlme.AuthMachine.CurrState    = AUTH_REQ_IDLE;
+		pAd->Mlme.AuthRspMachine.CurrState = AUTH_RSP_IDLE;
+		pAd->Mlme.SyncMachine.CurrState    = SYNC_IDLE;
+		pAd->Mlme.ActMachine.CurrState    = ACT_IDLE;
+#ifdef QOS_DLS_SUPPORT
+		pAd->Mlme.DlsMachine.CurrState    = DLS_IDLE;
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+/*! \brief	test if the MLME Queue is empty
+ *	\param	*Queue	  The MLME Queue
+ *	\return TRUE if the Queue is empty, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeQueueEmpty(
+	IN MLME_QUEUE *Queue)
+{
+	BOOLEAN Ans;
+
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Ans = (Queue->Num == 0);
+	NdisReleaseSpinLock(&(Queue->Lock));
+
+	return Ans;
+}
+
+/*! \brief	 test if the MLME Queue is full
+ *	\param	 *Queue 	 The MLME Queue
+ *	\return  TRUE if the Queue is empty, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeQueueFull(
+	IN MLME_QUEUE *Queue)
+{
+	BOOLEAN Ans;
+
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Ans = (Queue->Num == MAX_LEN_OF_MLME_QUEUE || Queue->Entry[Queue->Tail].Occupied);
+	NdisReleaseSpinLock(&(Queue->Lock));
+
+	return Ans;
+}
+
+/*! \brief	 The destructor of MLME Queue
+ *	\param
+ *	\return
+ *	\pre
+ *	\post
+ *	\note	Clear Mlme Queue, Set Queue->Num to Zero.
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+VOID MlmeQueueDestroy(
+	IN MLME_QUEUE *pQueue)
+{
+	NdisAcquireSpinLock(&(pQueue->Lock));
+	pQueue->Num  = 0;
+	pQueue->Head = 0;
+	pQueue->Tail = 0;
+	NdisReleaseSpinLock(&(pQueue->Lock));
+	NdisFreeSpinLock(&(pQueue->Lock));
+}
+
+/*! \brief	 To substitute the message type if the message is coming from external
+ *	\param	pFrame		   The frame received
+ *	\param	*Machine	   The state machine
+ *	\param	*MsgType	   the message type for the state machine
+ *	\return TRUE if the substitution is successful, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+#ifdef CONFIG_STA_SUPPORT
+BOOLEAN MsgTypeSubst(
+	IN PRTMP_ADAPTER  pAd,
+	IN PFRAME_802_11 pFrame,
+	OUT INT *Machine,
+	OUT INT *MsgType)
+{
+	USHORT	Seq;
+	UCHAR	EAPType;
+	PUCHAR	pData;
+
+	// Pointer to start of data frames including SNAP header
+	pData = (PUCHAR) pFrame + LENGTH_802_11;
+
+	// The only data type will pass to this function is EAPOL frame
+	if (pFrame->Hdr.FC.Type == BTYPE_DATA)
+	{
+		if (NdisEqualMemory(SNAP_AIRONET, pData, LENGTH_802_1_H))
+		{
+			// Cisco Aironet SNAP header
+			*Machine = AIRONET_STATE_MACHINE;
+			*MsgType = MT2_AIRONET_MSG;
+			return (TRUE);
+		}
+#ifdef LEAP_SUPPORT
+		if ( pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP ) //LEAP
+		{
+			// LEAP frames
+			*Machine = LEAP_STATE_MACHINE;
+			EAPType = *((UCHAR*)pFrame + LENGTH_802_11 + LENGTH_802_1_H + 1);
+			return (LeapMsgTypeSubst(EAPType, MsgType));
+		}
+		else
+#endif // LEAP_SUPPORT //
+		{
+			*Machine = WPA_PSK_STATE_MACHINE;
+			EAPType = *((UCHAR*)pFrame + LENGTH_802_11 + LENGTH_802_1_H + 1);
+			return(WpaMsgTypeSubst(EAPType, MsgType));
+		}
+	}
+
+	switch (pFrame->Hdr.FC.SubType)
+	{
+		case SUBTYPE_ASSOC_REQ:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_ASSOC_REQ;
+			break;
+		case SUBTYPE_ASSOC_RSP:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_ASSOC_RSP;
+			break;
+		case SUBTYPE_REASSOC_REQ:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_REASSOC_REQ;
+			break;
+		case SUBTYPE_REASSOC_RSP:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_REASSOC_RSP;
+			break;
+		case SUBTYPE_PROBE_REQ:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_PROBE_REQ;
+			break;
+		case SUBTYPE_PROBE_RSP:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_PROBE_RSP;
+			break;
+		case SUBTYPE_BEACON:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_BEACON;
+			break;
+		case SUBTYPE_ATIM:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_ATIM;
+			break;
+		case SUBTYPE_DISASSOC:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_DISASSOC_REQ;
+			break;
+		case SUBTYPE_AUTH:
+			// get the sequence number from payload 24 Mac Header + 2 bytes algorithm
+			NdisMoveMemory(&Seq, &pFrame->Octet[2], sizeof(USHORT));
+			if (Seq == 1 || Seq == 3)
+			{
+				*Machine = AUTH_RSP_STATE_MACHINE;
+				*MsgType = MT2_PEER_AUTH_ODD;
+			}
+			else if (Seq == 2 || Seq == 4)
+			{
+				*Machine = AUTH_STATE_MACHINE;
+				*MsgType = MT2_PEER_AUTH_EVEN;
+			}
+			else
+			{
+				return FALSE;
+			}
+			break;
+		case SUBTYPE_DEAUTH:
+			*Machine = AUTH_RSP_STATE_MACHINE;
+			*MsgType = MT2_PEER_DEAUTH;
+			break;
+		case SUBTYPE_ACTION:
+			*Machine = ACTION_STATE_MACHINE;
+			//  Sometimes Sta will return with category bytes with MSB = 1, if they receive catogory out of their support
+			if ((pFrame->Octet[0]&0x7F) > MAX_PEER_CATE_MSG)
+			{
+				*MsgType = MT2_ACT_INVALID;
+			}
+			else
+			{
+				*MsgType = (pFrame->Octet[0]&0x7F);
+			}
+			break;
+		default:
+			return FALSE;
+			break;
+	}
+
+	return TRUE;
+}
+#endif // CONFIG_STA_SUPPORT //
+
+// ===========================================================================================
+// state_machine.c
+// ===========================================================================================
+
+/*! \brief Initialize the state machine.
+ *	\param *S			pointer to the state machine
+ *	\param	Trans		State machine transition function
+ *	\param	StNr		number of states
+ *	\param	MsgNr		number of messages
+ *	\param	DefFunc 	default function, when there is invalid state/message combination
+ *	\param	InitState	initial state of the state machine
+ *	\param	Base		StateMachine base, internal use only
+ *	\pre p_sm should be a legal pointer
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+VOID StateMachineInit(
+	IN STATE_MACHINE *S,
+	IN STATE_MACHINE_FUNC Trans[],
+	IN ULONG StNr,
+	IN ULONG MsgNr,
+	IN STATE_MACHINE_FUNC DefFunc,
+	IN ULONG InitState,
+	IN ULONG Base)
+{
+	ULONG i, j;
+
+	// set number of states and messages
+	S->NrState = StNr;
+	S->NrMsg   = MsgNr;
+	S->Base    = Base;
+
+	S->TransFunc  = Trans;
+
+	// init all state transition to default function
+	for (i = 0; i < StNr; i++)
+	{
+		for (j = 0; j < MsgNr; j++)
+		{
+			S->TransFunc[i * MsgNr + j] = DefFunc;
+		}
+	}
+
+	// set the starting state
+	S->CurrState = InitState;
+}
+
+/*! \brief This function fills in the function pointer into the cell in the state machine
+ *	\param *S	pointer to the state machine
+ *	\param St	state
+ *	\param Msg	incoming message
+ *	\param f	the function to be executed when (state, message) combination occurs at the state machine
+ *	\pre *S should be a legal pointer to the state machine, st, msg, should be all within the range, Base should be set in the initial state
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+VOID StateMachineSetAction(
+	IN STATE_MACHINE *S,
+	IN ULONG St,
+	IN ULONG Msg,
+	IN STATE_MACHINE_FUNC Func)
+{
+	ULONG MsgIdx;
+
+	MsgIdx = Msg - S->Base;
+
+	if (St < S->NrState && MsgIdx < S->NrMsg)
+	{
+		// boundary checking before setting the action
+		S->TransFunc[St * S->NrMsg + MsgIdx] = Func;
+	}
+}
+
+/*! \brief	 This function does the state transition
+ *	\param	 *Adapter the NIC adapter pointer
+ *	\param	 *S 	  the state machine
+ *	\param	 *Elem	  the message to be executed
+ *	\return   None
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID StateMachinePerformAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN STATE_MACHINE *S,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	(*(S->TransFunc[S->CurrState * S->NrMsg + Elem->MsgType - S->Base]))(pAd, Elem);
+}
+
+/*
+	==========================================================================
+	Description:
+		The drop function, when machine executes this, the message is simply
+		ignored. This function does nothing, the message is freed in
+		StateMachinePerformAction()
+	==========================================================================
+ */
+VOID Drop(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+// ===========================================================================================
+// lfsr.c
+// ===========================================================================================
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID LfsrInit(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Seed)
+{
+	if (Seed == 0)
+		pAd->Mlme.ShiftReg = 1;
+	else
+		pAd->Mlme.ShiftReg = Seed;
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+UCHAR RandomByte(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG i;
+	UCHAR R, Result;
+
+	R = 0;
+
+	if (pAd->Mlme.ShiftReg == 0)
+	NdisGetSystemUpTime((ULONG *)&pAd->Mlme.ShiftReg);
+
+	for (i = 0; i < 8; i++)
+	{
+		if (pAd->Mlme.ShiftReg & 0x00000001)
+		{
+			pAd->Mlme.ShiftReg = ((pAd->Mlme.ShiftReg ^ LFSR_MASK) >> 1) | 0x80000000;
+			Result = 1;
+		}
+		else
+		{
+			pAd->Mlme.ShiftReg = pAd->Mlme.ShiftReg >> 1;
+			Result = 0;
+		}
+		R = (R << 1) | Result;
+	}
+
+	return R;
+}
+
+VOID AsicUpdateAutoFallBackTable(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pRateTable)
+{
+	UCHAR					i;
+	HT_FBK_CFG0_STRUC		HtCfg0;
+	HT_FBK_CFG1_STRUC		HtCfg1;
+	LG_FBK_CFG0_STRUC		LgCfg0;
+	LG_FBK_CFG1_STRUC		LgCfg1;
+	PRTMP_TX_RATE_SWITCH	pCurrTxRate, pNextTxRate;
+
+	// set to initial value
+	HtCfg0.word = 0x65432100;
+	HtCfg1.word = 0xedcba988;
+	LgCfg0.word = 0xedcba988;
+	LgCfg1.word = 0x00002100;
+
+	pNextTxRate = (PRTMP_TX_RATE_SWITCH)pRateTable+1;
+	for (i = 1; i < *((PUCHAR) pRateTable); i++)
+	{
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH)pRateTable+1+i;
+		switch (pCurrTxRate->Mode)
+		{
+			case 0:		//CCK
+				break;
+			case 1:		//OFDM
+				{
+					switch(pCurrTxRate->CurrMCS)
+					{
+						case 0:
+							LgCfg0.field.OFDMMCS0FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 1:
+							LgCfg0.field.OFDMMCS1FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 2:
+							LgCfg0.field.OFDMMCS2FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 3:
+							LgCfg0.field.OFDMMCS3FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 4:
+							LgCfg0.field.OFDMMCS4FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 5:
+							LgCfg0.field.OFDMMCS5FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 6:
+							LgCfg0.field.OFDMMCS6FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 7:
+							LgCfg0.field.OFDMMCS7FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+					}
+				}
+				break;
+#ifdef DOT11_N_SUPPORT
+			case 2:		//HT-MIX
+			case 3:		//HT-GF
+				{
+					if ((pNextTxRate->Mode >= MODE_HTMIX) && (pCurrTxRate->CurrMCS != pNextTxRate->CurrMCS))
+					{
+						switch(pCurrTxRate->CurrMCS)
+						{
+							case 0:
+								HtCfg0.field.HTMCS0FBK = pNextTxRate->CurrMCS;
+								break;
+							case 1:
+								HtCfg0.field.HTMCS1FBK = pNextTxRate->CurrMCS;
+								break;
+							case 2:
+								HtCfg0.field.HTMCS2FBK = pNextTxRate->CurrMCS;
+								break;
+							case 3:
+								HtCfg0.field.HTMCS3FBK = pNextTxRate->CurrMCS;
+								break;
+							case 4:
+								HtCfg0.field.HTMCS4FBK = pNextTxRate->CurrMCS;
+								break;
+							case 5:
+								HtCfg0.field.HTMCS5FBK = pNextTxRate->CurrMCS;
+								break;
+							case 6:
+								HtCfg0.field.HTMCS6FBK = pNextTxRate->CurrMCS;
+								break;
+							case 7:
+								HtCfg0.field.HTMCS7FBK = pNextTxRate->CurrMCS;
+								break;
+							case 8:
+								HtCfg1.field.HTMCS8FBK = pNextTxRate->CurrMCS;
+								break;
+							case 9:
+								HtCfg1.field.HTMCS9FBK = pNextTxRate->CurrMCS;
+								break;
+							case 10:
+								HtCfg1.field.HTMCS10FBK = pNextTxRate->CurrMCS;
+								break;
+							case 11:
+								HtCfg1.field.HTMCS11FBK = pNextTxRate->CurrMCS;
+								break;
+							case 12:
+								HtCfg1.field.HTMCS12FBK = pNextTxRate->CurrMCS;
+								break;
+							case 13:
+								HtCfg1.field.HTMCS13FBK = pNextTxRate->CurrMCS;
+								break;
+							case 14:
+								HtCfg1.field.HTMCS14FBK = pNextTxRate->CurrMCS;
+								break;
+							case 15:
+								HtCfg1.field.HTMCS15FBK = pNextTxRate->CurrMCS;
+								break;
+							default:
+								DBGPRINT(RT_DEBUG_ERROR, ("AsicUpdateAutoFallBackTable: not support CurrMCS=%d\n", pCurrTxRate->CurrMCS));
+						}
+					}
+				}
+				break;
+#endif // DOT11_N_SUPPORT //
+		}
+
+		pNextTxRate = pCurrTxRate;
+	}
+
+	RTMP_IO_WRITE32(pAd, HT_FBK_CFG0, HtCfg0.word);
+	RTMP_IO_WRITE32(pAd, HT_FBK_CFG1, HtCfg1.word);
+	RTMP_IO_WRITE32(pAd, LG_FBK_CFG0, LgCfg0.word);
+	RTMP_IO_WRITE32(pAd, LG_FBK_CFG1, LgCfg1.word);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set MAC register value according operation mode.
+		OperationMode AND bNonGFExist are for MM and GF Proteciton.
+		If MM or GF mask is not set, those passing argument doesn't not take effect.
+
+		Operation mode meaning:
+		= 0 : Pure HT, no preotection.
+		= 0x01; there may be non-HT devices in both the control and extension channel, protection is optional in BSS.
+		= 0x10: No Transmission in 40M is protected.
+		= 0x11: Transmission in both 40M and 20M shall be protected
+		if (bNonGFExist)
+			we should choose not to use GF. But still set correct ASIC registers.
+	========================================================================
+*/
+VOID 	AsicUpdateProtect(
+	IN		PRTMP_ADAPTER	pAd,
+	IN 		USHORT			OperationMode,
+	IN 		UCHAR			SetMask,
+	IN		BOOLEAN			bDisableBGProtect,
+	IN		BOOLEAN			bNonGFExist)
+{
+	PROT_CFG_STRUC	ProtCfg, ProtCfg4;
+	UINT32 Protect[6];
+	USHORT			offset;
+	UCHAR			i;
+	UINT32 MacReg = 0;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+#ifdef DOT11_N_SUPPORT
+	if (!(pAd->CommonCfg.bHTProtect) && (OperationMode != 8))
+	{
+		return;
+	}
+
+	if (pAd->BATable.numAsOriginator)
+	{
+		//
+		// enable the RTS/CTS to avoid channel collision
+		//
+		SetMask = ALLN_SETPROTECT;
+		OperationMode = 8;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	// Config ASIC RTS threshold register
+	RTMP_IO_READ32(pAd, TX_RTS_CFG, &MacReg);
+	MacReg &= 0xFF0000FF;
+#if 0
+	MacReg |= (pAd->CommonCfg.RtsThreshold << 8);
+#else
+	// If the user want disable RtsThreshold and enbale Amsdu/Ralink-Aggregation, set the RtsThreshold as 4096
+        if ((
+#ifdef DOT11_N_SUPPORT
+			(pAd->CommonCfg.BACapability.field.AmsduEnable) ||
+#endif // DOT11_N_SUPPORT //
+			(pAd->CommonCfg.bAggregationCapable == TRUE))
+            && pAd->CommonCfg.RtsThreshold == MAX_RTS_THRESHOLD)
+        {
+			MacReg |= (0x1000 << 8);
+        }
+        else
+        {
+			MacReg |= (pAd->CommonCfg.RtsThreshold << 8);
+        }
+#endif
+
+	RTMP_IO_WRITE32(pAd, TX_RTS_CFG, MacReg);
+
+	// Initial common protection settings
+	RTMPZeroMemory(Protect, sizeof(Protect));
+	ProtCfg4.word = 0;
+	ProtCfg.word = 0;
+	ProtCfg.field.TxopAllowGF40 = 1;
+	ProtCfg.field.TxopAllowGF20 = 1;
+	ProtCfg.field.TxopAllowMM40 = 1;
+	ProtCfg.field.TxopAllowMM20 = 1;
+	ProtCfg.field.TxopAllowOfdm = 1;
+	ProtCfg.field.TxopAllowCck = 1;
+	ProtCfg.field.RTSThEn = 1;
+	ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+
+	// update PHY mode and rate
+	if (pAd->CommonCfg.Channel > 14)
+		ProtCfg.field.ProtectRate = 0x4000;
+	ProtCfg.field.ProtectRate |= pAd->CommonCfg.RtsRate;
+
+	// Handle legacy(B/G) protection
+	if (bDisableBGProtect)
+	{
+		//ProtCfg.field.ProtectRate = pAd->CommonCfg.RtsRate;
+		ProtCfg.field.ProtectCtrl = 0;
+		Protect[0] = ProtCfg.word;
+		Protect[1] = ProtCfg.word;
+	}
+	else
+	{
+		//ProtCfg.field.ProtectRate = pAd->CommonCfg.RtsRate;
+		ProtCfg.field.ProtectCtrl = 0;			// CCK do not need to be protected
+		Protect[0] = ProtCfg.word;
+		ProtCfg.field.ProtectCtrl = ASIC_CTS;	// OFDM needs using CCK to protect
+		Protect[1] = ProtCfg.word;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	// Decide HT frame protection.
+	if ((SetMask & ALLN_SETPROTECT) != 0)
+	{
+		switch(OperationMode)
+		{
+			case 0x0:
+				// NO PROTECT
+				// 1.All STAs in the BSS are 20/40 MHz HT
+				// 2. in ai 20/40MHz BSS
+				// 3. all STAs are 20MHz in a 20MHz BSS
+				// Pure HT. no protection.
+
+				// MM20_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 010111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+				Protect[2] = 0x01744004;
+
+				// MM40_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 111111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+				Protect[3] = 0x03f44084;
+
+				// CF20_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 010111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+				Protect[4] = 0x01744004;
+
+				// CF40_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 111111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+				Protect[5] = 0x03f44084;
+
+				if (bNonGFExist)
+				{
+					// PROT_NAV(19:18)  -- 01 (Short NAV protectiion)
+					// PROT_CTRL(17:16) -- 01 (RTS/CTS)
+					Protect[4] = 0x01754004;
+					Protect[5] = 0x03f54084;
+				}
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = FALSE;
+				break;
+
+ 			case 1:
+				// This is "HT non-member protection mode."
+				// If there may be non-HT STAs my BSS
+				ProtCfg.word = 0x01744004;	// PROT_CTRL(17:16) : 0 (None)
+				ProtCfg4.word = 0x03f44084; // duplicaet legacy 24M. BW set 1.
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED))
+				{
+					ProtCfg.word = 0x01740003;	//ERP use Protection bit is set, use protection rate at Clause 18..
+					ProtCfg4.word = 0x03f40003; // Don't duplicate RTS/CTS in CCK mode. 0x03f40083;
+				}
+				//Assign Protection method for 20&40 MHz packets
+				ProtCfg.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+				ProtCfg4.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg4.field.ProtectNav = ASIC_SHORTNAV;
+				Protect[2] = ProtCfg.word;
+				Protect[3] = ProtCfg4.word;
+				Protect[4] = ProtCfg.word;
+				Protect[5] = ProtCfg4.word;
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = TRUE;
+				break;
+
+			case 2:
+				// If only HT STAs are in BSS. at least one is 20MHz. Only protect 40MHz packets
+				ProtCfg.word = 0x01744004;  // PROT_CTRL(17:16) : 0 (None)
+				ProtCfg4.word = 0x03f44084; // duplicaet legacy 24M. BW set 1.
+
+				//Assign Protection method for 40MHz packets
+				ProtCfg4.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg4.field.ProtectNav = ASIC_SHORTNAV;
+				Protect[2] = ProtCfg.word;
+				Protect[3] = ProtCfg4.word;
+				if (bNonGFExist)
+				{
+					ProtCfg.field.ProtectCtrl = ASIC_RTS;
+					ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+				}
+				Protect[4] = ProtCfg.word;
+				Protect[5] = ProtCfg4.word;
+
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = FALSE;
+				break;
+
+			case 3:
+				// HT mixed mode.	 PROTECT ALL!
+				// Assign Rate
+				ProtCfg.word = 0x01744004;	//duplicaet legacy 24M. BW set 1.
+				ProtCfg4.word = 0x03f44084;
+				// both 20MHz and 40MHz are protected. Whether use RTS or CTS-to-self depends on the
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED))
+				{
+					ProtCfg.word = 0x01740003;	//ERP use Protection bit is set, use protection rate at Clause 18..
+					ProtCfg4.word = 0x03f40003; // Don't duplicate RTS/CTS in CCK mode. 0x03f40083
+				}
+				//Assign Protection method for 20&40 MHz packets
+				ProtCfg.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+				ProtCfg4.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg4.field.ProtectNav = ASIC_SHORTNAV;
+				Protect[2] = ProtCfg.word;
+				Protect[3] = ProtCfg4.word;
+				Protect[4] = ProtCfg.word;
+				Protect[5] = ProtCfg4.word;
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = TRUE;
+				break;
+
+			case 8:
+				// Special on for Atheros problem n chip.
+				Protect[2] = 0x01754004;
+				Protect[3] = 0x03f54084;
+				Protect[4] = 0x01754004;
+				Protect[5] = 0x03f54084;
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = TRUE;
+				break;
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	offset = CCK_PROT_CFG;
+	for (i = 0;i < 6;i++)
+	{
+		if ((SetMask & (1<< i)))
+		{
+			RTMP_IO_WRITE32(pAd, offset + i*4, Protect[i]);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSwitchChannel(
+					  IN PRTMP_ADAPTER pAd,
+	IN	UCHAR			Channel,
+	IN	BOOLEAN			bScan)
+{
+	ULONG			R2 = 0, R3 = DEFAULT_RF_TX_POWER, R4 = 0;
+	CHAR    TxPwer = 0, TxPwer2 = DEFAULT_RF_TX_POWER; //Bbp94 = BBPR94_DEFAULT, TxPwer2 = DEFAULT_RF_TX_POWER;
+	UCHAR	index;
+	UINT32 	Value = 0; //BbpReg, Value;
+	RTMP_RF_REGS *RFRegTable;
+
+	// Search Tx power value
+	for (index = 0; index < pAd->ChannelListNum; index++)
+	{
+		if (Channel == pAd->ChannelList[index].Channel)
+		{
+			TxPwer = pAd->ChannelList[index].Power;
+			TxPwer2 = pAd->ChannelList[index].Power2;
+			break;
+		}
+	}
+
+	if (index == MAX_NUM_OF_CHANNELS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("AsicSwitchChannel: Cant find the Channel#%d \n", Channel));
+	}
+
+#ifdef RT2870
+	// The RF programming sequence is difference between 3xxx and 2xxx
+	if (IS_RT3070(pAd) && ((pAd->RfIcType == RFIC_3020) || (pAd->RfIcType == RFIC_2020)))
+	{
+		/* modify by WY for Read RF Reg. error */
+		UCHAR RFValue;
+
+		for (index = 0; index < NUM_OF_3020_CHNL; index++)
+		{
+			if (Channel == FreqItems3020[index].Channel)
+			{
+				// Programming channel parameters
+				RT30xxWriteRFRegister(pAd, RF_R02, FreqItems3020[index].N);
+				RT30xxWriteRFRegister(pAd, RF_R03, FreqItems3020[index].K);
+
+				RT30xxReadRFRegister(pAd, RF_R06, (PUCHAR)&RFValue);
+				RFValue = (RFValue & 0xFC) | FreqItems3020[index].R;
+				RT30xxWriteRFRegister(pAd, RF_R06, (UCHAR)RFValue);
+
+				// Set Tx Power
+				RT30xxReadRFRegister(pAd, RF_R12, (PUCHAR)&RFValue);
+				RFValue = (RFValue & 0xE0) | TxPwer;
+				RT30xxWriteRFRegister(pAd, RF_R12, (UCHAR)RFValue);
+
+				// Set RF offset
+				RT30xxReadRFRegister(pAd, RF_R23, (PUCHAR)&RFValue);
+				RFValue = (RFValue & 0x80) | pAd->RfFreqOffset;
+				RT30xxWriteRFRegister(pAd, RF_R23, (UCHAR)RFValue);
+
+				// Set BW
+				if (!bScan && (pAd->CommonCfg.BBPCurrentBW == BW_40))
+				{
+					RFValue = pAd->Mlme.CaliBW40RfR24;
+					//DISABLE_11N_CHECK(pAd);
+				}
+				else
+				{
+					RFValue = pAd->Mlme.CaliBW20RfR24;
+				}
+				RT30xxWriteRFRegister(pAd, RF_R24, (UCHAR)RFValue);
+
+				// Enable RF tuning
+				RT30xxReadRFRegister(pAd, RF_R07, (PUCHAR)&RFValue);
+				RFValue = RFValue | 0x1;
+				RT30xxWriteRFRegister(pAd, RF_R07, (UCHAR)RFValue);
+
+				// latch channel for future usage.
+				pAd->LatchRfRegs.Channel = Channel;
+
+				break;
+			}
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, Pwr0=%d, Pwr1=%d, %dT), N=0x%02X, K=0x%02X, R=0x%02X\n",
+			Channel,
+			pAd->RfIcType,
+			TxPwer,
+			TxPwer2,
+			pAd->Antenna.field.TxPath,
+			FreqItems3020[index].N,
+			FreqItems3020[index].K,
+			FreqItems3020[index].R));
+	}
+	else
+#endif // RT2870 //
+	{
+		RFRegTable = RF2850RegTable;
+
+		switch (pAd->RfIcType)
+		{
+			case RFIC_2820:
+			case RFIC_2850:
+			case RFIC_2720:
+			case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R2 = RFRegTable[index].R2;
+					if (pAd->Antenna.field.TxPath == 1)
+					{
+						R2 |= 0x4000;	// If TXpath is 1, bit 14 = 1;
+					}
+
+					if (pAd->Antenna.field.RxPath == 2)
+					{
+						R2 |= 0x40;	// write 1 to off Rxpath.
+					}
+					else if (pAd->Antenna.field.RxPath == 1)
+					{
+						R2 |= 0x20040;	// write 1 to off RxPath
+					}
+
+					if (Channel > 14)
+					{
+						// initialize R3, R4
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff);
+						R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->RfFreqOffset << 15);
+
+						// 5G band power range: 0xF9~0X0F, TX0 Reg3 bit9/TX1 Reg4 bit6="0" means the TX power reduce 7dB
+						// R3
+						if ((TxPwer >= -7) && (TxPwer < 0))
+						{
+							TxPwer = (7+TxPwer);
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10);
+							DBGPRINT(RT_DEBUG_ERROR, ("AsicSwitchChannel: TxPwer=%d \n", TxPwer));
+						}
+						else
+						{
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10) | (1 << 9);
+						}
+
+						// R4
+						if ((TxPwer2 >= -7) && (TxPwer2 < 0))
+						{
+							TxPwer2 = (7+TxPwer2);
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7);
+							DBGPRINT(RT_DEBUG_ERROR, ("AsicSwitchChannel: TxPwer2=%d \n", TxPwer2));
+						}
+						else
+						{
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7) | (1 << 6);
+						}
+					}
+					else
+					{
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff) | (TxPwer << 9); // set TX power0
+					R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->RfFreqOffset << 15) | (TxPwer2 <<6);// Set freq Offset & TxPwr1
+					}
+
+					// Based on BBP current mode before changing RF channel.
+					if (!bScan && (pAd->CommonCfg.BBPCurrentBW == BW_40))
+					{
+						R4 |=0x200000;
+					}
+
+					// Update variables
+					pAd->LatchRfRegs.Channel = Channel;
+					pAd->LatchRfRegs.R1 = RFRegTable[index].R1;
+					pAd->LatchRfRegs.R2 = R2;
+					pAd->LatchRfRegs.R3 = R3;
+					pAd->LatchRfRegs.R4 = R4;
+
+					// Set RF value 1's set R3[bit2] = [0]
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+					RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+					RTMPusecDelay(200);
+
+					// Set RF value 2's set R3[bit2] = [1]
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+					RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 | 0x04));
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+					RTMPusecDelay(200);
+
+					// Set RF value 3's set R3[bit2] = [0]
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+					RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+					break;
+				}
+			}
+			break;
+
+			default:
+			break;
+		}
+	}
+
+	// Change BBP setting during siwtch from a->g, g->a
+	if (Channel <= 14)
+	{
+	    ULONG	TxPinCfg = 0x00050F0A;//Gary 2007/08/09 0x050A0A
+
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0);//(0x44 - GET_LNA_GAIN(pAd)));	// According the Rory's suggestion to solve the middle range issue.
+		//RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x62);
+
+		// Rx High power VGA offset for LNA select
+	    if (pAd->NicConfig2.field.ExternalLNAForG)
+	    {
+	        RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x62);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x46);
+	    }
+	    else
+	    {
+	        RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x84);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x50);
+	    }
+
+		// 5G band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x04);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+		}
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+		}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+	else
+	{
+	    ULONG	TxPinCfg = 0x00050F05;//Gary 2007/8/9 0x050505
+
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0);//(0x44 - GET_LNA_GAIN(pAd)));   // According the Rory's suggestion to solve the middle range issue.
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0xF2);
+
+		// Rx High power VGA offset for LNA select
+		if (pAd->NicConfig2.field.ExternalLNAForA)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x46);
+		}
+		else
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x50);
+		}
+
+		// 5G band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x02);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+	}
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+	}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+
+    // R66 should be set according to Channel and use 20MHz when scanning
+	//RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, (0x2E + GET_LNA_GAIN(pAd)));
+	if (bScan)
+		RTMPSetAGCInitValue(pAd, BW_20);
+	else
+		RTMPSetAGCInitValue(pAd, pAd->CommonCfg.BBPCurrentBW);
+
+	//
+	// On 11A, We should delay and wait RF/BBP to be stable
+	// and the appropriate time should be 1000 micro seconds
+	// 2005/06/05 - On 11G, We also need this delay time. Otherwise it's difficult to pass the WHQL.
+	//
+	RTMPusecDelay(1000);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, Pwr0=%lu, Pwr1=%lu, %dT) to , R1=0x%08lx, R2=0x%08lx, R3=0x%08lx, R4=0x%08lx\n",
+							  Channel,
+							  pAd->RfIcType,
+							  (R3 & 0x00003e00) >> 9,
+							  (R4 & 0x000007c0) >> 6,
+							  pAd->Antenna.field.TxPath,
+							  pAd->LatchRfRegs.R1,
+							  pAd->LatchRfRegs.R2,
+							  pAd->LatchRfRegs.R3,
+							  pAd->LatchRfRegs.R4));
+}
+
+/*
+	==========================================================================
+	Description:
+		This function is required for 2421 only, and should not be used during
+		site survey. It's only required after NIC decided to stay at a channel
+		for a longer period.
+		When this function is called, it's always after AsicSwitchChannel().
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicLockChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Channel)
+{
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID	AsicAntennaSelect(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Channel)
+{
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Antenna miscellaneous setting.
+
+	Arguments:
+		pAd						Pointer to our adapter
+		BandState				Indicate current Band State.
+
+	Return Value:
+		None
+
+	IRQL <= DISPATCH_LEVEL
+
+	Note:
+		1.) Frame End type control
+			only valid for G only (RF_2527 & RF_2529)
+			0: means DPDT, set BBP R4 bit 5 to 1
+			1: means SPDT, set BBP R4 bit 5 to 0
+
+
+	========================================================================
+*/
+VOID	AsicAntennaSetting(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ABGBAND_STATE	BandState)
+{
+}
+
+VOID AsicRfTuningExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+}
+
+/*
+	==========================================================================
+	Description:
+		Gives CCK TX rate 2 more dB TX power.
+		This routine works only in LINK UP in INFRASTRUCTURE mode.
+
+		calculate desired Tx power in RF R3.Tx0~5,	should consider -
+		0. if current radio is a noisy environment (pAd->DrsCounters.fNoisyEnvironment)
+		1. TxPowerPercentage
+		2. auto calibration based on TSSI feedback
+		3. extra 2 db for CCK
+		4. -10 db upon very-short distance (AvgRSSI >= -40db) to AP
+
+	NOTE: Since this routine requires the value of (pAd->DrsCounters.fNoisyEnvironment),
+		it should be called AFTER MlmeDynamicTxRatSwitching()
+	==========================================================================
+ */
+VOID AsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT			i, j;
+	CHAR		DeltaPwr = 0;
+	BOOLEAN		bAutoTxAgc = FALSE;
+	UCHAR		TssiRef, *pTssiMinusBoundary, *pTssiPlusBoundary, TxAgcStep;
+	UCHAR		BbpR1 = 0, BbpR49 = 0, idx;
+	PCHAR		pTxAgcCompensate;
+	ULONG		TxPwr[5];
+	CHAR		Value;
+
+	if (pAd->CommonCfg.BBPCurrentBW == BW_40)
+	{
+		if (pAd->CommonCfg.CentralChannel > 14)
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgGBand[4];
+		}
+	}
+	else
+	{
+		if (pAd->CommonCfg.Channel > 14)
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgGBand[4];
+		}
+	}
+
+	// TX power compensation for temperature variation based on TSSI. try every 4 second
+	if (pAd->Mlme.OneSecPeriodicRound % 4 == 0)
+	{
+		if (pAd->CommonCfg.Channel <= 14)
+		{
+			/* bg channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			TssiRef            = pAd->TssiRefG;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryG[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryG[0];
+			TxAgcStep          = pAd->TxAgcStepG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			/* a channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			TssiRef            = pAd->TssiRefA;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryA[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryA[0];
+			TxAgcStep          = pAd->TxAgcStepA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+		{
+			/* BbpR1 is unsigned char */
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R49, &BbpR49);
+
+			/* (p) TssiPlusBoundaryG[0] = 0 = (m) TssiMinusBoundaryG[0] */
+			/* compensate: +4     +3   +2   +1    0   -1   -2   -3   -4 * steps */
+			/* step value is defined in pAd->TxAgcStepG for tx power value */
+
+			/* [4]+1+[4]   p4     p3   p2   p1   o1   m1   m2   m3   m4 */
+			/* ex:         0x00 0x15 0x25 0x45 0x88 0xA0 0xB5 0xD0 0xF0
+			   above value are examined in mass factory production */
+			/*             [4]    [3]  [2]  [1]  [0]  [1]  [2]  [3]  [4] */
+
+			/* plus (+) is 0x00 ~ 0x45, minus (-) is 0xa0 ~ 0xf0 */
+			/* if value is between p1 ~ o1 or o1 ~ s1, no need to adjust tx power */
+			/* if value is 0xa5, tx power will be -= TxAgcStep*(2-1) */
+
+			if (BbpR49 > pTssiMinusBoundary[1])
+			{
+				// Reading is larger than the reference value
+				// check for how large we need to decrease the Tx power
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 <= pTssiMinusBoundary[idx])  // Found the range
+						break;
+				}
+				// The index is the step we should decrease, idx = 0 means there is nothing to compensate
+//				if (R3 > (ULONG) (TxAgcStep * (idx-1)))
+					*pTxAgcCompensate = -(TxAgcStep * (idx-1));
+//				else
+//					*pTxAgcCompensate = -((UCHAR)R3);
+
+				DeltaPwr += (*pTxAgcCompensate);
+				DBGPRINT(RT_DEBUG_TRACE, ("-- Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = -%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else if (BbpR49 < pTssiPlusBoundary[1])
+			{
+				// Reading is smaller than the reference value
+				// check for how large we need to increase the Tx power
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 >= pTssiPlusBoundary[idx])   // Found the range
+						break;
+				}
+				// The index is the step we should increase, idx = 0 means there is nothing to compensate
+				*pTxAgcCompensate = TxAgcStep * (idx-1);
+				DeltaPwr += (*pTxAgcCompensate);
+				DBGPRINT(RT_DEBUG_TRACE, ("++ Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else
+			{
+				*pTxAgcCompensate = 0;
+				DBGPRINT(RT_DEBUG_TRACE, ("   Tx Power, BBP R49=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, 0));
+			}
+		}
+	}
+	else
+	{
+		if (pAd->CommonCfg.Channel <= 14)
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+			DeltaPwr += (*pTxAgcCompensate);
+	}
+
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpR1);
+	BbpR1 &= 0xFC;
+
+#ifdef SINGLE_SKU
+	// Handle regulatory max tx power constrain
+	do
+	{
+		UCHAR    TxPwrInEEPROM = 0xFF, CountryTxPwr = 0xFF, criterion;
+		UCHAR    AdjustMaxTxPwr[40];
+
+		if (pAd->CommonCfg.Channel > 14) // 5G band
+			TxPwrInEEPROM = ((pAd->CommonCfg.DefineMaxTxPwr & 0xFF00) >> 8);
+		else // 2.4G band
+			TxPwrInEEPROM = (pAd->CommonCfg.DefineMaxTxPwr & 0x00FF);
+		CountryTxPwr = GetCuntryMaxTxPwr(pAd, pAd->CommonCfg.Channel);
+
+		// error handling, range check
+		if ((TxPwrInEEPROM > 0x50) || (CountryTxPwr > 0x50))
+		{
+			DBGPRINT(RT_DEBUG_ERROR,("AsicAdjustTxPower - Invalid max tx power (=0x%02x), CountryTxPwr=%d\n", TxPwrInEEPROM, CountryTxPwr));
+			break;
+		}
+
+		criterion = *((PUCHAR)TxPwr + 2) & 0xF;        // FAE use OFDM 6M as criterion
+
+		DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (criterion=%d, TxPwrInEEPROM=%d, CountryTxPwr=%d)\n", criterion, TxPwrInEEPROM, CountryTxPwr));
+
+		// Adjust max tx power according to the relationship of tx power in E2PROM
+		for (i=0; i<5; i++)
+		{
+			// CCK will have 4dBm larger than OFDM
+			// Therefore, we should separate to parse the tx power field
+			if (i == 0)
+			{
+				for (j=0; j<8; j++)
+				{
+					Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F);
+
+					if (j < 4)
+					{
+						// CCK will have 4dBm larger than OFDM
+						AdjustMaxTxPwr[i*8+j] = TxPwrInEEPROM + (Value - criterion) + 4;
+					}
+					else
+					{
+						AdjustMaxTxPwr[i*8+j] = TxPwrInEEPROM + (Value - criterion);
+					}
+					DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+				}
+			}
+			else
+			{
+				for (j=0; j<8; j++)
+				{
+					Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F);
+
+					AdjustMaxTxPwr[i*8+j] = TxPwrInEEPROM + (Value - criterion);
+					DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+				}
+			}
+		}
+
+		// Adjust tx power according to the relationship
+		for (i=0; i<5; i++)
+		{
+			if (TxPwr[i] != 0xffffffff)
+			{
+				for (j=0; j<8; j++)
+				{
+					Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F);
+
+					// The system tx power is larger than the regulatory, the power should be restrain
+					if (AdjustMaxTxPwr[i*8+j] > CountryTxPwr)
+					{
+						// decrease to zero and don't need to take care BBPR1
+						if ((Value - (AdjustMaxTxPwr[i*8+j] - CountryTxPwr)) > 0)
+							Value -= (AdjustMaxTxPwr[i*8+j] - CountryTxPwr);
+						else
+							Value = 0;
+
+						DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+					}
+					else
+						DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d, no change)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+
+						TxPwr[i] = (TxPwr[i] & ~(0x0000000F << j*4)) | (Value << j*4);
+				}
+			}
+		}
+	} while (FALSE);
+#endif // SINGLE_SKU //
+
+	/* calculate delta power based on the percentage specified from UI */
+	// E2PROM setting is calibrated for maximum TX power (i.e. 100%)
+	// We lower TX power here according to the percentage specified from UI
+	if (pAd->CommonCfg.TxPowerPercentage == 0xffffffff)       // AUTO TX POWER control
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 90)  // 91 ~ 100% & AUTO, treat as 100% in terms of mW
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 60)  // 61 ~ 90%, treat as 75% in terms of mW		// DeltaPwr -= 1;
+	{
+		DeltaPwr -= 1;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 30)  // 31 ~ 60%, treat as 50% in terms of mW		// DeltaPwr -= 3;
+	{
+		DeltaPwr -= 3;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 15)  // 16 ~ 30%, treat as 25% in terms of mW		// DeltaPwr -= 6;
+	{
+		BbpR1 |= 0x01;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 9)   // 10 ~ 15%, treat as 12.5% in terms of mW		// DeltaPwr -= 9;
+	{
+		BbpR1 |= 0x01;
+		DeltaPwr -= 3;
+	}
+	else                                           // 0 ~ 9 %, treat as MIN(~3%) in terms of mW		// DeltaPwr -= 12;
+	{
+		BbpR1 |= 0x02;
+	}
+
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpR1);
+
+	/* reset different new tx power for different TX rate */
+	for(i=0; i<5; i++)
+	{
+		if (TxPwr[i] != 0xffffffff)
+		{
+			for (j=0; j<8; j++)
+			{
+				Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F); /* 0 ~ 15 */
+
+				if ((Value + DeltaPwr) < 0)
+				{
+					Value = 0; /* min */
+				}
+				else if ((Value + DeltaPwr) > 0xF)
+				{
+					Value = 0xF; /* max */
+				}
+				else
+				{
+					Value += DeltaPwr; /* temperature compensation */
+				}
+
+				/* fill new value to CSR offset */
+				TxPwr[i] = (TxPwr[i] & ~(0x0000000F << j*4)) | (Value << j*4);
+			}
+
+			/* write tx power value to CSR */
+			/* TX_PWR_CFG_0 (8 tx rate) for	TX power for OFDM 12M/18M
+											TX power for OFDM 6M/9M
+											TX power for CCK5.5M/11M
+											TX power for CCK1M/2M */
+			/* TX_PWR_CFG_1 ~ TX_PWR_CFG_4 */
+			RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, TxPwr[i]);
+		}
+	}
+
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	==========================================================================
+	Description:
+		put PHY to sleep here, and set next wakeup timer. PHY doesn't not wakeup
+		automatically. Instead, MCU will issue a TwakeUpInterrupt to host after
+		the wakeup timer timeout. Driver has to issue a separate command to wake
+		PHY up.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSleepThenAutoWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TbttNumToNextWakeUp)
+{
+    RT28XX_STA_SLEEP_THEN_AUTO_WAKEUP(pAd, TbttNumToNextWakeUp);
+}
+
+/*
+	==========================================================================
+	Description:
+		AsicForceWakeup() is used whenever manual wakeup is required
+		AsicForceSleep() should only be used when not in INFRA BSS. When
+		in INFRA BSS, we should use AsicSleepThenAutoWakeup() instead.
+	==========================================================================
+ */
+VOID AsicForceSleep(
+	IN PRTMP_ADAPTER pAd)
+{
+
+}
+
+/*
+	==========================================================================
+	Description:
+		AsicForceWakeup() is used whenever Twakeup timer (set via AsicSleepThenAutoWakeup)
+		expired.
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+	==========================================================================
+ */
+VOID AsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN    bFromTx)
+{
+    DBGPRINT(RT_DEBUG_TRACE, ("--> AsicForceWakeup \n"));
+    RT28XX_STA_FORCE_WAKEUP(pAd, bFromTx);
+}
+#endif // CONFIG_STA_SUPPORT //
+/*
+	==========================================================================
+	Description:
+		Set My BSSID
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSetBssid(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pBssid)
+{
+	ULONG		  Addr4;
+	DBGPRINT(RT_DEBUG_TRACE, ("==============> AsicSetBssid %x:%x:%x:%x:%x:%x\n",
+		pBssid[0],pBssid[1],pBssid[2],pBssid[3], pBssid[4],pBssid[5]));
+
+	Addr4 = (ULONG)(pBssid[0])		 |
+			(ULONG)(pBssid[1] << 8)  |
+			(ULONG)(pBssid[2] << 16) |
+			(ULONG)(pBssid[3] << 24);
+	RTMP_IO_WRITE32(pAd, MAC_BSSID_DW0, Addr4);
+
+	Addr4 = 0;
+	// always one BSSID in STA mode
+	Addr4 = (ULONG)(pBssid[4]) | (ULONG)(pBssid[5] << 8);
+
+	RTMP_IO_WRITE32(pAd, MAC_BSSID_DW1, Addr4);
+}
+
+VOID AsicSetMcastWC(
+	IN PRTMP_ADAPTER pAd)
+{
+	MAC_TABLE_ENTRY *pEntry = &pAd->MacTab.Content[MCAST_WCID];
+	USHORT		offset;
+
+	pEntry->Sst        = SST_ASSOC;
+	pEntry->Aid        = MCAST_WCID;	// Softap supports 1 BSSID and use WCID=0 as multicast Wcid index
+	pEntry->PsMode     = PWR_ACTIVE;
+	pEntry->CurrTxRate = pAd->CommonCfg.MlmeRate;
+	offset = MAC_WCID_BASE + BSS0Mcast_WCID * HW_WCID_ENTRY_SIZE;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicDelWcidTab(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	Wcid)
+{
+	ULONG		  Addr0 = 0x0, Addr1 = 0x0;
+	ULONG 		offset;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicDelWcidTab==>Wcid = 0x%x\n",Wcid));
+	offset = MAC_WCID_BASE + Wcid * HW_WCID_ENTRY_SIZE;
+	RTMP_IO_WRITE32(pAd, offset, Addr0);
+	offset += 4;
+	RTMP_IO_WRITE32(pAd, offset, Addr1);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicEnableRDG(
+	IN PRTMP_ADAPTER pAd)
+{
+	TX_LINK_CFG_STRUC	TxLinkCfg;
+	UINT32				Data = 0;
+
+	RTMP_IO_READ32(pAd, TX_LINK_CFG, &TxLinkCfg.word);
+	TxLinkCfg.field.TxRDGEn = 1;
+	RTMP_IO_WRITE32(pAd, TX_LINK_CFG, TxLinkCfg.word);
+
+	RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+	Data  &= 0xFFFFFF00;
+	Data  |= 0x80;
+	RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+
+	//OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicDisableRDG(
+	IN PRTMP_ADAPTER pAd)
+{
+	TX_LINK_CFG_STRUC	TxLinkCfg;
+	UINT32				Data = 0;
+
+
+	RTMP_IO_READ32(pAd, TX_LINK_CFG, &TxLinkCfg.word);
+	TxLinkCfg.field.TxRDGEn = 0;
+	RTMP_IO_WRITE32(pAd, TX_LINK_CFG, TxLinkCfg.word);
+
+	RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+
+	Data  &= 0xFFFFFF00;
+	//Data  |= 0x20;
+#ifndef WIFI_TEST
+	//if ( pAd->CommonCfg.bEnableTxBurst )
+	//	Data |= 0x60; // for performance issue not set the TXOP to 0
+#endif
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_DYNAMIC_BE_TXOP_ACTIVE)
+#ifdef DOT11_N_SUPPORT
+		&& (pAd->MacTab.fAnyStationMIMOPSDynamic == FALSE)
+#endif // DOT11_N_SUPPORT //
+	)
+	{
+		// For CWC test, change txop from 0x30 to 0x20 in TxBurst mode
+		if (pAd->CommonCfg.bEnableTxBurst)
+			Data |= 0x20;
+	}
+	RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicDisableSync(
+	IN PRTMP_ADAPTER pAd)
+{
+	BCN_TIME_CFG_STRUC csr;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->Disable TSF synchronization\n"));
+
+	// 2003-12-20 disable TSF and TBTT while NIC in power-saving have side effect
+	//			  that NIC will never wakes up because TSF stops and no more
+	//			  TBTT interrupts
+	pAd->TbttTickCount = 0;
+	RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+	csr.field.bBeaconGen = 0;
+	csr.field.bTBTTEnable = 0;
+	csr.field.TsfSyncMode = 0;
+	csr.field.bTsfTicking = 0;
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicEnableBssSync(
+	IN PRTMP_ADAPTER pAd)
+{
+	BCN_TIME_CFG_STRUC csr;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->AsicEnableBssSync(INFRA mode)\n"));
+
+	RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+//	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, 0x00000000);
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		csr.field.BeaconInterval = pAd->CommonCfg.BeaconPeriod << 4; // ASIC register in units of 1/16 TU
+		csr.field.bTsfTicking = 1;
+		csr.field.TsfSyncMode = 1; // sync TSF in INFRASTRUCTURE mode
+		csr.field.bBeaconGen  = 0; // do NOT generate BEACON
+		csr.field.bTBTTEnable = 1;
+	}
+#endif // CONFIG_STA_SUPPORT //
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+}
+
+/*
+	==========================================================================
+	Description:
+	Note:
+		BEACON frame in shared memory should be built ok before this routine
+		can be called. Otherwise, a garbage frame maybe transmitted out every
+		Beacon period.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicEnableIbssSync(
+	IN PRTMP_ADAPTER pAd)
+{
+	BCN_TIME_CFG_STRUC csr9;
+	PUCHAR			ptr;
+	UINT i;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->AsicEnableIbssSync(ADHOC mode. MPDUtotalByteCount = %d)\n", pAd->BeaconTxWI.MPDUtotalByteCount));
+
+	RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr9.word);
+	csr9.field.bBeaconGen = 0;
+	csr9.field.bTBTTEnable = 0;
+	csr9.field.bTsfTicking = 0;
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr9.word);
+
+
+#ifdef RT2870
+	// move BEACON TXD and frame content to on-chip memory
+	ptr = (PUCHAR)&pAd->BeaconTxWI;
+	for (i=0; i<TXWI_SIZE; i+=2)  // 16-byte TXWI field
+	{
+		//UINT32 longptr =  *ptr + (*(ptr+1)<<8) + (*(ptr+2)<<16) + (*(ptr+3)<<24);
+		//RTMP_IO_WRITE32(pAd, HW_BEACON_BASE0 + i, longptr);
+		RTUSBMultiWrite(pAd, HW_BEACON_BASE0 + i, ptr, 2);
+		ptr += 2;
+	}
+
+	// start right after the 16-byte TXWI field
+	ptr = pAd->BeaconBuf;
+	for (i=0; i< pAd->BeaconTxWI.MPDUtotalByteCount; i+=2)
+	{
+		//UINT32 longptr =  *ptr + (*(ptr+1)<<8) + (*(ptr+2)<<16) + (*(ptr+3)<<24);
+		//RTMP_IO_WRITE32(pAd, HW_BEACON_BASE0 + TXWI_SIZE + i, longptr);
+		RTUSBMultiWrite(pAd, HW_BEACON_BASE0 + TXWI_SIZE + i, ptr, 2);
+		ptr +=2;
+	}
+#endif // RT2870 //
+
+	//
+	// For Wi-Fi faily generated beacons between participating stations.
+	// Set TBTT phase adaptive adjustment step to 8us (default 16us)
+	// don't change settings 2006-5- by Jerry
+	//RTMP_IO_WRITE32(pAd, TBTT_SYNC_CFG, 0x00001010);
+
+	// start sending BEACON
+	csr9.field.BeaconInterval = pAd->CommonCfg.BeaconPeriod << 4; // ASIC register in units of 1/16 TU
+	csr9.field.bTsfTicking = 1;
+	csr9.field.TsfSyncMode = 2; // sync TSF in IBSS mode
+	csr9.field.bTBTTEnable = 1;
+	csr9.field.bBeaconGen = 1;
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr9.word);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSetEdcaParm(
+	IN PRTMP_ADAPTER pAd,
+	IN PEDCA_PARM	 pEdcaParm)
+{
+	EDCA_AC_CFG_STRUC   Ac0Cfg, Ac1Cfg, Ac2Cfg, Ac3Cfg;
+	AC_TXOP_CSR0_STRUC csr0;
+	AC_TXOP_CSR1_STRUC csr1;
+	AIFSN_CSR_STRUC    AifsnCsr;
+	CWMIN_CSR_STRUC    CwminCsr;
+	CWMAX_CSR_STRUC    CwmaxCsr;
+	int i;
+
+	Ac0Cfg.word = 0;
+	Ac1Cfg.word = 0;
+	Ac2Cfg.word = 0;
+	Ac3Cfg.word = 0;
+	if ((pEdcaParm == NULL) || (pEdcaParm->bValid == FALSE))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("AsicSetEdcaParm\n"));
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_WMM_INUSED);
+		for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+		{
+			if (pAd->MacTab.Content[i].ValidAsCLI || pAd->MacTab.Content[i].ValidAsApCli)
+				CLIENT_STATUS_CLEAR_FLAG(&pAd->MacTab.Content[i], fCLIENT_STATUS_WMM_CAPABLE);
+		}
+
+		//========================================================
+		//      MAC Register has a copy .
+		//========================================================
+//#ifndef WIFI_TEST
+		if( pAd->CommonCfg.bEnableTxBurst )
+		{
+			// For CWC test, change txop from 0x30 to 0x20 in TxBurst mode
+			Ac0Cfg.field.AcTxop = 0x20; // Suggest by John for TxBurst in HT Mode
+		}
+		else
+			Ac0Cfg.field.AcTxop = 0;	// QID_AC_BE
+//#else
+//		Ac0Cfg.field.AcTxop = 0;	// QID_AC_BE
+//#endif
+		Ac0Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac0Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac0Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Ac0Cfg.word);
+
+		Ac1Cfg.field.AcTxop = 0;	// QID_AC_BK
+		Ac1Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac1Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac1Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC1_CFG, Ac1Cfg.word);
+
+		if (pAd->CommonCfg.PhyMode == PHY_11B)
+		{
+			Ac2Cfg.field.AcTxop = 192;	// AC_VI: 192*32us ~= 6ms
+			Ac3Cfg.field.AcTxop = 96;	// AC_VO: 96*32us  ~= 3ms
+		}
+		else
+		{
+			Ac2Cfg.field.AcTxop = 96;	// AC_VI: 96*32us ~= 3ms
+			Ac3Cfg.field.AcTxop = 48;	// AC_VO: 48*32us ~= 1.5ms
+		}
+		Ac2Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac2Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac2Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC2_CFG, Ac2Cfg.word);
+		Ac3Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac3Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac3Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC3_CFG, Ac3Cfg.word);
+
+		//========================================================
+		//      DMA Register has a copy too.
+		//========================================================
+		csr0.field.Ac0Txop = 0;		// QID_AC_BE
+		csr0.field.Ac1Txop = 0;		// QID_AC_BK
+		RTMP_IO_WRITE32(pAd, WMM_TXOP0_CFG, csr0.word);
+		if (pAd->CommonCfg.PhyMode == PHY_11B)
+		{
+			csr1.field.Ac2Txop = 192;		// AC_VI: 192*32us ~= 6ms
+			csr1.field.Ac3Txop = 96;		// AC_VO: 96*32us  ~= 3ms
+		}
+		else
+		{
+			csr1.field.Ac2Txop = 96;		// AC_VI: 96*32us ~= 3ms
+			csr1.field.Ac3Txop = 48;		// AC_VO: 48*32us ~= 1.5ms
+		}
+		RTMP_IO_WRITE32(pAd, WMM_TXOP1_CFG, csr1.word);
+
+		CwminCsr.word = 0;
+		CwminCsr.field.Cwmin0 = CW_MIN_IN_BITS;
+		CwminCsr.field.Cwmin1 = CW_MIN_IN_BITS;
+		CwminCsr.field.Cwmin2 = CW_MIN_IN_BITS;
+		CwminCsr.field.Cwmin3 = CW_MIN_IN_BITS;
+		RTMP_IO_WRITE32(pAd, WMM_CWMIN_CFG, CwminCsr.word);
+
+		CwmaxCsr.word = 0;
+		CwmaxCsr.field.Cwmax0 = CW_MAX_IN_BITS;
+		CwmaxCsr.field.Cwmax1 = CW_MAX_IN_BITS;
+		CwmaxCsr.field.Cwmax2 = CW_MAX_IN_BITS;
+		CwmaxCsr.field.Cwmax3 = CW_MAX_IN_BITS;
+		RTMP_IO_WRITE32(pAd, WMM_CWMAX_CFG, CwmaxCsr.word);
+
+		RTMP_IO_WRITE32(pAd, WMM_AIFSN_CFG, 0x00002222);
+
+		NdisZeroMemory(&pAd->CommonCfg.APEdcaParm, sizeof(EDCA_PARM));
+	}
+	else
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_WMM_INUSED);
+		//========================================================
+		//      MAC Register has a copy.
+		//========================================================
+		//
+		// Modify Cwmin/Cwmax/Txop on queue[QID_AC_VI], Recommend by Jerry 2005/07/27
+		// To degrade our VIDO Queue's throughput for WiFi WMM S3T07 Issue.
+		//
+		//pEdcaParm->Txop[QID_AC_VI] = pEdcaParm->Txop[QID_AC_VI] * 7 / 10; // rt2860c need this
+
+		Ac0Cfg.field.AcTxop =  pEdcaParm->Txop[QID_AC_BE];
+		Ac0Cfg.field.Cwmin= pEdcaParm->Cwmin[QID_AC_BE];
+		Ac0Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_BE];
+		Ac0Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_BE]; //+1;
+
+		Ac1Cfg.field.AcTxop =  pEdcaParm->Txop[QID_AC_BK];
+		Ac1Cfg.field.Cwmin = pEdcaParm->Cwmin[QID_AC_BK]; //+2;
+		Ac1Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_BK];
+		Ac1Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_BK]; //+1;
+
+		Ac2Cfg.field.AcTxop = (pEdcaParm->Txop[QID_AC_VI] * 6) / 10;
+		Ac2Cfg.field.Cwmin = pEdcaParm->Cwmin[QID_AC_VI];
+		Ac2Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_VI];
+		Ac2Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_VI];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			// Tuning for Wi-Fi WMM S06
+			if (pAd->CommonCfg.bWiFiTest &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+				Ac2Cfg.field.Aifsn -= 1;
+
+			// Tuning for TGn Wi-Fi 5.2.32
+			// STA TestBed changes in this item: conexant legacy sta ==> broadcom 11n sta
+			if (STA_TGN_WIFI_ON(pAd) &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+			{
+				Ac0Cfg.field.Aifsn = 3;
+				Ac2Cfg.field.AcTxop = 5;
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		Ac3Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_VO];
+		Ac3Cfg.field.Cwmin = pEdcaParm->Cwmin[QID_AC_VO];
+		Ac3Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_VO];
+		Ac3Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_VO];
+
+//#ifdef WIFI_TEST
+		if (pAd->CommonCfg.bWiFiTest)
+		{
+			if (Ac3Cfg.field.AcTxop == 102)
+			{
+			Ac0Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_BE] ? pEdcaParm->Txop[QID_AC_BE] : 10;
+				Ac0Cfg.field.Aifsn  = pEdcaParm->Aifsn[QID_AC_BE]-1; /* AIFSN must >= 1 */
+			Ac1Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_BK];
+				Ac1Cfg.field.Aifsn  = pEdcaParm->Aifsn[QID_AC_BK];
+			Ac2Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_VI];
+			} /* End of if */
+		}
+//#endif // WIFI_TEST //
+
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Ac0Cfg.word);
+		RTMP_IO_WRITE32(pAd, EDCA_AC1_CFG, Ac1Cfg.word);
+		RTMP_IO_WRITE32(pAd, EDCA_AC2_CFG, Ac2Cfg.word);
+		RTMP_IO_WRITE32(pAd, EDCA_AC3_CFG, Ac3Cfg.word);
+
+
+		//========================================================
+		//      DMA Register has a copy too.
+		//========================================================
+		csr0.field.Ac0Txop = Ac0Cfg.field.AcTxop;
+		csr0.field.Ac1Txop = Ac1Cfg.field.AcTxop;
+		RTMP_IO_WRITE32(pAd, WMM_TXOP0_CFG, csr0.word);
+
+		csr1.field.Ac2Txop = Ac2Cfg.field.AcTxop;
+		csr1.field.Ac3Txop = Ac3Cfg.field.AcTxop;
+		RTMP_IO_WRITE32(pAd, WMM_TXOP1_CFG, csr1.word);
+
+		CwminCsr.word = 0;
+		CwminCsr.field.Cwmin0 = pEdcaParm->Cwmin[QID_AC_BE];
+		CwminCsr.field.Cwmin1 = pEdcaParm->Cwmin[QID_AC_BK];
+		CwminCsr.field.Cwmin2 = pEdcaParm->Cwmin[QID_AC_VI];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			CwminCsr.field.Cwmin3 = pEdcaParm->Cwmin[QID_AC_VO] - 1; //for TGn wifi test
+#endif // CONFIG_STA_SUPPORT //
+		RTMP_IO_WRITE32(pAd, WMM_CWMIN_CFG, CwminCsr.word);
+
+		CwmaxCsr.word = 0;
+		CwmaxCsr.field.Cwmax0 = pEdcaParm->Cwmax[QID_AC_BE];
+		CwmaxCsr.field.Cwmax1 = pEdcaParm->Cwmax[QID_AC_BK];
+		CwmaxCsr.field.Cwmax2 = pEdcaParm->Cwmax[QID_AC_VI];
+		CwmaxCsr.field.Cwmax3 = pEdcaParm->Cwmax[QID_AC_VO];
+		RTMP_IO_WRITE32(pAd, WMM_CWMAX_CFG, CwmaxCsr.word);
+
+		AifsnCsr.word = 0;
+		AifsnCsr.field.Aifsn0 = Ac0Cfg.field.Aifsn; //pEdcaParm->Aifsn[QID_AC_BE];
+		AifsnCsr.field.Aifsn1 = Ac1Cfg.field.Aifsn; //pEdcaParm->Aifsn[QID_AC_BK];
+		AifsnCsr.field.Aifsn2 = Ac2Cfg.field.Aifsn; //pEdcaParm->Aifsn[QID_AC_VI];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			// Tuning for Wi-Fi WMM S06
+			if (pAd->CommonCfg.bWiFiTest &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+				AifsnCsr.field.Aifsn2 = Ac2Cfg.field.Aifsn - 4;
+
+			// Tuning for TGn Wi-Fi 5.2.32
+			// STA TestBed changes in this item: conexant legacy sta ==> broadcom 11n sta
+			if (STA_TGN_WIFI_ON(pAd) &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+			{
+				AifsnCsr.field.Aifsn0 = 3;
+				AifsnCsr.field.Aifsn2 = 7;
+			}
+
+			if (INFRA_ON(pAd))
+				CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[BSSID_WCID], fCLIENT_STATUS_WMM_CAPABLE);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			AifsnCsr.field.Aifsn3 = Ac3Cfg.field.Aifsn - 1; //pEdcaParm->Aifsn[QID_AC_VO]; //for TGn wifi test
+#endif // CONFIG_STA_SUPPORT //
+		RTMP_IO_WRITE32(pAd, WMM_AIFSN_CFG, AifsnCsr.word);
+
+		NdisMoveMemory(&pAd->CommonCfg.APEdcaParm, pEdcaParm, sizeof(EDCA_PARM));
+		if (!ADHOC_ON(pAd))
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("EDCA [#%d]: AIFSN CWmin CWmax  TXOP(us)  ACM\n", pEdcaParm->EdcaUpdateCount));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_BE      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[0],
+									 pEdcaParm->Cwmin[0],
+									 pEdcaParm->Cwmax[0],
+									 pEdcaParm->Txop[0]<<5,
+									 pEdcaParm->bACM[0]));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_BK      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[1],
+									 pEdcaParm->Cwmin[1],
+									 pEdcaParm->Cwmax[1],
+									 pEdcaParm->Txop[1]<<5,
+									 pEdcaParm->bACM[1]));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_VI      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[2],
+									 pEdcaParm->Cwmin[2],
+									 pEdcaParm->Cwmax[2],
+									 pEdcaParm->Txop[2]<<5,
+									 pEdcaParm->bACM[2]));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_VO      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[3],
+									 pEdcaParm->Cwmin[3],
+									 pEdcaParm->Cwmax[3],
+									 pEdcaParm->Txop[3]<<5,
+									 pEdcaParm->bACM[3]));
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID 	AsicSetSlotTime(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN bUseShortSlotTime)
+{
+	ULONG	SlotTime;
+	UINT32	RegValue = 0;
+
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->CommonCfg.Channel > 14)
+		bUseShortSlotTime = TRUE;
+#endif // CONFIG_STA_SUPPORT //
+
+	if (bUseShortSlotTime)
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_SHORT_SLOT_INUSED);
+	else
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_SLOT_INUSED);
+
+	SlotTime = (bUseShortSlotTime)? 9 : 20;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// force using short SLOT time for FAE to demo performance when TxBurst is ON
+		if (((pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED)))
+#ifdef DOT11_N_SUPPORT
+			|| ((pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE) && (pAd->CommonCfg.BACapability.field.Policy == BA_NOTUSE))
+#endif // DOT11_N_SUPPORT //
+			)
+		{
+			// In this case, we will think it is doing Wi-Fi test
+			// And we will not set to short slot when bEnableTxBurst is TRUE.
+		}
+		else if (pAd->CommonCfg.bEnableTxBurst)
+			SlotTime = 9;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	//
+	// For some reasons, always set it to short slot time.
+	//
+	// ToDo: Should consider capability with 11B
+	//
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->StaCfg.BssType == BSS_ADHOC)
+			SlotTime = 20;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_IO_READ32(pAd, BKOFF_SLOT_CFG, &RegValue);
+	RegValue = RegValue & 0xFFFFFF00;
+
+	RegValue |= SlotTime;
+
+	RTMP_IO_WRITE32(pAd, BKOFF_SLOT_CFG, RegValue);
+}
+
+/*
+	========================================================================
+	Description:
+		Add Shared key information into ASIC.
+		Update shared key, TxMic and RxMic to Asic Shared key table
+		Update its cipherAlg to Asic Shared key Mode.
+
+    Return:
+	========================================================================
+*/
+VOID AsicAddSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 BssIndex,
+	IN UCHAR		 KeyIdx,
+	IN UCHAR		 CipherAlg,
+	IN PUCHAR		 pKey,
+	IN PUCHAR		 pTxMic,
+	IN PUCHAR		 pRxMic)
+{
+	ULONG offset; //, csr0;
+	SHAREDKEY_MODE_STRUC csr1;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicAddSharedKeyEntry BssIndex=%d, KeyIdx=%d\n", BssIndex,KeyIdx));
+//============================================================================================
+
+	DBGPRINT(RT_DEBUG_TRACE,("AsicAddSharedKeyEntry: %s key #%d\n", CipherName[CipherAlg], BssIndex*4 + KeyIdx));
+	DBGPRINT_RAW(RT_DEBUG_TRACE, (" 	Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pKey[0],pKey[1],pKey[2],pKey[3],pKey[4],pKey[5],pKey[6],pKey[7],pKey[8],pKey[9],pKey[10],pKey[11],pKey[12],pKey[13],pKey[14],pKey[15]));
+	if (pRxMic)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, (" 	Rx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pRxMic[0],pRxMic[1],pRxMic[2],pRxMic[3],pRxMic[4],pRxMic[5],pRxMic[6],pRxMic[7]));
+	}
+	if (pTxMic)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, (" 	Tx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pTxMic[0],pTxMic[1],pTxMic[2],pTxMic[3],pTxMic[4],pTxMic[5],pTxMic[6],pTxMic[7]));
+	}
+//============================================================================================
+	//
+	// fill key material - key + TX MIC + RX MIC
+	//
+
+#ifdef RT2870
+{
+	offset = SHARED_KEY_TABLE_BASE + (4*BssIndex + KeyIdx)*HW_KEY_ENTRY_SIZE;
+	RTUSBMultiWrite(pAd, offset, pKey, MAX_LEN_OF_SHARE_KEY);
+
+	offset += MAX_LEN_OF_SHARE_KEY;
+	if (pTxMic)
+	{
+		RTUSBMultiWrite(pAd, offset, pTxMic, 8);
+	}
+
+	offset += 8;
+	if (pRxMic)
+	{
+		RTUSBMultiWrite(pAd, offset, pRxMic, 8);
+	}
+}
+#endif // RT2870 //
+
+	//
+	// Update cipher algorithm. WSTA always use BSS0
+	//
+	RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), &csr1.word);
+	DBGPRINT(RT_DEBUG_TRACE,("Read: SHARED_KEY_MODE_BASE at this Bss[%d] KeyIdx[%d]= 0x%x \n", BssIndex,KeyIdx, csr1.word));
+	if ((BssIndex%2) == 0)
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss0Key0CipherAlg = CipherAlg;
+		else if (KeyIdx == 1)
+			csr1.field.Bss0Key1CipherAlg = CipherAlg;
+		else if (KeyIdx == 2)
+			csr1.field.Bss0Key2CipherAlg = CipherAlg;
+		else
+			csr1.field.Bss0Key3CipherAlg = CipherAlg;
+	}
+	else
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss1Key0CipherAlg = CipherAlg;
+		else if (KeyIdx == 1)
+			csr1.field.Bss1Key1CipherAlg = CipherAlg;
+		else if (KeyIdx == 2)
+			csr1.field.Bss1Key2CipherAlg = CipherAlg;
+		else
+			csr1.field.Bss1Key3CipherAlg = CipherAlg;
+	}
+	DBGPRINT(RT_DEBUG_TRACE,("Write: SHARED_KEY_MODE_BASE at this Bss[%d] = 0x%x \n", BssIndex, csr1.word));
+	RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), csr1.word);
+
+}
+
+//	IRQL = DISPATCH_LEVEL
+VOID AsicRemoveSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 BssIndex,
+	IN UCHAR		 KeyIdx)
+{
+	//ULONG SecCsr0;
+	SHAREDKEY_MODE_STRUC csr1;
+
+	DBGPRINT(RT_DEBUG_TRACE,("AsicRemoveSharedKeyEntry: #%d \n", BssIndex*4 + KeyIdx));
+
+	RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), &csr1.word);
+	if ((BssIndex%2) == 0)
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss0Key0CipherAlg = 0;
+		else if (KeyIdx == 1)
+			csr1.field.Bss0Key1CipherAlg = 0;
+		else if (KeyIdx == 2)
+			csr1.field.Bss0Key2CipherAlg = 0;
+		else
+			csr1.field.Bss0Key3CipherAlg = 0;
+	}
+	else
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss1Key0CipherAlg = 0;
+		else if (KeyIdx == 1)
+			csr1.field.Bss1Key1CipherAlg = 0;
+		else if (KeyIdx == 2)
+			csr1.field.Bss1Key2CipherAlg = 0;
+		else
+			csr1.field.Bss1Key3CipherAlg = 0;
+	}
+	DBGPRINT(RT_DEBUG_TRACE,("Write: SHARED_KEY_MODE_BASE at this Bss[%d] = 0x%x \n", BssIndex, csr1.word));
+	RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), csr1.word);
+	ASSERT(BssIndex < 4);
+	ASSERT(KeyIdx < 4);
+
+}
+
+
+VOID AsicUpdateWCIDAttribute(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR        CipherAlg,
+	IN BOOLEAN		bUsePairewiseKeyTable)
+{
+	ULONG   WCIDAttri = 0, offset;
+
+	//
+	// Update WCID attribute.
+	// Only TxKey could update WCID attribute.
+	//
+	offset = MAC_WCID_ATTRIBUTE_BASE + (WCID * HW_WCID_ATTRI_SIZE);
+	WCIDAttri = (BssIndex << 4) | (CipherAlg << 1) | (bUsePairewiseKeyTable);
+	RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+}
+
+VOID AsicUpdateWCIDIVEIV(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN ULONG        uIV,
+	IN ULONG        uEIV)
+{
+	ULONG	offset;
+
+	offset = MAC_IVEIV_TABLE_BASE + (WCID * HW_IVEIV_ENTRY_SIZE);
+
+	RTMP_IO_WRITE32(pAd, offset, uIV);
+	RTMP_IO_WRITE32(pAd, offset + 4, uEIV);
+}
+
+VOID AsicUpdateRxWCIDTable(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN PUCHAR        pAddr)
+{
+	ULONG offset;
+	ULONG Addr;
+
+	offset = MAC_WCID_BASE + (WCID * HW_WCID_ENTRY_SIZE);
+	Addr = pAddr[0] + (pAddr[1] << 8) +(pAddr[2] << 16) +(pAddr[3] << 24);
+	RTMP_IO_WRITE32(pAd, offset, Addr);
+	Addr = pAddr[4] + (pAddr[5] << 8);
+	RTMP_IO_WRITE32(pAd, offset + 4, Addr);
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Set Cipher Key, Cipher algorithm, IV/EIV to Asic
+
+    Arguments:
+        pAd                     Pointer to our adapter
+        WCID                    WCID Entry number.
+        BssIndex                BSSID index, station or none multiple BSSID support
+                                this value should be 0.
+        KeyIdx                  This KeyIdx will set to IV's KeyID if bTxKey enabled
+        pCipherKey              Pointer to Cipher Key.
+        bUsePairewiseKeyTable   TRUE means saved the key in SharedKey table,
+                                otherwise PairewiseKey table
+        bTxKey                  This is the transmit key if enabled.
+
+    Return Value:
+        None
+
+    Note:
+        This routine will set the relative key stuff to Asic including WCID attribute,
+        Cipher Key, Cipher algorithm and IV/EIV.
+
+        IV/EIV will be update if this CipherKey is the transmission key because
+        ASIC will base on IV's KeyID value to select Cipher Key.
+
+        If bTxKey sets to FALSE, this is not the TX key, but it could be
+        RX key
+
+    	For AP mode bTxKey must be always set to TRUE.
+    ========================================================================
+*/
+VOID AsicAddKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR		KeyIdx,
+	IN PCIPHER_KEY	pCipherKey,
+	IN BOOLEAN		bUsePairewiseKeyTable,
+	IN BOOLEAN		bTxKey)
+{
+	ULONG	offset;
+//	ULONG   WCIDAttri = 0;
+	UCHAR	IV4 = 0;
+	PUCHAR		pKey = pCipherKey->Key;
+//	ULONG		KeyLen = pCipherKey->KeyLen;
+	PUCHAR		pTxMic = pCipherKey->TxMic;
+	PUCHAR		pRxMic = pCipherKey->RxMic;
+	PUCHAR		pTxtsc = pCipherKey->TxTsc;
+	UCHAR		CipherAlg = pCipherKey->CipherAlg;
+	SHAREDKEY_MODE_STRUC csr1;
+
+//	ASSERT(KeyLen <= MAX_LEN_OF_PEER_KEY);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> AsicAddKeyEntry\n"));
+	//
+	// 1.) decide key table offset
+	//
+	if (bUsePairewiseKeyTable)
+		offset = PAIRWISE_KEY_TABLE_BASE + (WCID * HW_KEY_ENTRY_SIZE);
+	else
+		offset = SHARED_KEY_TABLE_BASE + (4 * BssIndex + KeyIdx) * HW_KEY_ENTRY_SIZE;
+
+	//
+	// 2.) Set Key to Asic
+	//
+	//for (i = 0; i < KeyLen; i++)
+
+#ifdef RT2870
+	RTUSBMultiWrite(pAd, offset, pKey, MAX_LEN_OF_PEER_KEY);
+	offset += MAX_LEN_OF_PEER_KEY;
+
+	//
+	// 3.) Set MIC key if available
+	//
+	if (pTxMic)
+	{
+		RTUSBMultiWrite(pAd, offset, pTxMic, 8);
+	}
+	offset += LEN_TKIP_TXMICK;
+
+	if (pRxMic)
+	{
+		RTUSBMultiWrite(pAd, offset, pRxMic, 8);
+	}
+#endif // RT2870 //
+
+	//
+	// 4.) Modify IV/EIV if needs
+	//     This will force Asic to use this key ID by setting IV.
+	//
+	if (bTxKey)
+	{
+
+#ifdef RT2870
+		UINT32 tmpVal;
+
+		//
+		// Write IV
+		//
+		IV4 = (KeyIdx << 6);
+		if ((CipherAlg == CIPHER_TKIP) || (CipherAlg == CIPHER_TKIP_NO_MIC) ||(CipherAlg == CIPHER_AES))
+			IV4 |= 0x20;  // turn on extension bit means EIV existence
+
+		tmpVal = pTxtsc[1] + (((pTxtsc[1] | 0x20) & 0x7f) << 8) + (pTxtsc[0] << 16) + (IV4 << 24);
+		RTMP_IO_WRITE32(pAd, offset, tmpVal);
+
+		//
+		// Write EIV
+		//
+		offset += 4;
+		RTMP_IO_WRITE32(pAd, offset, *(PUINT32)&pCipherKey->TxTsc[2]);
+#endif // RT2870 //
+		AsicUpdateWCIDAttribute(pAd, WCID, BssIndex, CipherAlg, bUsePairewiseKeyTable);
+	}
+
+	if (!bUsePairewiseKeyTable)
+	{
+		//
+		// Only update the shared key security mode
+		//
+		RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE + 4 * (BssIndex / 2), &csr1.word);
+		if ((BssIndex % 2) == 0)
+		{
+			if (KeyIdx == 0)
+				csr1.field.Bss0Key0CipherAlg = CipherAlg;
+			else if (KeyIdx == 1)
+				csr1.field.Bss0Key1CipherAlg = CipherAlg;
+			else if (KeyIdx == 2)
+				csr1.field.Bss0Key2CipherAlg = CipherAlg;
+			else
+				csr1.field.Bss0Key3CipherAlg = CipherAlg;
+		}
+		else
+		{
+			if (KeyIdx == 0)
+				csr1.field.Bss1Key0CipherAlg = CipherAlg;
+			else if (KeyIdx == 1)
+				csr1.field.Bss1Key1CipherAlg = CipherAlg;
+			else if (KeyIdx == 2)
+				csr1.field.Bss1Key2CipherAlg = CipherAlg;
+			else
+				csr1.field.Bss1Key3CipherAlg = CipherAlg;
+		}
+		RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE + 4 * (BssIndex / 2), csr1.word);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== AsicAddKeyEntry\n"));
+}
+
+
+/*
+	========================================================================
+	Description:
+		Add Pair-wise key material into ASIC.
+		Update pairwise key, TxMic and RxMic to Asic Pair-wise key table
+
+    Return:
+	========================================================================
+*/
+VOID AsicAddPairwiseKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR		WCID,
+	IN CIPHER_KEY		 *pCipherKey)
+{
+	INT i;
+	ULONG 		offset;
+	PUCHAR		 pKey = pCipherKey->Key;
+	PUCHAR		 pTxMic = pCipherKey->TxMic;
+	PUCHAR		 pRxMic = pCipherKey->RxMic;
+#ifdef DBG
+	UCHAR		CipherAlg = pCipherKey->CipherAlg;
+#endif // DBG //
+
+	// EKEY
+	offset = PAIRWISE_KEY_TABLE_BASE + (WCID * HW_KEY_ENTRY_SIZE);
+#ifdef RT2870
+	RTUSBMultiWrite(pAd, offset, &pCipherKey->Key[0], MAX_LEN_OF_PEER_KEY);
+#endif // RT2870 //
+	for (i=0; i<MAX_LEN_OF_PEER_KEY; i+=4)
+	{
+		UINT32 Value;
+		RTMP_IO_READ32(pAd, offset + i, &Value);
+	}
+
+	offset += MAX_LEN_OF_PEER_KEY;
+
+	//  MIC KEY
+	if (pTxMic)
+	{
+#ifdef RT2870
+		RTUSBMultiWrite(pAd, offset, &pCipherKey->TxMic[0], 8);
+#endif // RT2870 //
+	}
+	offset += 8;
+	if (pRxMic)
+	{
+#ifdef RT2870
+		RTUSBMultiWrite(pAd, offset, &pCipherKey->RxMic[0], 8);
+#endif // RT2870 //
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("AsicAddPairwiseKeyEntry: WCID #%d Alg=%s\n",WCID, CipherName[CipherAlg]));
+	DBGPRINT(RT_DEBUG_TRACE,("	Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pKey[0],pKey[1],pKey[2],pKey[3],pKey[4],pKey[5],pKey[6],pKey[7],pKey[8],pKey[9],pKey[10],pKey[11],pKey[12],pKey[13],pKey[14],pKey[15]));
+	if (pRxMic)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("	Rx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pRxMic[0],pRxMic[1],pRxMic[2],pRxMic[3],pRxMic[4],pRxMic[5],pRxMic[6],pRxMic[7]));
+	}
+	if (pTxMic)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("	Tx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pTxMic[0],pTxMic[1],pTxMic[2],pTxMic[3],pTxMic[4],pTxMic[5],pTxMic[6],pTxMic[7]));
+	}
+}
+/*
+	========================================================================
+	Description:
+		Remove Pair-wise key material from ASIC.
+
+    Return:
+	========================================================================
+*/
+VOID AsicRemovePairwiseKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 BssIdx,
+	IN UCHAR		 Wcid)
+{
+	ULONG		WCIDAttri;
+	USHORT		offset;
+
+	// re-set the entry's WCID attribute as OPEN-NONE.
+	offset = MAC_WCID_ATTRIBUTE_BASE + (Wcid * HW_WCID_ATTRI_SIZE);
+	WCIDAttri = (BssIdx<<4) | PAIRWISEKEYTABLE;
+	RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+}
+
+BOOLEAN AsicSendCommandToMcu(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 Command,
+	IN UCHAR		 Token,
+	IN UCHAR		 Arg0,
+	IN UCHAR		 Arg1)
+{
+	HOST_CMD_CSR_STRUC	H2MCmd;
+	H2M_MAILBOX_STRUC	H2MMailbox;
+	ULONG				i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, H2M_MAILBOX_CSR, &H2MMailbox.word);
+		if (H2MMailbox.field.Owner == 0)
+			break;
+
+		RTMPusecDelay(2);
+	} while(i++ < 100);
+
+	if (i >= 100)
+	{
+		{
+		DBGPRINT_ERR(("H2M_MAILBOX still hold by MCU. command fail\n"));
+		}
+		return FALSE;
+	}
+
+
+	H2MMailbox.field.Owner	  = 1;	   // pass ownership to MCU
+	H2MMailbox.field.CmdToken = Token;
+	H2MMailbox.field.HighByte = Arg1;
+	H2MMailbox.field.LowByte  = Arg0;
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CSR, H2MMailbox.word);
+
+	H2MCmd.word 			  = 0;
+	H2MCmd.field.HostCommand  = Command;
+	RTMP_IO_WRITE32(pAd, HOST_CMD_CSR, H2MCmd.word);
+
+	if (Command != 0x80)
+	{
+	}
+
+	return TRUE;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Verify the support rate for different PHY type
+
+	Arguments:
+		pAd 				Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPCheckRates(
+	IN		PRTMP_ADAPTER	pAd,
+	IN OUT	UCHAR			SupRate[],
+	IN OUT	UCHAR			*SupRateLen)
+{
+	UCHAR	RateIdx, i, j;
+	UCHAR	NewRate[12], NewRateLen;
+
+	NewRateLen = 0;
+
+	if (pAd->CommonCfg.PhyMode == PHY_11B)
+		RateIdx = 4;
+	else
+		RateIdx = 12;
+
+	// Check for support rates exclude basic rate bit
+	for (i = 0; i < *SupRateLen; i++)
+		for (j = 0; j < RateIdx; j++)
+			if ((SupRate[i] & 0x7f) == RateIdTo500Kbps[j])
+				NewRate[NewRateLen++] = SupRate[i];
+
+	*SupRateLen = NewRateLen;
+	NdisMoveMemory(SupRate, NewRate, NewRateLen);
+}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+BOOLEAN RTMPCheckChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		CentralChannel,
+	IN UCHAR		Channel)
+{
+	UCHAR		k;
+	UCHAR		UpperChannel = 0, LowerChannel = 0;
+	UCHAR		NoEffectChannelinList = 0;
+
+	// Find upper and lower channel according to 40MHz current operation.
+	if (CentralChannel < Channel)
+	{
+		UpperChannel = Channel;
+		if (CentralChannel > 2)
+			LowerChannel = CentralChannel - 2;
+		else
+			return FALSE;
+	}
+	else if (CentralChannel > Channel)
+	{
+		UpperChannel = CentralChannel + 2;
+		LowerChannel = Channel;
+	}
+
+	for (k = 0;k < pAd->ChannelListNum;k++)
+	{
+		if (pAd->ChannelList[k].Channel == UpperChannel)
+		{
+			NoEffectChannelinList ++;
+		}
+		if (pAd->ChannelList[k].Channel == LowerChannel)
+		{
+			NoEffectChannelinList ++;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("Total Channel in Channel List = [%d]\n", NoEffectChannelinList));
+	if (NoEffectChannelinList == 2)
+		return TRUE;
+	else
+		return FALSE;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Verify the support rate for HT phy type
+
+	Arguments:
+		pAd 				Pointer to our adapter
+
+	Return Value:
+		FALSE if pAd->CommonCfg.SupportedHtPhy doesn't accept the pHtCapability.  (AP Mode)
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+BOOLEAN 	RTMPCheckHt(
+	IN	PRTMP_ADAPTER			pAd,
+	IN	UCHAR					Wcid,
+	IN 	HT_CAPABILITY_IE		*pHtCapability,
+	IN 	ADD_HT_INFO_IE			*pAddHtInfo)
+{
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	// If use AMSDU, set flag.
+	if (pAd->CommonCfg.DesiredHtPhy.AmsduEnable)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_AMSDU_INUSED);
+	// Save Peer Capability
+	if (pHtCapability->HtCapInfo.ShortGIfor20)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_SGI20_CAPABLE);
+	if (pHtCapability->HtCapInfo.ShortGIfor40)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_SGI40_CAPABLE);
+	if (pHtCapability->HtCapInfo.TxSTBC)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_TxSTBC_CAPABLE);
+	if (pHtCapability->HtCapInfo.RxSTBC)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_RxSTBC_CAPABLE);
+	if (pAd->CommonCfg.bRdg && pHtCapability->ExtHtCapInfo.RDGSupport)
+	{
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_RDG_CAPABLE);
+	}
+
+	if (Wcid < MAX_LEN_OF_MAC_TABLE)
+	{
+		pAd->MacTab.Content[Wcid].MpduDensity = pHtCapability->HtCapParm.MpduDensity;
+	}
+
+	// Will check ChannelWidth for MCSSet[4] below
+	pAd->MlmeAux.HtCapability.MCSSet[4] = 0x1;
+    switch (pAd->CommonCfg.RxStream)
+	{
+		case 1:
+			pAd->MlmeAux.HtCapability.MCSSet[0] = 0xff;
+			pAd->MlmeAux.HtCapability.MCSSet[1] = 0x00;
+            pAd->MlmeAux.HtCapability.MCSSet[2] = 0x00;
+            pAd->MlmeAux.HtCapability.MCSSet[3] = 0x00;
+			break;
+		case 2:
+			pAd->MlmeAux.HtCapability.MCSSet[0] = 0xff;
+			pAd->MlmeAux.HtCapability.MCSSet[1] = 0xff;
+            pAd->MlmeAux.HtCapability.MCSSet[2] = 0x00;
+            pAd->MlmeAux.HtCapability.MCSSet[3] = 0x00;
+			break;
+		case 3:
+			pAd->MlmeAux.HtCapability.MCSSet[0] = 0xff;
+			pAd->MlmeAux.HtCapability.MCSSet[1] = 0xff;
+            pAd->MlmeAux.HtCapability.MCSSet[2] = 0xff;
+            pAd->MlmeAux.HtCapability.MCSSet[3] = 0x00;
+			break;
+	}
+
+	pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth = pAddHtInfo->AddHtInfo.RecomWidth & pAd->CommonCfg.DesiredHtPhy.ChannelWidth;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("RTMPCheckHt:: HtCapInfo.ChannelWidth=%d, RecomWidth=%d, DesiredHtPhy.ChannelWidth=%d, BW40MAvailForA/G=%d/%d, PhyMode=%d \n",
+		pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth, pAddHtInfo->AddHtInfo.RecomWidth, pAd->CommonCfg.DesiredHtPhy.ChannelWidth,
+		pAd->NicConfig2.field.BW40MAvailForA, pAd->NicConfig2.field.BW40MAvailForG, pAd->CommonCfg.PhyMode));
+
+	pAd->MlmeAux.HtCapability.HtCapInfo.GF =  pHtCapability->HtCapInfo.GF &pAd->CommonCfg.DesiredHtPhy.GF;
+
+	// Send Assoc Req with my HT capability.
+	pAd->MlmeAux.HtCapability.HtCapInfo.AMsduSize =  pAd->CommonCfg.DesiredHtPhy.AmsduSize;
+	pAd->MlmeAux.HtCapability.HtCapInfo.MimoPs =  pAd->CommonCfg.DesiredHtPhy.MimoPs;
+	pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor20 =  (pAd->CommonCfg.DesiredHtPhy.ShortGIfor20) & (pHtCapability->HtCapInfo.ShortGIfor20);
+	pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor40 =  (pAd->CommonCfg.DesiredHtPhy.ShortGIfor40) & (pHtCapability->HtCapInfo.ShortGIfor40);
+	pAd->MlmeAux.HtCapability.HtCapInfo.TxSTBC =  (pAd->CommonCfg.DesiredHtPhy.TxSTBC)&(pHtCapability->HtCapInfo.RxSTBC);
+	pAd->MlmeAux.HtCapability.HtCapInfo.RxSTBC =  (pAd->CommonCfg.DesiredHtPhy.RxSTBC)&(pHtCapability->HtCapInfo.TxSTBC);
+	pAd->MlmeAux.HtCapability.HtCapParm.MaxRAmpduFactor = pAd->CommonCfg.DesiredHtPhy.MaxRAmpduFactor;
+    pAd->MlmeAux.HtCapability.HtCapParm.MpduDensity = pAd->CommonCfg.HtCapability.HtCapParm.MpduDensity;
+	pAd->MlmeAux.HtCapability.ExtHtCapInfo.PlusHTC = pHtCapability->ExtHtCapInfo.PlusHTC;
+	pAd->MacTab.Content[Wcid].HTCapability.ExtHtCapInfo.PlusHTC = pHtCapability->ExtHtCapInfo.PlusHTC;
+	if (pAd->CommonCfg.bRdg)
+	{
+		pAd->MlmeAux.HtCapability.ExtHtCapInfo.RDGSupport = pHtCapability->ExtHtCapInfo.RDGSupport;
+        pAd->MlmeAux.HtCapability.ExtHtCapInfo.PlusHTC = 1;
+	}
+
+    if (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_20)
+        pAd->MlmeAux.HtCapability.MCSSet[4] = 0x0;  // BW20 can't transmit MCS32
+
+	COPY_AP_HTSETTINGS_FROM_BEACON(pAd, pHtCapability);
+	return TRUE;
+}
+#endif // DOT11_N_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	========================================================================
+
+	Routine Description:
+		Verify the support rate for different PHY type
+
+	Arguments:
+		pAd 				Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID RTMPUpdateMlmeRate(
+	IN PRTMP_ADAPTER	pAd)
+{
+	UCHAR	MinimumRate;
+	UCHAR	ProperMlmeRate; //= RATE_54;
+	UCHAR	i, j, RateIdx = 12; //1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54
+	BOOLEAN	bMatch = FALSE;
+
+	switch (pAd->CommonCfg.PhyMode)
+	{
+		case PHY_11B:
+			ProperMlmeRate = RATE_11;
+			MinimumRate = RATE_1;
+			break;
+		case PHY_11BG_MIXED:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11ABGN_MIXED:
+		case PHY_11BGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			if ((pAd->MlmeAux.SupRateLen == 4) &&
+				(pAd->MlmeAux.ExtRateLen == 0))
+				// B only AP
+				ProperMlmeRate = RATE_11;
+			else
+				ProperMlmeRate = RATE_24;
+
+			if (pAd->MlmeAux.Channel <= 14)
+				MinimumRate = RATE_1;
+			else
+				MinimumRate = RATE_6;
+			break;
+		case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11N_2_4G:	// rt2860 need to check mlmerate for 802.11n
+		case PHY_11GN_MIXED:
+		case PHY_11AGN_MIXED:
+		case PHY_11AN_MIXED:
+		case PHY_11N_5G:
+#endif // DOT11_N_SUPPORT //
+			ProperMlmeRate = RATE_24;
+			MinimumRate = RATE_6;
+			break;
+		case PHY_11ABG_MIXED:
+			ProperMlmeRate = RATE_24;
+			if (pAd->MlmeAux.Channel <= 14)
+			   MinimumRate = RATE_1;
+			else
+				MinimumRate = RATE_6;
+			break;
+		default: // error
+			ProperMlmeRate = RATE_1;
+			MinimumRate = RATE_1;
+			break;
+	}
+
+	for (i = 0; i < pAd->MlmeAux.SupRateLen; i++)
+	{
+		for (j = 0; j < RateIdx; j++)
+		{
+			if ((pAd->MlmeAux.SupRate[i] & 0x7f) == RateIdTo500Kbps[j])
+			{
+				if (j == ProperMlmeRate)
+				{
+					bMatch = TRUE;
+					break;
+				}
+			}
+		}
+
+		if (bMatch)
+			break;
+	}
+
+	if (bMatch == FALSE)
+	{
+		for (i = 0; i < pAd->MlmeAux.ExtRateLen; i++)
+		{
+			for (j = 0; j < RateIdx; j++)
+			{
+				if ((pAd->MlmeAux.ExtRate[i] & 0x7f) == RateIdTo500Kbps[j])
+				{
+					if (j == ProperMlmeRate)
+					{
+						bMatch = TRUE;
+						break;
+					}
+				}
+			}
+
+			if (bMatch)
+				break;
+		}
+	}
+
+	if (bMatch == FALSE)
+	{
+		ProperMlmeRate = MinimumRate;
+	}
+
+	pAd->CommonCfg.MlmeRate = MinimumRate;
+	pAd->CommonCfg.RtsRate = ProperMlmeRate;
+	if (pAd->CommonCfg.MlmeRate >= RATE_6)
+	{
+		pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+		pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MODE = MODE_OFDM;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+	}
+	else
+	{
+		pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+		pAd->CommonCfg.MlmeTransmit.field.MCS = pAd->CommonCfg.MlmeRate;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MODE = MODE_CCK;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MCS = pAd->CommonCfg.MlmeRate;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPUpdateMlmeRate ==>   MlmeTransmit = 0x%x  \n" , pAd->CommonCfg.MlmeTransmit.word));
+}
+
+CHAR RTMPMaxRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN CHAR				Rssi0,
+	IN CHAR				Rssi1,
+	IN CHAR				Rssi2)
+{
+	CHAR	larger = -127;
+
+	if ((pAd->Antenna.field.RxPath == 1) && (Rssi0 != 0))
+	{
+		larger = Rssi0;
+	}
+
+	if ((pAd->Antenna.field.RxPath >= 2) && (Rssi1 != 0))
+	{
+		larger = max(Rssi0, Rssi1);
+	}
+
+	if ((pAd->Antenna.field.RxPath == 3) && (Rssi2 != 0))
+	{
+		larger = max(larger, Rssi2);
+	}
+
+	if (larger == -127)
+		larger = 0;
+
+	return larger;
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Periodic evaluate antenna link status
+
+    Arguments:
+        pAd         - Adapter pointer
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID AsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd)
+{
+	UCHAR	BBPR3 = 0;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS	|
+								fRTMP_ADAPTER_HALT_IN_PROGRESS	|
+								fRTMP_ADAPTER_RADIO_OFF			|
+								fRTMP_ADAPTER_NIC_NOT_EXIST		|
+								fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			return;
+
+		if (pAd->StaCfg.Psm == PWR_SAVE)
+			return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
+	BBPR3 &= (~0x18);
+	if(pAd->Antenna.field.RxPath == 3)
+	{
+		BBPR3 |= (0x10);
+	}
+	else if(pAd->Antenna.field.RxPath == 2)
+	{
+		BBPR3 |= (0x8);
+	}
+	else if(pAd->Antenna.field.RxPath == 1)
+	{
+		BBPR3 |= (0x0);
+	}
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPR3);
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+		)
+	{
+		ULONG	TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+								pAd->RalinkCounters.OneSecTxRetryOkCount +
+								pAd->RalinkCounters.OneSecTxFailCount;
+
+		if (TxTotalCnt > 50)
+		{
+			RTMPSetTimer(&pAd->Mlme.RxAntEvalTimer, 20);
+			pAd->Mlme.bLowThroughput = FALSE;
+		}
+		else
+		{
+			RTMPSetTimer(&pAd->Mlme.RxAntEvalTimer, 300);
+			pAd->Mlme.bLowThroughput = TRUE;
+		}
+	}
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        After evaluation, check antenna link status
+
+    Arguments:
+        pAd         - Adapter pointer
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID AsicRxAntEvalTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER	*pAd = (RTMP_ADAPTER *)FunctionContext;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR			BBPR3 = 0;
+	CHAR			larger = -127, rssi0, rssi1, rssi2;
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)	||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)		||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF)			||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+			return;
+
+		if (pAd->StaCfg.Psm == PWR_SAVE)
+			return;
+
+
+		// if the traffic is low, use average rssi as the criteria
+		if (pAd->Mlme.bLowThroughput == TRUE)
+		{
+			rssi0 = pAd->StaCfg.RssiSample.LastRssi0;
+			rssi1 = pAd->StaCfg.RssiSample.LastRssi1;
+			rssi2 = pAd->StaCfg.RssiSample.LastRssi2;
+		}
+		else
+		{
+			rssi0 = pAd->StaCfg.RssiSample.AvgRssi0;
+			rssi1 = pAd->StaCfg.RssiSample.AvgRssi1;
+			rssi2 = pAd->StaCfg.RssiSample.AvgRssi2;
+		}
+
+		if(pAd->Antenna.field.RxPath == 3)
+		{
+			larger = max(rssi0, rssi1);
+
+			if (larger > (rssi2 + 20))
+				pAd->Mlme.RealRxPath = 2;
+			else
+				pAd->Mlme.RealRxPath = 3;
+		}
+		else if(pAd->Antenna.field.RxPath == 2)
+		{
+			if (rssi0 > (rssi1 + 20))
+				pAd->Mlme.RealRxPath = 1;
+			else
+				pAd->Mlme.RealRxPath = 2;
+		}
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
+		BBPR3 &= (~0x18);
+		if(pAd->Mlme.RealRxPath == 3)
+		{
+			BBPR3 |= (0x10);
+		}
+		else if(pAd->Mlme.RealRxPath == 2)
+		{
+			BBPR3 |= (0x8);
+		}
+		else if(pAd->Mlme.RealRxPath == 1)
+		{
+			BBPR3 |= (0x0);
+		}
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPR3);
+	}
+
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+
+
+VOID APSDPeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+		return;
+
+	pAd->CommonCfg.TriggerTimerCount++;
+
+// Driver should not send trigger frame, it should be send by application layer
+/*
+	if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable
+		&& (pAd->CommonCfg.bNeedSendTriggerFrame ||
+		(((pAd->CommonCfg.TriggerTimerCount%20) == 19) && (!pAd->CommonCfg.bAPSDAC_BE || !pAd->CommonCfg.bAPSDAC_BK || !pAd->CommonCfg.bAPSDAC_VI || !pAd->CommonCfg.bAPSDAC_VO))))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("Sending trigger frame and enter service period when support APSD\n"));
+		RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+		pAd->CommonCfg.bNeedSendTriggerFrame = FALSE;
+		pAd->CommonCfg.TriggerTimerCount = 0;
+		pAd->CommonCfg.bInServicePeriod = TRUE;
+	}*/
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Set/reset MAC registers according to bPiggyBack parameter
+
+    Arguments:
+        pAd         - Adapter pointer
+        bPiggyBack  - Enable / Disable Piggy-Back
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID RTMPSetPiggyBack(
+    IN PRTMP_ADAPTER    pAd,
+    IN BOOLEAN          bPiggyBack)
+{
+	TX_LINK_CFG_STRUC  TxLinkCfg;
+
+	RTMP_IO_READ32(pAd, TX_LINK_CFG, &TxLinkCfg.word);
+
+	TxLinkCfg.field.TxCFAckEn = bPiggyBack;
+	RTMP_IO_WRITE32(pAd, TX_LINK_CFG, TxLinkCfg.word);
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        check if this entry need to switch rate automatically
+
+    Arguments:
+        pAd
+        pEntry
+
+    Return Value:
+        TURE
+        FALSE
+
+    ========================================================================
+*/
+BOOLEAN RTMPCheckEntryEnableAutoRateSwitch(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry)
+{
+	BOOLEAN		result = TRUE;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// only associated STA counts
+		if (pEntry && (pEntry->ValidAsCLI) && (pEntry->Sst == SST_ASSOC))
+		{
+			result = pAd->StaCfg.bAutoTxRateSwitch;
+		}
+		else
+			result = FALSE;
+
+#ifdef QOS_DLS_SUPPORT
+		if (pEntry && (pEntry->ValidAsDls))
+			result = pAd->StaCfg.bAutoTxRateSwitch;
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+	return result;
+}
+
+
+BOOLEAN RTMPAutoRateSwitchCheck(
+	IN PRTMP_ADAPTER    pAd)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->StaCfg.bAutoTxRateSwitch)
+			return TRUE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+	return FALSE;
+}
+
+
+/*
+    ========================================================================
+    Routine Description:
+        check if this entry need to fix tx legacy rate
+
+    Arguments:
+        pAd
+        pEntry
+
+    Return Value:
+        TURE
+        FALSE
+
+    ========================================================================
+*/
+UCHAR RTMPStaFixedTxMode(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry)
+{
+	UCHAR	tx_mode = FIXED_TXMODE_HT;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		tx_mode = (UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	return tx_mode;
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Overwrite HT Tx Mode by Fixed Legency Tx Mode, if specified.
+
+    Arguments:
+        pAd
+        pEntry
+
+    Return Value:
+        TURE
+        FALSE
+
+    ========================================================================
+*/
+VOID RTMPUpdateLegacyTxSetting(
+		UCHAR				fixed_tx_mode,
+		PMAC_TABLE_ENTRY	pEntry)
+{
+	HTTRANSMIT_SETTING TransmitSetting;
+
+	if (fixed_tx_mode == FIXED_TXMODE_HT)
+		return;
+
+	TransmitSetting.word = 0;
+
+	TransmitSetting.field.MODE = pEntry->HTPhyMode.field.MODE;
+	TransmitSetting.field.MCS = pEntry->HTPhyMode.field.MCS;
+
+	if (fixed_tx_mode == FIXED_TXMODE_CCK)
+	{
+		TransmitSetting.field.MODE = MODE_CCK;
+		// CCK mode allow MCS 0~3
+		if (TransmitSetting.field.MCS > MCS_3)
+			TransmitSetting.field.MCS = MCS_3;
+	}
+	else
+	{
+		TransmitSetting.field.MODE = MODE_OFDM;
+		// OFDM mode allow MCS 0~7
+		if (TransmitSetting.field.MCS > MCS_7)
+			TransmitSetting.field.MCS = MCS_7;
+	}
+
+	if (pEntry->HTPhyMode.field.MODE >= TransmitSetting.field.MODE)
+	{
+		pEntry->HTPhyMode.word = TransmitSetting.word;
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPUpdateLegacyTxSetting : wcid-%d, MODE=%s, MCS=%d \n",
+				pEntry->Aid, GetPhyMode(pEntry->HTPhyMode.field.MODE), pEntry->HTPhyMode.field.MCS));
+	}
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	==========================================================================
+	Description:
+		dynamic tune BBP R66 to find a balance between sensibility and
+		noise isolation
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicStaBbpTuning(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR	OrigR66Value = 0, R66;//, R66UpperBound = 0x30, R66LowerBound = 0x30;
+	CHAR	Rssi;
+
+	// 2860C did not support Fase CCA, therefore can't tune
+	if (pAd->MACVersion == 0x28600100)
+		return;
+
+	//
+	// work as a STA
+	//
+	if (pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE)  // no R66 tuning when SCANNING
+		return;
+
+	if ((pAd->OpMode == OPMODE_STA)
+		&& (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+			)
+		&& !(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+		)
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &OrigR66Value);
+		R66 = OrigR66Value;
+
+		if (pAd->Antenna.field.RxPath > 1)
+			Rssi = (pAd->StaCfg.RssiSample.AvgRssi0 + pAd->StaCfg.RssiSample.AvgRssi1) >> 1;
+		else
+			Rssi = pAd->StaCfg.RssiSample.AvgRssi0;
+
+		if (pAd->LatchRfRegs.Channel <= 14)
+		{	//BG band
+#ifdef RT2870
+			// RT3070 is a no LNA solution, it should have different control regarding to AGC gain control
+			// Otherwise, it will have some throughput side effect when low RSSI
+			if (IS_RT3070(pAd))
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = 0x1C + 2*GET_LNA_GAIN(pAd) + 0x20;
+					if (OrigR66Value != R66)
+					{
+						RTUSBWriteBBPRegister(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x1C + 2*GET_LNA_GAIN(pAd);
+					if (OrigR66Value != R66)
+					{
+						RTUSBWriteBBPRegister(pAd, BBP_R66, R66);
+					}
+				}
+			}
+			else
+#endif // RT2870 //
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = (0x2E + GET_LNA_GAIN(pAd)) + 0x10;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x2E + GET_LNA_GAIN(pAd);
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+			}
+		}
+		else
+		{	//A band
+			if (pAd->CommonCfg.BBPCurrentBW == BW_20)
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = 0x32 + (GET_LNA_GAIN(pAd)*5)/3 + 0x10;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x32 + (GET_LNA_GAIN(pAd)*5)/3;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+			}
+			else
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = 0x3A + (GET_LNA_GAIN(pAd)*5)/3 + 0x10;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x3A + (GET_LNA_GAIN(pAd)*5)/3;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+			}
+		}
+
+
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+VOID RTMPSetAGCInitValue(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			BandWidth)
+{
+	UCHAR	R66 = 0x30;
+
+	if (pAd->LatchRfRegs.Channel <= 14)
+	{	// BG band
+		R66 = 0x2E + GET_LNA_GAIN(pAd);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+	}
+	else
+	{	//A band
+		if (BandWidth == BW_20)
+		{
+			R66 = (UCHAR)(0x32 + (GET_LNA_GAIN(pAd)*5)/3);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+#ifdef DOT11_N_SUPPORT
+		else
+		{
+			R66 = (UCHAR)(0x3A + (GET_LNA_GAIN(pAd)*5)/3);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+#endif // DOT11_N_SUPPORT //
+	}
+
+}
+
+VOID AsicTurnOffRFClk(
+	IN PRTMP_ADAPTER pAd,
+	IN	UCHAR		Channel)
+{
+
+	// RF R2 bit 18 = 0
+	UINT32			R1 = 0, R2 = 0, R3 = 0;
+	UCHAR			index;
+	RTMP_RF_REGS	*RFRegTable;
+
+	RFRegTable = RF2850RegTable;
+
+	switch (pAd->RfIcType)
+	{
+		case RFIC_2820:
+		case RFIC_2850:
+		case RFIC_2720:
+		case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R1 = RFRegTable[index].R1 & 0xffffdfff;
+					R2 = RFRegTable[index].R2 & 0xfffbffff;
+					R3 = RFRegTable[index].R3 & 0xfff3ffff;
+
+					RTMP_RF_IO_WRITE32(pAd, R1);
+					RTMP_RF_IO_WRITE32(pAd, R2);
+
+					// Program R1b13 to 1, R3/b18,19 to 0, R2b18 to 0.
+					// Set RF R2 bit18=0, R3 bit[18:19]=0
+					//if (pAd->StaCfg.bRadio == FALSE)
+					if (1)
+					{
+						RTMP_RF_IO_WRITE32(pAd, R3);
+
+						DBGPRINT(RT_DEBUG_TRACE, ("AsicTurnOffRFClk#%d(RF=%d, ) , R2=0x%08x,  R3 = 0x%08x \n",
+							Channel, pAd->RfIcType, R2, R3));
+					}
+					else
+						DBGPRINT(RT_DEBUG_TRACE, ("AsicTurnOffRFClk#%d(RF=%d, ) , R2=0x%08x \n",
+							Channel, pAd->RfIcType, R2));
+					break;
+				}
+			}
+			break;
+
+		default:
+			break;
+	}
+}
+
+
+VOID AsicTurnOnRFClk(
+	IN PRTMP_ADAPTER pAd,
+	IN	UCHAR			Channel)
+{
+
+	// RF R2 bit 18 = 0
+	UINT32			R1 = 0, R2 = 0, R3 = 0;
+	UCHAR			index;
+	RTMP_RF_REGS	*RFRegTable;
+
+	RFRegTable = RF2850RegTable;
+
+	switch (pAd->RfIcType)
+	{
+		case RFIC_2820:
+		case RFIC_2850:
+		case RFIC_2720:
+		case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R3 = pAd->LatchRfRegs.R3;
+					R3 &= 0xfff3ffff;
+					R3 |= 0x00080000;
+					RTMP_RF_IO_WRITE32(pAd, R3);
+
+					R1 = RFRegTable[index].R1;
+					RTMP_RF_IO_WRITE32(pAd, R1);
+
+					R2 = RFRegTable[index].R2;
+					if (pAd->Antenna.field.TxPath == 1)
+					{
+						R2 |= 0x4000;	// If TXpath is 1, bit 14 = 1;
+					}
+
+					if (pAd->Antenna.field.RxPath == 2)
+					{
+						R2 |= 0x40;	// write 1 to off Rxpath.
+					}
+					else if (pAd->Antenna.field.RxPath == 1)
+					{
+						R2 |= 0x20040;	// write 1 to off RxPath
+					}
+					RTMP_RF_IO_WRITE32(pAd, R2);
+
+					break;
+				}
+			}
+			break;
+
+		default:
+			break;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicTurnOnRFClk#%d(RF=%d, ) , R2=0x%08x\n",
+		Channel,
+		pAd->RfIcType,
+		R2));
+}
+
diff --git a/drivers/staging/rt2870/common/netif_block.c b/drivers/staging/rt2870/common/netif_block.c
new file mode 100644
index 000000000000..d3f7d087e7f0
--- /dev/null
+++ b/drivers/staging/rt2870/common/netif_block.c
@@ -0,0 +1,144 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "../rt_config.h"
+#include "netif_block.h"
+
+static NETIF_ENTRY freeNetIfEntryPool[FREE_NETIF_POOL_SIZE];
+static LIST_HEADER freeNetIfEntryList;
+
+void initblockQueueTab(
+	IN PRTMP_ADAPTER pAd)
+{
+	int i;
+
+	initList(&freeNetIfEntryList);
+	for (i = 0; i < FREE_NETIF_POOL_SIZE; i++)
+		insertTailList(&freeNetIfEntryList, (PLIST_ENTRY)&freeNetIfEntryPool[i]);
+
+	for (i=0; i < NUM_OF_TX_RING; i++)
+		initList(&pAd->blockQueueTab[i].NetIfList);
+
+	return;
+}
+
+BOOLEAN blockNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry,
+	IN PNET_DEV pNetDev)
+{
+	PNETIF_ENTRY pNetIfEntry = NULL;
+
+	if ((pNetIfEntry = (PNETIF_ENTRY)removeHeadList(&freeNetIfEntryList)) != NULL)
+	{
+		netif_stop_queue(pNetDev);
+		pNetIfEntry->pNetDev = pNetDev;
+		insertTailList(&pBlockQueueEntry->NetIfList, (PLIST_ENTRY)pNetIfEntry);
+
+		pBlockQueueEntry->SwTxQueueBlockFlag = TRUE;
+		DBGPRINT(RT_DEBUG_TRACE, ("netif_stop_queue(%s)\n", pNetDev->name));
+	}
+	else
+		return FALSE;
+
+	return TRUE;
+}
+
+VOID releaseNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry)
+{
+	PNETIF_ENTRY pNetIfEntry = NULL;
+	PLIST_HEADER pNetIfList = &pBlockQueueEntry->NetIfList;
+
+	while((pNetIfEntry = (PNETIF_ENTRY)removeHeadList(pNetIfList)) !=  NULL)
+	{
+		PNET_DEV pNetDev = pNetIfEntry->pNetDev;
+		netif_wake_queue(pNetDev);
+		insertTailList(&freeNetIfEntryList, (PLIST_ENTRY)pNetIfEntry);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("netif_wake_queue(%s)\n", pNetDev->name));
+	}
+	pBlockQueueEntry->SwTxQueueBlockFlag = FALSE;
+	return;
+}
+
+
+VOID StopNetIfQueue(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR QueIdx,
+	IN PNDIS_PACKET pPacket)
+{
+	PNET_DEV NetDev = NULL;
+	UCHAR IfIdx = 0;
+	BOOLEAN valid = FALSE;
+
+#ifdef APCLI_SUPPORT
+	if (RTMP_GET_PACKET_NET_DEVICE(pPacket) >= MIN_NET_DEVICE_FOR_APCLI)
+	{
+		IfIdx = (RTMP_GET_PACKET_NET_DEVICE(pPacket) - MIN_NET_DEVICE_FOR_APCLI) % MAX_APCLI_NUM;
+		NetDev = pAd->ApCfg.ApCliTab[IfIdx].dev;
+	}
+	else
+#endif // APCLI_SUPPORT //
+#ifdef WDS_SUPPORT
+	if (RTMP_GET_PACKET_NET_DEVICE(pPacket) >= MIN_NET_DEVICE_FOR_WDS)
+	{
+		IfIdx = (RTMP_GET_PACKET_NET_DEVICE(pPacket) - MIN_NET_DEVICE_FOR_WDS) % MAX_WDS_ENTRY;
+		NetDev = pAd->WdsTab.WdsEntry[IfIdx].dev;
+	}
+	else
+#endif // WDS_SUPPORT //
+	{
+#ifdef MBSS_SUPPORT
+		if (pAd->OpMode == OPMODE_AP)
+		{
+			IfIdx = (RTMP_GET_PACKET_NET_DEVICE(pPacket) - MIN_NET_DEVICE_FOR_MBSSID) % MAX_MBSSID_NUM;
+			NetDev = pAd->ApCfg.MBSSID[IfIdx].MSSIDDev;
+		}
+		else
+		{
+			IfIdx = MAIN_MBSSID;
+			NetDev = pAd->net_dev;
+		}
+#else
+		IfIdx = MAIN_MBSSID;
+		NetDev = pAd->net_dev;
+#endif
+	}
+
+	// WMM support 4 software queues.
+	// One software queue full doesn't mean device have no capbility to transmit packet.
+	// So disable block Net-If queue function while WMM enable.
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		valid = (pAd->CommonCfg.bWmmCapable == TRUE) ? FALSE : TRUE;
+#endif // CONFIG_STA_SUPPORT //
+
+	if (valid)
+		blockNetIf(&pAd->blockQueueTab[QueIdx], NetDev);
+	return;
+}
+
diff --git a/drivers/staging/rt2870/common/rtmp_init.c b/drivers/staging/rt2870/common/rtmp_init.c
new file mode 100644
index 000000000000..870a00da3dac
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtmp_init.c
@@ -0,0 +1,4132 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_init.c
+
+	Abstract:
+	Miniport generic portion header file
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Paul Lin    2002-08-01    created
+    John Chang  2004-08-20    RT2561/2661 use scatter-gather scheme
+    Jan Lee  2006-09-15    RT2860. Change for 802.11n , EEPROM, Led, BA, HT.
+*/
+#include "../rt_config.h"
+#include "firmware.h"
+
+//#define BIN_IN_FILE /* use *.bin firmware */
+
+UCHAR    BIT8[] = {0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80};
+ULONG    BIT32[] = {0x00000001, 0x00000002, 0x00000004, 0x00000008,
+					0x00000010, 0x00000020, 0x00000040, 0x00000080,
+					0x00000100, 0x00000200, 0x00000400, 0x00000800,
+					0x00001000, 0x00002000, 0x00004000, 0x00008000,
+					0x00010000, 0x00020000, 0x00040000, 0x00080000,
+					0x00100000, 0x00200000, 0x00400000, 0x00800000,
+					0x01000000, 0x02000000, 0x04000000, 0x08000000,
+					0x10000000, 0x20000000, 0x40000000, 0x80000000};
+
+char*   CipherName[] = {"none","wep64","wep128","TKIP","AES","CKIP64","CKIP128"};
+
+const unsigned short ccitt_16Table[] = {
+	0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
+	0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
+	0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
+	0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
+	0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
+	0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
+	0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
+	0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
+	0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
+	0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
+	0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
+	0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
+	0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
+	0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
+	0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
+	0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
+	0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
+	0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
+	0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
+	0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
+	0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
+	0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+	0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
+	0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
+	0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
+	0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
+	0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
+	0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
+	0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
+	0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
+	0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
+	0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
+};
+#define ByteCRC16(v, crc) \
+	(unsigned short)((crc << 8) ^  ccitt_16Table[((crc >> 8) ^ (v)) & 255])
+
+unsigned char BitReverse(unsigned char x)
+{
+	int i;
+	unsigned char Temp=0;
+	for(i=0; ; i++)
+	{
+		if(x & 0x80)	Temp |= 0x80;
+		if(i==7)		break;
+		x	<<= 1;
+		Temp >>= 1;
+	}
+	return Temp;
+}
+
+//
+// BBP register initialization set
+//
+REG_PAIR   BBPRegTable[] = {
+	{BBP_R65,		0x2C},		// fix rssi issue
+	{BBP_R66,		0x38},	// Also set this default value to pAd->BbpTuning.R66CurrentValue at initial
+	{BBP_R69,		0x12},
+	{BBP_R70,		0xa},	// BBP_R70 will change to 0x8 in ApStartUp and LinkUp for rt2860C, otherwise value is 0xa
+	{BBP_R73,		0x10},
+	{BBP_R81,		0x37},
+	{BBP_R82,		0x62},
+	{BBP_R83,		0x6A},
+	{BBP_R84,		0x99},	// 0x19 is for rt2860E and after. This is for extension channel overlapping IOT. 0x99 is for rt2860D and before
+	{BBP_R86,		0x00},	// middle range issue, Rory @2008-01-28
+	{BBP_R91,		0x04},	// middle range issue, Rory @2008-01-28
+	{BBP_R92,		0x00},	// middle range issue, Rory @2008-01-28
+	{BBP_R103,  	0x00}, 	// near range high-power issue, requested from Gary @2008-0528
+	{BBP_R105,		0x05},	// 0x05 is for rt2860E to turn on FEQ control. It is safe for rt2860D and before, because Bit 7:2 are reserved in rt2860D and before.
+};
+#define	NUM_BBP_REG_PARMS	(sizeof(BBPRegTable) / sizeof(REG_PAIR))
+
+//
+// RF register initialization set
+//
+#ifdef RT2870
+REG_PAIR   RT30xx_RFRegTable[] = {
+        {RF_R04,          0x40},
+        {RF_R05,          0x03},
+        {RF_R06,          0x02},
+        {RF_R07,          0x70},
+        {RF_R09,          0x0F},
+        {RF_R10,          0x71},
+        {RF_R11,          0x21},
+        {RF_R12,          0x7B},
+        {RF_R14,          0x90},
+        {RF_R15,          0x58},
+        {RF_R16,          0xB3},
+        {RF_R17,          0x92},
+        {RF_R18,          0x2C},
+        {RF_R19,          0x02},
+        {RF_R20,          0xBA},
+        {RF_R21,          0xDB},
+        {RF_R24,          0x16},
+        {RF_R25,          0x01},
+        {RF_R27,          0x03},
+        {RF_R29,          0x1F},
+};
+#define	NUM_RF_REG_PARMS	(sizeof(RT30xx_RFRegTable) / sizeof(REG_PAIR))
+#endif // RT2870 //
+
+//
+// ASIC register initialization sets
+//
+
+RTMP_REG_PAIR	MACRegTable[] =	{
+#if defined(HW_BEACON_OFFSET) && (HW_BEACON_OFFSET == 0x200)
+	{BCN_OFFSET0,			0xf8f0e8e0}, /* 0x3800(e0), 0x3A00(e8), 0x3C00(f0), 0x3E00(f8), 512B for each beacon */
+	{BCN_OFFSET1,			0x6f77d0c8}, /* 0x3200(c8), 0x3400(d0), 0x1DC0(77), 0x1BC0(6f), 512B for each beacon */
+#elif defined(HW_BEACON_OFFSET) && (HW_BEACON_OFFSET == 0x100)
+	{BCN_OFFSET0,			0xece8e4e0}, /* 0x3800, 0x3A00, 0x3C00, 0x3E00, 512B for each beacon */
+	{BCN_OFFSET1,			0xfcf8f4f0}, /* 0x3800, 0x3A00, 0x3C00, 0x3E00, 512B for each beacon */
+#else
+    #error You must re-calculate new value for BCN_OFFSET0 & BCN_OFFSET1 in MACRegTable[]!!!
+#endif // HW_BEACON_OFFSET //
+
+	{LEGACY_BASIC_RATE,		0x0000013f}, //  Basic rate set bitmap
+	{HT_BASIC_RATE,		0x00008003}, // Basic HT rate set , 20M, MCS=3, MM. Format is the same as in TXWI.
+	{MAC_SYS_CTRL,		0x00}, // 0x1004, , default Disable RX
+	{RX_FILTR_CFG,		0x17f97}, //0x1400  , RX filter control,
+	{BKOFF_SLOT_CFG,	0x209}, // default set short slot time, CC_DELAY_TIME should be 2
+	{TX_SW_CFG0,		0x0}, 		// Gary,2008-05-21 for CWC test
+	{TX_SW_CFG1,		0x80606}, // Gary,2006-08-23
+	{TX_LINK_CFG,		0x1020},		// Gary,2006-08-23
+	//{TX_TIMEOUT_CFG,	0x00182090},	// CCK has some problem. So increase timieout value. 2006-10-09// MArvek RT
+	{TX_TIMEOUT_CFG,	0x000a2090},	// CCK has some problem. So increase timieout value. 2006-10-09// MArvek RT , Modify for 2860E ,2007-08-01
+	{MAX_LEN_CFG,		MAX_AGGREGATION_SIZE | 0x00001000},	// 0x3018, MAX frame length. Max PSDU = 16kbytes.
+	{LED_CFG,		0x7f031e46}, // Gary, 2006-08-23
+	{PBF_MAX_PCNT,			0x1F3FBF9F}, 	//0x1F3f7f9f},		//Jan, 2006/04/20
+	//{TX_RTY_CFG,			0x6bb80408},	// Jan, 2006/11/16
+	{TX_RTY_CFG,			0x47d01f0f},	// Jan, 2006/11/16, Set TxWI->ACK =0 in Probe Rsp Modify for 2860E ,2007-08-03
+	{AUTO_RSP_CFG,			0x00000013},	// Initial Auto_Responder, because QA will turn off Auto-Responder
+	{CCK_PROT_CFG,			0x05740003 /*0x01740003*/},	// Initial Auto_Responder, because QA will turn off Auto-Responder. And RTS threshold is enabled.
+	{OFDM_PROT_CFG,			0x05740003 /*0x01740003*/},	// Initial Auto_Responder, because QA will turn off Auto-Responder. And RTS threshold is enabled.
+#ifdef RT2870
+	{PBF_CFG, 				0xf40006}, 		// Only enable Queue 2
+	{MM40_PROT_CFG,			0x3F44084},		// Initial Auto_Responder, because QA will turn off Auto-Responder
+	{WPDMA_GLO_CFG,			0x00000030},
+#endif // RT2870 //
+	{GF20_PROT_CFG,			0x01744004},    // set 19:18 --> Short NAV for MIMO PS
+	{GF40_PROT_CFG,			0x03F44084},
+	{MM20_PROT_CFG,			0x01744004},
+	{TXOP_CTRL_CFG,			0x0000583f, /*0x0000243f*/ /*0x000024bf*/},	//Extension channel backoff.
+	{TX_RTS_CFG,			0x00092b20},
+//#ifdef WIFI_TEST
+	{EXP_ACK_TIME,			0x002400ca},	// default value
+//#else
+//	{EXP_ACK_TIME,			0x005400ca},	// suggested by Gray @ 20070323 for 11n intel-sta throughput
+//#endif // end - WIFI_TEST //
+	{TXOP_HLDR_ET, 			0x00000002},
+
+	/* Jerry comments 2008/01/16: we use SIFS = 10us in CCK defaultly, but it seems that 10us
+		is too small for INTEL 2200bg card, so in MBSS mode, the delta time between beacon0
+		and beacon1 is SIFS (10us), so if INTEL 2200bg card connects to BSS0, the ping
+		will always lost. So we change the SIFS of CCK from 10us to 16us. */
+	{XIFS_TIME_CFG,			0x33a41010},
+	{PWR_PIN_CFG,			0x00000003},	// patch for 2880-E
+};
+
+
+#ifdef CONFIG_STA_SUPPORT
+RTMP_REG_PAIR	STAMACRegTable[] =	{
+	{WMM_AIFSN_CFG,		0x00002273},
+	{WMM_CWMIN_CFG,	0x00002344},
+	{WMM_CWMAX_CFG,	0x000034aa},
+};
+#endif // CONFIG_STA_SUPPORT //
+
+#define	NUM_MAC_REG_PARMS		(sizeof(MACRegTable) / sizeof(RTMP_REG_PAIR))
+#ifdef CONFIG_STA_SUPPORT
+#define	NUM_STA_MAC_REG_PARMS	(sizeof(STAMACRegTable) / sizeof(RTMP_REG_PAIR))
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RT2870
+//
+// RT2870 Firmware Spec only used 1 oct for version expression
+//
+#define FIRMWARE_MINOR_VERSION	7
+
+#endif // RT2870 //
+
+// New 8k byte firmware size for RT3071/RT3072
+#define FIRMWAREIMAGE_MAX_LENGTH	0x2000
+#define FIRMWAREIMAGE_LENGTH		(sizeof (FirmwareImage) / sizeof(UCHAR))
+#define FIRMWARE_MAJOR_VERSION	0
+
+#define FIRMWAREIMAGEV1_LENGTH	0x1000
+#define FIRMWAREIMAGEV2_LENGTH	0x1000
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Allocate RTMP_ADAPTER data block and do some initialization
+
+	Arguments:
+		Adapter		Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	RTMPAllocAdapterBlock(
+	IN  PVOID	handle,
+	OUT	PRTMP_ADAPTER	*ppAdapter)
+{
+	PRTMP_ADAPTER	pAd;
+	NDIS_STATUS		Status;
+	INT 			index;
+	UCHAR			*pBeaconBuf = NULL;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> RTMPAllocAdapterBlock\n"));
+
+	*ppAdapter = NULL;
+
+	do
+	{
+		// Allocate RTMP_ADAPTER memory block
+		pBeaconBuf = kmalloc(MAX_BEACON_SIZE, MEM_ALLOC_FLAG);
+		if (pBeaconBuf == NULL)
+		{
+			Status = NDIS_STATUS_FAILURE;
+			DBGPRINT_ERR(("Failed to allocate memory - BeaconBuf!\n"));
+			break;
+		}
+
+		Status = AdapterBlockAllocateMemory(handle, (PVOID *)&pAd);
+		if (Status != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT_ERR(("Failed to allocate memory - ADAPTER\n"));
+			break;
+		}
+		pAd->BeaconBuf = pBeaconBuf;
+		printk("\n\n=== pAd = %p, size = %d ===\n\n", pAd, (UINT32)sizeof(RTMP_ADAPTER));
+
+
+		// Init spin locks
+		NdisAllocateSpinLock(&pAd->MgmtRingLock);
+
+		for (index =0 ; index < NUM_OF_TX_RING; index++)
+		{
+			NdisAllocateSpinLock(&pAd->TxSwQueueLock[index]);
+			NdisAllocateSpinLock(&pAd->DeQueueLock[index]);
+			pAd->DeQueueRunning[index] = FALSE;
+		}
+
+		NdisAllocateSpinLock(&pAd->irq_lock);
+
+	} while (FALSE);
+
+	if ((Status != NDIS_STATUS_SUCCESS) && (pBeaconBuf))
+		kfree(pBeaconBuf);
+
+	*ppAdapter = pAd;
+
+	DBGPRINT_S(Status, ("<-- RTMPAllocAdapterBlock, Status=%x\n", Status));
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read initial Tx power per MCS and BW from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPReadTxPwrPerRate(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	ULONG		data, Adata, Gdata;
+	USHORT		i, value, value2;
+	INT			Apwrdelta, Gpwrdelta;
+	UCHAR		t1,t2,t3,t4;
+	BOOLEAN		bValid, bApwrdeltaMinus = TRUE, bGpwrdeltaMinus = TRUE;
+
+	//
+	// Get power delta for 20MHz and 40MHz.
+	//
+	DBGPRINT(RT_DEBUG_TRACE, ("Txpower per Rate\n"));
+	RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_DELTA, value2);
+	Apwrdelta = 0;
+	Gpwrdelta = 0;
+
+	if ((value2 & 0xff) != 0xff)
+	{
+		if ((value2 & 0x80))
+			Gpwrdelta = (value2&0xf);
+
+		if ((value2 & 0x40))
+			bGpwrdeltaMinus = FALSE;
+		else
+			bGpwrdeltaMinus = TRUE;
+	}
+	if ((value2 & 0xff00) != 0xff00)
+	{
+		if ((value2 & 0x8000))
+			Apwrdelta = ((value2&0xf00)>>8);
+
+		if ((value2 & 0x4000))
+			bApwrdeltaMinus = FALSE;
+		else
+			bApwrdeltaMinus = TRUE;
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("Gpwrdelta = %x, Apwrdelta = %x .\n", Gpwrdelta, Apwrdelta));
+
+	//
+	// Get Txpower per MCS for 20MHz in 2.4G.
+	//
+	for (i=0; i<5; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_2_4G + i*4, value);
+		data = value;
+		if (bApwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Apwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Apwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Apwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Apwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Apwrdelta)
+				t1 = (value&0xf)-(Apwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Apwrdelta)
+				t2 = ((value&0xf0)>>4)-(Apwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Apwrdelta)
+				t3 = ((value&0xf00)>>8)-(Apwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Apwrdelta)
+				t4 = ((value&0xf000)>>12)-(Apwrdelta);
+			else
+				t4 = 0;
+		}
+		Adata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+		if (bGpwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Gpwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Gpwrdelta)
+				t1 = (value&0xf)-(Gpwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Gpwrdelta)
+				t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Gpwrdelta)
+				t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Gpwrdelta)
+				t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+			else
+				t4 = 0;
+		}
+		Gdata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_2_4G + i*4 + 2, value);
+		if (bApwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Apwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Apwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Apwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Apwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Apwrdelta)
+				t1 = (value&0xf)-(Apwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Apwrdelta)
+				t2 = ((value&0xf0)>>4)-(Apwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Apwrdelta)
+				t3 = ((value&0xf00)>>8)-(Apwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Apwrdelta)
+				t4 = ((value&0xf000)>>12)-(Apwrdelta);
+			else
+				t4 = 0;
+		}
+		Adata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+		if (bGpwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Gpwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Gpwrdelta)
+				t1 = (value&0xf)-(Gpwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Gpwrdelta)
+				t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Gpwrdelta)
+				t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Gpwrdelta)
+				t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+			else
+				t4 = 0;
+		}
+		Gdata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+		data |= (value<<16);
+
+		pAd->Tx20MPwrCfgABand[i] = pAd->Tx40MPwrCfgABand[i] = Adata;
+		pAd->Tx20MPwrCfgGBand[i] = pAd->Tx40MPwrCfgGBand[i] = Gdata;
+
+		if (data != 0xffffffff)
+			RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, data);
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("20MHz BW, 2.4G band-%lx,  Adata = %lx,  Gdata = %lx \n", data, Adata, Gdata));
+	}
+
+	//
+	// Check this block is valid for 40MHz in 2.4G. If invalid, use parameter for 20MHz in 2.4G
+	//
+	bValid = TRUE;
+	for (i=0; i<6; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_2_4G + 2 + i*2, value);
+		if (((value & 0x00FF) == 0x00FF) || ((value & 0xFF00) == 0xFF00))
+		{
+			bValid = FALSE;
+			break;
+		}
+	}
+
+	//
+	// Get Txpower per MCS for 40MHz in 2.4G.
+	//
+	if (bValid)
+	{
+		for (i=0; i<4; i++)
+		{
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_2_4G + i*4, value);
+			if (bGpwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Gpwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Gpwrdelta)
+					t1 = (value&0xf)-(Gpwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Gpwrdelta)
+					t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Gpwrdelta)
+					t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Gpwrdelta)
+					t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+				else
+					t4 = 0;
+			}
+			Gdata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_2_4G + i*4 + 2, value);
+			if (bGpwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Gpwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Gpwrdelta)
+					t1 = (value&0xf)-(Gpwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Gpwrdelta)
+					t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Gpwrdelta)
+					t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Gpwrdelta)
+					t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+				else
+					t4 = 0;
+			}
+			Gdata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+
+			if (i == 0)
+				pAd->Tx40MPwrCfgGBand[i+1] = (pAd->Tx40MPwrCfgGBand[i+1] & 0x0000FFFF) | (Gdata & 0xFFFF0000);
+			else
+				pAd->Tx40MPwrCfgGBand[i+1] = Gdata;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("40MHz BW, 2.4G band, Gdata = %lx \n", Gdata));
+		}
+	}
+
+	//
+	// Check this block is valid for 20MHz in 5G. If invalid, use parameter for 20MHz in 2.4G
+	//
+	bValid = TRUE;
+	for (i=0; i<8; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_5G + 2 + i*2, value);
+		if (((value & 0x00FF) == 0x00FF) || ((value & 0xFF00) == 0xFF00))
+		{
+			bValid = FALSE;
+			break;
+		}
+	}
+
+	//
+	// Get Txpower per MCS for 20MHz in 5G.
+	//
+	if (bValid)
+	{
+		for (i=0; i<5; i++)
+		{
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_5G + i*4, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_5G + i*4 + 2, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+
+			if (i == 0)
+				pAd->Tx20MPwrCfgABand[i] = (pAd->Tx20MPwrCfgABand[i] & 0x0000FFFF) | (Adata & 0xFFFF0000);
+			else
+				pAd->Tx20MPwrCfgABand[i] = Adata;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("20MHz BW, 5GHz band, Adata = %lx \n", Adata));
+		}
+	}
+
+	//
+	// Check this block is valid for 40MHz in 5G. If invalid, use parameter for 20MHz in 2.4G
+	//
+	bValid = TRUE;
+	for (i=0; i<6; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_5G + 2 + i*2, value);
+		if (((value & 0x00FF) == 0x00FF) || ((value & 0xFF00) == 0xFF00))
+		{
+			bValid = FALSE;
+			break;
+		}
+	}
+
+	//
+	// Get Txpower per MCS for 40MHz in 5G.
+	//
+	if (bValid)
+	{
+		for (i=0; i<4; i++)
+		{
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_5G + i*4, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_5G + i*4 + 2, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+
+			if (i == 0)
+				pAd->Tx40MPwrCfgABand[i+1] = (pAd->Tx40MPwrCfgABand[i+1] & 0x0000FFFF) | (Adata & 0xFFFF0000);
+			else
+				pAd->Tx40MPwrCfgABand[i+1] = Adata;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("40MHz BW, 5GHz band, Adata = %lx \n", Adata));
+		}
+	}
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read initial channel power parameters from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPReadChannelPwr(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR				i, choffset;
+	EEPROM_TX_PWR_STRUC	    Power;
+	EEPROM_TX_PWR_STRUC	    Power2;
+
+	// Read Tx power value for all channels
+	// Value from 1 - 0x7f. Default value is 24.
+	// Power value : 2.4G 0x00 (0) ~ 0x1F (31)
+	//             : 5.5G 0xF9 (-7) ~ 0x0F (15)
+
+	// 0. 11b/g, ch1 - ch 14
+	for (i = 0; i < 7; i++)
+	{
+//		Power.word = RTMP_EEPROM_READ16(pAd, EEPROM_G_TX_PWR_OFFSET + i * 2);
+//		Power2.word = RTMP_EEPROM_READ16(pAd, EEPROM_G_TX2_PWR_OFFSET + i * 2);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_G_TX_PWR_OFFSET + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_G_TX2_PWR_OFFSET + i * 2, Power2.word);
+		pAd->TxPower[i * 2].Channel = i * 2 + 1;
+		pAd->TxPower[i * 2 + 1].Channel = i * 2 + 2;
+
+		if ((Power.field.Byte0 > 31) || (Power.field.Byte0 < 0))
+			pAd->TxPower[i * 2].Power = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 > 31) || (Power.field.Byte1 < 0))
+			pAd->TxPower[i * 2 + 1].Power = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2 + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 > 31) || (Power2.field.Byte0 < 0))
+			pAd->TxPower[i * 2].Power2 = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 > 31) || (Power2.field.Byte1 < 0))
+			pAd->TxPower[i * 2 + 1].Power2 = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2 + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 1. U-NII lower/middle band: 36, 38, 40; 44, 46, 48; 52, 54, 56; 60, 62, 64 (including central frequency in BW 40MHz)
+	// 1.1 Fill up channel
+	choffset = 14;
+	for (i = 0; i < 4; i++)
+	{
+		pAd->TxPower[3 * i + choffset + 0].Channel	= 36 + i * 8 + 0;
+		pAd->TxPower[3 * i + choffset + 0].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 0].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 1].Channel	= 36 + i * 8 + 2;
+		pAd->TxPower[3 * i + choffset + 1].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 1].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 2].Channel	= 36 + i * 8 + 4;
+		pAd->TxPower[3 * i + choffset + 2].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 2].Power2	= DEFAULT_RF_TX_POWER;
+	}
+
+	// 1.2 Fill up power
+	for (i = 0; i < 6; i++)
+	{
+//		Power.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + i * 2);
+//		Power2.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + i * 2);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + i * 2, Power2.word);
+
+		if ((Power.field.Byte0 < 16) && (Power.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 16) && (Power.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 16) && (Power2.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 16) && (Power2.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 2. HipperLAN 2 100, 102 ,104; 108, 110, 112; 116, 118, 120; 124, 126, 128; 132, 134, 136; 140 (including central frequency in BW 40MHz)
+	// 2.1 Fill up channel
+	choffset = 14 + 12;
+	for (i = 0; i < 5; i++)
+	{
+		pAd->TxPower[3 * i + choffset + 0].Channel	= 100 + i * 8 + 0;
+		pAd->TxPower[3 * i + choffset + 0].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 0].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 1].Channel	= 100 + i * 8 + 2;
+		pAd->TxPower[3 * i + choffset + 1].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 1].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 2].Channel	= 100 + i * 8 + 4;
+		pAd->TxPower[3 * i + choffset + 2].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 2].Power2	= DEFAULT_RF_TX_POWER;
+	}
+	pAd->TxPower[3 * 5 + choffset + 0].Channel		= 140;
+	pAd->TxPower[3 * 5 + choffset + 0].Power		= DEFAULT_RF_TX_POWER;
+	pAd->TxPower[3 * 5 + choffset + 0].Power2		= DEFAULT_RF_TX_POWER;
+
+	// 2.2 Fill up power
+	for (i = 0; i < 8; i++)
+	{
+//		Power.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2);
+//		Power2.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2, Power2.word);
+
+		if ((Power.field.Byte0 < 16) && (Power.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 16) && (Power.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 16) && (Power2.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 16) && (Power2.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 3. U-NII upper band: 149, 151, 153; 157, 159, 161; 165 (including central frequency in BW 40MHz)
+	// 3.1 Fill up channel
+	choffset = 14 + 12 + 16;
+	for (i = 0; i < 2; i++)
+	{
+		pAd->TxPower[3 * i + choffset + 0].Channel	= 149 + i * 8 + 0;
+		pAd->TxPower[3 * i + choffset + 0].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 0].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 1].Channel	= 149 + i * 8 + 2;
+		pAd->TxPower[3 * i + choffset + 1].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 1].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 2].Channel	= 149 + i * 8 + 4;
+		pAd->TxPower[3 * i + choffset + 2].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 2].Power2	= DEFAULT_RF_TX_POWER;
+	}
+	pAd->TxPower[3 * 2 + choffset + 0].Channel		= 165;
+	pAd->TxPower[3 * 2 + choffset + 0].Power		= DEFAULT_RF_TX_POWER;
+	pAd->TxPower[3 * 2 + choffset + 0].Power2		= DEFAULT_RF_TX_POWER;
+
+	// 3.2 Fill up power
+	for (i = 0; i < 4; i++)
+	{
+//		Power.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2);
+//		Power2.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2, Power2.word);
+
+		if ((Power.field.Byte0 < 16) && (Power.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 16) && (Power.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 16) && (Power2.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 16) && (Power2.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 4. Print and Debug
+	choffset = 14 + 12 + 16 + 7;
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read the following from the registry
+		1. All the parameters
+		2. NetworkAddres
+
+	Arguments:
+		Adapter						Pointer to our adapter
+		WrapperConfigurationContext	For use by NdisOpenConfiguration
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_RESOURCES
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	NICReadRegParameters(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	NDIS_HANDLE			WrapperConfigurationContext
+	)
+{
+	NDIS_STATUS						Status = NDIS_STATUS_SUCCESS;
+	DBGPRINT_S(Status, ("<-- NICReadRegParameters, Status=%x\n", Status));
+	return Status;
+}
+
+
+#ifdef RT2870
+/*
+	========================================================================
+
+	Routine Description:
+		For RF filter calibration purpose
+
+	Arguments:
+		pAd                          Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID RTUSBFilterCalibration(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR	R55x = 0, value, FilterTarget = 0x1E, BBPValue;
+	UINT	loop = 0, count = 0, loopcnt = 0, ReTry = 0;
+	UCHAR	RF_R24_Value = 0;
+
+	// Give bbp filter initial value
+	pAd->Mlme.CaliBW20RfR24 = 0x16;
+	pAd->Mlme.CaliBW40RfR24 = 0x36;  //Bit[5] must be 1 for BW 40
+
+	do
+	{
+		if (loop == 1)	//BandWidth = 40 MHz
+		{
+			// Write 0x27 to RF_R24 to program filter
+			RF_R24_Value = 0x27;
+			RT30xxWriteRFRegister(pAd, RF_R24, RF_R24_Value);
+			FilterTarget = 0x19;
+
+			// when calibrate BW40, BBP mask must set to BW40.
+			RTUSBReadBBPRegister(pAd, BBP_R4, &BBPValue);
+			BBPValue&= (~0x18);
+			BBPValue|= (0x10);
+			RTUSBWriteBBPRegister(pAd, BBP_R4, BBPValue);
+		}
+		else			//BandWidth = 20 MHz
+		{
+			// Write 0x07 to RF_R24 to program filter
+			RF_R24_Value = 0x07;
+			RT30xxWriteRFRegister(pAd, RF_R24, RF_R24_Value);
+			FilterTarget = 0x16;
+		}
+
+		// Write 0x01 to RF_R22 to enable baseband loopback mode
+		RT30xxReadRFRegister(pAd, RF_R22, &value);
+		value |= 0x01;
+		RT30xxWriteRFRegister(pAd, RF_R22, value);
+
+		// Write 0x00 to BBP_R24 to set power & frequency of passband test tone
+		RTUSBWriteBBPRegister(pAd, BBP_R24, 0);
+
+		do
+		{
+			// Write 0x90 to BBP_R25 to transmit test tone
+			RTUSBWriteBBPRegister(pAd, BBP_R25, 0x90);
+
+			RTMPusecDelay(1000);
+			// Read BBP_R55[6:0] for received power, set R55x = BBP_R55[6:0]
+			RTUSBReadBBPRegister(pAd, BBP_R55, &value);
+			R55x = value & 0xFF;
+
+		} while ((ReTry++ < 100) && (R55x == 0));
+
+		// Write 0x06 to BBP_R24 to set power & frequency of stopband test tone
+		RTUSBWriteBBPRegister(pAd, BBP_R24, 0x06);
+
+		while(TRUE)
+		{
+			// Write 0x90 to BBP_R25 to transmit test tone
+			RTUSBWriteBBPRegister(pAd, BBP_R25, 0x90);
+
+			//We need to wait for calibration
+			RTMPusecDelay(1000);
+			RTUSBReadBBPRegister(pAd, BBP_R55, &value);
+			value &= 0xFF;
+			if ((R55x - value) < FilterTarget)
+			{
+				RF_R24_Value ++;
+			}
+			else if ((R55x - value) == FilterTarget)
+			{
+				RF_R24_Value ++;
+				count ++;
+			}
+			else
+			{
+				break;
+			}
+
+			// prevent infinite loop cause driver hang.
+			if (loopcnt++ > 100)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("RTUSBFilterCalibration - can't find a valid value, loopcnt=%d stop calibrating", loopcnt));
+				break;
+			}
+
+			// Write RF_R24 to program filter
+			RT30xxWriteRFRegister(pAd, RF_R24, RF_R24_Value);
+		}
+
+		if (count > 0)
+		{
+			RF_R24_Value = RF_R24_Value - ((count) ? (1) : (0));
+		}
+
+		// Store for future usage
+		if (loopcnt < 100)
+		{
+			if (loop++ == 0)
+			{
+				//BandWidth = 20 MHz
+				pAd->Mlme.CaliBW20RfR24 = (UCHAR)RF_R24_Value;
+			}
+			else
+			{
+				//BandWidth = 40 MHz
+				pAd->Mlme.CaliBW40RfR24 = (UCHAR)RF_R24_Value;
+				break;
+			}
+		}
+		else
+			break;
+
+		RT30xxWriteRFRegister(pAd, RF_R24, RF_R24_Value);
+
+		// reset count
+		count = 0;
+	} while(TRUE);
+
+	//
+	// Set back to initial state
+	//
+	RTUSBWriteBBPRegister(pAd, BBP_R24, 0);
+
+	RT30xxReadRFRegister(pAd, RF_R22, &value);
+	value &= ~(0x01);
+	RT30xxWriteRFRegister(pAd, RF_R22, value);
+
+	// set BBP back to BW20
+	RTUSBReadBBPRegister(pAd, BBP_R4, &BBPValue);
+	BBPValue&= (~0x18);
+	RTUSBWriteBBPRegister(pAd, BBP_R4, BBPValue);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTUSBFilterCalibration - CaliBW20RfR24=0x%x, CaliBW40RfR24=0x%x\n", pAd->Mlme.CaliBW20RfR24, pAd->Mlme.CaliBW40RfR24));
+}
+
+
+VOID NICInitRT30xxRFRegisters(IN PRTMP_ADAPTER pAd)
+{
+	INT i;
+	// Driver must read EEPROM to get RfIcType before initial RF registers
+	// Initialize RF register to default value
+        if (IS_RT3070(pAd) && ((pAd->RfIcType == RFIC_3020) ||(pAd->RfIcType == RFIC_2020)))
+        {
+                // Init RF calibration
+                // Driver should toggle RF R30 bit7 before init RF registers
+                ULONG RfReg = 0;
+                RT30xxReadRFRegister(pAd, RF_R30, (PUCHAR)&RfReg);
+                RfReg |= 0x80;
+                RT30xxWriteRFRegister(pAd, RF_R30, (UCHAR)RfReg);
+                RTMPusecDelay(1000);
+                RfReg &= 0x7F;
+                RT30xxWriteRFRegister(pAd, RF_R30, (UCHAR)RfReg);
+
+                // Initialize RF register to default value
+                for (i = 0; i < NUM_RF_REG_PARMS; i++)
+                {
+                        RT30xxWriteRFRegister(pAd, RT30xx_RFRegTable[i].Register, RT30xx_RFRegTable[i].Value);
+                }
+
+                //For RF filter Calibration
+                RTUSBFilterCalibration(pAd);
+        }
+
+}
+#endif // RT2870 //
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read initial parameters from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	NICReadEEPROMParameters(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			mac_addr)
+{
+	UINT32			data = 0;
+	USHORT			i, value, value2;
+	UCHAR			TmpPhy;
+	EEPROM_TX_PWR_STRUC	    Power;
+	EEPROM_VERSION_STRUC    Version;
+	EEPROM_ANTENNA_STRUC	Antenna;
+	EEPROM_NIC_CONFIG2_STRUC    NicConfig2;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICReadEEPROMParameters\n"));
+
+	// Init EEPROM Address Number, before access EEPROM; if 93c46, EEPROMAddressNum=6, else if 93c66, EEPROMAddressNum=8
+	RTMP_IO_READ32(pAd, E2PROM_CSR, &data);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> E2PROM_CSR = 0x%x\n", data));
+
+	if((data & 0x30) == 0)
+		pAd->EEPROMAddressNum = 6;		// 93C46
+	else if((data & 0x30) == 0x10)
+		pAd->EEPROMAddressNum = 8;     // 93C66
+	else
+		pAd->EEPROMAddressNum = 8;     // 93C86
+	DBGPRINT(RT_DEBUG_TRACE, ("--> EEPROMAddressNum = %d\n", pAd->EEPROMAddressNum ));
+
+	// RT2860 MAC no longer auto load MAC address from E2PROM. Driver has to intialize
+	// MAC address registers according to E2PROM setting
+	if (mac_addr == NULL ||
+		strlen(mac_addr) != 17 ||
+		mac_addr[2] != ':'  || mac_addr[5] != ':'  || mac_addr[8] != ':' ||
+		mac_addr[11] != ':' || mac_addr[14] != ':')
+	{
+		USHORT  Addr01,Addr23,Addr45 ;
+
+		RT28xx_EEPROM_READ16(pAd, 0x04, Addr01);
+		RT28xx_EEPROM_READ16(pAd, 0x06, Addr23);
+		RT28xx_EEPROM_READ16(pAd, 0x08, Addr45);
+
+		pAd->PermanentAddress[0] = (UCHAR)(Addr01 & 0xff);
+		pAd->PermanentAddress[1] = (UCHAR)(Addr01 >> 8);
+		pAd->PermanentAddress[2] = (UCHAR)(Addr23 & 0xff);
+		pAd->PermanentAddress[3] = (UCHAR)(Addr23 >> 8);
+		pAd->PermanentAddress[4] = (UCHAR)(Addr45 & 0xff);
+		pAd->PermanentAddress[5] = (UCHAR)(Addr45 >> 8);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Initialize MAC Address from E2PROM \n"));
+	}
+	else
+	{
+		INT		j;
+		PUCHAR	macptr;
+
+		macptr = mac_addr;
+
+		for (j=0; j<MAC_ADDR_LEN; j++)
+		{
+			AtoH(macptr, &pAd->PermanentAddress[j], 1);
+			macptr=macptr+3;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Initialize MAC Address from module parameter \n"));
+	}
+
+
+	{
+#if 0
+		USHORT  Addr01,Addr23,Addr45 ;
+
+		Addr01=RTMP_EEPROM_READ16(pAd, 0x04);
+		Addr23=RTMP_EEPROM_READ16(pAd, 0x06);
+		Addr45=RTMP_EEPROM_READ16(pAd, 0x08);
+
+		pAd->PermanentAddress[0] = (UCHAR)(Addr01 & 0xff);
+		pAd->PermanentAddress[1] = (UCHAR)(Addr01 >> 8);
+		pAd->PermanentAddress[2] = (UCHAR)(Addr23 & 0xff);
+		pAd->PermanentAddress[3] = (UCHAR)(Addr23 >> 8);
+		pAd->PermanentAddress[4] = (UCHAR)(Addr45 & 0xff);
+		pAd->PermanentAddress[5] = (UCHAR)(Addr45 >> 8);
+#endif
+		//more conveninet to test mbssid, so ap's bssid &0xf1
+		if (pAd->PermanentAddress[0] == 0xff)
+			pAd->PermanentAddress[0] = RandomByte(pAd)&0xf8;
+
+		//if (pAd->PermanentAddress[5] == 0xff)
+		//	pAd->PermanentAddress[5] = RandomByte(pAd)&0xf8;
+
+		DBGPRINT_RAW(RT_DEBUG_TRACE,("E2PROM MAC: =%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pAd->PermanentAddress[0], pAd->PermanentAddress[1],
+			pAd->PermanentAddress[2], pAd->PermanentAddress[3],
+			pAd->PermanentAddress[4], pAd->PermanentAddress[5]));
+		if (pAd->bLocalAdminMAC == FALSE)
+		{
+			MAC_DW0_STRUC csr2;
+			MAC_DW1_STRUC csr3;
+			COPY_MAC_ADDR(pAd->CurrentAddress, pAd->PermanentAddress);
+			csr2.field.Byte0 = pAd->CurrentAddress[0];
+			csr2.field.Byte1 = pAd->CurrentAddress[1];
+			csr2.field.Byte2 = pAd->CurrentAddress[2];
+			csr2.field.Byte3 = pAd->CurrentAddress[3];
+			RTMP_IO_WRITE32(pAd, MAC_ADDR_DW0, csr2.word);
+			csr3.word = 0;
+			csr3.field.Byte4 = pAd->CurrentAddress[4];
+			csr3.field.Byte5 = pAd->CurrentAddress[5];
+			csr3.field.U2MeMask = 0xff;
+			RTMP_IO_WRITE32(pAd, MAC_ADDR_DW1, csr3.word);
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("E2PROM MAC: =%02x:%02x:%02x:%02x:%02x:%02x\n",
+				pAd->PermanentAddress[0], pAd->PermanentAddress[1],
+				pAd->PermanentAddress[2], pAd->PermanentAddress[3],
+				pAd->PermanentAddress[4], pAd->PermanentAddress[5]));
+		}
+	}
+
+	// if not return early. cause fail at emulation.
+	// Init the channel number for TX channel power
+	RTMPReadChannelPwr(pAd);
+
+	// if E2PROM version mismatch with driver's expectation, then skip
+	// all subsequent E2RPOM retieval and set a system error bit to notify GUI
+	RT28xx_EEPROM_READ16(pAd, EEPROM_VERSION_OFFSET, Version.word);
+	pAd->EepromVersion = Version.field.Version + Version.field.FaeReleaseNumber * 256;
+	DBGPRINT(RT_DEBUG_TRACE, ("E2PROM: Version = %d, FAE release #%d\n", Version.field.Version, Version.field.FaeReleaseNumber));
+
+	if (Version.field.Version > VALID_EEPROM_VERSION)
+	{
+		DBGPRINT_ERR(("E2PROM: WRONG VERSION 0x%x, should be %d\n",Version.field.Version, VALID_EEPROM_VERSION));
+		/*pAd->SystemErrorBitmap |= 0x00000001;
+
+		// hard-code default value when no proper E2PROM installed
+		pAd->bAutoTxAgcA = FALSE;
+		pAd->bAutoTxAgcG = FALSE;
+
+		// Default the channel power
+		for (i = 0; i < MAX_NUM_OF_CHANNELS; i++)
+			pAd->TxPower[i].Power = DEFAULT_RF_TX_POWER;
+
+		// Default the channel power
+		for (i = 0; i < MAX_NUM_OF_11JCHANNELS; i++)
+			pAd->TxPower11J[i].Power = DEFAULT_RF_TX_POWER;
+
+		for(i = 0; i < NUM_EEPROM_BBP_PARMS; i++)
+			pAd->EEPROMDefaultValue[i] = 0xffff;
+		return;  */
+	}
+
+	// Read BBP default value from EEPROM and store to array(EEPROMDefaultValue) in pAd
+	RT28xx_EEPROM_READ16(pAd, EEPROM_NIC1_OFFSET, value);
+	pAd->EEPROMDefaultValue[0] = value;
+
+	RT28xx_EEPROM_READ16(pAd, EEPROM_NIC2_OFFSET, value);
+	pAd->EEPROMDefaultValue[1] = value;
+
+	RT28xx_EEPROM_READ16(pAd, 0x38, value);	// Country Region
+	pAd->EEPROMDefaultValue[2] = value;
+
+	for(i = 0; i < 8; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_BBP_BASE_OFFSET + i*2, value);
+		pAd->EEPROMDefaultValue[i+3] = value;
+	}
+
+	// We have to parse NIC configuration 0 at here.
+	// If TSSI did not have preloaded value, it should reset the TxAutoAgc to false
+	// Therefore, we have to read TxAutoAgc control beforehand.
+	// Read Tx AGC control bit
+	Antenna.word = pAd->EEPROMDefaultValue[0];
+	if (Antenna.word == 0xFFFF)
+	{
+		Antenna.word = 0;
+		Antenna.field.RfIcType = RFIC_2820;
+		Antenna.field.TxPath = 1;
+		Antenna.field.RxPath = 2;
+		DBGPRINT(RT_DEBUG_WARN, ("E2PROM error, hard code as 0x%04x\n", Antenna.word));
+	}
+
+	// Choose the desired Tx&Rx stream.
+	if ((pAd->CommonCfg.TxStream == 0) || (pAd->CommonCfg.TxStream > Antenna.field.TxPath))
+		pAd->CommonCfg.TxStream = Antenna.field.TxPath;
+
+	if ((pAd->CommonCfg.RxStream == 0) || (pAd->CommonCfg.RxStream > Antenna.field.RxPath))
+	{
+		pAd->CommonCfg.RxStream = Antenna.field.RxPath;
+
+		if ((pAd->MACVersion < RALINK_2883_VERSION) &&
+			(pAd->CommonCfg.RxStream > 2))
+		{
+			// only 2 Rx streams for RT2860 series
+			pAd->CommonCfg.RxStream = 2;
+		}
+	}
+
+	// 3*3
+	// read value from EEPROM and set them to CSR174 ~ 177 in chain0 ~ chain2
+	// yet implement
+	for(i=0; i<3; i++)
+	{
+	}
+
+	NicConfig2.word = pAd->EEPROMDefaultValue[1];
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		NicConfig2.word = 0;
+		if ((NicConfig2.word & 0x00ff) == 0xff)
+		{
+			NicConfig2.word &= 0xff00;
+		}
+
+		if ((NicConfig2.word >> 8) == 0xff)
+		{
+			NicConfig2.word &= 0x00ff;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	if (NicConfig2.field.DynamicTxAgcControl == 1)
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = TRUE;
+	else
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = FALSE;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("NICReadEEPROMParameters: RxPath = %d, TxPath = %d\n", Antenna.field.RxPath, Antenna.field.TxPath));
+
+	// Save the antenna for future use
+	pAd->Antenna.word = Antenna.word;
+
+	//
+	// Reset PhyMode if we don't support 802.11a
+	// Only RFIC_2850 & RFIC_2750 support 802.11a
+	//
+	if ((Antenna.field.RfIcType != RFIC_2850) && (Antenna.field.RfIcType != RFIC_2750))
+	{
+		if ((pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED) ||
+			(pAd->CommonCfg.PhyMode == PHY_11A))
+			pAd->CommonCfg.PhyMode = PHY_11BG_MIXED;
+#ifdef DOT11_N_SUPPORT
+		else if ((pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED)	||
+				 (pAd->CommonCfg.PhyMode == PHY_11AN_MIXED) 	||
+				 (pAd->CommonCfg.PhyMode == PHY_11AGN_MIXED) 	||
+				 (pAd->CommonCfg.PhyMode == PHY_11N_5G))
+			pAd->CommonCfg.PhyMode = PHY_11BGN_MIXED;
+#endif // DOT11_N_SUPPORT //
+	}
+
+	// Read TSSI reference and TSSI boundary for temperature compensation. This is ugly
+	// 0. 11b/g
+	{
+		/* these are tempature reference value (0x00 ~ 0xFE)
+		   ex: 0x00 0x15 0x25 0x45 0x88 0xA0 0xB5 0xD0 0xF0
+		   TssiPlusBoundaryG [4] [3] [2] [1] [0] (smaller) +
+		   TssiMinusBoundaryG[0] [1] [2] [3] [4] (larger) */
+		RT28xx_EEPROM_READ16(pAd, 0x6E, Power.word);
+		pAd->TssiMinusBoundaryG[4] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryG[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x70, Power.word);
+		pAd->TssiMinusBoundaryG[2] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryG[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x72, Power.word);
+		pAd->TssiRefG   = Power.field.Byte0; /* reference value [0] */
+		pAd->TssiPlusBoundaryG[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x74, Power.word);
+		pAd->TssiPlusBoundaryG[2] = Power.field.Byte0;
+		pAd->TssiPlusBoundaryG[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x76, Power.word);
+		pAd->TssiPlusBoundaryG[4] = Power.field.Byte0;
+		pAd->TxAgcStepG = Power.field.Byte1;
+		pAd->TxAgcCompensateG = 0;
+		pAd->TssiMinusBoundaryG[0] = pAd->TssiRefG;
+		pAd->TssiPlusBoundaryG[0]  = pAd->TssiRefG;
+
+		// Disable TxAgc if the based value is not right
+		if (pAd->TssiRefG == 0xff)
+			pAd->bAutoTxAgcG = FALSE;
+
+		DBGPRINT(RT_DEBUG_TRACE,("E2PROM: G Tssi[-4 .. +4] = %d %d %d %d - %d -%d %d %d %d, step=%d, tuning=%d\n",
+			pAd->TssiMinusBoundaryG[4], pAd->TssiMinusBoundaryG[3], pAd->TssiMinusBoundaryG[2], pAd->TssiMinusBoundaryG[1],
+			pAd->TssiRefG,
+			pAd->TssiPlusBoundaryG[1], pAd->TssiPlusBoundaryG[2], pAd->TssiPlusBoundaryG[3], pAd->TssiPlusBoundaryG[4],
+			pAd->TxAgcStepG, pAd->bAutoTxAgcG));
+	}
+	// 1. 11a
+	{
+		RT28xx_EEPROM_READ16(pAd, 0xD4, Power.word);
+		pAd->TssiMinusBoundaryA[4] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryA[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xD6, Power.word);
+		pAd->TssiMinusBoundaryA[2] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryA[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xD8, Power.word);
+		pAd->TssiRefA   = Power.field.Byte0;
+		pAd->TssiPlusBoundaryA[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xDA, Power.word);
+		pAd->TssiPlusBoundaryA[2] = Power.field.Byte0;
+		pAd->TssiPlusBoundaryA[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xDC, Power.word);
+		pAd->TssiPlusBoundaryA[4] = Power.field.Byte0;
+		pAd->TxAgcStepA = Power.field.Byte1;
+		pAd->TxAgcCompensateA = 0;
+		pAd->TssiMinusBoundaryA[0] = pAd->TssiRefA;
+		pAd->TssiPlusBoundaryA[0]  = pAd->TssiRefA;
+
+		// Disable TxAgc if the based value is not right
+		if (pAd->TssiRefA == 0xff)
+			pAd->bAutoTxAgcA = FALSE;
+
+		DBGPRINT(RT_DEBUG_TRACE,("E2PROM: A Tssi[-4 .. +4] = %d %d %d %d - %d -%d %d %d %d, step=%d, tuning=%d\n",
+			pAd->TssiMinusBoundaryA[4], pAd->TssiMinusBoundaryA[3], pAd->TssiMinusBoundaryA[2], pAd->TssiMinusBoundaryA[1],
+			pAd->TssiRefA,
+			pAd->TssiPlusBoundaryA[1], pAd->TssiPlusBoundaryA[2], pAd->TssiPlusBoundaryA[3], pAd->TssiPlusBoundaryA[4],
+			pAd->TxAgcStepA, pAd->bAutoTxAgcA));
+	}
+	pAd->BbpRssiToDbmDelta = 0x0;
+
+	// Read frequency offset setting for RF
+	RT28xx_EEPROM_READ16(pAd, EEPROM_FREQ_OFFSET, value);
+	if ((value & 0x00FF) != 0x00FF)
+		pAd->RfFreqOffset = (ULONG) (value & 0x00FF);
+	else
+		pAd->RfFreqOffset = 0;
+	DBGPRINT(RT_DEBUG_TRACE, ("E2PROM: RF FreqOffset=0x%lx \n", pAd->RfFreqOffset));
+
+	//CountryRegion byte offset (38h)
+	value = pAd->EEPROMDefaultValue[2] >> 8;		// 2.4G band
+	value2 = pAd->EEPROMDefaultValue[2] & 0x00FF;	// 5G band
+
+	if ((value <= REGION_MAXIMUM_BG_BAND) && (value2 <= REGION_MAXIMUM_A_BAND))
+	{
+		pAd->CommonCfg.CountryRegion = ((UCHAR) value) | 0x80;
+		pAd->CommonCfg.CountryRegionForABand = ((UCHAR) value2) | 0x80;
+		TmpPhy = pAd->CommonCfg.PhyMode;
+		pAd->CommonCfg.PhyMode = 0xff;
+		RTMPSetPhyMode(pAd, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+	}
+
+	//
+	// Get RSSI Offset on EEPROM 0x9Ah & 0x9Ch.
+	// The valid value are (-10 ~ 10)
+	//
+	RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_BG_OFFSET, value);
+	pAd->BGRssiOffset0 = value & 0x00ff;
+	pAd->BGRssiOffset1 = (value >> 8);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_BG_OFFSET+2, value);
+	pAd->BGRssiOffset2 = value & 0x00ff;
+	pAd->ALNAGain1 = (value >> 8);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LNA_OFFSET, value);
+	pAd->BLNAGain = value & 0x00ff;
+	pAd->ALNAGain0 = (value >> 8);
+
+	// Validate 11b/g RSSI_0 offset.
+	if ((pAd->BGRssiOffset0 < -10) || (pAd->BGRssiOffset0 > 10))
+		pAd->BGRssiOffset0 = 0;
+
+	// Validate 11b/g RSSI_1 offset.
+	if ((pAd->BGRssiOffset1 < -10) || (pAd->BGRssiOffset1 > 10))
+		pAd->BGRssiOffset1 = 0;
+
+	// Validate 11b/g RSSI_2 offset.
+	if ((pAd->BGRssiOffset2 < -10) || (pAd->BGRssiOffset2 > 10))
+		pAd->BGRssiOffset2 = 0;
+
+	RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_A_OFFSET, value);
+	pAd->ARssiOffset0 = value & 0x00ff;
+	pAd->ARssiOffset1 = (value >> 8);
+	RT28xx_EEPROM_READ16(pAd, (EEPROM_RSSI_A_OFFSET+2), value);
+	pAd->ARssiOffset2 = value & 0x00ff;
+	pAd->ALNAGain2 = (value >> 8);
+
+	if (((UCHAR)pAd->ALNAGain1 == 0xFF) || (pAd->ALNAGain1 == 0x00))
+		pAd->ALNAGain1 = pAd->ALNAGain0;
+	if (((UCHAR)pAd->ALNAGain2 == 0xFF) || (pAd->ALNAGain2 == 0x00))
+		pAd->ALNAGain2 = pAd->ALNAGain0;
+
+	// Validate 11a RSSI_0 offset.
+	if ((pAd->ARssiOffset0 < -10) || (pAd->ARssiOffset0 > 10))
+		pAd->ARssiOffset0 = 0;
+
+	// Validate 11a RSSI_1 offset.
+	if ((pAd->ARssiOffset1 < -10) || (pAd->ARssiOffset1 > 10))
+		pAd->ARssiOffset1 = 0;
+
+	//Validate 11a RSSI_2 offset.
+	if ((pAd->ARssiOffset2 < -10) || (pAd->ARssiOffset2 > 10))
+		pAd->ARssiOffset2 = 0;
+
+	//
+	// Get LED Setting.
+	//
+	RT28xx_EEPROM_READ16(pAd, 0x3a, value);
+	pAd->LedCntl.word = (value&0xff00) >> 8;
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LED1_OFFSET, value);
+	pAd->Led1 = value;
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LED2_OFFSET, value);
+	pAd->Led2 = value;
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LED3_OFFSET, value);
+	pAd->Led3 = value;
+
+	RTMPReadTxPwrPerRate(pAd);
+
+#ifdef SINGLE_SKU
+	//pAd->CommonCfg.DefineMaxTxPwr = RTMP_EEPROM_READ16(pAd, EEPROM_DEFINE_MAX_TXPWR);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_DEFINE_MAX_TXPWR, pAd->CommonCfg.DefineMaxTxPwr);
+#endif // SINGLE_SKU //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICReadEEPROMParameters\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set default value from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	NICInitAsicFromEEPROM(
+	IN	PRTMP_ADAPTER	pAd)
+{
+#ifdef CONFIG_STA_SUPPORT
+	UINT32					data = 0;
+	UCHAR	BBPR1 = 0;
+#endif // CONFIG_STA_SUPPORT //
+	USHORT					i;
+	EEPROM_ANTENNA_STRUC	Antenna;
+	EEPROM_NIC_CONFIG2_STRUC    NicConfig2;
+	UCHAR	BBPR3 = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitAsicFromEEPROM\n"));
+	for(i = 3; i < NUM_EEPROM_BBP_PARMS; i++)
+	{
+		UCHAR BbpRegIdx, BbpValue;
+
+		if ((pAd->EEPROMDefaultValue[i] != 0xFFFF) && (pAd->EEPROMDefaultValue[i] != 0))
+		{
+			BbpRegIdx = (UCHAR)(pAd->EEPROMDefaultValue[i] >> 8);
+			BbpValue  = (UCHAR)(pAd->EEPROMDefaultValue[i] & 0xff);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BbpRegIdx, BbpValue);
+		}
+	}
+
+	Antenna.word = pAd->Antenna.word;
+	pAd->Mlme.RealRxPath = (UCHAR) Antenna.field.RxPath;
+	pAd->RfIcType = (UCHAR) Antenna.field.RfIcType;
+
+	NicConfig2.word = pAd->EEPROMDefaultValue[1];
+
+
+	// Save the antenna for future use
+	pAd->NicConfig2.word = NicConfig2.word;
+
+	//
+	// Send LED Setting to MCU.
+	//
+	if (pAd->LedCntl.word == 0xFF)
+	{
+		pAd->LedCntl.word = 0x01;
+		pAd->Led1 = 0x5555;
+		pAd->Led2 = 0x2221;
+
+#ifdef RT2870
+		pAd->Led3 = 0x5627;
+#endif // RT2870 //
+	}
+
+	AsicSendCommandToMcu(pAd, 0x52, 0xff, (UCHAR)pAd->Led1, (UCHAR)(pAd->Led1 >> 8));
+	AsicSendCommandToMcu(pAd, 0x53, 0xff, (UCHAR)pAd->Led2, (UCHAR)(pAd->Led2 >> 8));
+	AsicSendCommandToMcu(pAd, 0x54, 0xff, (UCHAR)pAd->Led3, (UCHAR)(pAd->Led3 >> 8));
+    pAd->LedIndicatorStregth = 0xFF;
+    RTMPSetSignalLED(pAd, -100);	// Force signal strength Led to be turned off, before link up
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Read Hardware controlled Radio state enable bit
+		if (NicConfig2.field.HardwareRadioControl == 1)
+		{
+			pAd->StaCfg.bHardwareRadio = TRUE;
+
+			// Read GPIO pin2 as Hardware controlled radio state
+			RTMP_IO_READ32(pAd, GPIO_CTRL_CFG, &data);
+			if ((data & 0x04) == 0)
+			{
+				pAd->StaCfg.bHwRadio = FALSE;
+				pAd->StaCfg.bRadio = FALSE;
+//				RTMP_IO_WRITE32(pAd, PWR_PIN_CFG, 0x00001818);
+				RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+			}
+		}
+		else
+			pAd->StaCfg.bHardwareRadio = FALSE;
+
+		if (pAd->StaCfg.bRadio == FALSE)
+		{
+			RTMPSetLED(pAd, LED_RADIO_OFF);
+		}
+		else
+		{
+			RTMPSetLED(pAd, LED_RADIO_ON);
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Turn off patching for cardbus controller
+	if (NicConfig2.field.CardbusAcceleration == 1)
+	{
+//		pAd->bTest1 = TRUE;
+	}
+
+	if (NicConfig2.field.DynamicTxAgcControl == 1)
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = TRUE;
+	else
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = FALSE;
+	//
+	// Since BBP has been progamed, to make sure BBP setting will be
+	// upate inside of AsicAntennaSelect, so reset to UNKNOWN_BAND!!
+	//
+	pAd->CommonCfg.BandState = UNKNOWN_BAND;
+
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
+	BBPR3 &= (~0x18);
+	if(pAd->Antenna.field.RxPath == 3)
+	{
+		BBPR3 |= (0x10);
+	}
+	else if(pAd->Antenna.field.RxPath == 2)
+	{
+		BBPR3 |= (0x8);
+	}
+	else if(pAd->Antenna.field.RxPath == 1)
+	{
+		BBPR3 |= (0x0);
+	}
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPR3);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Handle the difference when 1T
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BBPR1);
+		if(pAd->Antenna.field.TxPath == 1)
+		{
+		BBPR1 &= (~0x18);
+		}
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BBPR1);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Use Hw Radio Control Pin=%d; if used Pin=%d;\n", pAd->CommonCfg.bHardwareRadio, pAd->CommonCfg.bHardwareRadio));
+	}
+#endif // CONFIG_STA_SUPPORT //
+	DBGPRINT(RT_DEBUG_TRACE, ("TxPath = %d, RxPath = %d, RFIC=%d, Polar+LED mode=%x\n", pAd->Antenna.field.TxPath, pAd->Antenna.field.RxPath, pAd->RfIcType, pAd->LedCntl.word));
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitAsicFromEEPROM\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize NIC hardware
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	NICInitializeAdapter(
+	IN	PRTMP_ADAPTER	pAd,
+	IN   BOOLEAN    bHardReset)
+{
+	NDIS_STATUS     Status = NDIS_STATUS_SUCCESS;
+	WPDMA_GLO_CFG_STRUC	GloCfg;
+//	INT_MASK_CSR_STRUC		IntMask;
+	ULONG	i =0, j=0;
+	AC_TXOP_CSR0_STRUC	csr0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitializeAdapter\n"));
+
+	// 3. Set DMA global configuration except TX_DMA_EN and RX_DMA_EN bits:
+retry:
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0)  && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		RTMPusecDelay(1000);
+		i++;
+	}while ( i<100);
+	DBGPRINT(RT_DEBUG_TRACE, ("<== DMA offset 0x208 = 0x%x\n", GloCfg.word));
+	GloCfg.word &= 0xff0;
+	GloCfg.field.EnTXWriteBackDDONE =1;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+	// Record HW Beacon offset
+	pAd->BeaconOffset[0] = HW_BEACON_BASE0;
+	pAd->BeaconOffset[1] = HW_BEACON_BASE1;
+	pAd->BeaconOffset[2] = HW_BEACON_BASE2;
+	pAd->BeaconOffset[3] = HW_BEACON_BASE3;
+	pAd->BeaconOffset[4] = HW_BEACON_BASE4;
+	pAd->BeaconOffset[5] = HW_BEACON_BASE5;
+	pAd->BeaconOffset[6] = HW_BEACON_BASE6;
+	pAd->BeaconOffset[7] = HW_BEACON_BASE7;
+
+	//
+	// write all shared Ring's base address into ASIC
+	//
+
+	// asic simulation sequence put this ahead before loading firmware.
+	// pbf hardware reset
+
+	// Initialze ASIC for TX & Rx operation
+	if (NICInitializeAsic(pAd , bHardReset) != NDIS_STATUS_SUCCESS)
+	{
+		if (j++ == 0)
+		{
+			NICLoadFirmware(pAd);
+			goto retry;
+		}
+		return NDIS_STATUS_FAILURE;
+	}
+
+
+
+
+	// WMM parameter
+	csr0.word = 0;
+	RTMP_IO_WRITE32(pAd, WMM_TXOP0_CFG, csr0.word);
+	if (pAd->CommonCfg.PhyMode == PHY_11B)
+	{
+		csr0.field.Ac0Txop = 192;	// AC_VI: 192*32us ~= 6ms
+		csr0.field.Ac1Txop = 96;	// AC_VO: 96*32us  ~= 3ms
+	}
+	else
+	{
+		csr0.field.Ac0Txop = 96;	// AC_VI: 96*32us ~= 3ms
+		csr0.field.Ac1Txop = 48;	// AC_VO: 48*32us ~= 1.5ms
+	}
+	RTMP_IO_WRITE32(pAd, WMM_TXOP1_CFG, csr0.word);
+
+
+
+
+	// reset action
+	// Load firmware
+	//  Status = NICLoadFirmware(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitializeAdapter\n"));
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize ASIC
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	NICInitializeAsic(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  BOOLEAN		bHardReset)
+{
+	ULONG			Index = 0;
+	UCHAR			R0 = 0xff;
+	UINT32			MacCsr12 = 0, Counter = 0;
+#ifdef RT2870
+	UINT32			MacCsr0 = 0;
+	NTSTATUS		Status;
+	UCHAR			Value = 0xff;
+#endif // RT2870 //
+	USHORT			KeyIdx;
+	INT				i,apidx;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitializeAsic\n"));
+
+
+#ifdef RT2870
+	//
+	// Make sure MAC gets ready after NICLoadFirmware().
+	//
+	Index = 0;
+
+	//To avoid hang-on issue when interface up in kernel 2.4,
+	//we use a local variable "MacCsr0" instead of using "pAd->MACVersion" directly.
+	do
+	{
+		RTMP_IO_READ32(pAd, MAC_CSR0, &MacCsr0);
+
+		if ((MacCsr0 != 0x00) && (MacCsr0 != 0xFFFFFFFF))
+			break;
+
+		RTMPusecDelay(10);
+	} while (Index++ < 100);
+
+	pAd->MACVersion = MacCsr0;
+	DBGPRINT(RT_DEBUG_TRACE, ("MAC_CSR0  [ Ver:Rev=0x%08x]\n", pAd->MACVersion));
+	// turn on bit13 (set to zero) after rt2860D. This is to solve high-current issue.
+	RTMP_IO_READ32(pAd, PBF_SYS_CTRL, &MacCsr12);
+	MacCsr12 &= (~0x2000);
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, MacCsr12);
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x3);
+	RTMP_IO_WRITE32(pAd, USB_DMA_CFG, 0x0);
+	Status = RTUSBVenderReset(pAd);
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x0);
+
+	// Initialize MAC register to default value
+	for(Index=0; Index<NUM_MAC_REG_PARMS; Index++)
+	{
+		RTMP_IO_WRITE32(pAd, (USHORT)MACRegTable[Index].Register, MACRegTable[Index].Value);
+	}
+
+	if(IS_RT3070(pAd))
+	{
+		// According to Frank Hsu (from Gary Tsao)
+		RTMP_IO_WRITE32(pAd, (USHORT)TX_SW_CFG0, 0x00000400);
+
+		// Initialize RT3070 serial MAC registers which is different from RT2870 serial
+		RTUSBWriteMACRegister(pAd, TX_SW_CFG1, 0);
+		RTUSBWriteMACRegister(pAd, TX_SW_CFG2, 0);
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		for (Index = 0; Index < NUM_STA_MAC_REG_PARMS; Index++)
+		{
+			RTMP_IO_WRITE32(pAd, (USHORT)STAMACRegTable[Index].Register, STAMACRegTable[Index].Value);
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+#endif // RT2870 //
+
+	//
+	// Before program BBP, we need to wait BBP/RF get wake up.
+	//
+	Index = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, MAC_STATUS_CFG, &MacCsr12);
+
+		if ((MacCsr12 & 0x03) == 0)	// if BB.RF is stable
+			break;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Check MAC_STATUS_CFG  = Busy = %x\n", MacCsr12));
+		RTMPusecDelay(1000);
+	} while (Index++ < 100);
+
+    // The commands to firmware should be after these commands, these commands will init firmware
+	// PCI and USB are not the same because PCI driver needs to wait for PCI bus ready
+	RTMP_IO_WRITE32(pAd, H2M_BBP_AGENT, 0);	// initialize BBP R/W access agent
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CSR, 0);
+	RTMPusecDelay(1000);
+
+	// Read BBP register, make sure BBP is up and running before write new data
+	Index = 0;
+	do
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R0, &R0);
+		DBGPRINT(RT_DEBUG_TRACE, ("BBP version = %x\n", R0));
+	} while ((++Index < 20) && ((R0 == 0xff) || (R0 == 0x00)));
+	//ASSERT(Index < 20); //this will cause BSOD on Check-build driver
+
+	if ((R0 == 0xff) || (R0 == 0x00))
+		return NDIS_STATUS_FAILURE;
+
+	// Initialize BBP register to default value
+	for (Index = 0; Index < NUM_BBP_REG_PARMS; Index++)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBPRegTable[Index].Register, BBPRegTable[Index].Value);
+	}
+
+	// for rt2860E and after, init BBP_R84 with 0x19. This is for extension channel overlapping IOT.
+	if ((pAd->MACVersion&0xffff) != 0x0101)
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R84, 0x19);
+
+#ifdef RT2870
+	//write RT3070 BBP wchich different with 2870 after write RT2870 BBP
+	if (IS_RT3070(pAd))
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x0a);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R84, 0x99);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R105, 0x05);
+	}
+#endif // RT2870 //
+
+	if (pAd->MACVersion == 0x28600100)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x16);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x12);
+    }
+
+	if (pAd->MACVersion >= RALINK_2880E_VERSION && pAd->MACVersion < RALINK_3070_VERSION) // 3*3
+	{
+		// enlarge MAX_LEN_CFG
+		UINT32 csr;
+		RTMP_IO_READ32(pAd, MAX_LEN_CFG, &csr);
+		csr &= 0xFFF;
+		csr |= 0x2000;
+		RTMP_IO_WRITE32(pAd, MAX_LEN_CFG, csr);
+	}
+
+#ifdef RT2870
+{
+	UCHAR	MAC_Value[]={0xff,0xff,0xff,0xff,0xff,0xff,0xff,0,0};
+
+	//Initialize WCID table
+	Value = 0xff;
+	for(Index =0 ;Index < 254;Index++)
+	{
+		RTUSBMultiWrite(pAd, (USHORT)(MAC_WCID_BASE + Index * 8), MAC_Value, 8);
+	}
+}
+#endif // RT2870 //
+
+	// Add radio off control
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->StaCfg.bRadio == FALSE)
+		{
+//			RTMP_IO_WRITE32(pAd, PWR_PIN_CFG, 0x00001818);
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+			DBGPRINT(RT_DEBUG_TRACE, ("Set Radio Off\n"));
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Clear raw counters
+	RTMP_IO_READ32(pAd, RX_STA_CNT0, &Counter);
+	RTMP_IO_READ32(pAd, RX_STA_CNT1, &Counter);
+	RTMP_IO_READ32(pAd, RX_STA_CNT2, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT0, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT1, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT2, &Counter);
+
+	// ASIC will keep garbage value after boot
+	// Clear all seared key table when initial
+	// This routine can be ignored in radio-ON/OFF operation.
+	if (bHardReset)
+	{
+		for (KeyIdx = 0; KeyIdx < 4; KeyIdx++)
+		{
+			RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE + 4*KeyIdx, 0);
+		}
+
+		// Clear all pairwise key table when initial
+		for (KeyIdx = 0; KeyIdx < 256; KeyIdx++)
+		{
+			RTMP_IO_WRITE32(pAd, MAC_WCID_ATTRIBUTE_BASE + (KeyIdx * HW_WCID_ATTRI_SIZE), 1);
+		}
+	}
+
+	// assert HOST ready bit
+//  RTMP_IO_WRITE32(pAd, MAC_CSR1, 0x0); // 2004-09-14 asked by Mark
+//  RTMP_IO_WRITE32(pAd, MAC_CSR1, 0x4);
+
+	// It isn't necessary to clear this space when not hard reset.
+	if (bHardReset == TRUE)
+	{
+		// clear all on-chip BEACON frame space
+		for (apidx = 0; apidx < HW_BEACON_MAX_COUNT; apidx++)
+		{
+			for (i = 0; i < HW_BEACON_OFFSET>>2; i+=4)
+				RTMP_IO_WRITE32(pAd, pAd->BeaconOffset[apidx] + i, 0x00);
+		}
+	}
+#ifdef RT2870
+	AsicDisableSync(pAd);
+	// Clear raw counters
+	RTMP_IO_READ32(pAd, RX_STA_CNT0, &Counter);
+	RTMP_IO_READ32(pAd, RX_STA_CNT1, &Counter);
+	RTMP_IO_READ32(pAd, RX_STA_CNT2, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT0, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT1, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT2, &Counter);
+	// Default PCI clock cycle per ms is different as default setting, which is based on PCI.
+	RTMP_IO_READ32(pAd, USB_CYC_CFG, &Counter);
+	Counter&=0xffffff00;
+	Counter|=0x000001e;
+	RTMP_IO_WRITE32(pAd, USB_CYC_CFG, Counter);
+#endif // RT2870 //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// for rt2860E and after, init TXOP_CTRL_CFG with 0x583f. This is for extension channel overlapping IOT.
+		if ((pAd->MACVersion&0xffff) != 0x0101)
+			RTMP_IO_WRITE32(pAd, TXOP_CTRL_CFG, 0x583f);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitializeAsic\n"));
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Reset NIC Asics
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Reset NIC to initial state AS IS system boot up time.
+
+	========================================================================
+*/
+VOID	NICIssueReset(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UINT32	Value = 0;
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICIssueReset\n"));
+
+	// Abort Tx, prevent ASIC from writing to Host memory
+	//RTMP_IO_WRITE32(pAd, TX_CNTL_CSR, 0x001f0000);
+
+	// Disable Rx, register value supposed will remain after reset
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+	Value &= (0xfffffff3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+	// Issue reset and clear from reset state
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x03); // 2004-09-17 change from 0x01
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x00);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICIssueReset\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Check ASIC registers and find any reason the system might hang
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+BOOLEAN	NICCheckForHang(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	return (FALSE);
+}
+
+VOID NICUpdateFifoStaCounters(
+	IN PRTMP_ADAPTER pAd)
+{
+	TX_STA_FIFO_STRUC	StaFifo;
+	MAC_TABLE_ENTRY		*pEntry;
+	UCHAR				i = 0;
+	UCHAR			pid = 0, wcid = 0;
+	CHAR				reTry;
+	UCHAR				succMCS;
+
+#ifdef RALINK_ATE
+	/* Nothing to do in ATE mode */
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+		do
+		{
+			RTMP_IO_READ32(pAd, TX_STA_FIFO, &StaFifo.word);
+
+			if (StaFifo.field.bValid == 0)
+				break;
+
+			wcid = (UCHAR)StaFifo.field.wcid;
+
+
+		/* ignore NoACK and MGMT frame use 0xFF as WCID */
+			if ((StaFifo.field.TxAckRequired == 0) || (wcid >= MAX_LEN_OF_MAC_TABLE))
+			{
+				i++;
+				continue;
+			}
+
+			/* PID store Tx MCS Rate */
+			pid = (UCHAR)StaFifo.field.PidType;
+
+			pEntry = &pAd->MacTab.Content[wcid];
+
+			pEntry->DebugFIFOCount++;
+
+#ifdef DOT11_N_SUPPORT
+			if (StaFifo.field.TxBF) // 3*3
+				pEntry->TxBFCount++;
+#endif // DOT11_N_SUPPORT //
+
+#ifdef UAPSD_AP_SUPPORT
+			UAPSD_SP_AUE_Handle(pAd, pEntry, StaFifo.field.TxSuccess);
+#endif // UAPSD_AP_SUPPORT //
+
+			if (!StaFifo.field.TxSuccess)
+			{
+				pEntry->FIFOCount++;
+				pEntry->OneSecTxFailCount++;
+
+				if (pEntry->FIFOCount >= 1)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("#"));
+#if 0
+					SendRefreshBAR(pAd, pEntry);
+					pEntry->NoBADataCountDown = 64;
+#else
+#ifdef DOT11_N_SUPPORT
+					pEntry->NoBADataCountDown = 64;
+#endif // DOT11_N_SUPPORT //
+
+					if(pEntry->PsMode == PWR_ACTIVE)
+					{
+#ifdef DOT11_N_SUPPORT
+						int tid;
+						for (tid=0; tid<NUM_OF_TID; tid++)
+						{
+							BAOriSessionTearDown(pAd, pEntry->Aid,  tid, FALSE, FALSE);
+						}
+#endif // DOT11_N_SUPPORT //
+
+						// Update the continuous transmission counter except PS mode
+						pEntry->ContinueTxFailCnt++;
+					}
+					else
+					{
+						// Clear the FIFOCount when sta in Power Save mode. Basically we assume
+						//     this tx error happened due to sta just go to sleep.
+						pEntry->FIFOCount = 0;
+						pEntry->ContinueTxFailCnt = 0;
+					}
+#endif
+					//pEntry->FIFOCount = 0;
+				}
+				//pEntry->bSendBAR = TRUE;
+			}
+			else
+			{
+#ifdef DOT11_N_SUPPORT
+				if ((pEntry->PsMode != PWR_SAVE) && (pEntry->NoBADataCountDown > 0))
+				{
+					pEntry->NoBADataCountDown--;
+					if (pEntry->NoBADataCountDown==0)
+					{
+						DBGPRINT(RT_DEBUG_TRACE, ("@\n"));
+					}
+				}
+#endif // DOT11_N_SUPPORT //
+				pEntry->FIFOCount = 0;
+				pEntry->OneSecTxNoRetryOkCount++;
+				// update NoDataIdleCount when sucessful send packet to STA.
+				pEntry->NoDataIdleCount = 0;
+				pEntry->ContinueTxFailCnt = 0;
+			}
+
+			succMCS = StaFifo.field.SuccessRate & 0x7F;
+
+			reTry = pid - succMCS;
+
+			if (StaFifo.field.TxSuccess)
+			{
+				pEntry->TXMCSExpected[pid]++;
+				if (pid == succMCS)
+				{
+					pEntry->TXMCSSuccessful[pid]++;
+				}
+				else
+				{
+					pEntry->TXMCSAutoFallBack[pid][succMCS]++;
+				}
+			}
+			else
+			{
+				pEntry->TXMCSFailed[pid]++;
+			}
+
+			if (reTry > 0)
+			{
+				if ((pid >= 12) && succMCS <=7)
+				{
+					reTry -= 4;
+				}
+				pEntry->OneSecTxRetryOkCount += reTry;
+			}
+
+			i++;
+			// ASIC store 16 stack
+		} while ( i < (2*TX_RING_SIZE) );
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read statistical counters from hardware registers and record them
+		in software variables for later on query
+
+	Arguments:
+		pAd					Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID NICUpdateRawCounters(
+	IN PRTMP_ADAPTER pAd)
+{
+	UINT32	OldValue;
+	RX_STA_CNT0_STRUC	 RxStaCnt0;
+	RX_STA_CNT1_STRUC   RxStaCnt1;
+	RX_STA_CNT2_STRUC   RxStaCnt2;
+	TX_STA_CNT0_STRUC 	 TxStaCnt0;
+	TX_STA_CNT1_STRUC	 StaTx1;
+	TX_STA_CNT2_STRUC	 StaTx2;
+	TX_AGG_CNT_STRUC	TxAggCnt;
+	TX_AGG_CNT0_STRUC	TxAggCnt0;
+	TX_AGG_CNT1_STRUC	TxAggCnt1;
+	TX_AGG_CNT2_STRUC	TxAggCnt2;
+	TX_AGG_CNT3_STRUC	TxAggCnt3;
+	TX_AGG_CNT4_STRUC	TxAggCnt4;
+	TX_AGG_CNT5_STRUC	TxAggCnt5;
+	TX_AGG_CNT6_STRUC	TxAggCnt6;
+	TX_AGG_CNT7_STRUC	TxAggCnt7;
+
+
+	RTMP_IO_READ32(pAd, RX_STA_CNT0, &RxStaCnt0.word);
+	RTMP_IO_READ32(pAd, RX_STA_CNT2, &RxStaCnt2.word);
+
+	{
+		RTMP_IO_READ32(pAd, RX_STA_CNT1, &RxStaCnt1.word);
+	    // Update RX PLCP error counter
+	    pAd->PrivateInfo.PhyRxErrCnt += RxStaCnt1.field.PlcpErr;
+		// Update False CCA counter
+		pAd->RalinkCounters.OneSecFalseCCACnt += RxStaCnt1.field.FalseCca;
+	}
+
+	// Update FCS counters
+	OldValue= pAd->WlanCounters.FCSErrorCount.u.LowPart;
+	pAd->WlanCounters.FCSErrorCount.u.LowPart += (RxStaCnt0.field.CrcErr); // >> 7);
+	if (pAd->WlanCounters.FCSErrorCount.u.LowPart < OldValue)
+		pAd->WlanCounters.FCSErrorCount.u.HighPart++;
+
+	// Add FCS error count to private counters
+	pAd->RalinkCounters.OneSecRxFcsErrCnt += RxStaCnt0.field.CrcErr;
+	OldValue = pAd->RalinkCounters.RealFcsErrCount.u.LowPart;
+	pAd->RalinkCounters.RealFcsErrCount.u.LowPart += RxStaCnt0.field.CrcErr;
+	if (pAd->RalinkCounters.RealFcsErrCount.u.LowPart < OldValue)
+		pAd->RalinkCounters.RealFcsErrCount.u.HighPart++;
+
+	// Update Duplicate Rcv check
+	pAd->RalinkCounters.DuplicateRcv += RxStaCnt2.field.RxDupliCount;
+	pAd->WlanCounters.FrameDuplicateCount.u.LowPart += RxStaCnt2.field.RxDupliCount;
+	// Update RX Overflow counter
+	pAd->Counters8023.RxNoBuffer += (RxStaCnt2.field.RxFifoOverflowCount);
+
+	//pAd->RalinkCounters.RxCount = 0;
+#ifdef RT2870
+	if (pAd->RalinkCounters.RxCount != pAd->watchDogRxCnt)
+	{
+		pAd->watchDogRxCnt = pAd->RalinkCounters.RxCount;
+		pAd->watchDogRxOverFlowCnt = 0;
+	}
+	else
+	{
+		if (RxStaCnt2.field.RxFifoOverflowCount)
+			pAd->watchDogRxOverFlowCnt++;
+		else
+			pAd->watchDogRxOverFlowCnt = 0;
+	}
+#endif // RT2870 //
+
+
+	//if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED) ||
+	//	(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED) && (pAd->MacTab.Size != 1)))
+	if (!pAd->bUpdateBcnCntDone)
+	{
+	// Update BEACON sent count
+	RTMP_IO_READ32(pAd, TX_STA_CNT0, &TxStaCnt0.word);
+	RTMP_IO_READ32(pAd, TX_STA_CNT1, &StaTx1.word);
+	RTMP_IO_READ32(pAd, TX_STA_CNT2, &StaTx2.word);
+	pAd->RalinkCounters.OneSecBeaconSentCnt += TxStaCnt0.field.TxBeaconCount;
+	pAd->RalinkCounters.OneSecTxRetryOkCount += StaTx1.field.TxRetransmit;
+	pAd->RalinkCounters.OneSecTxNoRetryOkCount += StaTx1.field.TxSuccess;
+	pAd->RalinkCounters.OneSecTxFailCount += TxStaCnt0.field.TxFailCount;
+	pAd->WlanCounters.TransmittedFragmentCount.u.LowPart += StaTx1.field.TxSuccess;
+	pAd->WlanCounters.RetryCount.u.LowPart += StaTx1.field.TxRetransmit;
+	pAd->WlanCounters.FailedCount.u.LowPart += TxStaCnt0.field.TxFailCount;
+	}
+
+#if 0
+	Retry = StaTx1.field.TxRetransmit;
+	Fail = TxStaCnt0.field.TxFailCount;
+	TxErrorRatio = 0;
+	OneSecTransmitCount = pAd->WlanCounters.TransmittedFragmentCount.u.LowPart- pAd->WlanCounters.LastTransmittedFragmentCount.u.LowPart;
+	if ((OneSecTransmitCount+Retry + Fail) > 0)
+		TxErrorRatio = (( Retry + Fail) *100) / (OneSecTransmitCount+Retry + Fail);
+
+	if ((OneSecTransmitCount+Retry + Fail) > 0)
+		TxErrorRatio = (( Retry + Fail) *100) / (OneSecTransmitCount+Retry + Fail);
+	DBGPRINT(RT_DEBUG_INFO, ("TX ERROR Rate = %ld %%, Retry = %ld, Fail = %ld, Total = %ld  \n",TxErrorRatio, Retry, Fail, (OneSecTransmitCount+Retry + Fail)));
+	pAd->WlanCounters.LastTransmittedFragmentCount.u.LowPart = pAd->WlanCounters.TransmittedFragmentCount.u.LowPart;
+#endif
+
+	//if (pAd->bStaFifoTest == TRUE)
+	{
+		RTMP_IO_READ32(pAd, TX_AGG_CNT, &TxAggCnt.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT0, &TxAggCnt0.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT1, &TxAggCnt1.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT2, &TxAggCnt2.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT3, &TxAggCnt3.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT4, &TxAggCnt4.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT5, &TxAggCnt5.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT6, &TxAggCnt6.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT7, &TxAggCnt7.word);
+		pAd->RalinkCounters.TxAggCount += TxAggCnt.field.AggTxCount;
+		pAd->RalinkCounters.TxNonAggCount += TxAggCnt.field.NonAggTxCount;
+		pAd->RalinkCounters.TxAgg1MPDUCount += TxAggCnt0.field.AggSize1Count;
+		pAd->RalinkCounters.TxAgg2MPDUCount += TxAggCnt0.field.AggSize2Count;
+
+		pAd->RalinkCounters.TxAgg3MPDUCount += TxAggCnt1.field.AggSize3Count;
+		pAd->RalinkCounters.TxAgg4MPDUCount += TxAggCnt1.field.AggSize4Count;
+		pAd->RalinkCounters.TxAgg5MPDUCount += TxAggCnt2.field.AggSize5Count;
+		pAd->RalinkCounters.TxAgg6MPDUCount += TxAggCnt2.field.AggSize6Count;
+
+		pAd->RalinkCounters.TxAgg7MPDUCount += TxAggCnt3.field.AggSize7Count;
+		pAd->RalinkCounters.TxAgg8MPDUCount += TxAggCnt3.field.AggSize8Count;
+		pAd->RalinkCounters.TxAgg9MPDUCount += TxAggCnt4.field.AggSize9Count;
+		pAd->RalinkCounters.TxAgg10MPDUCount += TxAggCnt4.field.AggSize10Count;
+
+		pAd->RalinkCounters.TxAgg11MPDUCount += TxAggCnt5.field.AggSize11Count;
+		pAd->RalinkCounters.TxAgg12MPDUCount += TxAggCnt5.field.AggSize12Count;
+		pAd->RalinkCounters.TxAgg13MPDUCount += TxAggCnt6.field.AggSize13Count;
+		pAd->RalinkCounters.TxAgg14MPDUCount += TxAggCnt6.field.AggSize14Count;
+
+		pAd->RalinkCounters.TxAgg15MPDUCount += TxAggCnt7.field.AggSize15Count;
+		pAd->RalinkCounters.TxAgg16MPDUCount += TxAggCnt7.field.AggSize16Count;
+
+		// Calculate the transmitted A-MPDU count
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += TxAggCnt0.field.AggSize1Count;
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt0.field.AggSize2Count / 2);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt1.field.AggSize3Count / 3);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt1.field.AggSize4Count / 4);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt2.field.AggSize5Count / 5);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt2.field.AggSize6Count / 6);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt3.field.AggSize7Count / 7);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt3.field.AggSize8Count / 8);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt4.field.AggSize9Count / 9);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt4.field.AggSize10Count / 10);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt5.field.AggSize11Count / 11);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt5.field.AggSize12Count / 12);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt6.field.AggSize13Count / 13);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt6.field.AggSize14Count / 14);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt7.field.AggSize15Count / 15);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt7.field.AggSize16Count / 16);
+	}
+
+#ifdef DBG_DIAGNOSE
+	{
+		RtmpDiagStruct	*pDiag;
+		COUNTER_RALINK	*pRalinkCounters;
+		UCHAR			ArrayCurIdx, i;
+
+		pDiag = &pAd->DiagStruct;
+		pRalinkCounters = &pAd->RalinkCounters;
+		ArrayCurIdx = pDiag->ArrayCurIdx;
+
+		if (pDiag->inited == 0)
+		{
+			NdisZeroMemory(pDiag, sizeof(struct _RtmpDiagStrcut_));
+			pDiag->ArrayStartIdx = pDiag->ArrayCurIdx = 0;
+			pDiag->inited = 1;
+		}
+		else
+		{
+			// Tx
+			pDiag->TxFailCnt[ArrayCurIdx] = TxStaCnt0.field.TxFailCount;
+			pDiag->TxAggCnt[ArrayCurIdx] = TxAggCnt.field.AggTxCount;
+			pDiag->TxNonAggCnt[ArrayCurIdx] = TxAggCnt.field.NonAggTxCount;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][0] = TxAggCnt0.field.AggSize1Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][1] = TxAggCnt0.field.AggSize2Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][2] = TxAggCnt1.field.AggSize3Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][3] = TxAggCnt1.field.AggSize4Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][4] = TxAggCnt2.field.AggSize5Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][5] = TxAggCnt2.field.AggSize6Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][6] = TxAggCnt3.field.AggSize7Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][7] = TxAggCnt3.field.AggSize8Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][8] = TxAggCnt4.field.AggSize9Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][9] = TxAggCnt4.field.AggSize10Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][10] = TxAggCnt5.field.AggSize11Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][11] = TxAggCnt5.field.AggSize12Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][12] = TxAggCnt6.field.AggSize13Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][13] = TxAggCnt6.field.AggSize14Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][14] = TxAggCnt7.field.AggSize15Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][15] = TxAggCnt7.field.AggSize16Count;
+
+			pDiag->RxCrcErrCnt[ArrayCurIdx] = RxStaCnt0.field.CrcErr;
+
+			INC_RING_INDEX(pDiag->ArrayCurIdx,  DIAGNOSE_TIME);
+			ArrayCurIdx = pDiag->ArrayCurIdx;
+			for (i =0; i < 9; i++)
+			{
+				pDiag->TxDescCnt[ArrayCurIdx][i]= 0;
+				pDiag->TxSWQueCnt[ArrayCurIdx][i] =0;
+				pDiag->TxMcsCnt[ArrayCurIdx][i] = 0;
+				pDiag->RxMcsCnt[ArrayCurIdx][i] = 0;
+			}
+			pDiag->TxDataCnt[ArrayCurIdx] = 0;
+			pDiag->TxFailCnt[ArrayCurIdx] = 0;
+			pDiag->RxDataCnt[ArrayCurIdx] = 0;
+			pDiag->RxCrcErrCnt[ArrayCurIdx]  = 0;
+//			for (i = 9; i < 16; i++)
+			for (i = 9; i < 24; i++) // 3*3
+			{
+				pDiag->TxDescCnt[ArrayCurIdx][i] = 0;
+				pDiag->TxMcsCnt[ArrayCurIdx][i] = 0;
+				pDiag->RxMcsCnt[ArrayCurIdx][i] = 0;
+}
+
+			if (pDiag->ArrayCurIdx == pDiag->ArrayStartIdx)
+				INC_RING_INDEX(pDiag->ArrayStartIdx,  DIAGNOSE_TIME);
+		}
+
+	}
+#endif // DBG_DIAGNOSE //
+
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Reset NIC from error
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Reset NIC from error state
+
+	========================================================================
+*/
+VOID	NICResetFromError(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	// Reset BBP (according to alex, reset ASIC will force reset BBP
+	// Therefore, skip the reset BBP
+	// RTMP_IO_WRITE32(pAd, MAC_CSR1, 0x2);
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x1);
+	// Remove ASIC from reset state
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x0);
+
+	NICInitializeAdapter(pAd, FALSE);
+	NICInitAsicFromEEPROM(pAd);
+
+	// Switch to current channel, since during reset process, the connection should remains on.
+	AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		erase 8051 firmware image in MAC ASIC
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID NICEraseFirmware(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG i;
+
+	for(i=0; i<MAX_FIRMWARE_IMAGE_SIZE; i+=4)
+		RTMP_IO_WRITE32(pAd, FIRMWARE_IMAGE_BASE + i, 0);
+
+}/* End of NICEraseFirmware */
+
+/*
+	========================================================================
+
+	Routine Description:
+		Load 8051 firmware RT2561.BIN file into MAC ASIC
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS         firmware image load ok
+		NDIS_STATUS_FAILURE         image not found
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+NDIS_STATUS NICLoadFirmware(
+	IN PRTMP_ADAPTER pAd)
+{
+#ifdef BIN_IN_FILE
+#define NICLF_DEFAULT_USE()	\
+	flg_default_firm_use = TRUE; \
+	printk("%s - Use default firmware!\n", __func__);
+
+	NDIS_STATUS		Status = NDIS_STATUS_SUCCESS;
+	PUCHAR			src;
+	struct file		*srcf;
+	INT 			retval, orgfsuid, orgfsgid, i;
+   	mm_segment_t	orgfs;
+	PUCHAR			pFirmwareImage;
+	UINT			FileLength = 0;
+	UINT32			MacReg;
+	ULONG			Index;
+	ULONG			firm;
+	BOOLEAN			flg_default_firm_use = FALSE;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> %s\n", __func__));
+
+	/* init */
+	pFirmwareImage = NULL;
+	src = RTMP_FIRMWARE_FILE_NAME;
+
+	/* save uid and gid used for filesystem access.
+	   set user and group to 0 (root) */
+	orgfsuid = current->fsuid;
+	orgfsgid = current->fsgid;
+	current->fsuid = current->fsgid = 0;
+    orgfs = get_fs();
+    set_fs(KERNEL_DS);
+
+	pAd->FirmwareVersion = (FIRMWARE_MAJOR_VERSION << 8) + \
+						   FIRMWARE_MINOR_VERSION;
+
+
+	/* allocate firmware buffer */
+    pFirmwareImage = kmalloc(MAX_FIRMWARE_IMAGE_SIZE, MEM_ALLOC_FLAG);
+    if (pFirmwareImage == NULL)
+	{
+		/* allocate fail, use default firmware array in firmware.h */
+		printk("%s - Allocate memory fail!\n", __func__);
+		NICLF_DEFAULT_USE();
+    }
+	else
+	{
+		/* allocate ok! zero the firmware buffer */
+		memset(pFirmwareImage, 0x00, MAX_FIRMWARE_IMAGE_SIZE);
+	} /* End of if */
+
+
+	/* if ok, read firmware file from *.bin file */
+	if (flg_default_firm_use == FALSE)
+	{
+		do
+		{
+			/* open the bin file */
+			srcf = filp_open(src, O_RDONLY, 0);
+
+			if (IS_ERR(srcf))
+			{
+				printk("%s - Error %ld opening %s\n",
+					   __func__, -PTR_ERR(srcf), src);
+				NICLF_DEFAULT_USE();
+				break;
+			} /* End of if */
+
+			/* the object must have a read method */
+			if ((srcf->f_op == NULL) || (srcf->f_op->read == NULL))
+			{
+				printk("%s - %s does not have a write method\n", __func__, src);
+				NICLF_DEFAULT_USE();
+				break;
+			} /* End of if */
+
+			/* read the firmware from the file *.bin */
+			FileLength = srcf->f_op->read(srcf,
+										  pFirmwareImage,
+										  MAX_FIRMWARE_IMAGE_SIZE,
+										  &srcf->f_pos);
+
+			if (FileLength != MAX_FIRMWARE_IMAGE_SIZE)
+			{
+				printk("%s: error file length (=%d) in RT2860AP.BIN\n",
+					   __func__, FileLength);
+				NICLF_DEFAULT_USE();
+				break;
+			}
+			else
+			{
+				PUCHAR ptr = pFirmwareImage;
+				USHORT crc = 0xffff;
+
+
+				/* calculate firmware CRC */
+				for(i=0; i<(MAX_FIRMWARE_IMAGE_SIZE-2); i++, ptr++)
+					crc = ByteCRC16(BitReverse(*ptr), crc);
+				/* End of for */
+
+				if ((pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-2] != \
+								(UCHAR)BitReverse((UCHAR)(crc>>8))) ||
+					(pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-1] != \
+								(UCHAR)BitReverse((UCHAR)crc)))
+				{
+					/* CRC fail */
+					printk("%s: CRC = 0x%02x 0x%02x "
+						   "error, should be 0x%02x 0x%02x\n",
+						   __func__,
+						   pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-2],
+						   pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-1],
+						   (UCHAR)(crc>>8), (UCHAR)(crc));
+					NICLF_DEFAULT_USE();
+					break;
+				}
+				else
+				{
+					/* firmware is ok */
+					pAd->FirmwareVersion = \
+						(pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-4] << 8) +
+						pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-3];
+
+					/* check if firmware version of the file is too old */
+					if ((pAd->FirmwareVersion) < \
+											((FIRMWARE_MAJOR_VERSION << 8) +
+									  	 	 FIRMWARE_MINOR_VERSION))
+					{
+						printk("%s: firmware version too old!\n", __func__);
+						NICLF_DEFAULT_USE();
+						break;
+					} /* End of if */
+				} /* End of if */
+
+				DBGPRINT(RT_DEBUG_TRACE,
+						 ("NICLoadFirmware: CRC ok, ver=%d.%d\n",
+						  pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-4],
+						  pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-3]));
+			} /* End of if (FileLength == MAX_FIRMWARE_IMAGE_SIZE) */
+			break;
+		} while(TRUE);
+
+		/* close firmware file */
+		if (IS_ERR(srcf))
+			;
+		else
+		{
+			retval = filp_close(srcf, NULL);
+			if (retval)
+			{
+				DBGPRINT(RT_DEBUG_ERROR,
+						 ("--> Error %d closing %s\n", -retval, src));
+			} /* End of if */
+		} /* End of if */
+	} /* End of if */
+
+
+	/* write firmware to ASIC */
+	if (flg_default_firm_use == TRUE)
+	{
+		/* use default fimeware, free allocated buffer */
+		if (pFirmwareImage != NULL)
+			kfree(pFirmwareImage);
+		/* End of if */
+
+		/* use default *.bin array */
+		pFirmwareImage = FirmwareImage;
+		FileLength = sizeof(FirmwareImage);
+	} /* End of if */
+
+	/* enable Host program ram write selection */
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x10000);
+
+	for(i=0; i<FileLength; i+=4)
+	{
+		firm = pFirmwareImage[i] +
+			   (pFirmwareImage[i+3] << 24) +
+			   (pFirmwareImage[i+2] << 16) +
+			   (pFirmwareImage[i+1] << 8);
+
+		RTMP_IO_WRITE32(pAd, FIRMWARE_IMAGE_BASE + i, firm);
+	} /* End of for */
+
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x00000);
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x00001);
+
+	/* initialize BBP R/W access agent */
+	RTMP_IO_WRITE32(pAd, H2M_BBP_AGENT, 0);
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CSR, 0);
+
+	if (flg_default_firm_use == FALSE)
+	{
+		/* use file firmware, free allocated buffer */
+		if (pFirmwareImage != NULL)
+			kfree(pFirmwareImage);
+		/* End of if */
+	} /* End of if */
+
+	set_fs(orgfs);
+	current->fsuid = orgfsuid;
+	current->fsgid = orgfsgid;
+#else
+
+	NDIS_STATUS		Status = NDIS_STATUS_SUCCESS;
+	PUCHAR			pFirmwareImage;
+	ULONG			FileLength, Index;
+	//ULONG			firm;
+	UINT32			MacReg = 0;
+#ifdef RT2870
+	UINT32			Version = (pAd->MACVersion >> 16);
+#endif // RT2870 //
+
+	pFirmwareImage = FirmwareImage;
+	FileLength = sizeof(FirmwareImage);
+#ifdef RT2870
+	// New 8k byte firmware size for RT3071/RT3072
+	//printk("Usb Chip\n");
+	if (FIRMWAREIMAGE_LENGTH == FIRMWAREIMAGE_MAX_LENGTH)
+	//The firmware image consists of two parts. One is the origianl and the other is the new.
+	//Use Second Part
+	{
+		if ((Version != 0x2860) && (Version != 0x2872) && (Version != 0x3070))
+		{	// Use Firmware V2.
+			//printk("KH:Use New Version,part2\n");
+			pFirmwareImage = (PUCHAR)&FirmwareImage[FIRMWAREIMAGEV1_LENGTH];
+			FileLength = FIRMWAREIMAGEV2_LENGTH;
+		}
+		else
+		{
+			//printk("KH:Use New Version,part1\n");
+			pFirmwareImage = FirmwareImage;
+			FileLength = FIRMWAREIMAGEV1_LENGTH;
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("KH: bin file should be 8KB.\n"));
+		Status = NDIS_STATUS_FAILURE;
+	}
+
+#endif // RT2870 //
+
+#if 0
+	/* enable Host program ram write selection */
+	RT28XX_FIRMUD_INIT(pAd);
+
+	for(i=0; i<FileLength; i+=4)
+	{
+		firm = pFirmwareImage[i] +
+			   (pFirmwareImage[i+3] << 24) +
+			   (pFirmwareImage[i+2] << 16) +
+			   (pFirmwareImage[i+1] << 8);
+
+		RTMP_IO_WRITE32(pAd, FIRMWARE_IMAGE_BASE + i, firm);
+	} /* End of for */
+
+	RT28XX_FIRMUD_END(pAd);
+#else
+	RT28XX_WRITE_FIRMWARE(pAd, pFirmwareImage, FileLength);
+#endif
+
+#endif
+
+	/* check if MCU is ready */
+	Index = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, PBF_SYS_CTRL, &MacReg);
+
+		if (MacReg & 0x80)
+			break;
+
+		RTMPusecDelay(1000);
+	} while (Index++ < 1000);
+
+    if (Index >= 1000)
+	{
+		Status = NDIS_STATUS_FAILURE;
+		DBGPRINT(RT_DEBUG_ERROR, ("NICLoadFirmware: MCU is not ready\n\n\n"));
+	} /* End of if */
+
+#if 0
+    DBGPRINT(RT_DEBUG_TRACE,
+			 ("<=== %s (src=%s, status=%d)\n", __func__, src, Status));
+#else
+    DBGPRINT(RT_DEBUG_TRACE,
+			 ("<=== %s (status=%d)\n", __func__, Status));
+#endif
+    return Status;
+} /* End of NICLoadFirmware */
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Load Tx rate switching parameters
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS         firmware image load ok
+		NDIS_STATUS_FAILURE         image not found
+
+	IRQL = PASSIVE_LEVEL
+
+	Rate Table Format:
+		1. (B0: Valid Item number) (B1:Initial item from zero)
+		2. Item Number(Dec)      Mode(Hex)     Current MCS(Dec)    TrainUp(Dec)    TrainDown(Dec)
+
+	========================================================================
+*/
+NDIS_STATUS NICLoadRateSwitchingParams(
+	IN PRTMP_ADAPTER pAd)
+{
+#if 0
+	NDIS_STATUS Status;
+
+	NDIS_HANDLE FileHandle;
+	UINT FileLength = 0, i, j;
+	PUCHAR pFirmwareImage;
+	NDIS_STRING FileName;
+	NDIS_PHYSICAL_ADDRESS HighestAcceptableMax = NDIS_PHYSICAL_ADDRESS_CONST(-1, -1);
+
+	DBGPRINT(RT_DEBUG_TRACE,("===> NICLoadRateSwitchingParams \n"));
+	pAd->CommonCfg.TxRateTableSize = 0;
+
+	if ((pAd->DeviceID == NIC2860_PCI_DEVICE_ID) || (pAd->DeviceID == NIC2860_PCIe_DEVICE_ID))
+	{
+		NdisInitializeString(&FileName,"rate.bin");
+		DBGPRINT(RT_DEBUG_TRACE, ("NICLoadRateSwitchingParams: load file - rate.bin for tx rate switch \n"));
+	}
+	else
+	{
+		DBGPRINT_ERR(("NICLoadRateSwitchingParams: wrong DeviceID = 0x%04x, can't find Tx rate switch parameters file\n", pAd->DeviceID));
+		return NDIS_STATUS_SUCCESS;
+	}
+	NdisOpenFile(&Status, &FileHandle, &FileLength, &FileName, HighestAcceptableMax);
+	NdisFreeString(FileName);
+
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("NICLoadRateSwitchingParams: NdisOpenFile() failed, used RateSwitchTable instead\n"));
+		return NDIS_STATUS_SUCCESS;
+	}
+
+	if ((FileLength == 0) || (FileLength > (MAX_STEP_OF_TX_RATE_SWITCH+1)*16))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("NICLoadRateSwitchingParams: file size is not reasonable, used RateSwitchTable instead\n"));
+
+		NdisCloseFile(FileHandle);
+		return NDIS_STATUS_SUCCESS;
+	}
+	else
+	{
+		//
+		// NDIS_STATUS_SUCCESS means
+		// The handle at FileHandle is valid for a subsequent call to NdisMapFile.
+		//
+		NdisMapFile(&Status, &pFirmwareImage, FileHandle);
+		DBGPRINT(RT_DEBUG_TRACE, ("NdisMapFile FileLength=%d\n", FileLength));
+	}
+
+	for (i=0, j=0; i<FileLength; i++)
+	{
+		if ((i%16) <= 4)	// trim reserved field
+		{
+			if (i%16 == 1)	// deal with DEC and HEX, only row0 is Hex, others are Dec
+			{
+				RateSwitchTable[j] = *(pFirmwareImage + i);
+			}
+			else
+			{
+				RateSwitchTable[j] = (*(pFirmwareImage + i)>>4) * 10 + (*(pFirmwareImage + i) & 0x0F);
+			}
+
+			j++;
+		}
+	}
+
+	pAd->CommonCfg.TxRateTableSize = RateSwitchTable[0];		// backup table size
+
+	if (Status == NDIS_STATUS_SUCCESS)
+	{
+		NdisUnmapFile(FileHandle);
+		NdisCloseFile(FileHandle);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("<=== NICLoadRateSwitchingParams(Valid TxRateTable item number=%d)\n", pAd->CommonCfg.TxRateTableSize));
+#endif
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		if  pSrc1 all zero with length Length, return 0.
+		If not all zero, return 1
+
+	Arguments:
+		pSrc1
+
+	Return Value:
+		1:			not all zero
+		0:			all zero
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+ULONG	RTMPNotAllZero(
+	IN	PVOID	pSrc1,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem1;
+	ULONG	Index = 0;
+
+	pMem1 = (PUCHAR) pSrc1;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		if (pMem1[Index] != 0x0)
+		{
+			break;
+		}
+	}
+
+	if (Index == Length)
+	{
+		return (0);
+	}
+	else
+	{
+		return (1);
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Compare two memory block
+
+	Arguments:
+		pSrc1		Pointer to first memory address
+		pSrc2		Pointer to second memory address
+
+	Return Value:
+		0:			memory is equal
+		1:			pSrc1 memory is larger
+		2:			pSrc2 memory is larger
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+ULONG	RTMPCompareMemory(
+	IN	PVOID	pSrc1,
+	IN	PVOID	pSrc2,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem1;
+	PUCHAR	pMem2;
+	ULONG	Index = 0;
+
+	pMem1 = (PUCHAR) pSrc1;
+	pMem2 = (PUCHAR) pSrc2;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		if (pMem1[Index] > pMem2[Index])
+			return (1);
+		else if (pMem1[Index] < pMem2[Index])
+			return (2);
+	}
+
+	// Equal
+	return (0);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Zero out memory block
+
+	Arguments:
+		pSrc1		Pointer to memory address
+		Length		Size
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPZeroMemory(
+	IN	PVOID	pSrc,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem;
+	ULONG	Index = 0;
+
+	pMem = (PUCHAR) pSrc;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		pMem[Index] = 0x00;
+	}
+}
+
+VOID	RTMPFillMemory(
+	IN	PVOID	pSrc,
+	IN	ULONG	Length,
+	IN	UCHAR	Fill)
+{
+	PUCHAR	pMem;
+	ULONG	Index = 0;
+
+	pMem = (PUCHAR) pSrc;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		pMem[Index] = Fill;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Copy data from memory block 1 to memory block 2
+
+	Arguments:
+		pDest		Pointer to destination memory address
+		pSrc		Pointer to source memory address
+		Length		Copy size
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPMoveMemory(
+	OUT	PVOID	pDest,
+	IN	PVOID	pSrc,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem1;
+	PUCHAR	pMem2;
+	UINT	Index;
+
+	ASSERT((Length==0) || (pDest && pSrc));
+
+	pMem1 = (PUCHAR) pDest;
+	pMem2 = (PUCHAR) pSrc;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		pMem1[Index] = pMem2[Index];
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize port configuration structure
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	UserCfgInit(
+	IN	PRTMP_ADAPTER pAd)
+{
+//	EDCA_PARM DefaultEdcaParm;
+    UINT key_index, bss_index;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> UserCfgInit\n"));
+
+	//
+	//  part I. intialize common configuration
+	//
+#ifdef RT2870
+	pAd->BulkOutReq = 0;
+
+	pAd->BulkOutComplete = 0;
+	pAd->BulkOutCompleteOther = 0;
+	pAd->BulkOutCompleteCancel = 0;
+	pAd->BulkInReq = 0;
+	pAd->BulkInComplete = 0;
+	pAd->BulkInCompleteFail = 0;
+
+	//pAd->QuickTimerP = 100;
+	//pAd->TurnAggrBulkInCount = 0;
+	pAd->bUsbTxBulkAggre = 0;
+
+	// init as unsed value to ensure driver will set to MCU once.
+	pAd->LedIndicatorStregth = 0xFF;
+
+	pAd->CommonCfg.MaxPktOneTxBulk = 2;
+	pAd->CommonCfg.TxBulkFactor = 1;
+	pAd->CommonCfg.RxBulkFactor =1;
+
+	pAd->CommonCfg.TxPower = 100; //mW
+
+	NdisZeroMemory(&pAd->CommonCfg.IOTestParm, sizeof(pAd->CommonCfg.IOTestParm));
+#endif // RT2870 //
+
+	for(key_index=0; key_index<SHARE_KEY_NUM; key_index++)
+	{
+		for(bss_index = 0; bss_index < MAX_MBSSID_NUM; bss_index++)
+		{
+			pAd->SharedKey[bss_index][key_index].KeyLen = 0;
+			pAd->SharedKey[bss_index][key_index].CipherAlg = CIPHER_NONE;
+		}
+	}
+
+	pAd->Antenna.word = 0;
+	pAd->CommonCfg.BBPCurrentBW = BW_20;
+
+	pAd->LedCntl.word = 0;
+
+	pAd->bAutoTxAgcA = FALSE;			// Default is OFF
+	pAd->bAutoTxAgcG = FALSE;			// Default is OFF
+	pAd->RfIcType = RFIC_2820;
+
+	// Init timer for reset complete event
+	pAd->CommonCfg.CentralChannel = 1;
+	pAd->bForcePrintTX = FALSE;
+	pAd->bForcePrintRX = FALSE;
+	pAd->bStaFifoTest = FALSE;
+	pAd->bProtectionTest = FALSE;
+	pAd->bHCCATest = FALSE;
+	pAd->bGenOneHCCA = FALSE;
+	pAd->CommonCfg.Dsifs = 10;      // in units of usec
+	pAd->CommonCfg.TxPower = 100; //mW
+	pAd->CommonCfg.TxPowerPercentage = 0xffffffff; // AUTO
+	pAd->CommonCfg.TxPowerDefault = 0xffffffff; // AUTO
+	pAd->CommonCfg.TxPreamble = Rt802_11PreambleAuto; // use Long preamble on TX by defaut
+	pAd->CommonCfg.bUseZeroToDisableFragment = FALSE;
+	pAd->CommonCfg.RtsThreshold = 2347;
+	pAd->CommonCfg.FragmentThreshold = 2346;
+	pAd->CommonCfg.UseBGProtection = 0;    // 0: AUTO
+	pAd->CommonCfg.bEnableTxBurst = TRUE; //0;
+	pAd->CommonCfg.PhyMode = 0xff;     // unknown
+	pAd->CommonCfg.BandState = UNKNOWN_BAND;
+	pAd->CommonCfg.RadarDetect.CSPeriod = 10;
+	pAd->CommonCfg.RadarDetect.CSCount = 0;
+	pAd->CommonCfg.RadarDetect.RDMode = RD_NORMAL_MODE;
+	pAd->CommonCfg.RadarDetect.ChMovingTime = 65;
+	pAd->CommonCfg.RadarDetect.LongPulseRadarTh = 3;
+	pAd->CommonCfg.bAPSDCapable = FALSE;
+	pAd->CommonCfg.bNeedSendTriggerFrame = FALSE;
+	pAd->CommonCfg.TriggerTimerCount = 0;
+	pAd->CommonCfg.bAPSDForcePowerSave = FALSE;
+	pAd->CommonCfg.bCountryFlag = FALSE;
+	pAd->CommonCfg.TxStream = 0;
+	pAd->CommonCfg.RxStream = 0;
+
+	NdisZeroMemory(&pAd->BeaconTxWI, sizeof(pAd->BeaconTxWI));
+
+#ifdef DOT11_N_SUPPORT
+	NdisZeroMemory(&pAd->CommonCfg.HtCapability, sizeof(pAd->CommonCfg.HtCapability));
+	pAd->HTCEnable = FALSE;
+	pAd->bBroadComHT = FALSE;
+	pAd->CommonCfg.bRdg = FALSE;
+
+#ifdef DOT11N_DRAFT3
+	pAd->CommonCfg.Dot11OBssScanPassiveDwell = dot11OBSSScanPassiveDwell;	// Unit : TU. 5~1000
+	pAd->CommonCfg.Dot11OBssScanActiveDwell = dot11OBSSScanActiveDwell;	// Unit : TU. 10~1000
+	pAd->CommonCfg.Dot11BssWidthTriggerScanInt = dot11BSSWidthTriggerScanInterval;	// Unit : Second
+	pAd->CommonCfg.Dot11OBssScanPassiveTotalPerChannel = dot11OBSSScanPassiveTotalPerChannel;	// Unit : TU. 200~10000
+	pAd->CommonCfg.Dot11OBssScanActiveTotalPerChannel = dot11OBSSScanActiveTotalPerChannel;	// Unit : TU. 20~10000
+	pAd->CommonCfg.Dot11BssWidthChanTranDelayFactor = dot11BSSWidthChannelTransactionDelayFactor;
+	pAd->CommonCfg.Dot11OBssScanActivityThre = dot11BSSScanActivityThreshold;	// Unit : percentage
+	pAd->CommonCfg.Dot11BssWidthChanTranDelay = (pAd->CommonCfg.Dot11BssWidthTriggerScanInt * pAd->CommonCfg.Dot11BssWidthChanTranDelayFactor);
+#endif  // DOT11N_DRAFT3 //
+
+	NdisZeroMemory(&pAd->CommonCfg.AddHTInfo, sizeof(pAd->CommonCfg.AddHTInfo));
+	pAd->CommonCfg.BACapability.field.MMPSmode = MMPS_ENABLE;
+	pAd->CommonCfg.BACapability.field.MpduDensity = 0;
+	pAd->CommonCfg.BACapability.field.Policy = IMMED_BA;
+	pAd->CommonCfg.BACapability.field.RxBAWinLimit = 64; //32;
+	pAd->CommonCfg.BACapability.field.TxBAWinLimit = 64; //32;
+	DBGPRINT(RT_DEBUG_TRACE, ("--> UserCfgInit. BACapability = 0x%x\n", pAd->CommonCfg.BACapability.word));
+
+	pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+	BATableInit(pAd, &pAd->BATable);
+
+	pAd->CommonCfg.bExtChannelSwitchAnnouncement = 1;
+	pAd->CommonCfg.bHTProtect = 1;
+	pAd->CommonCfg.bMIMOPSEnable = TRUE;
+	pAd->CommonCfg.bBADecline = FALSE;
+	pAd->CommonCfg.bDisableReordering = FALSE;
+
+	pAd->CommonCfg.TxBASize = 7;
+
+	pAd->CommonCfg.REGBACapability.word = pAd->CommonCfg.BACapability.word;
+#endif // DOT11_N_SUPPORT //
+
+	//pAd->CommonCfg.HTPhyMode.field.BW = BW_20;
+	//pAd->CommonCfg.HTPhyMode.field.MCS = MCS_AUTO;
+	//pAd->CommonCfg.HTPhyMode.field.ShortGI = GI_800;
+	//pAd->CommonCfg.HTPhyMode.field.STBC = STBC_NONE;
+	pAd->CommonCfg.TxRate = RATE_6;
+
+	pAd->CommonCfg.MlmeTransmit.field.MCS = MCS_RATE_6;
+	pAd->CommonCfg.MlmeTransmit.field.BW = BW_20;
+	pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+
+	pAd->CommonCfg.BeaconPeriod = 100;     // in mSec
+
+	//
+	// part II. intialize STA specific configuration
+	//
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		RX_FILTER_SET_FLAG(pAd, fRX_FILTER_ACCEPT_DIRECT);
+		RX_FILTER_CLEAR_FLAG(pAd, fRX_FILTER_ACCEPT_MULTICAST);
+		RX_FILTER_SET_FLAG(pAd, fRX_FILTER_ACCEPT_BROADCAST);
+		RX_FILTER_SET_FLAG(pAd, fRX_FILTER_ACCEPT_ALL_MULTICAST);
+
+		pAd->StaCfg.Psm = PWR_ACTIVE;
+
+		pAd->StaCfg.OrigWepStatus = Ndis802_11EncryptionDisabled;
+		pAd->StaCfg.PairCipher = Ndis802_11EncryptionDisabled;
+		pAd->StaCfg.GroupCipher = Ndis802_11EncryptionDisabled;
+		pAd->StaCfg.bMixCipher = FALSE;
+		pAd->StaCfg.DefaultKeyId = 0;
+
+		// 802.1x port control
+		pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilter8021xWEP;
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+		pAd->StaCfg.LastMicErrorTime = 0;
+		pAd->StaCfg.MicErrCnt        = 0;
+		pAd->StaCfg.bBlockAssoc      = FALSE;
+		pAd->StaCfg.WpaState         = SS_NOTUSE;
+
+		pAd->CommonCfg.NdisRadioStateOff = FALSE;		// New to support microsoft disable radio with OID command
+
+		pAd->StaCfg.RssiTrigger = 0;
+		NdisZeroMemory(&pAd->StaCfg.RssiSample, sizeof(RSSI_SAMPLE));
+		pAd->StaCfg.RssiTriggerMode = RSSI_TRIGGERED_UPON_BELOW_THRESHOLD;
+		pAd->StaCfg.AtimWin = 0;
+		pAd->StaCfg.DefaultListenCount = 3;//default listen count;
+		pAd->StaCfg.BssType = BSS_INFRA;  // BSS_INFRA or BSS_ADHOC or BSS_MONITOR
+		pAd->StaCfg.bScanReqIsFromWebUI = FALSE;
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_WAKEUP_NOW);
+
+		pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+	}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	pAd->StaCfg.IEEE80211dClientMode = Rt802_11_D_None;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+
+	// global variables mXXXX used in MAC protocol state machines
+	OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_INFRA_ON);
+
+	// PHY specification
+	pAd->CommonCfg.PhyMode = PHY_11BG_MIXED;		// default PHY mode
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);  // CCK use LONG preamble
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// user desired power mode
+		pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+		pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+		pAd->StaCfg.bWindowsACCAMEnable = FALSE;
+
+#ifdef LEAP_SUPPORT
+		// CCX v1.0 releated init value
+		RTMPInitTimer(pAd, &pAd->StaCfg.LeapAuthTimer, GET_TIMER_FUNCTION(LeapAuthTimeout), pAd, FALSE);
+		pAd->StaCfg.LeapAuthMode = CISCO_AuthModeLEAPNone;
+		pAd->StaCfg.bCkipOn = FALSE;
+#endif // LEAP_SUPPORT //
+
+		RTMPInitTimer(pAd, &pAd->StaCfg.StaQuickResponeForRateUpTimer, GET_TIMER_FUNCTION(StaQuickResponeForRateUpExec), pAd, FALSE);
+		pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = FALSE;
+
+		// Patch for Ndtest
+		pAd->StaCfg.ScanCnt = 0;
+
+		// CCX 2.0 control flag init
+		pAd->StaCfg.CCXEnable = FALSE;
+		pAd->StaCfg.CCXReqType = MSRN_TYPE_UNUSED;
+		pAd->StaCfg.CCXQosECWMin	= 4;
+		pAd->StaCfg.CCXQosECWMax	= 10;
+
+		pAd->StaCfg.bHwRadio  = TRUE; // Default Hardware Radio status is On
+		pAd->StaCfg.bSwRadio  = TRUE; // Default Software Radio status is On
+		pAd->StaCfg.bRadio    = TRUE; // bHwRadio && bSwRadio
+		pAd->StaCfg.bHardwareRadio = FALSE;		// Default is OFF
+		pAd->StaCfg.bShowHiddenSSID = FALSE;		// Default no show
+
+		// Nitro mode control
+		pAd->StaCfg.bAutoReconnect = TRUE;
+
+		// Save the init time as last scan time, the system should do scan after 2 seconds.
+		// This patch is for driver wake up from standby mode, system will do scan right away.
+		pAd->StaCfg.LastScanTime = 0;
+		NdisZeroMemory(pAd->nickname, IW_ESSID_MAX_SIZE+1);
+		sprintf(pAd->nickname, "%s", STA_NIC_DEVICE_NAME);
+		RTMPInitTimer(pAd, &pAd->StaCfg.WpaDisassocAndBlockAssocTimer, GET_TIMER_FUNCTION(WpaDisassocApAndBlockAssoc), pAd, FALSE);
+#ifdef WPA_SUPPLICANT_SUPPORT
+		pAd->StaCfg.IEEE8021X = FALSE;
+		pAd->StaCfg.IEEE8021x_required_keys = FALSE;
+		pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+		pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE;
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Default for extra information is not valid
+	pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+
+	// Default Config change flag
+	pAd->bConfigChanged = FALSE;
+
+	//
+	// part III. AP configurations
+	//
+
+
+	//
+	// part IV. others
+	//
+	// dynamic BBP R66:sensibity tuning to overcome background noise
+	pAd->BbpTuning.bEnable                = TRUE;
+	pAd->BbpTuning.FalseCcaLowerThreshold = 100;
+	pAd->BbpTuning.FalseCcaUpperThreshold = 512;
+	pAd->BbpTuning.R66Delta               = 4;
+	pAd->Mlme.bEnableAutoAntennaCheck = TRUE;
+
+	//
+	// Also initial R66CurrentValue, RTUSBResumeMsduTransmission might use this value.
+	// if not initial this value, the default value will be 0.
+	//
+	pAd->BbpTuning.R66CurrentValue = 0x38;
+
+	pAd->Bbp94 = BBPR94_DEFAULT;
+	pAd->BbpForCCK = FALSE;
+
+	// Default is FALSE for test bit 1
+	//pAd->bTest1 = FALSE;
+
+	// initialize MAC table and allocate spin lock
+	NdisZeroMemory(&pAd->MacTab, sizeof(MAC_TABLE));
+	InitializeQueueHeader(&pAd->MacTab.McastPsQueue);
+	NdisAllocateSpinLock(&pAd->MacTabLock);
+
+	//RTMPInitTimer(pAd, &pAd->RECBATimer, RECBATimerTimeout, pAd, TRUE);
+	//RTMPSetTimer(&pAd->RECBATimer, REORDER_EXEC_INTV);
+
+#ifdef RALINK_ATE
+	NdisZeroMemory(&pAd->ate, sizeof(ATE_INFO));
+	pAd->ate.Mode = ATE_STOP;
+	pAd->ate.TxCount = 200;/* to exceed TX_RING_SIZE ... */
+	pAd->ate.TxLength = 1024;
+	pAd->ate.TxWI.ShortGI = 0;// LONG GI : 800 ns
+	pAd->ate.TxWI.PHYMODE = MODE_CCK;
+	pAd->ate.TxWI.MCS = 3;
+	pAd->ate.TxWI.BW = BW_20;
+	pAd->ate.Channel = 1;
+	pAd->ate.QID = QID_AC_BE;
+	pAd->ate.Addr1[0] = 0x00;
+	pAd->ate.Addr1[1] = 0x11;
+	pAd->ate.Addr1[2] = 0x22;
+	pAd->ate.Addr1[3] = 0xAA;
+	pAd->ate.Addr1[4] = 0xBB;
+	pAd->ate.Addr1[5] = 0xCC;
+	NdisMoveMemory(pAd->ate.Addr2, pAd->ate.Addr1, ETH_LENGTH_OF_ADDRESS);
+	NdisMoveMemory(pAd->ate.Addr3, pAd->ate.Addr1, ETH_LENGTH_OF_ADDRESS);
+	pAd->ate.bRxFer = 0;
+	pAd->ate.bQATxStart = FALSE;
+	pAd->ate.bQARxStart = FALSE;
+#ifdef RALINK_28xx_QA
+	//pAd->ate.Repeat = 0;
+	pAd->ate.TxStatus = 0;
+	pAd->ate.AtePid = THREAD_PID_INIT_VALUE;
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+
+	pAd->CommonCfg.bWiFiTest = FALSE;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- UserCfgInit\n"));
+}
+
+// IRQL = PASSIVE_LEVEL
+UCHAR BtoH(char ch)
+{
+	if (ch >= '0' && ch <= '9') return (ch - '0');        // Handle numerals
+	if (ch >= 'A' && ch <= 'F') return (ch - 'A' + 0xA);  // Handle capitol hex digits
+	if (ch >= 'a' && ch <= 'f') return (ch - 'a' + 0xA);  // Handle small hex digits
+	return(255);
+}
+
+//
+//  FUNCTION: AtoH(char *, UCHAR *, int)
+//
+//  PURPOSE:  Converts ascii string to network order hex
+//
+//  PARAMETERS:
+//    src    - pointer to input ascii string
+//    dest   - pointer to output hex
+//    destlen - size of dest
+//
+//  COMMENTS:
+//
+//    2 ascii bytes make a hex byte so must put 1st ascii byte of pair
+//    into upper nibble and 2nd ascii byte of pair into lower nibble.
+//
+// IRQL = PASSIVE_LEVEL
+
+void AtoH(char * src, UCHAR * dest, int destlen)
+{
+	char * srcptr;
+	PUCHAR destTemp;
+
+	srcptr = src;
+	destTemp = (PUCHAR) dest;
+
+	while(destlen--)
+	{
+		*destTemp = BtoH(*srcptr++) << 4;    // Put 1st ascii byte in upper nibble.
+		*destTemp += BtoH(*srcptr++);      // Add 2nd ascii byte to above.
+		destTemp++;
+	}
+}
+
+VOID	RTMPPatchMacBbpBug(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	ULONG	Index;
+
+	// Initialize BBP register to default value
+	for (Index = 0; Index < NUM_BBP_REG_PARMS; Index++)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBPRegTable[Index].Register, (UCHAR)BBPRegTable[Index].Value);
+	}
+
+	// Initialize RF register to default value
+	AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+	// Re-init BBP register from EEPROM value
+	NICInitAsicFromEEPROM(pAd);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init timer objects
+
+	Arguments:
+		pAd			Pointer to our adapter
+		pTimer				Timer structure
+		pTimerFunc			Function to execute when timer expired
+		Repeat				Ture for period timer
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitTimer(
+	IN	PRTMP_ADAPTER			pAd,
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	PVOID					pTimerFunc,
+	IN	PVOID					pData,
+	IN	BOOLEAN					Repeat)
+{
+	//
+	// Set Valid to TRUE for later used.
+	// It will crash if we cancel a timer or set a timer
+	// that we haven't initialize before.
+	//
+	pTimer->Valid      = TRUE;
+
+	pTimer->PeriodicType = Repeat;
+	pTimer->State      = FALSE;
+	pTimer->cookie = (ULONG) pData;
+
+#ifdef RT2870
+	pTimer->pAd = pAd;
+#endif // RT2870 //
+
+	RTMP_OS_Init_Timer(pAd,	&pTimer->TimerObj,	pTimerFunc, (PVOID) pTimer);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init timer objects
+
+	Arguments:
+		pTimer				Timer structure
+		Value				Timer value in milliseconds
+
+	Return Value:
+		None
+
+	Note:
+		To use this routine, must call RTMPInitTimer before.
+
+	========================================================================
+*/
+VOID	RTMPSetTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	ULONG					Value)
+{
+	if (pTimer->Valid)
+	{
+		pTimer->TimerValue = Value;
+		pTimer->State      = FALSE;
+		if (pTimer->PeriodicType == TRUE)
+		{
+			pTimer->Repeat = TRUE;
+			RTMP_SetPeriodicTimer(&pTimer->TimerObj, Value);
+		}
+		else
+		{
+			pTimer->Repeat = FALSE;
+			RTMP_OS_Add_Timer(&pTimer->TimerObj, Value);
+		}
+	}
+	else
+	{
+		DBGPRINT_ERR(("RTMPSetTimer failed, Timer hasn't been initialize!\n"));
+	}
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init timer objects
+
+	Arguments:
+		pTimer				Timer structure
+		Value				Timer value in milliseconds
+
+	Return Value:
+		None
+
+	Note:
+		To use this routine, must call RTMPInitTimer before.
+
+	========================================================================
+*/
+VOID	RTMPModTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	ULONG					Value)
+{
+	BOOLEAN	Cancel;
+
+	if (pTimer->Valid)
+	{
+		pTimer->TimerValue = Value;
+		pTimer->State      = FALSE;
+		if (pTimer->PeriodicType == TRUE)
+		{
+			RTMPCancelTimer(pTimer, &Cancel);
+			RTMPSetTimer(pTimer, Value);
+		}
+		else
+		{
+			RTMP_OS_Mod_Timer(&pTimer->TimerObj, Value);
+		}
+	}
+	else
+	{
+		DBGPRINT_ERR(("RTMPModTimer failed, Timer hasn't been initialize!\n"));
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Cancel timer objects
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		1.) To use this routine, must call RTMPInitTimer before.
+		2.) Reset NIC to initial state AS IS system boot up time.
+
+	========================================================================
+*/
+VOID	RTMPCancelTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	OUT	BOOLEAN					*pCancelled)
+{
+	if (pTimer->Valid)
+	{
+		if (pTimer->State == FALSE)
+			pTimer->Repeat = FALSE;
+			RTMP_OS_Del_Timer(&pTimer->TimerObj, pCancelled);
+
+		if (*pCancelled == TRUE)
+			pTimer->State = TRUE;
+
+#ifdef RT2870
+		// We need to go-through the TimerQ to findout this timer handler and remove it if
+		//		it's still waiting for execution.
+
+		RT2870_TimerQ_Remove(pTimer->pAd, pTimer);
+#endif // RT2870 //
+	}
+	else
+	{
+		//
+		// NdisMCancelTimer just canced the timer and not mean release the timer.
+		// And don't set the "Valid" to False. So that we can use this timer again.
+		//
+		DBGPRINT_ERR(("RTMPCancelTimer failed, Timer hasn't been initialize!\n"));
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set LED Status
+
+	Arguments:
+		pAd						Pointer to our adapter
+		Status					LED Status
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPSetLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN UCHAR			Status)
+{
+	//ULONG			data;
+	UCHAR			HighByte = 0;
+	UCHAR			LowByte;
+
+// In ATE mode of RT2860 AP/STA, we have erased 8051 firmware.
+// So LED mode is not supported when ATE is running.
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	LowByte = pAd->LedCntl.field.LedMode&0x7f;
+	switch (Status)
+	{
+		case LED_LINK_DOWN:
+			HighByte = 0x20;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			pAd->LedIndicatorStregth = 0;
+			break;
+		case LED_LINK_UP:
+			if (pAd->CommonCfg.Channel > 14)
+				HighByte = 0xa0;
+			else
+				HighByte = 0x60;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_RADIO_ON:
+			HighByte = 0x20;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_HALT:
+			LowByte = 0; // Driver sets MAC register and MAC controls LED
+		case LED_RADIO_OFF:
+			HighByte = 0;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+        case LED_WPS:
+			HighByte = 0x10;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_ON_SITE_SURVEY:
+			HighByte = 0x08;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_POWER_UP:
+			HighByte = 0x04;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		default:
+			DBGPRINT(RT_DEBUG_WARN, ("RTMPSetLED::Unknown Status %d\n", Status));
+			break;
+	}
+
+    //
+	// Keep LED status for LED SiteSurvey mode.
+	// After SiteSurvey, we will set the LED mode to previous status.
+	//
+	if ((Status != LED_ON_SITE_SURVEY) && (Status != LED_POWER_UP))
+		pAd->LedStatus = Status;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetLED::Mode=%d,HighByte=0x%02x,LowByte=0x%02x\n", pAd->LedCntl.field.LedMode, HighByte, LowByte));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set LED Signal Stregth
+
+	Arguments:
+		pAd						Pointer to our adapter
+		Dbm						Signal Stregth
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Can be run on any IRQL level.
+
+		According to Microsoft Zero Config Wireless Signal Stregth definition as belows.
+		<= -90  No Signal
+		<= -81  Very Low
+		<= -71  Low
+		<= -67  Good
+		<= -57  Very Good
+		 > -57  Excellent
+	========================================================================
+*/
+VOID RTMPSetSignalLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN NDIS_802_11_RSSI Dbm)
+{
+	UCHAR		nLed = 0;
+
+	//
+	// if not Signal Stregth, then do nothing.
+	//
+	if (pAd->LedCntl.field.LedMode != LED_MODE_SIGNAL_STREGTH)
+	{
+		return;
+	}
+
+	if (Dbm <= -90)
+		nLed = 0;
+	else if (Dbm <= -81)
+		nLed = 1;
+	else if (Dbm <= -71)
+		nLed = 3;
+	else if (Dbm <= -67)
+		nLed = 7;
+	else if (Dbm <= -57)
+		nLed = 15;
+	else
+		nLed = 31;
+
+	//
+	// Update Signal Stregth to firmware if changed.
+	//
+	if (pAd->LedIndicatorStregth != nLed)
+	{
+		AsicSendCommandToMcu(pAd, 0x51, 0xff, nLed, pAd->LedCntl.field.Polarity);
+		pAd->LedIndicatorStregth = nLed;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Enable RX
+
+	Arguments:
+		pAd						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL <= DISPATCH_LEVEL
+
+	Note:
+		Before Enable RX, make sure you have enabled Interrupt.
+	========================================================================
+*/
+VOID RTMPEnableRxTx(
+	IN PRTMP_ADAPTER	pAd)
+{
+//	WPDMA_GLO_CFG_STRUC	GloCfg;
+//	ULONG	i = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> RTMPEnableRxTx\n"));
+
+#if 0
+	// Enable Rx DMA.
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x4);
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0)  && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("==>  DMABusy\n"));
+		RTMPusecDelay(1000);
+		i++;
+	}while ( i <200);
+
+	RTMPusecDelay(50);
+	RT28XX_DMA_WRITE_INIT(GloCfg);
+	DBGPRINT(RT_DEBUG_TRACE, ("<== WRITE DMA offset 0x208 = 0x%x\n", GloCfg.word));
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+	RT28XX_DMA_POST_WRITE(pAd);
+#else
+	// Enable Rx DMA.
+	RT28XXDMAEnable(pAd);
+#endif
+
+	// enable RX of MAC block
+	if (pAd->OpMode == OPMODE_AP)
+	{
+		UINT32 rx_filter_flag = APNORMAL;
+
+
+		RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, rx_filter_flag);     // enable RX of DMA block
+	}
+	else
+	{
+		RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL);     // Staion not drop control frame will fail WiFi Certification.
+	}
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0xc);
+	DBGPRINT(RT_DEBUG_TRACE, ("<== RTMPEnableRxTx\n"));
+}
+
+
diff --git a/drivers/staging/rt2870/common/rtmp_tkip.c b/drivers/staging/rt2870/common/rtmp_tkip.c
new file mode 100644
index 000000000000..2847409ff081
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtmp_tkip.c
@@ -0,0 +1,1613 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_tkip.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Paul Wu		02-25-02		Initial
+*/
+
+#include "../rt_config.h"
+
+// Rotation functions on 32 bit values
+#define ROL32( A, n ) \
+	( ((A) << (n)) | ( ((A)>>(32-(n))) & ( (1UL << (n)) - 1 ) ) )
+#define ROR32( A, n ) ROL32( (A), 32-(n) )
+
+UINT Tkip_Sbox_Lower[256] =
+{
+	0xA5,0x84,0x99,0x8D,0x0D,0xBD,0xB1,0x54,
+	0x50,0x03,0xA9,0x7D,0x19,0x62,0xE6,0x9A,
+	0x45,0x9D,0x40,0x87,0x15,0xEB,0xC9,0x0B,
+	0xEC,0x67,0xFD,0xEA,0xBF,0xF7,0x96,0x5B,
+	0xC2,0x1C,0xAE,0x6A,0x5A,0x41,0x02,0x4F,
+	0x5C,0xF4,0x34,0x08,0x93,0x73,0x53,0x3F,
+	0x0C,0x52,0x65,0x5E,0x28,0xA1,0x0F,0xB5,
+	0x09,0x36,0x9B,0x3D,0x26,0x69,0xCD,0x9F,
+	0x1B,0x9E,0x74,0x2E,0x2D,0xB2,0xEE,0xFB,
+	0xF6,0x4D,0x61,0xCE,0x7B,0x3E,0x71,0x97,
+	0xF5,0x68,0x00,0x2C,0x60,0x1F,0xC8,0xED,
+	0xBE,0x46,0xD9,0x4B,0xDE,0xD4,0xE8,0x4A,
+	0x6B,0x2A,0xE5,0x16,0xC5,0xD7,0x55,0x94,
+	0xCF,0x10,0x06,0x81,0xF0,0x44,0xBA,0xE3,
+	0xF3,0xFE,0xC0,0x8A,0xAD,0xBC,0x48,0x04,
+	0xDF,0xC1,0x75,0x63,0x30,0x1A,0x0E,0x6D,
+	0x4C,0x14,0x35,0x2F,0xE1,0xA2,0xCC,0x39,
+	0x57,0xF2,0x82,0x47,0xAC,0xE7,0x2B,0x95,
+	0xA0,0x98,0xD1,0x7F,0x66,0x7E,0xAB,0x83,
+	0xCA,0x29,0xD3,0x3C,0x79,0xE2,0x1D,0x76,
+	0x3B,0x56,0x4E,0x1E,0xDB,0x0A,0x6C,0xE4,
+	0x5D,0x6E,0xEF,0xA6,0xA8,0xA4,0x37,0x8B,
+	0x32,0x43,0x59,0xB7,0x8C,0x64,0xD2,0xE0,
+	0xB4,0xFA,0x07,0x25,0xAF,0x8E,0xE9,0x18,
+	0xD5,0x88,0x6F,0x72,0x24,0xF1,0xC7,0x51,
+	0x23,0x7C,0x9C,0x21,0xDD,0xDC,0x86,0x85,
+	0x90,0x42,0xC4,0xAA,0xD8,0x05,0x01,0x12,
+	0xA3,0x5F,0xF9,0xD0,0x91,0x58,0x27,0xB9,
+	0x38,0x13,0xB3,0x33,0xBB,0x70,0x89,0xA7,
+	0xB6,0x22,0x92,0x20,0x49,0xFF,0x78,0x7A,
+	0x8F,0xF8,0x80,0x17,0xDA,0x31,0xC6,0xB8,
+	0xC3,0xB0,0x77,0x11,0xCB,0xFC,0xD6,0x3A
+};
+
+UINT Tkip_Sbox_Upper[256] =
+{
+	0xC6,0xF8,0xEE,0xF6,0xFF,0xD6,0xDE,0x91,
+	0x60,0x02,0xCE,0x56,0xE7,0xB5,0x4D,0xEC,
+	0x8F,0x1F,0x89,0xFA,0xEF,0xB2,0x8E,0xFB,
+	0x41,0xB3,0x5F,0x45,0x23,0x53,0xE4,0x9B,
+	0x75,0xE1,0x3D,0x4C,0x6C,0x7E,0xF5,0x83,
+	0x68,0x51,0xD1,0xF9,0xE2,0xAB,0x62,0x2A,
+	0x08,0x95,0x46,0x9D,0x30,0x37,0x0A,0x2F,
+	0x0E,0x24,0x1B,0xDF,0xCD,0x4E,0x7F,0xEA,
+	0x12,0x1D,0x58,0x34,0x36,0xDC,0xB4,0x5B,
+	0xA4,0x76,0xB7,0x7D,0x52,0xDD,0x5E,0x13,
+	0xA6,0xB9,0x00,0xC1,0x40,0xE3,0x79,0xB6,
+	0xD4,0x8D,0x67,0x72,0x94,0x98,0xB0,0x85,
+	0xBB,0xC5,0x4F,0xED,0x86,0x9A,0x66,0x11,
+	0x8A,0xE9,0x04,0xFE,0xA0,0x78,0x25,0x4B,
+	0xA2,0x5D,0x80,0x05,0x3F,0x21,0x70,0xF1,
+	0x63,0x77,0xAF,0x42,0x20,0xE5,0xFD,0xBF,
+	0x81,0x18,0x26,0xC3,0xBE,0x35,0x88,0x2E,
+	0x93,0x55,0xFC,0x7A,0xC8,0xBA,0x32,0xE6,
+	0xC0,0x19,0x9E,0xA3,0x44,0x54,0x3B,0x0B,
+	0x8C,0xC7,0x6B,0x28,0xA7,0xBC,0x16,0xAD,
+	0xDB,0x64,0x74,0x14,0x92,0x0C,0x48,0xB8,
+	0x9F,0xBD,0x43,0xC4,0x39,0x31,0xD3,0xF2,
+	0xD5,0x8B,0x6E,0xDA,0x01,0xB1,0x9C,0x49,
+	0xD8,0xAC,0xF3,0xCF,0xCA,0xF4,0x47,0x10,
+	0x6F,0xF0,0x4A,0x5C,0x38,0x57,0x73,0x97,
+	0xCB,0xA1,0xE8,0x3E,0x96,0x61,0x0D,0x0F,
+	0xE0,0x7C,0x71,0xCC,0x90,0x06,0xF7,0x1C,
+	0xC2,0x6A,0xAE,0x69,0x17,0x99,0x3A,0x27,
+	0xD9,0xEB,0x2B,0x22,0xD2,0xA9,0x07,0x33,
+	0x2D,0x3C,0x15,0xC9,0x87,0xAA,0x50,0xA5,
+	0x03,0x59,0x09,0x1A,0x65,0xD7,0x84,0xD0,
+	0x82,0x29,0x5A,0x1E,0x7B,0xA8,0x6D,0x2C
+};
+
+/*****************************/
+/******** SBOX Table *********/
+/*****************************/
+
+UCHAR SboxTable[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
+};
+
+VOID xor_32(
+	IN  PUCHAR              a,
+	IN  PUCHAR              b,
+	OUT PUCHAR              out);
+
+VOID xor_128(
+	IN  PUCHAR              a,
+	IN  PUCHAR              b,
+	OUT PUCHAR              out);
+
+VOID next_key(
+	IN  PUCHAR              key,
+	IN  INT                 round);
+
+VOID byte_sub(
+	IN  PUCHAR              in,
+	OUT PUCHAR              out);
+
+VOID shift_row(
+	IN  PUCHAR              in,
+	OUT PUCHAR              out);
+
+VOID mix_column(
+	IN  PUCHAR              in,
+	OUT PUCHAR              out);
+
+UCHAR RTMPCkipSbox(
+	IN  UCHAR               a);
+//
+// Expanded IV for TKIP function.
+//
+typedef	struct	PACKED _IV_CONTROL_
+{
+	union PACKED
+	{
+		struct PACKED
+		{
+			UCHAR		rc0;
+			UCHAR		rc1;
+			UCHAR		rc2;
+
+			union PACKED
+			{
+				struct PACKED
+				{
+#ifdef RT_BIG_ENDIAN
+					UCHAR	KeyID:2;
+					UCHAR	ExtIV:1;
+					UCHAR	Rsvd:5;
+#else
+					UCHAR	Rsvd:5;
+					UCHAR	ExtIV:1;
+					UCHAR	KeyID:2;
+#endif
+				}	field;
+				UCHAR		Byte;
+			}	CONTROL;
+		}	field;
+
+		ULONG	word;
+	}	IV16;
+
+	ULONG	IV32;
+}	TKIP_IV, *PTKIP_IV;
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Convert from UCHAR[] to ULONG in a portable way
+
+	Arguments:
+      pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+ULONG	RTMPTkipGetUInt32(
+	IN	PUCHAR	pMICKey)
+{
+	ULONG	res = 0;
+	INT		i;
+
+	for (i = 0; i < 4; i++)
+	{
+		res |= (*pMICKey++) << (8 * i);
+	}
+
+	return res;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Convert from ULONG to UCHAR[] in a portable way
+
+	Arguments:
+      pDst			pointer to destination for convert ULONG to UCHAR[]
+      val			the value for convert
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPTkipPutUInt32(
+	IN OUT	PUCHAR		pDst,
+	IN		ULONG		val)
+{
+	INT i;
+
+	for(i = 0; i < 4; i++)
+	{
+		*pDst++ = (UCHAR) (val & 0xff);
+		val >>= 8;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Set the MIC Key.
+
+	Arguments:
+      pAd		Pointer to our adapter
+      pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPTkipSetMICKey(
+	IN	PTKIP_KEY_INFO	pTkip,
+	IN	PUCHAR			pMICKey)
+{
+	// Set the key
+	pTkip->K0 = RTMPTkipGetUInt32(pMICKey);
+	pTkip->K1 = RTMPTkipGetUInt32(pMICKey + 4);
+	// and reset the message
+	pTkip->L = pTkip->K0;
+	pTkip->R = pTkip->K1;
+	pTkip->nBytesInM = 0;
+	pTkip->M = 0;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Calculate the MIC Value.
+
+	Arguments:
+      pAd		Pointer to our adapter
+      uChar			Append this uChar
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPTkipAppendByte(
+	IN	PTKIP_KEY_INFO	pTkip,
+	IN	UCHAR 			uChar)
+{
+	// Append the byte to our word-sized buffer
+	pTkip->M |= (uChar << (8* pTkip->nBytesInM));
+	pTkip->nBytesInM++;
+	// Process the word if it is full.
+	if( pTkip->nBytesInM >= 4 )
+	{
+		pTkip->L ^= pTkip->M;
+		pTkip->R ^= ROL32( pTkip->L, 17 );
+		pTkip->L += pTkip->R;
+		pTkip->R ^= ((pTkip->L & 0xff00ff00) >> 8) | ((pTkip->L & 0x00ff00ff) << 8);
+		pTkip->L += pTkip->R;
+		pTkip->R ^= ROL32( pTkip->L, 3 );
+		pTkip->L += pTkip->R;
+		pTkip->R ^= ROR32( pTkip->L, 2 );
+		pTkip->L += pTkip->R;
+		// Clear the buffer
+		pTkip->M = 0;
+		pTkip->nBytesInM = 0;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Calculate the MIC Value.
+
+	Arguments:
+      pAd		Pointer to our adapter
+      pSrc			Pointer to source data for Calculate MIC Value
+      Len			Indicate the length of the source data
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPTkipAppend(
+	IN	PTKIP_KEY_INFO	pTkip,
+	IN	PUCHAR			pSrc,
+	IN	UINT			nBytes)
+{
+	// This is simple
+	while(nBytes > 0)
+	{
+		RTMPTkipAppendByte(pTkip, *pSrc++);
+		nBytes--;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Get the MIC Value.
+
+	Arguments:
+      pAd		Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		the MIC Value is store in pAd->PrivateInfo.MIC
+	========================================================================
+*/
+VOID	RTMPTkipGetMIC(
+	IN	PTKIP_KEY_INFO	pTkip)
+{
+	// Append the minimum padding
+	RTMPTkipAppendByte(pTkip, 0x5a );
+	RTMPTkipAppendByte(pTkip, 0 );
+	RTMPTkipAppendByte(pTkip, 0 );
+	RTMPTkipAppendByte(pTkip, 0 );
+	RTMPTkipAppendByte(pTkip, 0 );
+	// and then zeroes until the length is a multiple of 4
+	while( pTkip->nBytesInM != 0 )
+	{
+		RTMPTkipAppendByte(pTkip, 0 );
+	}
+	// The appendByte function has already computed the result.
+	RTMPTkipPutUInt32(pTkip->MIC, pTkip->L);
+	RTMPTkipPutUInt32(pTkip->MIC + 4, pTkip->R);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Init Tkip function.
+
+	Arguments:
+      pAd		Pointer to our adapter
+		pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
+		KeyId		TK Key ID
+		pTA			Pointer to transmitter address
+		pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitTkipEngine(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pKey,
+	IN	UCHAR			KeyId,
+	IN	PUCHAR			pTA,
+	IN	PUCHAR			pMICKey,
+	IN	PUCHAR			pTSC,
+	OUT	PULONG			pIV16,
+	OUT	PULONG			pIV32)
+{
+	TKIP_IV	tkipIv;
+
+	// Prepare 8 bytes TKIP encapsulation for MPDU
+	NdisZeroMemory(&tkipIv, sizeof(TKIP_IV));
+	tkipIv.IV16.field.rc0 = *(pTSC + 1);
+	tkipIv.IV16.field.rc1 = (tkipIv.IV16.field.rc0 | 0x20) & 0x7f;
+	tkipIv.IV16.field.rc2 = *pTSC;
+	tkipIv.IV16.field.CONTROL.field.ExtIV = 1;  // 0: non-extended IV, 1: an extended IV
+	tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;
+//	tkipIv.IV32 = *(PULONG)(pTSC + 2);
+	NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4);   // Copy IV
+
+	*pIV16 = tkipIv.IV16.word;
+	*pIV32 = tkipIv.IV32;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Init MIC Value calculation function which include set MIC key &
+		calculate first 16 bytes (DA + SA + priority +  0)
+
+	Arguments:
+      pAd		Pointer to our adapter
+		pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
+		pDA			Pointer to DA address
+		pSA			Pointer to SA address
+		pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitMICEngine(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pKey,
+	IN	PUCHAR			pDA,
+	IN	PUCHAR			pSA,
+	IN  UCHAR           UserPriority,
+	IN	PUCHAR			pMICKey)
+{
+	ULONG Priority = UserPriority;
+
+	// Init MIC value calculation
+	RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
+	// DA
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
+	// SA
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
+	// Priority + 3 bytes of 0
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Compare MIC value of received MSDU
+
+	Arguments:
+		pAd	Pointer to our adapter
+		pSrc        Pointer to the received Plain text data
+		pDA			Pointer to DA address
+		pSA			Pointer to SA address
+		pMICKey		pointer to MIC Key
+		Len         the length of the received plain text data exclude MIC value
+
+	Return Value:
+		TRUE        MIC value matched
+		FALSE       MIC value mismatched
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN	RTMPTkipCompareMICValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pSrc,
+	IN	PUCHAR			pDA,
+	IN	PUCHAR			pSA,
+	IN	PUCHAR			pMICKey,
+	IN	UCHAR			UserPriority,
+	IN	UINT			Len)
+{
+	UCHAR	OldMic[8];
+	ULONG	Priority = UserPriority;
+
+	// Init MIC value calculation
+	RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
+	// DA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
+	// SA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
+	// Priority + 3 bytes of 0
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
+
+	// Calculate MIC value from plain text data
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
+
+	// Get MIC valude from received frame
+	NdisMoveMemory(OldMic, pSrc + Len, 8);
+
+	// Get MIC value from decrypted plain data
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
+
+	// Move MIC value from MSDU, this steps should move to data path.
+	// Since the MIC value might cross MPDUs.
+	if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n"));  //MIC error.
+
+
+		return (FALSE);
+	}
+	return (TRUE);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Compare MIC value of received MSDU
+
+	Arguments:
+		pAd	Pointer to our adapter
+		pLLC		LLC header
+		pSrc        Pointer to the received Plain text data
+		pDA			Pointer to DA address
+		pSA			Pointer to SA address
+		pMICKey		pointer to MIC Key
+		Len         the length of the received plain text data exclude MIC value
+
+	Return Value:
+		TRUE        MIC value matched
+		FALSE       MIC value mismatched
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN	RTMPTkipCompareMICValueWithLLC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pLLC,
+	IN	PUCHAR			pSrc,
+	IN	PUCHAR			pDA,
+	IN	PUCHAR			pSA,
+	IN	PUCHAR			pMICKey,
+	IN	UINT			Len)
+{
+	UCHAR	OldMic[8];
+	ULONG	Priority = 0;
+
+	// Init MIC value calculation
+	RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
+	// DA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
+	// SA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
+	// Priority + 3 bytes of 0
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
+
+	// Start with LLC header
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pLLC, 8);
+
+	// Calculate MIC value from plain text data
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
+
+	// Get MIC valude from received frame
+	NdisMoveMemory(OldMic, pSrc + Len, 8);
+
+	// Get MIC value from decrypted plain data
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
+
+	// Move MIC value from MSDU, this steps should move to data path.
+	// Since the MIC value might cross MPDUs.
+	if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValueWithLLC(): TKIP MIC Error !\n"));  //MIC error.
+
+
+		return (FALSE);
+	}
+	return (TRUE);
+}
+/*
+	========================================================================
+
+	Routine	Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware transmit function
+
+	Arguments:
+		pAd		Pointer	to our adapter
+		PNDIS_PACKET	Pointer to Ndis Packet for MIC calculation
+		pEncap			Pointer to LLC encap data
+		LenEncap		Total encap length, might be 0 which indicates no encap
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPCalculateMICValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			pEncap,
+	IN	PCIPHER_KEY		pKey,
+	IN	UCHAR			apidx)
+{
+	PACKET_INFO		PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	PUCHAR			pSrc;
+    UCHAR           UserPriority;
+	UCHAR			vlan_offset = 0;
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+
+	UserPriority = RTMP_GET_PACKET_UP(pPacket);
+	pSrc = pSrcBufVA;
+
+	// determine if this is a vlan packet
+	if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
+		vlan_offset = 4;
+
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+	{
+		RTMPInitMICEngine(
+			pAd,
+			pKey->Key,
+			pSrc,
+			pSrc + 6,
+			UserPriority,
+			pKey->TxMic);
+	}
+
+
+	if (pEncap != NULL)
+	{
+		// LLC encapsulation
+		RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
+		// Protocol Type
+		RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset, 2);
+	}
+	SrcBufLen -= (14 + vlan_offset);
+	pSrc += (14 + vlan_offset);
+	do
+	{
+		if (SrcBufLen > 0)
+		{
+			RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
+		}
+
+		break;	// No need handle next packet
+
+	}	while (TRUE);		// End of copying payload
+
+	// Compute the final MIC Value
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
+}
+
+
+/************************************************************/
+/* tkip_sbox()																*/
+/* Returns a 16 bit value from a 64K entry table. The Table */
+/* is synthesized from two 256 entry byte wide tables.		*/
+/************************************************************/
+
+UINT tkip_sbox(UINT index)
+{
+	UINT index_low;
+	UINT index_high;
+	UINT left, right;
+
+	index_low = (index % 256);
+	index_high = ((index >> 8) % 256);
+
+	left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256);
+	right = Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256);
+
+	return (left ^ right);
+}
+
+UINT rotr1(UINT a)
+{
+	unsigned int b;
+
+	if ((a & 0x01) == 0x01)
+	{
+		b = (a >> 1) | 0x8000;
+	}
+	else
+	{
+		b = (a >> 1) & 0x7fff;
+	}
+	b = b % 65536;
+	return b;
+}
+
+VOID RTMPTkipMixKey(
+	UCHAR *key,
+	UCHAR *ta,
+	ULONG pnl, /* Least significant 16 bits of PN */
+	ULONG pnh, /* Most significant 32 bits of PN */
+	UCHAR *rc4key,
+	UINT *p1k)
+{
+
+	UINT tsc0;
+	UINT tsc1;
+	UINT tsc2;
+
+	UINT ppk0;
+	UINT ppk1;
+	UINT ppk2;
+	UINT ppk3;
+	UINT ppk4;
+	UINT ppk5;
+
+	INT i;
+	INT j;
+
+	tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
+	tsc1 = (unsigned int)(pnh % 65536);
+	tsc2 = (unsigned int)(pnl % 65536); /* lsb */
+
+	/* Phase 1, step 1 */
+	p1k[0] = tsc1;
+	p1k[1] = tsc0;
+	p1k[2] = (UINT)(ta[0] + (ta[1]*256));
+	p1k[3] = (UINT)(ta[2] + (ta[3]*256));
+	p1k[4] = (UINT)(ta[4] + (ta[5]*256));
+
+	/* Phase 1, step 2 */
+	for (i=0; i<8; i++)
+	{
+		j = 2*(i & 1);
+		p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536;
+		p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536;
+		p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536;
+		p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536;
+		p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536;
+		p1k[4] = (p1k[4] + i) % 65536;
+	}
+
+	/* Phase 2, Step 1 */
+	ppk0 = p1k[0];
+	ppk1 = p1k[1];
+	ppk2 = p1k[2];
+	ppk3 = p1k[3];
+	ppk4 = p1k[4];
+	ppk5 = (p1k[4] + tsc2) % 65536;
+
+	/* Phase2, Step 2 */
+	ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536);
+	ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536);
+	ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536);
+	ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536);
+	ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536);
+	ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536);
+
+	ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12]));
+	ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14]));
+	ppk2 = ppk2 + rotr1(ppk1);
+	ppk3 = ppk3 + rotr1(ppk2);
+	ppk4 = ppk4 + rotr1(ppk3);
+	ppk5 = ppk5 + rotr1(ppk4);
+
+	/* Phase 2, Step 3 */
+    /* Phase 2, Step 3 */
+
+	tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
+	tsc1 = (unsigned int)(pnh % 65536);
+	tsc2 = (unsigned int)(pnl % 65536); /* lsb */
+
+	rc4key[0] = (tsc2 >> 8) % 256;
+	rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
+	rc4key[2] = tsc2 % 256;
+	rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256;
+
+	rc4key[4] = ppk0 % 256;
+	rc4key[5] = (ppk0 >> 8) % 256;
+
+	rc4key[6] = ppk1 % 256;
+	rc4key[7] = (ppk1 >> 8) % 256;
+
+	rc4key[8] = ppk2 % 256;
+	rc4key[9] = (ppk2 >> 8) % 256;
+
+	rc4key[10] = ppk3 % 256;
+	rc4key[11] = (ppk3 >> 8) % 256;
+
+	rc4key[12] = ppk4 % 256;
+	rc4key[13] = (ppk4 >> 8) % 256;
+
+	rc4key[14] = ppk5 % 256;
+	rc4key[15] = (ppk5 >> 8) % 256;
+}
+
+
+/************************************************/
+/* construct_mic_header1()                      */
+/* Builds the first MIC header block from       */
+/* header fields.                               */
+/************************************************/
+
+void construct_mic_header1(
+	unsigned char *mic_header1,
+	int header_length,
+	unsigned char *mpdu)
+{
+	mic_header1[0] = (unsigned char)((header_length - 2) / 256);
+	mic_header1[1] = (unsigned char)((header_length - 2) % 256);
+	mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
+	mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
+	mic_header1[4] = mpdu[4];       /* A1 */
+	mic_header1[5] = mpdu[5];
+	mic_header1[6] = mpdu[6];
+	mic_header1[7] = mpdu[7];
+	mic_header1[8] = mpdu[8];
+	mic_header1[9] = mpdu[9];
+	mic_header1[10] = mpdu[10];     /* A2 */
+	mic_header1[11] = mpdu[11];
+	mic_header1[12] = mpdu[12];
+	mic_header1[13] = mpdu[13];
+	mic_header1[14] = mpdu[14];
+	mic_header1[15] = mpdu[15];
+}
+
+/************************************************/
+/* construct_mic_header2()                      */
+/* Builds the last MIC header block from        */
+/* header fields.                               */
+/************************************************/
+
+void construct_mic_header2(
+	unsigned char *mic_header2,
+	unsigned char *mpdu,
+	int a4_exists,
+	int qc_exists)
+{
+	int i;
+
+	for (i = 0; i<16; i++) mic_header2[i]=0x00;
+
+	mic_header2[0] = mpdu[16];    /* A3 */
+	mic_header2[1] = mpdu[17];
+	mic_header2[2] = mpdu[18];
+	mic_header2[3] = mpdu[19];
+	mic_header2[4] = mpdu[20];
+	mic_header2[5] = mpdu[21];
+
+	// In Sequence Control field, mute sequence numer bits (12-bit)
+	mic_header2[6] = mpdu[22] & 0x0f;   /* SC */
+	mic_header2[7] = 0x00; /* mpdu[23]; */
+
+	if ((!qc_exists) & a4_exists)
+	{
+		for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
+
+	}
+
+	if (qc_exists && (!a4_exists))
+	{
+		mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
+		mic_header2[9] = mpdu[25] & 0x00;
+	}
+
+	if (qc_exists && a4_exists)
+	{
+		for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
+
+		mic_header2[14] = mpdu[30] & 0x0f;
+		mic_header2[15] = mpdu[31] & 0x00;
+	}
+}
+
+
+/************************************************/
+/* construct_mic_iv()                           */
+/* Builds the MIC IV from header fields and PN  */
+/************************************************/
+
+void construct_mic_iv(
+	unsigned char *mic_iv,
+	int qc_exists,
+	int a4_exists,
+	unsigned char *mpdu,
+	unsigned int payload_length,
+	unsigned char *pn_vector)
+{
+	int i;
+
+	mic_iv[0] = 0x59;
+	if (qc_exists && a4_exists)
+		mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC           */
+	if (qc_exists && !a4_exists)
+		mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4    */
+	if (!qc_exists)
+		mic_iv[1] = 0x00;
+	for (i = 2; i < 8; i++)
+		mic_iv[i] = mpdu[i + 8];                    /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
+#ifdef CONSISTENT_PN_ORDER
+		for (i = 8; i < 14; i++)
+			mic_iv[i] = pn_vector[i - 8];           /* mic_iv[8:13] = PN[0:5] */
+#else
+		for (i = 8; i < 14; i++)
+			mic_iv[i] = pn_vector[13 - i];          /* mic_iv[8:13] = PN[5:0] */
+#endif
+	i = (payload_length / 256);
+	i = (payload_length % 256);
+	mic_iv[14] = (unsigned char) (payload_length / 256);
+	mic_iv[15] = (unsigned char) (payload_length % 256);
+
+}
+
+
+
+/************************************/
+/* bitwise_xor()                    */
+/* A 128 bit, bitwise exclusive or  */
+/************************************/
+
+void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out)
+{
+	int i;
+	for (i=0; i<16; i++)
+	{
+		out[i] = ina[i] ^ inb[i];
+	}
+}
+
+
+void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
+{
+	int round;
+	int i;
+	unsigned char intermediatea[16];
+	unsigned char intermediateb[16];
+	unsigned char round_key[16];
+
+	for(i=0; i<16; i++) round_key[i] = key[i];
+
+	for (round = 0; round < 11; round++)
+	{
+		if (round == 0)
+		{
+			xor_128(round_key, data, ciphertext);
+			next_key(round_key, round);
+		}
+		else if (round == 10)
+		{
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			xor_128(intermediateb, round_key, ciphertext);
+		}
+		else    /* 1 - 9 */
+		{
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			mix_column(&intermediateb[0], &intermediatea[0]);
+			mix_column(&intermediateb[4], &intermediatea[4]);
+			mix_column(&intermediateb[8], &intermediatea[8]);
+			mix_column(&intermediateb[12], &intermediatea[12]);
+			xor_128(intermediatea, round_key, ciphertext);
+			next_key(round_key, round);
+		}
+	}
+
+}
+
+void construct_ctr_preload(
+	unsigned char *ctr_preload,
+	int a4_exists,
+	int qc_exists,
+	unsigned char *mpdu,
+	unsigned char *pn_vector,
+	int c)
+{
+
+	int i = 0;
+	for (i=0; i<16; i++) ctr_preload[i] = 0x00;
+	i = 0;
+
+	ctr_preload[0] = 0x01;                                  /* flag */
+	if (qc_exists && a4_exists) ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control  */
+	if (qc_exists && !a4_exists) ctr_preload[1] = mpdu[24] & 0x0f;
+
+	for (i = 2; i < 8; i++)
+		ctr_preload[i] = mpdu[i + 8];                       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
+#ifdef CONSISTENT_PN_ORDER
+	  for (i = 8; i < 14; i++)
+			ctr_preload[i] =    pn_vector[i - 8];           /* ctr_preload[8:13] = PN[0:5] */
+#else
+	  for (i = 8; i < 14; i++)
+			ctr_preload[i] =    pn_vector[13 - i];          /* ctr_preload[8:13] = PN[5:0] */
+#endif
+	ctr_preload[14] =  (unsigned char) (c / 256); // Ctr
+	ctr_preload[15] =  (unsigned char) (c % 256);
+
+}
+
+
+//
+// TRUE: Success!
+// FALSE: Decrypt Error!
+//
+BOOLEAN RTMPSoftDecryptTKIP(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN UCHAR    UserPriority,
+	IN PCIPHER_KEY	pWpaKey)
+{
+	UCHAR			KeyID;
+	UINT			HeaderLen;
+    UCHAR			fc0;
+	UCHAR			fc1;
+	USHORT			fc;
+	UINT			frame_type;
+	UINT			frame_subtype;
+    UINT			from_ds;
+    UINT			to_ds;
+	INT				a4_exists;
+	INT				qc_exists;
+	USHORT			duration;
+	USHORT			seq_control;
+	USHORT			qos_control;
+	UCHAR			TA[MAC_ADDR_LEN];
+	UCHAR			DA[MAC_ADDR_LEN];
+	UCHAR			SA[MAC_ADDR_LEN];
+	UCHAR			RC4Key[16];
+	UINT			p1k[5]; //for mix_key;
+	ULONG			pnl;/* Least significant 16 bits of PN */
+	ULONG			pnh;/* Most significant 32 bits of PN */
+	UINT			num_blocks;
+	UINT			payload_remainder;
+	ARCFOURCONTEXT 	ArcFourContext;
+	UINT			crc32 = 0;
+	UINT			trailfcs = 0;
+	UCHAR			MIC[8];
+	UCHAR			TrailMIC[8];
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+	fc0 = *pData;
+	fc1 = *(pData + 1);
+
+	fc = *((PUSHORT)pData);
+
+	frame_type = ((fc0 >> 2) & 0x03);
+	frame_subtype = ((fc0 >> 4) & 0x0f);
+
+    from_ds = (fc1 & 0x2) >> 1;
+    to_ds = (fc1 & 0x1);
+
+    a4_exists = (from_ds & to_ds);
+    qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
+                  (frame_subtype == 0x09) ||   /* Likely to change.    */
+                  (frame_subtype == 0x0a) ||
+                  (frame_subtype == 0x0b)
+                 );
+
+	HeaderLen = 24;
+	if (a4_exists)
+		HeaderLen += 6;
+
+	KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
+	KeyID = KeyID >> 6;
+
+	if (pWpaKey[KeyID].KeyLen == 0)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID));
+		return FALSE;
+	}
+
+	duration = *((PUSHORT)(pData+2));
+
+	seq_control = *((PUSHORT)(pData+22));
+
+	if (qc_exists)
+	{
+		if (a4_exists)
+		{
+			qos_control = *((PUSHORT)(pData+30));
+		}
+		else
+		{
+			qos_control = *((PUSHORT)(pData+24));
+		}
+	}
+
+	if (to_ds == 0 && from_ds == 1)
+	{
+		NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
+		NdisMoveMemory(SA, pData+16, MAC_ADDR_LEN);
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);  //BSSID
+	}
+	else if (to_ds == 0 && from_ds == 0 )
+	{
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
+		NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
+	}
+	else if (to_ds == 1 && from_ds == 0)
+	{
+		NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
+	}
+	else if (to_ds == 1 && from_ds == 1)
+	{
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
+		NdisMoveMemory(SA, pData+22, MAC_ADDR_LEN);
+	}
+
+	num_blocks = (DataByteCnt - 16) / 16;
+	payload_remainder = (DataByteCnt - 16) % 16;
+
+	pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
+	pnh = *((PULONG)(pData + HeaderLen + 4));
+	pnh = cpu2le32(pnh);
+	RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
+
+	ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
+
+	ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
+	NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
+	crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4);  //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
+	crc32 ^= 0xffffffff;             /* complement */
+
+    if(crc32 != cpu2le32(trailfcs))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP, WEP Data ICV Error !\n"));	 //ICV error.
+
+		return (FALSE);
+	}
+
+	NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
+	RTMPInitMICEngine(pAd, pWpaKey[KeyID].Key, DA, SA, UserPriority, pWpaKey[KeyID].RxMic);
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
+	NdisMoveMemory(MIC, pAd->PrivateInfo.Tx.MIC, 8);
+
+	if (!NdisEqualMemory(MIC, TrailMIC, 8))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptTKIP, WEP Data MIC Error !\n"));	 //MIC error.
+		//RTMPReportMicError(pAd, &pWpaKey[KeyID]);	// marked by AlbertY @ 20060630
+		return (FALSE);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+	//DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP Decript done!!\n");
+	return TRUE;
+}
+
+
+
+
+BOOLEAN RTMPSoftDecryptAES(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN PCIPHER_KEY	pWpaKey)
+{
+	UCHAR			KeyID;
+	UINT			HeaderLen;
+	UCHAR			PN[6];
+	UINT			payload_len;
+	UINT			num_blocks;
+	UINT			payload_remainder;
+	USHORT			fc;
+	UCHAR			fc0;
+	UCHAR			fc1;
+	UINT			frame_type;
+	UINT			frame_subtype;
+	UINT			from_ds;
+	UINT			to_ds;
+	INT				a4_exists;
+	INT				qc_exists;
+	UCHAR			aes_out[16];
+	int 			payload_index;
+	UINT 			i;
+	UCHAR 			ctr_preload[16];
+	UCHAR 			chain_buffer[16];
+	UCHAR 			padded_buffer[16];
+	UCHAR 			mic_iv[16];
+	UCHAR 			mic_header1[16];
+	UCHAR 			mic_header2[16];
+	UCHAR			MIC[8];
+	UCHAR			TrailMIC[8];
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+	fc0 = *pData;
+	fc1 = *(pData + 1);
+
+	fc = *((PUSHORT)pData);
+
+	frame_type = ((fc0 >> 2) & 0x03);
+	frame_subtype = ((fc0 >> 4) & 0x0f);
+
+	from_ds = (fc1 & 0x2) >> 1;
+	to_ds = (fc1 & 0x1);
+
+	a4_exists = (from_ds & to_ds);
+	qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
+				  (frame_subtype == 0x09) ||   /* Likely to change.    */
+				  (frame_subtype == 0x0a) ||
+				  (frame_subtype == 0x0b)
+				 );
+
+	HeaderLen = 24;
+	if (a4_exists)
+		HeaderLen += 6;
+
+	KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
+	KeyID = KeyID >> 6;
+
+	if (pWpaKey[KeyID].KeyLen == 0)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptAES failed!(KeyID[%d] Length can not be 0)\n", KeyID));
+		return FALSE;
+	}
+
+	PN[0] = *(pData+ HeaderLen);
+	PN[1] = *(pData+ HeaderLen + 1);
+	PN[2] = *(pData+ HeaderLen + 4);
+	PN[3] = *(pData+ HeaderLen + 5);
+	PN[4] = *(pData+ HeaderLen + 6);
+	PN[5] = *(pData+ HeaderLen + 7);
+
+	payload_len = DataByteCnt - HeaderLen - 8 - 8;	// 8 bytes for CCMP header , 8 bytes for MIC
+	payload_remainder = (payload_len) % 16;
+	num_blocks = (payload_len) / 16;
+
+
+
+	// Find start of payload
+	payload_index = HeaderLen + 8; //IV+EIV
+
+	for (i=0; i< num_blocks; i++)
+	{
+		construct_ctr_preload(ctr_preload,
+								a4_exists,
+								qc_exists,
+								pData,
+								PN,
+								i+1 );
+
+		aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
+
+		bitwise_xor(aes_out, pData + payload_index, chain_buffer);
+		NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
+		payload_index += 16;
+	}
+
+	//
+	// If there is a short final block, then pad it
+	// encrypt it and copy the unpadded part back
+	//
+	if (payload_remainder > 0)
+	{
+		construct_ctr_preload(ctr_preload,
+								a4_exists,
+								qc_exists,
+								pData,
+								PN,
+								num_blocks + 1);
+
+		NdisZeroMemory(padded_buffer, 16);
+		NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
+
+		aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
+
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
+		payload_index += payload_remainder;
+	}
+
+	//
+	// Descrypt the MIC
+	//
+	construct_ctr_preload(ctr_preload,
+							a4_exists,
+							qc_exists,
+							pData,
+							PN,
+							0);
+	NdisZeroMemory(padded_buffer, 16);
+	NdisMoveMemory(padded_buffer, pData + payload_index, 8);
+
+	aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
+
+	bitwise_xor(aes_out, padded_buffer, chain_buffer);
+
+	NdisMoveMemory(TrailMIC, chain_buffer, 8);
+
+	//
+	// Calculate MIC
+	//
+
+	//Force the protected frame bit on
+	*(pData + 1) = *(pData + 1) | 0x40;
+
+	// Find start of payload
+	// Because the CCMP header has been removed
+	payload_index = HeaderLen;
+
+	construct_mic_iv(
+					mic_iv,
+					qc_exists,
+					a4_exists,
+					pData,
+					payload_len,
+					PN);
+
+	construct_mic_header1(
+						mic_header1,
+						HeaderLen,
+						pData);
+
+	construct_mic_header2(
+						mic_header2,
+						pData,
+						a4_exists,
+						qc_exists);
+
+	aes128k128d(pWpaKey[KeyID].Key, mic_iv, aes_out);
+	bitwise_xor(aes_out, mic_header1, chain_buffer);
+	aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+	bitwise_xor(aes_out, mic_header2, chain_buffer);
+	aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+
+	// iterate through each 16 byte payload block
+	for (i = 0; i < num_blocks; i++)
+	{
+		bitwise_xor(aes_out, pData + payload_index, chain_buffer);
+		payload_index += 16;
+		aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+	}
+
+	// Add on the final payload block if it needs padding
+	if (payload_remainder > 0)
+	{
+		NdisZeroMemory(padded_buffer, 16);
+		NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
+
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+	}
+
+	// aes_out contains padded mic, discard most significant
+	// 8 bytes to generate 64 bit MIC
+	for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];
+
+	if (!NdisEqualMemory(MIC, TrailMIC, 8))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptAES, MIC Error !\n"));	 //MIC error.
+		return FALSE;
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+	return TRUE;
+}
+
+/****************************************/
+/* aes128k128d()                        */
+/* Performs a 128 bit AES encrypt with  */
+/* 128 bit data.                        */
+/****************************************/
+VOID xor_128(
+	IN  PUCHAR  a,
+	IN  PUCHAR  b,
+	OUT PUCHAR  out)
+{
+	INT i;
+
+	for (i=0;i<16; i++)
+	{
+		out[i] = a[i] ^ b[i];
+	}
+}
+
+VOID next_key(
+	IN  PUCHAR  key,
+	IN  INT     round)
+{
+	UCHAR       rcon;
+	UCHAR       sbox_key[4];
+	UCHAR       rcon_table[12] =
+	{
+		0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
+		0x1b, 0x36, 0x36, 0x36
+	};
+
+	sbox_key[0] = RTMPCkipSbox(key[13]);
+	sbox_key[1] = RTMPCkipSbox(key[14]);
+	sbox_key[2] = RTMPCkipSbox(key[15]);
+	sbox_key[3] = RTMPCkipSbox(key[12]);
+
+	rcon = rcon_table[round];
+
+	xor_32(&key[0], sbox_key, &key[0]);
+	key[0] = key[0] ^ rcon;
+
+	xor_32(&key[4], &key[0], &key[4]);
+	xor_32(&key[8], &key[4], &key[8]);
+	xor_32(&key[12], &key[8], &key[12]);
+}
+
+VOID xor_32(
+	IN  PUCHAR  a,
+	IN  PUCHAR  b,
+	OUT PUCHAR  out)
+{
+	INT i;
+
+	for (i=0;i<4; i++)
+	{
+		out[i] = a[i] ^ b[i];
+	}
+}
+
+VOID byte_sub(
+	IN  PUCHAR  in,
+	OUT PUCHAR  out)
+{
+	INT i;
+
+	for (i=0; i< 16; i++)
+	{
+		out[i] = RTMPCkipSbox(in[i]);
+	}
+}
+
+UCHAR RTMPCkipSbox(
+	IN  UCHAR   a)
+{
+	return SboxTable[(int)a];
+}
+
+VOID shift_row(
+	IN  PUCHAR  in,
+	OUT PUCHAR  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];
+}
+
+VOID mix_column(
+	IN  PUCHAR  in,
+	OUT PUCHAR  out)
+{
+	INT         i;
+	UCHAR       add1b[4];
+	UCHAR       add1bf7[4];
+	UCHAR       rotl[4];
+	UCHAR       swap_halfs[4];
+	UCHAR       andf7[4];
+	UCHAR       rotr[4];
+	UCHAR       temp[4];
+	UCHAR       tempb[4];
+
+	for (i=0 ; i<4; i++)
+	{
+		if ((in[i] & 0x80)== 0x80)
+			add1b[i] = 0x1b;
+		else
+			add1b[i] = 0x00;
+	}
+
+	swap_halfs[0] = in[2];    /* Swap halfs */
+	swap_halfs[1] = in[3];
+	swap_halfs[2] = in[0];
+	swap_halfs[3] = in[1];
+
+	rotl[0] = in[3];        /* Rotate left 8 bits */
+	rotl[1] = in[0];
+	rotl[2] = in[1];
+	rotl[3] = in[2];
+
+	andf7[0] = in[0] & 0x7f;
+	andf7[1] = in[1] & 0x7f;
+	andf7[2] = in[2] & 0x7f;
+	andf7[3] = in[3] & 0x7f;
+
+	for (i = 3; i>0; i--)    /* logical shift left 1 bit */
+	{
+		andf7[i] = andf7[i] << 1;
+		if ((andf7[i-1] & 0x80) == 0x80)
+		{
+			andf7[i] = (andf7[i] | 0x01);
+		}
+	}
+	andf7[0] = andf7[0] << 1;
+	andf7[0] = andf7[0] & 0xfe;
+
+	xor_32(add1b, andf7, add1bf7);
+
+	xor_32(in, add1bf7, rotr);
+
+	temp[0] = rotr[0];         /* Rotate right 8 bits */
+	rotr[0] = rotr[1];
+	rotr[1] = rotr[2];
+	rotr[2] = rotr[3];
+	rotr[3] = temp[0];
+
+	xor_32(add1bf7, rotr, temp);
+	xor_32(swap_halfs, rotl,tempb);
+	xor_32(temp, tempb, out);
+}
+
diff --git a/drivers/staging/rt2870/common/rtmp_wep.c b/drivers/staging/rt2870/common/rtmp_wep.c
new file mode 100644
index 000000000000..62f9e58729d7
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtmp_wep.c
@@ -0,0 +1,508 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_wep.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Paul Wu		10-28-02		Initial
+*/
+
+#include "../rt_config.h"
+
+UINT FCSTAB_32[256] =
+{
+	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba,
+	0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
+	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
+	0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
+	0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de,
+	0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec,
+	0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
+	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
+	0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
+	0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940,
+	0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116,
+	0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
+	0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
+	0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
+	0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a,
+	0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818,
+	0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
+	0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
+	0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
+	0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c,
+	0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2,
+	0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
+	0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
+	0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
+	0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086,
+	0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4,
+	0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
+	0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
+	0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
+	0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8,
+	0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe,
+	0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
+	0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
+	0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
+	0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252,
+	0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60,
+	0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
+	0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
+	0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
+	0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04,
+	0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a,
+	0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
+	0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
+	0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
+	0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e,
+	0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c,
+	0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
+	0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
+	0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
+	0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0,
+	0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6,
+	0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
+	0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
+	0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
+};
+
+/*
+UCHAR   WEPKEY[] = {
+		//IV
+		0x00, 0x11, 0x22,
+		//WEP KEY
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xAA, 0xBB, 0xCC
+	};
+ */
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Init WEP function.
+
+	Arguments:
+      pAd		Pointer to our adapter
+		pKey        Pointer to the WEP KEY
+		KeyId		   WEP Key ID
+		KeyLen      the length of WEP KEY
+		pDest       Pointer to the destination which Encryption data will store in.
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitWepEngine(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pKey,
+	IN	UCHAR			KeyId,
+	IN	UCHAR			KeyLen,
+	IN OUT	PUCHAR		pDest)
+{
+	UINT i;
+	UCHAR   WEPKEY[] = {
+		//IV
+		0x00, 0x11, 0x22,
+		//WEP KEY
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xAA, 0xBB, 0xCC
+	};
+
+	pAd->PrivateInfo.FCSCRC32 = PPPINITFCS32;   //Init crc32.
+
+#ifdef CONFIG_STA_SUPPORT
+    if (pAd->StaCfg.bCkipOn && (pAd->StaCfg.CkipFlag & 0x10) && (pAd->OpMode == OPMODE_STA))
+    {
+        ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, pKey, KeyLen);  //INIT SBOX, KEYLEN+3(IV)
+        NdisMoveMemory(pDest, pKey, 3);  //Append Init Vector
+    }
+    else
+#endif // CONFIG_STA_SUPPORT //
+    {
+		NdisMoveMemory(WEPKEY + 3, pKey, KeyLen);
+
+        for(i = 0; i < 3; i++)
+			WEPKEY[i] = RandomByte(pAd);   //Call mlme RandomByte() function.
+		ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, WEPKEY, KeyLen + 3);  //INIT SBOX, KEYLEN+3(IV)
+
+		NdisMoveMemory(pDest, WEPKEY, 3);  //Append Init Vector
+    }
+	*(pDest+3) = (KeyId << 6);       //Append KEYID
+
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Encrypt transimitted data
+
+	Arguments:
+      pAd		Pointer to our adapter
+      pSrc        Pointer to the transimitted source data that will be encrypt
+      pDest       Pointer to the destination where entryption data will be store in.
+      Len			Indicate the length of the source data
+
+	Return Value:
+      None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPEncryptData(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pSrc,
+	IN	PUCHAR			pDest,
+	IN	UINT			Len)
+{
+	pAd->PrivateInfo.FCSCRC32 = RTMP_CALC_FCS32(pAd->PrivateInfo.FCSCRC32, pSrc, Len);
+	ARCFOUR_ENCRYPT(&pAd->PrivateInfo.WEPCONTEXT, pDest, pSrc, Len);
+}
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Decrypt received WEP data
+
+	Arguments:
+		pAdapter		Pointer to our adapter
+		pSrc        Pointer to the received data
+		Len         the length of the received data
+
+	Return Value:
+		TRUE        Decrypt WEP data success
+		FALSE       Decrypt WEP data failed
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN	RTMPSoftDecryptWEP(
+	IN PRTMP_ADAPTER 	pAd,
+	IN PUCHAR			pData,
+	IN ULONG			DataByteCnt,
+	IN PCIPHER_KEY		pGroupKey)
+{
+	UINT	trailfcs;
+	UINT    crc32;
+	UCHAR	KeyIdx;
+	UCHAR   WEPKEY[] = {
+		//IV
+		0x00, 0x11, 0x22,
+		//WEP KEY
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xAA, 0xBB, 0xCC
+	};
+	UCHAR 	*pPayload = (UCHAR *)pData + LENGTH_802_11;
+	ULONG	payload_len = DataByteCnt - LENGTH_802_11;
+
+	NdisMoveMemory(WEPKEY, pPayload, 3);    //Get WEP IV
+
+	KeyIdx = (*(pPayload + 3) & 0xc0) >> 6;
+	if (pGroupKey[KeyIdx].KeyLen == 0)
+		return (FALSE);
+
+	NdisMoveMemory(WEPKEY + 3, pGroupKey[KeyIdx].Key, pGroupKey[KeyIdx].KeyLen);
+	ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, WEPKEY, pGroupKey[KeyIdx].KeyLen + 3);
+	ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, pPayload, pPayload + 4, payload_len - 4);
+	NdisMoveMemory(&trailfcs, pPayload + payload_len - 8, 4);
+	crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pPayload, payload_len - 8);  //Skip last 4 bytes(FCS).
+	crc32 ^= 0xffffffff;             /* complement */
+
+    if(crc32 != cpu2le32(trailfcs))
+    {
+		DBGPRINT(RT_DEBUG_TRACE, ("! WEP Data CRC Error !\n"));	 //CRC error.
+		return (FALSE);
+	}
+	return (TRUE);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Encryption Algorithm "ARCFOUR" initialize
+
+	Arguments:
+	   Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pKey        Pointer to the WEP KEY
+		KeyLen      Indicate the length fo the WEP KEY
+
+	Return Value:
+	   None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	ARCFOUR_INIT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pKey,
+	IN	UINT			KeyLen)
+{
+	UCHAR	t, u;
+	UINT	keyindex;
+	UINT	stateindex;
+	PUCHAR	state;
+	UINT	counter;
+
+	state = Ctx->STATE;
+	Ctx->X = 0;
+	Ctx->Y = 0;
+	for (counter = 0; counter < 256; counter++)
+		state[counter] = (UCHAR)counter;
+	keyindex = 0;
+	stateindex = 0;
+	for (counter = 0; counter < 256; counter++)
+	{
+		t = state[counter];
+		stateindex = (stateindex + pKey[keyindex] + t) & 0xff;
+		u = state[stateindex];
+		state[stateindex] = t;
+		state[counter] = u;
+		if (++keyindex >= KeyLen)
+			keyindex = 0;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Get bytes from ARCFOUR CONTEXT (S-BOX)
+
+	Arguments:
+	   Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+
+	Return Value:
+	   UCHAR  - the value of the ARCFOUR CONTEXT (S-BOX)
+
+	Note:
+
+	========================================================================
+*/
+UCHAR	ARCFOUR_BYTE(
+	IN	PARCFOURCONTEXT		Ctx)
+{
+  UINT x;
+  UINT y;
+  UCHAR sx, sy;
+  PUCHAR state;
+
+  state = Ctx->STATE;
+  x = (Ctx->X + 1) & 0xff;
+  sx = state[x];
+  y = (sx + Ctx->Y) & 0xff;
+  sy = state[y];
+  Ctx->X = x;
+  Ctx->Y = y;
+  state[y] = sx;
+  state[x] = sy;
+
+  return(state[(sx + sy) & 0xff]);
+
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Decryption Algorithm
+
+	Arguments:
+		Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pDest			Pointer to the Destination
+		pSrc        Pointer to the Source data
+		Len         Indicate the length of the Source data
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ARCFOUR_DECRYPT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pDest,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len)
+{
+	UINT i;
+
+	for (i = 0; i < Len; i++)
+		pDest[i] = pSrc[i] ^ ARCFOUR_BYTE(Ctx);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Encryption Algorithm
+
+	Arguments:
+		Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pDest			Pointer to the Destination
+		pSrc        Pointer to the Source data
+		Len         Indicate the length of the Source dta
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	ARCFOUR_ENCRYPT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pDest,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len)
+{
+	UINT i;
+
+	for (i = 0; i < Len; i++)
+		pDest[i] = pSrc[i] ^ ARCFOUR_BYTE(Ctx);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Encryption Algorithm which conform to the special requirement to encrypt  GTK.
+
+	Arguments:
+		Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pDest			Pointer to the Destination
+		pSrc        Pointer to the Source data
+		Len         Indicate the length of the Source dta
+
+
+	========================================================================
+*/
+
+VOID	WPAARCFOUR_ENCRYPT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pDest,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len)
+{
+	UINT i;
+        //discard first 256 bytes
+	for (i = 0; i < 256; i++)
+            ARCFOUR_BYTE(Ctx);
+
+	for (i = 0; i < Len; i++)
+		pDest[i] = pSrc[i] ^ ARCFOUR_BYTE(Ctx);
+}
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Calculate a new FCS given the current FCS and the new data.
+
+	Arguments:
+		Fcs	      the original FCS value
+		Cp          pointer to the data which will be calculate the FCS
+		Len         the length of the data
+
+	Return Value:
+		UINT - FCS 32 bits
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+UINT	RTMP_CALC_FCS32(
+	IN	UINT	Fcs,
+	IN	PUCHAR	Cp,
+	IN	INT		Len)
+{
+	while (Len--)
+	   Fcs = (((Fcs) >> 8) ^ FCSTAB_32[((Fcs) ^ (*Cp++)) & 0xff]);
+
+	return (Fcs);
+}
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Get last FCS and encrypt it to the destination
+
+	Arguments:
+		pDest			Pointer to the Destination
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPSetICV(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR	pDest)
+{
+	pAd->PrivateInfo.FCSCRC32 ^= 0xffffffff;             /* complement */
+	pAd->PrivateInfo.FCSCRC32 = cpu2le32(pAd->PrivateInfo.FCSCRC32);
+
+	ARCFOUR_ENCRYPT(&pAd->PrivateInfo.WEPCONTEXT, pDest, (PUCHAR) &pAd->PrivateInfo.FCSCRC32, 4);
+}
+
diff --git a/drivers/staging/rt2870/common/rtusb_bulk.c b/drivers/staging/rt2870/common/rtusb_bulk.c
new file mode 100644
index 000000000000..c46d9166ccf4
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtusb_bulk.c
@@ -0,0 +1,1981 @@
+ /*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtusb_bulk.c
+
+	Abstract:
+
+	Revision History:
+	Who			When		What
+	--------	----------	----------------------------------------------
+	Name		Date		Modification logs
+	Paul Lin	06-25-2004	created
+
+*/
+
+#include "../rt_config.h"
+// Match total 6 bulkout endpoint to corresponding queue.
+UCHAR	EpToQueue[6]={FIFO_EDCA, FIFO_EDCA, FIFO_EDCA, FIFO_EDCA, FIFO_EDCA, FIFO_MGMT};
+
+//static BOOLEAN SingleBulkOut = FALSE;
+
+void RTUSB_FILL_BULK_URB (struct urb *pUrb,
+	struct usb_device *pUsb_Dev,
+	unsigned int bulkpipe,
+	void *pTransferBuf,
+	int BufSize,
+	usb_complete_t Complete,
+	void *pContext)
+{
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	usb_fill_bulk_urb(pUrb, pUsb_Dev, bulkpipe, pTransferBuf, BufSize, (usb_complete_t)Complete, pContext);
+#else
+	FILL_BULK_URB(pUrb, pUsb_Dev, bulkpipe, pTransferBuf, BufSize, Complete, pContext);
+#endif
+
+}
+
+VOID	RTUSBInitTxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTX_CONTEXT		pTxContext,
+	IN	UCHAR			BulkOutPipeId,
+	IN	usb_complete_t	Func)
+{
+	PURB				pUrb;
+	PUCHAR				pSrc = NULL;
+	POS_COOKIE			pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	pUrb = pTxContext->pUrb;
+	ASSERT(pUrb);
+
+	// Store BulkOut PipeId
+	pTxContext->BulkOutPipeId = BulkOutPipeId;
+
+	if (pTxContext->bAggregatible)
+	{
+		pSrc = &pTxContext->TransferBuffer->Aggregation[2];
+	}
+	else
+	{
+		pSrc = (PUCHAR) pTxContext->TransferBuffer->field.WirelessPacket;
+	}
+
+
+	//Initialize a tx bulk urb
+	RTUSB_FILL_BULK_URB(pUrb,
+						pObj->pUsb_Dev,
+						usb_sndbulkpipe(pObj->pUsb_Dev, pAd->BulkOutEpAddr[BulkOutPipeId]),
+						pSrc,
+						pTxContext->BulkOutSize,
+						Func,
+						pTxContext);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	if (pTxContext->bAggregatible)
+		pUrb->transfer_dma	= (pTxContext->data_dma + TX_BUFFER_NORMSIZE + 2);
+	else
+		pUrb->transfer_dma	= pTxContext->data_dma;
+
+	pUrb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+#endif
+
+}
+
+VOID	RTUSBInitHTTxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PHT_TX_CONTEXT	pTxContext,
+	IN	UCHAR			BulkOutPipeId,
+	IN	ULONG			BulkOutSize,
+	IN	usb_complete_t	Func)
+{
+	PURB				pUrb;
+	PUCHAR				pSrc = NULL;
+	POS_COOKIE			pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	pUrb = pTxContext->pUrb;
+	ASSERT(pUrb);
+
+	// Store BulkOut PipeId
+	pTxContext->BulkOutPipeId = BulkOutPipeId;
+
+	pSrc = &pTxContext->TransferBuffer->field.WirelessPacket[pTxContext->NextBulkOutPosition];
+
+
+	//Initialize a tx bulk urb
+	RTUSB_FILL_BULK_URB(pUrb,
+						pObj->pUsb_Dev,
+						usb_sndbulkpipe(pObj->pUsb_Dev, pAd->BulkOutEpAddr[BulkOutPipeId]),
+						pSrc,
+						BulkOutSize,
+						Func,
+						pTxContext);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	pUrb->transfer_dma	= (pTxContext->data_dma + pTxContext->NextBulkOutPosition);
+	pUrb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+#endif
+
+}
+
+VOID	RTUSBInitRxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PRX_CONTEXT		pRxContext)
+{
+	PURB				pUrb;
+	POS_COOKIE			pObj = (POS_COOKIE) pAd->OS_Cookie;
+	ULONG				RX_bulk_size;
+
+
+	pUrb = pRxContext->pUrb;
+	ASSERT(pUrb);
+
+	if ( pAd->BulkInMaxPacketSize == 64)
+		RX_bulk_size = 4096;
+	else
+		RX_bulk_size = MAX_RXBULK_SIZE;
+
+	//Initialize a rx bulk urb
+	RTUSB_FILL_BULK_URB(pUrb,
+						pObj->pUsb_Dev,
+						usb_rcvbulkpipe(pObj->pUsb_Dev, pAd->BulkInEpAddr),
+						&(pRxContext->TransferBuffer[pAd->NextRxBulkInPosition]),
+						RX_bulk_size - (pAd->NextRxBulkInPosition),
+						(usb_complete_t)RTUSBBulkRxComplete,
+						(void *)pRxContext);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	pUrb->transfer_dma	= pRxContext->data_dma + pAd->NextRxBulkInPosition;
+	pUrb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+#endif
+
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+
+#define BULK_OUT_LOCK(pLock, IrqFlags)	\
+		if(1 /*!(in_interrupt() & 0xffff0000)*/)	\
+			RTMP_IRQ_LOCK((pLock), IrqFlags);
+
+#define BULK_OUT_UNLOCK(pLock, IrqFlags)	\
+		if(1 /*!(in_interrupt() & 0xffff0000)*/)	\
+			RTMP_IRQ_UNLOCK((pLock), IrqFlags);
+
+
+VOID	RTUSBBulkOutDataPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BulkOutPipeId,
+	IN	UCHAR			Index)
+{
+
+	PHT_TX_CONTEXT	pHTTXContext;
+	PURB			pUrb;
+	int				ret = 0;
+	PTXINFO_STRUC	pTxInfo, pLastTxInfo = NULL;
+	PTXWI_STRUC             pTxWI;
+	ULONG			TmpBulkEndPos, ThisBulkSize;
+	unsigned long	IrqFlags = 0, IrqFlags2 = 0;
+	PUCHAR			pWirelessPkt, pAppendant;
+	BOOLEAN			bTxQLastRound = FALSE;
+	UCHAR			allzero[4]= {0x0,0x0,0x0,0x0};
+
+	BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	if ((pAd->BulkOutPending[BulkOutPipeId] == TRUE) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_TX))
+	{
+		BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		return;
+	}
+	pAd->BulkOutPending[BulkOutPipeId] = TRUE;
+
+	if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+		)
+	{
+		pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+		BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		return;
+	}
+	BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+
+	pHTTXContext = &(pAd->TxContext[BulkOutPipeId]);
+
+	BULK_OUT_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags2);
+	if ((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition)
+		|| ((pHTTXContext->ENextBulkOutPosition-8) == pHTTXContext->CurWritePosition))
+	{
+		BULK_OUT_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags2);
+
+		BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+
+		// Clear Data flag
+		RTUSB_CLEAR_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_FRAG << BulkOutPipeId));
+		RTUSB_CLEAR_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+
+		BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		return;
+	}
+
+	// Clear Data flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_FRAG << BulkOutPipeId));
+	RTUSB_CLEAR_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+
+	//DBGPRINT(RT_DEBUG_TRACE,("BulkOut-B:I=0x%lx, CWPos=%ld, CWRPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d!\n", in_interrupt(),
+	//							pHTTXContext->CurWritePosition, pHTTXContext->CurWriteRealPos, pHTTXContext->NextBulkOutPosition,
+	//							pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad));
+	pHTTXContext->NextBulkOutPosition = pHTTXContext->ENextBulkOutPosition;
+	ThisBulkSize = 0;
+	TmpBulkEndPos = pHTTXContext->NextBulkOutPosition;
+	pWirelessPkt = &pHTTXContext->TransferBuffer->field.WirelessPacket[0];
+
+	if ((pHTTXContext->bCopySavePad == TRUE))
+	{
+		if (RTMPEqualMemory(pHTTXContext->SavedPad, allzero,4))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("e1, allzero : %x  %x  %x  %x  %x  %x  %x  %x \n",
+				pHTTXContext->SavedPad[0], pHTTXContext->SavedPad[1], pHTTXContext->SavedPad[2],pHTTXContext->SavedPad[3]
+				,pHTTXContext->SavedPad[4], pHTTXContext->SavedPad[5], pHTTXContext->SavedPad[6],pHTTXContext->SavedPad[7]));
+		}
+		NdisMoveMemory(&pWirelessPkt[TmpBulkEndPos], pHTTXContext->SavedPad, 8);
+		pHTTXContext->bCopySavePad = FALSE;
+		if (pAd->bForcePrintTX == TRUE)
+			DBGPRINT(RT_DEBUG_TRACE,("RTUSBBulkOutDataPacket --> COPY PAD. CurWrite = %ld, NextBulk = %ld.   ENextBulk = %ld.\n",   pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition));
+	}
+
+	do
+	{
+		pTxInfo = (PTXINFO_STRUC)&pWirelessPkt[TmpBulkEndPos];
+		pTxWI = (PTXWI_STRUC)&pWirelessPkt[TmpBulkEndPos + TXINFO_SIZE];
+
+		if (pAd->bForcePrintTX == TRUE)
+			DBGPRINT(RT_DEBUG_TRACE, ("RTUSBBulkOutDataPacket AMPDU = %d.\n",   pTxWI->AMPDU));
+
+		// add by Iverson, limit BulkOut size to 4k to pass WMM b mode 2T1R test items
+		//if ((ThisBulkSize != 0)  && (pTxWI->AMPDU == 0))
+		if ((ThisBulkSize != 0) && (pTxWI->PHYMODE == MODE_CCK))
+		{
+			if (((ThisBulkSize&0xffff8000) != 0) || ((ThisBulkSize&0x1000) == 0x1000))
+			{
+				// Limit BulkOut size to about 4k bytes.
+				pHTTXContext->ENextBulkOutPosition = TmpBulkEndPos;
+				break;
+			}
+			else if (((pAd->BulkOutMaxPacketSize < 512) && ((ThisBulkSize&0xfffff800) != 0) ) /*|| ( (ThisBulkSize != 0)  && (pTxWI->AMPDU == 0))*/)
+			{
+				// For USB 1.1 or peer which didn't support AMPDU, limit the BulkOut size.
+				// For performence in b/g mode, now just check for USB 1.1 and didn't care about the APMDU or not! 2008/06/04.
+				pHTTXContext->ENextBulkOutPosition = TmpBulkEndPos;
+				break;
+			}
+		}
+		// end Iverson
+		else
+		{
+			if (((ThisBulkSize&0xffff8000) != 0) || ((ThisBulkSize&0x6000) == 0x6000))
+			{	// Limit BulkOut size to about 24k bytes.
+				pHTTXContext->ENextBulkOutPosition = TmpBulkEndPos;
+				break;
+			}
+			else if (((pAd->BulkOutMaxPacketSize < 512) && ((ThisBulkSize&0xfffff800) != 0) ) /*|| ( (ThisBulkSize != 0)  && (pTxWI->AMPDU == 0))*/)
+			{	// For USB 1.1 or peer which didn't support AMPDU, limit the BulkOut size.
+				// For performence in b/g mode, now just check for USB 1.1 and didn't care about the APMDU or not! 2008/06/04.
+				pHTTXContext->ENextBulkOutPosition = TmpBulkEndPos;
+				break;
+			}
+		}
+
+		if (TmpBulkEndPos == pHTTXContext->CurWritePosition)
+		{
+			pHTTXContext->ENextBulkOutPosition = TmpBulkEndPos;
+			break;
+		}
+
+		if (pTxInfo->QSEL != FIFO_EDCA)
+		{
+			printk("%s(): ====> pTxInfo->QueueSel(%d)!= FIFO_EDCA!!!!\n", __func__, pTxInfo->QSEL);
+			printk("\tCWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d!\n", pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad);
+			hex_dump("Wrong QSel Pkt:", (PUCHAR)&pWirelessPkt[TmpBulkEndPos], (pHTTXContext->CurWritePosition - pHTTXContext->NextBulkOutPosition));
+		}
+
+		if (pTxInfo->USBDMATxPktLen <= 8)
+		{
+			BULK_OUT_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags2);
+			DBGPRINT(RT_DEBUG_ERROR /*RT_DEBUG_TRACE*/,("e2, USBDMATxPktLen==0, Size=%ld, bCSPad=%d, CWPos=%ld, NBPos=%ld, CWRPos=%ld!\n",
+					pHTTXContext->BulkOutSize, pHTTXContext->bCopySavePad, pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition, pHTTXContext->CurWriteRealPos));
+			{
+				DBGPRINT_RAW(RT_DEBUG_ERROR /*RT_DEBUG_TRACE*/,("%x  %x  %x  %x  %x  %x  %x  %x \n",
+					pHTTXContext->SavedPad[0], pHTTXContext->SavedPad[1], pHTTXContext->SavedPad[2],pHTTXContext->SavedPad[3]
+					,pHTTXContext->SavedPad[4], pHTTXContext->SavedPad[5], pHTTXContext->SavedPad[6],pHTTXContext->SavedPad[7]));
+			}
+			pAd->bForcePrintTX = TRUE;
+			BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+			pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+			BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+			//DBGPRINT(RT_DEBUG_LOUD,("Out:pTxInfo->USBDMATxPktLen=%d!\n", pTxInfo->USBDMATxPktLen));
+			return;
+		}
+
+			// Increase Total transmit byte counter
+		pAd->RalinkCounters.OneSecTransmittedByteCount +=  pTxWI->MPDUtotalByteCount;
+		pAd->RalinkCounters.TransmittedByteCount +=  pTxWI->MPDUtotalByteCount;
+
+		pLastTxInfo = pTxInfo;
+
+		// Make sure we use EDCA QUEUE.
+		pTxInfo->QSEL = FIFO_EDCA;
+		ThisBulkSize += (pTxInfo->USBDMATxPktLen+4);
+		TmpBulkEndPos += (pTxInfo->USBDMATxPktLen+4);
+
+		if (TmpBulkEndPos != pHTTXContext->CurWritePosition)
+			pTxInfo->USBDMANextVLD = 1;
+
+		if (pTxInfo->SwUseLastRound == 1)
+		{
+			if (pHTTXContext->CurWritePosition == 8)
+				pTxInfo->USBDMANextVLD = 0;
+			pTxInfo->SwUseLastRound = 0;
+
+			bTxQLastRound = TRUE;
+			pHTTXContext->ENextBulkOutPosition = 8;
+
+	#ifdef RT_BIG_ENDIAN
+			RTMPDescriptorEndianChange((PUCHAR)pTxInfo, TYPE_TXINFO);
+			RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+	#endif // RT_BIG_ENDIAN //
+
+			break;
+		}
+
+#ifdef RT_BIG_ENDIAN
+		RTMPDescriptorEndianChange((PUCHAR)pTxInfo, TYPE_TXINFO);
+		RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+#endif // RT_BIG_ENDIAN //
+
+	}while (TRUE);
+
+	// adjust the pTxInfo->USBDMANextVLD value of last pTxInfo.
+	if (pLastTxInfo)
+	{
+#ifdef RT_BIG_ENDIAN
+		RTMPDescriptorEndianChange((PUCHAR)pLastTxInfo, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+		pLastTxInfo->USBDMANextVLD = 0;
+#ifdef RT_BIG_ENDIAN
+		RTMPDescriptorEndianChange((PUCHAR)pLastTxInfo, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+	}
+
+	/*
+		We need to copy SavedPad when following condition matched!
+			1. Not the last round of the TxQueue and
+			2. any match of following cases:
+				(1). The End Position of this bulk out is reach to the Currenct Write position and
+						the TxInfo and related header already write to the CurWritePosition.
+			   		=>(ENextBulkOutPosition == CurWritePosition) && (CurWriteRealPos > CurWritePosition)
+
+				(2). The EndPosition of the bulk out is not reach to the Current Write Position.
+					=>(ENextBulkOutPosition != CurWritePosition)
+	*/
+	if ((bTxQLastRound == FALSE) &&
+		 (((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition) && (pHTTXContext->CurWriteRealPos > pHTTXContext->CurWritePosition)) ||
+		  (pHTTXContext->ENextBulkOutPosition != pHTTXContext->CurWritePosition))
+		)
+	{
+		NdisMoveMemory(pHTTXContext->SavedPad, &pWirelessPkt[pHTTXContext->ENextBulkOutPosition], 8);
+		pHTTXContext->bCopySavePad = TRUE;
+		if (RTMPEqualMemory(pHTTXContext->SavedPad, allzero,4))
+		{
+			PUCHAR	pBuf = &pHTTXContext->SavedPad[0];
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("WARNING-Zero-3:%02x%02x%02x%02x%02x%02x%02x%02x,CWPos=%ld, CWRPos=%ld, bCW=%d, NBPos=%ld, TBPos=%ld, TBSize=%ld\n",
+				pBuf[0], pBuf[1], pBuf[2],pBuf[3],pBuf[4], pBuf[5], pBuf[6],pBuf[7], pHTTXContext->CurWritePosition, pHTTXContext->CurWriteRealPos,
+				pHTTXContext->bCurWriting, pHTTXContext->NextBulkOutPosition, TmpBulkEndPos, ThisBulkSize));
+
+			pBuf = &pWirelessPkt[pHTTXContext->CurWritePosition];
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("\tCWPos=%02x%02x%02x%02x%02x%02x%02x%02x\n", pBuf[0], pBuf[1], pBuf[2],pBuf[3],pBuf[4], pBuf[5], pBuf[6],pBuf[7]));
+		}
+		//DBGPRINT(RT_DEBUG_LOUD,("ENPos==CWPos=%ld, CWRPos=%ld, bCSPad=%d!\n", pHTTXContext->CurWritePosition, pHTTXContext->CurWriteRealPos, pHTTXContext->bCopySavePad));
+	}
+
+	if (pAd->bForcePrintTX == TRUE)
+		DBGPRINT(RT_DEBUG_TRACE,("BulkOut-A:Size=%ld, CWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d!\n", ThisBulkSize, pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad));
+	//DBGPRINT(RT_DEBUG_LOUD,("BulkOut-A:Size=%ld, CWPos=%ld, CWRPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d, bLRound=%d!\n", ThisBulkSize, pHTTXContext->CurWritePosition, pHTTXContext->CurWriteRealPos, pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad, bTxQLastRound));
+
+		// USB DMA engine requires to pad extra 4 bytes. This pad doesn't count into real bulkoutsize.
+	pAppendant = &pWirelessPkt[TmpBulkEndPos];
+	NdisZeroMemory(pAppendant, 8);
+		ThisBulkSize += 4;
+		pHTTXContext->LastOne = TRUE;
+		if ((ThisBulkSize % pAd->BulkOutMaxPacketSize) == 0)
+			ThisBulkSize += 4;
+	pHTTXContext->BulkOutSize = ThisBulkSize;
+
+	pAd->watchDogTxPendingCnt[BulkOutPipeId] = 1;
+	BULK_OUT_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags2);
+
+	// Init Tx context descriptor
+	RTUSBInitHTTxDesc(pAd, pHTTXContext, BulkOutPipeId, ThisBulkSize, (usb_complete_t)RTUSBBulkOutDataPacketComplete);
+
+	pUrb = pHTTXContext->pUrb;
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkOutDataPacket: Submit Tx URB failed %d\n", ret));
+
+		BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+		pAd->watchDogTxPendingCnt[BulkOutPipeId] = 0;
+		BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+		return;
+	}
+
+	BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	pHTTXContext->IRPPending = TRUE;
+	BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	pAd->BulkOutReq++;
+
+}
+
+
+VOID RTUSBBulkOutDataPacketComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+#if 0 // sample, IRQ LOCK
+	PRTMP_ADAPTER		pAd;
+	POS_COOKIE			pObj;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId;
+	NTSTATUS			Status;
+	unsigned long		IrqFlags;
+
+	DBGPRINT_RAW(RT_DEBUG_INFO, ("--->RTUSBBulkOutDataPacketComplete\n"));
+
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd			= pHTTXContext->pAd;
+	pObj 		= (POS_COOKIE) pAd->OS_Cookie;
+	Status		= pUrb->status;
+
+	// Store BulkOut PipeId
+	BulkOutPipeId = pHTTXContext->BulkOutPipeId;
+	pAd->BulkOutDataOneSecCount++;
+
+	//DBGPRINT(RT_DEBUG_LOUD, ("Done-B(%d):I=0x%lx, CWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d!\n", BulkOutPipeId, in_interrupt(), pHTTXContext->CurWritePosition,
+	//		pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad));
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+	pHTTXContext->IRPPending = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+	if (Status == USB_ST_NOERROR)
+	{
+		pAd->BulkOutComplete++;
+
+		pAd->Counters8023.GoodTransmits++;
+		//RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+		FREE_HTTX_RING(pAd, BulkOutPipeId, pHTTXContext);
+		//RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+
+	}
+	else	// STATUS_OTHER
+	{
+		PUCHAR	pBuf;
+
+		pAd->BulkOutCompleteOther++;
+
+		pBuf = &pHTTXContext->TransferBuffer->WirelessPacket[pHTTXContext->NextBulkOutPosition];
+
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("BulkOutDataPacket failed: ReasonCode=%d!\n", Status));
+		DBGPRINT_RAW(RT_DEBUG_ERROR, (">>BulkOut Req=0x%lx, Complete=0x%lx, Other=0x%lx\n", pAd->BulkOutReq, pAd->BulkOutComplete, pAd->BulkOutCompleteOther));
+		DBGPRINT_RAW(RT_DEBUG_ERROR, (">>BulkOut Header:%x %x %x %x %x %x %x %x\n", pBuf[0], pBuf[1], pBuf[2], pBuf[3], pBuf[4], pBuf[5], pBuf[6], pBuf[7]));
+		//DBGPRINT_RAW(RT_DEBUG_ERROR, (">>BulkOutCompleteCancel=0x%x, BulkOutCompleteOther=0x%x\n", pAd->BulkOutCompleteCancel, pAd->BulkOutCompleteOther));
+
+		if (!RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+									fRTMP_ADAPTER_HALT_IN_PROGRESS |
+									fRTMP_ADAPTER_NIC_NOT_EXIST |
+									fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = BulkOutPipeId;
+		}
+	}
+
+	//
+	// bInUse = TRUE, means some process are filling TX data, after that must turn on bWaitingBulkOut
+	// bWaitingBulkOut = TRUE, means the TX data are waiting for bulk out.
+	//
+	//RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+	if ((pHTTXContext->ENextBulkOutPosition != pHTTXContext->CurWritePosition) &&
+		(pHTTXContext->ENextBulkOutPosition != (pHTTXContext->CurWritePosition+8)) &&
+		!RTUSB_TEST_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_FRAG << BulkOutPipeId)))
+	{
+		// Indicate There is data avaliable
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+	}
+	//RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protection of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+
+
+	//DBGPRINT(RT_DEBUG_LOUD,("Done-A(%d):I=0x%lx, CWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d\n", BulkOutPipeId, in_interrupt(),
+	//		pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad));
+
+	switch (BulkOutPipeId)
+	{
+		case 0:
+				pObj->ac0_dma_done_task.data = (unsigned long)pAd;
+				tasklet_hi_schedule(&pObj->ac0_dma_done_task);
+				break;
+		case 1:
+				pObj->ac1_dma_done_task.data = (unsigned long)pAd;
+				tasklet_hi_schedule(&pObj->ac1_dma_done_task);
+				break;
+		case 2:
+				pObj->ac2_dma_done_task.data = (unsigned long)pAd;
+				tasklet_hi_schedule(&pObj->ac2_dma_done_task);
+				break;
+		case 3:
+				pObj->ac3_dma_done_task.data = (unsigned long)pAd;
+				tasklet_hi_schedule(&pObj->ac3_dma_done_task);
+				break;
+		case 4:
+				pObj->hcca_dma_done_task.data = (unsigned long)pAd;
+				tasklet_hi_schedule(&pObj->hcca_dma_done_task);
+				break;
+	}
+#else
+
+{
+	PHT_TX_CONTEXT	pHTTXContext;
+	PRTMP_ADAPTER	pAd;
+	POS_COOKIE 		pObj;
+	UCHAR			BulkOutPipeId;
+
+
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd 			= pHTTXContext->pAd;
+	pObj 			= (POS_COOKIE) pAd->OS_Cookie;
+
+	// Store BulkOut PipeId
+	BulkOutPipeId	= pHTTXContext->BulkOutPipeId;
+	pAd->BulkOutDataOneSecCount++;
+
+	switch (BulkOutPipeId)
+	{
+		case 0:
+				pObj->ac0_dma_done_task.data = (unsigned long)pUrb;
+				tasklet_hi_schedule(&pObj->ac0_dma_done_task);
+				break;
+		case 1:
+				pObj->ac1_dma_done_task.data = (unsigned long)pUrb;
+				tasklet_hi_schedule(&pObj->ac1_dma_done_task);
+				break;
+		case 2:
+				pObj->ac2_dma_done_task.data = (unsigned long)pUrb;
+				tasklet_hi_schedule(&pObj->ac2_dma_done_task);
+				break;
+		case 3:
+				pObj->ac3_dma_done_task.data = (unsigned long)pUrb;
+				tasklet_hi_schedule(&pObj->ac3_dma_done_task);
+				break;
+		case 4:
+				pObj->hcca_dma_done_task.data = (unsigned long)pUrb;
+				tasklet_hi_schedule(&pObj->hcca_dma_done_task);
+				break;
+	}
+}
+#endif
+
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note: NULL frame use BulkOutPipeId = 0
+
+	========================================================================
+*/
+VOID	RTUSBBulkOutNullFrame(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PTX_CONTEXT		pNullContext = &(pAd->NullContext);
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+	if ((pAd->BulkOutPending[0] == TRUE) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_TX))
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+		return;
+	}
+	pAd->BulkOutPending[0] = TRUE;
+	pAd->watchDogTxPendingCnt[0] = 1;
+	pNullContext->IRPPending = TRUE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+	// Increase Total transmit byte counter
+	pAd->RalinkCounters.TransmittedByteCount +=  pNullContext->BulkOutSize;
+
+
+	// Clear Null frame bulk flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NULL);
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pNullContext->TransferBuffer, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+
+	// Init Tx context descriptor
+	RTUSBInitTxDesc(pAd, pNullContext, 0, (usb_complete_t)RTUSBBulkOutNullFrameComplete);
+
+	pUrb = pNullContext->pUrb;
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+		pAd->BulkOutPending[0] = FALSE;
+		pAd->watchDogTxPendingCnt[0] = 0;
+		pNullContext->IRPPending = FALSE;
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkOutNullFrame: Submit Tx URB failed %d\n", ret));
+		return;
+	}
+
+}
+
+// NULL frame use BulkOutPipeId = 0
+VOID RTUSBBulkOutNullFrameComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+	PRTMP_ADAPTER		pAd;
+	PTX_CONTEXT			pNullContext;
+	NTSTATUS			Status;
+#if 0 // sample, IRQ LOCK
+	unsigned long		IrqFlags;
+#endif
+	POS_COOKIE			pObj;
+
+
+	pNullContext	= (PTX_CONTEXT)pUrb->context;
+	pAd 			= pNullContext->pAd;
+	Status 			= pUrb->status;
+
+#if 0 // sample, IRQ LOCK
+	// Reset Null frame context flags
+	pNullContext->IRPPending 	= FALSE;
+	pNullContext->InUse 		= FALSE;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		// Don't worry about the queue is empty or not, this function will check itself
+		//RTMPUSBDeQueuePacket(pAd, 0);
+		RTMPDeQueuePacket(pAd, TRUE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else	// STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out Null Frame Failed\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+	}
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+	pAd->BulkOutPending[0] = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+#else
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+	pObj->null_frame_complete_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->null_frame_complete_task);
+#endif
+
+}
+
+#if 0	// For RT2870, RTS frame not used now, but maybe will use it latter.
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note: RTS frame use BulkOutPipeId = 0
+
+	========================================================================
+*/
+VOID	RTUSBBulkOutRTSFrame(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PTX_CONTEXT		pRTSContext = &(pAd->RTSContext);
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+	UCHAR			PipeID=0;
+
+	if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_4))
+		PipeID= 3;
+	else if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_3))
+		PipeID= 2;
+	else if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_2))
+		PipeID= 1;
+	else if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL))
+		PipeID= 0;
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[PipeID], IrqFlags);
+	if ((pAd->BulkOutPending[PipeID] == TRUE) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_TX))
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[PipeID], IrqFlags);
+		return;
+	}
+	pAd->BulkOutPending[PipeID] = TRUE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[PipeID], IrqFlags);
+
+	// Increase Total transmit byte counter
+	pAd->RalinkCounters.TransmittedByteCount +=  pRTSContext->BulkOutSize;
+
+	DBGPRINT_RAW(RT_DEBUG_INFO, ("--->RTUSBBulkOutRTSFrame \n"));
+
+	// Clear RTS frame bulk flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_RTS);
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pRTSContext->TransferBuffer, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+
+	// Init Tx context descriptor
+	RTUSBInitTxDesc(pAd, pRTSContext, PipeID, (usb_complete_t)RTUSBBulkOutRTSFrameComplete);
+	pRTSContext->IRPPending = TRUE;
+
+	pUrb = pRTSContext->pUrb;
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkOutRTSFrame: Submit Tx URB failed %d\n", ret));
+		return;
+	}
+
+	DBGPRINT_RAW(RT_DEBUG_INFO, ("<---RTUSBBulkOutRTSFrame \n"));
+
+}
+
+// RTS frame use BulkOutPipeId = 0
+VOID RTUSBBulkOutRTSFrameComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+	PRTMP_ADAPTER	pAd;
+	PTX_CONTEXT		pRTSContext;
+	NTSTATUS		Status;
+#if 0 // sample, IRQ LOCK
+	unsigned long	IrqFlags;
+#endif
+	POS_COOKIE		pObj;
+
+	DBGPRINT_RAW(RT_DEBUG_INFO, ("--->RTUSBBulkOutRTSFrameComplete\n"));
+
+	pRTSContext	= (PTX_CONTEXT)pUrb->context;
+	pAd			= pRTSContext->pAd;
+	Status		= pUrb->status;
+
+#if 0 // sample, IRQ LOCK
+	// Reset RTS frame context flags
+	pRTSContext->IRPPending = FALSE;
+	pRTSContext->InUse		= FALSE;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		// Don't worry about the queue is empty or not, this function will check itself
+		//RTMPUSBDeQueuePacket(pAd, pRTSContext->BulkOutPipeId);
+		RTMPDeQueuePacket(pAd, TRUE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else	// STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out RTS Frame Failed\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+	}
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[pRTSContext->BulkOutPipeId], IrqFlags);
+	pAd->BulkOutPending[pRTSContext->BulkOutPipeId] = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[pRTSContext->BulkOutPipeId], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+#else
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+	pObj->rts_frame_complete_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->rts_frame_complete_task);
+#endif
+
+	DBGPRINT_RAW(RT_DEBUG_INFO, ("<---RTUSBBulkOutRTSFrameComplete\n"));
+
+}
+#endif
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note: MLME use BulkOutPipeId = 0
+
+	========================================================================
+*/
+VOID	RTUSBBulkOutMLMEPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PTX_CONTEXT		pMLMEContext;
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+	pMLMEContext = (PTX_CONTEXT)pAd->MgmtRing.Cell[pAd->MgmtRing.TxDmaIdx].AllocVa;
+	pUrb = pMLMEContext->pUrb;
+
+	if ((pAd->MgmtRing.TxSwFreeIdx >= MGMT_RING_SIZE) ||
+		(pMLMEContext->InUse == FALSE) ||
+		(pMLMEContext->bWaitingBulkOut == FALSE))
+	{
+
+
+		// Clear MLME bulk flag
+		RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME);
+
+		return;
+	}
+
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+	if ((pAd->BulkOutPending[MGMTPIPEIDX] == TRUE) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_TX))
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+		return;
+	}
+
+	pAd->BulkOutPending[MGMTPIPEIDX] = TRUE;
+	pAd->watchDogTxPendingCnt[MGMTPIPEIDX] = 1;
+	pMLMEContext->IRPPending = TRUE;
+	pMLMEContext->bWaitingBulkOut = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+	// Increase Total transmit byte counter
+	pAd->RalinkCounters.TransmittedByteCount +=  pMLMEContext->BulkOutSize;
+
+	// Clear MLME bulk flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME);
+
+
+	//DBGPRINT_RAW(RT_DEBUG_INFO, ("--->RTUSBBulkOutMLMEPacket\n"));
+#if 0 // for debug
+{
+	printk("MLME-Out, C=%d!, D=%d, F=%d!\n", pAd->MgmtRing.TxCpuIdx, pAd->MgmtRing.TxDmaIdx, pAd->MgmtRing.TxSwFreeIdx);
+
+	//TODO: Need to remove it when formal release
+	PTXINFO_STRUC pTxInfo;
+
+	pTxInfo = (PTXINFO_STRUC)pMLMEContext->TransferBuffer;
+	if (pTxInfo->QSEL != FIFO_EDCA)
+	{
+			printk("%s(): ====> pTxInfo->QueueSel(%d)!= FIFO_EDCA!!!!\n", __func__, pTxInfo->QSEL);
+			printk("\tMLME_Index=%d!\n", Index);
+			hex_dump("Wrong QSel Pkt:", (PUCHAR)pMLMEContext->TransferBuffer, pTxInfo->USBDMATxPktLen);
+	}
+}
+#endif
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pMLMEContext->TransferBuffer, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+
+	// Init Tx context descriptor
+	RTUSBInitTxDesc(pAd, pMLMEContext, MGMTPIPEIDX, (usb_complete_t)RTUSBBulkOutMLMEPacketComplete);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	//For mgmt urb buffer, because we use sk_buff, so we need to notify the USB controller do dma mapping.
+	pUrb->transfer_dma	= 0;
+	pUrb->transfer_flags &= (~URB_NO_TRANSFER_DMA_MAP);
+#endif
+
+	pUrb = pMLMEContext->pUrb;
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkOutMLMEPacket: Submit MLME URB failed %d\n", ret));
+		RTMP_IRQ_LOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+		pAd->BulkOutPending[MGMTPIPEIDX] = FALSE;
+		pAd->watchDogTxPendingCnt[MGMTPIPEIDX] = 0;
+		pMLMEContext->IRPPending = FALSE;
+		pMLMEContext->bWaitingBulkOut = TRUE;
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+		return;
+	}
+
+	//DBGPRINT_RAW(RT_DEBUG_INFO, ("<---RTUSBBulkOutMLMEPacket \n"));
+//	printk("<---RTUSBBulkOutMLMEPacket,Cpu=%d!, Dma=%d, SwIdx=%d!\n", pAd->MgmtRing.TxCpuIdx, pAd->MgmtRing.TxDmaIdx, pAd->MgmtRing.TxSwFreeIdx);
+}
+
+
+VOID RTUSBBulkOutMLMEPacketComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+	PTX_CONTEXT			pMLMEContext;
+	PRTMP_ADAPTER		pAd;
+	NTSTATUS			Status;
+	POS_COOKIE 			pObj;
+	int					index;
+#if 0 // sample, IRQ LOCK
+	unsigned long		IrqFlags;
+	PNDIS_PACKET		pPacket;
+#endif
+
+
+	//DBGPRINT_RAW(RT_DEBUG_INFO, ("--->RTUSBBulkOutMLMEPacketComplete\n"));
+	pMLMEContext	= (PTX_CONTEXT)pUrb->context;
+	pAd 			= pMLMEContext->pAd;
+	pObj 			= (POS_COOKIE)pAd->OS_Cookie;
+	Status			= pUrb->status;
+	index 			= pMLMEContext->SelfIdx;
+
+
+#if 0 // sample, IRQ LOCK
+	ASSERT((pAd->MgmtRing.TxDmaIdx == index));
+	//printk("MLME-Done-B: C=%d, D=%d, F=%d, Self=%d!\n", pAd->MgmtRing.TxCpuIdx, pAd->MgmtRing.TxDmaIdx, pAd->MgmtRing.TxSwFreeIdx, pMLMEContext->SelfIdx);
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+
+	if (Status != USB_ST_NOERROR)
+	{
+		//Bulk-Out fail status handle
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out MLME Failed, Status=%d!\n", Status));
+			// TODO: How to handle about the MLMEBulkOut failed issue. Need to resend the mgmt pkt?
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+		}
+	}
+	pAd->BulkOutPending[MGMTPIPEIDX] = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+	RTMP_IRQ_LOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+	// Reset MLME context flags
+	pMLMEContext->IRPPending = FALSE;
+	pMLMEContext->InUse = FALSE;
+	pMLMEContext->bWaitingBulkOut = FALSE;
+	pMLMEContext->BulkOutSize = 0;
+
+	pPacket = pAd->MgmtRing.Cell[index].pNdisPacket;
+	pAd->MgmtRing.Cell[index].pNdisPacket = NULL;
+
+	// Increase MgmtRing Index
+	INC_RING_INDEX(pAd->MgmtRing.TxDmaIdx, MGMT_RING_SIZE);
+	pAd->MgmtRing.TxSwFreeIdx++;
+
+	RTMP_IRQ_UNLOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+
+	// No-matter success or fail, we free the mgmt packet.
+	if (pPacket)
+		RTMPFreeNdisPacket(pAd, pPacket);
+
+#if 0
+	//Bulk-Out fail status handle
+	if (Status != USB_ST_NOERROR)
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out MLME Failed, Status=%d!\n", Status));
+			// TODO: How to handle about the MLMEBulkOut failed issue. Need to reset the endpoint?
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+		}
+	}
+#endif
+
+	//printk("MLME-Done-A: C=%d, D=%d, F=%d!\n", pAd->MgmtRing.TxCpuIdx, pAd->MgmtRing.TxDmaIdx, pAd->MgmtRing.TxSwFreeIdx);
+
+	pObj->mgmt_dma_done_task.data = (unsigned long)pAd;
+	tasklet_hi_schedule(&pObj->mgmt_dma_done_task);
+
+	//DBGPRINT_RAW(RT_DEBUG_INFO, ("<---RTUSBBulkOutMLMEPacketComplete\n"));
+//	printk("<---RTUSBBulkOutMLMEPacketComplete, Cpu=%d, Dma=%d, SwIdx=%d!\n",
+//				pAd->MgmtRing.TxCpuIdx, pAd->MgmtRing.TxDmaIdx, pAd->MgmtRing.TxSwFreeIdx);
+
+#else
+
+	pObj->mgmt_dma_done_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->mgmt_dma_done_task);
+#endif
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note: PsPoll use BulkOutPipeId = 0
+
+	========================================================================
+*/
+VOID	RTUSBBulkOutPsPoll(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PTX_CONTEXT		pPsPollContext = &(pAd->PsPollContext);
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+	if ((pAd->BulkOutPending[0] == TRUE) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_TX))
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+		return;
+	}
+	pAd->BulkOutPending[0] = TRUE;
+	pAd->watchDogTxPendingCnt[0] = 1;
+	pPsPollContext->IRPPending = TRUE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+
+	// Clear PS-Poll bulk flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_PSPOLL);
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pPsPollContext->TransferBuffer, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+
+	// Init Tx context descriptor
+	RTUSBInitTxDesc(pAd, pPsPollContext, MGMTPIPEIDX, (usb_complete_t)RTUSBBulkOutPsPollComplete);
+
+	pUrb = pPsPollContext->pUrb;
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+		pAd->BulkOutPending[0] = FALSE;
+		pAd->watchDogTxPendingCnt[0] = 0;
+		pPsPollContext->IRPPending = FALSE;
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkOutPsPoll: Submit Tx URB failed %d\n", ret));
+		return;
+	}
+
+}
+
+// PS-Poll frame use BulkOutPipeId = 0
+VOID RTUSBBulkOutPsPollComplete(purbb_t pUrb,struct pt_regs *pt_regs)
+{
+	PRTMP_ADAPTER		pAd;
+	PTX_CONTEXT			pPsPollContext;
+	NTSTATUS			Status;
+#if 0 // sample, IRQ LOCK
+	unsigned long		IrqFlags;
+#endif
+	POS_COOKIE			pObj;
+
+
+	pPsPollContext= (PTX_CONTEXT)pUrb->context;
+	pAd = pPsPollContext->pAd;
+	Status = pUrb->status;
+
+#if 0 // sample, IRQ LOCK
+	// Reset PsPoll context flags
+	pPsPollContext->IRPPending	= FALSE;
+	pPsPollContext->InUse		= FALSE;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		// Don't worry about the queue is empty or not, this function will check itself
+		RTMPDeQueuePacket(pAd, TRUE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else // STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out PSPoll Failed\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+	}
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+	pAd->BulkOutPending[0] = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+#else
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+	pObj->pspoll_frame_complete_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->pspoll_frame_complete_task);
+#endif
+}
+
+
+#if 0
+/*
+	========================================================================
+
+	Routine Description:
+	USB_RxPacket initializes a URB and uses the Rx IRP to submit it
+	to USB. It checks if an Rx Descriptor is available and passes the
+	the coresponding buffer to be filled. If no descriptor is available
+	fails the request. When setting the completion routine we pass our
+	Adapter Object as Context.
+
+	Arguments:
+
+	Return Value:
+		TRUE			found matched tuple cache
+		FALSE			no matched found
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBBulkReceive(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PRX_CONTEXT		pRxContext;
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+
+	/* device had been closed */
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_REMOVE_IN_PROGRESS))
+		return;
+
+	RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+
+	// Last is time point between 2 separate URB.
+	if (pAd->NextRxBulkInPosition == 0)
+	{
+		//pAd->NextRxBulkInIndex = (pAd->NextRxBulkInIndex + 1) % (RX_RING_SIZE);
+		INC_RING_INDEX(pAd->NextRxBulkInIndex, RX_RING_SIZE);
+	}
+	else if ((pAd->NextRxBulkInPosition&0x1ff) != 0)
+	{
+		//pAd->NextRxBulkInIndex = (pAd->NextRxBulkInIndex + 1) % (RX_RING_SIZE);
+		INC_RING_INDEX(pAd->NextRxBulkInIndex, RX_RING_SIZE);
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("pAd->NextRxBulkInPosition = 0x%lx.  End of URB.\n", pAd->NextRxBulkInPosition ));
+		pAd->NextRxBulkInPosition = 0;
+	}
+
+	if (pAd->NextRxBulkInPosition == MAX_RXBULK_SIZE)
+		pAd->NextRxBulkInPosition = 0;
+
+	pRxContext = &(pAd->RxContext[pAd->NextRxBulkInIndex]);
+
+	// TODO: Why need to check if pRxContext->InUsed == TRUE?
+	//if ((pRxContext->InUse == TRUE) || (pRxContext->Readable == TRUE))
+	if ((pRxContext->InUse == FALSE) && (pRxContext->Readable == TRUE))
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("pRxContext[%d] InUse = %d.pRxContext->Readable = %d.  Return.\n", pAd->NextRxBulkInIndex,pRxContext->InUse, pRxContext->Readable ));
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+		// read RxContext, Since not
+#ifdef CONFIG_STA_SUPPORT
+		STARxDoneInterruptHandle(pAd, TRUE);
+#endif // CONFIG_STA_SUPPORT //
+
+		//return;
+	}
+	pRxContext->InUse = TRUE;
+	pRxContext->IRPPending= TRUE;
+
+	RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+	// Init Rx context descriptor
+	NdisZeroMemory(pRxContext->TransferBuffer, BUFFER_SIZE);
+	RTUSBInitRxDesc(pAd, pRxContext);
+
+	pUrb = pRxContext->pUrb;
+	if ((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkReceive: Submit Rx URB failed %d\n", ret));
+		return;
+	}
+	else // success
+	{
+		NdisInterlockedIncrement(&pAd->PendingRx);
+		pAd->BulkInReq++;
+	}
+
+	// read RxContext, Since not
+#ifdef CONFIG_STA_SUPPORT
+	STARxDoneInterruptHandle(pAd, FALSE);
+#endif // CONFIG_STA_SUPPORT //
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		This routine process Rx Irp and call rx complete function.
+
+	Arguments:
+		DeviceObject	Pointer to the device object for next lower
+						device. DeviceObject passed in here belongs to
+						the next lower driver in the stack because we
+						were invoked via IoCallDriver in USB_RxPacket
+						AND it is not OUR device object
+	  Irp				Ptr to completed IRP
+	  Context			Ptr to our Adapter object (context specified
+						in IoSetCompletionRoutine
+
+	Return Value:
+		Always returns STATUS_MORE_PROCESSING_REQUIRED
+
+	Note:
+		Always returns STATUS_MORE_PROCESSING_REQUIRED
+	========================================================================
+*/
+VOID RTUSBBulkRxComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+#if 0
+	PRX_CONTEXT			pRxContext;
+	PRTMP_ADAPTER		pAd;
+	NTSTATUS			Status;
+//	POS_COOKIE			pObj;
+
+	pRxContext	= (PRX_CONTEXT)pUrb->context;
+	pAd 		= pRxContext->pAd;
+//	pObj		= (POS_COOKIE) pAd->OS_Cookie;
+
+
+	Status = pUrb->status;
+	//pRxContext->pIrp = NULL;
+
+	pRxContext->InUse = FALSE;
+	pRxContext->IRPPending = FALSE;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		pAd->BulkInComplete++;
+		pRxContext->Readable = TRUE;
+		pAd->NextRxBulkInPosition = 0;
+
+	}
+	else	 // STATUS_OTHER
+	{
+		pAd->BulkInCompleteFail++;
+		// Still read this packet although it may comtain wrong bytes.
+		pRxContext->Readable = FALSE;
+		// Parsing all packets. because after reset, the index will reset to all zero.
+
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+		{
+
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk In Failed. Status = %d\n", Status));
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("==>NextRxBulkInIndex=0x%x, NextRxBulkInReadIndex=0x%x, TransferBufferLength= 0x%x\n",
+				pAd->NextRxBulkInIndex, pAd->NextRxBulkInReadIndex, pRxContext->pUrb->actual_length));
+
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_IN, NULL, 0);
+		}
+		//pUrb = NULL;
+	}
+
+	if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET)) &&
+//		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)) &&
+		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF)) &&
+		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		RTUSBBulkReceive(pAd);
+#if 0
+#if 1
+		STARxDoneInterruptHandle(pAd, FALSE);
+#else
+		pObj->rx_bh.data = (unsigned long)pUrb;
+		tasklet_schedule(&pObj->rx_bh);
+#endif
+#endif
+	}
+
+	// Call RxPacket to process packet and return the status
+	NdisInterlockedDecrement(&pAd->PendingRx);
+#else
+
+
+	// use a receive tasklet to handle received packets;
+	// or sometimes hardware IRQ will be disabled here, so we can not
+	// use spin_lock_bh()/spin_unlock_bh() after IRQ is disabled. :<
+	PRX_CONTEXT		pRxContext;
+	PRTMP_ADAPTER	pAd;
+	POS_COOKIE 		pObj;
+
+
+	pRxContext	= (PRX_CONTEXT)pUrb->context;
+	pAd 		= pRxContext->pAd;
+	pObj 		= (POS_COOKIE) pAd->OS_Cookie;
+
+	pObj->rx_done_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->rx_done_task);
+#endif
+}
+
+#else
+
+VOID DoBulkIn(IN RTMP_ADAPTER *pAd)
+{
+	PRX_CONTEXT		pRxContext;
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+	RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+	pRxContext = &(pAd->RxContext[pAd->NextRxBulkInIndex]);
+	if ((pAd->PendingRx > 0) || (pRxContext->Readable == TRUE) || (pRxContext->InUse == TRUE))
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+		return;
+	}
+	pRxContext->InUse = TRUE;
+	pRxContext->IRPPending = TRUE;
+	pAd->PendingRx++;
+	pAd->BulkInReq++;
+	RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+	// Init Rx context descriptor
+	NdisZeroMemory(pRxContext->TransferBuffer, pRxContext->BulkInOffset);
+	RTUSBInitRxDesc(pAd, pRxContext);
+
+	pUrb = pRxContext->pUrb;
+	if ((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{	// fail
+
+		RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+		pRxContext->InUse = FALSE;
+		pRxContext->IRPPending = FALSE;
+		pAd->PendingRx--;
+		pAd->BulkInReq--;
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkReceive: Submit Rx URB failed %d\n", ret));
+	}
+	else
+	{	// success
+#if 0
+		RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+		pRxContext->IRPPending = TRUE;
+		//NdisInterlockedIncrement(&pAd->PendingRx);
+		pAd->PendingRx++;
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+		pAd->BulkInReq++;
+#endif
+		ASSERT((pRxContext->InUse == pRxContext->IRPPending));
+		//printk("BIDone, Pend=%d,BIIdx=%d,BIRIdx=%d!\n", pAd->PendingRx, pAd->NextRxBulkInIndex, pAd->NextRxBulkInReadIndex);
+	}
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+	USB_RxPacket initializes a URB and uses the Rx IRP to submit it
+	to USB. It checks if an Rx Descriptor is available and passes the
+	the coresponding buffer to be filled. If no descriptor is available
+	fails the request. When setting the completion routine we pass our
+	Adapter Object as Context.
+
+	Arguments:
+
+	Return Value:
+		TRUE			found matched tuple cache
+		FALSE			no matched found
+
+	Note:
+
+	========================================================================
+*/
+#define fRTMP_ADAPTER_NEED_STOP_RX		\
+		(fRTMP_ADAPTER_NIC_NOT_EXIST | fRTMP_ADAPTER_HALT_IN_PROGRESS |	\
+		 fRTMP_ADAPTER_RADIO_OFF | fRTMP_ADAPTER_RESET_IN_PROGRESS | \
+		 fRTMP_ADAPTER_REMOVE_IN_PROGRESS | fRTMP_ADAPTER_BULKIN_RESET)
+
+#define fRTMP_ADAPTER_NEED_STOP_HANDLE_RX	\
+		(fRTMP_ADAPTER_NIC_NOT_EXIST | fRTMP_ADAPTER_HALT_IN_PROGRESS |	\
+		 fRTMP_ADAPTER_RADIO_OFF | fRTMP_ADAPTER_RESET_IN_PROGRESS | \
+		 fRTMP_ADAPTER_REMOVE_IN_PROGRESS)
+
+VOID	RTUSBBulkReceive(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PRX_CONTEXT		pRxContext;
+	unsigned long	IrqFlags;
+
+
+	/* sanity check */
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_HANDLE_RX))
+		return;
+
+	while(1)
+	{
+
+		RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+		pRxContext = &(pAd->RxContext[pAd->NextRxBulkInReadIndex]);
+		if (((pRxContext->InUse == FALSE) && (pRxContext->Readable == TRUE)) &&
+			(pRxContext->bRxHandling == FALSE))
+		{
+			pRxContext->bRxHandling = TRUE;
+			RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+			// read RxContext, Since not
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				STARxDoneInterruptHandle(pAd, TRUE);
+#endif // CONFIG_STA_SUPPORT //
+
+			// Finish to handle this bulkIn buffer.
+			RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+			pRxContext->BulkInOffset = 0;
+			pRxContext->Readable = FALSE;
+			pRxContext->bRxHandling = FALSE;
+			pAd->ReadPosition = 0;
+			pAd->TransferBufferLength = 0;
+			INC_RING_INDEX(pAd->NextRxBulkInReadIndex, RX_RING_SIZE);
+			RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+			break;
+		}
+	}
+
+	if (!(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_RX)))
+		DoBulkIn(pAd);
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		This routine process Rx Irp and call rx complete function.
+
+	Arguments:
+		DeviceObject	Pointer to the device object for next lower
+						device. DeviceObject passed in here belongs to
+						the next lower driver in the stack because we
+						were invoked via IoCallDriver in USB_RxPacket
+						AND it is not OUR device object
+	  Irp				Ptr to completed IRP
+	  Context			Ptr to our Adapter object (context specified
+						in IoSetCompletionRoutine
+
+	Return Value:
+		Always returns STATUS_MORE_PROCESSING_REQUIRED
+
+	Note:
+		Always returns STATUS_MORE_PROCESSING_REQUIRED
+	========================================================================
+*/
+VOID RTUSBBulkRxComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+	// use a receive tasklet to handle received packets;
+	// or sometimes hardware IRQ will be disabled here, so we can not
+	// use spin_lock_bh()/spin_unlock_bh() after IRQ is disabled. :<
+	PRX_CONTEXT		pRxContext;
+	PRTMP_ADAPTER	pAd;
+	POS_COOKIE 		pObj;
+
+
+	pRxContext	= (PRX_CONTEXT)pUrb->context;
+	pAd 		= pRxContext->pAd;
+	pObj 		= (POS_COOKIE) pAd->OS_Cookie;
+
+	pObj->rx_done_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->rx_done_task);
+
+}
+
+#endif
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBKickBulkOut(
+	IN	PRTMP_ADAPTER pAd)
+{
+	// BulkIn Reset will reset whole USB PHY. So we need to make sure fRTMP_ADAPTER_BULKIN_RESET not flaged.
+	if (!RTMP_TEST_FLAG(pAd ,fRTMP_ADAPTER_NEED_STOP_TX)
+#ifdef RALINK_ATE
+		&& !(ATE_ON(pAd))
+#endif // RALINK_ATE //
+		)
+	{
+#if 0	// not used now in RT28xx, but may used latter.
+		// 1. Data Fragment has highest priority
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_FRAG))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 0, pAd->NextBulkOutIndex[0]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_FRAG_2))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 1, pAd->NextBulkOutIndex[1]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_FRAG_3))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 2, pAd->NextBulkOutIndex[2]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_FRAG_4))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 3, pAd->NextBulkOutIndex[3]);
+			}
+		}
+#endif
+
+		// 2. PS-Poll frame is next
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_PSPOLL))
+		{
+			RTUSBBulkOutPsPoll(pAd);
+		}
+
+		// 5. Mlme frame is next
+		else if ((RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME)) &&
+				 (pAd->MgmtRing.TxSwFreeIdx < MGMT_RING_SIZE))
+		{
+			RTUSBBulkOutMLMEPacket(pAd, pAd->MgmtRing.TxDmaIdx);
+		}
+
+		// 6. Data frame normal is next
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 0, pAd->NextBulkOutIndex[0]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_2))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 1, pAd->NextBulkOutIndex[1]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_3))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 2, pAd->NextBulkOutIndex[2]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_4))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 3, pAd->NextBulkOutIndex[3]);
+			}
+		}
+
+		// 7. Null frame is the last
+		else if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NULL))
+		{
+			if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			{
+				RTUSBBulkOutNullFrame(pAd);
+			}
+		}
+
+		// 8. No data avaliable
+		else
+		{
+
+		}
+	}
+#ifdef RALINK_ATE
+	/* If the mode is in ATE mode. */
+	else if((ATE_ON(pAd)) &&
+		!RTMP_TEST_FLAG(pAd ,fRTMP_ADAPTER_NEED_STOP_TX))// PETER : watch out !
+	{
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE))
+		{
+			ATE_RTUSBBulkOutDataPacket(pAd, 0);
+		}
+	}
+#endif // RALINK_ATE //
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+	Call from Reset action after BulkOut failed.
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBCleanUpDataBulkOutQueue(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR			Idx;
+	PHT_TX_CONTEXT	pTxContext;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->CleanUpDataBulkOutQueue\n"));
+
+	for (Idx = 0; Idx < 4; Idx++)
+	{
+		pTxContext = &pAd->TxContext[Idx];
+
+		pTxContext->CurWritePosition = pTxContext->NextBulkOutPosition;
+		pTxContext->LastOne = FALSE;
+		NdisAcquireSpinLock(&pAd->BulkOutLock[Idx]);
+		pAd->BulkOutPending[Idx] = FALSE;
+		NdisReleaseSpinLock(&pAd->BulkOutLock[Idx]);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<---CleanUpDataBulkOutQueue\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBCleanUpMLMEBulkOutQueue(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("--->CleanUpMLMEBulkOutQueue\n"));
+
+#if 0	// Do nothing!
+	NdisAcquireSpinLock(&pAd->MLMEBulkOutLock);
+	while (pAd->PrioRingTxCnt > 0)
+	{
+		pAd->MLMEContext[pAd->PrioRingFirstIndex].InUse = FALSE;
+
+		pAd->PrioRingFirstIndex++;
+		if (pAd->PrioRingFirstIndex >= MGMT_RING_SIZE)
+		{
+			pAd->PrioRingFirstIndex = 0;
+		}
+
+		pAd->PrioRingTxCnt--;
+	}
+	NdisReleaseSpinLock(&pAd->MLMEBulkOutLock);
+#endif
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<---CleanUpMLMEBulkOutQueue\n"));
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBCancelPendingIRPs(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	RTUSBCancelPendingBulkInIRP(pAd);
+	RTUSBCancelPendingBulkOutIRP(pAd);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBCancelPendingBulkInIRP(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PRX_CONTEXT		pRxContext;
+	UINT			i;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("--->RTUSBCancelPendingBulkInIRP\n"));
+	for ( i = 0; i < (RX_RING_SIZE); i++)
+	{
+		pRxContext = &(pAd->RxContext[i]);
+		if(pRxContext->IRPPending == TRUE)
+		{
+			RTUSB_UNLINK_URB(pRxContext->pUrb);
+			pRxContext->IRPPending = FALSE;
+			pRxContext->InUse = FALSE;
+			//NdisInterlockedDecrement(&pAd->PendingRx);
+			//pAd->PendingRx--;
+		}
+	}
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("<---RTUSBCancelPendingBulkInIRP\n"));
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBCancelPendingBulkOutIRP(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PHT_TX_CONTEXT		pHTTXContext;
+	PTX_CONTEXT			pMLMEContext;
+	PTX_CONTEXT			pBeaconContext;
+	PTX_CONTEXT			pNullContext;
+	PTX_CONTEXT			pPsPollContext;
+	PTX_CONTEXT			pRTSContext;
+	UINT				i, Idx;
+//	unsigned int 		IrqFlags;
+//	NDIS_SPIN_LOCK		*pLock;
+//	BOOLEAN				*pPending;
+
+
+//	pLock = &pAd->BulkOutLock[MGMTPIPEIDX];
+//	pPending = &pAd->BulkOutPending[MGMTPIPEIDX];
+
+	for (Idx = 0; Idx < 4; Idx++)
+	{
+		pHTTXContext = &(pAd->TxContext[Idx]);
+
+		if (pHTTXContext->IRPPending == TRUE)
+		{
+
+			// Get the USB_CONTEXT and cancel it's IRP; the completion routine will itself
+			// remove it from the HeadPendingSendList and NULL out HeadPendingSendList
+			//	when the last IRP on the list has been	cancelled; that's how we exit this loop
+			//
+
+			RTUSB_UNLINK_URB(pHTTXContext->pUrb);
+
+			// Sleep 200 microseconds to give cancellation time to work
+			RTMPusecDelay(200);
+		}
+
+#ifdef RALINK_ATE
+		pHTTXContext->bCopySavePad = 0;
+		pHTTXContext->CurWritePosition = 0;
+		pHTTXContext->CurWriteRealPos = 0;
+		pHTTXContext->bCurWriting = FALSE;
+		pHTTXContext->NextBulkOutPosition = 0;
+		pHTTXContext->ENextBulkOutPosition = 0;
+#endif // RALINK_ATE //
+		pAd->BulkOutPending[Idx] = FALSE;
+	}
+
+	//RTMP_IRQ_LOCK(pLock, IrqFlags);
+	for (i = 0; i < MGMT_RING_SIZE; i++)
+	{
+		pMLMEContext = (PTX_CONTEXT)pAd->MgmtRing.Cell[i].AllocVa;
+		if(pMLMEContext && (pMLMEContext->IRPPending == TRUE))
+		{
+
+			// Get the USB_CONTEXT and cancel it's IRP; the completion routine will itself
+			// remove it from the HeadPendingSendList and NULL out HeadPendingSendList
+			//	when the last IRP on the list has been	cancelled; that's how we exit this loop
+			//
+
+			RTUSB_UNLINK_URB(pMLMEContext->pUrb);
+			pMLMEContext->IRPPending = FALSE;
+
+			// Sleep 200 microsecs to give cancellation time to work
+			RTMPusecDelay(200);
+		}
+	}
+	pAd->BulkOutPending[MGMTPIPEIDX] = FALSE;
+	//RTMP_IRQ_UNLOCK(pLock, IrqFlags);
+
+
+	for (i = 0; i < BEACON_RING_SIZE; i++)
+	{
+		pBeaconContext = &(pAd->BeaconContext[i]);
+
+		if(pBeaconContext->IRPPending == TRUE)
+		{
+
+			// Get the USB_CONTEXT and cancel it's IRP; the completion routine will itself
+			// remove it from the HeadPendingSendList and NULL out HeadPendingSendList
+			//	when the last IRP on the list has been	cancelled; that's how we exit this loop
+			//
+
+			RTUSB_UNLINK_URB(pBeaconContext->pUrb);
+
+			// Sleep 200 microsecs to give cancellation time to work
+			RTMPusecDelay(200);
+		}
+	}
+
+	pNullContext = &(pAd->NullContext);
+	if (pNullContext->IRPPending == TRUE)
+		RTUSB_UNLINK_URB(pNullContext->pUrb);
+
+	pRTSContext = &(pAd->RTSContext);
+	if (pRTSContext->IRPPending == TRUE)
+		RTUSB_UNLINK_URB(pRTSContext->pUrb);
+
+	pPsPollContext = &(pAd->PsPollContext);
+	if (pPsPollContext->IRPPending == TRUE)
+		RTUSB_UNLINK_URB(pPsPollContext->pUrb);
+
+	for (Idx = 0; Idx < 4; Idx++)
+	{
+		NdisAcquireSpinLock(&pAd->BulkOutLock[Idx]);
+		pAd->BulkOutPending[Idx] = FALSE;
+		NdisReleaseSpinLock(&pAd->BulkOutLock[Idx]);
+	}
+}
+
diff --git a/drivers/staging/rt2870/common/rtusb_data.c b/drivers/staging/rt2870/common/rtusb_data.c
new file mode 100644
index 000000000000..5a0d78389f41
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtusb_data.c
@@ -0,0 +1,229 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtusb_data.c
+
+	Abstract:
+	Ralink USB driver Tx/Rx functions.
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Jan            03-25-2006    created
+
+*/
+#include "../rt_config.h"
+
+extern  UCHAR Phy11BGNextRateUpward[]; // defined in mlme.c
+extern UCHAR	EpToQueue[];
+
+VOID REPORT_AMSDU_FRAMES_TO_LLC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize)
+{
+	PNDIS_PACKET	pPacket;
+	UINT			nMSDU;
+	struct			sk_buff *pSkb;
+
+	nMSDU = 0;
+	/* allocate a rx packet */
+	pSkb = dev_alloc_skb(RX_BUFFER_AGGRESIZE);
+	pPacket = (PNDIS_PACKET)OSPKT_TO_RTPKT(pSkb);
+	if (pSkb)
+	{
+
+		/* convert 802.11 to 802.3 packet */
+		pSkb->dev = get_netdev_from_bssid(pAd, BSS0);
+		RTMP_SET_PACKET_SOURCE(pPacket, PKTSRC_NDIS);
+		deaggregate_AMSDU_announce(pAd, pPacket, pData, DataSize);
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("Can't allocate skb\n"));
+	}
+}
+
+NDIS_STATUS	RTUSBFreeDescriptorRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BulkOutPipeId,
+	IN	UINT32			NumberRequired)
+{
+//	UCHAR			FreeNumber = 0;
+//	UINT			Index;
+	NDIS_STATUS		Status = NDIS_STATUS_FAILURE;
+	unsigned long   IrqFlags;
+	HT_TX_CONTEXT	*pHTTXContext;
+
+
+	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+	if ((pHTTXContext->CurWritePosition < pHTTXContext->NextBulkOutPosition) && ((pHTTXContext->CurWritePosition + NumberRequired + LOCAL_TXBUF_SIZE) > pHTTXContext->NextBulkOutPosition))
+	{
+
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+	}
+	else if ((pHTTXContext->CurWritePosition == 8) && (pHTTXContext->NextBulkOutPosition < (NumberRequired + LOCAL_TXBUF_SIZE)))
+	{
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+	}
+	else if (pHTTXContext->bCurWriting == TRUE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("RTUSBFreeD c3 --> QueIdx=%d, CWPos=%ld, NBOutPos=%ld!\n", BulkOutPipeId, pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition));
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+	}
+	else
+	{
+		Status = NDIS_STATUS_SUCCESS;
+	}
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+
+	return (Status);
+}
+
+NDIS_STATUS RTUSBFreeDescriptorRelease(
+	IN RTMP_ADAPTER *pAd,
+	IN UCHAR		BulkOutPipeId)
+{
+	unsigned long   IrqFlags;
+	HT_TX_CONTEXT	*pHTTXContext;
+
+	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+	pHTTXContext->bCurWriting = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+	return (NDIS_STATUS_SUCCESS);
+}
+
+
+BOOLEAN	RTUSBNeedQueueBackForAgg(
+	IN RTMP_ADAPTER *pAd,
+	IN UCHAR		BulkOutPipeId)
+{
+	unsigned long   IrqFlags;
+	HT_TX_CONTEXT	*pHTTXContext;
+	BOOLEAN			needQueBack = FALSE;
+
+	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+	if ((pHTTXContext->IRPPending == TRUE)  /*&& (pAd->TxSwQueue[BulkOutPipeId].Number == 0) */)
+	{
+#if 0
+		if ((pHTTXContext->CurWritePosition <= 8) &&
+			(pHTTXContext->NextBulkOutPosition > 8 && (pHTTXContext->NextBulkOutPosition+MAX_AGGREGATION_SIZE) < MAX_TXBULK_LIMIT))
+		{
+			needQueBack = TRUE;
+		}
+		else if ((pHTTXContext->CurWritePosition < pHTTXContext->NextBulkOutPosition) &&
+				 ((pHTTXContext->NextBulkOutPosition + MAX_AGGREGATION_SIZE) < MAX_TXBULK_LIMIT))
+		{
+			needQueBack = TRUE;
+		}
+#else
+		if ((pHTTXContext->CurWritePosition < pHTTXContext->ENextBulkOutPosition) &&
+			(((pHTTXContext->ENextBulkOutPosition+MAX_AGGREGATION_SIZE) < MAX_TXBULK_LIMIT) || (pHTTXContext->CurWritePosition > MAX_AGGREGATION_SIZE)))
+		{
+			needQueBack = TRUE;
+		}
+#endif
+		else if ((pHTTXContext->CurWritePosition > pHTTXContext->ENextBulkOutPosition) &&
+				 ((pHTTXContext->ENextBulkOutPosition + MAX_AGGREGATION_SIZE) < pHTTXContext->CurWritePosition))
+		{
+			needQueBack = TRUE;
+		}
+	}
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+	return needQueBack;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBRejectPendingPackets(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR			Index;
+	PQUEUE_ENTRY	pEntry;
+	PNDIS_PACKET	pPacket;
+	PQUEUE_HEADER	pQueue;
+
+
+	for (Index = 0; Index < 4; Index++)
+	{
+		NdisAcquireSpinLock(&pAd->TxSwQueueLock[Index]);
+		while (pAd->TxSwQueue[Index].Head != NULL)
+		{
+			pQueue = (PQUEUE_HEADER) &(pAd->TxSwQueue[Index]);
+			pEntry = RemoveHeadQueue(pQueue);
+			pPacket = QUEUE_ENTRY_TO_PACKET(pEntry);
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		}
+		NdisReleaseSpinLock(&pAd->TxSwQueueLock[Index]);
+
+	}
+
+}
+
+VOID RTMPWriteTxInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXINFO_STRUC 	pTxInfo,
+	IN	  USHORT		USBDMApktLen,
+	IN	  BOOLEAN		bWiv,
+	IN	  UCHAR			QueueSel,
+	IN	  UCHAR			NextValid,
+	IN	  UCHAR			TxBurst)
+{
+	pTxInfo->USBDMATxPktLen = USBDMApktLen;
+	pTxInfo->QSEL = QueueSel;
+	if (QueueSel != FIFO_EDCA)
+		DBGPRINT(RT_DEBUG_TRACE, ("====> QueueSel != FIFO_EDCA<============\n"));
+	pTxInfo->USBDMANextVLD = FALSE; //NextValid;  // Need to check with Jan about this.
+	pTxInfo->USBDMATxburst = TxBurst;
+	pTxInfo->WIV = bWiv;
+	pTxInfo->SwUseLastRound = 0;
+	pTxInfo->rsv = 0;
+	pTxInfo->rsv2 = 0;
+}
+
diff --git a/drivers/staging/rt2870/common/rtusb_io.c b/drivers/staging/rt2870/common/rtusb_io.c
new file mode 100644
index 000000000000..6db443e9268c
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtusb_io.c
@@ -0,0 +1,2006 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+ 	Module Name:
+	rtusb_io.c
+
+	Abstract:
+
+	Revision History:
+	Who			When	    What
+	--------	----------  ----------------------------------------------
+	Name		Date	    Modification logs
+	Paul Lin    06-25-2004  created
+*/
+
+#include "../rt_config.h"
+
+
+/*
+	========================================================================
+
+	Routine Description: NIC initialization complete
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+
+NTSTATUS	RTUSBFirmwareRun(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	NTSTATUS	Status;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x01,
+		0x8,
+		0,
+		NULL,
+		0);
+
+	return Status;
+}
+
+
+
+/*
+	========================================================================
+
+	Routine Description: Write Firmware to NIC.
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS RTUSBFirmwareWrite(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR		pFwImage,
+	IN ULONG		FwLen)
+{
+	UINT32		MacReg;
+	NTSTATUS 	Status;
+//	ULONG 		i;
+	USHORT		writeLen;
+
+	Status = RTUSBReadMACRegister(pAd, MAC_CSR0, &MacReg);
+
+
+	writeLen = FwLen;
+	RTUSBMultiWrite(pAd, FIRMWARE_IMAGE_BASE, pFwImage, writeLen);
+
+	Status = RTUSBWriteMACRegister(pAd, 0x7014, 0xffffffff);
+	Status = RTUSBWriteMACRegister(pAd, 0x701c, 0xffffffff);
+	Status = RTUSBFirmwareRun(pAd);
+
+	return Status;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description: Get current firmware operation mode (Return Value)
+
+	Arguments:
+
+	Return Value:
+		0 or 1 = Downloaded by host driver
+		others = Driver doesn't download firmware
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBFirmwareOpmode(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUINT32			pValue)
+{
+	NTSTATUS	Status;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		(USBD_TRANSFER_DIRECTION_IN | USBD_SHORT_TRANSFER_OK),
+		DEVICE_VENDOR_REQUEST_IN,
+		0x1,
+		0x11,
+		0,
+		pValue,
+		4);
+	return Status;
+}
+NTSTATUS	RTUSBVenderReset(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	NTSTATUS	Status;
+	DBGPRINT_RAW(RT_DEBUG_ERROR, ("-->RTUSBVenderReset\n"));
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x01,
+		0x1,
+		0,
+		NULL,
+		0);
+
+	DBGPRINT_RAW(RT_DEBUG_ERROR, ("<--RTUSBVenderReset\n"));
+	return Status;
+}
+/*
+	========================================================================
+
+	Routine Description: Read various length data from RT2573
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBMultiRead(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	OUT	PUCHAR			pData,
+	IN	USHORT			length)
+{
+	NTSTATUS	Status;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		(USBD_TRANSFER_DIRECTION_IN | USBD_SHORT_TRANSFER_OK),
+		DEVICE_VENDOR_REQUEST_IN,
+		0x7,
+		0,
+		Offset,
+		pData,
+		length);
+
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description: Write various length data to RT2573
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBMultiWrite_OneByte(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	PUCHAR			pData)
+{
+	NTSTATUS	Status;
+
+	// TODO: In 2870, use this funciton carefully cause it's not stable.
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x6,
+		0,
+		Offset,
+		pData,
+		1);
+
+	return Status;
+}
+
+NTSTATUS	RTUSBMultiWrite(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	PUCHAR			pData,
+	IN	USHORT			length)
+{
+	NTSTATUS	Status;
+
+
+        USHORT          index = 0,Value;
+        PUCHAR          pSrc = pData;
+        USHORT          resude = 0;
+
+        resude = length % 2;
+		length  += resude;
+		do
+		{
+			Value =(USHORT)( *pSrc  | (*(pSrc + 1) << 8));
+		Status = RTUSBSingleWrite(pAd,Offset + index,Value);
+            index +=2;
+            length -= 2;
+            pSrc = pSrc + 2;
+        }while(length > 0);
+
+	return Status;
+}
+
+
+NTSTATUS RTUSBSingleWrite(
+	IN 	RTMP_ADAPTER 	*pAd,
+	IN	USHORT			Offset,
+	IN	USHORT			Value)
+{
+	NTSTATUS	Status;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x2,
+		Value,
+		Offset,
+		NULL,
+		0);
+
+	return Status;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description: Read 32-bit MAC register
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBReadMACRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	OUT	PUINT32			pValue)
+{
+	NTSTATUS	Status;
+	UINT32		localVal;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		(USBD_TRANSFER_DIRECTION_IN | USBD_SHORT_TRANSFER_OK),
+		DEVICE_VENDOR_REQUEST_IN,
+		0x7,
+		0,
+		Offset,
+		&localVal,
+		4);
+
+	*pValue = le2cpu32(localVal);
+
+
+	if (Status < 0)
+		*pValue = 0xffffffff;
+
+	return Status;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description: Write 32-bit MAC register
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBWriteMACRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	UINT32			Value)
+{
+	NTSTATUS	Status;
+	UINT32		localVal;
+
+	localVal = Value;
+
+	Status = RTUSBSingleWrite(pAd, Offset, (USHORT)(localVal & 0xffff));
+	Status = RTUSBSingleWrite(pAd, Offset + 2, (USHORT)((localVal & 0xffff0000) >> 16));
+
+	return Status;
+}
+
+
+
+#if 1
+/*
+	========================================================================
+
+	Routine Description: Read 8-bit BBP register
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBReadBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	PUCHAR			pValue)
+{
+	BBP_CSR_CFG_STRUC	BbpCsr;
+	UINT			i = 0;
+	NTSTATUS		status;
+
+	// Verify the busy condition
+	do
+	{
+		status = RTUSBReadMACRegister(pAd, BBP_CSR_CFG, &BbpCsr.word);
+		if(status >= 0)
+		{
+		if (!(BbpCsr.field.Busy == BUSY))
+			break;
+		}
+		printk("RTUSBReadBBPRegister(BBP_CSR_CFG_1):retry count=%d!\n", i);
+		i++;
+	}
+	while ((i < RETRY_LIMIT) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)));
+
+	if ((i == RETRY_LIMIT) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		//
+		// Read failed then Return Default value.
+		//
+		*pValue = pAd->BbpWriteLatch[Id];
+
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Retry count exhausted or device removed!!!\n"));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	// Prepare for write material
+	BbpCsr.word 				= 0;
+	BbpCsr.field.fRead			= 1;
+	BbpCsr.field.Busy			= 1;
+	BbpCsr.field.RegNum 		= Id;
+	RTUSBWriteMACRegister(pAd, BBP_CSR_CFG, BbpCsr.word);
+
+	i = 0;
+	// Verify the busy condition
+	do
+	{
+		status = RTUSBReadMACRegister(pAd, BBP_CSR_CFG, &BbpCsr.word);
+		if (status >= 0)
+		{
+		if (!(BbpCsr.field.Busy == BUSY))
+		{
+			*pValue = (UCHAR)BbpCsr.field.Value;
+			break;
+		}
+		}
+		printk("RTUSBReadBBPRegister(BBP_CSR_CFG_2):retry count=%d!\n", i);
+		i++;
+	}
+	while ((i < RETRY_LIMIT) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)));
+
+	if ((i == RETRY_LIMIT) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		//
+		// Read failed then Return Default value.
+		//
+		*pValue = pAd->BbpWriteLatch[Id];
+
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Retry count exhausted or device removed!!!\n"));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	return STATUS_SUCCESS;
+}
+#else
+/*
+	========================================================================
+
+	Routine Description: Read 8-bit BBP register via firmware
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBReadBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	PUCHAR			pValue)
+{
+	BBP_CSR_CFG_STRUC	BbpCsr;
+	int					i, k;
+	for (i=0; i<MAX_BUSY_COUNT; i++)
+	{
+		RTUSBReadMACRegister(pAd, H2M_BBP_AGENT, &BbpCsr.word);
+		if (BbpCsr.field.Busy == BUSY)
+		{
+			continue;
+		}
+		BbpCsr.word = 0;
+		BbpCsr.field.fRead = 1;
+		BbpCsr.field.BBP_RW_MODE = 1;
+		BbpCsr.field.Busy = 1;
+		BbpCsr.field.RegNum = Id;
+		RTUSBWriteMACRegister(pAd, H2M_BBP_AGENT, BbpCsr.word);
+		AsicSendCommandToMcu(pAd, 0x80, 0xff, 0x0, 0x0);
+		for (k=0; k<MAX_BUSY_COUNT; k++)
+		{
+			RTUSBReadMACRegister(pAd, H2M_BBP_AGENT, &BbpCsr.word);
+			if (BbpCsr.field.Busy == IDLE)
+				break;
+		}
+		if ((BbpCsr.field.Busy == IDLE) &&
+			(BbpCsr.field.RegNum == Id))
+		{
+			*pValue = (UCHAR)BbpCsr.field.Value;
+			break;
+		}
+	}
+	if (BbpCsr.field.Busy == BUSY)
+	{
+		DBGPRINT_ERR(("BBP read R%d=0x%x fail\n", Id, BbpCsr.word));
+		*pValue = pAd->BbpWriteLatch[Id];
+		return STATUS_UNSUCCESSFUL;
+	}
+	return STATUS_SUCCESS;
+}
+#endif
+
+#if 1
+/*
+	========================================================================
+
+	Routine Description: Write 8-bit BBP register
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBWriteBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	UCHAR			Value)
+{
+	BBP_CSR_CFG_STRUC	BbpCsr;
+	UINT			i = 0;
+	NTSTATUS		status;
+	// Verify the busy condition
+	do
+	{
+		status = RTUSBReadMACRegister(pAd, BBP_CSR_CFG, &BbpCsr.word);
+		if (status >= 0)
+		{
+		if (!(BbpCsr.field.Busy == BUSY))
+			break;
+		}
+		printk("RTUSBWriteBBPRegister(BBP_CSR_CFG):retry count=%d!\n", i);
+		i++;
+	}
+	while ((i < RETRY_LIMIT) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)));
+
+	if ((i == RETRY_LIMIT) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Retry count exhausted or device removed!!!\n"));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	// Prepare for write material
+	BbpCsr.word 				= 0;
+	BbpCsr.field.fRead			= 0;
+	BbpCsr.field.Value			= Value;
+	BbpCsr.field.Busy			= 1;
+	BbpCsr.field.RegNum 		= Id;
+	RTUSBWriteMACRegister(pAd, BBP_CSR_CFG, BbpCsr.word);
+
+	pAd->BbpWriteLatch[Id] = Value;
+
+	return STATUS_SUCCESS;
+}
+#else
+/*
+	========================================================================
+
+	Routine Description: Write 8-bit BBP register via firmware
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+
+NTSTATUS	RTUSBWriteBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	UCHAR			Value)
+
+{
+	BBP_CSR_CFG_STRUC	BbpCsr;
+	int					BusyCnt;
+	for (BusyCnt=0; BusyCnt<MAX_BUSY_COUNT; BusyCnt++)
+	{
+		RTMP_IO_READ32(pAd, H2M_BBP_AGENT, &BbpCsr.word);
+		if (BbpCsr.field.Busy == BUSY)
+			continue;
+		BbpCsr.word = 0;
+		BbpCsr.field.fRead = 0;
+		BbpCsr.field.BBP_RW_MODE = 1;
+		BbpCsr.field.Busy = 1;
+		BbpCsr.field.Value = Value;
+		BbpCsr.field.RegNum = Id;
+		RTMP_IO_WRITE32(pAd, H2M_BBP_AGENT, BbpCsr.word);
+		AsicSendCommandToMcu(pAd, 0x80, 0xff, 0x0, 0x0);
+		pAd->BbpWriteLatch[Id] = Value;
+		break;
+	}
+	if (BusyCnt == MAX_BUSY_COUNT)
+	{
+		DBGPRINT_ERR(("BBP write R%d=0x%x fail\n", Id, BbpCsr.word));
+		return STATUS_UNSUCCESSFUL;
+	}
+	return STATUS_SUCCESS;
+}
+#endif
+/*
+	========================================================================
+
+	Routine Description: Write RF register through MAC
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBWriteRFRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UINT32			Value)
+{
+	PHY_CSR4_STRUC	PhyCsr4;
+	UINT			i = 0;
+	NTSTATUS		status;
+
+	NdisZeroMemory(&PhyCsr4, sizeof(PHY_CSR4_STRUC));
+	do
+	{
+		status = RTUSBReadMACRegister(pAd, RF_CSR_CFG0, &PhyCsr4.word);
+		if (status >= 0)
+		{
+		if (!(PhyCsr4.field.Busy))
+			break;
+		}
+		printk("RTUSBWriteRFRegister(RF_CSR_CFG0):retry count=%d!\n", i);
+		i++;
+	}
+	while ((i < RETRY_LIMIT) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)));
+
+	if ((i == RETRY_LIMIT) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Retry count exhausted or device removed!!!\n"));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	RTUSBWriteMACRegister(pAd, RF_CSR_CFG0, Value);
+
+	return STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description: Write RT3070 RF register through MAC
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RT30xxWriteRFRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			RegID,
+	IN	UCHAR			Value)
+{
+	RF_CSR_CFG_STRUC	rfcsr;
+	UINT				i = 0;
+
+	do
+	{
+		RTUSBReadMACRegister(pAd, RF_CSR_CFG, &rfcsr.word);
+
+		if (!rfcsr.field.RF_CSR_KICK)
+			break;
+		i++;
+	}
+	while ((i < RETRY_LIMIT) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)));
+
+	if ((i == RETRY_LIMIT) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Retry count exhausted or device removed!!!\n"));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	rfcsr.field.RF_CSR_WR = 1;
+	rfcsr.field.RF_CSR_KICK = 1;
+	rfcsr.field.TESTCSR_RFACC_REGNUM = RegID;
+	rfcsr.field.RF_CSR_DATA = Value;
+
+	RTUSBWriteMACRegister(pAd, RF_CSR_CFG, rfcsr.word);
+
+	return STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description: Read RT3070 RF register through MAC
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RT30xxReadRFRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			RegID,
+	IN	PUCHAR			pValue)
+{
+	RF_CSR_CFG_STRUC	rfcsr;
+	UINT				i=0, k;
+
+	for (i=0; i<MAX_BUSY_COUNT; i++)
+	{
+		RTUSBReadMACRegister(pAd, RF_CSR_CFG, &rfcsr.word);
+
+		if (rfcsr.field.RF_CSR_KICK == BUSY)
+		{
+			continue;
+		}
+		rfcsr.word = 0;
+		rfcsr.field.RF_CSR_WR = 0;
+		rfcsr.field.RF_CSR_KICK = 1;
+		rfcsr.field.TESTCSR_RFACC_REGNUM = RegID;
+		RTUSBWriteMACRegister(pAd, RF_CSR_CFG, rfcsr.word);
+		for (k=0; k<MAX_BUSY_COUNT; k++)
+		{
+			RTUSBReadMACRegister(pAd, RF_CSR_CFG, &rfcsr.word);
+
+			if (rfcsr.field.RF_CSR_KICK == IDLE)
+				break;
+		}
+		if ((rfcsr.field.RF_CSR_KICK == IDLE) &&
+			(rfcsr.field.TESTCSR_RFACC_REGNUM == RegID))
+		{
+			*pValue = (UCHAR)rfcsr.field.RF_CSR_DATA;
+			break;
+		}
+	}
+	if (rfcsr.field.RF_CSR_KICK == BUSY)
+	{
+		DBGPRINT_ERR(("RF read R%d=0x%x fail\n", RegID, rfcsr.word));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBReadEEPROM(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	OUT	PUCHAR			pData,
+	IN	USHORT			length)
+{
+	NTSTATUS	Status = STATUS_SUCCESS;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		(USBD_TRANSFER_DIRECTION_IN | USBD_SHORT_TRANSFER_OK),
+		DEVICE_VENDOR_REQUEST_IN,
+		0x9,
+		0,
+		Offset,
+		pData,
+		length);
+
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBWriteEEPROM(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	PUCHAR			pData,
+	IN	USHORT			length)
+{
+	NTSTATUS	Status = STATUS_SUCCESS;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x8,
+		0,
+		Offset,
+		pData,
+		length);
+
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+VOID RTUSBPutToSleep(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UINT32		value;
+
+	// Timeout 0x40 x 50us
+	value = (SLEEPCID<<16)+(OWNERMCU<<24)+ (0x40<<8)+1;
+	RTUSBWriteMACRegister(pAd, 0x7010, value);
+	RTUSBWriteMACRegister(pAd, 0x404, 0x30);
+	//RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+	DBGPRINT_RAW(RT_DEBUG_ERROR, ("Sleep Mailbox testvalue %x\n", value));
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS RTUSBWakeUp(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	NTSTATUS	Status;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x01,
+		0x09,
+		0,
+		NULL,
+		0);
+
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBInitializeCmdQ(
+	IN	PCmdQ	cmdq)
+{
+	cmdq->head = NULL;
+	cmdq->tail = NULL;
+	cmdq->size = 0;
+	cmdq->CmdQState = RT2870_THREAD_INITED;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	RTUSBEnqueueCmdFromNdis(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	NDIS_OID		Oid,
+	IN	BOOLEAN			SetInformation,
+	IN	PVOID			pInformationBuffer,
+	IN	UINT32			InformationBufferLength)
+{
+	NDIS_STATUS	status;
+	PCmdQElmt	cmdqelmt = NULL;
+	POS_COOKIE pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+
+	CHECK_PID_LEGALITY(pObj->RTUSBCmdThr_pid)
+		return (NDIS_STATUS_RESOURCES);
+
+	status = RTMPAllocateMemory((PVOID *)&cmdqelmt, sizeof(CmdQElmt));
+	if ((status != NDIS_STATUS_SUCCESS) || (cmdqelmt == NULL))
+		return (NDIS_STATUS_RESOURCES);
+
+		cmdqelmt->buffer = NULL;
+		if (pInformationBuffer != NULL)
+		{
+			status = RTMPAllocateMemory((PVOID *)&cmdqelmt->buffer, InformationBufferLength);
+			if ((status != NDIS_STATUS_SUCCESS) || (cmdqelmt->buffer == NULL))
+			{
+				kfree(cmdqelmt);
+				return (NDIS_STATUS_RESOURCES);
+			}
+			else
+			{
+				NdisMoveMemory(cmdqelmt->buffer, pInformationBuffer, InformationBufferLength);
+				cmdqelmt->bufferlength = InformationBufferLength;
+			}
+		}
+		else
+			cmdqelmt->bufferlength = 0;
+
+	cmdqelmt->command = Oid;
+	cmdqelmt->CmdFromNdis = TRUE;
+	if (SetInformation == TRUE)
+		cmdqelmt->SetOperation = TRUE;
+	else
+		cmdqelmt->SetOperation = FALSE;
+
+	NdisAcquireSpinLock(&pAd->CmdQLock);
+	if (pAd->CmdQ.CmdQState & RT2870_THREAD_CAN_DO_INSERT)
+	{
+		EnqueueCmd((&pAd->CmdQ), cmdqelmt);
+		status = NDIS_STATUS_SUCCESS;
+	}
+	else
+	{
+		status = NDIS_STATUS_FAILURE;
+	}
+	NdisReleaseSpinLock(&pAd->CmdQLock);
+
+	if (status == NDIS_STATUS_FAILURE)
+	{
+		if (cmdqelmt->buffer)
+			NdisFreeMemory(cmdqelmt->buffer, cmdqelmt->bufferlength, 0);
+		NdisFreeMemory(cmdqelmt, sizeof(CmdQElmt), 0);
+	}
+	else
+	RTUSBCMDUp(pAd);
+
+
+    return(NDIS_STATUS_SUCCESS);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS RTUSBEnqueueInternalCmd(
+	IN PRTMP_ADAPTER	pAd,
+	IN NDIS_OID			Oid,
+	IN PVOID			pInformationBuffer,
+	IN UINT32			InformationBufferLength)
+{
+	NDIS_STATUS	status;
+	PCmdQElmt	cmdqelmt = NULL;
+
+
+	status = RTMPAllocateMemory((PVOID *)&cmdqelmt, sizeof(CmdQElmt));
+	if ((status != NDIS_STATUS_SUCCESS) || (cmdqelmt == NULL))
+		return (NDIS_STATUS_RESOURCES);
+	NdisZeroMemory(cmdqelmt, sizeof(CmdQElmt));
+
+	if(InformationBufferLength > 0)
+	{
+		status = RTMPAllocateMemory((PVOID *)&cmdqelmt->buffer, InformationBufferLength);
+		if ((status != NDIS_STATUS_SUCCESS) || (cmdqelmt->buffer == NULL))
+		{
+			NdisFreeMemory(cmdqelmt, sizeof(CmdQElmt), 0);
+			return (NDIS_STATUS_RESOURCES);
+		}
+		else
+		{
+			NdisMoveMemory(cmdqelmt->buffer, pInformationBuffer, InformationBufferLength);
+			cmdqelmt->bufferlength = InformationBufferLength;
+		}
+	}
+	else
+	{
+		cmdqelmt->buffer = NULL;
+		cmdqelmt->bufferlength = 0;
+	}
+
+	cmdqelmt->command = Oid;
+	cmdqelmt->CmdFromNdis = FALSE;
+
+	if (cmdqelmt != NULL)
+	{
+		NdisAcquireSpinLock(&pAd->CmdQLock);
+		if (pAd->CmdQ.CmdQState & RT2870_THREAD_CAN_DO_INSERT)
+		{
+			EnqueueCmd((&pAd->CmdQ), cmdqelmt);
+			status = NDIS_STATUS_SUCCESS;
+		}
+		else
+		{
+			status = NDIS_STATUS_FAILURE;
+		}
+		NdisReleaseSpinLock(&pAd->CmdQLock);
+
+		if (status == NDIS_STATUS_FAILURE)
+		{
+			if (cmdqelmt->buffer)
+				NdisFreeMemory(cmdqelmt->buffer, cmdqelmt->bufferlength, 0);
+			NdisFreeMemory(cmdqelmt, sizeof(CmdQElmt), 0);
+		}
+		else
+		RTUSBCMDUp(pAd);
+	}
+	return(NDIS_STATUS_SUCCESS);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBDequeueCmd(
+	IN	PCmdQ		cmdq,
+	OUT	PCmdQElmt	*pcmdqelmt)
+{
+	*pcmdqelmt = cmdq->head;
+
+	if (*pcmdqelmt != NULL)
+	{
+		cmdq->head = cmdq->head->next;
+		cmdq->size--;
+		if (cmdq->size == 0)
+			cmdq->tail = NULL;
+	}
+}
+
+/*
+    ========================================================================
+	  usb_control_msg - Builds a control urb, sends it off and waits for completion
+	  @dev: pointer to the usb device to send the message to
+	  @pipe: endpoint "pipe" to send the message to
+	  @request: USB message request value
+	  @requesttype: USB message request type value
+	  @value: USB message value
+	  @index: USB message index value
+	  @data: pointer to the data to send
+	  @size: length in bytes of the data to send
+	  @timeout: time in jiffies to wait for the message to complete before
+			  timing out (if 0 the wait is forever)
+	  Context: !in_interrupt ()
+
+	  This function sends a simple control message to a specified endpoint
+	  and waits for the message to complete, or timeout.
+	  If successful, it returns the number of bytes transferred, otherwise a negative error number.
+
+	 Don't use this function from within an interrupt context, like a
+	  bottom half handler.	If you need an asynchronous message, or need to send
+	  a message from within interrupt context, use usb_submit_urb()
+	  If a thread in your driver uses this call, make sure your disconnect()
+	  method can wait for it to complete.  Since you don't have a handle on
+	  the URB used, you can't cancel the request.
+
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS    RTUSB_VendorRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UINT32			TransferFlags,
+	IN	UCHAR			RequestType,
+	IN	UCHAR			Request,
+	IN	USHORT			Value,
+	IN	USHORT			Index,
+	IN	PVOID			TransferBuffer,
+	IN	UINT32			TransferBufferLength)
+{
+	int				ret;
+	POS_COOKIE		pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("device disconnected\n"));
+		return -1;
+	}
+	else if (in_interrupt())
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("in_interrupt, RTUSB_VendorRequest Request%02x Value%04x Offset%04x\n",Request,Value,Index));
+
+		return -1;
+	}
+	else
+	{
+#define MAX_RETRY_COUNT  10
+
+		int retryCount = 0;
+		void	*tmpBuf = TransferBuffer;
+
+		// Acquire Control token
+#ifdef INF_AMAZON_SE
+		//Semaphore fix INF_AMAZON_SE hang
+		//pAd->UsbVendorReqBuf is the swap for DEVICE_VENDOR_REQUEST_IN to fix dma bug.
+		ret = down_interruptible(&(pAd->UsbVendorReq_semaphore));
+		if (pAd->UsbVendorReqBuf)
+		{
+			ASSERT(TransferBufferLength <MAX_PARAM_BUFFER_SIZE);
+
+		   	tmpBuf = (void *)pAd->UsbVendorReqBuf;
+		   	NdisZeroMemory(pAd->UsbVendorReqBuf, TransferBufferLength);
+
+		   	if (RequestType == DEVICE_VENDOR_REQUEST_OUT)
+		   	 NdisMoveMemory(tmpBuf, TransferBuffer, TransferBufferLength);
+		}
+#endif // INF_AMAZON_SE //
+		do {
+		if( RequestType == DEVICE_VENDOR_REQUEST_OUT)
+			ret=usb_control_msg(pObj->pUsb_Dev, usb_sndctrlpipe( pObj->pUsb_Dev, 0 ), Request, RequestType, Value,Index, tmpBuf, TransferBufferLength, CONTROL_TIMEOUT_JIFFIES);
+		else if(RequestType == DEVICE_VENDOR_REQUEST_IN)
+			ret=usb_control_msg(pObj->pUsb_Dev, usb_rcvctrlpipe( pObj->pUsb_Dev, 0 ), Request, RequestType, Value,Index, tmpBuf, TransferBufferLength, CONTROL_TIMEOUT_JIFFIES);
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("vendor request direction is failed\n"));
+			ret = -1;
+		}
+
+			retryCount++;
+			if (ret < 0) {
+				printk("#\n");
+				RTMPusecDelay(5000);
+			}
+		} while((ret < 0) && (retryCount < MAX_RETRY_COUNT));
+
+#ifdef INF_AMAZON_SE
+	  	if ((pAd->UsbVendorReqBuf) && (RequestType == DEVICE_VENDOR_REQUEST_IN))
+			NdisMoveMemory(TransferBuffer, tmpBuf, TransferBufferLength);
+	  	up(&(pAd->UsbVendorReq_semaphore));
+#endif // INF_AMAZON_SE //
+
+        if (ret < 0) {
+//			DBGPRINT(RT_DEBUG_ERROR, ("USBVendorRequest failed ret=%d \n",ret));
+			DBGPRINT(RT_DEBUG_ERROR, ("RTUSB_VendorRequest failed(%d),TxFlags=0x%x, ReqType=%s, Req=0x%x, Index=0x%x\n",
+						ret, TransferFlags, (RequestType == DEVICE_VENDOR_REQUEST_OUT ? "OUT" : "IN"), Request, Index));
+			if (Request == 0x2)
+				DBGPRINT(RT_DEBUG_ERROR, ("\tRequest Value=0x%04x!\n", Value));
+
+			if ((TransferBuffer!= NULL) && (TransferBufferLength > 0))
+				hex_dump("Failed TransferBuffer value", TransferBuffer, TransferBufferLength);
+        }
+
+#if 0
+        // retry
+		if (ret < 0) {
+			int temp_i=0;
+			DBGPRINT(RT_DEBUG_ERROR, ("USBVendorRequest failed ret=%d, \n",ret));
+			ret = 0;
+			do
+			{
+				if( RequestType == DEVICE_VENDOR_REQUEST_OUT)
+					ret=usb_control_msg(pObj->pUsb_Dev, usb_sndctrlpipe( pObj->pUsb_Dev, 0 ), Request, RequestType, Value,Index, TransferBuffer, TransferBufferLength, CONTROL_TIMEOUT_JIFFIES);
+				else if(RequestType == DEVICE_VENDOR_REQUEST_IN)
+					ret=usb_control_msg(pObj->pUsb_Dev, usb_rcvctrlpipe( pObj->pUsb_Dev, 0 ), Request, RequestType, Value,Index, TransferBuffer, TransferBufferLength, CONTROL_TIMEOUT_JIFFIES);
+				temp_i++;
+			} while( (ret < 0) && (temp_i <= 1) );
+
+			if( ret >= 0)
+				return ret;
+
+		}
+#endif
+
+	}
+	return ret;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+	  Creates an IRP to submite an IOCTL_INTERNAL_USB_RESET_PORT
+	  synchronously. Callers of this function must be running at
+	  PASSIVE LEVEL.
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSB_ResetDevice(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	NTSTATUS		Status = TRUE;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("--->USB_ResetDevice\n"));
+	//RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS);
+	return Status;
+}
+
+VOID CMDHandler(
+    IN PRTMP_ADAPTER pAd)
+{
+	PCmdQElmt		cmdqelmt;
+	PUCHAR			pData;
+	NDIS_STATUS		NdisStatus = NDIS_STATUS_SUCCESS;
+//	ULONG			Now = 0;
+	NTSTATUS		ntStatus;
+//	unsigned long	IrqFlags;
+
+	while (pAd->CmdQ.size > 0)
+	{
+		NdisStatus = NDIS_STATUS_SUCCESS;
+
+		NdisAcquireSpinLock(&pAd->CmdQLock);
+		RTUSBDequeueCmd(&pAd->CmdQ, &cmdqelmt);
+		NdisReleaseSpinLock(&pAd->CmdQLock);
+
+		if (cmdqelmt == NULL)
+			break;
+
+		pData = cmdqelmt->buffer;
+
+		if(!(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)))
+		{
+			switch (cmdqelmt->command)
+			{
+				case CMDTHREAD_CHECK_GPIO:
+					{
+#ifdef CONFIG_STA_SUPPORT
+						UINT32 data;
+#endif // CONFIG_STA_SUPPORT //
+#ifdef RALINK_ATE
+       					if(ATE_ON(pAd))
+						{
+							DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+							break;
+						}
+#endif // RALINK_ATE //
+
+#ifdef CONFIG_STA_SUPPORT
+
+
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						{
+							// Read GPIO pin2 as Hardware controlled radio state
+
+							RTUSBReadMACRegister( pAd, GPIO_CTRL_CFG, &data);
+
+							if (data & 0x04)
+							{
+								pAd->StaCfg.bHwRadio = TRUE;
+							}
+							else
+							{
+								pAd->StaCfg.bHwRadio = FALSE;
+							}
+
+							if(pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+							{
+								pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+								if(pAd->StaCfg.bRadio == TRUE)
+								{
+									DBGPRINT_RAW(RT_DEBUG_ERROR, ("!!! Radio On !!!\n"));
+
+									MlmeRadioOn(pAd);
+									// Update extra information
+									pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+								}
+								else
+								{
+									DBGPRINT_RAW(RT_DEBUG_ERROR, ("!!! Radio Off !!!\n"));
+
+									MlmeRadioOff(pAd);
+									// Update extra information
+									pAd->ExtraInfo = HW_RADIO_OFF;
+								}
+							}
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+					break;
+
+#ifdef CONFIG_STA_SUPPORT
+				case CMDTHREAD_QKERIODIC_EXECUT:
+					{
+						StaQuickResponeForRateUpExec(NULL, pAd, NULL, NULL);
+					}
+					break;
+#endif // CONFIG_STA_SUPPORT //
+
+				case CMDTHREAD_RESET_BULK_OUT:
+					{
+						UINT32		MACValue;
+						UCHAR		Index;
+						int			ret=0;
+						PHT_TX_CONTEXT	pHTTXContext;
+//						RTMP_TX_RING *pTxRing;
+						unsigned long IrqFlags;
+#ifdef RALINK_ATE
+						PTX_CONTEXT		pNullContext = &(pAd->NullContext);
+#endif // RALINK_ATE //
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("CmdThread : CMDTHREAD_RESET_BULK_OUT(ResetPipeid=0x%0x)===>\n", pAd->bulkResetPipeid));
+						// All transfers must be aborted or cancelled before attempting to reset the pipe.
+						//RTUSBCancelPendingBulkOutIRP(pAd);
+						// Wait 10ms to let previous packet that are already in HW FIFO to clear. by MAXLEE 12-25-2007
+						Index = 0;
+						do
+						{
+							RTUSBReadMACRegister(pAd, TXRXQ_PCNT, &MACValue);
+							if ((MACValue & 0xf00000/*0x800000*/) == 0)
+								break;
+							Index++;
+							RTMPusecDelay(10000);
+						}while(Index < 100);
+						MACValue = 0;
+						RTUSBReadMACRegister(pAd, USB_DMA_CFG, &MACValue);
+						// To prevent Read Register error, we 2nd check the validity.
+						if ((MACValue & 0xc00000) == 0)
+							RTUSBReadMACRegister(pAd, USB_DMA_CFG, &MACValue);
+						// To prevent Read Register error, we 3rd check the validity.
+						if ((MACValue & 0xc00000) == 0)
+							RTUSBReadMACRegister(pAd, USB_DMA_CFG, &MACValue);
+						MACValue |= 0x80000;
+						RTUSBWriteMACRegister(pAd, USB_DMA_CFG, MACValue);
+
+						// Wait 1ms to prevent next URB to bulkout before HW reset. by MAXLEE 12-25-2007
+						RTMPusecDelay(1000);
+
+						MACValue &= (~0x80000);
+						RTUSBWriteMACRegister(pAd, USB_DMA_CFG, MACValue);
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("\tSet 0x2a0 bit19. Clear USB DMA TX path\n"));
+
+						// Wait 5ms to prevent next URB to bulkout before HW reset. by MAXLEE 12-25-2007
+						//RTMPusecDelay(5000);
+
+						if ((pAd->bulkResetPipeid & BULKOUT_MGMT_RESET_FLAG) == BULKOUT_MGMT_RESET_FLAG)
+						{
+							RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+							if (pAd->MgmtRing.TxSwFreeIdx < MGMT_RING_SIZE /* pMLMEContext->bWaitingBulkOut == TRUE */)
+							{
+								RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME);
+							}
+							RTUSBKickBulkOut(pAd);
+
+							DBGPRINT_RAW(RT_DEBUG_TRACE, ("\tTX MGMT RECOVER Done!\n"));
+						}
+						else
+						{
+							pHTTXContext = &(pAd->TxContext[pAd->bulkResetPipeid]);
+							//NdisAcquireSpinLock(&pAd->BulkOutLock[pAd->bulkResetPipeid]);
+							RTMP_INT_LOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+							if ( pAd->BulkOutPending[pAd->bulkResetPipeid] == FALSE)
+							{
+								pAd->BulkOutPending[pAd->bulkResetPipeid] = TRUE;
+								pHTTXContext->IRPPending = TRUE;
+								pAd->watchDogTxPendingCnt[pAd->bulkResetPipeid] = 1;
+
+								// no matter what, clean the flag
+								RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+
+								//NdisReleaseSpinLock(&pAd->BulkOutLock[pAd->bulkResetPipeid]);
+								RTMP_INT_UNLOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+/*-----------------------------------------------------------------------------------------------*/
+#ifdef RALINK_ATE
+								if(ATE_ON(pAd))
+							    {
+									pNullContext->IRPPending = TRUE;
+									//
+									// If driver is still in ATE TXFRAME mode,
+									// keep on transmitting ATE frames.
+									//
+									DBGPRINT_RAW(RT_DEBUG_TRACE, ("pAd->ate.Mode == %d\npAd->ContinBulkOut == %d\npAd->BulkOutRemained == %d\n", pAd->ate.Mode, pAd->ContinBulkOut, atomic_read(&pAd->BulkOutRemained)));
+									if((pAd->ate.Mode == ATE_TXFRAME) && ((pAd->ContinBulkOut == TRUE) || (atomic_read(&pAd->BulkOutRemained) > 0)))
+								    {
+										DBGPRINT_RAW(RT_DEBUG_TRACE, ("After CMDTHREAD_RESET_BULK_OUT, continue to bulk out frames !\n"));
+
+										// Init Tx context descriptor
+										RTUSBInitTxDesc(pAd, pNullContext, 0/* pAd->bulkResetPipeid */, (usb_complete_t)ATE_RTUSBBulkOutDataPacketComplete);
+
+										if((ret = RTUSB_SUBMIT_URB(pNullContext->pUrb))!=0)
+										{
+											DBGPRINT(RT_DEBUG_ERROR, ("ATE_RTUSBBulkOutDataPacket: Submit Tx URB failed %d\n", ret));
+										}
+
+										pAd->BulkOutReq++;
+									}
+								}
+								else
+#endif // RALINK_ATE //
+/*-----------------------------------------------------------------------------------------------*/
+								{
+								RTUSBInitHTTxDesc(pAd, pHTTXContext, pAd->bulkResetPipeid, pHTTXContext->BulkOutSize, (usb_complete_t)RTUSBBulkOutDataPacketComplete);
+
+								if((ret = RTUSB_SUBMIT_URB(pHTTXContext->pUrb))!=0)
+								{
+										RTMP_INT_LOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+									pAd->BulkOutPending[pAd->bulkResetPipeid] = FALSE;
+									pHTTXContext->IRPPending = FALSE;
+										pAd->watchDogTxPendingCnt[pAd->bulkResetPipeid] = 0;
+										RTMP_INT_UNLOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+
+										DBGPRINT(RT_DEBUG_ERROR, ("CmdThread : CMDTHREAD_RESET_BULK_OUT: Submit Tx URB failed %d\n", ret));
+								}
+									else
+									{
+										RTMP_IRQ_LOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+										DBGPRINT_RAW(RT_DEBUG_TRACE,("\tCMDTHREAD_RESET_BULK_OUT: TxContext[%d]:CWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d, pending=%d!\n",
+												pAd->bulkResetPipeid, pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition,
+															pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad, pAd->BulkOutPending[pAd->bulkResetPipeid]));
+										DBGPRINT_RAW(RT_DEBUG_TRACE,("\t\tBulkOut Req=0x%lx, Complete=0x%lx, Other=0x%lx\n",
+															pAd->BulkOutReq, pAd->BulkOutComplete, pAd->BulkOutCompleteOther));
+										RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+										DBGPRINT_RAW(RT_DEBUG_TRACE, ("\tCMDTHREAD_RESET_BULK_OUT: Submit Tx DATA URB for failed BulkReq(0x%lx) Done, status=%d!\n", pAd->bulkResetReq[pAd->bulkResetPipeid], pHTTXContext->pUrb->status));
+
+									}
+								}
+							}
+							else
+							{
+								//NdisReleaseSpinLock(&pAd->BulkOutLock[pAd->bulkResetPipeid]);
+								//RTMP_INT_UNLOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+
+								DBGPRINT_RAW(RT_DEBUG_ERROR, ("CmdThread : TX DATA RECOVER FAIL for BulkReq(0x%lx) because BulkOutPending[%d] is TRUE!\n", pAd->bulkResetReq[pAd->bulkResetPipeid], pAd->bulkResetPipeid));
+								if (pAd->bulkResetPipeid == 0)
+								{
+									UCHAR	pendingContext = 0;
+									PHT_TX_CONTEXT pHTTXContext = (PHT_TX_CONTEXT)(&pAd->TxContext[pAd->bulkResetPipeid ]);
+									PTX_CONTEXT pMLMEContext = (PTX_CONTEXT)(pAd->MgmtRing.Cell[pAd->MgmtRing.TxDmaIdx].AllocVa);
+									PTX_CONTEXT pNULLContext = (PTX_CONTEXT)(&pAd->PsPollContext);
+									PTX_CONTEXT pPsPollContext = (PTX_CONTEXT)(&pAd->NullContext);
+
+									if (pHTTXContext->IRPPending)
+										pendingContext |= 1;
+									else if (pMLMEContext->IRPPending)
+										pendingContext |= 2;
+									else if (pNULLContext->IRPPending)
+										pendingContext |= 4;
+									else if (pPsPollContext->IRPPending)
+										pendingContext |= 8;
+									else
+										pendingContext = 0;
+
+									DBGPRINT_RAW(RT_DEBUG_ERROR, ("\tTX Occupied by %d!\n", pendingContext));
+								}
+
+							// no matter what, clean the flag
+							RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+
+								RTMP_INT_UNLOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+
+								RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << pAd->bulkResetPipeid));
+							}
+
+							RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+							//RTUSBKickBulkOut(pAd);
+						}
+
+					}
+					/*
+						// Don't cancel BULKIN.
+						while ((atomic_read(&pAd->PendingRx) > 0) &&
+								(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+						{
+							if (atomic_read(&pAd->PendingRx) > 0)
+							{
+								DBGPRINT_RAW(RT_DEBUG_ERROR, ("BulkIn IRP Pending!!cancel it!\n"));
+								RTUSBCancelPendingBulkInIRP(pAd);
+							}
+							RTMPusecDelay(100000);
+						}
+
+						if ((atomic_read(&pAd->PendingRx) == 0) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)))
+						{
+							UCHAR	i;
+							RTUSBRxPacket(pAd);
+							pAd->NextRxBulkInReadIndex = 0;	// Next Rx Read index
+							pAd->NextRxBulkInIndex		= 0;	// Rx Bulk pointer
+							for (i = 0; i < (RX_RING_SIZE); i++)
+							{
+								PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+
+								pRxContext->pAd	= pAd;
+								pRxContext->InUse		= FALSE;
+								pRxContext->IRPPending	= FALSE;
+								pRxContext->Readable	= FALSE;
+								pRxContext->ReorderInUse = FALSE;
+
+							}
+							RTUSBBulkReceive(pAd);
+							DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTUSBBulkReceive\n"));
+						}*/
+					DBGPRINT_RAW(RT_DEBUG_TRACE, ("CmdThread : CMDTHREAD_RESET_BULK_OUT<===\n"));
+    	   			break;
+
+				case CMDTHREAD_RESET_BULK_IN:
+					DBGPRINT_RAW(RT_DEBUG_TRACE, ("CmdThread : CMDTHREAD_RESET_BULK_IN === >\n"));
+
+					// All transfers must be aborted or cancelled before attempting to reset the pipe.
+					{
+						UINT32		MACValue;
+/*-----------------------------------------------------------------------------------------------*/
+#ifdef RALINK_ATE
+						if (ATE_ON(pAd))
+						{
+							if((pAd->PendingRx > 0) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+							{
+								DBGPRINT_RAW(RT_DEBUG_ERROR, ("ATE : BulkIn IRP Pending!!!\n"));
+								ATE_RTUSBCancelPendingBulkInIRP(pAd);
+								RTMPusecDelay(100000);
+								pAd->PendingRx = 0;
+							}
+						}
+						else
+#endif // RALINK_ATE //
+/*-----------------------------------------------------------------------------------------------*/
+						{
+						//while ((atomic_read(&pAd->PendingRx) > 0) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+						if((pAd->PendingRx > 0) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+						{
+							DBGPRINT_RAW(RT_DEBUG_ERROR, ("BulkIn IRP Pending!!!\n"));
+							RTUSBCancelPendingBulkInIRP(pAd);
+							RTMPusecDelay(100000);
+							pAd->PendingRx = 0;
+						}
+						}
+
+						// Wait 10ms before reading register.
+						RTMPusecDelay(10000);
+						ntStatus = RTUSBReadMACRegister(pAd, MAC_CSR0, &MACValue);
+
+						if ((NT_SUCCESS(ntStatus) == TRUE) &&
+							(!(RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS | fRTMP_ADAPTER_RADIO_OFF |
+													fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST)))))
+						{
+							UCHAR	i;
+
+							if (RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS | fRTMP_ADAPTER_RADIO_OFF |
+														fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST)))
+								break;
+							pAd->NextRxBulkInPosition = pAd->RxContext[pAd->NextRxBulkInIndex].BulkInOffset;
+							DBGPRINT(RT_DEBUG_TRACE, ("BULK_IN_RESET: NBIIdx=0x%x,NBIRIdx=0x%x, BIRPos=0x%lx. BIReq=x%lx, BIComplete=0x%lx, BICFail0x%lx\n",
+									pAd->NextRxBulkInIndex,  pAd->NextRxBulkInReadIndex, pAd->NextRxBulkInPosition, pAd->BulkInReq, pAd->BulkInComplete, pAd->BulkInCompleteFail));
+							for (i = 0; i < RX_RING_SIZE; i++)
+							{
+ 								DBGPRINT(RT_DEBUG_TRACE, ("\tRxContext[%d]: IRPPending=%d, InUse=%d, Readable=%d!\n"
+									, i, pAd->RxContext[i].IRPPending, pAd->RxContext[i].InUse, pAd->RxContext[i].Readable));
+							}
+ 							/*
+
+							DBGPRINT_RAW(RT_DEBUG_ERROR, ("==========================================\n"));
+
+							pAd->NextRxBulkInReadIndex = 0;	// Next Rx Read index
+							pAd->NextRxBulkInIndex		= 0;	// Rx Bulk pointer
+							for (i = 0; i < (RX_RING_SIZE); i++)
+							{
+								PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+
+								pRxContext->pAd	= pAd;
+								pRxContext->InUse		= FALSE;
+								pRxContext->IRPPending	= FALSE;
+								pRxContext->Readable	= FALSE;
+								pRxContext->ReorderInUse = FALSE;
+
+							}*/
+							RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET);
+							for (i = 0; i < pAd->CommonCfg.NumOfBulkInIRP; i++)
+							{
+								//RTUSBBulkReceive(pAd);
+								PRX_CONTEXT		pRxContext;
+								PURB			pUrb;
+								int				ret = 0;
+								unsigned long	IrqFlags;
+
+
+								RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+								pRxContext = &(pAd->RxContext[pAd->NextRxBulkInIndex]);
+								if ((pAd->PendingRx > 0) || (pRxContext->Readable == TRUE) || (pRxContext->InUse == TRUE))
+								{
+									RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+									break;
+								}
+								pRxContext->InUse = TRUE;
+								pRxContext->IRPPending = TRUE;
+								pAd->PendingRx++;
+								pAd->BulkInReq++;
+								RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+								// Init Rx context descriptor
+								RTUSBInitRxDesc(pAd, pRxContext);
+								pUrb = pRxContext->pUrb;
+								if ((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+								{	// fail
+
+									RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+									pRxContext->InUse = FALSE;
+									pRxContext->IRPPending = FALSE;
+									pAd->PendingRx--;
+									pAd->BulkInReq--;
+									RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+									DBGPRINT(RT_DEBUG_ERROR, ("CMDTHREAD_RESET_BULK_IN: Submit Rx URB failed(%d), status=%d\n", ret, pUrb->status));
+								}
+								else
+								{	// success
+#if 0
+									RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+									pRxContext->IRPPending = TRUE;
+									//NdisInterlockedIncrement(&pAd->PendingRx);
+									pAd->PendingRx++;
+									RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+									pAd->BulkInReq++;
+#endif
+									//printk("BIDone, Pend=%d,BIIdx=%d,BIRIdx=%d!\n", pAd->PendingRx, pAd->NextRxBulkInIndex, pAd->NextRxBulkInReadIndex);
+									DBGPRINT_RAW(RT_DEBUG_TRACE, ("CMDTHREAD_RESET_BULK_IN: Submit Rx URB Done, status=%d!\n", pUrb->status));
+									ASSERT((pRxContext->InUse == pRxContext->IRPPending));
+								}
+							}
+
+						}
+						else
+						{
+							// Card must be removed
+							if (NT_SUCCESS(ntStatus) != TRUE)
+							{
+							RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST);
+								DBGPRINT_RAW(RT_DEBUG_ERROR, ("CMDTHREAD_RESET_BULK_IN: Read Register Failed!Card must be removed!!\n\n"));
+							}
+							else
+							{
+								DBGPRINT_RAW(RT_DEBUG_ERROR, ("CMDTHREAD_RESET_BULK_IN: Cannot do bulk in because flags(0x%lx) on !\n", pAd->Flags));
+						}
+					}
+					}
+					DBGPRINT_RAW(RT_DEBUG_TRACE, ("CmdThread : CMDTHREAD_RESET_BULK_IN <===\n"));
+					break;
+
+				case CMDTHREAD_SET_ASIC_WCID:
+					{
+						RT_SET_ASIC_WCID	SetAsicWcid;
+						USHORT		offset;
+						UINT32		MACValue, MACRValue = 0;
+						SetAsicWcid = *((PRT_SET_ASIC_WCID)(pData));
+
+						if (SetAsicWcid.WCID >= MAX_LEN_OF_MAC_TABLE)
+							return;
+
+						offset = MAC_WCID_BASE + ((UCHAR)SetAsicWcid.WCID)*HW_WCID_ENTRY_SIZE;
+
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("CmdThread : CMDTHREAD_SET_ASIC_WCID : WCID = %ld, SetTid  = %lx, DeleteTid = %lx.\n", SetAsicWcid.WCID, SetAsicWcid.SetTid, SetAsicWcid.DeleteTid));
+						MACValue = (pAd->MacTab.Content[SetAsicWcid.WCID].Addr[3]<<24)+(pAd->MacTab.Content[SetAsicWcid.WCID].Addr[2]<<16)+(pAd->MacTab.Content[SetAsicWcid.WCID].Addr[1]<<8)+(pAd->MacTab.Content[SetAsicWcid.WCID].Addr[0]);
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("1-MACValue= %x,\n", MACValue));
+						RTUSBWriteMACRegister(pAd, offset, MACValue);
+						// Read bitmask
+						RTUSBReadMACRegister(pAd, offset+4, &MACRValue);
+						if ( SetAsicWcid.DeleteTid != 0xffffffff)
+							MACRValue &= (~SetAsicWcid.DeleteTid);
+						if (SetAsicWcid.SetTid != 0xffffffff)
+							MACRValue |= (SetAsicWcid.SetTid);
+						MACRValue &= 0xffff0000;
+
+						MACValue = (pAd->MacTab.Content[SetAsicWcid.WCID].Addr[5]<<8)+pAd->MacTab.Content[SetAsicWcid.WCID].Addr[4];
+						MACValue |= MACRValue;
+						RTUSBWriteMACRegister(pAd, offset+4, MACValue);
+
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("2-MACValue= %x,\n", MACValue));
+					}
+					break;
+
+				case CMDTHREAD_SET_ASIC_WCID_CIPHER:
+					{
+#ifdef CONFIG_STA_SUPPORT
+						RT_SET_ASIC_WCID_ATTRI	SetAsicWcidAttri;
+						USHORT		offset;
+						UINT32		MACRValue = 0;
+						SHAREDKEY_MODE_STRUC csr1;
+						SetAsicWcidAttri = *((PRT_SET_ASIC_WCID_ATTRI)(pData));
+
+						if (SetAsicWcidAttri.WCID >= MAX_LEN_OF_MAC_TABLE)
+							return;
+
+						offset = MAC_WCID_ATTRIBUTE_BASE + ((UCHAR)SetAsicWcidAttri.WCID)*HW_WCID_ATTRI_SIZE;
+
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("Cmd : CMDTHREAD_SET_ASIC_WCID_CIPHER : WCID = %ld, Cipher = %lx.\n", SetAsicWcidAttri.WCID, SetAsicWcidAttri.Cipher));
+						// Read bitmask
+						RTUSBReadMACRegister(pAd, offset, &MACRValue);
+						MACRValue = 0;
+						MACRValue |= (((UCHAR)SetAsicWcidAttri.Cipher) << 1);
+
+						RTUSBWriteMACRegister(pAd, offset, MACRValue);
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("2-offset = %x , MACValue= %x,\n", offset, MACRValue));
+
+						offset = PAIRWISE_IVEIV_TABLE_BASE + ((UCHAR)SetAsicWcidAttri.WCID)*HW_IVEIV_ENTRY_SIZE;
+						MACRValue = 0;
+						if ( (SetAsicWcidAttri.Cipher <= CIPHER_WEP128))
+							MACRValue |= ( pAd->StaCfg.DefaultKeyId << 30);
+						else
+							MACRValue |= (0x20000000);
+						RTUSBWriteMACRegister(pAd, offset, MACRValue);
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("2-offset = %x , MACValue= %x,\n", offset, MACRValue));
+
+						//
+						// Update cipher algorithm. WSTA always use BSS0
+						//
+						// for adhoc mode only ,because wep status slow than add key, when use zero config
+						if (pAd->StaCfg.BssType == BSS_ADHOC )
+						{
+							offset = MAC_WCID_ATTRIBUTE_BASE;
+
+							RTUSBReadMACRegister(pAd, offset, &MACRValue);
+							MACRValue &= (~0xe);
+							MACRValue |= (((UCHAR)SetAsicWcidAttri.Cipher) << 1);
+
+							RTUSBWriteMACRegister(pAd, offset, MACRValue);
+
+							//Update group key cipher,,because wep status slow than add key, when use zero config
+							RTUSBReadMACRegister(pAd, SHARED_KEY_MODE_BASE+4*(0/2), &csr1.word);
+
+							csr1.field.Bss0Key0CipherAlg = SetAsicWcidAttri.Cipher;
+							csr1.field.Bss0Key1CipherAlg = SetAsicWcidAttri.Cipher;
+
+							RTUSBWriteMACRegister(pAd, SHARED_KEY_MODE_BASE+4*(0/2), csr1.word);
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+					break;
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+				// avoid in interrupt when write key
+				case RT_CMD_SET_KEY_TABLE: //General call for AsicAddPairwiseKeyEntry()
+					{
+						RT_ADD_PAIRWISE_KEY_ENTRY KeyInfo;
+						KeyInfo  = *((PRT_ADD_PAIRWISE_KEY_ENTRY)(pData));
+						AsicAddPairwiseKeyEntry(pAd,
+												KeyInfo.MacAddr,
+												(UCHAR)KeyInfo.MacTabMatchWCID,
+												&KeyInfo.CipherKey);
+					}
+					break;
+
+				case RT_CMD_SET_RX_WCID_TABLE: //General call for RTMPAddWcidAttributeEntry()
+					{
+						PMAC_TABLE_ENTRY pEntry ;
+						pEntry = (PMAC_TABLE_ENTRY)(pData);
+						RTMPAddWcidAttributeEntry(pAd,
+													BSS0,
+													0,
+													pEntry->PairwiseKey.CipherAlg,
+													pEntry);
+					}
+					break;
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+				case CMDTHREAD_SET_CLIENT_MAC_ENTRY:
+					{
+						MAC_TABLE_ENTRY *pEntry;
+						pEntry = (MAC_TABLE_ENTRY *)pData;
+
+
+#ifdef CONFIG_STA_SUPPORT
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						{
+							AsicRemovePairwiseKeyEntry(pAd, pEntry->apidx, (UCHAR)pEntry->Aid);
+							if ((pEntry->AuthMode <= Ndis802_11AuthModeAutoSwitch) && (pEntry->WepStatus == Ndis802_11Encryption1Enabled))
+							{
+								UINT32 uIV = 0;
+								PUCHAR  ptr;
+
+								ptr = (PUCHAR) &uIV;
+								*(ptr + 3) = (pAd->StaCfg.DefaultKeyId << 6);
+								AsicUpdateWCIDIVEIV(pAd, pEntry->Aid, uIV, 0);
+								AsicUpdateWCIDAttribute(pAd, pEntry->Aid, BSS0, pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg, FALSE);
+							}
+							else if (pEntry->AuthMode == Ndis802_11AuthModeWPANone)
+							{
+								UINT32 uIV = 0;
+								PUCHAR  ptr;
+
+								ptr = (PUCHAR) &uIV;
+								*(ptr + 3) = (pAd->StaCfg.DefaultKeyId << 6);
+								AsicUpdateWCIDIVEIV(pAd, pEntry->Aid, uIV, 0);
+								AsicUpdateWCIDAttribute(pAd, pEntry->Aid, BSS0, pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg, FALSE);
+							}
+							else
+							{
+								//
+								// Other case, disable engine.
+								// Don't worry WPA key, we will add WPA Key after 4-Way handshaking.
+								//
+								USHORT   offset;
+								offset = MAC_WCID_ATTRIBUTE_BASE + (pEntry->Aid * HW_WCID_ATTRI_SIZE);
+								// RX_PKEY_MODE:0 for no security; RX_KEY_TAB:0 for shared key table; BSS_IDX:0
+								RTUSBWriteMACRegister(pAd, offset, 0);
+							}
+						}
+#endif // CONFIG_STA_SUPPORT //
+
+						AsicUpdateRxWCIDTable(pAd, pEntry->Aid, pEntry->Addr);
+						printk("UpdateRxWCIDTable(): Aid=%d, Addr=%02x:%02x:%02x:%02x:%02x:%02x!\n", pEntry->Aid,
+								pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2], pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+					}
+					break;
+
+				case OID_802_11_ADD_WEP:
+					{
+#ifdef CONFIG_STA_SUPPORT
+						UINT	i;
+						UINT32	KeyIdx;
+						PNDIS_802_11_WEP	pWepKey;
+
+						DBGPRINT(RT_DEBUG_TRACE, ("CmdThread::OID_802_11_ADD_WEP  \n"));
+
+						pWepKey = (PNDIS_802_11_WEP)pData;
+						KeyIdx = pWepKey->KeyIndex & 0x0fffffff;
+
+						// it is a shared key
+						if ((KeyIdx >= 4) || ((pWepKey->KeyLength != 5) && (pWepKey->KeyLength != 13)))
+						{
+							NdisStatus = NDIS_STATUS_INVALID_DATA;
+							DBGPRINT(RT_DEBUG_ERROR, ("CmdThread::OID_802_11_ADD_WEP, INVALID_DATA!!\n"));
+						}
+						else
+						{
+							UCHAR CipherAlg;
+							pAd->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+							NdisMoveMemory(pAd->SharedKey[BSS0][KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+							CipherAlg = (pAd->SharedKey[BSS0][KeyIdx].KeyLen == 5)? CIPHER_WEP64 : CIPHER_WEP128;
+
+							//
+							// Change the WEP cipher to CKIP cipher if CKIP KP on.
+							// Funk UI or Meetinghouse UI will add ckip key from this path.
+							//
+
+							if (pAd->OpMode == OPMODE_STA)
+						 	{
+								pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+								pAd->MacTab.Content[BSSID_WCID].PairwiseKey.KeyLen = pAd->SharedKey[BSS0][KeyIdx].KeyLen;
+						 	}
+							pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CipherAlg;
+							if (pWepKey->KeyIndex & 0x80000000)
+							{
+								// Default key for tx (shared key)
+								UCHAR	IVEIV[8];
+								UINT32	WCIDAttri, Value;
+								USHORT	offset, offset2;
+								NdisZeroMemory(IVEIV, 8);
+								pAd->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+								// Add BSSID to WCTable. because this is Tx wep key.
+								// WCID Attribute UDF:3, BSSIdx:3, Alg:3, Keytable:1=PAIRWISE KEY, BSSIdx is 0
+								WCIDAttri = (CipherAlg<<1)|SHAREDKEYTABLE;
+
+								offset = MAC_WCID_ATTRIBUTE_BASE + (BSSID_WCID* HW_WCID_ATTRI_SIZE);
+								RTUSBWriteMACRegister(pAd, offset, WCIDAttri);
+								// 1. IV/EIV
+								// Specify key index to find shared key.
+								IVEIV[3] = (UCHAR)(KeyIdx<< 6);	//WEP Eiv bit off. groupkey index is not 0
+								offset = PAIRWISE_IVEIV_TABLE_BASE + (BSS0Mcast_WCID * HW_IVEIV_ENTRY_SIZE);
+								offset2 = PAIRWISE_IVEIV_TABLE_BASE + (BSSID_WCID* HW_IVEIV_ENTRY_SIZE);
+								for (i=0; i<8;)
+								{
+									Value = IVEIV[i];
+									Value += (IVEIV[i+1]<<8);
+									Value += (IVEIV[i+2]<<16);
+									Value += (IVEIV[i+3]<<24);
+									RTUSBWriteMACRegister(pAd, offset+i, Value);
+									RTUSBWriteMACRegister(pAd, offset2+i, Value);
+									i+=4;
+								}
+
+								// 2. WCID Attribute UDF:3, BSSIdx:3, Alg:3, Keytable:use share key, BSSIdx is 0
+								WCIDAttri = (pAd->SharedKey[BSS0][KeyIdx].CipherAlg<<1)|SHAREDKEYTABLE;
+								offset = MAC_WCID_ATTRIBUTE_BASE + (BSS0Mcast_WCID* HW_WCID_ATTRI_SIZE);
+							        DBGPRINT(RT_DEBUG_TRACE, ("BSS0Mcast_WCID : offset = %x, WCIDAttri = %x\n", offset, WCIDAttri));
+								RTUSBWriteMACRegister(pAd, offset, WCIDAttri);
+
+							}
+							AsicAddSharedKeyEntry(pAd, BSS0, (UCHAR)KeyIdx, CipherAlg, pWepKey->KeyMaterial, NULL, NULL);
+							DBGPRINT(RT_DEBUG_TRACE, ("CmdThread::OID_802_11_ADD_WEP (KeyIdx=%d, Len=%d-byte)\n", KeyIdx, pWepKey->KeyLength));
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+					break;
+
+				case CMDTHREAD_802_11_COUNTER_MEASURE:
+					break;
+				default:
+					DBGPRINT(RT_DEBUG_ERROR, ("--> Control Thread !! ERROR !! Unknown(cmdqelmt->command=0x%x) !! \n", cmdqelmt->command));
+					break;
+			}
+		}
+
+		if (cmdqelmt->CmdFromNdis == TRUE)
+		{
+				if (cmdqelmt->buffer != NULL)
+					NdisFreeMemory(cmdqelmt->buffer, cmdqelmt->bufferlength, 0);
+
+			NdisFreeMemory(cmdqelmt, sizeof(CmdQElmt), 0);
+		}
+		else
+		{
+			if ((cmdqelmt->buffer != NULL) && (cmdqelmt->bufferlength != 0))
+				NdisFreeMemory(cmdqelmt->buffer, cmdqelmt->bufferlength, 0);
+            {
+				NdisFreeMemory(cmdqelmt, sizeof(CmdQElmt), 0);
+			}
+		}
+	}	/* end of while */
+}
+
diff --git a/drivers/staging/rt2870/common/spectrum.c b/drivers/staging/rt2870/common/spectrum.c
new file mode 100644
index 000000000000..43782ce9288f
--- /dev/null
+++ b/drivers/staging/rt2870/common/spectrum.c
@@ -0,0 +1,1876 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+	action.c
+
+    Abstract:
+    Handle association related requests either from WSTA or from local MLME
+
+    Revision History:
+    Who          When          What
+    ---------    ----------    ----------------------------------------------
+	Fonchi Wu    2008	  	   created for 802.11h
+ */
+
+#include "../rt_config.h"
+#include "action.h"
+
+VOID MeasureReqTabInit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisAllocateSpinLock(&pAd->CommonCfg.MeasureReqTabLock);
+
+	pAd->CommonCfg.pMeasureReqTab = kmalloc(sizeof(MEASURE_REQ_TAB), GFP_ATOMIC);
+	if (pAd->CommonCfg.pMeasureReqTab)
+		NdisZeroMemory(pAd->CommonCfg.pMeasureReqTab, sizeof(MEASURE_REQ_TAB));
+	else
+		DBGPRINT(RT_DEBUG_ERROR, ("%s Fail to alloc memory for pAd->CommonCfg.pMeasureReqTab.\n", __func__));
+
+	return;
+}
+
+VOID MeasureReqTabExit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisFreeSpinLock(pAd->CommonCfg.MeasureReqTabLock);
+
+	if (pAd->CommonCfg.pMeasureReqTab)
+		kfree(pAd->CommonCfg.pMeasureReqTab);
+	pAd->CommonCfg.pMeasureReqTab = NULL;
+
+	return;
+}
+
+static PMEASURE_REQ_ENTRY MeasureReqLookUp(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	UINT HashIdx;
+	PMEASURE_REQ_TAB pTab = pAd->CommonCfg.pMeasureReqTab;
+	PMEASURE_REQ_ENTRY pEntry = NULL;
+	PMEASURE_REQ_ENTRY pPrevEntry = NULL;
+
+	if (pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab doesn't exist.\n", __func__));
+		return NULL;
+	}
+
+	RTMP_SEM_LOCK(&pAd->CommonCfg.MeasureReqTabLock);
+
+	HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(DialogToken);
+	pEntry = pTab->Hash[HashIdx];
+
+	while (pEntry)
+	{
+		if (pEntry->DialogToken == DialogToken)
+			break;
+		else
+		{
+			pPrevEntry = pEntry;
+			pEntry = pEntry->pNext;
+		}
+	}
+
+	RTMP_SEM_UNLOCK(&pAd->CommonCfg.MeasureReqTabLock);
+
+	return pEntry;
+}
+
+static PMEASURE_REQ_ENTRY MeasureReqInsert(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	INT i;
+	ULONG HashIdx;
+	PMEASURE_REQ_TAB pTab = pAd->CommonCfg.pMeasureReqTab;
+	PMEASURE_REQ_ENTRY pEntry = NULL, pCurrEntry;
+	ULONG Now;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab doesn't exist.\n", __func__));
+		return NULL;
+	}
+
+	pEntry = MeasureReqLookUp(pAd, DialogToken);
+	if (pEntry == NULL)
+	{
+		RTMP_SEM_LOCK(&pAd->CommonCfg.MeasureReqTabLock);
+		for (i = 0; i < MAX_MEASURE_REQ_TAB_SIZE; i++)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry = &pTab->Content[i];
+
+			if ((pEntry->Valid == TRUE)
+				&& RTMP_TIME_AFTER((unsigned long)Now, (unsigned long)(pEntry->lastTime + MQ_REQ_AGE_OUT)))
+			{
+				PMEASURE_REQ_ENTRY pPrevEntry = NULL;
+				ULONG HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+				PMEASURE_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+				// update Hash list
+				do
+				{
+					if (pProbeEntry == pEntry)
+					{
+						if (pPrevEntry == NULL)
+						{
+							pTab->Hash[HashIdx] = pEntry->pNext;
+						}
+						else
+						{
+							pPrevEntry->pNext = pEntry->pNext;
+						}
+						break;
+					}
+
+					pPrevEntry = pProbeEntry;
+					pProbeEntry = pProbeEntry->pNext;
+				} while (pProbeEntry);
+
+				NdisZeroMemory(pEntry, sizeof(MEASURE_REQ_ENTRY));
+				pTab->Size--;
+
+				break;
+			}
+
+			if (pEntry->Valid == FALSE)
+				break;
+		}
+
+		if (i < MAX_MEASURE_REQ_TAB_SIZE)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry->lastTime = Now;
+			pEntry->Valid = TRUE;
+			pEntry->DialogToken = DialogToken;
+			pTab->Size++;
+		}
+		else
+		{
+			pEntry = NULL;
+			DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab tab full.\n", __func__));
+		}
+
+		// add this Neighbor entry into HASH table
+		if (pEntry)
+		{
+			HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(DialogToken);
+			if (pTab->Hash[HashIdx] == NULL)
+			{
+				pTab->Hash[HashIdx] = pEntry;
+			}
+			else
+			{
+				pCurrEntry = pTab->Hash[HashIdx];
+				while (pCurrEntry->pNext != NULL)
+					pCurrEntry = pCurrEntry->pNext;
+				pCurrEntry->pNext = pEntry;
+			}
+		}
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.MeasureReqTabLock);
+	}
+
+	return pEntry;
+}
+
+static VOID MeasureReqDelete(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	PMEASURE_REQ_TAB pTab = pAd->CommonCfg.pMeasureReqTab;
+	PMEASURE_REQ_ENTRY pEntry = NULL;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab doesn't exist.\n", __func__));
+		return;
+	}
+
+	// if empty, return
+	if (pTab->Size == 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("pMeasureReqTab empty.\n"));
+		return;
+	}
+
+	pEntry = MeasureReqLookUp(pAd, DialogToken);
+	if (pEntry != NULL)
+	{
+		PMEASURE_REQ_ENTRY pPrevEntry = NULL;
+		ULONG HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+		PMEASURE_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+		RTMP_SEM_LOCK(&pAd->CommonCfg.MeasureReqTabLock);
+		// update Hash list
+		do
+		{
+			if (pProbeEntry == pEntry)
+			{
+				if (pPrevEntry == NULL)
+				{
+					pTab->Hash[HashIdx] = pEntry->pNext;
+				}
+				else
+				{
+					pPrevEntry->pNext = pEntry->pNext;
+				}
+				break;
+			}
+
+			pPrevEntry = pProbeEntry;
+			pProbeEntry = pProbeEntry->pNext;
+		} while (pProbeEntry);
+
+		NdisZeroMemory(pEntry, sizeof(MEASURE_REQ_ENTRY));
+		pTab->Size--;
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.MeasureReqTabLock);
+	}
+
+	return;
+}
+
+VOID TpcReqTabInit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisAllocateSpinLock(&pAd->CommonCfg.TpcReqTabLock);
+
+	pAd->CommonCfg.pTpcReqTab = kmalloc(sizeof(TPC_REQ_TAB), GFP_ATOMIC);
+	if (pAd->CommonCfg.pTpcReqTab)
+		NdisZeroMemory(pAd->CommonCfg.pTpcReqTab, sizeof(TPC_REQ_TAB));
+	else
+		DBGPRINT(RT_DEBUG_ERROR, ("%s Fail to alloc memory for pAd->CommonCfg.pTpcReqTab.\n", __func__));
+
+	return;
+}
+
+VOID TpcReqTabExit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisFreeSpinLock(pAd->CommonCfg.TpcReqTabLock);
+
+	if (pAd->CommonCfg.pTpcReqTab)
+		kfree(pAd->CommonCfg.pTpcReqTab);
+	pAd->CommonCfg.pTpcReqTab = NULL;
+
+	return;
+}
+
+static PTPC_REQ_ENTRY TpcReqLookUp(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	UINT HashIdx;
+	PTPC_REQ_TAB pTab = pAd->CommonCfg.pTpcReqTab;
+	PTPC_REQ_ENTRY pEntry = NULL;
+	PTPC_REQ_ENTRY pPrevEntry = NULL;
+
+	if (pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab doesn't exist.\n", __func__));
+		return NULL;
+	}
+
+	RTMP_SEM_LOCK(&pAd->CommonCfg.TpcReqTabLock);
+
+	HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(DialogToken);
+	pEntry = pTab->Hash[HashIdx];
+
+	while (pEntry)
+	{
+		if (pEntry->DialogToken == DialogToken)
+			break;
+		else
+		{
+			pPrevEntry = pEntry;
+			pEntry = pEntry->pNext;
+		}
+	}
+
+	RTMP_SEM_UNLOCK(&pAd->CommonCfg.TpcReqTabLock);
+
+	return pEntry;
+}
+
+
+static PTPC_REQ_ENTRY TpcReqInsert(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	INT i;
+	ULONG HashIdx;
+	PTPC_REQ_TAB pTab = pAd->CommonCfg.pTpcReqTab;
+	PTPC_REQ_ENTRY pEntry = NULL, pCurrEntry;
+	ULONG Now;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab doesn't exist.\n", __func__));
+		return NULL;
+	}
+
+	pEntry = TpcReqLookUp(pAd, DialogToken);
+	if (pEntry == NULL)
+	{
+		RTMP_SEM_LOCK(&pAd->CommonCfg.TpcReqTabLock);
+		for (i = 0; i < MAX_TPC_REQ_TAB_SIZE; i++)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry = &pTab->Content[i];
+
+			if ((pEntry->Valid == TRUE)
+				&& RTMP_TIME_AFTER((unsigned long)Now, (unsigned long)(pEntry->lastTime + TPC_REQ_AGE_OUT)))
+			{
+				PTPC_REQ_ENTRY pPrevEntry = NULL;
+				ULONG HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+				PTPC_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+				// update Hash list
+				do
+				{
+					if (pProbeEntry == pEntry)
+					{
+						if (pPrevEntry == NULL)
+						{
+							pTab->Hash[HashIdx] = pEntry->pNext;
+						}
+						else
+						{
+							pPrevEntry->pNext = pEntry->pNext;
+						}
+						break;
+					}
+
+					pPrevEntry = pProbeEntry;
+					pProbeEntry = pProbeEntry->pNext;
+				} while (pProbeEntry);
+
+				NdisZeroMemory(pEntry, sizeof(TPC_REQ_ENTRY));
+				pTab->Size--;
+
+				break;
+			}
+
+			if (pEntry->Valid == FALSE)
+				break;
+		}
+
+		if (i < MAX_TPC_REQ_TAB_SIZE)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry->lastTime = Now;
+			pEntry->Valid = TRUE;
+			pEntry->DialogToken = DialogToken;
+			pTab->Size++;
+		}
+		else
+		{
+			pEntry = NULL;
+			DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab tab full.\n", __func__));
+		}
+
+		// add this Neighbor entry into HASH table
+		if (pEntry)
+		{
+			HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(DialogToken);
+			if (pTab->Hash[HashIdx] == NULL)
+			{
+				pTab->Hash[HashIdx] = pEntry;
+			}
+			else
+			{
+				pCurrEntry = pTab->Hash[HashIdx];
+				while (pCurrEntry->pNext != NULL)
+					pCurrEntry = pCurrEntry->pNext;
+				pCurrEntry->pNext = pEntry;
+			}
+		}
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.TpcReqTabLock);
+	}
+
+	return pEntry;
+}
+
+static VOID TpcReqDelete(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	PTPC_REQ_TAB pTab = pAd->CommonCfg.pTpcReqTab;
+	PTPC_REQ_ENTRY pEntry = NULL;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab doesn't exist.\n", __func__));
+		return;
+	}
+
+	// if empty, return
+	if (pTab->Size == 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("pTpcReqTab empty.\n"));
+		return;
+	}
+
+	pEntry = TpcReqLookUp(pAd, DialogToken);
+	if (pEntry != NULL)
+	{
+		PTPC_REQ_ENTRY pPrevEntry = NULL;
+		ULONG HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+		PTPC_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+		RTMP_SEM_LOCK(&pAd->CommonCfg.TpcReqTabLock);
+		// update Hash list
+		do
+		{
+			if (pProbeEntry == pEntry)
+			{
+				if (pPrevEntry == NULL)
+				{
+					pTab->Hash[HashIdx] = pEntry->pNext;
+				}
+				else
+				{
+					pPrevEntry->pNext = pEntry->pNext;
+				}
+				break;
+			}
+
+			pPrevEntry = pProbeEntry;
+			pProbeEntry = pProbeEntry->pNext;
+		} while (pProbeEntry);
+
+		NdisZeroMemory(pEntry, sizeof(TPC_REQ_ENTRY));
+		pTab->Size--;
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.TpcReqTabLock);
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Get Current TimeS tamp.
+
+	Parametrs:
+
+	Return	: Current Time Stamp.
+	==========================================================================
+ */
+static UINT64 GetCurrentTimeStamp(
+	IN PRTMP_ADAPTER pAd)
+{
+	// get current time stamp.
+	return 0;
+}
+
+/*
+	==========================================================================
+	Description:
+		Get Current Transmit Power.
+
+	Parametrs:
+
+	Return	: Current Time Stamp.
+	==========================================================================
+ */
+static UINT8 GetCurTxPwr(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT8 Wcid)
+{
+	return 16; /* 16 dBm */
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Dialog Token into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Dialog token.
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertDialogToken(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 DialogToken)
+{
+	ULONG TempLen;
+	MakeOutgoingFrame(pFrameBuf,	&TempLen,
+					1,				&DialogToken,
+					END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert TPC Request IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+
+	Return	: None.
+	==========================================================================
+ */
+ static VOID InsertTpcReqIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen)
+{
+	ULONG TempLen;
+	ULONG Len = 0;
+	UINT8 ElementID = IE_TPC_REQUEST;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert TPC Report IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Transmit Power.
+		4. Link Margin.
+
+	Return	: None.
+	==========================================================================
+ */
+ static VOID InsertTpcReportIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 TxPwr,
+	IN UINT8 LinkMargin)
+{
+	ULONG TempLen;
+	ULONG Len = sizeof(TPC_REPORT_INFO);
+	UINT8 ElementID = IE_TPC_REPORT;
+	TPC_REPORT_INFO TpcReportIE;
+
+	TpcReportIE.TxPwr = TxPwr;
+	TpcReportIE.LinkMargin = LinkMargin;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						Len,						&TpcReportIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Channel Switch Announcement IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. channel switch announcement mode.
+		4. new selected channel.
+		5. channel switch announcement count.
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertChSwAnnIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 ChSwMode,
+	IN UINT8 NewChannel,
+	IN UINT8 ChSwCnt)
+{
+	ULONG TempLen;
+	ULONG Len = sizeof(CH_SW_ANN_INFO);
+	UINT8 ElementID = IE_CHANNEL_SWITCH_ANNOUNCEMENT;
+	CH_SW_ANN_INFO ChSwAnnIE;
+
+	ChSwAnnIE.ChSwMode = ChSwMode;
+	ChSwAnnIE.Channel = NewChannel;
+	ChSwAnnIE.ChSwCnt = ChSwCnt;
+
+	MakeOutgoingFrame(pFrameBuf,				&TempLen,
+						1,						&ElementID,
+						1,						&Len,
+						Len,					&ChSwAnnIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Measure Request IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Measure Token.
+		4. Measure Request Mode.
+		5. Measure Request Type.
+		6. Measure Channel.
+		7. Measure Start time.
+		8. Measure Duration.
+
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertMeasureReqIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN PMEASURE_REQ_INFO pMeasureReqIE)
+{
+	ULONG TempLen;
+	UINT8 Len = sizeof(MEASURE_REQ_INFO);
+	UINT8 ElementID = IE_MEASUREMENT_REQUEST;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						Len,						pMeasureReqIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Measure Report IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Measure Token.
+		4. Measure Request Mode.
+		5. Measure Request Type.
+		6. Length of Report Infomation
+		7. Pointer of Report Infomation Buffer.
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertMeasureReportIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN PMEASURE_REPORT_INFO pMeasureReportIE,
+	IN UINT8 ReportLnfoLen,
+	IN PUINT8 pReportInfo)
+{
+	ULONG TempLen;
+	ULONG Len;
+	UINT8 ElementID = IE_MEASUREMENT_REPORT;
+
+	Len = sizeof(MEASURE_REPORT_INFO) + ReportLnfoLen;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						Len,						pMeasureReportIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	if ((ReportLnfoLen > 0) && (pReportInfo != NULL))
+	{
+		MakeOutgoingFrame(pFrameBuf + *pFrameLen,		&TempLen,
+							ReportLnfoLen,				pReportInfo,
+							END_OF_ARGS);
+
+		*pFrameLen = *pFrameLen + TempLen;
+	}
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 MeasureCh,
+	IN UINT16 MeasureDuration)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+	HEADER_802_11 ActHdr;
+	MEASURE_REQ_INFO MeasureReqIE;
+	UINT8 RmReqDailogToken = RandomByte(pAd);
+	UINT64 MeasureStartTime = GetCurrentTimeStamp(pAd);
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __func__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_MRQ);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, MeasureToken);
+
+	// prepare Measurement IE.
+	NdisZeroMemory(&MeasureReqIE, sizeof(MEASURE_REQ_INFO));
+	MeasureReqIE.Token = RmReqDailogToken;
+	MeasureReqIE.ReqMode.word = MeasureReqMode;
+	MeasureReqIE.ReqType = MeasureReqType;
+	MeasureReqIE.MeasureReq.ChNum = MeasureCh;
+	MeasureReqIE.MeasureReq.MeasureStartTime = cpu2le64(MeasureStartTime);
+	MeasureReqIE.MeasureReq.MeasureDuration = cpu2le16(MeasureDuration);
+	InsertMeasureReqIE(pAd, (pOutBuffer + FrameLen), &FrameLen, &MeasureReqIE);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 ReportInfoLen,
+	IN PUINT8 pReportInfo)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+	HEADER_802_11 ActHdr;
+	MEASURE_REPORT_INFO MeasureRepIE;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __func__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_MRP);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, DialogToken);
+
+	// prepare Measurement IE.
+	NdisZeroMemory(&MeasureRepIE, sizeof(MEASURE_REPORT_INFO));
+	MeasureRepIE.Token = MeasureToken;
+	MeasureRepIE.ReportMode.word = MeasureReqMode;
+	MeasureRepIE.ReportType = MeasureReqType;
+	InsertMeasureReportIE(pAd, (pOutBuffer + FrameLen), &FrameLen, &MeasureRepIE, ReportInfoLen, pReportInfo);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UCHAR DialogToken)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+
+	HEADER_802_11 ActHdr;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __func__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_TPCRQ);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, DialogToken);
+
+	// Insert TPC Request IE.
+	InsertTpcReqIE(pAd, (pOutBuffer + FrameLen), &FrameLen);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 TxPwr,
+	IN UINT8 LinkMargin)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+
+	HEADER_802_11 ActHdr;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __func__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_TPCRP);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, DialogToken);
+
+	// Insert TPC Request IE.
+	InsertTpcReportIE(pAd, (pOutBuffer + FrameLen), &FrameLen, TxPwr, LinkMargin);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare Channel Switch Announcement action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+		2. Channel switch announcement mode.
+		2. a New selected channel.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueChSwAnn(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 ChSwMode,
+	IN UINT8 NewCh)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+
+	HEADER_802_11 ActHdr;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __func__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_CHANNEL_SWITCH);
+
+	InsertChSwAnnIE(pAd, (pOutBuffer + FrameLen), &FrameLen, ChSwMode, NewCh, 0);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+static BOOLEAN DfsRequirementCheck(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT8 Channel)
+{
+	BOOLEAN Result = FALSE;
+	INT i;
+
+	do
+	{
+		// check DFS procedure is running.
+		// make sure DFS procedure won't start twice.
+		if (pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE)
+		{
+			Result = FALSE;
+			break;
+		}
+
+		// check the new channel carried from Channel Switch Announcemnet is valid.
+		for (i=0; i<pAd->ChannelListNum; i++)
+		{
+			if ((Channel == pAd->ChannelList[i].Channel)
+				&&(pAd->ChannelList[i].RemainingTimeForUse == 0))
+			{
+				// found radar signal in the channel. the channel can't use at least for 30 minutes.
+				pAd->ChannelList[i].RemainingTimeForUse = 1800;//30 min = 1800 sec
+				Result = TRUE;
+				break;
+			}
+		}
+	} while(FALSE);
+
+	return Result;
+}
+
+VOID NotifyChSwAnnToPeerAPs(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pRA,
+	IN PUCHAR pTA,
+	IN UINT8 ChSwMode,
+	IN UINT8 Channel)
+{
+#ifdef WDS_SUPPORT
+	if (!((pRA[0] & 0xff) == 0xff)) // is pRA a broadcase address.
+	{
+		INT i;
+		// info neighbor APs that Radar signal found throgh WDS link.
+		for (i = 0; i < MAX_WDS_ENTRY; i++)
+		{
+			if (ValidWdsEntry(pAd, i))
+			{
+				PUCHAR pDA = pAd->WdsTab.WdsEntry[i].PeerWdsAddr;
+
+				// DA equal to SA. have no necessary orignal AP which found Radar signal.
+				if (MAC_ADDR_EQUAL(pTA, pDA))
+					continue;
+
+				// send Channel Switch Action frame to info Neighbro APs.
+				EnqueueChSwAnn(pAd, pDA, ChSwMode, Channel);
+			}
+		}
+	}
+#endif // WDS_SUPPORT //
+}
+
+static VOID StartDFSProcedure(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Channel,
+	IN UINT8 ChSwMode)
+{
+	// start DFS procedure
+	pAd->CommonCfg.Channel = Channel;
+#ifdef DOT11_N_SUPPORT
+	N_ChannelCheck(pAd);
+#endif // DOT11_N_SUPPORT //
+	pAd->CommonCfg.RadarDetect.RDMode = RD_SWITCHING_MODE;
+	pAd->CommonCfg.RadarDetect.CSCount = 0;
+}
+
+/*
+	==========================================================================
+	Description:
+		Channel Switch Announcement action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Channel switch announcement infomation buffer.
+
+
+	Return	: None.
+	==========================================================================
+ */
+
+/*
+  Channel Switch Announcement IE.
+  +----+-----+-----------+------------+-----------+
+  | ID | Len |Ch Sw Mode | New Ch Num | Ch Sw Cnt |
+  +----+-----+-----------+------------+-----------+
+    1    1        1           1            1
+*/
+static BOOLEAN PeerChSwAnnSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PCH_SW_ANN_INFO pChSwAnnInfo)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+
+	// skip 802.11 header.
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pChSwAnnInfo == NULL)
+		return result;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_CHANNEL_SWITCH_ANNOUNCEMENT:
+				NdisMoveMemory(&pChSwAnnInfo->ChSwMode, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pChSwAnnInfo->Channel, eid_ptr->Octet + 1, 1);
+				NdisMoveMemory(&pChSwAnnInfo->ChSwCnt, eid_ptr->Octet + 2, 1);
+
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		Measurement request action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Measurement request infomation buffer.
+
+	Return	: None.
+	==========================================================================
+ */
+static BOOLEAN PeerMeasureReqSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken,
+	OUT PMEASURE_REQ_INFO pMeasureReqInfo)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+	PUCHAR ptr;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+
+	// skip 802.11 header.
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pMeasureReqInfo == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_MEASUREMENT_REQUEST:
+				NdisMoveMemory(&pMeasureReqInfo->Token, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pMeasureReqInfo->ReqMode.word, eid_ptr->Octet + 1, 1);
+				NdisMoveMemory(&pMeasureReqInfo->ReqType, eid_ptr->Octet + 2, 1);
+				ptr = eid_ptr->Octet + 3;
+				NdisMoveMemory(&pMeasureReqInfo->MeasureReq.ChNum, ptr, 1);
+				NdisMoveMemory(&MeasureStartTime, ptr + 1, 8);
+				pMeasureReqInfo->MeasureReq.MeasureStartTime = SWAP64(MeasureStartTime);
+				NdisMoveMemory(&MeasureDuration, ptr + 9, 2);
+				pMeasureReqInfo->MeasureReq.MeasureDuration = SWAP16(MeasureDuration);
+
+				result = TRUE;
+				break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		Measurement report action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Measurement report infomation buffer.
+		4. basic report infomation buffer.
+
+	Return	: None.
+	==========================================================================
+ */
+
+/*
+  Measurement Report IE.
+  +----+-----+-------+-------------+--------------+----------------+
+  | ID | Len | Token | Report Mode | Measure Type | Measure Report |
+  +----+-----+-------+-------------+--------------+----------------+
+    1     1      1          1             1            variable
+
+  Basic Report.
+  +--------+------------+----------+-----+
+  | Ch Num | Start Time | Duration | Map |
+  +--------+------------+----------+-----+
+      1          8           2        1
+
+  Map Field Bit Format.
+  +-----+---------------+---------------------+-------+------------+----------+
+  | Bss | OFDM Preamble | Unidentified signal | Radar | Unmeasured | Reserved |
+  +-----+---------------+---------------------+-------+------------+----------+
+     0          1                  2              3         4          5-7
+*/
+static BOOLEAN PeerMeasureReportSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken,
+	OUT PMEASURE_REPORT_INFO pMeasureReportInfo,
+	OUT PUINT8 pReportBuf)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+	PUCHAR ptr;
+
+	// skip 802.11 header.
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pMeasureReportInfo == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_MEASUREMENT_REPORT:
+				NdisMoveMemory(&pMeasureReportInfo->Token, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pMeasureReportInfo->ReportMode, eid_ptr->Octet + 1, 1);
+				NdisMoveMemory(&pMeasureReportInfo->ReportType, eid_ptr->Octet + 2, 1);
+				if (pMeasureReportInfo->ReportType == RM_BASIC)
+				{
+					PMEASURE_BASIC_REPORT pReport = (PMEASURE_BASIC_REPORT)pReportBuf;
+					ptr = eid_ptr->Octet + 3;
+					NdisMoveMemory(&pReport->ChNum, ptr, 1);
+					NdisMoveMemory(&pReport->MeasureStartTime, ptr + 1, 8);
+					NdisMoveMemory(&pReport->MeasureDuration, ptr + 9, 2);
+					NdisMoveMemory(&pReport->Map, ptr + 11, 1);
+
+				}
+				else if (pMeasureReportInfo->ReportType == RM_CCA)
+				{
+					PMEASURE_CCA_REPORT pReport = (PMEASURE_CCA_REPORT)pReportBuf;
+					ptr = eid_ptr->Octet + 3;
+					NdisMoveMemory(&pReport->ChNum, ptr, 1);
+					NdisMoveMemory(&pReport->MeasureStartTime, ptr + 1, 8);
+					NdisMoveMemory(&pReport->MeasureDuration, ptr + 9, 2);
+					NdisMoveMemory(&pReport->CCA_Busy_Fraction, ptr + 11, 1);
+
+				}
+				else if (pMeasureReportInfo->ReportType == RM_RPI_HISTOGRAM)
+				{
+					PMEASURE_RPI_REPORT pReport = (PMEASURE_RPI_REPORT)pReportBuf;
+					ptr = eid_ptr->Octet + 3;
+					NdisMoveMemory(&pReport->ChNum, ptr, 1);
+					NdisMoveMemory(&pReport->MeasureStartTime, ptr + 1, 8);
+					NdisMoveMemory(&pReport->MeasureDuration, ptr + 9, 2);
+					NdisMoveMemory(&pReport->RPI_Density, ptr + 11, 8);
+				}
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Request action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Dialog Token.
+
+	Return	: None.
+	==========================================================================
+ */
+static BOOLEAN PeerTpcReqSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pDialogToken == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_TPC_REQUEST:
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Report action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Dialog Token.
+		4. TPC Report IE.
+
+	Return	: None.
+	==========================================================================
+ */
+static BOOLEAN PeerTpcRepSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken,
+	OUT PTPC_REPORT_INFO pTpcRepInfo)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pDialogToken == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_TPC_REPORT:
+				NdisMoveMemory(&pTpcRepInfo->TxPwr, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pTpcRepInfo->LinkMargin, eid_ptr->Octet + 1, 1);
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		Channel Switch Announcement action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerChSwAnnAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	CH_SW_ANN_INFO ChSwAnnInfo;
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR index = 0, Channel = 0, NewChannel = 0;
+	ULONG Bssidx = 0;
+#endif // CONFIG_STA_SUPPORT //
+
+	NdisZeroMemory(&ChSwAnnInfo, sizeof(CH_SW_ANN_INFO));
+	if (! PeerChSwAnnSanity(pAd, Elem->Msg, Elem->MsgLen, &ChSwAnnInfo))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("Invalid Channel Switch Action Frame.\n"));
+		return;
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->OpMode == OPMODE_STA)
+	{
+		Bssidx = BssTableSearch(&pAd->ScanTab, pFr->Hdr.Addr3, pAd->CommonCfg.Channel);
+		if (Bssidx == BSS_NOT_FOUND)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("PeerChSwAnnAction - Bssidx is not found\n"));
+			return;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("\n****Bssidx is %d, Channel = %d\n", index, pAd->ScanTab.BssEntry[Bssidx].Channel));
+		hex_dump("SSID",pAd->ScanTab.BssEntry[Bssidx].Bssid ,6);
+
+		Channel = pAd->CommonCfg.Channel;
+		NewChannel = ChSwAnnInfo.Channel;
+
+		if ((pAd->CommonCfg.bIEEE80211H == 1) && (NewChannel != 0) && (Channel != NewChannel))
+		{
+			// Switching to channel 1 can prevent from rescanning the current channel immediately (by auto reconnection).
+			// In addition, clear the MLME queue and the scan table to discard the RX packets and previous scanning results.
+			AsicSwitchChannel(pAd, 1, FALSE);
+			AsicLockChannel(pAd, 1);
+		    LinkDown(pAd, FALSE);
+			MlmeQueueInit(&pAd->Mlme.Queue);
+			BssTableInit(&pAd->ScanTab);
+		    RTMPusecDelay(1000000);		// use delay to prevent STA do reassoc
+
+			// channel sanity check
+			for (index = 0 ; index < pAd->ChannelListNum; index++)
+			{
+				if (pAd->ChannelList[index].Channel == NewChannel)
+				{
+					pAd->ScanTab.BssEntry[Bssidx].Channel = NewChannel;
+					pAd->CommonCfg.Channel = NewChannel;
+					AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+					DBGPRINT(RT_DEBUG_TRACE, ("&&&&&&&&&&&&&&&&PeerChSwAnnAction - STA receive channel switch announcement IE (New Channel =%d)\n", NewChannel));
+					break;
+				}
+			}
+
+			if (index >= pAd->ChannelListNum)
+			{
+				DBGPRINT_ERR(("&&&&&&&&&&&&&&&&&&&&&&&&&&PeerChSwAnnAction(can not find New Channel=%d in ChannelList[%d]\n", pAd->CommonCfg.Channel, pAd->ChannelListNum));
+			}
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	return;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Measurement Request action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerMeasureReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+	UINT8 DialogToken;
+	MEASURE_REQ_INFO MeasureReqInfo;
+	MEASURE_REPORT_MODE ReportMode;
+
+	if(PeerMeasureReqSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken, &MeasureReqInfo))
+	{
+		ReportMode.word = 0;
+		ReportMode.field.Incapable = 1;
+		EnqueueMeasurementRep(pAd, pFr->Hdr.Addr2, DialogToken, MeasureReqInfo.Token, ReportMode.word, MeasureReqInfo.ReqType, 0, NULL);
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Measurement Report action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerMeasureReportAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MEASURE_REPORT_INFO MeasureReportInfo;
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+	UINT8 DialogToken;
+	PUINT8 pMeasureReportInfo;
+
+//	if (pAd->CommonCfg.bIEEE80211H != TRUE)
+//		return;
+
+	if ((pMeasureReportInfo = kmalloc(sizeof(MEASURE_RPI_REPORT), GFP_ATOMIC)) == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s unable to alloc memory for measure report buffer (size=%d).\n", __func__, sizeof(MEASURE_RPI_REPORT)));
+		return;
+	}
+
+	NdisZeroMemory(&MeasureReportInfo, sizeof(MEASURE_REPORT_INFO));
+	NdisZeroMemory(pMeasureReportInfo, sizeof(MEASURE_RPI_REPORT));
+	if (PeerMeasureReportSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken, &MeasureReportInfo, pMeasureReportInfo))
+	{
+		do {
+			PMEASURE_REQ_ENTRY pEntry = NULL;
+
+			// Not a autonomous measure report.
+			// check the dialog token field. drop it if the dialog token doesn't match.
+			if ((DialogToken != 0)
+				&& ((pEntry = MeasureReqLookUp(pAd, DialogToken)) == NULL))
+				break;
+
+			if (pEntry != NULL)
+				MeasureReqDelete(pAd, pEntry->DialogToken);
+
+			if (MeasureReportInfo.ReportType == RM_BASIC)
+			{
+				PMEASURE_BASIC_REPORT pBasicReport = (PMEASURE_BASIC_REPORT)pMeasureReportInfo;
+				if ((pBasicReport->Map.field.Radar)
+					&& (DfsRequirementCheck(pAd, pBasicReport->ChNum) == TRUE))
+				{
+					NotifyChSwAnnToPeerAPs(pAd, pFr->Hdr.Addr1, pFr->Hdr.Addr2, 1, pBasicReport->ChNum);
+					StartDFSProcedure(pAd, pBasicReport->ChNum, 1);
+				}
+			}
+		} while (FALSE);
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, ("Invalid Measurement Report Frame.\n"));
+
+	kfree(pMeasureReportInfo);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Request action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerTpcReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+	PUCHAR pFramePtr = pFr->Octet;
+	UINT8 DialogToken;
+	UINT8 TxPwr = GetCurTxPwr(pAd, Elem->Wcid);
+	UINT8 LinkMargin = 0;
+	CHAR RealRssi;
+
+	// link margin: Ratio of the received signal power to the minimum desired by the station (STA). The
+	//				STA may incorporate rate information and channel conditions, including interference, into its computation
+	//				of link margin.
+
+	RealRssi = RTMPMaxRssi(pAd, ConvertToRssi(pAd, Elem->Rssi0, RSSI_0),
+								ConvertToRssi(pAd, Elem->Rssi1, RSSI_1),
+								ConvertToRssi(pAd, Elem->Rssi2, RSSI_2));
+
+	// skip Category and action code.
+	pFramePtr += 2;
+
+	// Dialog token.
+	NdisMoveMemory(&DialogToken, pFramePtr, 1);
+
+	LinkMargin = (RealRssi / MIN_RCV_PWR);
+	if (PeerTpcReqSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken))
+		EnqueueTPCRep(pAd, pFr->Hdr.Addr2, DialogToken, TxPwr, LinkMargin);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Report action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerTpcRepAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UINT8 DialogToken;
+	TPC_REPORT_INFO TpcRepInfo;
+	PTPC_REQ_ENTRY pEntry = NULL;
+
+	NdisZeroMemory(&TpcRepInfo, sizeof(TPC_REPORT_INFO));
+	if (PeerTpcRepSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken, &TpcRepInfo))
+	{
+		if ((pEntry = TpcReqLookUp(pAd, DialogToken)) != NULL)
+		{
+			TpcReqDelete(pAd, pEntry->DialogToken);
+			DBGPRINT(RT_DEBUG_TRACE, ("%s: DialogToken=%x, TxPwr=%d, LinkMargin=%d\n",
+				__func__, DialogToken, TpcRepInfo.TxPwr, TpcRepInfo.LinkMargin));
+		}
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Spectrun action frames Handler such as channel switch annoucement,
+		measurement report, measurement request actions frames.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+VOID PeerSpectrumAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	if (pAd->CommonCfg.bIEEE80211H != TRUE)
+		return;
+
+	switch(Action)
+	{
+		case SPEC_MRQ:
+			// current rt2860 unable do such measure specified in Measurement Request.
+			// reject all measurement request.
+			PeerMeasureReqAction(pAd, Elem);
+			break;
+
+		case SPEC_MRP:
+			PeerMeasureReportAction(pAd, Elem);
+			break;
+
+		case SPEC_TPCRQ:
+			PeerTpcReqAction(pAd, Elem);
+			break;
+
+		case SPEC_TPCRP:
+			PeerTpcRepAction(pAd, Elem);
+			break;
+
+		case SPEC_CHANNEL_SWITCH:
+{
+#ifdef DOT11N_DRAFT3
+				SEC_CHA_OFFSET_IE	Secondary;
+				CHA_SWITCH_ANNOUNCE_IE	ChannelSwitch;
+
+				// 802.11h only has Channel Switch Announcement IE.
+				RTMPMoveMemory(&ChannelSwitch, &Elem->Msg[LENGTH_802_11+4], sizeof (CHA_SWITCH_ANNOUNCE_IE));
+
+				// 802.11n D3.03 adds secondary channel offset element in the end.
+				if (Elem->MsgLen ==  (LENGTH_802_11 + 2 + sizeof (CHA_SWITCH_ANNOUNCE_IE) + sizeof (SEC_CHA_OFFSET_IE)))
+				{
+					RTMPMoveMemory(&Secondary, &Elem->Msg[LENGTH_802_11+9], sizeof (SEC_CHA_OFFSET_IE));
+				}
+				else
+				{
+					Secondary.SecondaryChannelOffset = 0;
+				}
+
+				if ((Elem->Msg[LENGTH_802_11+2] == IE_CHANNEL_SWITCH_ANNOUNCEMENT) && (Elem->Msg[LENGTH_802_11+3] == 3))
+				{
+					ChannelSwitchAction(pAd, Elem->Wcid, ChannelSwitch.NewChannel, Secondary.SecondaryChannelOffset);
+				}
+#endif // DOT11N_DRAFT3 //
+}
+			PeerChSwAnnAction(pAd, Elem);
+			break;
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	Parametrs:
+
+	Return	: None.
+	==========================================================================
+ */
+INT Set_MeasureReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT Aid = 1;
+	UINT ArgIdx;
+	PUCHAR thisChar;
+
+	MEASURE_REQ_MODE MeasureReqMode;
+	UINT8 MeasureReqToken = RandomByte(pAd);
+	UINT8 MeasureReqType = RM_BASIC;
+	UINT8 MeasureCh = 1;
+
+	ArgIdx = 1;
+	while ((thisChar = strsep((char **)&arg, "-")) != NULL)
+	{
+		switch(ArgIdx)
+		{
+			case 1:	// Aid.
+				Aid = simple_strtol(thisChar, 0, 16);
+				break;
+
+			case 2: // Measurement Request Type.
+				MeasureReqType = simple_strtol(thisChar, 0, 16);
+				if (MeasureReqType > 3)
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("%s: unknow MeasureReqType(%d)\n", __func__, MeasureReqType));
+					return TRUE;
+				}
+				break;
+
+			case 3: // Measurement channel.
+				MeasureCh = simple_strtol(thisChar, 0, 16);
+				break;
+		}
+		ArgIdx++;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s::Aid = %d, MeasureReqType=%d MeasureCh=%d\n", __func__, Aid, MeasureReqType, MeasureCh));
+	if (!VALID_WCID(Aid))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: unknow sta of Aid(%d)\n", __func__, Aid));
+		return TRUE;
+	}
+
+	MeasureReqMode.word = 0;
+	MeasureReqMode.field.Enable = 1;
+
+	MeasureReqInsert(pAd, MeasureReqToken);
+
+	EnqueueMeasurementReq(pAd, pAd->MacTab.Content[Aid].Addr,
+		MeasureReqToken, MeasureReqMode.word, MeasureReqType, MeasureCh, 2000);
+
+	return TRUE;
+}
+
+INT Set_TpcReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT Aid;
+
+	UINT8 TpcReqToken = RandomByte(pAd);
+
+	Aid = simple_strtol(arg, 0, 16);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s::Aid = %d\n", __func__, Aid));
+	if (!VALID_WCID(Aid))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: unknow sta of Aid(%d)\n", __func__, Aid));
+		return TRUE;
+	}
+
+	TpcReqInsert(pAd, TpcReqToken);
+
+	EnqueueTPCReq(pAd, pAd->MacTab.Content[Aid].Addr, TpcReqToken);
+
+	return TRUE;
+}
+
diff --git a/drivers/staging/rt2870/dfs.h b/drivers/staging/rt2870/dfs.h
new file mode 100644
index 000000000000..752a6352d9dd
--- /dev/null
+++ b/drivers/staging/rt2870/dfs.h
@@ -0,0 +1,100 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    dfs.h
+
+    Abstract:
+    Support DFS function.
+
+    Revision History:
+    Who       When            What
+    --------  ----------      ----------------------------------------------
+    Fonchi    03-12-2007      created
+*/
+
+#define RADAR_PULSE 1
+#define RADAR_WIDTH 2
+
+#define WIDTH_RD_IDLE 0
+#define WIDTH_RD_CHECK 1
+
+
+VOID BbpRadarDetectionStart(
+	IN PRTMP_ADAPTER pAd);
+
+VOID BbpRadarDetectionStop(
+	IN PRTMP_ADAPTER pAd);
+
+VOID RadarDetectionStart(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN CTS_Protect,
+	IN UINT8 CTSPeriod);
+
+VOID RadarDetectionStop(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID RadarDetectPeriodic(
+	IN PRTMP_ADAPTER	pAd);
+
+
+BOOLEAN RadarChannelCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			Ch);
+
+ULONG JapRadarType(
+	IN PRTMP_ADAPTER pAd);
+
+ULONG RTMPBbpReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd);
+
+ULONG RTMPReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID RTMPCleanRadarDuration(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID RTMPPrepareRDCTSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA,
+	IN	ULONG			Duration,
+	IN  UCHAR           RTSRate,
+	IN  ULONG           CTSBaseAddr,
+	IN  UCHAR			FrameGap);
+
+VOID RTMPPrepareRadarDetectParams(
+	IN PRTMP_ADAPTER	pAd);
+
+
+INT Set_ChMovingTime_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg);
+
+INT Set_LongPulseRadarTh_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg);
+
+
diff --git a/drivers/staging/rt2870/leap.h b/drivers/staging/rt2870/leap.h
new file mode 100644
index 000000000000..6818c1ff4d73
--- /dev/null
+++ b/drivers/staging/rt2870/leap.h
@@ -0,0 +1,215 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	leap.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#ifndef __LEAP_H__
+#define __LEAP_H__
+
+// Messages for Associate state machine
+#define LEAP_MACHINE_BASE                   30
+
+#define LEAP_MSG_REQUEST_IDENTITY           31
+#define LEAP_MSG_REQUEST_LEAP               32
+#define LEAP_MSG_SUCCESS                    33
+#define LEAP_MSG_FAILED                     34
+#define LEAP_MSG_RESPONSE_LEAP              35
+#define LEAP_MSG_EAPOLKEY                   36
+#define LEAP_MSG_UNKNOWN                    37
+#define LEAP_MSG                            38
+//! assoc state-machine states
+#define LEAP_IDLE                           0
+#define LEAP_WAIT_IDENTITY_REQUEST          1
+#define LEAP_WAIT_CHANLLENGE_REQUEST        2
+#define LEAP_WAIT_SUCCESS                   3
+#define LEAP_WAIT_CHANLLENGE_RESPONSE       4
+#define LEAP_WAIT_EAPOLKEY                  5
+
+#define LEAP_REASON_INVALID_AUTH                    0x01
+#define LEAP_REASON_AUTH_TIMEOUT                    0x02
+#define LEAP_REASON_CHALLENGE_FROM_AP_FAILED        0x03
+#define LEAP_REASON_CHALLENGE_TO_AP_FAILED          0x04
+
+#define CISCO_AuthModeLEAP                          0x80
+#define CISCO_AuthModeLEAPNone                      0x00
+#define LEAP_AUTH_TIMEOUT                           30000
+#define LEAP_CHALLENGE_RESPONSE_LENGTH              24
+#define LEAP_CHALLENGE_REQUEST_LENGTH               8
+
+typedef struct _LEAP_EAPOL_HEADER_ {
+    UCHAR       Version;
+    UCHAR       Type;
+    UCHAR       Length[2];
+} LEAP_EAPOL_HEADER, *PLEAP_EAPOL_HEADER;
+
+typedef struct _LEAP_EAPOL_PACKET_ {
+    UCHAR       Code;
+    UCHAR       Identifier;
+    UCHAR       Length[2];
+    UCHAR       Type;
+} LEAP_EAPOL_PACKET, *PLEAP_EAPOL_PACKET;
+
+typedef struct _LEAP_EAP_CONTENTS_ {
+    UCHAR       Version;
+    UCHAR       Reserved;
+    UCHAR       Length;
+} LEAP_EAP_CONTENTS, *PLEAP_EAP_CONTENTS;
+
+/*** EAPOL key ***/
+typedef struct _EAPOL_KEY_HEADER_ {
+    UCHAR       Type;
+    UCHAR       Length[2];
+    UCHAR       Counter[8];
+    UCHAR       IV[16];
+    UCHAR       Index;
+    UCHAR       Signature[16];
+} EAPOL_KEY_HEADER, *PEAPOL_KEY_HEADER;
+
+BOOLEAN LeapMsgTypeSubst(
+    IN  UCHAR   EAPType,
+    OUT ULONG   *MsgType);
+
+VOID LeapMachinePerformAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN STATE_MACHINE    *S,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapMacHeaderInit(
+    IN  PRTMP_ADAPTER       pAd,
+    IN  OUT PHEADER_802_11  pHdr80211,
+    IN  UCHAR               wep,
+    IN  PUCHAR              pAddr3);
+
+VOID LeapStartAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapIdentityAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapPeerChallengeAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID HashPwd(
+    IN  PUCHAR  pwd,
+    IN  INT     pwdlen,
+    OUT PUCHAR  hash);
+
+VOID PeerChallengeResponse(
+    IN  PUCHAR  szChallenge,
+    IN  PUCHAR  smbPasswd,
+    OUT PUCHAR  szResponse);
+
+VOID ParityKey(
+    OUT PUCHAR  szOut,
+    IN  PUCHAR  szIn);
+
+VOID DesKey(
+    OUT ULONG   k[16][2],
+    IN  PUCHAR  key,
+    IN  INT     decrypt);
+
+VOID Des(
+    IN  ULONG   ks[16][2],
+    OUT UCHAR   block[8]);
+
+VOID DesEncrypt(
+    IN  PUCHAR  szClear,
+    IN  PUCHAR  szKey,
+    OUT PUCHAR  szOut);
+
+VOID LeapNetworkChallengeAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapNetworkChallengeResponse(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID HashpwdHash(
+    IN  PUCHAR  hash,
+    IN  PUCHAR  hashhash);
+
+VOID ProcessSessionKey(
+    OUT PUCHAR  SessionKey,
+    IN  PUCHAR  hash2,
+    IN  PUCHAR  ChallengeToRadius,
+    IN  PUCHAR  ChallengeResponseFromRadius,
+    IN  PUCHAR  ChallengeFromRadius,
+    IN  PUCHAR  ChallengeResponseToRadius);
+
+VOID LeapEapolKeyAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID RogueApTableInit(
+    IN ROGUEAP_TABLE    *Tab);
+
+ULONG RogueApTableSearch(
+    IN ROGUEAP_TABLE    *Tab,
+    IN PUCHAR           pAddr);
+
+VOID RogueApEntrySet(
+    IN  PRTMP_ADAPTER   pAd,
+    OUT ROGUEAP_ENTRY   *pRogueAp,
+    IN PUCHAR           pAddr,
+    IN UCHAR            FaileCode);
+
+ULONG RogueApTableSetEntry(
+    IN  PRTMP_ADAPTER   pAd,
+    OUT ROGUEAP_TABLE  *Tab,
+    IN PUCHAR           pAddr,
+    IN UCHAR            FaileCode);
+
+VOID RogueApTableDeleteEntry(
+    IN OUT ROGUEAP_TABLE *Tab,
+    IN PUCHAR          pAddr);
+
+VOID LeapAuthTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID LeapSendRogueAPReport(
+    IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN CCKMAssocRspSanity(
+    IN PRTMP_ADAPTER    pAd,
+    IN VOID             *Msg,
+    IN ULONG            MsgLen);
+
+#endif  // __LEAP_H__
diff --git a/drivers/staging/rt2870/link_list.h b/drivers/staging/rt2870/link_list.h
new file mode 100644
index 000000000000..f6521133fd5e
--- /dev/null
+++ b/drivers/staging/rt2870/link_list.h
@@ -0,0 +1,134 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __LINK_LIST_H__
+#define __LINK_LIST_H__
+
+typedef struct _LIST_ENTRY
+{
+	struct _LIST_ENTRY *pNext;
+} LIST_ENTRY, *PLIST_ENTRY;
+
+typedef struct _LIST_HEADR
+{
+	PLIST_ENTRY pHead;
+	PLIST_ENTRY pTail;
+	UCHAR size;
+} LIST_HEADER, *PLIST_HEADER;
+
+static inline VOID initList(
+	IN PLIST_HEADER pList)
+{
+	pList->pHead = pList->pTail = NULL;
+	pList->size = 0;
+	return;
+}
+
+static inline VOID insertTailList(
+	IN PLIST_HEADER pList,
+	IN PLIST_ENTRY pEntry)
+{
+	pEntry->pNext = NULL;
+	if (pList->pTail)
+		pList->pTail->pNext = pEntry;
+	else
+		pList->pHead = pEntry;
+	pList->pTail = pEntry;
+	pList->size++;
+
+	return;
+}
+
+static inline PLIST_ENTRY removeHeadList(
+	IN PLIST_HEADER pList)
+{
+	PLIST_ENTRY pNext;
+	PLIST_ENTRY pEntry;
+
+	pEntry = pList->pHead;
+	if (pList->pHead != NULL)
+	{
+		pNext = pList->pHead->pNext;
+		pList->pHead = pNext;
+		if (pNext == NULL)
+			pList->pTail = NULL;
+		pList->size--;
+	}
+	return pEntry;
+}
+
+static inline int getListSize(
+	IN PLIST_HEADER pList)
+{
+	return pList->size;
+}
+
+static inline PLIST_ENTRY delEntryList(
+	IN PLIST_HEADER pList,
+	IN PLIST_ENTRY pEntry)
+{
+	PLIST_ENTRY pCurEntry;
+	PLIST_ENTRY pPrvEntry;
+
+	if(pList->pHead == NULL)
+		return NULL;
+
+	if(pEntry == pList->pHead)
+	{
+		pCurEntry = pList->pHead;
+		pList->pHead = pCurEntry->pNext;
+
+		if(pList->pHead == NULL)
+			pList->pTail = NULL;
+
+		pList->size--;
+		return pCurEntry;
+	}
+
+	pPrvEntry = pList->pHead;
+	pCurEntry = pPrvEntry->pNext;
+	while(pCurEntry != NULL)
+	{
+		if (pEntry == pCurEntry)
+		{
+			pPrvEntry->pNext = pCurEntry->pNext;
+
+			if(pEntry == pList->pTail)
+				pList->pTail = pPrvEntry;
+
+			pList->size--;
+			break;
+		}
+		pPrvEntry = pCurEntry;
+		pCurEntry = pPrvEntry->pNext;
+	}
+
+	return pCurEntry;
+}
+
+#endif // ___LINK_LIST_H__ //
+
diff --git a/drivers/staging/rt2870/md4.h b/drivers/staging/rt2870/md4.h
new file mode 100644
index 000000000000..f1e5b526350a
--- /dev/null
+++ b/drivers/staging/rt2870/md4.h
@@ -0,0 +1,42 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __MD4_H__
+#define __MD4_H__
+
+/* MD4 context. */
+typedef	struct	_MD4_CTX_	{
+	ULONG	state[4];        /* state (ABCD) */
+	ULONG	count[2];        /* number of bits, modulo 2^64 (lsb first) */
+	UCHAR	buffer[64];      /* input buffer */
+}	MD4_CTX;
+
+VOID MD4Init (MD4_CTX *);
+VOID MD4Update (MD4_CTX *, PUCHAR, UINT);
+VOID MD4Final (UCHAR [16], MD4_CTX *);
+
+#endif //__MD4_H__
\ No newline at end of file
diff --git a/drivers/staging/rt2870/md5.h b/drivers/staging/rt2870/md5.h
new file mode 100644
index 000000000000..d85db12170d5
--- /dev/null
+++ b/drivers/staging/rt2870/md5.h
@@ -0,0 +1,107 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	md5.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	jan			10-28-03		Initial
+	Rita    	11-23-04		Modify MD5 and SHA-1
+*/
+
+#ifndef	uint8
+#define	uint8  unsigned	char
+#endif
+
+#ifndef	uint32
+#define	uint32 unsigned	long int
+#endif
+
+
+#ifndef	__MD5_H__
+#define	__MD5_H__
+
+#define MD5_MAC_LEN 16
+
+typedef struct _MD5_CTX {
+    UINT32   Buf[4];             // buffers of four states
+	UCHAR   Input[64];          // input message
+	UINT32   LenInBitCount[2];   // length counter for input message, 0 up to 64 bits
+}   MD5_CTX;
+
+VOID MD5Init(MD5_CTX *pCtx);
+VOID MD5Update(MD5_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes);
+VOID MD5Final(UCHAR Digest[16], MD5_CTX *pCtx);
+VOID MD5Transform(UINT32 Buf[4], UINT32 Mes[16]);
+
+void md5_mac(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac);
+void hmac_md5(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac);
+
+//
+// SHA context
+//
+typedef	struct _SHA_CTX
+{
+	UINT32   Buf[5];             // buffers of five states
+	UCHAR   Input[80];          // input message
+	UINT32   LenInBitCount[2];   // length counter for input message, 0 up to 64 bits
+
+}	SHA_CTX;
+
+VOID SHAInit(SHA_CTX *pCtx);
+UCHAR SHAUpdate(SHA_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes);
+VOID SHAFinal(SHA_CTX *pCtx, UCHAR Digest[20]);
+VOID SHATransform(UINT32 Buf[5], UINT32 Mes[20]);
+
+#define SHA_DIGEST_LEN 20
+#endif // __MD5_H__
+
+/******************************************************************************/
+#ifndef	_AES_H
+#define	_AES_H
+
+typedef	struct
+{
+	uint32 erk[64];		/* encryption round	keys */
+	uint32 drk[64];		/* decryption round	keys */
+	int	nr;				/* number of rounds	*/
+}
+aes_context;
+
+int	 rtmp_aes_set_key( aes_context *ctx,	uint8 *key,	int	nbits );
+void rtmp_aes_encrypt( aes_context *ctx,	uint8 input[16], uint8 output[16] );
+void rtmp_aes_decrypt( aes_context *ctx,	uint8 input[16], uint8 output[16] );
+
+void F(char *password, unsigned char *ssid, int ssidlength, int iterations, int count, unsigned char *output);
+int PasswordHash(char *password, unsigned char *ssid, int ssidlength, unsigned char *output);
+
+#endif /* aes.h	*/
+
diff --git a/drivers/staging/rt2870/mlme.h b/drivers/staging/rt2870/mlme.h
new file mode 100644
index 000000000000..52fb8e1c2906
--- /dev/null
+++ b/drivers/staging/rt2870/mlme.h
@@ -0,0 +1,1471 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	mlme.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2003-08-28		Created
+	John Chang  2004-09-06      modified for RT2600
+
+*/
+#ifndef __MLME_H__
+#define __MLME_H__
+
+//extern UCHAR BROADCAST_ADDR[];
+
+// maximum supported capability information -
+// ESS, IBSS, Privacy, Short Preamble, Spectrum mgmt, Short Slot
+#define SUPPORTED_CAPABILITY_INFO   0x0533
+
+#define END_OF_ARGS                 -1
+#define LFSR_MASK                   0x80000057
+#define MLME_TASK_EXEC_INTV         100/*200*/       //
+#define LEAD_TIME                   5
+#define MLME_TASK_EXEC_MULTIPLE       10  /*5*/       // MLME_TASK_EXEC_MULTIPLE * MLME_TASK_EXEC_INTV = 1 sec
+#define REORDER_EXEC_INTV         	100       // 0.1 sec
+//#define TBTT_PRELOAD_TIME         384        // usec. LomgPreamble + 24-byte at 1Mbps
+
+// The definition of Radar detection duration region
+#define CE		0
+#define FCC		1
+#define JAP		2
+#define JAP_W53	3
+#define JAP_W56	4
+#define MAX_RD_REGION 5
+
+#ifdef	NDIS51_MINIPORT
+#define BEACON_LOST_TIME            4000       // 2048 msec = 2 sec
+#else
+#define BEACON_LOST_TIME            4 * OS_HZ    // 2048 msec = 2 sec
+#endif
+
+#define DLS_TIMEOUT                 1200      // unit: msec
+#define AUTH_TIMEOUT                300       // unit: msec
+#define ASSOC_TIMEOUT               300       // unit: msec
+#define JOIN_TIMEOUT                2 * OS_HZ      // unit: msec
+#define SHORT_CHANNEL_TIME          90        // unit: msec
+#define MIN_CHANNEL_TIME            110        // unit: msec, for dual band scan
+#define MAX_CHANNEL_TIME            140       // unit: msec, for single band scan
+#define	FAST_ACTIVE_SCAN_TIME	    30 		  // Active scan waiting for probe response time
+#define CW_MIN_IN_BITS              4         // actual CwMin = 2^CW_MIN_IN_BITS - 1
+
+
+#ifdef CONFIG_STA_SUPPORT
+#ifndef CONFIG_AP_SUPPORT
+#define CW_MAX_IN_BITS              10        // actual CwMax = 2^CW_MAX_IN_BITS - 1
+#endif
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+extern UINT32 CW_MAX_IN_BITS;
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+// Note: RSSI_TO_DBM_OFFSET has been changed to variable for new RF (2004-0720).
+// SHould not refer to this constant anymore
+//#define RSSI_TO_DBM_OFFSET          120 // for RT2530 RSSI-115 = dBm
+#define RSSI_FOR_MID_TX_POWER       -55  // -55 db is considered mid-distance
+#define RSSI_FOR_LOW_TX_POWER       -45  // -45 db is considered very short distance and
+                                        // eligible to use a lower TX power
+#define RSSI_FOR_LOWEST_TX_POWER    -30
+//#define MID_TX_POWER_DELTA          0   // 0 db from full TX power upon mid-distance to AP
+#define LOW_TX_POWER_DELTA          6    // -3 db from full TX power upon very short distance. 1 grade is 0.5 db
+#define LOWEST_TX_POWER_DELTA       16   // -8 db from full TX power upon shortest distance. 1 grade is 0.5 db
+
+#define RSSI_TRIGGERED_UPON_BELOW_THRESHOLD     0
+#define RSSI_TRIGGERED_UPON_EXCCEED_THRESHOLD   1
+#define RSSI_THRESHOLD_FOR_ROAMING              25
+#define RSSI_DELTA                              5
+
+// Channel Quality Indication
+#define CQI_IS_GOOD(cqi)            ((cqi) >= 50)
+//#define CQI_IS_FAIR(cqi)          (((cqi) >= 20) && ((cqi) < 50))
+#define CQI_IS_POOR(cqi)            (cqi < 50)  //(((cqi) >= 5) && ((cqi) < 20))
+#define CQI_IS_BAD(cqi)             (cqi < 5)
+#define CQI_IS_DEAD(cqi)            (cqi == 0)
+
+// weighting factor to calculate Channel quality, total should be 100%
+#define RSSI_WEIGHTING                   50
+#define TX_WEIGHTING                     30
+#define RX_WEIGHTING                     20
+
+//#define PEER_KEY_NOT_USED                0
+//#define PEER_KEY_64_BIT                  64
+//#define PEER_KEY_128_BIT                 128
+
+//#define PEER_KEY_64BIT_LEN               8
+//#define PEER_KEY_128BIT_LEN              16
+
+#define BSS_NOT_FOUND                    0xFFFFFFFF
+
+
+#ifdef CONFIG_STA_SUPPORT
+#define MAX_LEN_OF_MLME_QUEUE            40 //10
+#endif // CONFIG_STA_SUPPORT //
+
+#define SCAN_PASSIVE                     18		// scan with no probe request, only wait beacon and probe response
+#define SCAN_ACTIVE                      19		// scan with probe request, and wait beacon and probe response
+#define	SCAN_CISCO_PASSIVE				 20		// Single channel passive scan
+#define	SCAN_CISCO_ACTIVE				 21		// Single channel active scan
+#define	SCAN_CISCO_NOISE				 22		// Single channel passive scan for noise histogram collection
+#define	SCAN_CISCO_CHANNEL_LOAD			 23		// Single channel passive scan for channel load collection
+#define FAST_SCAN_ACTIVE                 24		// scan with probe request, and wait beacon and probe response
+
+#ifdef DOT11N_DRAFT3
+#define SCAN_2040_BSS_COEXIST                  26
+#endif // DOT11N_DRAFT3 //
+
+//#define BSS_TABLE_EMPTY(x)             ((x).BssNr == 0)
+#define MAC_ADDR_IS_GROUP(Addr)       (((Addr[0]) & 0x01))
+#define MAC_ADDR_HASH(Addr)            (Addr[0] ^ Addr[1] ^ Addr[2] ^ Addr[3] ^ Addr[4] ^ Addr[5])
+#define MAC_ADDR_HASH_INDEX(Addr)      (MAC_ADDR_HASH(Addr) % HASH_TABLE_SIZE)
+#define TID_MAC_HASH(Addr,TID)            (TID^Addr[0] ^ Addr[1] ^ Addr[2] ^ Addr[3] ^ Addr[4] ^ Addr[5])
+#define TID_MAC_HASH_INDEX(Addr,TID)      (TID_MAC_HASH(Addr,TID) % HASH_TABLE_SIZE)
+
+// LED Control
+// assoiation ON. one LED ON. another blinking when TX, OFF when idle
+// no association, both LED off
+#define ASIC_LED_ACT_ON(pAd)        RTMP_IO_WRITE32(pAd, MAC_CSR14, 0x00031e46)
+#define ASIC_LED_ACT_OFF(pAd)       RTMP_IO_WRITE32(pAd, MAC_CSR14, 0x00001e46)
+
+// bit definition of the 2-byte pBEACON->Capability field
+#define CAP_IS_ESS_ON(x)                 (((x) & 0x0001) != 0)
+#define CAP_IS_IBSS_ON(x)                (((x) & 0x0002) != 0)
+#define CAP_IS_CF_POLLABLE_ON(x)         (((x) & 0x0004) != 0)
+#define CAP_IS_CF_POLL_REQ_ON(x)         (((x) & 0x0008) != 0)
+#define CAP_IS_PRIVACY_ON(x)             (((x) & 0x0010) != 0)
+#define CAP_IS_SHORT_PREAMBLE_ON(x)      (((x) & 0x0020) != 0)
+#define CAP_IS_PBCC_ON(x)                (((x) & 0x0040) != 0)
+#define CAP_IS_AGILITY_ON(x)             (((x) & 0x0080) != 0)
+#define CAP_IS_SPECTRUM_MGMT(x)          (((x) & 0x0100) != 0)  // 802.11e d9
+#define CAP_IS_QOS(x)                    (((x) & 0x0200) != 0)  // 802.11e d9
+#define CAP_IS_SHORT_SLOT(x)             (((x) & 0x0400) != 0)
+#define CAP_IS_APSD(x)                   (((x) & 0x0800) != 0)  // 802.11e d9
+#define CAP_IS_IMMED_BA(x)               (((x) & 0x1000) != 0)  // 802.11e d9
+#define CAP_IS_DSSS_OFDM(x)              (((x) & 0x2000) != 0)
+#define CAP_IS_DELAY_BA(x)               (((x) & 0x4000) != 0)  // 802.11e d9
+
+#define CAP_GENERATE(ess,ibss,priv,s_pre,s_slot,spectrum)  (((ess) ? 0x0001 : 0x0000) | ((ibss) ? 0x0002 : 0x0000) | ((priv) ? 0x0010 : 0x0000) | ((s_pre) ? 0x0020 : 0x0000) | ((s_slot) ? 0x0400 : 0x0000) | ((spectrum) ? 0x0100 : 0x0000))
+
+//#define STA_QOS_CAPABILITY               0 // 1-byte. see 802.11e d9.0 for bit definition
+
+#define ERP_IS_NON_ERP_PRESENT(x)        (((x) & 0x01) != 0)    // 802.11g
+#define ERP_IS_USE_PROTECTION(x)         (((x) & 0x02) != 0)    // 802.11g
+#define ERP_IS_USE_BARKER_PREAMBLE(x)    (((x) & 0x04) != 0)    // 802.11g
+
+#define DRS_TX_QUALITY_WORST_BOUND       8// 3  // just test by gary
+#define DRS_PENALTY                      8
+
+#define BA_NOTUSE 	2
+//BA Policy subfiled value in ADDBA frame
+#define IMMED_BA 	1
+#define DELAY_BA	0
+
+// BA Initiator subfield in DELBA frame
+#define ORIGINATOR	1
+#define RECIPIENT	0
+
+// ADDBA Status Code
+#define ADDBA_RESULTCODE_SUCCESS					0
+#define ADDBA_RESULTCODE_REFUSED					37
+#define ADDBA_RESULTCODE_INVALID_PARAMETERS			38
+
+// DELBA Reason Code
+#define DELBA_REASONCODE_QSTA_LEAVING				36
+#define DELBA_REASONCODE_END_BA						37
+#define DELBA_REASONCODE_UNKNOWN_BA					38
+#define DELBA_REASONCODE_TIMEOUT					39
+
+// reset all OneSecTx counters
+#define RESET_ONE_SEC_TX_CNT(__pEntry) \
+if (((__pEntry)) != NULL) \
+{ \
+	(__pEntry)->OneSecTxRetryOkCount = 0; \
+	(__pEntry)->OneSecTxFailCount = 0; \
+	(__pEntry)->OneSecTxNoRetryOkCount = 0; \
+}
+
+//
+// 802.11 frame formats
+//
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	USHORT	LSIGTxopProSup:1;
+	USHORT	Forty_Mhz_Intolerant:1;
+	USHORT	PSMP:1;
+	USHORT	CCKmodein40:1;
+	USHORT	AMsduSize:1;
+	USHORT	DelayedBA:1;	//rt2860c not support
+	USHORT	RxSTBC:2;
+	USHORT	TxSTBC:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	ShortGIfor20:1;
+	USHORT	GF:1;	//green field
+	USHORT	MimoPs:2;//momi power safe
+	USHORT	ChannelWidth:1;
+	USHORT	AdvCoding:1;
+#else
+	USHORT	AdvCoding:1;
+	USHORT	ChannelWidth:1;
+	USHORT	MimoPs:2;//momi power safe
+	USHORT	GF:1;	//green field
+	USHORT	ShortGIfor20:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	TxSTBC:1;
+	USHORT	RxSTBC:2;
+	USHORT	DelayedBA:1;	//rt2860c not support
+	USHORT	AMsduSize:1;	// only support as zero
+	USHORT	CCKmodein40:1;
+	USHORT	PSMP:1;
+	USHORT	Forty_Mhz_Intolerant:1;
+	USHORT	LSIGTxopProSup:1;
+#endif	/* !RT_BIG_ENDIAN */
+} HT_CAP_INFO, *PHT_CAP_INFO;
+
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:3;//momi power safe
+	UCHAR	MpduDensity:3;
+	UCHAR	MaxRAmpduFactor:2;
+#else
+	UCHAR	MaxRAmpduFactor:2;
+	UCHAR	MpduDensity:3;
+	UCHAR	rsv:3;//momi power safe
+#endif /* !RT_BIG_ENDIAN */
+} HT_CAP_PARM, *PHT_CAP_PARM;
+
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+	UCHAR	MCSSet[10];
+	UCHAR	SupRate[2];  // unit : 1Mbps
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:3;
+	UCHAR	MpduDensity:1;
+	UCHAR	TxStream:2;
+	UCHAR	TxRxNotEqual:1;
+	UCHAR	TxMCSSetDefined:1;
+#else
+	UCHAR	TxMCSSetDefined:1;
+	UCHAR	TxRxNotEqual:1;
+	UCHAR	TxStream:2;
+	UCHAR	MpduDensity:1;
+	UCHAR	rsv:3;
+#endif // RT_BIG_ENDIAN //
+	UCHAR	rsv3[3];
+} HT_MCS_SET, *PHT_MCS_SET;
+
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv2:4;
+	USHORT	RDGSupport:1;	//reverse Direction Grant  support
+	USHORT	PlusHTC:1;	//+HTC control field support
+	USHORT	MCSFeedback:2;	//0:no MCS feedback, 2:unsolicited MCS feedback, 3:Full MCS feedback,  1:rsv.
+	USHORT	rsv:5;//momi power safe
+	USHORT	TranTime:2;
+	USHORT	Pco:1;
+#else
+	USHORT	Pco:1;
+	USHORT	TranTime:2;
+	USHORT	rsv:5;//momi power safe
+	USHORT	MCSFeedback:2;	//0:no MCS feedback, 2:unsolicited MCS feedback, 3:Full MCS feedback,  1:rsv.
+	USHORT	PlusHTC:1;	//+HTC control field support
+	USHORT	RDGSupport:1;	//reverse Direction Grant  support
+	USHORT	rsv2:4;
+#endif /* RT_BIG_ENDIAN */
+} EXT_HT_CAP_INFO, *PEXT_HT_CAP_INFO;
+
+//  HT Beamforming field in HT Cap IE .
+typedef struct PACKED _HT_BF_CAP{
+#ifdef RT_BIG_ENDIAN
+	ULONG	rsv:3;
+	ULONG	ChanEstimation:2;
+	ULONG	CSIRowBFSup:2;
+	ULONG	ComSteerBFAntSup:2;
+	ULONG	NoComSteerBFAntSup:2;
+	ULONG	CSIBFAntSup:2;
+	ULONG	MinGrouping:2;
+	ULONG	ExpComBF:2;
+	ULONG	ExpNoComBF:2;
+	ULONG	ExpCSIFbk:2;
+	ULONG	ExpComSteerCapable:1;
+	ULONG	ExpNoComSteerCapable:1;
+	ULONG	ExpCSICapable:1;
+	ULONG	Calibration:2;
+	ULONG	ImpTxBFCapable:1;
+	ULONG	TxNDPCapable:1;
+	ULONG	RxNDPCapable:1;
+	ULONG	TxSoundCapable:1;
+	ULONG	RxSoundCapable:1;
+	ULONG	TxBFRecCapable:1;
+#else
+	ULONG	TxBFRecCapable:1;
+	ULONG	RxSoundCapable:1;
+	ULONG	TxSoundCapable:1;
+	ULONG	RxNDPCapable:1;
+	ULONG	TxNDPCapable:1;
+	ULONG	ImpTxBFCapable:1;
+	ULONG	Calibration:2;
+	ULONG	ExpCSICapable:1;
+	ULONG	ExpNoComSteerCapable:1;
+	ULONG	ExpComSteerCapable:1;
+	ULONG	ExpCSIFbk:2;
+	ULONG	ExpNoComBF:2;
+	ULONG	ExpComBF:2;
+	ULONG	MinGrouping:2;
+	ULONG	CSIBFAntSup:2;
+	ULONG	NoComSteerBFAntSup:2;
+	ULONG	ComSteerBFAntSup:2;
+	ULONG	CSIRowBFSup:2;
+	ULONG	ChanEstimation:2;
+	ULONG	rsv:3;
+#endif // RT_BIG_ENDIAN //
+} HT_BF_CAP, *PHT_BF_CAP;
+
+//  HT antenna selection field in HT Cap IE .
+typedef struct PACKED _HT_AS_CAP{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:1;
+	UCHAR	TxSoundPPDU:1;
+	UCHAR	RxASel:1;
+	UCHAR	AntIndFbk:1;
+	UCHAR	ExpCSIFbk:1;
+	UCHAR	AntIndFbkTxASEL:1;
+	UCHAR	ExpCSIFbkTxASEL:1;
+	UCHAR	AntSelect:1;
+#else
+	UCHAR	AntSelect:1;
+	UCHAR	ExpCSIFbkTxASEL:1;
+	UCHAR	AntIndFbkTxASEL:1;
+	UCHAR	ExpCSIFbk:1;
+	UCHAR	AntIndFbk:1;
+	UCHAR	RxASel:1;
+	UCHAR	TxSoundPPDU:1;
+	UCHAR	rsv:1;
+#endif // RT_BIG_ENDIAN //
+} HT_AS_CAP, *PHT_AS_CAP;
+
+// Draft 1.0 set IE length 26, but is extensible..
+#define SIZE_HT_CAP_IE		26
+// The structure for HT Capability IE.
+typedef struct PACKED _HT_CAPABILITY_IE{
+	HT_CAP_INFO		HtCapInfo;
+	HT_CAP_PARM		HtCapParm;
+//	HT_MCS_SET		HtMCSSet;
+	UCHAR			MCSSet[16];
+	EXT_HT_CAP_INFO	ExtHtCapInfo;
+	HT_BF_CAP		TxBFCap;	// beamforming cap. rt2860c not support beamforming.
+	HT_AS_CAP		ASCap;	//antenna selection.
+} HT_CAPABILITY_IE, *PHT_CAPABILITY_IE;
+
+
+// 802.11n draft3 related structure definitions.
+// 7.3.2.60
+#define dot11OBSSScanPassiveDwell							20	// in TU. min amount of time that the STA continously scans each channel when performing an active OBSS scan.
+#define dot11OBSSScanActiveDwell							10	// in TU.min amount of time that the STA continously scans each channel when performing an passive OBSS scan.
+#define dot11BSSWidthTriggerScanInterval					300  // in sec. max interval between scan operations to be performed to detect BSS channel width trigger events.
+#define dot11OBSSScanPassiveTotalPerChannel					200	// in TU. min total amount of time that the STA scans each channel when performing a passive OBSS scan.
+#define dot11OBSSScanActiveTotalPerChannel					20	//in TU. min total amount of time that the STA scans each channel when performing a active OBSS scan
+#define dot11BSSWidthChannelTransactionDelayFactor			5	// min ratio between the delay time in performing a switch from 20MHz BSS to 20/40 BSS operation and the maxima
+																//	interval between overlapping BSS scan operations.
+#define dot11BSSScanActivityThreshold						25	// in %%, max total time that a STA may be active on the medium during a period of
+																//	(dot11BSSWidthChannelTransactionDelayFactor * dot11BSSWidthTriggerScanInterval) seconds without
+																//	being obligated to perform OBSS Scan operations. default is 25(== 0.25%)
+
+typedef struct PACKED _OVERLAP_BSS_SCAN_IE{
+	USHORT		ScanPassiveDwell;
+	USHORT		ScanActiveDwell;
+	USHORT		TriggerScanInt;				// Trigger scan interval
+	USHORT		PassiveTalPerChannel;		// passive total per channel
+	USHORT		ActiveTalPerChannel;		// active total per channel
+	USHORT		DelayFactor;				// BSS width channel transition delay factor
+	USHORT		ScanActThre;				// Scan Activity threshold
+}OVERLAP_BSS_SCAN_IE, *POVERLAP_BSS_SCAN_IE;
+
+
+//  7.3.2.56. 20/40 Coexistence element used in  Element ID = 72 = IE_2040_BSS_COEXIST
+typedef union PACKED _BSS_2040_COEXIST_IE{
+ struct PACKED {
+ #ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:5;
+	UCHAR	BSS20WidthReq:1;
+	UCHAR	Intolerant40:1;
+	UCHAR	InfoReq:1;
+ #else
+	UCHAR	InfoReq:1;
+	UCHAR	Intolerant40:1;			// Inter-BSS. set 1 when prohibits a receiving BSS from operating as a 20/40 Mhz BSS.
+	UCHAR	BSS20WidthReq:1;		// Intra-BSS set 1 when prohibits a receiving AP from operating its BSS as a 20/40MHz BSS.
+	UCHAR	rsv:5;
+#endif // RT_BIG_ENDIAN //
+    } field;
+ UCHAR   word;
+} BSS_2040_COEXIST_IE, *PBSS_2040_COEXIST_IE;
+
+
+typedef struct  _TRIGGER_EVENTA{
+	BOOLEAN			bValid;
+	UCHAR	BSSID[6];
+	UCHAR	RegClass;	// Regulatory Class
+	USHORT	Channel;
+	ULONG	CDCounter;   // Maintain a seperate count down counter for each Event A.
+} TRIGGER_EVENTA, *PTRIGGER_EVENTA;
+
+// 20/40 trigger event table
+// If one Event A delete or created, or if Event B is detected or not detected, STA should send 2040BSSCoexistence to AP.
+#define MAX_TRIGGER_EVENT		64
+typedef struct  _TRIGGER_EVENT_TAB{
+	UCHAR	EventANo;
+	TRIGGER_EVENTA	EventA[MAX_TRIGGER_EVENT];
+	ULONG			EventBCountDown;	// Count down counter for Event B.
+} TRIGGER_EVENT_TAB, *PTRIGGER_EVENT_TAB;
+
+// 7.3.27 20/40 Bss Coexistence Mgmt capability used in extended capabilities information IE( ID = 127 = IE_EXT_CAPABILITY).
+//	This is the first octet and was defined in 802.11n D3.03 and 802.11yD9.0
+typedef struct PACKED _EXT_CAP_INFO_ELEMENT{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv2:5;
+	UCHAR	ExtendChannelSwitch:1;
+	UCHAR	rsv:1;
+	UCHAR	BssCoexistMgmtSupport:1;
+#else
+	UCHAR	BssCoexistMgmtSupport:1;
+	UCHAR	rsv:1;
+	UCHAR	ExtendChannelSwitch:1;
+	UCHAR	rsv2:5;
+#endif // RT_BIG_ENDIAN //
+}EXT_CAP_INFO_ELEMENT, *PEXT_CAP_INFO_ELEMENT;
+
+
+// 802.11n 7.3.2.61
+typedef struct PACKED _BSS_2040_COEXIST_ELEMENT{
+	UCHAR					ElementID;		// ID = IE_2040_BSS_COEXIST = 72
+	UCHAR					Len;
+	BSS_2040_COEXIST_IE		BssCoexistIe;
+}BSS_2040_COEXIST_ELEMENT, *PBSS_2040_COEXIST_ELEMENT;
+
+
+//802.11n 7.3.2.59
+typedef struct PACKED _BSS_2040_INTOLERANT_CH_REPORT{
+	UCHAR				ElementID;		// ID = IE_2040_BSS_INTOLERANT_REPORT = 73
+	UCHAR				Len;
+	UCHAR				RegulatoryClass;
+	UCHAR				ChList[0];
+}BSS_2040_INTOLERANT_CH_REPORT, *PBSS_2040_INTOLERANT_CH_REPORT;
+
+
+// The structure for channel switch annoucement IE. This is in 802.11n D3.03
+typedef struct PACKED _CHA_SWITCH_ANNOUNCE_IE{
+	UCHAR			SwitchMode;	//channel switch mode
+	UCHAR			NewChannel;	//
+	UCHAR			SwitchCount;	//
+} CHA_SWITCH_ANNOUNCE_IE, *PCHA_SWITCH_ANNOUNCE_IE;
+
+
+// The structure for channel switch annoucement IE. This is in 802.11n D3.03
+typedef struct PACKED _SEC_CHA_OFFSET_IE{
+	UCHAR			SecondaryChannelOffset;	 // 1: Secondary above, 3: Secondary below, 0: no Secondary
+} SEC_CHA_OFFSET_IE, *PSEC_CHA_OFFSET_IE;
+
+
+// This structure is extracted from struct RT_HT_CAPABILITY
+typedef struct {
+	BOOLEAN			bHtEnable;	 // If we should use ht rate.
+	BOOLEAN			bPreNHt;	 // If we should use ht rate.
+	//Substract from HT Capability IE
+	UCHAR			MCSSet[16];	//only supoort MCS=0-15,32 ,
+} RT_HT_PHY_INFO, *PRT_HT_PHY_INFO;
+
+//This structure substracts ralink supports from all 802.11n-related features.
+//Features not listed here but contained in 802.11n spec are not supported in rt2860.
+typedef struct {
+#if 0	// move to
+	BOOLEAN			bHtEnable;	 // If we should use ht rate.
+	BOOLEAN			bPreNHt;	 // If we should use ht rate.
+	//Substract from HT Capability IE
+	UCHAR			MCSSet[16];	//only supoort MCS=0-15,32 ,
+#endif
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv:5;
+	USHORT	AmsduSize:1;	// Max receiving A-MSDU size
+	USHORT	AmsduEnable:1;	// Enable to transmit A-MSDU. Suggest disable. We should use A-MPDU to gain best benifit of 802.11n
+	USHORT	RxSTBC:2;	// 2 bits
+	USHORT	TxSTBC:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	ShortGIfor20:1;
+	USHORT	GF:1;	//green field
+	USHORT	MimoPs:2;//mimo power safe MMPS_
+	USHORT	ChannelWidth:1;
+#else
+	USHORT	ChannelWidth:1;
+	USHORT	MimoPs:2;//mimo power safe MMPS_
+	USHORT	GF:1;	//green field
+	USHORT	ShortGIfor20:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	TxSTBC:1;
+	USHORT	RxSTBC:2;	// 2 bits
+	USHORT	AmsduEnable:1;	// Enable to transmit A-MSDU. Suggest disable. We should use A-MPDU to gain best benifit of 802.11n
+	USHORT	AmsduSize:1;	// Max receiving A-MSDU size
+	USHORT	rsv:5;
+#endif
+
+	//Substract from Addiont HT INFO IE
+#ifdef RT_BIG_ENDIAN
+	UCHAR	RecomWidth:1;
+	UCHAR	ExtChanOffset:2;	// Please not the difference with following 	UCHAR	NewExtChannelOffset; from 802.11n
+	UCHAR	MpduDensity:3;
+	UCHAR	MaxRAmpduFactor:2;
+#else
+	UCHAR	MaxRAmpduFactor:2;
+	UCHAR	MpduDensity:3;
+	UCHAR	ExtChanOffset:2;	// Please not the difference with following 	UCHAR	NewExtChannelOffset; from 802.11n
+	UCHAR	RecomWidth:1;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv2:11;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv3:1;
+	USHORT	NonGfPresent:1;
+	USHORT	OperaionMode:2;
+#else
+	USHORT	OperaionMode:2;
+	USHORT	NonGfPresent:1;
+	USHORT	rsv3:1;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv2:11;
+#endif
+
+	// New Extension Channel Offset IE
+	UCHAR	NewExtChannelOffset;
+	// Extension Capability IE = 127
+	UCHAR	BSSCoexist2040;
+} RT_HT_CAPABILITY, *PRT_HT_CAPABILITY;
+
+//   field in Addtional HT Information IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR	SerInterGranu:3;
+	UCHAR	S_PSMPSup:1;
+	UCHAR	RifsMode:1;
+	UCHAR	RecomWidth:1;
+	UCHAR	ExtChanOffset:2;
+#else
+	UCHAR	ExtChanOffset:2;
+	UCHAR	RecomWidth:1;
+	UCHAR	RifsMode:1;
+	UCHAR	S_PSMPSup:1;	 //Indicate support for scheduled PSMP
+	UCHAR	SerInterGranu:3;	 //service interval granularity
+#endif
+} ADD_HTINFO, *PADD_HTINFO;
+
+typedef struct PACKED{
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv2:11;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv:1;
+	USHORT	NonGfPresent:1;
+	USHORT	OperaionMode:2;
+#else
+	USHORT	OperaionMode:2;
+	USHORT	NonGfPresent:1;
+	USHORT	rsv:1;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv2:11;
+#endif
+} ADD_HTINFO2, *PADD_HTINFO2;
+
+
+// TODO: Need sync with spec about the definition of StbcMcs. In Draft 3.03, it's reserved.
+typedef struct PACKED{
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv:4;
+	USHORT	PcoPhase:1;
+	USHORT	PcoActive:1;
+	USHORT	LsigTxopProt:1;
+	USHORT	STBCBeacon:1;
+	USHORT	DualCTSProtect:1;
+	USHORT	DualBeacon:1;
+	USHORT	StbcMcs:6;
+#else
+	USHORT	StbcMcs:6;
+	USHORT	DualBeacon:1;
+	USHORT	DualCTSProtect:1;
+	USHORT	STBCBeacon:1;
+	USHORT	LsigTxopProt:1;	// L-SIG TXOP protection full support
+	USHORT	PcoActive:1;
+	USHORT	PcoPhase:1;
+	USHORT	rsv:4;
+#endif // RT_BIG_ENDIAN //
+} ADD_HTINFO3, *PADD_HTINFO3;
+
+#define SIZE_ADD_HT_INFO_IE		22
+typedef struct  PACKED{
+	UCHAR				ControlChan;
+	ADD_HTINFO			AddHtInfo;
+	ADD_HTINFO2			AddHtInfo2;
+	ADD_HTINFO3			AddHtInfo3;
+	UCHAR				MCSSet[16];		// Basic MCS set
+} ADD_HT_INFO_IE, *PADD_HT_INFO_IE;
+
+typedef struct  PACKED{
+	UCHAR				NewExtChanOffset;
+} NEW_EXT_CHAN_IE, *PNEW_EXT_CHAN_IE;
+
+
+// 4-byte HTC field.  maybe included in any frame except non-QOS data frame.  The Order bit must set 1.
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    UINT32		RDG:1;	//RDG / More PPDU
+    UINT32		ACConstraint:1;	//feedback request
+    UINT32		rsv:5;  //calibration sequence
+    UINT32		ZLFAnnouce:1;	// ZLF announcement
+    UINT32		CSISTEERING:2;	//CSI/ STEERING
+    UINT32		FBKReq:2;	//feedback request
+    UINT32		CalSeq:2;  //calibration sequence
+    UINT32		CalPos:2;	// calibration position
+    UINT32		MFBorASC:7;	//Link adaptation feedback containing recommended MCS. 0x7f for no feedback or not available
+    UINT32		MFS:3;	//SET to the received value of MRS. 0x111 for unsolicited MFB.
+    UINT32		MRSorASI:3;	// MRQ Sequence identifier. unchanged during entire procedure. 0x000-0x110.
+    UINT32		MRQ:1;	//MCS feedback. Request for a MCS feedback
+    UINT32		TRQ:1;	//sounding request
+    UINT32		MA:1;	//management action payload exist in (QoS Null+HTC)
+#else
+    UINT32		MA:1;	//management action payload exist in (QoS Null+HTC)
+    UINT32		TRQ:1;	//sounding request
+    UINT32		MRQ:1;	//MCS feedback. Request for a MCS feedback
+    UINT32		MRSorASI:3;	// MRQ Sequence identifier. unchanged during entire procedure. 0x000-0x110.
+    UINT32		MFS:3;	//SET to the received value of MRS. 0x111 for unsolicited MFB.
+    UINT32		MFBorASC:7;	//Link adaptation feedback containing recommended MCS. 0x7f for no feedback or not available
+    UINT32		CalPos:2;	// calibration position
+    UINT32		CalSeq:2;  //calibration sequence
+    UINT32		FBKReq:2;	//feedback request
+    UINT32		CSISTEERING:2;	//CSI/ STEERING
+    UINT32		ZLFAnnouce:1;	// ZLF announcement
+    UINT32		rsv:5;  //calibration sequence
+    UINT32		ACConstraint:1;	//feedback request
+    UINT32		RDG:1;	//RDG / More PPDU
+#endif /* !RT_BIG_ENDIAN */
+} HT_CONTROL, *PHT_CONTROL;
+
+// 2-byte QOS CONTROL field
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      Txop_QueueSize:8;
+    USHORT      AMsduPresent:1;
+    USHORT      AckPolicy:2;  //0: normal ACK 1:No ACK 2:scheduled under MTBA/PSMP  3: BA
+    USHORT      EOSP:1;
+    USHORT      TID:4;
+#else
+    USHORT      TID:4;
+    USHORT      EOSP:1;
+    USHORT      AckPolicy:2;  //0: normal ACK 1:No ACK 2:scheduled under MTBA/PSMP  3: BA
+    USHORT      AMsduPresent:1;
+    USHORT      Txop_QueueSize:8;
+#endif /* !RT_BIG_ENDIAN */
+} QOS_CONTROL, *PQOS_CONTROL;
+
+// 2-byte Frame control field
+typedef	struct	PACKED {
+#ifdef RT_BIG_ENDIAN
+	USHORT		Order:1;			// Strict order expected
+	USHORT		Wep:1;				// Wep data
+	USHORT		MoreData:1;			// More data bit
+	USHORT		PwrMgmt:1;			// Power management bit
+	USHORT		Retry:1;			// Retry status bit
+	USHORT		MoreFrag:1;			// More fragment bit
+	USHORT		FrDs:1;				// From DS indication
+	USHORT		ToDs:1;				// To DS indication
+	USHORT		SubType:4;			// MSDU subtype
+	USHORT		Type:2;				// MSDU type
+	USHORT		Ver:2;				// Protocol version
+#else
+	USHORT		Ver:2;				// Protocol version
+	USHORT		Type:2;				// MSDU type
+	USHORT		SubType:4;			// MSDU subtype
+	USHORT		ToDs:1;				// To DS indication
+	USHORT		FrDs:1;				// From DS indication
+	USHORT		MoreFrag:1;			// More fragment bit
+	USHORT		Retry:1;			// Retry status bit
+	USHORT		PwrMgmt:1;			// Power management bit
+	USHORT		MoreData:1;			// More data bit
+	USHORT		Wep:1;				// Wep data
+	USHORT		Order:1;			// Strict order expected
+#endif /* !RT_BIG_ENDIAN */
+} FRAME_CONTROL, *PFRAME_CONTROL;
+
+typedef	struct	PACKED _HEADER_802_11	{
+    FRAME_CONTROL   FC;
+    USHORT          Duration;
+    UCHAR           Addr1[MAC_ADDR_LEN];
+    UCHAR           Addr2[MAC_ADDR_LEN];
+	UCHAR			Addr3[MAC_ADDR_LEN];
+#ifdef RT_BIG_ENDIAN
+	USHORT			Sequence:12;
+	USHORT			Frag:4;
+#else
+	USHORT			Frag:4;
+	USHORT			Sequence:12;
+#endif /* !RT_BIG_ENDIAN */
+	UCHAR			Octet[0];
+}	HEADER_802_11, *PHEADER_802_11;
+
+typedef struct PACKED _FRAME_802_11 {
+    HEADER_802_11   Hdr;
+    UCHAR            Octet[1];
+}   FRAME_802_11, *PFRAME_802_11;
+
+// QoSNull embedding of management action. When HT Control MA field set to 1.
+typedef struct PACKED _MA_BODY {
+    UCHAR            Category;
+    UCHAR            Action;
+    UCHAR            Octet[1];
+}   MA_BODY, *PMA_BODY;
+
+typedef	struct	PACKED _HEADER_802_3	{
+    UCHAR           DAAddr1[MAC_ADDR_LEN];
+    UCHAR           SAAddr2[MAC_ADDR_LEN];
+    UCHAR           Octet[2];
+}	HEADER_802_3, *PHEADER_802_3;
+////Block ACK related format
+// 2-byte BA Parameter  field  in 	DELBA frames to terminate an already set up bA
+typedef struct PACKED{
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;	// value of TC os TS
+    USHORT      Initiator:1;	// 1: originator    0:recipient
+    USHORT      Rsv:11;	// always set to 0
+#else
+    USHORT      Rsv:11;	// always set to 0
+    USHORT      Initiator:1;	// 1: originator    0:recipient
+    USHORT      TID:4;	// value of TC os TS
+#endif /* !RT_BIG_ENDIAN */
+} DELBA_PARM, *PDELBA_PARM;
+
+// 2-byte BA Parameter Set field  in ADDBA frames to signal parm for setting up a BA
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      BufSize:10;	// number of buffe of size 2304 octetsr
+    USHORT      TID:4;	// value of TC os TS
+    USHORT      BAPolicy:1;	// 1: immediately BA    0:delayed BA
+    USHORT      AMSDUSupported:1;	// 0: not permitted		1: permitted
+#else
+    USHORT      AMSDUSupported:1;	// 0: not permitted		1: permitted
+    USHORT      BAPolicy:1;	// 1: immediately BA    0:delayed BA
+    USHORT      TID:4;	// value of TC os TS
+    USHORT      BufSize:10;	// number of buffe of size 2304 octetsr
+#endif /* !RT_BIG_ENDIAN */
+} BA_PARM, *PBA_PARM;
+
+// 2-byte BA Starting Seq CONTROL field
+typedef union PACKED {
+    struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      StartSeq:12;   // sequence number of the 1st MSDU for which this BAR is sent
+	USHORT      FragNum:4;	// always set to 0
+#else
+    USHORT      FragNum:4;	// always set to 0
+	USHORT      StartSeq:12;   // sequence number of the 1st MSDU for which this BAR is sent
+#endif /* RT_BIG_ENDIAN */
+    }   field;
+    USHORT           word;
+} BASEQ_CONTROL, *PBASEQ_CONTROL;
+
+//BAControl and BARControl are the same
+// 2-byte BA CONTROL field in BA frame
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;
+    USHORT      Rsv:9;
+    USHORT      Compressed:1;
+    USHORT      MTID:1;		//EWC V1.24
+    USHORT      ACKPolicy:1; // only related to N-Delayed BA. But not support in RT2860b. 0:NormalACK  1:No ACK
+#else
+    USHORT      ACKPolicy:1; // only related to N-Delayed BA. But not support in RT2860b. 0:NormalACK  1:No ACK
+    USHORT      MTID:1;		//EWC V1.24
+    USHORT      Compressed:1;
+    USHORT      Rsv:9;
+    USHORT      TID:4;
+#endif /* !RT_BIG_ENDIAN */
+} BA_CONTROL, *PBA_CONTROL;
+
+// 2-byte BAR CONTROL field in BAR frame
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;
+    USHORT      Rsv1:9;
+    USHORT      Compressed:1;
+    USHORT      MTID:1;		//if this bit1, use  FRAME_MTBA_REQ,  if 0, use FRAME_BA_REQ
+    USHORT      ACKPolicy:1;
+#else
+    USHORT      ACKPolicy:1; // 0:normal ack,  1:no ack.
+    USHORT      MTID:1;		//if this bit1, use  FRAME_MTBA_REQ,  if 0, use FRAME_BA_REQ
+    USHORT      Compressed:1;
+    USHORT      Rsv1:9;
+    USHORT      TID:4;
+#endif /* !RT_BIG_ENDIAN */
+} BAR_CONTROL, *PBAR_CONTROL;
+
+// BARControl in MTBAR frame
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      NumTID:4;
+    USHORT      Rsv1:9;
+    USHORT      Compressed:1;
+    USHORT      MTID:1;
+    USHORT      ACKPolicy:1;
+#else
+    USHORT      ACKPolicy:1;
+    USHORT      MTID:1;
+    USHORT      Compressed:1;
+    USHORT      Rsv1:9;
+    USHORT      NumTID:4;
+#endif /* !RT_BIG_ENDIAN */
+} MTBAR_CONTROL, *PMTBAR_CONTROL;
+
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;
+    USHORT      Rsv1:12;
+#else
+    USHORT      Rsv1:12;
+    USHORT      TID:4;
+#endif /* !RT_BIG_ENDIAN */
+} PER_TID_INFO, *PPER_TID_INFO;
+
+typedef struct {
+	PER_TID_INFO      PerTID;
+	BASEQ_CONTROL 	 BAStartingSeq;
+} EACH_TID, *PEACH_TID;
+
+
+typedef struct PACKED _PSPOLL_FRAME {
+    FRAME_CONTROL   FC;
+    USHORT          Aid;
+    UCHAR           Bssid[MAC_ADDR_LEN];
+    UCHAR           Ta[MAC_ADDR_LEN];
+}   PSPOLL_FRAME, *PPSPOLL_FRAME;
+
+typedef	struct	PACKED _RTS_FRAME	{
+    FRAME_CONTROL   FC;
+    USHORT          Duration;
+    UCHAR           Addr1[MAC_ADDR_LEN];
+    UCHAR           Addr2[MAC_ADDR_LEN];
+}RTS_FRAME, *PRTS_FRAME;
+
+// BAREQ AND MTBAREQ have the same subtype BAR, 802.11n BAR use compressed bitmap.
+typedef struct PACKED _FRAME_BA_REQ {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BAR_CONTROL  BARControl;
+	BASEQ_CONTROL 	 BAStartingSeq;
+}   FRAME_BA_REQ, *PFRAME_BA_REQ;
+
+typedef struct PACKED _FRAME_MTBA_REQ {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	MTBAR_CONTROL  MTBARControl;
+	PER_TID_INFO	PerTIDInfo;
+	BASEQ_CONTROL 	 BAStartingSeq;
+}   FRAME_MTBA_REQ, *PFRAME_MTBA_REQ;
+
+// Compressed format is mandantory in HT STA
+typedef struct PACKED _FRAME_MTBA {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BA_CONTROL  BAControl;
+	BASEQ_CONTROL 	 BAStartingSeq;
+	UCHAR		BitMap[8];
+}   FRAME_MTBA, *PFRAME_MTBA;
+
+typedef struct PACKED _FRAME_PSMP_ACTION {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Psmp;	// 7.3.1.25
+}   FRAME_PSMP_ACTION, *PFRAME_PSMP_ACTION;
+
+typedef struct PACKED _FRAME_ACTION_HDR {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+}   FRAME_ACTION_HDR, *PFRAME_ACTION_HDR;
+
+//Action Frame
+//Action Frame  Category:Spectrum,  Action:Channel Switch. 7.3.2.20
+typedef struct PACKED _CHAN_SWITCH_ANNOUNCE {
+	UCHAR					ElementID;	// ID = IE_CHANNEL_SWITCH_ANNOUNCEMENT = 37
+	UCHAR					Len;
+	CHA_SWITCH_ANNOUNCE_IE	CSAnnounceIe;
+}   CHAN_SWITCH_ANNOUNCE, *PCHAN_SWITCH_ANNOUNCE;
+
+
+//802.11n : 7.3.2.20a
+typedef struct PACKED _SECOND_CHAN_OFFSET {
+	UCHAR				ElementID;		// ID = IE_SECONDARY_CH_OFFSET = 62
+	UCHAR				Len;
+	SEC_CHA_OFFSET_IE	SecChOffsetIe;
+}   SECOND_CHAN_OFFSET, *PSECOND_CHAN_OFFSET;
+
+
+typedef struct PACKED _FRAME_SPETRUM_CS {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	CHAN_SWITCH_ANNOUNCE	CSAnnounce;
+	SECOND_CHAN_OFFSET		SecondChannel;
+}   FRAME_SPETRUM_CS, *PFRAME_SPETRUM_CS;
+
+
+typedef struct PACKED _FRAME_ADDBA_REQ {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Token;	// 1
+	BA_PARM		BaParm;	      //  2 - 10
+	USHORT		TimeOutValue;	// 0 - 0
+	BASEQ_CONTROL	BaStartSeq; // 0-0
+}   FRAME_ADDBA_REQ, *PFRAME_ADDBA_REQ;
+
+typedef struct PACKED _FRAME_ADDBA_RSP {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Token;
+	USHORT	StatusCode;
+	BA_PARM		BaParm; //0 - 2
+	USHORT		TimeOutValue;
+}   FRAME_ADDBA_RSP, *PFRAME_ADDBA_RSP;
+
+typedef struct PACKED _FRAME_DELBA_REQ {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	DELBA_PARM		DelbaParm;
+	USHORT	ReasonCode;
+}   FRAME_DELBA_REQ, *PFRAME_DELBA_REQ;
+
+
+//7.2.1.7
+typedef struct PACKED _FRAME_BAR {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BAR_CONTROL		BarControl;
+	BASEQ_CONTROL	StartingSeq;
+}   FRAME_BAR, *PFRAME_BAR;
+
+//7.2.1.7
+typedef struct PACKED _FRAME_BA {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BAR_CONTROL		BarControl;
+	BASEQ_CONTROL	StartingSeq;
+	UCHAR		bitmask[8];
+}   FRAME_BA, *PFRAME_BA;
+
+
+// Radio Measuement Request Frame Format
+typedef struct PACKED _FRAME_RM_REQ_ACTION {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Token;
+	USHORT	Repetition;
+	UCHAR   data[0];
+}   FRAME_RM_REQ_ACTION, *PFRAME_RM_REQ_ACTION;
+
+typedef struct PACKED {
+	UCHAR		ID;
+	UCHAR		Length;
+	UCHAR		ChannelSwitchMode;
+	UCHAR		NewRegClass;
+	UCHAR		NewChannelNum;
+	UCHAR		ChannelSwitchCount;
+} HT_EXT_CHANNEL_SWITCH_ANNOUNCEMENT_IE, *PHT_EXT_CHANNEL_SWITCH_ANNOUNCEMENT_IE;
+
+
+//
+// _Limit must be the 2**n - 1
+// _SEQ1 , _SEQ2 must be within 0 ~ _Limit
+//
+#define SEQ_STEPONE(_SEQ1, _SEQ2, _Limit)	((_SEQ1 == ((_SEQ2+1) & _Limit)))
+#define SEQ_SMALLER(_SEQ1, _SEQ2, _Limit)	(((_SEQ1-_SEQ2) & ((_Limit+1)>>1)))
+#define SEQ_LARGER(_SEQ1, _SEQ2, _Limit)	((_SEQ1 != _SEQ2) && !(((_SEQ1-_SEQ2) & ((_Limit+1)>>1))))
+#define SEQ_WITHIN_WIN(_SEQ1, _SEQ2, _WIN, _Limit) (SEQ_LARGER(_SEQ1, _SEQ2, _Limit) &&  \
+												SEQ_SMALLER(_SEQ1, ((_SEQ2+_WIN+1)&_Limit), _Limit))
+
+//
+// Contention-free parameter (without ID and Length)
+//
+typedef struct PACKED {
+    BOOLEAN     bValid;         // 1: variable contains valid value
+    UCHAR       CfpCount;
+    UCHAR       CfpPeriod;
+    USHORT      CfpMaxDuration;
+    USHORT      CfpDurRemaining;
+} CF_PARM, *PCF_PARM;
+
+typedef	struct	_CIPHER_SUITE	{
+	NDIS_802_11_ENCRYPTION_STATUS	PairCipher;		// Unicast cipher 1, this one has more secured cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS	PairCipherAux;	// Unicast cipher 2 if AP announce two unicast cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS	GroupCipher;	// Group cipher
+	USHORT							RsnCapability;	// RSN capability from beacon
+	BOOLEAN							bMixMode;		// Indicate Pair & Group cipher might be different
+}	CIPHER_SUITE, *PCIPHER_SUITE;
+
+// EDCA configuration from AP's BEACON/ProbeRsp
+typedef struct {
+    BOOLEAN     bValid;         // 1: variable contains valid value
+    BOOLEAN     bAdd;         // 1: variable contains valid value
+    BOOLEAN     bQAck;
+    BOOLEAN     bQueueRequest;
+    BOOLEAN     bTxopRequest;
+    BOOLEAN     bAPSDCapable;
+//  BOOLEAN     bMoreDataAck;
+    UCHAR       EdcaUpdateCount;
+    UCHAR       Aifsn[4];       // 0:AC_BK, 1:AC_BE, 2:AC_VI, 3:AC_VO
+    UCHAR       Cwmin[4];
+    UCHAR       Cwmax[4];
+    USHORT      Txop[4];      // in unit of 32-us
+    BOOLEAN     bACM[4];      // 1: Admission Control of AC_BK is mandattory
+} EDCA_PARM, *PEDCA_PARM;
+
+// QBSS LOAD information from QAP's BEACON/ProbeRsp
+typedef struct {
+    BOOLEAN     bValid;                     // 1: variable contains valid value
+    USHORT      StaNum;
+    UCHAR       ChannelUtilization;
+    USHORT      RemainingAdmissionControl;  // in unit of 32-us
+} QBSS_LOAD_PARM, *PQBSS_LOAD_PARM;
+
+// QBSS Info field in QSTA's assoc req
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR		Rsv2:1;
+	UCHAR		MaxSPLength:2;
+	UCHAR		Rsv1:1;
+	UCHAR		UAPSD_AC_BE:1;
+	UCHAR		UAPSD_AC_BK:1;
+	UCHAR		UAPSD_AC_VI:1;
+	UCHAR		UAPSD_AC_VO:1;
+#else
+    UCHAR		UAPSD_AC_VO:1;
+	UCHAR		UAPSD_AC_VI:1;
+	UCHAR		UAPSD_AC_BK:1;
+	UCHAR		UAPSD_AC_BE:1;
+	UCHAR		Rsv1:1;
+	UCHAR		MaxSPLength:2;
+	UCHAR		Rsv2:1;
+#endif /* !RT_BIG_ENDIAN */
+} QBSS_STA_INFO_PARM, *PQBSS_STA_INFO_PARM;
+
+// QBSS Info field in QAP's Beacon/ProbeRsp
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR		UAPSD:1;
+	UCHAR		Rsv:3;
+    UCHAR		ParamSetCount:4;
+#else
+    UCHAR		ParamSetCount:4;
+	UCHAR		Rsv:3;
+	UCHAR		UAPSD:1;
+#endif /* !RT_BIG_ENDIAN */
+} QBSS_AP_INFO_PARM, *PQBSS_AP_INFO_PARM;
+
+// QOS Capability reported in QAP's BEACON/ProbeRsp
+// QOS Capability sent out in QSTA's AssociateReq/ReAssociateReq
+typedef struct {
+    BOOLEAN     bValid;                     // 1: variable contains valid value
+    BOOLEAN     bQAck;
+    BOOLEAN     bQueueRequest;
+    BOOLEAN     bTxopRequest;
+//  BOOLEAN     bMoreDataAck;
+    UCHAR       EdcaUpdateCount;
+} QOS_CAPABILITY_PARM, *PQOS_CAPABILITY_PARM;
+
+#ifdef CONFIG_STA_SUPPORT
+typedef struct {
+    UCHAR       IELen;
+    UCHAR       IE[MAX_CUSTOM_LEN];
+} WPA_IE_;
+#endif // CONFIG_STA_SUPPORT //
+
+
+typedef struct {
+    UCHAR   Bssid[MAC_ADDR_LEN];
+    UCHAR   Channel;
+	UCHAR   CentralChannel;	//Store the wide-band central channel for 40MHz.  .used in 40MHz AP. Or this is the same as Channel.
+    UCHAR   BssType;
+    USHORT  AtimWin;
+    USHORT  BeaconPeriod;
+
+    UCHAR   SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+    UCHAR   SupRateLen;
+    UCHAR   ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+    UCHAR   ExtRateLen;
+	HT_CAPABILITY_IE HtCapability;
+	UCHAR			HtCapabilityLen;
+	ADD_HT_INFO_IE AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChanOffset;
+	CHAR    Rssi;
+    UCHAR   Privacy;			// Indicate security function ON/OFF. Don't mess up with auth mode.
+	UCHAR	Hidden;
+
+    USHORT  DtimPeriod;
+    USHORT  CapabilityInfo;
+
+    USHORT  CfpCount;
+    USHORT  CfpPeriod;
+    USHORT  CfpMaxDuration;
+    USHORT  CfpDurRemaining;
+    UCHAR   SsidLen;
+    CHAR    Ssid[MAX_LEN_OF_SSID];
+
+    ULONG   LastBeaconRxTime; // OS's timestamp
+
+	BOOLEAN	bSES;
+
+	// New for WPA2
+	CIPHER_SUITE					WPA;			// AP announced WPA cipher suite
+	CIPHER_SUITE					WPA2;			// AP announced WPA2 cipher suite
+
+	// New for microsoft WPA support
+	NDIS_802_11_FIXED_IEs	FixIEs;
+	NDIS_802_11_AUTHENTICATION_MODE	AuthModeAux;	// Addition mode for WPA2 / WPA capable AP
+	NDIS_802_11_AUTHENTICATION_MODE	AuthMode;
+	NDIS_802_11_WEP_STATUS	WepStatus;				// Unicast Encryption Algorithm extract from VAR_IE
+	USHORT					VarIELen;				// Length of next VIE include EID & Length
+	UCHAR					VarIEs[MAX_VIE_LEN];
+
+	// CCX Ckip information
+    UCHAR   CkipFlag;
+
+	// CCX 2 TSF
+	UCHAR	PTSF[4];		// Parent TSF
+	UCHAR	TTSF[8];		// Target TSF
+
+    // 802.11e d9, and WMM
+	EDCA_PARM           EdcaParm;
+	QOS_CAPABILITY_PARM QosCapability;
+	QBSS_LOAD_PARM      QbssLoad;
+#ifdef CONFIG_STA_SUPPORT
+    WPA_IE_     WpaIE;
+    WPA_IE_     RsnIE;
+#ifdef EXT_BUILD_CHANNEL_LIST
+	UCHAR		CountryString[3];
+	BOOLEAN		bHasCountryIE;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+} BSS_ENTRY, *PBSS_ENTRY;
+
+typedef struct {
+    UCHAR           BssNr;
+    UCHAR           BssOverlapNr;
+    BSS_ENTRY       BssEntry[MAX_LEN_OF_BSS_TABLE];
+} BSS_TABLE, *PBSS_TABLE;
+
+
+typedef struct _MLME_QUEUE_ELEM {
+    ULONG             Machine;
+    ULONG             MsgType;
+    ULONG             MsgLen;
+    UCHAR             Msg[MGMT_DMA_BUFFER_SIZE];
+    LARGE_INTEGER     TimeStamp;
+    UCHAR             Rssi0;
+    UCHAR             Rssi1;
+    UCHAR             Rssi2;
+    UCHAR             Signal;
+    UCHAR             Channel;
+    UCHAR             Wcid;
+    BOOLEAN           Occupied;
+} MLME_QUEUE_ELEM, *PMLME_QUEUE_ELEM;
+
+typedef struct _MLME_QUEUE {
+    ULONG             Num;
+    ULONG             Head;
+    ULONG             Tail;
+    NDIS_SPIN_LOCK   Lock;
+    MLME_QUEUE_ELEM  Entry[MAX_LEN_OF_MLME_QUEUE];
+} MLME_QUEUE, *PMLME_QUEUE;
+
+typedef VOID (*STATE_MACHINE_FUNC)(VOID *Adaptor, MLME_QUEUE_ELEM *Elem);
+
+typedef struct _STATE_MACHINE {
+    ULONG                           Base;
+    ULONG                           NrState;
+    ULONG                           NrMsg;
+    ULONG                           CurrState;
+    STATE_MACHINE_FUNC             *TransFunc;
+} STATE_MACHINE, *PSTATE_MACHINE;
+
+
+// MLME AUX data structure that hold temporarliy settings during a connection attempt.
+// Once this attemp succeeds, all settings will be copy to pAd->StaActive.
+// A connection attempt (user set OID, roaming, CCX fast roaming,..) consists of
+// several steps (JOIN, AUTH, ASSOC or REASSOC) and may fail at any step. We purposely
+// separate this under-trial settings away from pAd->StaActive so that once
+// this new attempt failed, driver can auto-recover back to the active settings.
+typedef struct _MLME_AUX {
+    UCHAR               BssType;
+    UCHAR               Ssid[MAX_LEN_OF_SSID];
+    UCHAR               SsidLen;
+    UCHAR               Bssid[MAC_ADDR_LEN];
+	UCHAR				AutoReconnectSsid[MAX_LEN_OF_SSID];
+	UCHAR				AutoReconnectSsidLen;
+    USHORT              Alg;
+    UCHAR               ScanType;
+    UCHAR               Channel;
+	UCHAR               CentralChannel;
+    USHORT              Aid;
+    USHORT              CapabilityInfo;
+    USHORT              BeaconPeriod;
+    USHORT              CfpMaxDuration;
+    USHORT              CfpPeriod;
+    USHORT              AtimWin;
+
+	// Copy supported rate from desired AP's beacon. We are trying to match
+	// AP's supported and extended rate settings.
+	UCHAR		        SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		        ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		        SupRateLen;
+	UCHAR		        ExtRateLen;
+	HT_CAPABILITY_IE		HtCapability;
+	UCHAR		        	HtCapabilityLen;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			NewExtChannelOffset;
+	//RT_HT_CAPABILITY	SupportedHtPhy;
+
+    // new for QOS
+    QOS_CAPABILITY_PARM APQosCapability;    // QOS capability of the current associated AP
+    EDCA_PARM           APEdcaParm;         // EDCA parameters of the current associated AP
+    QBSS_LOAD_PARM      APQbssLoad;         // QBSS load of the current associated AP
+
+    // new to keep Ralink specific feature
+    ULONG               APRalinkIe;
+
+    BSS_TABLE           SsidBssTab;     // AP list for the same SSID
+    BSS_TABLE           RoamTab;        // AP list eligible for roaming
+    ULONG               BssIdx;
+    ULONG               RoamIdx;
+
+	BOOLEAN				CurrReqIsFromNdis;
+
+    RALINK_TIMER_STRUCT BeaconTimer, ScanTimer;
+    RALINK_TIMER_STRUCT AuthTimer;
+    RALINK_TIMER_STRUCT AssocTimer, ReassocTimer, DisassocTimer;
+} MLME_AUX, *PMLME_AUX;
+
+typedef struct _MLME_ADDBA_REQ_STRUCT{
+	UCHAR   Wcid;	//
+	UCHAR   pAddr[MAC_ADDR_LEN];
+	UCHAR   BaBufSize;
+	USHORT	TimeOutValue;
+	UCHAR   TID;
+	UCHAR   Token;
+	USHORT	BaStartSeq;
+} MLME_ADDBA_REQ_STRUCT, *PMLME_ADDBA_REQ_STRUCT;
+
+
+typedef struct _MLME_DELBA_REQ_STRUCT{
+	UCHAR   Wcid;	//
+	UCHAR     Addr[MAC_ADDR_LEN];
+	UCHAR   TID;
+	UCHAR	Initiator;
+} MLME_DELBA_REQ_STRUCT, *PMLME_DELBA_REQ_STRUCT;
+
+// assoc struct is equal to reassoc
+typedef struct _MLME_ASSOC_REQ_STRUCT{
+    UCHAR     Addr[MAC_ADDR_LEN];
+    USHORT    CapabilityInfo;
+    USHORT    ListenIntv;
+    ULONG     Timeout;
+} MLME_ASSOC_REQ_STRUCT, *PMLME_ASSOC_REQ_STRUCT, MLME_REASSOC_REQ_STRUCT, *PMLME_REASSOC_REQ_STRUCT;
+
+typedef struct _MLME_DISASSOC_REQ_STRUCT{
+    UCHAR     Addr[MAC_ADDR_LEN];
+    USHORT    Reason;
+} MLME_DISASSOC_REQ_STRUCT, *PMLME_DISASSOC_REQ_STRUCT;
+
+typedef struct _MLME_AUTH_REQ_STRUCT {
+    UCHAR        Addr[MAC_ADDR_LEN];
+    USHORT       Alg;
+    ULONG        Timeout;
+} MLME_AUTH_REQ_STRUCT, *PMLME_AUTH_REQ_STRUCT;
+
+typedef struct _MLME_DEAUTH_REQ_STRUCT {
+    UCHAR        Addr[MAC_ADDR_LEN];
+    USHORT       Reason;
+} MLME_DEAUTH_REQ_STRUCT, *PMLME_DEAUTH_REQ_STRUCT;
+
+typedef struct {
+    ULONG      BssIdx;
+} MLME_JOIN_REQ_STRUCT;
+
+typedef struct _MLME_SCAN_REQ_STRUCT {
+    UCHAR      Bssid[MAC_ADDR_LEN];
+    UCHAR      BssType;
+    UCHAR      ScanType;
+    UCHAR      SsidLen;
+    CHAR       Ssid[MAX_LEN_OF_SSID];
+} MLME_SCAN_REQ_STRUCT, *PMLME_SCAN_REQ_STRUCT;
+
+typedef struct _MLME_START_REQ_STRUCT {
+    CHAR        Ssid[MAX_LEN_OF_SSID];
+    UCHAR       SsidLen;
+} MLME_START_REQ_STRUCT, *PMLME_START_REQ_STRUCT;
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+// structure for DLS
+typedef struct _RT_802_11_DLS {
+	USHORT						TimeOut;		// Use to time out while slience, unit: second , set by UI
+	USHORT						CountDownTimer;	// Use to time out while slience,unit: second , used by driver only
+	NDIS_802_11_MAC_ADDRESS		MacAddr;		// set by UI
+	UCHAR						Status;			// 0: none , 1: wait STAkey, 2: finish DLS setup , set by driver only
+	BOOLEAN						Valid;			// 1: valid , 0: invalid , set by UI, use to setup or tear down DLS link
+	RALINK_TIMER_STRUCT			Timer;			// Use to time out while handshake
+	USHORT						Sequence;
+	USHORT						MacTabMatchWCID;	// ASIC
+	BOOLEAN						bHTCap;
+	PVOID						pAd;
+} RT_802_11_DLS, *PRT_802_11_DLS;
+
+typedef struct _MLME_DLS_REQ_STRUCT {
+    PRT_802_11_DLS	pDLS;
+    USHORT			Reason;
+} MLME_DLS_REQ_STRUCT, *PMLME_DLS_REQ_STRUCT;
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+typedef struct PACKED {
+    UCHAR   Eid;
+    UCHAR   Len;
+    CHAR   Octet[1];
+} EID_STRUCT,*PEID_STRUCT, BEACON_EID_STRUCT, *PBEACON_EID_STRUCT;
+
+typedef struct PACKED _RTMP_TX_RATE_SWITCH
+{
+	UCHAR   ItemNo;
+#ifdef RT_BIG_ENDIAN
+	UCHAR	Rsv2:2;
+	UCHAR	Mode:2;
+	UCHAR	Rsv1:1;
+	UCHAR	BW:1;
+	UCHAR	ShortGI:1;
+	UCHAR	STBC:1;
+#else
+	UCHAR	STBC:1;
+	UCHAR	ShortGI:1;
+	UCHAR	BW:1;
+	UCHAR	Rsv1:1;
+	UCHAR	Mode:2;
+	UCHAR	Rsv2:2;
+#endif
+	UCHAR   CurrMCS;
+	UCHAR   TrainUp;
+	UCHAR   TrainDown;
+} RRTMP_TX_RATE_SWITCH, *PRTMP_TX_RATE_SWITCH;
+
+// ========================== AP mlme.h ===============================
+#define TBTT_PRELOAD_TIME       384        // usec. LomgPreamble + 24-byte at 1Mbps
+#define DEFAULT_DTIM_PERIOD     1
+
+// weighting factor to calculate Channel quality, total should be 100%
+//#define RSSI_WEIGHTING                   0
+//#define TX_WEIGHTING                     40
+//#define RX_WEIGHTING                     60
+
+#define MAC_TABLE_AGEOUT_TIME			300			// unit: sec
+#define MAC_TABLE_ASSOC_TIMEOUT			5			// unit: sec
+#define MAC_TABLE_FULL(Tab)				((Tab).size == MAX_LEN_OF_MAC_TABLE)
+
+// AP shall drop the sta if contine Tx fail count reach it.
+#define MAC_ENTRY_LIFE_CHECK_CNT		20			// packet cnt.
+
+// Value domain of pMacEntry->Sst
+typedef enum _Sst {
+    SST_NOT_AUTH,   // 0: equivalent to IEEE 802.11/1999 state 1
+    SST_AUTH,       // 1: equivalent to IEEE 802.11/1999 state 2
+    SST_ASSOC       // 2: equivalent to IEEE 802.11/1999 state 3
+} SST;
+
+// value domain of pMacEntry->AuthState
+typedef enum _AuthState {
+    AS_NOT_AUTH,
+    AS_AUTH_OPEN,       // STA has been authenticated using OPEN SYSTEM
+    AS_AUTH_KEY,        // STA has been authenticated using SHARED KEY
+    AS_AUTHENTICATING   // STA is waiting for AUTH seq#3 using SHARED KEY
+} AUTH_STATE;
+
+//for-wpa value domain of pMacEntry->WpaState  802.1i D3   p.114
+typedef enum _ApWpaState {
+    AS_NOTUSE,              // 0
+    AS_DISCONNECT,          // 1
+    AS_DISCONNECTED,        // 2
+    AS_INITIALIZE,          // 3
+    AS_AUTHENTICATION,      // 4
+    AS_AUTHENTICATION2,     // 5
+    AS_INITPMK,             // 6
+    AS_INITPSK,             // 7
+    AS_PTKSTART,            // 8
+    AS_PTKINIT_NEGOTIATING, // 9
+    AS_PTKINITDONE,         // 10
+    AS_UPDATEKEYS,          // 11
+    AS_INTEGRITY_FAILURE,   // 12
+    AS_KEYUPDATE,           // 13
+} AP_WPA_STATE;
+
+// for-wpa value domain of pMacEntry->WpaState  802.1i D3   p.114
+typedef enum _GTKState {
+    REKEY_NEGOTIATING,
+    REKEY_ESTABLISHED,
+    KEYERROR,
+} GTK_STATE;
+
+//  for-wpa  value domain of pMacEntry->WpaState  802.1i D3   p.114
+typedef enum _WpaGTKState {
+    SETKEYS,
+    SETKEYS_DONE,
+} WPA_GTK_STATE;
+// ====================== end of AP mlme.h ============================
+
+
+#endif	// MLME_H__
diff --git a/drivers/staging/rt2870/netif_block.h b/drivers/staging/rt2870/netif_block.h
new file mode 100644
index 000000000000..6e5151c41095
--- /dev/null
+++ b/drivers/staging/rt2870/netif_block.h
@@ -0,0 +1,58 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __NET_IF_BLOCK_H__
+#define __NET_IF_BLOCK_H__
+
+//#include <linux/device.h>
+#include "link_list.h"
+#include "rtmp.h"
+
+#define FREE_NETIF_POOL_SIZE 32
+
+typedef struct _NETIF_ENTRY
+{
+	struct _NETIF_ENTRY *pNext;
+	PNET_DEV pNetDev;
+} NETIF_ENTRY, *PNETIF_ENTRY;
+
+void initblockQueueTab(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN blockNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry,
+	IN PNET_DEV pNetDev);
+
+VOID releaseNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry);
+
+VOID StopNetIfQueue(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR QueIdx,
+	IN PNDIS_PACKET pPacket);
+#endif // __NET_IF_BLOCK_H__
+
diff --git a/drivers/staging/rt2870/oid.h b/drivers/staging/rt2870/oid.h
new file mode 100644
index 000000000000..d788db6b6264
--- /dev/null
+++ b/drivers/staging/rt2870/oid.h
@@ -0,0 +1,1091 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	oid.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#ifndef _OID_H_
+#define _OID_H_
+
+#define TRUE				1
+#define FALSE				0
+//
+// IEEE 802.11 Structures and definitions
+//
+#define MAX_TX_POWER_LEVEL              100   /* mW */
+#define MAX_RSSI_TRIGGER                -10    /* dBm */
+#define MIN_RSSI_TRIGGER                -200   /* dBm */
+#define MAX_FRAG_THRESHOLD              2346  /* byte count */
+#define MIN_FRAG_THRESHOLD              256   /* byte count */
+#define MAX_RTS_THRESHOLD               2347  /* byte count */
+
+// new types for Media Specific Indications
+// Extension channel offset
+#define EXTCHA_NONE			0
+#define EXTCHA_ABOVE		0x1
+#define EXTCHA_BELOW		0x3
+
+// BW
+#define BAND_WIDTH_20		0
+#define BAND_WIDTH_40		1
+#define BAND_WIDTH_BOTH		2
+#define BAND_WIDTH_10		3	// 802.11j has 10MHz. This definition is for internal usage. doesn't fill in the IE or other field.
+// SHORTGI
+#define GAP_INTERVAL_400	1	// only support in HT mode
+#define GAP_INTERVAL_800	0
+#define GAP_INTERVAL_BOTH	2
+
+#define NdisMediaStateConnected			1
+#define NdisMediaStateDisconnected		0
+
+#define NDIS_802_11_LENGTH_SSID         32
+#define NDIS_802_11_LENGTH_RATES        8
+#define NDIS_802_11_LENGTH_RATES_EX     16
+#define MAC_ADDR_LENGTH                 6
+#define MAX_NUM_OF_CHS					49 // 14 channels @2.4G +  12@UNII + 4 @MMAC + 11 @HiperLAN2 + 7 @Japan + 1 as NULL terminationc
+#define MAX_NUMBER_OF_EVENT				10  // entry # in EVENT table
+#define MAX_NUMBER_OF_MAC				32 // if MAX_MBSSID_NUM is 8, this value can't be larger than 211
+#define MAX_NUMBER_OF_ACL				64
+#define MAX_LENGTH_OF_SUPPORT_RATES		12    // 1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54
+#define MAX_NUMBER_OF_DLS_ENTRY			4
+
+#ifndef UNDER_CE
+// OID definition, since NDIS 5.0 didn't define these, we need to define for our own
+//#if _WIN32_WINNT<=0x0500
+
+#define OID_GEN_MACHINE_NAME               0x0001021A
+
+#ifdef RALINK_ATE
+#define RT_QUERY_ATE_TXDONE_COUNT			0x0401
+#endif // RALINK_ATE //
+#define RT_QUERY_SIGNAL_CONTEXT				0x0402
+#define RT_SET_IAPP_PID                 	0x0404
+#define RT_SET_APD_PID						0x0405
+#define RT_SET_DEL_MAC_ENTRY				0x0406
+
+//
+// IEEE 802.11 OIDs
+//
+#define	OID_GET_SET_TOGGLE			0x8000
+
+#define	OID_802_11_NETWORK_TYPES_SUPPORTED			0x0103
+#define	OID_802_11_NETWORK_TYPE_IN_USE				0x0104
+#define	OID_802_11_RSSI_TRIGGER						0x0107
+#define	RT_OID_802_11_RSSI							0x0108 //rt2860	only , kathy
+#define	RT_OID_802_11_RSSI_1						0x0109 //rt2860	only , kathy
+#define	RT_OID_802_11_RSSI_2						0x010A //rt2860	only , kathy
+#define	OID_802_11_NUMBER_OF_ANTENNAS				0x010B
+#define	OID_802_11_RX_ANTENNA_SELECTED				0x010C
+#define	OID_802_11_TX_ANTENNA_SELECTED				0x010D
+#define	OID_802_11_SUPPORTED_RATES					0x010E
+#define	OID_802_11_ADD_WEP							0x0112
+#define	OID_802_11_REMOVE_WEP						0x0113
+#define	OID_802_11_DISASSOCIATE						0x0114
+#define	OID_802_11_PRIVACY_FILTER					0x0118
+#define	OID_802_11_ASSOCIATION_INFORMATION			0x011E
+#define	OID_802_11_TEST								0x011F
+#define	RT_OID_802_11_COUNTRY_REGION				0x0507
+#define	OID_802_11_BSSID_LIST_SCAN					0x0508
+#define	OID_802_11_SSID								0x0509
+#define	OID_802_11_BSSID							0x050A
+#define	RT_OID_802_11_RADIO							0x050B
+#define	RT_OID_802_11_PHY_MODE						0x050C
+#define	RT_OID_802_11_STA_CONFIG					0x050D
+#define	OID_802_11_DESIRED_RATES					0x050E
+#define	RT_OID_802_11_PREAMBLE						0x050F
+#define	OID_802_11_WEP_STATUS						0x0510
+#define	OID_802_11_AUTHENTICATION_MODE				0x0511
+#define	OID_802_11_INFRASTRUCTURE_MODE				0x0512
+#define	RT_OID_802_11_RESET_COUNTERS				0x0513
+#define	OID_802_11_RTS_THRESHOLD					0x0514
+#define	OID_802_11_FRAGMENTATION_THRESHOLD			0x0515
+#define	OID_802_11_POWER_MODE						0x0516
+#define	OID_802_11_TX_POWER_LEVEL					0x0517
+#define	RT_OID_802_11_ADD_WPA						0x0518
+#define	OID_802_11_REMOVE_KEY						0x0519
+#define	OID_802_11_ADD_KEY							0x0520
+#define	OID_802_11_CONFIGURATION					0x0521
+#define	OID_802_11_TX_PACKET_BURST					0x0522
+#define	RT_OID_802_11_QUERY_NOISE_LEVEL				0x0523
+#define	RT_OID_802_11_EXTRA_INFO					0x0524
+#ifdef	DBG
+#define	RT_OID_802_11_HARDWARE_REGISTER				0x0525
+#endif
+#define OID_802_11_ENCRYPTION_STATUS            OID_802_11_WEP_STATUS
+#define OID_802_11_DEAUTHENTICATION                 0x0526
+#define OID_802_11_DROP_UNENCRYPTED                 0x0527
+#define OID_802_11_MIC_FAILURE_REPORT_FRAME         0x0528
+
+// For 802.1x daemin using to require current driver configuration
+#define OID_802_11_RADIUS_QUERY_SETTING				0x0540
+
+#define	RT_OID_DEVICE_NAME							0x0607
+#define	RT_OID_VERSION_INFO							0x0608
+#define	OID_802_11_BSSID_LIST						0x0609
+#define	OID_802_3_CURRENT_ADDRESS					0x060A
+#define	OID_GEN_MEDIA_CONNECT_STATUS				0x060B
+#define	RT_OID_802_11_QUERY_LINK_STATUS				0x060C
+#define	OID_802_11_RSSI								0x060D
+#define	OID_802_11_STATISTICS						0x060E
+#define	OID_GEN_RCV_OK								0x060F
+#define	OID_GEN_RCV_NO_BUFFER						0x0610
+#define	RT_OID_802_11_QUERY_EEPROM_VERSION			0x0611
+#define	RT_OID_802_11_QUERY_FIRMWARE_VERSION		0x0612
+#define	RT_OID_802_11_QUERY_LAST_RX_RATE			0x0613
+#define	RT_OID_802_11_TX_POWER_LEVEL_1				0x0614
+#define	RT_OID_802_11_QUERY_PIDVID					0x0615
+//for WPA_SUPPLICANT_SUPPORT
+#define OID_SET_COUNTERMEASURES                     0x0616
+#define OID_802_11_SET_IEEE8021X                    0x0617
+#define OID_802_11_SET_IEEE8021X_REQUIRE_KEY        0x0618
+#define OID_802_11_PMKID                            0x0620
+#define RT_OID_WPA_SUPPLICANT_SUPPORT               0x0621
+#define RT_OID_WE_VERSION_COMPILED                  0x0622
+#define RT_OID_NEW_DRIVER                           0x0623
+
+
+//rt2860 , kathy
+#define	RT_OID_802_11_SNR_0							0x0630
+#define	RT_OID_802_11_SNR_1							0x0631
+#define	RT_OID_802_11_QUERY_LAST_TX_RATE			0x0632
+#define	RT_OID_802_11_QUERY_HT_PHYMODE				0x0633
+#define	RT_OID_802_11_SET_HT_PHYMODE				0x0634
+#define	OID_802_11_RELOAD_DEFAULTS					0x0635
+#define	RT_OID_802_11_QUERY_APSD_SETTING			0x0636
+#define	RT_OID_802_11_SET_APSD_SETTING				0x0637
+#define	RT_OID_802_11_QUERY_APSD_PSM				0x0638
+#define	RT_OID_802_11_SET_APSD_PSM					0x0639
+#define	RT_OID_802_11_QUERY_DLS						0x063A
+#define	RT_OID_802_11_SET_DLS						0x063B
+#define	RT_OID_802_11_QUERY_DLS_PARAM				0x063C
+#define	RT_OID_802_11_SET_DLS_PARAM					0x063D
+#define RT_OID_802_11_QUERY_WMM              		0x063E
+#define RT_OID_802_11_SET_WMM      					0x063F
+#define RT_OID_802_11_QUERY_IMME_BA_CAP				0x0640
+#define RT_OID_802_11_SET_IMME_BA_CAP				0x0641
+#define RT_OID_802_11_QUERY_BATABLE					0x0642
+#define RT_OID_802_11_ADD_IMME_BA					0x0643
+#define RT_OID_802_11_TEAR_IMME_BA					0x0644
+#define RT_OID_DRIVER_DEVICE_NAME                   0x0645
+#define RT_OID_802_11_QUERY_DAT_HT_PHYMODE          0x0646
+#define RT_OID_QUERY_MULTIPLE_CARD_SUPPORT          0x0647
+
+// Ralink defined OIDs
+// Dennis Lee move to platform specific
+
+#define	RT_OID_802_11_BSSID					  (OID_GET_SET_TOGGLE |	OID_802_11_BSSID)
+#define	RT_OID_802_11_SSID					  (OID_GET_SET_TOGGLE |	OID_802_11_SSID)
+#define	RT_OID_802_11_INFRASTRUCTURE_MODE	  (OID_GET_SET_TOGGLE |	OID_802_11_INFRASTRUCTURE_MODE)
+#define	RT_OID_802_11_ADD_WEP				  (OID_GET_SET_TOGGLE |	OID_802_11_ADD_WEP)
+#define	RT_OID_802_11_ADD_KEY				  (OID_GET_SET_TOGGLE |	OID_802_11_ADD_KEY)
+#define	RT_OID_802_11_REMOVE_WEP			  (OID_GET_SET_TOGGLE |	OID_802_11_REMOVE_WEP)
+#define	RT_OID_802_11_REMOVE_KEY			  (OID_GET_SET_TOGGLE |	OID_802_11_REMOVE_KEY)
+#define	RT_OID_802_11_DISASSOCIATE			  (OID_GET_SET_TOGGLE |	OID_802_11_DISASSOCIATE)
+#define	RT_OID_802_11_AUTHENTICATION_MODE	  (OID_GET_SET_TOGGLE |	OID_802_11_AUTHENTICATION_MODE)
+#define	RT_OID_802_11_PRIVACY_FILTER		  (OID_GET_SET_TOGGLE |	OID_802_11_PRIVACY_FILTER)
+#define	RT_OID_802_11_BSSID_LIST_SCAN		  (OID_GET_SET_TOGGLE |	OID_802_11_BSSID_LIST_SCAN)
+#define	RT_OID_802_11_WEP_STATUS			  (OID_GET_SET_TOGGLE |	OID_802_11_WEP_STATUS)
+#define	RT_OID_802_11_RELOAD_DEFAULTS		  (OID_GET_SET_TOGGLE |	OID_802_11_RELOAD_DEFAULTS)
+#define	RT_OID_802_11_NETWORK_TYPE_IN_USE	  (OID_GET_SET_TOGGLE |	OID_802_11_NETWORK_TYPE_IN_USE)
+#define	RT_OID_802_11_TX_POWER_LEVEL		  (OID_GET_SET_TOGGLE |	OID_802_11_TX_POWER_LEVEL)
+#define	RT_OID_802_11_RSSI_TRIGGER			  (OID_GET_SET_TOGGLE |	OID_802_11_RSSI_TRIGGER)
+#define	RT_OID_802_11_FRAGMENTATION_THRESHOLD (OID_GET_SET_TOGGLE |	OID_802_11_FRAGMENTATION_THRESHOLD)
+#define	RT_OID_802_11_RTS_THRESHOLD			  (OID_GET_SET_TOGGLE |	OID_802_11_RTS_THRESHOLD)
+#define	RT_OID_802_11_RX_ANTENNA_SELECTED	  (OID_GET_SET_TOGGLE |	OID_802_11_RX_ANTENNA_SELECTED)
+#define	RT_OID_802_11_TX_ANTENNA_SELECTED	  (OID_GET_SET_TOGGLE |	OID_802_11_TX_ANTENNA_SELECTED)
+#define	RT_OID_802_11_SUPPORTED_RATES		  (OID_GET_SET_TOGGLE |	OID_802_11_SUPPORTED_RATES)
+#define	RT_OID_802_11_DESIRED_RATES			  (OID_GET_SET_TOGGLE |	OID_802_11_DESIRED_RATES)
+#define	RT_OID_802_11_CONFIGURATION			  (OID_GET_SET_TOGGLE |	OID_802_11_CONFIGURATION)
+#define	RT_OID_802_11_POWER_MODE			  (OID_GET_SET_TOGGLE |	OID_802_11_POWER_MODE)
+
+typedef enum _NDIS_802_11_STATUS_TYPE
+{
+    Ndis802_11StatusType_Authentication,
+    Ndis802_11StatusType_MediaStreamMode,
+    Ndis802_11StatusType_PMKID_CandidateList,
+    Ndis802_11StatusTypeMax    // not a real type, defined as an upper bound
+} NDIS_802_11_STATUS_TYPE, *PNDIS_802_11_STATUS_TYPE;
+
+typedef UCHAR   NDIS_802_11_MAC_ADDRESS[6];
+
+typedef struct _NDIS_802_11_STATUS_INDICATION
+{
+    NDIS_802_11_STATUS_TYPE StatusType;
+} NDIS_802_11_STATUS_INDICATION, *PNDIS_802_11_STATUS_INDICATION;
+
+// 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
+
+typedef struct _NDIS_802_11_AUTHENTICATION_REQUEST
+{
+    ULONG Length;            // Length of structure
+    NDIS_802_11_MAC_ADDRESS Bssid;
+    ULONG Flags;
+} NDIS_802_11_AUTHENTICATION_REQUEST, *PNDIS_802_11_AUTHENTICATION_REQUEST;
+
+//Added new types for PMKID Candidate lists.
+typedef struct _PMKID_CANDIDATE {
+    NDIS_802_11_MAC_ADDRESS BSSID;
+    ULONG Flags;
+} PMKID_CANDIDATE, *PPMKID_CANDIDATE;
+
+typedef struct _NDIS_802_11_PMKID_CANDIDATE_LIST
+{
+    ULONG Version;       // Version of the structure
+    ULONG NumCandidates; // No. of pmkid candidates
+    PMKID_CANDIDATE CandidateList[1];
+} NDIS_802_11_PMKID_CANDIDATE_LIST, *PNDIS_802_11_PMKID_CANDIDATE_LIST;
+
+//Flags for PMKID Candidate list structure
+#define NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED	0x01
+
+// Added new types for OFDM 5G and 2.4G
+typedef enum _NDIS_802_11_NETWORK_TYPE
+{
+   Ndis802_11FH,
+   Ndis802_11DS,
+    Ndis802_11OFDM5,
+    Ndis802_11OFDM5_N,
+    Ndis802_11OFDM24,
+    Ndis802_11OFDM24_N,
+   Ndis802_11Automode,
+    Ndis802_11NetworkTypeMax    // not a real type, defined as an upper bound
+} NDIS_802_11_NETWORK_TYPE, *PNDIS_802_11_NETWORK_TYPE;
+
+typedef struct _NDIS_802_11_NETWORK_TYPE_LIST
+{
+    UINT                       NumberOfItems;  // in list below, at least 1
+   NDIS_802_11_NETWORK_TYPE    NetworkType [1];
+} NDIS_802_11_NETWORK_TYPE_LIST, *PNDIS_802_11_NETWORK_TYPE_LIST;
+
+typedef enum _NDIS_802_11_POWER_MODE
+{
+    Ndis802_11PowerModeCAM,
+    Ndis802_11PowerModeMAX_PSP,
+    Ndis802_11PowerModeFast_PSP,
+    Ndis802_11PowerModeLegacy_PSP,
+    Ndis802_11PowerModeMax      // not a real mode, defined as an upper bound
+} NDIS_802_11_POWER_MODE, *PNDIS_802_11_POWER_MODE;
+
+typedef ULONG   NDIS_802_11_TX_POWER_LEVEL; // in milliwatts
+
+//
+// Received Signal Strength Indication
+//
+typedef LONG    NDIS_802_11_RSSI;           // in dBm
+
+typedef struct _NDIS_802_11_CONFIGURATION_FH
+{
+   ULONG           Length;            // Length of structure
+   ULONG           HopPattern;        // As defined by 802.11, MSB set
+   ULONG           HopSet;            // to one if non-802.11
+   ULONG           DwellTime;         // units are Kusec
+} NDIS_802_11_CONFIGURATION_FH, *PNDIS_802_11_CONFIGURATION_FH;
+
+typedef struct _NDIS_802_11_CONFIGURATION
+{
+   ULONG                           Length;             // Length of structure
+   ULONG                           BeaconPeriod;       // units are Kusec
+   ULONG                           ATIMWindow;         // units are Kusec
+   ULONG                           DSConfig;           // Frequency, units are kHz
+   NDIS_802_11_CONFIGURATION_FH    FHConfig;
+} NDIS_802_11_CONFIGURATION, *PNDIS_802_11_CONFIGURATION;
+
+typedef struct _NDIS_802_11_STATISTICS
+{
+   ULONG           Length;             // Length of structure
+   LARGE_INTEGER   TransmittedFragmentCount;
+   LARGE_INTEGER   MulticastTransmittedFrameCount;
+   LARGE_INTEGER   FailedCount;
+   LARGE_INTEGER   RetryCount;
+   LARGE_INTEGER   MultipleRetryCount;
+   LARGE_INTEGER   RTSSuccessCount;
+   LARGE_INTEGER   RTSFailureCount;
+   LARGE_INTEGER   ACKFailureCount;
+   LARGE_INTEGER   FrameDuplicateCount;
+   LARGE_INTEGER   ReceivedFragmentCount;
+   LARGE_INTEGER   MulticastReceivedFrameCount;
+   LARGE_INTEGER   FCSErrorCount;
+   LARGE_INTEGER   TKIPLocalMICFailures;
+   LARGE_INTEGER   TKIPRemoteMICErrors;
+   LARGE_INTEGER   TKIPICVErrors;
+   LARGE_INTEGER   TKIPCounterMeasuresInvoked;
+   LARGE_INTEGER   TKIPReplays;
+   LARGE_INTEGER   CCMPFormatErrors;
+   LARGE_INTEGER   CCMPReplays;
+   LARGE_INTEGER   CCMPDecryptErrors;
+   LARGE_INTEGER   FourWayHandshakeFailures;
+} NDIS_802_11_STATISTICS, *PNDIS_802_11_STATISTICS;
+
+typedef  ULONG  NDIS_802_11_KEY_INDEX;
+typedef ULONGLONG   NDIS_802_11_KEY_RSC;
+
+#define MAX_RADIUS_SRV_NUM			2	  // 802.1x failover number
+
+typedef struct PACKED _RADIUS_SRV_INFO {
+	UINT32			radius_ip;
+	UINT32			radius_port;
+	UCHAR			radius_key[64];
+	UCHAR			radius_key_len;
+} RADIUS_SRV_INFO, *PRADIUS_SRV_INFO;
+
+typedef struct PACKED _RADIUS_KEY_INFO
+{
+	UCHAR			radius_srv_num;
+	RADIUS_SRV_INFO	radius_srv_info[MAX_RADIUS_SRV_NUM];
+	UCHAR			ieee8021xWEP;		 // dynamic WEP
+    UCHAR           key_index;
+    UCHAR           key_length;          // length of key in bytes
+    UCHAR           key_material[13];
+} RADIUS_KEY_INFO, *PRADIUS_KEY_INFO;
+
+// It's used by 802.1x daemon to require relative configuration
+typedef struct PACKED _RADIUS_CONF
+{
+    UINT32          Length;             // Length of this structure
+    UCHAR			mbss_num;			// indicate multiple BSS number
+	UINT32			own_ip_addr;
+	UINT32			retry_interval;
+	UINT32			session_timeout_interval;
+	UCHAR			EAPifname[IFNAMSIZ];
+	UCHAR			EAPifname_len;
+	UCHAR 			PreAuthifname[IFNAMSIZ];
+	UCHAR			PreAuthifname_len;
+	RADIUS_KEY_INFO	RadiusInfo[8/*MAX_MBSSID_NUM*/];
+} RADIUS_CONF, *PRADIUS_CONF;
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+// Key mapping keys require a BSSID
+typedef struct _NDIS_802_11_KEY
+{
+    UINT           Length;             // Length of this structure
+    UINT           KeyIndex;
+    UINT           KeyLength;          // length of key in bytes
+    NDIS_802_11_MAC_ADDRESS BSSID;
+    NDIS_802_11_KEY_RSC KeyRSC;
+    UCHAR           KeyMaterial[1];     // variable length depending on above field
+} NDIS_802_11_KEY, *PNDIS_802_11_KEY;
+#endif // CONFIG_STA_SUPPORT //
+
+typedef struct _NDIS_802_11_REMOVE_KEY
+{
+    UINT           Length;             // Length of this structure
+    UINT           KeyIndex;
+    NDIS_802_11_MAC_ADDRESS BSSID;
+} NDIS_802_11_REMOVE_KEY, *PNDIS_802_11_REMOVE_KEY;
+
+typedef struct _NDIS_802_11_WEP
+{
+   UINT     Length;        // Length of this structure
+   UINT     KeyIndex;           // 0 is the per-client key, 1-N are the
+                                        // global keys
+   UINT     KeyLength;     // length of key in bytes
+   UCHAR     KeyMaterial[1];// variable length depending on above field
+} NDIS_802_11_WEP, *PNDIS_802_11_WEP;
+
+
+typedef enum _NDIS_802_11_NETWORK_INFRASTRUCTURE
+{
+   Ndis802_11IBSS,
+   Ndis802_11Infrastructure,
+   Ndis802_11AutoUnknown,
+   Ndis802_11Monitor,
+   Ndis802_11InfrastructureMax     // Not a real value, defined as upper bound
+} NDIS_802_11_NETWORK_INFRASTRUCTURE, *PNDIS_802_11_NETWORK_INFRASTRUCTURE;
+
+// Add new authentication modes
+typedef enum _NDIS_802_11_AUTHENTICATION_MODE
+{
+   Ndis802_11AuthModeOpen,
+   Ndis802_11AuthModeShared,
+   Ndis802_11AuthModeAutoSwitch,
+    Ndis802_11AuthModeWPA,
+    Ndis802_11AuthModeWPAPSK,
+    Ndis802_11AuthModeWPANone,
+   Ndis802_11AuthModeWPA2,
+   Ndis802_11AuthModeWPA2PSK,
+   	Ndis802_11AuthModeWPA1WPA2,
+	Ndis802_11AuthModeWPA1PSKWPA2PSK,
+   Ndis802_11AuthModeMax           // Not a real mode, defined as upper bound
+} NDIS_802_11_AUTHENTICATION_MODE, *PNDIS_802_11_AUTHENTICATION_MODE;
+
+typedef UCHAR   NDIS_802_11_RATES[NDIS_802_11_LENGTH_RATES];        // Set of 8 data rates
+typedef UCHAR   NDIS_802_11_RATES_EX[NDIS_802_11_LENGTH_RATES_EX];  // Set of 16 data rates
+
+typedef struct PACKED _NDIS_802_11_SSID
+{
+    UINT   SsidLength;         // length of SSID field below, in bytes;
+                                // this can be zero.
+    UCHAR   Ssid[NDIS_802_11_LENGTH_SSID];           // SSID information field
+} NDIS_802_11_SSID, *PNDIS_802_11_SSID;
+
+
+typedef struct PACKED _NDIS_WLAN_BSSID
+{
+   ULONG                               Length;     // Length of this structure
+   NDIS_802_11_MAC_ADDRESS             MacAddress; // BSSID
+   UCHAR                               Reserved[2];
+   NDIS_802_11_SSID                    Ssid;       // SSID
+   ULONG                               Privacy;    // WEP encryption requirement
+   NDIS_802_11_RSSI                    Rssi;       // receive signal strength in dBm
+   NDIS_802_11_NETWORK_TYPE            NetworkTypeInUse;
+   NDIS_802_11_CONFIGURATION           Configuration;
+   NDIS_802_11_NETWORK_INFRASTRUCTURE  InfrastructureMode;
+   NDIS_802_11_RATES                   SupportedRates;
+} NDIS_WLAN_BSSID, *PNDIS_WLAN_BSSID;
+
+typedef struct PACKED _NDIS_802_11_BSSID_LIST
+{
+   UINT           NumberOfItems;      // in list below, at least 1
+   NDIS_WLAN_BSSID Bssid[1];
+} NDIS_802_11_BSSID_LIST, *PNDIS_802_11_BSSID_LIST;
+
+// Added Capabilities, IELength and IEs for each BSSID
+typedef struct PACKED _NDIS_WLAN_BSSID_EX
+{
+    ULONG                               Length;             // Length of this structure
+    NDIS_802_11_MAC_ADDRESS             MacAddress;         // BSSID
+    UCHAR                               Reserved[2];
+    NDIS_802_11_SSID                    Ssid;               // SSID
+    UINT                                Privacy;            // WEP encryption requirement
+    NDIS_802_11_RSSI                    Rssi;               // receive signal
+                                                            // strength in dBm
+    NDIS_802_11_NETWORK_TYPE            NetworkTypeInUse;
+    NDIS_802_11_CONFIGURATION           Configuration;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  InfrastructureMode;
+    NDIS_802_11_RATES_EX                SupportedRates;
+    ULONG                               IELength;
+    UCHAR                               IEs[1];
+} NDIS_WLAN_BSSID_EX, *PNDIS_WLAN_BSSID_EX;
+
+typedef struct PACKED _NDIS_802_11_BSSID_LIST_EX
+{
+    UINT                   NumberOfItems;      // in list below, at least 1
+    NDIS_WLAN_BSSID_EX      Bssid[1];
+} NDIS_802_11_BSSID_LIST_EX, *PNDIS_802_11_BSSID_LIST_EX;
+
+typedef struct PACKED _NDIS_802_11_FIXED_IEs
+{
+    UCHAR Timestamp[8];
+    USHORT BeaconInterval;
+    USHORT Capabilities;
+} NDIS_802_11_FIXED_IEs, *PNDIS_802_11_FIXED_IEs;
+
+typedef struct _NDIS_802_11_VARIABLE_IEs
+{
+    UCHAR ElementID;
+    UCHAR Length;    // Number of bytes in data field
+    UCHAR data[1];
+} NDIS_802_11_VARIABLE_IEs, *PNDIS_802_11_VARIABLE_IEs;
+
+typedef  ULONG   NDIS_802_11_FRAGMENTATION_THRESHOLD;
+
+typedef  ULONG   NDIS_802_11_RTS_THRESHOLD;
+
+typedef  ULONG   NDIS_802_11_ANTENNA;
+
+typedef enum _NDIS_802_11_PRIVACY_FILTER
+{
+   Ndis802_11PrivFilterAcceptAll,
+   Ndis802_11PrivFilter8021xWEP
+} NDIS_802_11_PRIVACY_FILTER, *PNDIS_802_11_PRIVACY_FILTER;
+
+// Added new encryption types
+// Also aliased typedef to new name
+typedef enum _NDIS_802_11_WEP_STATUS
+{
+   Ndis802_11WEPEnabled,
+    Ndis802_11Encryption1Enabled = Ndis802_11WEPEnabled,
+   Ndis802_11WEPDisabled,
+    Ndis802_11EncryptionDisabled = Ndis802_11WEPDisabled,
+   Ndis802_11WEPKeyAbsent,
+    Ndis802_11Encryption1KeyAbsent = Ndis802_11WEPKeyAbsent,
+   Ndis802_11WEPNotSupported,
+    Ndis802_11EncryptionNotSupported = Ndis802_11WEPNotSupported,
+    Ndis802_11Encryption2Enabled,
+    Ndis802_11Encryption2KeyAbsent,
+    Ndis802_11Encryption3Enabled,
+    Ndis802_11Encryption3KeyAbsent,
+    Ndis802_11Encryption4Enabled,	// TKIP or AES mix
+    Ndis802_11Encryption4KeyAbsent,
+    Ndis802_11GroupWEP40Enabled,
+	Ndis802_11GroupWEP104Enabled,
+} NDIS_802_11_WEP_STATUS, *PNDIS_802_11_WEP_STATUS,
+  NDIS_802_11_ENCRYPTION_STATUS, *PNDIS_802_11_ENCRYPTION_STATUS;
+
+typedef enum _NDIS_802_11_RELOAD_DEFAULTS
+{
+   Ndis802_11ReloadWEPKeys
+} NDIS_802_11_RELOAD_DEFAULTS, *PNDIS_802_11_RELOAD_DEFAULTS;
+
+#define NDIS_802_11_AI_REQFI_CAPABILITIES      1
+#define NDIS_802_11_AI_REQFI_LISTENINTERVAL    2
+#define NDIS_802_11_AI_REQFI_CURRENTAPADDRESS  4
+
+#define NDIS_802_11_AI_RESFI_CAPABILITIES      1
+#define NDIS_802_11_AI_RESFI_STATUSCODE        2
+#define NDIS_802_11_AI_RESFI_ASSOCIATIONID     4
+
+typedef struct _NDIS_802_11_AI_REQFI
+{
+    USHORT Capabilities;
+    USHORT ListenInterval;
+    NDIS_802_11_MAC_ADDRESS  CurrentAPAddress;
+} NDIS_802_11_AI_REQFI, *PNDIS_802_11_AI_REQFI;
+
+typedef struct _NDIS_802_11_AI_RESFI
+{
+    USHORT Capabilities;
+    USHORT StatusCode;
+    USHORT AssociationId;
+} NDIS_802_11_AI_RESFI, *PNDIS_802_11_AI_RESFI;
+
+typedef struct _NDIS_802_11_ASSOCIATION_INFORMATION
+{
+    ULONG                   Length;
+    USHORT                  AvailableRequestFixedIEs;
+    NDIS_802_11_AI_REQFI    RequestFixedIEs;
+    ULONG                   RequestIELength;
+    ULONG                   OffsetRequestIEs;
+    USHORT                  AvailableResponseFixedIEs;
+    NDIS_802_11_AI_RESFI    ResponseFixedIEs;
+    ULONG                   ResponseIELength;
+    ULONG                   OffsetResponseIEs;
+} NDIS_802_11_ASSOCIATION_INFORMATION, *PNDIS_802_11_ASSOCIATION_INFORMATION;
+
+typedef struct _NDIS_802_11_AUTHENTICATION_EVENT
+{
+    NDIS_802_11_STATUS_INDICATION       Status;
+    NDIS_802_11_AUTHENTICATION_REQUEST  Request[1];
+} NDIS_802_11_AUTHENTICATION_EVENT, *PNDIS_802_11_AUTHENTICATION_EVENT;
+
+/*
+typedef struct _NDIS_802_11_TEST
+{
+    ULONG Length;
+    ULONG Type;
+    union
+    {
+        NDIS_802_11_AUTHENTICATION_EVENT AuthenticationEvent;
+        NDIS_802_11_RSSI RssiTrigger;
+    };
+} NDIS_802_11_TEST, *PNDIS_802_11_TEST;
+ */
+
+// 802.11 Media stream constraints, associated with OID_802_11_MEDIA_STREAM_MODE
+typedef enum _NDIS_802_11_MEDIA_STREAM_MODE
+{
+    Ndis802_11MediaStreamOff,
+    Ndis802_11MediaStreamOn,
+} NDIS_802_11_MEDIA_STREAM_MODE, *PNDIS_802_11_MEDIA_STREAM_MODE;
+
+// PMKID Structures
+typedef UCHAR   NDIS_802_11_PMKID_VALUE[16];
+
+#ifdef CONFIG_STA_SUPPORT
+typedef struct _BSSID_INFO
+{
+    NDIS_802_11_MAC_ADDRESS BSSID;
+    NDIS_802_11_PMKID_VALUE PMKID;
+} BSSID_INFO, *PBSSID_INFO;
+
+typedef struct _NDIS_802_11_PMKID
+{
+    UINT    Length;
+    UINT    BSSIDInfoCount;
+    BSSID_INFO BSSIDInfo[1];
+} NDIS_802_11_PMKID, *PNDIS_802_11_PMKID;
+#endif // CONFIG_STA_SUPPORT //
+
+
+typedef struct _NDIS_802_11_AUTHENTICATION_ENCRYPTION
+{
+    NDIS_802_11_AUTHENTICATION_MODE AuthModeSupported;
+    NDIS_802_11_ENCRYPTION_STATUS EncryptStatusSupported;
+} NDIS_802_11_AUTHENTICATION_ENCRYPTION, *PNDIS_802_11_AUTHENTICATION_ENCRYPTION;
+
+typedef struct _NDIS_802_11_CAPABILITY
+{
+     ULONG Length;
+     ULONG Version;
+     ULONG NoOfPMKIDs;
+     ULONG NoOfAuthEncryptPairsSupported;
+     NDIS_802_11_AUTHENTICATION_ENCRYPTION AuthenticationEncryptionSupported[1];
+} NDIS_802_11_CAPABILITY, *PNDIS_802_11_CAPABILITY;
+
+//#endif //of WIN 2k
+#endif //UNDER_CE
+
+#if WIRELESS_EXT <= 11
+#ifndef SIOCDEVPRIVATE
+#define SIOCDEVPRIVATE                              0x8BE0
+#endif
+#define SIOCIWFIRSTPRIV								SIOCDEVPRIVATE
+#endif
+
+#ifdef CONFIG_STA_SUPPORT
+#define RTPRIV_IOCTL_SET							(SIOCIWFIRSTPRIV + 0x02)
+
+#ifdef DBG
+#define RTPRIV_IOCTL_BBP                            (SIOCIWFIRSTPRIV + 0x03)
+#define RTPRIV_IOCTL_MAC                            (SIOCIWFIRSTPRIV + 0x05)
+#define RTPRIV_IOCTL_E2P                            (SIOCIWFIRSTPRIV + 0x07)
+#endif
+
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+#define RTPRIV_IOCTL_ATE							(SIOCIWFIRSTPRIV + 0x08)
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#define RTPRIV_IOCTL_STATISTICS                     (SIOCIWFIRSTPRIV + 0x09)
+#define RTPRIV_IOCTL_ADD_PMKID_CACHE                (SIOCIWFIRSTPRIV + 0x0A)
+#define RTPRIV_IOCTL_RADIUS_DATA                    (SIOCIWFIRSTPRIV + 0x0C)
+#define RTPRIV_IOCTL_GSITESURVEY					(SIOCIWFIRSTPRIV + 0x0D)
+#define RT_PRIV_IOCTL								(SIOCIWFIRSTPRIV + 0x0E) // Sync. with RT61 (for wpa_supplicant)
+#define RTPRIV_IOCTL_GET_MAC_TABLE					(SIOCIWFIRSTPRIV + 0x0F)
+
+#define RTPRIV_IOCTL_SHOW							(SIOCIWFIRSTPRIV + 0x11)
+enum {
+    SHOW_CONN_STATUS = 4,
+    SHOW_DRVIER_VERION = 5,
+    SHOW_BA_INFO = 6,
+	SHOW_DESC_INFO = 7,
+#ifdef RT2870
+	SHOW_RXBULK_INFO = 8,
+	SHOW_TXBULK_INFO = 9,
+#endif // RT2870 //
+    RAIO_OFF = 10,
+    RAIO_ON = 11,
+#ifdef QOS_DLS_SUPPORT
+	SHOW_DLS_ENTRY_INFO = 19,
+#endif // QOS_DLS_SUPPORT //
+	SHOW_CFG_VALUE = 20,
+	SHOW_ADHOC_ENTRY_INFO = 21,
+};
+
+
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef SNMP_SUPPORT
+//SNMP ieee 802dot11, kathy , 2008_0220
+// dot11res(3)
+#define RT_OID_802_11_MANUFACTUREROUI			0x0700
+#define RT_OID_802_11_MANUFACTURERNAME			0x0701
+#define RT_OID_802_11_RESOURCETYPEIDNAME		0x0702
+
+// dot11smt(1)
+#define RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED	0x0703
+#define RT_OID_802_11_POWERMANAGEMENTMODE		0x0704
+#define OID_802_11_WEPDEFAULTKEYVALUE			0x0705 // read , write
+#define OID_802_11_WEPDEFAULTKEYID				0x0706
+#define RT_OID_802_11_WEPKEYMAPPINGLENGTH		0x0707
+#define OID_802_11_SHORTRETRYLIMIT				0x0708
+#define OID_802_11_LONGRETRYLIMIT				0x0709
+#define RT_OID_802_11_PRODUCTID					0x0710
+#define RT_OID_802_11_MANUFACTUREID				0x0711
+
+// //dot11Phy(4)
+#define OID_802_11_CURRENTCHANNEL				0x0712
+
+//dot11mac
+#define RT_OID_802_11_MAC_ADDRESS				0x0713
+#endif // SNMP_SUPPORT //
+
+#define OID_802_11_BUILD_CHANNEL_EX				0x0714
+#define OID_802_11_GET_CH_LIST					0x0715
+#define OID_802_11_GET_COUNTRY_CODE				0x0716
+#define OID_802_11_GET_CHANNEL_GEOGRAPHY		0x0717
+
+#ifdef LLTD_SUPPORT
+// for consistency with RT61
+#define RT_OID_GET_PHY_MODE                         0x761
+#endif // LLTD_SUPPORT //
+
+// MIMO Tx parameter, ShortGI, MCS, STBC, etc.  these are fields in TXWI. Don't change this definition!!!
+typedef union  _HTTRANSMIT_SETTING {
+#ifdef RT_BIG_ENDIAN
+	struct	{
+	USHORT		MODE:2;	// Use definition MODE_xxx.
+//	USHORT		rsv:3;
+	USHORT		TxBF:1;
+	USHORT		rsv:2;
+	USHORT		STBC:2;	//SPACE
+	USHORT		ShortGI:1;
+	USHORT		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	USHORT   	MCS:7;                 // MCS
+	}	field;
+#else
+	struct	{
+	USHORT   	MCS:7;                 // MCS
+	USHORT		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	USHORT		ShortGI:1;
+	USHORT		STBC:2;	//SPACE
+//	USHORT		rsv:3;
+	USHORT		rsv:2;
+	USHORT		TxBF:1;
+	USHORT		MODE:2;	// Use definition MODE_xxx.
+	}	field;
+#endif
+	USHORT		word;
+ } HTTRANSMIT_SETTING, *PHTTRANSMIT_SETTING;
+
+typedef enum _RT_802_11_PREAMBLE {
+    Rt802_11PreambleLong,
+    Rt802_11PreambleShort,
+    Rt802_11PreambleAuto
+} RT_802_11_PREAMBLE, *PRT_802_11_PREAMBLE;
+
+// Only for STA, need to sync with AP
+typedef enum _RT_802_11_PHY_MODE {
+	PHY_11BG_MIXED = 0,
+	PHY_11B,
+	PHY_11A,
+	PHY_11ABG_MIXED,
+	PHY_11G,
+#ifdef DOT11_N_SUPPORT
+	PHY_11ABGN_MIXED,	// both band   5
+	PHY_11N_2_4G,		// 11n-only with 2.4G band   	6
+	PHY_11GN_MIXED,	// 2.4G band      7
+	PHY_11AN_MIXED,	// 5G  band       8
+	PHY_11BGN_MIXED,	// if check 802.11b.      9
+	PHY_11AGN_MIXED,	// if check 802.11b.      10
+	PHY_11N_5G,			// 11n-only with 5G band		11
+#endif // DOT11_N_SUPPORT //
+} RT_802_11_PHY_MODE;
+
+// put all proprietery for-query objects here to reduce # of Query_OID
+typedef struct _RT_802_11_LINK_STATUS {
+    ULONG   CurrTxRate;         // in units of 0.5Mbps
+    ULONG   ChannelQuality;     // 0..100 %
+    ULONG   TxByteCount;        // both ok and fail
+    ULONG   RxByteCount;        // both ok and fail
+    ULONG	CentralChannel;		// 40MHz central channel number
+} RT_802_11_LINK_STATUS, *PRT_802_11_LINK_STATUS;
+
+typedef struct _RT_802_11_EVENT_LOG {
+    LARGE_INTEGER   SystemTime;  // timestammp via NdisGetCurrentSystemTime()
+    UCHAR           Addr[MAC_ADDR_LENGTH];
+    USHORT          Event;       // EVENT_xxx
+} RT_802_11_EVENT_LOG, *PRT_802_11_EVENT_LOG;
+
+typedef struct _RT_802_11_EVENT_TABLE {
+    ULONG       Num;
+    ULONG       Rsv;     // to align Log[] at LARGE_INEGER boundary
+    RT_802_11_EVENT_LOG   Log[MAX_NUMBER_OF_EVENT];
+} RT_802_11_EVENT_TABLE, PRT_802_11_EVENT_TABLE;
+
+// MIMO Tx parameter, ShortGI, MCS, STBC, etc.  these are fields in TXWI. Don't change this definition!!!
+typedef union  _MACHTTRANSMIT_SETTING {
+	struct	{
+	USHORT   	MCS:7;                 // MCS
+	USHORT		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	USHORT		ShortGI:1;
+	USHORT		STBC:2;	//SPACE
+	USHORT		rsv:3;
+	USHORT		MODE:2;	// Use definition MODE_xxx.
+	}	field;
+	USHORT		word;
+ } MACHTTRANSMIT_SETTING, *PMACHTTRANSMIT_SETTING;
+
+typedef struct _RT_802_11_MAC_ENTRY {
+    UCHAR       Addr[MAC_ADDR_LENGTH];
+    UCHAR       Aid;
+    UCHAR       Psm;     // 0:PWR_ACTIVE, 1:PWR_SAVE
+    UCHAR		MimoPs;  // 0:MMPS_STATIC, 1:MMPS_DYNAMIC, 3:MMPS_Enabled
+    CHAR		AvgRssi0;
+	CHAR		AvgRssi1;
+	CHAR		AvgRssi2;
+	UINT32		ConnectedTime;
+    MACHTTRANSMIT_SETTING	TxRate;
+} RT_802_11_MAC_ENTRY, *PRT_802_11_MAC_ENTRY;
+
+typedef struct _RT_802_11_MAC_TABLE {
+    ULONG       Num;
+    RT_802_11_MAC_ENTRY Entry[MAX_NUMBER_OF_MAC];
+} RT_802_11_MAC_TABLE, *PRT_802_11_MAC_TABLE;
+
+// structure for query/set hardware register - MAC, BBP, RF register
+typedef struct _RT_802_11_HARDWARE_REGISTER {
+    ULONG   HardwareType;       // 0:MAC, 1:BBP, 2:RF register, 3:EEPROM
+    ULONG   Offset;             // Q/S register offset addr
+    ULONG   Data;               // R/W data buffer
+} RT_802_11_HARDWARE_REGISTER, *PRT_802_11_HARDWARE_REGISTER;
+
+// structure to tune BBP R17 "RX AGC VGC init"
+//typedef struct _RT_802_11_RX_AGC_VGC_TUNING {
+//    UCHAR   FalseCcaLowerThreshold;  // 0-255, def 10
+//    UCHAR   FalseCcaUpperThreshold;  // 0-255, def 100
+//    UCHAR   VgcDelta;                // R17 +-= VgcDelta whenever flase CCA over UpprThreshold
+//                                     // or lower than LowerThresholdupper threshold
+//    UCHAR   VgcUpperBound;           // max value of R17
+//} RT_802_11_RX_AGC_VGC_TUNING, *PRT_802_11_RX_AGC_VGC_TUNING;
+
+typedef struct _RT_802_11_AP_CONFIG {
+    ULONG   EnableTxBurst;      // 0-disable, 1-enable
+    ULONG   EnableTurboRate;    // 0-disable, 1-enable 72/100mbps turbo rate
+    ULONG   IsolateInterStaTraffic;     // 0-disable, 1-enable isolation
+    ULONG   HideSsid;           // 0-disable, 1-enable hiding
+    ULONG   UseBGProtection;    // 0-AUTO, 1-always ON, 2-always OFF
+    ULONG   UseShortSlotTime;   // 0-no use, 1-use 9-us short slot time
+    ULONG   Rsv1;               // must be 0
+    ULONG   SystemErrorBitmap;  // ignore upon SET, return system error upon QUERY
+} RT_802_11_AP_CONFIG, *PRT_802_11_AP_CONFIG;
+
+// structure to query/set STA_CONFIG
+typedef struct _RT_802_11_STA_CONFIG {
+    ULONG   EnableTxBurst;      // 0-disable, 1-enable
+    ULONG   EnableTurboRate;    // 0-disable, 1-enable 72/100mbps turbo rate
+    ULONG   UseBGProtection;    // 0-AUTO, 1-always ON, 2-always OFF
+    ULONG   UseShortSlotTime;   // 0-no use, 1-use 9-us short slot time when applicable
+    ULONG   AdhocMode; 			// 0-11b rates only (WIFI spec), 1 - b/g mixed, 2 - g only
+    ULONG   HwRadioStatus;      // 0-OFF, 1-ON, default is 1, Read-Only
+    ULONG   Rsv1;               // must be 0
+    ULONG   SystemErrorBitmap;  // ignore upon SET, return system error upon QUERY
+} RT_802_11_STA_CONFIG, *PRT_802_11_STA_CONFIG;
+
+//
+//  For OID Query or Set about BA structure
+//
+typedef	struct	_OID_BACAP_STRUC	{
+		UCHAR		RxBAWinLimit;
+		UCHAR		TxBAWinLimit;
+		UCHAR		Policy;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use. other value invalid
+		UCHAR		MpduDensity;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use. other value invalid
+		UCHAR       	AmsduEnable;	//Enable AMSDU transmisstion
+		UCHAR       	AmsduSize;	// 0:3839, 1:7935 bytes. UINT  MSDUSizeToBytes[]	= { 3839, 7935};
+		UCHAR       	MMPSmode;	// MIMO power save more, 0:static, 1:dynamic, 2:rsv, 3:mimo enable
+		BOOLEAN		AutoBA;	// Auto BA will automatically
+} OID_BACAP_STRUC, *POID_BACAP_STRUC;
+
+typedef struct _RT_802_11_ACL_ENTRY {
+    UCHAR   Addr[MAC_ADDR_LENGTH];
+    USHORT  Rsv;
+} RT_802_11_ACL_ENTRY, *PRT_802_11_ACL_ENTRY;
+
+typedef struct PACKED _RT_802_11_ACL {
+    ULONG   Policy;             // 0-disable, 1-positive list, 2-negative list
+    ULONG   Num;
+    RT_802_11_ACL_ENTRY Entry[MAX_NUMBER_OF_ACL];
+} RT_802_11_ACL, *PRT_802_11_ACL;
+
+typedef struct _RT_802_11_WDS {
+    ULONG						Num;
+    NDIS_802_11_MAC_ADDRESS		Entry[24/*MAX_NUM_OF_WDS_LINK*/];
+	ULONG						KeyLength;
+	UCHAR						KeyMaterial[32];
+} RT_802_11_WDS, *PRT_802_11_WDS;
+
+typedef struct _RT_802_11_TX_RATES_ {
+    UCHAR       SupRateLen;
+    UCHAR       SupRate[MAX_LENGTH_OF_SUPPORT_RATES];
+    UCHAR       ExtRateLen;
+    UCHAR       ExtRate[MAX_LENGTH_OF_SUPPORT_RATES];
+} RT_802_11_TX_RATES, *PRT_802_11_TX_RATES;
+
+
+// Definition of extra information code
+#define	GENERAL_LINK_UP			0x0			// Link is Up
+#define	GENERAL_LINK_DOWN		0x1			// Link is Down
+#define	HW_RADIO_OFF			0x2			// Hardware radio off
+#define	SW_RADIO_OFF			0x3			// Software radio off
+#define	AUTH_FAIL				0x4			// Open authentication fail
+#define	AUTH_FAIL_KEYS			0x5			// Shared authentication fail
+#define	ASSOC_FAIL				0x6			// Association failed
+#define	EAP_MIC_FAILURE			0x7			// Deauthencation because MIC failure
+#define	EAP_4WAY_TIMEOUT		0x8			// Deauthencation on 4-way handshake timeout
+#define	EAP_GROUP_KEY_TIMEOUT	0x9			// Deauthencation on group key handshake timeout
+#define	EAP_SUCCESS				0xa			// EAP succeed
+#define	DETECT_RADAR_SIGNAL		0xb         // Radar signal occur in current channel
+#define EXTRA_INFO_MAX			0xb			// Indicate Last OID
+
+#define EXTRA_INFO_CLEAR		0xffffffff
+
+// This is OID setting structure. So only GF or MM as Mode. This is valid when our wirelss mode has 802.11n in use.
+typedef struct {
+	RT_802_11_PHY_MODE		PhyMode; 	//
+	UCHAR		TransmitNo;
+	UCHAR		HtMode; 	//HTMODE_GF or HTMODE_MM
+	UCHAR		ExtOffset;	//extension channel above or below
+	UCHAR		MCS;
+	UCHAR   	BW;
+	UCHAR		STBC;
+	UCHAR		SHORTGI;
+	UCHAR		rsv;
+} OID_SET_HT_PHYMODE, *POID_SET_HT_PHYMODE;
+
+#ifdef NINTENDO_AP
+#define NINTENDO_MAX_ENTRY 16
+#define NINTENDO_SSID_NAME_LN 8
+#define NINTENDO_SSID_NAME "NWCUSBAP"
+#define NINTENDO_PROBE_REQ_FLAG_MASK 0x03
+#define NINTENDO_PROBE_REQ_ON 0x01
+#define NINTENDO_PROBE_REQ_SIGNAL 0x02
+#define NINTENDO_PROBE_RSP_ON 0x01
+#define NINTENDO_SSID_NICKNAME_LN 20
+
+#define NINTENDO_WEPKEY_LN 13
+
+typedef struct _NINTENDO_SSID
+{
+	UCHAR	NINTENDOFixChar[NINTENDO_SSID_NAME_LN];
+	UCHAR	zero1;
+	UCHAR	registe;
+	UCHAR	ID;
+	UCHAR	zero2;
+	UCHAR	NICKname[NINTENDO_SSID_NICKNAME_LN];
+} RT_NINTENDO_SSID, *PRT_NINTENDO_SSID;
+
+typedef struct _NINTENDO_ENTRY
+{
+	UCHAR	NICKname[NINTENDO_SSID_NICKNAME_LN];
+    UCHAR   DS_Addr[ETH_LENGTH_OF_ADDRESS];
+	UCHAR	registe;
+	UCHAR	UserSpaceAck;
+} RT_NINTENDO_ENTRY, *PRT_NINTENDO_ENTRY;
+
+//RTPRIV_IOCTL_NINTENDO_GET_TABLE
+//RTPRIV_IOCTL_NINTENDO_SET_TABLE
+typedef struct _NINTENDO_TABLE
+{
+	UINT				number;
+	RT_NINTENDO_ENTRY	entry[NINTENDO_MAX_ENTRY];
+} RT_NINTENDO_TABLE, *PRT_NINTENDO_TABLE;
+
+//RTPRIV_IOCTL_NINTENDO_SEED_WEPKEY
+typedef struct _NINTENDO_SEED_WEPKEY
+{
+	UCHAR	seed[NINTENDO_SSID_NICKNAME_LN];
+	UCHAR	wepkey[16];//use 13 for 104 bits wep key
+} RT_NINTENDO_SEED_WEPKEY, *PRT_NINTENDO_SEED_WEPKEY;
+#endif // NINTENDO_AP //
+
+#ifdef LLTD_SUPPORT
+typedef struct _RT_LLTD_ASSOICATION_ENTRY {
+    UCHAR           Addr[ETH_LENGTH_OF_ADDRESS];
+    unsigned short  MOR;        // maximum operational rate
+    UCHAR           phyMode;
+} RT_LLTD_ASSOICATION_ENTRY, *PRT_LLTD_ASSOICATION_ENTRY;
+
+typedef struct _RT_LLTD_ASSOICATION_TABLE {
+    unsigned int                Num;
+    RT_LLTD_ASSOICATION_ENTRY   Entry[MAX_NUMBER_OF_MAC];
+} RT_LLTD_ASSOICATION_TABLE, *PRT_LLTD_ASSOICATION_TABLE;
+#endif // LLTD_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+//rt2860, kathy 2007-0118
+// structure for DLS
+typedef struct _RT_802_11_DLS_UI {
+	USHORT						TimeOut;		// unit: second , set by UI
+	USHORT						CountDownTimer;	// unit: second , used by driver only
+	NDIS_802_11_MAC_ADDRESS		MacAddr;		// set by UI
+	UCHAR						Status;			// 0: none , 1: wait STAkey, 2: finish DLS setup , set by driver only
+	BOOLEAN						Valid;			// 1: valid , 0: invalid , set by UI, use to setup or tear down DLS link
+} RT_802_11_DLS_UI, *PRT_802_11_DLS_UI;
+
+typedef struct _RT_802_11_DLS_INFO {
+	RT_802_11_DLS_UI	Entry[MAX_NUMBER_OF_DLS_ENTRY];
+	UCHAR				num;
+} RT_802_11_DLS_INFO, *PRT_802_11_DLS_INFO;
+
+typedef enum _RT_802_11_DLS_MODE {
+    DLS_NONE,
+    DLS_WAIT_KEY,
+    DLS_FINISH
+} RT_802_11_DLS_MODE;
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+#define	RT_ASSOC_EVENT_FLAG                         0x0101
+#define	RT_DISASSOC_EVENT_FLAG                      0x0102
+#define	RT_REQIE_EVENT_FLAG                         0x0103
+#define	RT_RESPIE_EVENT_FLAG                        0x0104
+#define	RT_ASSOCINFO_EVENT_FLAG                     0x0105
+#define RT_PMKIDCAND_FLAG                           0x0106
+#define RT_INTERFACE_DOWN                           0x0107
+#define RT_INTERFACE_UP                             0x0108
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+
+#define MAX_CUSTOM_LEN 128
+
+#ifdef CONFIG_STA_SUPPORT
+typedef enum _RT_802_11_D_CLIENT_MODE
+{
+   Rt802_11_D_None,
+   Rt802_11_D_Flexible,
+   Rt802_11_D_Strict,
+} RT_802_11_D_CLIENT_MODE, *PRT_802_11_D_CLIENT_MODE;
+#endif // CONFIG_STA_SUPPORT //
+
+typedef struct _RT_CHANNEL_LIST_INFO
+{
+	UCHAR ChannelList[MAX_NUM_OF_CHS];   // list all supported channels for site survey
+	UCHAR ChannelListNum; // number of channel in ChannelList[]
+} RT_CHANNEL_LIST_INFO, *PRT_CHANNEL_LIST_INFO;
+
+// WSC configured credential
+typedef	struct	_WSC_CREDENTIAL
+{
+	NDIS_802_11_SSID	SSID;				// mandatory
+	USHORT				AuthType;			// mandatory, 1: open, 2: wpa-psk, 4: shared, 8:wpa, 0x10: wpa2, 0x20: wpa2-psk
+	USHORT				EncrType;			// mandatory, 1: none, 2: wep, 4: tkip, 8: aes
+	UCHAR				Key[64];			// mandatory, Maximum 64 byte
+	USHORT				KeyLength;
+	UCHAR				MacAddr[6];			// mandatory, AP MAC address
+	UCHAR				KeyIndex;			// optional, default is 1
+	UCHAR				Rsvd[3];			// Make alignment
+}	WSC_CREDENTIAL, *PWSC_CREDENTIAL;
+
+// WSC configured profiles
+typedef	struct	_WSC_PROFILE
+{
+	UINT			ProfileCnt;
+	WSC_CREDENTIAL	Profile[8];				// Support up to 8 profiles
+}	WSC_PROFILE, *PWSC_PROFILE;
+
+
+#endif // _OID_H_
+
diff --git a/drivers/staging/rt2870/rt2870.h b/drivers/staging/rt2870/rt2870.h
new file mode 100644
index 000000000000..30af4b57c3d8
--- /dev/null
+++ b/drivers/staging/rt2870/rt2870.h
@@ -0,0 +1,761 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __RT2870_H__
+#define __RT2870_H__
+
+//usb header files
+#include <linux/usb.h>
+
+/* rtmp_def.h */
+//
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#define BULKAGGRE_ZISE          100
+#define RT28XX_DRVDATA_SET(_a)                                             usb_set_intfdata(_a, pAd);
+#define RT28XX_PUT_DEVICE                                                  usb_put_dev
+#define RTUSB_ALLOC_URB(iso)                                               usb_alloc_urb(iso, GFP_ATOMIC)
+#define RTUSB_SUBMIT_URB(pUrb)                                             usb_submit_urb(pUrb, GFP_ATOMIC)
+#define	RTUSB_URB_ALLOC_BUFFER(pUsb_Dev, BufSize, pDma_addr)               usb_buffer_alloc(pUsb_Dev, BufSize, GFP_ATOMIC, pDma_addr)
+#define	RTUSB_URB_FREE_BUFFER(pUsb_Dev, BufSize, pTransferBuf, Dma_addr)   usb_buffer_free(pUsb_Dev, BufSize, pTransferBuf, Dma_addr)
+#else
+#define BULKAGGRE_ZISE          60
+#define RT28XX_DRVDATA_SET(_a)
+#define RT28XX_PUT_DEVICE(dev_p)
+#define RTUSB_ALLOC_URB(iso)                                               usb_alloc_urb(iso)
+#define RTUSB_SUBMIT_URB(pUrb)                                             usb_submit_urb(pUrb)
+#define RTUSB_URB_ALLOC_BUFFER(pUsb_Dev, BufSize, pDma_addr)               kmalloc(BufSize, GFP_ATOMIC)
+#define	RTUSB_URB_FREE_BUFFER(pUsb_Dev, BufSize, pTransferBuf, Dma_addr)   kfree(pTransferBuf)
+#endif
+
+#define RXBULKAGGRE_ZISE        12
+#define MAX_TXBULK_LIMIT        (LOCAL_TXBUF_SIZE*(BULKAGGRE_ZISE-1))
+#define MAX_TXBULK_SIZE         (LOCAL_TXBUF_SIZE*BULKAGGRE_ZISE)
+#define MAX_RXBULK_SIZE         (LOCAL_TXBUF_SIZE*RXBULKAGGRE_ZISE)
+#define MAX_MLME_HANDLER_MEMORY 20
+#define	RETRY_LIMIT             10
+#define BUFFER_SIZE				2400	//2048
+#define	TX_RING					0xa
+#define	PRIO_RING				0xc
+
+
+// Flags for Bulkflags control for bulk out data
+//
+#define	fRTUSB_BULK_OUT_DATA_NULL				0x00000001
+#define fRTUSB_BULK_OUT_RTS						0x00000002
+#define	fRTUSB_BULK_OUT_MLME					0x00000004
+
+#define	fRTUSB_BULK_OUT_DATA_NORMAL				0x00010000
+#define	fRTUSB_BULK_OUT_DATA_NORMAL_2			0x00020000
+#define	fRTUSB_BULK_OUT_DATA_NORMAL_3			0x00040000
+#define	fRTUSB_BULK_OUT_DATA_NORMAL_4			0x00080000
+
+#define	fRTUSB_BULK_OUT_PSPOLL					0x00000020
+#define	fRTUSB_BULK_OUT_DATA_FRAG				0x00000040
+#define	fRTUSB_BULK_OUT_DATA_FRAG_2				0x00000080
+#define	fRTUSB_BULK_OUT_DATA_FRAG_3				0x00000100
+#define	fRTUSB_BULK_OUT_DATA_FRAG_4				0x00000200
+
+#ifdef RALINK_ATE
+#define	fRTUSB_BULK_OUT_DATA_ATE				0x00100000
+#endif // RALINK_ATE //
+
+#define RT2870_USB_DEVICES	\
+{	\
+	{USB_DEVICE(0x148F,0x2770)}, /* Ralink */		\
+	{USB_DEVICE(0x148F,0x2870)}, /* Ralink */		\
+	{USB_DEVICE(0x148F,0x3070)}, /* Ralink */		\
+	{USB_DEVICE(0x0B05,0x1731)}, /* Asus */			\
+	{USB_DEVICE(0x0B05,0x1732)}, /* Asus */			\
+	{USB_DEVICE(0x0B05,0x1742)}, /* Asus */			\
+	{USB_DEVICE(0x0DF6,0x0017)}, /* Sitecom */		\
+	{USB_DEVICE(0x0DF6,0x002B)}, /* Sitecom */		\
+	{USB_DEVICE(0x0DF6,0x002C)}, /* Sitecom */		\
+	{USB_DEVICE(0x0DF6,0x002D)}, /* Sitecom */		\
+	{USB_DEVICE(0x14B2,0x3C06)}, /* Conceptronic */		\
+	{USB_DEVICE(0x14B2,0x3C28)}, /* Conceptronic */		\
+	{USB_DEVICE(0x2019,0xED06)}, /* Planex Communications, Inc. */		\
+	{USB_DEVICE(0x2019,0xAB25)}, /* Planex Communications, Inc. RT3070 */		\
+	{USB_DEVICE(0x07D1,0x3C09)}, /* D-Link */		\
+	{USB_DEVICE(0x07D1,0x3C11)}, /* D-Link */		\
+	{USB_DEVICE(0x14B2,0x3C07)}, /* AL */			\
+	{USB_DEVICE(0x14B2,0x3C12)}, /* AL */           \
+	{USB_DEVICE(0x050D,0x8053)}, /* Belkin */		\
+	{USB_DEVICE(0x14B2,0x3C23)}, /* Airlink */		\
+	{USB_DEVICE(0x14B2,0x3C27)}, /* Airlink */		\
+	{USB_DEVICE(0x07AA,0x002F)}, /* Corega */		\
+	{USB_DEVICE(0x07AA,0x003C)}, /* Corega */		\
+	{USB_DEVICE(0x07AA,0x003F)}, /* Corega */		\
+	{USB_DEVICE(0x18C5,0x0012)}, /* Corega */		\
+	{USB_DEVICE(0x1044,0x800B)}, /* Gigabyte */		\
+	{USB_DEVICE(0x15A9,0x0006)}, /* Sparklan */		\
+	{USB_DEVICE(0x083A,0xB522)}, /* SMC */			\
+	{USB_DEVICE(0x083A,0xA618)}, /* SMC */			\
+	{USB_DEVICE(0x083A,0x7522)}, /* Arcadyan */		\
+	{USB_DEVICE(0x0CDE,0x0022)}, /* ZCOM */			\
+	{USB_DEVICE(0x0586,0x3416)}, /* Zyxel */		\
+	{USB_DEVICE(0x0CDE,0x0025)}, /* Zyxel */		\
+	{USB_DEVICE(0x1740,0x9701)}, /* EnGenius */		\
+	{USB_DEVICE(0x1740,0x9702)}, /* EnGenius */		\
+	{USB_DEVICE(0x0471,0x200f)}, /* Philips */		\
+	{USB_DEVICE(0x14B2,0x3C25)}, /* Draytek */		\
+	{USB_DEVICE(0x13D3,0x3247)}, /* AzureWave */	\
+	{USB_DEVICE(0x083A,0x6618)}, /* Accton */		\
+	{USB_DEVICE(0x15c5,0x0008)}, /* Amit */			\
+	{USB_DEVICE(0x0E66,0x0001)}, /* Hawking */		\
+	{USB_DEVICE(0x0E66,0x0003)}, /* Hawking */		\
+	{USB_DEVICE(0x129B,0x1828)}, /* Siemens */		\
+	{USB_DEVICE(0x157E,0x300E)},	/* U-Media */	\
+	{USB_DEVICE(0x050d,0x805c)},					\
+	{USB_DEVICE(0x1482,0x3C09)}, /* Abocom*/		\
+	{USB_DEVICE(0x14B2,0x3C09)}, /* Alpha */		\
+	{USB_DEVICE(0x04E8,0x2018)}, /* samsung */  	\
+	{USB_DEVICE(0x07B8,0x3070)}, /* AboCom */		\
+	{USB_DEVICE(0x07B8,0x3071)}, /* AboCom */		\
+	{USB_DEVICE(0x07B8,0x2870)}, /* AboCom */		\
+	{USB_DEVICE(0x07B8,0x2770)}, /* AboCom */		\
+	{USB_DEVICE(0x7392,0x7711)}, /* Edimax */		\
+	{USB_DEVICE(0x5A57,0x0280)}, /* Zinwell */		\
+	{USB_DEVICE(0x5A57,0x0282)}, /* Zinwell */		\
+	{USB_DEVICE(0x0789,0x0162)}, /* Logitec */		\
+	{USB_DEVICE(0x0789,0x0163)}, /* Logitec */		\
+	{USB_DEVICE(0x0789,0x0164)}, /* Logitec */		\
+	{ }/* Terminating entry */                      \
+}
+
+#define	FREE_HTTX_RING(_p, _b, _t)			\
+{										\
+	if ((_t)->ENextBulkOutPosition == (_t)->CurWritePosition)				\
+	{																	\
+		(_t)->bRingEmpty = TRUE;			\
+	}																	\
+	/*NdisInterlockedDecrement(&(_p)->TxCount); */\
+}
+
+//
+// RXINFO appends at the end of each rx packet.
+//
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _RXINFO_STRUC {
+	UINT32		PlcpSignal:12;
+	UINT32		LastAMSDU:1;
+	UINT32		CipherAlg:1;
+	UINT32		PlcpRssil:1;
+	UINT32		Decrypted:1;
+	UINT32		AMPDU:1;		// To be moved
+	UINT32		L2PAD:1;
+	UINT32		RSSI:1;
+	UINT32		HTC:1;
+	UINT32		AMSDU:1;		// rx with 802.3 header, not 802.11 header.
+	UINT32		CipherErr:2;        // 0: decryption okay, 1:ICV error, 2:MIC error, 3:KEY not valid
+	UINT32		Crc:1;              // 1: CRC error
+	UINT32		MyBss:1;  	// 1: this frame belongs to the same BSSID
+	UINT32		Bcast:1;            // 1: this is a broadcast frame
+	UINT32		Mcast:1;            // 1: this is a multicast frame
+	UINT32		U2M:1;              // 1: this RX frame is unicast to me
+	UINT32		FRAG:1;
+	UINT32		NULLDATA:1;
+	UINT32		DATA:1;
+	UINT32		BA:1;
+}	RXINFO_STRUC, *PRXINFO_STRUC, RT28XX_RXD_STRUC, *PRT28XX_RXD_STRUC;
+#else
+typedef	struct	PACKED _RXINFO_STRUC {
+	UINT32		BA:1;
+	UINT32		DATA:1;
+	UINT32		NULLDATA:1;
+	UINT32		FRAG:1;
+	UINT32		U2M:1;              // 1: this RX frame is unicast to me
+	UINT32		Mcast:1;            // 1: this is a multicast frame
+	UINT32		Bcast:1;            // 1: this is a broadcast frame
+	UINT32		MyBss:1;  	// 1: this frame belongs to the same BSSID
+	UINT32		Crc:1;              // 1: CRC error
+	UINT32		CipherErr:2;        // 0: decryption okay, 1:ICV error, 2:MIC error, 3:KEY not valid
+	UINT32		AMSDU:1;		// rx with 802.3 header, not 802.11 header.
+	UINT32		HTC:1;
+	UINT32		RSSI:1;
+	UINT32		L2PAD:1;
+	UINT32		AMPDU:1;		// To be moved
+	UINT32		Decrypted:1;
+	UINT32		PlcpRssil:1;
+	UINT32		CipherAlg:1;
+	UINT32		LastAMSDU:1;
+	UINT32		PlcpSignal:12;
+}	RXINFO_STRUC, *PRXINFO_STRUC, RT28XX_RXD_STRUC, *PRT28XX_RXD_STRUC;
+#endif
+
+
+//
+// TXINFO
+//
+#ifdef RT_BIG_ENDIAN
+typedef	struct	_TXINFO_STRUC {
+	// Word	0
+	UINT32		USBDMATxburst:1;//used ONLY in USB bulk Aggre. Force USB DMA transmit frame from current selected endpoint
+	UINT32		USBDMANextVLD:1;	//used ONLY in USB bulk Aggregation, NextValid
+	UINT32		rsv2:2;  // Software use.
+	UINT32		SwUseLastRound:1; // Software use.
+	UINT32		QSEL:2;	// select on-chip FIFO ID for 2nd-stage output scheduler.0:MGMT, 1:HCCA 2:EDCA
+	UINT32		WIV:1;	// Wireless Info Valid. 1 if Driver already fill WI,  o if DMA needs to copy WI to correctposition
+	UINT32		rsv:8;
+	UINT32		USBDMATxPktLen:16;	//used ONLY in USB bulk Aggregation,  Total byte counts of all sub-frame.
+}	TXINFO_STRUC, *PTXINFO_STRUC;
+#else
+typedef	struct	_TXINFO_STRUC {
+	// Word	0
+	UINT32		USBDMATxPktLen:16;	//used ONLY in USB bulk Aggregation,  Total byte counts of all sub-frame.
+	UINT32		rsv:8;
+	UINT32		WIV:1;	// Wireless Info Valid. 1 if Driver already fill WI,  o if DMA needs to copy WI to correctposition
+	UINT32		QSEL:2;	// select on-chip FIFO ID for 2nd-stage output scheduler.0:MGMT, 1:HCCA 2:EDCA
+	UINT32		SwUseLastRound:1; // Software use.
+	UINT32		rsv2:2;  // Software use.
+	UINT32		USBDMANextVLD:1;	//used ONLY in USB bulk Aggregation, NextValid
+	UINT32		USBDMATxburst:1;//used ONLY in USB bulk Aggre. Force USB DMA transmit frame from current selected endpoint
+}	TXINFO_STRUC, *PTXINFO_STRUC;
+#endif
+
+#define TXINFO_SIZE				4
+#define RXINFO_SIZE				4
+#define TXPADDING_SIZE			11
+
+//
+// Management ring buffer format
+//
+typedef	struct	_MGMT_STRUC	{
+	BOOLEAN		Valid;
+	PUCHAR		pBuffer;
+	ULONG		Length;
+}	MGMT_STRUC, *PMGMT_STRUC;
+
+
+/* ----------------- EEPROM Related MACRO ----------------- */
+#define RT28xx_EEPROM_READ16(pAd, offset, var)					\
+	do {														\
+		RTUSBReadEEPROM(pAd, offset, (PUCHAR)&(var), 2);		\
+		var = le2cpu16(var);									\
+	}while(0)
+
+#define RT28xx_EEPROM_WRITE16(pAd, offset, var)					\
+	do{															\
+		USHORT _tmpVar;											\
+		_tmpVar = cpu2le16(var);								\
+		RTUSBWriteEEPROM(pAd, offset, (PUCHAR)&(_tmpVar), 2);	\
+	}while(0)
+
+/* ----------------- TASK/THREAD Related MACRO ----------------- */
+#define RT28XX_TASK_THREAD_INIT(pAd, Status)		\
+	Status = CreateThreads(net_dev);
+
+
+/* ----------------- Frimware Related MACRO ----------------- */
+#if 0
+#define RT28XX_FIRMUD_INIT(pAd)		\
+	{	UINT32	MacReg;				\
+		RTUSBReadMACRegister(pAd, MAC_CSR0, &MacReg); }
+
+#define RT28XX_FIRMUD_END(pAd)	\
+	RTUSBWriteMACRegister(pAd, 0x7014, 0xffffffff);	\
+	RTUSBWriteMACRegister(pAd, 0x701c, 0xffffffff);	\
+	RTUSBFirmwareRun(pAd);
+#else
+#define RT28XX_WRITE_FIRMWARE(_pAd, _pFwImage, _FwLen)		\
+	RTUSBFirmwareWrite(_pAd, _pFwImage, _FwLen)
+#endif
+
+/* ----------------- TX Related MACRO ----------------- */
+#define RT28XX_START_DEQUEUE(pAd, QueIdx, irqFlags)				\
+			{													\
+				RTMP_IRQ_LOCK(&pAd->DeQueueLock[QueIdx], irqFlags);		\
+				if (pAd->DeQueueRunning[QueIdx])						\
+				{														\
+					RTMP_IRQ_UNLOCK(&pAd->DeQueueLock[QueIdx], irqFlags);\
+					printk("DeQueueRunning[%d]= TRUE!\n", QueIdx);		\
+					continue;											\
+				}														\
+				else													\
+				{														\
+					pAd->DeQueueRunning[QueIdx] = TRUE;					\
+					RTMP_IRQ_UNLOCK(&pAd->DeQueueLock[QueIdx], irqFlags);\
+				}														\
+			}
+#define RT28XX_STOP_DEQUEUE(pAd, QueIdx, irqFlags)						\
+			do{															\
+				RTMP_IRQ_LOCK(&pAd->DeQueueLock[QueIdx], irqFlags);		\
+				pAd->DeQueueRunning[QueIdx] = FALSE;					\
+				RTMP_IRQ_UNLOCK(&pAd->DeQueueLock[QueIdx], irqFlags);	\
+			}while(0)
+
+
+#define	RT28XX_HAS_ENOUGH_FREE_DESC(pAd, pTxBlk, freeNum, pPacket) \
+		(RTUSBFreeDescriptorRequest(pAd, pTxBlk->QueIdx, (pTxBlk->TotalFrameLen + GET_OS_PKT_LEN(pPacket))) == NDIS_STATUS_SUCCESS)
+
+#define RT28XX_RELEASE_DESC_RESOURCE(pAd, QueIdx)			\
+		do{}while(0)
+
+#define NEED_QUEUE_BACK_FOR_AGG(_pAd, _QueIdx, _freeNum, _TxFrameType) 		\
+		((_TxFrameType == TX_RALINK_FRAME) && (RTUSBNeedQueueBackForAgg(_pAd, _QueIdx)))
+
+
+
+#define fRTMP_ADAPTER_NEED_STOP_TX		\
+		(fRTMP_ADAPTER_NIC_NOT_EXIST | fRTMP_ADAPTER_HALT_IN_PROGRESS |	\
+		 fRTMP_ADAPTER_RESET_IN_PROGRESS | fRTMP_ADAPTER_BULKOUT_RESET | \
+		 fRTMP_ADAPTER_RADIO_OFF | fRTMP_ADAPTER_REMOVE_IN_PROGRESS)
+
+
+#define HAL_WriteSubTxResource(pAd, pTxBlk, bIsLast, pFreeNumber)	\
+			RtmpUSB_WriteSubTxResource(pAd, pTxBlk, bIsLast, pFreeNumber)
+
+#define HAL_WriteTxResource(pAd, pTxBlk,bIsLast, pFreeNumber)	\
+			RtmpUSB_WriteSingleTxResource(pAd, pTxBlk,bIsLast, pFreeNumber)
+
+#define HAL_WriteFragTxResource(pAd, pTxBlk, fragNum, pFreeNumber) \
+			RtmpUSB_WriteFragTxResource(pAd, pTxBlk, fragNum, pFreeNumber)
+
+#define HAL_WriteMultiTxResource(pAd, pTxBlk,frameNum, pFreeNumber)	\
+			RtmpUSB_WriteMultiTxResource(pAd, pTxBlk,frameNum, pFreeNumber)
+
+#define HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, TxIdx)	\
+			RtmpUSB_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, TxIdx)
+
+#define HAL_LastTxIdx(pAd, QueIdx,TxIdx) \
+			/*RtmpUSBDataLastTxIdx(pAd, QueIdx,TxIdx)*/
+
+#define HAL_KickOutTx(pAd, pTxBlk, QueIdx)	\
+			RtmpUSBDataKickOut(pAd, pTxBlk, QueIdx)
+
+
+#define HAL_KickOutMgmtTx(pAd, QueIdx, pPacket, pSrcBufVA, SrcBufLen)	\
+			RtmpUSBMgmtKickOut(pAd, QueIdx, pPacket, pSrcBufVA, SrcBufLen)
+
+#define HAL_KickOutNullFrameTx(_pAd, _QueIdx, _pNullFrame, _frameLen)	\
+			RtmpUSBNullFrameKickOut(_pAd, _QueIdx, _pNullFrame, _frameLen)
+
+#define RTMP_PKT_TAIL_PADDING 	11 // 3(max 4 byte padding) + 4 (last packet padding) + 4 (MaxBulkOutsize align padding)
+
+extern UCHAR EpToQueue[6];
+
+
+#ifdef RT2870
+#define GET_TXRING_FREENO(_pAd, _QueIdx) 	(_QueIdx) //(_pAd->TxRing[_QueIdx].TxSwFreeIdx)
+#define GET_MGMTRING_FREENO(_pAd) 			(_pAd->MgmtRing.TxSwFreeIdx)
+#endif // RT2870 //
+
+
+/* ----------------- RX Related MACRO ----------------- */
+//#define RT28XX_RX_ERROR_CHECK				RTMPCheckRxWI
+
+#if 0
+#define RT28XX_RCV_INIT(pAd)					\
+	pAd->TransferBufferLength = 0;				\
+	pAd->ReadPosition = 0;						\
+	pAd->pCurrRxContext = NULL;
+#endif
+
+#define RT28XX_RV_ALL_BUF_END(bBulkReceive)		\
+	/* We return STATUS_MORE_PROCESSING_REQUIRED so that the completion */	\
+	/* routine (IofCompleteRequest) will stop working on the irp. */		\
+	if (bBulkReceive == TRUE)	RTUSBBulkReceive(pAd);
+
+
+/* ----------------- ASIC Related MACRO ----------------- */
+#if 0
+#define RT28XX_DMA_WRITE_INIT(GloCfg)			\
+	{	GloCfg.field.EnTXWriteBackDDONE = 1;	\
+		GloCfg.field.EnableRxDMA = 1;			\
+		GloCfg.field.EnableTxDMA = 1; }
+
+#define RT28XX_DMA_POST_WRITE(_pAd)				\
+	do{	USB_DMA_CFG_STRUC	UsbCfg;				\
+		UsbCfg.word = 0;						\
+		/* for last packet, PBF might use more than limited, so minus 2 to prevent from error */ \
+		UsbCfg.field.RxBulkAggLmt = (MAX_RXBULK_SIZE /1024)-3;	\
+		UsbCfg.field.phyclear = 0;								\
+		/* usb version is 1.1,do not use bulk in aggregation */	\
+		if (_pAd->BulkInMaxPacketSize == 512)					\
+			UsbCfg.field.RxBulkAggEn = 1;						\
+		UsbCfg.field.RxBulkEn = 1;								\
+		UsbCfg.field.TxBulkEn = 1;								\
+		UsbCfg.field.RxBulkAggTOut = 0x80; /* 2006-10-18 */		\
+		RTUSBWriteMACRegister(_pAd, USB_DMA_CFG, UsbCfg.word); 	\
+	}while(0)
+#endif
+
+// reset MAC of a station entry to 0xFFFFFFFFFFFF
+#define RT28XX_STA_ENTRY_MAC_RESET(pAd, Wcid)					\
+	{	RT_SET_ASIC_WCID	SetAsicWcid;						\
+		SetAsicWcid.WCID = Wcid;								\
+		SetAsicWcid.SetTid = 0xffffffff;						\
+		SetAsicWcid.DeleteTid = 0xffffffff;						\
+		RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_SET_ASIC_WCID, 	\
+				&SetAsicWcid, sizeof(RT_SET_ASIC_WCID));	}
+
+// add this entry into ASIC RX WCID search table
+#define RT28XX_STA_ENTRY_ADD(pAd, pEntry)							\
+	RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_SET_CLIENT_MAC_ENTRY, 	\
+							pEntry, sizeof(MAC_TABLE_ENTRY));
+
+// remove Pair-wise key material from ASIC
+// yet implement
+#define RT28XX_STA_ENTRY_KEY_DEL(pAd, BssIdx, Wcid)
+
+// add Client security information into ASIC WCID table and IVEIV table
+#define RT28XX_STA_SECURITY_INFO_ADD(pAd, apidx, KeyID, pEntry)						\
+	{	RT28XX_STA_ENTRY_MAC_RESET(pAd, pEntry->Aid);								\
+		if (pEntry->Aid >= 1) {														\
+			RT_SET_ASIC_WCID_ATTRI	SetAsicWcidAttri;								\
+			SetAsicWcidAttri.WCID = pEntry->Aid;									\
+			if ((pEntry->AuthMode <= Ndis802_11AuthModeAutoSwitch) &&				\
+				(pEntry->WepStatus == Ndis802_11Encryption1Enabled))				\
+			{																		\
+				SetAsicWcidAttri.Cipher = pAd->SharedKey[apidx][KeyID].CipherAlg;	\
+			}																		\
+			else if (pEntry->AuthMode == Ndis802_11AuthModeWPANone)					\
+			{																		\
+				SetAsicWcidAttri.Cipher = pAd->SharedKey[apidx][KeyID].CipherAlg;	\
+			}																		\
+			else SetAsicWcidAttri.Cipher = 0;										\
+            DBGPRINT(RT_DEBUG_TRACE, ("aid cipher = %ld\n",SetAsicWcidAttri.Cipher));       \
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_SET_ASIC_WCID_CIPHER, 			\
+							&SetAsicWcidAttri, sizeof(RT_SET_ASIC_WCID_ATTRI)); } }
+
+// Insert the BA bitmap to ASIC for the Wcid entry
+#define RT28XX_ADD_BA_SESSION_TO_ASIC(_pAd, _Aid, _TID)					\
+		do{																\
+			RT_SET_ASIC_WCID	SetAsicWcid;							\
+			SetAsicWcid.WCID = (_Aid);									\
+			SetAsicWcid.SetTid = (0x10000<<(_TID));						\
+			SetAsicWcid.DeleteTid = 0xffffffff;							\
+			RTUSBEnqueueInternalCmd((_pAd), CMDTHREAD_SET_ASIC_WCID, &SetAsicWcid, sizeof(RT_SET_ASIC_WCID));	\
+		}while(0)
+
+// Remove the BA bitmap from ASIC for the Wcid entry
+#define RT28XX_DEL_BA_SESSION_FROM_ASIC(_pAd, _Wcid, _TID)				\
+		do{																\
+			RT_SET_ASIC_WCID	SetAsicWcid;							\
+			SetAsicWcid.WCID = (_Wcid);									\
+			SetAsicWcid.SetTid = (0xffffffff);							\
+			SetAsicWcid.DeleteTid = (0x10000<<(_TID) );					\
+			RTUSBEnqueueInternalCmd((_pAd), CMDTHREAD_SET_ASIC_WCID, &SetAsicWcid, sizeof(RT_SET_ASIC_WCID));	\
+		}while(0)
+
+
+/* ----------------- PCI/USB Related MACRO ----------------- */
+#define RT28XX_HANDLE_DEV_ASSIGN(handle, dev_p)			\
+	((POS_COOKIE)handle)->pUsb_Dev = dev_p;
+
+// no use
+#define RT28XX_UNMAP()
+#define RT28XX_IRQ_REQUEST(net_dev)
+#define RT28XX_IRQ_RELEASE(net_dev)
+#define RT28XX_IRQ_INIT(pAd)
+#define RT28XX_IRQ_ENABLE(pAd)
+
+
+/* ----------------- MLME Related MACRO ----------------- */
+#define RT28XX_MLME_HANDLER(pAd)			RTUSBMlmeUp(pAd)
+
+#define RT28XX_MLME_PRE_SANITY_CHECK(pAd)								\
+	{	if ((pAd->CommonCfg.bHardwareRadio == TRUE) && 					\
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&		\
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))) {	\
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_CHECK_GPIO, NULL, 0); } }
+
+#define RT28XX_MLME_STA_QUICK_RSP_WAKE_UP(pAd)	\
+	{	RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_QKERIODIC_EXECUT, NULL, 0);	\
+		RTUSBMlmeUp(pAd); }
+
+#define RT28XX_MLME_RESET_STATE_MACHINE(pAd)	\
+		        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_RESET_CONF, 0, NULL);	\
+		        RTUSBMlmeUp(pAd);
+
+#define RT28XX_HANDLE_COUNTER_MEASURE(_pAd, _pEntry)		\
+	{	RTUSBEnqueueInternalCmd(_pAd, CMDTHREAD_802_11_COUNTER_MEASURE, _pEntry, sizeof(MAC_TABLE_ENTRY));	\
+		RTUSBMlmeUp(_pAd);									\
+	}
+
+
+/* ----------------- Power Save Related MACRO ----------------- */
+#define RT28XX_PS_POLL_ENQUEUE(pAd)						\
+	{	RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_PSPOLL);	\
+		RTUSBKickBulkOut(pAd); }
+
+#define RT28xx_CHIP_NAME            "RT2870"
+#define USB_CYC_CFG                 0x02a4
+#define STATUS_SUCCESS				0x00
+#define STATUS_UNSUCCESSFUL 		0x01
+#define NT_SUCCESS(status)			(((status) > 0) ? (1):(0))
+#define InterlockedIncrement 	 	atomic_inc
+#define NdisInterlockedIncrement 	atomic_inc
+#define InterlockedDecrement		atomic_dec
+#define NdisInterlockedDecrement 	atomic_dec
+#define InterlockedExchange			atomic_set
+//#define NdisMSendComplete			RTMP_SendComplete
+#define NdisMCancelTimer			RTMPCancelTimer
+#define NdisAllocMemory(_ptr, _size, _flag)	\
+									do{_ptr = kmalloc((_size),(_flag));}while(0)
+#define NdisFreeMemory(a, b, c) 	kfree((a))
+#define NdisMSleep					RTMPusecDelay		/* unit: microsecond */
+
+
+#define USBD_TRANSFER_DIRECTION_OUT		0
+#define USBD_TRANSFER_DIRECTION_IN		0
+#define USBD_SHORT_TRANSFER_OK			0
+#define PURB			purbb_t
+
+#define RTUSB_FREE_URB(pUrb)	usb_free_urb(pUrb)
+
+//#undef MlmeAllocateMemory
+//#undef MlmeFreeMemory
+
+typedef int				NTSTATUS;
+typedef struct usb_device	* PUSB_DEV;
+
+/* MACRO for linux usb */
+typedef struct urb *purbb_t;
+typedef struct usb_ctrlrequest devctrlrequest;
+#define PIRP		PVOID
+#define PMDL		PVOID
+#define NDIS_OID	UINT
+#ifndef USB_ST_NOERROR
+#define USB_ST_NOERROR     0
+#endif
+
+// vendor-specific control operations
+#define CONTROL_TIMEOUT_JIFFIES ( (100 * HZ) / 1000)
+#define UNLINK_TIMEOUT_MS		3
+
+/* unlink urb	*/
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,7)
+#define RTUSB_UNLINK_URB(pUrb)		usb_kill_urb(pUrb)
+#else
+#define RTUSB_UNLINK_URB(pUrb)		usb_unlink_urb(pUrb)
+#endif
+
+// Prototypes of completion funuc.
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+#define RTUSBBulkOutDataPacketComplete(purb, pt_regs)    RTUSBBulkOutDataPacketComplete(purb)
+#define RTUSBBulkOutMLMEPacketComplete(pUrb, pt_regs)    RTUSBBulkOutMLMEPacketComplete(pUrb)
+#define RTUSBBulkOutNullFrameComplete(pUrb, pt_regs)     RTUSBBulkOutNullFrameComplete(pUrb)
+#define RTUSBBulkOutRTSFrameComplete(pUrb, pt_regs)      RTUSBBulkOutRTSFrameComplete(pUrb)
+#define RTUSBBulkOutPsPollComplete(pUrb, pt_regs)        RTUSBBulkOutPsPollComplete(pUrb)
+#define RTUSBBulkRxComplete(pUrb, pt_regs)               RTUSBBulkRxComplete(pUrb)
+#endif
+
+
+VOID RTUSBBulkOutDataPacketComplete(purbb_t purb, struct pt_regs *pt_regs);
+VOID RTUSBBulkOutMLMEPacketComplete(purbb_t pUrb, struct pt_regs *pt_regs);
+VOID RTUSBBulkOutNullFrameComplete(purbb_t pUrb, struct pt_regs *pt_regs);
+VOID RTUSBBulkOutRTSFrameComplete(purbb_t pUrb, struct pt_regs *pt_regs);
+VOID RTUSBBulkOutPsPollComplete(purbb_t pUrb, struct pt_regs *pt_regs);
+VOID RTUSBBulkRxComplete(purbb_t pUrb, struct pt_regs *pt_regs);
+
+
+#define RTUSBMlmeUp(pAd)	        \
+{								    \
+	POS_COOKIE pObj = (POS_COOKIE) pAd->OS_Cookie;	\
+	CHECK_PID_LEGALITY(pObj->MLMEThr_pid)		    \
+        up(&(pAd->mlme_semaphore)); \
+}
+
+#define RTUSBCMDUp(pAd)	                \
+{									    \
+	POS_COOKIE pObj = (POS_COOKIE) pAd->OS_Cookie;	\
+	CHECK_PID_LEGALITY(pObj->RTUSBCmdThr_pid)	    \
+	    up(&(pAd->RTUSBCmd_semaphore)); \
+}
+
+
+static inline NDIS_STATUS RTMPAllocateMemory(
+	OUT PVOID *ptr,
+	IN size_t size)
+{
+	*ptr = kmalloc(size, GFP_ATOMIC);
+	if(*ptr)
+		return NDIS_STATUS_SUCCESS;
+	else
+		return NDIS_STATUS_RESOURCES;
+}
+
+/* rtmp.h */
+#define	BEACON_RING_SIZE                2
+#define DEVICE_VENDOR_REQUEST_OUT       0x40
+#define DEVICE_VENDOR_REQUEST_IN        0xc0
+#define INTERFACE_VENDOR_REQUEST_OUT    0x41
+#define INTERFACE_VENDOR_REQUEST_IN     0xc1
+#define MGMTPIPEIDX						0	// EP6 is highest priority
+
+#define BULKOUT_MGMT_RESET_FLAG				0x80
+
+#define RTUSB_SET_BULK_FLAG(_M, _F)				((_M)->BulkFlags |= (_F))
+#define RTUSB_CLEAR_BULK_FLAG(_M, _F)			((_M)->BulkFlags &= ~(_F))
+#define RTUSB_TEST_BULK_FLAG(_M, _F)			(((_M)->BulkFlags & (_F)) != 0)
+
+#define EnqueueCmd(cmdq, cmdqelmt)		\
+{										\
+	if (cmdq->size == 0)				\
+		cmdq->head = cmdqelmt;			\
+	else								\
+		cmdq->tail->next = cmdqelmt;	\
+	cmdq->tail = cmdqelmt;				\
+	cmdqelmt->next = NULL;				\
+	cmdq->size++;						\
+}
+
+typedef struct   _RT_SET_ASIC_WCID {
+	ULONG WCID;          // mechanism for rekeying: 0:disable, 1: time-based, 2: packet-based
+	ULONG SetTid;        // time-based: seconds, packet-based: kilo-packets
+	ULONG DeleteTid;        // time-based: seconds, packet-based: kilo-packets
+	UCHAR Addr[MAC_ADDR_LEN];	// avoid in interrupt when write key
+} RT_SET_ASIC_WCID,*PRT_SET_ASIC_WCID;
+
+typedef struct   _RT_SET_ASIC_WCID_ATTRI {
+	ULONG	WCID;          // mechanism for rekeying: 0:disable, 1: time-based, 2: packet-based
+	ULONG	Cipher;        // ASIC Cipher definition
+	UCHAR	Addr[ETH_LENGTH_OF_ADDRESS];
+} RT_SET_ASIC_WCID_ATTRI,*PRT_SET_ASIC_WCID_ATTRI;
+
+typedef struct _MLME_MEMORY_STRUCT {
+	PVOID                           AllocVa;    //Pointer to the base virtual address of the allocated memory
+	struct _MLME_MEMORY_STRUCT      *Next;      //Pointer to the next virtual address of the allocated memory
+}   MLME_MEMORY_STRUCT, *PMLME_MEMORY_STRUCT;
+
+typedef struct  _MLME_MEMORY_HANDLER {
+	BOOLEAN                 MemRunning;         //The flag of the Mlme memory handler's status
+	UINT                    MemoryCount;        //Total nonpaged system-space memory not size
+	UINT                    InUseCount;         //Nonpaged system-space memory in used counts
+	UINT                    UnUseCount;         //Nonpaged system-space memory available counts
+	INT                    PendingCount;       //Nonpaged system-space memory for free counts
+	PMLME_MEMORY_STRUCT     pInUseHead;         //Pointer to the first nonpaed memory not used
+	PMLME_MEMORY_STRUCT     pInUseTail;         //Pointer to the last nonpaged memory not used
+	PMLME_MEMORY_STRUCT     pUnUseHead;         //Pointer to the first nonpaged memory in used
+	PMLME_MEMORY_STRUCT     pUnUseTail;         //Pointer to the last nonpaged memory in used
+	PULONG                  MemFreePending[MAX_MLME_HANDLER_MEMORY];   //an array to keep pending free-memory's pointer (32bits)
+}   MLME_MEMORY_HANDLER, *PMLME_MEMORY_HANDLER;
+
+typedef	struct _CmdQElmt	{
+	UINT				command;
+	PVOID				buffer;
+	ULONG				bufferlength;
+	BOOLEAN				CmdFromNdis;
+	BOOLEAN				SetOperation;
+	struct _CmdQElmt	*next;
+}	CmdQElmt, *PCmdQElmt;
+
+typedef	struct	_CmdQ	{
+	UINT		size;
+	CmdQElmt	*head;
+	CmdQElmt	*tail;
+	UINT32		CmdQState;
+}CmdQ, *PCmdQ;
+
+//
+// For WPA SUPPLICANT: WIRELESS EXT support wireless events: v14 or newer
+//
+#if WIRELESS_EXT >= 14
+//#define WPA_SUPPLICANT_SUPPORT  1
+#endif
+
+/* oid.h */
+// Cipher suite type for mixed mode group cipher, P802.11i-2004
+typedef enum _RT_802_11_CIPHER_SUITE_TYPE {
+	Cipher_Type_NONE,
+	Cipher_Type_WEP40,
+	Cipher_Type_TKIP,
+	Cipher_Type_RSVD,
+	Cipher_Type_CCMP,
+	Cipher_Type_WEP104
+} RT_802_11_CIPHER_SUITE_TYPE, *PRT_802_11_CIPHER_SUITE_TYPE;
+
+//CMDTHREAD_MULTI_READ_MAC
+//CMDTHREAD_MULTI_WRITE_MAC
+//CMDTHREAD_VENDOR_EEPROM_READ
+//CMDTHREAD_VENDOR_EEPROM_WRITE
+typedef	struct	_CMDHandler_TLV	{
+	USHORT		Offset;
+	USHORT		Length;
+	UCHAR		DataFirst;
+}	CMDHandler_TLV, *PCMDHandler_TLV;
+
+// New for MeetingHouse Api support
+#define CMDTHREAD_VENDOR_RESET                      0x0D730101	// cmd
+#define CMDTHREAD_VENDOR_UNPLUG                     0x0D730102	// cmd
+#define CMDTHREAD_VENDOR_SWITCH_FUNCTION            0x0D730103	// cmd
+#define CMDTHREAD_MULTI_WRITE_MAC                   0x0D730107	// cmd
+#define CMDTHREAD_MULTI_READ_MAC                    0x0D730108	// cmd
+#define CMDTHREAD_VENDOR_EEPROM_WRITE               0x0D73010A	// cmd
+#define CMDTHREAD_VENDOR_EEPROM_READ                0x0D73010B	// cmd
+#define CMDTHREAD_VENDOR_ENTER_TESTMODE             0x0D73010C	// cmd
+#define CMDTHREAD_VENDOR_EXIT_TESTMODE              0x0D73010D	// cmd
+#define CMDTHREAD_VENDOR_WRITE_BBP                  0x0D730119	// cmd
+#define CMDTHREAD_VENDOR_READ_BBP                   0x0D730118	// cmd
+#define CMDTHREAD_VENDOR_WRITE_RF                   0x0D73011A	// cmd
+#define CMDTHREAD_VENDOR_FLIP_IQ                    0x0D73011D	// cmd
+#define CMDTHREAD_RESET_BULK_OUT                    0x0D730210	// cmd
+#define CMDTHREAD_RESET_BULK_IN                     0x0D730211	// cmd
+#define CMDTHREAD_SET_PSM_BIT_SAVE                  0x0D730212	// cmd
+#define CMDTHREAD_SET_RADIO                         0x0D730214	// cmd
+#define CMDTHREAD_UPDATE_TX_RATE                    0x0D730216	// cmd
+#define CMDTHREAD_802_11_ADD_KEY_WEP                0x0D730218	// cmd
+#define CMDTHREAD_RESET_FROM_ERROR                  0x0D73021A	// cmd
+#define CMDTHREAD_LINK_DOWN                         0x0D73021B	// cmd
+#define CMDTHREAD_RESET_FROM_NDIS                   0x0D73021C	// cmd
+#define CMDTHREAD_CHECK_GPIO                        0x0D730215	// cmd
+#define CMDTHREAD_FORCE_WAKE_UP                     0x0D730222	// cmd
+#define CMDTHREAD_SET_BW                            0x0D730225	// cmd
+#define CMDTHREAD_SET_ASIC_WCID                     0x0D730226	// cmd
+#define CMDTHREAD_SET_ASIC_WCID_CIPHER              0x0D730227	// cmd
+#define CMDTHREAD_QKERIODIC_EXECUT                  0x0D73023D	// cmd
+#define RT_CMD_SET_KEY_TABLE                        0x0D730228  // cmd
+#define RT_CMD_SET_RX_WCID_TABLE                    0x0D730229  // cmd
+#define CMDTHREAD_SET_CLIENT_MAC_ENTRY              0x0D73023E	// cmd
+#define CMDTHREAD_802_11_QUERY_HARDWARE_REGISTER    0x0D710105	// cmd
+#define CMDTHREAD_802_11_SET_PHY_MODE               0x0D79010C	// cmd
+#define CMDTHREAD_802_11_SET_STA_CONFIG             0x0D790111	// cmd
+#define CMDTHREAD_802_11_SET_PREAMBLE               0x0D790101	// cmd
+#define CMDTHREAD_802_11_COUNTER_MEASURE			0x0D790102	// cmd
+
+
+#define WPA1AKMBIT	    0x01
+#define WPA2AKMBIT	    0x02
+#define WPA1PSKAKMBIT   0x04
+#define WPA2PSKAKMBIT   0x08
+#define TKIPBIT         0x01
+#define CCMPBIT         0x02
+
+
+#define RT28XX_STA_FORCE_WAKEUP(pAd, bFromTx) \
+    RT28xxUsbStaAsicForceWakeup(pAd, bFromTx);
+
+#define RT28XX_STA_SLEEP_THEN_AUTO_WAKEUP(pAd, TbttNumToNextWakeUp) \
+    RT28xxUsbStaAsicSleepThenAutoWakeup(pAd, TbttNumToNextWakeUp);
+
+#define RT28XX_MLME_RADIO_ON(pAd) \
+    RT28xxUsbMlmeRadioOn(pAd);
+
+#define RT28XX_MLME_RADIO_OFF(pAd) \
+    RT28xxUsbMlmeRadioOFF(pAd);
+
+#endif //__RT2870_H__
diff --git a/drivers/staging/rt2870/rt28xx.h b/drivers/staging/rt2870/rt28xx.h
new file mode 100644
index 000000000000..3927d22f78fb
--- /dev/null
+++ b/drivers/staging/rt2870/rt28xx.h
@@ -0,0 +1,2689 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rt28xx.h
+
+	Abstract:
+	RT28xx ASIC related definition & structures
+
+	Revision History:
+	Who			When		  What
+	--------	----------	  ----------------------------------------------
+       Jan Lee           Jan-3-2006     created for RT2860c
+*/
+
+#ifndef	__RT28XX_H__
+#define	__RT28XX_H__
+
+
+//
+// PCI registers - base address 0x0000
+//
+#define PCI_CFG			0x0000
+#define PCI_EECTRL			0x0004
+#define PCI_MCUCTRL			0x0008
+
+//
+// SCH/DMA registers - base address 0x0200
+//
+// INT_SOURCE_CSR: Interrupt source register. Write one to clear corresponding bit
+//
+#define DMA_CSR0      0x200
+#define INT_SOURCE_CSR      0x200
+#ifdef RT_BIG_ENDIAN
+typedef	union	_INT_SOURCE_CSR_STRUC	{
+	struct	{
+		UINT32       	:14;
+		UINT32       	TxCoherent:1;
+		UINT32       	RxCoherent:1;
+		UINT32       	GPTimer:1;
+		UINT32       	AutoWakeup:1;//bit14
+		UINT32       	TXFifoStatusInt:1;//FIFO Statistics is full, sw should read 0x171c
+		UINT32       	PreTBTT:1;
+		UINT32       	TBTTInt:1;
+		UINT32       	RxTxCoherent:1;
+		UINT32       	MCUCommandINT:1;
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	HccaDmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac1DmaDone:1;
+		UINT32		Ac0DmaDone:1;
+		UINT32		RxDone:1;
+		UINT32		TxDelayINT:1;	//delayed interrupt, not interrupt until several int or time limit hit
+		UINT32		RxDelayINT:1; //dealyed interrupt
+	}	field;
+	UINT32			word;
+}	INT_SOURCE_CSR_STRUC, *PINT_SOURCE_CSR_STRUC;
+#else
+typedef	union	_INT_SOURCE_CSR_STRUC	{
+	struct	{
+		UINT32		RxDelayINT:1;
+		UINT32		TxDelayINT:1;
+		UINT32		RxDone:1;
+		UINT32		Ac0DmaDone:1;//4
+		UINT32       	Ac1DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	HccaDmaDone:1; // bit7
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	MCUCommandINT:1;//bit 9
+		UINT32       	RxTxCoherent:1;
+		UINT32       	TBTTInt:1;
+		UINT32       	PreTBTT:1;
+		UINT32       	TXFifoStatusInt:1;//FIFO Statistics is full, sw should read 0x171c
+		UINT32       	AutoWakeup:1;//bit14
+		UINT32       	GPTimer:1;
+		UINT32       	RxCoherent:1;//bit16
+		UINT32       	TxCoherent:1;
+		UINT32       	:14;
+	}	field;
+	UINT32			word;
+} INT_SOURCE_CSR_STRUC, *PINT_SOURCE_CSR_STRUC;
+#endif
+
+//
+// INT_MASK_CSR:   Interrupt MASK register.   1: the interrupt is mask OFF
+//
+#define INT_MASK_CSR        0x204
+#ifdef RT_BIG_ENDIAN
+typedef	union	_INT_MASK_CSR_STRUC	{
+	struct	{
+		UINT32       	TxCoherent:1;
+		UINT32       	RxCoherent:1;
+		UINT32       	:20;
+		UINT32       	MCUCommandINT:1;
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	HccaDmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac1DmaDone:1;
+		UINT32		Ac0DmaDone:1;
+		UINT32		RxDone:1;
+		UINT32		TxDelay:1;
+		UINT32		RXDelay_INT_MSK:1;
+	}	field;
+	UINT32			word;
+}INT_MASK_CSR_STRUC, *PINT_MASK_CSR_STRUC;
+#else
+typedef	union	_INT_MASK_CSR_STRUC	{
+	struct	{
+		UINT32		RXDelay_INT_MSK:1;
+		UINT32		TxDelay:1;
+		UINT32		RxDone:1;
+		UINT32		Ac0DmaDone:1;
+		UINT32       	Ac1DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	HccaDmaDone:1;
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	MCUCommandINT:1;
+		UINT32       	:20;
+		UINT32       	RxCoherent:1;
+		UINT32       	TxCoherent:1;
+	}	field;
+	UINT32			word;
+} INT_MASK_CSR_STRUC, *PINT_MASK_CSR_STRUC;
+#endif
+#define WPDMA_GLO_CFG 	0x208
+#ifdef RT_BIG_ENDIAN
+typedef	union	_WPDMA_GLO_CFG_STRUC	{
+	struct	{
+		UINT32       	HDR_SEG_LEN:16;
+		UINT32       	RXHdrScater:8;
+		UINT32       	BigEndian:1;
+		UINT32       	EnTXWriteBackDDONE:1;
+		UINT32       	WPDMABurstSIZE:2;
+		UINT32		RxDMABusy:1;
+		UINT32		EnableRxDMA:1;
+		UINT32		TxDMABusy:1;
+		UINT32		EnableTxDMA:1;
+	}	field;
+	UINT32			word;
+}WPDMA_GLO_CFG_STRUC, *PWPDMA_GLO_CFG_STRUC;
+#else
+typedef	union	_WPDMA_GLO_CFG_STRUC	{
+	struct	{
+		UINT32		EnableTxDMA:1;
+		UINT32		TxDMABusy:1;
+		UINT32		EnableRxDMA:1;
+		UINT32		RxDMABusy:1;
+		UINT32       	WPDMABurstSIZE:2;
+		UINT32       	EnTXWriteBackDDONE:1;
+		UINT32       	BigEndian:1;
+		UINT32       	RXHdrScater:8;
+		UINT32       	HDR_SEG_LEN:16;
+	}	field;
+	UINT32			word;
+} WPDMA_GLO_CFG_STRUC, *PWPDMA_GLO_CFG_STRUC;
+#endif
+#define WPDMA_RST_IDX 	0x20c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_WPDMA_RST_IDX_STRUC	{
+	struct	{
+		UINT32       	:15;
+		UINT32       	RST_DRX_IDX0:1;
+		UINT32       	rsv:10;
+		UINT32       	RST_DTX_IDX5:1;
+		UINT32       	RST_DTX_IDX4:1;
+		UINT32		RST_DTX_IDX3:1;
+		UINT32		RST_DTX_IDX2:1;
+		UINT32		RST_DTX_IDX1:1;
+		UINT32		RST_DTX_IDX0:1;
+	}	field;
+	UINT32			word;
+}WPDMA_RST_IDX_STRUC, *PWPDMA_RST_IDX_STRUC;
+#else
+typedef	union	_WPDMA_RST_IDX_STRUC	{
+	struct	{
+		UINT32		RST_DTX_IDX0:1;
+		UINT32		RST_DTX_IDX1:1;
+		UINT32		RST_DTX_IDX2:1;
+		UINT32		RST_DTX_IDX3:1;
+		UINT32       	RST_DTX_IDX4:1;
+		UINT32       	RST_DTX_IDX5:1;
+		UINT32       	rsv:10;
+		UINT32       	RST_DRX_IDX0:1;
+		UINT32       	:15;
+	}	field;
+	UINT32			word;
+} WPDMA_RST_IDX_STRUC, *PWPDMA_RST_IDX_STRUC;
+#endif
+#define DELAY_INT_CFG  0x0210
+#ifdef RT_BIG_ENDIAN
+typedef	union	_DELAY_INT_CFG_STRUC	{
+	struct	{
+		UINT32       	TXDLY_INT_EN:1;
+		UINT32       	TXMAX_PINT:7;
+		UINT32       	TXMAX_PTIME:8;
+		UINT32       	RXDLY_INT_EN:1;
+		UINT32       	RXMAX_PINT:7;
+		UINT32		RXMAX_PTIME:8;
+	}	field;
+	UINT32			word;
+}DELAY_INT_CFG_STRUC, *PDELAY_INT_CFG_STRUC;
+#else
+typedef	union	_DELAY_INT_CFG_STRUC	{
+	struct	{
+		UINT32		RXMAX_PTIME:8;
+		UINT32       	RXMAX_PINT:7;
+		UINT32       	RXDLY_INT_EN:1;
+		UINT32       	TXMAX_PTIME:8;
+		UINT32       	TXMAX_PINT:7;
+		UINT32       	TXDLY_INT_EN:1;
+	}	field;
+	UINT32			word;
+} DELAY_INT_CFG_STRUC, *PDELAY_INT_CFG_STRUC;
+#endif
+#define WMM_AIFSN_CFG   0x0214
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AIFSN_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:16;
+	    UINT32   Aifsn3:4;       // for AC_VO
+	    UINT32   Aifsn2:4;       // for AC_VI
+	    UINT32   Aifsn1:4;       // for AC_BK
+	    UINT32   Aifsn0:4;       // for AC_BE
+	}	field;
+	UINT32			word;
+}	AIFSN_CSR_STRUC, *PAIFSN_CSR_STRUC;
+#else
+typedef	union	_AIFSN_CSR_STRUC	{
+	struct	{
+	    UINT32   Aifsn0:4;       // for AC_BE
+	    UINT32   Aifsn1:4;       // for AC_BK
+	    UINT32   Aifsn2:4;       // for AC_VI
+	    UINT32   Aifsn3:4;       // for AC_VO
+	    UINT32   Rsv:16;
+	}	field;
+	UINT32			word;
+}	AIFSN_CSR_STRUC, *PAIFSN_CSR_STRUC;
+#endif
+//
+// CWMIN_CSR: CWmin for each EDCA AC
+//
+#define WMM_CWMIN_CFG   0x0218
+#ifdef RT_BIG_ENDIAN
+typedef	union	_CWMIN_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:16;
+	    UINT32   Cwmin3:4;       // for AC_VO
+	    UINT32   Cwmin2:4;       // for AC_VI
+	    UINT32   Cwmin1:4;       // for AC_BK
+	    UINT32   Cwmin0:4;       // for AC_BE
+	}	field;
+	UINT32			word;
+}	CWMIN_CSR_STRUC, *PCWMIN_CSR_STRUC;
+#else
+typedef	union	_CWMIN_CSR_STRUC	{
+	struct	{
+	    UINT32   Cwmin0:4;       // for AC_BE
+	    UINT32   Cwmin1:4;       // for AC_BK
+	    UINT32   Cwmin2:4;       // for AC_VI
+	    UINT32   Cwmin3:4;       // for AC_VO
+	    UINT32   Rsv:16;
+	}	field;
+	UINT32			word;
+}	CWMIN_CSR_STRUC, *PCWMIN_CSR_STRUC;
+#endif
+
+//
+// CWMAX_CSR: CWmin for each EDCA AC
+//
+#define WMM_CWMAX_CFG   0x021c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_CWMAX_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:16;
+	    UINT32   Cwmax3:4;       // for AC_VO
+	    UINT32   Cwmax2:4;       // for AC_VI
+	    UINT32   Cwmax1:4;       // for AC_BK
+	    UINT32   Cwmax0:4;       // for AC_BE
+	}	field;
+	UINT32			word;
+}	CWMAX_CSR_STRUC, *PCWMAX_CSR_STRUC;
+#else
+typedef	union	_CWMAX_CSR_STRUC	{
+	struct	{
+	    UINT32   Cwmax0:4;       // for AC_BE
+	    UINT32   Cwmax1:4;       // for AC_BK
+	    UINT32   Cwmax2:4;       // for AC_VI
+	    UINT32   Cwmax3:4;       // for AC_VO
+	    UINT32   Rsv:16;
+	}	field;
+	UINT32			word;
+}	CWMAX_CSR_STRUC, *PCWMAX_CSR_STRUC;
+#endif
+
+
+//
+// AC_TXOP_CSR0: AC_BK/AC_BE TXOP register
+//
+#define WMM_TXOP0_CFG    0x0220
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AC_TXOP_CSR0_STRUC	{
+	struct	{
+	    USHORT  Ac1Txop;        // for AC_BE, in unit of 32us
+	    USHORT  Ac0Txop;        // for AC_BK, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR0_STRUC, *PAC_TXOP_CSR0_STRUC;
+#else
+typedef	union	_AC_TXOP_CSR0_STRUC	{
+	struct	{
+	    USHORT  Ac0Txop;        // for AC_BK, in unit of 32us
+	    USHORT  Ac1Txop;        // for AC_BE, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR0_STRUC, *PAC_TXOP_CSR0_STRUC;
+#endif
+
+//
+// AC_TXOP_CSR1: AC_VO/AC_VI TXOP register
+//
+#define WMM_TXOP1_CFG    0x0224
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AC_TXOP_CSR1_STRUC	{
+	struct	{
+	    USHORT  Ac3Txop;        // for AC_VO, in unit of 32us
+	    USHORT  Ac2Txop;        // for AC_VI, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR1_STRUC, *PAC_TXOP_CSR1_STRUC;
+#else
+typedef	union	_AC_TXOP_CSR1_STRUC	{
+	struct	{
+	    USHORT  Ac2Txop;        // for AC_VI, in unit of 32us
+	    USHORT  Ac3Txop;        // for AC_VO, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR1_STRUC, *PAC_TXOP_CSR1_STRUC;
+#endif
+#define RINGREG_DIFF			0x10
+#define GPIO_CTRL_CFG    0x0228	//MAC_CSR13
+#define MCU_CMD_CFG    0x022c
+#define TX_BASE_PTR0     0x0230	//AC_BK base address
+#define TX_MAX_CNT0      0x0234
+#define TX_CTX_IDX0       0x0238
+#define TX_DTX_IDX0      0x023c
+#define TX_BASE_PTR1     0x0240 	//AC_BE base address
+#define TX_MAX_CNT1      0x0244
+#define TX_CTX_IDX1       0x0248
+#define TX_DTX_IDX1      0x024c
+#define TX_BASE_PTR2     0x0250 	//AC_VI base address
+#define TX_MAX_CNT2      0x0254
+#define TX_CTX_IDX2       0x0258
+#define TX_DTX_IDX2      0x025c
+#define TX_BASE_PTR3     0x0260 	//AC_VO base address
+#define TX_MAX_CNT3      0x0264
+#define TX_CTX_IDX3       0x0268
+#define TX_DTX_IDX3      0x026c
+#define TX_BASE_PTR4     0x0270 	//HCCA base address
+#define TX_MAX_CNT4      0x0274
+#define TX_CTX_IDX4       0x0278
+#define TX_DTX_IDX4      0x027c
+#define TX_BASE_PTR5     0x0280 	//MGMT base address
+#define  TX_MAX_CNT5     0x0284
+#define TX_CTX_IDX5       0x0288
+#define TX_DTX_IDX5      0x028c
+#define TX_MGMTMAX_CNT      TX_MAX_CNT5
+#define TX_MGMTCTX_IDX       TX_CTX_IDX5
+#define TX_MGMTDTX_IDX      TX_DTX_IDX5
+#define RX_BASE_PTR     0x0290 	//RX base address
+#define RX_MAX_CNT      0x0294
+#define RX_CRX_IDX       0x0298
+#define RX_DRX_IDX      0x029c
+#define USB_DMA_CFG      0x02a0
+#ifdef RT_BIG_ENDIAN
+typedef	union	_USB_DMA_CFG_STRUC	{
+	struct	{
+	    UINT32  TxBusy:1;   	//USB DMA TX FSM busy . debug only
+	    UINT32  RxBusy:1;        //USB DMA RX FSM busy . debug only
+	    UINT32  EpoutValid:6;        //OUT endpoint data valid. debug only
+	    UINT32  TxBulkEn:1;        //Enable USB DMA Tx
+	    UINT32  RxBulkEn:1;        //Enable USB DMA Rx
+	    UINT32  RxBulkAggEn:1;        //Enable Rx Bulk Aggregation
+	    UINT32  TxopHalt:1;        //Halt TXOP count down when TX buffer is full.
+	    UINT32  TxClear:1;        //Clear USB DMA TX path
+	    UINT32  rsv:2;
+	    UINT32  phyclear:1;        		//phy watch dog enable. write 1
+	    UINT32  RxBulkAggLmt:8;        //Rx Bulk Aggregation Limit  in unit of 1024 bytes
+	    UINT32  RxBulkAggTOut:8;        //Rx Bulk Aggregation TimeOut  in unit of 33ns
+	}	field;
+	UINT32			word;
+}	USB_DMA_CFG_STRUC, *PUSB_DMA_CFG_STRUC;
+#else
+typedef	union	_USB_DMA_CFG_STRUC	{
+	struct	{
+	    UINT32  RxBulkAggTOut:8;        //Rx Bulk Aggregation TimeOut  in unit of 33ns
+	    UINT32  RxBulkAggLmt:8;        //Rx Bulk Aggregation Limit  in unit of 256 bytes
+	    UINT32  phyclear:1;        		//phy watch dog enable. write 1
+	    UINT32  rsv:2;
+	    UINT32  TxClear:1;        //Clear USB DMA TX path
+	    UINT32  TxopHalt:1;        //Halt TXOP count down when TX buffer is full.
+	    UINT32  RxBulkAggEn:1;        //Enable Rx Bulk Aggregation
+	    UINT32  RxBulkEn:1;        //Enable USB DMA Rx
+	    UINT32  TxBulkEn:1;        //Enable USB DMA Tx
+	    UINT32  EpoutValid:6;        //OUT endpoint data valid
+	    UINT32  RxBusy:1;        //USB DMA RX FSM busy
+	    UINT32  TxBusy:1;   	//USB DMA TX FSM busy
+	}	field;
+	UINT32			word;
+}	USB_DMA_CFG_STRUC, *PUSB_DMA_CFG_STRUC;
+#endif
+
+//
+//  3  PBF  registers
+//
+//
+// Most are for debug. Driver doesn't touch PBF register.
+#define 	PBF_SYS_CTRL 	 0x0400
+#define     PBF_CFG                 0x0408
+#define 	PBF_MAX_PCNT 	 0x040C
+#define 	PBF_CTRL	 	0x0410
+#define 	PBF_INT_STA	 0x0414
+#define 	PBF_INT_ENA	 0x0418
+#define 	TXRXQ_PCNT  	 0x0438
+#define 	PBF_DBG 	 	 0x043c
+#define     PBF_CAP_CTRL     0x0440
+
+//
+//  4  MAC  registers
+//
+//
+//  4.1 MAC SYSTEM  configuration registers (offset:0x1000)
+//
+#define MAC_CSR0            0x1000
+#ifdef RT_BIG_ENDIAN
+typedef	union	_ASIC_VER_ID_STRUC	{
+	struct	{
+	    USHORT  ASICVer;        // version : 2860
+	    USHORT  ASICRev;        // reversion  : 0
+	}	field;
+	UINT32			word;
+}	ASIC_VER_ID_STRUC, *PASIC_VER_ID_STRUC;
+#else
+typedef	union	_ASIC_VER_ID_STRUC	{
+	struct	{
+	    USHORT  ASICRev;        // reversion  : 0
+	    USHORT  ASICVer;        // version : 2860
+	}	field;
+	UINT32			word;
+}	ASIC_VER_ID_STRUC, *PASIC_VER_ID_STRUC;
+#endif
+#define MAC_SYS_CTRL            0x1004		//MAC_CSR1
+#define MAC_ADDR_DW0            		0x1008		// MAC ADDR DW0
+#define MAC_ADDR_DW1           		 0x100c		// MAC ADDR DW1
+//
+// MAC_CSR2: STA MAC register 0
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MAC_DW0_STRUC	{
+	struct	{
+		UCHAR		Byte3;		// MAC address byte 3
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte0;		// MAC address byte 0
+	}	field;
+	UINT32			word;
+}	MAC_DW0_STRUC, *PMAC_DW0_STRUC;
+#else
+typedef	union	_MAC_DW0_STRUC	{
+	struct	{
+		UCHAR		Byte0;		// MAC address byte 0
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte3;		// MAC address byte 3
+	}	field;
+	UINT32			word;
+}	MAC_DW0_STRUC, *PMAC_DW0_STRUC;
+#endif
+
+//
+// MAC_CSR3: STA MAC register 1
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MAC_DW1_STRUC	{
+	struct	{
+		UCHAR		Rsvd1;
+		UCHAR		U2MeMask;
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		Byte4;		// MAC address byte 4
+	}	field;
+	UINT32			word;
+}	MAC_DW1_STRUC, *PMAC_DW1_STRUC;
+#else
+typedef	union	_MAC_DW1_STRUC	{
+	struct	{
+		UCHAR		Byte4;		// MAC address byte 4
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		U2MeMask;
+		UCHAR		Rsvd1;
+	}	field;
+	UINT32			word;
+}	MAC_DW1_STRUC, *PMAC_DW1_STRUC;
+#endif
+
+#define MAC_BSSID_DW0            		0x1010		// MAC BSSID DW0
+#define MAC_BSSID_DW1            		0x1014		// MAC BSSID DW1
+
+//
+// MAC_CSR5: BSSID register 1
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MAC_CSR5_STRUC	{
+	struct	{
+		USHORT		Rsvd:11;
+		USHORT		MBssBcnNum:3;
+		USHORT		BssIdMode:2; // 0: one BSSID, 10: 4 BSSID,  01: 2 BSSID , 11: 8BSSID
+		UCHAR		Byte5;		 // BSSID byte 5
+		UCHAR		Byte4;		 // BSSID byte 4
+	}	field;
+	UINT32			word;
+}	MAC_CSR5_STRUC, *PMAC_CSR5_STRUC;
+#else
+typedef	union	_MAC_CSR5_STRUC	{
+	struct	{
+		UCHAR		Byte4;		 // BSSID byte 4
+		UCHAR		Byte5;		 // BSSID byte 5
+		USHORT      	BssIdMask:2; // 0: one BSSID, 10: 4 BSSID,  01: 2 BSSID , 11: 8BSSID
+		USHORT		MBssBcnNum:3;
+		USHORT		Rsvd:11;
+	}	field;
+	UINT32			word;
+}	MAC_CSR5_STRUC, *PMAC_CSR5_STRUC;
+#endif
+
+#define MAX_LEN_CFG              0x1018		// rt2860b max 16k bytes. bit12:13 Maximum PSDU length (power factor) 0:2^13, 1:2^14, 2:2^15, 3:2^16
+#define BBP_CSR_CFG            		0x101c		//
+//
+// BBP_CSR_CFG: BBP serial control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_BBP_CSR_CFG_STRUC	{
+	struct	{
+		UINT32		:12;
+		UINT32		BBP_RW_MODE:1;		// 0: use serial mode  1:parallel
+		UINT32		BBP_PAR_DUR:1;		    // 0: 4 MAC clock cycles  1: 8 MAC clock cycles
+		UINT32		Busy:1;				// 1: ASIC is busy execute BBP programming.
+		UINT32		fRead:1;		    // 0: Write	BBP, 1:	Read BBP
+		UINT32		RegNum:8;			// Selected	BBP	register
+		UINT32		Value:8;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	BBP_CSR_CFG_STRUC, *PBBP_CSR_CFG_STRUC;
+#else
+typedef	union	_BBP_CSR_CFG_STRUC	{
+	struct	{
+		UINT32		Value:8;			// Register	value to program into BBP
+		UINT32		RegNum:8;			// Selected	BBP	register
+		UINT32		fRead:1;		    // 0: Write	BBP, 1:	Read BBP
+		UINT32		Busy:1;				// 1: ASIC is busy execute BBP programming.
+		UINT32		BBP_PAR_DUR:1;		     // 0: 4 MAC clock cycles  1: 8 MAC clock cycles
+		UINT32		BBP_RW_MODE:1;		// 0: use serial mode  1:parallel
+		UINT32		:12;
+	}	field;
+	UINT32			word;
+}	BBP_CSR_CFG_STRUC, *PBBP_CSR_CFG_STRUC;
+#endif
+#define RF_CSR_CFG0            		0x1020
+//
+// RF_CSR_CFG: RF control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG0_STRUC	{
+	struct	{
+		UINT32		Busy:1;		    // 0: idle 1: 8busy
+		UINT32		Sel:1;				// 0:RF_LE0 activate  1:RF_LE1 activate
+		UINT32		StandbyMode:1;		    // 0: high when stand by 1:	low when standby
+		UINT32		bitwidth:5;			// Selected	BBP	register
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG0_STRUC, *PRF_CSR_CFG0_STRUC;
+#else
+typedef	union	_RF_CSR_CFG0_STRUC	{
+	struct	{
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+		UINT32		bitwidth:5;			// Selected	BBP	register
+		UINT32		StandbyMode:1;		    // 0: high when stand by 1:	low when standby
+		UINT32		Sel:1;				// 0:RF_LE0 activate  1:RF_LE1 activate
+		UINT32		Busy:1;		    // 0: idle 1: 8busy
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG0_STRUC, *PRF_CSR_CFG0_STRUC;
+#endif
+#define RF_CSR_CFG1           		0x1024
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG1_STRUC	{
+	struct	{
+		UINT32		rsv:7;		    // 0: idle 1: 8busy
+		UINT32		RFGap:5;			// Gap between BB_CONTROL_RF and RF_LE. 0: 3 system clock cycle (37.5usec) 1: 5 system clock cycle (62.5usec)
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG1_STRUC, *PRF_CSR_CFG1_STRUC;
+#else
+typedef	union	_RF_CSR_CFG1_STRUC	{
+	struct	{
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+		UINT32		RFGap:5;			// Gap between BB_CONTROL_RF and RF_LE. 0: 3 system clock cycle (37.5usec) 1: 5 system clock cycle (62.5usec)
+		UINT32		rsv:7;		    // 0: idle 1: 8busy
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG1_STRUC, *PRF_CSR_CFG1_STRUC;
+#endif
+#define RF_CSR_CFG2           		0x1028		//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG2_STRUC	{
+	struct	{
+		UINT32		rsv:8;		    // 0: idle 1: 8busy
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG2_STRUC, *PRF_CSR_CFG2_STRUC;
+#else
+typedef	union	_RF_CSR_CFG2_STRUC	{
+	struct	{
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+		UINT32		rsv:8;		    // 0: idle 1: 8busy
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG2_STRUC, *PRF_CSR_CFG2_STRUC;
+#endif
+#define LED_CFG           		0x102c		//  MAC_CSR14
+#ifdef RT_BIG_ENDIAN
+typedef	union	_LED_CFG_STRUC	{
+	struct	{
+		UINT32		:1;
+		UINT32		LedPolar:1;			// Led Polarity.  0: active low1: active high
+		UINT32		YLedMode:2;			// yellow Led Mode
+		UINT32		GLedMode:2;			// green Led Mode
+		UINT32		RLedMode:2;			// red Led Mode    0: off1: blinking upon TX2: periodic slow blinking3: always on
+		UINT32		rsv:2;
+		UINT32		SlowBlinkPeriod:6;			// slow blinking period. unit:1ms
+		UINT32		OffPeriod:8;			// blinking off period unit 1ms
+		UINT32		OnPeriod:8;			// blinking on period unit 1ms
+	}	field;
+	UINT32			word;
+}	LED_CFG_STRUC, *PLED_CFG_STRUC;
+#else
+typedef	union	_LED_CFG_STRUC	{
+	struct	{
+		UINT32		OnPeriod:8;			// blinking on period unit 1ms
+		UINT32		OffPeriod:8;			// blinking off period unit 1ms
+		UINT32		SlowBlinkPeriod:6;			// slow blinking period. unit:1ms
+		UINT32		rsv:2;
+		UINT32		RLedMode:2;			// red Led Mode    0: off1: blinking upon TX2: periodic slow blinking3: always on
+		UINT32		GLedMode:2;			// green Led Mode
+		UINT32		YLedMode:2;			// yellow Led Mode
+		UINT32		LedPolar:1;			// Led Polarity.  0: active low1: active high
+		UINT32		:1;
+	}	field;
+	UINT32			word;
+}	LED_CFG_STRUC, *PLED_CFG_STRUC;
+#endif
+//
+//  4.2 MAC TIMING  configuration registers (offset:0x1100)
+//
+#define XIFS_TIME_CFG             0x1100		 // MAC_CSR8  MAC_CSR9
+#ifdef RT_BIG_ENDIAN
+typedef	union	_IFS_SLOT_CFG_STRUC	{
+	struct	{
+	    UINT32  rsv:2;
+	    UINT32  BBRxendEnable:1;        //  reference RXEND signal to begin XIFS defer
+	    UINT32  EIFS:9;        //  unit 1us
+	    UINT32  OfdmXifsTime:4;        //OFDM SIFS. unit 1us. Applied after OFDM RX when MAC doesn't reference BBP signal BBRXEND
+	    UINT32  OfdmSifsTime:8;        //  unit 1us. Applied after OFDM RX/TX
+	    UINT32  CckmSifsTime:8;        //  unit 1us. Applied after CCK RX/TX
+	}	field;
+	UINT32			word;
+}	IFS_SLOT_CFG_STRUC, *PIFS_SLOT_CFG_STRUC;
+#else
+typedef	union	_IFS_SLOT_CFG_STRUC	{
+	struct	{
+	    UINT32  CckmSifsTime:8;        //  unit 1us. Applied after CCK RX/TX
+	    UINT32  OfdmSifsTime:8;        //  unit 1us. Applied after OFDM RX/TX
+	    UINT32  OfdmXifsTime:4;        //OFDM SIFS. unit 1us. Applied after OFDM RX when MAC doesn't reference BBP signal BBRXEND
+	    UINT32  EIFS:9;        //  unit 1us
+	    UINT32  BBRxendEnable:1;        //  reference RXEND signal to begin XIFS defer
+	    UINT32  rsv:2;
+	}	field;
+	UINT32			word;
+}	IFS_SLOT_CFG_STRUC, *PIFS_SLOT_CFG_STRUC;
+#endif
+
+#define BKOFF_SLOT_CFG             0x1104		 //  mac_csr9 last 8 bits
+#define NAV_TIME_CFG             0x1108		 // NAV  (MAC_CSR15)
+#define CH_TIME_CFG             0x110C		 	// Count as channel busy
+#define PBF_LIFE_TIMER             0x1110		 //TX/RX MPDU timestamp timer (free run)Unit: 1us
+#define BCN_TIME_CFG             0x1114		 // TXRX_CSR9
+
+#define BCN_OFFSET0				0x042C
+#define BCN_OFFSET1				0x0430
+
+//
+// BCN_TIME_CFG : Synchronization control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_BCN_TIME_CFG_STRUC	{
+	struct	{
+		UINT32		TxTimestampCompensate:8;
+        UINT32       :3;
+		UINT32		bBeaconGen:1;		// Enable beacon generator
+        UINT32       bTBTTEnable:1;
+		UINT32		TsfSyncMode:2;		// Enable TSF sync, 00: disable, 01: infra mode, 10: ad-hoc mode
+		UINT32		bTsfTicking:1;		// Enable TSF auto counting
+		UINT32       BeaconInterval:16;  // in unit of 1/16 TU
+	}	field;
+	UINT32			word;
+}	BCN_TIME_CFG_STRUC, *PBCN_TIME_CFG_STRUC;
+#else
+typedef	union	_BCN_TIME_CFG_STRUC	{
+	struct	{
+		UINT32       BeaconInterval:16;  // in unit of 1/16 TU
+		UINT32		bTsfTicking:1;		// Enable TSF auto counting
+		UINT32		TsfSyncMode:2;		// Enable TSF sync, 00: disable, 01: infra mode, 10: ad-hoc mode
+        UINT32       bTBTTEnable:1;
+		UINT32		bBeaconGen:1;		// Enable beacon generator
+        UINT32       :3;
+		UINT32		TxTimestampCompensate:8;
+	}	field;
+	UINT32			word;
+}	BCN_TIME_CFG_STRUC, *PBCN_TIME_CFG_STRUC;
+#endif
+#define TBTT_SYNC_CFG            0x1118  		// txrx_csr10
+#define TSF_TIMER_DW0             0x111C  		// Local TSF timer lsb 32 bits. Read-only
+#define TSF_TIMER_DW1             0x1120  		// msb 32 bits. Read-only.
+#define TBTT_TIMER             	0x1124  		// TImer remains till next TBTT. Read-only.  TXRX_CSR14
+#define INT_TIMER_CFG              	0x1128  		//
+#define INT_TIMER_EN             	0x112c  		//  GP-timer and pre-tbtt Int enable
+#define CH_IDLE_STA              	0x1130  		//  channel idle time
+#define CH_BUSY_STA              	0x1134  		//  channle busy time
+//
+//  4.2 MAC POWER  configuration registers (offset:0x1200)
+//
+#define MAC_STATUS_CFG             0x1200		 // old MAC_CSR12
+#define PWR_PIN_CFG             0x1204		 // old MAC_CSR12
+#define AUTO_WAKEUP_CFG             0x1208		 // old MAC_CSR10
+//
+// AUTO_WAKEUP_CFG: Manual power control / status register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AUTO_WAKEUP_STRUC	{
+	struct	{
+		UINT32		:16;
+		UINT32		EnableAutoWakeup:1;	// 0:sleep, 1:awake
+		UINT32       NumofSleepingTbtt:7;          // ForceWake has high privilege than PutToSleep when both set
+		UINT32       AutoLeadTime:8;
+	}	field;
+	UINT32			word;
+}	AUTO_WAKEUP_STRUC, *PAUTO_WAKEUP_STRUC;
+#else
+typedef	union	_AUTO_WAKEUP_STRUC	{
+	struct	{
+		UINT32       AutoLeadTime:8;
+		UINT32       NumofSleepingTbtt:7;          // ForceWake has high privilege than PutToSleep when both set
+		UINT32		EnableAutoWakeup:1;	// 0:sleep, 1:awake
+		UINT32		:16;
+	}	field;
+	UINT32			word;
+}	AUTO_WAKEUP_STRUC, *PAUTO_WAKEUP_STRUC;
+#endif
+//
+//  4.3 MAC TX  configuration registers (offset:0x1300)
+//
+
+#define EDCA_AC0_CFG	0x1300		//AC_TXOP_CSR0 0x3474
+#define EDCA_AC1_CFG	0x1304
+#define EDCA_AC2_CFG	0x1308
+#define EDCA_AC3_CFG	0x130c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EDCA_AC_CFG_STRUC	{
+	struct	{
+	    UINT32  :12;        //
+	    UINT32  Cwmax:4;        //unit power of 2
+	    UINT32  Cwmin:4;        //
+	    UINT32  Aifsn:4;        // # of slot time
+	    UINT32  AcTxop:8;        //  in unit of 32us
+	}	field;
+	UINT32			word;
+}	EDCA_AC_CFG_STRUC, *PEDCA_AC_CFG_STRUC;
+#else
+typedef	union	_EDCA_AC_CFG_STRUC	{
+	struct	{
+	    UINT32  AcTxop:8;        //  in unit of 32us
+	    UINT32  Aifsn:4;        // # of slot time
+	    UINT32  Cwmin:4;        //
+	    UINT32  Cwmax:4;        //unit power of 2
+	    UINT32  :12;       //
+	}	field;
+	UINT32			word;
+}	EDCA_AC_CFG_STRUC, *PEDCA_AC_CFG_STRUC;
+#endif
+
+#define EDCA_TID_AC_MAP	0x1310
+#define TX_PWR_CFG_0	0x1314
+#define TX_PWR_CFG_1	0x1318
+#define TX_PWR_CFG_2	0x131C
+#define TX_PWR_CFG_3	0x1320
+#define TX_PWR_CFG_4	0x1324
+#define TX_PIN_CFG		0x1328
+#define TX_BAND_CFG	0x132c		// 0x1 use upper 20MHz. 0 juse lower 20MHz
+#define TX_SW_CFG0		0x1330
+#define TX_SW_CFG1		0x1334
+#define TX_SW_CFG2		0x1338
+#define TXOP_THRES_CFG		0x133c
+#define TXOP_CTRL_CFG		0x1340
+#define TX_RTS_CFG		0x1344
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_RTS_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:7;
+	    UINT32       RtsFbkEn:1;    // enable rts rate fallback
+	    UINT32       RtsThres:16;    // unit:byte
+	    UINT32       AutoRtsRetryLimit:8;
+	}	field;
+	UINT32			word;
+}	TX_RTS_CFG_STRUC, *PTX_RTS_CFG_STRUC;
+#else
+typedef	union	_TX_RTS_CFG_STRUC	{
+	struct	{
+	    UINT32       AutoRtsRetryLimit:8;
+	    UINT32       RtsThres:16;    // unit:byte
+	    UINT32       RtsFbkEn:1;    // enable rts rate fallback
+	    UINT32       rsv:7;     // 1: HT non-STBC control frame enable
+	}	field;
+	UINT32			word;
+}	TX_RTS_CFG_STRUC, *PTX_RTS_CFG_STRUC;
+#endif
+#define TX_TIMEOUT_CFG	0x1348
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_TIMEOUT_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv2:8;
+	    UINT32       TxopTimeout:8;	//TXOP timeout value for TXOP truncation.  It is recommended that (SLOT_TIME) > (TX_OP_TIMEOUT) > (RX_ACK_TIMEOUT)
+	    UINT32       RxAckTimeout:8;	// unit:slot. Used for TX precedure
+	    UINT32       MpduLifeTime:4;    //  expiration time = 2^(9+MPDU LIFE TIME)  us
+	    UINT32       rsv:4;
+	}	field;
+	UINT32			word;
+}	TX_TIMEOUT_CFG_STRUC, *PTX_TIMEOUT_CFG_STRUC;
+#else
+typedef	union	_TX_TIMEOUT_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:4;
+	    UINT32       MpduLifeTime:4;    //  expiration time = 2^(9+MPDU LIFE TIME)  us
+	    UINT32       RxAckTimeout:8;	// unit:slot. Used for TX precedure
+	    UINT32       TxopTimeout:8;	//TXOP timeout value for TXOP truncation.  It is recommended that (SLOT_TIME) > (TX_OP_TIMEOUT) > (RX_ACK_TIMEOUT)
+	    UINT32       rsv2:8;     // 1: HT non-STBC control frame enable
+	}	field;
+	UINT32			word;
+}	TX_TIMEOUT_CFG_STRUC, *PTX_TIMEOUT_CFG_STRUC;
+#endif
+#define TX_RTY_CFG	0x134c
+#ifdef RT_BIG_ENDIAN
+typedef	union PACKED _TX_RTY_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:1;
+	    UINT32       TxautoFBEnable:1;    // Tx retry PHY rate auto fallback enable
+	    UINT32       AggRtyMode:1;	// Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       NonAggRtyMode:1;	// Non-Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       LongRtyThre:12;	// Long retry threshoold
+	    UINT32       LongRtyLimit:8;	//long retry limit
+	    UINT32       ShortRtyLimit:8;	//  short retry limit
+
+	}	field;
+	UINT32			word;
+}	TX_RTY_CFG_STRUC, *PTX_RTY_CFG_STRUC;
+#else
+typedef	union PACKED _TX_RTY_CFG_STRUC	{
+	struct	{
+	    UINT32       ShortRtyLimit:8;	//  short retry limit
+	    UINT32       LongRtyLimit:8;	//long retry limit
+	    UINT32       LongRtyThre:12;	// Long retry threshoold
+	    UINT32       NonAggRtyMode:1;	// Non-Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       AggRtyMode:1;	// Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       TxautoFBEnable:1;    // Tx retry PHY rate auto fallback enable
+	    UINT32       rsv:1;     // 1: HT non-STBC control frame enable
+	}	field;
+	UINT32			word;
+}	TX_RTY_CFG_STRUC, *PTX_RTY_CFG_STRUC;
+#endif
+#define TX_LINK_CFG	0x1350
+#ifdef RT_BIG_ENDIAN
+typedef	union	PACKED _TX_LINK_CFG_STRUC	{
+	struct PACKED {
+	    UINT32       RemotMFS:8;	//remote MCS feedback sequence number
+	    UINT32       RemotMFB:8;    //  remote MCS feedback
+	    UINT32       rsv:3;	//
+	    UINT32       TxCFAckEn:1;	//   Piggyback CF-ACK enable
+	    UINT32       TxRDGEn:1;	// RDG TX enable
+	    UINT32       TxMRQEn:1;	//  MCS request TX enable
+	    UINT32       RemoteUMFSEnable:1;	//  remote unsolicit  MFB enable.  0: not apply remote remote unsolicit (MFS=7)
+	    UINT32       MFBEnable:1;	//  TX apply remote MFB 1:enable
+	    UINT32       RemoteMFBLifeTime:8;	//remote MFB life time. unit : 32us
+	}	field;
+	UINT32			word;
+}	TX_LINK_CFG_STRUC, *PTX_LINK_CFG_STRUC;
+#else
+typedef	union	PACKED _TX_LINK_CFG_STRUC	{
+	struct PACKED {
+	    UINT32       RemoteMFBLifeTime:8;	//remote MFB life time. unit : 32us
+	    UINT32       MFBEnable:1;	//  TX apply remote MFB 1:enable
+	    UINT32       RemoteUMFSEnable:1;	//  remote unsolicit  MFB enable.  0: not apply remote remote unsolicit (MFS=7)
+	    UINT32       TxMRQEn:1;	//  MCS request TX enable
+	    UINT32       TxRDGEn:1;	// RDG TX enable
+	    UINT32       TxCFAckEn:1;	//   Piggyback CF-ACK enable
+	    UINT32       rsv:3;	//
+	    UINT32       RemotMFB:8;    //  remote MCS feedback
+	    UINT32       RemotMFS:8;	//remote MCS feedback sequence number
+	}	field;
+	UINT32			word;
+}	TX_LINK_CFG_STRUC, *PTX_LINK_CFG_STRUC;
+#endif
+#define HT_FBK_CFG0	0x1354
+#ifdef RT_BIG_ENDIAN
+typedef	union PACKED _HT_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       HTMCS7FBK:4;
+	    UINT32       HTMCS6FBK:4;
+	    UINT32       HTMCS5FBK:4;
+	    UINT32       HTMCS4FBK:4;
+	    UINT32       HTMCS3FBK:4;
+	    UINT32       HTMCS2FBK:4;
+	    UINT32       HTMCS1FBK:4;
+	    UINT32       HTMCS0FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG0_STRUC, *PHT_FBK_CFG0_STRUC;
+#else
+typedef	union PACKED _HT_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       HTMCS0FBK:4;
+	    UINT32       HTMCS1FBK:4;
+	    UINT32       HTMCS2FBK:4;
+	    UINT32       HTMCS3FBK:4;
+	    UINT32       HTMCS4FBK:4;
+	    UINT32       HTMCS5FBK:4;
+	    UINT32       HTMCS6FBK:4;
+	    UINT32       HTMCS7FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG0_STRUC, *PHT_FBK_CFG0_STRUC;
+#endif
+#define HT_FBK_CFG1	0x1358
+#ifdef RT_BIG_ENDIAN
+typedef	union	_HT_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       HTMCS15FBK:4;
+	    UINT32       HTMCS14FBK:4;
+	    UINT32       HTMCS13FBK:4;
+	    UINT32       HTMCS12FBK:4;
+	    UINT32       HTMCS11FBK:4;
+	    UINT32       HTMCS10FBK:4;
+	    UINT32       HTMCS9FBK:4;
+	    UINT32       HTMCS8FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG1_STRUC, *PHT_FBK_CFG1_STRUC;
+#else
+typedef	union	_HT_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       HTMCS8FBK:4;
+	    UINT32       HTMCS9FBK:4;
+	    UINT32       HTMCS10FBK:4;
+	    UINT32       HTMCS11FBK:4;
+	    UINT32       HTMCS12FBK:4;
+	    UINT32       HTMCS13FBK:4;
+	    UINT32       HTMCS14FBK:4;
+	    UINT32       HTMCS15FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG1_STRUC, *PHT_FBK_CFG1_STRUC;
+#endif
+#define LG_FBK_CFG0	0x135c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_LG_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       OFDMMCS7FBK:4;	//initial value is 6
+	    UINT32       OFDMMCS6FBK:4;	//initial value is 5
+	    UINT32       OFDMMCS5FBK:4;	//initial value is 4
+	    UINT32       OFDMMCS4FBK:4;	//initial value is 3
+	    UINT32       OFDMMCS3FBK:4;	//initial value is 2
+	    UINT32       OFDMMCS2FBK:4;	//initial value is 1
+	    UINT32       OFDMMCS1FBK:4;	//initial value is 0
+	    UINT32       OFDMMCS0FBK:4;	//initial value is 0
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG0_STRUC, *PLG_FBK_CFG0_STRUC;
+#else
+typedef	union	_LG_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       OFDMMCS0FBK:4;	//initial value is 0
+	    UINT32       OFDMMCS1FBK:4;	//initial value is 0
+	    UINT32       OFDMMCS2FBK:4;	//initial value is 1
+	    UINT32       OFDMMCS3FBK:4;	//initial value is 2
+	    UINT32       OFDMMCS4FBK:4;	//initial value is 3
+	    UINT32       OFDMMCS5FBK:4;	//initial value is 4
+	    UINT32       OFDMMCS6FBK:4;	//initial value is 5
+	    UINT32       OFDMMCS7FBK:4;	//initial value is 6
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG0_STRUC, *PLG_FBK_CFG0_STRUC;
+#endif
+#define LG_FBK_CFG1		0x1360
+#ifdef RT_BIG_ENDIAN
+typedef	union	_LG_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       rsv:16;
+	    UINT32       CCKMCS3FBK:4;	//initial value is 2
+	    UINT32       CCKMCS2FBK:4;	//initial value is 1
+	    UINT32       CCKMCS1FBK:4;	//initial value is 0
+	    UINT32       CCKMCS0FBK:4;	//initial value is 0
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG1_STRUC, *PLG_FBK_CFG1_STRUC;
+#else
+typedef	union	_LG_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       CCKMCS0FBK:4;	//initial value is 0
+	    UINT32       CCKMCS1FBK:4;	//initial value is 0
+	    UINT32       CCKMCS2FBK:4;	//initial value is 1
+	    UINT32       CCKMCS3FBK:4;	//initial value is 2
+	    UINT32       rsv:16;
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG1_STRUC, *PLG_FBK_CFG1_STRUC;
+#endif
+
+//=======================================================
+//================ Protection Paramater================================
+//=======================================================
+#define CCK_PROT_CFG	0x1364		//CCK Protection
+#define ASIC_SHORTNAV		1
+#define ASIC_LONGNAV		2
+#define ASIC_RTS		1
+#define ASIC_CTS		2
+#ifdef RT_BIG_ENDIAN
+typedef	union	_PROT_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:5;
+	    UINT32       RTSThEn:1;	//RTS threshold enable on CCK TX
+	    UINT32       TxopAllowGF40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowGF20:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowMM40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowMM20:1;	//CCK TXOP allowance. 0:disallow.
+	    UINT32       TxopAllowOfdm:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowCck:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       ProtectNav:2;	//TXOP protection type for CCK TX. 0:None, 1:ShortNAVprotect,  2:LongNAVProtect, 3:rsv
+	    UINT32       ProtectCtrl:2;	//Protection control frame type for CCK TX. 1:RTS/CTS, 2:CTS-to-self, 0:None, 3:rsv
+	    UINT32       ProtectRate:16;	//Protection control frame rate for CCK TX(RTS/CTS/CFEnd).
+	}	field;
+	UINT32			word;
+}	PROT_CFG_STRUC, *PPROT_CFG_STRUC;
+#else
+typedef	union	_PROT_CFG_STRUC	{
+	struct	{
+	    UINT32       ProtectRate:16;	//Protection control frame rate for CCK TX(RTS/CTS/CFEnd).
+	    UINT32       ProtectCtrl:2;	//Protection control frame type for CCK TX. 1:RTS/CTS, 2:CTS-to-self, 0:None, 3:rsv
+	    UINT32       ProtectNav:2;	//TXOP protection type for CCK TX. 0:None, 1:ShortNAVprotect,  2:LongNAVProtect, 3:rsv
+	    UINT32       TxopAllowCck:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowOfdm:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowMM20:1;	//CCK TXOP allowance. 0:disallow.
+	    UINT32       TxopAllowMM40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowGF20:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowGF40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       RTSThEn:1;	//RTS threshold enable on CCK TX
+	    UINT32       rsv:5;
+	}	field;
+	UINT32			word;
+}	PROT_CFG_STRUC, *PPROT_CFG_STRUC;
+#endif
+
+#define OFDM_PROT_CFG	0x1368		//OFDM Protection
+#define MM20_PROT_CFG	0x136C		//MM20 Protection
+#define MM40_PROT_CFG	0x1370		//MM40 Protection
+#define GF20_PROT_CFG	0x1374		//GF20 Protection
+#define GF40_PROT_CFG	0x1378		//GR40 Protection
+#define EXP_CTS_TIME	0x137C		//
+#define EXP_ACK_TIME	0x1380		//
+
+//
+//  4.4 MAC RX configuration registers (offset:0x1400)
+//
+#define RX_FILTR_CFG	0x1400			//TXRX_CSR0
+#define AUTO_RSP_CFG	0x1404			//TXRX_CSR4
+//
+// TXRX_CSR4: Auto-Responder/
+//
+#ifdef RT_BIG_ENDIAN
+typedef union _AUTO_RSP_CFG_STRUC {
+ struct {
+     UINT32        :24;
+     UINT32       AckCtsPsmBit:1;   // Power bit value in conrtrol frame
+     UINT32       DualCTSEn:1;   // Power bit value in conrtrol frame
+     UINT32       rsv:1;   // Power bit value in conrtrol frame
+     UINT32       AutoResponderPreamble:1;    // 0:long, 1:short preamble
+     UINT32       CTS40MRef:1;  // Response CTS 40MHz duplicate mode
+     UINT32       CTS40MMode:1;  // Response CTS 40MHz duplicate mode
+     UINT32       BACAckPolicyEnable:1;    // 0:long, 1:short preamble
+     UINT32       AutoResponderEnable:1;
+ } field;
+ UINT32   word;
+} AUTO_RSP_CFG_STRUC, *PAUTO_RSP_CFG_STRUC;
+#else
+typedef union _AUTO_RSP_CFG_STRUC {
+ struct {
+     UINT32       AutoResponderEnable:1;
+     UINT32       BACAckPolicyEnable:1;    // 0:long, 1:short preamble
+     UINT32       CTS40MMode:1;  // Response CTS 40MHz duplicate mode
+     UINT32       CTS40MRef:1;  // Response CTS 40MHz duplicate mode
+     UINT32       AutoResponderPreamble:1;    // 0:long, 1:short preamble
+     UINT32       rsv:1;   // Power bit value in conrtrol frame
+     UINT32       DualCTSEn:1;   // Power bit value in conrtrol frame
+     UINT32       AckCtsPsmBit:1;   // Power bit value in conrtrol frame
+     UINT32        :24;
+ } field;
+ UINT32   word;
+} AUTO_RSP_CFG_STRUC, *PAUTO_RSP_CFG_STRUC;
+#endif
+
+#define LEGACY_BASIC_RATE	0x1408	//  TXRX_CSR5           0x3054
+#define HT_BASIC_RATE		0x140c
+#define HT_CTRL_CFG		0x1410
+#define SIFS_COST_CFG		0x1414
+#define RX_PARSER_CFG		0x1418	//Set NAV for all received frames
+
+//
+//  4.5 MAC Security configuration (offset:0x1500)
+//
+#define TX_SEC_CNT0		0x1500		//
+#define RX_SEC_CNT0		0x1504		//
+#define CCMP_FC_MUTE		0x1508		//
+//
+//  4.6 HCCA/PSMP (offset:0x1600)
+//
+#define TXOP_HLDR_ADDR0		0x1600
+#define TXOP_HLDR_ADDR1		0x1604
+#define TXOP_HLDR_ET		0x1608
+#define QOS_CFPOLL_RA_DW0		0x160c
+#define QOS_CFPOLL_A1_DW1		0x1610
+#define QOS_CFPOLL_QC		0x1614
+//
+//  4.7 MAC Statistis registers (offset:0x1700)
+//
+#define RX_STA_CNT0		0x1700		//
+#define RX_STA_CNT1		0x1704		//
+#define RX_STA_CNT2		0x1708		//
+
+//
+// RX_STA_CNT0_STRUC: RX PLCP error count & RX CRC error count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  PhyErr;
+	    USHORT  CrcErr;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT0_STRUC, *PRX_STA_CNT0_STRUC;
+#else
+typedef	union	_RX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  CrcErr;
+	    USHORT  PhyErr;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT0_STRUC, *PRX_STA_CNT0_STRUC;
+#endif
+
+//
+// RX_STA_CNT1_STRUC: RX False CCA count & RX LONG frame count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  PlcpErr;
+	    USHORT  FalseCca;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT1_STRUC, *PRX_STA_CNT1_STRUC;
+#else
+typedef	union	_RX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  FalseCca;
+	    USHORT  PlcpErr;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT1_STRUC, *PRX_STA_CNT1_STRUC;
+#endif
+
+//
+// RX_STA_CNT2_STRUC:
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  RxFifoOverflowCount;
+	    USHORT  RxDupliCount;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT2_STRUC, *PRX_STA_CNT2_STRUC;
+#else
+typedef	union	_RX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  RxDupliCount;
+	    USHORT  RxFifoOverflowCount;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT2_STRUC, *PRX_STA_CNT2_STRUC;
+#endif
+#define TX_STA_CNT0		0x170C		//
+//
+// STA_CSR3: TX Beacon count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  TxBeaconCount;
+	    USHORT  TxFailCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT0_STRUC, *PTX_STA_CNT0_STRUC;
+#else
+typedef	union	_TX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  TxFailCount;
+	    USHORT  TxBeaconCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT0_STRUC, *PTX_STA_CNT0_STRUC;
+#endif
+#define TX_STA_CNT1		0x1710		//
+//
+// TX_STA_CNT1: TX tx count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  TxRetransmit;
+	    USHORT  TxSuccess;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT1_STRUC, *PTX_STA_CNT1_STRUC;
+#else
+typedef	union	_TX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  TxSuccess;
+	    USHORT  TxRetransmit;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT1_STRUC, *PTX_STA_CNT1_STRUC;
+#endif
+#define TX_STA_CNT2		0x1714		//
+//
+// TX_STA_CNT2: TX tx count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  TxUnderFlowCount;
+	    USHORT  TxZeroLenCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT2_STRUC, *PTX_STA_CNT2_STRUC;
+#else
+typedef	union	_TX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  TxZeroLenCount;
+	    USHORT  TxUnderFlowCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT2_STRUC, *PTX_STA_CNT2_STRUC;
+#endif
+#define TX_STA_FIFO		0x1718		//
+//
+// TX_STA_FIFO_STRUC: TX Result for specific PID status fifo register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union PACKED _TX_STA_FIFO_STRUC	{
+	struct	{
+		UINT32		Reserve:2;
+		UINT32		TxBF:1; // 3*3
+		UINT32		SuccessRate:13;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+//		UINT32		SuccessRate:16;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+		UINT32		wcid:8;		//wireless client index
+		UINT32       	TxAckRequired:1;    // ack required
+		UINT32       	TxAggre:1;    // Tx is aggregated
+		UINT32       	TxSuccess:1;   // Tx success. whether success or not
+		UINT32       	PidType:4;
+		UINT32       	bValid:1;   // 1:This register contains a valid TX result
+	}	field;
+	UINT32			word;
+}	TX_STA_FIFO_STRUC, *PTX_STA_FIFO_STRUC;
+#else
+typedef	union PACKED _TX_STA_FIFO_STRUC	{
+	struct	{
+		UINT32       	bValid:1;   // 1:This register contains a valid TX result
+		UINT32       	PidType:4;
+		UINT32       	TxSuccess:1;   // Tx No retry success
+		UINT32       	TxAggre:1;    // Tx Retry Success
+		UINT32       	TxAckRequired:1;    // Tx fail
+		UINT32		wcid:8;		//wireless client index
+//		UINT32		SuccessRate:16;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+		UINT32		SuccessRate:13;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+		UINT32		TxBF:1;
+		UINT32		Reserve:2;
+	}	field;
+	UINT32			word;
+}	TX_STA_FIFO_STRUC, *PTX_STA_FIFO_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT	0x171c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT_STRUC	{
+	struct	{
+	    USHORT  AggTxCount;
+	    USHORT  NonAggTxCount;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT_STRUC, *PTX_AGG_CNT_STRUC;
+#else
+typedef	union	_TX_AGG_CNT_STRUC	{
+	struct	{
+	    USHORT  NonAggTxCount;
+	    USHORT  AggTxCount;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT_STRUC, *PTX_AGG_CNT_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT0	0x1720
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT0_STRUC	{
+	struct	{
+	    USHORT  AggSize2Count;
+	    USHORT  AggSize1Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT0_STRUC, *PTX_AGG_CNT0_STRUC;
+#else
+typedef	union	_TX_AGG_CNT0_STRUC	{
+	struct	{
+	    USHORT  AggSize1Count;
+	    USHORT  AggSize2Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT0_STRUC, *PTX_AGG_CNT0_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT1	0x1724
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT1_STRUC	{
+	struct	{
+	    USHORT  AggSize4Count;
+	    USHORT  AggSize3Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT1_STRUC, *PTX_AGG_CNT1_STRUC;
+#else
+typedef	union	_TX_AGG_CNT1_STRUC	{
+	struct	{
+	    USHORT  AggSize3Count;
+	    USHORT  AggSize4Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT1_STRUC, *PTX_AGG_CNT1_STRUC;
+#endif
+#define TX_AGG_CNT2	0x1728
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT2_STRUC	{
+	struct	{
+	    USHORT  AggSize6Count;
+	    USHORT  AggSize5Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT2_STRUC, *PTX_AGG_CNT2_STRUC;
+#else
+typedef	union	_TX_AGG_CNT2_STRUC	{
+	struct	{
+	    USHORT  AggSize5Count;
+	    USHORT  AggSize6Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT2_STRUC, *PTX_AGG_CNT2_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT3	0x172c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT3_STRUC	{
+	struct	{
+	    USHORT  AggSize8Count;
+	    USHORT  AggSize7Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT3_STRUC, *PTX_AGG_CNT3_STRUC;
+#else
+typedef	union	_TX_AGG_CNT3_STRUC	{
+	struct	{
+	    USHORT  AggSize7Count;
+	    USHORT  AggSize8Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT3_STRUC, *PTX_AGG_CNT3_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT4	0x1730
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT4_STRUC	{
+	struct	{
+	    USHORT  AggSize10Count;
+	    USHORT  AggSize9Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT4_STRUC, *PTX_AGG_CNT4_STRUC;
+#else
+typedef	union	_TX_AGG_CNT4_STRUC	{
+	struct	{
+	    USHORT  AggSize9Count;
+	    USHORT  AggSize10Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT4_STRUC, *PTX_AGG_CNT4_STRUC;
+#endif
+#define TX_AGG_CNT5	0x1734
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT5_STRUC	{
+	struct	{
+	    USHORT  AggSize12Count;
+	    USHORT  AggSize11Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT5_STRUC, *PTX_AGG_CNT5_STRUC;
+#else
+typedef	union	_TX_AGG_CNT5_STRUC	{
+	struct	{
+	    USHORT  AggSize11Count;
+	    USHORT  AggSize12Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT5_STRUC, *PTX_AGG_CNT5_STRUC;
+#endif
+#define TX_AGG_CNT6		0x1738
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT6_STRUC	{
+	struct	{
+	    USHORT  AggSize14Count;
+	    USHORT  AggSize13Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT6_STRUC, *PTX_AGG_CNT6_STRUC;
+#else
+typedef	union	_TX_AGG_CNT6_STRUC	{
+	struct	{
+	    USHORT  AggSize13Count;
+	    USHORT  AggSize14Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT6_STRUC, *PTX_AGG_CNT6_STRUC;
+#endif
+#define TX_AGG_CNT7		0x173c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT7_STRUC	{
+	struct	{
+	    USHORT  AggSize16Count;
+	    USHORT  AggSize15Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT7_STRUC, *PTX_AGG_CNT7_STRUC;
+#else
+typedef	union	_TX_AGG_CNT7_STRUC	{
+	struct	{
+	    USHORT  AggSize15Count;
+	    USHORT  AggSize16Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT7_STRUC, *PTX_AGG_CNT7_STRUC;
+#endif
+#define MPDU_DENSITY_CNT		0x1740
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MPDU_DEN_CNT_STRUC	{
+	struct	{
+	    USHORT  RXZeroDelCount;	//RX zero length delimiter count
+	    USHORT  TXZeroDelCount;	//TX zero length delimiter count
+	}	field;
+	UINT32			word;
+}	MPDU_DEN_CNT_STRUC, *PMPDU_DEN_CNT_STRUC;
+#else
+typedef	union	_MPDU_DEN_CNT_STRUC	{
+	struct	{
+	    USHORT  TXZeroDelCount;	//TX zero length delimiter count
+	    USHORT  RXZeroDelCount;	//RX zero length delimiter count
+	}	field;
+	UINT32			word;
+}	MPDU_DEN_CNT_STRUC, *PMPDU_DEN_CNT_STRUC;
+#endif
+//
+// TXRX control registers - base address 0x3000
+//
+// rt2860b  UNKNOWN reg use R/O Reg Addr 0x77d0 first..
+#define TXRX_CSR1           0x77d0
+
+//
+// Security key table memory, base address = 0x1000
+//
+#define MAC_WCID_BASE		0x1800 //8-bytes(use only 6-bytes) * 256 entry =
+#define HW_WCID_ENTRY_SIZE   8
+#define PAIRWISE_KEY_TABLE_BASE     0x4000      // 32-byte * 256-entry =  -byte
+#define HW_KEY_ENTRY_SIZE           0x20
+#define PAIRWISE_IVEIV_TABLE_BASE     0x6000      // 8-byte * 256-entry =  -byte
+#define MAC_IVEIV_TABLE_BASE     0x6000      // 8-byte * 256-entry =  -byte
+#define HW_IVEIV_ENTRY_SIZE   8
+#define MAC_WCID_ATTRIBUTE_BASE     0x6800      // 4-byte * 256-entry =  -byte
+#define HW_WCID_ATTRI_SIZE   4
+#define WCID_RESERVED          		0x6bfc
+#define SHARED_KEY_TABLE_BASE       0x6c00      // 32-byte * 16-entry = 512-byte
+#define SHARED_KEY_MODE_BASE       0x7000      // 32-byte * 16-entry = 512-byte
+#define HW_SHARED_KEY_MODE_SIZE   4
+#define SHAREDKEYTABLE			0
+#define PAIRWISEKEYTABLE			1
+
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_SHAREDKEY_MODE_STRUC	{
+	struct	{
+		UINT32       :1;
+		UINT32       Bss1Key3CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key0CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key3CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key0CipherAlg:3;
+	}	field;
+	UINT32			word;
+}	SHAREDKEY_MODE_STRUC, *PSHAREDKEY_MODE_STRUC;
+#else
+typedef	union	_SHAREDKEY_MODE_STRUC	{
+	struct	{
+		UINT32       Bss0Key0CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key3CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key0CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key3CipherAlg:3;
+		UINT32       :1;
+	}	field;
+	UINT32			word;
+}	SHAREDKEY_MODE_STRUC, *PSHAREDKEY_MODE_STRUC;
+#endif
+// 64-entry for pairwise key table
+typedef struct _HW_WCID_ENTRY {  // 8-byte per entry
+    UCHAR   Address[6];
+    UCHAR   Rsv[2];
+} HW_WCID_ENTRY, PHW_WCID_ENTRY;
+
+
+
+//
+// Other on-chip shared memory space, base = 0x2000
+//
+
+// CIS space - base address = 0x2000
+#define HW_CIS_BASE             0x2000
+
+// Carrier-sense CTS frame base address. It's where mac stores carrier-sense frame for carrier-sense function.
+#define HW_CS_CTS_BASE			0x7700
+// DFS CTS frame base address. It's where mac stores CTS frame for DFS.
+#define HW_DFS_CTS_BASE			0x7780
+#define HW_CTS_FRAME_SIZE		0x80
+
+// 2004-11-08 john - since NULL frame won't be that long (256 byte). We steal 16 tail bytes
+// to save debugging settings
+#define HW_DEBUG_SETTING_BASE   0x77f0  // 0x77f0~0x77ff total 16 bytes
+#define HW_DEBUG_SETTING_BASE2   0x7770  // 0x77f0~0x77ff total 16 bytes
+
+#if 0
+// on-chip BEACON frame space - base address = 0x7800
+#define HW_BEACON_MAX_SIZE      0x0800 /* unit: byte */
+#define HW_BEACON_BASE0         0x7800
+#define HW_BEACON_BASE1         0x7900
+#define HW_BEACON_BASE2         0x7a00
+#define HW_BEACON_BASE3         0x7b00
+#define HW_BEACON_BASE4         0x7c00
+#define HW_BEACON_BASE5         0x7d00
+#define HW_BEACON_BASE6         0x7e00
+#define HW_BEACON_BASE7         0x7f00
+/* 1. HW_BEACON_OFFSET/64B must be 0;
+   2. BCN_OFFSET0 must also be changed in NICInitializeAsic();
+   3. max 0x0800 for 8 beacon frames; */
+#else
+// In order to support maximum 8 MBSS and its maximum length is 512 for each beacon
+// Three section discontinue memory segments will be used.
+// 1. The original region for BCN 0~3
+// 2. Extract memory from FCE table for BCN 4~5
+// 3. Extract memory from Pair-wise key table for BCN 6~7
+//	  It occupied those memory of wcid 238~253 for BCN 6
+//						      and wcid 222~237 for BCN 7
+#define HW_BEACON_MAX_SIZE      0x1000 /* unit: byte */
+#define HW_BEACON_BASE0         0x7800
+#define HW_BEACON_BASE1         0x7A00
+#define HW_BEACON_BASE2         0x7C00
+#define HW_BEACON_BASE3         0x7E00
+#define HW_BEACON_BASE4         0x7200
+#define HW_BEACON_BASE5         0x7400
+#define HW_BEACON_BASE6         0x5DC0
+#define HW_BEACON_BASE7         0x5BC0
+#endif
+
+#define HW_BEACON_MAX_COUNT     8
+#define HW_BEACON_OFFSET		0x0200
+#define HW_BEACON_CONTENT_LEN	(HW_BEACON_OFFSET - TXWI_SIZE)
+
+// HOST-MCU shared memory - base address = 0x2100
+#define HOST_CMD_CSR		0x404
+#define H2M_MAILBOX_CSR         0x7010
+#define H2M_MAILBOX_CID         0x7014
+#define H2M_MAILBOX_STATUS      0x701c
+#define H2M_INT_SRC             0x7024
+#define H2M_BBP_AGENT           0x7028
+#define M2H_CMD_DONE_CSR        0x000c
+#define MCU_TXOP_ARRAY_BASE     0x000c   // TODO: to be provided by Albert
+#define MCU_TXOP_ENTRY_SIZE     32       // TODO: to be provided by Albert
+#define MAX_NUM_OF_TXOP_ENTRY   16       // TODO: must be same with 8051 firmware
+#define MCU_MBOX_VERSION        0x01     // TODO: to be confirmed by Albert
+#define MCU_MBOX_VERSION_OFFSET 5        // TODO: to be provided by Albert
+
+//
+// Host DMA registers - base address 0x200 .  TX0-3=EDCAQid0-3, TX4=HCCA, TX5=MGMT,
+//
+//
+//  DMA RING DESCRIPTOR
+//
+#define E2PROM_CSR          0x0004
+#define IO_CNTL_CSR         0x77d0
+
+#ifdef RT2870
+// 8051 firmware image for usb - use last-half base address = 0x3000
+#define FIRMWARE_IMAGE_BASE     0x3000
+#define MAX_FIRMWARE_IMAGE_SIZE 0x1000    // 4kbyte
+#endif // RT2870 //
+
+// TODO: ????? old RT2560 registers. to keep them or remove them?
+//#define MCAST0                  0x0178  // multicast filter register 0
+//#define MCAST1                  0x017c  // multicast filter register 1
+
+
+// ================================================================
+// Tx /	Rx / Mgmt ring descriptor definition
+// ================================================================
+
+// the following PID values are used to mark outgoing frame type in TXD->PID so that
+// proper TX statistics can be collected based on these categories
+// b3-2 of PID field -
+#define PID_MGMT			0x05
+#define PID_BEACON			0x0c
+#define PID_DATA_NORMALUCAST	 	0x02
+#define PID_DATA_AMPDU	 	0x04
+#define PID_DATA_NO_ACK    	0x08
+#define PID_DATA_NOT_NORM_ACK	 	0x03
+#if 0
+#define PTYPE_DATA_REQUIRE_ACK  0x00 // b7-6:00, b5-0: 0~59 is MAC table index (AID?), 60~63 is WDS index
+#define PTYPE_NULL_AT_HIGH_RATE 0x04 // b7-6:01, b5-0: 0~59 is MAC table index (AID?), 60~63 is WDS index
+#define PTYPE_RESERVED          0x08 // b7-6:10
+#define PTYPE_SPECIAL           0x0c // b7-6:11
+
+// when b3-2=11 (PTYPE_SPECIAL), b1-0 coube be ...
+#define PSUBTYPE_DATA_NO_ACK    0x00
+#define PSUBTYPE_MGMT           0x01
+#define PSUBTYPE_OTHER_CNTL     0x02
+#define PSUBTYPE_RTS            0x03
+#endif
+// value domain of pTxD->HostQId (4-bit: 0~15)
+#define QID_AC_BK               1   // meet ACI definition in 802.11e
+#define QID_AC_BE               0   // meet ACI definition in 802.11e
+#define QID_AC_VI               2
+#define QID_AC_VO               3
+#define QID_HCCA                4
+#define NUM_OF_TX_RING          5
+#define QID_MGMT                13
+#define QID_RX                  14
+#define QID_OTHER               15
+
+
+// ------------------------------------------------------
+// BBP & RF	definition
+// ------------------------------------------------------
+#define	BUSY		                1
+#define	IDLE		                0
+
+#define	RF_R00					    0
+#define	RF_R01					    1
+#define	RF_R02					    2
+#define	RF_R03					    3
+#define	RF_R04					    4
+#define	RF_R05					    5
+#define	RF_R06					    6
+#define	RF_R07					    7
+#define	RF_R08					    8
+#define	RF_R09					    9
+#define	RF_R10					    10
+#define	RF_R11					    11
+#define	RF_R12					    12
+#define	RF_R13					    13
+#define	RF_R14					    14
+#define	RF_R15					    15
+#define	RF_R16					    16
+#define	RF_R17					    17
+#define	RF_R18					    18
+#define	RF_R19					    19
+#define	RF_R20					    20
+#define	RF_R21					    21
+#define	RF_R22					    22
+#define	RF_R23					    23
+#define	RF_R24					    24
+#define	RF_R25					    25
+#define	RF_R26					    26
+#define	RF_R27					    27
+#define	RF_R28					    28
+#define	RF_R29					    29
+#define	RF_R30					    30
+#define	RF_R31					    31
+
+#define	BBP_R0					    0  // version
+#define	BBP_R1				        1  // TSSI
+#define	BBP_R2          			2  // TX configure
+#define BBP_R3                      3
+#define BBP_R4                      4
+#define BBP_R5                      5
+#define BBP_R6                      6
+#define	BBP_R14			            14 // RX configure
+#define BBP_R16                     16
+#define BBP_R17                     17 // RX sensibility
+#define BBP_R18                     18
+#define BBP_R21                     21
+#define BBP_R22                     22
+#define BBP_R24                     24
+#define BBP_R25                     25
+#define BBP_R49                     49 //TSSI
+#define BBP_R50                     50
+#define BBP_R51                     51
+#define BBP_R52                     52
+#define BBP_R55                     55
+#define BBP_R62                     62 // Rx SQ0 Threshold HIGH
+#define BBP_R63                     63
+#define BBP_R64                     64
+#define BBP_R65                     65
+#define BBP_R66                     66
+#define BBP_R67                     67
+#define BBP_R68                     68
+#define BBP_R69                     69
+#define BBP_R70                     70 // Rx AGC SQ CCK Xcorr threshold
+#define BBP_R73                     73
+#define BBP_R75						75
+#define BBP_R77                     77
+#define BBP_R81                     81
+#define BBP_R82                     82
+#define BBP_R83                     83
+#define BBP_R84                     84
+#define BBP_R86						86
+#define BBP_R91						91
+#define BBP_R92						92
+#define BBP_R94                     94 // Tx Gain Control
+#define BBP_R103                    103
+#define BBP_R105                    105
+#define BBP_R113                    113
+#define BBP_R114                    114
+#define BBP_R115                    115
+#define BBP_R116                    116
+#define BBP_R117                    117
+#define BBP_R118                    118
+#define BBP_R119                    119
+#define BBP_R120                    120
+#define BBP_R121                    121
+#define BBP_R122                    122
+#define BBP_R123                    123
+
+
+#define BBPR94_DEFAULT              0x06 // Add 1 value will gain 1db
+
+//#define PHY_TR_SWITCH_TIME          5  // usec
+
+//#define BBP_R17_LOW_SENSIBILITY     0x50
+//#define BBP_R17_MID_SENSIBILITY     0x41
+//#define BBP_R17_DYNAMIC_UP_BOUND    0x40
+#define RSSI_FOR_VERY_LOW_SENSIBILITY -35
+#define RSSI_FOR_LOW_SENSIBILITY      -58
+#define RSSI_FOR_MID_LOW_SENSIBILITY  -80
+#define RSSI_FOR_MID_SENSIBILITY      -90
+
+//-------------------------------------------------------------------------
+// EEPROM definition
+//-------------------------------------------------------------------------
+#define EEDO                        0x08
+#define EEDI                        0x04
+#define EECS                        0x02
+#define EESK                        0x01
+#define EERL                        0x80
+
+#define EEPROM_WRITE_OPCODE         0x05
+#define EEPROM_READ_OPCODE          0x06
+#define EEPROM_EWDS_OPCODE          0x10
+#define EEPROM_EWEN_OPCODE          0x13
+
+#define	NUM_EEPROM_BBP_PARMS		19			// Include NIC Config 0, 1, CR, TX ALC step, BBPs
+#define	NUM_EEPROM_TX_G_PARMS		7
+#define	EEPROM_NIC1_OFFSET          0x34		// The address is from NIC config 0, not BBP register ID
+#define	EEPROM_NIC2_OFFSET          0x36		// The address is from NIC config 0, not BBP register ID
+#define	EEPROM_BBP_BASE_OFFSET		0xf0		// The address is from NIC config 0, not BBP register ID
+#define	EEPROM_G_TX_PWR_OFFSET		0x52
+#define	EEPROM_G_TX2_PWR_OFFSET		0x60
+#define EEPROM_LED1_OFFSET			0x3c
+#define EEPROM_LED2_OFFSET			0x3e
+#define EEPROM_LED3_OFFSET			0x40
+#define EEPROM_LNA_OFFSET			0x44
+#define EEPROM_RSSI_BG_OFFSET		0x46
+#define EEPROM_RSSI_A_OFFSET		0x4a
+#define EEPROM_DEFINE_MAX_TXPWR		0x4e
+#define EEPROM_TXPOWER_BYRATE_20MHZ_2_4G	0xde	// 20MHZ 2.4G tx power.
+#define EEPROM_TXPOWER_BYRATE_40MHZ_2_4G	0xee	// 40MHZ 2.4G tx power.
+#define EEPROM_TXPOWER_BYRATE_20MHZ_5G		0xfa	// 20MHZ 5G tx power.
+#define EEPROM_TXPOWER_BYRATE_40MHZ_5G		0x10a	// 40MHZ 5G tx power.
+#define EEPROM_A_TX_PWR_OFFSET      0x78
+#define EEPROM_A_TX2_PWR_OFFSET      0xa6
+//#define EEPROM_Japan_TX_PWR_OFFSET      0x90 // 802.11j
+//#define EEPROM_Japan_TX2_PWR_OFFSET      0xbe
+//#define EEPROM_TSSI_REF_OFFSET	0x54
+//#define EEPROM_TSSI_DELTA_OFFSET	0x24
+//#define EEPROM_CCK_TX_PWR_OFFSET  0x62
+//#define EEPROM_CALIBRATE_OFFSET	0x7c
+#define EEPROM_VERSION_OFFSET       0x02
+#define	EEPROM_FREQ_OFFSET			0x3a
+#define EEPROM_TXPOWER_BYRATE 	0xde	// 20MHZ power.
+#define EEPROM_TXPOWER_DELTA		0x50	// 20MHZ AND 40 MHZ use different power. This is delta in 40MHZ.
+#define VALID_EEPROM_VERSION        1
+
+// PairKeyMode definition
+#define PKMODE_NONE                 0
+#define PKMODE_WEP64                1
+#define PKMODE_WEP128               2
+#define PKMODE_TKIP                 3
+#define PKMODE_AES                  4
+#define PKMODE_CKIP64               5
+#define PKMODE_CKIP128              6
+#define PKMODE_TKIP_NO_MIC          7       // MIC appended by driver: not a valid value in hardware key table
+
+// =================================================================================
+// WCID  format
+// =================================================================================
+//7.1	WCID  ENTRY  format  : 8bytes
+typedef	struct	_WCID_ENTRY_STRUC {
+	UCHAR		RXBABitmap7;    // bit0 for TID8, bit7 for TID 15
+	UCHAR		RXBABitmap0;    // bit0 for TID0, bit7 for TID 7
+	UCHAR		MAC[6];	// 0 for shared key table.  1 for pairwise key table
+}	WCID_ENTRY_STRUC, *PWCID_ENTRY_STRUC;
+
+//8.1.1	SECURITY  KEY  format  : 8DW
+// 32-byte per entry, total 16-entry for shared key table, 64-entry for pairwise key table
+typedef struct _HW_KEY_ENTRY {          // 32-byte per entry
+    UCHAR   Key[16];
+    UCHAR   TxMic[8];
+    UCHAR   RxMic[8];
+} HW_KEY_ENTRY, *PHW_KEY_ENTRY;
+
+//8.1.2	IV/EIV  format  : 2DW
+
+//8.1.3	RX attribute entry format  : 1DW
+#ifdef RT_BIG_ENDIAN
+typedef	struct	_MAC_ATTRIBUTE_STRUC {
+	UINT32		rsv:22;
+	UINT32		RXWIUDF:3;
+	UINT32		BSSIDIdx:3; //multipleBSS index for the WCID
+	UINT32		PairKeyMode:3;
+	UINT32		KeyTab:1;	// 0 for shared key table.  1 for pairwise key table
+}	MAC_ATTRIBUTE_STRUC, *PMAC_ATTRIBUTE_STRUC;
+#else
+typedef	struct	_MAC_ATTRIBUTE_STRUC {
+	UINT32		KeyTab:1;	// 0 for shared key table.  1 for pairwise key table
+	UINT32		PairKeyMode:3;
+	UINT32		BSSIDIdx:3; //multipleBSS index for the WCID
+	UINT32		RXWIUDF:3;
+	UINT32		rsv:22;
+}	MAC_ATTRIBUTE_STRUC, *PMAC_ATTRIBUTE_STRUC;
+#endif
+
+
+// =================================================================================
+// TX / RX ring descriptor format
+// =================================================================================
+
+// the first 24-byte in TXD is called TXINFO and will be DMAed to MAC block through TXFIFO.
+// MAC block use this TXINFO to control the transmission behavior of this frame.
+#define FIFO_MGMT                 0
+#define FIFO_HCCA                 1
+#define FIFO_EDCA                 2
+
+//
+// TX descriptor format, Tx	ring, Mgmt Ring
+//
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _TXD_STRUC {
+	// Word 0
+	UINT32		SDPtr0;
+	// Word 1
+	UINT32		DMADONE:1;
+	UINT32		LastSec0:1;
+	UINT32		SDLen0:14;
+	UINT32		Burst:1;
+	UINT32		LastSec1:1;
+	UINT32		SDLen1:14;
+	// Word 2
+	UINT32		SDPtr1;
+	// Word 3
+	UINT32		ICO:1;
+	UINT32		UCO:1;
+	UINT32		TCO:1;
+	UINT32		rsv:2;
+	UINT32		QSEL:2;	// select on-chip FIFO ID for 2nd-stage output scheduler.0:MGMT, 1:HCCA 2:EDCA
+	UINT32		WIV:1;	// Wireless Info Valid. 1 if Driver already fill WI,  o if DMA needs to copy WI to correctposition
+	UINT32		rsv2:24;
+}	TXD_STRUC, *PTXD_STRUC;
+#else
+typedef	struct	PACKED _TXD_STRUC {
+	// Word	0
+	UINT32		SDPtr0;
+	// Word	1
+	UINT32		SDLen1:14;
+	UINT32		LastSec1:1;
+	UINT32		Burst:1;
+	UINT32		SDLen0:14;
+	UINT32		LastSec0:1;
+	UINT32		DMADONE:1;
+	//Word2
+	UINT32		SDPtr1;
+	//Word3
+	UINT32		rsv2:24;
+	UINT32		WIV:1;	// Wireless Info Valid. 1 if Driver already fill WI,  o if DMA needs to copy WI to correctposition
+	UINT32		QSEL:2;	// select on-chip FIFO ID for 2nd-stage output scheduler.0:MGMT, 1:HCCA 2:EDCA
+	UINT32		rsv:2;
+	UINT32		TCO:1;	//
+	UINT32		UCO:1;	//
+	UINT32		ICO:1;	//
+}	TXD_STRUC, *PTXD_STRUC;
+#endif
+
+
+//
+// TXD Wireless Information format for Tx ring and Mgmt Ring
+//
+//txop : for txop mode
+// 0:txop for the MPDU frame will be handles by ASIC by register
+// 1/2/3:the MPDU frame is send after PIFS/backoff/SIFS
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _TXWI_STRUC {
+	// Word 0
+	UINT32		PHYMODE:2;
+	UINT32		TxBF:1;	// 3*3
+	UINT32		rsv2:1;
+//	UINT32		rsv2:2;
+	UINT32		Ifs:1;	//
+	UINT32		STBC:2;	//channel bandwidth 20MHz or 40 MHz
+	UINT32		ShortGI:1;
+	UINT32		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	UINT32		MCS:7;
+
+	UINT32		rsv:6;
+	UINT32		txop:2;	//tx back off mode 0:HT TXOP rule , 1:PIFS TX ,2:Backoff, 3:sifs only when previous frame exchange is successful.
+	UINT32		MpduDensity:3;
+	UINT32		AMPDU:1;
+
+	UINT32		TS:1;
+	UINT32		CFACK:1;
+	UINT32		MIMOps:1;	// the remote peer is in dynamic MIMO-PS mode
+	UINT32		FRAG:1;		// 1 to inform TKIP engine this is a fragment.
+	// Word 1
+	UINT32		PacketId:4;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		WirelessCliID:8;
+	UINT32		BAWinSize:6;
+	UINT32		NSEQ:1;
+	UINT32		ACK:1;
+	// Word 2
+	UINT32		IV;
+	// Word 3
+	UINT32		EIV;
+}	TXWI_STRUC, *PTXWI_STRUC;
+#else
+typedef	struct	PACKED _TXWI_STRUC {
+	// Word	0
+	UINT32		FRAG:1;		// 1 to inform TKIP engine this is a fragment.
+	UINT32		MIMOps:1;	// the remote peer is in dynamic MIMO-PS mode
+	UINT32		CFACK:1;
+	UINT32		TS:1;
+
+	UINT32		AMPDU:1;
+	UINT32		MpduDensity:3;
+	UINT32		txop:2;	//FOR "THIS" frame. 0:HT TXOP rule , 1:PIFS TX ,2:Backoff, 3:sifs only when previous frame exchange is successful.
+	UINT32		rsv:6;
+
+	UINT32		MCS:7;
+	UINT32		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	UINT32		ShortGI:1;
+	UINT32		STBC:2;	// 1: STBC support MCS =0-7,   2,3 : RESERVE
+	UINT32		Ifs:1;	//
+//	UINT32		rsv2:2;	//channel bandwidth 20MHz or 40 MHz
+	UINT32		rsv2:1;
+	UINT32		TxBF:1;	// 3*3
+	UINT32		PHYMODE:2;
+	// Word	1
+	UINT32		ACK:1;
+	UINT32		NSEQ:1;
+	UINT32		BAWinSize:6;
+	UINT32		WirelessCliID:8;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		PacketId:4;
+	//Word2
+	UINT32		IV;
+	//Word3
+	UINT32		EIV;
+}	TXWI_STRUC, *PTXWI_STRUC;
+#endif
+//
+// Rx descriptor format, Rx	Ring
+//
+//
+// RXWI wireless information format, in PBF. invisible in driver.
+//
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _RXWI_STRUC {
+	// Word 0
+	UINT32		TID:4;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		UDF:3;
+	UINT32		BSSID:3;
+	UINT32		KeyIndex:2;
+	UINT32		WirelessCliID:8;
+	// Word 1
+	UINT32		PHYMODE:2;              // 1: this RX frame is unicast to me
+	UINT32		rsv:3;
+	UINT32		STBC:2;
+	UINT32		ShortGI:1;
+	UINT32		BW:1;
+	UINT32		MCS:7;
+	UINT32		SEQUENCE:12;
+	UINT32		FRAG:4;
+	// Word 2
+	UINT32		rsv1:8;
+	UINT32		RSSI2:8;
+	UINT32		RSSI1:8;
+	UINT32		RSSI0:8;
+	// Word 3
+	UINT32		rsv2:16;
+	UINT32		SNR1:8;
+	UINT32		SNR0:8;
+}	RXWI_STRUC, *PRXWI_STRUC;
+#else
+typedef	struct	PACKED _RXWI_STRUC {
+	// Word	0
+	UINT32		WirelessCliID:8;
+	UINT32		KeyIndex:2;
+	UINT32		BSSID:3;
+	UINT32		UDF:3;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		TID:4;
+	// Word	1
+	UINT32		FRAG:4;
+	UINT32		SEQUENCE:12;
+	UINT32		MCS:7;
+	UINT32		BW:1;
+	UINT32		ShortGI:1;
+	UINT32		STBC:2;
+	UINT32		rsv:3;
+	UINT32		PHYMODE:2;              // 1: this RX frame is unicast to me
+	//Word2
+	UINT32		RSSI0:8;
+	UINT32		RSSI1:8;
+	UINT32		RSSI2:8;
+	UINT32		rsv1:8;
+	//Word3
+	UINT32		SNR0:8;
+	UINT32		SNR1:8;
+	UINT32		rsv2:16;
+}	RXWI_STRUC, *PRXWI_STRUC;
+#endif
+
+
+// =================================================================================
+// HOST-MCU communication data structure
+// =================================================================================
+
+//
+// H2M_MAILBOX_CSR: Host-to-MCU Mailbox
+//
+#ifdef RT_BIG_ENDIAN
+typedef union  _H2M_MAILBOX_STRUC {
+    struct {
+        UINT32       Owner:8;
+        UINT32       CmdToken:8;    // 0xff tells MCU not to report CmdDoneInt after excuting the command
+        UINT32       HighByte:8;
+        UINT32       LowByte:8;
+    }   field;
+    UINT32           word;
+} H2M_MAILBOX_STRUC, *PH2M_MAILBOX_STRUC;
+#else
+typedef union  _H2M_MAILBOX_STRUC {
+    struct {
+        UINT32       LowByte:8;
+        UINT32       HighByte:8;
+        UINT32       CmdToken:8;
+        UINT32       Owner:8;
+    }   field;
+    UINT32           word;
+} H2M_MAILBOX_STRUC, *PH2M_MAILBOX_STRUC;
+#endif
+
+//
+// M2H_CMD_DONE_CSR: MCU-to-Host command complete indication
+//
+#ifdef RT_BIG_ENDIAN
+typedef union _M2H_CMD_DONE_STRUC {
+    struct  {
+        UINT32       CmdToken3;
+        UINT32       CmdToken2;
+        UINT32       CmdToken1;
+        UINT32       CmdToken0;
+    } field;
+    UINT32           word;
+} M2H_CMD_DONE_STRUC, *PM2H_CMD_DONE_STRUC;
+#else
+typedef union _M2H_CMD_DONE_STRUC {
+    struct  {
+        UINT32       CmdToken0;
+        UINT32       CmdToken1;
+        UINT32       CmdToken2;
+        UINT32       CmdToken3;
+    } field;
+    UINT32           word;
+} M2H_CMD_DONE_STRUC, *PM2H_CMD_DONE_STRUC;
+#endif
+
+
+
+//
+// MCU_LEDCS: MCU LED Control Setting.
+//
+#ifdef RT_BIG_ENDIAN
+typedef union  _MCU_LEDCS_STRUC {
+	struct	{
+		UCHAR		Polarity:1;
+		UCHAR		LedMode:7;
+	} field;
+	UCHAR				word;
+} MCU_LEDCS_STRUC, *PMCU_LEDCS_STRUC;
+#else
+typedef union  _MCU_LEDCS_STRUC {
+	struct	{
+		UCHAR		LedMode:7;
+		UCHAR		Polarity:1;
+	} field;
+	UCHAR			word;
+} MCU_LEDCS_STRUC, *PMCU_LEDCS_STRUC;
+#endif
+// =================================================================================
+// Register format
+// =================================================================================
+
+
+
+//NAV_TIME_CFG :NAV
+#ifdef RT_BIG_ENDIAN
+typedef	union	_NAV_TIME_CFG_STRUC	{
+	struct	{
+		USHORT		rsv:6;
+		USHORT		ZeroSifs:1;               // Applied zero SIFS timer after OFDM RX 0: disable
+		USHORT		Eifs:9;               // in unit of 1-us
+		UCHAR       SlotTime;    // in unit of 1-us
+		UCHAR		Sifs;               // in unit of 1-us
+	}	field;
+	UINT32			word;
+}	NAV_TIME_CFG_STRUC, *PNAV_TIME_CFG_STRUC;
+#else
+typedef	union	_NAV_TIME_CFG_STRUC	{
+	struct	{
+		UCHAR		Sifs;               // in unit of 1-us
+		UCHAR       SlotTime;    // in unit of 1-us
+		USHORT		Eifs:9;               // in unit of 1-us
+		USHORT		ZeroSifs:1;               // Applied zero SIFS timer after OFDM RX 0: disable
+		USHORT		rsv:6;
+	}	field;
+	UINT32			word;
+}	NAV_TIME_CFG_STRUC, *PNAV_TIME_CFG_STRUC;
+#endif
+
+
+
+
+
+//
+// RX_FILTR_CFG:  /RX configuration register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	RX_FILTR_CFG_STRUC	{
+	struct	{
+		UINT32		:15;
+		UINT32       DropRsvCntlType:1;
+
+        	UINT32       	DropBAR:1;       //
+		UINT32		DropBA:1;		//
+		UINT32		DropPsPoll:1;		// Drop Ps-Poll
+		UINT32		DropRts:1;		// Drop Ps-Poll
+
+		UINT32		DropCts:1;		// Drop Ps-Poll
+		UINT32		DropAck:1;		// Drop Ps-Poll
+		UINT32		DropCFEnd:1;		// Drop Ps-Poll
+		UINT32		DropCFEndAck:1;		// Drop Ps-Poll
+
+		UINT32		DropDuplicate:1;		// Drop duplicate frame
+		UINT32		DropBcast:1;		// Drop broadcast frames
+		UINT32		DropMcast:1;		// Drop multicast frames
+		UINT32		DropVerErr:1;	    // Drop version error frame
+
+		UINT32		DropNotMyBSSID:1;			// Drop fram ToDs bit is true
+		UINT32		DropNotToMe:1;		// Drop not to me unicast frame
+		UINT32		DropPhyErr:1;		// Drop physical error
+		UINT32		DropCRCErr:1;		// Drop CRC error
+	}	field;
+	UINT32			word;
+}	RX_FILTR_CFG_STRUC, *PRX_FILTR_CFG_STRUC;
+#else
+typedef	union	_RX_FILTR_CFG_STRUC	{
+	struct	{
+		UINT32		DropCRCErr:1;		// Drop CRC error
+		UINT32		DropPhyErr:1;		// Drop physical error
+		UINT32		DropNotToMe:1;		// Drop not to me unicast frame
+		UINT32		DropNotMyBSSID:1;			// Drop fram ToDs bit is true
+
+		UINT32		DropVerErr:1;	    // Drop version error frame
+		UINT32		DropMcast:1;		// Drop multicast frames
+		UINT32		DropBcast:1;		// Drop broadcast frames
+		UINT32		DropDuplicate:1;		// Drop duplicate frame
+
+		UINT32		DropCFEndAck:1;		// Drop Ps-Poll
+		UINT32		DropCFEnd:1;		// Drop Ps-Poll
+		UINT32		DropAck:1;		// Drop Ps-Poll
+		UINT32		DropCts:1;		// Drop Ps-Poll
+
+		UINT32		DropRts:1;		// Drop Ps-Poll
+		UINT32		DropPsPoll:1;		// Drop Ps-Poll
+		UINT32		DropBA:1;		//
+        	UINT32       	DropBAR:1;       //
+
+		UINT32       	DropRsvCntlType:1;
+		UINT32		:15;
+	}	field;
+	UINT32			word;
+}	RX_FILTR_CFG_STRUC, *PRX_FILTR_CFG_STRUC;
+#endif
+
+
+
+
+//
+// PHY_CSR4: RF serial control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_PHY_CSR4_STRUC	{
+	struct	{
+		UINT32		Busy:1;				// 1: ASIC is busy execute RF programming.
+		UINT32		PLL_LD:1;			// RF PLL_LD status
+		UINT32		IFSelect:1;			// 1: select IF	to program,	0: select RF to	program
+		UINT32		NumberOfBits:5;		// Number of bits used in RFRegValue (I:20,	RFMD:22)
+		UINT32		RFRegValue:24;		// Register	value (include register	id)	serial out to RF/IF	chip.
+	}	field;
+	UINT32			word;
+}	PHY_CSR4_STRUC, *PPHY_CSR4_STRUC;
+#else
+typedef	union	_PHY_CSR4_STRUC	{
+	struct	{
+		UINT32		RFRegValue:24;		// Register	value (include register	id)	serial out to RF/IF	chip.
+		UINT32		NumberOfBits:5;		// Number of bits used in RFRegValue (I:20,	RFMD:22)
+		UINT32		IFSelect:1;			// 1: select IF	to program,	0: select RF to	program
+		UINT32		PLL_LD:1;			// RF PLL_LD status
+		UINT32		Busy:1;				// 1: ASIC is busy execute RF programming.
+	}	field;
+	UINT32			word;
+}	PHY_CSR4_STRUC, *PPHY_CSR4_STRUC;
+#endif
+
+
+//
+// SEC_CSR5: shared key table security mode register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_SEC_CSR5_STRUC	{
+	struct	{
+        UINT32       :1;
+        UINT32       Bss3Key3CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key0CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key3CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key0CipherAlg:3;
+	}	field;
+	UINT32			word;
+}	SEC_CSR5_STRUC, *PSEC_CSR5_STRUC;
+#else
+typedef	union	_SEC_CSR5_STRUC	{
+	struct	{
+        UINT32       Bss2Key0CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key3CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key0CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key3CipherAlg:3;
+        UINT32       :1;
+	}	field;
+	UINT32			word;
+}	SEC_CSR5_STRUC, *PSEC_CSR5_STRUC;
+#endif
+
+
+//
+// HOST_CMD_CSR: For HOST to interrupt embedded processor
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_HOST_CMD_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:24;
+	    UINT32   HostCommand:8;
+	}	field;
+	UINT32			word;
+}	HOST_CMD_CSR_STRUC, *PHOST_CMD_CSR_STRUC;
+#else
+typedef	union	_HOST_CMD_CSR_STRUC	{
+	struct	{
+	    UINT32   HostCommand:8;
+	    UINT32   Rsv:24;
+	}	field;
+	UINT32			word;
+}	HOST_CMD_CSR_STRUC, *PHOST_CMD_CSR_STRUC;
+#endif
+
+
+//
+// AIFSN_CSR: AIFSN for each EDCA AC
+//
+
+
+
+//
+// E2PROM_CSR: EEPROM control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_E2PROM_CSR_STRUC	{
+	struct	{
+		UINT32		Rsvd:25;
+		UINT32       LoadStatus:1;   // 1:loading, 0:done
+		UINT32		Type:1;			// 1: 93C46, 0:93C66
+		UINT32		EepromDO:1;
+		UINT32		EepromDI:1;
+		UINT32		EepromCS:1;
+		UINT32		EepromSK:1;
+		UINT32		Reload:1;		// Reload EEPROM content, write one to reload, self-cleared.
+	}	field;
+	UINT32			word;
+}	E2PROM_CSR_STRUC, *PE2PROM_CSR_STRUC;
+#else
+typedef	union	_E2PROM_CSR_STRUC	{
+	struct	{
+		UINT32		Reload:1;		// Reload EEPROM content, write one to reload, self-cleared.
+		UINT32		EepromSK:1;
+		UINT32		EepromCS:1;
+		UINT32		EepromDI:1;
+		UINT32		EepromDO:1;
+		UINT32		Type:1;			// 1: 93C46, 0:93C66
+		UINT32       LoadStatus:1;   // 1:loading, 0:done
+		UINT32		Rsvd:25;
+	}	field;
+	UINT32			word;
+}	E2PROM_CSR_STRUC, *PE2PROM_CSR_STRUC;
+#endif
+
+
+// -------------------------------------------------------------------
+//  E2PROM data layout
+// -------------------------------------------------------------------
+
+//
+// EEPROM antenna select format
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_ANTENNA_STRUC	{
+	struct	{
+		USHORT      Rsv:4;
+		USHORT      RfIcType:4;             // see E2PROM document
+		USHORT		TxPath:4;	// 1: 1T, 2: 2T
+		USHORT		RxPath:4;	// 1: 1R, 2: 2R, 3: 3R
+	}	field;
+	USHORT			word;
+}	EEPROM_ANTENNA_STRUC, *PEEPROM_ANTENNA_STRUC;
+#else
+typedef	union	_EEPROM_ANTENNA_STRUC	{
+	struct	{
+		USHORT		RxPath:4;	// 1: 1R, 2: 2R, 3: 3R
+		USHORT		TxPath:4;	// 1: 1T, 2: 2T
+		USHORT      RfIcType:4;             // see E2PROM document
+		USHORT      Rsv:4;
+	}	field;
+	USHORT			word;
+}	EEPROM_ANTENNA_STRUC, *PEEPROM_ANTENNA_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union _EEPROM_NIC_CINFIG2_STRUC	{
+	struct	{
+        USHORT		Rsv2:6;					// must be 0
+		USHORT		BW40MAvailForA:1;			// 0:enable, 1:disable
+		USHORT		BW40MAvailForG:1;			// 0:enable, 1:disable
+		USHORT		EnableWPSPBC:1;                 // WPS PBC Control bit
+		USHORT		BW40MSidebandForA:1;
+		USHORT		BW40MSidebandForG:1;
+		USHORT		CardbusAcceleration:1;	// !!! NOTE: 0 - enable, 1 - disable
+		USHORT		ExternalLNAForA:1;			// external LNA enable for 5G
+		USHORT		ExternalLNAForG:1;			// external LNA enable for 2.4G
+		USHORT		DynamicTxAgcControl:1;			//
+		USHORT		HardwareRadioControl:1;	// Whether RF is controlled by driver or HW. 1:enable hw control, 0:disable
+	}	field;
+	USHORT			word;
+}	EEPROM_NIC_CONFIG2_STRUC, *PEEPROM_NIC_CONFIG2_STRUC;
+#else
+typedef	union _EEPROM_NIC_CINFIG2_STRUC	{
+	struct {
+		USHORT		HardwareRadioControl:1;	// 1:enable, 0:disable
+		USHORT		DynamicTxAgcControl:1;			//
+		USHORT		ExternalLNAForG:1;				//
+		USHORT		ExternalLNAForA:1;			// external LNA enable for 2.4G
+		USHORT		CardbusAcceleration:1;	// !!! NOTE: 0 - enable, 1 - disable
+		USHORT		BW40MSidebandForG:1;
+		USHORT		BW40MSidebandForA:1;
+		USHORT		EnableWPSPBC:1;                 // WPS PBC Control bit
+		USHORT		BW40MAvailForG:1;			// 0:enable, 1:disable
+		USHORT		BW40MAvailForA:1;			// 0:enable, 1:disable
+		USHORT		Rsv2:6;                 // must be 0
+	}	field;
+	USHORT			word;
+}	EEPROM_NIC_CONFIG2_STRUC, *PEEPROM_NIC_CONFIG2_STRUC;
+#endif
+
+//
+// TX_PWR Value valid range 0xFA(-6) ~ 0x24(36)
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_TX_PWR_STRUC	{
+	struct	{
+		CHAR	Byte1;				// High Byte
+		CHAR	Byte0;				// Low Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_TX_PWR_STRUC, *PEEPROM_TX_PWR_STRUC;
+#else
+typedef	union	_EEPROM_TX_PWR_STRUC	{
+	struct	{
+		CHAR	Byte0;				// Low Byte
+		CHAR	Byte1;				// High Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_TX_PWR_STRUC, *PEEPROM_TX_PWR_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_VERSION_STRUC	{
+	struct	{
+		UCHAR	Version;			// High Byte
+		UCHAR	FaeReleaseNumber;	// Low Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_VERSION_STRUC, *PEEPROM_VERSION_STRUC;
+#else
+typedef	union	_EEPROM_VERSION_STRUC	{
+	struct	{
+		UCHAR	FaeReleaseNumber;	// Low Byte
+		UCHAR	Version;			// High Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_VERSION_STRUC, *PEEPROM_VERSION_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_LED_STRUC	{
+	struct	{
+		USHORT	Rsvd:3;				// Reserved
+		USHORT	LedMode:5;			// Led mode.
+		USHORT	PolarityGPIO_4:1;	// Polarity GPIO#4 setting.
+		USHORT	PolarityGPIO_3:1;	// Polarity GPIO#3 setting.
+		USHORT	PolarityGPIO_2:1;	// Polarity GPIO#2 setting.
+		USHORT	PolarityGPIO_1:1;	// Polarity GPIO#1 setting.
+		USHORT	PolarityGPIO_0:1;	// Polarity GPIO#0 setting.
+		USHORT	PolarityACT:1;		// Polarity ACT setting.
+		USHORT	PolarityRDY_A:1;		// Polarity RDY_A setting.
+		USHORT	PolarityRDY_G:1;		// Polarity RDY_G setting.
+	}	field;
+	USHORT	word;
+}	EEPROM_LED_STRUC, *PEEPROM_LED_STRUC;
+#else
+typedef	union	_EEPROM_LED_STRUC	{
+	struct	{
+		USHORT	PolarityRDY_G:1;		// Polarity RDY_G setting.
+		USHORT	PolarityRDY_A:1;		// Polarity RDY_A setting.
+		USHORT	PolarityACT:1;		// Polarity ACT setting.
+		USHORT	PolarityGPIO_0:1;	// Polarity GPIO#0 setting.
+		USHORT	PolarityGPIO_1:1;	// Polarity GPIO#1 setting.
+		USHORT	PolarityGPIO_2:1;	// Polarity GPIO#2 setting.
+		USHORT	PolarityGPIO_3:1;	// Polarity GPIO#3 setting.
+		USHORT	PolarityGPIO_4:1;	// Polarity GPIO#4 setting.
+		USHORT	LedMode:5;			// Led mode.
+		USHORT	Rsvd:3;				// Reserved
+	}	field;
+	USHORT	word;
+}	EEPROM_LED_STRUC, *PEEPROM_LED_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_TXPOWER_DELTA_STRUC	{
+	struct	{
+		UCHAR	TxPowerEnable:1;// Enable
+		UCHAR	Type:1;			// 1: plus the delta value, 0: minus the delta value
+		UCHAR	DeltaValue:6;	// Tx Power dalta value (MAX=4)
+	}	field;
+	UCHAR	value;
+}	EEPROM_TXPOWER_DELTA_STRUC, *PEEPROM_TXPOWER_DELTA_STRUC;
+#else
+typedef	union	_EEPROM_TXPOWER_DELTA_STRUC	{
+	struct	{
+		UCHAR	DeltaValue:6;	// Tx Power dalta value (MAX=4)
+		UCHAR	Type:1;			// 1: plus the delta value, 0: minus the delta value
+		UCHAR	TxPowerEnable:1;// Enable
+	}	field;
+	UCHAR	value;
+}	EEPROM_TXPOWER_DELTA_STRUC, *PEEPROM_TXPOWER_DELTA_STRUC;
+#endif
+
+//
+// QOS_CSR0: TXOP holder address0 register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_QOS_CSR0_STRUC	{
+	struct	{
+		UCHAR		Byte3;		// MAC address byte 3
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte0;		// MAC address byte 0
+	}	field;
+	UINT32			word;
+}	QOS_CSR0_STRUC, *PQOS_CSR0_STRUC;
+#else
+typedef	union	_QOS_CSR0_STRUC	{
+	struct	{
+		UCHAR		Byte0;		// MAC address byte 0
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte3;		// MAC address byte 3
+	}	field;
+	UINT32			word;
+}	QOS_CSR0_STRUC, *PQOS_CSR0_STRUC;
+#endif
+
+//
+// QOS_CSR1: TXOP holder address1 register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_QOS_CSR1_STRUC	{
+	struct	{
+		UCHAR		Rsvd1;
+		UCHAR		Rsvd0;
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		Byte4;		// MAC address byte 4
+	}	field;
+	UINT32			word;
+}	QOS_CSR1_STRUC, *PQOS_CSR1_STRUC;
+#else
+typedef	union	_QOS_CSR1_STRUC	{
+	struct	{
+		UCHAR		Byte4;		// MAC address byte 4
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		Rsvd0;
+		UCHAR		Rsvd1;
+	}	field;
+	UINT32			word;
+}	QOS_CSR1_STRUC, *PQOS_CSR1_STRUC;
+#endif
+
+#define	RF_CSR_CFG	0x500
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG_STRUC	{
+	struct	{
+		UINT	Rsvd1:14;				// Reserved
+		UINT	RF_CSR_KICK:1;			// kick RF register read/write
+		UINT	RF_CSR_WR:1;			// 0: read  1: write
+		UINT	Rsvd2:3;				// Reserved
+		UINT	TESTCSR_RFACC_REGNUM:5;	// RF register ID
+		UINT	RF_CSR_DATA:8;			// DATA
+	}	field;
+	UINT	word;
+}	RF_CSR_CFG_STRUC, *PRF_CSR_CFG_STRUC;
+#else
+typedef	union	_RF_CSR_CFG_STRUC	{
+	struct	{
+		UINT	RF_CSR_DATA:8;			// DATA
+		UINT	TESTCSR_RFACC_REGNUM:5;	// RF register ID
+		UINT	Rsvd2:3;				// Reserved
+		UINT	RF_CSR_WR:1;			// 0: read  1: write
+		UINT	RF_CSR_KICK:1;			// kick RF register read/write
+		UINT	Rsvd1:14;				// Reserved
+	}	field;
+	UINT	word;
+}	RF_CSR_CFG_STRUC, *PRF_CSR_CFG_STRUC;
+#endif
+
+#endif	// __RT28XX_H__
diff --git a/drivers/staging/rt2870/rt_ate.c b/drivers/staging/rt2870/rt_ate.c
new file mode 100644
index 000000000000..27c763ee9276
--- /dev/null
+++ b/drivers/staging/rt2870/rt_ate.c
@@ -0,0 +1,6452 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "rt_config.h"
+
+#ifdef UCOS
+INT IoctlResponse(PUCHAR payload, PUCHAR msg, INT len);
+#endif // UCOS //
+
+#ifdef RALINK_ATE
+UCHAR TemplateFrame[24] = {0x08/* Data type */,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0xAA,0xBB,0x12,0x34,0x56,0x00,0x11,0x22,0xAA,0xBB,0xCC,0x00,0x00};	// 802.11 MAC Header, Type:Data, Length:24bytes
+extern RTMP_RF_REGS RF2850RegTable[];
+extern UCHAR NUM_OF_2850_CHNL;
+
+#ifdef RT2870
+extern UCHAR EpToQueue[];
+extern VOID	RTUSBRejectPendingPackets(	IN	PRTMP_ADAPTER	pAd);
+#endif // RT2870 //
+
+#ifdef UCOS
+extern INT ConsoleResponse(IN PUCHAR buff);
+extern int (*remote_display)(char *);
+#endif // UCOS //
+
+static CHAR CCKRateTable[] = {0, 1, 2, 3, 8, 9, 10, 11, -1}; /* CCK Mode. */
+static CHAR OFDMRateTable[] = {0, 1, 2, 3, 4, 5, 6, 7, -1}; /* OFDM Mode. */
+static CHAR HTMIXRateTable[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, -1}; /* HT Mix Mode. */
+
+static INT TxDmaBusy(
+	IN PRTMP_ADAPTER pAd);
+
+static INT RxDmaBusy(
+	IN PRTMP_ADAPTER pAd);
+
+static VOID RtmpDmaEnable(
+	IN PRTMP_ADAPTER pAd,
+	IN INT Enable);
+
+static VOID BbpSoftReset(
+	IN PRTMP_ADAPTER pAd);
+
+static VOID RtmpRfIoWrite(
+	IN PRTMP_ADAPTER pAd);
+
+static INT ATESetUpFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT32 TxIdx);
+
+static INT ATETxPwrHandler(
+	IN PRTMP_ADAPTER pAd,
+	IN char index);
+
+static INT ATECmdHandler(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+static int CheckMCSValid(
+	IN UCHAR Mode,
+	IN UCHAR Mcs);
+
+
+#ifdef RT2870
+static VOID ATEWriteTxInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXINFO_STRUC 	pTxInfo,
+	IN	  USHORT		USBDMApktLen,
+	IN	  BOOLEAN		bWiv,
+	IN	  UCHAR			QueueSel,
+	IN	  UCHAR			NextValid,
+	IN	  UCHAR			TxBurst);
+
+static VOID ATEWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC 	pTxWI,
+	IN	BOOLEAN			FRAG,
+	IN	BOOLEAN			InsTimestamp,
+	IN	BOOLEAN 		AMPDU,
+	IN	BOOLEAN 		Ack,
+	IN	BOOLEAN 		NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN	UCHAR 			PID,
+	IN	UCHAR			MIMOps,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	Transmit);
+
+#endif // RT2870 //
+
+static VOID SetJapanFilter(
+	IN	PRTMP_ADAPTER	pAd);
+
+/*=========================end of prototype=========================*/
+
+
+#ifdef RT2870
+static INT TxDmaBusy(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT result;
+	USB_DMA_CFG_STRUC UsbCfg;
+
+	RTMP_IO_READ32(pAd, USB_DMA_CFG, &UsbCfg.word);	// disable DMA
+	if (UsbCfg.field.TxBusy)
+		result = 1;
+	else
+		result = 0;
+
+	return result;
+}
+
+static INT RxDmaBusy(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT result;
+	USB_DMA_CFG_STRUC UsbCfg;
+
+	RTMP_IO_READ32(pAd, USB_DMA_CFG, &UsbCfg.word);	// disable DMA
+	if (UsbCfg.field.RxBusy)
+		result = 1;
+	else
+		result = 0;
+
+	return result;
+}
+
+static VOID RtmpDmaEnable(
+	IN PRTMP_ADAPTER pAd,
+	IN INT Enable)
+{
+	BOOLEAN value;
+	ULONG WaitCnt;
+	USB_DMA_CFG_STRUC UsbCfg;
+
+	value = Enable > 0 ? 1 : 0;
+
+	// check DMA is in busy mode.
+	WaitCnt = 0;
+	while (TxDmaBusy(pAd) || RxDmaBusy(pAd))
+	{
+		RTMPusecDelay(10);
+		if (WaitCnt++ > 100)
+			break;
+	}
+
+	//Why not to clear USB DMA TX path first ???
+	RTMP_IO_READ32(pAd, USB_DMA_CFG, &UsbCfg.word);	// disable DMA
+	UsbCfg.field.TxBulkEn = value;
+	UsbCfg.field.RxBulkEn = value;
+	RTMP_IO_WRITE32(pAd, USB_DMA_CFG, UsbCfg.word);	// abort all TX rings
+	RTMPusecDelay(5000);
+
+	return;
+}
+#endif // RT2870 //
+
+static VOID BbpSoftReset(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR BbpData = 0;
+
+	// Soft reset, set BBP R21 bit0=1->0
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R21, &BbpData);
+	BbpData |= 0x00000001; //set bit0=1
+	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R21, BbpData);
+
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R21, &BbpData);
+	BbpData &= ~(0x00000001); //set bit0=0
+	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R21, BbpData);
+
+	return;
+}
+
+static VOID RtmpRfIoWrite(
+	IN PRTMP_ADAPTER pAd)
+{
+	// Set RF value 1's set R3[bit2] = [0]
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+	RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+	RTMPusecDelay(200);
+
+	// Set RF value 2's set R3[bit2] = [1]
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+	RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 | 0x04));
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+	RTMPusecDelay(200);
+
+	// Set RF value 3's set R3[bit2] = [0]
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+	RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+	return;
+}
+
+static int CheckMCSValid(
+	UCHAR Mode,
+	UCHAR Mcs)
+{
+	int i;
+	PCHAR pRateTab;
+
+	switch(Mode)
+	{
+		case 0:
+			pRateTab = CCKRateTable;
+			break;
+		case 1:
+			pRateTab = OFDMRateTable;
+			break;
+		case 2:
+		case 3:
+			pRateTab = HTMIXRateTable;
+			break;
+		default:
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("unrecognizable Tx Mode %d\n", Mode));
+			return -1;
+			break;
+	}
+
+	i = 0;
+	while(pRateTab[i] != -1)
+	{
+		if (pRateTab[i] == Mcs)
+			return 0;
+		i++;
+	}
+
+	return -1;
+}
+
+#if 1
+static INT ATETxPwrHandler(
+	IN PRTMP_ADAPTER pAd,
+	IN char index)
+{
+	ULONG R;
+	CHAR TxPower;
+	UCHAR Bbp94 = 0;
+	BOOLEAN bPowerReduce = FALSE;
+
+#ifdef RALINK_28xx_QA
+	if ((pAd->ate.bQATxStart == TRUE) || (pAd->ate.bQARxStart == TRUE))
+	{
+		/* When QA is used for Tx, pAd->ate.TxPower0/1 and real tx power
+		** are not synchronized.
+		*/
+/*
+		pAd->ate.TxPower0 = pAd->LatchRfRegs.xxx;
+		pAd->ate.TxPower1 = pAd->LatchRfRegs.xxx;
+*/
+		return 0;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		TxPower = index == 0 ? pAd->ate.TxPower0 : pAd->ate.TxPower1;
+
+		if (pAd->ate.Channel <= 14)
+		{
+			if (TxPower > 31)
+			{
+				//
+				// R3, R4 can't large than 31 (0x24), 31 ~ 36 used by BBP 94
+				//
+				R = 31;
+				if (TxPower <= 36)
+					Bbp94 = BBPR94_DEFAULT + (UCHAR)(TxPower - 31);
+			}
+			else if (TxPower < 0)
+			{
+				//
+				// R3, R4 can't less than 0, -1 ~ -6 used by BBP 94
+				//
+				R = 0;
+				if (TxPower >= -6)
+					Bbp94 = BBPR94_DEFAULT + TxPower;
+			}
+			else
+			{
+				// 0 ~ 31
+				R = (ULONG) TxPower;
+				Bbp94 = BBPR94_DEFAULT;
+			}
+
+			ATEDBGPRINT(RT_DEBUG_TRACE, ("%s (TxPower=%d, R=%ld, BBP_R94=%d)\n", __func__, TxPower, R, Bbp94));
+		}
+		else// 5.5 GHz
+		{
+			if (TxPower > 15)
+			{
+				//
+				// R3, R4 can't large than 15 (0x0F)
+				//
+				R = 15;
+			}
+			else if (TxPower < 0)
+			{
+				//
+				// R3, R4 can't less than 0
+				//
+				// -1 ~ -7
+				ASSERT((TxPower >= -7));
+				R = (ULONG)(TxPower + 7);
+				bPowerReduce = TRUE;
+			}
+			else
+			{
+				// 0 ~ 15
+				R = (ULONG) TxPower;
+			}
+
+			ATEDBGPRINT(RT_DEBUG_TRACE, ("%s (TxPower=%d, R=%lu)\n", __func__, TxPower, R));
+		}
+
+		if (pAd->ate.Channel <= 14)
+		{
+			if (index == 0)
+			{
+				R = R << 9;		// shift TX power control to correct RF(R3) register bit position
+				R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+				pAd->LatchRfRegs.R3 = R;
+			}
+			else
+			{
+				R = R << 6;		// shift TX power control to correct RF(R4) register bit position
+				R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+				pAd->LatchRfRegs.R4 = R;
+			}
+		}
+		else// 5.5GHz
+		{
+			if (bPowerReduce == FALSE)
+			{
+				if (index == 0)
+				{
+					R = (R << 10) | (1 << 9);		// shift TX power control to correct RF(R3) register bit position
+					R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+					pAd->LatchRfRegs.R3 = R;
+				}
+				else
+				{
+					R = (R << 7) | (1 << 6);		// shift TX power control to correct RF(R4) register bit position
+					R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+					pAd->LatchRfRegs.R4 = R;
+				}
+			}
+			else
+			{
+				if (index == 0)
+				{
+					R = (R << 10);		// shift TX power control to correct RF(R3) register bit position
+					R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+
+					/* Clear bit 9 of R3 to reduce 7dB. */
+					pAd->LatchRfRegs.R3 = (R & (~(1 << 9)));
+				}
+				else
+				{
+					R = (R << 7);		// shift TX power control to correct RF(R4) register bit position
+					R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+
+					/* Clear bit 6 of R4 to reduce 7dB. */
+					pAd->LatchRfRegs.R4 = (R & (~(1 << 6)));
+				}
+			}
+		}
+
+		RtmpRfIoWrite(pAd);
+
+		return 0;
+	}
+}
+#else// 1 //
+static INT ATETxPwrHandler(
+	IN PRTMP_ADAPTER pAd,
+	IN char index)
+{
+	ULONG R;
+	CHAR TxPower;
+	UCHAR Bbp94 = 0;
+
+#ifdef RALINK_28xx_QA
+	if ((pAd->ate.bQATxStart == TRUE) || (pAd->ate.bQARxStart == TRUE))
+	{
+		// TODO: how to get current TxPower0/1 from pAd->LatchRfRegs ?
+		/* When QA is used for Tx, pAd->ate.TxPower0/1 and real tx power
+		** are not synchronized.
+		*/
+/*
+		pAd->ate.TxPower0 = pAd->LatchRfRegs.xxx;
+		pAd->ate.TxPower1 = pAd->LatchRfRegs.xxx;
+*/
+		return 0;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		TxPower = index == 0 ? pAd->ate.TxPower0 : pAd->ate.TxPower1;
+
+	if (TxPower > 31)
+	{
+		//
+		// R3, R4 can't large than 36 (0x24), 31 ~ 36 used by BBP 94
+		//
+		R = 31;
+		if (TxPower <= 36)
+			Bbp94 = BBPR94_DEFAULT + (UCHAR)(TxPower - 31);
+	}
+	else if (TxPower < 0)
+	{
+		//
+		// R3, R4 can't less than 0, -1 ~ -6 used by BBP 94
+		//
+		R = 0;
+		if (TxPower >= -6)
+			Bbp94 = BBPR94_DEFAULT + TxPower;
+	}
+	else
+	{
+		// 0 ~ 31
+		R = (ULONG) TxPower;
+		Bbp94 = BBPR94_DEFAULT;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("%s (TxPower=%d, R3=%ld, BBP_R94=%d)\n", __func__, TxPower, R, Bbp94));
+
+		if (pAd->ate.Channel <= 14)
+		{
+	if (index == 0)
+	{
+		R = R << 9;		// shift TX power control to correct RF(R3) register bit position
+		R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+		pAd->LatchRfRegs.R3 = R;
+	}
+	else
+	{
+		R = R << 6;		// shift TX power control to correct RF(R4) register bit position
+		R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+		pAd->LatchRfRegs.R4 = R;
+	}
+		}
+		else
+		{
+			if (index == 0)
+			{
+				R = (R << 10) | (1 << 9);		// shift TX power control to correct RF(R3) register bit position
+				R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+				pAd->LatchRfRegs.R3 = R;
+			}
+			else
+			{
+				R = (R << 7) | (1 << 6);		// shift TX power control to correct RF(R4) register bit position
+				R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+				pAd->LatchRfRegs.R4 = R;
+			}
+		}
+
+	RtmpRfIoWrite(pAd);
+
+	return 0;
+	}
+}
+#endif // 1 //
+/*
+    ==========================================================================
+    Description:
+        Set ATE operation mode to
+        0. ATESTART  = Start ATE Mode
+        1. ATESTOP   = Stop ATE Mode
+        2. TXCONT    = Continuous Transmit
+        3. TXCARR    = Transmit Carrier
+        4. TXFRAME   = Transmit Frames
+        5. RXFRAME   = Receive Frames
+#ifdef RALINK_28xx_QA
+        6. TXSTOP    = Stop Any Type of Transmition
+        7. RXSTOP    = Stop Receiving Frames
+#endif // RALINK_28xx_QA //
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+/*                                                           */
+/*                                                           */
+/*=======================End of RT2860=======================*/
+
+
+/*======================Start of RT2870======================*/
+/*                                                           */
+/*                                                           */
+
+#ifdef RT2870
+static INT	ATECmdHandler(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32			Value;
+	UCHAR			BbpData;
+	UINT32          MacData;
+	UINT			i=0, atemode;
+	//NDIS_STATUS		Status = NDIS_STATUS_SUCCESS;
+	//PUCHAR			pDest;
+	UINT32 			temp;
+	ULONG			IrqFlags;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("===> ATECmdHandler()\n"));
+	ATEAsicSwitchChannel(pAd);
+	/* AsicLockChannel() is empty function so far in fact */
+	AsicLockChannel(pAd, pAd->ate.Channel);
+
+	RTMPusecDelay(5000);
+
+	// Default value in BBP R22 is 0x0.
+	BbpData = 0;
+
+	/* Enter ATE mode and set Tx/Rx Idle */
+	if (!strcmp(arg, "ATESTART"))
+	{
+#ifdef CONFIG_STA_SUPPORT
+		BOOLEAN       Cancelled;
+#endif // CONFIG_STA_SUPPORT //
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: ATESTART\n"));
+
+		netif_stop_queue(pAd->net_dev);
+
+		atemode = pAd->ate.Mode;
+		pAd->ate.Mode = ATE_START;
+//		pAd->ate.TxDoneCount = pAd->ate.TxCount;
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Disable auto responder
+		RTMP_IO_READ32(pAd, AUTO_RSP_CFG, &temp);
+		temp = temp & 0xFFFFFFFE;
+		RTMP_IO_WRITE32(pAd, AUTO_RSP_CFG, temp);
+
+		// read MAC_SYS_CTRL and backup MAC_SYS_CTRL value.
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+		// clean bit4 to stop continuous Tx production test.
+		MacData &= 0xFFFFFFEF;
+		// Stop continuous TX production test.
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);//disable or cancel pending irp first ???
+
+        if (atemode & ATE_TXCARR)
+		{
+			// No Carrier Test set BBP R22 bit7=0, bit6=0, bit[5~0]=0x0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF00; //clear bit7, bit6, bit[5~0]
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		}
+		else if (atemode & ATE_TXCARRSUPP)
+		{
+			// No Cont. TX set BBP R22 bit7=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= ~(1 << 7); //set bit7=0
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+			// No Carrier Suppression set BBP R24 bit0=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R24, &BbpData);
+			BbpData &= 0xFFFFFFFE; //clear bit0
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R24, BbpData);
+		}
+		// We should free some resource which allocate when ATE_TXFRAME , ATE_STOP, and ATE_TXCONT.
+		// TODO:Should we free some resource which was allocated when LoopBack and ATE_STOP ?
+		else if ((atemode & ATE_TXFRAME) || (atemode == ATE_STOP))
+		{
+			if (atemode & ATE_TXCONT)
+			{
+				// Not Cont. TX anymore, so set BBP R22 bit7=0
+				ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+				BbpData &= ~(1 << 7); //set bit7=0
+				ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+			}
+			// Abort Tx, Rx DMA.
+			RtmpDmaEnable(pAd, 0);
+
+			{
+				// It seems nothing to free,
+				// because we didn't allocate any resource when we entered ATE_TXFRAME mode latestly.
+			}
+
+			// Start Tx, RX DMA
+			RtmpDmaEnable(pAd, 1);
+		}
+
+		RTUSBRejectPendingPackets(pAd);
+		RTUSBCleanUpDataBulkOutQueue(pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+		//
+		// It will be called in MlmeSuspend().
+		//
+		// Cancel pending timers
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,     &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,       &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,     &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,       &Cancelled);
+#endif // CONFIG_STA_SUPPORT //
+
+		//RTUSBCleanUpMLMEWaitQueue(pAd);	/* not used in RT28xx */
+		RTUSBCleanUpMLMEBulkOutQueue(pAd);
+
+		// Sometimes kernel will hang on, so we avoid calling MlmeSuspend().
+//		MlmeSuspend(pAd, TRUE);
+		//RTMPCancelTimer(&pAd->Mlme.PeriodicTimer, &Cancelled);
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+		// Disable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Make sure there are no pending bulk in/out IRPs before we go on.
+/*=========================================================================*/
+		/* pAd->PendingRx is not of type atomic_t anymore in 28xx */
+//		while ((atomic_read(&pAd->PendingRx) > 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		while ((pAd->PendingRx > 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		{
+#if 1
+			ATE_RTUSBCancelPendingBulkInIRP(pAd);
+#else
+			NdisInterlockedDecrement(&pAd->PendingRx);
+#endif
+			/* delay 0.5 seconds */
+			RTMPusecDelay(500000);
+			pAd->PendingRx = 0;
+		}
+		/* peter : why don't we have to get BulkOutLock first ? */
+		while (((pAd->BulkOutPending[0] == TRUE) ||
+				(pAd->BulkOutPending[1] == TRUE) ||
+				(pAd->BulkOutPending[2] == TRUE) ||
+				(pAd->BulkOutPending[3] == TRUE)) && (pAd->BulkFlags != 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		{
+			do
+			{
+				/* pAd->BulkOutPending[y] will be set to FALSE in RTUSBCancelPendingBulkOutIRP(pAd) */
+				RTUSBCancelPendingBulkOutIRP(pAd);
+			} while (FALSE);
+
+			/* we have enough time delay in RTUSBCancelPendingBulkOutIRP(pAd)
+			** so this is not necessary
+			*/
+//			RTMPusecDelay(500000);
+		}
+
+		/* pAd->PendingRx is not of type atomic_t anymore in 28xx */
+//		ASSERT(atomic_read(&pAd->PendingRx) == 0);
+		ASSERT(pAd->PendingRx == 0);
+/*=========================================================================*/
+
+		// reset Rx statistics.
+		pAd->ate.LastSNR0 = 0;
+		pAd->ate.LastSNR1 = 0;
+		pAd->ate.LastRssi0 = 0;
+		pAd->ate.LastRssi1 = 0;
+		pAd->ate.LastRssi2 = 0;
+		pAd->ate.AvgRssi0 = 0;
+		pAd->ate.AvgRssi1 = 0;
+		pAd->ate.AvgRssi2 = 0;
+		pAd->ate.AvgRssi0X8 = 0;
+		pAd->ate.AvgRssi1X8 = 0;
+		pAd->ate.AvgRssi2X8 = 0;
+		pAd->ate.NumOfAvgRssiSample = 0;
+
+#ifdef RALINK_28xx_QA
+		// Tx frame
+		pAd->ate.bQATxStart = FALSE;
+		pAd->ate.bQARxStart = FALSE;
+		pAd->ate.seq = 0;
+
+		// counters
+		pAd->ate.U2M = 0;
+		pAd->ate.OtherData = 0;
+		pAd->ate.Beacon = 0;
+		pAd->ate.OtherCount = 0;
+		pAd->ate.TxAc0 = 0;
+		pAd->ate.TxAc1 = 0;
+		pAd->ate.TxAc2 = 0;
+		pAd->ate.TxAc3 = 0;
+		pAd->ate.TxHCCA = 0;
+		pAd->ate.TxMgmt = 0;
+		pAd->ate.RSSI0 = 0;
+		pAd->ate.RSSI1 = 0;
+		pAd->ate.RSSI2 = 0;
+		pAd->ate.SNR0 = 0;
+		pAd->ate.SNR1 = 0;
+
+		// control
+		pAd->ate.TxDoneCount = 0;
+		pAd->ate.TxStatus = 0; // task Tx status // 0 --> task is idle, 1 --> task is running
+#endif // RALINK_28xx_QA //
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+
+#ifdef CONFIG_STA_SUPPORT
+		AsicDisableSync(pAd);
+
+		/*
+		** If we skip "LinkDown()", we should disable protection
+		** to prevent from sending out RTS or CTS-to-self.
+		*/
+		ATEDisableAsicProtect(pAd);
+		RTMPStationStop(pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+		// Default value in BBP R22 is 0x0.
+		BbpData = 0;
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+
+		// Clean bit4 to stop continuous Tx production test.
+		MacData &= 0xFFFFFFEF;
+	   	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+	    	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+		//Clean ATE Bulk in/out counter and continue setup
+		InterlockedExchange(&pAd->BulkOutRemained, 0);
+
+		/* NdisAcquireSpinLock()/NdisReleaseSpinLock() need only one argument in RT28xx */
+		NdisAcquireSpinLock(&pAd->GenericLock);
+		pAd->ContinBulkOut = FALSE;
+		pAd->ContinBulkIn = FALSE;
+		NdisReleaseSpinLock(&pAd->GenericLock);
+
+		RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE);
+	}
+	else if (!strcmp(arg, "ATESTOP"))
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE : ATESTOP ===>\n"));
+
+		// Default value in BBP R22 is 0x0.
+		BbpData = 0;
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);//0820
+		// Clean bit4 to stop continuous Tx production test.
+		MacData &= 0xFFFFFFEF;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData); // recover the MAC_SYS_CTRL register back.
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		/*
+		** Abort Tx, RX DMA.
+		** Q   : How to do the following I/O if Tx, Rx DMA is aborted ?
+		** Ans : Bulk endpoints are aborted, while the control endpoint is not.
+		*/
+		RtmpDmaEnable(pAd, 0);
+
+		// Disable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		/* Make sure there are no pending bulk in/out IRPs before we go on. */
+/*=========================================================================*/
+//		while ((atomic_read(&pAd->PendingRx) > 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		while (pAd->PendingRx > 0)
+		{
+#if 1
+			ATE_RTUSBCancelPendingBulkInIRP(pAd);
+#else
+//			NdisInterlockedDecrement(&pAd->PendingRx);
+			pAd->PendingRx--;
+#endif
+			RTMPusecDelay(500000);
+		}
+
+		while (((pAd->BulkOutPending[0] == TRUE) ||
+				(pAd->BulkOutPending[1] == TRUE) ||
+				(pAd->BulkOutPending[2] == TRUE) ||
+				(pAd->BulkOutPending[3] == TRUE)) && (pAd->BulkFlags != 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		{
+			do
+			{
+				RTUSBCancelPendingBulkOutIRP(pAd);
+			} while (FALSE);
+
+			RTMPusecDelay(500000);
+		}
+
+//		ASSERT(atomic_read(&pAd->PendingRx) == 0);
+		ASSERT(pAd->PendingRx == 0);
+/*=========================================================================*/
+/*      Reset Rx RING                                                      */
+/*=========================================================================*/
+//		InterlockedExchange(&pAd->PendingRx, 0);
+		pAd->PendingRx = 0;
+		pAd->NextRxBulkInReadIndex = 0;	// Next Rx Read index
+		pAd->NextRxBulkInIndex = RX_RING_SIZE - 1;	// Rx Bulk pointer
+		pAd->NextRxBulkInPosition = 0;
+		for (i = 0; i < (RX_RING_SIZE); i++)
+		{
+			PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+			NdisZeroMemory(pRxContext->TransferBuffer, MAX_RXBULK_SIZE);
+			/* peter : why don't we have to get BulkInLock first ? */
+			pRxContext->pAd	= pAd;
+			pRxContext->pIrp = NULL;
+            /* peter debug ++ */
+			pRxContext->BulkInOffset = 0;
+			pRxContext->bRxHandling = FALSE;
+            /* peter debug -- */
+			pRxContext->InUse		= FALSE;
+			pRxContext->IRPPending	= FALSE;
+			pRxContext->Readable	= FALSE;
+//			pRxContext->ReorderInUse = FALSE;
+//			pRxContext->ReadPosOffset = 0;
+		}
+
+/*=========================================================================*/
+/*      Reset Tx RING                                                      */
+/*=========================================================================*/
+		do
+		{
+			RTUSBCancelPendingBulkOutIRP(pAd);
+		} while (FALSE);
+
+/*=========================================================================*/
+		// Enable auto responder.
+		RTMP_IO_READ32(pAd, AUTO_RSP_CFG, &temp);
+		temp = temp | (0x01);
+		RTMP_IO_WRITE32(pAd, AUTO_RSP_CFG, temp);
+
+/*================================================*/
+		AsicEnableBssSync(pAd);
+
+		/* Soft reset BBP.*/
+		/* In 2870 chipset, ATE_BBP_IO_READ8_BY_REG_ID() == RTMP_BBP_IO_READ8_BY_REG_ID() */
+		/* Both rt2870ap and rt2870sta use BbpSoftReset(pAd) to do BBP soft reset */
+		BbpSoftReset(pAd);
+/*================================================*/
+		{
+#ifdef CONFIG_STA_SUPPORT
+			// Set all state machines back IDLE
+			pAd->Mlme.CntlMachine.CurrState    = CNTL_IDLE;
+			pAd->Mlme.AssocMachine.CurrState   = ASSOC_IDLE;
+			pAd->Mlme.AuthMachine.CurrState    = AUTH_REQ_IDLE;
+			pAd->Mlme.AuthRspMachine.CurrState = AUTH_RSP_IDLE;
+			pAd->Mlme.SyncMachine.CurrState    = SYNC_IDLE;
+			pAd->Mlme.ActMachine.CurrState    = ACT_IDLE;
+#endif // CONFIG_STA_SUPPORT //
+
+			//
+			// ===> refer to MlmeRestartStateMachine().
+			// When we entered ATE_START mode, PeriodicTimer was not cancelled.
+			// So we don't have to set it here.
+			//
+			//RTMPSetTimer(pAd, &pAd->Mlme.PeriodicTimer, MLME_TASK_EXEC_INTV);
+
+			ASSERT(pAd->CommonCfg.Channel != 0);
+
+			AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+
+#ifdef CONFIG_STA_SUPPORT
+		    RTMPStationStart(pAd);
+#endif // CONFIG_STA_SUPPORT //
+		}
+//
+// These two steps have been done when entering ATE_STOP mode.
+//
+#if 0
+	    RTUSBWriteBBPRegister(pAd, BBP_R22, BbpData);
+	   	RTUSBWriteMACRegister(pAd, MAC_SYS_CTRL, MacData);
+#endif
+		// Clean ATE Bulk in/out counter and continue setup.
+		InterlockedExchange(&pAd->BulkOutRemained, 0);
+		NdisAcquireSpinLock(&pAd->GenericLock);
+		pAd->ContinBulkOut = FALSE;
+		pAd->ContinBulkIn = FALSE;
+		NdisReleaseSpinLock(&pAd->GenericLock);
+
+		/* Wait 50ms to prevent next URB to bulkout during HW reset. */
+		/* todo : remove this if not necessary */
+		NdisMSleep(50000);
+
+		pAd->ate.Mode = ATE_STOP;
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+/*=========================================================================*/
+		/* restore RX_FILTR_CFG */
+#ifdef CONFIG_STA_SUPPORT
+		/* restore RX_FILTR_CFG in order that QA maybe set it to 0x3 */
+		RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL);
+#endif // CONFIG_STA_SUPPORT //
+/*=========================================================================*/
+
+		// Enable Tx, RX DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		// Enable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Wait 10ms to wait all of the bulk-in URBs to complete.
+		/* todo : remove this if not necessary */
+		NdisMSleep(10000);
+
+		// Everything is ready to start normal Tx/Rx.
+		RTUSBBulkReceive(pAd);
+		netif_start_queue(pAd->net_dev);
+
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("<=== ATE : ATESTOP \n"));
+	}
+	else if (!strcmp(arg, "TXCARR"))	// Tx Carrier
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXCARR\n"));
+		pAd->ate.Mode |= ATE_TXCARR;
+
+		// Disable Rx
+		// May be we need not to do this, because these have been done in ATE_START mode ???
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// QA has done the following steps if it is used.
+		if (pAd->ate.bQATxStart == FALSE)
+		{
+			// Soft reset BBP.
+			BbpSoftReset(pAd);
+
+			// Carrier Test set BBP R22 bit7=1, bit6=1, bit[5~0]=0x01
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF00; //clear bit7, bit6, bit[5~0]
+			BbpData |= 0x000000C1; //set bit7=1, bit6=1, bit[5~0]=0x01
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+			// set MAC_SYS_CTRL(0x1004) Continuous Tx Production Test (bit4) = 1
+			RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+			Value = Value | 0x00000010;
+			RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+		}
+	}
+	else if (!strcmp(arg, "TXCONT"))	// Tx Continue
+	{
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			/* set MAC_SYS_CTRL(0x1004) bit4(Continuous Tx Production Test)
+			   and bit2(MAC TX enable) back to zero. */
+			RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+			MacData &= 0xFFFFFFEB;
+			RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+			// set BBP R22 bit7=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF7F; //set bit7=0
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		}
+
+		/* for TxCont mode.
+		** Step 1: Send 50 packets first then wait for a moment.
+		** Step 2: Send more 50 packet then start continue mode.
+		*/
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXCONT\n"));
+		// Step 1: send 50 packets first.
+		pAd->ate.Mode |= ATE_TXCONT;
+		pAd->ate.TxCount = 50;
+		pAd->ate.TxDoneCount = 0;
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+
+		/* Only needed if we have to send some normal frames. */
+		SetJapanFilter(pAd);
+
+		// Setup frame format.
+		ATESetUpFrame(pAd, 0);
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Start Tx, RX DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		InterlockedExchange(&pAd->BulkOutRemained, pAd->ate.TxCount);
+
+#ifdef	RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			pAd->ate.TxStatus = 1;
+			//pAd->ate.Repeat = 0;
+		}
+#endif	// RALINK_28xx_QA //
+
+		NdisAcquireSpinLock(&pAd->GenericLock);//0820
+		pAd->ContinBulkOut = FALSE;
+		NdisReleaseSpinLock(&pAd->GenericLock);
+
+		RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE);
+
+		// Kick bulk out
+		RTUSBKickBulkOut(pAd);
+
+		/* To make sure all the 50 frames have been bulk out before executing step 2 */
+		while (atomic_read(&pAd->BulkOutRemained) > 0)
+		{
+			RTMPusecDelay(5000);
+		}
+
+		// Step 2: send more 50 packets then start continue mode.
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+		// Cont. TX set BBP R22 bit7=1
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+		BbpData |= 0x00000080; //set bit7=1
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+		pAd->ate.TxCount = 50;
+		pAd->ate.TxDoneCount = 0;
+
+		SetJapanFilter(pAd);
+
+		// Setup frame format.
+		ATESetUpFrame(pAd, 0);
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+
+		// Start Tx, RX DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		InterlockedExchange(&pAd->BulkOutRemained, pAd->ate.TxCount);
+
+#ifdef	RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			pAd->ate.TxStatus = 1;
+			//pAd->ate.Repeat = 0;
+		}
+#endif	// RALINK_28xx_QA //
+
+		NdisAcquireSpinLock(&pAd->GenericLock);//0820
+		pAd->ContinBulkOut = FALSE;
+		NdisReleaseSpinLock(&pAd->GenericLock);
+
+		RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE);
+		// Kick bulk out
+		RTUSBKickBulkOut(pAd);
+
+#if 1
+		RTMPusecDelay(500);
+#else
+		while (atomic_read(&pAd->BulkOutRemained) > 0)
+		{
+			RTMPusecDelay(5000);
+		}
+#endif // 1 //
+
+		// Set MAC_SYS_CTRL(0x1004) Continuous Tx Production Test (bit4) = 1.
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+		MacData |= 0x00000010;
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+	}
+	else if (!strcmp(arg, "TXFRAME"))	// Tx Frames
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXFRAME(Count=0x%08x)\n", pAd->ate.TxCount));
+		pAd->ate.Mode |= ATE_TXFRAME;
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+		// Default value in BBP R22 is 0x0.
+		BbpData = 0;
+
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+
+		// Clean bit4 to stop continuous Tx production test.
+		MacData &= 0xFFFFFFEF;
+
+	   	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+#ifdef RALINK_28xx_QA
+		// add this for LoopBack mode
+		if (pAd->ate.bQARxStart == FALSE)
+		{
+			// Disable Rx
+			RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+			Value &= ~(1 << 3);
+			RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+		}
+
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			pAd->ate.TxStatus = 1;
+			//pAd->ate.Repeat = 0;
+		}
+#else
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+#endif // RALINK_28xx_QA //
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+        SetJapanFilter(pAd);
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+		pAd->ate.TxDoneCount = 0;
+
+        // Setup frame format
+		ATESetUpFrame(pAd, 0);
+
+		// Start Tx, RX DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		// Check count is continuous or not yet.
+		//
+		// Due to the type mismatch between "pAd->BulkOutRemained"(atomic_t) and "pAd->ate.TxCount"(UINT32)
+		//
+		if (pAd->ate.TxCount == 0)
+		{
+			InterlockedExchange(&pAd->BulkOutRemained, 0);
+		}
+		else
+		{
+			InterlockedExchange(&pAd->BulkOutRemained, pAd->ate.TxCount);
+		}
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("bulk out count = %d\n", atomic_read(&pAd->BulkOutRemained)));
+		ASSERT((atomic_read(&pAd->BulkOutRemained) >= 0));
+
+		if (atomic_read(&pAd->BulkOutRemained) == 0)
+		{
+			ATEDBGPRINT(RT_DEBUG_TRACE, ("Send packet countinuously\n"));
+
+			/* In 28xx, NdisAcquireSpinLock() == spin_lock_bh() */
+			/* NdisAcquireSpinLock only need one argument in 28xx. */
+			NdisAcquireSpinLock(&pAd->GenericLock);
+			pAd->ContinBulkOut = TRUE;
+			NdisReleaseSpinLock(&pAd->GenericLock);
+
+			/* In 28xx, BULK_OUT_LOCK() == spin_lock_irqsave() */
+			BULK_OUT_LOCK(&pAd->BulkOutLock[0], IrqFlags);// peter : NdisAcquireSpinLock ==> BULK_OUT_LOCK
+			pAd->BulkOutPending[0] = FALSE;
+			BULK_OUT_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);// peter : NdisAcquireSpinLock ==> BULK_OUT_LOCK
+		}
+		else
+		{
+			ATEDBGPRINT(RT_DEBUG_TRACE, ("Send packets depend on counter\n"));
+
+			NdisAcquireSpinLock(&pAd->GenericLock);
+			pAd->ContinBulkOut = FALSE;
+			NdisReleaseSpinLock(&pAd->GenericLock);
+
+			BULK_OUT_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+			pAd->BulkOutPending[0] = FALSE;
+			BULK_OUT_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+		}
+
+		RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE);
+
+		// Kick bulk out
+		RTUSBKickBulkOut(pAd);
+	}
+#ifdef RALINK_28xx_QA
+	else if (!strcmp(arg, "TXSTOP")) 		//Enter ATE mode and set Tx/Rx Idle
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXSTOP\n"));
+
+		atemode = pAd->ate.Mode;
+		pAd->ate.Mode &= ATE_TXSTOP;
+		pAd->ate.bQATxStart = FALSE;
+//		pAd->ate.TxDoneCount = pAd->ate.TxCount;
+
+/*=========================================================================*/
+		if (atemode & ATE_TXCARR)
+		{
+			// No Carrier Test set BBP R22 bit7=0, bit6=0, bit[5~0]=0x0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF00; //clear bit7, bit6, bit[5~0]
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		}
+		else if (atemode & ATE_TXCARRSUPP)
+		{
+			// No Cont. TX set BBP R22 bit7=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= ~(1 << 7); //set bit7=0
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+			// No Carrier Suppression set BBP R24 bit0=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R24, &BbpData);
+			BbpData &= 0xFFFFFFFE; //clear bit0
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R24, BbpData);
+		}
+		else if ((atemode & ATE_TXFRAME) || (atemode == ATE_STOP))
+		{
+			if (atemode & ATE_TXCONT)
+			{
+				// No Cont. TX set BBP R22 bit7=0
+				ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+				BbpData &= ~(1 << 7); //set bit7=0
+				ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+			}
+		}
+
+/*=========================================================================*/
+		RTUSBRejectPendingPackets(pAd);
+		RTUSBCleanUpDataBulkOutQueue(pAd);
+
+		/* not used in RT28xx */
+		//RTUSBCleanUpMLMEWaitQueue(pAd);
+		/* empty function so far */
+		RTUSBCleanUpMLMEBulkOutQueue(pAd);
+/*=========================================================================*/
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+/*=========================================================================*/
+
+		/* In 28xx, pAd->PendingRx is not of type atomic_t anymore */
+//		while ((atomic_read(&pAd->PendingRx) > 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		/* peter todo : BulkInLock */
+		while (pAd->PendingRx > 0)
+		{
+#if 1
+			ATE_RTUSBCancelPendingBulkInIRP(pAd);
+#else
+//			NdisInterlockedDecrement(&pAd->PendingRx);
+			pAd->PendingRx--;
+#endif
+			RTMPusecDelay(500000);
+		}
+
+		while (((pAd->BulkOutPending[0] == TRUE) ||
+				(pAd->BulkOutPending[1] == TRUE) ||
+				(pAd->BulkOutPending[2] == TRUE) ||
+				(pAd->BulkOutPending[3] == TRUE)) && (pAd->BulkFlags != 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		{
+			do
+			{
+				RTUSBCancelPendingBulkOutIRP(pAd);
+			} while (FALSE);
+
+			RTMPusecDelay(500000);
+		}
+
+		ASSERT(pAd->PendingRx == 0);
+/*=========================================================================*/
+		// Enable Tx, Rx DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		/* task Tx status : 0 --> task is idle, 1 --> task is running */
+		pAd->ate.TxStatus = 0;
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+		// Disable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+		MacData &= (0xfffffffb);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+		//Clean ATE Bulk in/out counter and continue setup
+		InterlockedExchange(&pAd->BulkOutRemained, 0);
+
+		pAd->ContinBulkOut = FALSE;
+	}
+	else if (!strcmp(arg, "RXSTOP"))
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: RXSTOP\n"));
+		atemode = pAd->ate.Mode;
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		pAd->ate.Mode &= ATE_RXSTOP;
+		pAd->ate.bQARxStart = FALSE;
+//		pAd->ate.TxDoneCount = pAd->ate.TxCount;
+
+/*=========================================================================*/
+		RTUSBRejectPendingPackets(pAd);
+		RTUSBCleanUpDataBulkOutQueue(pAd);
+
+		/* not used in RT28xx */
+		//RTUSBCleanUpMLMEWaitQueue(pAd);
+		RTUSBCleanUpMLMEBulkOutQueue(pAd);
+/*=========================================================================*/
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+/*=========================================================================*/
+//		while ((atomic_read(&pAd->PendingRx) > 0))
+		while (pAd->PendingRx > 0)
+		{
+#if 1
+			ATE_RTUSBCancelPendingBulkInIRP(pAd);
+#else
+//			NdisInterlockedDecrement(&pAd->PendingRx);
+			pAd->PendingRx--;
+#endif
+			RTMPusecDelay(500000);
+		}
+
+		while (((pAd->BulkOutPending[0] == TRUE) ||
+				(pAd->BulkOutPending[1] == TRUE) ||
+				(pAd->BulkOutPending[2] == TRUE) ||
+				(pAd->BulkOutPending[3] == TRUE)) && (pAd->BulkFlags != 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		{
+			do
+			{
+				RTUSBCancelPendingBulkOutIRP(pAd);
+			} while (FALSE);
+
+			RTMPusecDelay(500000);
+		}
+
+		ASSERT(pAd->PendingRx == 0);
+/*=========================================================================*/
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+		pAd->ContinBulkIn = FALSE;
+	}
+#endif // RALINK_28xx_QA //
+	else if (!strcmp(arg, "RXFRAME")) // Rx Frames
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: RXFRAME\n"));
+
+		// Disable Rx of MAC block
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Default value in BBP R22 is 0x0.
+		BbpData = 0;
+
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+		// Clean bit4 to stop continuous Tx production test.
+		MacData &= 0xFFFFFFEF;
+
+	   	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+		pAd->ate.Mode |= ATE_RXFRAME;
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+		// Disable TX of MAC block
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+        // Reset Rx RING.
+		for ( i = 0; i < (RX_RING_SIZE); i++)
+		{
+			PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+
+			pRxContext->InUse = FALSE;
+			pRxContext->IRPPending = FALSE;
+			pRxContext->Readable = FALSE;
+
+			//
+			// Get the urb from kernel back to driver.
+			//
+			RTUSB_UNLINK_URB(pRxContext->pUrb);
+
+			/* Sleep 200 microsecs to give cancellation time to work. */
+			NdisMSleep(200);
+			pAd->BulkInReq = 0;
+
+//			InterlockedExchange(&pAd->PendingRx, 0);
+			pAd->PendingRx = 0;
+			pAd->NextRxBulkInReadIndex = 0;	// Next Rx Read index
+			pAd->NextRxBulkInIndex		= RX_RING_SIZE - 1;	// Rx Bulk pointer
+			pAd->NextRxBulkInPosition = 0;
+		}
+
+		// read to clear counters
+		RTUSBReadMACRegister(pAd, RX_STA_CNT0, &temp); //RX PHY & RX CRC count
+		RTUSBReadMACRegister(pAd, RX_STA_CNT1, &temp); //RX PLCP error count & CCA false alarm count
+		RTUSBReadMACRegister(pAd, RX_STA_CNT2, &temp); //RX FIFO overflow frame count & RX duplicated filtered frame count
+
+		pAd->ContinBulkIn = TRUE;
+
+		// Enable Tx, RX DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		// Enable RX of MAC block
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Kick bulk in
+		RTUSBBulkReceive(pAd);
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: Invalid arg!\n"));
+		return FALSE;
+	}
+	RTMPusecDelay(5000);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("<=== ATECmdHandler()\n"));
+
+	return TRUE;
+}
+#endif // RT2870 //
+
+INT	Set_ATE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	if (ATECmdHandler(pAd, arg))
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_Proc Success\n"));
+
+
+		return TRUE;
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_Proc Failed\n"));
+		return FALSE;
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE ADDR1=DA for TxFrame(AP  : To DS = 0 ; From DS = 1)
+        or
+        Set ATE ADDR3=DA for TxFrame(STA : To DS = 1 ; From DS = 0)
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_DA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR				*value;
+	INT					i;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+    for (i=0, value = rstrtok(arg, ":"); value; value = rstrtok(NULL, ":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+		AtoH(value, &pAd->ate.Addr3[i++], 1);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	if(i != 6)
+		return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_DA_Proc (DA = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAd->ate.Addr3[0],
+		pAd->ate.Addr3[1], pAd->ate.Addr3[2], pAd->ate.Addr3[3], pAd->ate.Addr3[4], pAd->ate.Addr3[5]));
+#endif // CONFIG_STA_SUPPORT //
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_DA_Proc Success\n"));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE ADDR3=SA for TxFrame(AP  : To DS = 0 ; From DS = 1)
+        or
+        Set ATE ADDR2=SA for TxFrame(STA : To DS = 1 ; From DS = 0)
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_SA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR				*value;
+	INT					i;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+    for (i=0, value = rstrtok(arg, ":"); value; value = rstrtok(NULL, ":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+		AtoH(value, &pAd->ate.Addr2[i++], 1);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	if(i != 6)
+		return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_SA_Proc (SA = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAd->ate.Addr2[0],
+		pAd->ate.Addr2[1], pAd->ate.Addr2[2], pAd->ate.Addr2[3], pAd->ate.Addr2[4], pAd->ate.Addr2[5]));
+#endif // CONFIG_STA_SUPPORT //
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_SA_Proc Success\n"));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE ADDR2=BSSID for TxFrame(AP  : To DS = 0 ; From DS = 1)
+        or
+        Set ATE ADDR1=BSSID for TxFrame(STA : To DS = 1 ; From DS = 0)
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_BSSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR				*value;
+	INT					i;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+    for (i=0, value = rstrtok(arg, ":"); value; value = rstrtok(NULL, ":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+		AtoH(value, &pAd->ate.Addr1[i++], 1);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	if(i != 6)
+		return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_BSSID_Proc (BSSID = %2X:%2X:%2X:%2X:%2X:%2X)\n",	pAd->ate.Addr1[0],
+		pAd->ate.Addr1[1], pAd->ate.Addr1[2], pAd->ate.Addr1[3], pAd->ate.Addr1[4], pAd->ate.Addr1[5]));
+#endif // CONFIG_STA_SUPPORT //
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_BSSID_Proc Success\n"));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Channel
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_CHANNEL_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR channel;
+
+	channel = simple_strtol(arg, 0, 10);
+
+	if ((channel < 1) || (channel > 216))// to allow A band channel : ((channel < 1) || (channel > 14))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_CHANNEL_Proc::Out of range, it should be in range of 1~14.\n"));
+		return FALSE;
+	}
+	pAd->ate.Channel = channel;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_CHANNEL_Proc (ATE Channel = %d)\n", pAd->ate.Channel));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_CHANNEL_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Power0
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_POWER0_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR TxPower;
+
+	TxPower = simple_strtol(arg, 0, 10);
+
+	if (pAd->ate.Channel <= 14)
+	{
+		if ((TxPower > 31) || (TxPower < 0))
+		{
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER0_Proc::Out of range (Value=%d)\n", TxPower));
+			return FALSE;
+		}
+	}
+	else// 5.5GHz
+	{
+		if ((TxPower > 15) || (TxPower < -7))
+		{
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER0_Proc::Out of range (Value=%d)\n", TxPower));
+			return FALSE;
+		}
+	}
+
+	pAd->ate.TxPower0 = TxPower;
+	ATETxPwrHandler(pAd, 0);
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_POWER0_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Power1
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_POWER1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR TxPower;
+
+	TxPower = simple_strtol(arg, 0, 10);
+
+	if (pAd->ate.Channel <= 14)
+	{
+	if ((TxPower > 31) || (TxPower < 0))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER1_Proc::Out of range (Value=%d)\n", TxPower));
+		return FALSE;
+	}
+	}
+	else
+	{
+		if ((TxPower > 15) || (TxPower < -7))
+		{
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER1_Proc::Out of range (Value=%d)\n", TxPower));
+			return FALSE;
+		}
+	}
+
+	pAd->ate.TxPower1 = TxPower;
+	ATETxPwrHandler(pAd, 1);
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_POWER1_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Antenna
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR value;
+
+	value = simple_strtol(arg, 0, 10);
+
+	if ((value > 2) || (value < 0))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_Antenna_Proc::Out of range (Value=%d)\n", value));
+		return FALSE;
+	}
+
+	pAd->ate.TxAntennaSel = value;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_Antenna_Proc (Antenna = %d)\n", pAd->ate.TxAntennaSel));
+	ATEDBGPRINT(RT_DEBUG_TRACE,("Ralink: Set_ATE_TX_Antenna_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Rx Antenna
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_RX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR value;
+
+	value = simple_strtol(arg, 0, 10);
+
+	if ((value > 3) || (value < 0))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_RX_Antenna_Proc::Out of range (Value=%d)\n", value));
+		return FALSE;
+	}
+
+	pAd->ate.RxAntennaSel = value;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_RX_Antenna_Proc (Antenna = %d)\n", pAd->ate.RxAntennaSel));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_RX_Antenna_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE RF frequence offset
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_FREQOFFSET_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR RFFreqOffset;
+	ULONG R4;
+
+	RFFreqOffset = simple_strtol(arg, 0, 10);
+
+	if(RFFreqOffset >= 64)
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_FREQOFFSET_Proc::Out of range, it should be in range of 0~63.\n"));
+		return FALSE;
+	}
+
+	pAd->ate.RFFreqOffset = RFFreqOffset;
+	R4 = pAd->ate.RFFreqOffset << 15;		// shift TX power control to correct RF register bit position
+	R4 |= (pAd->LatchRfRegs.R4 & ((~0x001f8000)));
+	pAd->LatchRfRegs.R4 = R4;
+
+	RtmpRfIoWrite(pAd);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_FREQOFFSET_Proc (RFFreqOffset = %d)\n", pAd->ate.RFFreqOffset));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_FREQOFFSET_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE RF BW
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_BW_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	int i;
+	UCHAR value = 0;
+	UCHAR BBPCurrentBW;
+
+	BBPCurrentBW = simple_strtol(arg, 0, 10);
+
+	if(BBPCurrentBW == 0)
+		pAd->ate.TxWI.BW = BW_20;
+	else
+		pAd->ate.TxWI.BW = BW_40;
+
+	if(pAd->ate.TxWI.BW == BW_20)
+	{
+		if(pAd->ate.Channel <= 14)
+		{
+ 		for (i=0; i<5; i++)
+ 		{
+				if (pAd->Tx20MPwrCfgGBand[i] != 0xffffffff)
+				{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx20MPwrCfgGBand[i]);
+					RTMPusecDelay(5000);
+				}
+			}
+		}
+		else
+		{
+			for (i=0; i<5; i++)
+			{
+				if (pAd->Tx20MPwrCfgABand[i] != 0xffffffff)
+ 			{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx20MPwrCfgABand[i]);
+ 				RTMPusecDelay(5000);
+ 			}
+ 		}
+		}
+
+		//Set BBP R4 bit[4:3]=0:0
+ 		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &value);
+ 		value &= (~0x18);
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, value);
+
+  		//Set BBP R66=0x3C
+ 		value = 0x3C;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, value);
+		//Set BBP R68=0x0B
+		//to improve Rx sensitivity.
+		value = 0x0B;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R68, value);
+		//Set BBP R69=0x16
+		value = 0x16;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, value);
+		//Set BBP R70=0x08
+		value = 0x08;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, value);
+		//Set BBP R73=0x11
+		value = 0x11;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, value);
+
+	    // If Channel=14, Bandwidth=20M and Mode=CCK, Set BBP R4 bit5=1
+	    // (Japan filter coefficients)
+	    // This segment of code will only works when ATETXMODE and ATECHANNEL
+	    // were set to MODE_CCK and 14 respectively before ATETXBW is set to 0.
+	    //=====================================================================
+		if (pAd->ate.Channel == 14)
+		{
+			int TxMode = pAd->ate.TxWI.PHYMODE;
+			if (TxMode == MODE_CCK)
+			{
+				// when Channel==14 && Mode==CCK && BandWidth==20M, BBP R4 bit5=1
+ 				ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &value);
+				value |= 0x20; //set bit5=1
+ 				ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, value);
+			}
+		}
+
+	    //=====================================================================
+		// If bandwidth != 40M, RF Reg4 bit 21 = 0.
+		pAd->LatchRfRegs.R4 &= ~0x00200000;
+		RtmpRfIoWrite(pAd);
+	}
+	else if(pAd->ate.TxWI.BW == BW_40)
+	{
+		if(pAd->ate.Channel <= 14)
+		{
+			for (i=0; i<5; i++)
+			{
+				if (pAd->Tx40MPwrCfgGBand[i] != 0xffffffff)
+				{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx40MPwrCfgGBand[i]);
+					RTMPusecDelay(5000);
+				}
+			}
+		}
+		else
+		{
+			for (i=0; i<5; i++)
+			{
+				if (pAd->Tx40MPwrCfgABand[i] != 0xffffffff)
+				{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx40MPwrCfgABand[i]);
+					RTMPusecDelay(5000);
+				}
+			}
+#ifdef DOT11_N_SUPPORT
+			if ((pAd->ate.TxWI.PHYMODE >= MODE_HTMIX) && (pAd->ate.TxWI.MCS == 7))
+			{
+    			value = 0x28;
+    			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R67, value);
+			}
+#endif // DOT11_N_SUPPORT //
+		}
+
+		//Set BBP R4 bit[4:3]=1:0
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &value);
+		value &= (~0x18);
+		value |= 0x10;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, value);
+
+		//Set BBP R66=0x3C
+		value = 0x3C;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, value);
+		//Set BBP R68=0x0C
+		//to improve Rx sensitivity.
+		value = 0x0C;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R68, value);
+		//Set BBP R69=0x1A
+		value = 0x1A;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, value);
+		//Set BBP R70=0x0A
+		value = 0x0A;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, value);
+		//Set BBP R73=0x16
+		value = 0x16;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, value);
+
+		// If bandwidth = 40M, set RF Reg4 bit 21 = 1.
+		pAd->LatchRfRegs.R4 |= 0x00200000;
+		RtmpRfIoWrite(pAd);
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_BW_Proc (BBPCurrentBW = %d)\n", pAd->ate.TxWI.BW));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_BW_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame length
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_LENGTH_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxLength = simple_strtol(arg, 0, 10);
+
+	if((pAd->ate.TxLength < 24) || (pAd->ate.TxLength > (MAX_FRAME_SIZE - 34/* == 2312 */)))
+	{
+		pAd->ate.TxLength = (MAX_FRAME_SIZE - 34/* == 2312 */);
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_LENGTH_Proc::Out of range, it should be in range of 24~%d.\n", (MAX_FRAME_SIZE - 34/* == 2312 */)));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_LENGTH_Proc (TxLength = %d)\n", pAd->ate.TxLength));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_LENGTH_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame count
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_COUNT_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxCount = simple_strtol(arg, 0, 10);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_COUNT_Proc (TxCount = %d)\n", pAd->ate.TxCount));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_COUNT_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame MCS
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_MCS_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR MCS;
+	int result;
+
+	MCS = simple_strtol(arg, 0, 10);
+	result = CheckMCSValid(pAd->ate.TxWI.PHYMODE, MCS);
+
+	if (result != -1)
+	{
+		pAd->ate.TxWI.MCS = (UCHAR)MCS;
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_MCS_Proc::Out of range, refer to rate table.\n"));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_MCS_Proc (MCS = %d)\n", pAd->ate.TxWI.MCS));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_MCS_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame Mode
+        0: MODE_CCK
+        1: MODE_OFDM
+        2: MODE_HTMIX
+        3: MODE_HTGREENFIELD
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_MODE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxWI.PHYMODE = simple_strtol(arg, 0, 10);
+
+	if(pAd->ate.TxWI.PHYMODE > 3)
+	{
+		pAd->ate.TxWI.PHYMODE = 0;
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_MODE_Proc::Out of range. it should be in range of 0~3\n"));
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("0: CCK, 1: OFDM, 2: HT_MIX, 3: HT_GREEN_FIELD.\n"));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_MODE_Proc (TxMode = %d)\n", pAd->ate.TxWI.PHYMODE));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_MODE_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame GI
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_GI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxWI.ShortGI = simple_strtol(arg, 0, 10);
+
+	if(pAd->ate.TxWI.ShortGI > 1)
+	{
+		pAd->ate.TxWI.ShortGI = 0;
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_GI_Proc::Out of range\n"));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_GI_Proc (GI = %d)\n", pAd->ate.TxWI.ShortGI));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_GI_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+    ==========================================================================
+ */
+INT	Set_ATE_RX_FER_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.bRxFer = simple_strtol(arg, 0, 10);
+
+	if (pAd->ate.bRxFer == 1)
+	{
+		pAd->ate.RxCntPerSec = 0;
+		pAd->ate.RxTotalCnt = 0;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_RX_FER_Proc (bRxFer = %d)\n", pAd->ate.bRxFer));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_RX_FER_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+INT Set_ATE_Read_RF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ate_print(KERN_EMERG "R1 = %lx\n", pAd->LatchRfRegs.R1);
+	ate_print(KERN_EMERG "R2 = %lx\n", pAd->LatchRfRegs.R2);
+	ate_print(KERN_EMERG "R3 = %lx\n", pAd->LatchRfRegs.R3);
+	ate_print(KERN_EMERG "R4 = %lx\n", pAd->LatchRfRegs.R4);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R1 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R2 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R3 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R4 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Load and Write EEPROM from a binary file prepared in advance.
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+#ifndef UCOS
+INT Set_ATE_Load_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	BOOLEAN		    ret = FALSE;
+	PUCHAR			src = EEPROM_BIN_FILE_NAME;
+	struct file		*srcf;
+	INT32 			retval, orgfsuid, orgfsgid;
+   	mm_segment_t	orgfs;
+	USHORT 			WriteEEPROM[(EEPROM_SIZE/2)];
+	UINT32			FileLength = 0;
+	UINT32 			value = simple_strtol(arg, 0, 10);
+
+	ATEDBGPRINT(RT_DEBUG_ERROR, ("===> %s (value=%d)\n\n", __func__, value));
+
+	if (value > 0)
+	{
+		/* zero the e2p buffer */
+		NdisZeroMemory((PUCHAR)WriteEEPROM, EEPROM_SIZE);
+
+		/* save uid and gid used for filesystem access.
+	    ** set user and group to 0 (root)
+	    */
+		orgfsuid = current->fsuid;
+		orgfsgid = current->fsgid;
+		/* as root */
+		current->fsuid = current->fsgid = 0;
+    	orgfs = get_fs();
+    	set_fs(KERNEL_DS);
+
+		do
+		{
+			/* open the bin file */
+			srcf = filp_open(src, O_RDONLY, 0);
+
+			if (IS_ERR(srcf))
+			{
+				ate_print("%s - Error %ld opening %s\n", __func__, -PTR_ERR(srcf), src);
+				break;
+			}
+
+			/* the object must have a read method */
+			if ((srcf->f_op == NULL) || (srcf->f_op->read == NULL))
+			{
+				ate_print("%s - %s does not have a read method\n", __func__, src);
+				break;
+			}
+
+			/* read the firmware from the file *.bin */
+			FileLength = srcf->f_op->read(srcf,
+										  (PUCHAR)WriteEEPROM,
+										  EEPROM_SIZE,
+										  &srcf->f_pos);
+
+			if (FileLength != EEPROM_SIZE)
+			{
+				ate_print("%s: error file length (=%d) in e2p.bin\n",
+					   __func__, FileLength);
+				break;
+			}
+			else
+			{
+				/* write the content of .bin file to EEPROM */
+				rt_ee_write_all(pAd, WriteEEPROM);
+				ret = TRUE;
+			}
+			break;
+		} while(TRUE);
+
+		/* close firmware file */
+		if (IS_ERR(srcf))
+		{
+				;
+		}
+		else
+		{
+			retval = filp_close(srcf, NULL);
+			if (retval)
+			{
+				ATEDBGPRINT(RT_DEBUG_ERROR, ("--> Error %d closing %s\n", -retval, src));
+
+			}
+		}
+
+		/* restore */
+		set_fs(orgfs);
+		current->fsuid = orgfsuid;
+		current->fsgid = orgfsgid;
+	}
+    ATEDBGPRINT(RT_DEBUG_ERROR, ("<=== %s (ret=%d)\n", __func__, ret));
+
+    return ret;
+
+}
+#else
+INT Set_ATE_Load_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT 			WriteEEPROM[(EEPROM_SIZE/2)];
+	struct iwreq	*wrq = (struct iwreq *)arg;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("===> %s (wrq->u.data.length = %d)\n\n", __func__, wrq->u.data.length));
+
+	if (wrq->u.data.length != EEPROM_SIZE)
+	{
+		ate_print("%s: error length (=%d) from host\n",
+			   __func__, wrq->u.data.length);
+		return FALSE;
+	}
+	else/* (wrq->u.data.length == EEPROM_SIZE) */
+	{
+		/* zero the e2p buffer */
+		NdisZeroMemory((PUCHAR)WriteEEPROM, EEPROM_SIZE);
+
+		/* fill the local buffer */
+		NdisMoveMemory((PUCHAR)WriteEEPROM, wrq->u.data.pointer, wrq->u.data.length);
+
+		do
+		{
+				/* write the content of .bin file to EEPROM */
+				rt_ee_write_all(pAd, WriteEEPROM);
+
+		} while(FALSE);
+		}
+
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("<=== %s\n", __func__));
+
+    return TRUE;
+
+}
+#endif // !UCOS //
+
+INT Set_ATE_Read_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT buffer[EEPROM_SIZE/2];
+	USHORT *p;
+	int i;
+
+	rt_ee_read_all(pAd, (USHORT *)buffer);
+	p = buffer;
+	for (i = 0; i < (EEPROM_SIZE/2); i++)
+	{
+		ate_print("%4.4x ", *p);
+		if (((i+1) % 16) == 0)
+			ate_print("\n");
+		p++;
+	}
+	return TRUE;
+}
+
+INT	Set_ATE_Show_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ate_print("Mode=%d\n", pAd->ate.Mode);
+	ate_print("TxPower0=%d\n", pAd->ate.TxPower0);
+	ate_print("TxPower1=%d\n", pAd->ate.TxPower1);
+	ate_print("TxAntennaSel=%d\n", pAd->ate.TxAntennaSel);
+	ate_print("RxAntennaSel=%d\n", pAd->ate.RxAntennaSel);
+	ate_print("BBPCurrentBW=%d\n", pAd->ate.TxWI.BW);
+	ate_print("GI=%d\n", pAd->ate.TxWI.ShortGI);
+	ate_print("MCS=%d\n", pAd->ate.TxWI.MCS);
+	ate_print("TxMode=%d\n", pAd->ate.TxWI.PHYMODE);
+	ate_print("Addr1=%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pAd->ate.Addr1[0], pAd->ate.Addr1[1], pAd->ate.Addr1[2], pAd->ate.Addr1[3], pAd->ate.Addr1[4], pAd->ate.Addr1[5]);
+	ate_print("Addr2=%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pAd->ate.Addr2[0], pAd->ate.Addr2[1], pAd->ate.Addr2[2], pAd->ate.Addr2[3], pAd->ate.Addr2[4], pAd->ate.Addr2[5]);
+	ate_print("Addr3=%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pAd->ate.Addr3[0], pAd->ate.Addr3[1], pAd->ate.Addr3[2], pAd->ate.Addr3[3], pAd->ate.Addr3[4], pAd->ate.Addr3[5]);
+	ate_print("Channel=%d\n", pAd->ate.Channel);
+	ate_print("TxLength=%d\n", pAd->ate.TxLength);
+	ate_print("TxCount=%u\n", pAd->ate.TxCount);
+	ate_print("RFFreqOffset=%d\n", pAd->ate.RFFreqOffset);
+	ate_print(KERN_EMERG "Set_ATE_Show_Proc Success\n");
+	return TRUE;
+}
+
+INT	Set_ATE_Help_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ate_print("ATE=ATESTART, ATESTOP, TXCONT, TXCARR, TXFRAME, RXFRAME\n");
+	ate_print("ATEDA\n");
+	ate_print("ATESA\n");
+	ate_print("ATEBSSID\n");
+	ate_print("ATECHANNEL, range:0~14(unless A band !)\n");
+	ate_print("ATETXPOW0, set power level of antenna 1.\n");
+	ate_print("ATETXPOW1, set power level of antenna 2.\n");
+	ate_print("ATETXANT, set TX antenna. 0:all, 1:antenna one, 2:antenna two.\n");
+	ate_print("ATERXANT, set RX antenna.0:all, 1:antenna one, 2:antenna two, 3:antenna three.\n");
+	ate_print("ATETXFREQOFFSET, set frequency offset, range 0~63\n");
+	ate_print("ATETXBW, set BandWidth, 0:20MHz, 1:40MHz.\n");
+	ate_print("ATETXLEN, set Frame length, range 24~%d\n", (MAX_FRAME_SIZE - 34/* == 2312 */));
+	ate_print("ATETXCNT, set how many frame going to transmit.\n");
+	ate_print("ATETXMCS, set MCS, reference to rate table.\n");
+	ate_print("ATETXMODE, set Mode 0:CCK, 1:OFDM, 2:HT-Mix, 3:GreenField, reference to rate table.\n");
+	ate_print("ATETXGI, set GI interval, 0:Long, 1:Short\n");
+	ate_print("ATERXFER, 0:disable Rx Frame error rate. 1:enable Rx Frame error rate.\n");
+	ate_print("ATERRF, show all RF registers.\n");
+	ate_print("ATEWRF1, set RF1 register.\n");
+	ate_print("ATEWRF2, set RF2 register.\n");
+	ate_print("ATEWRF3, set RF3 register.\n");
+	ate_print("ATEWRF4, set RF4 register.\n");
+	ate_print("ATELDE2P, load EEPROM from .bin file.\n");
+	ate_print("ATERE2P, display all EEPROM content.\n");
+	ate_print("ATESHOW, display all parameters of ATE.\n");
+	ate_print("ATEHELP, online help.\n");
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	AsicSwitchChannel() dedicated for ATE.
+
+    ==========================================================================
+*/
+VOID ATEAsicSwitchChannel(
+    IN PRTMP_ADAPTER pAd)
+{
+	UINT32 R2 = 0, R3 = DEFAULT_RF_TX_POWER, R4 = 0, Value = 0;
+	CHAR TxPwer = 0, TxPwer2 = 0;
+	UCHAR index, BbpValue = 0, R66 = 0x30;
+	RTMP_RF_REGS *RFRegTable;
+	UCHAR Channel;
+
+#ifdef RALINK_28xx_QA
+	if ((pAd->ate.bQATxStart == TRUE) || (pAd->ate.bQARxStart == TRUE))
+	{
+		if (pAd->ate.Channel != pAd->LatchRfRegs.Channel)
+		{
+			pAd->ate.Channel = pAd->LatchRfRegs.Channel;
+		}
+		return;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	Channel = pAd->ate.Channel;
+
+	// Select antenna
+	AsicAntennaSelect(pAd, Channel);
+
+	// fill Tx power value
+	TxPwer = pAd->ate.TxPower0;
+	TxPwer2 = pAd->ate.TxPower1;
+
+	RFRegTable = RF2850RegTable;
+
+	switch (pAd->RfIcType)
+	{
+		/* But only 2850 and 2750 support 5.5GHz band... */
+		case RFIC_2820:
+		case RFIC_2850:
+		case RFIC_2720:
+		case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R2 = RFRegTable[index].R2;
+					if (pAd->Antenna.field.TxPath == 1)
+					{
+						R2 |= 0x4000;	// If TXpath is 1, bit 14 = 1;
+					}
+
+					if (pAd->Antenna.field.RxPath == 2)
+					{
+						switch (pAd->ate.RxAntennaSel)
+						{
+							case 1:
+								R2 |= 0x20040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x00;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							case 2:
+								R2 |= 0x10040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x01;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							default:
+								R2 |= 0x40;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								/* Only enable two Antenna to receive. */
+								BbpValue |= 0x08;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+						}
+					}
+					else if (pAd->Antenna.field.RxPath == 1)
+					{
+						R2 |= 0x20040;	// write 1 to off RxPath
+					}
+
+					if (pAd->Antenna.field.TxPath == 2)
+					{
+						if (pAd->ate.TxAntennaSel == 1)
+						{
+							R2 |= 0x4000;	// If TX Antenna select is 1 , bit 14 = 1; Disable Ant 2
+							ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpValue);
+							BbpValue &= 0xE7;		//11100111B
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpValue);
+						}
+						else if (pAd->ate.TxAntennaSel == 2)
+						{
+							R2 |= 0x8000;	// If TX Antenna select is 2 , bit 15 = 1; Disable Ant 1
+							ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpValue);
+							BbpValue &= 0xE7;
+							BbpValue |= 0x08;
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpValue);
+						}
+						else
+						{
+							ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpValue);
+							BbpValue &= 0xE7;
+							BbpValue |= 0x10;
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpValue);
+						}
+					}
+					if (pAd->Antenna.field.RxPath == 3)
+					{
+						switch (pAd->ate.RxAntennaSel)
+						{
+							case 1:
+								R2 |= 0x20040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x00;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							case 2:
+								R2 |= 0x10040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x01;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							case 3:
+								R2 |= 0x30000;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x02;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							default:
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x10;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+						}
+					}
+
+					if (Channel > 14)
+					{
+						// initialize R3, R4
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff);
+						R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->ate.RFFreqOffset << 15);
+
+                        // According the Rory's suggestion to solve the middle range issue.
+						// 5.5G band power range: 0xF9~0X0F, TX0 Reg3 bit9/TX1 Reg4 bit6="0" means the TX power reduce 7dB
+						// R3
+						if ((TxPwer >= -7) && (TxPwer < 0))
+						{
+							TxPwer = (7+TxPwer);
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10);
+							ATEDBGPRINT(RT_DEBUG_TRACE, ("ATEAsicSwitchChannel: TxPwer=%d \n", TxPwer));
+						}
+						else
+						{
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10) | (1 << 9);
+						}
+
+						// R4
+						if ((TxPwer2 >= -7) && (TxPwer2 < 0))
+						{
+							TxPwer2 = (7+TxPwer2);
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7);
+							ATEDBGPRINT(RT_DEBUG_TRACE, ("ATEAsicSwitchChannel: TxPwer2=%d \n", TxPwer2));
+						}
+						else
+						{
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7) | (1 << 6);
+						}
+					}
+					else
+					{
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff) | (TxPwer << 9); // set TX power0
+						R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->ate.RFFreqOffset << 15) | (TxPwer2 <<6);// Set freq offset & TxPwr1
+					}
+
+					// Based on BBP current mode before changing RF channel.
+					if (pAd->ate.TxWI.BW == BW_40)
+					{
+						R4 |=0x200000;
+					}
+
+					// Update variables
+					pAd->LatchRfRegs.Channel = Channel;
+					pAd->LatchRfRegs.R1 = RFRegTable[index].R1;
+					pAd->LatchRfRegs.R2 = R2;
+					pAd->LatchRfRegs.R3 = R3;
+					pAd->LatchRfRegs.R4 = R4;
+
+					RtmpRfIoWrite(pAd);
+
+					break;
+				}
+			}
+			break;
+
+		default:
+			break;
+	}
+
+	// Change BBP setting during switch from a->g, g->a
+	if (Channel <= 14)
+	{
+	    ULONG	TxPinCfg = 0x00050F0A;// 2007.10.09 by Brian : 0x0005050A ==> 0x00050F0A
+
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+
+		/* For 1T/2R chip only... */
+	    if (pAd->NicConfig2.field.ExternalLNAForG)
+	    {
+	        ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x62);
+	    }
+	    else
+	    {
+	        ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x84);
+	    }
+
+        // According the Rory's suggestion to solve the middle range issue.
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R86, &BbpValue);
+		ASSERT((BbpValue == 0x00));
+		if ((BbpValue != 0x00))
+		{
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0x00);
+		}
+
+		// 5.5GHz band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x04);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R.
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+		}
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+		}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+	else
+	{
+	    ULONG	TxPinCfg = 0x00050F05;//2007.10.09 by Brian : 0x00050505 ==> 0x00050F05
+
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0xF2);
+
+        // According the Rory's suggestion to solve the middle range issue.
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R86, &BbpValue);
+		ASSERT((BbpValue == 0x00));
+		if ((BbpValue != 0x00))
+		{
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0x00);
+		}
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R91, &BbpValue);
+		ASSERT((BbpValue == 0x04));
+
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R92, &BbpValue);
+		ASSERT((BbpValue == 0x00));
+
+		// 5.5GHz band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x02);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R.
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+	    }
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+		}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+
+    // R66 should be set according to Channel and use 20MHz when scanning
+	if (Channel <= 14)
+	{
+		// BG band
+		R66 = 0x2E + GET_LNA_GAIN(pAd);
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+	}
+	else
+	{
+		// 5.5 GHz band
+		if (pAd->ate.TxWI.BW == BW_20)
+		{
+			R66 = (UCHAR)(0x32 + (GET_LNA_GAIN(pAd)*5)/3);
+    		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+		else
+		{
+			R66 = (UCHAR)(0x3A + (GET_LNA_GAIN(pAd)*5)/3);
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+	}
+
+	//
+	// On 11A, We should delay and wait RF/BBP to be stable
+	// and the appropriate time should be 1000 micro seconds
+	// 2005/06/05 - On 11G, We also need this delay time. Otherwise it's difficult to pass the WHQL.
+	//
+	RTMPusecDelay(1000);
+
+	if (Channel > 14)
+	{
+		// When 5.5GHz band the LSB of TxPwr will be used to reduced 7dB or not.
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, %dT) to , R1=0x%08lx, R2=0x%08lx, R3=0x%08lx, R4=0x%08lx\n",
+								  Channel,
+								  pAd->RfIcType,
+								  pAd->Antenna.field.TxPath,
+								  pAd->LatchRfRegs.R1,
+								  pAd->LatchRfRegs.R2,
+								  pAd->LatchRfRegs.R3,
+								  pAd->LatchRfRegs.R4));
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, Pwr0=%u, Pwr1=%u, %dT) to , R1=0x%08lx, R2=0x%08lx, R3=0x%08lx, R4=0x%08lx\n",
+								  Channel,
+								  pAd->RfIcType,
+								  (R3 & 0x00003e00) >> 9,
+								  (R4 & 0x000007c0) >> 6,
+								  pAd->Antenna.field.TxPath,
+								  pAd->LatchRfRegs.R1,
+								  pAd->LatchRfRegs.R2,
+								  pAd->LatchRfRegs.R3,
+								  pAd->LatchRfRegs.R4));
+    }
+}
+
+//
+// In fact, no one will call this routine so far !
+//
+/*
+	==========================================================================
+	Description:
+		Gives CCK TX rate 2 more dB TX power.
+		This routine works only in ATE mode.
+
+		calculate desired Tx power in RF R3.Tx0~5,	should consider -
+		0. if current radio is a noisy environment (pAd->DrsCounters.fNoisyEnvironment)
+		1. TxPowerPercentage
+		2. auto calibration based on TSSI feedback
+		3. extra 2 db for CCK
+		4. -10 db upon very-short distance (AvgRSSI >= -40db) to AP
+
+	NOTE: Since this routine requires the value of (pAd->DrsCounters.fNoisyEnvironment),
+		it should be called AFTER MlmeDynamicTxRateSwitching()
+	==========================================================================
+ */
+VOID ATEAsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT			i, j;
+	CHAR		DeltaPwr = 0;
+	BOOLEAN		bAutoTxAgc = FALSE;
+	UCHAR		TssiRef, *pTssiMinusBoundary, *pTssiPlusBoundary, TxAgcStep;
+	UCHAR		BbpR49 = 0, idx;
+	PCHAR		pTxAgcCompensate;
+	ULONG		TxPwr[5];
+	CHAR		Value;
+
+	/* no one calls this procedure so far */
+	if (pAd->ate.TxWI.BW == BW_40)
+	{
+		if (pAd->ate.Channel > 14)
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgGBand[4];
+		}
+	}
+	else
+	{
+		if (pAd->ate.Channel > 14)
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgGBand[4];
+		}
+	}
+
+	// TX power compensation for temperature variation based on TSSI.
+	// Do it per 4 seconds.
+	if (pAd->Mlme.OneSecPeriodicRound % 4 == 0)
+	{
+		if (pAd->ate.Channel <= 14)
+		{
+			/* bg channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			TssiRef            = pAd->TssiRefG;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryG[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryG[0];
+			TxAgcStep          = pAd->TxAgcStepG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			/* a channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			TssiRef            = pAd->TssiRefA;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryA[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryA[0];
+			TxAgcStep          = pAd->TxAgcStepA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+		{
+			/* BbpR49 is unsigned char */
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R49, &BbpR49);
+
+			/* (p) TssiPlusBoundaryG[0] = 0 = (m) TssiMinusBoundaryG[0] */
+			/* compensate: +4     +3   +2   +1    0   -1   -2   -3   -4 * steps */
+			/* step value is defined in pAd->TxAgcStepG for tx power value */
+
+			/* [4]+1+[4]   p4     p3   p2   p1   o1   m1   m2   m3   m4 */
+			/* ex:         0x00 0x15 0x25 0x45 0x88 0xA0 0xB5 0xD0 0xF0
+			   above value are examined in mass factory production */
+			/*             [4]    [3]  [2]  [1]  [0]  [1]  [2]  [3]  [4] */
+
+			/* plus is 0x10 ~ 0x40, minus is 0x60 ~ 0x90 */
+			/* if value is between p1 ~ o1 or o1 ~ s1, no need to adjust tx power */
+			/* if value is 0x65, tx power will be -= TxAgcStep*(2-1) */
+
+			if (BbpR49 > pTssiMinusBoundary[1])
+			{
+				// Reading is larger than the reference value.
+				// Check for how large we need to decrease the Tx power.
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 <= pTssiMinusBoundary[idx])  // Found the range
+						break;
+				}
+				// The index is the step we should decrease, idx = 0 means there is nothing to compensate
+//				if (R3 > (ULONG) (TxAgcStep * (idx-1)))
+					*pTxAgcCompensate = -(TxAgcStep * (idx-1));
+//				else
+//					*pTxAgcCompensate = -((UCHAR)R3);
+
+				DeltaPwr += (*pTxAgcCompensate);
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("-- Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = -%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else if (BbpR49 < pTssiPlusBoundary[1])
+			{
+				// Reading is smaller than the reference value
+				// check for how large we need to increase the Tx power
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 >= pTssiPlusBoundary[idx])   // Found the range
+						break;
+				}
+				// The index is the step we should increase, idx = 0 means there is nothing to compensate
+				*pTxAgcCompensate = TxAgcStep * (idx-1);
+				DeltaPwr += (*pTxAgcCompensate);
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("++ Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else
+			{
+				*pTxAgcCompensate = 0;
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("   Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, 0));
+			}
+		}
+	}
+	else
+	{
+		if (pAd->ate.Channel <= 14)
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+			DeltaPwr += (*pTxAgcCompensate);
+	}
+
+	/* calculate delta power based on the percentage specified from UI */
+	// E2PROM setting is calibrated for maximum TX power (i.e. 100%)
+	// We lower TX power here according to the percentage specified from UI
+	if (pAd->CommonCfg.TxPowerPercentage == 0xffffffff)       // AUTO TX POWER control
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 90)  // 91 ~ 100% & AUTO, treat as 100% in terms of mW
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 60)  // 61 ~ 90%, treat as 75% in terms of mW
+	{
+		DeltaPwr -= 1;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 30)  // 31 ~ 60%, treat as 50% in terms of mW
+	{
+		DeltaPwr -= 3;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 15)  // 16 ~ 30%, treat as 25% in terms of mW
+	{
+		DeltaPwr -= 6;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 9)   // 10 ~ 15%, treat as 12.5% in terms of mW
+	{
+		DeltaPwr -= 9;
+	}
+	else                                           // 0 ~ 9 %, treat as MIN(~3%) in terms of mW
+	{
+		DeltaPwr -= 12;
+	}
+
+	/* reset different new tx power for different TX rate */
+	for(i=0; i<5; i++)
+	{
+		if (TxPwr[i] != 0xffffffff)
+		{
+			for (j=0; j<8; j++)
+			{
+				Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F); /* 0 ~ 15 */
+
+				if ((Value + DeltaPwr) < 0)
+				{
+					Value = 0; /* min */
+				}
+				else if ((Value + DeltaPwr) > 0xF)
+				{
+					Value = 0xF; /* max */
+				}
+				else
+				{
+					Value += DeltaPwr; /* temperature compensation */
+				}
+
+				/* fill new value to CSR offset */
+				TxPwr[i] = (TxPwr[i] & ~(0x0000000F << j*4)) | (Value << j*4);
+			}
+
+			/* write tx power value to CSR */
+			/* TX_PWR_CFG_0 (8 tx rate) for	TX power for OFDM 12M/18M
+											TX power for OFDM 6M/9M
+											TX power for CCK5.5M/11M
+											TX power for CCK1M/2M */
+			/* TX_PWR_CFG_1 ~ TX_PWR_CFG_4 */
+			RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, TxPwr[i]);
+
+
+		}
+	}
+
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Write TxWI for ATE mode.
+
+	Return Value:
+		None
+	========================================================================
+*/
+
+#ifdef RT2870
+static VOID ATEWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC 	pTxWI,
+	IN	BOOLEAN			FRAG,
+	IN	BOOLEAN			InsTimestamp,
+	IN	BOOLEAN 		AMPDU,
+	IN	BOOLEAN 		Ack,
+	IN	BOOLEAN 		NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN	UCHAR 			PID,
+	IN	UCHAR			MIMOps,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	Transmit)
+{
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+	pTxWI->FRAG= FRAG;
+	pTxWI->TS= InsTimestamp;
+	pTxWI->AMPDU = AMPDU;
+
+	pTxWI->MIMOps = PWR_ACTIVE;
+	pTxWI->MpduDensity = 4;
+	pTxWI->ACK = Ack;
+	pTxWI->txop = Txopmode;
+	pTxWI->NSEQ = NSeq;
+	pTxWI->BAWinSize = BASize;
+
+	pTxWI->WirelessCliID = WCID;
+	pTxWI->MPDUtotalByteCount = Length;
+	pTxWI->PacketId = PID;
+
+	pTxWI->BW = Transmit.field.BW;
+	pTxWI->ShortGI = Transmit.field.ShortGI;
+	pTxWI->STBC= Transmit.field.STBC;
+
+	pTxWI->MCS = Transmit.field.MCS;
+	pTxWI->PHYMODE= Transmit.field.MODE;
+
+#ifdef DOT11_N_SUPPORT
+	//
+	// MMPS is 802.11n features. Because TxWI->MCS > 7 must be HT mode,
+	// so need not check if it's HT rate.
+	//
+	if ((MIMOps == MMPS_STATIC) && (pTxWI->MCS > 7))
+		pTxWI->MCS = 7;
+
+	if ((MIMOps == MMPS_DYNAMIC) && (pTxWI->MCS > 7)) // SMPS protect 2 spatial.
+		pTxWI->MIMOps = 1;
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->CFACK = CfAck;
+
+	return;
+}
+#endif // RT2870 //
+/*
+	========================================================================
+
+	Routine Description:
+		Disable protection for ATE.
+	========================================================================
+*/
+VOID ATEDisableAsicProtect(
+	IN		PRTMP_ADAPTER	pAd)
+{
+	PROT_CFG_STRUC	ProtCfg, ProtCfg4;
+	UINT32 Protect[6];
+	USHORT			offset;
+	UCHAR			i;
+	UINT32 MacReg = 0;
+
+	// Config ASIC RTS threshold register
+	RTMP_IO_READ32(pAd, TX_RTS_CFG, &MacReg);
+	MacReg &= 0xFF0000FF;
+	MacReg |= (pAd->CommonCfg.RtsThreshold << 8);
+	RTMP_IO_WRITE32(pAd, TX_RTS_CFG, MacReg);
+
+	// Initial common protection settings
+	RTMPZeroMemory(Protect, sizeof(Protect));
+	ProtCfg4.word = 0;
+	ProtCfg.word = 0;
+	ProtCfg.field.TxopAllowGF40 = 1;
+	ProtCfg.field.TxopAllowGF20 = 1;
+	ProtCfg.field.TxopAllowMM40 = 1;
+	ProtCfg.field.TxopAllowMM20 = 1;
+	ProtCfg.field.TxopAllowOfdm = 1;
+	ProtCfg.field.TxopAllowCck = 1;
+	ProtCfg.field.RTSThEn = 1;
+	ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+
+	// Handle legacy(B/G) protection
+	ProtCfg.field.ProtectRate = pAd->CommonCfg.RtsRate;
+	ProtCfg.field.ProtectCtrl = 0;
+	Protect[0] = ProtCfg.word;
+	Protect[1] = ProtCfg.word;
+
+	// NO PROTECT
+	// 1.All STAs in the BSS are 20/40 MHz HT
+	// 2. in ai 20/40MHz BSS
+	// 3. all STAs are 20MHz in a 20MHz BSS
+	// Pure HT. no protection.
+
+	// MM20_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 010111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+	Protect[2] = 0x01744004;
+
+	// MM40_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 111111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+	Protect[3] = 0x03f44084;
+
+	// CF20_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 010111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+	Protect[4] = 0x01744004;
+
+	// CF40_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 111111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+	Protect[5] = 0x03f44084;
+
+	pAd->CommonCfg.IOTestParm.bRTSLongProtOn = FALSE;
+
+	offset = CCK_PROT_CFG;
+	for (i = 0;i < 6;i++)
+		RTMP_IO_WRITE32(pAd, offset + i*4, Protect[i]);
+
+}
+
+#ifdef RT2870
+/*
+	========================================================================
+	Routine	Description:
+		Write TxInfo for ATE mode.
+
+	Return Value:
+		None
+	========================================================================
+*/
+static VOID ATEWriteTxInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXINFO_STRUC 	pTxInfo,
+	IN	USHORT		USBDMApktLen,
+	IN	BOOLEAN		bWiv,
+	IN	UCHAR			QueueSel,
+	IN	UCHAR			NextValid,
+	IN	UCHAR			TxBurst)
+{
+	pTxInfo->USBDMATxPktLen = USBDMApktLen;
+	pTxInfo->QSEL = QueueSel;
+
+	if (QueueSel != FIFO_EDCA)
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("=======> QueueSel != FIFO_EDCA<=======\n"));
+
+	pTxInfo->USBDMANextVLD = NextValid;
+	pTxInfo->USBDMATxburst = TxBurst;
+	pTxInfo->WIV = bWiv;
+	pTxInfo->SwUseLastRound = 0;
+	pTxInfo->rsv = 0;
+	pTxInfo->rsv2 = 0;
+
+	return;
+}
+#endif // RT2870 //
+
+/* There are two ways to convert Rssi */
+#if 1
+//
+// The way used with GET_LNA_GAIN().
+//
+CHAR ATEConvertToRssi(
+	IN PRTMP_ADAPTER pAd,
+	IN	CHAR	Rssi,
+	IN  UCHAR   RssiNumber)
+{
+	UCHAR	RssiOffset, LNAGain;
+
+	// Rssi equals to zero should be an invalid value
+	if (Rssi == 0)
+		return -99;
+
+	LNAGain = GET_LNA_GAIN(pAd);
+	if (pAd->LatchRfRegs.Channel > 14)
+	{
+		if (RssiNumber == 0)
+			RssiOffset = pAd->ARssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->ARssiOffset1;
+		else
+			RssiOffset = pAd->ARssiOffset2;
+	}
+	else
+	{
+		if (RssiNumber == 0)
+			RssiOffset = pAd->BGRssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->BGRssiOffset1;
+		else
+			RssiOffset = pAd->BGRssiOffset2;
+	}
+
+	return (-12 - RssiOffset - LNAGain - Rssi);
+}
+#else
+//
+// The way originally used in ATE of rt2860ap.
+//
+CHAR ATEConvertToRssi(
+	IN PRTMP_ADAPTER pAd,
+	IN	CHAR			Rssi,
+	IN  UCHAR   RssiNumber)
+{
+	UCHAR	RssiOffset, LNAGain;
+
+	// Rssi equals to zero should be an invalid value
+	if (Rssi == 0)
+		return -99;
+
+    if (pAd->LatchRfRegs.Channel > 14)
+    {
+        LNAGain = pAd->ALNAGain;
+        if (RssiNumber == 0)
+			RssiOffset = pAd->ARssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->ARssiOffset1;
+		else
+			RssiOffset = pAd->ARssiOffset2;
+    }
+    else
+    {
+        LNAGain = pAd->BLNAGain;
+        if (RssiNumber == 0)
+			RssiOffset = pAd->BGRssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->BGRssiOffset1;
+		else
+			RssiOffset = pAd->BGRssiOffset2;
+    }
+
+    return (-32 - RssiOffset + LNAGain - Rssi);
+}
+#endif /* end of #if 1 */
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set Japan filter coefficients if needed.
+	Note:
+		This routine should only be called when
+		entering TXFRAME mode or TXCONT mode.
+
+	========================================================================
+*/
+static VOID SetJapanFilter(
+	IN		PRTMP_ADAPTER	pAd)
+{
+	UCHAR			BbpData = 0;
+
+	//
+	// If Channel=14 and Bandwidth=20M and Mode=CCK, set BBP R4 bit5=1
+	// (Japan Tx filter coefficients)when (TXFRAME or TXCONT).
+	//
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BbpData);
+
+    if ((pAd->ate.TxWI.PHYMODE == MODE_CCK) && (pAd->ate.Channel == 14) && (pAd->ate.TxWI.BW == BW_20))
+    {
+        BbpData |= 0x20;    // turn on
+        ATEDBGPRINT(RT_DEBUG_TRACE, ("SetJapanFilter!!!\n"));
+    }
+    else
+    {
+		BbpData &= 0xdf;    // turn off
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ClearJapanFilter!!!\n"));
+    }
+
+	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BbpData);
+}
+
+VOID ATESampleRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN PRXWI_STRUC		pRxWI)
+{
+	/* There are two ways to collect RSSI. */
+#if 1
+	//pAd->LastRxRate = (USHORT)((pRxWI->MCS) + (pRxWI->BW <<7) + (pRxWI->ShortGI <<8)+ (pRxWI->PHYMODE <<14)) ;
+	if (pRxWI->RSSI0 != 0)
+	{
+		pAd->ate.LastRssi0	= ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI0, RSSI_0);
+		pAd->ate.AvgRssi0X8	= (pAd->ate.AvgRssi0X8 - pAd->ate.AvgRssi0) + pAd->ate.LastRssi0;
+		pAd->ate.AvgRssi0  	= pAd->ate.AvgRssi0X8 >> 3;
+	}
+	if (pRxWI->RSSI1 != 0)
+	{
+		pAd->ate.LastRssi1	= ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI1, RSSI_1);
+		pAd->ate.AvgRssi1X8	= (pAd->ate.AvgRssi1X8 - pAd->ate.AvgRssi1) + pAd->ate.LastRssi1;
+		pAd->ate.AvgRssi1	= pAd->ate.AvgRssi1X8 >> 3;
+	}
+	if (pRxWI->RSSI2 != 0)
+	{
+		pAd->ate.LastRssi2	= ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI2, RSSI_2);
+		pAd->ate.AvgRssi2X8	= (pAd->ate.AvgRssi2X8 - pAd->ate.AvgRssi2) + pAd->ate.LastRssi2;
+		pAd->ate.AvgRssi2	= pAd->ate.AvgRssi2X8 >> 3;
+	}
+
+	pAd->ate.LastSNR0 = (CHAR)(pRxWI->SNR0);// CHAR ==> UCHAR ?
+	pAd->ate.LastSNR1 = (CHAR)(pRxWI->SNR1);// CHAR ==> UCHAR ?
+
+	pAd->ate.NumOfAvgRssiSample ++;
+#else
+	pAd->ate.LastSNR0 = (CHAR)(pRxWI->SNR0);
+	pAd->ate.LastSNR1 = (CHAR)(pRxWI->SNR1);
+	pAd->ate.RxCntPerSec++;
+	pAd->ate.LastRssi0 = ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI0, RSSI_0);
+	pAd->ate.LastRssi1 = ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI1, RSSI_1);
+	pAd->ate.LastRssi2 = ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI2, RSSI_2);
+	pAd->ate.AvgRssi0X8 = (pAd->ate.AvgRssi0X8 - pAd->ate.AvgRssi0) + pAd->ate.LastRssi0;
+	pAd->ate.AvgRssi0 = pAd->ate.AvgRssi0X8 >> 3;
+	pAd->ate.AvgRssi1X8 = (pAd->ate.AvgRssi1X8 - pAd->ate.AvgRssi1) + pAd->ate.LastRssi1;
+	pAd->ate.AvgRssi1 = pAd->ate.AvgRssi1X8 >> 3;
+	pAd->ate.AvgRssi2X8 = (pAd->ate.AvgRssi2X8 - pAd->ate.AvgRssi2) + pAd->ate.LastRssi2;
+	pAd->ate.AvgRssi2 = pAd->ate.AvgRssi2X8 >> 3;
+	pAd->ate.NumOfAvgRssiSample ++;
+#endif
+}
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPStationStop(
+    IN  PRTMP_ADAPTER   pAd)
+{
+//	BOOLEAN       Cancelled;
+
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("==> RTMPStationStop\n"));
+
+#if 0
+	RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,      &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,    &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,   &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,       &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,     &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,       &Cancelled);
+#endif
+	// For rx statistics, we need to keep this timer running.
+//	RTMPCancelTimer(&pAd->Mlme.PeriodicTimer,      &Cancelled);
+
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("<== RTMPStationStop\n"));
+}
+
+VOID RTMPStationStart(
+    IN  PRTMP_ADAPTER   pAd)
+{
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("==> RTMPStationStart\n"));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("<== RTMPStationStart\n"));
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	==========================================================================
+	Description:
+		Setup Frame format.
+	NOTE:
+		This routine should only be used in ATE mode.
+	==========================================================================
+ */
+
+#ifdef RT2870
+/*======================Start of RT2870======================*/
+/*                                                           */
+/*                                                           */
+static INT ATESetUpFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT32 TxIdx)
+{
+	UINT j;
+	PTX_CONTEXT	pNullContext;
+	PUCHAR			pDest;
+	HTTRANSMIT_SETTING	TxHTPhyMode;
+	PTXWI_STRUC		pTxWI;
+	PTXINFO_STRUC		pTxInfo;
+	UINT32			TransferBufferLength, OrgBufferLength = 0;
+	UCHAR			padLen = 0;
+#ifdef RALINK_28xx_QA
+	PHEADER_802_11	pHeader80211 = NULL;
+#endif // RALINK_28xx_QA //
+
+	if ((RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) ||
+		(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)) ||
+		(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) ||
+		(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		return -1;
+	}
+
+	/* We always use QID_AC_BE and FIFO_EDCA in ATE mode. */
+
+	pNullContext = &(pAd->NullContext);
+	ASSERT(pNullContext != NULL);
+
+	if (pNullContext->InUse == FALSE)
+	{
+		// Set the in use bit
+		pNullContext->InUse = TRUE;
+		NdisZeroMemory(&(pAd->NullFrame), sizeof(HEADER_802_11));
+
+		// Fill 802.11 header.
+#ifdef RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			pHeader80211 = NdisMoveMemory(&(pAd->NullFrame), pAd->ate.Header, pAd->ate.HLen);
+//			pDest = NdisMoveMemory(&(pAd->NullFrame), pAd->ate.Header, pAd->ate.HLen);
+//			pHeader80211 = (PHEADER_802_11)pDest;
+		}
+		else
+#endif // RALINK_28xx_QA //
+		{
+			// Fill 802.11 header.
+			NdisMoveMemory(&(pAd->NullFrame), TemplateFrame, sizeof(HEADER_802_11));
+		}
+#ifdef RT_BIG_ENDIAN
+		RTMPFrameEndianChange(pAd, (PUCHAR)&(pAd->NullFrame), DIR_READ, FALSE);
+#endif // RT_BIG_ENDIAN //
+
+#ifdef RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			/* modify sequence number.... */
+			if (pAd->ate.TxDoneCount == 0)
+			{
+				pAd->ate.seq = pHeader80211->Sequence;
+			}
+			else
+			{
+				pHeader80211->Sequence = ++pAd->ate.seq;
+			}
+			/* We already got all the addr. fields from QA GUI. */
+		}
+		else
+#endif // RALINK_28xx_QA //
+		{
+			COPY_MAC_ADDR(pAd->NullFrame.Addr1, pAd->ate.Addr1);
+			COPY_MAC_ADDR(pAd->NullFrame.Addr2, pAd->ate.Addr2);
+			COPY_MAC_ADDR(pAd->NullFrame.Addr3, pAd->ate.Addr3);
+		}
+
+		RTMPZeroMemory(&pAd->NullContext.TransferBuffer->field.WirelessPacket[0], TX_BUFFER_NORMSIZE);//???
+		pTxInfo = (PTXINFO_STRUC)&pAd->NullContext.TransferBuffer->field.WirelessPacket[0];
+
+#ifdef RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			// Avoid to exceed the range of WirelessPacket[].
+			ASSERT(pAd->ate.TxInfo.USBDMATxPktLen <= (MAX_FRAME_SIZE - 34/* == 2312 */));
+			NdisMoveMemory(pTxInfo, &(pAd->ate.TxInfo), sizeof(pAd->ate.TxInfo));
+		}
+		else
+#endif // RALINK_28xx_QA //
+		{
+			// Avoid to exceed the range of WirelessPacket[].
+			ASSERT(pAd->ate.TxLength <= (MAX_FRAME_SIZE - 34/* == 2312 */));
+
+			// pTxInfo->USBDMATxPktLen will be updated to include padding later.
+			ATEWriteTxInfo(pAd, pTxInfo, (USHORT)(TXWI_SIZE + pAd->ate.TxLength), TRUE, EpToQueue[MGMTPIPEIDX], FALSE,  FALSE);
+			pTxInfo->QSEL = FIFO_EDCA;
+		}
+
+		pTxWI = (PTXWI_STRUC)&pAd->NullContext.TransferBuffer->field.WirelessPacket[TXINFO_SIZE];
+
+		// Fill TxWI.
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			TxHTPhyMode.field.BW = pAd->ate.TxWI.BW;
+			TxHTPhyMode.field.ShortGI = pAd->ate.TxWI.ShortGI;
+			TxHTPhyMode.field.STBC = pAd->ate.TxWI.STBC;
+			TxHTPhyMode.field.MCS = pAd->ate.TxWI.MCS;
+			TxHTPhyMode.field.MODE = pAd->ate.TxWI.PHYMODE;
+			ATEWriteTxWI(pAd, pTxWI, pAd->ate.TxWI.FRAG, pAd->ate.TxWI.TS, pAd->ate.TxWI.AMPDU, pAd->ate.TxWI.ACK, pAd->ate.TxWI.NSEQ,
+				pAd->ate.TxWI.BAWinSize, BSSID_WCID, pAd->ate.TxWI.MPDUtotalByteCount/* include 802.11 header */, pAd->ate.TxWI.PacketId, 0, pAd->ate.TxWI.txop/*IFS_HTTXOP*/, pAd->ate.TxWI.CFACK/*FALSE*/, TxHTPhyMode);
+		}
+		else
+		{
+			TxHTPhyMode.field.BW = pAd->ate.TxWI.BW;
+			TxHTPhyMode.field.ShortGI = pAd->ate.TxWI.ShortGI;
+			TxHTPhyMode.field.STBC = 0;
+			TxHTPhyMode.field.MCS = pAd->ate.TxWI.MCS;
+			TxHTPhyMode.field.MODE = pAd->ate.TxWI.PHYMODE;
+
+			ATEWriteTxWI(pAd, pTxWI,  FALSE, FALSE, FALSE, FALSE/* No ack required. */, FALSE, 0, BSSID_WCID, pAd->ate.TxLength,
+				0, 0, IFS_HTTXOP, FALSE, TxHTPhyMode);// "MMPS_STATIC" instead of "MMPS_DYNAMIC" ???
+		}
+
+		RTMPMoveMemory(&pAd->NullContext.TransferBuffer->field.WirelessPacket[TXINFO_SIZE+TXWI_SIZE], &pAd->NullFrame, sizeof(HEADER_802_11));
+
+		pDest = &(pAd->NullContext.TransferBuffer->field.WirelessPacket[TXINFO_SIZE+TXWI_SIZE+sizeof(HEADER_802_11)]);
+
+		// Prepare frame payload
+#ifdef RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			// copy pattern
+			if ((pAd->ate.PLen != 0))
+			{
+				for (j = 0; j < pAd->ate.DLen; j+=pAd->ate.PLen)
+				{
+					RTMPMoveMemory(pDest, pAd->ate.Pattern, pAd->ate.PLen);
+					pDest += pAd->ate.PLen;
+				}
+			}
+			TransferBufferLength = TXINFO_SIZE + TXWI_SIZE + pAd->ate.TxWI.MPDUtotalByteCount;
+		}
+		else
+#endif // RALINK_28xx_QA //
+		{
+		    for (j = 0; j < (pAd->ate.TxLength - sizeof(HEADER_802_11)); j++)
+		    {
+				*pDest = 0xA5;
+				pDest += 1;
+		    }
+			TransferBufferLength = TXINFO_SIZE + TXWI_SIZE + pAd->ate.TxLength;
+		}
+
+#if 1
+		OrgBufferLength = TransferBufferLength;
+		TransferBufferLength = (TransferBufferLength + 3) & (~3);
+
+		// Always add 4 extra bytes at every packet.
+		padLen = TransferBufferLength - OrgBufferLength + 4;/* 4 == last packet padding */
+		ASSERT((padLen <= (RTMP_PKT_TAIL_PADDING - 4/* 4 == MaxBulkOutsize alignment padding */)));
+
+		/* Now memzero all extra padding bytes. */
+		NdisZeroMemory(pDest, padLen);
+		pDest += padLen;
+#else
+		if ((TransferBufferLength % 4) == 1)
+		{
+			NdisZeroMemory(pDest, 7);
+			pDest += 7;
+			TransferBufferLength  += 3;
+		}
+		else if ((TransferBufferLength % 4) == 2)
+		{
+			NdisZeroMemory(pDest, 6);
+			pDest += 6;
+			TransferBufferLength  += 2;
+		}
+		else if ((TransferBufferLength % 4) == 3)
+		{
+			NdisZeroMemory(pDest, 5);
+			pDest += 5;
+			TransferBufferLength  += 1;
+		}
+#endif // 1 //
+
+		// Update pTxInfo->USBDMATxPktLen to include padding.
+		pTxInfo->USBDMATxPktLen = TransferBufferLength - TXINFO_SIZE;
+
+		TransferBufferLength += 4;
+
+		// If TransferBufferLength is multiple of 64, add extra 4 bytes again.
+		if ((TransferBufferLength % pAd->BulkOutMaxPacketSize) == 0)
+		{
+			NdisZeroMemory(pDest, 4);
+			TransferBufferLength += 4;
+		}
+
+		// Fill out frame length information for global Bulk out arbitor
+		pAd->NullContext.BulkOutSize = TransferBufferLength;
+	}
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+	RTMPFrameEndianChange(pAd, (((PUCHAR)pTxInfo)+TXWI_SIZE+TXINFO_SIZE), DIR_WRITE, FALSE);
+    RTMPDescriptorEndianChange((PUCHAR)pTxInfo, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+	return 0;
+}
+
+VOID ATE_RTUSBBulkOutDataPacketComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+	PRTMP_ADAPTER		pAd;
+	PTX_CONTEXT		    pNullContext;
+	UCHAR				BulkOutPipeId;
+	NTSTATUS			Status;
+	unsigned long		IrqFlags;
+	ULONG			    OldValue;
+
+	pNullContext = (PTX_CONTEXT)pUrb->context;
+	pAd = pNullContext->pAd;
+
+
+	// Reset Null frame context flags
+	pNullContext->IRPPending = FALSE;
+	pNullContext->InUse = FALSE;
+	Status = pUrb->status;
+
+	// Store BulkOut PipeId
+	BulkOutPipeId = pNullContext->BulkOutPipeId;
+	pAd->BulkOutDataOneSecCount++;
+
+	if (Status == USB_ST_NOERROR)
+	{
+#ifdef RALINK_28xx_QA
+		if ((ATE_ON(pAd)) && (pAd->ate.bQATxStart == TRUE))
+		{
+			if (pAd->ate.QID == BulkOutPipeId)
+			{
+				// Let Rx can have a chance to break in during Tx process,
+				// especially for loopback mode in QA ATE.
+				// To trade off between tx performance and loopback mode integrity.
+				/* Q   : Now Rx is handled by tasklet, do we still need this delay ? */
+				/* Ans : Even tasklet is used, Rx/Tx < 1 if we do not delay for a while right here. */
+				RTMPusecDelay(500);
+				pAd->ate.TxDoneCount++;
+				pAd->RalinkCounters.KickTxCount++;
+				ASSERT(pAd->ate.QID == 0);
+				pAd->ate.TxAc0++;
+			}
+		}
+#endif // RALINK_28xx_QA //
+		pAd->BulkOutComplete++;
+
+		pAd->Counters8023.GoodTransmits++;
+
+		/* Don't worry about the queue is empty or not. This function will check itself. */
+		RTMPDeQueuePacket(pAd, TRUE, BulkOutPipeId, MAX_TX_PROCESS);
+
+		/* In 28xx, SendTxWaitQueue ==> TxSwQueue  */
+/*
+		if (pAd->SendTxWaitQueue[BulkOutPipeId].Number > 0)
+		{
+			RTMPDeQueuePacket(pAd, BulkOutPipeId);
+		}
+*/
+	}
+	else	// STATUS_OTHER
+	{
+		pAd->BulkOutCompleteOther++;
+
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("BulkOutDataPacket Failed STATUS_OTHER = 0x%x . \n", Status));
+		ATEDBGPRINT(RT_DEBUG_ERROR, (">>BulkOutReq=0x%lx, BulkOutComplete=0x%lx\n", pAd->BulkOutReq, pAd->BulkOutComplete));
+
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			/* In 28xx, RT_OID_USB_RESET_BULK_OUT ==> CMDTHREAD_RESET_BULK_OUT */
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+			// Check
+			BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+			pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+			pAd->bulkResetPipeid = BulkOutPipeId;
+			BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+			return;
+		}
+	}
+
+
+
+	if (atomic_read(&pAd->BulkOutRemained) > 0)
+	{
+		atomic_dec(&pAd->BulkOutRemained);
+	}
+
+	// 1st - Transmit Success
+	OldValue = pAd->WlanCounters.TransmittedFragmentCount.u.LowPart;
+	pAd->WlanCounters.TransmittedFragmentCount.u.LowPart++;
+
+	if (pAd->WlanCounters.TransmittedFragmentCount.u.LowPart < OldValue)
+	{
+		pAd->WlanCounters.TransmittedFragmentCount.u.HighPart++;
+	}
+
+	if(((pAd->ContinBulkOut == TRUE ) ||(atomic_read(&pAd->BulkOutRemained) > 0)) && (pAd->ate.Mode & ATE_TXFRAME))
+	{
+		RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE);
+	}
+	else
+	{
+		RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE);
+#ifdef RALINK_28xx_QA
+		pAd->ate.TxStatus = 0;
+#endif // RALINK_28xx_QA //
+	}
+
+	BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+	BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protection of rest bulk should be in BulkOut routine.
+	RTUSBKickBulkOut(pAd);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	ATE_RTUSBBulkOutDataPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BulkOutPipeId)
+{
+	PTX_CONTEXT		pNullContext = &(pAd->NullContext);
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+
+	ASSERT(BulkOutPipeId == 0);
+
+	/* Build up the frame first. */
+//	ATESetUpFrame(pAd, 0);
+
+	BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+	if (pAd->BulkOutPending[BulkOutPipeId] == TRUE)
+	{
+		BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		return;
+	}
+
+	pAd->BulkOutPending[BulkOutPipeId] = TRUE;
+	BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+	// Increase Total transmit byte counter
+	pAd->RalinkCounters.OneSecTransmittedByteCount +=  pNullContext->BulkOutSize;
+	pAd->RalinkCounters.TransmittedByteCount +=  pNullContext->BulkOutSize;
+
+	// Clear ATE frame bulk out flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE);
+
+	// Init Tx context descriptor
+	pNullContext->IRPPending = TRUE;
+	RTUSBInitTxDesc(pAd, pNullContext, BulkOutPipeId, (usb_complete_t)ATE_RTUSBBulkOutDataPacketComplete);
+	pUrb = pNullContext->pUrb;
+
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("ATE_RTUSBBulkOutDataPacket: Submit Tx URB failed %d\n", ret));
+		return;
+	}
+
+	pAd->BulkOutReq++;
+	return;
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	ATE_RTUSBCancelPendingBulkInIRP(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PRX_CONTEXT		pRxContext;
+	UINT			i;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("--->ATE_RTUSBCancelPendingBulkInIRP\n"));
+#if 1
+	for ( i = 0; i < (RX_RING_SIZE); i++)
+	{
+		pRxContext = &(pAd->RxContext[i]);
+		if(pRxContext->IRPPending == TRUE)
+		{
+			RTUSB_UNLINK_URB(pRxContext->pUrb);
+			pRxContext->IRPPending = FALSE;
+			pRxContext->InUse = FALSE;
+			//NdisInterlockedDecrement(&pAd->PendingRx);
+			//pAd->PendingRx--;
+		}
+	}
+#else
+	for ( i = 0; i < (RX_RING_SIZE); i++)
+	{
+		pRxContext = &(pAd->RxContext[i]);
+		if(atomic_read(&pRxContext->IrpLock) == IRPLOCK_CANCELABLE)
+		{
+			RTUSB_UNLINK_URB(pRxContext->pUrb);
+		}
+		InterlockedExchange(&pRxContext->IrpLock, IRPLOCK_CANCE_START);
+	}
+#endif // 1 //
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("<---ATE_RTUSBCancelPendingBulkInIRP\n"));
+	return;
+}
+#endif // RT2870 //
+
+VOID rt_ee_read_all(PRTMP_ADAPTER pAd, USHORT *Data)
+{
+	USHORT i;
+	USHORT value;
+
+	for (i = 0 ; i < EEPROM_SIZE/2 ; )
+	{
+		/* "value" is expecially for some compilers... */
+		RT28xx_EEPROM_READ16(pAd, i*2, value);
+		Data[i] = value;
+		i++;
+	}
+}
+
+VOID rt_ee_write_all(PRTMP_ADAPTER pAd, USHORT *Data)
+{
+	USHORT i;
+	USHORT value;
+
+	for (i = 0 ; i < EEPROM_SIZE/2 ; )
+	{
+		/* "value" is expecially for some compilers... */
+		value = Data[i];
+		RT28xx_EEPROM_WRITE16(pAd, i*2, value);
+		i ++;
+	}
+}
+#ifdef RALINK_28xx_QA
+VOID ATE_QA_Statistics(
+	IN PRTMP_ADAPTER			pAd,
+	IN PRXWI_STRUC				pRxWI,
+	IN PRT28XX_RXD_STRUC		pRxD,
+	IN PHEADER_802_11			pHeader)
+{
+	// update counter first
+	if (pHeader != NULL)
+	{
+		if (pHeader->FC.Type == BTYPE_DATA)
+		{
+			if (pRxD->U2M)
+				pAd->ate.U2M++;
+			else
+				pAd->ate.OtherData++;
+		}
+		else if (pHeader->FC.Type == BTYPE_MGMT)
+		{
+			if (pHeader->FC.SubType == SUBTYPE_BEACON)
+				pAd->ate.Beacon++;
+			else
+				pAd->ate.OtherCount++;
+		}
+		else if (pHeader->FC.Type == BTYPE_CNTL)
+		{
+			pAd->ate.OtherCount++;
+		}
+	}
+	pAd->ate.RSSI0 = pRxWI->RSSI0;
+	pAd->ate.RSSI1 = pRxWI->RSSI1;
+	pAd->ate.RSSI2 = pRxWI->RSSI2;
+	pAd->ate.SNR0 = pRxWI->SNR0;
+	pAd->ate.SNR1 = pRxWI->SNR1;
+}
+
+/* command id with Cmd Type == 0x0008(for 28xx)/0x0005(for iNIC) */
+#define RACFG_CMD_RF_WRITE_ALL		0x0000
+#define RACFG_CMD_E2PROM_READ16		0x0001
+#define RACFG_CMD_E2PROM_WRITE16	0x0002
+#define RACFG_CMD_E2PROM_READ_ALL	0x0003
+#define RACFG_CMD_E2PROM_WRITE_ALL	0x0004
+#define RACFG_CMD_IO_READ			0x0005
+#define RACFG_CMD_IO_WRITE			0x0006
+#define RACFG_CMD_IO_READ_BULK		0x0007
+#define RACFG_CMD_BBP_READ8			0x0008
+#define RACFG_CMD_BBP_WRITE8		0x0009
+#define RACFG_CMD_BBP_READ_ALL		0x000a
+#define RACFG_CMD_GET_COUNTER		0x000b
+#define RACFG_CMD_CLEAR_COUNTER		0x000c
+
+#define RACFG_CMD_RSV1				0x000d
+#define RACFG_CMD_RSV2				0x000e
+#define RACFG_CMD_RSV3				0x000f
+
+#define RACFG_CMD_TX_START			0x0010
+#define RACFG_CMD_GET_TX_STATUS		0x0011
+#define RACFG_CMD_TX_STOP			0x0012
+#define RACFG_CMD_RX_START			0x0013
+#define RACFG_CMD_RX_STOP			0x0014
+#define RACFG_CMD_GET_NOISE_LEVEL	0x0015
+
+#define RACFG_CMD_ATE_START			0x0080
+#define RACFG_CMD_ATE_STOP			0x0081
+
+#define RACFG_CMD_ATE_START_TX_CARRIER		0x0100
+#define RACFG_CMD_ATE_START_TX_CONT			0x0101
+#define RACFG_CMD_ATE_START_TX_FRAME		0x0102
+#define RACFG_CMD_ATE_SET_BW	            0x0103
+#define RACFG_CMD_ATE_SET_TX_POWER0	        0x0104
+#define RACFG_CMD_ATE_SET_TX_POWER1			0x0105
+#define RACFG_CMD_ATE_SET_FREQ_OFFSET		0x0106
+#define RACFG_CMD_ATE_GET_STATISTICS		0x0107
+#define RACFG_CMD_ATE_RESET_COUNTER			0x0108
+#define RACFG_CMD_ATE_SEL_TX_ANTENNA		0x0109
+#define RACFG_CMD_ATE_SEL_RX_ANTENNA		0x010a
+#define RACFG_CMD_ATE_SET_PREAMBLE			0x010b
+#define RACFG_CMD_ATE_SET_CHANNEL			0x010c
+#define RACFG_CMD_ATE_SET_ADDR1				0x010d
+#define RACFG_CMD_ATE_SET_ADDR2				0x010e
+#define RACFG_CMD_ATE_SET_ADDR3				0x010f
+#define RACFG_CMD_ATE_SET_RATE				0x0110
+#define RACFG_CMD_ATE_SET_TX_FRAME_LEN		0x0111
+#define RACFG_CMD_ATE_SET_TX_FRAME_COUNT	0x0112
+#define RACFG_CMD_ATE_START_RX_FRAME		0x0113
+#define RACFG_CMD_ATE_E2PROM_READ_BULK	0x0114
+#define RACFG_CMD_ATE_E2PROM_WRITE_BULK	0x0115
+#define RACFG_CMD_ATE_IO_WRITE_BULK		0x0116
+#define RACFG_CMD_ATE_BBP_READ_BULK		0x0117
+#define RACFG_CMD_ATE_BBP_WRITE_BULK	0x0118
+#define RACFG_CMD_ATE_RF_READ_BULK		0x0119
+#define RACFG_CMD_ATE_RF_WRITE_BULK		0x011a
+
+
+
+#define A2Hex(_X, _p) 				\
+{									\
+	UCHAR *p;						\
+	_X = 0;							\
+	p = _p;							\
+	while (((*p >= 'a') && (*p <= 'f')) || ((*p >= 'A') && (*p <= 'F')) || ((*p >= '0') && (*p <= '9')))		\
+	{												\
+		if ((*p >= 'a') && (*p <= 'f'))				\
+			_X = _X * 16 + *p - 87;					\
+		else if ((*p >= 'A') && (*p <= 'F'))		\
+			_X = _X * 16 + *p - 55;					\
+		else if ((*p >= '0') && (*p <= '9'))		\
+			_X = _X * 16 + *p - 48;					\
+		p++;										\
+	}												\
+}
+
+
+static VOID memcpy_exl(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len);
+static VOID memcpy_exs(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len);
+static VOID RTMP_IO_READ_BULK(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, UINT32 len);
+
+#ifdef UCOS
+int ate_copy_to_user(
+	IN PUCHAR payload,
+	IN PUCHAR msg,
+	IN INT    len)
+{
+	memmove(payload, msg, len);
+	return 0;
+}
+
+#undef	copy_to_user
+#define copy_to_user(x,y,z) ate_copy_to_user((PUCHAR)x, (PUCHAR)y, z)
+#endif // UCOS //
+
+#define	LEN_OF_ARG 16
+
+VOID RtmpDoAte(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	unsigned short Command_Id;
+	struct ate_racfghdr *pRaCfg;
+	INT	Status = NDIS_STATUS_SUCCESS;
+
+
+
+	if((pRaCfg = kmalloc(sizeof(struct ate_racfghdr), GFP_KERNEL)) == NULL)
+	{
+		Status = -EINVAL;
+		return;
+	}
+
+	NdisZeroMemory(pRaCfg, sizeof(struct ate_racfghdr));
+
+    if (copy_from_user((PUCHAR)pRaCfg, wrq->u.data.pointer, wrq->u.data.length))
+	{
+		Status = -EFAULT;
+		kfree(pRaCfg);
+		return;
+	}
+
+
+	Command_Id = ntohs(pRaCfg->command_id);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE,("\n%s: Command_Id = 0x%04x !\n", __func__, Command_Id));
+
+	switch (Command_Id)
+	{
+ 		// We will get this command when QA starts.
+		case RACFG_CMD_ATE_START:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START\n"));
+
+				// prepare feedback as soon as we can to avoid QA timeout.
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_TRACE, ("copy_to_user() fail in case RACFG_CMD_ATE_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START is done !\n"));
+				}
+				Set_ATE_Proc(pAdapter, "ATESTART");
+			}
+			break;
+
+ 		// We will get this command either QA is closed or ated is killed by user.
+		case RACFG_CMD_ATE_STOP:
+			{
+#ifndef UCOS
+				INT32 ret;
+#endif // !UCOS //
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_STOP\n"));
+
+				// Distinguish this command came from QA(via ated)
+				// or ate daemon according to the existence of pid in payload.
+				// No need to prepare feedback if this cmd came directly from ate daemon.
+				pRaCfg->length = ntohs(pRaCfg->length);
+
+				if (pRaCfg->length == sizeof(pAdapter->ate.AtePid))
+				{
+					// This command came from QA.
+					// Get the pid of ATE daemon.
+					memcpy((UCHAR *)&pAdapter->ate.AtePid,
+						(&pRaCfg->data[0]) - 2/* == &(pRaCfg->status) */,
+						sizeof(pAdapter->ate.AtePid));
+
+					// prepare feedback as soon as we can to avoid QA timeout.
+					pRaCfg->length = htons(2);
+					pRaCfg->status = htons(0);
+
+					wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+										+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+										+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+	            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+	            	{
+	            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_STOP\n"));
+	                    Status = -EFAULT;
+	            	}
+
+					//
+					// kill ATE daemon when leaving ATE mode.
+					// We must kill ATE daemon first before setting ATESTOP,
+					// or Microsoft will report sth. wrong.
+#ifndef UCOS
+					ret = KILL_THREAD_PID(pAdapter->ate.AtePid, SIGTERM, 1);
+					if (ret)
+					{
+						ATEDBGPRINT(RT_DEBUG_ERROR, ("%s: unable to signal thread\n", pAdapter->net_dev->name));
+					}
+#endif // !UCOS //
+				}
+
+				// AP might have in ATE_STOP mode due to cmd from QA.
+				if (ATE_ON(pAdapter))
+				{
+					// Someone has killed ate daemon while QA GUI is still open.
+					Set_ATE_Proc(pAdapter, "ATESTOP");
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_AP_START is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_RF_WRITE_ALL:
+			{
+				UINT32 R1, R2, R3, R4;
+				USHORT channel;
+
+				memcpy(&R1, pRaCfg->data-2, 4);
+				memcpy(&R2, pRaCfg->data+2, 4);
+				memcpy(&R3, pRaCfg->data+6, 4);
+				memcpy(&R4, pRaCfg->data+10, 4);
+				memcpy(&channel, pRaCfg->data+14, 2);
+
+				pAdapter->LatchRfRegs.R1 = ntohl(R1);
+				pAdapter->LatchRfRegs.R2 = ntohl(R2);
+				pAdapter->LatchRfRegs.R3 = ntohl(R3);
+				pAdapter->LatchRfRegs.R4 = ntohl(R4);
+				pAdapter->LatchRfRegs.Channel = ntohs(channel);
+
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R1);
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R2);
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R3);
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R4);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RF_WRITE_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RF_WRITE_ALL is done !\n"));
+				}
+			}
+            break;
+
+		case RACFG_CMD_E2PROM_READ16:
+			{
+				USHORT	offset, value, tmp;
+
+				offset = ntohs(pRaCfg->status);
+				/* "tmp" is expecially for some compilers... */
+				RT28xx_EEPROM_READ16(pAdapter, offset, tmp);
+				value = tmp;
+				value = htons(value);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("EEPROM Read offset = 0x%04x, value = 0x%04x\n", offset, value));
+
+				// prepare feedback
+				pRaCfg->length = htons(4);
+				pRaCfg->status = htons(0);
+				memcpy(pRaCfg->data, &value, 2);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("sizeof(struct ate_racfghdr) = %d\n", sizeof(struct ate_racfghdr)));
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_READ16\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_E2PROM_READ16 is done !\n"));
+				}
+           	}
+			break;
+
+		case RACFG_CMD_E2PROM_WRITE16:
+			{
+				USHORT	offset, value;
+
+				offset = ntohs(pRaCfg->status);
+				memcpy(&value, pRaCfg->data, 2);
+				value = ntohs(value);
+				RT28xx_EEPROM_WRITE16(pAdapter, offset, value);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_WRITE16\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_E2PROM_WRITE16 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_E2PROM_READ_ALL:
+			{
+				USHORT buffer[EEPROM_SIZE/2];
+
+				rt_ee_read_all(pAdapter,(USHORT *)buffer);
+				memcpy_exs(pAdapter, pRaCfg->data, (UCHAR *)buffer, EEPROM_SIZE);
+
+				// prepare feedback
+				pRaCfg->length = htons(2+EEPROM_SIZE);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_READ_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_E2PROM_READ_ALL is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_E2PROM_WRITE_ALL:
+			{
+				USHORT buffer[EEPROM_SIZE/2];
+
+				NdisZeroMemory((UCHAR *)buffer, EEPROM_SIZE);
+				memcpy_exs(pAdapter, (UCHAR *)buffer, (UCHAR *)&pRaCfg->status, EEPROM_SIZE);
+				rt_ee_write_all(pAdapter,(USHORT *)buffer);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_WRITE_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_ERROR, ("RACFG_CMD_E2PROM_WRITE_ALL is done !\n"));
+				}
+
+			}
+			break;
+
+		case RACFG_CMD_IO_READ:
+			{
+				UINT32	offset;
+				UINT32	value;
+
+				memcpy(&offset, &pRaCfg->status, 4);
+				offset = ntohl(offset);
+
+				// We do not need the base address.
+				// So just extract the offset out.
+				offset &= 0x0000FFFF;
+				RTMP_IO_READ32(pAdapter, offset, &value);
+				value = htonl(value);
+
+				// prepare feedback
+				pRaCfg->length = htons(6);
+				pRaCfg->status = htons(0);
+				memcpy(pRaCfg->data, &value, 4);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_IO_READ\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_IO_READ is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_IO_WRITE:
+			{
+				UINT32	offset, value;
+
+				memcpy(&offset, pRaCfg->data-2, 4);
+				memcpy(&value, pRaCfg->data+2, 4);
+
+				offset = ntohl(offset);
+
+				// We do not need the base address.
+				// So just extract out the offset.
+				offset &= 0x0000FFFF;
+				value = ntohl(value);
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_IO_WRITE: offset = %x, value = %x\n", offset, value));
+				RTMP_IO_WRITE32(pAdapter, offset, value);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_IO_WRITE\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_IO_WRITE is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_IO_READ_BULK:
+			{
+				UINT32	offset;
+				USHORT	len;
+
+				memcpy(&offset, &pRaCfg->status, 4);
+				offset = ntohl(offset);
+
+				// We do not need the base address.
+				// So just extract the offset.
+				offset &= 0x0000FFFF;
+				memcpy(&len, pRaCfg->data+2, 2);
+				len = ntohs(len);
+
+				if (len > 371)
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE,("len is too large, make it smaller\n"));
+					pRaCfg->length = htons(2);
+					pRaCfg->status = htons(1);
+					break;
+				}
+
+				RTMP_IO_READ_BULK(pAdapter, pRaCfg->data, (UCHAR *)offset, len*4);// unit in four bytes
+
+				// prepare feedback
+				pRaCfg->length = htons(2+len*4);// unit in four bytes
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_IO_READ_BULK\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_IO_READ_BULK is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_BBP_READ8:
+			{
+				USHORT	offset;
+				UCHAR	value;
+
+				value = 0;
+				offset = ntohs(pRaCfg->status);
+
+				if (ATE_ON(pAdapter))
+				{
+					ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, offset,  &value);
+				}
+				else
+				{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, offset,  &value);
+				}
+				// prepare feedback
+				pRaCfg->length = htons(3);
+				pRaCfg->status = htons(0);
+				pRaCfg->data[0] = value;
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("BBP value = %x\n", value));
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_BBP_READ8\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_BBP_READ8 is done !\n"));
+				}
+			}
+			break;
+		case RACFG_CMD_BBP_WRITE8:
+			{
+				USHORT	offset;
+				UCHAR	value;
+
+				offset = ntohs(pRaCfg->status);
+				memcpy(&value, pRaCfg->data, 1);
+
+				if (ATE_ON(pAdapter))
+				{
+					ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, offset,  value);
+				}
+				else
+				{
+					RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, offset,  value);
+				}
+
+				if ((offset == BBP_R1) || (offset == BBP_R3))
+				{
+					SyncTxRxConfig(pAdapter, offset, value);
+				}
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_BBP_WRITE8\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_BBP_WRITE8 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_BBP_READ_ALL:
+			{
+				USHORT j;
+
+				for (j = 0; j < 137; j++)
+				{
+					pRaCfg->data[j] = 0;
+
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j]);
+					}
+					else
+					{
+						RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j]);
+					}
+				}
+
+				// prepare feedback
+				pRaCfg->length = htons(2+137);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_BBP_READ_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_BBP_READ_ALL is done !\n"));
+				}
+			}
+
+			break;
+
+		case RACFG_CMD_ATE_E2PROM_READ_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT buffer[EEPROM_SIZE/2];
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			rt_ee_read_all(pAdapter,(USHORT *)buffer);
+			if (offset + len <= EEPROM_SIZE)
+				memcpy_exs(pAdapter, pRaCfg->data, (UCHAR *)buffer+offset, len);
+			else
+				ATEDBGPRINT(RT_DEBUG_ERROR, ("exceed EEPROM size\n"));
+
+			// prepare feedback
+			pRaCfg->length = htons(2+len);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_E2PROM_READ_BULK\n"));
+                Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_E2PROM_READ_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_E2PROM_WRITE_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT buffer[EEPROM_SIZE/2];
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			rt_ee_read_all(pAdapter,(USHORT *)buffer);
+			memcpy_exs(pAdapter, (UCHAR *)buffer + offset, (UCHAR *)pRaCfg->data + 2, len);
+			rt_ee_write_all(pAdapter,(USHORT *)buffer);
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_E2PROM_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_ERROR, ("RACFG_CMD_ATE_E2PROM_WRITE_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_IO_WRITE_BULK:
+		{
+			UINT32 offset, i, value;
+			USHORT len;
+
+			memcpy(&offset, &pRaCfg->status, 4);
+			offset = ntohl(offset);
+			memcpy(&len, pRaCfg->data+2, 2);
+			len = ntohs(len);
+
+			for (i = 0; i < len; i += 4)
+			{
+				memcpy_exl(pAdapter, (UCHAR *)&value, pRaCfg->data+4+i, 4);
+				printk("Write %x %x\n", offset + i, value);
+				RTMP_IO_WRITE32(pAdapter, (offset +i) & 0xffff, value);
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_IO_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_ERROR, ("RACFG_CMD_ATE_IO_WRITE_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_BBP_READ_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				pRaCfg->data[j - offset] = 0;
+
+				if (pAdapter->ate.Mode == ATE_STOP)
+				{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j - offset]);
+				}
+				else
+				{
+					ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j - offset]);
+				}
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2+len);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_BBP_READ_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_BBP_READ_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_BBP_WRITE_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+			UCHAR *value;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				value = pRaCfg->data + 2 + (j - offset);
+				if (pAdapter->ate.Mode == ATE_STOP)
+				{
+					RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, j,  *value);
+				}
+				else
+				{
+					ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, j,  *value);
+				}
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_BBP_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_BBP_WRITE_BULK is done !\n"));
+			}
+		}
+			break;
+
+#ifdef CONFIG_RALINK_RT3052
+		case RACFG_CMD_ATE_RF_READ_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				pRaCfg->data[j - offset] = 0;
+				RT30xxReadRFRegister(pAdapter, j,  &pRaCfg->data[j - offset]);
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2+len);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_RF_READ_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_RF_READ_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_RF_WRITE_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+			UCHAR *value;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				value = pRaCfg->data + 2 + (j - offset);
+				RT30xxWriteRFRegister(pAdapter, j,  *value);
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_RF_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_RF_WRITE_BULK is done !\n"));
+			}
+
+		}
+			break;
+#endif
+
+
+		case RACFG_CMD_GET_NOISE_LEVEL:
+			{
+				UCHAR	channel;
+				INT32   buffer[3][10];/* 3 : RxPath ; 10 : no. of per rssi samples */
+
+				channel = (ntohs(pRaCfg->status) & 0x00FF);
+				CalNoiseLevel(pAdapter, channel, buffer);
+				memcpy_exl(pAdapter, (UCHAR *)pRaCfg->data, (UCHAR *)&(buffer[0][0]), (sizeof(INT32)*3*10));
+
+				// prepare feedback
+				pRaCfg->length = htons(2 + (sizeof(INT32)*3*10));
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_GET_NOISE_LEVEL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_GET_NOISE_LEVEL is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_GET_COUNTER:
+			{
+				memcpy_exl(pAdapter, &pRaCfg->data[0], (UCHAR *)&pAdapter->ate.U2M, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[4], (UCHAR *)&pAdapter->ate.OtherData, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[8], (UCHAR *)&pAdapter->ate.Beacon, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[12], (UCHAR *)&pAdapter->ate.OtherCount, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[16], (UCHAR *)&pAdapter->ate.TxAc0, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[20], (UCHAR *)&pAdapter->ate.TxAc1, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[24], (UCHAR *)&pAdapter->ate.TxAc2, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[28], (UCHAR *)&pAdapter->ate.TxAc3, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[32], (UCHAR *)&pAdapter->ate.TxHCCA, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[36], (UCHAR *)&pAdapter->ate.TxMgmt, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[40], (UCHAR *)&pAdapter->ate.RSSI0, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[44], (UCHAR *)&pAdapter->ate.RSSI1, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[48], (UCHAR *)&pAdapter->ate.RSSI2, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[52], (UCHAR *)&pAdapter->ate.SNR0, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[56], (UCHAR *)&pAdapter->ate.SNR1, 4);
+
+				pRaCfg->length = htons(2+60);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_GET_COUNTER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_GET_COUNTER is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_CLEAR_COUNTER:
+			{
+				pAdapter->ate.U2M = 0;
+				pAdapter->ate.OtherData = 0;
+				pAdapter->ate.Beacon = 0;
+				pAdapter->ate.OtherCount = 0;
+				pAdapter->ate.TxAc0 = 0;
+				pAdapter->ate.TxAc1 = 0;
+				pAdapter->ate.TxAc2 = 0;
+				pAdapter->ate.TxAc3 = 0;
+				pAdapter->ate.TxHCCA = 0;
+				pAdapter->ate.TxMgmt = 0;
+				pAdapter->ate.TxDoneCount = 0;
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_CLEAR_COUNTER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_CLEAR_COUNTER is done !\n"));
+				}
+			}
+
+			break;
+
+		case RACFG_CMD_TX_START:
+			{
+				USHORT *p;
+				USHORT	err = 1;
+				UCHAR   Bbp22Value = 0, Bbp24Value = 0;
+
+				if ((pAdapter->ate.TxStatus != 0) && (pAdapter->ate.Mode & ATE_TXFRAME))
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE,("Ate Tx is already running, to run next Tx, you must stop it first\n"));
+					err = 2;
+					goto TX_START_ERROR;
+				}
+				else if ((pAdapter->ate.TxStatus != 0) && !(pAdapter->ate.Mode & ATE_TXFRAME))
+				{
+					int i = 0;
+
+					while ((i++ < 10) && (pAdapter->ate.TxStatus != 0))
+					{
+						RTMPusecDelay(5000);
+					}
+
+					// force it to stop
+					pAdapter->ate.TxStatus = 0;
+					pAdapter->ate.TxDoneCount = 0;
+					//pAdapter->ate.Repeat = 0;
+					pAdapter->ate.bQATxStart = FALSE;
+				}
+
+				// If pRaCfg->length == 0, this "RACFG_CMD_TX_START" is for Carrier test or Carrier Suppression.
+				if (ntohs(pRaCfg->length) != 0)
+				{
+					// Get frame info
+#ifdef RT2870
+					NdisMoveMemory(&pAdapter->ate.TxInfo, pRaCfg->data - 2, 4);
+#ifdef RT_BIG_ENDIAN
+					RTMPDescriptorEndianChange((PUCHAR) &pAdapter->ate.TxInfo, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+#endif // RT2870 //
+
+					NdisMoveMemory(&pAdapter->ate.TxWI, pRaCfg->data + 2, 16);
+#ifdef RT_BIG_ENDIAN
+					RTMPWIEndianChange((PUCHAR)&pAdapter->ate.TxWI, TYPE_TXWI);
+#endif // RT_BIG_ENDIAN //
+
+					NdisMoveMemory(&pAdapter->ate.TxCount, pRaCfg->data + 18, 4);
+					pAdapter->ate.TxCount = ntohl(pAdapter->ate.TxCount);
+
+					p = (USHORT *)(&pRaCfg->data[22]);
+					//p = pRaCfg->data + 22;
+					// always use QID_AC_BE
+					pAdapter->ate.QID = 0;
+					p = (USHORT *)(&pRaCfg->data[24]);
+					//p = pRaCfg->data + 24;
+					pAdapter->ate.HLen = ntohs(*p);
+
+					if (pAdapter->ate.HLen > 32)
+					{
+						ATEDBGPRINT(RT_DEBUG_ERROR,("pAdapter->ate.HLen > 32\n"));
+						err = 3;
+						goto TX_START_ERROR;
+					}
+
+					NdisMoveMemory(&pAdapter->ate.Header, pRaCfg->data + 26, pAdapter->ate.HLen);
+
+
+					pAdapter->ate.PLen = ntohs(pRaCfg->length) - (pAdapter->ate.HLen + 28);
+
+					if (pAdapter->ate.PLen > 32)
+					{
+						ATEDBGPRINT(RT_DEBUG_ERROR,("pAdapter->ate.PLen > 32\n"));
+						err = 4;
+						goto TX_START_ERROR;
+					}
+
+					NdisMoveMemory(&pAdapter->ate.Pattern, pRaCfg->data + 26 + pAdapter->ate.HLen, pAdapter->ate.PLen);
+					pAdapter->ate.DLen = pAdapter->ate.TxWI.MPDUtotalByteCount - pAdapter->ate.HLen;
+				}
+
+				ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R22, &Bbp22Value);
+
+				switch (Bbp22Value)
+				{
+					case BBP22_TXFRAME:
+						{
+							if (pAdapter->ate.TxCount == 0)
+							{
+							}
+							ATEDBGPRINT(RT_DEBUG_TRACE,("START TXFRAME\n"));
+							pAdapter->ate.bQATxStart = TRUE;
+							Set_ATE_Proc(pAdapter, "TXFRAME");
+						}
+						break;
+
+					case BBP22_TXCONT_OR_CARRSUPP:
+						{
+							ATEDBGPRINT(RT_DEBUG_TRACE,("BBP22_TXCONT_OR_CARRSUPP\n"));
+							ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, 24, &Bbp24Value);
+
+							switch (Bbp24Value)
+							{
+								case BBP24_TXCONT:
+									{
+										ATEDBGPRINT(RT_DEBUG_TRACE,("START TXCONT\n"));
+										pAdapter->ate.bQATxStart = TRUE;
+										Set_ATE_Proc(pAdapter, "TXCONT");
+									}
+									break;
+
+								case BBP24_CARRSUPP:
+									{
+										ATEDBGPRINT(RT_DEBUG_TRACE,("START TXCARRSUPP\n"));
+										pAdapter->ate.bQATxStart = TRUE;
+										pAdapter->ate.Mode |= ATE_TXCARRSUPP;
+									}
+									break;
+
+								default:
+									{
+										ATEDBGPRINT(RT_DEBUG_ERROR,("Unknown Start TX subtype !"));
+									}
+									break;
+							}
+						}
+						break;
+
+					case BBP22_TXCARR:
+						{
+							ATEDBGPRINT(RT_DEBUG_TRACE,("START TXCARR\n"));
+							pAdapter->ate.bQATxStart = TRUE;
+							Set_ATE_Proc(pAdapter, "TXCARR");
+						}
+						break;
+
+					default:
+						{
+							ATEDBGPRINT(RT_DEBUG_ERROR,("Unknown Start TX subtype !"));
+						}
+						break;
+				}
+
+				if (pAdapter->ate.bQATxStart == TRUE)
+				{
+					// prepare feedback
+					pRaCfg->length = htons(2);
+					pRaCfg->status = htons(0);
+
+					wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+										+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+										+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+	            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+	            	{
+	            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() was failed in case RACFG_CMD_TX_START\n"));
+	                    Status = -EFAULT;
+	            	}
+					else
+					{
+	                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_TX_START is done !\n"));
+					}
+					break;
+				}
+
+TX_START_ERROR:
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(err);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_TX_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("feedback of TX_START_ERROR is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_GET_TX_STATUS:
+			{
+				UINT32 count;
+
+				// prepare feedback
+				pRaCfg->length = htons(6);
+				pRaCfg->status = htons(0);
+				count = htonl(pAdapter->ate.TxDoneCount);
+				NdisMoveMemory(pRaCfg->data, &count, 4);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_GET_TX_STATUS\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_GET_TX_STATUS is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_TX_STOP:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_TX_STOP\n"));
+
+				Set_ATE_Proc(pAdapter, "TXSTOP");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_TRACE, ("copy_to_user() fail in case RACFG_CMD_TX_STOP\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_TX_STOP is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_RX_START:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_RX_START\n"));
+
+				pAdapter->ate.bQARxStart = TRUE;
+				Set_ATE_Proc(pAdapter, "RXFRAME");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RX_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RX_START is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_RX_STOP:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_RX_STOP\n"));
+
+				Set_ATE_Proc(pAdapter, "RXSTOP");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RX_STOP\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RX_STOP is done !\n"));
+				}
+			}
+			break;
+
+		/* The following cases are for new ATE GUI(not QA). */
+		/*==================================================*/
+		case RACFG_CMD_ATE_START_TX_CARRIER:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START_TX_CARRIER\n"));
+
+				Set_ATE_Proc(pAdapter, "TXCARR");
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_START_TX_CARRIER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START_TX_CARRIER is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_START_TX_CONT:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START_TX_CONT\n"));
+
+				Set_ATE_Proc(pAdapter, "TXCONT");
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_START_TX_CONT\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START_TX_CONT is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_START_TX_FRAME:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START_TX_FRAME\n"));
+
+				Set_ATE_Proc(pAdapter, "TXFRAME");
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_START_TX_FRAME\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START_TX_FRAME is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_BW:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_BW\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+
+				Set_ATE_TX_BW_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_BW\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_BW is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_POWER0:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_POWER0\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_POWER0_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_POWER0\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_POWER0 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_POWER1:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_POWER1\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_POWER1_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_POWER1\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_POWER1 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_FREQ_OFFSET:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_FREQ_OFFSET\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_FREQOFFSET_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_FREQ_OFFSET\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_FREQ_OFFSET is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_GET_STATISTICS:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_GET_STATISTICS\n"));
+
+				memcpy_exl(pAdapter, &pRaCfg->data[0], (UCHAR *)&pAdapter->ate.TxDoneCount, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[4], (UCHAR *)&pAdapter->WlanCounters.RetryCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[8], (UCHAR *)&pAdapter->WlanCounters.FailedCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[12], (UCHAR *)&pAdapter->WlanCounters.RTSSuccessCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[16], (UCHAR *)&pAdapter->WlanCounters.RTSFailureCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[20], (UCHAR *)&pAdapter->WlanCounters.ReceivedFragmentCount.QuadPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[24], (UCHAR *)&pAdapter->WlanCounters.FCSErrorCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[28], (UCHAR *)&pAdapter->Counters8023.RxNoBuffer, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[32], (UCHAR *)&pAdapter->WlanCounters.FrameDuplicateCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[36], (UCHAR *)&pAdapter->RalinkCounters.OneSecFalseCCACnt, 4);
+
+				if (pAdapter->ate.RxAntennaSel == 0)
+				{
+					INT32 RSSI0 = 0;
+					INT32 RSSI1 = 0;
+					INT32 RSSI2 = 0;
+
+					RSSI0 = (INT32)(pAdapter->ate.LastRssi0 - pAdapter->BbpRssiToDbmDelta);
+					RSSI1 = (INT32)(pAdapter->ate.LastRssi1 - pAdapter->BbpRssiToDbmDelta);
+					RSSI2 = (INT32)(pAdapter->ate.LastRssi2 - pAdapter->BbpRssiToDbmDelta);
+					memcpy_exl(pAdapter, &pRaCfg->data[40], (UCHAR *)&RSSI0, 4);
+					memcpy_exl(pAdapter, &pRaCfg->data[44], (UCHAR *)&RSSI1, 4);
+					memcpy_exl(pAdapter, &pRaCfg->data[48], (UCHAR *)&RSSI2, 4);
+					pRaCfg->length = htons(2+52);
+				}
+				else
+				{
+					INT32 RSSI0 = 0;
+
+					RSSI0 = (INT32)(pAdapter->ate.LastRssi0 - pAdapter->BbpRssiToDbmDelta);
+					memcpy_exl(pAdapter, &pRaCfg->data[40], (UCHAR *)&RSSI0, 4);
+					pRaCfg->length = htons(2+44);
+				}
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_GET_STATISTICS\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_GET_STATISTICS is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_RESET_COUNTER:
+			{
+				SHORT    value = 1;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_RESET_COUNTER\n"));
+
+				sprintf((PCHAR)str, "%d", value);
+				Set_ResetStatCounter_Proc(pAdapter, str);
+
+				pAdapter->ate.TxDoneCount = 0;
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_RESET_COUNTER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_RESET_COUNTER is done !\n"));
+				}
+			}
+
+			break;
+
+		case RACFG_CMD_ATE_SEL_TX_ANTENNA:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SEL_TX_ANTENNA\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_Antenna_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SEL_TX_ANTENNA\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SEL_TX_ANTENNA is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SEL_RX_ANTENNA:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SEL_RX_ANTENNA\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_RX_Antenna_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SEL_RX_ANTENNA\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SEL_RX_ANTENNA is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_PREAMBLE:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_PREAMBLE\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_MODE_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_PREAMBLE\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_PREAMBLE is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_CHANNEL:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_CHANNEL\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_CHANNEL_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_CHANNEL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_CHANNEL is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_ADDR1:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_ADDR1\n"));
+
+				// Addr is an array of UCHAR,
+				// so no need to perform endian swap.
+				memcpy(pAdapter->ate.Addr1, (PUCHAR)(pRaCfg->data - 2), MAC_ADDR_LEN);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_ADDR1\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_ADDR1 is done !\n (ADDR1 = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAdapter->ate.Addr1[0],
+						pAdapter->ate.Addr1[1], pAdapter->ate.Addr1[2], pAdapter->ate.Addr1[3], pAdapter->ate.Addr1[4], pAdapter->ate.Addr1[5]));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_ADDR2:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_ADDR2\n"));
+
+				// Addr is an array of UCHAR,
+				// so no need to perform endian swap.
+				memcpy(pAdapter->ate.Addr2, (PUCHAR)(pRaCfg->data - 2), MAC_ADDR_LEN);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_ADDR2\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_ADDR2 is done !\n (ADDR2 = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAdapter->ate.Addr2[0],
+						pAdapter->ate.Addr2[1], pAdapter->ate.Addr2[2], pAdapter->ate.Addr2[3], pAdapter->ate.Addr2[4], pAdapter->ate.Addr2[5]));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_ADDR3:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_ADDR3\n"));
+
+				// Addr is an array of UCHAR,
+				// so no need to perform endian swap.
+				memcpy(pAdapter->ate.Addr3, (PUCHAR)(pRaCfg->data - 2), MAC_ADDR_LEN);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_ADDR3\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_ADDR3 is done !\n (ADDR3 = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAdapter->ate.Addr3[0],
+						pAdapter->ate.Addr3[1], pAdapter->ate.Addr3[2], pAdapter->ate.Addr3[3], pAdapter->ate.Addr3[4], pAdapter->ate.Addr3[5]));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_RATE:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_RATE\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_MCS_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_RATE\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_RATE is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_FRAME_LEN:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_FRAME_LEN\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_LENGTH_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_FRAME_LEN\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_FRAME_LEN is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_FRAME_COUNT:
+			{
+				USHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_FRAME_COUNT\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				{
+					sprintf((PCHAR)str, "%d", value);
+					Set_ATE_TX_COUNT_Proc(pAdapter, str);
+				}
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_FRAME_COUNT\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_FRAME_COUNT is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_START_RX_FRAME:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_RX_START\n"));
+
+				Set_ATE_Proc(pAdapter, "RXFRAME");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RX_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RX_START is done !\n"));
+				}
+			}
+			break;
+		default:
+			break;
+	}
+    ASSERT(pRaCfg != NULL);
+    if (pRaCfg != NULL)
+    {
+    kfree(pRaCfg);
+    }
+	return;
+}
+
+VOID BubbleSort(INT32 n, INT32 a[])
+{
+	INT32 k, j, temp;
+
+	for (k = n-1;  k>0;  k--)
+	{
+		for (j = 0; j<k; j++)
+		{
+			if(a[j] > a[j+1])
+			{
+				temp = a[j];
+				a[j]=a[j+1];
+				a[j+1]=temp;
+			}
+		}
+	}
+}
+
+VOID CalNoiseLevel(PRTMP_ADAPTER pAd, UCHAR channel, INT32 RSSI[3][10])
+{
+	INT32		RSSI0, RSSI1, RSSI2;
+ 	CHAR		Rssi0Offset, Rssi1Offset, Rssi2Offset;
+	UCHAR		BbpR50Rssi0 = 0, BbpR51Rssi1 = 0, BbpR52Rssi2 = 0;
+	UCHAR		Org_BBP66value = 0, Org_BBP69value = 0, Org_BBP70value = 0, data = 0;
+	USHORT		LNA_Gain = 0;
+	INT32       j = 0;
+	UCHAR		Org_Channel = pAd->ate.Channel;
+	USHORT	    GainValue = 0, OffsetValue = 0;
+
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &Org_BBP66value);
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R69, &Org_BBP69value);
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R70, &Org_BBP70value);
+
+	//**********************************************************************
+	// Read the value of LNA gain and Rssi offset
+	//**********************************************************************
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LNA_OFFSET, GainValue);
+
+	// for Noise Level
+	if (channel <= 14)
+	{
+		LNA_Gain = GainValue & 0x00FF;
+
+		RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_BG_OFFSET, OffsetValue);
+		Rssi0Offset = OffsetValue & 0x00FF;
+		Rssi1Offset = (OffsetValue & 0xFF00) >> 8;
+		RT28xx_EEPROM_READ16(pAd, (EEPROM_RSSI_BG_OFFSET + 2)/* 0x48 */, OffsetValue);
+		Rssi2Offset = OffsetValue & 0x00FF;
+	}
+	else
+	{
+		LNA_Gain = (GainValue & 0xFF00) >> 8;
+
+		RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_A_OFFSET, OffsetValue);
+		Rssi0Offset = OffsetValue & 0x00FF;
+		Rssi1Offset = (OffsetValue & 0xFF00) >> 8;
+		RT28xx_EEPROM_READ16(pAd, (EEPROM_RSSI_A_OFFSET + 2)/* 0x4C */, OffsetValue);
+		Rssi2Offset = OffsetValue & 0x00FF;
+	}
+	//**********************************************************************
+	{
+		pAd->ate.Channel = channel;
+		ATEAsicSwitchChannel(pAd);
+		mdelay(5);
+
+		data = 0x10;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, data);
+		data = 0x40;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, data);
+		data = 0x40;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, data);
+		mdelay(5);
+
+		// Start Rx
+		pAd->ate.bQARxStart = TRUE;
+		Set_ATE_Proc(pAd, "RXFRAME");
+
+		mdelay(5);
+
+		for (j = 0; j < 10; j++)
+		{
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R50, &BbpR50Rssi0);
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R51, &BbpR51Rssi1);
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R52, &BbpR52Rssi2);
+
+			mdelay(10);
+
+			// Calculate RSSI 0
+			if (BbpR50Rssi0 == 0)
+			{
+				RSSI0 = -100;
+			}
+			else
+			{
+				RSSI0 = (INT32)(-12 - BbpR50Rssi0 - LNA_Gain - Rssi0Offset);
+			}
+			RSSI[0][j] = RSSI0;
+
+			if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+			{
+				// Calculate RSSI 1
+				if (BbpR51Rssi1 == 0)
+				{
+					RSSI1 = -100;
+				}
+				else
+				{
+					RSSI1 = (INT32)(-12 - BbpR51Rssi1 - LNA_Gain - Rssi1Offset);
+				}
+				RSSI[1][j] = RSSI1;
+			}
+
+			if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+			{
+				// Calculate RSSI 2
+				if (BbpR52Rssi2 == 0)
+					RSSI2 = -100;
+				else
+					RSSI2 = (INT32)(-12 - BbpR52Rssi2 - LNA_Gain - Rssi2Offset);
+
+				RSSI[2][j] = RSSI2;
+			}
+		}
+
+		// Stop Rx
+		Set_ATE_Proc(pAd, "RXSTOP");
+
+		mdelay(5);
+
+#if 0// Debug Message................
+		ate_print("\n**********************************************************\n");
+		ate_print("Noise Level: Channel %d\n", channel);
+		ate_print("RSSI0 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+			RSSI[0][0], RSSI[0][1], RSSI[0][2],
+			RSSI[0][3], RSSI[0][4], RSSI[0][5],
+			RSSI[0][6], RSSI[0][7], RSSI[0][8],
+			RSSI[0][9]);
+		if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+		{
+			ate_print("RSSI1 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[1][0], RSSI[1][1], RSSI[1][2],
+				RSSI[1][3], RSSI[1][4], RSSI[1][5],
+				RSSI[1][6], RSSI[1][7], RSSI[1][8],
+				RSSI[1][9]);
+		}
+		if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+		{
+			ate_print("RSSI2 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[2][0], RSSI[2][1], RSSI[2][2],
+				RSSI[2][3], RSSI[2][4], RSSI[2][5],
+				RSSI[2][6], RSSI[2][7], RSSI[2][8],
+				RSSI[2][9]);
+		}
+#endif // 0 //
+		BubbleSort(10, RSSI[0]);	// 1R
+
+		if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+		{
+			BubbleSort(10, RSSI[1]);
+		}
+
+		if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+		{
+			BubbleSort(10, RSSI[2]);
+		}
+
+#if 0// Debug Message................
+		ate_print("\nAfter Sorting....Channel %d\n", channel);
+		ate_print("RSSI0 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+			RSSI[0][0], RSSI[0][1], RSSI[0][2],
+			RSSI[0][3], RSSI[0][4], RSSI[0][5],
+			RSSI[0][6], RSSI[0][7], RSSI[0][8],
+			RSSI[0][9]);
+		if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+		{
+			ate_print("RSSI1 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[1][0], RSSI[1][1], RSSI[1][2],
+				RSSI[1][3], RSSI[1][4], RSSI[1][5],
+				RSSI[1][6], RSSI[1][7], RSSI[1][8],
+				RSSI[1][9]);
+		}
+		if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+		{
+			ate_print("RSSI2 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[2][0], RSSI[2][1], RSSI[2][2],
+				RSSI[2][3], RSSI[2][4], RSSI[2][5],
+				RSSI[2][6], RSSI[2][7], RSSI[2][8],
+				RSSI[2][9]);
+		}
+		ate_print("**********************************************************\n");
+#endif // 0 //
+	}
+
+	pAd->ate.Channel = Org_Channel;
+	ATEAsicSwitchChannel(pAd);
+
+	// Restore original value
+    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, Org_BBP66value);
+    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, Org_BBP69value);
+    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, Org_BBP70value);
+
+	return;
+}
+
+BOOLEAN SyncTxRxConfig(PRTMP_ADAPTER pAd, USHORT offset, UCHAR value)
+{
+	UCHAR tmp = 0, bbp_data = 0;
+
+	if (ATE_ON(pAd))
+	{
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, offset, &bbp_data);
+	}
+	else
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, offset, &bbp_data);
+	}
+
+	/* confirm again */
+	ASSERT(bbp_data == value);
+
+	switch(offset)
+	{
+		case BBP_R1:
+			/* Need to sync. tx configuration with legacy ATE. */
+			tmp = (bbp_data & ((1 << 4) | (1 << 3))/* 0x18 */) >> 3;
+		    switch(tmp)
+		    {
+				/* The BBP R1 bit[4:3] = 2 :: Both DACs will be used by QA. */
+		        case 2:
+					/* All */
+					pAd->ate.TxAntennaSel = 0;
+		            break;
+				/* The BBP R1 bit[4:3] = 0 :: DAC 0 will be used by QA. */
+		        case 0:
+					/* Antenna one */
+					pAd->ate.TxAntennaSel = 1;
+		            break;
+				/* The BBP R1 bit[4:3] = 1 :: DAC 1 will be used by QA. */
+		        case 1:
+					/* Antenna two */
+					pAd->ate.TxAntennaSel = 2;
+		            break;
+		        default:
+		            DBGPRINT(RT_DEBUG_TRACE, ("%s -- Sth. wrong!  : return FALSE; \n", __func__));
+		            return FALSE;
+		    }
+			break;/* case BBP_R1 */
+
+		case BBP_R3:
+			/* Need to sync. rx configuration with legacy ATE. */
+			tmp = (bbp_data & ((1 << 1) | (1 << 0))/* 0x03 */);
+		    switch(tmp)
+		    {
+				/* The BBP R3 bit[1:0] = 3 :: All ADCs will be used by QA. */
+		        case 3:
+					/* All */
+					pAd->ate.RxAntennaSel = 0;
+		            break;
+				/* The BBP R3 bit[1:0] = 0 :: ADC 0 will be used by QA, */
+				/* unless the BBP R3 bit[4:3] = 2 */
+		        case 0:
+					/* Antenna one */
+					pAd->ate.RxAntennaSel = 1;
+					tmp = ((bbp_data & ((1 << 4) | (1 << 3))/* 0x03 */) >> 3);
+					if (tmp == 2)// 3R
+					{
+						/* Default : All ADCs will be used by QA */
+						pAd->ate.RxAntennaSel = 0;
+					}
+		            break;
+				/* The BBP R3 bit[1:0] = 1 :: ADC 1 will be used by QA. */
+		        case 1:
+					/* Antenna two */
+					pAd->ate.RxAntennaSel = 2;
+		            break;
+				/* The BBP R3 bit[1:0] = 2 :: ADC 2 will be used by QA. */
+		        case 2:
+					/* Antenna three */
+					pAd->ate.RxAntennaSel = 3;
+		            break;
+		        default:
+		            DBGPRINT(RT_DEBUG_ERROR, ("%s -- Impossible!  : return FALSE; \n", __func__));
+		            return FALSE;
+		    }
+			break;/* case BBP_R3 */
+
+        default:
+            DBGPRINT(RT_DEBUG_ERROR, ("%s -- Sth. wrong!  : return FALSE; \n", __func__));
+            return FALSE;
+
+	}
+	return TRUE;
+}
+
+static VOID memcpy_exl(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len)
+{
+	ULONG i, Value = 0;
+	ULONG *pDst, *pSrc;
+	UCHAR *p8;
+
+	p8 = src;
+	pDst = (ULONG *) dst;
+	pSrc = (ULONG *) src;
+
+	for (i = 0 ; i < (len/4); i++)
+	{
+		/* For alignment issue, we need a variable "Value". */
+		memmove(&Value, pSrc, 4);
+		Value = htonl(Value);
+		memmove(pDst, &Value, 4);
+		pDst++;
+		pSrc++;
+	}
+	if ((len % 4) != 0)
+	{
+		/* wish that it will never reach here */
+		memmove(&Value, pSrc, (len % 4));
+		Value = htonl(Value);
+		memmove(pDst, &Value, (len % 4));
+	}
+}
+
+static VOID memcpy_exs(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len)
+{
+	ULONG i;
+	UCHAR *pDst, *pSrc;
+
+	pDst = dst;
+	pSrc = src;
+
+	for (i = 0; i < (len/2); i++)
+	{
+		memmove(pDst, pSrc, 2);
+		*((USHORT *)pDst) = htons(*((USHORT *)pDst));
+		pDst+=2;
+		pSrc+=2;
+	}
+
+	if ((len % 2) != 0)
+	{
+		memmove(pDst, pSrc, 1);
+	}
+}
+
+static VOID RTMP_IO_READ_BULK(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, UINT32 len)
+{
+	UINT32 i, Value;
+	UINT32 *pDst, *pSrc;
+
+	pDst = (UINT32 *) dst;
+	pSrc = (UINT32 *) src;
+
+	for (i = 0 ; i < (len/4); i++)
+	{
+		RTMP_IO_READ32(pAd, (ULONG)pSrc, &Value);
+		Value = htonl(Value);
+		memmove(pDst, &Value, 4);
+		pDst++;
+		pSrc++;
+	}
+	return;
+}
+
+// TODO:
+#if 0
+/* These work only when RALINK_ATE is defined */
+INT Set_TxStart_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG value = simple_strtol(arg, 0, 10);
+	UCHAR buffer[26] = {0x88, 0x02, 0x2c, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x00, 0x55, 0x44, 0x33, 0x22, 0x11, 0xc0, 0x22, 0x00, 0x00};
+	POS_COOKIE pObj;
+
+	if (pAd->ate.TxStatus != 0)
+		return FALSE;
+
+	pAd->ate.TxInfo = 0x04000000;
+	bzero(&pAd->ate.TxWI, sizeof(TXWI_STRUC));
+	pAd->ate.TxWI.PHYMODE = 0;// MODE_CCK
+	pAd->ate.TxWI.MPDUtotalByteCount = 1226;
+	pAd->ate.TxWI.MCS = 3;
+	//pAd->ate.Mode = ATE_START;
+	pAd->ate.Mode |= ATE_TXFRAME;
+	pAd->ate.TxCount = value;
+	pAd->ate.QID = 0;
+	pAd->ate.HLen = 26;
+	pAd->ate.PLen = 0;
+	pAd->ate.DLen = 1200;
+	memcpy(pAd->ate.Header, buffer, 26);
+	pAd->ate.bQATxStart = TRUE;
+	//pObj = (POS_COOKIE) pAd->OS_Cookie;
+	//tasklet_hi_schedule(&pObj->AteTxTask);
+	return TRUE;
+}
+#endif  /* end of #if 0 */
+
+INT Set_TxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ATEDBGPRINT(RT_DEBUG_TRACE,("Set_TxStop_Proc\n"));
+
+	if (Set_ATE_Proc(pAd, "TXSTOP"))
+	{
+	return TRUE;
+}
+	else
+	{
+		return FALSE;
+	}
+}
+
+INT Set_RxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ATEDBGPRINT(RT_DEBUG_TRACE,("Set_RxStop_Proc\n"));
+
+	if (Set_ATE_Proc(pAd, "RXSTOP"))
+	{
+	return TRUE;
+}
+	else
+	{
+		return FALSE;
+	}
+}
+
+#if 0
+INT Set_EEWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT offset = 0, value;
+	PUCHAR p2 = arg;
+
+	while((*p2 != ':') && (*p2 != '\0'))
+	{
+		p2++;
+	}
+
+	if (*p2 == ':')
+	{
+		A2Hex(offset, arg);
+		A2Hex(value, p2+ 1);
+	}
+	else
+	{
+		A2Hex(value, arg);
+	}
+
+	if (offset >= EEPROM_SIZE)
+	{
+		ate_print("Offset can not exceed EEPROM_SIZE( == 0x%04x)\n", EEPROM_SIZE);
+		return FALSE;
+	}
+
+	RTMP_EEPROM_WRITE16(pAd, offset, value);
+
+	return TRUE;
+}
+
+INT Set_BBPRead_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR value = 0, offset;
+
+	A2Hex(offset, arg);
+
+	if (ATE_ON(pAd))
+	{
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, offset,  &value);
+	}
+	else
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, offset,  &value);
+	}
+
+	ate_print("%x\n", value);
+
+	return TRUE;
+}
+
+
+INT Set_BBPWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT offset = 0;
+	PUCHAR p2 = arg;
+	UCHAR value;
+
+	while((*p2 != ':') && (*p2 != '\0'))
+	{
+		p2++;
+	}
+
+	if (*p2 == ':')
+	{
+		A2Hex(offset, arg);
+		A2Hex(value, p2+ 1);
+	}
+	else
+	{
+		A2Hex(value, arg);
+	}
+
+	if (ATE_ON(pAd))
+	{
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, offset,  value);
+	}
+	else
+	{
+		RTNP_BBP_IO_WRITE8_BY_REG_ID(pAd, offset,  value);
+	}
+
+	return TRUE;
+}
+
+INT Set_RFWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	PUCHAR p2, p3, p4;
+	ULONG R1, R2, R3, R4;
+
+	p2 = arg;
+
+	while((*p2 != ':') && (*p2 != '\0'))
+	{
+		p2++;
+	}
+
+	if (*p2 != ':')
+		return FALSE;
+
+	p3 = p2 + 1;
+
+	while((*p3 != ':') && (*p3 != '\0'))
+	{
+		p3++;
+	}
+
+	if (*p3 != ':')
+		return FALSE;
+
+	p4 = p3 + 1;
+
+	while((*p4 != ':') && (*p4 != '\0'))
+	{
+		p4++;
+	}
+
+	if (*p4 != ':')
+		return FALSE;
+
+
+	A2Hex(R1, arg);
+	A2Hex(R2, p2 + 1);
+	A2Hex(R3, p3 + 1);
+	A2Hex(R4, p4 + 1);
+
+	RTMP_RF_IO_WRITE32(pAd, R1);
+	RTMP_RF_IO_WRITE32(pAd, R2);
+	RTMP_RF_IO_WRITE32(pAd, R3);
+	RTMP_RF_IO_WRITE32(pAd, R4);
+
+	return TRUE;
+}
+#endif  // end of #if 0 //
+#endif	// RALINK_28xx_QA //
+
+#endif	// RALINK_ATE //
+
diff --git a/drivers/staging/rt2870/rt_ate.h b/drivers/staging/rt2870/rt_ate.h
new file mode 100644
index 000000000000..b618ce3599ac
--- /dev/null
+++ b/drivers/staging/rt2870/rt_ate.h
@@ -0,0 +1,315 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __ATE_H__
+#define __ATE_H__
+
+#ifndef UCOS
+#define ate_print printk
+#define ATEDBGPRINT DBGPRINT
+
+#ifdef RT2870
+#define EEPROM_SIZE								0x400
+#ifdef CONFIG_STA_SUPPORT
+#define EEPROM_BIN_FILE_NAME  "/etc/Wireless/RT2870STA/e2p.bin"
+#endif // CONFIG_STA_SUPPORT //
+#endif // RT2870 //
+#else // !UCOS //
+#define fATE_LOAD_EEPROM						0x0C43
+#ifdef CONFIG_PRINTK
+extern INT ConsoleResponse(IN PUCHAR buff);
+extern int (*remote_display)(char *);
+extern void puts (const char *s);
+
+/* specificly defined to redirect and show ate-related messages to host. */
+/* Try to define ate_print as a macro. */
+#define ate_print(fmt, args...)                 \
+do{   int (*org_remote_display)(char *) = NULL;   \
+	org_remote_display = remote_display;\
+	/* Save original "remote_display" */\
+	remote_display = (int (*)(char *))ConsoleResponse;           \
+	printk(fmt, ## args);                       \
+	/* Restore the remote_display function pointer */        \
+	remote_display = org_remote_display; }while(0)
+
+#define ATEDBGPRINT(Level, Fmt)    	\
+{                                   \
+    if ((Level) <= RTDebugLevel)      \
+    {                               \
+        ate_print Fmt;					\
+    }                               \
+}
+#endif // CONFIG_PRINTK //
+#endif // !UCOS //
+
+#define ATE_ON(_p)              (((_p)->ate.Mode) != ATE_STOP)
+
+/* RT2880_iNIC will define "RT2860". */
+
+/* RT2880_iNIC will define RT2860. */
+
+#ifdef RT2870
+#define EEPROM_SIZE								0x400
+#ifdef CONFIG_STA_SUPPORT
+#define EEPROM_BIN_FILE_NAME  "/etc/Wireless/RT2870STA/e2p.bin"
+#endif // CONFIG_STA_SUPPORT //
+#endif // RT2870 //
+
+#ifdef RT2870
+#define ATE_BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)    RTMP_BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)
+#define ATE_BBP_IO_WRITE8_BY_REG_ID(_A, _I, _V)    RTMP_BBP_IO_WRITE8_BY_REG_ID(_A, _I, _V)
+
+#define BULK_OUT_LOCK(pLock, IrqFlags)	\
+		if(1 /*!(in_interrupt() & 0xffff0000)*/)	\
+			RTMP_IRQ_LOCK((pLock), IrqFlags);
+
+#define BULK_OUT_UNLOCK(pLock, IrqFlags)	\
+		if(1 /*!(in_interrupt() & 0xffff0000)*/)	\
+			RTMP_IRQ_UNLOCK((pLock), IrqFlags);
+
+// Prototypes of completion funuc.
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+#define ATE_RTUSBBulkOutDataPacketComplete(purb, pt_regs)    ATE_RTUSBBulkOutDataPacketComplete(purb)
+#endif
+
+VOID ATE_RTUSBBulkOutDataPacketComplete(
+	IN purbb_t purb,
+	OUT struct pt_regs *pt_regs);
+
+VOID ATE_RTUSBBulkOutDataPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BulkOutPipeId);
+
+VOID ATE_RTUSBCancelPendingBulkInIRP(
+	IN	PRTMP_ADAPTER	pAd);
+#endif // RT2870 //
+
+VOID rt_ee_read_all(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT USHORT *Data);
+
+
+VOID rt_ee_write_all(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT *Data);
+
+INT Set_ATE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_DA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_SA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_BSSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_CHANNEL_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_POWER0_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_POWER1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_RX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_FREQOFFSET_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_BW_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_LENGTH_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_COUNT_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_MCS_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_MODE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_GI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+
+INT	Set_ATE_RX_FER_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Read_RF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Load_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Read_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_Show_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_Help_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+VOID ATE_QA_Statistics(
+	IN PRTMP_ADAPTER		pAd,
+	IN PRXWI_STRUC			pRxWI,
+	IN PRT28XX_RXD_STRUC    p28xxRxD,
+	IN PHEADER_802_11		pHeader);
+
+VOID RtmpDoAte(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID BubbleSort(
+	IN  INT32 n,
+	IN  INT32 a[]);
+
+VOID CalNoiseLevel(
+	IN  PRTMP_ADAPTER   pAdapter,
+	IN  UCHAR           channel,
+	OUT INT32           buffer[3][10]);
+
+BOOLEAN SyncTxRxConfig(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	USHORT			offset,
+	IN	UCHAR			value);
+
+#if 0
+INT Set_TxStart_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif  // 0 //
+
+INT Set_TxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_RxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#if 0
+INT Set_EERead_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_EEWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_BBPRead_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_BBPWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_RFWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // end of #if 0 //
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+VOID ATEAsicSwitchChannel(
+	IN PRTMP_ADAPTER pAd);
+
+VOID ATEAsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd);
+
+VOID ATEDisableAsicProtect(
+	IN		PRTMP_ADAPTER	pAd);
+
+CHAR ATEConvertToRssi(
+	IN PRTMP_ADAPTER  pAd,
+	IN CHAR				Rssi,
+	IN UCHAR    RssiNumber);
+
+VOID ATESampleRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN PRXWI_STRUC		pRxWI);
+
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPStationStop(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPStationStart(
+    IN  PRTMP_ADAPTER   pAd);
+#endif // CONFIG_STA_SUPPORT //
+#endif // __ATE_H__ //
diff --git a/drivers/staging/rt2870/rt_config.h b/drivers/staging/rt2870/rt_config.h
new file mode 100644
index 000000000000..e3fe2642633a
--- /dev/null
+++ b/drivers/staging/rt2870/rt_config.h
@@ -0,0 +1,104 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rt_config.h
+
+	Abstract:
+	Central header file to maintain all include files for all NDIS
+	miniport driver routines.
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Paul Lin    08-01-2002    created
+
+*/
+#ifndef	__RT_CONFIG_H__
+#define	__RT_CONFIG_H__
+
+#include    "rtmp_type.h"
+#ifdef UCOS
+#include "includes.h"
+#include <stdio.h>
+#include 	"rt_ucos.h"
+#endif
+
+#ifdef LINUX
+#include	"rt_linux.h"
+#endif
+#include    "rtmp_def.h"
+#include    "rt28xx.h"
+
+
+#ifdef RT2870
+#include	"rt2870.h"
+#endif // RT2870 //
+
+#include    "oid.h"
+#include    "mlme.h"
+#include    "wpa.h"
+#include    "md5.h"
+#include    "rtmp.h"
+#include	"ap.h"
+#include	"dfs.h"
+#include	"chlist.h"
+#include	"spectrum.h"
+
+
+#ifdef LEAP_SUPPORT
+#include    "leap.h"
+#endif // LEAP_SUPPORT //
+
+#ifdef BLOCK_NET_IF
+#include "netif_block.h"
+#endif // BLOCK_NET_IF //
+
+#ifdef IGMP_SNOOP_SUPPORT
+#include "igmp_snoop.h"
+#endif // IGMP_SNOOP_SUPPORT //
+
+#ifdef RALINK_ATE
+#include "rt_ate.h"
+#endif // RALINK_ATE //
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifndef WPA_SUPPLICANT_SUPPORT
+#error "Build for being controlled by NetworkManager or wext, please set HAS_WPA_SUPPLICANT=y and HAS_NATIVE_WPA_SUPPLICANT_SUPPORT=y"
+#endif // WPA_SUPPLICANT_SUPPORT //
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef IKANOS_VX_1X0
+#include	"vr_ikans.h"
+#endif // IKANOS_VX_1X0 //
+
+#endif	// __RT_CONFIG_H__
+
diff --git a/drivers/staging/rt2870/rt_linux.c b/drivers/staging/rt2870/rt_linux.c
new file mode 100644
index 000000000000..992e3d16f9b4
--- /dev/null
+++ b/drivers/staging/rt2870/rt_linux.c
@@ -0,0 +1,1095 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "rt_config.h"
+
+ULONG	RTDebugLevel = RT_DEBUG_ERROR;
+
+BUILD_TIMER_FUNCTION(MlmePeriodicExec);
+//BUILD_TIMER_FUNCTION(MlmeRssiReportExec);
+BUILD_TIMER_FUNCTION(AsicRxAntEvalTimeout);
+BUILD_TIMER_FUNCTION(APSDPeriodicExec);
+BUILD_TIMER_FUNCTION(AsicRfTuningExec);
+#ifdef RT2870
+BUILD_TIMER_FUNCTION(BeaconUpdateExec);
+#endif // RT2870 //
+
+
+#ifdef CONFIG_STA_SUPPORT
+BUILD_TIMER_FUNCTION(BeaconTimeout);
+BUILD_TIMER_FUNCTION(ScanTimeout);
+BUILD_TIMER_FUNCTION(AuthTimeout);
+BUILD_TIMER_FUNCTION(AssocTimeout);
+BUILD_TIMER_FUNCTION(ReassocTimeout);
+BUILD_TIMER_FUNCTION(DisassocTimeout);
+BUILD_TIMER_FUNCTION(LinkDownExec);
+#ifdef LEAP_SUPPORT
+BUILD_TIMER_FUNCTION(LeapAuthTimeout);
+#endif
+BUILD_TIMER_FUNCTION(StaQuickResponeForRateUpExec);
+BUILD_TIMER_FUNCTION(WpaDisassocApAndBlockAssoc);
+#ifdef QOS_DLS_SUPPORT
+BUILD_TIMER_FUNCTION(DlsTimeoutAction);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+// for wireless system event message
+char const *pWirelessSysEventText[IW_SYS_EVENT_TYPE_NUM] = {
+	// system status event
+    "had associated successfully",							/* IW_ASSOC_EVENT_FLAG */
+    "had disassociated",									/* IW_DISASSOC_EVENT_FLAG */
+    "had deauthenticated",									/* IW_DEAUTH_EVENT_FLAG */
+    "had been aged-out and disassociated",					/* IW_AGEOUT_EVENT_FLAG */
+    "occurred CounterMeasures attack",						/* IW_COUNTER_MEASURES_EVENT_FLAG */
+    "occurred replay counter different in Key Handshaking",	/* IW_REPLAY_COUNTER_DIFF_EVENT_FLAG */
+    "occurred RSNIE different in Key Handshaking",			/* IW_RSNIE_DIFF_EVENT_FLAG */
+    "occurred MIC different in Key Handshaking",			/* IW_MIC_DIFF_EVENT_FLAG */
+    "occurred ICV error in RX",								/* IW_ICV_ERROR_EVENT_FLAG */
+    "occurred MIC error in RX",								/* IW_MIC_ERROR_EVENT_FLAG */
+	"Group Key Handshaking timeout",						/* IW_GROUP_HS_TIMEOUT_EVENT_FLAG */
+	"Pairwise Key Handshaking timeout",						/* IW_PAIRWISE_HS_TIMEOUT_EVENT_FLAG */
+	"RSN IE sanity check failure",							/* IW_RSNIE_SANITY_FAIL_EVENT_FLAG */
+	"set key done in WPA/WPAPSK",							/* IW_SET_KEY_DONE_WPA1_EVENT_FLAG */
+	"set key done in WPA2/WPA2PSK",                         /* IW_SET_KEY_DONE_WPA2_EVENT_FLAG */
+	"connects with our wireless client",                    /* IW_STA_LINKUP_EVENT_FLAG */
+	"disconnects with our wireless client",                 /* IW_STA_LINKDOWN_EVENT_FLAG */
+	"scan completed"										/* IW_SCAN_COMPLETED_EVENT_FLAG */
+	"scan terminate!! Busy!! Enqueue fail!!"				/* IW_SCAN_ENQUEUE_FAIL_EVENT_FLAG */
+	};
+
+// for wireless IDS_spoof_attack event message
+char const *pWirelessSpoofEventText[IW_SPOOF_EVENT_TYPE_NUM] = {
+    "detected conflict SSID",								/* IW_CONFLICT_SSID_EVENT_FLAG */
+    "detected spoofed association response",				/* IW_SPOOF_ASSOC_RESP_EVENT_FLAG */
+    "detected spoofed reassociation responses",				/* IW_SPOOF_REASSOC_RESP_EVENT_FLAG */
+    "detected spoofed probe response",						/* IW_SPOOF_PROBE_RESP_EVENT_FLAG */
+    "detected spoofed beacon",								/* IW_SPOOF_BEACON_EVENT_FLAG */
+    "detected spoofed disassociation",						/* IW_SPOOF_DISASSOC_EVENT_FLAG */
+    "detected spoofed authentication",						/* IW_SPOOF_AUTH_EVENT_FLAG */
+    "detected spoofed deauthentication",					/* IW_SPOOF_DEAUTH_EVENT_FLAG */
+    "detected spoofed unknown management frame",			/* IW_SPOOF_UNKNOWN_MGMT_EVENT_FLAG */
+	"detected replay attack"								/* IW_REPLAY_ATTACK_EVENT_FLAG */
+	};
+
+// for wireless IDS_flooding_attack event message
+char const *pWirelessFloodEventText[IW_FLOOD_EVENT_TYPE_NUM] = {
+	"detected authentication flooding",						/* IW_FLOOD_AUTH_EVENT_FLAG */
+    "detected association request flooding",				/* IW_FLOOD_ASSOC_REQ_EVENT_FLAG */
+    "detected reassociation request flooding",				/* IW_FLOOD_REASSOC_REQ_EVENT_FLAG */
+    "detected probe request flooding",						/* IW_FLOOD_PROBE_REQ_EVENT_FLAG */
+    "detected disassociation flooding",						/* IW_FLOOD_DISASSOC_EVENT_FLAG */
+    "detected deauthentication flooding",					/* IW_FLOOD_DEAUTH_EVENT_FLAG */
+    "detected 802.1x eap-request flooding"					/* IW_FLOOD_EAP_REQ_EVENT_FLAG */
+	};
+
+/* timeout -- ms */
+VOID RTMP_SetPeriodicTimer(
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	unsigned long timeout)
+{
+	timeout = ((timeout*HZ) / 1000);
+	pTimer->expires = jiffies + timeout;
+	add_timer(pTimer);
+}
+
+/* convert NdisMInitializeTimer --> RTMP_OS_Init_Timer */
+VOID RTMP_OS_Init_Timer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	TIMER_FUNCTION function,
+	IN	PVOID data)
+{
+	init_timer(pTimer);
+    pTimer->data = (unsigned long)data;
+    pTimer->function = function;
+}
+
+
+VOID RTMP_OS_Add_Timer(
+	IN	NDIS_MINIPORT_TIMER		*pTimer,
+	IN	unsigned long timeout)
+{
+	if (timer_pending(pTimer))
+		return;
+
+	timeout = ((timeout*HZ) / 1000);
+	pTimer->expires = jiffies + timeout;
+	add_timer(pTimer);
+}
+
+VOID RTMP_OS_Mod_Timer(
+	IN	NDIS_MINIPORT_TIMER		*pTimer,
+	IN	unsigned long timeout)
+{
+	timeout = ((timeout*HZ) / 1000);
+	mod_timer(pTimer, jiffies + timeout);
+}
+
+VOID RTMP_OS_Del_Timer(
+	IN	NDIS_MINIPORT_TIMER		*pTimer,
+	OUT	BOOLEAN					*pCancelled)
+{
+	if (timer_pending(pTimer))
+	{
+		*pCancelled = del_timer_sync(pTimer);
+	}
+	else
+	{
+		*pCancelled = TRUE;
+	}
+
+}
+
+VOID RTMP_OS_Release_Packet(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PQUEUE_ENTRY  pEntry)
+{
+	//RTMPFreeNdisPacket(pAd, (struct sk_buff *)pEntry);
+}
+
+// Unify all delay routine by using udelay
+VOID RTMPusecDelay(
+	IN	ULONG	usec)
+{
+	ULONG	i;
+
+	for (i = 0; i < (usec / 50); i++)
+		udelay(50);
+
+	if (usec % 50)
+		udelay(usec % 50);
+}
+
+void RTMP_GetCurrentSystemTime(LARGE_INTEGER *time)
+{
+	time->u.LowPart = jiffies;
+}
+
+// pAd MUST allow to be NULL
+NDIS_STATUS os_alloc_mem(
+	IN	PRTMP_ADAPTER pAd,
+	OUT	PUCHAR *mem,
+	IN	ULONG  size)
+{
+	*mem = (PUCHAR) kmalloc(size, GFP_ATOMIC);
+	if (*mem)
+		return (NDIS_STATUS_SUCCESS);
+	else
+		return (NDIS_STATUS_FAILURE);
+}
+
+// pAd MUST allow to be NULL
+NDIS_STATUS os_free_mem(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PUCHAR mem)
+{
+
+	ASSERT(mem);
+	kfree(mem);
+	return (NDIS_STATUS_SUCCESS);
+}
+
+
+PNDIS_PACKET RTMP_AllocateFragPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length)
+{
+	struct sk_buff *pkt;
+
+	pkt = dev_alloc_skb(Length);
+
+	if (pkt == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("can't allocate frag rx %ld size packet\n",Length));
+	}
+
+	if (pkt)
+	{
+		RTMP_SET_PACKET_SOURCE(OSPKT_TO_RTPKT(pkt), PKTSRC_NDIS);
+	}
+
+	return (PNDIS_PACKET) pkt;
+}
+
+
+PNDIS_PACKET RTMP_AllocateTxPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress)
+{
+	struct sk_buff *pkt;
+
+	pkt = dev_alloc_skb(Length);
+
+	if (pkt == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("can't allocate tx %ld size packet\n",Length));
+	}
+
+	if (pkt)
+	{
+		RTMP_SET_PACKET_SOURCE(OSPKT_TO_RTPKT(pkt), PKTSRC_NDIS);
+		*VirtualAddress = (PVOID) pkt->data;
+	}
+	else
+	{
+		*VirtualAddress = (PVOID) NULL;
+	}
+
+	return (PNDIS_PACKET) pkt;
+}
+
+
+VOID build_tx_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR	pFrame,
+	IN	ULONG	FrameLen)
+{
+
+	struct sk_buff	*pTxPkt;
+
+	ASSERT(pPacket);
+	pTxPkt = RTPKT_TO_OSPKT(pPacket);
+
+	NdisMoveMemory(skb_put(pTxPkt, FrameLen), pFrame, FrameLen);
+}
+
+VOID	RTMPFreeAdapter(
+	IN	PRTMP_ADAPTER	pAd)
+{
+    POS_COOKIE os_cookie;
+	int index;
+
+	os_cookie=(POS_COOKIE)pAd->OS_Cookie;
+
+	kfree(pAd->BeaconBuf);
+
+
+	NdisFreeSpinLock(&pAd->MgmtRingLock);
+
+
+	for (index =0 ; index < NUM_OF_TX_RING; index++)
+	{
+    	NdisFreeSpinLock(&pAd->TxSwQueueLock[index]);
+		NdisFreeSpinLock(&pAd->DeQueueLock[index]);
+		pAd->DeQueueRunning[index] = FALSE;
+	}
+
+	NdisFreeSpinLock(&pAd->irq_lock);
+
+
+	vfree(pAd); // pci_free_consistent(os_cookie->pci_dev,sizeof(RTMP_ADAPTER),pAd,os_cookie->pAd_pa);
+	kfree(os_cookie);
+}
+
+BOOLEAN OS_Need_Clone_Packet(void)
+{
+	return (FALSE);
+}
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		clone an input NDIS PACKET to another one. The new internally created NDIS PACKET
+		must have only one NDIS BUFFER
+		return - byte copied. 0 means can't create NDIS PACKET
+		NOTE: internally created NDIS_PACKET should be destroyed by RTMPFreeNdisPacket
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pInsAMSDUHdr	EWC A-MSDU format has extra 14-bytes header. if TRUE, insert this 14-byte hdr in front of MSDU.
+		*pSrcTotalLen			return total packet length. This lenght is calculated with 802.3 format packet.
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS RTMPCloneNdisPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	BOOLEAN			pInsAMSDUHdr,
+	IN	PNDIS_PACKET	pInPacket,
+	OUT PNDIS_PACKET   *ppOutPacket)
+{
+
+	struct sk_buff *pkt;
+
+	ASSERT(pInPacket);
+	ASSERT(ppOutPacket);
+
+	// 1. Allocate a packet
+	pkt = dev_alloc_skb(2048);
+
+	if (pkt == NULL)
+	{
+		return NDIS_STATUS_FAILURE;
+	}
+
+ 	skb_put(pkt, GET_OS_PKT_LEN(pInPacket));
+	NdisMoveMemory(pkt->data, GET_OS_PKT_DATAPTR(pInPacket), GET_OS_PKT_LEN(pInPacket));
+	*ppOutPacket = OSPKT_TO_RTPKT(pkt);
+
+
+	RTMP_SET_PACKET_SOURCE(OSPKT_TO_RTPKT(pkt), PKTSRC_NDIS);
+
+	printk("###Clone###\n");
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+// the allocated NDIS PACKET must be freed via RTMPFreeNdisPacket()
+NDIS_STATUS RTMPAllocateNdisPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT PNDIS_PACKET   *ppPacket,
+	IN	PUCHAR			pHeader,
+	IN	UINT			HeaderLen,
+	IN	PUCHAR			pData,
+	IN	UINT			DataLen)
+{
+	PNDIS_PACKET	pPacket;
+	ASSERT(pData);
+	ASSERT(DataLen);
+
+	// 1. Allocate a packet
+	pPacket = (PNDIS_PACKET *) dev_alloc_skb(HeaderLen + DataLen + TXPADDING_SIZE);
+	if (pPacket == NULL)
+ 	{
+		*ppPacket = NULL;
+#ifdef DEBUG
+		printk("RTMPAllocateNdisPacket Fail\n\n");
+#endif
+		return NDIS_STATUS_FAILURE;
+	}
+
+	// 2. clone the frame content
+	if (HeaderLen > 0)
+		NdisMoveMemory(GET_OS_PKT_DATAPTR(pPacket), pHeader, HeaderLen);
+	if (DataLen > 0)
+		NdisMoveMemory(GET_OS_PKT_DATAPTR(pPacket) + HeaderLen, pData, DataLen);
+
+	// 3. update length of packet
+ 	skb_put(GET_OS_PKT_TYPE(pPacket), HeaderLen+DataLen);
+
+	RTMP_SET_PACKET_SOURCE(pPacket, PKTSRC_NDIS);
+//	printk("%s : pPacket = %p, len = %d\n", __func__, pPacket, GET_OS_PKT_LEN(pPacket));
+	*ppPacket = pPacket;
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*
+  ========================================================================
+  Description:
+	This routine frees a miniport internally allocated NDIS_PACKET and its
+	corresponding NDIS_BUFFER and allocated memory.
+  ========================================================================
+*/
+VOID RTMPFreeNdisPacket(
+	IN PRTMP_ADAPTER pAd,
+	IN PNDIS_PACKET  pPacket)
+{
+	dev_kfree_skb_any(RTPKT_TO_OSPKT(pPacket));
+}
+
+
+// IRQL = DISPATCH_LEVEL
+// NOTE: we do have an assumption here, that Byte0 and Byte1 always reasid at the same
+//			 scatter gather buffer
+NDIS_STATUS Sniff2BytesFromNdisBuffer(
+	IN	PNDIS_BUFFER	pFirstBuffer,
+	IN	UCHAR			DesiredOffset,
+	OUT PUCHAR			pByte0,
+	OUT PUCHAR			pByte1)
+{
+    *pByte0 = *(PUCHAR)(pFirstBuffer + DesiredOffset);
+    *pByte1 = *(PUCHAR)(pFirstBuffer + DesiredOffset + 1);
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+void RTMP_QueryPacketInfo(
+	IN  PNDIS_PACKET pPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen)
+{
+	pPacketInfo->BufferCount = 1;
+	pPacketInfo->pFirstBuffer = GET_OS_PKT_DATAPTR(pPacket);
+	pPacketInfo->PhysicalBufferCount = 1;
+	pPacketInfo->TotalPacketLength = GET_OS_PKT_LEN(pPacket);
+
+	*pSrcBufVA = GET_OS_PKT_DATAPTR(pPacket);
+	*pSrcBufLen = GET_OS_PKT_LEN(pPacket);
+}
+
+void RTMP_QueryNextPacketInfo(
+	IN  PNDIS_PACKET *ppPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen)
+{
+	PNDIS_PACKET pPacket = NULL;
+
+	if (*ppPacket)
+		pPacket = GET_OS_PKT_NEXT(*ppPacket);
+
+	if (pPacket)
+	{
+		pPacketInfo->BufferCount = 1;
+		pPacketInfo->pFirstBuffer = GET_OS_PKT_DATAPTR(pPacket);
+		pPacketInfo->PhysicalBufferCount = 1;
+		pPacketInfo->TotalPacketLength = GET_OS_PKT_LEN(pPacket);
+
+		*pSrcBufVA = GET_OS_PKT_DATAPTR(pPacket);
+		*pSrcBufLen = GET_OS_PKT_LEN(pPacket);
+		*ppPacket = GET_OS_PKT_NEXT(pPacket);
+	}
+	else
+	{
+		pPacketInfo->BufferCount = 0;
+		pPacketInfo->pFirstBuffer = NULL;
+		pPacketInfo->PhysicalBufferCount = 0;
+		pPacketInfo->TotalPacketLength = 0;
+
+		*pSrcBufVA = NULL;
+		*pSrcBufLen = 0;
+		*ppPacket = NULL;
+	}
+}
+
+// not yet support MBSS
+PNET_DEV get_netdev_from_bssid(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			FromWhichBSSID)
+{
+    PNET_DEV dev_p = NULL;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		dev_p = pAd->net_dev;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	ASSERT(dev_p);
+	return dev_p; /* return one of MBSS */
+}
+
+PNDIS_PACKET DuplicatePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*skb;
+	PNDIS_PACKET	pRetPacket = NULL;
+	USHORT			DataSize;
+	UCHAR			*pData;
+
+	DataSize = (USHORT) GET_OS_PKT_LEN(pPacket);
+	pData = (PUCHAR) GET_OS_PKT_DATAPTR(pPacket);
+
+
+	skb = skb_clone(RTPKT_TO_OSPKT(pPacket), MEM_ALLOC_FLAG);
+	if (skb)
+	{
+		skb->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+		pRetPacket = OSPKT_TO_RTPKT(skb);
+	}
+
+#if 0
+	if ((skb = __dev_alloc_skb(DataSize + 2+32, MEM_ALLOC_FLAG)) != NULL)
+	{
+		skb_reserve(skb, 2+32);
+		NdisMoveMemory(skb->tail, pData, DataSize);
+		skb_put(skb, DataSize);
+		skb->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+		pRetPacket = OSPKT_TO_RTPKT(skb);
+	}
+#endif
+
+	return pRetPacket;
+
+}
+
+PNDIS_PACKET duplicate_pkt(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+    IN  UINT            HdrLen,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN	UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*skb;
+	PNDIS_PACKET	pPacket = NULL;
+
+
+	if ((skb = __dev_alloc_skb(HdrLen + DataSize + 2, MEM_ALLOC_FLAG)) != NULL)
+	{
+		skb_reserve(skb, 2);
+		NdisMoveMemory(skb->tail, pHeader802_3, HdrLen);
+		skb_put(skb, HdrLen);
+		NdisMoveMemory(skb->tail, pData, DataSize);
+		skb_put(skb, DataSize);
+		skb->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+		pPacket = OSPKT_TO_RTPKT(skb);
+	}
+
+	return pPacket;
+}
+
+
+#define TKIP_TX_MIC_SIZE		8
+PNDIS_PACKET duplicate_pkt_with_TKIP_MIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	struct sk_buff	*skb, *newskb;
+
+
+	skb = RTPKT_TO_OSPKT(pPacket);
+	if (skb_tailroom(skb) < TKIP_TX_MIC_SIZE)
+	{
+		// alloc a new skb and copy the packet
+		newskb = skb_copy_expand(skb, skb_headroom(skb), TKIP_TX_MIC_SIZE, GFP_ATOMIC);
+		dev_kfree_skb_any(skb);
+		if (newskb == NULL)
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Extend Tx.MIC for packet failed!, dropping packet!\n"));
+			return NULL;
+		}
+		skb = newskb;
+	}
+
+	return OSPKT_TO_RTPKT(skb);
+
+#if 0
+	if ((data = skb_put(skb, TKIP_TX_MIC_SIZE)) != NULL)
+	{	// If we can extend it, well, copy it first.
+		NdisMoveMemory(data, pAd->PrivateInfo.Tx.MIC, TKIP_TX_MIC_SIZE);
+	}
+	else
+	{
+		// Otherwise, copy the packet.
+		newskb = skb_copy_expand(skb, skb_headroom(skb), TKIP_TX_MIC_SIZE, GFP_ATOMIC);
+		dev_kfree_skb_any(skb);
+		if (newskb == NULL)
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Extend Tx.MIC to packet failed!, dropping packet\n"));
+			return NULL;
+		}
+		skb = newskb;
+
+		NdisMoveMemory(skb->tail, pAd->PrivateInfo.Tx.MIC, TKIP_TX_MIC_SIZE);
+		skb_put(skb, TKIP_TX_MIC_SIZE);
+	}
+
+	return OSPKT_TO_RTPKT(skb);
+#endif
+
+}
+
+
+
+
+PNDIS_PACKET ClonePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize)
+{
+	struct sk_buff	*pRxPkt;
+	struct sk_buff	*pClonedPkt;
+
+	ASSERT(pPacket);
+	pRxPkt = RTPKT_TO_OSPKT(pPacket);
+
+	// clone the packet
+	pClonedPkt = skb_clone(pRxPkt, MEM_ALLOC_FLAG);
+
+	if (pClonedPkt)
+	{
+    	// set the correct dataptr and data len
+    	pClonedPkt->dev = pRxPkt->dev;
+    	pClonedPkt->data = pData;
+    	pClonedPkt->len = DataSize;
+    	pClonedPkt->tail = pClonedPkt->data + pClonedPkt->len;
+		ASSERT(DataSize < 1530);
+	}
+	return pClonedPkt;
+}
+
+//
+// change OS packet DataPtr and DataLen
+//
+void  update_os_packet_info(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN  UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*pOSPkt;
+
+	ASSERT(pRxBlk->pRxPacket);
+	pOSPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+
+	pOSPkt->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+	pOSPkt->data = pRxBlk->pData;
+	pOSPkt->len = pRxBlk->DataSize;
+	pOSPkt->tail = pOSPkt->data + pOSPkt->len;
+}
+
+
+void wlan_802_11_to_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	PUCHAR			pHeader802_3,
+	IN  UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*pOSPkt;
+
+	ASSERT(pRxBlk->pRxPacket);
+	ASSERT(pHeader802_3);
+
+	pOSPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+
+	pOSPkt->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+	pOSPkt->data = pRxBlk->pData;
+	pOSPkt->len = pRxBlk->DataSize;
+	pOSPkt->tail = pOSPkt->data + pOSPkt->len;
+
+	//
+	// copy 802.3 header
+	//
+	//
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		NdisMoveMemory(skb_push(pOSPkt, LENGTH_802_3), pHeader802_3, LENGTH_802_3);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+
+void announce_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+
+	struct sk_buff	*pRxPkt;
+
+	ASSERT(pPacket);
+
+	pRxPkt = RTPKT_TO_OSPKT(pPacket);
+
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+
+    /* Push up the protocol stack */
+#ifdef IKANOS_VX_1X0
+	IKANOS_DataFrameRx(pAd, pRxPkt->dev, pRxPkt, pRxPkt->len);
+#else
+	pRxPkt->protocol = eth_type_trans(pRxPkt, pRxPkt->dev);
+
+//#ifdef CONFIG_5VT_ENHANCE
+//	*(int*)(pRxPkt->cb) = BRIDGE_TAG;
+//#endif
+	netif_rx(pRxPkt);
+#endif // IKANOS_VX_1X0 //
+}
+
+
+PRTMP_SCATTER_GATHER_LIST
+rt_get_sg_list_from_packet(PNDIS_PACKET pPacket, RTMP_SCATTER_GATHER_LIST *sg)
+{
+	sg->NumberOfElements = 1;
+	sg->Elements[0].Address =  GET_OS_PKT_DATAPTR(pPacket);
+	sg->Elements[0].Length = GET_OS_PKT_LEN(pPacket);
+	return (sg);
+}
+
+void hex_dump(char *str, unsigned char *pSrcBufVA, unsigned int SrcBufLen)
+{
+	unsigned char *pt;
+	int x;
+
+	if (RTDebugLevel < RT_DEBUG_TRACE)
+		return;
+
+	pt = pSrcBufVA;
+	printk("%s: %p, len = %d\n",str,  pSrcBufVA, SrcBufLen);
+	for (x=0; x<SrcBufLen; x++)
+	{
+		if (x % 16 == 0)
+			printk("0x%04x : ", x);
+		printk("%02x ", ((unsigned char)pt[x]));
+		if (x%16 == 15) printk("\n");
+	}
+	printk("\n");
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Send log message through wireless event
+
+		Support standard iw_event with IWEVCUSTOM. It is used below.
+
+		iwreq_data.data.flags is used to store event_flag that is defined by user.
+		iwreq_data.data.length is the length of the event log.
+
+		The format of the event log is composed of the entry's MAC address and
+		the desired log message (refer to pWirelessEventText).
+
+			ex: 11:22:33:44:55:66 has associated successfully
+
+		p.s. The requirement of Wireless Extension is v15 or newer.
+
+	========================================================================
+*/
+VOID RTMPSendWirelessEvent(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Event_flag,
+	IN	PUCHAR 			pAddr,
+	IN	UCHAR			BssIdx,
+	IN	CHAR			Rssi)
+{
+#if WIRELESS_EXT >= 15
+
+	union 	iwreq_data      wrqu;
+	PUCHAR 	pBuf = NULL, pBufPtr = NULL;
+	USHORT	event, type, BufLen;
+	UCHAR	event_table_len = 0;
+
+	type = Event_flag & 0xFF00;
+	event = Event_flag & 0x00FF;
+
+	switch (type)
+	{
+		case IW_SYS_EVENT_FLAG_START:
+			event_table_len = IW_SYS_EVENT_TYPE_NUM;
+			break;
+
+		case IW_SPOOF_EVENT_FLAG_START:
+			event_table_len = IW_SPOOF_EVENT_TYPE_NUM;
+			break;
+
+		case IW_FLOOD_EVENT_FLAG_START:
+			event_table_len = IW_FLOOD_EVENT_TYPE_NUM;
+			break;
+	}
+
+	if (event_table_len == 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s : The type(%0x02x) is not valid.\n", __func__, type));
+		return;
+	}
+
+	if (event >= event_table_len)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s : The event(%0x02x) is not valid.\n", __func__, event));
+		return;
+	}
+
+	//Allocate memory and copy the msg.
+	if((pBuf = kmalloc(IW_CUSTOM_MAX_LEN, GFP_ATOMIC)) != NULL)
+	{
+		//Prepare the payload
+		memset(pBuf, 0, IW_CUSTOM_MAX_LEN);
+
+		pBufPtr = pBuf;
+
+		if (pAddr)
+			pBufPtr += sprintf(pBufPtr, "(RT2860) STA(%02x:%02x:%02x:%02x:%02x:%02x) ", PRINT_MAC(pAddr));
+		else if (BssIdx < MAX_MBSSID_NUM)
+			pBufPtr += sprintf(pBufPtr, "(RT2860) BSS(ra%d) ", BssIdx);
+		else
+			pBufPtr += sprintf(pBufPtr, "(RT2860) ");
+
+		if (type == IW_SYS_EVENT_FLAG_START)
+			pBufPtr += sprintf(pBufPtr, "%s", pWirelessSysEventText[event]);
+		else if (type == IW_SPOOF_EVENT_FLAG_START)
+			pBufPtr += sprintf(pBufPtr, "%s (RSSI=%d)", pWirelessSpoofEventText[event], Rssi);
+		else if (type == IW_FLOOD_EVENT_FLAG_START)
+			pBufPtr += sprintf(pBufPtr, "%s", pWirelessFloodEventText[event]);
+		else
+			pBufPtr += sprintf(pBufPtr, "%s", "unknown event");
+
+		pBufPtr[pBufPtr - pBuf] = '\0';
+		BufLen = pBufPtr - pBuf;
+
+		memset(&wrqu, 0, sizeof(wrqu));
+	    wrqu.data.flags = Event_flag;
+		wrqu.data.length = BufLen;
+
+		//send wireless event
+	    wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, pBuf);
+
+		//DBGPRINT(RT_DEBUG_TRACE, ("%s : %s\n", __func__, pBuf));
+
+		kfree(pBuf);
+	}
+	else
+		DBGPRINT(RT_DEBUG_ERROR, ("%s : Can't allocate memory for wireless event.\n", __func__));
+#else
+	DBGPRINT(RT_DEBUG_ERROR, ("%s : The Wireless Extension MUST be v15 or newer.\n", __func__));
+#endif  /* WIRELESS_EXT >= 15 */
+}
+
+
+#ifdef CONFIG_STA_SUPPORT
+void send_monitor_packets(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+    struct sk_buff	*pOSPkt;
+    wlan_ng_prism2_header *ph;
+    int rate_index = 0;
+    USHORT header_len = 0;
+    UCHAR temp_header[40] = {0};
+
+    u_int32_t ralinkrate[256] = {2,4,11,22, 12,18,24,36,48,72,96,  108,   109, 110, 111, 112, 13, 26, 39, 52,78,104, 117, 130, 26, 52, 78,104, 156, 208, 234, 260, 27, 54,81,108,162, 216, 243, 270, // Last 38
+	54, 108, 162, 216, 324, 432, 486, 540,  14, 29, 43, 57, 87, 115, 130, 144, 29, 59,87,115, 173, 230,260, 288, 30, 60,90,120,180,240,270,300,60,120,180,240,360,480,540,600, 0,1,2,3,4,5,6,7,8,9,10,
+	11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80};
+
+
+    ASSERT(pRxBlk->pRxPacket);
+    if (pRxBlk->DataSize < 10)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("%s : Size is too small! (%d)\n", __func__, pRxBlk->DataSize));
+		goto err_free_sk_buff;
+    }
+
+    if (pRxBlk->DataSize + sizeof(wlan_ng_prism2_header) > RX_BUFFER_AGGRESIZE)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("%s : Size is too large! (%d)\n", __func__, pRxBlk->DataSize + sizeof(wlan_ng_prism2_header)));
+		goto err_free_sk_buff;
+    }
+
+    pOSPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+	pOSPkt->dev = get_netdev_from_bssid(pAd, BSS0);
+    if (pRxBlk->pHeader->FC.Type == BTYPE_DATA)
+    {
+        pRxBlk->DataSize -= LENGTH_802_11;
+        if ((pRxBlk->pHeader->FC.ToDs == 1) &&
+            (pRxBlk->pHeader->FC.FrDs == 1))
+            header_len = LENGTH_802_11_WITH_ADDR4;
+        else
+            header_len = LENGTH_802_11;
+
+        // QOS
+    	if (pRxBlk->pHeader->FC.SubType & 0x08)
+    	{
+    	    header_len += 2;
+    		// Data skip QOS contorl field
+    		pRxBlk->DataSize -=2;
+    	}
+
+    	// Order bit: A-Ralink or HTC+
+    	if (pRxBlk->pHeader->FC.Order)
+    	{
+    	    header_len += 4;
+			// Data skip HTC contorl field
+			pRxBlk->DataSize -= 4;
+    	}
+
+        // Copy Header
+        if (header_len <= 40)
+            NdisMoveMemory(temp_header, pRxBlk->pData, header_len);
+
+        // skip HW padding
+    	if (pRxBlk->RxD.L2PAD)
+    	    pRxBlk->pData += (header_len + 2);
+        else
+            pRxBlk->pData += header_len;
+    } //end if
+
+
+	if (pRxBlk->DataSize < pOSPkt->len) {
+        skb_trim(pOSPkt,pRxBlk->DataSize);
+    } else {
+        skb_put(pOSPkt,(pRxBlk->DataSize - pOSPkt->len));
+    } //end if
+
+    if ((pRxBlk->pData - pOSPkt->data) > 0) {
+	    skb_put(pOSPkt,(pRxBlk->pData - pOSPkt->data));
+	    skb_pull(pOSPkt,(pRxBlk->pData - pOSPkt->data));
+    } //end if
+
+    if (skb_headroom(pOSPkt) < (sizeof(wlan_ng_prism2_header)+ header_len)) {
+        if (pskb_expand_head(pOSPkt, (sizeof(wlan_ng_prism2_header) + header_len), 0, GFP_ATOMIC)) {
+	        DBGPRINT(RT_DEBUG_ERROR, ("%s : Reallocate header size of sk_buff fail!\n", __func__));
+			goto err_free_sk_buff;
+	    } //end if
+    } //end if
+
+    if (header_len > 0)
+        NdisMoveMemory(skb_push(pOSPkt, header_len), temp_header, header_len);
+
+    ph = (wlan_ng_prism2_header *) skb_push(pOSPkt, sizeof(wlan_ng_prism2_header));
+	NdisZeroMemory(ph, sizeof(wlan_ng_prism2_header));
+
+    ph->msgcode		    = DIDmsg_lnxind_wlansniffrm;
+	ph->msglen		    = sizeof(wlan_ng_prism2_header);
+	strcpy(ph->devname, pAd->net_dev->name);
+
+    ph->hosttime.did = DIDmsg_lnxind_wlansniffrm_hosttime;
+	ph->hosttime.status = 0;
+	ph->hosttime.len = 4;
+	ph->hosttime.data = jiffies;
+
+	ph->mactime.did = DIDmsg_lnxind_wlansniffrm_mactime;
+	ph->mactime.status = 0;
+	ph->mactime.len = 0;
+	ph->mactime.data = 0;
+
+    ph->istx.did = DIDmsg_lnxind_wlansniffrm_istx;
+	ph->istx.status = 0;
+	ph->istx.len = 0;
+	ph->istx.data = 0;
+
+    ph->channel.did = DIDmsg_lnxind_wlansniffrm_channel;
+	ph->channel.status = 0;
+	ph->channel.len = 4;
+
+    ph->channel.data = (u_int32_t)pAd->CommonCfg.Channel;
+
+    ph->rssi.did = DIDmsg_lnxind_wlansniffrm_rssi;
+	ph->rssi.status = 0;
+	ph->rssi.len = 4;
+    ph->rssi.data = (u_int32_t)RTMPMaxRssi(pAd, ConvertToRssi(pAd, pRxBlk->pRxWI->RSSI0, RSSI_0), ConvertToRssi(pAd, pRxBlk->pRxWI->RSSI1, RSSI_1), ConvertToRssi(pAd, pRxBlk->pRxWI->RSSI2, RSSI_2));;
+
+	ph->signal.did = DIDmsg_lnxind_wlansniffrm_signal;
+	ph->signal.status = 0;
+	ph->signal.len = 4;
+	ph->signal.data = 0; //rssi + noise;
+
+	ph->noise.did = DIDmsg_lnxind_wlansniffrm_noise;
+	ph->noise.status = 0;
+	ph->noise.len = 4;
+	ph->noise.data = 0;
+
+#ifdef DOT11_N_SUPPORT
+    if (pRxBlk->pRxWI->PHYMODE >= MODE_HTMIX)
+    {
+    	rate_index = 16 + ((UCHAR)pRxBlk->pRxWI->BW *16) + ((UCHAR)pRxBlk->pRxWI->ShortGI *32) + ((UCHAR)pRxBlk->pRxWI->MCS);
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+	if (pRxBlk->pRxWI->PHYMODE == MODE_OFDM)
+    	rate_index = (UCHAR)(pRxBlk->pRxWI->MCS) + 4;
+    else
+    	rate_index = (UCHAR)(pRxBlk->pRxWI->MCS);
+    if (rate_index < 0)
+        rate_index = 0;
+    if (rate_index > 255)
+        rate_index = 255;
+
+	ph->rate.did = DIDmsg_lnxind_wlansniffrm_rate;
+	ph->rate.status = 0;
+	ph->rate.len = 4;
+    ph->rate.data = ralinkrate[rate_index];
+
+	ph->frmlen.did = DIDmsg_lnxind_wlansniffrm_frmlen;
+    ph->frmlen.status = 0;
+	ph->frmlen.len = 4;
+	ph->frmlen.data	= (u_int32_t)pRxBlk->DataSize;
+
+
+    pOSPkt->pkt_type = PACKET_OTHERHOST;
+    pOSPkt->protocol = eth_type_trans(pOSPkt, pOSPkt->dev);
+    pOSPkt->ip_summed = CHECKSUM_NONE;
+    netif_rx(pOSPkt);
+
+    return;
+
+err_free_sk_buff:
+	RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	return;
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+void rtmp_os_thread_init(PUCHAR pThreadName, PVOID pNotify)
+{
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	daemonize(pThreadName /*"%s",pAd->net_dev->name*/);
+
+	allow_signal(SIGTERM);
+	allow_signal(SIGKILL);
+	current->flags |= PF_NOFREEZE;
+#else
+	unsigned long flags;
+
+	daemonize();
+	reparent_to_init();
+	strcpy(current->comm, pThreadName);
+
+	siginitsetinv(&current->blocked, sigmask(SIGTERM) | sigmask(SIGKILL));
+
+	/* Allow interception of SIGKILL only
+	 * Don't allow other signals to interrupt the transmission */
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,4,22)
+	spin_lock_irqsave(&current->sigmask_lock, flags);
+	flush_signals(current);
+	recalc_sigpending(current);
+	spin_unlock_irqrestore(&current->sigmask_lock, flags);
+#endif
+#endif
+
+    /* signal that we've started the thread */
+	complete(pNotify);
+
+}
+
+void RTMP_IndicateMediaState(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	if (pAd->CommonCfg.bWirelessEvent)
+	{
+		if (pAd->IndicateMediaState == NdisMediaStateConnected)
+		{
+			RTMPSendWirelessEvent(pAd, IW_STA_LINKUP_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+		else
+		{
+			RTMPSendWirelessEvent(pAd, IW_STA_LINKDOWN_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+	}
+}
+
diff --git a/drivers/staging/rt2870/rt_linux.h b/drivers/staging/rt2870/rt_linux.h
new file mode 100644
index 000000000000..859f9cef0a19
--- /dev/null
+++ b/drivers/staging/rt2870/rt_linux.h
@@ -0,0 +1,908 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+/***********************************************************************/
+/*                                                                     */
+/*   Program:    rt_linux.c                                            */
+/*   Created:    4/21/2006 1:17:38 PM                                  */
+/*   Author:     Wu Xi-Kun                                             */
+/*   Comments:   `description`                                         */
+/*                                                                     */
+/*---------------------------------------------------------------------*/
+/*                                                                     */
+/* History:                                                            */
+/*    Revision 1.1 4/21/2006 1:17:38 PM  xsikun                        */
+/*    Initial revision                                                 */
+/*                                                                     */
+/***********************************************************************/
+
+#include "rtmp_type.h"
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/kernel.h>
+
+#include <linux/spinlock.h>
+#include <linux/init.h>
+#include <linux/string.h>
+#include <linux/timer.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/skbuff.h>
+#include <linux/ethtool.h>
+#include <linux/wireless.h>
+#include <linux/proc_fs.h>
+#include <linux/delay.h>
+#include <linux/if_arp.h>
+#include <linux/ctype.h>
+#include <linux/vmalloc.h>
+
+
+#include <linux/wireless.h>
+#include <net/iw_handler.h>
+
+// load firmware
+#define __KERNEL_SYSCALLS__
+#include <linux/unistd.h>
+#include <asm/uaccess.h>
+
+
+#define MEM_ALLOC_FLAG      (GFP_ATOMIC) //(GFP_DMA | GFP_ATOMIC)
+
+#ifndef IFNAMSIZ
+#define IFNAMSIZ 16
+#endif
+
+//#define CONFIG_CKIP_SUPPORT
+
+#undef __inline
+#define __inline	   static inline
+
+typedef int (*HARD_START_XMIT_FUNC)(struct sk_buff *skb, struct net_device *net_dev);
+
+// add by kathy
+
+#ifdef CONFIG_STA_SUPPORT
+
+#ifdef RT2870
+#define STA_PROFILE_PATH			"/etc/Wireless/RT2870STA/RT2870STA.dat"
+#define STA_RT2870_IMAGE_FILE_NAME  "/etc/Wireless/RT2870STA/rt2870.bin"
+#define STA_NIC_DEVICE_NAME			"RT2870STA"
+#define STA_DRIVER_VERSION			"1.4.0.0"
+#ifdef MULTIPLE_CARD_SUPPORT
+#define CARD_INFO_PATH			"/etc/Wireless/RT2870STA/RT2870STACard.dat"
+#endif // MULTIPLE_CARD_SUPPORT //
+#endif // RT2870 //
+
+#endif // CONFIG_STA_SUPPORT //
+
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+
+#define RTMP_TIME_AFTER(a,b)		\
+	(typecheck(unsigned long, (unsigned long)a) && \
+	 typecheck(unsigned long, (unsigned long)b) && \
+	 ((long)(b) - (long)(a) < 0))
+
+#define RTMP_TIME_AFTER_EQ(a,b)	\
+	(typecheck(unsigned long, (unsigned long)a) && \
+	 typecheck(unsigned long, (unsigned long)b) && \
+	 ((long)(a) - (long)(b) >= 0))
+#define RTMP_TIME_BEFORE(a,b)	RTMP_TIME_AFTER_EQ(b,a)
+#else
+#define RTMP_TIME_AFTER(a,b) time_after(a, b)
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#define RT_MOD_INC_USE_COUNT() \
+	if (!try_module_get(THIS_MODULE)) \
+	{ \
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: cannot reserve module\n", __func__)); \
+		return -1; \
+	}
+
+#define RT_MOD_DEC_USE_COUNT() module_put(THIS_MODULE);
+#else
+#define RT_MOD_INC_USE_COUNT()	MOD_INC_USE_COUNT;
+#define RT_MOD_DEC_USE_COUNT() MOD_DEC_USE_COUNT;
+#endif
+
+#define OS_HZ			HZ
+
+#define ETH_LENGTH_OF_ADDRESS	6
+
+#define IN
+#define OUT
+
+#define NDIS_STATUS                             INT
+#define NDIS_STATUS_SUCCESS                     0x00
+#define NDIS_STATUS_FAILURE                     0x01
+#define NDIS_STATUS_INVALID_DATA				0x02
+#define NDIS_STATUS_RESOURCES                   0x03
+
+#define MIN_NET_DEVICE_FOR_AID			0x00		//0x00~0x3f
+#define MIN_NET_DEVICE_FOR_MBSSID		0x00		//0x00,0x10,0x20,0x30
+#define MIN_NET_DEVICE_FOR_WDS			0x10		//0x40,0x50,0x60,0x70
+#define MIN_NET_DEVICE_FOR_APCLI		0x20
+#define MIN_NET_DEVICE_FOR_MESH			0x30
+#ifdef CONFIG_STA_SUPPORT
+#define MIN_NET_DEVICE_FOR_DLS			0x40
+#endif // CONFIG_STA_SUPPORT //
+
+
+#ifdef CONFIG_STA_SUPPORT
+#define NDIS_PACKET_TYPE_DIRECTED		0
+#define NDIS_PACKET_TYPE_MULTICAST		1
+#define NDIS_PACKET_TYPE_BROADCAST		2
+#define NDIS_PACKET_TYPE_ALL_MULTICAST	3
+#endif // CONFIG_STA_SUPPORT //
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+typedef	struct pid *	THREAD_PID;
+#define	THREAD_PID_INIT_VALUE	NULL
+#define	GET_PID(_v)	find_get_pid(_v)
+#define	GET_PID_NUMBER(_v)	pid_nr(_v)
+#define CHECK_PID_LEGALITY(_pid)	if (pid_nr(_pid) >= 0)
+#define KILL_THREAD_PID(_A, _B, _C)	kill_pid(_A, _B, _C)
+#else
+typedef	pid_t	THREAD_PID;
+#define	THREAD_PID_INIT_VALUE	-1
+#define	GET_PID(_v)	_v
+#define	GET_PID_NUMBER(_v)	_v
+#define CHECK_PID_LEGALITY(_pid)	if (_pid >= 0)
+#define KILL_THREAD_PID(_A, _B, _C)	kill_proc(_A, _B, _C)
+#endif
+
+struct os_lock  {
+	spinlock_t		lock;
+	unsigned long  	flags;
+};
+
+
+struct os_cookie {
+
+#ifdef RT2870
+	struct usb_device		*pUsb_Dev;
+
+	THREAD_PID				MLMEThr_pid;
+	THREAD_PID				RTUSBCmdThr_pid;
+	THREAD_PID				TimerQThr_pid;
+#endif // RT2870 //
+
+	struct tasklet_struct 	rx_done_task;
+	struct tasklet_struct 	mgmt_dma_done_task;
+	struct tasklet_struct 	ac0_dma_done_task;
+	struct tasklet_struct 	ac1_dma_done_task;
+	struct tasklet_struct 	ac2_dma_done_task;
+	struct tasklet_struct 	ac3_dma_done_task;
+	struct tasklet_struct 	hcca_dma_done_task;
+	struct tasklet_struct	tbtt_task;
+#ifdef RT2870
+	struct tasklet_struct	null_frame_complete_task;
+	struct tasklet_struct	rts_frame_complete_task;
+	struct tasklet_struct	pspoll_frame_complete_task;
+#endif // RT2870 //
+
+
+	unsigned long			apd_pid; //802.1x daemon pid
+	INT						ioctl_if_type;
+	INT 					ioctl_if;
+};
+
+typedef struct _VIRTUAL_ADAPTER
+{
+	struct net_device		*RtmpDev;
+	struct net_device		*VirtualDev;
+} VIRTUAL_ADAPTER, PVIRTUAL_ADAPTER;
+
+#undef  ASSERT
+#define ASSERT(x)                                                               \
+{                                                                               \
+    if (!(x))                                                                   \
+    {                                                                           \
+        printk(KERN_WARNING __FILE__ ":%d assert " #x "failed\n", __LINE__);    \
+    }                                                                           \
+}
+
+typedef struct os_cookie	* POS_COOKIE;
+typedef struct pci_dev 		* PPCI_DEV;
+typedef struct net_device	* PNET_DEV;
+typedef void				* PNDIS_PACKET;
+typedef char				NDIS_PACKET;
+typedef PNDIS_PACKET		* PPNDIS_PACKET;
+typedef	dma_addr_t			NDIS_PHYSICAL_ADDRESS;
+typedef	dma_addr_t			* PNDIS_PHYSICAL_ADDRESS;
+//typedef struct timer_list	RALINK_TIMER_STRUCT;
+//typedef struct timer_list	* PRALINK_TIMER_STRUCT;
+//typedef struct os_lock		NDIS_SPIN_LOCK;
+typedef spinlock_t			NDIS_SPIN_LOCK;
+typedef struct timer_list	NDIS_MINIPORT_TIMER;
+typedef void				* NDIS_HANDLE;
+typedef char 				* PNDIS_BUFFER;
+
+
+
+void hex_dump(char *str, unsigned char *pSrcBufVA, unsigned int SrcBufLen);
+
+dma_addr_t linux_pci_map_single(void *handle, void *ptr, size_t size, int sd_idx, int direction);
+void linux_pci_unmap_single(void *handle, dma_addr_t dma_addr, size_t size, int direction);
+
+
+////////////////////////////////////////
+// MOVE TO rtmp.h ?
+/////////////////////////////////////////
+#define PKTSRC_NDIS             0x7f
+#define PKTSRC_DRIVER           0x0f
+#define PRINT_MAC(addr)	\
+	addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]
+
+
+#define RT2860_PCI_DEVICE_ID		0x0601
+
+
+#ifdef RT2870
+#define PCI_MAP_SINGLE(_handle, _ptr, _size, _dir) (ULONG)0
+
+#define PCI_UNMAP_SINGLE(_handle, _ptr, _size, _dir)
+#endif // RT2870 //
+
+
+#define BEACON_FRAME_DMA_CACHE_WBACK(_ptr, _size)	\
+	dma_cache_wback(_ptr, _size)
+
+
+//////////////////////////////////////////
+//
+//////////////////////////////////////////
+
+
+#define NdisMIndicateStatus(_w, _x, _y, _z)
+
+typedef struct timer_list	RTMP_OS_TIMER;
+
+#ifdef RT2870
+/* ----------------- Timer Related MARCO ---------------*/
+// In RT2870, we have a lot of timer functions and will read/write register, it's
+//	not allowed in Linux USB sub-system to do it ( because of sleep issue when submit
+//  to ctrl pipe). So we need a wrapper function to take care it.
+
+typedef VOID (*RT2870_TIMER_HANDLE)(
+	IN  PVOID   SystemSpecific1,
+	IN  PVOID   FunctionContext,
+	IN  PVOID   SystemSpecific2,
+	IN  PVOID   SystemSpecific3);
+#endif // RT2870 //
+
+
+typedef struct  _RALINK_TIMER_STRUCT    {
+    RTMP_OS_TIMER		TimerObj;       // Ndis Timer object
+	BOOLEAN				Valid;			// Set to True when call RTMPInitTimer
+    BOOLEAN             State;          // True if timer cancelled
+    BOOLEAN	      		PeriodicType;	// True if timer is periodic timer
+    BOOLEAN             Repeat;         // True if periodic timer
+    ULONG               TimerValue;     // Timer value in milliseconds
+	ULONG				cookie;			// os specific object
+#ifdef RT2870
+	RT2870_TIMER_HANDLE	handle;
+	void				*pAd;
+#endif // RT2870 //
+}   RALINK_TIMER_STRUCT, *PRALINK_TIMER_STRUCT;
+
+
+#ifdef RT2870
+
+typedef enum _RT2870_KERNEL_THREAD_STATUS_
+{
+	RT2870_THREAD_UNKNOWN = 0,
+	RT2870_THREAD_INITED = 1,
+	RT2870_THREAD_RUNNING = 2,
+	RT2870_THREAD_STOPED = 4,
+}RT2870_KERNEL_THREAD_STATUS;
+
+#define RT2870_THREAD_CAN_DO_INSERT		(RT2870_THREAD_INITED |RT2870_THREAD_RUNNING)
+
+typedef struct _RT2870_TIMER_ENTRY_
+{
+	RALINK_TIMER_STRUCT 			*pRaTimer;
+	struct _RT2870_TIMER_ENTRY_ 	*pNext;
+}RT2870_TIMER_ENTRY;
+
+
+#define TIMER_QUEUE_SIZE_MAX	128
+typedef struct _RT2870_TIMER_QUEUE_
+{
+	unsigned int		status;
+	//wait_queue_head_t 	timerWaitQ;
+	//atomic_t			count;
+	UCHAR				*pTimerQPoll;
+	RT2870_TIMER_ENTRY	*pQPollFreeList;
+	RT2870_TIMER_ENTRY 	*pQHead;
+	RT2870_TIMER_ENTRY 	*pQTail;
+}RT2870_TIMER_QUEUE;
+#endif // RT2870 //
+
+
+//#define DBG	1
+
+//
+//  MACRO for debugging information
+//
+
+#ifdef DBG
+extern ULONG    RTDebugLevel;
+
+#define DBGPRINT_RAW(Level, Fmt)    \
+{                                   \
+    if (Level <= RTDebugLevel)      \
+    {                               \
+        printk Fmt;               \
+    }                               \
+}
+
+#define DBGPRINT(Level, Fmt)    DBGPRINT_RAW(Level, Fmt)
+
+
+#define DBGPRINT_ERR(Fmt)           \
+{                                   \
+    printk("ERROR!!! ");          \
+    printk Fmt;                  \
+}
+
+#define DBGPRINT_S(Status, Fmt)		\
+{									\
+	printk Fmt;					\
+}
+
+
+#else
+#define DBGPRINT(Level, Fmt)
+#define DBGPRINT_RAW(Level, Fmt)
+#define DBGPRINT_S(Status, Fmt)
+#define DBGPRINT_ERR(Fmt)
+#endif
+
+
+//
+//  spin_lock enhanced for Nested spin lock
+//
+#define NdisAllocateSpinLock(__lock)      \
+{                                       \
+    spin_lock_init((spinlock_t *)(__lock));               \
+}
+
+#define NdisFreeSpinLock(lock)          \
+{                                       \
+}
+
+
+#define RTMP_SEM_LOCK(__lock)					\
+{												\
+	spin_lock_bh((spinlock_t *)(__lock));				\
+}
+
+#define RTMP_SEM_UNLOCK(__lock)					\
+{												\
+	spin_unlock_bh((spinlock_t *)(__lock));				\
+}
+
+#if 0 // sample, IRQ LOCK
+#define RTMP_IRQ_LOCK(__lock, __irqflags)					\
+{													\
+	spin_lock_irqsave((spinlock_t *)__lock, __irqflags);	\
+	pAd->irq_disabled |= 1; \
+}
+
+#define RTMP_IRQ_UNLOCK(__lock, __irqflag)						\
+{														\
+	pAd->irq_disabled &= 0; \
+	spin_unlock_irqrestore((spinlock_t *)(__lock), ((unsigned long)__irqflag));	\
+}
+#else
+
+// sample, use semaphore lock to replace IRQ lock, 2007/11/15
+#define RTMP_IRQ_LOCK(__lock, __irqflags)			\
+{													\
+	__irqflags = 0;									\
+	spin_lock_bh((spinlock_t *)(__lock));			\
+	pAd->irq_disabled |= 1; \
+}
+
+#define RTMP_IRQ_UNLOCK(__lock, __irqflag)			\
+{													\
+	pAd->irq_disabled &= 0; \
+	spin_unlock_bh((spinlock_t *)(__lock));			\
+}
+
+#define RTMP_INT_LOCK(__lock, __irqflags)			\
+{													\
+	spin_lock_irqsave((spinlock_t *)__lock, __irqflags);	\
+}
+
+#define RTMP_INT_UNLOCK(__lock, __irqflag)			\
+{													\
+	spin_unlock_irqrestore((spinlock_t *)(__lock), ((unsigned long)__irqflag));	\
+}
+#endif
+
+
+
+#ifdef RT2870
+//Patch for ASIC turst read/write bug, needs to remove after metel fix
+#define RTMP_IO_READ32(_A, _R, _pV)								\
+	RTUSBReadMACRegister(_A, _R, _pV)
+
+#define RTMP_IO_READ8(_A, _R, _pV)								\
+{																\
+}
+
+#define RTMP_IO_WRITE32(_A, _R, _V)								\
+	RTUSBWriteMACRegister(_A, _R, _V)
+
+
+#define RTMP_IO_WRITE8(_A, _R, _V)								\
+{																\
+	USHORT	_Val = _V;											\
+	RTUSBSingleWrite(_A, _R, _Val);								\
+}
+
+
+#define RTMP_IO_WRITE16(_A, _R, _V)								\
+{																\
+	RTUSBSingleWrite(_A, _R, _V);								\
+}
+#endif // RT2870 //
+
+#ifndef wait_event_interruptible_timeout
+#define __wait_event_interruptible_timeout(wq, condition, ret) \
+do { \
+        wait_queue_t __wait; \
+        init_waitqueue_entry(&__wait, current); \
+        add_wait_queue(&wq, &__wait); \
+        for (;;) { \
+                set_current_state(TASK_INTERRUPTIBLE); \
+                if (condition) \
+                        break; \
+                if (!signal_pending(current)) { \
+                        ret = schedule_timeout(ret); \
+                        if (!ret) \
+                                break; \
+                        continue; \
+                } \
+                ret = -ERESTARTSYS; \
+                break; \
+        } \
+        current->state = TASK_RUNNING; \
+        remove_wait_queue(&wq, &__wait); \
+} while (0)
+
+#define wait_event_interruptible_timeout(wq, condition, timeout) \
+({ \
+        long __ret = timeout; \
+        if (!(condition)) \
+                __wait_event_interruptible_timeout(wq, condition, __ret); \
+        __ret; \
+})
+#endif
+#define ONE_TICK 1
+#define OS_WAIT(_time) \
+{	int _i; \
+	long _loop = ((_time)/(1000/OS_HZ)) > 0 ? ((_time)/(1000/OS_HZ)) : 1;\
+	wait_queue_head_t _wait; \
+	init_waitqueue_head(&_wait); \
+	for (_i=0; _i<(_loop); _i++) \
+		wait_event_interruptible_timeout(_wait, 0, ONE_TICK); }
+
+
+typedef void (*TIMER_FUNCTION)(unsigned long);
+
+#define COPY_MAC_ADDR(Addr1, Addr2)             memcpy((Addr1), (Addr2), MAC_ADDR_LEN)
+
+#define MlmeAllocateMemory(_pAd, _ppVA) os_alloc_mem(_pAd, _ppVA, MGMT_DMA_BUFFER_SIZE)
+#define MlmeFreeMemory(_pAd, _pVA)     os_free_mem(_pAd, _pVA)
+
+
+#ifdef RT2870
+#define BUILD_TIMER_FUNCTION(_func)													\
+void linux_##_func(unsigned long data)												\
+{																					\
+	PRALINK_TIMER_STRUCT	_pTimer = (PRALINK_TIMER_STRUCT)data;					\
+	RT2870_TIMER_ENTRY		*_pQNode;												\
+	RTMP_ADAPTER			*_pAd;													\
+																				\
+	_pTimer->handle = _func;															\
+	_pAd = (RTMP_ADAPTER *)_pTimer->pAd;												\
+	_pQNode = RT2870_TimerQ_Insert(_pAd, _pTimer); 									\
+	if ((_pQNode == NULL) && (_pAd->TimerQ.status & RT2870_THREAD_CAN_DO_INSERT))	\
+		RTMP_OS_Add_Timer(&_pTimer->TimerObj, HZ);               					\
+}
+#endif // RT2870 //
+
+
+#define DECLARE_TIMER_FUNCTION(_func)			\
+void linux_##_func(unsigned long data)
+
+#define GET_TIMER_FUNCTION(_func)				\
+		linux_##_func
+
+DECLARE_TIMER_FUNCTION(MlmePeriodicExec);
+DECLARE_TIMER_FUNCTION(MlmeRssiReportExec);
+DECLARE_TIMER_FUNCTION(AsicRxAntEvalTimeout);
+DECLARE_TIMER_FUNCTION(APSDPeriodicExec);
+DECLARE_TIMER_FUNCTION(AsicRfTuningExec);
+#ifdef RT2870
+DECLARE_TIMER_FUNCTION(BeaconUpdateExec);
+#endif // RT2870 //
+
+
+#ifdef CONFIG_STA_SUPPORT
+DECLARE_TIMER_FUNCTION(BeaconTimeout);
+DECLARE_TIMER_FUNCTION(ScanTimeout);
+DECLARE_TIMER_FUNCTION(AuthTimeout);
+DECLARE_TIMER_FUNCTION(AssocTimeout);
+DECLARE_TIMER_FUNCTION(ReassocTimeout);
+DECLARE_TIMER_FUNCTION(DisassocTimeout);
+DECLARE_TIMER_FUNCTION(LinkDownExec);
+#ifdef LEAP_SUPPORT
+DECLARE_TIMER_FUNCTION(LeapAuthTimeout);
+#endif
+DECLARE_TIMER_FUNCTION(StaQuickResponeForRateUpExec);
+DECLARE_TIMER_FUNCTION(WpaDisassocApAndBlockAssoc);
+DECLARE_TIMER_FUNCTION(PsPollWakeExec);
+DECLARE_TIMER_FUNCTION(RadioOnExec);
+
+#ifdef QOS_DLS_SUPPORT
+DECLARE_TIMER_FUNCTION(DlsTimeoutAction);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+void RTMP_GetCurrentSystemTime(LARGE_INTEGER *time);
+
+
+/*
+ * packet helper
+ * 	- convert internal rt packet to os packet or
+ *             os packet to rt packet
+ */
+#define RTPKT_TO_OSPKT(_p)		((struct sk_buff *)(_p))
+#define OSPKT_TO_RTPKT(_p)		((PNDIS_PACKET)(_p))
+
+#define GET_OS_PKT_DATAPTR(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->data)
+
+#define GET_OS_PKT_LEN(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->len)
+
+#define GET_OS_PKT_DATATAIL(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->tail)
+
+#define GET_OS_PKT_HEAD(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->head)
+
+#define GET_OS_PKT_END(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->end)
+
+#define GET_OS_PKT_NETDEV(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->dev)
+
+#define GET_OS_PKT_TYPE(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt))
+
+#define GET_OS_PKT_NEXT(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->next)
+
+
+#define OS_NTOHS(_Val) \
+		(ntohs(_Val))
+#define OS_HTONS(_Val) \
+		(htons(_Val))
+#define OS_NTOHL(_Val) \
+		(ntohl(_Val))
+#define OS_HTONL(_Val) \
+		(htonl(_Val))
+
+/* statistics counter */
+#define STATS_INC_RX_PACKETS(_pAd, _dev)
+#define STATS_INC_TX_PACKETS(_pAd, _dev)
+
+#define STATS_INC_RX_BYTESS(_pAd, _dev, len)
+#define STATS_INC_TX_BYTESS(_pAd, _dev, len)
+
+#define STATS_INC_RX_ERRORS(_pAd, _dev)
+#define STATS_INC_TX_ERRORS(_pAd, _dev)
+
+#define STATS_INC_RX_DROPPED(_pAd, _dev)
+#define STATS_INC_TX_DROPPED(_pAd, _dev)
+
+
+#define CB_OFF  10
+
+
+//   check DDK NDIS_PACKET data structure and find out only MiniportReservedEx[0..7] can be used by our driver without
+//   ambiguity. Fields after pPacket->MiniportReservedEx[8] may be used by other wrapper layer thus crashes the driver
+//
+//#define RTMP_GET_PACKET_MR(_p)			(RTPKT_TO_OSPKT(_p))
+
+// User Priority
+#define RTMP_SET_PACKET_UP(_p, _prio)			(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+0] = _prio)
+#define RTMP_GET_PACKET_UP(_p)					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+0])
+
+// Fragment #
+#define RTMP_SET_PACKET_FRAGMENTS(_p, _num)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+1] = _num)
+#define RTMP_GET_PACKET_FRAGMENTS(_p)			(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+1])
+
+// 0x0 ~0x7f: TX to AP's own BSS which has the specified AID. if AID>127, set bit 7 in RTMP_SET_PACKET_EMACTAB too.
+//(this value also as MAC(on-chip WCID) table index)
+// 0x80~0xff: TX to a WDS link. b0~6: WDS index
+#define RTMP_SET_PACKET_WCID(_p, _wdsidx)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+2] = _wdsidx)
+#define RTMP_GET_PACKET_WCID(_p)          		((UCHAR)(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+2]))
+
+// 0xff: PKTSRC_NDIS, others: local TX buffer index. This value affects how to a packet
+#define RTMP_SET_PACKET_SOURCE(_p, _pktsrc)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+3] = _pktsrc)
+#define RTMP_GET_PACKET_SOURCE(_p)       		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+3])
+
+// RTS/CTS-to-self protection method
+#define RTMP_SET_PACKET_RTS(_p, _num)      		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+4] = _num)
+#define RTMP_GET_PACKET_RTS(_p)          		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+4])
+// see RTMP_S(G)ET_PACKET_EMACTAB
+
+// TX rate index
+#define RTMP_SET_PACKET_TXRATE(_p, _rate)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+5] = _rate)
+#define RTMP_GET_PACKET_TXRATE(_p)		  		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+5])
+
+// From which Interface
+#define RTMP_SET_PACKET_IF(_p, _ifdx)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+6] = _ifdx)
+#define RTMP_GET_PACKET_IF(_p)		  		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+6])
+#define RTMP_SET_PACKET_NET_DEVICE_MBSSID(_p, _bss)		RTMP_SET_PACKET_IF((_p), (_bss))
+#define RTMP_SET_PACKET_NET_DEVICE_WDS(_p, _bss)		RTMP_SET_PACKET_IF((_p), ((_bss) + MIN_NET_DEVICE_FOR_WDS))
+#define RTMP_SET_PACKET_NET_DEVICE_APCLI(_p, _idx)   	RTMP_SET_PACKET_IF((_p), ((_idx) + MIN_NET_DEVICE_FOR_APCLI))
+#define RTMP_SET_PACKET_NET_DEVICE_MESH(_p, _idx)   	RTMP_SET_PACKET_IF((_p), ((_idx) + MIN_NET_DEVICE_FOR_MESH))
+#define RTMP_GET_PACKET_NET_DEVICE_MBSSID(_p)			RTMP_GET_PACKET_IF((_p))
+#define RTMP_GET_PACKET_NET_DEVICE(_p)					RTMP_GET_PACKET_IF((_p))
+
+#define RTMP_SET_PACKET_MOREDATA(_p, _morebit)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+7] = _morebit)
+#define RTMP_GET_PACKET_MOREDATA(_p)				(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+7])
+
+//#define RTMP_SET_PACKET_NET_DEVICE_MBSSID(_p, _bss)	(RTPKT_TO_OSPKT(_p)->cb[8] = _bss)
+//#define RTMP_GET_PACKET_NET_DEVICE_MBSSID(_p)		(RTPKT_TO_OSPKT(_p)->cb[8])
+
+
+
+
+#if 0
+//#define RTMP_SET_PACKET_DHCP(_p, _flg)   	(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] = _flg)
+//#define RTMP_GET_PACKET_DHCP(_p)         	(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11])
+#else
+//
+//	Sepcific Pakcet Type definition
+//
+#define RTMP_PACKET_SPECIFIC_CB_OFFSET	11
+
+#define RTMP_PACKET_SPECIFIC_DHCP		0x01
+#define RTMP_PACKET_SPECIFIC_EAPOL		0x02
+#define RTMP_PACKET_SPECIFIC_IPV4		0x04
+#define RTMP_PACKET_SPECIFIC_WAI		0x08
+#define RTMP_PACKET_SPECIFIC_VLAN		0x10
+#define RTMP_PACKET_SPECIFIC_LLCSNAP	0x20
+
+//Specific
+#define RTMP_SET_PACKET_SPECIFIC(_p, _flg)	   	(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] = _flg)
+
+//DHCP
+#define RTMP_SET_PACKET_DHCP(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_DHCP);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_DHCP);	\
+			}while(0)
+#define RTMP_GET_PACKET_DHCP(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_DHCP)
+
+//EAPOL
+#define RTMP_SET_PACKET_EAPOL(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_EAPOL);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_EAPOL);	\
+			}while(0)
+#define RTMP_GET_PACKET_EAPOL(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_EAPOL)
+
+//WAI
+#define RTMP_SET_PACKET_WAI(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_WAI);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_WAI);	\
+			}while(0)
+#define RTMP_GET_PACKET_WAI(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_WAI)
+
+#define RTMP_GET_PACKET_LOWRATE(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & (RTMP_PACKET_SPECIFIC_EAPOL | RTMP_PACKET_SPECIFIC_DHCP | RTMP_PACKET_SPECIFIC_WAI))
+
+//VLAN
+#define RTMP_SET_PACKET_VLAN(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_VLAN);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_VLAN);	\
+			}while(0)
+#define RTMP_GET_PACKET_VLAN(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_VLAN)
+
+//LLC/SNAP
+#define RTMP_SET_PACKET_LLCSNAP(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_LLCSNAP);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_LLCSNAP);		\
+			}while(0)
+
+#define RTMP_GET_PACKET_LLCSNAP(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_LLCSNAP)
+
+// IP
+#define RTMP_SET_PACKET_IPV4(_p, _flg)														\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_IPV4);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_IPV4);	\
+			}while(0)
+
+#define RTMP_GET_PACKET_IPV4(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_IPV4)
+
+#endif
+
+
+// If this flag is set, it indicates that this EAPoL frame MUST be clear.
+#define RTMP_SET_PACKET_CLEAR_EAP_FRAME(_p, _flg)   (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+12] = _flg)
+#define RTMP_GET_PACKET_CLEAR_EAP_FRAME(_p)         (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+12])
+
+#define RTMP_SET_PACKET_5VT(_p, _flg)   (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+22] = _flg)
+#define RTMP_GET_PACKET_5VT(_p)         (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+22])
+
+
+#ifdef CONFIG_5VT_ENHANCE
+#define BRIDGE_TAG 0x35564252    // depends on 5VT define in br_input.c
+#endif
+
+
+#define NDIS_SET_PACKET_STATUS(_p, _status)
+
+
+#define GET_SG_LIST_FROM_PACKET(_p, _sc)	\
+    rt_get_sg_list_from_packet(_p, _sc)
+
+
+#define NdisMoveMemory(Destination, Source, Length) memmove(Destination, Source, Length)
+#define NdisZeroMemory(Destination, Length)         memset(Destination, 0, Length)
+#define NdisFillMemory(Destination, Length, Fill)   memset(Destination, Fill, Length)
+#define NdisEqualMemory(Source1, Source2, Length)   (!memcmp(Source1, Source2, Length))
+#define RTMPEqualMemory(Source1, Source2, Length)	(!memcmp(Source1, Source2, Length))
+
+
+#define RTMP_INC_REF(_A)		0
+#define RTMP_DEC_REF(_A)		0
+#define RTMP_GET_REF(_A)		0
+
+
+
+/*
+ * ULONG
+ * RTMP_GetPhysicalAddressLow(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress);
+ */
+#define RTMP_GetPhysicalAddressLow(PhysicalAddress)		(PhysicalAddress)
+
+/*
+ * ULONG
+ * RTMP_GetPhysicalAddressHigh(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress);
+ */
+#define RTMP_GetPhysicalAddressHigh(PhysicalAddress)		(0)
+
+/*
+ * VOID
+ * RTMP_SetPhysicalAddressLow(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress,
+ *   IN ULONG  Value);
+ */
+#define RTMP_SetPhysicalAddressLow(PhysicalAddress, Value)	\
+			PhysicalAddress = Value;
+
+/*
+ * VOID
+ * RTMP_SetPhysicalAddressHigh(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress,
+ *   IN ULONG  Value);
+ */
+#define RTMP_SetPhysicalAddressHigh(PhysicalAddress, Value)
+
+
+//CONTAINING_RECORD(pEntry, NDIS_PACKET, MiniportReservedEx);
+#define QUEUE_ENTRY_TO_PACKET(pEntry) \
+	(PNDIS_PACKET)(pEntry)
+
+#define PACKET_TO_QUEUE_ENTRY(pPacket) \
+	(PQUEUE_ENTRY)(pPacket)
+
+
+#ifndef CONTAINING_RECORD
+#define CONTAINING_RECORD(address, type, field)			\
+((type *)((PCHAR)(address) - offsetof(type, field)))
+#endif
+
+
+#define RELEASE_NDIS_PACKET(_pAd, _pPacket, _Status)                    \
+{                                                                       \
+        RTMPFreeNdisPacket(_pAd, _pPacket);                             \
+}
+
+
+#define SWITCH_PhyAB(_pAA, _pBB)    \
+{                                                                           \
+    ULONG	AABasePaHigh;                           \
+    ULONG	AABasePaLow;                           \
+    ULONG	BBBasePaHigh;                           \
+    ULONG	BBBasePaLow;                           \
+    BBBasePaHigh = RTMP_GetPhysicalAddressHigh(_pBB);                                                 \
+    BBBasePaLow = RTMP_GetPhysicalAddressLow(_pBB);                                                 \
+    AABasePaHigh = RTMP_GetPhysicalAddressHigh(_pAA);                                                 \
+    AABasePaLow = RTMP_GetPhysicalAddressLow(_pAA);                                                 \
+    RTMP_SetPhysicalAddressHigh(_pAA, BBBasePaHigh);                                                 \
+    RTMP_SetPhysicalAddressLow(_pAA, BBBasePaLow);                                                 \
+    RTMP_SetPhysicalAddressHigh(_pBB, AABasePaHigh);                                                 \
+    RTMP_SetPhysicalAddressLow(_pBB, AABasePaLow);                                                 \
+}
+
+
+#define NdisWriteErrorLogEntry(_a, _b, _c, _d)
+#define NdisMAllocateMapRegisters(_a, _b, _c, _d, _e)		NDIS_STATUS_SUCCESS
+
+
+#define NdisAcquireSpinLock		RTMP_SEM_LOCK
+#define NdisReleaseSpinLock		RTMP_SEM_UNLOCK
+
+static inline void NdisGetSystemUpTime(ULONG *time)
+{
+	*time = jiffies;
+}
+
+//pPacket = CONTAINING_RECORD(pEntry, NDIS_PACKET, MiniportReservedEx);
+#define QUEUE_ENTRY_TO_PKT(pEntry) \
+		((PNDIS_PACKET) (pEntry))
+
+int rt28xx_packet_xmit(struct sk_buff *skb);
+
+
+
+void rtmp_os_thread_init(PUCHAR pThreadName, PVOID pNotify);
+
+
diff --git a/drivers/staging/rt2870/rt_main_dev.c b/drivers/staging/rt2870/rt_main_dev.c
new file mode 100644
index 000000000000..313ecea0bfa8
--- /dev/null
+++ b/drivers/staging/rt2870/rt_main_dev.c
@@ -0,0 +1,1863 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rt_main_dev.c
+
+    Abstract:
+    Create and register network interface.
+
+    Revision History:
+    Who         When            What
+    --------    ----------      ----------------------------------------------
+	Sample		Mar/21/07		Merge RT2870 and RT2860 drivers.
+*/
+
+#include "rt_config.h"
+
+#define FORTY_MHZ_INTOLERANT_INTERVAL	(60*1000) // 1 min
+
+#ifdef MULTIPLE_CARD_SUPPORT
+// record whether the card in the card list is used in the card file
+UINT8  MC_CardUsed[MAX_NUM_OF_MULTIPLE_CARD];
+// record used card mac address in the card list
+static UINT8  MC_CardMac[MAX_NUM_OF_MULTIPLE_CARD][6];
+#endif // MULTIPLE_CARD_SUPPORT //
+
+/*---------------------------------------------------------------------*/
+/* Private Variables Used                                              */
+/*---------------------------------------------------------------------*/
+//static RALINK_TIMER_STRUCT     PeriodicTimer;
+
+char *mac = "";		   // default 00:00:00:00:00:00
+char *hostname = "";
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,12)
+MODULE_PARM (mac, "s");
+#else
+module_param (mac, charp, 0);
+#endif
+MODULE_PARM_DESC (mac, "rt28xx: wireless mac addr");
+
+
+/*---------------------------------------------------------------------*/
+/* Prototypes of Functions Used                                        */
+/*---------------------------------------------------------------------*/
+#ifdef DOT11_N_SUPPORT
+extern BOOLEAN ba_reordering_resource_init(PRTMP_ADAPTER pAd, int num);
+extern void ba_reordering_resource_release(PRTMP_ADAPTER pAd);
+#endif // DOT11_N_SUPPORT //
+extern NDIS_STATUS NICLoadRateSwitchingParams(IN PRTMP_ADAPTER pAd);
+
+
+// public function prototype
+INT __devinit rt28xx_probe(IN void *_dev_p, IN void *_dev_id_p,
+							IN UINT argc, OUT PRTMP_ADAPTER *ppAd);
+
+// private function prototype
+static int rt28xx_init(IN struct net_device *net_dev);
+INT rt28xx_send_packets(IN struct sk_buff *skb_p, IN struct net_device *net_dev);
+
+#if LINUX_VERSION_CODE <= 0x20402	// Red Hat 7.1
+struct net_device *alloc_netdev(
+	int sizeof_priv,
+	const char *mask,
+	void (*setup)(struct net_device *));
+#endif // LINUX_VERSION_CODE //
+
+static void CfgInitHook(PRTMP_ADAPTER pAd);
+//static BOOLEAN RT28XXAvailRANameAssign(IN CHAR *name_p);
+
+#ifdef CONFIG_STA_SUPPORT
+extern	const struct iw_handler_def rt28xx_iw_handler_def;
+#endif // CONFIG_STA_SUPPORT //
+
+#if WIRELESS_EXT >= 12
+// This function will be called when query /proc
+struct iw_statistics *rt28xx_get_wireless_stats(
+    IN struct net_device *net_dev);
+#endif
+
+struct net_device_stats *RT28xx_get_ether_stats(
+    IN  struct net_device *net_dev);
+
+/*
+========================================================================
+Routine Description:
+    Close raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+	1. if open fail, kernel will not call the close function.
+	2. Free memory for
+		(1) Mlme Memory Handler:		MlmeHalt()
+		(2) TX & RX:					RTMPFreeTxRxRingMemory()
+		(3) BA Reordering: 				ba_reordering_resource_release()
+========================================================================
+*/
+int MainVirtualIF_close(IN struct net_device *net_dev)
+{
+    RTMP_ADAPTER *pAd = net_dev->ml_priv;
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+		return 0; // close ok
+
+	netif_carrier_off(pAd->net_dev);
+	netif_stop_queue(pAd->net_dev);
+
+
+
+	VIRTUAL_IF_DOWN(pAd);
+
+	RT_MOD_DEC_USE_COUNT();
+
+	return 0; // close ok
+}
+
+/*
+========================================================================
+Routine Description:
+    Open raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+	1. if open fail, kernel will not call the close function.
+	2. Free memory for
+		(1) Mlme Memory Handler:		MlmeHalt()
+		(2) TX & RX:					RTMPFreeTxRxRingMemory()
+		(3) BA Reordering: 				ba_reordering_resource_release()
+========================================================================
+*/
+int MainVirtualIF_open(IN struct net_device *net_dev)
+{
+    RTMP_ADAPTER *pAd = net_dev->ml_priv;
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+		return 0; // close ok
+
+	if (VIRTUAL_IF_UP(pAd) != 0)
+		return -1;
+
+	// increase MODULE use count
+	RT_MOD_INC_USE_COUNT();
+
+	netif_start_queue(net_dev);
+	netif_carrier_on(net_dev);
+	netif_wake_queue(net_dev);
+
+	return 0;
+}
+
+/*
+========================================================================
+Routine Description:
+    Close raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+	1. if open fail, kernel will not call the close function.
+	2. Free memory for
+		(1) Mlme Memory Handler:		MlmeHalt()
+		(2) TX & RX:					RTMPFreeTxRxRingMemory()
+		(3) BA Reordering: 				ba_reordering_resource_release()
+========================================================================
+*/
+int rt28xx_close(IN PNET_DEV dev)
+{
+	struct net_device * net_dev = (struct net_device *)dev;
+    RTMP_ADAPTER	*pAd = net_dev->ml_priv;
+	BOOLEAN 		Cancelled = FALSE;
+	UINT32			i = 0;
+#ifdef RT2870
+	DECLARE_WAIT_QUEUE_HEAD(unlink_wakeup);
+	DECLARE_WAITQUEUE(wait, current);
+
+	//RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_REMOVE_IN_PROGRESS);
+#endif // RT2870 //
+
+
+    DBGPRINT(RT_DEBUG_TRACE, ("===> rt28xx_close\n"));
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+		return 0; // close ok
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+
+		// If dirver doesn't wake up firmware here,
+		// NICLoadFirmware will hang forever when interface is up again.
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+        {
+		    AsicForceWakeup(pAd, TRUE);
+        }
+
+#ifdef QOS_DLS_SUPPORT
+		// send DLS-TEAR_DOWN message,
+		if (pAd->CommonCfg.bDLSCapable)
+		{
+			UCHAR i;
+
+			// tear down local dls table entry
+			for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+				{
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+					pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				}
+			}
+
+			// tear down peer dls table entry
+			for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+				{
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				}
+			}
+			RT28XX_MLME_HANDLER(pAd);
+		}
+#endif // QOS_DLS_SUPPORT //
+
+		if (INFRA_ON(pAd) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+		{
+			MLME_DISASSOC_REQ_STRUCT	DisReq;
+			MLME_QUEUE_ELEM *MsgElem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+			COPY_MAC_ADDR(DisReq.Addr, pAd->CommonCfg.Bssid);
+			DisReq.Reason =  REASON_DEAUTH_STA_LEAVING;
+
+			MsgElem->Machine = ASSOC_STATE_MACHINE;
+			MsgElem->MsgType = MT2_MLME_DISASSOC_REQ;
+			MsgElem->MsgLen = sizeof(MLME_DISASSOC_REQ_STRUCT);
+			NdisMoveMemory(MsgElem->Msg, &DisReq, sizeof(MLME_DISASSOC_REQ_STRUCT));
+
+			// Prevent to connect AP again in STAMlmePeriodicExec
+			pAd->MlmeAux.AutoReconnectSsidLen= 32;
+			NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+			MlmeDisassocReqAction(pAd, MsgElem);
+			kfree(MsgElem);
+
+			RTMPusecDelay(1000);
+		}
+
+#ifdef RT2870
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_REMOVE_IN_PROGRESS);
+#endif // RT2870 //
+
+#ifdef CCX_SUPPORT
+		RTMPCancelTimer(&pAd->StaCfg.LeapAuthTimer, &Cancelled);
+#endif
+
+		RTMPCancelTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, &Cancelled);
+		RTMPCancelTimer(&pAd->StaCfg.WpaDisassocAndBlockAssocTimer, &Cancelled);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+		{
+			union iwreq_data    wrqu;
+			// send wireless event to wpa_supplicant for infroming interface down.
+			memset(&wrqu, 0, sizeof(wrqu));
+			wrqu.data.flags = RT_INTERFACE_DOWN;
+			wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+		}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+		MlmeRadioOff(pAd);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+
+	for (i = 0 ; i < NUM_OF_TX_RING; i++)
+	{
+		while (pAd->DeQueueRunning[i] == TRUE)
+		{
+			printk("Waiting for TxQueue[%d] done..........\n", i);
+			RTMPusecDelay(1000);
+		}
+	}
+
+#ifdef RT2870
+	// ensure there are no more active urbs.
+	add_wait_queue (&unlink_wakeup, &wait);
+	pAd->wait = &unlink_wakeup;
+
+	// maybe wait for deletions to finish.
+	i = 0;
+	//while((i < 25) && atomic_read(&pAd->PendingRx) > 0)
+	while(i < 25)
+	{
+		unsigned long IrqFlags;
+
+		RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+		if (pAd->PendingRx == 0)
+		{
+			RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+			break;
+		}
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,9)
+		msleep(UNLINK_TIMEOUT_MS);	//Time in millisecond
+#else
+		RTMPusecDelay(UNLINK_TIMEOUT_MS*1000);	//Time in microsecond
+#endif
+		i++;
+	}
+	pAd->wait = NULL;
+	remove_wait_queue (&unlink_wakeup, &wait);
+#endif // RT2870 //
+
+	//RTUSBCleanUpMLMEWaitQueue(pAd);	/*not used in RT28xx*/
+
+
+#ifdef RT2870
+	// We need clear timerQ related structure before exits of the timer thread.
+	RT2870_TimerQ_Exit(pAd);
+	// Close kernel threads or tasklets
+	RT28xxThreadTerminate(pAd);
+#endif // RT2870 //
+
+	// Stop Mlme state machine
+	MlmeHalt(pAd);
+
+	// Close kernel threads or tasklets
+	kill_thread_task(pAd);
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		MacTableReset(pAd);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+	MeasureReqTabExit(pAd);
+	TpcReqTabExit(pAd);
+
+
+
+
+	// Free Ring or USB buffers
+	RTMPFreeTxRxRingMemory(pAd);
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+
+#ifdef DOT11_N_SUPPORT
+	// Free BA reorder resource
+	ba_reordering_resource_release(pAd);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef RT2870
+#ifdef INF_AMAZON_SE
+	if (pAd->UsbVendorReqBuf)
+		os_free_mem(pAd, pAd->UsbVendorReqBuf);
+#endif // INF_AMAZON_SE //
+#endif // RT2870 //
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_START_UP);
+
+	return 0; // close ok
+} /* End of rt28xx_close */
+
+static int rt28xx_init(IN struct net_device *net_dev)
+{
+	PRTMP_ADAPTER 			pAd = net_dev->ml_priv;
+	UINT					index;
+	UCHAR					TmpPhy;
+	NDIS_STATUS				Status;
+	UINT32 		MacCsr0 = 0;
+
+#ifdef RT2870
+#ifdef INF_AMAZON_SE
+	init_MUTEX(&(pAd->UsbVendorReq_semaphore));
+	os_alloc_mem(pAd, (PUCHAR)&pAd->UsbVendorReqBuf, MAX_PARAM_BUFFER_SIZE - 1);
+	if (pAd->UsbVendorReqBuf == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Allocate vendor request temp buffer failed!\n"));
+		goto err0;
+	}
+#endif // INF_AMAZON_SE //
+#endif // RT2870 //
+
+#ifdef DOT11_N_SUPPORT
+	// Allocate BA Reordering memory
+	ba_reordering_resource_init(pAd, MAX_REORDERING_MPDU_NUM);
+#endif // DOT11_N_SUPPORT //
+
+	// Make sure MAC gets ready.
+	index = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, MAC_CSR0, &MacCsr0);
+		pAd->MACVersion = MacCsr0;
+
+		if ((pAd->MACVersion != 0x00) && (pAd->MACVersion != 0xFFFFFFFF))
+			break;
+
+		RTMPusecDelay(10);
+	} while (index++ < 100);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("MAC_CSR0  [ Ver:Rev=0x%08x]\n", pAd->MACVersion));
+
+	// Disable DMA
+	RT28XXDMADisable(pAd);
+
+
+	// Load 8051 firmware
+	Status = NICLoadFirmware(pAd);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("NICLoadFirmware failed, Status[=0x%08x]\n", Status));
+		goto err1;
+	}
+
+	NICLoadRateSwitchingParams(pAd);
+
+	// Disable interrupts here which is as soon as possible
+	// This statement should never be true. We might consider to remove it later
+
+	Status = RTMPAllocTxRxRingMemory(pAd);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("RTMPAllocDMAMemory failed, Status[=0x%08x]\n", Status));
+		goto err1;
+	}
+
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE);
+
+	// initialize MLME
+	//
+
+	Status = MlmeInit(pAd);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("MlmeInit failed, Status[=0x%08x]\n", Status));
+		goto err2;
+	}
+
+	// Initialize pAd->StaCfg, pAd->ApCfg, pAd->CommonCfg to manufacture default
+	//
+	UserCfgInit(pAd);
+
+#ifdef RT2870
+	// We need init timerQ related structure before create the timer thread.
+	RT2870_TimerQ_Init(pAd);
+#endif // RT2870 //
+
+	RT28XX_TASK_THREAD_INIT(pAd, Status);
+	if (Status != NDIS_STATUS_SUCCESS)
+		goto err1;
+
+//	COPY_MAC_ADDR(pAd->ApCfg.MBSSID[apidx].Bssid, netif->hwaddr);
+//	pAd->bForcePrintTX = TRUE;
+
+	CfgInitHook(pAd);
+
+
+#ifdef BLOCK_NET_IF
+	initblockQueueTab(pAd);
+#endif // BLOCK_NET_IF //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		NdisAllocateSpinLock(&pAd->MacTabLock);
+#endif // CONFIG_STA_SUPPORT //
+
+	MeasureReqTabInit(pAd);
+	TpcReqTabInit(pAd);
+
+	//
+	// Init the hardware, we need to init asic before read registry, otherwise mac register will be reset
+	//
+	Status = NICInitializeAdapter(pAd, TRUE);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("NICInitializeAdapter failed, Status[=0x%08x]\n", Status));
+		if (Status != NDIS_STATUS_SUCCESS)
+		goto err3;
+	}
+
+	// Read parameters from Config File
+	Status = RTMPReadParametersHook(pAd);
+
+	printk("1. Phy Mode = %d\n", pAd->CommonCfg.PhyMode);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("NICReadRegParameters failed, Status[=0x%08x]\n",Status));
+		goto err4;
+	}
+
+#ifdef RT2870
+	pAd->CommonCfg.bMultipleIRP = FALSE;
+
+	if (pAd->CommonCfg.bMultipleIRP)
+		pAd->CommonCfg.NumOfBulkInIRP = RX_RING_SIZE;
+	else
+		pAd->CommonCfg.NumOfBulkInIRP = 1;
+#endif // RT2870 //
+
+
+   	//Init Ba Capability parameters.
+//	RT28XX_BA_INIT(pAd);
+#ifdef DOT11_N_SUPPORT
+	pAd->CommonCfg.DesiredHtPhy.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+	pAd->CommonCfg.DesiredHtPhy.AmsduEnable = (USHORT)pAd->CommonCfg.BACapability.field.AmsduEnable;
+	pAd->CommonCfg.DesiredHtPhy.AmsduSize = (USHORT)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.DesiredHtPhy.MimoPs = (USHORT)pAd->CommonCfg.BACapability.field.MMPSmode;
+	// UPdata to HT IE
+	pAd->CommonCfg.HtCapability.HtCapInfo.MimoPs = (USHORT)pAd->CommonCfg.BACapability.field.MMPSmode;
+	pAd->CommonCfg.HtCapability.HtCapInfo.AMsduSize = (USHORT)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.HtCapability.HtCapParm.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+#endif // DOT11_N_SUPPORT //
+
+	// after reading Registry, we now know if in AP mode or STA mode
+
+	// Load 8051 firmware; crash when FW image not existent
+	// Status = NICLoadFirmware(pAd);
+	// if (Status != NDIS_STATUS_SUCCESS)
+	//    break;
+
+	printk("2. Phy Mode = %d\n", pAd->CommonCfg.PhyMode);
+
+	// We should read EEPROM for all cases.  rt2860b
+	NICReadEEPROMParameters(pAd, mac);
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+
+	printk("3. Phy Mode = %d\n", pAd->CommonCfg.PhyMode);
+
+	NICInitAsicFromEEPROM(pAd); //rt2860b
+
+	// Set PHY to appropriate mode
+	TmpPhy = pAd->CommonCfg.PhyMode;
+	pAd->CommonCfg.PhyMode = 0xff;
+	RTMPSetPhyMode(pAd, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+	SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+
+	// No valid channels.
+	if (pAd->ChannelListNum == 0)
+	{
+		printk("Wrong configuration. No valid channel found. Check \"ContryCode\" and \"ChannelGeography\" setting.\n");
+		goto err4;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	printk("MCS Set = %02x %02x %02x %02x %02x\n", pAd->CommonCfg.HtCapability.MCSSet[0],
+           pAd->CommonCfg.HtCapability.MCSSet[1], pAd->CommonCfg.HtCapability.MCSSet[2],
+           pAd->CommonCfg.HtCapability.MCSSet[3], pAd->CommonCfg.HtCapability.MCSSet[4]);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef RT2870
+    //Init RT30xx RFRegisters after read RFIC type from EEPROM
+	NICInitRT30xxRFRegisters(pAd);
+#endif // RT2870 //
+
+#if 0
+	// Patch cardbus controller if EEPROM said so.
+	if (pAd->bTest1 == FALSE)
+		RTMPPatchCardBus(pAd);
+#endif
+
+
+//		APInitialize(pAd);
+
+#ifdef IKANOS_VX_1X0
+	VR_IKANOS_FP_Init(pAd->ApCfg.BssidNum, pAd->PermanentAddress);
+#endif // IKANOS_VX_1X0 //
+
+		//
+	// Initialize RF register to default value
+	//
+	AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+	// 8051 firmware require the signal during booting time.
+	AsicSendCommandToMcu(pAd, 0x72, 0xFF, 0x00, 0x00);
+
+	if (pAd && (Status != NDIS_STATUS_SUCCESS))
+	{
+		//
+		// Undo everything if it failed
+		//
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+		{
+//			NdisMDeregisterInterrupt(&pAd->Interrupt);
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE);
+		}
+//		RTMPFreeAdapter(pAd); // we will free it in disconnect()
+	}
+	else if (pAd)
+	{
+		// Microsoft HCT require driver send a disconnect event after driver initialization.
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+//		pAd->IndicateMediaState = NdisMediaStateDisconnected;
+		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_CHANGE);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event B!\n"));
+
+
+#ifdef RT2870
+		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS);
+		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_REMOVE_IN_PROGRESS);
+
+		//
+		// Support multiple BulkIn IRP,
+		// the value on pAd->CommonCfg.NumOfBulkInIRP may be large than 1.
+		//
+		for(index=0; index<pAd->CommonCfg.NumOfBulkInIRP; index++)
+		{
+			RTUSBBulkReceive(pAd);
+			DBGPRINT(RT_DEBUG_TRACE, ("RTUSBBulkReceive!\n" ));
+		}
+#endif // RT2870 //
+	}// end of else
+
+
+	DBGPRINT_S(Status, ("<==== RTMPInitialize, Status=%x\n", Status));
+
+	return TRUE;
+
+
+err4:
+err3:
+	MlmeHalt(pAd);
+err2:
+	RTMPFreeTxRxRingMemory(pAd);
+//	RTMPFreeAdapter(pAd);
+err1:
+
+#ifdef DOT11_N_SUPPORT
+	os_free_mem(pAd, pAd->mpdu_blk_pool.mem); // free BA pool
+#endif // DOT11_N_SUPPORT //
+	RT28XX_IRQ_RELEASE(net_dev);
+
+	// shall not set ml_priv to NULL here because the ml_priv didn't been free yet.
+	//net_dev->ml_priv = 0;
+#ifdef INF_AMAZON_SE
+err0:
+#endif // INF_AMAZON_SE //
+	printk("!!! %s Initialized fail !!!\n", RT28xx_CHIP_NAME);
+	return FALSE;
+} /* End of rt28xx_init */
+
+
+/*
+========================================================================
+Routine Description:
+    Open raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+========================================================================
+*/
+int rt28xx_open(IN PNET_DEV dev)
+{
+	struct net_device * net_dev = (struct net_device *)dev;
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+	int retval = 0;
+ 	POS_COOKIE pObj;
+
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -1;
+	}
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	if (pAd->OpMode == OPMODE_AP)
+	{
+		CW_MAX_IN_BITS = 6;
+	}
+	else if (pAd->OpMode == OPMODE_STA)
+	{
+		CW_MAX_IN_BITS = 10;
+	}
+
+#if WIRELESS_EXT >= 12
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		if (pAd->OpMode == OPMODE_AP)
+			net_dev->wireless_handlers = (struct iw_handler_def *) &rt28xx_ap_iw_handler_def;
+		else if (pAd->OpMode == OPMODE_STA)
+			net_dev->wireless_handlers = (struct iw_handler_def *) &rt28xx_iw_handler_def;
+	}
+#endif // WIRELESS_EXT >= 12 //
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+
+	// Init
+ 	pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	// reset Adapter flags
+	RTMP_CLEAR_FLAGS(pAd);
+
+	// Request interrupt service routine for PCI device
+	// register the interrupt routine with the os
+	RT28XX_IRQ_REQUEST(net_dev);
+
+
+	// Init BssTab & ChannelInfo tabbles for auto channel select.
+
+
+	// Chip & other init
+	if (rt28xx_init(net_dev) == FALSE)
+		goto err;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		NdisZeroMemory(pAd->StaCfg.dev_name, 16);
+		NdisMoveMemory(pAd->StaCfg.dev_name, net_dev->name, strlen(net_dev->name));
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Set up the Mac address
+	NdisMoveMemory(net_dev->dev_addr, (void *) pAd->CurrentAddress, 6);
+
+	// Init IRQ parameters
+	RT28XX_IRQ_INIT(pAd);
+
+	// Various AP function init
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+		{
+			union iwreq_data    wrqu;
+			// send wireless event to wpa_supplicant for infroming interface down.
+			memset(&wrqu, 0, sizeof(wrqu));
+			wrqu.data.flags = RT_INTERFACE_UP;
+			wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+		}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Enable Interrupt
+	RT28XX_IRQ_ENABLE(pAd);
+
+	// Now Enable RxTx
+	RTMPEnableRxTx(pAd);
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_START_UP);
+
+	{
+	UINT32 reg = 0;
+	RTMP_IO_READ32(pAd, 0x1300, &reg);  // clear garbage interrupts
+	printk("0x1300 = %08x\n", reg);
+	}
+
+	{
+//	u32 reg;
+//	u8  byte;
+//	u16 tmp;
+
+//	RTMP_IO_READ32(pAd, XIFS_TIME_CFG, &reg);
+
+//	tmp = 0x0805;
+//	reg  = (reg & 0xffff0000) | tmp;
+//	RTMP_IO_WRITE32(pAd, XIFS_TIME_CFG, reg);
+
+	}
+
+#if 0
+	/*
+	 * debugging helper
+	 * 		show the size of main table in Adapter structure
+	 *		MacTab  -- 185K
+	 *		BATable -- 137K
+	 * 		Total 	-- 385K  !!!!! (5/26/2006)
+	 */
+	printk("sizeof(pAd->MacTab) = %ld\n", sizeof(pAd->MacTab));
+	printk("sizeof(pAd->AccessControlList) = %ld\n", sizeof(pAd->AccessControlList));
+	printk("sizeof(pAd->ApCfg) = %ld\n", sizeof(pAd->ApCfg));
+	printk("sizeof(pAd->BATable) = %ld\n", sizeof(pAd->BATable));
+	BUG();
+#endif
+
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+
+	return (retval);
+
+err:
+	return (-1);
+} /* End of rt28xx_open */
+
+
+/* Must not be called for mdev and apdev */
+static NDIS_STATUS rt_ieee80211_if_setup(struct net_device *dev, PRTMP_ADAPTER pAd)
+{
+	NDIS_STATUS Status;
+	INT     i=0;
+	CHAR    slot_name[IFNAMSIZ];
+	struct net_device   *device;
+
+
+	//ether_setup(dev);
+	dev->hard_start_xmit = rt28xx_send_packets;
+
+#ifdef IKANOS_VX_1X0
+	dev->hard_start_xmit = IKANOS_DataFramesTx;
+#endif // IKANOS_VX_1X0 //
+
+//	dev->set_multicast_list = ieee80211_set_multicast_list;
+//	dev->change_mtu = ieee80211_change_mtu;
+#ifdef CONFIG_STA_SUPPORT
+#if WIRELESS_EXT >= 12
+	if (pAd->OpMode == OPMODE_STA)
+	{
+		dev->wireless_handlers = &rt28xx_iw_handler_def;
+	}
+#endif //WIRELESS_EXT >= 12
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+#if WIRELESS_EXT >= 12
+	if (pAd->OpMode == OPMODE_AP)
+	{
+		dev->wireless_handlers = &rt28xx_ap_iw_handler_def;
+	}
+#endif //WIRELESS_EXT >= 12
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+#if WIRELESS_EXT < 21
+		dev->get_wireless_stats = rt28xx_get_wireless_stats;
+#endif
+	dev->get_stats = RT28xx_get_ether_stats;
+	dev->open = MainVirtualIF_open; //rt28xx_open;
+	dev->stop = MainVirtualIF_close; //rt28xx_close;
+//	dev->uninit = ieee80211_if_reinit;
+//	dev->destructor = ieee80211_if_free;
+	dev->priv_flags = INT_MAIN;
+	dev->do_ioctl = rt28xx_ioctl;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,24)
+    dev->validate_addr = NULL;
+#endif
+	// find available device name
+	for (i = 0; i < 8; i++)
+	{
+#ifdef MULTIPLE_CARD_SUPPORT
+		if (pAd->MC_RowID >= 0)
+			sprintf(slot_name, "ra%02d_%d", pAd->MC_RowID, i);
+		else
+#endif // MULTIPLE_CARD_SUPPORT //
+		sprintf(slot_name, "ra%d", i);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,24)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+        device = dev_get_by_name(dev_net(dev), slot_name);
+#else
+        device = dev_get_by_name(dev->nd_net, slot_name);
+#endif
+#else
+		device = dev_get_by_name(slot_name);
+#endif
+		if (device != NULL) dev_put(device);
+#else
+		for (device = dev_base; device != NULL; device = device->next)
+		{
+			if (strncmp(device->name, slot_name, 4) == 0)
+				break;
+		}
+#endif
+		if(device == NULL)
+			break;
+	}
+
+	if(i == 8)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("No available slot name\n"));
+		Status = NDIS_STATUS_FAILURE;
+	}
+	else
+	{
+#ifdef MULTIPLE_CARD_SUPPORT
+		if (pAd->MC_RowID >= 0)
+	        sprintf(dev->name, "ra%02d_%d", pAd->MC_RowID, i);
+		else
+#endif // MULTIPLE_CARD_SUPPORT //
+		sprintf(dev->name, "ra%d", i);
+		Status = NDIS_STATUS_SUCCESS;
+	}
+
+	return Status;
+
+}
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+/*
+========================================================================
+Routine Description:
+    Get card profile path.
+
+Arguments:
+    pAd
+
+Return Value:
+    TRUE		- Find a card profile
+	FALSE		- use default profile
+
+Note:
+========================================================================
+*/
+extern INT RTMPGetKeyParameter(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    IN  INT     destsize,
+    IN  PCHAR   buffer);
+
+BOOLEAN RTMP_CardInfoRead(
+	IN	PRTMP_ADAPTER pAd)
+{
+#define MC_SELECT_CARDID		0	/* use CARD ID (0 ~ 31) to identify different cards */
+#define MC_SELECT_MAC			1	/* use CARD MAC to identify different cards */
+#define MC_SELECT_CARDTYPE		2	/* use CARD type (abgn or bgn) to identify different cards */
+
+#define LETTER_CASE_TRANSLATE(txt_p, card_id)			\
+	{	UINT32 _len; char _char;						\
+		for(_len=0; _len<strlen(card_id); _len++) {		\
+			_char = *(txt_p + _len);					\
+			if (('A' <= _char) && (_char <= 'Z'))		\
+				*(txt_p+_len) = 'a'+(_char-'A');		\
+		} }
+
+	struct file *srcf;
+	INT retval, orgfsuid, orgfsgid;
+   	mm_segment_t orgfs;
+	CHAR *buffer, *tmpbuf, card_id_buf[30], RFIC_word[30];
+	BOOLEAN flg_match_ok = FALSE;
+	INT32 card_select_method;
+	INT32 card_free_id, card_nouse_id, card_same_mac_id, card_match_id;
+	EEPROM_ANTENNA_STRUC antenna;
+	USHORT addr01, addr23, addr45;
+	UINT8 mac[6];
+	UINT32 data, card_index;
+	UCHAR *start_ptr;
+
+
+	// init
+	buffer = kmalloc(MAX_INI_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if (buffer == NULL)
+        return FALSE;
+
+	tmpbuf = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if(tmpbuf == NULL)
+	{
+		kfree(buffer);
+        return NDIS_STATUS_FAILURE;
+	}
+
+	orgfsuid = current->fsuid;
+	orgfsgid = current->fsgid;
+	current->fsuid = current->fsgid = 0;
+    orgfs = get_fs();
+    set_fs(KERNEL_DS);
+
+	// get RF IC type
+	RTMP_IO_READ32(pAd, E2PROM_CSR, &data);
+
+	if ((data & 0x30) == 0)
+		pAd->EEPROMAddressNum = 6;	// 93C46
+	else if ((data & 0x30) == 0x10)
+		pAd->EEPROMAddressNum = 8;	// 93C66
+	else
+		pAd->EEPROMAddressNum = 8;	// 93C86
+
+	//antenna.word = RTMP_EEPROM_READ16(pAd, EEPROM_NIC1_OFFSET);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_NIC1_OFFSET, antenna.word);
+
+	if ((antenna.field.RfIcType == RFIC_2850) ||
+		(antenna.field.RfIcType == RFIC_2750))
+	{
+		/* ABGN card */
+		strcpy(RFIC_word, "abgn");
+	}
+	else
+	{
+		/* BGN card */
+		strcpy(RFIC_word, "bgn");
+	}
+
+	// get MAC address
+	//addr01 = RTMP_EEPROM_READ16(pAd, 0x04);
+	//addr23 = RTMP_EEPROM_READ16(pAd, 0x06);
+	//addr45 = RTMP_EEPROM_READ16(pAd, 0x08);
+	RT28xx_EEPROM_READ16(pAd, 0x04, addr01);
+	RT28xx_EEPROM_READ16(pAd, 0x06, addr23);
+	RT28xx_EEPROM_READ16(pAd, 0x08, addr45);
+
+	mac[0] = (UCHAR)(addr01 & 0xff);
+	mac[1] = (UCHAR)(addr01 >> 8);
+	mac[2] = (UCHAR)(addr23 & 0xff);
+	mac[3] = (UCHAR)(addr23 >> 8);
+	mac[4] = (UCHAR)(addr45 & 0xff);
+	mac[5] = (UCHAR)(addr45 >> 8);
+
+	// open card information file
+	srcf = filp_open(CARD_INFO_PATH, O_RDONLY, 0);
+	if (IS_ERR(srcf))
+	{
+		/* card information file does not exist */
+			DBGPRINT(RT_DEBUG_TRACE,
+				("--> Error %ld opening %s\n", -PTR_ERR(srcf), CARD_INFO_PATH));
+		return FALSE;
+	}
+
+	if (srcf->f_op && srcf->f_op->read)
+	{
+		/* card information file exists so reading the card information */
+		memset(buffer, 0x00, MAX_INI_BUFFER_SIZE);
+		retval = srcf->f_op->read(srcf, buffer, MAX_INI_BUFFER_SIZE, &srcf->f_pos);
+		if (retval < 0)
+		{
+			/* read fail */
+				DBGPRINT(RT_DEBUG_TRACE,
+					("--> Read %s error %d\n", CARD_INFO_PATH, -retval));
+		}
+		else
+		{
+			/* get card selection method */
+			memset(tmpbuf, 0x00, MAX_PARAM_BUFFER_SIZE);
+			card_select_method = MC_SELECT_CARDTYPE; // default
+
+			if (RTMPGetKeyParameter("SELECT", tmpbuf, 256, buffer))
+			{
+				if (strcmp(tmpbuf, "CARDID") == 0)
+					card_select_method = MC_SELECT_CARDID;
+				else if (strcmp(tmpbuf, "MAC") == 0)
+					card_select_method = MC_SELECT_MAC;
+				else if (strcmp(tmpbuf, "CARDTYPE") == 0)
+					card_select_method = MC_SELECT_CARDTYPE;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,
+					("MC> Card Selection = %d\n", card_select_method));
+
+			// init
+			card_free_id = -1;
+			card_nouse_id = -1;
+			card_same_mac_id = -1;
+			card_match_id = -1;
+
+			// search current card information records
+			for(card_index=0;
+				card_index<MAX_NUM_OF_MULTIPLE_CARD;
+				card_index++)
+			{
+				if ((*(UINT32 *)&MC_CardMac[card_index][0] == 0) &&
+					(*(UINT16 *)&MC_CardMac[card_index][4] == 0))
+				{
+					// MAC is all-0 so the entry is available
+					MC_CardUsed[card_index] = 0;
+
+					if (card_free_id < 0)
+						card_free_id = card_index; // 1st free entry
+				}
+				else
+				{
+					if (memcmp(MC_CardMac[card_index], mac, 6) == 0)
+					{
+						// we find the entry with same MAC
+						if (card_same_mac_id < 0)
+							card_same_mac_id = card_index; // 1st same entry
+					}
+					else
+					{
+						// MAC is not all-0 but used flag == 0
+						if ((MC_CardUsed[card_index] == 0) &&
+							(card_nouse_id < 0))
+						{
+							card_nouse_id = card_index; // 1st available entry
+						}
+					}
+				}
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,
+					("MC> Free = %d, Same = %d, NOUSE = %d\n",
+					card_free_id, card_same_mac_id, card_nouse_id));
+
+			if ((card_same_mac_id >= 0) &&
+				((card_select_method == MC_SELECT_CARDID) ||
+				(card_select_method == MC_SELECT_CARDTYPE)))
+			{
+				// same MAC entry is found
+				card_match_id = card_same_mac_id;
+
+				if (card_select_method == MC_SELECT_CARDTYPE)
+				{
+					// for CARDTYPE
+					sprintf(card_id_buf, "%02dCARDTYPE%s",
+							card_match_id, RFIC_word);
+
+					if ((start_ptr=rtstrstruncasecmp(buffer, card_id_buf)) != NULL)
+					{
+						// we found the card ID
+						LETTER_CASE_TRANSLATE(start_ptr, card_id_buf);
+					}
+				}
+			}
+			else
+			{
+				// the card is 1st plug-in, try to find the match card profile
+				switch(card_select_method)
+				{
+					case MC_SELECT_CARDID: // CARDID
+					default:
+						if (card_free_id >= 0)
+							card_match_id = card_free_id;
+						else
+							card_match_id = card_nouse_id;
+						break;
+
+					case MC_SELECT_MAC: // MAC
+						sprintf(card_id_buf, "MAC%02x:%02x:%02x:%02x:%02x:%02x",
+								mac[0], mac[1], mac[2],
+								mac[3], mac[4], mac[5]);
+
+						/* try to find the key word in the card file */
+						if ((start_ptr=rtstrstruncasecmp(buffer, card_id_buf)) != NULL)
+						{
+							LETTER_CASE_TRANSLATE(start_ptr, card_id_buf);
+
+							/* get the row ID (2 ASCII characters) */
+							start_ptr -= 2;
+							card_id_buf[0] = *(start_ptr);
+							card_id_buf[1] = *(start_ptr+1);
+							card_id_buf[2] = 0x00;
+
+							card_match_id = simple_strtol(card_id_buf, 0, 10);
+						}
+						break;
+
+					case MC_SELECT_CARDTYPE: // CARDTYPE
+						card_nouse_id = -1;
+
+						for(card_index=0;
+							card_index<MAX_NUM_OF_MULTIPLE_CARD;
+							card_index++)
+						{
+							sprintf(card_id_buf, "%02dCARDTYPE%s",
+									card_index, RFIC_word);
+
+							if ((start_ptr=rtstrstruncasecmp(buffer,
+														card_id_buf)) != NULL)
+							{
+								LETTER_CASE_TRANSLATE(start_ptr, card_id_buf);
+
+								if (MC_CardUsed[card_index] == 0)
+								{
+									/* current the card profile is not used */
+									if ((*(UINT32 *)&MC_CardMac[card_index][0] == 0) &&
+										(*(UINT16 *)&MC_CardMac[card_index][4] == 0))
+									{
+										// find it and no previous card use it
+										card_match_id = card_index;
+										break;
+									}
+									else
+									{
+										// ever a card use it
+										if (card_nouse_id < 0)
+											card_nouse_id = card_index;
+									}
+								}
+							}
+						}
+
+						// if not find a free one, use the available one
+						if (card_match_id < 0)
+							card_match_id = card_nouse_id;
+						break;
+				}
+			}
+
+			if (card_match_id >= 0)
+			{
+				// make up search keyword
+				switch(card_select_method)
+				{
+					case MC_SELECT_CARDID: // CARDID
+						sprintf(card_id_buf, "%02dCARDID", card_match_id);
+						break;
+
+					case MC_SELECT_MAC: // MAC
+						sprintf(card_id_buf,
+								"%02dmac%02x:%02x:%02x:%02x:%02x:%02x",
+								card_match_id,
+								mac[0], mac[1], mac[2],
+								mac[3], mac[4], mac[5]);
+						break;
+
+					case MC_SELECT_CARDTYPE: // CARDTYPE
+					default:
+						sprintf(card_id_buf, "%02dcardtype%s",
+								card_match_id, RFIC_word);
+						break;
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Search Keyword = %s\n", card_id_buf));
+
+				// read card file path
+				if (RTMPGetKeyParameter(card_id_buf, tmpbuf, 256, buffer))
+				{
+					if (strlen(tmpbuf) < sizeof(pAd->MC_FileName))
+					{
+						// backup card information
+						pAd->MC_RowID = card_match_id; /* base 0 */
+						MC_CardUsed[card_match_id] = 1;
+						memcpy(MC_CardMac[card_match_id], mac, sizeof(mac));
+
+						// backup card file path
+						NdisMoveMemory(pAd->MC_FileName, tmpbuf , strlen(tmpbuf));
+						pAd->MC_FileName[strlen(tmpbuf)] = '\0';
+						flg_match_ok = TRUE;
+
+						DBGPRINT(RT_DEBUG_TRACE,
+								("Card Profile Name = %s\n", pAd->MC_FileName));
+					}
+					else
+					{
+						DBGPRINT(RT_DEBUG_ERROR,
+								("Card Profile Name length too large!\n"));
+					}
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_ERROR,
+							("Can not find search key word in card.dat!\n"));
+				}
+
+				if ((flg_match_ok != TRUE) &&
+					(card_match_id < MAX_NUM_OF_MULTIPLE_CARD))
+				{
+					MC_CardUsed[card_match_id] = 0;
+					memset(MC_CardMac[card_match_id], 0, sizeof(mac));
+				}
+			} // if (card_match_id >= 0)
+		}
+	}
+
+	// close file
+	retval = filp_close(srcf, NULL);
+	set_fs(orgfs);
+	current->fsuid = orgfsuid;
+	current->fsgid = orgfsgid;
+	kfree(buffer);
+	kfree(tmpbuf);
+	return flg_match_ok;
+}
+#endif // MULTIPLE_CARD_SUPPORT //
+
+
+/*
+========================================================================
+Routine Description:
+    Probe RT28XX chipset.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+	_dev_id_p			Point to the PCI or USB device ID
+
+Return Value:
+    0					Probe OK
+	-ENODEV				Probe Fail
+
+Note:
+========================================================================
+*/
+INT __devinit   rt28xx_probe(
+    IN  void *_dev_p,
+    IN  void *_dev_id_p,
+	IN  UINT argc,
+	OUT PRTMP_ADAPTER *ppAd)
+{
+    struct  net_device	*net_dev;
+    PRTMP_ADAPTER       pAd = (PRTMP_ADAPTER) NULL;
+    INT                 status;
+	PVOID				handle;
+#ifdef RT2870
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+	struct usb_device *dev_p = (struct usb_device *)_dev_p;
+#else
+	struct usb_interface *intf = (struct usb_interface *)_dev_p;
+	struct usb_device *dev_p = interface_to_usbdev(intf);
+
+	dev_p = usb_get_dev(dev_p);
+#endif // LINUX_VERSION_CODE //
+#endif // RT2870 //
+
+
+#ifdef CONFIG_STA_SUPPORT
+    DBGPRINT(RT_DEBUG_TRACE, ("STA Driver version-%s\n", STA_DRIVER_VERSION));
+#endif // CONFIG_STA_SUPPORT //
+
+	// Check chipset vendor/product ID
+//	if (RT28XXChipsetCheck(_dev_p) == FALSE)
+//		goto err_out;
+
+#if LINUX_VERSION_CODE <= 0x20402       // Red Hat 7.1
+    net_dev = alloc_netdev(sizeof(PRTMP_ADAPTER), "eth%d", ether_setup);
+#else
+    net_dev = alloc_etherdev(sizeof(PRTMP_ADAPTER));
+#endif
+    if (net_dev == NULL)
+    {
+        printk("alloc_netdev failed\n");
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,15)
+		module_put(THIS_MODULE);
+#endif //LINUX_VERSION_CODE < KERNEL_VERSION(2,6,15)
+#else
+		MOD_DEC_USE_COUNT;
+#endif
+        goto err_out;
+    }
+
+// sample
+//	if (rt_ieee80211_if_setup(net_dev) != NDIS_STATUS_SUCCESS)
+//		goto err_out;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
+    SET_MODULE_OWNER(net_dev);
+#endif
+
+	netif_stop_queue(net_dev);
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+/* for supporting Network Manager */
+/* Set the sysfs physical device reference for the network logical device
+ * if set prior to registration will cause a symlink during initialization.
+ */
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0))
+    SET_NETDEV_DEV(net_dev, &(dev_p->dev));
+#endif
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+	// Allocate RTMP_ADAPTER miniport adapter structure
+	handle = kmalloc(sizeof(struct os_cookie), GFP_KERNEL);
+	RT28XX_HANDLE_DEV_ASSIGN(handle, dev_p);
+
+	status = RTMPAllocAdapterBlock(handle, &pAd);
+	if (status != NDIS_STATUS_SUCCESS)
+		goto err_out_free_netdev;
+
+	net_dev->ml_priv = (PVOID)pAd;
+    pAd->net_dev = net_dev; // must be before RT28XXNetDevInit()
+
+	RT28XXNetDevInit(_dev_p, net_dev, pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+    pAd->StaCfg.OriDevType = net_dev->type;
+#endif // CONFIG_STA_SUPPORT //
+
+	// Find and assign a free interface name, raxx
+//	RT28XXAvailRANameAssign(net_dev->name);
+
+	// Post config
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+	if (RT28XXProbePostConfig(_dev_p, pAd, argc) == FALSE)
+		goto err_out_unmap;
+#else
+	if (RT28XXProbePostConfig(_dev_p, pAd, 0) == FALSE)
+		goto err_out_unmap;
+#endif // LINUX_VERSION_CODE //
+
+#ifdef CONFIG_STA_SUPPORT
+	pAd->OpMode = OPMODE_STA;
+#endif // CONFIG_STA_SUPPORT //
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+	// find its profile path
+	pAd->MC_RowID = -1; // use default profile path
+	RTMP_CardInfoRead(pAd);
+
+	if (pAd->MC_RowID == -1)
+#ifdef CONFIG_STA_SUPPORT
+		strcpy(pAd->MC_FileName, STA_PROFILE_PATH);
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE,
+			("MC> ROW = %d, PATH = %s\n", pAd->MC_RowID, pAd->MC_FileName));
+#endif // MULTIPLE_CARD_SUPPORT //
+
+	// sample move
+	if (rt_ieee80211_if_setup(net_dev, pAd) != NDIS_STATUS_SUCCESS)
+		goto err_out_unmap;
+
+    // Register this device
+    status = register_netdev(net_dev);
+    if (status)
+        goto err_out_unmap;
+
+    // Set driver data
+	RT28XX_DRVDATA_SET(_dev_p);
+
+
+
+	*ppAd = pAd;
+    return 0; // probe ok
+
+
+	/* --------------------------- ERROR HANDLE --------------------------- */
+err_out_unmap:
+	RTMPFreeAdapter(pAd);
+	RT28XX_UNMAP();
+
+err_out_free_netdev:
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+    free_netdev(net_dev);
+#else
+	kfree(net_dev);
+#endif
+
+err_out:
+	RT28XX_PUT_DEVICE(dev_p);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+	return (LONG)NULL;
+#else
+    return -ENODEV; /* probe fail */
+#endif // LINUX_VERSION_CODE //
+} /* End of rt28xx_probe */
+
+
+/*
+========================================================================
+Routine Description:
+    The entry point for Linux kernel sent packet to our driver.
+
+Arguments:
+    sk_buff *skb		the pointer refer to a sk_buffer.
+
+Return Value:
+    0
+
+Note:
+	This function is the entry point of Tx Path for Os delivery packet to
+	our driver. You only can put OS-depened & STA/AP common handle procedures
+	in here.
+========================================================================
+*/
+int rt28xx_packet_xmit(struct sk_buff *skb)
+{
+	struct net_device *net_dev = skb->dev;
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+	int status = 0;
+	PNDIS_PACKET pPacket = (PNDIS_PACKET) skb;
+
+	/* RT2870STA does this in RTMPSendPackets() */
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_RESOURCES);
+		return 0;
+	}
+#endif // RALINK_ATE //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Drop send request since we are in monitor mode
+		if (MONITOR_ON(pAd))
+		{
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+			goto done;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+        // EapolStart size is 18
+	if (skb->len < 14)
+	{
+		//printk("bad packet size: %d\n", pkt->len);
+		hex_dump("bad packet", skb->data, skb->len);
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		goto done;
+	}
+
+#if 0
+//	if ((pkt->data[0] & 0x1) == 0)
+	{
+		//hex_dump(__func__, pkt->data, pkt->len);
+		printk("pPacket = %x\n", pPacket);
+	}
+#endif
+
+	RTMP_SET_PACKET_5VT(pPacket, 0);
+//	MiniportMMRequest(pAd, pkt->data, pkt->len);
+#ifdef CONFIG_5VT_ENHANCE
+    if (*(int*)(skb->cb) == BRIDGE_TAG) {
+		RTMP_SET_PACKET_5VT(pPacket, 1);
+    }
+#endif
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+
+		STASendPackets((NDIS_HANDLE)pAd, (PPNDIS_PACKET) &pPacket, 1);
+	}
+
+#endif // CONFIG_STA_SUPPORT //
+
+	status = 0;
+done:
+
+	return status;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Send a packet to WLAN.
+
+Arguments:
+    skb_p           points to our adapter
+    dev_p           which WLAN network interface
+
+Return Value:
+    0: transmit successfully
+    otherwise: transmit fail
+
+Note:
+========================================================================
+*/
+INT rt28xx_send_packets(
+	IN struct sk_buff 		*skb_p,
+	IN struct net_device 	*net_dev)
+{
+    RTMP_ADAPTER *pAd = net_dev->ml_priv;
+
+	if (!(net_dev->flags & IFF_UP))
+	{
+		RELEASE_NDIS_PACKET(pAd, (PNDIS_PACKET)skb_p, NDIS_STATUS_FAILURE);
+		return 0;
+	}
+
+	NdisZeroMemory((PUCHAR)&skb_p->cb[CB_OFF], 15);
+	RTMP_SET_PACKET_NET_DEVICE_MBSSID(skb_p, MAIN_MBSSID);
+
+	return rt28xx_packet_xmit(skb_p);
+} /* End of MBSS_VirtualIF_PacketSend */
+
+
+
+
+#if LINUX_VERSION_CODE <= 0x20402	// Red Hat 7.1
+//static struct net_device *alloc_netdev(int sizeof_priv, const char *mask, void (*setup)(struct net_device *)) //sample
+struct net_device *alloc_netdev(
+	int sizeof_priv,
+	const char *mask,
+	void (*setup)(struct net_device *))
+{
+    struct net_device	*dev;
+    INT					alloc_size;
+
+
+    /* ensure 32-byte alignment of the private area */
+    alloc_size = sizeof (*dev) + sizeof_priv + 31;
+
+    dev = (struct net_device *) kmalloc(alloc_size, GFP_KERNEL);
+    if (dev == NULL)
+    {
+        DBGPRINT(RT_DEBUG_ERROR,
+				("alloc_netdev: Unable to allocate device memory.\n"));
+        return NULL;
+    }
+
+    memset(dev, 0, alloc_size);
+
+    if (sizeof_priv)
+        dev->priv = (void *) (((long)(dev + 1) + 31) & ~31);
+
+    setup(dev);
+    strcpy(dev->name, mask);
+
+    return dev;
+}
+#endif // LINUX_VERSION_CODE //
+
+
+void CfgInitHook(PRTMP_ADAPTER pAd)
+{
+	pAd->bBroadComHT = TRUE;
+} /* End of CfgInitHook */
+
+
+#if 0	// Not used now, should keep it in our source tree??
+/*
+========================================================================
+Routine Description:
+    Find and assign a free interface name (raxx).
+
+Arguments:
+    *name_p				the interface name pointer
+
+Return Value:
+	TRUE				OK
+	FALSE				FAIL
+
+Note:
+========================================================================
+*/
+static BOOLEAN RT28XXAvailRANameAssign(
+	IN CHAR			*name_p)
+{
+    CHAR				slot_name[IFNAMSIZ];
+    struct net_device	*device;
+	UINT32				if_id;
+
+
+    for(if_id=0; if_id<8; if_id++)
+    {
+        sprintf(slot_name, "ra%d", if_id);
+
+        for(device=dev_base; device!=NULL; device=device->next)
+        {
+            if (strncmp(device->name, slot_name, 4) == 0)
+                break;
+        }
+
+        if (device == NULL)
+			break;
+    }
+
+    if (if_id == 8)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("No available slot name\n"));
+        return FALSE;
+    }
+
+    sprintf(name_p, "ra%d", if_id);
+	return TRUE;
+} /* End of RT28XXAvailRANameAssign */
+#endif
+
+#if WIRELESS_EXT >= 12
+// This function will be called when query /proc
+struct iw_statistics *rt28xx_get_wireless_stats(
+    IN struct net_device *net_dev)
+{
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("rt28xx_get_wireless_stats --->\n"));
+
+	pAd->iw_stats.status = 0; // Status - device dependent for now
+
+	// link quality
+	pAd->iw_stats.qual.qual = ((pAd->Mlme.ChannelQuality * 12)/10 + 10);
+	if(pAd->iw_stats.qual.qual > 100)
+		pAd->iw_stats.qual.qual = 100;
+
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->OpMode == OPMODE_STA)
+		pAd->iw_stats.qual.level = RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2);
+#endif // CONFIG_STA_SUPPORT //
+
+	pAd->iw_stats.qual.noise = pAd->BbpWriteLatch[66]; // noise level (dBm)
+
+	pAd->iw_stats.qual.noise += 256 - 143;
+	pAd->iw_stats.qual.updated = 1;     // Flags to know if updated
+#ifdef IW_QUAL_DBM
+	pAd->iw_stats.qual.updated |= IW_QUAL_DBM;	// Level + Noise are dBm
+#endif // IW_QUAL_DBM //
+
+	pAd->iw_stats.discard.nwid = 0;     // Rx : Wrong nwid/essid
+	pAd->iw_stats.miss.beacon = 0;      // Missed beacons/superframe
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<--- rt28xx_get_wireless_stats\n"));
+	return &pAd->iw_stats;
+} /* End of rt28xx_get_wireless_stats */
+#endif // WIRELESS_EXT //
+
+
+
+void tbtt_tasklet(unsigned long data)
+{
+#define MAX_TX_IN_TBTT		(16)
+
+}
+
+INT rt28xx_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT					cmd)
+{
+	VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+	RTMP_ADAPTER	*pAd = NULL;
+	INT				ret = 0;
+
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		pAd = net_dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = net_dev->ml_priv;
+		pAd = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		ret = rt28xx_sta_ioctl(net_dev, rq, cmd);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	return ret;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        return ethernet statistics counter
+
+    Arguments:
+        net_dev                     Pointer to net_device
+
+    Return Value:
+        net_device_stats*
+
+    Note:
+
+    ========================================================================
+*/
+struct net_device_stats *RT28xx_get_ether_stats(
+    IN  struct net_device *net_dev)
+{
+    RTMP_ADAPTER *pAd = NULL;
+
+	if (net_dev)
+		pAd = net_dev->ml_priv;
+
+	if (pAd)
+	{
+
+		pAd->stats.rx_packets = pAd->WlanCounters.ReceivedFragmentCount.QuadPart;
+		pAd->stats.tx_packets = pAd->WlanCounters.TransmittedFragmentCount.QuadPart;
+
+		pAd->stats.rx_bytes = pAd->RalinkCounters.ReceivedByteCount;
+		pAd->stats.tx_bytes = pAd->RalinkCounters.TransmittedByteCount;
+
+		pAd->stats.rx_errors = pAd->Counters8023.RxErrors;
+		pAd->stats.tx_errors = pAd->Counters8023.TxErrors;
+
+		pAd->stats.rx_dropped = 0;
+		pAd->stats.tx_dropped = 0;
+
+	    pAd->stats.multicast = pAd->WlanCounters.MulticastReceivedFrameCount.QuadPart;   // multicast packets received
+	    pAd->stats.collisions = pAd->Counters8023.OneCollision + pAd->Counters8023.MoreCollisions;  // Collision packets
+
+	    pAd->stats.rx_length_errors = 0;
+	    pAd->stats.rx_over_errors = pAd->Counters8023.RxNoBuffer;                   // receiver ring buff overflow
+	    pAd->stats.rx_crc_errors = 0;//pAd->WlanCounters.FCSErrorCount;     // recved pkt with crc error
+	    pAd->stats.rx_frame_errors = pAd->Counters8023.RcvAlignmentErrors;          // recv'd frame alignment error
+	    pAd->stats.rx_fifo_errors = pAd->Counters8023.RxNoBuffer;                   // recv'r fifo overrun
+	    pAd->stats.rx_missed_errors = 0;                                            // receiver missed packet
+
+	    // detailed tx_errors
+	    pAd->stats.tx_aborted_errors = 0;
+	    pAd->stats.tx_carrier_errors = 0;
+	    pAd->stats.tx_fifo_errors = 0;
+	    pAd->stats.tx_heartbeat_errors = 0;
+	    pAd->stats.tx_window_errors = 0;
+
+	    // for cslip etc
+	    pAd->stats.rx_compressed = 0;
+	    pAd->stats.tx_compressed = 0;
+
+		return &pAd->stats;
+	}
+	else
+    	return NULL;
+}
+
diff --git a/drivers/staging/rt2870/rt_profile.c b/drivers/staging/rt2870/rt_profile.c
new file mode 100644
index 000000000000..467fea35e4a9
--- /dev/null
+++ b/drivers/staging/rt2870/rt_profile.c
@@ -0,0 +1,2020 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "rt_config.h"
+
+#ifdef DOT11_N_SUPPORT
+static void HTParametersHook(
+	IN	PRTMP_ADAPTER pAd,
+	IN	CHAR		  *pValueStr,
+	IN	CHAR		  *pInput);
+#endif // DOT11_N_SUPPORT //
+
+#define ETH_MAC_ADDR_STR_LEN 17  // in format of xx:xx:xx:xx:xx:xx
+
+// We assume the s1 is a sting, s2 is a memory space with 6 bytes. and content of s1 will be changed.
+BOOLEAN rtstrmactohex(char *s1, char *s2)
+{
+	int i = 0;
+	char *ptokS = s1, *ptokE = s1;
+
+	if (strlen(s1) != ETH_MAC_ADDR_STR_LEN)
+		return FALSE;
+
+	while((*ptokS) != '\0')
+	{
+		if((ptokE = strchr(ptokS, ':')) != NULL)
+			*ptokE++ = '\0';
+		if ((strlen(ptokS) != 2) || (!isxdigit(*ptokS)) || (!isxdigit(*(ptokS+1))))
+			break; // fail
+		AtoH(ptokS, &s2[i++], 1);
+		ptokS = ptokE;
+		if (i == 6)
+			break; // parsing finished
+	}
+
+	return ( i == 6 ? TRUE : FALSE);
+
+}
+
+
+// we assume the s1 and s2 both are strings.
+BOOLEAN rtstrcasecmp(char *s1, char *s2)
+{
+	char *p1 = s1, *p2 = s2;
+
+	if (strlen(s1) != strlen(s2))
+		return FALSE;
+
+	while(*p1 != '\0')
+	{
+		if((*p1 != *p2) && ((*p1 ^ *p2) != 0x20))
+			return FALSE;
+		p1++;
+		p2++;
+	}
+
+	return TRUE;
+}
+
+// we assume the s1 (buffer) and s2 (key) both are strings.
+char * rtstrstruncasecmp(char * s1, char * s2)
+{
+	INT l1, l2, i;
+	char temp1, temp2;
+
+	l2 = strlen(s2);
+	if (!l2)
+		return (char *) s1;
+
+	l1 = strlen(s1);
+
+	while (l1 >= l2)
+	{
+		l1--;
+
+		for(i=0; i<l2; i++)
+		{
+			temp1 = *(s1+i);
+			temp2 = *(s2+i);
+
+			if (('a' <= temp1) && (temp1 <= 'z'))
+				temp1 = 'A'+(temp1-'a');
+			if (('a' <= temp2) && (temp2 <= 'z'))
+				temp2 = 'A'+(temp2-'a');
+
+			if (temp1 != temp2)
+				break;
+		}
+
+		if (i == l2)
+			return (char *) s1;
+
+		s1++;
+	}
+
+	return NULL; // not found
+}
+
+//add by kathy
+
+ /**
+  * strstr - Find the first substring in a %NUL terminated string
+  * @s1: The string to be searched
+  * @s2: The string to search for
+  */
+char * rtstrstr(const char * s1,const char * s2)
+{
+	INT l1, l2;
+
+	l2 = strlen(s2);
+	if (!l2)
+		return (char *) s1;
+
+	l1 = strlen(s1);
+
+	while (l1 >= l2)
+	{
+		l1--;
+		if (!memcmp(s1,s2,l2))
+			return (char *) s1;
+		s1++;
+	}
+
+	return NULL;
+}
+
+/**
+ * rstrtok - Split a string into tokens
+ * @s: The string to be searched
+ * @ct: The characters to search for
+ * * WARNING: strtok is deprecated, use strsep instead. However strsep is not compatible with old architecture.
+ */
+char * __rstrtok;
+char * rstrtok(char * s,const char * ct)
+{
+	char *sbegin, *send;
+
+	sbegin  = s ? s : __rstrtok;
+	if (!sbegin)
+	{
+		return NULL;
+	}
+
+	sbegin += strspn(sbegin,ct);
+	if (*sbegin == '\0')
+	{
+		__rstrtok = NULL;
+		return( NULL );
+	}
+
+	send = strpbrk( sbegin, ct);
+	if (send && *send != '\0')
+		*send++ = '\0';
+
+	__rstrtok = send;
+
+	return (sbegin);
+}
+
+/**
+ * delimitcnt - return the count of a given delimiter in a given string.
+ * @s: The string to be searched.
+ * @ct: The delimiter to search for.
+ * Notice : We suppose the delimiter is a single-char string(for example : ";").
+ */
+INT delimitcnt(char * s,const char * ct)
+{
+	INT count = 0;
+	/* point to the beginning of the line */
+	const char *token = s;
+
+	for ( ;; )
+	{
+		token = strpbrk(token, ct); /* search for delimiters */
+
+        if ( token == NULL )
+		{
+			/* advanced to the terminating null character */
+			break;
+		}
+		/* skip the delimiter */
+	    ++token;
+
+		/*
+		 * Print the found text: use len with %.*s to specify field width.
+		 */
+
+		/* accumulate delimiter count */
+	    ++count;
+	}
+    return count;
+}
+
+/*
+  * converts the Internet host address from the standard numbers-and-dots notation
+  * into binary data.
+  * returns nonzero if the address is valid, zero if not.
+  */
+int rtinet_aton(const char *cp, unsigned int *addr)
+{
+	unsigned int 	val;
+	int         	base, n;
+	char        	c;
+	unsigned int    parts[4];
+	unsigned int    *pp = parts;
+
+	for (;;)
+    {
+         /*
+          * Collect number up to ``.''.
+          * Values are specified as for C:
+          *	0x=hex, 0=octal, other=decimal.
+          */
+         val = 0;
+         base = 10;
+         if (*cp == '0')
+         {
+             if (*++cp == 'x' || *cp == 'X')
+                 base = 16, cp++;
+             else
+                 base = 8;
+         }
+         while ((c = *cp) != '\0')
+         {
+             if (isdigit((unsigned char) c))
+             {
+                 val = (val * base) + (c - '0');
+                 cp++;
+                 continue;
+             }
+             if (base == 16 && isxdigit((unsigned char) c))
+             {
+                 val = (val << 4) +
+                     (c + 10 - (islower((unsigned char) c) ? 'a' : 'A'));
+                 cp++;
+                 continue;
+             }
+             break;
+         }
+         if (*cp == '.')
+         {
+             /*
+              * Internet format: a.b.c.d a.b.c   (with c treated as 16-bits)
+              * a.b     (with b treated as 24 bits)
+              */
+             if (pp >= parts + 3 || val > 0xff)
+                 return 0;
+             *pp++ = val, cp++;
+         }
+         else
+             break;
+     }
+
+     /*
+      * Check for trailing junk.
+      */
+     while (*cp)
+         if (!isspace((unsigned char) *cp++))
+             return 0;
+
+     /*
+      * Concoct the address according to the number of parts specified.
+      */
+     n = pp - parts + 1;
+     switch (n)
+     {
+
+         case 1:         /* a -- 32 bits */
+             break;
+
+         case 2:         /* a.b -- 8.24 bits */
+             if (val > 0xffffff)
+                 return 0;
+             val |= parts[0] << 24;
+             break;
+
+         case 3:         /* a.b.c -- 8.8.16 bits */
+             if (val > 0xffff)
+                 return 0;
+             val |= (parts[0] << 24) | (parts[1] << 16);
+             break;
+
+         case 4:         /* a.b.c.d -- 8.8.8.8 bits */
+             if (val > 0xff)
+                 return 0;
+             val |= (parts[0] << 24) | (parts[1] << 16) | (parts[2] << 8);
+             break;
+     }
+
+     *addr = htonl(val);
+     return 1;
+
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Find key section for Get key parameter.
+
+    Arguments:
+        buffer                      Pointer to the buffer to start find the key section
+        section                     the key of the secion to be find
+
+    Return Value:
+        NULL                        Fail
+        Others                      Success
+    ========================================================================
+*/
+PUCHAR  RTMPFindSection(
+    IN  PCHAR   buffer)
+{
+    CHAR temp_buf[32];
+    PUCHAR  ptr;
+
+    strcpy(temp_buf, "Default");
+
+    if((ptr = rtstrstr(buffer, temp_buf)) != NULL)
+            return (ptr+strlen("\n"));
+        else
+            return NULL;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Get key parameter.
+
+    Arguments:
+        key                         Pointer to key string
+        dest                        Pointer to destination
+        destsize                    The datasize of the destination
+        buffer                      Pointer to the buffer to start find the key
+
+    Return Value:
+        TRUE                        Success
+        FALSE                       Fail
+
+    Note:
+        This routine get the value with the matched key (case case-sensitive)
+    ========================================================================
+*/
+INT RTMPGetKeyParameter(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    IN  INT     destsize,
+    IN  PCHAR   buffer)
+{
+    UCHAR *temp_buf1 = NULL;
+    UCHAR *temp_buf2 = NULL;
+    CHAR *start_ptr;
+    CHAR *end_ptr;
+    CHAR *ptr;
+    CHAR *offset = 0;
+    INT  len;
+
+	//temp_buf1 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf1, MAX_PARAM_BUFFER_SIZE);
+
+	if(temp_buf1 == NULL)
+        return (FALSE);
+
+	//temp_buf2 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf2, MAX_PARAM_BUFFER_SIZE);
+	if(temp_buf2 == NULL)
+	{
+		os_free_mem(NULL, temp_buf1);
+        return (FALSE);
+	}
+
+    //find section
+    if((offset = RTMPFindSection(buffer)) == NULL)
+    {
+    	os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf1, "\n");
+    strcat(temp_buf1, key);
+    strcat(temp_buf1, "=");
+
+    //search key
+    if((start_ptr=rtstrstr(offset, temp_buf1))==NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    start_ptr+=strlen("\n");
+    if((end_ptr=rtstrstr(start_ptr, "\n"))==NULL)
+       end_ptr=start_ptr+strlen(start_ptr);
+
+    if (end_ptr<start_ptr)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    NdisMoveMemory(temp_buf2, start_ptr, end_ptr-start_ptr);
+    temp_buf2[end_ptr-start_ptr]='\0';
+    len = strlen(temp_buf2);
+    strcpy(temp_buf1, temp_buf2);
+    if((start_ptr=rtstrstr(temp_buf1, "=")) == NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf2, start_ptr+1);
+    ptr = temp_buf2;
+    //trim space or tab
+    while(*ptr != 0x00)
+    {
+        if( (*ptr == ' ') || (*ptr == '\t') )
+            ptr++;
+        else
+           break;
+    }
+
+    len = strlen(ptr);
+    memset(dest, 0x00, destsize);
+    strncpy(dest, ptr, len >= destsize ?  destsize: len);
+
+	os_free_mem(NULL, temp_buf1);
+    os_free_mem(NULL, temp_buf2);
+    return TRUE;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Get key parameter.
+
+    Arguments:
+        key                         Pointer to key string
+        dest                        Pointer to destination
+        destsize                    The datasize of the destination
+        buffer                      Pointer to the buffer to start find the key
+
+    Return Value:
+        TRUE                        Success
+        FALSE                       Fail
+
+    Note:
+        This routine get the value with the matched key (case case-sensitive).
+        It is called for parsing SSID and any key string.
+    ========================================================================
+*/
+INT RTMPGetCriticalParameter(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    IN  INT     destsize,
+    IN  PCHAR   buffer)
+{
+    UCHAR *temp_buf1 = NULL;
+    UCHAR *temp_buf2 = NULL;
+    CHAR *start_ptr;
+    CHAR *end_ptr;
+    CHAR *ptr;
+    CHAR *offset = 0;
+    INT  len;
+
+	//temp_buf1 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf1, MAX_PARAM_BUFFER_SIZE);
+
+	if(temp_buf1 == NULL)
+        return (FALSE);
+
+	//temp_buf2 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf2, MAX_PARAM_BUFFER_SIZE);
+	if(temp_buf2 == NULL)
+	{
+		os_free_mem(NULL, temp_buf1);
+        return (FALSE);
+	}
+
+    //find section
+    if((offset = RTMPFindSection(buffer)) == NULL)
+    {
+    	os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf1, "\n");
+    strcat(temp_buf1, key);
+    strcat(temp_buf1, "=");
+
+    //search key
+    if((start_ptr=rtstrstr(offset, temp_buf1))==NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    start_ptr+=strlen("\n");
+    if((end_ptr=rtstrstr(start_ptr, "\n"))==NULL)
+       end_ptr=start_ptr+strlen(start_ptr);
+
+    if (end_ptr<start_ptr)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    NdisMoveMemory(temp_buf2, start_ptr, end_ptr-start_ptr);
+    temp_buf2[end_ptr-start_ptr]='\0';
+    len = strlen(temp_buf2);
+    strcpy(temp_buf1, temp_buf2);
+    if((start_ptr=rtstrstr(temp_buf1, "=")) == NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf2, start_ptr+1);
+    ptr = temp_buf2;
+
+    //trim tab
+    /* We cannot trim space(' ') for SSID and key string. */
+    while(*ptr != 0x00)
+    {
+        //if( (*ptr == ' ') || (*ptr == '\t') )
+        if( (*ptr == '\t') )
+            ptr++;
+        else
+           break;
+    }
+
+    len = strlen(ptr);
+    memset(dest, 0x00, destsize);
+    strncpy(dest, ptr, len >= destsize ?  destsize: len);
+
+	os_free_mem(NULL, temp_buf1);
+    os_free_mem(NULL, temp_buf2);
+    return TRUE;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Get multiple key parameter.
+
+    Arguments:
+        key                         Pointer to key string
+        dest                        Pointer to destination
+        destsize                    The datasize of the destination
+        buffer                      Pointer to the buffer to start find the key
+
+    Return Value:
+        TRUE                        Success
+        FALSE                       Fail
+
+    Note:
+        This routine get the value with the matched key (case case-sensitive)
+    ========================================================================
+*/
+INT RTMPGetKeyParameterWithOffset(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    OUT	USHORT	*end_offset,
+    IN  INT     destsize,
+    IN  PCHAR   buffer,
+    IN	BOOLEAN	bTrimSpace)
+{
+    UCHAR *temp_buf1 = NULL;
+    UCHAR *temp_buf2 = NULL;
+    CHAR *start_ptr;
+    CHAR *end_ptr;
+    CHAR *ptr;
+    CHAR *offset = 0;
+    INT  len;
+
+	if (*end_offset >= MAX_INI_BUFFER_SIZE)
+		return (FALSE);
+
+	os_alloc_mem(NULL, &temp_buf1, MAX_PARAM_BUFFER_SIZE);
+
+	if(temp_buf1 == NULL)
+        return (FALSE);
+
+	os_alloc_mem(NULL, &temp_buf2, MAX_PARAM_BUFFER_SIZE);
+	if(temp_buf2 == NULL)
+	{
+		os_free_mem(NULL, temp_buf1);
+        return (FALSE);
+	}
+
+    //find section
+	if(*end_offset == 0)
+    {
+		if ((offset = RTMPFindSection(buffer)) == NULL)
+		{
+			os_free_mem(NULL, temp_buf1);
+	    	os_free_mem(NULL, temp_buf2);
+    	    return (FALSE);
+		}
+    }
+	else
+		offset = buffer + (*end_offset);
+
+    strcpy(temp_buf1, "\n");
+    strcat(temp_buf1, key);
+    strcat(temp_buf1, "=");
+
+    //search key
+    if((start_ptr=rtstrstr(offset, temp_buf1))==NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    start_ptr+=strlen("\n");
+    if((end_ptr=rtstrstr(start_ptr, "\n"))==NULL)
+       end_ptr=start_ptr+strlen(start_ptr);
+
+    if (end_ptr<start_ptr)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+	*end_offset = end_ptr - buffer;
+
+    NdisMoveMemory(temp_buf2, start_ptr, end_ptr-start_ptr);
+    temp_buf2[end_ptr-start_ptr]='\0';
+    len = strlen(temp_buf2);
+    strcpy(temp_buf1, temp_buf2);
+    if((start_ptr=rtstrstr(temp_buf1, "=")) == NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf2, start_ptr+1);
+    ptr = temp_buf2;
+    //trim space or tab
+    while(*ptr != 0x00)
+    {
+        if((bTrimSpace && (*ptr == ' ')) || (*ptr == '\t') )
+            ptr++;
+        else
+           break;
+    }
+
+    len = strlen(ptr);
+    memset(dest, 0x00, destsize);
+    strncpy(dest, ptr, len >= destsize ?  destsize: len);
+
+	os_free_mem(NULL, temp_buf1);
+    os_free_mem(NULL, temp_buf2);
+    return TRUE;
+}
+
+
+static int rtmp_parse_key_buffer_from_file(IN  PRTMP_ADAPTER pAd,IN  char *buffer,IN  ULONG KeyType,IN  INT BSSIdx,IN  INT KeyIdx)
+{
+	PUCHAR		keybuff;
+	INT			i = BSSIdx, idx = KeyIdx;
+	ULONG		KeyLen;
+	UCHAR		CipherAlg = CIPHER_WEP64;
+
+	keybuff = buffer;
+	KeyLen = strlen(keybuff);
+
+	if (KeyType == 1)
+	{//Ascii
+		if( (KeyLen == 5) || (KeyLen == 13))
+		{
+			pAd->SharedKey[i][idx].KeyLen = KeyLen;
+			NdisMoveMemory(pAd->SharedKey[i][idx].Key, keybuff, KeyLen);
+			if (KeyLen == 5)
+				CipherAlg = CIPHER_WEP64;
+			else
+				CipherAlg = CIPHER_WEP128;
+			pAd->SharedKey[i][idx].CipherAlg = CipherAlg;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("I/F(ra%d) Key%dStr=%s and type=%s\n", i, idx+1, keybuff, (KeyType == 0) ? "Hex":"Ascii"));
+			return 1;
+		}
+		else
+		{//Invalid key length
+			DBGPRINT(RT_DEBUG_ERROR, ("Key%dStr is Invalid key length! KeyLen = %ld!\n", idx+1, KeyLen));
+			return 0;
+		}
+	}
+	else
+	{//Hex type
+		if( (KeyLen == 10) || (KeyLen == 26))
+		{
+			pAd->SharedKey[i][idx].KeyLen = KeyLen / 2;
+			AtoH(keybuff, pAd->SharedKey[i][idx].Key, KeyLen / 2);
+			if (KeyLen == 10)
+				CipherAlg = CIPHER_WEP64;
+			else
+				CipherAlg = CIPHER_WEP128;
+			pAd->SharedKey[i][idx].CipherAlg = CipherAlg;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("I/F(ra%d) Key%dStr=%s and type=%s\n", i, idx+1, keybuff, (KeyType == 0) ? "Hex":"Ascii"));
+			return 1;
+		}
+		else
+		{//Invalid key length
+			DBGPRINT(RT_DEBUG_ERROR, ("I/F(ra%d) Key%dStr is Invalid key length! KeyLen = %ld!\n", i, idx+1, KeyLen));
+			return 0;
+		}
+	}
+}
+static void rtmp_read_key_parms_from_file(IN  PRTMP_ADAPTER pAd, char *tmpbuf, char *buffer)
+{
+	char		tok_str[16];
+	PUCHAR		macptr;
+	INT			i = 0, idx;
+	ULONG		KeyType[MAX_MBSSID_NUM];
+	ULONG		KeyIdx;
+
+	NdisZeroMemory(KeyType, MAX_MBSSID_NUM);
+
+	//DefaultKeyID
+	if(RTMPGetKeyParameter("DefaultKeyID", tmpbuf, 25, buffer))
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			KeyIdx = simple_strtol(tmpbuf, 0, 10);
+			if((KeyIdx >= 1 ) && (KeyIdx <= 4))
+				pAd->StaCfg.DefaultKeyId = (UCHAR) (KeyIdx - 1);
+			else
+				pAd->StaCfg.DefaultKeyId = 0;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("DefaultKeyID(0~3)=%d\n", pAd->StaCfg.DefaultKeyId));
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+	for (idx = 0; idx < 4; idx++)
+	{
+		sprintf(tok_str, "Key%dType", idx + 1);
+		//Key1Type
+		if (RTMPGetKeyParameter(tok_str, tmpbuf, 128, buffer))
+		{
+		    for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+		    {
+			    KeyType[i] = simple_strtol(macptr, 0, 10);
+		    }
+
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			{
+				sprintf(tok_str, "Key%dStr", idx + 1);
+				if (RTMPGetCriticalParameter(tok_str, tmpbuf, 128, buffer))
+				{
+					rtmp_parse_key_buffer_from_file(pAd, tmpbuf, KeyType[BSS0], BSS0, idx);
+				}
+			}
+#endif // CONFIG_STA_SUPPORT //
+		}
+	}
+}
+
+
+#ifdef CONFIG_STA_SUPPORT
+static void rtmp_read_sta_wmm_parms_from_file(IN  PRTMP_ADAPTER pAd, char *tmpbuf, char *buffer)
+{
+	PUCHAR					macptr;
+	INT						i=0;
+	BOOLEAN					bWmmEnable = FALSE;
+
+	//WmmCapable
+	if(RTMPGetKeyParameter("WmmCapable", tmpbuf, 32, buffer))
+	{
+		if(simple_strtol(tmpbuf, 0, 10) != 0) //Enable
+		{
+			pAd->CommonCfg.bWmmCapable = TRUE;
+			bWmmEnable = TRUE;
+		}
+		else //Disable
+		{
+			pAd->CommonCfg.bWmmCapable = FALSE;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("WmmCapable=%d\n", pAd->CommonCfg.bWmmCapable));
+	}
+
+#ifdef QOS_DLS_SUPPORT
+	//DLSCapable
+	if(RTMPGetKeyParameter("DLSCapable", tmpbuf, 32, buffer))
+	{
+		if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+		{
+			pAd->CommonCfg.bDLSCapable = TRUE;
+		}
+		else //Disable
+		{
+			pAd->CommonCfg.bDLSCapable = FALSE;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("bDLSCapable=%d\n", pAd->CommonCfg.bDLSCapable));
+	}
+#endif // QOS_DLS_SUPPORT //
+
+	//AckPolicy for AC_BK, AC_BE, AC_VI, AC_VO
+	if(RTMPGetKeyParameter("AckPolicy", tmpbuf, 32, buffer))
+	{
+		for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+		{
+			pAd->CommonCfg.AckPolicy[i] = (UCHAR)simple_strtol(macptr, 0, 10);
+
+			DBGPRINT(RT_DEBUG_TRACE, ("AckPolicy[%d]=%d\n", i, pAd->CommonCfg.AckPolicy[i]));
+		}
+	}
+
+	if (bWmmEnable)
+	{
+		//APSDCapable
+		if(RTMPGetKeyParameter("APSDCapable", tmpbuf, 10, buffer))
+		{
+			if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+				pAd->CommonCfg.bAPSDCapable = TRUE;
+			else
+				pAd->CommonCfg.bAPSDCapable = FALSE;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("APSDCapable=%d\n", pAd->CommonCfg.bAPSDCapable));
+		}
+
+		//APSDAC for AC_BE, AC_BK, AC_VI, AC_VO
+		if(RTMPGetKeyParameter("APSDAC", tmpbuf, 32, buffer))
+		{
+			BOOLEAN apsd_ac[4];
+
+			for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+			{
+				apsd_ac[i] = (BOOLEAN)simple_strtol(macptr, 0, 10);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("APSDAC%d  %d\n", i,  apsd_ac[i]));
+			}
+
+			pAd->CommonCfg.bAPSDAC_BE = apsd_ac[0];
+			pAd->CommonCfg.bAPSDAC_BK = apsd_ac[1];
+			pAd->CommonCfg.bAPSDAC_VI = apsd_ac[2];
+			pAd->CommonCfg.bAPSDAC_VO = apsd_ac[3];
+		}
+	}
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+NDIS_STATUS	RTMPReadParametersHook(
+	IN	PRTMP_ADAPTER pAd)
+{
+	PUCHAR					src = NULL;
+	struct file				*srcf;
+	INT 					retval, orgfsuid, orgfsgid;
+   	mm_segment_t			orgfs;
+	CHAR					*buffer;
+	CHAR					*tmpbuf;
+	ULONG					RtsThresh;
+	ULONG					FragThresh;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR	                keyMaterial[40];
+#endif // CONFIG_STA_SUPPORT //
+
+
+	PUCHAR					macptr;
+	INT						i = 0;
+
+	buffer = kmalloc(MAX_INI_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if(buffer == NULL)
+        return NDIS_STATUS_FAILURE;
+
+	tmpbuf = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if(tmpbuf == NULL)
+	{
+		kfree(buffer);
+        return NDIS_STATUS_FAILURE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		src = STA_PROFILE_PATH;
+#endif // CONFIG_STA_SUPPORT //
+#ifdef MULTIPLE_CARD_SUPPORT
+	src = pAd->MC_FileName;
+#endif // MULTIPLE_CARD_SUPPORT //
+
+	// Save uid and gid used for filesystem access.
+	// Set user and group to 0 (root)
+	orgfsuid = current_fsuid();
+	orgfsgid = current_fsgid();
+	/* Hm, can't really do this nicely anymore, so rely on these files
+	 * being set to the proper permission to read them... */
+	/* current->cred->fsuid = current->cred->fsgid = 0; */
+    orgfs = get_fs();
+    set_fs(KERNEL_DS);
+
+	if (src && *src)
+	{
+		srcf = filp_open(src, O_RDONLY, 0);
+		if (IS_ERR(srcf))
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("--> Error %ld opening %s\n", -PTR_ERR(srcf),src));
+		}
+		else
+		{
+			// The object must have a read method
+			if (srcf->f_op && srcf->f_op->read)
+			{
+				memset(buffer, 0x00, MAX_INI_BUFFER_SIZE);
+				retval=srcf->f_op->read(srcf, buffer, MAX_INI_BUFFER_SIZE, &srcf->f_pos);
+				if (retval < 0)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("--> Read %s error %d\n", src, -retval));
+				}
+				else
+				{
+					// set file parameter to portcfg
+					//CountryRegion
+					if(RTMPGetKeyParameter("CountryRegion", tmpbuf, 25, buffer))
+					{
+						pAd->CommonCfg.CountryRegion = (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("CountryRegion=%d\n", pAd->CommonCfg.CountryRegion));
+					}
+					//CountryRegionABand
+					if(RTMPGetKeyParameter("CountryRegionABand", tmpbuf, 25, buffer))
+					{
+						pAd->CommonCfg.CountryRegionForABand= (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("CountryRegionABand=%d\n", pAd->CommonCfg.CountryRegionForABand));
+					}
+					//CountryCode
+					if(RTMPGetKeyParameter("CountryCode", tmpbuf, 25, buffer))
+					{
+						NdisMoveMemory(pAd->CommonCfg.CountryCode, tmpbuf , 2);
+#ifdef CONFIG_STA_SUPPORT
+#ifdef EXT_BUILD_CHANNEL_LIST
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+							NdisMoveMemory(pAd->StaCfg.StaOriCountryCode, tmpbuf , 2);
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+						if (strlen(pAd->CommonCfg.CountryCode) != 0)
+						{
+							pAd->CommonCfg.bCountryFlag = TRUE;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("CountryCode=%s\n", pAd->CommonCfg.CountryCode));
+					}
+					//ChannelGeography
+					if(RTMPGetKeyParameter("ChannelGeography", tmpbuf, 25, buffer))
+					{
+						UCHAR Geography = (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						if (Geography <= BOTH)
+						{
+							pAd->CommonCfg.Geography = Geography;
+							pAd->CommonCfg.CountryCode[2] =
+								(pAd->CommonCfg.Geography == BOTH) ? ' ' : ((pAd->CommonCfg.Geography == IDOR) ? 'I' : 'O');
+#ifdef CONFIG_STA_SUPPORT
+#ifdef EXT_BUILD_CHANNEL_LIST
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+							pAd->StaCfg.StaOriGeography = pAd->CommonCfg.Geography;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+							DBGPRINT(RT_DEBUG_TRACE, ("ChannelGeography=%d\n", pAd->CommonCfg.Geography));
+						}
+					}
+					else
+					{
+						pAd->CommonCfg.Geography = BOTH;
+						pAd->CommonCfg.CountryCode[2] = ' ';
+					}
+
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						//SSID
+						if (RTMPGetCriticalParameter("SSID", tmpbuf, 256, buffer))
+						{
+							if (strlen(tmpbuf) <= 32)
+							{
+			 					pAd->CommonCfg.SsidLen = (UCHAR) strlen(tmpbuf);
+								NdisZeroMemory(pAd->CommonCfg.Ssid, NDIS_802_11_LENGTH_SSID);
+								NdisMoveMemory(pAd->CommonCfg.Ssid, tmpbuf, pAd->CommonCfg.SsidLen);
+								pAd->MlmeAux.AutoReconnectSsidLen = pAd->CommonCfg.SsidLen;
+								NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, NDIS_802_11_LENGTH_SSID);
+								NdisMoveMemory(pAd->MlmeAux.AutoReconnectSsid, tmpbuf, pAd->MlmeAux.AutoReconnectSsidLen);
+								pAd->MlmeAux.SsidLen = pAd->CommonCfg.SsidLen;
+								NdisZeroMemory(pAd->MlmeAux.Ssid, NDIS_802_11_LENGTH_SSID);
+								NdisMoveMemory(pAd->MlmeAux.Ssid, tmpbuf, pAd->MlmeAux.SsidLen);
+								DBGPRINT(RT_DEBUG_TRACE, ("%s::(SSID=%s)\n", __func__, tmpbuf));
+							}
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						//NetworkType
+						if (RTMPGetKeyParameter("NetworkType", tmpbuf, 25, buffer))
+						{
+							pAd->bConfigChanged = TRUE;
+							if (strcmp(tmpbuf, "Adhoc") == 0)
+								pAd->StaCfg.BssType = BSS_ADHOC;
+							else //Default Infrastructure mode
+								pAd->StaCfg.BssType = BSS_INFRA;
+							// Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+							pAd->StaCfg.WpaState = SS_NOTUSE;
+							DBGPRINT(RT_DEBUG_TRACE, ("%s::(NetworkType=%d)\n", __func__, pAd->StaCfg.BssType));
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+					//Channel
+					if(RTMPGetKeyParameter("Channel", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.Channel = (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("Channel=%d\n", pAd->CommonCfg.Channel));
+					}
+					//WirelessMode
+					if(RTMPGetKeyParameter("WirelessMode", tmpbuf, 10, buffer))
+					{
+						int value  = 0, maxPhyMode = PHY_11G;
+
+#ifdef DOT11_N_SUPPORT
+						maxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+						value = simple_strtol(tmpbuf, 0, 10);
+
+						if (value <= maxPhyMode)
+						{
+							pAd->CommonCfg.PhyMode = value;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("PhyMode=%d\n", pAd->CommonCfg.PhyMode));
+					}
+                    //BasicRate
+					if(RTMPGetKeyParameter("BasicRate", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.BasicRateBitmap = (ULONG) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("BasicRate=%ld\n", pAd->CommonCfg.BasicRateBitmap));
+					}
+					//BeaconPeriod
+					if(RTMPGetKeyParameter("BeaconPeriod", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.BeaconPeriod = (USHORT) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("BeaconPeriod=%d\n", pAd->CommonCfg.BeaconPeriod));
+					}
+                    //TxPower
+					if(RTMPGetKeyParameter("TxPower", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.TxPowerPercentage = (ULONG) simple_strtol(tmpbuf, 0, 10);
+#ifdef CONFIG_STA_SUPPORT
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+							pAd->CommonCfg.TxPowerDefault = pAd->CommonCfg.TxPowerPercentage;
+#endif // CONFIG_STA_SUPPORT //
+						DBGPRINT(RT_DEBUG_TRACE, ("TxPower=%ld\n", pAd->CommonCfg.TxPowerPercentage));
+					}
+					//BGProtection
+					if(RTMPGetKeyParameter("BGProtection", tmpbuf, 10, buffer))
+					{
+						switch (simple_strtol(tmpbuf, 0, 10))
+						{
+							case 1: //Always On
+								pAd->CommonCfg.UseBGProtection = 1;
+								break;
+							case 2: //Always OFF
+								pAd->CommonCfg.UseBGProtection = 2;
+								break;
+							case 0: //AUTO
+							default:
+								pAd->CommonCfg.UseBGProtection = 0;
+								break;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("BGProtection=%ld\n", pAd->CommonCfg.UseBGProtection));
+					}
+					//OLBCDetection
+					if(RTMPGetKeyParameter("DisableOLBC", tmpbuf, 10, buffer))
+					{
+						switch (simple_strtol(tmpbuf, 0, 10))
+						{
+							case 1: //disable OLBC Detection
+								pAd->CommonCfg.DisableOLBCDetect = 1;
+								break;
+							case 0: //enable OLBC Detection
+								pAd->CommonCfg.DisableOLBCDetect = 0;
+								break;
+							default:
+								pAd->CommonCfg.DisableOLBCDetect= 0;
+								break;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("OLBCDetection=%ld\n", pAd->CommonCfg.DisableOLBCDetect));
+					}
+					//TxPreamble
+					if(RTMPGetKeyParameter("TxPreamble", tmpbuf, 10, buffer))
+					{
+						switch (simple_strtol(tmpbuf, 0, 10))
+						{
+							case Rt802_11PreambleShort:
+								pAd->CommonCfg.TxPreamble = Rt802_11PreambleShort;
+								break;
+							case Rt802_11PreambleLong:
+							default:
+								pAd->CommonCfg.TxPreamble = Rt802_11PreambleLong;
+								break;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("TxPreamble=%ld\n", pAd->CommonCfg.TxPreamble));
+					}
+					//RTSThreshold
+					if(RTMPGetKeyParameter("RTSThreshold", tmpbuf, 10, buffer))
+					{
+						RtsThresh = simple_strtol(tmpbuf, 0, 10);
+						if( (RtsThresh >= 1) && (RtsThresh <= MAX_RTS_THRESHOLD) )
+							pAd->CommonCfg.RtsThreshold  = (USHORT)RtsThresh;
+						else
+							pAd->CommonCfg.RtsThreshold = MAX_RTS_THRESHOLD;
+
+						DBGPRINT(RT_DEBUG_TRACE, ("RTSThreshold=%d\n", pAd->CommonCfg.RtsThreshold));
+					}
+					//FragThreshold
+					if(RTMPGetKeyParameter("FragThreshold", tmpbuf, 10, buffer))
+					{
+						FragThresh = simple_strtol(tmpbuf, 0, 10);
+						pAd->CommonCfg.bUseZeroToDisableFragment = FALSE;
+
+						if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+						{ //illegal FragThresh so we set it to default
+							pAd->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+							pAd->CommonCfg.bUseZeroToDisableFragment = TRUE;
+						}
+						else if (FragThresh % 2 == 1)
+						{
+							// The length of each fragment shall always be an even number of octets, except for the last fragment
+							// of an MSDU or MMPDU, which may be either an even or an odd number of octets.
+							pAd->CommonCfg.FragmentThreshold = (USHORT)(FragThresh - 1);
+						}
+						else
+						{
+							pAd->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+						}
+						//pAd->CommonCfg.AllowFragSize = (pAd->CommonCfg.FragmentThreshold) - LENGTH_802_11 - LENGTH_CRC;
+						DBGPRINT(RT_DEBUG_TRACE, ("FragThreshold=%d\n", pAd->CommonCfg.FragmentThreshold));
+					}
+					//TxBurst
+					if(RTMPGetKeyParameter("TxBurst", tmpbuf, 10, buffer))
+					{
+//#ifdef WIFI_TEST
+//						pAd->CommonCfg.bEnableTxBurst = FALSE;
+//#else
+						if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+							pAd->CommonCfg.bEnableTxBurst = TRUE;
+						else //Disable
+							pAd->CommonCfg.bEnableTxBurst = FALSE;
+//#endif
+						DBGPRINT(RT_DEBUG_TRACE, ("TxBurst=%d\n", pAd->CommonCfg.bEnableTxBurst));
+					}
+
+#ifdef AGGREGATION_SUPPORT
+					//PktAggregate
+					if(RTMPGetKeyParameter("PktAggregate", tmpbuf, 10, buffer))
+					{
+						if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+							pAd->CommonCfg.bAggregationCapable = TRUE;
+						else //Disable
+							pAd->CommonCfg.bAggregationCapable = FALSE;
+#ifdef PIGGYBACK_SUPPORT
+						pAd->CommonCfg.bPiggyBackCapable = pAd->CommonCfg.bAggregationCapable;
+#endif // PIGGYBACK_SUPPORT //
+						DBGPRINT(RT_DEBUG_TRACE, ("PktAggregate=%d\n", pAd->CommonCfg.bAggregationCapable));
+					}
+#else
+					pAd->CommonCfg.bAggregationCapable = FALSE;
+					pAd->CommonCfg.bPiggyBackCapable = FALSE;
+#endif // AGGREGATION_SUPPORT //
+
+					// WmmCapable
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						rtmp_read_sta_wmm_parms_from_file(pAd, tmpbuf, buffer);
+#endif // CONFIG_STA_SUPPORT //
+
+					//ShortSlot
+					if(RTMPGetKeyParameter("ShortSlot", tmpbuf, 10, buffer))
+					{
+						if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+							pAd->CommonCfg.bUseShortSlotTime = TRUE;
+						else //Disable
+							pAd->CommonCfg.bUseShortSlotTime = FALSE;
+
+						DBGPRINT(RT_DEBUG_TRACE, ("ShortSlot=%d\n", pAd->CommonCfg.bUseShortSlotTime));
+					}
+					//IEEE80211H
+					if(RTMPGetKeyParameter("IEEE80211H", tmpbuf, 10, buffer))
+					{
+					    for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+					    {
+    						if(simple_strtol(macptr, 0, 10) != 0)  //Enable
+    							pAd->CommonCfg.bIEEE80211H = TRUE;
+    						else //Disable
+    							pAd->CommonCfg.bIEEE80211H = FALSE;
+
+    						DBGPRINT(RT_DEBUG_TRACE, ("IEEE80211H=%d\n", pAd->CommonCfg.bIEEE80211H));
+					    }
+					}
+					//CSPeriod
+					if(RTMPGetKeyParameter("CSPeriod", tmpbuf, 10, buffer))
+					{
+					    if(simple_strtol(tmpbuf, 0, 10) != 0)
+							pAd->CommonCfg.RadarDetect.CSPeriod = simple_strtol(tmpbuf, 0, 10);
+						else
+							pAd->CommonCfg.RadarDetect.CSPeriod = 0;
+
+   						DBGPRINT(RT_DEBUG_TRACE, ("CSPeriod=%d\n", pAd->CommonCfg.RadarDetect.CSPeriod));
+					}
+
+					//RDRegion
+					if(RTMPGetKeyParameter("RDRegion", tmpbuf, 128, buffer))
+					{
+						if ((strncmp(tmpbuf, "JAP_W53", 7) == 0) || (strncmp(tmpbuf, "jap_w53", 7) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = JAP_W53;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 15;
+						}
+						else if ((strncmp(tmpbuf, "JAP_W56", 7) == 0) || (strncmp(tmpbuf, "jap_w56", 7) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = JAP_W56;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+						}
+						else if ((strncmp(tmpbuf, "JAP", 3) == 0) || (strncmp(tmpbuf, "jap", 3) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = JAP;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 5;
+						}
+						else  if ((strncmp(tmpbuf, "FCC", 3) == 0) || (strncmp(tmpbuf, "fcc", 3) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = FCC;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 5;
+						}
+						else if ((strncmp(tmpbuf, "CE", 2) == 0) || (strncmp(tmpbuf, "ce", 2) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = CE;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+						}
+						else
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = CE;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+						}
+
+						DBGPRINT(RT_DEBUG_TRACE, ("RDRegion=%d\n", pAd->CommonCfg.RadarDetect.RDDurRegion));
+					}
+					else
+					{
+						pAd->CommonCfg.RadarDetect.RDDurRegion = CE;
+						pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+					}
+
+					//WirelessEvent
+					if(RTMPGetKeyParameter("WirelessEvent", tmpbuf, 10, buffer))
+					{
+#if WIRELESS_EXT >= 15
+					    if(simple_strtol(tmpbuf, 0, 10) != 0)
+							pAd->CommonCfg.bWirelessEvent = simple_strtol(tmpbuf, 0, 10);
+						else
+							pAd->CommonCfg.bWirelessEvent = 0;	// disable
+#else
+						pAd->CommonCfg.bWirelessEvent = 0;	// disable
+#endif
+   						DBGPRINT(RT_DEBUG_TRACE, ("WirelessEvent=%d\n", pAd->CommonCfg.bWirelessEvent));
+					}
+					if(RTMPGetKeyParameter("WiFiTest", tmpbuf, 10, buffer))
+					{
+					    if(simple_strtol(tmpbuf, 0, 10) != 0)
+							pAd->CommonCfg.bWiFiTest= simple_strtol(tmpbuf, 0, 10);
+						else
+							pAd->CommonCfg.bWiFiTest = 0;	// disable
+
+   						DBGPRINT(RT_DEBUG_TRACE, ("WiFiTest=%d\n", pAd->CommonCfg.bWiFiTest));
+					}
+					//AuthMode
+					if(RTMPGetKeyParameter("AuthMode", tmpbuf, 128, buffer))
+					{
+#ifdef CONFIG_STA_SUPPORT
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						{
+							if ((strcmp(tmpbuf, "WEPAUTO") == 0) || (strcmp(tmpbuf, "wepauto") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeAutoSwitch;
+							else if ((strcmp(tmpbuf, "SHARED") == 0) || (strcmp(tmpbuf, "shared") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+							else if ((strcmp(tmpbuf, "WPAPSK") == 0) || (strcmp(tmpbuf, "wpapsk") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+							else if ((strcmp(tmpbuf, "WPANONE") == 0) || (strcmp(tmpbuf, "wpanone") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+							else if ((strcmp(tmpbuf, "WPA2PSK") == 0) || (strcmp(tmpbuf, "wpa2psk") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+#ifdef WPA_SUPPLICANT_SUPPORT
+							else if ((strcmp(tmpbuf, "WPA") == 0) || (strcmp(tmpbuf, "wpa") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+							else if ((strcmp(tmpbuf, "WPA2") == 0) || (strcmp(tmpbuf, "wpa2") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#endif // WPA_SUPPLICANT_SUPPORT //
+				                        else
+				                            pAd->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+
+				                        pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("%s::(EncrypType=%d)\n", __func__, pAd->StaCfg.WepStatus));
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+					//EncrypType
+					if(RTMPGetKeyParameter("EncrypType", tmpbuf, 128, buffer))
+					{
+
+#ifdef CONFIG_STA_SUPPORT
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						{
+							if ((strcmp(tmpbuf, "WEP") == 0) || (strcmp(tmpbuf, "wep") == 0))
+								pAd->StaCfg.WepStatus	= Ndis802_11WEPEnabled;
+							else if ((strcmp(tmpbuf, "TKIP") == 0) || (strcmp(tmpbuf, "tkip") == 0))
+								pAd->StaCfg.WepStatus	= Ndis802_11Encryption2Enabled;
+							else if ((strcmp(tmpbuf, "AES") == 0) || (strcmp(tmpbuf, "aes") == 0))
+								pAd->StaCfg.WepStatus	= Ndis802_11Encryption3Enabled;
+							else
+								pAd->StaCfg.WepStatus	= Ndis802_11WEPDisabled;
+
+							// Update all wepstatus related
+							pAd->StaCfg.PairCipher		= pAd->StaCfg.WepStatus;
+							pAd->StaCfg.GroupCipher 	= pAd->StaCfg.WepStatus;
+							pAd->StaCfg.OrigWepStatus 	= pAd->StaCfg.WepStatus;
+							pAd->StaCfg.bMixCipher 		= FALSE;
+
+							//RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, 0);
+							DBGPRINT(RT_DEBUG_TRACE, ("%s::(EncrypType=%d)\n", __func__, pAd->StaCfg.WepStatus));
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						if(RTMPGetCriticalParameter("WPAPSK", tmpbuf, 512, buffer))
+						{
+							int     err=0;
+
+							tmpbuf[strlen(tmpbuf)] = '\0'; // make STA can process .$^& for WPAPSK input
+
+							if ((pAd->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+								(pAd->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+								(pAd->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+								)
+							{
+								err = 1;
+							}
+							else if ((strlen(tmpbuf) >= 8) && (strlen(tmpbuf) < 64))
+							{
+								PasswordHash((char *)tmpbuf, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen, keyMaterial);
+								NdisMoveMemory(pAd->StaCfg.PMK, keyMaterial, 32);
+
+							}
+							else if (strlen(tmpbuf) == 64)
+							{
+								AtoH(tmpbuf, keyMaterial, 32);
+								NdisMoveMemory(pAd->StaCfg.PMK, keyMaterial, 32);
+							}
+							else
+							{
+								err = 1;
+								DBGPRINT(RT_DEBUG_ERROR, ("%s::(WPAPSK key-string required 8 ~ 64 characters!)\n", __func__));
+							}
+
+							if (err == 0)
+	                        			{
+	                        				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+									(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+								{
+									// Start STA supplicant state machine
+									pAd->StaCfg.WpaState = SS_START;
+								}
+								else if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+								{
+	/*
+									NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+									pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+									NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, pAd->StaCfg.PMK, LEN_TKIP_EK);
+									NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PMK[16], LEN_TKIP_RXMICK);
+									NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PMK[16], LEN_TKIP_TXMICK);
+
+									// Decide its ChiperAlg
+									if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+										pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+									else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+										pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+									else
+										pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+	*/
+									pAd->StaCfg.WpaState = SS_NOTUSE;
+								}
+
+								DBGPRINT(RT_DEBUG_TRACE, ("%s::(WPAPSK=%s)\n", __func__, tmpbuf));
+							}
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+
+					//DefaultKeyID, KeyType, KeyStr
+					rtmp_read_key_parms_from_file(pAd, tmpbuf, buffer);
+
+
+					//HSCounter
+					/*if(RTMPGetKeyParameter("HSCounter", tmpbuf, 10, buffer))
+					{
+						switch (simple_strtol(tmpbuf, 0, 10))
+						{
+							case 1: //Enable
+								pAd->CommonCfg.bEnableHSCounter = TRUE;
+								break;
+							case 0: //Disable
+							default:
+								pAd->CommonCfg.bEnableHSCounter = FALSE;
+								break;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, "HSCounter=%d\n", pAd->CommonCfg.bEnableHSCounter);
+					}*/
+
+#ifdef DOT11_N_SUPPORT
+					HTParametersHook(pAd, tmpbuf, buffer);
+#endif // DOT11_N_SUPPORT //
+
+
+#ifdef CARRIER_DETECTION_SUPPORT
+						//CarrierDetect
+						if(RTMPGetKeyParameter("CarrierDetect", tmpbuf, 128, buffer))
+						{
+							if ((strncmp(tmpbuf, "0", 1) == 0))
+								pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+							else if ((strncmp(tmpbuf, "1", 1) == 0))
+								pAd->CommonCfg.CarrierDetect.Enable = TRUE;
+							else
+								pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("CarrierDetect.Enable=%d\n", pAd->CommonCfg.CarrierDetect.Enable));
+						}
+						else
+							pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						//PSMode
+						if (RTMPGetKeyParameter("PSMode", tmpbuf, 10, buffer))
+						{
+							if (pAd->StaCfg.BssType == BSS_INFRA)
+							{
+								if ((strcmp(tmpbuf, "MAX_PSP") == 0) || (strcmp(tmpbuf, "max_psp") == 0))
+								{
+									// do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+									// to exclude certain situations.
+									//	   MlmeSetPsm(pAd, PWR_SAVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeMAX_PSP;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeMAX_PSP;
+									pAd->StaCfg.DefaultListenCount = 5;
+								}
+								else if ((strcmp(tmpbuf, "Fast_PSP") == 0) || (strcmp(tmpbuf, "fast_psp") == 0)
+									|| (strcmp(tmpbuf, "FAST_PSP") == 0))
+								{
+									// do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+									// to exclude certain situations.
+									//	   MlmeSetPsmBit(pAd, PWR_SAVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeFast_PSP;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeFast_PSP;
+									pAd->StaCfg.DefaultListenCount = 3;
+								}
+								else if ((strcmp(tmpbuf, "Legacy_PSP") == 0) || (strcmp(tmpbuf, "legacy_psp") == 0)
+									|| (strcmp(tmpbuf, "LEGACY_PSP") == 0))
+								{
+									// do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+									// to exclude certain situations.
+									//	   MlmeSetPsmBit(pAd, PWR_SAVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeLegacy_PSP;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeLegacy_PSP;
+									pAd->StaCfg.DefaultListenCount = 3;
+								}
+								else
+								{ //Default Ndis802_11PowerModeCAM
+									// clear PSM bit immediately
+									MlmeSetPsmBit(pAd, PWR_ACTIVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+								}
+								DBGPRINT(RT_DEBUG_TRACE, ("PSMode=%ld\n", pAd->StaCfg.WindowsPowerMode));
+							}
+						}
+						// FastRoaming
+						if (RTMPGetKeyParameter("FastRoaming", tmpbuf, 32, buffer))
+						{
+							if (simple_strtol(tmpbuf, 0, 10) == 0)
+								pAd->StaCfg.bFastRoaming = FALSE;
+							else
+								pAd->StaCfg.bFastRoaming = TRUE;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("FastRoaming=%d\n", pAd->StaCfg.bFastRoaming));
+						}
+						// RoamThreshold
+						if (RTMPGetKeyParameter("RoamThreshold", tmpbuf, 32, buffer))
+						{
+							long lInfo = simple_strtol(tmpbuf, 0, 10);
+
+							if (lInfo > 90 || lInfo < 60)
+								pAd->StaCfg.dBmToRoam = -70;
+							else
+								pAd->StaCfg.dBmToRoam = (CHAR)(-1)*lInfo;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("RoamThreshold=%d  dBm\n", pAd->StaCfg.dBmToRoam));
+						}
+
+						if(RTMPGetKeyParameter("TGnWifiTest", tmpbuf, 10, buffer))
+						{
+							if(simple_strtol(tmpbuf, 0, 10) == 0)
+								pAd->StaCfg.bTGnWifiTest = FALSE;
+							else
+								pAd->StaCfg.bTGnWifiTest = TRUE;
+								DBGPRINT(RT_DEBUG_TRACE, ("TGnWifiTest=%d\n", pAd->StaCfg.bTGnWifiTest));
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+				}
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("--> %s does not have a write method\n", src));
+			}
+
+			retval=filp_close(srcf,NULL);
+
+			if (retval)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("--> Error %d closing %s\n", -retval, src));
+			}
+		}
+	}
+
+	set_fs(orgfs);
+#if 0
+	current->fsuid = orgfsuid;
+	current->fsgid = orgfsgid;
+#endif
+
+	kfree(buffer);
+	kfree(tmpbuf);
+
+	return (NDIS_STATUS_SUCCESS);
+}
+
+#ifdef DOT11_N_SUPPORT
+static void	HTParametersHook(
+	IN	PRTMP_ADAPTER pAd,
+	IN	CHAR		  *pValueStr,
+	IN	CHAR		  *pInput)
+{
+
+	INT Value;
+
+    if (RTMPGetKeyParameter("HT_PROTECT", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bHTProtect = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bHTProtect = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: Protection  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+    if (RTMPGetKeyParameter("HT_MIMOPSEnable", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bMIMOPSEnable = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bMIMOPSEnable = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: MIMOPSEnable  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+
+    if (RTMPGetKeyParameter("HT_MIMOPSMode", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value > MMPS_ENABLE)
+        {
+			pAd->CommonCfg.BACapability.field.MMPSmode = MMPS_ENABLE;
+        }
+        else
+        {
+            //TODO: add mimo power saving mechanism
+            pAd->CommonCfg.BACapability.field.MMPSmode = MMPS_ENABLE;
+			//pAd->CommonCfg.BACapability.field.MMPSmode = Value;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: MIMOPS Mode  = %d\n", Value));
+    }
+
+    if (RTMPGetKeyParameter("HT_BADecline", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bBADecline = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bBADecline = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: BA Decline  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+
+    if (RTMPGetKeyParameter("HT_DisableReordering", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bDisableReordering = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bDisableReordering = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: DisableReordering  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+    if (RTMPGetKeyParameter("HT_AutoBA", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.BACapability.field.AutoBA = TRUE;
+        }
+        pAd->CommonCfg.REGBACapability.field.AutoBA = pAd->CommonCfg.BACapability.field.AutoBA;
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: Auto BA  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+	// Tx_+HTC frame
+    if (RTMPGetKeyParameter("HT_HTC", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->HTCEnable = FALSE;
+		}
+		else
+		{
+            		pAd->HTCEnable = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Tx +HTC frame = %s\n", (Value==0) ? "Disable" : "Enable"));
+	}
+
+	// Enable HT Link Adaptation Control
+	if (RTMPGetKeyParameter("HT_LinkAdapt", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->bLinkAdapt = FALSE;
+		}
+		else
+		{
+			pAd->HTCEnable = TRUE;
+			pAd->bLinkAdapt = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Link Adaptation Control = %s\n", (Value==0) ? "Disable" : "Enable(+HTC)"));
+	}
+
+	// Reverse Direction Mechanism
+    if (RTMPGetKeyParameter("HT_RDG", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->CommonCfg.bRdg = FALSE;
+		}
+		else
+		{
+			pAd->HTCEnable = TRUE;
+            pAd->CommonCfg.bRdg = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: RDG = %s\n", (Value==0) ? "Disable" : "Enable(+HTC)"));
+	}
+
+
+
+
+	// Tx A-MSUD ?
+    if (RTMPGetKeyParameter("HT_AMSDU", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->CommonCfg.BACapability.field.AmsduEnable = FALSE;
+		}
+		else
+		{
+            pAd->CommonCfg.BACapability.field.AmsduEnable = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Tx A-MSDU = %s\n", (Value==0) ? "Disable" : "Enable"));
+	}
+
+	// MPDU Density
+    if (RTMPGetKeyParameter("HT_MpduDensity", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value <=7 && Value >= 0)
+		{
+			pAd->CommonCfg.BACapability.field.MpduDensity = Value;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: MPDU Density = %d\n", Value));
+		}
+		else
+		{
+			pAd->CommonCfg.BACapability.field.MpduDensity = 4;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: MPDU Density = %d (Default)\n", 4));
+		}
+	}
+
+	// Max Rx BA Window Size
+    if (RTMPGetKeyParameter("HT_BAWinSize", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value >=1 && Value <= 64)
+		{
+			pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = Value;
+			pAd->CommonCfg.BACapability.field.RxBAWinLimit = Value;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: BA Windw Size = %d\n", Value));
+		}
+		else
+		{
+            pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = 64;
+			pAd->CommonCfg.BACapability.field.RxBAWinLimit = 64;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: BA Windw Size = 64 (Defualt)\n"));
+		}
+
+	}
+
+	// Guard Interval
+	if (RTMPGetKeyParameter("HT_GI", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == GI_400)
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_400;
+		}
+		else
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_800;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Guard Interval = %s\n", (Value==GI_400) ? "400" : "800" ));
+	}
+
+	// HT Operation Mode : Mixed Mode , Green Field
+	if (RTMPGetKeyParameter("HT_OpMode", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == HTMODE_GF)
+		{
+
+			pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_GF;
+		}
+		else
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_MM;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Operate Mode = %s\n", (Value==HTMODE_GF) ? "Green Field" : "Mixed Mode" ));
+	}
+
+	// Fixed Tx mode : CCK, OFDM
+	if (RTMPGetKeyParameter("FixedTxMode", pValueStr, 25, pInput))
+	{
+		UCHAR	fix_tx_mode;
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			fix_tx_mode = FIXED_TXMODE_HT;
+
+			if (strcmp(pValueStr, "OFDM") == 0 || strcmp(pValueStr, "ofdm") == 0)
+			{
+				fix_tx_mode = FIXED_TXMODE_OFDM;
+			}
+			else if (strcmp(pValueStr, "CCK") == 0 || strcmp(pValueStr, "cck") == 0)
+			{
+		        fix_tx_mode = FIXED_TXMODE_CCK;
+			}
+			else if (strcmp(pValueStr, "HT") == 0 || strcmp(pValueStr, "ht") == 0)
+			{
+		        fix_tx_mode = FIXED_TXMODE_HT;
+		}
+		else
+		{
+				Value = simple_strtol(pValueStr, 0, 10);
+				// 1 : CCK
+				// 2 : OFDM
+				// otherwise : HT
+				if (Value == FIXED_TXMODE_CCK || Value == FIXED_TXMODE_OFDM)
+					fix_tx_mode = Value;
+				else
+					fix_tx_mode = FIXED_TXMODE_HT;
+		}
+
+			pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode = fix_tx_mode;
+			DBGPRINT(RT_DEBUG_TRACE, ("Fixed Tx Mode = %d\n", fix_tx_mode));
+
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+	// Channel Width
+	if (RTMPGetKeyParameter("HT_BW", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == BW_40)
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_40;
+		}
+		else
+		{
+            pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+		}
+
+#ifdef MCAST_RATE_SPECIFIC
+		pAd->CommonCfg.MCastPhyMode.field.BW = pAd->CommonCfg.RegTransmitSetting.field.BW;
+#endif // MCAST_RATE_SPECIFIC //
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Channel Width = %s\n", (Value==BW_40) ? "40 MHz" : "20 MHz" ));
+	}
+
+	if (RTMPGetKeyParameter("HT_EXTCHA", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == 0)
+		{
+
+			pAd->CommonCfg.RegTransmitSetting.field.EXTCHA  = EXTCHA_BELOW;
+		}
+		else
+		{
+            pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Ext Channel = %s\n", (Value==0) ? "BELOW" : "ABOVE" ));
+	}
+
+	// MSC
+	if (RTMPGetKeyParameter("HT_MCS", pValueStr, 50, pInput))
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			Value = simple_strtol(pValueStr, 0, 10);
+
+//			if ((Value >= 0 && Value <= 15) || (Value == 32))
+			if ((Value >= 0 && Value <= 23) || (Value == 32)) // 3*3
+		{
+				pAd->StaCfg.DesiredTransmitSetting.field.MCS  = Value;
+				pAd->StaCfg.bAutoTxRateSwitch = FALSE;
+				DBGPRINT(RT_DEBUG_TRACE, ("HT: MCS = %d\n", pAd->StaCfg.DesiredTransmitSetting.field.MCS));
+		}
+		else
+		{
+				pAd->StaCfg.DesiredTransmitSetting.field.MCS  = MCS_AUTO;
+				pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+				DBGPRINT(RT_DEBUG_TRACE, ("HT: MCS = AUTO\n"));
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// STBC
+    if (RTMPGetKeyParameter("HT_STBC", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == STBC_USE)
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_USE;
+		}
+		else
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_NONE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: STBC = %d\n", pAd->CommonCfg.RegTransmitSetting.field.STBC));
+	}
+
+	// 40_Mhz_Intolerant
+	if (RTMPGetKeyParameter("HT_40MHZ_INTOLERANT", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->CommonCfg.bForty_Mhz_Intolerant = FALSE;
+		}
+		else
+		{
+			pAd->CommonCfg.bForty_Mhz_Intolerant = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: 40MHZ INTOLERANT = %d\n", pAd->CommonCfg.bForty_Mhz_Intolerant));
+	}
+	//HT_TxStream
+	if(RTMPGetKeyParameter("HT_TxStream", pValueStr, 10, pInput))
+	{
+		switch (simple_strtol(pValueStr, 0, 10))
+		{
+			case 1:
+				pAd->CommonCfg.TxStream = 1;
+				break;
+			case 2:
+				pAd->CommonCfg.TxStream = 2;
+				break;
+			case 3: // 3*3
+			default:
+				pAd->CommonCfg.TxStream = 3;
+
+				if (pAd->MACVersion < RALINK_2883_VERSION)
+					pAd->CommonCfg.TxStream = 2; // only 2 tx streams for RT2860 series
+				break;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Tx Stream = %d\n", pAd->CommonCfg.TxStream));
+	}
+	//HT_RxStream
+	if(RTMPGetKeyParameter("HT_RxStream", pValueStr, 10, pInput))
+	{
+		switch (simple_strtol(pValueStr, 0, 10))
+		{
+			case 1:
+				pAd->CommonCfg.RxStream = 1;
+				break;
+			case 2:
+				pAd->CommonCfg.RxStream = 2;
+				break;
+			case 3:
+			default:
+				pAd->CommonCfg.RxStream = 3;
+
+				if (pAd->MACVersion < RALINK_2883_VERSION)
+					pAd->CommonCfg.RxStream = 2; // only 2 rx streams for RT2860 series
+				break;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Rx Stream = %d\n", pAd->CommonCfg.RxStream));
+	}
+
+}
+#endif // DOT11_N_SUPPORT //
+
diff --git a/drivers/staging/rt2870/rtmp.h b/drivers/staging/rt2870/rtmp.h
new file mode 100644
index 000000000000..c2a4784ec338
--- /dev/null
+++ b/drivers/staging/rt2870/rtmp.h
@@ -0,0 +1,7586 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rtmp.h
+
+    Abstract:
+    Miniport generic portion header file
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Paul Lin    2002-08-01    created
+    James Tan   2002-09-06    modified (Revise NTCRegTable)
+    John Chang  2004-09-06    modified for RT2600
+*/
+#ifndef __RTMP_H__
+#define __RTMP_H__
+
+#include "link_list.h"
+#include "spectrum_def.h"
+
+
+#ifdef CONFIG_STA_SUPPORT
+#include "aironet.h"
+#endif // CONFIG_STA_SUPPORT //
+
+//#define DBG		1
+
+//#define DBG_DIAGNOSE		1
+
+#if defined(CONFIG_AP_SUPPORT) && defined(CONFIG_STA_SUPPORT)
+#define IF_DEV_CONFIG_OPMODE_ON_AP(_pAd)	if(_pAd->OpMode == OPMODE_AP)
+#define IF_DEV_CONFIG_OPMODE_ON_STA(_pAd)	if(_pAd->OpMode == OPMODE_STA)
+#else
+#define IF_DEV_CONFIG_OPMODE_ON_AP(_pAd)
+#define IF_DEV_CONFIG_OPMODE_ON_STA(_pAd)
+#endif
+
+#define VIRTUAL_IF_INC(__pAd) ((__pAd)->VirtualIfCnt++)
+#define VIRTUAL_IF_DEC(__pAd) ((__pAd)->VirtualIfCnt--)
+#define VIRTUAL_IF_NUM(__pAd) ((__pAd)->VirtualIfCnt)
+
+#ifdef RT2870
+////////////////////////////////////////////////////////////////////////////
+// The TX_BUFFER structure forms the transmitted USB packet to the device
+////////////////////////////////////////////////////////////////////////////
+typedef struct __TX_BUFFER{
+	union	{
+		UCHAR			WirelessPacket[TX_BUFFER_NORMSIZE];
+		HEADER_802_11	NullFrame;
+		PSPOLL_FRAME	PsPollPacket;
+		RTS_FRAME		RTSFrame;
+	}field;
+	UCHAR			Aggregation[4];  //Buffer for save Aggregation size.
+} TX_BUFFER, *PTX_BUFFER;
+
+typedef struct __HTTX_BUFFER{
+	union	{
+		UCHAR			WirelessPacket[MAX_TXBULK_SIZE];
+		HEADER_802_11	NullFrame;
+		PSPOLL_FRAME	PsPollPacket;
+		RTS_FRAME		RTSFrame;
+	}field;
+	UCHAR			Aggregation[4];  //Buffer for save Aggregation size.
+} HTTX_BUFFER, *PHTTX_BUFFER;
+
+
+// used to track driver-generated write irps
+typedef struct _TX_CONTEXT
+{
+	PVOID			pAd;		//Initialized in MiniportInitialize
+	PURB			pUrb;			//Initialized in MiniportInitialize
+	PIRP			pIrp;			//used to cancel pending bulk out.
+									//Initialized in MiniportInitialize
+	PTX_BUFFER		TransferBuffer;	//Initialized in MiniportInitialize
+	ULONG			BulkOutSize;
+	UCHAR			BulkOutPipeId;
+	UCHAR			SelfIdx;
+	BOOLEAN			InUse;
+	BOOLEAN			bWaitingBulkOut; // at least one packet is in this TxContext, ready for making IRP anytime.
+	BOOLEAN			bFullForBulkOut; // all tx buffer are full , so waiting for tx bulkout.
+	BOOLEAN			IRPPending;
+	BOOLEAN			LastOne;
+	BOOLEAN			bAggregatible;
+	UCHAR			Header_802_3[LENGTH_802_3];
+	UCHAR			Rsv[2];
+	ULONG			DataOffset;
+	UINT			TxRate;
+	dma_addr_t		data_dma;		// urb dma on linux
+
+}	TX_CONTEXT, *PTX_CONTEXT, **PPTX_CONTEXT;
+
+
+// used to track driver-generated write irps
+typedef struct _HT_TX_CONTEXT
+{
+	PVOID			pAd;		//Initialized in MiniportInitialize
+	PURB			pUrb;			//Initialized in MiniportInitialize
+	PIRP			pIrp;			//used to cancel pending bulk out.
+									//Initialized in MiniportInitialize
+	PHTTX_BUFFER	TransferBuffer;	//Initialized in MiniportInitialize
+	ULONG			BulkOutSize;	// Indicate the total bulk-out size in bytes in one bulk-transmission
+	UCHAR			BulkOutPipeId;
+	BOOLEAN			IRPPending;
+	BOOLEAN			LastOne;
+	BOOLEAN			bCurWriting;
+	BOOLEAN			bRingEmpty;
+	BOOLEAN			bCopySavePad;
+	UCHAR			SavedPad[8];
+	UCHAR			Header_802_3[LENGTH_802_3];
+	ULONG			CurWritePosition;		// Indicate the buffer offset which packet will be inserted start from.
+	ULONG			CurWriteRealPos;		// Indicate the buffer offset which packet now are writing to.
+	ULONG			NextBulkOutPosition;	// Indicate the buffer start offset of a bulk-transmission
+	ULONG			ENextBulkOutPosition;	// Indicate the buffer end offset of a bulk-transmission
+	UINT			TxRate;
+	dma_addr_t		data_dma;		// urb dma on linux
+}	HT_TX_CONTEXT, *PHT_TX_CONTEXT, **PPHT_TX_CONTEXT;
+
+
+//
+// Structure to keep track of receive packets and buffers to indicate
+// receive data to the protocol.
+//
+typedef struct _RX_CONTEXT
+{
+	PUCHAR				TransferBuffer;
+	PVOID				pAd;
+	PIRP				pIrp;//used to cancel pending bulk in.
+	PURB				pUrb;
+	//These 2 Boolean shouldn't both be 1 at the same time.
+	ULONG				BulkInOffset;	// number of packets waiting for reordering .
+//	BOOLEAN				ReorderInUse;	// At least one packet in this buffer are in reordering buffer and wait for receive indication
+	BOOLEAN				bRxHandling;	// Notify this packet is being process now.
+	BOOLEAN				InUse;			// USB Hardware Occupied. Wait for USB HW to put packet.
+	BOOLEAN				Readable;		// Receive Complete back. OK for driver to indicate receiving packet.
+	BOOLEAN				IRPPending;		// TODO: To be removed
+	atomic_t			IrpLock;
+	NDIS_SPIN_LOCK		RxContextLock;
+	dma_addr_t			data_dma;		// urb dma on linux
+}	RX_CONTEXT, *PRX_CONTEXT;
+#endif // RT2870 //
+
+
+//
+//  NDIS Version definitions
+//
+#ifdef  NDIS50_MINIPORT
+#define RTMP_NDIS_MAJOR_VERSION     5
+#define RTMP_NDIS_MINOR_VERSION     0
+#endif
+
+#ifdef  NDIS51_MINIPORT
+#define RTMP_NDIS_MAJOR_VERSION     5
+#define RTMP_NDIS_MINOR_VERSION     1
+#endif
+
+extern  char    NIC_VENDOR_DESC[];
+extern  int     NIC_VENDOR_DESC_LEN;
+
+extern  unsigned char   SNAP_AIRONET[];
+extern  unsigned char   CipherSuiteCiscoCCKM[];
+extern  unsigned char   CipherSuiteCiscoCCKMLen;
+extern	unsigned char	CipherSuiteCiscoCCKM24[];
+extern	unsigned char	CipherSuiteCiscoCCKM24Len;
+extern  unsigned char   CipherSuiteCCXTkip[];
+extern  unsigned char   CipherSuiteCCXTkipLen;
+extern  unsigned char   CISCO_OUI[];
+extern  UCHAR	BaSizeArray[4];
+
+extern UCHAR BROADCAST_ADDR[MAC_ADDR_LEN];
+extern UCHAR MULTICAST_ADDR[MAC_ADDR_LEN];
+extern UCHAR ZERO_MAC_ADDR[MAC_ADDR_LEN];
+extern ULONG BIT32[32];
+extern UCHAR BIT8[8];
+extern char* CipherName[];
+extern char* MCSToMbps[];
+extern UCHAR	 RxwiMCSToOfdmRate[12];
+extern UCHAR SNAP_802_1H[6];
+extern UCHAR SNAP_BRIDGE_TUNNEL[6];
+extern UCHAR SNAP_AIRONET[8];
+extern UCHAR CKIP_LLC_SNAP[8];
+extern UCHAR EAPOL_LLC_SNAP[8];
+extern UCHAR EAPOL[2];
+extern UCHAR IPX[2];
+extern UCHAR APPLE_TALK[2];
+extern UCHAR RateIdToPlcpSignal[12]; // see IEEE802.11a-1999 p.14
+extern UCHAR	 OfdmRateToRxwiMCS[];
+extern UCHAR OfdmSignalToRateId[16] ;
+extern UCHAR default_cwmin[4];
+extern UCHAR default_cwmax[4];
+extern UCHAR default_sta_aifsn[4];
+extern UCHAR MapUserPriorityToAccessCategory[8];
+
+extern USHORT RateUpPER[];
+extern USHORT RateDownPER[];
+extern UCHAR  Phy11BNextRateDownward[];
+extern UCHAR  Phy11BNextRateUpward[];
+extern UCHAR  Phy11BGNextRateDownward[];
+extern UCHAR  Phy11BGNextRateUpward[];
+extern UCHAR  Phy11ANextRateDownward[];
+extern UCHAR  Phy11ANextRateUpward[];
+extern CHAR   RssiSafeLevelForTxRate[];
+extern UCHAR  RateIdToMbps[];
+extern USHORT RateIdTo500Kbps[];
+
+extern UCHAR  CipherSuiteWpaNoneTkip[];
+extern UCHAR  CipherSuiteWpaNoneTkipLen;
+
+extern UCHAR  CipherSuiteWpaNoneAes[];
+extern UCHAR  CipherSuiteWpaNoneAesLen;
+
+extern UCHAR  SsidIe;
+extern UCHAR  SupRateIe;
+extern UCHAR  ExtRateIe;
+
+#ifdef DOT11_N_SUPPORT
+extern UCHAR  HtCapIe;
+extern UCHAR  AddHtInfoIe;
+extern UCHAR  NewExtChanIe;
+#ifdef DOT11N_DRAFT3
+extern UCHAR  ExtHtCapIe;
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+extern UCHAR  ErpIe;
+extern UCHAR  DsIe;
+extern UCHAR  TimIe;
+extern UCHAR  WpaIe;
+extern UCHAR  Wpa2Ie;
+extern UCHAR  IbssIe;
+extern UCHAR  Ccx2Ie;
+
+extern UCHAR  WPA_OUI[];
+extern UCHAR  RSN_OUI[];
+extern UCHAR  WME_INFO_ELEM[];
+extern UCHAR  WME_PARM_ELEM[];
+extern UCHAR  Ccx2QosInfo[];
+extern UCHAR  Ccx2IeInfo[];
+extern UCHAR  RALINK_OUI[];
+extern UCHAR  PowerConstraintIE[];
+
+
+extern UCHAR  RateSwitchTable[];
+extern UCHAR  RateSwitchTable11B[];
+extern UCHAR  RateSwitchTable11G[];
+extern UCHAR  RateSwitchTable11BG[];
+
+#ifdef DOT11_N_SUPPORT
+extern UCHAR  RateSwitchTable11BGN1S[];
+extern UCHAR  RateSwitchTable11BGN2S[];
+extern UCHAR  RateSwitchTable11BGN2SForABand[];
+extern UCHAR  RateSwitchTable11N1S[];
+extern UCHAR  RateSwitchTable11N2S[];
+extern UCHAR  RateSwitchTable11N2SForABand[];
+
+#ifdef CONFIG_STA_SUPPORT
+extern UCHAR  PRE_N_HT_OUI[];
+#endif // CONFIG_STA_SUPPORT //
+#endif // DOT11_N_SUPPORT //
+
+#define	MAXSEQ		(0xFFF)
+
+#ifdef RALINK_ATE
+typedef	struct _ATE_INFO {
+	UCHAR	Mode;
+	CHAR	TxPower0;
+	CHAR	TxPower1;
+	CHAR    TxAntennaSel;
+	CHAR    RxAntennaSel;
+	TXWI_STRUC  TxWI; 	  // TXWI
+	USHORT	QID;
+	UCHAR	Addr1[MAC_ADDR_LEN];
+	UCHAR	Addr2[MAC_ADDR_LEN];
+	UCHAR	Addr3[MAC_ADDR_LEN];
+	UCHAR	Channel;
+	UINT32	TxLength;
+	UINT32	TxCount;
+	UINT32	TxDoneCount; // Tx DMA Done
+	UINT32	RFFreqOffset;
+	BOOLEAN	bRxFer;
+	BOOLEAN	bQATxStart; // Have compiled QA in and use it to ATE tx.
+	BOOLEAN	bQARxStart;	// Have compiled QA in and use it to ATE rx.
+	UINT32	RxTotalCnt;
+	UINT32	RxCntPerSec;
+
+	CHAR	LastSNR0;             // last received SNR
+	CHAR    LastSNR1;             // last received SNR for 2nd  antenna
+	CHAR    LastRssi0;            // last received RSSI
+	CHAR    LastRssi1;            // last received RSSI for 2nd  antenna
+	CHAR    LastRssi2;            // last received RSSI for 3rd  antenna
+	CHAR    AvgRssi0;             // last 8 frames' average RSSI
+	CHAR    AvgRssi1;             // last 8 frames' average RSSI
+	CHAR    AvgRssi2;             // last 8 frames' average RSSI
+	SHORT   AvgRssi0X8;           // sum of last 8 frames' RSSI
+	SHORT   AvgRssi1X8;           // sum of last 8 frames' RSSI
+	SHORT   AvgRssi2X8;           // sum of last 8 frames' RSSI
+
+	UINT32	NumOfAvgRssiSample;
+
+#ifdef RALINK_28xx_QA
+	// Tx frame
+#ifdef RT2870
+	/* not used in RT2860 */
+	TXINFO_STRUC		TxInfo; // TxInfo
+#endif // RT2870 //
+	USHORT		HLen; // Header Length
+	USHORT		PLen; // Pattern Length
+	UCHAR 		Header[32]; // Header buffer
+	UCHAR		Pattern[32]; // Pattern buffer
+	USHORT		DLen; // Data Length
+	USHORT		seq;
+	UINT32		CID;
+	THREAD_PID 		AtePid;
+	// counters
+	UINT32		U2M;
+	UINT32		OtherData;
+	UINT32		Beacon;
+	UINT32		OtherCount;
+	UINT32		TxAc0;
+	UINT32		TxAc1;
+	UINT32		TxAc2;
+	UINT32		TxAc3;
+	UINT32		TxHCCA;
+	UINT32		TxMgmt;
+	UINT32		RSSI0;
+	UINT32		RSSI1;
+	UINT32		RSSI2;
+	UINT32		SNR0;
+	UINT32		SNR1;
+	// control
+	//UINT32		Repeat; // Tx Cpu count
+	UCHAR		TxStatus; // task Tx status // 0 --> task is idle, 1 --> task is running
+#endif // RALINK_28xx_QA //
+}	ATE_INFO, *PATE_INFO;
+
+#ifdef RALINK_28xx_QA
+struct ate_racfghdr {
+ 	UINT32		magic_no;
+	USHORT		command_type;
+	USHORT		command_id;
+	USHORT		length;
+	USHORT		sequence;
+	USHORT		status;
+	UCHAR		data[2046];
+}  __attribute__((packed));
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#ifdef DOT11_N_SUPPORT
+struct reordering_mpdu
+{
+	struct reordering_mpdu	*next;
+	PNDIS_PACKET			pPacket;		/* coverted to 802.3 frame */
+	int						Sequence;		/* sequence number of MPDU */
+	BOOLEAN					bAMSDU;
+};
+
+struct reordering_list
+{
+	struct reordering_mpdu *next;
+	int 	qlen;
+};
+
+struct reordering_mpdu_pool
+{
+	PVOID					mem;
+	NDIS_SPIN_LOCK			lock;
+	struct reordering_list 	freelist;
+};
+#endif // DOT11_N_SUPPORT //
+
+typedef struct 	_RSSI_SAMPLE {
+	CHAR			LastRssi0;             // last received RSSI
+	CHAR			LastRssi1;             // last received RSSI
+	CHAR			LastRssi2;             // last received RSSI
+	CHAR			AvgRssi0;
+	CHAR			AvgRssi1;
+	CHAR			AvgRssi2;
+	SHORT			AvgRssi0X8;
+	SHORT			AvgRssi1X8;
+	SHORT			AvgRssi2X8;
+} RSSI_SAMPLE;
+
+//
+//  Queue structure and macros
+//
+typedef struct  _QUEUE_ENTRY    {
+	struct _QUEUE_ENTRY     *Next;
+}   QUEUE_ENTRY, *PQUEUE_ENTRY;
+
+// Queue structure
+typedef struct  _QUEUE_HEADER   {
+	PQUEUE_ENTRY    Head;
+	PQUEUE_ENTRY    Tail;
+	ULONG           Number;
+}   QUEUE_HEADER, *PQUEUE_HEADER;
+
+#define InitializeQueueHeader(QueueHeader)              \
+{                                                       \
+	(QueueHeader)->Head = (QueueHeader)->Tail = NULL;   \
+	(QueueHeader)->Number = 0;                          \
+}
+
+#define RemoveHeadQueue(QueueHeader)                \
+(QueueHeader)->Head;                                \
+{                                                   \
+	PQUEUE_ENTRY pNext;                             \
+	if ((QueueHeader)->Head != NULL)				\
+	{												\
+		pNext = (QueueHeader)->Head->Next;          \
+		(QueueHeader)->Head = pNext;                \
+		if (pNext == NULL)                          \
+			(QueueHeader)->Tail = NULL;             \
+		(QueueHeader)->Number--;                    \
+	}												\
+}
+
+#define InsertHeadQueue(QueueHeader, QueueEntry)            \
+{                                                           \
+		((PQUEUE_ENTRY)QueueEntry)->Next = (QueueHeader)->Head; \
+		(QueueHeader)->Head = (PQUEUE_ENTRY)(QueueEntry);       \
+		if ((QueueHeader)->Tail == NULL)                        \
+			(QueueHeader)->Tail = (PQUEUE_ENTRY)(QueueEntry);   \
+		(QueueHeader)->Number++;                                \
+}
+
+#define InsertTailQueue(QueueHeader, QueueEntry)                \
+{                                                               \
+	((PQUEUE_ENTRY)QueueEntry)->Next = NULL;                    \
+	if ((QueueHeader)->Tail)                                    \
+		(QueueHeader)->Tail->Next = (PQUEUE_ENTRY)(QueueEntry); \
+	else                                                        \
+		(QueueHeader)->Head = (PQUEUE_ENTRY)(QueueEntry);       \
+	(QueueHeader)->Tail = (PQUEUE_ENTRY)(QueueEntry);           \
+	(QueueHeader)->Number++;                                    \
+}
+
+//
+//  Macros for flag and ref count operations
+//
+#define RTMP_SET_FLAG(_M, _F)       ((_M)->Flags |= (_F))
+#define RTMP_CLEAR_FLAG(_M, _F)     ((_M)->Flags &= ~(_F))
+#define RTMP_CLEAR_FLAGS(_M)        ((_M)->Flags = 0)
+#define RTMP_TEST_FLAG(_M, _F)      (((_M)->Flags & (_F)) != 0)
+#define RTMP_TEST_FLAGS(_M, _F)     (((_M)->Flags & (_F)) == (_F))
+
+#define OPSTATUS_SET_FLAG(_pAd, _F)     ((_pAd)->CommonCfg.OpStatusFlags |= (_F))
+#define OPSTATUS_CLEAR_FLAG(_pAd, _F)   ((_pAd)->CommonCfg.OpStatusFlags &= ~(_F))
+#define OPSTATUS_TEST_FLAG(_pAd, _F)    (((_pAd)->CommonCfg.OpStatusFlags & (_F)) != 0)
+
+#define CLIENT_STATUS_SET_FLAG(_pEntry,_F)      ((_pEntry)->ClientStatusFlags |= (_F))
+#define CLIENT_STATUS_CLEAR_FLAG(_pEntry,_F)    ((_pEntry)->ClientStatusFlags &= ~(_F))
+#define CLIENT_STATUS_TEST_FLAG(_pEntry,_F)     (((_pEntry)->ClientStatusFlags & (_F)) != 0)
+
+#define RX_FILTER_SET_FLAG(_pAd, _F)    ((_pAd)->CommonCfg.PacketFilter |= (_F))
+#define RX_FILTER_CLEAR_FLAG(_pAd, _F)  ((_pAd)->CommonCfg.PacketFilter &= ~(_F))
+#define RX_FILTER_TEST_FLAG(_pAd, _F)   (((_pAd)->CommonCfg.PacketFilter & (_F)) != 0)
+
+#ifdef CONFIG_STA_SUPPORT
+#define STA_NO_SECURITY_ON(_p)          (_p->StaCfg.WepStatus == Ndis802_11EncryptionDisabled)
+#define STA_WEP_ON(_p)                  (_p->StaCfg.WepStatus == Ndis802_11Encryption1Enabled)
+#define STA_TKIP_ON(_p)                 (_p->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+#define STA_AES_ON(_p)                  (_p->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+
+#define STA_TGN_WIFI_ON(_p)             (_p->StaCfg.bTGnWifiTest == TRUE)
+#endif // CONFIG_STA_SUPPORT //
+
+#define CKIP_KP_ON(_p)				((((_p)->StaCfg.CkipFlag) & 0x10) && ((_p)->StaCfg.bCkipCmicOn == TRUE))
+#define CKIP_CMIC_ON(_p)			((((_p)->StaCfg.CkipFlag) & 0x08) && ((_p)->StaCfg.bCkipCmicOn == TRUE))
+
+
+#define INC_RING_INDEX(_idx, _RingSize)    \
+{                                          \
+    (_idx) = (_idx+1) % (_RingSize);       \
+}
+
+#define IS_RT3070(_pAd)				(((_pAd)->MACVersion & 0xffff0000) == 0x30700000)
+
+#define RING_PACKET_INIT(_TxRing, _idx)    \
+{                                          \
+    _TxRing->Cell[_idx].pNdisPacket = NULL;                              \
+    _TxRing->Cell[_idx].pNextNdisPacket = NULL;                              \
+}
+
+#define TXDT_INIT(_TxD)    \
+{                                          \
+	NdisZeroMemory(_TxD, TXD_SIZE);	\
+	_TxD->DMADONE = 1;                              \
+}
+
+//Set last data segment
+#define RING_SET_LASTDS(_TxD, _IsSD0)    \
+{                                          \
+    if (_IsSD0) {_TxD->LastSec0 = 1;}     \
+    else {_TxD->LastSec1 = 1;}     \
+}
+
+// Increase TxTsc value for next transmission
+// TODO:
+// When i==6, means TSC has done one full cycle, do re-keying stuff follow specs
+// Should send a special event microsoft defined to request re-key
+#define INC_TX_TSC(_tsc)                                \
+{                                                       \
+    int i=0;                                            \
+    while (++_tsc[i] == 0x0)                            \
+    {                                                   \
+        i++;                                            \
+        if (i == 6)                                     \
+            break;                                      \
+    }                                                   \
+}
+
+#ifdef DOT11_N_SUPPORT
+// StaActive.SupportedHtPhy.MCSSet is copied from AP beacon.  Don't need to update here.
+#define COPY_HTSETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(_pAd)                                 \
+{                                                                                       \
+	_pAd->StaActive.SupportedHtPhy.ChannelWidth = _pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth;      \
+	_pAd->StaActive.SupportedHtPhy.MimoPs = _pAd->MlmeAux.HtCapability.HtCapInfo.MimoPs;      \
+	_pAd->StaActive.SupportedHtPhy.GF = _pAd->MlmeAux.HtCapability.HtCapInfo.GF;      \
+	_pAd->StaActive.SupportedHtPhy.ShortGIfor20 = _pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor20;      \
+	_pAd->StaActive.SupportedHtPhy.ShortGIfor40 = _pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor40;      \
+	_pAd->StaActive.SupportedHtPhy.TxSTBC = _pAd->MlmeAux.HtCapability.HtCapInfo.TxSTBC;      \
+	_pAd->StaActive.SupportedHtPhy.RxSTBC = _pAd->MlmeAux.HtCapability.HtCapInfo.RxSTBC;      \
+	_pAd->StaActive.SupportedHtPhy.ExtChanOffset = _pAd->MlmeAux.AddHtInfo.AddHtInfo.ExtChanOffset;      \
+	_pAd->StaActive.SupportedHtPhy.RecomWidth = _pAd->MlmeAux.AddHtInfo.AddHtInfo.RecomWidth;      \
+	_pAd->StaActive.SupportedHtPhy.OperaionMode = _pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode;      \
+	_pAd->StaActive.SupportedHtPhy.NonGfPresent = _pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent;      \
+	NdisMoveMemory((_pAd)->MacTab.Content[BSSID_WCID].HTCapability.MCSSet, (_pAd)->StaActive.SupportedPhyInfo.MCSSet, sizeof(UCHAR) * 16);\
+}
+
+#define COPY_AP_HTSETTINGS_FROM_BEACON(_pAd, _pHtCapability)                                 \
+{                                                                                       \
+	_pAd->MacTab.Content[BSSID_WCID].AMsduSize = (UCHAR)(_pHtCapability->HtCapInfo.AMsduSize);	\
+	_pAd->MacTab.Content[BSSID_WCID].MmpsMode= (UCHAR)(_pHtCapability->HtCapInfo.MimoPs);	\
+	_pAd->MacTab.Content[BSSID_WCID].MaxRAmpduFactor = (UCHAR)(_pHtCapability->HtCapParm.MaxRAmpduFactor);	\
+}
+#endif // DOT11_N_SUPPORT //
+
+//
+// BBP & RF are using indirect access. Before write any value into it.
+// We have to make sure there is no outstanding command pending via checking busy bit.
+//
+#define MAX_BUSY_COUNT  100         // Number of retry before failing access BBP & RF indirect register
+//
+
+#ifdef RT2870
+#define RTMP_RF_IO_WRITE32(_A, _V)                 RTUSBWriteRFRegister(_A, _V)
+#define RTMP_BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)   RTUSBReadBBPRegister(_A, _I, _pV)
+#define RTMP_BBP_IO_WRITE8_BY_REG_ID(_A, _I, _V)   RTUSBWriteBBPRegister(_A, _I, _V)
+
+#define BBP_IO_WRITE8_BY_REG_ID(_A, _I, _V)			RTUSBWriteBBPRegister(_A, _I, _V)
+#define BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)   		RTUSBReadBBPRegister(_A, _I, _pV)
+#endif // RT2870 //
+
+#define     MAP_CHANNEL_ID_TO_KHZ(ch, khz)  {               \
+                switch (ch)                                 \
+                {                                           \
+                    case 1:     khz = 2412000;   break;     \
+                    case 2:     khz = 2417000;   break;     \
+                    case 3:     khz = 2422000;   break;     \
+                    case 4:     khz = 2427000;   break;     \
+                    case 5:     khz = 2432000;   break;     \
+                    case 6:     khz = 2437000;   break;     \
+                    case 7:     khz = 2442000;   break;     \
+                    case 8:     khz = 2447000;   break;     \
+                    case 9:     khz = 2452000;   break;     \
+                    case 10:    khz = 2457000;   break;     \
+                    case 11:    khz = 2462000;   break;     \
+                    case 12:    khz = 2467000;   break;     \
+                    case 13:    khz = 2472000;   break;     \
+                    case 14:    khz = 2484000;   break;     \
+                    case 36:  /* UNII */  khz = 5180000;   break;     \
+                    case 40:  /* UNII */  khz = 5200000;   break;     \
+                    case 44:  /* UNII */  khz = 5220000;   break;     \
+                    case 48:  /* UNII */  khz = 5240000;   break;     \
+                    case 52:  /* UNII */  khz = 5260000;   break;     \
+                    case 56:  /* UNII */  khz = 5280000;   break;     \
+                    case 60:  /* UNII */  khz = 5300000;   break;     \
+                    case 64:  /* UNII */  khz = 5320000;   break;     \
+                    case 149: /* UNII */  khz = 5745000;   break;     \
+                    case 153: /* UNII */  khz = 5765000;   break;     \
+                    case 157: /* UNII */  khz = 5785000;   break;     \
+                    case 161: /* UNII */  khz = 5805000;   break;     \
+                    case 165: /* UNII */  khz = 5825000;   break;     \
+                    case 100: /* HiperLAN2 */  khz = 5500000;   break;     \
+                    case 104: /* HiperLAN2 */  khz = 5520000;   break;     \
+                    case 108: /* HiperLAN2 */  khz = 5540000;   break;     \
+                    case 112: /* HiperLAN2 */  khz = 5560000;   break;     \
+                    case 116: /* HiperLAN2 */  khz = 5580000;   break;     \
+                    case 120: /* HiperLAN2 */  khz = 5600000;   break;     \
+                    case 124: /* HiperLAN2 */  khz = 5620000;   break;     \
+                    case 128: /* HiperLAN2 */  khz = 5640000;   break;     \
+                    case 132: /* HiperLAN2 */  khz = 5660000;   break;     \
+                    case 136: /* HiperLAN2 */  khz = 5680000;   break;     \
+                    case 140: /* HiperLAN2 */  khz = 5700000;   break;     \
+                    case 34:  /* Japan MMAC */   khz = 5170000;   break;   \
+                    case 38:  /* Japan MMAC */   khz = 5190000;   break;   \
+                    case 42:  /* Japan MMAC */   khz = 5210000;   break;   \
+                    case 46:  /* Japan MMAC */   khz = 5230000;   break;   \
+                    case 184: /* Japan */   khz = 4920000;   break;   \
+                    case 188: /* Japan */   khz = 4940000;   break;   \
+                    case 192: /* Japan */   khz = 4960000;   break;   \
+                    case 196: /* Japan */   khz = 4980000;   break;   \
+                    case 208: /* Japan, means J08 */   khz = 5040000;   break;   \
+                    case 212: /* Japan, means J12 */   khz = 5060000;   break;   \
+                    case 216: /* Japan, means J16 */   khz = 5080000;   break;   \
+                    default:    khz = 2412000;   break;     \
+                }                                           \
+            }
+
+#define     MAP_KHZ_TO_CHANNEL_ID(khz, ch)  {               \
+                switch (khz)                                \
+                {                                           \
+                    case 2412000:    ch = 1;     break;     \
+                    case 2417000:    ch = 2;     break;     \
+                    case 2422000:    ch = 3;     break;     \
+                    case 2427000:    ch = 4;     break;     \
+                    case 2432000:    ch = 5;     break;     \
+                    case 2437000:    ch = 6;     break;     \
+                    case 2442000:    ch = 7;     break;     \
+                    case 2447000:    ch = 8;     break;     \
+                    case 2452000:    ch = 9;     break;     \
+                    case 2457000:    ch = 10;    break;     \
+                    case 2462000:    ch = 11;    break;     \
+                    case 2467000:    ch = 12;    break;     \
+                    case 2472000:    ch = 13;    break;     \
+                    case 2484000:    ch = 14;    break;     \
+                    case 5180000:    ch = 36;  /* UNII */  break;     \
+                    case 5200000:    ch = 40;  /* UNII */  break;     \
+                    case 5220000:    ch = 44;  /* UNII */  break;     \
+                    case 5240000:    ch = 48;  /* UNII */  break;     \
+                    case 5260000:    ch = 52;  /* UNII */  break;     \
+                    case 5280000:    ch = 56;  /* UNII */  break;     \
+                    case 5300000:    ch = 60;  /* UNII */  break;     \
+                    case 5320000:    ch = 64;  /* UNII */  break;     \
+                    case 5745000:    ch = 149; /* UNII */  break;     \
+                    case 5765000:    ch = 153; /* UNII */  break;     \
+                    case 5785000:    ch = 157; /* UNII */  break;     \
+                    case 5805000:    ch = 161; /* UNII */  break;     \
+                    case 5825000:    ch = 165; /* UNII */  break;     \
+                    case 5500000:    ch = 100; /* HiperLAN2 */  break;     \
+                    case 5520000:    ch = 104; /* HiperLAN2 */  break;     \
+                    case 5540000:    ch = 108; /* HiperLAN2 */  break;     \
+                    case 5560000:    ch = 112; /* HiperLAN2 */  break;     \
+                    case 5580000:    ch = 116; /* HiperLAN2 */  break;     \
+                    case 5600000:    ch = 120; /* HiperLAN2 */  break;     \
+                    case 5620000:    ch = 124; /* HiperLAN2 */  break;     \
+                    case 5640000:    ch = 128; /* HiperLAN2 */  break;     \
+                    case 5660000:    ch = 132; /* HiperLAN2 */  break;     \
+                    case 5680000:    ch = 136; /* HiperLAN2 */  break;     \
+                    case 5700000:    ch = 140; /* HiperLAN2 */  break;     \
+                    case 5170000:    ch = 34;  /* Japan MMAC */   break;   \
+                    case 5190000:    ch = 38;  /* Japan MMAC */   break;   \
+                    case 5210000:    ch = 42;  /* Japan MMAC */   break;   \
+                    case 5230000:    ch = 46;  /* Japan MMAC */   break;   \
+                    case 4920000:    ch = 184; /* Japan */  break;   \
+                    case 4940000:    ch = 188; /* Japan */  break;   \
+                    case 4960000:    ch = 192; /* Japan */  break;   \
+                    case 4980000:    ch = 196; /* Japan */  break;   \
+                    case 5040000:    ch = 208; /* Japan, means J08 */  break;   \
+                    case 5060000:    ch = 212; /* Japan, means J12 */  break;   \
+                    case 5080000:    ch = 216; /* Japan, means J16 */  break;   \
+                    default:         ch = 1;     break;     \
+                }                                           \
+            }
+
+//
+// Common fragment list structure -  Identical to the scatter gather frag list structure
+//
+//#define RTMP_SCATTER_GATHER_ELEMENT         SCATTER_GATHER_ELEMENT
+//#define PRTMP_SCATTER_GATHER_ELEMENT        PSCATTER_GATHER_ELEMENT
+#define NIC_MAX_PHYS_BUF_COUNT              8
+
+typedef struct _RTMP_SCATTER_GATHER_ELEMENT {
+    PVOID		Address;
+    ULONG		Length;
+    PULONG		Reserved;
+} RTMP_SCATTER_GATHER_ELEMENT, *PRTMP_SCATTER_GATHER_ELEMENT;
+
+
+typedef struct _RTMP_SCATTER_GATHER_LIST {
+    ULONG  NumberOfElements;
+    PULONG Reserved;
+    RTMP_SCATTER_GATHER_ELEMENT Elements[NIC_MAX_PHYS_BUF_COUNT];
+} RTMP_SCATTER_GATHER_LIST, *PRTMP_SCATTER_GATHER_LIST;
+
+//
+//  Some utility macros
+//
+#ifndef min
+#define min(_a, _b)     (((_a) < (_b)) ? (_a) : (_b))
+#endif
+
+#ifndef max
+#define max(_a, _b)     (((_a) > (_b)) ? (_a) : (_b))
+#endif
+
+#define GET_LNA_GAIN(_pAd)	((_pAd->LatchRfRegs.Channel <= 14) ? (_pAd->BLNAGain) : ((_pAd->LatchRfRegs.Channel <= 64) ? (_pAd->ALNAGain0) : ((_pAd->LatchRfRegs.Channel <= 128) ? (_pAd->ALNAGain1) : (_pAd->ALNAGain2))))
+
+#define INC_COUNTER64(Val)          (Val.QuadPart++)
+
+#define INFRA_ON(_p)                (OPSTATUS_TEST_FLAG(_p, fOP_STATUS_INFRA_ON))
+#define ADHOC_ON(_p)                (OPSTATUS_TEST_FLAG(_p, fOP_STATUS_ADHOC_ON))
+#define MONITOR_ON(_p)              (((_p)->StaCfg.BssType) == BSS_MONITOR)
+#define IDLE_ON(_p)                 (!INFRA_ON(_p) && !ADHOC_ON(_p))
+
+// Check LEAP & CCKM flags
+#define LEAP_ON(_p)                 (((_p)->StaCfg.LeapAuthMode) == CISCO_AuthModeLEAP)
+#define LEAP_CCKM_ON(_p)            ((((_p)->StaCfg.LeapAuthMode) == CISCO_AuthModeLEAP) && ((_p)->StaCfg.LeapAuthInfo.CCKM == TRUE))
+
+// if orginal Ethernet frame contains no LLC/SNAP, then an extra LLC/SNAP encap is required
+#define EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(_pBufVA, _pExtraLlcSnapEncap)		\
+{																\
+	if (((*(_pBufVA + 12) << 8) + *(_pBufVA + 13)) > 1500)		\
+	{															\
+		_pExtraLlcSnapEncap = SNAP_802_1H;						\
+		if (NdisEqualMemory(IPX, _pBufVA + 12, 2) || 			\
+			NdisEqualMemory(APPLE_TALK, _pBufVA + 12, 2))		\
+		{														\
+			_pExtraLlcSnapEncap = SNAP_BRIDGE_TUNNEL;			\
+		}														\
+	}															\
+	else														\
+	{															\
+		_pExtraLlcSnapEncap = NULL;								\
+	}															\
+}
+
+// New Define for new Tx Path.
+#define EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(_pBufVA, _pExtraLlcSnapEncap)	\
+{																\
+	if (((*(_pBufVA) << 8) + *(_pBufVA + 1)) > 1500)			\
+	{															\
+		_pExtraLlcSnapEncap = SNAP_802_1H;						\
+		if (NdisEqualMemory(IPX, _pBufVA, 2) || 				\
+			NdisEqualMemory(APPLE_TALK, _pBufVA, 2))			\
+		{														\
+			_pExtraLlcSnapEncap = SNAP_BRIDGE_TUNNEL;			\
+		}														\
+	}															\
+	else														\
+	{															\
+		_pExtraLlcSnapEncap = NULL;								\
+	}															\
+}
+
+
+#define MAKE_802_3_HEADER(_p, _pMac1, _pMac2, _pType)                   \
+{                                                                       \
+    NdisMoveMemory(_p, _pMac1, MAC_ADDR_LEN);                           \
+    NdisMoveMemory((_p + MAC_ADDR_LEN), _pMac2, MAC_ADDR_LEN);          \
+    NdisMoveMemory((_p + MAC_ADDR_LEN * 2), _pType, LENGTH_802_3_TYPE); \
+}
+
+// if pData has no LLC/SNAP (neither RFC1042 nor Bridge tunnel), keep it that way.
+// else if the received frame is LLC/SNAP-encaped IPX or APPLETALK, preserve the LLC/SNAP field
+// else remove the LLC/SNAP field from the result Ethernet frame
+// Patch for WHQL only, which did not turn on Netbios but use IPX within its payload
+// Note:
+//     _pData & _DataSize may be altered (remove 8-byte LLC/SNAP) by this MACRO
+//     _pRemovedLLCSNAP: pointer to removed LLC/SNAP; NULL is not removed
+#define CONVERT_TO_802_3(_p8023hdr, _pDA, _pSA, _pData, _DataSize, _pRemovedLLCSNAP)      \
+{                                                                       \
+    char LLC_Len[2];                                                    \
+                                                                        \
+    _pRemovedLLCSNAP = NULL;                                            \
+    if (NdisEqualMemory(SNAP_802_1H, _pData, 6)  ||                     \
+        NdisEqualMemory(SNAP_BRIDGE_TUNNEL, _pData, 6))                 \
+    {                                                                   \
+        PUCHAR pProto = _pData + 6;                                     \
+                                                                        \
+        if ((NdisEqualMemory(IPX, pProto, 2) || NdisEqualMemory(APPLE_TALK, pProto, 2)) &&  \
+            NdisEqualMemory(SNAP_802_1H, _pData, 6))                    \
+        {                                                               \
+            LLC_Len[0] = (UCHAR)(_DataSize / 256);                      \
+            LLC_Len[1] = (UCHAR)(_DataSize % 256);                      \
+            MAKE_802_3_HEADER(_p8023hdr, _pDA, _pSA, LLC_Len);          \
+        }                                                               \
+        else                                                            \
+        {                                                               \
+            MAKE_802_3_HEADER(_p8023hdr, _pDA, _pSA, pProto);           \
+            _pRemovedLLCSNAP = _pData;                                  \
+            _DataSize -= LENGTH_802_1_H;                                \
+            _pData += LENGTH_802_1_H;                                   \
+        }                                                               \
+    }                                                                   \
+    else                                                                \
+    {                                                                   \
+        LLC_Len[0] = (UCHAR)(_DataSize / 256);                          \
+        LLC_Len[1] = (UCHAR)(_DataSize % 256);                          \
+        MAKE_802_3_HEADER(_p8023hdr, _pDA, _pSA, LLC_Len);              \
+    }                                                                   \
+}
+
+#define SWITCH_AB( _pAA, _pBB)    \
+{                                                                           \
+    PVOID pCC;                                                          \
+    pCC = _pBB;                                                 \
+    _pBB = _pAA;                                                 \
+    _pAA = pCC;                                                 \
+}
+
+// Enqueue this frame to MLME engine
+// We need to enqueue the whole frame because MLME need to pass data type
+// information from 802.11 header
+#ifdef RT2870
+#define REPORT_MGMT_FRAME_TO_MLME(_pAd, Wcid, _pFrame, _FrameSize, _Rssi0, _Rssi1, _Rssi2, _PlcpSignal)        \
+{                                                                                       \
+    UINT32 High32TSF=0, Low32TSF=0;                                                          \
+    MlmeEnqueueForRecv(_pAd, Wcid, High32TSF, Low32TSF, (UCHAR)_Rssi0, (UCHAR)_Rssi1,(UCHAR)_Rssi2,_FrameSize, _pFrame, (UCHAR)_PlcpSignal);   \
+}
+#endif // RT2870 //
+
+#define NDIS_QUERY_BUFFER(_NdisBuf, _ppVA, _pBufLen)                    \
+    NdisQueryBuffer(_NdisBuf, _ppVA, _pBufLen)
+
+#define MAC_ADDR_EQUAL(pAddr1,pAddr2)           RTMPEqualMemory((PVOID)(pAddr1), (PVOID)(pAddr2), MAC_ADDR_LEN)
+#define SSID_EQUAL(ssid1, len1, ssid2, len2)    ((len1==len2) && (RTMPEqualMemory(ssid1, ssid2, len1)))
+
+//
+// Check if it is Japan W53(ch52,56,60,64) channel.
+//
+#define JapanChannelCheck(channel)  ((channel == 52) || (channel == 56) || (channel == 60) || (channel == 64))
+
+#ifdef CONFIG_STA_SUPPORT
+#define STA_PORT_SECURED(_pAd) \
+{ \
+	_pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED; \
+	NdisAcquireSpinLock(&_pAd->MacTabLock); \
+	_pAd->MacTab.Content[BSSID_WCID].PortSecured = _pAd->StaCfg.PortSecured; \
+	NdisReleaseSpinLock(&_pAd->MacTabLock); \
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+//
+// Register set pair for initialzation register set definition
+//
+typedef struct  _RTMP_REG_PAIR
+{
+	ULONG   Register;
+	ULONG   Value;
+} RTMP_REG_PAIR, *PRTMP_REG_PAIR;
+
+typedef struct  _REG_PAIR
+{
+	UCHAR   Register;
+	UCHAR   Value;
+} REG_PAIR, *PREG_PAIR;
+
+//
+// Register set pair for initialzation register set definition
+//
+typedef struct  _RTMP_RF_REGS
+{
+	UCHAR   Channel;
+	ULONG   R1;
+	ULONG   R2;
+	ULONG   R3;
+	ULONG   R4;
+} RTMP_RF_REGS, *PRTMP_RF_REGS;
+
+typedef struct _FREQUENCY_ITEM {
+	UCHAR	Channel;
+	UCHAR	N;
+	UCHAR	R;
+	UCHAR	K;
+} FREQUENCY_ITEM, *PFREQUENCY_ITEM;
+
+//
+//  Data buffer for DMA operation, the buffer must be contiguous physical memory
+//  Both DMA to / from CPU use the same structure.
+//
+typedef struct  _RTMP_DMABUF
+{
+	ULONG                   AllocSize;
+	PVOID                   AllocVa;            // TxBuf virtual address
+	NDIS_PHYSICAL_ADDRESS   AllocPa;            // TxBuf physical address
+} RTMP_DMABUF, *PRTMP_DMABUF;
+
+
+typedef	union	_HEADER_802_11_SEQ{
+#ifdef RT_BIG_ENDIAN
+    struct {
+   	USHORT			Sequence:12;
+	USHORT			Frag:4;
+    }   field;
+#else
+    struct {
+	USHORT			Frag:4;
+	USHORT			Sequence:12;
+    }   field;
+#endif
+    USHORT           value;
+}	HEADER_802_11_SEQ, *PHEADER_802_11_SEQ;
+
+//
+//  Data buffer for DMA operation, the buffer must be contiguous physical memory
+//  Both DMA to / from CPU use the same structure.
+//
+typedef struct  _RTMP_REORDERBUF
+{
+	BOOLEAN			IsFull;
+	PVOID                   AllocVa;            // TxBuf virtual address
+	UCHAR			Header802_3[14];
+	HEADER_802_11_SEQ			Sequence;	//support compressed bitmap BA, so no consider fragment in BA
+	UCHAR 		DataOffset;
+	USHORT 		Datasize;
+	ULONG                   AllocSize;
+#ifdef RT2870
+	PUCHAR					AllocPa;
+#endif // RT2870 //
+}   RTMP_REORDERBUF, *PRTMP_REORDERBUF;
+
+//
+// Control block (Descriptor) for all ring descriptor DMA operation, buffer must be
+// contiguous physical memory. NDIS_PACKET stored the binding Rx packet descriptor
+// which won't be released, driver has to wait until upper layer return the packet
+// before giveing up this rx ring descriptor to ASIC. NDIS_BUFFER is assocaited pair
+// to describe the packet buffer. For Tx, NDIS_PACKET stored the tx packet descriptor
+// which driver should ACK upper layer when the tx is physically done or failed.
+//
+typedef struct _RTMP_DMACB
+{
+	ULONG                   AllocSize;          // Control block size
+	PVOID                   AllocVa;            // Control block virtual address
+	NDIS_PHYSICAL_ADDRESS   AllocPa;            // Control block physical address
+	PNDIS_PACKET pNdisPacket;
+	PNDIS_PACKET pNextNdisPacket;
+
+	RTMP_DMABUF             DmaBuf;             // Associated DMA buffer structure
+} RTMP_DMACB, *PRTMP_DMACB;
+
+typedef struct _RTMP_TX_BUF
+{
+	PQUEUE_ENTRY    Next;
+	UCHAR           Index;
+	ULONG                   AllocSize;          // Control block size
+	PVOID                   AllocVa;            // Control block virtual address
+	NDIS_PHYSICAL_ADDRESS   AllocPa;            // Control block physical address
+} RTMP_TXBUF, *PRTMP_TXBUF;
+
+typedef struct _RTMP_RX_BUF
+{
+	BOOLEAN           InUse;
+	ULONG           	ByBaRecIndex;
+	RTMP_REORDERBUF	MAP_RXBuf[MAX_RX_REORDERBUF];
+} RTMP_RXBUF, *PRTMP_RXBUF;
+typedef struct _RTMP_TX_RING
+{
+	RTMP_DMACB  Cell[TX_RING_SIZE];
+	UINT32		TxCpuIdx;
+	UINT32		TxDmaIdx;
+	UINT32		TxSwFreeIdx; 	// software next free tx index
+} RTMP_TX_RING, *PRTMP_TX_RING;
+
+typedef struct _RTMP_RX_RING
+{
+	RTMP_DMACB  Cell[RX_RING_SIZE];
+	UINT32		RxCpuIdx;
+	UINT32		RxDmaIdx;
+	INT32		RxSwReadIdx; 	// software next read index
+} RTMP_RX_RING, *PRTMP_RX_RING;
+
+typedef struct _RTMP_MGMT_RING
+{
+	RTMP_DMACB  Cell[MGMT_RING_SIZE];
+	UINT32		TxCpuIdx;
+	UINT32		TxDmaIdx;
+	UINT32		TxSwFreeIdx; // software next free tx index
+} RTMP_MGMT_RING, *PRTMP_MGMT_RING;
+
+//
+//  Statistic counter structure
+//
+typedef struct _COUNTER_802_3
+{
+	// General Stats
+	ULONG       GoodTransmits;
+	ULONG       GoodReceives;
+	ULONG       TxErrors;
+	ULONG       RxErrors;
+	ULONG       RxNoBuffer;
+
+	// Ethernet Stats
+	ULONG       RcvAlignmentErrors;
+	ULONG       OneCollision;
+	ULONG       MoreCollisions;
+
+} COUNTER_802_3, *PCOUNTER_802_3;
+
+typedef struct _COUNTER_802_11 {
+	ULONG           Length;
+	LARGE_INTEGER   LastTransmittedFragmentCount;
+	LARGE_INTEGER   TransmittedFragmentCount;
+	LARGE_INTEGER   MulticastTransmittedFrameCount;
+	LARGE_INTEGER   FailedCount;
+	LARGE_INTEGER   RetryCount;
+	LARGE_INTEGER   MultipleRetryCount;
+	LARGE_INTEGER   RTSSuccessCount;
+	LARGE_INTEGER   RTSFailureCount;
+	LARGE_INTEGER   ACKFailureCount;
+	LARGE_INTEGER   FrameDuplicateCount;
+	LARGE_INTEGER   ReceivedFragmentCount;
+	LARGE_INTEGER   MulticastReceivedFrameCount;
+	LARGE_INTEGER   FCSErrorCount;
+} COUNTER_802_11, *PCOUNTER_802_11;
+
+typedef struct _COUNTER_RALINK {
+	ULONG           TransmittedByteCount;   // both successful and failure, used to calculate TX throughput
+	ULONG           ReceivedByteCount;      // both CRC okay and CRC error, used to calculate RX throughput
+	ULONG           BeenDisassociatedCount;
+	ULONG           BadCQIAutoRecoveryCount;
+	ULONG           PoorCQIRoamingCount;
+	ULONG           MgmtRingFullCount;
+	ULONG           RxCountSinceLastNULL;
+	ULONG           RxCount;
+	ULONG           RxRingErrCount;
+	ULONG           KickTxCount;
+	ULONG           TxRingErrCount;
+	LARGE_INTEGER   RealFcsErrCount;
+	ULONG           PendingNdisPacketCount;
+
+	ULONG           OneSecOsTxCount[NUM_OF_TX_RING];
+	ULONG           OneSecDmaDoneCount[NUM_OF_TX_RING];
+	UINT32          OneSecTxDoneCount;
+	ULONG           OneSecRxCount;
+	UINT32          OneSecTxAggregationCount;
+	UINT32          OneSecRxAggregationCount;
+
+	UINT32   		OneSecFrameDuplicateCount;
+
+#ifdef RT2870
+	ULONG           OneSecTransmittedByteCount;   // both successful and failure, used to calculate TX throughput
+#endif // RT2870 //
+
+	UINT32          OneSecTxNoRetryOkCount;
+	UINT32          OneSecTxRetryOkCount;
+	UINT32          OneSecTxFailCount;
+	UINT32          OneSecFalseCCACnt;      // CCA error count, for debug purpose, might move to global counter
+	UINT32          OneSecRxOkCnt;          // RX without error
+	UINT32          OneSecRxOkDataCnt;      // unicast-to-me DATA frame count
+	UINT32          OneSecRxFcsErrCnt;      // CRC error
+	UINT32          OneSecBeaconSentCnt;
+	UINT32          LastOneSecTotalTxCount; // OneSecTxNoRetryOkCount + OneSecTxRetryOkCount + OneSecTxFailCount
+	UINT32          LastOneSecRxOkDataCnt;  // OneSecRxOkDataCnt
+	ULONG		DuplicateRcv;
+	ULONG		TxAggCount;
+	ULONG		TxNonAggCount;
+	ULONG		TxAgg1MPDUCount;
+	ULONG		TxAgg2MPDUCount;
+	ULONG		TxAgg3MPDUCount;
+	ULONG		TxAgg4MPDUCount;
+	ULONG		TxAgg5MPDUCount;
+	ULONG		TxAgg6MPDUCount;
+	ULONG		TxAgg7MPDUCount;
+	ULONG		TxAgg8MPDUCount;
+	ULONG		TxAgg9MPDUCount;
+	ULONG		TxAgg10MPDUCount;
+	ULONG		TxAgg11MPDUCount;
+	ULONG		TxAgg12MPDUCount;
+	ULONG		TxAgg13MPDUCount;
+	ULONG		TxAgg14MPDUCount;
+	ULONG		TxAgg15MPDUCount;
+	ULONG		TxAgg16MPDUCount;
+
+	LARGE_INTEGER       TransmittedOctetsInAMSDU;
+	LARGE_INTEGER       TransmittedAMSDUCount;
+	LARGE_INTEGER       ReceivedOctesInAMSDUCount;
+	LARGE_INTEGER       ReceivedAMSDUCount;
+	LARGE_INTEGER       TransmittedAMPDUCount;
+	LARGE_INTEGER       TransmittedMPDUsInAMPDUCount;
+	LARGE_INTEGER       TransmittedOctetsInAMPDUCount;
+	LARGE_INTEGER       MPDUInReceivedAMPDUCount;
+} COUNTER_RALINK, *PCOUNTER_RALINK;
+
+typedef struct _PID_COUNTER {
+	ULONG           TxAckRequiredCount;      // CRC error
+	ULONG           TxAggreCount;
+	ULONG           TxSuccessCount; // OneSecTxNoRetryOkCount + OneSecTxRetryOkCount + OneSecTxFailCount
+	ULONG		LastSuccessRate;
+} PID_COUNTER, *PPID_COUNTER;
+
+typedef struct _COUNTER_DRS {
+	// to record the each TX rate's quality. 0 is best, the bigger the worse.
+	USHORT          TxQuality[MAX_STEP_OF_TX_RATE_SWITCH];
+	UCHAR           PER[MAX_STEP_OF_TX_RATE_SWITCH];
+	UCHAR           TxRateUpPenalty;      // extra # of second penalty due to last unstable condition
+	ULONG           CurrTxRateStableTime; // # of second in current TX rate
+	BOOLEAN         fNoisyEnvironment;
+	BOOLEAN         fLastSecAccordingRSSI;
+	UCHAR           LastSecTxRateChangeAction; // 0: no change, 1:rate UP, 2:rate down
+	UCHAR			LastTimeTxRateChangeAction; //Keep last time value of LastSecTxRateChangeAction
+	ULONG			LastTxOkCount;
+} COUNTER_DRS, *PCOUNTER_DRS;
+
+//
+//  Arcfour Structure Added by PaulWu
+//
+typedef struct  _ARCFOUR
+{
+	UINT            X;
+	UINT            Y;
+	UCHAR           STATE[256];
+} ARCFOURCONTEXT, *PARCFOURCONTEXT;
+
+// MIMO Tx parameter, ShortGI, MCS, STBC, etc.  these are fields in TXWI too. just copy to TXWI.
+typedef struct  _RECEIVE_SETTING {
+#ifdef RT_BIG_ENDIAN
+	USHORT		MIMO:1;
+	USHORT		OFDM:1;
+	USHORT		rsv:3;
+	USHORT		STBC:2;	//SPACE
+	USHORT		ShortGI:1;
+	USHORT		Mode:2;	//channel bandwidth 20MHz or 40 MHz
+	USHORT   	NumOfRX:2;                 // MIMO. WE HAVE 3R
+#else
+	USHORT   	NumOfRX:2;                 // MIMO. WE HAVE 3R
+	USHORT		Mode:2;	//channel bandwidth 20MHz or 40 MHz
+	USHORT		ShortGI:1;
+	USHORT		STBC:2;	//SPACE
+	USHORT		rsv:3;
+	USHORT		OFDM:1;
+	USHORT		MIMO:1;
+#endif
+ } RECEIVE_SETTING, *PRECEIVE_SETTING;
+
+// Shared key data structure
+typedef struct  _WEP_KEY {
+	UCHAR   KeyLen;                     // Key length for each key, 0: entry is invalid
+	UCHAR   Key[MAX_LEN_OF_KEY];        // right now we implement 4 keys, 128 bits max
+} WEP_KEY, *PWEP_KEY;
+
+typedef struct _CIPHER_KEY {
+	UCHAR   Key[16];            // right now we implement 4 keys, 128 bits max
+	UCHAR   RxMic[8];			// make alignment
+	UCHAR   TxMic[8];
+	UCHAR   TxTsc[6];           // 48bit TSC value
+	UCHAR   RxTsc[6];           // 48bit TSC value
+	UCHAR   CipherAlg;          // 0-none, 1:WEP64, 2:WEP128, 3:TKIP, 4:AES, 5:CKIP64, 6:CKIP128
+	UCHAR   KeyLen;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR   BssId[6];
+#endif // CONFIG_STA_SUPPORT //
+            // Key length for each key, 0: entry is invalid
+	UCHAR   Type;               // Indicate Pairwise/Group when reporting MIC error
+} CIPHER_KEY, *PCIPHER_KEY;
+
+typedef struct _BBP_TUNING_STRUCT {
+	BOOLEAN     Enable;
+	UCHAR       FalseCcaCountUpperBound;  // 100 per sec
+	UCHAR       FalseCcaCountLowerBound;  // 10 per sec
+	UCHAR       R17LowerBound;            // specified in E2PROM
+	UCHAR       R17UpperBound;            // 0x68 according to David Tung
+	UCHAR       CurrentR17Value;
+} BBP_TUNING, *PBBP_TUNING;
+
+typedef struct _SOFT_RX_ANT_DIVERSITY_STRUCT {
+	UCHAR     EvaluatePeriod;		 // 0:not evalute status, 1: evaluate status, 2: switching status
+	UCHAR     Pair1PrimaryRxAnt;     // 0:Ant-E1, 1:Ant-E2
+	UCHAR     Pair1SecondaryRxAnt;   // 0:Ant-E1, 1:Ant-E2
+	UCHAR     Pair2PrimaryRxAnt;     // 0:Ant-E3, 1:Ant-E4
+	UCHAR     Pair2SecondaryRxAnt;   // 0:Ant-E3, 1:Ant-E4
+	SHORT     Pair1AvgRssi[2];       // AvgRssi[0]:E1, AvgRssi[1]:E2
+	SHORT     Pair2AvgRssi[2];       // AvgRssi[0]:E3, AvgRssi[1]:E4
+	SHORT     Pair1LastAvgRssi;      //
+	SHORT     Pair2LastAvgRssi;      //
+	ULONG     RcvPktNumWhenEvaluate;
+	BOOLEAN   FirstPktArrivedWhenEvaluate;
+	RALINK_TIMER_STRUCT    RxAntDiversityTimer;
+} SOFT_RX_ANT_DIVERSITY, *PSOFT_RX_ANT_DIVERSITY;
+
+typedef struct _LEAP_AUTH_INFO {
+	BOOLEAN         Enabled;        //Ture: Enable LEAP Authentication
+	BOOLEAN         CCKM;           //Ture: Use Fast Reauthentication with CCKM
+	UCHAR           Reserve[2];
+	UCHAR           UserName[256];  //LEAP, User name
+	ULONG           UserNameLen;
+	UCHAR           Password[256];  //LEAP, User Password
+	ULONG           PasswordLen;
+} LEAP_AUTH_INFO, *PLEAP_AUTH_INFO;
+
+typedef struct {
+	UCHAR        Addr[MAC_ADDR_LEN];
+	UCHAR        ErrorCode[2];  //00 01-Invalid authentication type
+								//00 02-Authentication timeout
+								//00 03-Challenge from AP failed
+								//00 04-Challenge to AP failed
+	BOOLEAN      Reported;
+} ROGUEAP_ENTRY, *PROGUEAP_ENTRY;
+
+typedef struct {
+	UCHAR               RogueApNr;
+	ROGUEAP_ENTRY       RogueApEntry[MAX_LEN_OF_BSS_TABLE];
+} ROGUEAP_TABLE, *PROGUEAP_TABLE;
+
+typedef struct {
+	BOOLEAN     Enable;
+	UCHAR       Delta;
+	BOOLEAN     PlusSign;
+} CCK_TX_POWER_CALIBRATE, *PCCK_TX_POWER_CALIBRATE;
+
+//
+// Receive Tuple Cache Format
+//
+typedef struct  _TUPLE_CACHE    {
+	BOOLEAN         Valid;
+	UCHAR           MacAddress[MAC_ADDR_LEN];
+	USHORT          Sequence;
+	USHORT          Frag;
+} TUPLE_CACHE, *PTUPLE_CACHE;
+
+//
+// Fragment Frame structure
+//
+typedef struct  _FRAGMENT_FRAME {
+	PNDIS_PACKET    pFragPacket;
+	ULONG       RxSize;
+	USHORT      Sequence;
+	USHORT      LastFrag;
+	ULONG       Flags;          // Some extra frame information. bit 0: LLC presented
+} FRAGMENT_FRAME, *PFRAGMENT_FRAME;
+
+
+//
+// Packet information for NdisQueryPacket
+//
+typedef struct  _PACKET_INFO    {
+	UINT            PhysicalBufferCount;    // Physical breaks of buffer descripor chained
+	UINT            BufferCount ;           // Number of Buffer descriptor chained
+	UINT            TotalPacketLength ;     // Self explained
+	PNDIS_BUFFER    pFirstBuffer;           // Pointer to first buffer descriptor
+} PACKET_INFO, *PPACKET_INFO;
+
+//
+// Tkip Key structure which RC4 key & MIC calculation
+//
+typedef struct  _TKIP_KEY_INFO  {
+	UINT        nBytesInM;  // # bytes in M for MICKEY
+	ULONG       IV16;
+	ULONG       IV32;
+	ULONG       K0;         // for MICKEY Low
+	ULONG       K1;         // for MICKEY Hig
+	ULONG       L;          // Current state for MICKEY
+	ULONG       R;          // Current state for MICKEY
+	ULONG       M;          // Message accumulator for MICKEY
+	UCHAR       RC4KEY[16];
+	UCHAR       MIC[8];
+} TKIP_KEY_INFO, *PTKIP_KEY_INFO;
+
+//
+// Private / Misc data, counters for driver internal use
+//
+typedef struct  __PRIVATE_STRUC {
+	UINT       SystemResetCnt;         // System reset counter
+	UINT       TxRingFullCnt;          // Tx ring full occurrance number
+	UINT       PhyRxErrCnt;            // PHY Rx error count, for debug purpose, might move to global counter
+	// Variables for WEP encryption / decryption in rtmp_wep.c
+	UINT       FCSCRC32;
+	ARCFOURCONTEXT  WEPCONTEXT;
+	// Tkip stuff
+	TKIP_KEY_INFO   Tx;
+	TKIP_KEY_INFO   Rx;
+} PRIVATE_STRUC, *PPRIVATE_STRUC;
+
+// structure to tune BBP R66 (BBP TUNING)
+typedef struct _BBP_R66_TUNING {
+	BOOLEAN     bEnable;
+	USHORT      FalseCcaLowerThreshold;  // default 100
+	USHORT      FalseCcaUpperThreshold;  // default 512
+	UCHAR       R66Delta;
+	UCHAR       R66CurrentValue;
+	BOOLEAN		R66LowerUpperSelect; //Before LinkUp, Used LowerBound or UpperBound as R66 value.
+} BBP_R66_TUNING, *PBBP_R66_TUNING;
+
+// structure to store channel TX power
+typedef struct _CHANNEL_TX_POWER {
+	USHORT     RemainingTimeForUse;		//unit: sec
+	UCHAR      Channel;
+#ifdef DOT11N_DRAFT3
+	BOOLEAN       bEffectedChannel;	// For BW 40 operating in 2.4GHz , the "effected channel" is the channel that is covered in 40Mhz.
+#endif // DOT11N_DRAFT3 //
+	CHAR       Power;
+	CHAR       Power2;
+	UCHAR      MaxTxPwr;
+	UCHAR      DfsReq;
+} CHANNEL_TX_POWER, *PCHANNEL_TX_POWER;
+
+// structure to store 802.11j channel TX power
+typedef struct _CHANNEL_11J_TX_POWER {
+	UCHAR      Channel;
+	UCHAR      BW;	// BW_10 or BW_20
+	CHAR       Power;
+	CHAR       Power2;
+	USHORT     RemainingTimeForUse;		//unit: sec
+} CHANNEL_11J_TX_POWER, *PCHANNEL_11J_TX_POWER;
+
+typedef enum _ABGBAND_STATE_ {
+	UNKNOWN_BAND,
+	BG_BAND,
+	A_BAND,
+} ABGBAND_STATE;
+
+typedef struct _MLME_STRUCT {
+#ifdef CONFIG_STA_SUPPORT
+	// STA state machines
+	STATE_MACHINE           CntlMachine;
+	STATE_MACHINE           AssocMachine;
+	STATE_MACHINE           AuthMachine;
+	STATE_MACHINE           AuthRspMachine;
+	STATE_MACHINE           SyncMachine;
+	STATE_MACHINE           WpaPskMachine;
+	STATE_MACHINE           LeapMachine;
+	STATE_MACHINE           AironetMachine;
+	STATE_MACHINE_FUNC      AssocFunc[ASSOC_FUNC_SIZE];
+	STATE_MACHINE_FUNC      AuthFunc[AUTH_FUNC_SIZE];
+	STATE_MACHINE_FUNC      AuthRspFunc[AUTH_RSP_FUNC_SIZE];
+	STATE_MACHINE_FUNC      SyncFunc[SYNC_FUNC_SIZE];
+	STATE_MACHINE_FUNC      WpaPskFunc[WPA_PSK_FUNC_SIZE];
+	STATE_MACHINE_FUNC      AironetFunc[AIRONET_FUNC_SIZE];
+#endif // CONFIG_STA_SUPPORT //
+	STATE_MACHINE_FUNC      ActFunc[ACT_FUNC_SIZE];
+	// Action
+	STATE_MACHINE           ActMachine;
+
+
+#ifdef QOS_DLS_SUPPORT
+	STATE_MACHINE			DlsMachine;
+	STATE_MACHINE_FUNC      DlsFunc[DLS_FUNC_SIZE];
+#endif // QOS_DLS_SUPPORT //
+
+
+
+
+	ULONG                   ChannelQuality;  // 0..100, Channel Quality Indication for Roaming
+	ULONG                   Now32;           // latch the value of NdisGetSystemUpTime()
+	ULONG                   LastSendNULLpsmTime;
+
+	BOOLEAN                 bRunning;
+	NDIS_SPIN_LOCK          TaskLock;
+	MLME_QUEUE              Queue;
+
+	UINT                    ShiftReg;
+
+	RALINK_TIMER_STRUCT     PeriodicTimer;
+	RALINK_TIMER_STRUCT     APSDPeriodicTimer;
+	RALINK_TIMER_STRUCT     LinkDownTimer;
+	RALINK_TIMER_STRUCT     LinkUpTimer;
+	ULONG                   PeriodicRound;
+	ULONG                   OneSecPeriodicRound;
+
+	UCHAR					RealRxPath;
+	BOOLEAN					bLowThroughput;
+	BOOLEAN					bEnableAutoAntennaCheck;
+	RALINK_TIMER_STRUCT		RxAntEvalTimer;
+
+#ifdef RT2870
+	UCHAR CaliBW40RfR24;
+	UCHAR CaliBW20RfR24;
+#endif // RT2870 //
+
+} MLME_STRUCT, *PMLME_STRUCT;
+
+// structure for radar detection and channel switch
+typedef struct _RADAR_DETECT_STRUCT {
+    //BOOLEAN		IEEE80211H;			// 0: disable, 1: enable IEEE802.11h
+	UCHAR		CSCount;			//Channel switch counter
+	UCHAR		CSPeriod;			//Channel switch period (beacon count)
+	UCHAR		RDCount;			//Radar detection counter
+	UCHAR		RDMode;				//Radar Detection mode
+	UCHAR		RDDurRegion;		//Radar detection duration region
+	UCHAR		BBPR16;
+	UCHAR		BBPR17;
+	UCHAR		BBPR18;
+	UCHAR		BBPR21;
+	UCHAR		BBPR22;
+	UCHAR		BBPR64;
+	ULONG		InServiceMonitorCount; // unit: sec
+	UINT8		DfsSessionTime;
+	BOOLEAN		bFastDfs;
+	UINT8		ChMovingTime;
+	UINT8		LongPulseRadarTh;
+} RADAR_DETECT_STRUCT, *PRADAR_DETECT_STRUCT;
+
+#ifdef CARRIER_DETECTION_SUPPORT
+typedef enum CD_STATE_n
+{
+	CD_NORMAL,
+	CD_SILENCE,
+	CD_MAX_STATE
+} CD_STATE;
+
+typedef struct CARRIER_DETECTION_s
+{
+	BOOLEAN					Enable;
+	UINT8					CDSessionTime;
+	UINT8					CDPeriod;
+	CD_STATE				CD_State;
+} CARRIER_DETECTION, *PCARRIER_DETECTION;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+typedef enum _REC_BLOCKACK_STATUS
+{
+    Recipient_NONE=0,
+	Recipient_USED,
+	Recipient_HandleRes,
+    Recipient_Accept
+} REC_BLOCKACK_STATUS, *PREC_BLOCKACK_STATUS;
+
+typedef enum _ORI_BLOCKACK_STATUS
+{
+    Originator_NONE=0,
+	Originator_USED,
+    Originator_WaitRes,
+    Originator_Done
+} ORI_BLOCKACK_STATUS, *PORI_BLOCKACK_STATUS;
+
+#ifdef DOT11_N_SUPPORT
+typedef struct _BA_ORI_ENTRY{
+	UCHAR   Wcid;
+	UCHAR   TID;
+	UCHAR   BAWinSize;
+	UCHAR   Token;
+// Sequence is to fill every outgoing QoS DATA frame's sequence field in 802.11 header.
+	USHORT	Sequence;
+	USHORT	TimeOutValue;
+	ORI_BLOCKACK_STATUS  ORI_BA_Status;
+	RALINK_TIMER_STRUCT ORIBATimer;
+	PVOID	pAdapter;
+} BA_ORI_ENTRY, *PBA_ORI_ENTRY;
+
+typedef struct _BA_REC_ENTRY {
+	UCHAR   Wcid;
+	UCHAR   TID;
+	UCHAR   BAWinSize;	// 7.3.1.14. each buffer is capable of holding a max AMSDU or MSDU.
+	//UCHAR	NumOfRxPkt;
+	//UCHAR    Curindidx; // the head in the RX reordering buffer
+	USHORT		LastIndSeq;
+//	USHORT		LastIndSeqAtTimer;
+	USHORT		TimeOutValue;
+	RALINK_TIMER_STRUCT RECBATimer;
+	ULONG		LastIndSeqAtTimer;
+	ULONG		nDropPacket;
+	ULONG		rcvSeq;
+	REC_BLOCKACK_STATUS  REC_BA_Status;
+//	UCHAR	RxBufIdxUsed;
+	// corresponding virtual address for RX reordering packet storage.
+	//RTMP_REORDERDMABUF MAP_RXBuf[MAX_RX_REORDERBUF];
+	NDIS_SPIN_LOCK          RxReRingLock;                 // Rx Ring spinlock
+//	struct _BA_REC_ENTRY *pNext;
+	PVOID	pAdapter;
+	struct reordering_list	list;
+} BA_REC_ENTRY, *PBA_REC_ENTRY;
+
+
+typedef struct {
+	ULONG		numAsRecipient;		// I am recipient of numAsRecipient clients. These client are in the BARecEntry[]
+	ULONG		numAsOriginator;	// I am originator of 	numAsOriginator clients. These clients are in the BAOriEntry[]
+	BA_ORI_ENTRY       BAOriEntry[MAX_LEN_OF_BA_ORI_TABLE];
+	BA_REC_ENTRY       BARecEntry[MAX_LEN_OF_BA_REC_TABLE];
+} BA_TABLE, *PBA_TABLE;
+
+//For QureyBATableOID use;
+typedef struct  PACKED _OID_BA_REC_ENTRY{
+	UCHAR   MACAddr[MAC_ADDR_LEN];
+	UCHAR   BaBitmap;   // if (BaBitmap&(1<<TID)), this session with{MACAddr, TID}exists, so read BufSize[TID] for BufferSize
+	UCHAR   rsv;
+	UCHAR   BufSize[8];
+	REC_BLOCKACK_STATUS	REC_BA_Status[8];
+} OID_BA_REC_ENTRY, *POID_BA_REC_ENTRY;
+
+//For QureyBATableOID use;
+typedef struct  PACKED _OID_BA_ORI_ENTRY{
+	UCHAR   MACAddr[MAC_ADDR_LEN];
+	UCHAR   BaBitmap;  // if (BaBitmap&(1<<TID)), this session with{MACAddr, TID}exists, so read BufSize[TID] for BufferSize, read ORI_BA_Status[TID] for status
+	UCHAR   rsv;
+	UCHAR   BufSize[8];
+	ORI_BLOCKACK_STATUS  ORI_BA_Status[8];
+} OID_BA_ORI_ENTRY, *POID_BA_ORI_ENTRY;
+
+typedef struct _QUERYBA_TABLE{
+	OID_BA_ORI_ENTRY       BAOriEntry[32];
+	OID_BA_REC_ENTRY       BARecEntry[32];
+	UCHAR   OriNum;// Number of below BAOriEntry
+	UCHAR   RecNum;// Number of below BARecEntry
+} QUERYBA_TABLE, *PQUERYBA_TABLE;
+
+typedef	union	_BACAP_STRUC	{
+#ifdef RT_BIG_ENDIAN
+	struct	{
+		UINT32     :4;
+		UINT32     b2040CoexistScanSup:1;		//As Sta, support do 2040 coexistence scan for AP. As Ap, support monitor trigger event to check if can use BW 40MHz.
+		UINT32     bHtAdhoc:1;			// adhoc can use ht rate.
+		UINT32     MMPSmode:2;	// MIMO power save more, 0:static, 1:dynamic, 2:rsv, 3:mimo enable
+		UINT32     AmsduSize:1;	// 0:3839, 1:7935 bytes. UINT  MSDUSizeToBytes[]	= { 3839, 7935};
+		UINT32     AmsduEnable:1;	//Enable AMSDU transmisstion
+		UINT32		MpduDensity:3;
+		UINT32		Policy:2;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use
+		UINT32		AutoBA:1;	// automatically BA
+		UINT32		TxBAWinLimit:8;
+		UINT32		RxBAWinLimit:8;
+	}	field;
+#else
+	struct	{
+		UINT32		RxBAWinLimit:8;
+		UINT32		TxBAWinLimit:8;
+		UINT32		AutoBA:1;	// automatically BA
+		UINT32		Policy:2;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use
+		UINT32		MpduDensity:3;
+		UINT32       	AmsduEnable:1;	//Enable AMSDU transmisstion
+		UINT32       	AmsduSize:1;	// 0:3839, 1:7935 bytes. UINT  MSDUSizeToBytes[]	= { 3839, 7935};
+		UINT32       	MMPSmode:2;	// MIMO power save more, 0:static, 1:dynamic, 2:rsv, 3:mimo enable
+		UINT32       	bHtAdhoc:1;			// adhoc can use ht rate.
+		UINT32       	b2040CoexistScanSup:1;		//As Sta, support do 2040 coexistence scan for AP. As Ap, support monitor trigger event to check if can use BW 40MHz.
+		UINT32       	:4;
+	}	field;
+#endif
+	UINT32			word;
+} BACAP_STRUC, *PBACAP_STRUC;
+#endif // DOT11_N_SUPPORT //
+
+//This structure is for all 802.11n card InterOptibilityTest action. Reset all Num every n second.  (Details see MLMEPeriodic)
+typedef	struct	_IOT_STRUC	{
+	UCHAR			Threshold[2];
+	UCHAR			ReorderTimeOutNum[MAX_LEN_OF_BA_REC_TABLE];	// compare with threshold[0]
+	UCHAR			RefreshNum[MAX_LEN_OF_BA_REC_TABLE];	// compare with threshold[1]
+	ULONG			OneSecInWindowCount;
+	ULONG			OneSecFrameDuplicateCount;
+	ULONG			OneSecOutWindowCount;
+	UCHAR			DelOriAct;
+	UCHAR			DelRecAct;
+	UCHAR			RTSShortProt;
+	UCHAR			RTSLongProt;
+	BOOLEAN			bRTSLongProtOn;
+#ifdef CONFIG_STA_SUPPORT
+	BOOLEAN			bLastAtheros;
+    BOOLEAN			bCurrentAtheros;
+    BOOLEAN         bNowAtherosBurstOn;
+	BOOLEAN			bNextDisableRxBA;
+    BOOLEAN			bToggle;
+#endif // CONFIG_STA_SUPPORT //
+} IOT_STRUC, *PIOT_STRUC;
+
+// This is the registry setting for 802.11n transmit setting.  Used in advanced page.
+typedef union _REG_TRANSMIT_SETTING {
+#ifdef RT_BIG_ENDIAN
+ struct {
+         UINT32  rsv:13;
+		 UINT32  EXTCHA:2;
+		 UINT32  HTMODE:1;
+		 UINT32  TRANSNO:2;
+		 UINT32  STBC:1; //SPACE
+		 UINT32  ShortGI:1;
+		 UINT32  BW:1; //channel bandwidth 20MHz or 40 MHz
+		 UINT32  TxBF:1; // 3*3
+		 UINT32  rsv0:10;
+		 //UINT32  MCS:7;                 // MCS
+         //UINT32  PhyMode:4;
+    } field;
+#else
+ struct {
+         //UINT32  PhyMode:4;
+         //UINT32  MCS:7;                 // MCS
+		 UINT32  rsv0:10;
+		 UINT32  TxBF:1;
+         UINT32  BW:1; //channel bandwidth 20MHz or 40 MHz
+         UINT32  ShortGI:1;
+         UINT32  STBC:1; //SPACE
+         UINT32  TRANSNO:2;
+         UINT32  HTMODE:1;
+         UINT32  EXTCHA:2;
+         UINT32  rsv:13;
+    } field;
+#endif
+ UINT32   word;
+} REG_TRANSMIT_SETTING, *PREG_TRANSMIT_SETTING;
+
+typedef union  _DESIRED_TRANSMIT_SETTING {
+#ifdef RT_BIG_ENDIAN
+	struct	{
+			USHORT		rsv:3;
+			USHORT		FixedTxMode:2;			// If MCS isn't AUTO, fix rate in CCK, OFDM or HT mode.
+			USHORT		PhyMode:4;
+			USHORT   	MCS:7;                 // MCS
+	}	field;
+#else
+	struct	{
+			USHORT   	MCS:7;                 	// MCS
+			USHORT		PhyMode:4;
+			USHORT	 	FixedTxMode:2;			// If MCS isn't AUTO, fix rate in CCK, OFDM or HT mode.
+			USHORT		rsv:3;
+	}	field;
+#endif
+	USHORT		word;
+ } DESIRED_TRANSMIT_SETTING, *PDESIRED_TRANSMIT_SETTING;
+
+typedef struct {
+	BOOLEAN		IsRecipient;
+	UCHAR   MACAddr[MAC_ADDR_LEN];
+	UCHAR   TID;
+	UCHAR   nMSDU;
+	USHORT   TimeOut;
+	BOOLEAN bAllTid;  // If True, delete all TID for BA sessions with this MACaddr.
+} OID_ADD_BA_ENTRY, *POID_ADD_BA_ENTRY;
+
+//
+// Multiple SSID structure
+//
+#define WLAN_MAX_NUM_OF_TIM			((MAX_LEN_OF_MAC_TABLE >> 3) + 1) /* /8 + 1 */
+#define WLAN_CT_TIM_BCMC_OFFSET		0 /* unit: 32B */
+
+/* clear bcmc TIM bit */
+#define WLAN_MR_TIM_BCMC_CLEAR(apidx) \
+	pAd->ApCfg.MBSSID[apidx].TimBitmaps[WLAN_CT_TIM_BCMC_OFFSET] &= ~BIT8[0];
+
+/* set bcmc TIM bit */
+#define WLAN_MR_TIM_BCMC_SET(apidx) \
+	pAd->ApCfg.MBSSID[apidx].TimBitmaps[WLAN_CT_TIM_BCMC_OFFSET] |= BIT8[0];
+
+/* clear a station PS TIM bit */
+#define WLAN_MR_TIM_BIT_CLEAR(ad_p, apidx, wcid) \
+	{	UCHAR tim_offset = wcid >> 3; \
+		UCHAR bit_offset = wcid & 0x7; \
+		ad_p->ApCfg.MBSSID[apidx].TimBitmaps[tim_offset] &= (~BIT8[bit_offset]); }
+
+/* set a station PS TIM bit */
+#define WLAN_MR_TIM_BIT_SET(ad_p, apidx, wcid) \
+	{	UCHAR tim_offset = wcid >> 3; \
+		UCHAR bit_offset = wcid & 0x7; \
+		ad_p->ApCfg.MBSSID[apidx].TimBitmaps[tim_offset] |= BIT8[bit_offset]; }
+
+#ifdef RT2870
+#define BEACON_BITMAP_MASK		0xff
+typedef struct _BEACON_SYNC_STRUCT_
+{
+	UCHAR        			BeaconBuf[HW_BEACON_MAX_COUNT][HW_BEACON_OFFSET];
+	UCHAR					BeaconTxWI[HW_BEACON_MAX_COUNT][TXWI_SIZE];
+	ULONG 					TimIELocationInBeacon[HW_BEACON_MAX_COUNT];
+	ULONG					CapabilityInfoLocationInBeacon[HW_BEACON_MAX_COUNT];
+	BOOLEAN					EnableBeacon;		// trigger to enable beacon transmission.
+	UCHAR					BeaconBitMap;		// NOTE: If the MAX_MBSSID_NUM is larger than 8, this parameter need to change.
+	UCHAR					DtimBitOn;			// NOTE: If the MAX_MBSSID_NUM is larger than 8, this parameter need to change.
+}BEACON_SYNC_STRUCT;
+#endif // RT2870 //
+
+typedef struct _MULTISSID_STRUCT {
+	UCHAR								Bssid[MAC_ADDR_LEN];
+    UCHAR                               SsidLen;
+    CHAR                                Ssid[MAX_LEN_OF_SSID];
+    USHORT                              CapabilityInfo;
+
+    PNET_DEV                   			MSSIDDev;
+
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;
+	NDIS_802_11_WEP_STATUS              WepStatus;
+	NDIS_802_11_WEP_STATUS				GroupKeyWepStatus;
+	WPA_MIX_PAIR_CIPHER					WpaMixPairCipher;
+
+	ULONG								TxCount;
+	ULONG								RxCount;
+	ULONG								ReceivedByteCount;
+	ULONG								TransmittedByteCount;
+	ULONG								RxErrorCount;
+	ULONG								RxDropCount;
+
+	HTTRANSMIT_SETTING					HTPhyMode, MaxHTPhyMode, MinHTPhyMode;// For transmit phy setting in TXWI.
+	RT_HT_PHY_INFO						DesiredHtPhyInfo;
+	DESIRED_TRANSMIT_SETTING        	DesiredTransmitSetting; // Desired transmit setting. this is for reading registry setting only. not useful.
+	BOOLEAN								bAutoTxRateSwitch;
+
+	//CIPHER_KEY                          SharedKey[SHARE_KEY_NUM]; // ref pAd->SharedKey[BSS][4]
+	UCHAR                               DefaultKeyId;
+
+	UCHAR								TxRate;       // RATE_1, RATE_2, RATE_5_5, RATE_11, ...
+	UCHAR     							DesiredRates[MAX_LEN_OF_SUPPORTED_RATES];// OID_802_11_DESIRED_RATES
+	UCHAR								DesiredRatesIndex;
+	UCHAR     							MaxTxRate;            // RATE_1, RATE_2, RATE_5_5, RATE_11
+
+//	ULONG           					TimBitmap;      // bit0 for broadcast, 1 for AID1, 2 for AID2, ...so on
+//    ULONG           					TimBitmap2;     // b0 for AID32, b1 for AID33, ... and so on
+	UCHAR								TimBitmaps[WLAN_MAX_NUM_OF_TIM];
+
+    // WPA
+    UCHAR                               GMK[32];
+    UCHAR                               PMK[32];
+	UCHAR								GTK[32];
+    BOOLEAN                             IEEE8021X;
+    BOOLEAN                             PreAuth;
+    UCHAR                               GNonce[32];
+    UCHAR                               PortSecured;
+    NDIS_802_11_PRIVACY_FILTER          PrivacyFilter;
+    UCHAR                               BANClass3Data;
+    ULONG                               IsolateInterStaTraffic;
+
+    UCHAR                               RSNIE_Len[2];
+    UCHAR                               RSN_IE[2][MAX_LEN_OF_RSNIE];
+
+
+    UCHAR                   			TimIELocationInBeacon;
+    UCHAR                   			CapabilityInfoLocationInBeacon;
+    // outgoing BEACON frame buffer and corresponding TXWI
+	// PTXWI_STRUC                           BeaconTxWI; //
+    CHAR                                BeaconBuf[MAX_BEACON_SIZE]; // NOTE: BeaconBuf should be 4-byte aligned
+
+    BOOLEAN                             bHideSsid;
+	UINT16								StationKeepAliveTime; // unit: second
+
+    USHORT                              VLAN_VID;
+    USHORT                              VLAN_Priority;
+
+    RT_802_11_ACL						AccessControlList;
+
+	// EDCA Qos
+    BOOLEAN								bWmmCapable;	// 0:disable WMM, 1:enable WMM
+    BOOLEAN								bDLSCapable;	// 0:disable DLS, 1:enable DLS
+
+	UCHAR           					DlsPTK[64];		// Due to windows dirver count on meetinghouse to handle 4-way shake
+
+	// For 802.1x daemon setting per BSS
+	UCHAR								radius_srv_num;
+	RADIUS_SRV_INFO						radius_srv_info[MAX_RADIUS_SRV_NUM];
+
+#ifdef RTL865X_SOC
+	unsigned int						mylinkid;
+#endif
+
+
+	UINT32					RcvdConflictSsidCount;
+	UINT32					RcvdSpoofedAssocRespCount;
+	UINT32					RcvdSpoofedReassocRespCount;
+	UINT32					RcvdSpoofedProbeRespCount;
+	UINT32					RcvdSpoofedBeaconCount;
+	UINT32					RcvdSpoofedDisassocCount;
+	UINT32					RcvdSpoofedAuthCount;
+	UINT32					RcvdSpoofedDeauthCount;
+	UINT32					RcvdSpoofedUnknownMgmtCount;
+	UINT32					RcvdReplayAttackCount;
+
+	CHAR					RssiOfRcvdConflictSsid;
+	CHAR					RssiOfRcvdSpoofedAssocResp;
+	CHAR					RssiOfRcvdSpoofedReassocResp;
+	CHAR					RssiOfRcvdSpoofedProbeResp;
+	CHAR					RssiOfRcvdSpoofedBeacon;
+	CHAR					RssiOfRcvdSpoofedDisassoc;
+	CHAR					RssiOfRcvdSpoofedAuth;
+	CHAR					RssiOfRcvdSpoofedDeauth;
+	CHAR					RssiOfRcvdSpoofedUnknownMgmt;
+	CHAR					RssiOfRcvdReplayAttack;
+
+	BOOLEAN					bBcnSntReq;
+	UCHAR					BcnBufIdx;
+} MULTISSID_STRUCT, *PMULTISSID_STRUCT;
+
+
+
+#ifdef DOT11N_DRAFT3
+typedef enum _BSS2040COEXIST_FLAG{
+	BSS_2040_COEXIST_DISABLE = 0,
+	BSS_2040_COEXIST_TIMER_FIRED  = 1,
+	BSS_2040_COEXIST_INFO_SYNC = 2,
+	BSS_2040_COEXIST_INFO_NOTIFY = 4,
+}BSS2040COEXIST_FLAG;
+#endif // DOT11N_DRAFT3 //
+
+// configuration common to OPMODE_AP as well as OPMODE_STA
+typedef struct _COMMON_CONFIG {
+
+	BOOLEAN		bCountryFlag;
+	UCHAR		CountryCode[3];
+	UCHAR		Geography;
+	UCHAR       CountryRegion;      // Enum of country region, 0:FCC, 1:IC, 2:ETSI, 3:SPAIN, 4:France, 5:MKK, 6:MKK1, 7:Israel
+	UCHAR       CountryRegionForABand;	// Enum of country region for A band
+	UCHAR       PhyMode;            // PHY_11A, PHY_11B, PHY_11BG_MIXED, PHY_ABG_MIXED
+	USHORT      Dsifs;              // in units of usec
+	ULONG       PacketFilter;       // Packet filter for receiving
+
+	CHAR        Ssid[MAX_LEN_OF_SSID]; // NOT NULL-terminated
+	UCHAR       SsidLen;               // the actual ssid length in used
+	UCHAR       LastSsidLen;               // the actual ssid length in used
+	CHAR        LastSsid[MAX_LEN_OF_SSID]; // NOT NULL-terminated
+	UCHAR		LastBssid[MAC_ADDR_LEN];
+
+	UCHAR       Bssid[MAC_ADDR_LEN];
+	USHORT      BeaconPeriod;
+	UCHAR       Channel;
+	UCHAR       CentralChannel;    	// Central Channel when using 40MHz is indicating. not real channel.
+
+#if 0	// move to STA_ADMIN_CONFIG
+	UCHAR       DefaultKeyId;
+
+	NDIS_802_11_PRIVACY_FILTER          PrivacyFilter;  // PrivacyFilter enum for 802.1X
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;       // This should match to whatever microsoft defined
+	NDIS_802_11_WEP_STATUS              WepStatus;
+	NDIS_802_11_WEP_STATUS				OrigWepStatus;	// Original wep status set from OID
+
+	// Add to support different cipher suite for WPA2/WPA mode
+	NDIS_802_11_ENCRYPTION_STATUS		GroupCipher;		// Multicast cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS		PairCipher;			// Unicast cipher suite
+	BOOLEAN								bMixCipher;			// Indicate current Pair & Group use different cipher suites
+	USHORT								RsnCapability;
+
+	NDIS_802_11_WEP_STATUS              GroupKeyWepStatus;
+#endif
+
+	UCHAR       SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       SupRateLen;
+	UCHAR       ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       ExtRateLen;
+	UCHAR       DesireRate[MAX_LEN_OF_SUPPORTED_RATES];      // OID_802_11_DESIRED_RATES
+	UCHAR       MaxDesiredRate;
+	UCHAR       ExpectedACKRate[MAX_LEN_OF_SUPPORTED_RATES];
+
+	ULONG       BasicRateBitmap;        // backup basic ratebitmap
+
+	BOOLEAN		bAPSDCapable;
+	BOOLEAN		bInServicePeriod;
+	BOOLEAN		bAPSDAC_BE;
+	BOOLEAN		bAPSDAC_BK;
+	BOOLEAN		bAPSDAC_VI;
+	BOOLEAN		bAPSDAC_VO;
+	BOOLEAN		bNeedSendTriggerFrame;
+	BOOLEAN		bAPSDForcePowerSave;	// Force power save mode, should only use in APSD-STAUT
+	ULONG		TriggerTimerCount;
+	UCHAR		MaxSPLength;
+	UCHAR		BBPCurrentBW;	// BW_10, 	BW_20, BW_40
+	// move to MULTISSID_STRUCT for MBSS
+	//HTTRANSMIT_SETTING	HTPhyMode, MaxHTPhyMode, MinHTPhyMode;// For transmit phy setting in TXWI.
+	REG_TRANSMIT_SETTING        RegTransmitSetting; //registry transmit setting. this is for reading registry setting only. not useful.
+	//UCHAR       FixedTxMode;              // Fixed Tx Mode (CCK, OFDM), for HT fixed tx mode (GF, MIX) , refer to RegTransmitSetting.field.HTMode
+	UCHAR       TxRate;                 // Same value to fill in TXD. TxRate is 6-bit
+	UCHAR       MaxTxRate;              // RATE_1, RATE_2, RATE_5_5, RATE_11
+	UCHAR       TxRateIndex;            // Tx rate index in RateSwitchTable
+	UCHAR       TxRateTableSize;        // Valid Tx rate table size in RateSwitchTable
+	//BOOLEAN		bAutoTxRateSwitch;
+	UCHAR       MinTxRate;              // RATE_1, RATE_2, RATE_5_5, RATE_11
+	UCHAR       RtsRate;                // RATE_xxx
+	HTTRANSMIT_SETTING	MlmeTransmit;   // MGMT frame PHY rate setting when operatin at Ht rate.
+	UCHAR       MlmeRate;               // RATE_xxx, used to send MLME frames
+	UCHAR       BasicMlmeRate;          // Default Rate for sending MLME frames
+
+	USHORT      RtsThreshold;           // in unit of BYTE
+	USHORT      FragmentThreshold;      // in unit of BYTE
+
+	UCHAR       TxPower;                // in unit of mW
+	ULONG       TxPowerPercentage;      // 0~100 %
+	ULONG       TxPowerDefault;         // keep for TxPowerPercentage
+
+#ifdef DOT11_N_SUPPORT
+	BACAP_STRUC        BACapability; //   NO USE = 0XFF  ;  IMMED_BA =1  ;  DELAY_BA=0
+	BACAP_STRUC        REGBACapability; //   NO USE = 0XFF  ;  IMMED_BA =1  ;  DELAY_BA=0
+#endif // DOT11_N_SUPPORT //
+	IOT_STRUC		IOTestParm;	// 802.11n InterOpbility Test Parameter;
+	ULONG       TxPreamble;             // Rt802_11PreambleLong, Rt802_11PreambleShort, Rt802_11PreambleAuto
+	BOOLEAN     bUseZeroToDisableFragment;     // Microsoft use 0 as disable
+	ULONG       UseBGProtection;        // 0: auto, 1: always use, 2: always not use
+	BOOLEAN     bUseShortSlotTime;      // 0: disable, 1 - use short slot (9us)
+	BOOLEAN     bEnableTxBurst;         // 1: enble TX PACKET BURST, 0: disable TX PACKET BURST
+	BOOLEAN     bAggregationCapable;      // 1: enable TX aggregation when the peer supports it
+	BOOLEAN     bPiggyBackCapable;		// 1: enable TX piggy-back according MAC's version
+	BOOLEAN     bIEEE80211H;			// 1: enable IEEE802.11h spec.
+	ULONG		DisableOLBCDetect;		// 0: enable OLBC detect; 1 disable OLBC detect
+
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN				bRdg;
+#endif // DOT11_N_SUPPORT //
+	BOOLEAN             bWmmCapable;        // 0:disable WMM, 1:enable WMM
+	QOS_CAPABILITY_PARM APQosCapability;    // QOS capability of the current associated AP
+	EDCA_PARM           APEdcaParm;         // EDCA parameters of the current associated AP
+	QBSS_LOAD_PARM      APQbssLoad;         // QBSS load of the current associated AP
+	UCHAR               AckPolicy[4];       // ACK policy of the specified AC. see ACK_xxx
+#ifdef CONFIG_STA_SUPPORT
+	BOOLEAN				bDLSCapable;		// 0:disable DLS, 1:enable DLS
+#endif // CONFIG_STA_SUPPORT //
+	// a bitmap of BOOLEAN flags. each bit represent an operation status of a particular
+	// BOOLEAN control, either ON or OFF. These flags should always be accessed via
+	// OPSTATUS_TEST_FLAG(), OPSTATUS_SET_FLAG(), OP_STATUS_CLEAR_FLAG() macros.
+	// see fOP_STATUS_xxx in RTMP_DEF.C for detail bit definition
+	ULONG               OpStatusFlags;
+
+	BOOLEAN				NdisRadioStateOff; //For HCT 12.0, set this flag to TRUE instead of called MlmeRadioOff.
+	ABGBAND_STATE		BandState;		// For setting BBP used on B/G or A mode.
+
+	// IEEE802.11H--DFS.
+	RADAR_DETECT_STRUCT	RadarDetect;
+
+#ifdef CARRIER_DETECTION_SUPPORT
+	CARRIER_DETECTION		CarrierDetect;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	// HT
+	UCHAR			BASize;		// USer desired BAWindowSize. Should not exceed our max capability
+	//RT_HT_CAPABILITY	SupportedHtPhy;
+	RT_HT_CAPABILITY	DesiredHtPhy;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHTInfo;	// Useful as AP.
+	//This IE is used with channel switch announcement element when changing to a new 40MHz.
+	//This IE is included in channel switch ammouncement frames 7.4.1.5, beacons, probe Rsp.
+	NEW_EXT_CHAN_IE	NewExtChanOffset;	//7.3.2.20A, 1 if extension channel is above the control channel, 3 if below, 0 if not present
+
+#ifdef DOT11N_DRAFT3
+	UCHAR					Bss2040CoexistFlag;		// bit 0: bBssCoexistTimerRunning, bit 1: NeedSyncAddHtInfo.
+	RALINK_TIMER_STRUCT	Bss2040CoexistTimer;
+
+	//This IE is used for 20/40 BSS Coexistence.
+	BSS_2040_COEXIST_IE		BSS2040CoexistInfo;
+	// ====== 11n D3.0 =======================>
+	USHORT					Dot11OBssScanPassiveDwell;				// Unit : TU. 5~1000
+	USHORT					Dot11OBssScanActiveDwell;				// Unit : TU. 10~1000
+	USHORT					Dot11BssWidthTriggerScanInt;			// Unit : Second
+	USHORT					Dot11OBssScanPassiveTotalPerChannel;	// Unit : TU. 200~10000
+	USHORT					Dot11OBssScanActiveTotalPerChannel;	// Unit : TU. 20~10000
+	USHORT					Dot11BssWidthChanTranDelayFactor;
+	USHORT					Dot11OBssScanActivityThre;				// Unit : percentage
+
+	ULONG					Dot11BssWidthChanTranDelay;			// multiple of (Dot11BssWidthTriggerScanInt * Dot11BssWidthChanTranDelayFactor)
+	ULONG					CountDownCtr;	// CountDown Counter from (Dot11BssWidthTriggerScanInt * Dot11BssWidthChanTranDelayFactor)
+
+	NDIS_SPIN_LOCK          TriggerEventTabLock;
+	BSS_2040_COEXIST_IE		LastBSSCoexist2040;
+	BSS_2040_COEXIST_IE		BSSCoexist2040;
+	TRIGGER_EVENT_TAB		TriggerEventTab;
+	UCHAR					ChannelListIdx;
+	// <====== 11n D3.0 =======================
+	BOOLEAN					bOverlapScanning;
+#endif // DOT11N_DRAFT3 //
+
+    BOOLEAN                 bHTProtect;
+    BOOLEAN                 bMIMOPSEnable;
+    BOOLEAN					bBADecline;
+	BOOLEAN					bDisableReordering;
+	BOOLEAN					bForty_Mhz_Intolerant;
+	BOOLEAN					bExtChannelSwitchAnnouncement;
+	BOOLEAN					bRcvBSSWidthTriggerEvents;
+	ULONG					LastRcvBSSWidthTriggerEventsTime;
+
+	UCHAR					TxBASize;
+#endif // DOT11_N_SUPPORT //
+
+	// Enable wireless event
+	BOOLEAN				bWirelessEvent;
+	BOOLEAN				bWiFiTest;				// Enable this parameter for WiFi test
+
+	// Tx & Rx Stream number selection
+	UCHAR				TxStream;
+	UCHAR				RxStream;
+
+	// transmit phy mode, trasmit rate for Multicast.
+#ifdef MCAST_RATE_SPECIFIC
+	UCHAR				McastTransmitMcs;
+	UCHAR				McastTransmitPhyMode;
+#endif // MCAST_RATE_SPECIFIC //
+
+	BOOLEAN     		bHardwareRadio;     // Hardware controlled Radio enabled
+
+#ifdef RT2870
+	BOOLEAN     		bMultipleIRP;       // Multiple Bulk IN flag
+	UCHAR       		NumOfBulkInIRP;     // if bMultipleIRP == TRUE, NumOfBulkInIRP will be 4 otherwise be 1
+ 	RT_HT_CAPABILITY	SupportedHtPhy;
+	ULONG				MaxPktOneTxBulk;
+	UCHAR				TxBulkFactor;
+	UCHAR				RxBulkFactor;
+
+	BEACON_SYNC_STRUCT	*pBeaconSync;
+	RALINK_TIMER_STRUCT	BeaconUpdateTimer;
+	UINT32				BeaconAdjust;
+	UINT32				BeaconFactor;
+	UINT32				BeaconRemain;
+#endif // RT2870 //
+
+
+ 	NDIS_SPIN_LOCK			MeasureReqTabLock;
+	PMEASURE_REQ_TAB		pMeasureReqTab;
+
+	NDIS_SPIN_LOCK			TpcReqTabLock;
+	PTPC_REQ_TAB			pTpcReqTab;
+
+	// transmit phy mode, trasmit rate for Multicast.
+#ifdef MCAST_RATE_SPECIFIC
+	HTTRANSMIT_SETTING		MCastPhyMode;
+#endif // MCAST_RATE_SPECIFIC //
+
+#ifdef SINGLE_SKU
+	UINT16					DefineMaxTxPwr;
+#endif // SINGLE_SKU //
+
+
+} COMMON_CONFIG, *PCOMMON_CONFIG;
+
+
+#ifdef CONFIG_STA_SUPPORT
+/* Modified by Wu Xi-Kun 4/21/2006 */
+// STA configuration and status
+typedef struct _STA_ADMIN_CONFIG {
+	// GROUP 1 -
+	//   User configuration loaded from Registry, E2PROM or OID_xxx. These settings describe
+	//   the user intended configuration, but not necessary fully equal to the final
+	//   settings in ACTIVE BSS after negotiation/compromize with the BSS holder (either
+	//   AP or IBSS holder).
+	//   Once initialized, user configuration can only be changed via OID_xxx
+	UCHAR       BssType;              // BSS_INFRA or BSS_ADHOC
+	USHORT      AtimWin;          // used when starting a new IBSS
+
+	// GROUP 2 -
+	//   User configuration loaded from Registry, E2PROM or OID_xxx. These settings describe
+	//   the user intended configuration, and should be always applied to the final
+	//   settings in ACTIVE BSS without compromising with the BSS holder.
+	//   Once initialized, user configuration can only be changed via OID_xxx
+	UCHAR       RssiTrigger;
+	UCHAR       RssiTriggerMode;      // RSSI_TRIGGERED_UPON_BELOW_THRESHOLD or RSSI_TRIGGERED_UPON_EXCCEED_THRESHOLD
+	USHORT      DefaultListenCount;   // default listen count;
+	ULONG       WindowsPowerMode;           // Power mode for AC power
+	ULONG       WindowsBatteryPowerMode;    // Power mode for battery if exists
+	BOOLEAN     bWindowsACCAMEnable;        // Enable CAM power mode when AC on
+	BOOLEAN     bAutoReconnect;         // Set to TRUE when setting OID_802_11_SSID with no matching BSSID
+	ULONG       WindowsPowerProfile;    // Windows power profile, for NDIS5.1 PnP
+
+	// MIB:ieee802dot11.dot11smt(1).dot11StationConfigTable(1)
+	USHORT      Psm;                  // power management mode   (PWR_ACTIVE|PWR_SAVE)
+	USHORT      DisassocReason;
+	UCHAR       DisassocSta[MAC_ADDR_LEN];
+	USHORT      DeauthReason;
+	UCHAR       DeauthSta[MAC_ADDR_LEN];
+	USHORT      AuthFailReason;
+	UCHAR       AuthFailSta[MAC_ADDR_LEN];
+
+	NDIS_802_11_PRIVACY_FILTER          PrivacyFilter;  // PrivacyFilter enum for 802.1X
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;       // This should match to whatever microsoft defined
+	NDIS_802_11_WEP_STATUS              WepStatus;
+	NDIS_802_11_WEP_STATUS				OrigWepStatus;	// Original wep status set from OID
+
+	// Add to support different cipher suite for WPA2/WPA mode
+	NDIS_802_11_ENCRYPTION_STATUS		GroupCipher;		// Multicast cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS		PairCipher;			// Unicast cipher suite
+	BOOLEAN								bMixCipher;			// Indicate current Pair & Group use different cipher suites
+	USHORT								RsnCapability;
+
+	NDIS_802_11_WEP_STATUS              GroupKeyWepStatus;
+
+	UCHAR		PMK[32];                // WPA PSK mode PMK
+	UCHAR       PTK[64];                // WPA PSK mode PTK
+	UCHAR		GTK[32];				// GTK from authenticator
+	BSSID_INFO	SavedPMK[PMKID_NO];
+	UINT		SavedPMKNum;			// Saved PMKID number
+
+	UCHAR		DefaultKeyId;
+
+
+	// WPA 802.1x port control, WPA_802_1X_PORT_SECURED, WPA_802_1X_PORT_NOT_SECURED
+	UCHAR       PortSecured;
+
+	// For WPA countermeasures
+	ULONG       LastMicErrorTime;   // record last MIC error time
+	ULONG       MicErrCnt;          // Should be 0, 1, 2, then reset to zero (after disassoiciation).
+	BOOLEAN     bBlockAssoc;        // Block associate attempt for 60 seconds after counter measure occurred.
+	// For WPA-PSK supplicant state
+	WPA_STATE   WpaState;           // Default is SS_NOTUSE and handled by microsoft 802.1x
+	UCHAR       ReplayCounter[8];
+	UCHAR       ANonce[32];         // ANonce for WPA-PSK from aurhenticator
+	UCHAR       SNonce[32];         // SNonce for WPA-PSK
+
+	UCHAR       LastSNR0;             // last received BEACON's SNR
+	UCHAR       LastSNR1;            // last received BEACON's SNR for 2nd  antenna
+	RSSI_SAMPLE RssiSample;
+	ULONG       NumOfAvgRssiSample;
+
+	ULONG       LastBeaconRxTime;     // OS's timestamp of the last BEACON RX time
+	ULONG       Last11bBeaconRxTime;  // OS's timestamp of the last 11B BEACON RX time
+	ULONG		Last11gBeaconRxTime;	// OS's timestamp of the last 11G BEACON RX time
+	ULONG		Last20NBeaconRxTime;	// OS's timestamp of the last 20MHz N BEACON RX time
+
+	ULONG       LastScanTime;       // Record last scan time for issue BSSID_SCAN_LIST
+	ULONG       ScanCnt;            // Scan counts since most recent SSID, BSSID, SCAN OID request
+	BOOLEAN     bSwRadio;           // Software controlled Radio On/Off, TRUE: On
+	BOOLEAN     bHwRadio;           // Hardware controlled Radio On/Off, TRUE: On
+	BOOLEAN     bRadio;             // Radio state, And of Sw & Hw radio state
+	BOOLEAN     bHardwareRadio;     // Hardware controlled Radio enabled
+	BOOLEAN     bShowHiddenSSID;    // Show all known SSID in SSID list get operation
+
+
+	// New for WPA, windows want us to to keep association information and
+	// Fixed IEs from last association response
+	NDIS_802_11_ASSOCIATION_INFORMATION     AssocInfo;
+	USHORT       ReqVarIELen;                // Length of next VIE include EID & Length
+	UCHAR       ReqVarIEs[MAX_VIE_LEN];		// The content saved here should be little-endian format.
+	USHORT       ResVarIELen;                // Length of next VIE include EID & Length
+	UCHAR       ResVarIEs[MAX_VIE_LEN];
+
+	UCHAR       RSNIE_Len;
+	UCHAR       RSN_IE[MAX_LEN_OF_RSNIE];	// The content saved here should be little-endian format.
+
+	// New variables used for CCX 1.0
+	BOOLEAN             bCkipOn;
+	BOOLEAN             bCkipCmicOn;
+	UCHAR               CkipFlag;
+	UCHAR               GIV[3];  //for CCX iv
+	UCHAR               RxSEQ[4];
+	UCHAR               TxSEQ[4];
+	UCHAR               CKIPMIC[4];
+	UCHAR               LeapAuthMode;
+	LEAP_AUTH_INFO      LeapAuthInfo;
+	UCHAR               HashPwd[16];
+	UCHAR               NetworkChallenge[8];
+	UCHAR               NetworkChallengeResponse[24];
+	UCHAR               PeerChallenge[8];
+
+	UCHAR               PeerChallengeResponse[24];
+	UCHAR               SessionKey[16]; //Network session keys (NSK)
+	RALINK_TIMER_STRUCT LeapAuthTimer;
+	ROGUEAP_TABLE       RogueApTab;   //Cisco CCX1 Rogue AP Detection
+
+	// New control flags for CCX
+	CCX_CONTROL         CCXControl;                 // Master administration state
+	BOOLEAN             CCXEnable;                  // Actual CCX state
+	UCHAR               CCXScanChannel;             // Selected channel for CCX beacon request
+	USHORT              CCXScanTime;                // Time out to wait for beacon and probe response
+	UCHAR               CCXReqType;                 // Current processing CCX request type
+	BSS_TABLE           CCXBssTab;                  // BSS Table
+	UCHAR               FrameReportBuf[2048];       // Buffer for creating frame report
+	USHORT              FrameReportLen;             // Current Frame report length
+	ULONG               CLBusyBytes;                // Save the total bytes received durning channel load scan time
+	USHORT              RPIDensity[8];              // Array for RPI density collection
+	// Start address of each BSS table within FrameReportBuf
+	// It's important to update the RxPower of the corresponding Bss
+	USHORT              BssReportOffset[MAX_LEN_OF_BSS_TABLE];
+	USHORT              BeaconToken;                // Token for beacon report
+	ULONG               LastBssIndex;               // Most current reported Bss index
+	RM_REQUEST_ACTION   MeasurementRequest[16];     // Saved measurement request
+	UCHAR               RMReqCnt;                   // Number of measurement request saved.
+	UCHAR               CurrentRMReqIdx;            // Number of measurement request saved.
+	BOOLEAN             ParallelReq;                // Parallel measurement, only one request performed,
+													// It must be the same channel with maximum duration
+	USHORT              ParallelDuration;           // Maximum duration for parallel measurement
+	UCHAR               ParallelChannel;            // Only one channel with parallel measurement
+	USHORT              IAPPToken;                  // IAPP dialog token
+	UCHAR               CCXQosECWMin;               // Cisco QOS ECWMin for AC 0
+	UCHAR               CCXQosECWMax;               // Cisco QOS ECWMax for AC 0
+	// Hack for channel load and noise histogram parameters
+	UCHAR               NHFactor;                   // Parameter for Noise histogram
+	UCHAR               CLFactor;                   // Parameter for channel load
+
+	UCHAR               KRK[16];        //Key Refresh Key.
+	UCHAR               BTK[32];        //Base Transient Key
+	BOOLEAN             CCKMLinkUpFlag;
+	ULONG               CCKMRN;    //(Re)Association request number.
+	LARGE_INTEGER       CCKMBeaconAtJoinTimeStamp;  //TSF timer for Re-assocaite to the new AP
+	UCHAR               AironetCellPowerLimit;      //in dBm
+	UCHAR               AironetIPAddress[4];        //eg. 192.168.1.1
+	BOOLEAN             CCXAdjacentAPReportFlag;    //flag for determining report Assoc Lost time
+	CHAR                CCXAdjacentAPSsid[MAX_LEN_OF_SSID]; //Adjacent AP's SSID report
+	UCHAR               CCXAdjacentAPSsidLen;               // the actual ssid length in used
+	UCHAR               CCXAdjacentAPBssid[MAC_ADDR_LEN];         //Adjacent AP's BSSID report
+	USHORT              CCXAdjacentAPChannel;
+	ULONG               CCXAdjacentAPLinkDownTime;  //for Spec S32.
+
+	RALINK_TIMER_STRUCT	StaQuickResponeForRateUpTimer;
+	BOOLEAN				StaQuickResponeForRateUpTimerRunning;
+
+	UCHAR           	DtimCount;      // 0.. DtimPeriod-1
+	UCHAR           	DtimPeriod;     // default = 3
+
+#ifdef QOS_DLS_SUPPORT
+	RT_802_11_DLS		DLSEntry[MAX_NUM_OF_DLS_ENTRY];
+	UCHAR				DlsReplayCounter[8];
+#endif // QOS_DLS_SUPPORT //
+	////////////////////////////////////////////////////////////////////////////////////////
+	// This is only for WHQL test.
+	BOOLEAN				WhqlTest;
+	////////////////////////////////////////////////////////////////////////////////////////
+
+    RALINK_TIMER_STRUCT WpaDisassocAndBlockAssocTimer;
+    // Fast Roaming
+	BOOLEAN		        bFastRoaming;       // 0:disable fast roaming, 1:enable fast roaming
+	CHAR		        dBmToRoam;          // the condition to roam when receiving Rssi less than this value. It's negative value.
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    BOOLEAN             IEEE8021X;
+    BOOLEAN             IEEE8021x_required_keys;
+    CIPHER_KEY	        DesireSharedKey[4];	// Record user desired WEP keys
+    UCHAR               DesireSharedKeyId;
+
+    // 0: driver ignores wpa_supplicant
+    // 1: wpa_supplicant initiates scanning and AP selection
+    // 2: driver takes care of scanning, AP selection, and IEEE 802.11 association parameters
+    UCHAR               WpaSupplicantUP;
+	UCHAR				WpaSupplicantScanCount;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+    CHAR                dev_name[16];
+    USHORT              OriDevType;
+
+    BOOLEAN             bTGnWifiTest;
+	BOOLEAN			    bScanReqIsFromWebUI;
+
+	HTTRANSMIT_SETTING				HTPhyMode, MaxHTPhyMode, MinHTPhyMode;// For transmit phy setting in TXWI.
+	DESIRED_TRANSMIT_SETTING       	DesiredTransmitSetting;
+	RT_HT_PHY_INFO					DesiredHtPhyInfo;
+	BOOLEAN							bAutoTxRateSwitch;
+
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	UCHAR				IEEE80211dClientMode;
+	UCHAR				StaOriCountryCode[3];
+	UCHAR				StaOriGeography;
+#endif // EXT_BUILD_CHANNEL_LIST //
+} STA_ADMIN_CONFIG, *PSTA_ADMIN_CONFIG;
+
+// This data structure keep the current active BSS/IBSS's configuration that this STA
+// had agreed upon joining the network. Which means these parameters are usually decided
+// by the BSS/IBSS creator instead of user configuration. Data in this data structurre
+// is valid only when either ADHOC_ON(pAd) or INFRA_ON(pAd) is TRUE.
+// Normally, after SCAN or failed roaming attempts, we need to recover back to
+// the current active settings.
+typedef struct _STA_ACTIVE_CONFIG {
+	USHORT      Aid;
+	USHORT      AtimWin;                // in kusec; IBSS parameter set element
+	USHORT      CapabilityInfo;
+	USHORT      CfpMaxDuration;
+	USHORT      CfpPeriod;
+
+	// Copy supported rate from desired AP's beacon. We are trying to match
+	// AP's supported and extended rate settings.
+	UCHAR       SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       SupRateLen;
+	UCHAR       ExtRateLen;
+	// Copy supported ht from desired AP's beacon. We are trying to match
+	RT_HT_PHY_INFO		SupportedPhyInfo;
+	RT_HT_CAPABILITY	SupportedHtPhy;
+} STA_ACTIVE_CONFIG, *PSTA_ACTIVE_CONFIG;
+
+#ifdef RT2870
+// for USB interface, avoid in interrupt when write key
+typedef struct   RT_ADD_PAIRWISE_KEY_ENTRY {
+        NDIS_802_11_MAC_ADDRESS         MacAddr;
+        USHORT                          MacTabMatchWCID;        // ASIC
+        CIPHER_KEY                      CipherKey;
+} RT_ADD_PAIRWISE_KEY_ENTRY,*PRT_ADD_PAIRWISE_KEY_ENTRY;
+#endif // RT2870 //
+#endif // CONFIG_STA_SUPPORT //
+
+// ----------- start of AP --------------------------
+// AUTH-RSP State Machine Aux data structure
+typedef struct _AP_MLME_AUX {
+	UCHAR               Addr[MAC_ADDR_LEN];
+	USHORT              Alg;
+	CHAR                Challenge[CIPHER_TEXT_LEN];
+} AP_MLME_AUX, *PAP_MLME_AUX;
+
+// structure to define WPA Group Key Rekey Interval
+typedef struct PACKED _RT_802_11_WPA_REKEY {
+	ULONG ReKeyMethod;          // mechanism for rekeying: 0:disable, 1: time-based, 2: packet-based
+	ULONG ReKeyInterval;        // time-based: seconds, packet-based: kilo-packets
+} RT_WPA_REKEY,*PRT_WPA_REKEY, RT_802_11_WPA_REKEY, *PRT_802_11_WPA_REKEY;
+
+typedef struct _MAC_TABLE_ENTRY {
+	//Choose 1 from ValidAsWDS and ValidAsCLI  to validize.
+	BOOLEAN		ValidAsCLI;		// Sta mode, set this TRUE after Linkup,too.
+	BOOLEAN		ValidAsWDS;	// This is WDS Entry. only for AP mode.
+	BOOLEAN		ValidAsApCli;   //This is a AP-Client entry, only for AP mode which enable AP-Client functions.
+	BOOLEAN		ValidAsMesh;
+	BOOLEAN		ValidAsDls;	// This is DLS Entry. only for STA mode.
+	BOOLEAN		isCached;
+	BOOLEAN		bIAmBadAtheros;	// Flag if this is Atheros chip that has IOT problem.  We need to turn on RTS/CTS protection.
+
+	UCHAR         	EnqueueEapolStartTimerRunning;  // Enqueue EAPoL-Start for triggering EAP SM
+	//jan for wpa
+	// record which entry revoke MIC Failure , if it leaves the BSS itself, AP won't update aMICFailTime MIB
+	UCHAR           CMTimerRunning;
+	UCHAR           apidx;			// MBSS number
+	UCHAR           RSNIE_Len;
+	UCHAR           RSN_IE[MAX_LEN_OF_RSNIE];
+	UCHAR           ANonce[LEN_KEY_DESC_NONCE];
+	UCHAR           R_Counter[LEN_KEY_DESC_REPLAY];
+	UCHAR           PTK[64];
+	UCHAR           ReTryCounter;
+	RALINK_TIMER_STRUCT                 RetryTimer;
+	RALINK_TIMER_STRUCT					EnqueueStartForPSKTimer;	// A timer which enqueue EAPoL-Start for triggering PSK SM
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;   // This should match to whatever microsoft defined
+	NDIS_802_11_WEP_STATUS              WepStatus;
+	AP_WPA_STATE    WpaState;
+	GTK_STATE       GTKState;
+	USHORT          PortSecured;
+	NDIS_802_11_PRIVACY_FILTER  PrivacyFilter;      // PrivacyFilter enum for 802.1X
+	CIPHER_KEY      PairwiseKey;
+	PVOID           pAd;
+    INT				PMKID_CacheIdx;
+    UCHAR			PMKID[LEN_PMKID];
+
+
+	UCHAR           Addr[MAC_ADDR_LEN];
+	UCHAR           PsMode;
+	SST             Sst;
+	AUTH_STATE      AuthState; // for SHARED KEY authentication state machine used only
+	BOOLEAN			IsReassocSta;	// Indicate whether this is a reassociation procedure
+	USHORT          Aid;
+	USHORT          CapabilityInfo;
+	UCHAR           LastRssi;
+	ULONG           NoDataIdleCount;
+	UINT16			StationKeepAliveCount; // unit: second
+	ULONG           PsQIdleCount;
+	QUEUE_HEADER    PsQueue;
+
+	UINT32			StaConnectTime;		// the live time of this station since associated with AP
+
+
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN			bSendBAR;
+	USHORT			NoBADataCountDown;
+
+	UINT32   		CachedBuf[16];		// UINT (4 bytes) for alignment
+	UINT			TxBFCount; // 3*3
+#endif // DOT11_N_SUPPORT //
+	UINT			FIFOCount;
+	UINT			DebugFIFOCount;
+	UINT			DebugTxCount;
+    BOOLEAN			bDlsInit;
+
+
+//====================================================
+//WDS entry needs these
+// rt2860 add this. if ValidAsWDS==TRUE, MatchWDSTabIdx is the index in WdsTab.MacTab
+	UINT			MatchWDSTabIdx;
+	UCHAR           MaxSupportedRate;
+	UCHAR           CurrTxRate;
+	UCHAR           CurrTxRateIndex;
+	// to record the each TX rate's quality. 0 is best, the bigger the worse.
+	USHORT          TxQuality[MAX_STEP_OF_TX_RATE_SWITCH];
+//	USHORT          OneSecTxOkCount;
+	UINT32			OneSecTxNoRetryOkCount;
+	UINT32          OneSecTxRetryOkCount;
+	UINT32          OneSecTxFailCount;
+	UINT32			ContinueTxFailCnt;
+	UINT32          CurrTxRateStableTime; // # of second in current TX rate
+	UCHAR           TxRateUpPenalty;      // extra # of second penalty due to last unstable condition
+//====================================================
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+	UINT			MatchDlsEntryIdx; // indicate the index in pAd->StaCfg.DLSEntry
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+	BOOLEAN         fNoisyEnvironment;
+	BOOLEAN			fLastSecAccordingRSSI;
+	UCHAR           LastSecTxRateChangeAction; // 0: no change, 1:rate UP, 2:rate down
+	CHAR			LastTimeTxRateChangeAction; //Keep last time value of LastSecTxRateChangeAction
+	ULONG			LastTxOkCount;
+	UCHAR           PER[MAX_STEP_OF_TX_RATE_SWITCH];
+
+	// a bitmap of BOOLEAN flags. each bit represent an operation status of a particular
+	// BOOLEAN control, either ON or OFF. These flags should always be accessed via
+	// CLIENT_STATUS_TEST_FLAG(), CLIENT_STATUS_SET_FLAG(), CLIENT_STATUS_CLEAR_FLAG() macros.
+	// see fOP_STATUS_xxx in RTMP_DEF.C for detail bit definition. fCLIENT_STATUS_AMSDU_INUSED
+	ULONG           ClientStatusFlags;
+
+	// TODO: Shall we move that to DOT11_N_SUPPORT???
+	HTTRANSMIT_SETTING	HTPhyMode, MaxHTPhyMode, MinHTPhyMode;// For transmit phy setting in TXWI.
+
+#ifdef DOT11_N_SUPPORT
+	// HT EWC MIMO-N used parameters
+	USHORT		RXBAbitmap;	// fill to on-chip  RXWI_BA_BITMASK in 8.1.3RX attribute entry format
+	USHORT		TXBAbitmap;	// This bitmap as originator, only keep in software used to mark AMPDU bit in TXWI
+	USHORT		TXAutoBAbitmap;
+	USHORT		BADeclineBitmap;
+	USHORT		BARecWcidArray[NUM_OF_TID];	// The mapping wcid of recipient session. if RXBAbitmap bit is masked
+	USHORT		BAOriWcidArray[NUM_OF_TID]; // The mapping wcid of originator session. if TXBAbitmap bit is masked
+	USHORT		BAOriSequence[NUM_OF_TID]; // The mapping wcid of originator session. if TXBAbitmap bit is masked
+
+	// 802.11n features.
+	UCHAR		MpduDensity;
+	UCHAR		MaxRAmpduFactor;
+	UCHAR		AMsduSize;
+	UCHAR		MmpsMode;	// MIMO power save more.
+
+	HT_CAPABILITY_IE		HTCapability;
+
+#ifdef DOT11N_DRAFT3
+	UCHAR		BSS2040CoexistenceMgmtSupport;
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+	BOOLEAN		bAutoTxRateSwitch;
+
+	UCHAR       RateLen;
+	struct _MAC_TABLE_ENTRY *pNext;
+    USHORT      TxSeq[NUM_OF_TID];
+	USHORT		NonQosDataSeq;
+
+	RSSI_SAMPLE	RssiSample;
+
+	UINT32			TXMCSExpected[16];
+	UINT32			TXMCSSuccessful[16];
+	UINT32			TXMCSFailed[16];
+	UINT32			TXMCSAutoFallBack[16][16];
+
+#ifdef CONFIG_STA_SUPPORT
+	ULONG   		LastBeaconRxTime;
+#endif // CONFIG_STA_SUPPORT //
+} MAC_TABLE_ENTRY, *PMAC_TABLE_ENTRY;
+
+typedef struct _MAC_TABLE {
+	USHORT			Size;
+	MAC_TABLE_ENTRY *Hash[HASH_TABLE_SIZE];
+	MAC_TABLE_ENTRY Content[MAX_LEN_OF_MAC_TABLE];
+	QUEUE_HEADER    McastPsQueue;
+	ULONG           PsQIdleCount;
+	BOOLEAN         fAnyStationInPsm;
+	BOOLEAN         fAnyStationBadAtheros;	// Check if any Station is atheros 802.11n Chip.  We need to use RTS/CTS with Atheros 802,.11n chip.
+	BOOLEAN			fAnyTxOPForceDisable;	// Check if it is necessary to disable BE TxOP
+	BOOLEAN			fAllStationAsRalink; 	// Check if all stations are ralink-chipset
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN         fAnyStationIsLegacy;	// Check if I use legacy rate to transmit to my BSS Station/
+	BOOLEAN         fAnyStationNonGF;		// Check if any Station can't support GF.
+	BOOLEAN         fAnyStation20Only;		// Check if any Station can't support GF.
+	BOOLEAN			fAnyStationMIMOPSDynamic; // Check if any Station is MIMO Dynamic
+	BOOLEAN         fAnyBASession;   // Check if there is BA session.  Force turn on RTS/CTS
+#endif // DOT11_N_SUPPORT //
+} MAC_TABLE, *PMAC_TABLE;
+
+#ifdef DOT11_N_SUPPORT
+#define IS_HT_STA(_pMacEntry)	\
+	(_pMacEntry->MaxHTPhyMode.field.MODE >= MODE_HTMIX)
+
+#define IS_HT_RATE(_pMacEntry)	\
+	(_pMacEntry->HTPhyMode.field.MODE >= MODE_HTMIX)
+
+#define PEER_IS_HT_RATE(_pMacEntry)	\
+	(_pMacEntry->HTPhyMode.field.MODE >= MODE_HTMIX)
+#endif // DOT11_N_SUPPORT //
+
+typedef struct _WDS_ENTRY {
+	BOOLEAN         Valid;
+	UCHAR           Addr[MAC_ADDR_LEN];
+	ULONG           NoDataIdleCount;
+	struct _WDS_ENTRY *pNext;
+} WDS_ENTRY, *PWDS_ENTRY;
+
+typedef struct  _WDS_TABLE_ENTRY {
+	USHORT			Size;
+	UCHAR           WdsAddr[MAC_ADDR_LEN];
+	WDS_ENTRY       *Hash[HASH_TABLE_SIZE];
+	WDS_ENTRY       Content[MAX_LEN_OF_MAC_TABLE];
+	UCHAR           MaxSupportedRate;
+	UCHAR           CurrTxRate;
+	USHORT          TxQuality[MAX_LEN_OF_SUPPORTED_RATES];
+	USHORT          OneSecTxOkCount;
+	USHORT          OneSecTxRetryOkCount;
+	USHORT          OneSecTxFailCount;
+	ULONG           CurrTxRateStableTime; // # of second in current TX rate
+	UCHAR           TxRateUpPenalty;      // extra # of second penalty due to last unstable condition
+} WDS_TABLE_ENTRY, *PWDS_TABLE_ENTRY;
+
+typedef struct _RT_802_11_WDS_ENTRY {
+	PNET_DEV			dev;
+	UCHAR				Valid;
+	UCHAR				PhyMode;
+	UCHAR				PeerWdsAddr[MAC_ADDR_LEN];
+	UCHAR				MacTabMatchWCID;	// ASIC
+	NDIS_802_11_WEP_STATUS  WepStatus;
+	UCHAR					KeyIdx;
+	CIPHER_KEY          	WdsKey;
+	HTTRANSMIT_SETTING				HTPhyMode, MaxHTPhyMode, MinHTPhyMode;
+	RT_HT_PHY_INFO					DesiredHtPhyInfo;
+	BOOLEAN							bAutoTxRateSwitch;
+	DESIRED_TRANSMIT_SETTING       	DesiredTransmitSetting; // Desired transmit setting.
+} RT_802_11_WDS_ENTRY, *PRT_802_11_WDS_ENTRY;
+
+typedef struct _WDS_TABLE {
+	UCHAR               Mode;
+	ULONG               Size;
+	RT_802_11_WDS_ENTRY	WdsEntry[MAX_WDS_ENTRY];
+} WDS_TABLE, *PWDS_TABLE;
+
+typedef struct _APCLI_STRUCT {
+	PNET_DEV				dev;
+#ifdef RTL865X_SOC
+	unsigned int            mylinkid;
+#endif
+	BOOLEAN                 Enable;	// Set it as 1 if the apcli interface was configured to "1"  or by iwpriv cmd "ApCliEnable"
+	BOOLEAN                 Valid;	// Set it as 1 if the apcli interface associated success to remote AP.
+	UCHAR					MacTabWCID;	//WCID value, which point to the entry of ASIC Mac table.
+	UCHAR                   SsidLen;
+	CHAR                    Ssid[MAX_LEN_OF_SSID];
+
+	UCHAR                   CfgSsidLen;
+	CHAR                    CfgSsid[MAX_LEN_OF_SSID];
+	UCHAR                   CfgApCliBssid[ETH_LENGTH_OF_ADDRESS];
+	UCHAR                   CurrentAddress[ETH_LENGTH_OF_ADDRESS];
+
+	ULONG                   ApCliRcvBeaconTime;
+
+	ULONG                   CtrlCurrState;
+	ULONG                   SyncCurrState;
+	ULONG                   AuthCurrState;
+	ULONG                   AssocCurrState;
+	ULONG					WpaPskCurrState;
+
+	USHORT                  AuthReqCnt;
+	USHORT                  AssocReqCnt;
+
+	ULONG                   ClientStatusFlags;
+	UCHAR                   MpduDensity;
+
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;   // This should match to whatever microsoft defined
+	NDIS_802_11_WEP_STATUS              WepStatus;
+
+	// Add to support different cipher suite for WPA2/WPA mode
+	NDIS_802_11_ENCRYPTION_STATUS		GroupCipher;		// Multicast cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS		PairCipher;			// Unicast cipher suite
+	BOOLEAN								bMixCipher;			// Indicate current Pair & Group use different cipher suites
+	USHORT								RsnCapability;
+
+	UCHAR		PSK[100];				// reserve PSK key material
+	UCHAR       PSKLen;
+	UCHAR       PMK[32];                // WPA PSK mode PMK
+	//UCHAR       PTK[64];                // WPA PSK mode PTK
+	UCHAR		GTK[32];				// GTK from authenticator
+
+	//CIPHER_KEY		PairwiseKey;
+	CIPHER_KEY      SharedKey[SHARE_KEY_NUM];
+	UCHAR           DefaultKeyId;
+
+	// WPA 802.1x port control, WPA_802_1X_PORT_SECURED, WPA_802_1X_PORT_NOT_SECURED
+	//UCHAR       PortSecured;
+
+	// store RSN_IE built by driver
+	UCHAR		RSN_IE[MAX_LEN_OF_RSNIE];  // The content saved here should be convert to little-endian format.
+	UCHAR		RSNIE_Len;
+
+	// For WPA countermeasures
+	ULONG       LastMicErrorTime;   // record last MIC error time
+	//ULONG       MicErrCnt;          // Should be 0, 1, 2, then reset to zero (after disassoiciation).
+	BOOLEAN                 bBlockAssoc; // Block associate attempt for 60 seconds after counter measure occurred.
+
+	// For WPA-PSK supplicant state
+	//WPA_STATE   	WpaState;           // Default is SS_NOTUSE
+	//UCHAR       	ReplayCounter[8];
+	//UCHAR       	ANonce[32];         // ANonce for WPA-PSK from authenticator
+	UCHAR       	SNonce[32];         // SNonce for WPA-PSK
+	UCHAR			GNonce[32];			// GNonce for WPA-PSK from authenticator
+
+	HTTRANSMIT_SETTING				HTPhyMode, MaxHTPhyMode, MinHTPhyMode;
+	RT_HT_PHY_INFO					DesiredHtPhyInfo;
+	BOOLEAN							bAutoTxRateSwitch;
+	DESIRED_TRANSMIT_SETTING       	DesiredTransmitSetting; // Desired transmit setting.
+} APCLI_STRUCT, *PAPCLI_STRUCT;
+
+// ----------- end of AP ----------------------------
+
+#ifdef BLOCK_NET_IF
+typedef struct _BLOCK_QUEUE_ENTRY
+{
+	BOOLEAN SwTxQueueBlockFlag;
+	LIST_HEADER NetIfList;
+} BLOCK_QUEUE_ENTRY, *PBLOCK_QUEUE_ENTRY;
+#endif // BLOCK_NET_IF //
+
+struct wificonf
+{
+	BOOLEAN	bShortGI;
+	BOOLEAN bGreenField;
+};
+
+
+
+typedef struct _INF_PCI_CONFIG
+{
+	PUCHAR                  CSRBaseAddress;     // PCI MMIO Base Address, all access will use
+}INF_PCI_CONFIG;
+
+typedef struct _INF_USB_CONFIG
+{
+	UINT                BulkInEpAddr;		// bulk-in endpoint address
+	UINT                BulkOutEpAddr[6];	// bulk-out endpoint address
+
+}INF_USB_CONFIG;
+
+#ifdef IKANOS_VX_1X0
+	typedef void (*IkanosWlanTxCbFuncP)(void *, void *);
+
+	struct IKANOS_TX_INFO
+	{
+		struct net_device *netdev;
+		IkanosWlanTxCbFuncP *fp;
+	};
+#endif // IKANOS_VX_1X0 //
+
+#ifdef NINTENDO_AP
+typedef struct _NINDO_CTRL_BLOCK {
+
+	RT_NINTENDO_TABLE	DS_TABLE;
+
+#ifdef CHIP25XX
+	spinlock_t			NINTENDO_TABLE_Lock;
+#else
+	NDIS_SPIN_LOCK		NINTENDO_TABLE_Lock;
+#endif // CHIP25XX //
+
+	UCHAR				NINTENDO_UP_BUFFER[512];
+	UCHAR				Local_KeyIdx;
+	CIPHER_KEY			Local_SharedKey;
+	UCHAR				Local_bHideSsid;
+	UCHAR				Local_AuthMode;
+	UCHAR				Local_WepStatus;
+	USHORT				Local_CapabilityInfo;
+} NINDO_CTRL_BLOCK;
+#endif // NINTENDO_AP //
+
+
+#ifdef DBG_DIAGNOSE
+#define DIAGNOSE_TIME	10   // 10 sec
+typedef struct _RtmpDiagStrcut_
+{	// Diagnosis Related element
+	unsigned char		inited;
+	unsigned char 	qIdx;
+	unsigned char 	ArrayStartIdx;
+	unsigned char		ArrayCurIdx;
+	// Tx Related Count
+	USHORT			TxDataCnt[DIAGNOSE_TIME];
+	USHORT			TxFailCnt[DIAGNOSE_TIME];
+//	USHORT			TxDescCnt[DIAGNOSE_TIME][16];		// TxDesc queue length in scale of 0~14, >=15
+	USHORT			TxDescCnt[DIAGNOSE_TIME][24]; // 3*3	// TxDesc queue length in scale of 0~14, >=15
+//	USHORT			TxMcsCnt[DIAGNOSE_TIME][16];			// TxDate MCS Count in range from 0 to 15, step in 1.
+	USHORT			TxMcsCnt[DIAGNOSE_TIME][24]; // 3*3
+	USHORT			TxSWQueCnt[DIAGNOSE_TIME][9];		// TxSwQueue length in scale of 0, 1, 2, 3, 4, 5, 6, 7, >=8
+
+	USHORT			TxAggCnt[DIAGNOSE_TIME];
+	USHORT			TxNonAggCnt[DIAGNOSE_TIME];
+//	USHORT			TxAMPDUCnt[DIAGNOSE_TIME][16];		// 10 sec, TxDMA APMDU Aggregation count in range from 0 to 15, in setp of 1.
+	USHORT			TxAMPDUCnt[DIAGNOSE_TIME][24]; // 3*3 // 10 sec, TxDMA APMDU Aggregation count in range from 0 to 15, in setp of 1.
+	USHORT			TxRalinkCnt[DIAGNOSE_TIME];			// TxRalink Aggregation Count in 1 sec scale.
+	USHORT			TxAMSDUCnt[DIAGNOSE_TIME];			// TxAMSUD Aggregation Count in 1 sec scale.
+
+	// Rx Related Count
+	USHORT			RxDataCnt[DIAGNOSE_TIME];			// Rx Total Data count.
+	USHORT			RxCrcErrCnt[DIAGNOSE_TIME];
+//	USHORT			RxMcsCnt[DIAGNOSE_TIME][16];		// Rx MCS Count in range from 0 to 15, step in 1.
+	USHORT			RxMcsCnt[DIAGNOSE_TIME][24]; // 3*3
+}RtmpDiagStruct;
+#endif // DBG_DIAGNOSE //
+
+
+//
+//  The miniport adapter structure
+//
+typedef struct _RTMP_ADAPTER
+{
+	PVOID					OS_Cookie;	// save specific structure relative to OS
+	PNET_DEV				net_dev;
+	ULONG					VirtualIfCnt;
+
+
+
+	NDIS_SPIN_LOCK          irq_lock;
+	UCHAR                   irq_disabled;
+
+#ifdef RT2870
+/*****************************************************************************************/
+/*      USB related parameters                                                           */
+/*****************************************************************************************/
+	struct usb_config_descriptor		*config;
+	UINT								BulkInEpAddr;		// bulk-in endpoint address
+	UINT								BulkOutEpAddr[6];	// bulk-out endpoint address
+
+	UINT								NumberOfPipes;
+	USHORT								BulkOutMaxPacketSize;
+	USHORT								BulkInMaxPacketSize;
+
+	//======Control Flags
+	LONG                    	PendingIoCount;
+	ULONG						BulkFlags;
+	BOOLEAN                     bUsbTxBulkAggre;	// Flags for bulk out data priority
+
+
+	//======Timer Thread
+	RT2870_TIMER_QUEUE		TimerQ;
+	NDIS_SPIN_LOCK			TimerQLock;
+
+
+	//======Cmd Thread
+	CmdQ					CmdQ;
+	NDIS_SPIN_LOCK			CmdQLock;				// CmdQLock spinlock
+
+	BOOLEAN					TimerFunc_kill;
+	BOOLEAN					mlme_kill;
+
+
+	//======Semaphores (event)
+	struct semaphore			mlme_semaphore;			/* to sleep thread on	*/
+	struct semaphore			RTUSBCmd_semaphore;		/* to sleep thread on	*/
+	struct semaphore			RTUSBTimer_semaphore;
+#ifdef INF_AMAZON_SE
+	struct semaphore			UsbVendorReq_semaphore;
+	PVOID						UsbVendorReqBuf;
+#endif // INF_AMAZON_SE //
+	struct completion			TimerQComplete;
+	struct completion			mlmeComplete;
+	struct completion			CmdQComplete;
+	wait_queue_head_t			*wait;
+
+	//======Lock for 2870 ATE
+#ifdef RALINK_ATE
+	NDIS_SPIN_LOCK			GenericLock;		// ATE Tx/Rx generic spinlock
+#endif // RALINK_ATE //
+
+#endif // RT2870 //
+
+
+/*****************************************************************************************/
+	/*      Both PCI/USB related parameters                                                  */
+/*****************************************************************************************/
+
+
+/*****************************************************************************************/
+/*      Tx related parameters                                                           */
+/*****************************************************************************************/
+	BOOLEAN                 DeQueueRunning[NUM_OF_TX_RING];  // for ensuring RTUSBDeQueuePacket get call once
+	NDIS_SPIN_LOCK          DeQueueLock[NUM_OF_TX_RING];
+
+#ifdef RT2870
+	// Data related context and AC specified, 4 AC supported
+	NDIS_SPIN_LOCK			BulkOutLock[6];			// BulkOut spinlock for 4 ACs
+	NDIS_SPIN_LOCK			MLMEBulkOutLock;	// MLME BulkOut lock
+
+	HT_TX_CONTEXT			TxContext[NUM_OF_TX_RING];
+	NDIS_SPIN_LOCK			TxContextQueueLock[NUM_OF_TX_RING];		// TxContextQueue spinlock
+
+	// 4 sets of Bulk Out index and pending flag
+	UCHAR					NextBulkOutIndex[4];	// only used for 4 EDCA bulkout pipe
+
+	BOOLEAN					BulkOutPending[6];	// used for total 6 bulkout pipe
+	UCHAR					bulkResetPipeid;
+	BOOLEAN					MgmtBulkPending;
+	ULONG					bulkResetReq[6];
+#endif // RT2870 //
+
+	// resource for software backlog queues
+	QUEUE_HEADER            TxSwQueue[NUM_OF_TX_RING];  // 4 AC + 1 HCCA
+	NDIS_SPIN_LOCK          TxSwQueueLock[NUM_OF_TX_RING];	// TxSwQueue spinlock
+
+	RTMP_DMABUF             MgmtDescRing;               	// Shared memory for MGMT descriptors
+	RTMP_MGMT_RING          MgmtRing;
+	NDIS_SPIN_LOCK          MgmtRingLock;               	// Prio Ring spinlock
+
+
+/*****************************************************************************************/
+/*      Rx related parameters                                                           */
+/*****************************************************************************************/
+
+
+#ifdef RT2870
+	RX_CONTEXT				RxContext[RX_RING_SIZE];  // 1 for redundant multiple IRP bulk in.
+	NDIS_SPIN_LOCK			BulkInLock;				// BulkIn spinlock for 4 ACs
+	UCHAR					PendingRx;				// The Maxima pending Rx value should be 	RX_RING_SIZE.
+	UCHAR					NextRxBulkInIndex;		// Indicate the current RxContext Index which hold by Host controller.
+	UCHAR					NextRxBulkInReadIndex;	// Indicate the current RxContext Index which driver can read & process it.
+	ULONG					NextRxBulkInPosition;   // Want to contatenate 2 URB buffer while 1st is bulkin failed URB. This Position is 1st URB TransferLength.
+	ULONG					TransferBufferLength;	// current length of the packet buffer
+	ULONG					ReadPosition;			// current read position in a packet buffer
+#endif // RT2870 //
+
+
+/*****************************************************************************************/
+/*      ASIC related parameters                                                          */
+/*****************************************************************************************/
+	UINT32               	MACVersion;      	// MAC version. Record rt2860C(0x28600100) or rt2860D (0x28600101)..
+
+	// ---------------------------
+	// E2PROM
+	// ---------------------------
+	ULONG                   EepromVersion;          // byte 0: version, byte 1: revision, byte 2~3: unused
+	UCHAR                   EEPROMAddressNum;       // 93c46=6  93c66=8
+	USHORT                  EEPROMDefaultValue[NUM_EEPROM_BBP_PARMS];
+	ULONG                   FirmwareVersion;        // byte 0: Minor version, byte 1: Major version, otherwise unused.
+
+	// ---------------------------
+	// BBP Control
+	// ---------------------------
+	UCHAR                   BbpWriteLatch[140];     // record last BBP register value written via BBP_IO_WRITE/BBP_IO_WRITE_VY_REG_ID
+	UCHAR                   BbpRssiToDbmDelta;
+	BBP_R66_TUNING          BbpTuning;
+
+	// ----------------------------
+	// RFIC control
+	// ----------------------------
+	UCHAR                   RfIcType;       // RFIC_xxx
+	ULONG                   RfFreqOffset;   // Frequency offset for channel switching
+	RTMP_RF_REGS            LatchRfRegs;    // latch th latest RF programming value since RF IC doesn't support READ
+
+	EEPROM_ANTENNA_STRUC    Antenna;                            // Since ANtenna definition is different for a & g. We need to save it for future reference.
+	EEPROM_NIC_CONFIG2_STRUC    NicConfig2;
+
+	// This soft Rx Antenna Diversity mechanism is used only when user set
+	// RX Antenna = DIVERSITY ON
+	SOFT_RX_ANT_DIVERSITY   RxAnt;
+
+	UCHAR                   RFProgSeq;
+	CHANNEL_TX_POWER        TxPower[MAX_NUM_OF_CHANNELS];       // Store Tx power value for all channels.
+	CHANNEL_TX_POWER        ChannelList[MAX_NUM_OF_CHANNELS];   // list all supported channels for site survey
+	CHANNEL_11J_TX_POWER    TxPower11J[MAX_NUM_OF_11JCHANNELS];       // 802.11j channel and bw
+	CHANNEL_11J_TX_POWER    ChannelList11J[MAX_NUM_OF_11JCHANNELS];   // list all supported channels for site survey
+
+	UCHAR                   ChannelListNum;                     // number of channel in ChannelList[]
+	UCHAR					Bbp94;
+	BOOLEAN					BbpForCCK;
+	ULONG		Tx20MPwrCfgABand[5];
+	ULONG		Tx20MPwrCfgGBand[5];
+	ULONG		Tx40MPwrCfgABand[5];
+	ULONG		Tx40MPwrCfgGBand[5];
+
+	BOOLEAN     bAutoTxAgcA;                // Enable driver auto Tx Agc control
+	UCHAR	    TssiRefA;					// Store Tssi reference value as 25 temperature.
+	UCHAR	    TssiPlusBoundaryA[5];		// Tssi boundary for increase Tx power to compensate.
+	UCHAR	    TssiMinusBoundaryA[5];		// Tssi boundary for decrease Tx power to compensate.
+	UCHAR	    TxAgcStepA;					// Store Tx TSSI delta increment / decrement value
+	CHAR		TxAgcCompensateA;			// Store the compensation (TxAgcStep * (idx-1))
+
+	BOOLEAN     bAutoTxAgcG;                // Enable driver auto Tx Agc control
+	UCHAR	    TssiRefG;					// Store Tssi reference value as 25 temperature.
+	UCHAR	    TssiPlusBoundaryG[5];		// Tssi boundary for increase Tx power to compensate.
+	UCHAR	    TssiMinusBoundaryG[5];		// Tssi boundary for decrease Tx power to compensate.
+	UCHAR	    TxAgcStepG;					// Store Tx TSSI delta increment / decrement value
+	CHAR		TxAgcCompensateG;			// Store the compensation (TxAgcStep * (idx-1))
+
+	//+++For RT2870, the parameteres is start from BGRssiOffset1 ~ BGRssiOffset3
+	CHAR		BGRssiOffset0;				// Store B/G RSSI#0 Offset value on EEPROM 0x46h
+	CHAR		BGRssiOffset1;				// Store B/G RSSI#1 Offset value
+	CHAR		BGRssiOffset2;				// Store B/G RSSI#2 Offset value
+	//---
+
+	//+++For RT2870, the parameteres is start from ARssiOffset1 ~ ARssiOffset3
+	CHAR		ARssiOffset0;				// Store A RSSI#0 Offset value on EEPROM 0x4Ah
+	CHAR		ARssiOffset1;				// Store A RSSI#1 Offset value
+	CHAR		ARssiOffset2;				// Store A RSSI#2 Offset value
+	//---
+
+	CHAR		BLNAGain;					// Store B/G external LNA#0 value on EEPROM 0x44h
+	CHAR		ALNAGain0;					// Store A external LNA#0 value for ch36~64
+	CHAR		ALNAGain1;					// Store A external LNA#1 value for ch100~128
+	CHAR		ALNAGain2;					// Store A external LNA#2 value for ch132~165
+
+	// ----------------------------
+	// LED control
+	// ----------------------------
+	MCU_LEDCS_STRUC		LedCntl;
+	USHORT				Led1;	// read from EEPROM 0x3c
+	USHORT				Led2;	// EEPROM 0x3e
+	USHORT				Led3;	// EEPROM 0x40
+	UCHAR				LedIndicatorStregth;
+	UCHAR				RssiSingalstrengthOffet;
+    BOOLEAN				bLedOnScanning;
+	UCHAR				LedStatus;
+
+/*****************************************************************************************/
+/*      802.11 related parameters                                                        */
+/*****************************************************************************************/
+	// outgoing BEACON frame buffer and corresponding TXD
+	TXWI_STRUC              	BeaconTxWI;
+	PUCHAR						BeaconBuf;
+	USHORT						BeaconOffset[HW_BEACON_MAX_COUNT];
+
+	// pre-build PS-POLL and NULL frame upon link up. for efficiency purpose.
+	PSPOLL_FRAME            	PsPollFrame;
+	HEADER_802_11           	NullFrame;
+
+#ifdef RT2870
+	TX_CONTEXT				BeaconContext[BEACON_RING_SIZE];
+	TX_CONTEXT				NullContext;
+	TX_CONTEXT				PsPollContext;
+	TX_CONTEXT				RTSContext;
+#endif // RT2870 //
+
+
+
+//=========AP===========
+
+
+//=======STA===========
+#ifdef CONFIG_STA_SUPPORT
+/* Modified by Wu Xi-Kun 4/21/2006 */
+	// -----------------------------------------------
+	// STA specific configuration & operation status
+	// used only when pAd->OpMode == OPMODE_STA
+	// -----------------------------------------------
+	STA_ADMIN_CONFIG        StaCfg;           // user desired settings
+	STA_ACTIVE_CONFIG       StaActive;         // valid only when ADHOC_ON(pAd) || INFRA_ON(pAd)
+	CHAR                    nickname[IW_ESSID_MAX_SIZE+1]; // nickname, only used in the iwconfig i/f
+	NDIS_MEDIA_STATE        PreMediaState;
+#endif // CONFIG_STA_SUPPORT //
+
+//=======Common===========
+	// OP mode: either AP or STA
+	UCHAR                   OpMode;                     // OPMODE_STA, OPMODE_AP
+
+	NDIS_MEDIA_STATE        IndicateMediaState;			// Base on Indication state, default is NdisMediaStateDisConnected
+
+
+	// configuration: read from Registry & E2PROM
+	BOOLEAN                 bLocalAdminMAC;             // Use user changed MAC
+	UCHAR                   PermanentAddress[MAC_ADDR_LEN];    // Factory default MAC address
+	UCHAR                   CurrentAddress[MAC_ADDR_LEN];      // User changed MAC address
+
+	// ------------------------------------------------------
+	// common configuration to both OPMODE_STA and OPMODE_AP
+	// ------------------------------------------------------
+	COMMON_CONFIG           CommonCfg;
+	MLME_STRUCT             Mlme;
+
+	// AP needs those vaiables for site survey feature.
+	MLME_AUX                MlmeAux;           // temporary settings used during MLME state machine
+	BSS_TABLE               ScanTab;           // store the latest SCAN result
+
+	//About MacTab, the sta driver will use #0 and #1 for multicast and AP.
+	MAC_TABLE                 MacTab;     // ASIC on-chip WCID entry table.  At TX, ASIC always use key according to this on-chip table.
+	NDIS_SPIN_LOCK          MacTabLock;
+
+#ifdef DOT11_N_SUPPORT
+	BA_TABLE			BATable;
+#endif // DOT11_N_SUPPORT //
+	NDIS_SPIN_LOCK          BATabLock;
+	RALINK_TIMER_STRUCT RECBATimer;
+
+	// encryption/decryption KEY tables
+	CIPHER_KEY              SharedKey[MAX_MBSSID_NUM][4]; // STA always use SharedKey[BSS0][0..3]
+
+		// RX re-assembly buffer for fragmentation
+	FRAGMENT_FRAME          FragFrame;                  // Frame storage for fragment frame
+
+	// various Counters
+	COUNTER_802_3           Counters8023;               // 802.3 counters
+	COUNTER_802_11          WlanCounters;               // 802.11 MIB counters
+	COUNTER_RALINK          RalinkCounters;             // Ralink propriety counters
+	COUNTER_DRS             DrsCounters;                // counters for Dynamic TX Rate Switching
+	PRIVATE_STRUC           PrivateInfo;                // Private information & counters
+
+	// flags, see fRTMP_ADAPTER_xxx flags
+	ULONG                   Flags;                      // Represent current device status
+
+	// current TX sequence #
+	USHORT                  Sequence;
+
+	// Control disconnect / connect event generation
+	//+++Didn't used anymore
+	ULONG                   LinkDownTime;
+	//---
+	ULONG                   LastRxRate;
+	ULONG                   LastTxRate;
+	//+++Used only for Station
+	BOOLEAN                 bConfigChanged;         // Config Change flag for the same SSID setting
+	//---
+
+	ULONG                   ExtraInfo;              // Extra information for displaying status
+	ULONG                   SystemErrorBitmap;      // b0: E2PROM version error
+
+	//+++Didn't used anymore
+	ULONG                   MacIcVersion;           // MAC/BBP serial interface issue solved after ver.D
+	//---
+
+	// ---------------------------
+	// System event log
+	// ---------------------------
+	RT_802_11_EVENT_TABLE   EventTab;
+
+
+	BOOLEAN		HTCEnable;
+
+	/*****************************************************************************************/
+	/*      Statistic related parameters                                                     */
+	/*****************************************************************************************/
+#ifdef RT2870
+	ULONG						BulkOutDataOneSecCount;
+	ULONG						BulkInDataOneSecCount;
+	ULONG						BulkLastOneSecCount; // BulkOutDataOneSecCount + BulkInDataOneSecCount
+	ULONG						watchDogRxCnt;
+	ULONG						watchDogRxOverFlowCnt;
+	ULONG						watchDogTxPendingCnt[NUM_OF_TX_RING];
+#endif // RT2870 //
+
+	BOOLEAN						bUpdateBcnCntDone;
+	ULONG						watchDogMacDeadlock;	// prevent MAC/BBP into deadlock condition
+	// ----------------------------
+	// DEBUG paramerts
+	// ----------------------------
+	//ULONG		DebugSetting[4];
+	BOOLEAN		bBanAllBaSetup;
+	BOOLEAN		bPromiscuous;
+
+	// ----------------------------
+	// rt2860c emulation-use Parameters
+	// ----------------------------
+	ULONG		rtsaccu[30];
+	ULONG		ctsaccu[30];
+	ULONG		cfendaccu[30];
+	ULONG		bacontent[16];
+	ULONG		rxint[RX_RING_SIZE+1];
+	UCHAR		rcvba[60];
+	BOOLEAN		bLinkAdapt;
+	BOOLEAN		bForcePrintTX;
+	BOOLEAN		bForcePrintRX;
+	BOOLEAN		bDisablescanning;		//defined in RT2870 USB
+	BOOLEAN		bStaFifoTest;
+	BOOLEAN		bProtectionTest;
+	BOOLEAN		bHCCATest;
+	BOOLEAN		bGenOneHCCA;
+	BOOLEAN		bBroadComHT;
+	//+++Following add from RT2870 USB.
+	ULONG		BulkOutReq;
+	ULONG		BulkOutComplete;
+	ULONG		BulkOutCompleteOther;
+	ULONG		BulkOutCompleteCancel;	// seems not use now?
+	ULONG		BulkInReq;
+	ULONG		BulkInComplete;
+	ULONG		BulkInCompleteFail;
+	//---
+
+    struct wificonf			WIFItestbed;
+
+#ifdef RALINK_ATE
+	ATE_INFO				ate;
+#ifdef RT2870
+	BOOLEAN					ContinBulkOut;		//ATE bulk out control
+	BOOLEAN					ContinBulkIn;		//ATE bulk in control
+	atomic_t				BulkOutRemained;
+	atomic_t				BulkInRemained;
+#endif // RT2870 //
+#endif // RALINK_ATE //
+
+#ifdef DOT11_N_SUPPORT
+	struct reordering_mpdu_pool mpdu_blk_pool;
+#endif // DOT11_N_SUPPORT //
+
+	ULONG					OneSecondnonBEpackets;		// record non BE packets per second
+
+#if WIRELESS_EXT >= 12
+    struct iw_statistics    iw_stats;
+#endif
+
+	struct net_device_stats	stats;
+
+#ifdef BLOCK_NET_IF
+	BLOCK_QUEUE_ENTRY		blockQueueTab[NUM_OF_TX_RING];
+#endif // BLOCK_NET_IF //
+
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+	INT32					MC_RowID;
+	UCHAR					MC_FileName[256];
+#endif // MULTIPLE_CARD_SUPPORT //
+
+	ULONG					TbttTickCount;
+#ifdef PCI_MSI_SUPPORT
+	BOOLEAN					HaveMsi;
+#endif // PCI_MSI_SUPPORT //
+
+
+	UCHAR					is_on;
+
+#define TIME_BASE			(1000000/OS_HZ)
+#define TIME_ONE_SECOND		(1000000/TIME_BASE)
+	UCHAR					flg_be_adjust;
+	ULONG					be_adjust_last_time;
+
+
+#ifdef IKANOS_VX_1X0
+	struct IKANOS_TX_INFO	IkanosTxInfo;
+	struct IKANOS_TX_INFO	IkanosRxInfo[MAX_MBSSID_NUM + MAX_WDS_ENTRY + MAX_APCLI_NUM + MAX_MESH_NUM];
+#endif // IKANOS_VX_1X0 //
+
+
+#ifdef DBG_DIAGNOSE
+	RtmpDiagStruct	DiagStruct;
+#endif // DBG_DIAGNOSE //
+
+
+	UINT8					PM_FlgSuspend;
+} RTMP_ADAPTER, *PRTMP_ADAPTER;
+
+//
+// Cisco IAPP format
+//
+typedef struct  _CISCO_IAPP_CONTENT_
+{
+	USHORT     Length;        //IAPP Length
+	UCHAR      MessageType;      //IAPP type
+	UCHAR      FunctionCode;     //IAPP function type
+	UCHAR      DestinaionMAC[MAC_ADDR_LEN];
+	UCHAR      SourceMAC[MAC_ADDR_LEN];
+	USHORT     Tag;           //Tag(element IE) - Adjacent AP report
+	USHORT     TagLength;     //Length of element not including 4 byte header
+	UCHAR      OUI[4];           //0x00, 0x40, 0x96, 0x00
+	UCHAR      PreviousAP[MAC_ADDR_LEN];       //MAC Address of access point
+	USHORT     Channel;
+	USHORT     SsidLen;
+	UCHAR      Ssid[MAX_LEN_OF_SSID];
+	USHORT     Seconds;          //Seconds that the client has been disassociated.
+} CISCO_IAPP_CONTENT, *PCISCO_IAPP_CONTENT;
+
+#define DELAYINTMASK		0x0003fffb
+#define INTMASK				0x0003fffb
+#define IndMask				0x0003fffc
+#define RxINT				0x00000005	// Delayed Rx or indivi rx
+#define TxDataInt			0x000000fa	// Delayed Tx or indivi tx
+#define TxMgmtInt			0x00000102	// Delayed Tx or indivi tx
+#define TxCoherent			0x00020000	// tx coherent
+#define RxCoherent			0x00010000	// rx coherent
+#define McuCommand			0x00000200	// mcu
+#define PreTBTTInt			0x00001000	// Pre-TBTT interrupt
+#define TBTTInt				0x00000800		// TBTT interrupt
+#define GPTimeOutInt			0x00008000		// GPtimeout interrupt
+#define AutoWakeupInt		0x00004000		// AutoWakeupInt interrupt
+#define FifoStaFullInt			0x00002000	//  fifo statistics full interrupt
+
+
+typedef struct _RX_BLK_
+{
+//	RXD_STRUC		RxD; // sample
+	RT28XX_RXD_STRUC	RxD;
+	PRXWI_STRUC			pRxWI;
+	PHEADER_802_11		pHeader;
+	PNDIS_PACKET		pRxPacket;
+	UCHAR				*pData;
+	USHORT				DataSize;
+	USHORT				Flags;
+	UCHAR				UserPriority;	// for calculate TKIP MIC using
+} RX_BLK;
+
+
+#define RX_BLK_SET_FLAG(_pRxBlk, _flag)		(_pRxBlk->Flags |= _flag)
+#define RX_BLK_TEST_FLAG(_pRxBlk, _flag)	(_pRxBlk->Flags & _flag)
+#define RX_BLK_CLEAR_FLAG(_pRxBlk, _flag)	(_pRxBlk->Flags &= ~(_flag))
+
+
+#define fRX_WDS			0x0001
+#define fRX_AMSDU       0x0002
+#define fRX_ARALINK     0x0004
+#define fRX_HTC         0x0008
+#define fRX_PAD         0x0010
+#define fRX_AMPDU       0x0020
+#define fRX_QOS			0x0040
+#define fRX_INFRA		0x0080
+#define fRX_EAP			0x0100
+#define fRX_MESH		0x0200
+#define fRX_APCLI		0x0400
+#define fRX_DLS			0x0800
+#define fRX_WPI			0x1000
+
+#define LENGTH_AMSDU_SUBFRAMEHEAD	14
+#define LENGTH_ARALINK_SUBFRAMEHEAD	14
+#define LENGTH_ARALINK_HEADER_FIELD	 2
+
+#define TX_UNKOWN_FRAME			0x00
+#define TX_MCAST_FRAME			0x01
+#define TX_LEGACY_FRAME			0x02
+#define TX_AMPDU_FRAME			0x04
+#define TX_AMSDU_FRAME			0x08
+#define TX_RALINK_FRAME			0x10
+#define TX_FRAG_FRAME			0x20
+
+
+//	Currently the sizeof(TX_BLK) is 148 bytes.
+typedef struct _TX_BLK_
+{
+	UCHAR				QueIdx;
+	UCHAR				TxFrameType;				// Indicate the Transmission type of the all frames in one batch
+	UCHAR				TotalFrameNum;				// Total frame number want to send-out in one batch
+	USHORT				TotalFragNum;				// Total frame fragments required in one batch
+	USHORT				TotalFrameLen;				// Total length of all frames want to send-out in one batch
+
+	QUEUE_HEADER		TxPacketList;
+	MAC_TABLE_ENTRY		*pMacEntry;					// NULL: packet with 802.11 RA field is multicast/broadcast address
+	HTTRANSMIT_SETTING	*pTransmit;
+
+	// Following structure used for the characteristics of a specific packet.
+	PNDIS_PACKET		pPacket;
+	PUCHAR				pSrcBufHeader;				// Reference to the head of sk_buff->data
+	PUCHAR				pSrcBufData;				// Reference to the sk_buff->data, will changed depends on hanlding progresss
+	UINT				SrcBufLen;					// Length of packet payload which not including Layer 2 header
+	PUCHAR				pExtraLlcSnapEncap;			// NULL means no extra LLC/SNAP is required
+	UCHAR				HeaderBuf[80];				// TempBuffer for TX_INFO + TX_WI + 802.11 Header + padding + AMSDU SubHeader + LLC/SNAP
+	UCHAR				MpduHeaderLen;				// 802.11 header length NOT including the padding
+	UCHAR				HdrPadLen;					// recording Header Padding Length;
+	UCHAR				apidx;						// The interface associated to this packet
+	UCHAR				Wcid;						// The MAC entry associated to this packet
+	UCHAR				UserPriority;				// priority class of packet
+	UCHAR				FrameGap;					// what kind of IFS this packet use
+	UCHAR				MpduReqNum;					// number of fragments of this frame
+	UCHAR				TxRate;						// TODO: Obsoleted? Should change to MCS?
+	UCHAR				CipherAlg;					// cipher alogrithm
+	PCIPHER_KEY			pKey;
+
+
+
+	USHORT				Flags;						//See following definitions for detail.
+
+	//YOU SHOULD NOT TOUCH IT! Following parameters are used for hardware-depended layer.
+	ULONG				Priv;						// Hardware specific value saved in here.
+} TX_BLK, *PTX_BLK;
+
+
+#define fTX_bRtsRequired		0x0001	// Indicate if need send RTS frame for protection. Not used in RT2860/RT2870.
+#define fTX_bAckRequired       	0x0002	// the packet need ack response
+#define fTX_bPiggyBack     		0x0004	// Legacy device use Piggback or not
+#define fTX_bHTRate         	0x0008	// allow to use HT rate
+//#define fTX_bForceLowRate       0x0010	// force to use Low Rate
+#define fTX_bForceNonQoS       	0x0010	// force to transmit frame without WMM-QoS in HT mode
+#define fTX_bAllowFrag       	0x0020	// allow to fragment the packet, A-MPDU, A-MSDU, A-Ralink is not allowed to fragment
+#define fTX_bMoreData			0x0040	// there are more data packets in PowerSave Queue
+#define fTX_bWMM				0x0080	// QOS Data
+
+#define fTX_bClearEAPFrame		0x0100
+
+
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+#define TX_BLK_ASSIGN_FLAG(_pTxBlk, _flag, value)	\
+		do {										\
+			if (value) 								\
+				(_pTxBlk->Flags |= _flag) 			\
+			else 									\
+				(_pTxBlk->Flags &= ~(_flag))		\
+		}while(0)
+
+#define TX_BLK_SET_FLAG(_pTxBlk, _flag)		(_pTxBlk->Flags |= _flag)
+#define TX_BLK_TEST_FLAG(_pTxBlk, _flag)	(((_pTxBlk->Flags & _flag) == _flag) ? 1 : 0)
+#define TX_BLK_CLEAR_FLAG(_pTxBlk, _flag)	(_pTxBlk->Flags &= ~(_flag))
+
+
+
+
+
+//------------------------------------------------------------------------------------------
+
+
+
+#ifdef RT_BIG_ENDIAN
+static inline VOID	WriteBackToDescriptor(
+	IN  PUCHAR			Dest,
+ 	IN	PUCHAR			Src,
+    IN  BOOLEAN			DoEncrypt,
+	IN  ULONG           DescriptorType)
+{
+	UINT32 *p1, *p2;
+
+	p1 = ((UINT32 *)Dest);
+	p2 = ((UINT32 *)Src);
+
+	*p1 = *p2;
+	*(p1+2) = *(p2+2);
+	*(p1+3) = *(p2+3);
+	*(p1+1) = *(p2+1); // Word 1; this must be written back last
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Endian conversion of Tx/Rx descriptor .
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pData			Pointer to Tx/Rx descriptor
+		DescriptorType	Direction of the frame
+
+	Return Value:
+		None
+
+	Note:
+		Call this function when read or update descriptor
+	========================================================================
+*/
+static inline VOID	RTMPWIEndianChange(
+	IN	PUCHAR			pData,
+	IN	ULONG			DescriptorType)
+{
+	int size;
+	int i;
+
+	size = ((DescriptorType == TYPE_TXWI) ? TXWI_SIZE : RXWI_SIZE);
+
+	if(DescriptorType == TYPE_TXWI)
+	{
+		*((UINT32 *)(pData)) = SWAP32(*((UINT32 *)(pData)));		// Byte 0~3
+		*((UINT32 *)(pData + 4)) = SWAP32(*((UINT32 *)(pData+4)));	// Byte 4~7
+	}
+	else
+	{
+		for(i=0; i < size/4 ; i++)
+			*(((UINT32 *)pData) +i) = SWAP32(*(((UINT32 *)pData)+i));
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Endian conversion of Tx/Rx descriptor .
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pData			Pointer to Tx/Rx descriptor
+		DescriptorType	Direction of the frame
+
+	Return Value:
+		None
+
+	Note:
+		Call this function when read or update descriptor
+	========================================================================
+*/
+
+#ifdef RT2870
+static inline VOID	RTMPDescriptorEndianChange(
+	IN	PUCHAR			pData,
+	IN	ULONG			DescriptorType)
+{
+	*((UINT32 *)(pData)) = SWAP32(*((UINT32 *)(pData)));
+}
+#endif // RT2870 //
+/*
+	========================================================================
+
+	Routine Description:
+		Endian conversion of all kinds of 802.11 frames .
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pData			Pointer to the 802.11 frame structure
+		Dir 			Direction of the frame
+		FromRxDoneInt	Caller is from RxDone interrupt
+
+	Return Value:
+		None
+
+	Note:
+		Call this function when read or update buffer data
+	========================================================================
+*/
+static inline VOID	RTMPFrameEndianChange(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pData,
+	IN	ULONG			Dir,
+	IN	BOOLEAN 		FromRxDoneInt)
+{
+	PHEADER_802_11 pFrame;
+	PUCHAR	pMacHdr;
+
+	// swab 16 bit fields - Frame Control field
+	if(Dir == DIR_READ)
+	{
+		*(USHORT *)pData = SWAP16(*(USHORT *)pData);
+	}
+
+	pFrame = (PHEADER_802_11) pData;
+	pMacHdr = (PUCHAR) pFrame;
+
+	// swab 16 bit fields - Duration/ID field
+	*(USHORT *)(pMacHdr + 2) = SWAP16(*(USHORT *)(pMacHdr + 2));
+
+	// swab 16 bit fields - Sequence Control field
+	*(USHORT *)(pMacHdr + 22) = SWAP16(*(USHORT *)(pMacHdr + 22));
+
+	if(pFrame->FC.Type == BTYPE_MGMT)
+	{
+		switch(pFrame->FC.SubType)
+		{
+			case SUBTYPE_ASSOC_REQ:
+			case SUBTYPE_REASSOC_REQ:
+				// swab 16 bit fields - CapabilityInfo field
+				pMacHdr += sizeof(HEADER_802_11);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - Listen Interval field
+				pMacHdr += 2;
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+
+			case SUBTYPE_ASSOC_RSP:
+			case SUBTYPE_REASSOC_RSP:
+				// swab 16 bit fields - CapabilityInfo field
+				pMacHdr += sizeof(HEADER_802_11);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - Status Code field
+				pMacHdr += 2;
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - AID field
+				pMacHdr += 2;
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+
+			case SUBTYPE_AUTH:
+				// If from APHandleRxDoneInterrupt routine, it is still a encrypt format.
+				// The convertion is delayed to RTMPHandleDecryptionDoneInterrupt.
+				if(!FromRxDoneInt && pFrame->FC.Wep == 1)
+					break;
+				else
+				{
+					// swab 16 bit fields - Auth Alg No. field
+					pMacHdr += sizeof(HEADER_802_11);
+					*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+					// swab 16 bit fields - Auth Seq No. field
+					pMacHdr += 2;
+					*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+					// swab 16 bit fields - Status Code field
+					pMacHdr += 2;
+					*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				}
+				break;
+
+			case SUBTYPE_BEACON:
+			case SUBTYPE_PROBE_RSP:
+				// swab 16 bit fields - BeaconInterval field
+				pMacHdr += (sizeof(HEADER_802_11) + TIMESTAMP_LEN);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - CapabilityInfo field
+				pMacHdr += sizeof(USHORT);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+
+			case SUBTYPE_DEAUTH:
+			case SUBTYPE_DISASSOC:
+				// swab 16 bit fields - Reason code field
+				pMacHdr += sizeof(HEADER_802_11);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+		}
+	}
+	else if( pFrame->FC.Type == BTYPE_DATA )
+	{
+	}
+	else if(pFrame->FC.Type == BTYPE_CNTL)
+	{
+		switch(pFrame->FC.SubType)
+		{
+			case SUBTYPE_BLOCK_ACK_REQ:
+				{
+					PFRAME_BA_REQ pBAReq = (PFRAME_BA_REQ)pFrame;
+					*(USHORT *)(&pBAReq->BARControl) = SWAP16(*(USHORT *)(&pBAReq->BARControl));
+					pBAReq->BAStartingSeq.word = SWAP16(pBAReq->BAStartingSeq.word);
+				}
+				break;
+			case SUBTYPE_BLOCK_ACK:
+				// For Block Ack packet, the HT_CONTROL field is in the same offset with Addr3
+				*(UINT32 *)(&pFrame->Addr3[0]) = SWAP32(*(UINT32 *)(&pFrame->Addr3[0]));
+				break;
+
+			case SUBTYPE_ACK:
+				//For ACK packet, the HT_CONTROL field is in the same offset with Addr2
+				*(UINT32 *)(&pFrame->Addr2[0])=	SWAP32(*(UINT32 *)(&pFrame->Addr2[0]));
+				break;
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("Invalid Frame Type!!!\n"));
+	}
+
+	// swab 16 bit fields - Frame Control
+	if(Dir == DIR_WRITE)
+	{
+		*(USHORT *)pData = SWAP16(*(USHORT *)pData);
+	}
+}
+#endif // RT_BIG_ENDIAN //
+
+
+static inline VOID ConvertMulticastIP2MAC(
+	IN PUCHAR pIpAddr,
+	IN PUCHAR *ppMacAddr,
+	IN UINT16 ProtoType)
+{
+	if (pIpAddr == NULL)
+		return;
+
+	if (ppMacAddr == NULL || *ppMacAddr == NULL)
+		return;
+
+	switch (ProtoType)
+	{
+		case ETH_P_IPV6:
+//			memset(*ppMacAddr, 0, ETH_LENGTH_OF_ADDRESS);
+			*(*ppMacAddr) = 0x33;
+			*(*ppMacAddr + 1) = 0x33;
+			*(*ppMacAddr + 2) = pIpAddr[12];
+			*(*ppMacAddr + 3) = pIpAddr[13];
+			*(*ppMacAddr + 4) = pIpAddr[14];
+			*(*ppMacAddr + 5) = pIpAddr[15];
+			break;
+
+		case ETH_P_IP:
+		default:
+//			memset(*ppMacAddr, 0, ETH_LENGTH_OF_ADDRESS);
+			*(*ppMacAddr) = 0x01;
+			*(*ppMacAddr + 1) = 0x00;
+			*(*ppMacAddr + 2) = 0x5e;
+			*(*ppMacAddr + 3) = pIpAddr[1] & 0x7f;
+			*(*ppMacAddr + 4) = pIpAddr[2];
+			*(*ppMacAddr + 5) = pIpAddr[3];
+			break;
+	}
+
+	return;
+}
+
+BOOLEAN RTMPCheckForHang(
+	IN  NDIS_HANDLE MiniportAdapterContext
+	);
+
+VOID  RTMPHalt(
+	IN  NDIS_HANDLE MiniportAdapterContext
+	);
+
+//
+//  Private routines in rtmp_init.c
+//
+NDIS_STATUS RTMPAllocAdapterBlock(
+	IN PVOID			handle,
+	OUT PRTMP_ADAPTER   *ppAdapter
+	);
+
+NDIS_STATUS RTMPAllocTxRxRingMemory(
+	IN  PRTMP_ADAPTER   pAd
+	);
+
+NDIS_STATUS RTMPFindAdapter(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  NDIS_HANDLE     WrapperConfigurationContext
+	);
+
+NDIS_STATUS	RTMPReadParametersHook(
+	IN	PRTMP_ADAPTER pAd
+	);
+
+VOID RTMPFreeAdapter(
+	IN  PRTMP_ADAPTER   pAd
+	);
+
+NDIS_STATUS NICReadRegParameters(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  NDIS_HANDLE         WrapperConfigurationContext
+	);
+
+#ifdef RT2870
+VOID NICInitRT30xxRFRegisters(
+	IN PRTMP_ADAPTER pAd);
+#endif // RT2870 //
+
+VOID NICReadEEPROMParameters(
+	IN  PRTMP_ADAPTER       pAd,
+	IN	PUCHAR				mac_addr);
+
+VOID NICInitAsicFromEEPROM(
+	IN  PRTMP_ADAPTER       pAd);
+
+VOID NICInitTxRxRingAndBacklogQueue(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS NICInitializeAdapter(
+	IN  PRTMP_ADAPTER   pAd,
+	IN   BOOLEAN    bHardReset);
+
+NDIS_STATUS NICInitializeAsic(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  BOOLEAN		bHardReset);
+
+VOID NICIssueReset(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPRingCleanUp(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           RingType);
+
+VOID RxTest(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS DbgSendPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket);
+
+VOID UserCfgInit(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID NICResetFromError(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID NICEraseFirmware(
+	IN PRTMP_ADAPTER pAd);
+
+NDIS_STATUS NICLoadFirmware(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS NICLoadRateSwitchingParams(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN NICCheckForHang(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID NICUpdateFifoStaCounters(
+	IN PRTMP_ADAPTER pAd);
+
+VOID NICUpdateRawCounters(
+	IN  PRTMP_ADAPTER   pAd);
+
+#if 0
+ULONG RTMPEqualMemory(
+	IN  PVOID   pSrc1,
+	IN  PVOID   pSrc2,
+	IN  ULONG   Length);
+#endif
+
+ULONG	RTMPNotAllZero(
+	IN	PVOID	pSrc1,
+	IN	ULONG	Length);
+
+VOID RTMPZeroMemory(
+	IN  PVOID   pSrc,
+	IN  ULONG   Length);
+
+ULONG RTMPCompareMemory(
+	IN  PVOID   pSrc1,
+	IN  PVOID   pSrc2,
+	IN  ULONG   Length);
+
+VOID RTMPMoveMemory(
+	OUT PVOID   pDest,
+	IN  PVOID   pSrc,
+	IN  ULONG   Length);
+
+VOID AtoH(
+	char	*src,
+	UCHAR	*dest,
+	int		destlen);
+
+UCHAR BtoH(
+	char ch);
+
+VOID RTMPPatchMacBbpBug(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPPatchCardBus(
+	IN	PRTMP_ADAPTER	pAdapter);
+
+VOID RTMPPatchRalinkCardBus(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	ULONG			Bus);
+
+ULONG RTMPReadCBConfig(
+	IN	ULONG	Bus,
+	IN	ULONG	Slot,
+	IN	ULONG	Func,
+	IN	ULONG	Offset);
+
+VOID RTMPWriteCBConfig(
+	IN	ULONG	Bus,
+	IN	ULONG	Slot,
+	IN	ULONG	Func,
+	IN	ULONG	Offset,
+	IN	ULONG	Value);
+
+VOID RTMPInitTimer(
+	IN  PRTMP_ADAPTER           pAd,
+	IN  PRALINK_TIMER_STRUCT    pTimer,
+	IN  PVOID                   pTimerFunc,
+	IN	PVOID					pData,
+	IN  BOOLEAN                 Repeat);
+
+VOID RTMPSetTimer(
+	IN  PRALINK_TIMER_STRUCT    pTimer,
+	IN  ULONG                   Value);
+
+
+VOID RTMPModTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	ULONG					Value);
+
+VOID RTMPCancelTimer(
+	IN  PRALINK_TIMER_STRUCT    pTimer,
+	OUT BOOLEAN                 *pCancelled);
+
+VOID RTMPSetLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN UCHAR			Status);
+
+VOID RTMPSetSignalLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN NDIS_802_11_RSSI Dbm);
+
+VOID RTMPEnableRxTx(
+	IN PRTMP_ADAPTER	pAd);
+
+//
+// prototype in action.c
+//
+VOID ActionStateMachineInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  STATE_MACHINE *S,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID MlmeADDBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeDELBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeDLSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeInvalidAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeQOSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef DOT11_N_SUPPORT
+VOID PeerAddBAReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAddBARspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDelBAAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+#endif // DOT11_N_SUPPORT //
+
+VOID SendPSMPAction(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			Wcid,
+	IN UCHAR			Psmp);
+
+
+#ifdef DOT11N_DRAFT3
+VOID SendBSS2040CoexistMgmtAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	Wcid,
+	IN	UCHAR	apidx,
+	IN	UCHAR	InfoReq);
+
+VOID SendNotifyBWActionFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR  Wcid,
+	IN UCHAR apidx);
+
+BOOLEAN ChannelSwitchSanityCheck(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  NewChannel,
+	IN    UCHAR  Secondary);
+
+VOID ChannelSwitchAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  Channel,
+	IN    UCHAR  Secondary);
+
+ULONG BuildIntolerantChannelRep(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    PUCHAR  pDest);
+
+VOID Update2040CoexistFrameAndNotify(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha);
+
+VOID Send2040CoexistAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha);
+#endif // DOT11N_DRAFT3 //
+
+VOID PeerRMAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID StaPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Bss2040Coexist);
+#endif // CONFIG_STA_SUPPORT //
+
+
+VOID PeerBSSTranAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef DOT11_N_SUPPORT
+VOID PeerHTAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+#endif // DOT11_N_SUPPORT //
+
+VOID PeerQOSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef QOS_DLS_SUPPORT
+VOID PeerDLSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+VOID DlsParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_DLS_REQ_STRUCT *pDlsReq,
+	IN PRT_802_11_DLS pDls,
+	IN USHORT reason);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+VOID RECBATimerTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID ORIBATimerTimeout(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID SendRefreshBAR(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry);
+#endif // DOT11_N_SUPPORT //
+
+VOID ActHeaderInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN OUT PHEADER_802_11 pHdr80211,
+    IN PUCHAR Addr1,
+    IN PUCHAR Addr2,
+    IN PUCHAR Addr3);
+
+VOID BarHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PFRAME_BAR pCntlBar,
+	IN PUCHAR pDA,
+	IN PUCHAR pSA);
+
+VOID InsertActField(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 Category,
+	IN UINT8 ActCode);
+
+BOOLEAN QosBADataParse(
+	IN PRTMP_ADAPTER	pAd,
+	IN BOOLEAN bAMSDU,
+	IN PUCHAR p8023Header,
+	IN UCHAR	WCID,
+	IN UCHAR	TID,
+	IN USHORT Sequence,
+	IN UCHAR DataOffset,
+	IN USHORT Datasize,
+	IN UINT   CurRxIndex);
+
+#ifdef DOT11_N_SUPPORT
+BOOLEAN CntlEnqueueForRecv(
+    IN	PRTMP_ADAPTER	pAd,
+	IN ULONG Wcid,
+    IN ULONG MsgLen,
+	IN PFRAME_BA_REQ pMsg);
+
+VOID BaAutoManSwitch(
+	IN	PRTMP_ADAPTER	pAd);
+#endif // DOT11_N_SUPPORT //
+
+VOID HTIOTCheck(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR     BatRecIdx);
+
+//
+// Private routines in rtmp_data.c
+//
+BOOLEAN RTMPHandleRxDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPHandleTxDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN RTMPHandleTxRingDmaDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  INT_SOURCE_CSR_STRUC TxRingBitmap);
+
+VOID RTMPHandleMgmtRingDmaDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPHandleTBTTInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPHandlePreTBTTInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+void RTMPHandleTwakeupInterrupt(
+	IN PRTMP_ADAPTER pAd);
+
+VOID	RTMPHandleRxCoherentInterrupt(
+	IN	PRTMP_ADAPTER	pAd);
+
+BOOLEAN TxFrameIsAggregatible(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pPrevAddr1,
+	IN  PUCHAR          p8023hdr);
+
+BOOLEAN PeerIsAggreOn(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  ULONG          TxRate,
+    IN  PMAC_TABLE_ENTRY pMacEntry);
+
+#if 0	// It's not be used
+HTTRANSMIT_SETTING  *GetTxMode(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk);
+#endif
+
+NDIS_STATUS Sniff2BytesFromNdisBuffer(
+	IN  PNDIS_BUFFER    pFirstBuffer,
+	IN  UCHAR           DesiredOffset,
+	OUT PUCHAR          pByte0,
+	OUT PUCHAR          pByte1);
+
+NDIS_STATUS STASendPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket);
+
+VOID STASendPackets(
+	IN  NDIS_HANDLE     MiniportAdapterContext,
+	IN  PPNDIS_PACKET   ppPacketArray,
+	IN  UINT            NumberOfPackets);
+
+VOID RTMPDeQueuePacket(
+	IN  PRTMP_ADAPTER   pAd,
+   	IN	BOOLEAN			bIntContext,
+	IN  UCHAR			QueIdx,
+	IN	UCHAR			Max_Tx_Packets);
+
+NDIS_STATUS	RTMPHardTransmit(
+	IN PRTMP_ADAPTER	pAd,
+	IN PNDIS_PACKET		pPacket,
+	IN  UCHAR			QueIdx,
+	OUT	PULONG			pFreeTXDLeft);
+
+NDIS_STATUS	STAHardTransmit(
+	IN PRTMP_ADAPTER	pAd,
+	IN TX_BLK			*pTxBlk,
+	IN  UCHAR			QueIdx);
+
+VOID STARxEAPOLFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+NDIS_STATUS RTMPFreeTXDRequest(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           RingType,
+	IN  UCHAR           NumberRequired,
+	IN 	PUCHAR          FreeNumberIs);
+
+NDIS_STATUS MlmeHardTransmit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR	QueIdx,
+	IN  PNDIS_PACKET    pPacket);
+
+NDIS_STATUS MlmeHardTransmitMgmtRing(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR	QueIdx,
+	IN  PNDIS_PACKET    pPacket);
+
+NDIS_STATUS MlmeHardTransmitTxRing(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR	QueIdx,
+	IN  PNDIS_PACKET    pPacket);
+
+USHORT  RTMPCalcDuration(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Rate,
+	IN  ULONG           Size);
+
+VOID RTMPWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC		pTxWI,
+	IN  BOOLEAN    		FRAG,
+	IN  BOOLEAN    		CFACK,
+	IN  BOOLEAN    		InsTimestamp,
+	IN	BOOLEAN			AMPDU,
+	IN	BOOLEAN			Ack,
+	IN	BOOLEAN			NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN  UCHAR      		PID,
+	IN	UCHAR			TID,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	*pTransmit);
+
+
+VOID RTMPWriteTxWI_Data(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk);
+
+
+VOID RTMPWriteTxWI_Cache(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk);
+
+VOID RTMPWriteTxDescriptor(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXD_STRUC		pTxD,
+	IN	BOOLEAN			bWIV,
+	IN	UCHAR			QSEL);
+
+VOID RTMPSuspendMsduTransmission(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPResumeMsduTransmission(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS MiniportMMRequest(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			QueIdx,
+	IN	PUCHAR			pData,
+	IN  UINT            Length);
+
+NDIS_STATUS MiniportDataMMRequest(
+	 IN  PRTMP_ADAPTER   pAd,
+	 IN  UCHAR           QueIdx,
+	 IN  PUCHAR          pData,
+	 IN  UINT            Length);
+
+VOID RTMPSendNullFrame(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           TxRate,
+	IN	BOOLEAN			bQosNull);
+
+VOID RTMPSendDisassociationFrame(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTMPSendRTSFrame(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pDA,
+	IN	unsigned int	NextMpduSize,
+	IN  UCHAR           TxRate,
+	IN  UCHAR           RTSRate,
+	IN  USHORT          AckDuration,
+	IN  UCHAR           QueIdx,
+	IN  UCHAR			FrameGap);
+
+
+NDIS_STATUS RTMPApplyPacketFilter(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PRT28XX_RXD_STRUC      pRxD,
+	IN  PHEADER_802_11  pHeader);
+
+PQUEUE_HEADER   RTMPCheckTxSwQueue(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT UCHAR           *QueIdx);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPReportMicError(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PCIPHER_KEY     pWpaKey);
+
+VOID	WpaMicFailureReportFrame(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaDisassocApAndBlockAssoc(
+    IN  PVOID SystemSpecific1,
+    IN  PVOID FunctionContext,
+    IN  PVOID SystemSpecific2,
+    IN  PVOID SystemSpecific3);
+#endif // CONFIG_STA_SUPPORT //
+
+NDIS_STATUS RTMPCloneNdisPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	BOOLEAN    pInsAMSDUHdr,
+	IN  PNDIS_PACKET    pInPacket,
+	OUT PNDIS_PACKET   *ppOutPacket);
+
+NDIS_STATUS RTMPAllocateNdisPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    *pPacket,
+	IN  PUCHAR          pHeader,
+	IN  UINT            HeaderLen,
+	IN  PUCHAR          pData,
+	IN  UINT            DataLen);
+
+VOID RTMPFreeNdisPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket);
+
+BOOLEAN RTMPFreeTXDUponTxDmaDone(
+	IN PRTMP_ADAPTER    pAd,
+	IN UCHAR            QueIdx);
+
+BOOLEAN RTMPCheckDHCPFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+BOOLEAN RTMPCheckEtherType(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+VOID RTMPCckBbpTuning(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UINT			TxRate);
+
+//
+// Private routines in rtmp_wep.c
+//
+VOID RTMPInitWepEngine(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKey,
+	IN  UCHAR           KeyId,
+	IN  UCHAR           KeyLen,
+	IN  PUCHAR          pDest);
+
+VOID RTMPEncryptData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pSrc,
+	IN  PUCHAR          pDest,
+	IN  UINT            Len);
+
+BOOLEAN	RTMPDecryptData(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len,
+	IN	UINT			idx);
+
+BOOLEAN	RTMPSoftDecryptWEP(
+	IN PRTMP_ADAPTER 	pAd,
+	IN PUCHAR			pData,
+	IN ULONG			DataByteCnt,
+	IN PCIPHER_KEY		pGroupKey);
+
+VOID RTMPSetICV(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pDest);
+
+VOID ARCFOUR_INIT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pKey,
+	IN  UINT            KeyLen);
+
+UCHAR   ARCFOUR_BYTE(
+	IN  PARCFOURCONTEXT     Ctx);
+
+VOID ARCFOUR_DECRYPT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pDest,
+	IN  PUCHAR          pSrc,
+	IN  UINT            Len);
+
+VOID ARCFOUR_ENCRYPT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pDest,
+	IN  PUCHAR          pSrc,
+	IN  UINT            Len);
+
+VOID WPAARCFOUR_ENCRYPT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pDest,
+	IN  PUCHAR          pSrc,
+	IN  UINT            Len);
+
+UINT RTMP_CALC_FCS32(
+	IN  UINT   Fcs,
+	IN  PUCHAR  Cp,
+	IN  INT     Len);
+
+//
+// MLME routines
+//
+
+// Asic/RF/BBP related functions
+
+VOID AsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd);
+
+VOID 	AsicUpdateProtect(
+	IN		PRTMP_ADAPTER	pAd,
+	IN 		USHORT			OperaionMode,
+	IN 		UCHAR			SetMask,
+	IN		BOOLEAN			bDisableBGProtect,
+	IN		BOOLEAN			bNonGFExist);
+
+VOID AsicSwitchChannel(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			Channel,
+	IN	BOOLEAN			bScan);
+
+VOID AsicLockChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Channel) ;
+
+VOID AsicAntennaSelect(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Channel);
+
+VOID AsicAntennaSetting(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ABGBAND_STATE	BandState);
+
+VOID AsicRfTuningExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID AsicSleepThenAutoWakeup(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT TbttNumToNextWakeUp);
+
+VOID AsicForceSleep(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN    bFromTx);
+#endif // CONFIG_STA_SUPPORT //
+
+VOID AsicSetBssid(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pBssid);
+
+VOID AsicSetMcastWC(
+	IN PRTMP_ADAPTER pAd);
+
+#if 0	// removed by AlbertY
+VOID AsicSetBssidWC(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pBssid);
+#endif
+
+VOID AsicDelWcidTab(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	Wcid);
+
+VOID AsicEnableRDG(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicDisableRDG(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicDisableSync(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicEnableBssSync(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicEnableIbssSync(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicSetEdcaParm(
+	IN PRTMP_ADAPTER pAd,
+	IN PEDCA_PARM    pEdcaParm);
+
+VOID AsicSetSlotTime(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN bUseShortSlotTime);
+
+#if 0
+VOID AsicAddWcidCipherEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 WCID,
+	IN UCHAR		 BssIndex,
+	IN UCHAR		 KeyTable,
+	IN UCHAR		 CipherAlg,
+	IN PUCHAR		 pAddr,
+	IN CIPHER_KEY		 *pCipherKey);
+#endif
+
+VOID AsicAddSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         BssIndex,
+	IN UCHAR         KeyIdx,
+	IN UCHAR         CipherAlg,
+	IN PUCHAR        pKey,
+	IN PUCHAR        pTxMic,
+	IN PUCHAR        pRxMic);
+
+VOID AsicRemoveSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         BssIndex,
+	IN UCHAR         KeyIdx);
+
+VOID AsicUpdateWCIDAttribute(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR        CipherAlg,
+	IN BOOLEAN		bUsePairewiseKeyTable);
+
+VOID AsicUpdateWCIDIVEIV(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN ULONG        uIV,
+	IN ULONG        uEIV);
+
+VOID AsicUpdateRxWCIDTable(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN PUCHAR        pAddr);
+
+VOID AsicAddKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR		KeyIdx,
+	IN PCIPHER_KEY	pCipherKey,
+	IN BOOLEAN		bUsePairewiseKeyTable,
+	IN BOOLEAN		bTxKey);
+
+VOID AsicAddPairwiseKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR		WCID,
+	IN CIPHER_KEY		 *pCipherKey);
+
+VOID AsicRemovePairwiseKeyEntry(
+	IN PRTMP_ADAPTER  pAd,
+	IN UCHAR		 BssIdx,
+	IN UCHAR		 Wcid);
+
+BOOLEAN AsicSendCommandToMcu(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         Command,
+	IN UCHAR         Token,
+	IN UCHAR         Arg0,
+	IN UCHAR         Arg1);
+
+
+VOID MacAddrRandomBssid(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT PUCHAR pAddr);
+
+VOID MgtMacHeaderInit(
+	IN  PRTMP_ADAPTER     pAd,
+	IN OUT PHEADER_802_11 pHdr80211,
+	IN UCHAR SubType,
+	IN UCHAR ToDs,
+	IN PUCHAR pDA,
+	IN PUCHAR pBssid);
+
+VOID MlmeRadioOff(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MlmeRadioOn(
+	IN PRTMP_ADAPTER pAd);
+
+
+VOID BssTableInit(
+	IN BSS_TABLE *Tab);
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableInit(
+	IN PRTMP_ADAPTER pAd,
+    IN BA_TABLE *Tab);
+#endif // DOT11_N_SUPPORT //
+
+ULONG BssTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR pBssid,
+	IN UCHAR Channel);
+
+ULONG BssSsidTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR    pBssid,
+	IN PUCHAR    pSsid,
+	IN UCHAR     SsidLen,
+	IN UCHAR     Channel);
+
+ULONG BssTableSearchWithSSID(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR    Bssid,
+	IN PUCHAR    pSsid,
+	IN UCHAR     SsidLen,
+	IN UCHAR     Channel);
+
+VOID BssTableDeleteEntry(
+	IN OUT  PBSS_TABLE pTab,
+	IN      PUCHAR pBssid,
+	IN      UCHAR Channel);
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableDeleteORIEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		BA_ORI_ENTRY	*pBAORIEntry);
+
+VOID BATableDeleteRECEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		BA_REC_ENTRY	*pBARECEntry);
+
+VOID BATableTearORIEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		UCHAR TID,
+	IN		UCHAR Wcid,
+	IN		BOOLEAN bForceDelete,
+	IN		BOOLEAN ALL);
+
+VOID BATableTearRECEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		UCHAR TID,
+	IN		UCHAR WCID,
+	IN		BOOLEAN ALL);
+#endif // DOT11_N_SUPPORT //
+
+VOID  BssEntrySet(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT PBSS_ENTRY pBss,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN PCF_PARM CfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR Channel,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE);
+
+ULONG  BssTableSetEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT PBSS_TABLE pTab,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN CF_PARM *CfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR Channel,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE);
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableInsertEntry(
+    IN	PRTMP_ADAPTER	pAd,
+	IN USHORT Aid,
+    IN USHORT		TimeOutValue,
+	IN USHORT		StartingSeq,
+    IN UCHAR TID,
+	IN UCHAR BAWinSize,
+	IN UCHAR OriginatorStatus,
+    IN BOOLEAN IsRecipient);
+
+#ifdef DOT11N_DRAFT3
+VOID Bss2040CoexistTimeOut(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+
+VOID  TriEventInit(
+	IN	PRTMP_ADAPTER	pAd);
+
+ULONG TriEventTableSetEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT TRIGGER_EVENT_TAB *Tab,
+	IN PUCHAR pBssid,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			RegClass,
+	IN UCHAR ChannelNo);
+
+VOID TriEventCounterMaintenance(
+	IN	PRTMP_ADAPTER	pAd);
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+VOID BssTableSsidSort(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT BSS_TABLE *OutTab,
+	IN  CHAR Ssid[],
+	IN  UCHAR SsidLen);
+
+VOID  BssTableSortByRssi(
+	IN OUT BSS_TABLE *OutTab);
+
+VOID BssCipherParse(
+	IN OUT  PBSS_ENTRY  pBss);
+
+NDIS_STATUS  MlmeQueueInit(
+	IN MLME_QUEUE *Queue);
+
+VOID  MlmeQueueDestroy(
+	IN MLME_QUEUE *Queue);
+
+BOOLEAN MlmeEnqueue(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Machine,
+	IN ULONG MsgType,
+	IN ULONG MsgLen,
+	IN VOID *Msg);
+
+BOOLEAN MlmeEnqueueForRecv(
+	IN  PRTMP_ADAPTER   pAd,
+	IN ULONG Wcid,
+	IN ULONG TimeStampHigh,
+	IN ULONG TimeStampLow,
+	IN UCHAR Rssi0,
+	IN UCHAR Rssi1,
+	IN UCHAR Rssi2,
+	IN ULONG MsgLen,
+	IN PVOID Msg,
+	IN UCHAR Signal);
+
+
+BOOLEAN MlmeDequeue(
+	IN MLME_QUEUE *Queue,
+	OUT MLME_QUEUE_ELEM **Elem);
+
+VOID    MlmeRestartStateMachine(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN  MlmeQueueEmpty(
+	IN MLME_QUEUE *Queue);
+
+BOOLEAN  MlmeQueueFull(
+	IN MLME_QUEUE *Queue);
+
+BOOLEAN  MsgTypeSubst(
+	IN PRTMP_ADAPTER pAd,
+	IN PFRAME_802_11 pFrame,
+	OUT INT *Machine,
+	OUT INT *MsgType);
+
+VOID StateMachineInit(
+	IN STATE_MACHINE *Sm,
+	IN STATE_MACHINE_FUNC Trans[],
+	IN ULONG StNr,
+	IN ULONG MsgNr,
+	IN STATE_MACHINE_FUNC DefFunc,
+	IN ULONG InitState,
+	IN ULONG Base);
+
+VOID StateMachineSetAction(
+	IN STATE_MACHINE *S,
+	IN ULONG St,
+	ULONG Msg,
+	IN STATE_MACHINE_FUNC F);
+
+VOID StateMachinePerformAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN STATE_MACHINE *S,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID Drop(
+	IN  PRTMP_ADAPTER   pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID AssocStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *Sm,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID ReassocTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID AssocTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID DisassocTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+//----------------------------------------------
+VOID MlmeDisassocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeAssocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeReassocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeDisassocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAssocRspAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerReassocRspAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDisassocAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID DisassocTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID AssocTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  ReassocTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  Cls3errAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pAddr);
+
+VOID SwitchBetweenWepAndCkip(
+	IN PRTMP_ADAPTER pAd);
+
+VOID  InvalidStateWhenAssoc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  InvalidStateWhenReassoc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenDisassociate(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+#ifdef RT2870
+VOID MlmeCntlConfirm(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG MsgType,
+	IN USHORT Msg);
+#endif // RT2870 //
+
+VOID  ComposePsPoll(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID  ComposeNullFrame(
+	IN  PRTMP_ADAPTER pAd);
+
+VOID  AssocPostProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pAddr2,
+	IN  USHORT CapabilityInfo,
+	IN  USHORT Aid,
+	IN  UCHAR SupRate[],
+	IN  UCHAR SupRateLen,
+	IN  UCHAR ExtRate[],
+	IN  UCHAR ExtRateLen,
+	IN PEDCA_PARM pEdcaParm,
+	IN HT_CAPABILITY_IE		*pHtCapability,
+	IN  UCHAR HtCapabilityLen,
+	IN ADD_HT_INFO_IE		*pAddHtInfo);
+
+VOID AuthStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN PSTATE_MACHINE sm,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID AuthTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID MlmeAuthReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAuthRspAtSeq2Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAuthRspAtSeq4Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID AuthTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID Cls2errAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pAddr);
+
+VOID MlmeDeauthReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenAuth(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+//=============================================
+
+VOID AuthRspStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PSTATE_MACHINE Sm,
+	IN  STATE_MACHINE_FUNC Trans[]);
+
+VOID PeerDeauthAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAuthSimpleRspGenAndSend(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PHEADER_802_11  pHdr80211,
+	IN  USHORT Alg,
+	IN  USHORT Seq,
+	IN  USHORT Reason,
+	IN  USHORT Status);
+
+//
+// Private routines in dls.c
+//
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+void DlsStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID MlmeDlsReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDlsReqAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM	*Elem);
+
+VOID PeerDlsRspAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM	*Elem);
+
+VOID MlmeDlsTearDownAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDlsTearDownAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM	*Elem);
+
+VOID RTMPCheckDLSTimeOut(
+	IN PRTMP_ADAPTER	pAd);
+
+BOOLEAN RTMPRcvFrameDLSCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN PHEADER_802_11	pHeader,
+	IN ULONG			Len,
+	IN PRT28XX_RXD_STRUC	pRxD);
+
+INT	RTMPCheckDLSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA);
+
+VOID RTMPSendDLSTearDownFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA);
+
+NDIS_STATUS RTMPSendSTAKeyRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA);
+
+NDIS_STATUS RTMPSendSTAKeyHandShake(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA);
+
+VOID DlsTimeoutAction(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+BOOLEAN MlmeDlsReqSanity(
+	IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PRT_802_11_DLS *pDLS,
+    OUT PUSHORT pReason);
+
+INT Set_DlsEntryInfo_Display_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg);
+
+MAC_TABLE_ENTRY *MacTableInsertDlsEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR	pAddr,
+	IN  UINT	DlsEntryIdx);
+
+BOOLEAN MacTableDeleteDlsEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT wcid,
+	IN PUCHAR pAddr);
+
+MAC_TABLE_ENTRY *DlsEntryTableLookup(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount);
+
+MAC_TABLE_ENTRY *DlsEntryTableLookupByWcid(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	wcid,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount);
+
+INT	Set_DlsAddEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_DlsTearDownEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef QOS_DLS_SUPPORT
+BOOLEAN PeerDlsReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pDlsTimeout,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability);
+
+BOOLEAN PeerDlsRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pStatus,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability);
+
+BOOLEAN PeerDlsTearDownSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pReason);
+#endif // QOS_DLS_SUPPORT //
+
+//========================================
+
+VOID SyncStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *Sm,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID BeaconTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID ScanTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID MlmeScanReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenScan(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenJoin(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenStart(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeacon(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID EnqueueProbeRequest(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN ScanRunning(
+		IN PRTMP_ADAPTER pAd);
+//=========================================
+
+VOID MlmeCntlInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *S,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID MlmeCntlMachinePerformAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *S,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlIdleProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlOidScanProc(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlOidSsidProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM * Elem);
+
+VOID CntlOidRTBssidProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM * Elem);
+
+VOID CntlMlmeRoamingProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM * Elem);
+
+VOID CntlWaitDisassocProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitJoinProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitReassocProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitStartProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitAuthProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitAuthProc2(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitAssocProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+#ifdef QOS_DLS_SUPPORT
+VOID CntlOidDLSSetupProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+#endif // QOS_DLS_SUPPORT //
+
+VOID LinkUp(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR BssType);
+
+VOID LinkDown(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  BOOLEAN         IsReqFromAP);
+
+VOID IterateOnBssTab(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID IterateOnBssTab2(
+	IN  PRTMP_ADAPTER   pAd);;
+
+VOID JoinParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_JOIN_REQ_STRUCT *JoinReq,
+	IN  ULONG BssIdx);
+
+VOID AssocParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN OUT MLME_ASSOC_REQ_STRUCT *AssocReq,
+	IN  PUCHAR pAddr,
+	IN  USHORT CapabilityInfo,
+	IN  ULONG Timeout,
+	IN  USHORT ListenIntv);
+
+VOID ScanParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_SCAN_REQ_STRUCT *ScanReq,
+	IN  CHAR Ssid[],
+	IN  UCHAR SsidLen,
+	IN  UCHAR BssType,
+	IN  UCHAR ScanType);
+
+VOID DisassocParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_DISASSOC_REQ_STRUCT *DisassocReq,
+	IN  PUCHAR pAddr,
+	IN  USHORT Reason);
+
+VOID StartParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_START_REQ_STRUCT *StartReq,
+	IN  CHAR Ssid[],
+	IN  UCHAR SsidLen);
+
+VOID AuthParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_AUTH_REQ_STRUCT *AuthReq,
+	IN  PUCHAR pAddr,
+	IN  USHORT Alg);
+
+VOID EnqueuePsPoll(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID EnqueueBeaconFrame(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeJoinReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeScanReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeStartReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID ScanTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID BeaconTimeoutAtJoinAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeaconAtScanAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeaconAtJoinAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeacon(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerProbeReqAction(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID ScanNextChannel(
+	IN  PRTMP_ADAPTER   pAd);
+
+ULONG MakeIbssBeacon(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID CCXAdjacentAPReport(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN MlmeScanReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT UCHAR *BssType,
+	OUT CHAR ssid[],
+	OUT UCHAR *SsidLen,
+	OUT UCHAR *ScanType);
+
+BOOLEAN PeerBeaconAndProbeRspSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	IN  UCHAR MsgChannel,
+	OUT PUCHAR pAddr2,
+	OUT PUCHAR pBssid,
+	OUT CHAR Ssid[],
+	OUT UCHAR *pSsidLen,
+	OUT UCHAR *pBssType,
+	OUT USHORT *pBeaconPeriod,
+	OUT UCHAR *pChannel,
+	OUT UCHAR *pNewChannel,
+	OUT LARGE_INTEGER *pTimestamp,
+	OUT CF_PARM *pCfParm,
+	OUT USHORT *pAtimWin,
+	OUT USHORT *pCapabilityInfo,
+	OUT UCHAR *pErp,
+	OUT UCHAR *pDtimCount,
+	OUT UCHAR *pDtimPeriod,
+	OUT UCHAR *pBcastFlag,
+	OUT UCHAR *pMessageToMe,
+	OUT UCHAR SupRate[],
+	OUT UCHAR *pSupRateLen,
+	OUT UCHAR ExtRate[],
+	OUT UCHAR *pExtRateLen,
+	OUT	UCHAR *pCkipFlag,
+	OUT	UCHAR *pAironetCellPowerLimit,
+	OUT PEDCA_PARM       pEdcaParm,
+	OUT PQBSS_LOAD_PARM  pQbssLoad,
+	OUT PQOS_CAPABILITY_PARM pQosCapability,
+	OUT ULONG *pRalinkIe,
+	OUT UCHAR		 *pHtCapabilityLen,
+#ifdef CONFIG_STA_SUPPORT
+	OUT UCHAR		 *pPreNHtCapabilityLen,
+#endif // CONFIG_STA_SUPPORT //
+	OUT HT_CAPABILITY_IE *pHtCapability,
+	OUT UCHAR		 *AddHtInfoLen,
+	OUT ADD_HT_INFO_IE *AddHtInfo,
+	OUT UCHAR *NewExtChannel,
+	OUT USHORT *LengthVIE,
+	OUT PNDIS_802_11_VARIABLE_IEs pVIE);
+
+BOOLEAN PeerAddBAReqActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen,
+	OUT PUCHAR pAddr2);
+
+BOOLEAN PeerAddBARspActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen);
+
+BOOLEAN PeerDelBAActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR Wcid,
+    IN VOID *pMsg,
+    IN ULONG MsgLen);
+
+BOOLEAN MlmeAssocReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pApAddr,
+	OUT USHORT *CapabilityInfo,
+	OUT ULONG *Timeout,
+	OUT USHORT *ListenIntv);
+
+BOOLEAN MlmeAuthReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr,
+	OUT ULONG *Timeout,
+	OUT USHORT *Alg);
+
+BOOLEAN MlmeStartReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT CHAR Ssid[],
+	OUT UCHAR *Ssidlen);
+
+BOOLEAN PeerAuthSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr,
+	OUT USHORT *Alg,
+	OUT USHORT *Seq,
+	OUT USHORT *Status,
+	OUT CHAR ChlgText[]);
+
+BOOLEAN PeerAssocRspSanity(
+	IN  PRTMP_ADAPTER   pAd,
+    IN VOID *pMsg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT USHORT *pCapabilityInfo,
+	OUT USHORT *pStatus,
+	OUT USHORT *pAid,
+	OUT UCHAR SupRate[],
+	OUT UCHAR *pSupRateLen,
+	OUT UCHAR ExtRate[],
+	OUT UCHAR *pExtRateLen,
+    OUT HT_CAPABILITY_IE		*pHtCapability,
+    OUT ADD_HT_INFO_IE		*pAddHtInfo,	// AP might use this additional ht info IE
+    OUT UCHAR			*pHtCapabilityLen,
+    OUT UCHAR			*pAddHtInfoLen,
+    OUT UCHAR			*pNewExtChannelOffset,
+	OUT PEDCA_PARM pEdcaParm,
+	OUT UCHAR *pCkipFlag);
+
+BOOLEAN PeerDisassocSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT USHORT *Reason);
+
+BOOLEAN PeerWpaMessageSanity(
+    IN 	PRTMP_ADAPTER 		pAd,
+    IN 	PEAPOL_PACKET 		pMsg,
+    IN 	ULONG 				MsgLen,
+    IN 	UCHAR				MsgType,
+    IN 	MAC_TABLE_ENTRY  	*pEntry);
+
+BOOLEAN PeerDeauthSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT USHORT *Reason);
+
+BOOLEAN PeerProbeReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT CHAR Ssid[],
+	OUT UCHAR *pSsidLen);
+
+BOOLEAN GetTimBit(
+	IN  CHAR *Ptr,
+	IN  USHORT Aid,
+	OUT UCHAR *TimLen,
+	OUT UCHAR *BcastFlag,
+	OUT UCHAR *DtimCount,
+	OUT UCHAR *DtimPeriod,
+	OUT UCHAR *MessageToMe);
+
+UCHAR ChannelSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR channel);
+
+NDIS_802_11_NETWORK_TYPE NetworkTypeInUseSanity(
+	IN PBSS_ENTRY pBss);
+
+#if 0	// It's omitted
+NDIS_STATUS	RTMPWepKeySanity(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PVOID			pBuf);
+#endif
+
+BOOLEAN MlmeDelBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen);
+
+BOOLEAN MlmeAddBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2);
+
+ULONG MakeOutgoingFrame(
+	OUT CHAR *Buffer,
+	OUT ULONG *Length, ...);
+
+VOID  LfsrInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG Seed);
+
+UCHAR RandomByte(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicUpdateAutoFallBackTable(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pTxRate);
+
+VOID  MlmePeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID LinkDownExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID LinkUpExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID STAMlmePeriodicExec(
+	PRTMP_ADAPTER pAd);
+
+VOID MlmeAutoScan(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MlmeAutoReconnectLastSSID(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN MlmeValidateSSID(
+	IN PUCHAR pSsid,
+	IN UCHAR  SsidLen);
+
+VOID MlmeCheckForRoaming(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG    Now32);
+
+VOID MlmeCheckForFastRoaming(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG           Now);
+
+VOID MlmeDynamicTxRateSwitching(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MlmeSetTxRate(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PRTMP_TX_RATE_SWITCH	pTxRate);
+
+VOID MlmeSelectTxRateTable(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PUCHAR				*ppTable,
+	IN PUCHAR				pTableSize,
+	IN PUCHAR				pInitTxRateIdx);
+
+VOID MlmeCalculateChannelQuality(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Now);
+
+VOID MlmeCheckPsmChange(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG    Now32);
+
+VOID MlmeSetPsmBit(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT psm);
+
+VOID MlmeSetTxPreamble(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TxPreamble);
+
+VOID UpdateBasicRateBitmap(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID MlmeUpdateTxRates(
+	IN PRTMP_ADAPTER 	pAd,
+	IN 	BOOLEAN		 	bLinkUp,
+	IN	UCHAR			apidx);
+
+#ifdef DOT11_N_SUPPORT
+VOID MlmeUpdateHtTxRates(
+	IN PRTMP_ADAPTER 		pAd,
+	IN	UCHAR				apidx);
+#endif // DOT11_N_SUPPORT //
+
+VOID    RTMPCheckRates(
+	IN      PRTMP_ADAPTER   pAd,
+	IN OUT  UCHAR           SupRate[],
+	IN OUT  UCHAR           *SupRateLen);
+
+#ifdef CONFIG_STA_SUPPORT
+BOOLEAN RTMPCheckChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		CentralChannel,
+	IN UCHAR		Channel);
+#endif // CONFIG_STA_SUPPORT //
+
+BOOLEAN 	RTMPCheckHt(
+	IN		PRTMP_ADAPTER	pAd,
+	IN		UCHAR	Wcid,
+	IN OUT	HT_CAPABILITY_IE			*pHtCapability,
+	IN OUT	ADD_HT_INFO_IE			*pAddHtInfo);
+
+VOID StaQuickResponeForRateUpExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID AsicBbpTuning1(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicBbpTuning2(
+	IN PRTMP_ADAPTER pAd);
+
+VOID RTMPUpdateMlmeRate(
+	IN PRTMP_ADAPTER	pAd);
+
+CHAR RTMPMaxRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN CHAR				Rssi0,
+	IN CHAR				Rssi1,
+	IN CHAR				Rssi2);
+
+VOID AsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID AsicRxAntEvalTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID APSDPeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+BOOLEAN RTMPCheckEntryEnableAutoRateSwitch(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry);
+
+UCHAR RTMPStaFixedTxMode(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry);
+
+VOID RTMPUpdateLegacyTxSetting(
+		UCHAR				fixed_tx_mode,
+		PMAC_TABLE_ENTRY	pEntry);
+
+BOOLEAN RTMPAutoRateSwitchCheck(
+	IN PRTMP_ADAPTER    pAd);
+
+NDIS_STATUS MlmeInit(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeHandler(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeHalt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeResetRalinkCounters(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID BuildChannelList(
+	IN PRTMP_ADAPTER pAd);
+
+UCHAR FirstChannel(
+	IN  PRTMP_ADAPTER   pAd);
+
+UCHAR NextChannel(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR channel);
+
+VOID ChangeToCellPowerLimit(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         AironetCellPowerLimit);
+
+VOID RaiseClock(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UINT32 *x);
+
+VOID LowerClock(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UINT32 *x);
+
+USHORT ShiftInBits(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID ShiftOutBits(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT data,
+	IN  USHORT count);
+
+VOID EEpromCleanup(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID EWDS(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID EWEN(
+	IN  PRTMP_ADAPTER   pAd);
+
+USHORT RTMP_EEPROM_READ16(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT Offset);
+
+VOID RTMP_EEPROM_WRITE16(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT Offset,
+	IN  USHORT Data);
+
+//
+// Prototypes of function definition in rtmp_tkip.c
+//
+VOID    RTMPInitTkipEngine(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pTKey,
+	IN  UCHAR           KeyId,
+	IN  PUCHAR          pTA,
+	IN  PUCHAR          pMICKey,
+	IN  PUCHAR          pTSC,
+	OUT PULONG          pIV16,
+	OUT PULONG          pIV32);
+
+VOID    RTMPInitMICEngine(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKey,
+	IN  PUCHAR          pDA,
+	IN  PUCHAR          pSA,
+	IN  UCHAR           UserPriority,
+	IN  PUCHAR          pMICKey);
+
+BOOLEAN RTMPTkipCompareMICValue(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pSrc,
+	IN  PUCHAR          pDA,
+	IN  PUCHAR          pSA,
+	IN  PUCHAR          pMICKey,
+	IN	UCHAR			UserPriority,
+	IN  UINT            Len);
+
+VOID    RTMPCalculateMICValue(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket,
+	IN  PUCHAR          pEncap,
+	IN  PCIPHER_KEY     pKey,
+	IN	UCHAR			apidx);
+
+BOOLEAN RTMPTkipCompareMICValueWithLLC(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pLLC,
+	IN  PUCHAR          pSrc,
+	IN  PUCHAR          pDA,
+	IN  PUCHAR          pSA,
+	IN  PUCHAR          pMICKey,
+	IN  UINT            Len);
+
+VOID    RTMPTkipAppendByte(
+	IN  PTKIP_KEY_INFO  pTkip,
+	IN  UCHAR           uChar);
+
+VOID    RTMPTkipAppend(
+	IN  PTKIP_KEY_INFO  pTkip,
+	IN  PUCHAR          pSrc,
+	IN  UINT            nBytes);
+
+VOID    RTMPTkipGetMIC(
+	IN  PTKIP_KEY_INFO  pTkip);
+
+BOOLEAN RTMPSoftDecryptTKIP(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN UCHAR    UserPriority,
+	IN PCIPHER_KEY	pWpaKey);
+
+BOOLEAN RTMPSoftDecryptAES(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN PCIPHER_KEY	pWpaKey);
+
+#if 0	// removed by AlbertY
+NDIS_STATUS RTMPWPAAddKeyProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PVOID           pBuf);
+#endif
+
+//
+// Prototypes of function definition in cmm_info.c
+//
+NDIS_STATUS RTMPWPARemoveKeyProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PVOID           pBuf);
+
+VOID    RTMPWPARemoveAllKeys(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN RTMPCheckStrPrintAble(
+    IN  CHAR *pInPutStr,
+    IN  UCHAR strLen);
+
+VOID    RTMPSetPhyMode(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG phymode);
+
+VOID	RTMPUpdateHTIE(
+	IN	RT_HT_CAPABILITY	*pRtHt,
+	IN		UCHAR				*pMcsSet,
+	OUT		HT_CAPABILITY_IE *pHtCapability,
+	OUT		ADD_HT_INFO_IE		*pAddHtInfo);
+
+VOID	RTMPAddWcidAttributeEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BssIdx,
+	IN 	UCHAR		 	KeyIdx,
+	IN 	UCHAR		 	CipherAlg,
+	IN 	MAC_TABLE_ENTRY *pEntry);
+
+CHAR *GetEncryptType(
+	CHAR enc);
+
+CHAR *GetAuthMode(
+	CHAR auth);
+
+VOID RTMPIoctlGetSiteSurvey(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlGetMacTable(
+	IN PRTMP_ADAPTER pAd,
+	IN struct iwreq *wrq);
+
+VOID RTMPIndicateWPA2Status(
+	IN  PRTMP_ADAPTER  pAdapter);
+
+VOID	RTMPOPModeSwitching(
+	IN	PRTMP_ADAPTER	pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID    RTMPAddBSSIDCipher(
+    IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR	Aid,
+    IN  PNDIS_802_11_KEY    pKey,
+    IN  UCHAR   CipherAlg);
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+VOID	RTMPSetHT(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	OID_SET_HT_PHYMODE *pHTPhyMode);
+
+VOID	RTMPSetIndividualHT(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	UCHAR				apidx);
+#endif // DOT11_N_SUPPORT //
+
+VOID RTMPSendWirelessEvent(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Event_flag,
+	IN	PUCHAR 			pAddr,
+	IN  UCHAR			BssIdx,
+	IN	CHAR			Rssi);
+
+VOID	NICUpdateCntlCounters(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PHEADER_802_11	pHeader,
+	IN    UCHAR			SubType,
+	IN	PRXWI_STRUC 	pRxWI);
+//
+// prototype in wpa.c
+//
+BOOLEAN WpaMsgTypeSubst(
+	IN  UCHAR   EAPType,
+	OUT INT		*MsgType);
+
+VOID WpaPskStateMachineInit(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  STATE_MACHINE       *S,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID WpaEAPOLKeyAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaPairMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaPairMsg3Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaGroupMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaMacHeaderInit(
+	IN      PRTMP_ADAPTER   pAd,
+	IN OUT  PHEADER_802_11  pHdr80211,
+	IN      UCHAR           wep,
+	IN      PUCHAR          pAddr1);
+
+VOID    Wpa2PairMsg1Action(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    Wpa2PairMsg3Action(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+BOOLEAN ParseKeyData(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pKeyData,
+    IN  UCHAR           KeyDataLen,
+	IN	UCHAR			bPairewise);
+
+VOID    RTMPToWirelessSta(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pHeader802_3,
+    IN  UINT            HdrLen,
+	IN  PUCHAR          pData,
+    IN  UINT            DataLen,
+    IN	BOOLEAN			is4wayFrame);
+
+VOID    HMAC_SHA1(
+	IN  UCHAR   *text,
+	IN  UINT    text_len,
+	IN  UCHAR   *key,
+	IN  UINT    key_len,
+	IN  UCHAR   *digest);
+
+VOID    PRF(
+	IN  UCHAR   *key,
+	IN  INT     key_len,
+	IN  UCHAR   *prefix,
+	IN  INT     prefix_len,
+	IN  UCHAR   *data,
+	IN  INT     data_len,
+	OUT UCHAR   *output,
+	IN  INT     len);
+
+VOID    CCKMPRF(
+	IN  UCHAR   *key,
+	IN  INT     key_len,
+	IN  UCHAR   *data,
+	IN  INT     data_len,
+	OUT UCHAR   *output,
+	IN  INT     len);
+
+VOID WpaCountPTK(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR   *PMK,
+	IN  UCHAR   *ANonce,
+	IN  UCHAR   *AA,
+	IN  UCHAR   *SNonce,
+	IN  UCHAR   *SA,
+	OUT UCHAR   *output,
+	IN  UINT    len);
+
+VOID    GenRandom(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			*macAddr,
+	OUT	UCHAR			*random);
+
+//
+// prototype in aironet.c
+//
+VOID    AironetStateMachineInit(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  STATE_MACHINE       *S,
+	OUT STATE_MACHINE_FUNC  Trans[]);
+
+VOID    AironetMsgAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    AironetRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    ChannelLoadRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    NoiseHistRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    BeaconRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    AironetReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    ChannelLoadReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    NoiseHistReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    AironetFinalReportAction(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID    BeaconReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    AironetAddBeaconReport(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  ULONG               Index,
+	IN  PMLME_QUEUE_ELEM    pElem);
+
+VOID    AironetCreateBeaconReportFromBssTable(
+	IN  PRTMP_ADAPTER       pAd);
+
+VOID    DBGPRINT_TX_RING(
+	IN PRTMP_ADAPTER  pAd,
+	IN UCHAR          QueIdx);
+
+VOID DBGPRINT_RX_RING(
+	IN PRTMP_ADAPTER  pAd);
+
+CHAR    ConvertToRssi(
+	IN PRTMP_ADAPTER  pAd,
+	IN CHAR				Rssi,
+	IN UCHAR    RssiNumber);
+
+
+#ifdef DOT11N_DRAFT3
+VOID BuildEffectedChannelList(
+	IN PRTMP_ADAPTER pAd);
+#endif // DOT11N_DRAFT3 //
+
+
+VOID APAsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd);
+
+
+VOID APAsicRxAntEvalTimeout(
+	IN PRTMP_ADAPTER	pAd);
+
+//
+// function prototype in cmm_wpa.c
+//
+BOOLEAN RTMPCheckWPAframe(
+	IN PRTMP_ADAPTER pAd,
+	IN PMAC_TABLE_ENTRY	pEntry,
+	IN PUCHAR 			pData,
+	IN ULONG 			DataByteCount,
+	IN UCHAR			FromWhichBSSID);
+
+VOID AES_GTK_KEY_UNWRAP(
+	IN  UCHAR   *key,
+	OUT UCHAR   *plaintext,
+	IN	UCHAR	c_len,
+	IN  UCHAR   *ciphertext);
+
+BOOLEAN RTMPCheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	OUT	UCHAR			*Offset);
+
+BOOLEAN RTMPParseEapolKeyData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKeyData,
+	IN  UCHAR           KeyDataLen,
+	IN	UCHAR			GroupKeyIndex,
+	IN	UCHAR			MsgType,
+	IN	BOOLEAN			bWPA2,
+	IN  MAC_TABLE_ENTRY *pEntry);
+
+VOID	ConstructEapolMsg(
+	IN 	PRTMP_ADAPTER    	pAd,
+    IN 	UCHAR				PeerAuthMode,
+    IN 	UCHAR				PeerWepStatus,
+    IN 	UCHAR				MyGroupKeyWepStatus,
+    IN 	UCHAR				MsgType,
+    IN	UCHAR				DefaultKeyIdx,
+    IN 	UCHAR				*ReplayCounter,
+	IN 	UCHAR				*KeyNonce,
+	IN	UCHAR				*TxRSC,
+	IN	UCHAR				*PTK,
+	IN	UCHAR				*GTK,
+	IN	UCHAR				*RSNIE,
+	IN	UCHAR				RSNIE_Len,
+    OUT PEAPOL_PACKET       pMsg);
+
+VOID	CalculateMIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			PeerWepStatus,
+	IN	UCHAR			*PTK,
+	OUT PEAPOL_PACKET   pMsg);
+
+NDIS_STATUS	RTMPSoftDecryptBroadCastData(
+	IN	PRTMP_ADAPTER					pAd,
+	IN	RX_BLK							*pRxBlk,
+	IN  NDIS_802_11_ENCRYPTION_STATUS 	GroupCipher,
+	IN  PCIPHER_KEY						pShard_key);
+
+VOID	ConstructEapolKeyData(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			PeerAuthMode,
+	IN	UCHAR			PeerWepStatus,
+	IN	UCHAR			GroupKeyWepStatus,
+	IN 	UCHAR			MsgType,
+	IN	UCHAR			DefaultKeyIdx,
+	IN	BOOLEAN			bWPA2Capable,
+	IN	UCHAR			*PTK,
+	IN	UCHAR			*GTK,
+	IN	UCHAR			*RSNIE,
+	IN	UCHAR			RSNIE_LEN,
+	OUT PEAPOL_PACKET   pMsg);
+
+VOID RTMPMakeRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UINT            AuthMode,
+	IN  UINT            WepStatus,
+	IN	UCHAR			apidx);
+
+//
+// function prototype in ap_wpa.c
+//
+
+BOOLEAN APWpaMsgTypeSubst(
+	IN UCHAR    EAPType,
+	OUT INT *MsgType) ;
+
+MAC_TABLE_ENTRY *PACInquiry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG           Wcid);
+
+BOOLEAN RTMPCheckMcast(
+	IN PRTMP_ADAPTER pAd,
+	IN PEID_STRUCT      eid_ptr,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+BOOLEAN RTMPCheckUcast(
+	IN PRTMP_ADAPTER pAd,
+	IN PEID_STRUCT      eid_ptr,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+BOOLEAN RTMPCheckAUTH(
+	IN PRTMP_ADAPTER pAd,
+	IN PEID_STRUCT      eid_ptr,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+VOID WPAStart4WayHS(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	IN	ULONG			TimeInterval);
+
+VOID WPAStart2WayGroupHS(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MAC_TABLE_ENTRY *pEntry);
+
+VOID APWpaEAPPacketAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLStartAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLLogoffAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLKeyAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLASFAlertAction(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  MLME_QUEUE_ELEM  *Elem);
+
+VOID HandleCounterMeasure(
+	IN PRTMP_ADAPTER pAd,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+VOID PeerPairMsg2Action(
+	IN PRTMP_ADAPTER pAd,
+	IN MAC_TABLE_ENTRY  *pEntry,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerPairMsg4Action(
+	IN PRTMP_ADAPTER pAd,
+	IN MAC_TABLE_ENTRY  *pEntry,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID CMTimerExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID WPARetryExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID EnqueueStartForPSKExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID RTMPHandleSTAKey(
+    IN PRTMP_ADAPTER    pAdapter,
+    IN MAC_TABLE_ENTRY  *pEntry,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+#if 0 // merge into PeerPairMsg4Action
+VOID Wpa1PeerPairMsg4Action(
+	IN PRTMP_ADAPTER pAd,
+	IN MAC_TABLE_ENTRY  *pEntry,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID Wpa2PeerPairMsg4Action(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry,
+	IN  MLME_QUEUE_ELEM  *Elem);
+#endif // 0 //
+
+VOID PeerGroupMsg2Action(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry,
+	IN  VOID             *Msg,
+	IN  UINT             MsgLen);
+
+#if 0	// replaced by WPAStart2WayGroupHS
+NDIS_STATUS APWpaHardTransmit(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry);
+#endif // 0 //
+
+VOID PairDisAssocAction(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry,
+	IN  USHORT           Reason);
+
+VOID MlmeDeAuthAction(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry,
+	IN  USHORT           Reason);
+
+VOID GREKEYPeriodicExec(
+	IN  PVOID   SystemSpecific1,
+	IN  PVOID   FunctionContext,
+	IN  PVOID   SystemSpecific2,
+	IN  PVOID   SystemSpecific3);
+
+VOID CountGTK(
+	IN  UCHAR   *PMK,
+	IN  UCHAR   *GNonce,
+	IN  UCHAR   *AA,
+	OUT UCHAR   *output,
+	IN  UINT    len);
+
+VOID    GetSmall(
+	IN  PVOID   pSrc1,
+	IN  PVOID   pSrc2,
+	OUT PUCHAR  out,
+	IN  ULONG   Length);
+
+VOID    GetLarge(
+	IN  PVOID   pSrc1,
+	IN  PVOID   pSrc2,
+	OUT PUCHAR  out,
+	IN  ULONG   Length);
+
+VOID APGenRandom(
+	IN PRTMP_ADAPTER pAd,
+	OUT UCHAR       *random);
+
+VOID AES_GTK_KEY_WRAP(
+	IN UCHAR *key,
+	IN UCHAR *plaintext,
+	IN UCHAR p_len,
+	OUT UCHAR *ciphertext);
+
+VOID    WpaSend(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          pPacket,
+    IN  ULONG           Len);
+
+VOID    APToWirelessSta(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	IN  PUCHAR          pHeader802_3,
+	IN  UINT            HdrLen,
+	IN  PUCHAR          pData,
+	IN  UINT            DataLen,
+    IN	BOOLEAN			bClearFrame);
+
+VOID RTMPAddPMKIDCache(
+	IN  PRTMP_ADAPTER   		pAd,
+	IN	INT						apidx,
+	IN	PUCHAR				pAddr,
+	IN	UCHAR					*PMKID,
+	IN	UCHAR					*PMK);
+
+INT RTMPSearchPMKIDCache(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx,
+	IN	PUCHAR		pAddr);
+
+VOID RTMPDeletePMKIDCache(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx,
+	IN  INT				idx);
+
+VOID RTMPMaintainPMKIDCache(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID	RTMPSendTriggerFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PVOID			pBuffer,
+	IN	ULONG			Length,
+	IN  UCHAR           TxRate,
+	IN	BOOLEAN			bQosNull);
+
+
+//typedef void (*TIMER_FUNCTION)(unsigned long);
+
+
+/* timeout -- ms */
+VOID RTMP_SetPeriodicTimer(
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	unsigned long timeout);
+
+VOID RTMP_OS_Init_Timer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	TIMER_FUNCTION function,
+	IN	PVOID data);
+
+VOID RTMP_OS_Add_Timer(
+	IN	NDIS_MINIPORT_TIMER	*pTimer,
+	IN	unsigned long timeout);
+
+VOID RTMP_OS_Mod_Timer(
+	IN	NDIS_MINIPORT_TIMER	*pTimer,
+	IN	unsigned long timeout);
+
+
+VOID RTMP_OS_Del_Timer(
+	IN	NDIS_MINIPORT_TIMER	*pTimer,
+	OUT	BOOLEAN				 *pCancelled);
+
+
+VOID RTMP_OS_Release_Packet(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PQUEUE_ENTRY  pEntry);
+
+VOID RTMPusecDelay(
+	IN	ULONG	usec);
+
+NDIS_STATUS os_alloc_mem(
+	IN	PRTMP_ADAPTER pAd,
+	OUT	PUCHAR *mem,
+	IN	ULONG  size);
+
+NDIS_STATUS os_free_mem(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PUCHAR mem);
+
+
+void RTMP_AllocateSharedMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+VOID RTMPFreeTxRxRingMemory(
+    IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS AdapterBlockAllocateMemory(
+	IN PVOID	handle,
+	OUT	PVOID	*ppAd);
+
+void RTMP_AllocateTxDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	UINT	Index,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+void RTMP_AllocateFirstTxBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	UINT	Index,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+void RTMP_AllocateMgmtDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+void RTMP_AllocateRxDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+PNDIS_PACKET RTMP_AllocateRxPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+PNDIS_PACKET RTMP_AllocateTxPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress);
+
+PNDIS_PACKET RTMP_AllocateFragPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length);
+
+void RTMP_QueryPacketInfo(
+	IN  PNDIS_PACKET pPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen);
+
+void RTMP_QueryNextPacketInfo(
+	IN  PNDIS_PACKET *ppPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen);
+
+
+BOOLEAN RTMP_FillTxBlkInfo(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK *pTxBlk);
+
+
+PRTMP_SCATTER_GATHER_LIST
+rt_get_sg_list_from_packet(PNDIS_PACKET pPacket, RTMP_SCATTER_GATHER_LIST *sg);
+
+
+ void announce_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+UINT BA_Reorder_AMSDU_Annnounce(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+UINT Handle_AMSDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN  UCHAR			FromWhichBSSID);
+
+
+void convert_802_11_to_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			p8023hdr,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN  UCHAR			FromWhichBSSID);
+
+
+PNET_DEV get_netdev_from_bssid(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			FromWhichBSSID);
+
+
+PNDIS_PACKET duplicate_pkt(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+    IN  UINT            HdrLen,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN	UCHAR			FromWhichBSSID);
+
+
+PNDIS_PACKET duplicate_pkt_with_TKIP_MIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pOldPkt);
+
+PNDIS_PACKET duplicate_pkt_with_VLAN(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+    IN  UINT            HdrLen,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN	UCHAR			FromWhichBSSID);
+
+PNDIS_PACKET duplicate_pkt_with_WPI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UINT32			ext_head_len,
+	IN	UINT32			ext_tail_len);
+
+UCHAR VLAN_8023_Header_Copy(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+	IN	UINT            HdrLen,
+	OUT PUCHAR			pData,
+	IN	UCHAR			FromWhichBSSID);
+
+#ifdef DOT11_N_SUPPORT
+void ba_flush_reordering_timeout_mpdus(
+	IN PRTMP_ADAPTER	pAd,
+	IN PBA_REC_ENTRY	pBAEntry,
+	IN ULONG			Now32);
+
+
+VOID BAOriSessionSetUp(
+			IN PRTMP_ADAPTER    pAd,
+			IN MAC_TABLE_ENTRY	*pEntry,
+			IN UCHAR			TID,
+			IN USHORT			TimeOut,
+			IN ULONG			DelayTime,
+			IN BOOLEAN		isForced);
+
+VOID BASessionTearDownALL(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		UCHAR Wcid);
+#endif // DOT11_N_SUPPORT //
+
+BOOLEAN OS_Need_Clone_Packet(void);
+
+
+VOID build_tx_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR	pFrame,
+	IN	ULONG	FrameLen);
+
+
+VOID BAOriSessionTearDown(
+	IN OUT	PRTMP_ADAPTER	pAd,
+	IN		UCHAR			Wcid,
+	IN		UCHAR			TID,
+	IN		BOOLEAN			bPassive,
+	IN		BOOLEAN			bForceSend);
+
+VOID BARecSessionTearDown(
+	IN OUT	PRTMP_ADAPTER	pAd,
+	IN		UCHAR			Wcid,
+	IN		UCHAR			TID,
+	IN		BOOLEAN			bPassive);
+
+BOOLEAN ba_reordering_resource_init(PRTMP_ADAPTER pAd, int num);
+void ba_reordering_resource_release(PRTMP_ADAPTER pAd);
+
+ULONG AutoChBssInsertEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR ChannelNo,
+	IN CHAR Rssi);
+
+void AutoChBssTableInit(
+	IN PRTMP_ADAPTER pAd);
+
+void ChannelInfoInit(
+	IN PRTMP_ADAPTER pAd);
+
+void AutoChBssTableDestroy(
+	IN PRTMP_ADAPTER pAd);
+
+void ChannelInfoDestroy(
+	IN PRTMP_ADAPTER pAd);
+
+UCHAR New_ApAutoSelectChannel(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN rtstrmactohex(
+	IN char *s1,
+	IN char *s2);
+
+BOOLEAN rtstrcasecmp(
+	IN char *s1,
+	IN char *s2);
+
+char *rtstrstruncasecmp(
+	IN char *s1,
+	IN char *s2);
+
+char    *rtstrstr(
+	IN	const char * s1,
+	IN	const char * s2);
+
+char *rstrtok(
+	IN char * s,
+	IN const char * ct);
+
+int rtinet_aton(
+	const char *cp,
+	unsigned int *addr);
+
+////////// common ioctl functions //////////
+INT Set_DriverVersion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ShortSlot_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_BGProtection_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_TxPreamble_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_RTSThreshold_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_FragThreshold_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_TxBurst_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+#ifdef AGGREGATION_SUPPORT
+INT	Set_PktAggregate_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+#endif
+
+INT	Set_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef DBG
+INT	Set_Debug_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif
+
+INT	Show_DescInfo_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ResetStatCounter_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef DOT11_N_SUPPORT
+INT	Set_BASetup_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_BADecline_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_BAOriTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_BARecTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtStbc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtHtc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtLinkAdapt_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtProtect_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMimoPs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+
+INT	Set_ForceShortGI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ForceGF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	SetCommonHT(
+	IN	PRTMP_ADAPTER	pAd);
+
+INT	Set_SendPSMPAction_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMIMOPSmode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+
+INT	Set_HtTxBASize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // DOT11_N_SUPPORT //
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+//Dls ,	kathy
+VOID RTMPSendDLSTearDownFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA);
+
+#ifdef DOT11_N_SUPPORT
+//Block ACK
+VOID QueryBATABLE(
+	IN  PRTMP_ADAPTER pAd,
+	OUT PQUERYBA_TABLE pBAT);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+INT	    WpaCheckEapCode(
+	IN  PRTMP_ADAPTER   	pAd,
+	IN  PUCHAR				pFrame,
+	IN  USHORT				FrameLen,
+	IN  USHORT				OffSet);
+
+VOID    WpaSendMicFailureToWpaSupplicant(
+    IN  PRTMP_ADAPTER       pAd,
+    IN  BOOLEAN             bUnicast);
+
+VOID    SendAssocIEsToWpaSupplicant(
+    IN  PRTMP_ADAPTER       pAd);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+int wext_notify_event_assoc(
+	IN  RTMP_ADAPTER *pAd);
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+#ifdef DOT11_N_SUPPORT
+VOID Handle_BSS_Width_Trigger_Events(
+	IN PRTMP_ADAPTER pAd);
+
+void build_ext_channel_switch_ie(
+	IN PRTMP_ADAPTER pAd,
+	IN HT_EXT_CHANNEL_SWITCH_ANNOUNCEMENT_IE *pIE);
+#endif // DOT11_N_SUPPORT //
+
+
+BOOLEAN APRxDoneInterruptHandle(
+	IN	PRTMP_ADAPTER	pAd);
+
+BOOLEAN STARxDoneInterruptHandle(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	BOOLEAN			argc);
+
+#ifdef DOT11_N_SUPPORT
+// AMPDU packet indication
+VOID Indicate_AMPDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+// AMSDU packet indication
+VOID Indicate_AMSDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+#endif // DOT11_N_SUPPORT //
+
+// Normal legacy Rx packet indication
+VOID Indicate_Legacy_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID Indicate_EAPOL_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+void  update_os_packet_info(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+void wlan_802_11_to_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	PUCHAR			pHeader802_3,
+	IN  UCHAR			FromWhichBSSID);
+
+UINT deaggregate_AMSDU_announce(
+	IN	PRTMP_ADAPTER	pAd,
+	PNDIS_PACKET		pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize);
+
+
+#ifdef CONFIG_STA_SUPPORT
+// remove LLC and get 802_3 Header
+#define  RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(_pRxBlk, _pHeader802_3)	\
+{																				\
+	PUCHAR _pRemovedLLCSNAP = NULL, _pDA, _pSA;                                 \
+																				\
+	if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_MESH))                                    \
+	{                                                                           \
+		_pDA = _pRxBlk->pHeader->Addr3;                                         \
+		_pSA = (PUCHAR)_pRxBlk->pHeader + sizeof(HEADER_802_11);                \
+	}                                                                           \
+	else                                                                        \
+	{                                                                           \
+		if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_INFRA))                              	\
+		{                                                                       \
+			_pDA = _pRxBlk->pHeader->Addr1;                                     \
+		if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_DLS))									\
+			_pSA = _pRxBlk->pHeader->Addr2;										\
+		else																	\
+			_pSA = _pRxBlk->pHeader->Addr3;                                     \
+		}                                                                       \
+		else                                                                    \
+		{                                                                       \
+			_pDA = _pRxBlk->pHeader->Addr1;                                     \
+			_pSA = _pRxBlk->pHeader->Addr2;                                     \
+		}                                                                       \
+	}                                                                           \
+																				\
+	CONVERT_TO_802_3(_pHeader802_3, _pDA, _pSA, _pRxBlk->pData, 				\
+		_pRxBlk->DataSize, _pRemovedLLCSNAP);                                   \
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+BOOLEAN APFowardWirelessStaToWirelessSta(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	ULONG			FromWhichBSSID);
+
+VOID Announce_or_Forward_802_3_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID Sta_Announce_or_Forward_802_3_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID);
+
+
+#ifdef CONFIG_STA_SUPPORT
+#define ANNOUNCE_OR_FORWARD_802_3_PACKET(_pAd, _pPacket, _FromWhichBSS)\
+			Sta_Announce_or_Forward_802_3_Packet(_pAd, _pPacket, _FromWhichBSS);
+			//announce_802_3_packet(_pAd, _pPacket);
+#endif // CONFIG_STA_SUPPORT //
+
+
+PNDIS_PACKET DuplicatePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID);
+
+
+PNDIS_PACKET ClonePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize);
+
+
+// Normal, AMPDU or AMSDU
+VOID CmmRxnonRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID CmmRxRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID Update_Rssi_Sample(
+	IN PRTMP_ADAPTER	pAd,
+	IN RSSI_SAMPLE		*pRssi,
+	IN PRXWI_STRUC		pRxWI);
+
+PNDIS_PACKET GetPacketFromRxRing(
+	IN		PRTMP_ADAPTER	pAd,
+	OUT		PRT28XX_RXD_STRUC		pSaveRxD,
+	OUT		BOOLEAN			*pbReschedule,
+	IN OUT	UINT32			*pRxPending);
+
+PNDIS_PACKET RTMPDeFragmentDataFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk);
+
+////////////////////////////////////////
+
+
+
+
+
+#ifdef SNMP_SUPPORT
+//for snmp , kathy
+typedef struct _DefaultKeyIdxValue
+{
+	UCHAR	KeyIdx;
+	UCHAR	Value[16];
+} DefaultKeyIdxValue, *PDefaultKeyIdxValue;
+#endif
+
+
+#ifdef CONFIG_STA_SUPPORT
+enum {
+	DIDmsg_lnxind_wlansniffrm		= 0x00000044,
+	DIDmsg_lnxind_wlansniffrm_hosttime	= 0x00010044,
+	DIDmsg_lnxind_wlansniffrm_mactime	= 0x00020044,
+	DIDmsg_lnxind_wlansniffrm_channel	= 0x00030044,
+	DIDmsg_lnxind_wlansniffrm_rssi		= 0x00040044,
+	DIDmsg_lnxind_wlansniffrm_sq		= 0x00050044,
+	DIDmsg_lnxind_wlansniffrm_signal	= 0x00060044,
+	DIDmsg_lnxind_wlansniffrm_noise		= 0x00070044,
+	DIDmsg_lnxind_wlansniffrm_rate		= 0x00080044,
+	DIDmsg_lnxind_wlansniffrm_istx		= 0x00090044,
+	DIDmsg_lnxind_wlansniffrm_frmlen	= 0x000A0044
+};
+enum {
+	P80211ENUM_msgitem_status_no_value	= 0x00
+};
+enum {
+	P80211ENUM_truth_false			= 0x00,
+	P80211ENUM_truth_true			= 0x01
+};
+
+/* Definition from madwifi */
+typedef struct {
+        UINT32 did;
+        UINT16 status;
+        UINT16 len;
+        UINT32 data;
+} p80211item_uint32_t;
+
+typedef struct {
+        UINT32 msgcode;
+        UINT32 msglen;
+#define WLAN_DEVNAMELEN_MAX 16
+        UINT8 devname[WLAN_DEVNAMELEN_MAX];
+        p80211item_uint32_t hosttime;
+        p80211item_uint32_t mactime;
+        p80211item_uint32_t channel;
+        p80211item_uint32_t rssi;
+        p80211item_uint32_t sq;
+        p80211item_uint32_t signal;
+        p80211item_uint32_t noise;
+        p80211item_uint32_t rate;
+        p80211item_uint32_t istx;
+        p80211item_uint32_t frmlen;
+} wlan_ng_prism2_header;
+
+/* The radio capture header precedes the 802.11 header. */
+typedef struct PACKED _ieee80211_radiotap_header {
+    UINT8	it_version;	/* Version 0. Only increases
+				 * for drastic changes,
+				 * introduction of compatible
+				 * new fields does not count.
+				 */
+    UINT8	it_pad;
+    UINT16     it_len;         /* length of the whole
+				 * header in bytes, including
+				 * it_version, it_pad,
+				 * it_len, and data fields.
+				 */
+    UINT32   it_present;	/* A bitmap telling which
+					 * fields are present. Set bit 31
+					 * (0x80000000) to extend the
+					 * bitmap by another 32 bits.
+					 * Additional extensions are made
+					 * by setting bit 31.
+					 */
+}ieee80211_radiotap_header ;
+
+enum ieee80211_radiotap_type {
+    IEEE80211_RADIOTAP_TSFT = 0,
+    IEEE80211_RADIOTAP_FLAGS = 1,
+    IEEE80211_RADIOTAP_RATE = 2,
+    IEEE80211_RADIOTAP_CHANNEL = 3,
+    IEEE80211_RADIOTAP_FHSS = 4,
+    IEEE80211_RADIOTAP_DBM_ANTSIGNAL = 5,
+    IEEE80211_RADIOTAP_DBM_ANTNOISE = 6,
+    IEEE80211_RADIOTAP_LOCK_QUALITY = 7,
+    IEEE80211_RADIOTAP_TX_ATTENUATION = 8,
+    IEEE80211_RADIOTAP_DB_TX_ATTENUATION = 9,
+    IEEE80211_RADIOTAP_DBM_TX_POWER = 10,
+    IEEE80211_RADIOTAP_ANTENNA = 11,
+    IEEE80211_RADIOTAP_DB_ANTSIGNAL = 12,
+    IEEE80211_RADIOTAP_DB_ANTNOISE = 13
+};
+
+#define WLAN_RADIOTAP_PRESENT (			\
+	(1 << IEEE80211_RADIOTAP_TSFT)	|	\
+	(1 << IEEE80211_RADIOTAP_FLAGS) |	\
+	(1 << IEEE80211_RADIOTAP_RATE)  | 	\
+	 0)
+
+typedef struct _wlan_radiotap_header {
+	ieee80211_radiotap_header wt_ihdr;
+	INT64 wt_tsft;
+	UINT8 wt_flags;
+	UINT8 wt_rate;
+} wlan_radiotap_header;
+/* Definition from madwifi */
+
+void send_monitor_packets(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk);
+
+#if WIRELESS_EXT >= 12
+// This function will be called when query /proc
+struct iw_statistics *rt28xx_get_wireless_stats(
+    IN struct net_device *net_dev);
+#endif
+
+VOID    RTMPSetDesiredRates(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  LONG            Rates);
+#endif // CONFIG_STA_SUPPORT //
+
+INT	Set_FixedTxMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+INT	Set_OpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+static inline char* GetPhyMode(
+	int Mode)
+{
+	switch(Mode)
+	{
+		case MODE_CCK:
+			return "CCK";
+
+		case MODE_OFDM:
+			return "OFDM";
+#ifdef DOT11_N_SUPPORT
+		case MODE_HTMIX:
+			return "HTMIX";
+
+		case MODE_HTGREENFIELD:
+			return "GREEN";
+#endif // DOT11_N_SUPPORT //
+		default:
+			return "N/A";
+	}
+}
+
+
+static inline char* GetBW(
+	int BW)
+{
+	switch(BW)
+	{
+		case BW_10:
+			return "10M";
+
+		case BW_20:
+			return "20M";
+#ifdef DOT11_N_SUPPORT
+		case BW_40:
+			return "40M";
+#endif // DOT11_N_SUPPORT //
+		default:
+			return "N/A";
+	}
+}
+
+
+VOID RT28xxThreadTerminate(
+	IN RTMP_ADAPTER *pAd);
+
+BOOLEAN RT28XXChipsetCheck(
+	IN void *_dev_p);
+
+BOOLEAN RT28XXNetDevInit(
+	IN void 				*_dev_p,
+	IN struct  net_device	*net_dev,
+	IN RTMP_ADAPTER 		*pAd);
+
+BOOLEAN RT28XXProbePostConfig(
+	IN void 				*_dev_p,
+	IN RTMP_ADAPTER 		*pAd,
+	IN INT32				argc);
+
+VOID RT28XXDMADisable(
+	IN RTMP_ADAPTER 		*pAd);
+
+VOID RT28XXDMAEnable(
+	IN RTMP_ADAPTER 		*pAd);
+
+VOID RT28xx_UpdateBeaconToAsic(
+	IN RTMP_ADAPTER * pAd,
+	IN INT apidx,
+	IN ULONG BeaconLen,
+	IN ULONG UpdatePos);
+
+INT rt28xx_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT			cmd);
+
+
+#ifdef CONFIG_STA_SUPPORT
+INT rt28xx_sta_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT			cmd);
+#endif // CONFIG_STA_SUPPORT //
+
+BOOLEAN RT28XXSecurityKeyAdd(
+	IN		PRTMP_ADAPTER		pAd,
+	IN		ULONG				apidx,
+	IN		ULONG				KeyIdx,
+	IN		MAC_TABLE_ENTRY 	*pEntry);
+
+////////////////////////////////////////
+PNDIS_PACKET GetPacketFromRxRing(
+	IN		PRTMP_ADAPTER	pAd,
+	OUT		PRT28XX_RXD_STRUC	pSaveRxD,
+	OUT		BOOLEAN			*pbReschedule,
+	IN OUT	UINT32			*pRxPending);
+
+
+void kill_thread_task(PRTMP_ADAPTER pAd);
+
+void tbtt_tasklet(unsigned long data);
+
+
+VOID AsicTurnOffRFClk(
+	IN PRTMP_ADAPTER    pAd,
+	IN	UCHAR           Channel);
+
+VOID AsicTurnOnRFClk(
+	IN PRTMP_ADAPTER 	pAd,
+	IN	UCHAR			Channel);
+
+#ifdef RT2870
+//
+// Function Prototype in rtusb_bulk.c
+//
+VOID	RTUSBInitTxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTX_CONTEXT		pTxContext,
+	IN	UCHAR			BulkOutPipeId,
+	IN	usb_complete_t	Func);
+
+VOID	RTUSBInitHTTxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PHT_TX_CONTEXT	pTxContext,
+	IN	UCHAR			BulkOutPipeId,
+	IN	ULONG			BulkOutSize,
+	IN	usb_complete_t	Func);
+
+VOID	RTUSBInitRxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PRX_CONTEXT		pRxContext);
+
+VOID RTUSBCleanUpDataBulkOutQueue(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBCancelPendingBulkOutIRP(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBBulkOutDataPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BulkOutPipeId,
+	IN	UCHAR			Index);
+
+VOID RTUSBBulkOutNullFrame(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBBulkOutRTSFrame(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBCancelPendingBulkInIRP(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBCancelPendingIRPs(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBBulkOutMLMEPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index);
+
+VOID RTUSBBulkOutPsPoll(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBCleanUpMLMEBulkOutQueue(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBKickBulkOut(
+	IN	PRTMP_ADAPTER pAd);
+
+VOID	RTUSBBulkReceive(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID DoBulkIn(
+	IN RTMP_ADAPTER *pAd);
+
+VOID RTUSBInitRxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PRX_CONTEXT		pRxContext);
+
+VOID RTUSBBulkRxHandle(
+	IN unsigned long data);
+
+//
+// Function Prototype in rtusb_io.c
+//
+NTSTATUS RTUSBMultiRead(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	OUT	PUCHAR			pData,
+	IN	USHORT			length);
+
+NTSTATUS RTUSBMultiWrite(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	PUCHAR			pData,
+	IN	USHORT			length);
+
+NTSTATUS RTUSBMultiWrite_OneByte(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	PUCHAR			pData);
+
+NTSTATUS RTUSBReadBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	PUCHAR			pValue);
+
+NTSTATUS RTUSBWriteBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	UCHAR			Value);
+
+NTSTATUS RTUSBWriteRFRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UINT32			Value);
+
+NTSTATUS	RT30xxWriteRFRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			RegID,
+	IN	UCHAR			Value);
+
+NTSTATUS	RT30xxReadRFRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			RegID,
+	IN	PUCHAR			pValue);
+
+NTSTATUS RTUSB_VendorRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UINT32			TransferFlags,
+	IN	UCHAR			ReservedBits,
+	IN	UCHAR			Request,
+	IN	USHORT			Value,
+	IN	USHORT			Index,
+	IN	PVOID			TransferBuffer,
+	IN	UINT32			TransferBufferLength);
+
+NTSTATUS RTUSBReadEEPROM(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	OUT	PUCHAR			pData,
+	IN	USHORT			length);
+
+NTSTATUS RTUSBWriteEEPROM(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	PUCHAR			pData,
+	IN	USHORT			length);
+
+VOID RTUSBPutToSleep(
+	IN	PRTMP_ADAPTER	pAd);
+
+NTSTATUS RTUSBWakeUp(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBInitializeCmdQ(
+	IN	PCmdQ	cmdq);
+
+NDIS_STATUS	RTUSBEnqueueCmdFromNdis(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	NDIS_OID		Oid,
+	IN	BOOLEAN			SetInformation,
+	IN	PVOID			pInformationBuffer,
+	IN	UINT32			InformationBufferLength);
+
+NDIS_STATUS RTUSBEnqueueInternalCmd(
+	IN	PRTMP_ADAPTER	pAd,
+	IN NDIS_OID			Oid,
+	IN PVOID			pInformationBuffer,
+	IN UINT32			InformationBufferLength);
+
+VOID RTUSBDequeueCmd(
+	IN	PCmdQ		cmdq,
+	OUT	PCmdQElmt	*pcmdqelmt);
+
+INT RTUSBCmdThread(
+	IN OUT PVOID Context);
+
+INT TimerQThread(
+	IN OUT PVOID Context);
+
+RT2870_TIMER_ENTRY *RT2870_TimerQ_Insert(
+	IN RTMP_ADAPTER *pAd,
+	IN RALINK_TIMER_STRUCT *pTimer);
+
+BOOLEAN RT2870_TimerQ_Remove(
+	IN RTMP_ADAPTER *pAd,
+	IN RALINK_TIMER_STRUCT *pTimer);
+
+void RT2870_TimerQ_Exit(
+	IN RTMP_ADAPTER *pAd);
+
+void RT2870_TimerQ_Init(
+	IN RTMP_ADAPTER *pAd);
+
+VOID RT2870_BssBeaconExit(
+	IN RTMP_ADAPTER *pAd);
+
+VOID RT2870_BssBeaconStop(
+	IN RTMP_ADAPTER *pAd);
+
+VOID RT2870_BssBeaconStart(
+	IN RTMP_ADAPTER * pAd);
+
+VOID RT2870_BssBeaconInit(
+	IN RTMP_ADAPTER *pAd);
+
+VOID RT2870_WatchDog(
+	IN RTMP_ADAPTER *pAd);
+
+NTSTATUS RTUSBWriteMACRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	UINT32			Value);
+
+NTSTATUS RTUSBReadMACRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	OUT	PUINT32			pValue);
+
+NTSTATUS RTUSBSingleWrite(
+	IN 	RTMP_ADAPTER 	*pAd,
+	IN	USHORT			Offset,
+	IN	USHORT			Value);
+
+NTSTATUS RTUSBFirmwareRun(
+	IN	PRTMP_ADAPTER	pAd);
+
+NTSTATUS RTUSBFirmwareWrite(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR		pFwImage,
+	IN ULONG		FwLen);
+
+NTSTATUS	RTUSBFirmwareOpmode(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUINT32			pValue);
+
+NTSTATUS	RTUSBVenderReset(
+	IN	PRTMP_ADAPTER	pAd);
+
+NDIS_STATUS RTUSBSetHardWareRegister(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PVOID			pBuf);
+
+NDIS_STATUS RTUSBQueryHardWareRegister(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PVOID			pBuf);
+
+VOID CMDHandler(
+    IN PRTMP_ADAPTER pAd);
+
+
+NDIS_STATUS	 CreateThreads(
+	IN	struct net_device *net_dev );
+
+
+VOID MacTableInitialize(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeSetPsm(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT psm);
+
+NDIS_STATUS RTMPWPAAddKeyProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PVOID           pBuf);
+
+VOID AsicRxAntEvalAction(
+	IN PRTMP_ADAPTER pAd);
+
+#if 0 // Mark because not used in RT28xx.
+NTSTATUS RTUSBRxPacket(
+	IN	PRTMP_ADAPTER  pAd,
+	IN    BOOLEAN          bBulkReceive);
+
+VOID RTUSBDequeueMLMEPacket(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBCleanUpMLMEWaitQueue(
+	IN	PRTMP_ADAPTER	pAd);
+#endif
+
+void append_pkt(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+    IN  UINT            HdrLen,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	OUT  PNDIS_PACKET	*ppPacket);
+
+UINT deaggregate_AMSDU_announce(
+	IN	PRTMP_ADAPTER	pAd,
+	PNDIS_PACKET		pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize);
+
+NDIS_STATUS	RTMPCheckRxError(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PHEADER_802_11	pHeader,
+	IN	PRXWI_STRUC	pRxWI,
+	IN	PRT28XX_RXD_STRUC	pRxINFO);
+
+
+VOID RTUSBMlmeHardTransmit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PMGMT_STRUC		pMgmt);
+
+INT MlmeThread(
+	IN PVOID Context);
+
+#if 0
+VOID    RTUSBResumeMsduTransmission(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID    RTUSBSuspendMsduTransmission(
+	IN	PRTMP_ADAPTER	pAd);
+#endif
+
+//
+// Function Prototype in rtusb_data.c
+//
+NDIS_STATUS	RTUSBFreeDescriptorRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BulkOutPipeId,
+	IN	UINT32			NumberRequired);
+
+
+BOOLEAN	RTUSBNeedQueueBackForAgg(
+	IN RTMP_ADAPTER *pAd,
+	IN UCHAR		BulkOutPipeId);
+
+
+VOID RTMPWriteTxInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXINFO_STRUC 	pTxInfo,
+	IN	  USHORT		USBDMApktLen,
+	IN	  BOOLEAN		bWiv,
+	IN	  UCHAR			QueueSel,
+	IN	  UCHAR			NextValid,
+	IN	  UCHAR			TxBurst);
+
+//
+// Function Prototype in cmm_data_2870.c
+//
+USHORT RtmpUSB_WriteSubTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber);
+
+USHORT RtmpUSB_WriteSingleTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber);
+
+USHORT	RtmpUSB_WriteFragTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			fragNum,
+	OUT	USHORT			*FreeNumber);
+
+USHORT RtmpUSB_WriteMultiTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			frameNum,
+	OUT	USHORT			*FreeNumber);
+
+VOID RtmpUSB_FinalWriteTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	USHORT			totalMPDUSize,
+	IN	USHORT			TxIdx);
+
+VOID RtmpUSBDataLastTxIdx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	USHORT			TxIdx);
+
+VOID RtmpUSBDataKickOut(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			QueIdx);
+
+
+int RtmpUSBMgmtKickOut(
+	IN RTMP_ADAPTER 	*pAd,
+	IN UCHAR 			QueIdx,
+	IN PNDIS_PACKET		pPacket,
+	IN PUCHAR			pSrcBufVA,
+	IN UINT 			SrcBufLen);
+
+VOID RtmpUSBNullFrameKickOut(
+	IN RTMP_ADAPTER *pAd,
+	IN UCHAR		QueIdx,
+	IN UCHAR		*pNullFrame,
+	IN UINT32		frameLen);
+
+VOID RT28xxUsbStaAsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN       bFromTx);
+
+VOID RT28xxUsbStaAsicSleepThenAutoWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TbttNumToNextWakeUp);
+
+VOID RT28xxUsbMlmeRadioOn(
+	IN PRTMP_ADAPTER pAd);
+
+VOID RT28xxUsbMlmeRadioOFF(
+	IN PRTMP_ADAPTER pAd);
+#endif // RT2870 //
+
+////////////////////////////////////////
+
+VOID QBSS_LoadInit(
+ 	IN		RTMP_ADAPTER	*pAd);
+
+UINT32 QBSS_LoadElementAppend(
+ 	IN		RTMP_ADAPTER	*pAd,
+	OUT		UINT8			*buf_p);
+
+VOID QBSS_LoadUpdate(
+ 	IN		RTMP_ADAPTER	*pAd);
+
+///////////////////////////////////////
+INT RTMPShowCfgValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pName,
+	IN	PUCHAR			pBuf);
+
+PCHAR   RTMPGetRalinkAuthModeStr(
+    IN  NDIS_802_11_AUTHENTICATION_MODE authMode);
+
+PCHAR   RTMPGetRalinkEncryModeStr(
+    IN  USHORT encryMode);
+//////////////////////////////////////
+
+#ifdef CONFIG_STA_SUPPORT
+VOID AsicStaBbpTuning(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN StaAddMacTableEntry(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PMAC_TABLE_ENTRY	pEntry,
+	IN  UCHAR				MaxSupportedRateIn500Kbps,
+	IN  HT_CAPABILITY_IE	*pHtCapability,
+	IN  UCHAR				HtCapabilityLen,
+	IN  USHORT        		CapabilityInfo);
+#endif // CONFIG_STA_SUPPORT //
+
+void RTMP_IndicateMediaState(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID ReSyncBeaconTime(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPSetAGCInitValue(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			BandWidth);
+
+int rt28xx_close(IN PNET_DEV dev);
+int rt28xx_open(IN PNET_DEV dev);
+
+__inline INT VIRTUAL_IF_UP(PRTMP_ADAPTER pAd)
+{
+extern VOID MeshMakeBeacon(IN PRTMP_ADAPTER pAd, IN UCHAR idx);
+extern VOID MeshUpdateBeaconFrame(IN PRTMP_ADAPTER pAd, IN UCHAR idx);
+
+	if (VIRTUAL_IF_NUM(pAd) == 0)
+	{
+		if (rt28xx_open(pAd->net_dev) != 0)
+			return -1;
+	}
+	else
+	{
+	}
+	VIRTUAL_IF_INC(pAd);
+	return 0;
+}
+
+__inline VOID VIRTUAL_IF_DOWN(PRTMP_ADAPTER pAd)
+{
+	VIRTUAL_IF_DEC(pAd);
+	if (VIRTUAL_IF_NUM(pAd) == 0)
+		rt28xx_close(pAd->net_dev);
+	return;
+}
+
+
+#endif  // __RTMP_H__
+
diff --git a/drivers/staging/rt2870/rtmp_ckipmic.h b/drivers/staging/rt2870/rtmp_ckipmic.h
new file mode 100644
index 000000000000..a3d949a39d39
--- /dev/null
+++ b/drivers/staging/rt2870/rtmp_ckipmic.h
@@ -0,0 +1,113 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_ckipmic.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#ifndef	__RTMP_CKIPMIC_H__
+#define	__RTMP_CKIPMIC_H__
+
+typedef	struct	_MIC_CONTEXT	{
+	/* --- MMH context                            */
+	UCHAR		CK[16];				/* the key                                    */
+	UCHAR		coefficient[16];	/* current aes counter mode coefficients      */
+	ULONGLONG	accum;				/* accumulated mic, reduced to u32 in final() */
+	UINT		position;			/* current position (byte offset) in message  */
+	UCHAR		part[4];			/* for conversion of message to u32 for mmh   */
+}	MIC_CONTEXT, *PMIC_CONTEXT;
+
+VOID	CKIP_key_permute(
+	OUT	UCHAR	*PK,			/* output permuted key */
+	IN	UCHAR	*CK,			/* input CKIP key */
+	IN	UCHAR	toDsFromDs,		/* input toDs/FromDs bits */
+	IN	UCHAR	*piv);			/* input pointer to IV */
+
+VOID	RTMPCkipMicInit(
+	IN	PMIC_CONTEXT		pContext,
+	IN	PUCHAR				CK);
+
+VOID RTMPMicUpdate(
+    IN  PMIC_CONTEXT        pContext,
+    IN  PUCHAR              pOctets,
+    IN  INT                 len);
+
+ULONG RTMPMicGetCoefficient(
+    IN  PMIC_CONTEXT         pContext);
+
+VOID xor_128(
+    IN  PUCHAR              a,
+    IN  PUCHAR              b,
+    OUT PUCHAR              out);
+
+UCHAR RTMPCkipSbox(
+    IN  UCHAR               a);
+
+VOID xor_32(
+    IN  PUCHAR              a,
+    IN  PUCHAR              b,
+    OUT PUCHAR              out);
+
+VOID next_key(
+    IN  PUCHAR              key,
+    IN  INT                 round);
+
+VOID byte_sub(
+    IN  PUCHAR              in,
+    OUT PUCHAR              out);
+
+VOID shift_row(
+    IN  PUCHAR              in,
+    OUT PUCHAR              out);
+
+VOID mix_column(
+    IN  PUCHAR              in,
+    OUT PUCHAR              out);
+
+VOID RTMPAesEncrypt(
+    IN  PUCHAR              key,
+    IN  PUCHAR              data,
+    IN  PUCHAR              ciphertext);
+
+VOID RTMPMicFinal(
+    IN  PMIC_CONTEXT        pContext,
+    OUT UCHAR               digest[4]);
+
+VOID RTMPCkipInsertCMIC(
+    IN  PRTMP_ADAPTER   pAd,
+    OUT PUCHAR          pMIC,
+    IN  PUCHAR          p80211hdr,
+    IN  PNDIS_PACKET    pPacket,
+    IN  PCIPHER_KEY     pKey,
+    IN  PUCHAR          mic_snap);
+
+#endif //__RTMP_CKIPMIC_H__
diff --git a/drivers/staging/rt2870/rtmp_def.h b/drivers/staging/rt2870/rtmp_def.h
new file mode 100644
index 000000000000..c0f7561a9d9d
--- /dev/null
+++ b/drivers/staging/rt2870/rtmp_def.h
@@ -0,0 +1,1622 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rtmp_def.h
+
+    Abstract:
+    Miniport related definition header
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Paul Lin    08-01-2002    created
+    John Chang  08-05-2003    add definition for 11g & other drafts
+*/
+#ifndef __RTMP_DEF_H__
+#define __RTMP_DEF_H__
+
+#include "oid.h"
+
+//
+//  Debug information verbosity: lower values indicate higher urgency
+//
+#define RT_DEBUG_OFF        0
+#define RT_DEBUG_ERROR      1
+#define RT_DEBUG_WARN       2
+#define RT_DEBUG_TRACE      3
+#define RT_DEBUG_INFO       4
+#define RT_DEBUG_LOUD       5
+
+#define NIC_TAG             ((ULONG)'0682')
+#define NIC_DBG_STRING      ("**RT28xx**")
+
+#ifdef SNMP_SUPPORT
+// for snmp
+// to get manufacturer OUI, kathy, 2008_0220
+#define ManufacturerOUI_LEN			3
+#define ManufacturerNAME			("Ralink Technology Company.")
+#define	ResourceTypeIdName			("Ralink_ID")
+#endif
+
+
+//#define PACKED
+
+#define RALINK_2883_VERSION		((UINT32)0x28830300)
+#define RALINK_2880E_VERSION	((UINT32)0x28720200)
+#define RALINK_3070_VERSION		((UINT32)0x30700200)
+
+//
+// NDIS version in use by the NIC driver.
+// The high byte is the major version. The low byte is the minor version.
+//
+#ifdef  NDIS51_MINIPORT
+#define NIC_DRIVER_VERSION      0x0501
+#else
+#define NIC_DRIVER_VERSION      0x0500
+#endif
+
+//
+// NDIS media type, current is ethernet, change if native wireless supported
+//
+#define NIC_MEDIA_TYPE          NdisMedium802_3
+#define NIC_PCI_HDR_LENGTH      0xe2
+#define NIC_MAX_PACKET_SIZE     2304
+#define NIC_HEADER_SIZE         14
+#define MAX_MAP_REGISTERS_NEEDED 32
+#define MIN_MAP_REGISTERS_NEEDED 2   //Todo: should consider fragment issue.
+
+//
+// interface type, we use PCI
+//
+#define NIC_INTERFACE_TYPE      NdisInterfacePci
+#define NIC_INTERRUPT_MODE      NdisInterruptLevelSensitive
+
+//
+// buffer size passed in NdisMQueryAdapterResources
+// We should only need three adapter resources (IO, interrupt and memory),
+// Some devices get extra resources, so have room for 10 resources
+//                    UF_SIZE   (sizeof(NDIS_RESOURCE_LIST) + (10*sizeof(CM_PARTIAL_RESOURCE_DESCRIPTOR)))
+
+
+#define NIC_RESOURCE_B//
+// IO space length
+//
+#define NIC_MAP_IOSPACE_LENGTH  sizeof(CSR_STRUC)
+
+#define MAX_RX_PKT_LEN	1520
+
+//
+// Entry number for each DMA descriptor ring
+//
+
+
+#ifdef RT2870
+#define TX_RING_SIZE            8 // 1
+#define PRIO_RING_SIZE          8
+#define MGMT_RING_SIZE       32 // PRIO_RING_SIZE
+#define RX_RING_SIZE            8
+#define MAX_TX_PROCESS          4
+#define LOCAL_TXBUF_SIZE        2048
+#endif // RT2870 //
+
+#ifdef MULTIPLE_CARD_SUPPORT
+// MC: Multple Cards
+#define MAX_NUM_OF_MULTIPLE_CARD		32
+#endif // MULTIPLE_CARD_SUPPORT //
+
+#define MAX_RX_PROCESS          128 //64 //32
+#define NUM_OF_LOCAL_TXBUF      2
+#define TXD_SIZE                16
+#define TXWI_SIZE               16
+#define RXD_SIZE               	16
+#define RXWI_SIZE             	16
+// TXINFO_SIZE + TXWI_SIZE + 802.11 Header Size + AMSDU sub frame header
+#define TX_DMA_1ST_BUFFER_SIZE  96    // only the 1st physical buffer is pre-allocated
+#define MGMT_DMA_BUFFER_SIZE    1536 //2048
+#define RX_BUFFER_AGGRESIZE     3840 //3904 //3968 //4096 //2048 //4096
+#define RX_BUFFER_NORMSIZE      3840 //3904 //3968 //4096 //2048 //4096
+#define TX_BUFFER_NORMSIZE		RX_BUFFER_NORMSIZE
+#define MAX_FRAME_SIZE          2346                    // Maximum 802.11 frame size
+#define MAX_AGGREGATION_SIZE    3840 //3904 //3968 //4096
+#define MAX_NUM_OF_TUPLE_CACHE  2
+#define MAX_MCAST_LIST_SIZE     32
+#define MAX_LEN_OF_VENDOR_DESC  64
+//#define MAX_SIZE_OF_MCAST_PSQ   (NUM_OF_LOCAL_TXBUF >> 2) // AP won't spend more than 1/4 of total buffers on M/BCAST PSQ
+#define MAX_SIZE_OF_MCAST_PSQ               32
+
+#define MAX_RX_PROCESS_CNT	(RX_RING_SIZE)
+
+
+#define MAX_PACKETS_IN_QUEUE				(512) //(512)    // to pass WMM A5-WPAPSK
+#define MAX_PACKETS_IN_MCAST_PS_QUEUE		32
+#define MAX_PACKETS_IN_PS_QUEUE				128	//32
+#define WMM_NUM_OF_AC                       4  /* AC0, AC1, AC2, and AC3 */
+
+
+
+// RxFilter
+#define STANORMAL	 0x17f97
+#define APNORMAL	 0x15f97
+//
+//  RTMP_ADAPTER flags
+//
+#define fRTMP_ADAPTER_MAP_REGISTER          0x00000001
+#define fRTMP_ADAPTER_INTERRUPT_IN_USE      0x00000002
+#define fRTMP_ADAPTER_HARDWARE_ERROR        0x00000004
+#define fRTMP_ADAPTER_SCATTER_GATHER        0x00000008
+#define fRTMP_ADAPTER_SEND_PACKET_ERROR     0x00000010
+#define fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS 0x00000020
+#define fRTMP_ADAPTER_HALT_IN_PROGRESS      0x00000040
+#define fRTMP_ADAPTER_RESET_IN_PROGRESS     0x00000080
+#define fRTMP_ADAPTER_NIC_NOT_EXIST         0x00000100
+#define fRTMP_ADAPTER_TX_RING_ALLOCATED     0x00000200
+#define fRTMP_ADAPTER_REMOVE_IN_PROGRESS    0x00000400
+#define fRTMP_ADAPTER_MIMORATE_INUSED       0x00000800
+#define fRTMP_ADAPTER_RX_RING_ALLOCATED     0x00001000
+#define fRTMP_ADAPTER_INTERRUPT_ACTIVE      0x00002000
+#define fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS  0x00004000
+#define	fRTMP_ADAPTER_REASSOC_IN_PROGRESS	0x00008000
+#define	fRTMP_ADAPTER_MEDIA_STATE_PENDING	0x00010000
+#define	fRTMP_ADAPTER_RADIO_OFF				0x00020000
+#define fRTMP_ADAPTER_BULKOUT_RESET			0x00040000
+#define	fRTMP_ADAPTER_BULKIN_RESET			0x00080000
+#define fRTMP_ADAPTER_RDG_ACTIVE			0x00100000
+#define fRTMP_ADAPTER_DYNAMIC_BE_TXOP_ACTIVE 0x00200000
+#define fRTMP_ADAPTER_SCAN_2040 			0x04000000
+#define	fRTMP_ADAPTER_RADIO_MEASUREMENT		0x08000000
+
+#define fRTMP_ADAPTER_START_UP         		0x10000000	//Devive already initialized and enabled Tx/Rx.
+#define fRTMP_ADAPTER_MEDIA_STATE_CHANGE    0x20000000
+#define fRTMP_ADAPTER_IDLE_RADIO_OFF        0x40000000
+
+// Lock bit for accessing different ring buffers
+//#define fRTMP_ADAPTER_TX_RING_BUSY        0x80000000
+//#define fRTMP_ADAPTER_MGMT_RING_BUSY      0x40000000
+//#define fRTMP_ADAPTER_ATIM_RING_BUSY      0x20000000
+//#define fRTMP_ADAPTER_RX_RING_BUSY        0x10000000
+
+// Lock bit for accessing different queue
+//#define   fRTMP_ADAPTER_TX_QUEUE_BUSY     0x08000000
+//#define   fRTMP_ADAPTER_MGMT_QUEUE_BUSY   0x04000000
+
+//
+//  STA operation status flags
+//
+#define fOP_STATUS_INFRA_ON                 0x00000001
+#define fOP_STATUS_ADHOC_ON                 0x00000002
+#define fOP_STATUS_BG_PROTECTION_INUSED     0x00000004
+#define fOP_STATUS_SHORT_SLOT_INUSED        0x00000008
+#define fOP_STATUS_SHORT_PREAMBLE_INUSED    0x00000010
+#define fOP_STATUS_RECEIVE_DTIM             0x00000020
+//#define fOP_STATUS_TX_RATE_SWITCH_ENABLED   0x00000040
+#define fOP_STATUS_MEDIA_STATE_CONNECTED    0x00000080
+#define fOP_STATUS_WMM_INUSED               0x00000100
+#define fOP_STATUS_AGGREGATION_INUSED       0x00000200
+#define fOP_STATUS_DOZE                     0x00000400  // debug purpose
+#define fOP_STATUS_PIGGYBACK_INUSED         0x00000800  // piggy-back, and aggregation
+#define fOP_STATUS_APSD_INUSED				0x00001000
+#define fOP_STATUS_TX_AMSDU_INUSED			0x00002000
+#define fOP_STATUS_MAX_RETRY_ENABLED		0x00004000
+#define fOP_STATUS_WAKEUP_NOW               0x00008000
+#define fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE       0x00020000
+
+#ifdef DOT11N_DRAFT3
+#define fOP_STATUS_SCAN_2040               	    0x00040000
+#endif // DOT11N_DRAFT3 //
+
+#define CCKSETPROTECT		0x1
+#define OFDMSETPROTECT		0x2
+#define MM20SETPROTECT		0x4
+#define MM40SETPROTECT		0x8
+#define GF20SETPROTECT		0x10
+#define GR40SETPROTECT		0x20
+#define ALLN_SETPROTECT		(GR40SETPROTECT | GF20SETPROTECT | MM40SETPROTECT | MM20SETPROTECT)
+
+//
+//  AP's client table operation status flags
+//
+#define fCLIENT_STATUS_WMM_CAPABLE          0x00000001  // CLIENT can parse QOS DATA frame
+#define fCLIENT_STATUS_AGGREGATION_CAPABLE  0x00000002  // CLIENT can receive Ralink's proprietary TX aggregation frame
+#define fCLIENT_STATUS_PIGGYBACK_CAPABLE    0x00000004  // CLIENT support piggy-back
+#define fCLIENT_STATUS_AMSDU_INUSED			0x00000008
+#define fCLIENT_STATUS_SGI20_CAPABLE		0x00000010
+#define fCLIENT_STATUS_SGI40_CAPABLE		0x00000020
+#define fCLIENT_STATUS_TxSTBC_CAPABLE		0x00000040
+#define fCLIENT_STATUS_RxSTBC_CAPABLE		0x00000080
+#define fCLIENT_STATUS_HTC_CAPABLE			0x00000100
+#define fCLIENT_STATUS_RDG_CAPABLE			0x00000200
+#define fCLIENT_STATUS_MCSFEEDBACK_CAPABLE  0x00000400
+#define fCLIENT_STATUS_APSD_CAPABLE         0x00000800  /* UAPSD STATION */
+
+#ifdef DOT11N_DRAFT3
+#define fCLIENT_STATUS_BSSCOEXIST_CAPABLE	0x00001000
+#endif // DOT11N_DRAFT3 //
+
+#define fCLIENT_STATUS_RALINK_CHIPSET		0x00100000
+//
+//  STA configuration flags
+//
+//#define fSTA_CFG_ENABLE_TX_BURST          0x00000001
+
+// 802.11n Operating Mode Definition. 0-3 also used in ASICUPdateProtect switch case
+#define HT_NO_PROTECT	0
+#define HT_LEGACY_PROTECT	1
+#define HT_40_PROTECT	2
+#define HT_2040_PROTECT	3
+#define HT_RTSCTS_6M	7
+//following is our own definition in order to turn on our ASIC protection register in INFRASTRUCTURE.
+#define HT_ATHEROS	8	// rt2860c has problem with atheros chip. we need to turn on RTS/CTS .
+#define HT_FORCERTSCTS	9	// Force turn on RTS/CTS first. then go to evaluate if this force RTS is necessary.
+
+//
+// RX Packet Filter control flags. Apply on pAd->PacketFilter
+//
+#define fRX_FILTER_ACCEPT_DIRECT            NDIS_PACKET_TYPE_DIRECTED
+#define fRX_FILTER_ACCEPT_MULTICAST         NDIS_PACKET_TYPE_MULTICAST
+#define fRX_FILTER_ACCEPT_BROADCAST         NDIS_PACKET_TYPE_BROADCAST
+#define fRX_FILTER_ACCEPT_ALL_MULTICAST     NDIS_PACKET_TYPE_ALL_MULTICAST
+
+//
+// Error code section
+//
+// NDIS_ERROR_CODE_ADAPTER_NOT_FOUND
+#define ERRLOG_READ_PCI_SLOT_FAILED     0x00000101L
+#define ERRLOG_WRITE_PCI_SLOT_FAILED    0x00000102L
+#define ERRLOG_VENDOR_DEVICE_NOMATCH    0x00000103L
+
+// NDIS_ERROR_CODE_ADAPTER_DISABLED
+#define ERRLOG_BUS_MASTER_DISABLED      0x00000201L
+
+// NDIS_ERROR_CODE_UNSUPPORTED_CONFIGURATION
+#define ERRLOG_INVALID_SPEED_DUPLEX     0x00000301L
+#define ERRLOG_SET_SECONDARY_FAILED     0x00000302L
+
+// NDIS_ERROR_CODE_OUT_OF_RESOURCES
+#define ERRLOG_OUT_OF_MEMORY            0x00000401L
+#define ERRLOG_OUT_OF_SHARED_MEMORY     0x00000402L
+#define ERRLOG_OUT_OF_MAP_REGISTERS     0x00000403L
+#define ERRLOG_OUT_OF_BUFFER_POOL       0x00000404L
+#define ERRLOG_OUT_OF_NDIS_BUFFER       0x00000405L
+#define ERRLOG_OUT_OF_PACKET_POOL       0x00000406L
+#define ERRLOG_OUT_OF_NDIS_PACKET       0x00000407L
+#define ERRLOG_OUT_OF_LOOKASIDE_MEMORY  0x00000408L
+
+// NDIS_ERROR_CODE_HARDWARE_FAILURE
+#define ERRLOG_SELFTEST_FAILED          0x00000501L
+#define ERRLOG_INITIALIZE_ADAPTER       0x00000502L
+#define ERRLOG_REMOVE_MINIPORT          0x00000503L
+
+// NDIS_ERROR_CODE_RESOURCE_CONFLICT
+#define ERRLOG_MAP_IO_SPACE             0x00000601L
+#define ERRLOG_QUERY_ADAPTER_RESOURCES  0x00000602L
+#define ERRLOG_NO_IO_RESOURCE           0x00000603L
+#define ERRLOG_NO_INTERRUPT_RESOURCE    0x00000604L
+#define ERRLOG_NO_MEMORY_RESOURCE       0x00000605L
+
+
+// WDS definition
+#define	MAX_WDS_ENTRY               4
+#define WDS_PAIRWISE_KEY_OFFSET     60    // WDS links uses pairwise key#60 ~ 63 in ASIC pairwise key table
+
+#define	WDS_DISABLE_MODE            0
+#define	WDS_RESTRICT_MODE           1
+#define	WDS_BRIDGE_MODE             2
+#define	WDS_REPEATER_MODE           3
+#define	WDS_LAZY_MODE               4
+
+
+#define MAX_MESH_NUM				0
+
+#define MAX_APCLI_NUM				0
+#ifdef APCLI_SUPPORT
+#undef	MAX_APCLI_NUM
+#define MAX_APCLI_NUM				1
+#endif // APCLI_SUPPORT //
+
+#define MAX_MBSSID_NUM				1
+
+/* sanity check for apidx */
+#define MBSS_MR_APIDX_SANITY_CHECK(apidx) \
+    { if (apidx > MAX_MBSSID_NUM) { \
+          printk("%s> Error! apidx = %d > MAX_MBSSID_NUM!\n", __func__, apidx); \
+	  apidx = MAIN_MBSSID; } }
+
+#define VALID_WCID(_wcid)	((_wcid) > 0 && (_wcid) < MAX_LEN_OF_MAC_TABLE )
+
+#define MAIN_MBSSID                 0
+#define FIRST_MBSSID                1
+
+
+#define MAX_BEACON_SIZE				512
+// If the MAX_MBSSID_NUM is larger than 6,
+// it shall reserve some WCID space(wcid 222~253) for beacon frames.
+// -	these wcid 238~253 are reserved for beacon#6(ra6).
+// -	these wcid 222~237 are reserved for beacon#7(ra7).
+#if defined(MAX_MBSSID_NUM) && (MAX_MBSSID_NUM == 8)
+#define HW_RESERVED_WCID	222
+#elif defined(MAX_MBSSID_NUM) && (MAX_MBSSID_NUM == 7)
+#define HW_RESERVED_WCID	238
+#else
+#define HW_RESERVED_WCID	255
+#endif
+
+// Then dedicate wcid of DFS and Carrier-Sense.
+#define DFS_CTS_WCID 		(HW_RESERVED_WCID - 1)
+#define CS_CTS_WCID 		(HW_RESERVED_WCID - 2)
+#define LAST_SPECIFIC_WCID	(HW_RESERVED_WCID - 2)
+
+// If MAX_MBSSID_NUM is 8, the maximum available wcid for the associated STA is 211.
+// If MAX_MBSSID_NUM is 7, the maximum available wcid for the associated STA is 228.
+#define MAX_AVAILABLE_CLIENT_WCID	(LAST_SPECIFIC_WCID - MAX_MBSSID_NUM - 1)
+
+// TX need WCID to find Cipher Key
+// these wcid 212 ~ 219 are reserved for bc/mc packets if MAX_MBSSID_NUM is 8.
+#define GET_GroupKey_WCID(__wcid, __bssidx) \
+	{										\
+		__wcid = LAST_SPECIFIC_WCID - (MAX_MBSSID_NUM) + __bssidx;	\
+	}
+
+#define IsGroupKeyWCID(__wcid) (((__wcid) < LAST_SPECIFIC_WCID) && ((__wcid) >= (LAST_SPECIFIC_WCID - (MAX_MBSSID_NUM))))
+
+
+// definition to support multiple BSSID
+#define BSS0                            0
+#define BSS1                            1
+#define BSS2                            2
+#define BSS3                            3
+#define BSS4                            4
+#define BSS5                            5
+#define BSS6                            6
+#define BSS7                            7
+
+
+//============================================================
+// Length definitions
+#define PEER_KEY_NO                     2
+#define MAC_ADDR_LEN                    6
+#define TIMESTAMP_LEN                   8
+#define MAX_LEN_OF_SUPPORTED_RATES      MAX_LENGTH_OF_SUPPORT_RATES // 1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54
+#define MAX_LEN_OF_KEY                  32      // 32 octets == 256 bits, Redefine for WPA
+#define MAX_NUM_OF_CHANNELS             MAX_NUM_OF_CHS      // 14 channels @2.4G +  12@UNII + 4 @MMAC + 11 @HiperLAN2 + 7 @Japan + 1 as NULL termination
+#define MAX_NUM_OF_11JCHANNELS             20      // 14 channels @2.4G +  12@UNII + 4 @MMAC + 11 @HiperLAN2 + 7 @Japan + 1 as NULL termination
+#define MAX_LEN_OF_SSID                 32
+#define CIPHER_TEXT_LEN                 128
+#define HASH_TABLE_SIZE                 256
+#define MAX_VIE_LEN                     1024   // New for WPA cipher suite variable IE sizes.
+#define MAX_SUPPORT_MCS             32
+
+//============================================================
+// ASIC WCID Table definition.
+//============================================================
+#define BSSID_WCID		1	// in infra mode, always put bssid with this WCID
+#define MCAST_WCID	0x0
+#define BSS0Mcast_WCID	0x0
+#define BSS1Mcast_WCID	0xf8
+#define BSS2Mcast_WCID	0xf9
+#define BSS3Mcast_WCID	0xfa
+#define BSS4Mcast_WCID	0xfb
+#define BSS5Mcast_WCID	0xfc
+#define BSS6Mcast_WCID	0xfd
+#define BSS7Mcast_WCID	0xfe
+#define RESERVED_WCID		0xff
+
+#define MAX_NUM_OF_ACL_LIST				MAX_NUMBER_OF_ACL
+
+#define MAX_LEN_OF_MAC_TABLE            MAX_NUMBER_OF_MAC // if MAX_MBSSID_NUM is 8, this value can't be larger than 211
+
+#if MAX_LEN_OF_MAC_TABLE>MAX_AVAILABLE_CLIENT_WCID
+#error MAX_LEN_OF_MAC_TABLE can not be larger than MAX_AVAILABLE_CLIENT_WCID!!!!
+#endif
+
+#define MAX_NUM_OF_WDS_LINK_PERBSSID	            3
+#define MAX_NUM_OF_WDS_LINK	            (MAX_NUM_OF_WDS_LINK_PERBSSID*MAX_MBSSID_NUM)
+#define MAX_NUM_OF_EVENT                MAX_NUMBER_OF_EVENT
+#define WDS_LINK_START_WCID				(MAX_LEN_OF_MAC_TABLE-1)
+
+#define NUM_OF_TID			8
+#define MAX_AID_BA                    4
+#define MAX_LEN_OF_BA_REC_TABLE          ((NUM_OF_TID * MAX_LEN_OF_MAC_TABLE)/2)//   (NUM_OF_TID*MAX_AID_BA + 32)	 //Block ACK recipient
+#define MAX_LEN_OF_BA_ORI_TABLE          ((NUM_OF_TID * MAX_LEN_OF_MAC_TABLE)/2)//   (NUM_OF_TID*MAX_AID_BA + 32)   // Block ACK originator
+#define MAX_LEN_OF_BSS_TABLE             64
+#define MAX_REORDERING_MPDU_NUM			 512
+
+// key related definitions
+#define SHARE_KEY_NUM                   4
+#define MAX_LEN_OF_SHARE_KEY            16    // byte count
+#define MAX_LEN_OF_PEER_KEY             16    // byte count
+#define PAIRWISE_KEY_NUM                64    // in MAC ASIC pairwise key table
+#define GROUP_KEY_NUM                   4
+#define PMK_LEN                         32
+#define WDS_PAIRWISE_KEY_OFFSET         60    // WDS links uses pairwise key#60 ~ 63 in ASIC pairwise key table
+#define	PMKID_NO                        4     // Number of PMKID saved supported
+#define MAX_LEN_OF_MLME_BUFFER          2048
+
+// power status related definitions
+#define PWR_ACTIVE                      0
+#define PWR_SAVE                        1
+#define PWR_MMPS                        2			//MIMO power save
+//#define PWR_UNKNOWN                   2
+
+// Auth and Assoc mode related definitions
+#define AUTH_MODE_OPEN                  0x00
+#define AUTH_MODE_KEY                   0x01
+//#define AUTH_MODE_AUTO_SWITCH         0x03
+//#define AUTH_MODE_DEAUTH              0x04
+//#define AUTH_MODE_UPLAYER             0x05 // reserved for 802.11i use
+
+// BSS Type definitions
+#define BSS_ADHOC                       0  // = Ndis802_11IBSS
+#define BSS_INFRA                       1  // = Ndis802_11Infrastructure
+#define BSS_ANY                         2  // = Ndis802_11AutoUnknown
+#define BSS_MONITOR			            3  // = Ndis802_11Monitor
+
+
+// Reason code definitions
+#define REASON_RESERVED                 0
+#define REASON_UNSPECIFY                1
+#define REASON_NO_LONGER_VALID          2
+#define REASON_DEAUTH_STA_LEAVING       3
+#define REASON_DISASSOC_INACTIVE        4
+#define REASON_DISASSPC_AP_UNABLE       5
+#define REASON_CLS2ERR                  6
+#define REASON_CLS3ERR                  7
+#define REASON_DISASSOC_STA_LEAVING     8
+#define REASON_STA_REQ_ASSOC_NOT_AUTH   9
+#define REASON_INVALID_IE               13
+#define REASON_MIC_FAILURE              14
+#define REASON_4_WAY_TIMEOUT            15
+#define REASON_GROUP_KEY_HS_TIMEOUT     16
+#define REASON_IE_DIFFERENT             17
+#define REASON_MCIPHER_NOT_VALID        18
+#define REASON_UCIPHER_NOT_VALID        19
+#define REASON_AKMP_NOT_VALID           20
+#define REASON_UNSUPPORT_RSNE_VER       21
+#define REASON_INVALID_RSNE_CAP         22
+#define REASON_8021X_AUTH_FAIL          23
+#define REASON_CIPHER_SUITE_REJECTED    24
+#define REASON_DECLINED                 37
+
+#define REASON_QOS_UNSPECIFY              32
+#define REASON_QOS_LACK_BANDWIDTH         33
+#define REASON_POOR_CHANNEL_CONDITION     34
+#define REASON_QOS_OUTSIDE_TXOP_LIMITION  35
+#define REASON_QOS_QSTA_LEAVING_QBSS      36
+#define REASON_QOS_UNWANTED_MECHANISM     37
+#define REASON_QOS_MECH_SETUP_REQUIRED    38
+#define REASON_QOS_REQUEST_TIMEOUT        39
+#define REASON_QOS_CIPHER_NOT_SUPPORT     45
+
+// Status code definitions
+#define MLME_SUCCESS                    0
+#define MLME_UNSPECIFY_FAIL             1
+#define MLME_CANNOT_SUPPORT_CAP         10
+#define MLME_REASSOC_DENY_ASSOC_EXIST   11
+#define MLME_ASSOC_DENY_OUT_SCOPE       12
+#define MLME_ALG_NOT_SUPPORT            13
+#define MLME_SEQ_NR_OUT_OF_SEQUENCE     14
+#define MLME_REJ_CHALLENGE_FAILURE      15
+#define MLME_REJ_TIMEOUT                  16
+#define MLME_ASSOC_REJ_UNABLE_HANDLE_STA  17
+#define MLME_ASSOC_REJ_DATA_RATE          18
+
+#define MLME_ASSOC_REJ_NO_EXT_RATE        22
+#define MLME_ASSOC_REJ_NO_EXT_RATE_PBCC   23
+#define MLME_ASSOC_REJ_NO_CCK_OFDM        24
+
+#define MLME_QOS_UNSPECIFY                32
+#define MLME_REQUEST_DECLINED             37
+#define MLME_REQUEST_WITH_INVALID_PARAM   38
+#define MLME_DLS_NOT_ALLOW_IN_QBSS        48
+#define MLME_DEST_STA_NOT_IN_QBSS         49
+#define MLME_DEST_STA_IS_NOT_A_QSTA       50
+
+#define MLME_INVALID_FORMAT             0x51
+#define MLME_FAIL_NO_RESOURCE           0x52
+#define MLME_STATE_MACHINE_REJECT       0x53
+#define MLME_MAC_TABLE_FAIL             0x54
+
+// IE code
+#define IE_SSID                         0
+#define IE_SUPP_RATES                   1
+#define IE_FH_PARM                      2
+#define IE_DS_PARM                      3
+#define IE_CF_PARM                      4
+#define IE_TIM                          5
+#define IE_IBSS_PARM                    6
+#define IE_COUNTRY                      7     // 802.11d
+#define IE_802_11D_REQUEST              10    // 802.11d
+#define IE_QBSS_LOAD                    11    // 802.11e d9
+#define IE_EDCA_PARAMETER               12    // 802.11e d9
+#define IE_TSPEC                        13    // 802.11e d9
+#define IE_TCLAS                        14    // 802.11e d9
+#define IE_SCHEDULE                     15    // 802.11e d9
+#define IE_CHALLENGE_TEXT               16
+#define IE_POWER_CONSTRAINT             32    // 802.11h d3.3
+#define IE_POWER_CAPABILITY             33    // 802.11h d3.3
+#define IE_TPC_REQUEST                  34    // 802.11h d3.3
+#define IE_TPC_REPORT                   35    // 802.11h d3.3
+#define IE_SUPP_CHANNELS                36    // 802.11h d3.3
+#define IE_CHANNEL_SWITCH_ANNOUNCEMENT  37    // 802.11h d3.3
+#define IE_MEASUREMENT_REQUEST          38    // 802.11h d3.3
+#define IE_MEASUREMENT_REPORT           39    // 802.11h d3.3
+#define IE_QUIET                        40    // 802.11h d3.3
+#define IE_IBSS_DFS                     41    // 802.11h d3.3
+#define IE_ERP                          42    // 802.11g
+#define IE_TS_DELAY                     43    // 802.11e d9
+#define IE_TCLAS_PROCESSING             44    // 802.11e d9
+#define IE_QOS_CAPABILITY               46    // 802.11e d6
+#define IE_HT_CAP                       45    // 802.11n d1. HT CAPABILITY. ELEMENT ID TBD
+#define IE_AP_CHANNEL_REPORT			51    // 802.11k d6
+#define IE_HT_CAP2                         52    // 802.11n d1. HT CAPABILITY. ELEMENT ID TBD
+#define IE_RSN                          48    // 802.11i d3.0
+#define IE_WPA2                         48    // WPA2
+#define IE_EXT_SUPP_RATES               50    // 802.11g
+#define IE_SUPP_REG_CLASS               59    // 802.11y. Supported regulatory classes.
+#define IE_EXT_CHANNEL_SWITCH_ANNOUNCEMENT	60	// 802.11n
+#define IE_ADD_HT                         61    // 802.11n d1. ADDITIONAL HT CAPABILITY. ELEMENT ID TBD
+#define IE_ADD_HT2                        53    // 802.11n d1. ADDITIONAL HT CAPABILITY. ELEMENT ID TBD
+
+
+// For 802.11n D3.03
+//#define IE_NEW_EXT_CHA_OFFSET             62    // 802.11n d1. New extension channel offset elemet
+#define IE_SECONDARY_CH_OFFSET		62	// 802.11n D3.03	Secondary Channel Offset element
+#define IE_WAPI							68		// WAPI information element
+#define IE_2040_BSS_COEXIST               72    // 802.11n D3.0.3
+#define IE_2040_BSS_INTOLERANT_REPORT     73    // 802.11n D3.03
+#define IE_OVERLAPBSS_SCAN_PARM           74    // 802.11n D3.03
+#define IE_EXT_CAPABILITY                127   // 802.11n D3.03
+
+
+#define IE_WPA                          221   // WPA
+#define IE_VENDOR_SPECIFIC              221   // Wifi WMM (WME)
+
+#define OUI_BROADCOM_HT              51   //
+#define OUI_BROADCOM_HTADD              52   //
+#define OUI_PREN_HT_CAP              51   //
+#define OUI_PREN_ADD_HT              52   //
+
+// CCX information
+#define IE_AIRONET_CKIP                 133   // CCX1.0 ID 85H for CKIP
+#define IE_AP_TX_POWER                  150   // CCX 2.0 for AP transmit power
+#define IE_MEASUREMENT_CAPABILITY       221   // CCX 2.0
+#define IE_CCX_V2                       221
+#define IE_AIRONET_IPADDRESS            149   // CCX ID 95H for IP Address
+#define IE_AIRONET_CCKMREASSOC          156   // CCX ID 9CH for CCKM Reassociation Request element
+#define CKIP_NEGOTIATION_LENGTH         30
+#define AIRONET_IPADDRESS_LENGTH        10
+#define AIRONET_CCKMREASSOC_LENGTH      24
+
+// ========================================================
+// MLME state machine definition
+// ========================================================
+
+// STA MLME state mahcines
+#define ASSOC_STATE_MACHINE             1
+#define AUTH_STATE_MACHINE              2
+#define AUTH_RSP_STATE_MACHINE          3
+#define SYNC_STATE_MACHINE              4
+#define MLME_CNTL_STATE_MACHINE         5
+#define WPA_PSK_STATE_MACHINE           6
+#define LEAP_STATE_MACHINE              7
+#define AIRONET_STATE_MACHINE           8
+#define ACTION_STATE_MACHINE           9
+
+// AP MLME state machines
+#define AP_ASSOC_STATE_MACHINE          11
+#define AP_AUTH_STATE_MACHINE           12
+#define AP_AUTH_RSP_STATE_MACHINE       13
+#define AP_SYNC_STATE_MACHINE           14
+#define AP_CNTL_STATE_MACHINE           15
+#define AP_WPA_STATE_MACHINE            16
+
+#ifdef QOS_DLS_SUPPORT
+#define DLS_STATE_MACHINE               26
+#endif // QOS_DLS_SUPPORT //
+
+//
+// STA's CONTROL/CONNECT state machine: states, events, total function #
+//
+#define CNTL_IDLE                       0
+#define CNTL_WAIT_DISASSOC              1
+#define CNTL_WAIT_JOIN                  2
+#define CNTL_WAIT_REASSOC               3
+#define CNTL_WAIT_START                 4
+#define CNTL_WAIT_AUTH                  5
+#define CNTL_WAIT_ASSOC                 6
+#define CNTL_WAIT_AUTH2                 7
+#define CNTL_WAIT_OID_LIST_SCAN         8
+#define CNTL_WAIT_OID_DISASSOC          9
+#ifdef RT2870
+#define CNTL_WAIT_SCAN_FOR_CONNECT      10
+#endif // RT2870 //
+
+#define MT2_ASSOC_CONF                  34
+#define MT2_AUTH_CONF                   35
+#define MT2_DEAUTH_CONF                 36
+#define MT2_DISASSOC_CONF               37
+#define MT2_REASSOC_CONF                38
+#define MT2_PWR_MGMT_CONF               39
+#define MT2_JOIN_CONF                   40
+#define MT2_SCAN_CONF                   41
+#define MT2_START_CONF                  42
+#define MT2_GET_CONF                    43
+#define MT2_SET_CONF                    44
+#define MT2_RESET_CONF                  45
+#define MT2_MLME_ROAMING_REQ            52
+
+#define CNTL_FUNC_SIZE                  1
+
+//
+// STA's ASSOC state machine: states, events, total function #
+//
+#define ASSOC_IDLE                      0
+#define ASSOC_WAIT_RSP                  1
+#define REASSOC_WAIT_RSP                2
+#define DISASSOC_WAIT_RSP               3
+#define MAX_ASSOC_STATE                 4
+
+#define ASSOC_MACHINE_BASE              0
+#define MT2_MLME_ASSOC_REQ              0
+#define MT2_MLME_REASSOC_REQ            1
+#define MT2_MLME_DISASSOC_REQ           2
+#define MT2_PEER_DISASSOC_REQ           3
+#define MT2_PEER_ASSOC_REQ              4
+#define MT2_PEER_ASSOC_RSP              5
+#define MT2_PEER_REASSOC_REQ            6
+#define MT2_PEER_REASSOC_RSP            7
+#define MT2_DISASSOC_TIMEOUT            8
+#define MT2_ASSOC_TIMEOUT               9
+#define MT2_REASSOC_TIMEOUT             10
+#define MAX_ASSOC_MSG                   11
+
+#define ASSOC_FUNC_SIZE                 (MAX_ASSOC_STATE * MAX_ASSOC_MSG)
+
+//
+// ACT state machine: states, events, total function #
+//
+#define ACT_IDLE                      0
+#define MAX_ACT_STATE                 1
+
+#define ACT_MACHINE_BASE              0
+
+//Those PEER_xx_CATE number is based on real Categary value in IEEE spec. Please don'es modify it by your self.
+//Category
+#define MT2_PEER_SPECTRUM_CATE              0
+#define MT2_PEER_QOS_CATE              1
+#define MT2_PEER_DLS_CATE             2
+#define MT2_PEER_BA_CATE             3
+#define MT2_PEER_PUBLIC_CATE             4
+#define MT2_PEER_RM_CATE             5
+#define MT2_PEER_HT_CATE             7	//	7.4.7
+#define MAX_PEER_CATE_MSG                   7
+#define MT2_MLME_ADD_BA_CATE             8
+#define MT2_MLME_ORI_DELBA_CATE             9
+#define MT2_MLME_REC_DELBA_CATE             10
+#define MT2_MLME_QOS_CATE              11
+#define MT2_MLME_DLS_CATE             12
+#define MT2_ACT_INVALID             13
+#define MAX_ACT_MSG                   14
+
+//Category field
+#define CATEGORY_SPECTRUM		0
+#define CATEGORY_QOS			1
+#define CATEGORY_DLS			2
+#define CATEGORY_BA			3
+#define CATEGORY_PUBLIC		4
+#define CATEGORY_RM			5
+#define CATEGORY_HT			7
+
+
+// DLS Action frame definition
+#define ACTION_DLS_REQUEST			0
+#define ACTION_DLS_RESPONSE			1
+#define ACTION_DLS_TEARDOWN			2
+
+//Spectrum  Action field value 802.11h 7.4.1
+#define SPEC_MRQ	0	// Request
+#define SPEC_MRP	1	//Report
+#define SPEC_TPCRQ	2
+#define SPEC_TPCRP	3
+#define SPEC_CHANNEL_SWITCH	4
+
+
+//BA  Action field value
+#define ADDBA_REQ	0
+#define ADDBA_RESP	1
+#define DELBA   2
+
+//Public's  Action field value in Public Category.  Some in 802.11y and some in 11n
+#define ACTION_BSS_2040_COEXIST				0	// 11n
+#define ACTION_DSE_ENABLEMENT					1	// 11y D9.0
+#define ACTION_DSE_DEENABLEMENT				2	// 11y D9.0
+#define ACTION_DSE_REG_LOCATION_ANNOUNCE	3	// 11y D9.0
+#define ACTION_EXT_CH_SWITCH_ANNOUNCE		4	// 11y D9.0
+#define ACTION_DSE_MEASUREMENT_REQ			5	// 11y D9.0
+#define ACTION_DSE_MEASUREMENT_REPORT		6	// 11y D9.0
+#define ACTION_MEASUREMENT_PILOT_ACTION		7  	// 11y D9.0
+#define ACTION_DSE_POWER_CONSTRAINT			8	// 11y D9.0
+
+
+//HT  Action field value
+#define NOTIFY_BW_ACTION				0
+#define SMPS_ACTION						1
+#define PSMP_ACTION   					2
+#define SETPCO_ACTION					3
+#define MIMO_CHA_MEASURE_ACTION			4
+#define MIMO_N_BEACONFORM				5
+#define MIMO_BEACONFORM					6
+#define ANTENNA_SELECT					7
+#define HT_INFO_EXCHANGE				8
+
+#define ACT_FUNC_SIZE                 (MAX_ACT_STATE * MAX_ACT_MSG)
+//
+// STA's AUTHENTICATION state machine: states, evvents, total function #
+//
+#define AUTH_REQ_IDLE                   0
+#define AUTH_WAIT_SEQ2                  1
+#define AUTH_WAIT_SEQ4                  2
+#define MAX_AUTH_STATE                  3
+
+#define AUTH_MACHINE_BASE               0
+#define MT2_MLME_AUTH_REQ               0
+#define MT2_PEER_AUTH_EVEN              1
+#define MT2_AUTH_TIMEOUT                2
+#define MAX_AUTH_MSG                    3
+
+#define AUTH_FUNC_SIZE                  (MAX_AUTH_STATE * MAX_AUTH_MSG)
+
+//
+// STA's AUTH_RSP state machine: states, events, total function #
+//
+#define AUTH_RSP_IDLE                   0
+#define AUTH_RSP_WAIT_CHAL              1
+#define MAX_AUTH_RSP_STATE              2
+
+#define AUTH_RSP_MACHINE_BASE           0
+#define MT2_AUTH_CHALLENGE_TIMEOUT      0
+#define MT2_PEER_AUTH_ODD               1
+#define MT2_PEER_DEAUTH                 2
+#define MAX_AUTH_RSP_MSG                3
+
+#define AUTH_RSP_FUNC_SIZE              (MAX_AUTH_RSP_STATE * MAX_AUTH_RSP_MSG)
+
+//
+// STA's SYNC state machine: states, events, total function #
+//
+#define SYNC_IDLE                       0  // merge NO_BSS,IBSS_IDLE,IBSS_ACTIVE and BSS in to 1 state
+#define JOIN_WAIT_BEACON                1
+#define SCAN_LISTEN                     2
+#define MAX_SYNC_STATE                  3
+
+#define SYNC_MACHINE_BASE               0
+#define MT2_MLME_SCAN_REQ               0
+#define MT2_MLME_JOIN_REQ               1
+#define MT2_MLME_START_REQ              2
+#define MT2_PEER_BEACON                 3
+#define MT2_PEER_PROBE_RSP              4
+#define MT2_PEER_ATIM                   5
+#define MT2_SCAN_TIMEOUT                6
+#define MT2_BEACON_TIMEOUT              7
+#define MT2_ATIM_TIMEOUT                8
+#define MT2_PEER_PROBE_REQ              9
+#define MAX_SYNC_MSG                    10
+
+#define SYNC_FUNC_SIZE                  (MAX_SYNC_STATE * MAX_SYNC_MSG)
+
+//Messages for the DLS state machine
+#define DLS_IDLE						0
+#define MAX_DLS_STATE					1
+
+#define DLS_MACHINE_BASE				0
+#define MT2_MLME_DLS_REQ			    0
+#define MT2_PEER_DLS_REQ			    1
+#define MT2_PEER_DLS_RSP			    2
+#define MT2_MLME_DLS_TEAR_DOWN		    3
+#define MT2_PEER_DLS_TEAR_DOWN		    4
+#define MAX_DLS_MSG				        5
+
+#define DLS_FUNC_SIZE					(MAX_DLS_STATE * MAX_DLS_MSG)
+
+//
+// STA's WPA-PSK State machine: states, events, total function #
+//
+#define WPA_PSK_IDLE					0
+#define MAX_WPA_PSK_STATE				1
+
+#define WPA_MACHINE_BASE                0
+#define MT2_EAPPacket                   0
+#define MT2_EAPOLStart                  1
+#define MT2_EAPOLLogoff                 2
+#define MT2_EAPOLKey                    3
+#define MT2_EAPOLASFAlert               4
+#define MAX_WPA_PSK_MSG                 5
+
+#define	WPA_PSK_FUNC_SIZE				(MAX_WPA_PSK_STATE * MAX_WPA_PSK_MSG)
+
+//
+// STA's CISCO-AIRONET State machine: states, events, total function #
+//
+#define AIRONET_IDLE					0
+#define	AIRONET_SCANNING				1
+#define MAX_AIRONET_STATE				2
+
+#define AIRONET_MACHINE_BASE		    0
+#define MT2_AIRONET_MSG				    0
+#define MT2_AIRONET_SCAN_REQ		    1
+#define MT2_AIRONET_SCAN_DONE		    2
+#define MAX_AIRONET_MSG				    3
+
+#define	AIRONET_FUNC_SIZE				(MAX_AIRONET_STATE * MAX_AIRONET_MSG)
+
+//
+// AP's CONTROL/CONNECT state machine: states, events, total function #
+//
+#define AP_CNTL_FUNC_SIZE               1
+
+//
+// AP's ASSOC state machine: states, events, total function #
+//
+#define AP_ASSOC_IDLE                   0
+#define AP_MAX_ASSOC_STATE              1
+
+#define AP_ASSOC_MACHINE_BASE           0
+#define APMT2_MLME_DISASSOC_REQ         0
+#define APMT2_PEER_DISASSOC_REQ         1
+#define APMT2_PEER_ASSOC_REQ            2
+#define APMT2_PEER_REASSOC_REQ          3
+#define APMT2_CLS3ERR                   4
+#define AP_MAX_ASSOC_MSG                5
+
+#define AP_ASSOC_FUNC_SIZE              (AP_MAX_ASSOC_STATE * AP_MAX_ASSOC_MSG)
+
+//
+// AP's AUTHENTICATION state machine: states, events, total function #
+//
+#define AP_AUTH_REQ_IDLE                0
+#define AP_MAX_AUTH_STATE               1
+
+#define AP_AUTH_MACHINE_BASE            0
+#define APMT2_MLME_DEAUTH_REQ           0
+#define APMT2_CLS2ERR                   1
+#define AP_MAX_AUTH_MSG                 2
+
+#define AP_AUTH_FUNC_SIZE               (AP_MAX_AUTH_STATE * AP_MAX_AUTH_MSG)
+
+//
+// AP's AUTH-RSP state machine: states, events, total function #
+//
+#define AP_AUTH_RSP_IDLE                0
+#define AP_MAX_AUTH_RSP_STATE           1
+
+#define AP_AUTH_RSP_MACHINE_BASE        0
+#define APMT2_AUTH_CHALLENGE_TIMEOUT    0
+#define APMT2_PEER_AUTH_ODD             1
+#define APMT2_PEER_DEAUTH               2
+#define AP_MAX_AUTH_RSP_MSG             3
+
+#define AP_AUTH_RSP_FUNC_SIZE           (AP_MAX_AUTH_RSP_STATE * AP_MAX_AUTH_RSP_MSG)
+
+//
+// AP's SYNC state machine: states, events, total function #
+//
+#define AP_SYNC_IDLE                    0
+#define AP_SCAN_LISTEN					1
+#define AP_MAX_SYNC_STATE               2
+
+#define AP_SYNC_MACHINE_BASE            0
+#define APMT2_PEER_PROBE_REQ            0
+#define APMT2_PEER_BEACON               1
+#define APMT2_MLME_SCAN_REQ				2
+#define APMT2_PEER_PROBE_RSP			3
+#define APMT2_SCAN_TIMEOUT				4
+#define APMT2_MLME_SCAN_CNCL			5
+#define AP_MAX_SYNC_MSG                 6
+
+#define AP_SYNC_FUNC_SIZE               (AP_MAX_SYNC_STATE * AP_MAX_SYNC_MSG)
+
+//
+// AP's WPA state machine: states, events, total function #
+//
+#define AP_WPA_PTK                      0
+#define AP_MAX_WPA_PTK_STATE            1
+
+#define AP_WPA_MACHINE_BASE             0
+#define APMT2_EAPPacket                 0
+#define APMT2_EAPOLStart                1
+#define APMT2_EAPOLLogoff               2
+#define APMT2_EAPOLKey                  3
+#define APMT2_EAPOLASFAlert             4
+#define AP_MAX_WPA_MSG                  5
+
+#define AP_WPA_FUNC_SIZE                (AP_MAX_WPA_PTK_STATE * AP_MAX_WPA_MSG)
+
+#ifdef APCLI_SUPPORT
+//ApCli authentication state machine
+#define APCLI_AUTH_REQ_IDLE                0
+#define APCLI_AUTH_WAIT_SEQ2               1
+#define APCLI_AUTH_WAIT_SEQ4               2
+#define APCLI_MAX_AUTH_STATE               3
+
+#define APCLI_AUTH_MACHINE_BASE            0
+#define APCLI_MT2_MLME_AUTH_REQ            0
+#define APCLI_MT2_MLME_DEAUTH_REQ          1
+#define APCLI_MT2_PEER_AUTH_EVEN           2
+#define APCLI_MT2_PEER_DEAUTH              3
+#define APCLI_MT2_AUTH_TIMEOUT             4
+#define APCLI_MAX_AUTH_MSG                 5
+
+#define APCLI_AUTH_FUNC_SIZE               (APCLI_MAX_AUTH_STATE * APCLI_MAX_AUTH_MSG)
+
+//ApCli association state machine
+#define APCLI_ASSOC_IDLE                   0
+#define APCLI_ASSOC_WAIT_RSP               1
+#define APCLI_MAX_ASSOC_STATE              2
+
+#define APCLI_ASSOC_MACHINE_BASE           0
+#define APCLI_MT2_MLME_ASSOC_REQ           0
+#define APCLI_MT2_MLME_DISASSOC_REQ        1
+#define APCLI_MT2_PEER_DISASSOC_REQ        2
+#define APCLI_MT2_PEER_ASSOC_RSP           3
+#define APCLI_MT2_ASSOC_TIMEOUT            4
+#define APCLI_MAX_ASSOC_MSG                5
+
+#define APCLI_ASSOC_FUNC_SIZE              (APCLI_MAX_ASSOC_STATE * APCLI_MAX_ASSOC_MSG)
+
+//ApCli sync state machine
+#define APCLI_SYNC_IDLE                   0  // merge NO_BSS,IBSS_IDLE,IBSS_ACTIVE and BSS in to 1 state
+#define APCLI_JOIN_WAIT_PROBE_RSP         1
+#define APCLI_MAX_SYNC_STATE              2
+
+#define APCLI_SYNC_MACHINE_BASE           0
+#define APCLI_MT2_MLME_PROBE_REQ          0
+#define APCLI_MT2_PEER_PROBE_RSP          1
+#define APCLI_MT2_PROBE_TIMEOUT           2
+#define APCLI_MAX_SYNC_MSG                3
+
+#define APCLI_SYNC_FUNC_SIZE              (APCLI_MAX_SYNC_STATE * APCLI_MAX_SYNC_MSG)
+
+//ApCli ctrl state machine
+#define APCLI_CTRL_DISCONNECTED           0  // merge NO_BSS,IBSS_IDLE,IBSS_ACTIVE and BSS in to 1 state
+#define APCLI_CTRL_PROBE                  1
+#define APCLI_CTRL_AUTH                   2
+#define APCLI_CTRL_AUTH_2                 3
+#define APCLI_CTRL_ASSOC                  4
+#define APCLI_CTRL_DEASSOC                5
+#define APCLI_CTRL_CONNECTED              6
+#define APCLI_MAX_CTRL_STATE              7
+
+#define APCLI_CTRL_MACHINE_BASE           0
+#define APCLI_CTRL_JOIN_REQ               0
+#define APCLI_CTRL_PROBE_RSP              1
+#define APCLI_CTRL_AUTH_RSP               2
+#define APCLI_CTRL_DISCONNECT_REQ         3
+#define APCLI_CTRL_PEER_DISCONNECT_REQ    4
+#define APCLI_CTRL_ASSOC_RSP              5
+#define APCLI_CTRL_DEASSOC_RSP            6
+#define APCLI_CTRL_JOIN_REQ_TIMEOUT       7
+#define APCLI_CTRL_AUTH_REQ_TIMEOUT       8
+#define APCLI_CTRL_ASSOC_REQ_TIMEOUT      9
+#define APCLI_MAX_CTRL_MSG                10
+
+#define APCLI_CTRL_FUNC_SIZE              (APCLI_MAX_CTRL_STATE * APCLI_MAX_CTRL_MSG)
+
+#if 0	// remove those variables by AlbertY
+// ApCli WPA state machine
+#define APCLI_WPA_PSK_IDLE				0
+#define APCLI_MAX_WPA_PSK_STATE			1
+
+// ApCli WPA MSG Type
+#define APCLI_WPA_MACHINE_BASE			0
+#define APCLI_MT2_EAPPacket				0
+#define APCLI_MT2_EAPOLStart			1
+#define APCLI_MT2_EAPOLLogoff			2
+#define APCLI_MT2_EAPOLKey				3
+#define APCLI_MT2_EAPOLASFAlert			4
+#define APCLI_MAX_WPA_PSK_MSG			5
+
+#define	APCLI_WPA_PSK_FUNC_SIZE			(APCLI_MAX_WPA_PSK_STATE * APCLI_MAX_WPA_PSK_MSG)
+#endif // end - 0 //
+
+#endif	// APCLI_SUPPORT //
+
+
+// =============================================================================
+
+// value domain of 802.11 header FC.Tyte, which is b3..b2 of the 1st-byte of MAC header
+#define BTYPE_MGMT                  0
+#define BTYPE_CNTL                  1
+#define BTYPE_DATA                  2
+
+// value domain of 802.11 MGMT frame's FC.subtype, which is b7..4 of the 1st-byte of MAC header
+#define SUBTYPE_ASSOC_REQ           0
+#define SUBTYPE_ASSOC_RSP           1
+#define SUBTYPE_REASSOC_REQ         2
+#define SUBTYPE_REASSOC_RSP         3
+#define SUBTYPE_PROBE_REQ           4
+#define SUBTYPE_PROBE_RSP           5
+#define SUBTYPE_BEACON              8
+#define SUBTYPE_ATIM                9
+#define SUBTYPE_DISASSOC            10
+#define SUBTYPE_AUTH                11
+#define SUBTYPE_DEAUTH              12
+#define SUBTYPE_ACTION              13
+#define SUBTYPE_ACTION_NO_ACK              14
+
+// value domain of 802.11 CNTL frame's FC.subtype, which is b7..4 of the 1st-byte of MAC header
+#define SUBTYPE_WRAPPER       	7
+#define SUBTYPE_BLOCK_ACK_REQ       8
+#define SUBTYPE_BLOCK_ACK           9
+#define SUBTYPE_PS_POLL             10
+#define SUBTYPE_RTS                 11
+#define SUBTYPE_CTS                 12
+#define SUBTYPE_ACK                 13
+#define SUBTYPE_CFEND               14
+#define SUBTYPE_CFEND_CFACK         15
+
+// value domain of 802.11 DATA frame's FC.subtype, which is b7..4 of the 1st-byte of MAC header
+#define SUBTYPE_DATA                0
+#define SUBTYPE_DATA_CFACK          1
+#define SUBTYPE_DATA_CFPOLL         2
+#define SUBTYPE_DATA_CFACK_CFPOLL   3
+#define SUBTYPE_NULL_FUNC           4
+#define SUBTYPE_CFACK               5
+#define SUBTYPE_CFPOLL              6
+#define SUBTYPE_CFACK_CFPOLL        7
+#define SUBTYPE_QDATA               8
+#define SUBTYPE_QDATA_CFACK         9
+#define SUBTYPE_QDATA_CFPOLL        10
+#define SUBTYPE_QDATA_CFACK_CFPOLL  11
+#define SUBTYPE_QOS_NULL            12
+#define SUBTYPE_QOS_CFACK           13
+#define SUBTYPE_QOS_CFPOLL          14
+#define SUBTYPE_QOS_CFACK_CFPOLL    15
+
+// ACK policy of QOS Control field bit 6:5
+#define NORMAL_ACK                  0x00  // b6:5 = 00
+#define NO_ACK                      0x20  // b6:5 = 01
+#define NO_EXPLICIT_ACK             0x40  // b6:5 = 10
+#define BLOCK_ACK                   0x60  // b6:5 = 11
+
+//
+// rtmp_data.c use these definition
+//
+#define LENGTH_802_11               24
+#define LENGTH_802_11_AND_H         30
+#define LENGTH_802_11_CRC_H         34
+#define LENGTH_802_11_CRC           28
+#define LENGTH_802_11_WITH_ADDR4    30
+#define LENGTH_802_3                14
+#define LENGTH_802_3_TYPE           2
+#define LENGTH_802_1_H              8
+#define LENGTH_EAPOL_H              4
+#define LENGTH_WMMQOS_H				2
+#define LENGTH_CRC                  4
+#define MAX_SEQ_NUMBER              0x0fff
+#define LENGTH_802_3_NO_TYPE		12
+#define LENGTH_802_1Q				4 /* VLAN related */
+
+// STA_CSR4.field.TxResult
+#define TX_RESULT_SUCCESS           0
+#define TX_RESULT_ZERO_LENGTH       1
+#define TX_RESULT_UNDER_RUN         2
+#define TX_RESULT_OHY_ERROR         4
+#define TX_RESULT_RETRY_FAIL        6
+
+// All PHY rate summary in TXD
+// Preamble MODE in TxD
+#define MODE_CCK	0
+#define MODE_OFDM   1
+#ifdef DOT11_N_SUPPORT
+#define MODE_HTMIX	2
+#define MODE_HTGREENFIELD	3
+#endif // DOT11_N_SUPPORT //
+// MCS for CCK.  BW.SGI.STBC are reserved
+#define MCS_LONGP_RATE_1                      0	 // long preamble CCK 1Mbps
+#define MCS_LONGP_RATE_2                      1	// long preamble CCK 1Mbps
+#define MCS_LONGP_RATE_5_5                    2
+#define MCS_LONGP_RATE_11                     3
+#define MCS_SHORTP_RATE_1                      4	 // long preamble CCK 1Mbps. short is forbidden in 1Mbps
+#define MCS_SHORTP_RATE_2                      5	// short preamble CCK 2Mbps
+#define MCS_SHORTP_RATE_5_5                    6
+#define MCS_SHORTP_RATE_11                     7
+// To send duplicate legacy OFDM. set BW=BW_40.  SGI.STBC are reserved
+#define MCS_RATE_6                      0   // legacy OFDM
+#define MCS_RATE_9                      1   // OFDM
+#define MCS_RATE_12                     2   // OFDM
+#define MCS_RATE_18                     3   // OFDM
+#define MCS_RATE_24                     4  // OFDM
+#define MCS_RATE_36                     5   // OFDM
+#define MCS_RATE_48                     6  // OFDM
+#define MCS_RATE_54                     7 // OFDM
+// HT
+#define MCS_0		0	// 1S
+#define MCS_1		1
+#define MCS_2		2
+#define MCS_3		3
+#define MCS_4		4
+#define MCS_5		5
+#define MCS_6		6
+#define MCS_7		7
+#define MCS_8		8	// 2S
+#define MCS_9		9
+#define MCS_10		10
+#define MCS_11		11
+#define MCS_12		12
+#define MCS_13		13
+#define MCS_14		14
+#define MCS_15		15
+#define MCS_16		16	// 3*3
+#define MCS_17		17
+#define MCS_18		18
+#define MCS_19		19
+#define MCS_20		20
+#define MCS_21		21
+#define MCS_22		22
+#define MCS_23		23
+#define MCS_32		32
+#define MCS_AUTO		33
+
+#ifdef DOT11_N_SUPPORT
+// OID_HTPHYMODE
+// MODE
+#define HTMODE_MM	0
+#define HTMODE_GF	1
+#endif // DOT11_N_SUPPORT //
+
+// Fixed Tx MODE - HT, CCK or OFDM
+#define FIXED_TXMODE_HT		0
+#define FIXED_TXMODE_CCK	1
+#define FIXED_TXMODE_OFDM 	2
+// BW
+#define BW_20		BAND_WIDTH_20
+#define BW_40		BAND_WIDTH_40
+#define BW_BOTH		BAND_WIDTH_BOTH
+#define BW_10		BAND_WIDTH_10	// 802.11j has 10MHz. This definition is for internal usage. doesn't fill in the IE or other field.
+
+#ifdef DOT11_N_SUPPORT
+// SHORTGI
+#define GI_400		GAP_INTERVAL_400	// only support in HT mode
+#define GI_BOTH		GAP_INTERVAL_BOTH
+#endif // DOT11_N_SUPPORT //
+#define GI_800		GAP_INTERVAL_800
+// STBC
+#define STBC_NONE	0
+#ifdef DOT11_N_SUPPORT
+#define STBC_USE	1	// limited use in rt2860b phy
+#define RXSTBC_ONE	1	// rx support of one spatial stream
+#define RXSTBC_TWO	2	// rx support of 1 and 2 spatial stream
+#define RXSTBC_THR	3	// rx support of 1~3 spatial stream
+// MCS FEEDBACK
+#define MCSFBK_NONE	0  // not support mcs feedback /
+#define MCSFBK_RSV	1	// reserved
+#define MCSFBK_UNSOLICIT	2	// only support unsolict mcs feedback
+#define MCSFBK_MRQ	3	// response to both MRQ and unsolict mcs feedback
+
+// MIMO power safe
+#define	MMPS_STATIC	0
+#define	MMPS_DYNAMIC		1
+#define   MMPS_RSV		2
+#define MMPS_ENABLE		3
+
+
+// A-MSDU size
+#define	AMSDU_0	0
+#define	AMSDU_1		1
+
+#endif // DOT11_N_SUPPORT //
+
+// MCS use 7 bits
+#define TXRATEMIMO		0x80
+#define TXRATEMCS		0x7F
+#define TXRATEOFDM		0x7F
+#define RATE_1                      0
+#define RATE_2                      1
+#define RATE_5_5                    2
+#define RATE_11                     3
+#define RATE_6                      4   // OFDM
+#define RATE_9                      5   // OFDM
+#define RATE_12                     6   // OFDM
+#define RATE_18                     7   // OFDM
+#define RATE_24                     8   // OFDM
+#define RATE_36                     9   // OFDM
+#define RATE_48                     10  // OFDM
+#define RATE_54                     11  // OFDM
+#define RATE_FIRST_OFDM_RATE        RATE_6
+#define RATE_LAST_OFDM_RATE        	RATE_54
+#define RATE_6_5                    12  // HT mix
+#define RATE_13                     13  // HT mix
+#define RATE_19_5                   14  // HT mix
+#define RATE_26                     15  // HT mix
+#define RATE_39                     16  // HT mix
+#define RATE_52                     17  // HT mix
+#define RATE_58_5                   18  // HT mix
+#define RATE_65                     19  // HT mix
+#define RATE_78                     20  // HT mix
+#define RATE_104                    21  // HT mix
+#define RATE_117                    22  // HT mix
+#define RATE_130                    23  // HT mix
+//#define RATE_AUTO_SWITCH            255 // for StaCfg.FixedTxRate only
+#define HTRATE_0                      12
+#define RATE_FIRST_MM_RATE        HTRATE_0
+#define RATE_FIRST_HT_RATE        HTRATE_0
+#define RATE_LAST_HT_RATE        HTRATE_0
+
+// pTxWI->txop
+#define IFS_HTTXOP                 0	// The txop will be handles by ASIC.
+#define IFS_PIFS                    1
+#define IFS_SIFS                    2
+#define IFS_BACKOFF                 3
+
+// pTxD->RetryMode
+#define LONG_RETRY                  1
+#define SHORT_RETRY                 0
+
+// Country Region definition
+#define REGION_MINIMUM_BG_BAND            0
+#define REGION_0_BG_BAND                  0       // 1-11
+#define REGION_1_BG_BAND                  1       // 1-13
+#define REGION_2_BG_BAND                  2       // 10-11
+#define REGION_3_BG_BAND                  3       // 10-13
+#define REGION_4_BG_BAND                  4       // 14
+#define REGION_5_BG_BAND                  5       // 1-14
+#define REGION_6_BG_BAND                  6       // 3-9
+#define REGION_7_BG_BAND                  7       // 5-13
+#define REGION_31_BG_BAND                 31       // 5-13
+#define REGION_MAXIMUM_BG_BAND            7
+
+#define REGION_MINIMUM_A_BAND             0
+#define REGION_0_A_BAND                   0       // 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165
+#define REGION_1_A_BAND                   1       // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140
+#define REGION_2_A_BAND                   2       // 36, 40, 44, 48, 52, 56, 60, 64
+#define REGION_3_A_BAND                   3       // 52, 56, 60, 64, 149, 153, 157, 161
+#define REGION_4_A_BAND                   4       // 149, 153, 157, 161, 165
+#define REGION_5_A_BAND                   5       // 149, 153, 157, 161
+#define REGION_6_A_BAND                   6       // 36, 40, 44, 48
+#define REGION_7_A_BAND                   7       // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165
+#define REGION_8_A_BAND                   8       // 52, 56, 60, 64
+#define REGION_9_A_BAND                   9       // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165
+#define REGION_10_A_BAND                  10	  // 36, 40, 44, 48, 149, 153, 157, 161, 165
+#define REGION_11_A_BAND                  11	  // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 149, 153, 157, 161
+#define REGION_MAXIMUM_A_BAND             11
+
+// pTxD->CipherAlg
+#define CIPHER_NONE                 0
+#define CIPHER_WEP64                1
+#define CIPHER_WEP128               2
+#define CIPHER_TKIP                 3
+#define CIPHER_AES                  4
+#define CIPHER_CKIP64               5
+#define CIPHER_CKIP128              6
+#define CIPHER_TKIP_NO_MIC          7       // MIC appended by driver: not a valid value in hardware key table
+#define CIPHER_SMS4					8
+
+// value domain of pAd->RfIcType
+#define RFIC_2820                   1       // 2.4G 2T3R
+#define RFIC_2850                   2       // 2.4G/5G 2T3R
+#define RFIC_2720                   3       // 2.4G 1T2R
+#define RFIC_2750                   4       // 2.4G/5G 1T2R
+#define RFIC_3020                   5       // 2.4G 1T1R
+#define RFIC_2020                   6       // 2.4G B/G
+
+// LED Status.
+#define LED_LINK_DOWN               0
+#define LED_LINK_UP                 1
+#define LED_RADIO_OFF               2
+#define LED_RADIO_ON                3
+#define LED_HALT                    4
+#define LED_WPS                     5
+#define LED_ON_SITE_SURVEY          6
+#define LED_POWER_UP                7
+
+// value domain of pAd->LedCntl.LedMode and E2PROM
+#define LED_MODE_DEFAULT            0
+#define LED_MODE_TWO_LED			1
+#define LED_MODE_SIGNAL_STREGTH		8  // EEPROM define =8
+
+// RC4 init value, used fro WEP & TKIP
+#define PPPINITFCS32                0xffffffff   /* Initial FCS value */
+
+// value domain of pAd->StaCfg.PortSecured. 802.1X controlled port definition
+#define WPA_802_1X_PORT_SECURED     1
+#define WPA_802_1X_PORT_NOT_SECURED 2
+
+#define PAIRWISE_KEY                1
+#define GROUP_KEY                   2
+
+//definition of DRS
+#define MAX_STEP_OF_TX_RATE_SWITCH	32
+
+
+// pre-allocated free NDIS PACKET/BUFFER poll for internal usage
+#define MAX_NUM_OF_FREE_NDIS_PACKET 128
+
+//Block ACK
+#define MAX_TX_REORDERBUF   64
+#define MAX_RX_REORDERBUF   64
+#define DEFAULT_TX_TIMEOUT   30
+#define DEFAULT_RX_TIMEOUT   30
+
+// definition of Recipient or Originator
+#define I_RECIPIENT                  TRUE
+#define I_ORIGINATOR                   FALSE
+
+#define DEFAULT_BBP_TX_POWER        0
+#define DEFAULT_RF_TX_POWER         5
+
+#define MAX_INI_BUFFER_SIZE			4096
+#define MAX_PARAM_BUFFER_SIZE		(2048) // enough for ACL (18*64)
+											//18 : the length of Mac address acceptable format "01:02:03:04:05:06;")
+											//64 : MAX_NUM_OF_ACL_LIST
+// definition of pAd->OpMode
+#define OPMODE_STA                  0
+#define OPMODE_AP                   1
+//#define OPMODE_L3_BRG               2       // as AP and STA at the same time
+
+#ifdef RT_BIG_ENDIAN
+#define DIR_READ                    0
+#define DIR_WRITE                   1
+#define TYPE_TXD                    0
+#define TYPE_RXD                    1
+#define TYPE_TXINFO					0
+#define TYPE_RXINFO					1
+#define TYPE_TXWI					0
+#define TYPE_RXWI					1
+#endif
+
+// ========================= AP rtmp_def.h ===========================
+// value domain for pAd->EventTab.Log[].Event
+#define EVENT_RESET_ACCESS_POINT    0 // Log = "hh:mm:ss   Restart Access Point"
+#define EVENT_ASSOCIATED            1 // Log = "hh:mm:ss   STA 00:01:02:03:04:05 associated"
+#define EVENT_DISASSOCIATED         2 // Log = "hh:mm:ss   STA 00:01:02:03:04:05 left this BSS"
+#define EVENT_AGED_OUT              3 // Log = "hh:mm:ss   STA 00:01:02:03:04:05 was aged-out and removed from this BSS"
+#define EVENT_COUNTER_M             4
+#define EVENT_INVALID_PSK           5
+#define EVENT_MAX_EVENT_TYPE        6
+// ==== end of AP rtmp_def.h ============
+
+// definition RSSI Number
+#define RSSI_0					0
+#define RSSI_1					1
+#define RSSI_2					2
+
+// definition of radar detection
+#define RD_NORMAL_MODE				0	// Not found radar signal
+#define RD_SWITCHING_MODE			1	// Found radar signal, and doing channel switch
+#define RD_SILENCE_MODE				2	// After channel switch, need to be silence a while to ensure radar not found
+
+//Driver defined cid for mapping status and command.
+#define  SLEEPCID	0x11
+#define  WAKECID	0x22
+#define  QUERYPOWERCID	0x33
+#define  OWNERMCU	0x1
+#define  OWNERCPU	0x0
+
+// MBSSID definition
+#define ENTRY_NOT_FOUND             0xFF
+
+
+/* After Linux 2.6.9,
+ * VLAN module use Private (from user) interface flags (netdevice->priv_flags).
+ * #define IFF_802_1Q_VLAN 0x1         --    802.1Q VLAN device.  in if.h
+ * ref to ip_sabotage_out() [ out->priv_flags & IFF_802_1Q_VLAN ] in br_netfilter.c
+ *
+ * For this reason, we MUST use EVEN value in priv_flags
+ */
+#define INT_MAIN                    0x0100
+#define INT_MBSSID                  0x0200
+#define INT_WDS                     0x0300
+#define INT_APCLI                   0x0400
+#define INT_MESH                   	0x0500
+
+// Use bitmap to allow coexist of ATE_TXFRAME and ATE_RXFRAME(i.e.,to support LoopBack mode)
+#ifdef RALINK_ATE
+#define	ATE_START                   0x00   // Start ATE
+#define	ATE_STOP                    0x80   // Stop ATE
+#define	ATE_TXCONT                  0x05   // Continuous Transmit
+#define	ATE_TXCARR                  0x09   // Transmit Carrier
+#define	ATE_TXCARRSUPP              0x11   // Transmit Carrier Suppression
+#define	ATE_TXFRAME                 0x01   // Transmit Frames
+#define	ATE_RXFRAME                 0x02   // Receive Frames
+#ifdef RALINK_28xx_QA
+#define ATE_TXSTOP                  0xe2   // Stop Transmition(i.e., TXCONT, TXCARR, TXCARRSUPP, and TXFRAME)
+#define ATE_RXSTOP					0xfd   // Stop receiving Frames
+#define	BBP22_TXFRAME     			0x00   // Transmit Frames
+#define	BBP22_TXCONT_OR_CARRSUPP    0x80   // Continuous Transmit or Carrier Suppression
+#define	BBP22_TXCARR                0xc1   // Transmit Carrier
+#define	BBP24_TXCONT                0x00   // Continuous Transmit
+#define	BBP24_CARRSUPP              0x01   // Carrier Suppression
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+// WEP Key TYPE
+#define WEP_HEXADECIMAL_TYPE    0
+#define WEP_ASCII_TYPE          1
+
+
+
+// WIRELESS EVENTS definition
+/* Max number of char in custom event, refer to wireless_tools.28/wireless.20.h */
+#define IW_CUSTOM_MAX_LEN				  			255	/* In bytes */
+
+// For system event - start
+#define	IW_SYS_EVENT_FLAG_START                     0x0200
+#define	IW_ASSOC_EVENT_FLAG                         0x0200
+#define	IW_DISASSOC_EVENT_FLAG                      0x0201
+#define	IW_DEAUTH_EVENT_FLAG                      	0x0202
+#define	IW_AGEOUT_EVENT_FLAG                      	0x0203
+#define	IW_COUNTER_MEASURES_EVENT_FLAG              0x0204
+#define	IW_REPLAY_COUNTER_DIFF_EVENT_FLAG           0x0205
+#define	IW_RSNIE_DIFF_EVENT_FLAG           			0x0206
+#define	IW_MIC_DIFF_EVENT_FLAG           			0x0207
+#define IW_ICV_ERROR_EVENT_FLAG						0x0208
+#define IW_MIC_ERROR_EVENT_FLAG						0x0209
+#define IW_GROUP_HS_TIMEOUT_EVENT_FLAG				0x020A
+#define	IW_PAIRWISE_HS_TIMEOUT_EVENT_FLAG			0x020B
+#define IW_RSNIE_SANITY_FAIL_EVENT_FLAG				0x020C
+#define IW_SET_KEY_DONE_WPA1_EVENT_FLAG				0x020D
+#define IW_SET_KEY_DONE_WPA2_EVENT_FLAG				0x020E
+#define IW_STA_LINKUP_EVENT_FLAG					0x020F
+#define IW_STA_LINKDOWN_EVENT_FLAG					0x0210
+#define IW_SCAN_COMPLETED_EVENT_FLAG				0x0211
+#define IW_SCAN_ENQUEUE_FAIL_EVENT_FLAG				0x0212
+// if add new system event flag, please upadte the IW_SYS_EVENT_FLAG_END
+#define	IW_SYS_EVENT_FLAG_END                       0x0212
+#define	IW_SYS_EVENT_TYPE_NUM						(IW_SYS_EVENT_FLAG_END - IW_SYS_EVENT_FLAG_START + 1)
+// For system event - end
+
+// For spoof attack event - start
+#define	IW_SPOOF_EVENT_FLAG_START                   0x0300
+#define IW_CONFLICT_SSID_EVENT_FLAG					0x0300
+#define IW_SPOOF_ASSOC_RESP_EVENT_FLAG				0x0301
+#define IW_SPOOF_REASSOC_RESP_EVENT_FLAG			0x0302
+#define IW_SPOOF_PROBE_RESP_EVENT_FLAG				0x0303
+#define IW_SPOOF_BEACON_EVENT_FLAG					0x0304
+#define IW_SPOOF_DISASSOC_EVENT_FLAG				0x0305
+#define IW_SPOOF_AUTH_EVENT_FLAG					0x0306
+#define IW_SPOOF_DEAUTH_EVENT_FLAG					0x0307
+#define IW_SPOOF_UNKNOWN_MGMT_EVENT_FLAG			0x0308
+#define IW_REPLAY_ATTACK_EVENT_FLAG					0x0309
+// if add new spoof attack event flag, please upadte the IW_SPOOF_EVENT_FLAG_END
+#define	IW_SPOOF_EVENT_FLAG_END                     0x0309
+#define	IW_SPOOF_EVENT_TYPE_NUM						(IW_SPOOF_EVENT_FLAG_END - IW_SPOOF_EVENT_FLAG_START + 1)
+// For spoof attack event - end
+
+// For flooding attack event - start
+#define	IW_FLOOD_EVENT_FLAG_START                   0x0400
+#define IW_FLOOD_AUTH_EVENT_FLAG					0x0400
+#define IW_FLOOD_ASSOC_REQ_EVENT_FLAG				0x0401
+#define IW_FLOOD_REASSOC_REQ_EVENT_FLAG				0x0402
+#define IW_FLOOD_PROBE_REQ_EVENT_FLAG				0x0403
+#define IW_FLOOD_DISASSOC_EVENT_FLAG				0x0404
+#define IW_FLOOD_DEAUTH_EVENT_FLAG					0x0405
+#define IW_FLOOD_EAP_REQ_EVENT_FLAG					0x0406
+// if add new flooding attack event flag, please upadte the IW_FLOOD_EVENT_FLAG_END
+#define	IW_FLOOD_EVENT_FLAG_END                   	0x0406
+#define	IW_FLOOD_EVENT_TYPE_NUM						(IW_FLOOD_EVENT_FLAG_END - IW_FLOOD_EVENT_FLAG_START + 1)
+// For flooding attack - end
+
+// End - WIRELESS EVENTS definition
+
+#ifdef CONFIG_STA_SUPPORT
+// definition for DLS, kathy
+#define	MAX_NUM_OF_INIT_DLS_ENTRY   1
+#define	MAX_NUM_OF_DLS_ENTRY        MAX_NUMBER_OF_DLS_ENTRY
+
+//Block ACK , rt2860, kathy
+#define MAX_TX_REORDERBUF		64
+#define MAX_RX_REORDERBUF		64
+#define DEFAULT_TX_TIMEOUT		30
+#define DEFAULT_RX_TIMEOUT		30
+#ifndef CONFIG_AP_SUPPORT
+#define MAX_BARECI_SESSION		8
+#endif
+
+#ifndef IW_ESSID_MAX_SIZE
+/* Maximum size of the ESSID and pAd->nickname strings */
+#define IW_ESSID_MAX_SIZE   		32
+#endif
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef MCAST_RATE_SPECIFIC
+#define MCAST_DISABLE	0
+#define MCAST_CCK		1
+#define MCAST_OFDM		2
+#define MCAST_HTMIX		3
+#endif // MCAST_RATE_SPECIFIC //
+
+// For AsicRadioOff/AsicRadioOn function
+#define DOT11POWERSAVE		0
+#define GUIRADIO_OFF		1
+#define RTMP_HALT		    2
+#define GUI_IDLE_POWER_SAVE		3
+// --
+
+
+// definition for WpaSupport flag
+#define WPA_SUPPLICANT_DISABLE				0
+#define WPA_SUPPLICANT_ENABLE				1
+#define	WPA_SUPPLICANT_ENABLE_WITH_WEB_UI	2
+
+// Endian byte swapping codes
+#define SWAP16(x) \
+    ((UINT16)( \
+    (((UINT16)(x) & (UINT16) 0x00ffU) << 8) | \
+    (((UINT16)(x) & (UINT16) 0xff00U) >> 8) ))
+
+#define SWAP32(x) \
+    ((UINT32)( \
+    (((UINT32)(x) & (UINT32) 0x000000ffUL) << 24) | \
+    (((UINT32)(x) & (UINT32) 0x0000ff00UL) <<  8) | \
+    (((UINT32)(x) & (UINT32) 0x00ff0000UL) >>  8) | \
+    (((UINT32)(x) & (UINT32) 0xff000000UL) >> 24) ))
+
+#define SWAP64(x) \
+    ((UINT64)( \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x00000000000000ffULL) << 56) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x000000000000ff00ULL) << 40) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x0000000000ff0000ULL) << 24) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x00000000ff000000ULL) <<  8) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x000000ff00000000ULL) >>  8) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x0000ff0000000000ULL) >> 24) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x00ff000000000000ULL) >> 40) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0xff00000000000000ULL) >> 56) ))
+
+#ifdef RT_BIG_ENDIAN
+
+#define cpu2le64(x) SWAP64((x))
+#define le2cpu64(x) SWAP64((x))
+#define cpu2le32(x) SWAP32((x))
+#define le2cpu32(x) SWAP32((x))
+#define cpu2le16(x) SWAP16((x))
+#define le2cpu16(x) SWAP16((x))
+#define cpu2be64(x) ((UINT64)(x))
+#define be2cpu64(x) ((UINT64)(x))
+#define cpu2be32(x) ((UINT32)(x))
+#define be2cpu32(x) ((UINT32)(x))
+#define cpu2be16(x) ((UINT16)(x))
+#define be2cpu16(x) ((UINT16)(x))
+
+#else   // Little_Endian
+
+#define cpu2le64(x) ((UINT64)(x))
+#define le2cpu64(x) ((UINT64)(x))
+#define cpu2le32(x) ((UINT32)(x))
+#define le2cpu32(x) ((UINT32)(x))
+#define cpu2le16(x) ((UINT16)(x))
+#define le2cpu16(x) ((UINT16)(x))
+#define cpu2be64(x) SWAP64((x))
+#define be2cpu64(x) SWAP64((x))
+#define cpu2be32(x) SWAP32((x))
+#define be2cpu32(x) SWAP32((x))
+#define cpu2be16(x) SWAP16((x))
+#define be2cpu16(x) SWAP16((x))
+
+#endif  // RT_BIG_ENDIAN
+
+#endif  // __RTMP_DEF_H__
+
+
diff --git a/drivers/staging/rt2870/rtmp_type.h b/drivers/staging/rt2870/rtmp_type.h
new file mode 100644
index 000000000000..1fd7df1e1791
--- /dev/null
+++ b/drivers/staging/rt2870/rtmp_type.h
@@ -0,0 +1,94 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rtmp_type.h
+
+    Abstract:
+
+    Revision History:
+    Who         When            What
+    --------    ----------      ----------------------------------------------
+    Name        Date            Modification logs
+    Paul Lin    1-2-2004
+*/
+#ifndef __RTMP_TYPE_H__
+#define __RTMP_TYPE_H__
+
+#define PACKED  __attribute__ ((packed))
+
+// Put platform dependent declaration here
+// For example, linux type definition
+typedef unsigned char		UINT8;
+typedef unsigned short		UINT16;
+typedef unsigned int		UINT32;
+typedef unsigned long long	UINT64;
+typedef int					INT32;
+typedef long long 			INT64;
+
+typedef unsigned char *			PUINT8;
+typedef unsigned short *		PUINT16;
+typedef unsigned int *			PUINT32;
+typedef unsigned long long *	PUINT64;
+typedef int	*					PINT32;
+typedef long long * 			PINT64;
+
+typedef signed char			CHAR;
+typedef signed short		SHORT;
+typedef signed int			INT;
+typedef signed long			LONG;
+typedef signed long long	LONGLONG;
+
+
+typedef unsigned char		UCHAR;
+typedef unsigned short		USHORT;
+typedef unsigned int		UINT;
+typedef unsigned long		ULONG;
+typedef unsigned long long	ULONGLONG;
+
+typedef unsigned char		BOOLEAN;
+typedef void				VOID;
+
+typedef VOID *				PVOID;
+typedef CHAR *				PCHAR;
+typedef UCHAR * 			PUCHAR;
+typedef USHORT *			PUSHORT;
+typedef LONG *				PLONG;
+typedef ULONG *				PULONG;
+typedef UINT *				PUINT;
+
+typedef unsigned int	NDIS_MEDIA_STATE;
+
+typedef union _LARGE_INTEGER {
+    struct {
+        UINT LowPart;
+        INT32 HighPart;
+    } u;
+    INT64 QuadPart;
+} LARGE_INTEGER;
+
+#endif  // __RTMP_TYPE_H__
+
diff --git a/drivers/staging/rt2870/spectrum.h b/drivers/staging/rt2870/spectrum.h
new file mode 100644
index 000000000000..94cfa5b174fc
--- /dev/null
+++ b/drivers/staging/rt2870/spectrum.h
@@ -0,0 +1,322 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __SPECTRUM_H__
+#define __SPECTRUM_H__
+
+#include "rtmp_type.h"
+#include "spectrum_def.h"
+
+typedef struct PACKED _TPC_REPORT_INFO
+{
+	UINT8 TxPwr;
+	UINT8 LinkMargin;
+} TPC_REPORT_INFO, *PTPC_REPORT_INFO;
+
+typedef struct PACKED _CH_SW_ANN_INFO
+{
+	UINT8 ChSwMode;
+	UINT8 Channel;
+	UINT8 ChSwCnt;
+} CH_SW_ANN_INFO, *PCH_SW_ANN_INFO;
+
+typedef union PACKED _MEASURE_REQ_MODE
+{
+#ifdef RT_BIG_ENDIAN
+	struct PACKED
+	{
+		UINT8 Rev1:4;
+		UINT8 Report:1;
+		UINT8 Request:1;
+		UINT8 Enable:1;
+		UINT8 Rev0:1;
+	} field;
+#else
+	struct PACKED
+	{
+		UINT8 Rev0:1;
+		UINT8 Enable:1;
+		UINT8 Request:1;
+		UINT8 Report:1;
+		UINT8 Rev1:4;
+	} field;
+#endif // RT_BIG_ENDIAN //
+	UINT8 word;
+} MEASURE_REQ_MODE, *PMEASURE_REQ_MODE;
+
+typedef struct PACKED _MEASURE_REQ
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+} MEASURE_REQ, *PMEASURE_REQ;
+
+typedef struct PACKED _MEASURE_REQ_INFO
+{
+	UINT8 Token;
+	MEASURE_REQ_MODE ReqMode;
+	UINT8 ReqType;
+	MEASURE_REQ MeasureReq;
+} MEASURE_REQ_INFO, *PMEASURE_REQ_INFO;
+
+typedef union PACKED _MEASURE_BASIC_REPORT_MAP
+{
+#ifdef RT_BIG_ENDIAN
+	struct PACKED
+	{
+		UINT8 Rev:3;
+		UINT8 Unmeasure:1;
+		UINT8 Radar:1;
+		UINT8 UnidentifiedSignal:1;
+		UINT8 OfdmPreamble:1;
+		UINT8 BSS:1;
+	} field;
+#else
+	struct PACKED
+	{
+		UINT8 BSS:1;
+		UINT8 OfdmPreamble:1;
+		UINT8 UnidentifiedSignal:1;
+		UINT8 Radar:1;
+		UINT8 Unmeasure:1;
+		UINT8 Rev:3;
+	} field;
+#endif // RT_BIG_ENDIAN //
+	UINT8 word;
+} MEASURE_BASIC_REPORT_MAP, *PMEASURE_BASIC_REPORT_MAP;
+
+typedef struct PACKED _MEASURE_BASIC_REPORT
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+	MEASURE_BASIC_REPORT_MAP Map;
+} MEASURE_BASIC_REPORT, *PMEASURE_BASIC_REPORT;
+
+typedef struct PACKED _MEASURE_CCA_REPORT
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+	UINT8 CCA_Busy_Fraction;
+} MEASURE_CCA_REPORT, *PMEASURE_CCA_REPORT;
+
+typedef struct PACKED _MEASURE_RPI_REPORT
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+	UINT8 RPI_Density[8];
+} MEASURE_RPI_REPORT, *PMEASURE_RPI_REPORT;
+
+typedef union PACKED _MEASURE_REPORT_MODE
+{
+	struct PACKED
+	{
+#ifdef RT_BIG_ENDIAN
+		UINT8 Rev:5;
+		UINT8 Refused:1;
+		UINT8 Incapable:1;
+		UINT8 Late:1;
+#else
+		UINT8 Late:1;
+		UINT8 Incapable:1;
+		UINT8 Refused:1;
+		UINT8 Rev:5;
+#endif // RT_BIG_ENDIAN //
+	} field;
+	UINT8 word;
+} MEASURE_REPORT_MODE, *PMEASURE_REPORT_MODE;
+
+typedef struct PACKED _MEASURE_REPORT_INFO
+{
+	UINT8 Token;
+	MEASURE_REPORT_MODE ReportMode;
+	UINT8 ReportType;
+	UINT8 Octect[0];
+} MEASURE_REPORT_INFO, *PMEASURE_REPORT_INFO;
+
+typedef struct PACKED _QUIET_INFO
+{
+	UINT8 QuietCnt;
+	UINT8 QuietPeriod;
+	UINT8 QuietDuration;
+	UINT8 QuietOffset;
+} QUIET_INFO, *PQUIET_INFO;
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 MeasureCh,
+	IN UINT16 MeasureDuration);
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 ReportInfoLen,
+	IN PUINT8 pReportInfo);
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UCHAR DialogToken);
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 TxPwr,
+	IN UINT8 LinkMargin);
+
+/*
+	==========================================================================
+	Description:
+		Prepare Channel Switch Announcement action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+		2. Channel switch announcement mode.
+		2. a New selected channel.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueChSwAnn(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 ChSwMode,
+	IN UINT8 NewCh);
+
+/*
+	==========================================================================
+	Description:
+		Spectrun action frames Handler such as channel switch annoucement,
+		measurement report, measurement request actions frames.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+VOID PeerSpectrumAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+/*
+	==========================================================================
+	Description:
+
+	Parametrs:
+
+	Return	: None.
+	==========================================================================
+ */
+INT Set_MeasureReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_TpcReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+VOID MeasureReqTabInit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MeasureReqTabExit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID TpcReqTabInit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID TpcReqTabExit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID NotifyChSwAnnToPeerAPs(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pRA,
+	IN PUCHAR pTA,
+	IN UINT8 ChSwMode,
+	IN UINT8 Channel);
+#endif // __SPECTRUM_H__ //
+
diff --git a/drivers/staging/rt2870/spectrum_def.h b/drivers/staging/rt2870/spectrum_def.h
new file mode 100644
index 000000000000..4ca4817bba05
--- /dev/null
+++ b/drivers/staging/rt2870/spectrum_def.h
@@ -0,0 +1,95 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+	spectrum_def.h
+
+    Abstract:
+    Handle association related requests either from WSTA or from local MLME
+
+    Revision History:
+    Who          When          What
+    ---------    ----------    ----------------------------------------------
+	Fonchi Wu    2008	  	   created for 802.11h
+ */
+
+#ifndef __SPECTRUM_DEF_H__
+#define __SPECTRUM_DEF_H__
+
+#define MAX_MEASURE_REQ_TAB_SIZE		3
+#define MAX_HASH_MEASURE_REQ_TAB_SIZE	MAX_MEASURE_REQ_TAB_SIZE
+
+#define MAX_TPC_REQ_TAB_SIZE			3
+#define MAX_HASH_TPC_REQ_TAB_SIZE		MAX_TPC_REQ_TAB_SIZE
+
+#define MIN_RCV_PWR				100		/* Negative value ((dBm) */
+
+#define RM_TPC_REQ				0
+#define RM_MEASURE_REQ			1
+
+#define RM_BASIC				0
+#define RM_CCA					1
+#define RM_RPI_HISTOGRAM		2
+
+#define TPC_REQ_AGE_OUT			500		/* ms */
+#define MQ_REQ_AGE_OUT			500		/* ms */
+
+#define TPC_DIALOGTOKEN_HASH_INDEX(_DialogToken)	((_DialogToken) % MAX_HASH_TPC_REQ_TAB_SIZE)
+#define MQ_DIALOGTOKEN_HASH_INDEX(_DialogToken)		((_DialogToken) % MAX_MEASURE_REQ_TAB_SIZE)
+
+typedef struct _MEASURE_REQ_ENTRY
+{
+	struct _MEASURE_REQ_ENTRY *pNext;
+	ULONG lastTime;
+	BOOLEAN	Valid;
+	UINT8 DialogToken;
+	UINT8 MeasureDialogToken[3];	// 0:basic measure, 1: CCA measure, 2: RPI_Histogram measure.
+} MEASURE_REQ_ENTRY, *PMEASURE_REQ_ENTRY;
+
+typedef struct _MEASURE_REQ_TAB
+{
+	UCHAR Size;
+	PMEASURE_REQ_ENTRY Hash[MAX_HASH_MEASURE_REQ_TAB_SIZE];
+	MEASURE_REQ_ENTRY Content[MAX_MEASURE_REQ_TAB_SIZE];
+} MEASURE_REQ_TAB, *PMEASURE_REQ_TAB;
+
+typedef struct _TPC_REQ_ENTRY
+{
+	struct _TPC_REQ_ENTRY *pNext;
+	ULONG lastTime;
+	BOOLEAN Valid;
+	UINT8 DialogToken;
+} TPC_REQ_ENTRY, *PTPC_REQ_ENTRY;
+
+typedef struct _TPC_REQ_TAB
+{
+	UCHAR Size;
+	PTPC_REQ_ENTRY Hash[MAX_HASH_TPC_REQ_TAB_SIZE];
+	TPC_REQ_ENTRY Content[MAX_TPC_REQ_TAB_SIZE];
+} TPC_REQ_TAB, *PTPC_REQ_TAB;
+
+#endif // __SPECTRUM_DEF_H__ //
+
diff --git a/drivers/staging/rt2870/sta/aironet.c b/drivers/staging/rt2870/sta/aironet.c
new file mode 100644
index 000000000000..4af4a1906181
--- /dev/null
+++ b/drivers/staging/rt2870/sta/aironet.c
@@ -0,0 +1,1312 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	aironet.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Paul Lin	04-06-15		Initial
+*/
+#include "../rt_config.h"
+
+/*
+	==========================================================================
+	Description:
+		association	state machine init,	including state	transition and timer init
+	Parameters:
+		S -	pointer	to the association state machine
+	==========================================================================
+ */
+VOID	AironetStateMachineInit(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	STATE_MACHINE		*S,
+	OUT	STATE_MACHINE_FUNC	Trans[])
+{
+	StateMachineInit(S,	Trans, MAX_AIRONET_STATE, MAX_AIRONET_MSG, (STATE_MACHINE_FUNC)Drop, AIRONET_IDLE, AIRONET_MACHINE_BASE);
+	StateMachineSetAction(S, AIRONET_IDLE, MT2_AIRONET_MSG, (STATE_MACHINE_FUNC)AironetMsgAction);
+	StateMachineSetAction(S, AIRONET_IDLE, MT2_AIRONET_SCAN_REQ, (STATE_MACHINE_FUNC)AironetRequestAction);
+	StateMachineSetAction(S, AIRONET_SCANNING, MT2_AIRONET_SCAN_DONE, (STATE_MACHINE_FUNC)AironetReportAction);
+}
+
+/*
+	==========================================================================
+	Description:
+		This is	state machine function.
+		When receiving EAPOL packets which is  for 802.1x key management.
+		Use	both in	WPA, and WPAPSK	case.
+		In this	function, further dispatch to different	functions according	to the received	packet.	 3 categories are :
+		  1.  normal 4-way pairwisekey and 2-way groupkey handshake
+		  2.  MIC error	(Countermeasures attack)  report packet	from STA.
+		  3.  Request for pairwise/group key update	from STA
+	Return:
+	==========================================================================
+*/
+VOID	AironetMsgAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+{
+	USHORT							Length;
+	UCHAR							Index, i;
+	PUCHAR							pData;
+	PAIRONET_RM_REQUEST_FRAME		pRMReq;
+	PRM_REQUEST_ACTION				pReqElem;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-----> AironetMsgAction\n"));
+
+	// 0. Get Aironet IAPP header first
+	pRMReq = (PAIRONET_RM_REQUEST_FRAME) &Elem->Msg[LENGTH_802_11];
+	pData  = (PUCHAR) &Elem->Msg[LENGTH_802_11];
+
+	// 1. Change endian format form network to little endian
+	Length = be2cpu16(pRMReq->IAPP.Length);
+
+	// 2.0 Sanity check, this should only happen when CCX 2.0 support is enabled
+	if (pAd->StaCfg.CCXEnable != TRUE)
+		return;
+
+	// 2.1 Radio measurement must be on
+	if (pAd->StaCfg.CCXControl.field.RMEnable != 1)
+		return;
+
+	// 2.2. Debug print all bit information
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP ID & Length %d\n", Length));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Type %x\n", pRMReq->IAPP.Type));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP SubType %x\n", pRMReq->IAPP.SubType));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Dialog Token %x\n", pRMReq->IAPP.Token));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Activation Delay %x\n", pRMReq->Delay));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Measurement Offset %x\n", pRMReq->Offset));
+
+	// 3. Check IAPP frame type, it must be 0x32 for Cisco Aironet extension
+	if (pRMReq->IAPP.Type != AIRONET_IAPP_TYPE)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP type for Cisco Aironet extension\n"));
+		return;
+	}
+
+	// 4. Check IAPP frame subtype, it must be 0x01 for Cisco Aironet extension request.
+	//    Since we are acting as client only, we will disregards reply subtype.
+	if (pRMReq->IAPP.SubType != AIRONET_IAPP_SUBTYPE_REQUEST)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP subtype for Cisco Aironet extension\n"));
+		return;
+	}
+
+	// 5. Verify Destination MAC and Source MAC, both should be all zeros.
+	if (! MAC_ADDR_EQUAL(pRMReq->IAPP.DA, ZERO_MAC_ADDR))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP DA for Cisco Aironet extension, it's not Zero\n"));
+		return;
+	}
+
+	if (! MAC_ADDR_EQUAL(pRMReq->IAPP.SA, ZERO_MAC_ADDR))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP SA for Cisco Aironet extension, it's not Zero\n"));
+		return;
+	}
+
+	// 6. Reinit all report related fields
+	NdisZeroMemory(pAd->StaCfg.FrameReportBuf, 2048);
+	NdisZeroMemory(pAd->StaCfg.BssReportOffset, sizeof(USHORT) * MAX_LEN_OF_BSS_TABLE);
+	NdisZeroMemory(pAd->StaCfg.MeasurementRequest, sizeof(RM_REQUEST_ACTION) * 4);
+
+	// 7. Point to the start of first element report element
+	pAd->StaCfg.FrameReportLen   = LENGTH_802_11 + sizeof(AIRONET_IAPP_HEADER);
+	DBGPRINT(RT_DEBUG_TRACE, ("FR len = %d\n", pAd->StaCfg.FrameReportLen));
+	pAd->StaCfg.LastBssIndex     = 0xff;
+	pAd->StaCfg.RMReqCnt         = 0;
+	pAd->StaCfg.ParallelReq      = FALSE;
+	pAd->StaCfg.ParallelDuration = 0;
+	pAd->StaCfg.ParallelChannel  = 0;
+	pAd->StaCfg.IAPPToken        = pRMReq->IAPP.Token;
+	pAd->StaCfg.CurrentRMReqIdx  = 0;
+	pAd->StaCfg.CLBusyBytes      = 0;
+	// Reset the statistics
+	for (i = 0; i < 8; i++)
+		pAd->StaCfg.RPIDensity[i] = 0;
+
+	Index = 0;
+
+	// 8. Save dialog token for report
+	pAd->StaCfg.IAPPToken = pRMReq->IAPP.Token;
+
+	// Save Activation delay & measurement offset, Not really needed
+
+	// 9. Point to the first request element
+	pData += sizeof(AIRONET_RM_REQUEST_FRAME);
+	//    Length should exclude the CISCO Aironet SNAP header
+	Length -= (sizeof(AIRONET_RM_REQUEST_FRAME) - LENGTH_802_1_H);
+
+	// 10. Start Parsing the Measurement elements.
+	//    Be careful about multiple MR elements within one frames.
+	while (Length > 0)
+	{
+		pReqElem = (PRM_REQUEST_ACTION) pData;
+		switch (pReqElem->ReqElem.Eid)
+		{
+			case IE_MEASUREMENT_REQUEST:
+				// From the example, it seems we only need to support one request in one frame
+				// There is no multiple request in one frame.
+				// Besides, looks like we need to take care the measurement request only.
+				// The measurement request is always 4 bytes.
+
+				// Start parsing this type of request.
+				// 0. Eid is IE_MEASUREMENT_REQUEST
+				// 1. Length didn't include Eid and Length field, it always be 8.
+				// 2. Measurement Token, we nned to save it for the corresponding report.
+				// 3. Measurement Mode, Although there are definitions, but we din't see value other than
+				//    0 from test specs examples.
+				// 4. Measurement Type, this is what we need to do.
+				switch (pReqElem->ReqElem.Type)
+				{
+					case MSRN_TYPE_CHANNEL_LOAD_REQ:
+					case MSRN_TYPE_NOISE_HIST_REQ:
+					case MSRN_TYPE_BEACON_REQ:
+						// Check the Enable non-serving channel measurement control
+						if (pAd->StaCfg.CCXControl.field.DCRMEnable == 0)
+						{
+							// Check channel before enqueue the action
+							if (pReqElem->Measurement.Channel != pAd->CommonCfg.Channel)
+								break;
+						}
+						else
+						{
+							// If off channel measurement, check the TU duration limit
+							if (pReqElem->Measurement.Channel != pAd->CommonCfg.Channel)
+								if (pReqElem->Measurement.Duration > pAd->StaCfg.CCXControl.field.TuLimit)
+									break;
+						}
+
+						// Save requests and execute actions later
+						NdisMoveMemory(&pAd->StaCfg.MeasurementRequest[Index], pReqElem, sizeof(RM_REQUEST_ACTION));
+						Index += 1;
+						break;
+
+					case MSRN_TYPE_FRAME_REQ:
+						// Since it's option, we will support later
+						// FrameRequestAction(pAd, pData);
+						break;
+
+					default:
+						break;
+				}
+
+				// Point to next Measurement request
+				pData  += sizeof(RM_REQUEST_ACTION);
+				Length -= sizeof(RM_REQUEST_ACTION);
+				break;
+
+			// We accept request only, all others are dropped
+			case IE_MEASUREMENT_REPORT:
+			case IE_AP_TX_POWER:
+			case IE_MEASUREMENT_CAPABILITY:
+			default:
+				return;
+		}
+	}
+
+	// 11. Update some flags and index
+	pAd->StaCfg.RMReqCnt = Index;
+
+	if (Index)
+	{
+		MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_REQ, 0, NULL);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<----- AironetMsgAction\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+{
+	PRM_REQUEST_ACTION	pReq;
+
+	// 1. Point to next request element
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+	// 2. Parse measurement type and call appropriate functions
+	if (pReq->ReqElem.Type == MSRN_TYPE_CHANNEL_LOAD_REQ)
+		// Channel Load measurement request
+		ChannelLoadRequestAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_NOISE_HIST_REQ)
+		// Noise Histogram measurement request
+		NoiseHistRequestAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_BEACON_REQ)
+		// Beacon measurement request
+		BeaconRequestAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else
+		// Unknown. Do nothing and return, this should never happen
+		return;
+
+	// 3. Peek into the next request, if it's parallel, we will update the scan time to the largest one
+	if ((pAd->StaCfg.CurrentRMReqIdx + 1) < pAd->StaCfg.RMReqCnt)
+	{
+		pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx + 1];
+		// Check for parallel bit
+		if ((pReq->ReqElem.Mode & 0x01) && (pReq->Measurement.Channel == pAd->StaCfg.CCXScanChannel))
+		{
+			// Update parallel mode request information
+			pAd->StaCfg.ParallelReq = TRUE;
+			pAd->StaCfg.CCXScanTime = ((pReq->Measurement.Duration > pAd->StaCfg.CCXScanTime) ?
+			(pReq->Measurement.Duration) : (pAd->StaCfg.CCXScanTime));
+		}
+	}
+
+	// 4. Call RT28XX_MLME_HANDLER to execute the request mlme commands, Scan request is the only one used
+	RT28XX_MLME_HANDLER(pAd);
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare channel load report action, special scan operation added
+		to support
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		pData		Start from element ID
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ChannelLoadRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PRM_REQUEST_ACTION				pReq;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+	UCHAR							ZeroSsid[32];
+	NDIS_STATUS						NStatus;
+	PUCHAR							pOutBuffer = NULL;
+	PHEADER_802_11					pNullFrame;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadRequestAction ----->\n"));
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[Index];
+	NdisZeroMemory(ZeroSsid, 32);
+
+	// Prepare for special scan request
+	// The scan definition is different with our Active, Passive scan definition.
+	// For CCX2, Active means send out probe request with broadcast BSSID.
+	// Passive means no probe request sent, only listen to the beacons.
+	// The channel scanned is fixed as specified, no need to scan all channels.
+	// The scan wait time is specified in the request too.
+	// Passive scan Mode
+
+	// Control state machine is not idle, reject the request
+	if ((pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE) && (Index == 0))
+		return;
+
+	// Fill out stuff for scan request
+	ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_CHANNEL_LOAD);
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+	// Reset some internal control flags to make sure this scan works.
+	BssTableInit(&pAd->StaCfg.CCXBssTab);
+	pAd->StaCfg.ScanCnt        = 0;
+	pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+	pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Duration %d, Channel %d!\n", pReq->Measurement.Duration, pReq->Measurement.Channel));
+
+	// If it's non serving channel scan, send out a null frame with PSM bit on.
+	if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+	{
+		// Use MLME enqueue method
+		NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+		if (NStatus	!= NDIS_STATUS_SUCCESS)
+			return;
+
+		pNullFrame = (PHEADER_802_11) pOutBuffer;;
+		// Make the power save Null frame with PSM bit on
+		MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+		pNullFrame->Duration 	= 0;
+		pNullFrame->FC.Type 	= BTYPE_DATA;
+		pNullFrame->FC.PwrMgmt	= PWR_SAVE;
+
+		// Send using priority queue
+		MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+		MlmeFreeMemory(pAd, pOutBuffer);
+		DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+		RTMPusecDelay(5000);
+	}
+
+	pAd->StaCfg.CCXReqType     = MSRN_TYPE_CHANNEL_LOAD_REQ;
+	pAd->StaCfg.CLBusyBytes    = 0;
+	// Enable Rx with promiscuous reception
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, 0x1010);
+
+	// Set channel load measurement flag
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadRequestAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare noise histogram report action, special scan operation added
+		to support
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		pData		Start from element ID
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	NoiseHistRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PRM_REQUEST_ACTION				pReq;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+	UCHAR							ZeroSsid[32], i;
+	NDIS_STATUS						NStatus;
+	PUCHAR							pOutBuffer = NULL;
+	PHEADER_802_11					pNullFrame;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistRequestAction ----->\n"));
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[Index];
+	NdisZeroMemory(ZeroSsid, 32);
+
+	// Prepare for special scan request
+	// The scan definition is different with our Active, Passive scan definition.
+	// For CCX2, Active means send out probe request with broadcast BSSID.
+	// Passive means no probe request sent, only listen to the beacons.
+	// The channel scanned is fixed as specified, no need to scan all channels.
+	// The scan wait time is specified in the request too.
+	// Passive scan Mode
+
+	// Control state machine is not idle, reject the request
+	if ((pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE) && (Index == 0))
+		return;
+
+	// Fill out stuff for scan request
+	ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_NOISE);
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+	// Reset some internal control flags to make sure this scan works.
+	BssTableInit(&pAd->StaCfg.CCXBssTab);
+	pAd->StaCfg.ScanCnt        = 0;
+	pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+	pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+	pAd->StaCfg.CCXReqType     = MSRN_TYPE_NOISE_HIST_REQ;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Duration %d, Channel %d!\n", pReq->Measurement.Duration, pReq->Measurement.Channel));
+
+	// If it's non serving channel scan, send out a null frame with PSM bit on.
+	if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+	{
+		// Use MLME enqueue method
+		NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+		if (NStatus	!= NDIS_STATUS_SUCCESS)
+			return;
+
+		pNullFrame = (PHEADER_802_11) pOutBuffer;
+		// Make the power save Null frame with PSM bit on
+		MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+		pNullFrame->Duration 	= 0;
+		pNullFrame->FC.Type  	= BTYPE_DATA;
+		pNullFrame->FC.PwrMgmt	= PWR_SAVE;
+
+		// Send using priority queue
+		MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+		MlmeFreeMemory(pAd, pOutBuffer);
+		DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+		RTMPusecDelay(5000);
+	}
+
+	// Reset the statistics
+	for (i = 0; i < 8; i++)
+		pAd->StaCfg.RPIDensity[i] = 0;
+
+	// Enable Rx with promiscuous reception
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, 0x1010);
+
+	// Set channel load measurement flag
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistRequestAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare Beacon report action, special scan operation added
+		to support
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		pData		Start from element ID
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	BeaconRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PRM_REQUEST_ACTION				pReq;
+	NDIS_STATUS						NStatus;
+	PUCHAR							pOutBuffer = NULL;
+	PHEADER_802_11					pNullFrame;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+	UCHAR							ZeroSsid[32];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconRequestAction ----->\n"));
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[Index];
+	NdisZeroMemory(ZeroSsid, 32);
+
+	// Prepare for special scan request
+	// The scan definition is different with our Active, Passive scan definition.
+	// For CCX2, Active means send out probe request with broadcast BSSID.
+	// Passive means no probe request sent, only listen to the beacons.
+	// The channel scanned is fixed as specified, no need to scan all channels.
+	// The scan wait time is specified in the request too.
+	if (pReq->Measurement.ScanMode == MSRN_SCAN_MODE_PASSIVE)
+	{
+		// Passive scan Mode
+		DBGPRINT(RT_DEBUG_TRACE, ("Passive Scan Mode!\n"));
+
+		// Control state machine is not idle, reject the request
+		if ((pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE) && (Index == 0))
+			return;
+
+		// Fill out stuff for scan request
+		ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_PASSIVE);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+		// Reset some internal control flags to make sure this scan works.
+		BssTableInit(&pAd->StaCfg.CCXBssTab);
+		pAd->StaCfg.ScanCnt        = 0;
+		pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+		pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+		pAd->StaCfg.CCXReqType     = MSRN_TYPE_BEACON_REQ;
+		DBGPRINT(RT_DEBUG_TRACE, ("Duration %d!\n", pReq->Measurement.Duration));
+
+		// If it's non serving channel scan, send out a null frame with PSM bit on.
+		if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+		{
+			// Use MLME enqueue method
+			NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus	!= NDIS_STATUS_SUCCESS)
+				return;
+
+			pNullFrame = (PHEADER_802_11) pOutBuffer;
+			// Make the power save Null frame with PSM bit on
+			MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+			pNullFrame->Duration 	= 0;
+			pNullFrame->FC.Type     = BTYPE_DATA;
+			pNullFrame->FC.PwrMgmt  = PWR_SAVE;
+
+			// Send using priority queue
+			MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+			RTMPusecDelay(5000);
+		}
+
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+	}
+	else if (pReq->Measurement.ScanMode == MSRN_SCAN_MODE_ACTIVE)
+	{
+		// Active scan Mode
+		DBGPRINT(RT_DEBUG_TRACE, ("Active Scan Mode!\n"));
+
+		// Control state machine is not idle, reject the request
+		if (pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+			return;
+
+		// Fill out stuff for scan request
+		ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_ACTIVE);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+		// Reset some internal control flags to make sure this scan works.
+		BssTableInit(&pAd->StaCfg.CCXBssTab);
+		pAd->StaCfg.ScanCnt        = 0;
+		pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+		pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+		pAd->StaCfg.CCXReqType     = MSRN_TYPE_BEACON_REQ;
+		DBGPRINT(RT_DEBUG_TRACE, ("Duration %d!\n", pReq->Measurement.Duration));
+
+		// If it's non serving channel scan, send out a null frame with PSM bit on.
+		if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+		{
+			// Use MLME enqueue method
+			NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus	!= NDIS_STATUS_SUCCESS)
+				return;
+
+			pNullFrame = (PHEADER_802_11) pOutBuffer;
+			// Make the power save Null frame with PSM bit on
+			MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+			pNullFrame->Duration 	= 0;
+			pNullFrame->FC.Type     = BTYPE_DATA;
+			pNullFrame->FC.PwrMgmt  = PWR_SAVE;
+
+			// Send using priority queue
+			MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+			RTMPusecDelay(5000);
+		}
+
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+	}
+	else if (pReq->Measurement.ScanMode == MSRN_SCAN_MODE_BEACON_TABLE)
+	{
+		// Beacon report Mode, report all the APS in current bss table
+		DBGPRINT(RT_DEBUG_TRACE, ("Beacon Report Mode!\n"));
+
+		// Copy current BSS table to CCX table, we can omit this step later on.
+		NdisMoveMemory(&pAd->StaCfg.CCXBssTab, &pAd->ScanTab, sizeof(BSS_TABLE));
+
+		// Create beacon report from Bss table
+		AironetCreateBeaconReportFromBssTable(pAd);
+
+		// Set state to scanning
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+
+		// Enqueue report request
+		// Cisco scan request is finished, prepare beacon report
+		MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_DONE, 0, NULL);
+	}
+	else
+	{
+		// Wrong scan Mode
+		DBGPRINT(RT_DEBUG_TRACE, ("Wrong Scan Mode!\n"));
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconRequestAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+{
+	PRM_REQUEST_ACTION	pReq;
+	ULONG				Now32;
+
+    NdisGetSystemUpTime(&Now32);
+	pAd->StaCfg.LastBeaconRxTime = Now32;
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetReportAction ----->\n"));
+
+	// 1. Parse measurement type and call appropriate functions
+	if (pReq->ReqElem.Type == MSRN_TYPE_CHANNEL_LOAD_REQ)
+		// Channel Load measurement request
+		ChannelLoadReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_NOISE_HIST_REQ)
+		// Noise Histogram measurement request
+		NoiseHistReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_BEACON_REQ)
+		// Beacon measurement request
+		BeaconReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else
+		// Unknown. Do nothing and return
+		;
+
+	// 2. Point to the correct index of action element, start from 0
+	pAd->StaCfg.CurrentRMReqIdx++;
+
+	// 3. Check for parallel actions
+	if (pAd->StaCfg.ParallelReq == TRUE)
+	{
+		pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+		// Process next action right away
+		if (pReq->ReqElem.Type == MSRN_TYPE_CHANNEL_LOAD_REQ)
+			// Channel Load measurement request
+			ChannelLoadReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+		else if (pReq->ReqElem.Type == MSRN_TYPE_NOISE_HIST_REQ)
+			// Noise Histogram measurement request
+			NoiseHistReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+
+		pAd->StaCfg.ParallelReq = FALSE;
+		pAd->StaCfg.CurrentRMReqIdx++;
+	}
+
+	if (pAd->StaCfg.CurrentRMReqIdx >= pAd->StaCfg.RMReqCnt)
+	{
+		// 4. There is no more unprocessed measurement request, go for transmit this report
+		AironetFinalReportAction(pAd);
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+	}
+	else
+	{
+		pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+		if (pReq->Measurement.Channel != pAd->CommonCfg.Channel)
+		{
+			RTMPusecDelay(100000);
+		}
+
+		// 5. There are more requests to be measure
+		MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_REQ, 0, NULL);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetFinalReportAction(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PUCHAR					pDest;
+	PAIRONET_IAPP_HEADER	pIAPP;
+	PHEADER_802_11			pHeader;
+	UCHAR					AckRate = RATE_2;
+	USHORT					AckDuration = 0;
+	NDIS_STATUS				NStatus;
+	PUCHAR					pOutBuffer = NULL;
+	ULONG					FrameLen = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetFinalReportAction ----->\n"));
+
+	// 0. Set up the frame pointer, Frame was inited at the end of message action
+	pDest = &pAd->StaCfg.FrameReportBuf[LENGTH_802_11];
+
+	// 1. Update report IAPP fields
+	pIAPP = (PAIRONET_IAPP_HEADER) pDest;
+
+	// 2. Copy Cisco SNAP header
+	NdisMoveMemory(pIAPP->CiscoSnapHeader, SNAP_AIRONET, LENGTH_802_1_H);
+
+	// 3. network order for this 16bit length
+	pIAPP->Length  = cpu2be16(pAd->StaCfg.FrameReportLen - LENGTH_802_11 - LENGTH_802_1_H);
+
+	// 3.1 sanity check the report length, ignore it if there is nothing to report
+	if (be2cpu16(pIAPP->Length) <= 18)
+		return;
+
+	// 4. Type must be 0x32
+	pIAPP->Type    = AIRONET_IAPP_TYPE;
+
+	// 5. SubType for report must be 0x81
+	pIAPP->SubType = AIRONET_IAPP_SUBTYPE_REPORT;
+
+	// 6. DA is not used and must be zero, although the whole frame was cleared at the start of function
+	//    We will do it again here. We can use BSSID instead
+	COPY_MAC_ADDR(pIAPP->DA, pAd->CommonCfg.Bssid);
+
+	// 7. SA is the client reporting which must be our MAC
+	COPY_MAC_ADDR(pIAPP->SA, pAd->CurrentAddress);
+
+	// 8. Copy the saved dialog token
+	pIAPP->Token = pAd->StaCfg.IAPPToken;
+
+	// 9. Make the Report frame 802.11 header
+	//    Reuse function in wpa.c
+	pHeader = (PHEADER_802_11) pAd->StaCfg.FrameReportBuf;
+	pAd->Sequence ++;
+	WpaMacHeaderInit(pAd, pHeader, 0, pAd->CommonCfg.Bssid);
+
+	// ACK size	is 14 include CRC, and its rate	is based on real time information
+	AckRate     = pAd->CommonCfg.ExpectedACKRate[pAd->CommonCfg.MlmeRate];
+	AckDuration = RTMPCalcDuration(pAd, AckRate, 14);
+	pHeader->Duration = pAd->CommonCfg.Dsifs + AckDuration;
+
+	// Use MLME enqueue method
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus	!= NDIS_STATUS_SUCCESS)
+		return;
+
+	// 10. Prepare report frame with dynamic outbuffer. Just simply copy everything.
+	MakeOutgoingFrame(pOutBuffer,                       &FrameLen,
+	                  pAd->StaCfg.FrameReportLen, pAd->StaCfg.FrameReportBuf,
+		              END_OF_ARGS);
+
+	// 11. Send using priority queue
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	pAd->StaCfg.CCXReqType = MSRN_TYPE_UNUSED;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetFinalReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ChannelLoadReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PCHANNEL_LOAD_REPORT		pLoad;
+	PUCHAR						pDest;
+	UCHAR						CCABusyFraction;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadReportAction ----->\n"));
+
+	// Disable Rx with promiscuous reception, make it back to normal
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL); // Staion not drop control frame will fail WiFi Certification.
+
+	// 0. Setup pointer for processing beacon & probe response
+	pDest = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+	pReport = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+
+	// 1. Fill Measurement report element field.
+	pReport->Eid    = IE_MEASUREMENT_REPORT;
+	// Fixed Length at 9, not include Eid and length fields
+	pReport->Length = 9;
+	pReport->Token  = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Token;
+	pReport->Mode   = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Mode;
+	pReport->Type   = MSRN_TYPE_CHANNEL_LOAD_REQ;
+
+	// 2. Fill channel report measurement data
+	pDest += sizeof(MEASUREMENT_REPORT_ELEMENT);
+	pLoad  = (PCHANNEL_LOAD_REPORT) pDest;
+	pLoad->Channel  = pAd->StaCfg.MeasurementRequest[Index].Measurement.Channel;
+	pLoad->Spare    = 0;
+	pLoad->Duration = pAd->StaCfg.MeasurementRequest[Index].Measurement.Duration;
+
+	// 3. Calculate the CCA Busy Fraction
+	//    (Bytes + ACK size) * 8 / Tx speed * 255 / 1000 / measurement duration, use 24 us Tx speed
+	//     =  (Bytes + ACK) / 12 / duration
+	//     9 is the good value for pAd->StaCfg.CLFactor
+	// CCABusyFraction = (UCHAR) (pAd->StaCfg.CLBusyBytes / 9 / pLoad->Duration);
+	CCABusyFraction = (UCHAR) (pAd->StaCfg.CLBusyBytes / pAd->StaCfg.CLFactor / pLoad->Duration);
+	if (CCABusyFraction < 10)
+			CCABusyFraction = (UCHAR) (pAd->StaCfg.CLBusyBytes / 3 / pLoad->Duration) + 1;
+
+	pLoad->CCABusy = CCABusyFraction;
+	DBGPRINT(RT_DEBUG_TRACE, ("CLBusyByte %ld, Duration %d, Result, %d\n", pAd->StaCfg.CLBusyBytes, pLoad->Duration, CCABusyFraction));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen %d\n", pAd->StaCfg.FrameReportLen));
+	pAd->StaCfg.FrameReportLen += (sizeof(MEASUREMENT_REPORT_ELEMENT) + sizeof(CHANNEL_LOAD_REPORT));
+	DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen %d\n", pAd->StaCfg.FrameReportLen));
+
+	// 4. Clear channel load measurement flag
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	// 5. reset to idle state
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	NoiseHistReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PNOISE_HIST_REPORT			pNoise;
+	PUCHAR						pDest;
+	UCHAR						i,NoiseCnt;
+	USHORT						TotalRPICnt, TotalRPISum;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistReportAction ----->\n"));
+
+	// 0. Disable Rx with promiscuous reception, make it back to normal
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL); // Staion not drop control frame will fail WiFi Certification.
+	// 1. Setup pointer for processing beacon & probe response
+	pDest = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+	pReport = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+
+	// 2. Fill Measurement report element field.
+	pReport->Eid    = IE_MEASUREMENT_REPORT;
+	// Fixed Length at 16, not include Eid and length fields
+	pReport->Length = 16;
+	pReport->Token  = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Token;
+	pReport->Mode   = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Mode;
+	pReport->Type   = MSRN_TYPE_NOISE_HIST_REQ;
+
+	// 3. Fill noise histogram report measurement data
+	pDest += sizeof(MEASUREMENT_REPORT_ELEMENT);
+	pNoise  = (PNOISE_HIST_REPORT) pDest;
+	pNoise->Channel  = pAd->StaCfg.MeasurementRequest[Index].Measurement.Channel;
+	pNoise->Spare    = 0;
+	pNoise->Duration = pAd->StaCfg.MeasurementRequest[Index].Measurement.Duration;
+	// 4. Fill Noise histogram, the total RPI counts should be 0.4 * TU
+	//    We estimate 4000 normal packets received durning 10 seconds test.
+	//    Adjust it if required.
+	// 3 is a good value for pAd->StaCfg.NHFactor
+	// TotalRPICnt = pNoise->Duration * 3 / 10;
+	TotalRPICnt = pNoise->Duration * pAd->StaCfg.NHFactor / 10;
+	TotalRPISum = 0;
+
+	for (i = 0; i < 8; i++)
+	{
+		TotalRPISum += pAd->StaCfg.RPIDensity[i];
+		DBGPRINT(RT_DEBUG_TRACE, ("RPI %d Conuts %d\n", i, pAd->StaCfg.RPIDensity[i]));
+	}
+
+	// Double check if the counter is larger than our expectation.
+	// We will replace it with the total number plus a fraction.
+	if (TotalRPISum > TotalRPICnt)
+		TotalRPICnt = TotalRPISum + pNoise->Duration / 20;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Total RPI Conuts %d\n", TotalRPICnt));
+
+	// 5. Initialize noise count for the total summation of 0xff
+	NoiseCnt = 0;
+	for (i = 1; i < 8; i++)
+	{
+		pNoise->Density[i] = (UCHAR) (pAd->StaCfg.RPIDensity[i] * 255 / TotalRPICnt);
+		if ((pNoise->Density[i] == 0) && (pAd->StaCfg.RPIDensity[i] != 0))
+			pNoise->Density[i]++;
+		NoiseCnt += pNoise->Density[i];
+		DBGPRINT(RT_DEBUG_TRACE, ("Reported RPI[%d]  = 0x%02x\n", i, pNoise->Density[i]));
+	}
+
+	// 6. RPI[0] represents the rest of counts
+	pNoise->Density[0] = 0xff - NoiseCnt;
+	DBGPRINT(RT_DEBUG_TRACE, ("Reported RPI[0]  = 0x%02x\n", pNoise->Density[0]));
+
+	pAd->StaCfg.FrameReportLen += (sizeof(MEASUREMENT_REPORT_ELEMENT) + sizeof(NOISE_HIST_REPORT));
+
+	// 7. Clear channel load measurement flag
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	// 8. reset to idle state
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare Beacon report action,
+
+	Arguments:
+		pAd	Pointer	to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	BeaconReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconReportAction ----->\n"));
+
+	// Looks like we don't have anything thing need to do here.
+	// All measurement report already finished in AddBeaconReport
+	// The length is in the FrameReportLen
+
+	// reset Beacon index for next beacon request
+	pAd->StaCfg.LastBssIndex = 0xff;
+
+	// reset to idle state
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+		Index		Current BSSID in CCXBsstab entry index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetAddBeaconReport(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	ULONG				Index,
+	IN	PMLME_QUEUE_ELEM	pElem)
+{
+	PVOID						pMsg;
+	PUCHAR						pSrc, pDest;
+	UCHAR						ReqIdx;
+	ULONG						MsgLen;
+	USHORT						Length;
+	PFRAME_802_11				pFrame;
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PEID_STRUCT			        pEid;
+	PBEACON_REPORT				pBeaconReport;
+	PBSS_ENTRY					pBss;
+
+	// 0. Setup pointer for processing beacon & probe response
+	pMsg   = pElem->Msg;
+	MsgLen = pElem->MsgLen;
+	pFrame = (PFRAME_802_11) pMsg;
+	pSrc   = pFrame->Octet;				// Start from AP TSF
+	pBss   = (PBSS_ENTRY) &pAd->StaCfg.CCXBssTab.BssEntry[Index];
+	ReqIdx = pAd->StaCfg.CurrentRMReqIdx;
+
+	// 1 Check the Index, if we already create this entry, only update the average RSSI
+	if ((Index <= pAd->StaCfg.LastBssIndex) && (pAd->StaCfg.LastBssIndex != 0xff))
+	{
+		pDest  = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.BssReportOffset[Index]];
+		// Point to bss report information
+		pDest += sizeof(MEASUREMENT_REPORT_ELEMENT);
+		pBeaconReport = (PBEACON_REPORT) pDest;
+
+		// Update Rx power, in dBm
+		// Get the original RSSI readback from BBP
+		pBeaconReport->RxPower += pAd->BbpRssiToDbmDelta;
+		// Average the Rssi reading
+		pBeaconReport->RxPower  = (pBeaconReport->RxPower + pBss->Rssi) / 2;
+		// Get to dBm format
+		pBeaconReport->RxPower -= pAd->BbpRssiToDbmDelta;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Bssid %02x:%02x:%02x:%02x:%02x:%02x ",
+			pBss->Bssid[0], pBss->Bssid[1], pBss->Bssid[2],
+			pBss->Bssid[3], pBss->Bssid[4], pBss->Bssid[5]));
+		DBGPRINT(RT_DEBUG_TRACE, ("RxPower[%ld] Rssi %d, Avg Rssi %d\n", Index, (pBss->Rssi - pAd->BbpRssiToDbmDelta), pBeaconReport->RxPower - 256));
+		DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen = %d\n", pAd->StaCfg.BssReportOffset[Index]));
+
+		// Update other information here
+
+		// Done
+		return;
+	}
+
+	// 2. Update reported Index
+	pAd->StaCfg.LastBssIndex = Index;
+
+	// 3. Setup the buffer address for copying this BSSID into reporting frame
+	//    The offset should start after 802.11 header and report frame header.
+	pDest = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+
+	// 4. Save the start offset of each Bss in report frame
+	pAd->StaCfg.BssReportOffset[Index] = pAd->StaCfg.FrameReportLen;
+
+	// 5. Fill Measurement report fields
+	pReport = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+	pReport->Eid = IE_MEASUREMENT_REPORT;
+	pReport->Length = 0;
+	pReport->Token  = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Token;
+	pReport->Mode   = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Mode;
+	pReport->Type   = MSRN_TYPE_BEACON_REQ;
+	Length          = sizeof(MEASUREMENT_REPORT_ELEMENT);
+	pDest          += sizeof(MEASUREMENT_REPORT_ELEMENT);
+
+	// 6. Start thebeacon report format
+	pBeaconReport = (PBEACON_REPORT) pDest;
+	pDest        += sizeof(BEACON_REPORT);
+	Length       += sizeof(BEACON_REPORT);
+
+	// 7. Copy Channel number
+	pBeaconReport->Channel        = pBss->Channel;
+	pBeaconReport->Spare          = 0;
+	pBeaconReport->Duration       = pAd->StaCfg.MeasurementRequest[ReqIdx].Measurement.Duration;
+	pBeaconReport->PhyType        = ((pBss->SupRateLen+pBss->ExtRateLen > 4) ? PHY_ERP : PHY_DSS);
+	// 8. Rx power, in dBm
+	pBeaconReport->RxPower        = pBss->Rssi - pAd->BbpRssiToDbmDelta;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Bssid %02x:%02x:%02x:%02x:%02x:%02x ",
+		pBss->Bssid[0], pBss->Bssid[1], pBss->Bssid[2],
+		pBss->Bssid[3], pBss->Bssid[4], pBss->Bssid[5]));
+	DBGPRINT(RT_DEBUG_TRACE, ("RxPower[%ld], Rssi %d\n", Index, pBeaconReport->RxPower - 256));
+	DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen = %d\n", pAd->StaCfg.FrameReportLen));
+
+	pBeaconReport->BeaconInterval = pBss->BeaconPeriod;
+	COPY_MAC_ADDR(pBeaconReport->BSSID, pFrame->Hdr.Addr3);
+	NdisMoveMemory(pBeaconReport->ParentTSF, pSrc, 4);
+	NdisMoveMemory(pBeaconReport->TargetTSF, &pElem->TimeStamp.u.LowPart, 4);
+	NdisMoveMemory(&pBeaconReport->TargetTSF[4], &pElem->TimeStamp.u.HighPart, 4);
+
+	// 9. Skip the beacon frame and offset to start of capabilityinfo since we already processed capabilityinfo
+	pSrc += (TIMESTAMP_LEN + 2);
+	pBeaconReport->CapabilityInfo = *(USHORT *)pSrc;
+
+	// 10. Point to start of element ID
+	pSrc += 2;
+	pEid = (PEID_STRUCT) pSrc;
+
+	// 11. Start process all variable Eid oayload and add the appropriate to the frame report
+	while (((PUCHAR) pEid + pEid->Len + 1) < ((PUCHAR) pFrame + MsgLen))
+	{
+		// Only limited EID are required to report for CCX 2. It includes SSID, Supported rate,
+		// FH paramenter set, DS parameter set, CF parameter set, IBSS parameter set,
+		// TIM (report first 4 bytes only, radio measurement capability
+		switch (pEid->Eid)
+		{
+			case IE_SSID:
+			case IE_SUPP_RATES:
+			case IE_FH_PARM:
+			case IE_DS_PARM:
+			case IE_CF_PARM:
+			case IE_IBSS_PARM:
+				NdisMoveMemory(pDest, pEid, pEid->Len + 2);
+				pDest  += (pEid->Len + 2);
+				Length += (pEid->Len + 2);
+				break;
+
+			case IE_MEASUREMENT_CAPABILITY:
+				// Since this IE is duplicated with WPA security IE, we has to do sanity check before
+				// recognize it.
+				// 1. It also has fixed 6 bytes IE length.
+				if (pEid->Len != 6)
+					break;
+				// 2. Check the Cisco Aironet OUI
+				if (NdisEqualMemory(CISCO_OUI, (pSrc + 2), 3))
+				{
+					// Matched, this is what we want
+					NdisMoveMemory(pDest, pEid, pEid->Len + 2);
+					pDest  += (pEid->Len + 2);
+					Length += (pEid->Len + 2);
+				}
+				break;
+
+			case IE_TIM:
+				if (pEid->Len > 4)
+				{
+					// May truncate and report the first 4 bytes only, with the eid & len, total should be 6
+					NdisMoveMemory(pDest, pEid, 6);
+					pDest  += 6;
+					Length += 6;
+				}
+				else
+				{
+					NdisMoveMemory(pDest, pEid, pEid->Len + 2);
+					pDest  += (pEid->Len + 2);
+					Length += (pEid->Len + 2);
+				}
+				break;
+
+			default:
+				break;
+		}
+		// 12. Move to next element ID
+		pSrc += (2 + pEid->Len);
+		pEid = (PEID_STRUCT) pSrc;
+	}
+
+	// 13. Update the length in the header, not include EID and length
+	pReport->Length = Length - 4;
+
+	// 14. Update the frame report buffer data length
+	pAd->StaCfg.FrameReportLen += Length;
+	DBGPRINT(RT_DEBUG_TRACE, ("FR len = %d\n", pAd->StaCfg.FrameReportLen));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+		Index		Current BSSID in CCXBsstab entry index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetCreateBeaconReportFromBssTable(
+	IN	PRTMP_ADAPTER		pAd)
+{
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PBEACON_REPORT				pBeaconReport;
+	UCHAR						Index, ReqIdx;
+	USHORT						Length;
+	PUCHAR						pDest;
+	PBSS_ENTRY					pBss;
+
+	// 0. setup base pointer
+	ReqIdx = pAd->StaCfg.CurrentRMReqIdx;
+
+	for (Index = 0; Index < pAd->StaCfg.CCXBssTab.BssNr; Index++)
+	{
+		// 1. Setup the buffer address for copying this BSSID into reporting frame
+		//    The offset should start after 802.11 header and report frame header.
+		pDest  = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+		pBss   = (PBSS_ENTRY) &pAd->StaCfg.CCXBssTab.BssEntry[Index];
+		Length = 0;
+
+		// 2. Fill Measurement report fields
+		pReport         = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+		pReport->Eid    = IE_MEASUREMENT_REPORT;
+		pReport->Length = 0;
+		pReport->Token  = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Token;
+		pReport->Mode   = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Mode;
+		pReport->Type   = MSRN_TYPE_BEACON_REQ;
+		Length          = sizeof(MEASUREMENT_REPORT_ELEMENT);
+		pDest          += sizeof(MEASUREMENT_REPORT_ELEMENT);
+
+		// 3. Start the beacon report format
+		pBeaconReport = (PBEACON_REPORT) pDest;
+		pDest        += sizeof(BEACON_REPORT);
+		Length       += sizeof(BEACON_REPORT);
+
+		// 4. Copy Channel number
+		pBeaconReport->Channel        = pBss->Channel;
+		pBeaconReport->Spare          = 0;
+		pBeaconReport->Duration       = pAd->StaCfg.MeasurementRequest[ReqIdx].Measurement.Duration;
+		pBeaconReport->PhyType        = ((pBss->SupRateLen+pBss->ExtRateLen > 4) ? PHY_ERP : PHY_DSS);
+		pBeaconReport->RxPower        = pBss->Rssi - pAd->BbpRssiToDbmDelta;
+		pBeaconReport->BeaconInterval = pBss->BeaconPeriod;
+		pBeaconReport->CapabilityInfo = pBss->CapabilityInfo;
+		COPY_MAC_ADDR(pBeaconReport->BSSID, pBss->Bssid);
+		NdisMoveMemory(pBeaconReport->ParentTSF, pBss->PTSF, 4);
+		NdisMoveMemory(pBeaconReport->TargetTSF, pBss->TTSF, 8);
+
+		// 5. Create SSID
+		*pDest++ = 0x00;
+		*pDest++ = pBss->SsidLen;
+		NdisMoveMemory(pDest, pBss->Ssid, pBss->SsidLen);
+		pDest  += pBss->SsidLen;
+		Length += (2 + pBss->SsidLen);
+
+		// 6. Create SupportRates
+		*pDest++ = 0x01;
+		*pDest++ = pBss->SupRateLen;
+		NdisMoveMemory(pDest, pBss->SupRate, pBss->SupRateLen);
+		pDest  += pBss->SupRateLen;
+		Length += (2 + pBss->SupRateLen);
+
+		// 7. DS Parameter
+		*pDest++ = 0x03;
+		*pDest++ = 1;
+		*pDest++ = pBss->Channel;
+		Length  += 3;
+
+		// 8. IBSS parameter if presents
+		if (pBss->BssType == BSS_ADHOC)
+		{
+			*pDest++ = 0x06;
+			*pDest++ = 2;
+			*(PUSHORT) pDest = pBss->AtimWin;
+			pDest   += 2;
+			Length  += 4;
+		}
+
+		// 9. Update length field, not include EID and length
+		pReport->Length = Length - 4;
+
+		// 10. Update total frame size
+		pAd->StaCfg.FrameReportLen += Length;
+	}
+}
diff --git a/drivers/staging/rt2870/sta/assoc.c b/drivers/staging/rt2870/sta/assoc.c
new file mode 100644
index 000000000000..a76dab500bcb
--- /dev/null
+++ b/drivers/staging/rt2870/sta/assoc.c
@@ -0,0 +1,2039 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	assoc.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John		2004-9-3		porting from RT2500
+*/
+#include "../rt_config.h"
+
+UCHAR	CipherWpaTemplate[] = {
+		0xdd, 					// WPA IE
+		0x16,					// Length
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x01	// authentication
+		};
+
+UCHAR	CipherWpa2Template[] = {
+		0x30,					// RSN IE
+		0x14,					// Length
+		0x01, 0x00,				// Version
+		0x00, 0x0f, 0xac, 0x02,	// group cipher, TKIP
+		0x01, 0x00,				// number of pairwise
+		0x00, 0x0f, 0xac, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x0f, 0xac, 0x02,	// authentication
+		0x00, 0x00,				// RSN capability
+		};
+
+UCHAR	Ccx2IeInfo[] = { 0x00, 0x40, 0x96, 0x03, 0x02};
+
+/*
+	==========================================================================
+	Description:
+		association state machine init, including state transition and timer init
+	Parameters:
+		S - pointer to the association state machine
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID AssocStateMachineInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  STATE_MACHINE *S,
+	OUT STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(S, Trans, MAX_ASSOC_STATE, MAX_ASSOC_MSG, (STATE_MACHINE_FUNC)Drop, ASSOC_IDLE, ASSOC_MACHINE_BASE);
+
+	// first column
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)MlmeAssocReqAction);
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)MlmeReassocReqAction);
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)MlmeDisassocReqAction);
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+
+	// second column
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAssoc);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenReassoc);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenDisassociate);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_PEER_ASSOC_RSP, (STATE_MACHINE_FUNC)PeerAssocRspAction);
+	//
+	// Patch 3Com AP MOde:3CRWE454G72
+	// We send Assoc request frame to this AP, it always send Reassoc Rsp not Associate Rsp.
+	//
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_PEER_REASSOC_RSP, (STATE_MACHINE_FUNC)PeerAssocRspAction);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_ASSOC_TIMEOUT, (STATE_MACHINE_FUNC)AssocTimeoutAction);
+
+	// third column
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAssoc);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenReassoc);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenDisassociate);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_PEER_REASSOC_RSP, (STATE_MACHINE_FUNC)PeerReassocRspAction);
+	//
+	// Patch, AP doesn't send Reassociate Rsp frame to Station.
+	//
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_PEER_ASSOC_RSP, (STATE_MACHINE_FUNC)PeerReassocRspAction);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_REASSOC_TIMEOUT, (STATE_MACHINE_FUNC)ReassocTimeoutAction);
+
+	// fourth column
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAssoc);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenReassoc);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenDisassociate);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_DISASSOC_TIMEOUT, (STATE_MACHINE_FUNC)DisassocTimeoutAction);
+
+	// initialize the timer
+	RTMPInitTimer(pAd, &pAd->MlmeAux.AssocTimer, GET_TIMER_FUNCTION(AssocTimeout), pAd, FALSE);
+	RTMPInitTimer(pAd, &pAd->MlmeAux.ReassocTimer, GET_TIMER_FUNCTION(ReassocTimeout), pAd, FALSE);
+	RTMPInitTimer(pAd, &pAd->MlmeAux.DisassocTimer, GET_TIMER_FUNCTION(DisassocTimeout), pAd, FALSE);
+}
+
+/*
+	==========================================================================
+	Description:
+		Association timeout procedure. After association timeout, this function
+		will be called and it will put a message into the MLME queue
+	Parameters:
+		Standard timer parameters
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AssocTimeout(IN PVOID SystemSpecific1,
+				 IN PVOID FunctionContext,
+				 IN PVOID SystemSpecific2,
+				 IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_ASSOC_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		Reassociation timeout procedure. After reassociation timeout, this
+		function will be called and put a message into the MLME queue
+	Parameters:
+		Standard timer parameters
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID ReassocTimeout(IN PVOID SystemSpecific1,
+					IN PVOID FunctionContext,
+					IN PVOID SystemSpecific2,
+					IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_REASSOC_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		Disassociation timeout procedure. After disassociation timeout, this
+		function will be called and put a message into the MLME queue
+	Parameters:
+		Standard timer parameters
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID DisassocTimeout(IN PVOID SystemSpecific1,
+					IN PVOID FunctionContext,
+					IN PVOID SystemSpecific2,
+					IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_DISASSOC_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		mlme assoc req handling procedure
+	Parameters:
+		Adapter - Adapter pointer
+		Elem - MLME Queue Element
+	Pre:
+		the station has been authenticated and the following information is stored in the config
+			-# SSID
+			-# supported rates and their length
+			-# listen interval (Adapter->StaCfg.default_listen_count)
+			-# Transmit power  (Adapter->StaCfg.tx_power)
+	Post  :
+		-# An association request frame is generated and sent to the air
+		-# Association timer starts
+		-# Association state -> ASSOC_WAIT_RSP
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeAssocReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR			ApAddr[6];
+	HEADER_802_11	AssocHdr;
+	UCHAR			Ccx2Len = 5;
+	UCHAR			WmeIe[9] = {IE_VENDOR_SPECIFIC, 0x07, 0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
+	USHORT			ListenIntv;
+	ULONG			Timeout;
+	USHORT			CapabilityInfo;
+	BOOLEAN			TimerCancelled;
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	ULONG			tmp;
+	USHORT			VarIesOffset;
+	UCHAR			CkipFlag;
+	UCHAR			CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH];
+	UCHAR			AironetCkipIe = IE_AIRONET_CKIP;
+	UCHAR			AironetCkipLen = CKIP_NEGOTIATION_LENGTH;
+	UCHAR			AironetIPAddressIE = IE_AIRONET_IPADDRESS;
+	UCHAR			AironetIPAddressLen = AIRONET_IPADDRESS_LENGTH;
+	UCHAR			AironetIPAddressBuffer[AIRONET_IPADDRESS_LENGTH] = {0x00, 0x40, 0x96, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00};
+	USHORT			Status;
+
+	// Block all authentication request durning WPA block period
+	if (pAd->StaCfg.bBlockAssoc == TRUE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Block Assoc request durning WPA block period!\n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_STATE_MACHINE_REJECT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+	}
+	// check sanity first
+	else if (MlmeAssocReqSanity(pAd, Elem->Msg, Elem->MsgLen, ApAddr, &CapabilityInfo, &Timeout, &ListenIntv))
+	{
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer, &TimerCancelled);
+		COPY_MAC_ADDR(pAd->MlmeAux.Bssid, ApAddr);
+
+		// Get an unused nonpaged memory
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+		if (NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeAssocReqAction() allocate memory failed \n"));
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			Status = MLME_FAIL_NO_RESOURCE;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+			return;
+		}
+
+		// Add by James 03/06/27
+		pAd->StaCfg.AssocInfo.Length = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+		// Association don't need to report MAC address
+		pAd->StaCfg.AssocInfo.AvailableRequestFixedIEs =
+			NDIS_802_11_AI_REQFI_CAPABILITIES | NDIS_802_11_AI_REQFI_LISTENINTERVAL;
+		pAd->StaCfg.AssocInfo.RequestFixedIEs.Capabilities = CapabilityInfo;
+		pAd->StaCfg.AssocInfo.RequestFixedIEs.ListenInterval = ListenIntv;
+		// Only reassociate need this
+		//COPY_MAC_ADDR(pAd->StaCfg.AssocInfo.RequestFixedIEs.CurrentAPAddress, ApAddr);
+		pAd->StaCfg.AssocInfo.OffsetRequestIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+
+        NdisZeroMemory(pAd->StaCfg.ReqVarIEs, MAX_VIE_LEN);
+		// First add SSID
+		VarIesOffset = 0;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &SsidIe, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &pAd->MlmeAux.SsidLen, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+		VarIesOffset += pAd->MlmeAux.SsidLen;
+
+		// Second add Supported rates
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &SupRateIe, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &pAd->MlmeAux.SupRateLen, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, pAd->MlmeAux.SupRate, pAd->MlmeAux.SupRateLen);
+		VarIesOffset += pAd->MlmeAux.SupRateLen;
+		// End Add by James
+
+        if ((pAd->CommonCfg.Channel > 14) &&
+            (pAd->CommonCfg.bIEEE80211H == TRUE))
+            CapabilityInfo |= 0x0100;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Send ASSOC request...\n"));
+		MgtMacHeaderInit(pAd, &AssocHdr, SUBTYPE_ASSOC_REQ, 0, ApAddr, ApAddr);
+
+		// Build basic frame first
+		MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+						  sizeof(HEADER_802_11),	&AssocHdr,
+						  2,						&CapabilityInfo,
+						  2,						&ListenIntv,
+						  1,						&SsidIe,
+						  1,						&pAd->MlmeAux.SsidLen,
+						  pAd->MlmeAux.SsidLen, 	pAd->MlmeAux.Ssid,
+						  1,						&SupRateIe,
+						  1,						&pAd->MlmeAux.SupRateLen,
+						  pAd->MlmeAux.SupRateLen,  pAd->MlmeAux.SupRate,
+						  END_OF_ARGS);
+
+		if (pAd->MlmeAux.ExtRateLen != 0)
+		{
+			MakeOutgoingFrame(pOutBuffer + FrameLen,    &tmp,
+							  1,                        &ExtRateIe,
+							  1,                        &pAd->MlmeAux.ExtRateLen,
+							  pAd->MlmeAux.ExtRateLen,  pAd->MlmeAux.ExtRate,
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+#ifdef DOT11_N_SUPPORT
+		// HT
+		if ((pAd->MlmeAux.HtCapabilityLen > 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+		{
+			ULONG TmpLen;
+			UCHAR HtLen;
+			UCHAR BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+			if (pAd->StaActive.SupportedPhyInfo.bPreNHt == TRUE)
+			{
+				HtLen = SIZE_HT_CAP_IE + 4;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &WpaIe,
+							  1,                                &HtLen,
+							  4,                                &BROADCOM[0],
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+			}
+			else
+			{
+#ifdef RT_BIG_ENDIAN
+		        HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+
+#ifndef RT_BIG_ENDIAN
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &HtCapIe,
+							  1,                                &pAd->MlmeAux.HtCapabilityLen,
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+#else
+                NdisZeroMemory(&HtCapabilityTmp, sizeof(HT_CAPABILITY_IE));
+                NdisMoveMemory(&HtCapabilityTmp, &pAd->MlmeAux.HtCapability, pAd->MlmeAux.HtCapabilityLen);
+        		*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+        		*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+        		MakeOutgoingFrame(pOutBuffer + FrameLen,         &TmpLen,
+        							1,                           &HtCapIe,
+        							1,                           &pAd->MlmeAux.HtCapabilityLen,
+        							pAd->MlmeAux.HtCapabilityLen,&HtCapabilityTmp,
+        							END_OF_ARGS);
+#endif
+			}
+			FrameLen += TmpLen;
+		}
+#endif // DOT11_N_SUPPORT //
+
+		// add Ralink proprietary IE to inform AP this STA is going to use AGGREGATION or PIGGY-BACK+AGGREGATION
+		// Case I: (Aggregation + Piggy-Back)
+		// 1. user enable aggregation, AND
+		// 2. Mac support piggy-back
+		// 3. AP annouces it's PIGGY-BACK+AGGREGATION-capable in BEACON
+		// Case II: (Aggregation)
+		// 1. user enable aggregation, AND
+		// 2. AP annouces it's AGGREGATION-capable in BEACON
+		if (pAd->CommonCfg.bAggregationCapable)
+		{
+			if ((pAd->CommonCfg.bPiggyBackCapable) && ((pAd->MlmeAux.APRalinkIe & 0x00000003) == 3))
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x03, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+			else if (pAd->MlmeAux.APRalinkIe & 0x00000001)
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x01, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+		}
+		else
+		{
+			ULONG TmpLen;
+			UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x06, 0x00, 0x00, 0x00};
+			MakeOutgoingFrame(pOutBuffer+FrameLen,		 &TmpLen,
+							  9,						 RalinkIe,
+							  END_OF_ARGS);
+			FrameLen += TmpLen;
+		}
+
+		if (pAd->MlmeAux.APEdcaParm.bValid)
+		{
+			if (pAd->CommonCfg.bAPSDCapable && pAd->MlmeAux.APEdcaParm.bAPSDCapable)
+			{
+				QBSS_STA_INFO_PARM QosInfo;
+
+				NdisZeroMemory(&QosInfo, sizeof(QBSS_STA_INFO_PARM));
+				QosInfo.UAPSD_AC_BE = pAd->CommonCfg.bAPSDAC_BE;
+				QosInfo.UAPSD_AC_BK = pAd->CommonCfg.bAPSDAC_BK;
+				QosInfo.UAPSD_AC_VI = pAd->CommonCfg.bAPSDAC_VI;
+				QosInfo.UAPSD_AC_VO = pAd->CommonCfg.bAPSDAC_VO;
+				QosInfo.MaxSPLength = pAd->CommonCfg.MaxSPLength;
+				WmeIe[8] |= *(PUCHAR)&QosInfo;
+			}
+			else
+			{
+                // The Parameter Set Count is set to ¡§0¡¨ in the association request frames
+                // WmeIe[8] |= (pAd->MlmeAux.APEdcaParm.EdcaUpdateCount & 0x0f);
+			}
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen,    &tmp,
+							  9,                        &WmeIe[0],
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		//
+		// Let WPA(#221) Element ID on the end of this association frame.
+		// Otherwise some AP will fail on parsing Element ID and set status fail on Assoc Rsp.
+		// For example: Put Vendor Specific IE on the front of WPA IE.
+		// This happens on AP (Model No:Linksys WRK54G)
+		//
+		if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2)
+			)
+            )
+		{
+			UCHAR RSNIe = IE_WPA;
+
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+                (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2))
+			{
+				RSNIe = IE_WPA2;
+			}
+
+            	RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, BSS0);
+
+            // Check for WPA PMK cache list
+			if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2)
+			{
+			    INT     idx;
+                BOOLEAN FoundPMK = FALSE;
+				// Search chched PMKID, append it if existed
+				for (idx = 0; idx < PMKID_NO; idx++)
+				{
+					if (NdisEqualMemory(ApAddr, &pAd->StaCfg.SavedPMK[idx].BSSID, 6))
+					{
+						FoundPMK = TRUE;
+						break;
+					}
+				}
+
+				if (FoundPMK)
+				{
+					// Set PMK number
+					*(PUSHORT) &pAd->StaCfg.RSN_IE[pAd->StaCfg.RSNIE_Len] = 1;
+					NdisMoveMemory(&pAd->StaCfg.RSN_IE[pAd->StaCfg.RSNIE_Len + 2], &pAd->StaCfg.SavedPMK[idx].PMKID, 16);
+                    pAd->StaCfg.RSNIE_Len += 18;
+				}
+			}
+
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,    		&tmp,
+				              		1,                              &RSNIe,
+		                        	1,                              &pAd->StaCfg.RSNIE_Len,
+		                        	pAd->StaCfg.RSNIE_Len,			pAd->StaCfg.RSN_IE,
+		                        	END_OF_ARGS);
+			}
+
+			FrameLen += tmp;
+
+			{
+	            // Append Variable IE
+	            NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &RSNIe, 1);
+	            VarIesOffset += 1;
+	            NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &pAd->StaCfg.RSNIE_Len, 1);
+	            VarIesOffset += 1;
+			}
+			NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);
+			VarIesOffset += pAd->StaCfg.RSNIE_Len;
+
+			// Set Variable IEs Length
+			pAd->StaCfg.ReqVarIELen = VarIesOffset;
+		}
+
+		// We have update that at PeerBeaconAtJoinRequest()
+		CkipFlag = pAd->StaCfg.CkipFlag;
+		if (CkipFlag != 0)
+		{
+			NdisZeroMemory(CkipNegotiationBuffer, CKIP_NEGOTIATION_LENGTH);
+			CkipNegotiationBuffer[2] = 0x66;
+			// Make it try KP & MIC, since we have to follow the result from AssocRsp
+			CkipNegotiationBuffer[8] = 0x18;
+			CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH - 1] = 0x22;
+			CkipFlag = 0x18;
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, 	&tmp,
+						1,						  		&AironetCkipIe,
+						1,						  		&AironetCkipLen,
+						AironetCkipLen, 		  		CkipNegotiationBuffer,
+						END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		// Add CCX v2 request if CCX2 admin state is on
+		if (pAd->StaCfg.CCXControl.field.Enable == 1)
+		{
+
+			//
+			// Add AironetIPAddressIE for Cisco CCX 2.X
+			// Add CCX Version
+			//
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+						1,							&AironetIPAddressIE,
+						1,							&AironetIPAddressLen,
+						AironetIPAddressLen,		AironetIPAddressBuffer,
+						1,							&Ccx2Ie,
+						1,							&Ccx2Len,
+						Ccx2Len,				    Ccx2IeInfo,
+						END_OF_ARGS);
+			FrameLen += tmp;
+
+			//
+			// Add CipherSuite CCKM or LeapTkip if setting.
+			//
+#ifdef LEAP_SUPPORT
+			if (LEAP_CCKM_ON(pAd))
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+						CipherSuiteCiscoCCKMLen,		CipherSuiteCiscoCCKM,
+						END_OF_ARGS);
+				FrameLen += tmp;
+
+				// Third add RSN
+				NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, CipherSuiteCiscoCCKM, CipherSuiteCiscoCCKMLen); //Save CipherSuite
+				VarIesOffset += CipherSuiteCiscoCCKMLen;
+			}
+			else if ((pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP) && (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled))
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+						CipherSuiteCCXTkipLen,	    CipherSuiteCCXTkip,
+						END_OF_ARGS);
+				FrameLen += tmp;
+
+				// Third add RSN
+				NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, CipherSuiteCCXTkip, CipherSuiteCCXTkipLen);
+				VarIesOffset += CipherSuiteCCXTkipLen;
+			}
+#endif // LEAP_SUPPORT //
+
+			// Add by James 03/06/27
+			// Set Variable IEs Length
+			pAd->StaCfg.ReqVarIELen = VarIesOffset;
+			pAd->StaCfg.AssocInfo.RequestIELength = VarIesOffset;
+
+			// OffsetResponseIEs follow ReqVarIE
+			pAd->StaCfg.AssocInfo.OffsetResponseIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION) + pAd->StaCfg.ReqVarIELen;
+			// End Add by James
+		}
+
+
+		MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+
+		RTMPSetTimer(&pAd->MlmeAux.AssocTimer, Timeout);
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_WAIT_RSP;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeAssocReqAction() sanity check failed. BUG!!!!!! \n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+	}
+
+}
+
+/*
+	==========================================================================
+	Description:
+		mlme reassoc req handling procedure
+	Parameters:
+		Elem -
+	Pre:
+		-# SSID  (Adapter->StaCfg.ssid[])
+		-# BSSID (AP address, Adapter->StaCfg.bssid)
+		-# Supported rates (Adapter->StaCfg.supported_rates[])
+		-# Supported rates length (Adapter->StaCfg.supported_rates_len)
+		-# Tx power (Adapter->StaCfg.tx_power)
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeReassocReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR			ApAddr[6];
+	HEADER_802_11	ReassocHdr;
+	UCHAR			Ccx2Len = 5;
+	UCHAR			WmeIe[9] = {IE_VENDOR_SPECIFIC, 0x07, 0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
+	USHORT			CapabilityInfo, ListenIntv;
+	ULONG			Timeout;
+	ULONG			FrameLen = 0;
+	BOOLEAN			TimerCancelled;
+	NDIS_STATUS		NStatus;
+	ULONG			tmp;
+	PUCHAR			pOutBuffer = NULL;
+//CCX 2.X
+#ifdef LEAP_SUPPORT
+	UCHAR			CkipFlag;
+	UCHAR			CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH];
+	UCHAR			AironetCkipIe = IE_AIRONET_CKIP;
+	UCHAR			AironetCkipLen = CKIP_NEGOTIATION_LENGTH;
+	UCHAR			AironetIPAddressIE = IE_AIRONET_IPADDRESS;
+	UCHAR			AironetIPAddressLen = AIRONET_IPADDRESS_LENGTH;
+	UCHAR			AironetIPAddressBuffer[AIRONET_IPADDRESS_LENGTH] = {0x00, 0x40, 0x96, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00};
+	UCHAR			AironetCCKMReassocIE = IE_AIRONET_CCKMREASSOC;
+	UCHAR			AironetCCKMReassocLen = AIRONET_CCKMREASSOC_LENGTH;
+	UCHAR			AironetCCKMReassocBuffer[AIRONET_CCKMREASSOC_LENGTH];
+	UCHAR			AironetOUI[] = {0x00, 0x40, 0x96, 0x00};
+	UCHAR			MICMN[16];
+	UCHAR			CalcMicBuffer[80];
+	ULONG			CalcMicBufferLen = 0;
+#endif // LEAP_SUPPORT //
+	USHORT			Status;
+
+	// Block all authentication request durning WPA block period
+	if (pAd->StaCfg.bBlockAssoc == TRUE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Block ReAssoc request durning WPA block period!\n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_STATE_MACHINE_REJECT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+	}
+	// the parameters are the same as the association
+	else if(MlmeAssocReqSanity(pAd, Elem->Msg, Elem->MsgLen, ApAddr, &CapabilityInfo, &Timeout, &ListenIntv))
+	{
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer, &TimerCancelled);
+
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeReassocReqAction() allocate memory failed \n"));
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			Status = MLME_FAIL_NO_RESOURCE;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+			return;
+		}
+
+		COPY_MAC_ADDR(pAd->MlmeAux.Bssid, ApAddr);
+
+		// make frame, use bssid as the AP address??
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Send RE-ASSOC request...\n"));
+		MgtMacHeaderInit(pAd, &ReassocHdr, SUBTYPE_REASSOC_REQ, 0, ApAddr, ApAddr);
+		MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+						  sizeof(HEADER_802_11),    &ReassocHdr,
+						  2,                        &CapabilityInfo,
+						  2,                        &ListenIntv,
+						  MAC_ADDR_LEN,             ApAddr,
+						  1,                        &SsidIe,
+						  1,                        &pAd->MlmeAux.SsidLen,
+						  pAd->MlmeAux.SsidLen,     pAd->MlmeAux.Ssid,
+						  1,                        &SupRateIe,
+						  1,						&pAd->MlmeAux.SupRateLen,
+						  pAd->MlmeAux.SupRateLen,  pAd->MlmeAux.SupRate,
+						  END_OF_ARGS);
+
+		if (pAd->MlmeAux.ExtRateLen != 0)
+		{
+			MakeOutgoingFrame(pOutBuffer + FrameLen,        &tmp,
+							  1,                            &ExtRateIe,
+							  1,                            &pAd->MlmeAux.ExtRateLen,
+							  pAd->MlmeAux.ExtRateLen,	    pAd->MlmeAux.ExtRate,
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		if (pAd->MlmeAux.APEdcaParm.bValid)
+		{
+			if (pAd->CommonCfg.bAPSDCapable && pAd->MlmeAux.APEdcaParm.bAPSDCapable)
+			{
+				QBSS_STA_INFO_PARM QosInfo;
+
+				NdisZeroMemory(&QosInfo, sizeof(QBSS_STA_INFO_PARM));
+				QosInfo.UAPSD_AC_BE = pAd->CommonCfg.bAPSDAC_BE;
+				QosInfo.UAPSD_AC_BK = pAd->CommonCfg.bAPSDAC_BK;
+				QosInfo.UAPSD_AC_VI = pAd->CommonCfg.bAPSDAC_VI;
+				QosInfo.UAPSD_AC_VO = pAd->CommonCfg.bAPSDAC_VO;
+				QosInfo.MaxSPLength = pAd->CommonCfg.MaxSPLength;
+				WmeIe[8] |= *(PUCHAR)&QosInfo;
+			}
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen,    &tmp,
+							  9,                        &WmeIe[0],
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+#ifdef DOT11_N_SUPPORT
+		// HT
+		if ((pAd->MlmeAux.HtCapabilityLen > 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+		{
+			ULONG TmpLen;
+			UCHAR HtLen;
+			UCHAR BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+			if (pAd->StaActive.SupportedPhyInfo.bPreNHt == TRUE)
+			{
+				HtLen = SIZE_HT_CAP_IE + 4;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &WpaIe,
+							  1,                                &HtLen,
+							  4,                                &BROADCOM[0],
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+			}
+			else
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &HtCapIe,
+							  1,                                &pAd->MlmeAux.HtCapabilityLen,
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+			}
+			FrameLen += TmpLen;
+		}
+#endif // DOT11_N_SUPPORT //
+
+		// add Ralink proprietary IE to inform AP this STA is going to use AGGREGATION or PIGGY-BACK+AGGREGATION
+		// Case I: (Aggregation + Piggy-Back)
+		// 1. user enable aggregation, AND
+		// 2. Mac support piggy-back
+		// 3. AP annouces it's PIGGY-BACK+AGGREGATION-capable in BEACON
+		// Case II: (Aggregation)
+		// 1. user enable aggregation, AND
+		// 2. AP annouces it's AGGREGATION-capable in BEACON
+		if (pAd->CommonCfg.bAggregationCapable)
+		{
+			if ((pAd->CommonCfg.bPiggyBackCapable) && ((pAd->MlmeAux.APRalinkIe & 0x00000003) == 3))
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x03, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+			else if (pAd->MlmeAux.APRalinkIe & 0x00000001)
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x01, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+		}
+		else
+		{
+			ULONG TmpLen;
+			UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x04, 0x00, 0x00, 0x00};
+			MakeOutgoingFrame(pOutBuffer+FrameLen,		 &TmpLen,
+							  9,						 RalinkIe,
+							  END_OF_ARGS);
+			FrameLen += TmpLen;
+		}
+#ifdef LEAP_SUPPORT
+		if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+		{
+			CkipFlag = pAd->StaCfg.CkipFlag;	// We have update that at PeerBeaconAtJoinRequest()
+			if (CkipFlag != 0)
+			{
+				NdisZeroMemory(CkipNegotiationBuffer, CKIP_NEGOTIATION_LENGTH);
+				CkipNegotiationBuffer[2] = 0x66;
+				// Make it try KP & MIC, since we have to follow the result from AssocRsp
+				CkipNegotiationBuffer[8] = 0x18;
+				CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH - 1] = 0x22;
+
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &tmp,
+									1,                              &AironetCkipIe,
+									1,                              &AironetCkipLen,
+									AironetCkipLen,                 CkipNegotiationBuffer,
+									END_OF_ARGS);
+				FrameLen += tmp;
+			}
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+							1,                              &AironetIPAddressIE,
+							1,                              &AironetIPAddressLen,
+							AironetIPAddressLen,            AironetIPAddressBuffer,
+							END_OF_ARGS);
+			FrameLen += tmp;
+
+			//
+			// The RN is incremented before each reassociation request.
+			//
+			pAd->StaCfg.CCKMRN++;
+			//
+			// Calculate MIC = hmac-md5(krk, STA-ID|BSSID|RSNIE|TSF|RN);
+			//
+			COPY_MAC_ADDR(CalcMicBuffer, pAd->CurrentAddress);
+			CalcMicBufferLen = MAC_ADDR_LEN;
+			COPY_MAC_ADDR(CalcMicBuffer + CalcMicBufferLen, pAd->MlmeAux.Bssid);
+			CalcMicBufferLen += MAC_ADDR_LEN;
+			NdisMoveMemory(CalcMicBuffer + CalcMicBufferLen, CipherSuiteCiscoCCKM, CipherSuiteCiscoCCKMLen);
+			CalcMicBufferLen += CipherSuiteCiscoCCKMLen;
+			NdisMoveMemory(CalcMicBuffer + CalcMicBufferLen, (PUCHAR) &pAd->StaCfg.CCKMBeaconAtJoinTimeStamp, sizeof(pAd->StaCfg.CCKMBeaconAtJoinTimeStamp));
+			CalcMicBufferLen += sizeof(pAd->StaCfg.CCKMBeaconAtJoinTimeStamp);
+			NdisMoveMemory(CalcMicBuffer + CalcMicBufferLen, (PUCHAR)&pAd->StaCfg.CCKMRN, sizeof(pAd->StaCfg.CCKMRN));
+			CalcMicBufferLen += sizeof(pAd->StaCfg.CCKMRN);
+			hmac_md5(pAd->StaCfg.KRK, LEN_EAP_MICK, CalcMicBuffer, CalcMicBufferLen, MICMN);
+
+			//
+			// fill up CCKM reassociation request element
+			//
+			NdisMoveMemory(AironetCCKMReassocBuffer, AironetOUI, 4);
+			NdisMoveMemory(AironetCCKMReassocBuffer + 4, (PUCHAR)&pAd->StaCfg.CCKMBeaconAtJoinTimeStamp, 8);
+			NdisMoveMemory(AironetCCKMReassocBuffer + 12, (PUCHAR) &pAd->StaCfg.CCKMRN, 4);
+			NdisMoveMemory(AironetCCKMReassocBuffer +16, MICMN, 8);
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+							1,                      &AironetCCKMReassocIE,
+							1,                      &AironetCCKMReassocLen,
+							AironetCCKMReassocLen,  AironetCCKMReassocBuffer,
+							END_OF_ARGS);
+			FrameLen += tmp;
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+							CipherSuiteCiscoCCKMLen,CipherSuiteCiscoCCKM,
+							END_OF_ARGS);
+			FrameLen += tmp;
+		}
+#endif // LEAP_SUPPORT //
+
+		// Add CCX v2 request if CCX2 admin state is on
+		if (pAd->StaCfg.CCXControl.field.Enable == 1)
+		{
+			//
+			// Add CCX Version
+			//
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+						1,							&Ccx2Ie,
+						1,							&Ccx2Len,
+						Ccx2Len,				    Ccx2IeInfo,
+						END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+
+		RTMPSetTimer(&pAd->MlmeAux.ReassocTimer, Timeout); /* in mSec */
+		pAd->Mlme.AssocMachine.CurrState = REASSOC_WAIT_RSP;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeReassocReqAction() sanity check failed. BUG!!!! \n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		Upper layer issues disassoc request
+	Parameters:
+		Elem -
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeDisassocReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PMLME_DISASSOC_REQ_STRUCT pDisassocReq;
+	HEADER_802_11         DisassocHdr;
+	PHEADER_802_11        pDisassocHdr;
+	PUCHAR                pOutBuffer = NULL;
+	ULONG                 FrameLen = 0;
+	NDIS_STATUS           NStatus;
+	BOOLEAN               TimerCancelled;
+	ULONG                 Timeout = 0;
+	USHORT                Status;
+
+#ifdef QOS_DLS_SUPPORT
+	// send DLS-TEAR_DOWN message,
+	if (pAd->CommonCfg.bDLSCapable)
+	{
+		UCHAR i;
+
+		// tear down local dls table entry
+		for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+			{
+				RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			}
+		}
+
+		// tear down peer dls table entry
+		for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+			{
+				RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			}
+		}
+	}
+#endif // QOS_DLS_SUPPORT //
+
+	// skip sanity check
+	pDisassocReq = (PMLME_DISASSOC_REQ_STRUCT)(Elem->Msg);
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - MlmeDisassocReqAction() allocate memory failed\n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_FAIL_NO_RESOURCE;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DISASSOC_CONF, 2, &Status);
+		return;
+	}
+
+
+
+	RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer, &TimerCancelled);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Send DISASSOC request[BSSID::%02x:%02x:%02x:%02x:%02x:%02x (Reason=%d)\n",
+				pDisassocReq->Addr[0], pDisassocReq->Addr[1], pDisassocReq->Addr[2],
+				pDisassocReq->Addr[3], pDisassocReq->Addr[4], pDisassocReq->Addr[5], pDisassocReq->Reason));
+	MgtMacHeaderInit(pAd, &DisassocHdr, SUBTYPE_DISASSOC, 0, pDisassocReq->Addr, pDisassocReq->Addr);	// patch peap ttls switching issue
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+					  sizeof(HEADER_802_11),&DisassocHdr,
+					  2,                    &pDisassocReq->Reason,
+					  END_OF_ARGS);
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	// To patch Instance and Buffalo(N) AP
+	// Driver has to send deauth to Instance AP, but Buffalo(N) needs to send disassoc to reset Authenticator's state machine
+	// Therefore, we send both of them.
+	pDisassocHdr = (PHEADER_802_11)pOutBuffer;
+	pDisassocHdr->FC.SubType = SUBTYPE_DEAUTH;
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	pAd->StaCfg.DisassocReason = REASON_DISASSOC_STA_LEAVING;
+	COPY_MAC_ADDR(pAd->StaCfg.DisassocSta, pDisassocReq->Addr);
+
+	RTMPSetTimer(&pAd->MlmeAux.DisassocTimer, Timeout); /* in mSec */
+	pAd->Mlme.AssocMachine.CurrState = DISASSOC_WAIT_RSP;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+    if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+	{
+        union iwreq_data    wrqu;
+        //send disassociate event to wpa_supplicant
+        memset(&wrqu, 0, sizeof(wrqu));
+        wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+    }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+    {
+        union iwreq_data    wrqu;
+        memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+        wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+    }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+}
+
+/*
+	==========================================================================
+	Description:
+		peer sends assoc rsp back
+	Parameters:
+		Elme - MLME message containing the received frame
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerAssocRspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT        CapabilityInfo, Status, Aid;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES], SupRateLen;
+	UCHAR         ExtRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRateLen;
+	UCHAR         Addr2[MAC_ADDR_LEN];
+	BOOLEAN       TimerCancelled;
+	UCHAR         CkipFlag;
+	EDCA_PARM     EdcaParm;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+	if (PeerAssocRspSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &CapabilityInfo, &Status, &Aid, SupRate, &SupRateLen, ExtRate, &ExtRateLen,
+		&HtCapability,&AddHtInfo, &HtCapabilityLen,&AddHtInfoLen,&NewExtChannelOffset, &EdcaParm, &CkipFlag))
+	{
+		// The frame is for me ?
+		if(MAC_ADDR_EQUAL(Addr2, pAd->MlmeAux.Bssid))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspAction():ASSOC - receive ASSOC_RSP to me (status=%d)\n", Status));
+#ifdef DOT11_N_SUPPORT
+			DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspAction():MacTable [%d].AMsduSize = %d. ClientStatusFlags = 0x%lx \n",Elem->Wcid, pAd->MacTab.Content[BSSID_WCID].AMsduSize, pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+#endif // DOT11_N_SUPPORT //
+			RTMPCancelTimer(&pAd->MlmeAux.AssocTimer, &TimerCancelled);
+			if(Status == MLME_SUCCESS)
+			{
+				UCHAR			MaxSupportedRateIn500Kbps = 0;
+				UCHAR			idx;
+
+				// supported rates array may not be sorted. sort it and find the maximum rate
+			    for (idx=0; idx<SupRateLen; idx++)
+			    {
+			        if (MaxSupportedRateIn500Kbps < (SupRate[idx] & 0x7f))
+			            MaxSupportedRateIn500Kbps = SupRate[idx] & 0x7f;
+			    }
+
+				for (idx=0; idx<ExtRateLen; idx++)
+			    {
+			        if (MaxSupportedRateIn500Kbps < (ExtRate[idx] & 0x7f))
+			            MaxSupportedRateIn500Kbps = ExtRate[idx] & 0x7f;
+			    }
+				// go to procedure listed on page 376
+				AssocPostProc(pAd, Addr2, CapabilityInfo, Aid, SupRate, SupRateLen, ExtRate, ExtRateLen,
+					&EdcaParm, &HtCapability, HtCapabilityLen, &AddHtInfo);
+
+				StaAddMacTableEntry(pAd, &pAd->MacTab.Content[BSSID_WCID], MaxSupportedRateIn500Kbps, &HtCapability, HtCapabilityLen, CapabilityInfo);
+
+				pAd->StaCfg.CkipFlag = CkipFlag;
+				if (CkipFlag & 0x18)
+				{
+					NdisZeroMemory(pAd->StaCfg.TxSEQ, 4);
+					NdisZeroMemory(pAd->StaCfg.RxSEQ, 4);
+					NdisZeroMemory(pAd->StaCfg.CKIPMIC, 4);
+					pAd->StaCfg.GIV[0] = RandomByte(pAd);
+					pAd->StaCfg.GIV[1] = RandomByte(pAd);
+					pAd->StaCfg.GIV[2] = RandomByte(pAd);
+					pAd->StaCfg.bCkipOn = TRUE;
+					DBGPRINT(RT_DEBUG_TRACE, ("<CCX> pAd->StaCfg.CkipFlag = 0x%02x\n", pAd->StaCfg.CkipFlag));
+				}
+			}
+			else
+			{
+				// Faile on Association, we need to check the status code
+				// Is that a Rogue AP?
+#ifdef LEAP_SUPPORT
+				if ((pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP) && (Status == MLME_ALG_NOT_SUPPORT))
+				{ //Possibly Rogue AP
+					RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, pAd->MlmeAux.Bssid, LEAP_REASON_INVALID_AUTH);
+				}
+#endif // LEAP_SUPPORT //
+			}
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerAssocRspAction() sanity check fail\n"));
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		peer sends reassoc rsp
+	Parametrs:
+		Elem - MLME message cntaining the received frame
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerReassocRspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      CapabilityInfo;
+	USHORT      Status;
+	USHORT      Aid;
+	UCHAR       SupRate[MAX_LEN_OF_SUPPORTED_RATES], SupRateLen;
+	UCHAR       ExtRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRateLen;
+	UCHAR       Addr2[MAC_ADDR_LEN];
+	UCHAR       CkipFlag;
+	BOOLEAN     TimerCancelled;
+	EDCA_PARM   EdcaParm;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+	if(PeerAssocRspSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &CapabilityInfo, &Status, &Aid, SupRate, &SupRateLen, ExtRate, &ExtRateLen,
+								&HtCapability,	&AddHtInfo, &HtCapabilityLen, &AddHtInfoLen,&NewExtChannelOffset, &EdcaParm, &CkipFlag))
+	{
+		if(MAC_ADDR_EQUAL(Addr2, pAd->MlmeAux.Bssid)) // The frame is for me ?
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - receive REASSOC_RSP to me (status=%d)\n", Status));
+			RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer, &TimerCancelled);
+
+			if(Status == MLME_SUCCESS)
+			{
+				// go to procedure listed on page 376
+				AssocPostProc(pAd, Addr2, CapabilityInfo, Aid, SupRate, SupRateLen, ExtRate, ExtRateLen,
+					 &EdcaParm, &HtCapability, HtCapabilityLen, &AddHtInfo);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+                if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+                {
+                    union iwreq_data    wrqu;
+
+                    SendAssocIEsToWpaSupplicant(pAd);
+                    memset(&wrqu, 0, sizeof(wrqu));
+                    wrqu.data.flags = RT_ASSOC_EVENT_FLAG;
+                    wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+                }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+                {
+                    union iwreq_data    wrqu;
+                    wext_notify_event_assoc(pAd);
+
+                    memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                    memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+                    wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+                }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+			}
+
+			//
+			// Cisco Leap CCKM supported Re-association.
+			//
+#ifdef LEAP_SUPPORT
+			if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+			{
+				if (CCKMAssocRspSanity(pAd, Elem->Msg, Elem->MsgLen) == TRUE)
+				{
+					pAd->StaCfg.CkipFlag = CkipFlag;
+					if (CkipFlag & 0x18)
+					{
+						NdisZeroMemory(pAd->StaCfg.TxSEQ, 4);
+						NdisZeroMemory(pAd->StaCfg.RxSEQ, 4);
+						NdisZeroMemory(pAd->StaCfg.CKIPMIC, 4);
+						pAd->StaCfg.GIV[0] = RandomByte(pAd);
+						pAd->StaCfg.GIV[1] = RandomByte(pAd);
+						pAd->StaCfg.GIV[2] = RandomByte(pAd);
+						pAd->StaCfg.bCkipOn = TRUE;
+						DBGPRINT(RT_DEBUG_TRACE, ("<CCX> pAd->StaCfg.CkipFlag = 0x%02x\n", pAd->StaCfg.CkipFlag));
+					}
+
+					pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+					MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - CCKMAssocRspSanity() sanity check fail\n"));
+				}
+			}
+			else
+#endif // LEAP_SUPPORT //
+			{
+				// CkipFlag is no use for reassociate
+				pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+				MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+			}
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerReassocRspAction() sanity check fail\n"));
+	}
+
+}
+
+/*
+	==========================================================================
+	Description:
+		procedures on IEEE 802.11/1999 p.376
+	Parametrs:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AssocPostProc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pAddr2,
+	IN USHORT CapabilityInfo,
+	IN USHORT Aid,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN PEDCA_PARM pEdcaParm,
+	IN HT_CAPABILITY_IE		*pHtCapability,
+	IN UCHAR HtCapabilityLen,
+	IN ADD_HT_INFO_IE		*pAddHtInfo)	// AP might use this additional ht info IE
+{
+	ULONG Idx;
+
+	pAd->MlmeAux.BssType = BSS_INFRA;
+	COPY_MAC_ADDR(pAd->MlmeAux.Bssid, pAddr2);
+	pAd->MlmeAux.Aid = Aid;
+	pAd->MlmeAux.CapabilityInfo = CapabilityInfo & SUPPORTED_CAPABILITY_INFO;
+#ifdef DOT11_N_SUPPORT
+	// Some HT AP might lost WMM IE. We add WMM ourselves. beacuase HT requires QoS on.
+	if ((HtCapabilityLen > 0) && (pEdcaParm->bValid == FALSE))
+	{
+		pEdcaParm->bValid = TRUE;
+		pEdcaParm->Aifsn[0] = 3;
+		pEdcaParm->Aifsn[1] = 7;
+		pEdcaParm->Aifsn[2] = 2;
+		pEdcaParm->Aifsn[3] = 2;
+
+		pEdcaParm->Cwmin[0] = 4;
+		pEdcaParm->Cwmin[1] = 4;
+		pEdcaParm->Cwmin[2] = 3;
+		pEdcaParm->Cwmin[3] = 2;
+
+		pEdcaParm->Cwmax[0] = 10;
+		pEdcaParm->Cwmax[1] = 10;
+		pEdcaParm->Cwmax[2] = 4;
+		pEdcaParm->Cwmax[3] = 3;
+
+		pEdcaParm->Txop[0]  = 0;
+		pEdcaParm->Txop[1]  = 0;
+		pEdcaParm->Txop[2]  = 96;
+		pEdcaParm->Txop[3]  = 48;
+
+	}
+#endif // DOT11_N_SUPPORT //
+
+	NdisMoveMemory(&pAd->MlmeAux.APEdcaParm, pEdcaParm, sizeof(EDCA_PARM));
+
+	// filter out un-supported rates
+	pAd->MlmeAux.SupRateLen = SupRateLen;
+	NdisMoveMemory(pAd->MlmeAux.SupRate, SupRate, SupRateLen);
+	RTMPCheckRates(pAd, pAd->MlmeAux.SupRate, &pAd->MlmeAux.SupRateLen);
+
+	// filter out un-supported rates
+	pAd->MlmeAux.ExtRateLen = ExtRateLen;
+	NdisMoveMemory(pAd->MlmeAux.ExtRate, ExtRate, ExtRateLen);
+	RTMPCheckRates(pAd, pAd->MlmeAux.ExtRate, &pAd->MlmeAux.ExtRateLen);
+
+#ifdef DOT11_N_SUPPORT
+	if (HtCapabilityLen > 0)
+	{
+		RTMPCheckHt(pAd, BSSID_WCID, pHtCapability, pAddHtInfo);
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===>  AP.AMsduSize = %d. ClientStatusFlags = 0x%lx \n", pAd->MacTab.Content[BSSID_WCID].AMsduSize, pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===>    (Mmps=%d, AmsduSize=%d, )\n",
+		pAd->MacTab.Content[BSSID_WCID].MmpsMode, pAd->MacTab.Content[BSSID_WCID].AMsduSize));
+#endif // DOT11_N_SUPPORT //
+
+	// Set New WPA information
+	Idx = BssTableSearch(&pAd->ScanTab, pAddr2, pAd->MlmeAux.Channel);
+	if (Idx == BSS_NOT_FOUND)
+	{
+		DBGPRINT_ERR(("ASSOC - Can't find BSS after receiving Assoc response\n"));
+	}
+	else
+	{
+		// Init variable
+		pAd->MacTab.Content[BSSID_WCID].RSNIE_Len = 0;
+		NdisZeroMemory(pAd->MacTab.Content[BSSID_WCID].RSN_IE, MAX_LEN_OF_RSNIE);
+
+		// Store appropriate RSN_IE for WPA SM negotiation later
+		if ((pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) && (pAd->ScanTab.BssEntry[Idx].VarIELen != 0))
+		{
+			PUCHAR              pVIE;
+			USHORT              len;
+			PEID_STRUCT         pEid;
+
+			pVIE = pAd->ScanTab.BssEntry[Idx].VarIEs;
+			len	 = pAd->ScanTab.BssEntry[Idx].VarIELen;
+
+			while (len > 0)
+			{
+				pEid = (PEID_STRUCT) pVIE;
+				// For WPA/WPAPSK
+				if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4))
+					&& (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+				{
+					NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].RSN_IE, pVIE, (pEid->Len + 2));
+					pAd->MacTab.Content[BSSID_WCID].RSNIE_Len = (pEid->Len + 2);
+					DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===> Store RSN_IE for WPA SM negotiation \n"));
+				}
+				// For WPA2/WPA2PSK
+				else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3))
+					&& (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2 || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+				{
+					NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].RSN_IE, pVIE, (pEid->Len + 2));
+					pAd->MacTab.Content[BSSID_WCID].RSNIE_Len = (pEid->Len + 2);
+					DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===> Store RSN_IE for WPA2 SM negotiation \n"));
+				}
+
+				pVIE += (pEid->Len + 2);
+				len  -= (pEid->Len + 2);
+			}
+		}
+
+		if (pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == 0)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===> no RSN_IE \n"));
+		}
+		else
+		{
+			hex_dump("RSN_IE", pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		left part of IEEE 802.11/1999 p.374
+	Parameters:
+		Elem - MLME message containing the received frame
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerDisassocAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Addr2[MAC_ADDR_LEN];
+	USHORT        Reason;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerDisassocAction()\n"));
+	if(PeerDisassocSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Reason))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerDisassocAction() Reason = %d\n", Reason));
+		if (INFRA_ON(pAd) && MAC_ADDR_EQUAL(pAd->CommonCfg.Bssid, Addr2))
+		{
+
+			if (pAd->CommonCfg.bWirelessEvent)
+			{
+				RTMPSendWirelessEvent(pAd, IW_DISASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+			}
+
+
+#ifdef LEAP_SUPPORT
+			if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+			{
+				// Cisco_LEAP has start a timer
+				// We should cancel it if using LEAP
+				RTMPCancelTimer(&pAd->StaCfg.LeapAuthTimer, &TimerCancelled);
+				//Check is it mach the LEAP Authentication failed as possible a Rogue AP
+				//on it's PortSecured not equal to WPA_802_1X_PORT_SECURED while process the Association.
+				if ((pAd->Mlme.LeapMachine.CurrState != LEAP_IDLE) && (pAd->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED))
+				{
+					RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, Addr2, LEAP_REASON_AUTH_TIMEOUT);
+				}
+			}
+#endif	// LEAP_SUPPORT //
+			//
+			// Get Current System time and Turn on AdjacentAPReport
+			//
+			NdisGetSystemUpTime(&pAd->StaCfg.CCXAdjacentAPLinkDownTime);
+			pAd->StaCfg.CCXAdjacentAPReportFlag = TRUE;
+			LinkDown(pAd, TRUE);
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+			{
+                union iwreq_data    wrqu;
+                //send disassociate event to wpa_supplicant
+                memset(&wrqu, 0, sizeof(wrqu));
+                wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+                wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerDisassocAction() sanity check fail\n"));
+	}
+
+}
+
+/*
+	==========================================================================
+	Description:
+		what the state machine will do after assoc timeout
+	Parameters:
+		Elme -
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AssocTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - AssocTimeoutAction\n"));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_REJ_TIMEOUT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		what the state machine will do after reassoc timeout
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID ReassocTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - ReassocTimeoutAction\n"));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_REJ_TIMEOUT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		what the state machine will do after disassoc timeout
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID DisassocTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - DisassocTimeoutAction\n"));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_SUCCESS;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DISASSOC_CONF, 2, &Status);
+}
+
+VOID InvalidStateWhenAssoc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - InvalidStateWhenAssoc(state=%ld), reset ASSOC state machine\n",
+		pAd->Mlme.AssocMachine.CurrState));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+}
+
+VOID InvalidStateWhenReassoc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - InvalidStateWhenReassoc(state=%ld), reset ASSOC state machine\n",
+		pAd->Mlme.AssocMachine.CurrState));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+}
+
+VOID InvalidStateWhenDisassociate(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - InvalidStateWhenDisassoc(state=%ld), reset ASSOC state machine\n",
+		pAd->Mlme.AssocMachine.CurrState));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DISASSOC_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		right part of IEEE 802.11/1999 page 374
+	Note:
+		This event should never cause ASSOC state machine perform state
+		transition, and has no relationship with CNTL machine. So we separate
+		this routine as a service outside of ASSOC state transition table.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID Cls3errAction(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr)
+{
+	HEADER_802_11         DisassocHdr;
+	PHEADER_802_11        pDisassocHdr;
+	PUCHAR                pOutBuffer = NULL;
+	ULONG                 FrameLen = 0;
+	NDIS_STATUS           NStatus;
+	USHORT                Reason = REASON_CLS3ERR;
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Class 3 Error, Send DISASSOC frame\n"));
+	MgtMacHeaderInit(pAd, &DisassocHdr, SUBTYPE_DISASSOC, 0, pAddr, pAd->CommonCfg.Bssid);	// patch peap ttls switching issue
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+					  sizeof(HEADER_802_11),&DisassocHdr,
+					  2,                    &Reason,
+					  END_OF_ARGS);
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	// To patch Instance and Buffalo(N) AP
+	// Driver has to send deauth to Instance AP, but Buffalo(N) needs to send disassoc to reset Authenticator's state machine
+	// Therefore, we send both of them.
+	pDisassocHdr = (PHEADER_802_11)pOutBuffer;
+	pDisassocHdr->FC.SubType = SUBTYPE_DEAUTH;
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	pAd->StaCfg.DisassocReason = REASON_CLS3ERR;
+	COPY_MAC_ADDR(pAd->StaCfg.DisassocSta, pAddr);
+}
+
+ /*
+	 ==========================================================================
+	 Description:
+		 Switch between WEP and CKIP upon new association up.
+	 Parameters:
+
+	 IRQL = DISPATCH_LEVEL
+
+	 ==========================================================================
+  */
+VOID SwitchBetweenWepAndCkip(
+	IN PRTMP_ADAPTER pAd)
+{
+	int            i;
+	SHAREDKEY_MODE_STRUC  csr1;
+
+	// if KP is required. change the CipherAlg in hardware shard key table from WEP
+	// to CKIP. else remain as WEP
+	if (pAd->StaCfg.bCkipOn && (pAd->StaCfg.CkipFlag & 0x10))
+	{
+		// modify hardware key table so that MAC use correct algorithm to decrypt RX
+		RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE, &csr1.word);
+		if (csr1.field.Bss0Key0CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key0CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_CKIP128;
+
+		if (csr1.field.Bss0Key1CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key1CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_CKIP128;
+
+		if (csr1.field.Bss0Key2CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key2CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_CKIP128;
+
+		if (csr1.field.Bss0Key3CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key3CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_CKIP128;
+		RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE, csr1.word);
+		DBGPRINT(RT_DEBUG_TRACE, ("SwitchBetweenWepAndCkip: modify BSS0 cipher to %s\n", CipherName[csr1.field.Bss0Key0CipherAlg]));
+
+		// modify software key table so that driver can specify correct algorithm in TXD upon TX
+		for (i=0; i<SHARE_KEY_NUM; i++)
+		{
+			if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_WEP64)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_CKIP64;
+			else if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_WEP128)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_CKIP128;
+		}
+	}
+
+	// else if KP NOT inused. change the CipherAlg in hardware shard key table from CKIP
+	// to WEP.
+	else
+	{
+		// modify hardware key table so that MAC use correct algorithm to decrypt RX
+		RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE, &csr1.word);
+		if (csr1.field.Bss0Key0CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key0CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_WEP128;
+
+		if (csr1.field.Bss0Key1CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key1CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_WEP128;
+
+		if (csr1.field.Bss0Key2CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key2CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_WEP128;
+
+		if (csr1.field.Bss0Key3CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key3CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_WEP128;
+
+		// modify software key table so that driver can specify correct algorithm in TXD upon TX
+		for (i=0; i<SHARE_KEY_NUM; i++)
+		{
+			if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_CKIP64)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_WEP64;
+			else if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_CKIP128)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_WEP128;
+		}
+
+		//
+		// On WPA-NONE, must update CipherAlg.
+		// Because the OID_802_11_WEP_STATUS was been set after OID_802_11_ADD_KEY
+		// and CipherAlg will be CIPHER_NONE by Windows ZeroConfig.
+		// So we need to update CipherAlg after connect.
+		//
+		if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		{
+			for (i = 0; i < SHARE_KEY_NUM; i++)
+			{
+				if (pAd->SharedKey[BSS0][i].KeyLen != 0)
+				{
+					if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+					{
+						pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_TKIP;
+					}
+					else if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+					{
+						pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_AES;
+					}
+				}
+				else
+				{
+					pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_NONE;
+				}
+			}
+
+			csr1.field.Bss0Key0CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+			csr1.field.Bss0Key1CipherAlg = pAd->SharedKey[BSS0][1].CipherAlg;
+			csr1.field.Bss0Key2CipherAlg = pAd->SharedKey[BSS0][2].CipherAlg;
+			csr1.field.Bss0Key3CipherAlg = pAd->SharedKey[BSS0][3].CipherAlg;
+		}
+		RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE, csr1.word);
+		DBGPRINT(RT_DEBUG_TRACE, ("SwitchBetweenWepAndCkip: modify BSS0 cipher to %s\n", CipherName[csr1.field.Bss0Key0CipherAlg]));
+	}
+}
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+VOID    SendAssocIEsToWpaSupplicant(
+    IN  PRTMP_ADAPTER pAd)
+{
+    union iwreq_data    wrqu;
+    unsigned char custom[IW_CUSTOM_MAX] = {0};
+
+    if ((pAd->StaCfg.ReqVarIELen + 17) <= IW_CUSTOM_MAX)
+    {
+        sprintf(custom, "ASSOCINFO_ReqIEs=");
+	    NdisMoveMemory(custom+17, pAd->StaCfg.ReqVarIEs, pAd->StaCfg.ReqVarIELen);
+	    memset(&wrqu, 0, sizeof(wrqu));
+        wrqu.data.length = pAd->StaCfg.ReqVarIELen + 17;
+        wrqu.data.flags = RT_REQIE_EVENT_FLAG;
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, custom);
+
+        memset(&wrqu, 0, sizeof(wrqu));
+        wrqu.data.flags = RT_ASSOCINFO_EVENT_FLAG;
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+    }
+    else
+        DBGPRINT(RT_DEBUG_TRACE, ("pAd->StaCfg.ReqVarIELen + 17 > MAX_CUSTOM_LEN\n"));
+
+    return;
+}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+int wext_notify_event_assoc(
+	IN  RTMP_ADAPTER *pAd)
+{
+    union iwreq_data    wrqu;
+    char custom[IW_CUSTOM_MAX] = {0};
+
+#if WIRELESS_EXT > 17
+    if (pAd->StaCfg.ReqVarIELen <= IW_CUSTOM_MAX)
+    {
+        wrqu.data.length = pAd->StaCfg.ReqVarIELen;
+        memcpy(custom, pAd->StaCfg.ReqVarIEs, pAd->StaCfg.ReqVarIELen);
+        wireless_send_event(pAd->net_dev, IWEVASSOCREQIE, &wrqu, custom);
+    }
+    else
+        DBGPRINT(RT_DEBUG_TRACE, ("pAd->StaCfg.ReqVarIELen > MAX_CUSTOM_LEN\n"));
+#else
+    if (((pAd->StaCfg.ReqVarIELen*2) + 17) <= IW_CUSTOM_MAX)
+    {
+        UCHAR   idx;
+        wrqu.data.length = (pAd->StaCfg.ReqVarIELen*2) + 17;
+        sprintf(custom, "ASSOCINFO(ReqIEs=");
+        for (idx=0; idx<pAd->StaCfg.ReqVarIELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAd->StaCfg.ReqVarIEs[idx]);
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, custom);
+    }
+    else
+        DBGPRINT(RT_DEBUG_TRACE, ("(pAd->StaCfg.ReqVarIELen*2) + 17 > MAX_CUSTOM_LEN\n"));
+#endif
+
+	return 0;
+
+}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+
+BOOLEAN StaAddMacTableEntry(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PMAC_TABLE_ENTRY	pEntry,
+	IN  UCHAR				MaxSupportedRateIn500Kbps,
+	IN  HT_CAPABILITY_IE	*pHtCapability,
+	IN  UCHAR				HtCapabilityLen,
+	IN  USHORT        		CapabilityInfo)
+{
+	UCHAR            MaxSupportedRate = RATE_11;
+
+	if (ADHOC_ON(pAd))
+		CLIENT_STATUS_CLEAR_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+
+	switch (MaxSupportedRateIn500Kbps)
+    {
+        case 108: MaxSupportedRate = RATE_54;   break;
+        case 96:  MaxSupportedRate = RATE_48;   break;
+        case 72:  MaxSupportedRate = RATE_36;   break;
+        case 48:  MaxSupportedRate = RATE_24;   break;
+        case 36:  MaxSupportedRate = RATE_18;   break;
+        case 24:  MaxSupportedRate = RATE_12;   break;
+        case 18:  MaxSupportedRate = RATE_9;    break;
+        case 12:  MaxSupportedRate = RATE_6;    break;
+        case 22:  MaxSupportedRate = RATE_11;   break;
+        case 11:  MaxSupportedRate = RATE_5_5;  break;
+        case 4:   MaxSupportedRate = RATE_2;    break;
+        case 2:   MaxSupportedRate = RATE_1;    break;
+        default:  MaxSupportedRate = RATE_11;   break;
+    }
+
+    if ((pAd->CommonCfg.PhyMode == PHY_11G) && (MaxSupportedRate < RATE_FIRST_OFDM_RATE))
+        return FALSE;
+
+#ifdef DOT11_N_SUPPORT
+	// 11n only
+	if (((pAd->CommonCfg.PhyMode == PHY_11N_2_4G) || (pAd->CommonCfg.PhyMode == PHY_11N_5G))&& (HtCapabilityLen == 0))
+		return FALSE;
+#endif // DOT11_N_SUPPORT //
+
+	if (!pEntry)
+        return FALSE;
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+	if (pEntry)
+	{
+		pEntry->PortSecured = WPA_802_1X_PORT_SECURED;
+		if ((MaxSupportedRate < RATE_FIRST_OFDM_RATE) ||
+			(pAd->CommonCfg.PhyMode == PHY_11B))
+		{
+			pEntry->RateLen = 4;
+			if (MaxSupportedRate >= RATE_FIRST_OFDM_RATE)
+				MaxSupportedRate = RATE_11;
+		}
+		else
+			pEntry->RateLen = 12;
+
+		pEntry->MaxHTPhyMode.word = 0;
+		pEntry->MinHTPhyMode.word = 0;
+		pEntry->HTPhyMode.word = 0;
+		pEntry->MaxSupportedRate = MaxSupportedRate;
+		if (pEntry->MaxSupportedRate < RATE_FIRST_OFDM_RATE)
+		{
+			pEntry->MaxHTPhyMode.field.MODE = MODE_CCK;
+			pEntry->MaxHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+			pEntry->MinHTPhyMode.field.MODE = MODE_CCK;
+			pEntry->MinHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+			pEntry->HTPhyMode.field.MODE = MODE_CCK;
+			pEntry->HTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+		}
+		else
+		{
+			pEntry->MaxHTPhyMode.field.MODE = MODE_OFDM;
+			pEntry->MaxHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+			pEntry->MinHTPhyMode.field.MODE = MODE_OFDM;
+			pEntry->MinHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+			pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+			pEntry->HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+		}
+		pEntry->CapabilityInfo = CapabilityInfo;
+		CLIENT_STATUS_CLEAR_FLAG(pEntry, fCLIENT_STATUS_AGGREGATION_CAPABLE);
+		CLIENT_STATUS_CLEAR_FLAG(pEntry, fCLIENT_STATUS_PIGGYBACK_CAPABLE);
+	}
+
+#ifdef DOT11_N_SUPPORT
+	// If this Entry supports 802.11n, upgrade to HT rate.
+	if ((HtCapabilityLen != 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+	{
+		UCHAR	j, bitmask; //k,bitmask;
+		CHAR    i;
+
+		if (ADHOC_ON(pAd))
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+		if ((pHtCapability->HtCapInfo.GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+		{
+			pEntry->MaxHTPhyMode.field.MODE = MODE_HTGREENFIELD;
+		}
+		else
+		{
+			pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+			pAd->MacTab.fAnyStationNonGF = TRUE;
+			pAd->CommonCfg.AddHTInfo.AddHtInfo2.NonGfPresent = 1;
+		}
+
+		if ((pHtCapability->HtCapInfo.ChannelWidth) && (pAd->CommonCfg.DesiredHtPhy.ChannelWidth))
+		{
+			pEntry->MaxHTPhyMode.field.BW= BW_40;
+			pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor40)&(pHtCapability->HtCapInfo.ShortGIfor40));
+		}
+		else
+		{
+			pEntry->MaxHTPhyMode.field.BW = BW_20;
+			pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor20)&(pHtCapability->HtCapInfo.ShortGIfor20));
+			pAd->MacTab.fAnyStation20Only = TRUE;
+		}
+
+		// 3*3
+		if (pAd->MACVersion >= RALINK_2883_VERSION && pAd->MACVersion < RALINK_3070_VERSION)
+			pEntry->MaxHTPhyMode.field.TxBF = pAd->CommonCfg.RegTransmitSetting.field.TxBF;
+
+		// find max fixed rate
+		for (i=23; i>=0; i--) // 3*3
+		{
+			j = i/8;
+			bitmask = (1<<(i-(j*8)));
+			if ((pAd->StaCfg.DesiredHtPhyInfo.MCSSet[j] & bitmask) && (pHtCapability->MCSSet[j] & bitmask))
+			{
+				pEntry->MaxHTPhyMode.field.MCS = i;
+				break;
+			}
+			if (i==0)
+				break;
+		}
+
+
+		if (pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO)
+		{
+			if (pAd->StaCfg.DesiredTransmitSetting.field.MCS == 32)
+			{
+				// Fix MCS as HT Duplicated Mode
+				pEntry->MaxHTPhyMode.field.BW = 1;
+				pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+				pEntry->MaxHTPhyMode.field.STBC = 0;
+				pEntry->MaxHTPhyMode.field.ShortGI = 0;
+				pEntry->MaxHTPhyMode.field.MCS = 32;
+			}
+			else if (pEntry->MaxHTPhyMode.field.MCS > pAd->StaCfg.HTPhyMode.field.MCS)
+			{
+				// STA supports fixed MCS
+				pEntry->MaxHTPhyMode.field.MCS = pAd->StaCfg.HTPhyMode.field.MCS;
+			}
+		}
+
+		pEntry->MaxHTPhyMode.field.STBC = (pHtCapability->HtCapInfo.RxSTBC & (pAd->CommonCfg.DesiredHtPhy.TxSTBC));
+		pEntry->MpduDensity = pHtCapability->HtCapParm.MpduDensity;
+		pEntry->MaxRAmpduFactor = pHtCapability->HtCapParm.MaxRAmpduFactor;
+		pEntry->MmpsMode = (UCHAR)pHtCapability->HtCapInfo.MimoPs;
+		pEntry->AMsduSize = (UCHAR)pHtCapability->HtCapInfo.AMsduSize;
+		pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+
+		if (pAd->CommonCfg.DesiredHtPhy.AmsduEnable && (pAd->CommonCfg.REGBACapability.field.AutoBA == FALSE))
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_AMSDU_INUSED);
+		if (pHtCapability->HtCapInfo.ShortGIfor20)
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE);
+		if (pHtCapability->HtCapInfo.ShortGIfor40)
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE);
+		if (pHtCapability->HtCapInfo.TxSTBC)
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_TxSTBC_CAPABLE);
+		if (pHtCapability->HtCapInfo.RxSTBC)
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RxSTBC_CAPABLE);
+		if (pHtCapability->ExtHtCapInfo.PlusHTC)
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_HTC_CAPABLE);
+		if (pAd->CommonCfg.bRdg && pHtCapability->ExtHtCapInfo.RDGSupport)
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RDG_CAPABLE);
+		if (pHtCapability->ExtHtCapInfo.MCSFeedback == 0x03)
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_MCSFEEDBACK_CAPABLE);
+	}
+	else
+	{
+		pAd->MacTab.fAnyStationIsLegacy = TRUE;
+	}
+
+	NdisMoveMemory(&pEntry->HTCapability, pHtCapability, sizeof(HT_CAPABILITY_IE));
+#endif // DOT11_N_SUPPORT //
+
+	pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+	pEntry->CurrTxRate = pEntry->MaxSupportedRate;
+
+	// Set asic auto fall back
+	if (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+	{
+		PUCHAR					pTable;
+		UCHAR					TableSize = 0;
+
+		MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &pEntry->CurrTxRateIndex);
+		pEntry->bAutoTxRateSwitch = TRUE;
+	}
+	else
+	{
+		pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+		pEntry->HTPhyMode.field.MCS	= pAd->StaCfg.HTPhyMode.field.MCS;
+		pEntry->bAutoTxRateSwitch = FALSE;
+
+		// If the legacy mode is set, overwrite the transmit setting of this entry.
+		RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+	}
+
+	pEntry->PortSecured = WPA_802_1X_PORT_SECURED;
+	pEntry->Sst = SST_ASSOC;
+	pEntry->AuthState = AS_AUTH_OPEN;
+	pEntry->AuthMode = pAd->StaCfg.AuthMode;
+	pEntry->WepStatus = pAd->StaCfg.WepStatus;
+
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+    if (pAd->StaCfg.WpaSupplicantUP)
+    {
+        union iwreq_data    wrqu;
+
+        SendAssocIEsToWpaSupplicant(pAd);
+        memset(&wrqu, 0, sizeof(wrqu));
+        wrqu.data.flags = RT_ASSOC_EVENT_FLAG;
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+    }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+    {
+        union iwreq_data    wrqu;
+        wext_notify_event_assoc(pAd);
+
+        memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+        memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+        wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+    }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+	return TRUE;
+}
+
+
diff --git a/drivers/staging/rt2870/sta/auth.c b/drivers/staging/rt2870/sta/auth.c
new file mode 100644
index 000000000000..73fb8d6ea76f
--- /dev/null
+++ b/drivers/staging/rt2870/sta/auth.c
@@ -0,0 +1,474 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	auth.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John		2004-9-3		porting from RT2500
+*/
+#include "../rt_config.h"
+
+/*
+    ==========================================================================
+    Description:
+        authenticate state machine init, including state transition and timer init
+    Parameters:
+        Sm - pointer to the auth state machine
+    Note:
+        The state machine looks like this
+
+                        AUTH_REQ_IDLE           AUTH_WAIT_SEQ2                   AUTH_WAIT_SEQ4
+    MT2_MLME_AUTH_REQ   mlme_auth_req_action    invalid_state_when_auth          invalid_state_when_auth
+    MT2_PEER_AUTH_EVEN  drop                    peer_auth_even_at_seq2_action    peer_auth_even_at_seq4_action
+    MT2_AUTH_TIMEOUT    Drop                    auth_timeout_action              auth_timeout_action
+
+	IRQL = PASSIVE_LEVEL
+
+    ==========================================================================
+ */
+
+void AuthStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[])
+{
+    StateMachineInit(Sm, Trans, MAX_AUTH_STATE, MAX_AUTH_MSG, (STATE_MACHINE_FUNC)Drop, AUTH_REQ_IDLE, AUTH_MACHINE_BASE);
+
+    // the first column
+    StateMachineSetAction(Sm, AUTH_REQ_IDLE, MT2_MLME_AUTH_REQ, (STATE_MACHINE_FUNC)MlmeAuthReqAction);
+
+    // the second column
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ2, MT2_MLME_AUTH_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAuth);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ2, MT2_PEER_AUTH_EVEN, (STATE_MACHINE_FUNC)PeerAuthRspAtSeq2Action);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ2, MT2_AUTH_TIMEOUT, (STATE_MACHINE_FUNC)AuthTimeoutAction);
+
+    // the third column
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ4, MT2_MLME_AUTH_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAuth);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ4, MT2_PEER_AUTH_EVEN, (STATE_MACHINE_FUNC)PeerAuthRspAtSeq4Action);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ4, MT2_AUTH_TIMEOUT, (STATE_MACHINE_FUNC)AuthTimeoutAction);
+
+	RTMPInitTimer(pAd, &pAd->MlmeAux.AuthTimer, GET_TIMER_FUNCTION(AuthTimeout), pAd, FALSE);
+}
+
+/*
+    ==========================================================================
+    Description:
+        function to be executed at timer thread when auth timer expires
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID AuthTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+    RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+    DBGPRINT(RT_DEBUG_TRACE,("AUTH - AuthTimeout\n"));
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	// send a de-auth to reset AP's state machine (Patch AP-Dir635)
+	if (pAd->Mlme.AuthMachine.CurrState == AUTH_WAIT_SEQ2)
+		Cls2errAction(pAd, pAd->MlmeAux.Bssid);
+
+
+    MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_AUTH_TIMEOUT, 0, NULL);
+    RT28XX_MLME_HANDLER(pAd);
+}
+
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeAuthReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    UCHAR              Addr[6];
+    USHORT             Alg, Seq, Status;
+    ULONG              Timeout;
+    HEADER_802_11      AuthHdr;
+    BOOLEAN            TimerCancelled;
+    NDIS_STATUS        NStatus;
+    PUCHAR             pOutBuffer = NULL;
+    ULONG              FrameLen = 0;
+
+	// Block all authentication request durning WPA block period
+	if (pAd->StaCfg.bBlockAssoc == TRUE)
+	{
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Block Auth request durning WPA block period!\n"));
+        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+        Status = MLME_STATE_MACHINE_REJECT;
+        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+	}
+    else if(MlmeAuthReqSanity(pAd, Elem->Msg, Elem->MsgLen, Addr, &Timeout, &Alg))
+    {
+        // reset timer
+        RTMPCancelTimer(&pAd->MlmeAux.AuthTimer, &TimerCancelled);
+        COPY_MAC_ADDR(pAd->MlmeAux.Bssid, Addr);
+        pAd->MlmeAux.Alg  = Alg;
+        Seq = 1;
+        Status = MLME_SUCCESS;
+
+        NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+        if(NStatus != NDIS_STATUS_SUCCESS)
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("AUTH - MlmeAuthReqAction(Alg:%d) allocate memory failed\n", Alg));
+            pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+            Status = MLME_FAIL_NO_RESOURCE;
+            MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+            return;
+        }
+
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Send AUTH request seq#1 (Alg=%d)...\n", Alg));
+        MgtMacHeaderInit(pAd, &AuthHdr, SUBTYPE_AUTH, 0, Addr, pAd->MlmeAux.Bssid);
+        MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+                          sizeof(HEADER_802_11),&AuthHdr,
+                          2,                    &Alg,
+                          2,                    &Seq,
+                          2,                    &Status,
+                          END_OF_ARGS);
+        MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+    	MlmeFreeMemory(pAd, pOutBuffer);
+
+        RTMPSetTimer(&pAd->MlmeAux.AuthTimer, Timeout);
+        pAd->Mlme.AuthMachine.CurrState = AUTH_WAIT_SEQ2;
+    }
+    else
+    {
+        DBGPRINT_ERR(("AUTH - MlmeAuthReqAction() sanity check failed\n"));
+        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+        Status = MLME_INVALID_FORMAT;
+        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerAuthRspAtSeq2Action(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    UCHAR         Addr2[MAC_ADDR_LEN];
+    USHORT        Seq, Status, RemoteStatus, Alg;
+    UCHAR         ChlgText[CIPHER_TEXT_LEN];
+    UCHAR         CyperChlgText[CIPHER_TEXT_LEN + 8 + 8];
+    UCHAR         Element[2];
+    HEADER_802_11 AuthHdr;
+    BOOLEAN       TimerCancelled;
+    PUCHAR        pOutBuffer = NULL;
+    NDIS_STATUS   NStatus;
+    ULONG         FrameLen = 0;
+    USHORT        Status2;
+
+    if (PeerAuthSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Alg, &Seq, &Status, ChlgText))
+    {
+        if (MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, Addr2) && Seq == 2)
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Receive AUTH_RSP seq#2 to me (Alg=%d, Status=%d)\n", Alg, Status));
+            RTMPCancelTimer(&pAd->MlmeAux.AuthTimer, &TimerCancelled);
+
+            if (Status == MLME_SUCCESS)
+            {
+                // Authentication Mode "LEAP" has allow for CCX 1.X
+                if ((pAd->MlmeAux.Alg == Ndis802_11AuthModeOpen)
+#ifdef LEAP_SUPPORT
+					|| (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+				)
+                {
+                    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+#ifdef LEAP_SUPPORT
+                    pAd->Mlme.LeapMachine.CurrState = LEAP_IDLE;
+#endif // LEAP_SUPPORT //
+                    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+                }
+                else
+                {
+                    // 2. shared key, need to be challenged
+                    Seq++;
+                    RemoteStatus = MLME_SUCCESS;
+
+					// Get an unused nonpaged memory
+                    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+                    if(NStatus != NDIS_STATUS_SUCCESS)
+                    {
+                        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - PeerAuthRspAtSeq2Action() allocate memory fail\n"));
+                        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+                        Status2 = MLME_FAIL_NO_RESOURCE;
+                        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status2);
+                        return;
+                    }
+
+                    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Send AUTH request seq#3...\n"));
+                    MgtMacHeaderInit(pAd, &AuthHdr, SUBTYPE_AUTH, 0, Addr2, pAd->MlmeAux.Bssid);
+                    AuthHdr.FC.Wep = 1;
+                    // Encrypt challenge text & auth information
+                    RTMPInitWepEngine(
+                    	pAd,
+                    	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+                    	pAd->StaCfg.DefaultKeyId,
+                    	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen,
+                    	CyperChlgText);
+
+					Alg = cpu2le16(*(USHORT *)&Alg);
+					Seq = cpu2le16(*(USHORT *)&Seq);
+					RemoteStatus= cpu2le16(*(USHORT *)&RemoteStatus);
+
+					RTMPEncryptData(pAd, (PUCHAR) &Alg, CyperChlgText + 4, 2);
+					RTMPEncryptData(pAd, (PUCHAR) &Seq, CyperChlgText + 6, 2);
+					RTMPEncryptData(pAd, (PUCHAR) &RemoteStatus, CyperChlgText + 8, 2);
+					Element[0] = 16;
+					Element[1] = 128;
+					RTMPEncryptData(pAd, Element, CyperChlgText + 10, 2);
+					RTMPEncryptData(pAd, ChlgText, CyperChlgText + 12, 128);
+					RTMPSetICV(pAd, CyperChlgText + 140);
+                    MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+                                      sizeof(HEADER_802_11),    &AuthHdr,
+                                      CIPHER_TEXT_LEN + 16,     CyperChlgText,
+                                      END_OF_ARGS);
+                    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+                	MlmeFreeMemory(pAd, pOutBuffer);
+
+                    RTMPSetTimer(&pAd->MlmeAux.AuthTimer, AUTH_TIMEOUT);
+                    pAd->Mlme.AuthMachine.CurrState = AUTH_WAIT_SEQ4;
+                }
+            }
+            else
+            {
+#ifdef LEAP_SUPPORT
+                if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+                {
+                    //Invalid Authentication possible rogue AP
+                    //Add this Ap to Rogue AP.
+                    RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, Addr2, LEAP_REASON_INVALID_AUTH);
+				}
+#endif // LEAP_SUPPORT //
+                pAd->StaCfg.AuthFailReason = Status;
+                COPY_MAC_ADDR(pAd->StaCfg.AuthFailSta, Addr2);
+                pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+                MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+            }
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - PeerAuthSanity() sanity check fail\n"));
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerAuthRspAtSeq4Action(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    UCHAR         Addr2[MAC_ADDR_LEN];
+    USHORT        Alg, Seq, Status;
+    CHAR          ChlgText[CIPHER_TEXT_LEN];
+    BOOLEAN       TimerCancelled;
+
+    if(PeerAuthSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Alg, &Seq, &Status, ChlgText))
+    {
+        if(MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, Addr2) && Seq == 4)
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Receive AUTH_RSP seq#4 to me\n"));
+            RTMPCancelTimer(&pAd->MlmeAux.AuthTimer, &TimerCancelled);
+
+            if (Status != MLME_SUCCESS)
+            {
+                pAd->StaCfg.AuthFailReason = Status;
+                COPY_MAC_ADDR(pAd->StaCfg.AuthFailSta, Addr2);
+            }
+
+            pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+            MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - PeerAuthRspAtSeq4Action() sanity check fail\n"));
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDeauthReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    MLME_DEAUTH_REQ_STRUCT *pInfo;
+    HEADER_802_11 DeauthHdr;
+    PUCHAR        pOutBuffer = NULL;
+    NDIS_STATUS   NStatus;
+    ULONG         FrameLen = 0;
+    USHORT        Status;
+
+    pInfo = (MLME_DEAUTH_REQ_STRUCT *)Elem->Msg;
+
+    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+    if (NStatus != NDIS_STATUS_SUCCESS)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - MlmeDeauthReqAction() allocate memory fail\n"));
+        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+        Status = MLME_FAIL_NO_RESOURCE;
+        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DEAUTH_CONF, 2, &Status);
+        return;
+    }
+
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Send DE-AUTH request (Reason=%d)...\n", pInfo->Reason));
+    MgtMacHeaderInit(pAd, &DeauthHdr, SUBTYPE_DEAUTH, 0, pInfo->Addr, pAd->MlmeAux.Bssid);
+    MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+                      sizeof(HEADER_802_11),&DeauthHdr,
+                      2,                    &pInfo->Reason,
+                      END_OF_ARGS);
+    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+    pAd->StaCfg.DeauthReason = pInfo->Reason;
+    COPY_MAC_ADDR(pAd->StaCfg.DeauthSta, pInfo->Addr);
+    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+    Status = MLME_SUCCESS;
+    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DEAUTH_CONF, 2, &Status);
+
+	// send wireless event - for deauthentication
+	if (pAd->CommonCfg.bWirelessEvent)
+		RTMPSendWirelessEvent(pAd, IW_DEAUTH_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID AuthTimeoutAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    USHORT Status;
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - AuthTimeoutAction\n"));
+    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+    Status = MLME_REJ_TIMEOUT;
+    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID InvalidStateWhenAuth(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    USHORT Status;
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - InvalidStateWhenAuth (state=%ld), reset AUTH state machine\n", pAd->Mlme.AuthMachine.CurrState));
+    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+    Status = MLME_STATE_MACHINE_REJECT;
+    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+}
+
+/*
+    ==========================================================================
+    Description:
+        Some STA/AP
+    Note:
+        This action should never trigger AUTH state transition, therefore we
+        separate it from AUTH state machine, and make it as a standalone service
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID Cls2errAction(
+    IN PRTMP_ADAPTER pAd,
+    IN PUCHAR pAddr)
+{
+    HEADER_802_11 DeauthHdr;
+    PUCHAR        pOutBuffer = NULL;
+    NDIS_STATUS   NStatus;
+    ULONG         FrameLen = 0;
+    USHORT        Reason = REASON_CLS2ERR;
+
+    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+    if (NStatus != NDIS_STATUS_SUCCESS)
+        return;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Class 2 error, Send DEAUTH frame...\n"));
+    MgtMacHeaderInit(pAd, &DeauthHdr, SUBTYPE_DEAUTH, 0, pAddr, pAd->MlmeAux.Bssid);
+    MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+                      sizeof(HEADER_802_11),&DeauthHdr,
+                      2,                    &Reason,
+                      END_OF_ARGS);
+    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+    pAd->StaCfg.DeauthReason = Reason;
+    COPY_MAC_ADDR(pAd->StaCfg.DeauthSta, pAddr);
+}
+
+
diff --git a/drivers/staging/rt2870/sta/auth_rsp.c b/drivers/staging/rt2870/sta/auth_rsp.c
new file mode 100644
index 000000000000..6e3c2d24cdad
--- /dev/null
+++ b/drivers/staging/rt2870/sta/auth_rsp.c
@@ -0,0 +1,166 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	auth_rsp.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John		2004-10-1		copy from RT2560
+*/
+#include "../rt_config.h"
+
+/*
+    ==========================================================================
+    Description:
+        authentication state machine init procedure
+    Parameters:
+        Sm - the state machine
+
+	IRQL = PASSIVE_LEVEL
+
+    ==========================================================================
+ */
+VOID AuthRspStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN PSTATE_MACHINE Sm,
+    IN STATE_MACHINE_FUNC Trans[])
+{
+    StateMachineInit(Sm, Trans, MAX_AUTH_RSP_STATE, MAX_AUTH_RSP_MSG, (STATE_MACHINE_FUNC)Drop, AUTH_RSP_IDLE, AUTH_RSP_MACHINE_BASE);
+
+    // column 1
+    StateMachineSetAction(Sm, AUTH_RSP_IDLE, MT2_PEER_DEAUTH, (STATE_MACHINE_FUNC)PeerDeauthAction);
+
+    // column 2
+    StateMachineSetAction(Sm, AUTH_RSP_WAIT_CHAL, MT2_PEER_DEAUTH, (STATE_MACHINE_FUNC)PeerDeauthAction);
+
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+*/
+VOID PeerAuthSimpleRspGenAndSend(
+    IN PRTMP_ADAPTER pAd,
+    IN PHEADER_802_11 pHdr80211,
+    IN USHORT Alg,
+    IN USHORT Seq,
+    IN USHORT Reason,
+    IN USHORT Status)
+{
+    HEADER_802_11     AuthHdr;
+    ULONG             FrameLen = 0;
+    PUCHAR            pOutBuffer = NULL;
+    NDIS_STATUS       NStatus;
+
+    if (Reason != MLME_SUCCESS)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("Peer AUTH fail...\n"));
+        return;
+    }
+
+	//Get an unused nonpaged memory
+    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+    if (NStatus != NDIS_STATUS_SUCCESS)
+        return;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Send AUTH response (seq#2)...\n"));
+    MgtMacHeaderInit(pAd, &AuthHdr, SUBTYPE_AUTH, 0, pHdr80211->Addr2, pAd->MlmeAux.Bssid);
+    MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+                      sizeof(HEADER_802_11),    &AuthHdr,
+                      2,                        &Alg,
+                      2,                        &Seq,
+                      2,                        &Reason,
+                      END_OF_ARGS);
+    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+*/
+VOID PeerDeauthAction(
+    IN PRTMP_ADAPTER pAd,
+    IN PMLME_QUEUE_ELEM Elem)
+{
+    UCHAR       Addr2[MAC_ADDR_LEN];
+    USHORT      Reason;
+
+    if (PeerDeauthSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Reason))
+    {
+        if (INFRA_ON(pAd) && MAC_ADDR_EQUAL(Addr2, pAd->CommonCfg.Bssid))
+        {
+            DBGPRINT(RT_DEBUG_TRACE,("AUTH_RSP - receive DE-AUTH from our AP (Reason=%d)\n", Reason));
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+
+			// send wireless event - for deauthentication
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_DEAUTH_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+            LinkDown(pAd, TRUE);
+
+            // Authentication Mode Cisco_LEAP has start a timer
+            // We should cancel it if using LEAP
+#ifdef LEAP_SUPPORT
+            if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+            {
+                RTMPCancelTimer(&pAd->StaCfg.LeapAuthTimer, &TimerCancelled);
+                //Check is it mach the LEAP Authentication failed as possible a Rogue AP
+                //on it's PortSecured not equal to WPA_802_1X_PORT_SECURED while process the Authenticaton.
+                if ((pAd->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED) && (pAd->Mlme.LeapMachine.CurrState != LEAP_IDLE))
+                {
+                    RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, Addr2, LEAP_REASON_AUTH_TIMEOUT);
+                }
+            }
+#endif // LEAP_SUPPORT //
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE,("AUTH_RSP - PeerDeauthAction() sanity check fail\n"));
+    }
+}
+
diff --git a/drivers/staging/rt2870/sta/connect.c b/drivers/staging/rt2870/sta/connect.c
new file mode 100644
index 000000000000..c93140a8caa3
--- /dev/null
+++ b/drivers/staging/rt2870/sta/connect.c
@@ -0,0 +1,2822 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	connect.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John			2004-08-08			Major modification from RT2560
+*/
+#include "../rt_config.h"
+
+UCHAR	CipherSuiteWpaNoneTkip[] = {
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x00	// authentication
+		};
+UCHAR	CipherSuiteWpaNoneTkipLen = (sizeof(CipherSuiteWpaNoneTkip) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteWpaNoneAes[] = {
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x04,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x04,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x00	// authentication
+		};
+UCHAR	CipherSuiteWpaNoneAesLen = (sizeof(CipherSuiteWpaNoneAes) / sizeof(UCHAR));
+
+// The following MACRO is called after 1. starting an new IBSS, 2. succesfully JOIN an IBSS,
+// or 3. succesfully ASSOCIATE to a BSS, 4. successfully RE_ASSOCIATE to a BSS
+// All settings successfuly negotiated furing MLME state machines become final settings
+// and are copied to pAd->StaActive
+#define COPY_SETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(_pAd)                                 \
+{                                                                                       \
+	(_pAd)->CommonCfg.SsidLen = (_pAd)->MlmeAux.SsidLen;                                \
+	NdisMoveMemory((_pAd)->CommonCfg.Ssid, (_pAd)->MlmeAux.Ssid, (_pAd)->MlmeAux.SsidLen); \
+	COPY_MAC_ADDR((_pAd)->CommonCfg.Bssid, (_pAd)->MlmeAux.Bssid);                      \
+	(_pAd)->CommonCfg.Channel = (_pAd)->MlmeAux.Channel;                                \
+	(_pAd)->CommonCfg.CentralChannel = (_pAd)->MlmeAux.CentralChannel;                  \
+	(_pAd)->StaActive.Aid = (_pAd)->MlmeAux.Aid;                                        \
+	(_pAd)->StaActive.AtimWin = (_pAd)->MlmeAux.AtimWin;                                \
+	(_pAd)->StaActive.CapabilityInfo = (_pAd)->MlmeAux.CapabilityInfo;                  \
+	(_pAd)->CommonCfg.BeaconPeriod = (_pAd)->MlmeAux.BeaconPeriod;                      \
+	(_pAd)->StaActive.CfpMaxDuration = (_pAd)->MlmeAux.CfpMaxDuration;                  \
+	(_pAd)->StaActive.CfpPeriod = (_pAd)->MlmeAux.CfpPeriod;                            \
+	(_pAd)->StaActive.SupRateLen = (_pAd)->MlmeAux.SupRateLen;                          \
+	NdisMoveMemory((_pAd)->StaActive.SupRate, (_pAd)->MlmeAux.SupRate, (_pAd)->MlmeAux.SupRateLen);\
+	(_pAd)->StaActive.ExtRateLen = (_pAd)->MlmeAux.ExtRateLen;                          \
+	NdisMoveMemory((_pAd)->StaActive.ExtRate, (_pAd)->MlmeAux.ExtRate, (_pAd)->MlmeAux.ExtRateLen);\
+	NdisMoveMemory(&(_pAd)->CommonCfg.APEdcaParm, &(_pAd)->MlmeAux.APEdcaParm, sizeof(EDCA_PARM));\
+	NdisMoveMemory(&(_pAd)->CommonCfg.APQosCapability, &(_pAd)->MlmeAux.APQosCapability, sizeof(QOS_CAPABILITY_PARM));\
+	NdisMoveMemory(&(_pAd)->CommonCfg.APQbssLoad, &(_pAd)->MlmeAux.APQbssLoad, sizeof(QBSS_LOAD_PARM));\
+	COPY_MAC_ADDR((_pAd)->MacTab.Content[BSSID_WCID].Addr, (_pAd)->MlmeAux.Bssid);      \
+	(_pAd)->MacTab.Content[BSSID_WCID].Aid = (_pAd)->MlmeAux.Aid;                       \
+	(_pAd)->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = (_pAd)->StaCfg.PairCipher;\
+	COPY_MAC_ADDR((_pAd)->MacTab.Content[BSSID_WCID].PairwiseKey.BssId, (_pAd)->MlmeAux.Bssid);\
+	(_pAd)->MacTab.Content[BSSID_WCID].RateLen = (_pAd)->StaActive.SupRateLen + (_pAd)->StaActive.ExtRateLen;\
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+*/
+VOID MlmeCntlInit(
+	IN PRTMP_ADAPTER pAd,
+	IN STATE_MACHINE *S,
+	OUT STATE_MACHINE_FUNC Trans[])
+{
+	// Control state machine differs from other state machines, the interface
+	// follows the standard interface
+	pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID MlmeCntlMachinePerformAction(
+	IN PRTMP_ADAPTER pAd,
+	IN STATE_MACHINE *S,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	switch(pAd->Mlme.CntlMachine.CurrState)
+	{
+		case CNTL_IDLE:
+				CntlIdleProc(pAd, Elem);
+			break;
+		case CNTL_WAIT_DISASSOC:
+			CntlWaitDisassocProc(pAd, Elem);
+			break;
+		case CNTL_WAIT_JOIN:
+			CntlWaitJoinProc(pAd, Elem);
+			break;
+
+		// CNTL_WAIT_REASSOC is the only state in CNTL machine that does
+		// not triggered directly or indirectly by "RTMPSetInformation(OID_xxx)".
+		// Therefore not protected by NDIS's "only one outstanding OID request"
+		// rule. Which means NDIS may SET OID in the middle of ROAMing attempts.
+		// Current approach is to block new SET request at RTMPSetInformation()
+		// when CntlMachine.CurrState is not CNTL_IDLE
+		case CNTL_WAIT_REASSOC:
+			CntlWaitReassocProc(pAd, Elem);
+			break;
+
+		case CNTL_WAIT_START:
+			CntlWaitStartProc(pAd, Elem);
+			break;
+		case CNTL_WAIT_AUTH:
+			CntlWaitAuthProc(pAd, Elem);
+			break;
+		case CNTL_WAIT_AUTH2:
+			CntlWaitAuthProc2(pAd, Elem);
+			break;
+		case CNTL_WAIT_ASSOC:
+			CntlWaitAssocProc(pAd, Elem);
+			break;
+
+		case CNTL_WAIT_OID_LIST_SCAN:
+			if(Elem->MsgType == MT2_SCAN_CONF)
+			{
+				// Resume TxRing after SCANING complete. We hope the out-of-service time
+				// won't be too long to let upper layer time-out the waiting frames
+				RTMPResumeMsduTransmission(pAd);
+				if (pAd->StaCfg.CCXReqType != MSRN_TYPE_UNUSED)
+				{
+					// Cisco scan request is finished, prepare beacon report
+					MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_DONE, 0, NULL);
+				}
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+
+                //
+				// Set LED status to previous status.
+				//
+				if (pAd->bLedOnScanning)
+				{
+					pAd->bLedOnScanning = FALSE;
+					RTMPSetLED(pAd, pAd->LedStatus);
+				}
+#ifdef DOT11N_DRAFT3
+				// AP sent a 2040Coexistence mgmt frame, then station perform a scan, and then send back the respone.
+				if (pAd->CommonCfg.BSSCoexist2040.field.InfoReq == 1)
+				{
+					Update2040CoexistFrameAndNotify(pAd, BSSID_WCID, TRUE);
+				}
+#endif // DOT11N_DRAFT3 //
+			}
+			break;
+
+		case CNTL_WAIT_OID_DISASSOC:
+			if (Elem->MsgType == MT2_DISASSOC_CONF)
+			{
+				LinkDown(pAd, FALSE);
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			}
+			break;
+#ifdef RT2870
+		//
+		// This state is for that we want to connect to an AP but
+		// it didn't find on BSS List table. So we need to scan the air first,
+		// after that we can try to connect to the desired AP if available.
+		//
+		case CNTL_WAIT_SCAN_FOR_CONNECT:
+			if(Elem->MsgType == MT2_SCAN_CONF)
+			{
+				// Resume TxRing after SCANING complete. We hope the out-of-service time
+				// won't be too long to let upper layer time-out the waiting frames
+				RTMPResumeMsduTransmission(pAd);
+#ifdef CCX_SUPPORT
+				if (pAd->StaCfg.CCXReqType != MSRN_TYPE_UNUSED)
+				{
+					// Cisco scan request is finished, prepare beacon report
+					MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_DONE, 0, NULL);
+				}
+#endif // CCX_SUPPORT //
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+
+				//
+				// Check if we can connect to.
+				//
+				BssTableSsidSort(pAd, &pAd->MlmeAux.SsidBssTab, pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen);
+				if (pAd->MlmeAux.SsidBssTab.BssNr > 0)
+				{
+					MlmeAutoReconnectLastSSID(pAd);
+				}
+			}
+			break;
+#endif // RT2870 //
+		default:
+			DBGPRINT_ERR(("!ERROR! CNTL - Illegal message type(=%ld)", Elem->MsgType));
+			break;
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlIdleProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_DISASSOC_REQ_STRUCT   DisassocReq;
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+		return;
+
+	switch(Elem->MsgType)
+	{
+		case OID_802_11_SSID:
+			CntlOidSsidProc(pAd, Elem);
+			break;
+
+		case OID_802_11_BSSID:
+			CntlOidRTBssidProc(pAd,Elem);
+			break;
+
+		case OID_802_11_BSSID_LIST_SCAN:
+			CntlOidScanProc(pAd,Elem);
+			break;
+
+		case OID_802_11_DISASSOCIATE:
+#ifdef RALINK_ATE
+			if(ATE_ON(pAd))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_ENABLE_WITH_WEB_UI)
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+    			// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+    			// Since calling this indicate user don't want to connect to that SSID anymore.
+    			pAd->MlmeAux.AutoReconnectSsidLen= 32;
+    			NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen);
+            }
+			break;
+
+		case MT2_MLME_ROAMING_REQ:
+			CntlMlmeRoamingProc(pAd, Elem);
+			break;
+
+        case OID_802_11_MIC_FAILURE_REPORT_FRAME:
+            WpaMicFailureReportFrame(pAd, Elem);
+            break;
+
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_SET_DLS_PARAM:
+			CntlOidDLSSetupProc(pAd, Elem);
+			break;
+#endif // QOS_DLS_SUPPORT //
+
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Illegal message in CntlIdleProc(MsgType=%ld)\n",Elem->MsgType));
+			break;
+	}
+}
+
+VOID CntlOidScanProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_SCAN_REQ_STRUCT       ScanReq;
+	ULONG                      BssIdx = BSS_NOT_FOUND;
+	BSS_ENTRY                  CurrBss;
+
+#ifdef RALINK_ATE
+/* Disable scanning when ATE is running. */
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+
+	// record current BSS if network is connected.
+	// 2003-2-13 do not include current IBSS if this is the only STA in this IBSS.
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		BssIdx = BssSsidTableSearch(&pAd->ScanTab, pAd->CommonCfg.Bssid, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen, pAd->CommonCfg.Channel);
+		if (BssIdx != BSS_NOT_FOUND)
+		{
+			NdisMoveMemory(&CurrBss, &pAd->ScanTab.BssEntry[BssIdx], sizeof(BSS_ENTRY));
+		}
+	}
+
+	// clean up previous SCAN result, add current BSS back to table if any
+	BssTableInit(&pAd->ScanTab);
+	if (BssIdx != BSS_NOT_FOUND)
+	{
+		// DDK Note: If the NIC is associated with a particular BSSID and SSID
+		//    that are not contained in the list of BSSIDs generated by this scan, the
+		//    BSSID description of the currently associated BSSID and SSID should be
+		//    appended to the list of BSSIDs in the NIC's database.
+		// To ensure this, we append this BSS as the first entry in SCAN result
+		NdisMoveMemory(&pAd->ScanTab.BssEntry[0], &CurrBss, sizeof(BSS_ENTRY));
+		pAd->ScanTab.BssNr = 1;
+	}
+
+	ScanParmFill(pAd, &ScanReq, "", 0, BSS_ANY, SCAN_ACTIVE);
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ,
+		sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+}
+
+/*
+	==========================================================================
+	Description:
+		Before calling this routine, user desired SSID should already been
+		recorded in CommonCfg.Ssid[]
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlOidSsidProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM * Elem)
+{
+	PNDIS_802_11_SSID          pOidSsid = (NDIS_802_11_SSID *)Elem->Msg;
+	MLME_DISASSOC_REQ_STRUCT   DisassocReq;
+	ULONG					   Now;
+
+	// Step 1. record the desired user settings to MlmeAux
+	NdisZeroMemory(pAd->MlmeAux.Ssid, MAX_LEN_OF_SSID);
+	NdisMoveMemory(pAd->MlmeAux.Ssid, pOidSsid->Ssid, pOidSsid->SsidLength);
+	pAd->MlmeAux.SsidLen = (UCHAR)pOidSsid->SsidLength;
+	NdisZeroMemory(pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+	pAd->MlmeAux.BssType = pAd->StaCfg.BssType;
+
+
+	//
+	// Update Reconnect Ssid, that user desired to connect.
+	//
+	NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, MAX_LEN_OF_SSID);
+	NdisMoveMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+	pAd->MlmeAux.AutoReconnectSsidLen = pAd->MlmeAux.SsidLen;
+
+	// step 2. find all matching BSS in the lastest SCAN result (inBssTab)
+	//    & log them into MlmeAux.SsidBssTab for later-on iteration. Sort by RSSI order
+	BssTableSsidSort(pAd, &pAd->MlmeAux.SsidBssTab, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - %d BSS of %d BSS match the desire (%d)SSID - %s\n",
+			pAd->MlmeAux.SsidBssTab.BssNr, pAd->ScanTab.BssNr, pAd->MlmeAux.SsidLen, pAd->MlmeAux.Ssid));
+	NdisGetSystemUpTime(&Now);
+
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) &&
+		(pAd->CommonCfg.SsidLen == pAd->MlmeAux.SsidBssTab.BssEntry[0].SsidLen) &&
+		NdisEqualMemory(pAd->CommonCfg.Ssid, pAd->MlmeAux.SsidBssTab.BssEntry[0].Ssid, pAd->CommonCfg.SsidLen) &&
+		MAC_ADDR_EQUAL(pAd->CommonCfg.Bssid, pAd->MlmeAux.SsidBssTab.BssEntry[0].Bssid))
+	{
+		// Case 1. already connected with an AP who has the desired SSID
+		//         with highest RSSI
+
+		// Add checking Mode "LEAP" for CCX 1.0
+		if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+			 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+			 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+			 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+#ifdef LEAP_SUPPORT
+			 || (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+			 ) &&
+			(pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+		{
+			// case 1.1 For WPA, WPA-PSK, if the 1x port is not secured, we have to redo
+			//          connection process
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - disassociate with current AP...\n"));
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+						sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+		}
+		else if (pAd->bConfigChanged == TRUE)
+		{
+			// case 1.2 Important Config has changed, we have to reconnect to the same AP
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - disassociate with current AP Because config changed...\n"));
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+						sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+		}
+		else
+		{
+			// case 1.3. already connected to the SSID with highest RSSI.
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - already with this BSSID. ignore this SET_SSID request\n"));
+			//
+			// (HCT 12.1) 1c_wlan_mediaevents required
+			// media connect events are indicated when associating with the same AP
+			//
+			if (INFRA_ON(pAd))
+			{
+				//
+				// Since MediaState already is NdisMediaStateConnected
+				// We just indicate the connect event again to meet the WHQL required.
+				//
+				pAd->IndicateMediaState = NdisMediaStateConnected;
+				RTMP_IndicateMediaState(pAd);
+                pAd->ExtraInfo = GENERAL_LINK_UP;   // Update extra information to link is up
+			}
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+		}
+	}
+	else if (INFRA_ON(pAd))
+	{
+		//
+		// For RT61
+		// [88888] OID_802_11_SSID should have returned NDTEST_WEP_AP2(Returned: )
+		// RT61 may lost SSID, and not connect to NDTEST_WEP_AP2 and will connect to NDTEST_WEP_AP2 by Autoreconnect
+		// But media status is connected, so the SSID not report correctly.
+		//
+		if (!SSID_EQUAL(pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen))
+		{
+			//
+			// Different SSID means not Roaming case, so we let LinkDown() to Indicate a disconnect event.
+			//
+			pAd->MlmeAux.CurrReqIsFromNdis = TRUE;
+		}
+		// case 2. active INFRA association existent
+		//    roaming is done within miniport driver, nothing to do with configuration
+		//    utility. so upon a new SET(OID_802_11_SSID) is received, we just
+		//    disassociate with the current associated AP,
+		//    then perform a new association with this new SSID, no matter the
+		//    new/old SSID are the same or not.
+		DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - disassociate with current AP...\n"));
+		DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+		MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+					sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+	}
+	else
+	{
+		if (ADHOC_ON(pAd))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - drop current ADHOC\n"));
+			LinkDown(pAd, FALSE);
+			OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+			pAd->IndicateMediaState = NdisMediaStateDisconnected;
+			RTMP_IndicateMediaState(pAd);
+            pAd->ExtraInfo = GENERAL_LINK_DOWN;
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():NDIS_STATUS_MEDIA_DISCONNECT Event C!\n"));
+		}
+
+		if ((pAd->MlmeAux.SsidBssTab.BssNr == 0) &&
+			(pAd->StaCfg.bAutoReconnect == TRUE) &&
+			(pAd->MlmeAux.BssType == BSS_INFRA) &&
+			(MlmeValidateSSID(pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen) == TRUE)
+			)
+		{
+			MLME_SCAN_REQ_STRUCT       ScanReq;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - No matching BSS, start a new scan\n"));
+			ScanParmFill(pAd, &ScanReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen, BSS_ANY, SCAN_ACTIVE);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+			// Reset Missed scan number
+			pAd->StaCfg.LastScanTime = Now;
+		}
+		else
+		{
+			pAd->MlmeAux.BssIdx = 0;
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlOidRTBssidProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM * Elem)
+{
+	ULONG       BssIdx;
+	PUCHAR      pOidBssid = (PUCHAR)Elem->Msg;
+	MLME_DISASSOC_REQ_STRUCT    DisassocReq;
+	MLME_JOIN_REQ_STRUCT        JoinReq;
+
+#ifdef RALINK_ATE
+/* No need to perform this routine when ATE is running. */
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	// record user desired settings
+	COPY_MAC_ADDR(pAd->MlmeAux.Bssid, pOidBssid);
+	pAd->MlmeAux.BssType = pAd->StaCfg.BssType;
+
+	//
+	// Update Reconnect Ssid, that user desired to connect.
+	//
+	NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, MAX_LEN_OF_SSID);
+	pAd->MlmeAux.AutoReconnectSsidLen = pAd->MlmeAux.SsidLen;
+	NdisMoveMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+
+	// find the desired BSS in the latest SCAN result table
+	BssIdx = BssTableSearch(&pAd->ScanTab, pOidBssid, pAd->MlmeAux.Channel);
+	if (BssIdx == BSS_NOT_FOUND)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - BSSID not found. reply NDIS_STATUS_NOT_ACCEPTED\n"));
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+		return;
+	}
+
+	// copy the matched BSS entry from ScanTab to MlmeAux.SsidBssTab. Why?
+	// Because we need this entry to become the JOIN target in later on SYNC state machine
+	pAd->MlmeAux.BssIdx = 0;
+	pAd->MlmeAux.SsidBssTab.BssNr = 1;
+	NdisMoveMemory(&pAd->MlmeAux.SsidBssTab.BssEntry[0], &pAd->ScanTab.BssEntry[BssIdx], sizeof(BSS_ENTRY));
+
+	//pAd->MlmeAux.AutoReconnectSsidLen = pAd->ScanTab.BssEntry[BssIdx].SsidLen;
+	//NdisMoveMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->ScanTab.BssEntry[BssIdx].Ssid, pAd->ScanTab.BssEntry[BssIdx].SsidLen);
+
+	// Add SSID into MlmeAux for site surey joining hidden SSID
+	//pAd->MlmeAux.SsidLen = pAd->ScanTab.BssEntry[BssIdx].SsidLen;
+	//NdisMoveMemory(pAd->MlmeAux.Ssid, pAd->ScanTab.BssEntry[BssIdx].Ssid, pAd->MlmeAux.SsidLen);
+
+	// 2002-11-26 skip the following checking. i.e. if user wants to re-connect to same AP
+	//   we just follow normal procedure. The reason of user doing this may because he/she changed
+	//   AP to another channel, but we still received BEACON from it thus don't claim Link Down.
+	//   Since user knows he's changed AP channel, he'll re-connect again. By skipping the following
+	//   checking, we'll disassociate then re-do normal association with this AP at the new channel.
+	// 2003-1-6 Re-enable this feature based on microsoft requirement which prefer not to re-do
+	//   connection when setting the same BSSID.
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) &&
+		MAC_ADDR_EQUAL(pAd->CommonCfg.Bssid, pOidBssid))
+	{
+		// already connected to the same BSSID, go back to idle state directly
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - already in this BSSID. ignore this SET_BSSID request\n"));
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+	}
+	else
+	{
+		if (INFRA_ON(pAd))
+		{
+			// disassoc from current AP first
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - disassociate with current AP ...\n"));
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+						sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+		}
+		else
+		{
+			if (ADHOC_ON(pAd))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - drop current ADHOC\n"));
+				LinkDown(pAd, FALSE);
+				OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+				pAd->IndicateMediaState = NdisMediaStateDisconnected;
+				RTMP_IndicateMediaState(pAd);
+                pAd->ExtraInfo = GENERAL_LINK_DOWN;
+				DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event C!\n"));
+			}
+
+			// Change the wepstatus to original wepstatus
+			pAd->StaCfg.WepStatus   = pAd->StaCfg.OrigWepStatus;
+			pAd->StaCfg.PairCipher  = pAd->StaCfg.OrigWepStatus;
+			pAd->StaCfg.GroupCipher = pAd->StaCfg.OrigWepStatus;
+
+			// Check cipher suite, AP must have more secured cipher than station setting
+			// Set the Pairwise and Group cipher to match the intended AP setting
+			// We can only connect to AP with less secured cipher setting
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+			{
+				pAd->StaCfg.GroupCipher = pAd->ScanTab.BssEntry[BssIdx].WPA.GroupCipher;
+
+				if (pAd->StaCfg.WepStatus == pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipher)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipher;
+				else if (pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipherAux != Ndis802_11WEPDisabled)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipherAux;
+				else	// There is no PairCipher Aux, downgrade our capability to TKIP
+					pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+			}
+			else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+			{
+				pAd->StaCfg.GroupCipher = pAd->ScanTab.BssEntry[BssIdx].WPA2.GroupCipher;
+
+				if (pAd->StaCfg.WepStatus == pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipher)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipher;
+				else if (pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipherAux != Ndis802_11WEPDisabled)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipherAux;
+				else	// There is no PairCipher Aux, downgrade our capability to TKIP
+					pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+
+				// RSN capability
+				pAd->StaCfg.RsnCapability = pAd->ScanTab.BssEntry[BssIdx].WPA2.RsnCapability;
+			}
+
+			// Set Mix cipher flag
+			pAd->StaCfg.bMixCipher = (pAd->StaCfg.PairCipher == pAd->StaCfg.GroupCipher) ? FALSE : TRUE;
+			if (pAd->StaCfg.bMixCipher == TRUE)
+			{
+				// If mix cipher, re-build RSNIE
+				RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, 0);
+			}
+			// No active association, join the BSS immediately
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - joining %02x:%02x:%02x:%02x:%02x:%02x ...\n",
+				pOidBssid[0],pOidBssid[1],pOidBssid[2],pOidBssid[3],pOidBssid[4],pOidBssid[5]));
+
+			JoinParmFill(pAd, &JoinReq, pAd->MlmeAux.BssIdx);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_JOIN_REQ, sizeof(MLME_JOIN_REQ_STRUCT), &JoinReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_JOIN;
+		}
+	}
+}
+
+// Roaming is the only external request triggering CNTL state machine
+// despite of other "SET OID" operation. All "SET OID" related oerations
+// happen in sequence, because no other SET OID will be sent to this device
+// until the the previous SET operation is complete (successful o failed).
+// So, how do we quarantee this ROAMING request won't corrupt other "SET OID"?
+// or been corrupted by other "SET OID"?
+//
+// IRQL = DISPATCH_LEVEL
+VOID CntlMlmeRoamingProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	// TODO:
+	// AP in different channel may show lower RSSI than actual value??
+	// should we add a weighting factor to compensate it?
+	DBGPRINT(RT_DEBUG_TRACE,("CNTL - Roaming in MlmeAux.RoamTab...\n"));
+
+	NdisMoveMemory(&pAd->MlmeAux.SsidBssTab, &pAd->MlmeAux.RoamTab, sizeof(pAd->MlmeAux.RoamTab));
+	pAd->MlmeAux.SsidBssTab.BssNr = pAd->MlmeAux.RoamTab.BssNr;
+
+	BssTableSortByRssi(&pAd->MlmeAux.SsidBssTab);
+	pAd->MlmeAux.BssIdx = 0;
+	IterateOnBssTab(pAd);
+}
+
+#ifdef QOS_DLS_SUPPORT
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlOidDLSSetupProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PRT_802_11_DLS		pDLS = (PRT_802_11_DLS)Elem->Msg;
+	MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+	INT					i;
+	USHORT				reason = REASON_UNSPECIFY;
+
+	DBGPRINT(RT_DEBUG_TRACE,("CNTL - (OID set %02x:%02x:%02x:%02x:%02x:%02x with Valid=%d, Status=%d, TimeOut=%d, CountDownTimer=%d)\n",
+		pDLS->MacAddr[0], pDLS->MacAddr[1], pDLS->MacAddr[2], pDLS->MacAddr[3], pDLS->MacAddr[4], pDLS->MacAddr[5],
+		pDLS->Valid, pDLS->Status, pDLS->TimeOut, pDLS->CountDownTimer));
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return;
+
+	// DLS will not be supported when Adhoc mode
+	if (INFRA_ON(pAd))
+	{
+		for (i = 0; i < MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			if (pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				(pDLS->TimeOut == pAd->StaCfg.DLSEntry[i].TimeOut) && MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 1. Same setting, just drop it
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - setting unchanged\n"));
+				break;
+			}
+			else if (!pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 2. Disable DLS link case, just tear down DLS link
+				reason = REASON_QOS_UNWANTED_MECHANISM;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - start tear down procedure\n"));
+				break;
+			}
+			else if ((i < MAX_NUM_OF_DLS_ENTRY) && pDLS->Valid && !pAd->StaCfg.DLSEntry[i].Valid)
+			{
+				// 3. Enable case, start DLS setup procedure
+				NdisMoveMemory(&pAd->StaCfg.DLSEntry[i], pDLS, sizeof(RT_802_11_DLS_UI));
+
+				//Update countdown timer
+				pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS setup case\n"));
+				break;
+			}
+			else if ((i < MAX_NUM_OF_DLS_ENTRY) && pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid &&
+				(pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) && !MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 4. update mac case, tear down old DLS and setup new DLS
+				reason = REASON_QOS_UNWANTED_MECHANISM;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				NdisMoveMemory(&pAd->StaCfg.DLSEntry[i], pDLS, sizeof(RT_802_11_DLS_UI));
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS tear down and restart case\n"));
+				break;
+			}
+			else if (pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid &&
+				MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr) && (pAd->StaCfg.DLSEntry[i].TimeOut != pDLS->TimeOut))
+			{
+				// 5. update timeout case, start DLS setup procedure (no tear down)
+				pAd->StaCfg.DLSEntry[i].TimeOut	= pDLS->TimeOut;
+				//Update countdown timer
+				pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS update timeout case\n"));
+				break;
+			}
+			else if (pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid &&
+				(pAd->StaCfg.DLSEntry[i].Status != DLS_FINISH) && MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 6. re-setup case, start DLS setup procedure (no tear down)
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS retry setup procedure\n"));
+				break;
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN,("CNTL - DLS not changed in entry - %d - Valid=%d, Status=%d, TimeOut=%d\n",
+					i, pAd->StaCfg.DLSEntry[i].Valid, pAd->StaCfg.DLSEntry[i].Status, pAd->StaCfg.DLSEntry[i].TimeOut));
+			}
+		}
+	}
+}
+#endif // QOS_DLS_SUPPORT //
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitDisassocProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_START_REQ_STRUCT     StartReq;
+
+	if (Elem->MsgType == MT2_DISASSOC_CONF)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Dis-associate successful\n"));
+
+	    if (pAd->CommonCfg.bWirelessEvent)
+		{
+			RTMPSendWirelessEvent(pAd, IW_DISASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+
+		LinkDown(pAd, FALSE);
+
+		// case 1. no matching BSS, and user wants ADHOC, so we just start a new one
+		if ((pAd->MlmeAux.SsidBssTab.BssNr==0) && (pAd->StaCfg.BssType == BSS_ADHOC))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - No matching BSS, start a new ADHOC (Ssid=%s)...\n",pAd->MlmeAux.Ssid));
+			StartParmFill(pAd, &StartReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_START_REQ, sizeof(MLME_START_REQ_STRUCT), &StartReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_START;
+		}
+		// case 2. try each matched BSS
+		else
+		{
+			pAd->MlmeAux.BssIdx = 0;
+
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitJoinProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT                      Reason;
+	MLME_AUTH_REQ_STRUCT        AuthReq;
+
+	if (Elem->MsgType == MT2_JOIN_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			// 1. joined an IBSS, we are pretty much done here
+			if (pAd->MlmeAux.BssType == BSS_ADHOC)
+			{
+			    //
+				// 5G bands rules of Japan:
+				// Ad hoc must be disabled in W53(ch52,56,60,64) channels.
+				//
+				if ( (pAd->CommonCfg.bIEEE80211H == 1) &&
+                      RadarChannelCheck(pAd, pAd->CommonCfg.Channel)
+				   )
+				{
+					pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+					DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Channel=%d, Join adhoc on W53(52,56,60,64) Channels are not accepted\n", pAd->CommonCfg.Channel));
+					return;
+				}
+
+				LinkUp(pAd, BSS_ADHOC);
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - join the IBSS = %02x:%02x:%02x:%02x:%02x:%02x ...\n",
+				pAd->CommonCfg.Bssid[0],pAd->CommonCfg.Bssid[1],pAd->CommonCfg.Bssid[2],
+				pAd->CommonCfg.Bssid[3],pAd->CommonCfg.Bssid[4],pAd->CommonCfg.Bssid[5]));
+
+                pAd->IndicateMediaState = NdisMediaStateConnected;
+                pAd->ExtraInfo = GENERAL_LINK_UP;
+			}
+			// 2. joined a new INFRA network, start from authentication
+			else
+			{
+#ifdef LEAP_SUPPORT
+				// Add AuthMode "LEAP" for CCX 1.X
+				if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+				{
+					AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, CISCO_AuthModeLEAP);
+				}
+				else
+#endif // LEAP_SUPPORT //
+				{
+					// either Ndis802_11AuthModeShared or Ndis802_11AuthModeAutoSwitch, try shared key first
+					if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeShared) ||
+						(pAd->StaCfg.AuthMode == Ndis802_11AuthModeAutoSwitch))
+					{
+						AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeShared);
+					}
+					else
+					{
+						AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeOpen);
+					}
+				}
+				MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_MLME_AUTH_REQ,
+							sizeof(MLME_AUTH_REQ_STRUCT), &AuthReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_AUTH;
+			}
+		}
+		else
+		{
+			// 3. failed, try next BSS
+			pAd->MlmeAux.BssIdx++;
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitStartProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      Result;
+
+	if (Elem->MsgType == MT2_START_CONF)
+	{
+		NdisMoveMemory(&Result, Elem->Msg, sizeof(USHORT));
+		if (Result == MLME_SUCCESS)
+		{
+		    //
+			// 5G bands rules of Japan:
+			// Ad hoc must be disabled in W53(ch52,56,60,64) channels.
+			//
+			if ( (pAd->CommonCfg.bIEEE80211H == 1) &&
+                  RadarChannelCheck(pAd, pAd->CommonCfg.Channel)
+			   )
+			{
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Channel=%d, Start adhoc on W53(52,56,60,64) Channels are not accepted\n", pAd->CommonCfg.Channel));
+				return;
+			}
+#ifdef DOT11_N_SUPPORT
+			if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+			{
+				N_ChannelCheck(pAd);
+				SetCommonHT(pAd);
+				NdisMoveMemory(&pAd->MlmeAux.AddHtInfo, &pAd->CommonCfg.AddHTInfo, sizeof(ADD_HT_INFO_IE));
+				RTMPCheckHt(pAd, BSSID_WCID, &pAd->CommonCfg.HtCapability, &pAd->CommonCfg.AddHTInfo);
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = TRUE;
+				NdisZeroMemory(&pAd->StaActive.SupportedPhyInfo.MCSSet[0], 16);
+				NdisMoveMemory(&pAd->StaActive.SupportedPhyInfo.MCSSet[0], &pAd->CommonCfg.HtCapability.MCSSet[0], 16);
+				COPY_HTSETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(pAd);
+
+				if ((pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth  == BW_40) &&
+					(pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset == EXTCHA_ABOVE))
+				{
+					pAd->MlmeAux.CentralChannel = pAd->CommonCfg.Channel + 2;
+				}
+				else if ((pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth  == BW_40) &&
+						 (pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset == EXTCHA_BELOW))
+				{
+					pAd->MlmeAux.CentralChannel = pAd->CommonCfg.Channel - 2;
+				}
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+			{
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+			}
+			LinkUp(pAd, BSS_ADHOC);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			// Before send beacon, driver need do radar detection
+			if ((pAd->CommonCfg.Channel > 14 )
+				&& (pAd->CommonCfg.bIEEE80211H == 1)
+				&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+			{
+				pAd->CommonCfg.RadarDetect.RDMode = RD_SILENCE_MODE;
+				pAd->CommonCfg.RadarDetect.RDCount = 0;
+#ifdef DFS_SUPPORT
+				BbpRadarDetectionStart(pAd);
+#endif // DFS_SUPPORT //
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - start a new IBSS = %02x:%02x:%02x:%02x:%02x:%02x ...\n",
+				pAd->CommonCfg.Bssid[0],pAd->CommonCfg.Bssid[1],pAd->CommonCfg.Bssid[2],
+				pAd->CommonCfg.Bssid[3],pAd->CommonCfg.Bssid[4],pAd->CommonCfg.Bssid[5]));
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Start IBSS fail. BUG!!!!!\n"));
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitAuthProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT                       Reason;
+	MLME_ASSOC_REQ_STRUCT        AssocReq;
+	MLME_AUTH_REQ_STRUCT         AuthReq;
+
+	if (Elem->MsgType == MT2_AUTH_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH OK\n"));
+			AssocParmFill(pAd, &AssocReq, pAd->MlmeAux.Bssid, pAd->MlmeAux.CapabilityInfo,
+						  ASSOC_TIMEOUT, pAd->StaCfg.DefaultListenCount);
+
+#ifdef LEAP_SUPPORT
+			//
+			// Cisco Leap CCKM supported Re-association.
+			//
+			if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+			{
+				//if CCKM is turn on , that's mean Fast Reauthentication
+				//Use CCKM Reassociation instead of normal association for Fast Roaming.
+				MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_REASSOC_REQ,
+							sizeof(MLME_ASSOC_REQ_STRUCT), &AssocReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_REASSOC;
+			}
+			else
+#endif // LEAP_SUPPORT //
+			{
+				MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_ASSOC_REQ,
+							sizeof(MLME_ASSOC_REQ_STRUCT), &AssocReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_ASSOC;
+			}
+		}
+		else
+		{
+			// This fail may because of the AP already keep us in its MAC table without
+			// ageing-out. The previous authentication attempt must have let it remove us.
+			// so try Authentication again may help. For D-Link DWL-900AP+ compatibility.
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH FAIL, try again...\n"));
+#ifdef LEAP_SUPPORT
+			//Add AuthMode "LEAP" for CCX 1.X
+			if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+			{
+				AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, CISCO_AuthModeLEAP);
+			}
+			else
+#endif // LEAP_SUPPORT //
+			{
+				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeShared) ||
+					(pAd->StaCfg.AuthMode == Ndis802_11AuthModeAutoSwitch))
+				{
+					// either Ndis802_11AuthModeShared or Ndis802_11AuthModeAutoSwitch, try shared key first
+					AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeShared);
+				}
+				else
+				{
+					AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeOpen);
+				}
+			}
+			MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_MLME_AUTH_REQ,
+						sizeof(MLME_AUTH_REQ_STRUCT), &AuthReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_AUTH2;
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitAuthProc2(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT                       Reason;
+	MLME_ASSOC_REQ_STRUCT        AssocReq;
+	MLME_AUTH_REQ_STRUCT         AuthReq;
+
+	if (Elem->MsgType == MT2_AUTH_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH OK\n"));
+			AssocParmFill(pAd, &AssocReq, pAd->MlmeAux.Bssid, pAd->MlmeAux.CapabilityInfo,
+						  ASSOC_TIMEOUT, pAd->StaCfg.DefaultListenCount);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_ASSOC_REQ,
+						sizeof(MLME_ASSOC_REQ_STRUCT), &AssocReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_ASSOC;
+		}
+		else
+		{
+#ifdef LEAP_SUPPORT
+			// Process LEAP first, since it use different control variable
+			// We don't want to affect other poven operation
+			if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+			{
+				// LEAP Auth not success, try next BSS
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - *LEAP* AUTH FAIL, give up; try next BSS\n"));
+				DBGPRINT(RT_DEBUG_TRACE, ("Total match BSSID [=%d]\n", pAd->MlmeAux.SsidBssTab.BssNr));
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+				pAd->MlmeAux.BssIdx++;
+				IterateOnBssTab(pAd);
+			}
+			else
+#endif // LEAP_SUPPORT //
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeAutoSwitch) &&
+				 (pAd->MlmeAux.Alg == Ndis802_11AuthModeShared))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH FAIL, try OPEN system...\n"));
+				AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeOpen);
+				MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_MLME_AUTH_REQ,
+							sizeof(MLME_AUTH_REQ_STRUCT), &AuthReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_AUTH2;
+			}
+			else
+			{
+				// not success, try next BSS
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH FAIL, give up; try next BSS\n"));
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE; //???????
+				pAd->MlmeAux.BssIdx++;
+				IterateOnBssTab(pAd);
+			}
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitAssocProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      Reason;
+
+	if (Elem->MsgType == MT2_ASSOC_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			LinkUp(pAd, BSS_INFRA);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Association successful on BSS #%ld\n",pAd->MlmeAux.BssIdx));
+
+			if (pAd->CommonCfg.bWirelessEvent)
+			{
+				RTMPSendWirelessEvent(pAd, IW_ASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+			}
+		}
+		else
+		{
+			// not success, try next BSS
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Association fails on BSS #%ld\n",pAd->MlmeAux.BssIdx));
+			pAd->MlmeAux.BssIdx++;
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitReassocProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      Result;
+
+	if (Elem->MsgType == MT2_REASSOC_CONF)
+	{
+		NdisMoveMemory(&Result, Elem->Msg, sizeof(USHORT));
+		if (Result == MLME_SUCCESS)
+		{
+			//
+			// NDIS requires a new Link UP indication but no Link Down for RE-ASSOC
+			//
+			LinkUp(pAd, BSS_INFRA);
+
+			// send wireless event - for association
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_ASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+
+#ifdef LEAP_SUPPORT
+			if (LEAP_CCKM_ON(pAd))
+			{
+				STA_PORT_SECURED(pAd);
+				pAd->StaCfg.WpaState = SS_FINISH;
+			}
+#endif // LEAP_SUPPORT //
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Re-assocition successful on BSS #%ld\n", pAd->MlmeAux.RoamIdx));
+		}
+		else
+		{
+			// reassoc failed, try to pick next BSS in the BSS Table
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Re-assocition fails on BSS #%ld\n", pAd->MlmeAux.RoamIdx));
+			pAd->MlmeAux.RoamIdx++;
+			IterateOnBssTab2(pAd);
+		}
+	}
+}
+
+
+VOID	AdhocTurnOnQos(
+	IN  PRTMP_ADAPTER pAd)
+{
+#define AC0_DEF_TXOP		0
+#define AC1_DEF_TXOP		0
+#define AC2_DEF_TXOP		94
+#define AC3_DEF_TXOP		47
+
+	// Turn on QOs if use HT rate.
+	if (pAd->CommonCfg.APEdcaParm.bValid == FALSE)
+	{
+		pAd->CommonCfg.APEdcaParm.bValid = TRUE;
+		pAd->CommonCfg.APEdcaParm.Aifsn[0] = 3;
+		pAd->CommonCfg.APEdcaParm.Aifsn[1] = 7;
+		pAd->CommonCfg.APEdcaParm.Aifsn[2] = 1;
+		pAd->CommonCfg.APEdcaParm.Aifsn[3] = 1;
+
+		pAd->CommonCfg.APEdcaParm.Cwmin[0] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[1] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[2] = 3;
+		pAd->CommonCfg.APEdcaParm.Cwmin[3] = 2;
+
+		pAd->CommonCfg.APEdcaParm.Cwmax[0] = 10;
+		pAd->CommonCfg.APEdcaParm.Cwmax[1] = 6;
+		pAd->CommonCfg.APEdcaParm.Cwmax[2] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmax[3] = 3;
+
+		pAd->CommonCfg.APEdcaParm.Txop[0]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[1]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[2]  = AC2_DEF_TXOP;
+		pAd->CommonCfg.APEdcaParm.Txop[3]  = AC3_DEF_TXOP;
+	}
+	AsicSetEdcaParm(pAd, &pAd->CommonCfg.APEdcaParm);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID LinkUp(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR BssType)
+{
+	ULONG	Now;
+	UINT32	Data;
+	BOOLEAN	Cancelled;
+	UCHAR	Value = 0, idx;
+	MAC_TABLE_ENTRY *pEntry = NULL, *pCurrEntry;
+
+	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+
+	//
+	// ASSOC - DisassocTimeoutAction
+	// CNTL - Dis-associate successful
+	// !!! LINK DOWN !!!
+	// [88888] OID_802_11_SSID should have returned NDTEST_WEP_AP2(Returned: )
+	//
+	// To prevent DisassocTimeoutAction to call Link down after we link up,
+	// cancel the DisassocTimer no matter what it start or not.
+	//
+	RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,  &Cancelled);
+
+	COPY_SETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(pAd);
+
+#ifdef DOT11_N_SUPPORT
+	COPY_HTSETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(pAd);
+#endif // DOT11_N_SUPPORT //
+	// It's quite difficult to tell if a newly added KEY is WEP or CKIP until a new BSS
+	// is formed (either ASSOC/RE-ASSOC done or IBSS started. LinkUP should be a safe place
+	// to examine if cipher algorithm switching is required.
+	//rt2860b. Don't know why need this
+	SwitchBetweenWepAndCkip(pAd);
+
+
+	if (BssType == BSS_ADHOC)
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_INFRA_ON);
+
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+		// No carrier detection when adhoc
+		// CarrierDetectionStop(pAd);
+		pAd->CommonCfg.CarrierDetect.CD_State = CD_NORMAL;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+		if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+			AdhocTurnOnQos(pAd);
+#endif // DOT11_N_SUPPORT //
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!Adhoc LINK UP !!! \n" ));
+	}
+	else
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_INFRA_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!Infra LINK UP !!! \n" ));
+	}
+
+	// 3*3
+	// reset Tx beamforming bit
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+	Value &= (~0x01);
+	Value |= pAd->CommonCfg.RegTransmitSetting.field.TxBF;
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+#ifdef DOT11_N_SUPPORT
+	// Change to AP channel
+    if ((pAd->CommonCfg.CentralChannel > pAd->CommonCfg.Channel) && (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+	{
+		// Must using 40MHz.
+		pAd->CommonCfg.BBPCurrentBW = BW_40;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+		Value &= (~0x18);
+		Value |= 0x10;
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+		//  RX : control channel at lower
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+		Value &= (~0x20);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Data);
+		Data &= 0xfffffffe;
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Data);
+
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x1A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x0A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x16);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Lower LINK UP !!! Control Channel at Below. Central = %d \n", pAd->CommonCfg.CentralChannel ));
+	}
+	else if ((pAd->CommonCfg.CentralChannel < pAd->CommonCfg.Channel) && (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+    {
+	    // Must using 40MHz.
+		pAd->CommonCfg.BBPCurrentBW = BW_40;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+	    AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+		Value &= (~0x18);
+		Value |= 0x10;
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Data);
+		Data |= 0x1;
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Data);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+	    Value |= (0x20);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x1A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x0A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x16);
+			    DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+
+	    DBGPRINT(RT_DEBUG_TRACE, ("!!! 40MHz Upper LINK UP !!! Control Channel at UpperCentral = %d \n", pAd->CommonCfg.CentralChannel ));
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+    {
+	    pAd->CommonCfg.BBPCurrentBW = BW_20;
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+		Value &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Data);
+		Data &= 0xfffffffe;
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Data);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+		Value &= (~0x20);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x16);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x08);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x11);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+
+	    DBGPRINT(RT_DEBUG_TRACE, ("!!! 20MHz LINK UP !!! \n" ));
+    }
+
+	RTMPSetAGCInitValue(pAd, pAd->CommonCfg.BBPCurrentBW);
+	//
+	// Save BBP_R66 value, it will be used in RTUSBResumeMsduTransmission
+	//
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &pAd->BbpTuning.R66CurrentValue);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !!! (BssType=%d, AID=%d, ssid=%s, Channel=%d, CentralChannel = %d)\n",
+		BssType, pAd->StaActive.Aid, pAd->CommonCfg.Ssid, pAd->CommonCfg.Channel, pAd->CommonCfg.CentralChannel));
+
+#ifdef DOT11_N_SUPPORT
+	DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !!! (Density =%d, )\n", pAd->MacTab.Content[BSSID_WCID].MpduDensity));
+#endif // DOT11_N_SUPPORT //
+
+		AsicSetBssid(pAd, pAd->CommonCfg.Bssid);
+
+	AsicSetSlotTime(pAd, TRUE);
+	AsicSetEdcaParm(pAd, &pAd->CommonCfg.APEdcaParm);
+
+	// Call this for RTS protectionfor legacy rate, we will always enable RTS threshold, but normally it will not hit
+	AsicUpdateProtect(pAd, 0, (OFDMSETPROTECT | CCKSETPROTECT), TRUE, FALSE);
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE))
+	{
+		// Update HT protectionfor based on AP's operating mode.
+    	if (pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1)
+    	{
+    		AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode,  ALLN_SETPROTECT, FALSE, TRUE);
+    	}
+    	else
+   			AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode,  ALLN_SETPROTECT, FALSE, FALSE);
+	}
+#endif // DOT11_N_SUPPORT //
+
+	NdisZeroMemory(&pAd->DrsCounters, sizeof(COUNTER_DRS));
+
+	NdisGetSystemUpTime(&Now);
+	pAd->StaCfg.LastBeaconRxTime = Now;   // last RX timestamp
+
+	if ((pAd->CommonCfg.TxPreamble != Rt802_11PreambleLong) &&
+		CAP_IS_SHORT_PREAMBLE_ON(pAd->StaActive.CapabilityInfo))
+	{
+		MlmeSetTxPreamble(pAd, Rt802_11PreambleShort);
+	}
+
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+
+	if (pAd->CommonCfg.RadarDetect.RDMode == RD_SILENCE_MODE)
+	{
+#ifdef DFS_SUPPORT
+		RadarDetectionStop(pAd);
+#endif // DFS_SUPPORT //
+	}
+	pAd->CommonCfg.RadarDetect.RDMode = RD_NORMAL_MODE;
+
+	if (BssType == BSS_ADHOC)
+	{
+		MakeIbssBeacon(pAd);
+		if ((pAd->CommonCfg.Channel > 14)
+			&& (pAd->CommonCfg.bIEEE80211H == 1)
+			&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+		{
+			; //Do nothing
+		}
+		else
+		{
+			AsicEnableIbssSync(pAd);
+		}
+
+		// In ad hoc mode, use MAC table from index 1.
+		// p.s ASIC use all 0xff as termination of WCID table search.To prevent it's 0xff-ff-ff-ff-ff-ff, Write 0 here.
+		RTMP_IO_WRITE32(pAd, MAC_WCID_BASE, 0x00);
+		RTMP_IO_WRITE32(pAd, 0x1808, 0x00);
+
+		// If WEP is enabled, add key material and cipherAlg into Asic
+		// Fill in Shared Key Table(offset: 0x6c00) and Shared Key Mode(offset: 0x7000)
+
+		if (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)
+		{
+			PUCHAR	Key;
+			UCHAR 	CipherAlg;
+
+			for (idx=0; idx < SHARE_KEY_NUM; idx++)
+        	{
+				CipherAlg = pAd->SharedKey[BSS0][idx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][idx].Key;
+
+				if (pAd->SharedKey[BSS0][idx].KeyLen > 0)
+				{
+					// Set key material and cipherAlg to Asic
+    				AsicAddSharedKeyEntry(pAd, BSS0, idx, CipherAlg, Key, NULL, NULL);
+
+                    if (idx == pAd->StaCfg.DefaultKeyId)
+					{
+						// Update WCID attribute table and IVEIV table for this group key table
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, NULL);
+					}
+				}
+
+
+			}
+		}
+		// If WPANone is enabled, add key material and cipherAlg into Asic
+		// Fill in Shared Key Table(offset: 0x6c00) and Shared Key Mode(offset: 0x7000)
+		else if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		{
+			pAd->StaCfg.DefaultKeyId = 0;	// always be zero
+
+            NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+							pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+			NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, pAd->StaCfg.PMK, LEN_TKIP_EK);
+
+            if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+            {
+    			NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PMK[16], LEN_TKIP_RXMICK);
+    			NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PMK[16], LEN_TKIP_TXMICK);
+            }
+
+			// Decide its ChiperAlg
+			if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+				pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+			else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+				pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+			else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Unknow Cipher (=%d), set Cipher to AES\n", pAd->StaCfg.PairCipher));
+				pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+            }
+
+			// Set key material and cipherAlg to Asic
+			AsicAddSharedKeyEntry(pAd,
+								  BSS0,
+								  0,
+								  pAd->SharedKey[BSS0][0].CipherAlg,
+								  pAd->SharedKey[BSS0][0].Key,
+								  pAd->SharedKey[BSS0][0].TxMic,
+								  pAd->SharedKey[BSS0][0].RxMic);
+
+            // Update WCID attribute table and IVEIV table for this group key table
+			RTMPAddWcidAttributeEntry(pAd, BSS0, 0, pAd->SharedKey[BSS0][0].CipherAlg, NULL);
+
+		}
+
+	}
+	else // BSS_INFRA
+	{
+		// Check the new SSID with last SSID
+		while (Cancelled == TRUE)
+		{
+			if (pAd->CommonCfg.LastSsidLen == pAd->CommonCfg.SsidLen)
+			{
+				if (RTMPCompareMemory(pAd->CommonCfg.LastSsid, pAd->CommonCfg.Ssid, pAd->CommonCfg.LastSsidLen) == 0)
+				{
+					// Link to the old one no linkdown is required.
+					break;
+				}
+			}
+			// Send link down event before set to link up
+			pAd->IndicateMediaState = NdisMediaStateDisconnected;
+			RTMP_IndicateMediaState(pAd);
+            pAd->ExtraInfo = GENERAL_LINK_DOWN;
+			DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event AA!\n"));
+			break;
+		}
+
+		//
+		// On WPA mode, Remove All Keys if not connect to the last BSSID
+		// Key will be set after 4-way handshake.
+		//
+		if ((pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA))
+		{
+			ULONG 		IV;
+
+			// Remove all WPA keys
+			RTMPWPARemoveAllKeys(pAd);
+			pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilter8021xWEP;
+
+			// Fixed connection failed with Range Maximizer - 515 AP (Marvell Chip) when security is WPAPSK/TKIP
+			// If IV related values are too large in GroupMsg2, AP would ignore this message.
+			IV = 0;
+			IV |= (pAd->StaCfg.DefaultKeyId << 30);
+			AsicUpdateWCIDIVEIV(pAd, BSSID_WCID, IV, 0);
+		}
+		// NOTE:
+		// the decision of using "short slot time" or not may change dynamically due to
+		// new STA association to the AP. so we have to decide that upon parsing BEACON, not here
+
+		// NOTE:
+		// the decision to use "RTC/CTS" or "CTS-to-self" protection or not may change dynamically
+		// due to new STA association to the AP. so we have to decide that upon parsing BEACON, not here
+
+		ComposePsPoll(pAd);
+		ComposeNullFrame(pAd);
+
+			AsicEnableBssSync(pAd);
+
+		// Add BSSID to WCID search table
+		AsicUpdateRxWCIDTable(pAd, BSSID_WCID, pAd->CommonCfg.Bssid);
+
+		NdisAcquireSpinLock(&pAd->MacTabLock);
+		// add this BSSID entry into HASH table
+		{
+			UCHAR HashIdx;
+
+			//pEntry = &pAd->MacTab.Content[BSSID_WCID];
+			HashIdx = MAC_ADDR_HASH_INDEX(pAd->CommonCfg.Bssid);
+			if (pAd->MacTab.Hash[HashIdx] == NULL)
+			{
+				pAd->MacTab.Hash[HashIdx] = pEntry;
+			}
+			else
+			{
+				pCurrEntry = pAd->MacTab.Hash[HashIdx];
+				while (pCurrEntry->pNext != NULL)
+					pCurrEntry = pCurrEntry->pNext;
+				pCurrEntry->pNext = pEntry;
+			}
+		}
+		NdisReleaseSpinLock(&pAd->MacTabLock);
+
+
+		// If WEP is enabled, add paiewise and shared key
+#ifdef WPA_SUPPLICANT_SUPPORT
+        if (((pAd->StaCfg.WpaSupplicantUP)&&
+             (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)&&
+             (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED)) ||
+            ((pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_DISABLE)&&
+              (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)))
+#else
+		if (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)
+#endif // WPA_SUPPLICANT_SUPPORT //
+		{
+			PUCHAR	Key;
+			UCHAR 	CipherAlg;
+
+			for (idx=0; idx < SHARE_KEY_NUM; idx++)
+        	{
+				CipherAlg = pAd->SharedKey[BSS0][idx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][idx].Key;
+
+				if (pAd->SharedKey[BSS0][idx].KeyLen > 0)
+				{
+					// Set key material and cipherAlg to Asic
+    				AsicAddSharedKeyEntry(pAd, BSS0, idx, CipherAlg, Key, NULL, NULL);
+
+					if (idx == pAd->StaCfg.DefaultKeyId)
+					{
+						// Assign group key info
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, NULL);
+
+						// Assign pairwise key info
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, pEntry);
+					}
+				}
+			}
+		}
+
+		// only INFRASTRUCTURE mode need to indicate connectivity immediately; ADHOC mode
+		// should wait until at least 2 active nodes in this BSSID.
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+
+        // For GUI ++
+		if (pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+		{
+			pAd->IndicateMediaState = NdisMediaStateConnected;
+			pAd->ExtraInfo = GENERAL_LINK_UP;
+			RTMP_IndicateMediaState(pAd);
+		}
+        // --
+
+		// Add BSSID in my MAC Table.
+        NdisAcquireSpinLock(&pAd->MacTabLock);
+		RTMPMoveMemory(pAd->MacTab.Content[BSSID_WCID].Addr, pAd->CommonCfg.Bssid, MAC_ADDR_LEN);
+		pAd->MacTab.Content[BSSID_WCID].Aid = BSSID_WCID;
+		pAd->MacTab.Content[BSSID_WCID].pAd = pAd;
+		pAd->MacTab.Content[BSSID_WCID].ValidAsCLI = TRUE;	//Although this is bssid..still set ValidAsCl
+		pAd->MacTab.Size = 1;	// infra mode always set MACtab size =1.
+		pAd->MacTab.Content[BSSID_WCID].Sst = SST_ASSOC;
+		pAd->MacTab.Content[BSSID_WCID].AuthState = SST_ASSOC;
+		pAd->MacTab.Content[BSSID_WCID].WepStatus = pAd->StaCfg.WepStatus;
+        NdisReleaseSpinLock(&pAd->MacTabLock);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !!!  ClientStatusFlags=%lx)\n",
+			pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+
+		MlmeUpdateTxRates(pAd, TRUE, BSS0);
+#ifdef DOT11_N_SUPPORT
+		MlmeUpdateHtTxRates(pAd, BSS0);
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !! (StaActive.bHtEnable =%d, )\n", pAd->StaActive.SupportedPhyInfo.bHtEnable));
+#endif // DOT11_N_SUPPORT //
+
+		//
+		// Report Adjacent AP report.
+		//
+#ifdef LEAP_SUPPORT
+		CCXAdjacentAPReport(pAd);
+#endif // LEAP_SUPPORT //
+
+		if (pAd->CommonCfg.bAggregationCapable)
+		{
+			if ((pAd->CommonCfg.bPiggyBackCapable) && (pAd->MlmeAux.APRalinkIe & 0x00000003) == 3)
+			{
+
+				OPSTATUS_SET_FLAG(pAd, fOP_STATUS_PIGGYBACK_INUSED);
+				OPSTATUS_SET_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+                RTMPSetPiggyBack(pAd, TRUE);
+				DBGPRINT(RT_DEBUG_TRACE, ("Turn on Piggy-Back\n"));
+			}
+			else if (pAd->MlmeAux.APRalinkIe & 0x00000001)
+			{
+				OPSTATUS_SET_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+			}
+		}
+
+		if (pAd->MlmeAux.APRalinkIe != 0x0)
+		{
+#ifdef DOT11_N_SUPPORT
+			if (CLIENT_STATUS_TEST_FLAG(&pAd->MacTab.Content[BSSID_WCID], fCLIENT_STATUS_RDG_CAPABLE))
+			{
+				AsicEnableRDG(pAd);
+			}
+#endif // DOT11_N_SUPPORT //
+			OPSTATUS_SET_FLAG(pAd, fCLIENT_STATUS_RALINK_CHIPSET);
+			CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[BSSID_WCID], fCLIENT_STATUS_RALINK_CHIPSET);
+		}
+		else
+		{
+			OPSTATUS_CLEAR_FLAG(pAd, fCLIENT_STATUS_RALINK_CHIPSET);
+			CLIENT_STATUS_CLEAR_FLAG(&pAd->MacTab.Content[BSSID_WCID], fCLIENT_STATUS_RALINK_CHIPSET);
+		}
+	}
+
+#ifdef DOT11_N_SUPPORT
+	DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_CONNECT Event B!.BACapability = %x. ClientStatusFlags = %lx\n", pAd->CommonCfg.BACapability.word, pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+#endif // DOT11_N_SUPPORT //
+
+	// Set LED
+	RTMPSetLED(pAd, LED_LINK_UP);
+
+	pAd->Mlme.PeriodicRound = 0;
+	pAd->Mlme.OneSecPeriodicRound = 0;
+	pAd->bConfigChanged = FALSE;        // Reset config flag
+	pAd->ExtraInfo = GENERAL_LINK_UP;   // Update extra information to link is up
+
+	// Set asic auto fall back
+	{
+		PUCHAR					pTable;
+		UCHAR					TableSize = 0;
+
+		MlmeSelectTxRateTable(pAd, &pAd->MacTab.Content[BSSID_WCID], &pTable, &TableSize, &pAd->CommonCfg.TxRateIndex);
+		AsicUpdateAutoFallBackTable(pAd, pTable);
+	}
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+    pEntry->HTPhyMode.word = pAd->StaCfg.HTPhyMode.word;
+    pEntry->MaxHTPhyMode.word = pAd->StaCfg.HTPhyMode.word;
+	if (pAd->StaCfg.bAutoTxRateSwitch == FALSE)
+	{
+		pEntry->bAutoTxRateSwitch = FALSE;
+#ifdef DOT11_N_SUPPORT
+		if (pEntry->HTPhyMode.field.MCS == 32)
+			pEntry->HTPhyMode.field.ShortGI = GI_800;
+
+		if ((pEntry->HTPhyMode.field.MCS > MCS_7) || (pEntry->HTPhyMode.field.MCS == 32))
+			pEntry->HTPhyMode.field.STBC = STBC_NONE;
+#endif // DOT11_N_SUPPORT //
+		// If the legacy mode is set, overwrite the transmit setting of this entry.
+		if (pEntry->HTPhyMode.field.MODE <= MODE_OFDM)
+			RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+	}
+	else
+		pEntry->bAutoTxRateSwitch = TRUE;
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+	//  Let Link Status Page display first initial rate.
+	pAd->LastTxRate = (USHORT)(pEntry->HTPhyMode.word);
+	// Select DAC according to HT or Legacy
+	if (pAd->StaActive.SupportedPhyInfo.MCSSet[0] != 0x00)
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &Value);
+		Value &= (~0x18);
+		if (pAd->Antenna.field.TxPath == 2)
+		{
+		    Value |= 0x10;
+		}
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, Value);
+	}
+	else
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &Value);
+		Value &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, Value);
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if (pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE)
+	{
+	}
+	else if (pEntry->MaxRAmpduFactor == 0)
+	{
+	    // If HT AP doesn't support MaxRAmpduFactor = 1, we need to set max PSDU to 0.
+	    // Because our Init value is 1 at MACRegTable.
+		RTMP_IO_WRITE32(pAd, MAX_LEN_CFG, 0x0fff);
+	}
+#endif // DOT11_N_SUPPORT //
+
+	// Patch for Marvel AP to gain high throughput
+	// Need to set as following,
+	// 1. Set txop in register-EDCA_AC0_CFG as 0x60
+	// 2. Set EnTXWriteBackDDONE in register-WPDMA_GLO_CFG as zero
+	// 3. PBF_MAX_PCNT as 0x1F3FBF9F
+	// 4. kick per two packets when dequeue
+	//
+	// Txop can only be modified when RDG is off, WMM is disable and TxBurst is enable
+	//
+	// if 1. Legacy AP WMM on,  or 2. 11n AP, AMPDU disable.  Force turn off burst no matter what bEnableTxBurst is.
+#ifdef DOT11_N_SUPPORT
+	if (((pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED)))
+		|| ((pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE) && (pAd->CommonCfg.BACapability.field.Policy == BA_NOTUSE)))
+	{
+		RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+		Data  &= 0xFFFFFF00;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+
+		RTMP_IO_WRITE32(pAd, PBF_MAX_PCNT, 0x1F3F7F9F);
+		DBGPRINT(RT_DEBUG_TRACE, ("Txburst 1\n"));
+	}
+	else
+#endif // DOT11_N_SUPPORT //
+	if (pAd->CommonCfg.bEnableTxBurst)
+	{
+		RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+		Data  &= 0xFFFFFF00;
+		Data  |= 0x60;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+		pAd->CommonCfg.IOTestParm.bNowAtherosBurstOn = TRUE;
+
+		RTMP_IO_WRITE32(pAd, PBF_MAX_PCNT, 0x1F3FBF9F);
+		DBGPRINT(RT_DEBUG_TRACE, ("Txburst 2\n"));
+	}
+	else
+	{
+		RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+		Data  &= 0xFFFFFF00;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+
+		RTMP_IO_WRITE32(pAd, PBF_MAX_PCNT, 0x1F3F7F9F);
+		DBGPRINT(RT_DEBUG_TRACE, ("Txburst 3\n"));
+	}
+
+#ifdef DOT11_N_SUPPORT
+	// Re-check to turn on TX burst or not.
+	if ((pAd->CommonCfg.IOTestParm.bLastAtheros == TRUE) && ((STA_WEP_ON(pAd))||(STA_TKIP_ON(pAd))))
+	{
+		pAd->CommonCfg.IOTestParm.bNextDisableRxBA = TRUE;
+		if (pAd->CommonCfg.bEnableTxBurst)
+		{
+		    UINT32 MACValue = 0;
+			// Force disable  TXOP value in this case. The same action in MLMEUpdateProtect too.
+			// I didn't change PBF_MAX_PCNT setting.
+			RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &MACValue);
+			MACValue  &= 0xFFFFFF00;
+			RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, MACValue);
+			pAd->CommonCfg.IOTestParm.bNowAtherosBurstOn = FALSE;
+		}
+	}
+	else
+	{
+		pAd->CommonCfg.IOTestParm.bNextDisableRxBA = FALSE;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	pAd->CommonCfg.IOTestParm.bLastAtheros = FALSE;
+	COPY_MAC_ADDR(pAd->CommonCfg.LastBssid, pAd->CommonCfg.Bssid);
+	DBGPRINT(RT_DEBUG_TRACE, ("!!!pAd->bNextDisableRxBA= %d \n", pAd->CommonCfg.IOTestParm.bNextDisableRxBA));
+	// BSSID add in one MAC entry too.  Because in Tx, ASIC need to check Cipher and IV/EIV, BAbitmap
+	// Pther information in MACTab.Content[BSSID_WCID] is not necessary for driver.
+	// Note: As STA, The MACTab.Content[BSSID_WCID]. PairwiseKey and Shared Key for BSS0 are the same.
+
+    if (pAd->StaCfg.WepStatus <= Ndis802_11WEPDisabled)
+    {
+        pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+		pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilterAcceptAll;
+	}
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+	pEntry->PortSecured = pAd->StaCfg.PortSecured;
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+    //
+	// Patch Atheros AP TX will breakdown issue.
+	// AP Model: DLink DWL-8200AP
+	//
+	if (INFRA_ON(pAd) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) && STA_TKIP_ON(pAd))
+	{
+		RTMP_IO_WRITE32(pAd, RX_PARSER_CFG, 0x01);
+	}
+	else
+	{
+		RTMP_IO_WRITE32(pAd, RX_PARSER_CFG, 0x00);
+	}
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	if ((pAd->CommonCfg.BACapability.field.b2040CoexistScanSup) && (pAd->CommonCfg.Channel <= 11))
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_SCAN_2040);
+		BuildEffectedChannelList(pAd);
+	}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+}
+
+/*
+	==========================================================================
+
+	Routine	Description:
+		Disconnect current BSSID
+
+	Arguments:
+		pAd				- Pointer to our adapter
+		IsReqFromAP		- Request from AP
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		We need more information to know it's this requst from AP.
+		If yes! we need to do extra handling, for example, remove the WPA key.
+		Otherwise on 4-way handshaking will faied, since the WPA key didn't be
+		remove while auto reconnect.
+		Disconnect request from AP, it means we will start afresh 4-way handshaking
+		on WPA mode.
+
+	==========================================================================
+*/
+VOID LinkDown(
+	IN PRTMP_ADAPTER pAd,
+	IN  BOOLEAN      IsReqFromAP)
+{
+	UCHAR			    i, ByteValue = 0;
+
+	// Do nothing if monitor mode is on
+	if (MONITOR_ON(pAd))
+		return;
+
+#ifdef RALINK_ATE
+	// Nothing to do in ATE mode.
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+    if (pAd->CommonCfg.bWirelessEvent)
+	{
+		RTMPSendWirelessEvent(pAd, IW_STA_LINKDOWN_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK DOWN !!!\n"));
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+
+	if (ADHOC_ON(pAd))		// Adhoc mode link down
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK DOWN 1!!!\n"));
+
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+		pAd->IndicateMediaState = NdisMediaStateDisconnected;
+		RTMP_IndicateMediaState(pAd);
+        pAd->ExtraInfo = GENERAL_LINK_DOWN;
+		BssTableDeleteEntry(&pAd->ScanTab, pAd->CommonCfg.Bssid, pAd->CommonCfg.Channel);
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! MacTab.Size=%d !!!\n", pAd->MacTab.Size));
+	}
+	else					// Infra structure mode
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK DOWN 2!!!\n"));
+
+#ifdef QOS_DLS_SUPPORT
+		// DLS tear down frame must be sent before link down
+		// send DLS-TEAR_DOWN message
+		if (pAd->CommonCfg.bDLSCapable)
+		{
+			// tear down local dls table entry
+			for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+				{
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				}
+			}
+
+			// tear down peer dls table entry
+			for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status ==  DLS_FINISH))
+				{
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				}
+			}
+		}
+#endif // QOS_DLS_SUPPORT //
+
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_INFRA_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+
+		// Saved last SSID for linkup comparison
+		pAd->CommonCfg.LastSsidLen = pAd->CommonCfg.SsidLen;
+		NdisMoveMemory(pAd->CommonCfg.LastSsid, pAd->CommonCfg.Ssid, pAd->CommonCfg.LastSsidLen);
+		COPY_MAC_ADDR(pAd->CommonCfg.LastBssid, pAd->CommonCfg.Bssid);
+		if (pAd->MlmeAux.CurrReqIsFromNdis == TRUE)
+		{
+			pAd->IndicateMediaState = NdisMediaStateDisconnected;
+			RTMP_IndicateMediaState(pAd);
+            pAd->ExtraInfo = GENERAL_LINK_DOWN;
+			DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event A!\n"));
+			pAd->MlmeAux.CurrReqIsFromNdis = FALSE;
+		}
+		else
+		{
+            //
+			// If disassociation request is from NDIS, then we don't need to delete BSSID from entry.
+			// Otherwise lost beacon or receive De-Authentication from AP,
+			// then we should delete BSSID from BssTable.
+			// If we don't delete from entry, roaming will fail.
+			//
+			BssTableDeleteEntry(&pAd->ScanTab, pAd->CommonCfg.Bssid, pAd->CommonCfg.Channel);
+		}
+
+		// restore back to -
+		//      1. long slot (20 us) or short slot (9 us) time
+		//      2. turn on/off RTS/CTS and/or CTS-to-self protection
+		//      3. short preamble
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED);
+
+		if (pAd->StaCfg.CCXAdjacentAPReportFlag == TRUE)
+		{
+			//
+			// Record current AP's information.
+			// for later used reporting Adjacent AP report.
+			//
+			pAd->StaCfg.CCXAdjacentAPChannel = pAd->CommonCfg.Channel;
+			pAd->StaCfg.CCXAdjacentAPSsidLen = pAd->CommonCfg.SsidLen;
+			NdisMoveMemory(pAd->StaCfg.CCXAdjacentAPSsid, pAd->CommonCfg.Ssid, pAd->StaCfg.CCXAdjacentAPSsidLen);
+			COPY_MAC_ADDR(pAd->StaCfg.CCXAdjacentAPBssid, pAd->CommonCfg.Bssid);
+		}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+		// Country IE of the AP will be evaluated and will be used.
+		if (pAd->StaCfg.IEEE80211dClientMode != Rt802_11_D_None)
+		{
+			NdisMoveMemory(&pAd->CommonCfg.CountryCode[0], &pAd->StaCfg.StaOriCountryCode[0], 2);
+			pAd->CommonCfg.Geography = pAd->StaCfg.StaOriGeography;
+			BuildChannelListEx(pAd);
+		}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	}
+
+	for (i=1; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		if (pAd->MacTab.Content[i].ValidAsCLI == TRUE)
+			MacTableDeleteEntry(pAd, pAd->MacTab.Content[i].Aid, pAd->MacTab.Content[i].Addr);
+	}
+
+	pAd->StaCfg.CCXQosECWMin	= 4;
+	pAd->StaCfg.CCXQosECWMax	= 10;
+
+	AsicSetSlotTime(pAd, TRUE); //FALSE);
+	AsicSetEdcaParm(pAd, NULL);
+
+	// Set LED
+	RTMPSetLED(pAd, LED_LINK_DOWN);
+    pAd->LedIndicatorStregth = 0xF0;
+    RTMPSetSignalLED(pAd, -100);	// Force signal strength Led to be turned off, firmware is not done it.
+
+		AsicDisableSync(pAd);
+
+	pAd->Mlme.PeriodicRound = 0;
+	pAd->Mlme.OneSecPeriodicRound = 0;
+
+	if (pAd->StaCfg.BssType == BSS_INFRA)
+	{
+		// Remove StaCfg Information after link down
+		NdisZeroMemory(pAd->CommonCfg.Bssid, MAC_ADDR_LEN);
+		NdisZeroMemory(pAd->CommonCfg.Ssid, MAX_LEN_OF_SSID);
+		pAd->CommonCfg.SsidLen = 0;
+	}
+#ifdef DOT11_N_SUPPORT
+	NdisZeroMemory(&pAd->MlmeAux.HtCapability, sizeof(HT_CAPABILITY_IE));
+	NdisZeroMemory(&pAd->MlmeAux.AddHtInfo, sizeof(ADD_HT_INFO_IE));
+	pAd->MlmeAux.HtCapabilityLen = 0;
+	pAd->MlmeAux.NewExtChannelOffset = 0xff;
+#endif // DOT11_N_SUPPORT //
+
+	// Reset WPA-PSK state. Only reset when supplicant enabled
+	if (pAd->StaCfg.WpaState != SS_NOTUSE)
+	{
+		pAd->StaCfg.WpaState = SS_START;
+		// Clear Replay counter
+		NdisZeroMemory(pAd->StaCfg.ReplayCounter, 8);
+
+#ifdef QOS_DLS_SUPPORT
+		if (pAd->CommonCfg.bDLSCapable)
+			NdisZeroMemory(pAd->StaCfg.DlsReplayCounter, 8);
+#endif // QOS_DLS_SUPPORT //
+	}
+
+
+	//
+	// if link down come from AP, we need to remove all WPA keys on WPA mode.
+	// otherwise will cause 4-way handshaking failed, since the WPA key not empty.
+	//
+	if ((IsReqFromAP) && (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA))
+	{
+		// Remove all WPA keys
+		RTMPWPARemoveAllKeys(pAd);
+	}
+
+	// 802.1x port control
+#ifdef WPA_SUPPLICANT_SUPPORT
+	// Prevent clear PortSecured here with static WEP
+	// NetworkManger set security policy first then set SSID to connect AP.
+	if (pAd->StaCfg.WpaSupplicantUP &&
+		(pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled) &&
+		(pAd->StaCfg.IEEE8021X == FALSE))
+	{
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+	}
+	else
+#endif // WPA_SUPPLICANT_SUPPORT //
+	{
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+		pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilter8021xWEP;
+	}
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+	pAd->MacTab.Content[BSSID_WCID].PortSecured = pAd->StaCfg.PortSecured;
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+	pAd->StaCfg.MicErrCnt = 0;
+
+	// Turn off Ckip control flag
+	pAd->StaCfg.bCkipOn = FALSE;
+	pAd->StaCfg.CCXEnable = FALSE;
+
+    pAd->IndicateMediaState = NdisMediaStateDisconnected;
+	// Update extra information to link is up
+	pAd->ExtraInfo = GENERAL_LINK_DOWN;
+
+    //pAd->StaCfg.AdhocBOnlyJoined = FALSE;
+	//pAd->StaCfg.AdhocBGJoined = FALSE;
+	//pAd->StaCfg.Adhoc20NJoined = FALSE;
+    pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+
+	// Reset the Current AP's IP address
+	NdisZeroMemory(pAd->StaCfg.AironetIPAddress, 4);
+#ifdef RT2870
+	pAd->bUsbTxBulkAggre = FALSE;
+#endif // RT2870 //
+
+	// Clean association information
+	NdisZeroMemory(&pAd->StaCfg.AssocInfo, sizeof(NDIS_802_11_ASSOCIATION_INFORMATION));
+	pAd->StaCfg.AssocInfo.Length = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+	pAd->StaCfg.ReqVarIELen = 0;
+	pAd->StaCfg.ResVarIELen = 0;
+
+	//
+	// Reset RSSI value after link down
+	//
+	pAd->StaCfg.RssiSample.AvgRssi0 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi0X8 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi1 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi1X8 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi2 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi2X8 = 0;
+
+	// Restore MlmeRate
+	pAd->CommonCfg.MlmeRate = pAd->CommonCfg.BasicMlmeRate;
+	pAd->CommonCfg.RtsRate = pAd->CommonCfg.BasicMlmeRate;
+
+#ifdef DOT11_N_SUPPORT
+	//
+	// After Link down, reset piggy-back setting in ASIC. Disable RDG.
+	//
+	if (pAd->CommonCfg.BBPCurrentBW == BW_40)
+	{
+		pAd->CommonCfg.BBPCurrentBW = BW_20;
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &ByteValue);
+		ByteValue &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, ByteValue);
+	}
+#endif // DOT11_N_SUPPORT //
+	// Reset DAC
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &ByteValue);
+	ByteValue &= (~0x18);
+	if (pAd->Antenna.field.TxPath == 2)
+	{
+		ByteValue |= 0x10;
+	}
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, ByteValue);
+
+	RTMPSetPiggyBack(pAd,FALSE);
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_PIGGYBACK_INUSED);
+
+#ifdef DOT11_N_SUPPORT
+	pAd->CommonCfg.BACapability.word = pAd->CommonCfg.REGBACapability.word;
+#endif // DOT11_N_SUPPORT //
+
+	// Restore all settings in the following.
+	AsicUpdateProtect(pAd, 0, (ALLN_SETPROTECT|CCKSETPROTECT|OFDMSETPROTECT), TRUE, FALSE);
+	AsicDisableRDG(pAd);
+	pAd->CommonCfg.IOTestParm.bCurrentAtheros = FALSE;
+	pAd->CommonCfg.IOTestParm.bNowAtherosBurstOn = FALSE;
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SCAN_2040);
+	pAd->CommonCfg.BSSCoexist2040.word = 0;
+	TriEventInit(pAd);
+	for (i = 0; i < (pAd->ChannelListNum - 1); i++)
+	{
+		pAd->ChannelList[i].bEffectedChannel = FALSE;
+	}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+	RTMP_IO_WRITE32(pAd, MAX_LEN_CFG, 0x1fff);
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+	if (pAd->StaCfg.WpaSupplicantUP) {
+		union iwreq_data    wrqu;
+		//send disassociate event to wpa_supplicant
+		memset(&wrqu, 0, sizeof(wrqu));
+		wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+		wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+	}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+	{
+		union iwreq_data    wrqu;
+		memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+		wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+	}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID IterateOnBssTab(
+	IN PRTMP_ADAPTER pAd)
+{
+	MLME_START_REQ_STRUCT   StartReq;
+	MLME_JOIN_REQ_STRUCT    JoinReq;
+	ULONG                   BssIdx;
+
+	// Change the wepstatus to original wepstatus
+	pAd->StaCfg.WepStatus   = pAd->StaCfg.OrigWepStatus;
+	pAd->StaCfg.PairCipher  = pAd->StaCfg.OrigWepStatus;
+	pAd->StaCfg.GroupCipher = pAd->StaCfg.OrigWepStatus;
+
+	BssIdx = pAd->MlmeAux.BssIdx;
+	if (BssIdx < pAd->MlmeAux.SsidBssTab.BssNr)
+	{
+		// Check cipher suite, AP must have more secured cipher than station setting
+		// Set the Pairwise and Group cipher to match the intended AP setting
+		// We can only connect to AP with less secured cipher setting
+		if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+		{
+			pAd->StaCfg.GroupCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.GroupCipher;
+
+			if (pAd->StaCfg.WepStatus == pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipher)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipher;
+			else if (pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipherAux != Ndis802_11WEPDisabled)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipherAux;
+			else	// There is no PairCipher Aux, downgrade our capability to TKIP
+				pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+		}
+		else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+		{
+			pAd->StaCfg.GroupCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.GroupCipher;
+
+			if (pAd->StaCfg.WepStatus == pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipher)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipher;
+			else if (pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipherAux != Ndis802_11WEPDisabled)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipherAux;
+			else	// There is no PairCipher Aux, downgrade our capability to TKIP
+				pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+
+			// RSN capability
+			pAd->StaCfg.RsnCapability = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.RsnCapability;
+		}
+
+		// Set Mix cipher flag
+		pAd->StaCfg.bMixCipher = (pAd->StaCfg.PairCipher == pAd->StaCfg.GroupCipher) ? FALSE : TRUE;
+		if (pAd->StaCfg.bMixCipher == TRUE)
+		{
+			// If mix cipher, re-build RSNIE
+			RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, 0);
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - iterate BSS %ld of %d\n", BssIdx, pAd->MlmeAux.SsidBssTab.BssNr));
+		JoinParmFill(pAd, &JoinReq, BssIdx);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_JOIN_REQ, sizeof(MLME_JOIN_REQ_STRUCT),
+					&JoinReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_JOIN;
+	}
+	else if (pAd->StaCfg.BssType == BSS_ADHOC)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - All BSS fail; start a new ADHOC (Ssid=%s)...\n",pAd->MlmeAux.Ssid));
+		StartParmFill(pAd, &StartReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_START_REQ, sizeof(MLME_START_REQ_STRUCT), &StartReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_START;
+	}
+	else // no more BSS
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - All roaming failed, stay @ ch #%d\n", pAd->CommonCfg.Channel));
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+	}
+}
+
+// for re-association only
+// IRQL = DISPATCH_LEVEL
+VOID IterateOnBssTab2(
+	IN PRTMP_ADAPTER pAd)
+{
+	MLME_REASSOC_REQ_STRUCT ReassocReq;
+	ULONG                   BssIdx;
+	BSS_ENTRY               *pBss;
+
+	BssIdx = pAd->MlmeAux.RoamIdx;
+	pBss = &pAd->MlmeAux.RoamTab.BssEntry[BssIdx];
+
+	if (BssIdx < pAd->MlmeAux.RoamTab.BssNr)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - iterate BSS %ld of %d\n", BssIdx, pAd->MlmeAux.RoamTab.BssNr));
+
+		AsicSwitchChannel(pAd, pBss->Channel, FALSE);
+		AsicLockChannel(pAd, pBss->Channel);
+
+		// reassociate message has the same structure as associate message
+		AssocParmFill(pAd, &ReassocReq, pBss->Bssid, pBss->CapabilityInfo,
+					  ASSOC_TIMEOUT, pAd->StaCfg.DefaultListenCount);
+		MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_REASSOC_REQ,
+					sizeof(MLME_REASSOC_REQ_STRUCT), &ReassocReq);
+
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_REASSOC;
+	}
+	else // no more BSS
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - All fast roaming failed, back to ch #%d\n",pAd->CommonCfg.Channel));
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID JoinParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_JOIN_REQ_STRUCT *JoinReq,
+	IN ULONG BssIdx)
+{
+	JoinReq->BssIdx = BssIdx;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID ScanParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_SCAN_REQ_STRUCT *ScanReq,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN UCHAR ScanType)
+{
+    NdisZeroMemory(ScanReq->Ssid, MAX_LEN_OF_SSID);
+	ScanReq->SsidLen = SsidLen;
+	NdisMoveMemory(ScanReq->Ssid, Ssid, SsidLen);
+	ScanReq->BssType = BssType;
+	ScanReq->ScanType = ScanType;
+}
+
+#ifdef QOS_DLS_SUPPORT
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID DlsParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_DLS_REQ_STRUCT *pDlsReq,
+	IN PRT_802_11_DLS pDls,
+	IN USHORT reason)
+{
+	pDlsReq->pDLS = pDls;
+	pDlsReq->Reason = reason;
+}
+#endif // QOS_DLS_SUPPORT //
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID StartParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_START_REQ_STRUCT *StartReq,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen)
+{
+	ASSERT(SsidLen <= MAX_LEN_OF_SSID);
+	NdisMoveMemory(StartReq->Ssid, Ssid, SsidLen);
+	StartReq->SsidLen = SsidLen;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID AuthParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_AUTH_REQ_STRUCT *AuthReq,
+	IN PUCHAR pAddr,
+	IN USHORT Alg)
+{
+	COPY_MAC_ADDR(AuthReq->Addr, pAddr);
+	AuthReq->Alg = Alg;
+	AuthReq->Timeout = AUTH_TIMEOUT;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+
+
+#ifdef RT2870
+
+VOID MlmeCntlConfirm(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG MsgType,
+	IN USHORT Msg)
+{
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MsgType, sizeof(USHORT), &Msg);
+}
+
+VOID ComposePsPoll(
+	IN PRTMP_ADAPTER pAd)
+{
+	PTXINFO_STRUC		pTxInfo;
+	PTXWI_STRUC		pTxWI;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ComposePsPoll\n"));
+	NdisZeroMemory(&pAd->PsPollFrame, sizeof(PSPOLL_FRAME));
+
+	pAd->PsPollFrame.FC.PwrMgmt = 0;
+	pAd->PsPollFrame.FC.Type = BTYPE_CNTL;
+	pAd->PsPollFrame.FC.SubType = SUBTYPE_PS_POLL;
+	pAd->PsPollFrame.Aid = pAd->StaActive.Aid | 0xC000;
+	COPY_MAC_ADDR(pAd->PsPollFrame.Bssid, pAd->CommonCfg.Bssid);
+	COPY_MAC_ADDR(pAd->PsPollFrame.Ta, pAd->CurrentAddress);
+
+	RTMPZeroMemory(&pAd->PsPollContext.TransferBuffer->field.WirelessPacket[0], 100);
+	pTxInfo = (PTXINFO_STRUC)&pAd->PsPollContext.TransferBuffer->field.WirelessPacket[0];
+	RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(sizeof(PSPOLL_FRAME)+TXWI_SIZE), TRUE, EpToQueue[MGMTPIPEIDX], FALSE,  FALSE);
+	pTxWI = (PTXWI_STRUC)&pAd->PsPollContext.TransferBuffer->field.WirelessPacket[TXINFO_SIZE];
+	RTMPWriteTxWI(pAd, pTxWI, FALSE, FALSE, FALSE, FALSE, TRUE, FALSE, 0, BSSID_WCID, (sizeof(PSPOLL_FRAME)),
+		0,  0, (UCHAR)pAd->CommonCfg.MlmeTransmit.field.MCS, IFS_BACKOFF, FALSE, &pAd->CommonCfg.MlmeTransmit);
+	RTMPMoveMemory(&pAd->PsPollContext.TransferBuffer->field.WirelessPacket[TXWI_SIZE+TXINFO_SIZE], &pAd->PsPollFrame, sizeof(PSPOLL_FRAME));
+	// Append 4 extra zero bytes.
+	pAd->PsPollContext.BulkOutSize =  TXINFO_SIZE + TXWI_SIZE + sizeof(PSPOLL_FRAME) + 4;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID ComposeNullFrame(
+	IN PRTMP_ADAPTER pAd)
+{
+	PTXINFO_STRUC		pTxInfo;
+	PTXWI_STRUC		pTxWI;
+
+	NdisZeroMemory(&pAd->NullFrame, sizeof(HEADER_802_11));
+	pAd->NullFrame.FC.Type = BTYPE_DATA;
+	pAd->NullFrame.FC.SubType = SUBTYPE_NULL_FUNC;
+	pAd->NullFrame.FC.ToDs = 1;
+	COPY_MAC_ADDR(pAd->NullFrame.Addr1, pAd->CommonCfg.Bssid);
+	COPY_MAC_ADDR(pAd->NullFrame.Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pAd->NullFrame.Addr3, pAd->CommonCfg.Bssid);
+	RTMPZeroMemory(&pAd->NullContext.TransferBuffer->field.WirelessPacket[0], 100);
+	pTxInfo = (PTXINFO_STRUC)&pAd->NullContext.TransferBuffer->field.WirelessPacket[0];
+	RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(sizeof(HEADER_802_11)+TXWI_SIZE), TRUE, EpToQueue[MGMTPIPEIDX], FALSE,  FALSE);
+	pTxWI = (PTXWI_STRUC)&pAd->NullContext.TransferBuffer->field.WirelessPacket[TXINFO_SIZE];
+	RTMPWriteTxWI(pAd, pTxWI, FALSE, FALSE, FALSE, FALSE, TRUE, FALSE, 0, BSSID_WCID, (sizeof(HEADER_802_11)),
+		0, 0, (UCHAR)pAd->CommonCfg.MlmeTransmit.field.MCS, IFS_BACKOFF, FALSE, &pAd->CommonCfg.MlmeTransmit);
+	RTMPMoveMemory(&pAd->NullContext.TransferBuffer->field.WirelessPacket[TXWI_SIZE+TXINFO_SIZE], &pAd->NullFrame, sizeof(HEADER_802_11));
+	pAd->NullContext.BulkOutSize =  TXINFO_SIZE + TXWI_SIZE + sizeof(pAd->NullFrame) + 4;
+}
+#endif // RT2870 //
+
+
+/*
+	==========================================================================
+	Description:
+		Pre-build a BEACON frame in the shared memory
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+ULONG MakeIbssBeacon(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR         DsLen = 1, IbssLen = 2;
+	UCHAR         LocalErpIe[3] = {IE_ERP, 1, 0x04};
+	HEADER_802_11 BcnHdr;
+	USHORT        CapabilityInfo;
+	LARGE_INTEGER FakeTimestamp;
+	ULONG         FrameLen = 0;
+	PTXWI_STRUC	  pTxWI = &pAd->BeaconTxWI;
+	CHAR         *pBeaconFrame = pAd->BeaconBuf;
+	BOOLEAN       Privacy;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR         SupRateLen = 0;
+	UCHAR         ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR         ExtRateLen = 0;
+	UCHAR         RSNIe = IE_WPA;
+
+	if ((pAd->CommonCfg.PhyMode == PHY_11B) && (pAd->CommonCfg.Channel <= 14))
+	{
+		SupRate[0] = 0x82; // 1 mbps
+		SupRate[1] = 0x84; // 2 mbps
+		SupRate[2] = 0x8b; // 5.5 mbps
+		SupRate[3] = 0x96; // 11 mbps
+		SupRateLen = 4;
+		ExtRateLen = 0;
+	}
+	else if (pAd->CommonCfg.Channel > 14)
+	{
+		SupRate[0]  = 0x8C;    // 6 mbps, in units of 0.5 Mbps, basic rate
+		SupRate[1]  = 0x12;    // 9 mbps, in units of 0.5 Mbps
+		SupRate[2]  = 0x98;    // 12 mbps, in units of 0.5 Mbps, basic rate
+		SupRate[3]  = 0x24;    // 18 mbps, in units of 0.5 Mbps
+		SupRate[4]  = 0xb0;    // 24 mbps, in units of 0.5 Mbps, basic rate
+		SupRate[5]  = 0x48;    // 36 mbps, in units of 0.5 Mbps
+		SupRate[6]  = 0x60;    // 48 mbps, in units of 0.5 Mbps
+		SupRate[7]  = 0x6c;    // 54 mbps, in units of 0.5 Mbps
+		SupRateLen  = 8;
+		ExtRateLen  = 0;
+
+		//
+		// Also Update MlmeRate & RtsRate for G only & A only
+		//
+		pAd->CommonCfg.MlmeRate = RATE_6;
+		pAd->CommonCfg.RtsRate = RATE_6;
+		pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+		pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MODE = MODE_OFDM;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+	}
+	else
+	{
+		SupRate[0] = 0x82; // 1 mbps
+		SupRate[1] = 0x84; // 2 mbps
+		SupRate[2] = 0x8b; // 5.5 mbps
+		SupRate[3] = 0x96; // 11 mbps
+		SupRateLen = 4;
+
+		ExtRate[0]  = 0x0C;    // 6 mbps, in units of 0.5 Mbps,
+		ExtRate[1]  = 0x12;    // 9 mbps, in units of 0.5 Mbps
+		ExtRate[2]  = 0x18;    // 12 mbps, in units of 0.5 Mbps,
+		ExtRate[3]  = 0x24;    // 18 mbps, in units of 0.5 Mbps
+		ExtRate[4]  = 0x30;    // 24 mbps, in units of 0.5 Mbps,
+		ExtRate[5]  = 0x48;    // 36 mbps, in units of 0.5 Mbps
+		ExtRate[6]  = 0x60;    // 48 mbps, in units of 0.5 Mbps
+		ExtRate[7]  = 0x6c;    // 54 mbps, in units of 0.5 Mbps
+		ExtRateLen  = 8;
+	}
+
+	pAd->StaActive.SupRateLen = SupRateLen;
+	NdisMoveMemory(pAd->StaActive.SupRate, SupRate, SupRateLen);
+	pAd->StaActive.ExtRateLen = ExtRateLen;
+	NdisMoveMemory(pAd->StaActive.ExtRate, ExtRate, ExtRateLen);
+
+	// compose IBSS beacon frame
+	MgtMacHeaderInit(pAd, &BcnHdr, SUBTYPE_BEACON, 0, BROADCAST_ADDR, pAd->CommonCfg.Bssid);
+	Privacy = (pAd->StaCfg.WepStatus == Ndis802_11Encryption1Enabled) ||
+			  (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+			  (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled);
+	CapabilityInfo = CAP_GENERATE(0, 1, Privacy, (pAd->CommonCfg.TxPreamble == Rt802_11PreambleShort), 0, 0);
+
+	MakeOutgoingFrame(pBeaconFrame,                &FrameLen,
+					  sizeof(HEADER_802_11),           &BcnHdr,
+					  TIMESTAMP_LEN,                   &FakeTimestamp,
+					  2,                               &pAd->CommonCfg.BeaconPeriod,
+					  2,                               &CapabilityInfo,
+					  1,                               &SsidIe,
+					  1,                               &pAd->CommonCfg.SsidLen,
+					  pAd->CommonCfg.SsidLen,          pAd->CommonCfg.Ssid,
+					  1,                               &SupRateIe,
+					  1,                               &SupRateLen,
+					  SupRateLen,                      SupRate,
+					  1,                               &DsIe,
+					  1,                               &DsLen,
+					  1,                               &pAd->CommonCfg.Channel,
+					  1,                               &IbssIe,
+					  1,                               &IbssLen,
+					  2,                               &pAd->StaActive.AtimWin,
+					  END_OF_ARGS);
+
+	// add ERP_IE and EXT_RAE IE of in 802.11g
+	if (ExtRateLen)
+	{
+		ULONG	tmp;
+
+		MakeOutgoingFrame(pBeaconFrame + FrameLen,         &tmp,
+						  3,                               LocalErpIe,
+						  1,                               &ExtRateIe,
+						  1,                               &ExtRateLen,
+						  ExtRateLen,                      ExtRate,
+						  END_OF_ARGS);
+		FrameLen += tmp;
+	}
+
+	// If adhoc secruity is set for WPA-None, append the cipher suite IE
+	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+	{
+		ULONG tmp;
+        RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, BSS0);
+
+		MakeOutgoingFrame(pBeaconFrame + FrameLen,        	&tmp,
+						  1,                              	&RSNIe,
+						  1,                            	&pAd->StaCfg.RSNIE_Len,
+						  pAd->StaCfg.RSNIE_Len,      		pAd->StaCfg.RSN_IE,
+						  END_OF_ARGS);
+		FrameLen += tmp;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+	{
+		ULONG TmpLen;
+		UCHAR HtLen, HtLen1;
+
+#ifdef RT_BIG_ENDIAN
+		HT_CAPABILITY_IE HtCapabilityTmp;
+		ADD_HT_INFO_IE	addHTInfoTmp;
+		USHORT	b2lTmp, b2lTmp2;
+#endif
+
+		// add HT Capability IE
+		HtLen = sizeof(pAd->CommonCfg.HtCapability);
+		HtLen1 = sizeof(pAd->CommonCfg.AddHTInfo);
+#ifndef RT_BIG_ENDIAN
+		MakeOutgoingFrame(pBeaconFrame+FrameLen,	&TmpLen,
+						  1,						&HtCapIe,
+						  1,						&HtLen,
+						  HtLen,					&pAd->CommonCfg.HtCapability,
+						  1,						&AddHtInfoIe,
+						  1,						&HtLen1,
+						  HtLen1,					&pAd->CommonCfg.AddHTInfo,
+						  END_OF_ARGS);
+#else
+		NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, HtLen);
+		*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+		*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+		NdisMoveMemory(&addHTInfoTmp, &pAd->CommonCfg.AddHTInfo, HtLen1);
+		*(USHORT *)(&addHTInfoTmp.AddHtInfo2) = SWAP16(*(USHORT *)(&addHTInfoTmp.AddHtInfo2));
+		*(USHORT *)(&addHTInfoTmp.AddHtInfo3) = SWAP16(*(USHORT *)(&addHTInfoTmp.AddHtInfo3));
+
+		MakeOutgoingFrame(pBeaconFrame+FrameLen,	&TmpLen,
+						  1,						&HtCapIe,
+						  1,						&HtLen,
+						  HtLen,					&HtCapabilityTmp,
+						  1,						&AddHtInfoIe,
+						  1,						&HtLen1,
+						  HtLen1,					&addHTInfoTmp,
+						  END_OF_ARGS);
+#endif
+		FrameLen += TmpLen;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	//beacon use reserved WCID 0xff
+    if (pAd->CommonCfg.Channel > 14)
+    {
+		RTMPWriteTxWI(pAd, pTxWI, FALSE, FALSE,  TRUE, FALSE, FALSE, TRUE, 0, 0xff, FrameLen,
+			PID_MGMT, PID_BEACON, RATE_1, IFS_HTTXOP, FALSE, &pAd->CommonCfg.MlmeTransmit);
+    }
+    else
+    {
+        // Set to use 1Mbps for Adhoc beacon.
+		HTTRANSMIT_SETTING Transmit;
+        Transmit.word = 0;
+        RTMPWriteTxWI(pAd, pTxWI, FALSE, FALSE,  TRUE, FALSE, FALSE, TRUE, 0, 0xff, FrameLen,
+    		PID_MGMT, PID_BEACON, RATE_1, IFS_HTTXOP, FALSE, &Transmit);
+    }
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, pBeaconFrame, DIR_WRITE, FALSE);
+	RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+#endif
+
+    DBGPRINT(RT_DEBUG_TRACE, ("MakeIbssBeacon (len=%ld), SupRateLen=%d, ExtRateLen=%d, Channel=%d, PhyMode=%d\n",
+					FrameLen, SupRateLen, ExtRateLen, pAd->CommonCfg.Channel, pAd->CommonCfg.PhyMode));
+	return FrameLen;
+}
+
+
diff --git a/drivers/staging/rt2870/sta/dls.c b/drivers/staging/rt2870/sta/dls.c
new file mode 100644
index 000000000000..56bfbc33d6f5
--- /dev/null
+++ b/drivers/staging/rt2870/sta/dls.c
@@ -0,0 +1,2210 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    dls.c
+
+    Abstract:
+    Handle WMM-DLS state machine
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Rory Chen   02-14-2006
+	Arvin Tai	06-03-2008	  Modified for RT28xx
+ */
+
+#include "../rt_config.h"
+
+/*
+    ==========================================================================
+    Description:
+        dls state machine init, including state transition and timer init
+    Parameters:
+        Sm - pointer to the dls state machine
+    Note:
+        The state machine looks like this
+
+                            DLS_IDLE
+    MT2_MLME_DLS_REQUEST   MlmeDlsReqAction
+    MT2_PEER_DLS_REQUEST   PeerDlsReqAction
+    MT2_PEER_DLS_RESPONSE  PeerDlsRspAction
+    MT2_MLME_DLS_TEARDOWN  MlmeTearDownAction
+    MT2_PEER_DLS_TEARDOWN  PeerTearDownAction
+
+	IRQL = PASSIVE_LEVEL
+
+    ==========================================================================
+ */
+void DlsStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[])
+{
+	UCHAR	i;
+
+    StateMachineInit(Sm, (STATE_MACHINE_FUNC*)Trans, MAX_DLS_STATE, MAX_DLS_MSG, (STATE_MACHINE_FUNC)Drop, DLS_IDLE, DLS_MACHINE_BASE);
+
+    // the first column
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_MLME_DLS_REQ, (STATE_MACHINE_FUNC)MlmeDlsReqAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_PEER_DLS_REQ, (STATE_MACHINE_FUNC)PeerDlsReqAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_PEER_DLS_RSP, (STATE_MACHINE_FUNC)PeerDlsRspAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_MLME_DLS_TEAR_DOWN, (STATE_MACHINE_FUNC)MlmeDlsTearDownAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_PEER_DLS_TEAR_DOWN, (STATE_MACHINE_FUNC)PeerDlsTearDownAction);
+
+	for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		pAd->StaCfg.DLSEntry[i].pAd = pAd;
+		RTMPInitTimer(pAd, &pAd->StaCfg.DLSEntry[i].Timer, GET_TIMER_FUNCTION(DlsTimeoutAction), pAd, FALSE);
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDlsReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	HEADER_802_11	DlsReqHdr;
+	PRT_802_11_DLS	pDLS = NULL;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_REQUEST;
+	ULONG			tmp;
+	USHORT			reason;
+	ULONG			Timeout;
+	BOOLEAN			TimerCancelled;
+
+	if(!MlmeDlsReqSanity(pAd, Elem->Msg, Elem->MsgLen, &pDLS, &reason))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - MlmeDlsReqAction() \n"));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("DLS - MlmeDlsReqAction() allocate memory failed \n"));
+		return;
+	}
+
+	ActHeaderInit(pAd, &DlsReqHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+
+	// Build basic frame first
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+					sizeof(HEADER_802_11),		&DlsReqHdr,
+					1,							&Category,
+					1,							&Action,
+					6,							&pDLS->MacAddr,
+					6,							pAd->CurrentAddress,
+					2,							&pAd->StaActive.CapabilityInfo,
+					2,							&pDLS->TimeOut,
+					1,							&SupRateIe,
+					1,							&pAd->MlmeAux.SupRateLen,
+					pAd->MlmeAux.SupRateLen,	pAd->MlmeAux.SupRate,
+					END_OF_ARGS);
+
+	if (pAd->MlmeAux.ExtRateLen != 0)
+	{
+		MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+						  1,						&ExtRateIe,
+						  1,						&pAd->MlmeAux.ExtRateLen,
+						  pAd->MlmeAux.ExtRateLen,	pAd->MlmeAux.ExtRate,
+						  END_OF_ARGS);
+		FrameLen += tmp;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+	{
+		UCHAR HtLen;
+
+#ifdef RT_BIG_ENDIAN
+		HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+
+		// add HT Capability IE
+		HtLen = sizeof(HT_CAPABILITY_IE);
+#ifndef RT_BIG_ENDIAN
+		MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+							1,						&HtCapIe,
+							1,						&HtLen,
+							HtLen,					&pAd->CommonCfg.HtCapability,
+							END_OF_ARGS);
+#else
+		NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, HtLen);
+							*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+							*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+		MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+							1,						&HtCapIe,
+							1,						&HtLen,
+							HtLen,					&HtCapabilityTmp,
+							END_OF_ARGS);
+#endif
+		FrameLen = FrameLen + tmp;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	RTMPCancelTimer(&pDLS->Timer, &TimerCancelled);
+	Timeout = DLS_TIMEOUT;
+	RTMPSetTimer(&pDLS->Timer, Timeout);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerDlsReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	USHORT			StatusCode = MLME_SUCCESS;
+	HEADER_802_11	DlsRspHdr;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_RESPONSE;
+	ULONG			tmp;
+	USHORT			CapabilityInfo;
+	UCHAR			DA[MAC_ADDR_LEN], SA[MAC_ADDR_LEN];
+	USHORT			DLSTimeOut;
+	SHORT			i;
+	ULONG			Timeout;
+	BOOLEAN			TimerCancelled;
+	PRT_802_11_DLS	pDLS = NULL;
+	UCHAR			MaxSupportedRateIn500Kbps = 0;
+    UCHAR			SupportedRatesLen;
+    UCHAR			SupportedRates[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR			HtCapabilityLen;
+	HT_CAPABILITY_IE	HtCapability;
+
+	if (!PeerDlsReqSanity(pAd, Elem->Msg, Elem->MsgLen, DA, SA, &CapabilityInfo, &DLSTimeOut,
+							&SupportedRatesLen, &SupportedRates[0], &HtCapabilityLen, &HtCapability))
+		return;
+
+    // supported rates array may not be sorted. sort it and find the maximum rate
+    for (i = 0; i < SupportedRatesLen; i++)
+    {
+        if (MaxSupportedRateIn500Kbps < (SupportedRates[i] & 0x7f))
+            MaxSupportedRateIn500Kbps = SupportedRates[i] & 0x7f;
+    }
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsReqAction() from %02x:%02x:%02x:%02x:%02x:%02x\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsReqAction() allocate memory failed \n"));
+		return;
+	}
+
+	if (!INFRA_ON(pAd))
+	{
+		StatusCode = MLME_REQUEST_DECLINED;
+	}
+	else if (!pAd->CommonCfg.bWmmCapable)
+	{
+		StatusCode = MLME_DEST_STA_IS_NOT_A_QSTA;
+	}
+	else if (!pAd->CommonCfg.bDLSCapable)
+	{
+		StatusCode = MLME_REQUEST_DECLINED;
+	}
+	else
+	{
+		// find table to update parameters
+		for (i = (MAX_NUM_OF_DLS_ENTRY-1); i >= 0; i--)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+					pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+				else
+				{
+					RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+					pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+				}
+
+				pAd->StaCfg.DLSEntry[i].Sequence = 0;
+				pAd->StaCfg.DLSEntry[i].TimeOut = DLSTimeOut;
+				pAd->StaCfg.DLSEntry[i].CountDownTimer = DLSTimeOut;
+				if (HtCapabilityLen != 0)
+					pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+				else
+					pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+				pDLS = &pAd->StaCfg.DLSEntry[i];
+				break;
+			}
+		}
+
+		// can not find in table, create a new one
+		if (i < 0)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsReqAction() can not find same entry \n"));
+			for (i=(MAX_NUM_OF_DLS_ENTRY - 1); i >= MAX_NUM_OF_INIT_DLS_ENTRY; i--)
+			{
+				if (!pAd->StaCfg.DLSEntry[i].Valid)
+				{
+					MAC_TABLE_ENTRY *pEntry;
+					UCHAR MaxSupportedRate = RATE_11;
+
+					if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+					{
+						pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+					}
+					else
+					{
+						RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+						pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+					}
+
+					pAd->StaCfg.DLSEntry[i].Sequence = 0;
+					pAd->StaCfg.DLSEntry[i].Valid = TRUE;
+					pAd->StaCfg.DLSEntry[i].TimeOut = DLSTimeOut;
+					pAd->StaCfg.DLSEntry[i].CountDownTimer = DLSTimeOut;
+					NdisMoveMemory(pAd->StaCfg.DLSEntry[i].MacAddr, SA, MAC_ADDR_LEN);
+					if (HtCapabilityLen != 0)
+						pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+					else
+						pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+					pDLS = &pAd->StaCfg.DLSEntry[i];
+					pEntry = MacTableInsertDlsEntry(pAd, SA, i);
+
+					switch (MaxSupportedRateIn500Kbps)
+					{
+						case 108: MaxSupportedRate = RATE_54;   break;
+						case 96:  MaxSupportedRate = RATE_48;   break;
+						case 72:  MaxSupportedRate = RATE_36;   break;
+						case 48:  MaxSupportedRate = RATE_24;   break;
+						case 36:  MaxSupportedRate = RATE_18;   break;
+						case 24:  MaxSupportedRate = RATE_12;   break;
+						case 18:  MaxSupportedRate = RATE_9;    break;
+						case 12:  MaxSupportedRate = RATE_6;    break;
+						case 22:  MaxSupportedRate = RATE_11;   break;
+						case 11:  MaxSupportedRate = RATE_5_5;  break;
+						case 4:   MaxSupportedRate = RATE_2;    break;
+						case 2:   MaxSupportedRate = RATE_1;    break;
+						default:  MaxSupportedRate = RATE_11;   break;
+					}
+
+					pEntry->MaxSupportedRate = min(pAd->CommonCfg.MaxTxRate, MaxSupportedRate);
+
+					if (pEntry->MaxSupportedRate < RATE_FIRST_OFDM_RATE)
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MaxHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->MinHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MinHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->HTPhyMode.field.MODE = MODE_CCK;
+						pEntry->HTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MaxHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->MinHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MinHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					}
+
+					pEntry->MaxHTPhyMode.field.BW = BW_20;
+					pEntry->MinHTPhyMode.field.BW = BW_20;
+
+#ifdef DOT11_N_SUPPORT
+					pEntry->HTCapability.MCSSet[0] = 0;
+					pEntry->HTCapability.MCSSet[1] = 0;
+
+					// If this Entry supports 802.11n, upgrade to HT rate.
+					if ((HtCapabilityLen != 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+					{
+						UCHAR	j, bitmask; //k,bitmask;
+						CHAR    ii;
+
+						DBGPRINT(RT_DEBUG_OFF, ("DLS - PeerDlsReqAction() Receive Peer HT Capable STA from %02x:%02x:%02x:%02x:%02x:%02x\n",
+									SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+						if ((HtCapability.HtCapInfo.GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTGREENFIELD;
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+							pAd->MacTab.fAnyStationNonGF = TRUE;
+							pAd->CommonCfg.AddHTInfo.AddHtInfo2.NonGfPresent = 1;
+						}
+
+						if ((HtCapability.HtCapInfo.ChannelWidth) && (pAd->CommonCfg.DesiredHtPhy.ChannelWidth))
+						{
+							pEntry->MaxHTPhyMode.field.BW= BW_40;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor40)&(HtCapability.HtCapInfo.ShortGIfor40));
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.BW = BW_20;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor20)&(HtCapability.HtCapInfo.ShortGIfor20));
+							pAd->MacTab.fAnyStation20Only = TRUE;
+						}
+
+						// find max fixed rate
+						for (ii=15; ii>=0; ii--)
+						{
+							j = ii/8;
+							bitmask = (1<<(ii-(j*8)));
+							if ( (pAd->StaCfg.DesiredHtPhyInfo.MCSSet[j]&bitmask) && (HtCapability.MCSSet[j]&bitmask))
+							{
+								pEntry->MaxHTPhyMode.field.MCS = ii;
+								break;
+							}
+							if (ii==0)
+								break;
+						}
+
+
+						if (pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO)
+						{
+
+							printk("@@@ pAd->CommonCfg.RegTransmitSetting.field.MCS = %d\n",
+								pAd->StaCfg.DesiredTransmitSetting.field.MCS);
+							if (pAd->StaCfg.DesiredTransmitSetting.field.MCS == 32)
+							{
+								// Fix MCS as HT Duplicated Mode
+								pEntry->MaxHTPhyMode.field.BW = 1;
+								pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+								pEntry->MaxHTPhyMode.field.STBC = 0;
+								pEntry->MaxHTPhyMode.field.ShortGI = 0;
+								pEntry->MaxHTPhyMode.field.MCS = 32;
+							}
+							else if (pEntry->MaxHTPhyMode.field.MCS > pAd->StaCfg.HTPhyMode.field.MCS)
+							{
+								// STA supports fixed MCS
+								pEntry->MaxHTPhyMode.field.MCS = pAd->StaCfg.HTPhyMode.field.MCS;
+							}
+						}
+
+						pEntry->MaxHTPhyMode.field.STBC = (HtCapability.HtCapInfo.RxSTBC & (pAd->CommonCfg.DesiredHtPhy.TxSTBC));
+						pEntry->MpduDensity = HtCapability.HtCapParm.MpduDensity;
+						pEntry->MaxRAmpduFactor = HtCapability.HtCapParm.MaxRAmpduFactor;
+						pEntry->MmpsMode = (UCHAR)HtCapability.HtCapInfo.MimoPs;
+						pEntry->AMsduSize = (UCHAR)HtCapability.HtCapInfo.AMsduSize;
+						pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+
+						if (HtCapability.HtCapInfo.ShortGIfor20)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE);
+						if (HtCapability.HtCapInfo.ShortGIfor40)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE);
+						if (HtCapability.HtCapInfo.TxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_TxSTBC_CAPABLE);
+						if (HtCapability.HtCapInfo.RxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RxSTBC_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.PlusHTC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_HTC_CAPABLE);
+						if (pAd->CommonCfg.bRdg && HtCapability.ExtHtCapInfo.RDGSupport)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RDG_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.MCSFeedback == 0x03)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_MCSFEEDBACK_CAPABLE);
+
+						NdisMoveMemory(&pEntry->HTCapability, &HtCapability, sizeof(HT_CAPABILITY_IE));
+					}
+#endif // DOT11_N_SUPPORT //
+
+					pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+					pEntry->CurrTxRate = pEntry->MaxSupportedRate;
+					CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+
+					if (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+					{
+						PUCHAR pTable;
+						UCHAR TableSize = 0;
+
+						MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &pEntry->CurrTxRateIndex);
+						pEntry->bAutoTxRateSwitch = TRUE;
+					}
+					else
+					{
+						pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+						pEntry->HTPhyMode.field.MCS	= pAd->StaCfg.HTPhyMode.field.MCS;
+						pEntry->bAutoTxRateSwitch = FALSE;
+
+						RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+					}
+					pEntry->RateLen = SupportedRatesLen;
+
+					break;
+				}
+			}
+		}
+		StatusCode = MLME_SUCCESS;
+
+		// can not find in table, create a new one
+		if (i < 0)
+		{
+			StatusCode = MLME_QOS_UNSPECIFY;
+			DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsReqAction() DLSEntry table full(only can support %d DLS session) \n", MAX_NUM_OF_DLS_ENTRY - MAX_NUM_OF_INIT_DLS_ENTRY));
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsReqAction() use entry(%d) %02x:%02x:%02x:%02x:%02x:%02x\n",
+				i, SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+		}
+	}
+
+	ActHeaderInit(pAd, &DlsRspHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+
+	// Build basic frame first
+	if (StatusCode == MLME_SUCCESS)
+	{
+		MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+						sizeof(HEADER_802_11),		&DlsRspHdr,
+						1,							&Category,
+						1,							&Action,
+						2,							&StatusCode,
+						6,							SA,
+						6,							pAd->CurrentAddress,
+						2,							&pAd->StaActive.CapabilityInfo,
+						1,							&SupRateIe,
+						1,							&pAd->MlmeAux.SupRateLen,
+						pAd->MlmeAux.SupRateLen,	pAd->MlmeAux.SupRate,
+						END_OF_ARGS);
+
+		if (pAd->MlmeAux.ExtRateLen != 0)
+		{
+			MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+							  1,						&ExtRateIe,
+							  1,						&pAd->MlmeAux.ExtRateLen,
+							  pAd->MlmeAux.ExtRateLen, 	pAd->MlmeAux.ExtRate,
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+#ifdef DOT11_N_SUPPORT
+		if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+		{
+			UCHAR HtLen;
+
+#ifdef RT_BIG_ENDIAN
+			HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+
+			// add HT Capability IE
+			HtLen = sizeof(HT_CAPABILITY_IE);
+#ifndef RT_BIG_ENDIAN
+			MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+								1,						&HtCapIe,
+								1,						&HtLen,
+								HtLen,					&pAd->CommonCfg.HtCapability,
+								END_OF_ARGS);
+#else
+			NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, HtLen);
+								*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+								*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+								1,						&HtCapIe,
+								1,						&HtLen,
+								HtLen,					&HtCapabilityTmp,
+								END_OF_ARGS);
+#endif
+			FrameLen = FrameLen + tmp;
+		}
+#endif // DOT11_N_SUPPORT //
+
+		if (pDLS && (pDLS->Status != DLS_FINISH))
+		{
+			RTMPCancelTimer(&pDLS->Timer, &TimerCancelled);
+			Timeout = DLS_TIMEOUT;
+			RTMPSetTimer(&pDLS->Timer, Timeout);
+		}
+	}
+	else
+	{
+		MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+						sizeof(HEADER_802_11),		&DlsRspHdr,
+						1,							&Category,
+						1,							&Action,
+						2,							&StatusCode,
+						6,							SA,
+						6,							pAd->CurrentAddress,
+						END_OF_ARGS);
+	}
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerDlsRspAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT		CapabilityInfo;
+	UCHAR		DA[MAC_ADDR_LEN], SA[MAC_ADDR_LEN];
+	USHORT		StatusCode;
+	SHORT		i;
+	BOOLEAN		TimerCancelled;
+	UCHAR		MaxSupportedRateIn500Kbps = 0;
+    UCHAR		SupportedRatesLen;
+    UCHAR		SupportedRates[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		HtCapabilityLen;
+	HT_CAPABILITY_IE	HtCapability;
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return;
+
+	if (!INFRA_ON(pAd))
+		return;
+
+	if (!PeerDlsRspSanity(pAd, Elem->Msg, Elem->MsgLen, DA, SA, &CapabilityInfo, &StatusCode,
+							&SupportedRatesLen, &SupportedRates[0], &HtCapabilityLen, &HtCapability))
+		return;
+
+    // supported rates array may not be sorted. sort it and find the maximum rate
+    for (i=0; i<SupportedRatesLen; i++)
+    {
+        if (MaxSupportedRateIn500Kbps < (SupportedRates[i] & 0x7f))
+            MaxSupportedRateIn500Kbps = SupportedRates[i] & 0x7f;
+    }
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() from %02x:%02x:%02x:%02x:%02x:%02x with StatusCode=%d, CapabilityInfo=0x%x\n",
+		SA[0], SA[1], SA[2], SA[3], SA[4], SA[5], StatusCode, CapabilityInfo));
+
+	for (i = 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			if (StatusCode == MLME_SUCCESS)
+			{
+				MAC_TABLE_ENTRY *pEntry;
+				UCHAR MaxSupportedRate = RATE_11;
+
+				pEntry = MacTableInsertDlsEntry(pAd, SA, i);
+
+				switch (MaxSupportedRateIn500Kbps)
+				{
+					case 108: MaxSupportedRate = RATE_54;   break;
+					case 96:  MaxSupportedRate = RATE_48;   break;
+					case 72:  MaxSupportedRate = RATE_36;   break;
+					case 48:  MaxSupportedRate = RATE_24;   break;
+					case 36:  MaxSupportedRate = RATE_18;   break;
+					case 24:  MaxSupportedRate = RATE_12;   break;
+					case 18:  MaxSupportedRate = RATE_9;    break;
+					case 12:  MaxSupportedRate = RATE_6;    break;
+					case 22:  MaxSupportedRate = RATE_11;   break;
+					case 11:  MaxSupportedRate = RATE_5_5;  break;
+					case 4:   MaxSupportedRate = RATE_2;    break;
+					case 2:   MaxSupportedRate = RATE_1;    break;
+					default:  MaxSupportedRate = RATE_11;   break;
+				}
+
+				pEntry->MaxSupportedRate = min(pAd->CommonCfg.MaxTxRate, MaxSupportedRate);
+
+				if (pEntry->MaxSupportedRate < RATE_FIRST_OFDM_RATE)
+				{
+					pEntry->MaxHTPhyMode.field.MODE = MODE_CCK;
+					pEntry->MaxHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					pEntry->MinHTPhyMode.field.MODE = MODE_CCK;
+					pEntry->MinHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					pEntry->HTPhyMode.field.MODE = MODE_CCK;
+					pEntry->HTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+				}
+				else
+				{
+					pEntry->MaxHTPhyMode.field.MODE = MODE_OFDM;
+					pEntry->MaxHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					pEntry->MinHTPhyMode.field.MODE = MODE_OFDM;
+					pEntry->MinHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+					pEntry->HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+				}
+
+				pEntry->MaxHTPhyMode.field.BW = BW_20;
+				pEntry->MinHTPhyMode.field.BW = BW_20;
+
+#ifdef DOT11_N_SUPPORT
+				pEntry->HTCapability.MCSSet[0] = 0;
+				pEntry->HTCapability.MCSSet[1] = 0;
+
+				// If this Entry supports 802.11n, upgrade to HT rate.
+				if ((HtCapabilityLen != 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+				{
+					UCHAR	j, bitmask; //k,bitmask;
+					CHAR    ii;
+
+					DBGPRINT(RT_DEBUG_OFF, ("DLS - PeerDlsRspAction Receive Peer HT Capable STA from %02x:%02x:%02x:%02x:%02x:%02x\n",
+								SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+					if ((HtCapability.HtCapInfo.GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_HTGREENFIELD;
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+						pAd->MacTab.fAnyStationNonGF = TRUE;
+						pAd->CommonCfg.AddHTInfo.AddHtInfo2.NonGfPresent = 1;
+					}
+
+					if ((HtCapability.HtCapInfo.ChannelWidth) && (pAd->CommonCfg.DesiredHtPhy.ChannelWidth))
+					{
+						pEntry->MaxHTPhyMode.field.BW= BW_40;
+						pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor40)&(HtCapability.HtCapInfo.ShortGIfor40));
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.BW = BW_20;
+						pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor20)&(HtCapability.HtCapInfo.ShortGIfor20));
+						pAd->MacTab.fAnyStation20Only = TRUE;
+					}
+
+					// find max fixed rate
+					for (ii=15; ii>=0; ii--)
+					{
+						j = ii/8;
+						bitmask = (1<<(ii-(j*8)));
+						if ( (pAd->StaCfg.DesiredHtPhyInfo.MCSSet[j]&bitmask) && (HtCapability.MCSSet[j]&bitmask))
+						{
+							pEntry->MaxHTPhyMode.field.MCS = ii;
+							break;
+						}
+						if (ii==0)
+							break;
+					}
+
+					if (pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO)
+					{
+						if (pAd->StaCfg.DesiredTransmitSetting.field.MCS == 32)
+						{
+							// Fix MCS as HT Duplicated Mode
+							pEntry->MaxHTPhyMode.field.BW = 1;
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+							pEntry->MaxHTPhyMode.field.STBC = 0;
+							pEntry->MaxHTPhyMode.field.ShortGI = 0;
+							pEntry->MaxHTPhyMode.field.MCS = 32;
+						}
+						else if (pEntry->MaxHTPhyMode.field.MCS > pAd->StaCfg.HTPhyMode.field.MCS)
+						{
+							// STA supports fixed MCS
+							pEntry->MaxHTPhyMode.field.MCS = pAd->StaCfg.HTPhyMode.field.MCS;
+						}
+					}
+
+					pEntry->MaxHTPhyMode.field.STBC = (HtCapability.HtCapInfo.RxSTBC & (pAd->CommonCfg.DesiredHtPhy.TxSTBC));
+					pEntry->MpduDensity = HtCapability.HtCapParm.MpduDensity;
+					pEntry->MaxRAmpduFactor = HtCapability.HtCapParm.MaxRAmpduFactor;
+					pEntry->MmpsMode = (UCHAR)HtCapability.HtCapInfo.MimoPs;
+					pEntry->AMsduSize = (UCHAR)HtCapability.HtCapInfo.AMsduSize;
+					pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+
+					if (HtCapability.HtCapInfo.ShortGIfor20)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE);
+					if (HtCapability.HtCapInfo.ShortGIfor40)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE);
+					if (HtCapability.HtCapInfo.TxSTBC)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_TxSTBC_CAPABLE);
+					if (HtCapability.HtCapInfo.RxSTBC)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RxSTBC_CAPABLE);
+					if (HtCapability.ExtHtCapInfo.PlusHTC)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_HTC_CAPABLE);
+					if (pAd->CommonCfg.bRdg && HtCapability.ExtHtCapInfo.RDGSupport)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RDG_CAPABLE);
+					if (HtCapability.ExtHtCapInfo.MCSFeedback == 0x03)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_MCSFEEDBACK_CAPABLE);
+
+					NdisMoveMemory(&pEntry->HTCapability, &HtCapability, sizeof(HT_CAPABILITY_IE));
+				}
+#endif // DOT11_N_SUPPORT //
+				pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+				pEntry->CurrTxRate = pEntry->MaxSupportedRate;
+				CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+
+				if (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+				{
+					PUCHAR pTable;
+					UCHAR TableSize = 0;
+
+					MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &pEntry->CurrTxRateIndex);
+					pEntry->bAutoTxRateSwitch = TRUE;
+				}
+				else
+				{
+					pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+					pEntry->HTPhyMode.field.MCS	= pAd->StaCfg.HTPhyMode.field.MCS;
+					pEntry->bAutoTxRateSwitch = FALSE;
+
+					RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+				}
+				pEntry->RateLen = SupportedRatesLen;
+
+				if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+				{
+					// If support WPA or WPA2, start STAKey hand shake,
+					// If failed hand shake, just tear down peer DLS
+					if (RTMPSendSTAKeyRequest(pAd, pAd->StaCfg.DLSEntry[i].MacAddr) != NDIS_STATUS_SUCCESS)
+					{
+						MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+						USHORT				reason = REASON_QOS_CIPHER_NOT_SUPPORT;
+
+						DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+						MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+						pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+						pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+						DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed when call RTMPSendSTAKeyRequest \n"));
+					}
+					else
+					{
+						pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+						DBGPRINT(RT_DEBUG_TRACE,("DLS - waiting for STAKey handshake procedure\n"));
+					}
+				}
+				else
+				{
+					RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+					pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+					DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() from %02x:%02x:%02x:%02x:%02x:%02x Succeed with WEP or no security\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+				}
+
+				//initialize seq no for DLS frames.
+				pAd->StaCfg.DLSEntry[i].Sequence = 0;
+				if (HtCapabilityLen != 0)
+					pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+				else
+					pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+			}
+			else
+			{
+				// DLS setup procedure failed.
+				pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+				DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed with StatusCode=%d \n", StatusCode));
+			}
+		}
+	}
+
+	if (i >= MAX_NUM_OF_INIT_DLS_ENTRY)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() update timeout value \n"));
+		for (i=(MAX_NUM_OF_DLS_ENTRY-1); i>=MAX_NUM_OF_INIT_DLS_ENTRY; i--)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				if (StatusCode == MLME_SUCCESS)
+				{
+					MAC_TABLE_ENTRY *pEntry;
+					UCHAR MaxSupportedRate = RATE_11;
+
+					pEntry = MacTableInsertDlsEntry(pAd, SA, i);
+
+					switch (MaxSupportedRateIn500Kbps)
+					{
+						case 108: MaxSupportedRate = RATE_54;   break;
+						case 96:  MaxSupportedRate = RATE_48;   break;
+						case 72:  MaxSupportedRate = RATE_36;   break;
+						case 48:  MaxSupportedRate = RATE_24;   break;
+						case 36:  MaxSupportedRate = RATE_18;   break;
+						case 24:  MaxSupportedRate = RATE_12;   break;
+						case 18:  MaxSupportedRate = RATE_9;    break;
+						case 12:  MaxSupportedRate = RATE_6;    break;
+						case 22:  MaxSupportedRate = RATE_11;   break;
+						case 11:  MaxSupportedRate = RATE_5_5;  break;
+						case 4:   MaxSupportedRate = RATE_2;    break;
+						case 2:   MaxSupportedRate = RATE_1;    break;
+						default:  MaxSupportedRate = RATE_11;   break;
+					}
+
+					pEntry->MaxSupportedRate = min(pAd->CommonCfg.MaxTxRate, MaxSupportedRate);
+
+					if (pEntry->MaxSupportedRate < RATE_FIRST_OFDM_RATE)
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MaxHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->MinHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MinHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->HTPhyMode.field.MODE = MODE_CCK;
+						pEntry->HTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MaxHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->MinHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MinHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					}
+
+					pEntry->MaxHTPhyMode.field.BW = BW_20;
+					pEntry->MinHTPhyMode.field.BW = BW_20;
+
+#ifdef DOT11_N_SUPPORT
+					pEntry->HTCapability.MCSSet[0] = 0;
+					pEntry->HTCapability.MCSSet[1] = 0;
+
+					// If this Entry supports 802.11n, upgrade to HT rate.
+					if ((HtCapabilityLen != 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+					{
+						UCHAR	j, bitmask; //k,bitmask;
+						CHAR    ii;
+
+						DBGPRINT(RT_DEBUG_OFF, ("DLS - PeerDlsRspAction Receive Peer HT Capable STA from %02x:%02x:%02x:%02x:%02x:%02x\n",
+									SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+						if ((HtCapability.HtCapInfo.GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTGREENFIELD;
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+							pAd->MacTab.fAnyStationNonGF = TRUE;
+							pAd->CommonCfg.AddHTInfo.AddHtInfo2.NonGfPresent = 1;
+						}
+
+						if ((HtCapability.HtCapInfo.ChannelWidth) && (pAd->CommonCfg.DesiredHtPhy.ChannelWidth))
+						{
+							pEntry->MaxHTPhyMode.field.BW= BW_40;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor40)&(HtCapability.HtCapInfo.ShortGIfor40));
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.BW = BW_20;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor20)&(HtCapability.HtCapInfo.ShortGIfor20));
+							pAd->MacTab.fAnyStation20Only = TRUE;
+						}
+
+						// find max fixed rate
+						for (ii=15; ii>=0; ii--)
+						{
+							j = ii/8;
+							bitmask = (1<<(ii-(j*8)));
+							if ( (pAd->StaCfg.DesiredHtPhyInfo.MCSSet[j]&bitmask) && (HtCapability.MCSSet[j]&bitmask))
+							{
+								pEntry->MaxHTPhyMode.field.MCS = ii;
+								break;
+							}
+							if (ii==0)
+								break;
+						}
+
+						if (pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO)
+						{
+							printk("@@@ pAd->CommonCfg.RegTransmitSetting.field.MCS = %d\n",
+								pAd->StaCfg.DesiredTransmitSetting.field.MCS);
+							if (pAd->StaCfg.DesiredTransmitSetting.field.MCS == 32)
+							{
+								// Fix MCS as HT Duplicated Mode
+								pEntry->MaxHTPhyMode.field.BW = 1;
+								pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+								pEntry->MaxHTPhyMode.field.STBC = 0;
+								pEntry->MaxHTPhyMode.field.ShortGI = 0;
+								pEntry->MaxHTPhyMode.field.MCS = 32;
+							}
+							else if (pEntry->MaxHTPhyMode.field.MCS > pAd->StaCfg.HTPhyMode.field.MCS)
+							{
+								// STA supports fixed MCS
+								pEntry->MaxHTPhyMode.field.MCS = pAd->StaCfg.HTPhyMode.field.MCS;
+							}
+						}
+
+						pEntry->MaxHTPhyMode.field.STBC = (HtCapability.HtCapInfo.RxSTBC & (pAd->CommonCfg.DesiredHtPhy.TxSTBC));
+						pEntry->MpduDensity = HtCapability.HtCapParm.MpduDensity;
+						pEntry->MaxRAmpduFactor = HtCapability.HtCapParm.MaxRAmpduFactor;
+						pEntry->MmpsMode = (UCHAR)HtCapability.HtCapInfo.MimoPs;
+						pEntry->AMsduSize = (UCHAR)HtCapability.HtCapInfo.AMsduSize;
+						pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+
+						if (HtCapability.HtCapInfo.ShortGIfor20)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE);
+						if (HtCapability.HtCapInfo.ShortGIfor40)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE);
+						if (HtCapability.HtCapInfo.TxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_TxSTBC_CAPABLE);
+						if (HtCapability.HtCapInfo.RxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RxSTBC_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.PlusHTC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_HTC_CAPABLE);
+						if (pAd->CommonCfg.bRdg && HtCapability.ExtHtCapInfo.RDGSupport)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RDG_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.MCSFeedback == 0x03)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_MCSFEEDBACK_CAPABLE);
+
+						NdisMoveMemory(&pEntry->HTCapability, &HtCapability, sizeof(HT_CAPABILITY_IE));
+					}
+#endif // DOT11_N_SUPPORT //
+
+					pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+					pEntry->CurrTxRate = pEntry->MaxSupportedRate;
+					CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+
+					if (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+					{
+						PUCHAR pTable;
+						UCHAR TableSize = 0;
+
+						MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &pEntry->CurrTxRateIndex);
+						pEntry->bAutoTxRateSwitch = TRUE;
+					}
+					else
+					{
+						pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+						pEntry->HTPhyMode.field.MCS	= pAd->StaCfg.HTPhyMode.field.MCS;
+						pEntry->bAutoTxRateSwitch = FALSE;
+
+						RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+					}
+					pEntry->RateLen = SupportedRatesLen;
+
+					if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+					{
+						// If support WPA or WPA2, start STAKey hand shake,
+						// If failed hand shake, just tear down peer DLS
+						if (RTMPSendSTAKeyRequest(pAd, pAd->StaCfg.DLSEntry[i].MacAddr) != NDIS_STATUS_SUCCESS)
+						{
+							MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+							USHORT				reason = REASON_QOS_CIPHER_NOT_SUPPORT;
+
+							DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+							MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+							pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+							pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+							DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed when call RTMPSendSTAKeyRequest \n"));
+						}
+						else
+						{
+							pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+							DBGPRINT(RT_DEBUG_TRACE,("DLS - waiting for STAKey handshake procedure\n"));
+						}
+					}
+					else
+					{
+						RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+						pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+						DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() from %02x:%02x:%02x:%02x:%02x:%02x Succeed with WEP or no security\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+					}
+					pAd->StaCfg.DLSEntry[i].Sequence = 0;
+					if (HtCapabilityLen != 0)
+						pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+					else
+						pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+				}
+				else
+				{
+					// DLS setup procedure failed.
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+					RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+					DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed with StatusCode=%d \n", StatusCode));
+				}
+			}
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDlsTearDownAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_TEARDOWN;
+	USHORT			ReasonCode = REASON_QOS_UNSPECIFY;
+	HEADER_802_11	DlsTearDownHdr;
+	PRT_802_11_DLS	pDLS;
+	BOOLEAN			TimerCancelled;
+	UCHAR			i;
+
+	if(!MlmeDlsReqSanity(pAd, Elem->Msg, Elem->MsgLen, &pDLS, &ReasonCode))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - MlmeDlsTearDownAction() with ReasonCode=%d \n", ReasonCode));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("DLS - MlmeDlsTearDownAction() allocate memory failed \n"));
+		return;
+	}
+
+	ActHeaderInit(pAd, &DlsTearDownHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+
+	// Build basic frame first
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+					sizeof(HEADER_802_11),		&DlsTearDownHdr,
+					1,							&Category,
+					1,							&Action,
+					6,							&pDLS->MacAddr,
+					6,							pAd->CurrentAddress,
+					2,							&ReasonCode,
+					END_OF_ARGS);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+	RTMPCancelTimer(&pDLS->Timer, &TimerCancelled);
+
+	// Remove key in local dls table entry
+	for (i = 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	// clear peer dls table entry
+	for (i = MAX_NUM_OF_INIT_DLS_ENTRY; i < MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+			pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerDlsTearDownAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR			DA[MAC_ADDR_LEN], SA[MAC_ADDR_LEN];
+	USHORT			ReasonCode;
+	UINT			i;
+	BOOLEAN			TimerCancelled;
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return;
+
+	if (!INFRA_ON(pAd))
+		return;
+
+	if (!PeerDlsTearDownSanity(pAd, Elem->Msg, Elem->MsgLen, DA, SA, &ReasonCode))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsTearDownAction() from %02x:%02x:%02x:%02x:%02x:%02x with ReasonCode=%d\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5], ReasonCode));
+
+	// clear local dls table entry
+	for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+			pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+			//AsicDelWcidTab(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			//AsicRemovePairwiseKeyEntry(pAd, BSS0, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	// clear peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+			pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+			//AsicDelWcidTab(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			//AsicRemovePairwiseKeyEntry(pAd, BSS0, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID RTMPCheckDLSTimeOut(
+	IN PRTMP_ADAPTER	pAd)
+{
+	ULONG				i;
+	MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+	USHORT				reason = REASON_QOS_UNSPECIFY;
+
+	if (! pAd->CommonCfg.bDLSCapable)
+		return;
+
+	if (! INFRA_ON(pAd))
+		return;
+
+	// If timeout value is equaled to zero, it means always not be timeout.
+
+	// update local dls table entry
+	for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if ((pAd->StaCfg.DLSEntry[i].Valid) && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& (pAd->StaCfg.DLSEntry[i].TimeOut != 0))
+		{
+			pAd->StaCfg.DLSEntry[i].CountDownTimer --;
+
+			if (pAd->StaCfg.DLSEntry[i].CountDownTimer == 0)
+			{
+				reason = REASON_QOS_REQUEST_TIMEOUT;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+			}
+		}
+	}
+
+	// update peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if ((pAd->StaCfg.DLSEntry[i].Valid) && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& (pAd->StaCfg.DLSEntry[i].TimeOut != 0))
+		{
+			pAd->StaCfg.DLSEntry[i].CountDownTimer --;
+
+			if (pAd->StaCfg.DLSEntry[i].CountDownTimer == 0)
+			{
+				reason = REASON_QOS_REQUEST_TIMEOUT;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+			}
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN RTMPRcvFrameDLSCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN PHEADER_802_11	pHeader,
+	IN ULONG			Len,
+	IN PRT28XX_RXD_STRUC	pRxD)
+{
+	ULONG			i;
+	BOOLEAN			bFindEntry = FALSE;
+	BOOLEAN			bSTAKeyFrame = FALSE;
+	PEAPOL_PACKET	pEap;
+	PUCHAR			pProto, pAddr = NULL;
+	PUCHAR			pSTAKey = NULL;
+	UCHAR			ZeroReplay[LEN_KEY_DESC_REPLAY];
+	UCHAR			Mic[16], OldMic[16];
+	UCHAR			digest[80];
+	UCHAR			DlsPTK[80];
+	UCHAR			temp[64];
+	BOOLEAN			TimerCancelled;
+	CIPHER_KEY		PairwiseKey;
+
+
+	if (! pAd->CommonCfg.bDLSCapable)
+		return bSTAKeyFrame;
+
+	if (! INFRA_ON(pAd))
+		return bSTAKeyFrame;
+
+	if (! (pHeader->FC.SubType & 0x08))
+		return bSTAKeyFrame;
+
+	if (Len < LENGTH_802_11 + 6 + 2 + 2)
+		return bSTAKeyFrame;
+
+	pProto	= (PUCHAR)pHeader + LENGTH_802_11 + 2 + 6;	// QOS Control field , 0xAA 0xAA 0xAA 0x00 0x00 0x00
+	pAddr	= pHeader->Addr2;
+
+	// L2PAD bit on will pad 2 bytes at LLC
+	if (pRxD->L2PAD)
+	{
+		pProto += 2;
+	}
+
+	if (RTMPEqualMemory(EAPOL, pProto, 2) && (pAd->StaCfg.AuthMode >=  Ndis802_11AuthModeWPA))
+	{
+		pEap = (PEAPOL_PACKET) (pProto + 2);
+
+		DBGPRINT(RT_DEBUG_TRACE,("DLS - Sniff Len=%ld, DataLen=%d, KeyMic=%d, Install=%d, KeyAck=%d, Secure=%d, EKD_DL=%d, Error=%d, Request=%d\n", Len,
+			                                                             (LENGTH_802_11 + 6 + 2 + 2 + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE + 16),
+			                                                             pEap->KeyDesc.KeyInfo.KeyMic,
+			                                                             pEap->KeyDesc.KeyInfo.Install,
+			                                                             pEap->KeyDesc.KeyInfo.KeyAck,
+			                                                             pEap->KeyDesc.KeyInfo.Secure,
+			                                                             pEap->KeyDesc.KeyInfo.EKD_DL,
+			                                                             pEap->KeyDesc.KeyInfo.Error,
+			                                                             pEap->KeyDesc.KeyInfo.Request));
+
+		if ((Len >= (LENGTH_802_11 + 6 + 2 + 2 + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE + 16)) && pEap->KeyDesc.KeyInfo.KeyMic
+			&& pEap->KeyDesc.KeyInfo.Install && pEap->KeyDesc.KeyInfo.KeyAck && pEap->KeyDesc.KeyInfo.Secure
+			&& pEap->KeyDesc.KeyInfo.EKD_DL && !pEap->KeyDesc.KeyInfo.Error && !pEap->KeyDesc.KeyInfo.Request)
+		{
+			// First validate replay counter, only accept message with larger replay counter
+			// Let equal pass, some AP start with all zero replay counter
+			NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);
+			if ((RTMPCompareMemory(pEap->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1) &&
+				(RTMPCompareMemory(pEap->KeyDesc.ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
+				return bSTAKeyFrame;
+
+			//RTMPMoveMemory(pAd->StaCfg.ReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+			RTMPMoveMemory(pAd->StaCfg.DlsReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - Sniff replay counter (%02x-%02x-%02x-%02x-%02x-%02x-%02x-%02x) Len=%ld, KeyDataLen=%d\n",
+				pAd->StaCfg.ReplayCounter[0], pAd->StaCfg.ReplayCounter[1], pAd->StaCfg.ReplayCounter[2],
+				pAd->StaCfg.ReplayCounter[3], pAd->StaCfg.ReplayCounter[4],	pAd->StaCfg.ReplayCounter[5],
+				pAd->StaCfg.ReplayCounter[6], pAd->StaCfg.ReplayCounter[7], Len, pEap->KeyDesc.KeyData[1]));
+
+			// put these code segment to get the replay counter
+			if (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED)
+				return bSTAKeyFrame;
+
+			// Check MIC value
+			// Save the MIC and replace with zero
+			// use proprietary PTK
+			NdisZeroMemory(temp, 64);
+			NdisMoveMemory(temp, "IEEE802.11 WIRELESS ACCESS POINT", 32);
+			WpaCountPTK(pAd, temp, temp, pAd->CommonCfg.Bssid, temp, pAd->CurrentAddress, DlsPTK, LEN_PTK);
+
+			NdisMoveMemory(OldMic, pEap->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+			NdisZeroMemory(pEap->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+			if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+			{
+				// AES
+				HMAC_SHA1((PUCHAR) pEap, pEap->Body_Len[1] + 4, DlsPTK, LEN_EAP_MICK, digest);
+				NdisMoveMemory(Mic,	digest,	LEN_KEY_DESC_MIC);
+			}
+			else
+			{
+				hmac_md5(DlsPTK, LEN_EAP_MICK, (PUCHAR) pEap, pEap->Body_Len[1] + 4, Mic);
+			}
+
+			if (!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("MIC Different in Msg1 of STAKey handshake! \n"));
+				return bSTAKeyFrame;
+			}
+			else
+				DBGPRINT(RT_DEBUG_TRACE, ("MIC VALID in Msg1 of STAKey handshake! \n"));
+#if 1
+			if ((pEap->KeyDesc.KeyData[0] == 0xDD) && (pEap->KeyDesc.KeyData[2] == 0x00) && (pEap->KeyDesc.KeyData[3] == 0x0C)
+				&& (pEap->KeyDesc.KeyData[4] == 0x43) && (pEap->KeyDesc.KeyData[5] == 0x02))
+			{
+				pAddr			= pEap->KeyDesc.KeyData + 8;		// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2)
+				pSTAKey			= pEap->KeyDesc.KeyData + 14;	// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2), STAKey_Mac_Addr(6)
+
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 from %02x:%02x:%02x:%02x:%02x:%02x Len=%ld, KeyDataLen=%d\n",
+					pAddr[0], pAddr[1], pAddr[2], pAddr[3], pAddr[4], pAddr[5], Len, pEap->KeyDesc.KeyData[1]));
+
+				bSTAKeyFrame = TRUE;
+			}
+#else
+			if ((pEap->KeyDesc.KeyData[0] == 0xDD) && (pEap->KeyDesc.KeyData[2] == 0x00) && (pEap->KeyDesc.KeyData[3] == 0x0F)
+				&& (pEap->KeyDesc.KeyData[4] == 0xAC) && (pEap->KeyDesc.KeyData[5] == 0x02))
+			{
+				pAddr			= pEap->KeyDesc.KeyData + 8;		// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2)
+				pSTAKey			= pEap->KeyDesc.KeyData + 14;	// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2), STAKey_Mac_Addr(6)
+
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 from %02x:%02x:%02x:%02x:%02x:%02x Len=%d, KeyDataLen=%d\n",
+					pAddr[0], pAddr[1], pAddr[2], pAddr[3], pAddr[4], pAddr[5], Len, pEap->KeyDesc.KeyData[1]));
+
+				bSTAKeyFrame = TRUE;
+			}
+#endif
+
+		}
+		else if (Len >= (LENGTH_802_11 + 6 + 2 + 2 + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE))
+		{
+#if 0
+			RTMPMoveMemory(pAd->StaCfg.ReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+#endif
+			RTMPMoveMemory(pAd->StaCfg.DlsReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - Sniff replay counter 2(%02x-%02x-%02x-%02x-%02x-%02x-%02x-%02x) Len=%ld, KeyDataLen=%d\n",
+				pAd->StaCfg.ReplayCounter[0], pAd->StaCfg.ReplayCounter[1], pAd->StaCfg.ReplayCounter[2],
+				pAd->StaCfg.ReplayCounter[3], pAd->StaCfg.ReplayCounter[4],	pAd->StaCfg.ReplayCounter[5],
+				pAd->StaCfg.ReplayCounter[6], pAd->StaCfg.ReplayCounter[7], Len, pEap->KeyDesc.KeyData[1]));
+
+		}
+	}
+
+	// If timeout value is equaled to zero, it means always not be timeout.
+	// update local dls table entry
+	for (i= 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(pAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			if (bSTAKeyFrame)
+			{
+				PMAC_TABLE_ENTRY pEntry;
+
+				// STAKey frame, add pairwise key table
+				pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+				RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+
+				PairwiseKey.KeyLen = LEN_TKIP_EK;
+				NdisMoveMemory(PairwiseKey.Key, &pSTAKey[0], LEN_TKIP_EK);
+				NdisMoveMemory(PairwiseKey.TxMic, &pSTAKey[16], LEN_TKIP_RXMICK);
+				NdisMoveMemory(PairwiseKey.RxMic, &pSTAKey[24], LEN_TKIP_TXMICK);
+
+				PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg;
+
+				pEntry = DlsEntryTableLookup(pAd, pAd->StaCfg.DLSEntry[i].MacAddr, TRUE);
+				//AsicAddKeyEntry(pAd, (USHORT)(i + 2), BSS0, 0, &PairwiseKey, TRUE, TRUE);	// reserve 0 for multicast, 1 for unicast
+				//AsicUpdateRxWCIDTable(pAd, (USHORT)(i + 2), pAddr);
+				// Add Pair-wise key to Asic
+#ifdef RT2870
+				{
+					RT_ADD_PAIRWISE_KEY_ENTRY KeyInfo;
+					COPY_MAC_ADDR(KeyInfo.MacAddr,pAd->StaCfg.DLSEntry[i].MacAddr);
+					KeyInfo.MacTabMatchWCID=pAd->StaCfg.DLSEntry[i].MacTabMatchWCID;
+					NdisMoveMemory(&KeyInfo.CipherKey,  &PairwiseKey,sizeof(CIPHER_KEY));
+					RTUSBEnqueueInternalCmd(pAd, RT_CMD_SET_KEY_TABLE, &KeyInfo, sizeof(RT_ADD_PAIRWISE_KEY_ENTRY));
+				}
+				{
+					PMAC_TABLE_ENTRY pDLSEntry;
+					pDLSEntry = DlsEntryTableLookup(pAd, pAd->StaCfg.DLSEntry[i].MacAddr, TRUE);
+					pDLSEntry->PairwiseKey.CipherAlg=PairwiseKey.CipherAlg;
+					RTUSBEnqueueInternalCmd(pAd, RT_CMD_SET_RX_WCID_TABLE, pDLSEntry, sizeof(MAC_TABLE_ENTRY));
+				}
+#endif // RT2870 //
+				NdisMoveMemory(&pEntry->PairwiseKey, &PairwiseKey, sizeof(CIPHER_KEY));
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 (Peer STA MAC Address STAKey) \n"));
+
+				RTMPSendSTAKeyHandShake(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Finish STAKey handshake procedure (Initiator side)\n"));
+			}
+			else
+			{
+				// Data frame, update timeout value
+				if (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+				{
+					pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+					//AsicUpdateRxWCIDTable(pAd, (USHORT)(i + 2), pAddr);
+				}
+			}
+
+			bFindEntry = TRUE;
+		}
+	}
+
+	// update peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(pAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			if (bSTAKeyFrame)
+			{
+				PMAC_TABLE_ENTRY pEntry = NULL;
+
+				// STAKey frame, add pairwise key table, and send STAkey Msg-2
+				pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+				RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+
+				PairwiseKey.KeyLen = LEN_TKIP_EK;
+				NdisMoveMemory(PairwiseKey.Key, &pSTAKey[0], LEN_TKIP_EK);
+				NdisMoveMemory(PairwiseKey.TxMic, &pSTAKey[16], LEN_TKIP_RXMICK);
+				NdisMoveMemory(PairwiseKey.RxMic, &pSTAKey[24], LEN_TKIP_TXMICK);
+
+				PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg;
+
+				pEntry = DlsEntryTableLookup(pAd, pAd->StaCfg.DLSEntry[i].MacAddr, TRUE);
+				//AsicAddKeyEntry(pAd, (USHORT)(i + 2), BSS0, 0, &PairwiseKey, TRUE, TRUE);	// reserve 0 for multicast, 1 for unicast
+				//AsicUpdateRxWCIDTable(pAd, (USHORT)(i + 2), pAddr);
+				// Add Pair-wise key to Asic
+#ifdef RT2870
+				{
+					RT_ADD_PAIRWISE_KEY_ENTRY KeyInfo;
+					COPY_MAC_ADDR(KeyInfo.MacAddr,pAd->StaCfg.DLSEntry[i].MacAddr);
+					KeyInfo.MacTabMatchWCID=pAd->StaCfg.DLSEntry[i].MacTabMatchWCID;
+					NdisMoveMemory(&KeyInfo.CipherKey,  &PairwiseKey,sizeof(CIPHER_KEY));
+					RTUSBEnqueueInternalCmd(pAd, RT_CMD_SET_KEY_TABLE, &KeyInfo, sizeof(RT_ADD_PAIRWISE_KEY_ENTRY));
+				}
+				{
+					PMAC_TABLE_ENTRY pDLSEntry;
+					pDLSEntry = DlsEntryTableLookup(pAd, pAd->StaCfg.DLSEntry[i].MacAddr, TRUE);
+					pDLSEntry->PairwiseKey.CipherAlg=PairwiseKey.CipherAlg;
+					RTUSBEnqueueInternalCmd(pAd, RT_CMD_SET_RX_WCID_TABLE, pDLSEntry, sizeof(MAC_TABLE_ENTRY));
+				}
+#endif // RT2870 //
+				NdisMoveMemory(&pEntry->PairwiseKey, &PairwiseKey, sizeof(CIPHER_KEY));
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 (Initiator STA MAC Address STAKey)\n"));
+
+				// If support WPA or WPA2, start STAKey hand shake,
+				// If failed hand shake, just tear down peer DLS
+				if (RTMPSendSTAKeyHandShake(pAd, pAddr) != NDIS_STATUS_SUCCESS)
+				{
+					MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+					USHORT				reason = REASON_QOS_CIPHER_NOT_SUPPORT;
+
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+					pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+					DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+					MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE,("DLS - Finish STAKey handshake procedure (Peer side)\n"));
+				}
+			}
+			else
+			{
+				// Data frame, update timeout value
+				if (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+				{
+					pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+				}
+			}
+
+			bFindEntry = TRUE;
+		}
+	}
+
+
+	return bSTAKeyFrame;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Check if the frame can be sent through DLS direct link interface
+
+	Arguments:
+		pAd		Pointer	to adapter
+
+	Return Value:
+		DLS entry index
+
+	Note:
+
+	========================================================================
+*/
+INT	RTMPCheckDLSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA)
+{
+	INT rval = -1;
+	INT	i;
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return rval;
+
+	if (!INFRA_ON(pAd))
+		return rval;
+
+	do{
+		// check local dls table entry
+		for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				rval = i;
+				break;
+			}
+		}
+
+		// check peer dls table entry
+		for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				rval = i;
+				break;
+			}
+		}
+	} while (FALSE);
+
+	return rval;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID RTMPSendDLSTearDownFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	HEADER_802_11	DlsTearDownHdr;
+	ULONG			FrameLen = 0;
+	USHORT			Reason = REASON_QOS_QSTA_LEAVING_QBSS;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_TEARDOWN;
+	UCHAR			i = 0;
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+		RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Send DLS TearDown Frame \n"));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("ASSOC - RTMPSendDLSTearDownFrame() allocate memory failed \n"));
+		return;
+	}
+
+	ActHeaderInit(pAd, &DlsTearDownHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+					sizeof(HEADER_802_11),		&DlsTearDownHdr,
+					1,							&Category,
+					1,							&Action,
+					6,							pDA,
+					6,							pAd->CurrentAddress,
+					2,							&Reason,
+					END_OF_ARGS);
+
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	// Remove key in local dls table entry
+	for (i = 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	// Remove key in peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Send DLS TearDown Frame and remove key in (i=%d) \n", i));
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+NDIS_STATUS RTMPSendSTAKeyRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA)
+{
+	UCHAR				Header802_3[14];
+	NDIS_STATUS			NStatus;
+	ULONG				FrameLen = 0;
+	EAPOL_PACKET		Packet;
+	UCHAR				Mic[16];
+	UCHAR				digest[80];
+	PUCHAR				pOutBuffer = NULL;
+	PNDIS_PACKET		pNdisPacket;
+	UCHAR				temp[64];
+	UCHAR				DlsPTK[80];
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - RTMPSendSTAKeyRequest() to %02x:%02x:%02x:%02x:%02x:%02x\n", pDA[0], pDA[1], pDA[2], pDA[3], pDA[4], pDA[5]));
+
+	pAd->Sequence ++;
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer = EAPOL_VER;
+	Packet.ProType    = EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + 6 + MAC_ADDR_LEN;		// data field contain KDE andPeer MAC address
+
+	// STAKey Message is as EAPOL-Key(1,1,0,0,G/0,0,0, MIC, 0,Peer MAC KDE)
+	if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+    {
+        Packet.KeyDesc.Type = WPA1_KEY_DESC;
+    }
+    else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+    {
+        Packet.KeyDesc.Type = WPA2_KEY_DESC;
+    }
+
+	// Key descriptor version
+	Packet.KeyDesc.KeyInfo.KeyDescVer =
+		(((pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled) || (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)) ? (DESC_TYPE_AES) : (DESC_TYPE_TKIP));
+
+	Packet.KeyDesc.KeyInfo.KeyMic	= 1;
+	Packet.KeyDesc.KeyInfo.Secure	= 1;
+	Packet.KeyDesc.KeyInfo.Request	= 1;
+
+	Packet.KeyDesc.KeyDataLen[1]	= 12;
+
+	// use our own OUI to distinguish proprietary with standard.
+	Packet.KeyDesc.KeyData[0]		= 0xDD;
+	Packet.KeyDesc.KeyData[1]		= 0x0A;
+	Packet.KeyDesc.KeyData[2]		= 0x00;
+	Packet.KeyDesc.KeyData[3]		= 0x0C;
+	Packet.KeyDesc.KeyData[4]		= 0x43;
+	Packet.KeyDesc.KeyData[5]		= 0x03;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[6], pDA, MAC_ADDR_LEN);
+
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.DlsReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Allocate buffer for transmitting message
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+	if (NStatus	!= NDIS_STATUS_SUCCESS)
+		return NStatus;
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		              Packet.Body_Len[1] + 4,    &Packet,
+		              END_OF_ARGS);
+
+	// use proprietary PTK
+	NdisZeroMemory(temp, 64);
+	NdisMoveMemory(temp, "IEEE802.11 WIRELESS ACCESS POINT", 32);
+	WpaCountPTK(pAd, temp, temp, pAd->CommonCfg.Bssid, temp, pAd->CurrentAddress, DlsPTK, LEN_PTK);
+
+	// calculate MIC
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		NdisZeroMemory(digest,	sizeof(digest));
+		HMAC_SHA1(pOutBuffer, FrameLen, DlsPTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		NdisZeroMemory(Mic,	sizeof(Mic));
+		hmac_md5(DlsPTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+	}
+
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+	                  sizeof(Header802_3),	Header802_3,
+		              Packet.Body_Len[1] + 4,	&Packet,
+		              END_OF_ARGS);
+
+	NStatus = RTMPAllocateNdisPacket(pAd, &pNdisPacket, NULL, 0, pOutBuffer, FrameLen);
+	if (NStatus == NDIS_STATUS_SUCCESS)
+	{
+		RTMP_SET_PACKET_WCID(pNdisPacket, BSSID_WCID);
+		STASendPacket(pAd, pNdisPacket);
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSendSTAKeyRequest- Send STAKey request (NStatus=%x, FrameLen=%ld)\n", NStatus, FrameLen));
+
+	return NStatus;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+NDIS_STATUS RTMPSendSTAKeyHandShake(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA)
+{
+	UCHAR				Header802_3[14];
+	NDIS_STATUS			NStatus;
+	ULONG				FrameLen = 0;
+	EAPOL_PACKET		Packet;
+	UCHAR				Mic[16];
+	UCHAR				digest[80];
+	PUCHAR				pOutBuffer = NULL;
+	PNDIS_PACKET		pNdisPacket;
+	UCHAR				temp[64];
+	UCHAR				DlsPTK[80];			// Due to dirver can not get PTK, use proprietary PTK
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - RTMPSendSTAKeyHandShake() to %02x:%02x:%02x:%02x:%02x:%02x\n", pDA[0], pDA[1], pDA[2], pDA[3], pDA[4], pDA[5]));
+
+	pAd->Sequence ++;
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer = EAPOL_VER;
+	Packet.ProType    = EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + 6 + MAC_ADDR_LEN;		// data field contain KDE and Peer MAC address
+
+	// STAKey Message is as EAPOL-Key(1,1,0,0,G/0,0,0, MIC, 0,Peer MAC KDE)
+	if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+    {
+        Packet.KeyDesc.Type = WPA1_KEY_DESC;
+    }
+    else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+    {
+        Packet.KeyDesc.Type = WPA2_KEY_DESC;
+    }
+
+	// Key descriptor version
+	Packet.KeyDesc.KeyInfo.KeyDescVer =
+		(((pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled) || (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)) ? (DESC_TYPE_AES) : (DESC_TYPE_TKIP));
+
+	Packet.KeyDesc.KeyInfo.KeyMic	= 1;
+	Packet.KeyDesc.KeyInfo.Secure	= 1;
+
+	Packet.KeyDesc.KeyDataLen[1]	= 12;
+
+	// use our own OUI to distinguish proprietary with standard.
+	Packet.KeyDesc.KeyData[0]		= 0xDD;
+	Packet.KeyDesc.KeyData[1]		= 0x0A;
+	Packet.KeyDesc.KeyData[2]		= 0x00;
+	Packet.KeyDesc.KeyData[3]		= 0x0C;
+	Packet.KeyDesc.KeyData[4]		= 0x43;
+	Packet.KeyDesc.KeyData[5]		= 0x03;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[6], pDA, MAC_ADDR_LEN);
+
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.DlsReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Allocate buffer for transmitting message
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+	if (NStatus	!= NDIS_STATUS_SUCCESS)
+		return NStatus;
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		              Packet.Body_Len[1] + 4,    &Packet,
+		              END_OF_ARGS);
+
+	// use proprietary PTK
+	NdisZeroMemory(temp, 64);
+	NdisMoveMemory(temp, "IEEE802.11 WIRELESS ACCESS POINT", 32);
+	WpaCountPTK(pAd, temp, temp, pAd->CommonCfg.Bssid, temp, pAd->CurrentAddress, DlsPTK, LEN_PTK);
+
+	// calculate MIC
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		NdisZeroMemory(digest,	sizeof(digest));
+		HMAC_SHA1(pOutBuffer, FrameLen, DlsPTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		NdisZeroMemory(Mic,	sizeof(Mic));
+		hmac_md5(DlsPTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+	}
+
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+	                  sizeof(Header802_3),	Header802_3,
+		              Packet.Body_Len[1] + 4,	&Packet,
+		              END_OF_ARGS);
+
+	NStatus = RTMPAllocateNdisPacket(pAd, &pNdisPacket, NULL, 0, pOutBuffer, FrameLen);
+	if (NStatus == NDIS_STATUS_SUCCESS)
+	{
+		RTMP_SET_PACKET_WCID(pNdisPacket, BSSID_WCID);
+		STASendPacket(pAd, pNdisPacket);
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSendSTAKeyHandShake- Send STAKey Message-2 (NStatus=%x, FrameLen=%ld)\n", NStatus, FrameLen));
+
+	return NStatus;
+}
+
+VOID DlsTimeoutAction(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	MLME_DLS_REQ_STRUCT		MlmeDlsReq;
+	USHORT					reason;
+	PRT_802_11_DLS			pDLS = (PRT_802_11_DLS)FunctionContext;
+	PRTMP_ADAPTER			pAd = pDLS->pAd;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("DlsTimeout - Tear down DLS links (%02x:%02x:%02x:%02x:%02x:%02x)\n",
+		pDLS->MacAddr[0], pDLS->MacAddr[1], pDLS->MacAddr[2], pDLS->MacAddr[3], pDLS->MacAddr[4], pDLS->MacAddr[5]));
+
+	if ((pDLS) && (pDLS->Valid))
+	{
+		reason			= REASON_QOS_REQUEST_TIMEOUT;
+		pDLS->Valid		= FALSE;
+		pDLS->Status	= DLS_NONE;
+		DlsParmFill(pAd, &MlmeDlsReq, pDLS, reason);
+		MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+}
+
+/*
+================================================================
+Description : because DLS and CLI share the same WCID table in ASIC.
+Mesh entry also insert to pAd->MacTab.content[].  Such is marked as ValidAsDls = TRUE.
+Also fills the pairwise key.
+Because front MAX_AID_BA entries have direct mapping to BAEntry, which is only used as CLI. So we insert Dls
+from index MAX_AID_BA.
+================================================================
+*/
+MAC_TABLE_ENTRY *MacTableInsertDlsEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR	pAddr,
+	IN  UINT	DlsEntryIdx)
+{
+	PMAC_TABLE_ENTRY pEntry = NULL;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("====> MacTableInsertDlsEntry\n"));
+	// if FULL, return
+	if (pAd->MacTab.Size >= MAX_LEN_OF_MAC_TABLE)
+		return NULL;
+
+	do
+	{
+		if((pEntry = DlsEntryTableLookup(pAd, pAddr, TRUE)) != NULL)
+			break;
+
+		// allocate one MAC entry
+		pEntry = MacTableInsertEntry(pAd, pAddr, DlsEntryIdx + MIN_NET_DEVICE_FOR_DLS, TRUE);
+		if (pEntry)
+		{
+			pAd->StaCfg.DLSEntry[DlsEntryIdx].MacTabMatchWCID = pEntry->Aid;
+			pEntry->MatchDlsEntryIdx = DlsEntryIdx;
+			pEntry->AuthMode = pAd->StaCfg.AuthMode;
+			pEntry->WepStatus = pAd->StaCfg.WepStatus;
+			pEntry->PortSecured = WPA_802_1X_PORT_SECURED;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("MacTableInsertDlsEntry - allocate entry #%d, Total= %d\n",pEntry->Aid, pAd->MacTab.Size));
+
+			// If legacy WEP is used, set pair-wise cipherAlg into WCID attribute table for this entry
+			if ((pEntry->ValidAsDls) && (pEntry->WepStatus == Ndis802_11WEPEnabled))
+			{
+				UCHAR KeyIdx = 0;
+				UCHAR CipherAlg = 0;
+
+				KeyIdx	= pAd->StaCfg.DefaultKeyId;
+
+				CipherAlg 	= pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+
+				RTMPAddWcidAttributeEntry(pAd,
+											BSS0,
+											pAd->StaCfg.DefaultKeyId,
+											pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+											pEntry);
+			}
+
+			break;
+		}
+	} while(FALSE);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==== MacTableInsertDlsEntry\n"));
+
+	return pEntry;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Delete all Mesh Entry in pAd->MacTab
+	==========================================================================
+ */
+BOOLEAN MacTableDeleteDlsEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT wcid,
+	IN PUCHAR pAddr)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("====> MacTableDeleteDlsEntry\n"));
+
+	if (!VALID_WCID(wcid))
+		return FALSE;
+
+	MacTableDeleteEntry(pAd, wcid, pAddr);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==== MacTableDeleteDlsEntry\n"));
+
+	return TRUE;
+}
+
+MAC_TABLE_ENTRY *DlsEntryTableLookup(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount)
+{
+	ULONG HashIdx;
+	MAC_TABLE_ENTRY *pEntry = NULL;
+
+	RTMP_SEM_LOCK(&pAd->MacTabLock);
+	HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+	pEntry = pAd->MacTab.Hash[HashIdx];
+
+	while (pEntry)
+	{
+		if ((pEntry->ValidAsDls == TRUE)
+			&& MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
+		{
+			if(bResetIdelCount)
+				pEntry->NoDataIdleCount = 0;
+			break;
+		}
+		else
+			pEntry = pEntry->pNext;
+	}
+
+	RTMP_SEM_UNLOCK(&pAd->MacTabLock);
+	return pEntry;
+}
+
+MAC_TABLE_ENTRY *DlsEntryTableLookupByWcid(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	wcid,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount)
+{
+	ULONG DLsIndex;
+	PMAC_TABLE_ENTRY pCurEntry = NULL;
+	PMAC_TABLE_ENTRY pEntry = NULL;
+
+	if (!VALID_WCID(wcid))
+		return NULL;
+
+	RTMP_SEM_LOCK(&pAd->MacTabLock);
+
+	do
+	{
+		pCurEntry = &pAd->MacTab.Content[wcid];
+
+		DLsIndex = 0xff;
+		if ((pCurEntry) && (pCurEntry->ValidAsDls== TRUE))
+		{
+			DLsIndex = pCurEntry->MatchDlsEntryIdx;
+		}
+
+		if (DLsIndex == 0xff)
+			break;
+
+		if (MAC_ADDR_EQUAL(pCurEntry->Addr, pAddr))
+		{
+			if(bResetIdelCount)
+				pCurEntry->NoDataIdleCount = 0;
+			pEntry = pCurEntry;
+			break;
+		}
+	} while(FALSE);
+
+	RTMP_SEM_UNLOCK(&pAd->MacTabLock);
+
+	return pEntry;
+}
+
+INT Set_DlsEntryInfo_Display_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg)
+{
+	INT i;
+
+	printk("\n%-19s%-8s\n", "MAC", "TIMEOUT\n");
+	for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if ((pAd->StaCfg.DLSEntry[i].Valid) && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+		{
+			PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[pAd->StaCfg.DLSEntry[i].MacTabMatchWCID];
+
+			printk("%02x:%02x:%02x:%02x:%02x:%02x  ",
+				pAd->StaCfg.DLSEntry[i].MacAddr[0], pAd->StaCfg.DLSEntry[i].MacAddr[1], pAd->StaCfg.DLSEntry[i].MacAddr[2],
+				pAd->StaCfg.DLSEntry[i].MacAddr[3], pAd->StaCfg.DLSEntry[i].MacAddr[4], pAd->StaCfg.DLSEntry[i].MacAddr[5]);
+			printk("%-8d\n", pAd->StaCfg.DLSEntry[i].TimeOut);
+
+			printk("\n");
+			printk("\n%-19s%-4s%-4s%-4s%-4s%-7s%-7s%-7s","MAC", "AID", "BSS", "PSM", "WMM", "RSSI0", "RSSI1", "RSSI2");
+#ifdef DOT11_N_SUPPORT
+			printk("%-8s%-10s%-6s%-6s%-6s%-6s", "MIMOPS", "PhMd", "BW", "MCS", "SGI", "STBC");
+#endif // DOT11_N_SUPPORT //
+			printk("\n%02X:%02X:%02X:%02X:%02X:%02X  ",
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+			printk("%-4d", (int)pEntry->Aid);
+			printk("%-4d", (int)pEntry->apidx);
+			printk("%-4d", (int)pEntry->PsMode);
+			printk("%-4d", (int)CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE));
+			printk("%-7d", pEntry->RssiSample.AvgRssi0);
+			printk("%-7d", pEntry->RssiSample.AvgRssi1);
+			printk("%-7d", pEntry->RssiSample.AvgRssi2);
+#ifdef DOT11_N_SUPPORT
+			printk("%-8d", (int)pEntry->MmpsMode);
+			printk("%-10s", GetPhyMode(pEntry->HTPhyMode.field.MODE));
+			printk("%-6s", GetBW(pEntry->HTPhyMode.field.BW));
+			printk("%-6d", pEntry->HTPhyMode.field.MCS);
+			printk("%-6d", pEntry->HTPhyMode.field.ShortGI);
+			printk("%-6d", pEntry->HTPhyMode.field.STBC);
+#endif // DOT11_N_SUPPORT //
+			printk("%-10d, %d, %d%%\n", pEntry->DebugFIFOCount, pEntry->DebugTxCount,
+						(pEntry->DebugTxCount) ? ((pEntry->DebugTxCount-pEntry->DebugFIFOCount)*100/pEntry->DebugTxCount) : 0);
+			printk("\n");
+
+		}
+	}
+
+	return TRUE;
+}
+
+INT	Set_DlsAddEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR	mac[MAC_ADDR_LEN];
+	USHORT	Timeout;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    RT_802_11_DLS	Dls;
+
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and timeout value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		Timeout = simple_strtol((token+1), 0, 10);
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+	    printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%d", mac[0], mac[1],
+	           mac[2], mac[3], mac[4], mac[5], (int)Timeout);
+
+		NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+		Dls.TimeOut = Timeout;
+		COPY_MAC_ADDR(Dls.MacAddr, mac);
+		Dls.Valid = 1;
+
+		MlmeEnqueue(pAd,
+					MLME_CNTL_STATE_MACHINE,
+					RT_OID_802_11_SET_DLS_PARAM,
+					sizeof(RT_802_11_DLS),
+					&Dls);
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_DlsTearDownEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR			macAddr[MAC_ADDR_LEN];
+	CHAR			*value;
+	INT				i;
+	RT_802_11_DLS	Dls;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+	for (i=0, value = rstrtok(arg,":"); value; value = rstrtok(NULL,":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+		AtoH(value, &macAddr[i++], 2);
+	}
+
+	printk("\n%02x:%02x:%02x:%02x:%02x:%02x", macAddr[0], macAddr[1],
+	           macAddr[2], macAddr[3], macAddr[4], macAddr[5]);
+
+	NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+	COPY_MAC_ADDR(Dls.MacAddr, macAddr);
+	Dls.Valid = 0;
+
+	MlmeEnqueue(pAd,
+				MLME_CNTL_STATE_MACHINE,
+				RT_OID_802_11_SET_DLS_PARAM,
+				sizeof(RT_802_11_DLS),
+				&Dls);
+
+	return TRUE;
+}
+
diff --git a/drivers/staging/rt2870/sta/rtmp_data.c b/drivers/staging/rt2870/sta/rtmp_data.c
new file mode 100644
index 000000000000..9942ecaf5e2e
--- /dev/null
+++ b/drivers/staging/rt2870/sta/rtmp_data.c
@@ -0,0 +1,2619 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_data.c
+
+	Abstract:
+	Data path subroutines
+
+	Revision History:
+	Who 		When			What
+	--------	----------		----------------------------------------------
+	John		      Aug/17/04		major modification for RT2561/2661
+	Jan Lee	      Mar/17/06		major modification for RT2860 New Ring Design
+*/
+#include "../rt_config.h"
+
+
+VOID STARxEAPOLFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	PRT28XX_RXD_STRUC	pRxD = &(pRxBlk->RxD);
+	PRXWI_STRUC		pRxWI = pRxBlk->pRxWI;
+	UCHAR			*pTmpBuf;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAd->StaCfg.WpaSupplicantUP)
+	{
+		// All EAPoL frames have to pass to upper layer (ex. WPA_SUPPLICANT daemon)
+		// TBD : process fragmented EAPol frames
+		{
+			// In 802.1x mode, if the received frame is EAP-SUCCESS packet, turn on the PortSecured variable
+			if ( pAd->StaCfg.IEEE8021X == TRUE &&
+				 (EAP_CODE_SUCCESS == WpaCheckEapCode(pAd, pRxBlk->pData, pRxBlk->DataSize, LENGTH_802_1_H)))
+			{
+				PUCHAR	Key;
+				UCHAR 	CipherAlg;
+				int     idx = 0;
+
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("Receive EAP-SUCCESS Packet\n"));
+				//pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAd);
+
+                if (pAd->StaCfg.IEEE8021x_required_keys == FALSE)
+                {
+                    idx = pAd->StaCfg.DesireSharedKeyId;
+                    CipherAlg = pAd->StaCfg.DesireSharedKey[idx].CipherAlg;
+					Key = pAd->StaCfg.DesireSharedKey[idx].Key;
+
+                    if (pAd->StaCfg.DesireSharedKey[idx].KeyLen > 0)
+    				{
+#ifdef RT2870
+						union
+						{
+							char buf[sizeof(NDIS_802_11_WEP)+MAX_LEN_OF_KEY- 1];
+							NDIS_802_11_WEP keyinfo;
+						}  WepKey;
+						int len;
+
+
+						NdisZeroMemory(&WepKey, sizeof(WepKey));
+						len =pAd->StaCfg.DesireSharedKey[idx].KeyLen;
+
+						NdisMoveMemory(WepKey.keyinfo.KeyMaterial,
+							pAd->StaCfg.DesireSharedKey[idx].Key,
+							pAd->StaCfg.DesireSharedKey[idx].KeyLen);
+
+						WepKey.keyinfo.KeyIndex = 0x80000000 + idx;
+						WepKey.keyinfo.KeyLength = len;
+						pAd->SharedKey[BSS0][idx].KeyLen =(UCHAR) (len <= 5 ? 5 : 13);
+
+						pAd->IndicateMediaState = NdisMediaStateConnected;
+						pAd->ExtraInfo = GENERAL_LINK_UP;
+						// need to enqueue cmd to thread
+						RTUSBEnqueueCmdFromNdis(pAd, OID_802_11_ADD_WEP, TRUE, &WepKey, sizeof(WepKey.keyinfo) + len - 1);
+#endif // RT2870 //
+						// For Preventing ShardKey Table is cleared by remove key procedure.
+    					pAd->SharedKey[BSS0][idx].CipherAlg = CipherAlg;
+						pAd->SharedKey[BSS0][idx].KeyLen = pAd->StaCfg.DesireSharedKey[idx].KeyLen;
+						NdisMoveMemory(pAd->SharedKey[BSS0][idx].Key,
+									   pAd->StaCfg.DesireSharedKey[idx].Key,
+									   pAd->StaCfg.DesireSharedKey[idx].KeyLen);
+    				}
+				}
+			}
+
+			Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
+			return;
+		}
+	}
+	else
+#endif // WPA_SUPPLICANT_SUPPORT //
+	{
+		// Special DATA frame that has to pass to MLME
+		//	 1. Cisco Aironet frames for CCX2. We need pass it to MLME for special process
+		//	 2. EAPOL handshaking frames when driver supplicant enabled, pass to MLME for special process
+		{
+			pTmpBuf = pRxBlk->pData - LENGTH_802_11;
+			NdisMoveMemory(pTmpBuf, pRxBlk->pHeader, LENGTH_802_11);
+			REPORT_MGMT_FRAME_TO_MLME(pAd, pRxWI->WirelessCliID, pTmpBuf, pRxBlk->DataSize + LENGTH_802_11, pRxWI->RSSI0, pRxWI->RSSI1, pRxWI->RSSI2, pRxD->PlcpSignal);
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("!!! report EAPOL/AIRONET DATA to MLME (len=%d) !!!\n", pRxBlk->DataSize));
+		}
+	}
+
+	RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	return;
+
+}
+
+VOID STARxDataFrameAnnounce(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+
+	// non-EAP frame
+	if (!RTMPCheckWPAframe(pAd, pEntry, pRxBlk->pData, pRxBlk->DataSize, FromWhichBSSID))
+	{
+		{
+			// drop all non-EAP DATA frame before
+			// this client's Port-Access-Control is secured
+			if (pRxBlk->pHeader->FC.Wep)
+			{
+				// unsupported cipher suite
+				if (pAd->StaCfg.WepStatus == Ndis802_11EncryptionDisabled)
+				{
+					// release packet
+					RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+					return;
+				}
+			}
+			else
+			{
+				// encryption in-use but receive a non-EAPOL clear text frame, drop it
+				if ((pAd->StaCfg.WepStatus != Ndis802_11EncryptionDisabled) &&
+					(pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+				{
+					// release packet
+					RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+					return;
+				}
+			}
+		}
+		RX_BLK_CLEAR_FLAG(pRxBlk, fRX_EAP);
+		if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_ARALINK))
+		{
+			// Normal legacy, AMPDU or AMSDU
+			CmmRxnonRalinkFrameIndicate(pAd, pRxBlk, FromWhichBSSID);
+
+		}
+		else
+		{
+			// ARALINK
+			CmmRxRalinkFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
+		}
+#ifdef QOS_DLS_SUPPORT
+		RX_BLK_CLEAR_FLAG(pRxBlk, fRX_DLS);
+#endif // QOS_DLS_SUPPORT //
+	}
+	else
+	{
+		RX_BLK_SET_FLAG(pRxBlk, fRX_EAP);
+#ifdef DOT11_N_SUPPORT
+		if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMPDU) && (pAd->CommonCfg.bDisableReordering == 0))
+		{
+			Indicate_AMPDU_Packet(pAd, pRxBlk, FromWhichBSSID);
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			// Determin the destination of the EAP frame
+			//  to WPA state machine or upper layer
+			STARxEAPOLFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
+		}
+	}
+}
+
+
+// For TKIP frame, calculate the MIC value
+BOOLEAN STACheckTkipMICValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk)
+{
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	UCHAR			*pData = pRxBlk->pData;
+	USHORT			DataSize = pRxBlk->DataSize;
+	UCHAR			UserPriority = pRxBlk->UserPriority;
+	PCIPHER_KEY		pWpaKey;
+	UCHAR			*pDA, *pSA;
+
+	pWpaKey = &pAd->SharedKey[BSS0][pRxBlk->pRxWI->KeyIndex];
+
+	pDA = pHeader->Addr1;
+	if (RX_BLK_TEST_FLAG(pRxBlk, fRX_INFRA))
+	{
+		pSA = pHeader->Addr3;
+	}
+	else
+	{
+		pSA = pHeader->Addr2;
+	}
+
+	if (RTMPTkipCompareMICValue(pAd,
+								pData,
+								pDA,
+								pSA,
+								pWpaKey->RxMic,
+								UserPriority,
+								DataSize) == FALSE)
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR,("Rx MIC Value error 2\n"));
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+		if (pAd->StaCfg.WpaSupplicantUP)
+		{
+			WpaSendMicFailureToWpaSupplicant(pAd, (pWpaKey->Type == PAIRWISEKEY) ? TRUE : FALSE);
+		}
+		else
+#endif // WPA_SUPPLICANT_SUPPORT //
+		{
+			RTMPReportMicError(pAd, pWpaKey);
+		}
+
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+
+//
+// All Rx routines use RX_BLK structure to hande rx events
+// It is very important to build pRxBlk attributes
+//  1. pHeader pointer to 802.11 Header
+//  2. pData pointer to payload including LLC (just skip Header)
+//  3. set payload size including LLC to DataSize
+//  4. set some flags with RX_BLK_SET_FLAG()
+//
+VOID STAHandleRxDataFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+	PRT28XX_RXD_STRUC				pRxD = &(pRxBlk->RxD);
+	PRXWI_STRUC						pRxWI = pRxBlk->pRxWI;
+	PHEADER_802_11					pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET					pRxPacket = pRxBlk->pRxPacket;
+	BOOLEAN 						bFragment = FALSE;
+	MAC_TABLE_ENTRY	    			*pEntry = NULL;
+	UCHAR							FromWhichBSSID = BSS0;
+	UCHAR                           UserPriority = 0;
+
+	{
+		// before LINK UP, all DATA frames are rejected
+		if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+#ifdef QOS_DLS_SUPPORT
+		//if ((pHeader->FC.FrDs == 0) && (pHeader->FC.ToDs == 0))
+		if (RTMPRcvFrameDLSCheck(pAd, pHeader, pRxWI->MPDUtotalByteCount, pRxD))
+		{
+			return;
+		}
+#endif // QOS_DLS_SUPPORT //
+
+		// Drop not my BSS frames
+		if (pRxD->MyBss == 0)
+		{
+			{
+				// release packet
+				RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+				return;
+			}
+		}
+
+		pAd->RalinkCounters.RxCountSinceLastNULL++;
+		if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable && (pHeader->FC.SubType & 0x08))
+		{
+			UCHAR *pData;
+			DBGPRINT(RT_DEBUG_TRACE,("bAPSDCapable\n"));
+
+			// Qos bit 4
+			pData = (PUCHAR)pHeader + LENGTH_802_11;
+			if ((*pData >> 4) & 0x01)
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("RxDone- Rcv EOSP frame, driver may fall into sleep\n"));
+				pAd->CommonCfg.bInServicePeriod = FALSE;
+
+				// Force driver to fall into sleep mode when rcv EOSP frame
+				if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+				{
+					USHORT  TbttNumToNextWakeUp;
+					USHORT  NextDtim = pAd->StaCfg.DtimPeriod;
+					ULONG   Now;
+
+					NdisGetSystemUpTime(&Now);
+					NextDtim -= (USHORT)(Now - pAd->StaCfg.LastBeaconRxTime)/pAd->CommonCfg.BeaconPeriod;
+
+					TbttNumToNextWakeUp = pAd->StaCfg.DefaultListenCount;
+					if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM) && (TbttNumToNextWakeUp > NextDtim))
+						TbttNumToNextWakeUp = NextDtim;
+
+					MlmeSetPsmBit(pAd, PWR_SAVE);
+					// if WMM-APSD is failed, try to disable following line
+					AsicSleepThenAutoWakeup(pAd, TbttNumToNextWakeUp);
+				}
+			}
+
+			if ((pHeader->FC.MoreData) && (pAd->CommonCfg.bInServicePeriod))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("Sending another trigger frame when More Data bit is set to 1\n"));
+			}
+		}
+
+		// Drop NULL, CF-ACK(no data), CF-POLL(no data), and CF-ACK+CF-POLL(no data) data frame
+		if ((pHeader->FC.SubType & 0x04)) // bit 2 : no DATA
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+	    // Drop not my BSS frame (we can not only check the MyBss bit in RxD)
+#ifdef QOS_DLS_SUPPORT
+	    if (!pAd->CommonCfg.bDLSCapable)
+	    {
+#endif // QOS_DLS_SUPPORT //
+		if (INFRA_ON(pAd))
+		{
+			// Infrastructure mode, check address 2 for BSSID
+			if (!RTMPEqualMemory(&pHeader->Addr2, &pAd->CommonCfg.Bssid, 6))
+			{
+				// Receive frame not my BSSID
+	            // release packet
+	            RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+				return;
+			}
+		}
+		else	// Ad-Hoc mode or Not associated
+		{
+			// Ad-Hoc mode, check address 3 for BSSID
+			if (!RTMPEqualMemory(&pHeader->Addr3, &pAd->CommonCfg.Bssid, 6))
+			{
+				// Receive frame not my BSSID
+	            // release packet
+	            RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+				return;
+			}
+		}
+#ifdef QOS_DLS_SUPPORT
+	    }
+#endif // QOS_DLS_SUPPORT //
+
+		//
+		// find pEntry
+		//
+		if (pRxWI->WirelessCliID < MAX_LEN_OF_MAC_TABLE)
+		{
+			pEntry = &pAd->MacTab.Content[pRxWI->WirelessCliID];
+		}
+		else
+		{
+			// 1. release packet if infra mode
+			// 2. new a pEntry if ad-hoc mode
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+		// infra or ad-hoc
+		if (INFRA_ON(pAd))
+		{
+			RX_BLK_SET_FLAG(pRxBlk, fRX_INFRA);
+#ifdef QOS_DLS_SUPPORT
+			if ((pHeader->FC.FrDs == 0) && (pHeader->FC.ToDs == 0))
+				RX_BLK_SET_FLAG(pRxBlk, fRX_DLS);
+			else
+#endif // QOS_DLS_SUPPORT //
+			ASSERT(pRxWI->WirelessCliID == BSSID_WCID);
+		}
+
+		// check Atheros Client
+		if ((pEntry->bIAmBadAtheros == FALSE) &&  (pRxD->AMPDU == 1) && (pHeader->FC.Retry ))
+		{
+			pEntry->bIAmBadAtheros = TRUE;
+			pAd->CommonCfg.IOTestParm.bCurrentAtheros = TRUE;
+			pAd->CommonCfg.IOTestParm.bLastAtheros = TRUE;
+			if (!STA_AES_ON(pAd))
+			{
+				AsicUpdateProtect(pAd, 8, ALLN_SETPROTECT, TRUE, FALSE);
+			}
+		}
+	}
+
+	pRxBlk->pData = (UCHAR *)pHeader;
+
+	//
+	// update RxBlk->pData, DataSize
+	// 802.11 Header, QOS, HTC, Hw Padding
+	//
+
+	// 1. skip 802.11 HEADER
+	{
+		pRxBlk->pData += LENGTH_802_11;
+		pRxBlk->DataSize -= LENGTH_802_11;
+	}
+
+	// 2. QOS
+	if (pHeader->FC.SubType & 0x08)
+	{
+		RX_BLK_SET_FLAG(pRxBlk, fRX_QOS);
+		UserPriority = *(pRxBlk->pData) & 0x0f;
+		// bit 7 in QoS Control field signals the HT A-MSDU format
+		if ((*pRxBlk->pData) & 0x80)
+		{
+			RX_BLK_SET_FLAG(pRxBlk, fRX_AMSDU);
+		}
+
+		// skip QOS contorl field
+		pRxBlk->pData += 2;
+		pRxBlk->DataSize -=2;
+	}
+	pRxBlk->UserPriority = UserPriority;
+
+	// 3. Order bit: A-Ralink or HTC+
+	if (pHeader->FC.Order)
+	{
+#ifdef AGGREGATION_SUPPORT
+		if ((pRxWI->PHYMODE <= MODE_OFDM) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED)))
+		{
+			RX_BLK_SET_FLAG(pRxBlk, fRX_ARALINK);
+		}
+		else
+#endif
+		{
+#ifdef DOT11_N_SUPPORT
+			RX_BLK_SET_FLAG(pRxBlk, fRX_HTC);
+			// skip HTC contorl field
+			pRxBlk->pData += 4;
+			pRxBlk->DataSize -= 4;
+#endif // DOT11_N_SUPPORT //
+		}
+	}
+
+	// 4. skip HW padding
+	if (pRxD->L2PAD)
+	{
+		// just move pData pointer
+		// because DataSize excluding HW padding
+		RX_BLK_SET_FLAG(pRxBlk, fRX_PAD);
+		pRxBlk->pData += 2;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if (pRxD->BA)
+	{
+		RX_BLK_SET_FLAG(pRxBlk, fRX_AMPDU);
+	}
+#endif // DOT11_N_SUPPORT //
+
+
+	//
+	// Case I  Process Broadcast & Multicast data frame
+	//
+	if (pRxD->Bcast || pRxD->Mcast)
+	{
+		INC_COUNTER64(pAd->WlanCounters.MulticastReceivedFrameCount);
+
+		// Drop Mcast/Bcast frame with fragment bit on
+		if (pHeader->FC.MoreFrag)
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+		// Filter out Bcast frame which AP relayed for us
+		if (pHeader->FC.FrDs && MAC_ADDR_EQUAL(pHeader->Addr3, pAd->CurrentAddress))
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+		Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
+		return;
+	}
+	else if (pRxD->U2M)
+	{
+		pAd->LastRxRate = (USHORT)((pRxWI->MCS) + (pRxWI->BW <<7) + (pRxWI->ShortGI <<8)+ (pRxWI->PHYMODE <<14)) ;
+
+
+#ifdef QOS_DLS_SUPPORT
+        if (RX_BLK_TEST_FLAG(pRxBlk, fRX_DLS))
+		{
+			MAC_TABLE_ENTRY *pDlsEntry = NULL;
+
+			pDlsEntry = DlsEntryTableLookupByWcid(pAd, pRxWI->WirelessCliID, pHeader->Addr2, TRUE);
+										                        if(pDlsEntry)
+			Update_Rssi_Sample(pAd, &pDlsEntry->RssiSample, pRxWI);
+		}
+		else
+#endif // QOS_DLS_SUPPORT //
+		if (ADHOC_ON(pAd))
+		{
+			pEntry = MacTableLookup(pAd, pHeader->Addr2);
+			if (pEntry)
+				Update_Rssi_Sample(pAd, &pEntry->RssiSample, pRxWI);
+		}
+
+
+		Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, pRxWI);
+
+		pAd->StaCfg.LastSNR0 = (UCHAR)(pRxWI->SNR0);
+		pAd->StaCfg.LastSNR1 = (UCHAR)(pRxWI->SNR1);
+
+		pAd->RalinkCounters.OneSecRxOkDataCnt++;
+
+
+    	if (!((pHeader->Frag == 0) && (pHeader->FC.MoreFrag == 0)))
+    	{
+    		// re-assemble the fragmented packets
+    		// return complete frame (pRxPacket) or NULL
+    		bFragment = TRUE;
+    		pRxPacket = RTMPDeFragmentDataFrame(pAd, pRxBlk);
+    	}
+
+    	if (pRxPacket)
+    	{
+			pEntry = &pAd->MacTab.Content[pRxWI->WirelessCliID];
+
+    		// process complete frame
+    		if (bFragment && (pRxD->Decrypted) && (pEntry->WepStatus == Ndis802_11Encryption2Enabled))
+    		{
+				// Minus MIC length
+				pRxBlk->DataSize -= 8;
+
+    			// For TKIP frame, calculate the MIC value
+    			if (STACheckTkipMICValue(pAd, pEntry, pRxBlk) == FALSE)
+    			{
+    				return;
+    			}
+    		}
+
+    		STARxDataFrameAnnounce(pAd, pEntry, pRxBlk, FromWhichBSSID);
+			return;
+    	}
+    	else
+    	{
+    		// just return
+    		// because RTMPDeFragmentDataFrame() will release rx packet,
+    		// if packet is fragmented
+    		return;
+    	}
+	}
+
+	ASSERT(0);
+	// release packet
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+}
+
+VOID STAHandleRxMgmtFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+	PRT28XX_RXD_STRUC	pRxD = &(pRxBlk->RxD);
+	PRXWI_STRUC		pRxWI = pRxBlk->pRxWI;
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+
+	do
+	{
+
+		// We should collect RSSI not only U2M data but also my beacon
+		if ((pHeader->FC.SubType == SUBTYPE_BEACON) && (MAC_ADDR_EQUAL(&pAd->CommonCfg.Bssid, &pHeader->Addr2)))
+		{
+			Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, pRxWI);
+
+			pAd->StaCfg.LastSNR0 = (UCHAR)(pRxWI->SNR0);
+			pAd->StaCfg.LastSNR1 = (UCHAR)(pRxWI->SNR1);
+		}
+
+		// First check the size, it MUST not exceed the mlme queue size
+		if (pRxWI->MPDUtotalByteCount > MGMT_DMA_BUFFER_SIZE)
+		{
+			DBGPRINT_ERR(("STAHandleRxMgmtFrame: frame too large, size = %d \n", pRxWI->MPDUtotalByteCount));
+			break;
+		}
+
+		REPORT_MGMT_FRAME_TO_MLME(pAd, pRxWI->WirelessCliID, pHeader, pRxWI->MPDUtotalByteCount,
+									pRxWI->RSSI0, pRxWI->RSSI1, pRxWI->RSSI2, pRxD->PlcpSignal);
+	} while (FALSE);
+
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_SUCCESS);
+}
+
+VOID STAHandleRxControlFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+#ifdef DOT11_N_SUPPORT
+	PRXWI_STRUC		pRxWI = pRxBlk->pRxWI;
+#endif // DOT11_N_SUPPORT //
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+
+	switch (pHeader->FC.SubType)
+	{
+		case SUBTYPE_BLOCK_ACK_REQ:
+#ifdef DOT11_N_SUPPORT
+			{
+				CntlEnqueueForRecv(pAd, pRxWI->WirelessCliID, (pRxWI->MPDUtotalByteCount), (PFRAME_BA_REQ)pHeader);
+			}
+			break;
+#endif // DOT11_N_SUPPORT //
+		case SUBTYPE_BLOCK_ACK:
+		case SUBTYPE_ACK:
+		default:
+			break;
+	}
+
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process RxDone interrupt, running in DPC level
+
+	Arguments:
+		pAd Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		This routine has to maintain Rx ring read pointer.
+		Need to consider QOS DATA format when converting to 802.3
+	========================================================================
+*/
+BOOLEAN STARxDoneInterruptHandle(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	BOOLEAN			argc)
+{
+	NDIS_STATUS			Status;
+	UINT32			RxProcessed, RxPending;
+	BOOLEAN			bReschedule = FALSE;
+	RT28XX_RXD_STRUC	*pRxD;
+	UCHAR			*pData;
+	PRXWI_STRUC		pRxWI;
+	PNDIS_PACKET	pRxPacket;
+	PHEADER_802_11	pHeader;
+	RX_BLK			RxCell;
+
+	RxProcessed = RxPending = 0;
+
+	// process whole rx ring
+	while (1)
+	{
+
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF |
+								fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST) ||
+			!RTMP_TEST_FLAG(pAd,fRTMP_ADAPTER_START_UP))
+		{
+			break;
+		}
+
+
+		RxProcessed ++; // test
+
+		// 1. allocate a new data packet into rx ring to replace received packet
+		//    then processing the received packet
+		// 2. the callee must take charge of release of packet
+		// 3. As far as driver is concerned ,
+		//    the rx packet must
+		//      a. be indicated to upper layer or
+		//      b. be released if it is discarded
+		pRxPacket = GetPacketFromRxRing(pAd, &(RxCell.RxD), &bReschedule, &RxPending);
+		if (pRxPacket == NULL)
+		{
+			// no more packet to process
+			break;
+		}
+
+		// get rx ring descriptor
+		pRxD = &(RxCell.RxD);
+		// get rx data buffer
+		pData	= GET_OS_PKT_DATAPTR(pRxPacket);
+		pRxWI	= (PRXWI_STRUC) pData;
+		pHeader = (PHEADER_802_11) (pData+RXWI_SIZE) ;
+
+#ifdef RT_BIG_ENDIAN
+	    RTMPFrameEndianChange(pAd, (PUCHAR)pHeader, DIR_READ, TRUE);
+		RTMPWIEndianChange((PUCHAR)pRxWI, TYPE_RXWI);
+#endif
+
+		// build RxCell
+		RxCell.pRxWI = pRxWI;
+		RxCell.pHeader = pHeader;
+		RxCell.pRxPacket = pRxPacket;
+		RxCell.pData = (UCHAR *) pHeader;
+		RxCell.DataSize = pRxWI->MPDUtotalByteCount;
+		RxCell.Flags = 0;
+
+		// Increase Total receive byte counter after real data received no mater any error or not
+		pAd->RalinkCounters.ReceivedByteCount +=  pRxWI->MPDUtotalByteCount;
+		pAd->RalinkCounters.RxCount ++;
+
+		INC_COUNTER64(pAd->WlanCounters.ReceivedFragmentCount);
+
+		if (pRxWI->MPDUtotalByteCount < 14)
+			Status = NDIS_STATUS_FAILURE;
+
+        if (MONITOR_ON(pAd))
+		{
+            send_monitor_packets(pAd, &RxCell);
+			break;
+		}
+		/* RT2870 invokes STARxDoneInterruptHandle() in rtusb_bulk.c */
+#ifdef RALINK_ATE
+		if (ATE_ON(pAd))
+		{
+			pAd->ate.RxCntPerSec++;
+			ATESampleRssi(pAd, pRxWI);
+#ifdef RALINK_28xx_QA
+			if (pAd->ate.bQARxStart == TRUE)
+			{
+				/* (*pRxD) has been swapped in GetPacketFromRxRing() */
+				ATE_QA_Statistics(pAd, pRxWI, pRxD,	pHeader);
+			}
+#endif // RALINK_28xx_QA //
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_SUCCESS);
+			continue;
+		}
+#endif // RALINK_ATE //
+
+		// Check for all RxD errors
+		Status = RTMPCheckRxError(pAd, pHeader, pRxWI, pRxD);
+
+		// Handle the received frame
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			switch (pHeader->FC.Type)
+			{
+				// CASE I, receive a DATA frame
+				case BTYPE_DATA:
+				{
+					// process DATA frame
+					STAHandleRxDataFrame(pAd, &RxCell);
+				}
+				break;
+				// CASE II, receive a MGMT frame
+				case BTYPE_MGMT:
+				{
+					STAHandleRxMgmtFrame(pAd, &RxCell);
+				}
+				break;
+				// CASE III. receive a CNTL frame
+				case BTYPE_CNTL:
+				{
+					STAHandleRxControlFrame(pAd, &RxCell);
+				}
+				break;
+				// discard other type
+				default:
+					RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+					break;
+			}
+		}
+		else
+		{
+			pAd->Counters8023.RxErrors++;
+			// discard this frame
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+		}
+	}
+
+	return bReschedule;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPHandleTwakeupInterrupt(
+	IN PRTMP_ADAPTER pAd)
+{
+	AsicForceWakeup(pAd, FALSE);
+}
+
+/*
+========================================================================
+Routine Description:
+    Early checking and OS-depened parsing for Tx packet send to our STA driver.
+
+Arguments:
+    NDIS_HANDLE 	MiniportAdapterContext	Pointer refer to the device handle, i.e., the pAd.
+	PPNDIS_PACKET	ppPacketArray			The packet array need to do transmission.
+	UINT			NumberOfPackets			Number of packet in packet array.
+
+Return Value:
+	NONE
+
+Note:
+	This function do early checking and classification for send-out packet.
+	You only can put OS-depened & STA related code in here.
+========================================================================
+*/
+VOID STASendPackets(
+	IN	NDIS_HANDLE		MiniportAdapterContext,
+	IN	PPNDIS_PACKET	ppPacketArray,
+	IN	UINT			NumberOfPackets)
+{
+	UINT			Index;
+	PRTMP_ADAPTER	pAd = (PRTMP_ADAPTER) MiniportAdapterContext;
+	PNDIS_PACKET	pPacket;
+	BOOLEAN			allowToSend = FALSE;
+
+
+	for (Index = 0; Index < NumberOfPackets; Index++)
+	{
+		pPacket = ppPacketArray[Index];
+
+		do
+		{
+			if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+				RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS) ||
+				RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+			{
+				// Drop send request since hardware is in reset state
+					break;
+			}
+			else if (!INFRA_ON(pAd) && !ADHOC_ON(pAd))
+			{
+				// Drop send request since there are no physical connection yet
+					break;
+			}
+			else
+			{
+				// Record that orignal packet source is from NDIS layer,so that
+				// later on driver knows how to release this NDIS PACKET
+#ifdef QOS_DLS_SUPPORT
+				MAC_TABLE_ENTRY *pEntry;
+				PUCHAR pSrcBufVA = GET_OS_PKT_DATAPTR(pPacket);
+
+				pEntry = MacTableLookup(pAd, pSrcBufVA);
+				if (pEntry && (pEntry->ValidAsDls == TRUE))
+				{
+					RTMP_SET_PACKET_WCID(pPacket, pEntry->Aid);
+				}
+				else
+#endif // QOS_DLS_SUPPORT //
+				RTMP_SET_PACKET_WCID(pPacket, 0); // this field is useless when in STA mode
+				RTMP_SET_PACKET_SOURCE(pPacket, PKTSRC_NDIS);
+				NDIS_SET_PACKET_STATUS(pPacket, NDIS_STATUS_PENDING);
+				pAd->RalinkCounters.PendingNdisPacketCount++;
+
+				allowToSend = TRUE;
+			}
+		} while(FALSE);
+
+		if (allowToSend == TRUE)
+			STASendPacket(pAd, pPacket);
+		else
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+	}
+
+	// Dequeue outgoing frames from TxSwQueue[] and process it
+	RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+
+}
+
+
+/*
+========================================================================
+Routine Description:
+	This routine is used to do packet parsing and classification for Tx packet
+	to STA device, and it will en-queue packets to our TxSwQueue depends on AC
+	class.
+
+Arguments:
+	pAd    		Pointer to our adapter
+	pPacket 	Pointer to send packet
+
+Return Value:
+	NDIS_STATUS_SUCCESS			If succes to queue the packet into TxSwQueue.
+	NDIS_STATUS_FAILURE			If failed to do en-queue.
+
+Note:
+	You only can put OS-indepened & STA related code in here.
+========================================================================
+*/
+NDIS_STATUS STASendPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	UINT			AllowFragSize;
+	UCHAR			NumberOfFrag;
+//	UCHAR			RTSRequired;
+	UCHAR			QueIdx, UserPriority;
+	MAC_TABLE_ENTRY *pEntry = NULL;
+	unsigned int 	IrqFlags;
+	UCHAR			FlgIsIP = 0;
+	UCHAR			Rate;
+
+	// Prepare packet information structure for buffer descriptor
+	// chained within a single NDIS packet.
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+
+	if (pSrcBufVA == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("STASendPacket --> pSrcBufVA == NULL !!!SrcBufLen=%x\n",SrcBufLen));
+		// Resourece is low, system did not allocate virtual address
+		// return NDIS_STATUS_FAILURE directly to upper layer
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return NDIS_STATUS_FAILURE;
+	}
+
+
+	if (SrcBufLen < 14)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("STASendPacket --> Ndis Packet buffer error !!!\n"));
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return (NDIS_STATUS_FAILURE);
+	}
+
+	// In HT rate adhoc mode, A-MPDU is often used. So need to lookup BA Table and MAC Entry.
+	// Note multicast packets in adhoc also use BSSID_WCID index.
+	{
+		if(INFRA_ON(pAd))
+		{
+#ifdef QOS_DLS_SUPPORT
+			USHORT	tmpWcid;
+
+			tmpWcid = RTMP_GET_PACKET_WCID(pPacket);
+			if (VALID_WCID(tmpWcid) &&
+				(pAd->MacTab.Content[tmpWcid].ValidAsDls== TRUE))
+			{
+				pEntry = &pAd->MacTab.Content[tmpWcid];
+				Rate = pAd->MacTab.Content[tmpWcid].CurrTxRate;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+			{
+			pEntry = &pAd->MacTab.Content[BSSID_WCID];
+			RTMP_SET_PACKET_WCID(pPacket, BSSID_WCID);
+			Rate = pAd->CommonCfg.TxRate;
+		}
+		}
+		else if (ADHOC_ON(pAd))
+		{
+			if (*pSrcBufVA & 0x01)
+			{
+				RTMP_SET_PACKET_WCID(pPacket, MCAST_WCID);
+				pEntry = &pAd->MacTab.Content[MCAST_WCID];
+			}
+			else
+			{
+				pEntry = MacTableLookup(pAd, pSrcBufVA);
+			}
+			Rate = pAd->CommonCfg.TxRate;
+		}
+	}
+
+	if (!pEntry)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("STASendPacket->Cannot find pEntry(%2x:%2x:%2x:%2x:%2x:%2x) in MacTab!\n", PRINT_MAC(pSrcBufVA)));
+		// Resourece is low, system did not allocate virtual address
+		// return NDIS_STATUS_FAILURE directly to upper layer
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return NDIS_STATUS_FAILURE;
+	}
+
+	if (ADHOC_ON(pAd)
+		)
+	{
+		RTMP_SET_PACKET_WCID(pPacket, (UCHAR)pEntry->Aid);
+	}
+
+	//
+	// Check the Ethernet Frame type of this packet, and set the RTMP_SET_PACKET_SPECIFIC flags.
+	//		Here we set the PACKET_SPECIFIC flags(LLC, VLAN, DHCP/ARP, EAPOL).
+	RTMPCheckEtherType(pAd, pPacket);
+
+
+
+	//
+	// WPA 802.1x secured port control - drop all non-802.1x frame before port secured
+	//
+	if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+		 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+		 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+		 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+#ifdef WPA_SUPPLICANT_SUPPORT
+		  || (pAd->StaCfg.IEEE8021X == TRUE)
+#endif // WPA_SUPPLICANT_SUPPORT //
+#ifdef LEAP_SUPPORT
+		  || (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+		  )
+		  && ((pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED) || (pAd->StaCfg.MicErrCnt >= 2))
+		  && (RTMP_GET_PACKET_EAPOL(pPacket)== FALSE)
+		  )
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("STASendPacket --> Drop packet before port secured !!!\n"));
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+
+		return (NDIS_STATUS_FAILURE);
+	}
+
+
+	// STEP 1. Decide number of fragments required to deliver this MSDU.
+	//	   The estimation here is not very accurate because difficult to
+	//	   take encryption overhead into consideration here. The result
+	//	   "NumberOfFrag" is then just used to pre-check if enough free
+	//	   TXD are available to hold this MSDU.
+
+
+	if (*pSrcBufVA & 0x01)	// fragmentation not allowed on multicast & broadcast
+		NumberOfFrag = 1;
+	else if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED))
+		NumberOfFrag = 1;	// Aggregation overwhelms fragmentation
+	else if (CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_AMSDU_INUSED))
+		NumberOfFrag = 1;	// Aggregation overwhelms fragmentation
+#ifdef DOT11_N_SUPPORT
+	else if ((pAd->StaCfg.HTPhyMode.field.MODE == MODE_HTMIX) || (pAd->StaCfg.HTPhyMode.field.MODE == MODE_HTGREENFIELD))
+		NumberOfFrag = 1;	// MIMO RATE overwhelms fragmentation
+#endif // DOT11_N_SUPPORT //
+	else
+	{
+		// The calculated "NumberOfFrag" is a rough estimation because of various
+		// encryption/encapsulation overhead not taken into consideration. This number is just
+		// used to make sure enough free TXD are available before fragmentation takes place.
+		// In case the actual required number of fragments of an NDIS packet
+		// excceeds "NumberOfFrag"caculated here and not enough free TXD available, the
+		// last fragment (i.e. last MPDU) will be dropped in RTMPHardTransmit() due to out of
+		// resource, and the NDIS packet will be indicated NDIS_STATUS_FAILURE. This should
+		// rarely happen and the penalty is just like a TX RETRY fail. Affordable.
+
+		AllowFragSize = (pAd->CommonCfg.FragmentThreshold) - LENGTH_802_11 - LENGTH_CRC;
+		NumberOfFrag = ((PacketInfo.TotalPacketLength - LENGTH_802_3 + LENGTH_802_1_H) / AllowFragSize) + 1;
+		// To get accurate number of fragmentation, Minus 1 if the size just match to allowable fragment size
+		if (((PacketInfo.TotalPacketLength - LENGTH_802_3 + LENGTH_802_1_H) % AllowFragSize) == 0)
+		{
+			NumberOfFrag--;
+		}
+	}
+
+	// Save fragment number to Ndis packet reserved field
+	RTMP_SET_PACKET_FRAGMENTS(pPacket, NumberOfFrag);
+
+
+	// STEP 2. Check the requirement of RTS:
+	//	   If multiple fragment required, RTS is required only for the first fragment
+	//	   if the fragment size large than RTS threshold
+	//     For RT28xx, Let ASIC send RTS/CTS
+	RTMP_SET_PACKET_RTS(pPacket, 0);
+	RTMP_SET_PACKET_TXRATE(pPacket, pAd->CommonCfg.TxRate);
+
+	//
+	// STEP 3. Traffic classification. outcome = <UserPriority, QueIdx>
+	//
+	UserPriority = 0;
+	QueIdx		 = QID_AC_BE;
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) &&
+		CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE))
+	{
+		USHORT Protocol;
+		UCHAR  LlcSnapLen = 0, Byte0, Byte1;
+		do
+		{
+			// get Ethernet protocol field
+			Protocol = (USHORT)((pSrcBufVA[12] << 8) + pSrcBufVA[13]);
+			if (Protocol <= 1500)
+			{
+				// get Ethernet protocol field from LLC/SNAP
+				if (Sniff2BytesFromNdisBuffer(PacketInfo.pFirstBuffer, LENGTH_802_3 + 6, &Byte0, &Byte1) != NDIS_STATUS_SUCCESS)
+					break;
+
+				Protocol = (USHORT)((Byte0 << 8) + Byte1);
+				LlcSnapLen = 8;
+			}
+
+			// always AC_BE for non-IP packet
+			if (Protocol != 0x0800)
+				break;
+
+			// get IP header
+			if (Sniff2BytesFromNdisBuffer(PacketInfo.pFirstBuffer, LENGTH_802_3 + LlcSnapLen, &Byte0, &Byte1) != NDIS_STATUS_SUCCESS)
+				break;
+
+			// return AC_BE if packet is not IPv4
+			if ((Byte0 & 0xf0) != 0x40)
+				break;
+
+			FlgIsIP = 1;
+			UserPriority = (Byte1 & 0xe0) >> 5;
+			QueIdx = MapUserPriorityToAccessCategory[UserPriority];
+
+			// TODO: have to check ACM bit. apply TSPEC if ACM is ON
+			// TODO: downgrade UP & QueIdx before passing ACM
+			if (pAd->CommonCfg.APEdcaParm.bACM[QueIdx])
+			{
+				UserPriority = 0;
+				QueIdx		 = QID_AC_BE;
+			}
+		} while (FALSE);
+	}
+
+	RTMP_SET_PACKET_UP(pPacket, UserPriority);
+
+
+
+	// Make sure SendTxWait queue resource won't be used by other threads
+	RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
+	if (pAd->TxSwQueue[QueIdx].Number >= MAX_PACKETS_IN_QUEUE)
+	{
+		RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+#ifdef BLOCK_NET_IF
+		StopNetIfQueue(pAd, QueIdx, pPacket);
+#endif // BLOCK_NET_IF //
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+
+		return NDIS_STATUS_FAILURE;
+	}
+	else
+	{
+		InsertTailQueue(&pAd->TxSwQueue[QueIdx], PACKET_TO_QUEUE_ENTRY(pPacket));
+	}
+	RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+
+#ifdef DOT11_N_SUPPORT
+    if ((pAd->CommonCfg.BACapability.field.AutoBA == TRUE)&&
+        IS_HT_STA(pEntry))
+	{
+	    //PMAC_TABLE_ENTRY pMacEntry = &pAd->MacTab.Content[BSSID_WCID];
+		if (((pEntry->TXBAbitmap & (1<<UserPriority)) == 0) &&
+            ((pEntry->BADeclineBitmap & (1<<UserPriority)) == 0) &&
+            (pEntry->PortSecured == WPA_802_1X_PORT_SECURED)
+			 // For IOT compatibility, if
+			 // 1. It is Ralink chip or
+			 // 2. It is OPEN or AES mode,
+			 // then BA session can be bulit.
+			 && ((pEntry->ValidAsCLI && pAd->MlmeAux.APRalinkIe != 0x0) ||
+			 	 (pEntry->WepStatus == Ndis802_11WEPDisabled || pEntry->WepStatus == Ndis802_11Encryption3Enabled))
+			)
+		{
+			BAOriSessionSetUp(pAd, pEntry, 0, 0, 10, FALSE);
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	pAd->RalinkCounters.OneSecOsTxCount[QueIdx]++; // TODO: for debug only. to be removed
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		This subroutine will scan through releative ring descriptor to find
+		out avaliable free ring descriptor and compare with request size.
+
+	Arguments:
+		pAd Pointer to our adapter
+		QueIdx		Selected TX Ring
+
+	Return Value:
+		NDIS_STATUS_FAILURE 	Not enough free descriptor
+		NDIS_STATUS_SUCCESS 	Enough free descriptor
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+
+#ifdef RT2870
+/*
+	Actually, this function used to check if the TxHardware Queue still has frame need to send.
+	If no frame need to send, go to sleep, else, still wake up.
+*/
+NDIS_STATUS RTMPFreeTXDRequest(
+	IN		PRTMP_ADAPTER	pAd,
+	IN		UCHAR			QueIdx,
+	IN		UCHAR			NumberRequired,
+	IN		PUCHAR			FreeNumberIs)
+{
+	//ULONG		FreeNumber = 0;
+	NDIS_STATUS 	Status = NDIS_STATUS_FAILURE;
+	unsigned long   IrqFlags;
+	HT_TX_CONTEXT	*pHTTXContext;
+
+	switch (QueIdx)
+	{
+		case QID_AC_BK:
+		case QID_AC_BE:
+		case QID_AC_VI:
+		case QID_AC_VO:
+		case QID_HCCA:
+			{
+				pHTTXContext = &pAd->TxContext[QueIdx];
+				RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+				if ((pHTTXContext->CurWritePosition != pHTTXContext->ENextBulkOutPosition) ||
+					(pHTTXContext->IRPPending == TRUE))
+				{
+					Status = NDIS_STATUS_FAILURE;
+				}
+				else
+				{
+					Status = NDIS_STATUS_SUCCESS;
+				}
+				RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+			}
+			break;
+
+		case QID_MGMT:
+			if (pAd->MgmtRing.TxSwFreeIdx != MGMT_RING_SIZE)
+				Status = NDIS_STATUS_FAILURE;
+			else
+				Status = NDIS_STATUS_SUCCESS;
+			break;
+
+		default:
+			DBGPRINT(RT_DEBUG_ERROR,("RTMPFreeTXDRequest::Invalid QueIdx(=%d)\n", QueIdx));
+			break;
+	}
+
+	return (Status);
+
+}
+#endif // RT2870 //
+
+
+VOID RTMPSendDisassociationFrame(
+	IN	PRTMP_ADAPTER	pAd)
+{
+}
+
+VOID	RTMPSendNullFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			TxRate,
+	IN	BOOLEAN 		bQosNull)
+{
+	UCHAR	NullFrame[48];
+	ULONG	Length;
+	PHEADER_802_11	pHeader_802_11;
+
+
+#ifdef RALINK_ATE
+	if(ATE_ON(pAd))
+	{
+		return;
+	}
+#endif // RALINK_ATE //
+
+    // WPA 802.1x secured port control
+    if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+         (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+         (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+         (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+#ifdef WPA_SUPPLICANT_SUPPORT
+			  || (pAd->StaCfg.IEEE8021X == TRUE)
+#endif
+        ) &&
+       (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+	{
+		return;
+	}
+
+	NdisZeroMemory(NullFrame, 48);
+	Length = sizeof(HEADER_802_11);
+
+	pHeader_802_11 = (PHEADER_802_11) NullFrame;
+
+	pHeader_802_11->FC.Type = BTYPE_DATA;
+	pHeader_802_11->FC.SubType = SUBTYPE_NULL_FUNC;
+	pHeader_802_11->FC.ToDs = 1;
+	COPY_MAC_ADDR(pHeader_802_11->Addr1, pAd->CommonCfg.Bssid);
+	COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
+
+	if (pAd->CommonCfg.bAPSDForcePowerSave)
+	{
+		pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+	}
+	else
+	{
+		pHeader_802_11->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE) ? 1: 0;
+	}
+	pHeader_802_11->Duration = pAd->CommonCfg.Dsifs + RTMPCalcDuration(pAd, TxRate, 14);
+
+	pAd->Sequence++;
+	pHeader_802_11->Sequence = pAd->Sequence;
+
+	// Prepare QosNull function frame
+	if (bQosNull)
+	{
+		pHeader_802_11->FC.SubType = SUBTYPE_QOS_NULL;
+
+		// copy QOS control bytes
+		NullFrame[Length]	=  0;
+		NullFrame[Length+1] =  0;
+		Length += 2;// if pad with 2 bytes for alignment, APSD will fail
+	}
+
+	HAL_KickOutNullFrameTx(pAd, 0, NullFrame, Length);
+
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID	RTMPSendRTSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA,
+	IN	unsigned int	NextMpduSize,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			RTSRate,
+	IN	USHORT			AckDuration,
+	IN	UCHAR			QueIdx,
+	IN	UCHAR			FrameGap)
+{
+}
+
+
+
+// --------------------------------------------------------
+//  FIND ENCRYPT KEY AND DECIDE CIPHER ALGORITHM
+//		Find the WPA key, either Group or Pairwise Key
+//		LEAP + TKIP also use WPA key.
+// --------------------------------------------------------
+// Decide WEP bit and cipher suite to be used. Same cipher suite should be used for whole fragment burst
+// In Cisco CCX 2.0 Leap Authentication
+//		   WepStatus is Ndis802_11Encryption1Enabled but the key will use PairwiseKey
+//		   Instead of the SharedKey, SharedKey Length may be Zero.
+VOID STAFindCipherAlgorithm(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	NDIS_802_11_ENCRYPTION_STATUS	Cipher;				// To indicate cipher used for this packet
+	UCHAR							CipherAlg = CIPHER_NONE;		// cipher alogrithm
+	UCHAR							KeyIdx = 0xff;
+	PUCHAR							pSrcBufVA;
+	PCIPHER_KEY						pKey = NULL;
+
+	pSrcBufVA = GET_OS_PKT_DATAPTR(pTxBlk->pPacket);
+
+	{
+	    // Select Cipher
+	    if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd)))
+	        Cipher = pAd->StaCfg.GroupCipher; // Cipher for Multicast or Broadcast
+	    else
+	        Cipher = pAd->StaCfg.PairCipher; // Cipher for Unicast
+
+		if (RTMP_GET_PACKET_EAPOL(pTxBlk->pPacket))
+		{
+			ASSERT(pAd->SharedKey[BSS0][0].CipherAlg <= CIPHER_CKIP128);
+
+			// 4-way handshaking frame must be clear
+			if (!(TX_BLK_TEST_FLAG(pTxBlk, fTX_bClearEAPFrame)) && (pAd->SharedKey[BSS0][0].CipherAlg) &&
+				(pAd->SharedKey[BSS0][0].KeyLen))
+			{
+				CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+				KeyIdx = 0;
+			}
+		}
+		else if (Cipher == Ndis802_11Encryption1Enabled)
+		{
+#ifdef LEAP_SUPPORT
+			if (pAd->StaCfg.CkipFlag & 0x10) // Cisco CKIP KP is on
+			{
+				if (LEAP_CCKM_ON(pAd))
+				{
+					if (((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd))))
+						KeyIdx = 1;
+					else
+						KeyIdx = 0;
+				}
+				else
+					KeyIdx = pAd->StaCfg.DefaultKeyId;
+			}
+			else if (pAd->StaCfg.CkipFlag & 0x08) // only CKIP CMIC
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+			else if (LEAP_CCKM_ON(pAd))
+			{
+				if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd)))
+					KeyIdx = 1;
+				else
+					KeyIdx = 0;
+			}
+			else	// standard WEP64 or WEP128
+#endif // LEAP_SUPPORT //
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+		}
+		else if ((Cipher == Ndis802_11Encryption2Enabled) ||
+				 (Cipher == Ndis802_11Encryption3Enabled))
+		{
+			if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd))) // multicast
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+			else if (pAd->SharedKey[BSS0][0].KeyLen)
+				KeyIdx = 0;
+			else
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+		}
+
+		if (KeyIdx == 0xff)
+			CipherAlg = CIPHER_NONE;
+		else if ((Cipher == Ndis802_11EncryptionDisabled) || (pAd->SharedKey[BSS0][KeyIdx].KeyLen == 0))
+			CipherAlg = CIPHER_NONE;
+#ifdef WPA_SUPPLICANT_SUPPORT
+	    else if ( pAd->StaCfg.WpaSupplicantUP &&
+	             (Cipher == Ndis802_11Encryption1Enabled) &&
+	             (pAd->StaCfg.IEEE8021X == TRUE) &&
+	             (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+	        CipherAlg = CIPHER_NONE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+		else
+		{
+			//Header_802_11.FC.Wep = 1;
+			CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+			pKey = &pAd->SharedKey[BSS0][KeyIdx];
+		}
+	}
+
+	pTxBlk->CipherAlg = CipherAlg;
+	pTxBlk->pKey = pKey;
+}
+
+
+VOID STABuildCommon802_11Header(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  TX_BLK          *pTxBlk)
+{
+
+	HEADER_802_11	*pHeader_802_11;
+#ifdef QOS_DLS_SUPPORT
+	BOOLEAN	bDLSFrame = FALSE;
+	INT	DlsEntryIndex = 0;
+#endif // QOS_DLS_SUPPORT //
+
+	//
+	// MAKE A COMMON 802.11 HEADER
+	//
+
+	// normal wlan header size : 24 octets
+	pTxBlk->MpduHeaderLen = sizeof(HEADER_802_11);
+
+	pHeader_802_11 = (HEADER_802_11 *) &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+
+	NdisZeroMemory(pHeader_802_11, sizeof(HEADER_802_11));
+
+	pHeader_802_11->FC.FrDs = 0;
+	pHeader_802_11->FC.Type = BTYPE_DATA;
+	pHeader_802_11->FC.SubType = ((TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM)) ? SUBTYPE_QDATA : SUBTYPE_DATA);
+
+#ifdef QOS_DLS_SUPPORT
+	if (INFRA_ON(pAd))
+	{
+		// Check if the frame can be sent through DLS direct link interface
+		// If packet can be sent through DLS, then force aggregation disable. (Hard to determine peer STA's capability)
+		DlsEntryIndex = RTMPCheckDLSFrame(pAd, pTxBlk->pSrcBufHeader);
+		if (DlsEntryIndex >= 0)
+			bDLSFrame = TRUE;
+		else
+			bDLSFrame = FALSE;
+	}
+#endif // QOS_DLS_SUPPORT //
+
+    if (pTxBlk->pMacEntry)
+	{
+		if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bForceNonQoS))
+		{
+			pHeader_802_11->Sequence = pTxBlk->pMacEntry->NonQosDataSeq;
+			pTxBlk->pMacEntry->NonQosDataSeq = (pTxBlk->pMacEntry->NonQosDataSeq+1) & MAXSEQ;
+		}
+		else
+		{
+    	    pHeader_802_11->Sequence = pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority];
+    	    pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority] = (pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority]+1) & MAXSEQ;
+    	}
+	}
+	else
+	{
+		pHeader_802_11->Sequence = pAd->Sequence;
+		pAd->Sequence = (pAd->Sequence+1) & MAXSEQ; // next sequence
+	}
+
+	pHeader_802_11->Frag = 0;
+
+	pHeader_802_11->FC.MoreData = TX_BLK_TEST_FLAG(pTxBlk, fTX_bMoreData);
+
+	{
+		if (INFRA_ON(pAd))
+		{
+#ifdef QOS_DLS_SUPPORT
+			if (bDLSFrame)
+			{
+				COPY_MAC_ADDR(pHeader_802_11->Addr1, pTxBlk->pSrcBufHeader);
+				COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+				COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
+				pHeader_802_11->FC.ToDs = 0;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+			{
+			COPY_MAC_ADDR(pHeader_802_11->Addr1, pAd->CommonCfg.Bssid);
+			COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+			COPY_MAC_ADDR(pHeader_802_11->Addr3, pTxBlk->pSrcBufHeader);
+			pHeader_802_11->FC.ToDs = 1;
+		}
+		}
+		else if (ADHOC_ON(pAd))
+		{
+			COPY_MAC_ADDR(pHeader_802_11->Addr1, pTxBlk->pSrcBufHeader);
+			COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+			COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
+			pHeader_802_11->FC.ToDs = 0;
+		}
+	}
+
+	if (pTxBlk->CipherAlg != CIPHER_NONE)
+		pHeader_802_11->FC.Wep = 1;
+
+	// -----------------------------------------------------------------
+	// STEP 2. MAKE A COMMON 802.11 HEADER SHARED BY ENTIRE FRAGMENT BURST. Fill sequence later.
+	// -----------------------------------------------------------------
+	if (pAd->CommonCfg.bAPSDForcePowerSave)
+    	pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+	else
+    	pHeader_802_11->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
+}
+
+#ifdef DOT11_N_SUPPORT
+VOID STABuildCache802_11Header(
+	IN RTMP_ADAPTER		*pAd,
+	IN TX_BLK			*pTxBlk,
+	IN UCHAR			*pHeader)
+{
+	MAC_TABLE_ENTRY	*pMacEntry;
+	PHEADER_802_11	pHeader80211;
+
+	pHeader80211 = (PHEADER_802_11)pHeader;
+	pMacEntry = pTxBlk->pMacEntry;
+
+	//
+	// Update the cached 802.11 HEADER
+	//
+
+	// normal wlan header size : 24 octets
+	pTxBlk->MpduHeaderLen = sizeof(HEADER_802_11);
+
+	// More Bit
+	pHeader80211->FC.MoreData = TX_BLK_TEST_FLAG(pTxBlk, fTX_bMoreData);
+
+	// Sequence
+	pHeader80211->Sequence = pMacEntry->TxSeq[pTxBlk->UserPriority];
+    pMacEntry->TxSeq[pTxBlk->UserPriority] = (pMacEntry->TxSeq[pTxBlk->UserPriority]+1) & MAXSEQ;
+
+	{
+		// Check if the frame can be sent through DLS direct link interface
+		// If packet can be sent through DLS, then force aggregation disable. (Hard to determine peer STA's capability)
+#ifdef QOS_DLS_SUPPORT
+		BOOLEAN	bDLSFrame = FALSE;
+		INT	DlsEntryIndex = 0;
+
+		DlsEntryIndex = RTMPCheckDLSFrame(pAd, pTxBlk->pSrcBufHeader);
+		if (DlsEntryIndex >= 0)
+			bDLSFrame = TRUE;
+		else
+			bDLSFrame = FALSE;
+#endif // QOS_DLS_SUPPORT //
+
+		// The addr3 of normal packet send from DS is Dest Mac address.
+#ifdef QOS_DLS_SUPPORT
+		if (bDLSFrame)
+		{
+			COPY_MAC_ADDR(pHeader80211->Addr1, pTxBlk->pSrcBufHeader);
+			COPY_MAC_ADDR(pHeader80211->Addr3, pAd->CommonCfg.Bssid);
+			pHeader80211->FC.ToDs = 0;
+		}
+		else
+#endif // QOS_DLS_SUPPORT //
+		if (ADHOC_ON(pAd))
+			COPY_MAC_ADDR(pHeader80211->Addr3, pAd->CommonCfg.Bssid);
+		else
+			COPY_MAC_ADDR(pHeader80211->Addr3, pTxBlk->pSrcBufHeader);
+	}
+
+	// -----------------------------------------------------------------
+	// STEP 2. MAKE A COMMON 802.11 HEADER SHARED BY ENTIRE FRAGMENT BURST. Fill sequence later.
+	// -----------------------------------------------------------------
+	if (pAd->CommonCfg.bAPSDForcePowerSave)
+    	pHeader80211->FC.PwrMgmt = PWR_SAVE;
+	else
+    	pHeader80211->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
+}
+#endif // DOT11_N_SUPPORT //
+
+static inline PUCHAR STA_Build_ARalink_Frame_Header(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK		*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;
+	HEADER_802_11	*pHeader_802_11;
+	PNDIS_PACKET	pNextPacket;
+	UINT32			nextBufLen;
+	PQUEUE_ENTRY	pQEntry;
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+	// steal "order" bit to mark "aggregation"
+	pHeader_802_11->FC.Order = 1;
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
+	{
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+	}
+
+	// padding at front of LLC header. LLC header should at 4-bytes aligment.
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR)ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+	// For RA Aggregation,
+	// put the 2nd MSDU length(extra 2-byte field) after QOS_CONTROL in little endian format
+	pQEntry = pTxBlk->TxPacketList.Head;
+	pNextPacket = QUEUE_ENTRY_TO_PKT(pQEntry);
+	nextBufLen = GET_OS_PKT_LEN(pNextPacket);
+	if (RTMP_GET_PACKET_VLAN(pNextPacket))
+		nextBufLen -= LENGTH_802_1Q;
+
+	*pHeaderBufPtr = (UCHAR)nextBufLen & 0xff;
+	*(pHeaderBufPtr+1) = (UCHAR)(nextBufLen >> 8);
+
+	pHeaderBufPtr += 2;
+	pTxBlk->MpduHeaderLen += 2;
+
+	return pHeaderBufPtr;
+
+}
+
+#ifdef DOT11_N_SUPPORT
+static inline PUCHAR STA_Build_AMSDU_Frame_Header(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK		*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;//, pSaveBufPtr;
+	HEADER_802_11	*pHeader_802_11;
+
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	//
+	// build QOS Control bytes
+	//
+	*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+
+	//
+	// A-MSDU packet
+	//
+	*pHeaderBufPtr |= 0x80;
+
+	*(pHeaderBufPtr+1) = 0;
+	pHeaderBufPtr +=2;
+	pTxBlk->MpduHeaderLen += 2;
+
+	//pSaveBufPtr = pHeaderBufPtr;
+
+	//
+	// padding at front of LLC header
+	// LLC header should locate at 4-octets aligment
+	//
+	// @@@ MpduHeaderLen excluding padding @@@
+	//
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+	return pHeaderBufPtr;
+
+}
+
+
+VOID STA_AMPDU_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	HEADER_802_11	*pHeader_802_11;
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	MAC_TABLE_ENTRY	*pMacEntry;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+	ASSERT(pTxBlk);
+
+	while(pTxBlk->TxPacketList.Head)
+	{
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+		if ( RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+		{
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			continue;
+		}
+
+		bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+		pMacEntry = pTxBlk->pMacEntry;
+		if (pMacEntry->isCached)
+		{
+			// NOTE: Please make sure the size of pMacEntry->CachedBuf[] is smaller than pTxBlk->HeaderBuf[]!!!!
+			NdisMoveMemory((PUCHAR)&pTxBlk->HeaderBuf[TXINFO_SIZE], (PUCHAR)&pMacEntry->CachedBuf[0], TXWI_SIZE + sizeof(HEADER_802_11));
+			pHeaderBufPtr = (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE]);
+			STABuildCache802_11Header(pAd, pTxBlk, pHeaderBufPtr);
+		}
+		else
+		{
+			STAFindCipherAlgorithm(pAd, pTxBlk);
+			STABuildCommon802_11Header(pAd, pTxBlk);
+
+			pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+		}
+
+
+		pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+		// skip common header
+		pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+
+		//
+		// build HTC+
+		// HTC control filed following QoS field
+		//
+		if ((pAd->CommonCfg.bRdg == TRUE) && CLIENT_STATUS_TEST_FLAG(pTxBlk->pMacEntry, fCLIENT_STATUS_RDG_CAPABLE))
+		{
+			if (pMacEntry->isCached == FALSE)
+			{
+				// mark HTC bit
+				pHeader_802_11->FC.Order = 1;
+
+				NdisZeroMemory(pHeaderBufPtr, 4);
+				*(pHeaderBufPtr+3) |= 0x80;
+			}
+			pHeaderBufPtr += 4;
+			pTxBlk->MpduHeaderLen += 4;
+		}
+
+		//pTxBlk->MpduHeaderLen = pHeaderBufPtr - pTxBlk->HeaderBuf - TXWI_SIZE - TXINFO_SIZE;
+		ASSERT(pTxBlk->MpduHeaderLen >= 24);
+
+		// skip 802.3 header
+		pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+		pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+		// skip vlan tag
+		if (bVLANPkt)
+		{
+			pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+			pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+		}
+
+		//
+		// padding at front of LLC header
+		// LLC header should locate at 4-octets aligment
+		//
+		// @@@ MpduHeaderLen excluding padding @@@
+		//
+		pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+		pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+		pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+		{
+
+			//
+			// Insert LLC-SNAP encapsulation - 8 octets
+			//
+			EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
+			if (pTxBlk->pExtraLlcSnapEncap)
+			{
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+				pHeaderBufPtr += 6;
+				// get 2 octets (TypeofLen)
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+				pHeaderBufPtr += 2;
+				pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+			}
+
+		}
+
+		if (pMacEntry->isCached)
+		{
+            RTMPWriteTxWI_Cache(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+		}
+		else
+		{
+			RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+			NdisZeroMemory((PUCHAR)(&pMacEntry->CachedBuf[0]), sizeof(pMacEntry->CachedBuf));
+			NdisMoveMemory((PUCHAR)(&pMacEntry->CachedBuf[0]), (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), (pHeaderBufPtr - (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE])));
+			pMacEntry->isCached = TRUE;
+		}
+
+		// calculate Transmitted AMPDU count and ByteCount
+		{
+			pAd->RalinkCounters.TransmittedMPDUsInAMPDUCount.u.LowPart ++;
+			pAd->RalinkCounters.TransmittedOctetsInAMPDUCount.QuadPart += pTxBlk->SrcBufLen;
+		}
+
+		//FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
+
+		HAL_WriteTxResource(pAd, pTxBlk, TRUE, &FreeNumber);
+
+		//
+		// Kick out Tx
+		//
+		HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+	}
+
+}
+
+
+VOID STA_AMSDU_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	USHORT			subFramePayloadLen = 0;	// AMSDU Subframe length without AMSDU-Header / Padding.
+	USHORT			totalMPDUSize=0;
+	UCHAR			*subFrameHeader;
+	UCHAR			padding = 0;
+	USHORT			FirstTx = 0, LastTxIdx = 0;
+	BOOLEAN			bVLANPkt;
+	int 			frameNum = 0;
+	PQUEUE_ENTRY	pQEntry;
+
+
+	ASSERT(pTxBlk);
+
+	ASSERT((pTxBlk->TxPacketList.Number > 1));
+
+	while(pTxBlk->TxPacketList.Head)
+	{
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+		if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+		{
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			continue;
+		}
+
+		bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+		// skip 802.3 header
+		pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+		pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+		// skip vlan tag
+		if (bVLANPkt)
+		{
+			pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+			pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+		}
+
+		if (frameNum == 0)
+		{
+			pHeaderBufPtr = STA_Build_AMSDU_Frame_Header(pAd, pTxBlk);
+
+			// NOTE: TxWI->MPDUtotalByteCount will be updated after final frame was handled.
+			RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+		}
+		else
+		{
+			pHeaderBufPtr = &pTxBlk->HeaderBuf[0];
+			padding = ROUND_UP(LENGTH_AMSDU_SUBFRAMEHEAD + subFramePayloadLen, 4) - (LENGTH_AMSDU_SUBFRAMEHEAD + subFramePayloadLen);
+			NdisZeroMemory(pHeaderBufPtr, padding + LENGTH_AMSDU_SUBFRAMEHEAD);
+			pHeaderBufPtr += padding;
+			pTxBlk->MpduHeaderLen = padding;
+		}
+
+		//
+		// A-MSDU subframe
+		//   DA(6)+SA(6)+Length(2) + LLC/SNAP Encap
+		//
+		subFrameHeader = pHeaderBufPtr;
+		subFramePayloadLen = pTxBlk->SrcBufLen;
+
+		NdisMoveMemory(subFrameHeader, pTxBlk->pSrcBufHeader, 12);
+
+
+		pHeaderBufPtr += LENGTH_AMSDU_SUBFRAMEHEAD;
+		pTxBlk->MpduHeaderLen += LENGTH_AMSDU_SUBFRAMEHEAD;
+
+
+		//
+		// Insert LLC-SNAP encapsulation - 8 octets
+		//
+		EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
+
+		subFramePayloadLen = pTxBlk->SrcBufLen;
+
+		if (pTxBlk->pExtraLlcSnapEncap)
+		{
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+			pHeaderBufPtr += 6;
+			// get 2 octets (TypeofLen)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+			pHeaderBufPtr += 2;
+			pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+			subFramePayloadLen += LENGTH_802_1_H;
+		}
+
+		// update subFrame Length field
+		subFrameHeader[12] = (subFramePayloadLen & 0xFF00) >> 8;
+		subFrameHeader[13] = subFramePayloadLen & 0xFF;
+
+		totalMPDUSize += pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+
+		if (frameNum ==0)
+			FirstTx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+		else
+			LastTxIdx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+
+		frameNum++;
+
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+
+		// calculate Transmitted AMSDU Count and ByteCount
+		{
+			pAd->RalinkCounters.TransmittedAMSDUCount.u.LowPart ++;
+			pAd->RalinkCounters.TransmittedOctetsInAMSDU.QuadPart += totalMPDUSize;
+		}
+
+	}
+
+	HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, FirstTx);
+	HAL_LastTxIdx(pAd, pTxBlk->QueIdx, LastTxIdx);
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+}
+#endif // DOT11_N_SUPPORT //
+
+VOID STA_Legacy_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	HEADER_802_11	*pHeader_802_11;
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+	ASSERT(pTxBlk);
+
+
+	pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+	pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+	if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+	{
+		RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	if (pTxBlk->TxFrameType == TX_MCAST_FRAME)
+	{
+		INC_COUNTER64(pAd->WlanCounters.MulticastTransmittedFrameCount);
+	}
+
+	if (RTMP_GET_PACKET_RTS(pTxBlk->pPacket))
+		TX_BLK_SET_FLAG(pTxBlk, fTX_bRtsRequired);
+	else
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bRtsRequired);
+
+	bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+	if (pTxBlk->TxRate < pAd->CommonCfg.MinTxRate)
+		pTxBlk->TxRate = pAd->CommonCfg.MinTxRate;
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+
+	// skip 802.3 header
+	pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+	pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+	// skip vlan tag
+	if (bVLANPkt)
+	{
+		pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+		pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+	}
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
+	{
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+	}
+
+	// The remaining content of MPDU header should locate at 4-octets aligment
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+	{
+
+		//
+		// Insert LLC-SNAP encapsulation - 8 octets
+		//
+		//
+   		// if original Ethernet frame contains no LLC/SNAP,
+		// then an extra LLC/SNAP encap is required
+		//
+		EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(pTxBlk->pSrcBufHeader, pTxBlk->pExtraLlcSnapEncap);
+		if (pTxBlk->pExtraLlcSnapEncap)
+		{
+			UCHAR vlan_size;
+
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+			pHeaderBufPtr += 6;
+			// skip vlan tag
+			vlan_size =  (bVLANPkt) ? LENGTH_802_1Q : 0;
+			// get 2 octets (TypeofLen)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader+12+vlan_size, 2);
+			pHeaderBufPtr += 2;
+			pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+		}
+
+	}
+
+	//
+	// prepare for TXWI
+	// use Wcid as Key Index
+	//
+
+	RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+	//FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
+
+	HAL_WriteTxResource(pAd, pTxBlk, TRUE, &FreeNumber);
+
+	pAd->RalinkCounters.KickTxCount++;
+	pAd->RalinkCounters.OneSecTxDoneCount++;
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+}
+
+
+VOID STA_ARalink_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	USHORT			totalMPDUSize=0;
+	USHORT			FirstTx, LastTxIdx;
+	int 			frameNum = 0;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+
+	ASSERT(pTxBlk);
+
+	ASSERT((pTxBlk->TxPacketList.Number== 2));
+
+
+	FirstTx = LastTxIdx = 0;  // Is it ok init they as 0?
+	while(pTxBlk->TxPacketList.Head)
+	{
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+
+		if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+		{
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			continue;
+		}
+
+		bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+		// skip 802.3 header
+		pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+		pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+		// skip vlan tag
+		if (bVLANPkt)
+		{
+			pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+			pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+		}
+
+		if (frameNum == 0)
+		{	// For first frame, we need to create the 802.11 header + padding(optional) + RA-AGG-LEN + SNAP Header
+
+			pHeaderBufPtr = STA_Build_ARalink_Frame_Header(pAd, pTxBlk);
+
+			// It's ok write the TxWI here, because the TxWI->MPDUtotalByteCount
+			//	will be updated after final frame was handled.
+			RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+
+			//
+			// Insert LLC-SNAP encapsulation - 8 octets
+			//
+			EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
+
+			if (pTxBlk->pExtraLlcSnapEncap)
+			{
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+				pHeaderBufPtr += 6;
+				// get 2 octets (TypeofLen)
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+				pHeaderBufPtr += 2;
+				pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+			}
+		}
+		else
+		{	// For second aggregated frame, we need create the 802.3 header to headerBuf, because PCI will copy it to SDPtr0.
+
+			pHeaderBufPtr = &pTxBlk->HeaderBuf[0];
+			pTxBlk->MpduHeaderLen = 0;
+
+			// A-Ralink sub-sequent frame header is the same as 802.3 header.
+			//   DA(6)+SA(6)+FrameType(2)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader, 12);
+			pHeaderBufPtr += 12;
+			// get 2 octets (TypeofLen)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+			pHeaderBufPtr += 2;
+			pTxBlk->MpduHeaderLen = LENGTH_ARALINK_SUBFRAMEHEAD;
+		}
+
+		totalMPDUSize += pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+
+		//FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
+		if (frameNum ==0)
+			FirstTx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+		else
+			LastTxIdx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+
+		frameNum++;
+
+		pAd->RalinkCounters.OneSecTxAggregationCount++;
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+
+	}
+
+	HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, FirstTx);
+	HAL_LastTxIdx(pAd, pTxBlk->QueIdx, LastTxIdx);
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+
+}
+
+
+VOID STA_Fragment_Frame_Tx(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK		*pTxBlk)
+{
+	HEADER_802_11	*pHeader_802_11;
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	UCHAR 			fragNum = 0;
+	PACKET_INFO		PacketInfo;
+	USHORT			EncryptionOverhead = 0;
+	UINT32			FreeMpduSize, SrcRemainingBytes;
+	USHORT			AckDuration;
+	UINT 			NextMpduSize;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+
+	ASSERT(pTxBlk);
+
+	pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+	pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+	if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+	{
+		RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	ASSERT(TX_BLK_TEST_FLAG(pTxBlk, fTX_bAllowFrag));
+	bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+	if (pTxBlk->CipherAlg == CIPHER_TKIP)
+	{
+		pTxBlk->pPacket = duplicate_pkt_with_TKIP_MIC(pAd, pTxBlk->pPacket);
+		if (pTxBlk->pPacket == NULL)
+			return;
+		RTMP_QueryPacketInfo(pTxBlk->pPacket, &PacketInfo, &pTxBlk->pSrcBufHeader, &pTxBlk->SrcBufLen);
+	}
+
+	// skip 802.3 header
+	pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+	pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+
+	// skip vlan tag
+	if (bVLANPkt)
+	{
+		pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+		pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+	}
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *)pHeaderBufPtr;
+
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
+	{
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+	}
+
+	//
+	// padding at front of LLC header
+	// LLC header should locate at 4-octets aligment
+	//
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+
+
+	//
+	// Insert LLC-SNAP encapsulation - 8 octets
+	//
+	//
+   	// if original Ethernet frame contains no LLC/SNAP,
+	// then an extra LLC/SNAP encap is required
+	//
+	EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(pTxBlk->pSrcBufHeader, pTxBlk->pExtraLlcSnapEncap);
+	if (pTxBlk->pExtraLlcSnapEncap)
+	{
+		UCHAR vlan_size;
+
+		NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+		pHeaderBufPtr += 6;
+		// skip vlan tag
+		vlan_size =  (bVLANPkt) ? LENGTH_802_1Q : 0;
+		// get 2 octets (TypeofLen)
+		NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader+12+vlan_size, 2);
+		pHeaderBufPtr += 2;
+		pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+	}
+
+
+	// If TKIP is used and fragmentation is required. Driver has to
+	//	append TKIP MIC at tail of the scatter buffer
+	//	MAC ASIC will only perform IV/EIV/ICV insertion but no TKIP MIC
+	if (pTxBlk->CipherAlg == CIPHER_TKIP)
+	{
+
+		// NOTE: DON'T refer the skb->len directly after following copy. Becasue the length is not adjust
+		//			to correct lenght, refer to pTxBlk->SrcBufLen for the packet length in following progress.
+		NdisMoveMemory(pTxBlk->pSrcBufData + pTxBlk->SrcBufLen, &pAd->PrivateInfo.Tx.MIC[0], 8);
+		//skb_put((RTPKT_TO_OSPKT(pTxBlk->pPacket))->tail, 8);
+		pTxBlk->SrcBufLen += 8;
+		pTxBlk->TotalFrameLen += 8;
+		pTxBlk->CipherAlg = CIPHER_TKIP_NO_MIC;
+	}
+
+	//
+	// calcuate the overhead bytes that encryption algorithm may add. This
+	// affects the calculate of "duration" field
+	//
+	if ((pTxBlk->CipherAlg == CIPHER_WEP64) || (pTxBlk->CipherAlg == CIPHER_WEP128))
+		EncryptionOverhead = 8; //WEP: IV[4] + ICV[4];
+	else if (pTxBlk->CipherAlg == CIPHER_TKIP_NO_MIC)
+		EncryptionOverhead = 12;//TKIP: IV[4] + EIV[4] + ICV[4], MIC will be added to TotalPacketLength
+	else if (pTxBlk->CipherAlg == CIPHER_TKIP)
+		EncryptionOverhead = 20;//TKIP: IV[4] + EIV[4] + ICV[4] + MIC[8]
+	else if (pTxBlk->CipherAlg == CIPHER_AES)
+		EncryptionOverhead = 16;	// AES: IV[4] + EIV[4] + MIC[8]
+	else
+		EncryptionOverhead = 0;
+
+	// decide how much time an ACK/CTS frame will consume in the air
+	AckDuration = RTMPCalcDuration(pAd, pAd->CommonCfg.ExpectedACKRate[pTxBlk->TxRate], 14);
+
+	// Init the total payload length of this frame.
+	SrcRemainingBytes = pTxBlk->SrcBufLen;
+
+	pTxBlk->TotalFragNum = 0xff;
+
+	do {
+
+		FreeMpduSize = pAd->CommonCfg.FragmentThreshold - LENGTH_CRC;
+
+		FreeMpduSize -= pTxBlk->MpduHeaderLen;
+
+		if (SrcRemainingBytes <= FreeMpduSize)
+		{	// this is the last or only fragment
+
+			pTxBlk->SrcBufLen = SrcRemainingBytes;
+
+			pHeader_802_11->FC.MoreFrag = 0;
+			pHeader_802_11->Duration = pAd->CommonCfg.Dsifs + AckDuration;
+
+			// Indicate the lower layer that this's the last fragment.
+			pTxBlk->TotalFragNum = fragNum;
+		}
+		else
+		{	// more fragment is required
+
+			pTxBlk->SrcBufLen = FreeMpduSize;
+
+			NextMpduSize = min(((UINT)SrcRemainingBytes - pTxBlk->SrcBufLen), ((UINT)pAd->CommonCfg.FragmentThreshold));
+			pHeader_802_11->FC.MoreFrag = 1;
+			pHeader_802_11->Duration = (3 * pAd->CommonCfg.Dsifs) + (2 * AckDuration) + RTMPCalcDuration(pAd, pTxBlk->TxRate, NextMpduSize + EncryptionOverhead);
+		}
+
+		if (fragNum == 0)
+			pTxBlk->FrameGap = IFS_HTTXOP;
+		else
+			pTxBlk->FrameGap = IFS_SIFS;
+
+		RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+		HAL_WriteFragTxResource(pAd, pTxBlk, fragNum, &FreeNumber);
+
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+
+		// Update the frame number, remaining size of the NDIS packet payload.
+
+		// space for 802.11 header.
+		if (fragNum == 0 && pTxBlk->pExtraLlcSnapEncap)
+			pTxBlk->MpduHeaderLen -= LENGTH_802_1_H;
+
+		fragNum++;
+		SrcRemainingBytes -= pTxBlk->SrcBufLen;
+		pTxBlk->pSrcBufData += pTxBlk->SrcBufLen;
+
+		pHeader_802_11->Frag++;	 // increase Frag #
+
+	}while(SrcRemainingBytes > 0);
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+}
+
+
+#define RELEASE_FRAMES_OF_TXBLK(_pAd, _pTxBlk, _pQEntry, _Status) 										\
+		while(_pTxBlk->TxPacketList.Head)														\
+		{																						\
+			_pQEntry = RemoveHeadQueue(&_pTxBlk->TxPacketList);									\
+			RELEASE_NDIS_PACKET(_pAd, QUEUE_ENTRY_TO_PACKET(_pQEntry), _Status);	\
+		}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware encryption before really
+	sent out to air.
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		PNDIS_PACKET	Pointer to outgoing Ndis frame
+		NumberOfFrag	Number of fragment required
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS STAHardTransmit(
+	IN PRTMP_ADAPTER	pAd,
+	IN TX_BLK 			*pTxBlk,
+	IN	UCHAR			QueIdx)
+{
+	NDIS_PACKET		*pPacket;
+	PQUEUE_ENTRY	pQEntry;
+
+	// ---------------------------------------------
+	// STEP 0. DO SANITY CHECK AND SOME EARLY PREPARATION.
+	// ---------------------------------------------
+	//
+	ASSERT(pTxBlk->TxPacketList.Number);
+	if (pTxBlk->TxPacketList.Head == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("pTxBlk->TotalFrameNum == %ld!\n", pTxBlk->TxPacketList.Number));
+		return NDIS_STATUS_FAILURE;
+	}
+
+	pPacket = QUEUE_ENTRY_TO_PACKET(pTxBlk->TxPacketList.Head);
+
+#if 0 //def CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+		if ((pAd->CommonCfg.CarrierDetect.Enable == TRUE) && (isCarrierDetectExist(pAd) == TRUE))
+	{
+		DBGPRINT(RT_DEBUG_INFO,("STAHardTransmit --> radar detect not in normal mode !!!\n"));
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return (NDIS_STATUS_FAILURE);
+	}
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	// ------------------------------------------------------------------
+	// STEP 1. WAKE UP PHY
+	//		outgoing frame always wakeup PHY to prevent frame lost and
+	//		turn off PSM bit to improve performance
+	// ------------------------------------------------------------------
+	// not to change PSM bit, just send this frame out?
+	if ((pAd->StaCfg.Psm == PWR_SAVE) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+	{
+	    DBGPRINT_RAW(RT_DEBUG_TRACE, ("AsicForceWakeup At HardTx\n"));
+		AsicForceWakeup(pAd, TRUE);
+	}
+
+	// It should not change PSM bit, when APSD turn on.
+	if ((!(pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable) && (pAd->CommonCfg.bAPSDForcePowerSave == FALSE))
+		|| (RTMP_GET_PACKET_EAPOL(pTxBlk->pPacket))
+		|| (RTMP_GET_PACKET_WAI(pTxBlk->pPacket)))
+	{
+		if ((pAd->StaCfg.Psm == PWR_SAVE) &&
+            (pAd->StaCfg.WindowsPowerMode == Ndis802_11PowerModeFast_PSP))
+			MlmeSetPsmBit(pAd, PWR_ACTIVE);
+	}
+
+	switch (pTxBlk->TxFrameType)
+	{
+#ifdef DOT11_N_SUPPORT
+		case TX_AMPDU_FRAME:
+				STA_AMPDU_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_AMSDU_FRAME:
+				STA_AMSDU_Frame_Tx(pAd, pTxBlk);
+			break;
+#endif // DOT11_N_SUPPORT //
+		case TX_LEGACY_FRAME:
+				STA_Legacy_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_MCAST_FRAME:
+				STA_Legacy_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_RALINK_FRAME:
+				STA_ARalink_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_FRAG_FRAME:
+				STA_Fragment_Frame_Tx(pAd, pTxBlk);
+			break;
+		default:
+			{
+				// It should not happened!
+				DBGPRINT(RT_DEBUG_ERROR, ("Send a pacekt was not classified!! It should not happen!\n"));
+				while(pTxBlk->TxPacketList.Number)
+				{
+					pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+					pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+					if (pPacket)
+						RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+				}
+			}
+			break;
+	}
+
+	return (NDIS_STATUS_SUCCESS);
+
+}
+
+ULONG  HashBytesPolynomial(UCHAR *value, unsigned int len)
+{
+   unsigned char *word = value;
+   unsigned int ret = 0;
+   unsigned int i;
+
+   for(i=0; i < len; i++)
+   {
+	  int mod = i % 32;
+	  ret ^=(unsigned int) (word[i]) << mod;
+	  ret ^=(unsigned int) (word[i]) >> (32 - mod);
+   }
+   return ret;
+}
+
+VOID Sta_Announce_or_Forward_802_3_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID)
+{
+	if (TRUE
+		)
+	{
+		announce_802_3_packet(pAd, pPacket);
+	}
+	else
+	{
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+	}
+}
+
diff --git a/drivers/staging/rt2870/sta/sanity.c b/drivers/staging/rt2870/sta/sanity.c
new file mode 100644
index 000000000000..239872464bed
--- /dev/null
+++ b/drivers/staging/rt2870/sta/sanity.c
@@ -0,0 +1,420 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sanity.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang  2004-09-01      add WMM support
+*/
+#include "../rt_config.h"
+
+extern UCHAR	CISCO_OUI[];
+
+extern UCHAR	WPA_OUI[];
+extern UCHAR	RSN_OUI[];
+extern UCHAR	WME_INFO_ELEM[];
+extern UCHAR	WME_PARM_ELEM[];
+extern UCHAR	Ccx2QosInfo[];
+extern UCHAR	RALINK_OUI[];
+extern UCHAR	BROADCOM_OUI[];
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN MlmeStartReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT CHAR Ssid[],
+    OUT UCHAR *pSsidLen)
+{
+    MLME_START_REQ_STRUCT *Info;
+
+    Info = (MLME_START_REQ_STRUCT *)(Msg);
+
+    if (Info->SsidLen > MAX_LEN_OF_SSID)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeStartReqSanity fail - wrong SSID length\n"));
+        return FALSE;
+    }
+
+    *pSsidLen = Info->SsidLen;
+    NdisMoveMemory(Ssid, Info->Ssid, *pSsidLen);
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+    IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerAssocRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pStatus,
+    OUT USHORT *pAid,
+    OUT UCHAR SupRate[],
+    OUT UCHAR *pSupRateLen,
+    OUT UCHAR ExtRate[],
+    OUT UCHAR *pExtRateLen,
+    OUT HT_CAPABILITY_IE		*pHtCapability,
+    OUT ADD_HT_INFO_IE		*pAddHtInfo,	// AP might use this additional ht info IE
+    OUT UCHAR			*pHtCapabilityLen,
+    OUT UCHAR			*pAddHtInfoLen,
+    OUT UCHAR			*pNewExtChannelOffset,
+    OUT PEDCA_PARM pEdcaParm,
+    OUT UCHAR *pCkipFlag)
+{
+    CHAR          IeType, *Ptr;
+    PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+    PEID_STRUCT   pEid;
+    ULONG         Length = 0;
+
+	*pNewExtChannelOffset = 0xff;
+	*pHtCapabilityLen = 0;
+	*pAddHtInfoLen = 0;
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    Ptr = pFrame->Octet;
+    Length += LENGTH_802_11;
+
+    NdisMoveMemory(pCapabilityInfo, &pFrame->Octet[0], 2);
+    Length += 2;
+    NdisMoveMemory(pStatus,         &pFrame->Octet[2], 2);
+    Length += 2;
+    *pCkipFlag = 0;
+    *pExtRateLen = 0;
+    pEdcaParm->bValid = FALSE;
+
+    if (*pStatus != MLME_SUCCESS)
+        return TRUE;
+
+    NdisMoveMemory(pAid, &pFrame->Octet[4], 2);
+    Length += 2;
+
+    // Aid already swaped byte order in RTMPFrameEndianChange() for big endian platform
+    *pAid = (*pAid) & 0x3fff; // AID is low 14-bit
+
+    // -- get supported rates from payload and advance the pointer
+    IeType = pFrame->Octet[6];
+    *pSupRateLen = pFrame->Octet[7];
+    if ((IeType != IE_SUPP_RATES) || (*pSupRateLen > MAX_LEN_OF_SUPPORTED_RATES))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspSanity fail - wrong SupportedRates IE\n"));
+        return FALSE;
+    }
+    else
+        NdisMoveMemory(SupRate, &pFrame->Octet[8], *pSupRateLen);
+
+    Length = Length + 2 + *pSupRateLen;
+
+    // many AP implement proprietary IEs in non-standard order, we'd better
+    // tolerate mis-ordered IEs to get best compatibility
+    pEid = (PEID_STRUCT) &pFrame->Octet[8 + (*pSupRateLen)];
+
+    // get variable fields from payload and advance the pointer
+    while ((Length + 2 + pEid->Len) <= MsgLen)
+    {
+        switch (pEid->Eid)
+        {
+            case IE_EXT_SUPP_RATES:
+                if (pEid->Len <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(ExtRate, pEid->Octet, pEid->Len);
+                    *pExtRateLen = pEid->Len;
+                }
+                break;
+
+             case IE_HT_CAP:
+            case IE_HT_CAP2:
+			if (pEid->Len >= SIZE_HT_CAP_IE)  //Note: allow extension.!!
+			{
+				NdisMoveMemory(pHtCapability, pEid->Octet, SIZE_HT_CAP_IE);
+
+				*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+				*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+
+				*pHtCapabilityLen = SIZE_HT_CAP_IE;
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerAssocRspSanity - wrong IE_HT_CAP. \n"));
+			}
+
+		break;
+#ifdef DOT11_N_SUPPORT
+            case IE_ADD_HT:
+            case IE_ADD_HT2:
+			if (pEid->Len >= sizeof(ADD_HT_INFO_IE))
+			{
+				// This IE allows extension, but we can ignore extra bytes beyond our knowledge , so only
+				// copy first sizeof(ADD_HT_INFO_IE)
+				NdisMoveMemory(pAddHtInfo, pEid->Octet, sizeof(ADD_HT_INFO_IE));
+
+				*(USHORT *)(&pAddHtInfo->AddHtInfo2) = cpu2le16(*(USHORT *)(&pAddHtInfo->AddHtInfo2));
+				*(USHORT *)(&pAddHtInfo->AddHtInfo3) = cpu2le16(*(USHORT *)(&pAddHtInfo->AddHtInfo3));
+
+				*pAddHtInfoLen = SIZE_ADD_HT_INFO_IE;
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerAssocRspSanity - wrong IE_ADD_HT. \n"));
+			}
+
+		break;
+            case IE_SECONDARY_CH_OFFSET:
+			if (pEid->Len == 1)
+			{
+				*pNewExtChannelOffset = pEid->Octet[0];
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerAssocRspSanity - wrong IE_SECONDARY_CH_OFFSET. \n"));
+			}
+#endif // DOT11_N_SUPPORT //
+		break;
+            case IE_AIRONET_CKIP:
+                // 0. Check Aironet IE length, it must be larger or equal to 28
+                //    Cisco's AP VxWork version(will not be supported) used this IE length as 28
+                //    Cisco's AP IOS version used this IE length as 30
+                if (pEid->Len < (CKIP_NEGOTIATION_LENGTH - 2))
+                break;
+
+                // 1. Copy CKIP flag byte to buffer for process
+                *pCkipFlag = *(pEid->Octet + 8);
+                break;
+
+            case IE_AIRONET_IPADDRESS:
+                if (pEid->Len != 0x0A)
+                break;
+
+                // Get Cisco Aironet IP information
+                if (NdisEqualMemory(pEid->Octet, CISCO_OUI, 3) == 1)
+                    NdisMoveMemory(pAd->StaCfg.AironetIPAddress, pEid->Octet + 4, 4);
+                break;
+
+            // CCX2, WMM use the same IE value
+            // case IE_CCX_V2:
+            case IE_VENDOR_SPECIFIC:
+                // handle WME PARAMTER ELEMENT
+                if (NdisEqualMemory(pEid->Octet, WME_PARM_ELEM, 6) && (pEid->Len == 24))
+                {
+                    PUCHAR ptr;
+                    int i;
+
+                    // parsing EDCA parameters
+                    pEdcaParm->bValid          = TRUE;
+                    pEdcaParm->bQAck           = FALSE; // pEid->Octet[0] & 0x10;
+                    pEdcaParm->bQueueRequest   = FALSE; // pEid->Octet[0] & 0x20;
+                    pEdcaParm->bTxopRequest    = FALSE; // pEid->Octet[0] & 0x40;
+                    //pEdcaParm->bMoreDataAck    = FALSE; // pEid->Octet[0] & 0x80;
+                    pEdcaParm->EdcaUpdateCount = pEid->Octet[6] & 0x0f;
+                    pEdcaParm->bAPSDCapable    = (pEid->Octet[6] & 0x80) ? 1 : 0;
+                    ptr = &pEid->Octet[8];
+                    for (i=0; i<4; i++)
+                    {
+                        UCHAR aci = (*ptr & 0x60) >> 5; // b5~6 is AC INDEX
+                        pEdcaParm->bACM[aci]  = (((*ptr) & 0x10) == 0x10);   // b5 is ACM
+                        pEdcaParm->Aifsn[aci] = (*ptr) & 0x0f;               // b0~3 is AIFSN
+                        pEdcaParm->Cwmin[aci] = *(ptr+1) & 0x0f;             // b0~4 is Cwmin
+                        pEdcaParm->Cwmax[aci] = *(ptr+1) >> 4;               // b5~8 is Cwmax
+                        pEdcaParm->Txop[aci]  = *(ptr+2) + 256 * (*(ptr+3)); // in unit of 32-us
+                        ptr += 4; // point to next AC
+                    }
+                }
+
+                // handle CCX IE
+                else
+                {
+                    // 0. Check the size and CCX admin control
+                    if (pAd->StaCfg.CCXControl.field.Enable == 0)
+                        break;
+                    if (pEid->Len != 5)
+                        break;
+
+                    // Turn CCX2 if matched
+                    if (NdisEqualMemory(pEid->Octet, Ccx2IeInfo, 5) == 1)
+                        pAd->StaCfg.CCXEnable = TRUE;
+                    break;
+                }
+                break;
+
+            default:
+                DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspSanity - ignore unrecognized EID = %d\n", pEid->Eid));
+                break;
+        }
+
+        Length = Length + 2 + pEid->Len;
+        pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+    }
+
+    // Force CCX2 enable to TRUE for those AP didn't replay CCX v2 IE, we still force it to be on
+    if (pAd->StaCfg.CCXControl.field.Enable == 1)
+        pAd->StaCfg.CCXEnable = TRUE;
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerProbeReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT CHAR Ssid[],
+    OUT UCHAR *pSsidLen)
+{
+    UCHAR         Idx;
+    UCHAR	      RateLen;
+    CHAR          IeType;
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+
+    if ((pFrame->Octet[0] != IE_SSID) || (pFrame->Octet[1] > MAX_LEN_OF_SSID))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerProbeReqSanity fail - wrong SSID IE(Type=%d,Len=%d)\n",pFrame->Octet[0],pFrame->Octet[1]));
+        return FALSE;
+    }
+
+    *pSsidLen = pFrame->Octet[1];
+    NdisMoveMemory(Ssid, &pFrame->Octet[2], *pSsidLen);
+
+    Idx = *pSsidLen + 2;
+
+    // -- get supported rates from payload and advance the pointer
+    IeType = pFrame->Octet[Idx];
+    RateLen = pFrame->Octet[Idx + 1];
+    if (IeType != IE_SUPP_RATES)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerProbeReqSanity fail - wrong SupportRates IE(Type=%d,Len=%d)\n",pFrame->Octet[Idx],pFrame->Octet[Idx+1]));
+        return FALSE;
+    }
+    else
+    {
+        if ((pAd->CommonCfg.PhyMode == PHY_11G) && (RateLen < 8))
+            return (FALSE);
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN GetTimBit(
+    IN CHAR *Ptr,
+    IN USHORT Aid,
+    OUT UCHAR *TimLen,
+    OUT UCHAR *BcastFlag,
+    OUT UCHAR *DtimCount,
+    OUT UCHAR *DtimPeriod,
+    OUT UCHAR *MessageToMe)
+{
+    UCHAR          BitCntl, N1, N2, MyByte, MyBit;
+    CHAR          *IdxPtr;
+
+    IdxPtr = Ptr;
+
+    IdxPtr ++;
+    *TimLen = *IdxPtr;
+
+    // get DTIM Count from TIM element
+    IdxPtr ++;
+    *DtimCount = *IdxPtr;
+
+    // get DTIM Period from TIM element
+    IdxPtr++;
+    *DtimPeriod = *IdxPtr;
+
+    // get Bitmap Control from TIM element
+    IdxPtr++;
+    BitCntl = *IdxPtr;
+
+    if ((*DtimCount == 0) && (BitCntl & 0x01))
+        *BcastFlag = TRUE;
+    else
+        *BcastFlag = FALSE;
+
+    // Parse Partial Virtual Bitmap from TIM element
+    N1 = BitCntl & 0xfe;    // N1 is the first bitmap byte#
+    N2 = *TimLen - 4 + N1;  // N2 is the last bitmap byte#
+
+    if ((Aid < (N1 << 3)) || (Aid >= ((N2 + 1) << 3)))
+        *MessageToMe = FALSE;
+    else
+    {
+        MyByte = (Aid >> 3) - N1;                       // my byte position in the bitmap byte-stream
+        MyBit = Aid % 16 - ((MyByte & 0x01)? 8:0);
+
+        IdxPtr += (MyByte + 1);
+
+        //if (*IdxPtr)
+        //    DBGPRINT(RT_DEBUG_WARN, ("TIM bitmap = 0x%02x\n", *IdxPtr));
+
+        if (*IdxPtr & (0x01 << MyBit))
+            *MessageToMe = TRUE;
+        else
+            *MessageToMe = FALSE;
+    }
+
+    return TRUE;
+}
+
diff --git a/drivers/staging/rt2870/sta/sync.c b/drivers/staging/rt2870/sta/sync.c
new file mode 100644
index 000000000000..a48975566e08
--- /dev/null
+++ b/drivers/staging/rt2870/sta/sync.c
@@ -0,0 +1,1753 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sync.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2004-09-01      modified for rt2561/2661
+	Jan Lee		2006-08-01      modified for rt2860 for 802.11n
+*/
+#include "../rt_config.h"
+
+#define ADHOC_ENTRY_BEACON_LOST_TIME	(2*OS_HZ)	// 2 sec
+
+/*
+	==========================================================================
+	Description:
+		The sync state machine,
+	Parameters:
+		Sm - pointer to the state machine
+	Note:
+		the state machine looks like the following
+
+	==========================================================================
+ */
+VOID SyncStateMachineInit(
+	IN PRTMP_ADAPTER pAd,
+	IN STATE_MACHINE *Sm,
+	OUT STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(Sm, Trans, MAX_SYNC_STATE, MAX_SYNC_MSG, (STATE_MACHINE_FUNC)Drop, SYNC_IDLE, SYNC_MACHINE_BASE);
+
+	// column 1
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_MLME_SCAN_REQ, (STATE_MACHINE_FUNC)MlmeScanReqAction);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_MLME_JOIN_REQ, (STATE_MACHINE_FUNC)MlmeJoinReqAction);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_MLME_START_REQ, (STATE_MACHINE_FUNC)MlmeStartReqAction);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_PEER_BEACON, (STATE_MACHINE_FUNC)PeerBeacon);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_PEER_PROBE_REQ, (STATE_MACHINE_FUNC)PeerProbeReqAction);
+
+	//column 2
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_MLME_SCAN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenScan);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_MLME_JOIN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenJoin);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_MLME_START_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenStart);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_PEER_BEACON, (STATE_MACHINE_FUNC)PeerBeaconAtJoinAction);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_BEACON_TIMEOUT, (STATE_MACHINE_FUNC)BeaconTimeoutAtJoinAction);
+
+	// column 3
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_MLME_SCAN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenScan);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_MLME_JOIN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenJoin);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_MLME_START_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenStart);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_PEER_BEACON, (STATE_MACHINE_FUNC)PeerBeaconAtScanAction);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_PEER_PROBE_RSP, (STATE_MACHINE_FUNC)PeerBeaconAtScanAction);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_SCAN_TIMEOUT, (STATE_MACHINE_FUNC)ScanTimeoutAction);
+
+	// timer init
+	RTMPInitTimer(pAd, &pAd->MlmeAux.BeaconTimer, GET_TIMER_FUNCTION(BeaconTimeout), pAd, FALSE);
+	RTMPInitTimer(pAd, &pAd->MlmeAux.ScanTimer, GET_TIMER_FUNCTION(ScanTimeout), pAd, FALSE);
+}
+
+/*
+	==========================================================================
+	Description:
+		Beacon timeout handler, executed in timer thread
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID BeaconTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	DBGPRINT(RT_DEBUG_TRACE,("SYNC - BeaconTimeout\n"));
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))
+		return;
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->CommonCfg.BBPCurrentBW == BW_40)
+		)
+	{
+		UCHAR        BBPValue = 0;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+		BBPValue &= (~0x18);
+		BBPValue |= 0x10;
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - End of SCAN, restore to 40MHz channel %d, Total BSS[%02d]\n",pAd->CommonCfg.CentralChannel, pAd->ScanTab.BssNr));
+	}
+#endif // DOT11_N_SUPPORT //
+
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_BEACON_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		Scan timeout handler, executed in timer thread
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID ScanTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))
+		return;
+
+	if (MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_SCAN_TIMEOUT, 0, NULL))
+	{
+		RT28XX_MLME_HANDLER(pAd);
+	}
+	else
+	{
+		// To prevent SyncMachine.CurrState is SCAN_LISTEN forever.
+		pAd->MlmeAux.Channel = 0;
+		ScanNextChannel(pAd);
+		if (pAd->CommonCfg.bWirelessEvent)
+		{
+			RTMPSendWirelessEvent(pAd, IW_SCAN_ENQUEUE_FAIL_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		MLME SCAN req state machine procedure
+	==========================================================================
+ */
+VOID MlmeScanReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR          Ssid[MAX_LEN_OF_SSID], SsidLen, ScanType, BssType, BBPValue = 0;
+	BOOLEAN        TimerCancelled;
+	ULONG		   Now;
+	USHORT         Status;
+	PHEADER_802_11 pHdr80211;
+	PUCHAR         pOutBuffer = NULL;
+	NDIS_STATUS    NStatus;
+
+	// Check the total scan tries for one single OID command
+	// If this is the CCX 2.0 Case, skip that!
+	if ( !RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - MlmeScanReqAction before Startup\n"));
+		return;
+	}
+
+	// Increase the scan retry counters.
+	pAd->StaCfg.ScanCnt++;
+
+
+	// first check the parameter sanity
+	if (MlmeScanReqSanity(pAd,
+						  Elem->Msg,
+						  Elem->MsgLen,
+						  &BssType,
+						  Ssid,
+						  &SsidLen,
+						  &ScanType))
+	{
+
+		// Check for channel load and noise hist request
+		// Suspend MSDU only at scan request, not the last two mentioned
+		if ((ScanType == SCAN_CISCO_NOISE) || (ScanType == SCAN_CISCO_CHANNEL_LOAD))
+		{
+			if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+				RTMPSuspendMsduTransmission(pAd);			// Suspend MSDU transmission here
+		}
+		else
+		{
+			// Suspend MSDU transmission here
+			RTMPSuspendMsduTransmission(pAd);
+		}
+
+		//
+		// To prevent data lost.
+		// Send an NULL data with turned PSM bit on to current associated AP before SCAN progress.
+		// And should send an NULL data with turned PSM bit off to AP, when scan progress done
+		//
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) && (INFRA_ON(pAd)))
+		{
+			NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);
+			if (NStatus	== NDIS_STATUS_SUCCESS)
+			{
+				pHdr80211 = (PHEADER_802_11) pOutBuffer;
+				MgtMacHeaderInit(pAd, pHdr80211, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+				pHdr80211->Duration = 0;
+				pHdr80211->FC.Type = BTYPE_DATA;
+				pHdr80211->FC.PwrMgmt = PWR_SAVE;
+
+				// Send using priority queue
+				MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+				DBGPRINT(RT_DEBUG_TRACE, ("MlmeScanReqAction -- Send PSM Data frame for off channel RM\n"));
+				MlmeFreeMemory(pAd, pOutBuffer);
+				RTMPusecDelay(5000);
+			}
+		}
+
+		NdisGetSystemUpTime(&Now);
+		pAd->StaCfg.LastScanTime = Now;
+		// reset all the timers
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer, &TimerCancelled);
+
+		// record desired BSS parameters
+		pAd->MlmeAux.BssType = BssType;
+		pAd->MlmeAux.ScanType = ScanType;
+		pAd->MlmeAux.SsidLen = SsidLen;
+        NdisZeroMemory(pAd->MlmeAux.Ssid, MAX_LEN_OF_SSID);
+		NdisMoveMemory(pAd->MlmeAux.Ssid, Ssid, SsidLen);
+
+		// start from the first channel
+		pAd->MlmeAux.Channel = FirstChannel(pAd);
+
+		// Change the scan channel when dealing with CCX beacon report
+		if ((ScanType == SCAN_CISCO_PASSIVE) || (ScanType == SCAN_CISCO_ACTIVE) ||
+			(ScanType == SCAN_CISCO_CHANNEL_LOAD) || (ScanType == SCAN_CISCO_NOISE))
+			pAd->MlmeAux.Channel = pAd->StaCfg.CCXScanChannel;
+
+		// Let BBP register at 20MHz to do scan
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+		BBPValue &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - BBP R4 to 20MHz.l\n"));
+		ScanNextChannel(pAd);
+	}
+	else
+	{
+		DBGPRINT_ERR(("SYNC - MlmeScanReqAction() sanity check fail\n"));
+		pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		MLME JOIN req state machine procedure
+	==========================================================================
+ */
+VOID MlmeJoinReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR        BBPValue = 0;
+	BSS_ENTRY    *pBss;
+	BOOLEAN       TimerCancelled;
+	HEADER_802_11 Hdr80211;
+	NDIS_STATUS   NStatus;
+	ULONG         FrameLen = 0;
+	PUCHAR        pOutBuffer = NULL;
+	PUCHAR        pSupRate = NULL;
+	UCHAR         SupRateLen;
+	PUCHAR        pExtRate = NULL;
+	UCHAR         ExtRateLen;
+	UCHAR         ASupRate[] = {0x8C, 0x12, 0x98, 0x24, 0xb0, 0x48, 0x60, 0x6C};
+	UCHAR         ASupRateLen = sizeof(ASupRate)/sizeof(UCHAR);
+	MLME_JOIN_REQ_STRUCT *pInfo = (MLME_JOIN_REQ_STRUCT *)(Elem->Msg);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - MlmeJoinReqAction(BSS #%ld)\n", pInfo->BssIdx));
+
+
+	// reset all the timers
+	RTMPCancelTimer(&pAd->MlmeAux.ScanTimer, &TimerCancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+
+	pBss = &pAd->MlmeAux.SsidBssTab.BssEntry[pInfo->BssIdx];
+
+	// record the desired SSID & BSSID we're waiting for
+	COPY_MAC_ADDR(pAd->MlmeAux.Bssid, pBss->Bssid);
+
+	// If AP's SSID is not hidden, it is OK for updating ssid to MlmeAux again.
+	if (pBss->Hidden == 0)
+	{
+		NdisMoveMemory(pAd->MlmeAux.Ssid, pBss->Ssid, pBss->SsidLen);
+		pAd->MlmeAux.SsidLen = pBss->SsidLen;
+	}
+
+	pAd->MlmeAux.BssType = pBss->BssType;
+	pAd->MlmeAux.Channel = pBss->Channel;
+	pAd->MlmeAux.CentralChannel = pBss->CentralChannel;
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	// Country IE of the AP will be evaluated and will be used.
+	if ((pAd->StaCfg.IEEE80211dClientMode != Rt802_11_D_None) &&
+		(pBss->bHasCountryIE == TRUE))
+	{
+		NdisMoveMemory(&pAd->CommonCfg.CountryCode[0], &pBss->CountryString[0], 2);
+		if (pBss->CountryString[2] == 'I')
+			pAd->CommonCfg.Geography = IDOR;
+		else if (pBss->CountryString[2] == 'O')
+			pAd->CommonCfg.Geography = ODOR;
+		else
+			pAd->CommonCfg.Geography = BOTH;
+		BuildChannelListEx(pAd);
+	}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// Let BBP register at 20MHz to do scan
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+	BBPValue &= (~0x18);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - BBP R4 to 20MHz.l\n"));
+
+	// switch channel and waiting for beacon timer
+	AsicSwitchChannel(pAd, pAd->MlmeAux.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->MlmeAux.Channel);
+	RTMPSetTimer(&pAd->MlmeAux.BeaconTimer, JOIN_TIMEOUT);
+
+    do
+	{
+		if (((pAd->CommonCfg.bIEEE80211H == 1) &&
+            (pAd->MlmeAux.Channel > 14) &&
+             RadarChannelCheck(pAd, pAd->MlmeAux.Channel))
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+             || (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+#endif // CARRIER_DETECTION_SUPPORT //
+            )
+		{
+			//
+			// We can't send any Probe request frame to meet 802.11h.
+			//
+			if (pBss->Hidden == 0)
+				break;
+		}
+
+		//
+		// send probe request
+		//
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+		if (NStatus == NDIS_STATUS_SUCCESS)
+		{
+			if (pAd->MlmeAux.Channel <= 14)
+			{
+				pSupRate = pAd->CommonCfg.SupRate;
+				SupRateLen = pAd->CommonCfg.SupRateLen;
+				pExtRate = pAd->CommonCfg.ExtRate;
+				ExtRateLen = pAd->CommonCfg.ExtRateLen;
+			}
+			else
+			{
+				//
+				// Overwrite Support Rate, CCK rate are not allowed
+				//
+				pSupRate = ASupRate;
+				SupRateLen = ASupRateLen;
+				ExtRateLen = 0;
+			}
+
+			if (pAd->MlmeAux.BssType == BSS_INFRA)
+				MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, pAd->MlmeAux.Bssid, pAd->MlmeAux.Bssid);
+			else
+				MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, BROADCAST_ADDR, BROADCAST_ADDR);
+
+			MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+							  sizeof(HEADER_802_11),    &Hdr80211,
+							  1,                        &SsidIe,
+							  1,                        &pAd->MlmeAux.SsidLen,
+							  pAd->MlmeAux.SsidLen,	    pAd->MlmeAux.Ssid,
+							  1,                        &SupRateIe,
+							  1,                        &SupRateLen,
+							  SupRateLen,               pSupRate,
+							  END_OF_ARGS);
+
+			if (ExtRateLen)
+			{
+				ULONG Tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &Tmp,
+								  1,                                &ExtRateIe,
+								  1,                                &ExtRateLen,
+								  ExtRateLen,                       pExtRate,
+								  END_OF_ARGS);
+				FrameLen += Tmp;
+			}
+
+
+			MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+    } while (FALSE);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - Switch to ch %d, Wait BEACON from %02x:%02x:%02x:%02x:%02x:%02x\n",
+		pBss->Channel, pBss->Bssid[0], pBss->Bssid[1], pBss->Bssid[2], pBss->Bssid[3], pBss->Bssid[4], pBss->Bssid[5]));
+
+	pAd->Mlme.SyncMachine.CurrState = JOIN_WAIT_BEACON;
+}
+
+/*
+	==========================================================================
+	Description:
+		MLME START Request state machine procedure, starting an IBSS
+	==========================================================================
+ */
+VOID MlmeStartReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Ssid[MAX_LEN_OF_SSID], SsidLen;
+	BOOLEAN       TimerCancelled;
+
+	// New for WPA security suites
+	UCHAR						VarIE[MAX_VIE_LEN]; 	// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	LARGE_INTEGER				TimeStamp;
+	BOOLEAN Privacy;
+	USHORT Status;
+
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+	TimeStamp.u.LowPart  = 0;
+	TimeStamp.u.HighPart = 0;
+
+	if (MlmeStartReqSanity(pAd, Elem->Msg, Elem->MsgLen, Ssid, &SsidLen))
+	{
+		// reset all the timers
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer, &TimerCancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+
+		//
+		// Start a new IBSS. All IBSS parameters are decided now....
+		//
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeStartReqAction - Start a new IBSS. All IBSS parameters are decided now.... \n"));
+		pAd->MlmeAux.BssType           = BSS_ADHOC;
+		NdisMoveMemory(pAd->MlmeAux.Ssid, Ssid, SsidLen);
+		pAd->MlmeAux.SsidLen           = SsidLen;
+
+		// generate a radom number as BSSID
+		MacAddrRandomBssid(pAd, pAd->MlmeAux.Bssid);
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeStartReqAction - generate a radom number as BSSID \n"));
+
+		Privacy = (pAd->StaCfg.WepStatus == Ndis802_11Encryption1Enabled) ||
+				  (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+				  (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled);
+		pAd->MlmeAux.CapabilityInfo    = CAP_GENERATE(0,1,Privacy, (pAd->CommonCfg.TxPreamble == Rt802_11PreambleShort), 1, 0);
+		pAd->MlmeAux.BeaconPeriod      = pAd->CommonCfg.BeaconPeriod;
+		pAd->MlmeAux.AtimWin           = pAd->StaCfg.AtimWin;
+		pAd->MlmeAux.Channel           = pAd->CommonCfg.Channel;
+
+		pAd->CommonCfg.CentralChannel  = pAd->CommonCfg.Channel;
+		pAd->MlmeAux.CentralChannel    = pAd->CommonCfg.CentralChannel;
+
+		pAd->MlmeAux.SupRateLen= pAd->CommonCfg.SupRateLen;
+		NdisMoveMemory(pAd->MlmeAux.SupRate, pAd->CommonCfg.SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+		RTMPCheckRates(pAd, pAd->MlmeAux.SupRate, &pAd->MlmeAux.SupRateLen);
+		pAd->MlmeAux.ExtRateLen = pAd->CommonCfg.ExtRateLen;
+		NdisMoveMemory(pAd->MlmeAux.ExtRate, pAd->CommonCfg.ExtRate, MAX_LEN_OF_SUPPORTED_RATES);
+		RTMPCheckRates(pAd, pAd->MlmeAux.ExtRate, &pAd->MlmeAux.ExtRateLen);
+#ifdef DOT11_N_SUPPORT
+		if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+		{
+			RTMPUpdateHTIE(&pAd->CommonCfg.DesiredHtPhy, &pAd->StaCfg.DesiredHtPhyInfo.MCSSet[0], &pAd->MlmeAux.HtCapability, &pAd->MlmeAux.AddHtInfo);
+			pAd->MlmeAux.HtCapabilityLen = sizeof(HT_CAPABILITY_IE);
+			// Not turn pAd->StaActive.SupportedHtPhy.bHtEnable = TRUE here.
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC -pAd->StaActive.SupportedHtPhy.bHtEnable = TRUE\n"));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			pAd->MlmeAux.HtCapabilityLen = 0;
+			pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+		}
+		// temporarily not support QOS in IBSS
+		NdisZeroMemory(&pAd->MlmeAux.APEdcaParm, sizeof(EDCA_PARM));
+		NdisZeroMemory(&pAd->MlmeAux.APQbssLoad, sizeof(QBSS_LOAD_PARM));
+		NdisZeroMemory(&pAd->MlmeAux.APQosCapability, sizeof(QOS_CAPABILITY_PARM));
+
+		AsicSwitchChannel(pAd, pAd->MlmeAux.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->MlmeAux.Channel);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - MlmeStartReqAction(ch= %d,sup rates= %d, ext rates=%d)\n",
+			pAd->MlmeAux.Channel, pAd->MlmeAux.SupRateLen, pAd->MlmeAux.ExtRateLen));
+
+		pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+		Status = MLME_SUCCESS;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_START_CONF, 2, &Status);
+	}
+	else
+	{
+		DBGPRINT_ERR(("SYNC - MlmeStartReqAction() sanity check fail.\n"));
+		pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_START_CONF, 2, &Status);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		peer sends beacon back when scanning
+	==========================================================================
+ */
+VOID PeerBeaconAtScanAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR           Bssid[MAC_ADDR_LEN], Addr2[MAC_ADDR_LEN];
+	UCHAR           Ssid[MAX_LEN_OF_SSID], BssType, Channel, NewChannel,
+					SsidLen, DtimCount, DtimPeriod, BcastFlag, MessageToMe;
+	CF_PARM         CfParm;
+	USHORT          BeaconPeriod, AtimWin, CapabilityInfo;
+	PFRAME_802_11   pFrame;
+	LARGE_INTEGER   TimeStamp;
+	UCHAR           Erp;
+	UCHAR         	SupRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		  	SupRateLen, ExtRateLen;
+	USHORT 			LenVIE;
+	UCHAR			CkipFlag;
+	UCHAR			AironetCellPowerLimit;
+	EDCA_PARM       EdcaParm;
+	QBSS_LOAD_PARM  QbssLoad;
+	QOS_CAPABILITY_PARM QosCapability;
+	ULONG						RalinkIe;
+	UCHAR						VarIE[MAX_VIE_LEN];		// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen = 0, PreNHtCapabilityLen = 0;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+
+	// NdisFillMemory(Ssid, MAX_LEN_OF_SSID, 0x00);
+	pFrame = (PFRAME_802_11) Elem->Msg;
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+#ifdef DOT11_N_SUPPORT
+    RTMPZeroMemory(&HtCapability, sizeof(HtCapability));
+	RTMPZeroMemory(&AddHtInfo, sizeof(ADD_HT_INFO_IE));
+#endif // DOT11_N_SUPPORT //
+
+	if (PeerBeaconAndProbeRspSanity(pAd,
+								Elem->Msg,
+								Elem->MsgLen,
+								Elem->Channel,
+								Addr2,
+								Bssid,
+								Ssid,
+								&SsidLen,
+								&BssType,
+								&BeaconPeriod,
+								&Channel,
+								&NewChannel,
+								&TimeStamp,
+								&CfParm,
+								&AtimWin,
+								&CapabilityInfo,
+								&Erp,
+								&DtimCount,
+								&DtimPeriod,
+								&BcastFlag,
+								&MessageToMe,
+								SupRate,
+								&SupRateLen,
+								ExtRate,
+								&ExtRateLen,
+								&CkipFlag,
+								&AironetCellPowerLimit,
+								&EdcaParm,
+								&QbssLoad,
+								&QosCapability,
+								&RalinkIe,
+								&HtCapabilityLen,
+								&PreNHtCapabilityLen,
+								&HtCapability,
+								&AddHtInfoLen,
+								&AddHtInfo,
+								&NewExtChannelOffset,
+								&LenVIE,
+								pVIE))
+	{
+		ULONG Idx;
+		CHAR Rssi = 0;
+
+		Idx = BssTableSearch(&pAd->ScanTab, Bssid, Channel);
+		if (Idx != BSS_NOT_FOUND)
+			Rssi = pAd->ScanTab.BssEntry[Idx].Rssi;
+
+		Rssi = RTMPMaxRssi(pAd, ConvertToRssi(pAd, Elem->Rssi0, RSSI_0), ConvertToRssi(pAd, Elem->Rssi1, RSSI_1), ConvertToRssi(pAd, Elem->Rssi2, RSSI_2));
+
+
+#ifdef DOT11_N_SUPPORT
+		if ((HtCapabilityLen > 0) || (PreNHtCapabilityLen > 0))
+			HtCapabilityLen = SIZE_HT_CAP_IE;
+#endif // DOT11_N_SUPPORT //
+		if ((pAd->StaCfg.CCXReqType != MSRN_TYPE_UNUSED) && (Channel == pAd->StaCfg.CCXScanChannel))
+		{
+			Idx = BssTableSetEntry(pAd, &pAd->StaCfg.CCXBssTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+						 &CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen,ExtRate, ExtRateLen, &HtCapability,
+						 &AddHtInfo, HtCapabilityLen, AddHtInfoLen, NewExtChannelOffset, Channel, Rssi, TimeStamp, CkipFlag,
+						 &EdcaParm, &QosCapability, &QbssLoad, LenVIE, pVIE);
+			if (Idx != BSS_NOT_FOUND)
+			{
+				NdisMoveMemory(pAd->StaCfg.CCXBssTab.BssEntry[Idx].PTSF, &Elem->Msg[24], 4);
+				NdisMoveMemory(&pAd->StaCfg.CCXBssTab.BssEntry[Idx].TTSF[0], &Elem->TimeStamp.u.LowPart, 4);
+				NdisMoveMemory(&pAd->StaCfg.CCXBssTab.BssEntry[Idx].TTSF[4], &Elem->TimeStamp.u.LowPart, 4);
+				if (pAd->StaCfg.CCXReqType == MSRN_TYPE_BEACON_REQ)
+					AironetAddBeaconReport(pAd, Idx, Elem);
+			}
+		}
+		else
+		{
+			Idx = BssTableSetEntry(pAd, &pAd->ScanTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+						  &CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,  &HtCapability,
+						 &AddHtInfo, HtCapabilityLen, AddHtInfoLen, NewExtChannelOffset, Channel, Rssi, TimeStamp, CkipFlag,
+						 &EdcaParm, &QosCapability, &QbssLoad, LenVIE, pVIE);
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+			if (pAd->ChannelList[pAd->CommonCfg.ChannelListIdx].bEffectedChannel == TRUE)
+			{
+				UCHAR		RegClass;
+				PeerBeaconAndProbeRspSanity2(pAd, Elem->Msg, Elem->MsgLen, &RegClass);
+				TriEventTableSetEntry(pAd, &pAd->CommonCfg.TriggerEventTab, Bssid, &HtCapability, HtCapabilityLen, RegClass, Channel);
+			}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+			if (Idx != BSS_NOT_FOUND)
+			{
+				NdisMoveMemory(pAd->ScanTab.BssEntry[Idx].PTSF, &Elem->Msg[24], 4);
+				NdisMoveMemory(&pAd->ScanTab.BssEntry[Idx].TTSF[0], &Elem->TimeStamp.u.LowPart, 4);
+				NdisMoveMemory(&pAd->ScanTab.BssEntry[Idx].TTSF[4], &Elem->TimeStamp.u.LowPart, 4);
+			}
+		}
+	}
+	// sanity check fail, ignored
+}
+
+/*
+	==========================================================================
+	Description:
+		When waiting joining the (I)BSS, beacon received from external
+	==========================================================================
+ */
+VOID PeerBeaconAtJoinAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Bssid[MAC_ADDR_LEN], Addr2[MAC_ADDR_LEN];
+	UCHAR         Ssid[MAX_LEN_OF_SSID], SsidLen, BssType, Channel, MessageToMe,
+				  DtimCount, DtimPeriod, BcastFlag, NewChannel;
+	LARGE_INTEGER TimeStamp;
+	USHORT        BeaconPeriod, AtimWin, CapabilityInfo;
+	CF_PARM       Cf;
+	BOOLEAN       TimerCancelled;
+	UCHAR         Erp;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		  SupRateLen, ExtRateLen;
+	UCHAR         CkipFlag;
+	USHORT 		  LenVIE;
+	UCHAR		  AironetCellPowerLimit;
+	EDCA_PARM       EdcaParm;
+	QBSS_LOAD_PARM  QbssLoad;
+	QOS_CAPABILITY_PARM QosCapability;
+	USHORT        Status;
+	UCHAR						VarIE[MAX_VIE_LEN];		// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	ULONG           RalinkIe;
+	ULONG         Idx;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR				HtCapabilityLen = 0, PreNHtCapabilityLen = 0;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+#ifdef DOT11_N_SUPPORT
+	UCHAR			CentralChannel;
+#endif // DOT11_N_SUPPORT //
+
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+    RTMPZeroMemory(&HtCapability, sizeof(HtCapability));
+	RTMPZeroMemory(&AddHtInfo, sizeof(ADD_HT_INFO_IE));
+
+
+	if (PeerBeaconAndProbeRspSanity(pAd,
+								Elem->Msg,
+								Elem->MsgLen,
+								Elem->Channel,
+								Addr2,
+								Bssid,
+								Ssid,
+								&SsidLen,
+								&BssType,
+								&BeaconPeriod,
+								&Channel,
+								&NewChannel,
+								&TimeStamp,
+								&Cf,
+								&AtimWin,
+								&CapabilityInfo,
+								&Erp,
+								&DtimCount,
+								&DtimPeriod,
+								&BcastFlag,
+								&MessageToMe,
+								SupRate,
+								&SupRateLen,
+								ExtRate,
+								&ExtRateLen,
+								&CkipFlag,
+								&AironetCellPowerLimit,
+								&EdcaParm,
+								&QbssLoad,
+								&QosCapability,
+								&RalinkIe,
+								&HtCapabilityLen,
+								&PreNHtCapabilityLen,
+								&HtCapability,
+								&AddHtInfoLen,
+								&AddHtInfo,
+								&NewExtChannelOffset,
+								&LenVIE,
+								pVIE))
+	{
+		// Disqualify 11b only adhoc when we are in 11g only adhoc mode
+		if ((BssType == BSS_ADHOC) && (pAd->CommonCfg.PhyMode == PHY_11G) && ((SupRateLen+ExtRateLen)< 12))
+			return;
+
+		// BEACON from desired BSS/IBSS found. We should be able to decide most
+		// BSS parameters here.
+		// Q. But what happen if this JOIN doesn't conclude a successful ASSOCIATEION?
+		//    Do we need to receover back all parameters belonging to previous BSS?
+		// A. Should be not. There's no back-door recover to previous AP. It still need
+		//    a new JOIN-AUTH-ASSOC sequence.
+		if (MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, Bssid))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - receive desired BEACON at JoinWaitBeacon... Channel = %d\n", Channel));
+			RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+
+			// Update RSSI to prevent No signal display when cards first initialized
+			pAd->StaCfg.RssiSample.LastRssi0	= ConvertToRssi(pAd, Elem->Rssi0, RSSI_0);
+			pAd->StaCfg.RssiSample.LastRssi1	= ConvertToRssi(pAd, Elem->Rssi1, RSSI_1);
+			pAd->StaCfg.RssiSample.LastRssi2	= ConvertToRssi(pAd, Elem->Rssi2, RSSI_2);
+			pAd->StaCfg.RssiSample.AvgRssi0	= pAd->StaCfg.RssiSample.LastRssi0;
+			pAd->StaCfg.RssiSample.AvgRssi0X8	= pAd->StaCfg.RssiSample.AvgRssi0 << 3;
+			pAd->StaCfg.RssiSample.AvgRssi1	= pAd->StaCfg.RssiSample.LastRssi1;
+			pAd->StaCfg.RssiSample.AvgRssi1X8	= pAd->StaCfg.RssiSample.AvgRssi1 << 3;
+			pAd->StaCfg.RssiSample.AvgRssi2	= pAd->StaCfg.RssiSample.LastRssi2;
+			pAd->StaCfg.RssiSample.AvgRssi2X8	= pAd->StaCfg.RssiSample.AvgRssi2 << 3;
+
+			//
+			// We need to check if SSID only set to any, then we can record the current SSID.
+			// Otherwise will cause hidden SSID association failed.
+			//
+			if (pAd->MlmeAux.SsidLen == 0)
+			{
+				NdisMoveMemory(pAd->MlmeAux.Ssid, Ssid, SsidLen);
+				pAd->MlmeAux.SsidLen = SsidLen;
+			}
+			else
+			{
+				Idx = BssSsidTableSearch(&pAd->ScanTab, Bssid, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen, Channel);
+
+				if (Idx != BSS_NOT_FOUND)
+				{
+					//
+					// Multiple SSID case, used correct CapabilityInfo
+					//
+					CapabilityInfo = pAd->ScanTab.BssEntry[Idx].CapabilityInfo;
+				}
+			}
+			NdisMoveMemory(pAd->MlmeAux.Bssid, Bssid, MAC_ADDR_LEN);
+			pAd->MlmeAux.CapabilityInfo = CapabilityInfo & SUPPORTED_CAPABILITY_INFO;
+			pAd->MlmeAux.BssType = BssType;
+			pAd->MlmeAux.BeaconPeriod = BeaconPeriod;
+			pAd->MlmeAux.Channel = Channel;
+			pAd->MlmeAux.AtimWin = AtimWin;
+			pAd->MlmeAux.CfpPeriod = Cf.CfpPeriod;
+			pAd->MlmeAux.CfpMaxDuration = Cf.CfpMaxDuration;
+			pAd->MlmeAux.APRalinkIe = RalinkIe;
+
+			// Copy AP's supported rate to MlmeAux for creating assoication request
+			// Also filter out not supported rate
+			pAd->MlmeAux.SupRateLen = SupRateLen;
+			NdisMoveMemory(pAd->MlmeAux.SupRate, SupRate, SupRateLen);
+			RTMPCheckRates(pAd, pAd->MlmeAux.SupRate, &pAd->MlmeAux.SupRateLen);
+			pAd->MlmeAux.ExtRateLen = ExtRateLen;
+			NdisMoveMemory(pAd->MlmeAux.ExtRate, ExtRate, ExtRateLen);
+			RTMPCheckRates(pAd, pAd->MlmeAux.ExtRate, &pAd->MlmeAux.ExtRateLen);
+
+            NdisZeroMemory(pAd->StaActive.SupportedPhyInfo.MCSSet, 16);
+#ifdef DOT11_N_SUPPORT
+			pAd->MlmeAux.NewExtChannelOffset = NewExtChannelOffset;
+			pAd->MlmeAux.HtCapabilityLen = HtCapabilityLen;
+
+			// filter out un-supported ht rates
+			if (((HtCapabilityLen > 0) || (PreNHtCapabilityLen > 0)) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+			{
+				RTMPZeroMemory(&pAd->MlmeAux.HtCapability, SIZE_HT_CAP_IE);
+   				RTMPMoveMemory(&pAd->MlmeAux.AddHtInfo, &AddHtInfo, SIZE_ADD_HT_INFO_IE);
+
+				// StaActive.SupportedHtPhy.MCSSet stores Peer AP's 11n Rx capability
+				NdisMoveMemory(pAd->StaActive.SupportedPhyInfo.MCSSet, HtCapability.MCSSet, 16);
+				pAd->MlmeAux.NewExtChannelOffset = NewExtChannelOffset;
+				pAd->MlmeAux.HtCapabilityLen = SIZE_HT_CAP_IE;
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = TRUE;
+				if (PreNHtCapabilityLen > 0)
+					pAd->StaActive.SupportedPhyInfo.bPreNHt = TRUE;
+				RTMPCheckHt(pAd, BSSID_WCID, &HtCapability, &AddHtInfo);
+				// Copy AP Parameter to StaActive.  This is also in LinkUp.
+				DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAtJoinAction! (MpduDensity=%d, MaxRAmpduFactor=%d, BW=%d)\n",
+					pAd->StaActive.SupportedHtPhy.MpduDensity, pAd->StaActive.SupportedHtPhy.MaxRAmpduFactor, HtCapability.HtCapInfo.ChannelWidth));
+
+				if (AddHtInfoLen > 0)
+				{
+					CentralChannel = AddHtInfo.ControlChan;
+		 			// Check again the Bandwidth capability of this AP.
+		 			if ((AddHtInfo.ControlChan > 2)&& (AddHtInfo.AddHtInfo.ExtChanOffset == EXTCHA_BELOW) && (HtCapability.HtCapInfo.ChannelWidth == BW_40))
+		 			{
+		 				CentralChannel = AddHtInfo.ControlChan - 2;
+		 			}
+		 			else if ((AddHtInfo.AddHtInfo.ExtChanOffset == EXTCHA_ABOVE) && (HtCapability.HtCapInfo.ChannelWidth == BW_40))
+		 			{
+		 				CentralChannel = AddHtInfo.ControlChan + 2;
+		 			}
+
+					// Check Error .
+					if (pAd->MlmeAux.CentralChannel != CentralChannel)
+		 				DBGPRINT(RT_DEBUG_ERROR, ("PeerBeaconAtJoinAction HT===>Beacon Central Channel = %d, Control Channel = %d. Mlmeaux CentralChannel = %d\n", CentralChannel, AddHtInfo.ControlChan, pAd->MlmeAux.CentralChannel));
+
+		 			DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAtJoinAction HT===>Central Channel = %d, Control Channel = %d,  .\n", CentralChannel, AddHtInfo.ControlChan));
+
+				}
+
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+			{
+   				// To prevent error, let legacy AP must have same CentralChannel and Channel.
+				if ((HtCapabilityLen == 0) && (PreNHtCapabilityLen == 0))
+					pAd->MlmeAux.CentralChannel = pAd->MlmeAux.Channel;
+
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+				RTMPZeroMemory(&pAd->MlmeAux.HtCapability, SIZE_HT_CAP_IE);
+				RTMPZeroMemory(&pAd->MlmeAux.AddHtInfo, SIZE_ADD_HT_INFO_IE);
+			}
+
+			RTMPUpdateMlmeRate(pAd);
+
+			// copy QOS related information
+			if ((pAd->CommonCfg.bWmmCapable)
+#ifdef DOT11_N_SUPPORT
+				 || (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+#endif // DOT11_N_SUPPORT //
+				)
+			{
+				NdisMoveMemory(&pAd->MlmeAux.APEdcaParm, &EdcaParm, sizeof(EDCA_PARM));
+				NdisMoveMemory(&pAd->MlmeAux.APQbssLoad, &QbssLoad, sizeof(QBSS_LOAD_PARM));
+				NdisMoveMemory(&pAd->MlmeAux.APQosCapability, &QosCapability, sizeof(QOS_CAPABILITY_PARM));
+			}
+			else
+			{
+				NdisZeroMemory(&pAd->MlmeAux.APEdcaParm, sizeof(EDCA_PARM));
+				NdisZeroMemory(&pAd->MlmeAux.APQbssLoad, sizeof(QBSS_LOAD_PARM));
+				NdisZeroMemory(&pAd->MlmeAux.APQosCapability, sizeof(QOS_CAPABILITY_PARM));
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - after JOIN, SupRateLen=%d, ExtRateLen=%d\n",
+										pAd->MlmeAux.SupRateLen, pAd->MlmeAux.ExtRateLen));
+
+#ifdef LEAP_SUPPORT
+			// Update CkipFlag
+			pAd->StaCfg.CkipFlag = CkipFlag;
+
+			// Keep TimeStamp for Re-Association used.
+			if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+				pAd->StaCfg.CCKMBeaconAtJoinTimeStamp = TimeStamp;
+#endif // LEAP_SUPPORT //
+
+			if (AironetCellPowerLimit != 0xFF)
+			{
+				//We need to change our TxPower for CCX 2.0 AP Control of Client Transmit Power
+				ChangeToCellPowerLimit(pAd, AironetCellPowerLimit);
+			}
+			else  //Used the default TX Power Percentage.
+				pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+
+			pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+			Status = MLME_SUCCESS;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_JOIN_CONF, 2, &Status);
+		}
+		// not to me BEACON, ignored
+	}
+	// sanity check fail, ignore this frame
+}
+
+/*
+	==========================================================================
+	Description:
+		receive BEACON from peer
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerBeacon(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Bssid[MAC_ADDR_LEN], Addr2[MAC_ADDR_LEN];
+	CHAR          Ssid[MAX_LEN_OF_SSID];
+	CF_PARM       CfParm;
+	UCHAR         SsidLen, MessageToMe=0, BssType, Channel, NewChannel, index=0;
+	UCHAR         DtimCount=0, DtimPeriod=0, BcastFlag=0;
+	USHORT        CapabilityInfo, AtimWin, BeaconPeriod;
+	LARGE_INTEGER TimeStamp;
+	USHORT        TbttNumToNextWakeUp;
+	UCHAR         Erp;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		  SupRateLen, ExtRateLen;
+	UCHAR		  CkipFlag;
+	USHORT        LenVIE;
+	UCHAR		  AironetCellPowerLimit;
+	EDCA_PARM       EdcaParm;
+	QBSS_LOAD_PARM  QbssLoad;
+	QOS_CAPABILITY_PARM QosCapability;
+	ULONG           RalinkIe;
+	// New for WPA security suites
+	UCHAR						VarIE[MAX_VIE_LEN];		// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen, PreNHtCapabilityLen;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+
+#ifdef RALINK_ATE
+    if (ATE_ON(pAd))
+    {
+		return;
+    }
+#endif // RALINK_ATE //
+
+	if (!(INFRA_ON(pAd) || ADHOC_ON(pAd)
+		))
+		return;
+
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+    RTMPZeroMemory(&HtCapability, sizeof(HtCapability));
+	RTMPZeroMemory(&AddHtInfo, sizeof(ADD_HT_INFO_IE));
+
+	if (PeerBeaconAndProbeRspSanity(pAd,
+								Elem->Msg,
+								Elem->MsgLen,
+								Elem->Channel,
+								Addr2,
+								Bssid,
+								Ssid,
+								&SsidLen,
+								&BssType,
+								&BeaconPeriod,
+								&Channel,
+								&NewChannel,
+								&TimeStamp,
+								&CfParm,
+								&AtimWin,
+								&CapabilityInfo,
+								&Erp,
+								&DtimCount,
+								&DtimPeriod,
+								&BcastFlag,
+								&MessageToMe,
+								SupRate,
+								&SupRateLen,
+								ExtRate,
+								&ExtRateLen,
+								&CkipFlag,
+								&AironetCellPowerLimit,
+								&EdcaParm,
+								&QbssLoad,
+								&QosCapability,
+								&RalinkIe,
+								&HtCapabilityLen,
+								&PreNHtCapabilityLen,
+								&HtCapability,
+								&AddHtInfoLen,
+								&AddHtInfo,
+								&NewExtChannelOffset,
+								&LenVIE,
+								pVIE))
+	{
+		BOOLEAN is_my_bssid, is_my_ssid;
+		ULONG   Bssidx, Now;
+		BSS_ENTRY *pBss;
+		CHAR		RealRssi = RTMPMaxRssi(pAd, ConvertToRssi(pAd, Elem->Rssi0, RSSI_0), ConvertToRssi(pAd, Elem->Rssi1, RSSI_1), ConvertToRssi(pAd, Elem->Rssi2, RSSI_2));
+
+		is_my_bssid = MAC_ADDR_EQUAL(Bssid, pAd->CommonCfg.Bssid)? TRUE : FALSE;
+		is_my_ssid = SSID_EQUAL(Ssid, SsidLen, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen)? TRUE:FALSE;
+
+
+		// ignore BEACON not for my SSID
+		if ((! is_my_ssid) && (! is_my_bssid))
+			return;
+
+		// It means STA waits disassoc completely from this AP, ignores this beacon.
+		if (pAd->Mlme.CntlMachine.CurrState == CNTL_WAIT_DISASSOC)
+			return;
+
+#ifdef DOT11_N_SUPPORT
+		// Copy Control channel for this BSSID.
+		if (AddHtInfoLen != 0)
+			Channel = AddHtInfo.ControlChan;
+
+		if ((HtCapabilityLen > 0) || (PreNHtCapabilityLen > 0))
+			HtCapabilityLen = SIZE_HT_CAP_IE;
+#endif // DOT11_N_SUPPORT //
+
+		//
+		// Housekeeping "SsidBssTab" table for later-on ROAMing usage.
+		//
+		Bssidx = BssTableSearch(&pAd->ScanTab, Bssid, Channel);
+		if (Bssidx == BSS_NOT_FOUND)
+		{
+			// discover new AP of this network, create BSS entry
+			Bssidx = BssTableSetEntry(pAd, &pAd->ScanTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+						 &CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,
+						&HtCapability, &AddHtInfo,HtCapabilityLen,AddHtInfoLen,NewExtChannelOffset, Channel,
+						RealRssi, TimeStamp, CkipFlag, &EdcaParm, &QosCapability,
+						&QbssLoad, LenVIE, pVIE);
+			if (Bssidx == BSS_NOT_FOUND) // return if BSS table full
+				return;
+
+			NdisMoveMemory(pAd->ScanTab.BssEntry[Bssidx].PTSF, &Elem->Msg[24], 4);
+			NdisMoveMemory(&pAd->ScanTab.BssEntry[Bssidx].TTSF[0], &Elem->TimeStamp.u.LowPart, 4);
+			NdisMoveMemory(&pAd->ScanTab.BssEntry[Bssidx].TTSF[4], &Elem->TimeStamp.u.LowPart, 4);
+
+
+
+		}
+
+		if ((pAd->CommonCfg.bIEEE80211H == 1) && (NewChannel != 0) && (Channel != NewChannel))
+		{
+			// Switching to channel 1 can prevent from rescanning the current channel immediately (by auto reconnection).
+			// In addition, clear the MLME queue and the scan table to discard the RX packets and previous scanning results.
+			AsicSwitchChannel(pAd, 1, FALSE);
+			AsicLockChannel(pAd, 1);
+		    LinkDown(pAd, FALSE);
+			MlmeQueueInit(&pAd->Mlme.Queue);
+			BssTableInit(&pAd->ScanTab);
+		    RTMPusecDelay(1000000);		// use delay to prevent STA do reassoc
+
+			// channel sanity check
+			for (index = 0 ; index < pAd->ChannelListNum; index++)
+			{
+				if (pAd->ChannelList[index].Channel == NewChannel)
+				{
+					pAd->ScanTab.BssEntry[Bssidx].Channel = NewChannel;
+					pAd->CommonCfg.Channel = NewChannel;
+					AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerBeacon - STA receive channel switch announcement IE (New Channel =%d)\n", NewChannel));
+					break;
+				}
+			}
+
+			if (index >= pAd->ChannelListNum)
+			{
+				DBGPRINT_ERR(("PeerBeacon(can not find New Channel=%d in ChannelList[%d]\n", pAd->CommonCfg.Channel, pAd->ChannelListNum));
+			}
+		}
+
+		// if the ssid matched & bssid unmatched, we should select the bssid with large value.
+		// This might happened when two STA start at the same time
+		if ((! is_my_bssid) && ADHOC_ON(pAd))
+		{
+			INT	i;
+
+			// Add the safeguard against the mismatch of adhoc wep status
+			if (pAd->StaCfg.WepStatus != pAd->ScanTab.BssEntry[Bssidx].WepStatus)
+			{
+				return;
+			}
+
+			// collapse into the ADHOC network which has bigger BSSID value.
+			for (i = 0; i < 6; i++)
+			{
+				if (Bssid[i] > pAd->CommonCfg.Bssid[i])
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - merge to the IBSS with bigger BSSID=%02x:%02x:%02x:%02x:%02x:%02x\n",
+						Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+					AsicDisableSync(pAd);
+					COPY_MAC_ADDR(pAd->CommonCfg.Bssid, Bssid);
+					AsicSetBssid(pAd, pAd->CommonCfg.Bssid);
+					MakeIbssBeacon(pAd);        // re-build BEACON frame
+					AsicEnableIbssSync(pAd);    // copy BEACON frame to on-chip memory
+					is_my_bssid = TRUE;
+					break;
+				}
+				else if (Bssid[i] < pAd->CommonCfg.Bssid[i])
+					break;
+			}
+		}
+
+
+		NdisGetSystemUpTime(&Now);
+		pBss = &pAd->ScanTab.BssEntry[Bssidx];
+		pBss->Rssi = RealRssi;       // lastest RSSI
+		pBss->LastBeaconRxTime = Now;   // last RX timestamp
+
+		//
+		// BEACON from my BSSID - either IBSS or INFRA network
+		//
+		if (is_my_bssid)
+		{
+			RXWI_STRUC	RxWI;
+
+			pAd->StaCfg.DtimCount = DtimCount;
+			pAd->StaCfg.DtimPeriod = DtimPeriod;
+			pAd->StaCfg.LastBeaconRxTime = Now;
+
+
+			RxWI.RSSI0 = Elem->Rssi0;
+			RxWI.RSSI1 = Elem->Rssi1;
+			RxWI.RSSI2 = Elem->Rssi2;
+
+			Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, &RxWI);
+			if (AironetCellPowerLimit != 0xFF)
+			{
+				//
+				// We get the Cisco (ccx) "TxPower Limit" required
+				// Changed to appropriate TxPower Limit for Ciso Compatible Extensions
+				//
+				ChangeToCellPowerLimit(pAd, AironetCellPowerLimit);
+			}
+			else
+			{
+				//
+				// AironetCellPowerLimit equal to 0xFF means the Cisco (ccx) "TxPower Limit" not exist.
+				// Used the default TX Power Percentage, that set from UI.
+				//
+				pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+			}
+
+			if (ADHOC_ON(pAd) && (CAP_IS_IBSS_ON(CapabilityInfo)))
+			{
+				UCHAR			MaxSupportedRateIn500Kbps = 0;
+				UCHAR			idx;
+				MAC_TABLE_ENTRY *pEntry;
+
+				// supported rates array may not be sorted. sort it and find the maximum rate
+			    for (idx=0; idx<SupRateLen; idx++)
+			    {
+			        if (MaxSupportedRateIn500Kbps < (SupRate[idx] & 0x7f))
+			            MaxSupportedRateIn500Kbps = SupRate[idx] & 0x7f;
+			    }
+
+				for (idx=0; idx<ExtRateLen; idx++)
+			    {
+			        if (MaxSupportedRateIn500Kbps < (ExtRate[idx] & 0x7f))
+			            MaxSupportedRateIn500Kbps = ExtRate[idx] & 0x7f;
+			    }
+
+				// look up the existing table
+				pEntry = MacTableLookup(pAd, Addr2);
+
+				// Ad-hoc mode is using MAC address as BA session. So we need to continuously find newly joined adhoc station by receiving beacon.
+				// To prevent always check this, we use wcid == RESERVED_WCID to recognize it as newly joined adhoc station.
+				if ((ADHOC_ON(pAd) && (Elem->Wcid == RESERVED_WCID)) ||
+					(pEntry && ((pEntry->LastBeaconRxTime + ADHOC_ENTRY_BEACON_LOST_TIME) < Now)))
+				{
+					if (pEntry == NULL)
+						// Another adhoc joining, add to our MAC table.
+						pEntry = MacTableInsertEntry(pAd, Addr2, BSS0, FALSE);
+
+					if (StaAddMacTableEntry(pAd, pEntry, MaxSupportedRateIn500Kbps, &HtCapability, HtCapabilityLen, CapabilityInfo) == FALSE)
+					{
+						DBGPRINT(RT_DEBUG_TRACE, ("ADHOC - Add Entry failed.\n"));
+						return;
+					}
+
+					if (pEntry &&
+						(Elem->Wcid == RESERVED_WCID))
+					{
+						idx = pAd->StaCfg.DefaultKeyId;
+						RT28XX_STA_SECURITY_INFO_ADD(pAd, BSS0, idx, pEntry);
+					}
+				}
+
+				if (pEntry && pEntry->ValidAsCLI)
+					pEntry->LastBeaconRxTime = Now;
+
+				// At least another peer in this IBSS, declare MediaState as CONNECTED
+				if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				{
+					OPSTATUS_SET_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+
+					pAd->IndicateMediaState = NdisMediaStateConnected;
+					RTMP_IndicateMediaState(pAd);
+	                pAd->ExtraInfo = GENERAL_LINK_UP;
+					AsicSetBssid(pAd, pAd->CommonCfg.Bssid);
+
+					// 2003/03/12 - john
+					// Make sure this entry in "ScanTab" table, thus complies to Microsoft's policy that
+					// "site survey" result should always include the current connected network.
+					//
+					Bssidx = BssTableSearch(&pAd->ScanTab, Bssid, Channel);
+					if (Bssidx == BSS_NOT_FOUND)
+					{
+						Bssidx = BssTableSetEntry(pAd, &pAd->ScanTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+									&CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen, &HtCapability,
+									&AddHtInfo, HtCapabilityLen, AddHtInfoLen, NewExtChannelOffset, Channel, RealRssi, TimeStamp, 0,
+									&EdcaParm, &QosCapability, &QbssLoad, LenVIE, pVIE);
+					}
+					DBGPRINT(RT_DEBUG_TRACE, ("ADHOC  fOP_STATUS_MEDIA_STATE_CONNECTED.\n"));
+				}
+			}
+
+			if (INFRA_ON(pAd))
+			{
+				BOOLEAN bUseShortSlot, bUseBGProtection;
+
+				// decide to use/change to -
+				//      1. long slot (20 us) or short slot (9 us) time
+				//      2. turn on/off RTS/CTS and/or CTS-to-self protection
+				//      3. short preamble
+
+				//bUseShortSlot = pAd->CommonCfg.bUseShortSlotTime && CAP_IS_SHORT_SLOT(CapabilityInfo);
+				bUseShortSlot = CAP_IS_SHORT_SLOT(CapabilityInfo);
+				if (bUseShortSlot != OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SHORT_SLOT_INUSED))
+					AsicSetSlotTime(pAd, bUseShortSlot);
+
+				bUseBGProtection = (pAd->CommonCfg.UseBGProtection == 1) ||    // always use
+								   ((pAd->CommonCfg.UseBGProtection == 0) && ERP_IS_USE_PROTECTION(Erp));
+
+				if (pAd->CommonCfg.Channel > 14) // always no BG protection in A-band. falsely happened when switching A/G band to a dual-band AP
+					bUseBGProtection = FALSE;
+
+				if (bUseBGProtection != OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED))
+				{
+					if (bUseBGProtection)
+					{
+						OPSTATUS_SET_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED);
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, (OFDMSETPROTECT|CCKSETPROTECT|ALLN_SETPROTECT),FALSE,(pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1));
+					}
+					else
+					{
+						OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED);
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, (OFDMSETPROTECT|CCKSETPROTECT|ALLN_SETPROTECT),TRUE,(pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1));
+					}
+
+					DBGPRINT(RT_DEBUG_WARN, ("SYNC - AP changed B/G protection to %d\n", bUseBGProtection));
+				}
+
+#ifdef DOT11_N_SUPPORT
+				// check Ht protection mode. and adhere to the Non-GF device indication by AP.
+				if ((AddHtInfoLen != 0) &&
+					((AddHtInfo.AddHtInfo2.OperaionMode != pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode) ||
+					(AddHtInfo.AddHtInfo2.NonGfPresent != pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent)))
+				{
+					pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent = AddHtInfo.AddHtInfo2.NonGfPresent;
+					pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode = AddHtInfo.AddHtInfo2.OperaionMode;
+					if (pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1)
+				{
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, FALSE, TRUE);
+					}
+					else
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, FALSE, FALSE);
+
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - AP changed N OperaionMode to %d\n", pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode));
+				}
+#endif // DOT11_N_SUPPORT //
+
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED) &&
+					ERP_IS_USE_BARKER_PREAMBLE(Erp))
+				{
+					MlmeSetTxPreamble(pAd, Rt802_11PreambleLong);
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - AP forced to use LONG preamble\n"));
+				}
+
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED)    &&
+					(EdcaParm.bValid == TRUE)                          &&
+					(EdcaParm.EdcaUpdateCount != pAd->CommonCfg.APEdcaParm.EdcaUpdateCount))
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - AP change EDCA parameters(from %d to %d)\n",
+						pAd->CommonCfg.APEdcaParm.EdcaUpdateCount,
+						EdcaParm.EdcaUpdateCount));
+					AsicSetEdcaParm(pAd, &EdcaParm);
+				}
+
+				// copy QOS related information
+				NdisMoveMemory(&pAd->CommonCfg.APQbssLoad, &QbssLoad, sizeof(QBSS_LOAD_PARM));
+				NdisMoveMemory(&pAd->CommonCfg.APQosCapability, &QosCapability, sizeof(QOS_CAPABILITY_PARM));
+			}
+
+			// only INFRASTRUCTURE mode support power-saving feature
+			if ((INFRA_ON(pAd) && (pAd->StaCfg.Psm == PWR_SAVE)) || (pAd->CommonCfg.bAPSDForcePowerSave))
+			{
+				UCHAR FreeNumber;
+				//  1. AP has backlogged unicast-to-me frame, stay AWAKE, send PSPOLL
+				//  2. AP has backlogged broadcast/multicast frame and we want those frames, stay AWAKE
+				//  3. we have outgoing frames in TxRing or MgmtRing, better stay AWAKE
+				//  4. Psm change to PWR_SAVE, but AP not been informed yet, we better stay AWAKE
+				//  5. otherwise, put PHY back to sleep to save battery.
+				if (MessageToMe)
+				{
+					if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable &&
+						pAd->CommonCfg.bAPSDAC_BE && pAd->CommonCfg.bAPSDAC_BK && pAd->CommonCfg.bAPSDAC_VI && pAd->CommonCfg.bAPSDAC_VO)
+					{
+						pAd->CommonCfg.bNeedSendTriggerFrame = TRUE;
+					}
+					else
+						RT28XX_PS_POLL_ENQUEUE(pAd);
+				}
+				else if (BcastFlag && (DtimCount == 0) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM))
+				{
+				}
+				else if ((pAd->TxSwQueue[QID_AC_BK].Number != 0)													||
+						(pAd->TxSwQueue[QID_AC_BE].Number != 0)														||
+						(pAd->TxSwQueue[QID_AC_VI].Number != 0)														||
+						(pAd->TxSwQueue[QID_AC_VO].Number != 0)														||
+						(RTMPFreeTXDRequest(pAd, QID_AC_BK, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_AC_BE, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_AC_VI, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_AC_VO, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_MGMT, MGMT_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS))
+				{
+					// TODO: consider scheduled HCCA. might not be proper to use traditional DTIM-based power-saving scheme
+					// can we cheat here (i.e. just check MGMT & AC_BE) for better performance?
+				}
+				else
+				{
+					USHORT NextDtim = DtimCount;
+
+					if (NextDtim == 0)
+						NextDtim = DtimPeriod;
+
+					TbttNumToNextWakeUp = pAd->StaCfg.DefaultListenCount;
+					if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM) && (TbttNumToNextWakeUp > NextDtim))
+						TbttNumToNextWakeUp = NextDtim;
+
+					if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+					{
+						AsicSleepThenAutoWakeup(pAd, TbttNumToNextWakeUp);
+					}
+				}
+			}
+		}
+		// not my BSSID, ignore it
+	}
+	// sanity check fail, ignore this frame
+}
+
+/*
+	==========================================================================
+	Description:
+		Receive PROBE REQ from remote peer when operating in IBSS mode
+	==========================================================================
+ */
+VOID PeerProbeReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Addr2[MAC_ADDR_LEN];
+	CHAR          Ssid[MAX_LEN_OF_SSID];
+	UCHAR         SsidLen;
+#ifdef DOT11_N_SUPPORT
+	UCHAR		  HtLen, AddHtLen, NewExtLen;
+#endif // DOT11_N_SUPPORT //
+	HEADER_802_11 ProbeRspHdr;
+	NDIS_STATUS   NStatus;
+	PUCHAR        pOutBuffer = NULL;
+	ULONG         FrameLen = 0;
+	LARGE_INTEGER FakeTimestamp;
+	UCHAR         DsLen = 1, IbssLen = 2;
+	UCHAR         LocalErpIe[3] = {IE_ERP, 1, 0};
+	BOOLEAN       Privacy;
+	USHORT        CapabilityInfo;
+	UCHAR		  RSNIe = IE_WPA;
+
+	if (! ADHOC_ON(pAd))
+		return;
+
+	if (PeerProbeReqSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, Ssid, &SsidLen))
+	{
+		if ((SsidLen == 0) || SSID_EQUAL(Ssid, SsidLen, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen))
+		{
+			// allocate and send out ProbeRsp frame
+			NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus != NDIS_STATUS_SUCCESS)
+				return;
+
+			//pAd->StaCfg.AtimWin = 0;  // ??????
+
+			Privacy = (pAd->StaCfg.WepStatus == Ndis802_11Encryption1Enabled) ||
+					  (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+					  (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled);
+			CapabilityInfo = CAP_GENERATE(0, 1, Privacy, (pAd->CommonCfg.TxPreamble == Rt802_11PreambleShort), 0, 0);
+
+			MakeOutgoingFrame(pOutBuffer,                   &FrameLen,
+							  sizeof(HEADER_802_11),        &ProbeRspHdr,
+							  TIMESTAMP_LEN,                &FakeTimestamp,
+							  2,                            &pAd->CommonCfg.BeaconPeriod,
+							  2,                            &CapabilityInfo,
+							  1,                            &SsidIe,
+							  1,                            &pAd->CommonCfg.SsidLen,
+							  pAd->CommonCfg.SsidLen,       pAd->CommonCfg.Ssid,
+							  1,                            &SupRateIe,
+							  1,                            &pAd->StaActive.SupRateLen,
+							  pAd->StaActive.SupRateLen,    pAd->StaActive.SupRate,
+							  1,                            &DsIe,
+							  1,                            &DsLen,
+							  1,                            &pAd->CommonCfg.Channel,
+							  1,                            &IbssIe,
+							  1,                            &IbssLen,
+							  2,                            &pAd->StaActive.AtimWin,
+							  END_OF_ARGS);
+
+			if (pAd->StaActive.ExtRateLen)
+			{
+				ULONG tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,        &tmp,
+								  3,                            LocalErpIe,
+								  1,                            &ExtRateIe,
+								  1,                            &pAd->StaActive.ExtRateLen,
+								  pAd->StaActive.ExtRateLen,    &pAd->StaActive.ExtRate,
+								  END_OF_ARGS);
+				FrameLen += tmp;
+			}
+
+			// If adhoc secruity is set for WPA-None, append the cipher suite IE
+			if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+			{
+				ULONG tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,        	&tmp,
+						  			1,                              &RSNIe,
+						  			1,                            	&pAd->StaCfg.RSNIE_Len,
+						  			pAd->StaCfg.RSNIE_Len,      	pAd->StaCfg.RSN_IE,
+						  			END_OF_ARGS);
+				FrameLen += tmp;
+			}
+#ifdef DOT11_N_SUPPORT
+			if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+			{
+				ULONG TmpLen;
+				UCHAR	BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+				HtLen = sizeof(pAd->CommonCfg.HtCapability);
+				AddHtLen = sizeof(pAd->CommonCfg.AddHTInfo);
+				NewExtLen = 1;
+				//New extension channel offset IE is included in Beacon, Probe Rsp or channel Switch Announcement Frame
+				if (pAd->bBroadComHT == TRUE)
+				{
+					MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+								  1,                                &WpaIe,
+								  4,                                &BROADCOM[0],
+								 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+								  END_OF_ARGS);
+				}
+				else
+				{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+								  1,                                &HtCapIe,
+								  1,                                &HtLen,
+								 sizeof(HT_CAPABILITY_IE),          &pAd->CommonCfg.HtCapability,
+								  1,                                &AddHtInfoIe,
+								  1,                                &AddHtLen,
+								 sizeof(ADD_HT_INFO_IE),          &pAd->CommonCfg.AddHTInfo,
+								  1,                                &NewExtChanIe,
+								  1,                                &NewExtLen,
+								 sizeof(NEW_EXT_CHAN_IE),          &pAd->CommonCfg.NewExtChanOffset,
+								  END_OF_ARGS);
+				}
+				FrameLen += TmpLen;
+			}
+#endif // DOT11_N_SUPPORT //
+			MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+	}
+}
+
+VOID BeaconTimeoutAtJoinAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - BeaconTimeoutAtJoinAction\n"));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_REJ_TIMEOUT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_JOIN_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		Scan timeout procedure. basically add channel index by 1 and rescan
+	==========================================================================
+ */
+VOID ScanTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	pAd->MlmeAux.Channel = NextChannel(pAd, pAd->MlmeAux.Channel);
+
+	// Only one channel scanned for CISCO beacon request
+	if ((pAd->MlmeAux.ScanType == SCAN_CISCO_ACTIVE) ||
+		(pAd->MlmeAux.ScanType == SCAN_CISCO_PASSIVE) ||
+		(pAd->MlmeAux.ScanType == SCAN_CISCO_NOISE) ||
+		(pAd->MlmeAux.ScanType == SCAN_CISCO_CHANNEL_LOAD))
+		pAd->MlmeAux.Channel = 0;
+
+	// this routine will stop if pAd->MlmeAux.Channel == 0
+	ScanNextChannel(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID InvalidStateWhenScan(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("AYNC - InvalidStateWhenScan(state=%ld). Reset SYNC machine\n", pAd->Mlme.SyncMachine.CurrState));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID InvalidStateWhenJoin(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("InvalidStateWhenJoin(state=%ld). Reset SYNC machine\n", pAd->Mlme.SyncMachine.CurrState));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_JOIN_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID InvalidStateWhenStart(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("InvalidStateWhenStart(state=%ld). Reset SYNC machine\n", pAd->Mlme.SyncMachine.CurrState));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_START_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID EnqueuePsPoll(
+	IN PRTMP_ADAPTER pAd)
+{
+#ifdef RALINK_ATE
+    if (ATE_ON(pAd))
+    {
+		return;
+    }
+#endif // RALINK_ATE //
+
+
+	if (pAd->StaCfg.WindowsPowerMode == Ndis802_11PowerModeLegacy_PSP)
+    	pAd->PsPollFrame.FC.PwrMgmt = PWR_SAVE;
+	MiniportMMRequest(pAd, 0, (PUCHAR)&pAd->PsPollFrame, sizeof(PSPOLL_FRAME));
+}
+
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID EnqueueProbeRequest(
+	IN PRTMP_ADAPTER pAd)
+{
+	NDIS_STATUS     NState;
+	PUCHAR          pOutBuffer;
+	ULONG           FrameLen = 0;
+	HEADER_802_11   Hdr80211;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("force out a ProbeRequest ...\n"));
+
+	NState = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NState == NDIS_STATUS_SUCCESS)
+	{
+		MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, BROADCAST_ADDR, BROADCAST_ADDR);
+
+		// this ProbeRequest explicitly specify SSID to reduce unwanted ProbeResponse
+		MakeOutgoingFrame(pOutBuffer,                     &FrameLen,
+						  sizeof(HEADER_802_11),          &Hdr80211,
+						  1,                              &SsidIe,
+						  1,                              &pAd->CommonCfg.SsidLen,
+						  pAd->CommonCfg.SsidLen,		  pAd->CommonCfg.Ssid,
+						  1,                              &SupRateIe,
+						  1,                              &pAd->StaActive.SupRateLen,
+						  pAd->StaActive.SupRateLen,      pAd->StaActive.SupRate,
+						  END_OF_ARGS);
+		MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+	}
+
+}
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+VOID BuildEffectedChannelList(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR		EChannel[11];
+	UCHAR		i, j, k;
+	UCHAR		UpperChannel = 0, LowerChannel = 0;
+
+	RTMPZeroMemory(EChannel, 11);
+	i = 0;
+	// Find upper channel and lower channel.
+	if (pAd->CommonCfg.CentralChannel < pAd->CommonCfg.Channel)
+	{
+		UpperChannel = pAd->CommonCfg.Channel;
+		LowerChannel = pAd->CommonCfg.CentralChannel;
+	}
+	else if (pAd->CommonCfg.CentralChannel > pAd->CommonCfg.Channel)
+	{
+		UpperChannel = pAd->CommonCfg.CentralChannel;
+		LowerChannel = pAd->CommonCfg.Channel;
+	}
+	else
+	{
+		return;
+	}
+
+	// Record channels that is below lower channel..
+	if (LowerChannel > 1)
+	{
+		EChannel[0] = LowerChannel - 1;
+		i = 1;
+		if (LowerChannel > 2)
+		{
+			EChannel[1] = LowerChannel - 2;
+			i = 2;
+			if (LowerChannel > 3)
+			{
+				EChannel[2] = LowerChannel - 3;
+				i = 3;
+			}
+		}
+	}
+	// Record channels that is between  lower channel and upper channel.
+	for (k = LowerChannel;k < UpperChannel;k++)
+	{
+		EChannel[i] = k;
+		i++;
+	}
+	// Record channels that is above upper channel..
+	if (LowerChannel < 11)
+	{
+		EChannel[i] = UpperChannel + 1;
+		i++;
+		if (LowerChannel < 10)
+		{
+			EChannel[i] = LowerChannel + 2;
+			i++;
+			if (LowerChannel < 9)
+			{
+				EChannel[i] = LowerChannel + 3;
+				i++;
+			}
+		}
+	}
+	//
+	for (j = 0;j < i;j++)
+	{
+		for (k = 0;k < pAd->ChannelListNum;k++)
+		{
+			if (pAd->ChannelList[k].Channel == EChannel[j])
+			{
+				pAd->ChannelList[k].bEffectedChannel = TRUE;
+				DBGPRINT(RT_DEBUG_TRACE,(" EffectedChannel( =%d)\n", EChannel[j]));
+				break;
+			}
+		}
+	}
+}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+BOOLEAN ScanRunning(
+		IN PRTMP_ADAPTER pAd)
+{
+	return (pAd->Mlme.SyncMachine.CurrState == SCAN_LISTEN) ? TRUE : FALSE;
+}
+
diff --git a/drivers/staging/rt2870/sta/wpa.c b/drivers/staging/rt2870/sta/wpa.c
new file mode 100644
index 000000000000..8626dcde6054
--- /dev/null
+++ b/drivers/staging/rt2870/sta/wpa.c
@@ -0,0 +1,2107 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	wpa.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Jan	Lee		03-07-22		Initial
+	Paul Lin	03-11-28		Modify for supplicant
+*/
+#include "../rt_config.h"
+
+#define		WPARSNIE	0xdd
+#define		WPA2RSNIE	0x30
+
+//extern UCHAR BIT8[];
+UCHAR	CipherWpaPskTkip[] = {
+		0xDD, 0x16,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x02	// authentication
+		};
+UCHAR	CipherWpaPskTkipLen = (sizeof(CipherWpaPskTkip) / sizeof(UCHAR));
+
+UCHAR	CipherWpaPskAes[] = {
+		0xDD, 0x16, 			// RSN IE
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x04,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x04,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x02	// authentication
+		};
+UCHAR	CipherWpaPskAesLen = (sizeof(CipherWpaPskAes) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteCiscoCCKM[] = {
+		0xDD, 0x16,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01, // oui
+		0x01, 0x00,				// Version
+		0x00, 0x40, 0x96, 0x01, // Multicast
+		0x01, 0x00,				// Number of uicast
+		0x00, 0x40, 0x96, 0x01, // unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x40, 0x96, 0x00  // Authentication
+		};
+UCHAR	CipherSuiteCiscoCCKMLen = (sizeof(CipherSuiteCiscoCCKM) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteCiscoCCKM24[] = {
+		0xDD, 0x18,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01, // oui
+		0x01, 0x00,				// Version
+		0x00, 0x40, 0x96, 0x01, // Multicast
+		0x01, 0x00,				// Number of uicast
+		0x00, 0x40, 0x96, 0x01, // unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x40, 0x96, 0x00,
+		0x28, 0x00// Authentication
+		};
+
+UCHAR	CipherSuiteCiscoCCKM24Len = (sizeof(CipherSuiteCiscoCCKM24) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteCCXTkip[] = {
+		0xDD, 0x16,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x01	// authentication
+		};
+UCHAR	CipherSuiteCCXTkipLen = (sizeof(CipherSuiteCCXTkip) / sizeof(UCHAR));
+
+UCHAR	CCX_LLC_HDR[] = {0xAA, 0xAA, 0x03, 0x00, 0x40, 0x96, 0x00, 0x02};
+UCHAR	LLC_NORMAL[] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};
+
+UCHAR	EAPOL_FRAME[] = {0x88, 0x8E};
+
+BOOLEAN CheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	OUT	UCHAR			*Offset);
+
+void inc_byte_array(UCHAR *counter, int len);
+
+/*
+	========================================================================
+
+	Routine Description:
+		Classify WPA EAP message type
+
+	Arguments:
+		EAPType		Value of EAP message type
+		MsgType		Internal Message definition for MLME state machine
+
+	Return Value:
+		TRUE		Found appropriate message type
+		FALSE		No appropriate message type
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		All these constants are defined in wpa.h
+		For supplicant, there is only EAPOL Key message avaliable
+
+	========================================================================
+*/
+BOOLEAN	WpaMsgTypeSubst(
+	IN	UCHAR	EAPType,
+	OUT	INT		*MsgType)
+{
+	switch (EAPType)
+	{
+		case EAPPacket:
+			*MsgType = MT2_EAPPacket;
+			break;
+		case EAPOLStart:
+			*MsgType = MT2_EAPOLStart;
+			break;
+		case EAPOLLogoff:
+			*MsgType = MT2_EAPOLLogoff;
+			break;
+		case EAPOLKey:
+			*MsgType = MT2_EAPOLKey;
+			break;
+		case EAPOLASFAlert:
+			*MsgType = MT2_EAPOLASFAlert;
+			break;
+		default:
+			return FALSE;
+	}
+	return TRUE;
+}
+
+/*
+	==========================================================================
+	Description:
+		association	state machine init,	including state	transition and timer init
+	Parameters:
+		S -	pointer	to the association state machine
+	==========================================================================
+ */
+VOID WpaPskStateMachineInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	STATE_MACHINE *S,
+	OUT	STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(S,	Trans, MAX_WPA_PSK_STATE, MAX_WPA_PSK_MSG, (STATE_MACHINE_FUNC)Drop, WPA_PSK_IDLE, WPA_MACHINE_BASE);
+	StateMachineSetAction(S, WPA_PSK_IDLE, MT2_EAPOLKey, (STATE_MACHINE_FUNC)WpaEAPOLKeyAction);
+}
+
+/*
+	==========================================================================
+	Description:
+		This is	state machine function.
+		When receiving EAPOL packets which is  for 802.1x key management.
+		Use	both in	WPA, and WPAPSK	case.
+		In this	function, further dispatch to different	functions according	to the received	packet.	 3 categories are :
+		  1.  normal 4-way pairwisekey and 2-way groupkey handshake
+		  2.  MIC error	(Countermeasures attack)  report packet	from STA.
+		  3.  Request for pairwise/group key update	from STA
+	Return:
+	==========================================================================
+*/
+VOID WpaEAPOLKeyAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+
+{
+	INT             MsgType = EAPOL_MSG_INVALID;
+	PKEY_DESCRIPTER pKeyDesc;
+	PHEADER_802_11  pHeader; //red
+	UCHAR           ZeroReplay[LEN_KEY_DESC_REPLAY];
+	UCHAR EapolVr;
+	KEY_INFO		peerKeyInfo;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-----> WpaEAPOLKeyAction\n"));
+
+	// Get 802.11 header first
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Get EAPoL-Key Descriptor
+	pKeyDesc = (PKEY_DESCRIPTER) &Elem->Msg[(LENGTH_802_11 + LENGTH_802_1_H + LENGTH_EAPOL_H)];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pKeyDesc->KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT *)&peerKeyInfo) = cpu2le16(*((USHORT *)&peerKeyInfo));
+
+
+	// 1. Check EAPOL frame version and type
+	EapolVr	= (UCHAR) Elem->Msg[LENGTH_802_11+LENGTH_802_1_H];
+
+    if (((EapolVr != EAPOL_VER) && (EapolVr != EAPOL_VER2)) || ((pKeyDesc->Type != WPA1_KEY_DESC) && (pKeyDesc->Type != WPA2_KEY_DESC)))
+	{
+        DBGPRINT(RT_DEBUG_ERROR, ("Key descripter does not match with WPA rule\n"));
+			return;
+	}
+
+	// First validate replay counter, only accept message with larger replay counter
+	// Let equal pass, some AP start with all zero replay counter
+	NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);
+
+	if((RTMPCompareMemory(pKeyDesc->ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1) &&
+		(RTMPCompareMemory(pKeyDesc->ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("   ReplayCounter not match   \n"));
+		return;
+	}
+
+	// Process WPA2PSK frame
+	if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+	{
+		if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.EKD_DL == 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 0) &&
+			(peerKeyInfo.Secure == 0) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 1\n"));
+		} else if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.EKD_DL  == 1) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 1) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_3;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 3\n"));
+		} else if((peerKeyInfo.KeyType == GROUPKEY) &&
+			(peerKeyInfo.EKD_DL == 1) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 1) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_GROUP_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Group Message 1\n"));
+		}
+
+		// We will assume link is up (assoc suceess and port not secured).
+		// All state has to be able to process message from previous state
+		switch(pAd->StaCfg.WpaState)
+		{
+		case SS_START:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				Wpa2PairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			break;
+
+		case SS_WAIT_MSG_3:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				Wpa2PairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+				Wpa2PairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+			}
+			break;
+
+		case SS_WAIT_GROUP:		// When doing group key exchange
+		case SS_FINISH:			// This happened when update group key
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+			    // Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+				Wpa2PairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+			    // Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+				Wpa2PairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+			}
+			else if(MsgType == EAPOL_GROUP_MSG_1)
+			{
+				WpaGroupMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_FINISH;
+			}
+			break;
+
+		default:
+			break;
+		}
+	}
+	// Process WPAPSK Frame
+	// Classify message Type, either pairwise message 1, 3, or group message 1 for supplicant
+	else if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+	{
+		if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.KeyIndex == 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 0) &&
+			(peerKeyInfo.Secure == 0) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 1\n"));
+		}
+		else if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.KeyIndex == 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 0) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_3;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 3\n"));
+		}
+		else if((peerKeyInfo.KeyType == GROUPKEY) &&
+			(peerKeyInfo.KeyIndex != 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 1) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_GROUP_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Group Message 1\n"));
+		}
+
+		// We will assume link is up (assoc suceess and port not secured).
+		// All state has to be able to process message from previous state
+		switch(pAd->StaCfg.WpaState)
+		{
+		case SS_START:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				WpaPairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			break;
+
+		case SS_WAIT_MSG_3:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				WpaPairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+				WpaPairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+			}
+			break;
+
+		case SS_WAIT_GROUP:		// When doing group key exchange
+		case SS_FINISH:			// This happened when update group key
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				WpaPairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+				// Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+				WpaPairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+				// Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			}
+			else if(MsgType == EAPOL_GROUP_MSG_1)
+			{
+				WpaGroupMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_FINISH;
+			}
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<----- WpaEAPOLKeyAction\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process Pairwise key 4-way handshaking
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaPairMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem)
+{
+	PHEADER_802_11      pHeader;
+	UCHAR				*mpool, *PTK, *digest;
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	PEAPOL_PACKET       pMsg1;
+	EAPOL_PACKET        Packet;
+	UCHAR               Mic[16];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg1Action ----->\n"));
+
+	// allocate memory pool
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 256);
+
+	if (mpool == NULL)
+		return;
+
+	// PTK Len = 80.
+	PTK = (UCHAR *) ROUND_UP(mpool, 4);
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(PTK + 80, 4);
+
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Process message 1 from authenticator
+	pMsg1 = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	// 1. Save Replay counter, it will use to verify message 3 and construct message 2
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 2. Save ANonce
+	NdisMoveMemory(pAd->StaCfg.ANonce, pMsg1->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE);
+
+	// Generate random SNonce
+	GenRandom(pAd, pAd->CurrentAddress, pAd->StaCfg.SNonce);
+
+	// Calc PTK(ANonce, SNonce)
+	WpaCountPTK(pAd,
+		pAd->StaCfg.PMK,
+		pAd->StaCfg.ANonce,
+		pAd->CommonCfg.Bssid,
+		pAd->StaCfg.SNonce,
+		pAd->CurrentAddress,
+		PTK,
+		LEN_PTK);
+
+	// Save key to PTK entry
+	NdisMoveMemory(pAd->StaCfg.PTK, PTK, LEN_PTK);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero Message 2 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	//
+	// Message 2 as  EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
+	//
+	Packet.KeyDesc.Type = WPA1_KEY_DESC;
+	// 1. Key descriptor version and appropriate RSN IE
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
+	}
+	else	  // TKIP
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
+	}
+
+	// fill in Data Material and its length
+	Packet.KeyDesc.KeyData[0] = IE_WPA;
+	Packet.KeyDesc.KeyData[1] = pAd->StaCfg.RSNIE_Len;
+	Packet.KeyDesc.KeyDataLen[1] = pAd->StaCfg.RSNIE_Len + 2;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[2], pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);
+
+	// Update packet length after decide Key data payload
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + Packet.KeyDesc.KeyDataLen[1];
+
+	// Update Key length
+	Packet.KeyDesc.KeyLength[0] = pMsg1->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pMsg1->KeyDesc.KeyLength[1];
+	// 2. Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// 3. KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	//Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+
+	// 4. Fill SNonce
+	NdisMoveMemory(Packet.KeyDesc.KeyNonce, pAd->StaCfg.SNonce, LEN_KEY_DESC_NONCE);
+
+	// 5. Key Replay Count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Send EAPOL(0, 1, 0, 0, 0, P, 0, SNonce, MIC, RSN_IE)
+	// Out buffer for transmitting message 2
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		os_free_mem(pAd, mpool);
+		return;
+	}
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// 6. Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{	// AES
+
+		HMAC_SHA1(pOutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{	// TKIP
+		hmac_md5(PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+	//hex_dump("MIC", Mic, LEN_KEY_DESC_MIC);
+
+		MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     			&Header802_3,
+						Packet.Body_Len[1] + 4,    &Packet,
+						END_OF_ARGS);
+
+
+	// 5. Copy frame to Tx ring and send Msg 2 to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+	os_free_mem(pAd, (PUCHAR)mpool);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg1Action <-----\n"));
+}
+
+VOID Wpa2PairMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem)
+{
+	PHEADER_802_11      pHeader;
+	UCHAR				*mpool, *PTK, *digest;
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	PEAPOL_PACKET       pMsg1;
+	EAPOL_PACKET        Packet;
+	UCHAR               Mic[16];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg1Action ----->\n"));
+
+	// allocate memory pool
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 256);
+
+	if (mpool == NULL)
+		return;
+
+	// PTK Len = 80.
+	PTK = (UCHAR *) ROUND_UP(mpool, 4);
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(PTK + 80, 4);
+
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Process message 1 from authenticator
+		pMsg1 = (PEAPOL_PACKET)	&Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	// 1. Save Replay counter, it will use to verify message 3 and construct message 2
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 2. Save ANonce
+	NdisMoveMemory(pAd->StaCfg.ANonce, pMsg1->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE);
+
+	// Generate random SNonce
+	GenRandom(pAd, pAd->CurrentAddress, pAd->StaCfg.SNonce);
+
+	if(pMsg1->KeyDesc.KeyDataLen[1] > 0 )
+	{
+		// cached PMKID
+	}
+
+	// Calc PTK(ANonce, SNonce)
+	WpaCountPTK(pAd,
+		pAd->StaCfg.PMK,
+		pAd->StaCfg.ANonce,
+		pAd->CommonCfg.Bssid,
+		pAd->StaCfg.SNonce,
+		pAd->CurrentAddress,
+		PTK,
+		LEN_PTK);
+
+	// Save key to PTK entry
+	NdisMoveMemory(pAd->StaCfg.PTK, PTK, LEN_PTK);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message 2 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	//
+	// Message 2 as  EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
+	//
+	Packet.KeyDesc.Type = WPA2_KEY_DESC;
+
+	// 1. Key descriptor version and appropriate RSN IE
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
+	}
+	else	  // TKIP
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
+	}
+
+	// fill in Data Material and its length
+	Packet.KeyDesc.KeyData[0] = IE_WPA2;
+	Packet.KeyDesc.KeyData[1] = pAd->StaCfg.RSNIE_Len;
+	Packet.KeyDesc.KeyDataLen[1] = pAd->StaCfg.RSNIE_Len + 2;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[2], pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);
+
+	// Update packet length after decide Key data payload
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + Packet.KeyDesc.KeyDataLen[1];
+
+	// 2. Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// 3. KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = 0;
+	Packet.KeyDesc.KeyLength[1] = pMsg1->KeyDesc.KeyLength[1];
+
+	// 4. Fill SNonce
+	NdisMoveMemory(Packet.KeyDesc.KeyNonce, pAd->StaCfg.SNonce, LEN_KEY_DESC_NONCE);
+
+	// 5. Key Replay Count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Send EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
+	// Out buffer for transmitting message 2
+	MlmeAllocateMemory(pAd,  (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		os_free_mem(pAd, mpool);
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,        &FrameLen,
+		Packet.Body_Len[1] + 4, &Packet,
+		END_OF_ARGS);
+
+	// 6. Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1(pOutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+
+	// Make  Transmitting frame
+	MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// 5. Copy frame to Tx ring
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+	os_free_mem(pAd, mpool);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg1Action <-----\n"));
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process Pairwise key 4-way handshaking
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaPairMsg3Action(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+
+{
+	PHEADER_802_11      pHeader;
+	PUCHAR          	pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG           	FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	PEAPOL_PACKET       pMsg3;
+	UCHAR           	Mic[16], OldMic[16];
+	MAC_TABLE_ENTRY 	*pEntry = NULL;
+	UCHAR				skip_offset;
+	KEY_INFO			peerKeyInfo;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg3Action ----->\n"));
+
+	// Record 802.11 header & the received EAPOL packet Msg3
+	pHeader = (PHEADER_802_11) Elem->Msg;
+	pMsg3 = (PEAPOL_PACKET)	&Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pMsg3->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));
+
+
+	// 1. Verify cipher type match
+	if (pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer != 2))
+	{
+		return;
+	}
+	else if(pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
+	{
+		return;
+	}
+
+	// Verify RSN IE
+	//if (!RTMPEqualMemory(pMsg3->KeyDesc.KeyData, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len))
+	if (!CheckRSNIE(pAd, pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1], &skip_offset))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RSN_IE Different in Msg 3 of WPA1 4-way handshake!! \n"));
+		hex_dump("The original RSN_IE", pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len);
+		hex_dump("The received RSN_IE", pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1]);
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, ("RSN_IE VALID in Msg 3 of WPA1 4-way handshake!! \n"));
+
+
+	// 2. Check MIC value
+	// Save the MIC and replace with zero
+	NdisMoveMemory(OldMic, pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	NdisZeroMemory(pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		UCHAR digest[80];
+
+		HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else	// TKIP
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, Mic);
+	}
+
+	if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+
+	// 3. Check Replay Counter, it has to be larger than last one. No need to be exact one larger
+	if(RTMPCompareMemory(pMsg3->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
+		return;
+
+	// Update new replay counter
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 4. Double check ANonce
+	if(!NdisEqualMemory(pAd->StaCfg.ANonce, pMsg3->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE))
+		return;
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero Message 4 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;		// No data field
+
+	//
+	// Message 4 as  EAPOL-Key(0,1,0,0,0,P,0,0,MIC,0)
+	//
+	Packet.KeyDesc.Type = WPA1_KEY_DESC;
+
+	// Key descriptor version and appropriate RSN IE
+	Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = pMsg3->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pMsg3->KeyDesc.KeyLength[1];
+
+	// Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	// In Msg3,  KeyInfo.secure =0 if Group Key HS to come. 1 if no group key HS
+	// Station sends Msg4  KeyInfo.secure should be the same as that in Msg.3
+	Packet.KeyDesc.KeyInfo.Secure= peerKeyInfo.Secure;
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Key Replay count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Out buffer for transmitting message 4
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+		return;
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		UCHAR digest[80];
+
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+	// Update PTK
+	// Prepare pair-wise key information into shared key table
+	NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+	pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+
+	// Decide its ChiperAlg
+	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+	else
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+	// Update these related information to MAC_TABLE_ENTRY
+	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+	NdisMoveMemory(pEntry->PairwiseKey.Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(pEntry->PairwiseKey.RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pEntry->PairwiseKey.TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+	// Update pairwise key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  0,
+						  pAd->SharedKey[BSS0][0].CipherAlg,
+						  pAd->SharedKey[BSS0][0].Key,
+						  pAd->SharedKey[BSS0][0].TxMic,
+						  pAd->SharedKey[BSS0][0].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  0,
+							  pAd->SharedKey[BSS0][0].CipherAlg,
+							  pEntry);
+
+	// Make transmitting frame
+	MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// Copy frame to Tx ring and Send Message 4 to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg3Action <-----\n"));
+}
+
+VOID    Wpa2PairMsg3Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem)
+
+{
+	PHEADER_802_11      pHeader;
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	PEAPOL_PACKET       pMsg3;
+	UCHAR               Mic[16], OldMic[16];
+	UCHAR               *mpool, *KEYDATA, *digest;
+	UCHAR               Key[32];
+	MAC_TABLE_ENTRY 	*pEntry = NULL;
+	KEY_INFO			peerKeyInfo;
+
+	// allocate memory
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 1024);
+
+	if(mpool == NULL)
+		return;
+
+	// KEYDATA Len = 512.
+	KEYDATA = (UCHAR *) ROUND_UP(mpool, 4);
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(KEYDATA + 512, 4);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg3Action ----->\n"));
+
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Process message 3 frame.
+	pMsg3 = (PEAPOL_PACKET)	&Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pMsg3->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));
+
+	// 1. Verify cipher type match
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer!= 2))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else if(pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// 2. Check MIC value
+	// Save the MIC and replace with zero
+	NdisMoveMemory(OldMic, pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	NdisZeroMemory(pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, Mic);
+	}
+
+	if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+
+	// 3. Check Replay Counter, it has to be larger than last one. No need to be exact one larger
+	if(RTMPCompareMemory(pMsg3->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Update new replay counter
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 4. Double check ANonce
+	if(!NdisEqualMemory(pAd->StaCfg.ANonce, pMsg3->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Obtain GTK
+	// 5. Decrypt GTK from Key Data
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("EKD = %d\n", peerKeyInfo.EKD_DL));
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// Decrypt AES GTK
+		AES_GTK_KEY_UNWRAP(&pAd->StaCfg.PTK[16], KEYDATA, pMsg3->KeyDesc.KeyDataLen[1],pMsg3->KeyDesc.KeyData);
+	}
+	else	  // TKIP
+	{
+		INT i;
+		// Decrypt TKIP GTK
+		// Construct 32 bytes RC4 Key
+		NdisMoveMemory(Key, pMsg3->KeyDesc.KeyIv, 16);
+		NdisMoveMemory(&Key[16], &pAd->StaCfg.PTK[16], 16);
+		ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
+		//discard first 256 bytes
+		for(i = 0; i < 256; i++)
+			ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
+		// Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
+		ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1]);
+	}
+
+	if (!ParseKeyData(pAd, KEYDATA, pMsg3->KeyDesc.KeyDataLen[1], 1))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Update GTK to ASIC
+	// Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  pAd->StaCfg.DefaultKeyId,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  pAd->StaCfg.DefaultKeyId,
+							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+							  NULL);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message 4 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	Packet.Body_Len[1]  	= sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;		// No data field
+
+	//
+	// Message 4 as  EAPOL-Key(0,1,0,0,0,P,0,0,MIC,0)
+	//
+	Packet.KeyDesc.Type = WPA2_KEY_DESC;
+
+	// Key descriptor version and appropriate RSN IE
+	Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = pMsg3->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pMsg3->KeyDesc.KeyLength[1];
+
+	// Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+	Packet.KeyDesc.KeyInfo.Secure = 1;
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Key Replay count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Out buffer for transmitting message 4
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+	// Update PTK
+	// Prepare pair-wise key information into shared key table
+	NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+	pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+
+	// Decide its ChiperAlg
+	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+	else
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+	// Update these related information to MAC_TABLE_ENTRY
+	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+	NdisMoveMemory(&pEntry->PairwiseKey.Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(&pEntry->PairwiseKey.RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(&pEntry->PairwiseKey.TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+	// Update pairwise key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  0,
+						  pAd->SharedKey[BSS0][0].CipherAlg,
+						  pAd->SharedKey[BSS0][0].Key,
+						  pAd->SharedKey[BSS0][0].TxMic,
+						  pAd->SharedKey[BSS0][0].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  0,
+							  pAd->SharedKey[BSS0][0].CipherAlg,
+							  pEntry);
+
+	// Make  Transmitting frame
+	MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// Copy frame to Tx ring and Send Message 4 to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	// set 802.1x port control
+	//pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+	STA_PORT_SECURED(pAd);
+
+    // Indicate Connected for GUI
+    pAd->IndicateMediaState = NdisMediaStateConnected;
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+	os_free_mem(pAd, (PUCHAR)mpool);
+
+
+	// send wireless event - for set key done WPA2
+	if (pAd->CommonCfg.bWirelessEvent)
+		RTMPSendWirelessEvent(pAd, IW_SET_KEY_DONE_WPA2_EVENT_FLAG, pEntry->Addr, BSS0, 0);
+
+	DBGPRINT(RT_DEBUG_ERROR, ("Wpa2PairMsg3Action <-----\n"));
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process Group key 2-way handshaking
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaGroupMsg1Action(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+
+{
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	PEAPOL_PACKET       pGroup;
+	UCHAR               *mpool, *digest, *KEYDATA;
+	UCHAR               Mic[16], OldMic[16];
+	UCHAR               GTK[32], Key[32];
+	KEY_INFO			peerKeyInfo;
+
+	// allocate memory
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 1024);
+
+	if(mpool == NULL)
+		return;
+
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(mpool, 4);
+	// KEYDATA Len = 512.
+	KEYDATA = (UCHAR *) ROUND_UP(digest + 80, 4);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaGroupMsg1Action ----->\n"));
+
+	// Process Group Message 1 frame. skip 802.11 header(24) & LLC_SNAP header(8)
+	pGroup = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pGroup->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));
+
+	// 0. Check cipher type match
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer != 2))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// 1. Verify Replay counter
+	//    Check Replay Counter, it has to be larger than last one. No need to be exact one larger
+	if(RTMPCompareMemory(pGroup->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Update new replay counter
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 2. Verify MIC is valid
+	// Save the MIC and replace with zero
+	NdisMoveMemory(OldMic, pGroup->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	NdisZeroMemory(pGroup->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{	// AES
+		HMAC_SHA1((PUCHAR) pGroup, pGroup->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{	// TKIP
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pGroup, pGroup->Body_Len[1] + 4, Mic);
+	}
+
+	if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in group msg 1 of 2-way handshake!!!!!!!!!! \n"));
+		MlmeFreeMemory(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in group msg 1 of 2-way handshake!!!!!!!!!! \n"));
+
+
+	// 3. Decrypt GTK from Key Data
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// Decrypt AES GTK
+		AES_GTK_KEY_UNWRAP(&pAd->StaCfg.PTK[16], KEYDATA,  pGroup->KeyDesc.KeyDataLen[1], pGroup->KeyDesc.KeyData);
+	}
+	else	// TKIP
+	{
+		INT i;
+
+		// Decrypt TKIP GTK
+		// Construct 32 bytes RC4 Key
+		NdisMoveMemory(Key, pGroup->KeyDesc.KeyIv, 16);
+		NdisMoveMemory(&Key[16], &pAd->StaCfg.PTK[16], 16);
+		ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
+		//discard first 256 bytes
+		for(i = 0; i < 256; i++)
+			ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
+		// Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
+		ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pGroup->KeyDesc.KeyData, pGroup->KeyDesc.KeyDataLen[1]);
+	}
+
+	// Process decrypted key data material
+	// Parse keyData to handle KDE format for WPA2PSK
+	if (peerKeyInfo.EKD_DL)
+	{
+		if (!ParseKeyData(pAd, KEYDATA, pGroup->KeyDesc.KeyDataLen[1], 0))
+		{
+			os_free_mem(pAd, (PUCHAR)mpool);
+			return;
+		}
+	}
+	else	// WPAPSK
+	{
+		// set key material, TxMic and RxMic for WPAPSK
+		NdisMoveMemory(GTK, KEYDATA, 32);
+		NdisMoveMemory(pAd->StaCfg.GTK, GTK, 32);
+		pAd->StaCfg.DefaultKeyId = peerKeyInfo.KeyIndex;
+
+		// Prepare pair-wise key information into shared key table
+		NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+		NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, GTK, LEN_TKIP_EK);
+		NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, &GTK[16], LEN_TKIP_RXMICK);
+		NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, &GTK[24], LEN_TKIP_TXMICK);
+
+		// Update Shared Key CipherAlg
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+		if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+		else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+		else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP40Enabled)
+			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP64;
+		else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP104Enabled)
+			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP128;
+
+    	//hex_dump("Group Key :", pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, LEN_TKIP_EK);
+	}
+
+	// Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  pAd->StaCfg.DefaultKeyId,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  pAd->StaCfg.DefaultKeyId,
+							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+							  NULL);
+
+	// set 802.1x port control
+	//pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+	STA_PORT_SECURED(pAd);
+
+    // Indicate Connected for GUI
+    pAd->IndicateMediaState = NdisMediaStateConnected;
+
+	// init header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero Group message 1 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;		// No data field
+
+	//
+	// Group Message 2 as  EAPOL-Key(1,0,0,0,G,0,0,MIC,0)
+	//
+	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+	{
+		Packet.KeyDesc.Type = WPA2_KEY_DESC;
+	}
+	else
+	{
+		Packet.KeyDesc.Type = WPA1_KEY_DESC;
+	}
+
+	// Key descriptor version and appropriate RSN IE
+	Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = pGroup->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pGroup->KeyDesc.KeyLength[1];
+
+	// Key Index as G-Msg 1
+	if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+		Packet.KeyDesc.KeyInfo.KeyIndex = peerKeyInfo.KeyIndex;
+
+	// Key Type Group key
+	Packet.KeyDesc.KeyInfo.KeyType = GROUPKEY;
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	// Secure bit
+	Packet.KeyDesc.KeyInfo.Secure  = 1;
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Key Replay count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Out buffer for transmitting group message 2
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		MlmeFreeMemory(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+
+	MakeOutgoingFrame(pOutBuffer,       		&FrameLen,
+						LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// 5. Copy frame to Tx ring and prepare for encryption
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, FALSE);
+
+	// 6 Free allocated memory
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+	os_free_mem(pAd, (PUCHAR)mpool);
+
+	// send wireless event - for set key done WPA2
+	if (pAd->CommonCfg.bWirelessEvent)
+		RTMPSendWirelessEvent(pAd, IW_SET_KEY_DONE_WPA2_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaGroupMsg1Action <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init WPA MAC header
+
+	Arguments:
+		pAd	Pointer	to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaMacHeaderInit(
+	IN		PRTMP_ADAPTER	pAd,
+	IN OUT	PHEADER_802_11	pHdr80211,
+	IN		UCHAR			wep,
+	IN		PUCHAR		    pAddr1)
+{
+	NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+	pHdr80211->FC.Type	= BTYPE_DATA;
+	pHdr80211->FC.ToDs	= 1;
+	if (wep	== 1)
+		pHdr80211->FC.Wep = 1;
+
+	 //	Addr1: BSSID, Addr2: SA, Addr3:	DA
+	COPY_MAC_ADDR(pHdr80211->Addr1, pAddr1);
+	COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pHdr80211->Addr3, pAd->CommonCfg.Bssid);
+	pHdr80211->Sequence =	pAd->Sequence;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware encryption before really
+	sent out to air.
+
+	Arguments:
+		pAd		Pointer	to our adapter
+		PNDIS_PACKET	Pointer to outgoing Ndis frame
+		NumberOfFrag	Number of fragment required
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID    RTMPToWirelessSta(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pHeader802_3,
+    IN  UINT            HdrLen,
+	IN  PUCHAR          pData,
+    IN  UINT            DataLen,
+    IN	BOOLEAN			is4wayFrame)
+
+{
+	NDIS_STATUS     Status;
+	PNDIS_PACKET    pPacket;
+	UCHAR   Index;
+
+	do
+	{
+		// 1. build a NDIS packet and call RTMPSendPacket();
+		//    be careful about how/when to release this internal allocated NDIS PACKET buffer
+		Status = RTMPAllocateNdisPacket(pAd, &pPacket, pHeader802_3, HdrLen, pData, DataLen);
+		if (Status != NDIS_STATUS_SUCCESS)
+			break;
+
+		if (is4wayFrame)
+			RTMP_SET_PACKET_CLEAR_EAP_FRAME(pPacket, 1);
+		else
+			RTMP_SET_PACKET_CLEAR_EAP_FRAME(pPacket, 0);
+
+		// 2. send out the packet
+		Status = STASendPacket(pAd, pPacket);
+		if(Status == NDIS_STATUS_SUCCESS)
+		{
+			// Dequeue one frame from TxSwQueue0..3 queue and process it
+			// There are three place calling dequeue for TX ring.
+			// 1. Here, right after queueing the frame.
+			// 2. At the end of TxRingTxDone service routine.
+			// 3. Upon NDIS call RTMPSendPackets
+			if((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) &&
+				(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)))
+			{
+				for(Index = 0; Index < 5; Index ++)
+					if(pAd->TxSwQueue[Index].Number > 0)
+						RTMPDeQueuePacket(pAd, FALSE, Index, MAX_TX_PROCESS);
+			}
+		}
+	} while(FALSE);
+
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Check Sanity RSN IE form AP
+
+    Arguments:
+
+    Return Value:
+
+
+    ========================================================================
+*/
+BOOLEAN CheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	OUT	UCHAR			*Offset)
+{
+	PUCHAR              pVIE;
+	UCHAR               len;
+	PEID_STRUCT         pEid;
+	BOOLEAN				result = FALSE;
+
+	pVIE = pData;
+	len	 = DataLen;
+	*Offset = 0;
+
+	while (len > sizeof(RSNIE2))
+	{
+		pEid = (PEID_STRUCT) pVIE;
+		// WPA RSN IE
+		if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4)))
+		{
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) &&
+				(NdisEqualMemory(pVIE, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len)) &&
+				(pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == (pEid->Len + 2)))
+			{
+					DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> WPA/WPAPSK RSN IE matched in Msg 3, Length(%d) \n", (pEid->Len + 2)));
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		// WPA2 RSN IE
+		else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3)))
+		{
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2 || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) &&
+				(NdisEqualMemory(pVIE, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len)) &&
+				(pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == (pEid->Len + 2)))
+			{
+					DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", (pEid->Len + 2)));
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		else
+		{
+			break;
+		}
+
+		pVIE += (pEid->Len + 2);
+		len  -= (pEid->Len + 2);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> skip_offset(%d) \n", *Offset));
+
+	return result;
+
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Parse KEYDATA field.  KEYDATA[] May contain 2 RSN IE and optionally GTK.
+    GTK  is encaptulated in KDE format at  p.83 802.11i D10
+
+    Arguments:
+
+    Return Value:
+
+    Note:
+        802.11i D10
+
+    ========================================================================
+*/
+BOOLEAN ParseKeyData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKeyData,
+	IN  UCHAR           KeyDataLen,
+	IN	UCHAR			bPairewise)
+{
+    PKDE_ENCAP          pKDE = NULL;
+    PUCHAR              pMyKeyData = pKeyData;
+    UCHAR               KeyDataLength = KeyDataLen;
+    UCHAR               GTKLEN;
+	UCHAR				skip_offset;
+
+	// Verify The RSN IE contained in Pairewise-Msg 3 and skip it
+	if (bPairewise)
+    {
+		// Check RSN IE whether it is WPA2/WPA2PSK
+		if (!CheckRSNIE(pAd, pKeyData, KeyDataLen, &skip_offset))
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ParseKeyData ==> WPA2/WPA2PSK RSN IE mismatched \n"));
+			hex_dump("Get KEYDATA :", pKeyData, KeyDataLen);
+			return FALSE;
+    	}
+    	else
+		{
+			// skip RSN IE
+			pMyKeyData += skip_offset;
+			KeyDataLength -= skip_offset;
+
+			//DBGPRINT(RT_DEBUG_TRACE, ("ParseKeyData ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", skip_offset));
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("ParseKeyData ==> KeyDataLength %d without RSN_IE \n", KeyDataLength));
+
+	// Parse EKD format
+	if (KeyDataLength >= 8)
+    {
+        pKDE = (PKDE_ENCAP) pMyKeyData;
+    }
+	else
+    {
+		DBGPRINT(RT_DEBUG_ERROR, ("ERROR: KeyDataLength is too short \n"));
+        return FALSE;
+    }
+
+
+	// Sanity check - shared key index should not be 0
+	if (pKDE->GTKEncap.Kid == 0)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key index zero \n"));
+        return FALSE;
+    }
+
+	// Sanity check - KED length
+	if (KeyDataLength < (pKDE->Len + 2))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("ERROR: The len from KDE is too short \n"));
+        return FALSE;
+    }
+
+	// Get GTK length - refer to IEEE 802.11i-2004 p.82
+	GTKLEN = pKDE->Len -6;
+
+	if (GTKLEN < MIN_LEN_OF_GTK)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key length is too short (%d) \n", GTKLEN));
+        return FALSE;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, ("GTK Key with KDE formet got index=%d, len=%d \n", pKDE->GTKEncap.Kid, GTKLEN));
+
+	// Update GTK
+	// set key material, TxMic and RxMic for WPAPSK
+	NdisMoveMemory(pAd->StaCfg.GTK, pKDE->GTKEncap.GTK, 32);
+	pAd->StaCfg.DefaultKeyId = pKDE->GTKEncap.Kid;
+
+	// Update shared key table
+	NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, pKDE->GTKEncap.GTK, LEN_TKIP_EK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, &pKDE->GTKEncap.GTK[16], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, &pKDE->GTKEncap.GTK[24], LEN_TKIP_TXMICK);
+
+	// Update Shared Key CipherAlg
+	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+	if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+	else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+	else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP40Enabled)
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP64;
+	else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP104Enabled)
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP128;
+
+	return TRUE;
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Cisco CCKM PRF function
+
+	Arguments:
+		key				Cisco Base Transient Key (BTK)
+		key_len			The key length of the BTK
+		data			Ruquest Number(RN) + BSSID
+		data_len		The length of the data
+		output			Store for PTK(Pairwise transient keys)
+		len				The length of the output
+	Return Value:
+		None
+
+	Note:
+		802.1i	Annex F.9
+
+	========================================================================
+*/
+VOID CCKMPRF(
+	IN	UCHAR	*key,
+	IN	INT		key_len,
+	IN	UCHAR	*data,
+	IN	INT		data_len,
+	OUT	UCHAR	*output,
+	IN	INT		len)
+{
+	INT		i;
+	UCHAR	input[1024];
+	INT		currentindex = 0;
+	INT		total_len;
+
+	NdisMoveMemory(input, data, data_len);
+	total_len = data_len;
+	input[total_len] = 0;
+	total_len++;
+	for	(i = 0;	i <	(len + 19) / 20; i++)
+	{
+		HMAC_SHA1(input, total_len,	key, key_len, &output[currentindex]);
+		currentindex +=	20;
+		input[total_len - 1]++;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process MIC error indication and record MIC error timer.
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pWpaKey 		Pointer to the WPA key structure
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPReportMicError(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCIPHER_KEY 	pWpaKey)
+{
+	ULONG	Now;
+    UCHAR   unicastKey = (pWpaKey->Type == PAIRWISE_KEY ? 1:0);
+
+	// Record Last MIC error time and count
+	Now = jiffies;
+	if (pAd->StaCfg.MicErrCnt == 0)
+	{
+		pAd->StaCfg.MicErrCnt++;
+		pAd->StaCfg.LastMicErrorTime = Now;
+        NdisZeroMemory(pAd->StaCfg.ReplayCounter, 8);
+	}
+	else if (pAd->StaCfg.MicErrCnt == 1)
+	{
+		if ((pAd->StaCfg.LastMicErrorTime + (60 * OS_HZ)) < Now)
+		{
+			// Update Last MIC error time, this did not violate two MIC errors within 60 seconds
+			pAd->StaCfg.LastMicErrorTime = Now;
+		}
+		else
+		{
+
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_COUNTER_MEASURES_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+			pAd->StaCfg.LastMicErrorTime = Now;
+			// Violate MIC error counts, MIC countermeasures kicks in
+			pAd->StaCfg.MicErrCnt++;
+			// We shall block all reception
+			// We shall clean all Tx ring and disassoicate from AP after next EAPOL frame
+			//
+			// No necessary to clean all Tx ring, on RTMPHardTransmit will stop sending non-802.1X EAPOL packets
+			// if pAd->StaCfg.MicErrCnt greater than 2.
+			//
+			// RTMPRingCleanUp(pAd, QID_AC_BK);
+			// RTMPRingCleanUp(pAd, QID_AC_BE);
+			// RTMPRingCleanUp(pAd, QID_AC_VI);
+			// RTMPRingCleanUp(pAd, QID_AC_VO);
+			// RTMPRingCleanUp(pAd, QID_HCCA);
+		}
+	}
+	else
+	{
+		// MIC error count >= 2
+		// This should not happen
+		;
+	}
+    MlmeEnqueue(pAd,
+				MLME_CNTL_STATE_MACHINE,
+				OID_802_11_MIC_FAILURE_REPORT_FRAME,
+				1,
+				&unicastKey);
+
+    if (pAd->StaCfg.MicErrCnt == 2)
+    {
+        RTMPSetTimer(&pAd->StaCfg.WpaDisassocAndBlockAssocTimer, 100);
+    }
+}
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#define	LENGTH_EAP_H    4
+// If the received frame is EAP-Packet ,find out its EAP-Code (Request(0x01), Response(0x02), Success(0x03), Failure(0x04)).
+INT	    WpaCheckEapCode(
+	IN  PRTMP_ADAPTER   		pAd,
+	IN  PUCHAR				pFrame,
+	IN  USHORT				FrameLen,
+	IN  USHORT				OffSet)
+{
+
+	PUCHAR	pData;
+	INT	result = 0;
+
+	if( FrameLen < OffSet + LENGTH_EAPOL_H + LENGTH_EAP_H )
+		return result;
+
+	pData = pFrame + OffSet; // skip offset bytes
+
+	if(*(pData+1) == EAPPacket) 	// 802.1x header - Packet Type
+	{
+		 result = *(pData+4);		// EAP header - Code
+	}
+
+	return result;
+}
+
+VOID    WpaSendMicFailureToWpaSupplicant(
+    IN  PRTMP_ADAPTER    pAd,
+    IN  BOOLEAN          bUnicast)
+{
+    union iwreq_data    wrqu;
+    char custom[IW_CUSTOM_MAX] = {0};
+
+    sprintf(custom, "MLME-MICHAELMICFAILURE.indication");
+    if (bUnicast)
+        sprintf(custom, "%s unicast", custom);
+    wrqu.data.length = strlen(custom);
+    wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, custom);
+
+    return;
+}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+VOID	WpaMicFailureReportFrame(
+	IN  PRTMP_ADAPTER   pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	UCHAR               Mic[16];
+    BOOLEAN             bUnicast;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaMicFailureReportFrame ----->\n"));
+
+    bUnicast = (Elem->Msg[0] == 1 ? TRUE:FALSE);
+	pAd->Sequence = ((pAd->Sequence) + 1) & (MAX_SEQ_NUMBER);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+
+	Packet.KeyDesc.Type = WPA1_KEY_DESC;
+
+    // Request field presented
+    Packet.KeyDesc.KeyInfo.Request = 1;
+
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
+	}
+	else	  // TKIP
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
+	}
+
+    Packet.KeyDesc.KeyInfo.KeyType = (bUnicast ? PAIRWISEKEY : GROUPKEY);
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+    // Error field presented
+	Packet.KeyDesc.KeyInfo.Error  = 1;
+
+	// Update packet length after decide Key data payload
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;
+
+	// Key Replay Count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
+    inc_byte_array(pAd->StaCfg.ReplayCounter, 8);
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+		              Packet.Body_Len[1] + 4,   &Packet,
+		              END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{	// AES
+        UCHAR digest[20] = {0};
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{	// TKIP
+		hmac_md5(pAd->StaCfg.PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+    MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+    	  			LENGTH_802_3,     			&Header802_3,
+    				Packet.Body_Len[1] + 4,     &Packet,
+    				END_OF_ARGS);
+
+	// opy frame to Tx ring and send MIC failure report frame to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, FALSE);
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaMicFailureReportFrame <-----\n"));
+}
+
+/** from wpa_supplicant
+ * inc_byte_array - Increment arbitrary length byte array by one
+ * @counter: Pointer to byte array
+ * @len: Length of the counter in bytes
+ *
+ * This function increments the last byte of the counter by one and continues
+ * rolling over to more significant bytes if the byte was incremented from
+ * 0xff to 0x00.
+ */
+void inc_byte_array(UCHAR *counter, int len)
+{
+	int pos = len - 1;
+	while (pos >= 0) {
+		counter[pos]++;
+		if (counter[pos] != 0)
+			break;
+		pos--;
+	}
+}
+
+VOID WpaDisassocApAndBlockAssoc(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+    RTMP_ADAPTER                *pAd = (PRTMP_ADAPTER)FunctionContext;
+    MLME_DISASSOC_REQ_STRUCT    DisassocReq;
+
+	// disassoc from current AP first
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPReportMicError - disassociate with current AP after sending second continuous EAPOL frame\n"));
+	DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_MIC_FAILURE);
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+	pAd->StaCfg.bBlockAssoc = TRUE;
+}
+
diff --git a/drivers/staging/rt2870/sta_ioctl.c b/drivers/staging/rt2870/sta_ioctl.c
new file mode 100644
index 000000000000..91f0fab11313
--- /dev/null
+++ b/drivers/staging/rt2870/sta_ioctl.c
@@ -0,0 +1,7068 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    sta_ioctl.c
+
+    Abstract:
+    IOCTL related subroutines
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Rory Chen   01-03-2003    created
+	Rory Chen   02-14-2005    modify to support RT61
+*/
+
+#include	"rt_config.h"
+
+#ifdef DBG
+extern ULONG    RTDebugLevel;
+#endif
+
+#define NR_WEP_KEYS 				4
+#define WEP_SMALL_KEY_LEN 			(40/8)
+#define WEP_LARGE_KEY_LEN 			(104/8)
+
+#define GROUP_KEY_NO                4
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_A, _B, _C, _D, _E, _F)
+#else
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_B, _C, _D, _E, _F)
+#endif
+
+extern UCHAR    CipherWpa2Template[];
+extern UCHAR    CipherWpaPskTkip[];
+extern UCHAR    CipherWpaPskTkipLen;
+
+typedef struct PACKED _RT_VERSION_INFO{
+    UCHAR       DriverVersionW;
+    UCHAR       DriverVersionX;
+    UCHAR       DriverVersionY;
+    UCHAR       DriverVersionZ;
+    UINT        DriverBuildYear;
+    UINT        DriverBuildMonth;
+    UINT        DriverBuildDay;
+} RT_VERSION_INFO, *PRT_VERSION_INFO;
+
+struct iw_priv_args privtab[] = {
+{ RTPRIV_IOCTL_SET,
+  IW_PRIV_TYPE_CHAR | 1024, 0,
+  "set"},
+
+{ RTPRIV_IOCTL_SHOW, 0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+{ RTPRIV_IOCTL_SHOW, IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+/* --- sub-ioctls definitions --- */
+    { SHOW_CONN_STATUS,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "connStatus" },
+	{ SHOW_DRVIER_VERION,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "driverVer" },
+    { SHOW_BA_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "bainfo" },
+	{ SHOW_DESC_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "descinfo" },
+    { RAIO_OFF,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_off" },
+	{ RAIO_ON,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_on" },
+#ifdef QOS_DLS_SUPPORT
+	{ SHOW_DLS_ENTRY_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "dlsentryinfo" },
+#endif // QOS_DLS_SUPPORT //
+	{ SHOW_CFG_VALUE,
+	  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "show" },
+	{ SHOW_ADHOC_ENTRY_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "adhocEntry" },
+
+/* --- sub-ioctls relations --- */
+
+#ifdef DBG
+{ RTPRIV_IOCTL_BBP,
+  IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "bbp"},
+{ RTPRIV_IOCTL_MAC,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "mac"},
+{ RTPRIV_IOCTL_E2P,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "e2p"},
+#endif  /* DBG */
+
+{ RTPRIV_IOCTL_STATISTICS,
+  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "stat"},
+{ RTPRIV_IOCTL_GSITESURVEY,
+  0, IW_PRIV_TYPE_CHAR | 1024,
+  "get_site_survey"},
+};
+
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WMM_SUPPORT
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif
+
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+VOID RTMPIoctlBBP(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlE2PROM(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  struct iwreq    *wrq);
+#endif // DBG //
+
+
+NDIS_STATUS RTMPWPANoneAddKeyProc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN	PVOID			pBuf);
+
+INT Set_FragTest_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef DOT11_N_SUPPORT
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // DOT11_N_SUPPORT //
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // CARRIER_DETECTION_SUPPORT //
+
+INT	Show_Adhoc_MacTable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCHAR			extra);
+
+static struct {
+	CHAR *name;
+	INT (*set_proc)(PRTMP_ADAPTER pAdapter, PUCHAR arg);
+} *PRTMP_PRIVATE_SET_PROC, RTMP_PRIVATE_SUPPORT_PROC[] = {
+	{"DriverVersion",				Set_DriverVersion_Proc},
+	{"CountryRegion",				Set_CountryRegion_Proc},
+	{"CountryRegionABand",			Set_CountryRegionABand_Proc},
+	{"SSID",						Set_SSID_Proc},
+	{"WirelessMode",				Set_WirelessMode_Proc},
+	{"TxBurst",					Set_TxBurst_Proc},
+	{"TxPreamble",				Set_TxPreamble_Proc},
+	{"TxPower",					Set_TxPower_Proc},
+	{"Channel",					Set_Channel_Proc},
+	{"BGProtection",				Set_BGProtection_Proc},
+	{"RTSThreshold",				Set_RTSThreshold_Proc},
+	{"FragThreshold",				Set_FragThreshold_Proc},
+#ifdef DOT11_N_SUPPORT
+	{"HtBw",		                Set_HtBw_Proc},
+	{"HtMcs",		                Set_HtMcs_Proc},
+	{"HtGi",		                Set_HtGi_Proc},
+	{"HtOpMode",		            Set_HtOpMode_Proc},
+	{"HtExtcha",		            Set_HtExtcha_Proc},
+	{"HtMpduDensity",		        Set_HtMpduDensity_Proc},
+	{"HtBaWinSize",		        	Set_HtBaWinSize_Proc},
+	{"HtRdg",		        		Set_HtRdg_Proc},
+	{"HtAmsdu",		        		Set_HtAmsdu_Proc},
+	{"HtAutoBa",		        	Set_HtAutoBa_Proc},
+	{"HtBaDecline",					Set_BADecline_Proc},
+	{"HtProtect",		        	Set_HtProtect_Proc},
+	{"HtMimoPs",		        	Set_HtMimoPs_Proc},
+#endif // DOT11_N_SUPPORT //
+
+#ifdef AGGREGATION_SUPPORT
+	{"PktAggregate",				Set_PktAggregate_Proc},
+#endif
+
+#ifdef WMM_SUPPORT
+	{"WmmCapable",					Set_WmmCapable_Proc},
+#endif
+	{"IEEE80211H",					Set_IEEE80211H_Proc},
+    {"NetworkType",                 Set_NetworkType_Proc},
+	{"AuthMode",					Set_AuthMode_Proc},
+	{"EncrypType",					Set_EncrypType_Proc},
+	{"DefaultKeyID",				Set_DefaultKeyID_Proc},
+	{"Key1",						Set_Key1_Proc},
+	{"Key2",						Set_Key2_Proc},
+	{"Key3",						Set_Key3_Proc},
+	{"Key4",						Set_Key4_Proc},
+	{"WPAPSK",						Set_WPAPSK_Proc},
+	{"ResetCounter",				Set_ResetStatCounter_Proc},
+	{"PSMode",                      Set_PSMode_Proc},
+#ifdef DBG
+	{"Debug",						Set_Debug_Proc},
+#endif
+
+#ifdef RALINK_ATE
+	{"ATE",							Set_ATE_Proc},
+	{"ATEDA",						Set_ATE_DA_Proc},
+	{"ATESA",						Set_ATE_SA_Proc},
+	{"ATEBSSID",					Set_ATE_BSSID_Proc},
+	{"ATECHANNEL",					Set_ATE_CHANNEL_Proc},
+	{"ATETXPOW0",					Set_ATE_TX_POWER0_Proc},
+	{"ATETXPOW1",					Set_ATE_TX_POWER1_Proc},
+	{"ATETXANT",					Set_ATE_TX_Antenna_Proc},
+	{"ATERXANT",					Set_ATE_RX_Antenna_Proc},
+	{"ATETXFREQOFFSET",				Set_ATE_TX_FREQOFFSET_Proc},
+	{"ATETXBW",						Set_ATE_TX_BW_Proc},
+	{"ATETXLEN",					Set_ATE_TX_LENGTH_Proc},
+	{"ATETXCNT",					Set_ATE_TX_COUNT_Proc},
+	{"ATETXMCS",					Set_ATE_TX_MCS_Proc},
+	{"ATETXMODE",					Set_ATE_TX_MODE_Proc},
+	{"ATETXGI",						Set_ATE_TX_GI_Proc},
+	{"ATERXFER",					Set_ATE_RX_FER_Proc},
+	{"ATERRF",						Set_ATE_Read_RF_Proc},
+	{"ATEWRF1",						Set_ATE_Write_RF1_Proc},
+	{"ATEWRF2",						Set_ATE_Write_RF2_Proc},
+	{"ATEWRF3",						Set_ATE_Write_RF3_Proc},
+	{"ATEWRF4",						Set_ATE_Write_RF4_Proc},
+	{"ATELDE2P",				    Set_ATE_Load_E2P_Proc},
+	{"ATERE2P",						Set_ATE_Read_E2P_Proc},
+	{"ATESHOW",						Set_ATE_Show_Proc},
+	{"ATEHELP",						Set_ATE_Help_Proc},
+
+#ifdef RALINK_28xx_QA
+	{"TxStop",						Set_TxStop_Proc},
+	{"RxStop",						Set_RxStop_Proc},
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    {"WpaSupport",                  Set_Wpa_Support},
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+	{"FixedTxMode",                 Set_FixedTxMode_Proc},
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	{"OpMode",						Set_OpMode_Proc},
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+#ifdef DOT11_N_SUPPORT
+    {"TGnWifiTest",                 Set_TGnWifiTest_Proc},
+    {"ForceGF",		        		Set_ForceGF_Proc},
+#endif // DOT11_N_SUPPORT //
+#ifdef QOS_DLS_SUPPORT
+	{"DlsAddEntry",					Set_DlsAddEntry_Proc},
+	{"DlsTearDownEntry",			Set_DlsTearDownEntry_Proc},
+#endif // QOS_DLS_SUPPORT //
+	{"LongRetry",	        		Set_LongRetryLimit_Proc},
+	{"ShortRetry",	        		Set_ShortRetryLimit_Proc},
+#ifdef EXT_BUILD_CHANNEL_LIST
+	{"11dClientMode",				Set_Ieee80211dClientMode_Proc},
+#endif // EXT_BUILD_CHANNEL_LIST //
+#ifdef CARRIER_DETECTION_SUPPORT
+	{"CarrierDetect",				Set_CarrierDetect_Proc},
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	{NULL,}
+};
+
+
+VOID RTMPAddKey(
+	IN	PRTMP_ADAPTER	    pAd,
+	IN	PNDIS_802_11_KEY    pKey)
+{
+	ULONG				KeyIdx;
+	MAC_TABLE_ENTRY  	*pEntry;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey ------>\n"));
+
+	if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+	{
+		if (pKey->KeyIndex & 0x80000000)
+		{
+		    if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+            {
+                NdisZeroMemory(pAd->StaCfg.PMK, 32);
+                NdisMoveMemory(pAd->StaCfg.PMK, pKey->KeyMaterial, pKey->KeyLength);
+                goto end;
+            }
+		    // Update PTK
+		    NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Decide its ChiperAlg
+        	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+        	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+        	else
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+            // Update these related information to MAC_TABLE_ENTRY
+        	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+            NdisMoveMemory(pEntry->PairwiseKey.Key, pAd->SharedKey[BSS0][0].Key, LEN_TKIP_EK);
+        	NdisMoveMemory(pEntry->PairwiseKey.RxMic, pAd->SharedKey[BSS0][0].RxMic, LEN_TKIP_RXMICK);
+        	NdisMoveMemory(pEntry->PairwiseKey.TxMic, pAd->SharedKey[BSS0][0].TxMic, LEN_TKIP_TXMICK);
+        	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+        	// Update pairwise key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  0,
+        						  pAd->SharedKey[BSS0][0].CipherAlg,
+        						  pAd->SharedKey[BSS0][0].Key,
+        						  pAd->SharedKey[BSS0][0].TxMic,
+        						  pAd->SharedKey[BSS0][0].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  0,
+        							  pAd->SharedKey[BSS0][0].CipherAlg,
+        							  pEntry);
+
+            if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+            {
+                // set 802.1x port control
+	            //pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAd);
+
+                // Indicate Connected for GUI
+                pAd->IndicateMediaState = NdisMediaStateConnected;
+            }
+		}
+        else
+        {
+            // Update GTK
+            pAd->StaCfg.DefaultKeyId = (pKey->KeyIndex & 0xFF);
+            NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Update Shared Key CipherAlg
+    		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+    		if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+    		else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+
+            // Update group key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  pAd->StaCfg.DefaultKeyId,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  pAd->StaCfg.DefaultKeyId,
+        							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        							  NULL);
+
+            // set 802.1x port control
+	        //pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+			STA_PORT_SECURED(pAd);
+
+            // Indicate Connected for GUI
+            pAd->IndicateMediaState = NdisMediaStateConnected;
+        }
+	}
+	else	// dynamic WEP from wpa_supplicant
+	{
+		UCHAR	CipherAlg;
+    	PUCHAR	Key;
+
+		if(pKey->KeyLength == 32)
+			goto end;
+
+		KeyIdx = pKey->KeyIndex & 0x0fffffff;
+
+		if (KeyIdx < 4)
+		{
+			// it is a default shared key, for Pairwise key setting
+			if (pKey->KeyIndex & 0x80000000)
+			{
+				pEntry = MacTableLookup(pAd, pKey->BSSID);
+
+				if (pEntry)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey: Set Pair-wise Key\n"));
+
+					// set key material and key length
+ 					pEntry->PairwiseKey.KeyLen = (UCHAR)pKey->KeyLength;
+					NdisMoveMemory(pEntry->PairwiseKey.Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+					// set Cipher type
+					if (pKey->KeyLength == 5)
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP64;
+					else
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP128;
+
+					// Add Pair-wise key to Asic
+					AsicAddPairwiseKeyEntry(
+						pAd,
+						pEntry->Addr,
+						(UCHAR)pEntry->Aid,
+                		&pEntry->PairwiseKey);
+
+					// update WCID attribute table and IVEIV table for this entry
+					RTMPAddWcidAttributeEntry(
+						pAd,
+						BSS0,
+						KeyIdx, // The value may be not zero
+						pEntry->PairwiseKey.CipherAlg,
+						pEntry);
+
+				}
+			}
+			else
+            {
+				// Default key for tx (shared key)
+				pAd->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+
+				// set key material and key length
+				pAd->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(pAd->SharedKey[BSS0][KeyIdx].Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+				// Set Ciper type
+				if (pKey->KeyLength == 5)
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP64;
+				else
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP128;
+
+    			CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][KeyIdx].Key;
+
+				// Set Group key material to Asic
+    			AsicAddSharedKeyEntry(pAd, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+				// Update WCID attribute table and IVEIV table for this group key table
+				RTMPAddWcidAttributeEntry(pAd, BSS0, KeyIdx, CipherAlg, NULL);
+
+			}
+		}
+	}
+end:
+	return;
+}
+
+char * rtstrchr(const char * s, int c)
+{
+    for(; *s != (char) c; ++s)
+        if (*s == '\0')
+            return NULL;
+    return (char *) s;
+}
+
+/*
+This is required for LinEX2004/kernel2.6.7 to provide iwlist scanning function
+*/
+
+int
+rt_ioctl_giwname(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+//	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+#ifdef RT2870
+	strncpy(name, "RT2870 Wireless", IFNAMSIZ);
+#endif // RT2870 //
+	return 0;
+}
+
+int rt_ioctl_siwfreq(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_freq *freq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	int 	chan = -1;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+
+	if (freq->e > 1)
+		return -EINVAL;
+
+	if((freq->e == 0) && (freq->m <= 1000))
+		chan = freq->m;	// Setting by channel number
+	else
+		MAP_KHZ_TO_CHANNEL_ID( (freq->m /100) , chan); // Setting by frequency - search the table , like 2.412G, 2.422G,
+
+    if (ChannelSanity(pAdapter, chan) == TRUE)
+    {
+	pAdapter->CommonCfg.Channel = chan;
+	DBGPRINT(RT_DEBUG_ERROR, ("==>rt_ioctl_siwfreq::SIOCSIWFREQ[cmd=0x%x] (Channel=%d)\n", SIOCSIWFREQ, pAdapter->CommonCfg.Channel));
+    }
+    else
+        return -EINVAL;
+
+	return 0;
+}
+int rt_ioctl_giwfreq(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_freq *freq, char *extra)
+{
+    VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter = NULL;
+	UCHAR ch;
+	ULONG	m;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+		ch = pAdapter->CommonCfg.Channel;
+
+	DBGPRINT(RT_DEBUG_TRACE,("==>rt_ioctl_giwfreq  %d\n", ch));
+
+    MAP_CHANNEL_ID_TO_KHZ(ch, m);
+	freq->m = m * 100;
+	freq->e = 1;
+	return 0;
+}
+
+int rt_ioctl_siwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+    	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	switch (*mode)
+	{
+		case IW_MODE_ADHOC:
+			Set_NetworkType_Proc(pAdapter, "Adhoc");
+			break;
+		case IW_MODE_INFRA:
+			Set_NetworkType_Proc(pAdapter, "Infra");
+			break;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+        case IW_MODE_MONITOR:
+			Set_NetworkType_Proc(pAdapter, "Monitor");
+			break;
+#endif
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_siwmode::SIOCSIWMODE (unknown %d)\n", *mode));
+			return -EINVAL;
+	}
+
+	// Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+	pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+	return 0;
+}
+
+int rt_ioctl_giwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (ADHOC_ON(pAdapter))
+		*mode = IW_MODE_ADHOC;
+    else if (INFRA_ON(pAdapter))
+		*mode = IW_MODE_INFRA;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+    else if (MONITOR_ON(pAdapter))
+    {
+        *mode = IW_MODE_MONITOR;
+    }
+#endif
+    else
+        *mode = IW_MODE_AUTO;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==>rt_ioctl_giwmode(mode=%d)\n", *mode));
+	return 0;
+}
+
+int rt_ioctl_siwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+        	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	return 0;
+}
+
+int rt_ioctl_giwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	return 0;
+}
+
+int rt_ioctl_giwrange(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	struct iw_range *range = (struct iw_range *) extra;
+	u16 val;
+	int i;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===>rt_ioctl_giwrange\n"));
+	data->length = sizeof(struct iw_range);
+	memset(range, 0, sizeof(struct iw_range));
+
+	range->txpower_capa = IW_TXPOW_DBM;
+
+	if (INFRA_ON(pAdapter)||ADHOC_ON(pAdapter))
+	{
+		range->min_pmp = 1 * 1024;
+		range->max_pmp = 65535 * 1024;
+		range->min_pmt = 1 * 1024;
+		range->max_pmt = 1000 * 1024;
+		range->pmp_flags = IW_POWER_PERIOD;
+		range->pmt_flags = IW_POWER_TIMEOUT;
+		range->pm_capa = IW_POWER_PERIOD | IW_POWER_TIMEOUT |
+			IW_POWER_UNICAST_R | IW_POWER_ALL_R;
+	}
+
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 14;
+
+	range->retry_capa = IW_RETRY_LIMIT;
+	range->retry_flags = IW_RETRY_LIMIT;
+	range->min_retry = 0;
+	range->max_retry = 255;
+
+	range->num_channels =  pAdapter->ChannelListNum;
+
+	val = 0;
+	for (i = 1; i <= range->num_channels; i++)
+	{
+		u32 m;
+		range->freq[val].i = pAdapter->ChannelList[i-1].Channel;
+		MAP_CHANNEL_ID_TO_KHZ(pAdapter->ChannelList[i-1].Channel, m);
+		range->freq[val].m = m * 100; /* HZ */
+
+		range->freq[val].e = 1;
+		val++;
+		if (val == IW_MAX_FREQUENCIES)
+			break;
+	}
+	range->num_frequency = val;
+
+	range->max_qual.qual = 100; /* what is correct max? This was not
+					* documented exactly. At least
+					* 69 has been observed. */
+	range->max_qual.level = 0; /* dB */
+	range->max_qual.noise = 0; /* dB */
+
+	/* What would be suitable values for "average/typical" qual? */
+	range->avg_qual.qual = 20;
+	range->avg_qual.level = -60;
+	range->avg_qual.noise = -95;
+	range->sensitivity = 3;
+
+	range->max_encoding_tokens = NR_WEP_KEYS;
+	range->num_encoding_sizes = 2;
+	range->encoding_size[0] = 5;
+	range->encoding_size[1] = 13;
+
+	range->min_rts = 0;
+	range->max_rts = 2347;
+	range->min_frag = 256;
+	range->max_frag = 2346;
+
+#if WIRELESS_EXT > 17
+	/* IW_ENC_CAPA_* bit field */
+	range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
+					IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+#endif
+
+	return 0;
+}
+
+int rt_ioctl_siwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+    NDIS_802_11_MAC_ADDRESS Bssid;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+    {
+        RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+    }
+
+    // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+    // this request, because this request is initiated by NDIS.
+    pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+	// Prevent to connect AP again in STAMlmePeriodicExec
+	pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+    memset(Bssid, 0, MAC_ADDR_LEN);
+    memcpy(Bssid, ap_addr->sa_data, MAC_ADDR_LEN);
+    MlmeEnqueue(pAdapter,
+                MLME_CNTL_STATE_MACHINE,
+                OID_802_11_BSSID,
+                sizeof(NDIS_802_11_MAC_ADDRESS),
+                (VOID *)&Bssid);
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCSIWAP %02x:%02x:%02x:%02x:%02x:%02x\n",
+        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+
+	return 0;
+}
+
+int rt_ioctl_giwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+	{
+		ap_addr->sa_family = ARPHRD_ETHER;
+		memcpy(ap_addr->sa_data, &pAdapter->CommonCfg.Bssid, ETH_ALEN);
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    // Add for RT2870
+    else if (pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+    {
+        ap_addr->sa_family = ARPHRD_ETHER;
+        memcpy(ap_addr->sa_data, &pAdapter->MlmeAux.Bssid, ETH_ALEN);
+    }
+#endif // WPA_SUPPLICANT_SUPPORT //
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIWAP(=EMPTY)\n"));
+		return -ENOTCONN;
+	}
+
+	return 0;
+}
+
+/*
+ * Units are in db above the noise floor. That means the
+ * rssi values reported in the tx/rx descriptors in the
+ * driver are the SNR expressed in db.
+ *
+ * If you assume that the noise floor is -95, which is an
+ * excellent assumption 99.5 % of the time, then you can
+ * derive the absolute signal level (i.e. -95 + rssi).
+ * There are some other slight factors to take into account
+ * depending on whether the rssi measurement is from 11b,
+ * 11g, or 11a.   These differences are at most 2db and
+ * can be documented.
+ *
+ * NB: various calculations are based on the orinoco/wavelan
+ *     drivers for compatibility
+ */
+static void set_quality(PRTMP_ADAPTER pAdapter,
+                        struct iw_quality *iq,
+                        signed char rssi)
+{
+	__u8 ChannelQuality;
+
+	// Normalize Rssi
+	if (rssi >= -50)
+		ChannelQuality = 100;
+	else if (rssi >= -80) // between -50 ~ -80dbm
+		ChannelQuality = (__u8)(24 + ((rssi + 80) * 26)/10);
+	else if (rssi >= -90)   // between -80 ~ -90dbm
+        ChannelQuality = (__u8)((rssi + 90) * 26)/10;
+	else
+		ChannelQuality = 0;
+
+    iq->qual = (__u8)ChannelQuality;
+
+    iq->level = (__u8)(rssi);
+    iq->noise = (pAdapter->BbpWriteLatch[66] > pAdapter->BbpTuning.FalseCcaUpperThreshold) ? ((__u8)pAdapter->BbpTuning.FalseCcaUpperThreshold) : ((__u8) pAdapter->BbpWriteLatch[66]); 	// noise level (dBm)
+    iq->noise += 256 - 143;
+    iq->updated = pAdapter->iw_stats.qual.updated;
+}
+
+int rt_ioctl_iwaplist(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+ 	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	struct sockaddr addr[IW_MAX_AP];
+	struct iw_quality qual[IW_MAX_AP];
+	int i;
+
+   	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		data->length = 0;
+		return 0;
+        //return -ENETDOWN;
+	}
+
+	for (i = 0; i <IW_MAX_AP ; i++)
+	{
+		if (i >=  pAdapter->ScanTab.BssNr)
+			break;
+		addr[i].sa_family = ARPHRD_ETHER;
+			memcpy(addr[i].sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+		set_quality(pAdapter, &qual[i], pAdapter->ScanTab.BssEntry[i].Rssi);
+	}
+	data->length = i;
+	memcpy(extra, &addr, i*sizeof(addr[0]));
+	data->flags = 1;		/* signal quality present (sort of) */
+	memcpy(extra + i*sizeof(addr[0]), &qual, i*sizeof(qual[i]));
+
+	return 0;
+}
+
+#ifdef SIOCGIWSCAN
+int rt_ioctl_siwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	ULONG								Now;
+	int Status = NDIS_STATUS_SUCCESS;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		return -ENETDOWN;
+	}
+
+	if (MONITOR_ON(pAdapter))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+        return -EINVAL;
+    }
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount++;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+    pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		return 0;
+	do{
+		Now = jiffies;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+		if ((pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE) &&
+			(pAdapter->StaCfg.WpaSupplicantScanCount > 3))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! WpaSupplicantScanCount > 3\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+		if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+			((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+			(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)) &&
+			(pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+
+		if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+		{
+			RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+		}
+
+		// tell CNTL state machine to call NdisMSetInformationComplete() after completing
+		// this request, because this request is initiated by NDIS.
+		pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+		// Reset allowed scan retries
+		pAdapter->StaCfg.ScanCnt = 0;
+		pAdapter->StaCfg.LastScanTime = Now;
+
+		MlmeEnqueue(pAdapter,
+			MLME_CNTL_STATE_MACHINE,
+			OID_802_11_BSSID_LIST_SCAN,
+			0,
+			NULL);
+
+		Status = NDIS_STATUS_SUCCESS;
+		RT28XX_MLME_HANDLER(pAdapter);
+	}while(0);
+	return 0;
+}
+
+int rt_ioctl_giwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	int i=0;
+	char *current_ev = extra, *previous_ev = extra;
+	char *end_buf;
+	char *current_val, custom[MAX_CUSTOM_LEN] = {0};
+#ifndef IWEVGENIE
+	char idx;
+#endif // IWEVGENIE //
+	struct iw_event iwe;
+
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+    {
+		/*
+		 * Still scanning, indicate the caller should try again.
+		 */
+		return -EAGAIN;
+	}
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount = 0;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	if (pAdapter->ScanTab.BssNr == 0)
+	{
+		data->length = 0;
+		return 0;
+	}
+
+#if WIRELESS_EXT >= 17
+    if (data->length > 0)
+        end_buf = extra + data->length;
+    else
+        end_buf = extra + IW_SCAN_MAX_DATA;
+#else
+    end_buf = extra + IW_SCAN_MAX_DATA;
+#endif
+
+	for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+	{
+		if (current_ev >= end_buf)
+        {
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+        }
+
+		//MAC address
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWAP;
+		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+				memcpy(iwe.u.ap_addr.sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, ETH_ALEN);
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_ADDR_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//ESSID
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWESSID;
+		iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].SsidLen;
+		iwe.u.data.flags = 1;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_POINT(info, current_ev,end_buf, &iwe, pAdapter->ScanTab.BssEntry[i].Ssid);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Network Type
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWMODE;
+		if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11IBSS)
+		{
+			iwe.u.mode = IW_MODE_ADHOC;
+		}
+		else if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11Infrastructure)
+		{
+			iwe.u.mode = IW_MODE_INFRA;
+		}
+		else
+		{
+			iwe.u.mode = IW_MODE_AUTO;
+		}
+		iwe.len = IW_EV_UINT_LEN;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe,  IW_EV_UINT_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Channel and Frequency
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWFREQ;
+		if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		else
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		iwe.u.freq.e = 0;
+		iwe.u.freq.i = 0;
+
+		previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_FREQ_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+        //Add quality statistics
+        //================================
+        memset(&iwe, 0, sizeof(iwe));
+    	iwe.cmd = IWEVQUAL;
+    	iwe.u.qual.level = 0;
+    	iwe.u.qual.noise = 0;
+        set_quality(pAdapter, &iwe.u.qual, pAdapter->ScanTab.BssEntry[i].Rssi);
+    	current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Encyption key
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWENCODE;
+		if (CAP_IS_PRIVACY_ON (pAdapter->ScanTab.BssEntry[i].CapabilityInfo ))
+			iwe.u.data.flags =IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+		else
+			iwe.u.data.flags = IW_ENCODE_DISABLED;
+
+        previous_ev = current_ev;
+        current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf,&iwe, (char *)pAdapter->SharedKey[BSS0][(iwe.u.data.flags & IW_ENCODE_INDEX)-1].Key);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Bit Rate
+		//================================
+		if (pAdapter->ScanTab.BssEntry[i].SupRateLen)
+        {
+            UCHAR tmpRate = pAdapter->ScanTab.BssEntry[i].SupRate[pAdapter->ScanTab.BssEntry[i].SupRateLen-1];
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWRATE;
+    		current_val = current_ev + IW_EV_LCP_LEN;
+            if (tmpRate == 0x82)
+                iwe.u.bitrate.value =  1 * 1000000;
+            else if (tmpRate == 0x84)
+                iwe.u.bitrate.value =  2 * 1000000;
+            else if (tmpRate == 0x8B)
+                iwe.u.bitrate.value =  5.5 * 1000000;
+            else if (tmpRate == 0x96)
+                iwe.u.bitrate.value =  11 * 1000000;
+            else
+    		    iwe.u.bitrate.value =  (tmpRate/2) * 1000000;
+
+			iwe.u.bitrate.disabled = 0;
+			current_val = IWE_STREAM_ADD_VALUE(info, current_ev,
+				current_val, end_buf, &iwe,
+    			IW_EV_PARAM_LEN);
+
+        	if((current_val-current_ev)>IW_EV_LCP_LEN)
+            	current_ev = current_val;
+        	else
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+#ifdef IWEVGENIE
+		//WPA IE
+		if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+		{
+			memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].WpaIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].WpaIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].WpaIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+		}
+
+		//WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+        	memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].RsnIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].RsnIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].RsnIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#else
+        //WPA IE
+		//================================
+        if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "wpa_ie=", 7);
+            for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].WpaIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].WpaIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+        //WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "rsn_ie=", 7);
+			for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].RsnIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].RsnIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#endif // IWEVGENIE //
+	}
+
+	data->length = current_ev - extra;
+    pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+	DBGPRINT(RT_DEBUG_ERROR ,("===>rt_ioctl_giwscan. %d(%d) BSS returned, data->length = %d\n",i , pAdapter->ScanTab.BssNr, data->length));
+	return 0;
+}
+#endif
+
+int rt_ioctl_siwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (data->flags)
+	{
+		PCHAR	pSsidString = NULL;
+
+		// Includes null character.
+		if (data->length > (IW_ESSID_MAX_SIZE + 1))
+			return -E2BIG;
+
+		pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+		if (pSsidString)
+		{
+			NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+			NdisMoveMemory(pSsidString, essid, data->length);
+			if (Set_SSID_Proc(pAdapter, pSsidString) == FALSE)
+				return -EINVAL;
+		}
+		else
+			return -ENOMEM;
+	}
+	else
+	{
+		// ANY ssid
+		if (Set_SSID_Proc(pAdapter, "") == FALSE)
+			return -EINVAL;
+    }
+	return 0;
+}
+
+int rt_ioctl_giwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	data->flags = 1;
+    if (MONITOR_ON(pAdapter))
+    {
+        data->length  = 0;
+        return 0;
+    }
+
+	if (OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is connected\n"));
+		data->length = pAdapter->CommonCfg.SsidLen;
+		memcpy(essid, pAdapter->CommonCfg.Ssid, pAdapter->CommonCfg.SsidLen);
+	}
+#ifdef RT2870
+#ifdef WPA_SUPPLICANT_SUPPORT
+    // Add for RT2870
+    else if (pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+    {
+        data->length = pAdapter->CommonCfg.SsidLen;
+		memcpy(essid, pAdapter->CommonCfg.Ssid, pAdapter->CommonCfg.SsidLen);
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+#endif // RT2870 //
+	else
+	{//the ANY ssid was specified
+		data->length  = 0;
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is not connected, ess\n"));
+	}
+
+	return 0;
+
+}
+
+int rt_ioctl_siwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE ,("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (data->length > IW_ESSID_MAX_SIZE)
+		return -EINVAL;
+
+	memset(pAdapter->nickname, 0, IW_ESSID_MAX_SIZE + 1);
+	memcpy(pAdapter->nickname, nickname, data->length);
+
+
+	return 0;
+}
+
+int rt_ioctl_giwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (data->length > strlen(pAdapter->nickname) + 1)
+		data->length = strlen(pAdapter->nickname) + 1;
+	if (data->length > 0) {
+		memcpy(nickname, pAdapter->nickname, data->length-1);
+		nickname[data->length-1] = '\0';
+	}
+	return 0;
+}
+
+int rt_ioctl_siwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	u16 val;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (rts->disabled)
+		val = MAX_RTS_THRESHOLD;
+	else if (rts->value < 0 || rts->value > MAX_RTS_THRESHOLD)
+		return -EINVAL;
+	else if (rts->value == 0)
+	    val = MAX_RTS_THRESHOLD;
+	else
+		val = rts->value;
+
+	if (val != pAdapter->CommonCfg.RtsThreshold)
+		pAdapter->CommonCfg.RtsThreshold = val;
+
+	return 0;
+}
+
+int rt_ioctl_giwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	rts->value = pAdapter->CommonCfg.RtsThreshold;
+	rts->disabled = (rts->value == MAX_RTS_THRESHOLD);
+	rts->fixed = 1;
+
+	return 0;
+}
+
+int rt_ioctl_siwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	u16 val;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if (frag->disabled)
+		val = MAX_FRAG_THRESHOLD;
+	else if (frag->value >= MIN_FRAG_THRESHOLD || frag->value <= MAX_FRAG_THRESHOLD)
+        val = __cpu_to_le16(frag->value & ~0x1); /* even numbers only */
+	else if (frag->value == 0)
+	    val = MAX_FRAG_THRESHOLD;
+	else
+		return -EINVAL;
+
+	pAdapter->CommonCfg.FragmentThreshold = val;
+	return 0;
+}
+
+int rt_ioctl_giwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	frag->value = pAdapter->CommonCfg.FragmentThreshold;
+	frag->disabled = (frag->value == MAX_FRAG_THRESHOLD);
+	frag->fixed = 1;
+
+	return 0;
+}
+
+#define MAX_WEP_KEY_SIZE 13
+#define MIN_WEP_KEY_SIZE 5
+int rt_ioctl_siwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if ((erq->length == 0) &&
+        (erq->flags & IW_ENCODE_DISABLED))
+	{
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+	else if ((erq->length == 0) &&
+             (erq->flags & IW_ENCODE_RESTRICTED || erq->flags & IW_ENCODE_OPEN))
+	{
+	    //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+		STA_PORT_SECURED(pAdapter);
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+		if (erq->flags & IW_ENCODE_RESTRICTED)
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    	else
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+
+    if (erq->length > 0)
+	{
+		int keyIdx = (erq->flags & IW_ENCODE_INDEX) - 1;
+		/* Check the size of the key */
+		if (erq->length > MAX_WEP_KEY_SIZE) {
+			return -EINVAL;
+		}
+		/* Check key index */
+		if ((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+        {
+            DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::Wrong keyIdx=%d! Using default key instead (%d)\n",
+                                        keyIdx, pAdapter->StaCfg.DefaultKeyId));
+
+            //Using default key
+			keyIdx = pAdapter->StaCfg.DefaultKeyId;
+        }
+
+        NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+
+		if (erq->length == MAX_WEP_KEY_SIZE)
+        {
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+		}
+		else if (erq->length == MIN_WEP_KEY_SIZE)
+        {
+            pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+		}
+		else
+			/* Disable the key */
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+
+		/* Check if the key is not marked as invalid */
+		if(!(erq->flags & IW_ENCODE_NOKEY)) {
+			/* Copy the key in the driver */
+			NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, extra, erq->length);
+        }
+	}
+    else
+			{
+		/* Do we want to just set the transmit key index ? */
+		int index = (erq->flags & IW_ENCODE_INDEX) - 1;
+		if ((index >= 0) && (index < 4))
+        {
+			pAdapter->StaCfg.DefaultKeyId = index;
+            }
+        else
+			/* Don't complain if only change the mode */
+			if(!erq->flags & IW_ENCODE_MODE) {
+				return -EINVAL;
+		}
+	}
+
+done:
+    DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::erq->flags=%x\n",erq->flags));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::AuthMode=%x\n",pAdapter->StaCfg.AuthMode));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::DefaultKeyId=%x, KeyLen = %d\n",pAdapter->StaCfg.DefaultKeyId , pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::WepStatus=%x\n",pAdapter->StaCfg.WepStatus));
+	return 0;
+}
+
+int
+rt_ioctl_giwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *key)
+{
+	int kid;
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	kid = erq->flags & IW_ENCODE_INDEX;
+	DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_giwencode %d\n", erq->flags & IW_ENCODE_INDEX));
+
+	if (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled)
+	{
+		erq->length = 0;
+		erq->flags = IW_ENCODE_DISABLED;
+	}
+	else if ((kid > 0) && (kid <=4))
+	{
+		// copy wep key
+		erq->flags = kid ;			/* NB: base 1 */
+		if (erq->length > pAdapter->SharedKey[BSS0][kid-1].KeyLen)
+			erq->length = pAdapter->SharedKey[BSS0][kid-1].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][kid-1].Key, erq->length);
+		//if ((kid == pAdapter->PortCfg.DefaultKeyId))
+		//erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+
+	}
+	else if (kid == 0)
+	{
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->length = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, erq->length);
+		// copy default key ID
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->flags = pAdapter->StaCfg.DefaultKeyId + 1;			/* NB: base 1 */
+		erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+	}
+
+	return 0;
+
+}
+
+static int
+rt_ioctl_setparam(struct net_device *dev, struct iw_request_info *info,
+			 void *w, char *extra)
+{
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter;
+	POS_COOKIE pObj;
+	char *this_char = extra;
+	char *value;
+	int  Status=0;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+	pObj = (POS_COOKIE) pAdapter->OS_Cookie;
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+			return -ENETDOWN;
+    	}
+
+	if (!*this_char)
+		return -EINVAL;
+
+	if ((value = rtstrchr(this_char, '=')) != NULL)
+	    *value++ = 0;
+
+	if (!value)
+	    return -EINVAL;
+
+	// reject setting nothing besides ANY ssid(ssidLen=0)
+    if (!*value && (strcmp(this_char, "SSID") != 0))
+        return -EINVAL;
+
+	for (PRTMP_PRIVATE_SET_PROC = RTMP_PRIVATE_SUPPORT_PROC; PRTMP_PRIVATE_SET_PROC->name; PRTMP_PRIVATE_SET_PROC++)
+	{
+	    if (strcmp(this_char, PRTMP_PRIVATE_SET_PROC->name) == 0)
+	    {
+	        if(!PRTMP_PRIVATE_SET_PROC->set_proc(pAdapter, value))
+	        {	//FALSE:Set private failed then return Invalid argument
+			    Status = -EINVAL;
+	        }
+		    break;	//Exit for loop.
+	    }
+	}
+
+	if(PRTMP_PRIVATE_SET_PROC->name == NULL)
+	{  //Not found argument
+	    Status = -EINVAL;
+	    DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_setparam:: (iwpriv) Not Support Set Command [%s=%s]\n", this_char, value));
+	}
+
+    return Status;
+}
+
+
+static int
+rt_private_get_statistics(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+	INT				Status = 0;
+    PRTMP_ADAPTER   pAd = dev->ml_priv;
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+    sprintf(extra, "\n\n");
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+	    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	    //sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart - (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+	}
+    sprintf(extra+strlen(extra), "Tx success after retry          = %ld\n", (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx fail to Rcv ACK after retry  = %ld\n", (ULONG)pAd->WlanCounters.FailedCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Success Rcv CTS             = %ld\n", (ULONG)pAd->WlanCounters.RTSSuccessCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Fail Rcv CTS                = %ld\n", (ULONG)pAd->WlanCounters.RTSFailureCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "Rx success                      = %ld\n", (ULONG)pAd->WlanCounters.ReceivedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx with CRC                     = %ld\n", (ULONG)pAd->WlanCounters.FCSErrorCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx drop due to out of resource  = %ld\n", (ULONG)pAd->Counters8023.RxNoBuffer);
+    sprintf(extra+strlen(extra), "Rx duplicate frame              = %ld\n", (ULONG)pAd->WlanCounters.FrameDuplicateCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "False CCA (one second)          = %ld\n", (ULONG)pAd->RalinkCounters.OneSecFalseCCACnt);
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		if (pAd->ate.RxAntennaSel == 0)
+		{
+    		sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->ate.LastRssi1 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->ate.LastRssi2 - pAd->BbpRssiToDbmDelta));
+		}
+		else
+		{
+    		sprintf(extra+strlen(extra), "RSSI                            = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+		}
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    	sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi0 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi1 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi2 - pAd->BbpRssiToDbmDelta));
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    sprintf(extra+strlen(extra), "WpaSupplicantUP                 = %d\n\n", pAd->StaCfg.WpaSupplicantUP);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+    DBGPRINT(RT_DEBUG_TRACE, ("<== rt_private_get_statistics, wrq->length = %d\n", wrq->length));
+
+    return Status;
+}
+
+#ifdef DOT11_N_SUPPORT
+void	getBaInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pOutBuf)
+{
+	INT i, j;
+	BA_ORI_ENTRY *pOriBAEntry;
+	BA_REC_ENTRY *pRecBAEntry;
+
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+		if (((pEntry->ValidAsCLI || pEntry->ValidAsApCli) && (pEntry->Sst == SST_ASSOC))
+			|| (pEntry->ValidAsWDS) || (pEntry->ValidAsMesh))
+		{
+			sprintf(pOutBuf, "%s\n%02X:%02X:%02X:%02X:%02X:%02X (Aid = %d) (AP) -\n",
+                pOutBuf,
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5], pEntry->Aid);
+
+			sprintf(pOutBuf, "%s[Recipient]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BARecWcidArray[j] != 0)
+				{
+					pRecBAEntry =&pAd->BATable.BARecEntry[pEntry->BARecWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, LastIndSeq=%d, ReorderingPkts=%d\n", pOutBuf, j, pRecBAEntry->BAWinSize, pRecBAEntry->LastIndSeq, pRecBAEntry->list.qlen);
+				}
+			}
+			sprintf(pOutBuf, "%s\n", pOutBuf);
+
+			sprintf(pOutBuf, "%s[Originator]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BAOriWcidArray[j] != 0)
+				{
+					pOriBAEntry =&pAd->BATable.BAOriEntry[pEntry->BAOriWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, StartSeq=%d, CurTxSeq=%d\n", pOutBuf, j, pOriBAEntry->BAWinSize, pOriBAEntry->Sequence, pEntry->TxSeq[j]);
+				}
+			}
+			sprintf(pOutBuf, "%s\n\n", pOutBuf);
+		}
+        if (strlen(pOutBuf) > (IW_PRIV_SIZE_MASK - 30))
+                break;
+	}
+
+	return;
+}
+#endif // DOT11_N_SUPPORT //
+
+static int
+rt_private_show(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+    INT				Status = 0;
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+    PRTMP_ADAPTER   pAd;
+	POS_COOKIE		pObj;
+    u32             subcmd = wrq->flags;
+
+	if (dev->priv_flags == INT_MAIN)
+		pAd = dev->ml_priv;
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		pAd = pVirtualAd->RtmpDev->ml_priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+    switch(subcmd)
+    {
+
+        case SHOW_CONN_STATUS:
+            if (MONITOR_ON(pAd))
+            {
+#ifdef DOT11_N_SUPPORT
+                if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                    pAd->CommonCfg.RegTransmitSetting.field.BW)
+                    sprintf(extra, "Monitor Mode(CentralChannel %d)\n", pAd->CommonCfg.CentralChannel);
+                else
+#endif // DOT11_N_SUPPORT //
+                    sprintf(extra, "Monitor Mode(Channel %d)\n", pAd->CommonCfg.Channel);
+            }
+            else
+            {
+                if (pAd->IndicateMediaState == NdisMediaStateConnected)
+            	{
+            	    if (INFRA_ON(pAd))
+                    {
+                    sprintf(extra, "Connected(AP: %s[%02X:%02X:%02X:%02X:%02X:%02X])\n",
+                                    pAd->CommonCfg.Ssid,
+                                    pAd->CommonCfg.Bssid[0],
+                                    pAd->CommonCfg.Bssid[1],
+                                    pAd->CommonCfg.Bssid[2],
+                                    pAd->CommonCfg.Bssid[3],
+                                    pAd->CommonCfg.Bssid[4],
+                                    pAd->CommonCfg.Bssid[5]);
+            		DBGPRINT(RT_DEBUG_TRACE ,("Ssid=%s ,Ssidlen = %d\n",pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen));
+            	}
+                    else if (ADHOC_ON(pAd))
+                        sprintf(extra, "Connected\n");
+            	}
+            	else
+            	{
+            	    sprintf(extra, "Disconnected\n");
+            		DBGPRINT(RT_DEBUG_TRACE ,("ConnStatus is not connected\n"));
+            	}
+            }
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case SHOW_DRVIER_VERION:
+            sprintf(extra, "Driver version-%s, %s %s\n", STA_DRIVER_VERSION, __DATE__, __TIME__ );
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#ifdef DOT11_N_SUPPORT
+        case SHOW_BA_INFO:
+            getBaInfo(pAd, extra);
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#endif // DOT11_N_SUPPORT //
+		case SHOW_DESC_INFO:
+			{
+				Show_DescInfo_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+        case RAIO_OFF:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = FALSE;
+            if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == FALSE)
+                {
+                    MlmeRadioOff(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = SW_RADIO_OFF;
+                }
+            }
+            sprintf(extra, "Radio Off\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case RAIO_ON:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = TRUE;
+            //if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == TRUE)
+                {
+                    MlmeRadioOn(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+                }
+            }
+            sprintf(extra, "Radio On\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case SHOW_DLS_ENTRY_INFO:
+			{
+				Set_DlsEntryInfo_Display_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+
+		case SHOW_CFG_VALUE:
+			{
+				Status = RTMPShowCfgValue(pAd, wrq->pointer, extra);
+				if (Status == 0)
+					wrq->length = strlen(extra) + 1; // 1: size of '\0'
+			}
+			break;
+		case SHOW_ADHOC_ENTRY_INFO:
+			Show_Adhoc_MacTable_Proc(pAd, extra);
+			wrq->length = strlen(extra) + 1; // 1: size of '\0'
+			break;
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("%s - unknow subcmd = %d\n", __func__, subcmd));
+            break;
+    }
+
+    return Status;
+}
+
+#ifdef SIOCSIWMLME
+int rt_ioctl_siwmlme(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+	struct iw_mlme *pMlme = (struct iw_mlme *)wrqu->data.pointer;
+	MLME_QUEUE_ELEM				MsgElem;
+	MLME_DISASSOC_REQ_STRUCT	DisAssocReq;
+	MLME_DEAUTH_REQ_STRUCT      DeAuthReq;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("====> %s\n", __func__));
+
+	if (pMlme == NULL)
+		return -EINVAL;
+
+	switch(pMlme->cmd)
+	{
+#ifdef IW_MLME_DEAUTH
+		case IW_MLME_DEAUTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DEAUTH\n", __func__));
+			COPY_MAC_ADDR(DeAuthReq.Addr, pAd->CommonCfg.Bssid);
+			DeAuthReq.Reason = pMlme->reason_code;
+			MsgElem.MsgLen = sizeof(MLME_DEAUTH_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DeAuthReq, sizeof(MLME_DEAUTH_REQ_STRUCT));
+			MlmeDeauthReqAction(pAd, &MsgElem);
+			if (INFRA_ON(pAd))
+			{
+			    LinkDown(pAd, FALSE);
+			    pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			}
+			break;
+#endif // IW_MLME_DEAUTH //
+#ifdef IW_MLME_DISASSOC
+		case IW_MLME_DISASSOC:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DISASSOC\n", __func__));
+			COPY_MAC_ADDR(DisAssocReq.Addr, pAd->CommonCfg.Bssid);
+			DisAssocReq.Reason =  pMlme->reason_code;
+
+			MsgElem.Machine = ASSOC_STATE_MACHINE;
+			MsgElem.MsgType = MT2_MLME_DISASSOC_REQ;
+			MsgElem.MsgLen = sizeof(MLME_DISASSOC_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DisAssocReq, sizeof(MLME_DISASSOC_REQ_STRUCT));
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+			MlmeDisassocReqAction(pAd, &MsgElem);
+			break;
+#endif // IW_MLME_DISASSOC //
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - Unknow Command\n", __func__));
+			break;
+	}
+
+	return 0;
+}
+#endif // SIOCSIWMLME //
+
+#if WIRELESS_EXT > 17
+int rt_ioctl_siwauth(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = dev->ml_priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+	switch (param->flags & IW_AUTH_INDEX) {
+    	case IW_AUTH_WPA_VERSION:
+            if (param->value == IW_AUTH_WPA_VERSION_WPA)
+            {
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+				if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+					pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+            }
+            else if (param->value == IW_AUTH_WPA_VERSION_WPA2)
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_PAIRWISE:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption2Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption3Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_PAIRWISE - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_GROUP:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_GROUP - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_KEY_MGMT:
+            if (param->value == IW_AUTH_KEY_MGMT_802_1X)
+            {
+                if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+                else if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+#ifdef WPA_SUPPLICANT_SUPPORT
+                else
+                    // WEP 1x
+                    pAdapter->StaCfg.IEEE8021X = TRUE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == 0)
+            {
+                //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAdapter);
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_KEY_MGMT - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+            break;
+    	case IW_AUTH_PRIVACY_INVOKED:
+            /*if (param->value == 0)
+			{
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+        	    pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+            }*/
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_PRIVACY_INVOKED - param->value = %d!\n", __func__, param->value));
+    		break;
+    	case IW_AUTH_DROP_UNENCRYPTED:
+            if (param->value != 0)
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			else
+			{
+                //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAdapter);
+			}
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __func__, param->value));
+    		break;
+    	case IW_AUTH_80211_AUTH_ALG:
+			if (param->value & IW_AUTH_ALG_SHARED_KEY)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+			}
+            else if (param->value & IW_AUTH_ALG_OPEN_SYSTEM)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+			}
+            else
+				return -EINVAL;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_80211_AUTH_ALG - param->value = %d!\n", __func__, param->value));
+			break;
+    	case IW_AUTH_WPA_ENABLED:
+    		DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_ENABLED - Driver supports WPA!(param->value = %d)\n", __func__, param->value));
+    		break;
+    	default:
+    		return -EOPNOTSUPP;
+}
+
+	return 0;
+}
+
+int rt_ioctl_giwauth(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = dev->ml_priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+    }
+
+	switch (param->flags & IW_AUTH_INDEX) {
+	case IW_AUTH_DROP_UNENCRYPTED:
+        param->value = (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled) ? 0 : 1;
+		break;
+
+	case IW_AUTH_80211_AUTH_ALG:
+        param->value = (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared) ? IW_AUTH_ALG_SHARED_KEY : IW_AUTH_ALG_OPEN_SYSTEM;
+		break;
+
+	case IW_AUTH_WPA_ENABLED:
+		param->value = (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) ? 1 : 0;
+		break;
+
+	default:
+		return -EOPNOTSUPP;
+	}
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_giwauth::param->value = %d!\n", param->value));
+	return 0;
+}
+
+void fnSetCipherKey(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  INT             keyIdx,
+    IN  UCHAR           CipherAlg,
+    IN  BOOLEAN         bGTK,
+    IN  struct iw_encode_ext *ext)
+{
+    NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, LEN_TKIP_EK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].TxMic, ext->key + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].RxMic, ext->key + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CipherAlg;
+
+    // Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAdapter,
+						  BSS0,
+						  keyIdx,
+						  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+						  pAdapter->SharedKey[BSS0][keyIdx].Key,
+						  pAdapter->SharedKey[BSS0][keyIdx].TxMic,
+						  pAdapter->SharedKey[BSS0][keyIdx].RxMic);
+
+    if (bGTK)
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  NULL);
+    else
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  &pAdapter->MacTab.Content[BSSID_WCID]);
+}
+
+int rt_ioctl_siwencodeext(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+			{
+    PRTMP_ADAPTER   pAdapter = dev->ml_priv;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+    int keyIdx, alg = ext->alg;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if (encoding->flags & IW_ENCODE_DISABLED)
+	{
+        keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+        // set BSSID wcid entry of the Pair-wise Key table as no-security mode
+	    AsicRemovePairwiseKeyEntry(pAdapter, BSS0, BSSID_WCID);
+        pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+		pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_NONE;
+		AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)keyIdx);
+        NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+        DBGPRINT(RT_DEBUG_TRACE, ("%s::Remove all keys!(encoding->flags = %x)\n", __func__, encoding->flags));
+    }
+					else
+    {
+        // Get Key Index and convet to our own defined key index
+    	keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+    	if((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+    		return -EINVAL;
+
+        if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+        {
+            pAdapter->StaCfg.DefaultKeyId = keyIdx;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::DefaultKeyId = %d\n", __func__, pAdapter->StaCfg.DefaultKeyId));
+        }
+
+        switch (alg) {
+    		case IW_ENCODE_ALG_NONE:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_NONE\n", __func__));
+    			break;
+    		case IW_ENCODE_ALG_WEP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_WEP - ext->key_len = %d, keyIdx = %d\n", __func__, ext->key_len, keyIdx));
+    			if (ext->key_len == MAX_WEP_KEY_SIZE)
+                {
+        			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+				}
+        		else if (ext->key_len == MIN_WEP_KEY_SIZE)
+                {
+                    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+				}
+        		else
+                    return -EINVAL;
+
+                NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+			    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, ext->key_len);
+
+				if (pAdapter->StaCfg.GroupCipher == Ndis802_11GroupWEP40Enabled ||
+					pAdapter->StaCfg.GroupCipher == Ndis802_11GroupWEP104Enabled)
+				{
+					// Set Group key material to Asic
+					AsicAddSharedKeyEntry(pAdapter, BSS0, keyIdx, pAdapter->SharedKey[BSS0][keyIdx].CipherAlg, pAdapter->SharedKey[BSS0][keyIdx].Key, NULL, NULL);
+
+					// Update WCID attribute table and IVEIV table for this group key table
+					RTMPAddWcidAttributeEntry(pAdapter, BSS0, keyIdx, pAdapter->SharedKey[BSS0][keyIdx].CipherAlg, NULL);
+
+					STA_PORT_SECURED(pAdapter);
+
+    				// Indicate Connected for GUI
+    				pAdapter->IndicateMediaState = NdisMediaStateConnected;
+				}
+    			break;
+            case IW_ENCODE_ALG_TKIP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_TKIP - keyIdx = %d, ext->key_len = %d\n", __func__, keyIdx, ext->key_len));
+                if (ext->key_len == 32)
+                {
+                    if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, FALSE, ext);
+                        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                        {
+                            //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+                            STA_PORT_SECURED(pAdapter);
+                        }
+		}
+                    else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, TRUE, ext);
+
+                        // set 802.1x port control
+            	        //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+            	        STA_PORT_SECURED(pAdapter);
+                    }
+                }
+                else
+                    return -EINVAL;
+                break;
+            case IW_ENCODE_ALG_CCMP:
+                if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+		{
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, FALSE, ext);
+                    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                    	//pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+                    	STA_PORT_SECURED(pAdapter);
+                }
+                else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                {
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, TRUE, ext);
+
+                    // set 802.1x port control
+        	        //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+        	        STA_PORT_SECURED(pAdapter);
+                }
+                break;
+    		default:
+    			return -EINVAL;
+		}
+    }
+
+    return 0;
+}
+
+int
+rt_ioctl_giwencodeext(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER pAd = dev->ml_priv;
+	PCHAR pKey = NULL;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+	int idx, max_key_len;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_giwencodeext\n"));
+
+	max_key_len = encoding->length - sizeof(*ext);
+	if (max_key_len < 0)
+		return -EINVAL;
+
+	idx = encoding->flags & IW_ENCODE_INDEX;
+	if (idx)
+	{
+		if (idx < 1 || idx > 4)
+			return -EINVAL;
+		idx--;
+
+		if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+			(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled))
+		{
+			if (idx != pAd->StaCfg.DefaultKeyId)
+			{
+				ext->key_len = 0;
+				return 0;
+			}
+		}
+	}
+	else
+		idx = pAd->StaCfg.DefaultKeyId;
+
+	encoding->flags = idx + 1;
+	memset(ext, 0, sizeof(*ext));
+
+	ext->key_len = 0;
+	switch(pAd->StaCfg.WepStatus) {
+		case Ndis802_11WEPDisabled:
+			ext->alg = IW_ENCODE_ALG_NONE;
+			encoding->flags |= IW_ENCODE_DISABLED;
+			break;
+		case Ndis802_11WEPEnabled:
+			ext->alg = IW_ENCODE_ALG_WEP;
+			if (pAd->SharedKey[BSS0][idx].KeyLen > max_key_len)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = pAd->SharedKey[BSS0][idx].KeyLen;
+				pKey = &(pAd->SharedKey[BSS0][idx].Key[0]);
+			}
+			break;
+		case Ndis802_11Encryption2Enabled:
+		case Ndis802_11Encryption3Enabled:
+			if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+				ext->alg = IW_ENCODE_ALG_TKIP;
+			else
+				ext->alg = IW_ENCODE_ALG_CCMP;
+
+			if (max_key_len < 32)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = 32;
+				pKey = &pAd->StaCfg.PMK[0];
+			}
+			break;
+		default:
+			return -EINVAL;
+	}
+
+	if (ext->key_len && pKey)
+	{
+		encoding->flags |= IW_ENCODE_ENABLED;
+		memcpy(ext->key, pKey, ext->key_len);
+	}
+
+	return 0;
+}
+
+#ifdef SIOCSIWGENIE
+int rt_ioctl_siwgenie(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+
+	if (wrqu->data.length > MAX_LEN_OF_RSNIE ||
+	    (wrqu->data.length && extra == NULL))
+		return -EINVAL;
+
+	if (wrqu->data.length)
+	{
+		pAd->StaCfg.RSNIE_Len = wrqu->data.length;
+		NdisMoveMemory(&pAd->StaCfg.RSN_IE[0], extra, pAd->StaCfg.RSNIE_Len);
+	}
+	else
+	{
+		pAd->StaCfg.RSNIE_Len = 0;
+		NdisZeroMemory(&pAd->StaCfg.RSN_IE[0], MAX_LEN_OF_RSNIE);
+	}
+
+	return 0;
+}
+#endif // SIOCSIWGENIE //
+
+int rt_ioctl_giwgenie(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+
+	if ((pAd->StaCfg.RSNIE_Len == 0) ||
+		(pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA))
+	{
+		wrqu->data.length = 0;
+		return 0;
+	}
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifdef SIOCSIWGENIE
+	if (pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+	if (wrqu->data.length < pAd->StaCfg.RSNIE_Len)
+		return -E2BIG;
+
+	wrqu->data.length = pAd->StaCfg.RSNIE_Len;
+	memcpy(extra, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+	else
+#endif // SIOCSIWGENIE //
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+	{
+		UCHAR RSNIe = IE_WPA;
+
+		if (wrqu->data.length < (pAd->StaCfg.RSNIE_Len + 2)) // ID, Len
+			return -E2BIG;
+		wrqu->data.length = pAd->StaCfg.RSNIE_Len + 2;
+
+		if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2))
+			RSNIe = IE_RSN;
+
+		extra[0] = (char)RSNIe;
+		extra[1] = pAd->StaCfg.RSNIE_Len;
+		memcpy(extra+2, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+
+	return 0;
+}
+
+int rt_ioctl_siwpmksa(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+	struct iw_pmksa *pPmksa = (struct iw_pmksa *)wrqu->data.pointer;
+	INT	CachedIdx = 0, idx = 0;
+
+	if (pPmksa == NULL)
+		return -EINVAL;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_siwpmksa\n"));
+	switch(pPmksa->cmd)
+	{
+		case IW_PMKSA_FLUSH:
+			NdisZeroMemory(pAd->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_FLUSH\n"));
+			break;
+		case IW_PMKSA_REMOVE:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+		        {
+		        	NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN);
+					NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].PMKID, 16);
+					for (idx = CachedIdx; idx < (pAd->StaCfg.SavedPMKNum - 1); idx++)
+					{
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].BSSID[0], &pAd->StaCfg.SavedPMK[idx+1].BSSID[0], MAC_ADDR_LEN);
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].PMKID[0], &pAd->StaCfg.SavedPMK[idx+1].PMKID[0], 16);
+					}
+					pAd->StaCfg.SavedPMKNum--;
+			        break;
+		        }
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_REMOVE\n"));
+			break;
+		case IW_PMKSA_ADD:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+			        break;
+	        }
+
+	        // Found, replace it
+	        if (CachedIdx < PMKID_NO)
+	        {
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+		        pAd->StaCfg.SavedPMKNum++;
+	        }
+	        // Not found, replace the last one
+	        else
+	        {
+		        // Randomly replace one
+		        CachedIdx = (pPmksa->bssid.sa_data[5] % PMKID_NO);
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_ADD\n"));
+			break;
+		default:
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - Unknow Command!!\n"));
+			break;
+	}
+
+	return 0;
+}
+#endif // #if WIRELESS_EXT > 17
+
+#ifdef DBG
+static int
+rt_private_ioctl_bbp(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+			{
+	CHAR				*this_char;
+	CHAR				*value = NULL;
+	UCHAR				regBBP = 0;
+//	CHAR				arg[255]={0};
+	UINT32				bbpId;
+	UINT32				bbpValue;
+	BOOLEAN				bIsPrintAllBBP = FALSE;
+	INT					Status = 0;
+    PRTMP_ADAPTER       pAdapter = dev->ml_priv;
+
+
+	memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	if (wrq->length > 1) //No parameters.
+				{
+		sprintf(extra, "\n");
+
+		//Parsing Read or Write
+		this_char = wrq->pointer;
+		DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s\n", this_char));
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s, value=%s\n", this_char, value));
+			if (sscanf(this_char, "%d", &(bbpId)) == 1)
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		else
+		{ //Write
+			if ((sscanf(this_char, "%d", &(bbpId)) == 1) && (sscanf(value, "%x", &(bbpValue)) == 1))
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+						//Read it back for showing
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					    RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+    					//Read it back for showing
+    					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		}
+	else
+		bIsPrintAllBBP = TRUE;
+
+next:
+	if (bIsPrintAllBBP)
+	{
+		memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+		sprintf(extra, "\n");
+		for (bbpId = 0; bbpId <= 136; bbpId++)
+		{
+		    if (strlen(extra) >= (IW_PRIV_SIZE_MASK - 10))
+                break;
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+			else
+#endif // RALINK_ATE //
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X    ", bbpId, bbpId*2, regBBP);
+			if (bbpId%5 == 4)
+				sprintf(extra+strlen(extra), "\n");
+		}
+
+        wrq->length = strlen(extra) + 1; // 1: size of '\0'
+        DBGPRINT(RT_DEBUG_TRACE, ("wrq->length = %d\n", wrq->length));
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==rt_private_ioctl_bbp\n\n"));
+
+    return Status;
+}
+#endif // DBG //
+
+int rt_ioctl_siwrate(struct net_device *dev,
+			struct iw_request_info *info,
+			union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = dev->ml_priv;
+    UINT32          rate = wrqu->bitrate.value, fixed = wrqu->bitrate.fixed;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(rate = %d, fixed = %d)\n", rate, fixed));
+    /* rate = -1 => auto rate
+       rate = X, fixed = 1 => (fixed rate X)
+    */
+    if (rate == -1)
+    {
+		//Auto Rate
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+		pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+		if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+		    (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+			RTMPSetDesiredRates(pAd, -1);
+
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+    }
+    else
+    {
+        if (fixed)
+        {
+        	pAd->StaCfg.bAutoTxRateSwitch = FALSE;
+            if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+                (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+                RTMPSetDesiredRates(pAd, rate);
+            else
+            {
+                pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+#ifdef DOT11_N_SUPPORT
+                SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(HtMcs=%d)\n",pAd->StaCfg.DesiredTransmitSetting.field.MCS));
+        }
+        else
+        {
+            // TODO: rate = X, fixed = 0 => (rates <= X)
+            return -EOPNOTSUPP;
+        }
+    }
+
+    return 0;
+}
+
+int rt_ioctl_giwrate(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = dev->ml_priv;
+    int rate_index = 0, rate_count = 0;
+    HTTRANSMIT_SETTING ht_setting;
+    __s32 ralinkrate[] =
+	{2,  4,   11,  22, // CCK
+	12, 18,   24,  36, 48, 72, 96, 108, // OFDM
+	13, 26,   39,  52,  78, 104, 117, 130, 26,  52,  78, 104, 156, 208, 234, 260, // 20MHz, 800ns GI, MCS: 0 ~ 15
+	39, 78,  117, 156, 234, 312, 351, 390,										  // 20MHz, 800ns GI, MCS: 16 ~ 23
+	27, 54,   81, 108, 162, 216, 243, 270, 54, 108, 162, 216, 324, 432, 486, 540, // 40MHz, 800ns GI, MCS: 0 ~ 15
+	81, 162, 243, 324, 486, 648, 729, 810,										  // 40MHz, 800ns GI, MCS: 16 ~ 23
+	14, 29,   43,  57,  87, 115, 130, 144, 29, 59,   87, 115, 173, 230, 260, 288, // 20MHz, 400ns GI, MCS: 0 ~ 15
+	43, 87,  130, 173, 260, 317, 390, 433,										  // 20MHz, 400ns GI, MCS: 16 ~ 23
+	30, 60,   90, 120, 180, 240, 270, 300, 60, 120, 180, 240, 360, 480, 540, 600, // 40MHz, 400ns GI, MCS: 0 ~ 15
+	90, 180, 270, 360, 540, 720, 810, 900};										  // 40MHz, 400ns GI, MCS: 16 ~ 23
+
+    rate_count = sizeof(ralinkrate)/sizeof(__s32);
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if ((pAd->StaCfg.bAutoTxRateSwitch == FALSE) &&
+        (INFRA_ON(pAd)) &&
+        ((pAd->CommonCfg.PhyMode <= PHY_11G) || (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM)))
+        ht_setting.word = pAd->StaCfg.HTPhyMode.word;
+    else
+        ht_setting.word = pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+
+#ifdef DOT11_N_SUPPORT
+    if (ht_setting.field.MODE >= MODE_HTMIX)
+    {
+//    	rate_index = 12 + ((UCHAR)ht_setting.field.BW *16) + ((UCHAR)ht_setting.field.ShortGI *32) + ((UCHAR)ht_setting.field.MCS);
+    	rate_index = 12 + ((UCHAR)ht_setting.field.BW *24) + ((UCHAR)ht_setting.field.ShortGI *48) + ((UCHAR)ht_setting.field.MCS);
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+    if (ht_setting.field.MODE == MODE_OFDM)
+    	rate_index = (UCHAR)(ht_setting.field.MCS) + 4;
+    else if (ht_setting.field.MODE == MODE_CCK)
+    	rate_index = (UCHAR)(ht_setting.field.MCS);
+
+    if (rate_index < 0)
+        rate_index = 0;
+
+    if (rate_index > rate_count)
+        rate_index = rate_count;
+
+    wrqu->bitrate.value = ralinkrate[rate_index] * 500000;
+    wrqu->bitrate.disabled = 0;
+
+    return 0;
+}
+
+static const iw_handler rt_handler[] =
+{
+	(iw_handler) NULL,			            /* SIOCSIWCOMMIT */
+	(iw_handler) rt_ioctl_giwname,			/* SIOCGIWNAME   */
+	(iw_handler) NULL,			            /* SIOCSIWNWID   */
+	(iw_handler) NULL,			            /* SIOCGIWNWID   */
+	(iw_handler) rt_ioctl_siwfreq,		    /* SIOCSIWFREQ   */
+	(iw_handler) rt_ioctl_giwfreq,		    /* SIOCGIWFREQ   */
+	(iw_handler) rt_ioctl_siwmode,		    /* SIOCSIWMODE   */
+	(iw_handler) rt_ioctl_giwmode,		    /* SIOCGIWMODE   */
+	(iw_handler) NULL,		                /* SIOCSIWSENS   */
+	(iw_handler) NULL,		                /* SIOCGIWSENS   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWRANGE  */
+	(iw_handler) rt_ioctl_giwrange,		    /* SIOCGIWRANGE  */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWPRIV   */
+	(iw_handler) NULL /* kernel code */,    /* SIOCGIWPRIV   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWSTATS  */
+	(iw_handler) rt28xx_get_wireless_stats /* kernel code */,    /* SIOCGIWSTATS  */
+	(iw_handler) NULL,		                /* SIOCSIWSPY    */
+	(iw_handler) NULL,		                /* SIOCGIWSPY    */
+	(iw_handler) NULL,				        /* SIOCSIWTHRSPY */
+	(iw_handler) NULL,				        /* SIOCGIWTHRSPY */
+	(iw_handler) rt_ioctl_siwap,            /* SIOCSIWAP     */
+	(iw_handler) rt_ioctl_giwap,		    /* SIOCGIWAP     */
+#ifdef SIOCSIWMLME
+	(iw_handler) rt_ioctl_siwmlme,	        /* SIOCSIWMLME   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWMLME */
+#endif // SIOCSIWMLME //
+	(iw_handler) rt_ioctl_iwaplist,		    /* SIOCGIWAPLIST */
+#ifdef SIOCGIWSCAN
+	(iw_handler) rt_ioctl_siwscan,		    /* SIOCSIWSCAN   */
+	(iw_handler) rt_ioctl_giwscan,		    /* SIOCGIWSCAN   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWSCAN   */
+	(iw_handler) NULL,				        /* SIOCGIWSCAN   */
+#endif /* SIOCGIWSCAN */
+	(iw_handler) rt_ioctl_siwessid,		    /* SIOCSIWESSID  */
+	(iw_handler) rt_ioctl_giwessid,		    /* SIOCGIWESSID  */
+	(iw_handler) rt_ioctl_siwnickn,		    /* SIOCSIWNICKN  */
+	(iw_handler) rt_ioctl_giwnickn,		    /* SIOCGIWNICKN  */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) rt_ioctl_siwrate,          /* SIOCSIWRATE   */
+	(iw_handler) rt_ioctl_giwrate,          /* SIOCGIWRATE   */
+	(iw_handler) rt_ioctl_siwrts,		    /* SIOCSIWRTS    */
+	(iw_handler) rt_ioctl_giwrts,		    /* SIOCGIWRTS    */
+	(iw_handler) rt_ioctl_siwfrag,		    /* SIOCSIWFRAG   */
+	(iw_handler) rt_ioctl_giwfrag,		    /* SIOCGIWFRAG   */
+	(iw_handler) NULL,		                /* SIOCSIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCGIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCSIWRETRY  */
+	(iw_handler) NULL,		                /* SIOCGIWRETRY  */
+	(iw_handler) rt_ioctl_siwencode,		/* SIOCSIWENCODE */
+	(iw_handler) rt_ioctl_giwencode,		/* SIOCGIWENCODE */
+	(iw_handler) NULL,		                /* SIOCSIWPOWER  */
+	(iw_handler) NULL,		                /* SIOCGIWPOWER  */
+	(iw_handler) NULL,						/* -- hole -- */
+	(iw_handler) NULL,						/* -- hole -- */
+#if WIRELESS_EXT > 17
+    (iw_handler) rt_ioctl_siwgenie,         /* SIOCSIWGENIE  */
+	(iw_handler) rt_ioctl_giwgenie,         /* SIOCGIWGENIE  */
+	(iw_handler) rt_ioctl_siwauth,		    /* SIOCSIWAUTH   */
+	(iw_handler) rt_ioctl_giwauth,		    /* SIOCGIWAUTH   */
+	(iw_handler) rt_ioctl_siwencodeext,	    /* SIOCSIWENCODEEXT */
+	(iw_handler) rt_ioctl_giwencodeext,		/* SIOCGIWENCODEEXT */
+	(iw_handler) rt_ioctl_siwpmksa,         /* SIOCSIWPMKSA  */
+#endif
+};
+
+static const iw_handler rt_priv_handlers[] = {
+	(iw_handler) NULL, /* + 0x00 */
+	(iw_handler) NULL, /* + 0x01 */
+#ifndef CONFIG_AP_SUPPORT
+	(iw_handler) rt_ioctl_setparam, /* + 0x02 */
+#else
+	(iw_handler) NULL, /* + 0x02 */
+#endif // CONFIG_AP_SUPPORT //
+#ifdef DBG
+	(iw_handler) rt_private_ioctl_bbp, /* + 0x03 */
+#else
+	(iw_handler) NULL, /* + 0x03 */
+#endif
+	(iw_handler) NULL, /* + 0x04 */
+	(iw_handler) NULL, /* + 0x05 */
+	(iw_handler) NULL, /* + 0x06 */
+	(iw_handler) NULL, /* + 0x07 */
+	(iw_handler) NULL, /* + 0x08 */
+	(iw_handler) rt_private_get_statistics, /* + 0x09 */
+	(iw_handler) NULL, /* + 0x0A */
+	(iw_handler) NULL, /* + 0x0B */
+	(iw_handler) NULL, /* + 0x0C */
+	(iw_handler) NULL, /* + 0x0D */
+	(iw_handler) NULL, /* + 0x0E */
+	(iw_handler) NULL, /* + 0x0F */
+	(iw_handler) NULL, /* + 0x10 */
+	(iw_handler) rt_private_show, /* + 0x11 */
+    (iw_handler) NULL, /* + 0x12 */
+	(iw_handler) NULL, /* + 0x13 */
+	(iw_handler) NULL, /* + 0x15 */
+	(iw_handler) NULL, /* + 0x17 */
+	(iw_handler) NULL, /* + 0x18 */
+};
+
+const struct iw_handler_def rt28xx_iw_handler_def =
+{
+#define	N(a)	(sizeof (a) / sizeof (a[0]))
+	.standard	= (iw_handler *) rt_handler,
+	.num_standard	= sizeof(rt_handler) / sizeof(iw_handler),
+	.private	= (iw_handler *) rt_priv_handlers,
+	.num_private		= N(rt_priv_handlers),
+	.private_args	= (struct iw_priv_args *) privtab,
+	.num_private_args	= N(privtab),
+#if IW_HANDLER_VERSION >= 7
+    .get_wireless_stats = rt28xx_get_wireless_stats,
+#endif
+};
+
+INT RTMPSetInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_MAC_ADDRESS             Bssid;
+    RT_802_11_PHY_MODE                  PhyMode;
+    RT_802_11_STA_CONFIG                StaConfig;
+    NDIS_802_11_RATES                   aryRates;
+    RT_802_11_PREAMBLE                  Preamble;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode = Ndis802_11AuthModeMax;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    PNDIS_802_11_KEY                    pKey = NULL;
+    PNDIS_802_11_WEP			        pWepKey =NULL;
+    PNDIS_802_11_REMOVE_KEY             pRemoveKey = NULL;
+    NDIS_802_11_CONFIGURATION           Config, *pConfig = NULL;
+    NDIS_802_11_NETWORK_TYPE            NetType;
+    ULONG                               Now;
+    UINT                                KeyIdx = 0;
+    INT                                 Status = NDIS_STATUS_SUCCESS, MaxPhyMode = PHY_11G;
+    ULONG                               PowerTemp;
+    BOOLEAN                             RadioState;
+    BOOLEAN                             StateMachineTouched = FALSE;
+#ifdef DOT11_N_SUPPORT
+	OID_SET_HT_PHYMODE					HT_PhyMode;	//11n ,kathy
+#endif // DOT11_N_SUPPORT //
+#ifdef WPA_SUPPLICANT_SUPPORT
+    PNDIS_802_11_PMKID                  pPmkId = NULL;
+    BOOLEAN				                IEEE8021xState = FALSE;
+    BOOLEAN				                IEEE8021x_required_keys = FALSE;
+    UCHAR                               wpa_supplicant_enable = 0;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef SNMP_SUPPORT
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						ctmp;
+#endif // SNMP_SUPPORT //
+
+
+
+#ifdef DOT11_N_SUPPORT
+	MaxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-->RTMPSetInformation(),	0x%08x\n", cmd&0x7FFF));
+    switch(cmd & 0x7FFF) {
+        case RT_OID_802_11_COUNTRY_REGION:
+            if (wrq->u.data.length < sizeof(UCHAR))
+                Status = -EINVAL;
+			// Only avaliable when EEPROM not programming
+            else if (!(pAdapter->CommonCfg.CountryRegion & 0x80) && !(pAdapter->CommonCfg.CountryRegionForABand & 0x80))
+            {
+                ULONG   Country;
+                UCHAR	TmpPhy;
+
+				Status = copy_from_user(&Country, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->CommonCfg.CountryRegion = (UCHAR)(Country & 0x000000FF);
+				pAdapter->CommonCfg.CountryRegionForABand = (UCHAR)((Country >> 8) & 0x000000FF);
+                TmpPhy = pAdapter->CommonCfg.PhyMode;
+				pAdapter->CommonCfg.PhyMode = 0xff;
+				// Build all corresponding channel information
+				RTMPSetPhyMode(pAdapter, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+				SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_COUNTRY_REGION (A:%d  B/G:%d)\n", pAdapter->CommonCfg.CountryRegionForABand,
+				    pAdapter->CommonCfg.CountryRegion));
+            }
+            break;
+        case OID_802_11_BSSID_LIST_SCAN:
+ #ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            Now = jiffies;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID_LIST_SCAN, TxCnt = %d \n", pAdapter->RalinkCounters.LastOneSecTotalTxCount));
+
+            if (MONITOR_ON(pAdapter))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+                break;
+            }
+
+			//Benson add 20080527, when radio off, sta don't need to scan
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_RADIO_OFF))
+				break;
+
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			{
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is scanning now !!!\n"));
+				pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+				Status = NDIS_STATUS_SUCCESS;
+                break;
+            }
+
+			if (pAdapter->RalinkCounters.LastOneSecTotalTxCount > 100)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+            if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+				((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)) &&
+                (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+
+            if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+            {
+                RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+            }
+
+            // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+            // this request, because this request is initiated by NDIS.
+            pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+            // Reset allowed scan retries
+            pAdapter->StaCfg.ScanCnt = 0;
+            pAdapter->StaCfg.LastScanTime = Now;
+
+			pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+            RTMP_SET_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+            MlmeEnqueue(pAdapter,
+                        MLME_CNTL_STATE_MACHINE,
+                        OID_802_11_BSSID_LIST_SCAN,
+                        0,
+                        NULL);
+
+            Status = NDIS_STATUS_SUCCESS;
+            StateMachineTouched = TRUE;
+            break;
+        case OID_802_11_SSID:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_SSID))
+                Status = -EINVAL;
+            else
+            {
+            	PCHAR pSsidString = NULL;
+                Status = copy_from_user(&Ssid, wrq->u.data.pointer, wrq->u.data.length);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SSID (Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+                if (Ssid.SsidLength > MAX_LEN_OF_SSID)
+                    Status = -EINVAL;
+                else
+                {
+                	if (Ssid.SsidLength == 0)
+                	{
+                		Set_SSID_Proc(pAdapter, "");
+                	}
+					else
+                	{
+	                	pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+						if (pSsidString)
+						{
+							NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+							NdisMoveMemory(pSsidString, Ssid.Ssid, Ssid.SsidLength);
+	                		Set_SSID_Proc(pAdapter, pSsidString);
+							kfree(pSsidString);
+						}
+						else
+							Status = -ENOMEM;
+                	}
+                }
+            }
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            if (wrq->u.data.length != sizeof(NDIS_802_11_MAC_ADDRESS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Bssid, wrq->u.data.pointer, wrq->u.data.length);
+
+                // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+                // this request, because this request is initiated by NDIS.
+                pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+
+				// Prevent to connect AP again in STAMlmePeriodicExec
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+                // Reset allowed scan retries
+				pAdapter->StaCfg.ScanCnt = 0;
+
+                if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+                {
+                    RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                    DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+                }
+                MlmeEnqueue(pAdapter,
+                            MLME_CNTL_STATE_MACHINE,
+                            OID_802_11_BSSID,
+                            sizeof(NDIS_802_11_MAC_ADDRESS),
+                            (VOID *)&Bssid);
+                Status = NDIS_STATUS_SUCCESS;
+                StateMachineTouched = TRUE;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID %02x:%02x:%02x:%02x:%02x:%02x\n",
+                                        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+            }
+            break;
+        case RT_OID_802_11_RADIO:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RadioState, wrq->u.data.pointer, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RADIO (=%d)\n", RadioState));
+                if (pAdapter->StaCfg.bSwRadio != RadioState)
+                {
+                    pAdapter->StaCfg.bSwRadio = RadioState;
+                    if (pAdapter->StaCfg.bRadio != (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio))
+                    {
+                        pAdapter->StaCfg.bRadio = (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio);
+                        if (pAdapter->StaCfg.bRadio == TRUE)
+                        {
+                            MlmeRadioOn(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = EXTRA_INFO_CLEAR;
+                        }
+                        else
+                        {
+                            MlmeRadioOff(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = SW_RADIO_OFF;
+                        }
+                    }
+                }
+            }
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PHY_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				if (PhyMode <= MaxPhyMode)
+				{
+                	RTMPSetPhyMode(pAdapter, PhyMode);
+#ifdef DOT11_N_SUPPORT
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				}
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PHY_MODE (=%d)\n", PhyMode));
+            }
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            if (wrq->u.data.length != sizeof(RT_802_11_STA_CONFIG))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&StaConfig, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bEnableTxBurst = StaConfig.EnableTxBurst;
+                pAdapter->CommonCfg.UseBGProtection = StaConfig.UseBGProtection;
+                pAdapter->CommonCfg.bUseShortSlotTime = 1; // 2003-10-30 always SHORT SLOT capable
+                if ((pAdapter->CommonCfg.PhyMode != StaConfig.AdhocMode) &&
+					(StaConfig.AdhocMode <= MaxPhyMode))
+                {
+                    // allow dynamic change of "USE OFDM rate or not" in ADHOC mode
+                    // if setting changed, need to reset current TX rate as well as BEACON frame format
+                    if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                    {
+						pAdapter->CommonCfg.PhyMode = StaConfig.AdhocMode;
+                    	RTMPSetPhyMode(pAdapter, PhyMode);
+                        MlmeUpdateTxRates(pAdapter, FALSE, 0);
+                        MakeIbssBeacon(pAdapter);           // re-build BEACON frame
+                        AsicEnableIbssSync(pAdapter);   // copy to on-chip memory
+                    }
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_STA_CONFIG (Burst=%d, Protection=%ld,ShortSlot=%d\n",
+                                        pAdapter->CommonCfg.bEnableTxBurst,
+                                        pAdapter->CommonCfg.UseBGProtection,
+                                        pAdapter->CommonCfg.bUseShortSlotTime));
+            }
+            break;
+        case OID_802_11_DESIRED_RATES:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RATES))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&aryRates, wrq->u.data.pointer, wrq->u.data.length);
+                NdisZeroMemory(pAdapter->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+                NdisMoveMemory(pAdapter->CommonCfg.DesireRate, &aryRates, sizeof(NDIS_802_11_RATES));
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DESIRED_RATES (%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x)\n",
+                    pAdapter->CommonCfg.DesireRate[0],pAdapter->CommonCfg.DesireRate[1],
+                    pAdapter->CommonCfg.DesireRate[2],pAdapter->CommonCfg.DesireRate[3],
+                    pAdapter->CommonCfg.DesireRate[4],pAdapter->CommonCfg.DesireRate[5],
+                    pAdapter->CommonCfg.DesireRate[6],pAdapter->CommonCfg.DesireRate[7] ));
+                // Changing DesiredRate may affect the MAX TX rate we used to TX frames out
+                MlmeUpdateTxRates(pAdapter, FALSE, 0);
+            }
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PREAMBLE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Preamble, wrq->u.data.pointer, wrq->u.data.length);
+                if (Preamble == Rt802_11PreambleShort)
+                {
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleShort);
+                }
+                else if ((Preamble == Rt802_11PreambleLong) || (Preamble == Rt802_11PreambleAuto))
+                {
+                    // if user wants AUTO, initialize to LONG here, then change according to AP's
+                    // capability upon association.
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleLong);
+                }
+                else
+                {
+                    Status = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PREAMBLE (=%d)\n", Preamble));
+            }
+            break;
+        case OID_802_11_WEP_STATUS:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_WEP_STATUS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&WepStatus, wrq->u.data.pointer, wrq->u.data.length);
+                // Since TKIP, AES, WEP are all supported. It should not have any invalid setting
+                if (WepStatus <= Ndis802_11Encryption3KeyAbsent)
+                {
+                    if (pAdapter->StaCfg.WepStatus != WepStatus)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.WepStatus     = WepStatus;
+                    pAdapter->StaCfg.OrigWepStatus = WepStatus;
+                    pAdapter->StaCfg.PairCipher    = WepStatus;
+                	pAdapter->StaCfg.GroupCipher   = WepStatus;
+                }
+                else
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEP_STATUS (=%d)\n",WepStatus));
+            }
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_AUTHENTICATION_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&AuthMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (AuthMode > Ndis802_11AuthModeMax)
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                else
+                {
+                    if (pAdapter->StaCfg.AuthMode != AuthMode)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.AuthMode = AuthMode;
+                }
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_AUTHENTICATION_MODE (=%d) \n",pAdapter->StaCfg.AuthMode));
+            }
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_INFRASTRUCTURE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&BssType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (BssType == Ndis802_11IBSS)
+					Set_NetworkType_Proc(pAdapter, "Adhoc");
+				else if (BssType == Ndis802_11Infrastructure)
+					Set_NetworkType_Proc(pAdapter, "Infra");
+				else if (BssType == Ndis802_11Monitor)
+					Set_NetworkType_Proc(pAdapter, "Monitor");
+				else
+				{
+					Status  = -EINVAL;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_INFRASTRUCTURE_MODE (unknown)\n"));
+				}
+			}
+			break;
+	 case OID_802_11_REMOVE_WEP:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_WEP\n"));
+            if (wrq->u.data.length != sizeof(NDIS_802_11_KEY_INDEX))
+            {
+				Status = -EINVAL;
+            }
+            else
+            {
+				KeyIdx = *(NDIS_802_11_KEY_INDEX *) wrq->u.data.pointer;
+
+				if (KeyIdx & 0x80000000)
+				{
+					// Should never set default bit when remove key
+					Status = -EINVAL;
+				}
+				else
+				{
+					KeyIdx = KeyIdx & 0x0fffffff;
+					if (KeyIdx >= 4){
+						Status = -EINVAL;
+					}
+					else
+					{
+						pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+						pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+						AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+					}
+				}
+            }
+            break;
+        case RT_OID_802_11_RESET_COUNTERS:
+            NdisZeroMemory(&pAdapter->WlanCounters, sizeof(COUNTER_802_11));
+            NdisZeroMemory(&pAdapter->Counters8023, sizeof(COUNTER_802_3));
+            NdisZeroMemory(&pAdapter->RalinkCounters, sizeof(COUNTER_RALINK));
+            pAdapter->Counters8023.RxNoBuffer   = 0;
+			pAdapter->Counters8023.GoodReceives = 0;
+			pAdapter->Counters8023.RxNoBuffer   = 0;
+#ifdef RT2870
+			pAdapter->BulkOutComplete	= 0;
+			pAdapter->BulkOutCompleteOther= 0;
+			pAdapter->BulkOutCompleteCancel = 0;
+			pAdapter->BulkOutReq = 0;
+			pAdapter->BulkInReq= 0;
+			pAdapter->BulkInComplete = 0;
+			pAdapter->BulkInCompleteFail = 0;
+#endif // RT2870 //
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RESET_COUNTERS \n"));
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RTS_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RtsThresh, wrq->u.data.pointer, wrq->u.data.length);
+                if (RtsThresh > MAX_RTS_THRESHOLD)
+                    Status  = -EINVAL;
+                else
+                    pAdapter->CommonCfg.RtsThreshold = (USHORT)RtsThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_RTS_THRESHOLD (=%ld)\n",RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_FRAGMENTATION_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&FragThresh, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bUseZeroToDisableFragment = FALSE;
+                if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+                {
+                    if (FragThresh == 0)
+                    {
+                        pAdapter->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+                        pAdapter->CommonCfg.bUseZeroToDisableFragment = TRUE;
+                    }
+                    else
+                        Status  = -EINVAL;
+                }
+                else
+                    pAdapter->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_FRAGMENTATION_THRESHOLD (=%ld) \n",FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_POWER_MODE))
+                Status = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PowerMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (PowerMode == Ndis802_11PowerModeCAM)
+                	Set_PSMode_Proc(pAdapter, "CAM");
+                else if (PowerMode == Ndis802_11PowerModeMAX_PSP)
+                	Set_PSMode_Proc(pAdapter, "Max_PSP");
+                else if (PowerMode == Ndis802_11PowerModeFast_PSP)
+					Set_PSMode_Proc(pAdapter, "Fast_PSP");
+                else if (PowerMode == Ndis802_11PowerModeLegacy_PSP)
+					Set_PSMode_Proc(pAdapter, "Legacy_PSP");
+                else
+                    Status = -EINVAL;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_POWER_MODE (=%d)\n",PowerMode));
+            break;
+         case RT_OID_802_11_TX_POWER_LEVEL_1:
+			if (wrq->u.data.length  < sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&PowerTemp, wrq->u.data.pointer, wrq->u.data.length);
+				if (PowerTemp > 100)
+					PowerTemp = 0xffffffff;  // AUTO
+				pAdapter->CommonCfg.TxPowerDefault = PowerTemp; //keep current setting.
+					pAdapter->CommonCfg.TxPowerPercentage = pAdapter->CommonCfg.TxPowerDefault;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			}
+	        break;
+		case OID_802_11_NETWORK_TYPE_IN_USE:
+			if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_TYPE))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&NetType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (NetType == Ndis802_11DS)
+					RTMPSetPhyMode(pAdapter, PHY_11B);
+				else if (NetType == Ndis802_11OFDM24)
+					RTMPSetPhyMode(pAdapter, PHY_11BG_MIXED);
+				else if (NetType == Ndis802_11OFDM5)
+					RTMPSetPhyMode(pAdapter, PHY_11A);
+				else
+					Status = -EINVAL;
+#ifdef DOT11_N_SUPPORT
+				if (Status == NDIS_STATUS_SUCCESS)
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_NETWORK_TYPE_IN_USE (=%d)\n",NetType));
+		    }
+			break;
+        // For WPA PSK PMK key
+        case RT_OID_802_11_ADD_WPA:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!!\n"));
+            }
+            else
+            {
+                if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone) )
+                {
+                    Status = -EOPNOTSUPP;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!! [AuthMode != WPAPSK/WPA2PSK/WPANONE]\n"));
+                }
+                else if ((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone) )     // Only for WPA PSK mode
+				{
+                    NdisMoveMemory(pAdapter->StaCfg.PMK, &pKey->KeyMaterial, pKey->KeyLength);
+                    // Use RaConfig as PSK agent.
+                    // Start STA supplicant state machine
+                    if (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+                        pAdapter->StaCfg.WpaState = SS_START;
+
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+                else
+                {
+                    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_REMOVE_KEY:
+            pRemoveKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pRemoveKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pRemoveKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pRemoveKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!\n"));
+            }
+            else
+            {
+                if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+                {
+                    RTMPWPARemoveKeyProc(pAdapter, pRemoveKey);
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Remove WPA Key!!\n"));
+                }
+                else
+                {
+                    KeyIdx = pRemoveKey->KeyIndex;
+
+                    if (KeyIdx & 0x80000000)
+                    {
+                        // Should never set default bit when remove key
+                        Status  = -EINVAL;
+                        DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(Should never set default bit when remove key)\n"));
+                    }
+                    else
+                    {
+                        KeyIdx = KeyIdx & 0x0fffffff;
+                        if (KeyIdx > 3)
+                        {
+                            Status  = -EINVAL;
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(KeyId[%d] out of range)\n", KeyIdx));
+                        }
+                        else
+                        {
+                            pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+                            pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+                            AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY (id=0x%x, Len=%d-byte)\n", pRemoveKey->KeyIndex, pRemoveKey->Length));
+                        }
+                    }
+                }
+            }
+            kfree(pRemoveKey);
+            break;
+        // New for WPA
+        case OID_802_11_ADD_KEY:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY, Failed!!\n"));
+            }
+            else
+            {
+                RTMPAddKey(pAdapter, pKey);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_CONFIGURATION:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_CONFIGURATION))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Config, wrq->u.data.pointer, wrq->u.data.length);
+                pConfig = &Config;
+
+                if ((pConfig->BeaconPeriod >= 20) && (pConfig->BeaconPeriod <=400))
+                     pAdapter->CommonCfg.BeaconPeriod = (USHORT) pConfig->BeaconPeriod;
+
+                pAdapter->StaActive.AtimWin = (USHORT) pConfig->ATIMWindow;
+                MAP_KHZ_TO_CHANNEL_ID(pConfig->DSConfig, pAdapter->CommonCfg.Channel);
+                //
+				// Save the channel on MlmeAux for CntlOidRTBssidProc used.
+				//
+				pAdapter->MlmeAux.Channel = pAdapter->CommonCfg.Channel;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CONFIGURATION (BeacnPeriod=%ld,AtimW=%ld,Ch=%d)\n",
+                    pConfig->BeaconPeriod, pConfig->ATIMWindow, pAdapter->CommonCfg.Channel));
+                // Config has changed
+                pAdapter->bConfigChanged = TRUE;
+            }
+            break;
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_HT_PHYMODE:
+			if (wrq->u.data.length	!= sizeof(OID_SET_HT_PHYMODE))
+				Status = -EINVAL;
+			else
+			{
+			    POID_SET_HT_PHYMODE	pHTPhyMode = &HT_PhyMode;
+
+				Status = copy_from_user(&HT_PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::pHTPhyMode	(PhyMode = %d,TransmitNo = %d, HtMode =	%d,	ExtOffset =	%d , MCS = %d, BW =	%d,	STBC = %d, SHORTGI = %d) \n",
+				pHTPhyMode->PhyMode, pHTPhyMode->TransmitNo,pHTPhyMode->HtMode,pHTPhyMode->ExtOffset,
+				pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC,	pHTPhyMode->SHORTGI));
+				if (pAdapter->CommonCfg.PhyMode	>= PHY_11ABGN_MIXED)
+					RTMPSetHT(pAdapter,	pHTPhyMode);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_HT_PHYMODE(MCS=%d,BW=%d,SGI=%d,STBC=%d)\n",
+				pAdapter->StaCfg.HTPhyMode.field.MCS, pAdapter->StaCfg.HTPhyMode.field.BW, pAdapter->StaCfg.HTPhyMode.field.ShortGI,
+				pAdapter->StaCfg.HTPhyMode.field.STBC));
+			break;
+#endif // DOT11_N_SUPPORT //
+		case RT_OID_802_11_SET_APSD_SETTING:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				ULONG apsd ;
+				Status = copy_from_user(&apsd, wrq->u.data.pointer,	wrq->u.data.length);
+
+				/*-------------------------------------------------------------------
+				|B31~B7	|	B6~B5	 |	 B4	 |	 B3	 |	B2	 |	B1	 |	   B0		|
+				---------------------------------------------------------------------
+				| Rsvd	| Max SP Len | AC_VO | AC_VI | AC_BK | AC_BE | APSD	Capable	|
+				---------------------------------------------------------------------*/
+				pAdapter->CommonCfg.bAPSDCapable = (apsd & 0x00000001) ? TRUE :	FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BE = ((apsd	& 0x00000002) >> 1)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BK = ((apsd	& 0x00000004) >> 2)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VI = ((apsd	& 0x00000008) >> 3)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VO = ((apsd	& 0x00000010) >> 4)	? TRUE : FALSE;
+				pAdapter->CommonCfg.MaxSPLength	= (UCHAR)((apsd	& 0x00000060) >> 5);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_SETTING (apsd=0x%lx, APSDCap=%d, [BE,BK,VI,VO]=[%d/%d/%d/%d],	MaxSPLen=%d)\n", apsd, pAdapter->CommonCfg.bAPSDCapable,
+					pAdapter->CommonCfg.bAPSDAC_BE,	pAdapter->CommonCfg.bAPSDAC_BK,	pAdapter->CommonCfg.bAPSDAC_VI,	pAdapter->CommonCfg.bAPSDAC_VO,	pAdapter->CommonCfg.MaxSPLength));
+			}
+			break;
+
+		case RT_OID_802_11_SET_APSD_PSM:
+			if (wrq->u.data.length	!= sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				// Driver needs	to notify AP when PSM changes
+				Status = copy_from_user(&pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.pointer, wrq->u.data.length);
+				if (pAdapter->CommonCfg.bAPSDForcePowerSave	!= pAdapter->StaCfg.Psm)
+				{
+					MlmeSetPsmBit(pAdapter,	pAdapter->CommonCfg.bAPSDForcePowerSave);
+					RTMPSendNullFrame(pAdapter,	pAdapter->CommonCfg.TxRate,	TRUE);
+				}
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_PSM (bAPSDForcePowerSave:%d)\n",	pAdapter->CommonCfg.bAPSDForcePowerSave));
+			}
+			break;
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_SET_DLS:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				BOOLEAN	oldvalue = pAdapter->CommonCfg.bDLSCapable;
+				Status = copy_from_user(&pAdapter->CommonCfg.bDLSCapable, wrq->u.data.pointer, wrq->u.data.length);
+				if (oldvalue &&	!pAdapter->CommonCfg.bDLSCapable)
+				{
+					int	i;
+					// tear	down local dls table entry
+					for	(i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+
+					// tear	down peer dls table	entry
+					for	(i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS (=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			}
+			break;
+
+		case RT_OID_802_11_SET_DLS_PARAM:
+			if (wrq->u.data.length	!= sizeof(RT_802_11_DLS_UI))
+				Status = -EINVAL;
+			else
+			{
+				RT_802_11_DLS	Dls;
+
+				NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+				RTMPMoveMemory(&Dls, wrq->u.data.pointer, sizeof(RT_802_11_DLS_UI));
+				MlmeEnqueue(pAdapter,
+							MLME_CNTL_STATE_MACHINE,
+							RT_OID_802_11_SET_DLS_PARAM,
+							sizeof(RT_802_11_DLS),
+							&Dls);
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS_PARAM \n"));
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+		case RT_OID_802_11_SET_WMM:
+			if (wrq->u.data.length	!= sizeof(BOOLEAN))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&pAdapter->CommonCfg.bWmmCapable, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_WMM (=%d)	\n", pAdapter->CommonCfg.bWmmCapable));
+			}
+			break;
+
+		case OID_802_11_DISASSOCIATE:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+			//
+			// Set NdisRadioStateOff to	TRUE, instead of called	MlmeRadioOff.
+			// Later on, NDIS_802_11_BSSID_LIST_EX->NumberOfItems should be	0
+			// when	query OID_802_11_BSSID_LIST.
+			//
+			// TRUE:  NumberOfItems	will set to	0.
+			// FALSE: NumberOfItems	no change.
+			//
+			pAdapter->CommonCfg.NdisRadioStateOff =	TRUE;
+			// Set to immediately send the media disconnect	event
+			pAdapter->MlmeAux.CurrReqIsFromNdis	= TRUE;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DISASSOCIATE	\n"));
+
+			if (INFRA_ON(pAdapter))
+			{
+				if (pAdapter->Mlme.CntlMachine.CurrState !=	CNTL_IDLE)
+				{
+					RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+					DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME	busy, reset	MLME state machine !!!\n"));
+				}
+
+				MlmeEnqueue(pAdapter,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_DISASSOCIATE,
+					0,
+					NULL);
+
+				StateMachineTouched	= TRUE;
+			}
+			break;
+
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_IMME_BA_CAP:
+				if (wrq->u.data.length != sizeof(OID_BACAP_STRUC))
+					Status = -EINVAL;
+				else
+				{
+					OID_BACAP_STRUC Orde ;
+					Status = copy_from_user(&Orde, wrq->u.data.pointer, wrq->u.data.length);
+					if (Orde.Policy > BA_NOTUSE)
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+					}
+					else if (Orde.Policy == BA_NOTUSE)
+					{
+						pAdapter->CommonCfg.BACapability.field.Policy = BA_NOTUSE;
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs= Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+					}
+					else
+					{
+                        pAdapter->CommonCfg.BACapability.field.AutoBA = Orde.AutoBA;
+						pAdapter->CommonCfg.BACapability.field.Policy = IMMED_BA; // we only support immediate BA.
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+
+						if (pAdapter->CommonCfg.BACapability.field.RxBAWinLimit > MAX_RX_REORDERBUF)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = MAX_RX_REORDERBUF;
+
+					}
+
+					pAdapter->CommonCfg.REGBACapability.word = pAdapter->CommonCfg.BACapability.word;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(Orde.AutoBA = %d) (Policy=%d)(ReBAWinLimit=%d)(TxBAWinLimit=%d)(AutoMode=%d)\n",Orde.AutoBA, pAdapter->CommonCfg.BACapability.field.Policy,
+						pAdapter->CommonCfg.BACapability.field.RxBAWinLimit,pAdapter->CommonCfg.BACapability.field.TxBAWinLimit, pAdapter->CommonCfg.BACapability.field.AutoBA));
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(MimoPs = %d)(AmsduEnable = %d) (AmsduSize=%d)(MpduDensity=%d)\n",pAdapter->CommonCfg.DesiredHtPhy.MimoPs, pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable,
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize, pAdapter->CommonCfg.DesiredHtPhy.MpduDensity));
+				}
+
+				break;
+		case RT_OID_802_11_ADD_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, (" Set :: RT_OID_802_11_ADD_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				UCHAR		        index;
+				OID_ADD_BA_ENTRY    BA;
+				MAC_TABLE_ENTRY     *pEntry;
+
+				Status = copy_from_user(&BA, wrq->u.data.pointer, wrq->u.data.length);
+				if (BA.TID > 15)
+				{
+					Status = NDIS_STATUS_INVALID_DATA;
+					break;
+				}
+				else
+				{
+					//BATableInsertEntry
+					//As ad-hoc mode, BA pair is not limited to only BSSID. so add via OID.
+					index = BA.TID;
+					// in ad hoc mode, when adding BA pair, we should insert this entry into MACEntry too
+					pEntry = MacTableLookup(pAdapter, BA.MACAddr);
+					if (!pEntry)
+					{
+						DBGPRINT(RT_DEBUG_TRACE, ("RT_OID_802_11_ADD_IMME_BA. break on no connection.----:%x:%x\n", BA.MACAddr[4], BA.MACAddr[5]));
+						break;
+					}
+					if (BA.IsRecipient == FALSE)
+					{
+					    if (pEntry->bIAmBadAtheros == TRUE)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = 0x10;
+
+						BAOriSessionSetUp(pAdapter, pEntry, index, 0, 100, TRUE);
+					}
+					else
+					{
+						//BATableInsertEntry(pAdapter, pEntry->Aid, BA.MACAddr, 0, 0xffff, BA.TID, BA.nMSDU, BA.IsRecipient);
+					}
+
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_IMME_BA. Rec = %d. Mac = %x:%x:%x:%x:%x:%x . \n",
+						BA.IsRecipient, BA.MACAddr[0], BA.MACAddr[1], BA.MACAddr[2], BA.MACAddr[2]
+						, BA.MACAddr[4], BA.MACAddr[5]));
+				}
+			}
+			break;
+
+		case RT_OID_802_11_TEAR_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				POID_ADD_BA_ENTRY	pBA;
+				MAC_TABLE_ENTRY *pEntry;
+
+				pBA = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+				if (pBA == NULL)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA kmalloc() can't allocate enough memory\n"));
+					Status = NDIS_STATUS_FAILURE;
+				}
+				else
+				{
+					Status = copy_from_user(pBA, wrq->u.data.pointer, wrq->u.data.length);
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA(TID=%d, bAllTid=%d)\n", pBA->TID, pBA->bAllTid));
+
+					if (!pBA->bAllTid && (pBA->TID > NUM_OF_TID))
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+						break;
+					}
+
+					if (pBA->IsRecipient == FALSE)
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						DBGPRINT(RT_DEBUG_TRACE, (" pBA->IsRecipient == FALSE\n"));
+						if (pEntry)
+						{
+							DBGPRINT(RT_DEBUG_TRACE, (" pBA->pEntry\n"));
+							BAOriSessionTearDown(pAdapter, pEntry->Aid, pBA->TID, FALSE, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					else
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						if (pEntry)
+						{
+							BARecSessionTearDown( pAdapter, (UCHAR)pEntry->Aid, pBA->TID, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					kfree(pBA);
+				}
+            }
+            break;
+#endif // DOT11_N_SUPPORT //
+
+        // For WPA_SUPPLICANT to set static wep key
+    	case OID_802_11_ADD_WEP:
+    	    pWepKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+    	    if(pWepKey == NULL)
+            {
+                Status = -ENOMEM;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed!!\n"));
+                break;
+            }
+            Status = copy_from_user(pWepKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (Status)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (length mismatch)!!\n"));
+            }
+            else
+            {
+		        KeyIdx = pWepKey->KeyIndex & 0x0fffffff;
+                // KeyIdx must be 0 ~ 3
+                if (KeyIdx > 4)
+    			{
+                    Status  = -EINVAL;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (KeyIdx must be smaller than 4)!!\n"));
+                }
+                else
+                {
+                    UCHAR CipherAlg = 0;
+                    PUCHAR Key;
+
+                    // set key material and key length
+                    NdisZeroMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, 16);
+                    pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                    NdisMoveMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+
+                    switch(pWepKey->KeyLength)
+                    {
+                        case 5:
+                            CipherAlg = CIPHER_WEP64;
+                            break;
+                        case 13:
+                            CipherAlg = CIPHER_WEP128;
+                            break;
+                        default:
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, only support CIPHER_WEP64(len:5) & CIPHER_WEP128(len:13)!!\n"));
+                            Status = -EINVAL;
+                            break;
+                    }
+                    pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CipherAlg;
+
+                    // Default key for tx (shared key)
+                    if (pWepKey->KeyIndex & 0x80000000)
+                    {
+#ifdef WPA_SUPPLICANT_SUPPORT
+                        // set key material and key length
+                        NdisZeroMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, 16);
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                        NdisMoveMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+                        pAdapter->StaCfg.DesireSharedKeyId = KeyIdx;
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].CipherAlg = CipherAlg;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                        pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+                    }
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+					if ((pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE) &&
+						(pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA))
+					{
+						Key = pWepKey->KeyMaterial;
+
+						// Set Group key material to Asic
+    					AsicAddSharedKeyEntry(pAdapter, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+						// Update WCID attribute table and IVEIV table for this group key table
+						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, NULL);
+
+						STA_PORT_SECURED(pAdapter);
+
+        				// Indicate Connected for GUI
+        				pAdapter->IndicateMediaState = NdisMediaStateConnected;
+					}
+                    else if (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED)
+#endif // WPA_SUPPLICANT_SUPPORT
+                    {
+                        Key = pAdapter->SharedKey[BSS0][KeyIdx].Key;
+
+                        // Set key material and cipherAlg to Asic
+        				AsicAddSharedKeyEntry(pAdapter, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+                        if (pWepKey->KeyIndex & 0x80000000)
+                        {
+                            PMAC_TABLE_ENTRY pEntry = &pAdapter->MacTab.Content[BSSID_WCID];
+                            // Assign group key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, NULL);
+    						// Assign pairwise key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, pEntry);
+                        }
+                    }
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP (id=0x%x, Len=%d-byte), %s\n", pWepKey->KeyIndex, pWepKey->KeyLength, (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED) ? "Port Secured":"Port NOT Secured"));
+				}
+            }
+            kfree(pWepKey);
+            break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+	    case OID_SET_COUNTERMEASURES:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.bBlockAssoc = TRUE;
+                else
+                    // WPA MIC error should block association attempt for 60 seconds
+                    pAdapter->StaCfg.bBlockAssoc = FALSE;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_SET_COUNTERMEASURES bBlockAssoc=%s\n", pAdapter->StaCfg.bBlockAssoc ? "TRUE":"FALSE"));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+			if (wrq->u.data.length != sizeof(UCHAR))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&wpa_supplicant_enable, wrq->u.data.pointer, wrq->u.data.length);
+    			pAdapter->StaCfg.WpaSupplicantUP = wpa_supplicant_enable;
+    			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+			}
+            break;
+        case OID_802_11_DEAUTHENTICATION:
+            if (wrq->u.data.length != sizeof(MLME_DEAUTH_REQ_STRUCT))
+                Status  = -EINVAL;
+            else
+            {
+                MLME_DEAUTH_REQ_STRUCT      *pInfo;
+				MLME_QUEUE_ELEM *MsgElem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+                pInfo = (MLME_DEAUTH_REQ_STRUCT *) MsgElem->Msg;
+                Status = copy_from_user(pInfo, wrq->u.data.pointer, wrq->u.data.length);
+                MlmeDeauthReqAction(pAdapter, MsgElem);
+				kfree(MsgElem);
+
+                if (INFRA_ON(pAdapter))
+                {
+                    LinkDown(pAdapter, FALSE);
+                    pAdapter->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DEAUTHENTICATION (Reason=%d)\n", pInfo->Reason));
+            }
+            break;
+        case OID_802_11_DROP_UNENCRYPTED:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                else
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				NdisAcquireSpinLock(&pAdapter->MacTabLock);
+				pAdapter->MacTab.Content[BSSID_WCID].PortSecured = pAdapter->StaCfg.PortSecured;
+				NdisReleaseSpinLock(&pAdapter->MacTabLock);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DROP_UNENCRYPTED (=%d)\n", enabled));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021xState, wrq->u.data.pointer, wrq->u.data.length);
+		        pAdapter->StaCfg.IEEE8021X = IEEE8021xState;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X (=%d)\n", IEEE8021xState));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X_REQUIRE_KEY:
+			if (wrq->u.data.length != sizeof(BOOLEAN))
+				 Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021x_required_keys, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->StaCfg.IEEE8021x_required_keys = IEEE8021x_required_keys;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X_REQUIRE_KEY (%d)\n", IEEE8021x_required_keys));
+			}
+			break;
+        case OID_802_11_PMKID:
+	        pPmkId = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+	        if(pPmkId == NULL) {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pPmkId, wrq->u.data.pointer, wrq->u.data.length);
+
+	        // check the PMKID information
+	        if (pPmkId->BSSIDInfoCount == 0)
+                NdisZeroMemory(pAdapter->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+	        else
+	        {
+		        PBSSID_INFO	pBssIdInfo;
+		        UINT		BssIdx;
+		        UINT		CachedIdx;
+
+		        for (BssIdx = 0; BssIdx < pPmkId->BSSIDInfoCount; BssIdx++)
+		        {
+			        // point to the indexed BSSID_INFO structure
+			        pBssIdInfo = (PBSSID_INFO) ((PUCHAR) pPmkId + 2 * sizeof(UINT) + BssIdx * sizeof(BSSID_INFO));
+			        // Find the entry in the saved data base.
+			        for (CachedIdx = 0; CachedIdx < pAdapter->StaCfg.SavedPMKNum; CachedIdx++)
+			        {
+				        // compare the BSSID
+				        if (NdisEqualMemory(pBssIdInfo->BSSID, pAdapter->StaCfg.SavedPMK[CachedIdx].BSSID, sizeof(NDIS_802_11_MAC_ADDRESS)))
+					        break;
+			        }
+
+			        // Found, replace it
+			        if (CachedIdx < PMKID_NO)
+			        {
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+				        pAdapter->StaCfg.SavedPMKNum++;
+			        }
+			        // Not found, replace the last one
+			        else
+			        {
+				        // Randomly replace one
+				        CachedIdx = (pBssIdInfo->BSSID[5] % PMKID_NO);
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+			        }
+		        }
+			}
+			if(pPmkId)
+				kfree(pPmkId);
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+#ifdef SNMP_SUPPORT
+		case OID_802_11_SHORTRETRYLIMIT:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ShortRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SHORTRETRYLIMIT (tx_rty_cfg.field.ShortRetryLimit=%d, ShortRetryLimit=%ld)\n", tx_rty_cfg.field.ShortRtyLimit, ShortRetryLimit));
+			}
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT \n"));
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&LongRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT (tx_rty_cfg.field.LongRetryLimit= %d,LongRetryLimit=%ld)\n", tx_rty_cfg.field.LongRtyLimit, LongRetryLimit));
+			}
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE\n"));
+			pKey = kmalloc(wrq->u.data.length, GFP_KERNEL);
+			Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+			//pKey = &WepKey;
+
+			if ( pKey->Length != wrq->u.data.length)
+			{
+				Status = -EINVAL;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE, Failed!!\n"));
+			}
+			KeyIdx = pKey->KeyIndex & 0x0fffffff;
+			DBGPRINT(RT_DEBUG_TRACE,("pKey->KeyIndex =%d, pKey->KeyLength=%d\n", pKey->KeyIndex, pKey->KeyLength));
+
+			// it is a shared key
+			if (KeyIdx > 4)
+				Status = -EINVAL;
+			else
+			{
+				pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, &pKey->KeyMaterial, pKey->KeyLength);
+				if (pKey->KeyIndex & 0x80000000)
+				{
+					// Default key for tx (shared key)
+					pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+				}
+				//RestartAPIsRequired = TRUE;
+			}
+			break;
+
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYID \n"));
+
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+				Status = copy_from_user(&pAdapter->StaCfg.DefaultKeyId, wrq->u.data.pointer, wrq->u.data.length);
+
+			break;
+
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CURRENTCHANNEL \n"));
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ctmp, wrq->u.data.pointer, wrq->u.data.length);
+				sprintf(&ctmp,"%d", ctmp);
+				Set_Channel_Proc(pAdapter, &ctmp);
+			}
+			break;
+#endif
+
+
+
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+
+
+    return Status;
+}
+
+INT RTMPQueryInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_BSSID_LIST_EX           *pBssidList = NULL;
+    PNDIS_WLAN_BSSID_EX                 pBss;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_CONFIGURATION           *pConfiguration = NULL;
+    RT_802_11_LINK_STATUS               *pLinkStatus = NULL;
+    RT_802_11_STA_CONFIG                *pStaConfig = NULL;
+    NDIS_802_11_STATISTICS              *pStatistics = NULL;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    RT_802_11_PREAMBLE                  PreamType;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_MEDIA_STATE                    MediaState;
+    ULONG                               BssBufSize, ulInfo=0, NetworkTypeList[4], apsd = 0;
+    USHORT                              BssLen = 0;
+    PUCHAR                              pBuf = NULL, pPtr;
+    INT                                 Status = NDIS_STATUS_SUCCESS;
+    UINT                                we_version_compiled;
+    UCHAR                               i, Padding = 0;
+    BOOLEAN                             RadioState;
+	UCHAR	driverVersion[8];
+    OID_SET_HT_PHYMODE			        *pHTPhyMode = NULL;
+
+
+#ifdef SNMP_SUPPORT
+	//for snmp, kathy
+	DefaultKeyIdxValue			*pKeyIdxValue;
+	INT							valueLen;
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						tmp[64];
+#endif //SNMP
+
+    switch(cmd)
+    {
+        case RT_OID_DEVICE_NAME:
+            wrq->u.data.length = sizeof(STA_NIC_DEVICE_NAME);
+            Status = copy_to_user(wrq->u.data.pointer, STA_NIC_DEVICE_NAME, wrq->u.data.length);
+            break;
+        case RT_OID_VERSION_INFO:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_VERSION_INFO \n"));
+			wrq->u.data.length = 8*sizeof(UCHAR);
+			sprintf(&driverVersion[0], "%s", STA_DRIVER_VERSION);
+			driverVersion[7] = '\0';
+			if (copy_to_user(wrq->u.data.pointer, &driverVersion, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+#ifdef RALINK_ATE
+		case RT_QUERY_ATE_TXDONE_COUNT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_QUERY_ATE_TXDONE_COUNT \n"));
+			wrq->u.data.length = sizeof(UINT32);
+			if (copy_to_user(wrq->u.data.pointer, &pAdapter->ate.TxDoneCount, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+			break;
+#endif // RALINK_ATE //
+        case OID_802_11_BSSID_LIST:
+            if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+            	/*
+            	 * Still scanning, indicate the caller should try again.
+            	 */
+            	DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (Still scanning)\n"));
+				return -EAGAIN;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (%d BSS returned)\n",pAdapter->ScanTab.BssNr));
+			pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+            // Claculate total buffer size required
+            BssBufSize = sizeof(ULONG);
+
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                // Align pointer to 4 bytes boundary.
+                //Padding = 4 - (pAdapter->ScanTab.BssEntry[i].VarIELen & 0x0003);
+                //if (Padding == 4)
+                //    Padding = 0;
+                BssBufSize += (sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+            }
+
+            // For safety issue, we add 256 bytes just in case
+            BssBufSize += 256;
+            // Allocate the same size as passed from higher layer
+            pBuf = kmalloc(BssBufSize, MEM_ALLOC_FLAG);
+            if(pBuf == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            // Init 802_11_BSSID_LIST_EX structure
+            NdisZeroMemory(pBuf, BssBufSize);
+            pBssidList = (PNDIS_802_11_BSSID_LIST_EX) pBuf;
+            pBssidList->NumberOfItems = pAdapter->ScanTab.BssNr;
+
+            // Calculate total buffer length
+            BssLen = 4; // Consist of NumberOfItems
+            // Point to start of NDIS_WLAN_BSSID_EX
+            // pPtr = pBuf + sizeof(ULONG);
+            pPtr = (PUCHAR) &pBssidList->Bssid[0];
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                pBss = (PNDIS_WLAN_BSSID_EX) pPtr;
+                NdisMoveMemory(&pBss->MacAddress, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+                if ((pAdapter->ScanTab.BssEntry[i].Hidden == 1) && (pAdapter->StaCfg.bShowHiddenSSID == FALSE))
+                {
+                    //
+					// We must return this SSID during 4way handshaking, otherwise Aegis will failed to parse WPA infomation
+					// and then failed to send EAPOl farame.
+					//
+					if ((pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) && (pAdapter->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED))
+					{
+						pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+						NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+					}
+					else
+                    	pBss->Ssid.SsidLength = 0;
+                }
+                else
+                {
+                    pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+                    NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+                }
+                pBss->Privacy = pAdapter->ScanTab.BssEntry[i].Privacy;
+                pBss->Rssi = pAdapter->ScanTab.BssEntry[i].Rssi - pAdapter->BbpRssiToDbmDelta;
+                pBss->NetworkTypeInUse = NetworkTypeInUseSanity(&pAdapter->ScanTab.BssEntry[i]);
+                pBss->Configuration.Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pBss->Configuration.BeaconPeriod = pAdapter->ScanTab.BssEntry[i].BeaconPeriod;
+                pBss->Configuration.ATIMWindow = pAdapter->ScanTab.BssEntry[i].AtimWin;
+
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->ScanTab.BssEntry[i].Channel, pBss->Configuration.DSConfig);
+
+                if (pAdapter->ScanTab.BssEntry[i].BssType == BSS_INFRA)
+                    pBss->InfrastructureMode = Ndis802_11Infrastructure;
+                else
+                    pBss->InfrastructureMode = Ndis802_11IBSS;
+
+                NdisMoveMemory(pBss->SupportedRates, pAdapter->ScanTab.BssEntry[i].SupRate, pAdapter->ScanTab.BssEntry[i].SupRateLen);
+                NdisMoveMemory(pBss->SupportedRates + pAdapter->ScanTab.BssEntry[i].SupRateLen,
+                               pAdapter->ScanTab.BssEntry[i].ExtRate,
+                               pAdapter->ScanTab.BssEntry[i].ExtRateLen);
+
+                if (pAdapter->ScanTab.BssEntry[i].VarIELen == 0)
+                {
+                    pBss->IELength = sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                }
+                else
+                {
+                    pBss->IELength = (ULONG)(sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    NdisMoveMemory(pBss->IEs + sizeof(NDIS_802_11_FIXED_IEs), pAdapter->ScanTab.BssEntry[i].VarIEs, pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr += pAdapter->ScanTab.BssEntry[i].VarIELen;
+                }
+                pBss->Length = (ULONG)(sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+
+#if WIRELESS_EXT < 17
+                if ((BssLen + pBss->Length) < wrq->u.data.length)
+                BssLen += pBss->Length;
+                else
+                {
+                    pBssidList->NumberOfItems = i;
+                    break;
+                }
+#else
+                BssLen += pBss->Length;
+#endif
+            }
+
+#if WIRELESS_EXT < 17
+            wrq->u.data.length = BssLen;
+#else
+            if (BssLen > wrq->u.data.length)
+            {
+                kfree(pBssidList);
+                return -E2BIG;
+            }
+            else
+                wrq->u.data.length = BssLen;
+#endif
+            Status = copy_to_user(wrq->u.data.pointer, pBssidList, BssLen);
+            kfree(pBssidList);
+            break;
+        case OID_802_3_CURRENT_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+            break;
+        case OID_GEN_MEDIA_CONNECT_STATUS:
+            if (pAdapter->IndicateMediaState == NdisMediaStateConnected)
+                MediaState = NdisMediaStateConnected;
+            else
+                MediaState = NdisMediaStateDisconnected;
+
+            wrq->u.data.length = sizeof(NDIS_MEDIA_STATE);
+            Status = copy_to_user(wrq->u.data.pointer, &MediaState, wrq->u.data.length);
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				Status = NDIS_STATUS_RESOURCES;
+				break;
+			}
+#endif // RALINK_ATE //
+            if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+            {
+                Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Bssid, sizeof(NDIS_802_11_MAC_ADDRESS));
+
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID(=EMPTY)\n"));
+                Status = -ENOTCONN;
+            }
+            break;
+        case OID_802_11_SSID:
+			NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+			NdisZeroMemory(Ssid.Ssid, MAX_LEN_OF_SSID);
+            Ssid.SsidLength = pAdapter->CommonCfg.SsidLen;
+			memcpy(Ssid.Ssid, pAdapter->CommonCfg.Ssid,	Ssid.SsidLength);
+            wrq->u.data.length = sizeof(NDIS_802_11_SSID);
+            Status = copy_to_user(wrq->u.data.pointer, &Ssid, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SSID (Len=%d, ssid=%s)\n", Ssid.SsidLength,Ssid.Ssid));
+            break;
+        case RT_OID_802_11_QUERY_LINK_STATUS:
+            pLinkStatus = (RT_802_11_LINK_STATUS *) kmalloc(sizeof(RT_802_11_LINK_STATUS), MEM_ALLOC_FLAG);
+            if (pLinkStatus)
+            {
+                pLinkStatus->CurrTxRate = RateIdTo500Kbps[pAdapter->CommonCfg.TxRate];   // unit : 500 kbps
+                pLinkStatus->ChannelQuality = pAdapter->Mlme.ChannelQuality;
+                pLinkStatus->RxByteCount = pAdapter->RalinkCounters.ReceivedByteCount;
+                pLinkStatus->TxByteCount = pAdapter->RalinkCounters.TransmittedByteCount;
+        		pLinkStatus->CentralChannel = pAdapter->CommonCfg.CentralChannel;
+                wrq->u.data.length = sizeof(RT_802_11_LINK_STATUS);
+                Status = copy_to_user(wrq->u.data.pointer, pLinkStatus, wrq->u.data.length);
+                kfree(pLinkStatus);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS\n"));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_CONFIGURATION:
+            pConfiguration = (NDIS_802_11_CONFIGURATION *) kmalloc(sizeof(NDIS_802_11_CONFIGURATION), MEM_ALLOC_FLAG);
+            if (pConfiguration)
+            {
+                pConfiguration->Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pConfiguration->BeaconPeriod = pAdapter->CommonCfg.BeaconPeriod;
+                pConfiguration->ATIMWindow = pAdapter->StaActive.AtimWin;
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->CommonCfg.Channel, pConfiguration->DSConfig);
+                wrq->u.data.length = sizeof(NDIS_802_11_CONFIGURATION);
+                Status = copy_to_user(wrq->u.data.pointer, pConfiguration, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(BeaconPeriod=%ld,AtimW=%ld,Channel=%d) \n",
+                                        pConfiguration->BeaconPeriod, pConfiguration->ATIMWindow, pAdapter->CommonCfg.Channel));
+				kfree(pConfiguration);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+		case RT_OID_802_11_SNR_0:
+			if ((pAdapter->StaCfg.LastSNR0 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR0) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_SNR_0(0x=%lx)\n", ulInfo));
+			}
+            else
+			    Status = -EFAULT;
+			break;
+		case RT_OID_802_11_SNR_1:
+			if ((pAdapter->Antenna.field.RxPath	> 1) &&
+                (pAdapter->StaCfg.LastSNR1 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR1) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(0x=%lx)\n",ulInfo));
+			}
+			else
+				Status = -EFAULT;
+            DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(pAdapter->StaCfg.LastSNR1=%d)\n",pAdapter->StaCfg.LastSNR1));
+			break;
+        case OID_802_11_RSSI_TRIGGER:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0 - pAdapter->BbpRssiToDbmDelta;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RSSI_TRIGGER(=%ld)\n", ulInfo));
+            break;
+		case OID_802_11_RSSI:
+        case RT_OID_802_11_RSSI:
+			ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+		case RT_OID_802_11_RSSI_1:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi1;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case RT_OID_802_11_RSSI_2:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi2;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case OID_802_11_STATISTICS:
+            pStatistics = (NDIS_802_11_STATISTICS *) kmalloc(sizeof(NDIS_802_11_STATISTICS), MEM_ALLOC_FLAG);
+            if (pStatistics)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS \n"));
+                // add the most up-to-date h/w raw counters into software counters
+			    NICUpdateRawCounters(pAdapter);
+
+                // Sanity check for calculation of sucessful count
+                if (pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart < pAdapter->WlanCounters.RetryCount.QuadPart)
+                    pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+
+                pStatistics->TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart;
+                pStatistics->MulticastTransmittedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastTransmittedFrameCount.QuadPart;
+                pStatistics->FailedCount.QuadPart = pAdapter->WlanCounters.FailedCount.QuadPart;
+                pStatistics->RetryCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+                pStatistics->MultipleRetryCount.QuadPart = pAdapter->WlanCounters.MultipleRetryCount.QuadPart;
+                pStatistics->RTSSuccessCount.QuadPart = pAdapter->WlanCounters.RTSSuccessCount.QuadPart;
+                pStatistics->RTSFailureCount.QuadPart = pAdapter->WlanCounters.RTSFailureCount.QuadPart;
+                pStatistics->ACKFailureCount.QuadPart = pAdapter->WlanCounters.ACKFailureCount.QuadPart;
+                pStatistics->FrameDuplicateCount.QuadPart = pAdapter->WlanCounters.FrameDuplicateCount.QuadPart;
+                pStatistics->ReceivedFragmentCount.QuadPart = pAdapter->WlanCounters.ReceivedFragmentCount.QuadPart;
+                pStatistics->MulticastReceivedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastReceivedFrameCount.QuadPart;
+#ifdef DBG
+                pStatistics->FCSErrorCount = pAdapter->RalinkCounters.RealFcsErrCount;
+#else
+                pStatistics->FCSErrorCount.QuadPart = pAdapter->WlanCounters.FCSErrorCount.QuadPart;
+                pStatistics->FrameDuplicateCount.u.LowPart = pAdapter->WlanCounters.FrameDuplicateCount.u.LowPart / 100;
+#endif
+                wrq->u.data.length = sizeof(NDIS_802_11_STATISTICS);
+                Status = copy_to_user(wrq->u.data.pointer, pStatistics, wrq->u.data.length);
+                kfree(pStatistics);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_GEN_RCV_OK:
+            ulInfo = pAdapter->Counters8023.GoodReceives;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case OID_GEN_RCV_NO_BUFFER:
+            ulInfo = pAdapter->Counters8023.RxNoBuffer;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            ulInfo = (ULONG)pAdapter->CommonCfg.PhyMode;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PHY_MODE (=%ld)\n", ulInfo));
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            pStaConfig = (RT_802_11_STA_CONFIG *) kmalloc(sizeof(RT_802_11_STA_CONFIG), MEM_ALLOC_FLAG);
+            if (pStaConfig)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG\n"));
+                pStaConfig->EnableTxBurst = pAdapter->CommonCfg.bEnableTxBurst;
+                pStaConfig->EnableTurboRate = 0;
+                pStaConfig->UseBGProtection = pAdapter->CommonCfg.UseBGProtection;
+                pStaConfig->UseShortSlotTime = pAdapter->CommonCfg.bUseShortSlotTime;
+                //pStaConfig->AdhocMode = pAdapter->StaCfg.AdhocMode;
+                pStaConfig->HwRadioStatus = (pAdapter->StaCfg.bHwRadio == TRUE) ? 1 : 0;
+                pStaConfig->Rsv1 = 0;
+                pStaConfig->SystemErrorBitmap = pAdapter->SystemErrorBitmap;
+                wrq->u.data.length = sizeof(RT_802_11_STA_CONFIG);
+                Status = copy_to_user(wrq->u.data.pointer, pStaConfig, wrq->u.data.length);
+                kfree(pStaConfig);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            RtsThresh = pAdapter->CommonCfg.RtsThreshold;
+            wrq->u.data.length = sizeof(RtsThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &RtsThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RTS_THRESHOLD(=%ld)\n", RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            FragThresh = pAdapter->CommonCfg.FragmentThreshold;
+            if (pAdapter->CommonCfg.bUseZeroToDisableFragment == TRUE)
+                FragThresh = 0;
+            wrq->u.data.length = sizeof(FragThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &FragThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_FRAGMENTATION_THRESHOLD(=%ld)\n", FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            PowerMode = pAdapter->StaCfg.WindowsPowerMode;
+            wrq->u.data.length = sizeof(PowerMode);
+            Status = copy_to_user(wrq->u.data.pointer, &PowerMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_POWER_MODE(=%d)\n", PowerMode));
+            break;
+        case RT_OID_802_11_RADIO:
+            RadioState = (BOOLEAN) pAdapter->StaCfg.bSwRadio;
+            wrq->u.data.length = sizeof(RadioState);
+            Status = copy_to_user(wrq->u.data.pointer, &RadioState, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_RADIO (=%d)\n", RadioState));
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                BssType = Ndis802_11IBSS;
+            else if (pAdapter->StaCfg.BssType == BSS_INFRA)
+                BssType = Ndis802_11Infrastructure;
+            else if (pAdapter->StaCfg.BssType == BSS_MONITOR)
+                BssType = Ndis802_11Monitor;
+            else
+                BssType = Ndis802_11AutoUnknown;
+
+            wrq->u.data.length = sizeof(BssType);
+            Status = copy_to_user(wrq->u.data.pointer, &BssType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_INFRASTRUCTURE_MODE(=%d)\n", BssType));
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            PreamType = pAdapter->CommonCfg.TxPreamble;
+            wrq->u.data.length = sizeof(PreamType);
+            Status = copy_to_user(wrq->u.data.pointer, &PreamType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PREAMBLE(=%d)\n", PreamType));
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            AuthMode = pAdapter->StaCfg.AuthMode;
+            wrq->u.data.length = sizeof(AuthMode);
+            Status = copy_to_user(wrq->u.data.pointer, &AuthMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_AUTHENTICATION_MODE(=%d)\n", AuthMode));
+            break;
+        case OID_802_11_WEP_STATUS:
+            WepStatus = pAdapter->StaCfg.WepStatus;
+            wrq->u.data.length = sizeof(WepStatus);
+            Status = copy_to_user(wrq->u.data.pointer, &WepStatus, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEP_STATUS(=%d)\n", WepStatus));
+            break;
+        case OID_802_11_TX_POWER_LEVEL:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPower, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_TX_POWER_LEVEL %x\n",pAdapter->CommonCfg.TxPower));
+			break;
+        case RT_OID_802_11_TX_POWER_LEVEL_1:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPowerPercentage, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			break;
+        case OID_802_11_NETWORK_TYPES_SUPPORTED:
+			if ((pAdapter->RfIcType	== RFIC_2850) || (pAdapter->RfIcType ==	RFIC_2750))
+			{
+				NetworkTypeList[0] = 3;                 // NumberOfItems = 3
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+				NetworkTypeList[3] = Ndis802_11OFDM5;   // NetworkType[3] = 11a
+                wrq->u.data.length = 16;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			else
+			{
+				NetworkTypeList[0] = 2;                 // NumberOfItems = 2
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+			    wrq->u.data.length = 12;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_NETWORK_TYPES_SUPPORTED\n"));
+				break;
+	    case OID_802_11_NETWORK_TYPE_IN_USE:
+            wrq->u.data.length = sizeof(ULONG);
+			if (pAdapter->CommonCfg.PhyMode == PHY_11A)
+				ulInfo = Ndis802_11OFDM5;
+			else if ((pAdapter->CommonCfg.PhyMode == PHY_11BG_MIXED) || (pAdapter->CommonCfg.PhyMode == PHY_11G))
+				ulInfo = Ndis802_11OFDM24;
+			else
+				ulInfo = Ndis802_11DS;
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+        case RT_OID_802_11_QUERY_LAST_RX_RATE:
+            ulInfo = (ULONG)pAdapter->LastRxRate;
+            wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_RX_RATE (=%ld)\n", ulInfo));
+			break;
+		case RT_OID_802_11_QUERY_LAST_TX_RATE:
+			//ulInfo = (ULONG)pAdapter->LastTxRate;
+			ulInfo = (ULONG)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_TX_RATE (=%lx)\n", ulInfo));
+			break;
+        case RT_OID_802_11_QUERY_EEPROM_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->EepromVersion, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_QUERY_FIRMWARE_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->FirmwareVersion, wrq->u.data.length);
+			break;
+	    case RT_OID_802_11_QUERY_NOISE_LEVEL:
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->BbpWriteLatch[66], wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_NOISE_LEVEL (=%d)\n", pAdapter->BbpWriteLatch[66]));
+			break;
+	    case RT_OID_802_11_EXTRA_INFO:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->ExtraInfo, wrq->u.data.length);
+	        DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_EXTRA_INFO (=%ld)\n", pAdapter->ExtraInfo));
+	        break;
+	    case RT_OID_WE_VERSION_COMPILED:
+	        wrq->u.data.length = sizeof(UINT);
+	        we_version_compiled = WIRELESS_EXT;
+	        Status = copy_to_user(wrq->u.data.pointer, &we_version_compiled, wrq->u.data.length);
+	        break;
+		case RT_OID_802_11_QUERY_APSD_SETTING:
+			apsd = (pAdapter->CommonCfg.bAPSDCapable | (pAdapter->CommonCfg.bAPSDAC_BE << 1) | (pAdapter->CommonCfg.bAPSDAC_BK << 2)
+				| (pAdapter->CommonCfg.bAPSDAC_VI << 3)	| (pAdapter->CommonCfg.bAPSDAC_VO << 4)	| (pAdapter->CommonCfg.MaxSPLength << 5));
+
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &apsd, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_SETTING (=0x%lx,APSDCap=%d,AC_BE=%d,AC_BK=%d,AC_VI=%d,AC_VO=%d,MAXSPLen=%d)\n",
+				apsd,pAdapter->CommonCfg.bAPSDCapable,pAdapter->CommonCfg.bAPSDAC_BE,pAdapter->CommonCfg.bAPSDAC_BK,pAdapter->CommonCfg.bAPSDAC_VI,pAdapter->CommonCfg.bAPSDAC_VO,pAdapter->CommonCfg.MaxSPLength));
+			break;
+		case RT_OID_802_11_QUERY_APSD_PSM:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_PSM (=%d)\n", pAdapter->CommonCfg.bAPSDForcePowerSave));
+			break;
+		case RT_OID_802_11_QUERY_WMM:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bWmmCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_WMM (=%d)\n",	pAdapter->CommonCfg.bWmmCapable));
+			break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+        case RT_OID_NEW_DRIVER:
+            {
+                UCHAR enabled = 1;
+    	        wrq->u.data.length = sizeof(UCHAR);
+    	        Status = copy_to_user(wrq->u.data.pointer, &enabled, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_NEW_DRIVER (=%d)\n", enabled));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+	        wrq->u.data.length = sizeof(UCHAR);
+	        Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.WpaSupplicantUP, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+        case RT_OID_DRIVER_DEVICE_NAME:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_DRIVER_DEVICE_NAME \n"));
+			wrq->u.data.length = 16;
+			if (copy_to_user(wrq->u.data.pointer, pAdapter->StaCfg.dev_name, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+            break;
+        case RT_OID_802_11_QUERY_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.STBC;
+
+    			pHTPhyMode->ExtOffset = ((pAdapter->CommonCfg.CentralChannel < pAdapter->CommonCfg.Channel) ? (EXTCHA_BELOW) : (EXTCHA_ABOVE));
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_COUNTRY_REGION:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_COUNTRY_REGION \n"));
+			wrq->u.data.length = sizeof(ulInfo);
+            ulInfo = pAdapter->CommonCfg.CountryRegionForABand;
+            ulInfo = (ulInfo << 8)|(pAdapter->CommonCfg.CountryRegion);
+			if (copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_QUERY_DAT_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.HTMODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->StaCfg.DesiredTransmitSetting.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.STBC;
+
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_QUERY_MULTIPLE_CARD_SUPPORT:
+			wrq->u.data.length = sizeof(UCHAR);
+            i = 0;
+#ifdef MULTIPLE_CARD_SUPPORT
+            i = 1;
+#endif // MULTIPLE_CARD_SUPPORT //
+			if (copy_to_user(wrq->u.data.pointer, &i, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_QUERY_MULTIPLE_CARD_SUPPORT(=%d) \n", i));
+            break;
+#ifdef SNMP_SUPPORT
+		case RT_OID_802_11_MAC_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREROUI:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREROUI \n"));
+			wrq->u.data.length = ManufacturerOUI_LEN;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTURERNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTURERNAME \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_RESOURCETYPEIDNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_RESOURCETYPEIDNAME \n"));
+			wrq->u.data.length = strlen(ResourceTypeIdName);
+			Status = copy_to_user(wrq->u.data.pointer, ResourceTypeIdName, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED \n"));
+			ulInfo = 1; // 1 is support wep else 2 is not support.
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_POWERMANAGEMENTMODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_POWERMANAGEMENTMODE \n"));
+			if (pAdapter->StaCfg.Psm == PSMP_ACTION)
+				ulInfo = 1; // 1 is power active else 2 is power save.
+			else
+				ulInfo = 2;
+
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEPDEFAULTKEYVALUE \n"));
+			//KeyIdxValue.KeyIdx = pAd->PortCfg.MBSSID[pAd->IoctlIF].DefaultKeyId;
+			pKeyIdxValue = wrq->u.data.pointer;
+			DBGPRINT(RT_DEBUG_TRACE,("KeyIdxValue.KeyIdx = %d, \n",pKeyIdxValue->KeyIdx));
+			valueLen = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+			NdisMoveMemory(pKeyIdxValue->Value,
+						   &pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key,
+						   valueLen);
+			pKeyIdxValue->Value[valueLen]='\0';
+
+			wrq->u.data.length = sizeof(DefaultKeyIdxValue);
+
+			Status = copy_to_user(wrq->u.data.pointer, pKeyIdxValue, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE,("DefaultKeyId = %d, total len = %d, str len=%d, KeyValue= %02x %02x %02x %02x \n", pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+			pAdapter->SharedKey[BSS0][0].Key[0],
+			pAdapter->SharedKey[BSS0][1].Key[0],
+			pAdapter->SharedKey[BSS0][2].Key[0],
+			pAdapter->SharedKey[BSS0][3].Key[0]));
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPDEFAULTKEYID \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("DefaultKeyId =%d \n", pAdapter->StaCfg.DefaultKeyId));
+			break;
+
+		case RT_OID_802_11_WEPKEYMAPPINGLENGTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPKEYMAPPINGLENGTH \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer,
+									&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+									wrq->u.data.length);
+			break;
+
+		case OID_802_11_SHORTRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SHORTRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			ShortRetryLimit = tx_rty_cfg.field.ShortRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("ShortRetryLimit =%ld,  tx_rty_cfg.field.ShortRetryLimit=%d\n", ShortRetryLimit, tx_rty_cfg.field.ShortRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &ShortRetryLimit, wrq->u.data.length);
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_LONGRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			LongRetryLimit = tx_rty_cfg.field.LongRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("LongRetryLimit =%ld,  tx_rty_cfg.field.LongRtyLimit=%d\n", LongRetryLimit, tx_rty_cfg.field.LongRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &LongRetryLimit, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRODUCTID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRODUCTID \n"));
+
+#ifdef RT2870
+			sprintf(tmp, "%04x %04x\n", ((POS_COOKIE)pAdapter->OS_Cookie)->pUsb_Dev->descriptor.idVendor ,((POS_COOKIE)pAdapter->OS_Cookie)->pUsb_Dev->descriptor.idProduct);
+
+#endif // RT2870 //
+			wrq->u.data.length = strlen(tmp);
+			Status = copy_to_user(wrq->u.data.pointer, tmp, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREID \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CURRENTCHANNEL \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			DBGPRINT(RT_DEBUG_TRACE, ("sizeof UCHAR=%d, channel=%d \n", sizeof(UCHAR), pAdapter->CommonCfg.Channel));
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Channel, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+#endif //SNMP_SUPPORT
+
+		case OID_802_11_BUILD_CHANNEL_EX:
+			{
+				UCHAR value;
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BUILD_CHANNEL_EX \n"));
+				wrq->u.data.length = sizeof(UCHAR);
+#ifdef EXT_BUILD_CHANNEL_LIST
+				DBGPRINT(RT_DEBUG_TRACE, ("Support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 1;
+#else
+				DBGPRINT(RT_DEBUG_TRACE, ("Doesn't support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 0;
+#endif // EXT_BUILD_CHANNEL_LIST //
+				Status = copy_to_user(wrq->u.data.pointer, &value, 1);
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			}
+			break;
+
+		case OID_802_11_GET_CH_LIST:
+			{
+				PRT_CHANNEL_LIST_INFO pChListBuf;
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CH_LIST \n"));
+				if (pAdapter->ChannelListNum == 0)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf = (RT_CHANNEL_LIST_INFO *) kmalloc(sizeof(RT_CHANNEL_LIST_INFO), MEM_ALLOC_FLAG);
+				if (pChListBuf == NULL)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf->ChannelListNum = pAdapter->ChannelListNum;
+				for (i = 0; i < pChListBuf->ChannelListNum; i++)
+					pChListBuf->ChannelList[i] = pAdapter->ChannelList[i].Channel;
+
+				wrq->u.data.length = sizeof(RT_CHANNEL_LIST_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pChListBuf, sizeof(RT_CHANNEL_LIST_INFO));
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+
+				if (pChListBuf)
+					kfree(pChListBuf);
+			}
+			break;
+
+		case OID_802_11_GET_COUNTRY_CODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_COUNTRY_CODE \n"));
+			wrq->u.data.length = 2;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.CountryCode, 2);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+		case OID_802_11_GET_CHANNEL_GEOGRAPHY:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CHANNEL_GEOGRAPHY \n"));
+			wrq->u.data.length = 1;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Geography, 1);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_QUERY_DLS:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bDLSCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS(=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			break;
+
+		case RT_OID_802_11_QUERY_DLS_PARAM:
+			{
+				PRT_802_11_DLS_INFO	pDlsInfo = kmalloc(sizeof(RT_802_11_DLS_INFO), GFP_ATOMIC);
+				if (pDlsInfo == NULL)
+					break;
+
+				for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+				{
+					RTMPMoveMemory(&pDlsInfo->Entry[i], &pAdapter->StaCfg.DLSEntry[i], sizeof(RT_802_11_DLS_UI));
+				}
+
+				pDlsInfo->num = MAX_NUM_OF_DLS_ENTRY;
+				wrq->u.data.length = sizeof(RT_802_11_DLS_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pDlsInfo, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS_PARAM\n"));
+
+				if (pDlsInfo)
+					kfree(pDlsInfo);
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+    return Status;
+}
+
+INT rt28xx_sta_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT					cmd)
+{
+	POS_COOKIE			pObj;
+	VIRTUAL_ADAPTER		*pVirtualAd = NULL;
+	RTMP_ADAPTER        *pAd = NULL;
+	struct iwreq        *wrq = (struct iwreq *) rq;
+	BOOLEAN				StateMachineTouched = FALSE;
+	INT					Status = NDIS_STATUS_SUCCESS;
+	USHORT				subcmd;
+
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		pAd = net_dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = net_dev->ml_priv;
+		pAd = pVirtualAd->RtmpDev->ml_priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	    if (wrq->u.data.pointer == NULL)
+	    {
+		    return Status;
+	    }
+
+	    if (strstr(wrq->u.data.pointer, "OpMode") == NULL)
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		    return -ENETDOWN;
+        }
+    }
+
+	{	// determine this ioctl command is comming from which interface.
+		pObj->ioctl_if_type = INT_MAIN;
+		pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	switch(cmd)
+	{
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+		case RTPRIV_IOCTL_ATE:
+			{
+				RtmpDoAte(pAd, wrq);
+			}
+			break;
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+        case SIOCGIFHWADDR:
+			DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIFHWADDR\n"));
+			memcpy(wrq->u.name, pAd->CurrentAddress, ETH_ALEN);
+			break;
+		case SIOCGIWNAME:
+        {
+        	char *name=&wrq->u.name[0];
+        	rt_ioctl_giwname(net_dev, NULL, name, NULL);
+			break;
+		}
+		case SIOCGIWESSID:  //Get ESSID
+        {
+        	struct iw_point *essid=&wrq->u.essid;
+        	rt_ioctl_giwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWESSID:  //Set ESSID
+        {
+        	struct iw_point	*essid=&wrq->u.essid;
+        	rt_ioctl_siwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWNWID:   // set network id (the cell)
+		case SIOCGIWNWID:   // get network id
+			Status = -EOPNOTSUPP;
+			break;
+		case SIOCSIWFREQ:   //set channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_siwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCGIWFREQ:   // get channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_giwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCSIWNICKN: //set node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_siwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWNICKN: //get node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_giwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWRATE:   //get default bit rate (bps)
+		    rt_ioctl_giwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+	    case SIOCSIWRATE:  //set default bit rate (bps)
+	        rt_ioctl_siwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+        case SIOCGIWRTS:  // get RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_giwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCSIWRTS:  //set RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_siwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCGIWFRAG:  //get fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_giwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCSIWFRAG:  //set fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_siwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCGIWENCODE:  //get encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_giwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+        case SIOCSIWENCODE:  //set encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_siwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+		case SIOCGIWAP:     //get access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_giwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+	    case SIOCSIWAP:  //set access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_siwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+		case SIOCGIWMODE:   //get operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_giwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCSIWMODE:   //set operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_siwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCGIWSENS:   //get sensitivity (dBm)
+		case SIOCSIWSENS:	//set sensitivity (dBm)
+		case SIOCGIWPOWER:  //get Power Management settings
+		case SIOCSIWPOWER:  //set Power Management settings
+		case SIOCGIWTXPOW:  //get transmit power (dBm)
+		case SIOCSIWTXPOW:  //set transmit power (dBm)
+		case SIOCGIWRANGE:	//Get range of parameters
+		case SIOCGIWRETRY:	//get retry limits and lifetime
+		case SIOCSIWRETRY:	//set retry limits and lifetime
+			Status = -EOPNOTSUPP;
+			break;
+		case RT_PRIV_IOCTL:
+			subcmd = wrq->u.data.flags;
+			if( subcmd & OID_GET_SET_TOGGLE)
+				Status = RTMPSetInformation(pAd, rq, subcmd);
+			else
+				Status = RTMPQueryInformation(pAd, rq, subcmd);
+			break;
+		case SIOCGIWPRIV:
+			if (wrq->u.data.pointer)
+			{
+				if ( access_ok(VERIFY_WRITE, wrq->u.data.pointer, sizeof(privtab)) != TRUE)
+					break;
+				wrq->u.data.length = sizeof(privtab) / sizeof(privtab[0]);
+				if (copy_to_user(wrq->u.data.pointer, privtab, sizeof(privtab)))
+					Status = -EFAULT;
+			}
+			break;
+		case RTPRIV_IOCTL_SET:
+			if(access_ok(VERIFY_READ, wrq->u.data.pointer, wrq->u.data.length) != TRUE)
+				break;
+			rt_ioctl_setparam(net_dev, NULL, NULL, wrq->u.data.pointer);
+			break;
+		case RTPRIV_IOCTL_GSITESURVEY:
+			RTMPIoctlGetSiteSurvey(pAd, wrq);
+		    break;
+#ifdef DBG
+		case RTPRIV_IOCTL_MAC:
+			RTMPIoctlMAC(pAd, wrq);
+			break;
+		case RTPRIV_IOCTL_E2P:
+			RTMPIoctlE2PROM(pAd, wrq);
+			break;
+#endif // DBG //
+        case SIOCETHTOOL:
+                break;
+		default:
+			DBGPRINT(RT_DEBUG_ERROR, ("IOCTL::unknown IOCTL's cmd = 0x%08x\n", cmd));
+			Status = -EOPNOTSUPP;
+			break;
+	}
+
+    if(StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAd);
+
+	return Status;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set SSID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    NDIS_802_11_SSID                    Ssid, *pSsid=NULL;
+    BOOLEAN                             StateMachineTouched = FALSE;
+    int                                 success = TRUE;
+
+    if( strlen(arg) <= MAX_LEN_OF_SSID)
+    {
+        NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+        if (strlen(arg) != 0)
+        {
+            NdisMoveMemory(Ssid.Ssid, arg, strlen(arg));
+            Ssid.SsidLength = strlen(arg);
+        }
+        else   //ANY ssid
+        {
+            Ssid.SsidLength = 0;
+		    memcpy(Ssid.Ssid, "", 0);
+			pAdapter->StaCfg.BssType = BSS_INFRA;
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+	        pAdapter->StaCfg.WepStatus  = Ndis802_11EncryptionDisabled;
+		}
+        pSsid = &Ssid;
+
+        if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+        {
+            RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+        }
+
+        pAdapter->MlmeAux.CurrReqIsFromNdis = TRUE;
+        pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+		pAdapter->bConfigChanged = TRUE;
+
+        MlmeEnqueue(pAdapter,
+                    MLME_CNTL_STATE_MACHINE,
+                    OID_802_11_SSID,
+                    sizeof(NDIS_802_11_SSID),
+                    (VOID *)pSsid);
+
+        StateMachineTouched = TRUE;
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_SSID_Proc::(Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+    }
+    else
+        success = FALSE;
+
+    if (StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAdapter);
+
+    return success;
+}
+
+#ifdef WMM_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WmmCapable Enable or Disable
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	BOOLEAN	bWmmCapable;
+
+	bWmmCapable = simple_strtol(arg, 0, 10);
+
+	if ((bWmmCapable == 1)
+#ifdef RT2870
+		&& (pAd->NumberOfPipes >= 5)
+#endif // RT2870 //
+		)
+		pAd->CommonCfg.bWmmCapable = TRUE;
+	else if (bWmmCapable == 0)
+		pAd->CommonCfg.bWmmCapable = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_WmmCapable_Proc::(bWmmCapable=%d)\n",
+		pAd->CommonCfg.bWmmCapable));
+
+	return TRUE;
+}
+#endif // WMM_SUPPORT //
+
+/*
+    ==========================================================================
+    Description:
+        Set Network Type(Infrastructure/Adhoc mode)
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UINT32	Value = 0;
+
+    if (strcmp(arg, "Adhoc") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_ADHOC)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (INFRA_ON(pAdapter))
+			{
+				//BOOLEAN Cancelled;
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event BB!\n"));
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_ADHOC;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(AD-HOC)\n"));
+	}
+    else if (strcmp(arg, "Infra") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_INFRA)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (ADHOC_ON(pAdapter))
+			{
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_INFRA;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(INFRA)\n"));
+
+        pAdapter->StaCfg.BssType = BSS_INFRA;
+	}
+    else if (strcmp(arg, "Monitor") == 0)
+    {
+		UCHAR	bbpValue = 0;
+		BCN_TIME_CFG_STRUC csr;
+		OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_INFRA_ON);
+        OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+		// disable all periodic state machine
+		pAdapter->StaCfg.bAutoReconnect = FALSE;
+		// reset all mlme state machine
+		RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+		DBGPRINT(RT_DEBUG_TRACE, ("fOP_STATUS_MEDIA_STATE_CONNECTED \n"));
+        if (pAdapter->CommonCfg.CentralChannel == 0)
+        {
+#ifdef DOT11_N_SUPPORT
+            if (pAdapter->CommonCfg.PhyMode == PHY_11AN_MIXED)
+                pAdapter->CommonCfg.CentralChannel = 36;
+            else
+#endif // DOT11_N_SUPPORT //
+                pAdapter->CommonCfg.CentralChannel = 6;
+        }
+#ifdef DOT11_N_SUPPORT
+        else
+            N_ChannelCheck(pAdapter);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_ABOVE)
+		{
+			// 40MHz ,control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			//  RX : control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue &= (~0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value &= 0xfffffffe;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel + 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_BELOW)
+		{
+			// 40MHz ,control channel at upper
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value |= 0x1;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue |= (0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel - 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			// 20MHz
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_20;
+			AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAdapter, pAdapter->CommonCfg.Channel);
+			DBGPRINT(RT_DEBUG_TRACE, ("BW_20, Channel(%d)\n", pAdapter->CommonCfg.Channel));
+		}
+		// Enable Rx with promiscuous reception
+		RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, 0x3);
+		// ASIC supporsts sniffer function with replacing RSSI with timestamp.
+		//RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+		//Value |= (0x80);
+		//RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+		// disable sync
+		RTMP_IO_READ32(pAdapter, BCN_TIME_CFG, &csr.word);
+		csr.field.bBeaconGen = 0;
+		csr.field.bTBTTEnable = 0;
+		csr.field.TsfSyncMode = 0;
+		RTMP_IO_WRITE32(pAdapter, BCN_TIME_CFG, csr.word);
+
+		pAdapter->StaCfg.BssType = BSS_MONITOR;
+        pAdapter->net_dev->type = ARPHRD_IEEE80211_PRISM; //ARPHRD_IEEE80211; // IEEE80211
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(MONITOR)\n"));
+    }
+
+    // Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_NetworkType_Proc::(NetworkType=%d)\n", pAdapter->StaCfg.BssType));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Authentication mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "WEPAUTO") == 0) || (strcmp(arg, "wepauto") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeAutoSwitch;
+    else if ((strcmp(arg, "OPEN") == 0) || (strcmp(arg, "open") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+    else if ((strcmp(arg, "SHARED") == 0) || (strcmp(arg, "shared") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    else if ((strcmp(arg, "WPAPSK") == 0) || (strcmp(arg, "wpapsk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+    else if ((strcmp(arg, "WPANONE") == 0) || (strcmp(arg, "wpanone") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+    else if ((strcmp(arg, "WPA2PSK") == 0) || (strcmp(arg, "wpa2psk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+#ifdef WPA_SUPPLICANT_SUPPORT
+    else if ((strcmp(arg, "WPA") == 0) || (strcmp(arg, "wpa") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+    else if ((strcmp(arg, "WPA2") == 0) || (strcmp(arg, "wpa2") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#endif // WPA_SUPPLICANT_SUPPORT //
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_AuthMode_Proc::(AuthMode=%d)\n", pAdapter->StaCfg.AuthMode));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Encryption Type
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "NONE") == 0) || (strcmp(arg, "none") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPDisabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPDisabled;
+    }
+    else if ((strcmp(arg, "WEP") == 0) || (strcmp(arg, "wep") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPEnabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPEnabled;
+    }
+    else if ((strcmp(arg, "TKIP") == 0) || (strcmp(arg, "tkip") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption2Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption2Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption2Enabled;
+    }
+    else if ((strcmp(arg, "AES") == 0) || (strcmp(arg, "aes") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption3Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption3Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption3Enabled;
+    }
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_EncrypType_Proc::(EncrypType=%d)\n", pAdapter->StaCfg.WepStatus));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Default Key ID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    ULONG                               KeyIdx;
+
+    KeyIdx = simple_strtol(arg, 0, 10);
+    if((KeyIdx >= 1 ) && (KeyIdx <= 4))
+        pAdapter->StaCfg.DefaultKeyId = (UCHAR) (KeyIdx - 1 );
+    else
+        return FALSE;  //Invalid argument
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_DefaultKeyID_Proc::(DefaultKeyID=%d)\n", pAdapter->StaCfg.DefaultKeyId));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY1
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+
+    pAdapter->SharedKey[BSS0][0].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              0,
+                              pAdapter->SharedKey[BSS0][0].CipherAlg,
+                              pAdapter->SharedKey[BSS0][0].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+
+    Description:
+        Set WEP KEY2
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][1].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              1,
+                              pAdapter->SharedKey[BSS0][1].CipherAlg,
+                              pAdapter->SharedKey[BSS0][1].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY3
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][2].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              2,
+                              pAdapter->SharedKey[BSS0][2].CipherAlg,
+                              pAdapter->SharedKey[BSS0][2].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY4
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][3].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              3,
+                              pAdapter->SharedKey[BSS0][3].CipherAlg,
+                              pAdapter->SharedKey[BSS0][3].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WPA PSK key
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UCHAR                   keyMaterial[40];
+
+    if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+        (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+	    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+		)
+        return TRUE;    // do nothing
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_WPAPSK_Proc::(WPAPSK=%s)\n", arg));
+
+    NdisZeroMemory(keyMaterial, 40);
+
+    if ((strlen(arg) < 8) || (strlen(arg) > 64))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("Set failed!!(WPAPSK=%s), WPAPSK key-string required 8 ~ 64 characters \n", arg));
+        return FALSE;
+    }
+
+    if (strlen(arg) == 64)
+    {
+        AtoH(arg, keyMaterial, 32);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+
+    }
+    else
+    {
+        PasswordHash((char *)arg, pAdapter->MlmeAux.Ssid, pAdapter->MlmeAux.SsidLen, keyMaterial);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+    }
+
+
+
+    if(pAdapter->StaCfg.BssType == BSS_ADHOC &&
+       pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+    {
+         pAdapter->StaCfg.WpaState = SS_NOTUSE;
+    }
+    else
+    {
+        // Start STA supplicant state machine
+        pAdapter->StaCfg.WpaState = SS_START;
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Power Saving mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (pAdapter->StaCfg.BssType == BSS_INFRA)
+    {
+        if ((strcmp(arg, "Max_PSP") == 0) ||
+			(strcmp(arg, "max_psp") == 0) ||
+			(strcmp(arg, "MAX_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeMAX_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeMAX_PSP;
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            pAdapter->StaCfg.DefaultListenCount = 5;
+
+        }
+        else if ((strcmp(arg, "Fast_PSP") == 0) ||
+				 (strcmp(arg, "fast_psp") == 0) ||
+                 (strcmp(arg, "FAST_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else if ((strcmp(arg, "Legacy_PSP") == 0) ||
+                 (strcmp(arg, "legacy_psp") == 0) ||
+                 (strcmp(arg, "LEGACY_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else
+        {
+            //Default Ndis802_11PowerModeCAM
+            // clear PSM bit immediately
+            MlmeSetPsmBit(pAdapter, PWR_ACTIVE);
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+        }
+
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_PSMode_Proc::(PSMode=%ld)\n", pAdapter->StaCfg.WindowsPowerMode));
+    }
+    else
+        return FALSE;
+
+
+    return TRUE;
+}
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WpaSupport flag.
+    Value:
+        0: Driver ignore wpa_supplicant.
+        1: wpa_supplicant initiates scanning and AP selection.
+        2: driver takes care of scanning, AP selection, and IEEE 802.11 association parameters.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+    if ( simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+    else if ( simple_strtol(arg, 0, 10) == 1)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE;
+    else if ( simple_strtol(arg, 0, 10) == 2)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE_WITH_WEB_UI;
+    else
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Wpa_Support::(WpaSupplicantUP=%d)\n", pAd->StaCfg.WpaSupplicantUP));
+
+    return TRUE;
+}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+/*
+    ==========================================================================
+    Description:
+        Read / Write MAC
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 mac 0        ==> read MAC where Addr=0x0
+               2.) iwpriv ra0 mac 0=12     ==> write MAC where Addr=0x0, value=12
+    ==========================================================================
+*/
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	ULONG				macAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	UINT32				macValue = 0;
+	INT					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+		//Parsing Read or Write
+	    this_char = arg;
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// Mac Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+				if (macAddr < 0xFFFF)
+				{
+					RTMP_IO_READ32(pAdapter, macAddr, &macValue);
+					DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%lx, MacValue=%x\n", macAddr, macValue));
+					sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr , macValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[8-k+j] = temp2[j];
+			}
+
+			while(k < 8)
+				temp2[7-k++]='0';
+			temp2[8]='\0';
+
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+
+				AtoH(temp2, temp, 4);
+				macValue = *temp*256*256*256 + temp[1]*256*256 + temp[2]*256 + temp[3];
+
+				// debug mode
+				if (macAddr == (HW_DEBUG_SETTING_BASE + 4))
+				{
+					// 0x2bf4: byte0 non-zero: enable R17 tuning, 0: disable R17 tuning
+                    if (macValue & 0x000000ff)
+                    {
+                        pAdapter->BbpTuning.bEnable = TRUE;
+                        DBGPRINT(RT_DEBUG_TRACE,("turn on R17 tuning\n"));
+                    }
+                    else
+                    {
+                        UCHAR R66;
+                        pAdapter->BbpTuning.bEnable = FALSE;
+                        R66 = 0x26 + GET_LNA_GAIN(pAdapter);
+#ifdef RALINK_ATE
+						if (ATE_ON(pAdapter))
+						{
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+						}
+						else
+#endif // RALINK_ATE //
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+                        DBGPRINT(RT_DEBUG_TRACE,("turn off R17 tuning, restore to 0x%02x\n", R66));
+                    }
+					return;
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%02lx, MacValue=0x%x\n", macAddr, macValue));
+
+				RTMP_IO_WRITE32(pAdapter, macAddr, macValue);
+				sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr, macValue);
+			}
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlMAC\n\n"));
+}
+
+/*
+    ==========================================================================
+    Description:
+        Read / Write E2PROM
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 e2p 0     	==> read E2PROM where Addr=0x0
+               2.) iwpriv ra0 e2p 0=1234    ==> write E2PROM where Addr=0x0, value=1234
+    ==========================================================================
+*/
+VOID RTMPIoctlE2PROM(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	USHORT				eepAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	USHORT				eepValue;
+	int					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+	    //Parsing Read or Write
+		this_char = arg;
+
+
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// E2PROM addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				eepAddr = *temp*256 + temp[1];
+				if (eepAddr < 0xFFFF)
+				{
+					RT28xx_EEPROM_READ16(pAdapter, eepAddr, eepValue);
+					sprintf(msg+strlen(msg), "[0x%04X]:0x%04X  ", eepAddr , eepValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[4-k+j] = temp2[j];
+			}
+
+			while(k < 4)
+				temp2[3-k++]='0';
+			temp2[4]='\0';
+
+			AtoH(this_char, temp, 2);
+			eepAddr = *temp*256 + temp[1];
+
+			AtoH(temp2, temp, 2);
+			eepValue = *temp*256 + temp[1];
+
+			RT28xx_EEPROM_WRITE16(pAdapter, eepAddr, eepValue);
+			sprintf(msg+strlen(msg), "[0x%02X]:%02X  ", eepAddr, eepValue);
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlE2PROM\n"));
+}
+#endif // DBG //
+
+
+
+
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.bTGnWifiTest = FALSE;
+    else
+        pAd->StaCfg.bTGnWifiTest = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_TGnWifiTest_Proc::(bTGnWifiTest=%d)\n", pAd->StaCfg.bTGnWifiTest));
+	return TRUE;
+}
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				LongRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_LongRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				ShortRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_ShortRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_None;
+    else if (simple_strtol(arg, 0, 10) == 1)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Flexible;
+    else if (simple_strtol(arg, 0, 10) == 2)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Strict;
+    else
+        return FALSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Ieee802dMode_Proc::(IEEEE0211dMode=%d)\n", pAdapter->StaCfg.IEEE80211dClientMode));
+    return TRUE;
+}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+    else
+        pAd->CommonCfg.CarrierDetect.Enable = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_CarrierDetect_Proc::(CarrierDetect.Enable=%d)\n", pAd->CommonCfg.CarrierDetect.Enable));
+	return TRUE;
+}
+#endif // CARRIER_DETECTION_SUPPORT //
+
+
+INT	Show_Adhoc_MacTable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCHAR			extra)
+{
+	INT i;
+
+	sprintf(extra, "\n");
+
+#ifdef DOT11_N_SUPPORT
+	sprintf(extra, "%sHT Operating Mode : %d\n", extra, pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode);
+#endif // DOT11_N_SUPPORT //
+
+	sprintf(extra, "%s\n%-19s%-4s%-4s%-7s%-7s%-7s%-10s%-6s%-6s%-6s%-6s\n", extra,
+			"MAC", "AID", "BSS", "RSSI0", "RSSI1", "RSSI2", "PhMd", "BW", "MCS", "SGI", "STBC");
+
+	for (i=1; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+
+		if (strlen(extra) > (IW_PRIV_SIZE_MASK - 30))
+		    break;
+		if ((pEntry->ValidAsCLI || pEntry->ValidAsApCli) && (pEntry->Sst == SST_ASSOC))
+		{
+			sprintf(extra, "%s%02X:%02X:%02X:%02X:%02X:%02X  ", extra,
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+			sprintf(extra, "%s%-4d", extra, (int)pEntry->Aid);
+			sprintf(extra, "%s%-4d", extra, (int)pEntry->apidx);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi0);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi1);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi2);
+			sprintf(extra, "%s%-10s", extra, GetPhyMode(pEntry->HTPhyMode.field.MODE));
+			sprintf(extra, "%s%-6s", extra, GetBW(pEntry->HTPhyMode.field.BW));
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.MCS);
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.ShortGI);
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.STBC);
+			sprintf(extra, "%s%-10d, %d, %d%%\n", extra, pEntry->DebugFIFOCount, pEntry->DebugTxCount,
+						(pEntry->DebugTxCount) ? ((pEntry->DebugTxCount-pEntry->DebugFIFOCount)*100/pEntry->DebugTxCount) : 0);
+			sprintf(extra, "%s\n", extra);
+		}
+	}
+
+	return TRUE;
+}
+
+
diff --git a/drivers/staging/rt2870/sta_ioctl.c.patch b/drivers/staging/rt2870/sta_ioctl.c.patch
new file mode 100644
index 000000000000..8672b147a761
--- /dev/null
+++ b/drivers/staging/rt2870/sta_ioctl.c.patch
@@ -0,0 +1,18 @@
+--- sta_ioctl.c	2008-09-19 14:37:52.000000000 +0800
++++ sta_ioctl.c.fc9	2008-09-19 14:38:20.000000000 +0800
+@@ -49,15 +49,9 @@
+
+ #define GROUP_KEY_NO                4
+
+-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+ #define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_A, _B, _C, _D, _E)
+ #define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_A, _B, _C, _D, _E)
+ #define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_A, _B, _C, _D, _E, _F)
+-#else
+-#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_B, _C, _D, _E)
+-#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_B, _C, _D, _E)
+-#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_B, _C, _D, _E, _F)
+-#endif
+
+ extern UCHAR    CipherWpa2Template[];
+ extern UCHAR    CipherWpaPskTkip[];
diff --git a/drivers/staging/rt2870/tmp60 b/drivers/staging/rt2870/tmp60
new file mode 100644
index 000000000000..975e44484379
--- /dev/null
+++ b/drivers/staging/rt2870/tmp60
@@ -0,0 +1,7037 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    sta_ioctl.c
+
+    Abstract:
+    IOCTL related subroutines
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Rory Chen   01-03-2003    created
+	Rory Chen   02-14-2005    modify to support RT61
+*/
+
+#include	"rt_config.h"
+
+#ifdef DBG
+extern ULONG    RTDebugLevel;
+#endif
+
+#define NR_WEP_KEYS 				4
+#define WEP_SMALL_KEY_LEN 			(40/8)
+#define WEP_LARGE_KEY_LEN 			(104/8)
+
+#define GROUP_KEY_NO                4
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_A, _B, _C, _D, _E, _F)
+#else
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_B, _C, _D, _E, _F)
+#endif
+
+extern UCHAR    CipherWpa2Template[];
+extern UCHAR    CipherWpaPskTkip[];
+extern UCHAR    CipherWpaPskTkipLen;
+
+typedef struct PACKED _RT_VERSION_INFO{
+    UCHAR       DriverVersionW;
+    UCHAR       DriverVersionX;
+    UCHAR       DriverVersionY;
+    UCHAR       DriverVersionZ;
+    UINT        DriverBuildYear;
+    UINT        DriverBuildMonth;
+    UINT        DriverBuildDay;
+} RT_VERSION_INFO, *PRT_VERSION_INFO;
+
+struct iw_priv_args privtab[] = {
+{ RTPRIV_IOCTL_SET,
+  IW_PRIV_TYPE_CHAR | 1024, 0,
+  "set"},
+
+{ RTPRIV_IOCTL_SHOW, 0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+{ RTPRIV_IOCTL_SHOW, IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+/* --- sub-ioctls definitions --- */
+    { SHOW_CONN_STATUS,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "connStatus" },
+	{ SHOW_DRVIER_VERION,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "driverVer" },
+    { SHOW_BA_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "bainfo" },
+	{ SHOW_DESC_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "descinfo" },
+    { RAIO_OFF,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_off" },
+	{ RAIO_ON,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_on" },
+#ifdef QOS_DLS_SUPPORT
+	{ SHOW_DLS_ENTRY_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "dlsentryinfo" },
+#endif // QOS_DLS_SUPPORT //
+	{ SHOW_CFG_VALUE,
+	  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "show" },
+	{ SHOW_ADHOC_ENTRY_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "adhocEntry" },
+
+/* --- sub-ioctls relations --- */
+
+#ifdef DBG
+{ RTPRIV_IOCTL_BBP,
+  IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "bbp"},
+{ RTPRIV_IOCTL_MAC,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "mac"},
+{ RTPRIV_IOCTL_E2P,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "e2p"},
+#endif  /* DBG */
+
+{ RTPRIV_IOCTL_STATISTICS,
+  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "stat"},
+{ RTPRIV_IOCTL_GSITESURVEY,
+  0, IW_PRIV_TYPE_CHAR | 1024,
+  "get_site_survey"},
+};
+
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WMM_SUPPORT
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif
+
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+VOID RTMPIoctlBBP(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlE2PROM(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  struct iwreq    *wrq);
+#endif // DBG //
+
+
+NDIS_STATUS RTMPWPANoneAddKeyProc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN	PVOID			pBuf);
+
+INT Set_FragTest_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef DOT11_N_SUPPORT
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // DOT11_N_SUPPORT //
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // CARRIER_DETECTION_SUPPORT //
+
+INT	Show_Adhoc_MacTable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCHAR			extra);
+
+static struct {
+	CHAR *name;
+	INT (*set_proc)(PRTMP_ADAPTER pAdapter, PUCHAR arg);
+} *PRTMP_PRIVATE_SET_PROC, RTMP_PRIVATE_SUPPORT_PROC[] = {
+	{"DriverVersion",				Set_DriverVersion_Proc},
+	{"CountryRegion",				Set_CountryRegion_Proc},
+	{"CountryRegionABand",			Set_CountryRegionABand_Proc},
+	{"SSID",						Set_SSID_Proc},
+	{"WirelessMode",				Set_WirelessMode_Proc},
+	{"TxBurst",					Set_TxBurst_Proc},
+	{"TxPreamble",				Set_TxPreamble_Proc},
+	{"TxPower",					Set_TxPower_Proc},
+	{"Channel",					Set_Channel_Proc},
+	{"BGProtection",				Set_BGProtection_Proc},
+	{"RTSThreshold",				Set_RTSThreshold_Proc},
+	{"FragThreshold",				Set_FragThreshold_Proc},
+#ifdef DOT11_N_SUPPORT
+	{"HtBw",		                Set_HtBw_Proc},
+	{"HtMcs",		                Set_HtMcs_Proc},
+	{"HtGi",		                Set_HtGi_Proc},
+	{"HtOpMode",		            Set_HtOpMode_Proc},
+	{"HtExtcha",		            Set_HtExtcha_Proc},
+	{"HtMpduDensity",		        Set_HtMpduDensity_Proc},
+	{"HtBaWinSize",		        	Set_HtBaWinSize_Proc},
+	{"HtRdg",		        		Set_HtRdg_Proc},
+	{"HtAmsdu",		        		Set_HtAmsdu_Proc},
+	{"HtAutoBa",		        	Set_HtAutoBa_Proc},
+	{"HtBaDecline",					Set_BADecline_Proc},
+	{"HtProtect",		        	Set_HtProtect_Proc},
+	{"HtMimoPs",		        	Set_HtMimoPs_Proc},
+#endif // DOT11_N_SUPPORT //
+
+#ifdef AGGREGATION_SUPPORT
+	{"PktAggregate",				Set_PktAggregate_Proc},
+#endif
+
+#ifdef WMM_SUPPORT
+	{"WmmCapable",					Set_WmmCapable_Proc},
+#endif
+	{"IEEE80211H",					Set_IEEE80211H_Proc},
+    {"NetworkType",                 Set_NetworkType_Proc},
+	{"AuthMode",					Set_AuthMode_Proc},
+	{"EncrypType",					Set_EncrypType_Proc},
+	{"DefaultKeyID",				Set_DefaultKeyID_Proc},
+	{"Key1",						Set_Key1_Proc},
+	{"Key2",						Set_Key2_Proc},
+	{"Key3",						Set_Key3_Proc},
+	{"Key4",						Set_Key4_Proc},
+	{"WPAPSK",						Set_WPAPSK_Proc},
+	{"ResetCounter",				Set_ResetStatCounter_Proc},
+	{"PSMode",                      Set_PSMode_Proc},
+#ifdef DBG
+	{"Debug",						Set_Debug_Proc},
+#endif
+
+#ifdef RALINK_ATE
+	{"ATE",							Set_ATE_Proc},
+	{"ATEDA",						Set_ATE_DA_Proc},
+	{"ATESA",						Set_ATE_SA_Proc},
+	{"ATEBSSID",					Set_ATE_BSSID_Proc},
+	{"ATECHANNEL",					Set_ATE_CHANNEL_Proc},
+	{"ATETXPOW0",					Set_ATE_TX_POWER0_Proc},
+	{"ATETXPOW1",					Set_ATE_TX_POWER1_Proc},
+	{"ATETXANT",					Set_ATE_TX_Antenna_Proc},
+	{"ATERXANT",					Set_ATE_RX_Antenna_Proc},
+	{"ATETXFREQOFFSET",				Set_ATE_TX_FREQOFFSET_Proc},
+	{"ATETXBW",						Set_ATE_TX_BW_Proc},
+	{"ATETXLEN",					Set_ATE_TX_LENGTH_Proc},
+	{"ATETXCNT",					Set_ATE_TX_COUNT_Proc},
+	{"ATETXMCS",					Set_ATE_TX_MCS_Proc},
+	{"ATETXMODE",					Set_ATE_TX_MODE_Proc},
+	{"ATETXGI",						Set_ATE_TX_GI_Proc},
+	{"ATERXFER",					Set_ATE_RX_FER_Proc},
+	{"ATERRF",						Set_ATE_Read_RF_Proc},
+	{"ATEWRF1",						Set_ATE_Write_RF1_Proc},
+	{"ATEWRF2",						Set_ATE_Write_RF2_Proc},
+	{"ATEWRF3",						Set_ATE_Write_RF3_Proc},
+	{"ATEWRF4",						Set_ATE_Write_RF4_Proc},
+	{"ATELDE2P",				    Set_ATE_Load_E2P_Proc},
+	{"ATERE2P",						Set_ATE_Read_E2P_Proc},
+	{"ATESHOW",						Set_ATE_Show_Proc},
+	{"ATEHELP",						Set_ATE_Help_Proc},
+
+#ifdef RALINK_28xx_QA
+	{"TxStop",						Set_TxStop_Proc},
+	{"RxStop",						Set_RxStop_Proc},
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    {"WpaSupport",                  Set_Wpa_Support},
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+	{"FixedTxMode",                 Set_FixedTxMode_Proc},
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	{"OpMode",						Set_OpMode_Proc},
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+#ifdef DOT11_N_SUPPORT
+    {"TGnWifiTest",                 Set_TGnWifiTest_Proc},
+    {"ForceGF",		        		Set_ForceGF_Proc},
+#endif // DOT11_N_SUPPORT //
+#ifdef QOS_DLS_SUPPORT
+	{"DlsAddEntry",					Set_DlsAddEntry_Proc},
+	{"DlsTearDownEntry",			Set_DlsTearDownEntry_Proc},
+#endif // QOS_DLS_SUPPORT //
+	{"LongRetry",	        		Set_LongRetryLimit_Proc},
+	{"ShortRetry",	        		Set_ShortRetryLimit_Proc},
+#ifdef EXT_BUILD_CHANNEL_LIST
+	{"11dClientMode",				Set_Ieee80211dClientMode_Proc},
+#endif // EXT_BUILD_CHANNEL_LIST //
+#ifdef CARRIER_DETECTION_SUPPORT
+	{"CarrierDetect",				Set_CarrierDetect_Proc},
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	{NULL,}
+};
+
+
+VOID RTMPAddKey(
+	IN	PRTMP_ADAPTER	    pAd,
+	IN	PNDIS_802_11_KEY    pKey)
+{
+	ULONG				KeyIdx;
+	MAC_TABLE_ENTRY  	*pEntry;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey ------>\n"));
+
+	if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+	{
+		if (pKey->KeyIndex & 0x80000000)
+		{
+		    if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+            {
+                NdisZeroMemory(pAd->StaCfg.PMK, 32);
+                NdisMoveMemory(pAd->StaCfg.PMK, pKey->KeyMaterial, pKey->KeyLength);
+                goto end;
+            }
+		    // Update PTK
+		    NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Decide its ChiperAlg
+        	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+        	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+        	else
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+            // Update these related information to MAC_TABLE_ENTRY
+        	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+            NdisMoveMemory(pEntry->PairwiseKey.Key, pAd->SharedKey[BSS0][0].Key, LEN_TKIP_EK);
+        	NdisMoveMemory(pEntry->PairwiseKey.RxMic, pAd->SharedKey[BSS0][0].RxMic, LEN_TKIP_RXMICK);
+        	NdisMoveMemory(pEntry->PairwiseKey.TxMic, pAd->SharedKey[BSS0][0].TxMic, LEN_TKIP_TXMICK);
+        	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+        	// Update pairwise key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  0,
+        						  pAd->SharedKey[BSS0][0].CipherAlg,
+        						  pAd->SharedKey[BSS0][0].Key,
+        						  pAd->SharedKey[BSS0][0].TxMic,
+        						  pAd->SharedKey[BSS0][0].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  0,
+        							  pAd->SharedKey[BSS0][0].CipherAlg,
+        							  pEntry);
+
+            if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+            {
+                // set 802.1x port control
+	            //pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAd);
+
+                // Indicate Connected for GUI
+                pAd->IndicateMediaState = NdisMediaStateConnected;
+            }
+		}
+        else
+        {
+            // Update GTK
+            pAd->StaCfg.DefaultKeyId = (pKey->KeyIndex & 0xFF);
+            NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Update Shared Key CipherAlg
+    		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+    		if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+    		else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+
+            // Update group key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  pAd->StaCfg.DefaultKeyId,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  pAd->StaCfg.DefaultKeyId,
+        							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        							  NULL);
+
+            // set 802.1x port control
+	        //pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+			STA_PORT_SECURED(pAd);
+
+            // Indicate Connected for GUI
+            pAd->IndicateMediaState = NdisMediaStateConnected;
+        }
+	}
+	else	// dynamic WEP from wpa_supplicant
+	{
+		UCHAR	CipherAlg;
+    	PUCHAR	Key;
+
+		if(pKey->KeyLength == 32)
+			goto end;
+
+		KeyIdx = pKey->KeyIndex & 0x0fffffff;
+
+		if (KeyIdx < 4)
+		{
+			// it is a default shared key, for Pairwise key setting
+			if (pKey->KeyIndex & 0x80000000)
+			{
+				pEntry = MacTableLookup(pAd, pKey->BSSID);
+
+				if (pEntry)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey: Set Pair-wise Key\n"));
+
+					// set key material and key length
+ 					pEntry->PairwiseKey.KeyLen = (UCHAR)pKey->KeyLength;
+					NdisMoveMemory(pEntry->PairwiseKey.Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+					// set Cipher type
+					if (pKey->KeyLength == 5)
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP64;
+					else
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP128;
+
+					// Add Pair-wise key to Asic
+					AsicAddPairwiseKeyEntry(
+						pAd,
+						pEntry->Addr,
+						(UCHAR)pEntry->Aid,
+                		&pEntry->PairwiseKey);
+
+					// update WCID attribute table and IVEIV table for this entry
+					RTMPAddWcidAttributeEntry(
+						pAd,
+						BSS0,
+						KeyIdx, // The value may be not zero
+						pEntry->PairwiseKey.CipherAlg,
+						pEntry);
+
+				}
+			}
+			else
+            {
+				// Default key for tx (shared key)
+				pAd->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+
+				// set key material and key length
+				pAd->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(pAd->SharedKey[BSS0][KeyIdx].Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+				// Set Ciper type
+				if (pKey->KeyLength == 5)
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP64;
+				else
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP128;
+
+    			CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][KeyIdx].Key;
+
+				// Set Group key material to Asic
+    			AsicAddSharedKeyEntry(pAd, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+				// Update WCID attribute table and IVEIV table for this group key table
+				RTMPAddWcidAttributeEntry(pAd, BSS0, KeyIdx, CipherAlg, NULL);
+
+			}
+		}
+	}
+end:
+	return;
+}
+
+char * rtstrchr(const char * s, int c)
+{
+    for(; *s != (char) c; ++s)
+        if (*s == '\0')
+            return NULL;
+    return (char *) s;
+}
+
+/*
+This is required for LinEX2004/kernel2.6.7 to provide iwlist scanning function
+*/
+
+int
+rt_ioctl_giwname(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+//	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+#ifdef RT2870
+	strncpy(name, "RT2870 Wireless", IFNAMSIZ);
+#endif // RT2870 //
+	return 0;
+}
+
+int rt_ioctl_siwfreq(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_freq *freq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	int 	chan = -1;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+
+	if (freq->e > 1)
+		return -EINVAL;
+
+	if((freq->e == 0) && (freq->m <= 1000))
+		chan = freq->m;	// Setting by channel number
+	else
+		MAP_KHZ_TO_CHANNEL_ID( (freq->m /100) , chan); // Setting by frequency - search the table , like 2.412G, 2.422G,
+
+    if (ChannelSanity(pAdapter, chan) == TRUE)
+    {
+	pAdapter->CommonCfg.Channel = chan;
+	DBGPRINT(RT_DEBUG_ERROR, ("==>rt_ioctl_siwfreq::SIOCSIWFREQ[cmd=0x%x] (Channel=%d)\n", SIOCSIWFREQ, pAdapter->CommonCfg.Channel));
+    }
+    else
+        return -EINVAL;
+
+	return 0;
+}
+int rt_ioctl_giwfreq(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_freq *freq, char *extra)
+{
+    VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter = NULL;
+	UCHAR ch;
+	ULONG	m;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+		ch = pAdapter->CommonCfg.Channel;
+
+	DBGPRINT(RT_DEBUG_TRACE,("==>rt_ioctl_giwfreq  %d\n", ch));
+
+    MAP_CHANNEL_ID_TO_KHZ(ch, m);
+	freq->m = m * 100;
+	freq->e = 1;
+	return 0;
+}
+
+int rt_ioctl_siwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+    	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	switch (*mode)
+	{
+		case IW_MODE_ADHOC:
+			Set_NetworkType_Proc(pAdapter, "Adhoc");
+			break;
+		case IW_MODE_INFRA:
+			Set_NetworkType_Proc(pAdapter, "Infra");
+			break;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+        case IW_MODE_MONITOR:
+			Set_NetworkType_Proc(pAdapter, "Monitor");
+			break;
+#endif
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_siwmode::SIOCSIWMODE (unknown %d)\n", *mode));
+			return -EINVAL;
+	}
+
+	// Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+	pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+	return 0;
+}
+
+int rt_ioctl_giwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (ADHOC_ON(pAdapter))
+		*mode = IW_MODE_ADHOC;
+    else if (INFRA_ON(pAdapter))
+		*mode = IW_MODE_INFRA;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+    else if (MONITOR_ON(pAdapter))
+    {
+        *mode = IW_MODE_MONITOR;
+    }
+#endif
+    else
+        *mode = IW_MODE_AUTO;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==>rt_ioctl_giwmode(mode=%d)\n", *mode));
+	return 0;
+}
+
+int rt_ioctl_siwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+        	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	return 0;
+}
+
+int rt_ioctl_giwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	return 0;
+}
+
+int rt_ioctl_giwrange(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	struct iw_range *range = (struct iw_range *) extra;
+	u16 val;
+	int i;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===>rt_ioctl_giwrange\n"));
+	data->length = sizeof(struct iw_range);
+	memset(range, 0, sizeof(struct iw_range));
+
+	range->txpower_capa = IW_TXPOW_DBM;
+
+	if (INFRA_ON(pAdapter)||ADHOC_ON(pAdapter))
+	{
+		range->min_pmp = 1 * 1024;
+		range->max_pmp = 65535 * 1024;
+		range->min_pmt = 1 * 1024;
+		range->max_pmt = 1000 * 1024;
+		range->pmp_flags = IW_POWER_PERIOD;
+		range->pmt_flags = IW_POWER_TIMEOUT;
+		range->pm_capa = IW_POWER_PERIOD | IW_POWER_TIMEOUT |
+			IW_POWER_UNICAST_R | IW_POWER_ALL_R;
+	}
+
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 14;
+
+	range->retry_capa = IW_RETRY_LIMIT;
+	range->retry_flags = IW_RETRY_LIMIT;
+	range->min_retry = 0;
+	range->max_retry = 255;
+
+	range->num_channels =  pAdapter->ChannelListNum;
+
+	val = 0;
+	for (i = 1; i <= range->num_channels; i++)
+	{
+		u32 m;
+		range->freq[val].i = pAdapter->ChannelList[i-1].Channel;
+		MAP_CHANNEL_ID_TO_KHZ(pAdapter->ChannelList[i-1].Channel, m);
+		range->freq[val].m = m * 100; /* HZ */
+
+		range->freq[val].e = 1;
+		val++;
+		if (val == IW_MAX_FREQUENCIES)
+			break;
+	}
+	range->num_frequency = val;
+
+	range->max_qual.qual = 100; /* what is correct max? This was not
+					* documented exactly. At least
+					* 69 has been observed. */
+	range->max_qual.level = 0; /* dB */
+	range->max_qual.noise = 0; /* dB */
+
+	/* What would be suitable values for "average/typical" qual? */
+	range->avg_qual.qual = 20;
+	range->avg_qual.level = -60;
+	range->avg_qual.noise = -95;
+	range->sensitivity = 3;
+
+	range->max_encoding_tokens = NR_WEP_KEYS;
+	range->num_encoding_sizes = 2;
+	range->encoding_size[0] = 5;
+	range->encoding_size[1] = 13;
+
+	range->min_rts = 0;
+	range->max_rts = 2347;
+	range->min_frag = 256;
+	range->max_frag = 2346;
+
+#if WIRELESS_EXT > 17
+	/* IW_ENC_CAPA_* bit field */
+	range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
+					IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+#endif
+
+	return 0;
+}
+
+int rt_ioctl_siwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+    NDIS_802_11_MAC_ADDRESS Bssid;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+    {
+        RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+    }
+
+    // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+    // this request, because this request is initiated by NDIS.
+    pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+	// Prevent to connect AP again in STAMlmePeriodicExec
+	pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+    memset(Bssid, 0, MAC_ADDR_LEN);
+    memcpy(Bssid, ap_addr->sa_data, MAC_ADDR_LEN);
+    MlmeEnqueue(pAdapter,
+                MLME_CNTL_STATE_MACHINE,
+                OID_802_11_BSSID,
+                sizeof(NDIS_802_11_MAC_ADDRESS),
+                (VOID *)&Bssid);
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCSIWAP %02x:%02x:%02x:%02x:%02x:%02x\n",
+        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+
+	return 0;
+}
+
+int rt_ioctl_giwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+	{
+		ap_addr->sa_family = ARPHRD_ETHER;
+		memcpy(ap_addr->sa_data, &pAdapter->CommonCfg.Bssid, ETH_ALEN);
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    // Add for RT2870
+    else if (pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+    {
+        ap_addr->sa_family = ARPHRD_ETHER;
+        memcpy(ap_addr->sa_data, &pAdapter->MlmeAux.Bssid, ETH_ALEN);
+    }
+#endif // WPA_SUPPLICANT_SUPPORT //
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIWAP(=EMPTY)\n"));
+		return -ENOTCONN;
+	}
+
+	return 0;
+}
+
+/*
+ * Units are in db above the noise floor. That means the
+ * rssi values reported in the tx/rx descriptors in the
+ * driver are the SNR expressed in db.
+ *
+ * If you assume that the noise floor is -95, which is an
+ * excellent assumption 99.5 % of the time, then you can
+ * derive the absolute signal level (i.e. -95 + rssi).
+ * There are some other slight factors to take into account
+ * depending on whether the rssi measurement is from 11b,
+ * 11g, or 11a.   These differences are at most 2db and
+ * can be documented.
+ *
+ * NB: various calculations are based on the orinoco/wavelan
+ *     drivers for compatibility
+ */
+static void set_quality(PRTMP_ADAPTER pAdapter,
+                        struct iw_quality *iq,
+                        signed char rssi)
+{
+	__u8 ChannelQuality;
+
+	// Normalize Rssi
+	if (rssi >= -50)
+		ChannelQuality = 100;
+	else if (rssi >= -80) // between -50 ~ -80dbm
+		ChannelQuality = (__u8)(24 + ((rssi + 80) * 26)/10);
+	else if (rssi >= -90)   // between -80 ~ -90dbm
+        ChannelQuality = (__u8)((rssi + 90) * 26)/10;
+	else
+		ChannelQuality = 0;
+
+    iq->qual = (__u8)ChannelQuality;
+
+    iq->level = (__u8)(rssi);
+    iq->noise = (pAdapter->BbpWriteLatch[66] > pAdapter->BbpTuning.FalseCcaUpperThreshold) ? ((__u8)pAdapter->BbpTuning.FalseCcaUpperThreshold) : ((__u8) pAdapter->BbpWriteLatch[66]); 	// noise level (dBm)
+    iq->noise += 256 - 143;
+    iq->updated = pAdapter->iw_stats.qual.updated;
+}
+
+int rt_ioctl_iwaplist(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+ 	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	struct sockaddr addr[IW_MAX_AP];
+	struct iw_quality qual[IW_MAX_AP];
+	int i;
+
+   	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		data->length = 0;
+		return 0;
+        //return -ENETDOWN;
+	}
+
+	for (i = 0; i <IW_MAX_AP ; i++)
+	{
+		if (i >=  pAdapter->ScanTab.BssNr)
+			break;
+		addr[i].sa_family = ARPHRD_ETHER;
+			memcpy(addr[i].sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+		set_quality(pAdapter, &qual[i], pAdapter->ScanTab.BssEntry[i].Rssi);
+	}
+	data->length = i;
+	memcpy(extra, &addr, i*sizeof(addr[0]));
+	data->flags = 1;		/* signal quality present (sort of) */
+	memcpy(extra + i*sizeof(addr[0]), &qual, i*sizeof(qual[i]));
+
+	return 0;
+}
+
+#ifdef SIOCGIWSCAN
+int rt_ioctl_siwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	ULONG								Now;
+	int Status = NDIS_STATUS_SUCCESS;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		return -ENETDOWN;
+	}
+
+	if (MONITOR_ON(pAdapter))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+        return -EINVAL;
+    }
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount++;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+    pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		return 0;
+	do{
+		Now = jiffies;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+		if ((pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE) &&
+			(pAdapter->StaCfg.WpaSupplicantScanCount > 3))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! WpaSupplicantScanCount > 3\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+		if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+			((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+			(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)) &&
+			(pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+
+		if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+		{
+			RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+		}
+
+		// tell CNTL state machine to call NdisMSetInformationComplete() after completing
+		// this request, because this request is initiated by NDIS.
+		pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+		// Reset allowed scan retries
+		pAdapter->StaCfg.ScanCnt = 0;
+		pAdapter->StaCfg.LastScanTime = Now;
+
+		MlmeEnqueue(pAdapter,
+			MLME_CNTL_STATE_MACHINE,
+			OID_802_11_BSSID_LIST_SCAN,
+			0,
+			NULL);
+
+		Status = NDIS_STATUS_SUCCESS;
+		RT28XX_MLME_HANDLER(pAdapter);
+	}while(0);
+	return 0;
+}
+
+int rt_ioctl_giwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	int i=0;
+	char *current_ev = extra, *previous_ev = extra;
+	char *end_buf;
+	char *current_val, custom[MAX_CUSTOM_LEN] = {0};
+#ifndef IWEVGENIE
+	char idx;
+#endif // IWEVGENIE //
+	struct iw_event iwe;
+
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+    {
+		/*
+		 * Still scanning, indicate the caller should try again.
+		 */
+		return -EAGAIN;
+	}
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount = 0;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	if (pAdapter->ScanTab.BssNr == 0)
+	{
+		data->length = 0;
+		return 0;
+	}
+
+#if WIRELESS_EXT >= 17
+    if (data->length > 0)
+        end_buf = extra + data->length;
+    else
+        end_buf = extra + IW_SCAN_MAX_DATA;
+#else
+    end_buf = extra + IW_SCAN_MAX_DATA;
+#endif
+
+	for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+	{
+		if (current_ev >= end_buf)
+        {
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+        }
+
+		//MAC address
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWAP;
+		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+				memcpy(iwe.u.ap_addr.sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, ETH_ALEN);
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_ADDR_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//ESSID
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWESSID;
+		iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].SsidLen;
+		iwe.u.data.flags = 1;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_POINT(info, current_ev,end_buf, &iwe, pAdapter->ScanTab.BssEntry[i].Ssid);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Network Type
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWMODE;
+		if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11IBSS)
+		{
+			iwe.u.mode = IW_MODE_ADHOC;
+		}
+		else if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11Infrastructure)
+		{
+			iwe.u.mode = IW_MODE_INFRA;
+		}
+		else
+		{
+			iwe.u.mode = IW_MODE_AUTO;
+		}
+		iwe.len = IW_EV_UINT_LEN;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe,  IW_EV_UINT_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Channel and Frequency
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWFREQ;
+		if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		else
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		iwe.u.freq.e = 0;
+		iwe.u.freq.i = 0;
+
+		previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_FREQ_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+        //Add quality statistics
+        //================================
+        memset(&iwe, 0, sizeof(iwe));
+    	iwe.cmd = IWEVQUAL;
+    	iwe.u.qual.level = 0;
+    	iwe.u.qual.noise = 0;
+        set_quality(pAdapter, &iwe.u.qual, pAdapter->ScanTab.BssEntry[i].Rssi);
+    	current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Encyption key
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWENCODE;
+		if (CAP_IS_PRIVACY_ON (pAdapter->ScanTab.BssEntry[i].CapabilityInfo ))
+			iwe.u.data.flags =IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+		else
+			iwe.u.data.flags = IW_ENCODE_DISABLED;
+
+        previous_ev = current_ev;
+        current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf,&iwe, (char *)pAdapter->SharedKey[BSS0][(iwe.u.data.flags & IW_ENCODE_INDEX)-1].Key);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Bit Rate
+		//================================
+		if (pAdapter->ScanTab.BssEntry[i].SupRateLen)
+        {
+            UCHAR tmpRate = pAdapter->ScanTab.BssEntry[i].SupRate[pAdapter->ScanTab.BssEntry[i].SupRateLen-1];
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWRATE;
+    		current_val = current_ev + IW_EV_LCP_LEN;
+            if (tmpRate == 0x82)
+                iwe.u.bitrate.value =  1 * 1000000;
+            else if (tmpRate == 0x84)
+                iwe.u.bitrate.value =  2 * 1000000;
+            else if (tmpRate == 0x8B)
+                iwe.u.bitrate.value =  5.5 * 1000000;
+            else if (tmpRate == 0x96)
+                iwe.u.bitrate.value =  11 * 1000000;
+            else
+    		    iwe.u.bitrate.value =  (tmpRate/2) * 1000000;
+
+			iwe.u.bitrate.disabled = 0;
+			current_val = IWE_STREAM_ADD_VALUE(info, current_ev,
+				current_val, end_buf, &iwe,
+    			IW_EV_PARAM_LEN);
+
+        	if((current_val-current_ev)>IW_EV_LCP_LEN)
+            	current_ev = current_val;
+        	else
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+#ifdef IWEVGENIE
+		//WPA IE
+		if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+		{
+			memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].WpaIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].WpaIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].WpaIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+		}
+
+		//WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+        	memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].RsnIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].RsnIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].RsnIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#else
+        //WPA IE
+		//================================
+        if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "wpa_ie=", 7);
+            for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].WpaIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].WpaIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+        //WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "rsn_ie=", 7);
+			for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].RsnIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].RsnIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#endif // IWEVGENIE //
+	}
+
+	data->length = current_ev - extra;
+    pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+	DBGPRINT(RT_DEBUG_ERROR ,("===>rt_ioctl_giwscan. %d(%d) BSS returned, data->length = %d\n",i , pAdapter->ScanTab.BssNr, data->length));
+	return 0;
+}
+#endif
+
+int rt_ioctl_siwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (data->flags)
+	{
+		PCHAR	pSsidString = NULL;
+
+		// Includes null character.
+		if (data->length > (IW_ESSID_MAX_SIZE + 1))
+			return -E2BIG;
+
+		pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+		if (pSsidString)
+		{
+			NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+			NdisMoveMemory(pSsidString, essid, data->length);
+			if (Set_SSID_Proc(pAdapter, pSsidString) == FALSE)
+				return -EINVAL;
+		}
+		else
+			return -ENOMEM;
+	}
+	else
+	{
+		// ANY ssid
+		if (Set_SSID_Proc(pAdapter, "") == FALSE)
+			return -EINVAL;
+    }
+	return 0;
+}
+
+int rt_ioctl_giwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	data->flags = 1;
+    if (MONITOR_ON(pAdapter))
+    {
+        data->length  = 0;
+        return 0;
+    }
+
+	if (OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is connected\n"));
+		data->length = pAdapter->CommonCfg.SsidLen;
+		memcpy(essid, pAdapter->CommonCfg.Ssid, pAdapter->CommonCfg.SsidLen);
+	}
+#ifdef RT2870
+#ifdef WPA_SUPPLICANT_SUPPORT
+    // Add for RT2870
+    else if (pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+    {
+        data->length = pAdapter->CommonCfg.SsidLen;
+		memcpy(essid, pAdapter->CommonCfg.Ssid, pAdapter->CommonCfg.SsidLen);
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+#endif // RT2870 //
+	else
+	{//the ANY ssid was specified
+		data->length  = 0;
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is not connected, ess\n"));
+	}
+
+	return 0;
+
+}
+
+int rt_ioctl_siwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE ,("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (data->length > IW_ESSID_MAX_SIZE)
+		return -EINVAL;
+
+	memset(pAdapter->nickname, 0, IW_ESSID_MAX_SIZE + 1);
+	memcpy(pAdapter->nickname, nickname, data->length);
+
+
+	return 0;
+}
+
+int rt_ioctl_giwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (data->length > strlen(pAdapter->nickname) + 1)
+		data->length = strlen(pAdapter->nickname) + 1;
+	if (data->length > 0) {
+		memcpy(nickname, pAdapter->nickname, data->length-1);
+		nickname[data->length-1] = '\0';
+	}
+	return 0;
+}
+
+int rt_ioctl_siwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	u16 val;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (rts->disabled)
+		val = MAX_RTS_THRESHOLD;
+	else if (rts->value < 0 || rts->value > MAX_RTS_THRESHOLD)
+		return -EINVAL;
+	else if (rts->value == 0)
+	    val = MAX_RTS_THRESHOLD;
+	else
+		val = rts->value;
+
+	if (val != pAdapter->CommonCfg.RtsThreshold)
+		pAdapter->CommonCfg.RtsThreshold = val;
+
+	return 0;
+}
+
+int rt_ioctl_giwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	rts->value = pAdapter->CommonCfg.RtsThreshold;
+	rts->disabled = (rts->value == MAX_RTS_THRESHOLD);
+	rts->fixed = 1;
+
+	return 0;
+}
+
+int rt_ioctl_siwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	u16 val;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if (frag->disabled)
+		val = MAX_FRAG_THRESHOLD;
+	else if (frag->value >= MIN_FRAG_THRESHOLD || frag->value <= MAX_FRAG_THRESHOLD)
+        val = __cpu_to_le16(frag->value & ~0x1); /* even numbers only */
+	else if (frag->value == 0)
+	    val = MAX_FRAG_THRESHOLD;
+	else
+		return -EINVAL;
+
+	pAdapter->CommonCfg.FragmentThreshold = val;
+	return 0;
+}
+
+int rt_ioctl_giwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	frag->value = pAdapter->CommonCfg.FragmentThreshold;
+	frag->disabled = (frag->value == MAX_FRAG_THRESHOLD);
+	frag->fixed = 1;
+
+	return 0;
+}
+
+#define MAX_WEP_KEY_SIZE 13
+#define MIN_WEP_KEY_SIZE 5
+int rt_ioctl_siwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if ((erq->length == 0) &&
+        (erq->flags & IW_ENCODE_DISABLED))
+	{
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+	else if ((erq->length == 0) &&
+             (erq->flags & IW_ENCODE_RESTRICTED || erq->flags & IW_ENCODE_OPEN))
+	{
+	    //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+		STA_PORT_SECURED(pAdapter);
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+		if (erq->flags & IW_ENCODE_RESTRICTED)
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    	else
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+
+    if (erq->length > 0)
+	{
+		int keyIdx = (erq->flags & IW_ENCODE_INDEX) - 1;
+		/* Check the size of the key */
+		if (erq->length > MAX_WEP_KEY_SIZE) {
+			return -EINVAL;
+		}
+		/* Check key index */
+		if ((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+        {
+            DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::Wrong keyIdx=%d! Using default key instead (%d)\n",
+                                        keyIdx, pAdapter->StaCfg.DefaultKeyId));
+
+            //Using default key
+			keyIdx = pAdapter->StaCfg.DefaultKeyId;
+        }
+
+        NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+
+		if (erq->length == MAX_WEP_KEY_SIZE)
+        {
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+		}
+		else if (erq->length == MIN_WEP_KEY_SIZE)
+        {
+            pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+		}
+		else
+			/* Disable the key */
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+
+		/* Check if the key is not marked as invalid */
+		if(!(erq->flags & IW_ENCODE_NOKEY)) {
+			/* Copy the key in the driver */
+			NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, extra, erq->length);
+        }
+	}
+    else
+			{
+		/* Do we want to just set the transmit key index ? */
+		int index = (erq->flags & IW_ENCODE_INDEX) - 1;
+		if ((index >= 0) && (index < 4))
+        {
+			pAdapter->StaCfg.DefaultKeyId = index;
+            }
+        else
+			/* Don't complain if only change the mode */
+			if(!erq->flags & IW_ENCODE_MODE) {
+				return -EINVAL;
+		}
+	}
+
+done:
+    DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::erq->flags=%x\n",erq->flags));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::AuthMode=%x\n",pAdapter->StaCfg.AuthMode));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::DefaultKeyId=%x, KeyLen = %d\n",pAdapter->StaCfg.DefaultKeyId , pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::WepStatus=%x\n",pAdapter->StaCfg.WepStatus));
+	return 0;
+}
+
+int
+rt_ioctl_giwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *key)
+{
+	int kid;
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	kid = erq->flags & IW_ENCODE_INDEX;
+	DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_giwencode %d\n", erq->flags & IW_ENCODE_INDEX));
+
+	if (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled)
+	{
+		erq->length = 0;
+		erq->flags = IW_ENCODE_DISABLED;
+	}
+	else if ((kid > 0) && (kid <=4))
+	{
+		// copy wep key
+		erq->flags = kid ;			/* NB: base 1 */
+		if (erq->length > pAdapter->SharedKey[BSS0][kid-1].KeyLen)
+			erq->length = pAdapter->SharedKey[BSS0][kid-1].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][kid-1].Key, erq->length);
+		//if ((kid == pAdapter->PortCfg.DefaultKeyId))
+		//erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+
+	}
+	else if (kid == 0)
+	{
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->length = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, erq->length);
+		// copy default key ID
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->flags = pAdapter->StaCfg.DefaultKeyId + 1;			/* NB: base 1 */
+		erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+	}
+
+	return 0;
+
+}
+
+static int
+rt_ioctl_setparam(struct net_device *dev, struct iw_request_info *info,
+			 void *w, char *extra)
+{
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter;
+	POS_COOKIE pObj;
+	char *this_char = extra;
+	char *value;
+	int  Status=0;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+	pObj = (POS_COOKIE) pAdapter->OS_Cookie;
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+			return -ENETDOWN;
+    	}
+
+	if (!*this_char)
+		return -EINVAL;
+
+	if ((value = rtstrchr(this_char, '=')) != NULL)
+	    *value++ = 0;
+
+	if (!value)
+	    return -EINVAL;
+
+	// reject setting nothing besides ANY ssid(ssidLen=0)
+    if (!*value && (strcmp(this_char, "SSID") != 0))
+        return -EINVAL;
+
+	for (PRTMP_PRIVATE_SET_PROC = RTMP_PRIVATE_SUPPORT_PROC; PRTMP_PRIVATE_SET_PROC->name; PRTMP_PRIVATE_SET_PROC++)
+	{
+	    if (strcmp(this_char, PRTMP_PRIVATE_SET_PROC->name) == 0)
+	    {
+	        if(!PRTMP_PRIVATE_SET_PROC->set_proc(pAdapter, value))
+	        {	//FALSE:Set private failed then return Invalid argument
+			    Status = -EINVAL;
+	        }
+		    break;	//Exit for loop.
+	    }
+	}
+
+	if(PRTMP_PRIVATE_SET_PROC->name == NULL)
+	{  //Not found argument
+	    Status = -EINVAL;
+	    DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_setparam:: (iwpriv) Not Support Set Command [%s=%s]\n", this_char, value));
+	}
+
+    return Status;
+}
+
+
+static int
+rt_private_get_statistics(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+	INT				Status = 0;
+    PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+    sprintf(extra, "\n\n");
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+	    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	    //sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart - (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+	}
+    sprintf(extra+strlen(extra), "Tx success after retry          = %ld\n", (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx fail to Rcv ACK after retry  = %ld\n", (ULONG)pAd->WlanCounters.FailedCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Success Rcv CTS             = %ld\n", (ULONG)pAd->WlanCounters.RTSSuccessCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Fail Rcv CTS                = %ld\n", (ULONG)pAd->WlanCounters.RTSFailureCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "Rx success                      = %ld\n", (ULONG)pAd->WlanCounters.ReceivedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx with CRC                     = %ld\n", (ULONG)pAd->WlanCounters.FCSErrorCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx drop due to out of resource  = %ld\n", (ULONG)pAd->Counters8023.RxNoBuffer);
+    sprintf(extra+strlen(extra), "Rx duplicate frame              = %ld\n", (ULONG)pAd->WlanCounters.FrameDuplicateCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "False CCA (one second)          = %ld\n", (ULONG)pAd->RalinkCounters.OneSecFalseCCACnt);
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		if (pAd->ate.RxAntennaSel == 0)
+		{
+    		sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->ate.LastRssi1 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->ate.LastRssi2 - pAd->BbpRssiToDbmDelta));
+		}
+		else
+		{
+    		sprintf(extra+strlen(extra), "RSSI                            = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+		}
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    	sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi0 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi1 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi2 - pAd->BbpRssiToDbmDelta));
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    sprintf(extra+strlen(extra), "WpaSupplicantUP                 = %d\n\n", pAd->StaCfg.WpaSupplicantUP);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+    DBGPRINT(RT_DEBUG_TRACE, ("<== rt_private_get_statistics, wrq->length = %d\n", wrq->length));
+
+    return Status;
+}
+
+#ifdef DOT11_N_SUPPORT
+void	getBaInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pOutBuf)
+{
+	INT i, j;
+	BA_ORI_ENTRY *pOriBAEntry;
+	BA_REC_ENTRY *pRecBAEntry;
+
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+		if (((pEntry->ValidAsCLI || pEntry->ValidAsApCli) && (pEntry->Sst == SST_ASSOC))
+			|| (pEntry->ValidAsWDS) || (pEntry->ValidAsMesh))
+		{
+			sprintf(pOutBuf, "%s\n%02X:%02X:%02X:%02X:%02X:%02X (Aid = %d) (AP) -\n",
+                pOutBuf,
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5], pEntry->Aid);
+
+			sprintf(pOutBuf, "%s[Recipient]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BARecWcidArray[j] != 0)
+				{
+					pRecBAEntry =&pAd->BATable.BARecEntry[pEntry->BARecWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, LastIndSeq=%d, ReorderingPkts=%d\n", pOutBuf, j, pRecBAEntry->BAWinSize, pRecBAEntry->LastIndSeq, pRecBAEntry->list.qlen);
+				}
+			}
+			sprintf(pOutBuf, "%s\n", pOutBuf);
+
+			sprintf(pOutBuf, "%s[Originator]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BAOriWcidArray[j] != 0)
+				{
+					pOriBAEntry =&pAd->BATable.BAOriEntry[pEntry->BAOriWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, StartSeq=%d, CurTxSeq=%d\n", pOutBuf, j, pOriBAEntry->BAWinSize, pOriBAEntry->Sequence, pEntry->TxSeq[j]);
+				}
+			}
+			sprintf(pOutBuf, "%s\n\n", pOutBuf);
+		}
+        if (strlen(pOutBuf) > (IW_PRIV_SIZE_MASK - 30))
+                break;
+	}
+
+	return;
+}
+#endif // DOT11_N_SUPPORT //
+
+static int
+rt_private_show(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+    INT				Status = 0;
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+    PRTMP_ADAPTER   pAd;
+	POS_COOKIE		pObj;
+    u32             subcmd = wrq->flags;
+
+	if (dev->priv_flags == INT_MAIN)
+		pAd = dev->priv;
+	else
+	{
+		pVirtualAd = dev->priv;
+		pAd = pVirtualAd->RtmpDev->priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+    switch(subcmd)
+    {
+
+        case SHOW_CONN_STATUS:
+            if (MONITOR_ON(pAd))
+            {
+#ifdef DOT11_N_SUPPORT
+                if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                    pAd->CommonCfg.RegTransmitSetting.field.BW)
+                    sprintf(extra, "Monitor Mode(CentralChannel %d)\n", pAd->CommonCfg.CentralChannel);
+                else
+#endif // DOT11_N_SUPPORT //
+                    sprintf(extra, "Monitor Mode(Channel %d)\n", pAd->CommonCfg.Channel);
+            }
+            else
+            {
+                if (pAd->IndicateMediaState == NdisMediaStateConnected)
+            	{
+            	    if (INFRA_ON(pAd))
+                    {
+                    sprintf(extra, "Connected(AP: %s[%02X:%02X:%02X:%02X:%02X:%02X])\n",
+                                    pAd->CommonCfg.Ssid,
+                                    pAd->CommonCfg.Bssid[0],
+                                    pAd->CommonCfg.Bssid[1],
+                                    pAd->CommonCfg.Bssid[2],
+                                    pAd->CommonCfg.Bssid[3],
+                                    pAd->CommonCfg.Bssid[4],
+                                    pAd->CommonCfg.Bssid[5]);
+            		DBGPRINT(RT_DEBUG_TRACE ,("Ssid=%s ,Ssidlen = %d\n",pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen));
+            	}
+                    else if (ADHOC_ON(pAd))
+                        sprintf(extra, "Connected\n");
+            	}
+            	else
+            	{
+            	    sprintf(extra, "Disconnected\n");
+            		DBGPRINT(RT_DEBUG_TRACE ,("ConnStatus is not connected\n"));
+            	}
+            }
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case SHOW_DRVIER_VERION:
+            sprintf(extra, "Driver version-%s, %s %s\n", STA_DRIVER_VERSION, __DATE__, __TIME__ );
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#ifdef DOT11_N_SUPPORT
+        case SHOW_BA_INFO:
+            getBaInfo(pAd, extra);
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#endif // DOT11_N_SUPPORT //
+		case SHOW_DESC_INFO:
+			{
+				Show_DescInfo_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+        case RAIO_OFF:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = FALSE;
+            if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == FALSE)
+                {
+                    MlmeRadioOff(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = SW_RADIO_OFF;
+                }
+            }
+            sprintf(extra, "Radio Off\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case RAIO_ON:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = TRUE;
+            //if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == TRUE)
+                {
+                    MlmeRadioOn(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+                }
+            }
+            sprintf(extra, "Radio On\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case SHOW_DLS_ENTRY_INFO:
+			{
+				Set_DlsEntryInfo_Display_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+
+		case SHOW_CFG_VALUE:
+			{
+				Status = RTMPShowCfgValue(pAd, wrq->pointer, extra);
+				if (Status == 0)
+					wrq->length = strlen(extra) + 1; // 1: size of '\0'
+			}
+			break;
+		case SHOW_ADHOC_ENTRY_INFO:
+			Show_Adhoc_MacTable_Proc(pAd, extra);
+			wrq->length = strlen(extra) + 1; // 1: size of '\0'
+			break;
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("%s - unknow subcmd = %d\n", __func__, subcmd));
+            break;
+    }
+
+    return Status;
+}
+
+#ifdef SIOCSIWMLME
+int rt_ioctl_siwmlme(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+	struct iw_mlme *pMlme = (struct iw_mlme *)wrqu->data.pointer;
+	MLME_QUEUE_ELEM				MsgElem;
+	MLME_DISASSOC_REQ_STRUCT	DisAssocReq;
+	MLME_DEAUTH_REQ_STRUCT      DeAuthReq;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("====> %s\n", __func__));
+
+	if (pMlme == NULL)
+		return -EINVAL;
+
+	switch(pMlme->cmd)
+	{
+#ifdef IW_MLME_DEAUTH
+		case IW_MLME_DEAUTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DEAUTH\n", __func__));
+			COPY_MAC_ADDR(DeAuthReq.Addr, pAd->CommonCfg.Bssid);
+			DeAuthReq.Reason = pMlme->reason_code;
+			MsgElem.MsgLen = sizeof(MLME_DEAUTH_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DeAuthReq, sizeof(MLME_DEAUTH_REQ_STRUCT));
+			MlmeDeauthReqAction(pAd, &MsgElem);
+			if (INFRA_ON(pAd))
+			{
+			    LinkDown(pAd, FALSE);
+			    pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			}
+			break;
+#endif // IW_MLME_DEAUTH //
+#ifdef IW_MLME_DISASSOC
+		case IW_MLME_DISASSOC:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DISASSOC\n", __func__));
+			COPY_MAC_ADDR(DisAssocReq.Addr, pAd->CommonCfg.Bssid);
+			DisAssocReq.Reason =  pMlme->reason_code;
+
+			MsgElem.Machine = ASSOC_STATE_MACHINE;
+			MsgElem.MsgType = MT2_MLME_DISASSOC_REQ;
+			MsgElem.MsgLen = sizeof(MLME_DISASSOC_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DisAssocReq, sizeof(MLME_DISASSOC_REQ_STRUCT));
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+			MlmeDisassocReqAction(pAd, &MsgElem);
+			break;
+#endif // IW_MLME_DISASSOC //
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - Unknow Command\n", __func__));
+			break;
+	}
+
+	return 0;
+}
+#endif // SIOCSIWMLME //
+
+#if WIRELESS_EXT > 17
+int rt_ioctl_siwauth(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = (PRTMP_ADAPTER) dev->priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+	switch (param->flags & IW_AUTH_INDEX) {
+    	case IW_AUTH_WPA_VERSION:
+            if (param->value == IW_AUTH_WPA_VERSION_WPA)
+            {
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+				if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+					pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+            }
+            else if (param->value == IW_AUTH_WPA_VERSION_WPA2)
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_PAIRWISE:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption2Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption3Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_PAIRWISE - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_GROUP:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_GROUP - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_KEY_MGMT:
+            if (param->value == IW_AUTH_KEY_MGMT_802_1X)
+            {
+                if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+                else if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+#ifdef WPA_SUPPLICANT_SUPPORT
+                else
+                    // WEP 1x
+                    pAdapter->StaCfg.IEEE8021X = TRUE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == 0)
+            {
+                //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAdapter);
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_KEY_MGMT - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+            break;
+    	case IW_AUTH_PRIVACY_INVOKED:
+            /*if (param->value == 0)
+			{
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+        	    pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+            }*/
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_PRIVACY_INVOKED - param->value = %d!\n", __func__, param->value));
+    		break;
+    	case IW_AUTH_DROP_UNENCRYPTED:
+            if (param->value != 0)
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			else
+			{
+                //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAdapter);
+			}
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __func__, param->value));
+    		break;
+    	case IW_AUTH_80211_AUTH_ALG:
+			if (param->value & IW_AUTH_ALG_SHARED_KEY)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+			}
+            else if (param->value & IW_AUTH_ALG_OPEN_SYSTEM)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+			}
+            else
+				return -EINVAL;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_80211_AUTH_ALG - param->value = %d!\n", __func__, param->value));
+			break;
+    	case IW_AUTH_WPA_ENABLED:
+    		DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_ENABLED - Driver supports WPA!(param->value = %d)\n", __func__, param->value));
+    		break;
+    	default:
+    		return -EOPNOTSUPP;
+}
+
+	return 0;
+}
+
+int rt_ioctl_giwauth(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = (PRTMP_ADAPTER) dev->priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+    }
+
+	switch (param->flags & IW_AUTH_INDEX) {
+	case IW_AUTH_DROP_UNENCRYPTED:
+        param->value = (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled) ? 0 : 1;
+		break;
+
+	case IW_AUTH_80211_AUTH_ALG:
+        param->value = (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared) ? IW_AUTH_ALG_SHARED_KEY : IW_AUTH_ALG_OPEN_SYSTEM;
+		break;
+
+	case IW_AUTH_WPA_ENABLED:
+		param->value = (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) ? 1 : 0;
+		break;
+
+	default:
+		return -EOPNOTSUPP;
+	}
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_giwauth::param->value = %d!\n", param->value));
+	return 0;
+}
+
+void fnSetCipherKey(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  INT             keyIdx,
+    IN  UCHAR           CipherAlg,
+    IN  BOOLEAN         bGTK,
+    IN  struct iw_encode_ext *ext)
+{
+    NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, LEN_TKIP_EK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].TxMic, ext->key + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].RxMic, ext->key + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CipherAlg;
+
+    // Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAdapter,
+						  BSS0,
+						  keyIdx,
+						  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+						  pAdapter->SharedKey[BSS0][keyIdx].Key,
+						  pAdapter->SharedKey[BSS0][keyIdx].TxMic,
+						  pAdapter->SharedKey[BSS0][keyIdx].RxMic);
+
+    if (bGTK)
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  NULL);
+    else
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  &pAdapter->MacTab.Content[BSSID_WCID]);
+}
+
+int rt_ioctl_siwencodeext(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+			{
+    PRTMP_ADAPTER   pAdapter = (PRTMP_ADAPTER) dev->priv;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+    int keyIdx, alg = ext->alg;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if (encoding->flags & IW_ENCODE_DISABLED)
+	{
+        keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+        // set BSSID wcid entry of the Pair-wise Key table as no-security mode
+	    AsicRemovePairwiseKeyEntry(pAdapter, BSS0, BSSID_WCID);
+        pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+		pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_NONE;
+		AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)keyIdx);
+        NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+        DBGPRINT(RT_DEBUG_TRACE, ("%s::Remove all keys!(encoding->flags = %x)\n", __func__, encoding->flags));
+    }
+					else
+    {
+        // Get Key Index and convet to our own defined key index
+    	keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+    	if((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+    		return -EINVAL;
+
+        if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+        {
+            pAdapter->StaCfg.DefaultKeyId = keyIdx;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::DefaultKeyId = %d\n", __func__, pAdapter->StaCfg.DefaultKeyId));
+        }
+
+        switch (alg) {
+    		case IW_ENCODE_ALG_NONE:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_NONE\n", __func__));
+    			break;
+    		case IW_ENCODE_ALG_WEP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_WEP - ext->key_len = %d, keyIdx = %d\n", __func__, ext->key_len, keyIdx));
+    			if (ext->key_len == MAX_WEP_KEY_SIZE)
+                {
+        			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+				}
+        		else if (ext->key_len == MIN_WEP_KEY_SIZE)
+                {
+                    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+			}
+        		else
+                    return -EINVAL;
+
+                NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+			    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, ext->key_len);
+    			break;
+            case IW_ENCODE_ALG_TKIP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_TKIP - keyIdx = %d, ext->key_len = %d\n", __func__, keyIdx, ext->key_len));
+                if (ext->key_len == 32)
+                {
+                    if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, FALSE, ext);
+                        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                        {
+                            //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+                            STA_PORT_SECURED(pAdapter);
+                        }
+		}
+                    else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, TRUE, ext);
+
+                        // set 802.1x port control
+            	        //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+            	        STA_PORT_SECURED(pAdapter);
+                    }
+                }
+                else
+                    return -EINVAL;
+                break;
+            case IW_ENCODE_ALG_CCMP:
+                if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+		{
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, FALSE, ext);
+                    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                    	//pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+                    	STA_PORT_SECURED(pAdapter);
+                }
+                else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                {
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, TRUE, ext);
+
+                    // set 802.1x port control
+        	        //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+        	        STA_PORT_SECURED(pAdapter);
+                }
+                break;
+    		default:
+    			return -EINVAL;
+		}
+    }
+
+    return 0;
+}
+
+int
+rt_ioctl_giwencodeext(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) dev->priv;
+	PCHAR pKey = NULL;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+	int idx, max_key_len;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_giwencodeext\n"));
+
+	max_key_len = encoding->length - sizeof(*ext);
+	if (max_key_len < 0)
+		return -EINVAL;
+
+	idx = encoding->flags & IW_ENCODE_INDEX;
+	if (idx)
+	{
+		if (idx < 1 || idx > 4)
+			return -EINVAL;
+		idx--;
+
+		if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+			(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled))
+		{
+			if (idx != pAd->StaCfg.DefaultKeyId)
+			{
+				ext->key_len = 0;
+				return 0;
+			}
+		}
+	}
+	else
+		idx = pAd->StaCfg.DefaultKeyId;
+
+	encoding->flags = idx + 1;
+	memset(ext, 0, sizeof(*ext));
+
+	ext->key_len = 0;
+	switch(pAd->StaCfg.WepStatus) {
+		case Ndis802_11WEPDisabled:
+			ext->alg = IW_ENCODE_ALG_NONE;
+			encoding->flags |= IW_ENCODE_DISABLED;
+			break;
+		case Ndis802_11WEPEnabled:
+			ext->alg = IW_ENCODE_ALG_WEP;
+			if (pAd->SharedKey[BSS0][idx].KeyLen > max_key_len)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = pAd->SharedKey[BSS0][idx].KeyLen;
+				pKey = &(pAd->SharedKey[BSS0][idx].Key[0]);
+			}
+			break;
+		case Ndis802_11Encryption2Enabled:
+		case Ndis802_11Encryption3Enabled:
+			if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+				ext->alg = IW_ENCODE_ALG_TKIP;
+			else
+				ext->alg = IW_ENCODE_ALG_CCMP;
+
+			if (max_key_len < 32)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = 32;
+				pKey = &pAd->StaCfg.PMK[0];
+			}
+			break;
+		default:
+			return -EINVAL;
+	}
+
+	if (ext->key_len && pKey)
+	{
+		encoding->flags |= IW_ENCODE_ENABLED;
+		memcpy(ext->key, pKey, ext->key_len);
+	}
+
+	return 0;
+}
+
+#ifdef SIOCSIWGENIE
+int rt_ioctl_siwgenie(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+
+	if (wrqu->data.length > MAX_LEN_OF_RSNIE ||
+	    (wrqu->data.length && extra == NULL))
+		return -EINVAL;
+
+	if (wrqu->data.length)
+	{
+		pAd->StaCfg.RSNIE_Len = wrqu->data.length;
+		NdisMoveMemory(&pAd->StaCfg.RSN_IE[0], extra, pAd->StaCfg.RSNIE_Len);
+	}
+	else
+	{
+		pAd->StaCfg.RSNIE_Len = 0;
+		NdisZeroMemory(&pAd->StaCfg.RSN_IE[0], MAX_LEN_OF_RSNIE);
+	}
+
+	return 0;
+}
+#endif // SIOCSIWGENIE //
+
+int rt_ioctl_giwgenie(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+
+	if ((pAd->StaCfg.RSNIE_Len == 0) ||
+		(pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA))
+	{
+		wrqu->data.length = 0;
+		return 0;
+	}
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifdef SIOCSIWGENIE
+	if (pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+	if (wrqu->data.length < pAd->StaCfg.RSNIE_Len)
+		return -E2BIG;
+
+	wrqu->data.length = pAd->StaCfg.RSNIE_Len;
+	memcpy(extra, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+	else
+#endif // SIOCSIWGENIE //
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+	{
+		UCHAR RSNIe = IE_WPA;
+
+		if (wrqu->data.length < (pAd->StaCfg.RSNIE_Len + 2)) // ID, Len
+			return -E2BIG;
+		wrqu->data.length = pAd->StaCfg.RSNIE_Len + 2;
+
+		if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2))
+			RSNIe = IE_RSN;
+
+		extra[0] = (char)RSNIe;
+		extra[1] = pAd->StaCfg.RSNIE_Len;
+		memcpy(extra+2, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+
+	return 0;
+}
+
+int rt_ioctl_siwpmksa(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+	struct iw_pmksa *pPmksa = (struct iw_pmksa *)wrqu->data.pointer;
+	INT	CachedIdx = 0, idx = 0;
+
+	if (pPmksa == NULL)
+		return -EINVAL;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_siwpmksa\n"));
+	switch(pPmksa->cmd)
+	{
+		case IW_PMKSA_FLUSH:
+			NdisZeroMemory(pAd->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_FLUSH\n"));
+			break;
+		case IW_PMKSA_REMOVE:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+		        {
+		        	NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN);
+					NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].PMKID, 16);
+					for (idx = CachedIdx; idx < (pAd->StaCfg.SavedPMKNum - 1); idx++)
+					{
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].BSSID[0], &pAd->StaCfg.SavedPMK[idx+1].BSSID[0], MAC_ADDR_LEN);
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].PMKID[0], &pAd->StaCfg.SavedPMK[idx+1].PMKID[0], 16);
+					}
+					pAd->StaCfg.SavedPMKNum--;
+			        break;
+		        }
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_REMOVE\n"));
+			break;
+		case IW_PMKSA_ADD:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+			        break;
+	        }
+
+	        // Found, replace it
+	        if (CachedIdx < PMKID_NO)
+	        {
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+		        pAd->StaCfg.SavedPMKNum++;
+	        }
+	        // Not found, replace the last one
+	        else
+	        {
+		        // Randomly replace one
+		        CachedIdx = (pPmksa->bssid.sa_data[5] % PMKID_NO);
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_ADD\n"));
+			break;
+		default:
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - Unknow Command!!\n"));
+			break;
+	}
+
+	return 0;
+}
+#endif // #if WIRELESS_EXT > 17
+
+#ifdef DBG
+static int
+rt_private_ioctl_bbp(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+			{
+	CHAR				*this_char;
+	CHAR				*value = NULL;
+	UCHAR				regBBP = 0;
+//	CHAR				arg[255]={0};
+	UINT32				bbpId;
+	UINT32				bbpValue;
+	BOOLEAN				bIsPrintAllBBP = FALSE;
+	INT					Status = 0;
+    PRTMP_ADAPTER       pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+
+	memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	if (wrq->length > 1) //No parameters.
+				{
+		sprintf(extra, "\n");
+
+		//Parsing Read or Write
+		this_char = wrq->pointer;
+		DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s\n", this_char));
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s, value=%s\n", this_char, value));
+			if (sscanf(this_char, "%d", &(bbpId)) == 1)
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		else
+		{ //Write
+			if ((sscanf(this_char, "%d", &(bbpId)) == 1) && (sscanf(value, "%x", &(bbpValue)) == 1))
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+						//Read it back for showing
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					    RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+    					//Read it back for showing
+    					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		}
+	else
+		bIsPrintAllBBP = TRUE;
+
+next:
+	if (bIsPrintAllBBP)
+	{
+		memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+		sprintf(extra, "\n");
+		for (bbpId = 0; bbpId <= 136; bbpId++)
+		{
+		    if (strlen(extra) >= (IW_PRIV_SIZE_MASK - 10))
+                break;
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+			else
+#endif // RALINK_ATE //
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X    ", bbpId, bbpId*2, regBBP);
+			if (bbpId%5 == 4)
+				sprintf(extra+strlen(extra), "\n");
+		}
+
+        wrq->length = strlen(extra) + 1; // 1: size of '\0'
+        DBGPRINT(RT_DEBUG_TRACE, ("wrq->length = %d\n", wrq->length));
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==rt_private_ioctl_bbp\n\n"));
+
+    return Status;
+}
+#endif // DBG //
+
+int rt_ioctl_siwrate(struct net_device *dev,
+			struct iw_request_info *info,
+			union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+    UINT32          rate = wrqu->bitrate.value, fixed = wrqu->bitrate.fixed;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(rate = %d, fixed = %d)\n", rate, fixed));
+    /* rate = -1 => auto rate
+       rate = X, fixed = 1 => (fixed rate X)
+    */
+    if (rate == -1)
+    {
+		//Auto Rate
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+		pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+		if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+		    (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+			RTMPSetDesiredRates(pAd, -1);
+
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+    }
+    else
+    {
+        if (fixed)
+        {
+        	pAd->StaCfg.bAutoTxRateSwitch = FALSE;
+            if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+                (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+                RTMPSetDesiredRates(pAd, rate);
+            else
+            {
+                pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+#ifdef DOT11_N_SUPPORT
+                SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(HtMcs=%d)\n",pAd->StaCfg.DesiredTransmitSetting.field.MCS));
+        }
+        else
+        {
+            // TODO: rate = X, fixed = 0 => (rates <= X)
+            return -EOPNOTSUPP;
+        }
+    }
+
+    return 0;
+}
+
+int rt_ioctl_giwrate(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+    int rate_index = 0, rate_count = 0;
+    HTTRANSMIT_SETTING ht_setting;
+    __s32 ralinkrate[] =
+	{2,  4,   11,  22, // CCK
+	12, 18,   24,  36, 48, 72, 96, 108, // OFDM
+	13, 26,   39,  52,  78, 104, 117, 130, 26,  52,  78, 104, 156, 208, 234, 260, // 20MHz, 800ns GI, MCS: 0 ~ 15
+	39, 78,  117, 156, 234, 312, 351, 390,										  // 20MHz, 800ns GI, MCS: 16 ~ 23
+	27, 54,   81, 108, 162, 216, 243, 270, 54, 108, 162, 216, 324, 432, 486, 540, // 40MHz, 800ns GI, MCS: 0 ~ 15
+	81, 162, 243, 324, 486, 648, 729, 810,										  // 40MHz, 800ns GI, MCS: 16 ~ 23
+	14, 29,   43,  57,  87, 115, 130, 144, 29, 59,   87, 115, 173, 230, 260, 288, // 20MHz, 400ns GI, MCS: 0 ~ 15
+	43, 87,  130, 173, 260, 317, 390, 433,										  // 20MHz, 400ns GI, MCS: 16 ~ 23
+	30, 60,   90, 120, 180, 240, 270, 300, 60, 120, 180, 240, 360, 480, 540, 600, // 40MHz, 400ns GI, MCS: 0 ~ 15
+	90, 180, 270, 360, 540, 720, 810, 900};										  // 40MHz, 400ns GI, MCS: 16 ~ 23
+
+    rate_count = sizeof(ralinkrate)/sizeof(__s32);
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if ((pAd->StaCfg.bAutoTxRateSwitch == FALSE) &&
+        (INFRA_ON(pAd)) &&
+        ((pAd->CommonCfg.PhyMode <= PHY_11G) || (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM)))
+        ht_setting.word = pAd->StaCfg.HTPhyMode.word;
+    else
+        ht_setting.word = pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+
+#ifdef DOT11_N_SUPPORT
+    if (ht_setting.field.MODE >= MODE_HTMIX)
+    {
+//    	rate_index = 12 + ((UCHAR)ht_setting.field.BW *16) + ((UCHAR)ht_setting.field.ShortGI *32) + ((UCHAR)ht_setting.field.MCS);
+    	rate_index = 12 + ((UCHAR)ht_setting.field.BW *24) + ((UCHAR)ht_setting.field.ShortGI *48) + ((UCHAR)ht_setting.field.MCS);
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+    if (ht_setting.field.MODE == MODE_OFDM)
+    	rate_index = (UCHAR)(ht_setting.field.MCS) + 4;
+    else if (ht_setting.field.MODE == MODE_CCK)
+    	rate_index = (UCHAR)(ht_setting.field.MCS);
+
+    if (rate_index < 0)
+        rate_index = 0;
+
+    if (rate_index > rate_count)
+        rate_index = rate_count;
+
+    wrqu->bitrate.value = ralinkrate[rate_index] * 500000;
+    wrqu->bitrate.disabled = 0;
+
+    return 0;
+}
+
+static const iw_handler rt_handler[] =
+{
+	(iw_handler) NULL,			            /* SIOCSIWCOMMIT */
+	(iw_handler) rt_ioctl_giwname,			/* SIOCGIWNAME   */
+	(iw_handler) NULL,			            /* SIOCSIWNWID   */
+	(iw_handler) NULL,			            /* SIOCGIWNWID   */
+	(iw_handler) rt_ioctl_siwfreq,		    /* SIOCSIWFREQ   */
+	(iw_handler) rt_ioctl_giwfreq,		    /* SIOCGIWFREQ   */
+	(iw_handler) rt_ioctl_siwmode,		    /* SIOCSIWMODE   */
+	(iw_handler) rt_ioctl_giwmode,		    /* SIOCGIWMODE   */
+	(iw_handler) NULL,		                /* SIOCSIWSENS   */
+	(iw_handler) NULL,		                /* SIOCGIWSENS   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWRANGE  */
+	(iw_handler) rt_ioctl_giwrange,		    /* SIOCGIWRANGE  */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWPRIV   */
+	(iw_handler) NULL /* kernel code */,    /* SIOCGIWPRIV   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWSTATS  */
+	(iw_handler) rt28xx_get_wireless_stats /* kernel code */,    /* SIOCGIWSTATS  */
+	(iw_handler) NULL,		                /* SIOCSIWSPY    */
+	(iw_handler) NULL,		                /* SIOCGIWSPY    */
+	(iw_handler) NULL,				        /* SIOCSIWTHRSPY */
+	(iw_handler) NULL,				        /* SIOCGIWTHRSPY */
+	(iw_handler) rt_ioctl_siwap,            /* SIOCSIWAP     */
+	(iw_handler) rt_ioctl_giwap,		    /* SIOCGIWAP     */
+#ifdef SIOCSIWMLME
+	(iw_handler) rt_ioctl_siwmlme,	        /* SIOCSIWMLME   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWMLME */
+#endif // SIOCSIWMLME //
+	(iw_handler) rt_ioctl_iwaplist,		    /* SIOCGIWAPLIST */
+#ifdef SIOCGIWSCAN
+	(iw_handler) rt_ioctl_siwscan,		    /* SIOCSIWSCAN   */
+	(iw_handler) rt_ioctl_giwscan,		    /* SIOCGIWSCAN   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWSCAN   */
+	(iw_handler) NULL,				        /* SIOCGIWSCAN   */
+#endif /* SIOCGIWSCAN */
+	(iw_handler) rt_ioctl_siwessid,		    /* SIOCSIWESSID  */
+	(iw_handler) rt_ioctl_giwessid,		    /* SIOCGIWESSID  */
+	(iw_handler) rt_ioctl_siwnickn,		    /* SIOCSIWNICKN  */
+	(iw_handler) rt_ioctl_giwnickn,		    /* SIOCGIWNICKN  */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) rt_ioctl_siwrate,          /* SIOCSIWRATE   */
+	(iw_handler) rt_ioctl_giwrate,          /* SIOCGIWRATE   */
+	(iw_handler) rt_ioctl_siwrts,		    /* SIOCSIWRTS    */
+	(iw_handler) rt_ioctl_giwrts,		    /* SIOCGIWRTS    */
+	(iw_handler) rt_ioctl_siwfrag,		    /* SIOCSIWFRAG   */
+	(iw_handler) rt_ioctl_giwfrag,		    /* SIOCGIWFRAG   */
+	(iw_handler) NULL,		                /* SIOCSIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCGIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCSIWRETRY  */
+	(iw_handler) NULL,		                /* SIOCGIWRETRY  */
+	(iw_handler) rt_ioctl_siwencode,		/* SIOCSIWENCODE */
+	(iw_handler) rt_ioctl_giwencode,		/* SIOCGIWENCODE */
+	(iw_handler) NULL,		                /* SIOCSIWPOWER  */
+	(iw_handler) NULL,		                /* SIOCGIWPOWER  */
+	(iw_handler) NULL,						/* -- hole -- */
+	(iw_handler) NULL,						/* -- hole -- */
+#if WIRELESS_EXT > 17
+    (iw_handler) rt_ioctl_siwgenie,         /* SIOCSIWGENIE  */
+	(iw_handler) rt_ioctl_giwgenie,         /* SIOCGIWGENIE  */
+	(iw_handler) rt_ioctl_siwauth,		    /* SIOCSIWAUTH   */
+	(iw_handler) rt_ioctl_giwauth,		    /* SIOCGIWAUTH   */
+	(iw_handler) rt_ioctl_siwencodeext,	    /* SIOCSIWENCODEEXT */
+	(iw_handler) rt_ioctl_giwencodeext,		/* SIOCGIWENCODEEXT */
+	(iw_handler) rt_ioctl_siwpmksa,         /* SIOCSIWPMKSA  */
+#endif
+};
+
+static const iw_handler rt_priv_handlers[] = {
+	(iw_handler) NULL, /* + 0x00 */
+	(iw_handler) NULL, /* + 0x01 */
+#ifndef CONFIG_AP_SUPPORT
+	(iw_handler) rt_ioctl_setparam, /* + 0x02 */
+#else
+	(iw_handler) NULL, /* + 0x02 */
+#endif // CONFIG_AP_SUPPORT //
+#ifdef DBG
+	(iw_handler) rt_private_ioctl_bbp, /* + 0x03 */
+#else
+	(iw_handler) NULL, /* + 0x03 */
+#endif
+	(iw_handler) NULL, /* + 0x04 */
+	(iw_handler) NULL, /* + 0x05 */
+	(iw_handler) NULL, /* + 0x06 */
+	(iw_handler) NULL, /* + 0x07 */
+	(iw_handler) NULL, /* + 0x08 */
+	(iw_handler) rt_private_get_statistics, /* + 0x09 */
+	(iw_handler) NULL, /* + 0x0A */
+	(iw_handler) NULL, /* + 0x0B */
+	(iw_handler) NULL, /* + 0x0C */
+	(iw_handler) NULL, /* + 0x0D */
+	(iw_handler) NULL, /* + 0x0E */
+	(iw_handler) NULL, /* + 0x0F */
+	(iw_handler) NULL, /* + 0x10 */
+	(iw_handler) rt_private_show, /* + 0x11 */
+    (iw_handler) NULL, /* + 0x12 */
+	(iw_handler) NULL, /* + 0x13 */
+	(iw_handler) NULL, /* + 0x15 */
+	(iw_handler) NULL, /* + 0x17 */
+	(iw_handler) NULL, /* + 0x18 */
+};
+
+const struct iw_handler_def rt28xx_iw_handler_def =
+{
+#define	N(a)	(sizeof (a) / sizeof (a[0]))
+	.standard	= (iw_handler *) rt_handler,
+	.num_standard	= sizeof(rt_handler) / sizeof(iw_handler),
+	.private	= (iw_handler *) rt_priv_handlers,
+	.num_private		= N(rt_priv_handlers),
+	.private_args	= (struct iw_priv_args *) privtab,
+	.num_private_args	= N(privtab),
+#if IW_HANDLER_VERSION >= 7
+    .get_wireless_stats = rt28xx_get_wireless_stats,
+#endif
+};
+
+INT RTMPSetInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_MAC_ADDRESS             Bssid;
+    RT_802_11_PHY_MODE                  PhyMode;
+    RT_802_11_STA_CONFIG                StaConfig;
+    NDIS_802_11_RATES                   aryRates;
+    RT_802_11_PREAMBLE                  Preamble;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode = Ndis802_11AuthModeMax;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    PNDIS_802_11_KEY                    pKey = NULL;
+    PNDIS_802_11_WEP			        pWepKey =NULL;
+    PNDIS_802_11_REMOVE_KEY             pRemoveKey = NULL;
+    NDIS_802_11_CONFIGURATION           Config, *pConfig = NULL;
+    NDIS_802_11_NETWORK_TYPE            NetType;
+    ULONG                               Now;
+    UINT                                KeyIdx = 0;
+    INT                                 Status = NDIS_STATUS_SUCCESS, MaxPhyMode = PHY_11G;
+    ULONG                               PowerTemp;
+    BOOLEAN                             RadioState;
+    BOOLEAN                             StateMachineTouched = FALSE;
+#ifdef DOT11_N_SUPPORT
+	OID_SET_HT_PHYMODE					HT_PhyMode;	//11n ,kathy
+#endif // DOT11_N_SUPPORT //
+#ifdef WPA_SUPPLICANT_SUPPORT
+    PNDIS_802_11_PMKID                  pPmkId = NULL;
+    BOOLEAN				                IEEE8021xState = FALSE;
+    BOOLEAN				                IEEE8021x_required_keys = FALSE;
+    UCHAR                               wpa_supplicant_enable = 0;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef SNMP_SUPPORT
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						ctmp;
+#endif // SNMP_SUPPORT //
+
+
+
+#ifdef DOT11_N_SUPPORT
+	MaxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-->RTMPSetInformation(),	0x%08x\n", cmd&0x7FFF));
+    switch(cmd & 0x7FFF) {
+        case RT_OID_802_11_COUNTRY_REGION:
+            if (wrq->u.data.length < sizeof(UCHAR))
+                Status = -EINVAL;
+			// Only avaliable when EEPROM not programming
+            else if (!(pAdapter->CommonCfg.CountryRegion & 0x80) && !(pAdapter->CommonCfg.CountryRegionForABand & 0x80))
+            {
+                ULONG   Country;
+                UCHAR	TmpPhy;
+
+				Status = copy_from_user(&Country, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->CommonCfg.CountryRegion = (UCHAR)(Country & 0x000000FF);
+				pAdapter->CommonCfg.CountryRegionForABand = (UCHAR)((Country >> 8) & 0x000000FF);
+                TmpPhy = pAdapter->CommonCfg.PhyMode;
+				pAdapter->CommonCfg.PhyMode = 0xff;
+				// Build all corresponding channel information
+				RTMPSetPhyMode(pAdapter, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+				SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_COUNTRY_REGION (A:%d  B/G:%d)\n", pAdapter->CommonCfg.CountryRegionForABand,
+				    pAdapter->CommonCfg.CountryRegion));
+            }
+            break;
+        case OID_802_11_BSSID_LIST_SCAN:
+ #ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            Now = jiffies;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID_LIST_SCAN, TxCnt = %d \n", pAdapter->RalinkCounters.LastOneSecTotalTxCount));
+
+            if (MONITOR_ON(pAdapter))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+                break;
+            }
+
+			//Benson add 20080527, when radio off, sta don't need to scan
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_RADIO_OFF))
+				break;
+
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			{
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is scanning now !!!\n"));
+				pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+				Status = NDIS_STATUS_SUCCESS;
+                break;
+            }
+
+			if (pAdapter->RalinkCounters.LastOneSecTotalTxCount > 100)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+            if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+				((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)) &&
+                (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+
+            if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+            {
+                RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+            }
+
+            // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+            // this request, because this request is initiated by NDIS.
+            pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+            // Reset allowed scan retries
+            pAdapter->StaCfg.ScanCnt = 0;
+            pAdapter->StaCfg.LastScanTime = Now;
+
+			pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+            RTMP_SET_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+            MlmeEnqueue(pAdapter,
+                        MLME_CNTL_STATE_MACHINE,
+                        OID_802_11_BSSID_LIST_SCAN,
+                        0,
+                        NULL);
+
+            Status = NDIS_STATUS_SUCCESS;
+            StateMachineTouched = TRUE;
+            break;
+        case OID_802_11_SSID:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_SSID))
+                Status = -EINVAL;
+            else
+            {
+            	PCHAR pSsidString = NULL;
+                Status = copy_from_user(&Ssid, wrq->u.data.pointer, wrq->u.data.length);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SSID (Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+                if (Ssid.SsidLength > MAX_LEN_OF_SSID)
+                    Status = -EINVAL;
+                else
+                {
+                	if (Ssid.SsidLength == 0)
+                	{
+                		Set_SSID_Proc(pAdapter, "");
+                	}
+					else
+                	{
+	                	pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+						if (pSsidString)
+						{
+							NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+							NdisMoveMemory(pSsidString, Ssid.Ssid, Ssid.SsidLength);
+	                		Set_SSID_Proc(pAdapter, pSsidString);
+							kfree(pSsidString);
+						}
+						else
+							Status = -ENOMEM;
+                	}
+                }
+            }
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            if (wrq->u.data.length != sizeof(NDIS_802_11_MAC_ADDRESS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Bssid, wrq->u.data.pointer, wrq->u.data.length);
+
+                // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+                // this request, because this request is initiated by NDIS.
+                pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+
+				// Prevent to connect AP again in STAMlmePeriodicExec
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+                // Reset allowed scan retries
+				pAdapter->StaCfg.ScanCnt = 0;
+
+                if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+                {
+                    RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                    DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+                }
+                MlmeEnqueue(pAdapter,
+                            MLME_CNTL_STATE_MACHINE,
+                            OID_802_11_BSSID,
+                            sizeof(NDIS_802_11_MAC_ADDRESS),
+                            (VOID *)&Bssid);
+                Status = NDIS_STATUS_SUCCESS;
+                StateMachineTouched = TRUE;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID %02x:%02x:%02x:%02x:%02x:%02x\n",
+                                        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+            }
+            break;
+        case RT_OID_802_11_RADIO:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RadioState, wrq->u.data.pointer, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RADIO (=%d)\n", RadioState));
+                if (pAdapter->StaCfg.bSwRadio != RadioState)
+                {
+                    pAdapter->StaCfg.bSwRadio = RadioState;
+                    if (pAdapter->StaCfg.bRadio != (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio))
+                    {
+                        pAdapter->StaCfg.bRadio = (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio);
+                        if (pAdapter->StaCfg.bRadio == TRUE)
+                        {
+                            MlmeRadioOn(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = EXTRA_INFO_CLEAR;
+                        }
+                        else
+                        {
+                            MlmeRadioOff(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = SW_RADIO_OFF;
+                        }
+                    }
+                }
+            }
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PHY_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				if (PhyMode <= MaxPhyMode)
+				{
+                	RTMPSetPhyMode(pAdapter, PhyMode);
+#ifdef DOT11_N_SUPPORT
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				}
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PHY_MODE (=%d)\n", PhyMode));
+            }
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            if (wrq->u.data.length != sizeof(RT_802_11_STA_CONFIG))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&StaConfig, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bEnableTxBurst = StaConfig.EnableTxBurst;
+                pAdapter->CommonCfg.UseBGProtection = StaConfig.UseBGProtection;
+                pAdapter->CommonCfg.bUseShortSlotTime = 1; // 2003-10-30 always SHORT SLOT capable
+                if ((pAdapter->CommonCfg.PhyMode != StaConfig.AdhocMode) &&
+					(StaConfig.AdhocMode <= MaxPhyMode))
+                {
+                    // allow dynamic change of "USE OFDM rate or not" in ADHOC mode
+                    // if setting changed, need to reset current TX rate as well as BEACON frame format
+                    if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                    {
+						pAdapter->CommonCfg.PhyMode = StaConfig.AdhocMode;
+                    	RTMPSetPhyMode(pAdapter, PhyMode);
+                        MlmeUpdateTxRates(pAdapter, FALSE, 0);
+                        MakeIbssBeacon(pAdapter);           // re-build BEACON frame
+                        AsicEnableIbssSync(pAdapter);   // copy to on-chip memory
+                    }
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_STA_CONFIG (Burst=%d, Protection=%ld,ShortSlot=%d\n",
+                                        pAdapter->CommonCfg.bEnableTxBurst,
+                                        pAdapter->CommonCfg.UseBGProtection,
+                                        pAdapter->CommonCfg.bUseShortSlotTime));
+            }
+            break;
+        case OID_802_11_DESIRED_RATES:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RATES))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&aryRates, wrq->u.data.pointer, wrq->u.data.length);
+                NdisZeroMemory(pAdapter->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+                NdisMoveMemory(pAdapter->CommonCfg.DesireRate, &aryRates, sizeof(NDIS_802_11_RATES));
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DESIRED_RATES (%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x)\n",
+                    pAdapter->CommonCfg.DesireRate[0],pAdapter->CommonCfg.DesireRate[1],
+                    pAdapter->CommonCfg.DesireRate[2],pAdapter->CommonCfg.DesireRate[3],
+                    pAdapter->CommonCfg.DesireRate[4],pAdapter->CommonCfg.DesireRate[5],
+                    pAdapter->CommonCfg.DesireRate[6],pAdapter->CommonCfg.DesireRate[7] ));
+                // Changing DesiredRate may affect the MAX TX rate we used to TX frames out
+                MlmeUpdateTxRates(pAdapter, FALSE, 0);
+            }
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PREAMBLE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Preamble, wrq->u.data.pointer, wrq->u.data.length);
+                if (Preamble == Rt802_11PreambleShort)
+                {
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleShort);
+                }
+                else if ((Preamble == Rt802_11PreambleLong) || (Preamble == Rt802_11PreambleAuto))
+                {
+                    // if user wants AUTO, initialize to LONG here, then change according to AP's
+                    // capability upon association.
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleLong);
+                }
+                else
+                {
+                    Status = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PREAMBLE (=%d)\n", Preamble));
+            }
+            break;
+        case OID_802_11_WEP_STATUS:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_WEP_STATUS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&WepStatus, wrq->u.data.pointer, wrq->u.data.length);
+                // Since TKIP, AES, WEP are all supported. It should not have any invalid setting
+                if (WepStatus <= Ndis802_11Encryption3KeyAbsent)
+                {
+                    if (pAdapter->StaCfg.WepStatus != WepStatus)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.WepStatus     = WepStatus;
+                    pAdapter->StaCfg.OrigWepStatus = WepStatus;
+                    pAdapter->StaCfg.PairCipher    = WepStatus;
+                	pAdapter->StaCfg.GroupCipher   = WepStatus;
+                }
+                else
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEP_STATUS (=%d)\n",WepStatus));
+            }
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_AUTHENTICATION_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&AuthMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (AuthMode > Ndis802_11AuthModeMax)
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                else
+                {
+                    if (pAdapter->StaCfg.AuthMode != AuthMode)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.AuthMode = AuthMode;
+                }
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_AUTHENTICATION_MODE (=%d) \n",pAdapter->StaCfg.AuthMode));
+            }
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_INFRASTRUCTURE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&BssType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (BssType == Ndis802_11IBSS)
+					Set_NetworkType_Proc(pAdapter, "Adhoc");
+				else if (BssType == Ndis802_11Infrastructure)
+					Set_NetworkType_Proc(pAdapter, "Infra");
+				else if (BssType == Ndis802_11Monitor)
+					Set_NetworkType_Proc(pAdapter, "Monitor");
+				else
+				{
+					Status  = -EINVAL;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_INFRASTRUCTURE_MODE (unknown)\n"));
+				}
+			}
+			break;
+	 case OID_802_11_REMOVE_WEP:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_WEP\n"));
+            if (wrq->u.data.length != sizeof(NDIS_802_11_KEY_INDEX))
+            {
+				Status = -EINVAL;
+            }
+            else
+            {
+				KeyIdx = *(NDIS_802_11_KEY_INDEX *) wrq->u.data.pointer;
+
+				if (KeyIdx & 0x80000000)
+				{
+					// Should never set default bit when remove key
+					Status = -EINVAL;
+				}
+				else
+				{
+					KeyIdx = KeyIdx & 0x0fffffff;
+					if (KeyIdx >= 4){
+						Status = -EINVAL;
+					}
+					else
+					{
+						pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+						pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+						AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+					}
+				}
+            }
+            break;
+        case RT_OID_802_11_RESET_COUNTERS:
+            NdisZeroMemory(&pAdapter->WlanCounters, sizeof(COUNTER_802_11));
+            NdisZeroMemory(&pAdapter->Counters8023, sizeof(COUNTER_802_3));
+            NdisZeroMemory(&pAdapter->RalinkCounters, sizeof(COUNTER_RALINK));
+            pAdapter->Counters8023.RxNoBuffer   = 0;
+			pAdapter->Counters8023.GoodReceives = 0;
+			pAdapter->Counters8023.RxNoBuffer   = 0;
+#ifdef RT2870
+			pAdapter->BulkOutComplete	= 0;
+			pAdapter->BulkOutCompleteOther= 0;
+			pAdapter->BulkOutCompleteCancel = 0;
+			pAdapter->BulkOutReq = 0;
+			pAdapter->BulkInReq= 0;
+			pAdapter->BulkInComplete = 0;
+			pAdapter->BulkInCompleteFail = 0;
+#endif // RT2870 //
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RESET_COUNTERS \n"));
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RTS_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RtsThresh, wrq->u.data.pointer, wrq->u.data.length);
+                if (RtsThresh > MAX_RTS_THRESHOLD)
+                    Status  = -EINVAL;
+                else
+                    pAdapter->CommonCfg.RtsThreshold = (USHORT)RtsThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_RTS_THRESHOLD (=%ld)\n",RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_FRAGMENTATION_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&FragThresh, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bUseZeroToDisableFragment = FALSE;
+                if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+                {
+                    if (FragThresh == 0)
+                    {
+                        pAdapter->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+                        pAdapter->CommonCfg.bUseZeroToDisableFragment = TRUE;
+                    }
+                    else
+                        Status  = -EINVAL;
+                }
+                else
+                    pAdapter->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_FRAGMENTATION_THRESHOLD (=%ld) \n",FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_POWER_MODE))
+                Status = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PowerMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (PowerMode == Ndis802_11PowerModeCAM)
+                	Set_PSMode_Proc(pAdapter, "CAM");
+                else if (PowerMode == Ndis802_11PowerModeMAX_PSP)
+                	Set_PSMode_Proc(pAdapter, "Max_PSP");
+                else if (PowerMode == Ndis802_11PowerModeFast_PSP)
+					Set_PSMode_Proc(pAdapter, "Fast_PSP");
+                else if (PowerMode == Ndis802_11PowerModeLegacy_PSP)
+					Set_PSMode_Proc(pAdapter, "Legacy_PSP");
+                else
+                    Status = -EINVAL;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_POWER_MODE (=%d)\n",PowerMode));
+            break;
+         case RT_OID_802_11_TX_POWER_LEVEL_1:
+			if (wrq->u.data.length  < sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&PowerTemp, wrq->u.data.pointer, wrq->u.data.length);
+				if (PowerTemp > 100)
+					PowerTemp = 0xffffffff;  // AUTO
+				pAdapter->CommonCfg.TxPowerDefault = PowerTemp; //keep current setting.
+					pAdapter->CommonCfg.TxPowerPercentage = pAdapter->CommonCfg.TxPowerDefault;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			}
+	        break;
+		case OID_802_11_NETWORK_TYPE_IN_USE:
+			if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_TYPE))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&NetType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (NetType == Ndis802_11DS)
+					RTMPSetPhyMode(pAdapter, PHY_11B);
+				else if (NetType == Ndis802_11OFDM24)
+					RTMPSetPhyMode(pAdapter, PHY_11BG_MIXED);
+				else if (NetType == Ndis802_11OFDM5)
+					RTMPSetPhyMode(pAdapter, PHY_11A);
+				else
+					Status = -EINVAL;
+#ifdef DOT11_N_SUPPORT
+				if (Status == NDIS_STATUS_SUCCESS)
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_NETWORK_TYPE_IN_USE (=%d)\n",NetType));
+		    }
+			break;
+        // For WPA PSK PMK key
+        case RT_OID_802_11_ADD_WPA:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!!\n"));
+            }
+            else
+            {
+                if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone) )
+                {
+                    Status = -EOPNOTSUPP;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!! [AuthMode != WPAPSK/WPA2PSK/WPANONE]\n"));
+                }
+                else if ((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone) )     // Only for WPA PSK mode
+				{
+                    NdisMoveMemory(pAdapter->StaCfg.PMK, &pKey->KeyMaterial, pKey->KeyLength);
+                    // Use RaConfig as PSK agent.
+                    // Start STA supplicant state machine
+                    if (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+                        pAdapter->StaCfg.WpaState = SS_START;
+
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+                else
+                {
+                    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_REMOVE_KEY:
+            pRemoveKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pRemoveKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pRemoveKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pRemoveKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!\n"));
+            }
+            else
+            {
+                if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+                {
+                    RTMPWPARemoveKeyProc(pAdapter, pRemoveKey);
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Remove WPA Key!!\n"));
+                }
+                else
+                {
+                    KeyIdx = pRemoveKey->KeyIndex;
+
+                    if (KeyIdx & 0x80000000)
+                    {
+                        // Should never set default bit when remove key
+                        Status  = -EINVAL;
+                        DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(Should never set default bit when remove key)\n"));
+                    }
+                    else
+                    {
+                        KeyIdx = KeyIdx & 0x0fffffff;
+                        if (KeyIdx > 3)
+                        {
+                            Status  = -EINVAL;
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(KeyId[%d] out of range)\n", KeyIdx));
+                        }
+                        else
+                        {
+                            pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+                            pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+                            AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY (id=0x%x, Len=%d-byte)\n", pRemoveKey->KeyIndex, pRemoveKey->Length));
+                        }
+                    }
+                }
+            }
+            kfree(pRemoveKey);
+            break;
+        // New for WPA
+        case OID_802_11_ADD_KEY:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY, Failed!!\n"));
+            }
+            else
+            {
+                RTMPAddKey(pAdapter, pKey);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_CONFIGURATION:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_CONFIGURATION))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Config, wrq->u.data.pointer, wrq->u.data.length);
+                pConfig = &Config;
+
+                if ((pConfig->BeaconPeriod >= 20) && (pConfig->BeaconPeriod <=400))
+                     pAdapter->CommonCfg.BeaconPeriod = (USHORT) pConfig->BeaconPeriod;
+
+                pAdapter->StaActive.AtimWin = (USHORT) pConfig->ATIMWindow;
+                MAP_KHZ_TO_CHANNEL_ID(pConfig->DSConfig, pAdapter->CommonCfg.Channel);
+                //
+				// Save the channel on MlmeAux for CntlOidRTBssidProc used.
+				//
+				pAdapter->MlmeAux.Channel = pAdapter->CommonCfg.Channel;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CONFIGURATION (BeacnPeriod=%ld,AtimW=%ld,Ch=%d)\n",
+                    pConfig->BeaconPeriod, pConfig->ATIMWindow, pAdapter->CommonCfg.Channel));
+                // Config has changed
+                pAdapter->bConfigChanged = TRUE;
+            }
+            break;
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_HT_PHYMODE:
+			if (wrq->u.data.length	!= sizeof(OID_SET_HT_PHYMODE))
+				Status = -EINVAL;
+			else
+			{
+			    POID_SET_HT_PHYMODE	pHTPhyMode = &HT_PhyMode;
+
+				Status = copy_from_user(&HT_PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::pHTPhyMode	(PhyMode = %d,TransmitNo = %d, HtMode =	%d,	ExtOffset =	%d , MCS = %d, BW =	%d,	STBC = %d, SHORTGI = %d) \n",
+				pHTPhyMode->PhyMode, pHTPhyMode->TransmitNo,pHTPhyMode->HtMode,pHTPhyMode->ExtOffset,
+				pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC,	pHTPhyMode->SHORTGI));
+				if (pAdapter->CommonCfg.PhyMode	>= PHY_11ABGN_MIXED)
+					RTMPSetHT(pAdapter,	pHTPhyMode);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_HT_PHYMODE(MCS=%d,BW=%d,SGI=%d,STBC=%d)\n",
+				pAdapter->StaCfg.HTPhyMode.field.MCS, pAdapter->StaCfg.HTPhyMode.field.BW, pAdapter->StaCfg.HTPhyMode.field.ShortGI,
+				pAdapter->StaCfg.HTPhyMode.field.STBC));
+			break;
+#endif // DOT11_N_SUPPORT //
+		case RT_OID_802_11_SET_APSD_SETTING:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				ULONG apsd ;
+				Status = copy_from_user(&apsd, wrq->u.data.pointer,	wrq->u.data.length);
+
+				/*-------------------------------------------------------------------
+				|B31~B7	|	B6~B5	 |	 B4	 |	 B3	 |	B2	 |	B1	 |	   B0		|
+				---------------------------------------------------------------------
+				| Rsvd	| Max SP Len | AC_VO | AC_VI | AC_BK | AC_BE | APSD	Capable	|
+				---------------------------------------------------------------------*/
+				pAdapter->CommonCfg.bAPSDCapable = (apsd & 0x00000001) ? TRUE :	FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BE = ((apsd	& 0x00000002) >> 1)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BK = ((apsd	& 0x00000004) >> 2)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VI = ((apsd	& 0x00000008) >> 3)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VO = ((apsd	& 0x00000010) >> 4)	? TRUE : FALSE;
+				pAdapter->CommonCfg.MaxSPLength	= (UCHAR)((apsd	& 0x00000060) >> 5);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_SETTING (apsd=0x%lx, APSDCap=%d, [BE,BK,VI,VO]=[%d/%d/%d/%d],	MaxSPLen=%d)\n", apsd, pAdapter->CommonCfg.bAPSDCapable,
+					pAdapter->CommonCfg.bAPSDAC_BE,	pAdapter->CommonCfg.bAPSDAC_BK,	pAdapter->CommonCfg.bAPSDAC_VI,	pAdapter->CommonCfg.bAPSDAC_VO,	pAdapter->CommonCfg.MaxSPLength));
+			}
+			break;
+
+		case RT_OID_802_11_SET_APSD_PSM:
+			if (wrq->u.data.length	!= sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				// Driver needs	to notify AP when PSM changes
+				Status = copy_from_user(&pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.pointer, wrq->u.data.length);
+				if (pAdapter->CommonCfg.bAPSDForcePowerSave	!= pAdapter->StaCfg.Psm)
+				{
+					MlmeSetPsmBit(pAdapter,	pAdapter->CommonCfg.bAPSDForcePowerSave);
+					RTMPSendNullFrame(pAdapter,	pAdapter->CommonCfg.TxRate,	TRUE);
+				}
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_PSM (bAPSDForcePowerSave:%d)\n",	pAdapter->CommonCfg.bAPSDForcePowerSave));
+			}
+			break;
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_SET_DLS:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				BOOLEAN	oldvalue = pAdapter->CommonCfg.bDLSCapable;
+				Status = copy_from_user(&pAdapter->CommonCfg.bDLSCapable, wrq->u.data.pointer, wrq->u.data.length);
+				if (oldvalue &&	!pAdapter->CommonCfg.bDLSCapable)
+				{
+					int	i;
+					// tear	down local dls table entry
+					for	(i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+
+					// tear	down peer dls table	entry
+					for	(i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS (=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			}
+			break;
+
+		case RT_OID_802_11_SET_DLS_PARAM:
+			if (wrq->u.data.length	!= sizeof(RT_802_11_DLS_UI))
+				Status = -EINVAL;
+			else
+			{
+				RT_802_11_DLS	Dls;
+
+				NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+				RTMPMoveMemory(&Dls, wrq->u.data.pointer, sizeof(RT_802_11_DLS_UI));
+				MlmeEnqueue(pAdapter,
+							MLME_CNTL_STATE_MACHINE,
+							RT_OID_802_11_SET_DLS_PARAM,
+							sizeof(RT_802_11_DLS),
+							&Dls);
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS_PARAM \n"));
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+		case RT_OID_802_11_SET_WMM:
+			if (wrq->u.data.length	!= sizeof(BOOLEAN))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&pAdapter->CommonCfg.bWmmCapable, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_WMM (=%d)	\n", pAdapter->CommonCfg.bWmmCapable));
+			}
+			break;
+
+		case OID_802_11_DISASSOCIATE:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+			//
+			// Set NdisRadioStateOff to	TRUE, instead of called	MlmeRadioOff.
+			// Later on, NDIS_802_11_BSSID_LIST_EX->NumberOfItems should be	0
+			// when	query OID_802_11_BSSID_LIST.
+			//
+			// TRUE:  NumberOfItems	will set to	0.
+			// FALSE: NumberOfItems	no change.
+			//
+			pAdapter->CommonCfg.NdisRadioStateOff =	TRUE;
+			// Set to immediately send the media disconnect	event
+			pAdapter->MlmeAux.CurrReqIsFromNdis	= TRUE;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DISASSOCIATE	\n"));
+
+			if (INFRA_ON(pAdapter))
+			{
+				if (pAdapter->Mlme.CntlMachine.CurrState !=	CNTL_IDLE)
+				{
+					RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+					DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME	busy, reset	MLME state machine !!!\n"));
+				}
+
+				MlmeEnqueue(pAdapter,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_DISASSOCIATE,
+					0,
+					NULL);
+
+				StateMachineTouched	= TRUE;
+			}
+			break;
+
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_IMME_BA_CAP:
+				if (wrq->u.data.length != sizeof(OID_BACAP_STRUC))
+					Status = -EINVAL;
+				else
+				{
+					OID_BACAP_STRUC Orde ;
+					Status = copy_from_user(&Orde, wrq->u.data.pointer, wrq->u.data.length);
+					if (Orde.Policy > BA_NOTUSE)
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+					}
+					else if (Orde.Policy == BA_NOTUSE)
+					{
+						pAdapter->CommonCfg.BACapability.field.Policy = BA_NOTUSE;
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs= Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+					}
+					else
+					{
+                        pAdapter->CommonCfg.BACapability.field.AutoBA = Orde.AutoBA;
+						pAdapter->CommonCfg.BACapability.field.Policy = IMMED_BA; // we only support immediate BA.
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+
+						if (pAdapter->CommonCfg.BACapability.field.RxBAWinLimit > MAX_RX_REORDERBUF)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = MAX_RX_REORDERBUF;
+
+					}
+
+					pAdapter->CommonCfg.REGBACapability.word = pAdapter->CommonCfg.BACapability.word;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(Orde.AutoBA = %d) (Policy=%d)(ReBAWinLimit=%d)(TxBAWinLimit=%d)(AutoMode=%d)\n",Orde.AutoBA, pAdapter->CommonCfg.BACapability.field.Policy,
+						pAdapter->CommonCfg.BACapability.field.RxBAWinLimit,pAdapter->CommonCfg.BACapability.field.TxBAWinLimit, pAdapter->CommonCfg.BACapability.field.AutoBA));
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(MimoPs = %d)(AmsduEnable = %d) (AmsduSize=%d)(MpduDensity=%d)\n",pAdapter->CommonCfg.DesiredHtPhy.MimoPs, pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable,
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize, pAdapter->CommonCfg.DesiredHtPhy.MpduDensity));
+				}
+
+				break;
+		case RT_OID_802_11_ADD_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, (" Set :: RT_OID_802_11_ADD_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				UCHAR		        index;
+				OID_ADD_BA_ENTRY    BA;
+				MAC_TABLE_ENTRY     *pEntry;
+
+				Status = copy_from_user(&BA, wrq->u.data.pointer, wrq->u.data.length);
+				if (BA.TID > 15)
+				{
+					Status = NDIS_STATUS_INVALID_DATA;
+					break;
+				}
+				else
+				{
+					//BATableInsertEntry
+					//As ad-hoc mode, BA pair is not limited to only BSSID. so add via OID.
+					index = BA.TID;
+					// in ad hoc mode, when adding BA pair, we should insert this entry into MACEntry too
+					pEntry = MacTableLookup(pAdapter, BA.MACAddr);
+					if (!pEntry)
+					{
+						DBGPRINT(RT_DEBUG_TRACE, ("RT_OID_802_11_ADD_IMME_BA. break on no connection.----:%x:%x\n", BA.MACAddr[4], BA.MACAddr[5]));
+						break;
+					}
+					if (BA.IsRecipient == FALSE)
+					{
+					    if (pEntry->bIAmBadAtheros == TRUE)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = 0x10;
+
+						BAOriSessionSetUp(pAdapter, pEntry, index, 0, 100, TRUE);
+					}
+					else
+					{
+						//BATableInsertEntry(pAdapter, pEntry->Aid, BA.MACAddr, 0, 0xffff, BA.TID, BA.nMSDU, BA.IsRecipient);
+					}
+
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_IMME_BA. Rec = %d. Mac = %x:%x:%x:%x:%x:%x . \n",
+						BA.IsRecipient, BA.MACAddr[0], BA.MACAddr[1], BA.MACAddr[2], BA.MACAddr[2]
+						, BA.MACAddr[4], BA.MACAddr[5]));
+				}
+			}
+			break;
+
+		case RT_OID_802_11_TEAR_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				POID_ADD_BA_ENTRY	pBA;
+				MAC_TABLE_ENTRY *pEntry;
+
+				pBA = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+				if (pBA == NULL)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA kmalloc() can't allocate enough memory\n"));
+					Status = NDIS_STATUS_FAILURE;
+				}
+				else
+				{
+					Status = copy_from_user(pBA, wrq->u.data.pointer, wrq->u.data.length);
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA(TID=%d, bAllTid=%d)\n", pBA->TID, pBA->bAllTid));
+
+					if (!pBA->bAllTid && (pBA->TID > NUM_OF_TID))
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+						break;
+					}
+
+					if (pBA->IsRecipient == FALSE)
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						DBGPRINT(RT_DEBUG_TRACE, (" pBA->IsRecipient == FALSE\n"));
+						if (pEntry)
+						{
+							DBGPRINT(RT_DEBUG_TRACE, (" pBA->pEntry\n"));
+							BAOriSessionTearDown(pAdapter, pEntry->Aid, pBA->TID, FALSE, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					else
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						if (pEntry)
+						{
+							BARecSessionTearDown( pAdapter, (UCHAR)pEntry->Aid, pBA->TID, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					kfree(pBA);
+				}
+            }
+            break;
+#endif // DOT11_N_SUPPORT //
+
+        // For WPA_SUPPLICANT to set static wep key
+    	case OID_802_11_ADD_WEP:
+    	    pWepKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+    	    if(pWepKey == NULL)
+            {
+                Status = -ENOMEM;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed!!\n"));
+                break;
+            }
+            Status = copy_from_user(pWepKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (Status)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (length mismatch)!!\n"));
+            }
+            else
+            {
+		        KeyIdx = pWepKey->KeyIndex & 0x0fffffff;
+                // KeyIdx must be 0 ~ 3
+                if (KeyIdx > 4)
+    			{
+                    Status  = -EINVAL;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (KeyIdx must be smaller than 4)!!\n"));
+                }
+                else
+                {
+                    UCHAR CipherAlg = 0;
+                    PUCHAR Key;
+
+                    // set key material and key length
+                    NdisZeroMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, 16);
+                    pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                    NdisMoveMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+
+                    switch(pWepKey->KeyLength)
+                    {
+                        case 5:
+                            CipherAlg = CIPHER_WEP64;
+                            break;
+                        case 13:
+                            CipherAlg = CIPHER_WEP128;
+                            break;
+                        default:
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, only support CIPHER_WEP64(len:5) & CIPHER_WEP128(len:13)!!\n"));
+                            Status = -EINVAL;
+                            break;
+                    }
+                    pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CipherAlg;
+
+                    // Default key for tx (shared key)
+                    if (pWepKey->KeyIndex & 0x80000000)
+                    {
+#ifdef WPA_SUPPLICANT_SUPPORT
+                        // set key material and key length
+                        NdisZeroMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, 16);
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                        NdisMoveMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+                        pAdapter->StaCfg.DesireSharedKeyId = KeyIdx;
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].CipherAlg = CipherAlg;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                        pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+                    }
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    if (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED)
+#endif // WPA_SUPPLICANT_SUPPORT
+                    {
+                        Key = pAdapter->SharedKey[BSS0][KeyIdx].Key;
+
+                        // Set key material and cipherAlg to Asic
+        				AsicAddSharedKeyEntry(pAdapter, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+                        if (pWepKey->KeyIndex & 0x80000000)
+                        {
+                            PMAC_TABLE_ENTRY pEntry = &pAdapter->MacTab.Content[BSSID_WCID];
+                            // Assign group key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, NULL);
+    						// Assign pairwise key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, pEntry);
+                        }
+                    }
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP (id=0x%x, Len=%d-byte), %s\n", pWepKey->KeyIndex, pWepKey->KeyLength, (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED) ? "Port Secured":"Port NOT Secured"));
+				}
+            }
+            kfree(pWepKey);
+            break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+	    case OID_SET_COUNTERMEASURES:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.bBlockAssoc = TRUE;
+                else
+                    // WPA MIC error should block association attempt for 60 seconds
+                    pAdapter->StaCfg.bBlockAssoc = FALSE;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_SET_COUNTERMEASURES bBlockAssoc=%s\n", pAdapter->StaCfg.bBlockAssoc ? "TRUE":"FALSE"));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+			if (wrq->u.data.length != sizeof(UCHAR))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&wpa_supplicant_enable, wrq->u.data.pointer, wrq->u.data.length);
+    			pAdapter->StaCfg.WpaSupplicantUP = wpa_supplicant_enable;
+    			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+			}
+            break;
+        case OID_802_11_DEAUTHENTICATION:
+            if (wrq->u.data.length != sizeof(MLME_DEAUTH_REQ_STRUCT))
+                Status  = -EINVAL;
+            else
+            {
+                MLME_DEAUTH_REQ_STRUCT      *pInfo;
+				MLME_QUEUE_ELEM *MsgElem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+                pInfo = (MLME_DEAUTH_REQ_STRUCT *) MsgElem->Msg;
+                Status = copy_from_user(pInfo, wrq->u.data.pointer, wrq->u.data.length);
+                MlmeDeauthReqAction(pAdapter, MsgElem);
+				kfree(MsgElem);
+
+                if (INFRA_ON(pAdapter))
+                {
+                    LinkDown(pAdapter, FALSE);
+                    pAdapter->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DEAUTHENTICATION (Reason=%d)\n", pInfo->Reason));
+            }
+            break;
+        case OID_802_11_DROP_UNENCRYPTED:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                else
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				NdisAcquireSpinLock(&pAdapter->MacTabLock);
+				pAdapter->MacTab.Content[BSSID_WCID].PortSecured = pAdapter->StaCfg.PortSecured;
+				NdisReleaseSpinLock(&pAdapter->MacTabLock);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DROP_UNENCRYPTED (=%d)\n", enabled));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021xState, wrq->u.data.pointer, wrq->u.data.length);
+		        pAdapter->StaCfg.IEEE8021X = IEEE8021xState;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X (=%d)\n", IEEE8021xState));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X_REQUIRE_KEY:
+			if (wrq->u.data.length != sizeof(BOOLEAN))
+				 Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021x_required_keys, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->StaCfg.IEEE8021x_required_keys = IEEE8021x_required_keys;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X_REQUIRE_KEY (%d)\n", IEEE8021x_required_keys));
+			}
+			break;
+        case OID_802_11_PMKID:
+	        pPmkId = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+	        if(pPmkId == NULL) {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pPmkId, wrq->u.data.pointer, wrq->u.data.length);
+
+	        // check the PMKID information
+	        if (pPmkId->BSSIDInfoCount == 0)
+                NdisZeroMemory(pAdapter->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+	        else
+	        {
+		        PBSSID_INFO	pBssIdInfo;
+		        UINT		BssIdx;
+		        UINT		CachedIdx;
+
+		        for (BssIdx = 0; BssIdx < pPmkId->BSSIDInfoCount; BssIdx++)
+		        {
+			        // point to the indexed BSSID_INFO structure
+			        pBssIdInfo = (PBSSID_INFO) ((PUCHAR) pPmkId + 2 * sizeof(UINT) + BssIdx * sizeof(BSSID_INFO));
+			        // Find the entry in the saved data base.
+			        for (CachedIdx = 0; CachedIdx < pAdapter->StaCfg.SavedPMKNum; CachedIdx++)
+			        {
+				        // compare the BSSID
+				        if (NdisEqualMemory(pBssIdInfo->BSSID, pAdapter->StaCfg.SavedPMK[CachedIdx].BSSID, sizeof(NDIS_802_11_MAC_ADDRESS)))
+					        break;
+			        }
+
+			        // Found, replace it
+			        if (CachedIdx < PMKID_NO)
+			        {
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+				        pAdapter->StaCfg.SavedPMKNum++;
+			        }
+			        // Not found, replace the last one
+			        else
+			        {
+				        // Randomly replace one
+				        CachedIdx = (pBssIdInfo->BSSID[5] % PMKID_NO);
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+			        }
+		        }
+			}
+			if(pPmkId)
+				kfree(pPmkId);
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+#ifdef SNMP_SUPPORT
+		case OID_802_11_SHORTRETRYLIMIT:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ShortRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SHORTRETRYLIMIT (tx_rty_cfg.field.ShortRetryLimit=%d, ShortRetryLimit=%ld)\n", tx_rty_cfg.field.ShortRtyLimit, ShortRetryLimit));
+			}
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT \n"));
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&LongRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT (tx_rty_cfg.field.LongRetryLimit= %d,LongRetryLimit=%ld)\n", tx_rty_cfg.field.LongRtyLimit, LongRetryLimit));
+			}
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE\n"));
+			pKey = kmalloc(wrq->u.data.length, GFP_KERNEL);
+			Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+			//pKey = &WepKey;
+
+			if ( pKey->Length != wrq->u.data.length)
+			{
+				Status = -EINVAL;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE, Failed!!\n"));
+			}
+			KeyIdx = pKey->KeyIndex & 0x0fffffff;
+			DBGPRINT(RT_DEBUG_TRACE,("pKey->KeyIndex =%d, pKey->KeyLength=%d\n", pKey->KeyIndex, pKey->KeyLength));
+
+			// it is a shared key
+			if (KeyIdx > 4)
+				Status = -EINVAL;
+			else
+			{
+				pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, &pKey->KeyMaterial, pKey->KeyLength);
+				if (pKey->KeyIndex & 0x80000000)
+				{
+					// Default key for tx (shared key)
+					pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+				}
+				//RestartAPIsRequired = TRUE;
+			}
+			break;
+
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYID \n"));
+
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+				Status = copy_from_user(&pAdapter->StaCfg.DefaultKeyId, wrq->u.data.pointer, wrq->u.data.length);
+
+			break;
+
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CURRENTCHANNEL \n"));
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ctmp, wrq->u.data.pointer, wrq->u.data.length);
+				sprintf(&ctmp,"%d", ctmp);
+				Set_Channel_Proc(pAdapter, &ctmp);
+			}
+			break;
+#endif
+
+
+
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+
+
+    return Status;
+}
+
+INT RTMPQueryInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_BSSID_LIST_EX           *pBssidList = NULL;
+    PNDIS_WLAN_BSSID_EX                 pBss;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_CONFIGURATION           *pConfiguration = NULL;
+    RT_802_11_LINK_STATUS               *pLinkStatus = NULL;
+    RT_802_11_STA_CONFIG                *pStaConfig = NULL;
+    NDIS_802_11_STATISTICS              *pStatistics = NULL;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    RT_802_11_PREAMBLE                  PreamType;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_MEDIA_STATE                    MediaState;
+    ULONG                               BssBufSize, ulInfo=0, NetworkTypeList[4], apsd = 0;
+    USHORT                              BssLen = 0;
+    PUCHAR                              pBuf = NULL, pPtr;
+    INT                                 Status = NDIS_STATUS_SUCCESS;
+    UINT                                we_version_compiled;
+    UCHAR                               i, Padding = 0;
+    BOOLEAN                             RadioState;
+	UCHAR	driverVersion[8];
+    OID_SET_HT_PHYMODE			        *pHTPhyMode = NULL;
+
+
+#ifdef SNMP_SUPPORT
+	//for snmp, kathy
+	DefaultKeyIdxValue			*pKeyIdxValue;
+	INT							valueLen;
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						tmp[64];
+#endif //SNMP
+
+    switch(cmd)
+    {
+        case RT_OID_DEVICE_NAME:
+            wrq->u.data.length = sizeof(STA_NIC_DEVICE_NAME);
+            Status = copy_to_user(wrq->u.data.pointer, STA_NIC_DEVICE_NAME, wrq->u.data.length);
+            break;
+        case RT_OID_VERSION_INFO:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_VERSION_INFO \n"));
+			wrq->u.data.length = 8*sizeof(UCHAR);
+			sprintf(&driverVersion[0], "%s", STA_DRIVER_VERSION);
+			driverVersion[7] = '\0';
+			if (copy_to_user(wrq->u.data.pointer, &driverVersion, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+#ifdef RALINK_ATE
+		case RT_QUERY_ATE_TXDONE_COUNT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_QUERY_ATE_TXDONE_COUNT \n"));
+			wrq->u.data.length = sizeof(UINT32);
+			if (copy_to_user(wrq->u.data.pointer, &pAdapter->ate.TxDoneCount, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+			break;
+#endif // RALINK_ATE //
+        case OID_802_11_BSSID_LIST:
+            if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+            	/*
+            	 * Still scanning, indicate the caller should try again.
+            	 */
+            	DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (Still scanning)\n"));
+				return -EAGAIN;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (%d BSS returned)\n",pAdapter->ScanTab.BssNr));
+			pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+            // Claculate total buffer size required
+            BssBufSize = sizeof(ULONG);
+
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                // Align pointer to 4 bytes boundary.
+                //Padding = 4 - (pAdapter->ScanTab.BssEntry[i].VarIELen & 0x0003);
+                //if (Padding == 4)
+                //    Padding = 0;
+                BssBufSize += (sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+            }
+
+            // For safety issue, we add 256 bytes just in case
+            BssBufSize += 256;
+            // Allocate the same size as passed from higher layer
+            pBuf = kmalloc(BssBufSize, MEM_ALLOC_FLAG);
+            if(pBuf == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            // Init 802_11_BSSID_LIST_EX structure
+            NdisZeroMemory(pBuf, BssBufSize);
+            pBssidList = (PNDIS_802_11_BSSID_LIST_EX) pBuf;
+            pBssidList->NumberOfItems = pAdapter->ScanTab.BssNr;
+
+            // Calculate total buffer length
+            BssLen = 4; // Consist of NumberOfItems
+            // Point to start of NDIS_WLAN_BSSID_EX
+            // pPtr = pBuf + sizeof(ULONG);
+            pPtr = (PUCHAR) &pBssidList->Bssid[0];
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                pBss = (PNDIS_WLAN_BSSID_EX) pPtr;
+                NdisMoveMemory(&pBss->MacAddress, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+                if ((pAdapter->ScanTab.BssEntry[i].Hidden == 1) && (pAdapter->StaCfg.bShowHiddenSSID == FALSE))
+                {
+                    //
+					// We must return this SSID during 4way handshaking, otherwise Aegis will failed to parse WPA infomation
+					// and then failed to send EAPOl farame.
+					//
+					if ((pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) && (pAdapter->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED))
+					{
+						pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+						NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+					}
+					else
+                    	pBss->Ssid.SsidLength = 0;
+                }
+                else
+                {
+                    pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+                    NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+                }
+                pBss->Privacy = pAdapter->ScanTab.BssEntry[i].Privacy;
+                pBss->Rssi = pAdapter->ScanTab.BssEntry[i].Rssi - pAdapter->BbpRssiToDbmDelta;
+                pBss->NetworkTypeInUse = NetworkTypeInUseSanity(&pAdapter->ScanTab.BssEntry[i]);
+                pBss->Configuration.Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pBss->Configuration.BeaconPeriod = pAdapter->ScanTab.BssEntry[i].BeaconPeriod;
+                pBss->Configuration.ATIMWindow = pAdapter->ScanTab.BssEntry[i].AtimWin;
+
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->ScanTab.BssEntry[i].Channel, pBss->Configuration.DSConfig);
+
+                if (pAdapter->ScanTab.BssEntry[i].BssType == BSS_INFRA)
+                    pBss->InfrastructureMode = Ndis802_11Infrastructure;
+                else
+                    pBss->InfrastructureMode = Ndis802_11IBSS;
+
+                NdisMoveMemory(pBss->SupportedRates, pAdapter->ScanTab.BssEntry[i].SupRate, pAdapter->ScanTab.BssEntry[i].SupRateLen);
+                NdisMoveMemory(pBss->SupportedRates + pAdapter->ScanTab.BssEntry[i].SupRateLen,
+                               pAdapter->ScanTab.BssEntry[i].ExtRate,
+                               pAdapter->ScanTab.BssEntry[i].ExtRateLen);
+
+                if (pAdapter->ScanTab.BssEntry[i].VarIELen == 0)
+                {
+                    pBss->IELength = sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                }
+                else
+                {
+                    pBss->IELength = (ULONG)(sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    NdisMoveMemory(pBss->IEs + sizeof(NDIS_802_11_FIXED_IEs), pAdapter->ScanTab.BssEntry[i].VarIEs, pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr += pAdapter->ScanTab.BssEntry[i].VarIELen;
+                }
+                pBss->Length = (ULONG)(sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+
+#if WIRELESS_EXT < 17
+                if ((BssLen + pBss->Length) < wrq->u.data.length)
+                BssLen += pBss->Length;
+                else
+                {
+                    pBssidList->NumberOfItems = i;
+                    break;
+                }
+#else
+                BssLen += pBss->Length;
+#endif
+            }
+
+#if WIRELESS_EXT < 17
+            wrq->u.data.length = BssLen;
+#else
+            if (BssLen > wrq->u.data.length)
+            {
+                kfree(pBssidList);
+                return -E2BIG;
+            }
+            else
+                wrq->u.data.length = BssLen;
+#endif
+            Status = copy_to_user(wrq->u.data.pointer, pBssidList, BssLen);
+            kfree(pBssidList);
+            break;
+        case OID_802_3_CURRENT_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+            break;
+        case OID_GEN_MEDIA_CONNECT_STATUS:
+            if (pAdapter->IndicateMediaState == NdisMediaStateConnected)
+                MediaState = NdisMediaStateConnected;
+            else
+                MediaState = NdisMediaStateDisconnected;
+
+            wrq->u.data.length = sizeof(NDIS_MEDIA_STATE);
+            Status = copy_to_user(wrq->u.data.pointer, &MediaState, wrq->u.data.length);
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				Status = NDIS_STATUS_RESOURCES;
+				break;
+			}
+#endif // RALINK_ATE //
+            if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+            {
+                Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Bssid, sizeof(NDIS_802_11_MAC_ADDRESS));
+
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID(=EMPTY)\n"));
+                Status = -ENOTCONN;
+            }
+            break;
+        case OID_802_11_SSID:
+			NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+			NdisZeroMemory(Ssid.Ssid, MAX_LEN_OF_SSID);
+            Ssid.SsidLength = pAdapter->CommonCfg.SsidLen;
+			memcpy(Ssid.Ssid, pAdapter->CommonCfg.Ssid,	Ssid.SsidLength);
+            wrq->u.data.length = sizeof(NDIS_802_11_SSID);
+            Status = copy_to_user(wrq->u.data.pointer, &Ssid, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SSID (Len=%d, ssid=%s)\n", Ssid.SsidLength,Ssid.Ssid));
+            break;
+        case RT_OID_802_11_QUERY_LINK_STATUS:
+            pLinkStatus = (RT_802_11_LINK_STATUS *) kmalloc(sizeof(RT_802_11_LINK_STATUS), MEM_ALLOC_FLAG);
+            if (pLinkStatus)
+            {
+                pLinkStatus->CurrTxRate = RateIdTo500Kbps[pAdapter->CommonCfg.TxRate];   // unit : 500 kbps
+                pLinkStatus->ChannelQuality = pAdapter->Mlme.ChannelQuality;
+                pLinkStatus->RxByteCount = pAdapter->RalinkCounters.ReceivedByteCount;
+                pLinkStatus->TxByteCount = pAdapter->RalinkCounters.TransmittedByteCount;
+        		pLinkStatus->CentralChannel = pAdapter->CommonCfg.CentralChannel;
+                wrq->u.data.length = sizeof(RT_802_11_LINK_STATUS);
+                Status = copy_to_user(wrq->u.data.pointer, pLinkStatus, wrq->u.data.length);
+                kfree(pLinkStatus);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS\n"));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_CONFIGURATION:
+            pConfiguration = (NDIS_802_11_CONFIGURATION *) kmalloc(sizeof(NDIS_802_11_CONFIGURATION), MEM_ALLOC_FLAG);
+            if (pConfiguration)
+            {
+                pConfiguration->Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pConfiguration->BeaconPeriod = pAdapter->CommonCfg.BeaconPeriod;
+                pConfiguration->ATIMWindow = pAdapter->StaActive.AtimWin;
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->CommonCfg.Channel, pConfiguration->DSConfig);
+                wrq->u.data.length = sizeof(NDIS_802_11_CONFIGURATION);
+                Status = copy_to_user(wrq->u.data.pointer, pConfiguration, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(BeaconPeriod=%ld,AtimW=%ld,Channel=%d) \n",
+                                        pConfiguration->BeaconPeriod, pConfiguration->ATIMWindow, pAdapter->CommonCfg.Channel));
+				kfree(pConfiguration);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+		case RT_OID_802_11_SNR_0:
+			if ((pAdapter->StaCfg.LastSNR0 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR0) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_SNR_0(0x=%lx)\n", ulInfo));
+			}
+            else
+			    Status = -EFAULT;
+			break;
+		case RT_OID_802_11_SNR_1:
+			if ((pAdapter->Antenna.field.RxPath	> 1) &&
+                (pAdapter->StaCfg.LastSNR1 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR1) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(0x=%lx)\n",ulInfo));
+			}
+			else
+				Status = -EFAULT;
+            DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(pAdapter->StaCfg.LastSNR1=%d)\n",pAdapter->StaCfg.LastSNR1));
+			break;
+        case OID_802_11_RSSI_TRIGGER:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0 - pAdapter->BbpRssiToDbmDelta;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RSSI_TRIGGER(=%ld)\n", ulInfo));
+            break;
+		case OID_802_11_RSSI:
+        case RT_OID_802_11_RSSI:
+			ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+		case RT_OID_802_11_RSSI_1:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi1;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case RT_OID_802_11_RSSI_2:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi2;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case OID_802_11_STATISTICS:
+            pStatistics = (NDIS_802_11_STATISTICS *) kmalloc(sizeof(NDIS_802_11_STATISTICS), MEM_ALLOC_FLAG);
+            if (pStatistics)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS \n"));
+                // add the most up-to-date h/w raw counters into software counters
+			    NICUpdateRawCounters(pAdapter);
+
+                // Sanity check for calculation of sucessful count
+                if (pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart < pAdapter->WlanCounters.RetryCount.QuadPart)
+                    pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+
+                pStatistics->TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart;
+                pStatistics->MulticastTransmittedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastTransmittedFrameCount.QuadPart;
+                pStatistics->FailedCount.QuadPart = pAdapter->WlanCounters.FailedCount.QuadPart;
+                pStatistics->RetryCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+                pStatistics->MultipleRetryCount.QuadPart = pAdapter->WlanCounters.MultipleRetryCount.QuadPart;
+                pStatistics->RTSSuccessCount.QuadPart = pAdapter->WlanCounters.RTSSuccessCount.QuadPart;
+                pStatistics->RTSFailureCount.QuadPart = pAdapter->WlanCounters.RTSFailureCount.QuadPart;
+                pStatistics->ACKFailureCount.QuadPart = pAdapter->WlanCounters.ACKFailureCount.QuadPart;
+                pStatistics->FrameDuplicateCount.QuadPart = pAdapter->WlanCounters.FrameDuplicateCount.QuadPart;
+                pStatistics->ReceivedFragmentCount.QuadPart = pAdapter->WlanCounters.ReceivedFragmentCount.QuadPart;
+                pStatistics->MulticastReceivedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastReceivedFrameCount.QuadPart;
+#ifdef DBG
+                pStatistics->FCSErrorCount = pAdapter->RalinkCounters.RealFcsErrCount;
+#else
+                pStatistics->FCSErrorCount.QuadPart = pAdapter->WlanCounters.FCSErrorCount.QuadPart;
+                pStatistics->FrameDuplicateCount.u.LowPart = pAdapter->WlanCounters.FrameDuplicateCount.u.LowPart / 100;
+#endif
+                wrq->u.data.length = sizeof(NDIS_802_11_STATISTICS);
+                Status = copy_to_user(wrq->u.data.pointer, pStatistics, wrq->u.data.length);
+                kfree(pStatistics);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_GEN_RCV_OK:
+            ulInfo = pAdapter->Counters8023.GoodReceives;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case OID_GEN_RCV_NO_BUFFER:
+            ulInfo = pAdapter->Counters8023.RxNoBuffer;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            ulInfo = (ULONG)pAdapter->CommonCfg.PhyMode;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PHY_MODE (=%ld)\n", ulInfo));
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            pStaConfig = (RT_802_11_STA_CONFIG *) kmalloc(sizeof(RT_802_11_STA_CONFIG), MEM_ALLOC_FLAG);
+            if (pStaConfig)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG\n"));
+                pStaConfig->EnableTxBurst = pAdapter->CommonCfg.bEnableTxBurst;
+                pStaConfig->EnableTurboRate = 0;
+                pStaConfig->UseBGProtection = pAdapter->CommonCfg.UseBGProtection;
+                pStaConfig->UseShortSlotTime = pAdapter->CommonCfg.bUseShortSlotTime;
+                //pStaConfig->AdhocMode = pAdapter->StaCfg.AdhocMode;
+                pStaConfig->HwRadioStatus = (pAdapter->StaCfg.bHwRadio == TRUE) ? 1 : 0;
+                pStaConfig->Rsv1 = 0;
+                pStaConfig->SystemErrorBitmap = pAdapter->SystemErrorBitmap;
+                wrq->u.data.length = sizeof(RT_802_11_STA_CONFIG);
+                Status = copy_to_user(wrq->u.data.pointer, pStaConfig, wrq->u.data.length);
+                kfree(pStaConfig);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            RtsThresh = pAdapter->CommonCfg.RtsThreshold;
+            wrq->u.data.length = sizeof(RtsThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &RtsThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RTS_THRESHOLD(=%ld)\n", RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            FragThresh = pAdapter->CommonCfg.FragmentThreshold;
+            if (pAdapter->CommonCfg.bUseZeroToDisableFragment == TRUE)
+                FragThresh = 0;
+            wrq->u.data.length = sizeof(FragThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &FragThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_FRAGMENTATION_THRESHOLD(=%ld)\n", FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            PowerMode = pAdapter->StaCfg.WindowsPowerMode;
+            wrq->u.data.length = sizeof(PowerMode);
+            Status = copy_to_user(wrq->u.data.pointer, &PowerMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_POWER_MODE(=%d)\n", PowerMode));
+            break;
+        case RT_OID_802_11_RADIO:
+            RadioState = (BOOLEAN) pAdapter->StaCfg.bSwRadio;
+            wrq->u.data.length = sizeof(RadioState);
+            Status = copy_to_user(wrq->u.data.pointer, &RadioState, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_RADIO (=%d)\n", RadioState));
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                BssType = Ndis802_11IBSS;
+            else if (pAdapter->StaCfg.BssType == BSS_INFRA)
+                BssType = Ndis802_11Infrastructure;
+            else if (pAdapter->StaCfg.BssType == BSS_MONITOR)
+                BssType = Ndis802_11Monitor;
+            else
+                BssType = Ndis802_11AutoUnknown;
+
+            wrq->u.data.length = sizeof(BssType);
+            Status = copy_to_user(wrq->u.data.pointer, &BssType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_INFRASTRUCTURE_MODE(=%d)\n", BssType));
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            PreamType = pAdapter->CommonCfg.TxPreamble;
+            wrq->u.data.length = sizeof(PreamType);
+            Status = copy_to_user(wrq->u.data.pointer, &PreamType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PREAMBLE(=%d)\n", PreamType));
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            AuthMode = pAdapter->StaCfg.AuthMode;
+            wrq->u.data.length = sizeof(AuthMode);
+            Status = copy_to_user(wrq->u.data.pointer, &AuthMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_AUTHENTICATION_MODE(=%d)\n", AuthMode));
+            break;
+        case OID_802_11_WEP_STATUS:
+            WepStatus = pAdapter->StaCfg.WepStatus;
+            wrq->u.data.length = sizeof(WepStatus);
+            Status = copy_to_user(wrq->u.data.pointer, &WepStatus, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEP_STATUS(=%d)\n", WepStatus));
+            break;
+        case OID_802_11_TX_POWER_LEVEL:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPower, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_TX_POWER_LEVEL %x\n",pAdapter->CommonCfg.TxPower));
+			break;
+        case RT_OID_802_11_TX_POWER_LEVEL_1:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPowerPercentage, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			break;
+        case OID_802_11_NETWORK_TYPES_SUPPORTED:
+			if ((pAdapter->RfIcType	== RFIC_2850) || (pAdapter->RfIcType ==	RFIC_2750))
+			{
+				NetworkTypeList[0] = 3;                 // NumberOfItems = 3
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+				NetworkTypeList[3] = Ndis802_11OFDM5;   // NetworkType[3] = 11a
+                wrq->u.data.length = 16;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			else
+			{
+				NetworkTypeList[0] = 2;                 // NumberOfItems = 2
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+			    wrq->u.data.length = 12;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_NETWORK_TYPES_SUPPORTED\n"));
+				break;
+	    case OID_802_11_NETWORK_TYPE_IN_USE:
+            wrq->u.data.length = sizeof(ULONG);
+			if (pAdapter->CommonCfg.PhyMode == PHY_11A)
+				ulInfo = Ndis802_11OFDM5;
+			else if ((pAdapter->CommonCfg.PhyMode == PHY_11BG_MIXED) || (pAdapter->CommonCfg.PhyMode == PHY_11G))
+				ulInfo = Ndis802_11OFDM24;
+			else
+				ulInfo = Ndis802_11DS;
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+        case RT_OID_802_11_QUERY_LAST_RX_RATE:
+            ulInfo = (ULONG)pAdapter->LastRxRate;
+            wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_RX_RATE (=%ld)\n", ulInfo));
+			break;
+		case RT_OID_802_11_QUERY_LAST_TX_RATE:
+			//ulInfo = (ULONG)pAdapter->LastTxRate;
+			ulInfo = (ULONG)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_TX_RATE (=%lx)\n", ulInfo));
+			break;
+        case RT_OID_802_11_QUERY_EEPROM_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->EepromVersion, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_QUERY_FIRMWARE_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->FirmwareVersion, wrq->u.data.length);
+			break;
+	    case RT_OID_802_11_QUERY_NOISE_LEVEL:
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->BbpWriteLatch[66], wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_NOISE_LEVEL (=%d)\n", pAdapter->BbpWriteLatch[66]));
+			break;
+	    case RT_OID_802_11_EXTRA_INFO:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->ExtraInfo, wrq->u.data.length);
+	        DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_EXTRA_INFO (=%ld)\n", pAdapter->ExtraInfo));
+	        break;
+	    case RT_OID_WE_VERSION_COMPILED:
+	        wrq->u.data.length = sizeof(UINT);
+	        we_version_compiled = WIRELESS_EXT;
+	        Status = copy_to_user(wrq->u.data.pointer, &we_version_compiled, wrq->u.data.length);
+	        break;
+		case RT_OID_802_11_QUERY_APSD_SETTING:
+			apsd = (pAdapter->CommonCfg.bAPSDCapable | (pAdapter->CommonCfg.bAPSDAC_BE << 1) | (pAdapter->CommonCfg.bAPSDAC_BK << 2)
+				| (pAdapter->CommonCfg.bAPSDAC_VI << 3)	| (pAdapter->CommonCfg.bAPSDAC_VO << 4)	| (pAdapter->CommonCfg.MaxSPLength << 5));
+
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &apsd, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_SETTING (=0x%lx,APSDCap=%d,AC_BE=%d,AC_BK=%d,AC_VI=%d,AC_VO=%d,MAXSPLen=%d)\n",
+				apsd,pAdapter->CommonCfg.bAPSDCapable,pAdapter->CommonCfg.bAPSDAC_BE,pAdapter->CommonCfg.bAPSDAC_BK,pAdapter->CommonCfg.bAPSDAC_VI,pAdapter->CommonCfg.bAPSDAC_VO,pAdapter->CommonCfg.MaxSPLength));
+			break;
+		case RT_OID_802_11_QUERY_APSD_PSM:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_PSM (=%d)\n", pAdapter->CommonCfg.bAPSDForcePowerSave));
+			break;
+		case RT_OID_802_11_QUERY_WMM:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bWmmCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_WMM (=%d)\n",	pAdapter->CommonCfg.bWmmCapable));
+			break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+        case RT_OID_NEW_DRIVER:
+            {
+                UCHAR enabled = 1;
+    	        wrq->u.data.length = sizeof(UCHAR);
+    	        Status = copy_to_user(wrq->u.data.pointer, &enabled, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_NEW_DRIVER (=%d)\n", enabled));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+	        wrq->u.data.length = sizeof(UCHAR);
+	        Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.WpaSupplicantUP, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+        case RT_OID_DRIVER_DEVICE_NAME:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_DRIVER_DEVICE_NAME \n"));
+			wrq->u.data.length = 16;
+			if (copy_to_user(wrq->u.data.pointer, pAdapter->StaCfg.dev_name, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+            break;
+        case RT_OID_802_11_QUERY_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.STBC;
+
+    			pHTPhyMode->ExtOffset = ((pAdapter->CommonCfg.CentralChannel < pAdapter->CommonCfg.Channel) ? (EXTCHA_BELOW) : (EXTCHA_ABOVE));
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_COUNTRY_REGION:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_COUNTRY_REGION \n"));
+			wrq->u.data.length = sizeof(ulInfo);
+            ulInfo = pAdapter->CommonCfg.CountryRegionForABand;
+            ulInfo = (ulInfo << 8)|(pAdapter->CommonCfg.CountryRegion);
+			if (copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_QUERY_DAT_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.HTMODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->StaCfg.DesiredTransmitSetting.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.STBC;
+
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_QUERY_MULTIPLE_CARD_SUPPORT:
+			wrq->u.data.length = sizeof(UCHAR);
+            i = 0;
+#ifdef MULTIPLE_CARD_SUPPORT
+            i = 1;
+#endif // MULTIPLE_CARD_SUPPORT //
+			if (copy_to_user(wrq->u.data.pointer, &i, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_QUERY_MULTIPLE_CARD_SUPPORT(=%d) \n", i));
+            break;
+#ifdef SNMP_SUPPORT
+		case RT_OID_802_11_MAC_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREROUI:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREROUI \n"));
+			wrq->u.data.length = ManufacturerOUI_LEN;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTURERNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTURERNAME \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_RESOURCETYPEIDNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_RESOURCETYPEIDNAME \n"));
+			wrq->u.data.length = strlen(ResourceTypeIdName);
+			Status = copy_to_user(wrq->u.data.pointer, ResourceTypeIdName, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED \n"));
+			ulInfo = 1; // 1 is support wep else 2 is not support.
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_POWERMANAGEMENTMODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_POWERMANAGEMENTMODE \n"));
+			if (pAdapter->StaCfg.Psm == PSMP_ACTION)
+				ulInfo = 1; // 1 is power active else 2 is power save.
+			else
+				ulInfo = 2;
+
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEPDEFAULTKEYVALUE \n"));
+			//KeyIdxValue.KeyIdx = pAd->PortCfg.MBSSID[pAd->IoctlIF].DefaultKeyId;
+			pKeyIdxValue = wrq->u.data.pointer;
+			DBGPRINT(RT_DEBUG_TRACE,("KeyIdxValue.KeyIdx = %d, \n",pKeyIdxValue->KeyIdx));
+			valueLen = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+			NdisMoveMemory(pKeyIdxValue->Value,
+						   &pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key,
+						   valueLen);
+			pKeyIdxValue->Value[valueLen]='\0';
+
+			wrq->u.data.length = sizeof(DefaultKeyIdxValue);
+
+			Status = copy_to_user(wrq->u.data.pointer, pKeyIdxValue, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE,("DefaultKeyId = %d, total len = %d, str len=%d, KeyValue= %02x %02x %02x %02x \n", pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+			pAdapter->SharedKey[BSS0][0].Key[0],
+			pAdapter->SharedKey[BSS0][1].Key[0],
+			pAdapter->SharedKey[BSS0][2].Key[0],
+			pAdapter->SharedKey[BSS0][3].Key[0]));
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPDEFAULTKEYID \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("DefaultKeyId =%d \n", pAdapter->StaCfg.DefaultKeyId));
+			break;
+
+		case RT_OID_802_11_WEPKEYMAPPINGLENGTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPKEYMAPPINGLENGTH \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer,
+									&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+									wrq->u.data.length);
+			break;
+
+		case OID_802_11_SHORTRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SHORTRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			ShortRetryLimit = tx_rty_cfg.field.ShortRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("ShortRetryLimit =%ld,  tx_rty_cfg.field.ShortRetryLimit=%d\n", ShortRetryLimit, tx_rty_cfg.field.ShortRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &ShortRetryLimit, wrq->u.data.length);
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_LONGRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			LongRetryLimit = tx_rty_cfg.field.LongRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("LongRetryLimit =%ld,  tx_rty_cfg.field.LongRtyLimit=%d\n", LongRetryLimit, tx_rty_cfg.field.LongRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &LongRetryLimit, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRODUCTID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRODUCTID \n"));
+
+#ifdef RT2870
+			sprintf(tmp, "%04x %04x\n", ((POS_COOKIE)pAdapter->OS_Cookie)->pUsb_Dev->descriptor.idVendor ,((POS_COOKIE)pAdapter->OS_Cookie)->pUsb_Dev->descriptor.idProduct);
+
+#endif // RT2870 //
+			wrq->u.data.length = strlen(tmp);
+			Status = copy_to_user(wrq->u.data.pointer, tmp, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREID \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CURRENTCHANNEL \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			DBGPRINT(RT_DEBUG_TRACE, ("sizeof UCHAR=%d, channel=%d \n", sizeof(UCHAR), pAdapter->CommonCfg.Channel));
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Channel, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+#endif //SNMP_SUPPORT
+
+		case OID_802_11_BUILD_CHANNEL_EX:
+			{
+				UCHAR value;
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BUILD_CHANNEL_EX \n"));
+				wrq->u.data.length = sizeof(UCHAR);
+#ifdef EXT_BUILD_CHANNEL_LIST
+				DBGPRINT(RT_DEBUG_TRACE, ("Support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 1;
+#else
+				DBGPRINT(RT_DEBUG_TRACE, ("Doesn't support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 0;
+#endif // EXT_BUILD_CHANNEL_LIST //
+				Status = copy_to_user(wrq->u.data.pointer, &value, 1);
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			}
+			break;
+
+		case OID_802_11_GET_CH_LIST:
+			{
+				PRT_CHANNEL_LIST_INFO pChListBuf;
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CH_LIST \n"));
+				if (pAdapter->ChannelListNum == 0)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf = (RT_CHANNEL_LIST_INFO *) kmalloc(sizeof(RT_CHANNEL_LIST_INFO), MEM_ALLOC_FLAG);
+				if (pChListBuf == NULL)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf->ChannelListNum = pAdapter->ChannelListNum;
+				for (i = 0; i < pChListBuf->ChannelListNum; i++)
+					pChListBuf->ChannelList[i] = pAdapter->ChannelList[i].Channel;
+
+				wrq->u.data.length = sizeof(RT_CHANNEL_LIST_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pChListBuf, sizeof(RT_CHANNEL_LIST_INFO));
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+
+				if (pChListBuf)
+					kfree(pChListBuf);
+			}
+			break;
+
+		case OID_802_11_GET_COUNTRY_CODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_COUNTRY_CODE \n"));
+			wrq->u.data.length = 2;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.CountryCode, 2);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+		case OID_802_11_GET_CHANNEL_GEOGRAPHY:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CHANNEL_GEOGRAPHY \n"));
+			wrq->u.data.length = 1;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Geography, 1);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_QUERY_DLS:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bDLSCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS(=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			break;
+
+		case RT_OID_802_11_QUERY_DLS_PARAM:
+			{
+				PRT_802_11_DLS_INFO	pDlsInfo = kmalloc(sizeof(RT_802_11_DLS_INFO), GFP_ATOMIC);
+				if (pDlsInfo == NULL)
+					break;
+
+				for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+				{
+					RTMPMoveMemory(&pDlsInfo->Entry[i], &pAdapter->StaCfg.DLSEntry[i], sizeof(RT_802_11_DLS_UI));
+				}
+
+				pDlsInfo->num = MAX_NUM_OF_DLS_ENTRY;
+				wrq->u.data.length = sizeof(RT_802_11_DLS_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pDlsInfo, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS_PARAM\n"));
+
+				if (pDlsInfo)
+					kfree(pDlsInfo);
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+    return Status;
+}
+
+INT rt28xx_sta_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT					cmd)
+{
+	POS_COOKIE			pObj;
+	VIRTUAL_ADAPTER		*pVirtualAd = NULL;
+	RTMP_ADAPTER        *pAd = NULL;
+	struct iwreq        *wrq = (struct iwreq *) rq;
+	BOOLEAN				StateMachineTouched = FALSE;
+	INT					Status = NDIS_STATUS_SUCCESS;
+	USHORT				subcmd;
+
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		pAd = net_dev->priv;
+	}
+	else
+	{
+		pVirtualAd = net_dev->priv;
+		pAd = pVirtualAd->RtmpDev->priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	    if (wrq->u.data.pointer == NULL)
+	    {
+		    return Status;
+	    }
+
+	    if (strstr(wrq->u.data.pointer, "OpMode") == NULL)
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		    return -ENETDOWN;
+        }
+    }
+
+	{	// determine this ioctl command is comming from which interface.
+		pObj->ioctl_if_type = INT_MAIN;
+		pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	switch(cmd)
+	{
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+		case RTPRIV_IOCTL_ATE:
+			{
+				RtmpDoAte(pAd, wrq);
+			}
+			break;
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+        case SIOCGIFHWADDR:
+			DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIFHWADDR\n"));
+			memcpy(wrq->u.name, pAd->CurrentAddress, ETH_ALEN);
+			break;
+		case SIOCGIWNAME:
+        {
+        	char *name=&wrq->u.name[0];
+        	rt_ioctl_giwname(net_dev, NULL, name, NULL);
+			break;
+		}
+		case SIOCGIWESSID:  //Get ESSID
+        {
+        	struct iw_point *essid=&wrq->u.essid;
+        	rt_ioctl_giwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWESSID:  //Set ESSID
+        {
+        	struct iw_point	*essid=&wrq->u.essid;
+        	rt_ioctl_siwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWNWID:   // set network id (the cell)
+		case SIOCGIWNWID:   // get network id
+			Status = -EOPNOTSUPP;
+			break;
+		case SIOCSIWFREQ:   //set channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_siwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCGIWFREQ:   // get channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_giwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCSIWNICKN: //set node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_siwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWNICKN: //get node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_giwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWRATE:   //get default bit rate (bps)
+		    rt_ioctl_giwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+	    case SIOCSIWRATE:  //set default bit rate (bps)
+	        rt_ioctl_siwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+        case SIOCGIWRTS:  // get RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_giwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCSIWRTS:  //set RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_siwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCGIWFRAG:  //get fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_giwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCSIWFRAG:  //set fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_siwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCGIWENCODE:  //get encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_giwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+        case SIOCSIWENCODE:  //set encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_siwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+		case SIOCGIWAP:     //get access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_giwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+	    case SIOCSIWAP:  //set access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_siwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+		case SIOCGIWMODE:   //get operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_giwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCSIWMODE:   //set operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_siwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCGIWSENS:   //get sensitivity (dBm)
+		case SIOCSIWSENS:	//set sensitivity (dBm)
+		case SIOCGIWPOWER:  //get Power Management settings
+		case SIOCSIWPOWER:  //set Power Management settings
+		case SIOCGIWTXPOW:  //get transmit power (dBm)
+		case SIOCSIWTXPOW:  //set transmit power (dBm)
+		case SIOCGIWRANGE:	//Get range of parameters
+		case SIOCGIWRETRY:	//get retry limits and lifetime
+		case SIOCSIWRETRY:	//set retry limits and lifetime
+			Status = -EOPNOTSUPP;
+			break;
+		case RT_PRIV_IOCTL:
+			subcmd = wrq->u.data.flags;
+			if( subcmd & OID_GET_SET_TOGGLE)
+				Status = RTMPSetInformation(pAd, rq, subcmd);
+			else
+				Status = RTMPQueryInformation(pAd, rq, subcmd);
+			break;
+		case SIOCGIWPRIV:
+			if (wrq->u.data.pointer)
+			{
+				if ( access_ok(VERIFY_WRITE, wrq->u.data.pointer, sizeof(privtab)) != TRUE)
+					break;
+				wrq->u.data.length = sizeof(privtab) / sizeof(privtab[0]);
+				if (copy_to_user(wrq->u.data.pointer, privtab, sizeof(privtab)))
+					Status = -EFAULT;
+			}
+			break;
+		case RTPRIV_IOCTL_SET:
+			if(access_ok(VERIFY_READ, wrq->u.data.pointer, wrq->u.data.length) != TRUE)
+				break;
+			rt_ioctl_setparam(net_dev, NULL, NULL, wrq->u.data.pointer);
+			break;
+		case RTPRIV_IOCTL_GSITESURVEY:
+			RTMPIoctlGetSiteSurvey(pAd, wrq);
+		    break;
+#ifdef DBG
+		case RTPRIV_IOCTL_MAC:
+			RTMPIoctlMAC(pAd, wrq);
+			break;
+		case RTPRIV_IOCTL_E2P:
+			RTMPIoctlE2PROM(pAd, wrq);
+			break;
+#endif // DBG //
+        case SIOCETHTOOL:
+                break;
+		default:
+			DBGPRINT(RT_DEBUG_ERROR, ("IOCTL::unknown IOCTL's cmd = 0x%08x\n", cmd));
+			Status = -EOPNOTSUPP;
+			break;
+	}
+
+    if(StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAd);
+
+	return Status;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set SSID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    NDIS_802_11_SSID                    Ssid, *pSsid=NULL;
+    BOOLEAN                             StateMachineTouched = FALSE;
+    int                                 success = TRUE;
+
+    if( strlen(arg) <= MAX_LEN_OF_SSID)
+    {
+        NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+        if (strlen(arg) != 0)
+        {
+            NdisMoveMemory(Ssid.Ssid, arg, strlen(arg));
+            Ssid.SsidLength = strlen(arg);
+        }
+        else   //ANY ssid
+        {
+            Ssid.SsidLength = 0;
+		    memcpy(Ssid.Ssid, "", 0);
+			pAdapter->StaCfg.BssType = BSS_INFRA;
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+	        pAdapter->StaCfg.WepStatus  = Ndis802_11EncryptionDisabled;
+		}
+        pSsid = &Ssid;
+
+        if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+        {
+            RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+        }
+
+        pAdapter->MlmeAux.CurrReqIsFromNdis = TRUE;
+        pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+		pAdapter->bConfigChanged = TRUE;
+
+        MlmeEnqueue(pAdapter,
+                    MLME_CNTL_STATE_MACHINE,
+                    OID_802_11_SSID,
+                    sizeof(NDIS_802_11_SSID),
+                    (VOID *)pSsid);
+
+        StateMachineTouched = TRUE;
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_SSID_Proc::(Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+    }
+    else
+        success = FALSE;
+
+    if (StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAdapter);
+
+    return success;
+}
+
+#ifdef WMM_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WmmCapable Enable or Disable
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	BOOLEAN	bWmmCapable;
+
+	bWmmCapable = simple_strtol(arg, 0, 10);
+
+	if ((bWmmCapable == 1)
+#ifdef RT2870
+		&& (pAd->NumberOfPipes >= 5)
+#endif // RT2870 //
+		)
+		pAd->CommonCfg.bWmmCapable = TRUE;
+	else if (bWmmCapable == 0)
+		pAd->CommonCfg.bWmmCapable = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_WmmCapable_Proc::(bWmmCapable=%d)\n",
+		pAd->CommonCfg.bWmmCapable));
+
+	return TRUE;
+}
+#endif // WMM_SUPPORT //
+
+/*
+    ==========================================================================
+    Description:
+        Set Network Type(Infrastructure/Adhoc mode)
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UINT32	Value = 0;
+
+    if (strcmp(arg, "Adhoc") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_ADHOC)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (INFRA_ON(pAdapter))
+			{
+				//BOOLEAN Cancelled;
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event BB!\n"));
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_ADHOC;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(AD-HOC)\n"));
+	}
+    else if (strcmp(arg, "Infra") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_INFRA)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (ADHOC_ON(pAdapter))
+			{
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_INFRA;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(INFRA)\n"));
+
+        pAdapter->StaCfg.BssType = BSS_INFRA;
+	}
+    else if (strcmp(arg, "Monitor") == 0)
+    {
+		UCHAR	bbpValue = 0;
+		BCN_TIME_CFG_STRUC csr;
+		OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_INFRA_ON);
+        OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+		// disable all periodic state machine
+		pAdapter->StaCfg.bAutoReconnect = FALSE;
+		// reset all mlme state machine
+		RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+		DBGPRINT(RT_DEBUG_TRACE, ("fOP_STATUS_MEDIA_STATE_CONNECTED \n"));
+        if (pAdapter->CommonCfg.CentralChannel == 0)
+        {
+#ifdef DOT11_N_SUPPORT
+            if (pAdapter->CommonCfg.PhyMode == PHY_11AN_MIXED)
+                pAdapter->CommonCfg.CentralChannel = 36;
+            else
+#endif // DOT11_N_SUPPORT //
+                pAdapter->CommonCfg.CentralChannel = 6;
+        }
+#ifdef DOT11_N_SUPPORT
+        else
+            N_ChannelCheck(pAdapter);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_ABOVE)
+		{
+			// 40MHz ,control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			//  RX : control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue &= (~0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value &= 0xfffffffe;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel + 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_BELOW)
+		{
+			// 40MHz ,control channel at upper
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value |= 0x1;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue |= (0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel - 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			// 20MHz
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_20;
+			AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAdapter, pAdapter->CommonCfg.Channel);
+			DBGPRINT(RT_DEBUG_TRACE, ("BW_20, Channel(%d)\n", pAdapter->CommonCfg.Channel));
+		}
+		// Enable Rx with promiscuous reception
+		RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, 0x3);
+		// ASIC supporsts sniffer function with replacing RSSI with timestamp.
+		//RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+		//Value |= (0x80);
+		//RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+		// disable sync
+		RTMP_IO_READ32(pAdapter, BCN_TIME_CFG, &csr.word);
+		csr.field.bBeaconGen = 0;
+		csr.field.bTBTTEnable = 0;
+		csr.field.TsfSyncMode = 0;
+		RTMP_IO_WRITE32(pAdapter, BCN_TIME_CFG, csr.word);
+
+		pAdapter->StaCfg.BssType = BSS_MONITOR;
+        pAdapter->net_dev->type = ARPHRD_IEEE80211_PRISM; //ARPHRD_IEEE80211; // IEEE80211
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(MONITOR)\n"));
+    }
+
+    // Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_NetworkType_Proc::(NetworkType=%d)\n", pAdapter->StaCfg.BssType));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Authentication mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "WEPAUTO") == 0) || (strcmp(arg, "wepauto") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeAutoSwitch;
+    else if ((strcmp(arg, "OPEN") == 0) || (strcmp(arg, "open") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+    else if ((strcmp(arg, "SHARED") == 0) || (strcmp(arg, "shared") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    else if ((strcmp(arg, "WPAPSK") == 0) || (strcmp(arg, "wpapsk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+    else if ((strcmp(arg, "WPANONE") == 0) || (strcmp(arg, "wpanone") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+    else if ((strcmp(arg, "WPA2PSK") == 0) || (strcmp(arg, "wpa2psk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+#ifdef WPA_SUPPLICANT_SUPPORT
+    else if ((strcmp(arg, "WPA") == 0) || (strcmp(arg, "wpa") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+    else if ((strcmp(arg, "WPA2") == 0) || (strcmp(arg, "wpa2") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#endif // WPA_SUPPLICANT_SUPPORT //
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_AuthMode_Proc::(AuthMode=%d)\n", pAdapter->StaCfg.AuthMode));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Encryption Type
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "NONE") == 0) || (strcmp(arg, "none") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPDisabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPDisabled;
+    }
+    else if ((strcmp(arg, "WEP") == 0) || (strcmp(arg, "wep") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPEnabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPEnabled;
+    }
+    else if ((strcmp(arg, "TKIP") == 0) || (strcmp(arg, "tkip") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption2Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption2Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption2Enabled;
+    }
+    else if ((strcmp(arg, "AES") == 0) || (strcmp(arg, "aes") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption3Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption3Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption3Enabled;
+    }
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_EncrypType_Proc::(EncrypType=%d)\n", pAdapter->StaCfg.WepStatus));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Default Key ID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    ULONG                               KeyIdx;
+
+    KeyIdx = simple_strtol(arg, 0, 10);
+    if((KeyIdx >= 1 ) && (KeyIdx <= 4))
+        pAdapter->StaCfg.DefaultKeyId = (UCHAR) (KeyIdx - 1 );
+    else
+        return FALSE;  //Invalid argument
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_DefaultKeyID_Proc::(DefaultKeyID=%d)\n", pAdapter->StaCfg.DefaultKeyId));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY1
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+
+    pAdapter->SharedKey[BSS0][0].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              0,
+                              pAdapter->SharedKey[BSS0][0].CipherAlg,
+                              pAdapter->SharedKey[BSS0][0].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+
+    Description:
+        Set WEP KEY2
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][1].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              1,
+                              pAdapter->SharedKey[BSS0][1].CipherAlg,
+                              pAdapter->SharedKey[BSS0][1].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY3
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][2].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              2,
+                              pAdapter->SharedKey[BSS0][2].CipherAlg,
+                              pAdapter->SharedKey[BSS0][2].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY4
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][3].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              3,
+                              pAdapter->SharedKey[BSS0][3].CipherAlg,
+                              pAdapter->SharedKey[BSS0][3].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WPA PSK key
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UCHAR                   keyMaterial[40];
+
+    if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+        (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+	    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+		)
+        return TRUE;    // do nothing
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_WPAPSK_Proc::(WPAPSK=%s)\n", arg));
+
+    NdisZeroMemory(keyMaterial, 40);
+
+    if ((strlen(arg) < 8) || (strlen(arg) > 64))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("Set failed!!(WPAPSK=%s), WPAPSK key-string required 8 ~ 64 characters \n", arg));
+        return FALSE;
+    }
+
+    if (strlen(arg) == 64)
+    {
+        AtoH(arg, keyMaterial, 32);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+
+    }
+    else
+    {
+        PasswordHash((char *)arg, pAdapter->MlmeAux.Ssid, pAdapter->MlmeAux.SsidLen, keyMaterial);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+    }
+
+
+
+    if(pAdapter->StaCfg.BssType == BSS_ADHOC &&
+       pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+    {
+         pAdapter->StaCfg.WpaState = SS_NOTUSE;
+    }
+    else
+    {
+        // Start STA supplicant state machine
+        pAdapter->StaCfg.WpaState = SS_START;
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Power Saving mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (pAdapter->StaCfg.BssType == BSS_INFRA)
+    {
+        if ((strcmp(arg, "Max_PSP") == 0) ||
+			(strcmp(arg, "max_psp") == 0) ||
+			(strcmp(arg, "MAX_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeMAX_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeMAX_PSP;
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            pAdapter->StaCfg.DefaultListenCount = 5;
+
+        }
+        else if ((strcmp(arg, "Fast_PSP") == 0) ||
+				 (strcmp(arg, "fast_psp") == 0) ||
+                 (strcmp(arg, "FAST_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else if ((strcmp(arg, "Legacy_PSP") == 0) ||
+                 (strcmp(arg, "legacy_psp") == 0) ||
+                 (strcmp(arg, "LEGACY_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else
+        {
+            //Default Ndis802_11PowerModeCAM
+            // clear PSM bit immediately
+            MlmeSetPsmBit(pAdapter, PWR_ACTIVE);
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+        }
+
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_PSMode_Proc::(PSMode=%ld)\n", pAdapter->StaCfg.WindowsPowerMode));
+    }
+    else
+        return FALSE;
+
+
+    return TRUE;
+}
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WpaSupport flag.
+    Value:
+        0: Driver ignore wpa_supplicant.
+        1: wpa_supplicant initiates scanning and AP selection.
+        2: driver takes care of scanning, AP selection, and IEEE 802.11 association parameters.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+    if ( simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+    else if ( simple_strtol(arg, 0, 10) == 1)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE;
+    else if ( simple_strtol(arg, 0, 10) == 2)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE_WITH_WEB_UI;
+    else
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Wpa_Support::(WpaSupplicantUP=%d)\n", pAd->StaCfg.WpaSupplicantUP));
+
+    return TRUE;
+}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+/*
+    ==========================================================================
+    Description:
+        Read / Write MAC
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 mac 0        ==> read MAC where Addr=0x0
+               2.) iwpriv ra0 mac 0=12     ==> write MAC where Addr=0x0, value=12
+    ==========================================================================
+*/
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	ULONG				macAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	UINT32				macValue = 0;
+	INT					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+		//Parsing Read or Write
+	    this_char = arg;
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// Mac Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+				if (macAddr < 0xFFFF)
+				{
+					RTMP_IO_READ32(pAdapter, macAddr, &macValue);
+					DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%lx, MacValue=%x\n", macAddr, macValue));
+					sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr , macValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[8-k+j] = temp2[j];
+			}
+
+			while(k < 8)
+				temp2[7-k++]='0';
+			temp2[8]='\0';
+
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+
+				AtoH(temp2, temp, 4);
+				macValue = *temp*256*256*256 + temp[1]*256*256 + temp[2]*256 + temp[3];
+
+				// debug mode
+				if (macAddr == (HW_DEBUG_SETTING_BASE + 4))
+				{
+					// 0x2bf4: byte0 non-zero: enable R17 tuning, 0: disable R17 tuning
+                    if (macValue & 0x000000ff)
+                    {
+                        pAdapter->BbpTuning.bEnable = TRUE;
+                        DBGPRINT(RT_DEBUG_TRACE,("turn on R17 tuning\n"));
+                    }
+                    else
+                    {
+                        UCHAR R66;
+                        pAdapter->BbpTuning.bEnable = FALSE;
+                        R66 = 0x26 + GET_LNA_GAIN(pAdapter);
+#ifdef RALINK_ATE
+						if (ATE_ON(pAdapter))
+						{
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+						}
+						else
+#endif // RALINK_ATE //
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+                        DBGPRINT(RT_DEBUG_TRACE,("turn off R17 tuning, restore to 0x%02x\n", R66));
+                    }
+					return;
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%02lx, MacValue=0x%x\n", macAddr, macValue));
+
+				RTMP_IO_WRITE32(pAdapter, macAddr, macValue);
+				sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr, macValue);
+			}
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlMAC\n\n"));
+}
+
+/*
+    ==========================================================================
+    Description:
+        Read / Write E2PROM
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 e2p 0     	==> read E2PROM where Addr=0x0
+               2.) iwpriv ra0 e2p 0=1234    ==> write E2PROM where Addr=0x0, value=1234
+    ==========================================================================
+*/
+VOID RTMPIoctlE2PROM(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	USHORT				eepAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	USHORT				eepValue;
+	int					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+	    //Parsing Read or Write
+		this_char = arg;
+
+
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// E2PROM addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				eepAddr = *temp*256 + temp[1];
+				if (eepAddr < 0xFFFF)
+				{
+					RT28xx_EEPROM_READ16(pAdapter, eepAddr, eepValue);
+					sprintf(msg+strlen(msg), "[0x%04X]:0x%04X  ", eepAddr , eepValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[4-k+j] = temp2[j];
+			}
+
+			while(k < 4)
+				temp2[3-k++]='0';
+			temp2[4]='\0';
+
+			AtoH(this_char, temp, 2);
+			eepAddr = *temp*256 + temp[1];
+
+			AtoH(temp2, temp, 2);
+			eepValue = *temp*256 + temp[1];
+
+			RT28xx_EEPROM_WRITE16(pAdapter, eepAddr, eepValue);
+			sprintf(msg+strlen(msg), "[0x%02X]:%02X  ", eepAddr, eepValue);
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlE2PROM\n"));
+}
+#endif // DBG //
+
+
+
+
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.bTGnWifiTest = FALSE;
+    else
+        pAd->StaCfg.bTGnWifiTest = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_TGnWifiTest_Proc::(bTGnWifiTest=%d)\n", pAd->StaCfg.bTGnWifiTest));
+	return TRUE;
+}
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				LongRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_LongRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				ShortRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_ShortRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_None;
+    else if (simple_strtol(arg, 0, 10) == 1)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Flexible;
+    else if (simple_strtol(arg, 0, 10) == 2)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Strict;
+    else
+        return FALSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Ieee802dMode_Proc::(IEEEE0211dMode=%d)\n", pAdapter->StaCfg.IEEE80211dClientMode));
+    return TRUE;
+}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+    else
+        pAd->CommonCfg.CarrierDetect.Enable = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_CarrierDetect_Proc::(CarrierDetect.Enable=%d)\n", pAd->CommonCfg.CarrierDetect.Enable));
+	return TRUE;
+}
+#endif // CARRIER_DETECTION_SUPPORT //
+
+
+INT	Show_Adhoc_MacTable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCHAR			extra)
+{
+	INT i;
+
+	sprintf(extra, "\n");
+
+#ifdef DOT11_N_SUPPORT
+	sprintf(extra, "%sHT Operating Mode : %d\n", extra, pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode);
+#endif // DOT11_N_SUPPORT //
+
+	sprintf(extra, "%s\n%-19s%-4s%-4s%-7s%-7s%-7s%-10s%-6s%-6s%-6s%-6s\n", extra,
+			"MAC", "AID", "BSS", "RSSI0", "RSSI1", "RSSI2", "PhMd", "BW", "MCS", "SGI", "STBC");
+
+	for (i=1; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+
+		if (strlen(extra) > (IW_PRIV_SIZE_MASK - 30))
+		    break;
+		if ((pEntry->ValidAsCLI || pEntry->ValidAsApCli) && (pEntry->Sst == SST_ASSOC))
+		{
+			sprintf(extra, "%s%02X:%02X:%02X:%02X:%02X:%02X  ", extra,
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+			sprintf(extra, "%s%-4d", extra, (int)pEntry->Aid);
+			sprintf(extra, "%s%-4d", extra, (int)pEntry->apidx);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi0);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi1);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi2);
+			sprintf(extra, "%s%-10s", extra, GetPhyMode(pEntry->HTPhyMode.field.MODE));
+			sprintf(extra, "%s%-6s", extra, GetBW(pEntry->HTPhyMode.field.BW));
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.MCS);
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.ShortGI);
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.STBC);
+			sprintf(extra, "%s%-10d, %d, %d%%\n", extra, pEntry->DebugFIFOCount, pEntry->DebugTxCount,
+						(pEntry->DebugTxCount) ? ((pEntry->DebugTxCount-pEntry->DebugFIFOCount)*100/pEntry->DebugTxCount) : 0);
+			sprintf(extra, "%s\n", extra);
+		}
+	}
+
+	return TRUE;
+}
+
+
diff --git a/drivers/staging/rt2870/tmp61 b/drivers/staging/rt2870/tmp61
new file mode 100644
index 000000000000..975e44484379
--- /dev/null
+++ b/drivers/staging/rt2870/tmp61
@@ -0,0 +1,7037 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    sta_ioctl.c
+
+    Abstract:
+    IOCTL related subroutines
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Rory Chen   01-03-2003    created
+	Rory Chen   02-14-2005    modify to support RT61
+*/
+
+#include	"rt_config.h"
+
+#ifdef DBG
+extern ULONG    RTDebugLevel;
+#endif
+
+#define NR_WEP_KEYS 				4
+#define WEP_SMALL_KEY_LEN 			(40/8)
+#define WEP_LARGE_KEY_LEN 			(104/8)
+
+#define GROUP_KEY_NO                4
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_A, _B, _C, _D, _E, _F)
+#else
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_B, _C, _D, _E, _F)
+#endif
+
+extern UCHAR    CipherWpa2Template[];
+extern UCHAR    CipherWpaPskTkip[];
+extern UCHAR    CipherWpaPskTkipLen;
+
+typedef struct PACKED _RT_VERSION_INFO{
+    UCHAR       DriverVersionW;
+    UCHAR       DriverVersionX;
+    UCHAR       DriverVersionY;
+    UCHAR       DriverVersionZ;
+    UINT        DriverBuildYear;
+    UINT        DriverBuildMonth;
+    UINT        DriverBuildDay;
+} RT_VERSION_INFO, *PRT_VERSION_INFO;
+
+struct iw_priv_args privtab[] = {
+{ RTPRIV_IOCTL_SET,
+  IW_PRIV_TYPE_CHAR | 1024, 0,
+  "set"},
+
+{ RTPRIV_IOCTL_SHOW, 0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+{ RTPRIV_IOCTL_SHOW, IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+/* --- sub-ioctls definitions --- */
+    { SHOW_CONN_STATUS,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "connStatus" },
+	{ SHOW_DRVIER_VERION,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "driverVer" },
+    { SHOW_BA_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "bainfo" },
+	{ SHOW_DESC_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "descinfo" },
+    { RAIO_OFF,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_off" },
+	{ RAIO_ON,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_on" },
+#ifdef QOS_DLS_SUPPORT
+	{ SHOW_DLS_ENTRY_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "dlsentryinfo" },
+#endif // QOS_DLS_SUPPORT //
+	{ SHOW_CFG_VALUE,
+	  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "show" },
+	{ SHOW_ADHOC_ENTRY_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "adhocEntry" },
+
+/* --- sub-ioctls relations --- */
+
+#ifdef DBG
+{ RTPRIV_IOCTL_BBP,
+  IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "bbp"},
+{ RTPRIV_IOCTL_MAC,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "mac"},
+{ RTPRIV_IOCTL_E2P,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "e2p"},
+#endif  /* DBG */
+
+{ RTPRIV_IOCTL_STATISTICS,
+  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "stat"},
+{ RTPRIV_IOCTL_GSITESURVEY,
+  0, IW_PRIV_TYPE_CHAR | 1024,
+  "get_site_survey"},
+};
+
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WMM_SUPPORT
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif
+
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+VOID RTMPIoctlBBP(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlE2PROM(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  struct iwreq    *wrq);
+#endif // DBG //
+
+
+NDIS_STATUS RTMPWPANoneAddKeyProc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN	PVOID			pBuf);
+
+INT Set_FragTest_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef DOT11_N_SUPPORT
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // DOT11_N_SUPPORT //
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // CARRIER_DETECTION_SUPPORT //
+
+INT	Show_Adhoc_MacTable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCHAR			extra);
+
+static struct {
+	CHAR *name;
+	INT (*set_proc)(PRTMP_ADAPTER pAdapter, PUCHAR arg);
+} *PRTMP_PRIVATE_SET_PROC, RTMP_PRIVATE_SUPPORT_PROC[] = {
+	{"DriverVersion",				Set_DriverVersion_Proc},
+	{"CountryRegion",				Set_CountryRegion_Proc},
+	{"CountryRegionABand",			Set_CountryRegionABand_Proc},
+	{"SSID",						Set_SSID_Proc},
+	{"WirelessMode",				Set_WirelessMode_Proc},
+	{"TxBurst",					Set_TxBurst_Proc},
+	{"TxPreamble",				Set_TxPreamble_Proc},
+	{"TxPower",					Set_TxPower_Proc},
+	{"Channel",					Set_Channel_Proc},
+	{"BGProtection",				Set_BGProtection_Proc},
+	{"RTSThreshold",				Set_RTSThreshold_Proc},
+	{"FragThreshold",				Set_FragThreshold_Proc},
+#ifdef DOT11_N_SUPPORT
+	{"HtBw",		                Set_HtBw_Proc},
+	{"HtMcs",		                Set_HtMcs_Proc},
+	{"HtGi",		                Set_HtGi_Proc},
+	{"HtOpMode",		            Set_HtOpMode_Proc},
+	{"HtExtcha",		            Set_HtExtcha_Proc},
+	{"HtMpduDensity",		        Set_HtMpduDensity_Proc},
+	{"HtBaWinSize",		        	Set_HtBaWinSize_Proc},
+	{"HtRdg",		        		Set_HtRdg_Proc},
+	{"HtAmsdu",		        		Set_HtAmsdu_Proc},
+	{"HtAutoBa",		        	Set_HtAutoBa_Proc},
+	{"HtBaDecline",					Set_BADecline_Proc},
+	{"HtProtect",		        	Set_HtProtect_Proc},
+	{"HtMimoPs",		        	Set_HtMimoPs_Proc},
+#endif // DOT11_N_SUPPORT //
+
+#ifdef AGGREGATION_SUPPORT
+	{"PktAggregate",				Set_PktAggregate_Proc},
+#endif
+
+#ifdef WMM_SUPPORT
+	{"WmmCapable",					Set_WmmCapable_Proc},
+#endif
+	{"IEEE80211H",					Set_IEEE80211H_Proc},
+    {"NetworkType",                 Set_NetworkType_Proc},
+	{"AuthMode",					Set_AuthMode_Proc},
+	{"EncrypType",					Set_EncrypType_Proc},
+	{"DefaultKeyID",				Set_DefaultKeyID_Proc},
+	{"Key1",						Set_Key1_Proc},
+	{"Key2",						Set_Key2_Proc},
+	{"Key3",						Set_Key3_Proc},
+	{"Key4",						Set_Key4_Proc},
+	{"WPAPSK",						Set_WPAPSK_Proc},
+	{"ResetCounter",				Set_ResetStatCounter_Proc},
+	{"PSMode",                      Set_PSMode_Proc},
+#ifdef DBG
+	{"Debug",						Set_Debug_Proc},
+#endif
+
+#ifdef RALINK_ATE
+	{"ATE",							Set_ATE_Proc},
+	{"ATEDA",						Set_ATE_DA_Proc},
+	{"ATESA",						Set_ATE_SA_Proc},
+	{"ATEBSSID",					Set_ATE_BSSID_Proc},
+	{"ATECHANNEL",					Set_ATE_CHANNEL_Proc},
+	{"ATETXPOW0",					Set_ATE_TX_POWER0_Proc},
+	{"ATETXPOW1",					Set_ATE_TX_POWER1_Proc},
+	{"ATETXANT",					Set_ATE_TX_Antenna_Proc},
+	{"ATERXANT",					Set_ATE_RX_Antenna_Proc},
+	{"ATETXFREQOFFSET",				Set_ATE_TX_FREQOFFSET_Proc},
+	{"ATETXBW",						Set_ATE_TX_BW_Proc},
+	{"ATETXLEN",					Set_ATE_TX_LENGTH_Proc},
+	{"ATETXCNT",					Set_ATE_TX_COUNT_Proc},
+	{"ATETXMCS",					Set_ATE_TX_MCS_Proc},
+	{"ATETXMODE",					Set_ATE_TX_MODE_Proc},
+	{"ATETXGI",						Set_ATE_TX_GI_Proc},
+	{"ATERXFER",					Set_ATE_RX_FER_Proc},
+	{"ATERRF",						Set_ATE_Read_RF_Proc},
+	{"ATEWRF1",						Set_ATE_Write_RF1_Proc},
+	{"ATEWRF2",						Set_ATE_Write_RF2_Proc},
+	{"ATEWRF3",						Set_ATE_Write_RF3_Proc},
+	{"ATEWRF4",						Set_ATE_Write_RF4_Proc},
+	{"ATELDE2P",				    Set_ATE_Load_E2P_Proc},
+	{"ATERE2P",						Set_ATE_Read_E2P_Proc},
+	{"ATESHOW",						Set_ATE_Show_Proc},
+	{"ATEHELP",						Set_ATE_Help_Proc},
+
+#ifdef RALINK_28xx_QA
+	{"TxStop",						Set_TxStop_Proc},
+	{"RxStop",						Set_RxStop_Proc},
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    {"WpaSupport",                  Set_Wpa_Support},
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+	{"FixedTxMode",                 Set_FixedTxMode_Proc},
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	{"OpMode",						Set_OpMode_Proc},
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+#ifdef DOT11_N_SUPPORT
+    {"TGnWifiTest",                 Set_TGnWifiTest_Proc},
+    {"ForceGF",		        		Set_ForceGF_Proc},
+#endif // DOT11_N_SUPPORT //
+#ifdef QOS_DLS_SUPPORT
+	{"DlsAddEntry",					Set_DlsAddEntry_Proc},
+	{"DlsTearDownEntry",			Set_DlsTearDownEntry_Proc},
+#endif // QOS_DLS_SUPPORT //
+	{"LongRetry",	        		Set_LongRetryLimit_Proc},
+	{"ShortRetry",	        		Set_ShortRetryLimit_Proc},
+#ifdef EXT_BUILD_CHANNEL_LIST
+	{"11dClientMode",				Set_Ieee80211dClientMode_Proc},
+#endif // EXT_BUILD_CHANNEL_LIST //
+#ifdef CARRIER_DETECTION_SUPPORT
+	{"CarrierDetect",				Set_CarrierDetect_Proc},
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	{NULL,}
+};
+
+
+VOID RTMPAddKey(
+	IN	PRTMP_ADAPTER	    pAd,
+	IN	PNDIS_802_11_KEY    pKey)
+{
+	ULONG				KeyIdx;
+	MAC_TABLE_ENTRY  	*pEntry;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey ------>\n"));
+
+	if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+	{
+		if (pKey->KeyIndex & 0x80000000)
+		{
+		    if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+            {
+                NdisZeroMemory(pAd->StaCfg.PMK, 32);
+                NdisMoveMemory(pAd->StaCfg.PMK, pKey->KeyMaterial, pKey->KeyLength);
+                goto end;
+            }
+		    // Update PTK
+		    NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Decide its ChiperAlg
+        	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+        	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+        	else
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+            // Update these related information to MAC_TABLE_ENTRY
+        	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+            NdisMoveMemory(pEntry->PairwiseKey.Key, pAd->SharedKey[BSS0][0].Key, LEN_TKIP_EK);
+        	NdisMoveMemory(pEntry->PairwiseKey.RxMic, pAd->SharedKey[BSS0][0].RxMic, LEN_TKIP_RXMICK);
+        	NdisMoveMemory(pEntry->PairwiseKey.TxMic, pAd->SharedKey[BSS0][0].TxMic, LEN_TKIP_TXMICK);
+        	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+        	// Update pairwise key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  0,
+        						  pAd->SharedKey[BSS0][0].CipherAlg,
+        						  pAd->SharedKey[BSS0][0].Key,
+        						  pAd->SharedKey[BSS0][0].TxMic,
+        						  pAd->SharedKey[BSS0][0].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  0,
+        							  pAd->SharedKey[BSS0][0].CipherAlg,
+        							  pEntry);
+
+            if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+            {
+                // set 802.1x port control
+	            //pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAd);
+
+                // Indicate Connected for GUI
+                pAd->IndicateMediaState = NdisMediaStateConnected;
+            }
+		}
+        else
+        {
+            // Update GTK
+            pAd->StaCfg.DefaultKeyId = (pKey->KeyIndex & 0xFF);
+            NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Update Shared Key CipherAlg
+    		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+    		if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+    		else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+
+            // Update group key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  pAd->StaCfg.DefaultKeyId,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  pAd->StaCfg.DefaultKeyId,
+        							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        							  NULL);
+
+            // set 802.1x port control
+	        //pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+			STA_PORT_SECURED(pAd);
+
+            // Indicate Connected for GUI
+            pAd->IndicateMediaState = NdisMediaStateConnected;
+        }
+	}
+	else	// dynamic WEP from wpa_supplicant
+	{
+		UCHAR	CipherAlg;
+    	PUCHAR	Key;
+
+		if(pKey->KeyLength == 32)
+			goto end;
+
+		KeyIdx = pKey->KeyIndex & 0x0fffffff;
+
+		if (KeyIdx < 4)
+		{
+			// it is a default shared key, for Pairwise key setting
+			if (pKey->KeyIndex & 0x80000000)
+			{
+				pEntry = MacTableLookup(pAd, pKey->BSSID);
+
+				if (pEntry)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey: Set Pair-wise Key\n"));
+
+					// set key material and key length
+ 					pEntry->PairwiseKey.KeyLen = (UCHAR)pKey->KeyLength;
+					NdisMoveMemory(pEntry->PairwiseKey.Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+					// set Cipher type
+					if (pKey->KeyLength == 5)
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP64;
+					else
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP128;
+
+					// Add Pair-wise key to Asic
+					AsicAddPairwiseKeyEntry(
+						pAd,
+						pEntry->Addr,
+						(UCHAR)pEntry->Aid,
+                		&pEntry->PairwiseKey);
+
+					// update WCID attribute table and IVEIV table for this entry
+					RTMPAddWcidAttributeEntry(
+						pAd,
+						BSS0,
+						KeyIdx, // The value may be not zero
+						pEntry->PairwiseKey.CipherAlg,
+						pEntry);
+
+				}
+			}
+			else
+            {
+				// Default key for tx (shared key)
+				pAd->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+
+				// set key material and key length
+				pAd->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(pAd->SharedKey[BSS0][KeyIdx].Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+				// Set Ciper type
+				if (pKey->KeyLength == 5)
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP64;
+				else
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP128;
+
+    			CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][KeyIdx].Key;
+
+				// Set Group key material to Asic
+    			AsicAddSharedKeyEntry(pAd, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+				// Update WCID attribute table and IVEIV table for this group key table
+				RTMPAddWcidAttributeEntry(pAd, BSS0, KeyIdx, CipherAlg, NULL);
+
+			}
+		}
+	}
+end:
+	return;
+}
+
+char * rtstrchr(const char * s, int c)
+{
+    for(; *s != (char) c; ++s)
+        if (*s == '\0')
+            return NULL;
+    return (char *) s;
+}
+
+/*
+This is required for LinEX2004/kernel2.6.7 to provide iwlist scanning function
+*/
+
+int
+rt_ioctl_giwname(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+//	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+#ifdef RT2870
+	strncpy(name, "RT2870 Wireless", IFNAMSIZ);
+#endif // RT2870 //
+	return 0;
+}
+
+int rt_ioctl_siwfreq(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_freq *freq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	int 	chan = -1;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+
+	if (freq->e > 1)
+		return -EINVAL;
+
+	if((freq->e == 0) && (freq->m <= 1000))
+		chan = freq->m;	// Setting by channel number
+	else
+		MAP_KHZ_TO_CHANNEL_ID( (freq->m /100) , chan); // Setting by frequency - search the table , like 2.412G, 2.422G,
+
+    if (ChannelSanity(pAdapter, chan) == TRUE)
+    {
+	pAdapter->CommonCfg.Channel = chan;
+	DBGPRINT(RT_DEBUG_ERROR, ("==>rt_ioctl_siwfreq::SIOCSIWFREQ[cmd=0x%x] (Channel=%d)\n", SIOCSIWFREQ, pAdapter->CommonCfg.Channel));
+    }
+    else
+        return -EINVAL;
+
+	return 0;
+}
+int rt_ioctl_giwfreq(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_freq *freq, char *extra)
+{
+    VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter = NULL;
+	UCHAR ch;
+	ULONG	m;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+		ch = pAdapter->CommonCfg.Channel;
+
+	DBGPRINT(RT_DEBUG_TRACE,("==>rt_ioctl_giwfreq  %d\n", ch));
+
+    MAP_CHANNEL_ID_TO_KHZ(ch, m);
+	freq->m = m * 100;
+	freq->e = 1;
+	return 0;
+}
+
+int rt_ioctl_siwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+    	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	switch (*mode)
+	{
+		case IW_MODE_ADHOC:
+			Set_NetworkType_Proc(pAdapter, "Adhoc");
+			break;
+		case IW_MODE_INFRA:
+			Set_NetworkType_Proc(pAdapter, "Infra");
+			break;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+        case IW_MODE_MONITOR:
+			Set_NetworkType_Proc(pAdapter, "Monitor");
+			break;
+#endif
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_siwmode::SIOCSIWMODE (unknown %d)\n", *mode));
+			return -EINVAL;
+	}
+
+	// Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+	pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+	return 0;
+}
+
+int rt_ioctl_giwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (ADHOC_ON(pAdapter))
+		*mode = IW_MODE_ADHOC;
+    else if (INFRA_ON(pAdapter))
+		*mode = IW_MODE_INFRA;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+    else if (MONITOR_ON(pAdapter))
+    {
+        *mode = IW_MODE_MONITOR;
+    }
+#endif
+    else
+        *mode = IW_MODE_AUTO;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==>rt_ioctl_giwmode(mode=%d)\n", *mode));
+	return 0;
+}
+
+int rt_ioctl_siwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+        	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	return 0;
+}
+
+int rt_ioctl_giwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	return 0;
+}
+
+int rt_ioctl_giwrange(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	struct iw_range *range = (struct iw_range *) extra;
+	u16 val;
+	int i;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===>rt_ioctl_giwrange\n"));
+	data->length = sizeof(struct iw_range);
+	memset(range, 0, sizeof(struct iw_range));
+
+	range->txpower_capa = IW_TXPOW_DBM;
+
+	if (INFRA_ON(pAdapter)||ADHOC_ON(pAdapter))
+	{
+		range->min_pmp = 1 * 1024;
+		range->max_pmp = 65535 * 1024;
+		range->min_pmt = 1 * 1024;
+		range->max_pmt = 1000 * 1024;
+		range->pmp_flags = IW_POWER_PERIOD;
+		range->pmt_flags = IW_POWER_TIMEOUT;
+		range->pm_capa = IW_POWER_PERIOD | IW_POWER_TIMEOUT |
+			IW_POWER_UNICAST_R | IW_POWER_ALL_R;
+	}
+
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 14;
+
+	range->retry_capa = IW_RETRY_LIMIT;
+	range->retry_flags = IW_RETRY_LIMIT;
+	range->min_retry = 0;
+	range->max_retry = 255;
+
+	range->num_channels =  pAdapter->ChannelListNum;
+
+	val = 0;
+	for (i = 1; i <= range->num_channels; i++)
+	{
+		u32 m;
+		range->freq[val].i = pAdapter->ChannelList[i-1].Channel;
+		MAP_CHANNEL_ID_TO_KHZ(pAdapter->ChannelList[i-1].Channel, m);
+		range->freq[val].m = m * 100; /* HZ */
+
+		range->freq[val].e = 1;
+		val++;
+		if (val == IW_MAX_FREQUENCIES)
+			break;
+	}
+	range->num_frequency = val;
+
+	range->max_qual.qual = 100; /* what is correct max? This was not
+					* documented exactly. At least
+					* 69 has been observed. */
+	range->max_qual.level = 0; /* dB */
+	range->max_qual.noise = 0; /* dB */
+
+	/* What would be suitable values for "average/typical" qual? */
+	range->avg_qual.qual = 20;
+	range->avg_qual.level = -60;
+	range->avg_qual.noise = -95;
+	range->sensitivity = 3;
+
+	range->max_encoding_tokens = NR_WEP_KEYS;
+	range->num_encoding_sizes = 2;
+	range->encoding_size[0] = 5;
+	range->encoding_size[1] = 13;
+
+	range->min_rts = 0;
+	range->max_rts = 2347;
+	range->min_frag = 256;
+	range->max_frag = 2346;
+
+#if WIRELESS_EXT > 17
+	/* IW_ENC_CAPA_* bit field */
+	range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
+					IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+#endif
+
+	return 0;
+}
+
+int rt_ioctl_siwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+    NDIS_802_11_MAC_ADDRESS Bssid;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+    {
+        RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+    }
+
+    // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+    // this request, because this request is initiated by NDIS.
+    pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+	// Prevent to connect AP again in STAMlmePeriodicExec
+	pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+    memset(Bssid, 0, MAC_ADDR_LEN);
+    memcpy(Bssid, ap_addr->sa_data, MAC_ADDR_LEN);
+    MlmeEnqueue(pAdapter,
+                MLME_CNTL_STATE_MACHINE,
+                OID_802_11_BSSID,
+                sizeof(NDIS_802_11_MAC_ADDRESS),
+                (VOID *)&Bssid);
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCSIWAP %02x:%02x:%02x:%02x:%02x:%02x\n",
+        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+
+	return 0;
+}
+
+int rt_ioctl_giwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+	{
+		ap_addr->sa_family = ARPHRD_ETHER;
+		memcpy(ap_addr->sa_data, &pAdapter->CommonCfg.Bssid, ETH_ALEN);
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    // Add for RT2870
+    else if (pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+    {
+        ap_addr->sa_family = ARPHRD_ETHER;
+        memcpy(ap_addr->sa_data, &pAdapter->MlmeAux.Bssid, ETH_ALEN);
+    }
+#endif // WPA_SUPPLICANT_SUPPORT //
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIWAP(=EMPTY)\n"));
+		return -ENOTCONN;
+	}
+
+	return 0;
+}
+
+/*
+ * Units are in db above the noise floor. That means the
+ * rssi values reported in the tx/rx descriptors in the
+ * driver are the SNR expressed in db.
+ *
+ * If you assume that the noise floor is -95, which is an
+ * excellent assumption 99.5 % of the time, then you can
+ * derive the absolute signal level (i.e. -95 + rssi).
+ * There are some other slight factors to take into account
+ * depending on whether the rssi measurement is from 11b,
+ * 11g, or 11a.   These differences are at most 2db and
+ * can be documented.
+ *
+ * NB: various calculations are based on the orinoco/wavelan
+ *     drivers for compatibility
+ */
+static void set_quality(PRTMP_ADAPTER pAdapter,
+                        struct iw_quality *iq,
+                        signed char rssi)
+{
+	__u8 ChannelQuality;
+
+	// Normalize Rssi
+	if (rssi >= -50)
+		ChannelQuality = 100;
+	else if (rssi >= -80) // between -50 ~ -80dbm
+		ChannelQuality = (__u8)(24 + ((rssi + 80) * 26)/10);
+	else if (rssi >= -90)   // between -80 ~ -90dbm
+        ChannelQuality = (__u8)((rssi + 90) * 26)/10;
+	else
+		ChannelQuality = 0;
+
+    iq->qual = (__u8)ChannelQuality;
+
+    iq->level = (__u8)(rssi);
+    iq->noise = (pAdapter->BbpWriteLatch[66] > pAdapter->BbpTuning.FalseCcaUpperThreshold) ? ((__u8)pAdapter->BbpTuning.FalseCcaUpperThreshold) : ((__u8) pAdapter->BbpWriteLatch[66]); 	// noise level (dBm)
+    iq->noise += 256 - 143;
+    iq->updated = pAdapter->iw_stats.qual.updated;
+}
+
+int rt_ioctl_iwaplist(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+ 	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	struct sockaddr addr[IW_MAX_AP];
+	struct iw_quality qual[IW_MAX_AP];
+	int i;
+
+   	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		data->length = 0;
+		return 0;
+        //return -ENETDOWN;
+	}
+
+	for (i = 0; i <IW_MAX_AP ; i++)
+	{
+		if (i >=  pAdapter->ScanTab.BssNr)
+			break;
+		addr[i].sa_family = ARPHRD_ETHER;
+			memcpy(addr[i].sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+		set_quality(pAdapter, &qual[i], pAdapter->ScanTab.BssEntry[i].Rssi);
+	}
+	data->length = i;
+	memcpy(extra, &addr, i*sizeof(addr[0]));
+	data->flags = 1;		/* signal quality present (sort of) */
+	memcpy(extra + i*sizeof(addr[0]), &qual, i*sizeof(qual[i]));
+
+	return 0;
+}
+
+#ifdef SIOCGIWSCAN
+int rt_ioctl_siwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	ULONG								Now;
+	int Status = NDIS_STATUS_SUCCESS;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		return -ENETDOWN;
+	}
+
+	if (MONITOR_ON(pAdapter))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+        return -EINVAL;
+    }
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount++;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+    pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		return 0;
+	do{
+		Now = jiffies;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+		if ((pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE) &&
+			(pAdapter->StaCfg.WpaSupplicantScanCount > 3))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! WpaSupplicantScanCount > 3\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+		if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+			((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+			(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)) &&
+			(pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+
+		if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+		{
+			RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+		}
+
+		// tell CNTL state machine to call NdisMSetInformationComplete() after completing
+		// this request, because this request is initiated by NDIS.
+		pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+		// Reset allowed scan retries
+		pAdapter->StaCfg.ScanCnt = 0;
+		pAdapter->StaCfg.LastScanTime = Now;
+
+		MlmeEnqueue(pAdapter,
+			MLME_CNTL_STATE_MACHINE,
+			OID_802_11_BSSID_LIST_SCAN,
+			0,
+			NULL);
+
+		Status = NDIS_STATUS_SUCCESS;
+		RT28XX_MLME_HANDLER(pAdapter);
+	}while(0);
+	return 0;
+}
+
+int rt_ioctl_giwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	int i=0;
+	char *current_ev = extra, *previous_ev = extra;
+	char *end_buf;
+	char *current_val, custom[MAX_CUSTOM_LEN] = {0};
+#ifndef IWEVGENIE
+	char idx;
+#endif // IWEVGENIE //
+	struct iw_event iwe;
+
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+    {
+		/*
+		 * Still scanning, indicate the caller should try again.
+		 */
+		return -EAGAIN;
+	}
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount = 0;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	if (pAdapter->ScanTab.BssNr == 0)
+	{
+		data->length = 0;
+		return 0;
+	}
+
+#if WIRELESS_EXT >= 17
+    if (data->length > 0)
+        end_buf = extra + data->length;
+    else
+        end_buf = extra + IW_SCAN_MAX_DATA;
+#else
+    end_buf = extra + IW_SCAN_MAX_DATA;
+#endif
+
+	for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+	{
+		if (current_ev >= end_buf)
+        {
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+        }
+
+		//MAC address
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWAP;
+		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+				memcpy(iwe.u.ap_addr.sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, ETH_ALEN);
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_ADDR_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//ESSID
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWESSID;
+		iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].SsidLen;
+		iwe.u.data.flags = 1;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_POINT(info, current_ev,end_buf, &iwe, pAdapter->ScanTab.BssEntry[i].Ssid);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Network Type
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWMODE;
+		if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11IBSS)
+		{
+			iwe.u.mode = IW_MODE_ADHOC;
+		}
+		else if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11Infrastructure)
+		{
+			iwe.u.mode = IW_MODE_INFRA;
+		}
+		else
+		{
+			iwe.u.mode = IW_MODE_AUTO;
+		}
+		iwe.len = IW_EV_UINT_LEN;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe,  IW_EV_UINT_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Channel and Frequency
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWFREQ;
+		if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		else
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		iwe.u.freq.e = 0;
+		iwe.u.freq.i = 0;
+
+		previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_FREQ_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+        //Add quality statistics
+        //================================
+        memset(&iwe, 0, sizeof(iwe));
+    	iwe.cmd = IWEVQUAL;
+    	iwe.u.qual.level = 0;
+    	iwe.u.qual.noise = 0;
+        set_quality(pAdapter, &iwe.u.qual, pAdapter->ScanTab.BssEntry[i].Rssi);
+    	current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Encyption key
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWENCODE;
+		if (CAP_IS_PRIVACY_ON (pAdapter->ScanTab.BssEntry[i].CapabilityInfo ))
+			iwe.u.data.flags =IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+		else
+			iwe.u.data.flags = IW_ENCODE_DISABLED;
+
+        previous_ev = current_ev;
+        current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf,&iwe, (char *)pAdapter->SharedKey[BSS0][(iwe.u.data.flags & IW_ENCODE_INDEX)-1].Key);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Bit Rate
+		//================================
+		if (pAdapter->ScanTab.BssEntry[i].SupRateLen)
+        {
+            UCHAR tmpRate = pAdapter->ScanTab.BssEntry[i].SupRate[pAdapter->ScanTab.BssEntry[i].SupRateLen-1];
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWRATE;
+    		current_val = current_ev + IW_EV_LCP_LEN;
+            if (tmpRate == 0x82)
+                iwe.u.bitrate.value =  1 * 1000000;
+            else if (tmpRate == 0x84)
+                iwe.u.bitrate.value =  2 * 1000000;
+            else if (tmpRate == 0x8B)
+                iwe.u.bitrate.value =  5.5 * 1000000;
+            else if (tmpRate == 0x96)
+                iwe.u.bitrate.value =  11 * 1000000;
+            else
+    		    iwe.u.bitrate.value =  (tmpRate/2) * 1000000;
+
+			iwe.u.bitrate.disabled = 0;
+			current_val = IWE_STREAM_ADD_VALUE(info, current_ev,
+				current_val, end_buf, &iwe,
+    			IW_EV_PARAM_LEN);
+
+        	if((current_val-current_ev)>IW_EV_LCP_LEN)
+            	current_ev = current_val;
+        	else
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+#ifdef IWEVGENIE
+		//WPA IE
+		if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+		{
+			memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].WpaIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].WpaIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].WpaIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+		}
+
+		//WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+        	memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].RsnIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].RsnIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].RsnIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#else
+        //WPA IE
+		//================================
+        if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "wpa_ie=", 7);
+            for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].WpaIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].WpaIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+        //WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "rsn_ie=", 7);
+			for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].RsnIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].RsnIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#endif // IWEVGENIE //
+	}
+
+	data->length = current_ev - extra;
+    pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+	DBGPRINT(RT_DEBUG_ERROR ,("===>rt_ioctl_giwscan. %d(%d) BSS returned, data->length = %d\n",i , pAdapter->ScanTab.BssNr, data->length));
+	return 0;
+}
+#endif
+
+int rt_ioctl_siwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (data->flags)
+	{
+		PCHAR	pSsidString = NULL;
+
+		// Includes null character.
+		if (data->length > (IW_ESSID_MAX_SIZE + 1))
+			return -E2BIG;
+
+		pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+		if (pSsidString)
+		{
+			NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+			NdisMoveMemory(pSsidString, essid, data->length);
+			if (Set_SSID_Proc(pAdapter, pSsidString) == FALSE)
+				return -EINVAL;
+		}
+		else
+			return -ENOMEM;
+	}
+	else
+	{
+		// ANY ssid
+		if (Set_SSID_Proc(pAdapter, "") == FALSE)
+			return -EINVAL;
+    }
+	return 0;
+}
+
+int rt_ioctl_giwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	data->flags = 1;
+    if (MONITOR_ON(pAdapter))
+    {
+        data->length  = 0;
+        return 0;
+    }
+
+	if (OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is connected\n"));
+		data->length = pAdapter->CommonCfg.SsidLen;
+		memcpy(essid, pAdapter->CommonCfg.Ssid, pAdapter->CommonCfg.SsidLen);
+	}
+#ifdef RT2870
+#ifdef WPA_SUPPLICANT_SUPPORT
+    // Add for RT2870
+    else if (pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+    {
+        data->length = pAdapter->CommonCfg.SsidLen;
+		memcpy(essid, pAdapter->CommonCfg.Ssid, pAdapter->CommonCfg.SsidLen);
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+#endif // RT2870 //
+	else
+	{//the ANY ssid was specified
+		data->length  = 0;
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is not connected, ess\n"));
+	}
+
+	return 0;
+
+}
+
+int rt_ioctl_siwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE ,("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (data->length > IW_ESSID_MAX_SIZE)
+		return -EINVAL;
+
+	memset(pAdapter->nickname, 0, IW_ESSID_MAX_SIZE + 1);
+	memcpy(pAdapter->nickname, nickname, data->length);
+
+
+	return 0;
+}
+
+int rt_ioctl_giwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (data->length > strlen(pAdapter->nickname) + 1)
+		data->length = strlen(pAdapter->nickname) + 1;
+	if (data->length > 0) {
+		memcpy(nickname, pAdapter->nickname, data->length-1);
+		nickname[data->length-1] = '\0';
+	}
+	return 0;
+}
+
+int rt_ioctl_siwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	u16 val;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (rts->disabled)
+		val = MAX_RTS_THRESHOLD;
+	else if (rts->value < 0 || rts->value > MAX_RTS_THRESHOLD)
+		return -EINVAL;
+	else if (rts->value == 0)
+	    val = MAX_RTS_THRESHOLD;
+	else
+		val = rts->value;
+
+	if (val != pAdapter->CommonCfg.RtsThreshold)
+		pAdapter->CommonCfg.RtsThreshold = val;
+
+	return 0;
+}
+
+int rt_ioctl_giwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	rts->value = pAdapter->CommonCfg.RtsThreshold;
+	rts->disabled = (rts->value == MAX_RTS_THRESHOLD);
+	rts->fixed = 1;
+
+	return 0;
+}
+
+int rt_ioctl_siwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	u16 val;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if (frag->disabled)
+		val = MAX_FRAG_THRESHOLD;
+	else if (frag->value >= MIN_FRAG_THRESHOLD || frag->value <= MAX_FRAG_THRESHOLD)
+        val = __cpu_to_le16(frag->value & ~0x1); /* even numbers only */
+	else if (frag->value == 0)
+	    val = MAX_FRAG_THRESHOLD;
+	else
+		return -EINVAL;
+
+	pAdapter->CommonCfg.FragmentThreshold = val;
+	return 0;
+}
+
+int rt_ioctl_giwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	frag->value = pAdapter->CommonCfg.FragmentThreshold;
+	frag->disabled = (frag->value == MAX_FRAG_THRESHOLD);
+	frag->fixed = 1;
+
+	return 0;
+}
+
+#define MAX_WEP_KEY_SIZE 13
+#define MIN_WEP_KEY_SIZE 5
+int rt_ioctl_siwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if ((erq->length == 0) &&
+        (erq->flags & IW_ENCODE_DISABLED))
+	{
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+	else if ((erq->length == 0) &&
+             (erq->flags & IW_ENCODE_RESTRICTED || erq->flags & IW_ENCODE_OPEN))
+	{
+	    //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+		STA_PORT_SECURED(pAdapter);
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+		if (erq->flags & IW_ENCODE_RESTRICTED)
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    	else
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+
+    if (erq->length > 0)
+	{
+		int keyIdx = (erq->flags & IW_ENCODE_INDEX) - 1;
+		/* Check the size of the key */
+		if (erq->length > MAX_WEP_KEY_SIZE) {
+			return -EINVAL;
+		}
+		/* Check key index */
+		if ((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+        {
+            DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::Wrong keyIdx=%d! Using default key instead (%d)\n",
+                                        keyIdx, pAdapter->StaCfg.DefaultKeyId));
+
+            //Using default key
+			keyIdx = pAdapter->StaCfg.DefaultKeyId;
+        }
+
+        NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+
+		if (erq->length == MAX_WEP_KEY_SIZE)
+        {
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+		}
+		else if (erq->length == MIN_WEP_KEY_SIZE)
+        {
+            pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+		}
+		else
+			/* Disable the key */
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+
+		/* Check if the key is not marked as invalid */
+		if(!(erq->flags & IW_ENCODE_NOKEY)) {
+			/* Copy the key in the driver */
+			NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, extra, erq->length);
+        }
+	}
+    else
+			{
+		/* Do we want to just set the transmit key index ? */
+		int index = (erq->flags & IW_ENCODE_INDEX) - 1;
+		if ((index >= 0) && (index < 4))
+        {
+			pAdapter->StaCfg.DefaultKeyId = index;
+            }
+        else
+			/* Don't complain if only change the mode */
+			if(!erq->flags & IW_ENCODE_MODE) {
+				return -EINVAL;
+		}
+	}
+
+done:
+    DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::erq->flags=%x\n",erq->flags));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::AuthMode=%x\n",pAdapter->StaCfg.AuthMode));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::DefaultKeyId=%x, KeyLen = %d\n",pAdapter->StaCfg.DefaultKeyId , pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::WepStatus=%x\n",pAdapter->StaCfg.WepStatus));
+	return 0;
+}
+
+int
+rt_ioctl_giwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *key)
+{
+	int kid;
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	kid = erq->flags & IW_ENCODE_INDEX;
+	DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_giwencode %d\n", erq->flags & IW_ENCODE_INDEX));
+
+	if (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled)
+	{
+		erq->length = 0;
+		erq->flags = IW_ENCODE_DISABLED;
+	}
+	else if ((kid > 0) && (kid <=4))
+	{
+		// copy wep key
+		erq->flags = kid ;			/* NB: base 1 */
+		if (erq->length > pAdapter->SharedKey[BSS0][kid-1].KeyLen)
+			erq->length = pAdapter->SharedKey[BSS0][kid-1].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][kid-1].Key, erq->length);
+		//if ((kid == pAdapter->PortCfg.DefaultKeyId))
+		//erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+
+	}
+	else if (kid == 0)
+	{
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->length = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, erq->length);
+		// copy default key ID
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->flags = pAdapter->StaCfg.DefaultKeyId + 1;			/* NB: base 1 */
+		erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+	}
+
+	return 0;
+
+}
+
+static int
+rt_ioctl_setparam(struct net_device *dev, struct iw_request_info *info,
+			 void *w, char *extra)
+{
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter;
+	POS_COOKIE pObj;
+	char *this_char = extra;
+	char *value;
+	int  Status=0;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+	pObj = (POS_COOKIE) pAdapter->OS_Cookie;
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+			return -ENETDOWN;
+    	}
+
+	if (!*this_char)
+		return -EINVAL;
+
+	if ((value = rtstrchr(this_char, '=')) != NULL)
+	    *value++ = 0;
+
+	if (!value)
+	    return -EINVAL;
+
+	// reject setting nothing besides ANY ssid(ssidLen=0)
+    if (!*value && (strcmp(this_char, "SSID") != 0))
+        return -EINVAL;
+
+	for (PRTMP_PRIVATE_SET_PROC = RTMP_PRIVATE_SUPPORT_PROC; PRTMP_PRIVATE_SET_PROC->name; PRTMP_PRIVATE_SET_PROC++)
+	{
+	    if (strcmp(this_char, PRTMP_PRIVATE_SET_PROC->name) == 0)
+	    {
+	        if(!PRTMP_PRIVATE_SET_PROC->set_proc(pAdapter, value))
+	        {	//FALSE:Set private failed then return Invalid argument
+			    Status = -EINVAL;
+	        }
+		    break;	//Exit for loop.
+	    }
+	}
+
+	if(PRTMP_PRIVATE_SET_PROC->name == NULL)
+	{  //Not found argument
+	    Status = -EINVAL;
+	    DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_setparam:: (iwpriv) Not Support Set Command [%s=%s]\n", this_char, value));
+	}
+
+    return Status;
+}
+
+
+static int
+rt_private_get_statistics(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+	INT				Status = 0;
+    PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+    sprintf(extra, "\n\n");
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+	    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	    //sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart - (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+	}
+    sprintf(extra+strlen(extra), "Tx success after retry          = %ld\n", (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx fail to Rcv ACK after retry  = %ld\n", (ULONG)pAd->WlanCounters.FailedCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Success Rcv CTS             = %ld\n", (ULONG)pAd->WlanCounters.RTSSuccessCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Fail Rcv CTS                = %ld\n", (ULONG)pAd->WlanCounters.RTSFailureCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "Rx success                      = %ld\n", (ULONG)pAd->WlanCounters.ReceivedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx with CRC                     = %ld\n", (ULONG)pAd->WlanCounters.FCSErrorCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx drop due to out of resource  = %ld\n", (ULONG)pAd->Counters8023.RxNoBuffer);
+    sprintf(extra+strlen(extra), "Rx duplicate frame              = %ld\n", (ULONG)pAd->WlanCounters.FrameDuplicateCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "False CCA (one second)          = %ld\n", (ULONG)pAd->RalinkCounters.OneSecFalseCCACnt);
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		if (pAd->ate.RxAntennaSel == 0)
+		{
+    		sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->ate.LastRssi1 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->ate.LastRssi2 - pAd->BbpRssiToDbmDelta));
+		}
+		else
+		{
+    		sprintf(extra+strlen(extra), "RSSI                            = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+		}
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    	sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi0 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi1 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi2 - pAd->BbpRssiToDbmDelta));
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    sprintf(extra+strlen(extra), "WpaSupplicantUP                 = %d\n\n", pAd->StaCfg.WpaSupplicantUP);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+    DBGPRINT(RT_DEBUG_TRACE, ("<== rt_private_get_statistics, wrq->length = %d\n", wrq->length));
+
+    return Status;
+}
+
+#ifdef DOT11_N_SUPPORT
+void	getBaInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pOutBuf)
+{
+	INT i, j;
+	BA_ORI_ENTRY *pOriBAEntry;
+	BA_REC_ENTRY *pRecBAEntry;
+
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+		if (((pEntry->ValidAsCLI || pEntry->ValidAsApCli) && (pEntry->Sst == SST_ASSOC))
+			|| (pEntry->ValidAsWDS) || (pEntry->ValidAsMesh))
+		{
+			sprintf(pOutBuf, "%s\n%02X:%02X:%02X:%02X:%02X:%02X (Aid = %d) (AP) -\n",
+                pOutBuf,
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5], pEntry->Aid);
+
+			sprintf(pOutBuf, "%s[Recipient]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BARecWcidArray[j] != 0)
+				{
+					pRecBAEntry =&pAd->BATable.BARecEntry[pEntry->BARecWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, LastIndSeq=%d, ReorderingPkts=%d\n", pOutBuf, j, pRecBAEntry->BAWinSize, pRecBAEntry->LastIndSeq, pRecBAEntry->list.qlen);
+				}
+			}
+			sprintf(pOutBuf, "%s\n", pOutBuf);
+
+			sprintf(pOutBuf, "%s[Originator]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BAOriWcidArray[j] != 0)
+				{
+					pOriBAEntry =&pAd->BATable.BAOriEntry[pEntry->BAOriWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, StartSeq=%d, CurTxSeq=%d\n", pOutBuf, j, pOriBAEntry->BAWinSize, pOriBAEntry->Sequence, pEntry->TxSeq[j]);
+				}
+			}
+			sprintf(pOutBuf, "%s\n\n", pOutBuf);
+		}
+        if (strlen(pOutBuf) > (IW_PRIV_SIZE_MASK - 30))
+                break;
+	}
+
+	return;
+}
+#endif // DOT11_N_SUPPORT //
+
+static int
+rt_private_show(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+    INT				Status = 0;
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+    PRTMP_ADAPTER   pAd;
+	POS_COOKIE		pObj;
+    u32             subcmd = wrq->flags;
+
+	if (dev->priv_flags == INT_MAIN)
+		pAd = dev->priv;
+	else
+	{
+		pVirtualAd = dev->priv;
+		pAd = pVirtualAd->RtmpDev->priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+    switch(subcmd)
+    {
+
+        case SHOW_CONN_STATUS:
+            if (MONITOR_ON(pAd))
+            {
+#ifdef DOT11_N_SUPPORT
+                if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                    pAd->CommonCfg.RegTransmitSetting.field.BW)
+                    sprintf(extra, "Monitor Mode(CentralChannel %d)\n", pAd->CommonCfg.CentralChannel);
+                else
+#endif // DOT11_N_SUPPORT //
+                    sprintf(extra, "Monitor Mode(Channel %d)\n", pAd->CommonCfg.Channel);
+            }
+            else
+            {
+                if (pAd->IndicateMediaState == NdisMediaStateConnected)
+            	{
+            	    if (INFRA_ON(pAd))
+                    {
+                    sprintf(extra, "Connected(AP: %s[%02X:%02X:%02X:%02X:%02X:%02X])\n",
+                                    pAd->CommonCfg.Ssid,
+                                    pAd->CommonCfg.Bssid[0],
+                                    pAd->CommonCfg.Bssid[1],
+                                    pAd->CommonCfg.Bssid[2],
+                                    pAd->CommonCfg.Bssid[3],
+                                    pAd->CommonCfg.Bssid[4],
+                                    pAd->CommonCfg.Bssid[5]);
+            		DBGPRINT(RT_DEBUG_TRACE ,("Ssid=%s ,Ssidlen = %d\n",pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen));
+            	}
+                    else if (ADHOC_ON(pAd))
+                        sprintf(extra, "Connected\n");
+            	}
+            	else
+            	{
+            	    sprintf(extra, "Disconnected\n");
+            		DBGPRINT(RT_DEBUG_TRACE ,("ConnStatus is not connected\n"));
+            	}
+            }
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case SHOW_DRVIER_VERION:
+            sprintf(extra, "Driver version-%s, %s %s\n", STA_DRIVER_VERSION, __DATE__, __TIME__ );
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#ifdef DOT11_N_SUPPORT
+        case SHOW_BA_INFO:
+            getBaInfo(pAd, extra);
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#endif // DOT11_N_SUPPORT //
+		case SHOW_DESC_INFO:
+			{
+				Show_DescInfo_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+        case RAIO_OFF:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = FALSE;
+            if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == FALSE)
+                {
+                    MlmeRadioOff(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = SW_RADIO_OFF;
+                }
+            }
+            sprintf(extra, "Radio Off\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case RAIO_ON:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = TRUE;
+            //if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == TRUE)
+                {
+                    MlmeRadioOn(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+                }
+            }
+            sprintf(extra, "Radio On\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case SHOW_DLS_ENTRY_INFO:
+			{
+				Set_DlsEntryInfo_Display_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+
+		case SHOW_CFG_VALUE:
+			{
+				Status = RTMPShowCfgValue(pAd, wrq->pointer, extra);
+				if (Status == 0)
+					wrq->length = strlen(extra) + 1; // 1: size of '\0'
+			}
+			break;
+		case SHOW_ADHOC_ENTRY_INFO:
+			Show_Adhoc_MacTable_Proc(pAd, extra);
+			wrq->length = strlen(extra) + 1; // 1: size of '\0'
+			break;
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("%s - unknow subcmd = %d\n", __func__, subcmd));
+            break;
+    }
+
+    return Status;
+}
+
+#ifdef SIOCSIWMLME
+int rt_ioctl_siwmlme(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+	struct iw_mlme *pMlme = (struct iw_mlme *)wrqu->data.pointer;
+	MLME_QUEUE_ELEM				MsgElem;
+	MLME_DISASSOC_REQ_STRUCT	DisAssocReq;
+	MLME_DEAUTH_REQ_STRUCT      DeAuthReq;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("====> %s\n", __func__));
+
+	if (pMlme == NULL)
+		return -EINVAL;
+
+	switch(pMlme->cmd)
+	{
+#ifdef IW_MLME_DEAUTH
+		case IW_MLME_DEAUTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DEAUTH\n", __func__));
+			COPY_MAC_ADDR(DeAuthReq.Addr, pAd->CommonCfg.Bssid);
+			DeAuthReq.Reason = pMlme->reason_code;
+			MsgElem.MsgLen = sizeof(MLME_DEAUTH_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DeAuthReq, sizeof(MLME_DEAUTH_REQ_STRUCT));
+			MlmeDeauthReqAction(pAd, &MsgElem);
+			if (INFRA_ON(pAd))
+			{
+			    LinkDown(pAd, FALSE);
+			    pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			}
+			break;
+#endif // IW_MLME_DEAUTH //
+#ifdef IW_MLME_DISASSOC
+		case IW_MLME_DISASSOC:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DISASSOC\n", __func__));
+			COPY_MAC_ADDR(DisAssocReq.Addr, pAd->CommonCfg.Bssid);
+			DisAssocReq.Reason =  pMlme->reason_code;
+
+			MsgElem.Machine = ASSOC_STATE_MACHINE;
+			MsgElem.MsgType = MT2_MLME_DISASSOC_REQ;
+			MsgElem.MsgLen = sizeof(MLME_DISASSOC_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DisAssocReq, sizeof(MLME_DISASSOC_REQ_STRUCT));
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+			MlmeDisassocReqAction(pAd, &MsgElem);
+			break;
+#endif // IW_MLME_DISASSOC //
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - Unknow Command\n", __func__));
+			break;
+	}
+
+	return 0;
+}
+#endif // SIOCSIWMLME //
+
+#if WIRELESS_EXT > 17
+int rt_ioctl_siwauth(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = (PRTMP_ADAPTER) dev->priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+	switch (param->flags & IW_AUTH_INDEX) {
+    	case IW_AUTH_WPA_VERSION:
+            if (param->value == IW_AUTH_WPA_VERSION_WPA)
+            {
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+				if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+					pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+            }
+            else if (param->value == IW_AUTH_WPA_VERSION_WPA2)
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_PAIRWISE:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption2Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption3Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_PAIRWISE - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_GROUP:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_GROUP - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_KEY_MGMT:
+            if (param->value == IW_AUTH_KEY_MGMT_802_1X)
+            {
+                if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+                else if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+#ifdef WPA_SUPPLICANT_SUPPORT
+                else
+                    // WEP 1x
+                    pAdapter->StaCfg.IEEE8021X = TRUE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == 0)
+            {
+                //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAdapter);
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_KEY_MGMT - param->value = %d!\n", __func__, param->value));
+            break;
+    	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+            break;
+    	case IW_AUTH_PRIVACY_INVOKED:
+            /*if (param->value == 0)
+			{
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+        	    pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+            }*/
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_PRIVACY_INVOKED - param->value = %d!\n", __func__, param->value));
+    		break;
+    	case IW_AUTH_DROP_UNENCRYPTED:
+            if (param->value != 0)
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			else
+			{
+                //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAdapter);
+			}
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __func__, param->value));
+    		break;
+    	case IW_AUTH_80211_AUTH_ALG:
+			if (param->value & IW_AUTH_ALG_SHARED_KEY)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+			}
+            else if (param->value & IW_AUTH_ALG_OPEN_SYSTEM)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+			}
+            else
+				return -EINVAL;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_80211_AUTH_ALG - param->value = %d!\n", __func__, param->value));
+			break;
+    	case IW_AUTH_WPA_ENABLED:
+    		DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_ENABLED - Driver supports WPA!(param->value = %d)\n", __func__, param->value));
+    		break;
+    	default:
+    		return -EOPNOTSUPP;
+}
+
+	return 0;
+}
+
+int rt_ioctl_giwauth(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = (PRTMP_ADAPTER) dev->priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+    }
+
+	switch (param->flags & IW_AUTH_INDEX) {
+	case IW_AUTH_DROP_UNENCRYPTED:
+        param->value = (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled) ? 0 : 1;
+		break;
+
+	case IW_AUTH_80211_AUTH_ALG:
+        param->value = (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared) ? IW_AUTH_ALG_SHARED_KEY : IW_AUTH_ALG_OPEN_SYSTEM;
+		break;
+
+	case IW_AUTH_WPA_ENABLED:
+		param->value = (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) ? 1 : 0;
+		break;
+
+	default:
+		return -EOPNOTSUPP;
+	}
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_giwauth::param->value = %d!\n", param->value));
+	return 0;
+}
+
+void fnSetCipherKey(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  INT             keyIdx,
+    IN  UCHAR           CipherAlg,
+    IN  BOOLEAN         bGTK,
+    IN  struct iw_encode_ext *ext)
+{
+    NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, LEN_TKIP_EK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].TxMic, ext->key + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].RxMic, ext->key + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CipherAlg;
+
+    // Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAdapter,
+						  BSS0,
+						  keyIdx,
+						  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+						  pAdapter->SharedKey[BSS0][keyIdx].Key,
+						  pAdapter->SharedKey[BSS0][keyIdx].TxMic,
+						  pAdapter->SharedKey[BSS0][keyIdx].RxMic);
+
+    if (bGTK)
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  NULL);
+    else
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  &pAdapter->MacTab.Content[BSSID_WCID]);
+}
+
+int rt_ioctl_siwencodeext(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+			{
+    PRTMP_ADAPTER   pAdapter = (PRTMP_ADAPTER) dev->priv;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+    int keyIdx, alg = ext->alg;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if (encoding->flags & IW_ENCODE_DISABLED)
+	{
+        keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+        // set BSSID wcid entry of the Pair-wise Key table as no-security mode
+	    AsicRemovePairwiseKeyEntry(pAdapter, BSS0, BSSID_WCID);
+        pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+		pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_NONE;
+		AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)keyIdx);
+        NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+        DBGPRINT(RT_DEBUG_TRACE, ("%s::Remove all keys!(encoding->flags = %x)\n", __func__, encoding->flags));
+    }
+					else
+    {
+        // Get Key Index and convet to our own defined key index
+    	keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+    	if((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+    		return -EINVAL;
+
+        if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+        {
+            pAdapter->StaCfg.DefaultKeyId = keyIdx;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::DefaultKeyId = %d\n", __func__, pAdapter->StaCfg.DefaultKeyId));
+        }
+
+        switch (alg) {
+    		case IW_ENCODE_ALG_NONE:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_NONE\n", __func__));
+    			break;
+    		case IW_ENCODE_ALG_WEP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_WEP - ext->key_len = %d, keyIdx = %d\n", __func__, ext->key_len, keyIdx));
+    			if (ext->key_len == MAX_WEP_KEY_SIZE)
+                {
+        			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+				}
+        		else if (ext->key_len == MIN_WEP_KEY_SIZE)
+                {
+                    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+			}
+        		else
+                    return -EINVAL;
+
+                NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+			    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, ext->key_len);
+    			break;
+            case IW_ENCODE_ALG_TKIP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_TKIP - keyIdx = %d, ext->key_len = %d\n", __func__, keyIdx, ext->key_len));
+                if (ext->key_len == 32)
+                {
+                    if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, FALSE, ext);
+                        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                        {
+                            //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+                            STA_PORT_SECURED(pAdapter);
+                        }
+		}
+                    else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, TRUE, ext);
+
+                        // set 802.1x port control
+            	        //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+            	        STA_PORT_SECURED(pAdapter);
+                    }
+                }
+                else
+                    return -EINVAL;
+                break;
+            case IW_ENCODE_ALG_CCMP:
+                if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+		{
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, FALSE, ext);
+                    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                    	//pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+                    	STA_PORT_SECURED(pAdapter);
+                }
+                else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                {
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, TRUE, ext);
+
+                    // set 802.1x port control
+        	        //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+        	        STA_PORT_SECURED(pAdapter);
+                }
+                break;
+    		default:
+    			return -EINVAL;
+		}
+    }
+
+    return 0;
+}
+
+int
+rt_ioctl_giwencodeext(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) dev->priv;
+	PCHAR pKey = NULL;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+	int idx, max_key_len;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_giwencodeext\n"));
+
+	max_key_len = encoding->length - sizeof(*ext);
+	if (max_key_len < 0)
+		return -EINVAL;
+
+	idx = encoding->flags & IW_ENCODE_INDEX;
+	if (idx)
+	{
+		if (idx < 1 || idx > 4)
+			return -EINVAL;
+		idx--;
+
+		if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+			(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled))
+		{
+			if (idx != pAd->StaCfg.DefaultKeyId)
+			{
+				ext->key_len = 0;
+				return 0;
+			}
+		}
+	}
+	else
+		idx = pAd->StaCfg.DefaultKeyId;
+
+	encoding->flags = idx + 1;
+	memset(ext, 0, sizeof(*ext));
+
+	ext->key_len = 0;
+	switch(pAd->StaCfg.WepStatus) {
+		case Ndis802_11WEPDisabled:
+			ext->alg = IW_ENCODE_ALG_NONE;
+			encoding->flags |= IW_ENCODE_DISABLED;
+			break;
+		case Ndis802_11WEPEnabled:
+			ext->alg = IW_ENCODE_ALG_WEP;
+			if (pAd->SharedKey[BSS0][idx].KeyLen > max_key_len)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = pAd->SharedKey[BSS0][idx].KeyLen;
+				pKey = &(pAd->SharedKey[BSS0][idx].Key[0]);
+			}
+			break;
+		case Ndis802_11Encryption2Enabled:
+		case Ndis802_11Encryption3Enabled:
+			if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+				ext->alg = IW_ENCODE_ALG_TKIP;
+			else
+				ext->alg = IW_ENCODE_ALG_CCMP;
+
+			if (max_key_len < 32)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = 32;
+				pKey = &pAd->StaCfg.PMK[0];
+			}
+			break;
+		default:
+			return -EINVAL;
+	}
+
+	if (ext->key_len && pKey)
+	{
+		encoding->flags |= IW_ENCODE_ENABLED;
+		memcpy(ext->key, pKey, ext->key_len);
+	}
+
+	return 0;
+}
+
+#ifdef SIOCSIWGENIE
+int rt_ioctl_siwgenie(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+
+	if (wrqu->data.length > MAX_LEN_OF_RSNIE ||
+	    (wrqu->data.length && extra == NULL))
+		return -EINVAL;
+
+	if (wrqu->data.length)
+	{
+		pAd->StaCfg.RSNIE_Len = wrqu->data.length;
+		NdisMoveMemory(&pAd->StaCfg.RSN_IE[0], extra, pAd->StaCfg.RSNIE_Len);
+	}
+	else
+	{
+		pAd->StaCfg.RSNIE_Len = 0;
+		NdisZeroMemory(&pAd->StaCfg.RSN_IE[0], MAX_LEN_OF_RSNIE);
+	}
+
+	return 0;
+}
+#endif // SIOCSIWGENIE //
+
+int rt_ioctl_giwgenie(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+
+	if ((pAd->StaCfg.RSNIE_Len == 0) ||
+		(pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA))
+	{
+		wrqu->data.length = 0;
+		return 0;
+	}
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifdef SIOCSIWGENIE
+	if (pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+	if (wrqu->data.length < pAd->StaCfg.RSNIE_Len)
+		return -E2BIG;
+
+	wrqu->data.length = pAd->StaCfg.RSNIE_Len;
+	memcpy(extra, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+	else
+#endif // SIOCSIWGENIE //
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+	{
+		UCHAR RSNIe = IE_WPA;
+
+		if (wrqu->data.length < (pAd->StaCfg.RSNIE_Len + 2)) // ID, Len
+			return -E2BIG;
+		wrqu->data.length = pAd->StaCfg.RSNIE_Len + 2;
+
+		if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2))
+			RSNIe = IE_RSN;
+
+		extra[0] = (char)RSNIe;
+		extra[1] = pAd->StaCfg.RSNIE_Len;
+		memcpy(extra+2, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+
+	return 0;
+}
+
+int rt_ioctl_siwpmksa(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+	struct iw_pmksa *pPmksa = (struct iw_pmksa *)wrqu->data.pointer;
+	INT	CachedIdx = 0, idx = 0;
+
+	if (pPmksa == NULL)
+		return -EINVAL;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_siwpmksa\n"));
+	switch(pPmksa->cmd)
+	{
+		case IW_PMKSA_FLUSH:
+			NdisZeroMemory(pAd->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_FLUSH\n"));
+			break;
+		case IW_PMKSA_REMOVE:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+		        {
+		        	NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN);
+					NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].PMKID, 16);
+					for (idx = CachedIdx; idx < (pAd->StaCfg.SavedPMKNum - 1); idx++)
+					{
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].BSSID[0], &pAd->StaCfg.SavedPMK[idx+1].BSSID[0], MAC_ADDR_LEN);
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].PMKID[0], &pAd->StaCfg.SavedPMK[idx+1].PMKID[0], 16);
+					}
+					pAd->StaCfg.SavedPMKNum--;
+			        break;
+		        }
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_REMOVE\n"));
+			break;
+		case IW_PMKSA_ADD:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+			        break;
+	        }
+
+	        // Found, replace it
+	        if (CachedIdx < PMKID_NO)
+	        {
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+		        pAd->StaCfg.SavedPMKNum++;
+	        }
+	        // Not found, replace the last one
+	        else
+	        {
+		        // Randomly replace one
+		        CachedIdx = (pPmksa->bssid.sa_data[5] % PMKID_NO);
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_ADD\n"));
+			break;
+		default:
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - Unknow Command!!\n"));
+			break;
+	}
+
+	return 0;
+}
+#endif // #if WIRELESS_EXT > 17
+
+#ifdef DBG
+static int
+rt_private_ioctl_bbp(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+			{
+	CHAR				*this_char;
+	CHAR				*value = NULL;
+	UCHAR				regBBP = 0;
+//	CHAR				arg[255]={0};
+	UINT32				bbpId;
+	UINT32				bbpValue;
+	BOOLEAN				bIsPrintAllBBP = FALSE;
+	INT					Status = 0;
+    PRTMP_ADAPTER       pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+
+	memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	if (wrq->length > 1) //No parameters.
+				{
+		sprintf(extra, "\n");
+
+		//Parsing Read or Write
+		this_char = wrq->pointer;
+		DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s\n", this_char));
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s, value=%s\n", this_char, value));
+			if (sscanf(this_char, "%d", &(bbpId)) == 1)
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		else
+		{ //Write
+			if ((sscanf(this_char, "%d", &(bbpId)) == 1) && (sscanf(value, "%x", &(bbpValue)) == 1))
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+						//Read it back for showing
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					    RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+    					//Read it back for showing
+    					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		}
+	else
+		bIsPrintAllBBP = TRUE;
+
+next:
+	if (bIsPrintAllBBP)
+	{
+		memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+		sprintf(extra, "\n");
+		for (bbpId = 0; bbpId <= 136; bbpId++)
+		{
+		    if (strlen(extra) >= (IW_PRIV_SIZE_MASK - 10))
+                break;
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+			else
+#endif // RALINK_ATE //
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X    ", bbpId, bbpId*2, regBBP);
+			if (bbpId%5 == 4)
+				sprintf(extra+strlen(extra), "\n");
+		}
+
+        wrq->length = strlen(extra) + 1; // 1: size of '\0'
+        DBGPRINT(RT_DEBUG_TRACE, ("wrq->length = %d\n", wrq->length));
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==rt_private_ioctl_bbp\n\n"));
+
+    return Status;
+}
+#endif // DBG //
+
+int rt_ioctl_siwrate(struct net_device *dev,
+			struct iw_request_info *info,
+			union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+    UINT32          rate = wrqu->bitrate.value, fixed = wrqu->bitrate.fixed;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(rate = %d, fixed = %d)\n", rate, fixed));
+    /* rate = -1 => auto rate
+       rate = X, fixed = 1 => (fixed rate X)
+    */
+    if (rate == -1)
+    {
+		//Auto Rate
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+		pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+		if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+		    (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+			RTMPSetDesiredRates(pAd, -1);
+
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+    }
+    else
+    {
+        if (fixed)
+        {
+        	pAd->StaCfg.bAutoTxRateSwitch = FALSE;
+            if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+                (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+                RTMPSetDesiredRates(pAd, rate);
+            else
+            {
+                pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+#ifdef DOT11_N_SUPPORT
+                SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(HtMcs=%d)\n",pAd->StaCfg.DesiredTransmitSetting.field.MCS));
+        }
+        else
+        {
+            // TODO: rate = X, fixed = 0 => (rates <= X)
+            return -EOPNOTSUPP;
+        }
+    }
+
+    return 0;
+}
+
+int rt_ioctl_giwrate(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+    int rate_index = 0, rate_count = 0;
+    HTTRANSMIT_SETTING ht_setting;
+    __s32 ralinkrate[] =
+	{2,  4,   11,  22, // CCK
+	12, 18,   24,  36, 48, 72, 96, 108, // OFDM
+	13, 26,   39,  52,  78, 104, 117, 130, 26,  52,  78, 104, 156, 208, 234, 260, // 20MHz, 800ns GI, MCS: 0 ~ 15
+	39, 78,  117, 156, 234, 312, 351, 390,										  // 20MHz, 800ns GI, MCS: 16 ~ 23
+	27, 54,   81, 108, 162, 216, 243, 270, 54, 108, 162, 216, 324, 432, 486, 540, // 40MHz, 800ns GI, MCS: 0 ~ 15
+	81, 162, 243, 324, 486, 648, 729, 810,										  // 40MHz, 800ns GI, MCS: 16 ~ 23
+	14, 29,   43,  57,  87, 115, 130, 144, 29, 59,   87, 115, 173, 230, 260, 288, // 20MHz, 400ns GI, MCS: 0 ~ 15
+	43, 87,  130, 173, 260, 317, 390, 433,										  // 20MHz, 400ns GI, MCS: 16 ~ 23
+	30, 60,   90, 120, 180, 240, 270, 300, 60, 120, 180, 240, 360, 480, 540, 600, // 40MHz, 400ns GI, MCS: 0 ~ 15
+	90, 180, 270, 360, 540, 720, 810, 900};										  // 40MHz, 400ns GI, MCS: 16 ~ 23
+
+    rate_count = sizeof(ralinkrate)/sizeof(__s32);
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if ((pAd->StaCfg.bAutoTxRateSwitch == FALSE) &&
+        (INFRA_ON(pAd)) &&
+        ((pAd->CommonCfg.PhyMode <= PHY_11G) || (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM)))
+        ht_setting.word = pAd->StaCfg.HTPhyMode.word;
+    else
+        ht_setting.word = pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+
+#ifdef DOT11_N_SUPPORT
+    if (ht_setting.field.MODE >= MODE_HTMIX)
+    {
+//    	rate_index = 12 + ((UCHAR)ht_setting.field.BW *16) + ((UCHAR)ht_setting.field.ShortGI *32) + ((UCHAR)ht_setting.field.MCS);
+    	rate_index = 12 + ((UCHAR)ht_setting.field.BW *24) + ((UCHAR)ht_setting.field.ShortGI *48) + ((UCHAR)ht_setting.field.MCS);
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+    if (ht_setting.field.MODE == MODE_OFDM)
+    	rate_index = (UCHAR)(ht_setting.field.MCS) + 4;
+    else if (ht_setting.field.MODE == MODE_CCK)
+    	rate_index = (UCHAR)(ht_setting.field.MCS);
+
+    if (rate_index < 0)
+        rate_index = 0;
+
+    if (rate_index > rate_count)
+        rate_index = rate_count;
+
+    wrqu->bitrate.value = ralinkrate[rate_index] * 500000;
+    wrqu->bitrate.disabled = 0;
+
+    return 0;
+}
+
+static const iw_handler rt_handler[] =
+{
+	(iw_handler) NULL,			            /* SIOCSIWCOMMIT */
+	(iw_handler) rt_ioctl_giwname,			/* SIOCGIWNAME   */
+	(iw_handler) NULL,			            /* SIOCSIWNWID   */
+	(iw_handler) NULL,			            /* SIOCGIWNWID   */
+	(iw_handler) rt_ioctl_siwfreq,		    /* SIOCSIWFREQ   */
+	(iw_handler) rt_ioctl_giwfreq,		    /* SIOCGIWFREQ   */
+	(iw_handler) rt_ioctl_siwmode,		    /* SIOCSIWMODE   */
+	(iw_handler) rt_ioctl_giwmode,		    /* SIOCGIWMODE   */
+	(iw_handler) NULL,		                /* SIOCSIWSENS   */
+	(iw_handler) NULL,		                /* SIOCGIWSENS   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWRANGE  */
+	(iw_handler) rt_ioctl_giwrange,		    /* SIOCGIWRANGE  */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWPRIV   */
+	(iw_handler) NULL /* kernel code */,    /* SIOCGIWPRIV   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWSTATS  */
+	(iw_handler) rt28xx_get_wireless_stats /* kernel code */,    /* SIOCGIWSTATS  */
+	(iw_handler) NULL,		                /* SIOCSIWSPY    */
+	(iw_handler) NULL,		                /* SIOCGIWSPY    */
+	(iw_handler) NULL,				        /* SIOCSIWTHRSPY */
+	(iw_handler) NULL,				        /* SIOCGIWTHRSPY */
+	(iw_handler) rt_ioctl_siwap,            /* SIOCSIWAP     */
+	(iw_handler) rt_ioctl_giwap,		    /* SIOCGIWAP     */
+#ifdef SIOCSIWMLME
+	(iw_handler) rt_ioctl_siwmlme,	        /* SIOCSIWMLME   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWMLME */
+#endif // SIOCSIWMLME //
+	(iw_handler) rt_ioctl_iwaplist,		    /* SIOCGIWAPLIST */
+#ifdef SIOCGIWSCAN
+	(iw_handler) rt_ioctl_siwscan,		    /* SIOCSIWSCAN   */
+	(iw_handler) rt_ioctl_giwscan,		    /* SIOCGIWSCAN   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWSCAN   */
+	(iw_handler) NULL,				        /* SIOCGIWSCAN   */
+#endif /* SIOCGIWSCAN */
+	(iw_handler) rt_ioctl_siwessid,		    /* SIOCSIWESSID  */
+	(iw_handler) rt_ioctl_giwessid,		    /* SIOCGIWESSID  */
+	(iw_handler) rt_ioctl_siwnickn,		    /* SIOCSIWNICKN  */
+	(iw_handler) rt_ioctl_giwnickn,		    /* SIOCGIWNICKN  */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) rt_ioctl_siwrate,          /* SIOCSIWRATE   */
+	(iw_handler) rt_ioctl_giwrate,          /* SIOCGIWRATE   */
+	(iw_handler) rt_ioctl_siwrts,		    /* SIOCSIWRTS    */
+	(iw_handler) rt_ioctl_giwrts,		    /* SIOCGIWRTS    */
+	(iw_handler) rt_ioctl_siwfrag,		    /* SIOCSIWFRAG   */
+	(iw_handler) rt_ioctl_giwfrag,		    /* SIOCGIWFRAG   */
+	(iw_handler) NULL,		                /* SIOCSIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCGIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCSIWRETRY  */
+	(iw_handler) NULL,		                /* SIOCGIWRETRY  */
+	(iw_handler) rt_ioctl_siwencode,		/* SIOCSIWENCODE */
+	(iw_handler) rt_ioctl_giwencode,		/* SIOCGIWENCODE */
+	(iw_handler) NULL,		                /* SIOCSIWPOWER  */
+	(iw_handler) NULL,		                /* SIOCGIWPOWER  */
+	(iw_handler) NULL,						/* -- hole -- */
+	(iw_handler) NULL,						/* -- hole -- */
+#if WIRELESS_EXT > 17
+    (iw_handler) rt_ioctl_siwgenie,         /* SIOCSIWGENIE  */
+	(iw_handler) rt_ioctl_giwgenie,         /* SIOCGIWGENIE  */
+	(iw_handler) rt_ioctl_siwauth,		    /* SIOCSIWAUTH   */
+	(iw_handler) rt_ioctl_giwauth,		    /* SIOCGIWAUTH   */
+	(iw_handler) rt_ioctl_siwencodeext,	    /* SIOCSIWENCODEEXT */
+	(iw_handler) rt_ioctl_giwencodeext,		/* SIOCGIWENCODEEXT */
+	(iw_handler) rt_ioctl_siwpmksa,         /* SIOCSIWPMKSA  */
+#endif
+};
+
+static const iw_handler rt_priv_handlers[] = {
+	(iw_handler) NULL, /* + 0x00 */
+	(iw_handler) NULL, /* + 0x01 */
+#ifndef CONFIG_AP_SUPPORT
+	(iw_handler) rt_ioctl_setparam, /* + 0x02 */
+#else
+	(iw_handler) NULL, /* + 0x02 */
+#endif // CONFIG_AP_SUPPORT //
+#ifdef DBG
+	(iw_handler) rt_private_ioctl_bbp, /* + 0x03 */
+#else
+	(iw_handler) NULL, /* + 0x03 */
+#endif
+	(iw_handler) NULL, /* + 0x04 */
+	(iw_handler) NULL, /* + 0x05 */
+	(iw_handler) NULL, /* + 0x06 */
+	(iw_handler) NULL, /* + 0x07 */
+	(iw_handler) NULL, /* + 0x08 */
+	(iw_handler) rt_private_get_statistics, /* + 0x09 */
+	(iw_handler) NULL, /* + 0x0A */
+	(iw_handler) NULL, /* + 0x0B */
+	(iw_handler) NULL, /* + 0x0C */
+	(iw_handler) NULL, /* + 0x0D */
+	(iw_handler) NULL, /* + 0x0E */
+	(iw_handler) NULL, /* + 0x0F */
+	(iw_handler) NULL, /* + 0x10 */
+	(iw_handler) rt_private_show, /* + 0x11 */
+    (iw_handler) NULL, /* + 0x12 */
+	(iw_handler) NULL, /* + 0x13 */
+	(iw_handler) NULL, /* + 0x15 */
+	(iw_handler) NULL, /* + 0x17 */
+	(iw_handler) NULL, /* + 0x18 */
+};
+
+const struct iw_handler_def rt28xx_iw_handler_def =
+{
+#define	N(a)	(sizeof (a) / sizeof (a[0]))
+	.standard	= (iw_handler *) rt_handler,
+	.num_standard	= sizeof(rt_handler) / sizeof(iw_handler),
+	.private	= (iw_handler *) rt_priv_handlers,
+	.num_private		= N(rt_priv_handlers),
+	.private_args	= (struct iw_priv_args *) privtab,
+	.num_private_args	= N(privtab),
+#if IW_HANDLER_VERSION >= 7
+    .get_wireless_stats = rt28xx_get_wireless_stats,
+#endif
+};
+
+INT RTMPSetInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_MAC_ADDRESS             Bssid;
+    RT_802_11_PHY_MODE                  PhyMode;
+    RT_802_11_STA_CONFIG                StaConfig;
+    NDIS_802_11_RATES                   aryRates;
+    RT_802_11_PREAMBLE                  Preamble;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode = Ndis802_11AuthModeMax;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    PNDIS_802_11_KEY                    pKey = NULL;
+    PNDIS_802_11_WEP			        pWepKey =NULL;
+    PNDIS_802_11_REMOVE_KEY             pRemoveKey = NULL;
+    NDIS_802_11_CONFIGURATION           Config, *pConfig = NULL;
+    NDIS_802_11_NETWORK_TYPE            NetType;
+    ULONG                               Now;
+    UINT                                KeyIdx = 0;
+    INT                                 Status = NDIS_STATUS_SUCCESS, MaxPhyMode = PHY_11G;
+    ULONG                               PowerTemp;
+    BOOLEAN                             RadioState;
+    BOOLEAN                             StateMachineTouched = FALSE;
+#ifdef DOT11_N_SUPPORT
+	OID_SET_HT_PHYMODE					HT_PhyMode;	//11n ,kathy
+#endif // DOT11_N_SUPPORT //
+#ifdef WPA_SUPPLICANT_SUPPORT
+    PNDIS_802_11_PMKID                  pPmkId = NULL;
+    BOOLEAN				                IEEE8021xState = FALSE;
+    BOOLEAN				                IEEE8021x_required_keys = FALSE;
+    UCHAR                               wpa_supplicant_enable = 0;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef SNMP_SUPPORT
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						ctmp;
+#endif // SNMP_SUPPORT //
+
+
+
+#ifdef DOT11_N_SUPPORT
+	MaxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-->RTMPSetInformation(),	0x%08x\n", cmd&0x7FFF));
+    switch(cmd & 0x7FFF) {
+        case RT_OID_802_11_COUNTRY_REGION:
+            if (wrq->u.data.length < sizeof(UCHAR))
+                Status = -EINVAL;
+			// Only avaliable when EEPROM not programming
+            else if (!(pAdapter->CommonCfg.CountryRegion & 0x80) && !(pAdapter->CommonCfg.CountryRegionForABand & 0x80))
+            {
+                ULONG   Country;
+                UCHAR	TmpPhy;
+
+				Status = copy_from_user(&Country, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->CommonCfg.CountryRegion = (UCHAR)(Country & 0x000000FF);
+				pAdapter->CommonCfg.CountryRegionForABand = (UCHAR)((Country >> 8) & 0x000000FF);
+                TmpPhy = pAdapter->CommonCfg.PhyMode;
+				pAdapter->CommonCfg.PhyMode = 0xff;
+				// Build all corresponding channel information
+				RTMPSetPhyMode(pAdapter, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+				SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_COUNTRY_REGION (A:%d  B/G:%d)\n", pAdapter->CommonCfg.CountryRegionForABand,
+				    pAdapter->CommonCfg.CountryRegion));
+            }
+            break;
+        case OID_802_11_BSSID_LIST_SCAN:
+ #ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            Now = jiffies;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID_LIST_SCAN, TxCnt = %d \n", pAdapter->RalinkCounters.LastOneSecTotalTxCount));
+
+            if (MONITOR_ON(pAdapter))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+                break;
+            }
+
+			//Benson add 20080527, when radio off, sta don't need to scan
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_RADIO_OFF))
+				break;
+
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			{
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is scanning now !!!\n"));
+				pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+				Status = NDIS_STATUS_SUCCESS;
+                break;
+            }
+
+			if (pAdapter->RalinkCounters.LastOneSecTotalTxCount > 100)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+            if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+				((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)) &&
+                (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+
+            if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+            {
+                RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+            }
+
+            // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+            // this request, because this request is initiated by NDIS.
+            pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+            // Reset allowed scan retries
+            pAdapter->StaCfg.ScanCnt = 0;
+            pAdapter->StaCfg.LastScanTime = Now;
+
+			pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+            RTMP_SET_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+            MlmeEnqueue(pAdapter,
+                        MLME_CNTL_STATE_MACHINE,
+                        OID_802_11_BSSID_LIST_SCAN,
+                        0,
+                        NULL);
+
+            Status = NDIS_STATUS_SUCCESS;
+            StateMachineTouched = TRUE;
+            break;
+        case OID_802_11_SSID:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_SSID))
+                Status = -EINVAL;
+            else
+            {
+            	PCHAR pSsidString = NULL;
+                Status = copy_from_user(&Ssid, wrq->u.data.pointer, wrq->u.data.length);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SSID (Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+                if (Ssid.SsidLength > MAX_LEN_OF_SSID)
+                    Status = -EINVAL;
+                else
+                {
+                	if (Ssid.SsidLength == 0)
+                	{
+                		Set_SSID_Proc(pAdapter, "");
+                	}
+					else
+                	{
+	                	pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+						if (pSsidString)
+						{
+							NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+							NdisMoveMemory(pSsidString, Ssid.Ssid, Ssid.SsidLength);
+	                		Set_SSID_Proc(pAdapter, pSsidString);
+							kfree(pSsidString);
+						}
+						else
+							Status = -ENOMEM;
+                	}
+                }
+            }
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            if (wrq->u.data.length != sizeof(NDIS_802_11_MAC_ADDRESS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Bssid, wrq->u.data.pointer, wrq->u.data.length);
+
+                // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+                // this request, because this request is initiated by NDIS.
+                pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+
+				// Prevent to connect AP again in STAMlmePeriodicExec
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+                // Reset allowed scan retries
+				pAdapter->StaCfg.ScanCnt = 0;
+
+                if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+                {
+                    RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                    DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+                }
+                MlmeEnqueue(pAdapter,
+                            MLME_CNTL_STATE_MACHINE,
+                            OID_802_11_BSSID,
+                            sizeof(NDIS_802_11_MAC_ADDRESS),
+                            (VOID *)&Bssid);
+                Status = NDIS_STATUS_SUCCESS;
+                StateMachineTouched = TRUE;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID %02x:%02x:%02x:%02x:%02x:%02x\n",
+                                        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+            }
+            break;
+        case RT_OID_802_11_RADIO:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RadioState, wrq->u.data.pointer, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RADIO (=%d)\n", RadioState));
+                if (pAdapter->StaCfg.bSwRadio != RadioState)
+                {
+                    pAdapter->StaCfg.bSwRadio = RadioState;
+                    if (pAdapter->StaCfg.bRadio != (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio))
+                    {
+                        pAdapter->StaCfg.bRadio = (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio);
+                        if (pAdapter->StaCfg.bRadio == TRUE)
+                        {
+                            MlmeRadioOn(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = EXTRA_INFO_CLEAR;
+                        }
+                        else
+                        {
+                            MlmeRadioOff(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = SW_RADIO_OFF;
+                        }
+                    }
+                }
+            }
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PHY_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				if (PhyMode <= MaxPhyMode)
+				{
+                	RTMPSetPhyMode(pAdapter, PhyMode);
+#ifdef DOT11_N_SUPPORT
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				}
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PHY_MODE (=%d)\n", PhyMode));
+            }
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            if (wrq->u.data.length != sizeof(RT_802_11_STA_CONFIG))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&StaConfig, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bEnableTxBurst = StaConfig.EnableTxBurst;
+                pAdapter->CommonCfg.UseBGProtection = StaConfig.UseBGProtection;
+                pAdapter->CommonCfg.bUseShortSlotTime = 1; // 2003-10-30 always SHORT SLOT capable
+                if ((pAdapter->CommonCfg.PhyMode != StaConfig.AdhocMode) &&
+					(StaConfig.AdhocMode <= MaxPhyMode))
+                {
+                    // allow dynamic change of "USE OFDM rate or not" in ADHOC mode
+                    // if setting changed, need to reset current TX rate as well as BEACON frame format
+                    if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                    {
+						pAdapter->CommonCfg.PhyMode = StaConfig.AdhocMode;
+                    	RTMPSetPhyMode(pAdapter, PhyMode);
+                        MlmeUpdateTxRates(pAdapter, FALSE, 0);
+                        MakeIbssBeacon(pAdapter);           // re-build BEACON frame
+                        AsicEnableIbssSync(pAdapter);   // copy to on-chip memory
+                    }
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_STA_CONFIG (Burst=%d, Protection=%ld,ShortSlot=%d\n",
+                                        pAdapter->CommonCfg.bEnableTxBurst,
+                                        pAdapter->CommonCfg.UseBGProtection,
+                                        pAdapter->CommonCfg.bUseShortSlotTime));
+            }
+            break;
+        case OID_802_11_DESIRED_RATES:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RATES))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&aryRates, wrq->u.data.pointer, wrq->u.data.length);
+                NdisZeroMemory(pAdapter->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+                NdisMoveMemory(pAdapter->CommonCfg.DesireRate, &aryRates, sizeof(NDIS_802_11_RATES));
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DESIRED_RATES (%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x)\n",
+                    pAdapter->CommonCfg.DesireRate[0],pAdapter->CommonCfg.DesireRate[1],
+                    pAdapter->CommonCfg.DesireRate[2],pAdapter->CommonCfg.DesireRate[3],
+                    pAdapter->CommonCfg.DesireRate[4],pAdapter->CommonCfg.DesireRate[5],
+                    pAdapter->CommonCfg.DesireRate[6],pAdapter->CommonCfg.DesireRate[7] ));
+                // Changing DesiredRate may affect the MAX TX rate we used to TX frames out
+                MlmeUpdateTxRates(pAdapter, FALSE, 0);
+            }
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PREAMBLE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Preamble, wrq->u.data.pointer, wrq->u.data.length);
+                if (Preamble == Rt802_11PreambleShort)
+                {
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleShort);
+                }
+                else if ((Preamble == Rt802_11PreambleLong) || (Preamble == Rt802_11PreambleAuto))
+                {
+                    // if user wants AUTO, initialize to LONG here, then change according to AP's
+                    // capability upon association.
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleLong);
+                }
+                else
+                {
+                    Status = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PREAMBLE (=%d)\n", Preamble));
+            }
+            break;
+        case OID_802_11_WEP_STATUS:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_WEP_STATUS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&WepStatus, wrq->u.data.pointer, wrq->u.data.length);
+                // Since TKIP, AES, WEP are all supported. It should not have any invalid setting
+                if (WepStatus <= Ndis802_11Encryption3KeyAbsent)
+                {
+                    if (pAdapter->StaCfg.WepStatus != WepStatus)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.WepStatus     = WepStatus;
+                    pAdapter->StaCfg.OrigWepStatus = WepStatus;
+                    pAdapter->StaCfg.PairCipher    = WepStatus;
+                	pAdapter->StaCfg.GroupCipher   = WepStatus;
+                }
+                else
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEP_STATUS (=%d)\n",WepStatus));
+            }
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_AUTHENTICATION_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&AuthMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (AuthMode > Ndis802_11AuthModeMax)
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                else
+                {
+                    if (pAdapter->StaCfg.AuthMode != AuthMode)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.AuthMode = AuthMode;
+                }
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_AUTHENTICATION_MODE (=%d) \n",pAdapter->StaCfg.AuthMode));
+            }
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_INFRASTRUCTURE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&BssType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (BssType == Ndis802_11IBSS)
+					Set_NetworkType_Proc(pAdapter, "Adhoc");
+				else if (BssType == Ndis802_11Infrastructure)
+					Set_NetworkType_Proc(pAdapter, "Infra");
+				else if (BssType == Ndis802_11Monitor)
+					Set_NetworkType_Proc(pAdapter, "Monitor");
+				else
+				{
+					Status  = -EINVAL;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_INFRASTRUCTURE_MODE (unknown)\n"));
+				}
+			}
+			break;
+	 case OID_802_11_REMOVE_WEP:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_WEP\n"));
+            if (wrq->u.data.length != sizeof(NDIS_802_11_KEY_INDEX))
+            {
+				Status = -EINVAL;
+            }
+            else
+            {
+				KeyIdx = *(NDIS_802_11_KEY_INDEX *) wrq->u.data.pointer;
+
+				if (KeyIdx & 0x80000000)
+				{
+					// Should never set default bit when remove key
+					Status = -EINVAL;
+				}
+				else
+				{
+					KeyIdx = KeyIdx & 0x0fffffff;
+					if (KeyIdx >= 4){
+						Status = -EINVAL;
+					}
+					else
+					{
+						pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+						pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+						AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+					}
+				}
+            }
+            break;
+        case RT_OID_802_11_RESET_COUNTERS:
+            NdisZeroMemory(&pAdapter->WlanCounters, sizeof(COUNTER_802_11));
+            NdisZeroMemory(&pAdapter->Counters8023, sizeof(COUNTER_802_3));
+            NdisZeroMemory(&pAdapter->RalinkCounters, sizeof(COUNTER_RALINK));
+            pAdapter->Counters8023.RxNoBuffer   = 0;
+			pAdapter->Counters8023.GoodReceives = 0;
+			pAdapter->Counters8023.RxNoBuffer   = 0;
+#ifdef RT2870
+			pAdapter->BulkOutComplete	= 0;
+			pAdapter->BulkOutCompleteOther= 0;
+			pAdapter->BulkOutCompleteCancel = 0;
+			pAdapter->BulkOutReq = 0;
+			pAdapter->BulkInReq= 0;
+			pAdapter->BulkInComplete = 0;
+			pAdapter->BulkInCompleteFail = 0;
+#endif // RT2870 //
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RESET_COUNTERS \n"));
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RTS_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RtsThresh, wrq->u.data.pointer, wrq->u.data.length);
+                if (RtsThresh > MAX_RTS_THRESHOLD)
+                    Status  = -EINVAL;
+                else
+                    pAdapter->CommonCfg.RtsThreshold = (USHORT)RtsThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_RTS_THRESHOLD (=%ld)\n",RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_FRAGMENTATION_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&FragThresh, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bUseZeroToDisableFragment = FALSE;
+                if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+                {
+                    if (FragThresh == 0)
+                    {
+                        pAdapter->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+                        pAdapter->CommonCfg.bUseZeroToDisableFragment = TRUE;
+                    }
+                    else
+                        Status  = -EINVAL;
+                }
+                else
+                    pAdapter->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_FRAGMENTATION_THRESHOLD (=%ld) \n",FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_POWER_MODE))
+                Status = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PowerMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (PowerMode == Ndis802_11PowerModeCAM)
+                	Set_PSMode_Proc(pAdapter, "CAM");
+                else if (PowerMode == Ndis802_11PowerModeMAX_PSP)
+                	Set_PSMode_Proc(pAdapter, "Max_PSP");
+                else if (PowerMode == Ndis802_11PowerModeFast_PSP)
+					Set_PSMode_Proc(pAdapter, "Fast_PSP");
+                else if (PowerMode == Ndis802_11PowerModeLegacy_PSP)
+					Set_PSMode_Proc(pAdapter, "Legacy_PSP");
+                else
+                    Status = -EINVAL;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_POWER_MODE (=%d)\n",PowerMode));
+            break;
+         case RT_OID_802_11_TX_POWER_LEVEL_1:
+			if (wrq->u.data.length  < sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&PowerTemp, wrq->u.data.pointer, wrq->u.data.length);
+				if (PowerTemp > 100)
+					PowerTemp = 0xffffffff;  // AUTO
+				pAdapter->CommonCfg.TxPowerDefault = PowerTemp; //keep current setting.
+					pAdapter->CommonCfg.TxPowerPercentage = pAdapter->CommonCfg.TxPowerDefault;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			}
+	        break;
+		case OID_802_11_NETWORK_TYPE_IN_USE:
+			if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_TYPE))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&NetType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (NetType == Ndis802_11DS)
+					RTMPSetPhyMode(pAdapter, PHY_11B);
+				else if (NetType == Ndis802_11OFDM24)
+					RTMPSetPhyMode(pAdapter, PHY_11BG_MIXED);
+				else if (NetType == Ndis802_11OFDM5)
+					RTMPSetPhyMode(pAdapter, PHY_11A);
+				else
+					Status = -EINVAL;
+#ifdef DOT11_N_SUPPORT
+				if (Status == NDIS_STATUS_SUCCESS)
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_NETWORK_TYPE_IN_USE (=%d)\n",NetType));
+		    }
+			break;
+        // For WPA PSK PMK key
+        case RT_OID_802_11_ADD_WPA:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!!\n"));
+            }
+            else
+            {
+                if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone) )
+                {
+                    Status = -EOPNOTSUPP;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!! [AuthMode != WPAPSK/WPA2PSK/WPANONE]\n"));
+                }
+                else if ((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone) )     // Only for WPA PSK mode
+				{
+                    NdisMoveMemory(pAdapter->StaCfg.PMK, &pKey->KeyMaterial, pKey->KeyLength);
+                    // Use RaConfig as PSK agent.
+                    // Start STA supplicant state machine
+                    if (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+                        pAdapter->StaCfg.WpaState = SS_START;
+
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+                else
+                {
+                    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_REMOVE_KEY:
+            pRemoveKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pRemoveKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pRemoveKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pRemoveKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!\n"));
+            }
+            else
+            {
+                if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+                {
+                    RTMPWPARemoveKeyProc(pAdapter, pRemoveKey);
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Remove WPA Key!!\n"));
+                }
+                else
+                {
+                    KeyIdx = pRemoveKey->KeyIndex;
+
+                    if (KeyIdx & 0x80000000)
+                    {
+                        // Should never set default bit when remove key
+                        Status  = -EINVAL;
+                        DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(Should never set default bit when remove key)\n"));
+                    }
+                    else
+                    {
+                        KeyIdx = KeyIdx & 0x0fffffff;
+                        if (KeyIdx > 3)
+                        {
+                            Status  = -EINVAL;
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(KeyId[%d] out of range)\n", KeyIdx));
+                        }
+                        else
+                        {
+                            pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+                            pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+                            AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY (id=0x%x, Len=%d-byte)\n", pRemoveKey->KeyIndex, pRemoveKey->Length));
+                        }
+                    }
+                }
+            }
+            kfree(pRemoveKey);
+            break;
+        // New for WPA
+        case OID_802_11_ADD_KEY:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY, Failed!!\n"));
+            }
+            else
+            {
+                RTMPAddKey(pAdapter, pKey);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_CONFIGURATION:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_CONFIGURATION))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Config, wrq->u.data.pointer, wrq->u.data.length);
+                pConfig = &Config;
+
+                if ((pConfig->BeaconPeriod >= 20) && (pConfig->BeaconPeriod <=400))
+                     pAdapter->CommonCfg.BeaconPeriod = (USHORT) pConfig->BeaconPeriod;
+
+                pAdapter->StaActive.AtimWin = (USHORT) pConfig->ATIMWindow;
+                MAP_KHZ_TO_CHANNEL_ID(pConfig->DSConfig, pAdapter->CommonCfg.Channel);
+                //
+				// Save the channel on MlmeAux for CntlOidRTBssidProc used.
+				//
+				pAdapter->MlmeAux.Channel = pAdapter->CommonCfg.Channel;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CONFIGURATION (BeacnPeriod=%ld,AtimW=%ld,Ch=%d)\n",
+                    pConfig->BeaconPeriod, pConfig->ATIMWindow, pAdapter->CommonCfg.Channel));
+                // Config has changed
+                pAdapter->bConfigChanged = TRUE;
+            }
+            break;
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_HT_PHYMODE:
+			if (wrq->u.data.length	!= sizeof(OID_SET_HT_PHYMODE))
+				Status = -EINVAL;
+			else
+			{
+			    POID_SET_HT_PHYMODE	pHTPhyMode = &HT_PhyMode;
+
+				Status = copy_from_user(&HT_PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::pHTPhyMode	(PhyMode = %d,TransmitNo = %d, HtMode =	%d,	ExtOffset =	%d , MCS = %d, BW =	%d,	STBC = %d, SHORTGI = %d) \n",
+				pHTPhyMode->PhyMode, pHTPhyMode->TransmitNo,pHTPhyMode->HtMode,pHTPhyMode->ExtOffset,
+				pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC,	pHTPhyMode->SHORTGI));
+				if (pAdapter->CommonCfg.PhyMode	>= PHY_11ABGN_MIXED)
+					RTMPSetHT(pAdapter,	pHTPhyMode);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_HT_PHYMODE(MCS=%d,BW=%d,SGI=%d,STBC=%d)\n",
+				pAdapter->StaCfg.HTPhyMode.field.MCS, pAdapter->StaCfg.HTPhyMode.field.BW, pAdapter->StaCfg.HTPhyMode.field.ShortGI,
+				pAdapter->StaCfg.HTPhyMode.field.STBC));
+			break;
+#endif // DOT11_N_SUPPORT //
+		case RT_OID_802_11_SET_APSD_SETTING:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				ULONG apsd ;
+				Status = copy_from_user(&apsd, wrq->u.data.pointer,	wrq->u.data.length);
+
+				/*-------------------------------------------------------------------
+				|B31~B7	|	B6~B5	 |	 B4	 |	 B3	 |	B2	 |	B1	 |	   B0		|
+				---------------------------------------------------------------------
+				| Rsvd	| Max SP Len | AC_VO | AC_VI | AC_BK | AC_BE | APSD	Capable	|
+				---------------------------------------------------------------------*/
+				pAdapter->CommonCfg.bAPSDCapable = (apsd & 0x00000001) ? TRUE :	FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BE = ((apsd	& 0x00000002) >> 1)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BK = ((apsd	& 0x00000004) >> 2)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VI = ((apsd	& 0x00000008) >> 3)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VO = ((apsd	& 0x00000010) >> 4)	? TRUE : FALSE;
+				pAdapter->CommonCfg.MaxSPLength	= (UCHAR)((apsd	& 0x00000060) >> 5);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_SETTING (apsd=0x%lx, APSDCap=%d, [BE,BK,VI,VO]=[%d/%d/%d/%d],	MaxSPLen=%d)\n", apsd, pAdapter->CommonCfg.bAPSDCapable,
+					pAdapter->CommonCfg.bAPSDAC_BE,	pAdapter->CommonCfg.bAPSDAC_BK,	pAdapter->CommonCfg.bAPSDAC_VI,	pAdapter->CommonCfg.bAPSDAC_VO,	pAdapter->CommonCfg.MaxSPLength));
+			}
+			break;
+
+		case RT_OID_802_11_SET_APSD_PSM:
+			if (wrq->u.data.length	!= sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				// Driver needs	to notify AP when PSM changes
+				Status = copy_from_user(&pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.pointer, wrq->u.data.length);
+				if (pAdapter->CommonCfg.bAPSDForcePowerSave	!= pAdapter->StaCfg.Psm)
+				{
+					MlmeSetPsmBit(pAdapter,	pAdapter->CommonCfg.bAPSDForcePowerSave);
+					RTMPSendNullFrame(pAdapter,	pAdapter->CommonCfg.TxRate,	TRUE);
+				}
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_PSM (bAPSDForcePowerSave:%d)\n",	pAdapter->CommonCfg.bAPSDForcePowerSave));
+			}
+			break;
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_SET_DLS:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				BOOLEAN	oldvalue = pAdapter->CommonCfg.bDLSCapable;
+				Status = copy_from_user(&pAdapter->CommonCfg.bDLSCapable, wrq->u.data.pointer, wrq->u.data.length);
+				if (oldvalue &&	!pAdapter->CommonCfg.bDLSCapable)
+				{
+					int	i;
+					// tear	down local dls table entry
+					for	(i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+
+					// tear	down peer dls table	entry
+					for	(i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS (=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			}
+			break;
+
+		case RT_OID_802_11_SET_DLS_PARAM:
+			if (wrq->u.data.length	!= sizeof(RT_802_11_DLS_UI))
+				Status = -EINVAL;
+			else
+			{
+				RT_802_11_DLS	Dls;
+
+				NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+				RTMPMoveMemory(&Dls, wrq->u.data.pointer, sizeof(RT_802_11_DLS_UI));
+				MlmeEnqueue(pAdapter,
+							MLME_CNTL_STATE_MACHINE,
+							RT_OID_802_11_SET_DLS_PARAM,
+							sizeof(RT_802_11_DLS),
+							&Dls);
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS_PARAM \n"));
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+		case RT_OID_802_11_SET_WMM:
+			if (wrq->u.data.length	!= sizeof(BOOLEAN))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&pAdapter->CommonCfg.bWmmCapable, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_WMM (=%d)	\n", pAdapter->CommonCfg.bWmmCapable));
+			}
+			break;
+
+		case OID_802_11_DISASSOCIATE:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+			//
+			// Set NdisRadioStateOff to	TRUE, instead of called	MlmeRadioOff.
+			// Later on, NDIS_802_11_BSSID_LIST_EX->NumberOfItems should be	0
+			// when	query OID_802_11_BSSID_LIST.
+			//
+			// TRUE:  NumberOfItems	will set to	0.
+			// FALSE: NumberOfItems	no change.
+			//
+			pAdapter->CommonCfg.NdisRadioStateOff =	TRUE;
+			// Set to immediately send the media disconnect	event
+			pAdapter->MlmeAux.CurrReqIsFromNdis	= TRUE;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DISASSOCIATE	\n"));
+
+			if (INFRA_ON(pAdapter))
+			{
+				if (pAdapter->Mlme.CntlMachine.CurrState !=	CNTL_IDLE)
+				{
+					RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+					DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME	busy, reset	MLME state machine !!!\n"));
+				}
+
+				MlmeEnqueue(pAdapter,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_DISASSOCIATE,
+					0,
+					NULL);
+
+				StateMachineTouched	= TRUE;
+			}
+			break;
+
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_IMME_BA_CAP:
+				if (wrq->u.data.length != sizeof(OID_BACAP_STRUC))
+					Status = -EINVAL;
+				else
+				{
+					OID_BACAP_STRUC Orde ;
+					Status = copy_from_user(&Orde, wrq->u.data.pointer, wrq->u.data.length);
+					if (Orde.Policy > BA_NOTUSE)
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+					}
+					else if (Orde.Policy == BA_NOTUSE)
+					{
+						pAdapter->CommonCfg.BACapability.field.Policy = BA_NOTUSE;
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs= Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+					}
+					else
+					{
+                        pAdapter->CommonCfg.BACapability.field.AutoBA = Orde.AutoBA;
+						pAdapter->CommonCfg.BACapability.field.Policy = IMMED_BA; // we only support immediate BA.
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+
+						if (pAdapter->CommonCfg.BACapability.field.RxBAWinLimit > MAX_RX_REORDERBUF)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = MAX_RX_REORDERBUF;
+
+					}
+
+					pAdapter->CommonCfg.REGBACapability.word = pAdapter->CommonCfg.BACapability.word;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(Orde.AutoBA = %d) (Policy=%d)(ReBAWinLimit=%d)(TxBAWinLimit=%d)(AutoMode=%d)\n",Orde.AutoBA, pAdapter->CommonCfg.BACapability.field.Policy,
+						pAdapter->CommonCfg.BACapability.field.RxBAWinLimit,pAdapter->CommonCfg.BACapability.field.TxBAWinLimit, pAdapter->CommonCfg.BACapability.field.AutoBA));
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(MimoPs = %d)(AmsduEnable = %d) (AmsduSize=%d)(MpduDensity=%d)\n",pAdapter->CommonCfg.DesiredHtPhy.MimoPs, pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable,
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize, pAdapter->CommonCfg.DesiredHtPhy.MpduDensity));
+				}
+
+				break;
+		case RT_OID_802_11_ADD_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, (" Set :: RT_OID_802_11_ADD_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				UCHAR		        index;
+				OID_ADD_BA_ENTRY    BA;
+				MAC_TABLE_ENTRY     *pEntry;
+
+				Status = copy_from_user(&BA, wrq->u.data.pointer, wrq->u.data.length);
+				if (BA.TID > 15)
+				{
+					Status = NDIS_STATUS_INVALID_DATA;
+					break;
+				}
+				else
+				{
+					//BATableInsertEntry
+					//As ad-hoc mode, BA pair is not limited to only BSSID. so add via OID.
+					index = BA.TID;
+					// in ad hoc mode, when adding BA pair, we should insert this entry into MACEntry too
+					pEntry = MacTableLookup(pAdapter, BA.MACAddr);
+					if (!pEntry)
+					{
+						DBGPRINT(RT_DEBUG_TRACE, ("RT_OID_802_11_ADD_IMME_BA. break on no connection.----:%x:%x\n", BA.MACAddr[4], BA.MACAddr[5]));
+						break;
+					}
+					if (BA.IsRecipient == FALSE)
+					{
+					    if (pEntry->bIAmBadAtheros == TRUE)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = 0x10;
+
+						BAOriSessionSetUp(pAdapter, pEntry, index, 0, 100, TRUE);
+					}
+					else
+					{
+						//BATableInsertEntry(pAdapter, pEntry->Aid, BA.MACAddr, 0, 0xffff, BA.TID, BA.nMSDU, BA.IsRecipient);
+					}
+
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_IMME_BA. Rec = %d. Mac = %x:%x:%x:%x:%x:%x . \n",
+						BA.IsRecipient, BA.MACAddr[0], BA.MACAddr[1], BA.MACAddr[2], BA.MACAddr[2]
+						, BA.MACAddr[4], BA.MACAddr[5]));
+				}
+			}
+			break;
+
+		case RT_OID_802_11_TEAR_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				POID_ADD_BA_ENTRY	pBA;
+				MAC_TABLE_ENTRY *pEntry;
+
+				pBA = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+				if (pBA == NULL)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA kmalloc() can't allocate enough memory\n"));
+					Status = NDIS_STATUS_FAILURE;
+				}
+				else
+				{
+					Status = copy_from_user(pBA, wrq->u.data.pointer, wrq->u.data.length);
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA(TID=%d, bAllTid=%d)\n", pBA->TID, pBA->bAllTid));
+
+					if (!pBA->bAllTid && (pBA->TID > NUM_OF_TID))
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+						break;
+					}
+
+					if (pBA->IsRecipient == FALSE)
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						DBGPRINT(RT_DEBUG_TRACE, (" pBA->IsRecipient == FALSE\n"));
+						if (pEntry)
+						{
+							DBGPRINT(RT_DEBUG_TRACE, (" pBA->pEntry\n"));
+							BAOriSessionTearDown(pAdapter, pEntry->Aid, pBA->TID, FALSE, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					else
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						if (pEntry)
+						{
+							BARecSessionTearDown( pAdapter, (UCHAR)pEntry->Aid, pBA->TID, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					kfree(pBA);
+				}
+            }
+            break;
+#endif // DOT11_N_SUPPORT //
+
+        // For WPA_SUPPLICANT to set static wep key
+    	case OID_802_11_ADD_WEP:
+    	    pWepKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+    	    if(pWepKey == NULL)
+            {
+                Status = -ENOMEM;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed!!\n"));
+                break;
+            }
+            Status = copy_from_user(pWepKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (Status)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (length mismatch)!!\n"));
+            }
+            else
+            {
+		        KeyIdx = pWepKey->KeyIndex & 0x0fffffff;
+                // KeyIdx must be 0 ~ 3
+                if (KeyIdx > 4)
+    			{
+                    Status  = -EINVAL;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (KeyIdx must be smaller than 4)!!\n"));
+                }
+                else
+                {
+                    UCHAR CipherAlg = 0;
+                    PUCHAR Key;
+
+                    // set key material and key length
+                    NdisZeroMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, 16);
+                    pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                    NdisMoveMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+
+                    switch(pWepKey->KeyLength)
+                    {
+                        case 5:
+                            CipherAlg = CIPHER_WEP64;
+                            break;
+                        case 13:
+                            CipherAlg = CIPHER_WEP128;
+                            break;
+                        default:
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, only support CIPHER_WEP64(len:5) & CIPHER_WEP128(len:13)!!\n"));
+                            Status = -EINVAL;
+                            break;
+                    }
+                    pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CipherAlg;
+
+                    // Default key for tx (shared key)
+                    if (pWepKey->KeyIndex & 0x80000000)
+                    {
+#ifdef WPA_SUPPLICANT_SUPPORT
+                        // set key material and key length
+                        NdisZeroMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, 16);
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                        NdisMoveMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+                        pAdapter->StaCfg.DesireSharedKeyId = KeyIdx;
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].CipherAlg = CipherAlg;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                        pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+                    }
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    if (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED)
+#endif // WPA_SUPPLICANT_SUPPORT
+                    {
+                        Key = pAdapter->SharedKey[BSS0][KeyIdx].Key;
+
+                        // Set key material and cipherAlg to Asic
+        				AsicAddSharedKeyEntry(pAdapter, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+                        if (pWepKey->KeyIndex & 0x80000000)
+                        {
+                            PMAC_TABLE_ENTRY pEntry = &pAdapter->MacTab.Content[BSSID_WCID];
+                            // Assign group key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, NULL);
+    						// Assign pairwise key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, pEntry);
+                        }
+                    }
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP (id=0x%x, Len=%d-byte), %s\n", pWepKey->KeyIndex, pWepKey->KeyLength, (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED) ? "Port Secured":"Port NOT Secured"));
+				}
+            }
+            kfree(pWepKey);
+            break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+	    case OID_SET_COUNTERMEASURES:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.bBlockAssoc = TRUE;
+                else
+                    // WPA MIC error should block association attempt for 60 seconds
+                    pAdapter->StaCfg.bBlockAssoc = FALSE;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_SET_COUNTERMEASURES bBlockAssoc=%s\n", pAdapter->StaCfg.bBlockAssoc ? "TRUE":"FALSE"));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+			if (wrq->u.data.length != sizeof(UCHAR))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&wpa_supplicant_enable, wrq->u.data.pointer, wrq->u.data.length);
+    			pAdapter->StaCfg.WpaSupplicantUP = wpa_supplicant_enable;
+    			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+			}
+            break;
+        case OID_802_11_DEAUTHENTICATION:
+            if (wrq->u.data.length != sizeof(MLME_DEAUTH_REQ_STRUCT))
+                Status  = -EINVAL;
+            else
+            {
+                MLME_DEAUTH_REQ_STRUCT      *pInfo;
+				MLME_QUEUE_ELEM *MsgElem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+                pInfo = (MLME_DEAUTH_REQ_STRUCT *) MsgElem->Msg;
+                Status = copy_from_user(pInfo, wrq->u.data.pointer, wrq->u.data.length);
+                MlmeDeauthReqAction(pAdapter, MsgElem);
+				kfree(MsgElem);
+
+                if (INFRA_ON(pAdapter))
+                {
+                    LinkDown(pAdapter, FALSE);
+                    pAdapter->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DEAUTHENTICATION (Reason=%d)\n", pInfo->Reason));
+            }
+            break;
+        case OID_802_11_DROP_UNENCRYPTED:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                else
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				NdisAcquireSpinLock(&pAdapter->MacTabLock);
+				pAdapter->MacTab.Content[BSSID_WCID].PortSecured = pAdapter->StaCfg.PortSecured;
+				NdisReleaseSpinLock(&pAdapter->MacTabLock);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DROP_UNENCRYPTED (=%d)\n", enabled));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021xState, wrq->u.data.pointer, wrq->u.data.length);
+		        pAdapter->StaCfg.IEEE8021X = IEEE8021xState;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X (=%d)\n", IEEE8021xState));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X_REQUIRE_KEY:
+			if (wrq->u.data.length != sizeof(BOOLEAN))
+				 Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021x_required_keys, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->StaCfg.IEEE8021x_required_keys = IEEE8021x_required_keys;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X_REQUIRE_KEY (%d)\n", IEEE8021x_required_keys));
+			}
+			break;
+        case OID_802_11_PMKID:
+	        pPmkId = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+	        if(pPmkId == NULL) {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pPmkId, wrq->u.data.pointer, wrq->u.data.length);
+
+	        // check the PMKID information
+	        if (pPmkId->BSSIDInfoCount == 0)
+                NdisZeroMemory(pAdapter->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+	        else
+	        {
+		        PBSSID_INFO	pBssIdInfo;
+		        UINT		BssIdx;
+		        UINT		CachedIdx;
+
+		        for (BssIdx = 0; BssIdx < pPmkId->BSSIDInfoCount; BssIdx++)
+		        {
+			        // point to the indexed BSSID_INFO structure
+			        pBssIdInfo = (PBSSID_INFO) ((PUCHAR) pPmkId + 2 * sizeof(UINT) + BssIdx * sizeof(BSSID_INFO));
+			        // Find the entry in the saved data base.
+			        for (CachedIdx = 0; CachedIdx < pAdapter->StaCfg.SavedPMKNum; CachedIdx++)
+			        {
+				        // compare the BSSID
+				        if (NdisEqualMemory(pBssIdInfo->BSSID, pAdapter->StaCfg.SavedPMK[CachedIdx].BSSID, sizeof(NDIS_802_11_MAC_ADDRESS)))
+					        break;
+			        }
+
+			        // Found, replace it
+			        if (CachedIdx < PMKID_NO)
+			        {
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+				        pAdapter->StaCfg.SavedPMKNum++;
+			        }
+			        // Not found, replace the last one
+			        else
+			        {
+				        // Randomly replace one
+				        CachedIdx = (pBssIdInfo->BSSID[5] % PMKID_NO);
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+			        }
+		        }
+			}
+			if(pPmkId)
+				kfree(pPmkId);
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+#ifdef SNMP_SUPPORT
+		case OID_802_11_SHORTRETRYLIMIT:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ShortRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SHORTRETRYLIMIT (tx_rty_cfg.field.ShortRetryLimit=%d, ShortRetryLimit=%ld)\n", tx_rty_cfg.field.ShortRtyLimit, ShortRetryLimit));
+			}
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT \n"));
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&LongRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT (tx_rty_cfg.field.LongRetryLimit= %d,LongRetryLimit=%ld)\n", tx_rty_cfg.field.LongRtyLimit, LongRetryLimit));
+			}
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE\n"));
+			pKey = kmalloc(wrq->u.data.length, GFP_KERNEL);
+			Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+			//pKey = &WepKey;
+
+			if ( pKey->Length != wrq->u.data.length)
+			{
+				Status = -EINVAL;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE, Failed!!\n"));
+			}
+			KeyIdx = pKey->KeyIndex & 0x0fffffff;
+			DBGPRINT(RT_DEBUG_TRACE,("pKey->KeyIndex =%d, pKey->KeyLength=%d\n", pKey->KeyIndex, pKey->KeyLength));
+
+			// it is a shared key
+			if (KeyIdx > 4)
+				Status = -EINVAL;
+			else
+			{
+				pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, &pKey->KeyMaterial, pKey->KeyLength);
+				if (pKey->KeyIndex & 0x80000000)
+				{
+					// Default key for tx (shared key)
+					pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+				}
+				//RestartAPIsRequired = TRUE;
+			}
+			break;
+
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYID \n"));
+
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+				Status = copy_from_user(&pAdapter->StaCfg.DefaultKeyId, wrq->u.data.pointer, wrq->u.data.length);
+
+			break;
+
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CURRENTCHANNEL \n"));
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ctmp, wrq->u.data.pointer, wrq->u.data.length);
+				sprintf(&ctmp,"%d", ctmp);
+				Set_Channel_Proc(pAdapter, &ctmp);
+			}
+			break;
+#endif
+
+
+
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+
+
+    return Status;
+}
+
+INT RTMPQueryInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_BSSID_LIST_EX           *pBssidList = NULL;
+    PNDIS_WLAN_BSSID_EX                 pBss;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_CONFIGURATION           *pConfiguration = NULL;
+    RT_802_11_LINK_STATUS               *pLinkStatus = NULL;
+    RT_802_11_STA_CONFIG                *pStaConfig = NULL;
+    NDIS_802_11_STATISTICS              *pStatistics = NULL;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    RT_802_11_PREAMBLE                  PreamType;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_MEDIA_STATE                    MediaState;
+    ULONG                               BssBufSize, ulInfo=0, NetworkTypeList[4], apsd = 0;
+    USHORT                              BssLen = 0;
+    PUCHAR                              pBuf = NULL, pPtr;
+    INT                                 Status = NDIS_STATUS_SUCCESS;
+    UINT                                we_version_compiled;
+    UCHAR                               i, Padding = 0;
+    BOOLEAN                             RadioState;
+	UCHAR	driverVersion[8];
+    OID_SET_HT_PHYMODE			        *pHTPhyMode = NULL;
+
+
+#ifdef SNMP_SUPPORT
+	//for snmp, kathy
+	DefaultKeyIdxValue			*pKeyIdxValue;
+	INT							valueLen;
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						tmp[64];
+#endif //SNMP
+
+    switch(cmd)
+    {
+        case RT_OID_DEVICE_NAME:
+            wrq->u.data.length = sizeof(STA_NIC_DEVICE_NAME);
+            Status = copy_to_user(wrq->u.data.pointer, STA_NIC_DEVICE_NAME, wrq->u.data.length);
+            break;
+        case RT_OID_VERSION_INFO:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_VERSION_INFO \n"));
+			wrq->u.data.length = 8*sizeof(UCHAR);
+			sprintf(&driverVersion[0], "%s", STA_DRIVER_VERSION);
+			driverVersion[7] = '\0';
+			if (copy_to_user(wrq->u.data.pointer, &driverVersion, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+#ifdef RALINK_ATE
+		case RT_QUERY_ATE_TXDONE_COUNT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_QUERY_ATE_TXDONE_COUNT \n"));
+			wrq->u.data.length = sizeof(UINT32);
+			if (copy_to_user(wrq->u.data.pointer, &pAdapter->ate.TxDoneCount, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+			break;
+#endif // RALINK_ATE //
+        case OID_802_11_BSSID_LIST:
+            if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+            	/*
+            	 * Still scanning, indicate the caller should try again.
+            	 */
+            	DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (Still scanning)\n"));
+				return -EAGAIN;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (%d BSS returned)\n",pAdapter->ScanTab.BssNr));
+			pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+            // Claculate total buffer size required
+            BssBufSize = sizeof(ULONG);
+
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                // Align pointer to 4 bytes boundary.
+                //Padding = 4 - (pAdapter->ScanTab.BssEntry[i].VarIELen & 0x0003);
+                //if (Padding == 4)
+                //    Padding = 0;
+                BssBufSize += (sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+            }
+
+            // For safety issue, we add 256 bytes just in case
+            BssBufSize += 256;
+            // Allocate the same size as passed from higher layer
+            pBuf = kmalloc(BssBufSize, MEM_ALLOC_FLAG);
+            if(pBuf == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            // Init 802_11_BSSID_LIST_EX structure
+            NdisZeroMemory(pBuf, BssBufSize);
+            pBssidList = (PNDIS_802_11_BSSID_LIST_EX) pBuf;
+            pBssidList->NumberOfItems = pAdapter->ScanTab.BssNr;
+
+            // Calculate total buffer length
+            BssLen = 4; // Consist of NumberOfItems
+            // Point to start of NDIS_WLAN_BSSID_EX
+            // pPtr = pBuf + sizeof(ULONG);
+            pPtr = (PUCHAR) &pBssidList->Bssid[0];
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                pBss = (PNDIS_WLAN_BSSID_EX) pPtr;
+                NdisMoveMemory(&pBss->MacAddress, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+                if ((pAdapter->ScanTab.BssEntry[i].Hidden == 1) && (pAdapter->StaCfg.bShowHiddenSSID == FALSE))
+                {
+                    //
+					// We must return this SSID during 4way handshaking, otherwise Aegis will failed to parse WPA infomation
+					// and then failed to send EAPOl farame.
+					//
+					if ((pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) && (pAdapter->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED))
+					{
+						pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+						NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+					}
+					else
+                    	pBss->Ssid.SsidLength = 0;
+                }
+                else
+                {
+                    pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+                    NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+                }
+                pBss->Privacy = pAdapter->ScanTab.BssEntry[i].Privacy;
+                pBss->Rssi = pAdapter->ScanTab.BssEntry[i].Rssi - pAdapter->BbpRssiToDbmDelta;
+                pBss->NetworkTypeInUse = NetworkTypeInUseSanity(&pAdapter->ScanTab.BssEntry[i]);
+                pBss->Configuration.Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pBss->Configuration.BeaconPeriod = pAdapter->ScanTab.BssEntry[i].BeaconPeriod;
+                pBss->Configuration.ATIMWindow = pAdapter->ScanTab.BssEntry[i].AtimWin;
+
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->ScanTab.BssEntry[i].Channel, pBss->Configuration.DSConfig);
+
+                if (pAdapter->ScanTab.BssEntry[i].BssType == BSS_INFRA)
+                    pBss->InfrastructureMode = Ndis802_11Infrastructure;
+                else
+                    pBss->InfrastructureMode = Ndis802_11IBSS;
+
+                NdisMoveMemory(pBss->SupportedRates, pAdapter->ScanTab.BssEntry[i].SupRate, pAdapter->ScanTab.BssEntry[i].SupRateLen);
+                NdisMoveMemory(pBss->SupportedRates + pAdapter->ScanTab.BssEntry[i].SupRateLen,
+                               pAdapter->ScanTab.BssEntry[i].ExtRate,
+                               pAdapter->ScanTab.BssEntry[i].ExtRateLen);
+
+                if (pAdapter->ScanTab.BssEntry[i].VarIELen == 0)
+                {
+                    pBss->IELength = sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                }
+                else
+                {
+                    pBss->IELength = (ULONG)(sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    NdisMoveMemory(pBss->IEs + sizeof(NDIS_802_11_FIXED_IEs), pAdapter->ScanTab.BssEntry[i].VarIEs, pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr += pAdapter->ScanTab.BssEntry[i].VarIELen;
+                }
+                pBss->Length = (ULONG)(sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+
+#if WIRELESS_EXT < 17
+                if ((BssLen + pBss->Length) < wrq->u.data.length)
+                BssLen += pBss->Length;
+                else
+                {
+                    pBssidList->NumberOfItems = i;
+                    break;
+                }
+#else
+                BssLen += pBss->Length;
+#endif
+            }
+
+#if WIRELESS_EXT < 17
+            wrq->u.data.length = BssLen;
+#else
+            if (BssLen > wrq->u.data.length)
+            {
+                kfree(pBssidList);
+                return -E2BIG;
+            }
+            else
+                wrq->u.data.length = BssLen;
+#endif
+            Status = copy_to_user(wrq->u.data.pointer, pBssidList, BssLen);
+            kfree(pBssidList);
+            break;
+        case OID_802_3_CURRENT_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+            break;
+        case OID_GEN_MEDIA_CONNECT_STATUS:
+            if (pAdapter->IndicateMediaState == NdisMediaStateConnected)
+                MediaState = NdisMediaStateConnected;
+            else
+                MediaState = NdisMediaStateDisconnected;
+
+            wrq->u.data.length = sizeof(NDIS_MEDIA_STATE);
+            Status = copy_to_user(wrq->u.data.pointer, &MediaState, wrq->u.data.length);
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				Status = NDIS_STATUS_RESOURCES;
+				break;
+			}
+#endif // RALINK_ATE //
+            if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+            {
+                Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Bssid, sizeof(NDIS_802_11_MAC_ADDRESS));
+
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID(=EMPTY)\n"));
+                Status = -ENOTCONN;
+            }
+            break;
+        case OID_802_11_SSID:
+			NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+			NdisZeroMemory(Ssid.Ssid, MAX_LEN_OF_SSID);
+            Ssid.SsidLength = pAdapter->CommonCfg.SsidLen;
+			memcpy(Ssid.Ssid, pAdapter->CommonCfg.Ssid,	Ssid.SsidLength);
+            wrq->u.data.length = sizeof(NDIS_802_11_SSID);
+            Status = copy_to_user(wrq->u.data.pointer, &Ssid, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SSID (Len=%d, ssid=%s)\n", Ssid.SsidLength,Ssid.Ssid));
+            break;
+        case RT_OID_802_11_QUERY_LINK_STATUS:
+            pLinkStatus = (RT_802_11_LINK_STATUS *) kmalloc(sizeof(RT_802_11_LINK_STATUS), MEM_ALLOC_FLAG);
+            if (pLinkStatus)
+            {
+                pLinkStatus->CurrTxRate = RateIdTo500Kbps[pAdapter->CommonCfg.TxRate];   // unit : 500 kbps
+                pLinkStatus->ChannelQuality = pAdapter->Mlme.ChannelQuality;
+                pLinkStatus->RxByteCount = pAdapter->RalinkCounters.ReceivedByteCount;
+                pLinkStatus->TxByteCount = pAdapter->RalinkCounters.TransmittedByteCount;
+        		pLinkStatus->CentralChannel = pAdapter->CommonCfg.CentralChannel;
+                wrq->u.data.length = sizeof(RT_802_11_LINK_STATUS);
+                Status = copy_to_user(wrq->u.data.pointer, pLinkStatus, wrq->u.data.length);
+                kfree(pLinkStatus);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS\n"));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_CONFIGURATION:
+            pConfiguration = (NDIS_802_11_CONFIGURATION *) kmalloc(sizeof(NDIS_802_11_CONFIGURATION), MEM_ALLOC_FLAG);
+            if (pConfiguration)
+            {
+                pConfiguration->Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pConfiguration->BeaconPeriod = pAdapter->CommonCfg.BeaconPeriod;
+                pConfiguration->ATIMWindow = pAdapter->StaActive.AtimWin;
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->CommonCfg.Channel, pConfiguration->DSConfig);
+                wrq->u.data.length = sizeof(NDIS_802_11_CONFIGURATION);
+                Status = copy_to_user(wrq->u.data.pointer, pConfiguration, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(BeaconPeriod=%ld,AtimW=%ld,Channel=%d) \n",
+                                        pConfiguration->BeaconPeriod, pConfiguration->ATIMWindow, pAdapter->CommonCfg.Channel));
+				kfree(pConfiguration);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+		case RT_OID_802_11_SNR_0:
+			if ((pAdapter->StaCfg.LastSNR0 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR0) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_SNR_0(0x=%lx)\n", ulInfo));
+			}
+            else
+			    Status = -EFAULT;
+			break;
+		case RT_OID_802_11_SNR_1:
+			if ((pAdapter->Antenna.field.RxPath	> 1) &&
+                (pAdapter->StaCfg.LastSNR1 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR1) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(0x=%lx)\n",ulInfo));
+			}
+			else
+				Status = -EFAULT;
+            DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(pAdapter->StaCfg.LastSNR1=%d)\n",pAdapter->StaCfg.LastSNR1));
+			break;
+        case OID_802_11_RSSI_TRIGGER:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0 - pAdapter->BbpRssiToDbmDelta;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RSSI_TRIGGER(=%ld)\n", ulInfo));
+            break;
+		case OID_802_11_RSSI:
+        case RT_OID_802_11_RSSI:
+			ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+		case RT_OID_802_11_RSSI_1:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi1;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case RT_OID_802_11_RSSI_2:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi2;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case OID_802_11_STATISTICS:
+            pStatistics = (NDIS_802_11_STATISTICS *) kmalloc(sizeof(NDIS_802_11_STATISTICS), MEM_ALLOC_FLAG);
+            if (pStatistics)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS \n"));
+                // add the most up-to-date h/w raw counters into software counters
+			    NICUpdateRawCounters(pAdapter);
+
+                // Sanity check for calculation of sucessful count
+                if (pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart < pAdapter->WlanCounters.RetryCount.QuadPart)
+                    pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+
+                pStatistics->TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart;
+                pStatistics->MulticastTransmittedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastTransmittedFrameCount.QuadPart;
+                pStatistics->FailedCount.QuadPart = pAdapter->WlanCounters.FailedCount.QuadPart;
+                pStatistics->RetryCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+                pStatistics->MultipleRetryCount.QuadPart = pAdapter->WlanCounters.MultipleRetryCount.QuadPart;
+                pStatistics->RTSSuccessCount.QuadPart = pAdapter->WlanCounters.RTSSuccessCount.QuadPart;
+                pStatistics->RTSFailureCount.QuadPart = pAdapter->WlanCounters.RTSFailureCount.QuadPart;
+                pStatistics->ACKFailureCount.QuadPart = pAdapter->WlanCounters.ACKFailureCount.QuadPart;
+                pStatistics->FrameDuplicateCount.QuadPart = pAdapter->WlanCounters.FrameDuplicateCount.QuadPart;
+                pStatistics->ReceivedFragmentCount.QuadPart = pAdapter->WlanCounters.ReceivedFragmentCount.QuadPart;
+                pStatistics->MulticastReceivedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastReceivedFrameCount.QuadPart;
+#ifdef DBG
+                pStatistics->FCSErrorCount = pAdapter->RalinkCounters.RealFcsErrCount;
+#else
+                pStatistics->FCSErrorCount.QuadPart = pAdapter->WlanCounters.FCSErrorCount.QuadPart;
+                pStatistics->FrameDuplicateCount.u.LowPart = pAdapter->WlanCounters.FrameDuplicateCount.u.LowPart / 100;
+#endif
+                wrq->u.data.length = sizeof(NDIS_802_11_STATISTICS);
+                Status = copy_to_user(wrq->u.data.pointer, pStatistics, wrq->u.data.length);
+                kfree(pStatistics);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_GEN_RCV_OK:
+            ulInfo = pAdapter->Counters8023.GoodReceives;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case OID_GEN_RCV_NO_BUFFER:
+            ulInfo = pAdapter->Counters8023.RxNoBuffer;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            ulInfo = (ULONG)pAdapter->CommonCfg.PhyMode;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PHY_MODE (=%ld)\n", ulInfo));
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            pStaConfig = (RT_802_11_STA_CONFIG *) kmalloc(sizeof(RT_802_11_STA_CONFIG), MEM_ALLOC_FLAG);
+            if (pStaConfig)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG\n"));
+                pStaConfig->EnableTxBurst = pAdapter->CommonCfg.bEnableTxBurst;
+                pStaConfig->EnableTurboRate = 0;
+                pStaConfig->UseBGProtection = pAdapter->CommonCfg.UseBGProtection;
+                pStaConfig->UseShortSlotTime = pAdapter->CommonCfg.bUseShortSlotTime;
+                //pStaConfig->AdhocMode = pAdapter->StaCfg.AdhocMode;
+                pStaConfig->HwRadioStatus = (pAdapter->StaCfg.bHwRadio == TRUE) ? 1 : 0;
+                pStaConfig->Rsv1 = 0;
+                pStaConfig->SystemErrorBitmap = pAdapter->SystemErrorBitmap;
+                wrq->u.data.length = sizeof(RT_802_11_STA_CONFIG);
+                Status = copy_to_user(wrq->u.data.pointer, pStaConfig, wrq->u.data.length);
+                kfree(pStaConfig);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            RtsThresh = pAdapter->CommonCfg.RtsThreshold;
+            wrq->u.data.length = sizeof(RtsThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &RtsThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RTS_THRESHOLD(=%ld)\n", RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            FragThresh = pAdapter->CommonCfg.FragmentThreshold;
+            if (pAdapter->CommonCfg.bUseZeroToDisableFragment == TRUE)
+                FragThresh = 0;
+            wrq->u.data.length = sizeof(FragThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &FragThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_FRAGMENTATION_THRESHOLD(=%ld)\n", FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            PowerMode = pAdapter->StaCfg.WindowsPowerMode;
+            wrq->u.data.length = sizeof(PowerMode);
+            Status = copy_to_user(wrq->u.data.pointer, &PowerMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_POWER_MODE(=%d)\n", PowerMode));
+            break;
+        case RT_OID_802_11_RADIO:
+            RadioState = (BOOLEAN) pAdapter->StaCfg.bSwRadio;
+            wrq->u.data.length = sizeof(RadioState);
+            Status = copy_to_user(wrq->u.data.pointer, &RadioState, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_RADIO (=%d)\n", RadioState));
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                BssType = Ndis802_11IBSS;
+            else if (pAdapter->StaCfg.BssType == BSS_INFRA)
+                BssType = Ndis802_11Infrastructure;
+            else if (pAdapter->StaCfg.BssType == BSS_MONITOR)
+                BssType = Ndis802_11Monitor;
+            else
+                BssType = Ndis802_11AutoUnknown;
+
+            wrq->u.data.length = sizeof(BssType);
+            Status = copy_to_user(wrq->u.data.pointer, &BssType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_INFRASTRUCTURE_MODE(=%d)\n", BssType));
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            PreamType = pAdapter->CommonCfg.TxPreamble;
+            wrq->u.data.length = sizeof(PreamType);
+            Status = copy_to_user(wrq->u.data.pointer, &PreamType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PREAMBLE(=%d)\n", PreamType));
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            AuthMode = pAdapter->StaCfg.AuthMode;
+            wrq->u.data.length = sizeof(AuthMode);
+            Status = copy_to_user(wrq->u.data.pointer, &AuthMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_AUTHENTICATION_MODE(=%d)\n", AuthMode));
+            break;
+        case OID_802_11_WEP_STATUS:
+            WepStatus = pAdapter->StaCfg.WepStatus;
+            wrq->u.data.length = sizeof(WepStatus);
+            Status = copy_to_user(wrq->u.data.pointer, &WepStatus, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEP_STATUS(=%d)\n", WepStatus));
+            break;
+        case OID_802_11_TX_POWER_LEVEL:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPower, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_TX_POWER_LEVEL %x\n",pAdapter->CommonCfg.TxPower));
+			break;
+        case RT_OID_802_11_TX_POWER_LEVEL_1:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPowerPercentage, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			break;
+        case OID_802_11_NETWORK_TYPES_SUPPORTED:
+			if ((pAdapter->RfIcType	== RFIC_2850) || (pAdapter->RfIcType ==	RFIC_2750))
+			{
+				NetworkTypeList[0] = 3;                 // NumberOfItems = 3
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+				NetworkTypeList[3] = Ndis802_11OFDM5;   // NetworkType[3] = 11a
+                wrq->u.data.length = 16;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			else
+			{
+				NetworkTypeList[0] = 2;                 // NumberOfItems = 2
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+			    wrq->u.data.length = 12;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_NETWORK_TYPES_SUPPORTED\n"));
+				break;
+	    case OID_802_11_NETWORK_TYPE_IN_USE:
+            wrq->u.data.length = sizeof(ULONG);
+			if (pAdapter->CommonCfg.PhyMode == PHY_11A)
+				ulInfo = Ndis802_11OFDM5;
+			else if ((pAdapter->CommonCfg.PhyMode == PHY_11BG_MIXED) || (pAdapter->CommonCfg.PhyMode == PHY_11G))
+				ulInfo = Ndis802_11OFDM24;
+			else
+				ulInfo = Ndis802_11DS;
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+        case RT_OID_802_11_QUERY_LAST_RX_RATE:
+            ulInfo = (ULONG)pAdapter->LastRxRate;
+            wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_RX_RATE (=%ld)\n", ulInfo));
+			break;
+		case RT_OID_802_11_QUERY_LAST_TX_RATE:
+			//ulInfo = (ULONG)pAdapter->LastTxRate;
+			ulInfo = (ULONG)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_TX_RATE (=%lx)\n", ulInfo));
+			break;
+        case RT_OID_802_11_QUERY_EEPROM_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->EepromVersion, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_QUERY_FIRMWARE_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->FirmwareVersion, wrq->u.data.length);
+			break;
+	    case RT_OID_802_11_QUERY_NOISE_LEVEL:
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->BbpWriteLatch[66], wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_NOISE_LEVEL (=%d)\n", pAdapter->BbpWriteLatch[66]));
+			break;
+	    case RT_OID_802_11_EXTRA_INFO:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->ExtraInfo, wrq->u.data.length);
+	        DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_EXTRA_INFO (=%ld)\n", pAdapter->ExtraInfo));
+	        break;
+	    case RT_OID_WE_VERSION_COMPILED:
+	        wrq->u.data.length = sizeof(UINT);
+	        we_version_compiled = WIRELESS_EXT;
+	        Status = copy_to_user(wrq->u.data.pointer, &we_version_compiled, wrq->u.data.length);
+	        break;
+		case RT_OID_802_11_QUERY_APSD_SETTING:
+			apsd = (pAdapter->CommonCfg.bAPSDCapable | (pAdapter->CommonCfg.bAPSDAC_BE << 1) | (pAdapter->CommonCfg.bAPSDAC_BK << 2)
+				| (pAdapter->CommonCfg.bAPSDAC_VI << 3)	| (pAdapter->CommonCfg.bAPSDAC_VO << 4)	| (pAdapter->CommonCfg.MaxSPLength << 5));
+
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &apsd, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_SETTING (=0x%lx,APSDCap=%d,AC_BE=%d,AC_BK=%d,AC_VI=%d,AC_VO=%d,MAXSPLen=%d)\n",
+				apsd,pAdapter->CommonCfg.bAPSDCapable,pAdapter->CommonCfg.bAPSDAC_BE,pAdapter->CommonCfg.bAPSDAC_BK,pAdapter->CommonCfg.bAPSDAC_VI,pAdapter->CommonCfg.bAPSDAC_VO,pAdapter->CommonCfg.MaxSPLength));
+			break;
+		case RT_OID_802_11_QUERY_APSD_PSM:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_PSM (=%d)\n", pAdapter->CommonCfg.bAPSDForcePowerSave));
+			break;
+		case RT_OID_802_11_QUERY_WMM:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bWmmCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_WMM (=%d)\n",	pAdapter->CommonCfg.bWmmCapable));
+			break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+        case RT_OID_NEW_DRIVER:
+            {
+                UCHAR enabled = 1;
+    	        wrq->u.data.length = sizeof(UCHAR);
+    	        Status = copy_to_user(wrq->u.data.pointer, &enabled, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_NEW_DRIVER (=%d)\n", enabled));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+	        wrq->u.data.length = sizeof(UCHAR);
+	        Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.WpaSupplicantUP, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+        case RT_OID_DRIVER_DEVICE_NAME:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_DRIVER_DEVICE_NAME \n"));
+			wrq->u.data.length = 16;
+			if (copy_to_user(wrq->u.data.pointer, pAdapter->StaCfg.dev_name, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+            break;
+        case RT_OID_802_11_QUERY_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.STBC;
+
+    			pHTPhyMode->ExtOffset = ((pAdapter->CommonCfg.CentralChannel < pAdapter->CommonCfg.Channel) ? (EXTCHA_BELOW) : (EXTCHA_ABOVE));
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_COUNTRY_REGION:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_COUNTRY_REGION \n"));
+			wrq->u.data.length = sizeof(ulInfo);
+            ulInfo = pAdapter->CommonCfg.CountryRegionForABand;
+            ulInfo = (ulInfo << 8)|(pAdapter->CommonCfg.CountryRegion);
+			if (copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_QUERY_DAT_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.HTMODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->StaCfg.DesiredTransmitSetting.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.STBC;
+
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_QUERY_MULTIPLE_CARD_SUPPORT:
+			wrq->u.data.length = sizeof(UCHAR);
+            i = 0;
+#ifdef MULTIPLE_CARD_SUPPORT
+            i = 1;
+#endif // MULTIPLE_CARD_SUPPORT //
+			if (copy_to_user(wrq->u.data.pointer, &i, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_QUERY_MULTIPLE_CARD_SUPPORT(=%d) \n", i));
+            break;
+#ifdef SNMP_SUPPORT
+		case RT_OID_802_11_MAC_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREROUI:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREROUI \n"));
+			wrq->u.data.length = ManufacturerOUI_LEN;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTURERNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTURERNAME \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_RESOURCETYPEIDNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_RESOURCETYPEIDNAME \n"));
+			wrq->u.data.length = strlen(ResourceTypeIdName);
+			Status = copy_to_user(wrq->u.data.pointer, ResourceTypeIdName, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED \n"));
+			ulInfo = 1; // 1 is support wep else 2 is not support.
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_POWERMANAGEMENTMODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_POWERMANAGEMENTMODE \n"));
+			if (pAdapter->StaCfg.Psm == PSMP_ACTION)
+				ulInfo = 1; // 1 is power active else 2 is power save.
+			else
+				ulInfo = 2;
+
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEPDEFAULTKEYVALUE \n"));
+			//KeyIdxValue.KeyIdx = pAd->PortCfg.MBSSID[pAd->IoctlIF].DefaultKeyId;
+			pKeyIdxValue = wrq->u.data.pointer;
+			DBGPRINT(RT_DEBUG_TRACE,("KeyIdxValue.KeyIdx = %d, \n",pKeyIdxValue->KeyIdx));
+			valueLen = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+			NdisMoveMemory(pKeyIdxValue->Value,
+						   &pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key,
+						   valueLen);
+			pKeyIdxValue->Value[valueLen]='\0';
+
+			wrq->u.data.length = sizeof(DefaultKeyIdxValue);
+
+			Status = copy_to_user(wrq->u.data.pointer, pKeyIdxValue, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE,("DefaultKeyId = %d, total len = %d, str len=%d, KeyValue= %02x %02x %02x %02x \n", pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+			pAdapter->SharedKey[BSS0][0].Key[0],
+			pAdapter->SharedKey[BSS0][1].Key[0],
+			pAdapter->SharedKey[BSS0][2].Key[0],
+			pAdapter->SharedKey[BSS0][3].Key[0]));
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPDEFAULTKEYID \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("DefaultKeyId =%d \n", pAdapter->StaCfg.DefaultKeyId));
+			break;
+
+		case RT_OID_802_11_WEPKEYMAPPINGLENGTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPKEYMAPPINGLENGTH \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer,
+									&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+									wrq->u.data.length);
+			break;
+
+		case OID_802_11_SHORTRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SHORTRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			ShortRetryLimit = tx_rty_cfg.field.ShortRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("ShortRetryLimit =%ld,  tx_rty_cfg.field.ShortRetryLimit=%d\n", ShortRetryLimit, tx_rty_cfg.field.ShortRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &ShortRetryLimit, wrq->u.data.length);
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_LONGRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			LongRetryLimit = tx_rty_cfg.field.LongRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("LongRetryLimit =%ld,  tx_rty_cfg.field.LongRtyLimit=%d\n", LongRetryLimit, tx_rty_cfg.field.LongRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &LongRetryLimit, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRODUCTID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRODUCTID \n"));
+
+#ifdef RT2870
+			sprintf(tmp, "%04x %04x\n", ((POS_COOKIE)pAdapter->OS_Cookie)->pUsb_Dev->descriptor.idVendor ,((POS_COOKIE)pAdapter->OS_Cookie)->pUsb_Dev->descriptor.idProduct);
+
+#endif // RT2870 //
+			wrq->u.data.length = strlen(tmp);
+			Status = copy_to_user(wrq->u.data.pointer, tmp, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREID \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CURRENTCHANNEL \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			DBGPRINT(RT_DEBUG_TRACE, ("sizeof UCHAR=%d, channel=%d \n", sizeof(UCHAR), pAdapter->CommonCfg.Channel));
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Channel, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+#endif //SNMP_SUPPORT
+
+		case OID_802_11_BUILD_CHANNEL_EX:
+			{
+				UCHAR value;
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BUILD_CHANNEL_EX \n"));
+				wrq->u.data.length = sizeof(UCHAR);
+#ifdef EXT_BUILD_CHANNEL_LIST
+				DBGPRINT(RT_DEBUG_TRACE, ("Support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 1;
+#else
+				DBGPRINT(RT_DEBUG_TRACE, ("Doesn't support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 0;
+#endif // EXT_BUILD_CHANNEL_LIST //
+				Status = copy_to_user(wrq->u.data.pointer, &value, 1);
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			}
+			break;
+
+		case OID_802_11_GET_CH_LIST:
+			{
+				PRT_CHANNEL_LIST_INFO pChListBuf;
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CH_LIST \n"));
+				if (pAdapter->ChannelListNum == 0)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf = (RT_CHANNEL_LIST_INFO *) kmalloc(sizeof(RT_CHANNEL_LIST_INFO), MEM_ALLOC_FLAG);
+				if (pChListBuf == NULL)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf->ChannelListNum = pAdapter->ChannelListNum;
+				for (i = 0; i < pChListBuf->ChannelListNum; i++)
+					pChListBuf->ChannelList[i] = pAdapter->ChannelList[i].Channel;
+
+				wrq->u.data.length = sizeof(RT_CHANNEL_LIST_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pChListBuf, sizeof(RT_CHANNEL_LIST_INFO));
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+
+				if (pChListBuf)
+					kfree(pChListBuf);
+			}
+			break;
+
+		case OID_802_11_GET_COUNTRY_CODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_COUNTRY_CODE \n"));
+			wrq->u.data.length = 2;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.CountryCode, 2);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+		case OID_802_11_GET_CHANNEL_GEOGRAPHY:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CHANNEL_GEOGRAPHY \n"));
+			wrq->u.data.length = 1;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Geography, 1);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_QUERY_DLS:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bDLSCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS(=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			break;
+
+		case RT_OID_802_11_QUERY_DLS_PARAM:
+			{
+				PRT_802_11_DLS_INFO	pDlsInfo = kmalloc(sizeof(RT_802_11_DLS_INFO), GFP_ATOMIC);
+				if (pDlsInfo == NULL)
+					break;
+
+				for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+				{
+					RTMPMoveMemory(&pDlsInfo->Entry[i], &pAdapter->StaCfg.DLSEntry[i], sizeof(RT_802_11_DLS_UI));
+				}
+
+				pDlsInfo->num = MAX_NUM_OF_DLS_ENTRY;
+				wrq->u.data.length = sizeof(RT_802_11_DLS_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pDlsInfo, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS_PARAM\n"));
+
+				if (pDlsInfo)
+					kfree(pDlsInfo);
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+    return Status;
+}
+
+INT rt28xx_sta_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT					cmd)
+{
+	POS_COOKIE			pObj;
+	VIRTUAL_ADAPTER		*pVirtualAd = NULL;
+	RTMP_ADAPTER        *pAd = NULL;
+	struct iwreq        *wrq = (struct iwreq *) rq;
+	BOOLEAN				StateMachineTouched = FALSE;
+	INT					Status = NDIS_STATUS_SUCCESS;
+	USHORT				subcmd;
+
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		pAd = net_dev->priv;
+	}
+	else
+	{
+		pVirtualAd = net_dev->priv;
+		pAd = pVirtualAd->RtmpDev->priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	    if (wrq->u.data.pointer == NULL)
+	    {
+		    return Status;
+	    }
+
+	    if (strstr(wrq->u.data.pointer, "OpMode") == NULL)
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		    return -ENETDOWN;
+        }
+    }
+
+	{	// determine this ioctl command is comming from which interface.
+		pObj->ioctl_if_type = INT_MAIN;
+		pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	switch(cmd)
+	{
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+		case RTPRIV_IOCTL_ATE:
+			{
+				RtmpDoAte(pAd, wrq);
+			}
+			break;
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+        case SIOCGIFHWADDR:
+			DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIFHWADDR\n"));
+			memcpy(wrq->u.name, pAd->CurrentAddress, ETH_ALEN);
+			break;
+		case SIOCGIWNAME:
+        {
+        	char *name=&wrq->u.name[0];
+        	rt_ioctl_giwname(net_dev, NULL, name, NULL);
+			break;
+		}
+		case SIOCGIWESSID:  //Get ESSID
+        {
+        	struct iw_point *essid=&wrq->u.essid;
+        	rt_ioctl_giwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWESSID:  //Set ESSID
+        {
+        	struct iw_point	*essid=&wrq->u.essid;
+        	rt_ioctl_siwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWNWID:   // set network id (the cell)
+		case SIOCGIWNWID:   // get network id
+			Status = -EOPNOTSUPP;
+			break;
+		case SIOCSIWFREQ:   //set channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_siwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCGIWFREQ:   // get channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_giwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCSIWNICKN: //set node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_siwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWNICKN: //get node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_giwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWRATE:   //get default bit rate (bps)
+		    rt_ioctl_giwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+	    case SIOCSIWRATE:  //set default bit rate (bps)
+	        rt_ioctl_siwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+        case SIOCGIWRTS:  // get RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_giwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCSIWRTS:  //set RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_siwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCGIWFRAG:  //get fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_giwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCSIWFRAG:  //set fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_siwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCGIWENCODE:  //get encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_giwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+        case SIOCSIWENCODE:  //set encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_siwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+		case SIOCGIWAP:     //get access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_giwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+	    case SIOCSIWAP:  //set access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_siwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+		case SIOCGIWMODE:   //get operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_giwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCSIWMODE:   //set operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_siwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCGIWSENS:   //get sensitivity (dBm)
+		case SIOCSIWSENS:	//set sensitivity (dBm)
+		case SIOCGIWPOWER:  //get Power Management settings
+		case SIOCSIWPOWER:  //set Power Management settings
+		case SIOCGIWTXPOW:  //get transmit power (dBm)
+		case SIOCSIWTXPOW:  //set transmit power (dBm)
+		case SIOCGIWRANGE:	//Get range of parameters
+		case SIOCGIWRETRY:	//get retry limits and lifetime
+		case SIOCSIWRETRY:	//set retry limits and lifetime
+			Status = -EOPNOTSUPP;
+			break;
+		case RT_PRIV_IOCTL:
+			subcmd = wrq->u.data.flags;
+			if( subcmd & OID_GET_SET_TOGGLE)
+				Status = RTMPSetInformation(pAd, rq, subcmd);
+			else
+				Status = RTMPQueryInformation(pAd, rq, subcmd);
+			break;
+		case SIOCGIWPRIV:
+			if (wrq->u.data.pointer)
+			{
+				if ( access_ok(VERIFY_WRITE, wrq->u.data.pointer, sizeof(privtab)) != TRUE)
+					break;
+				wrq->u.data.length = sizeof(privtab) / sizeof(privtab[0]);
+				if (copy_to_user(wrq->u.data.pointer, privtab, sizeof(privtab)))
+					Status = -EFAULT;
+			}
+			break;
+		case RTPRIV_IOCTL_SET:
+			if(access_ok(VERIFY_READ, wrq->u.data.pointer, wrq->u.data.length) != TRUE)
+				break;
+			rt_ioctl_setparam(net_dev, NULL, NULL, wrq->u.data.pointer);
+			break;
+		case RTPRIV_IOCTL_GSITESURVEY:
+			RTMPIoctlGetSiteSurvey(pAd, wrq);
+		    break;
+#ifdef DBG
+		case RTPRIV_IOCTL_MAC:
+			RTMPIoctlMAC(pAd, wrq);
+			break;
+		case RTPRIV_IOCTL_E2P:
+			RTMPIoctlE2PROM(pAd, wrq);
+			break;
+#endif // DBG //
+        case SIOCETHTOOL:
+                break;
+		default:
+			DBGPRINT(RT_DEBUG_ERROR, ("IOCTL::unknown IOCTL's cmd = 0x%08x\n", cmd));
+			Status = -EOPNOTSUPP;
+			break;
+	}
+
+    if(StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAd);
+
+	return Status;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set SSID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    NDIS_802_11_SSID                    Ssid, *pSsid=NULL;
+    BOOLEAN                             StateMachineTouched = FALSE;
+    int                                 success = TRUE;
+
+    if( strlen(arg) <= MAX_LEN_OF_SSID)
+    {
+        NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+        if (strlen(arg) != 0)
+        {
+            NdisMoveMemory(Ssid.Ssid, arg, strlen(arg));
+            Ssid.SsidLength = strlen(arg);
+        }
+        else   //ANY ssid
+        {
+            Ssid.SsidLength = 0;
+		    memcpy(Ssid.Ssid, "", 0);
+			pAdapter->StaCfg.BssType = BSS_INFRA;
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+	        pAdapter->StaCfg.WepStatus  = Ndis802_11EncryptionDisabled;
+		}
+        pSsid = &Ssid;
+
+        if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+        {
+            RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+        }
+
+        pAdapter->MlmeAux.CurrReqIsFromNdis = TRUE;
+        pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+		pAdapter->bConfigChanged = TRUE;
+
+        MlmeEnqueue(pAdapter,
+                    MLME_CNTL_STATE_MACHINE,
+                    OID_802_11_SSID,
+                    sizeof(NDIS_802_11_SSID),
+                    (VOID *)pSsid);
+
+        StateMachineTouched = TRUE;
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_SSID_Proc::(Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+    }
+    else
+        success = FALSE;
+
+    if (StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAdapter);
+
+    return success;
+}
+
+#ifdef WMM_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WmmCapable Enable or Disable
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	BOOLEAN	bWmmCapable;
+
+	bWmmCapable = simple_strtol(arg, 0, 10);
+
+	if ((bWmmCapable == 1)
+#ifdef RT2870
+		&& (pAd->NumberOfPipes >= 5)
+#endif // RT2870 //
+		)
+		pAd->CommonCfg.bWmmCapable = TRUE;
+	else if (bWmmCapable == 0)
+		pAd->CommonCfg.bWmmCapable = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_WmmCapable_Proc::(bWmmCapable=%d)\n",
+		pAd->CommonCfg.bWmmCapable));
+
+	return TRUE;
+}
+#endif // WMM_SUPPORT //
+
+/*
+    ==========================================================================
+    Description:
+        Set Network Type(Infrastructure/Adhoc mode)
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UINT32	Value = 0;
+
+    if (strcmp(arg, "Adhoc") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_ADHOC)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (INFRA_ON(pAdapter))
+			{
+				//BOOLEAN Cancelled;
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event BB!\n"));
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_ADHOC;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(AD-HOC)\n"));
+	}
+    else if (strcmp(arg, "Infra") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_INFRA)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (ADHOC_ON(pAdapter))
+			{
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_INFRA;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(INFRA)\n"));
+
+        pAdapter->StaCfg.BssType = BSS_INFRA;
+	}
+    else if (strcmp(arg, "Monitor") == 0)
+    {
+		UCHAR	bbpValue = 0;
+		BCN_TIME_CFG_STRUC csr;
+		OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_INFRA_ON);
+        OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+		// disable all periodic state machine
+		pAdapter->StaCfg.bAutoReconnect = FALSE;
+		// reset all mlme state machine
+		RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+		DBGPRINT(RT_DEBUG_TRACE, ("fOP_STATUS_MEDIA_STATE_CONNECTED \n"));
+        if (pAdapter->CommonCfg.CentralChannel == 0)
+        {
+#ifdef DOT11_N_SUPPORT
+            if (pAdapter->CommonCfg.PhyMode == PHY_11AN_MIXED)
+                pAdapter->CommonCfg.CentralChannel = 36;
+            else
+#endif // DOT11_N_SUPPORT //
+                pAdapter->CommonCfg.CentralChannel = 6;
+        }
+#ifdef DOT11_N_SUPPORT
+        else
+            N_ChannelCheck(pAdapter);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_ABOVE)
+		{
+			// 40MHz ,control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			//  RX : control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue &= (~0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value &= 0xfffffffe;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel + 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_BELOW)
+		{
+			// 40MHz ,control channel at upper
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value |= 0x1;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue |= (0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel - 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			// 20MHz
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_20;
+			AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAdapter, pAdapter->CommonCfg.Channel);
+			DBGPRINT(RT_DEBUG_TRACE, ("BW_20, Channel(%d)\n", pAdapter->CommonCfg.Channel));
+		}
+		// Enable Rx with promiscuous reception
+		RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, 0x3);
+		// ASIC supporsts sniffer function with replacing RSSI with timestamp.
+		//RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+		//Value |= (0x80);
+		//RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+		// disable sync
+		RTMP_IO_READ32(pAdapter, BCN_TIME_CFG, &csr.word);
+		csr.field.bBeaconGen = 0;
+		csr.field.bTBTTEnable = 0;
+		csr.field.TsfSyncMode = 0;
+		RTMP_IO_WRITE32(pAdapter, BCN_TIME_CFG, csr.word);
+
+		pAdapter->StaCfg.BssType = BSS_MONITOR;
+        pAdapter->net_dev->type = ARPHRD_IEEE80211_PRISM; //ARPHRD_IEEE80211; // IEEE80211
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(MONITOR)\n"));
+    }
+
+    // Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_NetworkType_Proc::(NetworkType=%d)\n", pAdapter->StaCfg.BssType));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Authentication mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "WEPAUTO") == 0) || (strcmp(arg, "wepauto") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeAutoSwitch;
+    else if ((strcmp(arg, "OPEN") == 0) || (strcmp(arg, "open") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+    else if ((strcmp(arg, "SHARED") == 0) || (strcmp(arg, "shared") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    else if ((strcmp(arg, "WPAPSK") == 0) || (strcmp(arg, "wpapsk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+    else if ((strcmp(arg, "WPANONE") == 0) || (strcmp(arg, "wpanone") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+    else if ((strcmp(arg, "WPA2PSK") == 0) || (strcmp(arg, "wpa2psk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+#ifdef WPA_SUPPLICANT_SUPPORT
+    else if ((strcmp(arg, "WPA") == 0) || (strcmp(arg, "wpa") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+    else if ((strcmp(arg, "WPA2") == 0) || (strcmp(arg, "wpa2") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#endif // WPA_SUPPLICANT_SUPPORT //
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_AuthMode_Proc::(AuthMode=%d)\n", pAdapter->StaCfg.AuthMode));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Encryption Type
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "NONE") == 0) || (strcmp(arg, "none") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPDisabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPDisabled;
+    }
+    else if ((strcmp(arg, "WEP") == 0) || (strcmp(arg, "wep") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPEnabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPEnabled;
+    }
+    else if ((strcmp(arg, "TKIP") == 0) || (strcmp(arg, "tkip") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption2Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption2Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption2Enabled;
+    }
+    else if ((strcmp(arg, "AES") == 0) || (strcmp(arg, "aes") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption3Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption3Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption3Enabled;
+    }
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_EncrypType_Proc::(EncrypType=%d)\n", pAdapter->StaCfg.WepStatus));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Default Key ID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    ULONG                               KeyIdx;
+
+    KeyIdx = simple_strtol(arg, 0, 10);
+    if((KeyIdx >= 1 ) && (KeyIdx <= 4))
+        pAdapter->StaCfg.DefaultKeyId = (UCHAR) (KeyIdx - 1 );
+    else
+        return FALSE;  //Invalid argument
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_DefaultKeyID_Proc::(DefaultKeyID=%d)\n", pAdapter->StaCfg.DefaultKeyId));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY1
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+
+    pAdapter->SharedKey[BSS0][0].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              0,
+                              pAdapter->SharedKey[BSS0][0].CipherAlg,
+                              pAdapter->SharedKey[BSS0][0].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+
+    Description:
+        Set WEP KEY2
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][1].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              1,
+                              pAdapter->SharedKey[BSS0][1].CipherAlg,
+                              pAdapter->SharedKey[BSS0][1].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY3
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][2].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              2,
+                              pAdapter->SharedKey[BSS0][2].CipherAlg,
+                              pAdapter->SharedKey[BSS0][2].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY4
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][3].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              3,
+                              pAdapter->SharedKey[BSS0][3].CipherAlg,
+                              pAdapter->SharedKey[BSS0][3].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WPA PSK key
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UCHAR                   keyMaterial[40];
+
+    if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+        (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+	    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+		)
+        return TRUE;    // do nothing
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_WPAPSK_Proc::(WPAPSK=%s)\n", arg));
+
+    NdisZeroMemory(keyMaterial, 40);
+
+    if ((strlen(arg) < 8) || (strlen(arg) > 64))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("Set failed!!(WPAPSK=%s), WPAPSK key-string required 8 ~ 64 characters \n", arg));
+        return FALSE;
+    }
+
+    if (strlen(arg) == 64)
+    {
+        AtoH(arg, keyMaterial, 32);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+
+    }
+    else
+    {
+        PasswordHash((char *)arg, pAdapter->MlmeAux.Ssid, pAdapter->MlmeAux.SsidLen, keyMaterial);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+    }
+
+
+
+    if(pAdapter->StaCfg.BssType == BSS_ADHOC &&
+       pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+    {
+         pAdapter->StaCfg.WpaState = SS_NOTUSE;
+    }
+    else
+    {
+        // Start STA supplicant state machine
+        pAdapter->StaCfg.WpaState = SS_START;
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Power Saving mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (pAdapter->StaCfg.BssType == BSS_INFRA)
+    {
+        if ((strcmp(arg, "Max_PSP") == 0) ||
+			(strcmp(arg, "max_psp") == 0) ||
+			(strcmp(arg, "MAX_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeMAX_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeMAX_PSP;
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            pAdapter->StaCfg.DefaultListenCount = 5;
+
+        }
+        else if ((strcmp(arg, "Fast_PSP") == 0) ||
+				 (strcmp(arg, "fast_psp") == 0) ||
+                 (strcmp(arg, "FAST_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else if ((strcmp(arg, "Legacy_PSP") == 0) ||
+                 (strcmp(arg, "legacy_psp") == 0) ||
+                 (strcmp(arg, "LEGACY_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else
+        {
+            //Default Ndis802_11PowerModeCAM
+            // clear PSM bit immediately
+            MlmeSetPsmBit(pAdapter, PWR_ACTIVE);
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+        }
+
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_PSMode_Proc::(PSMode=%ld)\n", pAdapter->StaCfg.WindowsPowerMode));
+    }
+    else
+        return FALSE;
+
+
+    return TRUE;
+}
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WpaSupport flag.
+    Value:
+        0: Driver ignore wpa_supplicant.
+        1: wpa_supplicant initiates scanning and AP selection.
+        2: driver takes care of scanning, AP selection, and IEEE 802.11 association parameters.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+    if ( simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+    else if ( simple_strtol(arg, 0, 10) == 1)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE;
+    else if ( simple_strtol(arg, 0, 10) == 2)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE_WITH_WEB_UI;
+    else
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Wpa_Support::(WpaSupplicantUP=%d)\n", pAd->StaCfg.WpaSupplicantUP));
+
+    return TRUE;
+}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+/*
+    ==========================================================================
+    Description:
+        Read / Write MAC
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 mac 0        ==> read MAC where Addr=0x0
+               2.) iwpriv ra0 mac 0=12     ==> write MAC where Addr=0x0, value=12
+    ==========================================================================
+*/
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	ULONG				macAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	UINT32				macValue = 0;
+	INT					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+		//Parsing Read or Write
+	    this_char = arg;
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// Mac Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+				if (macAddr < 0xFFFF)
+				{
+					RTMP_IO_READ32(pAdapter, macAddr, &macValue);
+					DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%lx, MacValue=%x\n", macAddr, macValue));
+					sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr , macValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[8-k+j] = temp2[j];
+			}
+
+			while(k < 8)
+				temp2[7-k++]='0';
+			temp2[8]='\0';
+
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+
+				AtoH(temp2, temp, 4);
+				macValue = *temp*256*256*256 + temp[1]*256*256 + temp[2]*256 + temp[3];
+
+				// debug mode
+				if (macAddr == (HW_DEBUG_SETTING_BASE + 4))
+				{
+					// 0x2bf4: byte0 non-zero: enable R17 tuning, 0: disable R17 tuning
+                    if (macValue & 0x000000ff)
+                    {
+                        pAdapter->BbpTuning.bEnable = TRUE;
+                        DBGPRINT(RT_DEBUG_TRACE,("turn on R17 tuning\n"));
+                    }
+                    else
+                    {
+                        UCHAR R66;
+                        pAdapter->BbpTuning.bEnable = FALSE;
+                        R66 = 0x26 + GET_LNA_GAIN(pAdapter);
+#ifdef RALINK_ATE
+						if (ATE_ON(pAdapter))
+						{
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+						}
+						else
+#endif // RALINK_ATE //
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+                        DBGPRINT(RT_DEBUG_TRACE,("turn off R17 tuning, restore to 0x%02x\n", R66));
+                    }
+					return;
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%02lx, MacValue=0x%x\n", macAddr, macValue));
+
+				RTMP_IO_WRITE32(pAdapter, macAddr, macValue);
+				sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr, macValue);
+			}
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlMAC\n\n"));
+}
+
+/*
+    ==========================================================================
+    Description:
+        Read / Write E2PROM
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 e2p 0     	==> read E2PROM where Addr=0x0
+               2.) iwpriv ra0 e2p 0=1234    ==> write E2PROM where Addr=0x0, value=1234
+    ==========================================================================
+*/
+VOID RTMPIoctlE2PROM(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	USHORT				eepAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	USHORT				eepValue;
+	int					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+	    //Parsing Read or Write
+		this_char = arg;
+
+
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// E2PROM addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				eepAddr = *temp*256 + temp[1];
+				if (eepAddr < 0xFFFF)
+				{
+					RT28xx_EEPROM_READ16(pAdapter, eepAddr, eepValue);
+					sprintf(msg+strlen(msg), "[0x%04X]:0x%04X  ", eepAddr , eepValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[4-k+j] = temp2[j];
+			}
+
+			while(k < 4)
+				temp2[3-k++]='0';
+			temp2[4]='\0';
+
+			AtoH(this_char, temp, 2);
+			eepAddr = *temp*256 + temp[1];
+
+			AtoH(temp2, temp, 2);
+			eepValue = *temp*256 + temp[1];
+
+			RT28xx_EEPROM_WRITE16(pAdapter, eepAddr, eepValue);
+			sprintf(msg+strlen(msg), "[0x%02X]:%02X  ", eepAddr, eepValue);
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlE2PROM\n"));
+}
+#endif // DBG //
+
+
+
+
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.bTGnWifiTest = FALSE;
+    else
+        pAd->StaCfg.bTGnWifiTest = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_TGnWifiTest_Proc::(bTGnWifiTest=%d)\n", pAd->StaCfg.bTGnWifiTest));
+	return TRUE;
+}
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				LongRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_LongRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				ShortRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_ShortRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_None;
+    else if (simple_strtol(arg, 0, 10) == 1)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Flexible;
+    else if (simple_strtol(arg, 0, 10) == 2)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Strict;
+    else
+        return FALSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Ieee802dMode_Proc::(IEEEE0211dMode=%d)\n", pAdapter->StaCfg.IEEE80211dClientMode));
+    return TRUE;
+}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+    else
+        pAd->CommonCfg.CarrierDetect.Enable = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_CarrierDetect_Proc::(CarrierDetect.Enable=%d)\n", pAd->CommonCfg.CarrierDetect.Enable));
+	return TRUE;
+}
+#endif // CARRIER_DETECTION_SUPPORT //
+
+
+INT	Show_Adhoc_MacTable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCHAR			extra)
+{
+	INT i;
+
+	sprintf(extra, "\n");
+
+#ifdef DOT11_N_SUPPORT
+	sprintf(extra, "%sHT Operating Mode : %d\n", extra, pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode);
+#endif // DOT11_N_SUPPORT //
+
+	sprintf(extra, "%s\n%-19s%-4s%-4s%-7s%-7s%-7s%-10s%-6s%-6s%-6s%-6s\n", extra,
+			"MAC", "AID", "BSS", "RSSI0", "RSSI1", "RSSI2", "PhMd", "BW", "MCS", "SGI", "STBC");
+
+	for (i=1; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+
+		if (strlen(extra) > (IW_PRIV_SIZE_MASK - 30))
+		    break;
+		if ((pEntry->ValidAsCLI || pEntry->ValidAsApCli) && (pEntry->Sst == SST_ASSOC))
+		{
+			sprintf(extra, "%s%02X:%02X:%02X:%02X:%02X:%02X  ", extra,
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+			sprintf(extra, "%s%-4d", extra, (int)pEntry->Aid);
+			sprintf(extra, "%s%-4d", extra, (int)pEntry->apidx);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi0);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi1);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi2);
+			sprintf(extra, "%s%-10s", extra, GetPhyMode(pEntry->HTPhyMode.field.MODE));
+			sprintf(extra, "%s%-6s", extra, GetBW(pEntry->HTPhyMode.field.BW));
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.MCS);
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.ShortGI);
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.STBC);
+			sprintf(extra, "%s%-10d, %d, %d%%\n", extra, pEntry->DebugFIFOCount, pEntry->DebugTxCount,
+						(pEntry->DebugTxCount) ? ((pEntry->DebugTxCount-pEntry->DebugFIFOCount)*100/pEntry->DebugTxCount) : 0);
+			sprintf(extra, "%s\n", extra);
+		}
+	}
+
+	return TRUE;
+}
+
+
diff --git a/drivers/staging/rt2870/wpa.h b/drivers/staging/rt2870/wpa.h
new file mode 100644
index 000000000000..0134ae6097cf
--- /dev/null
+++ b/drivers/staging/rt2870/wpa.h
@@ -0,0 +1,357 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	wpa.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+
+#ifndef	__WPA_H__
+#define	__WPA_H__
+
+// EAPOL Key descripter frame format related length
+#define LEN_KEY_DESC_NONCE			32
+#define LEN_KEY_DESC_IV				16
+#define LEN_KEY_DESC_RSC			8
+#define LEN_KEY_DESC_ID				8
+#define LEN_KEY_DESC_REPLAY			8
+#define LEN_KEY_DESC_MIC			16
+
+// The length is the EAPoL-Key frame except key data field.
+// Please refer to 802.11i-2004 ,Figure 43u in p.78
+#define LEN_EAPOL_KEY_MSG			(sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE)
+
+// EAP Code Type.
+#define EAP_CODE_REQUEST	1
+#define EAP_CODE_RESPONSE	2
+#define EAP_CODE_SUCCESS    3
+#define EAP_CODE_FAILURE    4
+
+// EAPOL frame Protocol Version
+#define	EAPOL_VER					1
+#define	EAPOL_VER2					2
+
+// EAPOL-KEY Descriptor Type
+#define	WPA1_KEY_DESC				0xfe
+#define WPA2_KEY_DESC               0x02
+
+// Key Descriptor Version of Key Information
+#define	DESC_TYPE_TKIP				1
+#define	DESC_TYPE_AES				2
+#define DESC_TYPE_MESH				3
+
+#define LEN_MSG1_2WAY               0x7f
+#define MAX_LEN_OF_EAP_HS           256
+
+#define LEN_MASTER_KEY				32
+
+// EAPOL EK, MK
+#define LEN_EAP_EK					16
+#define LEN_EAP_MICK				16
+#define LEN_EAP_KEY					((LEN_EAP_EK)+(LEN_EAP_MICK))
+// TKIP key related
+#define LEN_PMKID					16
+#define LEN_TKIP_EK					16
+#define LEN_TKIP_RXMICK				8
+#define LEN_TKIP_TXMICK				8
+#define LEN_AES_EK					16
+#define LEN_AES_KEY					LEN_AES_EK
+#define LEN_TKIP_KEY				((LEN_TKIP_EK)+(LEN_TKIP_RXMICK)+(LEN_TKIP_TXMICK))
+#define TKIP_AP_TXMICK_OFFSET		((LEN_EAP_KEY)+(LEN_TKIP_EK))
+#define TKIP_AP_RXMICK_OFFSET		(TKIP_AP_TXMICK_OFFSET+LEN_TKIP_TXMICK)
+#define TKIP_GTK_LENGTH				((LEN_TKIP_EK)+(LEN_TKIP_RXMICK)+(LEN_TKIP_TXMICK))
+#define LEN_PTK						((LEN_EAP_KEY)+(LEN_TKIP_KEY))
+#define MIN_LEN_OF_GTK				5
+
+// RSN IE Length definition
+#define MAX_LEN_OF_RSNIE         	90
+#define MIN_LEN_OF_RSNIE         	8
+
+//EAP Packet Type
+#define	EAPPacket		0
+#define	EAPOLStart		1
+#define	EAPOLLogoff		2
+#define	EAPOLKey		3
+#define	EAPOLASFAlert	4
+#define	EAPTtypeMax		5
+
+#define	EAPOL_MSG_INVALID	0
+#define	EAPOL_PAIR_MSG_1	1
+#define	EAPOL_PAIR_MSG_2	2
+#define	EAPOL_PAIR_MSG_3	3
+#define	EAPOL_PAIR_MSG_4	4
+#define	EAPOL_GROUP_MSG_1	5
+#define	EAPOL_GROUP_MSG_2	6
+
+#define PAIRWISEKEY					1
+#define GROUPKEY					0
+
+// Retry timer counter initial value
+#define PEER_MSG1_RETRY_TIMER_CTR           0
+#define PEER_MSG3_RETRY_TIMER_CTR           10
+#define GROUP_MSG1_RETRY_TIMER_CTR          20
+
+
+#define EAPOL_START_DISABLE					0
+#define EAPOL_START_PSK						1
+#define EAPOL_START_1X						2
+
+#define MIX_CIPHER_WPA_TKIP_ON(x)       (((x) & 0x08) != 0)
+#define MIX_CIPHER_WPA_AES_ON(x)        (((x) & 0x04) != 0)
+#define MIX_CIPHER_WPA2_TKIP_ON(x)      (((x) & 0x02) != 0)
+#define MIX_CIPHER_WPA2_AES_ON(x)       (((x) & 0x01) != 0)
+
+#define ROUND_UP(__x, __y) \
+	(((ULONG)((__x)+((__y)-1))) & ((ULONG)~((__y)-1)))
+
+#define	ADD_ONE_To_64BIT_VAR(_V)		\
+{										\
+	UCHAR	cnt = LEN_KEY_DESC_REPLAY;	\
+	do									\
+	{									\
+		cnt--;							\
+		_V[cnt]++;						\
+		if (cnt == 0)					\
+			break;						\
+	}while (_V[cnt] == 0);				\
+}
+
+#define IS_WPA_CAPABILITY(a)       (((a) >= Ndis802_11AuthModeWPA) && ((a) <= Ndis802_11AuthModeWPA1PSKWPA2PSK))
+
+// EAPOL Key Information definition within Key descriptor format
+typedef	struct PACKED _KEY_INFO
+{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	KeyAck:1;
+    UCHAR	Install:1;
+    UCHAR	KeyIndex:2;
+    UCHAR	KeyType:1;
+    UCHAR	KeyDescVer:3;
+    UCHAR	Rsvd:3;
+    UCHAR	EKD_DL:1;		// EKD for AP; DL for STA
+    UCHAR	Request:1;
+    UCHAR	Error:1;
+    UCHAR	Secure:1;
+    UCHAR	KeyMic:1;
+#else
+	UCHAR	KeyMic:1;
+	UCHAR	Secure:1;
+	UCHAR	Error:1;
+	UCHAR	Request:1;
+	UCHAR	EKD_DL:1;       // EKD for AP; DL for STA
+	UCHAR	Rsvd:3;
+	UCHAR	KeyDescVer:3;
+	UCHAR	KeyType:1;
+	UCHAR	KeyIndex:2;
+	UCHAR	Install:1;
+	UCHAR	KeyAck:1;
+#endif
+}	KEY_INFO, *PKEY_INFO;
+
+// EAPOL Key descriptor format
+typedef	struct PACKED _KEY_DESCRIPTER
+{
+	UCHAR		Type;
+	KEY_INFO	KeyInfo;
+	UCHAR		KeyLength[2];
+	UCHAR		ReplayCounter[LEN_KEY_DESC_REPLAY];
+	UCHAR		KeyNonce[LEN_KEY_DESC_NONCE];
+	UCHAR		KeyIv[LEN_KEY_DESC_IV];
+	UCHAR		KeyRsc[LEN_KEY_DESC_RSC];
+	UCHAR		KeyId[LEN_KEY_DESC_ID];
+	UCHAR		KeyMic[LEN_KEY_DESC_MIC];
+	UCHAR		KeyDataLen[2];
+	UCHAR		KeyData[MAX_LEN_OF_RSNIE];
+}	KEY_DESCRIPTER, *PKEY_DESCRIPTER;
+
+typedef	struct PACKED _EAPOL_PACKET
+{
+	UCHAR	 			ProVer;
+	UCHAR	 			ProType;
+	UCHAR	 			Body_Len[2];
+	KEY_DESCRIPTER		KeyDesc;
+}	EAPOL_PACKET, *PEAPOL_PACKET;
+
+//802.11i D10 page 83
+typedef struct PACKED _GTK_ENCAP
+{
+#ifndef RT_BIG_ENDIAN
+    UCHAR               Kid:2;
+    UCHAR               tx:1;
+    UCHAR               rsv:5;
+    UCHAR               rsv1;
+#else
+    UCHAR               rsv:5;
+    UCHAR               tx:1;
+    UCHAR               Kid:2;
+    UCHAR               rsv1;
+#endif
+    UCHAR               GTK[TKIP_GTK_LENGTH];
+}   GTK_ENCAP, *PGTK_ENCAP;
+
+typedef struct PACKED _KDE_ENCAP
+{
+    UCHAR               Type;
+    UCHAR               Len;
+    UCHAR               OUI[3];
+    UCHAR               DataType;
+    GTK_ENCAP      GTKEncap;
+}   KDE_ENCAP, *PKDE_ENCAP;
+
+// For WPA1
+typedef struct PACKED _RSNIE {
+    UCHAR   oui[4];
+    USHORT  version;
+    UCHAR   mcast[4];
+    USHORT  ucount;
+    struct PACKED {
+        UCHAR oui[4];
+    }ucast[1];
+} RSNIE, *PRSNIE;
+
+// For WPA2
+typedef struct PACKED _RSNIE2 {
+    USHORT  version;
+    UCHAR   mcast[4];
+    USHORT  ucount;
+    struct PACKED {
+        UCHAR oui[4];
+    }ucast[1];
+} RSNIE2, *PRSNIE2;
+
+// AKM Suite
+typedef struct PACKED _RSNIE_AUTH {
+    USHORT acount;
+    struct PACKED {
+        UCHAR oui[4];
+    }auth[1];
+} RSNIE_AUTH,*PRSNIE_AUTH;
+
+typedef	union PACKED _RSN_CAPABILITIES	{
+	struct	PACKED {
+#ifdef RT_BIG_ENDIAN
+        USHORT		Rsvd:10;
+        USHORT		GTKSA_R_Counter:2;
+        USHORT		PTKSA_R_Counter:2;
+        USHORT		No_Pairwise:1;
+		USHORT		PreAuth:1;
+#else
+        USHORT		PreAuth:1;
+		USHORT		No_Pairwise:1;
+		USHORT		PTKSA_R_Counter:2;
+		USHORT		GTKSA_R_Counter:2;
+		USHORT		Rsvd:10;
+#endif
+	}	field;
+	USHORT			word;
+}	RSN_CAPABILITIES, *PRSN_CAPABILITIES;
+
+typedef struct PACKED _EAP_HDR {
+    UCHAR   ProVer;
+    UCHAR   ProType;
+    UCHAR   Body_Len[2];
+    UCHAR   code;
+    UCHAR   identifier;
+    UCHAR   length[2]; // including code and identifier, followed by length-2 octets of data
+} EAP_HDR, *PEAP_HDR;
+
+// For supplicant state machine states. 802.11i Draft 4.1, p. 97
+// We simplified it
+typedef	enum	_WpaState
+{
+	SS_NOTUSE,				// 0
+	SS_START,				// 1
+	SS_WAIT_MSG_3,			// 2
+	SS_WAIT_GROUP,			// 3
+	SS_FINISH,  			// 4
+	SS_KEYUPDATE,			// 5
+}	WPA_STATE;
+
+//
+//	The definition of the cipher combination
+//
+// 	 bit3	bit2  bit1   bit0
+//	+------------+------------+
+// 	|	  WPA	 |	   WPA2   |
+//	+------+-----+------+-----+
+//	| TKIP | AES | TKIP | AES |
+//	|	0  |  1  |   1  |  0  | -> 0x06
+//	|	0  |  1  |   1  |  1  | -> 0x07
+//	|	1  |  0  |   0  |  1  | -> 0x09
+//	|	1  |  0  |   1  |  1  | -> 0x0B
+//	|	1  |  1  |   0  |  1  | -> 0x0D
+//	|	1  |  1  |   1  |  0  | -> 0x0E
+//	|	1  |  1  |   1  |  1  |	-> 0x0F
+//	+------+-----+------+-----+
+//
+typedef	enum	_WpaMixPairCipher
+{
+	MIX_CIPHER_NOTUSE 			= 0x00,
+	WPA_NONE_WPA2_TKIPAES		= 0x03,		// WPA2-TKIPAES
+	WPA_AES_WPA2_TKIP 			= 0x06,
+	WPA_AES_WPA2_TKIPAES		= 0x07,
+	WPA_TKIP_WPA2_AES			= 0x09,
+	WPA_TKIP_WPA2_TKIPAES		= 0x0B,
+	WPA_TKIPAES_WPA2_NONE		= 0x0C,		// WPA-TKIPAES
+	WPA_TKIPAES_WPA2_AES		= 0x0D,
+	WPA_TKIPAES_WPA2_TKIP		= 0x0E,
+	WPA_TKIPAES_WPA2_TKIPAES	= 0x0F,
+}	WPA_MIX_PAIR_CIPHER;
+
+typedef struct PACKED _RSN_IE_HEADER_STRUCT	{
+	UCHAR		Eid;
+	UCHAR		Length;
+	USHORT		Version;	// Little endian format
+}	RSN_IE_HEADER_STRUCT, *PRSN_IE_HEADER_STRUCT;
+
+// Cipher suite selector types
+typedef struct PACKED _CIPHER_SUITE_STRUCT	{
+	UCHAR		Oui[3];
+	UCHAR		Type;
+}	CIPHER_SUITE_STRUCT, *PCIPHER_SUITE_STRUCT;
+
+// Authentication and Key Management suite selector
+typedef struct PACKED _AKM_SUITE_STRUCT	{
+	UCHAR		Oui[3];
+	UCHAR		Type;
+}	AKM_SUITE_STRUCT, *PAKM_SUITE_STRUCT;
+
+// RSN capability
+typedef struct	PACKED _RSN_CAPABILITY	{
+	USHORT		Rsv:10;
+	USHORT		GTKSAReplayCnt:2;
+	USHORT		PTKSAReplayCnt:2;
+	USHORT		NoPairwise:1;
+	USHORT		PreAuth:1;
+}	RSN_CAPABILITY, *PRSN_CAPABILITY;
+
+#endif
diff --git a/drivers/staging/rtl8187se/Kconfig b/drivers/staging/rtl8187se/Kconfig
new file mode 100644
index 000000000000..79c225acd1ad
--- /dev/null
+++ b/drivers/staging/rtl8187se/Kconfig
@@ -0,0 +1,5 @@
+config RTL8187SE
+	tristate "RealTek RTL8187SE Wireless LAN NIC driver"
+	depends on PCI
+	default N
+	---help---
diff --git a/drivers/staging/rtl8187se/Makefile b/drivers/staging/rtl8187se/Makefile
new file mode 100644
index 000000000000..6bc7e29771c3
--- /dev/null
+++ b/drivers/staging/rtl8187se/Makefile
@@ -0,0 +1,55 @@
+
+#EXTRA_CFLAGS += -DCONFIG_IEEE80211_NOWEP=y
+#EXTRA_CFLAGS += -DCONFIG_RTL8180_IOMAP
+#EXTRA_CFLAGS += -std=gnu89
+#EXTRA_CFLAGS += -O2
+#CC            = gcc
+EXTRA_CFLAGS += -DTHOMAS_TURBO
+#CFLAGS += -DCONFIG_RTL8185B
+#CFLAGS += -DCONFIG_RTL818x_S
+
+#added for EeePC testing
+EXTRA_CFLAGS += -DENABLE_IPS
+EXTRA_CFLAGS += -DSW_ANTE
+EXTRA_CFLAGS += -DTX_TRACK
+EXTRA_CFLAGS += -DHIGH_POWER
+EXTRA_CFLAGS += -DSW_DIG
+EXTRA_CFLAGS += -DRATE_ADAPT
+EXTRA_CFLAGS += -DCONFIG_RTL8180_PM
+
+#+YJ,080626
+EXTRA_CFLAGS += -DENABLE_DOT11D
+
+#enable it for legacy power save, disable it for leisure  power save
+EXTRA_CFLAGS += -DENABLE_LPS
+
+
+#EXTRA_CFLAGS += -mhard-float -DCONFIG_FORCE_HARD_FLOAT=y
+
+rtl8187se-objs :=			\
+		r8180_core.o		\
+		r8180_sa2400.o		\
+		r8180_93cx6.o		\
+		r8180_wx.o		\
+		r8180_max2820.o		\
+		r8180_gct.o		\
+		r8180_rtl8225.o		\
+		r8180_rtl8255.o		\
+		r8180_rtl8225z2.o	\
+		r8185b_init.o		\
+		r8180_dm.o		\
+		r8180_pm.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_RTL8187SE)	+= rtl8187se.o
+
diff --git a/drivers/staging/rtl8187se/dot11d.h b/drivers/staging/rtl8187se/dot11d.h
new file mode 100644
index 000000000000..49f53fe52af4
--- /dev/null
+++ b/drivers/staging/rtl8187se/dot11d.h
@@ -0,0 +1,101 @@
+#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_DOT11D_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.h b/drivers/staging/rtl8187se/ieee80211.h
new file mode 100644
index 000000000000..58336080ad50
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211.h
@@ -0,0 +1,1755 @@
+/*
+ * 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
+ * <andreamrl@tiscali.it>
+ *
+ * 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/version.h>
+#include <linux/jiffies.h>
+#include <linux/timer.h>
+#include <linux/sched.h>
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,13))
+#include <linux/wireless.h>
+#endif
+
+/*
+#ifndef bool
+#define bool int
+#endif
+
+#ifndef true
+#define true   1
+#endif
+
+#ifndef false
+#define false  0
+#endif
+*/
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20))
+#ifndef bool
+typedef enum{false = 0, true} bool;
+#endif
+#endif
+//#ifdef JOHN_HWSEC
+#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
+//#endif
+
+
+#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
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,6,10))
+#define ieee80211_wx_get_scan ieee80211_wx_get_scan_rtl
+#define ieee80211_wx_set_encode ieee80211_wx_set_encode_rtl
+#define ieee80211_wx_get_encode ieee80211_wx_get_encode_rtl
+////////////////////////////////
+// added for kernel conflict under FC5
+#define ieee80211_wx_get_name   ieee80211_wx_get_name_rtl
+#define free_ieee80211          free_ieee80211_rtl
+#define alloc_ieee80211        alloc_ieee80211_rtl
+///////////////////////////////
+#endif
+//error in ubuntu2.6.22,so add these
+#define ieee80211_wake_queue ieee80211_wake_queue_rtl
+#define ieee80211_stop_queue ieee80211_stop_queue_rtl
+
+#define ieee80211_rx ieee80211_rx_rtl
+
+#define ieee80211_register_crypto_ops	ieee80211_register_crypto_ops_rtl
+#define ieee80211_unregister_crypto_ops	ieee80211_unregister_crypto_ops_rtl
+#define ieee80211_get_crypto_ops	ieee80211_get_crypto_ops_rtl
+#define ieee80211_crypt_deinit_entries	ieee80211_crypt_deinit_entries_rtl
+#define ieee80211_crypt_deinit_handler	ieee80211_crypt_deinit_handler_rtl
+#define ieee80211_crypt_delayed_deinit	ieee80211_crypt_delayed_deinit_rtl
+
+#define ieee80211_txb_free	ieee80211_txb_free_rtl
+#define ieee80211_wx_get_essid	ieee80211_wx_get_essid_rtl
+#define ieee80211_wx_set_essid	ieee80211_wx_set_essid_rtl
+#define ieee80211_wx_set_rate	ieee80211_wx_set_rate_rtl
+#define ieee80211_wx_get_rate	ieee80211_wx_get_rate_rtl
+#define ieee80211_wx_set_wap	ieee80211_wx_set_wap_rtl
+#define ieee80211_wx_get_wap	ieee80211_wx_get_wap_rtl
+#define ieee80211_wx_set_mode	ieee80211_wx_set_mode_rtl
+#define ieee80211_wx_get_mode	ieee80211_wx_get_mode_rtl
+#define ieee80211_wx_set_scan	ieee80211_wx_set_scan_rtl
+#define ieee80211_wx_get_freq	ieee80211_wx_get_freq_rtl
+#define ieee80211_wx_set_freq	ieee80211_wx_set_freq_rtl
+#define ieee80211_wx_set_rawtx	ieee80211_wx_set_rawtx_rtl
+#define ieee80211_wx_set_power	ieee80211_wx_set_power_rtl
+#define ieee80211_wx_get_power	ieee80211_wx_get_power_rtl
+#define ieee80211_wlan_frequencies	ieee80211_wlan_frequencies_rtl
+#define ieee80211_softmac_stop_protocol	ieee80211_softmac_stop_protocol_rtl
+#define ieee80211_softmac_start_protocol ieee80211_softmac_start_protocol_rtl
+#define	ieee80211_start_protocol	ieee80211_start_protocol_rtl
+#define	ieee80211_stop_protocol		ieee80211_stop_protocol_rtl
+#define	ieee80211_rx_mgt		ieee80211_rx_mgt_rtl
+
+#define ieee80211_wx_set_auth ieee80211_wx_set_auth_rtl
+//by amy for ps
+#define notify_wx_assoc_event  notify_wx_assoc_event_rtl
+#define ieee80211_stop_send_beacons ieee80211_stop_send_beacons_rtl
+#define ieee80211_disassociate ieee80211_disassociate_rtl
+#define ieee80211_start_scan ieee80211_start_scan_rtl
+//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;
+
+
+#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
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,9))
+#define MSECS(t)	(1000 * ((t) / HZ) + 1000 * ((t) % HZ) / HZ)
+static inline unsigned long msleep_interruptible_rtl(unsigned int msecs)
+{
+         unsigned long timeout = MSECS(msecs) + 1;
+
+         while (timeout) {
+                 set_current_state(TASK_UNINTERRUPTIBLE);
+                 timeout = schedule_timeout(timeout);
+         }
+         return timeout;
+}
+#else
+#define MSECS(t) msecs_to_jiffies(t)
+#define msleep_interruptible_rtl  msleep_interruptible
+#endif
+
+#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_HLEN			30
+#define IEEE80211_FRAME_LEN		(IEEE80211_DATA_LEN + IEEE80211_HLEN)
+
+/* 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;
+};
+
+struct ieee80211_hdr {
+	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];
+} __attribute__ ((packed));
+
+struct ieee80211_hdr_QOS {
+	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_hdr_3addr {
+	u16 frame_ctl;
+	u16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	u16 seq_ctl;
+} __attribute__ ((packed));
+
+struct ieee80211_hdr_3addr_QOS {
+	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));
+
+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));
+
+#define IEEE80211_3ADDR_LEN 24
+#define IEEE80211_4ADDR_LEN 30
+#define IEEE80211_FCS_LEN    4
+
+#define MIN_FRAG_THRESHOLD     256U
+#define	MAX_FRAG_THRESHOLD     2346U
+
+/* Frame control field constants */
+#define IEEE80211_FCTL_VERS		0x0002
+#define IEEE80211_FCTL_FTYPE		0x000c
+#define IEEE80211_FCTL_STYPE		0x00f0
+#define IEEE80211_FCTL_TODS		0x0100
+#define IEEE80211_FCTL_FROMDS		0x0200
+#define IEEE80211_FCTL_DSTODS		0x0300 //added by david
+#define IEEE80211_FCTL_MOREFRAGS	0x0400
+#define IEEE80211_FCTL_RETRY		0x0800
+#define IEEE80211_FCTL_PM		0x1000
+#define IEEE80211_FCTL_MOREDATA	0x2000
+#define IEEE80211_FCTL_WEP		0x4000
+#define IEEE80211_FCTL_ORDER		0x8000
+
+#define IEEE80211_FTYPE_MGMT		0x0000
+#define IEEE80211_FTYPE_CTL		0x0004
+#define IEEE80211_FTYPE_DATA		0x0008
+
+/* management */
+#define IEEE80211_STYPE_ASSOC_REQ	0x0000
+#define IEEE80211_STYPE_ASSOC_RESP 	0x0010
+#define IEEE80211_STYPE_REASSOC_REQ	0x0020
+#define IEEE80211_STYPE_REASSOC_RESP	0x0030
+#define IEEE80211_STYPE_PROBE_REQ	0x0040
+#define IEEE80211_STYPE_PROBE_RESP	0x0050
+#define IEEE80211_STYPE_BEACON		0x0080
+#define IEEE80211_STYPE_ATIM		0x0090
+#define IEEE80211_STYPE_DISASSOC	0x00A0
+#define IEEE80211_STYPE_AUTH		0x00B0
+#define IEEE80211_STYPE_DEAUTH		0x00C0
+#define IEEE80211_STYPE_MANAGE_ACT	0x00D0
+
+/* control */
+#define IEEE80211_STYPE_PSPOLL		0x00A0
+#define IEEE80211_STYPE_RTS		0x00B0
+#define IEEE80211_STYPE_CTS		0x00C0
+#define IEEE80211_STYPE_ACK		0x00D0
+#define IEEE80211_STYPE_CFEND		0x00E0
+#define IEEE80211_STYPE_CFENDACK	0x00F0
+
+/* data */
+#define IEEE80211_STYPE_DATA		0x0000
+#define IEEE80211_STYPE_DATA_CFACK	0x0010
+#define IEEE80211_STYPE_DATA_CFPOLL	0x0020
+#define IEEE80211_STYPE_DATA_CFACKPOLL	0x0030
+#define IEEE80211_STYPE_NULLFUNC	0x0040
+#define IEEE80211_STYPE_CFACK		0x0050
+#define IEEE80211_STYPE_CFPOLL		0x0060
+#define IEEE80211_STYPE_CFACKPOLL	0x0070
+#define IEEE80211_STYPE_QOS_DATA	0x0080 //added for WMM 2006/8/2
+#define IEEE80211_STYPE_QOS_NULL	0x00C0
+
+
+#define IEEE80211_SCTL_FRAG		0x000F
+#define IEEE80211_SCTL_SEQ		0xFFF0
+
+
+/* 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/wireless.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)
+
+/* Authentication algorithms */
+#define WLAN_AUTH_OPEN 0
+#define WLAN_AUTH_SHARED_KEY 1
+
+#define WLAN_AUTH_CHALLENGE_LEN 128
+
+#define WLAN_CAPABILITY_BSS (1<<0)
+#define WLAN_CAPABILITY_IBSS (1<<1)
+#define WLAN_CAPABILITY_CF_POLLABLE (1<<2)
+#define WLAN_CAPABILITY_CF_POLL_REQUEST (1<<3)
+#define WLAN_CAPABILITY_PRIVACY (1<<4)
+#define WLAN_CAPABILITY_SHORT_PREAMBLE (1<<5)
+#define WLAN_CAPABILITY_PBCC (1<<6)
+#define WLAN_CAPABILITY_CHANNEL_AGILITY (1<<7)
+#define WLAN_CAPABILITY_SHORT_SLOT (1<<10)
+
+/* Status codes */
+#define WLAN_STATUS_SUCCESS 0
+#define WLAN_STATUS_UNSPECIFIED_FAILURE 1
+#define WLAN_STATUS_CAPS_UNSUPPORTED 10
+#define WLAN_STATUS_REASSOC_NO_ASSOC 11
+#define WLAN_STATUS_ASSOC_DENIED_UNSPEC 12
+#define WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG 13
+#define WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION 14
+#define WLAN_STATUS_CHALLENGE_FAIL 15
+#define WLAN_STATUS_AUTH_TIMEOUT 16
+#define WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA 17
+#define WLAN_STATUS_ASSOC_DENIED_RATES 18
+/* 802.11b */
+#define WLAN_STATUS_ASSOC_DENIED_NOSHORT 19
+#define WLAN_STATUS_ASSOC_DENIED_NOPBCC 20
+#define WLAN_STATUS_ASSOC_DENIED_NOAGILITY 21
+
+/* Reason codes */
+#define WLAN_REASON_UNSPECIFIED 1
+#define WLAN_REASON_PREV_AUTH_NOT_VALID 2
+#define WLAN_REASON_DEAUTH_LEAVING 3
+#define WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY 4
+#define WLAN_REASON_DISASSOC_AP_BUSY 5
+#define WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA 6
+#define WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA 7
+#define WLAN_REASON_DISASSOC_STA_HAS_LEFT 8
+#define WLAN_REASON_STA_REQ_ASSOC_WITHOUT_AUTH 9
+
+
+/* Information Element IDs */
+#define WLAN_EID_SSID 0
+#define WLAN_EID_SUPP_RATES 1
+#define WLAN_EID_FH_PARAMS 2
+#define WLAN_EID_DS_PARAMS 3
+#define WLAN_EID_CF_PARAMS 4
+#define WLAN_EID_TIM 5
+#define WLAN_EID_IBSS_PARAMS 6
+#define WLAN_EID_CHALLENGE 16
+#define WLAN_EID_RSN 48
+#define WLAN_EID_GENERIC 221
+
+#define IEEE80211_MGMT_HDR_LEN 24
+#define IEEE80211_DATA_HDR3_LEN 24
+#define IEEE80211_DATA_HDR4_LEN 30
+
+
+#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
+
+
+
+
+/* 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_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;
+};
+
+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
+
+*/
+
+struct ieee80211_header_data {
+	u16 frame_ctl;
+	u16 duration_id;
+	u8 addr1[6];
+	u8 addr2[6];
+	u8 addr3[6];
+	u16 seq_ctrl;
+};
+
+#define BEACON_PROBE_SSID_ID_POSITION 12
+
+/* Management Frame Information Element Types */
+#define MFIE_TYPE_SSID       0
+#define MFIE_TYPE_RATES      1
+#define MFIE_TYPE_FH_SET     2
+#define MFIE_TYPE_DS_SET     3
+#define MFIE_TYPE_CF_SET     4
+#define MFIE_TYPE_TIM        5
+#define MFIE_TYPE_IBSS_SET   6
+#define MFIE_TYPE_COUNTRY  7 //+YJ,080625
+#define MFIE_TYPE_CHALLENGE  16
+#define MFIE_TYPE_ERP        42
+#define MFIE_TYPE_RSN	     48
+#define MFIE_TYPE_RATES_EX   50
+#define MFIE_TYPE_GENERIC    221
+
+#ifdef ENABLE_DOT11D
+typedef 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
+}country_code_type_t;
+#endif
+
+struct ieee80211_info_element_hdr {
+	u8 id;
+	u8 len;
+} __attribute__ ((packed));
+
+struct ieee80211_info_element {
+	u8 id;
+	u8 len;
+	u8 data[0];
+} __attribute__ ((packed));
+
+/*
+ * 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
+
+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_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_probe_request {
+	struct ieee80211_header_data header;
+	/*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_disassoc_frame{
+        struct ieee80211_hdr_3addr header;
+        u16    reasoncode;
+}__attribute__ ((packed));
+
+struct ieee80211_txb {
+	u8 nr_frags;
+	u8 encrypted;
+	u16 reserved;
+	u16 frag_size;
+	u16 payload_size;
+	struct sk_buff *fragments[0];
+};
+
+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 {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)
+
+/* 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                 161
+#define MAX_CHANNEL_NUMBER                 165 //YJ,modified,080625
+#define MAX_IE_LEN						0xFF //+YJ,080625
+
+typedef struct _CHANNEL_LIST{
+	u8	Channel[MAX_CHANNEL_NUMBER + 1];
+	u8	Len;
+}CHANNEL_LIST, *PCHANNEL_LIST;
+
+#define IEEE80211_SOFTMAC_SCAN_TIME	  100//400
+//(HZ / 2)
+//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_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)
+
+#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
+#ifdef THOMAS_TURBO
+	u8 Turbo_Enable;//enable turbo mode, added by thomas
+#endif
+#ifdef ENABLE_DOT11D
+	u16 CountryIeLen;
+	u8 CountryIeBuf[MAX_IE_LEN];
+#endif
+};
+
+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 authentcated
+	 * 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 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]
+
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,11))
+extern inline int is_multicast_ether_addr(const u8 *addr)
+{
+        return ((addr[0] != 0xff) && (0x01 & addr[0]));
+}
+#endif
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,13))
+extern inline int is_broadcast_ether_addr(const u8 *addr)
+{
+	return ((addr[0] == 0xff) && (addr[1] == 0xff) && (addr[2] == 0xff) &&   \
+		(addr[3] == 0xff) && (addr[4] == 0xff) && (addr[5] == 0xff));
+}
+#endif
+
+#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;
+
+
+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 streaming contain a 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;
+
+#ifdef ENABLE_DOT11D
+	void * pDot11dInfo;
+	bool bGlobalDomain;
+
+	// For Liteon Ch12~13 passive scan
+	u8	MinPassiveChnlNum;
+	u8	IbssStartChnl;
+#else
+	/* map of allowed channels. 0 is dummy */
+	// FIXME: remeber to default to a basic channel plan depending of the PHY type
+	int channel_map[MAX_CHANNEL_NUMBER+1];
+#endif
+
+	int rate;       /* current rate */
+	int basic_rate;
+	//FIXME: pleace 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;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
+        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;
+#else
+
+	struct work_struct start_ibss_wq;
+        struct work_struct softmac_scan_wq;
+        struct work_struct associate_retry_wq;
+	struct work_struct hw_wakeup_wq;
+	struct work_struct hw_sleep_wq;
+	struct work_struct watch_dog_wq;
+	struct work_struct sw_antenna_wq;
+//by amy for rate adaptive 080312
+    struct work_struct rate_adapter_wq;
+//by amy for rate adaptive
+	struct work_struct hw_dig_wq;
+	struct work_struct tx_pw_wq;
+
+//Added for RF power on power off by lizhaoming 080512
+	struct work_struct GPIOChangeRFWorkItem;
+#endif
+	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 (mamagement) 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 fucntion 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. I 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 swithced 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 swithcing to ad-hoc mode
+	 * or in behalf of iwlist scan when the card is associated
+	 * and root user ask for a scan.
+	 * the fucntion stop_scan should stop both the syncro and
+	 * background scanning and can sleep.
+	 * The fucntion 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 respones 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;
+}
+
+extern 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;
+}
+
+extern 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;
+}
+
+extern 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_xmit(struct sk_buff *skb,
+			  struct net_device *dev);
+extern void ieee80211_txb_free(struct ieee80211_txb *);
+
+
+/* ieee80211_rx.c */
+extern int ieee80211_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 *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(struct ieee80211_network net);
+extern short ieee80211_is_shortslot(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_wake_queue(struct ieee80211_device *ieee);
+extern void ieee80211_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_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);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+extern void ieee80211_wx_sync_scan_wq(struct work_struct *work);
+#else
+ extern void ieee80211_wx_sync_scan_wq(struct ieee80211_device *ieee);
+#endif
+//extern void ieee80211_wx_sync_scan_wq(struct ieee80211_device *ieee);
+
+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/dot11d.c b/drivers/staging/rtl8187se/ieee80211/dot11d.c
new file mode 100644
index 000000000000..5d8b752fc0fb
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/dot11d.c
@@ -0,0 +1,246 @@
+#ifdef ENABLE_DOT11D

+//-----------------------------------------------------------------------------

+//	File:

+//		Dot11d.c

+//

+//	Description:

+//		Implement 802.11d. 

+//

+//-----------------------------------------------------------------------------

+

+#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);

+

+	printk("Dot11d_Init()\n");

+}

+

+//

+//	Description:

+//		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);

+

+	//printk("Dot11d_Reset()\n");

+}

+

+//

+//	Description:

+//		Update country IE from Beacon or Probe Resopnse 

+//		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;

+	PCHNL_TXPOWER_TRIPLE pTriple;

+

+	if((CoutryIeLen - 3)%3 != 0)

+	{

+		printk("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.

+			printk("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.

+			printk("Dot11d_UpdateCountryIe(): Invalid country IE, skip it........2\n");

+			Dot11d_Reset(dev);

+			return; 

+		}

+

+		for(j = 0 ; j < pTriple->NumChnls; j++)

+		{

+			pDot11dInfo->channel_map[pTriple->FirstChnl + j] = 1;

+			pDot11dInfo->MaxTxPwrDbmList[pTriple->FirstChnl + j] = pTriple->MaxTxPowerInDbm;

+			MaxChnlNum = pTriple->FirstChnl + j;

+		}	

+

+		pTriple = (PCHNL_TXPOWER_TRIPLE)((u8*)pTriple + 3);

+	}

+#if 1

+	//printk("Dot11d_UpdateCountryIe(): Channel List:\n");

+	printk("Channel List:");

+	for(i=1; i<= MAX_CHANNEL_NUMBER; i++)

+		if(pDot11dInfo->channel_map[i] > 0)

+			printk(" %d", i);

+	printk("\n");

+#endif

+

+	UPDATE_CIE_SRC(dev, pTaddr);

+

+	pDot11dInfo->CountryIeLen = CoutryIeLen;

+	memcpy(pDot11dInfo->CountryIeBuf, pCoutryIe,CoutryIeLen);

+	pDot11dInfo->State = DOT11D_STATE_LEARNED;

+}

+

+void dump_chnl_map(u8 * channel_map)

+{

+	int i;

+	printk("Channel List:");

+	for(i=1; i<= MAX_CHANNEL_NUMBER; i++)

+		if(channel_map[i] > 0)

+			printk(" %d(%d)", i, channel_map[i]);

+	printk("\n");

+}

+

+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)

+	{ 

+		printk("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)

+	{ 

+		printk("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)

+	{ 

+		printk("IsLegalChannel(): Invalid Channel\n");

+		return default_chn; 

+	}

+	

+	if(pDot11dInfo->channel_map[channel] > 0)

+		return channel;

+	

+	return default_chn;

+}

+

+#if 0

+EXPORT_SYMBOL(Dot11d_Init);

+EXPORT_SYMBOL(Dot11d_Reset);

+EXPORT_SYMBOL(Dot11d_UpdateCountryIe);

+EXPORT_SYMBOL(DOT11D_GetMaxTxPwrInDbm);

+EXPORT_SYMBOL(DOT11D_ScanComplete);

+EXPORT_SYMBOL(IsLegalChannel);

+EXPORT_SYMBOL(ToLegalChannel);

+#endif

+#endif

diff --git a/drivers/staging/rtl8187se/ieee80211/dot11d.h b/drivers/staging/rtl8187se/ieee80211/dot11d.h
new file mode 100644
index 000000000000..64bcf15bd219
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/dot11d.h
@@ -0,0 +1,102 @@
+#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_DOT11D_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

+);

+

+void dump_chnl_map(u8 * channel_map);

+#endif // #ifndef __INC_DOT11D_H

diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211.h b/drivers/staging/rtl8187se/ieee80211/ieee80211.h
new file mode 100644
index 000000000000..58336080ad50
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211.h
@@ -0,0 +1,1755 @@
+/*
+ * 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
+ * <andreamrl@tiscali.it>
+ *
+ * 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/version.h>
+#include <linux/jiffies.h>
+#include <linux/timer.h>
+#include <linux/sched.h>
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,13))
+#include <linux/wireless.h>
+#endif
+
+/*
+#ifndef bool
+#define bool int
+#endif
+
+#ifndef true
+#define true   1
+#endif
+
+#ifndef false
+#define false  0
+#endif
+*/
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20))
+#ifndef bool
+typedef enum{false = 0, true} bool;
+#endif
+#endif
+//#ifdef JOHN_HWSEC
+#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
+//#endif
+
+
+#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
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,6,10))
+#define ieee80211_wx_get_scan ieee80211_wx_get_scan_rtl
+#define ieee80211_wx_set_encode ieee80211_wx_set_encode_rtl
+#define ieee80211_wx_get_encode ieee80211_wx_get_encode_rtl
+////////////////////////////////
+// added for kernel conflict under FC5
+#define ieee80211_wx_get_name   ieee80211_wx_get_name_rtl
+#define free_ieee80211          free_ieee80211_rtl
+#define alloc_ieee80211        alloc_ieee80211_rtl
+///////////////////////////////
+#endif
+//error in ubuntu2.6.22,so add these
+#define ieee80211_wake_queue ieee80211_wake_queue_rtl
+#define ieee80211_stop_queue ieee80211_stop_queue_rtl
+
+#define ieee80211_rx ieee80211_rx_rtl
+
+#define ieee80211_register_crypto_ops	ieee80211_register_crypto_ops_rtl
+#define ieee80211_unregister_crypto_ops	ieee80211_unregister_crypto_ops_rtl
+#define ieee80211_get_crypto_ops	ieee80211_get_crypto_ops_rtl
+#define ieee80211_crypt_deinit_entries	ieee80211_crypt_deinit_entries_rtl
+#define ieee80211_crypt_deinit_handler	ieee80211_crypt_deinit_handler_rtl
+#define ieee80211_crypt_delayed_deinit	ieee80211_crypt_delayed_deinit_rtl
+
+#define ieee80211_txb_free	ieee80211_txb_free_rtl
+#define ieee80211_wx_get_essid	ieee80211_wx_get_essid_rtl
+#define ieee80211_wx_set_essid	ieee80211_wx_set_essid_rtl
+#define ieee80211_wx_set_rate	ieee80211_wx_set_rate_rtl
+#define ieee80211_wx_get_rate	ieee80211_wx_get_rate_rtl
+#define ieee80211_wx_set_wap	ieee80211_wx_set_wap_rtl
+#define ieee80211_wx_get_wap	ieee80211_wx_get_wap_rtl
+#define ieee80211_wx_set_mode	ieee80211_wx_set_mode_rtl
+#define ieee80211_wx_get_mode	ieee80211_wx_get_mode_rtl
+#define ieee80211_wx_set_scan	ieee80211_wx_set_scan_rtl
+#define ieee80211_wx_get_freq	ieee80211_wx_get_freq_rtl
+#define ieee80211_wx_set_freq	ieee80211_wx_set_freq_rtl
+#define ieee80211_wx_set_rawtx	ieee80211_wx_set_rawtx_rtl
+#define ieee80211_wx_set_power	ieee80211_wx_set_power_rtl
+#define ieee80211_wx_get_power	ieee80211_wx_get_power_rtl
+#define ieee80211_wlan_frequencies	ieee80211_wlan_frequencies_rtl
+#define ieee80211_softmac_stop_protocol	ieee80211_softmac_stop_protocol_rtl
+#define ieee80211_softmac_start_protocol ieee80211_softmac_start_protocol_rtl
+#define	ieee80211_start_protocol	ieee80211_start_protocol_rtl
+#define	ieee80211_stop_protocol		ieee80211_stop_protocol_rtl
+#define	ieee80211_rx_mgt		ieee80211_rx_mgt_rtl
+
+#define ieee80211_wx_set_auth ieee80211_wx_set_auth_rtl
+//by amy for ps
+#define notify_wx_assoc_event  notify_wx_assoc_event_rtl
+#define ieee80211_stop_send_beacons ieee80211_stop_send_beacons_rtl
+#define ieee80211_disassociate ieee80211_disassociate_rtl
+#define ieee80211_start_scan ieee80211_start_scan_rtl
+//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;
+
+
+#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
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,9))
+#define MSECS(t)	(1000 * ((t) / HZ) + 1000 * ((t) % HZ) / HZ)
+static inline unsigned long msleep_interruptible_rtl(unsigned int msecs)
+{
+         unsigned long timeout = MSECS(msecs) + 1;
+
+         while (timeout) {
+                 set_current_state(TASK_UNINTERRUPTIBLE);
+                 timeout = schedule_timeout(timeout);
+         }
+         return timeout;
+}
+#else
+#define MSECS(t) msecs_to_jiffies(t)
+#define msleep_interruptible_rtl  msleep_interruptible
+#endif
+
+#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_HLEN			30
+#define IEEE80211_FRAME_LEN		(IEEE80211_DATA_LEN + IEEE80211_HLEN)
+
+/* 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;
+};
+
+struct ieee80211_hdr {
+	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];
+} __attribute__ ((packed));
+
+struct ieee80211_hdr_QOS {
+	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_hdr_3addr {
+	u16 frame_ctl;
+	u16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	u16 seq_ctl;
+} __attribute__ ((packed));
+
+struct ieee80211_hdr_3addr_QOS {
+	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));
+
+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));
+
+#define IEEE80211_3ADDR_LEN 24
+#define IEEE80211_4ADDR_LEN 30
+#define IEEE80211_FCS_LEN    4
+
+#define MIN_FRAG_THRESHOLD     256U
+#define	MAX_FRAG_THRESHOLD     2346U
+
+/* Frame control field constants */
+#define IEEE80211_FCTL_VERS		0x0002
+#define IEEE80211_FCTL_FTYPE		0x000c
+#define IEEE80211_FCTL_STYPE		0x00f0
+#define IEEE80211_FCTL_TODS		0x0100
+#define IEEE80211_FCTL_FROMDS		0x0200
+#define IEEE80211_FCTL_DSTODS		0x0300 //added by david
+#define IEEE80211_FCTL_MOREFRAGS	0x0400
+#define IEEE80211_FCTL_RETRY		0x0800
+#define IEEE80211_FCTL_PM		0x1000
+#define IEEE80211_FCTL_MOREDATA	0x2000
+#define IEEE80211_FCTL_WEP		0x4000
+#define IEEE80211_FCTL_ORDER		0x8000
+
+#define IEEE80211_FTYPE_MGMT		0x0000
+#define IEEE80211_FTYPE_CTL		0x0004
+#define IEEE80211_FTYPE_DATA		0x0008
+
+/* management */
+#define IEEE80211_STYPE_ASSOC_REQ	0x0000
+#define IEEE80211_STYPE_ASSOC_RESP 	0x0010
+#define IEEE80211_STYPE_REASSOC_REQ	0x0020
+#define IEEE80211_STYPE_REASSOC_RESP	0x0030
+#define IEEE80211_STYPE_PROBE_REQ	0x0040
+#define IEEE80211_STYPE_PROBE_RESP	0x0050
+#define IEEE80211_STYPE_BEACON		0x0080
+#define IEEE80211_STYPE_ATIM		0x0090
+#define IEEE80211_STYPE_DISASSOC	0x00A0
+#define IEEE80211_STYPE_AUTH		0x00B0
+#define IEEE80211_STYPE_DEAUTH		0x00C0
+#define IEEE80211_STYPE_MANAGE_ACT	0x00D0
+
+/* control */
+#define IEEE80211_STYPE_PSPOLL		0x00A0
+#define IEEE80211_STYPE_RTS		0x00B0
+#define IEEE80211_STYPE_CTS		0x00C0
+#define IEEE80211_STYPE_ACK		0x00D0
+#define IEEE80211_STYPE_CFEND		0x00E0
+#define IEEE80211_STYPE_CFENDACK	0x00F0
+
+/* data */
+#define IEEE80211_STYPE_DATA		0x0000
+#define IEEE80211_STYPE_DATA_CFACK	0x0010
+#define IEEE80211_STYPE_DATA_CFPOLL	0x0020
+#define IEEE80211_STYPE_DATA_CFACKPOLL	0x0030
+#define IEEE80211_STYPE_NULLFUNC	0x0040
+#define IEEE80211_STYPE_CFACK		0x0050
+#define IEEE80211_STYPE_CFPOLL		0x0060
+#define IEEE80211_STYPE_CFACKPOLL	0x0070
+#define IEEE80211_STYPE_QOS_DATA	0x0080 //added for WMM 2006/8/2
+#define IEEE80211_STYPE_QOS_NULL	0x00C0
+
+
+#define IEEE80211_SCTL_FRAG		0x000F
+#define IEEE80211_SCTL_SEQ		0xFFF0
+
+
+/* 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/wireless.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)
+
+/* Authentication algorithms */
+#define WLAN_AUTH_OPEN 0
+#define WLAN_AUTH_SHARED_KEY 1
+
+#define WLAN_AUTH_CHALLENGE_LEN 128
+
+#define WLAN_CAPABILITY_BSS (1<<0)
+#define WLAN_CAPABILITY_IBSS (1<<1)
+#define WLAN_CAPABILITY_CF_POLLABLE (1<<2)
+#define WLAN_CAPABILITY_CF_POLL_REQUEST (1<<3)
+#define WLAN_CAPABILITY_PRIVACY (1<<4)
+#define WLAN_CAPABILITY_SHORT_PREAMBLE (1<<5)
+#define WLAN_CAPABILITY_PBCC (1<<6)
+#define WLAN_CAPABILITY_CHANNEL_AGILITY (1<<7)
+#define WLAN_CAPABILITY_SHORT_SLOT (1<<10)
+
+/* Status codes */
+#define WLAN_STATUS_SUCCESS 0
+#define WLAN_STATUS_UNSPECIFIED_FAILURE 1
+#define WLAN_STATUS_CAPS_UNSUPPORTED 10
+#define WLAN_STATUS_REASSOC_NO_ASSOC 11
+#define WLAN_STATUS_ASSOC_DENIED_UNSPEC 12
+#define WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG 13
+#define WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION 14
+#define WLAN_STATUS_CHALLENGE_FAIL 15
+#define WLAN_STATUS_AUTH_TIMEOUT 16
+#define WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA 17
+#define WLAN_STATUS_ASSOC_DENIED_RATES 18
+/* 802.11b */
+#define WLAN_STATUS_ASSOC_DENIED_NOSHORT 19
+#define WLAN_STATUS_ASSOC_DENIED_NOPBCC 20
+#define WLAN_STATUS_ASSOC_DENIED_NOAGILITY 21
+
+/* Reason codes */
+#define WLAN_REASON_UNSPECIFIED 1
+#define WLAN_REASON_PREV_AUTH_NOT_VALID 2
+#define WLAN_REASON_DEAUTH_LEAVING 3
+#define WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY 4
+#define WLAN_REASON_DISASSOC_AP_BUSY 5
+#define WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA 6
+#define WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA 7
+#define WLAN_REASON_DISASSOC_STA_HAS_LEFT 8
+#define WLAN_REASON_STA_REQ_ASSOC_WITHOUT_AUTH 9
+
+
+/* Information Element IDs */
+#define WLAN_EID_SSID 0
+#define WLAN_EID_SUPP_RATES 1
+#define WLAN_EID_FH_PARAMS 2
+#define WLAN_EID_DS_PARAMS 3
+#define WLAN_EID_CF_PARAMS 4
+#define WLAN_EID_TIM 5
+#define WLAN_EID_IBSS_PARAMS 6
+#define WLAN_EID_CHALLENGE 16
+#define WLAN_EID_RSN 48
+#define WLAN_EID_GENERIC 221
+
+#define IEEE80211_MGMT_HDR_LEN 24
+#define IEEE80211_DATA_HDR3_LEN 24
+#define IEEE80211_DATA_HDR4_LEN 30
+
+
+#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
+
+
+
+
+/* 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_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;
+};
+
+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
+
+*/
+
+struct ieee80211_header_data {
+	u16 frame_ctl;
+	u16 duration_id;
+	u8 addr1[6];
+	u8 addr2[6];
+	u8 addr3[6];
+	u16 seq_ctrl;
+};
+
+#define BEACON_PROBE_SSID_ID_POSITION 12
+
+/* Management Frame Information Element Types */
+#define MFIE_TYPE_SSID       0
+#define MFIE_TYPE_RATES      1
+#define MFIE_TYPE_FH_SET     2
+#define MFIE_TYPE_DS_SET     3
+#define MFIE_TYPE_CF_SET     4
+#define MFIE_TYPE_TIM        5
+#define MFIE_TYPE_IBSS_SET   6
+#define MFIE_TYPE_COUNTRY  7 //+YJ,080625
+#define MFIE_TYPE_CHALLENGE  16
+#define MFIE_TYPE_ERP        42
+#define MFIE_TYPE_RSN	     48
+#define MFIE_TYPE_RATES_EX   50
+#define MFIE_TYPE_GENERIC    221
+
+#ifdef ENABLE_DOT11D
+typedef 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
+}country_code_type_t;
+#endif
+
+struct ieee80211_info_element_hdr {
+	u8 id;
+	u8 len;
+} __attribute__ ((packed));
+
+struct ieee80211_info_element {
+	u8 id;
+	u8 len;
+	u8 data[0];
+} __attribute__ ((packed));
+
+/*
+ * 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
+
+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_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_probe_request {
+	struct ieee80211_header_data header;
+	/*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_disassoc_frame{
+        struct ieee80211_hdr_3addr header;
+        u16    reasoncode;
+}__attribute__ ((packed));
+
+struct ieee80211_txb {
+	u8 nr_frags;
+	u8 encrypted;
+	u16 reserved;
+	u16 frag_size;
+	u16 payload_size;
+	struct sk_buff *fragments[0];
+};
+
+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 {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)
+
+/* 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                 161
+#define MAX_CHANNEL_NUMBER                 165 //YJ,modified,080625
+#define MAX_IE_LEN						0xFF //+YJ,080625
+
+typedef struct _CHANNEL_LIST{
+	u8	Channel[MAX_CHANNEL_NUMBER + 1];
+	u8	Len;
+}CHANNEL_LIST, *PCHANNEL_LIST;
+
+#define IEEE80211_SOFTMAC_SCAN_TIME	  100//400
+//(HZ / 2)
+//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_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)
+
+#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
+#ifdef THOMAS_TURBO
+	u8 Turbo_Enable;//enable turbo mode, added by thomas
+#endif
+#ifdef ENABLE_DOT11D
+	u16 CountryIeLen;
+	u8 CountryIeBuf[MAX_IE_LEN];
+#endif
+};
+
+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 authentcated
+	 * 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 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]
+
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,11))
+extern inline int is_multicast_ether_addr(const u8 *addr)
+{
+        return ((addr[0] != 0xff) && (0x01 & addr[0]));
+}
+#endif
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,13))
+extern inline int is_broadcast_ether_addr(const u8 *addr)
+{
+	return ((addr[0] == 0xff) && (addr[1] == 0xff) && (addr[2] == 0xff) &&   \
+		(addr[3] == 0xff) && (addr[4] == 0xff) && (addr[5] == 0xff));
+}
+#endif
+
+#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;
+
+
+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 streaming contain a 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;
+
+#ifdef ENABLE_DOT11D
+	void * pDot11dInfo;
+	bool bGlobalDomain;
+
+	// For Liteon Ch12~13 passive scan
+	u8	MinPassiveChnlNum;
+	u8	IbssStartChnl;
+#else
+	/* map of allowed channels. 0 is dummy */
+	// FIXME: remeber to default to a basic channel plan depending of the PHY type
+	int channel_map[MAX_CHANNEL_NUMBER+1];
+#endif
+
+	int rate;       /* current rate */
+	int basic_rate;
+	//FIXME: pleace 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;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
+        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;
+#else
+
+	struct work_struct start_ibss_wq;
+        struct work_struct softmac_scan_wq;
+        struct work_struct associate_retry_wq;
+	struct work_struct hw_wakeup_wq;
+	struct work_struct hw_sleep_wq;
+	struct work_struct watch_dog_wq;
+	struct work_struct sw_antenna_wq;
+//by amy for rate adaptive 080312
+    struct work_struct rate_adapter_wq;
+//by amy for rate adaptive
+	struct work_struct hw_dig_wq;
+	struct work_struct tx_pw_wq;
+
+//Added for RF power on power off by lizhaoming 080512
+	struct work_struct GPIOChangeRFWorkItem;
+#endif
+	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 (mamagement) 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 fucntion 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. I 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 swithced 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 swithcing to ad-hoc mode
+	 * or in behalf of iwlist scan when the card is associated
+	 * and root user ask for a scan.
+	 * the fucntion stop_scan should stop both the syncro and
+	 * background scanning and can sleep.
+	 * The fucntion 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 respones 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;
+}
+
+extern 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;
+}
+
+extern 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;
+}
+
+extern 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_xmit(struct sk_buff *skb,
+			  struct net_device *dev);
+extern void ieee80211_txb_free(struct ieee80211_txb *);
+
+
+/* ieee80211_rx.c */
+extern int ieee80211_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 *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(struct ieee80211_network net);
+extern short ieee80211_is_shortslot(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_wake_queue(struct ieee80211_device *ieee);
+extern void ieee80211_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_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);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+extern void ieee80211_wx_sync_scan_wq(struct work_struct *work);
+#else
+ extern void ieee80211_wx_sync_scan_wq(struct ieee80211_device *ieee);
+#endif
+//extern void ieee80211_wx_sync_scan_wq(struct ieee80211_device *ieee);
+
+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
new file mode 100644
index 000000000000..af64cfbe16db
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.c
@@ -0,0 +1,265 @@
+/*
+ * 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.
+ *
+ */
+
+//#include <linux/config.h>
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <asm/string.h>
+#include <asm/errno.h>
+
+#if (LINUX_VERSION_CODE<KERNEL_VERSION(2,6,18))
+#include<linux/config.h>
+#endif
+
+#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);
+			module_put(entry->ops->owner);
+		}
+		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)) {
+		printk(KERN_DEBUG "%s: entries remaining in delayed crypt "
+		       "deletion list\n", ieee->dev->name);
+		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 = kmalloc(sizeof(*alg), GFP_KERNEL);
+	if (alg == NULL)
+		return -ENOMEM;
+
+	memset(alg, 0, sizeof(*alg));
+	alg->ops = ops;
+
+	spin_lock_irqsave(&hcrypt->lock, flags);
+	list_add(&alg->list, &hcrypt->algs);
+	spin_unlock_irqrestore(&hcrypt->lock, flags);
+
+	printk(KERN_DEBUG "ieee80211_crypt: 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) {
+		printk(KERN_DEBUG "ieee80211_crypt: 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 = kmalloc(sizeof(*hcrypt), GFP_KERNEL);
+	if (!hcrypt)
+		goto out;
+
+	memset(hcrypt, 0, sizeof(*hcrypt));
+	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;
+
+	if (hcrypt == NULL)
+		return;
+
+	for (ptr = hcrypt->algs.next, n = ptr->next; ptr != &hcrypt->algs;
+	     ptr = n, n = ptr->next) {
+		struct ieee80211_crypto_alg *alg =
+			(struct ieee80211_crypto_alg *) ptr;
+		list_del(ptr);
+		printk(KERN_DEBUG "ieee80211_crypt: unregistered algorithm "
+		       "'%s' (deinit)\n", alg->ops->name);
+		kfree(alg);
+	}
+
+	kfree(hcrypt);
+}
+
+#if 0
+EXPORT_SYMBOL(ieee80211_crypt_deinit_entries);
+EXPORT_SYMBOL(ieee80211_crypt_deinit_handler);
+EXPORT_SYMBOL(ieee80211_crypt_delayed_deinit);
+
+EXPORT_SYMBOL(ieee80211_register_crypto_ops);
+EXPORT_SYMBOL(ieee80211_unregister_crypto_ops);
+EXPORT_SYMBOL(ieee80211_get_crypto_ops);
+#endif
+
+//module_init(ieee80211_crypto_init);
+//module_exit(ieee80211_crypto_deinit);
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.h b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.h
new file mode 100644
index 000000000000..b58a3bcc0dc0
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.h
@@ -0,0 +1,86 @@
+/*
+ * 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
new file mode 100644
index 000000000000..08add385790f
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_ccmp.c
@@ -0,0 +1,533 @@
+/*
+ * 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.
+ */
+
+//#include <linux/config.h>
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.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 <linux/wireless.h>
+
+#include "ieee80211.h"
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
+#include "rtl_crypto.h"
+#else
+#include <linux/crypto.h>
+#endif
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+    #include <asm/scatterlist.h>
+#else
+    #include <linux/scatterlist.h>
+#endif
+
+//#include <asm/scatterlist.h>
+
+MODULE_AUTHOR("Jouni Malinen");
+MODULE_DESCRIPTION("Host AP crypt: CCMP");
+MODULE_LICENSE("GPL");
+
+#ifdef OPENSUSE_SLED
+#ifndef IN_OPENSUSE_SLED
+#define IN_OPENSUSE_SLED 1
+#endif
+#endif
+
+#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];
+};
+
+void ieee80211_ccmp_aes_encrypt(struct crypto_tfm *tfm,
+			     const u8 pt[16], u8 ct[16])
+{
+      	#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))||(IN_OPENSUSE_SLED))
+	crypto_cipher_encrypt_one((void *)tfm, ct, pt);
+	#else
+	struct scatterlist src, dst;
+
+	src.page = virt_to_page(pt);
+	src.offset = offset_in_page(pt);
+	src.length = AES_BLOCK_LEN;
+
+	dst.page = virt_to_page(ct);
+	dst.offset = offset_in_page(ct);
+	dst.length = AES_BLOCK_LEN;
+
+	crypto_cipher_encrypt(tfm, &dst, &src, AES_BLOCK_LEN);
+	#endif
+}
+
+static void * ieee80211_ccmp_init(int key_idx)
+{
+	struct ieee80211_ccmp_data *priv;
+
+	priv = kmalloc(sizeof(*priv), GFP_ATOMIC);
+	if (priv == NULL)
+		goto fail;
+	memset(priv, 0, sizeof(*priv));
+	priv->key_idx = key_idx;
+
+       #if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21)) && (!IN_OPENSUSE_SLED))
+	priv->tfm = crypto_alloc_tfm("aes", 0);
+	if (priv->tfm == NULL) {
+		printk(KERN_DEBUG "ieee80211_crypt_ccmp: could not allocate "
+		       "crypto API aes\n");
+		goto fail;
+	}
+       #else
+       priv->tfm = (void *)crypto_alloc_cipher("aes", 0, CRYPTO_ALG_ASYNC);
+	if (IS_ERR(priv->tfm)) {
+		printk(KERN_DEBUG "ieee80211_crypt_ccmp: could not allocate "
+		       "crypto API aes\n");
+		priv->tfm = NULL;
+		goto fail;
+	}
+	#endif
+	return priv;
+
+fail:
+	if (priv) {
+		if (priv->tfm)
+			//#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
+       			#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21)) && (!IN_OPENSUSE_SLED))
+			crypto_free_tfm(priv->tfm);
+                    #else
+			crypto_free_cipher((void *)priv->tfm);
+		      #endif
+		kfree(priv);
+	}
+
+	return NULL;
+}
+
+
+static void ieee80211_ccmp_deinit(void *priv)
+{
+	struct ieee80211_ccmp_data *_priv = priv;
+	if (_priv && _priv->tfm)
+		//#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
+       		#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21)) && (!IN_OPENSUSE_SLED))
+		crypto_free_tfm(_priv->tfm);
+             #else
+		crypto_free_cipher((void *)_priv->tfm);
+		#endif
+	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];
+}
+
+#ifndef JOHN_CCMP
+static void ccmp_init_blocks(struct crypto_tfm *tfm,
+			     struct ieee80211_hdr *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));
+        */
+	// fixed by David :2006.9.6
+	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);
+}
+#endif
+
+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 *hdr;
+#ifndef JOHN_CCMP
+	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;
+#endif
+	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;
+//	mic = skb_put(skb, CCMP_MIC_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 *) skb->data;
+#ifndef JOHN_CCMP
+	//mic is moved to here by john
+	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];
+#endif
+	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 *hdr;
+	u8 pn[6];
+#ifndef JOHN_CCMP
+	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;
+#endif
+	if (skb->len < hdr_len + CCMP_HDR_LEN + CCMP_MIC_LEN) {
+		key->dot11RSNAStatsCCMPFormatErrors++;
+		return -1;
+	}
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+	pos = skb->data + hdr_len;
+	keyidx = pos[3];
+	if (!(keyidx & (1 << 5))) {
+		if (net_ratelimit()) {
+			printk(KERN_DEBUG "CCMP: received packet without ExtIV"
+			       " flag from " MAC_FMT "\n", MAC_ARG(hdr->addr2));
+		}
+		key->dot11RSNAStatsCCMPFormatErrors++;
+		return -2;
+	}
+	keyidx >>= 6;
+	if (key->key_idx != keyidx) {
+		printk(KERN_DEBUG "CCMP: 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()) {
+			printk(KERN_DEBUG "CCMP: received packet from " MAC_FMT
+			       " with keyid=%d that does not have a configured"
+			       " key\n", MAC_ARG(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()) {
+			printk(KERN_DEBUG "CCMP: replay detected: STA=" MAC_FMT
+			       " previous PN %02x%02x%02x%02x%02x%02x "
+			       "received PN %02x%02x%02x%02x%02x%02x\n",
+			       MAC_ARG(hdr->addr2), MAC_ARG(key->rx_pn),
+			       MAC_ARG(pn));
+		}
+		key->dot11RSNAStatsCCMPReplays++;
+		return -4;
+	}
+
+#ifndef JOHN_CCMP
+	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()) {
+			printk(KERN_DEBUG "CCMP: decrypt failed: STA="
+			       MAC_FMT "\n", MAC_ARG(hdr->addr2));
+		}
+		key->dot11RSNAStatsCCMPDecryptErrors++;
+		return -5;
+	}
+
+	memcpy(key->rx_pn, pn, CCMP_PN_LEN);
+
+#endif
+	/* 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=%02x%02x%02x%02x%02x%02x "
+		     "rx_pn=%02x%02x%02x%02x%02x%02x "
+		     "format_errors=%d replays=%d decrypt_errors=%d\n",
+		     ccmp->key_idx, ccmp->key_set,
+		     MAC_ARG(ccmp->tx_pn), MAC_ARG(ccmp->rx_pn),
+		     ccmp->dot11RSNAStatsCCMPFormatErrors,
+		     ccmp->dot11RSNAStatsCCMPReplays,
+		     ccmp->dot11RSNAStatsCCMPDecryptErrors);
+
+	return p;
+}
+
+void ieee80211_ccmp_null(void)
+{
+//    printk("============>%s()\n", __func__);
+	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);
+}
+
+#if 0
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+EXPORT_SYMBOL(ieee80211_ccmp_null);
+#else
+EXPORT_SYMBOL_NOVERS(ieee80211_ccmp_null);
+#endif
+#endif
+
+//module_init(ieee80211_crypto_ccmp_init);
+//module_exit(ieee80211_crypto_ccmp_exit);
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_tkip.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_tkip.c
new file mode 100644
index 000000000000..de97bbec0da5
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_tkip.c
@@ -0,0 +1,1001 @@
+/*
+ * 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/config.h>
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.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"
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
+#include "rtl_crypto.h"
+#else
+#include <linux/crypto.h>
+#endif
+//#include <asm/scatterlist.h>
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20))
+    #include <asm/scatterlist.h>
+#else
+    #include <linux/scatterlist.h>
+#endif
+
+#include <linux/crc32.h>
+
+MODULE_AUTHOR("Jouni Malinen");
+MODULE_DESCRIPTION("Host AP crypt: TKIP");
+MODULE_LICENSE("GPL");
+
+#ifdef OPENSUSE_SLED
+#ifndef IN_OPENSUSE_SLED
+#define IN_OPENSUSE_SLED 1
+#endif
+#endif
+
+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;
+
+       #if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))||(IN_OPENSUSE_SLED))
+	   	struct crypto_blkcipher *rx_tfm_arc4;
+	       struct crypto_hash *rx_tfm_michael;
+	       struct crypto_blkcipher *tx_tfm_arc4;
+	       struct crypto_hash *tx_tfm_michael;
+       #endif
+
+	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 = kmalloc(sizeof(*priv), GFP_ATOMIC);
+	if (priv == NULL)
+		goto fail;
+	memset(priv, 0, sizeof(*priv));
+	priv->key_idx = key_idx;
+
+      #if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	priv->tfm_arc4 = crypto_alloc_tfm("arc4", 0);
+	if (priv->tfm_arc4 == NULL) {
+		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
+		       "crypto API arc4\n");
+		goto fail;
+	}
+
+	priv->tfm_michael = crypto_alloc_tfm("michael_mic", 0);
+	if (priv->tfm_michael == NULL) {
+		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
+		       "crypto API michael_mic\n");
+		goto fail;
+	}
+
+	#else
+	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;
+	}
+       #endif
+	return priv;
+
+fail:
+	if (priv) {
+		#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+		if (priv->tfm_michael)
+			crypto_free_tfm(priv->tfm_michael);
+		if (priv->tfm_arc4)
+			crypto_free_tfm(priv->tfm_arc4);
+             #else
+		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);
+		#endif
+		kfree(priv);
+	}
+
+	return NULL;
+}
+
+
+static void ieee80211_tkip_deinit(void *priv)
+{
+	struct ieee80211_tkip_data *_priv = priv;
+	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	if (_priv && _priv->tfm_michael)
+		crypto_free_tfm(_priv->tfm_michael);
+	if (_priv && _priv->tfm_arc4)
+		crypto_free_tfm(_priv->tfm_arc4);
+	#else
+	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);
+	}
+	#endif
+	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));
+}
+
+#ifndef JOHN_TKIP
+#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
+}
+#endif
+static int ieee80211_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+        struct ieee80211_tkip_data *tkey = priv;
+        #if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))||(IN_OPENSUSE_SLED))
+        struct blkcipher_desc desc = {.tfm = tkey->tx_tfm_arc4};
+        #endif
+	int len;
+	u8  *pos;
+	struct ieee80211_hdr *hdr;
+#ifndef JOHN_TKIP
+	u8 rc4key[16],*icv;
+	u32 crc;
+	struct scatterlist sg;
+#endif
+	int ret;
+
+	ret = 0;
+	if (skb_headroom(skb) < 8 || skb_tailroom(skb) < 4 ||
+	    skb->len < hdr_len)
+		return -1;
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+#if 0
+printk("@@ tkey\n");
+printk("%x|", ((u32*)tkey->key)[0]);
+printk("%x|", ((u32*)tkey->key)[1]);
+printk("%x|", ((u32*)tkey->key)[2]);
+printk("%x|", ((u32*)tkey->key)[3]);
+printk("%x|", ((u32*)tkey->key)[4]);
+printk("%x|", ((u32*)tkey->key)[5]);
+printk("%x|", ((u32*)tkey->key)[6]);
+printk("%x\n", ((u32*)tkey->key)[7]);
+#endif
+
+#ifndef JOHN_TKIP
+	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);
+
+#else
+	tkey->tx_phase1_done = 1;
+#endif  /*JOHN_TKIP*/
+
+	len = skb->len - hdr_len;
+	pos = skb_push(skb, 8);
+	memmove(pos, pos + 8, hdr_len);
+	pos += hdr_len;
+
+#ifdef JOHN_TKIP
+	*pos++ = Hi8(tkey->tx_iv16);
+	*pos++ = (Hi8(tkey->tx_iv16) | 0x20) & 0x7F;
+	*pos++ = Lo8(tkey->tx_iv16);
+#else
+	*pos++ = rc4key[0];
+	*pos++ = rc4key[1];
+	*pos++ = rc4key[2];
+#endif
+	*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;
+#ifndef JOHN_TKIP
+	icv = skb_put(skb, 4);
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+	crc = ~crc32_le(~0, pos, len);
+#else
+	crc = ~ether_crc_le(len, pos);
+#endif
+	icv[0] = crc;
+	icv[1] = crc >> 8;
+	icv[2] = crc >> 16;
+	icv[3] = crc >> 24;
+      #if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	crypto_cipher_setkey(tkey->tfm_arc4, rc4key, 16);
+	sg.page = virt_to_page(pos);
+	sg.offset = offset_in_page(pos);
+	sg.length = len + 4;
+	crypto_cipher_encrypt(tkey->tfm_arc4, &sg, &sg, len + 4);
+      #else
+	crypto_blkcipher_setkey(tkey->tx_tfm_arc4, rc4key, 16);
+        #if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24))
+          sg.page = virt_to_page(pos);
+          sg.offset = offset_in_page(pos);
+          sg.length = len + 4;
+        #else
+          sg_init_one(&sg, pos, len+4);
+        #endif
+	ret= crypto_blkcipher_encrypt(&desc, &sg, &sg, len + 4);
+      #endif
+#endif
+	tkey->tx_iv16++;
+	if (tkey->tx_iv16 == 0) {
+		tkey->tx_phase1_done = 0;
+		tkey->tx_iv32++;
+	}
+#ifndef JOHN_TKIP
+      #if((LINUX_VERSION_CODE <KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	   return 0;
+      #else
+	   return ret;
+      #endif
+#else
+	return 0;
+#endif
+}
+
+static int ieee80211_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+        struct ieee80211_tkip_data *tkey = priv;
+        #if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21)) ||(IN_OPENSUSE_SLED))
+        struct blkcipher_desc desc = {.tfm = tkey->rx_tfm_arc4};
+        #endif
+	u8 keyidx, *pos;
+	u32 iv32;
+	u16 iv16;
+	struct ieee80211_hdr *hdr;
+#ifndef JOHN_TKIP
+	u8 icv[4];
+	u32 crc;
+	struct scatterlist sg;
+	u8 rc4key[16];
+	int plen;
+#endif
+	if (skb->len < hdr_len + 8 + 4)
+		return -1;
+
+	hdr = (struct ieee80211_hdr *) 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 " MAC_FMT "\n", MAC_ARG(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 " MAC_FMT
+			       " with keyid=%d that does not have a configured"
+			       " key\n", MAC_ARG(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;
+#ifndef JOHN_TKIP
+
+	if (iv32 < tkey->rx_iv32 ||
+	    (iv32 == tkey->rx_iv32 && iv16 <= tkey->rx_iv16)) {
+		if (net_ratelimit()) {
+			printk(KERN_DEBUG "TKIP: replay detected: STA=" MAC_FMT
+			       " previous TSC %08x%04x received TSC "
+			       "%08x%04x\n", MAC_ARG(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;
+       #if((LINUX_VERSION_CODE <KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	crypto_cipher_setkey(tkey->tfm_arc4, rc4key, 16);
+	sg.page = virt_to_page(pos);
+	sg.offset = offset_in_page(pos);
+	sg.length = plen + 4;
+	crypto_cipher_decrypt(tkey->tfm_arc4, &sg, &sg, plen + 4);
+	#else
+	crypto_blkcipher_setkey(tkey->rx_tfm_arc4, rc4key, 16);
+        #if(LINUX_VERSION_CODE <KERNEL_VERSION(2,6,24))
+          sg.page = virt_to_page(pos);
+          sg.offset = offset_in_page(pos);
+          sg.length = plen + 4;
+        #else
+          sg_init_one(&sg, pos, plen+4);
+        #endif
+	if (crypto_blkcipher_decrypt(&desc, &sg, &sg, plen + 4)) {
+		if (net_ratelimit()) {
+			printk(KERN_DEBUG ": TKIP: failed to decrypt "
+			       "received packet from " MAC_FMT "\n",
+			       MAC_ARG(hdr->addr2));
+		}
+		return -7;
+	}
+	#endif
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+	crc = ~crc32_le(~0, pos, plen);
+#else
+	crc = ~ether_crc_le(plen, pos);
+#endif
+	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="
+			       MAC_FMT "\n", MAC_ARG(hdr->addr2));
+		}
+		tkey->dot11RSNAStatsTKIPICVErrors++;
+		return -5;
+	}
+
+#endif 	/* JOHN_TKIP */
+
+	/* 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);
+
+//john's test
+#ifdef JOHN_DUMP
+if( ((u16*)skb->data)[0] & 0x4000){
+        printk("@@ rx decrypted skb->data");
+        int i;
+        for(i=0;i<skb->len;i++){
+                if( (i%24)==0 ) printk("\n");
+                printk("%2x ", ((u8*)skb->data)[i]);
+        }
+        printk("\n");
+}
+#endif /*JOHN_DUMP*/
+	return keyidx;
+}
+
+#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21)) && (!IN_OPENSUSE_SLED))
+static int michael_mic(struct ieee80211_tkip_data *tkey, u8 *key, u8 *hdr,
+		       u8 *data, size_t data_len, u8 *mic)
+{
+	struct scatterlist sg[2];
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
+	struct hash_desc desc;
+	int ret=0;
+#endif
+	if (tkey->tfm_michael == NULL) {
+		printk(KERN_WARNING "michael_mic: tfm_michael == NULL\n");
+		return -1;
+	}
+	sg[0].page = virt_to_page(hdr);
+	sg[0].offset = offset_in_page(hdr);
+	sg[0].length = 16;
+
+	sg[1].page = virt_to_page(data);
+	sg[1].offset = offset_in_page(data);
+	sg[1].length = data_len;
+
+	//crypto_digest_init(tkey->tfm_michael);
+	//crypto_digest_setkey(tkey->tfm_michael, key, 8);
+	//crypto_digest_update(tkey->tfm_michael, sg, 2);
+	//crypto_digest_final(tkey->tfm_michael, mic);
+
+	//return 0;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	crypto_digest_init(tkey->tfm_michael);
+	crypto_digest_setkey(tkey->tfm_michael, key, 8);
+	crypto_digest_update(tkey->tfm_michael, sg, 2);
+	crypto_digest_final(tkey->tfm_michael, mic);
+
+	return 0;
+#else
+if (crypto_hash_setkey(tkey->tfm_michael, key, 8))
+		return -1;
+
+//	return 0;
+	desc.tfm = tkey->tfm_michael;
+	desc.flags = 0;
+	ret = crypto_hash_digest(&desc, sg, data_len + 16, mic);
+	return ret;
+#endif
+}
+#else
+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;
+        }
+        #if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24))
+          sg[0].page = virt_to_page(hdr);
+          sg[0].offset = offset_in_page(hdr);
+          sg[0].length = 16;
+          sg[1].page = virt_to_page(data);
+          sg[1].offset = offset_in_page(data);
+          sg[1].length = data_len;
+        #else
+          sg_init_table(sg, 2);
+          sg_set_buf(&sg[0], hdr, 16);
+          sg_set_buf(&sg[1], data, data_len);
+        #endif
+
+        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);
+}
+#endif
+
+
+
+static void michael_mic_hdr(struct sk_buff *skb, u8 *hdr)
+{
+	struct ieee80211_hdr *hdr11;
+
+	hdr11 = (struct ieee80211_hdr *) 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 *hdr;
+
+	hdr = (struct ieee80211_hdr *) 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);
+
+	// { david, 2006.9.1
+	// fix the wpa process with wmm enabled.
+	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((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	if (michael_mic(tkey, &tkey->key[16], tkey->tx_hdr,
+			skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
+        #else
+        if (michael_mic(tkey->tx_tfm_michael, &tkey->key[16], tkey->tx_hdr,
+                        skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
+        #endif
+		return -1;
+
+	return 0;
+}
+
+
+#if WIRELESS_EXT >= 18
+static void ieee80211_michael_mic_failure(struct net_device *dev,
+				       struct ieee80211_hdr *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);
+}
+#elif WIRELESS_EXT >= 15
+static void ieee80211_michael_mic_failure(struct net_device *dev,
+				       struct ieee80211_hdr *hdr,
+				       int keyidx)
+{
+	union iwreq_data wrqu;
+	char buf[128];
+
+	/* TODO: needed parameters: count, keyid, key type, TSC */
+	sprintf(buf, "MLME-MICHAELMICFAILURE.indication(keyid=%d %scast addr="
+		MAC_FMT ")", keyidx, hdr->addr1[0] & 0x01 ? "broad" : "uni",
+		MAC_ARG(hdr->addr2));
+	memset(&wrqu, 0, sizeof(wrqu));
+	wrqu.data.length = strlen(buf);
+	wireless_send_event(dev, IWEVCUSTOM, &wrqu, buf);
+}
+#else /* WIRELESS_EXT >= 15 */
+static inline void ieee80211_michael_mic_failure(struct net_device *dev,
+					      struct ieee80211_hdr *hdr,
+					      int keyidx)
+{
+}
+#endif /* WIRELESS_EXT >= 15 */
+
+
+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 *hdr;
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+
+	if (!tkey->key_set)
+		return -1;
+
+	michael_mic_hdr(skb, tkey->rx_hdr);
+	// { david, 2006.9.1
+	// fix the wpa process with wmm enabled.
+	if(IEEE80211_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl))) {
+		tkey->rx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
+	}
+	// }
+        #if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	if (michael_mic(tkey, &tkey->key[24], tkey->rx_hdr,
+			skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
+        #else
+	if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
+                        skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
+        #endif
+            	return -1;
+	if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
+		struct ieee80211_hdr *hdr;
+		hdr = (struct ieee80211_hdr *) skb->data;
+		printk(KERN_DEBUG "%s: Michael MIC verification failed for "
+		       "MSDU from " MAC_FMT " keyidx=%d\n",
+		       skb->dev ? skb->dev->name : "N/A", MAC_ARG(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;
+	#if ((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	struct crypto_tfm *tfm = tkey->tfm_michael;
+	struct crypto_tfm *tfm2 = tkey->tfm_arc4;
+	#else
+	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;
+	#endif
+
+	keyidx = tkey->key_idx;
+	memset(tkey, 0, sizeof(*tkey));
+	tkey->key_idx = keyidx;
+
+	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	tkey->tfm_michael = tfm;
+	tkey->tfm_arc4 = tfm2;
+       #else
+	tkey->tx_tfm_michael = tfm;
+	tkey->tx_tfm_arc4 = tfm2;
+	tkey->rx_tfm_michael = tfm3;
+	tkey->rx_tfm_arc4 = tfm4;
+	#endif
+
+	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)
+{
+//    printk("============>%s()\n", __func__);
+        return;
+}
+
+#if 0
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+EXPORT_SYMBOL(ieee80211_tkip_null);
+#else
+EXPORT_SYMBOL_NOVERS(ieee80211_tkip_null);
+#endif
+#endif
+
+
+//module_init(ieee80211_crypto_tkip_init);
+//module_exit(ieee80211_crypto_tkip_exit);
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_wep.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_wep.c
new file mode 100644
index 000000000000..68a22b32fcf0
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_wep.c
@@ -0,0 +1,394 @@
+/*
+ * 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.
+ */
+
+//#include <linux/config.h>
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <linux/skbuff.h>
+#include <asm/string.h>
+
+#include "ieee80211.h"
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
+#include "rtl_crypto.h"
+#else
+#include <linux/crypto.h>
+#endif
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+    #include <asm/scatterlist.h>
+#else
+    #include <linux/scatterlist.h>
+#endif
+//#include <asm/scatterlist.h>
+#include <linux/crc32.h>
+
+MODULE_AUTHOR("Jouni Malinen");
+MODULE_DESCRIPTION("Host AP crypt: WEP");
+MODULE_LICENSE("GPL");
+
+#ifdef OPENSUSE_SLED
+#ifndef IN_OPENSUSE_SLED
+#define IN_OPENSUSE_SLED 1
+#endif
+#endif
+
+
+struct prism2_wep_data {
+	u32 iv;
+#define WEP_KEY_LEN 13
+	u8 key[WEP_KEY_LEN + 1];
+	u8 key_len;
+	u8 key_idx;
+	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	struct crypto_tfm *tfm;
+	#else
+	struct crypto_blkcipher *tx_tfm;
+	struct crypto_blkcipher *rx_tfm;
+	#endif
+};
+
+
+static void * prism2_wep_init(int keyidx)
+{
+	struct prism2_wep_data *priv;
+
+	priv = kmalloc(sizeof(*priv), GFP_ATOMIC);
+	if (priv == NULL)
+		goto fail;
+	memset(priv, 0, sizeof(*priv));
+	priv->key_idx = keyidx;
+	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	priv->tfm = crypto_alloc_tfm("arc4", 0);
+     	if (priv->tfm == NULL) {
+		printk(KERN_DEBUG "ieee80211_crypt_wep: could not allocate "
+		       "crypto API arc4\n");
+		goto fail;
+	}
+	#else
+	priv->tx_tfm = crypto_alloc_blkcipher("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
+	if (IS_ERR(priv->tx_tfm)) {
+		printk(KERN_DEBUG "ieee80211_crypt_wep: 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)) {
+		printk(KERN_DEBUG "ieee80211_crypt_wep: could not allocate "
+		       "crypto API arc4\n");
+		priv->rx_tfm = NULL;
+		goto fail;
+	}
+	#endif
+
+	/* start WEP IV from a random value */
+	get_random_bytes(&priv->iv, 4);
+
+	return priv;
+
+fail:
+	//#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
+	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	if (priv) {
+		if (priv->tfm)
+			crypto_free_tfm(priv->tfm);
+		kfree(priv);
+	}
+       #else
+	if (priv) {
+		if (priv->tx_tfm)
+			crypto_free_blkcipher(priv->tx_tfm);
+		if (priv->rx_tfm)
+			crypto_free_blkcipher(priv->rx_tfm);
+		kfree(priv);
+	}
+	#endif
+	return NULL;
+}
+
+
+static void prism2_wep_deinit(void *priv)
+{
+	struct prism2_wep_data *_priv = priv;
+	//#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
+	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	if (_priv && _priv->tfm)
+		crypto_free_tfm(_priv->tfm);
+	#else
+	if (_priv) {
+		if (_priv->tx_tfm)
+			crypto_free_blkcipher(_priv->tx_tfm);
+		if (_priv->rx_tfm)
+			crypto_free_blkcipher(_priv->rx_tfm);
+	}
+        #endif
+	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;
+//#if(LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))||(IN_OPENSUSE_SLED))
+        struct blkcipher_desc desc = {.tfm = wep->tx_tfm};
+#endif
+	u32 klen, len;
+	u8 key[WEP_KEY_LEN + 3];
+	u8 *pos;
+#ifndef JOHN_HWSEC
+	u32 crc;
+	u8 *icv;
+	struct scatterlist sg;
+#endif
+	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);
+
+#ifndef JOHN_HWSEC
+	/* Append little-endian CRC32 and encrypt it to produce ICV */
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+	crc = ~crc32_le(~0, pos, len);
+#else
+	crc = ~ether_crc_le(len, pos);
+#endif
+	icv = skb_put(skb, 4);
+	icv[0] = crc;
+	icv[1] = crc >> 8;
+	icv[2] = crc >> 16;
+	icv[3] = crc >> 24;
+
+        //#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
+	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	crypto_cipher_setkey(wep->tfm, key, klen);
+	sg.page = virt_to_page(pos);
+	sg.offset = offset_in_page(pos);
+	sg.length = len + 4;
+	crypto_cipher_encrypt(wep->tfm, &sg, &sg, len + 4);
+
+	return 0;
+	#else
+	crypto_blkcipher_setkey(wep->tx_tfm, key, klen);
+        #if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24))
+          sg.page = virt_to_page(pos);
+          sg.offset = offset_in_page(pos);
+          sg.length = len + 4;
+        #else
+          sg_init_one(&sg, pos, len+4);
+        #endif
+	return crypto_blkcipher_encrypt(&desc, &sg, &sg, len + 4);
+	#endif
+#endif /* JOHN_HWSEC */
+	return 0;
+}
+
+
+/* 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;
+        //#if(LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))
+	#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))||(IN_OPENSUSE_SLED))
+        struct blkcipher_desc desc = {.tfm = wep->rx_tfm};
+        #endif
+	u32 klen, plen;
+	u8 key[WEP_KEY_LEN + 3];
+	u8 keyidx, *pos;
+#ifndef JOHN_HWSEC
+	u32 crc;
+	u8 icv[4];
+	struct scatterlist sg;
+#endif
+	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;
+#ifndef JOHN_HWSEC
+//#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
+#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	crypto_cipher_setkey(wep->tfm, key, klen);
+	sg.page = virt_to_page(pos);
+	sg.offset = offset_in_page(pos);
+	sg.length = plen + 4;
+	crypto_cipher_decrypt(wep->tfm, &sg, &sg, plen + 4);
+#else
+	crypto_blkcipher_setkey(wep->rx_tfm, key, klen);
+        #if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24))
+          sg.page = virt_to_page(pos);
+          sg.offset = offset_in_page(pos);
+          sg.length = plen + 4;
+        #else
+          sg_init_one(&sg, pos, plen+4);
+        #endif
+	if (crypto_blkcipher_decrypt(&desc, &sg, &sg, plen + 4))
+		return -7;
+#endif
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+	crc = ~crc32_le(~0, pos, plen);
+#else
+	crc = ~ether_crc_le(plen, pos);
+#endif
+	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;
+	}
+#endif 	/* JOHN_HWSEC */
+
+	/* 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)
+{
+//	printk("============>%s()\n", __func__);
+        return;
+}
+#if 0
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+EXPORT_SYMBOL(ieee80211_wep_null);
+#else
+EXPORT_SYMBOL_NOVERS(ieee80211_wep_null);
+#endif
+#endif
+//module_init(ieee80211_crypto_wep_init);
+//module_exit(ieee80211_crypto_wep_exit);
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_module.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_module.c
new file mode 100644
index 000000000000..0c9fef0b4e3f
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_module.c
@@ -0,0 +1,301 @@
+/*******************************************************************************
+
+  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/version.h>
+#include <linux/wireless.h>
+#include <linux/etherdevice.h>
+#include <asm/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 = kmalloc(
+		MAX_NETWORK_COUNT * sizeof(struct ieee80211_network),
+		GFP_KERNEL);
+	if (!ieee->networks) {
+		printk(KERN_WARNING "%s: Out of memory allocating beacons\n",
+		       ieee->dev->name);
+		return -ENOMEM;
+	}
+
+	memset(ieee->networks, 0,
+	       MAX_NETWORK_COUNT * sizeof(struct ieee80211_network));
+
+	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);
+	dev->hard_start_xmit = ieee80211_xmit;
+
+	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);
+				module_put(crypt->ops->owner);
+			}
+			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);
+}
+
+//#ifdef CONFIG_IEEE80211_DEBUG
+#if 0
+
+static int debug = 0;
+u32 ieee80211_debug_level = 0;
+struct proc_dir_entry *ieee80211_proc = NULL;
+
+static int show_debug_level(char *page, char **start, off_t offset,
+			    int count, int *eof, void *data)
+{
+	return snprintf(page, count, "0x%08X\n", ieee80211_debug_level);
+}
+
+static int store_debug_level(struct file *file, const char *buffer,
+			     unsigned long count, void *data)
+{
+	char buf[] = "0x00000000";
+	unsigned long len = min(sizeof(buf) - 1, (u32)count);
+	char *p = (char *)buf;
+	unsigned long val;
+
+	if (copy_from_user(buf, buffer, len))
+		return count;
+	buf[len] = 0;
+	if (p[1] == 'x' || p[1] == 'X' || p[0] == 'x' || p[0] == 'X') {
+		p++;
+		if (p[0] == 'x' || p[0] == 'X')
+			p++;
+		val = simple_strtoul(p, &p, 16);
+	} else
+		val = simple_strtoul(p, &p, 10);
+	if (p == buf)
+		printk(KERN_INFO DRV_NAME
+		       ": %s is not in hex or decimal form.\n", buf);
+	else
+		ieee80211_debug_level = val;
+
+	return strnlen(buf, count);
+}
+
+static int __init ieee80211_init(void)
+{
+	struct proc_dir_entry *e;
+
+	ieee80211_debug_level = debug;
+	ieee80211_proc = create_proc_entry(DRV_NAME, S_IFDIR, proc_net);
+	if (ieee80211_proc == NULL) {
+		IEEE80211_ERROR("Unable to create " DRV_NAME
+				" proc directory\n");
+		return -EIO;
+	}
+	e = create_proc_entry("debug_level", S_IFREG | S_IRUGO | S_IWUSR,
+			      ieee80211_proc);
+	if (!e) {
+		remove_proc_entry(DRV_NAME, proc_net);
+		ieee80211_proc = NULL;
+		return -EIO;
+	}
+	e->read_proc = show_debug_level;
+	e->write_proc = store_debug_level;
+	e->data = NULL;
+
+	return 0;
+}
+
+static void __exit ieee80211_exit(void)
+{
+	if (ieee80211_proc) {
+		remove_proc_entry("debug_level", ieee80211_proc);
+		remove_proc_entry(DRV_NAME, proc_net);
+		ieee80211_proc = NULL;
+	}
+}
+
+#include <linux/moduleparam.h>
+module_param(debug, int, 0444);
+MODULE_PARM_DESC(debug, "debug output mask");
+
+
+module_exit(ieee80211_exit);
+module_init(ieee80211_init);
+#endif
+
+#if 0
+EXPORT_SYMBOL(alloc_ieee80211);
+EXPORT_SYMBOL(free_ieee80211);
+#endif
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_rx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_rx.c
new file mode 100644
index 000000000000..23519b37538f
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_rx.c
@@ -0,0 +1,1971 @@
+/*
+ * 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 <andreamrl@tiscali.it>
+
+  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/version.h>
+#include <linux/wireless.h>
+#include <linux/etherdevice.h>
+#include <asm/uaccess.h>
+#include <linux/ctype.h>
+
+#include "ieee80211.h"
+#ifdef ENABLE_DOT11D
+#include "dot11d.h"
+#endif
+static inline void ieee80211_monitor_rx(struct ieee80211_device *ieee,
+					struct sk_buff *skb,
+					struct ieee80211_rx_stats *rx_stats)
+{
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	u16 fc = le16_to_cpu(hdr->frame_ctl);
+
+	skb->dev = ieee->dev;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,22)
+        skb_reset_mac_header(skb);
+#else
+        skb->mac.raw = skb->data;
+#endif
+	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 *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_3addr_QOS *hdr_3addr_QoS;
+	struct ieee80211_hdr_QOS *hdr_4addr_QoS;
+	u8 tid;
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->iw_mode == ieee->iw_ext_mode)
+	{
+		tid = (hdr->addr2[ETH_ALEN-2] ^ hdr->addr2[ETH_ALEN-1]) & IEEE80211_QOS_TID;
+	}
+	else
+#endif
+	if (((fc & IEEE80211_FCTL_DSTODS) == IEEE80211_FCTL_DSTODS)&&IEEE80211_QOS_HAS_SEQ(fc)) {
+	  hdr_4addr_QoS = (struct ieee80211_hdr_QOS *)hdr;
+	  tid = le16_to_cpu(hdr_4addr_QoS->QOS_ctl) & IEEE80211_QOS_TID;
+	  tid = UP2AC(tid);
+	  tid ++;
+	} else if (IEEE80211_QOS_HAS_SEQ(fc)) {
+	  hdr_3addr_QoS = (struct ieee80211_hdr_3addr_QOS *)hdr;
+	  tid = le16_to_cpu(hdr_3addr_QoS->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) +
+				    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 *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_3addr_QOS *hdr_3addr_QoS;
+	struct ieee80211_hdr_QOS *hdr_4addr_QoS;
+	u8 tid;
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->iw_mode == ieee->iw_ext_mode)
+	{
+		tid = (hdr->addr2[ETH_ALEN-2] ^ hdr->addr2[ETH_ALEN-1]) & IEEE80211_QOS_TID;
+	}
+	else
+#endif
+	if(((fc & IEEE80211_FCTL_DSTODS) == IEEE80211_FCTL_DSTODS)&&IEEE80211_QOS_HAS_SEQ(fc)) {
+	  hdr_4addr_QoS = (struct ieee80211_hdr_QOS *)hdr;
+	  tid = le16_to_cpu(hdr_4addr_QoS->QOS_ctl) & IEEE80211_QOS_TID;
+	  tid = UP2AC(tid);
+	  tid ++;
+	} else if (IEEE80211_QOS_HAS_SEQ(fc)) {
+	  hdr_3addr_QoS = (struct ieee80211_hdr_3addr_QOS *)hdr;
+	  tid = le16_to_cpu(hdr_3addr_QoS->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 *hdr;
+
+	// cheat the the hdr type
+	hdr = (struct ieee80211_hdr *)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 *)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;
+
+	#ifdef NOT_YET
+	if (ieee->iw_mode == IW_MODE_MASTER) {
+		printk(KERN_DEBUG "%s: Master mode not yet suppported.\n",
+		       ieee->dev->name);
+		return 0;
+/*
+  hostap_update_sta_ps(ieee, (struct hostap_ieee80211_hdr *)
+  skb->data);*/
+	}
+
+	if (ieee->hostapd && type == IEEE80211_TYPE_MGMT) {
+		if (stype == WLAN_FC_STYPE_BEACON &&
+		    ieee->iw_mode == IW_MODE_MASTER) {
+			struct sk_buff *skb2;
+			/* Process beacon frames also in kernel driver to
+			 * update STA(AP) table statistics */
+			skb2 = skb_clone(skb, GFP_ATOMIC);
+			if (skb2)
+				hostap_rx(skb2->dev, skb2, rx_stats);
+		}
+
+		/* send management frames to the user space daemon for
+		 * processing */
+		ieee->apdevstats.rx_packets++;
+		ieee->apdevstats.rx_bytes += skb->len;
+		prism2_rx_80211(ieee->apdev, skb, rx_stats, PRISM2_RX_MGMT);
+		return 0;
+	}
+
+	    if (ieee->iw_mode == IW_MODE_MASTER) {
+		if (type != WLAN_FC_TYPE_MGMT && type != WLAN_FC_TYPE_CTRL) {
+			printk(KERN_DEBUG "%s: unknown management frame "
+			       "(type=0x%02x, stype=0x%02x) dropped\n",
+			       skb->dev->name, type, stype);
+			return -1;
+		}
+
+		hostap_rx(skb->dev, skb, rx_stats);
+		return 0;
+	}
+
+	printk(KERN_DEBUG "%s: hostap_rx_frame_mgmt: management frame "
+	       "received in non-Host AP mode\n", skb->dev->name);
+	return -1;
+	#endif
+}
+
+
+
+/* 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 *hdr;
+	u8 *pos;
+
+	if (skb->len < 24)
+		return 0;
+
+	hdr = (struct ieee80211_hdr *) 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 *hdr;
+	int res, hdrlen;
+
+	if (crypt == NULL || crypt->ops->decrypt_mpdu == NULL)
+		return 0;
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && (ieee->ext_patch_ieee80211_rx_frame_get_hdrlen))
+	{
+		hdrlen = ieee->ext_patch_ieee80211_rx_frame_get_hdrlen(ieee, skb);
+	}
+	else
+#endif
+	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()) {
+			printk(KERN_DEBUG "%s: TKIP countermeasures: dropped "
+			       "received packet from " MAC_FMT "\n",
+			       ieee->dev->name, MAC_ARG(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=" MAC_FMT
+			") res=%d\n", MAC_ARG(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 *hdr;
+	int res, hdrlen;
+
+	if (crypt == NULL || crypt->ops->decrypt_msdu == NULL)
+		return 0;
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && (ieee->ext_patch_ieee80211_rx_frame_get_hdrlen))
+	{
+		hdrlen = ieee->ext_patch_ieee80211_rx_frame_get_hdrlen(ieee, skb);
+	}
+	else
+#endif
+	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) {
+		printk(KERN_DEBUG "%s: MSDU decryption/MIC verification failed"
+		       " (SA=" MAC_FMT " keyidx=%d)\n",
+		       ieee->dev->name, MAC_ARG(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 *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_3addr_QOS *hdr_3addr_QoS;
+	struct ieee80211_hdr_QOS *hdr_4addr_QoS;
+	u8 tid;
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->iw_mode == ieee->iw_ext_mode)
+	{
+		tid = (header->addr2[ETH_ALEN-2] ^ header->addr2[ETH_ALEN-1]) & IEEE80211_QOS_TID;
+	}
+	else
+#endif
+	//TO2DS and QoS
+	if(((fc & IEEE80211_FCTL_DSTODS) == IEEE80211_FCTL_DSTODS)&&IEEE80211_QOS_HAS_SEQ(fc)) {
+	  hdr_4addr_QoS = (struct ieee80211_hdr_QOS *)header;
+	  tid = le16_to_cpu(hdr_4addr_QoS->QOS_ctl) & IEEE80211_QOS_TID;
+	  tid = UP2AC(tid);
+	  tid ++;
+	} else if(IEEE80211_QOS_HAS_SEQ(fc)) { //QoS
+	  hdr_3addr_QoS = (struct ieee80211_hdr_3addr_QOS*)header;
+	  tid = le16_to_cpu(hdr_3addr_QoS->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;
+		//for (pos = (head)->next; pos != (head); pos = pos->next)
+		__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) {
+				printk(KERN_WARNING "Cannot malloc new mac entry\n");
+				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:
+#ifdef _RTL8187_EXT_PATCH_
+		if(ieee->iw_mode == ieee->iw_ext_mode)
+		{
+			last_seq = &ieee->last_rxseq_num[tid];
+			last_frag = &ieee->last_rxfrag_num[tid];
+			last_time = &ieee->last_packet_time[tid];
+			break;
+		}
+		else
+#endif
+		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_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 *hdr;
+	//struct ieee80211_hdr_3addr_QOS *hdr;
+
+	size_t hdrlen;
+	u16 fc, type, stype, sc;
+	struct net_device_stats *stats;
+	unsigned int frag;
+	u8 *payload;
+	u16 ethertype;
+#ifdef NOT_YET
+	struct net_device *wds = NULL;
+	struct sk_buff *skb2 = NULL;
+	struct net_device *wds = NULL;
+	int frame_authorized = 0;
+	int from_assoc_ap = 0;
+	void *sta = NULL;
+#endif
+//	u16 QOS_ctl = 0;
+	u8 dst[ETH_ALEN];
+	u8 src[ETH_ALEN];
+	u8 bssid[ETH_ALEN];
+	struct ieee80211_crypt_data *crypt = NULL;
+	int keyidx = 0;
+
+	//Added for mesh by Lawrence.
+#ifdef _RTL8187_EXT_PATCH_
+	u8 status;
+	u32 flags;
+#endif
+	// cheat the the hdr type
+	hdr = (struct ieee80211_hdr *)skb->data;
+	stats = &ieee->stats;
+
+	if (skb->len < 10) {
+		printk(KERN_INFO "%s: SKB length < 10\n",
+		       dev->name);
+		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
+
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && (ieee->ext_patch_ieee80211_rx_frame_get_hdrlen))
+	{
+		hdrlen = ieee->ext_patch_ieee80211_rx_frame_get_hdrlen(ieee, skb);
+		if(skb->len < hdrlen)
+			goto rx_dropped;
+	}
+	else
+#endif
+	hdrlen = ieee80211_get_hdrlen(fc);
+
+#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 */
+	/* If spy monitoring on */
+	if (iface->spy_data.spy_number > 0) {
+		struct iw_quality wstats;
+		wstats.level = rx_stats->signal;
+		wstats.noise = rx_stats->noise;
+		wstats.updated = 6;	/* No qual value */
+		/* Update spy records */
+		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)
+			idx = skb->data[hdrlen + 3] >> 6;
+		crypt = ieee->crypt[idx];
+#ifdef NOT_YET
+		sta = NULL;
+
+		/* Use station specific key to override default keys if the
+		 * receiver address is a unicast address ("individual RA"). If
+		 * bcrx_sta_key parameter is set, station specific key is used
+		 * even with broad/multicast targets (this is against IEEE
+		 * 802.11, but makes it easier to use different keys with
+		 * stations that do not support WEP key mapping). */
+
+		if (!(hdr->addr1[0] & 0x01) || local->bcrx_sta_key)
+			(void) hostap_handle_sta_crypto(local, hdr, &crypt,
+							&sta);
+#endif
+
+		/* 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=" MAC_FMT ")\n",
+					     MAC_ARG(hdr->addr2));
+			ieee->ieee_stats.rx_discards_undecryptable++;
+			goto rx_dropped;
+		}
+	}
+
+	if (skb->len < IEEE80211_DATA_HDR3_LEN)
+		goto rx_dropped;
+
+#ifdef _RTL8187_EXT_PATCH_
+	if( ieee->iw_mode == ieee->iw_ext_mode && ieee->ext_patch_ieee80211_rx_mgt_update_expire )
+		ieee->ext_patch_ieee80211_rx_mgt_update_expire( ieee, skb );
+#endif
+
+	// 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 0
+		if ( stype == IEEE80211_STYPE_AUTH &&
+		    fc & IEEE80211_FCTL_WEP && ieee->host_decrypt &&
+		    (keyidx = hostap_rx_frame_decrypt(ieee, skb, crypt)) < 0)
+		{
+			printk(KERN_DEBUG "%s: failed to decrypt mgmt::auth "
+			       "from " MAC_FMT "\n", dev->name,
+			       MAC_ARG(hdr->addr2));
+			/* TODO: could inform hostapd about this so that it
+			 * could send auth failure report */
+			goto rx_dropped;
+		}
+	#endif
+
+
+		if (ieee80211_rx_frame_mgmt(ieee, skb, rx_stats, type, stype))
+			goto rx_dropped;
+		else
+			goto rx_exit;
+	}
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_rx_on_rx)
+	{
+		if(ieee->ext_patch_ieee80211_rx_on_rx(ieee, skb, rx_stats, type, stype)==0)
+		{
+			goto rx_exit;
+		}
+	}
+#endif
+
+	/* 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;
+	}
+
+#ifdef NOT_YET
+	if (hostap_rx_frame_wds(ieee, hdr, fc, &wds))
+		goto rx_dropped;
+	if (wds) {
+		skb->dev = dev = wds;
+		stats = hostap_get_stats(dev);
+	}
+
+	if (ieee->iw_mode == IW_MODE_MASTER && !wds &&
+	    (fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) == IEEE80211_FCTL_FROMDS &&
+	    ieee->stadev &&
+	    memcmp(hdr->addr2, ieee->assoc_ap_addr, ETH_ALEN) == 0) {
+		/* Frame from BSSID of the AP for which we are a client */
+		skb->dev = dev = ieee->stadev;
+		stats = hostap_get_stats(dev);
+		from_assoc_ap = 1;
+	}
+#endif
+
+	dev->last_rx = jiffies;
+
+#ifdef NOT_YET
+	if ((ieee->iw_mode == IW_MODE_MASTER ||
+	     ieee->iw_mode == IW_MODE_REPEAT) &&
+	    !from_assoc_ap) {
+		switch (hostap_handle_sta_rx(ieee, dev, skb, rx_stats,
+					     wds != NULL)) {
+		case AP_RX_CONTINUE_NOT_AUTHORIZED:
+			frame_authorized = 0;
+			break;
+		case AP_RX_CONTINUE:
+			frame_authorized = 1;
+			break;
+		case AP_RX_DROP:
+			goto rx_dropped;
+		case AP_RX_EXIT:
+			goto rx_exit;
+		}
+	}
+#endif
+
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_rx_is_valid_framectl)
+	{
+		if(ieee->ext_patch_ieee80211_rx_is_valid_framectl(ieee, fc, type, stype)==0)
+			goto rx_dropped;
+	}
+	else
+#endif
+	/* 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 *) 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) {
+			printk(KERN_WARNING "%s: host decrypted and "
+			       "reassembled frame did not fit skb\n",
+			       dev->name);
+			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 *) 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 *) 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=" MAC_FMT ")\n",
+				MAC_ARG(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 " MAC_FMT
+			" (drop_unencrypted=1)\n",
+			MAC_ARG(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];
+
+#ifdef NOT_YET
+	/* If IEEE 802.1X is used, check whether the port is authorized to send
+	 * the received frame. */
+	if (ieee->ieee802_1x && ieee->iw_mode == IW_MODE_MASTER) {
+		if (ethertype == ETH_P_PAE) {
+			printk(KERN_DEBUG "%s: RX: IEEE 802.1X frame\n",
+			       dev->name);
+			if (ieee->hostapd && ieee->apdev) {
+				/* Send IEEE 802.1X frames to the user
+				 * space daemon for processing */
+				prism2_rx_80211(ieee->apdev, skb, rx_stats,
+						PRISM2_RX_MGMT);
+				ieee->apdevstats.rx_packets++;
+				ieee->apdevstats.rx_bytes += skb->len;
+				goto rx_exit;
+			}
+		} else if (!frame_authorized) {
+			printk(KERN_DEBUG "%s: dropped frame from "
+			       "unauthorized port (IEEE 802.1X): "
+			       "ethertype=0x%04x\n",
+			       dev->name, ethertype);
+			goto rx_dropped;
+		}
+	}
+#endif
+
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_rx_process_dataframe)
+	{
+	//Added for mesh rx interrupt.
+		//spin_lock_irqsave(&ieee->lock,flags);
+		status = ieee->ext_patch_ieee80211_rx_process_dataframe(ieee, skb, rx_stats);
+		//spin_unlock_irqrestore(&ieee->lock,flags);
+
+		if(status)
+//	if(ieee->ext_patch_ieee80211_rx_process_dataframe(ieee, skb, rx_stats))
+			goto rx_exit;
+		else
+			goto rx_dropped;
+	}
+#endif
+
+	/* 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);
+	}
+
+#ifdef NOT_YET
+	if (wds && ((fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
+		    IEEE80211_FCTL_TODS) &&
+	    skb->len >= ETH_HLEN + ETH_ALEN) {
+		/* Non-standard frame: get addr4 from its bogus location after
+		 * the payload */
+		memcpy(skb->data + ETH_ALEN,
+		       skb->data + skb->len - ETH_ALEN, ETH_ALEN);
+		skb_trim(skb, skb->len - ETH_ALEN);
+	}
+#endif
+
+	stats->rx_packets++;
+	stats->rx_bytes += skb->len;
+
+#ifdef NOT_YET
+	if (ieee->iw_mode == IW_MODE_MASTER && !wds &&
+	    ieee->ap->bridge_packets) {
+		if (dst[0] & 0x01) {
+			/* copy multicast frame both to the higher layers and
+			 * to the wireless media */
+			ieee->ap->bridged_multicast++;
+			skb2 = skb_clone(skb, GFP_ATOMIC);
+			if (skb2 == NULL)
+				printk(KERN_DEBUG "%s: skb_clone failed for "
+				       "multicast frame\n", dev->name);
+		} else if (hostap_is_sta_assoc(ieee->ap, dst)) {
+			/* send frame directly to the associated STA using
+			 * wireless media and not passing to higher layers */
+			ieee->ap->bridged_unicast++;
+			skb2 = skb;
+			skb = NULL;
+		}
+	}
+
+	if (skb2 != NULL) {
+		/* send to wireless media */
+		skb2->protocol = __constant_htons(ETH_P_802_3);
+		skb2->mac.raw = skb2->nh.raw = skb2->data;
+		/* skb2->nh.raw = skb2->data + ETH_HLEN; */
+		skb2->dev = dev;
+		dev_queue_xmit(skb2);
+	}
+
+#endif
+	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:
+#ifdef NOT_YET
+	if (sta)
+		hostap_handle_sta_release(sta);
+#endif
+	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;
+}
+
+#ifdef _RTL8187_EXT_PATCH_
+int ieee_ext_skb_p80211_to_ether(struct sk_buff *skb, int hdrlen, u8 *dst, u8 *src)
+{
+	u8 *payload;
+	u16 ethertype;
+
+	/* 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);
+	}
+
+	return 1;
+}
+#endif // _RTL8187_EXT_PATCH_
+
+
+#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;
+}
+
+#ifdef ENABLE_DOT11D
+static inline void ieee80211_extract_country_ie(
+	struct ieee80211_device *ieee,
+	struct ieee80211_info_element *info_element,
+	struct ieee80211_network *network,
+	u8 * addr2
+)
+{
+#if 0
+	u32 i = 0;
+	u8 * p = (u8*)info_element->data;
+	printk("-----------------------\n");
+	printk("%s Country IE:", network->ssid);
+	for(i=0; i<info_element->len; i++)
+		printk("\t%2.2x", *(p+i));
+	printk("\n-----------------------\n");
+#endif
+	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);
+		}
+	}
+
+}
+#endif
+
+int
+ieee80211_TranslateToDbm(
+	unsigned char SignalStrengthIndex	// 0-100 index.
+	)
+{
+	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
+#ifdef THOMAS_TURBO
+	network->Turbo_Enable = 0;
+#endif
+#ifdef ENABLE_DOT11D
+	network->CountryIeLen = 0;
+	memset(network->CountryIeBuf, 0, MAX_IE_LEN);
+#endif
+
+	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;
+#if 0
+			network->last_dtim_sta_time[0] = stats->mac_time[0];
+#else
+			network->last_dtim_sta_time[0] = jiffies;
+#endif
+			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);
+			}
+
+#ifdef THOMAS_TURBO
+			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;
+			}
+#endif
+			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;
+#ifdef ENABLE_DOT11D
+		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;
+#endif
+		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 (" MAC_FMT ")' "
+				     "network.\n",
+				     escape_essid(network->ssid,
+						  network->ssid_len),
+				     MAC_ARG(network->bssid));
+		return 1;
+	}
+
+	if (ieee80211_is_empty_essid(network->ssid, network->ssid_len))
+		network->flags |= NETWORK_EMPTY_ESSID;
+#if 0
+	stats->signal = ieee80211_SignalStrengthTranslate(stats->signal);
+#endif
+	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[1].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;
+#ifdef THOMAS_TURBO
+	dst->Turbo_Enable = src->Turbo_Enable;
+#endif
+#ifdef ENABLE_DOT11D
+	dst->CountryIeLen = src->CountryIeLen;
+	memcpy(dst->CountryIeBuf, src->CountryIeBuf, src->CountryIeLen);
+#endif
+}
+
+
+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));
+//rz
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_process_probe_response_1) {
+		ieee->ext_patch_ieee80211_process_probe_response_1(ieee, beacon, stats);
+		return;
+	}
+#endif
+
+	IEEE80211_DEBUG_SCAN(
+		"'%s' (" MAC_FMT "): %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),
+		MAC_ARG(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 0
+	if(strcmp(escape_essid(beacon->info_element.data, beacon->info_element.len), "rtl_softap") == 0)
+	{
+		if(WLAN_FC_GET_STYPE(beacon->header.frame_ctl) == IEEE80211_STYPE_BEACON)
+		{
+			u32 i = 0, len = stats->len;
+			u8 * p = (u8*)beacon;
+			printk("-----------------------\n");
+			printk("rtl_softap Beacon:");
+			for(i=0; i<len; i++)
+				printk("\t%2.2x", *(p+i));
+			printk("\n-----------------------\n");
+		}
+	}
+#endif
+	if (ieee80211_network_init(ieee, beacon, &network, stats)) {
+		IEEE80211_DEBUG_SCAN("Dropped '%s' (" MAC_FMT ") via %s.\n",
+				     escape_essid(info_element->data,
+						  info_element->len),
+				     MAC_ARG(beacon->header.addr3),
+				     WLAN_FC_GET_STYPE(beacon->header.frame_ctl) ==
+				     IEEE80211_STYPE_PROBE_RESP ?
+				     "PROBE RESPONSE" : "BEACON");
+		return;
+	}
+
+#ifdef ENABLE_DOT11D
+	// 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;
+				}
+			}
+		}
+	}
+#endif
+	/* 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' (" MAC_FMT ") from "
+					     "network list.\n",
+					     escape_essid(target->ssid,
+							  target->ssid_len),
+					     MAC_ARG(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' (" MAC_FMT ") via %s.\n",
+				     escape_essid(network.ssid,
+						  network.ssid_len),
+				     MAC_ARG(network.bssid),
+				     WLAN_FC_GET_STYPE(beacon->header.frame_ctl) ==
+				     IEEE80211_STYPE_PROBE_RESP ?
+				     "PROBE RESPONSE" : "BEACON");
+#endif
+
+#ifdef _RTL8187_EXT_PATCH_
+	network.ext_entry = target->ext_entry;
+#endif
+		memcpy(target, &network, sizeof(*target));
+		list_add_tail(&target->list, &ieee->network_list);
+		if(ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE)
+			ieee80211_softmac_new_net(ieee,&network);
+	} else {
+		IEEE80211_DEBUG_SCAN("Updating '%s' (" MAC_FMT ") via %s.\n",
+				     escape_essid(target->ssid,
+						  target->ssid_len),
+				     MAC_ARG(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);
+		if(renew && (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE))
+			ieee80211_softmac_new_net(ieee,&network);
+	}
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+
+void ieee80211_rx_mgt(struct ieee80211_device *ieee,
+		      struct ieee80211_hdr *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;
+//rz
+#ifdef _RTL8187_EXT_PATCH_
+	case IEEE80211_STYPE_PROBE_REQ:
+		IEEE80211_DEBUG_MGMT("received PROBE REQUEST (%d)\n",
+				     WLAN_FC_GET_STYPE(header->frame_ctl));
+		IEEE80211_DEBUG_SCAN("Probe request\n");
+		///
+		if( ieee->iw_mode == ieee->iw_ext_mode && ieee->ext_patch_ieee80211_rx_mgt_on_probe_req )
+			ieee->ext_patch_ieee80211_rx_mgt_on_probe_req( ieee, (struct ieee80211_probe_request *)header, stats);
+		break;
+#endif // _RTL8187_EXT_PATCH_
+
+	}
+}
+
+#if 0
+EXPORT_SYMBOL(ieee80211_rx_mgt);
+EXPORT_SYMBOL(ieee80211_rx);
+EXPORT_SYMBOL(ieee80211_network_init);
+#ifdef _RTL8187_EXT_PATCH_
+EXPORT_SYMBOL(ieee_ext_skb_p80211_to_ether);
+#endif
+#endif
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c
new file mode 100644
index 000000000000..e5752f615e09
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c
@@ -0,0 +1,4029 @@
+/* IEEE 802.11 SoftMAC layer
+ * Copyright (c) 2005 Andrea Merello <andreamrl@tiscali.it>
+ *
+ * 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/version.h>
+#include <asm/uaccess.h>
+
+#ifdef ENABLE_DOT11D
+#include "dot11d.h"
+#endif
+u8 rsn_authen_cipher_suite[16][4] = {
+	{0x00,0x0F,0xAC,0x00}, //Use group key, //Reserved
+	{0x00,0x0F,0xAC,0x01}, //WEP-40         //RSNA default
+	{0x00,0x0F,0xAC,0x02}, //TKIP           //NONE		//{used just as default}
+	{0x00,0x0F,0xAC,0x03}, //WRAP-historical
+	{0x00,0x0F,0xAC,0x04}, //CCMP
+	{0x00,0x0F,0xAC,0x05}, //WEP-104
+};
+
+short ieee80211_is_54g(struct ieee80211_network net)
+{
+	return ((net.rates_ex_len > 0) || (net.rates_len > 4));
+}
+
+short ieee80211_is_shortslot(struct ieee80211_network net)
+{
+	return (net.capability & WLAN_CAPABILITY_SHORT_SLOT);
+}
+
+/* returns the total length needed for pleacing 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
+ */
+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;
+}
+
+/* pleace the MFIE rate, tag to the memory (double) poined.
+ * Then it updates the pointer so that
+ * it points after the new MFIE tag added.
+ */
+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;
+}
+
+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;
+}
+
+
+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;
+}
+
+#ifdef THOMAS_TURBO
+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");
+}
+#endif
+
+void enqueue_mgmt(struct ieee80211_device *ieee, struct sk_buff *skb)
+{
+	int nh;
+	nh = (ieee->mgmt_queue_head +1) % MGMT_QUEUE_NUM;
+
+/*
+ * if the queue is full but we have newer frames then
+ * just overwrites the oldest.
+ *
+ * if (nh == ieee->mgmt_queue_tail)
+ *		return -1;
+ */
+	ieee->mgmt_queue_head = nh;
+	ieee->mgmt_queue_ring[nh] = skb;
+
+	//return 0;
+}
+
+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;
+}
+
+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_ctl = 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_ctl = 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_ctl = 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_ctl = 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);
+
+	}
+//	dev_kfree_skb_any(skb);//edit by thomas
+}
+//by amy for power save
+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_ctl = 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);
+                //dev_kfree_skb_any(skb);//edit by thomas
+        }
+}
+
+//by amy for power save
+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;
+
+#ifdef _RTL8187_EXT_PATCH_
+	short extMore = 0;
+	if(ieee->ext_patch_ieee80211_probe_req_1)
+		extMore = ieee->ext_patch_ieee80211_probe_req_1(ieee);
+#endif
+
+	len = ieee->current_network.ssid_len;
+
+	rate_len = ieee80211_MFIE_rate_len(ieee);
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(!extMore)
+#endif
+	skb = dev_alloc_skb(sizeof(struct ieee80211_probe_request) +
+			    2 + len + rate_len);
+#ifdef _RTL8187_EXT_PATCH_
+	else
+		skb = dev_alloc_skb(sizeof(struct ieee80211_probe_request) +
+			    2 + len + rate_len+128); // MESHID + CAP
+#endif
+
+	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);
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(extMore)
+		ieee->ext_patch_ieee80211_probe_req_2(ieee, skb, tag);
+#endif
+	return skb;
+}
+
+struct sk_buff *ieee80211_get_beacon_(struct ieee80211_device *ieee);
+
+//#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
+//void ext_ieee80211_send_beacon_wq(struct work_struct *work)
+//{
+//	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, ext_send_beacon_wq);
+//#else
+void ext_ieee80211_send_beacon_wq(struct ieee80211_device *ieee)
+{
+//#endif
+
+	struct sk_buff *skb;
+
+	//unsigned long flags;
+
+	skb = ieee80211_get_beacon_(ieee);
+
+	if (skb){
+		softmac_mgmt_xmit(skb, ieee);
+		ieee->softmac_stats.tx_beacons++;
+		dev_kfree_skb_any(skb);//edit by thomas
+	}
+
+
+	//printk(KERN_WARNING "[1] beacon sending!\n");
+	ieee->beacon_timer.expires = jiffies +
+		(MSECS( ieee->current_network.beacon_interval -5));
+
+	//spin_lock_irqsave(&ieee->beacon_lock,flags);
+	if(ieee->beacon_txing)
+		add_timer(&ieee->beacon_timer);
+	//spin_unlock_irqrestore(&ieee->beacon_lock,flags);
+}
+
+void ieee80211_send_beacon(struct ieee80211_device *ieee)
+{
+	struct sk_buff *skb;
+
+	//unsigned long flags;
+
+	skb = ieee80211_get_beacon_(ieee);
+
+	if (skb){
+		softmac_mgmt_xmit(skb, ieee);
+		ieee->softmac_stats.tx_beacons++;
+		dev_kfree_skb_any(skb);//edit by thomas
+	}
+
+	//printk(KERN_WARNING "[1] beacon sending!\n");
+	ieee->beacon_timer.expires = jiffies +
+		(MSECS( ieee->current_network.beacon_interval -5));
+
+	//spin_lock_irqsave(&ieee->beacon_lock,flags);
+	if(ieee->beacon_txing)
+		add_timer(&ieee->beacon_timer);
+	//spin_unlock_irqrestore(&ieee->beacon_lock,flags);
+}
+
+
+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);
+}
+
+#ifdef _RTL8187_EXT_PATCH_
+
+inline struct sk_buff *ieee80211_probe_req_with_SSID(struct ieee80211_device *ieee, char *ssid, int len_ssid)
+{
+	unsigned int len,rate_len;
+	u8 *tag;
+	struct sk_buff *skb;
+	struct ieee80211_probe_request *req;
+
+#ifdef _RTL8187_EXT_PATCH_
+	short extMore = 0;
+	if(ieee->ext_patch_ieee80211_probe_req_1)
+		extMore = ieee->ext_patch_ieee80211_probe_req_1(ieee);
+#endif
+
+	len = len_ssid;
+
+	rate_len = ieee80211_MFIE_rate_len(ieee);
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(!extMore)
+#endif
+	skb = dev_alloc_skb(sizeof(struct ieee80211_probe_request) +
+			    2 + len + rate_len);
+#ifdef _RTL8187_EXT_PATCH_
+	else
+		skb = dev_alloc_skb(sizeof(struct ieee80211_probe_request) +
+			    2 + len + rate_len+128); // MESHID + CAP
+#endif
+
+	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;
+	if(len)
+	{
+		memcpy(tag, ssid, len);
+		tag += len;
+	}
+
+	ieee80211_MFIE_Brate(ieee,&tag);
+	ieee80211_MFIE_Grate(ieee,&tag);
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(extMore)
+		ieee->ext_patch_ieee80211_probe_req_2(ieee, skb, tag);
+#endif
+	return skb;
+}
+
+#endif // _RTL8187_EXT_PATCH_
+
+
+void ieee80211_send_probe(struct ieee80211_device *ieee)
+{
+	struct sk_buff *skb;
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->iw_mode == ieee->iw_ext_mode)
+		skb = ieee80211_probe_req_with_SSID(ieee, NULL, 0);
+	else
+#endif
+	skb = ieee80211_probe_req(ieee);
+	if (skb){
+		softmac_mgmt_xmit(skb, ieee);
+		ieee->softmac_stats.tx_probe_rq++;
+		//dev_kfree_skb_any(skb);//edit by thomas
+	}
+}
+
+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.
+ */
+void ieee80211_softmac_scan_syncro(struct ieee80211_device *ieee)
+{
+	short ch = 0;
+#ifdef ENABLE_DOT11D
+	u8 channel_map[MAX_CHANNEL_NUMBER+1];
+	memcpy(channel_map, GET_DOT11D_INFO(ieee)->channel_map, MAX_CHANNEL_NUMBER+1);
+#endif
+	down(&ieee->scan_sem);
+//	printk("==================> Sync scan\n");
+//	dump_chnl_map(channel_map);
+
+	while(1)
+	{
+
+		do{
+			ch++;
+			if (ch > MAX_CHANNEL_NUMBER)
+				goto out; /* scan completed */
+
+#ifdef ENABLE_DOT11D
+		}while(!channel_map[ch]);
+#else
+		}while(!ieee->channel_map[ch]);
+#endif
+		/* this fuction 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 temporarly '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);
+//		printk("=====>channel=%d   ",ch);
+#ifdef ENABLE_DOT11D
+		if(channel_map[ch] == 1)
+#endif
+		{
+//			printk("====send probe request\n");
+			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);
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(ieee))
+		DOT11D_ScanComplete(ieee);
+#endif
+}
+
+void ieee80211_softmac_ips_scan_syncro(struct ieee80211_device *ieee)
+{
+	int ch;
+        unsigned int watch_dog = 0;
+#ifdef ENABLE_DOT11D
+	u8 channel_map[MAX_CHANNEL_NUMBER+1];
+	memcpy(channel_map, GET_DOT11D_INFO(ieee)->channel_map, MAX_CHANNEL_NUMBER+1);
+#endif
+        down(&ieee->scan_sem);
+	ch = ieee->current_network.channel;
+//      	if(ieee->sync_scan_hurryup)
+//	{
+
+//		printk("stop scan sync\n");
+//   		goto out;
+//  	}
+//	printk("=======hh===============>ips scan\n");
+     	while(1)
+        {
+                /* this fuction 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 temporarly '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;
+		}
+#ifdef ENABLE_DOT11D
+		if(channel_map[ieee->current_network.channel] > 0)
+#endif
+		{
+			ieee->set_chan(ieee->dev, ieee->current_network.channel);
+//			printk("======>channel=%d  ",ieee->current_network.channel);
+		}
+#ifdef ENABLE_DOT11D
+		if(channel_map[ieee->current_network.channel] == 1)
+#endif
+		{
+//			printk("====send probe request\n");
+			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);
+
+		do{
+			if (watch_dog++ >= MAX_CHANNEL_NUMBER)
+		//	if (++watch_dog >= 15);//MAX_CHANNEL_NUMBER)  //YJ,modified,080630
+				goto out; /* scan completed */
+
+			ieee->current_network.channel = (ieee->current_network.channel + 1)%MAX_CHANNEL_NUMBER;
+#ifdef ENABLE_DOT11D
+		}while(!channel_map[ieee->current_network.channel]);
+#else
+		}while(!ieee->channel_map[ieee->current_network.channel]);
+#endif
+        }
+out:
+	//ieee->sync_scan_hurryup = 0;
+   	//ieee->set_chan(ieee->dev, ch);
+   	//ieee->current_network.channel = ch;
+	ieee->actscanning = false;
+	up(&ieee->scan_sem);
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(ieee))
+		DOT11D_ScanComplete(ieee);
+#endif
+}
+
+
+#if 0
+/* called both by wq with ieee->lock held */
+void ieee80211_softmac_scan(struct ieee80211_device *ieee)
+{
+	short watchdog = 0;
+
+	do{
+		ieee->current_network.channel =
+			(ieee->current_network.channel + 1) % MAX_CHANNEL_NUMBER;
+		if (watchdog++ > MAX_CHANNEL_NUMBER)
+				return; /* no good chans */
+
+	}while(!ieee->channel_map[ieee->current_network.channel]);
+
+
+	schedule_work(&ieee->softmac_scan_wq);
+}
+#endif
+#ifdef ENABLE_IPS
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void ieee80211_softmac_scan_wq(struct work_struct *work)
+{
+	struct delayed_work *dwork = container_of(work, struct delayed_work, work);
+	struct ieee80211_device *ieee = container_of(dwork, struct ieee80211_device, softmac_scan_wq);
+#else
+void ieee80211_softmac_scan_wq(struct ieee80211_device *ieee)
+{
+#endif
+	static short watchdog = 0;
+#ifdef ENABLE_DOT11D
+	u8 channel_map[MAX_CHANNEL_NUMBER+1];
+	memcpy(channel_map, GET_DOT11D_INFO(ieee)->channel_map, MAX_CHANNEL_NUMBER+1);
+#endif
+//	printk("ieee80211_softmac_scan_wq ENABLE_IPS\n");
+//	printk("in %s\n",__func__);
+	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 */
+
+#ifdef ENABLE_DOT11D
+ 	}while(!channel_map[ieee->current_network.channel]);
+#else
+ 	}while(!ieee->channel_map[ieee->current_network.channel]);
+#endif
+
+	//printk("current_network.channel:%d\n", 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);
+#ifdef ENABLE_DOT11D
+	if(channel_map[ieee->current_network.channel] == 1)
+#endif
+		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);
+
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(ieee))
+		DOT11D_ScanComplete(ieee);
+#endif
+	return;
+}
+#else
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void ieee80211_softmac_scan_wq(struct work_struct *work)
+{
+        struct delayed_work *dwork = container_of(work, struct delayed_work, work);
+        struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, softmac_scan_wq);
+#else
+void ieee80211_softmac_scan_wq(struct ieee80211_device *ieee)
+{
+#endif
+
+        short watchdog = 0;
+#ifdef ENABLE_DOT11D
+	u8 channel_map[MAX_CHANNEL_NUMBER+1];
+	memcpy(channel_map, GET_DOT11D_INFO(ieee)->channel_map, MAX_CHANNEL_NUMBER+1);
+#endif
+//      printk("enter scan wq,watchdog is %d\n",watchdog);
+        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 */
+
+#ifdef ENABLE_DOT11D
+        }while(!channel_map[ieee->current_network.channel]);
+#else
+        }while(!ieee->channel_map[ieee->current_network.channel]);
+#endif
+
+//      printk("current_network.channel:%d\n", 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);
+#ifdef ENABLE_DOT11D
+	if(channel_map[ieee->current_network.channel] == 1)
+#endif
+		ieee80211_send_probe_requests(ieee);
+
+	queue_delayed_work(ieee->wq, &ieee->softmac_scan_wq, IEEE80211_SOFTMAC_SCAN_TIME);
+out:
+	up(&ieee->scan_sem);
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(ieee))
+		DOT11D_ScanComplete(ieee);
+#endif
+}
+
+#endif
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+void ieee80211_softmac_scan_cb(unsigned long _dev)
+{
+	unsigned long flags;
+	struct ieee80211_device *ieee = (struct ieee80211_device *)_dev;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+	ieee80211_softmac_scan(ieee);
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+#endif
+
+
+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);
+}
+
+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);
+}
+
+
+void ieee80211_softmac_stop_scan(struct ieee80211_device *ieee)
+{
+//	unsigned long flags;
+
+	//ieee->sync_scan_hurryup = 1;
+
+	down(&ieee->scan_sem);
+//	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (ieee->scanning == 1){
+		ieee->scanning = 0;
+		//del_timer_sync(&ieee->scan_timer);
+		cancel_delayed_work(&ieee->softmac_scan_wq);
+	}
+
+//	spin_unlock_irqrestore(&ieee->lock, flags);
+	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_start_scan(struct ieee80211_device *ieee)
+{
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(ieee) )
+	{
+		if(IS_COUNTRY_IE_VALID(ieee))
+		{
+			RESET_CIE_WATCHDOG(ieee);
+		}
+	}
+#endif
+	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN){
+		if (ieee->scanning == 0)
+		{
+			ieee->scanning = 1;
+			//ieee80211_softmac_scan(ieee);
+		//	queue_work(ieee->wq, &ieee->softmac_scan_wq);
+		//care this,1203,2007,by lawrence
+#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)
+{
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(ieee) )
+	{
+		if(IS_COUNTRY_IE_VALID(ieee))
+		{
+			RESET_CIE_WATCHDOG(ieee);
+		}
+	}
+#endif
+	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))
+		cpu_to_le16((beacon_buf->capability |= 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. WB 2008.07.07
+			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;
+}
+#ifdef _RTL8187_EXT_PATCH_
+struct sk_buff* ieee80211_ext_probe_resp_by_net(struct ieee80211_device *ieee, u8 *dest, struct ieee80211_network *net)
+{
+	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;
+	u8 broadcast_addr[] = {0xff,0xff,0xff,0xff,0xff,0xff};
+	int wpa_ie_len = ieee->wpa_ie_len;
+	char *ssid = net->ssid;
+	int ssid_len = net->ssid_len;
+
+	int rate_len = ieee->current_network.rates_len+2;
+	int rate_ex_len = ieee->current_network.rates_ex_len;
+	if(rate_ex_len > 0) rate_ex_len+=2;
+
+	if( ieee->meshScanMode&4)
+		ieee->current_network.channel = ieee->ext_patch_ieee80211_ext_stop_scan_wq_set_channel(ieee);
+	if( ieee->meshScanMode&6)
+	{
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0)
+		queue_work(ieee->wq, &ieee->ext_stop_scan_wq);
+#else
+		schedule_task(&ieee->ext_stop_scan_wq);
+#endif
+	}
+	if(ieee->current_network.capability & WLAN_CAPABILITY_IBSS) // use current_network here
+		atim_len = 4;
+	else
+		atim_len = 0;
+
+	if(ieee80211_is_54g(*net))
+		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
+		+erp_len;
+//b
+	skb = dev_alloc_skb(beacon_size+196);
+
+	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);  // use current_network here
+	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))
+		cpu_to_le16((beacon_buf->capability |= 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;
+
+	// brocad cast / probe rsp
+	if(memcmp(dest, broadcast_addr, ETH_ALEN ))
+		memcpy(tag, ssid, ssid_len);
+	else
+		ssid_len=0;
+
+	tag += ssid_len;
+
+//get_bssrate_set(priv, _SUPPORTEDRATES_IE_, &pbssrate, &bssrate_len);
+//pbuf = set_ie(pbuf, _SUPPORTEDRATES_IE_, bssrate_len, pbssrate, &frlen);
+
+	*(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;  // use current_network here
+
+
+	if(atim_len){
+		*(tag++) = MFIE_TYPE_IBSS_SET;
+		*(tag++) = 2;
+		*((u16*)(tag)) = cpu_to_le16(ieee->current_network.atim_window); // use current_network here
+		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)
+		memcpy(tag, ieee->wpa_ie, ieee->wpa_ie_len);
+
+	skb->dev = ieee->dev;
+	return skb;
+}
+#endif // _RTL8187_EXT_PATCH_
+
+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_ctl = 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;
+}
+
+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);
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->iw_mode == ieee->iw_ext_mode)
+		memcpy(auth->header.addr3, dest, ETH_ALEN);
+#else
+	memcpy(auth->header.addr3, ieee->dev->dev_addr, ETH_ALEN);
+#endif
+	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;
+
+
+}
+
+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_ctl = cpu_to_le16(IEEE80211_FTYPE_DATA |
+		IEEE80211_STYPE_NULLFUNC | IEEE80211_FCTL_TODS |
+		(pwr ? IEEE80211_FCTL_PM:0));
+
+	return skb;
+
+
+}
+
+
+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);//edit by thomas
+	}
+}
+
+
+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);//edit by thomas
+	}
+}
+
+
+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);//edit by thomas
+	}
+}
+
+
+inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beacon,struct ieee80211_device *ieee)
+{
+	struct sk_buff *skb;
+	//unsigned long flags;
+
+	struct ieee80211_assoc_request_frame *hdr;
+	u8 *tag;
+	//short info_addr = 0;
+	//int i;
+	//u16 suite_count = 0;
+	//u8 suit_select = 0;
+	unsigned int wpa_len = beacon->wpa_ie_len;
+	//struct net_device *dev = ieee->dev;
+	//union iwreq_data wrqu;
+	//u8 *buff;
+	//u8 *p;
+#if 1
+	// for testing purpose
+	unsigned int rsn_len = beacon->rsn_ie_len;
+#else
+	unsigned int rsn_len = beacon->rsn_ie_len - 4;
+#endif
+	unsigned int rate_len = ieee80211_MFIE_rate_len(ieee);
+	unsigned int wmm_info_len = beacon->QoS_Enable?9:0;
+#ifdef THOMAS_TURBO
+	unsigned int turbo_info_len = beacon->Turbo_Enable?9:0;
+#endif
+
+	u8  encry_proto = ieee->wpax_type_notify & 0xff;
+	//u8  pairwise_type = (ieee->wpax_type_notify >> 8) & 0xff;
+	//u8  authen_type = (ieee->wpax_type_notify >> 16) & 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;
+		}
+	}
+#ifdef THOMAS_TURBO
+	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;
+#else
+	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;
+#endif
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->iw_mode == ieee->iw_ext_mode)
+		skb = dev_alloc_skb(len+256); // stanley
+	else
+#endif
+	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_ctl = 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, John
+
+	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);
+
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_association_req_1)
+		ieee->ext_patch_ieee80211_association_req_1(hdr);
+#endif
+
+	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), john2007.8.9
+	//choose AES encryption as default algorithm while using mixed mode
+#if 0
+	if(rsn_len == 0){
+
+		tag = skb_put(skb,wpa_len);
+
+		if(wpa_len) {
+
+
+		  //{add by david. 2006.8.31
+		  //fix linksys compatibility bug
+		  //}
+		  if(wpa_len > 24) {//22+2, mean include the capability
+			beacon->wpa_ie[wpa_len - 2] = 0;
+		  }
+		//multicast cipher OUI
+                  if(  beacon->wpa_ie[11]==0x2      ){ //0x0050f202 is the oui of tkip
+                  ieee->broadcast_key_type = KEY_TYPE_TKIP;
+                }
+                  else if(  beacon->wpa_ie[11]==0x4      ){//0x0050f204 is the oui of ccmp
+                  ieee->broadcast_key_type = KEY_TYPE_CCMP;
+                }
+ 		//unicast cipher OUI
+		  if(	beacon->wpa_ie[14]==0
+			&& beacon->wpa_ie[15]==0x50
+                        && beacon->wpa_ie[16]==0xf2
+                        && beacon->wpa_ie[17]==0x2  	){ //0x0050f202 is the oui of tkip
+                  ieee->pairwise_key_type = KEY_TYPE_TKIP;
+		}
+
+                  else if(   beacon->wpa_ie[14]==0
+                        && beacon->wpa_ie[15]==0x50
+                        && beacon->wpa_ie[16]==0xf2
+                        && beacon->wpa_ie[17]==0x4      ){//0x0050f204 is the oui of ccmp
+                  ieee->pairwise_key_type = KEY_TYPE_CCMP;
+		}
+		//indicate the wpa_ie content to WPA_SUPPLICANT
+		buff = kmalloc(IW_CUSTOM_MAX, GFP_ATOMIC);
+		memset(buff, 0, IW_CUSTOM_MAX);
+		p=buff;
+		p += sprintf(p, "ASSOCINFO(ReqIEs=");
+		for(i=0;i<wpa_len;i++){
+			p += sprintf(p, "%02x", beacon->wpa_ie[i]);
+		}
+		p += sprintf(p, ")");
+		memset(&wrqu, 0, sizeof(wrqu) );
+		wrqu.data.length = p - buff;
+
+		wireless_send_event(dev, IWEVCUSTOM, &wrqu, buff);
+		  memcpy(tag,beacon->wpa_ie,wpa_len);
+		}
+
+	}
+
+	if(rsn_len > 22) {
+
+	  					if(     beacon->rsn_ie[4]==0x0 &&
+                                beacon->rsn_ie[5]==0xf &&
+                                beacon->rsn_ie[6]==0xac){
+
+                                switch(beacon->rsn_ie[7]){
+                                        case 0x1:
+                                                ieee->broadcast_key_type = KEY_TYPE_WEP40;
+                                                break;
+                                        case 0x2:
+                                                ieee->broadcast_key_type = KEY_TYPE_TKIP;
+                                                break;
+                                        case 0x4:
+                                                ieee->broadcast_key_type = KEY_TYPE_CCMP;
+                                                break;
+                                        case 0x5:
+                                                ieee->broadcast_key_type = KEY_TYPE_WEP104;
+                                                break;
+                                        default:
+                                                printk("fault suite type in RSN broadcast key\n");
+                                                break;
+                                }
+                        }
+
+                        if(     beacon->rsn_ie[10]==0x0 &&
+                                beacon->rsn_ie[11]==0xf &&
+                                beacon->rsn_ie[12]==0xac){
+				if(beacon->rsn_ie[8]==1){//not mixed mode
+	                                switch(beacon->rsn_ie[13]){
+        	                                case 0x2:
+                	                                ieee->pairwise_key_type = KEY_TYPE_TKIP;
+                        	                        break;
+                                	        case 0x4:
+                                        	        ieee->pairwise_key_type = KEY_TYPE_CCMP;
+                                                	break;
+        	                                default:
+	                                                printk("fault suite type in RSN pairwise key\n");
+                	                                break;
+                                	}
+				}
+				else if(beacon->rsn_ie[8]==2){//mixed mode
+					ieee->pairwise_key_type = KEY_TYPE_CCMP;
+				}
+                        }
+
+
+
+		tag = skb_put(skb,22);
+		memcpy(tag,(beacon->rsn_ie + info_addr),8);
+		tag[1] =  20;
+		tag += 8;
+		info_addr += 8;
+
+		spin_lock_irqsave(&ieee->wpax_suitlist_lock,flags);
+		for (i = 0; i < 2; i++) {
+			tag[0] = 1;
+			tag[1] = 0;
+			tag += 2;
+			suite_count = beacon->rsn_ie[info_addr] + \
+				      (beacon->rsn_ie[info_addr + 1] << 8);
+			info_addr += 2;
+			if(1 == suite_count) {
+				memcpy(tag,(beacon->rsn_ie + info_addr),4);
+				info_addr += 4;
+			} else {
+				// if the wpax_type_notify has been set by the application,
+				// just use it, otherwise just use the default one.
+				if(ieee->wpax_type_set) {
+					suit_select = ((0 == i) ? pairwise_type:authen_type)&0x0f ;
+					memcpy(tag,rsn_authen_cipher_suite[suit_select],4);
+				} else {
+					//default set as ccmp, or none authentication
+					if(i == 0) {
+						memcpy(tag,rsn_authen_cipher_suite[4],4);
+					} else {
+						memcpy(tag,rsn_authen_cipher_suite[2],4);
+					}
+
+				}
+
+				info_addr += (suite_count * 4);
+			}
+			tag += 4;
+		}
+		spin_unlock_irqrestore(&ieee->wpax_suitlist_lock,flags);
+
+		tag[0] = 0;
+		tag[1] = beacon->rsn_ie[info_addr+1];
+
+	} else {
+		tag = skb_put(skb,rsn_len);
+		if(rsn_len) {
+
+
+			if( 	beacon->rsn_ie[4]==0x0 &&
+				beacon->rsn_ie[5]==0xf &&
+				beacon->rsn_ie[6]==0xac){
+				switch(beacon->rsn_ie[7]){
+					case 0x1:
+						ieee->broadcast_key_type = KEY_TYPE_WEP40;
+                                                break;
+					case 0x2:
+						ieee->broadcast_key_type = KEY_TYPE_TKIP;
+						break;
+					case 0x4:
+   	                                        ieee->broadcast_key_type = KEY_TYPE_CCMP;
+                                                break;
+                                        case 0x5:
+                                                ieee->broadcast_key_type = KEY_TYPE_WEP104;
+                                                break;
+					default:
+						printk("fault suite type in RSN broadcast key\n");
+						break;
+				}
+			}
+                        if(     beacon->rsn_ie[10]==0x0 &&
+                                beacon->rsn_ie[11]==0xf &&
+                                beacon->rsn_ie[12]==0xac){
+                                if(beacon->rsn_ie[8]==1){//not mixed mode
+                                        switch(beacon->rsn_ie[13]){
+                                                case 0x2:
+                                                        ieee->pairwise_key_type = KEY_TYPE_TKIP;
+                                                        break;
+                                                case 0x4:
+                                                        ieee->pairwise_key_type = KEY_TYPE_CCMP;
+                                                        break;
+                                                default:
+                                                        printk("fault suite type in RSN pairwise key\n");
+                                                        break;
+                                	}
+
+				}
+                                else if(beacon->rsn_ie[8]==2){//mixed mode
+                                        ieee->pairwise_key_type = KEY_TYPE_CCMP;
+                                }
+                        }
+
+
+			beacon->rsn_ie[rsn_len - 2] = 0;
+			memcpy(tag,beacon->rsn_ie,rsn_len);
+		}
+	}
+#else
+	tag = skb_put(skb,ieee->wpa_ie_len);
+	memcpy(tag,ieee->wpa_ie,ieee->wpa_ie_len);
+#endif
+	tag = skb_put(skb,wmm_info_len);
+	if(wmm_info_len) {
+	  ieee80211_WMM_Info(ieee, &tag);
+	}
+#ifdef THOMAS_TURBO
+	tag = skb_put(skb,turbo_info_len);
+        if(turbo_info_len) {
+                ieee80211_TURBO_Info(ieee, &tag);
+        }
+#endif
+
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_association_req_2)
+		ieee->ext_patch_ieee80211_association_req_2(ieee, beacon, skb);
+#endif
+
+	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 possibily
+	 * 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);
+}
+
+void ieee80211_associate_abort_cb(unsigned long dev)
+{
+	ieee80211_associate_abort((struct ieee80211_device *) dev);
+}
+
+
+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);
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->iw_mode == ieee->iw_ext_mode )	{
+		if(skb)
+			softmac_mgmt_xmit(skb, ieee);
+		return;
+	}else
+#endif
+	if (!skb){
+
+		ieee80211_associate_abort(ieee);
+	}
+	else{
+		ieee->state = IEEE80211_ASSOCIATING_AUTHENTICATING ;
+		IEEE80211_DEBUG_MGMT("Sending authentication request\n");
+		//printk("---Sending authentication request\n");
+		softmac_mgmt_xmit(skb, ieee);
+		//BUGON when you try to add_timer twice, using mod_timer may be better, john0709
+		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)
+		//So ... 1204 by lawrence.
+		//printk("\nIn %s,line %d call kfree skb.",__func__,__LINE__);
+		//dev_kfree_skb_any(skb);//edit by thomas
+	}
+}
+
+void ieee80211_auth_challenge(struct ieee80211_device *ieee, u8 *challenge, int chlen)
+{
+	u8 *c;
+	struct sk_buff *skb;
+	struct ieee80211_network *beacon = &ieee->current_network;
+//	int hlen = sizeof(struct ieee80211_authentication);
+	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)){
+		//printk("=========>add timer again, to crash\n");
+		ieee->associate_timer.expires = jiffies + (HZ / 2);
+		add_timer(&ieee->associate_timer);
+		}
+		dev_kfree_skb_any(skb);//edit by thomas
+	}
+	kfree(challenge);
+}
+
+#ifdef _RTL8187_EXT_PATCH_
+
+// based on ieee80211_assoc_resp
+struct sk_buff* ieee80211_assoc_resp_by_net(struct ieee80211_device *ieee, u8 *dest, unsigned short status, struct ieee80211_network *pstat, int pkt_type)
+{
+	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;
+
+	if(ieee->iw_mode == ieee->iw_ext_mode)
+		skb = dev_alloc_skb(len+256); // stanley
+	else
+		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_ctl = cpu_to_le16(pkt_type);
+
+	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->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_assoc_resp_by_net_1)
+		ieee->ext_patch_ieee80211_assoc_resp_by_net_1(assoc);
+
+	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++;
+
+	assoc->info_element.id = 230; // Stanley, an unused id (just a hot fix)
+	assoc->info_element.len = 0;
+
+	tag = (u8*) skb_put(skb, rate_len);
+
+	ieee80211_MFIE_Brate(ieee, &tag);
+	ieee80211_MFIE_Grate(ieee, &tag);
+
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_assoc_resp_by_net_2)
+		ieee->ext_patch_ieee80211_assoc_resp_by_net_2(ieee, pstat, pkt_type, skb);
+
+	return skb;
+}
+
+// based on ieee80211_resp_to_assoc_rq
+void ieee80211_ext_issue_assoc_rsp(struct ieee80211_device *ieee, u8 *dest, unsigned short status, struct ieee80211_network *pstat, int pkt_type)
+{
+	struct sk_buff *buf = ieee80211_assoc_resp_by_net(ieee, dest, status, pstat, pkt_type);
+
+	if (buf)
+		softmac_mgmt_xmit(buf, ieee);
+}
+
+// based on ieee80211_associate_step2
+void ieee80211_ext_issue_assoc_req(struct ieee80211_device *ieee, struct ieee80211_network *pstat)
+{
+
+	struct sk_buff* skb;
+
+	// printk("@@@@@ ieee80211_ext_issue_assoc_req on channel: %d\n", ieee->current_network.channel);
+
+	ieee->softmac_stats.tx_ass_rq++;
+	skb=ieee80211_association_req(pstat, ieee);
+	if (skb)
+		softmac_mgmt_xmit(skb, ieee);
+}
+
+void ieee80211_ext_issue_disassoc(struct ieee80211_device *ieee, struct ieee80211_network *pstat, int reason, unsigned char extReason)
+{
+	// do nothing
+	// printk("@@@@@ ieee80211_ext_issue_disassoc\n");
+	return;
+}
+#endif // _RTL8187_EXT_PATCH_
+
+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);
+		}
+		//dev_kfree_skb_any(skb);//edit by thomas
+	}
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void ieee80211_associate_complete_wq(struct work_struct *work)
+{
+	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, associate_complete_wq);
+#else
+void ieee80211_associate_complete_wq(struct ieee80211_device *ieee)
+{
+#endif
+	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);
+}
+
+void ieee80211_associate_complete(struct ieee80211_device *ieee)
+{
+	int i;
+	del_timer_sync(&ieee->associate_timer);
+
+	for(i = 0; i < 6; i++) {
+	  //ieee->seq_ctrl[i] = 0;
+	}
+	ieee->state = IEEE80211_LINKED;
+	IEEE80211_DEBUG_MGMT("Successfully associated\n");
+
+	queue_work(ieee->wq, &ieee->associate_complete_wq);
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void ieee80211_associate_procedure_wq(struct work_struct *work)
+{
+	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, associate_procedure_wq);
+#else
+void ieee80211_associate_procedure_wq(struct ieee80211_device *ieee)
+{
+#endif
+	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);
+}
+#ifdef _RTL8187_EXT_PATCH_
+// based on ieee80211_associate_procedure_wq
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
+void ieee80211_ext_stop_scan_wq(struct work_struct *work)
+{
+	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, ext_stop_scan_wq);
+#else
+void ieee80211_ext_stop_scan_wq(struct ieee80211_device *ieee)
+{
+#endif
+	  if (ieee->scanning == 0)
+	{
+		if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_ext_stop_scan_wq_set_channel
+				&& ( ieee->current_network.channel == ieee->ext_patch_ieee80211_ext_stop_scan_wq_set_channel(ieee) ) )
+		return;
+	}
+
+	ieee->sync_scan_hurryup = 1;
+
+	down(&ieee->wx_sem);
+
+	// printk("@@@@@@@@@@ ieee80211_ext_stop_scan_wq\n");
+	if (ieee->data_hard_stop)
+		ieee->data_hard_stop(ieee->dev);
+
+	ieee80211_stop_scan(ieee);
+
+	// set channel
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_ext_stop_scan_wq_set_channel)
+		ieee->set_chan(ieee->dev, ieee->ext_patch_ieee80211_ext_stop_scan_wq_set_channel(ieee));
+	else
+		ieee->set_chan(ieee->dev, ieee->current_network.channel);
+	//
+	up(&ieee->wx_sem);
+}
+
+
+void ieee80211_ext_send_11s_beacon(struct ieee80211_device *ieee)
+{
+	#if LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0)
+		queue_work(ieee->wq, &ieee->ext_send_beacon_wq);
+	#else
+		schedule_task(&ieee->ext_send_beacon_wq);
+	#endif
+
+}
+
+#endif // _RTL8187_EXT_PATCH_
+
+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;//(memcmp(ieee->current_network.bssid, zero,ETH_ALEN)!=0 );
+		ssidset = ieee->ssid_set;//ieee->current_network.ssid[0] != '\0';
+		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));
+
+		//printk("cur: %s, %d, net:%s, %d\n", ieee->current_network.ssid, ieee->current_network.ssid_len, net->ssid, net->ssid_len);
+		//printk("apset=%d apmatch=%d ssidset=%d ssidbroad=%d ssidmatch=%d\n",apset,apmatch,ssidset,ssidbroad,ssidmatch);
+
+		if (	/* if the user set the AP check if match.
+			 * if the network does not broadcast essid we check the user supplyed 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 bradcast and that those two bssid matches
+				 */
+				(!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 = (u8*)kmalloc(*chlen, GFP_ATOMIC);
+			memcpy(*challenge, t, *chlen);
+		}
+	}
+
+	return cpu_to_le16(a->status);
+
+}
+
+
+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 */
+	}
+
+	//IEEE80211DMESG("Card MAC address is "MACSTR, MAC2STR(src));
+	if (ssidlen == 0) return 1;
+
+	if (!ssid) return 1; /* ssid not found in tagged param */
+	return (!strncmp(ssid, ieee->current_network.ssid, ssidlen));
+
+}
+
+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];
+
+	//IEEE80211DMESG("Rx probe");
+	ieee->softmac_stats.rx_probe_rq++;
+	//DMESG("Dest is "MACSTR, MAC2STR(dest));
+	if (probe_rq_parse(ieee, skb, dest)){
+		//IEEE80211DMESG("Was for me!");
+		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;
+	//IEEE80211DMESG("Rx probe");
+	ieee->softmac_stats.rx_auth_rq++;
+
+	if ((status = auth_rq_parse(skb, dest))!= -1){
+		ieee80211_resp_to_auth(ieee, status, dest);
+	}
+	//DMESG("Dest is "MACSTR, MAC2STR(dest));
+
+}
+
+ inline void
+ieee80211_rx_assoc_rq(struct ieee80211_device *ieee, struct sk_buff *skb)
+{
+
+	u8 dest[ETH_ALEN];
+	//unsigned long flags;
+
+	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: "MAC_FMT"\n", MAC_ARG(dest));
+	//FIXME
+	#if 0
+	spin_lock_irqsave(&ieee->lock,flags);
+	add_associate(ieee,dest);
+	spin_unlock_irqrestore(&ieee->lock,flags);
+	#endif
+}
+
+
+
+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);
+
+}
+
+
+short ieee80211_sta_ps_sleep(struct ieee80211_device *ieee, u32 *time_h, u32 *time_l)
+{
+#if 0
+        int timeout = ieee->ps_timeout;
+#else
+        int timeout = 0;
+#endif
+	u8 dtim;
+	/*if(ieee->ps == IEEE80211_PS_DISABLED ||
+		ieee->iw_mode != IW_MODE_INFRA ||
+		ieee->state != IEEE80211_LINKED)
+
+		return 0;
+	*/
+	dtim = ieee->current_network.dtim_data;
+	//printk("DTIM\n");
+
+	if(!(dtim & IEEE80211_DTIM_VALID))
+		return 0;
+        else
+                timeout = ieee->current_network.beacon_interval;
+
+	//printk("VALID\n");
+	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 0
+	if(time_l){
+		*time_l = ieee->current_network.last_dtim_sta_time[0]
+			+ (ieee->current_network.beacon_interval
+			* ieee->current_network.dtim_period) * 1000;
+	}
+#else
+	if(time_l){
+		*time_l = ieee->current_network.last_dtim_sta_time[0]
+			+ MSECS((ieee->current_network.beacon_interval));
+			//* ieee->current_network.dtim_period));
+			//printk("beacon_interval:%x, dtim_period:%x, totol to Msecs:%x, HZ:%x\n", ieee->current_network.beacon_interval, ieee->current_network.dtim_period, MSECS(((ieee->current_network.beacon_interval * ieee->current_network.dtim_period))), HZ);
+	}
+
+#endif
+	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;
+
+
+}
+
+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);
+//	printk("===>%s,%d[2 wake, 1 sleep, 0 do nothing], ieee->sta_sleep = %d\n",__func__, sleep,ieee->sta_sleep);
+	/* 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){
+	//		printk("send null 1\n");
+			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);
+
+	//	printk("send wakeup packet\n");
+		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){
+		//	printk("Warning: driver is probably failing to report TX ps error\n");
+			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){
+
+		//	printk("==================> %s::enter sleep state\n",__func__);
+			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
+		 */
+	}
+	/* 21112005 - tx again null without PS bit if lost */
+	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_ctl) != IEEE80211_STYPE_PROBE_RESP &&
+		WLAN_FC_GET_STYPE(header->frame_ctl) != IEEE80211_STYPE_BEACON)
+		ieee->last_rx_ps_time = jiffies;
+
+	switch (WLAN_FC_GET_STYPE(header->frame_ctl)) {
+
+		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){
+				if (0 == (errcode=assoc_parse(skb, &aid))){
+					u16 left;
+
+					ieee->state=IEEE80211_LINKED;
+					ieee->assoc_id = aid;
+					ieee->softmac_stats.rx_ass_ok++;
+
+					//printk(KERN_WARNING "nic_type = %s", (rx_stats->nic_type == 1)?"rtl8187":"rtl8187B");
+					if(1 == rx_stats->nic_type) //card type is 8187
+					{
+						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 reeponse 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 paramete 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];
+					}
+					if(!ieee->init_wmmparam_flag) //legacy AP, reset the AC_xx_param register
+					{
+						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);
+				}
+			}
+#ifdef _RTL8187_EXT_PATCH_
+			else if ((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_rx_frame_softmac_on_assoc_rsp)
+			{
+					ieee->ext_patch_ieee80211_rx_frame_softmac_on_assoc_rsp(ieee, skb);
+			}
+#endif
+			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);
+#ifdef _RTL8187_EXT_PATCH_
+			else if ((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_rx_frame_softmac_on_assoc_req)
+			{
+					ieee->ext_patch_ieee80211_rx_frame_softmac_on_assoc_req(ieee, skb);
+			}
+#endif
+			break;
+
+		case IEEE80211_STYPE_AUTH:
+
+#ifdef _RTL8187_EXT_PATCH_
+printk("IEEE80211_STYPE_AUTH\n");
+			if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_rx_frame_softmac_on_auth)
+			if( ieee->ext_patch_ieee80211_rx_frame_softmac_on_auth(ieee, skb, rx_stats) );
+#endif
+			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");
+
+						if (0 == (errcode=auth_parse(skb, &challenge, &chlen))){
+							if(ieee->open_wep || !challenge){
+								ieee->state = IEEE80211_ASSOCIATING_AUTHENTICATED;
+								ieee->softmac_stats.rx_auth_rs_ok++;
+
+								ieee80211_associate_step2(ieee);
+							}else{
+								ieee80211_auth_challenge(ieee, challenge, chlen);
+							}
+						}else{
+							ieee->softmac_stats.rx_auth_rs_err++;
+							IEEE80211_DEBUG_MGMT("Authentication respose 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:
+#ifdef _RTL8187_EXT_PATCH_
+printk("IEEE80211_STYPE_DEAUTH\n");
+		if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_rx_frame_softmac_on_deauth)
+		if( ieee->ext_patch_ieee80211_rx_frame_softmac_on_deauth(ieee, skb, rx_stats) )	;
+#endif
+			/* 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++;
+
+				//notify_wx_assoc_event(ieee);  //YJ,del,080828, do not notify os here
+				queue_work(ieee->wq, &ieee->associate_procedure_wq);
+			}
+
+			break;
+
+		default:
+			return -1;
+			break;
+	}
+
+	//dev_kfree_skb_any(skb);
+	return 0;
+}
+
+
+
+/* following are for a simplier 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 enought 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 treshold is not enought.. 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;
+#ifdef _RTL8187_EXT_PATCH_
+	int rate = ieee->rate;
+#endif
+
+	spin_lock_irqsave(&ieee->lock,flags);
+	#if 0
+	if(ieee->queue_stop){
+		IEEE80211DMESG("EE: IEEE hard_start_xmit invoked when kernel queue should be stopped");
+		netif_stop_queue(ieee->dev);
+		ieee->ieee_stats.swtxstop++;
+		//dev_kfree_skb_any(skb);
+		err = 1;
+		goto exit;
+	}
+
+	ieee->stats.tx_bytes+=skb->len;
+
+
+	txb=ieee80211_skb_to_txb(ieee,skb);
+
+
+	if(txb==NULL){
+		IEEE80211DMESG("WW: IEEE stack failed to provide txb");
+		//dev_kfree_skb_any(skb);
+		err = 1;
+		goto exit;
+	}
+	#endif
+
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_softmac_xmit_get_rate && txb->nr_frags)
+	{
+		rate = ieee->ext_patch_ieee80211_softmac_xmit_get_rate(ieee, txb->fragments[0]);
+	}
+#endif
+	/* 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],
+#ifdef _RTL8187_EXT_PATCH_
+				ieee->dev, rate);
+#else
+				ieee->dev,ieee->rate);
+#endif
+				//(i+1)<txb->nr_frags);
+			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 */
+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);
+				//(i+1)<ieee->tx_pending.txb->nr_frags);
+			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_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_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+			if (ieee->seq_ctrl[0] == 0xFFF)
+				ieee->seq_ctrl[0] = 0;
+			else
+				ieee->seq_ctrl[0]++;
+
+			//printk(KERN_ALERT "ieee80211_wake_queue \n");
+			ieee->softmac_data_hard_start_xmit(skb,ieee->dev,ieee->basic_rate);
+			dev_kfree_skb_any(skb);//edit by thomas
+		}
+	}
+	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_stop_queue(struct ieee80211_device *ieee)
+{
+	//unsigned long flags;
+	//spin_lock_irqsave(&ieee->lock,flags);
+
+	if (! netif_queue_stopped(ieee->dev)){
+		netif_stop_queue(ieee->dev);
+		ieee->softmac_stats.swtxstop++;
+	}
+	ieee->queue_stop = 1;
+	//spin_unlock_irqrestore(&ieee->lock,flags);
+
+}
+
+
+inline void ieee80211_randomize_cell(struct ieee80211_device *ieee)
+{
+
+	get_random_bytes(ieee->current_network.bssid, ETH_ALEN);
+
+	/* an IBSS cell address must have the two less significant
+	 * bits of the first byte = 2
+	 */
+	ieee->current_network.bssid[0] &= ~0x01;
+	ieee->current_network.bssid[0] |= 0x02;
+}
+
+/* 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);
+}
+
+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);
+	}
+}
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void ieee80211_start_ibss_wq(struct work_struct *work)
+{
+	struct delayed_work *dwork = container_of(work, struct delayed_work, work);
+	struct ieee80211_device *ieee = container_of(dwork, struct ieee80211_device, start_ibss_wq);
+#else
+void ieee80211_start_ibss_wq(struct ieee80211_device *ieee)
+{
+#endif
+
+	/* 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);
+
+#ifdef ENABLE_DOT11D
+	if(ieee->state == IEEE80211_NOLINK)
+		ieee->current_network.channel = 10;
+#endif
+	/* if not then the state is not linked. Maybe the user swithced 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 tryed to associate when
+	 * we had just checked the ieee->state and we was going to start the
+	 * scan) beacause 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;
+#ifdef ENABLE_DOT11D
+	//
+	// 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;
+		}
+	}
+#endif
+	/* 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);
+
+//#ifdef ENABLE_IPS
+//	printk("start bss ENABLE_IPS\n");
+//#else
+	if (ieee->state == IEEE80211_NOLINK){
+		ieee->actscanning = true;
+		ieee80211_start_scan(ieee);
+	}
+//#endif
+	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);
+
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(ieee))
+		Dot11d_Reset(ieee);
+#endif
+	ieee->state = IEEE80211_NOLINK;
+	ieee->link_change(ieee->dev);
+	notify_wx_assoc_event(ieee);
+
+}
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void ieee80211_associate_retry_wq(struct work_struct *work)
+{
+	struct delayed_work *dwork = container_of(work, struct delayed_work, work);
+	struct ieee80211_device *ieee = container_of(dwork, struct ieee80211_device, associate_retry_wq);
+#else
+void ieee80211_associate_retry_wq(struct ieee80211_device *ieee)
+{
+#endif
+	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 procdure (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_start_scan(ieee);
+	}
+	//YJ,add,080828, notify os here
+	if(ieee->state == IEEE80211_NOLINK)
+	{
+		notify_wx_assoc_event(ieee);
+	}
+	//YJ,add,080828,end
+	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;
+
+//rz
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_get_beacon_get_probersp )
+		skb = ieee->ext_patch_get_beacon_get_probersp(ieee, broadcast_addr, &(ieee->current_network));
+	else
+		skb = ieee80211_probe_resp(ieee, broadcast_addr);
+#else
+	skb = ieee80211_probe_resp(ieee, broadcast_addr);
+#endif
+//
+	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;
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->ext_patch_ieee80211_stop_protocol)
+		ieee->ext_patch_ieee80211_stop_protocol(ieee);
+//if call queue_delayed_work,can call this,or do nothing..
+//edit by lawrence,20071118
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0)
+//	cancel_delayed_work(&ieee->ext_stop_scan_wq);
+//	cancel_delayed_work(&ieee->ext_send_beacon_wq);
+#endif
+#endif // _RTL8187_EXT_PATCH_
+
+	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 */
+
+#ifdef ENABLE_DOT11D
+		}while(!GET_DOT11D_INFO(ieee)->channel_map[ch]);
+#else
+		}while(!ieee->channel_map[ch]);
+#endif
+
+		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:
+#ifdef _RTL8187_EXT_PATCH_
+			if((ieee->iw_mode == ieee->iw_ext_mode) &&\
+			    ieee->ext_patch_ieee80211_start_protocol &&\
+                            ieee->ext_patch_ieee80211_start_protocol(ieee)) {
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0)
+				queue_work(ieee->wq, &ieee->ext_stop_scan_wq);
+#endif
+				// By default, WMM function will be disabled in
+				// EXTENSION mode
+				ieee->current_network.QoS_Enable = 0;
+
+				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;
+				}
+
+				/*
+				   spin_lock_irqsave(&ieee->lock, flags);
+				   if (ieee->state == IEEE80211_NOLINK)
+				   ieee80211_start_scan(ieee);
+				// ieee->set_chan(ieee->dev, 8);
+
+				spin_unlock_irqrestore(&ieee->lock, flags);
+				*/
+				memcpy(ieee->current_network.bssid, ieee->dev->dev_addr,\
+				      	 ETH_ALEN);
+				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);
+			} else {
+				ieee->iw_mode = IW_MODE_INFRA;
+				ieee80211_start_bss(ieee);
+			}
+#else
+			ieee->iw_mode = IW_MODE_INFRA;
+			ieee80211_start_bss(ieee);
+
+#endif
+			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;
+//#ifdef ENABLE_LPS
+	ieee->ps = IEEE80211_PS_MBCAST|IEEE80211_PS_UNICAST;
+//#else
+//	ieee->ps = IEEE80211_PS_DISABLED;
+//#endif
+	ieee->sta_sleep = 0;
+//by amy
+	ieee->bInactivePs = false;
+	ieee->actscanning = false;
+	ieee->ListenInterval = 2;
+	ieee->NumRxDataInPeriod = 0; //YJ,add,080828
+	ieee->NumRxBcnInPeriod = 0; //YJ,add,080828
+	ieee->NumRxOkTotal = 0;//+by amy 080312
+	ieee->NumRxUnicast = 0;//YJ,add,080828,for keep alive
+	ieee->beinretry = false;
+	ieee->bHwRadioOff = false;
+//by amy
+#ifdef _RTL8187_EXT_PATCH_
+	ieee->iw_ext_mode = 999;
+#endif
+
+	init_mgmt_queue(ieee);
+#if 0
+	init_timer(&ieee->scan_timer);
+	ieee->scan_timer.data = (unsigned long)ieee;
+	ieee->scan_timer.function = ieee80211_softmac_scan_cb;
+#endif
+	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;
+
+#ifdef PF_SYNCTHREAD
+	ieee->wq = create_workqueue(DRV_NAME,0);
+#else
+	ieee->wq = create_workqueue(DRV_NAME);
+#endif
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)//added by lawrence,070702
+	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);
+//	INIT_WORK(&ieee->watch_dog_wq,(void*) ieee80211_watch_dog_wq);
+//added by lawrence,20071118
+#ifdef _RTL8187_EXT_PATCH_
+	INIT_WORK(&ieee->ext_stop_scan_wq,(void*) ieee80211_ext_stop_scan_wq);
+	//INIT_WORK(&ieee->ext_send_beacon_wq,(void*) ieee80211_beacons_start,ieee);
+	INIT_WORK(&ieee->ext_send_beacon_wq,(void*) ext_ieee80211_send_beacon_wq);
+#endif //_RTL8187_EXT_PATCH_
+#else
+	INIT_WORK(&ieee->start_ibss_wq,(void*) ieee80211_start_ibss_wq,ieee);
+	INIT_WORK(&ieee->associate_retry_wq,(void*) ieee80211_associate_retry_wq,ieee);
+	INIT_WORK(&ieee->associate_complete_wq,(void*) ieee80211_associate_complete_wq,ieee);
+	INIT_WORK(&ieee->associate_procedure_wq,(void*) ieee80211_associate_procedure_wq,ieee);
+	INIT_WORK(&ieee->softmac_scan_wq,(void*) ieee80211_softmac_scan_wq,ieee);
+	INIT_WORK(&ieee->wx_sync_scan_wq,(void*) ieee80211_wx_sync_scan_wq,ieee);
+//	INIT_WORK(&ieee->watch_dog_wq,(void*) ieee80211_watch_dog_wq,ieee);
+#ifdef _RTL8187_EXT_PATCH_
+	INIT_WORK(&ieee->ext_stop_scan_wq,(void*) ieee80211_ext_stop_scan_wq,ieee);
+	//INIT_WORK(&ieee->ext_send_beacon_wq,(void*) ieee80211_beacons_start,ieee);
+	INIT_WORK(&ieee->ext_send_beacon_wq,(void*) ext_ieee80211_send_beacon_wq,ieee);
+#endif
+#endif
+	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);
+#ifdef ENABLE_DOT11D
+	ieee->pDot11dInfo = kmalloc(sizeof(RT_DOT11D_INFO), GFP_ATOMIC);
+#endif
+}
+
+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 by lizhaoming 080512
+	cancel_delayed_work(&ieee->GPIOChangeRFWorkItem);
+
+#ifdef _RTL8187_EXT_PATCH_
+	cancel_delayed_work(&ieee->ext_stop_scan_wq);
+	cancel_delayed_work(&ieee->ext_send_beacon_wq);
+#endif
+	destroy_workqueue(ieee->wq);
+#ifdef ENABLE_DOT11D
+	if(NULL != ieee->pDot11dInfo)
+		kfree(ieee->pDot11dInfo);
+#endif
+	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;
+}
+
+
+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 = kmalloc(param->u.wpa_ie.len, GFP_KERNEL);
+		if (buf == NULL)
+			return -ENOMEM;
+
+		memcpy(buf, param->u.wpa_ie.data, param->u.wpa_ie.len);
+		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:
+		// added for WPA2 mixed mode
+		//printk(KERN_WARNING "------------------------>wpax value = %x\n", value);
+		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 (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
+	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
+	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+		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.encrypt = 0;
+			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.encrypt = 1;
+	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) {
+		request_module("ieee80211_crypt_wep");
+		ops = ieee80211_get_crypto_ops(param->u.crypt.alg);
+	} else if (ops == NULL && strcmp(param->u.crypt.alg, "TKIP") == 0) {
+		request_module("ieee80211_crypt_tkip");
+		ops = ieee80211_get_crypto_ops(param->u.crypt.alg);
+	} else if (ops == NULL && strcmp(param->u.crypt.alg, "CCMP") == 0) {
+		request_module("ieee80211_crypt_ccmp");
+		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 = (struct ieee80211_crypt_data *)
+			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 && try_module_get(new_crypt->ops->owner))
+			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);
+	//IEEE_DEBUG_INFO("wpa_supplicant: len=%d\n", p->length);
+
+	if (p->length < sizeof(struct ieee_param) || !p->pointer){
+		ret = -EINVAL;
+		goto out;
+	}
+
+	param = (struct ieee_param *)kmalloc(p->length, GFP_KERNEL);
+	if (param == NULL){
+		ret = -ENOMEM;
+		goto out;
+	}
+	if (copy_from_user(param, p->pointer, p->length)) {
+		kfree(param);
+		ret = -EFAULT;
+		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);
+}
+
+
+#if 0
+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_start_protocol);
+EXPORT_SYMBOL(ieee80211_stop_protocol);
+EXPORT_SYMBOL(notify_wx_assoc_event);
+EXPORT_SYMBOL(ieee80211_stop_send_beacons);
+EXPORT_SYMBOL(SendDisassociation);
+EXPORT_SYMBOL(ieee80211_disassociate);
+EXPORT_SYMBOL(ieee80211_start_scan);
+EXPORT_SYMBOL(ieee80211_softmac_ips_scan_syncro);
+#ifdef _RTL8187_EXT_PATCH_
+EXPORT_SYMBOL(ieee80211_ext_issue_assoc_req);
+EXPORT_SYMBOL(ieee80211_ext_issue_disassoc);
+EXPORT_SYMBOL(ieee80211_ext_issue_assoc_rsp);
+EXPORT_SYMBOL(softmac_mgmt_xmit);
+EXPORT_SYMBOL(ieee80211_ext_probe_resp_by_net);
+EXPORT_SYMBOL(ieee80211_start_scan);
+EXPORT_SYMBOL(ieee80211_stop_scan);
+EXPORT_SYMBOL(ieee80211_ext_send_11s_beacon);
+EXPORT_SYMBOL(ieee80211_rx_auth_rq);
+EXPORT_SYMBOL(ieee80211_associate_step1);
+#endif // _RTL8187_EXT_PATCH_
+EXPORT_SYMBOL(ieee80211_sta_ps_send_null_frame);
+#endif
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c
new file mode 100644
index 000000000000..93af37e2d31a
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c
@@ -0,0 +1,602 @@
+/* IEEE 802.11 SoftMAC layer
+ * Copyright (c) 2005 Andrea Merello <andreamrl@tiscali.it>
+ *
+ * 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 "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;
+	u8 zero[] = {0,0,0,0,0,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 = memcmp(temp->sa_data, zero,ETH_ALEN)!=0;
+	//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;
+}
+
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
+void ieee80211_wx_sync_scan_wq(struct work_struct *work)
+{
+	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, wx_sync_scan_wq);
+#else
+void ieee80211_wx_sync_scan_wq(struct ieee80211_device *ieee)
+{
+#endif
+//void ieee80211_wx_sync_scan_wq(struct ieee80211_device *ieee)
+//{
+	short chan;
+
+	chan = ieee->current_network.channel;
+
+	netif_carrier_off(ieee->dev);
+
+	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);
+
+	netif_carrier_on(ieee->dev);
+
+	//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 consisten infos. not needed otherwise
+	 */
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (wrqu->essid.flags && wrqu->essid.length) {
+//YJ,modified,080819
+#if LINUX_VERSION_CODE <  KERNEL_VERSION(2,6,20)
+		len = ((wrqu->essid.length-1) < IW_ESSID_MAX_SIZE) ? (wrqu->essid.length-1) : IW_ESSID_MAX_SIZE;
+#else
+		len = (wrqu->essid.length < IW_ESSID_MAX_SIZE) ? (wrqu->essid.length) : IW_ESSID_MAX_SIZE;
+#endif
+		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;
+
+	printk(KERN_INFO"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)
+{
+	strcpy(wrqu->name, "802.11");
+	if(ieee->modulation & IEEE80211_CCK_MODULATION){
+		strcat(wrqu->name, "b");
+		if(ieee->modulation & IEEE80211_OFDM_MODULATION)
+			strcat(wrqu->name, "/g");
+	}else if(ieee->modulation & IEEE80211_OFDM_MODULATION)
+		strcat(wrqu->name, "g");
+
+	if((ieee->state == IEEE80211_LINKED) ||
+		(ieee->state == IEEE80211_LINKED_SCANNING))
+		strcat(wrqu->name," linked");
+	else if(ieee->state != IEEE80211_NOLINK)
+		strcat(wrqu->name," link..");
+
+
+	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 is tryied 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;
+
+}
+
+#if 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);
+#endif
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_tx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_tx.c
new file mode 100644
index 000000000000..33a0687252af
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_tx.c
@@ -0,0 +1,828 @@
+/******************************************************************************
+
+  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 <andreamrl@tiscali.it>
+
+  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/version.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 *header;
+
+	if (ieee->tkip_countermeasures &&
+	    crypt && crypt->ops && strcmp(crypt->ops->name, "TKIP") == 0) {
+		header = (struct ieee80211_hdr *) frag->data;
+		if (net_ratelimit()) {
+			printk(KERN_DEBUG "%s: TKIP countermeasures: dropped "
+			       "TX packet to " MAC_FMT "\n",
+			       ieee->dev->name, MAC_ARG(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) {
+		printk(KERN_INFO "%s: Encryption failed: len=%d.\n",
+		       ieee->dev->name, 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);
+}
+
+struct ieee80211_txb *ieee80211_alloc_txb(int nr_frags, int txb_size,
+					  int 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)) {
+    //printk(KERN_WARNING "type = normal packet\n");
+    wme_UP = 7;
+  }
+
+  skb->priority = wme_UP;
+  return(wme_UP);
+}
+
+#ifdef _RTL8187_EXT_PATCH_
+// based on part of ieee80211_xmit. Mainly allocate txb. ieee->lock is held
+struct ieee80211_txb *ieee80211_ext_alloc_txb(struct sk_buff *skb, struct net_device *dev, struct ieee80211_hdr_3addr *header, int hdr_len, u8 isQoS, u16 *pQOS_ctl, int isEncrypt, struct ieee80211_crypt_data* crypt)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0))
+	struct ieee80211_device *ieee = netdev_priv(dev);
+#else
+	struct ieee80211_device *ieee = (struct ieee80211_device *)dev->priv;
+#endif
+	struct ieee80211_txb *txb = NULL;
+	struct ieee80211_hdr_3addr *frag_hdr;
+	int i, bytes_per_frag, nr_frags, bytes_last_frag, frag_size;
+	int ether_type;
+	int bytes, QOS_ctl;
+	struct sk_buff *skb_frag;
+
+	ether_type = ntohs(((struct ethhdr *)skb->data)->h_proto);
+
+	/* 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);
+
+	/* Determine fragmentation size based on destination (multicast
+	 * and broadcast are not fragmented) */
+	// if (is_multicast_ether_addr(dest) ||
+	// is_broadcast_ether_addr(dest)) {
+	if (is_multicast_ether_addr(header->addr1) ||
+			is_broadcast_ether_addr(header->addr1)) {
+		frag_size = MAX_FRAG_THRESHOLD;
+		QOS_ctl = QOS_CTL_NOTCONTAIN_ACK;
+	}
+	else {
+		//printk(KERN_WARNING "&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&frag_size = %d\n", frag_size);
+		frag_size = ieee->fts;//default:392
+		QOS_ctl = 0;
+	}
+
+	if(isQoS) {
+		QOS_ctl |= skb->priority; //set in the ieee80211_classify
+		*pQOS_ctl = cpu_to_le16(QOS_ctl);
+	}
+	//printk(KERN_WARNING "header size = %d, QOS_ctl = %x\n", hdr_len,QOS_ctl);
+	/* 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 - (IEEE80211_3ADDR_LEN + (ieee->current_network->QoS_Enable ? 2:0));
+	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 encryptiong pre/postfix */
+	if (isEncrypt)
+		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)) {
+		printk(KERN_WARNING "%s: Could not allocate TXB\n",
+			ieee->dev->name);
+		return NULL;
+	}
+	txb->encrypted = isEncrypt;
+	txb->payload_size = bytes;
+
+	for (i = 0; i < nr_frags; i++) {
+		skb_frag = txb->fragments[i];
+		skb_frag->priority = UP2AC(skb->priority);
+		if (isEncrypt)
+			skb_reserve(skb_frag, crypt->ops->extra_prefix_len);
+
+		frag_hdr = (struct ieee80211_hdr_3addr *)skb_put(skb_frag, hdr_len);
+		memcpy(frag_hdr, (void *)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(
+					header->frame_ctl | IEEE80211_FCTL_MOREFRAGS);
+			bytes = bytes_per_frag;
+
+		} else {
+			/* The last fragment takes the remaining length */
+			bytes = bytes_last_frag;
+		}
+
+		frag_hdr->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0]<<4 | i);
+		//frag_hdr->seq_ctl = cpu_to_le16(ieee->seq_ctrl<<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 (isEncrypt)
+			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.
+	//printk(KERN_WARNING "QoS Enalbed? %s\n", ieee->current_network.QoS_Enable?"Y":"N");
+	if (ieee->seq_ctrl[0] == 0xFFF)
+		ieee->seq_ctrl[0] = 0;
+	else
+		ieee->seq_ctrl[0]++;
+	// stanley, just for debug
+/*
+{
+	int j=0;
+	for(j=0;j<nr_frags;j++)
+	{
+			int i;
+		struct sk_buff *skb = txb->fragments[j];
+			printk("send(%d): ", j);
+			for (i=0;i<skb->len;i++)
+				printk("%02X ", skb->data[i]&0xff);
+			printk("\n");
+	}
+}
+*/
+
+	return txb;
+}
+
+
+// based on part of ieee80211_xmit. Mainly allocate txb. ieee->lock is held
+// Assume no encryption, no FCS computing
+struct ieee80211_txb *ieee80211_ext_reuse_txb(struct sk_buff *skb, struct net_device *dev, struct ieee80211_hdr_3addr *header, int hdr_len, u8 isQoS, u16 *pQOS_ctl, int isEncrypt, struct ieee80211_crypt_data* crypt)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0))
+	struct ieee80211_device *ieee = netdev_priv(dev);
+#else
+	struct ieee80211_device *ieee = (struct ieee80211_device *)dev->priv;
+#endif
+	struct ieee80211_txb *txb = NULL;
+	struct ieee80211_hdr_3addr *frag_hdr;
+	int ether_type;
+	int bytes, QOS_ctl;
+
+	ether_type = ntohs(((struct ethhdr *)skb->data)->h_proto);
+
+	/* 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 (is_multicast_ether_addr(header->addr1) ||
+			is_broadcast_ether_addr(header->addr1)) {
+		QOS_ctl = QOS_CTL_NOTCONTAIN_ACK;
+	}
+	else {
+		QOS_ctl = 0;
+	}
+
+	if(isQoS) {
+		QOS_ctl |= skb->priority; //set in the ieee80211_classify
+		*pQOS_ctl = cpu_to_le16(QOS_ctl);
+	}
+
+	txb = kmalloc( sizeof(struct ieee80211_txb) + sizeof(u8*), GFP_ATOMIC );
+	if (unlikely(!txb)) {
+		printk(KERN_WARNING "%s: Could not allocate TXB\n",
+			ieee->dev->name);
+		return NULL;
+	}
+
+	txb->nr_frags = 1;
+	txb->frag_size = bytes;
+	txb->encrypted = isEncrypt;
+	txb->payload_size = bytes;
+
+	txb->fragments[0] = skb;
+	ieee80211_put_snap(
+			skb_push(skb, SNAP_SIZE + sizeof(u16)), ether_type);
+	frag_hdr = (struct ieee80211_hdr_3addr *)skb_push(skb, hdr_len);
+	memcpy(frag_hdr, (void *)header, hdr_len);
+	frag_hdr->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0]<<4 | 0);
+	skb->priority = UP2AC(skb->priority);
+
+	// Advance sequence number in data frame.
+	//printk(KERN_WARNING "QoS Enalbed? %s\n", ieee->current_network.QoS_Enable?"Y":"N");
+	if (ieee->seq_ctrl[0] == 0xFFF)
+		ieee->seq_ctrl[0] = 0;
+	else
+		ieee->seq_ctrl[0]++;
+
+	return txb;
+}
+
+#endif // _RTL8187_EXT_PATCH_
+
+/* SKBs are added to the ieee->tx_queue. */
+int ieee80211_xmit(struct sk_buff *skb,
+		   struct net_device *dev)
+{
+	struct ieee80211_device *ieee = netdev_priv(dev);
+	struct ieee80211_txb *txb = NULL;
+	struct ieee80211_hdr_3addr_QOS *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 header = { /* Ensure zero initialized */
+	//	.duration_id = 0,
+	//	.seq_ctl = 0
+	//};
+	struct ieee80211_hdr_3addr_QOS 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;
+
+	//printk(KERN_WARNING "upper layer packet!\n");
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	/* If there is no driver handler to take the TXB, dont' 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)))) {
+		printk(KERN_WARNING "%s: No xmit handler.\n",
+		       ieee->dev->name);
+		goto success;
+	}
+
+	ieee80211_classify(skb,&ieee->current_network);
+	if(likely(ieee->raw_tx == 0)){
+
+		if (unlikely(skb->len < SNAP_SIZE + sizeof(u16))) {
+			printk(KERN_WARNING "%s: skb too small (%d).\n",
+			ieee->dev->name, skb->len);
+			goto success;
+		}
+
+
+#ifdef _RTL8187_EXT_PATCH_
+		// note, skb->priority which was set by ieee80211_classify, and used by physical tx
+		if((ieee->iw_mode == ieee->iw_ext_mode) && (ieee->ext_patch_ieee80211_xmit))
+		{
+			txb = ieee->ext_patch_ieee80211_xmit(skb, dev);
+			goto success;
+		}
+#endif
+
+		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);
+		}
+	//	printk(KERN_WARNING "essid MAC address is "MAC_FMT, MAC_ARG(&header.addr1));
+		header.frame_ctl = cpu_to_le16(fc);
+		//hdr_len = IEEE80211_3ADDR_LEN;
+
+		/* Determine fragmentation size based on destination (multicast
+		* and broadcast are not fragmented) */
+//		if (is_multicast_ether_addr(dest) ||
+//		is_broadcast_ether_addr(dest)) {
+		if (is_multicast_ether_addr(header.addr1) ||
+		is_broadcast_ether_addr(header.addr1)) {
+			frag_size = MAX_FRAG_THRESHOLD;
+			QOS_ctl = QOS_CTL_NOTCONTAIN_ACK;
+		}
+		else {
+			//printk(KERN_WARNING "&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&frag_size = %d\n", frag_size);
+			frag_size = ieee->fts;//default:392
+			QOS_ctl = 0;
+		}
+
+		if (ieee->current_network.QoS_Enable)	{
+			hdr_len = IEEE80211_3ADDR_LEN + 2;
+			QOS_ctl |= skb->priority; //set in the ieee80211_classify
+			header.QOS_ctl = cpu_to_le16(QOS_ctl);
+		} else {
+			hdr_len = IEEE80211_3ADDR_LEN;
+		}
+		//printk(KERN_WARNING "header size = %d, QOS_ctl = %x\n", hdr_len,QOS_ctl);
+		/* 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 - (IEEE80211_3ADDR_LEN + (ieee->current_network->QoS_Enable ? 2:0));
+		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 encryptiong 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)) {
+			printk(KERN_WARNING "%s: Could not allocate TXB\n",
+			ieee->dev->name);
+			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_3addr_QOS *)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);
+			  //printk(KERN_WARNING "skb->priority = %d,", skb->priority);
+			  //printk(KERN_WARNING "type:%d: seq = %d\n",UP2AC(skb->priority),ieee->seq_ctrl[UP2AC(skb->priority)+1]);
+			} else {
+			  frag_hdr->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0]<<4 | i);
+			}
+			//frag_hdr->seq_ctl = cpu_to_le16(ieee->seq_ctrl<<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.
+		//printk(KERN_WARNING "QoS Enalbed? %s\n", ieee->current_network.QoS_Enable?"Y":"N");
+		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))) {
+			printk(KERN_WARNING "%s: skb too small (%d).\n",
+			ieee->dev->name, skb->len);
+			goto success;
+		}
+
+		txb = ieee80211_alloc_txb(1, skb->len, GFP_ATOMIC);
+		if(!txb){
+			printk(KERN_WARNING "%s: Could not allocate TXB\n",
+			ieee->dev->name);
+			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);
+#ifdef _RTL8187_EXT_PATCH_
+	// Sometimes, extension mode can reuse skb (by txb->fragments[0])
+	if( ! ((ieee->iw_mode == ieee->iw_ext_mode) && txb && (txb->fragments[0] == skb)) )
+#endif
+		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 0;
+			}
+			ieee80211_txb_free(txb);
+		}
+	}
+
+	return 0;
+
+ failed:
+	spin_unlock_irqrestore(&ieee->lock, flags);
+	netif_stop_queue(dev);
+	stats->tx_errors++;
+	return 1;
+
+}
+
+#if 0
+EXPORT_SYMBOL(ieee80211_txb_free);
+#ifdef _RTL8187_EXT_PATCH_
+EXPORT_SYMBOL(ieee80211_alloc_txb);
+EXPORT_SYMBOL(ieee80211_ext_alloc_txb);
+EXPORT_SYMBOL(ieee80211_ext_reuse_txb);
+#endif // _RTL8187_EXT_PATCH_
+#endif
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_wx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_wx.c
new file mode 100644
index 000000000000..6aad61e78041
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_wx.c
@@ -0,0 +1,884 @@
+/******************************************************************************
+
+  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/version.h>
+#include <linux/kmod.h>
+#include <linux/module.h>
+
+#include "ieee80211.h"
+static const char *ieee80211_modes[] = {
+	"?", "a", "b", "ab", "g", "ag", "bg", "abg"
+};
+
+#ifdef FEDORACORE_9
+#define IN_FEDORACORE_9 1
+#else
+#define IN_FEDORACORE_9 0
+#endif
+
+#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;
+	memcpy(iwe.u.ap_addr.sa_data, network->bssid, ETH_ALEN);
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_ADDR_LEN);
+#else
+	start = iwe_stream_add_event(start, stop, &iwe, IW_EV_ADDR_LEN);
+#endif
+
+	/* Remaining entries will be displayed in the order we provide them */
+
+	/* Add the ESSID */
+	iwe.cmd = SIOCGIWESSID;
+	iwe.u.data.flags = 1;
+	//YJ,modified,080903,for hidden ap
+	//if (network->flags & NETWORK_EMPTY_ESSID) {
+	if (network->ssid_len == 0) {
+	//YJ,modified,080903,end
+		iwe.u.data.length = sizeof("<hidden>");
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_point(info, start, stop, &iwe, "<hidden>");
+#else
+		start = iwe_stream_add_point(start, stop, &iwe, "<hidden>");
+#endif
+	} else {
+		iwe.u.data.length = min(network->ssid_len, (u8)32);
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_point(info, start, stop, &iwe, network->ssid);
+#else
+		start = iwe_stream_add_point(start, stop, &iwe, network->ssid);
+#endif
+	}
+	//printk("ESSID: %s\n",network->ssid);
+	/* Add the protocol name */
+	iwe.cmd = SIOCGIWNAME;
+	snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11%s", ieee80211_modes[network->mode]);
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_CHAR_LEN);
+#else
+	start = iwe_stream_add_event(start, stop, &iwe, IW_EV_CHAR_LEN);
+#endif
+
+        /* 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;
+
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_UINT_LEN);
+#else
+		start = iwe_stream_add_event(start, stop, &iwe, IW_EV_UINT_LEN);
+#endif
+	}
+
+        /* Add frequency/channel */
+	iwe.cmd = SIOCGIWFREQ;
+/*	iwe.u.freq.m = ieee80211_frequency(network->channel, network->mode);
+	iwe.u.freq.e = 3; */
+	iwe.u.freq.m = network->channel;
+	iwe.u.freq.e = 0;
+	iwe.u.freq.i = 0;
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_FREQ_LEN);
+#else
+	start = iwe_stream_add_event(start, stop, &iwe, IW_EV_FREQ_LEN);
+#endif
+
+	/* 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;
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+	start = iwe_stream_add_point(info, start, stop, &iwe, network->ssid);
+#else
+	start = iwe_stream_add_point(start, stop, &iwe, network->ssid);
+#endif
+
+	/* 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;
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_PARAM_LEN);
+#else
+	start = iwe_stream_add_event(start, stop, &iwe, IW_EV_PARAM_LEN);
+#endif
+
+	iwe.cmd = IWEVCUSTOM;
+	iwe.u.data.length = p - custom;
+	if (iwe.u.data.length)
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_point(info, start, stop, &iwe, custom);
+#else
+		start = iwe_stream_add_point(start, stop, &iwe, custom);
+#endif
+
+	/* Add quality statistics */
+	/* TODO: Fix these values... */
+	if (network->stats.signal == 0 || network->stats.rssi == 0)
+	printk("========>signal:%d, rssi:%d\n", network->stats.signal, network->stats.rssi);
+	iwe.cmd = IWEVQUAL;
+//	printk("SIGNAL: %d,RSSI: %d,NOISE: %d\n",network->stats.signal,network->stats.rssi,network->stats.noise);
+	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;
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_QUAL_LEN);
+#else
+	start = iwe_stream_add_event(start, stop, &iwe, IW_EV_QUAL_LEN);
+#endif
+
+	iwe.cmd = IWEVCUSTOM;
+	p = custom;
+
+	iwe.u.data.length = p - custom;
+	if (iwe.u.data.length)
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_point(info, start, stop, &iwe, custom);
+#else
+		start = iwe_stream_add_point(start, stop, &iwe, custom);
+#endif
+
+#if 0
+	if (ieee->wpa_enabled && network->wpa_ie_len){
+		char buf[MAX_WPA_IE_LEN * 2 + 30];
+	//	printk("WPA IE\n");
+		u8 *p = buf;
+		p += sprintf(p, "wpa_ie=");
+		for (i = 0; i < network->wpa_ie_len; i++) {
+			p += sprintf(p, "%02x", network->wpa_ie[i]);
+		}
+
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = IWEVCUSTOM;
+		iwe.u.data.length = strlen(buf);
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
+#else
+		start = iwe_stream_add_point(start, stop, &iwe, buf);
+#endif
+	}
+
+	if (ieee->wpa_enabled && network->rsn_ie_len){
+		char buf[MAX_WPA_IE_LEN * 2 + 30];
+
+		u8 *p = buf;
+		p += sprintf(p, "rsn_ie=");
+		for (i = 0; i < network->rsn_ie_len; i++) {
+			p += sprintf(p, "%02x", network->rsn_ie[i]);
+		}
+
+
+#else
+		memset(&iwe, 0, sizeof(iwe));
+        if (network->wpa_ie_len) {
+	//	printk("wpa_ie_len:%d\n", 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;
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+                start = iwe_stream_add_point(info, start, stop, &iwe, buf);
+#else
+                start = iwe_stream_add_point(start, stop, &iwe, buf);
+#endif
+        }
+
+        memset(&iwe, 0, sizeof(iwe));
+        if (network->rsn_ie_len) {
+	//	printk("=====>rsn_ie_len:\n", network->rsn_ie_len);
+		#if 0
+		{
+			int i;
+			for (i=0; i<network->rsn_ie_len; i++);
+			printk("%2x ", network->rsn_ie[i]);
+			printk("\n");
+		}
+		#endif
+                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;
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
+#else
+		start = iwe_stream_add_point(start, stop, &iwe, buf);
+#endif
+	}
+
+#endif
+
+	/* 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)
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_point(info, start, stop, &iwe, custom);
+#else
+		start = iwe_stream_add_point(start, stop, &iwe, custom);
+#endif
+
+	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;//IW_SCAN_MAX_DATA;
+	//char *stop = ev + IW_SCAN_MAX_DATA;
+	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 ("
+					MAC_FMT ")' due to age (%lums).\n",
+					escape_essid(network->ssid,
+						     network->ssid_len),
+					MAC_ARG(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 = kmalloc(sizeof(struct ieee80211_crypt_data),
+				    GFP_KERNEL);
+		if (new_crypt == NULL)
+			return -ENOMEM;
+		memset(new_crypt, 0, sizeof(struct ieee80211_crypt_data));
+		new_crypt->ops = ieee80211_get_crypto_ops("WEP");
+		if (!new_crypt->ops) {
+			request_module("ieee80211_crypt_wep");
+			new_crypt->ops = ieee80211_get_crypto_ops("WEP");
+		}
+
+		if (new_crypt->ops && try_module_get(new_crypt->ops->owner))
+			new_crypt->priv = new_crypt->ops->init(key);
+
+		if (!new_crypt->ops || !new_crypt->priv) {
+			kfree(new_crypt);
+			new_crypt = NULL;
+
+			printk(KERN_WARNING "%s: could not initialize WEP: "
+			       "load module ieee80211_crypt_wep\n",
+			       dev->name);
+			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
+		 * explicitely set */
+		if (key == sec.active_key)
+			sec.flags |= SEC_ACTIVE_KEY;
+		ieee->tx_keyidx = key;//by wb 080312
+	} 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)) {
+		printk(KERN_DEBUG "%s: reset_port failed\n", dev->name);
+		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, *module;
+        struct ieee80211_crypto_ops *ops;
+        struct ieee80211_crypt_data **crypt;
+
+        struct ieee80211_security sec = {
+                .flags = 0,
+        };
+	//printk("======>encoding flag:%x,ext flag:%x, ext alg:%d\n", encoding->flags,ext->ext_flags, ext->alg);
+        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 */
+		//printk("not group key, flags:%x, ext->alg:%d\n", ext->ext_flags, ext->alg);
+                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;// | SEC_ENCRYPT;
+        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.encrypt = 0;
+                        sec.level = SEC_LEVEL_0;
+                        sec.flags |= SEC_LEVEL;
+                }
+		//printk("disabled: flag:%x\n", encoding->flags);
+                goto done;
+        }
+
+	sec.enabled = 1;
+    //    sec.encrypt = 1;
+#if 0
+        if (group_key ? !ieee->host_mc_decrypt :
+            !(ieee->host_encrypt || ieee->host_decrypt ||
+              ieee->host_encrypt_msdu))
+                goto skip_host_crypt;
+#endif
+        switch (ext->alg) {
+        case IW_ENCODE_ALG_WEP:
+                alg = "WEP";
+                module = "ieee80211_crypt_wep";
+                break;
+        case IW_ENCODE_ALG_TKIP:
+                alg = "TKIP";
+                module = "ieee80211_crypt_tkip";
+                break;
+        case IW_ENCODE_ALG_CCMP:
+                alg = "CCMP";
+                module = "ieee80211_crypt_ccmp";
+                break;
+        default:
+                IEEE80211_DEBUG_WX("%s: unknown crypto alg %d\n",
+                                   dev->name, ext->alg);
+                ret = -EINVAL;
+                goto done;
+        }
+//	printk("8-09-08-9=====>%s, alg name:%s\n",__func__, alg);
+
+	 ops = ieee80211_get_crypto_ops(alg);
+        if (ops == NULL) {
+                request_module(module);
+                ops = ieee80211_get_crypto_ops(alg);
+        }
+        if (ops == NULL) {
+                IEEE80211_DEBUG_WX("%s: unknown crypto alg %d\n",
+                                   dev->name, ext->alg);
+		printk("========>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 && try_module_get(new_crypt->ops->owner))
+                        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);
+		printk("key setting failed\n");
+                ret = -EINVAL;
+                goto done;
+        }
+#if 1
+ //skip_host_crypt:
+	//printk("skip_host_crypt:ext_flags:%x\n", ext->ext_flags);
+        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.encode_alg[idx] = SEC_ALG_WEP;
+                        sec.flags |= SEC_LEVEL;
+                        sec.level = SEC_LEVEL_1;
+                } else if (ext->alg == IW_ENCODE_ALG_TKIP) {
+                      //  sec.encode_alg[idx] = SEC_ALG_TKIP;
+                        sec.flags |= SEC_LEVEL;
+                        sec.level = SEC_LEVEL_2;
+                } else if (ext->alg == IW_ENCODE_ALG_CCMP) {
+                       // sec.encode_alg[idx] = SEC_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;
+//	printk("\ndkgadfslkdjgalskdf===============>%s(), cmd:%x\n", __func__, mlme->cmd);
+#if 1
+	switch (mlme->cmd) {
+        case IW_MLME_DEAUTH:
+	case IW_MLME_DISASSOC:
+	//	printk("disassoc now\n");
+		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)
+{
+/*
+	 struct ieee80211_security sec = {
+                .flags = SEC_AUTH_MODE,
+	}
+*/
+	//printk("set auth:flag:%x, data value:%x\n", data->flags, data->value);
+	switch (data->flags & IW_AUTH_INDEX) {
+        case IW_AUTH_WPA_VERSION:
+	     /*need to support wpa2 here*/
+		//printk("wpa version:%x\n", data->value);
+		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;
+		//printk("open_wep:%d\n", ieee->open_wep);
+		break;
+
+#if 1
+	case IW_AUTH_WPA_ENABLED:
+		ieee->wpa_enabled = (data->value)?1:0;
+		//printk("enalbe wpa:%d\n", ieee->wpa_enabled);
+		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)
+{
+#if 0
+	printk("====>%s()\n", __func__);
+	{
+		int i;
+		for (i=0; i<len; i++)
+		printk("%2x ", ie[i]&0xff);
+		printk("\n");
+	}
+#endif
+	u8 *buf = NULL;
+
+	if (len>MAX_WPA_IE_LEN || (len && ie == NULL))
+	{
+		printk("return error out, len:%d\n", len);
+	return -EINVAL;
+	}
+
+	if (len)
+	{
+		if (len != ie[1]+2){
+			printk("len:%d, ie:%d\n", len, ie[1]);
+			return -EINVAL;
+		}
+		buf = kmalloc(len, GFP_KERNEL);
+		if (buf == NULL)
+			return -ENOMEM;
+		memcpy(buf, ie, len);
+		kfree(ieee->wpa_ie);
+		ieee->wpa_ie = buf;
+		ieee->wpa_ie_len = len;
+	}
+	else{
+		if (ieee->wpa_ie)
+		kfree(ieee->wpa_ie);
+		ieee->wpa_ie = NULL;
+		ieee->wpa_ie_len = 0;
+	}
+//	printk("<=====out %s()\n", __func__);
+
+	return 0;
+
+}
+#endif
+
+#if 0
+EXPORT_SYMBOL(ieee80211_wx_set_gen_ie);
+EXPORT_SYMBOL(ieee80211_wx_set_mlme);
+EXPORT_SYMBOL(ieee80211_wx_set_auth);
+EXPORT_SYMBOL(ieee80211_wx_set_encode_ext);
+EXPORT_SYMBOL(ieee80211_wx_get_scan);
+EXPORT_SYMBOL(ieee80211_wx_set_encode);
+EXPORT_SYMBOL(ieee80211_wx_get_encode);
+#endif
diff --git a/drivers/staging/rtl8187se/ieee80211/internal.h b/drivers/staging/rtl8187se/ieee80211/internal.h
new file mode 100644
index 000000000000..ddc22350d006
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/internal.h
@@ -0,0 +1,115 @@
+/*
+ * Cryptographic API.
+ *
+ * Copyright (c) 2002 James Morris <jmorris@intercode.com.au>
+ *
+ * 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 _CRYPTO_INTERNAL_H
+#define _CRYPTO_INTERNAL_H
+
+
+//#include <linux/crypto.h>
+#include "rtl_crypto.h"
+#include <linux/mm.h>
+#include <linux/highmem.h>
+#include <linux/init.h>
+#include <asm/hardirq.h>
+#include <asm/softirq.h>
+#include <asm/kmap_types.h>
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,20))
+#define list_for_each_entry(pos, head, member)				\
+	for (pos = list_entry((head)->next, typeof(*pos), member),	\
+		     prefetch(pos->member.next);			\
+	     &pos->member != (head); 					\
+	     pos = list_entry(pos->member.next, typeof(*pos), member),	\
+		     prefetch(pos->member.next))
+
+static inline void cond_resched(void)
+{
+	if (need_resched()) {
+		set_current_state(TASK_RUNNING);
+		schedule();
+	}
+}
+#endif
+
+extern enum km_type crypto_km_types[];
+
+static inline enum km_type crypto_kmap_type(int out)
+{
+	return crypto_km_types[(in_softirq() ? 2 : 0) + out];
+}
+
+static inline void *crypto_kmap(struct page *page, int out)
+{
+	return kmap_atomic(page, crypto_kmap_type(out));
+}
+
+static inline void crypto_kunmap(void *vaddr, int out)
+{
+	kunmap_atomic(vaddr, crypto_kmap_type(out));
+}
+
+static inline void crypto_yield(struct crypto_tfm *tfm)
+{
+	if (!in_softirq())
+		cond_resched();
+}
+
+static inline void *crypto_tfm_ctx(struct crypto_tfm *tfm)
+{
+	return (void *)&tfm[1];
+}
+
+struct crypto_alg *crypto_alg_lookup(const char *name);
+
+#ifdef CONFIG_KMOD
+void crypto_alg_autoload(const char *name);
+struct crypto_alg *crypto_alg_mod_lookup(const char *name);
+#else
+static inline struct crypto_alg *crypto_alg_mod_lookup(const char *name)
+{
+	return crypto_alg_lookup(name);
+}
+#endif
+
+#ifdef CONFIG_CRYPTO_HMAC
+int crypto_alloc_hmac_block(struct crypto_tfm *tfm);
+void crypto_free_hmac_block(struct crypto_tfm *tfm);
+#else
+static inline int crypto_alloc_hmac_block(struct crypto_tfm *tfm)
+{
+	return 0;
+}
+
+static inline void crypto_free_hmac_block(struct crypto_tfm *tfm)
+{ }
+#endif
+
+#ifdef CONFIG_PROC_FS
+void __init crypto_init_proc(void);
+#else
+static inline void crypto_init_proc(void)
+{ }
+#endif
+
+int crypto_init_digest_flags(struct crypto_tfm *tfm, u32 flags);
+int crypto_init_cipher_flags(struct crypto_tfm *tfm, u32 flags);
+int crypto_init_compress_flags(struct crypto_tfm *tfm, u32 flags);
+
+int crypto_init_digest_ops(struct crypto_tfm *tfm);
+int crypto_init_cipher_ops(struct crypto_tfm *tfm);
+int crypto_init_compress_ops(struct crypto_tfm *tfm);
+
+void crypto_exit_digest_ops(struct crypto_tfm *tfm);
+void crypto_exit_cipher_ops(struct crypto_tfm *tfm);
+void crypto_exit_compress_ops(struct crypto_tfm *tfm);
+
+#endif	/* _CRYPTO_INTERNAL_H */
+
diff --git a/drivers/staging/rtl8187se/ieee80211/rtl_crypto.h b/drivers/staging/rtl8187se/ieee80211/rtl_crypto.h
new file mode 100644
index 000000000000..9ed0ca420857
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/rtl_crypto.h
@@ -0,0 +1,399 @@
+/*
+ * Scatterlist Cryptographic API.
+ *
+ * Copyright (c) 2002 James Morris <jmorris@intercode.com.au>
+ * Copyright (c) 2002 David S. Miller (davem@redhat.com)
+ *
+ * Portions derived from Cryptoapi, by Alexander Kjeldaas <astor@fast.no>
+ * and Nettle, by Niels Mé°ˆler.
+ *
+ * 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 _LINUX_CRYPTO_H
+#define _LINUX_CRYPTO_H
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/string.h>
+#include <asm/page.h>
+#include <asm/errno.h>
+
+#define crypto_register_alg crypto_register_alg_rtl
+#define crypto_unregister_alg crypto_unregister_alg_rtl
+#define crypto_alloc_tfm crypto_alloc_tfm_rtl
+#define crypto_free_tfm crypto_free_tfm_rtl
+#define crypto_alg_available crypto_alg_available_rtl
+
+/*
+ * Algorithm masks and types.
+ */
+#define CRYPTO_ALG_TYPE_MASK		0x000000ff
+#define CRYPTO_ALG_TYPE_CIPHER		0x00000001
+#define CRYPTO_ALG_TYPE_DIGEST		0x00000002
+#define CRYPTO_ALG_TYPE_COMPRESS	0x00000004
+
+/*
+ * Transform masks and values (for crt_flags).
+ */
+#define CRYPTO_TFM_MODE_MASK		0x000000ff
+#define CRYPTO_TFM_REQ_MASK		0x000fff00
+#define CRYPTO_TFM_RES_MASK		0xfff00000
+
+#define CRYPTO_TFM_MODE_ECB		0x00000001
+#define CRYPTO_TFM_MODE_CBC		0x00000002
+#define CRYPTO_TFM_MODE_CFB		0x00000004
+#define CRYPTO_TFM_MODE_CTR		0x00000008
+
+#define CRYPTO_TFM_REQ_WEAK_KEY		0x00000100
+#define CRYPTO_TFM_RES_WEAK_KEY		0x00100000
+#define CRYPTO_TFM_RES_BAD_KEY_LEN   	0x00200000
+#define CRYPTO_TFM_RES_BAD_KEY_SCHED 	0x00400000
+#define CRYPTO_TFM_RES_BAD_BLOCK_LEN 	0x00800000
+#define CRYPTO_TFM_RES_BAD_FLAGS 	0x01000000
+
+/*
+ * Miscellaneous stuff.
+ */
+#define CRYPTO_UNSPEC			0
+#define CRYPTO_MAX_ALG_NAME		64
+
+struct scatterlist;
+
+/*
+ * Algorithms: modular crypto algorithm implementations, managed
+ * via crypto_register_alg() and crypto_unregister_alg().
+ */
+struct cipher_alg {
+	unsigned int cia_min_keysize;
+	unsigned int cia_max_keysize;
+	int (*cia_setkey)(void *ctx, const u8 *key,
+	                  unsigned int keylen, u32 *flags);
+	void (*cia_encrypt)(void *ctx, u8 *dst, const u8 *src);
+	void (*cia_decrypt)(void *ctx, u8 *dst, const u8 *src);
+};
+
+struct digest_alg {
+	unsigned int dia_digestsize;
+	void (*dia_init)(void *ctx);
+	void (*dia_update)(void *ctx, const u8 *data, unsigned int len);
+	void (*dia_final)(void *ctx, u8 *out);
+	int (*dia_setkey)(void *ctx, const u8 *key,
+	                  unsigned int keylen, u32 *flags);
+};
+
+struct compress_alg {
+	int (*coa_init)(void *ctx);
+	void (*coa_exit)(void *ctx);
+	int (*coa_compress)(void *ctx, const u8 *src, unsigned int slen,
+	                    u8 *dst, unsigned int *dlen);
+	int (*coa_decompress)(void *ctx, const u8 *src, unsigned int slen,
+	                      u8 *dst, unsigned int *dlen);
+};
+
+#define cra_cipher	cra_u.cipher
+#define cra_digest	cra_u.digest
+#define cra_compress	cra_u.compress
+
+struct crypto_alg {
+	struct list_head cra_list;
+	u32 cra_flags;
+	unsigned int cra_blocksize;
+	unsigned int cra_ctxsize;
+	const char cra_name[CRYPTO_MAX_ALG_NAME];
+
+	union {
+		struct cipher_alg cipher;
+		struct digest_alg digest;
+		struct compress_alg compress;
+	} cra_u;
+
+	struct module *cra_module;
+};
+
+/*
+ * Algorithm registration interface.
+ */
+int crypto_register_alg(struct crypto_alg *alg);
+int crypto_unregister_alg(struct crypto_alg *alg);
+
+/*
+ * Algorithm query interface.
+ */
+int crypto_alg_available(const char *name, u32 flags);
+
+/*
+ * Transforms: user-instantiated objects which encapsulate algorithms
+ * and core processing logic.  Managed via crypto_alloc_tfm() and
+ * crypto_free_tfm(), as well as the various helpers below.
+ */
+struct crypto_tfm;
+
+struct cipher_tfm {
+	void *cit_iv;
+	unsigned int cit_ivsize;
+	u32 cit_mode;
+	int (*cit_setkey)(struct crypto_tfm *tfm,
+	                  const u8 *key, unsigned int keylen);
+	int (*cit_encrypt)(struct crypto_tfm *tfm,
+			   struct scatterlist *dst,
+			   struct scatterlist *src,
+			   unsigned int nbytes);
+	int (*cit_encrypt_iv)(struct crypto_tfm *tfm,
+	                      struct scatterlist *dst,
+	                      struct scatterlist *src,
+	                      unsigned int nbytes, u8 *iv);
+	int (*cit_decrypt)(struct crypto_tfm *tfm,
+			   struct scatterlist *dst,
+			   struct scatterlist *src,
+			   unsigned int nbytes);
+	int (*cit_decrypt_iv)(struct crypto_tfm *tfm,
+			   struct scatterlist *dst,
+			   struct scatterlist *src,
+			   unsigned int nbytes, u8 *iv);
+	void (*cit_xor_block)(u8 *dst, const u8 *src);
+};
+
+struct digest_tfm {
+	void (*dit_init)(struct crypto_tfm *tfm);
+	void (*dit_update)(struct crypto_tfm *tfm,
+	                   struct scatterlist *sg, unsigned int nsg);
+	void (*dit_final)(struct crypto_tfm *tfm, u8 *out);
+	void (*dit_digest)(struct crypto_tfm *tfm, struct scatterlist *sg,
+	                   unsigned int nsg, u8 *out);
+	int (*dit_setkey)(struct crypto_tfm *tfm,
+	                  const u8 *key, unsigned int keylen);
+#ifdef CONFIG_CRYPTO_HMAC
+	void *dit_hmac_block;
+#endif
+};
+
+struct compress_tfm {
+	int (*cot_compress)(struct crypto_tfm *tfm,
+	                    const u8 *src, unsigned int slen,
+	                    u8 *dst, unsigned int *dlen);
+	int (*cot_decompress)(struct crypto_tfm *tfm,
+	                      const u8 *src, unsigned int slen,
+	                      u8 *dst, unsigned int *dlen);
+};
+
+#define crt_cipher	crt_u.cipher
+#define crt_digest	crt_u.digest
+#define crt_compress	crt_u.compress
+
+struct crypto_tfm {
+
+	u32 crt_flags;
+
+	union {
+		struct cipher_tfm cipher;
+		struct digest_tfm digest;
+		struct compress_tfm compress;
+	} crt_u;
+
+	struct crypto_alg *__crt_alg;
+};
+
+/*
+ * Transform user interface.
+ */
+
+/*
+ * crypto_alloc_tfm() will first attempt to locate an already loaded algorithm.
+ * If that fails and the kernel supports dynamically loadable modules, it
+ * will then attempt to load a module of the same name or alias.  A refcount
+ * is grabbed on the algorithm which is then associated with the new transform.
+ *
+ * crypto_free_tfm() frees up the transform and any associated resources,
+ * then drops the refcount on the associated algorithm.
+ */
+struct crypto_tfm *crypto_alloc_tfm(const char *alg_name, u32 tfm_flags);
+void crypto_free_tfm(struct crypto_tfm *tfm);
+
+/*
+ * Transform helpers which query the underlying algorithm.
+ */
+static inline const char *crypto_tfm_alg_name(struct crypto_tfm *tfm)
+{
+	return tfm->__crt_alg->cra_name;
+}
+
+static inline const char *crypto_tfm_alg_modname(struct crypto_tfm *tfm)
+{
+	struct crypto_alg *alg = tfm->__crt_alg;
+
+	if (alg->cra_module)
+		return alg->cra_module->name;
+	else
+		return NULL;
+}
+
+static inline u32 crypto_tfm_alg_type(struct crypto_tfm *tfm)
+{
+	return tfm->__crt_alg->cra_flags & CRYPTO_ALG_TYPE_MASK;
+}
+
+static inline unsigned int crypto_tfm_alg_min_keysize(struct crypto_tfm *tfm)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	return tfm->__crt_alg->cra_cipher.cia_min_keysize;
+}
+
+static inline unsigned int crypto_tfm_alg_max_keysize(struct crypto_tfm *tfm)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	return tfm->__crt_alg->cra_cipher.cia_max_keysize;
+}
+
+static inline unsigned int crypto_tfm_alg_ivsize(struct crypto_tfm *tfm)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	return tfm->crt_cipher.cit_ivsize;
+}
+
+static inline unsigned int crypto_tfm_alg_blocksize(struct crypto_tfm *tfm)
+{
+	return tfm->__crt_alg->cra_blocksize;
+}
+
+static inline unsigned int crypto_tfm_alg_digestsize(struct crypto_tfm *tfm)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
+	return tfm->__crt_alg->cra_digest.dia_digestsize;
+}
+
+/*
+ * API wrappers.
+ */
+static inline void crypto_digest_init(struct crypto_tfm *tfm)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
+	tfm->crt_digest.dit_init(tfm);
+}
+
+static inline void crypto_digest_update(struct crypto_tfm *tfm,
+                                        struct scatterlist *sg,
+                                        unsigned int nsg)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
+	tfm->crt_digest.dit_update(tfm, sg, nsg);
+}
+
+static inline void crypto_digest_final(struct crypto_tfm *tfm, u8 *out)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
+	tfm->crt_digest.dit_final(tfm, out);
+}
+
+static inline void crypto_digest_digest(struct crypto_tfm *tfm,
+                                        struct scatterlist *sg,
+                                        unsigned int nsg, u8 *out)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
+	tfm->crt_digest.dit_digest(tfm, sg, nsg, out);
+}
+
+static inline int crypto_digest_setkey(struct crypto_tfm *tfm,
+                                       const u8 *key, unsigned int keylen)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
+	if (tfm->crt_digest.dit_setkey == NULL)
+		return -ENOSYS;
+	return tfm->crt_digest.dit_setkey(tfm, key, keylen);
+}
+
+static inline int crypto_cipher_setkey(struct crypto_tfm *tfm,
+                                       const u8 *key, unsigned int keylen)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	return tfm->crt_cipher.cit_setkey(tfm, key, keylen);
+}
+
+static inline int crypto_cipher_encrypt(struct crypto_tfm *tfm,
+                                        struct scatterlist *dst,
+                                        struct scatterlist *src,
+                                        unsigned int nbytes)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	return tfm->crt_cipher.cit_encrypt(tfm, dst, src, nbytes);
+}
+
+static inline int crypto_cipher_encrypt_iv(struct crypto_tfm *tfm,
+                                           struct scatterlist *dst,
+                                           struct scatterlist *src,
+                                           unsigned int nbytes, u8 *iv)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	BUG_ON(tfm->crt_cipher.cit_mode == CRYPTO_TFM_MODE_ECB);
+	return tfm->crt_cipher.cit_encrypt_iv(tfm, dst, src, nbytes, iv);
+}
+
+static inline int crypto_cipher_decrypt(struct crypto_tfm *tfm,
+                                        struct scatterlist *dst,
+                                        struct scatterlist *src,
+                                        unsigned int nbytes)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	return tfm->crt_cipher.cit_decrypt(tfm, dst, src, nbytes);
+}
+
+static inline int crypto_cipher_decrypt_iv(struct crypto_tfm *tfm,
+                                           struct scatterlist *dst,
+                                           struct scatterlist *src,
+                                           unsigned int nbytes, u8 *iv)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	BUG_ON(tfm->crt_cipher.cit_mode == CRYPTO_TFM_MODE_ECB);
+	return tfm->crt_cipher.cit_decrypt_iv(tfm, dst, src, nbytes, iv);
+}
+
+static inline void crypto_cipher_set_iv(struct crypto_tfm *tfm,
+                                        const u8 *src, unsigned int len)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	memcpy(tfm->crt_cipher.cit_iv, src, len);
+}
+
+static inline void crypto_cipher_get_iv(struct crypto_tfm *tfm,
+                                        u8 *dst, unsigned int len)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	memcpy(dst, tfm->crt_cipher.cit_iv, len);
+}
+
+static inline int crypto_comp_compress(struct crypto_tfm *tfm,
+                                       const u8 *src, unsigned int slen,
+                                       u8 *dst, unsigned int *dlen)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_COMPRESS);
+	return tfm->crt_compress.cot_compress(tfm, src, slen, dst, dlen);
+}
+
+static inline int crypto_comp_decompress(struct crypto_tfm *tfm,
+                                         const u8 *src, unsigned int slen,
+                                         u8 *dst, unsigned int *dlen)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_COMPRESS);
+	return tfm->crt_compress.cot_decompress(tfm, src, slen, dst, dlen);
+}
+
+/*
+ * HMAC support.
+ */
+#ifdef CONFIG_CRYPTO_HMAC
+void crypto_hmac_init(struct crypto_tfm *tfm, u8 *key, unsigned int *keylen);
+void crypto_hmac_update(struct crypto_tfm *tfm,
+                        struct scatterlist *sg, unsigned int nsg);
+void crypto_hmac_final(struct crypto_tfm *tfm, u8 *key,
+                       unsigned int *keylen, u8 *out);
+void crypto_hmac(struct crypto_tfm *tfm, u8 *key, unsigned int *keylen,
+                 struct scatterlist *sg, unsigned int nsg, u8 *out);
+#endif	/* CONFIG_CRYPTO_HMAC */
+
+#endif	/* _LINUX_CRYPTO_H */
+
diff --git a/drivers/staging/rtl8187se/ieee80211_crypt.h b/drivers/staging/rtl8187se/ieee80211_crypt.h
new file mode 100644
index 000000000000..b58a3bcc0dc0
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211_crypt.h
@@ -0,0 +1,86 @@
+/*
+ * 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/r8180.h b/drivers/staging/rtl8187se/r8180.h
new file mode 100644
index 000000000000..12215fc61ddc
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180.h
@@ -0,0 +1,761 @@
+/*
+   This is part of rtl8180 OpenSource driver.
+   Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+   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.
+*/
+
+#ifndef R8180H
+#define R8180H
+
+
+#define RTL8180_MODULE_NAME "rtl8180"
+#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/config.h>
+#include <linux/init.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.h"
+#include <asm/io.h>
+//#include <asm/semaphore.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	MAX_FRAG_THRESHOLD     2342U
+#define DEFAULT_RTS_THRESHOLD 2342U
+#define MIN_RTS_THRESHOLD 0U
+#define MAX_RTS_THRESHOLD 2342U
+#define DEFAULT_BEACONINTERVAL 0x64U
+#define DEFAULT_BEACON_ESSID "Rtl8180"
+
+#define DEFAULT_SSID ""
+#define DEFAULT_RETRY_RTS 7
+#define DEFAULT_RETRY_DATA 7
+#define PRISM_HDR_SIZE 64
+
+#ifdef CONFIG_RTL8185B
+
+#define MGNT_QUEUE						0
+#define BK_QUEUE						1
+#define BE_QUEUE						2
+#define VI_QUEUE						3
+#define VO_QUEUE						4
+#define HIGH_QUEUE						5
+#define BEACON_QUEUE					6
+
+#define LOW_QUEUE						BE_QUEUE
+#define NORMAL_QUEUE					MGNT_QUEUE
+
+#define aSifsTime 	10
+
+#define sCrcLng         4
+#define sAckCtsLng	112		// bits in ACK and CTS frames
+//+by amy 080312
+#define RATE_ADAPTIVE_TIMER_PERIOD	300
+
+typedef 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;
+
+typedef enum _VERSION_8185{
+	// RTL8185
+	VERSION_8185_UNKNOWN,
+	VERSION_8185_C, // C-cut
+	VERSION_8185_D, // D-cut
+	// RTL8185B
+	VERSION_8185B_B, // B-cut
+	VERSION_8185B_D, // D-cut
+	VERSION_8185B_E, // E-cut
+	//RTL8187S-PCIE
+	VERSION_8187S_B, // B-cut
+	VERSION_8187S_C, // C-cut
+	VERSION_8187S_D, // D-cut
+
+}VERSION_8185,*PVERSION_8185;
+typedef struct 	ChnlAccessSetting {
+	u16 SIFS_Timer;
+	u16 DIFS_Timer;
+	u16 SlotTimeTimer;
+	u16 EIFS_Timer;
+	u16 CWminIndex;
+	u16 CWmaxIndex;
+}*PCHANNEL_ACCESS_SETTING,CHANNEL_ACCESS_SETTING;
+
+typedef enum{
+        NIC_8185 = 1,
+        NIC_8185B
+        } nic_t;
+
+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;
+
+/* it is a wrong definition. -xiong-2006-11-17
+typedef struct ThreeWireReg {
+	u16	longData;
+	struct {
+		u8	enableB;
+		u8	data;
+		u8	clk;
+		u8	read_write;
+	} struc;
+} ThreeWireReg;
+*/
+
+typedef	union _ThreeWire{
+	struct _ThreeWireStruc{
+		u16		data:1;
+		u16		clk:1;
+		u16		enableB:1;
+		u16		read_write:1;
+		u16		resv1:12;
+//		u2Byte	resv2:14;
+//		u2Byte	ThreeWireEnable:1;
+//		u2Byte	resv3:1;
+	}struc;
+	u16			longData;
+}ThreeWireReg;
+
+#endif
+
+typedef struct buffer
+{
+	struct buffer *next;
+	u32 *buf;
+	dma_addr_t dma;
+} buffer;
+
+//YJ,modified,080828
+typedef struct Stats
+{
+	unsigned long txrdu;
+	unsigned long rxrdu;
+	unsigned long rxnolast;
+	unsigned long rxnodata;
+//	unsigned long rxreset;
+//	unsigned long rxwrkaround;
+	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
+} Stats;
+
+#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
+
+typedef struct _link_detect_t
+{
+	u32				RxFrameNum[MAX_LD_SLOT_NUM];	// number of Rx Frame / CheckForHang_period  to determine link status
+	u16				SlotNum;	// number of CheckForHang period to determine link status, default is 2
+	u16				SlotIndex;
+
+	u32				NumTxOkInPeriod;  //number of packet transmitted during CheckForHang
+	u32				NumRxOkInPeriod;  //number of packet received during CheckForHang
+
+	u8				IdleCount;     // (KEEP_ALIVE_INTERVAL / CHECK_FOR_HANG_PERIOD)
+	u32				LastNumTxUnicast;
+	u32				LastNumRxUnicast;
+
+	bool				bBusyTraffic;    //when it is set to 1, UI cann't scan at will.
+}link_detect_t, *plink_detect_t;
+
+//YJ,modified,080828,end
+
+//by amy for led
+//================================================================================
+// LED customization.
+//================================================================================
+
+typedef	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)
+}LED_STRATEGY_8185, *PLED_STRATEGY_8185;
+//by amy for led
+//by amy for power save
+typedef	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_MODE;
+
+typedef	enum _RT_RF_POWER_STATE
+{
+	eRfOn,
+	eRfSleep,
+	eRfOff
+}RT_RF_POWER_STATE;
+
+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
+};
+typedef	enum _RT_PS_MODE
+{
+	eActive,	// Active/Continuous access.
+	eMaxPs,		// Max power save mode.
+	eFastPs		// Fast power save mode.
+}RT_PS_MODE;
+//by amy for power save
+typedef struct r8180_priv
+{
+	struct pci_dev *pdev;
+
+	short epromtype;
+	int irq;
+	struct ieee80211_device *ieee80211;
+
+        short card_8185; /* O: rtl8180, 1:rtl8185 V B/C, 2:rtl8185 V D, 3:rtl8185B */
+	short card_8185_Bversion; /* if TCR reports card V B/C this discriminates */
+	short phy_ver; /* meaningful for rtl8225 1:A 2:B 3:C */
+	short enable_gpio0;
+	enum card_type {PCI,MINIPCI,CARDBUS,USB/*rtl8187*/}card_type;
+	short hw_plcp_len;
+	short plcp_preamble_mode; // 0:auto 1:short 2:long
+
+	spinlock_t irq_lock;
+	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 challow[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
+	short prism_hdr;
+
+	struct timer_list scan_timer;
+	/*short scanpending;
+	short stopscan;*/
+	spinlock_t scan_lock;
+	u8 active_probe;
+	//u8 active_scan_num;
+	struct semaphore wx_sem;
+	struct semaphore rf_state;
+	short hw_wep;
+
+	short digphy;
+	short antb;
+	short diversity;
+	u8 cs_treshold;
+	short rcr_csense;
+	short rf_chip;
+	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;
+	struct proc_dir_entry *dir_dev;
+
+	/*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;
+
+	/*TX stuff*/
+/*
+	u32 *txlpring;
+	u32 *txhpring;
+	u32 *txnpring;
+	dma_addr_t txlpringdma;
+	dma_addr_t txhpringdma;
+	dma_addr_t txnpringdma;
+	u32 *txlpringtail;
+	u32 *txhpringtail;
+	u32 *txnpringtail;
+	u32 *txlpringhead;
+	u32 *txhpringhead;
+	u32 *txnpringhead;
+	struct buffer *txlpbufs;
+	struct buffer *txhpbufs;
+	struct buffer *txnpbufs;
+	struct buffer *txlpbufstail;
+	struct buffer *txhpbufstail;
+	struct buffer *txnpbufstail;
+*/
+	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 tx_pendingbuf txnp_pending;
+	//struct tasklet_struct irq_tx_tasklet;
+	struct tasklet_struct irq_rx_tasklet;
+	u8 dma_poll_mask;
+	//short tx_suspend;
+
+	/* adhoc/master mode stuff */
+	u32 *txbeaconringtail;
+	dma_addr_t txbeaconringdma;
+	u32 *txbeaconring;
+	int txbeaconcount;
+	struct buffer *txbeaconbufs;
+	struct buffer *txbeaconbufstail;
+	//char *master_essid;
+	//u16 master_beaconinterval;
+	//u32 master_beaconsize;
+	//u16 beacon_interval;
+
+	u8 retry_data;
+	u8 retry_rts;
+	u16 rts;
+
+//add for RF power on power off by lizhaoming 080512
+	u8	 RegThreeWireMode; // See "Three wire mode" defined above, 2006.05.31, by rcnjko.
+
+//by amy for led
+	LED_STRATEGY_8185 LedStrategy;
+//by amy for led
+
+//by amy for power save
+	struct timer_list watch_dog_timer;
+	bool bInactivePs;
+	bool bSwRfProcessing;
+	RT_RF_POWER_STATE	eInactivePowerState;
+	RT_RF_POWER_STATE eRFPowerState;
+	u32 RfOffReason;
+	bool RFChangeInProgress;
+	bool bInHctTest;
+	bool SetRFPowerStateInProgress;
+	u8   RFProgType;
+	bool bLeisurePs;
+	RT_PS_MODE dot11PowerSaveMode;
+	//u32 NumRxOkInPeriod;   //YJ,del,080828
+	//u32 NumTxOkInPeriod;   //YJ,del,080828
+	u8   TxPollingTimes;
+
+	bool	bApBufOurFrame;// TRUE if AP buffer our unicast data , we will keep eAwake untill 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 percentange, 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 Antenn 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 swithed 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 threhold 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					UndercorateSmoothedRxPower;
+	u8						RSSI;
+	char					RxPower;
+	 u8 InitialGain;
+	 //For adjust Dig Threshhold 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;
+//by amy for rate adaptive
+//by amy 080312}
+//	short wq_hurryup;
+//	struct workqueue_struct *workqueue;
+	struct work_struct reset_wq;
+	struct work_struct watch_dog_wq;
+	struct work_struct tx_irq_wq;
+	short ack_tx_to_ieee;
+
+	u8 PowerProfile;
+#ifdef CONFIG_RTL8185B
+	u32 CSMethod;
+	u8 cck_txpwr_base;
+	u8 ofdm_txpwr_base;
+	u8 dma_poll_stop_mask;
+
+	//u8 RegThreeWireMode;
+	u8 MWIEnable;
+	u16 ShortRetryLimit;
+	u16 LongRetryLimit;
+	u16 EarlyRxThreshold;
+	u32 TransmitConfig;
+	u32 ReceiveConfig;
+	u32 IntrMask;
+
+	struct 	ChnlAccessSetting  ChannelAccessSetting;
+#endif
+}r8180_priv;
+
+#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,
+	short 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_rx_enable(struct net_device *);
+void rtl8180_tx_enable(struct net_device *);
+void rtl8180_start_scanning(struct net_device *dev);
+void rtl8180_start_scanning_s(struct net_device *dev);
+void rtl8180_stop_scanning(struct net_device *dev);
+void rtl8180_disassociate(struct net_device *dev);
+//void fix_rx_fifo(struct net_device *dev);
+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_BSS_create(struct net_device *dev);
+void rtl8180_beacon_tx_disable(struct net_device *dev);
+void rtl8180_beacon_rx_disable(struct net_device *dev);
+void rtl8180_conttx_enable(struct net_device *dev);
+void rtl8180_conttx_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 rtl8180_set_master_essid(struct net_device *dev,char *essid);
+void rtl8180_update_beacon_security(struct net_device *dev);
+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 IBSS_randomize_cell(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);
+
+//#ifdef CONFIG_RTL8185B
+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);
+#if LINUX_VERSION_CODE >=KERNEL_VERSION(2,6,20)
+void rtl8180_rate_adapter(struct work_struct * work);
+#else
+void rtl8180_rate_adapter(struct net_device *dev);
+#endif
+//#endif
+bool MgntActSet_RF_State(struct net_device *dev, RT_RF_POWER_STATE StateToSet, u32 ChangeSource);
+
+#endif
diff --git a/drivers/staging/rtl8187se/r8180_93cx6.c b/drivers/staging/rtl8187se/r8180_93cx6.c
new file mode 100644
index 000000000000..7e4711fb930c
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_93cx6.c
@@ -0,0 +1,146 @@
+/*
+   This files contains card eeprom (93c46 or 93c56) programming routines,
+   memory is addressed by 16 bits words.
+
+   This is part of rtl8180 OpenSource driver.
+   Copyright (C) Andrea Merello 2004  <andreamrl@tiscali.it>
+   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.
+*/
+
+#include "r8180_93cx6.h"
+
+void eprom_cs(struct net_device *dev, short bit)
+{
+	if(bit)
+		write_nic_byte(dev, EPROM_CMD,
+			       (1<<EPROM_CS_SHIFT) | \
+			       read_nic_byte(dev, EPROM_CMD)); //enable EPROM
+	else
+		write_nic_byte(dev, EPROM_CMD, read_nic_byte(dev, EPROM_CMD)\
+			       &~(1<<EPROM_CS_SHIFT)); //disable EPROM
+
+	force_pci_posting(dev);
+	udelay(EPROM_DELAY);
+}
+
+
+void eprom_ck_cycle(struct net_device *dev)
+{
+	write_nic_byte(dev, EPROM_CMD,
+		       (1<<EPROM_CK_SHIFT) | read_nic_byte(dev,EPROM_CMD));
+	force_pci_posting(dev);
+	udelay(EPROM_DELAY);
+	write_nic_byte(dev, EPROM_CMD,
+		       read_nic_byte(dev, EPROM_CMD) &~ (1<<EPROM_CK_SHIFT));
+	force_pci_posting(dev);
+	udelay(EPROM_DELAY);
+}
+
+
+void eprom_w(struct net_device *dev,short bit)
+{
+	if(bit)
+		write_nic_byte(dev, EPROM_CMD, (1<<EPROM_W_SHIFT) | \
+			       read_nic_byte(dev,EPROM_CMD));
+	else
+		write_nic_byte(dev, EPROM_CMD, read_nic_byte(dev,EPROM_CMD)\
+			       &~(1<<EPROM_W_SHIFT));
+
+	force_pci_posting(dev);
+	udelay(EPROM_DELAY);
+}
+
+
+short eprom_r(struct net_device *dev)
+{
+	short bit;
+
+	bit=(read_nic_byte(dev, EPROM_CMD) & (1<<EPROM_R_SHIFT) );
+	udelay(EPROM_DELAY);
+
+	if(bit) return 1;
+	return 0;
+}
+
+
+void eprom_send_bits_string(struct net_device *dev, short b[], int len)
+{
+	int i;
+
+	for(i=0; i<len; i++){
+		eprom_w(dev, b[i]);
+		eprom_ck_cycle(dev);
+	}
+}
+
+
+u32 eprom_read(struct net_device *dev, u32 addr)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	short read_cmd[]={1,1,0};
+	short addr_str[8];
+	int i;
+	int addr_len;
+	u32 ret;
+
+	ret=0;
+        //enable EPROM programming
+	write_nic_byte(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;
+	}
+	eprom_cs(dev, 1);
+	eprom_ck_cycle(dev);
+	eprom_send_bits_string(dev, read_cmd, 3);
+	eprom_send_bits_string(dev, addr_str, addr_len);
+
+	//keep chip pin D to low state while reading.
+	//I'm unsure if it is necessary, but anyway shouldn't hurt
+	eprom_w(dev, 0);
+
+	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);
+		ret |= (eprom_r(dev)<<(15-i));
+	}
+
+	eprom_cs(dev, 0);
+	eprom_ck_cycle(dev);
+
+	//disable EPROM programming
+	write_nic_byte(dev, EPROM_CMD,
+		       (EPROM_CMD_NORMAL<<EPROM_CMD_OPERATING_MODE_SHIFT));
+	return ret;
+}
diff --git a/drivers/staging/rtl8187se/r8180_93cx6.h b/drivers/staging/rtl8187se/r8180_93cx6.h
new file mode 100644
index 000000000000..a028a51b23f0
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_93cx6.h
@@ -0,0 +1,59 @@
+/*
+	This is part of rtl8180 OpenSource driver
+	Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+	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
+#ifdef CONFIG_RTL8185B
+#define RF_ZEBRA2 11
+#define EPROM_TXPW_BASE 0x05
+#define RF_ZEBRA4 12
+#endif
+#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 0x10
+#define  EPROM_TXPW_CH1_2 0x30
+#define	EPROM_TXPW_CH3_4 0x11
+#define	EPROM_TXPW_CH5_6 0x12
+#define	EPROM_TXPW_CH7_8 0x13
+#define	EPROM_TXPW_CH9_10 0x14
+#define	EPROM_TXPW_CH11_12 0x15
+#define	EPROM_TXPW_CH13_14 0x16
+
+u32 eprom_read(struct net_device *dev,u32 addr); //reads a 16 bits word
diff --git a/drivers/staging/rtl8187se/r8180_core.c b/drivers/staging/rtl8187se/r8180_core.c
new file mode 100644
index 000000000000..94534955e38b
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_core.c
@@ -0,0 +1,6828 @@
+/*
+   This is part of rtl818x pci OpenSource driver - v 0.1
+   Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+   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.
+*/
+
+#if 0
+double __floatsidf (int i) { return i; }
+unsigned int __fixunsdfsi (double d) { return d; }
+double __adddf3(double a, double b) { return a+b; }
+double __addsf3(float a, float b) { return a+b; }
+double __subdf3(double a, double b) { return a-b; }
+double __extendsfdf2(float a) {return a;}
+#endif
+
+
+#undef DEBUG_TX_DESC2
+#undef RX_DONT_PASS_UL
+#undef DEBUG_EPROM
+#undef DEBUG_RX_VERBOSE
+#undef DUMMY_RX
+#undef DEBUG_ZERO_RX
+#undef DEBUG_RX_SKB
+#undef DEBUG_TX_FRAG
+#undef DEBUG_RX_FRAG
+#undef DEBUG_TX_FILLDESC
+#undef DEBUG_TX
+#undef DEBUG_IRQ
+#undef DEBUG_RX
+#undef DEBUG_RXALLOC
+#undef DEBUG_REGISTERS
+#undef DEBUG_RING
+#undef DEBUG_IRQ_TASKLET
+#undef DEBUG_TX_ALLOC
+#undef DEBUG_TX_DESC
+
+//#define DEBUG_TX
+//#define DEBUG_TX_DESC2
+//#define DEBUG_RX
+//#define DEBUG_RX_SKB
+
+//#define CONFIG_RTL8180_IO_MAP
+#include <linux/syscalls.h>
+//#include <linux/fcntl.h>
+//#include <asm/uaccess.h>
+#include "r8180_hw.h"
+#include "r8180.h"
+#include "r8180_sa2400.h"  /* PHILIPS Radio frontend */
+#include "r8180_max2820.h" /* MAXIM Radio frontend */
+#include "r8180_gct.h"     /* GCT Radio frontend */
+#include "r8180_rtl8225.h" /* RTL8225 Radio frontend */
+#include "r8180_rtl8255.h" /* RTL8255 Radio frontend */
+#include "r8180_93cx6.h"   /* Card EEPROM */
+#include "r8180_wx.h"
+#include "r8180_dm.h"
+
+#ifdef CONFIG_RTL8180_PM
+#include "r8180_pm.h"
+#endif
+
+#ifdef ENABLE_DOT11D
+#include "dot11d.h"
+#endif
+
+#ifdef CONFIG_RTL8185B
+//#define CONFIG_RTL8180_IO_MAP
+#endif
+
+#ifndef PCI_VENDOR_ID_BELKIN
+	#define PCI_VENDOR_ID_BELKIN 0x1799
+#endif
+#ifndef PCI_VENDOR_ID_DLINK
+	#define PCI_VENDOR_ID_DLINK 0x1186
+#endif
+
+static struct pci_device_id rtl8180_pci_id_tbl[] __devinitdata = {
+        {
+                .vendor = PCI_VENDOR_ID_REALTEK,
+//                .device = 0x8180,
+                .device = 0x8199,
+                .subvendor = PCI_ANY_ID,
+                .subdevice = PCI_ANY_ID,
+                .driver_data = 0,
+        },
+#if 0
+        {
+                .vendor = PCI_VENDOR_ID_BELKIN,
+                .device = 0x6001,
+                .subvendor = PCI_ANY_ID,
+                .subdevice = PCI_ANY_ID,
+                .driver_data = 1,
+        },
+        {       /* Belkin F5D6020 v3 */
+	        .vendor = PCI_VENDOR_ID_BELKIN,
+                .device = 0x6020,
+                .subvendor = PCI_ANY_ID,
+                .subdevice = PCI_ANY_ID,
+                .driver_data = 2,
+        },
+        {       /* D-Link DWL-610 */
+                .vendor = PCI_VENDOR_ID_DLINK,
+                .device = 0x3300,
+                .subvendor = PCI_ANY_ID,
+                .subdevice = PCI_ANY_ID,
+                .driver_data = 3,
+        },
+	{
+		.vendor = PCI_VENDOR_ID_REALTEK,
+		.device = 0x8185,
+		.subvendor = PCI_ANY_ID,
+		.subdevice = PCI_ANY_ID,
+		.driver_data = 4,
+	},
+#endif
+        {
+                .vendor = 0,
+                .device = 0,
+                .subvendor = 0,
+                .subdevice = 0,
+                .driver_data = 0,
+        }
+};
+
+
+static char* ifname = "wlan%d";
+static int hwseqnum = 0;
+//static char* ifname = "ath%d";
+static int hwwep = 0;
+static int channels = 0x3fff;
+
+#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])
+MODULE_LICENSE("GPL");
+MODULE_DEVICE_TABLE(pci, rtl8180_pci_id_tbl);
+MODULE_AUTHOR("Andrea Merello <andreamrl@tiscali.it>");
+MODULE_DESCRIPTION("Linux driver for Realtek RTL8180 / RTL8185 WiFi cards");
+
+
+
+/*
+MODULE_PARM(ifname, "s");
+MODULE_PARM_DESC(devname," Net interface name, wlan%d=default");
+
+MODULE_PARM(hwseqnum,"i");
+MODULE_PARM_DESC(hwseqnum," Try to use hardware 802.11 header sequence numbers. Zero=default");
+
+MODULE_PARM(hwwep,"i");
+MODULE_PARM_DESC(hwwep," Try to use hardware WEP support. Still broken and not available on all cards");
+
+MODULE_PARM(channels,"i");
+MODULE_PARM_DESC(channels," Channel bitmask for specific locales. NYI");
+*/
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 9)
+module_param(ifname, charp, S_IRUGO|S_IWUSR );
+module_param(hwseqnum,int, S_IRUGO|S_IWUSR);
+module_param(hwwep,int, S_IRUGO|S_IWUSR);
+module_param(channels,int, S_IRUGO|S_IWUSR);
+#else
+MODULE_PARM(ifname, "s");
+MODULE_PARM(hwseqnum,"i");
+MODULE_PARM(hwwep,"i");
+MODULE_PARM(channels,"i");
+#endif
+
+MODULE_PARM_DESC(devname," Net interface name, wlan%d=default");
+//MODULE_PARM_DESC(devname," Net interface name, ath%d=default");
+MODULE_PARM_DESC(hwseqnum," Try to use hardware 802.11 header sequence numbers. Zero=default");
+MODULE_PARM_DESC(hwwep," Try to use hardware WEP support. Still broken and not available on all cards");
+MODULE_PARM_DESC(channels," Channel bitmask for specific locales. NYI");
+
+
+static int __devinit rtl8180_pci_probe(struct pci_dev *pdev,
+				       const struct pci_device_id *id);
+
+static void __devexit rtl8180_pci_remove(struct pci_dev *pdev);
+
+static void rtl8180_shutdown (struct pci_dev *pdev)
+{
+	struct net_device *dev = pci_get_drvdata(pdev);
+	dev->stop(dev);
+	pci_disable_device(pdev);
+}
+
+static struct pci_driver rtl8180_pci_driver = {
+	.name		= RTL8180_MODULE_NAME,	          /* Driver name   */
+	.id_table	= rtl8180_pci_id_tbl,	          /* PCI_ID table  */
+	.probe		= rtl8180_pci_probe,	          /* probe fn      */
+	.remove		= __devexit_p(rtl8180_pci_remove),/* remove fn     */
+#ifdef CONFIG_RTL8180_PM
+	.suspend	= rtl8180_suspend,	          /* PM suspend fn */
+	.resume		= rtl8180_resume,                 /* PM resume fn  */
+#else
+	.suspend	= NULL,			          /* PM suspend fn */
+	.resume      	= NULL,			          /* PM resume fn  */
+#endif
+	.shutdown	= rtl8180_shutdown,
+};
+
+
+
+#ifdef CONFIG_RTL8180_IO_MAP
+
+u8 read_nic_byte(struct net_device *dev, int x)
+{
+        return 0xff&inb(dev->base_addr +x);
+}
+
+u32 read_nic_dword(struct net_device *dev, int x)
+{
+        return inl(dev->base_addr +x);
+}
+
+u16 read_nic_word(struct net_device *dev, int x)
+{
+        return inw(dev->base_addr +x);
+}
+
+void write_nic_byte(struct net_device *dev, int x,u8 y)
+{
+        outb(y&0xff,dev->base_addr +x);
+}
+
+void write_nic_word(struct net_device *dev, int x,u16 y)
+{
+        outw(y,dev->base_addr +x);
+}
+
+void write_nic_dword(struct net_device *dev, int x,u32 y)
+{
+        outl(y,dev->base_addr +x);
+}
+
+#else /* RTL_IO_MAP */
+
+u8 read_nic_byte(struct net_device *dev, int x)
+{
+        return 0xff&readb((u8*)dev->mem_start +x);
+}
+
+u32 read_nic_dword(struct net_device *dev, int x)
+{
+        return readl((u8*)dev->mem_start +x);
+}
+
+u16 read_nic_word(struct net_device *dev, int x)
+{
+        return readw((u8*)dev->mem_start +x);
+}
+
+void write_nic_byte(struct net_device *dev, int x,u8 y)
+{
+        writeb(y,(u8*)dev->mem_start +x);
+	udelay(20);
+}
+
+void write_nic_dword(struct net_device *dev, int x,u32 y)
+{
+        writel(y,(u8*)dev->mem_start +x);
+	udelay(20);
+}
+
+void write_nic_word(struct net_device *dev, int x,u16 y)
+{
+        writew(y,(u8*)dev->mem_start +x);
+	udelay(20);
+}
+
+#endif /* RTL_IO_MAP */
+
+
+
+
+
+inline void force_pci_posting(struct net_device *dev)
+{
+	read_nic_byte(dev,EPROM_CMD);
+#ifndef CONFIG_RTL8180_IO_MAP
+	mb();
+#endif
+}
+
+
+irqreturn_t rtl8180_interrupt(int irq, void *netdev, struct pt_regs *regs);
+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);
+
+/****************************************************************************
+   -----------------------------PROCFS STUFF-------------------------
+*****************************************************************************/
+
+static struct proc_dir_entry *rtl8180_proc = NULL;
+
+static int proc_get_registers(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+//	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	int len = 0;
+	int i,n;
+
+	int max=0xff;
+
+	/* This dump the current register page */
+	for(n=0;n<=max;)
+	{
+		//printk( "\nD: %2x> ", n);
+		len += snprintf(page + len, count - len,
+			"\nD:  %2x > ",n);
+
+		for(i=0;i<16 && n<=max;i++,n++)
+		len += snprintf(page + len, count - len,
+			"%2x ",read_nic_byte(dev,n));
+
+		//	printk("%2x ",read_nic_byte(dev,n));
+	}
+	len += snprintf(page + len, count - len,"\n");
+
+
+
+	*eof = 1;
+	return len;
+
+}
+
+int get_curr_tx_free_desc(struct net_device *dev, int priority);
+
+static int proc_get_stats_hw(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	//struct net_device *dev = data;
+	//struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	int len = 0;
+#ifdef 	CONFIG_RTL8185B
+
+#else
+	len += snprintf(page + len, count - len,
+		"NIC int: %lu\n"
+		"Total int: %lu\n"
+		"--------------------\n"
+		"LP avail desc %d\n"
+		"NP avail desc %d\n"
+		"--------------------\n"
+		"LP phys dma addr %x\n"
+		"LP NIC ptr %x\n"
+		"LP virt 32base %x\n"
+		"LP virt 32tail %x\n"
+		"--------------------\n"
+		"NP phys dma addr %x\n"
+		"NP NIC ptr %x\n"
+		"NP virt 32base %x\n"
+		"NP virt 32tail %x\n"
+		"--------------------\n"
+		"BP phys dma addr %x\n"
+		"BP NIC ptr %x\n"
+		"BP virt 32base %x\n"
+		"BP virt 32tail %x\n",
+		priv->stats.ints,
+		priv->stats.shints,
+		get_curr_tx_free_desc(dev,LOW_PRIORITY),
+		get_curr_tx_free_desc(dev,NORM_PRIORITY),
+		(u32)priv->txvipringdma,
+		read_nic_dword(dev,TLPDA),
+		(u32)priv->txvipring,
+		(u32)priv->txvipringtail,
+		(u32)priv->txvopringdma,
+		read_nic_dword(dev,TNPDA),
+		(u32)priv->txvopring,
+		(u32)priv->txvopringtail,
+		(u32)priv->txbeaconringdma,
+		read_nic_dword(dev,TBDA),
+		(u32)priv->txbeaconring,
+		(u32)priv->txbeaconringtail);
+#endif
+	*eof = 1;
+	return len;
+}
+
+
+static int proc_get_stats_rx(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	int len = 0;
+
+	len += snprintf(page + len, count - len,
+	/*	"RX descriptor not available: %lu\n"
+		"RX incomplete (missing last descriptor): %lu\n"
+		"RX not data: %lu\n"
+		//"RX descriptor pointer reset: %lu\n"
+		"RX descriptor pointer lost: %lu\n"
+		//"RX pointer workaround: %lu\n"
+		"RX error int: %lu\n"
+		"RX fifo overflow: %lu\n"
+		"RX int: %lu\n"
+		"RX packet: %lu\n"
+		"RX bytes: %lu\n"
+		"RX DMA fail: %lu\n",
+		priv->stats.rxrdu,
+		priv->stats.rxnolast,
+		priv->stats.rxnodata,
+		//priv->stats.rxreset,
+		priv->stats.rxnopointer,
+		//priv->stats.rxwrkaround,
+		priv->stats.rxerr,
+		priv->stats.rxoverflow,
+		priv->stats.rxint,
+		priv->ieee80211->stats.rx_packets,
+		priv->ieee80211->stats.rx_bytes,
+		priv->stats.rxdmafail  */
+		"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
+		);
+
+	*eof = 1;
+	return len;
+}
+
+#if 0
+static int proc_get_stats_ieee(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	int len = 0;
+
+	len += snprintf(page + len, count - len,
+		"TXed association requests: %u\n"
+		"TXed authentication requests: %u\n"
+		"RXed successful association response: %u\n"
+		"RXed failed association response: %u\n"
+		"RXed successful authentication response: %u\n"
+		"RXed failed authentication response: %u\n"
+		"Association requests without response: %u\n"
+		"Authentication requests without response: %u\n"
+		"TX probe response: %u\n"
+		"RX probe request: %u\n"
+		"TX probe request: %lu\n"
+		"RX authentication requests: %lu\n"
+		"RX association requests: %lu\n"
+		"Reassociations: %lu\n",
+		priv->ieee80211->ieee_stats.tx_ass,
+		priv->ieee80211->ieee_stats.tx_aut,
+		priv->ieee80211->ieee_stats.rx_ass_ok,
+		priv->ieee80211->ieee_stats.rx_ass_err,
+		priv->ieee80211->ieee_stats.rx_aut_ok,
+		priv->ieee80211->ieee_stats.rx_aut_err,
+		priv->ieee80211->ieee_stats.ass_noresp,
+		priv->ieee80211->ieee_stats.aut_noresp,
+		priv->ieee80211->ieee_stats.tx_probe,
+		priv->ieee80211->ieee_stats.rx_probe,
+		priv->ieee80211->ieee_stats.tx_probe_rq,
+		priv->ieee80211->ieee_stats.rx_auth_rq,
+		priv->ieee80211->ieee_stats.rx_assoc_rq,
+		priv->ieee80211->ieee_stats.reassoc);
+
+	*eof = 1;
+	return len;
+}
+#endif
+#if 0
+static int proc_get_stats_ap(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct mac_htable_t *list;
+	int i;
+	int len = 0;
+
+	if(priv->ieee80211->iw_mode != IW_MODE_MASTER){
+		len += snprintf(page + len, count - len,
+		"Card is not acting as AP...\n"
+		);
+	}else{
+		len += snprintf(page + len, count - len,
+		"List of associated STA:\n"
+		);
+
+		for(i=0;i<MAC_HTABLE_ENTRY;i++)
+			for (list = priv->ieee80211->assoc_htable[i]; list!=NULL; list = list->next){
+				len += snprintf(page + len, count - len,
+					MACSTR"\n",MAC2STR(list->adr));
+			}
+
+	}
+	*eof = 1;
+	return len;
+}
+#endif
+
+static int proc_get_stats_tx(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	int len = 0;
+	unsigned long totalOK;
+
+	totalOK=priv->stats.txnpokint+priv->stats.txhpokint+priv->stats.txlpokint;
+	len += snprintf(page + len, count - len,
+	/*	"TX normal priority ok int: %lu\n"
+		"TX normal priority error int: %lu\n"
+		"TX high priority ok int: %lu\n"
+		"TX high priority failed error int: %lu\n"
+		"TX low priority ok int: %lu\n"
+		"TX low priority failed error int: %lu\n"
+		"TX bytes: %lu\n"
+		"TX packets: %lu\n"
+		"TX queue resume: %lu\n"
+		"TX queue stopped?: %d\n"
+		"TX fifo overflow: %lu\n"
+		//"SW TX stop: %lu\n"
+		//"SW TX wake: %lu\n"
+		"TX beacon: %lu\n"
+		"TX beacon aborted: %lu\n",
+		priv->stats.txnpokint,
+		priv->stats.txnperr,
+		priv->stats.txhpokint,
+		priv->stats.txhperr,
+		priv->stats.txlpokint,
+		priv->stats.txlperr,
+		priv->ieee80211->stats.tx_bytes,
+		priv->ieee80211->stats.tx_packets,
+		priv->stats.txresumed,
+		netif_queue_stopped(dev),
+		priv->stats.txoverflow,
+		//priv->ieee80211->ieee_stats.swtxstop,
+		//priv->ieee80211->ieee_stats.swtxawake,
+		priv->stats.txbeacon,
+		priv->stats.txbeaconerr  */
+		"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
+	);
+
+	*eof = 1;
+	return len;
+}
+
+
+#if WIRELESS_EXT < 17
+static struct iw_statistics *r8180_get_wireless_stats(struct net_device *dev)
+{
+       struct r8180_priv *priv = ieee80211_priv(dev);
+
+       return &priv->wstats;
+}
+#endif
+void rtl8180_proc_module_init(void)
+{
+	DMESG("Initializing proc filesystem");
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
+        rtl8180_proc=create_proc_entry(RTL8180_MODULE_NAME, S_IFDIR, proc_net);
+#else
+        rtl8180_proc=create_proc_entry(RTL8180_MODULE_NAME, S_IFDIR, init_net.proc_net);
+#endif
+}
+
+
+void rtl8180_proc_module_remove(void)
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
+        remove_proc_entry(RTL8180_MODULE_NAME, proc_net);
+#else
+        remove_proc_entry(RTL8180_MODULE_NAME, init_net.proc_net);
+#endif
+}
+
+
+void rtl8180_proc_remove_one(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	if (priv->dir_dev) {
+		remove_proc_entry("stats-hw", priv->dir_dev);
+		remove_proc_entry("stats-tx", priv->dir_dev);
+		remove_proc_entry("stats-rx", priv->dir_dev);
+//		remove_proc_entry("stats-ieee", priv->dir_dev);
+//		remove_proc_entry("stats-ap", priv->dir_dev);
+		remove_proc_entry("registers", priv->dir_dev);
+		remove_proc_entry(dev->name, rtl8180_proc);
+		priv->dir_dev = NULL;
+	}
+}
+
+
+void rtl8180_proc_init_one(struct net_device *dev)
+{
+	struct proc_dir_entry *e;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	priv->dir_dev = create_proc_entry(dev->name,
+					  S_IFDIR | S_IRUGO | S_IXUGO,
+					  rtl8180_proc);
+	if (!priv->dir_dev) {
+		DMESGE("Unable to initialize /proc/net/rtl8180/%s\n",
+		      dev->name);
+		return;
+	}
+
+	e = create_proc_read_entry("stats-hw", S_IFREG | S_IRUGO,
+				   priv->dir_dev, proc_get_stats_hw, dev);
+
+	if (!e) {
+		DMESGE("Unable to initialize "
+		      "/proc/net/rtl8180/%s/stats-hw\n",
+		      dev->name);
+	}
+
+	e = create_proc_read_entry("stats-rx", S_IFREG | S_IRUGO,
+				   priv->dir_dev, proc_get_stats_rx, dev);
+
+	if (!e) {
+		DMESGE("Unable to initialize "
+		      "/proc/net/rtl8180/%s/stats-rx\n",
+		      dev->name);
+	}
+
+
+	e = create_proc_read_entry("stats-tx", S_IFREG | S_IRUGO,
+				   priv->dir_dev, proc_get_stats_tx, dev);
+
+	if (!e) {
+		DMESGE("Unable to initialize "
+		      "/proc/net/rtl8180/%s/stats-tx\n",
+		      dev->name);
+	}
+	#if 0
+	e = create_proc_read_entry("stats-ieee", S_IFREG | S_IRUGO,
+				   priv->dir_dev, proc_get_stats_ieee, dev);
+
+	if (!e) {
+		DMESGE("Unable to initialize "
+		      "/proc/net/rtl8180/%s/stats-ieee\n",
+		      dev->name);
+	}
+	#endif
+	#if 0
+	e = create_proc_read_entry("stats-ap", S_IFREG | S_IRUGO,
+				   priv->dir_dev, proc_get_stats_ap, dev);
+
+	if (!e) {
+		DMESGE("Unable to initialize "
+		      "/proc/net/rtl8180/%s/stats-ap\n",
+		      dev->name);
+	}
+	#endif
+
+	e = create_proc_read_entry("registers", S_IFREG | S_IRUGO,
+				   priv->dir_dev, proc_get_registers, dev);
+
+	if (!e) {
+		DMESGE("Unable to initialize "
+		      "/proc/net/rtl8180/%s/registers\n",
+		      dev->name);
+	}
+}
+/****************************************************************************
+   -----------------------------MISC STUFF-------------------------
+*****************************************************************************/
+/*
+  FIXME: check if we can use some standard already-existent
+  data type+functions in kernel
+*/
+
+short buffer_add(struct buffer **buffer, u32 *buf, dma_addr_t dma,
+		struct buffer **bufferhead)
+{
+#ifdef DEBUG_RING
+	DMESG("adding buffer to TX/RX struct");
+#endif
+
+        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;
+	if ((tmp->next= kmalloc(sizeof(struct buffer),GFP_KERNEL)) == NULL){
+		DMESGE("Failed to kmalloc TX/RX struct");
+		return -1;
+	}
+	tmp->next->buf=buf;
+	tmp->next->dma=dma;
+	tmp->next->next=*buffer;
+
+	return 0;
+}
+
+
+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;
+	//int i;
+
+	if(! *buffer) return;
+
+	/*for(tmp=*buffer; tmp->next != *buffer; tmp=tmp->next)
+
+	*/
+	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;
+}
+
+
+void print_buffer(u32 *buffer, int len)
+{
+	int i;
+	u8 *buf =(u8*)buffer;
+
+	printk("ASCII BUFFER DUMP (len: %x):\n",len);
+
+	for(i=0;i<len;i++)
+		printk("%c",buf[i]);
+
+	printk("\nBINARY BUFFER DUMP (len: %x):\n",len);
+
+	for(i=0;i<len;i++)
+		printk("%02x",buf[i]);
+
+	printk("\n");
+}
+
+
+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;
+	}
+
+	//DMESG("%x %x", head, tail);
+
+	/* FIXME FIXME FIXME FIXME */
+
+#if 0
+	if( head <= tail ) return priv->txringcount-1 - (tail - head)/8;
+	return (head - tail)/8/4;
+#else
+	if( head <= tail )
+		ret = priv->txringcount - (tail - head)/8;
+	else
+		ret = (head - tail)/8;
+
+	if(ret > priv->txringcount ) DMESG("BUG");
+	return ret;
+#endif
+}
+
+
+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, 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));
+}
+
+
+/* This function is only for debuging purpose */
+void check_tx_ring(struct net_device *dev, int pri)
+{
+	static int maxlog =3;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u32* tmp;
+	struct buffer *buf;
+	int i;
+	int nic;
+	u32* tail;
+	u32* head;
+	u32* begin;
+	u32 nicbegin;
+	struct buffer* buffer;
+
+	maxlog --;
+	if (maxlog <0 ) return;
+
+	switch(pri) {
+	case MANAGE_PRIORITY:
+		tail = priv->txmapringtail;
+		begin = priv->txmapring;
+		head = priv->txmapringhead;
+		nic = read_nic_dword(dev,TX_MANAGEPRIORITY_RING_ADDR);
+		buffer = priv->txmapbufs;
+		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);
+		buffer = priv->txbkpbufs;
+		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);
+		buffer = priv->txbepbufs;
+		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);
+		buffer = priv->txvipbufs;
+		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);
+		buffer = priv->txvopbufs;
+		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);
+		buffer = priv->txhpbufs;
+		nicbegin = priv->txhpringdma;
+		break;
+
+	default:
+		return ;
+		break;
+	}
+
+	if(!priv->txvopbufs)
+		DMESGE ("NIC TX ack, but TX queue corrupted!");
+	else{
+
+		for(i=0,buf=buffer, tmp=begin;
+			tmp<begin+(priv->txringcount)*8;
+			tmp+=8,buf=buf->next,i++)
+
+			DMESG("BUF%d %s %x %s. Next : %x",i,
+			      *tmp & (1<<31) ? "filled" : "empty",
+			      *(buf->buf),
+			      *tmp & (1<<15)? "ok": "err", *(tmp+4));
+	}
+
+	DMESG("nic at %d",
+		(nic-nicbegin) / 8 /4);
+	DMESG("tail at %d", ((int)tail - (int)begin) /8 /4);
+	DMESG("head at %d", ((int)head - (int)begin) /8 /4);
+	DMESG("check free desc returns %d", check_nic_enought_desc(dev,pri));
+	DMESG("free desc is %d\n", get_curr_tx_free_desc(dev,pri));
+	//rtl8180_reset(dev);
+	return;
+}
+
+
+
+/* this function is only for debugging purpose */
+void check_rxbuf(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u32* tmp;
+	struct buffer *buf;
+	u8 rx_desc_size;
+
+#ifdef CONFIG_RTL8185B
+	rx_desc_size = 8;
+#else
+	rx_desc_size = 4;
+#endif
+
+	if(!priv->rxbuffer)
+		DMESGE ("NIC RX ack, but RX queue corrupted!");
+
+	else{
+
+		for(buf=priv->rxbuffer, tmp=priv->rxring;
+		    tmp < priv->rxring+(priv->rxringcount)*rx_desc_size;
+		    tmp+=rx_desc_size, buf=buf->next)
+
+			DMESG("BUF %s %x",
+			      *tmp & (1<<31) ? "empty" : "filled",
+			      *(buf->buf));
+	}
+
+	return;
+}
+
+
+void dump_eprom(struct net_device *dev)
+{
+	int i;
+	for(i=0; i<63; i++)
+		DMESG("EEPROM addr %x : %x", i, eprom_read(dev,i));
+}
+
+
+void rtl8180_dump_reg(struct net_device *dev)
+{
+	int i;
+	int n;
+	int max=0xff;
+
+	DMESG("Dumping NIC register map");
+
+	for(n=0;n<=max;)
+	{
+		printk( "\nD: %2x> ", n);
+		for(i=0;i<16 && n<=max;i++,n++)
+			printk("%2x ",read_nic_byte(dev,n));
+	}
+	printk("\n");
+}
+
+
+void fix_tx_fifo(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u32 *tmp;
+	int i;
+#ifdef DEBUG_TX_ALLOC
+	DMESG("FIXING TX FIFOs");
+#endif
+	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);
+	}
+#ifdef DEBUG_TX_ALLOC
+	DMESG("TX FIFOs FIXED");
+#endif
+	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;
+
+#ifdef CONFIG_RTL8185B
+	rx_desc_size = 8; // 4*8 = 32 bytes
+#else
+	rx_desc_size = 4;
+#endif
+
+#ifdef DEBUG_RXALLOC
+	DMESG("FIXING RX FIFO");
+	check_rxbuf(dev);
+#endif
+
+	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);
+	}
+
+#ifdef DEBUG_RXALLOC
+	DMESG("RX FIFO FIXED");
+	check_rxbuf(dev);
+#endif
+
+	priv->rxringtail=priv->rxring;
+	priv->rxbuffer=priv->rxbufferhead;
+	priv->rx_skb_complete=1;
+	set_nic_rxring(dev);
+}
+
+
+/****************************************************************************
+      ------------------------------HW STUFF---------------------------
+*****************************************************************************/
+
+unsigned char QUALITY_MAP[] = {
+  0x64, 0x64, 0x64, 0x63, 0x63, 0x62, 0x62, 0x61,
+  0x61, 0x60, 0x60, 0x5f, 0x5f, 0x5e, 0x5d, 0x5c,
+  0x5b, 0x5a, 0x59, 0x57, 0x56, 0x54, 0x52, 0x4f,
+  0x4c, 0x49, 0x45, 0x41, 0x3c, 0x37, 0x31, 0x29,
+  0x24, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22,
+  0x22, 0x22, 0x21, 0x21, 0x21, 0x21, 0x21, 0x20,
+  0x20, 0x20, 0x20, 0x1f, 0x1f, 0x1e, 0x1e, 0x1e,
+  0x1d, 0x1d, 0x1c, 0x1c, 0x1b, 0x1a, 0x19, 0x19,
+  0x18, 0x17, 0x16, 0x15, 0x14, 0x12, 0x11, 0x0f,
+  0x0e, 0x0c, 0x0a, 0x08, 0x06, 0x04, 0x01, 0x00
+};
+
+unsigned char STRENGTH_MAP[] = {
+  0x64, 0x64, 0x63, 0x62, 0x61, 0x60, 0x5f, 0x5e,
+  0x5d, 0x5c, 0x5b, 0x5a, 0x57, 0x54, 0x52, 0x50,
+  0x4e, 0x4c, 0x4a, 0x48, 0x46, 0x44, 0x41, 0x3f,
+  0x3c, 0x3a, 0x37, 0x36, 0x36, 0x1c, 0x1c, 0x1b,
+  0x1b, 0x1a, 0x1a, 0x19, 0x19, 0x18, 0x18, 0x17,
+  0x17, 0x16, 0x16, 0x15, 0x15, 0x14, 0x14, 0x13,
+  0x13, 0x12, 0x12, 0x11, 0x11, 0x10, 0x10, 0x0f,
+  0x0f, 0x0e, 0x0e, 0x0d, 0x0d, 0x0c, 0x0c, 0x0b,
+  0x0b, 0x0a, 0x0a, 0x09, 0x09, 0x08, 0x08, 0x07,
+  0x07, 0x06, 0x06, 0x05, 0x04, 0x03, 0x02, 0x00
+};
+
+void rtl8180_RSSI_calc(struct net_device *dev, u8 *rssi, u8 *qual){
+	//void Mlme_UpdateRssiSQ(struct net_device *dev, u8 *rssi, u8 *qual){
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u32 temp;
+	u32 temp2;
+	u32 temp3;
+	u32 lsb;
+	u32 q;
+	u32 orig_qual;
+	u8  _rssi;
+
+	q = *qual;
+	orig_qual = *qual;
+	_rssi = 0; // avoid gcc complains..
+
+	if (q <= 0x4e) {
+		temp = QUALITY_MAP[q];
+	} else {
+		if( q & 0x80 ) {
+			temp = 0x32;
+		} else {
+			temp = 1;
+		}
+	}
+
+	*qual = temp;
+	temp2 = *rssi;
+
+	switch(priv->rf_chip){
+	case RFCHIPID_RFMD:
+		lsb = temp2 & 1;
+		temp2 &= 0x7e;
+		if ( !lsb || !(temp2 <= 0x3c) ) {
+			temp2 = 0x64;
+		} else {
+			temp2 = 100 * temp2 / 0x3c;
+		}
+		*rssi = temp2 & 0xff;
+		_rssi = temp2 & 0xff;
+		break;
+	case RFCHIPID_INTERSIL:
+		lsb = temp2;
+		temp2 &= 0xfffffffe;
+		temp2 *= 251;
+		temp3 = temp2;
+		temp2 <<= 6;
+		temp3 += temp2;
+		temp3 <<= 1;
+		temp2 = 0x4950df;
+		temp2 -= temp3;
+		lsb &= 1;
+		if ( temp2 <= 0x3e0000 ) {
+			if ( temp2 < 0xffef0000 )
+				temp2 = 0xffef0000;
+		} else {
+			temp2 = 0x3e0000;
+		}
+		if ( !lsb ) {
+			temp2 -= 0xf0000;
+		} else {
+			temp2 += 0xf0000;
+		}
+
+		temp3 = 0x4d0000;
+		temp3 -= temp2;
+		temp3 *= 100;
+		temp3 = temp3 / 0x6d;
+		temp3 >>= 0x10;
+		_rssi = temp3 & 0xff;
+		*rssi = temp3 & 0xff;
+		break;
+	case RFCHIPID_GCT:
+	        lsb = temp2 & 1;
+		temp2 &= 0x7e;
+		if ( ! lsb || !(temp2 <= 0x3c) ){
+			temp2 = 0x64;
+		} else {
+			temp2 = (100 * temp2) / 0x3c;
+		}
+		*rssi = temp2 & 0xff;
+		_rssi = temp2 & 0xff;
+		break;
+	case RFCHIPID_PHILIPS:
+		if( orig_qual <= 0x4e ){
+			_rssi = STRENGTH_MAP[orig_qual];
+			*rssi = _rssi;
+		} else {
+			orig_qual -= 0x80;
+			if ( !orig_qual ){
+				_rssi = 1;
+				*rssi = 1;
+			} else {
+				_rssi = 0x32;
+				*rssi = 0x32;
+			}
+		}
+		break;
+
+	/* case 4 */
+	case RFCHIPID_MAXIM:
+		lsb = temp2 & 1;
+		temp2 &= 0x7e;
+		temp2 >>= 1;
+		temp2 += 0x42;
+		if( lsb != 0 ){
+			temp2 += 0xa;
+		}
+		*rssi = temp2 & 0xff;
+		_rssi = temp2 & 0xff;
+		break;
+	}
+
+	if ( _rssi < 0x64 ){
+		if ( _rssi == 0 ) {
+			*rssi = 1;
+		}
+	} else {
+		*rssi = 0x64;
+	}
+
+	return;
+}
+
+
+void rtl8180_irq_enable(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	priv->irq_enabled = 1;
+/*
+	write_nic_word(dev,INTA_MASK,INTA_RXOK | INTA_RXDESCERR | INTA_RXOVERFLOW |\
+	INTA_TXOVERFLOW | INTA_HIPRIORITYDESCERR | INTA_HIPRIORITYDESCOK |\
+	INTA_NORMPRIORITYDESCERR | INTA_NORMPRIORITYDESCOK |\
+	INTA_LOWPRIORITYDESCERR | INTA_LOWPRIORITYDESCOK | INTA_TIMEOUT);
+*/
+	write_nic_word(dev,INTA_MASK, priv->irq_mask);
+}
+
+
+void rtl8180_irq_disable(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+#ifdef CONFIG_RTL8185B
+	write_nic_dword(dev,IMR,0);
+#else
+	write_nic_word(dev,INTA_MASK,0);
+#endif
+	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_adapter_start(struct net_device *dev);
+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))
+	{
+		printk("In %s: Invalid chnanel %d\n", __func__, ch);
+		return;
+	}
+
+	priv->chan=ch;
+	//printk("in %s:channel is %d\n",__func__,ch);
+	priv->rf_set_chan(dev,priv->chan);
+
+}
+
+
+void rtl8180_rx_enable(struct net_device *dev)
+{
+	u8 cmd;
+	u32 rxconf;
+	/* for now we accept data, management & ctl frame*/
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	rxconf=read_nic_dword(dev,RX_CONF);
+	rxconf = rxconf &~ MAC_FILTER_MASK;
+	rxconf = rxconf | (1<<ACCEPT_MNG_FRAME_SHIFT);
+	rxconf = rxconf | (1<<ACCEPT_DATA_FRAME_SHIFT);
+	rxconf = rxconf | (1<<ACCEPT_BCAST_FRAME_SHIFT);
+	rxconf = rxconf | (1<<ACCEPT_MCAST_FRAME_SHIFT);
+//	rxconf = rxconf | (1<<ACCEPT_CRCERR_FRAME_SHIFT);
+	if (dev->flags & IFF_PROMISC) DMESG ("NIC in promisc mode");
+
+	if(priv->ieee80211->iw_mode == IW_MODE_MONITOR || \
+	   dev->flags & IFF_PROMISC){
+		rxconf = rxconf | (1<<ACCEPT_ALLMAC_FRAME_SHIFT);
+	}else{
+		rxconf = rxconf | (1<<ACCEPT_NICMAC_FRAME_SHIFT);
+		if(priv->card_8185 == 0)
+			rxconf = rxconf | (1<<RX_CHECK_BSSID_SHIFT);
+	}
+
+	/*if(priv->ieee80211->iw_mode == IW_MODE_MASTER){
+		rxconf = rxconf | (1<<ACCEPT_ALLMAC_FRAME_SHIFT);
+		rxconf = rxconf | (1<<RX_CHECK_BSSID_SHIFT);
+	}*/
+
+	if(priv->ieee80211->iw_mode == IW_MODE_MONITOR){
+		rxconf = rxconf | (1<<ACCEPT_CTL_FRAME_SHIFT);
+		rxconf = rxconf | (1<<ACCEPT_ICVERR_FRAME_SHIFT);
+		rxconf = rxconf | (1<<ACCEPT_PWR_FRAME_SHIFT);
+	}
+
+	if( priv->crcmon == 1 && priv->ieee80211->iw_mode == IW_MODE_MONITOR)
+		rxconf = rxconf | (1<<ACCEPT_CRCERR_FRAME_SHIFT);
+
+	//if(!priv->card_8185){
+		rxconf = rxconf &~ RX_FIFO_THRESHOLD_MASK;
+		rxconf = rxconf | (RX_FIFO_THRESHOLD_NONE<<RX_FIFO_THRESHOLD_SHIFT);
+	//}
+
+	rxconf = rxconf | (1<<RX_AUTORESETPHY_SHIFT);
+	rxconf = rxconf &~ MAX_RX_DMA_MASK;
+	rxconf = rxconf | (MAX_RX_DMA_2048<<MAX_RX_DMA_SHIFT);
+
+	//if(!priv->card_8185)
+		rxconf = rxconf | RCR_ONLYERLPKT;
+
+	rxconf = rxconf &~ RCR_CS_MASK;
+	if(!priv->card_8185)
+		rxconf |= (priv->rcr_csense<<RCR_CS_SHIFT);
+//	rxconf &=~ 0xfff00000;
+//	rxconf |= 0x90100000;//9014f76f;
+	write_nic_dword(dev, RX_CONF, rxconf);
+
+	fix_rx_fifo(dev);
+
+#ifdef DEBUG_RX
+	DMESG("rxconf: %x %x",rxconf ,read_nic_dword(dev,RX_CONF));
+#endif
+	cmd=read_nic_byte(dev,CMD);
+	write_nic_byte(dev,CMD,cmd | (1<<CMD_RX_ENABLE_SHIFT));
+
+	/* In rtl8139 driver seems that DMA threshold has to be written
+	 *  after enabling RX, so we rewrite RX_CONFIG register
+	 */
+	//mdelay(100);
+//	write_nic_dword(dev, RX_CONF, rxconf);
+
+}
+
+
+void set_nic_txring(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+//		DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+
+	write_nic_dword(dev, TX_MANAGEPRIORITY_RING_ADDR, priv->txmapringdma);
+//		DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+	write_nic_dword(dev, TX_BKPRIORITY_RING_ADDR, priv->txbkpringdma);
+//		DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+	write_nic_dword(dev, TX_BEPRIORITY_RING_ADDR, priv->txbepringdma);
+//		DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+	write_nic_dword(dev, TX_VIPRIORITY_RING_ADDR, priv->txvipringdma);
+//		DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+	write_nic_dword(dev, TX_VOPRIORITY_RING_ADDR, priv->txvopringdma);
+//		DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+	write_nic_dword(dev, TX_HIGHPRIORITY_RING_ADDR, priv->txhpringdma);
+//		DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+
+	write_nic_dword(dev, TX_BEACON_RING_ADDR, priv->txbeaconringdma);
+}
+
+
+void rtl8180_conttx_enable(struct net_device *dev)
+{
+	u32 txconf;
+	txconf = read_nic_dword(dev,TX_CONF);
+	txconf = txconf &~ TX_LOOPBACK_MASK;
+	txconf = txconf | (TX_LOOPBACK_CONTINUE <<TX_LOOPBACK_SHIFT);
+	write_nic_dword(dev,TX_CONF,txconf);
+}
+
+
+void rtl8180_conttx_disable(struct net_device *dev)
+{
+	u32 txconf;
+	txconf = read_nic_dword(dev,TX_CONF);
+	txconf = txconf &~ TX_LOOPBACK_MASK;
+	txconf = txconf | (TX_LOOPBACK_NONE <<TX_LOOPBACK_SHIFT);
+	write_nic_dword(dev,TX_CONF,txconf);
+}
+
+
+void rtl8180_tx_enable(struct net_device *dev)
+{
+	u8 cmd;
+	u8 tx_agc_ctl;
+	u8 byte;
+	u32 txconf;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	txconf= read_nic_dword(dev,TX_CONF);
+
+
+	if(priv->card_8185){
+
+
+		byte = read_nic_byte(dev,CW_CONF);
+		byte &= ~(1<<CW_CONF_PERPACKET_CW_SHIFT);
+		byte &= ~(1<<CW_CONF_PERPACKET_RETRY_SHIFT);
+		write_nic_byte(dev, CW_CONF, byte);
+
+		tx_agc_ctl = read_nic_byte(dev, TX_AGC_CTL);
+		tx_agc_ctl &= ~(1<<TX_AGC_CTL_PERPACKET_GAIN_SHIFT);
+		tx_agc_ctl &= ~(1<<TX_AGC_CTL_PERPACKET_ANTSEL_SHIFT);
+		tx_agc_ctl |=(1<<TX_AGC_CTL_FEEDBACK_ANT);
+		write_nic_byte(dev, TX_AGC_CTL, tx_agc_ctl);
+		/*
+		write_nic_word(dev, 0x5e, 0x01);
+		force_pci_posting(dev);
+		mdelay(1);
+		write_nic_word(dev, 0xfe, 0x10);
+		force_pci_posting(dev);
+		mdelay(1);
+		write_nic_word(dev, 0x5e, 0x00);
+		force_pci_posting(dev);
+		mdelay(1);
+		*/
+		write_nic_byte(dev, 0xec, 0x3f); /* Disable early TX */
+	}
+
+	if(priv->card_8185){
+
+		txconf = txconf &~ (1<<TCR_PROBE_NOTIMESTAMP_SHIFT);
+
+	}else{
+
+		if(hwseqnum)
+			txconf= txconf &~ (1<<TX_CONF_HEADER_AUTOICREMENT_SHIFT);
+		else
+			txconf= txconf | (1<<TX_CONF_HEADER_AUTOICREMENT_SHIFT);
+	}
+
+	txconf = txconf &~ TX_LOOPBACK_MASK;
+	txconf = txconf | (TX_LOOPBACK_NONE <<TX_LOOPBACK_SHIFT);
+	txconf = txconf &~ TCR_DPRETRY_MASK;
+	txconf = txconf &~ TCR_RTSRETRY_MASK;
+	txconf = txconf | (priv->retry_data<<TX_DPRETRY_SHIFT);
+	txconf = txconf | (priv->retry_rts<<TX_RTSRETRY_SHIFT);
+	txconf = txconf &~ (1<<TX_NOCRC_SHIFT);
+
+	if(priv->card_8185){
+		if(priv->hw_plcp_len)
+			txconf = txconf &~ TCR_PLCP_LEN;
+		else
+			txconf = txconf | TCR_PLCP_LEN;
+	}else{
+		txconf = txconf &~ TCR_SAT;
+	}
+	txconf = txconf &~ TCR_MXDMA_MASK;
+	txconf = txconf | (TCR_MXDMA_2048<<TCR_MXDMA_SHIFT);
+	txconf = txconf | TCR_CWMIN;
+	txconf = txconf | TCR_DISCW;
+
+//	if(priv->ieee80211->hw_wep)
+//		txconf=txconf &~ (1<<TX_NOICV_SHIFT);
+//	else
+		txconf=txconf | (1<<TX_NOICV_SHIFT);
+
+	write_nic_dword(dev,TX_CONF,txconf);
+
+
+	fix_tx_fifo(dev);
+
+#ifdef DEBUG_TX
+	DMESG("txconf: %x %x",txconf,read_nic_dword(dev,TX_CONF));
+#endif
+
+	cmd=read_nic_byte(dev,CMD);
+	write_nic_byte(dev,CMD,cmd | (1<<CMD_TX_ENABLE_SHIFT));
+
+//	mdelay(100);
+	write_nic_dword(dev,TX_CONF,txconf);
+//	#endif
+/*
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+	write_nic_byte(dev, TX_DMA_POLLING, priv->dma_poll_mask);
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+	*/
+}
+
+
+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);
+#ifdef CONFIG_RTL8185B
+	priv->dma_poll_stop_mask &= ~(TPPOLLSTOP_BQ);
+	write_nic_byte(dev,TPPollStop, priv->dma_poll_mask);
+#else
+	priv->dma_poll_mask &=~(1<<TX_DMA_STOP_BEACON_SHIFT);
+	write_nic_byte(dev,TX_DMA_POLLING,priv->dma_poll_mask);
+#endif
+	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);
+#ifdef CONFIG_RTL8185B
+	priv->dma_poll_stop_mask |= TPPOLLSTOP_BQ;
+	write_nic_byte(dev,TPPollStop, priv->dma_poll_stop_mask);
+#else
+	priv->dma_poll_mask |= (1<<TX_DMA_STOP_BEACON_SHIFT);
+	write_nic_byte(dev,TX_DMA_POLLING,priv->dma_poll_mask);
+#endif
+	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);
+	/*while (read_nic_byte(dev,CMD) & (1<<CMD_RX_ENABLE_SHIFT))
+	  udelay(10);
+	*/
+
+	if(!priv->rx_skb_complete)
+		dev_kfree_skb_any(priv->rx_skb);
+}
+
+#if 0
+int alloc_tx_beacon_desc_ring(struct net_device *dev, int count)
+{
+	int i;
+	u32 *tmp;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	priv->txbeaconring = (u32*)pci_alloc_consistent(priv->pdev,
+					  sizeof(u32)*8*count,
+					  &priv->txbeaconringdma);
+	if (!priv->txbeaconring) return -1;
+	for (tmp=priv->txbeaconring,i=0;i<count;i++){
+		*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)priv->txbeaconringdma+((i+1)*8*4);
+		else
+			*(tmp+4) = (u32)priv->txbeaconringdma;
+
+		tmp=tmp+8;
+	}
+	return 0;
+}
+#endif
+
+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 !
+		 */
+		DMESGW("Fixing TX alignment");
+		desc = (u32*)((u8*)desc + 256);
+#if (defined(CONFIG_HIGHMEM64G) || defined(CONFIG_64BIT_PHYS_ADDR))
+		desc = (u32*)((u64)desc &~ 0xff);
+		dma_desc = (dma_addr_t)((u8*)dma_desc + 256);
+		dma_desc = (dma_addr_t)((u64)dma_desc &~ 0xff);
+#else
+		desc = (u32*)((u32)desc &~ 0xff);
+		dma_desc = (dma_addr_t)((u8*)dma_desc + 256);
+		dma_desc = (dma_addr_t)((u32)dma_desc &~ 0xff);
+#endif
+	}
+	tmp=desc;
+	for (i=0;i<count;i++)
+	{
+		buf = (void*)pci_alloc_consistent(pdev,bufsize,&dma_tmp);
+		if (buf == NULL) return -ENOMEM;
+
+		switch(addr) {
+#if 0
+		case TX_NORMPRIORITY_RING_ADDR:
+			if(-1 == buffer_add(&(priv->txnpbufs),buf,dma_tmp,NULL)){
+				DMESGE("Unable to allocate mem for buffer NP");
+				return -ENOMEM;
+			}
+			break;
+
+		case TX_LOWPRIORITY_RING_ADDR:
+			if(-1 == buffer_add(&(priv->txlpbufs),buf,dma_tmp,NULL)){
+				DMESGE("Unable to allocate mem for buffer LP");
+				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;
+#else
+		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;
+#endif
+		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;
+
+	}
+
+#ifdef DEBUG_TX
+	DMESG("Tx dma physical address: %x",dma_desc);
+#endif
+
+	return 0;
+}
+
+
+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);
+}
+
+#if 0
+void free_beacon_desc_ring(struct net_device *dev,int count)
+{
+
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct pci_dev *pdev=priv->pdev;
+
+	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
+			    priv->txbeaconring, priv->txbeaconringdma);
+
+	if (priv->beacon_buf)
+		pci_free_consistent(priv->pdev,
+			priv->master_beaconsize,priv->beacon_buf,priv->beacondmabuf);
+
+}
+#endif
+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;
+
+#ifdef CONFIG_RTL8185B
+	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
+			    priv->rxring, priv->rxringdma);
+#else
+	pci_free_consistent(pdev, sizeof(u32)*4*count+256,
+			    priv->rxring, priv->rxringdma);
+#endif
+
+	buffer_free(dev,&(priv->rxbuffer),priv->rxbuffersize,0);
+}
+
+
+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;
+
+#ifdef CONFIG_RTL8185B
+	rx_desc_size = 8; // 4*8 = 32 bytes
+#else
+	rx_desc_size = 4;
+#endif
+
+	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
+		 */
+
+		DMESGW("Fixing RX alignment");
+		desc = (u32*)((u8*)desc + 256);
+#if (defined(CONFIG_HIGHMEM64G) || defined(CONFIG_64BIT_PHYS_ADDR))
+		desc = (u32*)((u64)desc &~ 0xff);
+		dma_desc = (dma_addr_t)((u8*)dma_desc + 256);
+		dma_desc = (dma_addr_t)((u64)dma_desc &~ 0xff);
+#else
+		desc = (u32*)((u32)desc &~ 0xff);
+		dma_desc = (dma_addr_t)((u8*)dma_desc + 256);
+		dma_desc = (dma_addr_t)((u32)dma_desc &~ 0xff);
+#endif
+	}
+
+	priv->rxring=desc;
+	priv->rxringdma=dma_desc;
+	tmp=desc;
+
+	for (i=0;i<count;i++){
+
+		if ((buf= kmalloc(bufsize * sizeof(u8),GFP_ATOMIC)) == NULL){
+			DMESGE("Failed to kmalloc RX buffer");
+			return -1;
+		}
+
+		dma_tmp = pci_map_single(pdev,buf,bufsize * sizeof(u8),
+					 PCI_DMA_FROMDEVICE);
+
+#ifdef DEBUG_ZERO_RX
+		int j;
+		for(j=0;j<bufsize;j++) ((u8*)buf)[i] = 0;
+#endif
+
+		//buf = (void*)pci_alloc_consistent(pdev,bufsize,&dma_tmp);
+		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
+
+#ifdef DEBUG_RXALLOC
+		DMESG("Alloc %x size buffer, DMA mem @ %x, virtual mem @ %x",
+		      (u32)(bufsize&0xfff), (u32)dma_tmp, (u32)buf);
+#endif
+
+		tmp=tmp+rx_desc_size;
+	}
+
+	*(tmp-rx_desc_size) = *(tmp-rx_desc_size) | (1<<30); // this is the last descriptor
+
+
+#ifdef DEBUG_RXALLOC
+	DMESG("RX DMA physical address: %x",dma_desc);
+#endif
+
+	return 0;
+}
+
+
+void set_nic_rxring(struct net_device *dev)
+{
+	u8 pgreg;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	//rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
+
+	pgreg=read_nic_byte(dev, PGSELECT);
+	write_nic_byte(dev, PGSELECT, pgreg &~ (1<<PGSELECT_PG_SHIFT));
+
+	//rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
+
+	write_nic_dword(dev, RXRING_ADDR,priv->rxringdma);
+}
+
+
+void rtl8180_reset(struct net_device *dev)
+{
+	//u32 txconf = 0x80e00707; //FIXME: Make me understandable
+	u8 cr;
+
+	//write_nic_dword(dev,TX_CONF,txconf);
+
+	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");
+
+//#ifndef CONFIG_RTL8185B
+	rtl8180_set_mode(dev,EPROM_CMD_LOAD);
+	force_pci_posting(dev);
+	mdelay(200);
+//#endif
+}
+
+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);
+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;
+}
+
+//{by amy 080312
+//
+//	Description:
+// 	For Netgear case, they want good-looking singal strength.
+//		2004.12.05, by rcnjko.
+//
+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;
+	}
+	//RT_TRACE(COMP_DBG, DBG_LOUD, ("##### After Mapping:  LastSS: %d, CurrSS: %d, RetSS: %d\n", LastSS, CurrSS, RetSS));
+
+	// Step 2. Smoothing.
+	if(LastSS > 0)
+	{
+		RetSS = ((LastSS * 5) + (RetSS)+ 5) / 6;
+	}
+	//RT_TRACE(COMP_DBG, DBG_LOUD, ("$$$$$ After Smoothing:  LastSS: %d, CurrSS: %d, RetSS: %d\n", LastSS, CurrSS, RetSS));
+
+	return RetSS;
+}
+//
+//	Description:
+//		Translate 0-100 signal strength index into dBm.
+//
+long
+TranslateToDbm8185(
+	u8 SignalStrengthIndex	// 0-100 index.
+	)
+{
+	long	SignalPower; // in dBm.
+
+	// Translate to dBm (x=0.5y-95).
+	SignalPower = (long)((SignalStrengthIndex + 1) >> 1);
+	SignalPower -= 95;
+
+	return SignalPower;
+}
+//
+//	Description:
+//		Perform signal smoothing for dynamic mechanism.
+//		This is different with PerformSignalSmoothing8185 in smoothing fomula.
+//		No dramatic adjustion is apply because dynamic mechanism need some degree
+//		of correctness. Ported from 8187B.
+//	2007-02-26, by Bruce.
+//
+void
+PerformUndecoratedSignalSmoothing8185(
+	struct r8180_priv *priv,
+	bool    bCckRate
+	)
+{
+
+
+	// Determin the current packet is CCK rate.
+	priv->bCurCCKPkt = bCckRate;
+
+	if(priv->UndecoratedSmoothedSS >= 0)
+	{
+		priv->UndecoratedSmoothedSS = ( (priv->UndecoratedSmoothedSS * 5) + (priv->SignalStrength * 10) ) / 6;
+	}
+	else
+	{
+		priv->UndecoratedSmoothedSS = priv->SignalStrength * 10;
+	}
+
+	priv->UndercorateSmoothedRxPower = ( (priv->UndercorateSmoothedRxPower * 50) + (priv->RxPower* 11)) / 60;
+
+//	printk("Sommthing SignalSterngth (%d) => UndecoratedSmoothedSS (%d)\n", priv->SignalStrength, priv->UndecoratedSmoothedSS);
+//	printk("Sommthing RxPower (%d) => UndecoratedRxPower (%d)\n", priv->RxPower, priv->UndercorateSmoothedRxPower);
+
+	//if(priv->CurCCKRSSI >= 0 && bCckRate)
+	if(bCckRate)
+	{
+		priv->CurCCKRSSI = priv->RSSI;
+	}
+	else
+	{
+		priv->CurCCKRSSI = 0;
+	}
+
+	// Boundary checking.
+	// TODO: The overflow condition does happen, if we want to fix,
+	// we shall recalculate thresholds first.
+	if(priv->UndecoratedSmoothedSS > 100)
+	{
+//		printk("UndecoratedSmoothedSS(%d) overflow, SignalStrength(%d)\n", priv->UndecoratedSmoothedSS, priv->SignalStrength);
+	}
+	if(priv->UndecoratedSmoothedSS < 0)
+	{
+//		printk("UndecoratedSmoothedSS(%d) underflow, SignalStrength(%d)\n", priv->UndecoratedSmoothedSS, priv->SignalStrength);
+	}
+
+}
+
+//by amy 080312}
+
+/* This is rough RX isr handling routine*/
+void rtl8180_rx(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct sk_buff *tmp_skb;
+
+	//struct sk_buff *skb;
+	short first,last;
+	u32 len;
+	int lastlen;
+	unsigned char quality, signal;
+	u8 rate;
+	//u32 *prism_hdr;
+	u32 *tmp,*tmp2;
+	u8 rx_desc_size;
+	u8 padding;
+	//u32 count=0;
+	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 *hdr;//by amy
+	u16 fc,type;
+	u8 bHwError = 0,bCRC = 0,bICV = 0;
+	//bHwError = 0;
+	//bCRC = 0;
+	//bICV = 0;
+	bool	bCckRate = false;
+	u8     RSSI = 0;
+	long	SignalStrengthIndex = 0;//+by amy 080312
+//	u8 SignalStrength = 0;
+	struct ieee80211_rx_stats stats = {
+		.signal = 0,
+		.noise = -98,
+		.rate = 0,
+	//	.mac_time = jiffies,
+		.freq = IEEE80211_24GHZ_BAND,
+	};
+
+#ifdef CONFIG_RTL8185B
+	stats.nic_type = NIC_8185B;
+	rx_desc_size = 8;
+
+#else
+	stats.nic_type = NIC_8185;
+	rx_desc_size = 4;
+#endif
+	//printk("receive frame!%d\n",count++);
+	//if (!priv->rxbuffer) DMESG ("EE: NIC RX ack, but RX queue corrupted!");
+	//else {
+
+	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++;
+
+	/*	if (! *(priv->rxring) & (1<<31)) {
+
+			priv->stats.rxreset++;
+			priv->rxringtail=priv->rxring;
+			priv->rxbuffer=priv->rxbufferhead;
+
+		}else{*/
+
+		#if 0
+		/* Maybe it is possible that the NIC has skipped some descriptors or
+		 * it has reset its internal pointer to the beginning of the ring
+		 * we search for the first filled descriptor in the ring, or we break
+		 * putting again the pointer in the old location if we do not found any.
+		 * This is quite dangerous, what does happen if the nic writes
+		 * two descriptor (say A and B) when we have just checked the descriptor
+		 * A and we are going to check the descriptor B..This might happen if the
+		 * interrupt was dummy, there was not really filled descriptors and
+		 * the NIC didn't lose pointer
+		 */
+
+		//priv->stats.rxwrkaround++;
+
+		tmp = priv->rxringtail;
+		while (*(priv->rxringtail) & (1<<31)){
+
+			priv->rxringtail+=4;
+
+			if(priv->rxringtail >=
+				(priv->rxring)+(priv->rxringcount )*4)
+				priv->rxringtail=priv->rxring;
+
+			priv->rxbuffer=(priv->rxbuffer->next);
+
+			if(priv->rxringtail == tmp ){
+				//DMESG("EE: Could not find RX pointer");
+				priv->stats.rxnopointer++;
+				break;
+			}
+		}
+		#else
+
+		tmp2 = NULL;
+		tmp = priv->rxringtail;
+		do{
+			if(tmp == priv->rxring)
+				//tmp  = priv->rxring + (priv->rxringcount )*rx_desc_size; xiong-2006-11-15
+				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;
+		#endif
+		//}
+	}
+
+	/* 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++;
+			//DMESG("EE: RX DMA FAILED at buffer pointed by descriptor %x",(u32)priv->rxringtail);
+			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)) {
+//lastlen=((*priv->rxringtail) &0xfff);
+				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;
+		}
+
+#ifdef CONFIG_RTL8185B
+		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;
+		}
+#ifdef CONFIG_RTL818X_S
+               padding = 0;
+#endif
+#endif
+		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 exausting
+			*/
+			if(!priv->rx_skb_complete)
+				dev_kfree_skb_any(priv->rx_skb);
+			priv->rx_skb_complete = 1;
+		}
+
+#ifdef DEBUG_RX_FRAG
+		DMESG("Iteration.. len %x",len);
+		if(first) DMESG ("First descriptor");
+		if(last) DMESG("Last descriptor");
+
+#endif
+#ifdef DEBUG_RX_VERBOSE
+		print_buffer( priv->rxbuffer->buf, len);
+#endif
+
+#ifdef CONFIG_RTL8185B
+		signal=(unsigned char)(((*(priv->rxringtail+3))& (0x00ff0000))>>16);
+		signal=(signal&0xfe)>>1;	// Modify by hikaru 6.6
+
+		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);
+
+#else
+		signal=((*(priv->rxringtail+1))& (0xff0000))>>16;
+		signal=(signal&0xfe)>>1;	// Modify by hikaru 6.6
+
+		quality=((*(priv->rxringtail+1)) & (0xff));
+
+		stats.mac_time[0] = *(priv->rxringtail+2);
+		stats.mac_time[1] = *(priv->rxringtail+3);
+#endif
+		rate=((*(priv->rxringtail)) &
+			((1<<23)|(1<<22)|(1<<21)|(1<<20)))>>20;
+
+		stats.rate = rtl8180_rate2rate(rate);
+		//DMESG("%d",rate);
+		Antenna = (((*(priv->rxringtail +3))& (0x00008000)) == 0 )? 0:1 ;
+//		printk("in rtl8180_rx():Antenna is %d\n",Antenna);
+//by amy for antenna
+		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
+
+   			RxAGC_dBm = -( LNA_gain[LNA] + (BB *2) ); //Pin_11b=-(LNA_gain+BB_gain) (dBm)
+
+			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 ;  // It can use directly by SD3 CMLin
+		priv->RxPower = rxpower;
+		priv->RSSI = RSSI;
+//{by amy 080312
+		// SQ translation formular is provided by SD3 DZ. 2006.06.27, by rcnjko.
+		if(quality >= 127)
+			quality = 1;//0; //0 will cause epc to show signal zero , walk aroud now;
+		else if(quality < 27)
+			quality = 100;
+		else
+			quality = 127 - quality;
+		priv->SignalQuality = quality;
+		if(!priv->card_8185)
+			printk("check your card type\n");
+
+		stats.signal = (u8)quality;//priv->wstats.qual.level = priv->SignalStrength;
+		stats.signalstrength = RXAGC;
+		if(stats.signalstrength > 100)
+			stats.signalstrength = 100;
+		stats.signalstrength = (stats.signalstrength * 70)/100 + 30;
+	//	printk("==========================>rx : RXAGC is %d,signalstrength is %d\n",RXAGC,stats.signalstrength);
+		stats.rssi = priv->wstats.qual.qual = priv->SignalQuality;
+		stats.noise = priv->wstats.qual.noise = 100 - priv ->wstats.qual.qual;
+//by amy 080312}
+		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 *)priv->rxbuffer->buf;
+		    fc = le16_to_cpu(hdr->frame_ctl);
+	        type = WLAN_FC_GET_TYPE(fc);
+
+			if((IEEE80211_FTYPE_CTL != type) &&
+				(eqMacAddr(priv->ieee80211->current_network.bssid, (fc & IEEE80211_FCTL_TODS)? hdr->addr1 : (fc & IEEE80211_FCTL_FROMDS )? hdr->addr2 : hdr->addr3))
+				 && (!bHwError) && (!bCRC)&& (!bICV))
+			{
+//by amy 080312
+				// 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. 2007.01.23, by shien chang.
+				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 that received the lasted packet.
+				priv->LastRxPktAntenna = Antenna ? 1 : 0; // 0: aux, 1: main.
+//by amy 080312
+			    SwAntennaDiversityRxOk8185(dev, priv->SignalStrength);
+			}
+
+//by amy for antenna
+
+
+
+
+
+
+#ifndef DUMMY_RX
+		if(first){
+			if(!priv->rx_skb_complete){
+				/* seems that HW sometimes fails to reiceve and
+				   doesn't provide the last descriptor */
+#ifdef DEBUG_RX_SKB
+				DMESG("going to free incomplete skb");
+#endif
+				dev_kfree_skb_any(priv->rx_skb);
+				priv->stats.rxnolast++;
+#ifdef DEBUG_RX_SKB
+				DMESG("free incomplete skb OK");
+#endif
+			}
+			/* support for prism header has been originally added by Christian */
+			if(priv->prism_hdr && priv->ieee80211->iw_mode == IW_MODE_MONITOR){
+
+#if 0
+				priv->rx_skb = dev_alloc_skb(len+2+PRISM_HDR_SIZE);
+				if(! priv->rx_skb) goto drop;
+
+				prism_hdr = (u32*) skb_put(priv->rx_skb,PRISM_HDR_SIZE);
+				prism_hdr[0]=htonl(0x80211001);        //version
+				prism_hdr[1]=htonl(0x40);              //length
+				prism_hdr[2]=htonl(stats.mac_time[1]);    //mactime (HIGH)
+				prism_hdr[3]=htonl(stats.mac_time[0]);    //mactime (LOW)
+				rdtsc(prism_hdr[5], prism_hdr[4]);         //hostime (LOW+HIGH)
+				prism_hdr[4]=htonl(prism_hdr[4]);          //Byte-Order aendern
+				prism_hdr[5]=htonl(prism_hdr[5]);          //Byte-Order aendern
+				prism_hdr[6]=0x00;                     //phytype
+				prism_hdr[7]=htonl(priv->chan);        //channel
+				prism_hdr[8]=htonl(stats.rate);        //datarate
+				prism_hdr[9]=0x00;                     //antenna
+				prism_hdr[10]=0x00;                    //priority
+				prism_hdr[11]=0x00;                    //ssi_type
+				prism_hdr[12]=htonl(stats.signal);     //ssi_signal
+				prism_hdr[13]=htonl(stats.noise);      //ssi_noise
+				prism_hdr[14]=0x00;                    //preamble
+				prism_hdr[15]=0x00;                    //encoding
+
+#endif
+			}else{
+				priv->rx_skb = dev_alloc_skb(len+2);
+				if( !priv->rx_skb) goto drop;
+#ifdef DEBUG_RX_SKB
+				DMESG("Alloc initial skb %x",len+2);
+#endif
+			}
+
+			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;
+#ifdef DEBUG_RX_SKB
+				DMESG("Realloc skb %x",len+2);
+#endif
+
+#ifdef DEBUG_RX_SKB
+				DMESG("going copy prev frag %x",priv->rx_skb->len);
+#endif
+				memcpy(skb_put(tmp_skb,priv->rx_skb->len),
+					priv->rx_skb->data,
+					priv->rx_skb->len);
+#ifdef DEBUG_RX_SKB
+				DMESG("skb copy prev frag complete");
+#endif
+
+				dev_kfree_skb_any(priv->rx_skb);
+#ifdef DEBUG_RX_SKB
+				DMESG("prev skb free ok");
+#endif
+
+				priv->rx_skb=tmp_skb;
+			}
+		}
+#ifdef DEBUG_RX_SKB
+		DMESG("going to copy current payload %x",len);
+#endif
+		if(!priv->rx_skb_complete) {
+#ifdef CONFIG_RTL8185B
+			if(padding) {
+				memcpy(skb_put(priv->rx_skb,len),
+					(((unsigned char *)priv->rxbuffer->buf) + 2),len);
+			} else {
+#endif
+				memcpy(skb_put(priv->rx_skb,len),
+					priv->rxbuffer->buf,len);
+#ifdef CONFIG_RTL8185B
+			}
+#endif
+		}
+#ifdef DEBUG_RX_SKB
+		DMESG("current fragment skb copy complete");
+#endif
+
+		if(last && !priv->rx_skb_complete){
+
+#ifdef DEBUG_RX_SKB
+			DMESG("Got last fragment");
+#endif
+
+			if(priv->rx_skb->len > 4)
+				skb_trim(priv->rx_skb,priv->rx_skb->len-4);
+#ifdef DEBUG_RX_SKB
+			DMESG("yanked out crc, passing to the upper layer");
+#endif
+
+#ifndef RX_DONT_PASS_UL
+			if(!ieee80211_rx(priv->ieee80211,
+					 priv->rx_skb, &stats)){
+#ifdef DEBUG_RX
+				DMESGW("Packet not consumed");
+#endif
+#endif // RX_DONT_PASS_UL
+
+				dev_kfree_skb_any(priv->rx_skb);
+#ifndef RX_DONT_PASS_UL
+			}
+#endif
+#ifdef DEBUG_RX
+			else{
+					DMESG("Rcv frag");
+			}
+#endif
+			priv->rx_skb_complete=1;
+		}
+
+#endif //DUMMY_RX
+
+		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 enought 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);
+			//^empty descriptor
+
+			//wmb();
+
+#ifdef DEBUG_RX
+		DMESG("Current descriptor: %x",(u32)priv->rxringtail);
+#endif
+		//unsigned long flags;
+		//spin_lock_irqsave(&priv->irq_lock,flags);
+
+		priv->rxringtail+=rx_desc_size;
+		if(priv->rxringtail >=
+		   (priv->rxring)+(priv->rxringcount )*rx_desc_size)
+			priv->rxringtail=priv->rxring;
+
+		//spin_unlock_irqrestore(&priv->irq_lock,flags);
+
+
+		priv->rxbuffer=(priv->rxbuffer->next);
+
+	}
+
+
+
+//	if(get_curr_tx_free_desc(dev,priority))
+//	ieee80211_sta_ps_sleep(priv->ieee80211, &tmp, &tmp2);
+
+
+
+}
+
+
+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);
+/*
+
+	switch(priority){
+
+		case LOW_PRIORITY:
+
+		write_nic_byte(dev,TX_DMA_POLLING,
+		       (1<< TX_DMA_POLLING_LOWPRIORITY_SHIFT) |
+			        priv->dma_poll_mask);
+		break;
+
+		case NORM_PRIORITY:
+
+		write_nic_byte(dev,TX_DMA_POLLING,
+		       (1<< TX_DMA_POLLING_NORMPRIORITY_SHIFT) |
+			        priv->dma_poll_mask);
+		break;
+
+		case HI_PRIORITY:
+
+		write_nic_byte(dev,TX_DMA_POLLING,
+		       (1<< TX_DMA_POLLING_HIPRIORITY_SHIFT) |
+			        priv->dma_poll_mask);
+		break;
+
+	}
+*/
+	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);
+}
+
+#if 0
+void rtl8180_tx_queues_stop(struct net_device *dev)
+{
+	//struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u8 dma_poll_mask = (1<<TX_DMA_STOP_LOWPRIORITY_SHIFT);
+	dma_poll_mask |= (1<<TX_DMA_STOP_HIPRIORITY_SHIFT);
+	dma_poll_mask |= (1<<TX_DMA_STOP_NORMPRIORITY_SHIFT);
+	dma_poll_mask |= (1<<TX_DMA_STOP_BEACON_SHIFT);
+
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+	write_nic_byte(dev,TX_DMA_POLLING,dma_poll_mask);
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+}
+#endif
+
+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);
+#ifdef CONFIG_RTL8185B
+	priv->dma_poll_stop_mask |= TPPOLLSTOP_AC_VIQ;
+	write_nic_byte(dev,TPPollStop, priv->dma_poll_stop_mask);
+#else
+	priv->dma_poll_mask |= (1<<TX_DMA_STOP_LOWPRIORITY_SHIFT);
+	write_nic_byte(dev,TX_DMA_POLLING,priv->dma_poll_mask);
+#endif
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+}
+
+
+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);
+#ifdef  CONFIG_RTL8185B
+	priv->dma_poll_stop_mask &= ~(TPPOLLSTOP_AC_VIQ);
+	write_nic_byte(dev,TPPollStop, priv->dma_poll_stop_mask);
+#else
+	priv->dma_poll_mask &= ~(1<<TX_DMA_STOP_LOWPRIORITY_SHIFT);
+	write_nic_byte(dev,TX_DMA_POLLING,priv->dma_poll_mask);
+#endif
+	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
+ */
+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;
+	short morefrag = (h->frame_ctl) & IEEE80211_FCTL_MOREFRAGS;
+	unsigned long flags;
+	int priority;
+	//static int count = 0;
+
+	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 (trought
+	* the ieee stack, or from the try_wake_queue (again trought
+	* the ieee stack.
+	*/
+#ifdef CONFIG_RTL8185B
+	priority = AC2Q(skb->priority);
+#else
+	priority = LOW_PRIORITY;
+#endif
+	spin_lock_irqsave(&priv->tx_lock,flags);
+
+	if(priv->ieee80211->bHwRadioOff)
+	{
+		spin_unlock_irqrestore(&priv->tx_lock,flags);
+
+		return;
+	}
+
+	//printk(KERN_WARNING "priority = %d@%d\n", priority, count++);
+	if (!check_nic_enought_desc(dev, priority)){
+		//DMESG("Error: no descriptor left by previous TX (avail %d) ",
+		//	get_curr_tx_free_desc(dev, priority));
+		DMESGW("Error: no descriptor left by previous TX (avail %d) ",
+			get_curr_tx_free_desc(dev, priority));
+	//printk(KERN_WARNING "==============================================================> \n");
+		ieee80211_stop_queue(priv->ieee80211);
+	}
+	rtl8180_tx(dev, skb->data, skb->len, priority, morefrag,0,rate);
+	if (!check_nic_enought_desc(dev, priority))
+		ieee80211_stop_queue(priv->ieee80211);
+
+	//dev_kfree_skb_any(skb);
+	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 packet 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 found
+ * 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 loops if invoked with 0 descriptors or 0 len buffer*/
+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;
+
+#ifdef CONFIG_RTL8185B
+	priority = MANAGE_PRIORITY;
+#else
+	priority = NORM_PRIORITY;
+#endif
+
+	spin_lock_irqsave(&priv->tx_lock,flags);
+
+	if(priv->ieee80211->bHwRadioOff)
+	{
+		spin_unlock_irqrestore(&priv->tx_lock,flags);
+
+		dev_kfree_skb_any(skb);
+		return 0;
+	}
+
+	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 0;
+}
+
+// longpre 144+48 shortpre 72+24
+u16 rtl8180_len2duration(u32 len, short rate,short* ext)
+{
+	u16 duration;
+	u16 drift;
+	*ext=0;
+
+	switch(rate){
+	case 0://1mbps
+		*ext=0;
+		duration = ((len+4)<<4) /0x2;
+		drift = ((len+4)<<4) % 0x2;
+		if(drift ==0 ) break;
+		duration++;
+		break;
+
+	case 1://2mbps
+		*ext=0;
+		duration = ((len+4)<<4) /0x4;
+		drift = ((len+4)<<4) % 0x4;
+		if(drift ==0 ) break;
+		duration++;
+		break;
+
+	case 2: //5.5mbps
+		*ext=0;
+		duration = ((len+4)<<4) /0xb;
+		drift = ((len+4)<<4) % 0xb;
+		if(drift ==0 )
+			break;
+		duration++;
+		break;
+
+	default:
+	case 3://11mbps
+		*ext=0;
+		duration = ((len+4)<<4) /0x16;
+		drift = ((len+4)<<4) % 0x16;
+		if(drift ==0 )
+			break;
+		duration++;
+		if(drift > 6)
+			break;
+		*ext=1;
+		break;
+	}
+
+	return duration;
+}
+
+
+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 |= cpu_to_le16(priv->ieee80211->current_network.beacon_interval);//0x64;
+	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);
+	}
+	#if 0
+	//DMESG("size %x",len);
+	if(*tail & (1<<31)){
+
+		//DMESG("No more beacon TX desc");
+		return ;
+
+	}
+	//while(! *tail & (1<<31)){
+		*tail= 0; // zeroes header
+
+		*tail = *tail| (1<<29) ; //fist segment of the packet
+		*tail = (*tail) | (1<<28); // last segment
+	//	*tail = *tail | (1<<18); // this is  a beacon frame
+		*(tail+3)=*(tail+3) &~ 0xfff;
+		*(tail+3)=*(tail+3) | len; // buffer lenght
+		*tail = *tail |len;
+		// zeroes the second 32-bits dword of the descriptor
+		*(tail+1)= 0;
+		*tail = *tail | (rate << 24);
+
+			duration = rtl8180_len2duration(len,rate,&ext);
+
+		*(tail+1) = *(tail+1) | ((duration & 0x7fff)<<16);
+
+		*tail = *tail | (1<<31);
+		//^ descriptor ready to be txed
+		if((tail - begin)/8 == priv->txbeaconcount-1)
+			tail=begin;
+		else
+			tail=tail+8;
+	//}
+#endif
+}
+
+/* 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,
+		 short 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;
+	//u16	AckCtsTime;
+	//u16	FrameTime;
+	u16 duration;
+	short ext;
+	struct buffer* buflist;
+	//unsigned long flags;
+#ifdef CONFIG_RTL8185B
+	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			RTSRate = 22;
+	//u8			RetryLimit = 0;
+	u16 			Duration = 0;
+	u16			RtsDur = 0;
+	u16			ThisFrameTime = 0;
+	u16			TxDescDuration = 0;
+	u8 			ownbit_flag = false; //added by david woo for sync Tx, 2007.12.14
+#endif
+
+	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;
+ 	}
+
+	//printk("in rtl8180_tx(): rate is %d\n",priv->ieee80211->rate);
+#if 1
+		memcpy(&dest, frag_hdr->addr1, ETH_ALEN);
+		if (is_multicast_ether_addr(dest) ||
+				is_broadcast_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
+			//u8 AckRate;
+			u16 AckTime;
+
+			//YJ,add,080828,for Keep alive
+			priv->NumTxUnicast++;
+
+			// Figure out ACK rate according to BSS basic rate and Tx rate, 2006.03.08 by rcnjko.
+			//AckRate = ComputeAckRate( pMgntInfo->mBrates, (u1Byte)(pTcb->DataRate) );
+			// Figure out ACK time according to the AckRate and assume long preamble is used on receiver, 2006.03.08, by rcnjko.
+			//AckTime = ComputeTxTime( sAckCtsLng/8, AckRate, FALSE, FALSE);
+			//For simplicity, just use the 1M basic rate
+			//AckTime = ComputeTxTime(14, 540,0, 0);	// AckCTSLng = 14 use 1M bps send
+			AckTime = ComputeTxTime(14, 10,0, 0);	// AckCTSLng = 14 use 1M bps send
+			//AckTime = ComputeTxTime(14, 2,false, false);	// AckCTSLng = 14 use 1M bps send
+
+			if ( ((len + sCrcLng) > priv->rts) && priv->rts )
+			{ // RTS/CTS.
+				u16 RtsTime, CtsTime;
+				//u16 CtsRate;
+				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.
+				CtsTime = ComputeTxTime(14, 10,0, 0);	// AckCTSLng = 14 use 1M bps send
+
+				// 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(!(frag_hdr->frame_ctl & IEEE80211_FCTL_MOREFRAGS)) { //no more fragment
+				// ThisFrame-ACK.
+				Duration = aSifsTime + AckTime;
+			} else { // One or more fragments remained.
+				u16 NextFragTime;
+				NextFragTime = ComputeTxTime( len + sCrcLng, //pretend following packet length equal current packet
+						rtl8180_rate2rate(rate),
+						0,
+						bUseShortPreamble );
+
+				//ThisFrag-ACk-NextFrag-ACK.
+				Duration = NextFragTime + 3*aSifsTime + 2*AckTime;
+			}
+
+		} // End of Unicast packet
+
+		frag_hdr->duration_id = Duration;
+#endif
+
+	buflen=priv->txbuffsize;
+	remain=len;
+	temp_tail = tail;
+//printk("================================>buflen = %d, remain = %d!\n", buflen,remain);
+	while(remain!=0){
+#ifdef DEBUG_TX_FRAG
+		DMESG("TX iteration");
+#endif
+#ifdef DEBUG_TX
+		DMESG("TX: filling descriptor %x",(u32)tail);
+#endif
+		mb();
+		if(!buflist){
+			DMESGE("TX buffer error, cannot TX frames. pri %d.", priority);
+			//spin_unlock_irqrestore(&priv->tx_lock,flags);
+			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++;
+#ifdef DEBUG_TX_DESC
+				check_tx_ring(dev,priority);
+			//	netif_stop_queue(dev);
+			//	netif_carrier_off(dev);
+#endif
+			//	spin_unlock_irqrestore(&priv->tx_lock,flags);
+
+			return remain;
+
+		}
+
+		*tail= 0; // zeroes header
+		*(tail+1) = 0;
+		*(tail+3) = 0;
+		*(tail+5) = 0;
+		*(tail+6) = 0;
+		*(tail+7) = 0;
+
+		if(priv->card_8185){
+			//FIXME: this should be triggered by HW encryption parameters.
+			*tail |= (1<<15); //no encrypt
+//			*tail |= (1<<30); //raise int when completed
+		}
+	//	*tail = *tail | (1<<16);
+		if(remain==len && !descfrag) {
+			ownbit_flag = false;	//added by david woo,2007.12.14
+#ifdef DEBUG_TX_FRAG
+			DMESG("First descriptor");
+#endif
+			*tail = *tail| (1<<29) ; //fist segment of the packet
+			*tail = *tail |(len);
+		} else {
+			ownbit_flag = true;
+		}
+
+		for(i=0;i<buflen&& remain >0;i++,remain--){
+			((u8*)buf)[i]=txbuf[i]; //copy data into descriptor pointed DMAble buffer
+			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 lenght
+		// Use short preamble or not
+		if (priv->ieee80211->current_network.capability&WLAN_CAPABILITY_SHORT_PREAMBLE)
+			if (priv->plcp_preamble_mode==1 && rate!=0)	//  short mode now, not long!
+			//	*tail |= (1<<16);				// enable short preamble mode.
+
+#ifdef CONFIG_RTL8185B
+		if(bCTSEnable) {
+			*tail |= (1<<18);
+		}
+
+		if(bRTSEnable) //rts enable
+		{
+			*tail |= ((ieeerate2rtlrate(priv->ieee80211->basic_rate))<<19);//RTS RATE
+			*tail |= (1<<23);//rts enable
+			*(tail+1) |=(RtsDur&0xffff);//RTS Duration
+		}
+		*(tail+3) |= ((TxDescDuration&0xffff)<<16); //DURATION
+//	        *(tail+3) |= (0xe6<<16);
+        	*(tail+5) |= (11<<8);//(priv->retry_data<<8); //retry lim ;
+#else
+		//Use RTS or not
+#ifdef CONFIG_RTL8187B
+		if ( (len>priv->rts) && priv->rts && priority!=MANAGE_PRIORITY){
+#else
+		if ( (len>priv->rts) && priv->rts && priority==LOW_PRIORITY){
+#endif
+			*tail |= (1<<23);	//enalbe RTS function
+			*tail |= (0<<19);	//use 1M bps send RTS packet
+			AckCtsTime = ComputeTxTime(14, 10,0, 0);	// AckCTSLng = 14 use 1M bps send
+			FrameTime = ComputeTxTime(len + 4, rtl8180_rate2rate(rate), 0, *tail&(1<<16));
+			// RTS/CTS time is calculate as follow
+			duration = FrameTime + 3*10 + 2*AckCtsTime;	//10us is the SifsTime;
+			*(tail+1) |= duration; 	//Need to edit here!  ----hikaru
+		}else{
+			*(tail+1)= 0; // zeroes the second 32-bits dword of the descriptor
+		}
+#endif
+
+		*tail = *tail | ((rate&0xf) << 24);
+		//DMESG("rate %d",rate);
+
+		if(priv->card_8185){
+
+			#if 0
+			*(tail+5)&= ~(1<<24); /* tx ant 0 */
+
+			*(tail+5) &= ~(1<<23); /* random tx agc 23-16 */
+			*(tail+5) |= (1<<22)|(1<<21)|(1<<20)|(1<<19)|(1<<18)|(1<<17)|(1<<16);
+
+			*(tail+5) &=
+~((1<<15)|(1<<14)|(1<<13)|(1<<12)|(1<<11)|(1<<10)|(1<<9)|(1<<8));
+			*(tail+5) |= (7<<8); // Max retry limit
+
+			*(tail+5) &= ~((1<<7)|(1<<6)|(1<<5)|(1<<4)|(1<<3)|(1<<2)|(1<<1)|(1<<0));
+			*(tail+5) |= (8<<4); // Max contention window
+			*(tail+6) |= 4; // Min contention window
+			#endif
+           //    	*(tail+5) = 0;
+		}
+
+		/* hw_plcp_len is not used for rtl8180 chip */
+		/* FIXME */
+		if(priv->card_8185 == 0 || !priv->hw_plcp_len){
+
+			duration = rtl8180_len2duration(len,
+				rate,&ext);
+
+
+#ifdef DEBUG_TX
+			DMESG("PLCP duration %d",duration );
+			//DMESG("drift %d",drift);
+			DMESG("extension %s", (ext==1) ? "on":"off");
+#endif
+			*(tail+1) = *(tail+1) | ((duration & 0x7fff)<<16);
+			if(ext) *(tail+1) = *(tail+1) |(1<<31); //plcp length extension
+		}
+
+		if(morefrag) *tail = (*tail) | (1<<17); // more fragment
+		if(!remain) *tail = (*tail) | (1<<28); // last segment of frame
+
+#ifdef DEBUG_TX_FRAG
+		if(!remain)DMESG("Last descriptor");
+		if(morefrag)DMESG("More frag");
+#endif
+               *(tail+5) = *(tail+5)|(2<<27);
+                *(tail+7) = *(tail+7)|(1<<4);
+
+		wmb();
+		if(ownbit_flag)
+		{
+			*tail = *tail | (1<<31); // descriptor ready to be txed
+		}
+
+#ifdef DEBUG_TX_DESC2
+                printk("tx desc is:\n");
+		DMESG("%8x %8x %8x %8x %8x %8x %8x %8x", tail[0], tail[1], tail[2], tail[3],
+			tail[4], tail[5], tail[6], tail[7]);
+#endif
+
+		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
+				 */
+			//	priv->txbeaconringtail=tail;
+			//	priv->txbeaconbufstail=buflist;
+
+				break;
+
+		}
+
+		//rtl8180_dma_kick(dev,priority);
+	}
+	*temp_tail = *temp_tail | (1<<31); // descriptor ready to be txed
+	rtl8180_dma_kick(dev,priority);
+	//spin_unlock_irqrestore(&priv->tx_lock,flags);
+
+	return 0;
+
+}
+
+
+void rtl8180_irq_rx_tasklet(struct r8180_priv * priv);
+
+
+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_adapter_start(dev);
+	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_dword(dev,BEACON_INTERVAL);
+	beacon_interval &= ~ BEACON_INTERVAL_MASK;
+	beacon_interval |= net->beacon_interval;
+	write_nic_dword(dev, BEACON_INTERVAL, beacon_interval);
+
+	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
+
+
+	/*
+	u16 atim = read_nic_dword(dev,ATIM);
+	u16 = u16 &~ ATIM_MASK;
+	u16 = u16 | beacon->atim;
+	*/
+#if 0
+	if (net->capability & WLAN_CAPABILITY_PRIVACY) {
+		if (priv->hw_wep) {
+			DMESG("Enabling hardware WEP support");
+			rtl8180_set_hw_wep(dev);
+			priv->ieee80211->host_encrypt=0;
+			priv->ieee80211->host_decrypt=0;
+		}
+#ifndef CONFIG_IEEE80211_NOWEP
+		else {
+			priv->ieee80211->host_encrypt=1;
+			priv->ieee80211->host_decrypt=1;
+		}
+#endif
+	}
+#ifndef CONFIG_IEEE80211_NOWEP
+	else{
+		priv->ieee80211->host_encrypt=0;
+		priv->ieee80211->host_decrypt=0;
+	}
+#endif
+#endif
+
+
+	if(priv->card_8185)
+		rtl8180_set_chan(dev, priv->chan);
+
+
+}
+
+void rtl8180_rq_tx_ack(struct net_device *dev){
+
+	struct r8180_priv *priv = ieee80211_priv(dev);
+//	printk("====================>%s\n",__func__);
+	write_nic_byte(dev,CONFIG4,read_nic_byte(dev,CONFIG4)|CONFIG4_PWRMGT);
+	priv->ack_tx_to_ieee = 1;
+}
+
+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;
+}
+/* FIXME FIXME 5msecs is random */
+#define HW_WAKE_DELAY 5
+
+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);
+	//DMESG("Waken up!");
+	write_nic_byte(dev,CONFIG4,read_nic_byte(dev,CONFIG4)&~CONFIG4_PWRMGT);
+
+	if(priv->rf_wakeup)
+		priv->rf_wakeup(dev);
+//	mdelay(HW_WAKE_DELAY);
+	spin_unlock_irqrestore(&priv->ps_lock,flags);
+}
+
+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);
+       //DMESG("Sleep!");
+
+        if(priv->rf_sleep)
+                priv->rf_sleep(dev);
+        spin_unlock_irqrestore(&priv->ps_lock,flags);
+}
+
+
+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(tl == 0) tl = 1;
+
+	/* FIXME HACK FIXME HACK */
+//	force_pci_posting(dev);
+	//mdelay(1);
+
+//	rb = read_nic_dword(dev, TSFTR);
+
+	/* If the interval in witch 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);
+		printk("too short to sleep\n");
+		return;
+	}
+
+//	write_nic_dword(dev, TimerInt, tl);
+//	rb = read_nic_dword(dev, TSFTR);
+	{
+		u32 tmp = (tl>rb)?(tl-rb):(rb-tl);
+	//	if (tl<rb)
+
+		//lzm,add,080828
+		priv->DozePeriodInPast2Sec += jiffies_to_msecs(tmp);
+
+		queue_delayed_work(priv->ieee80211->wq, &priv->ieee80211->hw_wakeup_wq, tmp); //as tl may be less than rb
+	}
+	/* if we suspect the TimerInt is gone beyond tl
+	 * while setting it, then give up
+	 */
+#if 1
+	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;
+	}
+#endif
+//	if(priv->rf_sleep)
+//		priv->rf_sleep(dev);
+
+	queue_work(priv->ieee80211->wq, (void *)&priv->ieee80211->hw_sleep_wq);
+	spin_unlock_irqrestore(&priv->ps_lock,flags);
+}
+
+
+//void rtl8180_wmm_param_update(struct net_device *dev,u8 *ac_param)
+#if LINUX_VERSION_CODE >=KERNEL_VERSION(2,6,20)
+void rtl8180_wmm_param_update(struct work_struct * work)
+{
+	struct ieee80211_device * ieee = container_of(work, struct ieee80211_device,wmm_param_update_wq);
+	//struct r8180_priv *priv = (struct r8180_priv*)(ieee->priv);
+	struct net_device *dev = ieee->dev;
+#else
+void rtl8180_wmm_param_update(struct ieee80211_device *ieee)
+{
+	struct net_device *dev = ieee->dev;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+#endif
+	u8 *ac_param = (u8 *)(ieee->current_network.wmm_param);
+	u8 mode = ieee->current_network.mode;
+	AC_CODING	eACI;
+	AC_PARAM	AcParam;
+	PAC_PARAM	pAcParam;
+	u8 i;
+
+#ifndef CONFIG_RTL8185B
+        //for legacy 8185 keep the PARAM unchange.
+	return;
+#else
+	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;
+			{
+				u8		u1bAIFS;
+				u32		u4bAcParam;
+				pAcParam = (PAC_PARAM)(&AcParam);
+				// Retrive paramters to udpate.
+				u1bAIFS = pAcParam->f.AciAifsn.f.AIFSN *(((mode&IEEE_G) == IEEE_G)?9:20) + aSifsTime;
+				u4bAcParam = ((((u32)(pAcParam->f.TXOPLimit))<<AC_PARAM_TXOP_LIMIT_OFFSET)|
+					      (((u32)(pAcParam->f.Ecw.f.ECWmax))<<AC_PARAM_ECW_MAX_OFFSET)|
+					      (((u32)(pAcParam->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);
+						break;
+
+					case AC0_BE:
+						write_nic_dword(dev, AC_BE_PARAM, u4bAcParam);
+						break;
+
+					case AC2_VI:
+						write_nic_dword(dev, AC_VI_PARAM, u4bAcParam);
+						break;
+
+					case AC3_VO:
+						write_nic_dword(dev, AC_VO_PARAM, u4bAcParam);
+						break;
+
+					default:
+						printk(KERN_WARNING "SetHwReg8185():invalid ACI: %d!\n", eACI);
+						break;
+				}
+			}
+		}
+		return;
+	}
+
+	for(i = 0; i < AC_MAX; i++){
+		//AcParam.longData = 0;
+		pAcParam = (AC_PARAM * )ac_param;
+		{
+			AC_CODING	eACI;
+			u8		u1bAIFS;
+			u32		u4bAcParam;
+
+			// Retrive paramters to udpate.
+			eACI = pAcParam->f.AciAifsn.f.ACI;
+			//Mode G/A: slotTimeTimer = 9; Mode B: 20
+			u1bAIFS = pAcParam->f.AciAifsn.f.AIFSN * (((mode&IEEE_G) == IEEE_G)?9:20) + aSifsTime;
+			u4bAcParam = (	(((u32)(pAcParam->f.TXOPLimit)) << AC_PARAM_TXOP_LIMIT_OFFSET)	|
+					(((u32)(pAcParam->f.Ecw.f.ECWmax)) << AC_PARAM_ECW_MAX_OFFSET)	|
+					(((u32)(pAcParam->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);
+					break;
+
+				case AC0_BE:
+					write_nic_dword(dev, AC_BE_PARAM, u4bAcParam);
+					break;
+
+				case AC2_VI:
+					write_nic_dword(dev, AC_VI_PARAM, u4bAcParam);
+					break;
+
+				case AC3_VO:
+					write_nic_dword(dev, AC_VO_PARAM, u4bAcParam);
+					break;
+
+				default:
+					printk(KERN_WARNING "SetHwReg8185(): invalid ACI: %d !\n", eACI);
+					break;
+			}
+		}
+		ac_param += (sizeof(AC_PARAM));
+	}
+#endif
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_tx_irq_wq(struct work_struct *work);
+#else
+void rtl8180_tx_irq_wq(struct net_device *dev);
+#endif
+
+
+
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_restart_wq(struct work_struct *work);
+//void rtl8180_rq_tx_ack(struct work_struct *work);
+#else
+ void rtl8180_restart_wq(struct net_device *dev);
+//void rtl8180_rq_tx_ack(struct net_device *dev);
+#endif
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_watch_dog_wq(struct work_struct *work);
+#else
+void rtl8180_watch_dog_wq(struct net_device *dev);
+#endif
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_hw_wakeup_wq(struct work_struct *work);
+#else
+void rtl8180_hw_wakeup_wq(struct net_device *dev);
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_hw_sleep_wq(struct work_struct *work);
+#else
+void rtl8180_hw_sleep_wq(struct net_device *dev);
+#endif
+
+
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_sw_antenna_wq(struct work_struct *work);
+#else
+void rtl8180_sw_antenna_wq(struct net_device *dev);
+#endif
+ void rtl8180_watch_dog(struct net_device *dev);
+void watch_dog_adaptive(unsigned long data)
+{
+    struct r8180_priv* priv = ieee80211_priv((struct net_device *)data);
+//	DMESG("---->watch_dog_adaptive()\n");
+	if(!priv->up)
+	{
+		DMESG("<----watch_dog_adaptive():driver is not up!\n");
+		return;
+	}
+
+  //      queue_work(priv->ieee80211->wq,&priv->ieee80211->watch_dog_wq);
+//{by amy 080312
+#if 1
+	// Tx High Power Mechanism.
+#ifdef HIGH_POWER
+	if(CheckHighPower((struct net_device *)data))
+	{
+		queue_work(priv->ieee80211->wq, (void *)&priv->ieee80211->tx_pw_wq);
+	}
+#endif
+
+#ifdef CONFIG_RTL818X_S
+	// Tx Power Tracking on 87SE.
+#ifdef TX_TRACK
+	//if( priv->bTxPowerTrack )	//lzm mod 080826
+	if( CheckTxPwrTracking((struct net_device *)data));
+		TxPwrTracking87SE((struct net_device *)data);
+#endif
+#endif
+
+	// Perform DIG immediately.
+#ifdef SW_DIG
+	if(CheckDig((struct net_device *)data) == true)
+	{
+		queue_work(priv->ieee80211->wq, (void *)&priv->ieee80211->hw_dig_wq);
+	}
+#endif
+#endif
+//by amy 080312}
+   	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);
+//        DMESG("<----watch_dog_adaptive()\n");
+}
+
+#ifdef ENABLE_DOT11D
+
+static CHANNEL_LIST ChannelPlan[] = {
+	{{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
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13},13} //world wide 13: ch1~ch11 active scan, ch12~13 passive //lzm add 080826
+};
+
+static void rtl8180_set_channel_map(u8 channel_plan, struct ieee80211_device *ieee)
+{
+	int i;
+
+	//lzm add 080826
+	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 (ChannelPlan[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<ChannelPlan[channel_plan].Len;i++)
+				{
+					if(ChannelPlan[channel_plan].Channel[i] <= 14)
+						GET_DOT11D_INFO(ieee)->channel_map[ChannelPlan[channel_plan].Channel[i]] = 1;
+				}
+			}
+			break;
+		}
+		case COUNTRY_CODE_GLOBAL_DOMAIN:
+		{
+			GET_DOT11D_INFO(ieee)->bEnabled = 0;
+			Dot11d_Reset(ieee);
+			ieee->bGlobalDomain = true;
+			break;
+		}
+		case COUNTRY_CODE_WORLD_WIDE_13_INDEX://lzm add 080826
+		{
+		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;
+		}
+	}
+}
+#endif
+
+//Add for RF power on power off by lizhaoming 080512
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void GPIOChangeRFWorkItemCallBack(struct work_struct *work);
+#else
+void GPIOChangeRFWorkItemCallBack(struct ieee80211_device *ieee);
+#endif
+
+//YJ,add,080828
+static void rtl8180_statistics_init(struct Stats *pstats)
+{
+	memset(pstats, 0, sizeof(struct Stats));
+}
+static void rtl8180_link_detect_init(plink_detect_t plink_detect)
+{
+	memset(plink_detect, 0, sizeof(link_detect_t));
+	plink_detect->SlotNum = DEFAULT_SLOT_NUM;
+}
+//YJ,add,080828,end
+
+short rtl8180_init(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u16 word;
+	u16 version;
+	u8 hw_version;
+	//u8 config3;
+	u32 usValue;
+	u16 tmpu16;
+	int i, j;
+
+#ifdef ENABLE_DOT11D
+#if 0
+	for(i=0;i<0xFF;i++) {
+		if(i%16 == 0)
+			printk("\n[%x]: ", i/16);
+		printk("\t%4.4x", eprom_read(dev,i));
+	}
+#endif
+	priv->channel_plan = eprom_read(dev, EEPROM_COUNTRY_CODE>>1) & 0xFF;
+	if(priv->channel_plan > COUNTRY_CODE_GLOBAL_DOMAIN){
+		printk("rtl8180_init:Error channel plan! Set to default.\n");
+		priv->channel_plan = 0;
+	}
+	//priv->channel_plan = 9;  //Global Domain
+
+	DMESG("Channel plan is %d\n",priv->channel_plan);
+	rtl8180_set_channel_map(priv->channel_plan, priv->ieee80211);
+#else
+	int ch;
+	//Set Default Channel Plan
+	if(!channels){
+		DMESG("No channels, aborting");
+		return -1;
+	}
+	ch=channels;
+	priv->channel_plan = 0;//hikaru
+	 // set channels 1..14 allowed in given locale
+	for (i=1; i<=14; i++) {
+		(priv->ieee80211->channel_map)[i] = (u8)(ch & 0x01);
+		ch >>= 1;
+	}
+#endif
+
+	//memcpy(priv->stats,0,sizeof(struct Stats));
+
+	//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->txnp_pending.ispending=0;
+	/* ^^ the SKB does not containt a partial RXed
+	 * packet (is empty)
+	 */
+
+#ifdef CONFIG_RTL8185B
+#ifdef CONFIG_RTL818X_S
+	priv->RegThreeWireMode = HW_THREE_WIRE_SI;
+#else
+        priv->RegThreeWireMode = SW_THREE_WIRE;
+#endif
+#endif
+
+//Add for RF power on power off by lizhaoming 080512
+	priv->RFChangeInProgress = false;
+	priv->SetRFPowerStateInProgress = false;
+	priv->RFProgType = 0;
+	priv->bInHctTest = false;
+
+	priv->irq_enabled=0;
+
+//YJ,modified,080828
+#if 0
+	priv->stats.rxdmafail=0;
+	priv->stats.txrdu=0;
+	priv->stats.rxrdu=0;
+	priv->stats.rxnolast=0;
+	priv->stats.rxnodata=0;
+	//priv->stats.rxreset=0;
+	//priv->stats.rxwrkaround=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.txbeacon=0;
+	priv->stats.txbeaconerr=0;
+	priv->stats.txlperr=0;
+	priv->stats.txlpokint=0;
+	priv->stats.txretry=0;//tony 20060601
+	priv->stats.rxcrcerrmin=0;
+	priv->stats.rxcrcerrmid=0;
+	priv->stats.rxcrcerrmax=0;
+	priv->stats.rxicverr=0;
+#else
+	rtl8180_statistics_init(&priv->stats);
+	rtl8180_link_detect_init(&priv->link_detect);
+#endif
+//YJ,modified,080828,end
+
+
+	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->prism_hdr=0;
+	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->bInHctTest = false;
+	priv->bInactivePs = true;//false;
+	priv->ieee80211->bInactivePs = priv->bInactivePs;
+	priv->bSwRfProcessing = false;
+	priv->eRFPowerState = eRfOff;
+	priv->RfOffReason = 0;
+	priv->LedStrategy = SW_LED_MODE0;
+	//priv->NumRxOkInPeriod = 0;  //YJ,del,080828
+	//priv->NumTxOkInPeriod = 0;  //YJ,del,080828
+	priv->TxPollingTimes = 0;//lzm add 080826
+	priv->bLeisurePs = true;
+	priv->dot11PowerSaveMode = eActive;
+//by amy for antenna
+	priv->AdMinCheckPeriod = 5;
+	priv->AdMaxCheckPeriod = 10;
+// Lower signal strength threshold to fit the HW participation in antenna diversity. +by amy 080312
+	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;
+//by amy for antenna
+//{by amy 080312
+	priv->bDigMechanism = 1;
+	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 threhold 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;
+	//YJ,add,080828
+	priv->NumTxOkTotal = 0;
+	priv->NumTxUnicast = 0;
+	priv->keepAliveLevel = DEFAULT_KEEP_ALIVE_LEVEL;
+	priv->PowerProfile = POWER_PROFILE_AC;
+	//YJ,add,080828,end
+//by amy for rate adaptive
+    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;
+
+//by amy for rate adaptive
+//by amy 080312}
+	priv->promisc = (dev->flags & IFF_PROMISC) ? 1:0;
+	spin_lock_init(&priv->irq_lock);
+	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);
+	sema_init(&priv->rf_state,1);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+	INIT_WORK(&priv->reset_wq,(void*) rtl8180_restart_wq);
+	INIT_WORK(&priv->tx_irq_wq,(void*) rtl8180_tx_irq_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_DELAYED_WORK(&priv->ieee80211->watch_dog_wq,(void*) rtl8180_watch_dog_wq);
+	//INIT_DELAYED_WORK(&priv->ieee80211->sw_antenna_wq,(void*) rtl8180_sw_antenna_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);//+by amy 080312
+	INIT_DELAYED_WORK(&priv->ieee80211->hw_dig_wq,(void*)rtl8180_hw_dig_wq);//+by amy 080312
+	INIT_DELAYED_WORK(&priv->ieee80211->tx_pw_wq,(void*)rtl8180_tx_pw_wq);//+by amy 080312
+
+	//add for RF power on power off by lizhaoming 080512
+	INIT_DELAYED_WORK(&priv->ieee80211->GPIOChangeRFWorkItem,(void*) GPIOChangeRFWorkItemCallBack);
+#else
+	INIT_WORK(&priv->reset_wq,(void*) rtl8180_restart_wq,dev);
+	INIT_WORK(&priv->tx_irq_wq,(void*) rtl8180_tx_irq_wq,dev);
+	//INIT_WORK(&priv->ieee80211->watch_dog_wq,(void*) rtl8180_watch_dog_wq,dev);
+	INIT_WORK(&priv->ieee80211->hw_wakeup_wq,(void*) rtl8180_hw_wakeup_wq,dev);
+	INIT_WORK(&priv->ieee80211->hw_sleep_wq,(void*) rtl8180_hw_sleep_wq,dev);
+	//INIT_WORK(&priv->ieee80211->sw_antenna_wq,(void*) rtl8180_sw_antenna_wq,dev);
+	INIT_WORK(&priv->ieee80211->wmm_param_update_wq,(void*) rtl8180_wmm_param_update,priv->ieee80211);
+    INIT_WORK(&priv->ieee80211->rate_adapter_wq,(void*)rtl8180_rate_adapter,dev);//+by amy 080312
+	INIT_WORK(&priv->ieee80211->hw_dig_wq,(void*)rtl8180_hw_dig_wq,dev);//+by amy 080312
+	INIT_WORK(&priv->ieee80211->tx_pw_wq,(void*)rtl8180_tx_pw_wq,dev);//+by amy 080312
+
+	//add for RF power on power off by lizhaoming 080512
+	INIT_WORK(&priv->ieee80211->GPIOChangeRFWorkItem,(void*) GPIOChangeRFWorkItemCallBack, priv->ieee80211);
+#endif
+	//INIT_WORK(&priv->reset_wq,(void*) rtl8180_restart_wq,dev);
+
+	tasklet_init(&priv->irq_rx_tasklet,
+		     (void(*)(unsigned long)) rtl8180_irq_rx_tasklet,
+		     (unsigned long)priv);
+//by amy
+    init_timer(&priv->watch_dog_timer);
+	priv->watch_dog_timer.data = (unsigned long)dev;
+	priv->watch_dog_timer.function = watch_dog_adaptive;
+//by amy
+
+//{by amy 080312
+//by amy for rate 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;
+//by amy for rate adaptive
+//by amy 080312}
+	//priv->ieee80211->func =
+	//	kmalloc(sizeof(struct ieee80211_helper_functions),GFP_KERNEL);
+	//memset(priv->ieee80211->func, 0,
+	  //     sizeof(struct ieee80211_helper_functions));
+
+	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;
+
+#ifdef CONFIG_RTL8185B
+	priv->MWIEnable = 0;
+
+	priv->ShortRetryLimit = 7;
+	priv->LongRetryLimit = 7;
+	priv->EarlyRxThreshold = 7;
+
+	priv->CSMethod = (0x01 << 29);
+
+	priv->TransmitConfig	=
+									1<<TCR_DurProcMode_OFFSET |		//for RTL8185B, duration setting by HW
+									(7<<TCR_MXDMA_OFFSET) |	// Max DMA Burst Size per Tx DMA Burst, 7: reservied.
+									(priv->ShortRetryLimit<<TCR_SRL_OFFSET) |	// Short retry limit
+									(priv->LongRetryLimit<<TCR_LRL_OFFSET) |	// Long retry limit
+									(0 ? TCR_SAT : 0);	// FALSE: HW provies PLCP length and LENGEXT, TURE: SW proiveds them
+
+	priv->ReceiveConfig	=
+#ifdef CONFIG_RTL818X_S
+#else
+                                                        priv->CSMethod |
+#endif
+//								RCR_ENMARP |
+								RCR_AMF | RCR_ADF |				//accept management/data
+								RCR_ACF |						//accept control frame for SW AP needs PS-poll, 2005.07.07, by rcnjko.
+								RCR_AB | RCR_AM | RCR_APM |		//accept BC/MC/UC
+								//RCR_AICV | RCR_ACRC32 | 		//accept ICV/CRC error packet
+								(7<<RCR_MXDMA_OFFSET) | // Max DMA Burst Size per Rx DMA Burst, 7: unlimited.
+								(priv->EarlyRxThreshold<<RCR_FIFO_OFFSET) | // Rx FIFO Threshold, 7: No Rx threshold.
+								(priv->EarlyRxThreshold == 7 ? RCR_ONLYERLPKT:0);
+
+	priv->IntrMask		= IMR_TMGDOK | IMR_TBDER | IMR_THPDER |
+								IMR_THPDER | IMR_THPDOK |
+								IMR_TVODER | IMR_TVODOK |
+								IMR_TVIDER | IMR_TVIDOK |
+								IMR_TBEDER | IMR_TBEDOK |
+								IMR_TBKDER | IMR_TBKDOK |
+								IMR_RDU |						// To handle the defragmentation not enough Rx descriptors case. Annie, 2006-03-27.
+								IMR_RER | IMR_ROK |
+								IMR_RQoSOK; // <NOTE> ROK and RQoSOK are mutually exclusive, so, we must handle RQoSOK interrupt to receive QoS frames, 2005.12.09, by rcnjko.
+
+	priv->InitialGain = 6;
+#endif
+
+	hw_version =( read_nic_dword(dev, TCR) & TCR_HWVERID_MASK)>>TCR_HWVERID_SHIFT;
+
+	switch (hw_version){
+#ifdef CONFIG_RTL8185B
+		case HW_VERID_R8185B_B:
+#ifdef CONFIG_RTL818X_S
+                        priv->card_8185 = VERSION_8187S_C;
+		        DMESG("MAC controller is a RTL8187SE b/g");
+			priv->phy_ver = 2;
+			break;
+#else
+			DMESG("MAC controller is a RTL8185B b/g");
+			priv->card_8185 = 3;
+			priv->phy_ver = 2;
+			break;
+#endif
+#endif
+		case HW_VERID_R8185_ABC:
+			DMESG("MAC controller is a RTL8185 b/g");
+			priv->card_8185 = 1;
+			/* you should not find a card with 8225 PHY ver < C*/
+			priv->phy_ver = 2;
+			break;
+
+		case HW_VERID_R8185_D:
+			DMESG("MAC controller is a RTL8185 b/g (V. D)");
+			priv->card_8185 = 2;
+			/* you should not find a card with 8225 PHY ver < C*/
+			priv->phy_ver = 2;
+			break;
+
+		case HW_VERID_R8180_ABCD:
+			DMESG("MAC controller is a RTL8180");
+			priv->card_8185 = 0;
+			break;
+
+		case HW_VERID_R8180_F:
+			DMESG("MAC controller is a RTL8180 (v. F)");
+			priv->card_8185 = 0;
+			break;
+
+		default:
+			DMESGW("MAC chip not recognized: version %x. Assuming RTL8180",hw_version);
+			priv->card_8185 = 0;
+			break;
+	}
+
+	if(priv->card_8185){
+		priv->ieee80211->modulation |= IEEE80211_OFDM_MODULATION;
+		priv->ieee80211->short_slot = 1;
+	}
+	/* you should not found any 8185 Ver B Card */
+	priv->card_8185_Bversion = 0;
+
+#ifdef CONFIG_RTL8185B
+#ifdef CONFIG_RTL818X_S
+	// just for sync 85
+	priv->card_type = PCI;
+        DMESG("This is a PCI NIC");
+#else
+	config3 = read_nic_byte(dev, CONFIG3);
+	if(config3 & 0x8){
+		priv->card_type = CARDBUS;
+		DMESG("This is a CARDBUS NIC");
+	}
+	else if( config3 & 0x4){
+		priv->card_type = MINIPCI;
+		DMESG("This is a MINI-PCI NIC");
+	}else{
+		priv->card_type = PCI;
+		DMESG("This is a PCI NIC");
+	}
+#endif
+#endif
+	priv->enable_gpio0 = 0;
+
+//by amy for antenna
+#ifdef CONFIG_RTL8185B
+	usValue = eprom_read(dev, EEPROM_SW_REVD_OFFSET);
+	DMESG("usValue is 0x%x\n",usValue);
+#ifdef CONFIG_RTL818X_S
+	//3Read AntennaDiversity
+	// SW Antenna Diversity.
+	if(	(usValue & EEPROM_SW_AD_MASK) != EEPROM_SW_AD_ENABLE )
+	{
+		priv->EEPROMSwAntennaDiversity = false;
+		//printk("EEPROM Disable SW Antenna Diversity\n");
+	}
+	else
+	{
+		priv->EEPROMSwAntennaDiversity = true;
+		//printk("EEPROM Enable SW Antenna Diversity\n");
+	}
+	// Default Antenna to use.
+	if( (usValue & EEPROM_DEF_ANT_MASK) != EEPROM_DEF_ANT_1 )
+	{
+		priv->EEPROMDefaultAntenna1 = false;
+		//printk("EEPROM Default Antenna 0\n");
+	}
+	else
+	{
+		priv->EEPROMDefaultAntenna1 = true;
+		//printk("EEPROM Default Antenna 1\n");
+	}
+
+	//
+	// Antenna diversity mechanism. Added by Roger, 2007.11.05.
+	//
+	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 == 1)? false : true);
+	}
+	//printk("bSwAntennaDiverity = %d\n", priv->bSwAntennaDiverity);
+
+
+	//
+	// Default antenna settings. Added by Roger, 2007.11.05.
+	//
+	if( priv->RegDefaultAntenna == 0)
+	{// 0: default from EEPROM.
+		priv->bDefaultAntenna1 = priv->EEPROMDefaultAntenna1;
+	}
+	else
+	{// 1: main, 2: aux.
+		priv->bDefaultAntenna1 = ((priv->RegDefaultAntenna== 2) ? true : false);
+	}
+	//printk("bDefaultAntenna1 = %d\n", priv->bDefaultAntenna1);
+#endif
+#endif
+//by amy for antenna
+	/* rtl8185 can calc plcp len in HW.*/
+	priv->hw_plcp_len = 1;
+
+	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;
+		//DMESG("Reported EEPROM chip is a 93c56 (2Kbit)");
+	}else{
+		priv->epromtype=EPROM_93c46;
+		//DMESG("Reported EEPROM chip is a 93c46 (1Kbit)");
+	}
+
+	dev->get_stats = rtl8180_stats;
+
+	dev->dev_addr[0]=eprom_read(dev,MAC_ADR) & 0xff;
+	dev->dev_addr[1]=(eprom_read(dev,MAC_ADR) & 0xff00)>>8;
+	dev->dev_addr[2]=eprom_read(dev,MAC_ADR+1) & 0xff;
+	dev->dev_addr[3]=(eprom_read(dev,MAC_ADR+1) & 0xff00)>>8;
+	dev->dev_addr[4]=eprom_read(dev,MAC_ADR+2) & 0xff;
+	dev->dev_addr[5]=(eprom_read(dev,MAC_ADR+2) & 0xff00)>>8;
+	//DMESG("Card MAC address is "MAC_FMT, MAC_ARG(dev->dev_addr));
+
+
+	for(i=1,j=0; i<14; i+=2,j++){
+
+		word = eprom_read(dev,EPROM_TXPW_CH1_2 + j);
+		priv->chtxpwr[i]=word & 0xff;
+		priv->chtxpwr[i+1]=(word & 0xff00)>>8;
+#ifdef DEBUG_EPROM
+		DMESG("tx word %x:%x",j,word);
+		DMESG("ch %d pwr %x",i,priv->chtxpwr[i]);
+		DMESG("ch %d pwr %x",i+1,priv->chtxpwr[i+1]);
+#endif
+	}
+	if(priv->card_8185){
+		for(i=1,j=0; i<14; i+=2,j++){
+
+			word = eprom_read(dev,EPROM_TXPW_OFDM_CH1_2 + j);
+			priv->chtxpwr_ofdm[i]=word & 0xff;
+			priv->chtxpwr_ofdm[i+1]=(word & 0xff00)>>8;
+#ifdef DEBUG_EPROM
+			DMESG("ofdm tx word %x:%x",j,word);
+			DMESG("ofdm ch %d pwr %x",i,priv->chtxpwr_ofdm[i]);
+			DMESG("ofdm ch %d pwr %x",i+1,priv->chtxpwr_ofdm[i+1]);
+#endif
+		}
+	}
+//{by amy 080312
+	//3Read crystal calibtration and thermal meter indication on 87SE.
+
+	// By SD3 SY's request. Added by Roger, 2007.12.11.
+
+	tmpu16 = eprom_read(dev, EEPROM_RSV>>1);
+
+	//printk("ReadAdapterInfo8185(): EEPROM_RSV(%04x)\n", tmpu16);
+
+		// Crystal calibration for Xin and Xout resp.
+		priv->XtalCal_Xout = tmpu16 & EEPROM_XTAL_CAL_XOUT_MASK; // 0~7.5pF
+		priv->XtalCal_Xin = (tmpu16 & EEPROM_XTAL_CAL_XIN_MASK)>>4; // 0~7.5pF
+		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;
+
+//by amy 080312}
+#ifdef CONFIG_RTL8185B
+	word = eprom_read(dev,EPROM_TXPW_BASE);
+	priv->cck_txpwr_base = word & 0xf;
+	priv->ofdm_txpwr_base = (word>>4) & 0xf;
+#endif
+
+	version = eprom_read(dev,EPROM_VERSION);
+	DMESG("EEPROM version %x",version);
+	if( (!priv->card_8185) && version < 0x0101){
+		DMESG ("EEPROM version too old, assuming defaults");
+		DMESG ("If you see this message *plase* send your \
+DMESG output to andreamrl@tiscali.it THANKS");
+		priv->digphy=1;
+		priv->antb=0;
+		priv->diversity=1;
+		priv->cs_treshold=0xc;
+		priv->rcr_csense=1;
+		priv->rf_chip=RFCHIPID_PHILIPS;
+	}else{
+		if(!priv->card_8185){
+			u8 rfparam = eprom_read(dev,RF_PARAM);
+			DMESG("RfParam: %x",rfparam);
+
+			priv->digphy = rfparam & (1<<RF_PARAM_DIGPHY_SHIFT) ? 0:1;
+			priv->antb =  rfparam & (1<<RF_PARAM_ANTBDEFAULT_SHIFT) ? 1:0;
+
+			priv->rcr_csense = (rfparam & RF_PARAM_CARRIERSENSE_MASK) >>
+					RF_PARAM_CARRIERSENSE_SHIFT;
+
+			priv->diversity =
+				(read_nic_byte(dev,CONFIG2)&(1<<CONFIG2_ANTENNA_SHIFT)) ? 1:0;
+		}else{
+			priv->rcr_csense = 3;
+		}
+
+		priv->cs_treshold = (eprom_read(dev,ENERGY_TRESHOLD)&0xff00) >>8;
+
+		priv->rf_chip = 0xff & eprom_read(dev,RFCHIPID);
+	}
+
+#ifdef CONFIG_RTL8185B
+#ifdef CONFIG_RTL818X_S
+	priv->rf_chip = RF_ZEBRA4;
+	priv->rf_sleep = rtl8225z4_rf_sleep;
+	priv->rf_wakeup = rtl8225z4_rf_wakeup;
+#else
+        priv->rf_chip = RF_ZEBRA2;
+#endif
+	//DMESG("Card reports RF frontend Realtek 8225z2");
+	//DMESGW("This driver has EXPERIMENTAL support for this chipset.");
+	//DMESGW("use it with care and at your own risk and");
+	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;
+	//priv->rf_sleep = rtl8225_rf_sleep;
+	//priv->rf_wakeup = rtl8225_rf_wakeup;
+
+#else
+	/* check RF frontend chipset */
+	switch (priv->rf_chip) {
+
+		case RFCHIPID_RTL8225:
+
+		if(priv->card_8185){
+			DMESG("Card reports RF frontend Realtek 8225");
+			DMESGW("This driver has EXPERIMENTAL support for this chipset.");
+			DMESGW("use it with care and at your own risk and");
+			DMESGW("**PLEASE** REPORT SUCCESS/INSUCCESS TO andreamrl@tiscali.it");
+
+			priv->rf_close = rtl8225_rf_close;
+			priv->rf_init = rtl8225_rf_init;
+			priv->rf_set_chan = rtl8225_rf_set_chan;
+			priv->rf_set_sens = NULL;
+			priv->rf_sleep = rtl8225_rf_sleep;
+			priv->rf_wakeup = rtl8225_rf_wakeup;
+
+		}else{
+			DMESGW("Detected RTL8225 radio on a card recognized as RTL8180");
+			DMESGW("This could not be... something went wrong....");
+			return -ENODEV;
+		}
+			break;
+
+		case RFCHIPID_RTL8255:
+		if(priv->card_8185){
+			DMESG("Card reports RF frontend Realtek 8255");
+			DMESGW("This driver has EXPERIMENTAL support for this chipset.");
+			DMESGW("use it with care and at your own risk and");
+			DMESGW("**PLEASE** REPORT SUCCESS/INSUCCESS TO andreamrl@tiscali.it");
+
+			priv->rf_close = rtl8255_rf_close;
+			priv->rf_init = rtl8255_rf_init;
+			priv->rf_set_chan = rtl8255_rf_set_chan;
+			priv->rf_set_sens = NULL;
+			priv->rf_sleep = NULL;
+			priv->rf_wakeup = NULL;
+
+		}else{
+			DMESGW("Detected RTL8255 radio on a card recognized as RTL8180");
+			DMESGW("This could not be... something went wrong....");
+			return -ENODEV;
+		}
+			break;
+
+
+		case RFCHIPID_INTERSIL:
+			DMESGW("Card reports RF frontend by Intersil.");
+			DMESGW("This driver has NO support for this chipset.");
+			return -ENODEV;
+			break;
+
+		case RFCHIPID_RFMD:
+			DMESGW("Card reports RF frontend by RFMD.");
+			DMESGW("This driver has NO support for this chipset.");
+			return -ENODEV;
+			break;
+
+		case RFCHIPID_GCT:
+			DMESGW("Card reports RF frontend by GCT.");
+			DMESGW("This driver has EXPERIMENTAL support for this chipset.");
+			DMESGW("use it with care and at your own risk and");
+			DMESGW("**PLEASE** REPORT SUCCESS/INSUCCESS TO andreamrl@tiscali.it");
+			priv->rf_close = gct_rf_close;
+			priv->rf_init = gct_rf_init;
+			priv->rf_set_chan = gct_rf_set_chan;
+			priv->rf_set_sens = NULL;
+			priv->rf_sleep = NULL;
+			priv->rf_wakeup = NULL;
+			break;
+
+		case RFCHIPID_MAXIM:
+			DMESGW("Card reports RF frontend by MAXIM.");
+			DMESGW("This driver has EXPERIMENTAL support for this chipset.");
+			DMESGW("use it with care and at your own risk and");
+			DMESGW("**PLEASE** REPORT SUCCESS/INSUCCESS TO andreamrl@tiscali.it");
+			priv->rf_close = maxim_rf_close;
+			priv->rf_init = maxim_rf_init;
+			priv->rf_set_chan = maxim_rf_set_chan;
+			priv->rf_set_sens = NULL;
+			priv->rf_sleep = NULL;
+			priv->rf_wakeup = NULL;
+			break;
+
+		case RFCHIPID_PHILIPS:
+			DMESG("Card reports RF frontend by Philips.");
+			DMESG("OK! Philips SA2400 radio chipset is supported.");
+			priv->rf_close = sa2400_rf_close;
+			priv->rf_init = sa2400_rf_init;
+			priv->rf_set_chan = sa2400_rf_set_chan;
+			priv->rf_set_sens = sa2400_rf_set_sens;
+			priv->sens = SA2400_RF_DEF_SENS; /* default sensitivity */
+			priv->max_sens = SA2400_RF_MAX_SENS; /* maximum sensitivity */
+			priv->rf_sleep = NULL;
+			priv->rf_wakeup = NULL;
+
+			if(priv->digphy){
+				DMESGW("Digital PHY found");
+				DMESGW("Philips DIGITAL PHY is untested! *Please*\
+	report success/failure to <andreamrl@tiscali.it>");
+			}else{
+				DMESG ("Analog PHY found");
+			}
+
+			break;
+
+		default:
+			DMESGW("Unknown RF module %x",priv->rf_chip);
+			DMESGW("Exiting...");
+			return -1;
+
+	}
+#endif
+
+
+	if(!priv->card_8185){
+		if(priv->antb)
+			DMESG ("Antenna B is default antenna");
+		else
+			DMESG ("Antenna A is default antenna");
+
+		if(priv->diversity)
+			DMESG ("Antenna diversity is enabled");
+		else
+			DMESG("Antenna diversity is disabled");
+
+		DMESG("Carrier sense %d",priv->rcr_csense);
+	}
+
+	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;
+
+
+	//priv->beacon_buf=NULL;
+
+	if(!priv->card_8185){
+
+		if(read_nic_byte(dev, CONFIG0) & (1<<CONFIG0_WEP40_SHIFT))
+			DMESG ("40-bit WEP is supported in hardware");
+		else
+			DMESG ("40-bit WEP is NOT supported in hardware");
+
+		if(read_nic_byte(dev,CONFIG0) & (1<<CONFIG0_WEP104_SHIFT))
+			DMESG ("104-bit WEP is supported in hardware");
+		else
+			DMESG ("104-bit WEP is NOT supported in hardware");
+	}
+#if !defined(SA_SHIRQ)
+        if(request_irq(dev->irq, (void *)rtl8180_interrupt, IRQF_SHARED, dev->name, dev)){
+#else
+        if(request_irq(dev->irq, (void *)rtl8180_interrupt, SA_SHIRQ, dev->name, dev)){
+#endif
+                DMESGE("Error allocating IRQ %d",dev->irq);
+                return -1;
+	}else{
+		priv->irq=dev->irq;
+		DMESG("IRQ %d",dev->irq);
+	}
+
+#ifdef DEBUG_EPROM
+	dump_eprom(dev);
+#endif
+
+	return 0;
+
+}
+
+
+void rtl8180_no_hw_wep(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	if(!priv->card_8185)
+	{
+		u8 security;
+
+		security  = read_nic_byte(dev, SECURITY);
+		security &=~(1<<SECURITY_WEP_TX_ENABLE_SHIFT);
+		security &=~(1<<SECURITY_WEP_RX_ENABLE_SHIFT);
+
+		write_nic_byte(dev, SECURITY, security);
+
+	}else{
+
+		//FIXME!!!
+	}
+	/*
+	  write_nic_dword(dev,TX_CONF,read_nic_dword(dev,TX_CONF) |
+	  (1<<TX_NOICV_SHIFT) );
+	*/
+//	priv->ieee80211->hw_wep=0;
+}
+
+
+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));
+
+	/*
+	  TX_CONF,read_nic_dword(dev,TX_CONF) &~(1<<TX_NOICV_SHIFT));
+	*/
+
+	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));
+
+	//priv->ieee80211->hw_wep=1;
+}
+
+
+void rtl8185_rf_pins_enable(struct net_device *dev)
+{
+//	u16 tmp;
+//	tmp = read_nic_word(dev, RFPinsEnable);
+	write_nic_word(dev, RFPinsEnable, 0x1fff);// | tmp);
+//	write_nic_word(dev, RFPinsEnable,7 | 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);
+}
+
+
+void rtl8185_write_phy(struct net_device *dev, u8 adr, u32 data)
+{
+	//u8 phyr;
+	u32 phyw;
+	//int i;
+
+	adr |= 0x80;
+
+	phyw= ((data<<8) | adr);
+#if 0
+
+	write_nic_dword(dev, PHY_ADR, phyw);
+
+	//read_nic_dword(dev, PHY_ADR);
+	for(i=0;i<10;i++){
+		write_nic_dword(dev, PHY_ADR, 0xffffff7f & phyw);
+		phyr = read_nic_byte(dev, PHY_READ);
+		if(phyr == (data&0xff)) break;
+
+	}
+#else
+	// Note that, 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) ));
+#endif
+	/* this is ok to fail when we write AGC table. check for AGC table might be
+	 * done by masking with 0x7f instead of 0xff
+	 */
+	//if(phyr != (data&0xff)) DMESGW("Phy write timeout %x %x %x", phyr, data,adr);
+}
+
+
+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);
+}
+
+
+/* 70*3 = 210 ms
+ * I hope this is enougth
+ */
+#define MAX_PHY 70
+void write_phy(struct net_device *dev, u8 adr, u8 data)
+{
+	u32 phy;
+	int i;
+
+	phy = 0xff0000;
+	phy |= adr;
+	phy |= 0x80; /* this should enable writing */
+	phy |= (data<<8);
+
+	//PHY_ADR, PHY_R and PHY_W  are contig and treated as one dword
+	write_nic_dword(dev,PHY_ADR, phy);
+
+	phy= 0xffff00;
+	phy |= adr;
+
+	write_nic_dword(dev,PHY_ADR, phy);
+	for(i=0;i<MAX_PHY;i++){
+		phy=read_nic_dword(dev,PHY_ADR);
+		phy= phy & 0xff0000;
+		phy= phy >> 16;
+		if(phy == data){ //SUCCESS!
+			force_pci_posting(dev);
+			mdelay(3); //random value
+#ifdef DEBUG_BB
+			DMESG("Phy wr %x,%x",adr,data);
+#endif
+			return;
+		}else{
+			force_pci_posting(dev);
+			mdelay(3); //random value
+		}
+	}
+	DMESGW ("Phy writing %x %x failed!", adr,data);
+}
+
+void rtl8185_set_rate(struct net_device *dev)
+{
+	int i;
+	u16 word;
+	int basic_rate,min_rr_rate,max_rr_rate;
+
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	//if (ieee80211_is_54g(priv->ieee80211->current_network) &&
+//		priv->ieee80211->state == IEEE80211_LINKED){
+	basic_rate = ieeerate2rtlrate(240);
+	min_rr_rate = ieeerate2rtlrate(60);
+	max_rr_rate = ieeerate2rtlrate(240);
+
+//
+//	}else{
+//		basic_rate = ieeerate2rtlrate(20);
+//		min_rr_rate = ieeerate2rtlrate(10);
+//		max_rr_rate = ieeerate2rtlrate(110);
+//	}
+
+	write_nic_byte(dev, RESP_RATE,
+			max_rr_rate<<MAX_RESP_RATE_SHIFT| min_rr_rate<<MIN_RESP_RATE_SHIFT);
+
+	word  = read_nic_word(dev, BRSR);
+	word &= ~BRSR_MBR_8185;
+
+
+	for(i=0;i<=basic_rate;i++)
+		word |= (1<<i);
+
+	write_nic_word(dev, BRSR, word);
+	//DMESG("RR:%x BRSR: %x", read_nic_byte(dev,RESP_RATE),read_nic_word(dev,BRSR));
+}
+
+
+
+void rtl8180_adapter_start(struct net_device *dev)
+{
+        struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 anaparam;
+	u16 word;
+	u8 config3;
+//	int i;
+
+	rtl8180_rtx_disable(dev);
+	rtl8180_reset(dev);
+
+	/* seems that 0xffff or 0xafff will cause
+	 * HW interrupt line crash
+	 */
+
+	//priv->irq_mask = 0xafff;
+//	priv->irq_mask = 0x4fcf;
+
+	/* enable beacon timeout, beacon TX ok and err
+	 * LP tx ok and err, HP TX ok and err, NP TX ok and err,
+	 * RX ok and ERR, and GP timer */
+	priv->irq_mask = 0x6fcf;
+
+	priv->dma_poll_mask = 0;
+
+	rtl8180_beacon_tx_disable(dev);
+
+	if(priv->card_type == CARDBUS ){
+		config3=read_nic_byte(dev, CONFIG3);
+		write_nic_byte(dev,CONFIG3,config3 | CONFIG3_FuncRegEn);
+		write_nic_word(dev,FEMR, FEMR_INTR | FEMR_WKUP | FEMR_GWAKE |
+			read_nic_word(dev, FEMR));
+	}
+	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
+	write_nic_dword(dev, MAC0, ((u32*)dev->dev_addr)[0]);
+	write_nic_word(dev, MAC4, ((u32*)dev->dev_addr)[1] & 0xffff );
+	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
+
+	rtl8180_update_msr(dev);
+
+	if(!priv->card_8185){
+		anaparam  = eprom_read(dev,EPROM_ANAPARAM_ADDRLWORD);
+		anaparam |= eprom_read(dev,EPROM_ANAPARAM_ADDRHWORD)<<16;
+
+		rtl8180_set_anaparam(dev,anaparam);
+	}
+	/* These might be unnecessary since we do in rx_enable / tx_enable */
+	fix_rx_fifo(dev);
+	fix_tx_fifo(dev);
+	/*set_nic_rxring(dev);
+	  set_nic_txring(dev);*/
+
+	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
+
+	/*
+	   The following is very strange. seems to be that 1 means test mode,
+	   but we need to acknolwledges the nic when a packet is ready
+	   altought we set it to 0
+	*/
+
+	write_nic_byte(dev,
+		       CONFIG2, read_nic_byte(dev,CONFIG2) &~\
+		       (1<<CONFIG2_DMA_POLLING_MODE_SHIFT));
+	//^the nic isn't in test mode
+	if(priv->card_8185)
+			write_nic_byte(dev,
+		       CONFIG2, read_nic_byte(dev,CONFIG2)|(1<<4));
+
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+
+	write_nic_dword(dev,INT_TIMEOUT,0);
+#ifdef DEBUG_REGISTERS
+	rtl8180_dump_reg(dev);
+#endif
+
+	if(!priv->card_8185)
+	{
+		/*
+		experimental - this might be needed to calibrate AGC,
+		anyway it shouldn't hurt
+		*/
+		write_nic_byte(dev, CONFIG5,
+			read_nic_byte(dev, CONFIG5) | (1<<AGCRESET_SHIFT));
+		read_nic_byte(dev, CONFIG5);
+		udelay(15);
+		write_nic_byte(dev, CONFIG5,
+			read_nic_byte(dev, CONFIG5) &~ (1<<AGCRESET_SHIFT));
+	}else{
+
+		write_nic_byte(dev, WPA_CONFIG, 0);
+		//write_nic_byte(dev, TESTR, 0xd);
+	}
+
+	rtl8180_no_hw_wep(dev);
+
+	if(priv->card_8185){
+		rtl8185_set_rate(dev);
+		write_nic_byte(dev, RATE_FALLBACK, 0x81);
+	//	write_nic_byte(dev, 0xdf, 0x15);
+	}else{
+		word  = read_nic_word(dev, BRSR);
+		word &= ~BRSR_MBR;
+		word &= ~BRSR_BPLCP;
+		word |= ieeerate2rtlrate(priv->ieee80211->basic_rate);
+//by amy
+              word |= 0x0f;
+//by amy
+		write_nic_word(dev, BRSR, word);
+	}
+
+
+	if(priv->card_8185){
+		write_nic_byte(dev, GP_ENABLE,read_nic_byte(dev, GP_ENABLE) & ~(1<<6));
+
+		//FIXME cfg 3 ClkRun enable - isn't it ReadOnly ?
+		rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
+		write_nic_byte(dev,CONFIG3, read_nic_byte(dev, CONFIG3)
+|(1<<CONFIG3_CLKRUN_SHIFT));
+		rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
+
+	}
+
+	priv->rf_init(dev);
+
+	if(priv->rf_set_sens != NULL)
+		priv->rf_set_sens(dev,priv->sens);
+	rtl8180_irq_enable(dev);
+
+	netif_start_queue(dev);
+	/*DMESG ("lfree %d",get_curr_tx_free_desc(dev,LOW_PRIORITY));
+
+	DMESG ("nfree %d",get_curr_tx_free_desc(dev,NORM_PRIORITY));
+
+	DMESG ("hfree %d",get_curr_tx_free_desc(dev,HI_PRIORITY));
+	if(check_nic_enought_desc(dev,NORM_PRIORITY)) DMESG("NORM OK");
+	if(check_nic_enought_desc(dev,HI_PRIORITY)) DMESG("HI OK");
+	if(check_nic_enought_desc(dev,LOW_PRIORITY)) DMESG("LOW OK");*/
+}
+
+
+
+/* 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)
+{
+//	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u16 word;
+//	DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+
+	DMESG("Enabling beacon TX");
+	//write_nic_byte(dev, 0x42,0xe6);// TCR
+//	set_nic_txring(dev);
+//	fix_tx_fifo(dev);
+	rtl8180_prepare_beacon(dev);
+	rtl8180_irq_disable(dev);
+	rtl8180_beacon_tx_enable(dev);
+#if 0
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+	//write_nic_byte(dev,0x9d,0x20); //DMA Poll
+	//write_nic_word(dev,0x7a,0);
+	//write_nic_word(dev,0x7a,0x8000);
+
+#if 0
+	word  = read_nic_word(dev, BcnItv);
+	word &= ~BcnItv_BcnItv; // clear Bcn_Itv
+	word |= priv->ieee80211->current_network.beacon_interval;//0x64;
+	write_nic_word(dev, BcnItv, word);
+#endif
+#endif
+	word = read_nic_word(dev, AtimWnd) &~ AtimWnd_AtimWnd;
+	write_nic_word(dev, AtimWnd,word);// word |=
+//priv->ieee80211->current_network.atim_window);
+
+	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);
+
+//	rtl8180_beacon_tx_enable(dev);
+#ifdef CONFIG_RTL8185B
+        rtl8185b_irq_enable(dev);
+#else
+	rtl8180_irq_enable(dev);
+#endif
+	/* VV !!!!!!!!!! VV*/
+	/*
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+	write_nic_byte(dev,0x9d,0x00);
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+*/
+//	DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+
+}
+
+
+
+/***************************************************************************
+    -------------------------------NET STUFF---------------------------
+***************************************************************************/
+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.
+// 2005.01.06, by rcnjko.
+//
+bool
+MgntActSet_802_11_PowerSaveMode(
+	struct r8180_priv *priv,
+	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;
+	}
+
+	//
+	// <RJ_NOTE> If we make HW to fill up the PwrMgt bit for us,
+	// some AP will not response to our mgnt frames with PwrMgt bit set,
+	// e.g. cannot associate the AP.
+	// So I commented out it. 2005.02.16, by rcnjko.
+	//
+//	// Change device's power save mode.
+//	Adapter->HalFunc.SetPSModeHandler( Adapter, rtPsMode );
+
+	// Update power save mode configured.
+//	priv->dot11PowerSaveMode = rtPsMode;
+	priv->ieee80211->ps = rtPsMode;
+	// Determine ListenInterval.
+#if 0
+	if(priv->dot11PowerSaveMode == eMaxPs)
+	{
+		priv->ieee80211->ListenInterval = 10;
+	}
+	else
+	{
+		priv->ieee80211->ListenInterval = 2;
+	}
+#endif
+	return true;
+}
+
+//================================================================================
+// Leisure Power Save in linked state.
+//================================================================================
+
+//
+//	Description:
+//		Enter the leisure power save mode.
+//
+void
+LeisurePSEnter(
+	struct r8180_priv *priv
+	)
+{
+	if (priv->bLeisurePs)
+	{
+		if (priv->ieee80211->ps == IEEE80211_PS_DISABLED)
+		{
+			//printk("----Enter PS\n");
+			MgntActSet_802_11_PowerSaveMode(priv, IEEE80211_PS_MBCAST|IEEE80211_PS_UNICAST);//IEEE80211_PS_ENABLE
+		}
+	}
+}
+
+
+//
+//	Description:
+//		Leave the leisure power save mode.
+//
+void
+LeisurePSLeave(
+	struct r8180_priv *priv
+	)
+{
+	if (priv->bLeisurePs)
+	{
+		if (priv->ieee80211->ps != IEEE80211_PS_DISABLED)
+		{
+			//printk("----Leave PS\n");
+			MgntActSet_802_11_PowerSaveMode(priv, IEEE80211_PS_DISABLED);
+		}
+	}
+}
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_hw_wakeup_wq (struct work_struct *work)
+{
+//	struct r8180_priv *priv = container_of(work, struct r8180_priv, watch_dog_wq);
+//	struct ieee80211_device * ieee = (struct ieee80211_device*)
+//	                                       container_of(work, struct ieee80211_device, watch_dog_wq);
+	struct delayed_work *dwork = container_of(work,struct delayed_work,work);
+	struct ieee80211_device *ieee = container_of(dwork,struct ieee80211_device,hw_wakeup_wq);
+	struct net_device *dev = ieee->dev;
+#else
+void rtl8180_hw_wakeup_wq(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+#endif
+
+//	printk("dev is %d\n",dev);
+//	printk("&*&(^*(&(&=========>%s()\n", __func__);
+	rtl8180_hw_wakeup(dev);
+
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_hw_sleep_wq (struct work_struct *work)
+{
+//      struct r8180_priv *priv = container_of(work, struct r8180_priv, watch_dog_wq);
+//      struct ieee80211_device * ieee = (struct ieee80211_device*)
+//                                             container_of(work, struct ieee80211_device, watch_dog_wq);
+        struct delayed_work *dwork = container_of(work,struct delayed_work,work);
+        struct ieee80211_device *ieee = container_of(dwork,struct ieee80211_device,hw_sleep_wq);
+        struct net_device *dev = ieee->dev;
+#else
+void rtl8180_hw_sleep_wq(struct net_device *dev)
+{
+        struct r8180_priv *priv = ieee80211_priv(dev);
+#endif
+
+        rtl8180_hw_sleep_down(dev);
+}
+
+//YJ,add,080828,for KeepAlive
+static void MgntLinkKeepAlive(struct r8180_priv *priv )
+{
+	if (priv->keepAliveLevel == 0)
+		return;
+
+	if(priv->ieee80211->state == IEEE80211_LINKED)
+	{
+		//
+		// Keep-Alive.
+		//
+		//printk("LastTx:%d Tx:%d LastRx:%d Rx:%ld Idle:%d\n",priv->link_detect.LastNumTxUnicast,priv->NumTxUnicast, priv->link_detect.LastNumRxUnicast, priv->ieee80211->NumRxUnicast, priv->link_detect.IdleCount);
+
+		if ( (priv->keepAliveLevel== 2) ||
+			(priv->link_detect.LastNumTxUnicast == priv->NumTxUnicast &&
+			priv->link_detect.LastNumRxUnicast == priv->ieee80211->NumRxUnicast )
+			)
+		{
+			priv->link_detect.IdleCount++;
+
+			//
+			// Send a Keep-Alive packet packet to AP if we had been idle for a while.
+			//
+			if(priv->link_detect.IdleCount >= ((KEEP_ALIVE_INTERVAL / CHECK_FOR_HANG_PERIOD)-1) )
+			{
+				priv->link_detect.IdleCount = 0;
+				ieee80211_sta_ps_send_null_frame(priv->ieee80211, false);
+			}
+		}
+		else
+		{
+			priv->link_detect.IdleCount = 0;
+		}
+		priv->link_detect.LastNumTxUnicast = priv->NumTxUnicast;
+		priv->link_detect.LastNumRxUnicast = priv->ieee80211->NumRxUnicast;
+	}
+}
+//YJ,add,080828,for KeepAlive,end
+
+static u8 read_acadapter_file(char *filename);
+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;
+#ifdef ENABLE_IPS
+	if(priv->ieee80211->actscanning == false){
+		if((priv->ieee80211->iw_mode != IW_MODE_ADHOC) && (priv->ieee80211->state == IEEE80211_NOLINK) && (priv->ieee80211->beinretry == false) && (priv->eRFPowerState == eRfOn)){
+			IPSEnter(dev);
+		}
+	}
+#endif
+	//YJ,add,080828,for link state check
+	if((priv->ieee80211->state == IEEE80211_LINKED) && (priv->ieee80211->iw_mode == IW_MODE_INFRA)){
+		SlotIndex = (priv->link_detect.SlotIndex++) % priv->link_detect.SlotNum;
+		priv->link_detect.RxFrameNum[SlotIndex] = priv->ieee80211->NumRxDataInPeriod + priv->ieee80211->NumRxBcnInPeriod;
+		for( i=0; i<priv->link_detect.SlotNum; i++ )
+			TotalRxNum+= priv->link_detect.RxFrameNum[i];
+		//printk("&&&&&=== TotalRxNum = %d\n", TotalRxNum);
+		if(TotalRxNum == 0){
+			priv->ieee80211->state = IEEE80211_ASSOCIATING;
+			queue_work(priv->ieee80211->wq, &priv->ieee80211->associate_procedure_wq);
+		}
+	}
+
+	//YJ,add,080828,for KeepAlive
+	MgntLinkKeepAlive(priv);
+
+	//YJ,add,080828,for LPS
+#ifdef ENABLE_LPS
+	if(priv->PowerProfile == POWER_PROFILE_BATTERY )
+	{
+		//Turn on LeisurePS on battery power
+		//printk("!!!!!On battery power\n");
+		priv->bLeisurePs = true;
+	}
+	else if(priv->PowerProfile == POWER_PROFILE_AC )
+	{
+		// Turn off LeisurePS on AC power
+		//printk("----On AC power\n");
+		LeisurePSLeave(priv);
+		priv->bLeisurePs= false;
+	}
+#endif
+
+#if 0
+#ifndef ENABLE_LPS
+	if(priv->ieee80211->state == IEEE80211_LINKED){
+		if(	priv->NumRxOkInPeriod> 666 ||
+			priv->NumTxOkInPeriod > 666 ) {
+			bBusyTraffic = true;
+		}
+		if((priv->ieee80211->NumRxData + priv->NumTxOkInPeriod)<8) {
+			bEnterPS= true;
+		}
+		if(bEnterPS) {
+			LeisurePSEnter(priv);
+		}
+		else {
+			LeisurePSLeave(priv);
+		}
+	}
+	else	{
+		LeisurePSLeave(priv);
+	}
+#endif
+	priv->NumRxOkInPeriod = 0;
+	priv->NumTxOkInPeriod = 0;
+	priv->ieee80211->NumRxData = 0;
+#else
+#ifdef ENABLE_LPS
+	if(priv->ieee80211->state == IEEE80211_LINKED){
+		priv->link_detect.NumRxOkInPeriod = priv->ieee80211->NumRxDataInPeriod;
+		//printk("TxOk=%d RxOk=%d\n", priv->link_detect.NumTxOkInPeriod, priv->link_detect.NumRxOkInPeriod);
+		if(	priv->link_detect.NumRxOkInPeriod> 666 ||
+			priv->link_detect.NumTxOkInPeriod> 666 ) {
+			bBusyTraffic = true;
+		}
+		if(((priv->link_detect.NumRxOkInPeriod + priv->link_detect.NumTxOkInPeriod) > 8)
+			|| (priv->link_detect.NumRxOkInPeriod > 2)) {
+			bEnterPS= false;
+		}
+		else {
+			bEnterPS= true;
+		}
+
+		if(bEnterPS) {
+			LeisurePSEnter(priv);
+		}
+		else {
+			LeisurePSLeave(priv);
+		}
+	}
+	else{
+		LeisurePSLeave(priv);
+	}
+#endif
+	priv->link_detect.bBusyTraffic = bBusyTraffic;
+	priv->link_detect.NumRxOkInPeriod = 0;
+	priv->link_detect.NumTxOkInPeriod = 0;
+	priv->ieee80211->NumRxDataInPeriod = 0;
+	priv->ieee80211->NumRxBcnInPeriod = 0;
+#endif
+}
+int _rtl8180_up(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	//int i;
+
+	priv->up=1;
+
+	DMESG("Bringing up iface");
+#ifdef CONFIG_RTL8185B
+	rtl8185b_adapter_start(dev);
+	rtl8185b_rx_enable(dev);
+	rtl8185b_tx_enable(dev);
+#else
+	rtl8180_adapter_start(dev);
+	rtl8180_rx_enable(dev);
+	rtl8180_tx_enable(dev);
+#endif
+#ifdef ENABLE_IPS
+	if(priv->bInactivePs){
+		if(priv->ieee80211->iw_mode == IW_MODE_ADHOC)
+			IPSLeave(dev);
+	}
+#endif
+//by amy 080312
+#ifdef RATE_ADAPT
+       timer_rate_adaptive((unsigned long)dev);
+#endif
+//by amy 080312
+	watch_dog_adaptive((unsigned long)dev);
+#ifdef SW_ANTE
+        if(priv->bSwAntennaDiverity)
+			SwAntennaDiversityTimerCallback(dev);
+#endif
+//	IPSEnter(dev);
+	ieee80211_softmac_start_protocol(priv->ieee80211);
+
+//Add for RF power on power off by lizhaoming 080512
+//	priv->eRFPowerState = eRfOn;
+//	printk("\n--------Start queue_work:GPIOChangeRFWorkItem");
+//	queue_delayed_work(priv->ieee80211->wq,&priv->ieee80211->GPIOChangeRFWorkItem,1000);
+
+	return 0;
+}
+
+
+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);
+}
+
+
+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);
+	//cancel_delayed_work(&priv->ieee80211->watch_dog_wq);
+//{by amy 080312
+    del_timer_sync(&priv->rateadapter_timer);
+    cancel_delayed_work(&priv->ieee80211->rate_adapter_wq);
+//by amy 080312}
+	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,eRfOff);
+	//ieee80211_softmac_stop_protocol(priv->ieee80211);
+	memset(&(priv->ieee80211->current_network),0,sizeof(struct ieee80211_network));
+	priv->ieee80211->state = IEEE80211_NOLINK;
+	return 0;
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+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;
+#else
+void rtl8180_restart_wq(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+#endif
+	down(&priv->wx_sem);
+
+	rtl8180_commit(dev);
+
+	up(&priv->wx_sem);
+}
+
+void rtl8180_restart(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	//rtl8180_commit(dev);
+	schedule_work(&priv->reset_wq);
+	//DMESG("TXTIMEOUT");
+}
+
+
+void rtl8180_commit(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	if (priv->up == 0) return ;
+//+by amy 080312
+	del_timer_sync(&priv->watch_dog_timer);
+	//cancel_delayed_work(&priv->ieee80211->watch_dog_wq);
+//{by amy 080312
+//by amy for rate adaptive
+    del_timer_sync(&priv->rateadapter_timer);
+    cancel_delayed_work(&priv->ieee80211->rate_adapter_wq);
+//by amy for rate adaptive
+//by amy 080312}
+	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;
+
+	//down(&priv->wx_sem);
+
+	promisc = (dev->flags & IFF_PROMISC) ? 1:0;
+
+	if (promisc != priv->promisc)
+		rtl8180_restart(dev);
+
+	priv->promisc = promisc;
+
+	//up(&priv->wx_sem);
+}
+
+#if 0
+/* this is called by the kernel when it needs to TX a 802.3 encapsulated frame*/
+int rtl8180_8023_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int ret;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->tx_lock,flags);
+	ret = ieee80211_r8180_8023_hardstartxmit(skb,priv->ieee80211);
+	spin_unlock_irqrestore(&priv->tx_lock,flags);
+	return ret;
+}
+#endif
+
+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 */
+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;
+}
+
+
+
+/****************************************************************************
+     -----------------------------PCI STUFF---------------------------
+*****************************************************************************/
+
+
+static int __devinit 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 *ptr;
+	u8 unit = 0;
+
+#ifdef CONFIG_RTL8180_IO_MAP
+	unsigned long pio_start, pio_len, pio_flags;
+#else
+	unsigned long pmem_start, pmem_len, pmem_flags;
+#endif //end #ifdef RTL_IO_MAP
+
+	DMESG("Configuring chip resources");
+
+	if( pci_enable_device (pdev) ){
+		DMESG("Failed to enable PCI device");
+		return -EIO;
+	}
+
+	pci_set_master(pdev);
+	//pci_set_wmi(pdev);
+	pci_set_dma_mask(pdev, 0xffffff00ULL);
+	pci_set_consistent_dma_mask(pdev,0xffffff00ULL);
+	dev = alloc_ieee80211(sizeof(struct r8180_priv));
+	if (!dev)
+		return -ENOMEM;
+	priv = ieee80211_priv(dev);
+	priv->ieee80211 = netdev_priv(dev);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
+        SET_MODULE_OWNER(dev);
+#endif
+	pci_set_drvdata(pdev, dev);
+	SET_NETDEV_DEV(dev, &pdev->dev);
+
+	priv = ieee80211_priv(dev);
+//	memset(priv,0,sizeof(struct r8180_priv));
+	priv->pdev=pdev;
+
+
+#ifdef CONFIG_RTL8180_IO_MAP
+
+	pio_start = (unsigned long)pci_resource_start (pdev, 0);
+	pio_len = (unsigned long)pci_resource_len (pdev, 0);
+	pio_flags = (unsigned long)pci_resource_flags (pdev, 0);
+
+      	if (!(pio_flags & IORESOURCE_IO)) {
+		DMESG("region #0 not a PIO resource, aborting");
+		goto fail;
+	}
+
+	//DMESG("IO space @ 0x%08lx", pio_start );
+	if( ! request_region( pio_start, pio_len, RTL8180_MODULE_NAME ) ){
+		DMESG("request_region failed!");
+		goto fail;
+	}
+
+	ioaddr = pio_start;
+	dev->base_addr = ioaddr; // device I/O address
+
+#else
+
+	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;
+	}
+
+	//DMESG("Memory mapped space @ 0x%08lx ", pmem_start);
+	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!");
+	       // release_mem_region( pmem_start, pmem_len );
+		goto fail1;
+	}
+
+	dev->mem_start = ioaddr; // shared mem start
+	dev->mem_end = ioaddr + pci_resource_len(pdev, 0); // shared mem end
+
+#endif //end #ifdef RTL_IO_MAP
+
+#ifdef CONFIG_RTL8185B
+	//pci_read_config_byte(pdev, 0x05, ptr);
+	//pci_write_config_byte(pdev, 0x05, (*ptr) & (~0x04));
+	pci_read_config_byte(pdev, 0x05, &unit);
+	pci_write_config_byte(pdev, 0x05, unit & (~0x04));
+#endif
+
+	dev->irq = pdev->irq;
+	priv->irq = 0;
+
+	dev->open = rtl8180_open;
+	dev->stop = rtl8180_close;
+	//dev->hard_start_xmit = ieee80211_xmit;
+	dev->tx_timeout = rtl8180_restart;
+	dev->wireless_handlers = &r8180_wx_handlers_def;
+	dev->do_ioctl = rtl8180_ioctl;
+	dev->set_multicast_list = r8180_set_multicast;
+	dev->set_mac_address = r8180_set_mac_adr;
+
+#if WIRELESS_EXT >= 12
+#if WIRELESS_EXT < 17
+	dev->get_wireless_stats = r8180_get_wireless_stats;
+#endif
+	dev->wireless_handlers = (struct iw_handler_def *) &r8180_wx_handlers_def;
+#endif
+
+	dev->type=ARPHRD_ETHER;
+	dev->watchdog_timeo = HZ*3; //added by david woo, 2007.12.13
+
+	if (dev_alloc_name(dev, ifname) < 0){
+                DMESG("Oops: devname already taken! Trying wlan%%d...\n");
+		ifname = "wlan%d";
+	//	ifname = "ath%d";
+		dev_alloc_name(dev, ifname);
+        }
+
+
+	if(rtl8180_init(dev)!=0){
+		DMESG("Initialization failed");
+		goto fail1;
+	}
+
+	netif_carrier_off(dev);
+
+	register_netdev(dev);
+
+	rtl8180_proc_init_one(dev);
+
+	DMESG("Driver probe completed\n");
+	return 0;
+
+fail1:
+
+#ifdef CONFIG_RTL8180_IO_MAP
+
+	if( dev->base_addr != 0 ){
+
+		release_region(dev->base_addr,
+	       pci_resource_len(pdev, 0) );
+	}
+#else
+	if( dev->mem_start != (unsigned long)NULL ){
+		iounmap( (void *)dev->mem_start );
+		release_mem_region( pci_resource_start(pdev, 1),
+				    pci_resource_len(pdev, 1) );
+	}
+#endif //end #ifdef RTL_IO_MAP
+
+
+fail:
+	if(dev){
+
+		if (priv->irq) {
+			free_irq(dev->irq, dev);
+			dev->irq=0;
+		}
+		free_ieee80211(dev);
+	}
+
+	pci_disable_device(pdev);
+
+	DMESG("wlan driver load failed\n");
+	pci_set_drvdata(pdev, NULL);
+	return -ENODEV;
+
+}
+
+
+static void __devexit 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);
+		//rtl8180_rtx_disable(dev);
+		//rtl8180_irq_disable(dev);
+		mdelay(10);
+		//write_nic_word(dev,INTA,read_nic_word(dev,INTA));
+		//force_pci_posting(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);
+	//	free_beacon_desc_ring(dev,priv->txbeaconcount);
+
+#ifdef CONFIG_RTL8180_IO_MAP
+
+		if( dev->base_addr != 0 ){
+
+			release_region(dev->base_addr,
+				       pci_resource_len(pdev, 0) );
+		}
+#else
+		if( dev->mem_start != (unsigned long)NULL ){
+			iounmap( (void *)dev->mem_start );
+			release_mem_region( pci_resource_start(pdev, 1),
+					    pci_resource_len(pdev, 1) );
+		}
+#endif /*end #ifdef RTL_IO_MAP*/
+
+		free_ieee80211(dev);
+	}
+	pci_disable_device(pdev);
+
+	DMESG("wlan driver removed\n");
+}
+
+
+/* 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);
+
+static int __init rtl8180_pci_module_init(void)
+{
+	int ret;
+
+	ret = ieee80211_crypto_init();
+	if (ret) {
+		printk(KERN_ERR "ieee80211_crypto_init() failed %d\n", ret);
+		return ret;
+	}
+	ret = ieee80211_crypto_tkip_init();
+	if (ret) {
+		printk(KERN_ERR "ieee80211_crypto_tkip_init() failed %d\n", ret);
+		return ret;
+	}
+	ret = ieee80211_crypto_ccmp_init();
+	if (ret) {
+		printk(KERN_ERR "ieee80211_crypto_ccmp_init() failed %d\n", ret);
+		return ret;
+	}
+	ret = ieee80211_crypto_wep_init();
+	if (ret) {
+		printk(KERN_ERR "ieee80211_crypto_wep_init() failed %d\n", ret);
+		return ret;
+	}
+
+	printk(KERN_INFO "\nLinux kernel driver for RTL8180 \
+/ RTL8185 based WLAN cards\n");
+	printk(KERN_INFO "Copyright (c) 2004-2005, Andrea Merello\n");
+	DMESG("Initializing module");
+	DMESG("Wireless extensions version %d", WIRELESS_EXT);
+	rtl8180_proc_module_init();
+
+#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22))
+      if(0!=pci_module_init(&rtl8180_pci_driver))
+#else
+      if(0!=pci_register_driver(&rtl8180_pci_driver))
+#endif
+	//if(0!=pci_module_init(&rtl8180_pci_driver))
+	{
+		DMESG("No device found");
+		/*pci_unregister_driver (&rtl8180_pci_driver);*/
+		return -ENODEV;
+	}
+	return 0;
+}
+
+
+static void __exit rtl8180_pci_module_exit(void)
+{
+	pci_unregister_driver (&rtl8180_pci_driver);
+	rtl8180_proc_module_remove();
+	ieee80211_crypto_deinit();
+	ieee80211_crypto_tkip_exit();
+	ieee80211_crypto_ccmp_exit();
+	ieee80211_crypto_wep_exit();
+	DMESG("Exiting");
+}
+
+
+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_wake_queue(priv->ieee80211);
+}
+
+/*****************************************************************************
+      -----------------------------IRQ STUFF---------------------------
+******************************************************************************/
+
+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 *txdv; //packet just TXed
+	u32 nic; //nic pointer physical addr
+	u32 nicbegin;// start of ring physical addr
+//	short txed;
+	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++; //tony 20060601
+	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 ;
+	}
+/*	DMESG("%x %s %x %x",((int)nic & 0xfff)/8/4,
+	*(priv->txnpring + ((int)nic&0xfff)/4/8) & (1<<31) ? "filled" : "empty",
+	(priv->txnpringtail - priv->txnpring)/8,(priv->txnpringhead -
+priv->txnpring)/8);
+*/
+	//nicv = (u32*) ((nic - nicbegin) + (int)begin);
+	nicv = (u32*) ((nic - nicbegin) + (u8*)begin);
+	if((head <= tail && (nicv > tail || nicv < head)) ||
+		(head > tail && (nicv > tail && nicv < head))){
+
+			DMESGW("nic has lost pointer");
+#ifdef DEBUG_TX_DESC
+			//check_tx_ring(dev,NORM_PRIORITY);
+			check_tx_ring(dev,pri);
+#endif
+			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 currenly pointed by the nic (the next to be txed)
+	 * and the previous of the pointed (might be in process ??)
+	*/
+	//if (head == nic) return;
+	//DMESG("%x %x",head,nic);
+	offs = (nic - nicbegin);
+	//DMESG("%x %x %x",nic ,(u32)nicbegin, (int)nic -nicbegin);
+
+	offs = offs / 8 /4;
+
+	hd = (head - begin) /8;
+
+	if(offs >= hd)
+		j = offs - hd;
+	else
+		j = offs + (priv->txringcount -1 -hd);
+	//	j= priv->txringcount -1- (hd - offs);
+
+	j-=2;
+	if(j<0) j=0;
+
+
+	for(i=0;i<j;i++)
+	{
+//		printk("+++++++++++++check status desc\n");
+		if((*head) & (1<<31))
+			break;
+		if(((*head)&(0x10000000)) != 0){
+//			printk("++++++++++++++last desc,retry count is %d\n",((*head) & (0x000000ff)));
+			priv->CurrRetryCnt += (u16)((*head) & (0x000000ff));
+#if 1
+			if(!error)
+			{
+				priv->NumTxOkTotal++;
+//				printk("NumTxOkTotal is %d\n",priv->NumTxOkTotal++);
+			}
+#endif
+			//	printk("in function %s:curr_retry_count is %d\n",__func__,((*head) & (0x000000ff)));
+		}
+		if(!error){
+			priv->NumTxOkBytesTotal += (*(head+3)) & (0x00000fff);
+		}
+//		printk("in function %s:curr_txokbyte_count is %d\n",__func__,(*(head+3)) & (0x00000fff));
+		*head = *head &~ (1<<31);
+
+		if((head - begin)/8 == priv->txringcount-1)
+			head=begin;
+
+		else
+			head+=8;
+	}
+#if 0
+	if(nicv == begin)
+		txdv = begin + (priv->txringcount -1)*8;
+	else
+		txdv = nicv - 8;
+
+	txed = !(txdv[0] &(1<<31));
+
+	if(txed){
+		if(!(txdv[0] & (1<<15))) error = 1;
+		//if(!(txdv[0] & (1<<30))) error = 1;
+		if(error)DMESG("%x",txdv[0]);
+ 	}
+#endif
+	//DMESG("%x",txdv[0]);
+	/* 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 occour at all.
+	 */
+
+	switch(pri) {
+	case MANAGE_PRIORITY:
+		priv->txmapringhead = head;
+			//printk("1==========================================> priority check!\n");
+		if(priv->ack_tx_to_ieee){
+				// try to implement power-save mode 2008.1.22
+		//	printk("2==========================================> priority check!\n");
+#if 1
+			if(rtl8180_is_tx_queue_empty(dev)){
+			//	printk("tx queue empty, after send null sleep packet, try to sleep !\n");
+				priv->ack_tx_to_ieee = 0;
+				ieee80211_ps_tx_ack(priv->ieee80211,!error);
+			}
+#endif
+		}
+		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;
+	}
+
+	/*DMESG("%x %x %x", (priv->txnpringhead - priv->txnpring) /8 ,
+		(priv->txnpringtail - priv->txnpring) /8,
+		offs );
+	*/
+
+	spin_unlock_irqrestore(&priv->tx_lock,flag);
+
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_tx_irq_wq(struct work_struct *work)
+{
+	//struct r8180_priv *priv = container_of(work, struct r8180_priv, reset_wq);
+        struct delayed_work *dwork = container_of(work,struct delayed_work,work);
+	struct ieee80211_device * ieee = (struct ieee80211_device*)
+	                                       container_of(dwork, struct ieee80211_device, watch_dog_wq);
+	struct net_device *dev = ieee->dev;
+#else
+void rtl8180_tx_irq_wq(struct net_device *dev)
+{
+	//struct r8180_priv *priv = ieee80211_priv(dev);
+#endif
+	rtl8180_tx_isr(dev,MANAGE_PRIORITY,0);
+}
+irqreturn_t rtl8180_interrupt(int irq, void *netdev, struct pt_regs *regs)
+{
+	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);
+
+#ifdef CONFIG_RTL8185B
+	//ISR: 4bytes
+	inta = read_nic_dword(dev, ISR);// & priv->IntrMask;
+	write_nic_dword(dev,ISR,inta); // reset int situation
+#else
+	inta = read_nic_word(dev,INTA) & priv->irq_mask;
+	write_nic_word(dev,INTA,inta); // reset int situation
+#endif
+
+	priv->stats.shints++;
+
+	//DMESG("Enter interrupt, ISR value = 0x%08x", inta);
+
+	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 disappared */
+			spin_unlock_irqrestore(&priv->irq_th_lock,flags);
+			return IRQ_HANDLED;
+	}
+
+	priv->stats.ints++;
+#ifdef DEBUG_IRQ
+	DMESG("NIC irq %x",inta);
+#endif
+	//priv->irqpending = inta;
+
+
+	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);
+		//DMESG("=================>waking up");
+//		rtl8180_hw_wakeup(dev);
+	}
+
+	if(inta & ISR_TBDOK){
+		priv->stats.txbeacon++;
+	}
+
+	if(inta & ISR_TBDER){
+		priv->stats.txbeaconerr++;
+	}
+
+	if(inta  & IMR_TMGDOK ) {
+//		priv->NumTxOkTotal++;
+		rtl8180_tx_isr(dev,MANAGE_PRIORITY,0);
+//			schedule_work(&priv->tx_irq_wq);
+
+	}
+
+	if(inta & ISR_THPDER){
+#ifdef DEBUG_TX
+		DMESG ("TX high priority ERR");
+#endif
+		priv->stats.txhperr++;
+		rtl8180_tx_isr(dev,HI_PRIORITY,1);
+		priv->ieee80211->stats.tx_errors++;
+	}
+
+	if(inta & ISR_THPDOK){ //High priority tx ok
+#ifdef DEBUG_TX
+		DMESG ("TX high priority OK");
+#endif
+//		priv->NumTxOkTotal++;
+		//priv->NumTxOkInPeriod++;  //YJ,del,080828
+		priv->link_detect.NumTxOkInPeriod++; //YJ,add,080828
+		priv->stats.txhpokint++;
+		rtl8180_tx_isr(dev,HI_PRIORITY,0);
+	}
+
+	if(inta & ISR_RER) {
+		priv->stats.rxerr++;
+#ifdef DEBUG_RX
+		DMESGW("RX error int");
+#endif
+	}
+#ifdef CONFIG_RTL8185B
+	if(inta & ISR_TBKDER){ //corresponding to BK_PRIORITY
+		priv->stats.txbkperr++;
+		priv->ieee80211->stats.tx_errors++;
+#ifdef DEBUG_TX
+		DMESGW("TX bkp error int");
+#endif
+		//tasklet_schedule(&priv->irq_tx_tasklet);
+		rtl8180_tx_isr(dev,BK_PRIORITY,1);
+		rtl8180_try_wake_queue(dev, BE_PRIORITY);
+	}
+
+	if(inta & ISR_TBEDER){ //corresponding to BE_PRIORITY
+		priv->stats.txbeperr++;
+		priv->ieee80211->stats.tx_errors++;
+#ifdef DEBUG_TX
+		DMESGW("TX bep error int");
+#endif
+		rtl8180_tx_isr(dev,BE_PRIORITY,1);
+		//tasklet_schedule(&priv->irq_tx_tasklet);
+		rtl8180_try_wake_queue(dev, BE_PRIORITY);
+	}
+#endif
+	if(inta & ISR_TNPDER){ //corresponding to VO_PRIORITY
+		priv->stats.txnperr++;
+		priv->ieee80211->stats.tx_errors++;
+#ifdef DEBUG_TX
+		DMESGW("TX np error int");
+#endif
+		//tasklet_schedule(&priv->irq_tx_tasklet);
+		rtl8180_tx_isr(dev,NORM_PRIORITY,1);
+#ifdef CONFIG_RTL8185B
+		rtl8180_try_wake_queue(dev, NORM_PRIORITY);
+#endif
+	}
+
+	if(inta & ISR_TLPDER){ //corresponding to VI_PRIORITY
+		priv->stats.txlperr++;
+		priv->ieee80211->stats.tx_errors++;
+#ifdef DEBUG_TX
+		DMESGW("TX lp error int");
+#endif
+		rtl8180_tx_isr(dev,LOW_PRIORITY,1);
+		//tasklet_schedule(&priv->irq_tx_tasklet);
+		rtl8180_try_wake_queue(dev, LOW_PRIORITY);
+	}
+
+	if(inta & ISR_ROK){
+#ifdef DEBUG_RX
+		DMESG("Frame arrived !");
+#endif
+		//priv->NumRxOkInPeriod++;  //YJ,del,080828
+		priv->stats.rxint++;
+		tasklet_schedule(&priv->irq_rx_tasklet);
+	}
+
+	if(inta & ISR_RQoSOK ){
+#ifdef DEBUG_RX
+		DMESG("QoS Frame arrived !");
+#endif
+		//priv->NumRxOkInPeriod++;  //YJ,del,080828
+		priv->stats.rxint++;
+		tasklet_schedule(&priv->irq_rx_tasklet);
+	}
+	if(inta & ISR_BcnInt) {
+		//DMESG("Preparing Beacons");
+		rtl8180_prepare_beacon(dev);
+	}
+
+	if(inta & ISR_RDU){
+//#ifdef DEBUG_RX
+		DMESGW("No RX descriptor available");
+		priv->stats.rxrdu++;
+//#endif
+		tasklet_schedule(&priv->irq_rx_tasklet);
+		/*queue_work(priv->workqueue ,&priv->restart_work);*/
+
+	}
+	if(inta & ISR_RXFOVW){
+#ifdef DEBUG_RX
+		DMESGW("RX fifo overflow");
+#endif
+		priv->stats.rxoverflow++;
+		tasklet_schedule(&priv->irq_rx_tasklet);
+		//queue_work(priv->workqueue ,&priv->restart_work);
+	}
+
+	if(inta & ISR_TXFOVW) priv->stats.txoverflow++;
+
+	if(inta & ISR_TNPDOK){ //Normal priority tx ok
+#ifdef DEBUG_TX
+		DMESG ("TX normal priority OK");
+#endif
+//		priv->NumTxOkTotal++;
+		//priv->NumTxOkInPeriod++;  //YJ,del,080828
+		priv->link_detect.NumTxOkInPeriod++; //YJ,add,080828
+		//	priv->ieee80211->stats.tx_packets++;
+		priv->stats.txnpokint++;
+		rtl8180_tx_isr(dev,NORM_PRIORITY,0);
+	}
+
+	if(inta & ISR_TLPDOK){ //Low priority tx ok
+#ifdef DEBUG_TX
+		DMESG ("TX low priority OK");
+#endif
+//		priv->NumTxOkTotal++;
+		//priv->NumTxOkInPeriod++;  //YJ,del,080828
+		priv->link_detect.NumTxOkInPeriod++; //YJ,add,080828
+		//	priv->ieee80211->stats.tx_packets++;
+		priv->stats.txlpokint++;
+		rtl8180_tx_isr(dev,LOW_PRIORITY,0);
+		rtl8180_try_wake_queue(dev, LOW_PRIORITY);
+	}
+
+#ifdef CONFIG_RTL8185B
+	if(inta & ISR_TBKDOK){ //corresponding to BK_PRIORITY
+		priv->stats.txbkpokint++;
+#ifdef DEBUG_TX
+		DMESGW("TX bk priority ok");
+#endif
+//		priv->NumTxOkTotal++;
+		//priv->NumTxOkInPeriod++;  //YJ,del,080828
+		priv->link_detect.NumTxOkInPeriod++; //YJ,add,080828
+		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++;
+#ifdef DEBUG_TX
+		DMESGW("TX be priority ok");
+#endif
+//		priv->NumTxOkTotal++;
+		//priv->NumTxOkInPeriod++;  //YJ,del,080828
+		priv->link_detect.NumTxOkInPeriod++; //YJ,add,080828
+		rtl8180_tx_isr(dev,BE_PRIORITY,0);
+		rtl8180_try_wake_queue(dev, BE_PRIORITY);
+	}
+#endif
+	force_pci_posting(dev);
+	spin_unlock_irqrestore(&priv->irq_th_lock,flags);
+
+	return IRQ_HANDLED;
+}
+
+
+void rtl8180_irq_rx_tasklet(struct r8180_priv* priv)
+{
+//	unsigned long flags;
+
+/*	spin_lock_irqsave(&priv->irq_lock, flags);
+	priv->irq_mask &=~IMR_ROK;
+	priv->irq_mask &=~IMR_RDU;
+
+	rtl8180_irq_enable(priv->dev);
+	spin_unlock_irqrestore(&priv->irq_lock, flags);
+*/
+	rtl8180_rx(priv->dev);
+
+/*	spin_lock_irqsave(&priv->irq_lock, flags);
+	priv->irq_mask |= IMR_ROK;
+	priv->irq_mask |= IMR_RDU;
+	rtl8180_irq_enable(priv->dev);
+	spin_unlock_irqrestore(&priv->irq_lock, flags);
+*/
+}
+
+/****************************************************************************
+lizhaoming--------------------------- RF power on/power off -----------------
+*****************************************************************************/
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void GPIOChangeRFWorkItemCallBack(struct work_struct *work)
+{
+	//struct delayed_work *dwork = container_of(work, struct delayed_work, 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);
+#else
+void GPIOChangeRFWorkItemCallBack(struct ieee80211_device *ieee)
+{
+	struct net_device *dev = ieee->dev;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+#endif
+
+	//u16 tmp2byte;
+	u8 btPSR;
+	u8 btConfig0;
+	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 = 0;
+	//printk("============>%s in \n", __func__);
+
+#ifdef ENABLE_LPS
+	if(readf_count % 10 == 0)
+		priv->PowerProfile = read_acadapter_file("/proc/acpi/ac_adapter/AC0/state");
+
+	readf_count = (readf_count+1)%0xffff;
+#endif
+#if 0
+	if(priv->up == 0)//driver stopped
+		{
+			printk("\nDo nothing...");
+			goto out;
+		}
+	else
+#endif
+		{
+			// 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 by Jong, 2008-01-16
+			udelay(4);
+
+			//HW radio On/Off according to the value of FF51[4](config0)
+			btConfig0 = btPSR = read_nic_byte(dev, CONFIG0);
+
+			//Turn on LED.
+			write_nic_byte(dev, PSR, btPSR| BIT3);
+
+			eRfPowerStateToSet = (btConfig0 & BIT4) ?  eRfOn : eRfOff;
+
+			if((priv->ieee80211->bHwRadioOff == true) && (eRfPowerStateToSet == eRfOn))
+			{
+				priv->ieee80211->bHwRadioOff = false;
+				bActuallySet = true;
+			}
+			else if((priv->ieee80211->bHwRadioOff == false) && (eRfPowerStateToSet == eRfOff))
+			{
+				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{
+                                        //if(!priv->RfOffReason)
+                                                argv[1] = "RFON";
+                                        //else
+                                        //      argv[1] = "RFOFF";
+                                }
+                                argv[0] = RadioPowerPath;
+                                argv[2] = NULL;
+
+                                call_usermodehelper(RadioPowerPath,argv,envp,1);
+			}
+
+		}
+
+}
+
+static u8 read_acadapter_file(char *filename)
+{
+//#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))
+#if 0
+	int fd;
+	char buf[1];
+	char ret[50];
+	int i = 0;
+	int n = 0;
+	mm_segment_t old_fs = get_fs();
+	set_fs(KERNEL_DS);
+
+	fd = sys_open(filename, O_RDONLY, 0);
+	if (fd >= 0) {
+		while (sys_read(fd, buf, 1) == 1)
+		{
+			i++;
+			if(i>10)
+			{
+				if(buf[0]!=' ')
+				{
+					ret[n]=buf[0];
+					n++;
+				}
+			}
+		}
+		sys_close(fd);
+	}
+	ret[n]='\0';
+//	printk("%s \n", ret);
+	set_fs(old_fs);
+
+	if(strncmp(ret, "off-line",8) == 0)
+	{
+		return 1;
+	}
+#endif
+	return 0;
+}
+
+/***************************************************************************
+     ------------------- module init / exit stubs ----------------
+****************************************************************************/
+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
new file mode 100644
index 000000000000..742dc11fcac9
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_dm.c
@@ -0,0 +1,1725 @@
+//#include "r8180.h"

+#include "r8180_dm.h"

+#include "r8180_hw.h"

+#include "r8180_93cx6.h"

+//{by amy 080312

+

+//

+//	Description:

+//		Return TRUE if we shall perform High Power Mecahnism, FALSE otherwise.	

+//

+//+by amy 080312

+#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 chane 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. 

+//

+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;

+

+	//printk("----> DoTxHighPower()\n");

+

+	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]; 

+

+	//	printk("DoTxHighPower() - UndecoratedSmoothedSS:%d, CurCCKRSSI = %d , bCurCCKPkt= %d \n", priv->UndecoratedSmoothedSS, priv->CurCCKRSSI, priv->bCurCCKPkt );

+	

+	if((priv->UndecoratedSmoothedSS > HiPwrUpperTh) ||

+		(priv->bCurCCKPkt && (priv->CurCCKRSSI > RSSIHiPwrUpperTh)))

+	{

+		// Stevenl suggested that degrade 8dbm in high power sate. 2007-12-04 Isaiah 

+		

+	//	printk("=====>DoTxHighPower() - High Power - UndecoratedSmoothedSS:%d,  HiPwrUpperTh = %d \n", priv->UndecoratedSmoothedSS, HiPwrUpperTh );

+		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))

+	{

+	//	 printk("DoTxHighPower() - lower Power - UndecoratedSmoothedSS:%d,  HiPwrUpperTh = %d \n", priv->UndecoratedSmoothedSS, HiPwrLowerTh );

+		if(priv->bToUpdateTxPwr)

+		{

+			priv->bToUpdateTxPwr = false;

+			//SD3 required.

+			u1bTmp= read_nic_byte(dev, CCK_TXAGC);			

+			if(u1bTmp < CckTxPwrIdx)

+			{

+			//u1bTmp = ((u1bTmp+16) > 35) ? 35: (u1bTmp+16);  // 8dbm

+			//write_nic_byte(dev, CCK_TXAGC, u1bTmp);

+			write_nic_byte(dev, CCK_TXAGC, CckTxPwrIdx);

+			}

+

+			u1bTmp= read_nic_byte(dev, OFDM_TXAGC);

+			if(u1bTmp < OfdmTxPwrIdx)

+			{

+			//u1bTmp = ((u1bTmp+16) > 35) ? 35: (u1bTmp+16);  // 8dbm

+			//write_nic_byte(dev, OFDM_TXAGC, u1bTmp);

+			write_nic_byte(dev, OFDM_TXAGC, OfdmTxPwrIdx);

+			}

+		}

+	}

+

+	//printk("<---- DoTxHighPower()\n");

+}

+

+

+//

+//	Description:

+//		Callback function of UpdateTxPowerWorkItem.

+//		Because of some event happend, e.g. CCX TPC, High Power Mechanism, 

+//		We update Tx power of current channel again. 

+//

+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))

+void rtl8180_tx_pw_wq (struct work_struct *work)

+{

+//      struct r8180_priv *priv = container_of(work, struct r8180_priv, watch_dog_wq);

+//      struct ieee80211_device * ieee = (struct ieee80211_device*)

+//                                             container_of(work, struct ieee80211_device, watch_dog_wq);

+        struct delayed_work *dwork = container_of(work,struct delayed_work,work);

+        struct ieee80211_device *ieee = container_of(dwork,struct ieee80211_device,tx_pw_wq);

+        struct net_device *dev = ieee->dev;

+#else

+void rtl8180_tx_pw_wq(struct net_device *dev)

+{

+	// struct r8180_priv *priv = ieee80211_priv(dev);

+#endif

+

+//	printk("----> UpdateTxPowerWorkItemCallback()\n");

+	

+	DoTxHighPower(dev);	

+	

+//	printk("<---- UpdateTxPowerWorkItemCallback()\n");

+}

+

+

+//

+//	Description:

+//		Return TRUE if we shall perform DIG Mecahnism, 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->CurrentOperaRate < 36) // Schedule Dig under all OFDM rates. By Bruce, 2007-06-01.

+	if((priv->ieee80211->rate/5) < 36) // Schedule Dig under all OFDM rates. By Bruce, 2007-06-01.

+		return false;

+	return true;

+}

+//

+//	Description:

+//		Implementation of DIG for Zebra and Zebra2.	

+//

+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;

+

+	//printk("---------> DIG_Zebra()\n");

+

+	CCKFalseAlarm = (u16)(priv->FalseAlarmRegValue & 0x0000ffff);

+	OFDMFalseAlarm = (u16)((priv->FalseAlarmRegValue >> 16) & 0x0000ffff);

+	OfdmFA1 =  0x15;

+	OfdmFA2 = ((u16)(priv->RegDigOfdmFaUpTh)) << 8;

+

+//	printk("DIG**********CCK False Alarm: %#X \n",CCKFalseAlarm);

+//	printk("DIG**********OFDM False Alarm: %#X \n",OFDMFalseAlarm);

+

+        // 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)

+	}

+	//if(pHalData->VersionID != VERSION_8187B_B)

+	{ // Advised from SD3 DZ

+		OfdmFA1 =  0x20;

+	}

+	

+#if 1 //lzm reserved 080826

+	AwakePeriodIn2Sec = (2000-priv ->DozePeriodInPast2Sec);

+	//printk("&&& DozePeriod=%d AwakePeriod=%d\n", priv->DozePeriodInPast2Sec, AwakePeriodIn2Sec);

+	priv ->DozePeriodInPast2Sec=0;

+	

+	if(AwakePeriodIn2Sec)

+	{	

+		//RT_TRACE(COMP_DIG, DBG_TRACE, ("DIG: AwakePeriodIn2Sec(%d) - FATh(0x%X , 0x%X) ->",AwakePeriodIn2Sec, OfdmFA1, OfdmFA2));

+		// adjuest DIG threshold.

+		OfdmFA1 =  (u16)((OfdmFA1*AwakePeriodIn2Sec)  / 2000) ;  	

+		OfdmFA2 =  (u16)((OfdmFA2*AwakePeriodIn2Sec)  / 2000) ;

+		//RT_TRACE(COMP_DIG, DBG_TRACE, ("( 0x%X , 0x%X)\n", OfdmFA1, OfdmFA2));

+	}

+	else

+	{

+		;//RT_TRACE(COMP_DIG, DBG_WARNING, ("ERROR!!  AwakePeriodIn2Sec should not be ZERO!!\n"));

+	}

+#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);

+//					printk("DIG**********OFDM False Alarm: %#X,  OfdmFA1: %#X, OfdmFA2: %#X\n", OFDMFalseAlarm, OfdmFA1, OfdmFA2);

+//					printk("DIG+++++++ fallback OFDM:%d \n", priv->InitialGain);

+					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);

+//				printk("DIG**********OFDM False Alarm: %#X,  OfdmFA1: %#X, OfdmFA2: %#X\n", OFDMFalseAlarm, OfdmFA1, OfdmFA2);

+//				printk("DIG--------- Upgrade OFDM:%d \n", priv->InitialGain);

+				UpdateInitialGain(dev); 

+			}

+			priv->DIG_NumberFallbackVote = 0;

+			priv->DIG_NumberUpgradeVote=0;

+		}

+	}

+

+//	printk("DIG+++++++ OFDM:%d\n", priv->InitialGain);	

+	//printk("<--------- DIG_Zebra()\n");

+}

+

+//

+//	Description:

+//		Dispatch DIG implementation according to RF. 	

+//

+void

+DynamicInitGain(

+	struct net_device *dev

+	)

+{

+	struct r8180_priv *priv = ieee80211_priv(dev);

+

+	switch(priv->rf_chip)

+	{

+		case RF_ZEBRA2:  // [AnnieWorkaround] For Zebra2, 2005-08-01.

+		case RF_ZEBRA4:

+			DIG_Zebra( dev );

+			break;

+		

+		default:

+			printk("DynamicInitGain(): unknown RFChipID(%d) !!!\n", priv->rf_chip);

+			break;

+	}

+}

+

+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))

+void rtl8180_hw_dig_wq (struct work_struct *work)

+{

+//      struct r8180_priv *priv = container_of(work, struct r8180_priv, watch_dog_wq);

+//      struct ieee80211_device * ieee = (struct ieee80211_device*)

+//                                             container_of(work, struct ieee80211_device, watch_dog_wq);

+        struct delayed_work *dwork = container_of(work,struct delayed_work,work);

+        struct ieee80211_device *ieee = container_of(dwork,struct ieee80211_device,hw_dig_wq);

+        struct net_device *dev = ieee->dev;

+#else

+void rtl8180_hw_dig_wq(struct net_device *dev)

+{

+	

+#endif

+	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);

+	

+}

+

+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;

+}

+

+//

+//      Description:

+//              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.

+//      By Bruce, 2007-06-05.

+// 

+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))

+        {

+//              printk("GetUpgradeTxRate(): GetUpgrade Tx rate(%d) from %d !\n", UpRate, priv->CurrentOperaRate);

+                return UpRate;

+        }

+        else

+        {

+                //printk("GetUpgradeTxRate(): Tx rate (%d) is not in supported rates\n", UpRate);

+                return rate;

+        }

+        return rate;

+}

+//

+//      Description:

+//              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.

+//      By Bruce, 2007-06-05.

+// 

+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))

+        {

+//              printk("GetDegradeTxRate(): GetDegrade Tx rate(%d) from %d!\n", DownRate, priv->CurrentOperaRate);

+                return DownRate;

+        }

+        else

+        {

+                //printk("GetDegradeTxRate(): Tx rate (%d) is not in supported rates\n", DownRate);

+                return rate;

+        }

+        return rate;

+}

+//

+//      Helper function to determine if specified data rate is 

+//      CCK rate.

+//      2005.01.25, by rcnjko.

+//

+bool

+MgntIsCckRate(

+        u16     rate

+        )

+{

+        bool bReturn = false;

+

+        if((rate <= 22) && (rate != 12) && (rate != 18))

+        {

+                bReturn = true;

+        }

+

+        return bReturn;

+}

+#ifdef CONFIG_RTL818X_S

+//

+//	Description:

+//		Tx Power tracking mechanism routine on 87SE.

+// 	Created by Roger, 2007.12.11.

+//

+void

+TxPwrTracking87SE(

+	struct net_device *dev

+)

+{		

+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);

+	u8	tmpu1Byte, CurrentThermal, Idx;	

+	char	CckTxPwrIdx, OfdmTxPwrIdx;	

+	//u32	u4bRfReg;

+	

+	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

+

+	//printk("TxPwrTracking87SE(): CurrentThermal(%d)\n", CurrentThermal);

+	

+	if( CurrentThermal != priv->ThermalMeter)

+	{	

+//		printk("TxPwrTracking87SE(): Thermal meter changed!!!\n");

+

+		// 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;					

+}

+void

+StaRateAdaptive87SE(

+	struct net_device *dev

+	)

+{

+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);

+	unsigned long 			CurrTxokCnt;

+	u16			CurrRetryCnt;

+	u16			CurrRetryRate;

+	//u16			i,idx;

+	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;

+	//printk("priv->CurrentOperaRate is %d\n",priv->CurrentOperaRate);

+	//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);

+	}

+

+

+	//

+	// Added by Roger, 2007.01.02.

+	// For debug information.

+	//

+	//printk("\n(1) pHalData->LastRetryRate: %d \n",priv->LastRetryRate);

+	//printk("(2) RetryCnt = %d  \n", CurrRetryCnt);	

+	//printk("(3) TxokCnt = %d \n", CurrTxokCnt);

+	//printk("(4) CurrRetryRate = %d \n", CurrRetryRate);	

+	//printk("(5) CurrSignalStrength = %d \n",CurrSignalStrength);

+	//printk("(6) TxThroughput is %d\n",TxThroughput);

+	//printk("priv->NumTxOkBytesTotal is %d\n",priv->NumTxOkBytesTotal);		

+

+	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++;

+		

+//		printk("No Tx packets, TryupingCountNoData(%d)\n", 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).  

+	// 2007.04.09, by Roger.	

+	//	

+

+	//

+	// 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 mehanism of trying to upgrade tx rate.

+	// (5) Record the information of upping tx rate to avoid trying upping tx rate constantly.

+	// By Bruce, 2007-06-05.

+	//	

+	//

+

+	// 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 bwtween 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;

+			//printk("case1-1: Not necessary raising rate, fall back rate....\n");

+			//printk("case1-1: pMgntInfo->CurrentOperaRate =%d, TxThroughput = %d, LastThroughput = %d\n",

+			//		priv->CurrentOperaRate, TxThroughput, priv->LastTxThroughput);			

+		}

+		else

+		{

+			priv->bTryuping = false;

+		}

+	}	

+	else if (CurrSignalStrength > -47 && (CurrRetryRate < 50))

+	{

+		//2For High Power

+		//

+		// Added by Roger, 2007.04.09.

+		// 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;

+		}

+//		printk("case2: StaRateAdaptive87SE: Power(%d) is high enough!!. \n", CurrSignalStrength);			

+					

+	}

+	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;

+//		printk("case3: RA: Tx Retry is serious. Degrade Tx Rate to %d directly...\n", priv->CurrentOperaRate);	

+	}

+	else if ( priv->CurrentOperaRate == 108 )

+	{

+		//2For 54Mbps

+		// Air Link

+		if ( (CurrRetryRate>26)&&(priv->LastRetryRate>25))

+//		if ( (CurrRetryRate>40)&&(priv->LastRetryRate>39))

+		{

+			//Down to rate 48Mbps.

+			bTryDown = true;

+		}

+		// Cable Link

+		else if ( (CurrRetryRate>17)&&(priv->LastRetryRate>16) && (CurrSignalStrength > -72)) 

+//		else if ( (CurrRetryRate>17)&&(priv->LastRetryRate>16) && (CurrSignalStrength > -72))

+		{

+			//Down to rate 48Mbps.

+			bTryDown = true;

+		}

+

+		if(bTryDown && (CurrSignalStrength < -75)) //cable link

+		{

+			priv->TryDownCountLowData += TryDownTh;

+		}

+		//printk("case4---54M \n");	

+

+	}

+	else if ( priv->CurrentOperaRate == 96 )

+	{

+		//2For 48Mbps

+		//Air Link

+		if ( ((CurrRetryRate>48) && (priv->LastRetryRate>47)))

+//		if ( ((CurrRetryRate>65) && (priv->LastRetryRate>64)))

+

+		{	

+			//Down to rate 36Mbps.

+			bTryDown = true;

+		}

+		//Cable Link

+		else if ( ((CurrRetryRate>21) && (priv->LastRetryRate>20)) && (CurrSignalStrength > -74))

+		{

+			//Down to rate 36Mbps.

+			bTryDown = true;

+		}

+		else if((CurrRetryRate>  (priv->LastRetryRate + 50 )) && (priv->FailTxRateCount >2 ))

+//		else if((CurrRetryRate>  (priv->LastRetryRate + 70 )) && (priv->FailTxRateCount >2 ))

+		{

+			bTryDown = true;

+			priv->TryDownCountLowData += TryDownTh;

+		}

+		else if ( (CurrRetryRate<8) && (priv->LastRetryRate<8) ) //TO DO: need to consider (RSSI)

+//		else if ( (CurrRetryRate<28) && (priv->LastRetryRate<8) ) 

+		{

+			bTryUp = true;

+		}

+

+		if(bTryDown && (CurrSignalStrength < -75))

+		{

+			priv->TryDownCountLowData += TryDownTh;

+		}

+		//printk("case5---48M \n");	

+	}

+	else if ( priv->CurrentOperaRate == 72 )

+	{

+		//2For 36Mbps

+		if ( (CurrRetryRate>43) && (priv->LastRetryRate>41)) 

+//		if ( (CurrRetryRate>60) && (priv->LastRetryRate>59))

+		{	

+			//Down to rate 24Mbps.

+			bTryDown = true;

+		}

+		else if((CurrRetryRate>  (priv->LastRetryRate + 50 )) && (priv->FailTxRateCount >2 ))

+//		else if((CurrRetryRate>  (priv->LastRetryRate + 70 )) && (priv->FailTxRateCount >2 ))

+		{

+			bTryDown = true;

+			priv->TryDownCountLowData += TryDownTh;

+		}

+		else if ( (CurrRetryRate<15) &&  (priv->LastRetryRate<16)) //TO DO: need to consider (RSSI)

+//		else if ( (CurrRetryRate<35) &&  (priv->LastRetryRate<36))

+		{

+			bTryUp = true;

+		}

+

+		if(bTryDown && (CurrSignalStrength < -80))

+		{

+			priv->TryDownCountLowData += TryDownTh;

+		}

+		//printk("case6---36M \n");	

+	}

+	else if ( priv->CurrentOperaRate == 48 )

+	{

+		//2For 24Mbps

+		// Air Link

+		if ( ((CurrRetryRate>63) && (priv->LastRetryRate>62)))

+//		if ( ((CurrRetryRate>83) && (priv->LastRetryRate>82)))

+		{	

+			//Down to rate 18Mbps.

+			bTryDown = true;

+		}

+		//Cable Link

+		else if ( ((CurrRetryRate>33) && (priv->LastRetryRate>32)) && (CurrSignalStrength > -82) )

+//		 else if ( ((CurrRetryRate>50) && (priv->LastRetryRate>49)) && (CurrSignalStrength > -82) )

+		{

+			//Down to rate 18Mbps.

+			bTryDown = true;

+		}

+		else if((CurrRetryRate>  (priv->LastRetryRate + 50 )) && (priv->FailTxRateCount >2 ))

+//		else if((CurrRetryRate>  (priv->LastRetryRate + 70 )) && (priv->FailTxRateCount >2 ))

+

+		{

+			bTryDown = true;

+			priv->TryDownCountLowData += TryDownTh;

+		}

+  		else if ( (CurrRetryRate<20) && (priv->LastRetryRate<21)) //TO DO: need to consider (RSSI)

+//		else if ( (CurrRetryRate<40) && (priv->LastRetryRate<41))

+		{	

+			bTryUp = true;	

+		}

+

+		if(bTryDown && (CurrSignalStrength < -82))

+		{

+			priv->TryDownCountLowData += TryDownTh;

+		}

+		//printk("case7---24M \n");	

+	}

+	else if ( priv->CurrentOperaRate == 36 )

+	{

+		//2For 18Mbps

+		// original (109, 109) 

+		//[TRC Dell Lab] (90, 91), Isaiah 2008-02-18 23:24

+		//			     (85, 86), Isaiah 2008-02-18 24:00

+		if ( ((CurrRetryRate>85) && (priv->LastRetryRate>86)))

+//		if ( ((CurrRetryRate>115) && (priv->LastRetryRate>116)))

+		{

+			//Down to rate 11Mbps.

+			bTryDown = true;

+		}

+		//[TRC Dell Lab]  Isaiah 2008-02-18 23:24

+		else if((CurrRetryRate>  (priv->LastRetryRate + 50 )) && (priv->FailTxRateCount >2 ))

+//		else if((CurrRetryRate>  (priv->LastRetryRate + 70 )) && (priv->FailTxRateCount >2 ))

+		{

+			bTryDown = true;

+			priv->TryDownCountLowData += TryDownTh;

+		}

+		else if ( (CurrRetryRate<22) && (priv->LastRetryRate<23)) //TO DO: need to consider (RSSI)

+//		else if ( (CurrRetryRate<42) && (priv->LastRetryRate<43))

+		{	

+			bTryUp = true;	

+		}

+		//printk("case8---18M \n");	

+	}

+	else if ( priv->CurrentOperaRate == 22 )

+	{

+		//2For 11Mbps

+		if (CurrRetryRate>95)

+//		if (CurrRetryRate>155)

+		{

+			bTryDown = true;

+		}

+		else if ( (CurrRetryRate<29) && (priv->LastRetryRate <30) )//TO DO: need to consider (RSSI)

+//		else if ( (CurrRetryRate<49) && (priv->LastRetryRate <50) )

+			{

+			bTryUp = true;

+			}

+		//printk("case9---11M \n");	

+		}	

+	else if ( priv->CurrentOperaRate == 11 )

+	{

+		//2For 5.5Mbps

+		if (CurrRetryRate>149) 

+//		if (CurrRetryRate>189)

+		{	

+			bTryDown = true;			

+		}

+		else if ( (CurrRetryRate<60) && (priv->LastRetryRate < 65))

+//		else if ( (CurrRetryRate<80) && (priv->LastRetryRate < 85))

+

+			{

+			bTryUp = true;

+			}		

+		//printk("case10---5.5M \n");	

+		}

+	else if ( priv->CurrentOperaRate == 4 )

+	{

+		//2For 2 Mbps

+		if((CurrRetryRate>99) && (priv->LastRetryRate>99))

+//		if((CurrRetryRate>199) && (priv->LastRetryRate>199))

+		{

+			bTryDown = true;			

+		}

+		else if ( (CurrRetryRate < 65) && (priv->LastRetryRate < 70))

+//		else if ( (CurrRetryRate < 85) && (priv->LastRetryRate < 90))

+		{

+			bTryUp = true;

+		}

+		//printk("case11---2M \n");	

+	}

+	else if ( priv->CurrentOperaRate == 2 )

+	{

+		//2For 1 Mbps

+		if( (CurrRetryRate<70) && (priv->LastRetryRate<75))

+//		if( (CurrRetryRate<90) && (priv->LastRetryRate<95))

+		{

+			bTryUp = true;

+		}

+		//printk("case12---1M \n");	

+	}

+

+	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;

+			//printk("StaRateAdaptive87SE(): Randomly try upgrading...\n");

+		}

+	}

+

+	//1 Rate Mechanism

+	if(bTryUp)

+	{

+		priv->TryupingCount++;

+		priv->TryDownCountLowData = 0;

+			

+		{

+//			printk("UP: pHalData->TryupingCount = %d\n", priv->TryupingCount);

+//			printk("UP: TryUpTh(%d)+ (FailTxRateCount(%d))^2 =%d\n", 

+//				TryUpTh, priv->FailTxRateCount, (TryUpTh + priv->FailTxRateCount * priv->FailTxRateCount) );

+//			printk("UP: pHalData->bTryuping=%d\n",  priv->bTryuping);

+

+		}

+		

+		//

+		// 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 transfering 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 carefully 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);

+//			printk("StaRateAdaptive87SE(): Upgrade Tx Rate to %d\n", priv->CurrentOperaRate);

+

+			//[TRC Dell Lab] Bypass 12/9/6, Isaiah 2008-02-18 20:00 

+			if(priv->CurrentOperaRate ==36)

+			{

+				priv->bUpdateARFR=true;

+				write_nic_word(dev, ARFR, 0x0F8F); //bypass 12/9/6

+//				printk("UP: ARFR=0xF8F\n");

+			}

+			else if(priv->bUpdateARFR)

+			{

+				priv->bUpdateARFR=false;

+				write_nic_word(dev, ARFR, 0x0FFF); //set 1M ~ 54Mbps.

+//				printk("UP: ARFR=0xFFF\n");

+			}

+				

+			// 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;

+		{

+//			printk("DN: pHalData->TryDownCountLowData = %d\n",priv->TryDownCountLowData);

+//			printk("DN: TryDownTh =%d\n", TryDownTh);

+//			printk("DN: pHalData->bTryuping=%d\n",  priv->bTryuping);

+		}

+						

+		//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. 

+			//[TRC Dell Lab] Revise Signal Threshold from -75 to -80 , Isaiah 2008-02-18 20:00 

+			if( (CurrSignalStrength < -80) && (priv->CurrentOperaRate > 72 ))

+			{

+				priv->CurrentOperaRate = 72;

+//				printk("DN: weak signal strength (%d), degrade to 36Mbps\n", CurrSignalStrength);

+			}

+

+			//[TRC Dell Lab] Bypass 12/9/6, Isaiah 2008-02-18 20:00 

+			if(priv->CurrentOperaRate ==36)

+			{

+				priv->bUpdateARFR=true;

+				write_nic_word(dev, ARFR, 0x0F8F); //bypass 12/9/6

+//				printk("DN: ARFR=0xF8F\n");

+			}

+			else if(priv->bUpdateARFR)

+			{

+				priv->bUpdateARFR=false;

+				write_nic_word(dev, ARFR, 0x0FFF); //set 1M ~ 54Mbps.

+//				printk("DN: ARFR=0xFFF\n");

+			}

+				

+			//

+			// When it is CCK rate, it may need to update initial gain to receive lower power packets.

+			//

+			if(MgntIsCckRate(priv->CurrentOperaRate))

+			{

+				bUpdateInitialGain = true;

+			}

+//			printk("StaRateAdaptive87SE(): Degrade Tx Rate to %d\n", priv->CurrentOperaRate);

+		}

+	}

+	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]; 

+	

+	//[TRC Dell Lab] Mac0x9e increase 2 level in 36M~18M situation, Isaiah 2008-02-18 24:00 

+	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);

+//			printk("Enhance OFDM_TXAGC : +++++ u1bOfdm= 0x%x\n", u1bOfdm);

+			}

+		}

+		// case 2: enter high power

+		else if(u1bCck < CckTxPwrIdx)

+		{

+			if(!priv->bEnhanceTxPwr)

+			{

+				priv->bEnhanceTxPwr= true;

+				u1bOfdm = ((u1bOfdm+2) > 35) ? 35: (u1bOfdm+2);

+				write_nic_byte(dev, OFDM_TXAGC, u1bOfdm);

+				//RT_TRACE(COMP_RATE, DBG_TRACE, ("Enhance OFDM_TXAGC(2) : +++++ u1bOfdm= 0x%x\n", 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);

+		//printk("Recover OFDM_TXAGC : ===== u1bOfdm= 0x%x\n", 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);

+			//RT_TRACE(COMP_RATE, DBG_TRACE, ("Recover OFDM_TXAGC(2): ===== u1bOfdm= 0x%x\n", 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;

+}

+

+#endif

+#if LINUX_VERSION_CODE >=KERNEL_VERSION(2,6,20)

+void rtl8180_rate_adapter(struct work_struct * work)

+{

+	struct delayed_work *dwork = container_of(work,struct delayed_work,work);

+        struct ieee80211_device *ieee = container_of(dwork,struct ieee80211_device,rate_adapter_wq);

+        struct net_device *dev = ieee->dev;

+#else

+void rtl8180_rate_adapter(struct net_device *dev)

+{

+

+#endif

+        //struct r8180_priv *priv = ieee80211_priv(dev);

+//    DMESG("---->rtl8180_rate_adapter");

+        StaRateAdaptive87SE(dev);

+//   DMESG("<----rtl8180_rate_adapter");

+}

+void timer_rate_adaptive(unsigned long data)

+{

+	struct r8180_priv* priv = ieee80211_priv((struct net_device *)data);

+	//DMESG("---->timer_rate_adaptive()\n");

+	if(!priv->up)

+	{

+//		DMESG("<----timer_rate_adaptive():driver is not up!\n");

+		return;

+	}

+	if((priv->ieee80211->iw_mode != IW_MODE_MASTER)

+			&& (priv->ieee80211->state == IEEE80211_LINKED) &&

+			(priv->ForcedDataRate == 0) )

+	{

+//	DMESG("timer_rate_adaptive():schedule rate_adapter_wq\n");

+#ifdef CONFIG_RTL818X_S

+		queue_work(priv->ieee80211->wq, (void *)&priv->ieee80211->rate_adapter_wq);

+//		StaRateAdaptive87SE((struct net_device *)data);

+#endif

+	}

+	priv->rateadapter_timer.expires = jiffies + MSECS(priv->RateAdaptivePeriod);

+	add_timer(&priv->rateadapter_timer);

+	//DMESG("<----timer_rate_adaptive()\n");

+}

+//by amy 080312}

+void

+SwAntennaDiversityRxOk8185(

+	struct net_device *dev, 

+	u8 SignalStrength

+	)

+{

+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);

+

+//	printk("+SwAntennaDiversityRxOk8185: RxSs: %d\n", SignalStrength);

+

+	priv->AdRxOkCnt++;

+

+	if( priv->AdRxSignalStrength != -1)

+	{

+		priv->AdRxSignalStrength = ((priv->AdRxSignalStrength*7) + (SignalStrength*3)) / 10;

+	}

+	else

+	{ // Initialization case.

+		priv->AdRxSignalStrength = SignalStrength;

+	}

+//{+by amy 080312

+	if( priv->LastRxPktAntenna ) //Main antenna.	

+		priv->AdMainAntennaRxOkCnt++;	

+	else	 // Aux antenna.

+		priv->AdAuxAntennaRxOkCnt++;

+//+by amy 080312

+//	printk("-SwAntennaDiversityRxOk8185: AdRxOkCnt: %d AdRxSignalStrength: %d\n", priv->AdRxOkCnt, priv->AdRxSignalStrength);

+}

+//

+//	Description:

+//		Change Antenna Switch.

+//

+bool

+SetAntenna8185(

+	struct net_device *dev,

+	u8		u1bAntennaIndex

+	)

+{

+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);

+	bool bAntennaSwitched = false;

+

+//	printk("+SetAntenna8185(): Antenna is switching to: %d \n", u1bAntennaIndex);

+

+	switch(u1bAntennaIndex)

+	{

+	case 0:

+		switch(priv->rf_chip)

+		{

+		case RF_ZEBRA2:

+		case RF_ZEBRA4:

+#ifdef CONFIG_RTL8185B

+#ifdef CONFIG_RTL818X_S

+			// 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.

+

+#else

+			// Mac register, main antenna

+			write_nic_byte(dev, ANTSEL, 0x03); 

+			//base band

+			write_phy_cck(dev, 0x10, 0x9b); // Config CCK RX antenna.

+			write_phy_ofdm(dev, 0x0d, 0x5c); // Config OFDM RX antenna.

+#endif

+#endif

+

+			bAntennaSwitched = true;

+			break;

+

+		default:

+			printk("SetAntenna8185: unkown RFChipID(%d)\n", priv->rf_chip);

+			break;

+		}

+		break;

+

+	case 1:

+		switch(priv->rf_chip)

+		{

+		case RF_ZEBRA2:

+		case RF_ZEBRA4:

+#ifdef CONFIG_RTL8185B

+#ifdef CONFIG_RTL818X_S

+			// 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.

+#else

+			// Mac register, aux antenna

+			write_nic_byte(dev, ANTSEL, 0x00); 

+			//base band

+			write_phy_cck(dev, 0x10, 0xbb); // Config CCK RX antenna.

+			write_phy_ofdm(dev, 0x0d, 0x54); // Config OFDM RX antenna.

+#endif

+#endif

+

+			bAntennaSwitched = true;

+			break;

+

+		default:

+			printk("SetAntenna8185: unkown RFChipID(%d)\n", priv->rf_chip);

+			break;

+		}

+		break;

+

+	default:

+		printk("SetAntenna8185: unkown u1bAntennaIndex(%d)\n", u1bAntennaIndex);

+		break;

+	}

+

+	if(bAntennaSwitched)

+	{

+		priv->CurrAntennaIndex = u1bAntennaIndex;

+	}

+

+//	printk("-SetAntenna8185(): return (%#X)\n", bAntennaSwitched);

+

+	return bAntennaSwitched;

+}

+//

+//	Description:

+//		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)

+	{

+#if 0//lzm del 080826

+//by amy 080312

+#ifdef CONFIG_RTL818X_S

+		if(priv->bSwAntennaDiverity)

+			bResult = SetAntennaConfig87SE(dev, 1, true);

+		else			

+#endif

+#endif

+			bResult = SetAntenna8185(dev, 1);

+//by amy 080312

+//		printk("SwitchAntenna(): switching to antenna 1 ......\n");

+//		bResult = SetAntenna8185(dev, 1);//-by amy 080312

+	}

+	else

+	{

+#if 0//lzm del 080826

+//by amy 080312

+#ifdef CONFIG_RTL818X_S

+		if(priv->bSwAntennaDiverity)

+			bResult = SetAntennaConfig87SE(dev, 0, true);

+		else	

+#endif

+#endif

+			bResult = SetAntenna8185(dev, 0);

+//by amy 080312

+//		printk("SwitchAntenna(): switching to antenna 0 ......\n");

+//		bResult = SetAntenna8185(dev, 0);//-by amy 080312

+	}

+

+	return bResult;

+}

+//

+//	Description:

+//		Engine of SW Antenna Diversity mechanism.

+//		Since 8187 has no Tx part information, 

+//		this implementation is only dependend on Rx part information. 

+//

+//	2006.04.17, by rcnjko.

+//

+void

+SwAntennaDiversity(

+	struct net_device *dev

+	)

+{

+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);

+	bool   bSwCheckSS=false;

+//	printk("+SwAntennaDiversity(): CurrAntennaIndex: %d\n", priv->CurrAntennaIndex);

+//	printk("AdTickCount is %d\n",priv->AdTickCount);

+//by amy 080312

+	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);

+	}

+//	priv->AdTickCount++;//-by amy 080312

+	

+	// Case 1. No Link.

+	if(priv->ieee80211->state != IEEE80211_LINKED)

+	{

+	//	printk("SwAntennaDiversity(): Case 1. No Link.\n");

+

+		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 received.

+	else if(priv->AdRxOkCnt == 0)

+	{

+	//	printk("SwAntennaDiversity(): Case 2. Linked but no packet received.\n");

+

+		priv->bAdSwitchedChecking = false;

+		SwitchAntenna(dev);

+	}

+	// Case 3. Evaluate last antenna switch action and undo it if necessary.

+	else if(priv->bAdSwitchedChecking == true)

+	{

+	//	printk("SwAntennaDiversity(): Case 3. Evaluate last antenna switch action.\n");

+

+		priv->bAdSwitchedChecking = false;

+

+		// Adjust Rx signal strength threashold.

+		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.

+//			printk("SwAntennaDiversity(): Rx Signal Strength is not improved, CurrRxSs: %d, LastRxSs: %d\n", 

+//				priv->AdRxSignalStrength, priv->AdRxSsBeforeSwitched);

+//by amy 080312

+			// Increase Antenna Diversity checking period due to bad decision.

+			priv->AdCheckPeriod *= 2;

+//by amy 080312

+			// Increase Antenna Diversity checking period.

+			if(priv->AdCheckPeriod > priv->AdMaxCheckPeriod)

+				priv->AdCheckPeriod = priv->AdMaxCheckPeriod;

+	

+			// Wrong deceision => switch back.

+			SwitchAntenna(dev);

+		}

+		else

+		{ // Rx Signal Strength is improved. 

+//			printk("SwAntennaDiversity(): Rx Signal Strength is improved, CurrRxSs: %d, LastRxSs: %d\n", 

+//				priv->AdRxSignalStrength, priv->AdRxSsBeforeSwitched);

+

+			// Reset Antenna Diversity checking period to its min value.

+			priv->AdCheckPeriod = priv->AdMinCheckPeriod;

+		}

+

+//		printk("SwAntennaDiversity(): AdRxSsThreshold: %d, AdCheckPeriod: %d\n",

+//			priv->AdRxSsThreshold, priv->AdCheckPeriod);

+	}

+	// 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// if(priv->AdTickCount >= priv->AdCheckPeriod)//-by amy 080312

+	{

+//		printk("SwAntennaDiversity(): Case 4. Evaluate if we shall switch antenna now.\n");

+

+		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.

+		// Added by Roger, 2008.02.21.

+//{by amy 080312

+		if((priv->AdMainAntennaRxOkCnt < priv->AdAuxAntennaRxOkCnt) 

+			&& (priv->CurrAntennaIndex == 0))

+		{ // We set Main antenna as default but RxOk count was less than Aux ones.

+

+	//		printk("SwAntennaDiversity(): Main antenna RxOK is poor, AdMainAntennaRxOkCnt: %d, AdAuxAntennaRxOkCnt: %d\n", 

+	//			priv->AdMainAntennaRxOkCnt, priv->AdAuxAntennaRxOkCnt);

+			

+			// 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.

+

+	//		printk("SwAntennaDiversity(): Aux antenna RxOK is poor, AdMainAntennaRxOkCnt: %d, AdAuxAntennaRxOkCnt: %d\n", 

+	//			priv->AdMainAntennaRxOkCnt, priv->AdAuxAntennaRxOkCnt);

+			

+			// Switch to Main antenna.

+			SwitchAntenna(dev);

+			priv->bHWAdSwitched = true;

+		}

+		else

+		{// Default antenna is better.

+

+	//		printk("SwAntennaDiversity(): Default antenna is better., AdMainAntennaRxOkCnt: %d, AdAuxAntennaRxOkCnt: %d\n", 

+	//			priv->AdMainAntennaRxOkCnt, priv->AdAuxAntennaRxOkCnt);

+

+			// Still need to check current signal strength.

+			priv->bHWAdSwitched = false;	

+		}

+		//

+		// <Roger_Notes> We evaluate Rx signal strength ONLY when default antenna 

+		// didn't changed 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))

+		{

+//by amy 080312}

+		// Evaluate Rx signal strength if we shall switch antenna now.

+		if(priv->AdRxSignalStrength < priv->AdRxSsThreshold)

+		{ // Rx signal strength is weak => Switch Antenna.

+//			printk("SwAntennaDiversity(): Rx Signal Strength is weak, CurrRxSs: %d, RxSsThreshold: %d\n", 

+//				priv->AdRxSignalStrength, priv->AdRxSsThreshold);	

+

+			priv->AdRxSsBeforeSwitched = priv->AdRxSignalStrength; 

+			priv->bAdSwitchedChecking = true;

+

+			SwitchAntenna(dev);

+		}

+		else

+		{ // Rx signal strength is OK. 

+//			printk("SwAntennaDiversity(): Rx Signal Strength is OK, CurrRxSs: %d, RxSsThreshold: %d\n", 

+//				priv->AdRxSignalStrength, priv->AdRxSsThreshold);

+

+			priv->bAdSwitchedChecking = false;

+			// Increase Rx signal strength threashold 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; 

+			}

+		}

+		}

+	}

+//by amy 080312

+	// Reset antenna diversity Rx related statistics.

+	priv->AdRxOkCnt = 0;

+	priv->AdMainAntennaRxOkCnt = 0;

+	priv->AdAuxAntennaRxOkCnt = 0;

+//by amy 080312

+

+//	priv->AdRxOkCnt = 0;//-by amy 080312

+

+//	printk("-SwAntennaDiversity()\n");

+}

+

+//

+//	Description:

+//		Return TRUE if we shall perform Tx Power Tracking Mecahnism, FALSE otherwise.	

+//

+bool

+CheckTxPwrTracking(	struct net_device *dev)

+{

+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);

+

+	if(!priv->bTxPowerTrack)

+	{

+		return false;

+	}

+

+//lzm reserved 080826

+	//if(priv->bScanInProgress)

+	//{

+	//	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;

+}

+

+

+//

+//	Description:

+//		Timer callback function of SW Antenna Diversity.

+//

+void

+SwAntennaDiversityTimerCallback(

+	struct net_device *dev

+	)

+{

+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);

+	RT_RF_POWER_STATE rtState;

+	

+	//printk("+SwAntennaDiversityTimerCallback()\n");

+

+	//

+	// We do NOT need to switch antenna while RF is off.

+	// 2007.05.09, added by Roger.

+	//

+	rtState = priv->eRFPowerState;

+	do{

+		if (rtState == eRfOff)

+		{	

+//			printk("SwAntennaDiversityTimer - RF is OFF.\n");

+			break;

+		}  	

+		else if (rtState == eRfSleep)

+		{	

+			// Don't access BB/RF under Disable PLL situation.

+			//RT_TRACE((COMP_RF|COMP_ANTENNA), DBG_LOUD, ("SwAntennaDiversityTimerCallback(): RF is Sleep => skip it\n"));

+			break;

+		}  

+		SwAntennaDiversity(dev);

+

+	}while(false);

+

+	if(priv->up)

+	{

+		priv->SwAntennaDiversityTimer.expires = jiffies + MSECS(ANTENNA_DIVERSITY_TIMER_PERIOD);

+		add_timer(&priv->SwAntennaDiversityTimer);

+	}

+

+	//printk("-SwAntennaDiversityTimerCallback()\n");

+}

+

diff --git a/drivers/staging/rtl8187se/r8180_dm.h b/drivers/staging/rtl8187se/r8180_dm.h
new file mode 100644
index 000000000000..3de92f040f99
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_dm.h
@@ -0,0 +1,41 @@
+#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);

+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))

+void rtl8180_hw_dig_wq (struct work_struct *work);

+#else

+void rtl8180_hw_dig_wq(struct net_device *dev);

+#endif

+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))

+void rtl8180_tx_pw_wq (struct work_struct *work);

+#else

+void rtl8180_tx_pw_wq(struct net_device *dev);

+#endif

+#if LINUX_VERSION_CODE >=KERNEL_VERSION(2,6,20)

+void rtl8180_rate_adapter(struct work_struct * work);

+

+#else

+void rtl8180_rate_adapter(struct net_device *dev);

+

+#endif

+void TxPwrTracking87SE(struct net_device *dev);

+bool CheckTxPwrTracking(struct net_device *dev);

+#if LINUX_VERSION_CODE >=KERNEL_VERSION(2,6,20)

+void rtl8180_rate_adapter(struct work_struct * work);

+#else

+void rtl8180_rate_adapter(struct net_device *dev);

+#endif

+void timer_rate_adaptive(unsigned long data);

+

+

+#endif

diff --git a/drivers/staging/rtl8187se/r8180_gct.c b/drivers/staging/rtl8187se/r8180_gct.c
new file mode 100644
index 000000000000..86cb427a7a40
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_gct.c
@@ -0,0 +1,296 @@
+/*
+   This files contains GCT radio frontend programming routines.
+
+   This is part of rtl8180 OpenSource driver
+   Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+   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.
+
+   Code from Rtw8180 NetBSD driver by David Young has been really useful to
+   understand some things and gets some ideas
+
+   Code from rtl8181 project has been useful to me to understand some things.
+
+   Some code from 'Deuce' work
+
+   We want to tanks the Authors of such projects and the Ndiswrapper
+   project Authors.
+*/
+
+
+#include "r8180.h"
+#include "r8180_hw.h"
+#include "r8180_gct.h"
+
+
+//#define DEBUG_GCT
+
+/* the following experiment are just experiments.
+ * this means if you enable them you can have every kind
+ * of result, included damage the RF chip, so don't
+ * touch them if you don't know what you are doing.
+ * In any case, if you do it, do at your own risk
+ */
+
+//#define GCT_EXPERIMENT1  //improve RX sensivity
+
+//#define GCT_EXPERIMENT2
+
+//#define GCT_EXPERIMENT3  //iprove a bit RX signal quality ?
+
+//#define GCT_EXPERIMENT4 //maybe solve some brokeness with experiment1 ?
+
+//#define GCT_EXPERIMENT5
+
+//#define GCT_EXPERIMENT6  //not good
+
+
+u32 gct_chan[] = {
+	0x0,	//dummy channel 0
+	0x0, //1
+	0x1, //2
+	0x2, //3
+	0x3, //4
+	0x4, //5
+	0x5, //6
+	0x6, //7
+	0x7, //8
+	0x8, //9
+	0x9, //10
+	0xa, //11
+	0xb, //12
+	0xc, //13
+	0xd, //14
+};
+
+int gct_encode[16] = {
+	0, 8, 4, 0xC,
+	2, 0xA, 6, 0xE,
+	1, 9, 5, 0xD,
+	3, 0xB, 7, 0xF
+};
+
+void gct_rf_stabilize(struct net_device *dev)
+{
+	force_pci_posting(dev);
+	mdelay(3); //for now use a great value.. we may optimize in future
+}
+
+
+void write_gct(struct net_device *dev, u8 adr, u32 data)
+{
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 phy_config;
+
+	phy_config =  gct_encode[(data & 0xf00) >> 8];
+	phy_config |= gct_encode[(data & 0xf0) >> 4 ] << 4;
+	phy_config |= gct_encode[(data & 0xf)       ] << 8;
+	phy_config |= gct_encode[(adr >> 1) & 0xf   ] << 12;
+	phy_config |=            (adr & 1 )           << 16;
+	phy_config |= gct_encode[(data & 0xf000)>>12] << 24;
+
+	phy_config |= 0x90000000; // MAC will bang bits to the chip
+
+
+	write_nic_dword(dev,PHY_CONFIG,phy_config);
+#ifdef DEBUG_GCT
+	DMESG("Writing GCT: %x (adr %x)",phy_config,adr);
+#endif
+	gct_rf_stabilize(dev);
+}
+
+
+
+void gct_write_phy_antenna(struct net_device *dev,short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u8 ant;
+
+	ant = GCT_ANTENNA;
+	if(priv->antb) /*default antenna is antenna B */
+		ant |= BB_ANTENNA_B;
+	if(ch == 14)
+		ant |= BB_ANTATTEN_CHAN14;
+	write_phy(dev,0x10,ant);
+	//DMESG("BB antenna %x ",ant);
+}
+
+
+void gct_rf_set_chan(struct net_device *dev, short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 txpw = 0xff & priv->chtxpwr[ch];
+	u32 chan = gct_chan[ch];
+
+	//write_phy(dev,3,txpw);
+#ifdef DEBUG_GCT
+	DMESG("Gct set channel");
+#endif
+	/* set TX power */
+	write_gct(dev,0x15,0);
+ 	write_gct(dev,6, txpw);
+	write_gct(dev,0x15, 0x10);
+	write_gct(dev,0x15,0);
+
+	/*set frequency*/
+	write_gct(dev,7, 0);
+      	write_gct(dev,0xB, chan);
+      	write_gct(dev,7, 0x1000);
+
+#ifdef DEBUG_GCT
+	DMESG("Gct set channel > write phy antenna");
+#endif
+
+
+	gct_write_phy_antenna(dev,ch);
+
+}
+
+
+void gct_rf_close(struct net_device *dev)
+{
+	u32 anaparam;
+
+	anaparam = read_nic_dword(dev,ANAPARAM);
+	anaparam &= 0x000fffff;
+	anaparam |= 0x3f900000;
+	rtl8180_set_anaparam(dev, anaparam);
+
+	write_gct(dev, 0x7, 0);
+	write_gct(dev, 0x1f, 0x45);
+	write_gct(dev, 0x1f, 0x5);
+	write_gct(dev, 0x0, 0x8e4);
+}
+
+
+void gct_rf_init(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	//u32 anaparam;
+
+
+	write_nic_byte(dev,PHY_DELAY,0x6);	//this is general
+	write_nic_byte(dev,CARRIER_SENSE_COUNTER,0x4c); //this is general
+
+	//DMESG("%x", read_nic_dword(dev,ANAPARAM));
+	/* we should set anaparm here*/
+	//rtl8180_set_anaparam(dev,anaparam);
+
+	write_gct(dev,0x1f,0);
+	write_gct(dev,0x1f,0);
+	write_gct(dev,0x1f,0x40);
+	write_gct(dev,0x1f,0x60);
+	write_gct(dev,0x1f,0x61);
+	write_gct(dev,0x1f,0x61);
+	write_gct(dev,0x0,0xae4);
+	write_gct(dev,0x1f,0x1);
+	write_gct(dev,0x1f,0x41);
+	write_gct(dev,0x1f,0x61);
+	write_gct(dev,0x1,0x1a23);
+	write_gct(dev,0x2,0x4971);
+	write_gct(dev,0x3,0x41de);
+	write_gct(dev,0x4,0x2d80);
+#ifdef GCT_EXPERIMENT1
+	//write_gct(dev,0x5,0x6810);  // from zydas driver. sens+ but quite slow
+	//write_gct(dev,0x5,0x681f);  //good+ (somewhat stable, better sens, performance decent)
+	write_gct(dev,0x5,0x685f);  //good performances, not sure sens is really so beeter
+	//write_gct(dev,0x5,0x687f);  //good performances, maybe sens is not improved
+	//write_gct(dev,0x5,0x689f);  //like above
+	//write_gct(dev,0x5,0x685e);  //bad
+	//write_gct(dev,0x5,0x68ff);  //good+ (somewhat stable, better sens(?), performance decent)
+	//write_gct(dev,0x5,0x68f0);  //bad
+	//write_gct(dev,0x5,0x6cff);  //sens+ but not so good
+	//write_gct(dev,0x5,0x6dff);  //sens+,apparentely very good but broken
+	//write_gct(dev,0x5,0x65ff);  //sens+,good
+	//write_gct(dev,0x5,0x78ff);  //sens + but almost broken
+	//write_gct(dev,0x5,0x7810);  //- //snes + but broken
+	//write_gct(dev,0x5,0x781f);  //-- //sens +
+	//write_gct(dev,0x5,0x78f0);  //low sens
+#else
+	write_gct(dev,0x5,0x61ff);   //best performance but weak sensitivity
+#endif
+#ifdef GCT_EXPERIMENT2
+	write_gct(dev,0x6,0xe);
+#else
+	write_gct(dev,0x6,0x0);
+#endif
+	write_gct(dev,0x7,0x0);
+	write_gct(dev,0x8,0x7533);
+	write_gct(dev,0x9,0xc401);
+	write_gct(dev,0xa,0x0);
+	write_gct(dev,0xc,0x1c7);
+	write_gct(dev,0xd,0x29d3);
+	write_gct(dev,0xe,0x2e8);
+	write_gct(dev,0x10,0x192);
+#ifdef GCT_EXPERIMENT3
+	write_gct(dev,0x11,0x246);
+#else
+	write_gct(dev,0x11,0x248);
+#endif
+	write_gct(dev,0x12,0x0);
+	write_gct(dev,0x13,0x20c4);
+#ifdef GCT_EXPERIMENT4
+	write_gct(dev,0x14,0xf488);
+#else
+	write_gct(dev,0x14,0xf4fc);
+#endif
+#ifdef GCT_EXPERIMENT5
+	write_gct(dev,0x15,0xb152);
+#else
+	write_gct(dev,0x15,0x0);
+#endif
+#ifdef GCT_EXPERIMENT6
+	write_gct(dev,0x1e,0x1);
+#endif
+	write_gct(dev,0x16,0x1500);
+
+	write_gct(dev,0x7,0x1000);
+	/*write_gct(dev,0x15,0x0);
+	write_gct(dev,0x6,0x15);
+	write_gct(dev,0x15,0x8);
+	write_gct(dev,0x15,0x0);
+*/
+	write_phy(dev,0,0xa8);
+
+/*	write_gct(dev,0x15,0x0);
+	write_gct(dev,0x6,0x12);
+	write_gct(dev,0x15,0x8);
+	write_gct(dev,0x15,0x0);
+*/
+	write_phy(dev,3,0x0);
+	write_phy(dev,4,0xc0); /* lna det*/
+	write_phy(dev,5,0x90);
+	write_phy(dev,6,0x1e);
+	write_phy(dev,7,0x64);
+
+#ifdef DEBUG_GCT
+	DMESG("Gct init> write phy antenna");
+#endif
+
+	gct_write_phy_antenna(dev,priv->chan);
+
+	write_phy(dev,0x11,0x88);
+	if(!priv->diversity)
+		write_phy(dev,0x12,0xc0);
+	else
+		write_phy(dev,0x12,0x40);
+
+	write_phy(dev,0x13,0x90 | priv->cs_treshold );
+
+	write_phy(dev,0x19,0x0);
+	write_phy(dev,0x1a,0xa0);
+	write_phy(dev,0x1b,0x44);
+
+#ifdef DEBUG_GCT
+	DMESG("Gct init > set channel2");
+#endif
+
+	gct_rf_set_chan(dev,priv->chan);
+}
diff --git a/drivers/staging/rtl8187se/r8180_gct.h b/drivers/staging/rtl8187se/r8180_gct.h
new file mode 100644
index 000000000000..fe965ca64304
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_gct.h
@@ -0,0 +1,25 @@
+/*
+	This is part of rtl8180 OpenSource driver - v 0.20
+	Copyright (C) Andrea Merello 2004  <andreamrl@tiscali.it>
+	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.
+*/
+
+#define GCT_ANTENNA 0xA3
+
+
+// we use the untouched eeprom value- cross your finger ;-)
+#define GCT_ANAPARAM_PWR1_ON ??
+#define GCT_ANAPARAM_PWR0_ON ??
+
+
+
+void gct_rf_init(struct net_device *dev);
+void gct_rf_set_chan(struct net_device *dev,short ch);
+
+void gct_rf_close(struct net_device *dev);
diff --git a/drivers/staging/rtl8187se/r8180_hw.h b/drivers/staging/rtl8187se/r8180_hw.h
new file mode 100644
index 000000000000..bf38934bc090
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_hw.h
@@ -0,0 +1,956 @@
+/*
+	This is part of rtl8180 OpenSource driver.
+	Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+	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 CONFIG_RTL8185B  //support for rtl8185B, xiong-2006-11-15
+#define CONFIG_RTL818X_S
+
+#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 MAX_SLEEP_TIME (10000)
+#define MIN_SLEEP_TIME (50)
+
+#define BB_ANTATTEN_CHAN14	0x0c
+#define BB_ANTENNA_B 0x40
+
+#define BB_HOST_BANG (1<<30)
+#define BB_HOST_BANG_EN (1<<2)
+#define BB_HOST_BANG_CLK (1<<1)
+#define BB_HOST_BANG_DATA	 1
+
+#define ANAPARAM_TXDACOFF_SHIFT 27
+#define ANAPARAM_PWR0_MASK ((1<<30)|(1<<29)|(1<<28))
+#define ANAPARAM_PWR0_SHIFT 28
+#define ANAPARAM_PWR1_MASK ((1<<26)|(1<<25)|(1<<24)|(1<<23)|(1<<22)|(1<<21)|(1<<20))
+#define ANAPARAM_PWR1_SHIFT 20
+
+#define MAC0 0
+#define MAC1 1
+#define MAC2 2
+#define MAC3 3
+#define MAC4 4
+#define MAC5 5
+#define CMD 0x37
+#define CMD_RST_SHIFT 4
+#define CMD_RESERVED_MASK ((1<<1) | (1<<5) | (1<<6) | (1<<7))
+#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 0x3e
+#define INTA_TXOVERFLOW (1<<15)
+#define INTA_TIMEOUT (1<<14)
+#define INTA_BEACONTIMEOUT (1<<13)
+#define INTA_ATIM (1<<12)
+#define INTA_BEACONDESCERR (1<<11)
+#define INTA_BEACONDESCOK (1<<10)
+#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_RXCRCERR (1<<1)
+#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_128 3
+#define RX_FIFO_THRESHOLD_256 4
+#define RX_FIFO_THRESHOLD_512 5
+#define RX_FIFO_THRESHOLD_1024 6
+#define RX_FIFO_THRESHOLD_NONE 7
+#define RX_AUTORESETPHY_SHIFT 28
+#define EPROM_TYPE_SHIFT 6
+#define TX_CONF 0x40
+#define TX_CONF_HEADER_AUTOICREMENT_SHIFT 30
+#define TX_LOOPBACK_SHIFT 17
+#define TX_LOOPBACK_MAC 1
+#define TX_LOOPBACK_BASEBAND 2
+#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_DMA_STOP_BEACON_SHIFT 3
+#define TX_DMA_STOP_HIPRIORITY_SHIFT 2
+#define TX_DMA_STOP_NORMPRIORITY_SHIFT 1
+#define TX_DMA_STOP_LOWPRIORITY_SHIFT 0
+#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
+//AC_VI and Low priority share the sane queue
+#define TX_LOWPRIORITY_RING_ADDR TX_VIPRIORITY_RING_ADDR
+//AC_VO and Norm priority share the same queue
+#define TX_NORMPRIORITY_RING_ADDR TX_VOPRIORITY_RING_ADDR
+
+#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_DELAY 0x78
+#define PHY_CONFIG 0x80
+#define PHY_ADR 0x7c
+#define PHY_READ 0x7e
+#define CARRIER_SENSE_COUNTER 0x79 //byte
+#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 IDR0 0x0000
+#define IDR1 0x0001
+#define IDR2 0x0002
+#define IDR3 0x0003
+#define IDR4 0x0004
+#define IDR5 0x0005
+
+/* 0x0006 - 0x0007 - reserved */
+
+#define MAR0 0x0008
+#define MAR1 0x0009
+#define MAR2 0x000A
+#define MAR3 0x000B
+#define MAR4 0x000C
+#define MAR5 0x000D
+#define MAR6 0x000E
+#define MAR7 0x000F
+
+/* 0x0010 - 0x0017 - reserved */
+
+#define TSFTR 0x0018
+#define TSFTR_END 0x001F
+
+#define TLPDA 0x0020
+#define TLPDA_END 0x0023
+#define TNPDA 0x0024
+#define TNPDA_END 0x0027
+#define THPDA 0x0028
+#define THPDA_END 0x002B
+
+#define BSSID 0x002E
+#define BSSID_END 0x0033
+
+#define CR 0x0037
+
+#ifdef CONFIG_RTL8185B
+#define RF_SW_CONFIG	        0x8			// store data which is transmitted to RF for driver
+#define RF_SW_CFG_SI		BIT1
+#define PIFS			0x2C			// PCF InterFrame Spacing Timer Setting.
+#define EIFS			0x2D			// Extended InterFrame Space Timer, in unit of 4 us.
+
+#define BRSR			0x34			// Basic rate set
+
+#define IMR 0x006C
+#define ISR 0x003C
+#else
+#define BRSR 0x002C
+#define BRSR_END 0x002D
+
+/* 0x0034 - 0x0034 - reserved */
+#define EIFS 0x0035
+
+#define IMR 0x003C
+#define IMR_END 0x003D
+#define ISR 0x003E
+#define ISR_END 0x003F
+#endif
+
+#define TCR 0x0040
+#define TCR_END 0x0043
+
+#define RCR 0x0044
+#define RCR_END 0x0047
+
+#define TimerInt 0x0048
+#define TimerInt_END 0x004B
+
+#define TBDA 0x004C
+#define TBDA_END 0x004F
+
+#define CR9346 0x0050
+
+#define CONFIG0 0x0051
+#define CONFIG1 0x0052
+#define CONFIG2 0x0053
+
+#define ANA_PARM 0x0054
+#define ANA_PARM_END 0x0x0057
+
+#define MSR 0x0058
+
+#define CONFIG3 0x0059
+#define CONFIG4 0x005A
+#ifdef CONFIG_RTL8185B
+#ifdef CONFIG_RTL818X_S
+	// 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
+#else
+	// SD3 CMLin:
+	#define ANAPARM_ASIC_ON    0x45090658
+	#define ANAPARM2_ASIC_ON  0x727f3f52
+
+	#define ANAPARM_ON ANAPARM_ASIC_ON
+	#define ANAPARM2_ON ANAPARM2_ASIC_ON
+#endif
+#endif
+
+#define TESTR 0x005B
+
+/* 0x005C - 0x005D - reserved */
+
+#define PSR 0x005E
+
+/* 0x0060 - 0x006F - reserved */
+
+#define BcnItv 0x0070
+#define BcnItv_END 0x0071
+
+#define AtimWnd 0x0072
+#define AtimWnd_END 0x0073
+
+#define BintrItv 0x0074
+#define BintrItv_END 0x0075
+
+#define AtimtrItv 0x0076
+#define AtimtrItv_END 0x0077
+
+#define PhyDelay 0x0078
+
+#define CRCount 0x0079
+
+/* 0x007A - 0x007B - reserved */
+
+#define PhyAddr 0x007C
+#define PhyDataW 0x007D
+#define PhyDataR 0x007E
+
+#define PhyCFG 0x0080
+#define PhyCFG_END 0x0083
+
+/* 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
+/*
+ *  0x0084 - 0x00D3 is selected to page 1 when PSEn bit (bit0, PSR)
+ *  is set to 1
+ */
+
+#define Wakeup0 0x0084
+#define Wakeup0_END 0x008B
+
+#define Wakeup1 0x008C
+#define Wakeup1_END 0x0093
+
+#define Wakeup2LD 0x0094
+#define Wakeup2LD_END 0x009B
+#define Wakeup2HD 0x009C
+#define Wakeup2HD_END 0x00A3
+
+#define Wakeup3LD 0x00A4
+#define Wakeup3LD_END 0x00AB
+#define Wakeup3HD 0x00AC
+#define Wakeup3HD_END 0x00B3
+
+#define Wakeup4LD 0x00B4
+#define Wakeup4LD_END 0x00BB
+#define Wakeup4HD 0x00BC
+#define Wakeup4HD_END 0x00C3
+
+#define CRC0 0x00C4
+#define CRC0_END 0x00C5
+#define CRC1 0x00C6
+#define CRC1_END 0x00C7
+#define CRC2 0x00C8
+#define CRC2_END 0x00C9
+#define CRC3 0x00CA
+#define CRC3_END 0x00CB
+#define CRC4 0x00CC
+#define CRC4_END 0x00CD
+
+/* 0x00CE - 0x00D3 - reserved */
+
+
+
+/*
+ *  0x0084 - 0x00D3 is selected to page 0 when PSEn bit (bit0, PSR)
+ *  is set to 0
+ */
+
+/* 0x0084 - 0x008F - reserved */
+
+#define DK0 0x0090
+#define DK0_END 0x009F
+#define DK1 0x00A0
+#define DK1_END 0x00AF
+#define DK2 0x00B0
+#define DK2_END 0x00BF
+#define DK3 0x00C0
+#define DK3_END 0x00CF
+
+/* 0x00D0 - 0x00D3 - reserved */
+
+
+
+
+
+/* 0x00D4 - 0x00D7 - reserved */
+
+#define CONFIG5 0x00D8
+
+#define TPPoll 0x00D9
+
+/* 0x00DA - 0x00DB - reserved */
+
+#ifdef CONFIG_RTL818X_S
+#define PHYPR			0xDA			//0xDA - 0x0B PHY Parameter Register.
+#endif
+
+#define CWR 0x00DC
+#define CWR_END 0x00DD
+
+#define RetryCTR 0x00DE
+
+/* 0x00DF - 0x00E3 - reserved */
+
+#define RDSAR 0x00E4
+#define RDSAR_END 0x00E7
+
+/* 0x00E8 - 0x00EF - reserved */
+#ifdef CONFIG_RTL818X_S
+#define LED_CONTROL		0xED
+#endif
+
+#define FER 0x00F0
+#define FER_END 0x00F3
+
+#ifdef CONFIG_RTL8185B
+#define FEMR			0x1D4	// Function Event Mask register
+#else
+#define FEMR 0x00F4
+#define FEMR_END 0x00F7
+#endif
+
+#define FPSR 0x00F8
+#define FPSR_END 0x00FB
+
+#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))
+
+#ifdef CONFIG_RTL8185B
+#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
+
+#else
+#define IMR_TXFOVW  ((1<<15))
+#define IMR_TimeOut ((1<<14))
+#define IMR_BcnInt  ((1<<13))
+#define IMR_ATIMInt ((1<<12))
+#define IMR_TBDER   ((1<<11))
+#define IMR_TBDOK   ((1<<10))
+#define IMR_THPDER  ((1<< 9))
+#define IMR_THPDOK  ((1<< 8))
+#define IMR_TNPDER  ((1<< 7))
+#define IMR_TNPDOK  ((1<< 6))
+#define IMR_RXFOVW  ((1<< 5))
+#define IMR_RDU     ((1<< 4))
+#define IMR_TLPDER  ((1<< 3))
+#define IMR_TLPDOK  ((1<< 2))
+#define IMR_RER     ((1<< 1))
+#define IMR_ROK     ((1<< 0))
+
+#define ISR_TXFOVW  ((1<<15))
+#define ISR_TimeOut ((1<<14))
+#define ISR_BcnInt  ((1<<13))
+#define ISR_ATIMInt ((1<<12))
+#define ISR_TBDER   ((1<<11))
+#define ISR_TBDOK   ((1<<10))
+#define ISR_THPDER  ((1<< 9))
+#define ISR_THPDOK  ((1<< 8))
+#define ISR_TNPDER  ((1<< 7))
+#define ISR_TNPDOK  ((1<< 6))
+#define ISR_RXFOVW  ((1<< 5))
+#define ISR_RDU     ((1<< 4))
+#define ISR_TLPDER  ((1<< 3))
+#define ISR_TLPDOK  ((1<< 2))
+#define ISR_RER     ((1<< 1))
+#define ISR_ROK     ((1<< 0))
+#endif
+
+#define HW_VERID_R8180_F 3
+#define HW_VERID_R8180_ABCD 2
+#define HW_VERID_R8185_ABC 4
+#define HW_VERID_R8185_D 5
+#ifdef CONFIG_RTL8185B
+#define HW_VERID_R8185B_B 6
+#endif
+
+#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 CONFIG0_WEP104     ((1<<6))
+#define CONFIG0_LEDGPO_En  ((1<<4))
+#define CONFIG0_Aux_Status ((1<<3))
+#define CONFIG0_GL         ((1<<1)|(1<<0))
+#define CONFIG0_GL1        ((1<<1))
+#define CONFIG0_GL0        ((1<<0))
+
+#define CONFIG1_LEDS       ((1<<7)|(1<<6))
+#define CONFIG1_LEDS1      ((1<<7))
+#define CONFIG1_LEDS0      ((1<<6))
+#define CONFIG1_LWACT      ((1<<4))
+#define CONFIG1_MEMMAP     ((1<<3))
+#define CONFIG1_IOMAP      ((1<<2))
+#define CONFIG1_VPD        ((1<<1))
+#define CONFIG1_PMEn       ((1<<0))
+
+#define CONFIG2_LCK        ((1<<7))
+#define CONFIG2_ANT        ((1<<6))
+#define CONFIG2_DPS        ((1<<3))
+#define CONFIG2_PAPE_sign  ((1<<2))
+#define CONFIG2_PAPE_time  ((1<<1)|(1<<0))
+#define CONFIG2_PAPE_time1 ((1<<1))
+#define CONFIG2_PAPE_time0 ((1<<0))
+
+#define CONFIG3_GNTSel     ((1<<7))
+#define CONFIG3_PARM_En    ((1<<6))
+#define CONFIG3_Magic      ((1<<5))
+#define CONFIG3_CardB_En   ((1<<3))
+#define CONFIG3_CLKRUN_En  ((1<<2))
+#define CONFIG3_FuncRegEn  ((1<<1))
+#define CONFIG3_FBtbEn     ((1<<0))
+
+#define CONFIG4_VCOPDN     ((1<<7))
+#define CONFIG4_PWROFF     ((1<<6))
+#define CONFIG4_PWRMGT     ((1<<5))
+#define CONFIG4_LWPME      ((1<<4))
+#define CONFIG4_LWPTN      ((1<<2))
+#define CONFIG4_RFTYPE     ((1<<1)|(1<<0))
+#define CONFIG4_RFTYPE1    ((1<<1))
+#define CONFIG4_RFTYPE0    ((1<<0))
+
+#define CONFIG5_TX_FIFO_OK ((1<<7))
+#define CONFIG5_RX_FIFO_OK ((1<<6))
+#define CONFIG5_CALON      ((1<<5))
+#define CONFIG5_EACPI      ((1<<2))
+#define CONFIG5_LANWake    ((1<<1))
+#define CONFIG5_PME_STS    ((1<<0))
+
+#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 PSR_GPO            ((1<<7))
+#define PSR_GPI            ((1<<6))
+#define PSR_LEDGPO1        ((1<<5))
+#define PSR_LEDGPO0        ((1<<4))
+#define PSR_UWF            ((1<<1))
+#define PSR_PSEn           ((1<<0))
+
+#define SCR_KM             ((1<<5)|(1<<4))
+#define SCR_KM1            ((1<<5))
+#define SCR_KM0            ((1<<4))
+#define SCR_TXSECON        ((1<<1))
+#define SCR_RXSECON        ((1<<0))
+
+#define BcnItv_BcnItv      (0x01FF)
+
+#define AtimWnd_AtimWnd    (0x01FF)
+
+#define BintrItv_BintrItv  (0x01FF)
+
+#define AtimtrItv_AtimtrItv (0x01FF)
+
+#define PhyDelay_PhyDelay  ((1<<2)|(1<<1)|(1<<0))
+
+#define TPPoll_BQ    ((1<<7))
+#define TPPoll_HPQ   ((1<<6))
+#define TPPoll_NPQ   ((1<<5))
+#define TPPoll_LPQ   ((1<<4))
+#define TPPoll_SBQ   ((1<<3))
+#define TPPoll_SHPQ  ((1<<2))
+#define TPPoll_SNPQ  ((1<<1))
+#define TPPoll_SLPQ  ((1<<0))
+
+#define CWR_CW       (0x01FF)
+
+#define FER_INTR     ((1<<15))
+#define FER_GWAKE    ((1<< 4))
+
+#define FEMR_INTR    ((1<<15))
+#define FEMR_WKUP    ((1<<14))
+#define FEMR_GWAKE   ((1<< 4))
+
+#define FPSR_INTR    ((1<<15))
+#define FPSR_GWAKE   ((1<< 4))
+
+#define FFER_INTR    ((1<<15))
+#define FFER_GWAKE   ((1<< 4))
+
+#ifdef CONFIG_RTL8185B
+// 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 TMGDS			0x0C			// Tx Management Descriptor Address
+#define TBKDS			0x10			// Tx AC_BK Descriptor Address
+#define TBEDS			0x14			// Tx AC_BE Descriptor Address
+#define TLPDS			0x20			// Tx AC_VI Descriptor Address
+#define TNPDS			0x24			// Tx AC_VO Descriptor Address
+#define THPDS			0x28			// Tx Hign Priority Descriptor Address
+
+#define TBDS			0x4c			// Beacon descriptor queue start address
+
+#define RDSA			0xE4			// Receive descriptor queue start address
+
+#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 RTL8185B_VER_REG    0xE1
+
+#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
+
+#ifdef CONFIG_RTL818X_S
+#define BcnTimingAdjust	0x16A			// Beacon Timing Adjust Register.
+#define GPIOCtrl			0x16B			// GPIO Control Register.
+#define PSByGC			0x180			// 0x180 - 0x183 Power Saving by Gated Clock.
+#endif
+#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.
+
+#ifdef CONFIG_RTL818X_S
+#define PI_DATA_READ		0X360	// 0x360 - 0x361  Parallel Interface Data Register.
+#define SI_DATA_READ		0x362	// 0x362 - 0x363  Serial Interface Data Register.
+#endif
+
+//----------------------------------------------------------------------------
+//       8185B TPPoll bits 				(offset 0xd9, 1 byte)
+//----------------------------------------------------------------------------
+#define TPPOLL_BQ			(0x01 << 7)
+#define TPPOLL_HPQ			(0x01 << 6)
+#define TPPOLL_AC_VOQ		(0x01 << 5)
+#define TPPOLL_AC_VIQ		(0x01 << 4)
+#define TPPOLL_AC_BEQ		(0x01 << 3)
+#define TPPOLL_AC_BKQ		(0x01 << 2)
+#define TPPOLL_AC_MGQ		(0x01 << 1)
+
+//----------------------------------------------------------------------------
+//       8185B TPPollStop bits 				(offset 0x93, 1 byte)
+//----------------------------------------------------------------------------
+#define TPPOLLSTOP_BQ			(0x01 << 7)
+#define TPPOLLSTOP_HPQ			(0x01 << 6)
+#define TPPOLLSTOP_AC_VOQ		(0x01 << 5)
+#define TPPOLLSTOP_AC_VIQ		(0x01 << 4)
+#define TPPOLLSTOP_AC_BEQ		(0x01 << 3)
+#define TPPOLLSTOP_AC_BKQ		(0x01 << 2)
+#define TPPOLLSTOP_AC_MGQ		(0x01 << 1)
+
+
+#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 TXOPSEL					(0x01 << 3) //BIT3
+#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_SW BIT31
+#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 diable.
+#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_MASK			0x00FF	// 0x7C[7:0], Crystal calibration mask.
+#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 EEPROM_CID_RSVD1				0x3F
+#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
+#define OFDM_FALSE_ALARM	0xD2
+//by amy 080312}
+
+//YJ,add for Country IE, 080630
+#define EEPROM_COUNTRY_CODE  0x2E
+//YJ,add,080630,end
+#endif
+
+#endif
diff --git a/drivers/staging/rtl8187se/r8180_max2820.c b/drivers/staging/rtl8187se/r8180_max2820.c
new file mode 100644
index 000000000000..cea08463d5ee
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_max2820.c
@@ -0,0 +1,240 @@
+/*
+   This files contains MAXIM MAX2820 radio frontend programming routines.
+
+   This is part of rtl8180 OpenSource driver
+   Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+   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.
+
+   NetBSD rtl8180 driver from Dave Young has been really useful to
+   understand how to program the MAXIM radio. Thanks a lot!!!
+
+   'The Deuce' tested this and fixed some bugs.
+
+   Code from rtl8181 project has been useful to me to understand some things.
+
+   We want to tanks the Authors of such projects and the Ndiswrapper
+   project Authors.
+*/
+
+
+#include "r8180.h"
+#include "r8180_hw.h"
+#include "r8180_max2820.h"
+
+
+//#define DEBUG_MAXIM
+
+u32 maxim_chan[] = {
+	0,	//dummy channel 0
+	12, //1
+	17, //2
+	22, //3
+	27, //4
+	32, //5
+	37, //6
+	42, //7
+	47, //8
+	52, //9
+	57, //10
+	62, //11
+	67, //12
+	72, //13
+	84, //14
+};
+
+#if 0
+/* maxim expects 4 bit address MSF, then 12 bit data MSF*/
+void write_maxim(struct net_device *dev,u8 adr, u32 data)
+{
+
+	int shift;
+	short bit;
+	u16 word;
+
+	adr = adr &0xf;
+	word = (u16)data & 0xfff;
+	word |= (adr<<12);
+	/*write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG | BB_HOST_BANG_EN);
+	read_nic_dword(dev,PHY_CONFIG);
+	mdelay(1);
+
+	write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG | BB_HOST_BANG_EN | BB_HOST_BANG_CLK);
+	read_nic_dword(dev,PHY_CONFIG);
+	mdelay(1);
+	*/
+
+	/* MAX2820 will sample data on rising edge of clock */
+	for(shift = 15;shift >=0; shift--){
+		bit = word>>shift & 1;
+
+		write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG | (bit<<BB_HOST_BANG_DATA));
+
+		read_nic_dword(dev,PHY_CONFIG);
+		mdelay(2);
+
+		write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG |
+		(bit<<BB_HOST_BANG_DATA) | BB_HOST_BANG_CLK); /* sample data */
+
+		read_nic_dword(dev,PHY_CONFIG);
+		mdelay(1);
+
+		write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG |
+		(bit<<BB_HOST_BANG_DATA));
+
+		read_nic_dword(dev,PHY_CONFIG);
+		mdelay(2);
+
+	}
+	write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG | (bit<<BB_HOST_BANG_DATA)|
+					BB_HOST_BANG_EN);
+	read_nic_dword(dev,PHY_CONFIG);
+	mdelay(2);
+
+	/* The shift register fill flush to the requested register the
+	 * last 12 bits data shifted in
+	 */
+	write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG | (bit<<BB_HOST_BANG_DATA)|
+					BB_HOST_BANG_EN | BB_HOST_BANG_CLK);
+	read_nic_dword(dev,PHY_CONFIG);
+	mdelay(2);
+
+	write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG | (bit<<BB_HOST_BANG_DATA)|
+					BB_HOST_BANG_EN);
+	read_nic_dword(dev,PHY_CONFIG);
+	mdelay(2);
+
+
+#ifdef DEBUG_MAXIM
+	DMESG("Writing maxim: %x (adr %x)",phy_config,adr);
+#endif
+
+}
+#endif
+
+void write_maxim(struct net_device *dev,u8 adr, u32 data) {
+	u32 temp;
+	temp =  0x90 + (data & 0xf);
+	temp <<= 16;
+	temp += adr;
+	temp <<= 8;
+	temp += (data >> 4) & 0xff;
+#ifdef DEBUG_MAXIM
+	DMESG("write_maxim: %08x", temp);
+#endif
+	write_nic_dword(dev, PHY_CONFIG, temp);
+	force_pci_posting(dev);
+	mdelay(1);
+}
+
+
+void maxim_write_phy_antenna(struct net_device *dev,short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u8 ant;
+
+	ant = MAXIM_ANTENNA;
+	if(priv->antb) /*default antenna is antenna B */
+		ant |= BB_ANTENNA_B;
+	if(ch == 14)
+		ant |= BB_ANTATTEN_CHAN14;
+	write_phy(dev,0x10,ant);
+	//DMESG("BB antenna %x ",ant);
+}
+
+
+void maxim_rf_set_chan(struct net_device *dev, short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 txpw = 0xff & priv->chtxpwr[ch];
+	u32 chan = maxim_chan[ch];
+
+	/*While philips SA2400 drive the PA bias
+	 *seems that for MAXIM we delegate this
+	 *to the BB
+	 */
+
+	//write_maxim(dev,5,txpw);
+	write_phy(dev,3,txpw);
+
+	maxim_write_phy_antenna(dev,ch);
+	write_maxim(dev,3,chan);
+}
+
+
+void maxim_rf_close(struct net_device *dev)
+{
+	write_phy(dev, 3, 0x8);
+	write_maxim(dev, 1, 0);
+}
+
+
+void maxim_rf_init(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 anaparam;
+
+	write_nic_byte(dev,PHY_DELAY,0x6);	//this is general
+	write_nic_byte(dev,CARRIER_SENSE_COUNTER,0x4c); //this is general
+
+	/*these are maxim specific*/
+	anaparam = read_nic_dword(dev,ANAPARAM);
+	anaparam = anaparam &~ (ANAPARAM_TXDACOFF_SHIFT);
+	anaparam = anaparam &~ANAPARAM_PWR1_MASK;
+	anaparam = anaparam &~ANAPARAM_PWR0_MASK;
+	anaparam |= (MAXIM_ANAPARAM_PWR1_ON<<ANAPARAM_PWR1_SHIFT);
+	anaparam |= (MAXIM_ANAPARAM_PWR0_ON<<ANAPARAM_PWR0_SHIFT);
+
+	//rtl8180_set_anaparam(dev,anaparam);
+
+	/* MAXIM from netbsd driver */
+
+	write_maxim(dev,0, 7); /* test mode as indicated in datasheet*/
+	write_maxim(dev,1, 0x1e); /* enable register*/
+	write_maxim(dev,2, 1); /* synt register */
+
+
+	maxim_rf_set_chan(dev,priv->chan);
+
+	write_maxim(dev,4, 0x313); /* rx register*/
+
+	/* PA is driven directly by the BB, we keep the MAXIM bias
+	 * at the highest value in the boubt tha pleacing it to lower
+	 * values may introduce some further attenuation somewhere..
+	 */
+
+	write_maxim(dev,5, 0xf);
+
+
+	/*baseband configuration*/
+	write_phy(dev,0,0x88); //sys1
+	write_phy(dev,3,0x8); //txagc
+	write_phy(dev,4,0xf8); // lnadet
+	write_phy(dev,5,0x90); // ifagcinit
+	write_phy(dev,6,0x1a); // ifagclimit
+	write_phy(dev,7,0x64); // ifagcdet
+
+	/*Should be done something more here??*/
+
+	maxim_write_phy_antenna(dev,priv->chan);
+
+	write_phy(dev,0x11,0x88); //trl
+	if(priv->diversity)
+		write_phy(dev,0x12,0xc7);
+	else
+		write_phy(dev,0x12,0x47);
+
+	write_phy(dev,0x13,0x9b);
+
+	write_phy(dev,0x19,0x0); //CHESTLIM
+	write_phy(dev,0x1a,0x9f); //CHSQLIM
+
+	maxim_rf_set_chan(dev,priv->chan);
+}
diff --git a/drivers/staging/rtl8187se/r8180_max2820.h b/drivers/staging/rtl8187se/r8180_max2820.h
new file mode 100644
index 000000000000..5d4fb5504841
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_max2820.h
@@ -0,0 +1,21 @@
+/*
+	This is part of rtl8180 OpenSource driver
+	Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+	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.
+*/
+
+#define MAXIM_ANTENNA 0xb3
+#define MAXIM_ANAPARAM_PWR1_ON 0x8
+#define MAXIM_ANAPARAM_PWR0_ON 0x0
+
+
+void maxim_rf_init(struct net_device *dev);
+void maxim_rf_set_chan(struct net_device *dev,short ch);
+
+void maxim_rf_close(struct net_device *dev);
diff --git a/drivers/staging/rtl8187se/r8180_pm.c b/drivers/staging/rtl8187se/r8180_pm.c
new file mode 100644
index 000000000000..3851b9368356
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_pm.c
@@ -0,0 +1,90 @@
+/*
+   Power management interface routines.
+   Written by Mariusz Matuszek.
+   This code is currently just a placeholder for later work and
+   does not do anything useful.
+
+   This is part of rtl8180 OpenSource driver.
+   Copyright (C) Andrea Merello 2004  <andreamrl@tiscali.it>
+   Released under the terms of GPL (General Public Licence)
+*/
+
+#ifdef CONFIG_RTL8180_PM
+
+
+#include "r8180_hw.h"
+#include "r8180_pm.h"
+#include "r8180.h"
+
+int rtl8180_save_state (struct pci_dev *dev, u32 state)
+{
+        printk(KERN_NOTICE "r8180 save state call (state %u).\n", state);
+	return(-EAGAIN);
+}
+
+int rtl8180_suspend (struct pci_dev *pdev, pm_message_t state)
+{
+	struct net_device *dev = pci_get_drvdata(pdev);
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	if (!netif_running(dev))
+		goto out_pci_suspend;
+
+	dev->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;
+}
+
+int rtl8180_resume (struct pci_dev *pdev)
+{
+	struct net_device *dev = pci_get_drvdata(pdev);
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+	int err;
+	u32 val;
+
+	pci_set_power_state(pdev, PCI_D0);
+
+	err = pci_enable_device(pdev);
+	if(err) {
+		printk(KERN_ERR "%s: pci_enable_device failed on resume\n",
+				dev->name);
+
+		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;
+
+	dev->open(dev);
+	netif_device_attach(dev);
+out:
+	return 0;
+}
+
+
+int rtl8180_enable_wake (struct pci_dev *dev, u32 state, int enable)
+{
+        printk(KERN_NOTICE "r8180 enable wake call (state %u, enable %d).\n",
+	       state, enable);
+	return(-EAGAIN);
+}
+
+
+
+#endif //CONFIG_RTL8180_PM
diff --git a/drivers/staging/rtl8187se/r8180_pm.h b/drivers/staging/rtl8187se/r8180_pm.h
new file mode 100644
index 000000000000..7958b3a734db
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_pm.h
@@ -0,0 +1,28 @@
+/*
+        Power management interface routines.
+	Written by Mariusz Matuszek.
+	This code is currently just a placeholder for later work and
+	does not do anything useful.
+
+	This is part of rtl8180 OpenSource driver.
+	Copyright (C) Andrea Merello 2004  <andreamrl@tiscali.it>
+	Released under the terms of GPL (General Public Licence)
+
+*/
+
+#ifdef CONFIG_RTL8180_PM
+
+#ifndef R8180_PM_H
+#define R8180_PM_H
+
+#include <linux/types.h>
+#include <linux/pci.h>
+
+int rtl8180_save_state (struct pci_dev *dev, u32 state);
+int rtl8180_suspend (struct pci_dev *pdev, pm_message_t state);
+int rtl8180_resume (struct pci_dev *pdev);
+int rtl8180_enable_wake (struct pci_dev *dev, u32 state, int enable);
+
+#endif //R8180_PM_H
+
+#endif // CONFIG_RTL8180_PM
diff --git a/drivers/staging/rtl8187se/r8180_rtl8225.c b/drivers/staging/rtl8187se/r8180_rtl8225.c
new file mode 100644
index 000000000000..96ed029ed64a
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_rtl8225.c
@@ -0,0 +1,933 @@
+/*
+  This is part of the rtl8180-sa2400 driver
+  released under the GPL (See file COPYING for details).
+  Copyright (c) 2005 Andrea Merello <andreamrl@tiscali.it>
+
+  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"
+
+
+u8 rtl8225_gain[]={
+	0x23,0x88,0x7c,0xa5,// -82dbm
+	0x23,0x88,0x7c,0xb5,// -82dbm
+	0x23,0x88,0x7c,0xc5,// -82dbm
+	0x33,0x80,0x79,0xc5,// -78dbm
+	0x43,0x78,0x76,0xc5,// -74dbm
+	0x53,0x60,0x73,0xc5,// -70dbm
+	0x63,0x58,0x70,0xc5,// -66dbm
+};
+
+#if 0
+u8 rtl8225_init_gain[]={
+	//0x00,0x00,0x00,0x00,//0x00,0x00,0x00,0x00,
+	0x33,0x80,0x6c,0xc5,//0x00,0x49,0x06,0xb5,//Gain = 0 ~ -78dbm
+	0x43,0x78,0x69,0xc5,//0x00,0x45,0x06,0xb1,//Gain = 1 ~ -74dbm
+	0x53,0x60,0x66,0xc5,//0x00,0x41,0x06,0xab,//Gain = 2 ~ -70dbm
+	0x63,0x58,0x63,0xc5,//0x00,0x3d,0x06,0xa5,//Gain = 3 ~ -66dbm
+	0x73,0x50,0x62,0xc5,//0x00,0x39,0x06,0xa1,//Gain = 4 ~ -62dbm
+	0x83,0x43,0x61,0xc5,//0x00,0x35,0x06,0x9b,//Gain = 5 ~ -58dbm
+	0x93,0x38,0x5a,0xc5,//0x00,0x31,0x06,0x99,//Gain = 6 ~ -54dbm
+};
+#endif
+#ifdef CONFIG_RTL818X_S
+u32 rtl8225_chan[] ={
+              0,
+		0x0080, //ch1
+		0x0100, //ch2
+		0x0180, //ch3
+		0x0200, //ch4
+		0x0280,
+		0x0300,
+		0x0380,
+		0x0400,
+		0x0480,
+		0x0500,
+		0x0580,
+		0x0600,
+		0x0680,
+		0x074A, //ch14
+};
+#else
+u32 rtl8225_chan[] = {
+	0,	//dummy channel 0
+	0x085c, //1
+	0x08dc, //2
+	0x095c, //3
+	0x09dc, //4
+	0x0a5c, //5
+	0x0adc, //6
+	0x0b5c, //7
+	0x0bdc, //8
+	0x0c5c, //9
+	0x0cdc, //10
+	0x0d5c, //11
+	0x0ddc, //12
+	0x0e5c, //13
+	//0x0f5c, //14
+	0x0f72, // 14
+};
+#endif
+
+u16 rtl8225bcd_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,
+	0x07aa, 0x07ab, 0x07ac, 0x07ad, 0x07b0, 0x07b1, 0x07b2, 0x07b3,
+	0x07b4, 0x07b5, 0x07b8, 0x07b9, 0x07ba, 0x07bb, 0x07bb
+
+};
+
+
+#if 0
+u16 rtl8225bc_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, 0x039a, 0x039b, 0x039c, 0x039d,
+	0x03a0, 0x03a1, 0x03a2, 0x03a3, 0x03a4, 0x03a5, 0x03a8, 0x03a9,
+	0x03aa, 0x03ab, 0x03ac, 0x03ad, 0x03b0, 0x03b1, 0x03b2, 0x03b3,
+	0x03b4, 0x03b5, 0x03b8, 0x03b9, 0x03ba, 0x03bb, 0x03bb
+
+};
+
+
+u16 rtl8225a_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,
+	0x07aa, 0x07ab, 0x07ac, 0x07ad, 0x07ad, 0x07ad, 0x07ad, 0x07ad,
+	0x07ad, 0x07ad, 0x07ad, 0x07ad, 0x07ad, 0x07ad, 0x07ad
+};
+#endif
+
+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,
+};
+
+
+u8 rtl8225_tx_gain_cck_ofdm[]={
+	0x02,0x06,0x0e,0x1e,0x3e,0x7e
+};
+
+
+u8 rtl8225_tx_power_ofdm[]={
+	0x80,0x90,0xa2,0xb5,0xcb,0xe4
+};
+
+
+u8 rtl8225_tx_power_cck_ch14[]={
+	0x18,0x17,0x15,0x0c,0x00,0x00,0x00,0x00,
+	0x1b,0x1a,0x17,0x0e,0x00,0x00,0x00,0x00,
+	0x1f,0x1e,0x1a,0x0f,0x00,0x00,0x00,0x00,
+	0x22,0x21,0x1d,0x11,0x00,0x00,0x00,0x00,
+	0x26,0x25,0x21,0x13,0x00,0x00,0x00,0x00,
+	0x2b,0x2a,0x25,0x15,0x00,0x00,0x00,0x00
+};
+
+
+u8 rtl8225_tx_power_cck[]={
+	0x18,0x17,0x15,0x11,0x0c,0x08,0x04,0x02,
+	0x1b,0x1a,0x17,0x13,0x0e,0x09,0x04,0x02,
+	0x1f,0x1e,0x1a,0x15,0x10,0x0a,0x05,0x02,
+	0x22,0x21,0x1d,0x18,0x11,0x0b,0x06,0x02,
+	0x26,0x25,0x21,0x1b,0x14,0x0d,0x06,0x03,
+	0x2b,0x2a,0x25,0x1e,0x16,0x0e,0x07,0x03
+};
+
+
+void rtl8225_set_gain(struct net_device *dev, short gain)
+{
+	write_phy_ofdm(dev, 0x0d, rtl8225_gain[gain * 4]);
+	write_phy_ofdm(dev, 0x23, rtl8225_gain[gain * 4 + 1]);
+	write_phy_ofdm(dev, 0x1b, rtl8225_gain[gain * 4 + 2]);
+	write_phy_ofdm(dev, 0x1d, rtl8225_gain[gain * 4 + 3]);
+}
+#if 0
+
+void rtl8225_set_gain(struct net_device *dev, short gain)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_ON);
+
+	if(priv->card_8185 == 2)
+		write_phy_ofdm(dev, 0x21, 0x27);
+	else
+		write_phy_ofdm(dev, 0x21, 0x37);
+
+	write_phy_ofdm(dev, 0x25, 0x20);
+	write_phy_ofdm(dev, 0x11, 0x6);
+
+	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+		write_phy_ofdm(dev, 0x27, 0x8);
+	else
+		write_phy_ofdm(dev, 0x27, 0x88);
+
+	write_phy_ofdm(dev, 0x14, 0);
+	write_phy_ofdm(dev, 0x16, 0);
+	write_phy_ofdm(dev, 0x15, 0x40);
+	write_phy_ofdm(dev, 0x17, 0x40);
+
+	write_phy_ofdm(dev, 0x0d, rtl8225_gain[gain * 4]);
+	write_phy_ofdm(dev, 0x23, rtl8225_gain[gain * 4 + 1]);
+	write_phy_ofdm(dev, 0x1b, rtl8225_gain[gain * 4 + 2]);
+	write_phy_ofdm(dev, 0x1d, rtl8225_gain[gain * 4 + 3]);
+	//rtl8225_set_gain_usb(dev, gain);
+}
+#endif
+
+
+void write_rtl8225(struct net_device *dev, u8 adr, u16 data)
+{
+	int i;
+	u16 out,select;
+	u8 bit;
+	u32 bangdata = (data << 4) | (adr & 0xf);
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	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 |
+		((priv->card_type == USB) ? 0 : SW_CONTROL_GPIO));
+
+	force_pci_posting(dev);
+	udelay(10);
+
+	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN );//| 0x1fff);
+
+	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 |
+		((priv->card_type == USB) ? 4 : BB_HOST_BANG_EN));
+
+	write_nic_word(dev, RFPinsSelect, select |
+		((priv->card_type == USB) ? 0 : SW_CONTROL_GPIO));
+
+	if(priv->card_type == USB)
+		mdelay(2);
+	else
+		rtl8185_rf_pins_enable(dev);
+}
+
+void rtl8225_rf_close(struct net_device *dev)
+{
+	write_rtl8225(dev, 0x4, 0x1f);
+
+	force_pci_posting(dev);
+	mdelay(1);
+
+	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_OFF);
+	rtl8185_set_anaparam2(dev, RTL8225_ANAPARAM2_OFF);
+}
+
+void rtl8225_SetTXPowerLevel(struct net_device *dev, short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	int GainIdx;
+	int GainSetting;
+	int i;
+	u8 power;
+	u8 *cck_power_table;
+	u8 max_cck_power_level;
+	u8 max_ofdm_power_level;
+	u8 min_ofdm_power_level;
+	u8 cck_power_level = 0xff & priv->chtxpwr[ch];
+	u8 ofdm_power_level = 0xff & priv->chtxpwr_ofdm[ch];
+
+	if(priv->card_type == USB){
+		max_cck_power_level = 11;
+		max_ofdm_power_level = 25; //  12 -> 25
+		min_ofdm_power_level = 10;
+	}else{
+		max_cck_power_level = 35;
+		max_ofdm_power_level = 35;
+		min_ofdm_power_level = 0;
+	}
+	/* CCK power setting */
+	if(cck_power_level > max_cck_power_level)
+		cck_power_level = max_cck_power_level;
+	GainIdx=cck_power_level % 6;
+	GainSetting=cck_power_level / 6;
+
+	if(ch == 14)
+		cck_power_table = rtl8225_tx_power_cck_ch14;
+	else
+		cck_power_table = rtl8225_tx_power_cck;
+
+//	if(priv->card_8185 == 1 && priv->card_8185_Bversion ){
+		/*Ver B*/
+//		write_nic_byte(dev, TX_GAIN_CCK, rtl8225_tx_gain_cck_ofdm[GainSetting]);
+//	}else{
+		/*Ver C - D */
+	write_nic_byte(dev, TX_GAIN_CCK, rtl8225_tx_gain_cck_ofdm[GainSetting]>>1);
+//	}
+
+	for(i=0;i<8;i++){
+
+		power = cck_power_table[GainIdx * 8 + i];
+		write_phy_cck(dev, 0x44 + i, power);
+	}
+
+	/* FIXME Is this delay really needeed ? */
+	force_pci_posting(dev);
+	mdelay(1);
+
+	/* OFDM power setting */
+//  Old:
+//	if(ofdm_power_level > max_ofdm_power_level)
+//		ofdm_power_level = 35;
+//	ofdm_power_level += min_ofdm_power_level;
+//  Latest:
+	if(ofdm_power_level > (max_ofdm_power_level - min_ofdm_power_level))
+		ofdm_power_level = max_ofdm_power_level;
+	else
+		ofdm_power_level += min_ofdm_power_level;
+	if(ofdm_power_level > 35)
+		ofdm_power_level = 35;
+//
+
+	GainIdx=ofdm_power_level % 6;
+	GainSetting=ofdm_power_level / 6;
+#if 1
+//	if(priv->card_type == USB){
+		rtl8185_set_anaparam2(dev,RTL8225_ANAPARAM2_ON);
+
+		write_phy_ofdm(dev,2,0x42);
+		write_phy_ofdm(dev,6,0);
+		write_phy_ofdm(dev,8,0);
+//	}
+#endif
+//	if(priv->card_8185 == 1 && priv->card_8185_Bversion){
+//		/*Ver B*/
+//		write_nic_byte(dev, TX_GAIN_OFDM, rtl8225_tx_gain_cck_ofdm[GainSetting]);
+//	}else{
+		/*Ver C - D */
+	write_nic_byte(dev, TX_GAIN_OFDM, rtl8225_tx_gain_cck_ofdm[GainSetting]>>1);
+//	}
+
+
+	power = rtl8225_tx_power_ofdm[GainIdx];
+
+	write_phy_ofdm(dev, 0x5, power);
+	write_phy_ofdm(dev, 0x7, power);
+
+	force_pci_posting(dev);
+	mdelay(1);
+	//write_nic_byte(dev, TX_AGC_CONTROL,4);
+}
+#if 0
+/* switch between mode B and G */
+void rtl8225_set_mode(struct net_device *dev, short modeb)
+{
+	write_phy_ofdm(dev, 0x15, (modeb ? 0x0 : 0x40));
+	write_phy_ofdm(dev, 0x17, (modeb ? 0x0 : 0x40));
+}
+#endif
+void rtl8225_rf_set_chan(struct net_device *dev, short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	short gset = (priv->ieee80211->state == IEEE80211_LINKED &&
+		ieee80211_is_54g(priv->ieee80211->current_network)) ||
+		priv->ieee80211->iw_mode == IW_MODE_MONITOR;
+
+	rtl8225_SetTXPowerLevel(dev, ch);
+
+	write_rtl8225(dev, 0x7, rtl8225_chan[ch]);
+
+	force_pci_posting(dev);
+	mdelay(10);
+
+	// A mode sifs 0x44, difs 34-14, slot 9, eifs 23, cwm 3, cwM 7, ctstoself 0x10
+	if(gset){
+		write_nic_byte(dev,SIFS,0x22);// SIFS: 0x22
+		write_nic_byte(dev,DIFS,0x14); //DIFS: 20
+		//write_nic_byte(dev,DIFS,20); //DIFS: 20
+	}else{
+		write_nic_byte(dev,SIFS,0x44);// SIFS: 0x22
+		write_nic_byte(dev,DIFS,50 - 14); //DIFS: 36
+	}
+	if(priv->ieee80211->state == IEEE80211_LINKED &&
+		ieee80211_is_shortslot(priv->ieee80211->current_network))
+		write_nic_byte(dev,SLOT,0x9); //SLOT: 9
+
+	else
+		write_nic_byte(dev,SLOT,0x14); //SLOT: 20 (0x14)
+
+
+	if(gset){
+		write_nic_byte(dev,EIFS,81);//91 - 20); // EIFS: 91 (0x5B)
+		write_nic_byte(dev,CW_VAL,0x73); //CW VALUE: 0x37
+		//DMESG("using G net params");
+	}else{
+		write_nic_byte(dev,EIFS,81); // EIFS: 91 (0x5B)
+		write_nic_byte(dev,CW_VAL,0xa5); //CW VALUE: 0x37
+		//DMESG("using B net params");
+	}
+
+
+}
+
+void rtl8225_host_pci_init(struct net_device *dev)
+{
+	write_nic_word(dev, RFPinsOutput, 0x480);
+
+	rtl8185_rf_pins_enable(dev);
+
+	//if(priv->card_8185 == 2 && priv->enable_gpio0 ) /* version D */
+	//write_nic_word(dev, RFPinsSelect, 0x88);
+	//else
+	write_nic_word(dev, RFPinsSelect, 0x88 | SW_CONTROL_GPIO); /* 0x488 | SW_CONTROL_GPIO */
+
+	write_nic_byte(dev, GP_ENABLE, 0);
+
+	force_pci_posting(dev);
+	mdelay(200);
+
+	write_nic_word(dev, GP_ENABLE, 0xff & (~(1<<6))); /* bit 6 is for RF on/off detection */
+
+
+}
+
+void rtl8225_host_usb_init(struct net_device *dev)
+{
+	#if 0
+	write_nic_byte(dev,RFPinsSelect+1,0);
+
+	write_nic_byte(dev,GPIO,0);
+
+	write_nic_byte_E(dev,0x53,read_nic_byte_E(dev,0x53) | (1<<7));
+
+	write_nic_byte(dev,RFPinsSelect+1,4);
+
+	write_nic_byte(dev,GPIO,0x20);
+
+	write_nic_byte(dev,GP_ENABLE,0);
+
+
+	/* Config BB & RF */
+	write_nic_word(dev, RFPinsOutput, 0x80);
+
+	write_nic_word(dev, RFPinsSelect, 0x80);
+
+	write_nic_word(dev, RFPinsEnable, 0x80);
+
+
+	mdelay(100);
+
+	mdelay(1000);
+#endif
+
+}
+
+void rtl8225_rf_sleep(struct net_device *dev)
+{
+	write_rtl8225(dev,0x4,0xdff);
+	force_pci_posting(dev);
+	mdelay(1);
+	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_SLEEP);
+	rtl8185_set_anaparam2(dev, RTL8225_ANAPARAM2_SLEEP);
+	force_pci_posting(dev);
+}
+
+void rtl8225_rf_wakeup(struct net_device *dev)
+{
+	write_rtl8225(dev,0x4,0x9ff);
+	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_ON);
+	rtl8185_set_anaparam2(dev, RTL8225_ANAPARAM2_ON);
+	force_pci_posting(dev);
+}
+
+void rtl8225_rf_init(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int i;
+	short channel = 1;
+	u16 brsr;
+
+	priv->chan = channel;
+
+	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_ON);
+
+
+	if(priv->card_type == USB)
+		rtl8225_host_usb_init(dev);
+	else
+		rtl8225_host_pci_init(dev);
+
+	write_nic_dword(dev, RF_TIMING, 0x000a8008);
+
+	brsr = read_nic_word(dev, BRSR);
+
+	write_nic_word(dev, BRSR, 0xffff);
+
+	#if 0
+	if(priv->card_8185 == 1){/* version C or B */
+		if(priv->card_8185_Bversion)  /* version B*/
+			write_nic_dword(dev, RF_PARA, 0x44);
+		else    /* version C */
+			write_nic_dword(dev, RF_PARA, 0x100044);
+	}else{ /* version D */
+		if(priv->enable_gpio0)
+			write_nic_dword(dev, RF_PARA, 0x20100044);
+		else /* also USB */
+			write_nic_dword(dev, RF_PARA, 0x100044);
+	}
+	#endif
+
+	write_nic_dword(dev, RF_PARA, 0x100044);
+
+	#if 1  //0->1
+	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
+	write_nic_byte(dev, CONFIG3, 0x44);
+	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
+	#endif
+
+	if(priv->card_type == USB){
+		rtl8185_rf_pins_enable(dev);
+
+		mdelay(1000);
+	}
+
+	write_rtl8225(dev, 0x0, 0x67); mdelay(1);
+
+
+	write_rtl8225(dev, 0x1, 0xfe0); mdelay(1);
+
+	write_rtl8225(dev, 0x2, 0x44d); mdelay(1);
+
+	write_rtl8225(dev, 0x3, 0x441); mdelay(1);
+
+	if(priv->card_type == USB)
+		write_rtl8225(dev, 0x4, 0x486);
+	else
+		write_rtl8225(dev, 0x4, 0x8be);
+
+	mdelay(1);
+
+
+	#if 0
+	}else if(priv->phy_ver == 1){
+		/* version A */
+		write_rtl8225(dev, 0x5, 0xbc0 + 2);
+	}else{
+	#endif
+	/* version B & C */
+
+	if(priv->card_type == USB)
+		write_rtl8225(dev, 0x5, 0xbc0);
+	else if(priv->card_type == MINIPCI)
+		write_rtl8225(dev, 0x5, 0xbc0 + 3 +(6<<3));
+	else
+		write_rtl8225(dev, 0x5, 0xbc0 + (6<<3));
+
+	 mdelay(1);
+//	}
+
+	write_rtl8225(dev, 0x6, 0xae6);  mdelay(1);
+
+	write_rtl8225(dev, 0x7, ((priv->card_type == USB)? 0x82a : rtl8225_chan[channel]));  mdelay(1);
+
+	write_rtl8225(dev, 0x8, 0x1f);  mdelay(1);
+
+	write_rtl8225(dev, 0x9, 0x334);  mdelay(1);
+
+	write_rtl8225(dev, 0xa, 0xfd4);  mdelay(1);
+
+	write_rtl8225(dev, 0xb, 0x391);  mdelay(1);
+
+	write_rtl8225(dev, 0xc, 0x50);  mdelay(1);
+
+
+	write_rtl8225(dev, 0xd, 0x6db);   mdelay(1);
+
+	write_rtl8225(dev, 0xe, 0x29);  mdelay(1);
+
+	write_rtl8225(dev, 0xf, 0x914);
+
+	if(priv->card_type == USB){
+		//force_pci_posting(dev);
+		mdelay(100);
+	}
+
+	write_rtl8225(dev, 0x2, 0xc4d);
+
+	if(priv->card_type == USB){
+	//	force_pci_posting(dev);
+		mdelay(200);
+
+		write_rtl8225(dev, 0x2, 0x44d);
+
+	//	force_pci_posting(dev);
+		mdelay(100);
+
+	}//End of if(priv->card_type == USB)
+	/* FIXME!! rtl8187 we have to check if calibrarion
+	 * is successful and eventually cal. again (repeat
+	 * the two write on reg 2)
+	*/
+	force_pci_posting(dev);
+
+	mdelay(100); //200 for 8187
+
+	//if(priv->card_type != USB) /* maybe not needed even for 8185 */
+//	write_rtl8225(dev, 0x7, rtl8225_chan[channel]);
+
+	write_rtl8225(dev, 0x0, 0x127);
+
+	for(i=0;i<95;i++){
+		write_rtl8225(dev, 0x1, (u8)(i+1));
+
+		#if 0
+		if(priv->phy_ver == 1)
+			/* version A */
+			write_rtl8225(dev, 0x2, rtl8225a_rxgain[i]);
+		else
+		#endif
+		/* version B & C & D*/
+
+		write_rtl8225(dev, 0x2, rtl8225bcd_rxgain[i]);
+	}
+
+	write_rtl8225(dev, 0x0, 0x27);
+
+
+//	//if(priv->card_type != USB){
+//		write_rtl8225(dev, 0x2, 0x44d);
+//		write_rtl8225(dev, 0x7, rtl8225_chan[channel]);
+//		write_rtl8225(dev, 0x2, 0x47d);
+//
+//		force_pci_posting(dev);
+//		mdelay(100);
+//
+//		write_rtl8225(dev, 0x2, 0x44d);
+//	//}
+
+	write_rtl8225(dev, 0x0, 0x22f);
+
+	if(priv->card_type != USB)
+		rtl8185_rf_pins_enable(dev);
+
+	for(i=0;i<128;i++){
+		write_phy_ofdm(dev, 0xb, rtl8225_agc[i]);
+
+		mdelay(1);
+		write_phy_ofdm(dev, 0xa, (u8)i+ 0x80);
+
+		mdelay(1);
+	}
+
+	force_pci_posting(dev);
+	mdelay(1);
+
+	write_phy_ofdm(dev, 0x0, 0x1); mdelay(1);
+	write_phy_ofdm(dev, 0x1, 0x2); mdelay(1);
+	write_phy_ofdm(dev, 0x2, ((priv->card_type == USB)? 0x42 : 0x62)); mdelay(1);
+	write_phy_ofdm(dev, 0x3, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x4, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x5, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x6, 0x40); mdelay(1);
+	write_phy_ofdm(dev, 0x7, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x8, 0x40); mdelay(1);
+	write_phy_ofdm(dev, 0x9, 0xfe); mdelay(1);
+
+	#if 0
+	if(priv->card_type == USB){
+		write_phy_ofdm(dev, 0xa, 0x9);
+	}else{
+		if(priv->card_8185 == 1 && priv->card_8185_Bversion){
+			/* Ver B
+			* maybe later version can accept this also?
+			*/
+			write_phy_ofdm(dev, 0xa, 0x6);
+			write_phy_ofdm(dev, 0x18, 0x6f);
+		}else{
+	#endif
+			/* ver C & D */
+	write_phy_ofdm(dev, 0xa, 0x9); mdelay(1);
+
+	//write_phy_ofdm(dev, 0x18, 0xef);
+	//	}
+	//}
+	write_phy_ofdm(dev, 0xb, 0x80); mdelay(1);
+
+	write_phy_ofdm(dev, 0xc, 0x1);mdelay(1);
+
+
+	//if(priv->card_type != USB)
+	//write_phy_ofdm(dev, 0xd, 0x33); // <>
+
+	write_phy_ofdm(dev, 0xe, 0xd3);mdelay(1);
+
+
+	#if 0
+	if(priv->card_8185 == 1){
+		if(priv->card_8185_Bversion)
+			write_phy_ofdm(dev, 0xf, 0x20);/*ver B*/
+		else
+			write_phy_ofdm(dev, 0xf, 0x28);/*ver C*/
+	}else{
+	#endif
+	write_phy_ofdm(dev, 0xf, 0x38);mdelay(1);
+/*ver D & 8187*/
+//	}
+
+//	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+//		write_phy_ofdm(dev, 0x10, 0x04);/*ver B*/
+//	else
+	write_phy_ofdm(dev, 0x10, 0x84);mdelay(1);
+/*ver C & D & 8187*/
+
+	write_phy_ofdm(dev, 0x11, 0x06);mdelay(1);
+/*agc resp time 700*/
+
+
+//	if(priv->card_8185 == 2){
+	/* Ver D & 8187*/
+	write_phy_ofdm(dev, 0x12, 0x20);mdelay(1);
+
+	write_phy_ofdm(dev, 0x13, 0x20);mdelay(1);
+
+#if 0
+	}else{
+		/* Ver B & C*/
+		write_phy_ofdm(dev, 0x12, 0x0);
+		write_phy_ofdm(dev, 0x13, 0x0);
+	}
+#endif
+	write_phy_ofdm(dev, 0x14, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x15, 0x40); mdelay(1);
+	write_phy_ofdm(dev, 0x16, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x17, 0x40); mdelay(1);
+
+//	if (priv->card_type == USB)
+//		write_phy_ofdm(dev, 0x18, 0xef);
+
+	write_phy_ofdm(dev, 0x18, 0xef);mdelay(1);
+
+
+	write_phy_ofdm(dev, 0x19, 0x19); mdelay(1);
+	write_phy_ofdm(dev, 0x1a, 0x20); mdelay(1);
+
+//	if (priv->card_type != USB){
+//		if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+//			write_phy_ofdm(dev, 0x1b, 0x66); /* Ver B */
+//		else
+	write_phy_ofdm(dev, 0x1b, 0x76);mdelay(1);
+ /* Ver C & D */ //FIXME:MAYBE not needed
+//	}
+
+	write_phy_ofdm(dev, 0x1c, 0x4);mdelay(1);
+
+#if 0
+	if(priv->card_8185 == 1){
+		if(priv->card_8185_Bversion){
+			/*ver B*/
+			write_phy_ofdm(dev, 0x1e, 0x95);
+			write_phy_ofdm(dev, 0x1f, 0x55);
+		}else{
+			/*ver C*/
+			write_phy_ofdm(dev, 0x1e, 0x90);
+			write_phy_ofdm(dev, 0x1f, 0x34);
+
+		}
+	}else{
+#endif
+		/*ver D & 8187*/
+	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, 0x27);mdelay(1);
+
+	write_phy_ofdm(dev, 0x22, 0x16);mdelay(1);
+
+//	if(priv->card_type != USB)
+	//write_phy_ofdm(dev, 0x23, 0x43); //FIXME maybe not needed // <>
+
+	write_phy_ofdm(dev, 0x24, 0x46); mdelay(1);
+	write_phy_ofdm(dev, 0x25, 0x20); mdelay(1);
+	write_phy_ofdm(dev, 0x26, 0x90); mdelay(1);
+#if 0
+	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+		write_phy_ofdm(dev, 0x27, 0x08); /* Ver B. might work also fo ver C&D ?*/
+	else
+#endif
+	write_phy_ofdm(dev, 0x27, 0x88); mdelay(1);
+/* Ver C & D & 8187*/
+
+	// <> Set init. gain to m74dBm.
+	write_phy_ofdm(dev, 0x0d, 0x43);	 mdelay(1);
+	write_phy_ofdm(dev, 0x1b, 0x76);	 mdelay(1);
+	write_phy_ofdm(dev, 0x1d, 0xc5);	 mdelay(1);
+	write_phy_ofdm(dev, 0x23, 0x78);	 mdelay(1);
+
+	//if(priv->card_type == USB);
+	//	rtl8225_set_gain_usb(dev, 1); /* FIXME this '2' is random */
+
+	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);
+#if 0
+	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+		write_phy_cck(dev, 0x7, 0xd8); /* Ver B */
+	else
+#endif
+	write_phy_cck(dev, 0x7, 0x78);mdelay(1);
+ /* Ver C & D & 8187*/
+
+	write_phy_cck(dev, 0x8, 0x2e);mdelay(1);
+
+	write_phy_cck(dev, 0x10, ((priv->card_type == USB) ? 0x9b: 0x93)); mdelay(1);
+	write_phy_cck(dev, 0x11, 0x88); mdelay(1);
+	write_phy_cck(dev, 0x12, 0x47); mdelay(1);
+#if 0
+	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+		write_phy_cck(dev, 0x13, 0x98); /* Ver B */
+	else
+#endif
+	write_phy_cck(dev, 0x13, 0xd0); /* Ver C & D & 8187*/
+
+	write_phy_cck(dev, 0x19, 0x0);
+	write_phy_cck(dev, 0x1a, 0xa0);
+	write_phy_cck(dev, 0x1b, 0x8);
+	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, 0x1f); mdelay(1);
+	write_phy_cck(dev, 0x45, 0x1e); mdelay(1);
+	write_phy_cck(dev, 0x46, 0x1a); mdelay(1);
+	write_phy_cck(dev, 0x47, 0x15); mdelay(1);
+	write_phy_cck(dev, 0x48, 0x10); mdelay(1);
+	write_phy_cck(dev, 0x49, 0xa); mdelay(1);
+	write_phy_cck(dev, 0x4a, 0x5); mdelay(1);
+	write_phy_cck(dev, 0x4b, 0x2); mdelay(1);
+	write_phy_cck(dev, 0x4c, 0x5);mdelay(1);
+
+
+	write_nic_byte(dev, 0x5b, 0x0d); mdelay(1);
+
+
+
+// <>
+//	// TESTR 0xb 8187
+//	write_phy_cck(dev, 0x10, 0x93);// & 0xfb);
+//
+//	//if(priv->card_type != USB){
+//		write_phy_ofdm(dev, 0x2, 0x62);
+//		write_phy_ofdm(dev, 0x6, 0x0);
+//		write_phy_ofdm(dev, 0x8, 0x0);
+//	//}
+
+	rtl8225_SetTXPowerLevel(dev, channel);
+
+	write_phy_cck(dev, 0x10, 0x9b); mdelay(1); /* Rx ant A, 0xdb for B */
+	write_phy_ofdm(dev, 0x26, 0x90); mdelay(1); /* Rx ant A, 0x10 for B */
+
+	rtl8185_tx_antenna(dev, 0x3); /* TX ant A, 0x0 for B */
+
+	/* switch to high-speed 3-wire
+	 * last digit. 2 for both cck and ofdm
+	 */
+	if(priv->card_type == USB)
+		write_nic_dword(dev, 0x94, 0x3dc00002);
+	else{
+		write_nic_dword(dev, 0x94, 0x15c00002);
+		rtl8185_rf_pins_enable(dev);
+	}
+
+//	if(priv->card_type != USB)
+//	rtl8225_set_gain(dev, 4); /* FIXME this '1' is random */ // <>
+//	 rtl8225_set_mode(dev, 1); /* FIXME start in B mode */ // <>
+//
+//	/* make sure is waken up! */
+//	write_rtl8225(dev,0x4, 0x9ff);
+//	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_ON);
+//	rtl8185_set_anaparam2(dev, RTL8225_ANAPARAM2_ON);
+
+	rtl8225_rf_set_chan(dev, priv->chan);
+
+	write_nic_word(dev,BRSR,brsr);
+
+}
diff --git a/drivers/staging/rtl8187se/r8180_rtl8225.h b/drivers/staging/rtl8187se/r8180_rtl8225.h
new file mode 100644
index 000000000000..458de6629ad0
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_rtl8225.h
@@ -0,0 +1,44 @@
+/*
+  This is part of the rtl8180-sa2400 driver
+  released under the GPL (See file COPYING for details).
+  Copyright (c) 2005 Andrea Merello <andreamrl@tiscali.it>
+
+  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
+
+#ifdef CONFIG_RTL8185B
+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 rtl8225_host_pci_init(struct net_device *dev);
+void rtl8225_host_usb_init(struct net_device *dev);
+
+void write_rtl8225(struct net_device *dev, u8 adr, u16 data);
+void RF_WriteReg(struct net_device *dev, u8 offset, u32	data);
+u32 RF_ReadReg(struct net_device *dev, u8 offset);
+#endif
+void rtl8225_rf_init(struct net_device *dev);
+void rtl8225_rf_set_chan(struct net_device *dev,short ch);
+void rtl8225_rf_close(struct net_device *dev);
+void rtl8225_rf_sleep(struct net_device *dev);
+void rtl8225_rf_wakeup(struct net_device *dev);
+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,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
new file mode 100644
index 000000000000..4136b9429597
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_rtl8225z2.c
@@ -0,0 +1,1587 @@
+/*
+  This is part of the rtl8180-sa2400 driver
+  released under the GPL (See file COPYING for details).
+  Copyright (c) 2005 Andrea Merello <andreamrl@tiscali.it>
+
+  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"
+
+#ifdef ENABLE_DOT11D
+#include "dot11d.h"
+#endif
+
+#ifdef CONFIG_RTL8185B
+
+extern u8 rtl8225_agc[];
+
+extern u32 rtl8225_chan[];
+
+//2005.11.16
+u8 rtl8225z2_threshold[]={
+        0x8d, 0x8d, 0x8d, 0x8d, 0x9d, 0xad, 0xbd,
+};
+
+//      0xd 0x19 0x1b 0x21
+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
+};
+
+u8 rtl8225z2_gain_a[]={
+	0x13,0x27,0x5a,//,0x37,// -82dbm
+	0x23,0x23,0x58,//,0x37,// -82dbm
+	0x33,0x1f,0x56,//,0x37,// -82dbm
+	0x43,0x1b,0x54,//,0x37,// -78dbm
+	0x53,0x17,0x51,//,0x37,// -74dbm
+	0x63,0x24,0x4f,//,0x37,// -70dbm
+	0x73,0x0f,0x4c,//,0x37,// -66dbm
+};
+#if 0
+u32 rtl8225_chan[] = {
+	0,	//dummy channel 0
+	0x085c, //1
+	0x08dc, //2
+	0x095c, //3
+	0x09dc, //4
+	0x0a5c, //5
+	0x0adc, //6
+	0x0b5c, //7
+	0x0bdc, //8
+	0x0c5c, //9
+	0x0cdc, //10
+	0x0d5c, //11
+	0x0ddc, //12
+	0x0e5c, //13
+	//0x0f5c, //14
+	0x0f72, // 14
+};
+#endif
+
+//-
+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, 0x03bb
+
+};
+
+//2005.11.16,
+u8 ZEBRA2_CCK_OFDM_GAIN_SETTING[]={
+        0x00,0x01,0x02,0x03,0x04,0x05,
+        0x06,0x07,0x08,0x09,0x0a,0x0b,
+        0x0c,0x0d,0x0e,0x0f,0x10,0x11,
+        0x12,0x13,0x14,0x15,0x16,0x17,
+        0x18,0x19,0x1a,0x1b,0x1c,0x1d,
+        0x1e,0x1f,0x20,0x21,0x22,0x23,
+};
+
+#if 0
+//-
+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,
+};
+#endif
+/*
+ from 0 to 0x23
+u8 rtl8225_tx_gain_cck_ofdm[]={
+	0x02,0x06,0x0e,0x1e,0x3e,0x7e
+};
+*/
+
+//-
+u8 rtl8225z2_tx_power_ofdm[]={
+	0x42,0x00,0x40,0x00,0x40
+};
+
+
+//-
+u8 rtl8225z2_tx_power_cck_ch14[]={
+	0x36,0x35,0x2e,0x1b,0x00,0x00,0x00,0x00
+};
+
+
+//-
+u8 rtl8225z2_tx_power_cck[]={
+	0x36,0x35,0x2e,0x25,0x1c,0x12,0x09,0x04
+};
+
+
+void rtl8225z2_set_gain(struct net_device *dev, short gain)
+{
+	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, 0x0d, rtl8225_gain[gain * 3]);
+	//write_phy_ofdm(dev, 0x19, rtl8225_gain[gain * 3 + 1]);
+	//write_phy_ofdm(dev, 0x1b, rtl8225_gain[gain * 3 + 2]);
+        //2005.11.17, by ch-hsu
+        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);
+
+}
+
+#if 0
+
+void rtl8225_set_gain(struct net_device *dev, short gain)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_ON);
+
+	if(priv->card_8185 == 2)
+		write_phy_ofdm(dev, 0x21, 0x27);
+	else
+		write_phy_ofdm(dev, 0x21, 0x37);
+
+	write_phy_ofdm(dev, 0x25, 0x20);
+	write_phy_ofdm(dev, 0x11, 0x6);
+
+	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+		write_phy_ofdm(dev, 0x27, 0x8);
+	else
+		write_phy_ofdm(dev, 0x27, 0x88);
+
+	write_phy_ofdm(dev, 0x14, 0);
+	write_phy_ofdm(dev, 0x16, 0);
+	write_phy_ofdm(dev, 0x15, 0x40);
+	write_phy_ofdm(dev, 0x17, 0x40);
+
+	write_phy_ofdm(dev, 0x0d, rtl8225_gain[gain * 4]);
+	write_phy_ofdm(dev, 0x23, rtl8225_gain[gain * 4 + 1]);
+	write_phy_ofdm(dev, 0x1b, rtl8225_gain[gain * 4 + 2]);
+	write_phy_ofdm(dev, 0x1d, rtl8225_gain[gain * 4 + 3]);
+	//rtl8225_set_gain_usb(dev, gain);
+}
+#endif
+
+u32 read_rtl8225(struct net_device *dev, u8 adr)
+{
+	u32 data2Write = ((u32)(adr & 0x1f)) << 27;
+	u32 dataRead;
+	u32 mask;
+	u16 oval,oval2,oval3,tmp;
+//	ThreeWireReg twreg;
+//	ThreeWireReg tdata;
+	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);
+	}
+
+	//twreg.struc.clk = 0;
+	//twreg.struc.data = 0;
+	write_nic_word(dev, RFPinsOutput, rw|oval); udelay(2);
+	mask = (low2high) ? 0x01 : (((u32)0x01) << (12-1));
+
+	// We must set data pin to HW controled, otherwise RF can't driver it and
+	// value RF register won't be able to read back properly. 2006.06.13, by rcnjko.
+	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;
+
+}
+#if 0
+void write_rtl8225(struct net_device *dev, u8 adr, u16 data)
+{
+	int i;
+	u16 out,select;
+	u8 bit;
+	u32 bangdata = (data << 4) | (adr & 0xf);
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	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 |
+		((priv->card_type == USB) ? 0 : SW_CONTROL_GPIO));
+
+	force_pci_posting(dev);
+	udelay(10);
+
+	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN );//| 0x1fff);
+
+	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 |
+		((priv->card_type == USB) ? 4 : BB_HOST_BANG_EN));
+
+	write_nic_word(dev, RFPinsSelect, select |
+		((priv->card_type == USB) ? 0 : SW_CONTROL_GPIO));
+
+	if(priv->card_type == USB)
+		mdelay(2);
+	else
+		rtl8185_rf_pins_enable(dev);
+}
+
+#endif
+short rtl8225_is_V_z2(struct net_device *dev)
+{
+	short vz2 = 1;
+	//int i;
+	/* sw to reg pg 1 */
+	//write_rtl8225(dev, 0, 0x1b7);
+	//write_rtl8225(dev, 0, 0x0b7);
+
+	/* reg 8 pg 1 = 23*/
+	//printk(KERN_WARNING "RF Rigisters:\n");
+#if 0
+	for(i = 0; i <= 0xf; i++)
+		printk(KERN_WARNING "%08x,", read_rtl8225(dev, i));
+	//printk(KERN_WARNING "reg[9]@pg1 = 0x%x\n", read_rtl8225(dev, 0x0F));
+
+//	printk(KERN_WARNING "RF:\n");
+#endif
+	if( read_rtl8225(dev, 8) != 0x588)
+		vz2 = 0;
+
+	else	/* reg 9 pg 1 = 24 */
+		if( read_rtl8225(dev, 9) != 0x700)
+			vz2 = 0;
+
+	/* sw back to pg 0 */
+	write_rtl8225(dev, 0, 0xb7);
+
+	return vz2;
+
+}
+
+#if 0
+void rtl8225_rf_close(struct net_device *dev)
+{
+	write_rtl8225(dev, 0x4, 0x1f);
+
+	force_pci_posting(dev);
+	mdelay(1);
+
+	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_OFF);
+	rtl8185_set_anaparam2(dev, RTL8225_ANAPARAM2_OFF);
+}
+#endif
+#if 0
+short rtl8225_rf_set_sens(struct net_device *dev, short sens)
+{
+	if (sens <0 || sens > 6) return -1;
+
+	if(sens > 4)
+		write_rtl8225(dev, 0x0c, 0x850);
+	else
+		write_rtl8225(dev, 0x0c, 0x50);
+
+	sens= 6-sens;
+	rtl8225_set_gain(dev, sens);
+
+	write_phy_cck(dev, 0x41, rtl8225_threshold[sens]);
+	return 0;
+
+}
+#endif
+
+
+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);
+}
+
+#ifdef ENABLE_DOT11D
+//
+//	Description:
+//		Map dBm into Tx power index according to
+//		current HW model, for example, RF and PA, and
+//		current wireless mode.
+//
+s8
+DbmToTxPwrIdx(
+	struct r8180_priv *priv,
+	WIRELESS_MODE	WirelessMode,
+	s32			PowerInDbm
+	)
+{
+ 	bool bUseDefault = true;
+	s8 TxPwrIdx = 0;
+
+#ifdef CONFIG_RTL818X_S
+	//
+	// 071011, SD3 SY:
+	// OFDM Power in dBm = Index * 0.5 + 0
+	// CCK Power in dBm = Index * 0.25 + 13
+	//
+	if(priv->card_8185 >= VERSION_8187S_B)
+	{
+		s32 tmp = 0;
+
+		if(WirelessMode == 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(WirelessMode == 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;
+		}
+	}
+#endif
+
+	//
+	// TRUE if we want to use a default implementation.
+	// We shall set it to FALSE when we have exact translation formular
+	// 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;
+}
+#endif
+
+void rtl8225z2_SetTXPowerLevel(struct net_device *dev, short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+//	int GainIdx;
+//	int GainSetting;
+	//int i;
+	//u8 power;
+	//u8 *cck_power_table;
+	u8 max_cck_power_level;
+	//u8 min_cck_power_level;
+	u8 max_ofdm_power_level;
+	u8 min_ofdm_power_level;
+//	u8 cck_power_level = 0xff & priv->chtxpwr[ch];//-by amy 080312
+//	u8 ofdm_power_level = 0xff & priv->chtxpwr_ofdm[ch];//-by amy 080312
+	char cck_power_level = (char)(0xff & priv->chtxpwr[ch]);//+by amy 080312
+	char ofdm_power_level = (char)(0xff & priv->chtxpwr_ofdm[ch]);//+by amy 080312
+#if 0
+	//
+	// CCX 2 S31, AP control of client transmit power:
+	// 1. We shall not exceed Cell Power Limit as possible as we can.
+	// 2. Tolerance is +/- 5dB.
+	// 3. 802.11h Power Contraint takes higher precedence over CCX Cell Power Limit.
+	//
+	// TODO:
+	// 1. 802.11h power contraint
+	//
+	// 071011, by rcnjko.
+	//
+	if(	priv->OpMode == RT_OP_MODE_INFRASTRUCTURE &&
+		priv->bWithCcxCellPwr &&
+		ch == priv->dot11CurrentChannelNumber)
+	{
+		u8 CckCellPwrIdx = DbmToTxPwrIdx(dev, WIRELESS_MODE_B, pMgntInfo->CcxCellPwr);
+		u8 OfdmCellPwrIdx = DbmToTxPwrIdx(dev, WIRELESS_MODE_G, pMgntInfo->CcxCellPwr);
+
+		printk("CCX Cell Limit: %d dBm => CCK Tx power index : %d, OFDM Tx power index: %d\n",
+			priv->CcxCellPwr, CckCellPwrIdx, OfdmCellPwrIdx);
+		printk("EEPROM channel(%d) => CCK Tx power index: %d, OFDM Tx power index: %d\n",
+			channel, CckTxPwrIdx, OfdmTxPwrIdx);
+
+		if(cck_power_level > CckCellPwrIdx)
+			cck_power_level = CckCellPwrIdx;
+		if(ofdm_power_level > OfdmCellPwrIdx)
+			ofdm_power_level = OfdmCellPwrIdx;
+
+		printk("Altered CCK Tx power index : %d, OFDM Tx power index: %d\n",
+			CckTxPwrIdx, OfdmTxPwrIdx);
+	}
+#endif
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(priv->ieee80211) &&
+		IS_DOT11D_STATE_DONE(priv->ieee80211) )
+	{
+		//PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(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);
+
+		//printk("Max Tx Power dBm (%d) => CCK Tx power index : %d, OFDM Tx power index: %d\n", MaxTxPwrInDbm, CckMaxPwrIdx, OfdmMaxPwrIdx);
+
+		//printk("EEPROM channel(%d) => CCK Tx power index: %d, OFDM Tx power index: %d\n",
+		//	ch, cck_power_level, ofdm_power_level);
+
+		if(cck_power_level > CckMaxPwrIdx)
+			cck_power_level = CckMaxPwrIdx;
+		if(ofdm_power_level > OfdmMaxPwrIdx)
+			ofdm_power_level = OfdmMaxPwrIdx;
+	}
+
+	//priv->CurrentCckTxPwrIdx = cck_power_level;
+	//priv->CurrentOfdmTxPwrIdx = ofdm_power_level;
+#endif
+
+	max_cck_power_level = 15;
+	max_ofdm_power_level = 25; //  12 -> 25
+	min_ofdm_power_level = 10;
+
+#ifdef CONFIG_RTL8185B
+#ifdef CONFIG_RTL818X_S
+
+	if(cck_power_level > 35)
+	{
+		cck_power_level = 35;
+	}
+	//
+	// Set up CCK TXAGC. suggested by SD3 SY.
+	//
+       write_nic_byte(dev, CCK_TXAGC, (ZEBRA2_CCK_OFDM_GAIN_SETTING[(u8)cck_power_level]) );
+       //printk("CCK TX power is %x\n", (ZEBRA2_CCK_OFDM_GAIN_SETTING[cck_power_level]));
+       force_pci_posting(dev);
+	mdelay(1);
+#else
+
+	/* CCK power setting */
+	if(cck_power_level > max_cck_power_level)
+		cck_power_level = max_cck_power_level;
+
+	cck_power_level += priv->cck_txpwr_base;
+
+	if(cck_power_level > 35)
+		cck_power_level = 35;
+
+	if(ch == 14)
+		cck_power_table = rtl8225z2_tx_power_cck_ch14;
+	else
+		cck_power_table = rtl8225z2_tx_power_cck;
+
+
+	for(i=0;i<8;i++){
+
+		power = cck_power_table[i];
+		write_phy_cck(dev, 0x44 + i, power);
+	}
+
+	//write_nic_byte(dev, TX_GAIN_CCK, power);
+	//2005.11.17,
+	write_nic_byte(dev, CCK_TXAGC, ZEBRA2_CCK_OFDM_GAIN_SETTING[(u8)cck_power_level]);
+
+	force_pci_posting(dev);
+	mdelay(1);
+#endif
+#endif
+	/* OFDM power setting */
+//  Old:
+//	if(ofdm_power_level > max_ofdm_power_level)
+//		ofdm_power_level = 35;
+//	ofdm_power_level += min_ofdm_power_level;
+//  Latest:
+/*	if(ofdm_power_level > (max_ofdm_power_level - min_ofdm_power_level))
+		ofdm_power_level = max_ofdm_power_level;
+	else
+		ofdm_power_level += min_ofdm_power_level;
+
+	ofdm_power_level += priv->ofdm_txpwr_base;
+*/
+	if(ofdm_power_level > 35)
+		ofdm_power_level = 35;
+
+//	rtl8185_set_anaparam2(dev,RTL8225_ANAPARAM2_ON);
+
+	//rtl8185_set_anaparam2(dev, ANAPARM2_ASIC_ON);
+
+	if (priv->up == 0) {
+		//must add these for rtl8185B down, xiong-2006-11-21
+		write_phy_ofdm(dev,2,0x42);
+		write_phy_ofdm(dev,5,0);
+		write_phy_ofdm(dev,6,0x40);
+		write_phy_ofdm(dev,7,0);
+		write_phy_ofdm(dev,8,0x40);
+	}
+
+	//write_nic_byte(dev, TX_GAIN_OFDM, ofdm_power_level);
+	//2005.11.17,
+#ifdef CONFIG_RTL818X_S
+        write_nic_byte(dev, OFDM_TXAGC, ZEBRA2_CCK_OFDM_GAIN_SETTING[(u8)ofdm_power_level]);
+#else
+        write_nic_byte(dev, OFDM_TXAGC, ZEBRA2_CCK_OFDM_GAIN_SETTING[(u8)ofdm_power_level]*2);
+#endif
+        if(ofdm_power_level<=11)
+        {
+//            write_nic_dword(dev,PHY_ADR,0x00005c87);
+//            write_nic_dword(dev,PHY_ADR,0x00005c89);
+		write_phy_ofdm(dev,0x07,0x5c);
+		write_phy_ofdm(dev,0x09,0x5c);
+        }
+	if(ofdm_power_level<=17)
+        {
+//             write_nic_dword(dev,PHY_ADR,0x00005487);
+//             write_nic_dword(dev,PHY_ADR,0x00005489);
+		write_phy_ofdm(dev,0x07,0x54);
+		write_phy_ofdm(dev,0x09,0x54);
+        }
+        else
+        {
+//             write_nic_dword(dev,PHY_ADR,0x00005087);
+//             write_nic_dword(dev,PHY_ADR,0x00005089);
+		write_phy_ofdm(dev,0x07,0x50);
+		write_phy_ofdm(dev,0x09,0x50);
+        }
+	force_pci_posting(dev);
+	mdelay(1);
+
+}
+#if 0
+/* switch between mode B and G */
+void rtl8225_set_mode(struct net_device *dev, short modeb)
+{
+	write_phy_ofdm(dev, 0x15, (modeb ? 0x0 : 0x40));
+	write_phy_ofdm(dev, 0x17, (modeb ? 0x0 : 0x40));
+}
+#endif
+
+void rtl8225z2_rf_set_chan(struct net_device *dev, short ch)
+{
+/*
+	short gset = (priv->ieee80211->state == IEEE80211_LINKED &&
+		ieee80211_is_54g(priv->ieee80211->current_network)) ||
+		priv->ieee80211->iw_mode == IW_MODE_MONITOR;
+*/
+	rtl8225z2_SetTXPowerLevel(dev, ch);
+
+	RF_WriteReg(dev, 0x7, rtl8225_chan[ch]);
+
+	//YJ,add,080828, if set channel failed, write again
+	if((RF_ReadReg(dev, 0x7) & 0x0F80) != rtl8225_chan[ch])
+	{
+		RF_WriteReg(dev, 0x7, rtl8225_chan[ch]);
+	}
+
+	mdelay(1);
+
+	force_pci_posting(dev);
+	mdelay(10);
+//deleted by David : 2006/8/9
+#if 0
+	write_nic_byte(dev,SIFS,0x22);// SIFS: 0x22
+
+	if(gset)
+		write_nic_byte(dev,DIFS,20); //DIFS: 20
+	else
+		write_nic_byte(dev,DIFS,0x24); //DIFS: 36
+
+	if(priv->ieee80211->state == IEEE80211_LINKED &&
+		ieee80211_is_shortslot(priv->ieee80211->current_network))
+		write_nic_byte(dev,SLOT,0x9); //SLOT: 9
+
+	else
+		write_nic_byte(dev,SLOT,0x14); //SLOT: 20 (0x14)
+
+
+	if(gset){
+		write_nic_byte(dev,EIFS,91 - 20); // EIFS: 91 (0x5B)
+		write_nic_byte(dev,CW_VAL,0x73); //CW VALUE: 0x37
+		//DMESG("using G net params");
+	}else{
+		write_nic_byte(dev,EIFS,91 - 0x24); // EIFS: 91 (0x5B)
+		write_nic_byte(dev,CW_VAL,0xa5); //CW VALUE: 0x37
+		//DMESG("using B net params");
+	}
+#endif
+
+}
+#if 0
+void rtl8225_host_pci_init(struct net_device *dev)
+{
+	write_nic_word(dev, RFPinsOutput, 0x480);
+
+	rtl8185_rf_pins_enable(dev);
+
+	//if(priv->card_8185 == 2 && priv->enable_gpio0 ) /* version D */
+	//write_nic_word(dev, RFPinsSelect, 0x88);
+	//else
+	write_nic_word(dev, RFPinsSelect, 0x88 | SW_CONTROL_GPIO); /* 0x488 | SW_CONTROL_GPIO */
+
+	write_nic_byte(dev, GP_ENABLE, 0);
+
+	force_pci_posting(dev);
+	mdelay(200);
+
+	write_nic_word(dev, GP_ENABLE, 0xff & (~(1<<6))); /* bit 6 is for RF on/off detection */
+
+
+}
+
+void rtl8225_host_usb_init(struct net_device *dev)
+{
+	write_nic_byte(dev,RFPinsSelect+1,0);
+
+	write_nic_byte(dev,GPIO,0);
+
+	write_nic_byte_E(dev,0x53,read_nic_byte_E(dev,0x53) | (1<<7));
+
+	write_nic_byte(dev,RFPinsSelect+1,4);
+
+	write_nic_byte(dev,GPIO,0x20);
+
+	write_nic_byte(dev,GP_ENABLE,0);
+
+
+	/* Config BB & RF */
+	write_nic_word(dev, RFPinsOutput, 0x80);
+
+	write_nic_word(dev, RFPinsSelect, 0x80);
+
+	write_nic_word(dev, RFPinsEnable, 0x80);
+
+
+	mdelay(100);
+
+	mdelay(1000);
+
+}
+#endif
+void rtl8225z2_rf_init(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int i;
+	short channel = 1;
+	u16	brsr;
+	u32	data,addr;
+
+	priv->chan = channel;
+
+//	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_ON);
+
+
+	if(priv->card_type == USB)
+		rtl8225_host_usb_init(dev);
+	else
+		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);
+
+	#if 1  //0->1
+	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
+	write_nic_byte(dev, CONFIG3, 0x44);
+	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
+	#endif
+
+
+	rtl8185_rf_pins_enable(dev);
+
+//		mdelay(1000);
+
+	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, ((priv->card_type == USB)? 0x82a : 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);
+
+
+	//if(priv->card_type != USB) /* maybe not needed even for 8185 */
+//	write_rtl8225(dev, 0x7, rtl8225_chan[channel]);
+
+	write_rtl8225(dev, 0x0, 0x1b7);
+
+	for(i=0;i<95;i++){
+		write_rtl8225(dev, 0x1, (u8)(i+1));
+
+		#if 0
+		if(priv->phy_ver == 1)
+			/* version A */
+			write_rtl8225(dev, 0x2, rtl8225a_rxgain[i]);
+		else
+		#endif
+		/* version B & C & D*/
+
+		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);
+
+	if(priv->card_type == USB){
+	//	force_pci_posting(dev);
+		mdelay(200);
+
+		write_rtl8225(dev, 0x2, 0x44d);
+
+	//	force_pci_posting(dev);
+		mdelay(100);
+
+	}//End of if(priv->card_type == USB)
+	/* FIXME!! rtl8187 we have to check if calibrarion
+	 * is successful and eventually cal. again (repeat
+	 * the two write on reg 2)
+	*/
+	// Check for calibration status, 2005.11.17,
+        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");
+                        }
+        }
+	//force_pci_posting(dev);
+
+	mdelay(200); //200 for 8187
+
+
+//	//if(priv->card_type != USB){
+//		write_rtl8225(dev, 0x2, 0x44d);
+//		write_rtl8225(dev, 0x7, rtl8225_chan[channel]);
+//		write_rtl8225(dev, 0x2, 0x47d);
+//
+//		force_pci_posting(dev);
+//		mdelay(100);
+//
+//		write_rtl8225(dev, 0x2, 0x44d);
+//	//}
+
+	write_rtl8225(dev, 0x0, 0x2bf);
+
+	if(priv->card_type != USB)
+		rtl8185_rf_pins_enable(dev);
+	//set up ZEBRA AGC table, 2005.11.17,
+        for(i=0;i<128;i++){
+                data = rtl8225_agc[i];
+
+                addr = i + 0x80; //enable writing AGC table
+                write_phy_ofdm(dev, 0xb, data);
+
+                mdelay(1);
+                write_phy_ofdm(dev, 0xa, addr);
+
+                mdelay(1);
+        }
+#if 0
+	for(i=0;i<128;i++){
+		write_phy_ofdm(dev, 0xb, rtl8225_agc[i]);
+
+		mdelay(1);
+		write_phy_ofdm(dev, 0xa, (u8)i+ 0x80);
+
+		mdelay(1);
+	}
+#endif
+
+	force_pci_posting(dev);
+	mdelay(1);
+
+	write_phy_ofdm(dev, 0x0, 0x1); mdelay(1);
+	write_phy_ofdm(dev, 0x1, 0x2); mdelay(1);
+	write_phy_ofdm(dev, 0x2, ((priv->card_type == USB)? 0x42 : 0x62)); mdelay(1);
+	write_phy_ofdm(dev, 0x3, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x4, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x5, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x6, 0x40); mdelay(1);
+	write_phy_ofdm(dev, 0x7, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x8, 0x40); mdelay(1);
+	write_phy_ofdm(dev, 0x9, 0xfe); mdelay(1);
+
+	write_phy_ofdm(dev, 0xa, 0x8); mdelay(1);
+
+	//write_phy_ofdm(dev, 0x18, 0xef);
+	//	}
+	//}
+	write_phy_ofdm(dev, 0xb, 0x80); mdelay(1);
+
+	write_phy_ofdm(dev, 0xc, 0x1);mdelay(1);
+
+
+	//if(priv->card_type != USB)
+	write_phy_ofdm(dev, 0xd, 0x43);
+
+	write_phy_ofdm(dev, 0xe, 0xd3);mdelay(1);
+
+
+	#if 0
+	if(priv->card_8185 == 1){
+		if(priv->card_8185_Bversion)
+			write_phy_ofdm(dev, 0xf, 0x20);/*ver B*/
+		else
+			write_phy_ofdm(dev, 0xf, 0x28);/*ver C*/
+	}else{
+	#endif
+	write_phy_ofdm(dev, 0xf, 0x38);mdelay(1);
+/*ver D & 8187*/
+//	}
+
+//	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+//		write_phy_ofdm(dev, 0x10, 0x04);/*ver B*/
+//	else
+	write_phy_ofdm(dev, 0x10, 0x84);mdelay(1);
+/*ver C & D & 8187*/
+
+	write_phy_ofdm(dev, 0x11, 0x07);mdelay(1);
+/*agc resp time 700*/
+
+
+//	if(priv->card_8185 == 2){
+	/* Ver D & 8187*/
+	write_phy_ofdm(dev, 0x12, 0x20);mdelay(1);
+
+	write_phy_ofdm(dev, 0x13, 0x20);mdelay(1);
+
+#if 0
+	}else{
+		/* Ver B & C*/
+		write_phy_ofdm(dev, 0x12, 0x0);
+		write_phy_ofdm(dev, 0x13, 0x0);
+	}
+#endif
+	write_phy_ofdm(dev, 0x14, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x15, 0x40); mdelay(1);
+	write_phy_ofdm(dev, 0x16, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x17, 0x40); mdelay(1);
+
+//	if (priv->card_type == USB)
+//		write_phy_ofdm(dev, 0x18, 0xef);
+
+	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, 0x4);mdelay(1);
+
+	write_phy_ofdm(dev, 0x1d, 0xc5);mdelay(1); //2005.11.17,
+
+	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);
+
+//	if(priv->card_type != USB)
+	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);
+
+
+	// <> Set init. gain to m74dBm.
+
+	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);
+ /* Ver C & D & 8187*/
+
+	write_phy_cck(dev, 0x8, 0x2e);mdelay(1);
+
+	write_phy_cck(dev, 0x10, ((priv->card_type == USB) ? 0x9b: 0x93)); mdelay(1);
+	write_phy_cck(dev, 0x11, 0x88); mdelay(1);
+	write_phy_cck(dev, 0x12, 0x47); mdelay(1);
+#if 0
+	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+		write_phy_cck(dev, 0x13, 0x98); /* Ver B */
+	else
+#endif
+	write_phy_cck(dev, 0x13, 0xd0); /* Ver C & D & 8187*/
+
+	write_phy_cck(dev, 0x19, 0x0);
+	write_phy_cck(dev, 0x1a, 0xa0);
+	write_phy_cck(dev, 0x1b, 0x8);
+	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, 0x9); mdelay(1);
+	write_phy_cck(dev, 0x4b, 0x4); mdelay(1);
+	write_phy_cck(dev, 0x4c, 0x5);mdelay(1);
+
+
+	write_nic_byte(dev, 0x5b, 0x0d); mdelay(1);
+
+
+
+// <>
+//	// TESTR 0xb 8187
+//	write_phy_cck(dev, 0x10, 0x93);// & 0xfb);
+//
+//	//if(priv->card_type != USB){
+//		write_phy_ofdm(dev, 0x2, 0x62);
+//		write_phy_ofdm(dev, 0x6, 0x0);
+//		write_phy_ofdm(dev, 0x8, 0x0);
+//	//}
+
+	rtl8225z2_SetTXPowerLevel(dev, channel);
+#ifdef CONFIG_RTL818X_S
+        write_phy_cck(dev, 0x11, 0x9b); mdelay(1); /* Rx ant A, 0xdb for B */
+#else
+	write_phy_cck(dev, 0x10, 0x9b); mdelay(1); /* Rx ant A, 0xdb for B */
+#endif
+	write_phy_ofdm(dev, 0x26, 0x90); mdelay(1); /* Rx ant A, 0x10 for B */
+
+	rtl8185_tx_antenna(dev, 0x3); /* TX ant A, 0x0 for B */
+
+	/* switch to high-speed 3-wire
+	 * last digit. 2 for both cck and ofdm
+	 */
+	if(priv->card_type == USB)
+		write_nic_dword(dev, 0x94, 0x3dc00002);
+	else{
+		write_nic_dword(dev, 0x94, 0x15c00002);
+		rtl8185_rf_pins_enable(dev);
+	}
+
+//	if(priv->card_type != USB)
+//	rtl8225_set_gain(dev, 4); /* FIXME this '1' is random */ // <>
+//	 rtl8225_set_mode(dev, 1); /* FIXME start in B mode */ // <>
+//
+//	/* make sure is waken up! */
+//	write_rtl8225(dev,0x4, 0x9ff);
+//	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_ON);
+//	rtl8185_set_anaparam2(dev, RTL8225_ANAPARAM2_ON);
+
+	rtl8225_rf_set_chan(dev, priv->chan);
+
+	//write_nic_word(dev,BRSR,brsr);
+
+	//rtl8225z2_rf_set_mode(dev);
+}
+
+void rtl8225z2_rf_set_mode(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	if(priv->ieee80211->mode == IEEE_A)
+	{
+		write_rtl8225(dev, 0x5, 0x1865);
+		write_nic_dword(dev, RF_PARA, 0x10084);
+		write_nic_dword(dev, RF_TIMING, 0xa8008);
+		write_phy_ofdm(dev, 0x0, 0x0);
+		write_phy_ofdm(dev, 0xa, 0x6);
+		write_phy_ofdm(dev, 0xb, 0x99);
+		write_phy_ofdm(dev, 0xf, 0x20);
+		write_phy_ofdm(dev, 0x11, 0x7);
+
+		rtl8225z2_set_gain(dev,4);
+
+		write_phy_ofdm(dev,0x15, 0x40);
+		write_phy_ofdm(dev,0x17, 0x40);
+
+		write_nic_dword(dev, 0x94,0x10000000);
+	}else{
+
+		write_rtl8225(dev, 0x5, 0x1864);
+		write_nic_dword(dev, RF_PARA, 0x10044);
+		write_nic_dword(dev, RF_TIMING, 0xa8008);
+		write_phy_ofdm(dev, 0x0, 0x1);
+		write_phy_ofdm(dev, 0xa, 0x6);
+		write_phy_ofdm(dev, 0xb, 0x99);
+		write_phy_ofdm(dev, 0xf, 0x20);
+		write_phy_ofdm(dev, 0x11, 0x7);
+
+		rtl8225z2_set_gain(dev,4);
+
+		write_phy_ofdm(dev,0x15, 0x40);
+		write_phy_ofdm(dev,0x17, 0x40);
+
+		write_nic_dword(dev, 0x94,0x04000002);
+	}
+}
+
+//lzm mod 080826
+//#define MAX_DOZE_WAITING_TIMES_85B 64
+//#define MAX_POLLING_24F_TIMES_87SE 	5
+#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,
+	RT_RF_POWER_STATE	eRFPowerState
+	)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u8			btCR9346, btConfig3;
+	bool bActionAllowed= true, bTurnOffBB = true;//lzm mod 080826
+	//u32			DWordContent;
+	u8			u1bTmp;
+	int			i;
+	//u16			u2bTFPC = 0;
+	bool		bResult = true;
+	u8			QueueID;
+
+	if(priv->SetRFPowerStateInProgress == true)
+		return false;
+
+	priv->SetRFPowerStateInProgress = true;
+
+	// enable EEM0 and EEM1 in 9346CR
+	btCR9346 = read_nic_byte(dev, CR9346);
+	write_nic_byte(dev, CR9346, (btCR9346|0xC0) );
+	// enable PARM_En in Config3
+	btConfig3 = read_nic_byte(dev, CONFIG3);
+	write_nic_byte(dev, CONFIG3, (btConfig3|CONFIG3_PARM_En) );
+
+	switch( priv->rf_chip )
+	{
+	case RF_ZEBRA2:
+		switch( eRFPowerState )
+		{
+		case eRfOn:
+			RF_WriteReg(dev,0x4,0x9FF);
+
+			write_nic_dword(dev, ANAPARAM, ANAPARM_ON);
+			write_nic_dword(dev, ANAPARAM2, ANAPARM2_ON);
+
+			write_nic_byte(dev, CONFIG4, priv->RFProgType);
+
+			//Follow 87B, Isaiah 2007-04-27
+			u1bTmp = read_nic_byte(dev, 0x24E);
+			write_nic_byte(dev, 0x24E, (u1bTmp & (~(BIT5|BIT6))) );// 070124 SD1 Alex: turn on CCK and OFDM.
+			break;
+
+		case eRfSleep:
+			break;
+
+		case eRfOff:
+			break;
+
+		default:
+			bResult = false;
+			break;
+		}
+		break;
+
+	case RF_ZEBRA4:
+		switch( eRFPowerState )
+		{
+		case eRfOn:
+			//printk("===================================power on@jiffies:%d\n",jiffies);
+			write_nic_word(dev, 0x37C, 0x00EC);
+
+			//turn on AFE
+			write_nic_byte(dev, 0x54, 0x00);
+			write_nic_byte(dev, 0x62, 0x00);
+
+			//lzm mod 080826
+			//turn on RF
+			//RF_WriteReg(dev, 0x0, 0x009f); //mdelay(1);
+			//RF_WriteReg(dev, 0x4, 0x0972); //mdelay(1);
+			RF_WriteReg(dev, 0x0, 0x009f); udelay(500);
+			RF_WriteReg(dev, 0x4, 0x0972); udelay(500);
+			//turn on RF again, suggested by SD3 stevenl.
+			RF_WriteReg(dev, 0x0, 0x009f); udelay(500);
+			RF_WriteReg(dev, 0x4, 0x0972); udelay(500);
+
+			//turn on BB
+//			write_nic_dword(dev, PhyAddr, 0x4090); //ofdm 10=00
+//			write_nic_dword(dev, PhyAddr, 0x4092); //ofdm 12=00
+			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 eRfSleep:
+			// Make sure BusyQueue is empty befor turn off RFE pwoer.
+			//printk("===================================power sleep@jiffies:%d\n",jiffies);
+
+			for(QueueID = 0, i = 0; QueueID < 6; )
+			{
+				if(get_curr_tx_free_desc(dev,QueueID) == priv->txringcount)
+				{
+					QueueID++;
+					continue;
+				}
+#if 0		//reserved amy
+				else if(priv->NdisAdapter.CurrentPowerState != NdisDeviceStateD0)
+				{
+					RT_TRACE(COMP_POWER, DBG_LOUD, ("eRfSleep: %d times TcbBusyQueue[%d] !=0 but lower power state!\n", (pMgntInfo->TxPollingTimes+1), QueueID));
+					break;
+				}
+#endif
+				else//lzm mod 080826
+				{
+					priv->TxPollingTimes ++;
+					if(priv->TxPollingTimes >= LPS_MAX_SLEEP_WAITING_TIMES_87SE)
+						{
+							//RT_TRACE(COMP_POWER, DBG_WARNING, ("\n\n\n SetZebraRFPowerState8185B():eRfSleep:  %d times TcbBusyQueue[%d] != 0 !!!\n\n\n", LPS_MAX_SLEEP_WAITING_TIMES_87SE, QueueID));
+							bActionAllowed=false;
+							break;
+						}
+						else
+						{
+							udelay(10);  // Windows may delay 3~16ms actually.
+							//RT_TRACE(COMP_POWER, DBG_LOUD, ("eRfSleep: %d times TcbBusyQueue[%d] !=0 before doze!\n", (pMgntInfo->TxPollingTimes), QueueID));
+						}
+				}
+
+				//lzm del 080826
+				//if(i >= MAX_DOZE_WAITING_TIMES_85B)
+				//{
+					//printk("\n\n\n SetZebraRFPowerState8185B(): %d times BusyQueue[%d] != 0 !!!\n\n\n", MAX_DOZE_WAITING_TIMES_85B, QueueID);
+					//break;
+				//}
+			}
+
+			if(bActionAllowed)//lzm add 080826
+			{
+				//turn off BB RXIQ matrix to cut off rx signal
+//				write_nic_dword(dev, PhyAddr, 0x0090); //ofdm 10=00
+//				write_nic_dword(dev, PhyAddr, 0x0092); //ofdm 12=00
+				write_phy_ofdm(dev,0x10,0x00);
+				write_phy_ofdm(dev,0x12,0x00);
+				//turn off RF
+				RF_WriteReg(dev, 0x4, 0x0000); //mdelay(1);
+				RF_WriteReg(dev, 0x0, 0x0000); //mdelay(1);
+				//turn off AFE except PLL
+				write_nic_byte(dev, 0x62, 0xff);
+				write_nic_byte(dev, 0x54, 0xec);
+//				mdelay(10);
+
+#if 1
+				mdelay(1);
+				{
+					int i = 0;
+					while (true)
+					{
+						u8 tmp24F = read_nic_byte(dev, 0x24f);
+						if ((tmp24F == 0x01) || (tmp24F == 0x09))
+						{
+							bTurnOffBB = true;
+							break;
+						}
+						else//lzm mod 080826
+						{
+							udelay(10);
+							i++;
+							priv->TxPollingTimes++;
+
+							if(priv->TxPollingTimes >= LPS_MAX_SLEEP_WAITING_TIMES_87SE)
+							{
+								//RT_TRACE(COMP_POWER, DBG_WARNING, ("\n\n\n SetZebraRFPowerState8185B(): eRfOff: %d times Rx Mac0x24F=0x%x !!!\n\n\n", i, u1bTmp24F));
+								bTurnOffBB=false;
+								break;
+							}
+							else
+							{
+								udelay(10);// Windows may delay 3~16ms actually.
+								//RT_TRACE(COMP_POWER, DBG_LOUD,("(%d)eRfSleep- u1bTmp24F= 0x%X\n", i, u1bTmp24F));
+
+							}
+						}
+
+						//lzm del 080826
+						//if (i > MAX_POLLING_24F_TIMES_87SE)
+						//	break;
+					}
+				}
+#endif
+				if (bTurnOffBB)//lzm mod 080826
+				{
+				//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, 0xec);
+				//write_nic_word(dev, 0x37C, 0x00ec);
+				write_nic_byte(dev, 0x54, 0xFC);  //[ECS] FC-> EC->FC, asked by SD3 Stevenl
+				write_nic_word(dev, 0x37C, 0x00FC);//[ECS] FC-> EC->FC, asked by SD3 Stevenl
+				}
+			}
+			break;
+
+		case eRfOff:
+			// Make sure BusyQueue is empty befor turn off RFE pwoer.
+			//printk("===================================power off@jiffies:%d\n",jiffies);
+			for(QueueID = 0, i = 0; QueueID < 6; )
+			{
+				if(get_curr_tx_free_desc(dev,QueueID) == priv->txringcount)
+				{
+					QueueID++;
+					continue;
+				}
+#if 0
+				else if(Adapter->NdisAdapter.CurrentPowerState != NdisDeviceStateD0)
+				{
+					RT_TRACE(COMP_POWER, DBG_LOUD, ("%d times TcbBusyQueue[%d] !=0 but lower power state!\n", (i+1), QueueID));
+					break;
+				}
+#endif
+				else
+				{
+					udelay(10);
+					i++;
+				}
+
+				if(i >= MAX_DOZE_WAITING_TIMES_85B)
+				{
+					//printk("\n\n\n SetZebraRFPowerState8185B(): %d times BusyQueue[%d] != 0 !!!\n\n\n", MAX_DOZE_WAITING_TIMES_85B, QueueID);
+					break;
+				}
+			}
+
+			//turn off BB RXIQ matrix to cut off rx signal
+//			write_nic_dword(dev, PhyAddr, 0x0090); //ofdm 10=00
+//			write_nic_dword(dev, PhyAddr, 0x0092); //ofdm 12=00
+			write_phy_ofdm(dev,0x10,0x00);
+			write_phy_ofdm(dev,0x12,0x00);
+			//turn off RF
+			RF_WriteReg(dev, 0x4, 0x0000); //mdelay(1);
+			RF_WriteReg(dev, 0x0, 0x0000); //mdelay(1);
+			//turn off AFE except PLL
+			write_nic_byte(dev, 0x62, 0xff);
+			write_nic_byte(dev, 0x54, 0xec);
+//			mdelay(10);
+#if 1
+			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;
+				}
+			}
+#endif
+			if (bTurnOffBB)//lzm mod 080826
+			{
+
+			//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, 0xec);
+			//write_nic_word(dev, 0x37C, 0x00ec);
+			write_nic_byte(dev, 0x54, 0xFC); //[ECS] FC-> EC->FC, asked by SD3 Stevenl
+			write_nic_word(dev, 0x37C, 0x00FC); //[ECS] FC-> EC->FC, asked by SD3 Stevenl
+			}
+
+			break;
+
+		default:
+			bResult = false;
+			printk("SetZebraRFPowerState8185(): unknow state to set: 0x%X!!!\n", eRFPowerState);
+			break;
+		}
+		break;
+	}
+
+	// disable PARM_En in Config3
+	btConfig3 &= ~(CONFIG3_PARM_En);
+	write_nic_byte(dev, CONFIG3, btConfig3);
+	// disable EEM0 and EEM1 in 9346CR
+	btCR9346 &= ~(0xC0);
+	write_nic_byte(dev, CR9346, btCR9346);
+
+	if(bResult && bActionAllowed)//lzm mod 080826
+	{
+		// Update current RF state variable.
+		priv->eRFPowerState = eRFPowerState;
+#if 0
+		switch(priv->eRFPowerState)
+		{
+		case eRfOff:
+			//
+			//If Rf off reason is from IPS, Led should blink with no link, by Maddest 071015
+			//
+			if(priv->RfOffReason==RF_CHANGE_BY_IPS )
+			{
+				Adapter->HalFunc.LedControlHandler(Adapter,LED_CTL_NO_LINK);
+			}
+			else
+			{
+				// Turn off LED if RF is not ON.
+				Adapter->HalFunc.LedControlHandler(Adapter, LED_CTL_POWER_OFF);
+			}
+			break;
+
+		case eRfOn:
+			// Turn on RF we are still linked, which might happen when
+			// we quickly turn off and on HW RF. 2006.05.12, by rcnjko.
+			if( pMgntInfo->bMediaConnect == TRUE )
+			{
+				Adapter->HalFunc.LedControlHandler(Adapter, LED_CTL_LINK);
+			}
+			break;
+
+		default:
+			// do nothing.
+			break;
+		}
+#endif
+
+	}
+
+	priv->SetRFPowerStateInProgress = false;
+
+	return (bResult && bActionAllowed) ;
+}
+void rtl8225z4_rf_sleep(struct net_device *dev)
+{
+	//
+	// Turn off RF power.
+	//
+	//printk("=========>%s()\n", __func__);
+	MgntActSet_RF_State(dev, eRfSleep, RF_CHANGE_BY_PS);
+	//mdelay(2);	//FIXME
+}
+void rtl8225z4_rf_wakeup(struct net_device *dev)
+{
+	//
+	// Turn on RF power.
+	//
+	//printk("=========>%s()\n", __func__);
+	MgntActSet_RF_State(dev, eRfOn, RF_CHANGE_BY_PS);
+}
+#endif
+
diff --git a/drivers/staging/rtl8187se/r8180_rtl8255.c b/drivers/staging/rtl8187se/r8180_rtl8255.c
new file mode 100644
index 000000000000..1a62444dcc50
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_rtl8255.c
@@ -0,0 +1,1838 @@
+/*
+  This is part of the rtl8180-sa2400 driver
+  released under the GPL (See file COPYING for details).
+  Copyright (c) 2005 Andrea Merello <andreamrl@tiscali.it>
+
+  This files contains programming code for the rtl8255
+  radio frontend.
+
+  *Many* thanks to Realtek Corp. for their great support!
+
+*/
+
+#define BAND_A 1
+#define BAND_BG 2
+
+#include "r8180.h"
+#include "r8180_hw.h"
+#include "r8180_rtl8255.h"
+
+u32 rtl8255_chan[] = {
+	0,	//dummy channel 0
+	0x13, //1
+	0x115, //2
+	0x217, //3
+	0x219, //4
+	0x31b, //5
+	0x41d, //6
+	0x41f, //7
+	0x621, //8
+	0x623, //9
+	0x625, //10
+	0x627, //11
+	0x829, //12
+	0x82b, //13
+	0x92f, // 14
+};
+
+static short rtl8255_gain_2G[]={
+	0x33, 0x17, 0x7c, 0xc5,//-78
+	0x43, 0x17, 0x7a, 0xc5,//-74
+	0x53, 0x17, 0x78, 0xc5,//-70
+	0x63, 0x17, 0x76, 0xc5,//-66
+};
+
+
+static short rtl8255_agc[]={
+	0x1, 0x1, 0x1, 0x1, 0x1,         0x1, 0x1, 0x1, 0x1, 0x1,
+
+	0x1, 0x1, 0x2, 0x2, 0x3,         0x3, 0x4, 0x4, 0x5, 0x5,
+	0x6, 0x6, 0x7, 0x7, 0x8,         0x8, 0x9, 0x9, 0xa, 0xa,
+	0xb, 0xb, 0xc, 0xc, 0xd,         0xd, 0xe, 0xe, 0xf, 0xf,
+
+	0x10, 0x10, 0x11, 0x11, 0x12,    0x12, 0x13, 0x13, 0x14, 0x14,
+	0x15, 0x15, 0x16, 0x16, 0x17,    0x17, 0x18, 0x18, 0x19, 0x19,
+	0x1a, 0x1a, 0x1b, 0x1b, 0x1c,    0x1c, 0x1d, 0x1d, 0x1e, 0x1e,
+	0x1f, 0x1f,
+
+	0x20, 0x20, 0x21, 0x21, 0x22,    0x22, 0x23, 0x23, 0x24, 0x24,
+	0x25, 0x25, 0x26, 0x26, 0x27,    0x27, 0x28, 0x28, 0x29, 0x29,
+	0x2a, 0x2a,
+
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a,    0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a,    0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a,    0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a
+
+};
+
+void rtl8255_set_gain(struct net_device *dev, short gain)
+{
+
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	write_phy_ofdm(dev, 0x0d, rtl8255_gain_2G[gain * 4]);
+	write_phy_ofdm(dev, 0x23, rtl8255_gain_2G[gain * 4 + 1]);
+	write_phy_ofdm(dev, 0x1b, rtl8255_gain_2G[gain * 4 + 2]);
+	write_phy_ofdm(dev, 0x1d, rtl8255_gain_2G[gain * 4 + 3]);
+	//rtl8225_set_gain_usb(dev, gain);
+}
+
+void write_rtl8255_reg0c(struct net_device *dev, u32 d1, u32 d2, u32 d3, u32 d4,
+u32 d5, u32 d6, u32 d7, u32 d8, u32 d9, u32 d10)
+{
+	int i,j;
+	u16 out,select;
+	u8 bit;
+	u32 bangdata;
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	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);
+
+	out = read_nic_word(dev, RFPinsOutput) & 0xfff3;
+
+	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN );//| 0x1fff);
+
+	force_pci_posting(dev);
+	udelay(2);
+
+	write_nic_word(dev, RFPinsOutput, out);
+
+	force_pci_posting(dev);
+	udelay(2);
+
+	for(j=0;j<10;j++)
+	{
+		switch(j)
+		{
+			case 9:
+			bangdata = d10 | 0x0c;
+			break;
+			case 8:
+			bangdata = d9;
+			break;
+			case 7:
+			bangdata = d8;
+			break;
+			case 6:
+			bangdata = d7;
+			break;
+			case 5:
+			bangdata = d6;
+			break;
+			case 4:
+			bangdata = d5;
+			break;
+			case 3:
+			bangdata = d4;
+			break;
+			case 2:
+			bangdata = d3;
+			break;
+			case 1:
+			bangdata = d2;
+			break;
+			case 0:
+			bangdata = d1;
+			break;
+			default:
+			bangdata=0xbadc0de; /* avoid gcc complaints */
+			break;
+		}
+
+		for(i=31; i>=0;i--){
+
+			bit = (bangdata & (1<<i)) >> i;
+
+			write_nic_word(dev, RFPinsOutput, bit | out);
+			force_pci_posting(dev);
+			udelay(1);
+			write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+			force_pci_posting(dev);
+			udelay(1);
+		//	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);
+			force_pci_posting(dev);
+			udelay(1);
+		//	write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+			write_nic_word(dev, RFPinsOutput, bit | out);
+			force_pci_posting(dev);
+			udelay(1);
+		}
+	}
+
+	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);
+
+}
+
+void write_rtl8255(struct net_device *dev, u8 adr, u16 data)
+{
+	int i;
+	u16 out,select;
+	u8 bit;
+	u32 bangdata = (data << 4) | (adr & 0xf);
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	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 );//| 0x1fff);
+
+	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);
+}
+
+void rtl8255_rf_close(struct net_device *dev)
+{
+
+//	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_OFF);
+//	rtl8185_set_anaparam2(dev, RTL8225_ANAPARAM2_OFF);
+}
+
+void rtl8255_SetTXPowerLevel(struct net_device *dev, short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	u8 cck_power_level = 0xff & priv->chtxpwr[ch];
+	u8 ofdm_power_level = 0xff & priv->chtxpwr_ofdm[ch];
+	write_nic_byte(dev, TX_GAIN_OFDM, ofdm_power_level);
+	write_nic_byte(dev, TX_GAIN_CCK, cck_power_level);
+	force_pci_posting(dev);
+	mdelay(1);
+	//write_nic_byte(dev, TX_AGC_CONTROL,4);
+}
+#if 0
+/* switch between mode B and G */
+void rtl8255_set_mode(struct net_device *dev, short modeb)
+{
+	write_phy_ofdm(dev, 0x15, (modeb ? 0x0 : 0x40));
+	write_phy_ofdm(dev, 0x17, (modeb ? 0x0 : 0x40));
+}
+#endif
+
+void rtl8255_rf_set_chan(struct net_device *dev, short ch)
+{
+	//write_rtl8225(dev, 0x7, rtl8225_chan[1]);
+	write_rtl8255(dev, 0x5, 0x65);
+	write_rtl8255(dev, 0x6, rtl8255_chan[ch]);
+	write_rtl8255(dev, 0x7, 0x7c);
+	write_rtl8255(dev, 0x8, 0x6);
+
+
+	force_pci_posting(dev);
+	set_current_state(TASK_INTERRUPTIBLE);
+	schedule_timeout(HZ);
+//	rtl8225_set_mode_B(dev);
+
+	rtl8255_SetTXPowerLevel(dev, ch);
+	/* FIXME FIXME FIXME */
+
+	#if 0
+	write_nic_byte(dev,DIFS,0xe); //DIFS
+	write_nic_byte(dev,SLOT,0x14); //SLOT
+	write_nic_byte(dev,EIFS,0x5b); // EIFS
+	//write_nic_byte(dev,0xbc,0); //CW CONFIG
+	write_nic_byte(dev,0xbd,0xa4); //CW VALUE
+	//write_nic_byte(dev,TX_AGC_CONTROL,4);
+	//write_nic_byte(dev, 0x9d,7);
+//Apr 20 13:25:03 localhost kernel: w8. 409d<-7  // CCK AGC
+	/*write_nic_word(dev,0x84,0x488);
+	write_nic_byte(dev,0x91,0x3e);
+	write_nic_byte(dev,0x90,0x30);
+	write_nic_word(dev,0x84,0x488);
+	write_nic_byte(dev,0x91,0x3e);
+	write_nic_byte(dev,0x90,0x20);
+	*/
+	//mdelay(100);
+	#endif
+}
+
+void rtl8255_init_BGband(struct net_device *dev)
+{
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804187cf, 0x40000027,
+		0x92402ac0, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc00);
+	write_rtl8255(dev, 0x4, 0xe00);
+	write_rtl8255(dev, 0x4, 0xc00);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x800);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa00);
+	write_rtl8255(dev, 0x4, 0x800);
+	write_rtl8255(dev, 0x4, 0x400);
+	write_rtl8255(dev, 0x3, 0x26);
+	write_rtl8255(dev, 0x2, 0x27);
+	write_rtl8255(dev, 0x4, 0x600);
+	write_rtl8255(dev, 0x4, 0x400);
+	write_rtl8255(dev, 0x4, 0x400);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x600);
+	write_rtl8255(dev, 0x4, 0x400);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804187ce, 0x80000027,
+		0x92402ac0, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc01);
+	write_rtl8255(dev, 0x4, 0xe01);
+	write_rtl8255(dev, 0x4, 0xc01);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x801);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa01);
+	write_rtl8255(dev, 0x4, 0x801);
+	write_rtl8255(dev, 0x4, 0x401);
+	write_rtl8255(dev, 0x3, 0x26);
+	write_rtl8255(dev, 0x2, 0x27);
+	write_rtl8255(dev, 0x4, 0x601);
+	write_rtl8255(dev, 0x4, 0x401);
+	write_rtl8255(dev, 0x4, 0x401);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x601);
+	write_rtl8255(dev, 0x4, 0x401);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80418bdf, 0x40000027,
+		0x92402ac4, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc02);
+	write_rtl8255(dev, 0x4, 0xe02);
+	write_rtl8255(dev, 0x4, 0xc02);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x802);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa02);
+	write_rtl8255(dev, 0x4, 0x802);
+	write_rtl8255(dev, 0x4, 0x402);
+	write_rtl8255(dev, 0x3, 0x26);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x602);
+	write_rtl8255(dev, 0x4, 0x402);
+	write_rtl8255(dev, 0x4, 0x402);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x602);
+	write_rtl8255(dev, 0x4, 0x402);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80418bbf, 0x40000027,
+		0x92402ac4, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc03);
+	write_rtl8255(dev, 0x4, 0xe03);
+	write_rtl8255(dev, 0x4, 0xc03);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x803);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa03);
+	write_rtl8255(dev, 0x4, 0x803);
+	write_rtl8255(dev, 0x4, 0x403);
+	write_rtl8255(dev, 0x3, 0x26);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x603);
+	write_rtl8255(dev, 0x4, 0x403);
+	write_rtl8255(dev, 0x4, 0x403);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x603);
+	write_rtl8255(dev, 0x4, 0x403);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80418b9f, 0x40000027,
+		0x92402ac8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc04);
+	write_rtl8255(dev, 0x4, 0xe04);
+	write_rtl8255(dev, 0x4, 0xc04);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x804);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa04);
+	write_rtl8255(dev, 0x4, 0x804);
+	write_rtl8255(dev, 0x4, 0x404);
+	write_rtl8255(dev, 0x3, 0x26);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x604);
+	write_rtl8255(dev, 0x4, 0x404);
+	write_rtl8255(dev, 0x4, 0x404);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x604);
+	write_rtl8255(dev, 0x4, 0x404);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804183df, 0x40000027,
+		0x92402ac8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc05);
+	write_rtl8255(dev, 0x4, 0xe05);
+	write_rtl8255(dev, 0x4, 0xc05);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x805);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa05);
+	write_rtl8255(dev, 0x4, 0x805);
+	write_rtl8255(dev, 0x4, 0x405);
+	write_rtl8255(dev, 0x3, 0x26);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x605);
+	write_rtl8255(dev, 0x4, 0x405);
+	write_rtl8255(dev, 0x4, 0x405);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x605);
+	write_rtl8255(dev, 0x4, 0x405);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804183cf, 0x27,
+		0x92402acc, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc06);
+	write_rtl8255(dev, 0x4, 0xe06);
+	write_rtl8255(dev, 0x4, 0xc06);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x806);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa06);
+	write_rtl8255(dev, 0x4, 0x806);
+	write_rtl8255(dev, 0x4, 0x406);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x606);
+	write_rtl8255(dev, 0x4, 0x406);
+	write_rtl8255(dev, 0x4, 0x406);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x606);
+	write_rtl8255(dev, 0x4, 0x406);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804183af, 0x27,
+		0x92402acc, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc07);
+	write_rtl8255(dev, 0x4, 0xe07);
+	write_rtl8255(dev, 0x4, 0xc07);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x807);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa07);
+	write_rtl8255(dev, 0x4, 0x807);
+	write_rtl8255(dev, 0x4, 0x407);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x607);
+	write_rtl8255(dev, 0x4, 0x407);
+	write_rtl8255(dev, 0x4, 0x407);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x607);
+	write_rtl8255(dev, 0x4, 0x407);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804083d7, 0x40000027,
+		0x92402ad0, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc08);
+	write_rtl8255(dev, 0x4, 0xe08);
+	write_rtl8255(dev, 0x4, 0xc08);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x808);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa08);
+	write_rtl8255(dev, 0x4, 0x808);
+	write_rtl8255(dev, 0x4, 0x408);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x608);
+	write_rtl8255(dev, 0x4, 0x408);
+	write_rtl8255(dev, 0x4, 0x408);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x608);
+	write_rtl8255(dev, 0x4, 0x408);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804083c7, 0x27,
+		0x92402ad0, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc09);
+	write_rtl8255(dev, 0x4, 0xe09);
+	write_rtl8255(dev, 0x4, 0xc09);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x809);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa09);
+	write_rtl8255(dev, 0x4, 0x809);
+	write_rtl8255(dev, 0x4, 0x409);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x609);
+	write_rtl8255(dev, 0x4, 0x409);
+	write_rtl8255(dev, 0x4, 0x409);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x609);
+	write_rtl8255(dev, 0x4, 0x409);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804043d7, 0x40000027,
+		0x92402ad4, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc0a);
+	write_rtl8255(dev, 0x4, 0xe0a);
+	write_rtl8255(dev, 0x4, 0xc0a);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x80a);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa0a);
+	write_rtl8255(dev, 0x4, 0x80a);
+	write_rtl8255(dev, 0x4, 0x40a);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x60a);
+	write_rtl8255(dev, 0x4, 0x40a);
+	write_rtl8255(dev, 0x4, 0x40a);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x60a);
+	write_rtl8255(dev, 0x4, 0x40a);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804043d7, 0x40000027,
+		0x92402ad4, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc0b);
+	write_rtl8255(dev, 0x4, 0xe0b);
+	write_rtl8255(dev, 0x4, 0xc0b);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x80b);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa0b);
+	write_rtl8255(dev, 0x4, 0x80b);
+	write_rtl8255(dev, 0x4, 0x40b);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x60b);
+	write_rtl8255(dev, 0x4, 0x40b);
+	write_rtl8255(dev, 0x4, 0x40b);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x60b);
+	write_rtl8255(dev, 0x4, 0x40b);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804043c7, 0x27,
+		0x92402ad8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc0c);
+	write_rtl8255(dev, 0x4, 0xe0c);
+	write_rtl8255(dev, 0x4, 0xc0c);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x80c);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa0c);
+	write_rtl8255(dev, 0x4, 0x80c);
+	write_rtl8255(dev, 0x4, 0x40c);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x60c);
+	write_rtl8255(dev, 0x4, 0x40c);
+	write_rtl8255(dev, 0x4, 0x40c);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x60c);
+	write_rtl8255(dev, 0x4, 0x40c);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804043a7, 0x27,
+		0x92402ad8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc0d);
+	write_rtl8255(dev, 0x4, 0xe0d);
+	write_rtl8255(dev, 0x4, 0xc0d);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x80d);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa0d);
+	write_rtl8255(dev, 0x4, 0x80d);
+	write_rtl8255(dev, 0x4, 0x40d);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x60d);
+	write_rtl8255(dev, 0x4, 0x40d);
+	write_rtl8255(dev, 0x4, 0x40d);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x60d);
+	write_rtl8255(dev, 0x4, 0x40d);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404387, 0x27,
+		0x92402aa8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc0e);
+	write_rtl8255(dev, 0x4, 0xe0e);
+	write_rtl8255(dev, 0x4, 0xc0e);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x80e);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa0e);
+	write_rtl8255(dev, 0x4, 0x80e);
+	write_rtl8255(dev, 0x4, 0x40e);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x60e);
+	write_rtl8255(dev, 0x4, 0x40e);
+	write_rtl8255(dev, 0x4, 0x40e);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x60e);
+	write_rtl8255(dev, 0x4, 0x40e);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804041c7, 0x27,
+		0x92402aa8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc0f);
+	write_rtl8255(dev, 0x4, 0xe0f);
+	write_rtl8255(dev, 0x4, 0xc0f);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x80f);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa0f);
+	write_rtl8255(dev, 0x4, 0x80f);
+	write_rtl8255(dev, 0x4, 0x40f);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x60f);
+	write_rtl8255(dev, 0x4, 0x40f);
+	write_rtl8255(dev, 0x4, 0x40f);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x60f);
+	write_rtl8255(dev, 0x4, 0x40f);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804041a7, 0x27,
+		0x92402aac, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc10);
+	write_rtl8255(dev, 0x4, 0xe10);
+	write_rtl8255(dev, 0x4, 0xc10);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x810);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa10);
+	write_rtl8255(dev, 0x4, 0x810);
+	write_rtl8255(dev, 0x4, 0x410);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x610);
+	write_rtl8255(dev, 0x4, 0x410);
+	write_rtl8255(dev, 0x4, 0x410);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x610);
+	write_rtl8255(dev, 0x4, 0x410);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404187, 0x27,
+		0x92402aac, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc11);
+	write_rtl8255(dev, 0x4, 0xe11);
+	write_rtl8255(dev, 0x4, 0xc11);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x811);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa11);
+	write_rtl8255(dev, 0x4, 0x811);
+	write_rtl8255(dev, 0x4, 0x411);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x611);
+	write_rtl8255(dev, 0x4, 0x411);
+	write_rtl8255(dev, 0x4, 0x411);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x611);
+	write_rtl8255(dev, 0x4, 0x411);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404186, 0x80000027,
+		0x92402ab0, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc12);
+	write_rtl8255(dev, 0x4, 0xe12);
+	write_rtl8255(dev, 0x4, 0xc12);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x812);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa12);
+	write_rtl8255(dev, 0x4, 0x812);
+	write_rtl8255(dev, 0x4, 0x412);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x612);
+	write_rtl8255(dev, 0x4, 0x412);
+	write_rtl8255(dev, 0x4, 0x412);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x612);
+	write_rtl8255(dev, 0x4, 0x412);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404186, 0x27,
+		0x92402ab0, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc13);
+	write_rtl8255(dev, 0x4, 0xe13);
+	write_rtl8255(dev, 0x4, 0xc13);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x813);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa13);
+	write_rtl8255(dev, 0x4, 0x813);
+	write_rtl8255(dev, 0x4, 0x413);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x613);
+	write_rtl8255(dev, 0x4, 0x413);
+	write_rtl8255(dev, 0x4, 0x413);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x613);
+	write_rtl8255(dev, 0x4, 0x413);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404146, 0x27,
+		0x92402ab4, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc14);
+	write_rtl8255(dev, 0x4, 0xe14);
+	write_rtl8255(dev, 0x4, 0xc14);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x814);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa14);
+	write_rtl8255(dev, 0x4, 0x814);
+	write_rtl8255(dev, 0x4, 0x414);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x614);
+	write_rtl8255(dev, 0x4, 0x414);
+	write_rtl8255(dev, 0x4, 0x414);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x614);
+	write_rtl8255(dev, 0x4, 0x414);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404126, 0x27,
+		0x92402ab4, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc15);
+	write_rtl8255(dev, 0x4, 0xe15);
+	write_rtl8255(dev, 0x4, 0xc15);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x815);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa15);
+	write_rtl8255(dev, 0x4, 0x815);
+	write_rtl8255(dev, 0x4, 0x415);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x615);
+	write_rtl8255(dev, 0x4, 0x415);
+	write_rtl8255(dev, 0x4, 0x415);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x615);
+	write_rtl8255(dev, 0x4, 0x415);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404106, 0x27,
+		0x92402ab8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc16);
+	write_rtl8255(dev, 0x4, 0xe16);
+	write_rtl8255(dev, 0x4, 0xc16);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x816);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa16);
+	write_rtl8255(dev, 0x4, 0x816);
+	write_rtl8255(dev, 0x4, 0x416);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x616);
+	write_rtl8255(dev, 0x4, 0x416);
+	write_rtl8255(dev, 0x4, 0x416);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x616);
+	write_rtl8255(dev, 0x4, 0x416);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404105, 0x27,
+		0x92402ab8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc17);
+	write_rtl8255(dev, 0x4, 0xe17);
+	write_rtl8255(dev, 0x4, 0xc17);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x817);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa17);
+	write_rtl8255(dev, 0x4, 0x817);
+	write_rtl8255(dev, 0x4, 0x417);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x617);
+	write_rtl8255(dev, 0x4, 0x417);
+	write_rtl8255(dev, 0x4, 0x417);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x617);
+	write_rtl8255(dev, 0x4, 0x417);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404104, 0x80000027,
+		0x92402a88, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc18);
+	write_rtl8255(dev, 0x4, 0xe18);
+	write_rtl8255(dev, 0x4, 0xc18);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x818);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa18);
+	write_rtl8255(dev, 0x4, 0x818);
+	write_rtl8255(dev, 0x4, 0x418);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x618);
+	write_rtl8255(dev, 0x4, 0x418);
+	write_rtl8255(dev, 0x4, 0x418);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x618);
+	write_rtl8255(dev, 0x4, 0x418);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404104, 0x27,
+		0x92402a88, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc19);
+	write_rtl8255(dev, 0x4, 0xe19);
+	write_rtl8255(dev, 0x4, 0xc19);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x819);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa19);
+	write_rtl8255(dev, 0x4, 0x819);
+	write_rtl8255(dev, 0x4, 0x419);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x619);
+	write_rtl8255(dev, 0x4, 0x419);
+	write_rtl8255(dev, 0x4, 0x419);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x619);
+	write_rtl8255(dev, 0x4, 0x419);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404044, 0x27,
+		0x92402a8c, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc1a);
+	write_rtl8255(dev, 0x4, 0xe1a);
+	write_rtl8255(dev, 0x4, 0xc1a);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x81a);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa1a);
+	write_rtl8255(dev, 0x4, 0x81a);
+	write_rtl8255(dev, 0x4, 0x41a);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x61a);
+	write_rtl8255(dev, 0x4, 0x41a);
+	write_rtl8255(dev, 0x4, 0x41a);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x61a);
+	write_rtl8255(dev, 0x4, 0x41a);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404024, 0x27,
+		0x92402a8c, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc1b);
+	write_rtl8255(dev, 0x4, 0xe1b);
+	write_rtl8255(dev, 0x4, 0xc1b);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x81b);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa1b);
+	write_rtl8255(dev, 0x4, 0x81b);
+	write_rtl8255(dev, 0x4, 0x41b);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x61b);
+	write_rtl8255(dev, 0x4, 0x41b);
+	write_rtl8255(dev, 0x4, 0x41b);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x61b);
+	write_rtl8255(dev, 0x4, 0x41b);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404004, 0x27,
+		0x92402a90, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc1c);
+	write_rtl8255(dev, 0x4, 0xe1c);
+	write_rtl8255(dev, 0x4, 0xc1c);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x81c);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa1c);
+	write_rtl8255(dev, 0x4, 0x81c);
+	write_rtl8255(dev, 0x4, 0x41c);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x61c);
+	write_rtl8255(dev, 0x4, 0x41c);
+	write_rtl8255(dev, 0x4, 0x41c);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x61c);
+	write_rtl8255(dev, 0x4, 0x41c);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404001, 0x27,
+		0x92402a90, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc1d);
+	write_rtl8255(dev, 0x4, 0xe1d);
+	write_rtl8255(dev, 0x4, 0xc1d);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x81d);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa1d);
+	write_rtl8255(dev, 0x4, 0x81d);
+	write_rtl8255(dev, 0x4, 0x41d);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x61d);
+	write_rtl8255(dev, 0x4, 0x41d);
+	write_rtl8255(dev, 0x4, 0x41d);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x61d);
+	write_rtl8255(dev, 0x4, 0x41d);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a94, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc1e);
+	write_rtl8255(dev, 0x4, 0xe1e);
+	write_rtl8255(dev, 0x4, 0xc1e);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x81e);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa1e);
+	write_rtl8255(dev, 0x4, 0x81e);
+	write_rtl8255(dev, 0x4, 0x41e);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x61e);
+	write_rtl8255(dev, 0x4, 0x41e);
+	write_rtl8255(dev, 0x4, 0x41e);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x61e);
+	write_rtl8255(dev, 0x4, 0x41e);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x27,
+		0x92402a94, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc1f);
+	write_rtl8255(dev, 0x4, 0xe1f);
+	write_rtl8255(dev, 0x4, 0xc1f);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x81f);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa1f);
+	write_rtl8255(dev, 0x4, 0x81f);
+	write_rtl8255(dev, 0x4, 0x41f);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x61f);
+	write_rtl8255(dev, 0x4, 0x41f);
+	write_rtl8255(dev, 0x4, 0x41f);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x61f);
+	write_rtl8255(dev, 0x4, 0x41f);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404020, 0x80000027,
+		0x92402a98, 0xf8009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc20);
+	write_rtl8255(dev, 0x4, 0xe20);
+	write_rtl8255(dev, 0x4, 0xc20);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x820);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa20);
+	write_rtl8255(dev, 0x4, 0x820);
+	write_rtl8255(dev, 0x4, 0x420);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x620);
+	write_rtl8255(dev, 0x4, 0x420);
+	write_rtl8255(dev, 0x4, 0x420);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x620);
+	write_rtl8255(dev, 0x4, 0x420);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404020, 0x27,
+		0x92402a98, 0xf8009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc21);
+	write_rtl8255(dev, 0x4, 0xe21);
+	write_rtl8255(dev, 0x4, 0xc21);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x821);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa21);
+	write_rtl8255(dev, 0x4, 0x821);
+	write_rtl8255(dev, 0x4, 0x421);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x621);
+	write_rtl8255(dev, 0x4, 0x421);
+	write_rtl8255(dev, 0x4, 0x421);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x621);
+	write_rtl8255(dev, 0x4, 0x421);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a68, 0xf0009, 0x10028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc22);
+	write_rtl8255(dev, 0x4, 0xe22);
+	write_rtl8255(dev, 0x4, 0xc22);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x822);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa22);
+	write_rtl8255(dev, 0x4, 0x822);
+	write_rtl8255(dev, 0x4, 0x422);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x622);
+	write_rtl8255(dev, 0x4, 0x422);
+	write_rtl8255(dev, 0x4, 0x422);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x622);
+	write_rtl8255(dev, 0x4, 0x422);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404010, 0x80000027,
+		0x92402a68, 0xf0009, 0x20028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc23);
+	write_rtl8255(dev, 0x4, 0xe23);
+	write_rtl8255(dev, 0x4, 0xc23);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x823);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa23);
+	write_rtl8255(dev, 0x4, 0x823);
+	write_rtl8255(dev, 0x4, 0x423);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x623);
+	write_rtl8255(dev, 0x4, 0x423);
+	write_rtl8255(dev, 0x4, 0x423);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x623);
+	write_rtl8255(dev, 0x4, 0x423);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404010, 0x80000027,
+		0x92402a6c, 0xf0009, 0x30028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc24);
+	write_rtl8255(dev, 0x4, 0xe24);
+	write_rtl8255(dev, 0x4, 0xc24);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x824);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa24);
+	write_rtl8255(dev, 0x4, 0x824);
+	write_rtl8255(dev, 0x4, 0x424);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x624);
+	write_rtl8255(dev, 0x4, 0x424);
+	write_rtl8255(dev, 0x4, 0x424);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x624);
+	write_rtl8255(dev, 0x4, 0x424);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404010, 0x80000027,
+		0x92402a6c, 0xf0009, 0x40028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc25);
+	write_rtl8255(dev, 0x4, 0xe25);
+	write_rtl8255(dev, 0x4, 0xc25);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x825);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa25);
+	write_rtl8255(dev, 0x4, 0x825);
+	write_rtl8255(dev, 0x4, 0x425);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x625);
+	write_rtl8255(dev, 0x4, 0x425);
+	write_rtl8255(dev, 0x4, 0x425);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x625);
+	write_rtl8255(dev, 0x4, 0x425);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a70, 0xf0009, 0x60028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc26);
+	write_rtl8255(dev, 0x4, 0xe26);
+	write_rtl8255(dev, 0x4, 0xc26);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x826);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa26);
+	write_rtl8255(dev, 0x4, 0x826);
+	write_rtl8255(dev, 0x4, 0x426);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x626);
+	write_rtl8255(dev, 0x4, 0x426);
+	write_rtl8255(dev, 0x4, 0x426);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x626);
+	write_rtl8255(dev, 0x4, 0x426);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404031, 0x40000027,
+		0x92402a70, 0xf0011, 0x60028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc27);
+	write_rtl8255(dev, 0x4, 0xe27);
+	write_rtl8255(dev, 0x4, 0xc27);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x827);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa27);
+	write_rtl8255(dev, 0x4, 0x827);
+	write_rtl8255(dev, 0x4, 0x427);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x627);
+	write_rtl8255(dev, 0x4, 0x427);
+	write_rtl8255(dev, 0x4, 0x427);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x627);
+	write_rtl8255(dev, 0x4, 0x427);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404011, 0x40000027,
+		0x92402a74, 0xf0011, 0x60028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc28);
+	write_rtl8255(dev, 0x4, 0xe28);
+	write_rtl8255(dev, 0x4, 0xc28);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x828);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa28);
+	write_rtl8255(dev, 0x4, 0x828);
+	write_rtl8255(dev, 0x4, 0x428);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x628);
+	write_rtl8255(dev, 0x4, 0x428);
+	write_rtl8255(dev, 0x4, 0x428);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x628);
+	write_rtl8255(dev, 0x4, 0x428);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404010, 0xc0000027,
+		0x92402a74, 0xf0011, 0x60028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc29);
+	write_rtl8255(dev, 0x4, 0xe29);
+	write_rtl8255(dev, 0x4, 0xc29);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x829);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa29);
+	write_rtl8255(dev, 0x4, 0x829);
+	write_rtl8255(dev, 0x4, 0x429);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x629);
+	write_rtl8255(dev, 0x4, 0x429);
+	write_rtl8255(dev, 0x4, 0x429);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x629);
+	write_rtl8255(dev, 0x4, 0x429);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a78, 0xf0011, 0x60028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc2a);
+	write_rtl8255(dev, 0x4, 0xe2a);
+	write_rtl8255(dev, 0x4, 0xc2a);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x82a);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa2a);
+	write_rtl8255(dev, 0x4, 0x82a);
+	write_rtl8255(dev, 0x4, 0x42a);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x62a);
+	write_rtl8255(dev, 0x4, 0x42a);
+	write_rtl8255(dev, 0x4, 0x42a);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x62a);
+	write_rtl8255(dev, 0x4, 0x42a);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a78, 0xf0011, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc2b);
+	write_rtl8255(dev, 0x4, 0xe2b);
+	write_rtl8255(dev, 0x4, 0xc2b);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x82b);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa2b);
+	write_rtl8255(dev, 0x4, 0x82b);
+	write_rtl8255(dev, 0x4, 0x42b);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x62b);
+	write_rtl8255(dev, 0x4, 0x42b);
+	write_rtl8255(dev, 0x4, 0x42b);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x62b);
+	write_rtl8255(dev, 0x4, 0x42b);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a48, 0xf0019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc2c);
+	write_rtl8255(dev, 0x4, 0xe2c);
+	write_rtl8255(dev, 0x4, 0xc2c);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x82c);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa2c);
+	write_rtl8255(dev, 0x4, 0x82c);
+	write_rtl8255(dev, 0x4, 0x42c);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x62c);
+	write_rtl8255(dev, 0x4, 0x42c);
+	write_rtl8255(dev, 0x4, 0x42c);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x62c);
+	write_rtl8255(dev, 0x4, 0x42c);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a48, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc2d);
+	write_rtl8255(dev, 0x4, 0xe2d);
+	write_rtl8255(dev, 0x4, 0xc2d);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x82d);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa2d);
+	write_rtl8255(dev, 0x4, 0x82d);
+	write_rtl8255(dev, 0x4, 0x42d);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x62d);
+	write_rtl8255(dev, 0x4, 0x42d);
+	write_rtl8255(dev, 0x4, 0x42d);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x62d);
+	write_rtl8255(dev, 0x4, 0x42d);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a4c, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc2e);
+	write_rtl8255(dev, 0x4, 0xe2e);
+	write_rtl8255(dev, 0x4, 0xc2e);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x82e);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa2e);
+	write_rtl8255(dev, 0x4, 0x82e);
+	write_rtl8255(dev, 0x4, 0x42e);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x62e);
+	write_rtl8255(dev, 0x4, 0x42e);
+	write_rtl8255(dev, 0x4, 0x42e);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x62e);
+	write_rtl8255(dev, 0x4, 0x42e);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a4c, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc2f);
+	write_rtl8255(dev, 0x4, 0xe2f);
+	write_rtl8255(dev, 0x4, 0xc2f);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x82f);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa2f);
+	write_rtl8255(dev, 0x4, 0x82f);
+	write_rtl8255(dev, 0x4, 0x42f);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x62f);
+	write_rtl8255(dev, 0x4, 0x42f);
+	write_rtl8255(dev, 0x4, 0x42f);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x62f);
+	write_rtl8255(dev, 0x4, 0x42f);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a50, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc30);
+	write_rtl8255(dev, 0x4, 0xe30);
+	write_rtl8255(dev, 0x4, 0xc30);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x830);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa30);
+	write_rtl8255(dev, 0x4, 0x830);
+	write_rtl8255(dev, 0x4, 0x430);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x630);
+	write_rtl8255(dev, 0x4, 0x430);
+	write_rtl8255(dev, 0x4, 0x430);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x630);
+	write_rtl8255(dev, 0x4, 0x430);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a50, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc31);
+	write_rtl8255(dev, 0x4, 0xe31);
+	write_rtl8255(dev, 0x4, 0xc31);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x831);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa31);
+	write_rtl8255(dev, 0x4, 0x831);
+	write_rtl8255(dev, 0x4, 0x431);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x631);
+	write_rtl8255(dev, 0x4, 0x431);
+	write_rtl8255(dev, 0x4, 0x431);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x631);
+	write_rtl8255(dev, 0x4, 0x431);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a54, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc32);
+	write_rtl8255(dev, 0x4, 0xe32);
+	write_rtl8255(dev, 0x4, 0xc32);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x832);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa32);
+	write_rtl8255(dev, 0x4, 0x832);
+	write_rtl8255(dev, 0x4, 0x432);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x632);
+	write_rtl8255(dev, 0x4, 0x432);
+	write_rtl8255(dev, 0x4, 0x432);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x632);
+	write_rtl8255(dev, 0x4, 0x432);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a54, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc33);
+	write_rtl8255(dev, 0x4, 0xe33);
+	write_rtl8255(dev, 0x4, 0xc33);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x833);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa33);
+	write_rtl8255(dev, 0x4, 0x833);
+	write_rtl8255(dev, 0x4, 0x433);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x633);
+	write_rtl8255(dev, 0x4, 0x433);
+	write_rtl8255(dev, 0x4, 0x433);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x633);
+	write_rtl8255(dev, 0x4, 0x433);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a58, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc34);
+	write_rtl8255(dev, 0x4, 0xe34);
+	write_rtl8255(dev, 0x4, 0xc34);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x834);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa34);
+	write_rtl8255(dev, 0x4, 0x834);
+	write_rtl8255(dev, 0x4, 0x434);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x634);
+	write_rtl8255(dev, 0x4, 0x434);
+	write_rtl8255(dev, 0x4, 0x434);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x634);
+	write_rtl8255(dev, 0x4, 0x434);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a58, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc35);
+	write_rtl8255(dev, 0x4, 0xe35);
+	write_rtl8255(dev, 0x4, 0xc35);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x835);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa35);
+	write_rtl8255(dev, 0x4, 0x835);
+	write_rtl8255(dev, 0x4, 0x435);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x635);
+	write_rtl8255(dev, 0x4, 0x435);
+	write_rtl8255(dev, 0x4, 0x435);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x635);
+	write_rtl8255(dev, 0x4, 0x435);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a24, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc36);
+	write_rtl8255(dev, 0x4, 0xe36);
+	write_rtl8255(dev, 0x4, 0xc36);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x836);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa36);
+	write_rtl8255(dev, 0x4, 0x836);
+	write_rtl8255(dev, 0x4, 0x436);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x25);
+	write_rtl8255(dev, 0x4, 0x636);
+	write_rtl8255(dev, 0x4, 0x436);
+	write_rtl8255(dev, 0x4, 0x436);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x636);
+	write_rtl8255(dev, 0x4, 0x436);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a24, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc37);
+	write_rtl8255(dev, 0x4, 0xe37);
+	write_rtl8255(dev, 0x4, 0xc37);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x837);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa37);
+	write_rtl8255(dev, 0x4, 0x837);
+	write_rtl8255(dev, 0x4, 0x437);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x25);
+	write_rtl8255(dev, 0x4, 0x637);
+	write_rtl8255(dev, 0x4, 0x437);
+	write_rtl8255(dev, 0x4, 0x437);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x637);
+	write_rtl8255(dev, 0x4, 0x437);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a28, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc38);
+	write_rtl8255(dev, 0x4, 0xe38);
+	write_rtl8255(dev, 0x4, 0xc38);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x838);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa38);
+	write_rtl8255(dev, 0x4, 0x838);
+	write_rtl8255(dev, 0x4, 0x438);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x25);
+	write_rtl8255(dev, 0x4, 0x638);
+	write_rtl8255(dev, 0x4, 0x438);
+	write_rtl8255(dev, 0x4, 0x438);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x638);
+	write_rtl8255(dev, 0x4, 0x438);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a28, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc39);
+	write_rtl8255(dev, 0x4, 0xe39);
+	write_rtl8255(dev, 0x4, 0xc39);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x839);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa39);
+	write_rtl8255(dev, 0x4, 0x839);
+	write_rtl8255(dev, 0x4, 0x439);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x25);
+	write_rtl8255(dev, 0x4, 0x639);
+	write_rtl8255(dev, 0x4, 0x439);
+	write_rtl8255(dev, 0x4, 0x439);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x639);
+	write_rtl8255(dev, 0x4, 0x439);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a00, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc3a);
+	write_rtl8255(dev, 0x4, 0xe3a);
+	write_rtl8255(dev, 0x4, 0xc3a);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x83a);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa3a);
+	write_rtl8255(dev, 0x4, 0x83a);
+	write_rtl8255(dev, 0x4, 0x43a);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0x63a);
+	write_rtl8255(dev, 0x4, 0x43a);
+	write_rtl8255(dev, 0x4, 0x43a);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x63a);
+	write_rtl8255(dev, 0x4, 0x43a);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a00, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc3b);
+	write_rtl8255(dev, 0x4, 0xe3b);
+	write_rtl8255(dev, 0x4, 0xc3b);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x83b);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa3b);
+	write_rtl8255(dev, 0x4, 0x83b);
+	write_rtl8255(dev, 0x4, 0x43b);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0x63b);
+	write_rtl8255(dev, 0x4, 0x43b);
+	write_rtl8255(dev, 0x4, 0x43b);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x63b);
+	write_rtl8255(dev, 0x4, 0x43b);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a00, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc3c);
+	write_rtl8255(dev, 0x4, 0xe3c);
+	write_rtl8255(dev, 0x4, 0xc3c);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x83c);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa3c);
+	write_rtl8255(dev, 0x4, 0x83c);
+	write_rtl8255(dev, 0x4, 0x43c);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0x63c);
+	write_rtl8255(dev, 0x4, 0x43c);
+	write_rtl8255(dev, 0x4, 0x43c);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x63c);
+	write_rtl8255(dev, 0x4, 0x43c);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a00, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc3d);
+	write_rtl8255(dev, 0x4, 0xe3d);
+	write_rtl8255(dev, 0x4, 0xc3d);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x83d);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa3d);
+	write_rtl8255(dev, 0x4, 0x83d);
+	write_rtl8255(dev, 0x4, 0x43d);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0x63d);
+	write_rtl8255(dev, 0x4, 0x43d);
+	write_rtl8255(dev, 0x4, 0x43d);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x63d);
+	write_rtl8255(dev, 0x4, 0x43d);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a00, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc3e);
+	write_rtl8255(dev, 0x4, 0xe3e);
+	write_rtl8255(dev, 0x4, 0xc3e);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x83e);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa3e);
+	write_rtl8255(dev, 0x4, 0x83e);
+	write_rtl8255(dev, 0x4, 0x43e);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0x63e);
+	write_rtl8255(dev, 0x4, 0x43e);
+	write_rtl8255(dev, 0x4, 0x43e);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x63e);
+	write_rtl8255(dev, 0x4, 0x43e);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a00, 0xf8011, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc3f);
+	write_rtl8255(dev, 0x4, 0xe3f);
+	write_rtl8255(dev, 0x4, 0xc3f);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x83f);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa3f);
+	write_rtl8255(dev, 0x4, 0x83f);
+	write_rtl8255(dev, 0x4, 0x43f);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0x63f);
+	write_rtl8255(dev, 0x4, 0x43f);
+	write_rtl8255(dev, 0x4, 0x43f);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x63f);
+	write_rtl8255(dev, 0x4, 0x43f);
+	write_rtl8255(dev, 0x4, 0x0);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255_reg0c(dev, 0x3539, 0x70000c03, 0xfef46178, 0x408000, 0x403307,
+		0x924f80c0, 0xf955c, 0x8400, 0x429200, 0x1ce20);
+	write_rtl8255(dev, 0x1, 0x1c7);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x3, 0x27);
+	write_rtl8255(dev, 0x1, 0x47);
+	write_rtl8255(dev, 0x4, 0x98c);
+	write_rtl8255(dev, 0x5, 0x65);
+	write_rtl8255(dev, 0x6, 0x13);
+	write_rtl8255(dev, 0x7, 0x7c);
+	write_rtl8255(dev, 0x8, 0x6);
+	write_rtl8255(dev, 0x8, 0x7);
+	write_rtl8255(dev, 0x8, 0x6);
+	write_rtl8255(dev, 0x9, 0xce2);
+	write_rtl8255(dev, 0xb, 0x1c5);
+	write_rtl8255(dev, 0xd, 0xd7f);
+	write_rtl8255(dev, 0xe, 0x369);
+	write_rtl8255(dev, 0xa, 0xd56);
+	write_rtl8255(dev, 0xa, 0xd57);
+	mdelay(20);
+	write_rtl8255(dev, 0xd, 0xd7e);
+
+}
+
+
+void rtl8255_set_band_param(struct net_device *dev, short band)
+{
+	if(band != BAND_A){
+		write_nic_dword(dev, 0x94, 0x3dc00002);
+		write_nic_dword(dev, 0x88, 0x00100040);
+
+		write_phy_cck(dev, 0x13, 0xd0);
+
+		write_phy_cck(dev, 0x41, 0x9d);
+		write_nic_dword(dev, 0x8c, 0x00082205);
+		write_nic_byte(dev, 0xb4, 0x66);
+	}
+}
+
+void rtl8255_rf_init(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int i;
+	u16 brsr;
+//	short channel /*= priv->chan*/ = 1;
+	priv->chan = 1;
+
+	write_nic_word(dev, RFPinsOutput, 0x80);
+	write_nic_word(dev, RFPinsSelect, 0x80 | SW_CONTROL_GPIO);
+	write_nic_word(dev, RFPinsEnable, 0x80);
+	write_nic_word(dev, RFPinsSelect, SW_CONTROL_GPIO);
+
+	write_nic_dword(dev, RF_TIMING, 0x000f800f);
+
+	brsr = read_nic_word(dev, BRSR);
+
+	write_nic_word(dev, 0x2c, 0xffff);
+
+
+	rtl8180_set_anaparam(dev, RTL8255_ANAPARAM_ON);
+	rtl8185_set_anaparam2(dev, RTL8255_ANAPARAM2_ON);
+
+	write_nic_dword(dev, 0x94, 0x11c00002);
+
+	write_nic_dword(dev, RF_PARA, 0x100040);
+
+	rtl8185_rf_pins_enable(dev);
+
+	rtl8255_init_BGband(dev);
+	rtl8255_set_band_param(dev,BAND_BG);
+
+	write_phy_cck(dev, 0x0, 0x98);
+	write_phy_cck(dev, 0x3, 0x20);
+	write_phy_cck(dev, 0x4, 0x2e);
+	write_phy_cck(dev, 0x5, 0x12);
+	write_phy_cck(dev, 0x6, 0xfc);
+	write_phy_cck(dev, 0x7, 0xd8);
+	write_phy_cck(dev, 0x8, 0x2e);
+	write_phy_cck(dev, 0x10, 0xd3);
+	write_phy_cck(dev, 0x11, 0x88);
+	write_phy_cck(dev, 0x12, 0x47);
+	write_phy_cck(dev, 0x13, 0xd0); /* Ver C & D & 8187*/
+
+	write_phy_cck(dev, 0x19, 0x0);
+	write_phy_cck(dev, 0x1a, 0xa0);
+	write_phy_cck(dev, 0x1b, 0x8);
+	write_phy_cck(dev, 0x40, 0x86); /* CCK Carrier Sense Threshold */
+	write_phy_cck(dev, 0x41, 0x9d); /* Energy Threshold */
+	//write_phy_cck(dev, 0x42, 0x0);
+	write_phy_cck(dev, 0x43, 0x8);
+
+	write_nic_byte(dev, TESTR,0x8);
+
+	for(i=0;i<128;i++){
+		write_phy_ofdm(dev, 0x4b, rtl8255_agc[i]);
+		write_phy_ofdm(dev, 0x4a, (u8)i+ 0x80);
+	}
+
+
+	write_phy_ofdm(dev, 0x0, 0x1);
+	write_phy_ofdm(dev, 0x1, 0x2);
+	write_phy_ofdm(dev, 0x2, 0x43);
+	write_phy_ofdm(dev, 0x3, 0x0);
+	write_phy_ofdm(dev, 0x4, 0x0);
+	write_phy_ofdm(dev, 0x5, 0x0);
+	write_phy_ofdm(dev, 0x6, 0x40);
+	write_phy_ofdm(dev, 0x7, 0x0);
+	write_phy_ofdm(dev, 0x8, 0x40);
+	write_phy_ofdm(dev, 0x9, 0xfe);
+	write_phy_ofdm(dev, 0xa, 0x9);
+	write_phy_ofdm(dev, 0xb, 0x80);
+	write_phy_ofdm(dev, 0xc, 0x1);
+	write_phy_ofdm(dev, 0xd, 0x43);
+	write_phy_ofdm(dev, 0xe, 0xd3);
+	write_phy_ofdm(dev, 0xf, 0x38);
+	write_phy_ofdm(dev, 0x10, 0x4);
+	write_phy_ofdm(dev, 0x11, 0x06);/*agc resp time 700*/
+	write_phy_ofdm(dev, 0x12, 0x20);
+	write_phy_ofdm(dev, 0x13, 0x20);
+	write_phy_ofdm(dev, 0x14, 0x0);
+	write_phy_ofdm(dev, 0x15, 0x40);
+	write_phy_ofdm(dev, 0x16, 0x0);
+	write_phy_ofdm(dev, 0x17, 0x40);
+	write_phy_ofdm(dev, 0x18, 0xef);
+	write_phy_ofdm(dev, 0x19, 0x25);
+	write_phy_ofdm(dev, 0x1a, 0x20);
+	write_phy_ofdm(dev, 0x1b, 0x7a);
+	write_phy_ofdm(dev, 0x1c, 0x84);
+	write_phy_ofdm(dev, 0x1e, 0x95);
+	write_phy_ofdm(dev, 0x1f, 0x75);
+	write_phy_ofdm(dev, 0x20, 0x1f);
+	write_phy_ofdm(dev, 0x21, 0x17);
+	write_phy_ofdm(dev, 0x22, 0x16);
+	write_phy_ofdm(dev, 0x23, 0x70); //FIXME maybe not needed
+	write_phy_ofdm(dev, 0x24, 0x70);
+	write_phy_ofdm(dev, 0x25, 0x0);
+	write_phy_ofdm(dev, 0x26, 0x10);
+	write_phy_ofdm(dev, 0x27, 0x88);
+
+
+	write_nic_dword(dev, 0x94, 0x3dc00002); //BAND DEPEND.
+//	write_nic_dword(dev, 0x94, 0x15c00002); //BAND DEPEND.
+
+	write_phy_cck(dev, 0x4, 0x18);
+	write_phy_cck(dev, 0x43, 0x18);
+	write_phy_cck(dev, 0x6, 0xdc);
+	write_phy_cck(dev, 0x44, 0x2b);
+	write_phy_cck(dev, 0x45, 0x2b);
+	write_phy_cck(dev, 0x46, 0x25);
+	write_phy_cck(dev, 0x47, 0x15);
+	write_phy_cck(dev, 0x48, 0x0);
+	write_phy_cck(dev, 0x49, 0x0);
+	write_phy_cck(dev, 0x4a, 0x0);
+	write_phy_cck(dev, 0x4b, 0x0);
+//	write_phy_cck(dev, 0x4c, 0x5);
+#if 0
+	write_phy_cck(dev, 0x41, 0x9d); /* Energy Threshold */
+	// TESTR 0xb 8187
+	write_phy_cck(dev, 0x10, 0x93);// & 0xfb);
+#endif
+	//rtl8255_set_gain(dev, 1); /* FIXME this '1' is random */
+
+	rtl8255_SetTXPowerLevel(dev, priv->chan);
+
+	write_phy_cck(dev, 0x10, 0x93 |0x4); /* Rx ant B, 0xd3 for A */
+	write_phy_ofdm(dev, 0x26, 0x90); /* Rx ant B, 0x10 for A */
+
+	rtl8185_tx_antenna(dev, 0x3); /* TX ant B, 0x0 for A*/
+	/* make sure is waken up! */
+	rtl8180_set_anaparam(dev, RTL8255_ANAPARAM_ON);
+	rtl8185_set_anaparam2(dev, RTL8255_ANAPARAM2_ON);
+
+	rtl8255_set_band_param(dev,BAND_BG);
+
+	write_phy_cck(dev, 0x41, 0x9d);
+
+	rtl8255_set_gain(dev, 4);
+	//rtl8255_set_energy_threshold(dev);
+	write_phy_cck(dev, 0x41, 0x9d);
+	rtl8255_rf_set_chan(dev, priv->chan);
+
+	write_nic_word(dev, BRSR, brsr);
+}
+
diff --git a/drivers/staging/rtl8187se/r8180_rtl8255.h b/drivers/staging/rtl8187se/r8180_rtl8255.h
new file mode 100644
index 000000000000..be44ca6eb1d3
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_rtl8255.h
@@ -0,0 +1,19 @@
+/*
+  This is part of the rtl8180-sa2400 driver
+  released under the GPL (See file COPYING for details).
+  Copyright (c) 2005 Andrea Merello <andreamrl@tiscali.it>
+
+  This files contains programming code for the rtl8255
+  radio frontend.
+
+  *Many* thanks to Realtek Corp. for their great support!
+
+*/
+
+#define RTL8255_ANAPARAM_ON 0xa0000b59
+#define RTL8255_ANAPARAM2_ON 0x840cf311
+
+
+void rtl8255_rf_init(struct net_device *dev);
+void rtl8255_rf_set_chan(struct net_device *dev,short ch);
+void rtl8255_rf_close(struct net_device *dev);
diff --git a/drivers/staging/rtl8187se/r8180_sa2400.c b/drivers/staging/rtl8187se/r8180_sa2400.c
new file mode 100644
index 000000000000..d6495601715f
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_sa2400.c
@@ -0,0 +1,233 @@
+/*
+   This files contains PHILIPS SA2400 radio frontend programming routines.
+
+   This is part of rtl8180 OpenSource driver
+   Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+   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.
+
+   Code at http://che.ojctech.com/~dyoung/rtw/ has been useful to me to
+   understand some things.
+
+   Code from rtl8181 project has been useful to me to understand some things.
+
+   We want to tanks the Authors of such projects and the Ndiswrapper
+   project Authors.
+*/
+
+
+#include "r8180.h"
+#include "r8180_hw.h"
+#include "r8180_sa2400.h"
+
+
+//#define DEBUG_SA2400
+
+u32 sa2400_chan[] = {
+	0x0,	//dummy channel 0
+	0x00096c, //1
+	0x080970, //2
+	0x100974, //3
+	0x180978, //4
+	0x000980, //5
+	0x080984, //6
+	0x100988, //7
+	0x18098c, //8
+	0x000994, //9
+	0x080998, //10
+	0x10099c, //11
+	0x1809a0, //12
+	0x0009a8, //13
+	0x0009b4, //14
+};
+
+
+void rf_stabilize(struct net_device *dev)
+{
+	force_pci_posting(dev);
+	mdelay(3); //for now use a great value.. we may optimize in future
+}
+
+
+void write_sa2400(struct net_device *dev,u8 adr, u32 data)
+{
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 phy_config;
+
+        // philips sa2400 expects 24 bits data
+
+	/*if(adr == 4 && priv->digphy){
+		phy_config=0x60000000;
+	}else{
+		phy_config=0xb0000000;
+	}*/
+
+	phy_config = 0xb0000000; // MAC will bang bits to the sa2400
+
+	phy_config |= (((u32)(adr&0xf))<< 24);
+	phy_config |= (data & 0xffffff);
+	write_nic_dword(dev,PHY_CONFIG,phy_config);
+#ifdef DEBUG_SA2400
+	DMESG("Writing sa2400: %x (adr %x)",phy_config,adr);
+#endif
+	rf_stabilize(dev);
+}
+
+
+
+void sa2400_write_phy_antenna(struct net_device *dev,short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u8 ant;
+
+	ant = SA2400_ANTENNA;
+	if(priv->antb) /*default antenna is antenna B */
+		ant |= BB_ANTENNA_B;
+	if(ch == 14)
+		ant |= BB_ANTATTEN_CHAN14;
+	write_phy(dev,0x10,ant);
+	//DMESG("BB antenna %x ",ant);
+}
+
+
+/* from the rtl8181 embedded driver */
+short sa2400_rf_set_sens(struct net_device *dev, short sens)
+{
+	u8 finetune = 0;
+	if ((sens > 85) || (sens < 54)) return -1;
+
+	write_sa2400(dev,5,0x1dfb | (sens-54) << 15 |(finetune<<20));  // AGC	0xc9dfb
+
+	return 0;
+}
+
+
+void sa2400_rf_set_chan(struct net_device *dev, short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 txpw = 0xff & priv->chtxpwr[ch];
+	u32 chan = sa2400_chan[ch];
+
+	write_sa2400(dev,7,txpw);
+	//write_phy(dev,0x10,0xd1);
+	sa2400_write_phy_antenna(dev,ch);
+	write_sa2400(dev,0,chan);
+	write_sa2400(dev,1,0xbb50);
+	write_sa2400(dev,2,0x80);
+	write_sa2400(dev,3,0);
+}
+
+
+void sa2400_rf_close(struct net_device *dev)
+{
+	write_sa2400(dev, 4, 0);
+}
+
+
+void sa2400_rf_init(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 anaparam;
+	u8 firdac;
+
+	write_nic_byte(dev,PHY_DELAY,0x6);	//this is general
+	write_nic_byte(dev,CARRIER_SENSE_COUNTER,0x4c); //this is general
+
+	/*these are philips sa2400 specific*/
+	anaparam = read_nic_dword(dev,ANAPARAM);
+	anaparam = anaparam &~ (1<<ANAPARAM_TXDACOFF_SHIFT);
+
+	anaparam = anaparam &~ANAPARAM_PWR1_MASK;
+	anaparam = anaparam &~ANAPARAM_PWR0_MASK;
+	if(priv->digphy){
+		anaparam |= (SA2400_DIG_ANAPARAM_PWR1_ON<<ANAPARAM_PWR1_SHIFT);
+		anaparam |= (SA2400_ANAPARAM_PWR0_ON<<ANAPARAM_PWR0_SHIFT);
+	}else{
+		anaparam |= (SA2400_ANA_ANAPARAM_PWR1_ON<<ANAPARAM_PWR1_SHIFT);
+	}
+
+	rtl8180_set_anaparam(dev,anaparam);
+
+	firdac = (priv->digphy) ? (1<<SA2400_REG4_FIRDAC_SHIFT) : 0;
+	write_sa2400(dev,0,sa2400_chan[priv->chan]);
+	write_sa2400(dev,1,0xbb50);
+	write_sa2400(dev,2,0x80);
+	write_sa2400(dev,3,0);
+	write_sa2400(dev,4,0x19340 | firdac);
+	write_sa2400(dev,5,0xc9dfb);  // AGC
+	write_sa2400(dev,4,0x19348 | firdac);  //calibrates VCO
+
+	if(priv->digphy)
+		write_sa2400(dev,4,0x1938c); /*???*/
+
+	write_sa2400(dev,4,0x19340 | firdac);
+
+	write_sa2400(dev,0,sa2400_chan[priv->chan]);
+	write_sa2400(dev,1,0xbb50);
+	write_sa2400(dev,2,0x80);
+	write_sa2400(dev,3,0);
+	write_sa2400(dev,4,0x19344 | firdac); //calibrates filter
+
+	/* new from rtl8180 embedded driver (rtl8181 project) */
+	write_sa2400(dev,6,0x13ff | (1<<23)); // MANRX
+	write_sa2400(dev,8,0); //VCO
+
+	if(!priv->digphy)
+	{
+		rtl8180_set_anaparam(dev, anaparam | \
+				     (1<<ANAPARAM_TXDACOFF_SHIFT));
+
+		rtl8180_conttx_enable(dev);
+
+		write_sa2400(dev, 4, 0x19341); // calibrates DC
+
+		/* a 5us sleep is required here,
+		   we rely on the 3ms delay introduced in write_sa2400
+		*/
+		write_sa2400(dev, 4, 0x19345);
+		/* a 20us sleep is required here,
+		   we rely on the 3ms delay introduced in write_sa2400
+		*/
+		rtl8180_conttx_disable(dev);
+
+		rtl8180_set_anaparam(dev, anaparam);
+	}
+	/* end new */
+
+	write_sa2400(dev,4,0x19341 | firdac ); //RTX MODE
+
+	// Set tx power level !?
+
+
+	/*baseband configuration*/
+	write_phy(dev,0,0x98);
+	write_phy(dev,3,0x38);
+	write_phy(dev,4,0xe0);
+	write_phy(dev,5,0x90);
+	write_phy(dev,6,0x1a);
+	write_phy(dev,7,0x64);
+
+	/*Should be done something more here??*/
+
+	sa2400_write_phy_antenna(dev,priv->chan);
+
+	write_phy(dev,0x11,0x80);
+	if(priv->diversity)
+		write_phy(dev,0x12,0xc7);
+	else
+		write_phy(dev,0x12,0x47);
+
+	write_phy(dev,0x13,0x90 | priv->cs_treshold );
+
+	write_phy(dev,0x19,0x0);
+	write_phy(dev,0x1a,0xa0);
+
+	sa2400_rf_set_chan(dev,priv->chan);
+}
diff --git a/drivers/staging/rtl8187se/r8180_sa2400.h b/drivers/staging/rtl8187se/r8180_sa2400.h
new file mode 100644
index 000000000000..683a69b96af4
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_sa2400.h
@@ -0,0 +1,26 @@
+/*
+	This is part of rtl8180 OpenSource driver - v 0.7
+	Copyright (C) Andrea Merello 2004  <andreamrl@tiscali.it>
+	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.
+*/
+
+#define SA2400_ANTENNA 0x91
+#define SA2400_DIG_ANAPARAM_PWR1_ON 0x8
+#define SA2400_ANA_ANAPARAM_PWR1_ON 0x28
+#define SA2400_ANAPARAM_PWR0_ON 0x3
+
+#define SA2400_RF_MAX_SENS 85
+#define SA2400_RF_DEF_SENS 80
+
+#define SA2400_REG4_FIRDAC_SHIFT 7
+
+void sa2400_rf_init(struct net_device *dev);
+void sa2400_rf_set_chan(struct net_device *dev,short ch);
+short sa2400_rf_set_sens(struct net_device *dev,short sens);
+void sa2400_rf_close(struct net_device *dev);
diff --git a/drivers/staging/rtl8187se/r8180_wx.c b/drivers/staging/rtl8187se/r8180_wx.c
new file mode 100644
index 000000000000..8b3901d87f87
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_wx.c
@@ -0,0 +1,1644 @@
+/*
+   This file contains wireless extension handlers.
+
+   This is part of rtl8180 OpenSource driver.
+   Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+   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.
+*/
+
+
+#include "r8180.h"
+#include "r8180_hw.h"
+#include "r8180_sa2400.h"
+
+#ifdef ENABLE_DOT11D
+#include "dot11d.h"
+#endif
+
+//#define RATE_COUNT 4
+u32 rtl8180_rates[] = {1000000,2000000,5500000,11000000,
+	6000000,9000000,12000000,18000000,24000000,36000000,48000000,54000000};
+
+#define RATE_COUNT (sizeof(rtl8180_rates)/sizeof(rtl8180_rates[0]))
+
+static CHANNEL_LIST DefaultChannelPlan[] = {
+//	{{1,2,3,4,5,6,7,8,9,10,11,12,13,14},14},			//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);
+}
+
+
+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->flags & IW_ENCODE_DISABLED) {
+	}
+
+
+/*	i = erq->flags & IW_ENCODE_INDEX;
+	if (i < 1 || i > 4)
+*/
+
+	if (erq->length > 0) {
+
+		//int len = erq->length <= 5 ? 5 : 13;
+
+		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);
+#ifdef ENABLE_IPS
+//	printk("set mode ENABLE_IPS\n");
+	if(priv->bInactivePs){
+		if(wrqu->mode == IW_MODE_ADHOC)
+			IPSLeave(dev);
+	}
+#endif
+	ret = ieee80211_wx_set_mode(priv->ieee80211,a,wrqu,b);
+
+	//rtl8180_commit(dev);
+
+	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;
+	//struct iw_range_with_scan_capa* tmp = (struct iw_range_with_scan_capa*)range; //YJ,add,080819,for hidden ap
+
+	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' threshol 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->retry_capa;	/* What retry options are supported */
+//	range->retry_flags;	/* How to decode max/min retry limit */
+//	range->r_time_flags;	/* How to decode max/min retry life */
+//	range->min_retry;	/* Minimal number of retries */
+//	range->max_retry;	/* Maximal number of retries */
+//	range->min_r_time;	/* Minimal retry lifetime */
+//	range->max_r_time;	/* Maximal retry lifetime */
+
+        range->num_channels = 14;
+
+	for (i = 0, val = 0; i < 14; i++) {
+
+		// Include only legal frequencies for some countries
+#ifdef ENABLE_DOT11D
+		if ((GET_DOT11D_INFO(priv->ieee80211)->channel_map)[i+1]) {
+#else
+		if ((priv->ieee80211->channel_map)[i+1]) {
+#endif
+		        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;
+
+	//tmp->scan_capa = 0x01; //YJ,add,080819,for hidden ap
+
+	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;
+
+//YJ,add,080819, for hidden ap
+	//printk("==*&*&*&==>%s in\n", __func__);
+	//printk("=*&*&*&*===>flag:%x, %x\n", 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)
+		{
+			//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);
+			//printk("=====>network ssid:%s\n", ieee->current_network.ssid);
+		}
+	}
+//YJ,add,080819, for hidden ap, end
+
+	down(&priv->wx_sem);
+	if(priv->up){
+#ifdef ENABLE_IPS
+//		printk("set scan ENABLE_IPS\n");
+		priv->ieee80211->actscanning = true;
+		if(priv->bInactivePs && (priv->ieee80211->state != IEEE80211_LINKED)){
+			IPSLeave(dev);
+//			down(&priv->ieee80211->wx_sem);
+
+//			if (priv->ieee80211->iw_mode == IW_MODE_MONITOR || !(priv->ieee80211->proto_started)){
+//				ret = -1;
+//				up(&priv->ieee80211->wx_sem);
+//				up(&priv->wx_sem);
+//				return ret;
+//			}
+
+	//	queue_work(priv->ieee80211->wq, &priv->ieee80211->wx_sync_scan_wq);
+		//printk("start scan============================>\n");
+		ieee80211_softmac_ips_scan_syncro(priv->ieee80211);
+//ieee80211_start_scan(priv->ieee80211);
+		/* intentionally forget to up sem */
+//			up(&priv->ieee80211->wx_sem);
+			ret = 0;
+		}
+		else
+#endif
+		{
+			//YJ,add,080828, prevent scan in BusyTraffic
+			//FIXME: Need to consider last scan time
+			if ((priv->link_detect.bBusyTraffic) && (true))
+			{
+				ret = 0;
+				printk("Now traffic is busy, please try later!\n");
+			}
+			else
+			//YJ,add,080828, 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);
+#ifdef ENABLE_IPS
+	//printk("set essid ENABLE_IPS\n");
+	if(priv->bInactivePs)
+		IPSLeave(dev);
+#endif
+//	printk("haha:set essid %s essid_len = %d essid_flgs = %d\n",b,  wrqu->essid.length, wrqu->essid.flags);
+
+	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;
+}
+
+
+/* added by christian */
+/*
+static int r8180_wx_set_monitor_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->iw_mode != IW_MODE_MONITOR) return -1;
+  	priv->prism_hdr = mode;
+	if(!mode)dev->type=ARPHRD_IEEE80211;
+	else dev->type=ARPHRD_IEEE80211_PRISM;
+	DMESG("using %s RX encap", mode ? "AVS":"80211");
+	return 0;
+
+}
+*/
+//of         r8180_wx_set_monitor_type
+/* end added christian */
+
+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);
+	/*
+	if(priv->up){
+		rtl8180_rtx_disable(dev);
+		rtl8180_rx_enable(dev);
+		rtl8180_tx_enable(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;
+	}
+	//DMESG("returning %d",wrqu->retry.value);
+
+
+	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);
+	//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 */
+		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_psmode(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);
+
+	if(priv) {
+		ieee = priv->ieee80211;
+		if(ieee->ps == IEEE80211_PS_DISABLED) {
+			*((unsigned int *)extra) = IEEE80211_PS_DISABLED;
+			goto exit;
+		}
+		*((unsigned int *)extra) = IW_POWER_TIMEOUT;
+ 	if (ieee->ps & IEEE80211_PS_MBCAST)
+			*((unsigned int *)extra) |= IW_POWER_ALL_R;
+		else
+			*((unsigned int *)extra) |= IW_POWER_UNICAST_R;
+	} else
+		ret = -1;
+exit:
+	up(&priv->wx_sem);
+
+	return ret;
+}
+static int r8180_wx_set_psmode(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);
+
+	ret = ieee80211_wx_set_power(priv->ieee80211, info, wrqu, extra);
+
+	up(&priv->wx_sem);
+
+	return ret;
+
+}
+*/
+
+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;
+//		ieee80211_start_protocol(ieee);
+	}
+
+	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);
+	//struct ieee80211_network *network = &(priv->ieee80211->current_network);
+	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);
+	//struct ieee80211_network *network = &(priv->ieee80211->current_network);
+	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);
+	//struct ieee80211_device *ieee = netdev_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 (DefaultChannelPlan[*val].Len != 0){
+		priv ->channel_plan = *val;
+		// Clear old channel map
+		for (i=1;i<=MAX_CHANNEL_NUMBER;i++)
+		{
+#ifdef ENABLE_DOT11D
+			GET_DOT11D_INFO(priv->ieee80211)->channel_map[i] = 0;
+#else
+			priv->ieee80211->channel_map[i] = 0;
+#endif
+		}
+		// Set new channel map
+		for (i=1;i<=DefaultChannelPlan[*val].Len;i++)
+		{
+#ifdef ENABLE_DOT11D
+			GET_DOT11D_INFO(priv->ieee80211)->channel_map[DefaultChannelPlan[*val].Channel[i-1]] = 1;
+#else
+			priv->ieee80211->channel_map[DefaultChannelPlan[*val].Channel[i-1]] = 1;
+#endif
+		}
+	}
+	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);
+	//printk("===>%s()\n", __func__);
+
+	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,
+                                        struct iw_param *data, char *extra)
+{
+	//printk("====>%s()\n", __func__);
+	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, data, 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)
+{
+	//printk("====>%s()\n", __func__);
+
+	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,
+                                        struct iw_point *data, char *extra)
+{
+//	printk("====>%s(), len:%d\n", __func__, data->length);
+	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, data->length);
+#endif
+        up(&priv->wx_sem);
+	//printk("<======%s(), ret:%d\n", __func__, ret);
+        return ret;
+
+
+}
+static iw_handler r8180_wx_handlers[] =
+{
+        NULL,                     /* SIOCSIWCOMMIT */
+        r8180_wx_get_name,   	  /* SIOCGIWNAME */
+        dummy,                    /* SIOCSIWNWID */
+        dummy,                    /* SIOCGIWNWID */
+        r8180_wx_set_freq,        /* SIOCSIWFREQ */
+        r8180_wx_get_freq,        /* SIOCGIWFREQ */
+        r8180_wx_set_mode,        /* SIOCSIWMODE */
+        r8180_wx_get_mode,        /* SIOCGIWMODE */
+        r8180_wx_set_sens,        /* SIOCSIWSENS */
+        r8180_wx_get_sens,        /* SIOCGIWSENS */
+        NULL,                     /* SIOCSIWRANGE */
+        rtl8180_wx_get_range,	  /* SIOCGIWRANGE */
+        NULL,                     /* SIOCSIWPRIV */
+        NULL,                     /* SIOCGIWPRIV */
+        NULL,                     /* SIOCSIWSTATS */
+        NULL,                     /* SIOCGIWSTATS */
+        dummy,                    /* SIOCSIWSPY */
+        dummy,                    /* SIOCGIWSPY */
+        NULL,                     /* SIOCGIWTHRSPY */
+        NULL,                     /* SIOCWIWTHRSPY */
+        r8180_wx_set_wap,      	  /* SIOCSIWAP */
+        r8180_wx_get_wap,         /* SIOCGIWAP */
+	r8180_wx_set_mlme,        /* SIOCSIWMLME*/
+        dummy,                    /* SIOCGIWAPLIST -- depricated */
+        r8180_wx_set_scan,        /* SIOCSIWSCAN */
+        r8180_wx_get_scan,        /* SIOCGIWSCAN */
+        r8180_wx_set_essid,       /* SIOCSIWESSID */
+        r8180_wx_get_essid,       /* SIOCGIWESSID */
+        dummy,                    /* SIOCSIWNICKN */
+        dummy,                    /* SIOCGIWNICKN */
+        NULL,                     /* -- hole -- */
+        NULL,                     /* -- hole -- */
+        r8180_wx_set_rate,        /* SIOCSIWRATE */
+        r8180_wx_get_rate,        /* SIOCGIWRATE */
+        r8180_wx_set_rts,         /* SIOCSIWRTS */
+        r8180_wx_get_rts,         /* SIOCGIWRTS */
+        r8180_wx_set_frag,        /* SIOCSIWFRAG */
+        r8180_wx_get_frag,        /* SIOCGIWFRAG */
+        dummy,                    /* SIOCSIWTXPOW */
+        dummy,                    /* SIOCGIWTXPOW */
+        r8180_wx_set_retry,       /* SIOCSIWRETRY */
+        r8180_wx_get_retry,       /* SIOCGIWRETRY */
+        r8180_wx_set_enc,         /* SIOCSIWENCODE */
+        r8180_wx_get_enc,         /* SIOCGIWENCODE */
+        r8180_wx_set_power,       /* SIOCSIWPOWER */
+        r8180_wx_get_power,       /* SIOCGIWPOWER */
+        NULL,			  /*---hole---*/
+	NULL, 			  /*---hole---*/
+	r8180_wx_set_gen_ie,      /* SIOCSIWGENIE */
+	NULL, 			  /* SIOCSIWGENIE */
+	r8180_wx_set_auth,	  /* SIOCSIWAUTH */
+	NULL,                     /* SIOCSIWAUTH */
+	r8180_wx_set_enc_ext, 	  /* SIOCSIWENCODEEXT */
+	NULL,                  	  /* SIOCSIWENCODEEXT */
+	NULL, 			 /* SIOCSIWPMKSA */
+	NULL, 			  /*---hole---*/
+};
+
+
+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"
+
+	},
+	/* added by christian */
+	//{
+	//	SIOCIWFIRSTPRIV + 0x2,
+	//	IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "prismhdr"
+	//},
+	/* end added by christian */
+	{
+		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 + 0x5,
+//		0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "getpsmode"
+//	},
+//	{
+//		SIOCIWFIRSTPRIV + 0x6,
+//		IW_PRIV_SIZE_FIXED, 0, "setpsmode"
+//	},
+//set/get mode have been realized in public handlers
+
+	{
+		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,
+};
+
+#if WIRELESS_EXT >= 17
+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)));
+}
+
+//WB modefied 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;
+	//struct ieee80211_network* target = NULL;
+	int tmp_level = 0;
+	int tmp_qual = 0;
+	int tmp_noise = 0;
+	//unsigned long flag;
+
+	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;
+	}
+#if 0
+	spin_lock_irqsave(&ieee->lock, flag);
+	list_for_each_entry(target, &ieee->network_list, list)
+	{
+		if (is_same_network(target, &ieee->current_network, ieee))
+		{
+			printk("it's same network:%s\n", target->ssid);
+#if 0
+			if (!tmp_level)
+			{
+				tmp_level = target->stats.signalstrength;
+				tmp_qual = target->stats.signal;
+			}
+			else
+			{
+
+				tmp_level = (15*tmp_level + target->stats.signalstrength)/16;
+				tmp_qual = (15*tmp_qual + target->stats.signal)/16;
+			}
+#else
+			tmp_level = target->stats.signal;
+			tmp_qual = target->stats.signalstrength;
+			tmp_noise = target->stats.noise;
+			printk("level:%d, qual:%d, noise:%d\n", tmp_level, tmp_qual, tmp_noise);
+#endif
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&ieee->lock, flag);
+#endif
+	tmp_level = (&ieee->current_network)->stats.signal;
+	tmp_qual = (&ieee->current_network)->stats.signalstrength;
+	tmp_noise = (&ieee->current_network)->stats.noise;
+	//printk("level:%d, qual:%d, noise:%d\n", tmp_level, tmp_qual, tmp_noise);
+
+//	printk("level:%d\n", tmp_level);
+	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;
+}
+#endif
+
+
+struct iw_handler_def  r8180_wx_handlers_def={
+	.standard = r8180_wx_handlers,
+	.num_standard = sizeof(r8180_wx_handlers) / sizeof(iw_handler),
+	.private = r8180_private_handler,
+	.num_private = sizeof(r8180_private_handler) / sizeof(iw_handler),
+ 	.num_private_args = sizeof(r8180_private_args) / sizeof(struct iw_priv_args),
+#if WIRELESS_EXT >= 17
+	.get_wireless_stats = r8180_get_wireless_stats,
+#endif
+	.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
new file mode 100644
index 000000000000..b20a67d33411
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_wx.h
@@ -0,0 +1,21 @@
+/*
+	This is part of rtl8180 OpenSource driver - v 0.3
+	Copyright (C) Andrea Merello 2004  <andreamrl@tiscali.it>
+	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 file (will) contains wireless extension handlers*/
+
+#ifndef R8180_WX_H
+#define R8180_WX_H
+#include <linux/wireless.h>
+#include "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
new file mode 100644
index 000000000000..4b885a2319ee
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8185b_init.c
@@ -0,0 +1,3342 @@
+/*++
+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_sa2400.h"  /* PHILIPS Radio frontend */
+#include "r8180_max2820.h" /* MAXIM Radio frontend */
+#include "r8180_gct.h"     /* GCT Radio frontend */
+#include "r8180_rtl8225.h" /* RTL8225 Radio frontend */
+#include "r8180_rtl8255.h" /* RTL8255 Radio frontend */
+#include "r8180_93cx6.h"   /* Card EEPROM */
+#include "r8180_wx.h"
+
+#ifdef CONFIG_RTL8180_PM
+#include "r8180_pm.h"
+#endif
+
+#ifdef ENABLE_DOT11D
+#include "dot11d.h"
+#endif
+
+#ifdef CONFIG_RTL8185B
+
+//#define CONFIG_RTL8180_IO_MAP
+
+#define TC_3W_POLL_MAX_TRY_CNT 5
+#ifdef CONFIG_RTL818X_S
+static u8 MAC_REG_TABLE[][2]={
+                        //PAGA 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},
+                        {0xC4, 0x22}, {0xC5, 0x22}, {0xC6, 0x22}, {0xC7, 0x22}, {0xC8, 0x22}, //lzm add 080826
+			{0xC9, 0x22}, {0xCA, 0x22}, {0xCB, 0x22}, {0xCC, 0x22}, {0xCD, 0x22},//lzm add 080826
+                        {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},
+
+                        //PAGA 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
+		};
+#else
+ static u8 MAC_REG_TABLE[][2]={
+			//PAGA 0:
+			{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:
+			{0x5e, 0x01},
+			{0x58, 0x4b}, {0x59, 0x00}, {0x5a, 0x4b}, {0x5b, 0x00}, {0x60, 0x4b},
+			{0x61, 0x09}, {0x62, 0x4b}, {0x63, 0x09}, {0xce, 0x0f}, {0xcf, 0x00},
+			{0xe0, 0xff}, {0xe1, 0x0f}, {0xe2, 0x00}, {0xf0, 0x4e}, {0xf1, 0x01},
+			{0xf2, 0x02}, {0xf3, 0x03}, {0xf4, 0x04}, {0xf5, 0x05}, {0xf6, 0x06},
+			{0xf7, 0x07}, {0xf8, 0x08},
+
+
+			//PAGE 2:
+			{0x5e, 0x02},
+			{0x0c, 0x04}, {0x21, 0x61}, {0x22, 0x68}, {0x23, 0x6f}, {0x24, 0x76},
+			{0x25, 0x7d}, {0x26, 0x84}, {0x27, 0x8d}, {0x4d, 0x08}, {0x4e, 0x00},
+			{0x50, 0x05}, {0x51, 0xf5}, {0x52, 0x04}, {0x53, 0xa0}, {0x54, 0x1f},
+			{0x55, 0x23}, {0x56, 0x45}, {0x57, 0x67}, {0x58, 0x08}, {0x59, 0x08},
+			{0x5a, 0x08}, {0x5b, 0x08}, {0x60, 0x08}, {0x61, 0x08}, {0x62, 0x08},
+			{0x63, 0x08}, {0x64, 0xcf}, {0x72, 0x56}, {0x73, 0x9a},
+
+			//PAGA 0:
+			{0x5e, 0x00},
+			{0x34, 0xff}, {0x35, 0x0f}, {0x5b, 0x40}, {0x84, 0x88}, {0x85, 0x24},
+			{0x88, 0x54}, {0x8b, 0xb8}, {0x8c, 0x07}, {0x8d, 0x00}, {0x94, 0x1b},
+			{0x95, 0x12}, {0x96, 0x00}, {0x97, 0x06}, {0x9d, 0x1a}, {0x9f, 0x10},
+			{0xb4, 0x22}, {0xbe, 0x80}, {0xdb, 0x00}, {0xee, 0x00}, {0x5b, 0x42},
+			{0x91, 0x03},
+
+			//PAGE 2:
+			{0x5e, 0x02},
+			{0x4c, 0x03},
+
+			//PAGE 0:
+			{0x5e, 0x00},
+
+			//PAGE 3:
+			{0x5e, 0x03},
+			{0x9f, 0x00},
+
+			//PAGE 0:
+			{0x5e, 0x00},
+			{0x8c, 0x01}, {0x8d, 0x10},{0x8e, 0x08}, {0x8f, 0x00}
+		};
+
+
+static u8  ZEBRA_AGC[]={
+	0,
+	0x5e,0x5e,0x5e,0x5e,0x5d,0x5b,0x59,0x57,0x55,0x53,0x51,0x4f,0x4d,0x4b,0x49,0x47,
+	0x45,0x43,0x41,0x3f,0x3d,0x3b,0x39,0x37,0x35,0x33,0x31,0x2f,0x2d,0x2b,0x29,0x27,
+	0x25,0x23,0x21,0x1f,0x1d,0x1b,0x19,0x17,0x15,0x13,0x11,0x0f,0x0d,0x0b,0x09,0x07,
+	0x05,0x03,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,
+	0x19,0x19,0x19,0x019,0x19,0x19,0x19,0x19,0x19,0x19,0x1e,0x1f,0x20,0x21,0x21,0x22,
+	0x23,0x24,0x24,0x25,0x25,0x26,0x26,0x27,0x27,0x28,0x28,0x28,0x29,0x2a,0x2a,0x2b,
+	0x2b,0x2b,0x2c,0x2c,0x2c,0x2d,0x2d,0x2d,0x2e,0x2e,0x2f,0x30,0x31,0x31,0x31,0x31,
+	0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31
+	};
+
+static u32 ZEBRA_RF_RX_GAIN_TABLE[]={
+	0,
+	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,0x03bb
+};
+
+// 2006.07.13, SD3 szuyitasi:
+//	OFDM.0x03=0x0C (original is 0x0F)
+// Use the new SD3 given param, by shien chang, 2006.07.14
+static u8 OFDM_CONFIG[]={
+	0x10, 0x0d, 0x01, 0x0C, 0x14, 0xfb, 0x0f, 0x60, 0x00, 0x60,
+	0x00, 0x00, 0x00, 0x5c, 0x00, 0x00, 0x40, 0x00, 0x40, 0x00,
+	0x00, 0x00, 0xa8, 0x46, 0xb2, 0x33, 0x07, 0xa5, 0x6f, 0x55,
+	0xc8, 0xb3, 0x0a, 0xe1, 0x1c, 0x8a, 0xb6, 0x83, 0x34, 0x0f,
+	0x4f, 0x23, 0x6f, 0xc2, 0x6b, 0x40, 0x80, 0x00, 0xc0, 0xc1,
+	0x58, 0xf1, 0x00, 0xe4, 0x90, 0x3e, 0x6d, 0x3c, 0xff, 0x07
+};
+#endif
+
+/*---------------------------------------------------------------
+  * Hardware IO
+  * the code is ported from Windows source code
+  ----------------------------------------------------------------*/
+
+void
+PlatformIOWrite1Byte(
+	struct net_device *dev,
+	u32		offset,
+	u8		data
+	)
+{
+#ifndef CONFIG_RTL8180_IO_MAP
+	write_nic_byte(dev, offset, data);
+	read_nic_byte(dev, offset); // To make sure write operation is completed, 2005.11.09, by rcnjko.
+
+#else // Port IO
+	u32 Page = (offset >> 8);
+
+	switch(Page)
+	{
+	case 0: // Page 0
+		write_nic_byte(dev, offset, data);
+		break;
+
+	case 1: // Page 1
+	case 2: // Page 2
+	case 3: // Page 3
+		{
+			u8 psr = read_nic_byte(dev, PSR);
+
+			write_nic_byte(dev, PSR, ((psr & 0xfc) | (u8)Page)); // Switch to page N.
+			write_nic_byte(dev, (offset & 0xff), data);
+			write_nic_byte(dev, PSR, (psr & 0xfc)); // Switch to page 0.
+		}
+		break;
+
+	default:
+		// Illegal page number.
+		DMESGE("PlatformIOWrite1Byte(): illegal page number: %d, offset: %#X", Page, offset);
+		break;
+	}
+#endif
+}
+
+void
+PlatformIOWrite2Byte(
+	struct net_device *dev,
+	u32		offset,
+	u16		data
+	)
+{
+#ifndef CONFIG_RTL8180_IO_MAP
+	write_nic_word(dev, offset, data);
+	read_nic_word(dev, offset); // To make sure write operation is completed, 2005.11.09, by rcnjko.
+
+
+#else // Port IO
+	u32 Page = (offset >> 8);
+
+	switch(Page)
+	{
+	case 0: // Page 0
+		write_nic_word(dev, offset, data);
+		break;
+
+	case 1: // Page 1
+	case 2: // Page 2
+	case 3: // Page 3
+		{
+			u8 psr = read_nic_byte(dev, PSR);
+
+			write_nic_byte(dev, PSR, ((psr & 0xfc) | (u8)Page)); // Switch to page N.
+			write_nic_word(dev, (offset & 0xff), data);
+			write_nic_byte(dev, PSR, (psr & 0xfc)); // Switch to page 0.
+		}
+		break;
+
+	default:
+		// Illegal page number.
+		DMESGE("PlatformIOWrite2Byte(): illegal page number: %d, offset: %#X", Page, offset);
+		break;
+	}
+#endif
+}
+u8 PlatformIORead1Byte(struct net_device *dev, u32 offset);
+
+void
+PlatformIOWrite4Byte(
+	struct net_device *dev,
+	u32		offset,
+	u32		data
+	)
+{
+#ifndef CONFIG_RTL8180_IO_MAP
+//{by amy 080312
+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) );
+	}
+//by amy 080312}
+	else{
+		write_nic_dword(dev, offset, data);
+		read_nic_dword(dev, offset); // To make sure write operation is completed, 2005.11.09, by rcnjko.
+	}
+#else // Port IO
+	u32 Page = (offset >> 8);
+
+	switch(Page)
+	{
+	case 0: // Page 0
+		write_nic_word(dev, offset, data);
+		break;
+
+	case 1: // Page 1
+	case 2: // Page 2
+	case 3: // Page 3
+		{
+			u8 psr = read_nic_byte(dev, PSR);
+
+			write_nic_byte(dev, PSR, ((psr & 0xfc) | (u8)Page)); // Switch to page N.
+			write_nic_dword(dev, (offset & 0xff), data);
+			write_nic_byte(dev, PSR, (psr & 0xfc)); // Switch to page 0.
+		}
+		break;
+
+	default:
+		// Illegal page number.
+		DMESGE("PlatformIOWrite4Byte(): illegal page number: %d, offset: %#X", Page, offset);
+		break;
+	}
+#endif
+}
+
+u8
+PlatformIORead1Byte(
+	struct net_device *dev,
+	u32		offset
+	)
+{
+	u8	data = 0;
+
+#ifndef CONFIG_RTL8180_IO_MAP
+	data = read_nic_byte(dev, offset);
+
+#else // Port IO
+	u32 Page = (offset >> 8);
+
+	switch(Page)
+	{
+	case 0: // Page 0
+		data = read_nic_byte(dev, offset);
+		break;
+
+	case 1: // Page 1
+	case 2: // Page 2
+	case 3: // Page 3
+		{
+			u8 psr = read_nic_byte(dev, PSR);
+
+			write_nic_byte(dev, PSR, ((psr & 0xfc) | (u8)Page)); // Switch to page N.
+			data = read_nic_byte(dev, (offset & 0xff));
+			write_nic_byte(dev, PSR, (psr & 0xfc)); // Switch to page 0.
+		}
+		break;
+
+	default:
+		// Illegal page number.
+		DMESGE("PlatformIORead1Byte(): illegal page number: %d, offset: %#X", Page, offset);
+		break;
+	}
+#endif
+
+	return data;
+}
+
+u16
+PlatformIORead2Byte(
+	struct net_device *dev,
+	u32		offset
+	)
+{
+	u16	data = 0;
+
+#ifndef CONFIG_RTL8180_IO_MAP
+	data = read_nic_word(dev, offset);
+
+#else // Port IO
+	u32 Page = (offset >> 8);
+
+	switch(Page)
+	{
+	case 0: // Page 0
+		data = read_nic_word(dev, offset);
+		break;
+
+	case 1: // Page 1
+	case 2: // Page 2
+	case 3: // Page 3
+		{
+			u8 psr = read_nic_byte(dev, PSR);
+
+			write_nic_byte(dev, PSR, ((psr & 0xfc) | (u8)Page)); // Switch to page N.
+			data = read_nic_word(dev, (offset & 0xff));
+			write_nic_byte(dev, PSR, (psr & 0xfc)); // Switch to page 0.
+		}
+		break;
+
+	default:
+		// Illegal page number.
+		DMESGE("PlatformIORead2Byte(): illegal page number: %d, offset: %#X", Page, offset);
+		break;
+	}
+#endif
+
+	return data;
+}
+
+u32
+PlatformIORead4Byte(
+	struct net_device *dev,
+	u32		offset
+	)
+{
+	u32	data = 0;
+
+#ifndef CONFIG_RTL8180_IO_MAP
+	data = read_nic_dword(dev, offset);
+
+#else // Port IO
+	u32 Page = (offset >> 8);
+
+	switch(Page)
+	{
+	case 0: // Page 0
+		data = read_nic_dword(dev, offset);
+		break;
+
+	case 1: // Page 1
+	case 2: // Page 2
+	case 3: // Page 3
+		{
+			u8 psr = read_nic_byte(dev, PSR);
+
+			write_nic_byte(dev, PSR, ((psr & 0xfc) | (u8)Page)); // Switch to page N.
+			data = read_nic_dword(dev, (offset & 0xff));
+			write_nic_byte(dev, PSR, (psr & 0xfc)); // Switch to page 0.
+		}
+		break;
+
+	default:
+		// Illegal page number.
+		DMESGE("PlatformIORead4Byte(): illegal page number: %d, offset: %#X\n", Page, offset);
+		break;
+	}
+#endif
+
+	return data;
+}
+
+void
+SetOutputEnableOfRfPins(
+	struct net_device *dev
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	switch(priv->rf_chip)
+	{
+	case RFCHIPID_RTL8225:
+	case RF_ZEBRA2:
+	case RF_ZEBRA4:
+		write_nic_word(dev, RFPinsEnable, 0x1bff);
+		//write_nic_word(dev, RFPinsEnable, 0x1fff);
+		break;
+	}
+}
+
+void
+ZEBRA_RFSerialWrite(
+	struct net_device *dev,
+	u32			data2Write,
+	u8			totalLength,
+	u8			low2high
+	)
+{
+	ThreeWireReg		twreg;
+	int 				i;
+	u16				oval,oval2,oval3;
+	u32				mask;
+	u16				UshortBuffer;
+
+	u8			u1bTmp;
+#ifdef CONFIG_RTL818X_S
+	// 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);
+#endif
+	UshortBuffer = read_nic_word(dev, RFPinsOutput);
+	oval = UshortBuffer & 0xfff8; // We shall clear bit0, 1, 2 first, 2005.10.28, by rcnjko.
+
+	oval2 = read_nic_word(dev, RFPinsEnable);
+	oval3 = read_nic_word(dev, RFPinsSelect);
+
+	// <RJ_NOTE> 3-wire should be controled by HW when we finish SW 3-wire programming. 2005.08.10, by rcnjko.
+	oval3 &= 0xfff8;
+
+	write_nic_word(dev, RFPinsEnable, (oval2|0x0007)); // Set To Output Enable
+	write_nic_word(dev, RFPinsSelect, (oval3|0x0007)); // Set To SW Switch
+	udelay(10);
+
+	// Add this to avoid hardware and software 3-wire conflict.
+	// 2005.03.01, by rcnjko.
+	twreg.longData = 0;
+	twreg.struc.enableB = 1;
+	write_nic_word(dev, RFPinsOutput, (twreg.longData|oval)); // Set SI_EN (RFLE)
+	udelay(2);
+	twreg.struc.enableB = 0;
+	write_nic_word(dev, RFPinsOutput, (twreg.longData|oval)); // Clear SI_EN (RFLE)
+	udelay(10);
+
+	mask = (low2high)?0x01:((u32)0x01<<(totalLength-1));
+
+	for(i=0; i<totalLength/2; i++)
+	{
+		twreg.struc.data = ((data2Write&mask)!=0) ? 1 : 0;
+		write_nic_word(dev, RFPinsOutput, (twreg.longData|oval));
+		twreg.struc.clk = 1;
+		write_nic_word(dev, RFPinsOutput, (twreg.longData|oval));
+		write_nic_word(dev, RFPinsOutput, (twreg.longData|oval));
+
+		mask = (low2high)?(mask<<1):(mask>>1);
+		twreg.struc.data = ((data2Write&mask)!=0) ? 1 : 0;
+		write_nic_word(dev, RFPinsOutput, (twreg.longData|oval));
+		write_nic_word(dev, RFPinsOutput, (twreg.longData|oval));
+		twreg.struc.clk = 0;
+		write_nic_word(dev, RFPinsOutput, (twreg.longData|oval));
+		mask = (low2high)?(mask<<1):(mask>>1);
+	}
+
+	twreg.struc.enableB = 1;
+	twreg.struc.clk = 0;
+	twreg.struc.data = 0;
+	write_nic_word(dev, RFPinsOutput, twreg.longData|oval);
+	udelay(10);
+
+	write_nic_word(dev, RFPinsOutput, oval|0x0004);
+	write_nic_word(dev, RFPinsSelect, oval3|0x0000);
+
+	SetOutputEnableOfRfPins(dev);
+}
+//by amy
+
+
+int
+HwHSSIThreeWire(
+	struct net_device *dev,
+	u8			*pDataBuf,
+	u8			nDataBufBitCnt,
+	int			bSI,
+	int			bWrite
+	)
+{
+	int	bResult = 1;
+	u8	TryCnt;
+	u8	u1bTmp;
+
+	do
+	{
+		// 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)
+			panic("HwThreeWire(): CmdReg: %#X RE|WE bits are not clear!!\n", u1bTmp);
+
+		// RTL8187S HSSI Read/Write Function
+		u1bTmp = read_nic_byte(dev, RF_SW_CONFIG);
+
+		if(bSI)
+		{
+			u1bTmp |=   RF_SW_CFG_SI;   //reg08[1]=1 Serial Interface(SI)
+		}else
+		{
+			u1bTmp &= ~RF_SW_CFG_SI;  //reg08[1]=0 Parallel Interface(PI)
+		}
+
+		write_nic_byte(dev, RF_SW_CONFIG, u1bTmp);
+
+		if(bSI)
+		{
+			// 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.
+
+		if(bWrite)
+		{
+			if(nDataBufBitCnt == 16)
+			{
+				write_nic_word(dev, SW_3W_DB0, *((u16*)pDataBuf));
+			}
+			else if(nDataBufBitCnt == 64)  // RTL8187S shouldn't enter this case
+			{
+				write_nic_dword(dev, SW_3W_DB0, *((u32*)pDataBuf));
+				write_nic_dword(dev, SW_3W_DB1, *((u32*)(pDataBuf + 4)));
+			}
+			else
+			{
+				int idx;
+				int ByteCnt = nDataBufBitCnt / 8;
+                                //printk("%d\n",nDataBufBitCnt);
+				if ((nDataBufBitCnt % 8) != 0)
+				panic("HwThreeWire(): nDataBufBitCnt(%d) should be multiple of 8!!!\n",
+				nDataBufBitCnt);
+
+			       if (nDataBufBitCnt > 64)
+				panic("HwThreeWire(): nDataBufBitCnt(%d) should <= 64!!!\n",
+				nDataBufBitCnt);
+
+				for(idx = 0; idx < ByteCnt; idx++)
+				{
+					write_nic_byte(dev, (SW_3W_DB0+idx), *(pDataBuf+idx));
+				}
+			}
+		}
+		else		//read
+		{
+			if(bSI)
+			{
+				// SI - reg274[3:0] : RF register's Address
+				write_nic_word(dev, SW_3W_DB0, *((u16*)pDataBuf) );
+			}
+			else
+			{
+				// PI - reg274[15:12] : RF register's Address
+				write_nic_word(dev, SW_3W_DB0, (*((u16*)pDataBuf)) << 12);
+			}
+		}
+
+		// Set up command: WE or RE.
+		if(bWrite)
+		{
+			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) != 0 )
+			{
+				break;
+			}
+			udelay(10);
+		}
+
+		write_nic_byte(dev, SW_3W_CMD1, 0);
+
+		// Read back data for read operation.
+		if(bWrite == 0)
+		{
+			if(bSI)
+			{
+				//Serial Interface : reg363_362[11:0]
+				*((u16*)pDataBuf) = read_nic_word(dev, SI_DATA_READ) ;
+			}
+			else
+			{
+				//Parallel Interface : reg361_360[11:0]
+				*((u16*)pDataBuf) = read_nic_word(dev, PI_DATA_READ);
+			}
+
+			*((u16*)pDataBuf) &= 0x0FFF;
+		}
+
+	}while(0);
+
+	return bResult;
+}
+//by amy
+
+int
+HwThreeWire(
+	struct net_device *dev,
+	u8			*pDataBuf,
+	u8			nDataBufBitCnt,
+	int			bHold,
+	int			bWrite
+	)
+{
+	int	bResult = 1;
+	u8	TryCnt;
+	u8	u1bTmp;
+
+	do
+	{
+		// 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)
+			panic("HwThreeWire(): CmdReg: %#X RE|WE bits are not clear!!\n", u1bTmp);
+
+		// Fill up data buffer for write operation.
+		if(nDataBufBitCnt == 16)
+		{
+			write_nic_word(dev, SW_3W_DB0, *((u16 *)pDataBuf));
+		}
+		else if(nDataBufBitCnt == 64)
+		{
+			write_nic_dword(dev, SW_3W_DB0, *((u32 *)pDataBuf));
+			write_nic_dword(dev, SW_3W_DB1, *((u32 *)(pDataBuf + 4)));
+		}
+		else
+		{
+			int idx;
+			int ByteCnt = nDataBufBitCnt / 8;
+
+			if ((nDataBufBitCnt % 8) != 0)
+				panic("HwThreeWire(): nDataBufBitCnt(%d) should be multiple of 8!!!\n",
+				nDataBufBitCnt);
+
+			if (nDataBufBitCnt > 64)
+				panic("HwThreeWire(): nDataBufBitCnt(%d) should <= 64!!!\n",
+				nDataBufBitCnt);
+
+			for(idx = 0; idx < ByteCnt; idx++)
+			{
+				write_nic_byte(dev, (SW_3W_DB0+idx), *(pDataBuf+idx));
+			}
+		}
+
+		// Fill up length field.
+		u1bTmp = (u8)(nDataBufBitCnt - 1); // Number of bits - 1.
+		if(bHold)
+			u1bTmp |= SW_3W_CMD0_HOLD;
+		write_nic_byte(dev, SW_3W_CMD0, u1bTmp);
+
+		// Set up command: WE or RE.
+		if(bWrite)
+		{
+			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 WE and RE are cleared and 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_RE|SW_3W_CMD1_WE)) == 0 &&
+				(u1bTmp & SW_3W_CMD1_DONE) != 0 )
+			{
+				break;
+			}
+			udelay(10);
+		}
+		if(TryCnt == TC_3W_POLL_MAX_TRY_CNT)
+		{
+			//RT_ASSERT(TryCnt != TC_3W_POLL_MAX_TRY_CNT,
+			//	("HwThreeWire(): CmdReg: %#X RE|WE bits are not clear or DONE is not set!!\n", u1bTmp));
+			// Workaround suggested by wcchu: clear WE here. 2006.07.07, by rcnjko.
+			write_nic_byte(dev, SW_3W_CMD1, 0);
+		}
+
+		// Read back data for read operation.
+		// <RJ_TODO> I am not sure if this is correct output format of a read operation.
+		if(bWrite == 0)
+		{
+			if(nDataBufBitCnt == 16)
+			{
+				*((u16 *)pDataBuf) = read_nic_word(dev, SW_3W_DB0);
+			}
+			else if(nDataBufBitCnt == 64)
+			{
+				*((u32 *)pDataBuf) = read_nic_dword(dev, SW_3W_DB0);
+				*((u32 *)(pDataBuf + 4)) = read_nic_dword(dev, SW_3W_DB1);
+			}
+			else
+			{
+				int idx;
+				int ByteCnt = nDataBufBitCnt / 8;
+
+				if ((nDataBufBitCnt % 8) != 0)
+					panic("HwThreeWire(): nDataBufBitCnt(%d) should be multiple of 8!!!\n",
+					nDataBufBitCnt);
+
+				if (nDataBufBitCnt > 64)
+					panic("HwThreeWire(): nDataBufBitCnt(%d) should <= 64!!!\n",
+					nDataBufBitCnt);
+
+				for(idx = 0; idx < ByteCnt; idx++)
+				{
+					*(pDataBuf+idx) = read_nic_byte(dev, (SW_3W_DB0+idx));
+				}
+			}
+		}
+
+	}while(0);
+
+	return bResult;
+}
+
+
+void
+RF_WriteReg(
+	struct net_device *dev,
+	u8		offset,
+	u32		data
+	)
+{
+	//RFReg			reg;
+	u32			data2Write;
+	u8			len;
+	u8			low2high;
+	//u32			RF_Read = 0;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+
+	switch(priv->rf_chip)
+	{
+	case RFCHIPID_RTL8225:
+	case RF_ZEBRA2:		// Annie 2006-05-12.
+	case RF_ZEBRA4:        //by amy
+		switch(priv->RegThreeWireMode)
+		{
+		case SW_THREE_WIRE:
+			{ // Perform SW 3-wire programming by driver.
+				data2Write = (data << 4) | (u32)(offset & 0x0f);
+				len = 16;
+				low2high = 0;
+				ZEBRA_RFSerialWrite(dev, data2Write, len, low2high);
+       			}
+			break;
+
+ 		case HW_THREE_WIRE:
+			{ // Pure HW 3-wire.
+				data2Write = (data << 4) | (u32)(offset & 0x0f);
+				len = 16;
+				HwThreeWire(
+					dev,
+					(u8 *)(&data2Write),	// pDataBuf,
+					len,				// nDataBufBitCnt,
+					0,					// bHold,
+					1);					// bWrite
+         		}
+			break;
+  #ifdef CONFIG_RTL818X_S
+			case HW_THREE_WIRE_PI: //Parallel Interface
+			{ // Pure HW 3-wire.
+				data2Write = (data << 4) | (u32)(offset & 0x0f);
+				len = 16;
+					HwHSSIThreeWire(
+						dev,
+						(u8*)(&data2Write),	// pDataBuf,
+						len,						// nDataBufBitCnt,
+						0, 					// bSI
+						1); 					// bWrite
+
+                                //printk("33333\n");
+			}
+			break;
+
+			case HW_THREE_WIRE_SI: //Serial Interface
+			{ // Pure HW 3-wire.
+				data2Write = (data << 4) | (u32)(offset & 0x0f);
+				len = 16;
+//                                printk(" enter  ZEBRA_RFSerialWrite\n ");
+//                                low2high = 0;
+//                                ZEBRA_RFSerialWrite(dev, data2Write, len, low2high);
+
+				HwHSSIThreeWire(
+					dev,
+					(u8*)(&data2Write),	// pDataBuf,
+					len,						// nDataBufBitCnt,
+					1, 					// bSI
+					1); 					// bWrite
+
+//                                 printk(" exit ZEBRA_RFSerialWrite\n ");
+			}
+			break;
+  #endif
+
+
+		default:
+			DMESGE("RF_WriteReg(): invalid RegThreeWireMode(%d) !!!", priv->RegThreeWireMode);
+			break;
+		}
+		break;
+
+	default:
+		DMESGE("RF_WriteReg(): unknown RFChipID: %#X", priv->rf_chip);
+		break;
+	}
+}
+
+
+void
+ZEBRA_RFSerialRead(
+	struct net_device *dev,
+	u32		data2Write,
+	u8		wLength,
+	u32		*data2Read,
+	u8		rLength,
+	u8		low2high
+	)
+{
+	ThreeWireReg	twreg;
+	int				i;
+	u16			oval,oval2,oval3,tmp, wReg80;
+	u32			mask;
+	u8			u1bTmp;
+	ThreeWireReg	tdata;
+	//PHAL_DATA_8187	pHalData = GetHalData8187(pAdapter);
+#ifdef CONFIG_RTL818X_S
+	{ // 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);
+	}
+#endif
+
+	wReg80 = 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);
+
+	*data2Read = 0;
+
+	// We must clear BIT0-3 here, otherwise,
+	// SW_Enalbe will be true when we first call ZEBRA_RFSerialRead() after 8187MPVC open,
+	// which will cause the value read become 0. 2005.04.11, by rcnjko.
+	oval &= ~0xf;
+
+	// Avoid collision with hardware three-wire.
+	twreg.longData = 0;
+	twreg.struc.enableB = 1;
+	write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(4);
+
+	twreg.longData = 0;
+	twreg.struc.enableB = 0;
+	twreg.struc.clk = 0;
+	twreg.struc.read_write = 0;
+	write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(5);
+
+	mask = (low2high) ? 0x01 : ((u32)0x01<<(32-1));
+	for(i = 0; i < wLength/2; i++)
+	{
+		twreg.struc.data = ((data2Write&mask) != 0) ? 1 : 0;
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(1);
+		twreg.struc.clk = 1;
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+
+		mask = (low2high) ? (mask<<1): (mask>>1);
+
+		if(i == 2)
+		{
+			// Commented out by Jackie, 2004.08.26. <RJ_NOTE> We must comment out the following two lines for we cannot pull down VCOPDN during RF Serail Read.
+			//PlatformEFIOWrite2Byte(pAdapter, RFPinsEnable, 0xe);     // turn off data enable
+			//PlatformEFIOWrite2Byte(pAdapter, RFPinsSelect, 0xe);
+
+			twreg.struc.read_write=1;
+			write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+			twreg.struc.clk = 0;
+			write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+			break;
+		}
+		twreg.struc.data = ((data2Write&mask) != 0) ? 1: 0;
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+
+		twreg.struc.clk = 0;
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(1);
+
+		mask = (low2high) ? (mask<<1) : (mask>>1);
+	}
+
+	twreg.struc.clk = 0;
+	twreg.struc.data = 0;
+	write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+	mask = (low2high) ? 0x01 : ((u32)0x01 << (12-1));
+
+	//
+	// 061016, by rcnjko:
+	// We must set data pin to HW controled, otherwise RF can't driver it and
+	// value RF register won't be able to read back properly.
+	//
+	write_nic_word(dev, RFPinsEnable, ( ((oval2|0x0E) & (~0x01))) );
+
+	for(i = 0; i < rLength; i++)
+	{
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(1);
+		twreg.struc.clk = 1;
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+		tmp = read_nic_word(dev, RFPinsInput);
+		tdata.longData = tmp;
+		*data2Read |= tdata.struc.clk ? mask : 0;
+
+		twreg.struc.clk = 0;
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+
+		mask = (low2high) ? (mask<<1) : (mask>>1);
+	}
+	twreg.struc.enableB = 1;
+	twreg.struc.clk = 0;
+	twreg.struc.data = 0;
+	twreg.struc.read_write = 1;
+	write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+
+	//PlatformEFIOWrite2Byte(pAdapter, RFPinsEnable, oval2|0x8);   // Set To Output Enable
+	write_nic_word(dev, RFPinsEnable, oval2);   // Set To Output Enable, <RJ_NOTE> We cannot enable BIT3 here, otherwise, we will failed to switch channel. 2005.04.12.
+	//PlatformEFIOWrite2Byte(pAdapter, RFPinsEnable, 0x1bff);
+	write_nic_word(dev, RFPinsSelect, oval3);   // Set To SW Switch
+	//PlatformEFIOWrite2Byte(pAdapter, RFPinsSelect, 0x0488);
+	write_nic_word(dev, RFPinsOutput, 0x3a0);
+	//PlatformEFIOWrite2Byte(pAdapter, RFPinsOutput, 0x0480);
+}
+
+
+u32
+RF_ReadReg(
+	struct net_device *dev,
+	u8		offset
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u32			data2Write;
+	u8			wlen;
+	u8			rlen;
+	u8			low2high;
+	u32			dataRead;
+
+	switch(priv->rf_chip)
+	{
+	case RFCHIPID_RTL8225:
+	case RF_ZEBRA2:
+	case RF_ZEBRA4:
+		switch(priv->RegThreeWireMode)
+		{
+#ifdef CONFIG_RTL818X_S
+			case HW_THREE_WIRE_PI: // For 87S  Parallel Interface.
+			{
+				data2Write = ((u32)(offset&0x0f));
+				wlen=16;
+				HwHSSIThreeWire(
+					dev,
+					(u8*)(&data2Write),	// pDataBuf,
+					wlen,					// nDataBufBitCnt,
+					0, 					// bSI
+					0); 					// bWrite
+				dataRead= data2Write;
+			}
+			break;
+
+			case HW_THREE_WIRE_SI: // For 87S Serial Interface.
+			{
+				data2Write = ((u32)(offset&0x0f)) ;
+				wlen=16;
+				HwHSSIThreeWire(
+					dev,
+					(u8*)(&data2Write),	// pDataBuf,
+					wlen,					// nDataBufBitCnt,
+					1, 					// bSI
+					0					// bWrite
+					);
+				dataRead= data2Write;
+			}
+			break;
+
+#endif
+			// Perform SW 3-wire programming by driver.
+			default:
+			{
+				data2Write = ((u32)(offset&0x1f)) << 27; // For Zebra E-cut. 2005.04.11, by rcnjko.
+				wlen = 6;
+				rlen = 12;
+				low2high = 0;
+				ZEBRA_RFSerialRead(dev, data2Write, wlen,&dataRead,rlen, low2high);
+			}
+			break;
+		}
+		break;
+	default:
+		dataRead = 0;
+		break;
+	}
+
+	return dataRead;
+}
+
+
+// by Owen on 04/07/14 for writing BB register successfully
+void
+WriteBBPortUchar(
+	struct net_device *dev,
+	u32		Data
+	)
+{
+	//u8	TimeoutCounter;
+	u8	RegisterContent;
+	u8	UCharData;
+
+	UCharData = (u8)((Data & 0x0000ff00) >> 8);
+	PlatformIOWrite4Byte(dev, PhyAddr, Data);
+	//for(TimeoutCounter = 10; TimeoutCounter > 0; TimeoutCounter--)
+	{
+		PlatformIOWrite4Byte(dev, PhyAddr, Data & 0xffffff7f);
+		RegisterContent = PlatformIORead1Byte(dev, PhyDataR);
+		//if(UCharData == RegisterContent)
+		//	break;
+	}
+}
+
+u8
+ReadBBPortUchar(
+	struct net_device *dev,
+	u32		addr
+	)
+{
+	//u8	TimeoutCounter;
+	u8	RegisterContent;
+
+	PlatformIOWrite4Byte(dev, PhyAddr, addr & 0xffffff7f);
+	RegisterContent = PlatformIORead1Byte(dev, PhyDataR);
+
+	return RegisterContent;
+}
+//{by amy 080312
+#ifdef CONFIG_RTL818X_S
+//
+//	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;
+
+	//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.
+	{
+		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
+			write_phy_cck(dev, 0x01, 0xc7); // Reg01 : c7
+
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0D, 0x54);   // Reg0d : 54
+			write_phy_ofdm(dev, 0x18, 0xb2);  // Reg18 : b2
+		}
+		else //  use main antenna
+		{
+			// Mac register, main antenna
+			write_nic_byte(dev, ANTSEL, 0x03);
+			//base band
+			// Config CCK RX antenna.
+			write_phy_cck(dev, 0x11, 0x9b); // Reg11 : 9b
+			write_phy_cck(dev, 0x01, 0xc7); // Reg01 : c7
+
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0d, 0x5c);   // Reg0d : 5c
+			write_phy_ofdm(dev, 0x18, 0xb2);  // Reg18 : b2
+		}
+	}
+	else   // Disable Antenna Diversity.
+	{
+		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
+			write_phy_cck(dev, 0x01, 0x47); // Reg01 : 47
+
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0D, 0x54);   // Reg0d : 54
+			write_phy_ofdm(dev, 0x18, 0x32);  // Reg18 : 32
+		}
+		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
+			write_phy_cck(dev, 0x01, 0x47); // Reg01 : 47
+
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0D, 0x5c);   // Reg0d : 5c
+			write_phy_ofdm(dev, 0x18, 0x32);  // Reg18 : 32
+		}
+	}
+	priv->CurrAntennaIndex = DefaultAnt; // Update default settings.
+	return	bAntennaSwitched;
+}
+#endif
+//by amy 080312
+/*---------------------------------------------------------------
+  * Hardware Initialization.
+  * the code is ported from Windows source code
+  ----------------------------------------------------------------*/
+
+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, u4bRF23, u4bRF24;
+       u8			u1b24E;
+
+#ifdef CONFIG_RTL818X_S
+
+	//=============================================================================
+	// 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 && priv->card_8185 == VERSION_8187S_C)
+		priv->card_8185 = VERSION_8187S_D;
+
+	// Page0 : reg0-reg15
+
+//	RF_WriteReg(dev, 0x00, 0x003f);			mdelay(1);//1
+	RF_WriteReg(dev, 0x00, 0x009f);      	mdelay(1);// 1
+
+	RF_WriteReg(dev, 0x01, 0x06e0);			mdelay(1);
+
+//	RF_WriteReg(dev, 0x02, 0x004c);			mdelay(1);//2
+	RF_WriteReg(dev, 0x02, 0x004d);			mdelay(1);// 2
+
+//	RF_WriteReg(dev, 0x03, 0x0000);			mdelay(1);//3
+	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);
+
+	if(priv->card_8185 < VERSION_8187S_C)
+	{
+		RF_WriteReg(dev, 0x04, 0x03f7);			mdelay(1);
+		RF_WriteReg(dev, 0x05, 0x05ab);			mdelay(1);
+		RF_WriteReg(dev, 0x06, 0x00c1);			mdelay(1);
+	}
+	else
+	{
+		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, 0x08, 0x0597);			mdelay(1);
+//	RF_WriteReg(dev, 0x09, 0x050a);			mdelay(1);
+	RF_WriteReg(dev, 0x0a, 0x0001);			mdelay(1);
+	RF_WriteReg(dev, 0x0b, 0x0418);			mdelay(1);
+
+	if(priv->card_8185 == VERSION_8187S_D)
+	{
+		RF_WriteReg(dev, 0x0c, 0x0fbe);			mdelay(1);
+		RF_WriteReg(dev, 0x0d, 0x0008);			mdelay(1);
+		RF_WriteReg(dev, 0x0e, 0x0807);			mdelay(1); // RX LO buffer
+	}
+	else
+	{
+		RF_WriteReg(dev, 0x0c, 0x0fbe);			mdelay(1);
+		RF_WriteReg(dev, 0x0d, 0x0008);			mdelay(1);
+		RF_WriteReg(dev, 0x0e, 0x0806);			mdelay(1); // RX LO buffer
+	}
+
+	RF_WriteReg(dev, 0x0f, 0x0acc);			mdelay(1);
+
+//	RF_WriteReg(dev, 0x00, 0x017f);			mdelay(1);//6
+	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);
+		//DbgPrint("RF - 0x%x = 0x%x", i, ZEBRA_RF_RX_GAIN_TABLE[i]);
+	}
+
+	RF_WriteReg(dev, 0x05, 0x0203);			mdelay(1); 	/// 203, 343
+	//RF_WriteReg(dev, 0x06, 0x0300);			mdelay(1);	// 400
+	RF_WriteReg(dev, 0x06, 0x0200);			mdelay(1);	// 400
+
+	RF_WriteReg(dev, 0x00, 0x0137);			mdelay(1);	// switch to reg16-reg30, and HSSI disable 137
+	mdelay(10); 	// Deay 10 ms. //0xfd
+
+//	RF_WriteReg(dev, 0x0c, 0x09be);			mdelay(1);	// 7
+	//RF_WriteReg(dev, 0x0c, 0x07be);			mdelay(1);
+	//mdelay(10); 	// Deay 10 ms. //0xfd
+
+	RF_WriteReg(dev, 0x0d, 0x0008);			mdelay(1);	// Z4 synthesizer loop filter setting, 392
+	mdelay(10); 	// Deay 10 ms. //0xfd
+
+	RF_WriteReg(dev, 0x00, 0x0037);			mdelay(1);	// switch to reg0-reg15, and HSSI disable
+	mdelay(10); 	// Deay 10 ms. //0xfd
+
+	RF_WriteReg(dev, 0x04, 0x0160);			mdelay(1); 	// CBC on, Tx Rx disable, High gain
+	mdelay(10); 	// Deay 10 ms. //0xfd
+
+	RF_WriteReg(dev, 0x07, 0x0080);			mdelay(1);	// Z4 setted channel 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
+
+	RF_WriteReg(dev, 0x00, 0x0137);			mdelay(1);	// switch to reg16-reg30 137, and HSSI disable 137
+	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);
+//{by amy 080312
+	// 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);
+		printk("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);
+	}
+//by amy 080312
+//	RF_WriteReg(dev, 0x0f, 0x0acc);			mdelay(1);  //-by amy 080312
+
+	RF_WriteReg(dev, 0x00, 0x00bf);			mdelay(1); // switch to reg0-reg15, and HSSI enable
+//	RF_WriteReg(dev, 0x0d, 0x009f);			mdelay(1); // Rx BB start calibration, 00c//-edward
+	RF_WriteReg(dev, 0x0d, 0x08df);			mdelay(1); // Rx BB start calibration, 00c//+edward
+	RF_WriteReg(dev, 0x02, 0x004d);			mdelay(1); // temperature meter off
+	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
+	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
+
+#if 0//-edward
+	RF_WriteReg(dev, 0x00, 0x0197);			mdelay(1);
+	RF_WriteReg(dev, 0x05, 0x05ab);			mdelay(1);
+	RF_WriteReg(dev, 0x00, 0x009F);			mdelay(1);
+#endif
+	RF_WriteReg(dev, 0x01, 0x0000);			mdelay(1); // Rx mode//+edward
+	RF_WriteReg(dev, 0x02, 0x0000);			mdelay(1); // Rx mode//+edward
+	//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
+	*/
+#if 0
+	write_nic_dword(dev, PHY_ADR, 0x0100c880);
+	write_nic_dword(dev, PHY_ADR, 0x01001c86);
+	write_nic_dword(dev, PHY_ADR, 0x01007890);
+	write_nic_dword(dev, PHY_ADR, 0x0100d0ae);
+	write_nic_dword(dev, PHY_ADR, 0x010006af);
+	write_nic_dword(dev, PHY_ADR, 0x01004681);
+#endif
+	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);
+#else
+	//=============================================================================
+	// RADIOCFG.TXT
+	//=============================================================================
+
+	RF_WriteReg(dev, 0x00, 0x00b7);			mdelay(1);
+	RF_WriteReg(dev, 0x01, 0x0ee0);			mdelay(1);
+	RF_WriteReg(dev, 0x02, 0x044d);			mdelay(1);
+	RF_WriteReg(dev, 0x03, 0x0441);			mdelay(1);
+	RF_WriteReg(dev, 0x04, 0x08c3);			mdelay(1);
+	RF_WriteReg(dev, 0x05, 0x0c72);			mdelay(1);
+	RF_WriteReg(dev, 0x06, 0x00e6);			mdelay(1);
+	RF_WriteReg(dev, 0x07, 0x082a);			mdelay(1);
+	RF_WriteReg(dev, 0x08, 0x003f);			mdelay(1);
+	RF_WriteReg(dev, 0x09, 0x0335);			mdelay(1);
+	RF_WriteReg(dev, 0x0a, 0x09d4);			mdelay(1);
+	RF_WriteReg(dev, 0x0b, 0x07bb);			mdelay(1);
+	RF_WriteReg(dev, 0x0c, 0x0850);			mdelay(1);
+	RF_WriteReg(dev, 0x0d, 0x0cdf);			mdelay(1);
+	RF_WriteReg(dev, 0x0e, 0x002b);			mdelay(1);
+	RF_WriteReg(dev, 0x0f, 0x0114);			mdelay(1);
+
+	RF_WriteReg(dev, 0x00, 0x01b7);			mdelay(1);
+
+
+	for(i=1;i<=95;i++)
+	{
+		RF_WriteReg(dev, 0x01, i);	mdelay(1);
+		RF_WriteReg(dev, 0x02, ZEBRA_RF_RX_GAIN_TABLE[i]); mdelay(1);
+		//DbgPrint("RF - 0x%x = 0x%x", i, ZEBRA_RF_RX_GAIN_TABLE[i]);
+	}
+
+	RF_WriteReg(dev, 0x03, 0x0080);			mdelay(1); 	// write reg 18
+	RF_WriteReg(dev, 0x05, 0x0004);			mdelay(1);	// write reg 20
+	RF_WriteReg(dev, 0x00, 0x00b7);			mdelay(1);	// switch to reg0-reg15
+	//0xfd
+	//0xfd
+	//0xfd
+	RF_WriteReg(dev, 0x02, 0x0c4d);			mdelay(1);
+	mdelay(100);	// Deay 100 ms. //0xfe
+	mdelay(100);	// Deay 100 ms. //0xfe
+	RF_WriteReg(dev, 0x02, 0x044d);			mdelay(1);
+	RF_WriteReg(dev, 0x00, 0x02bf);			mdelay(1);	//0x002f disable 6us corner change,  06f--> enable
+
+	//=============================================================================
+
+	//=============================================================================
+	// CCKCONF.TXT
+	//=============================================================================
+
+	//=============================================================================
+
+	//=============================================================================
+	// Follow WMAC RTL8225_Config()
+	//=============================================================================
+
+	// power control
+	write_nic_byte(dev, CCK_TXAGC, 0x03);
+	write_nic_byte(dev, OFDM_TXAGC, 0x07);
+	write_nic_byte(dev, ANTSEL, 0x03);
+
+	//=============================================================================
+
+	// OFDM BBP setup
+//	SetOutputEnableOfRfPins(dev);//by amy
+#endif
+
+
+
+	//=============================================================================
+	// AGC.txt
+	//=============================================================================
+
+//	PlatformIOWrite4Byte( dev, PhyAddr, 0x00001280);	// Annie, 2006-05-05
+	write_phy_ofdm(dev, 0x00, 0x12);
+	//WriteBBPortUchar(dev, 0x00001280);
+
+	for (i=0; i<128; i++)
+	{
+		//DbgPrint("AGC - [%x+1] = 0x%x\n", i, ZEBRA_AGC[i+1]);
+
+		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
+	//WriteBBPortUchar(dev, 0x00001080);
+
+	//=============================================================================
+
+	//=============================================================================
+	// OFDMCONF.TXT
+	//=============================================================================
+
+	for(i=0; i<60; i++)
+	{
+		u4bRegOffset=i;
+		u4bRegValue=OFDM_CONFIG[i];
+
+		//DbgPrint("OFDM - 0x%x = 0x%x\n", u4bRegOffset, u4bRegValue);
+
+		WriteBBPortUchar(dev,
+						(0x00000080 |
+						(u4bRegOffset & 0x7f) |
+						((u4bRegValue & 0xff) << 8)));
+	}
+
+	//=============================================================================
+//by amy for antenna
+	//=============================================================================
+//{by amy 080312
+#ifdef CONFIG_RTL818X_S
+	// Config Sw/Hw  Combinational Antenna Diversity. Added by Roger, 2008.02.26.
+	SetAntennaConfig87SE(dev, priv->bDefaultAntenna1, priv->bSwAntennaDiverity);
+#endif
+//by amy 080312}
+#if 0
+	// Config Sw/Hw  Antenna Diversity
+	if( priv->bSwAntennaDiverity )  //  Use SW+Hw Antenna Diversity
+	{
+		if( priv->bDefaultAntenna1 == true )  // aux antenna
+		{
+			// Mac register, aux antenna
+			write_nic_byte(dev, ANTSEL, 0x00);
+			// Config CCK RX antenna.
+			write_phy_cck(dev, 0x11, 0xbb); // Reg11 : bb
+			write_phy_cck(dev, 0x0c, 0x09); // Reg0c : 09
+			write_phy_cck(dev, 0x01, 0xc7); // Reg01 : c7
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0d, 0x54);   // Reg0d : 54
+			write_phy_ofdm(dev, 0x18, 0xb2);  // Reg18 : b2
+		}
+		else //  main antenna
+		{
+			// Mac register, main antenna
+			write_nic_byte(dev, ANTSEL, 0x03);
+			//base band
+			// Config CCK RX antenna.
+			write_phy_cck(dev, 0x11, 0x9b); // Reg11 : 9b
+			write_phy_cck(dev, 0x0c, 0x09); // Reg0c : 09
+			write_phy_cck(dev, 0x01, 0xc7); // Reg01 : c7
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0d, 0x5c);   // Reg0d : 5c
+			write_phy_ofdm(dev, 0x18, 0xb2);  // Reg18 : b2
+		}
+	}
+	else   // Disable Antenna Diversity
+	{
+		if( priv->bDefaultAntenna1 == true ) // aux Antenna
+		{
+			// Mac register, aux antenna
+			write_nic_byte(dev, ANTSEL, 0x00);
+			// Config CCK RX antenna.
+			write_phy_cck(dev, 0x11, 0xbb); // Reg11 : bb
+			write_phy_cck(dev, 0x0c, 0x09); // Reg0c : 09
+			write_phy_cck(dev, 0x01, 0x47); // Reg01 : 47
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0d, 0x54);   // Reg0d : 54
+			write_phy_ofdm(dev, 0x18, 0x32);  // Reg18 : 32
+		}
+		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
+			write_phy_cck(dev, 0x0c, 0x09); // Reg0c : 09
+			write_phy_cck(dev, 0x01, 0x47); // Reg01 : 47
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0d, 0x5c);   // Reg0d : 5c
+			write_phy_ofdm(dev, 0x18, 0x32);  // Reg18 : 32
+		}
+	}
+#endif
+//by amy for antenna
+}
+
+
+void
+UpdateInitialGain(
+	struct net_device *dev
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	//unsigned char* IGTable;
+	//u8			DIG_CurrentInitialGain = 4;
+	//unsigned char u1Tmp;
+
+	//lzm add 080826
+	if(priv->eRFPowerState != eRfOn)
+	{
+		//Don't access BB/RF under disable PLL situation.
+		//RT_TRACE(COMP_DIG, DBG_LOUD, ("UpdateInitialGain - pHalData->eRFPowerState!=eRfOn\n"));
+		// Back to the original state
+		priv->InitialGain= priv->InitialGainBackUp;
+		return;
+	}
+
+	switch(priv->rf_chip)
+	{
+#if 0
+	case RF_ZEBRA2:
+		// Dynamic set initial gain, by shien chang, 2006.07.14
+		switch(priv->InitialGain)
+		{
+			case 1: //m861dBm
+				DMESG("RTL8185B + 8225 Initial Gain State 1: -82 dBm \n");
+				write_nic_dword(dev, PhyAddr, 0x2697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0x86a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfa85);	mdelay(1);
+				break;
+
+			case 2: //m862dBm
+				DMESG("RTL8185B + 8225 Initial Gain State 2: -82 dBm \n");
+				write_nic_dword(dev, PhyAddr, 0x2697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0x86a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfb85);	mdelay(1);
+				break;
+
+			case 3: //m863dBm
+				DMESG("RTL8185B + 8225 Initial Gain State 3: -82 dBm \n");
+				write_nic_dword(dev, PhyAddr, 0x2697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0x96a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfb85);	mdelay(1);
+				break;
+
+			case 4: //m864dBm
+				DMESG("RTL8185B + 8225 Initial Gain State 4: -78 dBm \n");
+				write_nic_dword(dev, PhyAddr, 0x2697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xa6a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfb85);	mdelay(1);
+				break;
+
+			case 5: //m82dBm
+				DMESG("RTL8185B + 8225 Initial Gain State 5: -74 dBm \n");
+				write_nic_dword(dev, PhyAddr, 0x3697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xa6a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfb85);	mdelay(1);
+				break;
+
+			case 6: //m78dBm
+				DMESG("RTL8185B + 8225 Initial Gain State 6: -70 dBm \n");
+				write_nic_dword(dev, PhyAddr, 0x4697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xa6a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfb85);	mdelay(1);
+				break;
+
+			case 7: //m74dBm
+				DMESG("RTL8185B + 8225 Initial Gain State 7: -66 dBm \n");
+				write_nic_dword(dev, PhyAddr, 0x5697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xa6a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfb85);	mdelay(1);
+				break;
+
+			default:	//MP
+				DMESG("RTL8185B + 8225 Initial Gain State 1: -82 dBm (default)\n");
+				write_nic_dword(dev, PhyAddr, 0x2697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0x86a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfa85);	mdelay(1);
+				break;
+		}
+		break;
+#endif
+	case RF_ZEBRA4:
+		// Dynamic set initial gain, follow 87B
+		switch(priv->InitialGain)
+		{
+			case 1: //m861dBm
+				//DMESG("RTL8187 + 8225 Initial Gain State 1: -82 dBm \n");
+				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
+				//DMESG("RTL8187 + 8225 Initial Gain State 2: -82 dBm \n");
+				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
+				//DMESG("RTL8187 + 8225 Initial Gain State 3: -82 dBm \n");
+				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
+				//DMESG("RTL8187 + 8225 Initial Gain State 4: -78 dBm \n");
+				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
+				//DMESG("RTL8187 + 8225 Initial Gain State 5: -74 dBm \n");
+				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
+				//DMESG ("RTL8187 + 8225 Initial Gain State 6: -70 dBm \n");
+				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
+				//DMESG("RTL8187 + 8225 Initial Gain State 7: -66 dBm \n");
+				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:
+				//DMESG("RTL8187 + 8225 Initial Gain State 8:\n");
+				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
+				//DMESG("RTL8187 + 8225 Initial Gain State 1: -82 dBm (default)\n");
+				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;
+		}
+		break;
+
+
+	default:
+		DMESG("UpdateInitialGain(): unknown RFChipID: %#X\n", priv->rf_chip);
+		break;
+	}
+}
+#ifdef CONFIG_RTL818X_S
+//
+//	Description:
+//		Tx Power tracking mechanism routine on 87SE.
+// 	Created by Roger, 2007.12.11.
+//
+void
+InitTxPwrTracking87SE(
+	struct net_device *dev
+)
+{
+	//struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u32	u4bRfReg;
+
+	u4bRfReg = RF_ReadReg(dev, 0x02);
+
+	// Enable Thermal meter indication.
+	//printk("InitTxPwrTracking87SE(): Enable thermal meter indication, Write RF[0x02] = %#x", u4bRfReg|PWR_METER_EN);
+	RF_WriteReg(dev, 0x02, u4bRfReg|PWR_METER_EN);			mdelay(1);
+}
+
+#endif
+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
+	switch(priv->rf_chip)
+	{
+	case RF_ZEBRA2:
+	case RF_ZEBRA4:
+		ZEBRA_Config_85BASIC_HardCode( dev);
+		break;
+	}
+//{by amy 080312
+#ifdef CONFIG_RTL818X_S
+	// 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;
+		//printk("PhyConfig8185(): DIG is enabled, set default initial gain index to %d\n", priv->InitialGain);
+	}
+
+	//
+	// 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);
+
+#endif
+//by amy 080312}
+	priv->InitialGainBackUp= priv->InitialGain;
+	UpdateInitialGain(dev);
+
+	return;
+}
+
+
+
+
+void
+HwConfigureRTL8185(
+		struct net_device *dev
+		)
+{
+	//RTL8185_TODO: Determine Retrylimit, TxAGC, AutoRateFallback control.
+//	u8		bUNIVERSAL_CONTROL_RL = 1;
+        u8              bUNIVERSAL_CONTROL_RL = 0;
+
+	u8		bUNIVERSAL_CONTROL_AGC = 1;
+	u8		bUNIVERSAL_CONTROL_ANT = 1;
+	u8		bAUTO_RATE_FALLBACK_CTL = 1;
+	u8		val8;
+	//struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	//struct ieee80211_device *ieee = priv->ieee80211;
+      	//if(IS_WIRELESS_MODE_A(dev) || IS_WIRELESS_MODE_G(dev))
+//{by amy 080312	if((ieee->mode == IEEE_G)||(ieee->mode == IEEE_A))
+//	{
+//		write_nic_word(dev, BRSR, 0xffff);
+//	}
+//	else
+//	{
+//		write_nic_word(dev, BRSR, 0x000f);
+//	}
+//by amy 080312}
+        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.
+		//write_nic_word(dev, ARFR, 0x0fff); // set 1M ~ 54M
+//by amy
+#if 0
+		PlatformIOWrite2Byte(dev, ARFR, 0x0fff); 	// set 1M ~ 54M
+#endif
+#ifdef CONFIG_RTL818X_S
+	        // Aadded by Roger, 2007.11.15.
+	        PlatformIOWrite2Byte(dev, ARFR, 0x0fff); //set 1M ~ 54Mbps.
+#else
+		PlatformIOWrite2Byte(dev, ARFR, 0x0c00); //set 48Mbps, 54Mbps.
+                // By SD3 szuyi's request. by Roger, 2007.03.26.
+#endif
+//by amy
+	}
+	else
+	{
+	}
+	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);
+                }
+                //DbgPrint("MAC - 0x%x = 0x%x\n", u4bRegOffset, u4bRegValue);
+		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.
+	//PlatformIOWrite4Byte(dev, RFTiming, 0x00004001);
+//by amy
+#if 0
+	write_nic_dword(dev, RFTiming, 0x00004001);
+#endif
+#ifdef CONFIG_RTL818X_S
+	// 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, 0x00FE);//-edward
+	write_nic_word(dev, 0x37E, 0x00EC);//+edward
+#else
+       write_nic_dword(dev, RFTiming, 0x00004003);
+#endif
+       write_nic_byte(dev, 0x24E,0x01);
+//by amy
+
+}
+
+
+
+
+u8
+GetSupportedWirelessMode8185(
+	struct net_device *dev
+)
+{
+	u8			btSupportedWirelessMode = 0;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	switch(priv->rf_chip)
+	{
+	case RF_ZEBRA2:
+	case RF_ZEBRA4:
+		btSupportedWirelessMode = (WIRELESS_MODE_B | WIRELESS_MODE_G);
+		break;
+	default:
+		btSupportedWirelessMode = WIRELESS_MODE_B;
+		break;
+	}
+
+	return btSupportedWirelessMode;
+}
+
+void
+ActUpdateChannelAccessSetting(
+	struct net_device *dev,
+	WIRELESS_MODE			WirelessMode,
+	PCHANNEL_ACCESS_SETTING	ChnlAccessSetting
+	)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+	AC_CODING	eACI;
+	AC_PARAM	AcParam;
+	//PSTA_QOS	pStaQos = Adapter->MgntInfo.pStaQos;
+	u8	bFollowLegacySetting = 0;
+	u8   u1bAIFS;
+
+	//
+	// <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.
+	//
+	ChnlAccessSetting->SIFS_Timer = 0x22; // Suggested by Jong, 2005.12.08.
+	ChnlAccessSetting->DIFS_Timer = 0x1C; // 2006.06.02, by rcnjko.
+	ChnlAccessSetting->SlotTimeTimer = 9; // 2006.06.02, by rcnjko.
+	ChnlAccessSetting->EIFS_Timer = 0x5B; // Suggested by wcchu, it is the default value of EIFS register, 2005.12.08.
+	ChnlAccessSetting->CWminIndex = 3; // 2006.06.02, by rcnjko.
+	ChnlAccessSetting->CWmaxIndex = 7; // 2006.06.02, by rcnjko.
+
+	write_nic_byte(dev, SIFS, ChnlAccessSetting->SIFS_Timer);
+	//Adapter->HalFunc.SetHwRegHandler( Adapter, HW_VAR_SLOT_TIME, &ChnlAccessSetting->SlotTimeTimer );	// Rewrited from directly use PlatformEFIOWrite1Byte(), by Annie, 2006-03-29.
+	write_nic_byte(dev, SLOT, ChnlAccessSetting->SlotTimeTimer);	// Rewrited from directly use PlatformEFIOWrite1Byte(), by Annie, 2006-03-29.
+
+	u1bAIFS = aSifsTime + (2 * ChnlAccessSetting->SlotTimeTimer );
+
+	//write_nic_byte(dev, AC_VO_PARAM, u1bAIFS);
+	//write_nic_byte(dev, AC_VI_PARAM, u1bAIFS);
+	//write_nic_byte(dev, AC_BE_PARAM, u1bAIFS);
+	//write_nic_byte(dev, AC_BK_PARAM, u1bAIFS);
+
+	write_nic_byte(dev, EIFS, ChnlAccessSetting->EIFS_Timer);
+
+	write_nic_byte(dev, AckTimeOutReg, 0x5B); // <RJ_EXPR_QOS> Suggested by wcchu, it is the default value of EIFS register, 2005.12.08.
+
+#ifdef TODO
+	// <RJ_TODO_NOW_8185B> Update ECWmin/ECWmax, AIFS, TXOP Limit of each AC to the value defined by SPEC.
+	if( pStaQos->CurrentQosMode > QOS_DISABLE )
+	{ // QoS mode.
+		if(pStaQos->QBssWirelessMode == WirelessMode)
+		{
+			// Follow AC Parameters of the QBSS.
+			for(eACI = 0; eACI < AC_MAX; eACI++)
+			{
+				Adapter->HalFunc.SetHwRegHandler(Adapter, HW_VAR_AC_PARAM, (pu1Byte)(&(pStaQos->WMMParamEle.AcParam[eACI])) );
+			}
+		}
+		else
+		{
+			// Follow Default WMM AC Parameters.
+			bFollowLegacySetting = 1;
+		}
+	}
+	else
+#endif
+	{ // Legacy 802.11.
+		bFollowLegacySetting = 1;
+
+	}
+
+	// this setting is copied from rtl8187B.  xiong-2006-11-13
+	if(bFollowLegacySetting)
+	{
+
+
+		//
+		// Follow 802.11 seeting to AC parameter, all AC shall use the same parameter.
+		// 2005.12.01, by rcnjko.
+		//
+		AcParam.longData = 0;
+		AcParam.f.AciAifsn.f.AIFSN = 2; // Follow 802.11 DIFS.
+		AcParam.f.AciAifsn.f.ACM = 0;
+		AcParam.f.Ecw.f.ECWmin = ChnlAccessSetting->CWminIndex; // Follow 802.11 CWmin.
+		AcParam.f.Ecw.f.ECWmax = ChnlAccessSetting->CWmaxIndex; // Follow 802.11 CWmax.
+		AcParam.f.TXOPLimit = 0;
+
+		//lzm reserved 080826
+#if 1
+#ifdef THOMAS_TURBO
+		// For turbo mode setting. port from 87B by Isaiah 2008-08-01
+		if( ieee->current_network.Turbo_Enable == 1 )
+			AcParam.f.TXOPLimit = 0x01FF;
+#endif
+		// For 87SE with Intel 4965  Ad-Hoc mode have poor throughput (19MB)
+		if (ieee->iw_mode == IW_MODE_ADHOC)
+			AcParam.f.TXOPLimit = 0x0020;
+#endif
+
+		for(eACI = 0; eACI < AC_MAX; eACI++)
+		{
+			AcParam.f.AciAifsn.f.ACI = (u8)eACI;
+			{
+				PAC_PARAM	pAcParam = (PAC_PARAM)(&AcParam);
+				AC_CODING	eACI;
+				u8		u1bAIFS;
+				u32		u4bAcParam;
+
+				// Retrive paramters to udpate.
+				eACI = pAcParam->f.AciAifsn.f.ACI;
+				u1bAIFS = pAcParam->f.AciAifsn.f.AIFSN * ChnlAccessSetting->SlotTimeTimer + aSifsTime;
+				u4bAcParam = (	(((u32)(pAcParam->f.TXOPLimit)) << AC_PARAM_TXOP_LIMIT_OFFSET)	|
+						(((u32)(pAcParam->f.Ecw.f.ECWmax)) << AC_PARAM_ECW_MAX_OFFSET)	|
+						(((u32)(pAcParam->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);
+						break;
+
+					case AC0_BE:
+						//write_nic_dword(dev, AC_BE_PARAM, u4bAcParam);
+						break;
+
+					case AC2_VI:
+						//write_nic_dword(dev, AC_VI_PARAM, u4bAcParam);
+						break;
+
+					case AC3_VO:
+						//write_nic_dword(dev, AC_VO_PARAM, u4bAcParam);
+						break;
+
+					default:
+						DMESGW( "SetHwReg8185(): invalid ACI: %d !\n", eACI);
+						break;
+				}
+
+				// Cehck ACM bit.
+				// If it is set, immediately set ACM control bit to downgrading AC for passing WMM testplan. Annie, 2005-12-13.
+				//write_nic_byte(dev, ACM_CONTROL, pAcParam->f.AciAifsn);
+				{
+					PACI_AIFSN	pAciAifsn = (PACI_AIFSN)(&pAcParam->f.AciAifsn);
+					AC_CODING	eACI = pAciAifsn->f.ACI;
+
+					//modified Joseph
+					//for 8187B AsynIORead issue
+#ifdef TODO
+					u8	AcmCtrl = pHalData->AcmControl;
+#else
+					u8	AcmCtrl = 0;
+#endif
+					if( pAciAifsn->f.ACM )
+					{ // ACM bit is 1.
+						switch(eACI)
+						{
+							case AC0_BE:
+								AcmCtrl |= (BEQ_ACM_EN|BEQ_ACM_CTL|ACM_HW_EN);	// or 0x21
+								break;
+
+							case AC2_VI:
+								AcmCtrl |= (VIQ_ACM_EN|VIQ_ACM_CTL|ACM_HW_EN);	// or 0x42
+								break;
+
+							case AC3_VO:
+								AcmCtrl |= (VOQ_ACM_EN|VOQ_ACM_CTL|ACM_HW_EN);	// or 0x84
+								break;
+
+							default:
+								DMESGW("SetHwReg8185(): [HW_VAR_ACM_CTRL] ACM set failed: eACI is %d\n", eACI );
+								break;
+						}
+					}
+					else
+					{ // ACM bit is 0.
+						switch(eACI)
+						{
+							case AC0_BE:
+								AcmCtrl &= ( (~BEQ_ACM_EN) & (~BEQ_ACM_CTL) & (~ACM_HW_EN) );	// and 0xDE
+								break;
+
+							case AC2_VI:
+								AcmCtrl &= ( (~VIQ_ACM_EN) & (~VIQ_ACM_CTL) & (~ACM_HW_EN) );	// and 0xBD
+								break;
+
+							case AC3_VO:
+								AcmCtrl &= ( (~VOQ_ACM_EN) & (~VOQ_ACM_CTL) & (~ACM_HW_EN) );	// and 0x7B
+								break;
+
+							default:
+								break;
+						}
+					}
+
+					//printk(KERN_WARNING "SetHwReg8185(): [HW_VAR_ACM_CTRL] Write 0x%X\n", AcmCtrl);
+
+#ifdef TO_DO
+					pHalData->AcmControl = AcmCtrl;
+#endif
+					//write_nic_byte(dev, ACM_CONTROL, AcmCtrl);
+					write_nic_byte(dev, ACM_CONTROL, 0);
+				}
+			}
+		}
+
+
+	}
+}
+
+void
+ActSetWirelessMode8185(
+	struct net_device *dev,
+	u8				btWirelessMode
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+	//PMGNT_INFO		pMgntInfo = &(Adapter->MgntInfo);
+	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 swtich 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.
+	switch(priv->rf_chip)
+	{
+	case RF_ZEBRA2:
+	case RF_ZEBRA4:
+		{
+			// Nothing to do for Zebra to switch band.
+			// Update current wireless mode if we swtich to specified band successfully.
+			ieee->mode = (WIRELESS_MODE)btWirelessMode;
+		}
+		break;
+
+	default:
+		DMESGW("ActSetWirelessMode8185(): unsupported RF: 0x%X !!!\n", priv->rf_chip);
+		break;
+	}
+
+	// 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);
+}
+//by amy for power save
+void
+DrvIFIndicateDisassociation(
+	struct net_device *dev,
+	u16			reason
+	)
+{
+	//printk("==> DrvIFIndicateDisassociation()\n");
+
+	// nothing is needed after disassociation request.
+
+	//printk("<== DrvIFIndicateDisassociation()\n");
+}
+void
+MgntDisconnectIBSS(
+	struct net_device *dev
+)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u8			i;
+
+	//printk("XXXXXXXXXX MgntDisconnect IBSS\n");
+
+	DrvIFIndicateDisassociation(dev, unspec_reason);
+
+//	PlatformZeroMemory( pMgntInfo->Bssid, 6 );
+	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 untill 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
+//	Adapter->HalFunc.SetTxDescOWNHandler(Adapter, BEACON_QUEUE, 0, 0);
+	ieee80211_stop_send_beacons(priv->ieee80211);
+
+	priv->ieee80211->link_change(dev);
+	notify_wx_assoc_event(priv->ieee80211);
+
+	// Stop SW Beacon.Use hw beacon so do not need to do so.by amy
+#if 0
+	if(pMgntInfo->bEnableSwBeaconTimer)
+	{
+		// SwBeaconTimer will stop if pMgntInfo->mIbss==FALSE, see SwBeaconCallback() for details.
+// comment out by haich, 2007.10.01
+//#if DEV_BUS_TYPE==USB_INTERFACE
+		PlatformCancelTimer( Adapter, &pMgntInfo->SwBeaconTimer);
+//#endif
+	}
+#endif
+
+//		MgntIndicateMediaStatus( Adapter, RT_MEDIA_DISCONNECT, GENERAL_INDICATE );
+
+}
+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.
+		//ShuChen TODO: What to do when disassociate.
+		DrvIFIndicateDisassociation(dev, unspec_reason);
+
+
+	//	pMgntInfo->AsocTimestamp = 0;
+		for(i=0;i<6;i++)  priv->ieee80211->current_network.bssid[i] = 0x22;
+//		pMgntInfo->mBrates.Length = 0;
+//		Adapter->HalFunc.SetHwRegHandler( Adapter, HW_VAR_BASIC_RATE, (pu1Byte)(&pMgntInfo->mBrates) );
+
+		ieee80211_disassociate(priv->ieee80211);
+
+
+	}
+
+}
+
+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
+//	SecClearAllKeys(Adapter);
+
+	// In WPA WPA2 need to Clear all key ... because new key will set after new handshaking.
+#ifdef TODO
+	if(   pMgntInfo->SecurityInfo.AuthMode > RT_802_11AuthModeAutoSwitch ||
+		(pMgntInfo->bAPSuportCCKM && pMgntInfo->bCCX8021xenable) )  // In CCKM mode will Clear key
+	{
+		SecClearAllKeys(Adapter);
+		RT_TRACE(COMP_SEC, DBG_LOUD,("======>CCKM clear key..."))
+	}
+#endif
+	// 2004.10.11, by rcnjko.
+	//MlmeDisassociateRequest( Adapter, pMgntInfo->Bssid, disas_lv_ss );
+	MlmeDisassociateRequest( dev, priv->ieee80211->current_network.bssid, asRsn );
+
+	priv->ieee80211->state = IEEE80211_NOLINK;
+//	pMgntInfo->AsocTimestamp = 0;
+}
+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.
+	//
+#ifdef TODO
+	if(pMgntInfo->mPss != eAwake)
+	{
+		//
+		// Using AwkaeTimer to prevent mismatch ps state.
+		// In the timer the state will be changed according to the RF is being awoke or not. By Bruce, 2007-10-31.
+		//
+		// PlatformScheduleWorkItem( &(pMgntInfo->AwakeWorkItem) );
+		PlatformSetTimer( Adapter, &(pMgntInfo->AwakeTimer), 0 );
+	}
+#endif
+
+	// Indication of disassociation event.
+	//DrvIFIndicateDisassociation(Adapter, asRsn);
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(priv->ieee80211))
+		Dot11d_Reset(priv->ieee80211);
+#endif
+	// In adhoc mode, update beacon frame.
+	if( priv->ieee80211->state == IEEE80211_LINKED )
+	{
+		if( priv->ieee80211->iw_mode == IW_MODE_ADHOC )
+		{
+//			RT_TRACE(COMP_MLME, DBG_LOUD, ("MgntDisconnect() ===> MgntDisconnectIBSS\n"));
+			//printk("MgntDisconnect() ===> MgntDisconnectIBSS\n");
+			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.
+//			SecClearAllKeys(Adapter);
+
+//			RT_TRACE(COMP_MLME, DBG_LOUD, ("MgntDisconnect() ===> MgntDisconnectAP\n"));
+			//printk("MgntDisconnect() ===> MgntDisconnectAP\n");
+			MgntDisconnectAP(dev, asRsn);
+		}
+
+		// Inidicate Disconnect, 2005.02.23, by rcnjko.
+//		MgntIndicateMediaStatus( Adapter, RT_MEDIA_DISCONNECT, GENERAL_INDICATE);
+	}
+
+	return true;
+}
+//
+//	Description:
+//		Chang RF Power State.
+//		Note that, only MgntActSet_RF_State() is allowed to set HW_VAR_RF_STATE.
+//
+//	Assumption:
+//		PASSIVE LEVEL.
+//
+bool
+SetRFPowerState(
+	struct net_device *dev,
+	RT_RF_POWER_STATE	eRFPowerState
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	bool			bResult = false;
+
+//	printk("---------> SetRFPowerState(): eRFPowerState(%d)\n", eRFPowerState);
+	if(eRFPowerState == priv->eRFPowerState)
+	{
+//		printk("<--------- SetRFPowerState(): discard the request for eRFPowerState(%d) is the same.\n", eRFPowerState);
+		return bResult;
+	}
+
+	switch(priv->rf_chip)
+	{
+		case RF_ZEBRA2:
+		case RF_ZEBRA4:
+			 bResult = SetZebraRFPowerState8185(dev, eRFPowerState);
+			break;
+
+		default:
+			printk("SetRFPowerState8185(): unknown RFChipID: 0x%X!!!\n", priv->rf_chip);
+			break;;
+}
+//	printk("<--------- SetRFPowerState(): bResult(%d)\n", bResult);
+
+	return bResult;
+}
+void
+HalEnableRx8185Dummy(
+	struct net_device *dev
+	)
+{
+}
+void
+HalDisableRx8185Dummy(
+	struct net_device *dev
+	)
+{
+}
+
+bool
+MgntActSet_RF_State(
+	struct net_device *dev,
+	RT_RF_POWER_STATE	StateToSet,
+	u32	ChangeSource
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	bool				bActionAllowed = false;
+	bool				bConnectBySSID = false;
+	RT_RF_POWER_STATE 	rtState;
+	u16				RFWaitCounter = 0;
+	unsigned long flag;
+//	 printk("===>MgntActSet_RF_State(): StateToSet(%d), ChangeSource(0x%x)\n",StateToSet, ChangeSource);
+	//
+	// 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.
+	//
+#if 1
+	while(true)
+	{
+//		down(&priv->rf_state);
+		spin_lock_irqsave(&priv->rf_ps_lock,flag);
+		if(priv->RFChangeInProgress)
+		{
+//			printk("====================>haha111111111\n");
+//			up(&priv->rf_state);
+//			RT_TRACE(COMP_RF, DBG_LOUD, ("MgntActSet_RF_State(): RF Change in progress! Wait to set..StateToSet(%d).\n", StateToSet));
+			spin_unlock_irqrestore(&priv->rf_ps_lock,flag);
+			// Set RF after the previous action is done.
+			while(priv->RFChangeInProgress)
+			{
+				RFWaitCounter ++;
+//				RT_TRACE(COMP_RF, DBG_LOUD, ("MgntActSet_RF_State(): Wait 1 ms (%d times)...\n", RFWaitCounter));
+				udelay(1000); // 1 ms
+
+				// Wait too long, return FALSE to avoid to be stuck here.
+				if(RFWaitCounter > 1000) // 1sec
+				{
+//					RT_ASSERT(FALSE, ("MgntActSet_RF_State(): Wait too logn to set RF\n"));
+					printk("MgntActSet_RF_State(): Wait too long to set RF\n");
+					// TODO: Reset RF state?
+					return false;
+				}
+			}
+		}
+		else
+		{
+//			printk("========================>haha2\n");
+			priv->RFChangeInProgress = true;
+//			up(&priv->rf_state);
+			spin_unlock_irqrestore(&priv->rf_ps_lock,flag);
+			break;
+		}
+	}
+#endif
+	rtState = priv->eRFPowerState;
+
+
+	switch(StateToSet)
+	{
+	case eRfOn:
+		//
+		// 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 == eRfOff && ChangeSource >=RF_CHANGE_BY_HW && !priv->bInHctTest)
+			{
+				bConnectBySSID = true;
+			}
+		}
+		else
+//			RT_TRACE(COMP_RF, DBG_LOUD, ("MgntActSet_RF_State - eRfon reject pMgntInfo->RfOffReason= 0x%x, ChangeSource=0x%X\n", pMgntInfo->RfOffReason, ChangeSource));
+			;
+		break;
+
+	case eRfOff:
+		 // 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.
+				// Revised by Roger, 2007.12.04.
+				//
+				MgntDisconnect( dev, disas_lv_ss );
+
+				// Clear content of bssDesc[] and bssDesc4Query[] to avoid reporting old bss to UI.
+				// 2007.05.28, by shien chang.
+//				PlatformZeroMemory( pMgntInfo->bssDesc, sizeof(RT_WLAN_BSS)*MAX_BSS_DESC );
+//				pMgntInfo->NumBssDesc = 0;
+//				PlatformZeroMemory( pMgntInfo->bssDesc4Query, sizeof(RT_WLAN_BSS)*MAX_BSS_DESC );
+//				pMgntInfo->NumBssDesc4Query = 0;
+			}
+
+
+
+		priv->RfOffReason |= ChangeSource;
+		bActionAllowed = true;
+		break;
+
+	case eRfSleep:
+		priv->RfOffReason |= ChangeSource;
+		bActionAllowed = true;
+		break;
+
+	default:
+		break;
+	}
+
+	if(bActionAllowed)
+	{
+//		RT_TRACE(COMP_RF, DBG_LOUD, ("MgntActSet_RF_State(): Action is allowed.... StateToSet(%d), RfOffReason(%#X)\n", StateToSet, pMgntInfo->RfOffReason));
+                // Config HW to the specified mode.
+//		printk("MgntActSet_RF_State(): Action is allowed.... StateToSet(%d), RfOffReason(%#X)\n", StateToSet, priv->RfOffReason);
+		SetRFPowerState(dev, StateToSet);
+
+		// Turn on RF.
+		if(StateToSet == eRfOn)
+		{
+			HalEnableRx8185Dummy(dev);
+			if(bConnectBySSID)
+			{
+			// by amy not supported
+//				MgntActSet_802_11_SSID(Adapter, Adapter->MgntInfo.Ssid.Octet, Adapter->MgntInfo.Ssid.Length, TRUE );
+			}
+		}
+		// Turn off RF.
+		else if(StateToSet == eRfOff)
+		{
+			HalDisableRx8185Dummy(dev);
+		}
+	}
+	else
+	{
+	//	printk("MgntActSet_RF_State(): Action is rejected.... StateToSet(%d), ChangeSource(%#X), RfOffReason(%#X)\n", StateToSet, ChangeSource, priv->RfOffReason);
+	}
+
+	// Release RF spinlock
+//	down(&priv->rf_state);
+	spin_lock_irqsave(&priv->rf_ps_lock,flag);
+	priv->RFChangeInProgress = false;
+//	up(&priv->rf_state);
+	spin_unlock_irqrestore(&priv->rf_ps_lock,flag);
+//	printk("<===MgntActSet_RF_State()\n");
+	return bActionAllowed;
+}
+void
+InactivePowerSave(
+	struct net_device *dev
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	//u8 index = 0;
+
+	//
+	// 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.
+	// By Bruce, 2007-12-25.
+	//
+	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.
+	//
+#if 0
+	while( index < 4 )
+	{
+		if( ( pMgntInfo->SecurityInfo.PairwiseEncAlgorithm == WEP104_Encryption ) ||
+			(pMgntInfo->SecurityInfo.PairwiseEncAlgorithm == WEP40_Encryption) )
+		{
+			if( pMgntInfo->SecurityInfo.KeyLen[index] != 0)
+			pAdapter->HalFunc.SetKeyHandler(pAdapter, index, 0, FALSE, pMgntInfo->SecurityInfo.PairwiseEncAlgorithm, TRUE, FALSE);
+
+		}
+		index++;
+	}
+#endif
+	priv->bSwRfProcessing = false;
+}
+
+//
+//	Description:
+//		Enter the inactive power save mode. RF will be off
+//	2007.08.17, by shien chang.
+//
+void
+IPSEnter(
+	struct net_device *dev
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	RT_RF_POWER_STATE rtState;
+	//printk("==============================>enter IPS\n");
+	if (priv->bInactivePs)
+	{
+		rtState = priv->eRFPowerState;
+
+		//
+		// Added by Bruce, 2007-12-25.
+		// 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) Connectted (only disconnected can trigger IPS)
+		// (4) IBSS (send Beacon)
+		// (5) AP mode (send Beacon)
+		//
+		if (rtState == eRfOn && !priv->bSwRfProcessing
+			&& (priv->ieee80211->state != IEEE80211_LINKED ))
+		{
+	//		printk("IPSEnter(): Turn off RF.\n");
+			priv->eInactivePowerState = eRfOff;
+			InactivePowerSave(dev);
+		}
+	}
+//	printk("priv->eRFPowerState is %d\n",priv->eRFPowerState);
+}
+void
+IPSLeave(
+	struct net_device *dev
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	RT_RF_POWER_STATE rtState;
+	//printk("===================================>leave IPS\n");
+	if (priv->bInactivePs)
+	{
+		rtState = priv->eRFPowerState;
+		if ((rtState == eRfOff || rtState == eRfSleep) && (!priv->bSwRfProcessing) && priv->RfOffReason <= RF_CHANGE_BY_IPS)
+		{
+//			printk("IPSLeave(): Turn on RF.\n");
+			priv->eInactivePowerState = eRfOn;
+			InactivePowerSave(dev);
+		}
+	}
+//	printk("priv->eRFPowerState is %d\n",priv->eRFPowerState);
+}
+//by amy for power save
+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;
+	//int i;
+	u8 tmpu8;
+    	//u8 u1tmp,u2tmp;
+	u8 btCR9346;
+	u8 TmpU1b;
+	u8 btPSR;
+
+	//rtl8180_rtx_disable(dev);
+//{by amy 080312
+	write_nic_byte(dev,0x24e, (BIT5|BIT6|BIT0));
+//by amy 080312}
+	rtl8180_reset(dev);
+
+	priv->dma_poll_mask = 0;
+	priv->dma_poll_stop_mask = 0;
+
+	//rtl8180_beacon_tx_disable(dev);
+
+	HwConfigureRTL8185(dev);
+
+	write_nic_dword(dev, MAC0, ((u32*)dev->dev_addr)[0]);
+	write_nic_word(dev, MAC4, ((u32*)dev->dev_addr)[1] & 0xffff );
+
+	write_nic_byte(dev, MSR, read_nic_byte(dev, MSR) & 0xf3);	// default network type to 'No	Link'
+
+	//write_nic_byte(dev, BRSR, 0x0);		// Set BRSR= 1M
+
+	write_nic_word(dev, BcnItv, 100);
+	write_nic_word(dev, AtimWnd, 2);
+
+	//PlatformEFIOWrite2Byte(dev, FEMR, 0xFFFF);
+	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);
+//by amy
+//#ifdef CONFIG_RTL818X_S
+		// for jong required
+//	PlatformIOWrite2Byte(dev, RFSW_CTRL, 0x9a56);
+//#endif
+//by amy
+	//BT_QA_BOARD
+	//PlatformIOWrite2Byte(dev, RFSW_CTRL, 0x9a56);
+
+	//-----------------------------------------------------------------------------
+	// Set up PHY related.
+	//-----------------------------------------------------------------------------
+	// Enable Config3.PARAM_En to revise AnaaParm.
+	write_nic_byte(dev, CR9346, 0xc0);	// enable config register write
+//by amy
+	tmpu8 = read_nic_byte(dev, CONFIG3);
+#ifdef CONFIG_RTL818X_S
+	write_nic_byte(dev, CONFIG3, (tmpu8 |CONFIG3_PARM_En) );
+#else
+	write_nic_byte(dev, CONFIG3, (tmpu8 |CONFIG3_PARM_En | CONFIG3_CLKRUN_En) );
+#endif
+//by amy
+	// 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);
+//by amy
+#ifdef CONFIG_RTL818X_S
+	write_nic_word(dev, ANAPARAM3, 0x0010);
+#else
+      write_nic_byte(dev, ANAPARAM3, 0x00);
+#endif
+//by amy
+
+	write_nic_byte(dev, CONFIG3, tmpu8);
+	write_nic_byte(dev, CR9346, 0x00);
+//{by amy 080312 for led
+	// 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));
+//by amy 080312 for led}
+	// 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 reasonble
+	// 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 = (WIRELESS_MODE)InitWirelessMode;
+		}
+	}
+	else
+	{ // One of B, G, A.
+		InitWirelessMode = ieee->mode;
+	}
+//by amy for power save
+#ifdef ENABLE_IPS
+//	printk("initialize ENABLE_IPS\n");
+	priv->eRFPowerState = eRfOff;
+	priv->RfOffReason = 0;
+	{
+	//	u32 tmp2;
+	//	u32 tmp = jiffies;
+		MgntActSet_RF_State(dev, eRfOn, 0);
+	//	tmp2 = jiffies;
+	//	printk("rf on cost jiffies:%lx\n", (tmp2-tmp)*1000/HZ);
+	}
+//	DrvIFIndicateCurrentPhyStatus(priv);
+		//
+		// If inactive power mode is enabled, disable rf while in disconnected state.
+		// 2007.07.16, by shien chang.
+		//
+	if (priv->bInactivePs)
+	{
+	//	u32 tmp2;
+	//	u32 tmp = jiffies;
+		MgntActSet_RF_State(dev,eRfOff, RF_CHANGE_BY_IPS);
+	//	tmp2 = jiffies;
+	//	printk("rf off cost jiffies:%lx\n", (tmp2-tmp)*1000/HZ);
+
+	}
+#endif
+//	IPSEnter(dev);
+//by amy for power save
+#ifdef TODO
+	// Turn off RF if necessary. 2005.08.23, by rcnjko.
+	// We shall turn off RF after setting CMDR, otherwise,
+	// RF will be turnned on after we enable MAC Tx/Rx.
+	if(Adapter->MgntInfo.RegRfOff == TRUE)
+	{
+		SetRFPowerState8185(Adapter, RF_OFF);
+	}
+	else
+	{
+		SetRFPowerState8185(Adapter, RF_ON);
+	}
+#endif
+
+/*   //these is equal with above TODO.
+	write_nic_byte(dev, CR9346, 0xc0);	// enable config register write
+	write_nic_byte(dev, CONFIG3, read_nic_byte(dev, CONFIG3) | CONFIG3_PARM_En);
+	RF_WriteReg(dev, 0x4, 0x9FF);
+	write_nic_dword(dev, ANAPARAM2, ANAPARM2_ASIC_ON);
+	write_nic_dword(dev, ANAPARAM, ANAPARM_ASIC_ON);
+	write_nic_byte(dev, CONFIG3, (read_nic_byte(dev, CONFIG3)&(~CONFIG3_PARM_En)));
+	write_nic_byte(dev, CR9346, 0x00);
+*/
+
+	ActSetWirelessMode8185(dev, (u8)(InitWirelessMode));
+
+	//-----------------------------------------------------------------------------
+
+	rtl8185b_irq_enable(dev);
+
+	netif_start_queue(dev);
+
+ }
+
+
+void rtl8185b_rx_enable(struct net_device *dev)
+{
+	u8 cmd;
+	//u32 rxconf;
+	/* for now we accept data, management & ctl frame*/
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+#if 0
+	rxconf=read_nic_dword(dev,RX_CONF);
+	rxconf = rxconf &~ MAC_FILTER_MASK;
+	rxconf = rxconf | (1<<ACCEPT_MNG_FRAME_SHIFT);
+	rxconf = rxconf | (1<<ACCEPT_DATA_FRAME_SHIFT);
+	rxconf = rxconf | (1<<ACCEPT_BCAST_FRAME_SHIFT);
+	rxconf = rxconf | (1<<ACCEPT_MCAST_FRAME_SHIFT);
+//	rxconf = rxconf | (1<<ACCEPT_CRCERR_FRAME_SHIFT);
+	if (dev->flags & IFF_PROMISC) DMESG ("NIC in promisc mode");
+
+	if(priv->ieee80211->iw_mode == IW_MODE_MONITOR || \
+	   dev->flags & IFF_PROMISC){
+		rxconf = rxconf | (1<<ACCEPT_ALLMAC_FRAME_SHIFT);
+	}else{
+		rxconf = rxconf | (1<<ACCEPT_NICMAC_FRAME_SHIFT);
+		if(priv->card_8185 == 0)
+			rxconf = rxconf | (1<<RX_CHECK_BSSID_SHIFT);
+	}
+
+	/*if(priv->ieee80211->iw_mode == IW_MODE_MASTER){
+		rxconf = rxconf | (1<<ACCEPT_ALLMAC_FRAME_SHIFT);
+		rxconf = rxconf | (1<<RX_CHECK_BSSID_SHIFT);
+	}*/
+
+	if(priv->ieee80211->iw_mode == IW_MODE_MONITOR){
+		rxconf = rxconf | (1<<ACCEPT_CTL_FRAME_SHIFT);
+		rxconf = rxconf | (1<<ACCEPT_ICVERR_FRAME_SHIFT);
+		rxconf = rxconf | (1<<ACCEPT_PWR_FRAME_SHIFT);
+	}
+
+	if( priv->crcmon == 1 && priv->ieee80211->iw_mode == IW_MODE_MONITOR)
+		rxconf = rxconf | (1<<ACCEPT_CRCERR_FRAME_SHIFT);
+
+	//if(!priv->card_8185){
+		rxconf = rxconf &~ RX_FIFO_THRESHOLD_MASK;
+		rxconf = rxconf | (RX_FIFO_THRESHOLD_NONE<<RX_FIFO_THRESHOLD_SHIFT);
+	//}
+
+	rxconf = rxconf | (1<<RX_AUTORESETPHY_SHIFT);
+	rxconf = rxconf &~ MAX_RX_DMA_MASK;
+	rxconf = rxconf | (MAX_RX_DMA_2048<<MAX_RX_DMA_SHIFT);
+
+	//if(!priv->card_8185)
+		rxconf = rxconf | RCR_ONLYERLPKT;
+
+	rxconf = rxconf &~ RCR_CS_MASK;
+	if(!priv->card_8185)
+		rxconf |= (priv->rcr_csense<<RCR_CS_SHIFT);
+//	rxconf &=~ 0xfff00000;
+//	rxconf |= 0x90100000;//9014f76f;
+	write_nic_dword(dev, RX_CONF, rxconf);
+#endif
+
+	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_MASTER){
+		rxconf = rxconf | (1<<ACCEPT_ALLMAC_FRAME_SHIFT);
+		rxconf = rxconf | (1<<RX_CHECK_BSSID_SHIFT);
+	}*/
+
+	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);
+
+#ifdef DEBUG_RX
+	DMESG("rxconf: %x %x",priv->ReceiveConfig ,read_nic_dword(dev,RCR));
+#endif
+	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 tx_agc_ctl;
+	u8 byte;
+	//u32 txconf;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+#if 0
+	txconf= read_nic_dword(dev,TX_CONF);
+	if(priv->card_8185){
+
+
+		byte = read_nic_byte(dev,CW_CONF);
+		byte &= ~(1<<CW_CONF_PERPACKET_CW_SHIFT);
+		byte &= ~(1<<CW_CONF_PERPACKET_RETRY_SHIFT);
+		write_nic_byte(dev, CW_CONF, byte);
+
+		tx_agc_ctl = read_nic_byte(dev, TX_AGC_CTL);
+		tx_agc_ctl &= ~(1<<TX_AGC_CTL_PERPACKET_GAIN_SHIFT);
+		tx_agc_ctl &= ~(1<<TX_AGC_CTL_PERPACKET_ANTSEL_SHIFT);
+		tx_agc_ctl |=(1<<TX_AGC_CTL_FEEDBACK_ANT);
+		write_nic_byte(dev, TX_AGC_CTL, tx_agc_ctl);
+		/*
+		write_nic_word(dev, 0x5e, 0x01);
+		force_pci_posting(dev);
+		mdelay(1);
+		write_nic_word(dev, 0xfe, 0x10);
+		force_pci_posting(dev);
+		mdelay(1);
+		write_nic_word(dev, 0x5e, 0x00);
+		force_pci_posting(dev);
+		mdelay(1);
+		*/
+		write_nic_byte(dev, 0xec, 0x3f); /* Disable early TX */
+	}
+
+	if(priv->card_8185){
+
+		txconf = txconf &~ (1<<TCR_PROBE_NOTIMESTAMP_SHIFT);
+
+	}else{
+
+		if(hwseqnum)
+			txconf= txconf &~ (1<<TX_CONF_HEADER_AUTOICREMENT_SHIFT);
+		else
+			txconf= txconf | (1<<TX_CONF_HEADER_AUTOICREMENT_SHIFT);
+	}
+
+	txconf = txconf &~ TX_LOOPBACK_MASK;
+	txconf = txconf | (TX_LOOPBACK_NONE <<TX_LOOPBACK_SHIFT);
+	txconf = txconf &~ TCR_DPRETRY_MASK;
+	txconf = txconf &~ TCR_RTSRETRY_MASK;
+	txconf = txconf | (priv->retry_data<<TX_DPRETRY_SHIFT);
+	txconf = txconf | (priv->retry_rts<<TX_RTSRETRY_SHIFT);
+	txconf = txconf &~ (1<<TX_NOCRC_SHIFT);
+
+	if(priv->card_8185){
+		if(priv->hw_plcp_len)
+			txconf = txconf &~ TCR_PLCP_LEN;
+		else
+			txconf = txconf | TCR_PLCP_LEN;
+	}else{
+		txconf = txconf &~ TCR_SAT;
+	}
+	txconf = txconf &~ TCR_MXDMA_MASK;
+	txconf = txconf | (TCR_MXDMA_2048<<TCR_MXDMA_SHIFT);
+	txconf = txconf | TCR_CWMIN;
+	txconf = txconf | TCR_DISCW;
+
+//	if(priv->ieee80211->hw_wep)
+//		txconf=txconf &~ (1<<TX_NOICV_SHIFT);
+//	else
+		txconf=txconf | (1<<TX_NOICV_SHIFT);
+
+	write_nic_dword(dev,TX_CONF,txconf);
+#endif
+
+	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);
+
+#ifdef DEBUG_TX
+	DMESG("txconf: %x %x",priv->TransmitConfig,read_nic_dword(dev,TCR));
+#endif
+
+	cmd=read_nic_byte(dev,CMD);
+	write_nic_byte(dev,CMD,cmd | (1<<CMD_TX_ENABLE_SHIFT));
+
+	//write_nic_dword(dev,TX_CONF,txconf);
+
+
+/*
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+	write_nic_byte(dev, TX_DMA_POLLING, priv->dma_poll_mask);
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+	*/
+}
+
+
+#endif
diff --git a/drivers/staging/slicoss/slic.h b/drivers/staging/slicoss/slic.h
index 0d5dc24c0b7d..a8ea59d7ea15 100644
--- a/drivers/staging/slicoss/slic.h
+++ b/drivers/staging/slicoss/slic.h
@@ -41,6 +41,40 @@
 #ifndef __SLIC_DRIVER_H__
 #define __SLIC_DRIVER_H__
 
+/* firmware stuff */
+#define OASIS_UCODE_VERS_STRING	"1.2"
+#define OASIS_UCODE_VERS_DATE  	"2006/03/27 15:10:37"
+#define OASIS_UCODE_HOSTIF_ID  	3
+
+static s32 ONumSections = 0x2;
+static u32 OSectionSize[] = {
+	0x00004000, 0x00010000,
+};
+
+static u32 OSectionStart[] = {
+	0x00000000, 0x00008000,
+};
+
+#define MOJAVE_UCODE_VERS_STRING	"1.2"
+#define MOJAVE_UCODE_VERS_DATE  	"2006/03/27 15:12:22"
+#define MOJAVE_UCODE_HOSTIF_ID  	3
+
+static s32 MNumSections = 0x2;
+static u32 MSectionSize[] =
+{
+	0x00008000, 0x00010000,
+};
+
+static u32 MSectionStart[] =
+{
+	0x00000000, 0x00008000,
+};
+
+#define GB_RCVUCODE_VERS_STRING	"1.2"
+#define GB_RCVUCODE_VERS_DATE  	"2006/03/27 15:12:15"
+static u32 OasisRcvUCodeLen = 512;
+static u32 GBRcvUCodeLen = 512;
+#define SECTION_SIZE 65536
 
 struct slic_spinlock {
 	spinlock_t	lock;
diff --git a/drivers/staging/slicoss/slicoss.c b/drivers/staging/slicoss/slicoss.c
index 00390362f10f..bf7da8f898ab 100644
--- a/drivers/staging/slicoss/slicoss.c
+++ b/drivers/staging/slicoss/slicoss.c
@@ -94,6 +94,7 @@
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 
+#include <linux/firmware.h>
 #include <linux/types.h>
 #include <linux/dma-mapping.h>
 #include <linux/mii.h>
@@ -105,15 +106,6 @@
 
 #include <linux/uaccess.h>
 #include "slicinc.h"
-#include "gbdownload.h"
-#include "gbrcvucode.h"
-#include "oasisrcvucode.h"
-
-#ifdef DEBUG_MICROCODE
-#include "oasisdbgdownload.h"
-#else
-#include "oasisdownload.h"
-#endif
 
 #if SLIC_DUMP_ENABLED
 #include "slicdump.h"
@@ -323,7 +315,7 @@ static void slic_init_adapter(struct net_device *netdev,
 	index, pslic_handle, adapter->pfree_slic_handles, pslic_handle->next);*/
 	adapter->pshmem = (struct slic_shmem *)
 					pci_alloc_consistent(adapter->pcidev,
-					sizeof(struct slic_shmem *),
+					sizeof(struct slic_shmem),
 					&adapter->
 					phys_shmem);
 /*
@@ -1431,7 +1423,7 @@ static void slic_init_cleanup(struct adapter *adapter)
 		DBG_MSG("adapter[%p] port %d pshmem[%p] FreeShmem ",
 			adapter, adapter->port, (void *) adapter->pshmem);
 		pci_free_consistent(adapter->pcidev,
-				    sizeof(struct slic_shmem *),
+				    sizeof(struct slic_shmem),
 				    adapter->pshmem, adapter->phys_shmem);
 		adapter->pshmem = NULL;
 		adapter->phys_shmem = (dma_addr_t) NULL;
@@ -2186,6 +2178,9 @@ static void slic_card_cleanup(struct sliccard *card)
 
 static int slic_card_download_gbrcv(struct adapter *adapter)
 {
+	const struct firmware *fw;
+	const char *file = "";
+	int ret;
 	__iomem struct slic_regs *slic_regs = adapter->slic_regs;
 	u32 codeaddr;
 	unsigned char *instruction = NULL;
@@ -2193,12 +2188,32 @@ static int slic_card_download_gbrcv(struct adapter *adapter)
 
 	switch (adapter->devid) {
 	case SLIC_2GB_DEVICE_ID:
-		instruction = (unsigned char *)&OasisRcvUCode[0];
-		rcvucodelen = OasisRcvUCodeLen;
+		file = "oasis_rcv.bin";
 		break;
 	case SLIC_1GB_DEVICE_ID:
-		instruction = (unsigned char *)&GBRcvUCode[0];
-		rcvucodelen = GBRcvUCodeLen;
+		file = "gb_rcv.bin";
+		break;
+	default:
+		ASSERT(0);
+		break;
+	}
+
+	ret = request_firmware(&fw, file, &adapter->pcidev->dev);
+	if (ret) {
+		printk(KERN_ERR "SLICOSS: Failed to load firmware %s\n", file);
+		return ret;
+	}
+
+	instruction = (unsigned char *)fw->data;
+	rcvucodelen = fw->size;
+	switch (adapter->devid) {
+	case SLIC_2GB_DEVICE_ID:
+		if (rcvucodelen != OasisRcvUCodeLen)
+			return -EINVAL;
+		break;
+	case SLIC_1GB_DEVICE_ID:
+		if (rcvucodelen != GBRcvUCodeLen)
+			return -EINVAL;
 		break;
 	default:
 		ASSERT(0);
@@ -2225,13 +2240,16 @@ static int slic_card_download_gbrcv(struct adapter *adapter)
 	}
 
 	/* download finished */
+	release_firmware(fw);
 	WRITE_REG(slic_regs->slic_rcv_wcs, SLIC_RCVWCS_FINISH, FLUSH);
-
 	return 0;
 }
 
 static int slic_card_download(struct adapter *adapter)
 {
+	const struct firmware *fw;
+	const char *file = "";
+	int ret;
 	u32 section;
 	int thissectionsize;
 	int codeaddr;
@@ -2255,6 +2273,7 @@ static int slic_card_download(struct adapter *adapter)
 	case SLIC_2GB_DEVICE_ID:
 /*      DBG_MSG ("slicoss: %s devid==SLIC_2GB_DEVICE_ID sections[%x]\n",
 	__func__, (uint) ONumSections); */
+		file = "slic_oasis.bin";
 		numsects = ONumSections;
 		for (i = 0; i < numsects; i++) {
 			sectsize[i] = OSectionSize[i];
@@ -2264,6 +2283,7 @@ static int slic_card_download(struct adapter *adapter)
 	case SLIC_1GB_DEVICE_ID:
 /*              DBG_MSG ("slicoss: %s devid==SLIC_1GB_DEVICE_ID sections[%x]\n",
 		__func__, (uint) MNumSections); */
+		file = "slic_mojave.bin";
 		numsects = MNumSections;
 		for (i = 0; i < numsects; i++) {
 			sectsize[i] = MSectionSize[i];
@@ -2274,26 +2294,33 @@ static int slic_card_download(struct adapter *adapter)
 		ASSERT(0);
 		break;
 	}
+	ret = request_firmware(&fw, file, &adapter->pcidev->dev);
+	if (ret) {
+		printk(KERN_ERR "SLICOSS: Failed to load firmware %s\n", file);
+		return ret;
+	}
 
 	ASSERT(numsects <= 3);
 
 	for (section = 0; section < numsects; section++) {
 		switch (adapter->devid) {
 		case SLIC_2GB_DEVICE_ID:
-			instruction = (u32 *) &OasisUCode[section][0];
+			instruction = (u32 *)(fw->data + (SECTION_SIZE *
+								section));
 			baseaddress = sectstart[section];
 			thissectionsize = sectsize[section] >> 3;
 			lastinstruct =
-			    (u32 *) &OasisUCode[section][sectsize[section] -
-							     8];
+			    (u32 *)(fw->data + (SECTION_SIZE * section) +
+					sectsize[section] - 8);
 			break;
 		case SLIC_1GB_DEVICE_ID:
-			instruction = (u32 *) &MojaveUCode[section][0];
+			instruction = (u32 *)(fw->data + (SECTION_SIZE *
+								section));
 			baseaddress = sectstart[section];
 			thissectionsize = sectsize[section] >> 3;
 			lastinstruct =
-			    (u32 *) &MojaveUCode[section][sectsize[section]
-							      - 8];
+			    (u32 *)(fw->data + (SECTION_SIZE * section) +
+					sectsize[section] - 8);
 			break;
 		default:
 			ASSERT(0);
@@ -2329,10 +2356,12 @@ static int slic_card_download(struct adapter *adapter)
 	for (section = 0; section < numsects; section++) {
 		switch (adapter->devid) {
 		case SLIC_2GB_DEVICE_ID:
-			instruction = (u32 *)&OasisUCode[section][0];
+			instruction = (u32 *)fw->data + (SECTION_SIZE *
+								section);
 			break;
 		case SLIC_1GB_DEVICE_ID:
-			instruction = (u32 *)&MojaveUCode[section][0];
+			instruction = (u32 *)fw->data + (SECTION_SIZE *
+								section);
 			break;
 		default:
 			ASSERT(0);
@@ -2374,13 +2403,13 @@ static int slic_card_download(struct adapter *adapter)
 				    thissectionsize[%x] failure[%x]\n",
 				     __func__, codeaddr, thissectionsize,
 				     failure);
-
+				release_firmware(fw);
 				return -EIO;
 			}
 		}
 	}
 /*    DBG_MSG ("slicoss: Compare done\n");*/
-
+	release_firmware(fw);
 	/* Everything OK, kick off the card */
 	mdelay(10);
 	WRITE_REG(slic_regs->slic_wcs, SLIC_WCS_START, FLUSH);
@@ -2832,9 +2861,8 @@ static u32 slic_card_locate(struct adapter *adapter)
 	}
 	if (!physcard) {
 		/* no structure allocated for this physical card yet */
-		physcard = kmalloc(sizeof(struct physcard *), GFP_ATOMIC);
+		physcard = kzalloc(sizeof(struct physcard), GFP_ATOMIC);
 		ASSERT(physcard);
-		memset(physcard, 0, sizeof(struct physcard *));
 
 		DBG_MSG
 		    ("\n%s Allocate a PHYSICALcard:\n    PHYSICAL_Card[%p]\n\
diff --git a/drivers/staging/sxg/README b/drivers/staging/sxg/README
index d514d1848803..e42f344ea5fa 100644
--- a/drivers/staging/sxg/README
+++ b/drivers/staging/sxg/README
@@ -2,8 +2,6 @@ This is the rough cut at a driver for the Alacritech SLIC Technology
 Non-Accelerated 10Gbe network driver.
 
 TODO:
-	- lindent the code
-	- remove typedefs
 	- remove wrappers
 	- checkpatch.pl cleanups
 	- new functionality that the card needs
diff --git a/drivers/staging/sxg/sxg.c b/drivers/staging/sxg/sxg.c
index 5272a18e2043..1e0cfcd7f0f3 100644
--- a/drivers/staging/sxg/sxg.c
+++ b/drivers/staging/sxg/sxg.c
@@ -80,13 +80,13 @@
 #include "sxgphycode.h"
 #include "saharadbgdownload.h"
 
-static int sxg_allocate_buffer_memory(p_adapter_t adapter, u32 Size,
-				      SXG_BUFFER_TYPE BufferType);
-static void sxg_allocate_rcvblock_complete(p_adapter_t adapter, void *RcvBlock,
+static int sxg_allocate_buffer_memory(struct adapter_t *adapter, u32 Size,
+				      enum SXG_BUFFER_TYPE BufferType);
+static void sxg_allocate_rcvblock_complete(struct adapter_t *adapter, void *RcvBlock,
 					   dma_addr_t PhysicalAddress,
 					   u32 Length);
-static void sxg_allocate_sgl_buffer_complete(p_adapter_t adapter,
-					     PSXG_SCATTER_GATHER SxgSgl,
+static void sxg_allocate_sgl_buffer_complete(struct adapter_t *adapter,
+					     struct SXG_SCATTER_GATHER *SxgSgl,
 					     dma_addr_t PhysicalAddress,
 					     u32 Length);
 
@@ -96,17 +96,17 @@ static int sxg_entry_open(p_net_device dev);
 static int sxg_entry_halt(p_net_device dev);
 static int sxg_ioctl(p_net_device dev, struct ifreq *rq, int cmd);
 static int sxg_send_packets(struct sk_buff *skb, p_net_device dev);
-static int sxg_transmit_packet(p_adapter_t adapter, struct sk_buff *skb);
-static void sxg_dumb_sgl(PSCATTER_GATHER_LIST pSgl, PSXG_SCATTER_GATHER SxgSgl);
-
-static void sxg_handle_interrupt(p_adapter_t adapter);
-static int sxg_process_isr(p_adapter_t adapter, u32 MessageId);
-static u32 sxg_process_event_queue(p_adapter_t adapter, u32 RssId);
-static void sxg_complete_slow_send(p_adapter_t adapter);
-static struct sk_buff *sxg_slow_receive(p_adapter_t adapter, PSXG_EVENT Event);
-static void sxg_process_rcv_error(p_adapter_t adapter, u32 ErrorStatus);
-static bool sxg_mac_filter(p_adapter_t adapter,
-			   p_ether_header EtherHdr, ushort length);
+static int sxg_transmit_packet(struct adapter_t *adapter, struct sk_buff *skb);
+static void sxg_dumb_sgl(struct SCATTER_GATHER_LIST *pSgl, struct SXG_SCATTER_GATHER *SxgSgl);
+
+static void sxg_handle_interrupt(struct adapter_t *adapter);
+static int sxg_process_isr(struct adapter_t *adapter, u32 MessageId);
+static u32 sxg_process_event_queue(struct adapter_t *adapter, u32 RssId);
+static void sxg_complete_slow_send(struct adapter_t *adapter);
+static struct sk_buff *sxg_slow_receive(struct adapter_t *adapter, struct SXG_EVENT *Event);
+static void sxg_process_rcv_error(struct adapter_t *adapter, u32 ErrorStatus);
+static bool sxg_mac_filter(struct adapter_t *adapter,
+			   struct ether_header *EtherHdr, ushort length);
 
 #if SLIC_GET_STATS_ENABLED
 static struct net_device_stats *sxg_get_stats(p_net_device dev);
@@ -119,22 +119,22 @@ static int sxg_mac_set_address(p_net_device dev, void *ptr);
 static void sxg_mcast_set_list(p_net_device dev);
 #endif
 
-static void sxg_adapter_set_hwaddr(p_adapter_t adapter);
+static void sxg_adapter_set_hwaddr(struct adapter_t *adapter);
 
-static void sxg_unmap_mmio_space(p_adapter_t adapter);
+static void sxg_unmap_mmio_space(struct adapter_t *adapter);
 
-static int sxg_initialize_adapter(p_adapter_t adapter);
-static void sxg_stock_rcv_buffers(p_adapter_t adapter);
-static void sxg_complete_descriptor_blocks(p_adapter_t adapter,
+static int sxg_initialize_adapter(struct adapter_t *adapter);
+static void sxg_stock_rcv_buffers(struct adapter_t *adapter);
+static void sxg_complete_descriptor_blocks(struct adapter_t *adapter,
 					   unsigned char Index);
-static int sxg_initialize_link(p_adapter_t adapter);
-static int sxg_phy_init(p_adapter_t adapter);
-static void sxg_link_event(p_adapter_t adapter);
-static SXG_LINK_STATE sxg_get_link_state(p_adapter_t adapter);
-static void sxg_link_state(p_adapter_t adapter, SXG_LINK_STATE LinkState);
-static int sxg_write_mdio_reg(p_adapter_t adapter,
+static int sxg_initialize_link(struct adapter_t *adapter);
+static int sxg_phy_init(struct adapter_t *adapter);
+static void sxg_link_event(struct adapter_t *adapter);
+static enum SXG_LINK_STATE sxg_get_link_state(struct adapter_t *adapter);
+static void sxg_link_state(struct adapter_t *adapter, enum SXG_LINK_STATE LinkState);
+static int sxg_write_mdio_reg(struct adapter_t *adapter,
 			      u32 DevAddr, u32 RegAddr, u32 Value);
-static int sxg_read_mdio_reg(p_adapter_t adapter,
+static int sxg_read_mdio_reg(struct adapter_t *adapter,
 			     u32 DevAddr, u32 RegAddr, u32 *pValue);
 
 static unsigned int sxg_first_init = 1;
@@ -145,7 +145,7 @@ static int sxg_debug = 1;
 static int debug = -1;
 static p_net_device head_netdevice = NULL;
 
-static sxgbase_driver_t sxg_global = {
+static struct sxgbase_driver_t sxg_global = {
 	.dynamic_intagg = 1,
 };
 static int intagg_delay = 100;
@@ -186,7 +186,7 @@ static inline void sxg_reg32_write(void __iomem *reg, u32 value, bool flush)
 		mb();
 }
 
-static inline void sxg_reg64_write(p_adapter_t adapter, void __iomem *reg,
+static inline void sxg_reg64_write(struct adapter_t *adapter, void __iomem *reg,
 				   u64 value, u32 cpu)
 {
 	u32 value_high = (u32) (value >> 32);
@@ -209,7 +209,7 @@ static void sxg_init_driver(void)
 	}
 }
 
-static void sxg_dbg_macaddrs(p_adapter_t adapter)
+static void sxg_dbg_macaddrs(struct adapter_t *adapter)
 {
 	DBG_ERROR("  (%s) curr %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n",
 		  adapter->netdev->name, adapter->currmacaddr[0],
@@ -225,12 +225,12 @@ static void sxg_dbg_macaddrs(p_adapter_t adapter)
 }
 
 /* SXG Globals */
-static SXG_DRIVER SxgDriver;
+static struct SXG_DRIVER SxgDriver;
 
 #ifdef  ATKDBG
-static sxg_trace_buffer_t LSxgTraceBuffer;
+static struct sxg_trace_buffer_t LSxgTraceBuffer;
 #endif /* ATKDBG */
-static sxg_trace_buffer_t *SxgTraceBuffer = NULL;
+static struct sxg_trace_buffer_t *SxgTraceBuffer = NULL;
 
 /*
  * sxg_download_microcode
@@ -244,9 +244,9 @@ static sxg_trace_buffer_t *SxgTraceBuffer = NULL;
  * Return
  *	int
  */
-static bool sxg_download_microcode(p_adapter_t adapter, SXG_UCODE_SEL UcodeSel)
+static bool sxg_download_microcode(struct adapter_t *adapter, enum SXG_UCODE_SEL UcodeSel)
 {
-	PSXG_HW_REGS HwRegs = adapter->HwRegs;
+	struct SXG_HW_REGS *HwRegs = adapter->HwRegs;
 	u32 Section;
 	u32 ThisSectionSize;
 	u32 *Instruction = NULL;
@@ -416,13 +416,13 @@ static bool sxg_download_microcode(p_adapter_t adapter, SXG_UCODE_SEL UcodeSel)
  * Return
  *	int
  */
-static int sxg_allocate_resources(p_adapter_t adapter)
+static int sxg_allocate_resources(struct adapter_t *adapter)
 {
 	int status;
 	u32 i;
 	u32 RssIds, IsrCount;
-/*      PSXG_XMT_RING                                   XmtRing; */
-/*      PSXG_RCV_RING                                   RcvRing; */
+/*      struct SXG_XMT_RING                                   *XmtRing; */
+/*      struct SXG_RCV_RING                                   *RcvRing; */
 
 	DBG_ERROR("%s ENTER\n", __func__);
 
@@ -461,13 +461,13 @@ static int sxg_allocate_resources(p_adapter_t adapter)
 
 	for (;;) {
 		DBG_ERROR("%s Allocate XmtRings size[%x]\n", __func__,
-			  (unsigned int)(sizeof(SXG_XMT_RING) * 1));
+			  (unsigned int)(sizeof(struct SXG_XMT_RING) * 1));
 
 		/* Start with big items first - receive and transmit rings.  At the moment */
 		/* I'm going to keep the ring size fixed and adjust the number of */
 		/* TCBs if we fail.  Later we might consider reducing the ring size as well.. */
 		adapter->XmtRings = pci_alloc_consistent(adapter->pcidev,
-							 sizeof(SXG_XMT_RING) *
+							 sizeof(struct SXG_XMT_RING) *
 							 1,
 							 &adapter->PXmtRings);
 		DBG_ERROR("%s XmtRings[%p]\n", __func__, adapter->XmtRings);
@@ -475,33 +475,33 @@ static int sxg_allocate_resources(p_adapter_t adapter)
 		if (!adapter->XmtRings) {
 			goto per_tcb_allocation_failed;
 		}
-		memset(adapter->XmtRings, 0, sizeof(SXG_XMT_RING) * 1);
+		memset(adapter->XmtRings, 0, sizeof(struct SXG_XMT_RING) * 1);
 
 		DBG_ERROR("%s Allocate RcvRings size[%x]\n", __func__,
-			  (unsigned int)(sizeof(SXG_RCV_RING) * 1));
+			  (unsigned int)(sizeof(struct SXG_RCV_RING) * 1));
 		adapter->RcvRings =
 		    pci_alloc_consistent(adapter->pcidev,
-					 sizeof(SXG_RCV_RING) * 1,
+					 sizeof(struct SXG_RCV_RING) * 1,
 					 &adapter->PRcvRings);
 		DBG_ERROR("%s RcvRings[%p]\n", __func__, adapter->RcvRings);
 		if (!adapter->RcvRings) {
 			goto per_tcb_allocation_failed;
 		}
-		memset(adapter->RcvRings, 0, sizeof(SXG_RCV_RING) * 1);
+		memset(adapter->RcvRings, 0, sizeof(struct SXG_RCV_RING) * 1);
 		break;
 
 	      per_tcb_allocation_failed:
 		/* an allocation failed.  Free any successful allocations. */
 		if (adapter->XmtRings) {
 			pci_free_consistent(adapter->pcidev,
-					    sizeof(SXG_XMT_RING) * 4096,
+					    sizeof(struct SXG_XMT_RING) * 4096,
 					    adapter->XmtRings,
 					    adapter->PXmtRings);
 			adapter->XmtRings = NULL;
 		}
 		if (adapter->RcvRings) {
 			pci_free_consistent(adapter->pcidev,
-					    sizeof(SXG_RCV_RING) * 4096,
+					    sizeof(struct SXG_RCV_RING) * 4096,
 					    adapter->RcvRings,
 					    adapter->PRcvRings);
 			adapter->RcvRings = NULL;
@@ -517,7 +517,7 @@ static int sxg_allocate_resources(p_adapter_t adapter)
 	/* Sanity check receive data structure format */
 	ASSERT((adapter->ReceiveBufferSize == SXG_RCV_DATA_BUFFER_SIZE) ||
 	       (adapter->ReceiveBufferSize == SXG_RCV_JUMBO_BUFFER_SIZE));
-	ASSERT(sizeof(SXG_RCV_DESCRIPTOR_BLOCK) ==
+	ASSERT(sizeof(struct SXG_RCV_DESCRIPTOR_BLOCK) ==
 	       SXG_RCV_DESCRIPTOR_BLOCK_SIZE);
 
 	/* Allocate receive data buffers.  We allocate a block of buffers and */
@@ -539,11 +539,11 @@ static int sxg_allocate_resources(p_adapter_t adapter)
 	}
 
 	DBG_ERROR("%s Allocate EventRings size[%x]\n", __func__,
-		  (unsigned int)(sizeof(SXG_EVENT_RING) * RssIds));
+		  (unsigned int)(sizeof(struct SXG_EVENT_RING) * RssIds));
 
 	/* Allocate event queues. */
 	adapter->EventRings = pci_alloc_consistent(adapter->pcidev,
-						   sizeof(SXG_EVENT_RING) *
+						   sizeof(struct SXG_EVENT_RING) *
 						   RssIds,
 						   &adapter->PEventRings);
 
@@ -554,7 +554,7 @@ static int sxg_allocate_resources(p_adapter_t adapter)
 		status = STATUS_RESOURCES;
 		goto per_tcb_allocation_failed;
 	}
-	memset(adapter->EventRings, 0, sizeof(SXG_EVENT_RING) * RssIds);
+	memset(adapter->EventRings, 0, sizeof(struct SXG_EVENT_RING) * RssIds);
 
 	DBG_ERROR("%s Allocate ISR size[%x]\n", __func__, IsrCount);
 	/* Allocate ISR */
@@ -628,7 +628,7 @@ static int sxg_entry_probe(struct pci_dev *pcidev,
 	static int did_version = 0;
 	int err;
 	struct net_device *netdev;
-	p_adapter_t adapter;
+	struct adapter_t *adapter;
 	void __iomem *memmapped_ioaddr;
 	u32 status = 0;
 	ulong mmio_start = 0;
@@ -681,7 +681,7 @@ static int sxg_entry_probe(struct pci_dev *pcidev,
 	pci_set_master(pcidev);
 
 	DBG_ERROR("call alloc_etherdev\n");
-	netdev = alloc_etherdev(sizeof(adapter_t));
+	netdev = alloc_etherdev(sizeof(struct adapter_t));
 	if (!netdev) {
 		err = -ENOMEM;
 		goto err_out_exit_sxg_probe;
@@ -871,7 +871,7 @@ static int sxg_entry_probe(struct pci_dev *pcidev,
  * Return Value:
  * 	None.
  */
-static void sxg_disable_interrupt(p_adapter_t adapter)
+static void sxg_disable_interrupt(struct adapter_t *adapter)
 {
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "DisIntr",
 		  adapter, adapter->InterruptsEnabled, 0, 0);
@@ -902,7 +902,7 @@ static void sxg_disable_interrupt(p_adapter_t adapter)
  * Return Value:
  * 	None.
  */
-static void sxg_enable_interrupt(p_adapter_t adapter)
+static void sxg_enable_interrupt(struct adapter_t *adapter)
 {
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "EnIntr",
 		  adapter, adapter->InterruptsEnabled, 0, 0);
@@ -935,7 +935,7 @@ static void sxg_enable_interrupt(p_adapter_t adapter)
 static irqreturn_t sxg_isr(int irq, void *dev_id)
 {
 	p_net_device dev = (p_net_device) dev_id;
-	p_adapter_t adapter = (p_adapter_t) netdev_priv(dev);
+	struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev);
 /*      u32                 CpuMask = 0, i; */
 
 	adapter->Stats.NumInts++;
@@ -963,8 +963,8 @@ static irqreturn_t sxg_isr(int irq, void *dev_id)
 		for (i = 0;
 		     i < adapter->RssSystemInfo->ProcessorInfo.RssCpuCount;
 		     i++) {
-			PSXG_EVENT_RING EventRing = &adapter->EventRings[i];
-			PSXG_EVENT Event =
+			struct XG_EVENT_RING *EventRing = &adapter->EventRings[i];
+			struct SXG_EVENT *Event =
 			    &EventRing->Ring[adapter->NextEvent[i]];
 			unsigned char Cpu =
 			    adapter->RssSystemInfo->RssIdToCpu[i];
@@ -992,7 +992,7 @@ static irqreturn_t sxg_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static void sxg_handle_interrupt(p_adapter_t adapter)
+static void sxg_handle_interrupt(struct adapter_t *adapter)
 {
 /*    unsigned char           RssId   = 0; */
 	u32 NewIsr;
@@ -1056,7 +1056,7 @@ static void sxg_handle_interrupt(p_adapter_t adapter)
  * Return Value:
  * 	None
  */
-static int sxg_process_isr(p_adapter_t adapter, u32 MessageId)
+static int sxg_process_isr(struct adapter_t *adapter, u32 MessageId)
 {
 	u32 Isr = adapter->IsrCopy[MessageId];
 	u32 NewIsr = 0;
@@ -1153,10 +1153,10 @@ static int sxg_process_isr(p_adapter_t adapter, u32 MessageId)
  * Return Value:
  * 	None.
  */
-static u32 sxg_process_event_queue(p_adapter_t adapter, u32 RssId)
+static u32 sxg_process_event_queue(struct adapter_t *adapter, u32 RssId)
 {
-	PSXG_EVENT_RING EventRing = &adapter->EventRings[RssId];
-	PSXG_EVENT Event = &EventRing->Ring[adapter->NextEvent[RssId]];
+	struct SXG_EVENT_RING *EventRing = &adapter->EventRings[RssId];
+	struct SXG_EVENT *Event = &EventRing->Ring[adapter->NextEvent[RssId]];
 	u32 EventsProcessed = 0, Batches = 0;
 	u32 num_skbs = 0;
 	struct sk_buff *skb;
@@ -1164,7 +1164,7 @@ static u32 sxg_process_event_queue(p_adapter_t adapter, u32 RssId)
 	struct sk_buff *prev_skb = NULL;
 	struct sk_buff *IndicationList[SXG_RCV_ARRAYSIZE];
 	u32 Index;
-	PSXG_RCV_DATA_BUFFER_HDR RcvDataBufferHdr;
+	struct SXG_RCV_DATA_BUFFER_HDR *RcvDataBufferHdr;
 #endif
 	u32 ReturnStatus = 0;
 
@@ -1293,12 +1293,12 @@ static u32 sxg_process_event_queue(p_adapter_t adapter, u32 RssId)
  * Return
  *	None
  */
-static void sxg_complete_slow_send(p_adapter_t adapter)
+static void sxg_complete_slow_send(struct adapter_t *adapter)
 {
-	PSXG_XMT_RING XmtRing = &adapter->XmtRings[0];
-	PSXG_RING_INFO XmtRingInfo = &adapter->XmtRingZeroInfo;
+	struct SXG_XMT_RING *XmtRing = &adapter->XmtRings[0];
+	struct SXG_RING_INFO *XmtRingInfo = &adapter->XmtRingZeroInfo;
 	u32 *ContextType;
-	PSXG_CMD XmtCmd;
+	struct SXG_CMD *XmtCmd;
 
 	/* NOTE - This lock is dropped and regrabbed in this loop. */
 	/* This means two different processors can both be running */
@@ -1359,12 +1359,12 @@ static void sxg_complete_slow_send(p_adapter_t adapter)
  * Return
  *	 skb
  */
-static struct sk_buff *sxg_slow_receive(p_adapter_t adapter, PSXG_EVENT Event)
+static struct sk_buff *sxg_slow_receive(struct adapter_t *adapter, struct SXG_EVENT *Event)
 {
-	PSXG_RCV_DATA_BUFFER_HDR RcvDataBufferHdr;
+	struct SXG_RCV_DATA_BUFFER_HDR *RcvDataBufferHdr;
 	struct sk_buff *Packet;
 
-	RcvDataBufferHdr = (PSXG_RCV_DATA_BUFFER_HDR) Event->HostHandle;
+	RcvDataBufferHdr = (struct SXG_RCV_DATA_BUFFER_HDR*) Event->HostHandle;
 	ASSERT(RcvDataBufferHdr);
 	ASSERT(RcvDataBufferHdr->State == SXG_BUFFER_ONCARD);
 	ASSERT(SXG_RECEIVE_DATA_LOCATION(RcvDataBufferHdr) ==
@@ -1400,7 +1400,7 @@ static struct sk_buff *sxg_slow_receive(p_adapter_t adapter, PSXG_EVENT Event)
 	}
 #if XXXTODO			/* VLAN stuff */
 	/* If there's a VLAN tag, extract it and validate it */
-	if (((p_ether_header) (SXG_RECEIVE_DATA_LOCATION(RcvDataBufferHdr)))->
+	if (((struct ether_header*) (SXG_RECEIVE_DATA_LOCATION(RcvDataBufferHdr)))->
 	    EtherType == ETHERTYPE_VLAN) {
 		if (SxgExtractVlanHeader(adapter, RcvDataBufferHdr, Event) !=
 		    STATUS_SUCCESS) {
@@ -1415,7 +1415,7 @@ static struct sk_buff *sxg_slow_receive(p_adapter_t adapter, PSXG_EVENT Event)
 	/* */
 	/* Dumb-nic frame.  See if it passes our mac filter and update stats */
 	/* */
-	if (!sxg_mac_filter(adapter, (p_ether_header)
+	if (!sxg_mac_filter(adapter, (struct ether_header*)
 			    SXG_RECEIVE_DATA_LOCATION(RcvDataBufferHdr),
 			    Event->Length)) {
 		SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "RcvFiltr",
@@ -1456,7 +1456,7 @@ static struct sk_buff *sxg_slow_receive(p_adapter_t adapter, PSXG_EVENT Event)
  * Return Value:
  * 	None
  */
-static void sxg_process_rcv_error(p_adapter_t adapter, u32 ErrorStatus)
+static void sxg_process_rcv_error(struct adapter_t *adapter, u32 ErrorStatus)
 {
 	u32 Error;
 
@@ -1535,7 +1535,7 @@ static void sxg_process_rcv_error(p_adapter_t adapter, u32 ErrorStatus)
  * Return Value:
  * 	TRUE if the frame is to be allowed
  */
-static bool sxg_mac_filter(p_adapter_t adapter, p_ether_header EtherHdr,
+static bool sxg_mac_filter(struct adapter_t *adapter, struct ether_header *EtherHdr,
 			   ushort length)
 {
 	bool EqualAddr;
@@ -1560,7 +1560,7 @@ static bool sxg_mac_filter(p_adapter_t adapter, p_ether_header EtherHdr,
 				return (TRUE);
 			}
 			if (adapter->MacFilter & MAC_MCAST) {
-				PSXG_MULTICAST_ADDRESS MulticastAddrs =
+				struct SXG_MULTICAST_ADDRESS *MulticastAddrs =
 				    adapter->MulticastAddrs;
 				while (MulticastAddrs) {
 					ETHER_EQ_ADDR(MulticastAddrs->Address,
@@ -1600,7 +1600,7 @@ static bool sxg_mac_filter(p_adapter_t adapter, p_ether_header EtherHdr,
 	return (FALSE);
 }
 
-static int sxg_register_interrupt(p_adapter_t adapter)
+static int sxg_register_interrupt(struct adapter_t *adapter)
 {
 	if (!adapter->intrregistered) {
 		int retval;
@@ -1635,7 +1635,7 @@ static int sxg_register_interrupt(p_adapter_t adapter)
 	return (STATUS_SUCCESS);
 }
 
-static void sxg_deregister_interrupt(p_adapter_t adapter)
+static void sxg_deregister_interrupt(struct adapter_t *adapter)
 {
 	DBG_ERROR("sxg: %s ENTER adapter[%p]\n", __func__, adapter);
 #if XXXTODO
@@ -1661,7 +1661,7 @@ static void sxg_deregister_interrupt(p_adapter_t adapter)
  *  Perform initialization of our slic interface.
  *
  */
-static int sxg_if_init(p_adapter_t adapter)
+static int sxg_if_init(struct adapter_t *adapter)
 {
 	p_net_device dev = adapter->netdev;
 	int status = 0;
@@ -1721,7 +1721,7 @@ static int sxg_if_init(p_adapter_t adapter)
 
 static int sxg_entry_open(p_net_device dev)
 {
-	p_adapter_t adapter = (p_adapter_t) netdev_priv(dev);
+	struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev);
 	int status;
 
 	ASSERT(adapter);
@@ -1777,7 +1777,7 @@ static void __devexit sxg_entry_remove(struct pci_dev *pcidev)
 	p_net_device dev = pci_get_drvdata(pcidev);
 	u32 mmio_start = 0;
 	unsigned int mmio_len = 0;
-	p_adapter_t adapter = (p_adapter_t) netdev_priv(dev);
+	struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev);
 
 	ASSERT(adapter);
 	DBG_ERROR("sxg: %s ENTER dev[%p] adapter[%p]\n", __func__, dev,
@@ -1805,7 +1805,7 @@ static void __devexit sxg_entry_remove(struct pci_dev *pcidev)
 
 static int sxg_entry_halt(p_net_device dev)
 {
-	p_adapter_t adapter = (p_adapter_t) netdev_priv(dev);
+	struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev);
 
 	spin_lock_irqsave(&sxg_global.driver_lock, sxg_global.flags);
 	DBG_ERROR("sxg: %s (%s) ENTER\n", __func__, dev->name);
@@ -1830,7 +1830,7 @@ static int sxg_ioctl(p_net_device dev, struct ifreq *rq, int cmd)
 	switch (cmd) {
 	case SIOCSLICSETINTAGG:
 		{
-/*                      p_adapter_t adapter = (p_adapter_t) netdev_priv(dev); */
+/*                      struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev); */
 			u32 data[7];
 			u32 intagg;
 
@@ -1868,7 +1868,7 @@ static int sxg_ioctl(p_net_device dev, struct ifreq *rq, int cmd)
  */
 static int sxg_send_packets(struct sk_buff *skb, p_net_device dev)
 {
-	p_adapter_t adapter = (p_adapter_t) netdev_priv(dev);
+	struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev);
 	u32 status = STATUS_SUCCESS;
 
 	DBG_ERROR("sxg: %s ENTER sxg_send_packets skb[%p]\n", __func__,
@@ -1934,10 +1934,10 @@ static int sxg_send_packets(struct sk_buff *skb, p_net_device dev)
  * Return -
  * 		STATUS of send
  */
-static int sxg_transmit_packet(p_adapter_t adapter, struct sk_buff *skb)
+static int sxg_transmit_packet(struct adapter_t *adapter, struct sk_buff *skb)
 {
-	PSCATTER_GATHER_LIST pSgl;
-	PSXG_SCATTER_GATHER SxgSgl;
+	struct SCATTER_GATHER_LIST *pSgl;
+	struct SXG_SCATTER_GATHER *SxgSgl;
 	void *SglBuffer;
 	u32 SglBufferLength;
 
@@ -1980,14 +1980,14 @@ static int sxg_transmit_packet(p_adapter_t adapter, struct sk_buff *skb)
  * Return Value:
  * 	None.
  */
-static void sxg_dumb_sgl(PSCATTER_GATHER_LIST pSgl, PSXG_SCATTER_GATHER SxgSgl)
+static void sxg_dumb_sgl(struct SCATTER_GATHER_LIST *pSgl, struct SXG_SCATTER_GATHER *SxgSgl)
 {
-	p_adapter_t adapter = SxgSgl->adapter;
+	struct adapter_t *adapter = SxgSgl->adapter;
 	struct sk_buff *skb = SxgSgl->DumbPacket;
 	/* For now, all dumb-nic sends go on RSS queue zero */
-	PSXG_XMT_RING XmtRing = &adapter->XmtRings[0];
-	PSXG_RING_INFO XmtRingInfo = &adapter->XmtRingZeroInfo;
-	PSXG_CMD XmtCmd = NULL;
+	struct SXG_XMT_RING *XmtRing = &adapter->XmtRings[0];
+	struct SXG_RING_INFO *XmtRingInfo = &adapter->XmtRingZeroInfo;
+	struct SXG_CMD *XmtCmd = NULL;
 /*      u32                         Index = 0; */
 	u32 DataLength = skb->len;
 /*  unsigned int                                BufLen; */
@@ -2117,9 +2117,9 @@ static void sxg_dumb_sgl(PSCATTER_GATHER_LIST pSgl, PSXG_SCATTER_GATHER SxgSgl)
  * Return
  *	status
  */
-static int sxg_initialize_link(p_adapter_t adapter)
+static int sxg_initialize_link(struct adapter_t *adapter)
 {
-	PSXG_HW_REGS HwRegs = adapter->HwRegs;
+	struct SXG_HW_REGS *HwRegs = adapter->HwRegs;
 	u32 Value;
 	u32 ConfigData;
 	u32 MaxFrame;
@@ -2274,10 +2274,10 @@ static int sxg_initialize_link(p_adapter_t adapter)
  * Return
  *	status
  */
-static int sxg_phy_init(p_adapter_t adapter)
+static int sxg_phy_init(struct adapter_t *adapter)
 {
 	u32 Value;
-	PPHY_UCODE p;
+	struct PHY_UCODE *p;
 	int status;
 
 	DBG_ERROR("ENTER %s\n", __func__);
@@ -2322,10 +2322,10 @@ static int sxg_phy_init(p_adapter_t adapter)
  * Return
  *	None
  */
-static void sxg_link_event(p_adapter_t adapter)
+static void sxg_link_event(struct adapter_t *adapter)
 {
-	PSXG_HW_REGS HwRegs = adapter->HwRegs;
-	SXG_LINK_STATE LinkState;
+	struct SXG_HW_REGS *HwRegs = adapter->HwRegs;
+	enum SXG_LINK_STATE LinkState;
 	int status;
 	u32 Value;
 
@@ -2379,7 +2379,7 @@ static void sxg_link_event(p_adapter_t adapter)
  * Return
  *	Link State
  */
-static SXG_LINK_STATE sxg_get_link_state(p_adapter_t adapter)
+static enum SXG_LINK_STATE sxg_get_link_state(struct adapter_t *adapter)
 {
 	int status;
 	u32 Value;
@@ -2433,8 +2433,8 @@ static SXG_LINK_STATE sxg_get_link_state(p_adapter_t adapter)
 	return (SXG_LINK_DOWN);
 }
 
-static void sxg_indicate_link_state(p_adapter_t adapter,
-				    SXG_LINK_STATE LinkState)
+static void sxg_indicate_link_state(struct adapter_t *adapter,
+				    enum SXG_LINK_STATE LinkState)
 {
 	if (adapter->LinkState == SXG_LINK_UP) {
 		DBG_ERROR("%s: LINK now UP, call netif_start_queue\n",
@@ -2460,7 +2460,7 @@ static void sxg_indicate_link_state(p_adapter_t adapter,
  * Return
  *	None
  */
-static void sxg_link_state(p_adapter_t adapter, SXG_LINK_STATE LinkState)
+static void sxg_link_state(struct adapter_t *adapter, enum SXG_LINK_STATE LinkState)
 {
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_IMPORTANT, "LnkINDCT",
 		  adapter, LinkState, adapter->LinkState, adapter->State);
@@ -2498,10 +2498,10 @@ static void sxg_link_state(p_adapter_t adapter, SXG_LINK_STATE LinkState)
  * Return
  *	status
  */
-static int sxg_write_mdio_reg(p_adapter_t adapter,
+static int sxg_write_mdio_reg(struct adapter_t *adapter,
 			      u32 DevAddr, u32 RegAddr, u32 Value)
 {
-	PSXG_HW_REGS HwRegs = adapter->HwRegs;
+	struct SXG_HW_REGS *HwRegs = adapter->HwRegs;
 	u32 AddrOp;		/* Address operation (written to MIIM field reg) */
 	u32 WriteOp;		/* Write operation (written to MIIM field reg) */
 	u32 Cmd;		/* Command (written to MIIM command reg) */
@@ -2588,10 +2588,10 @@ static int sxg_write_mdio_reg(p_adapter_t adapter,
  * Return
  *	status
  */
-static int sxg_read_mdio_reg(p_adapter_t adapter,
+static int sxg_read_mdio_reg(struct adapter_t *adapter,
 			     u32 DevAddr, u32 RegAddr, u32 *pValue)
 {
-	PSXG_HW_REGS HwRegs = adapter->HwRegs;
+	struct SXG_HW_REGS *HwRegs = adapter->HwRegs;
 	u32 AddrOp;		/* Address operation (written to MIIM field reg) */
 	u32 ReadOp;		/* Read operation (written to MIIM field reg) */
 	u32 Cmd;		/* Command (written to MIIM command reg) */
@@ -2735,9 +2735,9 @@ static unsigned char sxg_mcast_get_mac_hash(char *macaddr)
 	return (machash);
 }
 
-static void sxg_mcast_set_mask(p_adapter_t adapter)
+static void sxg_mcast_set_mask(struct adapter_t *adapter)
 {
-	PSXG_UCODE_REGS sxg_regs = adapter->UcodeRegs;
+	struct SXG_UCODE_REGS *sxg_regs = adapter->UcodeRegs;
 
 	DBG_ERROR("%s ENTER (%s) macopts[%x] mask[%llx]\n", __func__,
 		  adapter->netdev->name, (unsigned int)adapter->MacFilter,
@@ -2775,7 +2775,7 @@ static void sxg_mcast_set_mask(p_adapter_t adapter)
  *  Allocate a mcast_address structure to hold the multicast address.
  *  Link it in.
  */
-static int sxg_mcast_add_list(p_adapter_t adapter, char *address)
+static int sxg_mcast_add_list(struct adapter_t *adapter, char *address)
 {
 	p_mcast_address_t mcaddr, mlist;
 	bool equaladdr;
@@ -2803,7 +2803,7 @@ static int sxg_mcast_add_list(p_adapter_t adapter, char *address)
 	return (STATUS_SUCCESS);
 }
 
-static void sxg_mcast_set_bit(p_adapter_t adapter, char *address)
+static void sxg_mcast_set_bit(struct adapter_t *adapter, char *address)
 {
 	unsigned char crcpoly;
 
@@ -2821,7 +2821,7 @@ static void sxg_mcast_set_bit(p_adapter_t adapter, char *address)
 
 static void sxg_mcast_set_list(p_net_device dev)
 {
-	p_adapter_t adapter = (p_adapter_t) netdev_priv(dev);
+	struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev);
 	int status = STATUS_SUCCESS;
 	int i;
 	char *addresses;
@@ -2876,7 +2876,7 @@ static void sxg_mcast_set_list(p_net_device dev)
 }
 #endif
 
-static void sxg_unmap_mmio_space(p_adapter_t adapter)
+static void sxg_unmap_mmio_space(struct adapter_t *adapter)
 {
 #if LINUX_FREES_ADAPTER_RESOURCES
 /*      if (adapter->Regs) { */
@@ -2896,7 +2896,7 @@ static void sxg_unmap_mmio_space(p_adapter_t adapter)
  * Return
  *	none
  */
-void SxgFreeResources(p_adapter_t adapter)
+void SxgFreeResources(struct adapter_t *adapter)
 {
 	u32 RssIds, IsrCount;
 	PTCP_OBJECT TcpObject;
@@ -2924,7 +2924,7 @@ void SxgFreeResources(p_adapter_t adapter)
 	/* Free event queues. */
 	if (adapter->EventRings) {
 		pci_free_consistent(adapter->pcidev,
-				    sizeof(SXG_EVENT_RING) * RssIds,
+				    sizeof(struct SXG_EVENT_RING) * RssIds,
 				    adapter->EventRings, adapter->PEventRings);
 	}
 	if (adapter->Isr) {
@@ -2991,7 +2991,7 @@ void SxgFreeResources(p_adapter_t adapter)
  * This routine is called when a memory allocation has completed.
  *
  * Arguments -
- *	p_adapter_t    	- Our adapter structure
+ *	struct adapter_t *   	- Our adapter structure
  *	VirtualAddress	- Memory virtual address
  *	PhysicalAddress	- Memory physical address
  *	Length		- Length of memory allocated (or 0)
@@ -3000,10 +3000,10 @@ void SxgFreeResources(p_adapter_t adapter)
  * Return
  *	None.
  */
-static void sxg_allocate_complete(p_adapter_t adapter,
+static void sxg_allocate_complete(struct adapter_t *adapter,
 				  void *VirtualAddress,
 				  dma_addr_t PhysicalAddress,
-				  u32 Length, SXG_BUFFER_TYPE Context)
+				  u32 Length, enum SXG_BUFFER_TYPE Context)
 {
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "AllocCmp",
 		  adapter, VirtualAddress, Length, Context);
@@ -3018,7 +3018,7 @@ static void sxg_allocate_complete(p_adapter_t adapter,
 					       PhysicalAddress, Length);
 		break;
 	case SXG_BUFFER_TYPE_SGL:
-		sxg_allocate_sgl_buffer_complete(adapter, (PSXG_SCATTER_GATHER)
+		sxg_allocate_sgl_buffer_complete(adapter, (struct SXG_SCATTER_GATHER*)
 						 VirtualAddress,
 						 PhysicalAddress, Length);
 		break;
@@ -3039,8 +3039,8 @@ static void sxg_allocate_complete(p_adapter_t adapter,
  * Return
  *	int
  */
-static int sxg_allocate_buffer_memory(p_adapter_t adapter,
-				      u32 Size, SXG_BUFFER_TYPE BufferType)
+static int sxg_allocate_buffer_memory(struct adapter_t *adapter,
+				      u32 Size, enum SXG_BUFFER_TYPE BufferType)
 {
 	int status;
 	void *Buffer;
@@ -3091,7 +3091,7 @@ static int sxg_allocate_buffer_memory(p_adapter_t adapter,
  * Return
  *
  */
-static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
+static void sxg_allocate_rcvblock_complete(struct adapter_t *adapter,
 					   void *RcvBlock,
 					   dma_addr_t PhysicalAddress,
 					   u32 Length)
@@ -3099,11 +3099,11 @@ static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
 	u32 i;
 	u32 BufferSize = adapter->ReceiveBufferSize;
 	u64 Paddr;
-	PSXG_RCV_BLOCK_HDR RcvBlockHdr;
+	struct SXG_RCV_BLOCK_HDR *RcvBlockHdr;
 	unsigned char *RcvDataBuffer;
-	PSXG_RCV_DATA_BUFFER_HDR RcvDataBufferHdr;
-	PSXG_RCV_DESCRIPTOR_BLOCK RcvDescriptorBlock;
-	PSXG_RCV_DESCRIPTOR_BLOCK_HDR RcvDescriptorBlockHdr;
+	struct SXG_RCV_DATA_BUFFER_HDR *RcvDataBufferHdr;
+	struct SXG_RCV_DESCRIPTOR_BLOCK *RcvDescriptorBlock;
+	struct SXG_RCV_DESCRIPTOR_BLOCK_HDR *RcvDescriptorBlockHdr;
 
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "AlRcvBlk",
 		  adapter, RcvBlock, Length, 0);
@@ -3129,7 +3129,7 @@ static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
 		     i++, Paddr += BufferSize, RcvDataBuffer += BufferSize) {
 			/* */
 			RcvDataBufferHdr =
-			    (PSXG_RCV_DATA_BUFFER_HDR) (RcvDataBuffer +
+			    (struct SXG_RCV_DATA_BUFFER_HDR*) (RcvDataBuffer +
 							SXG_RCV_DATA_BUFFER_HDR_OFFSET
 							(BufferSize));
 			RcvDataBufferHdr->VirtualAddress = RcvDataBuffer;
@@ -3147,7 +3147,7 @@ static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
 	/* Place this entire block of memory on the AllRcvBlocks queue so it can be */
 	/* free later */
 	RcvBlockHdr =
-	    (PSXG_RCV_BLOCK_HDR) ((unsigned char *)RcvBlock +
+	    (struct SXG_RCV_BLOCK_HDR*) ((unsigned char *)RcvBlock +
 				  SXG_RCV_BLOCK_HDR_OFFSET(BufferSize));
 	RcvBlockHdr->VirtualAddress = RcvBlock;
 	RcvBlockHdr->PhysicalAddress = PhysicalAddress;
@@ -3161,7 +3161,7 @@ static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
 	for (i = 0, Paddr = PhysicalAddress;
 	     i < SXG_RCV_DESCRIPTORS_PER_BLOCK;
 	     i++, Paddr += BufferSize, RcvDataBuffer += BufferSize) {
-		RcvDataBufferHdr = (PSXG_RCV_DATA_BUFFER_HDR) (RcvDataBuffer +
+		RcvDataBufferHdr = (struct SXG_RCV_DATA_BUFFER_HDR*) (RcvDataBuffer +
 							       SXG_RCV_DATA_BUFFER_HDR_OFFSET
 							       (BufferSize));
 		spin_lock(&adapter->RcvQLock);
@@ -3171,11 +3171,11 @@ static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
 
 	/* Locate the descriptor block and put it on a separate free queue */
 	RcvDescriptorBlock =
-	    (PSXG_RCV_DESCRIPTOR_BLOCK) ((unsigned char *)RcvBlock +
+	    (struct SXG_RCV_DESCRIPTOR_BLOCK*) ((unsigned char *)RcvBlock +
 					 SXG_RCV_DESCRIPTOR_BLOCK_OFFSET
 					 (BufferSize));
 	RcvDescriptorBlockHdr =
-	    (PSXG_RCV_DESCRIPTOR_BLOCK_HDR) ((unsigned char *)RcvBlock +
+	    (struct SXG_RCV_DESCRIPTOR_BLOCK_HDR*) ((unsigned char *)RcvBlock +
 					     SXG_RCV_DESCRIPTOR_BLOCK_HDR_OFFSET
 					     (BufferSize));
 	RcvDescriptorBlockHdr->VirtualAddress = RcvDescriptorBlock;
@@ -3193,7 +3193,7 @@ static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
 		for (i = 0; i < SXG_RCV_DESCRIPTORS_PER_BLOCK;
 		     i++, RcvDataBuffer += BufferSize) {
 			RcvDataBufferHdr =
-			    (PSXG_RCV_DATA_BUFFER_HDR) (RcvDataBuffer +
+			    (struct SXG_RCV_DATA_BUFFER_HDR*) (RcvDataBuffer +
 							SXG_RCV_DATA_BUFFER_HDR_OFFSET
 							(BufferSize));
 			SXG_FREE_RCV_PACKET(RcvDataBufferHdr);
@@ -3220,8 +3220,8 @@ static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
  * Return
  *
  */
-static void sxg_allocate_sgl_buffer_complete(p_adapter_t adapter,
-					     PSXG_SCATTER_GATHER SxgSgl,
+static void sxg_allocate_sgl_buffer_complete(struct adapter_t *adapter,
+					     struct SXG_SCATTER_GATHER *SxgSgl,
 					     dma_addr_t PhysicalAddress,
 					     u32 Length)
 {
@@ -3229,7 +3229,7 @@ static void sxg_allocate_sgl_buffer_complete(p_adapter_t adapter,
 		  adapter, SxgSgl, Length, 0);
 	spin_lock(&adapter->SglQLock);
 	adapter->AllSglBufferCount++;
-	memset(SxgSgl, 0, sizeof(SXG_SCATTER_GATHER));
+	memset(SxgSgl, 0, sizeof(struct SXG_SCATTER_GATHER*));
 	SxgSgl->PhysicalAddress = PhysicalAddress;	/* *PhysicalAddress; */
 	SxgSgl->adapter = adapter;	/* Initialize backpointer once */
 	InsertTailList(&adapter->AllSglBuffers, &SxgSgl->AllList);
@@ -3243,14 +3243,14 @@ static void sxg_allocate_sgl_buffer_complete(p_adapter_t adapter,
 static unsigned char temp_mac_address[6] =
     { 0x00, 0xab, 0xcd, 0xef, 0x12, 0x69 };
 
-static void sxg_adapter_set_hwaddr(p_adapter_t adapter)
+static void sxg_adapter_set_hwaddr(struct adapter_t *adapter)
 {
 /*  DBG_ERROR ("%s ENTER card->config_set[%x] port[%d] physport[%d] funct#[%d]\n", __func__, */
 /*             card->config_set, adapter->port, adapter->physport, adapter->functionnumber); */
 /* */
 /*  sxg_dbg_macaddrs(adapter); */
 
-	memcpy(adapter->macaddr, temp_mac_address, sizeof(SXG_CONFIG_MAC));
+	memcpy(adapter->macaddr, temp_mac_address, sizeof(struct SXG_CONFIG_MAC));
 /*      DBG_ERROR ("%s AFTER copying from config.macinfo into currmacaddr\n", __func__); */
 /*      sxg_dbg_macaddrs(adapter); */
 	if (!(adapter->currmacaddr[0] ||
@@ -3271,7 +3271,7 @@ static void sxg_adapter_set_hwaddr(p_adapter_t adapter)
 #if XXXTODO
 static int sxg_mac_set_address(p_net_device dev, void *ptr)
 {
-	p_adapter_t adapter = (p_adapter_t) netdev_priv(dev);
+	struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev);
 	struct sockaddr *addr = ptr;
 
 	DBG_ERROR("%s ENTER (%s)\n", __func__, adapter->netdev->name);
@@ -3313,7 +3313,7 @@ static int sxg_mac_set_address(p_net_device dev, void *ptr)
  * Return
  *	int
  */
-static int sxg_initialize_adapter(p_adapter_t adapter)
+static int sxg_initialize_adapter(struct adapter_t *adapter)
 {
 	u32 RssIds, IsrCount;
 	u32 i;
@@ -3327,7 +3327,7 @@ static int sxg_initialize_adapter(p_adapter_t adapter)
 
 	/* Sanity check SXG_UCODE_REGS structure definition to */
 	/* make sure the length is correct */
-	ASSERT(sizeof(SXG_UCODE_REGS) == SXG_REGISTER_SIZE_PER_CPU);
+	ASSERT(sizeof(struct SXG_UCODE_REGS) == SXG_REGISTER_SIZE_PER_CPU);
 
 	/* Disable interrupts */
 	SXG_DISABLE_ALL_INTERRUPTS(adapter);
@@ -3412,16 +3412,16 @@ static int sxg_initialize_adapter(p_adapter_t adapter)
  * Return
  *	status
  */
-static int sxg_fill_descriptor_block(p_adapter_t adapter,
-				     PSXG_RCV_DESCRIPTOR_BLOCK_HDR
-				     RcvDescriptorBlockHdr)
+static int sxg_fill_descriptor_block(struct adapter_t *adapter,
+				     struct SXG_RCV_DESCRIPTOR_BLOCK_HDR
+				     *RcvDescriptorBlockHdr)
 {
 	u32 i;
-	PSXG_RING_INFO RcvRingInfo = &adapter->RcvRingZeroInfo;
-	PSXG_RCV_DATA_BUFFER_HDR RcvDataBufferHdr;
-	PSXG_RCV_DESCRIPTOR_BLOCK RcvDescriptorBlock;
-	PSXG_CMD RingDescriptorCmd;
-	PSXG_RCV_RING RingZero = &adapter->RcvRings[0];
+	struct SXG_RING_INFO *RcvRingInfo = &adapter->RcvRingZeroInfo;
+	struct SXG_RCV_DATA_BUFFER_HDR *RcvDataBufferHdr;
+	struct SXG_RCV_DESCRIPTOR_BLOCK *RcvDescriptorBlock;
+	struct SXG_CMD *RingDescriptorCmd;
+	struct SXG_RCV_RING *RingZero = &adapter->RcvRings[0];
 
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "FilBlk",
 		  adapter, adapter->RcvBuffersOnCard,
@@ -3442,7 +3442,7 @@ static int sxg_fill_descriptor_block(p_adapter_t adapter,
 	ASSERT(RingDescriptorCmd);
 	RcvDescriptorBlockHdr->State = SXG_BUFFER_ONCARD;
 	RcvDescriptorBlock =
-	    (PSXG_RCV_DESCRIPTOR_BLOCK) RcvDescriptorBlockHdr->VirtualAddress;
+	    (struct SXG_RCV_DESCRIPTOR_BLOCK*) RcvDescriptorBlockHdr->VirtualAddress;
 
 	/* Fill in the descriptor block */
 	for (i = 0; i < SXG_RCV_DESCRIPTORS_PER_BLOCK; i++) {
@@ -3484,9 +3484,9 @@ static int sxg_fill_descriptor_block(p_adapter_t adapter,
  * Return
  *	None
  */
-static void sxg_stock_rcv_buffers(p_adapter_t adapter)
+static void sxg_stock_rcv_buffers(struct adapter_t *adapter)
 {
-	PSXG_RCV_DESCRIPTOR_BLOCK_HDR RcvDescriptorBlockHdr;
+	struct SXG_RCV_DESCRIPTOR_BLOCK_HDR *RcvDescriptorBlockHdr;
 
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "StockBuf",
 		  adapter, adapter->RcvBuffersOnCard,
@@ -3506,14 +3506,14 @@ static void sxg_stock_rcv_buffers(p_adapter_t adapter)
 	/* Now grab the RcvQLock lock and proceed */
 	spin_lock(&adapter->RcvQLock);
 	while (adapter->RcvBuffersOnCard < SXG_RCV_DATA_BUFFERS) {
-		PLIST_ENTRY _ple;
+		struct LIST_ENTRY *_ple;
 
 		/* Get a descriptor block */
 		RcvDescriptorBlockHdr = NULL;
 		if (adapter->FreeRcvBlockCount) {
 			_ple = RemoveHeadList(&adapter->FreeRcvBlocks);
 			RcvDescriptorBlockHdr =
-			    container_of(_ple, SXG_RCV_DESCRIPTOR_BLOCK_HDR,
+			    container_of(_ple, struct SXG_RCV_DESCRIPTOR_BLOCK_HDR,
 					 FreeList);
 			adapter->FreeRcvBlockCount--;
 			RcvDescriptorBlockHdr->State = SXG_BUFFER_BUSY;
@@ -3550,13 +3550,13 @@ static void sxg_stock_rcv_buffers(p_adapter_t adapter)
  * Return
  *	None
  */
-static void sxg_complete_descriptor_blocks(p_adapter_t adapter,
+static void sxg_complete_descriptor_blocks(struct adapter_t *adapter,
 					   unsigned char Index)
 {
-	PSXG_RCV_RING RingZero = &adapter->RcvRings[0];
-	PSXG_RING_INFO RcvRingInfo = &adapter->RcvRingZeroInfo;
-	PSXG_RCV_DESCRIPTOR_BLOCK_HDR RcvDescriptorBlockHdr;
-	PSXG_CMD RingDescriptorCmd;
+	struct SXG_RCV_RING *RingZero = &adapter->RcvRings[0];
+	struct SXG_RING_INFO *RcvRingInfo = &adapter->RcvRingZeroInfo;
+	struct SXG_RCV_DESCRIPTOR_BLOCK_HDR *RcvDescriptorBlockHdr;
+	struct SXG_CMD *RingDescriptorCmd;
 
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "CmpRBlks",
 		  adapter, Index, RcvRingInfo->Head, RcvRingInfo->Tail);
diff --git a/drivers/staging/sxg/sxg.h b/drivers/staging/sxg/sxg.h
index 844ca56f2800..653cf3ba0c4c 100644
--- a/drivers/staging/sxg/sxg.h
+++ b/drivers/staging/sxg/sxg.h
@@ -45,7 +45,7 @@
 #define p_net_device struct net_device *
 // SXG_STATS - Probably move these to someplace where
 // the slicstat (sxgstat?) program can get them.
-typedef struct _SXG_STATS {
+struct SXG_STATS {
 	// Xmt
 	u32				XmtNBL;				// Offload send NBL count
 	u64				DumbXmtBytes;		// Dumbnic send bytes
@@ -109,7 +109,7 @@ typedef struct _SXG_STATS {
 	u64				LinkCrc;			// SXG_RCV_STATUS_LINK_CRC:
 	u64				LinkOflow;			// SXG_RCV_STATUS_LINK_OFLOW:
 	u64				LinkUflow;			// SXG_RCV_STATUS_LINK_UFLOW:
-} SXG_STATS, *PSXG_STATS;
+};
 
 
 /****************************************************************************
@@ -215,12 +215,12 @@ typedef struct _SXG_STATS {
 ///////////////////////////////////////////////////////////////////////////////
 // NOTE - Lock must be held with RCV macros
 #define SXG_GET_RCV_DATA_BUFFER(_pAdapt, _Hdr) {								\
-	PLIST_ENTRY     				_ple;										\
+	struct LIST_ENTRY     				*_ple;										\
 	_Hdr = NULL;																\
 	if((_pAdapt)->FreeRcvBufferCount) {											\
 		ASSERT(!(IsListEmpty(&(_pAdapt)->FreeRcvBuffers)));						\
 		_ple = RemoveHeadList(&(_pAdapt)->FreeRcvBuffers);	    				\
-		(_Hdr) = container_of(_ple, SXG_RCV_DATA_BUFFER_HDR, FreeList);	        \
+		(_Hdr) = container_of(_ple, struct SXG_RCV_DATA_BUFFER_HDR, FreeList);	        \
 		(_pAdapt)->FreeRcvBufferCount--;										\
 		ASSERT((_Hdr)->State == SXG_BUFFER_FREE);								\
 	}																			\
@@ -263,12 +263,12 @@ typedef struct _SXG_STATS {
 // until after that.  We're dealing with round numbers here, so we don't need to,
 // and not grabbing it avoids a possible double-trip.
 #define SXG_GET_SGL_BUFFER(_pAdapt, _Sgl) {				\
-	PLIST_ENTRY _ple;						\
+	struct LIST_ENTRY *_ple;						\
 	if ((_pAdapt->FreeSglBufferCount < SXG_MIN_SGL_BUFFERS) &&	\
 	   (_pAdapt->AllSglBufferCount < SXG_MAX_SGL_BUFFERS) &&	\
 	   (_pAdapt->AllocationsPending == 0)) {			\
 		sxg_allocate_buffer_memory(_pAdapt,			\
-			(sizeof(SXG_SCATTER_GATHER) + SXG_SGL_BUF_SIZE),\
+			(sizeof(struct SXG_SCATTER_GATHER) + SXG_SGL_BUF_SIZE),\
 			SXG_BUFFER_TYPE_SGL);				\
 	}								\
 	_Sgl = NULL;							\
@@ -276,7 +276,7 @@ typedef struct _SXG_STATS {
 	if((_pAdapt)->FreeSglBufferCount) {				\
 		ASSERT(!(IsListEmpty(&(_pAdapt)->FreeSglBuffers)));	\
 		_ple = RemoveHeadList(&(_pAdapt)->FreeSglBuffers);	\
-		(_Sgl) = container_of(_ple, SXG_SCATTER_GATHER, FreeList); \
+		(_Sgl) = container_of(_ple, struct SXG_SCATTER_GATHER, FreeList); \
             (_pAdapt)->FreeSglBufferCount--;				\
 		ASSERT((_Sgl)->State == SXG_BUFFER_FREE);		\
 		(_Sgl)->State = SXG_BUFFER_BUSY;			\
@@ -289,17 +289,17 @@ typedef struct _SXG_STATS {
 // SXG_MULTICAST_ADDRESS
 //
 // Linked list of multicast addresses.
-typedef struct _SXG_MULTICAST_ADDRESS {
+struct SXG_MULTICAST_ADDRESS {
 	unsigned char							Address[6];
-	struct _SXG_MULTICAST_ADDRESS	*Next;
-} SXG_MULTICAST_ADDRESS, *PSXG_MULTICAST_ADDRESS;
+	struct SXG_MULTICAST_ADDRESS	*Next;
+};
 
 // Structure to maintain chimney send and receive buffer queues.
 // This structure maintains NET_BUFFER_LIST queues that are
 // given to us via the Chimney MiniportTcpOffloadSend and
 // MiniportTcpOffloadReceive routines.  This structure DOES NOT
 // manage our data buffer queue
-typedef struct _SXG_BUFFER_QUEUE {
+struct SXG_BUFFER_QUEUE {
 	u32						Type;			// Slow or fast - See below
 	u32						Direction;		// Xmt or Rcv
 	u32						Bytes;			// Byte count
@@ -307,7 +307,7 @@ typedef struct _SXG_BUFFER_QUEUE {
 	u32 *        			Tail;			// Send queue tail
 //	PNET_BUFFER_LIST			NextNBL;		// Short cut - next NBL
 //	PNET_BUFFER					NextNB;			// Short cut - next NB
-} SXG_BUFFER_QUEUE, *PSXG_BUFFER_QUEUE;
+};
 
 #define		SXG_SLOW_SEND_BUFFER	0
 #define		SXG_FAST_SEND_BUFFER	1
@@ -335,7 +335,7 @@ typedef struct _SXG_BUFFER_QUEUE {
 
 // Adapter states - These states closely match the adapter states
 // documented in the DDK (with a few exceptions).
-typedef enum _SXG_STATE {
+enum SXG_STATE {
 	SXG_STATE_INITIALIZING,			// Initializing
 	SXG_STATE_BOOTDIAG,				// Boot-Diagnostic mode
 	SXG_STATE_PAUSING,				// Pausing
@@ -347,24 +347,24 @@ typedef enum _SXG_STATE {
 	SXG_STATE_HALTING,				// Halting
 	SXG_STATE_HALTED,				// Down or not-initialized
 	SXG_STATE_SHUTDOWN				// shutdown
-} SXG_STATE, *PSXG_STATE;
+};
 
 // Link state
-typedef enum _SXG_LINK_STATE {
+enum SXG_LINK_STATE {
 	SXG_LINK_DOWN,
 	SXG_LINK_UP
-} SXG_LINK_STATE, *PSXG_LINK_STATE;
+};
 
 // Link initialization timeout in 100us units
 #define SXG_LINK_TIMEOUT	100000		// 10 Seconds - REDUCE!
 
 
 // Microcode file selection codes
-typedef enum _SXG_UCODE_SEL {
+enum SXG_UCODE_SEL {
 	SXG_UCODE_SAHARA,				// Sahara ucode
 	SXG_UCODE_SDIAGCPU,				// Sahara CPU diagnostic ucode
 	SXG_UCODE_SDIAGSYS				// Sahara system diagnostic ucode
-} SXG_UCODE_SEL;
+};
 
 
 #define SXG_DISABLE_ALL_INTERRUPTS(_padapt) sxg_disable_interrupt(_padapt)
@@ -384,10 +384,10 @@ typedef enum _SXG_UCODE_SEL {
 //
 // contains information about the sxg driver.  There is only
 // one of these, and it is defined as a global.
-typedef struct _SXG_DRIVER {
-	struct _adapter_t	*Adapters;		// Linked list of adapters
+struct SXG_DRIVER {
+	struct adapter_t	*Adapters;		// Linked list of adapters
 	ushort				AdapterID;		// Maintain unique adapter ID
-} SXG_DRIVER, *PSXG_DRIVER;
+};
 
 #ifdef STATUS_SUCCESS
 #undef STATUS_SUCCESS
@@ -416,11 +416,10 @@ typedef struct _SXG_DRIVER {
 #define MIN(a, b) ((u32)(a) < (u32)(b) ? (a) : (b))
 #define MAX(a, b) ((u32)(a) > (u32)(b) ? (a) : (b))
 
-typedef struct _mcast_address_t
-{
+struct mcast_address_t {
     unsigned char                     address[6];
-    struct _mcast_address_t   *next;
-}  mcast_address_t, *p_mcast_address_t;
+    struct mcast_address_t   *next;
+};
 
 #define CARD_DOWN        0x00000000
 #define CARD_UP          0x00000001
@@ -472,41 +471,37 @@ typedef struct _mcast_address_t
 #define SLIC_CARD_STATE(x)    ((x==CARD_UP) ? "UP" : "Down")
 
 
-typedef struct _ether_header
-{
+struct ether_header {
     unsigned char    ether_dhost[6];
     unsigned char    ether_shost[6];
     ushort   ether_type;
-} ether_header, *p_ether_header;
+};
 
 
 #define NUM_CFG_SPACES      2
 #define NUM_CFG_REGS        64
 
-typedef struct _physcard_t
-{
-    struct _adapter_t  *adapter[SLIC_MAX_PORTS];
-    struct _physcard_t *next;
+struct physcard_t {
+    struct adapter_t  *adapter[SLIC_MAX_PORTS];
+    struct physcard_t *next;
     unsigned int                adapters_allocd;
-} physcard_t, *p_physcard_t;
+};
 
-typedef struct _sxgbase_driver
-{
+struct sxgbase_driver_t {
 	spinlock_t	driver_lock;
 	unsigned long	flags;	/* irqsave for spinlock */
 	u32		num_sxg_cards;
 	u32		num_sxg_ports;
 	u32		num_sxg_ports_active;
 	u32		dynamic_intagg;
-	p_physcard_t	phys_card;
-} sxgbase_driver_t;
+	struct physcard_t	*phys_card;
+};
 
 
-typedef struct _adapter_t
-{
+struct adapter_t {
 	void *               ifp;
 	unsigned int                port;
-	p_physcard_t        physcard;
+	struct physcard_t        *physcard;
 	unsigned int                physport;
 	unsigned int                cardindex;
 	unsigned int                card_size;
@@ -544,7 +539,7 @@ typedef struct _adapter_t
 	u32             macopts;
 	ushort              devflags_prev;
 	u64             mcastmask;
-	p_mcast_address_t   mcastaddrs;
+	struct mcast_address_t   *mcastaddrs;
 	struct timer_list   pingtimer;
 	u32             pingtimerset;
 	struct timer_list   statstimer;
@@ -580,11 +575,11 @@ typedef struct _adapter_t
 	u32             intagg_period;
 	struct net_device_stats stats;
 	u32 *					MiniportHandle;		// Our miniport handle
-	SXG_STATE					State;				// Adapter state
-	SXG_LINK_STATE				LinkState;			// Link state
+	enum SXG_STATE					State;				// Adapter state
+	enum SXG_LINK_STATE				LinkState;			// Link state
 	u64						LinkSpeed;			// Link Speed
 	u32						PowerState;			// NDIS power state
-	struct _adapter_t   		*Next;				// Linked list
+	struct adapter_t   		*Next;				// Linked list
 	ushort						AdapterID;			// 1..n
 	unsigned char						MacAddr[6];			// Our permanent HW mac address
 	unsigned char						CurrMacAddr[6];		// Our Current mac address
@@ -592,16 +587,16 @@ typedef struct _adapter_t
 	p_net_device                next_netdevice;
 	struct pci_dev            * pcidev;
 
-	PSXG_MULTICAST_ADDRESS		MulticastAddrs;		// Multicast list
+	struct SXG_MULTICAST_ADDRESS		*MulticastAddrs;		// Multicast list
 	u64     				MulticastMask;		// Multicast mask
 	u32 *					InterruptHandle;	// Register Interrupt handle
 	u32						InterruptLevel;		// From Resource list
 	u32						InterruptVector;	// From Resource list
 	spinlock_t	AdapterLock;	/* Serialize access adapter routines */
 	spinlock_t	Bit64RegLock;	/* For writing 64-bit addresses */
-	PSXG_HW_REGS				HwRegs;				// Sahara HW Register Memory (BAR0/1)
-	PSXG_UCODE_REGS				UcodeRegs;			// Microcode Register Memory (BAR2/3)
-	PSXG_TCB_REGS				TcbRegs;			// Same as Ucode regs - See sxghw.h
+	struct SXG_HW_REGS			*HwRegs;				// Sahara HW Register Memory (BAR0/1)
+	struct SXG_UCODE_REGS			*UcodeRegs;			// Microcode Register Memory (BAR2/3)
+	struct SXG_TCB_REGS			*TcbRegs;			// Same as Ucode regs - See sxghw.h
 	ushort						ResetDpcCount;		// For timeout
 	ushort						RssDpcCount;		// For timeout
 	ushort						VendorID;			// Vendor ID
@@ -613,25 +608,25 @@ typedef struct _adapter_t
 	u32 *					BufferPoolHandle;	// Used with NDIS 5.2 only.  Don't ifdef out
 	u32						MacFilter;			// NDIS MAC Filter
 	ushort						IpId;				// For slowpath
-	PSXG_EVENT_RING				EventRings;			// Host event rings.  1/CPU to 16 max
+	struct SXG_EVENT_RING			*EventRings;			// Host event rings.  1/CPU to 16 max
 	dma_addr_t              	PEventRings;		// Physical address
 	u32						NextEvent[SXG_MAX_RSS];	// Current location in ring
 	dma_addr_t          		PTcbBuffers;		// TCB Buffers - physical address
 	dma_addr_t	            	PTcbCompBuffers;	// TCB Composite Buffers - phys addr
-	PSXG_XMT_RING				XmtRings;			// Transmit rings
+	struct SXG_XMT_RING				*XmtRings;			// Transmit rings
 	dma_addr_t		            PXmtRings;			// Transmit rings - physical address
-	SXG_RING_INFO				XmtRingZeroInfo;	// Transmit ring 0 info
+	struct SXG_RING_INFO				XmtRingZeroInfo;	// Transmit ring 0 info
 	spinlock_t	XmtZeroLock;	/* Transmit ring 0 lock */
 	u32 *					XmtRingZeroIndex;	// Shared XMT ring 0 index
 	dma_addr_t          		PXmtRingZeroIndex;	// Shared XMT ring 0 index - physical
-	LIST_ENTRY					FreeProtocolHeaders;// Free protocol headers
+	struct LIST_ENTRY					FreeProtocolHeaders;// Free protocol headers
 	u32						FreeProtoHdrCount;	// Count
 	void *						ProtocolHeaders;	// Block of protocol header
 	dma_addr_t	            	PProtocolHeaders;	// Block of protocol headers - phys
 
-	PSXG_RCV_RING				RcvRings;			// Receive rings
+	struct SXG_RCV_RING		*RcvRings;			// Receive rings
 	dma_addr_t	            	PRcvRings;			// Receive rings - physical address
-	SXG_RING_INFO				RcvRingZeroInfo;	// Receive ring 0 info
+	struct SXG_RING_INFO				RcvRingZeroInfo;	// Receive ring 0 info
 
 	u32 *					Isr;				// Interrupt status register
 	dma_addr_t	            	PIsr;				// ISR - physical address
@@ -645,9 +640,9 @@ typedef struct _adapter_t
 	u32						HashInformation;
 	// Receive buffer queues
 	spinlock_t	RcvQLock;	/* Receive Queue Lock */
-	LIST_ENTRY					FreeRcvBuffers;		// Free SXG_DATA_BUFFER queue
-	LIST_ENTRY					FreeRcvBlocks;		// Free SXG_RCV_DESCRIPTOR_BLOCK Q
-	LIST_ENTRY					AllRcvBlocks;		// All SXG_RCV_BLOCKs
+	struct LIST_ENTRY					FreeRcvBuffers;		// Free SXG_DATA_BUFFER queue
+	struct LIST_ENTRY					FreeRcvBlocks;		// Free SXG_RCV_DESCRIPTOR_BLOCK Q
+	struct LIST_ENTRY					AllRcvBlocks;		// All SXG_RCV_BLOCKs
 	ushort						FreeRcvBufferCount;	// Number of free rcv data buffers
 	ushort						FreeRcvBlockCount;	// # of free rcv descriptor blocks
 	ushort						AllRcvBlockCount;	// Number of total receive blocks
@@ -656,8 +651,8 @@ typedef struct _adapter_t
 	u32						RcvBuffersOnCard;	// SXG_DATA_BUFFERS owned by card
 	// SGL buffers
 	spinlock_t	SglQLock;	/* SGL Queue Lock */
-	LIST_ENTRY					FreeSglBuffers;		// Free SXG_SCATTER_GATHER
-	LIST_ENTRY					AllSglBuffers;		// All SXG_SCATTER_GATHER
+	struct LIST_ENTRY					FreeSglBuffers;		// Free SXG_SCATTER_GATHER
+	struct LIST_ENTRY					AllSglBuffers;		// All SXG_SCATTER_GATHER
 	ushort						FreeSglBufferCount;	// Number of free SGL buffers
 	ushort						AllSglBufferCount;	// Number of total SGL buffers
 	u32						CurrentTime;		// Tick count
@@ -679,7 +674,7 @@ typedef struct _adapter_t
 	// Stats
 	u32						PendingRcvCount;	// Outstanding rcv indications
 	u32						PendingXmtCount;	// Outstanding send requests
-	SXG_STATS					Stats;				// Statistics
+	struct SXG_STATS				Stats;				// Statistics
 	u32						ReassBufs;			// Number of reassembly buffers
 	// Card Crash Info
 	ushort						CrashLocation;		// Microcode crash location
@@ -708,7 +703,7 @@ typedef struct _adapter_t
 	//	dma_addr_t		PDumpBuffer;		// Physical address
 	//#endif // SXG_FAILURE_DUMP
 
-} adapter_t, *p_adapter_t;
+};
 
 #if SLIC_DUMP_ENABLED
 #define SLIC_DUMP_REQUESTED      1
@@ -721,10 +716,10 @@ typedef struct _adapter_t
  * structure is written out to the card's SRAM when the microcode panic's.
  *
  ****************************************************************************/
-typedef struct _slic_crash_info {
+struct slic_crash_info {
     ushort  cpu_id;
     ushort  crash_pc;
-} slic_crash_info, *p_slic_crash_info;
+};
 
 #define CRASH_INFO_OFFSET   0x155C
 
diff --git a/drivers/staging/sxg/sxg_os.h b/drivers/staging/sxg/sxg_os.h
index 01182689aaba..6d3f23fb5e1b 100644
--- a/drivers/staging/sxg/sxg_os.h
+++ b/drivers/staging/sxg/sxg_os.h
@@ -44,10 +44,10 @@
 #define FALSE	(0)
 #define TRUE	(1)
 
-typedef struct _LIST_ENTRY {
-	struct _LIST_ENTRY *nle_flink;
-	struct _LIST_ENTRY *nle_blink;
-} list_entry, LIST_ENTRY, *PLIST_ENTRY;
+struct LIST_ENTRY {
+	struct LIST_ENTRY *nle_flink;
+	struct LIST_ENTRY *nle_blink;
+};
 
 #define InitializeListHead(l)                   \
         (l)->nle_flink = (l)->nle_blink = (l)
@@ -68,10 +68,10 @@ typedef struct _LIST_ENTRY {
 
 /* These two have to be inlined since they return things. */
 
-static __inline PLIST_ENTRY RemoveHeadList(list_entry * l)
+static __inline struct LIST_ENTRY *RemoveHeadList(struct LIST_ENTRY *l)
 {
-	list_entry *f;
-	list_entry *e;
+	struct LIST_ENTRY *f;
+	struct LIST_ENTRY *e;
 
 	e = l->nle_flink;
 	f = e->nle_flink;
@@ -81,10 +81,10 @@ static __inline PLIST_ENTRY RemoveHeadList(list_entry * l)
 	return (e);
 }
 
-static __inline PLIST_ENTRY RemoveTailList(list_entry * l)
+static __inline struct LIST_ENTRY *RemoveTailList(struct LIST_ENTRY *l)
 {
-	list_entry *b;
-	list_entry *e;
+	struct LIST_ENTRY *b;
+	struct LIST_ENTRY *e;
 
 	e = l->nle_blink;
 	b = e->nle_blink;
@@ -96,7 +96,7 @@ static __inline PLIST_ENTRY RemoveTailList(list_entry * l)
 
 #define InsertTailList(l, e)                    \
         do {                                    \
-                list_entry              *b;     \
+                struct LIST_ENTRY       *b;     \
                                                 \
                 b = (l)->nle_blink;             \
                 (e)->nle_flink = (l);           \
@@ -107,7 +107,7 @@ static __inline PLIST_ENTRY RemoveTailList(list_entry * l)
 
 #define InsertHeadList(l, e)                    \
         do {                                    \
-                list_entry              *f;     \
+                struct LIST_ENTRY       *f;     \
                                                 \
                 f = (l)->nle_flink;             \
                 (e)->nle_flink = f;             \
diff --git a/drivers/staging/sxg/sxgdbg.h b/drivers/staging/sxg/sxgdbg.h
index 4522b8d71495..bb58ddf39f30 100644
--- a/drivers/staging/sxg/sxgdbg.h
+++ b/drivers/staging/sxg/sxgdbg.h
@@ -86,7 +86,7 @@ extern ulong ATKTimerDiv;
  * needs of the trace entry.  Typically they are function call
  * parameters.
  */
-typedef struct _trace_entry_s {
+struct trace_entry_t {
         char      name[8];        /* 8 character name - like 's'i'm'b'a'r'c'v' */
         u32   time;           /* Current clock tic */
         unsigned char     cpu;            /* Current CPU */
@@ -97,7 +97,7 @@ typedef struct _trace_entry_s {
         u32   arg2;           /* Caller arg2 */
         u32   arg3;           /* Caller arg3 */
         u32   arg4;           /* Caller arg4 */
-} trace_entry_t, *ptrace_entry_t;
+};
 
 /*
  * Driver types for driver field in trace_entry_t
@@ -108,14 +108,13 @@ typedef struct _trace_entry_s {
 
 #define TRACE_ENTRIES   1024
 
-typedef struct _sxg_trace_buffer_t
-{
+struct sxg_trace_buffer_t {
         unsigned int                    size;                  /* aid for windbg extension */
         unsigned int                    in;                    /* Where to add */
         unsigned int                    level;                 /* Current Trace level */
 	spinlock_t	lock;                  /* For MP tracing */
-        trace_entry_t           entries[TRACE_ENTRIES];/* The circular buffer */
-} sxg_trace_buffer_t;
+        struct trace_entry_t           entries[TRACE_ENTRIES];/* The circular buffer */
+};
 
 /*
  * The trace levels
@@ -137,7 +136,7 @@ typedef struct _sxg_trace_buffer_t
 #if ATK_TRACE_ENABLED
 #define SXG_TRACE_INIT(buffer, tlevel)				\
 {								\
-	memset((buffer), 0, sizeof(sxg_trace_buffer_t));	\
+	memset((buffer), 0, sizeof(struct sxg_trace_buffer_t));	\
 	(buffer)->level = (tlevel);				\
 	(buffer)->size = TRACE_ENTRIES;				\
 	spin_lock_init(&(buffer)->lock);			\
@@ -154,7 +153,7 @@ typedef struct _sxg_trace_buffer_t
         if ((buffer) && ((buffer)->level >= (tlevel))) {                      \
                 unsigned int            trace_irql = 0;    /* ?????? FIX THIS  */    \
                 unsigned int            trace_len;                                   \
-                ptrace_entry_t  trace_entry;                                 \
+                struct trace_entry_t	*trace_entry;				\
                 struct timeval  timev;                                       \
                                                                              \
                 spin_lock(&(buffer)->lock);                       \
diff --git a/drivers/staging/sxg/sxghif.h b/drivers/staging/sxg/sxghif.h
index 88bffbaa3be8..a4e94685c544 100644
--- a/drivers/staging/sxg/sxghif.h
+++ b/drivers/staging/sxg/sxghif.h
@@ -12,7 +12,7 @@
 /*******************************************************************************
  * UCODE Registers
  *******************************************************************************/
-typedef struct _SXG_UCODE_REGS {
+struct SXG_UCODE_REGS {
 	// Address 0 - 0x3F = Command codes 0-15 for TCB 0.  Excode 0
 	u32 Icr;		// Code = 0 (extended), ExCode = 0 - Int control
 	u32 RsvdReg1;		// Code = 1 - TOE -NA
@@ -127,7 +127,7 @@ typedef struct _SXG_UCODE_REGS {
 	// base.  As extended codes are added, reduce the first array value in
 	// the following field
 	u32 PadToNextCpu[94][16];	// 94 = 128 - 34 (34 = Excodes 0 - 33)
-} SXG_UCODE_REGS, *PSXG_UCODE_REGS;
+};
 
 // Interrupt control register (0) values
 #define SXG_ICR_DISABLE					0x00000000
@@ -169,7 +169,7 @@ typedef struct _SXG_UCODE_REGS {
  * is happening is that these registers occupy the "PadEx[15]" areas in the
  * SXG_UCODE_REGS definition above
  */
-typedef struct _SXG_TCB_REGS {
+struct SXG_TCB_REGS {
 	u32 ExCode;		/* Extended codes - see SXG_UCODE_REGS */
 	u32 Xmt;		/* Code = 1 - # of Xmt descriptors added to ring */
 	u32 Rcv;		/* Code = 2 - # of Rcv descriptors added to ring */
@@ -180,7 +180,7 @@ typedef struct _SXG_TCB_REGS {
 	u32 Rsvd4;		/* Code = 7 - TOE NA */
 	u32 Rsvd5;		/* Code = 8 - TOE NA */
 	u32 Pad[7];		/* Codes 8-15 - Not used. */
-} SXG_TCB_REGS, *PSXG_TCB_REGS;
+};
 
 /***************************************************************************
  * ISR Format
@@ -272,7 +272,7 @@ typedef struct _SXG_TCB_REGS {
  *
  */
 #pragma pack(push, 1)
-typedef struct _SXG_EVENT {
+struct SXG_EVENT {
 	u32 Pad[1];		// not used
 	u32 SndUna;		// SndUna value
 	u32 Resid;		// receive MDL resid
@@ -294,7 +294,7 @@ typedef struct _SXG_EVENT {
 	unsigned char Code;	// Event code
 	unsigned char CommandIndex;	// New ring index
 	unsigned char Status;	// Event status
-} SXG_EVENT, *PSXG_EVENT;
+};
 #pragma pack(pop)
 
 // Event code definitions
@@ -321,9 +321,9 @@ typedef struct _SXG_EVENT {
 #define EVENT_RING_BATCH	16	// Hand entries back 16 at a time.
 #define EVENT_BATCH_LIMIT	256	// Stop processing events after 256 (16 * 16)
 
-typedef struct _SXG_EVENT_RING {
-	SXG_EVENT Ring[EVENT_RING_SIZE];
-} SXG_EVENT_RING, *PSXG_EVENT_RING;
+struct SXG_EVENT_RING {
+	struct SXG_EVENT Ring[EVENT_RING_SIZE];
+};
 
 /***************************************************************************
  *
@@ -400,12 +400,12 @@ typedef struct _SXG_EVENT_RING {
 #define SXG_MAX_ENTRIES     4096
 
 // Structure and macros to manage a ring
-typedef struct _SXG_RING_INFO {
+struct SXG_RING_INFO {
 	unsigned char Head;	// Where we add entries - Note unsigned char:RING_SIZE
 	unsigned char Tail;	// Where we pull off completed entries
 	ushort Size;		// Ring size - Must be multiple of 2
 	void *Context[SXG_MAX_RING_SIZE];	// Shadow ring
-} SXG_RING_INFO, *PSXG_RING_INFO;
+};
 
 #define SXG_INITIALIZE_RING(_ring, _size) {							\
 	(_ring).Head = 0;												\
@@ -481,7 +481,7 @@ typedef struct _SXG_RING_INFO {
  *  |_________|_________|_________|_________|28		0x1c
  */
 #pragma pack(push, 1)
-typedef struct _SXG_CMD {
+struct SXG_CMD {
 	dma_addr_t Sgl;		// Physical address of SGL
 	union {
 		struct {
@@ -518,14 +518,14 @@ typedef struct _SXG_CMD {
 			unsigned char NotUsed;
 		} Status;
 	};
-} SXG_CMD, *PSXG_CMD;
+};
 #pragma pack(pop)
 
 #pragma pack(push, 1)
-typedef struct _VLAN_HDR {
+struct VLAN_HDR {
 	ushort VlanTci;
 	ushort VlanTpid;
-} VLAN_HDR, *PVLAN_HDR;
+};
 #pragma pack(pop)
 
 /*
@@ -564,22 +564,22 @@ typedef struct _VLAN_HDR {
 #define SXG_SLOWCMD_CSUM_TCP		0x02	// Checksum TCP
 #define SXG_SLOWCMD_LSO				0x04	// Large segment send
 
-typedef struct _SXG_XMT_RING {
-	SXG_CMD Descriptors[SXG_XMT_RING_SIZE];
-} SXG_XMT_RING, *PSXG_XMT_RING;
+struct SXG_XMT_RING {
+	struct SXG_CMD Descriptors[SXG_XMT_RING_SIZE];
+};
 
-typedef struct _SXG_RCV_RING {
-	SXG_CMD Descriptors[SXG_RCV_RING_SIZE];
-} SXG_RCV_RING, *PSXG_RCV_RING;
+struct SXG_RCV_RING {
+	struct SXG_CMD Descriptors[SXG_RCV_RING_SIZE];
+};
 
 /***************************************************************************
  * Share memory buffer types - Used to identify asynchronous
  * shared memory allocation
  ***************************************************************************/
-typedef enum {
+enum SXG_BUFFER_TYPE {
 	SXG_BUFFER_TYPE_RCV,	// Receive buffer
 	SXG_BUFFER_TYPE_SGL	// SGL buffer
-} SXG_BUFFER_TYPE;
+};
 
 // State for SXG buffers
 #define SXG_BUFFER_FREE		0x01
@@ -670,19 +670,19 @@ typedef enum {
 #define SXG_MAX_RCV_BLOCKS				128	// = 16384 receive buffers
 
 // Receive buffer header
-typedef struct _SXG_RCV_DATA_BUFFER_HDR {
+struct SXG_RCV_DATA_BUFFER_HDR {
 	dma_addr_t PhysicalAddress;	// Buffer physical address
 	// Note - DO NOT USE the VirtualAddress field to locate data.
 	// Use the sxg.h:SXG_RECEIVE_DATA_LOCATION macro instead.
 	void *VirtualAddress;	// Start of buffer
-	LIST_ENTRY FreeList;	// Free queue of buffers
-	struct _SXG_RCV_DATA_BUFFER_HDR *Next;	// Fastpath data buffer queue
+	struct LIST_ENTRY FreeList;	// Free queue of buffers
+	struct SXG_RCV_DATA_BUFFER_HDR *Next;	// Fastpath data buffer queue
 	u32 Size;		// Buffer size
 	u32 ByteOffset;		// See SXG_RESTORE_MDL_OFFSET
 	unsigned char State;	// See SXG_BUFFER state above
 	unsigned char Status;	// Event status (to log PUSH)
 	struct sk_buff *skb;	// Double mapped (nbl and pkt)
-} SXG_RCV_DATA_BUFFER_HDR, *PSXG_RCV_DATA_BUFFER_HDR;
+};
 
 // SxgSlowReceive uses the PACKET (skb) contained
 // in the SXG_RCV_DATA_BUFFER_HDR when indicating dumb-nic data
@@ -693,42 +693,43 @@ typedef struct _SXG_RCV_DATA_BUFFER_HDR {
 #define SXG_RCV_JUMBO_BUFFER_SIZE		10240	// jumbo = 10k including HDR
 
 // Receive data descriptor
-typedef struct _SXG_RCV_DATA_DESCRIPTOR {
+struct SXG_RCV_DATA_DESCRIPTOR {
 	union {
 		struct sk_buff *VirtualAddress;	// Host handle
 		u64 ForceTo8Bytes;	// Force x86 to 8-byte boundary
 	};
 	dma_addr_t PhysicalAddress;
-} SXG_RCV_DATA_DESCRIPTOR, *PSXG_RCV_DATA_DESCRIPTOR;
+};
 
 // Receive descriptor block
 #define SXG_RCV_DESCRIPTORS_PER_BLOCK		128
 #define SXG_RCV_DESCRIPTOR_BLOCK_SIZE		2048	// For sanity check
-typedef struct _SXG_RCV_DESCRIPTOR_BLOCK {
-	SXG_RCV_DATA_DESCRIPTOR Descriptors[SXG_RCV_DESCRIPTORS_PER_BLOCK];
-} SXG_RCV_DESCRIPTOR_BLOCK, *PSXG_RCV_DESCRIPTOR_BLOCK;
+
+struct SXG_RCV_DESCRIPTOR_BLOCK {
+	struct SXG_RCV_DATA_DESCRIPTOR Descriptors[SXG_RCV_DESCRIPTORS_PER_BLOCK];
+};
 
 // Receive descriptor block header
-typedef struct _SXG_RCV_DESCRIPTOR_BLOCK_HDR {
+struct SXG_RCV_DESCRIPTOR_BLOCK_HDR {
 	void *VirtualAddress;	// Start of 2k buffer
 	dma_addr_t PhysicalAddress;	// ..and it's physical address
-	LIST_ENTRY FreeList;	// Free queue of descriptor blocks
+	struct LIST_ENTRY FreeList;	// Free queue of descriptor blocks
 	unsigned char State;	// See SXG_BUFFER state above
-} SXG_RCV_DESCRIPTOR_BLOCK_HDR, *PSXG_RCV_DESCRIPTOR_BLOCK_HDR;
+};
 
 // Receive block header
-typedef struct _SXG_RCV_BLOCK_HDR {
+struct SXG_RCV_BLOCK_HDR {
 	void *VirtualAddress;	// Start of virtual memory
 	dma_addr_t PhysicalAddress;	// ..and it's physical address
-	LIST_ENTRY AllList;	// Queue of all SXG_RCV_BLOCKS
-} SXG_RCV_BLOCK_HDR, *PSXG_RCV_BLOCK_HDR;
+	struct LIST_ENTRY AllList;	// Queue of all SXG_RCV_BLOCKS
+};
 
 // Macros to determine data structure offsets into receive block
 #define SXG_RCV_BLOCK_SIZE(_Buffersize) 					\
 	(((_Buffersize) * SXG_RCV_DESCRIPTORS_PER_BLOCK) +		\
-	 (sizeof(SXG_RCV_DESCRIPTOR_BLOCK))              +		\
-	 (sizeof(SXG_RCV_DESCRIPTOR_BLOCK_HDR))          +		\
-	 (sizeof(SXG_RCV_BLOCK_HDR)))
+	 (sizeof(struct SXG_RCV_DESCRIPTOR_BLOCK))              +		\
+	 (sizeof(struct SXG_RCV_DESCRIPTOR_BLOCK_HDR))          +		\
+	 (sizeof(struct SXG_RCV_BLOCK_HDR)))
 #define SXG_RCV_BUFFER_DATA_SIZE(_Buffersize)				\
 	((_Buffersize) - SXG_RCV_DATA_HDR_SIZE)
 #define SXG_RCV_DATA_BUFFER_HDR_OFFSET(_Buffersize)			\
@@ -737,18 +738,18 @@ typedef struct _SXG_RCV_BLOCK_HDR {
 	((_Buffersize) * SXG_RCV_DESCRIPTORS_PER_BLOCK)
 #define SXG_RCV_DESCRIPTOR_BLOCK_HDR_OFFSET(_Buffersize)	\
 	(((_Buffersize) * SXG_RCV_DESCRIPTORS_PER_BLOCK) +		\
-	 (sizeof(SXG_RCV_DESCRIPTOR_BLOCK)))
+	 (sizeof(struct SXG_RCV_DESCRIPTOR_BLOCK)))
 #define SXG_RCV_BLOCK_HDR_OFFSET(_Buffersize)				\
 	(((_Buffersize) * SXG_RCV_DESCRIPTORS_PER_BLOCK) +		\
-	 (sizeof(SXG_RCV_DESCRIPTOR_BLOCK))              +		\
-	 (sizeof(SXG_RCV_DESCRIPTOR_BLOCK_HDR)))
+	 (sizeof(struct SXG_RCV_DESCRIPTOR_BLOCK))              +		\
+	 (sizeof(struct SXG_RCV_DESCRIPTOR_BLOCK_HDR)))
 
 // Use the miniport reserved portion of the NBL to locate
 // our SXG_RCV_DATA_BUFFER_HDR structure.
-typedef struct _SXG_RCV_NBL_RESERVED {
-	PSXG_RCV_DATA_BUFFER_HDR RcvDataBufferHdr;
+struct SXG_RCV_NBL_RESERVED {
+	struct SXG_RCV_DATA_BUFFER_HDR *RcvDataBufferHdr;
 	void *Available;
-} SXG_RCV_NBL_RESERVED, *PSXG_RCV_NBL_RESERVED;
+};
 
 #define SXG_RCV_NBL_BUFFER_HDR(_NBL) (((PSXG_RCV_NBL_RESERVED)NET_BUFFER_LIST_MINIPORT_RESERVED(_NBL))->RcvDataBufferHdr)
 
@@ -760,11 +761,11 @@ typedef struct _SXG_RCV_NBL_RESERVED {
 #define SXG_MAX_SGL_BUFFERS			16384	// Maximum to allocate (note ADAPT:ushort)
 
 // Self identifying structure type
-typedef enum _SXG_SGL_TYPE {
+enum SXG_SGL_TYPE {
 	SXG_SGL_DUMB,		// Dumb NIC SGL
 	SXG_SGL_SLOW,		// Slowpath protocol header - see below
 	SXG_SGL_CHIMNEY		// Chimney offload SGL
-} SXG_SGL_TYPE, PSXG_SGL_TYPE;
+};
 
 // Note - the description below is Microsoft specific
 //
@@ -798,41 +799,41 @@ typedef enum _SXG_SGL_TYPE {
 // to the card directly.  For x86 systems we must reconstruct
 // the SGL.  The following structure defines an x64
 // formatted SGL entry
-typedef struct _SXG_X64_SGE {
+struct SXG_X64_SGE {
 	dma64_addr_t Address;	// same as wdm.h
 	u32 Length;		// same as wdm.h
 	u32 CompilerPad;	// The compiler pads to 8-bytes
 	u64 Reserved;		// u32 * in wdm.h.  Force to 8 bytes
-} SXG_X64_SGE, *PSXG_X64_SGE;
+};
 
-typedef struct _SCATTER_GATHER_ELEMENT {
+struct SCATTER_GATHER_ELEMENT {
 	dma64_addr_t Address;	// same as wdm.h
 	u32 Length;		// same as wdm.h
 	u32 CompilerPad;	// The compiler pads to 8-bytes
 	u64 Reserved;		// u32 * in wdm.h.  Force to 8 bytes
-} SCATTER_GATHER_ELEMENT, *PSCATTER_GATHER_ELEMENT;
+};
 
-typedef struct _SCATTER_GATHER_LIST {
+struct SCATTER_GATHER_LIST {
 	u32 NumberOfElements;
 	u32 *Reserved;
-	SCATTER_GATHER_ELEMENT Elements[];
-} SCATTER_GATHER_LIST, *PSCATTER_GATHER_LIST;
+	struct SCATTER_GATHER_ELEMENT Elements[];
+};
 
 // The card doesn't care about anything except elements, so
 // we can leave the u32 * reserved field alone in the following
 // SGL structure.  But redefine from wdm.h:SCATTER_GATHER_LIST so
 // we can specify SXG_X64_SGE and define a fixed number of elements
-typedef struct _SXG_X64_SGL {
+struct SXG_X64_SGL {
 	u32 NumberOfElements;
 	u32 *Reserved;
-	SXG_X64_SGE Elements[SXG_SGL_ENTRIES];
-} SXG_X64_SGL, *PSXG_X64_SGL;
+	struct SXG_X64_SGE Elements[SXG_SGL_ENTRIES];
+};
 
-typedef struct _SXG_SCATTER_GATHER {
-	SXG_SGL_TYPE Type;	// FIRST! Dumb-nic or offload
+struct SXG_SCATTER_GATHER {
+	enum SXG_SGL_TYPE Type;	// FIRST! Dumb-nic or offload
 	void *adapter;		// Back pointer to adapter
-	LIST_ENTRY FreeList;	// Free SXG_SCATTER_GATHER blocks
-	LIST_ENTRY AllList;	// All SXG_SCATTER_GATHER blocks
+	struct LIST_ENTRY FreeList;	// Free SXG_SCATTER_GATHER blocks
+	struct LIST_ENTRY AllList;	// All SXG_SCATTER_GATHER blocks
 	dma_addr_t PhysicalAddress;	// physical address
 	unsigned char State;	// See SXG_BUFFER state above
 	unsigned char CmdIndex;	// Command ring index
@@ -840,18 +841,18 @@ typedef struct _SXG_SCATTER_GATHER {
 	u32 Direction;		// For asynchronous completions
 	u32 CurOffset;		// Current SGL offset
 	u32 SglRef;		// SGL reference count
-	VLAN_HDR VlanTag;	// VLAN tag to be inserted into SGL
-	PSCATTER_GATHER_LIST pSgl;	// SGL Addr. Possibly &Sgl
-	SXG_X64_SGL Sgl;	// SGL handed to card
-} SXG_SCATTER_GATHER, *PSXG_SCATTER_GATHER;
+	struct VLAN_HDR VlanTag;	// VLAN tag to be inserted into SGL
+	struct SCATTER_GATHER_LIST *pSgl;	// SGL Addr. Possibly &Sgl
+	struct SXG_X64_SGL Sgl;	// SGL handed to card
+};
 
 #if defined(CONFIG_X86_64)
 #define SXG_SGL_BUFFER(_SxgSgl)		(&_SxgSgl->Sgl)
-#define SXG_SGL_BUF_SIZE			sizeof(SXG_X64_SGL)
+#define SXG_SGL_BUF_SIZE			sizeof(struct SXG_X64_SGL)
 #elif defined(CONFIG_X86)
 // Force NDIS to give us it's own buffer so we can reformat to our own
 #define SXG_SGL_BUFFER(_SxgSgl)		NULL
 #define SXG_SGL_BUF_SIZE			0
 #else
-Stop Compilation;
+#error staging: sxg: driver is for X86 only!
 #endif
diff --git a/drivers/staging/sxg/sxghw.h b/drivers/staging/sxg/sxghw.h
index 2222ae91fd97..b0efff9ff117 100644
--- a/drivers/staging/sxg/sxghw.h
+++ b/drivers/staging/sxg/sxghw.h
@@ -48,7 +48,7 @@
 #define SXG_HWREG_MEMSIZE	0x4000		// 16k
 
 #pragma pack(push, 1)
-typedef struct _SXG_HW_REGS {
+struct SXG_HW_REGS {
 	u32		Reset;				// Write 0xdead to invoke soft reset
 	u32		Pad1;				// No register defined at offset 4
 	u32		InterruptMask0;		// Deassert legacy interrupt on function 0
@@ -113,7 +113,7 @@ typedef struct _SXG_HW_REGS {
 	u32		Software[1920];		// 0x200 - 0x2000 - Software defined (not used)
 	u32		MsixTable[1024];	// 0x2000 - 0x3000 - MSIX Table
 	u32		MsixBitArray[1024];	// 0x3000 - 0x4000 - MSIX Pending Bit Array
-} SXG_HW_REGS, *PSXG_HW_REGS;
+};
 #pragma pack(pop)
 
 // Microcode Address Flags
@@ -519,10 +519,10 @@ typedef struct _SXG_HW_REGS {
 #define	XS_LANE_ALIGN			0x1000			// XS transmit lanes aligned
 
 // PHY Microcode download data structure
-typedef struct _PHY_UCODE {
+struct PHY_UCODE {
 	ushort	Addr;
 	ushort	Data;
-} PHY_UCODE, *PPHY_UCODE;
+};
 
 
 /*****************************************************************************
@@ -537,7 +537,7 @@ typedef struct _PHY_UCODE {
 // all commands - see the Sahara spec for details.  Note that this structure is
 // only valid when compiled on a little endian machine.
 #pragma pack(push, 1)
-typedef struct _XMT_DESC {
+struct XMT_DESC {
 	ushort	XmtLen;			// word 0, bits [15:0] -  transmit length
 	unsigned char	XmtCtl;			// word 0, bits [23:16] - transmit control byte
 	unsigned char	Cmd;			// word 0, bits [31:24] - transmit command plus misc.
@@ -551,7 +551,7 @@ typedef struct _XMT_DESC {
 	u32	Rsvd3;			// word 5, bits [31:0] -  PAD
 	u32	Rsvd4;		    // word 6, bits [31:0] -  PAD
 	u32	Rsvd5;		    // word 7, bits [31:0] -  PAD
-} XMT_DESC, *PXMT_DESC;
+};
 #pragma pack(pop)
 
 // XMT_DESC Cmd byte definitions
@@ -600,7 +600,7 @@ typedef struct _XMT_DESC {
 // Format of the 18 byte Receive Buffer returned by the
 // Receive Sequencer for received packets
 #pragma pack(push, 1)
-typedef struct _RCV_BUF_HDR {
+struct RCV_BUF_HDR {
 	u32	Status;				// Status word from Rcv Seq Parser
 	ushort	Length;				// Rcv packet byte count
 	union {
@@ -615,7 +615,7 @@ typedef struct _RCV_BUF_HDR {
 	unsigned char	IpHdrOffset;		// IP header offset into packet
 	u32	TpzHash;			// Toeplitz hash
 	ushort	Reserved;			// Reserved
-} RCV_BUF_HDR, *PRCV_BUF_HDR;
+};
 #pragma pack(pop)
 
 
@@ -665,28 +665,28 @@ typedef struct _RCV_BUF_HDR {
 #pragma pack(push, 1)
 
 /* */
-typedef struct _HW_CFG_DATA {
+struct HW_CFG_DATA {
 	ushort		Addr;
 	union {
 		ushort	Data;
 		ushort	Checksum;
 	};
-} HW_CFG_DATA, *PHW_CFG_DATA;
+};
 
 /* */
-#define	NUM_HW_CFG_ENTRIES	((128/sizeof(HW_CFG_DATA)) - 4)
+#define	NUM_HW_CFG_ENTRIES	((128/sizeof(struct HW_CFG_DATA)) - 4)
 
 /* MAC address */
-typedef struct _SXG_CONFIG_MAC {
+struct SXG_CONFIG_MAC {
 	unsigned char		MacAddr[6];			/* MAC Address */
-} SXG_CONFIG_MAC, *PSXG_CONFIG_MAC;
+};
 
 /* */
-typedef struct _ATK_FRU {
+struct ATK_FRU {
 	unsigned char		PartNum[6];
 	unsigned char		Revision[2];
 	unsigned char		Serial[14];
-} ATK_FRU, *PATK_FRU;
+};
 
 /* OEM FRU Format types */
 #define	ATK_FRU_FORMAT		0x0000
@@ -698,24 +698,24 @@ typedef struct _ATK_FRU {
 #define NO_FRU_FORMAT		0xFFFF
 
 /* EEPROM/Flash Format */
-typedef struct _SXG_CONFIG {
+struct SXG_CONFIG {
 	/* */
 	/* Section 1 (128 bytes) */
 	/* */
 	ushort			MagicWord;			/* EEPROM/FLASH Magic code 'A5A5' */
 	ushort			SpiClks;			/* SPI bus clock dividers */
-	HW_CFG_DATA		HwCfg[NUM_HW_CFG_ENTRIES];
+	struct HW_CFG_DATA		HwCfg[NUM_HW_CFG_ENTRIES];
 	/* */
 	/* */
 	/* */
 	ushort			Version;			/* EEPROM format version */
-	SXG_CONFIG_MAC	MacAddr[4];			/* space for 4 MAC addresses */
-	ATK_FRU			AtkFru;				/* FRU information */
+	struct SXG_CONFIG_MAC	MacAddr[4];			/* space for 4 MAC addresses */
+	struct ATK_FRU			AtkFru;				/* FRU information */
 	ushort			OemFruFormat;		/* OEM FRU format type */
 	unsigned char			OemFru[76];			/* OEM FRU information (optional) */
 	ushort			Checksum;			/* Checksum of section 2 */
 	/* CS info XXXTODO */
-} SXG_CONFIG, *PSXG_CONFIG;
+};
 #pragma pack(pop)
 
 /*****************************************************************************
diff --git a/drivers/staging/sxg/sxgphycode.h b/drivers/staging/sxg/sxgphycode.h
index 8dbaeda7eca4..167f356ef86b 100644
--- a/drivers/staging/sxg/sxgphycode.h
+++ b/drivers/staging/sxg/sxgphycode.h
@@ -18,7 +18,7 @@
 /*
  * Download for AEL2005C PHY with SR/LR transceiver (10GBASE-SR or 10GBASE-LR)
  */
-static PHY_UCODE PhyUcode[] = {
+static struct PHY_UCODE PhyUcode[] = {
 	/*
 	 * NOTE:  An address of 0 is a special case.  When the download routine
 	 * sees an address of 0, it does not write to the PHY.  Instead, it
diff --git a/drivers/staging/usbip/stub.h b/drivers/staging/usbip/stub.h
index f541a3a83bd3..022d0649ac5e 100644
--- a/drivers/staging/usbip/stub.h
+++ b/drivers/staging/usbip/stub.h
@@ -91,5 +91,5 @@ void stub_rx_loop(struct usbip_task *);
 void stub_enqueue_ret_unlink(struct stub_device *, __u32, __u32);
 
 /* stub_main.c */
-int match_busid(char *busid);
+int match_busid(const char *busid);
 void stub_device_cleanup_urbs(struct stub_device *sdev);
diff --git a/drivers/staging/usbip/stub_dev.c b/drivers/staging/usbip/stub_dev.c
index ee455a087eaf..1e320caf3d11 100644
--- a/drivers/staging/usbip/stub_dev.c
+++ b/drivers/staging/usbip/stub_dev.c
@@ -389,7 +389,7 @@ static int stub_probe(struct usb_interface *interface,
 {
 	struct usb_device *udev = interface_to_usbdev(interface);
 	struct stub_device *sdev = NULL;
-	char *udev_busid = interface->dev.parent->bus_id;
+	const char *udev_busid = dev_name(interface->dev.parent);
 	int err = 0;
 
 	dev_dbg(&interface->dev, "Enter\n");
diff --git a/drivers/staging/usbip/stub_main.c b/drivers/staging/usbip/stub_main.c
index c665d7f1ca9a..05e4c6064390 100644
--- a/drivers/staging/usbip/stub_main.c
+++ b/drivers/staging/usbip/stub_main.c
@@ -40,11 +40,12 @@ struct kmem_cache *stub_priv_cache;
  * remote host.
  */
 #define MAX_BUSID 16
-static char busid_table[MAX_BUSID][BUS_ID_SIZE];
+#define BUSID_SIZE 20
+static char busid_table[MAX_BUSID][BUSID_SIZE];
 static spinlock_t busid_table_lock;
 
 
-int match_busid(char *busid)
+int match_busid(const char *busid)
 {
 	int i;
 
@@ -52,7 +53,7 @@ int match_busid(char *busid)
 
 	for (i = 0; i < MAX_BUSID; i++)
 		if (busid_table[i][0])
-			if (!strncmp(busid_table[i], busid, BUS_ID_SIZE)) {
+			if (!strncmp(busid_table[i], busid, BUSID_SIZE)) {
 				/* already registerd */
 				spin_unlock(&busid_table_lock);
 				return 0;
@@ -92,7 +93,7 @@ static int add_match_busid(char *busid)
 
 	for (i = 0; i < MAX_BUSID; i++)
 		if (!busid_table[i][0]) {
-			strncpy(busid_table[i], busid, BUS_ID_SIZE);
+			strncpy(busid_table[i], busid, BUSID_SIZE);
 			spin_unlock(&busid_table_lock);
 			return 0;
 		}
@@ -109,9 +110,9 @@ static int del_match_busid(char *busid)
 	spin_lock(&busid_table_lock);
 
 	for (i = 0; i < MAX_BUSID; i++)
-		if (!strncmp(busid_table[i], busid, BUS_ID_SIZE)) {
+		if (!strncmp(busid_table[i], busid, BUSID_SIZE)) {
 			/* found */
-			memset(busid_table[i], 0, BUS_ID_SIZE);
+			memset(busid_table[i], 0, BUSID_SIZE);
 			spin_unlock(&busid_table_lock);
 			return 0;
 		}
@@ -125,19 +126,19 @@ static ssize_t store_match_busid(struct device_driver *dev, const char *buf,
 		size_t count)
 {
 	int len;
-	char busid[BUS_ID_SIZE];
+	char busid[BUSID_SIZE];
 
 	if (count < 5)
 		return -EINVAL;
 
 	/* strnlen() does not include \0 */
-	len = strnlen(buf + 4, BUS_ID_SIZE);
+	len = strnlen(buf + 4, BUSID_SIZE);
 
 	/* busid needs to include \0 termination */
-	if (!(len < BUS_ID_SIZE))
+	if (!(len < BUSID_SIZE))
 		return -EINVAL;
 
-	strncpy(busid, buf + 4, BUS_ID_SIZE);
+	strncpy(busid, buf + 4, BUSID_SIZE);
 
 
 	if (!strncmp(buf, "add ", 4)) {
diff --git a/drivers/staging/usbip/stub_rx.c b/drivers/staging/usbip/stub_rx.c
index 36ce898fced5..2eb61372fe0a 100644
--- a/drivers/staging/usbip/stub_rx.c
+++ b/drivers/staging/usbip/stub_rx.c
@@ -157,7 +157,7 @@ static int tweak_set_configuration_cmd(struct urb *urb)
 	 * A user may need to set a special configuration value before
 	 * exporting the device.
 	 */
-	uinfo("set_configuration (%d) to %s\n", config, urb->dev->dev.bus_id);
+	uinfo("set_configuration (%d) to %s\n", config, dev_name(&urb->dev->dev));
 	uinfo("but, skip!\n");
 
 	return 0;
@@ -175,7 +175,7 @@ static int tweak_reset_device_cmd(struct urb *urb)
 	value = le16_to_cpu(req->wValue);
 	index = le16_to_cpu(req->wIndex);
 
-	uinfo("reset_device (port %d) to %s\n", index, urb->dev->dev.bus_id);
+	uinfo("reset_device (port %d) to %s\n", index, dev_name(&urb->dev->dev));
 
 	/* all interfaces should be owned by usbip driver, so just reset it.  */
 	ret = usb_lock_device_for_reset(urb->dev, NULL);
@@ -234,8 +234,6 @@ static void tweak_special_requests(struct urb *urb)
 static int stub_recv_cmd_unlink(struct stub_device *sdev,
 						struct usbip_header *pdu)
 {
-	struct list_head *listhead = &sdev->priv_init;
-	struct list_head *ptr;
 	unsigned long flags;
 
 	struct stub_priv *priv;
@@ -243,8 +241,7 @@ static int stub_recv_cmd_unlink(struct stub_device *sdev,
 
 	spin_lock_irqsave(&sdev->priv_lock, flags);
 
-	for (ptr = listhead->next; ptr != listhead; ptr = ptr->next) {
-		priv = list_entry(ptr, struct stub_priv, list);
+	list_for_each_entry(priv, &sdev->priv_init, list) {
 		if (priv->seqnum == pdu->u.cmd_unlink.seqnum) {
 			int ret;
 
diff --git a/drivers/staging/usbip/stub_tx.c b/drivers/staging/usbip/stub_tx.c
index d5563cd980be..78058f5362b4 100644
--- a/drivers/staging/usbip/stub_tx.c
+++ b/drivers/staging/usbip/stub_tx.c
@@ -54,7 +54,6 @@ void stub_enqueue_ret_unlink(struct stub_device *sdev, __u32 seqnum,
 /**
  * stub_complete - completion handler of a usbip urb
  * @urb: pointer to the urb completed
- * @regs:
  *
  * When a urb has completed, the USB core driver calls this function mostly in
  * the interrupt context. To return the result of a urb, the completed urb is
diff --git a/drivers/staging/usbip/vhci_hcd.c b/drivers/staging/usbip/vhci_hcd.c
index 5b5a2e348ecb..f69ca346aa2f 100644
--- a/drivers/staging/usbip/vhci_hcd.c
+++ b/drivers/staging/usbip/vhci_hcd.c
@@ -1091,7 +1091,7 @@ static int vhci_hcd_probe(struct platform_device *pdev)
 	 * Allocate and initialize hcd.
 	 * Our private data is also allocated automatically.
 	 */
-	hcd = usb_create_hcd(&vhci_hc_driver, &pdev->dev, pdev->dev.bus_id);
+	hcd = usb_create_hcd(&vhci_hc_driver, &pdev->dev, dev_name(&pdev->dev));
 	if (!hcd) {
 		uerr("create hcd failed\n");
 		return -ENOMEM;
diff --git a/drivers/staging/usbip/vhci_sysfs.c b/drivers/staging/usbip/vhci_sysfs.c
index 24c2851a8f84..0fd33a62d934 100644
--- a/drivers/staging/usbip/vhci_sysfs.c
+++ b/drivers/staging/usbip/vhci_sysfs.c
@@ -60,7 +60,7 @@ static ssize_t show_status(struct device *dev, struct device_attribute *attr,
 			out += sprintf(out, "%03u %08x ",
 					vdev->speed, vdev->devid);
 			out += sprintf(out, "%16p ", vdev->ud.tcp_socket);
-			out += sprintf(out, "%s", vdev->udev->dev.bus_id);
+			out += sprintf(out, "%s", dev_name(&vdev->udev->dev));
 
 		} else
 			out += sprintf(out, "000 000 000 0000000000000000 0-0");
diff --git a/drivers/staging/winbond/Kconfig b/drivers/staging/winbond/Kconfig
index 425219ed7ab9..940460c39f36 100644
--- a/drivers/staging/winbond/Kconfig
+++ b/drivers/staging/winbond/Kconfig
@@ -1,7 +1,11 @@
 config W35UND
-	tristate "Winbond driver"
-	depends on MAC80211 && WLAN_80211 && USB && EXPERIMENTAL && !4KSTACKS
+	tristate "IS89C35 WLAN USB driver"
+	depends on MAC80211 && WLAN_80211 && USB && EXPERIMENTAL
 	default n
 	---help---
-	  This is highly experimental driver for winbond wifi card on some Kohjinsha notebooks
-	  Check http://code.google.com/p/winbondport/ for new version
+	  This is highly experimental driver for Winbond WIFI card.
+
+	  Hardware is present in some Kohjinsha subnotebooks, and in some
+	  stand-alone USB modules. Chipset name seems to be w89c35d.
+
+	  Check http://code.google.com/p/winbondport/ for new version.
diff --git a/drivers/staging/winbond/Makefile b/drivers/staging/winbond/Makefile
index 29c98bf1bc98..b49c9730eddf 100644
--- a/drivers/staging/winbond/Makefile
+++ b/drivers/staging/winbond/Makefile
@@ -1,15 +1,14 @@
-	DRIVER_DIR=./linux
-
-w35und-objs := $(DRIVER_DIR)/wbusb.o $(DRIVER_DIR)/wb35reg.o $(DRIVER_DIR)/wb35rx.o $(DRIVER_DIR)/wb35tx.o \
-	mds.o \
-	mlmetxrx.o \
-	mto.o	\
+w35und-objs :=			\
+	mds.o			\
+	mlmetxrx.o		\
+	mto.o			\
 	phy_calibration.o	\
 	reg.o			\
-	rxisr.o			\
-	sme_api.o		\
+	wb35reg.o		\
+	wb35rx.o		\
+	wb35tx.o		\
 	wbhal.o			\
-	wblinux.o		\
+	wbusb.o			\
 
 
 obj-$(CONFIG_W35UND) += w35und.o
diff --git a/drivers/staging/winbond/adapter.h b/drivers/staging/winbond/adapter.h
deleted file mode 100644
index 609701d21cf0..000000000000
--- a/drivers/staging/winbond/adapter.h
+++ /dev/null
@@ -1,23 +0,0 @@
-//
-// ADAPTER.H -
-// Windows NDIS global variable 'Adapter' typedef
-//
-#define MAX_ANSI_STRING		40
-typedef struct WB32_ADAPTER
-{
-	u32 AdapterIndex; // 20060703.4 Add for using pAdapterContext global Adapter point
-
-	WB_LOCALDESCRIPT	sLocalPara;		// Myself connected parameters
-	PWB_BSSDESCRIPTION	asBSSDescriptElement;
-
-	MLME_FRAME		sMlmeFrame;		// connect to peerSTA parameters
-
-	MTO_PARAMETERS		sMtoPara; // MTO_struct ...
-	hw_data_t			sHwData; //For HAL
-	MDS					Mds;
-
-	WBLINUX		WbLinux;
-        struct iw_statistics iw_stats;
-
-	u8	LinkName[MAX_ANSI_STRING];
-} WB32_ADAPTER, ADAPTER, *PWB32_ADAPTER, *PADAPTER;
diff --git a/drivers/staging/winbond/bss_f.h b/drivers/staging/winbond/bss_f.h
index 013183153993..a433b5a85924 100644
--- a/drivers/staging/winbond/bss_f.h
+++ b/drivers/staging/winbond/bss_f.h
@@ -1,59 +1,63 @@
+#ifndef __WINBOND_BSS_F_H
+#define __WINBOND_BSS_F_H
+
+#include "core.h"
+
+struct PMKID_Information_Element;
+
 //
 // BSS descriptor DataBase management global function
 //
 
-void vBSSdescriptionInit(PWB32_ADAPTER Adapter);
-void vBSSfoundList(PWB32_ADAPTER Adapter);
-u8 boChanFilter(PWB32_ADAPTER Adapter, u8 ChanNo);
-u16 wBSSallocateEntry(PWB32_ADAPTER Adapter);
-u16 wBSSGetEntry(PWB32_ADAPTER Adapter);
-void vSimpleHouseKeeping(PWB32_ADAPTER Adapter);
-u16 wBSShouseKeeping(PWB32_ADAPTER Adapter);
-void ClearBSSdescpt(PWB32_ADAPTER Adapter, u16 i);
-u16 wBSSfindBssID(PWB32_ADAPTER Adapter, u8 *pbBssid);
-u16 wBSSfindDedicateCandidate(PWB32_ADAPTER Adapter, struct SSID_Element *psSsid, u8 *pbBssid);
-u16 wBSSfindMACaddr(PWB32_ADAPTER Adapter, u8 *pbMacAddr);
-u16 wBSSsearchMACaddr(PWB32_ADAPTER Adapter, u8 *pbMacAddr, u8 band);
-u16 wBSSaddScanData(PWB32_ADAPTER, u16, psRXDATA);
-u16 wBSSUpdateScanData(PWB32_ADAPTER Adapter, u16 wBssIdx, psRXDATA psRcvData);
-u16 wBSScreateIBSSdata(PWB32_ADAPTER Adapter, PWB_BSSDESCRIPTION psDesData);
-void DesiredRate2BSSdescriptor(PWB32_ADAPTER Adapter, PWB_BSSDESCRIPTION psDesData,
+void vBSSdescriptionInit(struct wbsoft_priv * adapter);
+void vBSSfoundList(struct wbsoft_priv * adapter);
+u8 boChanFilter(struct wbsoft_priv * adapter, u8 ChanNo);
+u16 wBSSallocateEntry(struct wbsoft_priv * adapter);
+u16 wBSSGetEntry(struct wbsoft_priv * adapter);
+void vSimpleHouseKeeping(struct wbsoft_priv * adapter);
+u16 wBSShouseKeeping(struct wbsoft_priv * adapter);
+void ClearBSSdescpt(struct wbsoft_priv * adapter, u16 i);
+u16 wBSSfindBssID(struct wbsoft_priv * adapter, u8 *pbBssid);
+u16 wBSSfindDedicateCandidate(struct wbsoft_priv * adapter, struct SSID_Element *psSsid, u8 *pbBssid);
+u16 wBSSfindMACaddr(struct wbsoft_priv * adapter, u8 *pbMacAddr);
+u16 wBSSsearchMACaddr(struct wbsoft_priv * adapter, u8 *pbMacAddr, u8 band);
+u16 wBSSaddScanData(struct wbsoft_priv *, u16, psRXDATA);
+u16 wBSSUpdateScanData(struct wbsoft_priv * adapter, u16 wBssIdx, psRXDATA psRcvData);
+u16 wBSScreateIBSSdata(struct wbsoft_priv * adapter, PWB_BSSDESCRIPTION psDesData);
+void DesiredRate2BSSdescriptor(struct wbsoft_priv * adapter, PWB_BSSDESCRIPTION psDesData,
 							 u8 *pBasicRateSet, u8 BasicRateCount,
 							 u8 *pOperationRateSet, u8 OperationRateCount);
-void DesiredRate2InfoElement(PWB32_ADAPTER Adapter, u8	*addr, u16 *iFildOffset,
+void DesiredRate2InfoElement(struct wbsoft_priv * adapter, u8	*addr, u16 *iFildOffset,
 							 u8 *pBasicRateSet, u8 BasicRateCount,
 							 u8 *pOperationRateSet, u8 OperationRateCount);
-void BSSAddIBSSdata(PWB32_ADAPTER Adapter, PWB_BSSDESCRIPTION psDesData);
+void BSSAddIBSSdata(struct wbsoft_priv * adapter, PWB_BSSDESCRIPTION psDesData);
 unsigned char boCmpMacAddr( u8 *, u8 *);
 unsigned char boCmpSSID(struct SSID_Element *psSSID1, struct SSID_Element *psSSID2);
-u16 wBSSfindSSID(PWB32_ADAPTER Adapter, struct SSID_Element *psSsid);
-u16 wRoamingQuery(PWB32_ADAPTER Adapter);
-void vRateToBitmap(PWB32_ADAPTER Adapter, u16 index);
-u8 bRateToBitmapIndex(PWB32_ADAPTER Adapter, u8 bRate);
+u16 wBSSfindSSID(struct wbsoft_priv * adapter, struct SSID_Element *psSsid);
+u16 wRoamingQuery(struct wbsoft_priv * adapter);
+void vRateToBitmap(struct wbsoft_priv * adapter, u16 index);
+u8 bRateToBitmapIndex(struct wbsoft_priv * adapter, u8 bRate);
 u8 bBitmapToRate(u8 i);
-unsigned char boIsERPsta(PWB32_ADAPTER Adapter, u16 i);
-unsigned char boCheckConnect(PWB32_ADAPTER Adapter);
-unsigned char boCheckSignal(PWB32_ADAPTER Adapter);
-void AddIBSSIe(PWB32_ADAPTER Adapter,PWB_BSSDESCRIPTION psDesData );//added by ws for WPA_None06/01/04
-void BssScanUpToDate(PWB32_ADAPTER Adapter);
-void BssUpToDate(PWB32_ADAPTER Adapter);
+unsigned char boIsERPsta(struct wbsoft_priv * adapter, u16 i);
+unsigned char boCheckConnect(struct wbsoft_priv * adapter);
+unsigned char boCheckSignal(struct wbsoft_priv * adapter);
+void AddIBSSIe(struct wbsoft_priv * adapter,PWB_BSSDESCRIPTION psDesData );//added by ws for WPA_None06/01/04
+void BssScanUpToDate(struct wbsoft_priv * adapter);
+void BssUpToDate(struct wbsoft_priv * adapter);
 void RateSort(u8 *RateArray, u8 num, u8 mode);
-void RateReSortForSRate(PWB32_ADAPTER Adapter, u8 *RateArray, u8 num);
-void Assemble_IE(PWB32_ADAPTER Adapter, u16 wBssIdx);
-void SetMaxTxRate(PWB32_ADAPTER Adapter);
+void RateReSortForSRate(struct wbsoft_priv * adapter, u8 *RateArray, u8 num);
+void Assemble_IE(struct wbsoft_priv * adapter, u16 wBssIdx);
+void SetMaxTxRate(struct wbsoft_priv * adapter);
 
-void CreateWpaIE(PWB32_ADAPTER Adapter, u16* iFildOffset, u8 *msg, struct  Management_Frame* msgHeader,
+void CreateWpaIE(struct wbsoft_priv * adapter, u16* iFildOffset, u8 *msg, struct  Management_Frame* msgHeader,
 				 struct Association_Request_Frame_Body* msgBody, u16 iMSindex); //added by WS 05/14/05
 
 #ifdef _WPA2_
-void CreateRsnIE(PWB32_ADAPTER Adapter, u16* iFildOffset, u8 *msg, struct  Management_Frame* msgHeader,
+void CreateRsnIE(struct wbsoft_priv * adapter, u16* iFildOffset, u8 *msg, struct  Management_Frame* msgHeader,
 				 struct Association_Request_Frame_Body* msgBody, u16 iMSindex);//added by WS 05/14/05
 
-u16 SearchPmkid(PWB32_ADAPTER Adapter, struct  Management_Frame* msgHeader,
+u16 SearchPmkid(struct wbsoft_priv * adapter, struct  Management_Frame* msgHeader,
 				   struct PMKID_Information_Element * AssoReq_PMKID );
 #endif
 
-
-
-
-
+#endif
diff --git a/drivers/staging/winbond/bssdscpt.h b/drivers/staging/winbond/bssdscpt.h
index 97150a2655fb..3a71d4efb897 100644
--- a/drivers/staging/winbond/bssdscpt.h
+++ b/drivers/staging/winbond/bssdscpt.h
@@ -1,3 +1,11 @@
+#ifndef __WINBOND_BSSDSCPT_H
+#define __WINBOND_BSSDSCPT_H
+
+#include <linux/types.h>
+
+#include "mds_s.h"
+#include "mlme_s.h"
+
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //	bssdscpt.c
 //		BSS descriptor data base
@@ -78,8 +86,8 @@ typedef struct BSSDescriptionElement
     u16		wState;			// the current state of the system
 	u16		wIndex;			// THIS BSS element entry index
 
-	void*	psAdapter;		// pointer to THIS Adapter
-	OS_TIMER	nTimer;  // MLME timer
+	void*	psadapter;		// pointer to THIS adapter
+	struct timer_list timer;  // MLME timer
 
     // Authentication
     u16		wAuthAlgo;      // peer MAC MLME use Auth algorithm, default OPEN_AUTH
@@ -148,9 +156,9 @@ typedef struct BSSDescriptionElement
 
 } WB_BSSDESCRIPTION, *PWB_BSSDESCRIPTION;
 
-#define wBSSConnectedSTA(Adapter)             \
-    ((u16)(Adapter)->sLocalPara.wConnectedSTAindex)
-
-#define psBSS(i)			(&(Adapter->asBSSDescriptElement[(i)]))
+#define wBSSConnectedSTA(adapter)             \
+    ((u16)(adapter)->sLocalPara.wConnectedSTAindex)
 
+#define psBSS(i)			(&(adapter->asBSSDescriptElement[(i)]))
 
+#endif
diff --git a/drivers/staging/winbond/common.h b/drivers/staging/winbond/common.h
new file mode 100644
index 000000000000..c4d96041e9ac
--- /dev/null
+++ b/drivers/staging/winbond/common.h
@@ -0,0 +1,27 @@
+//
+// common.h
+//
+// This file contains the OS dependant definition and function.
+// Every OS has this file individual.
+//
+
+#define DebugUsbdStatusInformation( _A )
+
+#ifndef COMMON_DEF
+#define COMMON_DEF
+
+//#define DEBUG_ENABLED  1
+
+//==================================================================================================
+// Common function definition
+//==================================================================================================
+#define DEBUG_ENABLED
+#define ETH_LENGTH_OF_ADDRESS	6
+#ifdef DEBUG_ENABLED
+#define WBDEBUG( _M )	printk _M
+#else
+#define WBDEBUG( _M )	0
+#endif
+
+#endif // COMMON_DEF
+
diff --git a/drivers/staging/winbond/core.h b/drivers/staging/winbond/core.h
new file mode 100644
index 000000000000..fe142a10ea80
--- /dev/null
+++ b/drivers/staging/winbond/core.h
@@ -0,0 +1,42 @@
+#ifndef __WINBOND_CORE_H
+#define __WINBOND_CORE_H
+
+#include <linux/wireless.h>
+
+#include "bssdscpt.h"
+#include "mto.h"
+#include "wbhal_s.h"
+
+#define WBLINUX_PACKET_ARRAY_SIZE (ETHERNET_TX_DESCRIPTORS*4)
+
+#define WB_MAX_LINK_NAME_LEN 40
+
+struct wbsoft_priv {
+	u32 adapterIndex;	// 20060703.4 Add for using padapterContext global adapter point
+
+	WB_LOCALDESCRIPT sLocalPara;	// Myself connected parameters
+	PWB_BSSDESCRIPTION asBSSDescriptElement;
+
+	MLME_FRAME sMlmeFrame;	// connect to peerSTA parameters
+
+	MTO_PARAMETERS sMtoPara;	// MTO_struct ...
+	hw_data_t sHwData;	//For HAL
+	MDS Mds;
+
+	spinlock_t SpinLock;
+
+	atomic_t ThreadCount;
+
+	u32 RxByteCount;
+	u32 TxByteCount;
+
+	struct sk_buff *packet_return;
+	s32 netif_state_stop;	// 1: stop  0: normal
+	struct iw_statistics iw_stats;
+
+	u8 LinkName[WB_MAX_LINK_NAME_LEN];
+
+	bool enabled;
+};
+
+#endif /* __WINBOND_CORE_H */
diff --git a/drivers/staging/winbond/ds_tkip.h b/drivers/staging/winbond/ds_tkip.h
index 6841d66e7e8c..9c5c4e73f2c2 100644
--- a/drivers/staging/winbond/ds_tkip.h
+++ b/drivers/staging/winbond/ds_tkip.h
@@ -1,3 +1,8 @@
+#ifndef __WINBOND_DS_TKIP_H
+#define __WINBOND_DS_TKIP_H
+
+#include <linux/types.h>
+
 // Rotation functions on 32 bit values
 #define ROL32( A, n ) \
     ( ((A) << (n)) | ( ((A)>>(32-(n)))  & ( (1UL << (n)) - 1 ) ) )
@@ -26,8 +31,7 @@ typedef struct tkip
 } tkip_t;
 
 //void _append_data( u8 *pData, u16 size, tkip_t *p );
-void Mds_MicGet(  void* Adapter,  void* pRxLayer1,  u8 *pKey,  u8 *pMic );
-void Mds_MicFill(  void* Adapter,  void* pDes,  u8 *XmitBufAddress );
-
-
+void Mds_MicGet(  void* adapter,  void* pRxLayer1,  u8 *pKey,  u8 *pMic );
+void Mds_MicFill(  void* adapter,  void* pDes,  u8 *XmitBufAddress );
 
+#endif
diff --git a/drivers/staging/winbond/gl_80211.h b/drivers/staging/winbond/gl_80211.h
index 1806d817496e..5a244c44a61a 100644
--- a/drivers/staging/winbond/gl_80211.h
+++ b/drivers/staging/winbond/gl_80211.h
@@ -1,7 +1,8 @@
-
 #ifndef __GL_80211_H__
 #define __GL_80211_H__
 
+#include <linux/types.h>
+
 /****************** CONSTANT AND MACRO SECTION ******************************/
 
 /* BSS Type */
diff --git a/drivers/staging/winbond/linux/common.h b/drivers/staging/winbond/linux/common.h
deleted file mode 100644
index 712a86cfa68b..000000000000
--- a/drivers/staging/winbond/linux/common.h
+++ /dev/null
@@ -1,128 +0,0 @@
-//
-// common.h
-//
-// This file contains the OS dependant definition and function.
-// Every OS has this file individual.
-//
-
-#define DebugUsbdStatusInformation( _A )
-
-#ifndef COMMON_DEF
-#define COMMON_DEF
-
-#include <linux/version.h>
-#include <linux/usb.h>
-#include <linux/kernel.h> //need for kernel alert
-#include <linux/autoconf.h>
-#include <linux/sched.h>
-#include <linux/signal.h>
-#include <linux/slab.h> //memory allocate
-#include <linux/module.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/init.h>//need for init and exit modules marco
-#include <linux/ctype.h>
-#include <linux/wait.h>
-#include <linux/list.h>
-#include <linux/wireless.h>
-#include <linux/if_arp.h>
-#include <asm/uaccess.h>
-#include <net/iw_handler.h>
-#include <linux/skbuff.h>
-
-
-//#define DEBUG_ENABLED  1
-
-
-
-//===============================================================
-// Common type definition
-//===============================================================
-
-//===========================================
-#define IGNORE      2
-#define	SUCCESS     1
-#define	FAILURE     0
-
-
-#ifndef true
-#define true        1
-#endif
-
-#ifndef false
-#define false       0
-#endif
-
-// PD43 20021108
-#ifndef TRUE
-#define TRUE        1
-#endif
-
-#ifndef FALSE
-#define FALSE       0
-#endif
-
-#define STATUS_MEDIA_CONNECT 1
-#define STATUS_MEDIA_DISCONNECT 0
-
-#ifndef BIT
-#define BIT(x)                  (1 << (x))
-#endif
-
-typedef struct urb * PURB;
-
-
-
-//==================================================================================================
-// Common function definition
-//==================================================================================================
-#ifndef abs
-#define abs(_T)							((_T) < 0 ? -_T : _T)
-#endif
-#define DEBUG_ENABLED
-#define ETH_LENGTH_OF_ADDRESS	6
-#ifdef DEBUG_ENABLED
-#define WBDEBUG( _M )	printk _M
-#else
-#define WBDEBUG( _M )	0
-#endif
-
-#define OS_DISCONNECTED	0
-#define OS_CONNECTED	1
-
-
-#define OS_EVENT_INDICATE( _A, _B, _F )
-#define OS_PMKID_STATUS_EVENT( _A )
-
-
-/* Uff, no, longs are not atomic on all architectures Linux
- * supports. This should really use atomic_t */
-
-#define OS_ATOMIC			u32
-#define OS_ATOMIC_READ( _A, _V )	_V
-#define OS_ATOMIC_INC( _A, _V )		EncapAtomicInc( _A, (void*)_V )
-#define OS_ATOMIC_DEC( _A, _V )		EncapAtomicDec( _A, (void*)_V )
-#define OS_MEMORY_CLEAR( _A, _S )	memset( (u8 *)_A,0,_S)
-#define OS_MEMORY_COMPARE( _A, _B, _S )	(memcmp(_A,_B,_S)? 0 : 1) // Definition is reverse with Ndis 1: the same 0: different
-
-#define OS_TIMER	struct timer_list
-#define OS_TIMER_INITIAL( _T, _F, _P )			\
-{							\
-	init_timer( _T );				\
-	(_T)->function = (void *)_F##_1a;		\
-	(_T)->data = (unsigned long)_P;			\
-}
-
-// _S : Millisecond
-// 20060420 At least 1 large than jiffies
-#define OS_TIMER_SET( _T, _S )					\
-{								\
-	(_T)->expires = jiffies + ((_S*HZ+999)/1000);\
-	add_timer( _T );					\
-}
-#define OS_TIMER_CANCEL( _T, _B )		del_timer_sync( _T )
-#define OS_TIMER_GET_SYS_TIME( _T )		(*_T=jiffies)
-
-
-#endif // COMMON_DEF
-
diff --git a/drivers/staging/winbond/linux/wb35rx_f.h b/drivers/staging/winbond/linux/wb35rx_f.h
deleted file mode 100644
index daa3e73042bd..000000000000
--- a/drivers/staging/winbond/linux/wb35rx_f.h
+++ /dev/null
@@ -1,17 +0,0 @@
-//====================================
-// Interface function declare
-//====================================
-void		Wb35Rx_reset_descriptor(  phw_data_t pHwData );
-unsigned char		Wb35Rx_initial(  phw_data_t pHwData );
-void		Wb35Rx_destroy(  phw_data_t pHwData );
-void		Wb35Rx_stop(  phw_data_t pHwData );
-u16		Wb35Rx_indicate(  phw_data_t pHwData );
-void		Wb35Rx_adjust(  PDESCRIPTOR pRxDes );
-void		Wb35Rx_start(  phw_data_t pHwData );
-
-void		Wb35Rx(  phw_data_t pHwData );
-void		Wb35Rx_Complete(  PURB pUrb );
-
-
-
-
diff --git a/drivers/staging/winbond/linux/wb35tx_f.h b/drivers/staging/winbond/linux/wb35tx_f.h
deleted file mode 100644
index 107b12918137..000000000000
--- a/drivers/staging/winbond/linux/wb35tx_f.h
+++ /dev/null
@@ -1,20 +0,0 @@
-//====================================
-// Interface function declare
-//====================================
-unsigned char Wb35Tx_initial(	 phw_data_t pHwData );
-void Wb35Tx_destroy(  phw_data_t pHwData );
-unsigned char Wb35Tx_get_tx_buffer(  phw_data_t pHwData,  u8 **pBuffer );
-
-void Wb35Tx_EP2VM(  phw_data_t pHwData );
-void Wb35Tx_EP2VM_start(  phw_data_t pHwData );
-void Wb35Tx_EP2VM_complete(  PURB purb );
-
-void Wb35Tx_start(  phw_data_t pHwData );
-void Wb35Tx_stop(  phw_data_t pHwData );
-void Wb35Tx(  phw_data_t pHwData );
-void Wb35Tx_complete(  PURB purb );
-void Wb35Tx_reset_descriptor(  phw_data_t pHwData );
-
-void Wb35Tx_CurrentTime(  phw_data_t pHwData,  u32 TimeCount );
-
-
diff --git a/drivers/staging/winbond/linux/wbusb.c b/drivers/staging/winbond/linux/wbusb.c
deleted file mode 100644
index 39ca9b9878f8..000000000000
--- a/drivers/staging/winbond/linux/wbusb.c
+++ /dev/null
@@ -1,391 +0,0 @@
-/*
- * Copyright 2008 Pavel Machek <pavel@suse.cz>
- *
- * Distribute under GPLv2.
- */
-#include "sysdef.h"
-#include <net/mac80211.h>
-
-MODULE_AUTHOR(DRIVER_AUTHOR);
-MODULE_DESCRIPTION(DRIVER_DESC);
-MODULE_LICENSE("GPL");
-MODULE_VERSION("0.1");
-
-static struct usb_device_id wb35_table[] __devinitdata = {
-	{USB_DEVICE(0x0416, 0x0035)},
-	{USB_DEVICE(0x18E8, 0x6201)},
-	{USB_DEVICE(0x18E8, 0x6206)},
-	{USB_DEVICE(0x18E8, 0x6217)},
-	{USB_DEVICE(0x18E8, 0x6230)},
-	{USB_DEVICE(0x18E8, 0x6233)},
-	{USB_DEVICE(0x1131, 0x2035)},
-	{ 0, }
-};
-
-MODULE_DEVICE_TABLE(usb, wb35_table);
-
-static struct ieee80211_rate wbsoft_rates[] = {
-	{ .bitrate = 10, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
-};
-
-static struct ieee80211_channel wbsoft_channels[] = {
-	{ .center_freq = 2412},
-};
-
-int wbsoft_enabled;
-struct ieee80211_hw *my_dev;
-PADAPTER my_adapter;
-
-static int wbsoft_add_interface(struct ieee80211_hw *dev,
-				 struct ieee80211_if_init_conf *conf)
-{
-	printk("wbsoft_add interface called\n");
-	return 0;
-}
-
-static void wbsoft_remove_interface(struct ieee80211_hw *dev,
-				     struct ieee80211_if_init_conf *conf)
-{
-	printk("wbsoft_remove interface called\n");
-}
-
-static void wbsoft_stop(struct ieee80211_hw *hw)
-{
-	printk(KERN_INFO "%s called\n", __func__);
-}
-
-static int wbsoft_get_stats(struct ieee80211_hw *hw,
-			    struct ieee80211_low_level_stats *stats)
-{
-	printk(KERN_INFO "%s called\n", __func__);
-	return 0;
-}
-
-static int wbsoft_get_tx_stats(struct ieee80211_hw *hw,
-			       struct ieee80211_tx_queue_stats *stats)
-{
-	printk(KERN_INFO "%s called\n", __func__);
-	return 0;
-}
-
-static void wbsoft_configure_filter(struct ieee80211_hw *dev,
-				     unsigned int changed_flags,
-				     unsigned int *total_flags,
-				     int mc_count, struct dev_mc_list *mclist)
-{
-	unsigned int bit_nr, new_flags;
-	u32 mc_filter[2];
-	int i;
-
-	new_flags = 0;
-
-	if (*total_flags & FIF_PROMISC_IN_BSS) {
-		new_flags |= FIF_PROMISC_IN_BSS;
-		mc_filter[1] = mc_filter[0] = ~0;
-	} else if ((*total_flags & FIF_ALLMULTI) || (mc_count > 32)) {
-		new_flags |= FIF_ALLMULTI;
-		mc_filter[1] = mc_filter[0] = ~0;
-	} else {
-		mc_filter[1] = mc_filter[0] = 0;
-		for (i = 0; i < mc_count; i++) {
-			if (!mclist)
-				break;
-			printk("Should call ether_crc here\n");
-			//bit_nr = ether_crc(ETH_ALEN, mclist->dmi_addr) >> 26;
-			bit_nr = 0;
-
-			bit_nr &= 0x3F;
-			mc_filter[bit_nr >> 5] |= 1 << (bit_nr & 31);
-			mclist = mclist->next;
-		}
-	}
-
-	dev->flags &= ~IEEE80211_HW_RX_INCLUDES_FCS;
-
-	*total_flags = new_flags;
-}
-
-static int wbsoft_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
-{
-	char *buffer = kmalloc(skb->len, GFP_ATOMIC);
-	printk("Sending frame %d bytes\n", skb->len);
-	memcpy(buffer, skb->data, skb->len);
-	if (1 == MLMESendFrame(my_adapter, buffer, skb->len, FRAME_TYPE_802_11_MANAGEMENT))
-		printk("frame sent ok (%d bytes)?\n", skb->len);
-	return NETDEV_TX_OK;
-}
-
-
-static int wbsoft_start(struct ieee80211_hw *dev)
-{
-	wbsoft_enabled = 1;
-	printk("wbsoft_start called\n");
-	return 0;
-}
-
-static int wbsoft_config(struct ieee80211_hw *dev, struct ieee80211_conf *conf)
-{
-	ChanInfo ch;
-	printk("wbsoft_config called\n");
-
-	ch.band = 1;
-	ch.ChanNo = 1;	/* Should use channel_num, or something, as that is already pre-translated */
-
-
-	hal_set_current_channel(&my_adapter->sHwData, ch);
-	hal_set_beacon_period(&my_adapter->sHwData, conf->beacon_int);
-//	hal_set_cap_info(&my_adapter->sHwData, ?? );
-// hal_set_ssid(phw_data_t pHwData,  u8 * pssid,  u8 ssid_len); ??
-	hal_set_accept_broadcast(&my_adapter->sHwData, 1);
-	hal_set_accept_promiscuous(&my_adapter->sHwData,  1);
-	hal_set_accept_multicast(&my_adapter->sHwData,  1);
-	hal_set_accept_beacon(&my_adapter->sHwData,  1);
-	hal_set_radio_mode(&my_adapter->sHwData,  0);
-	//hal_set_antenna_number(  phw_data_t pHwData, u8 number )
-	//hal_set_rf_power(phw_data_t pHwData, u8 PowerIndex)
-
-
-//	hal_start_bss(&my_adapter->sHwData, WLAN_BSSTYPE_INFRASTRUCTURE);	??
-
-//void hal_set_rates(phw_data_t pHwData, u8 * pbss_rates,
-//		   u8 length, unsigned char basic_rate_set)
-
-	return 0;
-}
-
-static int wbsoft_config_interface(struct ieee80211_hw *dev,
-				    struct ieee80211_vif *vif,
-				    struct ieee80211_if_conf *conf)
-{
-	printk("wbsoft_config_interface called\n");
-	return 0;
-}
-
-static u64 wbsoft_get_tsf(struct ieee80211_hw *dev)
-{
-	printk("wbsoft_get_tsf called\n");
-	return 0;
-}
-
-static const struct ieee80211_ops wbsoft_ops = {
-	.tx			= wbsoft_tx,
-	.start			= wbsoft_start,		/* Start can be pretty much empty as we do WbWLanInitialize() during probe? */
-	.stop			= wbsoft_stop,
-	.add_interface		= wbsoft_add_interface,
-	.remove_interface	= wbsoft_remove_interface,
-	.config			= wbsoft_config,
-	.config_interface	= wbsoft_config_interface,
-	.configure_filter	= wbsoft_configure_filter,
-	.get_stats		= wbsoft_get_stats,
-	.get_tx_stats		= wbsoft_get_tx_stats,
-	.get_tsf		= wbsoft_get_tsf,
-// conf_tx: hal_set_cwmin()/hal_set_cwmax;
-};
-
-struct wbsoft_priv {
-};
-
-
-// Usb kernel subsystem will call this function when a new device is plugged into.
-int wb35_probe(struct usb_interface *intf, const struct usb_device_id *id_table)
-{
-	PADAPTER	Adapter;
-	PWBLINUX	pWbLinux;
-	PWBUSB		pWbUsb;
-        struct usb_host_interface *interface;
-	struct usb_endpoint_descriptor *endpoint;
-	int	ret = -1;
-	u32	ltmp;
-	struct usb_device *udev = interface_to_usbdev(intf);
-
-	usb_get_dev(udev);
-
-	printk("[w35und]wb35_probe ->\n");
-
-	// 20060630.2 Check the device if it already be opened
-	ret = usb_control_msg(udev, usb_rcvctrlpipe( udev, 0 ),
-			      0x01, USB_TYPE_VENDOR|USB_RECIP_DEVICE|USB_DIR_IN,
-			      0x0, 0x400, &ltmp, 4, HZ*100 );
-	if (ret < 0)
-		goto error;
-
-	ltmp = cpu_to_le32(ltmp);
-	if (ltmp)  // Is already initialized?
-		goto error;
-
-	Adapter = kzalloc(sizeof(ADAPTER), GFP_KERNEL);
-
-	my_adapter = Adapter;
-	pWbLinux = &Adapter->WbLinux;
-	pWbUsb = &Adapter->sHwData.WbUsb;
-	pWbUsb->udev = udev;
-
-        interface = intf->cur_altsetting;
-        endpoint = &interface->endpoint[0].desc;
-
-	if (endpoint[2].wMaxPacketSize == 512) {
-		printk("[w35und] Working on USB 2.0\n");
-		pWbUsb->IsUsb20 = 1;
-	}
-
-	if (!WbWLanInitialize(Adapter)) {
-		printk("[w35und]WbWLanInitialize fail\n");
-		goto error;
-	}
-
-	{
-		struct wbsoft_priv *priv;
-		struct ieee80211_hw *dev;
-		static struct ieee80211_supported_band band;
-		int res;
-
-		dev = ieee80211_alloc_hw(sizeof(*priv), &wbsoft_ops);
-
-		if (!dev) {
-			printk("w35und: ieee80211 alloc failed\n" );
-			BUG();
-		}
-
-		my_dev = dev;
-
-		SET_IEEE80211_DEV(dev, &udev->dev);
-		{
-			phw_data_t pHwData = &Adapter->sHwData;
-			unsigned char		dev_addr[MAX_ADDR_LEN];
-			hal_get_permanent_address(pHwData, dev_addr);
-			SET_IEEE80211_PERM_ADDR(dev, dev_addr);
-		}
-
-
-		dev->extra_tx_headroom = 12;	/* FIXME */
-		dev->flags = 0;
-
-		dev->channel_change_time = 1000;
-//		dev->max_rssi = 100;
-
-		dev->queues = 1;
-
-		band.channels = wbsoft_channels;
-		band.n_channels = ARRAY_SIZE(wbsoft_channels);
-		band.bitrates = wbsoft_rates;
-		band.n_bitrates = ARRAY_SIZE(wbsoft_rates);
-
-		dev->wiphy->bands[IEEE80211_BAND_2GHZ] = &band;
-#if 0
-		wbsoft_modes[0].num_channels = 1;
-		wbsoft_modes[0].channels = wbsoft_channels;
-		wbsoft_modes[0].mode = MODE_IEEE80211B;
-		wbsoft_modes[0].num_rates = ARRAY_SIZE(wbsoft_rates);
-		wbsoft_modes[0].rates = wbsoft_rates;
-
-		res = ieee80211_register_hwmode(dev, &wbsoft_modes[0]);
-		BUG_ON(res);
-#endif
-
-		res = ieee80211_register_hw(dev);
-		BUG_ON(res);
-	}
-
-	usb_set_intfdata( intf, Adapter );
-
-	printk("[w35und] _probe OK\n");
-	return 0;
-error:
-	return -ENOMEM;
-}
-
-void packet_came(char *pRxBufferAddress, int PacketSize)
-{
-	struct sk_buff *skb;
-	struct ieee80211_rx_status rx_status = {0};
-
-	if (!wbsoft_enabled)
-		return;
-
-	skb = dev_alloc_skb(PacketSize);
-	if (!skb) {
-		printk("Not enough memory for packet, FIXME\n");
-		return;
-	}
-
-	memcpy(skb_put(skb, PacketSize),
-	       pRxBufferAddress,
-	       PacketSize);
-
-/*
-	rx_status.rate = 10;
-	rx_status.channel = 1;
-	rx_status.freq = 12345;
-	rx_status.phymode = MODE_IEEE80211B;
-*/
-
-	ieee80211_rx_irqsafe(my_dev, skb, &rx_status);
-}
-
-unsigned char
-WbUsb_initial(phw_data_t pHwData)
-{
-	return 1;
-}
-
-
-void
-WbUsb_destroy(phw_data_t pHwData)
-{
-}
-
-int wb35_open(struct net_device *netdev)
-{
-	/* netdev_priv() or netdev->ml_priv should reference to the address of
-	 * private data(PADAPTER). It depends on whether private data memory is
-	 * allocated when alloc_netdev().
-	 */
-	PADAPTER Adapter = (PADAPTER)netdev_priv(netdev);
-	phw_data_t pHwData = &Adapter->sHwData;
-
-        netif_start_queue(netdev);
-
-	//TODO : put here temporarily
-	hal_set_accept_broadcast(pHwData, 1); // open accept broadcast
-
-	return 0;
-}
-
-int wb35_close(struct net_device *netdev)
-{
-	netif_stop_queue(netdev);
-	return 0;
-}
-
-void wb35_disconnect(struct usb_interface *intf)
-{
-	PWBLINUX pWbLinux;
-	PADAPTER Adapter = usb_get_intfdata(intf);
-	usb_set_intfdata(intf, NULL);
-
-        pWbLinux = &Adapter->WbLinux;
-
-	// Card remove
-	WbWlanHalt(Adapter);
-
-}
-
-static struct usb_driver wb35_driver = {
-	.name		= "w35und",
-	.id_table	= wb35_table,
-	.probe		= wb35_probe,
-	.disconnect	= wb35_disconnect,
-};
-
-static int __init wb35_init(void)
-{
-	return usb_register(&wb35_driver);
-}
-
-static void __exit wb35_exit(void)
-{
-	usb_deregister(&wb35_driver);
-}
-
-module_init(wb35_init);
-module_exit(wb35_exit);
diff --git a/drivers/staging/winbond/linux/wbusb_f.h b/drivers/staging/winbond/linux/wbusb_f.h
deleted file mode 100644
index cae29e107e11..000000000000
--- a/drivers/staging/winbond/linux/wbusb_f.h
+++ /dev/null
@@ -1,34 +0,0 @@
-//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Copyright (c) 1996-2004 Winbond Electronic Corporation
-//
-//  Module Name:
-//    wbusb_f.h
-//
-//  Abstract:
-//    Linux driver.
-//
-//  Author:
-//
-//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
-int wb35_open(struct net_device *netdev);
-int wb35_close(struct net_device *netdev);
-unsigned char WbUsb_initial(phw_data_t pHwData);
-void WbUsb_destroy(phw_data_t pHwData);
-unsigned char WbWLanInitialize(PADAPTER Adapter);
-#define	WbUsb_Stop( _A )
-
-int wb35_probe(struct usb_interface *intf,const struct usb_device_id *id_table);
-void wb35_disconnect(struct usb_interface *intf);
-
-
-#define wb_usb_submit_urb(_A) usb_submit_urb(_A, GFP_ATOMIC)
-#define wb_usb_alloc_urb(_A) usb_alloc_urb(_A, GFP_ATOMIC)
-
-#define WbUsb_CheckForHang( _P )
-#define WbUsb_DetectStart( _P, _I )
-
-
-
-
-
diff --git a/drivers/staging/winbond/localpara.h b/drivers/staging/winbond/localpara.h
index 268cf916ab48..607bb0526cf8 100644
--- a/drivers/staging/winbond/localpara.h
+++ b/drivers/staging/winbond/localpara.h
@@ -1,6 +1,12 @@
+#ifndef __WINBOND_LOCALPARA_H
+#define __WINBOND_LOCALPARA_H
+
 //=============================================================
 // LocalPara.h -
 //=============================================================
+
+#include "mac_structures.h"
+
 //Define the local ability
 
 #define LOCAL_DEFAULT_BEACON_PERIOD			100		//ms
@@ -25,7 +31,7 @@
 #define LOCAL_UNKNOWN_5_CHANNEL_NUM			34	//not include 165
 
 
-#define psLOCAL			(&(Adapter->sLocalPara))
+#define psLOCAL			(&(adapter->sLocalPara))
 
 #define MODE_802_11_BG			0
 #define MODE_802_11_A			1
@@ -143,7 +149,6 @@ typedef struct LOCAL_PARA
 
     //// power-save variables
     u8  		  	iPowerSaveMode;     // 0 indicates it is on, 1 indicates it is off
-	u8			ShutDowned;
 	u8			ATIMmode;
 	u8			ExcludeUnencrypted;
 
@@ -272,4 +277,4 @@ typedef struct LOCAL_PARA
 
 } WB_LOCALDESCRIPT, *PWB_LOCALDESCRIPT;
 
-
+#endif
diff --git a/drivers/staging/winbond/mac_structures.h b/drivers/staging/winbond/mac_structures.h
index 031d2cb6cd63..0d1619601c0b 100644
--- a/drivers/staging/winbond/mac_structures.h
+++ b/drivers/staging/winbond/mac_structures.h
@@ -21,6 +21,7 @@
 #ifndef _MAC_Structures_H_
 #define _MAC_Structures_H_
 
+#include <linux/skbuff.h>
 
 //=========================================================
 // Some miscellaneous definitions
@@ -115,10 +116,6 @@
 #define WLAN_MAX_PAIRWISE_CIPHER_SUITE_COUNT    ((u16) 6)
 #define WLAN_MAX_AUTH_KEY_MGT_SUITE_LIST_COUNT  ((u16) 2)
 
-#ifdef WB_LINUX
-#define UNALIGNED
-#endif
-
 //========================================================
 typedef enum enum_PowerManagementMode
 {
@@ -464,7 +461,7 @@ struct	RSN_Information_Element
 {
 	u8					Element_ID;
 	u8					Length;
-	UNALIGNED SUITE_SELECTOR	OuiWPAAdditional;//WPA version 2.0 additional field, and should be 00:50:F2:01
+	SUITE_SELECTOR	OuiWPAAdditional;//WPA version 2.0 additional field, and should be 00:50:F2:01
 	u16					Version;
 	SUITE_SELECTOR		GroupKeySuite;
 	u16					PairwiseKeySuiteCount;
diff --git a/drivers/staging/winbond/mds.c b/drivers/staging/winbond/mds.c
index f1de813f9c76..e431406e25a6 100644
--- a/drivers/staging/winbond/mds.c
+++ b/drivers/staging/winbond/mds.c
@@ -1,445 +1,39 @@
+#include "ds_tkip.h"
+#include "gl_80211.h"
+#include "mds_f.h"
+#include "mlmetxrx_f.h"
+#include "mto_f.h"
 #include "os_common.h"
-
-void
-Mds_reset_descriptor(PADAPTER Adapter)
-{
-	PMDS pMds = &Adapter->Mds;
-
-	pMds->TxPause = 0;
-	pMds->TxThreadCount = 0;
-	pMds->TxFillIndex = 0;
-	pMds->TxDesIndex = 0;
-	pMds->ScanTxPause = 0;
-	memset(pMds->TxOwner, 0, ((MAX_USB_TX_BUFFER_NUMBER + 3) & ~0x03));
-}
+#include "wbhal_f.h"
+#include "wblinux_f.h"
 
 unsigned char
-Mds_initial(PADAPTER Adapter)
+Mds_initial(struct wbsoft_priv * adapter)
 {
-	PMDS pMds = &Adapter->Mds;
+	PMDS pMds = &adapter->Mds;
 
-	pMds->TxPause = FALSE;
+	pMds->TxPause = false;
 	pMds->TxRTSThreshold = DEFAULT_RTSThreshold;
 	pMds->TxFragmentThreshold = DEFAULT_FRAGMENT_THRESHOLD;
 
-	vRxTimerInit(Adapter);//for WPA countermeasure
-
-	return hal_get_tx_buffer( &Adapter->sHwData, &pMds->pTxBuffer );
-}
-
-void
-Mds_Destroy(PADAPTER Adapter)
-{
-	vRxTimerStop(Adapter);
-}
-
-void
-Mds_Tx(PADAPTER Adapter)
-{
-	phw_data_t	pHwData = &Adapter->sHwData;
-	PMDS		pMds = &Adapter->Mds;
-	DESCRIPTOR	TxDes;
-	PDESCRIPTOR	pTxDes = &TxDes;
-	u8		*XmitBufAddress;
-	u16		XmitBufSize, PacketSize, stmp, CurrentSize, FragmentThreshold;
-	u8		FillIndex, TxDesIndex, FragmentCount, FillCount;
-	unsigned char	BufferFilled = FALSE, MICAdd = 0;
-
-
-	if (pMds->TxPause)
-		return;
-	if (!hal_driver_init_OK(pHwData))
-		return;
-
-	//Only one thread can be run here
-	if (!OS_ATOMIC_INC( Adapter, &pMds->TxThreadCount) == 1)
-		goto cleanup;
-
-	// Start to fill the data
-	do {
-		FillIndex = pMds->TxFillIndex;
-		if (pMds->TxOwner[FillIndex]) { // Is owned by software 0:Yes 1:No
-#ifdef _PE_TX_DUMP_
-			WBDEBUG(("[Mds_Tx] Tx Owner is H/W.\n"));
-#endif
-			break;
-		}
-
-		XmitBufAddress = pMds->pTxBuffer + (MAX_USB_TX_BUFFER * FillIndex); //Get buffer
-		XmitBufSize = 0;
-		FillCount = 0;
-		do {
-			PacketSize = Adapter->sMlmeFrame.len;
-			if (!PacketSize)
-				break;
-
-			//For Check the buffer resource
-			FragmentThreshold = CURRENT_FRAGMENT_THRESHOLD;
-			//931130.5.b
-			FragmentCount = PacketSize/FragmentThreshold + 1;
-			stmp = PacketSize + FragmentCount*32 + 8;//931130.5.c 8:MIC
-			if ((XmitBufSize + stmp) >= MAX_USB_TX_BUFFER) {
-				printk("[Mds_Tx] Excess max tx buffer.\n");
-				break; // buffer is not enough
-			}
-
-
-			//
-			// Start transmitting
-			//
-			BufferFilled = TRUE;
-
-			/* Leaves first u8 intact */
-			memset((u8 *)pTxDes + 1, 0, sizeof(DESCRIPTOR) - 1);
-
-			TxDesIndex = pMds->TxDesIndex;//Get the current ID
-			pTxDes->Descriptor_ID = TxDesIndex;
-			pMds->TxDesFrom[ TxDesIndex ] = 2;//Storing the information of source comming from
-			pMds->TxDesIndex++;
-			pMds->TxDesIndex %= MAX_USB_TX_DESCRIPTOR;
-
-			MLME_GetNextPacket( Adapter, pTxDes );
-
-			// Copy header. 8byte USB + 24byte 802.11Hdr. Set TxRate, Preamble type
-			Mds_HeaderCopy( Adapter, pTxDes, XmitBufAddress );
-
-			// For speed up Key setting
-			if (pTxDes->EapFix) {
-#ifdef _PE_TX_DUMP_
-				WBDEBUG(("35: EPA 4th frame detected. Size = %d\n", PacketSize));
-#endif
-				pHwData->IsKeyPreSet = 1;
-			}
-
-			// Copy (fragment) frame body, and set USB, 802.11 hdr flag
-			CurrentSize = Mds_BodyCopy(Adapter, pTxDes, XmitBufAddress);
-
-			// Set RTS/CTS and Normal duration field into buffer
-			Mds_DurationSet(Adapter, pTxDes, XmitBufAddress);
-
-			//
-			// Calculation MIC from buffer which maybe fragment, then fill into temporary address 8 byte
-			// 931130.5.e
-			if (MICAdd)
-				Mds_MicFill( Adapter, pTxDes, XmitBufAddress );
-
-			//Shift to the next address
-			XmitBufSize += CurrentSize;
-			XmitBufAddress += CurrentSize;
-
-#ifdef _IBSS_BEACON_SEQ_STICK_
-			if ((XmitBufAddress[ DOT_11_DA_OFFSET+8 ] & 0xfc) != MAC_SUBTYPE_MNGMNT_PROBE_REQUEST) // +8 for USB hdr
-#endif
-				pMds->TxToggle = TRUE;
-
-			// Get packet to transmit completed, 1:TESTSTA 2:MLME 3: Ndis data
-			MLME_SendComplete(Adapter, 0, TRUE);
-
-			// Software TSC count 20060214
-			pMds->TxTsc++;
-			if (pMds->TxTsc == 0)
-				pMds->TxTsc_2++;
-
-			FillCount++; // 20060928
-		} while (HAL_USB_MODE_BURST(pHwData)); // End of multiple MSDU copy loop. FALSE = single TRUE = multiple sending
-
-		// Move to the next one, if necessary
-		if (BufferFilled) {
-			// size setting
-			pMds->TxBufferSize[ FillIndex ] = XmitBufSize;
-
-			// 20060928 set Tx count
-			pMds->TxCountInBuffer[FillIndex] = FillCount;
-
-			// Set owner flag
-			pMds->TxOwner[FillIndex] = 1;
-
-			pMds->TxFillIndex++;
-			pMds->TxFillIndex %= MAX_USB_TX_BUFFER_NUMBER;
-			BufferFilled = FALSE;
-		} else
-			break;
-
-		if (!PacketSize) // No more pk for transmitting
-			break;
-
-	} while(TRUE);
-
-	//
-	// Start to send by lower module
-	//
-	if (!pHwData->IsKeyPreSet)
-		Wb35Tx_start(pHwData);
-
- cleanup:
-	OS_ATOMIC_DEC( Adapter, &pMds->TxThreadCount );
+	return hal_get_tx_buffer( &adapter->sHwData, &pMds->pTxBuffer );
 }
 
 void
-Mds_SendComplete(PADAPTER Adapter, PT02_DESCRIPTOR pT02)
-{
-	PMDS	pMds = &Adapter->Mds;
-	phw_data_t	pHwData = &Adapter->sHwData;
-	u8	PacketId = (u8)pT02->T02_Tx_PktID;
-	unsigned char	SendOK = TRUE;
-	u8	RetryCount, TxRate;
-
-	if (pT02->T02_IgnoreResult) // Don't care the result
-		return;
-	if (pT02->T02_IsLastMpdu) {
-		//TODO: DTO -- get the retry count and fragment count
-		// Tx rate
-		TxRate = pMds->TxRate[ PacketId ][ 0 ];
-		RetryCount = (u8)pT02->T02_MPDU_Cnt;
-		if (pT02->value & FLAG_ERROR_TX_MASK) {
-			SendOK = FALSE;
-
-			if (pT02->T02_transmit_abort || pT02->T02_out_of_MaxTxMSDULiftTime) {
-				//retry error
-				pHwData->dto_tx_retry_count += (RetryCount+1);
-				//[for tx debug]
-				if (RetryCount<7)
-					pHwData->tx_retry_count[RetryCount] += RetryCount;
-				else
-					pHwData->tx_retry_count[7] += RetryCount;
-				#ifdef _PE_STATE_DUMP_
-				WBDEBUG(("dto_tx_retry_count =%d\n", pHwData->dto_tx_retry_count));
-				#endif
-				MTO_SetTxCount(Adapter, TxRate, RetryCount);
-			}
-			pHwData->dto_tx_frag_count += (RetryCount+1);
-
-			//[for tx debug]
-			if (pT02->T02_transmit_abort_due_to_TBTT)
-				pHwData->tx_TBTT_start_count++;
-			if (pT02->T02_transmit_without_encryption_due_to_wep_on_false)
-				pHwData->tx_WepOn_false_count++;
-			if (pT02->T02_discard_due_to_null_wep_key)
-				pHwData->tx_Null_key_count++;
-		} else {
-			if (pT02->T02_effective_transmission_rate)
-				pHwData->tx_ETR_count++;
-			MTO_SetTxCount(Adapter, TxRate, RetryCount);
-		}
-
-		// Clear send result buffer
-		pMds->TxResult[ PacketId ] = 0;
-	} else
-		pMds->TxResult[ PacketId ] |= ((u16)(pT02->value & 0x0ffff));
-}
-
-void
-Mds_HeaderCopy(PADAPTER Adapter, PDESCRIPTOR pDes, u8 *TargetBuffer)
-{
-	PMDS	pMds = &Adapter->Mds;
-	u8	*src_buffer = pDes->buffer_address[0];//931130.5.g
-	PT00_DESCRIPTOR	pT00;
-	PT01_DESCRIPTOR	pT01;
-	u16	stmp;
-	u8	i, ctmp1, ctmp2, ctmpf;
-	u16	FragmentThreshold = CURRENT_FRAGMENT_THRESHOLD;
-
-
-	stmp = pDes->buffer_total_size;
-	//
-	// Set USB header 8 byte
-	//
-	pT00 = (PT00_DESCRIPTOR)TargetBuffer;
-	TargetBuffer += 4;
-	pT01 = (PT01_DESCRIPTOR)TargetBuffer;
-	TargetBuffer += 4;
-
-	pT00->value = 0;// Clear
-	pT01->value = 0;// Clear
-
-	pT00->T00_tx_packet_id = pDes->Descriptor_ID;// Set packet ID
-	pT00->T00_header_length = 24;// Set header length
-	pT01->T01_retry_abort_ebable = 1;//921013 931130.5.h
-
-	// Key ID setup
-	pT01->T01_wep_id = 0;
-
-	FragmentThreshold = DEFAULT_FRAGMENT_THRESHOLD;	//Do not fragment
-	// Copy full data, the 1'st buffer contain all the data 931130.5.j
-	memcpy( TargetBuffer, src_buffer, DOT_11_MAC_HEADER_SIZE );// Copy header
-	pDes->buffer_address[0] = src_buffer + DOT_11_MAC_HEADER_SIZE;
-	pDes->buffer_total_size -= DOT_11_MAC_HEADER_SIZE;
-	pDes->buffer_size[0] = pDes->buffer_total_size;
-
-	// Set fragment threshold
-	FragmentThreshold -= (DOT_11_MAC_HEADER_SIZE + 4);
-	pDes->FragmentThreshold = FragmentThreshold;
-
-	// Set more frag bit
-	TargetBuffer[1] |= 0x04;// Set more frag bit
-
-	//
-	// Set tx rate
-	//
-	stmp = *(u16 *)(TargetBuffer+30); // 2n alignment address
-
-	//Use basic rate
-	ctmp1 = ctmpf = CURRENT_TX_RATE_FOR_MNG;
-
-	pDes->TxRate = ctmp1;
-	#ifdef _PE_TX_DUMP_
-	WBDEBUG(("Tx rate =%x\n", ctmp1));
-	#endif
-
-	pT01->T01_modulation_type = (ctmp1%3) ? 0 : 1;
-
-	for( i=0; i<2; i++ ) {
-		if( i == 1 )
-			ctmp1 = ctmpf;
-
-		pMds->TxRate[pDes->Descriptor_ID][i] = ctmp1; // backup the ta rate and fall back rate
-
-		if( ctmp1 == 108) ctmp2 = 7;
-		else if( ctmp1 == 96 ) ctmp2 = 6; // Rate convert for USB
-		else if( ctmp1 == 72 ) ctmp2 = 5;
-		else if( ctmp1 == 48 ) ctmp2 = 4;
-		else if( ctmp1 == 36 ) ctmp2 = 3;
-		else if( ctmp1 == 24 ) ctmp2 = 2;
-		else if( ctmp1 == 18 ) ctmp2 = 1;
-		else if( ctmp1 == 12 ) ctmp2 = 0;
-		else if( ctmp1 == 22 ) ctmp2 = 3;
-		else if( ctmp1 == 11 ) ctmp2 = 2;
-		else if( ctmp1 == 4  ) ctmp2 = 1;
-		else ctmp2 = 0; // if( ctmp1 == 2  ) or default
-
-		if( i == 0 )
-			pT01->T01_transmit_rate = ctmp2;
-		else
-			pT01->T01_fall_back_rate = ctmp2;
-	}
-
-	//
-	// Set preamble type
-	//
-	if ((pT01->T01_modulation_type == 0) && (pT01->T01_transmit_rate == 0))	// RATE_1M
-		pDes->PreambleMode =  WLAN_PREAMBLE_TYPE_LONG;
-	else
-		pDes->PreambleMode =  CURRENT_PREAMBLE_MODE;
-	pT01->T01_plcp_header_length = pDes->PreambleMode;	// Set preamble
-
-}
-
-// The function return the 4n size of usb pk
-u16
-Mds_BodyCopy(PADAPTER Adapter, PDESCRIPTOR pDes, u8 *TargetBuffer)
+Mds_Destroy(struct wbsoft_priv * adapter)
 {
-	PT00_DESCRIPTOR	pT00;
-	PMDS	pMds = &Adapter->Mds;
-	u8	*buffer;
-	u8	*src_buffer;
-	u8	*pctmp;
-	u16	Size = 0;
-	u16	SizeLeft, CopySize, CopyLeft, stmp;
-	u8	buf_index, FragmentCount = 0;
-
-
-	// Copy fragment body
-	buffer = TargetBuffer; // shift 8B usb + 24B 802.11
-	SizeLeft = pDes->buffer_total_size;
-	buf_index = pDes->buffer_start_index;
-
-	pT00 = (PT00_DESCRIPTOR)buffer;
-	while (SizeLeft) {
-		pT00 = (PT00_DESCRIPTOR)buffer;
-		CopySize = SizeLeft;
-		if (SizeLeft > pDes->FragmentThreshold) {
-			CopySize = pDes->FragmentThreshold;
-			pT00->T00_frame_length = 24 + CopySize;//Set USB length
-		} else
-			pT00->T00_frame_length = 24 + SizeLeft;//Set USB length
-
-		SizeLeft -= CopySize;
-
-		// 1 Byte operation
-		pctmp = (u8 *)( buffer + 8 + DOT_11_SEQUENCE_OFFSET );
-		*pctmp &= 0xf0;
-		*pctmp |= FragmentCount;//931130.5.m
-		if( !FragmentCount )
-			pT00->T00_first_mpdu = 1;
-
-		buffer += 32; // 8B usb + 24B 802.11 header
-		Size += 32;
-
-		// Copy into buffer
-		stmp = CopySize + 3;
-		stmp &= ~0x03;//4n Alignment
-		Size += stmp;// Current 4n offset of mpdu
-
-		while (CopySize) {
-			// Copy body
-			src_buffer = pDes->buffer_address[buf_index];
-			CopyLeft = CopySize;
-			if (CopySize >= pDes->buffer_size[buf_index]) {
-				CopyLeft = pDes->buffer_size[buf_index];
-
-				// Get the next buffer of descriptor
-				buf_index++;
-				buf_index %= MAX_DESCRIPTOR_BUFFER_INDEX;
-			} else {
-				u8	*pctmp = pDes->buffer_address[buf_index];
-				pctmp += CopySize;
-				pDes->buffer_address[buf_index] = pctmp;
-				pDes->buffer_size[buf_index] -= CopySize;
-			}
-
-			memcpy(buffer, src_buffer, CopyLeft);
-			buffer += CopyLeft;
-			CopySize -= CopyLeft;
-		}
-
-		// 931130.5.n
-		if (pMds->MicAdd) {
-			if (!SizeLeft) {
-				pMds->MicWriteAddress[ pMds->MicWriteIndex ] = buffer - pMds->MicAdd;
-				pMds->MicWriteSize[ pMds->MicWriteIndex ] = pMds->MicAdd;
-				pMds->MicAdd = 0;
-			}
-			else if( SizeLeft < 8 ) //931130.5.p
-			{
-				pMds->MicAdd = SizeLeft;
-				pMds->MicWriteAddress[ pMds->MicWriteIndex ] = buffer - ( 8 - SizeLeft );
-				pMds->MicWriteSize[ pMds->MicWriteIndex ] = 8 - SizeLeft;
-				pMds->MicWriteIndex++;
-			}
-		}
-
-		// Does it need to generate the new header for next mpdu?
-		if (SizeLeft) {
-			buffer = TargetBuffer + Size; // Get the next 4n start address
-			memcpy( buffer, TargetBuffer, 32 );//Copy 8B USB +24B 802.11
-			pT00 = (PT00_DESCRIPTOR)buffer;
-			pT00->T00_first_mpdu = 0;
-		}
-
-		FragmentCount++;
-	}
-
-	pT00->T00_last_mpdu = 1;
-	pT00->T00_IsLastMpdu = 1;
-	buffer = (u8 *)pT00 + 8; // +8 for USB hdr
-	buffer[1] &= ~0x04; // Clear more frag bit of 802.11 frame control
-	pDes->FragmentCount = FragmentCount; // Update the correct fragment number
-	return Size;
 }
 
-
-void
-Mds_DurationSet(  PADAPTER Adapter,  PDESCRIPTOR pDes,  u8 *buffer )
+static void Mds_DurationSet(struct wbsoft_priv *adapter,  PDESCRIPTOR pDes,  u8 *buffer)
 {
 	PT00_DESCRIPTOR	pT00;
 	PT01_DESCRIPTOR	pT01;
 	u16	Duration, NextBodyLen, OffsetSize;
 	u8	Rate, i;
-	unsigned char	CTS_on = FALSE, RTS_on = FALSE;
+	unsigned char	CTS_on = false, RTS_on = false;
 	PT00_DESCRIPTOR pNextT00;
 	u16 BodyLen = 0;
-	unsigned char boGroupAddr = FALSE;
-
+	unsigned char boGroupAddr = false;
 
 	OffsetSize = pDes->FragmentThreshold + 32 + 3;
 	OffsetSize &= ~0x03;
@@ -452,7 +46,7 @@ Mds_DurationSet(  PADAPTER Adapter,  PDESCRIPTOR pDes,  u8 *buffer )
 	pNextT00 = (PT00_DESCRIPTOR)(buffer+OffsetSize);
 
 	if( buffer[ DOT_11_DA_OFFSET+8 ] & 0x1 ) // +8 for USB hdr
-		boGroupAddr = TRUE;
+		boGroupAddr = true;
 
 	//========================================
 	// Set RTS/CTS mechanism
@@ -467,13 +61,13 @@ Mds_DurationSet(  PADAPTER Adapter,  PDESCRIPTOR pDes,  u8 *buffer )
 		BodyLen += 4;	//CRC
 
 		if( BodyLen >= CURRENT_RTS_THRESHOLD )
-			RTS_on = TRUE; // Using RTS
+			RTS_on = true; // Using RTS
 		else
 		{
 			if( pT01->T01_modulation_type ) // Is using OFDM
 			{
 				if( CURRENT_PROTECT_MECHANISM ) // Is using protect
-					CTS_on = TRUE; // Using CTS
+					CTS_on = true; // Using CTS
 			}
 		}
 	}
@@ -624,9 +218,394 @@ Mds_DurationSet(  PADAPTER Adapter,  PDESCRIPTOR pDes,  u8 *buffer )
 
 }
 
-void MDS_EthernetPacketReceive(  PADAPTER Adapter,  PRXLAYER1 pRxLayer1 )
+// The function return the 4n size of usb pk
+static u16 Mds_BodyCopy(struct wbsoft_priv *adapter, PDESCRIPTOR pDes, u8 *TargetBuffer)
+{
+	PT00_DESCRIPTOR	pT00;
+	PMDS	pMds = &adapter->Mds;
+	u8	*buffer;
+	u8	*src_buffer;
+	u8	*pctmp;
+	u16	Size = 0;
+	u16	SizeLeft, CopySize, CopyLeft, stmp;
+	u8	buf_index, FragmentCount = 0;
+
+
+	// Copy fragment body
+	buffer = TargetBuffer; // shift 8B usb + 24B 802.11
+	SizeLeft = pDes->buffer_total_size;
+	buf_index = pDes->buffer_start_index;
+
+	pT00 = (PT00_DESCRIPTOR)buffer;
+	while (SizeLeft) {
+		pT00 = (PT00_DESCRIPTOR)buffer;
+		CopySize = SizeLeft;
+		if (SizeLeft > pDes->FragmentThreshold) {
+			CopySize = pDes->FragmentThreshold;
+			pT00->T00_frame_length = 24 + CopySize;//Set USB length
+		} else
+			pT00->T00_frame_length = 24 + SizeLeft;//Set USB length
+
+		SizeLeft -= CopySize;
+
+		// 1 Byte operation
+		pctmp = (u8 *)( buffer + 8 + DOT_11_SEQUENCE_OFFSET );
+		*pctmp &= 0xf0;
+		*pctmp |= FragmentCount;//931130.5.m
+		if( !FragmentCount )
+			pT00->T00_first_mpdu = 1;
+
+		buffer += 32; // 8B usb + 24B 802.11 header
+		Size += 32;
+
+		// Copy into buffer
+		stmp = CopySize + 3;
+		stmp &= ~0x03;//4n Alignment
+		Size += stmp;// Current 4n offset of mpdu
+
+		while (CopySize) {
+			// Copy body
+			src_buffer = pDes->buffer_address[buf_index];
+			CopyLeft = CopySize;
+			if (CopySize >= pDes->buffer_size[buf_index]) {
+				CopyLeft = pDes->buffer_size[buf_index];
+
+				// Get the next buffer of descriptor
+				buf_index++;
+				buf_index %= MAX_DESCRIPTOR_BUFFER_INDEX;
+			} else {
+				u8	*pctmp = pDes->buffer_address[buf_index];
+				pctmp += CopySize;
+				pDes->buffer_address[buf_index] = pctmp;
+				pDes->buffer_size[buf_index] -= CopySize;
+			}
+
+			memcpy(buffer, src_buffer, CopyLeft);
+			buffer += CopyLeft;
+			CopySize -= CopyLeft;
+		}
+
+		// 931130.5.n
+		if (pMds->MicAdd) {
+			if (!SizeLeft) {
+				pMds->MicWriteAddress[ pMds->MicWriteIndex ] = buffer - pMds->MicAdd;
+				pMds->MicWriteSize[ pMds->MicWriteIndex ] = pMds->MicAdd;
+				pMds->MicAdd = 0;
+			}
+			else if( SizeLeft < 8 ) //931130.5.p
+			{
+				pMds->MicAdd = SizeLeft;
+				pMds->MicWriteAddress[ pMds->MicWriteIndex ] = buffer - ( 8 - SizeLeft );
+				pMds->MicWriteSize[ pMds->MicWriteIndex ] = 8 - SizeLeft;
+				pMds->MicWriteIndex++;
+			}
+		}
+
+		// Does it need to generate the new header for next mpdu?
+		if (SizeLeft) {
+			buffer = TargetBuffer + Size; // Get the next 4n start address
+			memcpy( buffer, TargetBuffer, 32 );//Copy 8B USB +24B 802.11
+			pT00 = (PT00_DESCRIPTOR)buffer;
+			pT00->T00_first_mpdu = 0;
+		}
+
+		FragmentCount++;
+	}
+
+	pT00->T00_last_mpdu = 1;
+	pT00->T00_IsLastMpdu = 1;
+	buffer = (u8 *)pT00 + 8; // +8 for USB hdr
+	buffer[1] &= ~0x04; // Clear more frag bit of 802.11 frame control
+	pDes->FragmentCount = FragmentCount; // Update the correct fragment number
+	return Size;
+}
+
+static void Mds_HeaderCopy(struct wbsoft_priv * adapter, PDESCRIPTOR pDes, u8 *TargetBuffer)
+{
+	PMDS	pMds = &adapter->Mds;
+	u8	*src_buffer = pDes->buffer_address[0];//931130.5.g
+	PT00_DESCRIPTOR	pT00;
+	PT01_DESCRIPTOR	pT01;
+	u16	stmp;
+	u8	i, ctmp1, ctmp2, ctmpf;
+	u16	FragmentThreshold = CURRENT_FRAGMENT_THRESHOLD;
+
+
+	stmp = pDes->buffer_total_size;
+	//
+	// Set USB header 8 byte
+	//
+	pT00 = (PT00_DESCRIPTOR)TargetBuffer;
+	TargetBuffer += 4;
+	pT01 = (PT01_DESCRIPTOR)TargetBuffer;
+	TargetBuffer += 4;
+
+	pT00->value = 0;// Clear
+	pT01->value = 0;// Clear
+
+	pT00->T00_tx_packet_id = pDes->Descriptor_ID;// Set packet ID
+	pT00->T00_header_length = 24;// Set header length
+	pT01->T01_retry_abort_ebable = 1;//921013 931130.5.h
+
+	// Key ID setup
+	pT01->T01_wep_id = 0;
+
+	FragmentThreshold = DEFAULT_FRAGMENT_THRESHOLD;	//Do not fragment
+	// Copy full data, the 1'st buffer contain all the data 931130.5.j
+	memcpy( TargetBuffer, src_buffer, DOT_11_MAC_HEADER_SIZE );// Copy header
+	pDes->buffer_address[0] = src_buffer + DOT_11_MAC_HEADER_SIZE;
+	pDes->buffer_total_size -= DOT_11_MAC_HEADER_SIZE;
+	pDes->buffer_size[0] = pDes->buffer_total_size;
+
+	// Set fragment threshold
+	FragmentThreshold -= (DOT_11_MAC_HEADER_SIZE + 4);
+	pDes->FragmentThreshold = FragmentThreshold;
+
+	// Set more frag bit
+	TargetBuffer[1] |= 0x04;// Set more frag bit
+
+	//
+	// Set tx rate
+	//
+	stmp = *(u16 *)(TargetBuffer+30); // 2n alignment address
+
+	//Use basic rate
+	ctmp1 = ctmpf = CURRENT_TX_RATE_FOR_MNG;
+
+	pDes->TxRate = ctmp1;
+	#ifdef _PE_TX_DUMP_
+	WBDEBUG(("Tx rate =%x\n", ctmp1));
+	#endif
+
+	pT01->T01_modulation_type = (ctmp1%3) ? 0 : 1;
+
+	for( i=0; i<2; i++ ) {
+		if( i == 1 )
+			ctmp1 = ctmpf;
+
+		pMds->TxRate[pDes->Descriptor_ID][i] = ctmp1; // backup the ta rate and fall back rate
+
+		if( ctmp1 == 108) ctmp2 = 7;
+		else if( ctmp1 == 96 ) ctmp2 = 6; // Rate convert for USB
+		else if( ctmp1 == 72 ) ctmp2 = 5;
+		else if( ctmp1 == 48 ) ctmp2 = 4;
+		else if( ctmp1 == 36 ) ctmp2 = 3;
+		else if( ctmp1 == 24 ) ctmp2 = 2;
+		else if( ctmp1 == 18 ) ctmp2 = 1;
+		else if( ctmp1 == 12 ) ctmp2 = 0;
+		else if( ctmp1 == 22 ) ctmp2 = 3;
+		else if( ctmp1 == 11 ) ctmp2 = 2;
+		else if( ctmp1 == 4  ) ctmp2 = 1;
+		else ctmp2 = 0; // if( ctmp1 == 2  ) or default
+
+		if( i == 0 )
+			pT01->T01_transmit_rate = ctmp2;
+		else
+			pT01->T01_fall_back_rate = ctmp2;
+	}
+
+	//
+	// Set preamble type
+	//
+	if ((pT01->T01_modulation_type == 0) && (pT01->T01_transmit_rate == 0))	// RATE_1M
+		pDes->PreambleMode =  WLAN_PREAMBLE_TYPE_LONG;
+	else
+		pDes->PreambleMode =  CURRENT_PREAMBLE_MODE;
+	pT01->T01_plcp_header_length = pDes->PreambleMode;	// Set preamble
+
+}
+
+void
+Mds_Tx(struct wbsoft_priv * adapter)
 {
-		OS_RECEIVE_PACKET_INDICATE( Adapter, pRxLayer1 );
+	phw_data_t	pHwData = &adapter->sHwData;
+	PMDS		pMds = &adapter->Mds;
+	DESCRIPTOR	TxDes;
+	PDESCRIPTOR	pTxDes = &TxDes;
+	u8		*XmitBufAddress;
+	u16		XmitBufSize, PacketSize, stmp, CurrentSize, FragmentThreshold;
+	u8		FillIndex, TxDesIndex, FragmentCount, FillCount;
+	unsigned char	BufferFilled = false, MICAdd = 0;
+
+
+	if (pMds->TxPause)
+		return;
+	if (!hal_driver_init_OK(pHwData))
+		return;
+
+	//Only one thread can be run here
+	if (!atomic_inc_return(&pMds->TxThreadCount) == 1)
+		goto cleanup;
+
+	// Start to fill the data
+	do {
+		FillIndex = pMds->TxFillIndex;
+		if (pMds->TxOwner[FillIndex]) { // Is owned by software 0:Yes 1:No
+#ifdef _PE_TX_DUMP_
+			WBDEBUG(("[Mds_Tx] Tx Owner is H/W.\n"));
+#endif
+			break;
+		}
+
+		XmitBufAddress = pMds->pTxBuffer + (MAX_USB_TX_BUFFER * FillIndex); //Get buffer
+		XmitBufSize = 0;
+		FillCount = 0;
+		do {
+			PacketSize = adapter->sMlmeFrame.len;
+			if (!PacketSize)
+				break;
+
+			//For Check the buffer resource
+			FragmentThreshold = CURRENT_FRAGMENT_THRESHOLD;
+			//931130.5.b
+			FragmentCount = PacketSize/FragmentThreshold + 1;
+			stmp = PacketSize + FragmentCount*32 + 8;//931130.5.c 8:MIC
+			if ((XmitBufSize + stmp) >= MAX_USB_TX_BUFFER) {
+				printk("[Mds_Tx] Excess max tx buffer.\n");
+				break; // buffer is not enough
+			}
+
+
+			//
+			// Start transmitting
+			//
+			BufferFilled = true;
+
+			/* Leaves first u8 intact */
+			memset((u8 *)pTxDes + 1, 0, sizeof(DESCRIPTOR) - 1);
+
+			TxDesIndex = pMds->TxDesIndex;//Get the current ID
+			pTxDes->Descriptor_ID = TxDesIndex;
+			pMds->TxDesFrom[ TxDesIndex ] = 2;//Storing the information of source comming from
+			pMds->TxDesIndex++;
+			pMds->TxDesIndex %= MAX_USB_TX_DESCRIPTOR;
+
+			MLME_GetNextPacket( adapter, pTxDes );
+
+			// Copy header. 8byte USB + 24byte 802.11Hdr. Set TxRate, Preamble type
+			Mds_HeaderCopy( adapter, pTxDes, XmitBufAddress );
+
+			// For speed up Key setting
+			if (pTxDes->EapFix) {
+#ifdef _PE_TX_DUMP_
+				WBDEBUG(("35: EPA 4th frame detected. Size = %d\n", PacketSize));
+#endif
+				pHwData->IsKeyPreSet = 1;
+			}
+
+			// Copy (fragment) frame body, and set USB, 802.11 hdr flag
+			CurrentSize = Mds_BodyCopy(adapter, pTxDes, XmitBufAddress);
+
+			// Set RTS/CTS and Normal duration field into buffer
+			Mds_DurationSet(adapter, pTxDes, XmitBufAddress);
+
+			//
+			// Calculation MIC from buffer which maybe fragment, then fill into temporary address 8 byte
+			// 931130.5.e
+			if (MICAdd)
+				Mds_MicFill( adapter, pTxDes, XmitBufAddress );
+
+			//Shift to the next address
+			XmitBufSize += CurrentSize;
+			XmitBufAddress += CurrentSize;
+
+#ifdef _IBSS_BEACON_SEQ_STICK_
+			if ((XmitBufAddress[ DOT_11_DA_OFFSET+8 ] & 0xfc) != MAC_SUBTYPE_MNGMNT_PROBE_REQUEST) // +8 for USB hdr
+#endif
+				pMds->TxToggle = true;
+
+			// Get packet to transmit completed, 1:TESTSTA 2:MLME 3: Ndis data
+			MLME_SendComplete(adapter, 0, true);
+
+			// Software TSC count 20060214
+			pMds->TxTsc++;
+			if (pMds->TxTsc == 0)
+				pMds->TxTsc_2++;
+
+			FillCount++; // 20060928
+		} while (HAL_USB_MODE_BURST(pHwData)); // End of multiple MSDU copy loop. false = single true = multiple sending
+
+		// Move to the next one, if necessary
+		if (BufferFilled) {
+			// size setting
+			pMds->TxBufferSize[ FillIndex ] = XmitBufSize;
+
+			// 20060928 set Tx count
+			pMds->TxCountInBuffer[FillIndex] = FillCount;
+
+			// Set owner flag
+			pMds->TxOwner[FillIndex] = 1;
+
+			pMds->TxFillIndex++;
+			pMds->TxFillIndex %= MAX_USB_TX_BUFFER_NUMBER;
+			BufferFilled = false;
+		} else
+			break;
+
+		if (!PacketSize) // No more pk for transmitting
+			break;
+
+	} while(true);
+
+	//
+	// Start to send by lower module
+	//
+	if (!pHwData->IsKeyPreSet)
+		Wb35Tx_start(adapter);
+
+ cleanup:
+	atomic_dec(&pMds->TxThreadCount);
 }
 
+void
+Mds_SendComplete(struct wbsoft_priv * adapter, PT02_DESCRIPTOR pT02)
+{
+	PMDS	pMds = &adapter->Mds;
+	phw_data_t	pHwData = &adapter->sHwData;
+	u8	PacketId = (u8)pT02->T02_Tx_PktID;
+	unsigned char	SendOK = true;
+	u8	RetryCount, TxRate;
 
+	if (pT02->T02_IgnoreResult) // Don't care the result
+		return;
+	if (pT02->T02_IsLastMpdu) {
+		//TODO: DTO -- get the retry count and fragment count
+		// Tx rate
+		TxRate = pMds->TxRate[ PacketId ][ 0 ];
+		RetryCount = (u8)pT02->T02_MPDU_Cnt;
+		if (pT02->value & FLAG_ERROR_TX_MASK) {
+			SendOK = false;
+
+			if (pT02->T02_transmit_abort || pT02->T02_out_of_MaxTxMSDULiftTime) {
+				//retry error
+				pHwData->dto_tx_retry_count += (RetryCount+1);
+				//[for tx debug]
+				if (RetryCount<7)
+					pHwData->tx_retry_count[RetryCount] += RetryCount;
+				else
+					pHwData->tx_retry_count[7] += RetryCount;
+				#ifdef _PE_STATE_DUMP_
+				WBDEBUG(("dto_tx_retry_count =%d\n", pHwData->dto_tx_retry_count));
+				#endif
+				MTO_SetTxCount(adapter, TxRate, RetryCount);
+			}
+			pHwData->dto_tx_frag_count += (RetryCount+1);
+
+			//[for tx debug]
+			if (pT02->T02_transmit_abort_due_to_TBTT)
+				pHwData->tx_TBTT_start_count++;
+			if (pT02->T02_transmit_without_encryption_due_to_wep_on_false)
+				pHwData->tx_WepOn_false_count++;
+			if (pT02->T02_discard_due_to_null_wep_key)
+				pHwData->tx_Null_key_count++;
+		} else {
+			if (pT02->T02_effective_transmission_rate)
+				pHwData->tx_ETR_count++;
+			MTO_SetTxCount(adapter, TxRate, RetryCount);
+		}
+
+		// Clear send result buffer
+		pMds->TxResult[ PacketId ] = 0;
+	} else
+		pMds->TxResult[ PacketId ] |= ((u16)(pT02->value & 0x0ffff));
+}
diff --git a/drivers/staging/winbond/mds_f.h b/drivers/staging/winbond/mds_f.h
index 7a682d4cfbdc..ee0f12093dba 100644
--- a/drivers/staging/winbond/mds_f.h
+++ b/drivers/staging/winbond/mds_f.h
@@ -1,33 +1,23 @@
-unsigned char Mds_initial(  PADAPTER Adapter );
-void Mds_Destroy(  PADAPTER Adapter );
-void Mds_Tx(  PADAPTER Adapter );
-void Mds_HeaderCopy(  PADAPTER Adapter,  PDESCRIPTOR pDes,  u8 *TargetBuffer );
-u16 Mds_BodyCopy(  PADAPTER Adapter,  PDESCRIPTOR pDes,  u8 *TargetBuffer );
-void Mds_DurationSet(  PADAPTER Adapter,  PDESCRIPTOR pDes,  u8 *TargetBuffer );
-void Mds_SendComplete(  PADAPTER Adapter,  PT02_DESCRIPTOR pT02 );
-void Mds_MpduProcess(  PADAPTER Adapter,  PDESCRIPTOR pRxDes );
-void Mds_reset_descriptor(  PADAPTER Adapter );
-extern void DataDmp(u8 *pdata, u32 len, u32 offset);
-
+#ifndef __WINBOND_MDS_F_H
+#define __WINBOND_MDS_F_H
 
-void vRxTimerInit(PWB32_ADAPTER Adapter);
-void vRxTimerStart(PWB32_ADAPTER Adapter, int timeout_value);
-void RxTimerHandler_1a( PADAPTER Adapter);
-void vRxTimerStop(PWB32_ADAPTER Adapter);
-void RxTimerHandler( void*			SystemSpecific1,
-					   PWB32_ADAPTER 	Adapter,
-					   void*			SystemSpecific2,
-					   void*			SystemSpecific3);
+#include "wbhal_s.h"
+#include "core.h"
 
+unsigned char Mds_initial(  struct wbsoft_priv *adapter );
+void Mds_Destroy(  struct wbsoft_priv *adapter );
+void Mds_Tx(  struct wbsoft_priv *adapter );
+void Mds_SendComplete(  struct wbsoft_priv *adapter,  PT02_DESCRIPTOR pT02 );
+void Mds_MpduProcess(  struct wbsoft_priv *adapter,  PDESCRIPTOR pRxDes );
+extern void DataDmp(u8 *pdata, u32 len, u32 offset);
 
 // For Asynchronous indicating. The routine collocates with USB.
-void Mds_MsduProcess(  PWB32_ADAPTER Adapter,  PRXLAYER1 pRxLayer1,  u8 SlotIndex);
+void Mds_MsduProcess(  struct wbsoft_priv *adapter,  PRXLAYER1 pRxLayer1,  u8 SlotIndex);
 
 // For data frame sending 20060802
-u16 MDS_GetPacketSize(  PADAPTER Adapter );
-void MDS_GetNextPacket(  PADAPTER Adapter,  PDESCRIPTOR pDes );
-void MDS_GetNextPacketComplete(  PADAPTER Adapter,  PDESCRIPTOR pDes );
-void MDS_SendResult(  PADAPTER Adapter,  u8 PacketId,  unsigned char SendOK );
-void MDS_EthernetPacketReceive(  PADAPTER Adapter,  PRXLAYER1 pRxLayer1 );
-
+u16 MDS_GetPacketSize(  struct wbsoft_priv *adapter );
+void MDS_GetNextPacket(  struct wbsoft_priv *adapter,  PDESCRIPTOR pDes );
+void MDS_GetNextPacketComplete(  struct wbsoft_priv *adapter,  PDESCRIPTOR pDes );
+void MDS_SendResult(  struct wbsoft_priv *adapter,  u8 PacketId,  unsigned char SendOK );
 
+#endif
diff --git a/drivers/staging/winbond/mds_s.h b/drivers/staging/winbond/mds_s.h
index 9df2e0936bf8..ebf61e3ce1dc 100644
--- a/drivers/staging/winbond/mds_s.h
+++ b/drivers/staging/winbond/mds_s.h
@@ -1,9 +1,19 @@
+#ifndef __WINBOND_MDS_H
+#define __WINBOND_MDS_H
+
+#include <linux/timer.h>
+#include <linux/types.h>
+#include <asm/atomic.h>
+
+#include "localpara.h"
+#include "mac_structures.h"
+#include "scan_s.h"
+
 ////////////////////////////////////////////////////////////////////////////////////////////////////////
 #define MAX_USB_TX_DESCRIPTOR		15		// IS89C35 ability
 #define MAX_USB_TX_BUFFER_NUMBER	4		// Virtual pre-buffer number of MAX_USB_TX_BUFFER
 #define MAX_USB_TX_BUFFER			4096	// IS89C35 ability 4n alignment is required for hardware
 
-#define MDS_EVENT_INDICATE( _A, _B, _F )	OS_EVENT_INDICATE( _A, _B, _F )
 #define AUTH_REQUEST_PAIRWISE_ERROR			0		// _F flag setting
 #define AUTH_REQUEST_GROUP_ERROR			1		// _F flag setting
 
@@ -21,20 +31,19 @@
 #define CURRENT_PAIRWISE_KEY			psSME->tx_mic_key
 #define CURRENT_GROUP_KEY				psSME->group_tx_mic_key
 #define CURRENT_ENCRYPT_STATUS			psSME->encrypt_status
-#define CURRENT_WEP_ID					Adapter->sSmePara._dot11WEPDefaultKeyID
-#define CURRENT_CONTROL_PORT_BLOCK		( psSME->wpa_ok!=1 || (Adapter->Mds.boCounterMeasureBlock==1 && (CURRENT_ENCRYPT_STATUS==ENCRYPT_TKIP)) )
-#define CURRENT_FRAGMENT_THRESHOLD		(Adapter->Mds.TxFragmentThreshold & ~0x1)
+#define CURRENT_WEP_ID					adapter->sSmePara._dot11WEPDefaultKeyID
+#define CURRENT_CONTROL_PORT_BLOCK		( psSME->wpa_ok!=1 || (adapter->Mds.boCounterMeasureBlock==1 && (CURRENT_ENCRYPT_STATUS==ENCRYPT_TKIP)) )
+#define CURRENT_FRAGMENT_THRESHOLD		(adapter->Mds.TxFragmentThreshold & ~0x1)
 #define CURRENT_PREAMBLE_MODE			psLOCAL->boShortPreamble?WLAN_PREAMBLE_TYPE_SHORT:WLAN_PREAMBLE_TYPE_LONG
-#define CURRENT_LINK_ON					OS_LINK_STATUS
-#define CURRENT_TX_RATE					Adapter->sLocalPara.CurrentTxRate
-#define CURRENT_FALL_BACK_TX_RATE		Adapter->sLocalPara.CurrentTxFallbackRate
-#define CURRENT_TX_RATE_FOR_MNG			Adapter->sLocalPara.CurrentTxRateForMng
+#define CURRENT_TX_RATE					adapter->sLocalPara.CurrentTxRate
+#define CURRENT_FALL_BACK_TX_RATE		adapter->sLocalPara.CurrentTxFallbackRate
+#define CURRENT_TX_RATE_FOR_MNG			adapter->sLocalPara.CurrentTxRateForMng
 #define CURRENT_PROTECT_MECHANISM		psLOCAL->boProtectMechanism
-#define CURRENT_RTS_THRESHOLD			Adapter->Mds.TxRTSThreshold
+#define CURRENT_RTS_THRESHOLD			adapter->Mds.TxRTSThreshold
 
-#define MIB_GS_XMIT_OK_INC				Adapter->sLocalPara.GS_XMIT_OK++
-#define MIB_GS_RCV_OK_INC				Adapter->sLocalPara.GS_RCV_OK++
-#define MIB_GS_XMIT_ERROR_INC			Adapter->sLocalPara.GS_XMIT_ERROR
+#define MIB_GS_XMIT_OK_INC				adapter->sLocalPara.GS_XMIT_OK++
+#define MIB_GS_RCV_OK_INC				adapter->sLocalPara.GS_RCV_OK++
+#define MIB_GS_XMIT_ERROR_INC			adapter->sLocalPara.GS_XMIT_ERROR
 
 //---------- TX -----------------------------------
 #define ETHERNET_TX_DESCRIPTORS         MAX_USB_TX_BUFFER_NUMBER
@@ -96,9 +105,9 @@ typedef struct _MDS
 	u8	ScanTxPause;	//data Tx pause because the scanning is progressing, but probe request Tx won't.
 	u8	TxPause;//For pause the Mds_Tx modult
 
-	OS_ATOMIC	TxThreadCount;//For thread counting 931130.4.v
+	atomic_t	TxThreadCount;//For thread counting 931130.4.v
 //950301 delete due to HW
-//	OS_ATOMIC	TxConcurrentCount;//931130.4.w
+//	atomic_t	TxConcurrentCount;//931130.4.w
 
 	u16	TxResult[ ((MAX_USB_TX_DESCRIPTOR + 1) & ~0x01) ];//Collect the sending result of Mpdu
 
@@ -133,9 +142,6 @@ typedef struct _MDS
 	u8		boCounterMeasureBlock;
 	u8		reserved_4[2];
 
-	//NDIS_MINIPORT_TIMER	nTimer;
-	OS_TIMER	nTimer;
-
 	u32	TxTsc; // 20060214
 	u32	TxTsc_2; // 20060214
 
@@ -180,4 +186,4 @@ typedef struct _RXLAYER1
 
 }RXLAYER1, * PRXLAYER1;
 
-
+#endif
diff --git a/drivers/staging/winbond/mlme_mib.h b/drivers/staging/winbond/mlme_mib.h
index 89759739cbac..ca8922ec6338 100644
--- a/drivers/staging/winbond/mlme_mib.h
+++ b/drivers/staging/winbond/mlme_mib.h
@@ -22,15 +22,15 @@
 //   Set the dot11ExcludeUnencrypted value.
 //
 // Arguments:
-//   Adapter        - The pointer to the miniport adapter context.
+//   adapter        - The pointer to the miniport adapter context.
 //   ExUnencrypted  - unsigned char type. The value to be set.
 //
 // Return values:
 //   None.
 //============================================================================
-#define MLMESetExcludeUnencrypted(Adapter, ExUnencrypted)     \
+#define MLMESetExcludeUnencrypted(adapter, ExUnencrypted)     \
 {                                                              \
-    (Adapter)->sLocalPara.ExcludeUnencrypted = ExUnencrypted;             \
+    (adapter)->sLocalPara.ExcludeUnencrypted = ExUnencrypted;             \
 }
 
 //============================================================================
@@ -40,12 +40,12 @@
 //   Get the dot11ExcludeUnencrypted value.
 //
 // Arguments:
-//   Adapter        - The pointer to the miniport adapter context.
+//   adapter        - The pointer to the miniport adapter context.
 //
 // Return values:
 //   unsigned char type. The current dot11ExcludeUnencrypted value.
 //============================================================================
-#define MLMEGetExcludeUnencrypted(Adapter) ((unsigned char) (Adapter)->sLocalPara.ExcludeUnencrypted)
+#define MLMEGetExcludeUnencrypted(adapter) ((unsigned char) (adapter)->sLocalPara.ExcludeUnencrypted)
 
 //============================================================================
 // MLMESetMaxReceiveLifeTime --
@@ -54,15 +54,15 @@
 //   Set the dot11MaxReceiveLifeTime value.
 //
 // Arguments:
-//   Adapter        - The pointer to the miniport adapter context.
+//   adapter        - The pointer to the miniport adapter context.
 //   ReceiveLifeTime- u32 type. The value to be set.
 //
 // Return values:
 //   None.
 //============================================================================
-#define MLMESetMaxReceiveLifeTime(Adapter, ReceiveLifeTime)    \
+#define MLMESetMaxReceiveLifeTime(adapter, ReceiveLifeTime)    \
 {                                                               \
-    (Adapter)->Mds.MaxReceiveTime = ReceiveLifeTime;                \
+    (adapter)->Mds.MaxReceiveTime = ReceiveLifeTime;                \
 }
 
 //============================================================================
@@ -72,12 +72,12 @@
 //   Get the dot11MaxReceiveLifeTime value.
 //
 // Arguments:
-//   Adapter        - The pointer to the miniport adapter context.
+//   adapter        - The pointer to the miniport adapter context.
 //
 // Return values:
 //   u32 type. The current dot11MaxReceiveLifeTime value.
 //============================================================================
-#define MLMEGetMaxReceiveLifeTime(Adapter) ((u32) (Adapter)->Mds.MaxReceiveTime)
+#define MLMEGetMaxReceiveLifeTime(adapter) ((u32) (adapter)->Mds.MaxReceiveTime)
 
 #endif
 
diff --git a/drivers/staging/winbond/mlme_s.h b/drivers/staging/winbond/mlme_s.h
index 039fd408ba62..ea12684a2b1d 100644
--- a/drivers/staging/winbond/mlme_s.h
+++ b/drivers/staging/winbond/mlme_s.h
@@ -1,3 +1,12 @@
+#ifndef __WINBOND_MLME_H
+#define __WINBOND_MLME_H
+
+#include <linux/types.h>
+#include <linux/spinlock.h>
+
+#include "mac_structures.h"
+#include "mds_s.h"
+
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //	Mlme.h
 //		Define the related definitions of MLME module
@@ -192,4 +201,4 @@ typedef struct _RXDATA
 
 }__attribute__ ((packed)) RXDATA, *psRXDATA;
 
-
+#endif
diff --git a/drivers/staging/winbond/mlmetxrx.c b/drivers/staging/winbond/mlmetxrx.c
index e8533b8d1976..07802afd2b87 100644
--- a/drivers/staging/winbond/mlmetxrx.c
+++ b/drivers/staging/winbond/mlmetxrx.c
@@ -17,113 +17,56 @@
 //============================================================================
 #include "os_common.h"
 
-void MLMEResetTxRx(PWB32_ADAPTER Adapter)
-{
-	s32     i;
-
-	// Reset the interface between MDS and MLME
-	for (i = 0; i < MAX_NUM_TX_MMPDU; i++)
-		Adapter->sMlmeFrame.TxMMPDUInUse[i] = FALSE;
-	for (i = 0; i < MAX_NUM_RX_MMPDU; i++)
-		Adapter->sMlmeFrame.SaveRxBufSlotInUse[i] = FALSE;
-
-	Adapter->sMlmeFrame.wNumRxMMPDUInMLME   = 0;
-	Adapter->sMlmeFrame.wNumRxMMPDUDiscarded = 0;
-	Adapter->sMlmeFrame.wNumRxMMPDU          = 0;
-	Adapter->sMlmeFrame.wNumTxMMPDUDiscarded = 0;
-	Adapter->sMlmeFrame.wNumTxMMPDU          = 0;
-	Adapter->sLocalPara.boCCAbusy    = FALSE;
-	Adapter->sLocalPara.iPowerSaveMode     = PWR_ACTIVE;     // Power active
-}
+#include "mds_f.h"
 
 //=============================================================================
-//	Function:
-//    MLMEGetMMPDUBuffer()
-//
-//	Description:
-//    Return the pointer to an available data buffer with
-//    the size MAX_MMPDU_SIZE for a MMPDU.
-//
-//  Arguments:
-//    Adapter   - pointer to the miniport adapter context.
-//
-//	Return value:
-//    NULL     : No available data buffer available
-//    Otherwise: Pointer to the data buffer
-//=============================================================================
-
-/* FIXME: Should this just be replaced with kmalloc() and kfree()? */
-u8 *MLMEGetMMPDUBuffer(PWB32_ADAPTER Adapter)
-{
-	s32 i;
-	u8 *returnVal;
-
-	for (i = 0; i< MAX_NUM_TX_MMPDU; i++) {
-		if (Adapter->sMlmeFrame.TxMMPDUInUse[i] == FALSE)
-			break;
-	}
-	if (i >= MAX_NUM_TX_MMPDU) return NULL;
-
-	returnVal = (u8 *)&(Adapter->sMlmeFrame.TxMMPDU[i]);
-	Adapter->sMlmeFrame.TxMMPDUInUse[i] = TRUE;
-
-	return returnVal;
-}
-
-//=============================================================================
-u8 MLMESendFrame(PWB32_ADAPTER Adapter, u8 *pMMPDU, u16 len, u8 DataType)
+u8 MLMESendFrame(struct wbsoft_priv * adapter, u8 *pMMPDU, u16 len, u8 DataType)
 /*	DataType : FRAME_TYPE_802_11_MANAGEMENT, FRAME_TYPE_802_11_MANAGEMENT_CHALLENGE,
 				FRAME_TYPE_802_11_DATA */
 {
-	if (Adapter->sMlmeFrame.IsInUsed != PACKET_FREE_TO_USE) {
-		Adapter->sMlmeFrame.wNumTxMMPDUDiscarded++;
-		return FALSE;
+	if (adapter->sMlmeFrame.IsInUsed != PACKET_FREE_TO_USE) {
+		adapter->sMlmeFrame.wNumTxMMPDUDiscarded++;
+		return false;
 	}
-	Adapter->sMlmeFrame.IsInUsed = PACKET_COME_FROM_MLME;
+	adapter->sMlmeFrame.IsInUsed = PACKET_COME_FROM_MLME;
 
 	// Keep information for sending
-	Adapter->sMlmeFrame.pMMPDU = pMMPDU;
-	Adapter->sMlmeFrame.DataType = DataType;
+	adapter->sMlmeFrame.pMMPDU = pMMPDU;
+	adapter->sMlmeFrame.DataType = DataType;
 	// len must be the last setting due to QUERY_SIZE_SECOND of Mds
-	Adapter->sMlmeFrame.len = len;
-	Adapter->sMlmeFrame.wNumTxMMPDU++;
+	adapter->sMlmeFrame.len = len;
+	adapter->sMlmeFrame.wNumTxMMPDU++;
 
 	// H/W will enter power save by set the register. S/W don't send null frame
 	//with PWRMgt bit enbled to enter power save now.
 
 	// Transmit NDIS packet
-	Mds_Tx(Adapter);
-	return TRUE;
+	Mds_Tx(adapter);
+	return true;
 }
 
-void
-MLME_GetNextPacket(PADAPTER Adapter, PDESCRIPTOR pDes)
+void MLME_GetNextPacket(struct wbsoft_priv *adapter, PDESCRIPTOR desc)
 {
-#define DESCRIPTOR_ADD_BUFFER( _D, _A, _S ) \
-{\
-	_D->InternalUsed = _D->buffer_start_index + _D->buffer_number; \
-	_D->InternalUsed %= MAX_DESCRIPTOR_BUFFER_INDEX; \
-	_D->buffer_address[ _D->InternalUsed ] = _A; \
-	_D->buffer_size[ _D->InternalUsed ] = _S; \
-	_D->buffer_total_size += _S; \
-	_D->buffer_number++;\
-}
-
-	DESCRIPTOR_ADD_BUFFER( pDes, Adapter->sMlmeFrame.pMMPDU, Adapter->sMlmeFrame.len );
-	pDes->Type = Adapter->sMlmeFrame.DataType;
+	desc->InternalUsed = desc->buffer_start_index + desc->buffer_number;
+	desc->InternalUsed %= MAX_DESCRIPTOR_BUFFER_INDEX;
+	desc->buffer_address[desc->InternalUsed] = adapter->sMlmeFrame.pMMPDU;
+	desc->buffer_size[desc->InternalUsed] = adapter->sMlmeFrame.len;
+	desc->buffer_total_size += adapter->sMlmeFrame.len;
+	desc->buffer_number++;
+	desc->Type = adapter->sMlmeFrame.DataType;
 }
 
-void MLMEfreeMMPDUBuffer(PWB32_ADAPTER Adapter, s8 *pData)
+static void MLMEfreeMMPDUBuffer(struct wbsoft_priv *adapter, s8 *pData)
 {
 	int i;
 
 	// Reclaim the data buffer
 	for (i = 0; i < MAX_NUM_TX_MMPDU; i++) {
-		if (pData == (s8 *)&(Adapter->sMlmeFrame.TxMMPDU[i]))
+		if (pData == (s8 *)&(adapter->sMlmeFrame.TxMMPDU[i]))
 			break;
 	}
-	if (Adapter->sMlmeFrame.TxMMPDUInUse[i])
-		Adapter->sMlmeFrame.TxMMPDUInUse[i] = FALSE;
+	if (adapter->sMlmeFrame.TxMMPDUInUse[i])
+		adapter->sMlmeFrame.TxMMPDUInUse[i] = false;
 	else  {
 		// Something wrong
 		// PD43 Add debug code here???
@@ -131,19 +74,19 @@ void MLMEfreeMMPDUBuffer(PWB32_ADAPTER Adapter, s8 *pData)
 }
 
 void
-MLME_SendComplete(PADAPTER Adapter, u8 PacketID, unsigned char SendOK)
+MLME_SendComplete(struct wbsoft_priv * adapter, u8 PacketID, unsigned char SendOK)
 {
 	MLME_TXCALLBACK	TxCallback;
 
     // Reclaim the data buffer
-	Adapter->sMlmeFrame.len = 0;
-	MLMEfreeMMPDUBuffer( Adapter, Adapter->sMlmeFrame.pMMPDU );
+	adapter->sMlmeFrame.len = 0;
+	MLMEfreeMMPDUBuffer( adapter, adapter->sMlmeFrame.pMMPDU );
 
 
 	TxCallback.bResult = MLME_SUCCESS;
 
 	// Return resource
-	Adapter->sMlmeFrame.IsInUsed = PACKET_FREE_TO_USE;
+	adapter->sMlmeFrame.IsInUsed = PACKET_FREE_TO_USE;
 }
 
 
diff --git a/drivers/staging/winbond/mlmetxrx_f.h b/drivers/staging/winbond/mlmetxrx_f.h
index 24cd5f308d9f..5f05a6e1cda0 100644
--- a/drivers/staging/winbond/mlmetxrx_f.h
+++ b/drivers/staging/winbond/mlmetxrx_f.h
@@ -8,32 +8,25 @@
 #ifndef _MLMETXRX_H
 #define _MLMETXRX_H
 
+#include "core.h"
+
 void
 MLMEProcThread(
-     PWB32_ADAPTER    Adapter
+     struct wbsoft_priv *    adapter
 	);
 
-void MLMEResetTxRx( PWB32_ADAPTER Adapter);
-
-u8 *
-MLMEGetMMPDUBuffer(
-     PWB32_ADAPTER    Adapter
-   );
-
-void MLMEfreeMMPDUBuffer( PWB32_ADAPTER Adapter,  s8 * pData);
-
-void MLME_GetNextPacket(  PADAPTER Adapter,  PDESCRIPTOR pDes );
-u8 MLMESendFrame( PWB32_ADAPTER Adapter,
+void MLME_GetNextPacket(  struct wbsoft_priv * adapter,  PDESCRIPTOR pDes );
+u8 MLMESendFrame( struct wbsoft_priv * adapter,
 					u8	*pMMPDU,
 					u16	len,
 					 u8	DataType);
 
 void
-MLME_SendComplete(  PWB32_ADAPTER Adapter,  u8 PacketID,  unsigned char SendOK );
+MLME_SendComplete(  struct wbsoft_priv * adapter,  u8 PacketID,  unsigned char SendOK );
 
 void
 MLMERcvFrame(
-     PWB32_ADAPTER    Adapter,
+     struct wbsoft_priv *    adapter,
      PRXBUFFER        pRxBufferArray,
      u8            NumOfBuffer,
      u8            ReturnSlotIndex
@@ -41,11 +34,11 @@ MLMERcvFrame(
 
 void
 MLMEReturnPacket(
-     PWB32_ADAPTER    Adapter,
+     struct wbsoft_priv *    adapter,
      u8 *          pRxBufer
    );
 #ifdef _IBSS_BEACON_SEQ_STICK_
-s8 SendBCNullData(PWB32_ADAPTER Adapter, u16 wIdx);
+s8 SendBCNullData(struct wbsoft_priv * adapter, u16 wIdx);
 #endif
 
 #endif
diff --git a/drivers/staging/winbond/mto.c b/drivers/staging/winbond/mto.c
index 2ef60e5120cc..de11a05efae2 100644
--- a/drivers/staging/winbond/mto.c
+++ b/drivers/staging/winbond/mto.c
@@ -23,10 +23,12 @@
 
 // LA20040210_DTO kevin
 #include "os_common.h"
+#include "sme_api.h"
+#include "gl_80211.h"
+#include "wbhal_f.h"
 
 // Declare SQ3 to rate and fragmentation threshold table
 // Declare fragmentation thresholds table
-#define MTO_MAX_SQ3_LEVELS                      14
 #define MTO_MAX_FRAG_TH_LEVELS                  5
 #define MTO_MAX_DATA_RATE_LEVELS                12
 
@@ -35,181 +37,15 @@ u16 MTO_Frag_Th_Tbl[MTO_MAX_FRAG_TH_LEVELS] =
     256, 384, 512, 768, 1536
 };
 
-u8  MTO_SQ3_Level[MTO_MAX_SQ3_LEVELS] =
-{
-    0, 26, 30, 32, 34, 35, 37, 42, 44, 46, 54, 62, 78, 81
-};
-u8  MTO_SQ3toRate[MTO_MAX_SQ3_LEVELS] =
-{
-    0, 1, 1, 2, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11
-};
-u8  MTO_SQ3toFrag[MTO_MAX_SQ3_LEVELS] =
-{
-    0, 2, 2, 3, 3, 4, 4, 4, 4, 4, 4, 4, 4, 4
-};
-
-// One Exchange Time table
-//
-u16 MTO_One_Exchange_Time_Tbl_l[MTO_MAX_FRAG_TH_LEVELS][MTO_MAX_DATA_RATE_LEVELS] =
-{
-    { 2554, 1474,  822,    0,    0,  636,    0,    0,    0,    0,    0,    0},
-    { 3578, 1986, 1009,    0,    0,  729,    0,    0,    0,    0,    0,    0},
-    { 4602, 2498, 1195,    0,    0,  822,    0,    0,    0,    0,    0,    0},
-    { 6650, 3522, 1567,    0,    0, 1009,    0,    0,    0,    0,    0,    0},
-    {12794, 6594, 2684,    0,    0, 1567,    0,    0,    0,    0,    0,    0}
-};
-
-u16 MTO_One_Exchange_Time_Tbl_s[MTO_MAX_FRAG_TH_LEVELS][MTO_MAX_DATA_RATE_LEVELS] =
-{
-    {    0, 1282,  630,  404,  288,  444,  232,  172,  144,  116,  100,   96},
-    {    0, 1794,  817,  572,  400,  537,  316,  228,  188,  144,  124,  116},
-    {    0, 2306, 1003,  744,  516,  630,  400,  288,  228,  172,  144,  136},
-    {    0, 3330, 1375, 1084,  744,  817,  572,  400,  316,  228,  188,  172},
-    {    0, 6402, 2492, 2108, 1424, 1375, 1084,  740,  572,  400,  316,  284}
-};
-
-#define MTO_ONE_EXCHANGE_TIME(preamble_type, frag_th_lvl, data_rate_lvl) \
-            (preamble_type) ?   MTO_One_Exchange_Time_Tbl_s[frag_th_lvl][data_rate_lvl] : \
-                                MTO_One_Exchange_Time_Tbl_l[frag_th_lvl][data_rate_lvl]
-
 // Declare data rate table
 //The following table will be changed at anytime if the opration rate supported by AP don't
 //match the table
-u8  MTO_Data_Rate_Tbl[MTO_MAX_DATA_RATE_LEVELS] =
-{
+static u8 MTO_Data_Rate_Tbl[MTO_MAX_DATA_RATE_LEVELS] = {
     2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108
 };
 
-//The Stardard_Data_Rate_Tbl and Level2PerTbl table is used to indirectly retreive PER
-//information from Rate_PER_TBL
-//The default settings is AP can support full rate set.
-static u8  Stardard_Data_Rate_Tbl[MTO_MAX_DATA_RATE_LEVELS] =
-{
-	2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108
-};
-static u8  Level2PerTbl[MTO_MAX_DATA_RATE_LEVELS] =
-{
-	0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11
-};
-//How many kind of tx rate can be supported by AP
-//DTO will change Rate between MTO_Data_Rate_Tbl[0] and MTO_Data_Rate_Tbl[MTO_DataRateAvailableLevel-1]
-static u8  MTO_DataRateAvailableLevel = MTO_MAX_DATA_RATE_LEVELS;
-//Smoothed PER table for each different RATE based on packet length of 1514
-static int Rate_PER_TBL[91][MTO_MAX_DATA_RATE_LEVELS] = {
-//        1M    2M    5.5M  11M   6M    9M    12M     18M    24M    36M    48M   54M
-/* 0%  */{ 93,   177,  420,  538,  690,  774,  1001,  1401,  1768,  2358,  2838,  3039},
-/* 1%  */{ 92,   176,  416,  533,  683,  767,  992,   1389,  1752,  2336,  2811,  3010},
-/* 2%  */{ 91,   174,  412,  528,  675,  760,  983,   1376,  1735,  2313,  2783,  2979},
-/* 3%  */{ 90,   172,  407,  523,  667,  753,  973,   1363,  1719,  2290,  2755,  2948},
-/* 4%  */{ 90,   170,  403,  518,  659,  746,  964,   1350,  1701,  2266,  2726,  2916},
-/* 5%  */{ 89,   169,  398,  512,  651,  738,  954,   1336,  1684,  2242,  2696,  2884},
-/* 6%  */{ 88,   167,  394,  507,  643,  731,  944,   1322,  1666,  2217,  2665,  2851},
-/* 7%  */{ 87,   165,  389,  502,  635,  723,  935,   1308,  1648,  2192,  2634,  2817},
-/* 8%  */{ 86,   163,  384,  497,  626,  716,  924,   1294,  1629,  2166,  2602,  2782},
-/* 9%  */{ 85,   161,  380,  491,  618,  708,  914,   1279,  1611,  2140,  2570,  2747},
-/* 10% */{ 84,   160,  375,  486,  609,  700,  904,   1265,  1591,  2113,  2537,  2711},
-/* 11% */{ 83,   158,  370,  480,  600,  692,  894,   1250,  1572,  2086,  2503,  2675},
-/* 12% */{ 82,   156,  365,  475,  592,  684,  883,   1234,  1552,  2059,  2469,  2638},
-/* 13% */{ 81,   154,  360,  469,  583,  676,  872,   1219,  1532,  2031,  2435,  2600},
-/* 14% */{ 80,   152,  355,  464,  574,  668,  862,   1204,  1512,  2003,  2400,  2562},
-/* 15% */{ 79,   150,  350,  458,  565,  660,  851,   1188,  1492,  1974,  2365,  2524},
-/* 16% */{ 78,   148,  345,  453,  556,  652,  840,   1172,  1471,  1945,  2329,  2485},
-/* 17% */{ 77,   146,  340,  447,  547,  643,  829,   1156,  1450,  1916,  2293,  2446},
-/* 18% */{ 76,   144,  335,  441,  538,  635,  818,   1140,  1429,  1887,  2256,  2406},
-/* 19% */{ 75,   143,  330,  436,  529,  627,  807,   1124,  1408,  1857,  2219,  2366},
-/* 20% */{ 74,   141,  325,  430,  520,  618,  795,   1107,  1386,  1827,  2182,  2326},
-/* 21% */{ 73,   139,  320,  424,  510,  610,  784,   1091,  1365,  1797,  2145,  2285},
-/* 22% */{ 72,   137,  314,  418,  501,  601,  772,   1074,  1343,  1766,  2107,  2244},
-/* 23% */{ 71,   135,  309,  412,  492,  592,  761,   1057,  1321,  1736,  2069,  2203},
-/* 24% */{ 70,   133,  304,  407,  482,  584,  749,   1040,  1299,  1705,  2031,  2161},
-/* 25% */{ 69,   131,  299,  401,  473,  575,  738,   1023,  1277,  1674,  1992,  2120},
-/* 26% */{ 68,   129,  293,  395,  464,  566,  726,   1006,  1254,  1642,  1953,  2078},
-/* 27% */{ 67,   127,  288,  389,  454,  557,  714,   989,   1232,  1611,  1915,  2035},
-/* 28% */{ 66,   125,  283,  383,  445,  549,  703,   972,   1209,  1579,  1876,  1993},
-/* 29% */{ 65,   123,  278,  377,  436,  540,  691,   955,   1187,  1548,  1836,  1951},
-/* 30% */{ 64,   121,  272,  371,  426,  531,  679,   937,   1164,  1516,  1797,  1908},
-/* 31% */{ 63,   119,  267,  365,  417,  522,  667,   920,   1141,  1484,  1758,  1866},
-/* 32% */{ 62,   117,  262,  359,  407,  513,  655,   902,   1118,  1453,  1719,  1823},
-/* 33% */{ 61,   115,  256,  353,  398,  504,  643,   885,   1095,  1421,  1679,  1781},
-/* 34% */{ 60,   113,  251,  347,  389,  495,  631,   867,   1072,  1389,  1640,  1738},
-/* 35% */{ 59,   111,  246,  341,  379,  486,  619,   850,   1049,  1357,  1600,  1695},
-/* 36% */{ 58,   108,  240,  335,  370,  477,  607,   832,   1027,  1325,  1561,  1653},
-/* 37% */{ 57,   106,  235,  329,  361,  468,  595,   815,   1004,  1293,  1522,  1610},
-/* 38% */{ 56,   104,  230,  323,  351,  459,  584,   797,   981,   1261,  1483,  1568},
-/* 39% */{ 55,   102,  224,  317,  342,  450,  572,   780,   958,   1230,  1443,  1526},
-/* 40% */{ 54,   100,  219,  311,  333,  441,  560,   762,   935,   1198,  1404,  1484},
-/* 41% */{ 53,   98,   214,  305,  324,  432,  548,   744,   912,   1166,  1366,  1442},
-/* 42% */{ 52,   96,   209,  299,  315,  423,  536,   727,   889,   1135,  1327,  1400},
-/* 43% */{ 51,   94,   203,  293,  306,  414,  524,   709,   866,   1104,  1289,  1358},
-/* 44% */{ 50,   92,   198,  287,  297,  405,  512,   692,   844,   1072,  1250,  1317},
-/* 45% */{ 49,   90,   193,  281,  288,  396,  500,   675,   821,   1041,  1212,  1276},
-/* 46% */{ 48,   88,   188,  275,  279,  387,  488,   657,   799,   1011,  1174,  1236},
-/* 47% */{ 47,   86,   183,  269,  271,  378,  476,   640,   777,   980,   1137,  1195},
-/* 48% */{ 46,   84,   178,  262,  262,  369,  464,   623,   754,   949,   1100,  1155},
-/* 49% */{ 45,   82,   173,  256,  254,  360,  452,   606,   732,   919,   1063,  1116},
-/* 50% */{ 44,   80,   168,  251,  245,  351,  441,   589,   710,   889,   1026,  1076},
-/* 51% */{ 43,   78,   163,  245,  237,  342,  429,   572,   689,   860,   990,   1038},
-/* 52% */{ 42,   76,   158,  239,  228,  333,  417,   555,   667,   830,   955,   999},
-/* 53% */{ 41,   74,   153,  233,  220,  324,  406,   538,   645,   801,   919,   961},
-/* 54% */{ 40,   72,   148,  227,  212,  315,  394,   522,   624,   773,   884,   924},
-/* 55% */{ 39,   70,   143,  221,  204,  307,  383,   505,   603,   744,   850,   887},
-/* 56% */{ 38,   68,   138,  215,  196,  298,  371,   489,   582,   716,   816,   851},
-/* 57% */{ 37,   67,   134,  209,  189,  289,  360,   473,   562,   688,   783,   815},
-/* 58% */{ 36,   65,   129,  203,  181,  281,  349,   457,   541,   661,   750,   780},
-/* 59% */{ 35,   63,   124,  197,  174,  272,  338,   441,   521,   634,   717,   745},
-/* 60% */{ 34,   61,   120,  192,  166,  264,  327,   425,   501,   608,   686,   712},
-/* 61% */{ 33,   59,   115,  186,  159,  255,  316,   409,   482,   582,   655,   678},
-/* 62% */{ 32,   57,   111,  180,  152,  247,  305,   394,   462,   556,   624,   646},
-/* 63% */{ 31,   55,   107,  174,  145,  238,  294,   379,   443,   531,   594,   614},
-/* 64% */{ 30,   53,   102,  169,  138,  230,  283,   364,   425,   506,   565,   583},
-/* 65% */{ 29,   52,   98,   163,  132,  222,  273,   349,   406,   482,   536,   553},
-/* 66% */{ 28,   50,   94,   158,  125,  214,  262,   334,   388,   459,   508,   523},
-/* 67% */{ 27,   48,   90,   152,  119,  206,  252,   320,   370,   436,   481,   495},
-/* 68% */{ 26,   46,   86,   147,  113,  198,  242,   306,   353,   413,   455,   467},
-/* 69% */{ 26,   44,   82,   141,  107,  190,  231,   292,   336,   391,   429,   440},
-/* 70% */{ 25,   43,   78,   136,  101,  182,  221,   278,   319,   370,   405,   414},
-/* 71% */{ 24,   41,   74,   130,  95,   174,  212,   265,   303,   350,   381,   389},
-/* 72% */{ 23,   39,   71,   125,  90,   167,  202,   252,   287,   329,   358,   365},
-/* 73% */{ 22,   37,   67,   119,  85,   159,  192,   239,   271,   310,   335,   342},
-/* 74% */{ 21,   36,   63,   114,  80,   151,  183,   226,   256,   291,   314,   320},
-/* 75% */{ 20,   34,   60,   109,  75,   144,  174,   214,   241,   273,   294,   298},
-/* 76% */{ 19,   32,   57,   104,  70,   137,  164,   202,   227,   256,   274,   278},
-/* 77% */{ 18,   31,   53,   99,   66,   130,  155,   190,   213,   239,   256,   259},
-/* 78% */{ 17,   29,   50,   94,   62,   122,  146,   178,   200,   223,   238,   241},
-/* 79% */{ 16,   28,   47,   89,   58,   115,  138,   167,   187,   208,   222,   225},
-/* 80% */{ 16,   26,   44,   84,   54,   109,  129,   156,   175,   194,   206,   209},
-/* 81% */{ 15,   24,   41,   79,   50,   102,  121,   146,   163,   180,   192,   194},
-/* 82% */{ 14,   23,   39,   74,   47,   95,   113,   136,   151,   167,   178,   181},
-/* 83% */{ 13,   21,   36,   69,   44,   89,   105,   126,   140,   155,   166,   169},
-/* 84% */{ 12,   20,   33,   64,   41,   82,   97,    116,   130,   144,   155,   158},
-/* 85% */{ 11,   19,   31,   60,   39,   76,   89,    107,   120,   134,   145,   149},
-/* 86% */{ 11,   17,   29,   55,   36,   70,   82,    98,    110,   125,   136,   140},
-/* 87% */{ 10,   16,   26,   51,   34,   64,   75,    90,    102,   116,   128,   133},
-/* 88% */{ 9,    14,   24,   46,   32,   58,   68,    81,    93,    108,   121,   128},
-/* 89% */{ 8,    13,   22,   42,   31,   52,   61,    74,    86,    102,   116,   124},
-/* 90% */{ 7,    12,   21,   37,   29,   46,   54,    66,    79,    96,    112,   121}
-};
-
-#define RSSIBUF_NUM 10
-#define RSSI2RATE_SIZE 9
-
-static TXRETRY_REC TxRateRec={MTO_MAX_DATA_RATE_LEVELS - 1, 0};   //new record=>TxRateRec
-static int TxRetryRate;
-//static int SQ3, BSS_PK_CNT, NIDLESLOT, SLOT_CNT, INTERF_CNT, GAP_CNT, DS_EVM;
-static s32 RSSIBuf[RSSIBUF_NUM]={-70, -70, -70, -70, -70, -70, -70, -70, -70, -70};
-static s32 RSSISmoothed=-700;
-static int RSSIBufIndex=0;
-static u8 max_rssi_rate;
-static int rate_tbl[13] = {0,1,2,5,11,6,9,12,18,24,36,48,54};
-//[WKCHEN]static core_data_t *pMTOcore_data=NULL;
-
 static int TotalTxPkt = 0;
 static int TotalTxPktRetry = 0;
-static int TxPktPerAnt[3] = {0,0,0};
-static int RXRSSIANT[3] ={-70,-70,-70};
-static int TxPktRetryPerAnt[3] = {0,0,0};
-//static int TxDominateFlag=FALSE;
-static u8 old_antenna[4]={1 ,0 ,1 ,0};
 static int retryrate_rec[MTO_MAX_DATA_RATE_LEVELS];//this record the retry rate at different data rate
 
 static int PeriodTotalTxPkt = 0;
@@ -221,128 +57,14 @@ typedef struct
 	u8  TxRate;
 }RSSI2RATE;
 
-static RSSI2RATE RSSI2RateTbl[RSSI2RATE_SIZE] =
-{
-	{-740, 108},  // 54M
-	{-760, 96},  // 48M
-	{-820, 72},  // 36M
-	{-850, 48},  // 24M
-	{-870, 36},  // 18M
-	{-890, 24},  // 12M
-	{-900, 12},  // 6M
-	{-920, 11}, // 5.5M
-	{-950, 4}, // 2M
-};
-static u8 untogglecount;
-static u8 last_rate_ant; //this is used for antenna backoff-hh
-
-u8	boSparseTxTraffic = FALSE;
+static u8 boSparseTxTraffic = false;
 
 void MTO_Init(MTO_FUNC_INPUT);
-void AntennaToggleInitiator(MTO_FUNC_INPUT);
-void AntennaToggleState(MTO_FUNC_INPUT);
-void TxPwrControl(MTO_FUNC_INPUT);
-void GetFreshAntennaData(MTO_FUNC_INPUT);
 void TxRateReductionCtrl(MTO_FUNC_INPUT);
 /** 1.1.31.1000 Turbo modify */
-//void MTO_SetDTORateRange(int type);
-void MTO_SetDTORateRange(MTO_FUNC_INPUT, u8 *pRateArray, u8 ArraySize);
 void MTO_SetTxCount(MTO_FUNC_INPUT, u8 t0, u8 index);
 void MTO_TxFailed(MTO_FUNC_INPUT);
-void SmoothRSSI(s32 new_rssi);
 void hal_get_dto_para(MTO_FUNC_INPUT, char *buffer);
-u8 CalcNewRate(MTO_FUNC_INPUT, u8 old_rate, u32 retry_cnt, u32 tx_frag_cnt);
-u8 GetMaxRateLevelFromRSSI(void);
-u8 MTO_GetTxFallbackRate(MTO_FUNC_INPUT);
-int Divide(int a, int b);
-void multiagc(MTO_FUNC_INPUT, u8 high_gain_mode);
-
-//===========================================================================
-//  MTO_Init --
-//
-//  Description:
-//    Set DTO Tx Rate Scope because different AP could have different Rate set.
-//    After our staion join with AP, LM core will call this function to initialize
-//    Tx Rate table.
-//
-//  Arguments:
-//    pRateArray      - The pointer to the Tx Rate Array by the following order
-//                    - 2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108
-//                    - DTO won't check whether rate order is invalid or not
-//    ArraySize       - The array size to indicate how many tx rate we can choose
-//
-//  sample code:
-//	{
-//		u8 RateArray[4] = {2, 4, 11, 22};
-//		MTO_SetDTORateRange(RateArray, 4);
-//	}
-//
-//  Return Value:
-//    None
-//============================================================================
-void MTO_SetDTORateRange(MTO_FUNC_INPUT,u8 *pRateArray, u8 ArraySize)
-{
-	u8	i, j=0;
-
-	for(i=0;i<ArraySize;i++)
-	{
-		if(pRateArray[i] == 22)
-			break;
-	}
-	if(i < ArraySize) //we need adjust the order of rate list because 11Mbps rate exists
-	{
-		for(;i>0;i--)
-		{
-			if(pRateArray[i-1] <= 11)
-				break;
-			pRateArray[i] = pRateArray[i-1];
-		}
-		pRateArray[i] = 22;
-		MTO_OFDM_RATE_LEVEL() = i;
-	}
-	else
-	{
-		for(i=0; i<ArraySize; i++)
-		{
-			if (pRateArray[i] >= 12)
-				break;
-		}
-		MTO_OFDM_RATE_LEVEL() = i;
-	}
-
-	for(i=0;i<ArraySize;i++)
-	{
-		MTO_Data_Rate_Tbl[i] = pRateArray[i];
-		for(;j<MTO_MAX_DATA_RATE_LEVELS;j++)
-		{
-			if(Stardard_Data_Rate_Tbl[j] == pRateArray[i])
-				break;
-		}
-		Level2PerTbl[i] = j;
-		#ifdef _PE_DTO_DUMP_
-		WBDEBUG(("[MTO]:Op Rate[%d]: %d\n",i, MTO_Data_Rate_Tbl[i]));
-		#endif
-	}
-	MTO_DataRateAvailableLevel = ArraySize;
-	if( MTO_DATA().RatePolicy ) // 0 means that no registry setting
-	{
-		if( MTO_DATA().RatePolicy == 1 )
-			TxRateRec.tx_rate = 0;	//ascent
-		else
-			TxRateRec.tx_rate = MTO_DataRateAvailableLevel -1 ;	//descent
-	}
-	else
-	{
-		if( MTO_INITTXRATE_MODE )
-			TxRateRec.tx_rate = 0;	//ascent
-		else
-			TxRateRec.tx_rate = MTO_DataRateAvailableLevel -1 ;	//descent
-	}
-	TxRateRec.tx_retry_rate = 0;
-	//set default rate for initial use
-	MTO_RATE_LEVEL() = TxRateRec.tx_rate;
-	MTO_FALLBACK_RATE_LEVEL() = MTO_RATE_LEVEL();
-}
 
 //===========================================================================
 //  MTO_Init --
@@ -353,7 +75,7 @@ void MTO_SetDTORateRange(MTO_FUNC_INPUT,u8 *pRateArray, u8 ArraySize)
 //    This function should be invoked during system initialization.
 //
 //  Arguments:
-//    Adapter      - The pointer to the Miniport Adapter Context
+//    adapter      - The pointer to the Miniport adapter Context
 //
 //  Return Value:
 //    None
@@ -393,8 +115,8 @@ void MTO_Init(MTO_FUNC_INPUT)
     MTO_SQ_ANT(0)       = 0;
     MTO_SQ_ANT(1)       = 0;
     MTO_ANT_DIVERSITY() = MTO_ANTENNA_DIVERSITY_ON;
-    //CardSet_AntennaDiversity(Adapter, MTO_ANT_DIVERSITY());
-    //PLMESetAntennaDiversity( Adapter, MTO_ANT_DIVERSITY());
+    //CardSet_AntennaDiversity(adapter, MTO_ANT_DIVERSITY());
+    //PLMESetAntennaDiversity( adapter, MTO_ANT_DIVERSITY());
 
     MTO_AGING_TIMEOUT() = 0;//MTO_TMR_AGING() / MTO_TMR_PERIODIC();
 
@@ -402,7 +124,6 @@ void MTO_Init(MTO_FUNC_INPUT)
     //
     //MTO_RATE_LEVEL()            = 10;
     MTO_RATE_LEVEL()            = 0;
-	MTO_FALLBACK_RATE_LEVEL()	= MTO_RATE_LEVEL();
     MTO_FRAG_TH_LEVEL()         = 4;
     /** 1.1.23.1000 Turbo modify from -1 to +1
 	MTO_RTS_THRESHOLD()         = MTO_FRAG_TH() - 1;
@@ -470,669 +191,6 @@ void MTO_Init(MTO_FUNC_INPUT)
 	MTO_DATA().RSSI_low = -60;
 }
 
-//---------------------------------------------------------------------------//
-static u32 DTO_Rx_Info[13][3];
-static u32 DTO_RxCRCFail_Info[13][3];
-static u32 AntennaToggleBkoffTimer=5;
-typedef struct{
-	int RxRate;
-	int RxRatePkts;
-	int index;
-}RXRATE_ANT;
-RXRATE_ANT RxRatePeakAnt[3];
-
-#define ANT0    0
-#define ANT1    1
-#define OLD_ANT 2
-
-void SearchPeakRxRate(int index)
-{
-	int i;
-	RxRatePeakAnt[index].RxRatePkts=0;
-	//Find out the best rx rate which is used on different antenna
-	for(i=1;i<13;i++)
-	{
-		if(DTO_Rx_Info[i][index] > (u32) RxRatePeakAnt[index].RxRatePkts)
-		{
-			RxRatePeakAnt[index].RxRatePkts = DTO_Rx_Info[i][index];
-			RxRatePeakAnt[index].RxRate = rate_tbl[i];
-			RxRatePeakAnt[index].index = i;
-		}
-	}
-}
-
-void ResetDTO_RxInfo(int index, MTO_FUNC_INPUT)
-{
-	int i;
-
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("ResetDTOrx\n"));
-	#endif
-
-	for(i=0;i<13;i++)
-		DTO_Rx_Info[i][index] = MTO_HAL()->rx_ok_count[i];
-
-	for(i=0;i<13;i++)
-		DTO_RxCRCFail_Info[i][index] = MTO_HAL()->rx_err_count[i];
-
-	TotalTxPkt = 0;
-	TotalTxPktRetry = 0;
-}
-
-void GetDTO_RxInfo(int index, MTO_FUNC_INPUT)
-{
-	int i;
-
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("GetDTOrx\n"));
-	#endif
-
-	//PDEBUG(("[MTO]:DTO_Rx_Info[%d]=%d, rx_ok_count=%d\n", index, DTO_Rx_Info[0][index], phw_data->rx_ok_count[0]));
-	for(i=0;i<13;i++)
-		DTO_Rx_Info[i][index] = abs(MTO_HAL()->rx_ok_count[i] - DTO_Rx_Info[i][index]);
-	if(DTO_Rx_Info[0][index]==0) DTO_Rx_Info[0][index] = 1;
-
-	for(i=0;i<13;i++)
-		DTO_RxCRCFail_Info[i][index] = MTO_HAL()->rx_err_count[i] - DTO_RxCRCFail_Info[i][index];
-
-	TxPktPerAnt[index] = TotalTxPkt;
-	TxPktRetryPerAnt[index] = TotalTxPktRetry;
-	TotalTxPkt = 0;
-	TotalTxPktRetry = 0;
-}
-
-void OutputDebugInfo(int index1, int index2)
-{
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("[HHDTO]:Total Rx (%d)\t\t(%d) \n ", DTO_Rx_Info[0][index1], DTO_Rx_Info[0][index2]));
-    WBDEBUG(("[HHDTO]:RECEIVE RSSI: (%d)\t\t(%d) \n ", RXRSSIANT[index1], RXRSSIANT[index2]));
-	WBDEBUG(("[HHDTO]:TX packet correct rate: (%d)%%\t\t(%d)%% \n ",Divide(TxPktPerAnt[index1]*100,TxPktRetryPerAnt[index1]), Divide(TxPktPerAnt[index2]*100,TxPktRetryPerAnt[index2])));
-	#endif
-	{
-		int tmp1, tmp2;
-		#ifdef _PE_DTO_DUMP_
-		WBDEBUG(("[HHDTO]:Total Tx (%d)\t\t(%d) \n ", TxPktPerAnt[index1], TxPktPerAnt[index2]));
-		WBDEBUG(("[HHDTO]:Total Tx retry (%d)\t\t(%d) \n ", TxPktRetryPerAnt[index1], TxPktRetryPerAnt[index2]));
-		#endif
-		tmp1 = TxPktPerAnt[index1] + DTO_Rx_Info[0][index1];
-		tmp2 = TxPktPerAnt[index2] + DTO_Rx_Info[0][index2];
-		#ifdef _PE_DTO_DUMP_
-		WBDEBUG(("[HHDTO]:Total Tx+RX (%d)\t\t(%d) \n ", tmp1, tmp2));
-		#endif
-	}
-}
-
-unsigned char TxDominate(int index)
-{
-	int tmp;
-
-	tmp = TxPktPerAnt[index] + DTO_Rx_Info[0][index];
-
-	if(Divide(TxPktPerAnt[index]*100, tmp) > 40)
-		return TRUE;
-	else
-		return FALSE;
-}
-
-unsigned char CmpTxRetryRate(int index1, int index2)
-{
-	int tx_retry_rate1, tx_retry_rate2;
-	tx_retry_rate1 = Divide((TxPktRetryPerAnt[index1] - TxPktPerAnt[index1])*100, TxPktRetryPerAnt[index1]);
-	tx_retry_rate2 = Divide((TxPktRetryPerAnt[index2] - TxPktPerAnt[index2])*100, TxPktRetryPerAnt[index2]);
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("[MTO]:TxRetry Ant0: (%d%%)  Ant1: (%d%%) \n ", tx_retry_rate1, tx_retry_rate2));
-	#endif
-
-	if(tx_retry_rate1 > tx_retry_rate2)
-		return TRUE;
-	else
-		return FALSE;
-}
-
-void GetFreshAntennaData(MTO_FUNC_INPUT)
-{
-    u8      x;
-
-	x = hal_get_antenna_number(MTO_HAL());
-	//hal_get_bss_pk_cnt(MTO_HAL());
-	//hal_get_est_sq3(MTO_HAL(), 1);
-	old_antenna[0] = x;
-	//if this is the function for timer
-	ResetDTO_RxInfo(x, MTO_FUNC_INPUT_DATA);
-	if(AntennaToggleBkoffTimer)
-			AntennaToggleBkoffTimer--;
-	if (abs(last_rate_ant-MTO_RATE_LEVEL())>1)  //backoff timer reset
-		AntennaToggleBkoffTimer=0;
-
-	if (MTO_ANT_DIVERSITY() != MTO_ANTENNA_DIVERSITY_ON ||
-		MTO_ANT_DIVERSITY_ENABLE() != 1)
-	AntennaToggleBkoffTimer=1;
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("[HHDTO]:**last data rate=%d,now data rate=%d**antenna toggle timer=%d",last_rate_ant,MTO_RATE_LEVEL(),AntennaToggleBkoffTimer));
-	#endif
-	last_rate_ant=MTO_RATE_LEVEL();
-	if(AntennaToggleBkoffTimer==0)
-	{
-		MTO_TOGGLE_STATE() = TOGGLE_STATE_WAIT0;
-		#ifdef _PE_DTO_DUMP_
-		WBDEBUG(("[HHDTO]:===state is starting==for antenna toggle==="));
-		#endif
-	}
-	else
-		MTO_TOGGLE_STATE() = TOGGLE_STATE_IDLE;
-
-	if ((MTO_BACKOFF_TMR()!=0)&&(MTO_RATE_LEVEL()>MTO_DataRateAvailableLevel - 3))
-	{
-		MTO_TOGGLE_STATE() = TOGGLE_STATE_IDLE;
-		#ifdef _PE_DTO_DUMP_
-		WBDEBUG(("[HHDTO]:===the data rate is %d (good)and will not toogle  ===",MTO_DATA_RATE()>>1));
-		#endif
-	}
-
-
-}
-
-int WB_PCR[2]; //packet correct rate
-
-void AntennaToggleState(MTO_FUNC_INPUT)
-{
-    int decideantflag = 0;
-	u8      x;
-	s32     rssi;
-
-	if(MTO_ANT_DIVERSITY_ENABLE() != 1)
-		return;
-	x = hal_get_antenna_number(MTO_HAL());
-	switch(MTO_TOGGLE_STATE())
-	{
-
-	   //Missing.....
-	   case TOGGLE_STATE_IDLE:
-	 case TOGGLE_STATE_BKOFF:
-	             break;;
-
-		case TOGGLE_STATE_WAIT0://========
-	               GetDTO_RxInfo(x, MTO_FUNC_INPUT_DATA);
-			sme_get_rssi(MTO_FUNC_INPUT_DATA, &rssi);
-			RXRSSIANT[x] = rssi;
-			#ifdef _PE_DTO_DUMP_
-			WBDEBUG(("[HHDTO] **wait0==== Collecting Ant%d--rssi=%d\n", x,RXRSSIANT[x]));
-			#endif
-
-			//change antenna and reset the data at changed antenna
-			x = (~x) & 0x01;
-			MTO_ANT_SEL() = x;
-			hal_set_antenna_number(MTO_HAL(), MTO_ANT_SEL());
-			LOCAL_ANTENNA_NO() = x;
-
-			MTO_TOGGLE_STATE() = TOGGLE_STATE_WAIT1;//go to wait1
-			ResetDTO_RxInfo(x, MTO_FUNC_INPUT_DATA);
-			break;
-		case TOGGLE_STATE_WAIT1://=====wait1
-			//MTO_CNT_ANT(x) = hal_get_bss_pk_cnt(MTO_HAL());
-			//RXRSSIANT[x] = hal_get_rssi(MTO_HAL());
-			sme_get_rssi(MTO_FUNC_INPUT_DATA, &rssi);
-			RXRSSIANT[x] = rssi;
-			GetDTO_RxInfo(x, MTO_FUNC_INPUT_DATA);
-			#ifdef _PE_DTO_DUMP_
-			WBDEBUG(("[HHDTO] **wait1==== Collecting Ant%d--rssi=%d\n", x,RXRSSIANT[x]));
-			#endif
-			MTO_TOGGLE_STATE() = TOGGLE_STATE_MAKEDESISION;
-			break;
-		case TOGGLE_STATE_MAKEDESISION:
-			#ifdef _PE_DTO_DUMP_
-			WBDEBUG(("[HHDTO]:Ant--0-----------------1---\n"));
-			OutputDebugInfo(ANT0,ANT1);
-			#endif
-			//PDEBUG(("[HHDTO] **decision====\n "));
-
-			//=====following is the decision produrce
-			//
-			//    first: compare the rssi if difference >10
-			//           select the larger one
-			//           ,others go to second
-			//    second: comapre the tx+rx packet count if difference >100
-			//            use larger total packets antenna
-			//    third::compare the tx PER if packets>20
-			//                           if difference >5% using the bigger one
-			//
-			//    fourth:compare the RX PER if packets>20
-			//                    if PER difference <5%
-			//                   using old antenna
-			//
-			//
-			if (abs(RXRSSIANT[ANT0]-RXRSSIANT[ANT1]) > MTOPARA_RSSI_TH_FOR_ANTDIV())//====rssi_th
-			{
-				if (RXRSSIANT[ANT0]>RXRSSIANT[ANT1])
-				{
-					decideantflag=1;
-					MTO_ANT_MAC() = ANT0;
-				}
-				else
-				{
-					decideantflag=1;
-					MTO_ANT_MAC() = ANT1;
-				}
-				#ifdef _PE_DTO_DUMP_
-				WBDEBUG(("Select antenna by RSSI\n"));
-				#endif
-			}
-			else if  (abs(TxPktPerAnt[ANT0] + DTO_Rx_Info[0][ANT0]-TxPktPerAnt[ANT1]-DTO_Rx_Info[0][ANT1])<50)//=====total packet_th
-			{
-				#ifdef _PE_DTO_DUMP_
-				WBDEBUG(("Total tx/rx is close\n"));
-				#endif
-				if (TxDominate(ANT0) && TxDominate(ANT1))
-				{
-					if ((TxPktPerAnt[ANT0]>10) && (TxPktPerAnt[ANT1]>10))//====tx packet_th
-					{
-						WB_PCR[ANT0]=Divide(TxPktPerAnt[ANT0]*100,TxPktRetryPerAnt[ANT0]);
-						WB_PCR[ANT1]=Divide(TxPktPerAnt[ANT1]*100,TxPktRetryPerAnt[ANT1]);
-						if (abs(WB_PCR[ANT0]-WB_PCR[ANT1])>5)// tx PER_th
-						{
-							#ifdef _PE_DTO_DUMP_
-							WBDEBUG(("Decide by Tx correct rate\n"));
-							#endif
-							if (WB_PCR[ANT0]>WB_PCR[ANT1])
-							{
-								decideantflag=1;
-								MTO_ANT_MAC() = ANT0;
-							}
-							else
-							{
-								decideantflag=1;
-								MTO_ANT_MAC() = ANT1;
-							}
-						}
-						else
-						{
-							decideantflag=0;
-							untogglecount++;
-							MTO_ANT_MAC() = old_antenna[0];
-						}
-					}
-					else
-					{
-						decideantflag=0;
-						MTO_ANT_MAC() = old_antenna[0];
-					}
-				}
-				else if ((DTO_Rx_Info[0][ANT0]>10)&&(DTO_Rx_Info[0][ANT1]>10))//rx packet th
-				{
-					#ifdef _PE_DTO_DUMP_
-					WBDEBUG(("Decide by Rx\n"));
-					#endif
-					if (abs(DTO_Rx_Info[0][ANT0] - DTO_Rx_Info[0][ANT1])>50)
-					{
-						if (DTO_Rx_Info[0][ANT0] > DTO_Rx_Info[0][ANT1])
-						{
-							decideantflag=1;
-							MTO_ANT_MAC() = ANT0;
-						}
-						else
-						{
-							decideantflag=1;
-							MTO_ANT_MAC() = ANT1;
-						}
-					}
-					else
-					{
-						decideantflag=0;
-						untogglecount++;
-						MTO_ANT_MAC() = old_antenna[0];
-					}
-				}
-				else
-				{
-					decideantflag=0;
-					MTO_ANT_MAC() = old_antenna[0];
-				}
-			}
-			else if ((TxPktPerAnt[ANT0]+DTO_Rx_Info[0][ANT0])>(TxPktPerAnt[ANT1]+DTO_Rx_Info[0][ANT1]))//use more packekts
-			{
-				#ifdef _PE_DTO_DUMP_
-				WBDEBUG(("decide by total tx/rx : ANT 0\n"));
-				#endif
-
-				decideantflag=1;
-				MTO_ANT_MAC() = ANT0;
-			}
-			else
-			{
-				#ifdef _PE_DTO_DUMP_
-				WBDEBUG(("decide by total tx/rx : ANT 1\n"));
-				#endif
-				decideantflag=1;
-				MTO_ANT_MAC() = ANT1;
-
-			}
-			//this is force ant toggle
-			if (decideantflag==1)
-				untogglecount=0;
-
-			untogglecount=untogglecount%4;
-			if (untogglecount==3) //change antenna
-				MTO_ANT_MAC() = ((~old_antenna[0]) & 0x1);
-			#ifdef _PE_DTO_DUMP_
-			WBDEBUG(("[HHDTO]:==================untoggle-count=%d",untogglecount));
-			#endif
-
-
-
-
-			//PDEBUG(("[HHDTO] **********************************DTO ENABLE=%d",MTO_ANT_DIVERSITY_ENABLE()));
-			if(MTO_ANT_DIVERSITY_ENABLE() == 1)
-			{
-					MTO_ANT_SEL() = MTO_ANT_MAC();
-					hal_set_antenna_number(MTO_HAL(), MTO_ANT_SEL());
-					LOCAL_ANTENNA_NO() = MTO_ANT_SEL();
-					#ifdef _PE_DTO_DUMP_
-					WBDEBUG(("[HHDTO] ==decision==*******antflag=%d******************selected antenna=%d\n",decideantflag,MTO_ANT_SEL()));
-					#endif
-			}
-			if (decideantflag)
-			{
-				old_antenna[3]=old_antenna[2];//store antenna info
-				old_antenna[2]=old_antenna[1];
-				old_antenna[1]=old_antenna[0];
-				old_antenna[0]= MTO_ANT_MAC();
-			}
-			#ifdef _PE_DTO_DUMP_
-			WBDEBUG(("[HHDTO]:**old antenna=[%d][%d][%d][%d]\n",old_antenna[0],old_antenna[1],old_antenna[2],old_antenna[3]));
-			#endif
-			if (old_antenna[0]!=old_antenna[1])
-				AntennaToggleBkoffTimer=0;
-			else if (old_antenna[1]!=old_antenna[2])
-				AntennaToggleBkoffTimer=1;
-			else if (old_antenna[2]!=old_antenna[3])
-				AntennaToggleBkoffTimer=2;
-			else
-				AntennaToggleBkoffTimer=4;
-
-			#ifdef _PE_DTO_DUMP_
-			WBDEBUG(("[HHDTO]:**back off timer=%d",AntennaToggleBkoffTimer));
-			#endif
-
-			ResetDTO_RxInfo(MTO_ANT_MAC(), MTO_FUNC_INPUT_DATA);
-			if (AntennaToggleBkoffTimer==0 && decideantflag)
-				MTO_TOGGLE_STATE() = TOGGLE_STATE_WAIT0;
-			else
-				MTO_TOGGLE_STATE() = TOGGLE_STATE_IDLE;
-			break;
-	}
-
-}
-
-void multiagc(MTO_FUNC_INPUT, u8 high_gain_mode )
-{
-	s32		rssi;
-	hw_data_t	*pHwData = MTO_HAL();
-
-	sme_get_rssi(MTO_FUNC_INPUT_DATA, &rssi);
-
-	if( (RF_WB_242 == pHwData->phy_type) ||
-		(RF_WB_242_1 == pHwData->phy_type) ) // 20060619.5 Add
-	{
-		if (high_gain_mode==1)
-		{
-			//hw_set_dxx_reg(phw_data, 0x0C, 0xf8f52230);
-			//hw_set_dxx_reg(phw_data, 0x20, 0x06C43440);
-			Wb35Reg_Write( pHwData, 0x100C, 0xF2F32232 ); // 940916 0xf8f52230 );
-			Wb35Reg_Write( pHwData, 0x1020, 0x04cb3440 ); // 940915 0x06C43440
-		}
-		else if (high_gain_mode==0)
-		{
-			//hw_set_dxx_reg(phw_data, 0x0C, 0xEEEE000D);
-			//hw_set_dxx_reg(phw_data, 0x20, 0x06c41440);
-			Wb35Reg_Write( pHwData, 0x100C, 0xEEEE000D );
-			Wb35Reg_Write( pHwData, 0x1020, 0x04cb1440 ); // 940915 0x06c41440
-		}
-		#ifdef _PE_DTO_DUMP_
-		WBDEBUG(("[HHDTOAGC] **rssi=%d, high gain mode=%d", rssi, high_gain_mode));
-		#endif
-	}
-}
-
-void TxPwrControl(MTO_FUNC_INPUT)
-{
-    s32     rssi;
-	hw_data_t	*pHwData = MTO_HAL();
-
-	sme_get_rssi(MTO_FUNC_INPUT_DATA, &rssi);
-	if( (RF_WB_242 == pHwData->phy_type) ||
-		(RF_WB_242_1 == pHwData->phy_type) ) // 20060619.5 Add
-	{
-		static u8 high_gain_mode; //this is for winbond RF switch LNA
-		                          //using different register setting
-
-		if (high_gain_mode==1)
-		{
-			if( rssi > MTO_DATA().RSSI_high )
-			{
-				//hw_set_dxx_reg(phw_data, 0x0C, 0xf8f52230);
-				//hw_set_dxx_reg(phw_data, 0x20, 0x05541640);
-				high_gain_mode=0;
-			}
-			else
-			{
-				//hw_set_dxx_reg(phw_data, 0x0C, 0xf8f51830);
-				//hw_set_dxx_reg(phw_data, 0x20, 0x05543E40);
-				high_gain_mode=1;
-			}
-		}
-		else //if (high_gain_mode==0)
-		{
-			if( rssi < MTO_DATA().RSSI_low )
-			{
-				//hw_set_dxx_reg(phw_data, 0x0C, 0xf8f51830);
-				//hw_set_dxx_reg(phw_data, 0x20, 0x05543E40);
-				high_gain_mode=1;
-			}
-			else
-			{
-				//hw_set_dxx_reg(phw_data, 0x0C, 0xf8f52230);
-				//hw_set_dxx_reg(phw_data, 0x20, 0x05541640);
-				high_gain_mode=0;
-			}
-		}
-
-		// Always high gain 20051014. Using the initial value only.
-		multiagc(MTO_FUNC_INPUT_DATA, high_gain_mode);
-	}
-}
-
-
-u8 CalcNewRate(MTO_FUNC_INPUT, u8 old_rate, u32 retry_cnt, u32 tx_frag_cnt)
-{
-	int i;
-	u8 new_rate;
-    u32 retry_rate;
-	int TxThrouput1, TxThrouput2, TxThrouput3, BestThroupht;
-
-	if(tx_frag_cnt < MTOPARA_TXCOUNT_TH_FOR_CALC_RATE()) //too few packets transmit
-	{
-		return 0xff;
-	}
-	retry_rate = Divide(retry_cnt * 100, tx_frag_cnt);
-
-	if(retry_rate > 90) retry_rate = 90; //always truncate to 90% due to lookup table size
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("##### Current level =%d, Retry count =%d, Frag count =%d\n",
-						old_rate, retry_cnt, tx_frag_cnt));
-	WBDEBUG(("*##* Retry rate =%d, throughput =%d\n",
-						retry_rate, Rate_PER_TBL[retry_rate][old_rate]));
-	WBDEBUG(("TxRateRec.tx_rate =%d, Retry rate = %d, throughput = %d\n",
-						TxRateRec.tx_rate, TxRateRec.tx_retry_rate,
-						Rate_PER_TBL[TxRateRec.tx_retry_rate][Level2PerTbl[TxRateRec.tx_rate]]));
-	WBDEBUG(("old_rate-1 =%d, Retry rate = %d, throughput = %d\n",
-						old_rate-1, retryrate_rec[old_rate-1],
-						Rate_PER_TBL[retryrate_rec[old_rate-1]][old_rate-1]));
-	WBDEBUG(("old_rate+1 =%d, Retry rate = %d, throughput = %d\n",
-						old_rate+1, retryrate_rec[old_rate+1],
-						Rate_PER_TBL[retryrate_rec[old_rate+1]][old_rate+1]));
-	#endif
-
-	//following is for record the retry rate at the different data rate
-	if (abs(retry_rate-retryrate_rec[old_rate])<50)//---the per TH
-		retryrate_rec[old_rate] = retry_rate; //update retry rate
-	else
-	{
-		for (i=0;i<MTO_DataRateAvailableLevel;i++) //reset all retry rate
-			retryrate_rec[i]=0;
-		retryrate_rec[old_rate] = retry_rate;
-		#ifdef _PE_DTO_DUMP_
-		WBDEBUG(("Reset retry rate table\n"));
-		#endif
-	}
-
-	if(TxRateRec.tx_rate > old_rate)   //Decrease Tx Rate
-	{
-		TxThrouput1 = Rate_PER_TBL[TxRateRec.tx_retry_rate][Level2PerTbl[TxRateRec.tx_rate]];
-		TxThrouput2 = Rate_PER_TBL[retry_rate][Level2PerTbl[old_rate]];
-		if(TxThrouput1 > TxThrouput2)
-		{
-			new_rate = TxRateRec.tx_rate;
-			BestThroupht = TxThrouput1;
-		}
-		else
-		{
-			new_rate = old_rate;
-			BestThroupht = TxThrouput2;
-		}
-		if((old_rate > 0) &&(retry_rate>MTOPARA_TXRATE_DEC_TH()))   //Min Rate
-		{
-			TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate-1]][Level2PerTbl[old_rate-1]];
-			if(BestThroupht < TxThrouput3)
-			{
-				new_rate = old_rate - 1;
-				#ifdef _PE_DTO_DUMP_
-				WBDEBUG(("--------\n"));
-				#endif
-				BestThroupht = TxThrouput3;
-			}
-		}
-	}
-	else if(TxRateRec.tx_rate < old_rate)  //Increase Tx Rate
-	{
-		TxThrouput1 = Rate_PER_TBL[TxRateRec.tx_retry_rate][Level2PerTbl[TxRateRec.tx_rate]];
-		TxThrouput2 = Rate_PER_TBL[retry_rate][Level2PerTbl[old_rate]];
-		if(TxThrouput1 > TxThrouput2)
-		{
-			new_rate = TxRateRec.tx_rate;
-			BestThroupht = TxThrouput1;
-		}
-		else
-		{
-			new_rate = old_rate;
-			BestThroupht = TxThrouput2;
-		}
-		if ((old_rate < MTO_DataRateAvailableLevel - 1)&&(retry_rate<MTOPARA_TXRATE_INC_TH()))
-		{
-			//TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate+1]][Level2PerTbl[old_rate+1]];
-			if (retryrate_rec[old_rate+1] > MTOPARA_TXRETRYRATE_REDUCE())
-				TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate+1]-MTOPARA_TXRETRYRATE_REDUCE()][Level2PerTbl[old_rate+1]];
-			else
-				TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate+1]][Level2PerTbl[old_rate+1]];
-			if(BestThroupht < TxThrouput3)
-			{
-				new_rate = old_rate + 1;
-				#ifdef _PE_DTO_DUMP_
-				WBDEBUG(("++++++++++\n"));
-				#endif
-				BestThroupht = TxThrouput3;
-			}
-		}
-	}
-	else  //Tx Rate no change
-	{
-		TxThrouput2 = Rate_PER_TBL[retry_rate][Level2PerTbl[old_rate]];
-		new_rate = old_rate;
-		BestThroupht = TxThrouput2;
-
-		if (retry_rate <MTOPARA_TXRATE_EQ_TH())    //th for change higher rate
-		{
-			if(old_rate < MTO_DataRateAvailableLevel - 1)
-			{
-				//TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate+1]][Level2PerTbl[old_rate+1]];
-				if (retryrate_rec[old_rate+1] > MTOPARA_TXRETRYRATE_REDUCE())
-					TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate+1]-MTOPARA_TXRETRYRATE_REDUCE()][Level2PerTbl[old_rate+1]];
-				else
-					TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate+1]][Level2PerTbl[old_rate+1]];
-				if(BestThroupht < TxThrouput3)
-				{
-					new_rate = old_rate + 1;
-					BestThroupht = TxThrouput3;
-					#ifdef _PE_DTO_DUMP_
-					WBDEBUG(("=++++++++++\n"));
-					#endif
-				}
-			}
-		}
-		else
-		if(old_rate > 0)   //Min Rate
-		{
-			TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate-1]][Level2PerTbl[old_rate-1]];
-			if(BestThroupht < TxThrouput3)
-			{
-				new_rate = old_rate - 1;
-				#ifdef _PE_DTO_DUMP_
-				WBDEBUG(("=--------\n"));
-				#endif
-				BestThroupht = TxThrouput3;
-			}
-		}
-	}
-
-	if (!LOCAL_IS_IBSS_MODE())
-	{
-	max_rssi_rate = GetMaxRateLevelFromRSSI();
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("[MTO]:RSSI2Rate=%d\n", MTO_Data_Rate_Tbl[max_rssi_rate]));
-	#endif
-	if(new_rate > max_rssi_rate)
-		new_rate = max_rssi_rate;
-	}
-
-	//save new rate;
-	TxRateRec.tx_rate = old_rate;
-	TxRateRec.tx_retry_rate = (u8) retry_rate;
-	TxRetryRate = retry_rate;
-	return new_rate;
-}
-
-void SmoothRSSI(s32 new_rssi)
-{
-	RSSISmoothed = RSSISmoothed + new_rssi - RSSIBuf[RSSIBufIndex];
-	RSSIBuf[RSSIBufIndex] = new_rssi;
-	RSSIBufIndex = (RSSIBufIndex + 1) % 10;
-}
-
-u8 GetMaxRateLevelFromRSSI(void)
-{
-	u8 i;
-	u8 TxRate;
-
-	for(i=0;i<RSSI2RATE_SIZE;i++)
-	{
-		if(RSSISmoothed > RSSI2RateTbl[i].RSSI)
-			break;
-	}
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("[MTO]:RSSI=%d\n", Divide(RSSISmoothed, 10)));
-	#endif
-	if(i < RSSI2RATE_SIZE)
-		TxRate = RSSI2RateTbl[i].TxRate;
-	else
-		TxRate = 2;  //divided by 2 = 1Mbps
-
-	for(i=MTO_DataRateAvailableLevel-1;i>0;i--)
-	{
-		if(TxRate >=MTO_Data_Rate_Tbl[i])
-			break;
-	}
-	return i;
-}
-
 //===========================================================================
 //  Description:
 //      If we enable DTO, we will ignore the tx count with different tx rate from
@@ -1194,36 +252,3 @@ void MTO_SetTxCount(MTO_FUNC_INPUT, u8 tx_rate, u8 index)
 	PeriodTotalTxPkt ++;
 	PeriodTotalTxPktRetry += (index+1);
 }
-
-u8 MTO_GetTxFallbackRate(MTO_FUNC_INPUT)
-{
-	return MTO_DATA_FALLBACK_RATE();
-}
-
-
-//===========================================================================
-//  MTO_TxFailed --
-//
-//  Description:
-//    Failure of transmitting a packet indicates that certain MTO parmeters
-//    may need to be adjusted. This function is called when NIC just failed
-//    to transmit a packet or when MSDULifeTime expired.
-//
-//  Arguments:
-//    Adapter      - The pointer to the Miniport Adapter Context
-//
-//  Return Value:
-//    None
-//============================================================================
-void MTO_TxFailed(MTO_FUNC_INPUT)
-{
-	return;
-}
-
-int Divide(int a, int b)
-{
-	if (b==0) b=1;
-	return a/b;
-}
-
-
diff --git a/drivers/staging/winbond/mto.h b/drivers/staging/winbond/mto.h
index f47936f46d1e..0d3775eb2573 100644
--- a/drivers/staging/winbond/mto.h
+++ b/drivers/staging/winbond/mto.h
@@ -11,6 +11,8 @@
 #ifndef __MTO_H__
 #define __MTO_H__
 
+#include <linux/types.h>
+
 #define MTO_DEFAULT_TH_CNT              5
 #define MTO_DEFAULT_TH_SQ3              112  //OLD IS 13 reference JohnXu
 #define MTO_DEFAULT_TH_IDLE_SLOT        15
@@ -129,17 +131,17 @@ typedef struct _MTO_PARAMETERS
 } MTO_PARAMETERS, *PMTO_PARAMETERS;
 
 
-#define MTO_FUNC_INPUT              PWB32_ADAPTER	Adapter
-#define MTO_FUNC_INPUT_DATA         Adapter
-#define MTO_DATA()                  (Adapter->sMtoPara)
-#define MTO_HAL()                   (&Adapter->sHwData)
+#define MTO_FUNC_INPUT              struct wbsoft_priv *	adapter
+#define MTO_FUNC_INPUT_DATA         adapter
+#define MTO_DATA()                  (adapter->sMtoPara)
+#define MTO_HAL()                   (&adapter->sHwData)
 #define MTO_SET_PREAMBLE_TYPE(x)    // 20040511 Turbo mark LM_PREAMBLE_TYPE(&pcore_data->lm_data) = (x)
-#define MTO_ENABLE					(Adapter->sLocalPara.TxRateMode == RATE_AUTO)
-#define MTO_TXPOWER_FROM_EEPROM		(Adapter->sHwData.PowerIndexFromEEPROM)
-#define LOCAL_ANTENNA_NO()			(Adapter->sLocalPara.bAntennaNo)
-#define LOCAL_IS_CONNECTED()		(Adapter->sLocalPara.wConnectedSTAindex != 0)
-#define LOCAL_IS_IBSS_MODE()		(Adapter->asBSSDescriptElement[Adapter->sLocalPara.wConnectedSTAindex].bBssType == IBSS_NET)
-#define MTO_INITTXRATE_MODE			(Adapter->sHwData.SoftwareSet&0x2)	//bit 1
+#define MTO_ENABLE					(adapter->sLocalPara.TxRateMode == RATE_AUTO)
+#define MTO_TXPOWER_FROM_EEPROM		(adapter->sHwData.PowerIndexFromEEPROM)
+#define LOCAL_ANTENNA_NO()			(adapter->sLocalPara.bAntennaNo)
+#define LOCAL_IS_CONNECTED()		(adapter->sLocalPara.wConnectedSTAindex != 0)
+#define LOCAL_IS_IBSS_MODE()		(adapter->asBSSDescriptElement[adapter->sLocalPara.wConnectedSTAindex].bBssType == IBSS_NET)
+#define MTO_INITTXRATE_MODE			(adapter->sHwData.SoftwareSet&0x2)	//bit 1
 // 20040510 Turbo add
 #define MTO_TMR_CNT()               MTO_DATA().TmrCnt
 #define MTO_TOGGLE_STATE()          MTO_DATA().ToggleState
@@ -157,7 +159,7 @@ typedef struct _MTO_PARAMETERS
 #define MTO_TMR_PERIODIC()          MTO_DATA().Tmr_Periodic
 
 #define MTO_POWER_CHANGE_ENABLE()   MTO_DATA().PowerChangeEnable
-#define MTO_ANT_DIVERSITY_ENABLE()  Adapter->sLocalPara.boAntennaDiversity
+#define MTO_ANT_DIVERSITY_ENABLE()  adapter->sLocalPara.boAntennaDiversity
 #define MTO_ANT_MAC()               MTO_DATA().Ant_mac
 #define MTO_ANT_DIVERSITY()         MTO_DATA().Ant_div
 #define MTO_CCA_MODE()              MTO_DATA().CCA_Mode
@@ -166,7 +168,6 @@ typedef struct _MTO_PARAMETERS
 #define MTO_PREAMBLE_CHANGE_ENABLE()         MTO_DATA().PreambleChangeEnable
 
 #define MTO_RATE_LEVEL()            MTO_DATA().DataRateLevel
-#define MTO_FALLBACK_RATE_LEVEL()	MTO_DATA().FallbackRateLevel
 #define MTO_OFDM_RATE_LEVEL()		MTO_DATA().OfdmRateLevel
 #define MTO_RATE_CHANGE_ENABLE()    MTO_DATA().DataRateChangeEnable
 #define MTO_FRAG_TH_LEVEL()         MTO_DATA().FragThresholdLevel
@@ -199,11 +200,9 @@ typedef struct _MTO_PARAMETERS
 //------------------------------------------------
 
 
-extern u8   MTO_Data_Rate_Tbl[];
 extern u16  MTO_Frag_Th_Tbl[];
 
 #define MTO_DATA_RATE()          MTO_Data_Rate_Tbl[MTO_RATE_LEVEL()]
-#define MTO_DATA_FALLBACK_RATE() MTO_Data_Rate_Tbl[MTO_FALLBACK_RATE_LEVEL()]	//next level
 #define MTO_FRAG_TH()            MTO_Frag_Th_Tbl[MTO_FRAG_TH_LEVEL()]
 
 typedef struct {
diff --git a/drivers/staging/winbond/mto_f.h b/drivers/staging/winbond/mto_f.h
index 30b3df2ccb3c..81f59137c6d6 100644
--- a/drivers/staging/winbond/mto_f.h
+++ b/drivers/staging/winbond/mto_f.h
@@ -1,7 +1,13 @@
-extern void MTO_Init(PWB32_ADAPTER);
-extern void MTO_PeriodicTimerExpired(PWB32_ADAPTER);
-extern void MTO_SetDTORateRange(PWB32_ADAPTER, u8 *, u8);
+#ifndef __WINBOND_MTO_F_H
+#define __WINBOND_MTO_F_H
+
+#include "core.h"
+
+extern void MTO_Init(struct wbsoft_priv *);
+extern void MTO_PeriodicTimerExpired(struct wbsoft_priv *);
+extern void MTO_SetDTORateRange(struct wbsoft_priv *, u8 *, u8);
 extern u8 MTO_GetTxRate(MTO_FUNC_INPUT, u32 fpdu_len);
 extern u8 MTO_GetTxFallbackRate(MTO_FUNC_INPUT);
 extern void MTO_SetTxCount(MTO_FUNC_INPUT, u8 t0, u8 index);
 
+#endif
diff --git a/drivers/staging/winbond/os_common.h b/drivers/staging/winbond/os_common.h
index e24ff41e871e..2c276e3ab512 100644
--- a/drivers/staging/winbond/os_common.h
+++ b/drivers/staging/winbond/os_common.h
@@ -1,2 +1,2 @@
-#include "linux/sysdef.h"
+#include "sysdef.h"
 
diff --git a/drivers/staging/winbond/phy_calibration.c b/drivers/staging/winbond/phy_calibration.c
index 272a65066aba..6782552d366c 100644
--- a/drivers/staging/winbond/phy_calibration.c
+++ b/drivers/staging/winbond/phy_calibration.c
@@ -12,6 +12,7 @@
 /****************** INCLUDE FILES SECTION ***********************************/
 #include "os_common.h"
 #include "phy_calibration.h"
+#include "wbhal_f.h"
 
 
 /****************** DEBUG CONSTANT AND MACRO SECTION ************************/
@@ -431,7 +432,6 @@ void _rxadc_dc_offset_cancellation_winbond(hw_data_t *phw_data, u32 frequency)
 
 	val |= MASK_ADC_DC_CAL_STR;
 	hw_set_dxx_reg(phw_data, REG_MODE_CTRL, val);
-	pa_stall_execution(US); // *MUST* wait for a while
 
 	// e. The result are shown in "adc_dc_cal_i[8:0] and adc_dc_cal_q[8:0]"
 #ifdef _DEBUG
@@ -522,7 +522,6 @@ void _txidac_dc_offset_cancellation_winbond(hw_data_t *phw_data)
 	reg_mode_ctrl |= (MASK_CALIB_START|2|(2<<2));
 	hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 	PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-	pa_stall_execution(US);
 
 	hw_get_dxx_reg(phw_data, 0x5C, &reg_dc_cancel);
 	PHY_DEBUG(("[CAL]    DC_CANCEL (read) = 0x%08X\n", reg_dc_cancel));
@@ -536,7 +535,6 @@ void _txidac_dc_offset_cancellation_winbond(hw_data_t *phw_data)
 		reg_dc_cancel &= ~(0x03FF);
 		PHY_DEBUG(("[CAL]    DC_CANCEL (write) = 0x%08X\n", reg_dc_cancel));
 		hw_set_dxx_reg(phw_data, 0x5C, reg_dc_cancel);
-		pa_stall_execution(US);
 
 		hw_get_dxx_reg(phw_data, REG_CALIB_READ2, &val);
 		PHY_DEBUG(("[CAL]    CALIB_READ2 = 0x%08X\n", val));
@@ -552,7 +550,6 @@ void _txidac_dc_offset_cancellation_winbond(hw_data_t *phw_data)
 		reg_dc_cancel |= (1 << CANCEL_DC_I_SHIFT);
 		PHY_DEBUG(("[CAL]    DC_CANCEL (write) = 0x%08X\n", reg_dc_cancel));
 		hw_set_dxx_reg(phw_data, 0x5C, reg_dc_cancel);
-		pa_stall_execution(US);
 
 		hw_get_dxx_reg(phw_data, REG_CALIB_READ2, &val);
 		PHY_DEBUG(("[CAL]    CALIB_READ2 = 0x%08X\n", val));
@@ -600,7 +597,6 @@ void _txidac_dc_offset_cancellation_winbond(hw_data_t *phw_data)
 	reg_mode_ctrl &= ~MASK_CALIB_START;
 	hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 	PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-	pa_stall_execution(US);
 }
 
 ///////////////////////////////////////////////////////
@@ -651,7 +647,6 @@ void _txqdac_dc_offset_cacellation_winbond(hw_data_t *phw_data)
 	reg_mode_ctrl |= (MASK_CALIB_START|3);
 	hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 	PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-	pa_stall_execution(US);
 
 	hw_get_dxx_reg(phw_data, 0x5C, &reg_dc_cancel);
 	PHY_DEBUG(("[CAL]    DC_CANCEL (read) = 0x%08X\n", reg_dc_cancel));
@@ -665,11 +660,9 @@ void _txqdac_dc_offset_cacellation_winbond(hw_data_t *phw_data)
 		reg_dc_cancel &= ~(0x001F);
 		PHY_DEBUG(("[CAL]    DC_CANCEL (write) = 0x%08X\n", reg_dc_cancel));
 		hw_set_dxx_reg(phw_data, 0x5C, reg_dc_cancel);
-		pa_stall_execution(US);
 
 		hw_get_dxx_reg(phw_data, REG_CALIB_READ2, &val);
 		PHY_DEBUG(("[CAL]    CALIB_READ2 = 0x%08X\n", val));
-		pa_stall_execution(US);
 
 		iqcal_image_i = _s13_to_s32(val & 0x00001FFF);
 		iqcal_image_q = _s13_to_s32((val & 0x03FFE000) >> 13);
@@ -682,11 +675,9 @@ void _txqdac_dc_offset_cacellation_winbond(hw_data_t *phw_data)
 		reg_dc_cancel |= (1 << CANCEL_DC_Q_SHIFT);
 		PHY_DEBUG(("[CAL]    DC_CANCEL (write) = 0x%08X\n", reg_dc_cancel));
 		hw_set_dxx_reg(phw_data, 0x5C, reg_dc_cancel);
-		pa_stall_execution(US);
 
 		hw_get_dxx_reg(phw_data, REG_CALIB_READ2, &val);
 		PHY_DEBUG(("[CAL]    CALIB_READ2 = 0x%08X\n", val));
-		pa_stall_execution(US);
 
 		iqcal_image_i = _s13_to_s32(val & 0x00001FFF);
 		iqcal_image_q = _s13_to_s32((val & 0x03FFE000) >> 13);
@@ -732,7 +723,6 @@ void _txqdac_dc_offset_cacellation_winbond(hw_data_t *phw_data)
 	reg_mode_ctrl &= ~MASK_CALIB_START;
 	hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 	PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-	pa_stall_execution(US);
 }
 
 //20060612.1.a 20060718.1 Modify
@@ -792,12 +782,10 @@ u8 _tx_iq_calibration_loop_winbond(hw_data_t *phw_data,
 			reg_mode_ctrl |= (MASK_CALIB_START|0x02|2<<2);
 			hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 			PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-			pa_stall_execution(US);
 
 			// b.
 			hw_get_dxx_reg(phw_data, REG_CALIB_READ1, &val);
 			PHY_DEBUG(("[CAL]    CALIB_READ1 = 0x%08X\n", val));
-			pa_stall_execution(US);
 
 			iqcal_tone_i0 = _s13_to_s32(val & 0x00001FFF);
 			iqcal_tone_q0 = _s13_to_s32((val & 0x03FFE000) >> 13);
@@ -813,7 +801,6 @@ u8 _tx_iq_calibration_loop_winbond(hw_data_t *phw_data,
 			reg_mode_ctrl &= ~MASK_CALIB_START;
 			hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 			PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-			pa_stall_execution(US);
 
 			// d. Set iqcal_mode[1:0] to 0x3 and set "calib_start" to 0x1 to
 			//    enable "IQ alibration Mode II"
@@ -823,12 +810,10 @@ u8 _tx_iq_calibration_loop_winbond(hw_data_t *phw_data,
 			reg_mode_ctrl |= (MASK_CALIB_START|0x03);
 			hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 			PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-			pa_stall_execution(US);
 
 			// e.
 			hw_get_dxx_reg(phw_data, REG_CALIB_READ1, &val);
 			PHY_DEBUG(("[CAL]    CALIB_READ1 = 0x%08X\n", val));
-			pa_stall_execution(US);
 
 			iqcal_tone_i = _s13_to_s32(val & 0x00001FFF);
 			iqcal_tone_q = _s13_to_s32((val & 0x03FFE000) >> 13);
@@ -1075,7 +1060,7 @@ void _tx_iq_calibration_winbond(hw_data_t *phw_data)
 	//; [BB-chip]: Calibration (6h). Caculate TX-path IQ imbalance and setting TX path IQ compensation table
 	//phy_set_rf_data(phw_data, 3, (3<<24)|0x025586);
 
-	OS_SLEEP(30000); // 20060612.1.a 30ms delay. Add the follow 2 lines
+	msleep(30); // 20060612.1.a 30ms delay. Add the follow 2 lines
 	//To adjust TXVGA to fit iq_mag_0 range from 1250 ~ 1750
 	adjust_TXVGA_for_iq_mag( phw_data );
 
@@ -1282,13 +1267,11 @@ u8 _rx_iq_calibration_loop_winbond(hw_data_t *phw_data, u16 factor, u32 frequenc
 		if( !hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl) )//20060718.1 modify
 			return 0;
 		PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-		pa_stall_execution(US);
 
 		reg_mode_ctrl &= ~MASK_IQCAL_MODE;
 		reg_mode_ctrl |= (MASK_CALIB_START|0x1);
 		hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 		PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-		pa_stall_execution(US);  //Should be read out after 450us
 
 		// c.
 		hw_get_dxx_reg(phw_data, REG_CALIB_READ1, &val);
@@ -1697,11 +1680,10 @@ unsigned char adjust_TXVGA_for_iq_mag(hw_data_t *phw_data)
 		phy_set_rf_data(phw_data, 5, ((5<<24)|current_txvga) );
 		phw_data->txvga_setting_for_cal = current_txvga;
 
-		//pa_stall_execution(30000);//Sleep(30);
-		OS_SLEEP(30000); // 20060612.1.a
+		msleep(30); // 20060612.1.a
 
 		if( !hw_get_dxx_reg(phw_data, REG_MODE_CTRL, &reg_mode_ctrl) ) // 20060718.1 modify
-			return FALSE;
+			return false;
 
 		PHY_DEBUG(("[CAL]    MODE_CTRL (read) = 0x%08X\n", reg_mode_ctrl));
 
@@ -1714,19 +1696,15 @@ unsigned char adjust_TXVGA_for_iq_mag(hw_data_t *phw_data)
 		hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 		PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
 
-		//pa_stall_execution(US);
-		OS_SLEEP(1); // 20060612.1.a
+		udelay(1); // 20060612.1.a
 
-		//pa_stall_execution(300);//Sleep(30);
-		OS_SLEEP(300); // 20060612.1.a
+		udelay(300); // 20060612.1.a
 
 		// b.
 		hw_get_dxx_reg(phw_data, REG_CALIB_READ1, &val);
 
 		PHY_DEBUG(("[CAL]    CALIB_READ1 = 0x%08X\n", val));
-		//pa_stall_execution(US);
-		//pa_stall_execution(300);//Sleep(30);
-		OS_SLEEP(300); // 20060612.1.a
+		udelay(300); // 20060612.1.a
 
 		iqcal_tone_i0 = _s13_to_s32(val & 0x00001FFF);
 		iqcal_tone_q0 = _s13_to_s32((val & 0x03FFE000) >> 13);
@@ -1750,9 +1728,9 @@ unsigned char adjust_TXVGA_for_iq_mag(hw_data_t *phw_data)
 	}
 
 	if( iq_mag_0_tx>=700 && iq_mag_0_tx<=1750 )
-		return TRUE;
+		return true;
 	else
-		return FALSE;
+		return false;
 }
 
 
diff --git a/drivers/staging/winbond/phy_calibration.h b/drivers/staging/winbond/phy_calibration.h
index b6a65d313019..03b820c6181b 100644
--- a/drivers/staging/winbond/phy_calibration.h
+++ b/drivers/staging/winbond/phy_calibration.h
@@ -1,3 +1,8 @@
+#ifndef __WINBOND_PHY_CALIBRATION_H
+#define __WINBOND_PHY_CALIBRATION_H
+
+#include "wbhal_f.h"
+
 // 20031229 Turbo add
 #define REG_AGC_CTRL1               0x1000
 #define REG_AGC_CTRL2               0x1004
@@ -99,3 +104,4 @@
 void phy_set_rf_data(  phw_data_t pHwData,  u32 index,  u32 value );
 #define phy_init_rf( _A )	//RFSynthesizer_initial( _A )
 
+#endif
diff --git a/drivers/staging/winbond/reg.c b/drivers/staging/winbond/reg.c
index 57af5b831509..cd21272d7a9f 100644
--- a/drivers/staging/winbond/reg.c
+++ b/drivers/staging/winbond/reg.c
@@ -1,4 +1,5 @@
 #include "os_common.h"
+#include "wbhal_f.h"
 
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 // Original Phy.h
@@ -976,9 +977,9 @@ void Uxx_power_on_procedure(  phw_data_t pHwData )
 
 		// 20060511.1 Fix the following 4 steps for Rx of RF 2230 initial fail
 		Wb35Reg_WriteSync( pHwData, 0x03d4, 0x80 );// regulator on only
-		OS_SLEEP(10000); // Modify 20051221.1.b
+		msleep(10); // Modify 20051221.1.b
 		Wb35Reg_WriteSync( pHwData, 0x03d4, 0xb8 );// REG_ON RF_RSTN on, and
-		OS_SLEEP(10000); // Modify 20051221.1.b
+		msleep(10); // Modify 20051221.1.b
 
 		ltmp = 0x4968;
 		if( (pHwData->phy_type == RF_WB_242) ||
@@ -988,12 +989,12 @@ void Uxx_power_on_procedure(  phw_data_t pHwData )
 
 		Wb35Reg_WriteSync( pHwData, 0x03d4, 0xa0 );// PLL_PD REF_PD set to 0
 
-		OS_SLEEP(20000); // Modify 20051221.1.b
+		msleep(20); // Modify 20051221.1.b
 		Wb35Reg_ReadSync( pHwData, 0x03d0, &ltmp );
 		loop = 500; // Wait for 5 second 20061101
 		while( !(ltmp & 0x20) && loop-- )
 		{
-			OS_SLEEP(10000); // Modify 20051221.1.b
+			msleep(10); // Modify 20051221.1.b
 			if( !Wb35Reg_ReadSync( pHwData, 0x03d0, &ltmp ) )
 				break;
 		}
@@ -1002,7 +1003,7 @@ void Uxx_power_on_procedure(  phw_data_t pHwData )
 	}
 
 	Wb35Reg_WriteSync( pHwData, 0x03b0, 1 );// Reset hardware first
-	OS_SLEEP(10000); // Add this 20051221.1.b
+	msleep(10); // Add this 20051221.1.b
 
 	// Set burst write delay
 	Wb35Reg_WriteSync( pHwData, 0x03f8, 0x7ff );
@@ -1167,23 +1168,23 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			// 20060511.1 --- Modifying the follow step for Rx issue-----------------
 			ltmp = (1 << 31) | (0 << 30) | (20 << 24) | BitReverse( (0x07<<20)|0xE168E, 20);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(10000);
+			msleep(10);
 			ltmp = (1 << 31) | (0 << 30) | (20 << 24) | BitReverse( al2230_rf_data[7], 20);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(10000);
+			msleep(10);
 
 		case RF_AIROHA_2230S: // 20060420 Add this
 
 			// 20060511.1 --- Modifying the follow step for Rx issue-----------------
 			Wb35Reg_WriteSync( pHwData, 0x03d4, 0x80 );// regulator on only
-			OS_SLEEP(10000); // Modify 20051221.1.b
+			msleep(10); // Modify 20051221.1.b
 
 			Wb35Reg_WriteSync( pHwData, 0x03d4, 0xa0 );// PLL_PD REF_PD set to 0
-			OS_SLEEP(10000); // Modify 20051221.1.b
+			msleep(10); // Modify 20051221.1.b
 
 			Wb35Reg_WriteSync( pHwData, 0x03d4, 0xe0 );// MLK_EN
 			Wb35Reg_WriteSync( pHwData, 0x03b0, 1 );// Reset hardware first
-			OS_SLEEP(10000); // Add this 20051221.1.b
+			msleep(10); // Add this 20051221.1.b
 			//------------------------------------------------------------------------
 
 			// The follow code doesn't use the burst-write mode
@@ -1191,30 +1192,30 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			ltmp = (1 << 31) | (0 << 30) | (20 << 24) | BitReverse( (0x0F<<20) | 0xF01A0, 20);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
 
-			ltmp = pHwData->Wb35Reg.BB5C & 0xfffff000;
+			ltmp = pHwData->reg.BB5C & 0xfffff000;
 			Wb35Reg_WriteSync( pHwData, 0x105c, ltmp );
-			pHwData->Wb35Reg.BB50 |= 0x13;//(MASK_IQCAL_MODE|MASK_CALIB_START);//20060315.1 modify
-        	Wb35Reg_WriteSync( pHwData, 0x1050, pHwData->Wb35Reg.BB50);
-			OS_SLEEP(5000);
+			pHwData->reg.BB50 |= 0x13;//(MASK_IQCAL_MODE|MASK_CALIB_START);//20060315.1 modify
+	        	Wb35Reg_WriteSync(pHwData, 0x1050, pHwData->reg.BB50);
+			msleep(5);
 
 			//phy_set_rf_data(phw_data, 0x0F, (0x0F<<20) | 0xF01B0); //Activate Filter Cal.
 			ltmp = (1 << 31) | (0 << 30) | (20 << 24) | BitReverse( (0x0F<<20) | 0xF01B0, 20);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 
 			//phy_set_rf_data(phw_data, 0x0F, (0x0F<<20) | 0xF01e0); //Activate TX DCC
 			ltmp = (1 << 31) | (0 << 30) | (20 << 24) | BitReverse( (0x0F<<20) | 0xF01E0, 20);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 
 			//phy_set_rf_data(phw_data, 0x0F, (0x0F<<20) | 0xF01A0); //Resotre Initial Setting
 			ltmp = (1 << 31) | (0 << 30) | (20 << 24) | BitReverse( (0x0F<<20) | 0xF01A0, 20);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
 
 //			//Force TXI(Q)P(N) to normal control
-			Wb35Reg_WriteSync( pHwData, 0x105c, pHwData->Wb35Reg.BB5C );
-			pHwData->Wb35Reg.BB50 &= ~0x13;//(MASK_IQCAL_MODE|MASK_CALIB_START);
-        	Wb35Reg_WriteSync( pHwData, 0x1050, pHwData->Wb35Reg.BB50);
+			Wb35Reg_WriteSync( pHwData, 0x105c, pHwData->reg.BB5C );
+			pHwData->reg.BB50 &= ~0x13;//(MASK_IQCAL_MODE|MASK_CALIB_START);
+        	Wb35Reg_WriteSync( pHwData, 0x1050, pHwData->reg.BB50);
 			break;
 
 		case RF_AIROHA_7230:
@@ -1229,16 +1230,16 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//2.4GHz
 			//ltmp = (1 << 31) | (0 << 30) | (24 << 24) | 0x1ABA8F;
 			//Wb35Reg_WriteSync pHwData, 0x0864, ltmp );
-			//OS_SLEEP(1000); // Sleep 1 ms
+			//msleep(1); // Sleep 1 ms
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | 0x9ABA8F;
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | 0x3ABA8F;
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | 0x1ABA8F;
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 
 			//5GHz
 			Wb35Reg_WriteSync( pHwData, 0x03dc, 0x00000000 );
@@ -1251,7 +1252,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			// Write to register. number must less and equal than 16
 			for( i=0; i<number; i++ )
 				Wb35Reg_WriteSync( pHwData, 0x0864, pltmp[i] );
-			OS_SLEEP(5000);
+			msleep(5);
 
 			Wb35Reg_WriteSync( pHwData, 0x03dc, 0x00000080 );
 			#ifdef _PE_STATE_DUMP_
@@ -1262,13 +1263,13 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | 0x9ABA8F;
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | 0x3ABA8F;
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | 0x12BACF;
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 
 			//Wb35Reg_WriteSync( pHwData, 0x03dc, 0x00000080 );
 			//WBDEBUG(("* PLL_ON    high\n"));
@@ -1280,21 +1281,21 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//
 			// ; Version 1.3B revision items: for FA5976A , October 3, 2005 by HTHo
 			//
-			ltmp = pHwData->Wb35Reg.BB5C & 0xfffff000;
+			ltmp = pHwData->reg.BB5C & 0xfffff000;
 			Wb35Reg_WriteSync( pHwData, 0x105c, ltmp );
 			Wb35Reg_WriteSync( pHwData, 0x1058, 0 );
-			pHwData->Wb35Reg.BB50 |= 0x3;//(MASK_IQCAL_MODE|MASK_CALIB_START);//20060630
-	      	Wb35Reg_WriteSync( pHwData, 0x1050, pHwData->Wb35Reg.BB50);
+			pHwData->reg.BB50 |= 0x3;//(MASK_IQCAL_MODE|MASK_CALIB_START);//20060630
+		      	Wb35Reg_WriteSync(pHwData, 0x1050, pHwData->reg.BB50);
 
 			//----- Calibration (1). VCO frequency calibration
 			//Calibration (1a.0). Synthesizer reset (HTHo corrected 2005/05/10)
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x0F<<24) | 0x00101E, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP( 5000 ); // Sleep 5ms
+			msleep(5); // Sleep 5ms
 			//Calibration (1a). VCO frequency calibration mode ; waiting 2msec VCO calibration time
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFE69c0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP( 2000 ); // Sleep 2ms
+			msleep(2); // Sleep 2ms
 
 			//----- Calibration (2). TX baseband Gm-C filter auto-tuning
 			//Calibration (2a). turn off ENCAL signal
@@ -1309,7 +1310,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Calibration (2c). turn-on TX Gm-C filter auto-tuning
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFCEBC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP( 150 ); // Sleep 150 us
+			udelay(150); // Sleep 150 us
 			//turn off ENCAL signal
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xF8EBC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
@@ -1327,7 +1328,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Calibration (3c). turn-on RX Gm-C filter auto-tuning
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFEEDC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP( 150 ); // Sleep 150 us
+			udelay(150); // Sleep 150 us
 			//Calibration (3e). turn off ENCAL signal
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFAEDC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
@@ -1336,7 +1337,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Calibration (4a). TX LO leakage calibration
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFD6BC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP( 150 ); // Sleep 150 us
+			udelay(150); // Sleep 150 us
 
 			//----- Calibration (5). RX DC offset calibration
 			//Calibration (5a). turn off ENCAL signal and set to RX SW DC caliration mode
@@ -1353,7 +1354,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Calibration (5d). turn on RX DC offset cal function; and waiting 2 msec cal time
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFF6DC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(2000); // Sleep 2ms
+			msleep(2); // Sleep 2ms
 			//Calibration (5f). turn off ENCAL signal
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFAEDC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
@@ -1365,7 +1366,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Calibration (5d). turn on RX DC offset cal function; and waiting 2 msec cal time
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFF6DC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(2000); // Sleep 2ms
+			msleep(2); // Sleep 2ms
 			//Calibration (5f). turn off ENCAL signal
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFAEDC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
@@ -1377,7 +1378,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Calibration (5d). turn on RX DC offset cal function; and waiting 2 msec cal time
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFF6DC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(2000); // Sleep 2ms
+			msleep(2); // Sleep 2ms
 			//Calibration (5f). turn off ENCAL signal
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFAEDC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
@@ -1389,7 +1390,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Calibration (5d). turn on RX DC offset cal function; and waiting 2 msec cal time
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFF6DC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(2000); // Sleep 2ms
+			msleep(2); // Sleep 2ms
 			//Calibration (5f). turn off ENCAL signal
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFAEDC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
@@ -1399,30 +1400,30 @@ RFSynthesizer_initial(phw_data_t pHwData)
 
 			//; ----- Calibration (7). Switch RF chip to normal mode
 			//0x00 0xF86100 ; 3E184   ; Switch RF chip to normal mode
-//			OS_SLEEP(10000); // @@ 20060721
+//			msleep(10); // @@ 20060721
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xF86100, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000); // Sleep 5 ms
+			msleep(5); // Sleep 5 ms
 
 //			//write back
-//			Wb35Reg_WriteSync( pHwData, 0x105c, pHwData->Wb35Reg.BB5C );
-//			pHwData->Wb35Reg.BB50 &= ~0x13;//(MASK_IQCAL_MODE|MASK_CALIB_START); // 20060315.1 fix
-//      	Wb35Reg_WriteSync( pHwData, 0x1050, pHwData->Wb35Reg.BB50);
-//			OS_SLEEP(1000); // Sleep 1 ms
+//			Wb35Reg_WriteSync(pHwData, 0x105c, pHwData->reg.BB5C);
+//			pHwData->reg.BB50 &= ~0x13;//(MASK_IQCAL_MODE|MASK_CALIB_START); // 20060315.1 fix
+//		      	Wb35Reg_WriteSync(pHwData, 0x1050, pHwData->reg.BB50);
+//			msleep(1); // Sleep 1 ms
 			break;
 	}
 }
 
 void BBProcessor_AL7230_2400(  phw_data_t pHwData)
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	u32	pltmp[12];
 
 	pltmp[0] = 0x16A8337A; // 0x16a5215f; // 0x1000 AGC_Ctrl1
 	pltmp[1] = 0x9AFF9AA6; // 0x9aff9ca6; // 0x1004 AGC_Ctrl2
 	pltmp[2] = 0x55D00A04; // 0x55d00a04; // 0x1008 AGC_Ctrl3
 	pltmp[3] = 0xFFF72031; // 0xFfFf2138; // 0x100c AGC_Ctrl4
-	pWb35Reg->BB0C = 0xFFF72031;
+	reg->BB0C = 0xFFF72031;
 	pltmp[4] = 0x0FacDCC5; // 0x1010 AGC_Ctrl5 // 20050927 0x0FacDCB7
 	pltmp[5] = 0x00CAA333; // 0x00eaa333; // 0x1014 AGC_Ctrl6
 	pltmp[6] = 0xF2211111; // 0x11111111; // 0x1018 AGC_Ctrl7
@@ -1431,25 +1432,25 @@ void BBProcessor_AL7230_2400(  phw_data_t pHwData)
 	pltmp[9] = 0xA8002A79; // 0xa9002A79; // 0x1024 AGC_Ctrl10
 	pltmp[10] = 0x40000528; // 20050927 0x40000228
 	pltmp[11] = 0x232D7F30; // 0x23457f30;// 0x102c A_ACQ_Ctrl
-	pWb35Reg->BB2C = 0x232D7F30;
+	reg->BB2C = 0x232D7F30;
 	Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 	pltmp[0] = 0x00002c54; // 0x1030 B_ACQ_Ctrl
-	pWb35Reg->BB30 = 0x00002c54;
+	reg->BB30 = 0x00002c54;
 	pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 	pltmp[2] = 0x5B2C8769; // 0x1038 B_TXRX_Ctrl
 	pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-	pWb35Reg->BB3C = 0x00000000;
+	reg->BB3C = 0x00000000;
 	pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 	pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 	pltmp[6] = 0x00332C1B; // 0x00453B24; // 0x1048 11b TX RC filter
 	pltmp[7] = 0x0A00FEFF; // 0x0E00FEFF; // 0x104c 11b TX RC filter
 	pltmp[8] = 0x2B106208; // 0x1050 MODE_Ctrl
-	pWb35Reg->BB50 = 0x2B106208;
+	reg->BB50 = 0x2B106208;
 	pltmp[9] = 0; // 0x1054
-	pWb35Reg->BB54 = 0x00000000;
+	reg->BB54 = 0x00000000;
 	pltmp[10] = 0x52524242; // 0x64645252; // 0x1058 IQ_Alpha
-	pWb35Reg->BB58 = 0x52524242;
+	reg->BB58 = 0x52524242;
 	pltmp[11] = 0xAA0AC000; // 0x105c DC_Cancel
 	Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1457,14 +1458,14 @@ void BBProcessor_AL7230_2400(  phw_data_t pHwData)
 
 void BBProcessor_AL7230_5000(  phw_data_t pHwData)
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	u32	pltmp[12];
 
 	pltmp[0] = 0x16AA6678; // 0x1000 AGC_Ctrl1
 	pltmp[1] = 0x9AFFA0B2; // 0x1004 AGC_Ctrl2
 	pltmp[2] = 0x55D00A04; // 0x1008 AGC_Ctrl3
 	pltmp[3] = 0xEFFF233E; // 0x100c AGC_Ctrl4
-	pWb35Reg->BB0C = 0xEFFF233E;
+	reg->BB0C = 0xEFFF233E;
 	pltmp[4] = 0x0FacDCC5; // 0x1010 AGC_Ctrl5 // 20050927 0x0FacDCB7
 	pltmp[5] = 0x00CAA333; // 0x1014 AGC_Ctrl6
 	pltmp[6] = 0xF2432111; // 0x1018 AGC_Ctrl7
@@ -1473,24 +1474,24 @@ void BBProcessor_AL7230_5000(  phw_data_t pHwData)
 	pltmp[9] = 0x00002A79; // 0x1024 AGC_Ctrl10
 	pltmp[10] = 0x40000528; // 20050927 0x40000228
 	pltmp[11] = 0x232FDF30;// 0x102c A_ACQ_Ctrl
-	pWb35Reg->BB2C = 0x232FDF30;
+	reg->BB2C = 0x232FDF30;
 	Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 	pltmp[0] = 0x80002C7C; // 0x1030 B_ACQ_Ctrl
 	pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 	pltmp[2] = 0x5B2C8769; // 0x1038 B_TXRX_Ctrl
 	pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-	pWb35Reg->BB3C = 0x00000000;
+	reg->BB3C = 0x00000000;
 	pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 	pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 	pltmp[6] = 0x00332C1B; // 0x1048 11b TX RC filter
 	pltmp[7] = 0x0A00FEFF; // 0x104c 11b TX RC filter
 	pltmp[8] = 0x2B107208; // 0x1050 MODE_Ctrl
-	pWb35Reg->BB50 = 0x2B107208;
+	reg->BB50 = 0x2B107208;
 	pltmp[9] = 0; // 0x1054
-	pWb35Reg->BB54 = 0x00000000;
+	reg->BB54 = 0x00000000;
 	pltmp[10] = 0x52524242; // 0x1058 IQ_Alpha
-	pWb35Reg->BB58 = 0x52524242;
+	reg->BB58 = 0x52524242;
 	pltmp[11] = 0xAA0AC000; // 0x105c DC_Cancel
 	Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1511,7 +1512,7 @@ void BBProcessor_AL7230_5000(  phw_data_t pHwData)
 void
 BBProcessor_initial(  phw_data_t pHwData )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	u32	i, pltmp[12];
 
     switch( pHwData->phy_type )
@@ -1522,7 +1523,7 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[1] = 0x9AFFAEA4; // 0x1004 AGC_Ctrl2
 			pltmp[2] = 0x55D00A04; // 0x1008 AGC_Ctrl3
 			pltmp[3] = 0xEFFF1A34; // 0x100c AGC_Ctrl4
-			pWb35Reg->BB0C = 0xEFFF1A34;
+			reg->BB0C = 0xEFFF1A34;
 			pltmp[4] = 0x0FABE0B7; // 0x1010 AGC_Ctrl5
 			pltmp[5] = 0x00CAA332; // 0x1014 AGC_Ctrl6
 			pltmp[6] = 0xF6632111; // 0x1018 AGC_Ctrl7
@@ -1531,25 +1532,25 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[9] = 0x00002A79; // 0x1024 AGC_Ctrl10
 			pltmp[10] = (pHwData->phy_type==3) ? 0x40000a28 : 0x40000228; // 0x1028 MAXIM_331(b31=0) + WBRF_V1(b11=1) : MAXIM_331(b31=0) + WBRF_V2(b11=0)
 			pltmp[11] = 0x232FDF30; // 0x102c A_ACQ_Ctrl
-			pWb35Reg->BB2C = 0x232FDF30; //Modify for 33's 1.0.95.xxx version, antenna 1
+			reg->BB2C = 0x232FDF30; //Modify for 33's 1.0.95.xxx version, antenna 1
 			Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 			pltmp[0] = 0x00002C54; // 0x1030 B_ACQ_Ctrl
-			pWb35Reg->BB30 = 0x00002C54;
+			reg->BB30 = 0x00002C54;
 			pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 			pltmp[2] = 0x5B6C8769; // 0x1038 B_TXRX_Ctrl
 			pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-			pWb35Reg->BB3C = 0x00000000;
+			reg->BB3C = 0x00000000;
 			pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 			pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 			pltmp[6] = 0x00453B24; // 0x1048 11b TX RC filter
 			pltmp[7] = 0x0E00FEFF; // 0x104c 11b TX RC filter
 			pltmp[8] = 0x27106208; // 0x1050 MODE_Ctrl
-			pWb35Reg->BB50 = 0x27106208;
+			reg->BB50 = 0x27106208;
 			pltmp[9] = 0; // 0x1054
-			pWb35Reg->BB54 = 0x00000000;
+			reg->BB54 = 0x00000000;
 			pltmp[10] = 0x64646464; // 0x1058 IQ_Alpha
-			pWb35Reg->BB58 = 0x64646464;
+			reg->BB58 = 0x64646464;
 			pltmp[11] = 0xAA0AC000; // 0x105c DC_Cancel
 			Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1568,7 +1569,7 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[1] = 0x9affaea4; // 0x1004 AGC_Ctrl2
 			pltmp[2] = 0x55d00a04; // 0x1008 AGC_Ctrl3
 			pltmp[3] = 0xefff1a34; // 0x100c AGC_Ctrl4
-			pWb35Reg->BB0C = 0xefff1a34;
+			reg->BB0C = 0xefff1a34;
 			pltmp[4] = 0x0fabe0b7; // 0x1010 AGC_Ctrl5
 			pltmp[5] = 0x00caa332; // 0x1014 AGC_Ctrl6
 			pltmp[6] = 0xf6632111; // 0x1018 AGC_Ctrl7
@@ -1577,25 +1578,25 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[9] = 0x00002A79; // 0x1024 AGC_Ctrl10
 			pltmp[10] = 0x40000528; // 0x40000128; Modify for 33's 1.0.95
 			pltmp[11] = 0x232fdf30; // 0x102c A_ACQ_Ctrl
-			pWb35Reg->BB2C = 0x232fdf30; //Modify for 33's 1.0.95.xxx version, antenna 1
+			reg->BB2C = 0x232fdf30; //Modify for 33's 1.0.95.xxx version, antenna 1
 			Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 			pltmp[0] = 0x00002C54; // 0x1030 B_ACQ_Ctrl
-			pWb35Reg->BB30 = 0x00002C54;
+			reg->BB30 = 0x00002C54;
 			pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 			pltmp[2] = 0x5B6C8769; // 0x1038 B_TXRX_Ctrl
 			pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-			pWb35Reg->BB3C = 0x00000000;
+			reg->BB3C = 0x00000000;
 			pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 			pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 			pltmp[6] = 0x00453B24; // 0x1048 11b TX RC filter
 			pltmp[7] = 0x0D00FDFF; // 0x104c 11b TX RC filter
 			pltmp[8] = 0x27106208; // 0x1050 MODE_Ctrl
-			pWb35Reg->BB50 = 0x27106208;
+			reg->BB50 = 0x27106208;
 			pltmp[9] = 0; // 0x1054
-			pWb35Reg->BB54 = 0x00000000;
+			reg->BB54 = 0x00000000;
 			pltmp[10] = 0x64646464; // 0x1058 IQ_Alpha
-			pWb35Reg->BB58 = 0x64646464;
+			reg->BB58 = 0x64646464;
 			pltmp[11] = 0xAA28C000; // 0x105c DC_Cancel
 			Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1608,7 +1609,7 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[1] = 0x9affaea4; // 0x1004 AGC_Ctrl2
 			pltmp[2] = 0x55d00a04; // 0x1008 AGC_Ctrl3
 			pltmp[3] = 0xf4ff1632; // 0xefff1a34; // 0x100c AGC_Ctrl4 Modify for 33's 1.0.95
-			pWb35Reg->BB0C = 0xf4ff1632; // 0xefff1a34; Modify for 33's 1.0.95
+			reg->BB0C = 0xf4ff1632; // 0xefff1a34; Modify for 33's 1.0.95
 			pltmp[4] = 0x0fabe0b7; // 0x1010 AGC_Ctrl5
 			pltmp[5] = 0x00caa332; // 0x1014 AGC_Ctrl6
 			pltmp[6] = 0xf8632112; // 0xf6632111; // 0x1018 AGC_Ctrl7 Modify for 33's 1.0.95
@@ -1617,25 +1618,25 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[9] = 0x00002A79; // 0x1024 AGC_Ctrl10
 			pltmp[10] = 0x40000528; // 0x40000128; modify for 33's 1.0.95
 			pltmp[11] = 0x232fdf30; // 0x102c A_ACQ_Ctrl
-			pWb35Reg->BB2C = 0x232fdf30; //Modify for 33's 1.0.95.xxx version, antenna 1
+			reg->BB2C = 0x232fdf30; //Modify for 33's 1.0.95.xxx version, antenna 1
 			Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 			pltmp[0] = 0x00002C54; // 0x1030 B_ACQ_Ctrl
-			pWb35Reg->BB30 = 0x00002C54;
+			reg->BB30 = 0x00002C54;
 			pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 			pltmp[2] = 0x5b2c8769; // 0x5B6C8769; // 0x1038 B_TXRX_Ctrl Modify for 33's 1.0.95
 			pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-			pWb35Reg->BB3C = 0x00000000;
+			reg->BB3C = 0x00000000;
 			pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 			pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 			pltmp[6] = 0x002c2617; // 0x00453B24; // 0x1048 11b TX RC filter Modify for 33's 1.0.95
 			pltmp[7] = 0x0800feff; // 0x0D00FDFF; // 0x104c 11b TX RC filter Modify for 33's 1.0.95
 			pltmp[8] = 0x27106208; // 0x1050 MODE_Ctrl
-			pWb35Reg->BB50 = 0x27106208;
+			reg->BB50 = 0x27106208;
 			pltmp[9] = 0; // 0x1054
-			pWb35Reg->BB54 = 0x00000000;
+			reg->BB54 = 0x00000000;
 			pltmp[10] = 0x64644a4a; // 0x64646464; // 0x1058 IQ_Alpha Modify for 33's 1.0.95
-			pWb35Reg->BB58 = 0x64646464;
+			reg->BB58 = 0x64646464;
 			pltmp[11] = 0xAA28C000; // 0x105c DC_Cancel
 			Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1648,7 +1649,7 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[1] = 0x9affafb2; // 0x1004 AGC_Ctrl2
 			pltmp[2] = 0x55d00a04; // 0x1008 AGC_Ctrl3
 			pltmp[3] = 0xFFFd203c; // 0xFFFb203a; // 0x100c AGC_Ctrl4 Modify for 33's 1.0.95.xxx version
-			pWb35Reg->BB0C = 0xFFFd203c;
+			reg->BB0C = 0xFFFd203c;
 			pltmp[4] = 0X0FBFDCc5; // 0X0FBFDCA0; // 0x1010 AGC_Ctrl5 //0x0FB2E0B7 Modify for 33's 1.0.95.xxx version
 			pltmp[5] = 0x00caa332; // 0x00caa333; // 0x1014 AGC_Ctrl6 Modify for 33's 1.0.95.xxx version
 			pltmp[6] = 0XF6632111; // 0XF1632112; // 0x1018 AGC_Ctrl7		//0xf6632112 Modify for 33's 1.0.95.xxx version
@@ -1657,27 +1658,27 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[9] = 0x00002A79; // 0x1024 AGC_Ctrl10
 			pltmp[10] = 0X40000528;							//0x40000228
 			pltmp[11] = 0x232dfF30; // 0x232A9F30; // 0x102c A_ACQ_Ctrl	//0x232a9730
-			pWb35Reg->BB2C = 0x232dfF30; //Modify for 33's 1.0.95.xxx version, antenna 1
+			reg->BB2C = 0x232dfF30; //Modify for 33's 1.0.95.xxx version, antenna 1
 			Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 			pltmp[0] = 0x00002C54; // 0x1030 B_ACQ_Ctrl
-			pWb35Reg->BB30 = 0x00002C54;
+			reg->BB30 = 0x00002C54;
 			pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 			pltmp[2] = 0x5B2C8769; // 0x1038 B_TXRX_Ctrl	//0x5B6C8769
 			pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-			pWb35Reg->BB3C = 0x00000000;
+			reg->BB3C = 0x00000000;
 			pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 			pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 			pltmp[6] = BB48_DEFAULT_AL2230_11G; // 0x1048 11b TX RC filter 20060613.2
-			pWb35Reg->BB48 = BB48_DEFAULT_AL2230_11G; // 20051221 ch14 20060613.2
+			reg->BB48 = BB48_DEFAULT_AL2230_11G; // 20051221 ch14 20060613.2
 			pltmp[7] = BB4C_DEFAULT_AL2230_11G; // 0x104c 11b TX RC filter 20060613.2
-			pWb35Reg->BB4C = BB4C_DEFAULT_AL2230_11G; // 20060613.1 20060613.2
+			reg->BB4C = BB4C_DEFAULT_AL2230_11G; // 20060613.1 20060613.2
 			pltmp[8] = 0x27106200; // 0x1050 MODE_Ctrl
-			pWb35Reg->BB50 = 0x27106200;
+			reg->BB50 = 0x27106200;
 			pltmp[9] = 0; // 0x1054
-			pWb35Reg->BB54 = 0x00000000;
+			reg->BB54 = 0x00000000;
 			pltmp[10] = 0x52524242; // 0x1058 IQ_Alpha
-			pWb35Reg->BB58 = 0x52524242;
+			reg->BB58 = 0x52524242;
 			pltmp[11] = 0xAA0AC000; // 0x105c DC_Cancel
 			Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1690,7 +1691,7 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[1] = 0x9affafb2; // 0x1004 AGC_Ctrl2
 			pltmp[2] = 0x55d00a04; // 0x1008 AGC_Ctrl3
 			pltmp[3] = 0xFFFd203c; // 0xFFFb203a; // 0x100c AGC_Ctrl4 Modify for 33's 1.0.95.xxx version
-			pWb35Reg->BB0C = 0xFFFd203c;
+			reg->BB0C = 0xFFFd203c;
 			pltmp[4] = 0X0FBFDCc5; // 0X0FBFDCA0; // 0x1010 AGC_Ctrl5 //0x0FB2E0B7 Modify for 33's 1.0.95.xxx version
 			pltmp[5] = 0x00caa332; // 0x00caa333; // 0x1014 AGC_Ctrl6 Modify for 33's 1.0.95.xxx version
 			pltmp[6] = 0XF6632111; // 0XF1632112; // 0x1018 AGC_Ctrl7		//0xf6632112 Modify for 33's 1.0.95.xxx version
@@ -1699,27 +1700,27 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[9] = 0x00002A79; // 0x1024 AGC_Ctrl10
 			pltmp[10] = 0X40000528;							//0x40000228
 			pltmp[11] = 0x232dfF30; // 0x232A9F30; // 0x102c A_ACQ_Ctrl	//0x232a9730
-			pWb35Reg->BB2C = 0x232dfF30; //Modify for 33's 1.0.95.xxx version, antenna 1
+			reg->BB2C = 0x232dfF30; //Modify for 33's 1.0.95.xxx version, antenna 1
 			Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 			pltmp[0] = 0x00002C54; // 0x1030 B_ACQ_Ctrl
-			pWb35Reg->BB30 = 0x00002C54;
+			reg->BB30 = 0x00002C54;
 			pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 			pltmp[2] = 0x5B2C8769; // 0x1038 B_TXRX_Ctrl	//0x5B6C8769
 			pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-			pWb35Reg->BB3C = 0x00000000;
+			reg->BB3C = 0x00000000;
 			pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 			pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 			pltmp[6] = BB48_DEFAULT_AL2230_11G; // 0x1048 11b TX RC filter 20060613.2
-			pWb35Reg->BB48 = BB48_DEFAULT_AL2230_11G; // 20051221 ch14 20060613.2
+			reg->BB48 = BB48_DEFAULT_AL2230_11G; // 20051221 ch14 20060613.2
 			pltmp[7] = BB4C_DEFAULT_AL2230_11G; // 0x104c 11b TX RC filter 20060613.2
-			pWb35Reg->BB4C = BB4C_DEFAULT_AL2230_11G; // 20060613.1
+			reg->BB4C = BB4C_DEFAULT_AL2230_11G; // 20060613.1
 			pltmp[8] = 0x27106200; // 0x1050 MODE_Ctrl
-			pWb35Reg->BB50 = 0x27106200;
+			reg->BB50 = 0x27106200;
 			pltmp[9] = 0; // 0x1054
-			pWb35Reg->BB54 = 0x00000000;
+			reg->BB54 = 0x00000000;
 			pltmp[10] = 0x52523232; // 20060419 0x52524242; // 0x1058 IQ_Alpha
-			pWb35Reg->BB58 = 0x52523232; // 20060419 0x52524242;
+			reg->BB58 = 0x52523232; // 20060419 0x52524242;
 			pltmp[11] = 0xAA0AC000; // 0x105c DC_Cancel
 			Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1732,7 +1733,7 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[1] = 0x9affafb2; // 0x1004 AGC_Ctrl2
 			pltmp[2] = 0x55d00a04; // 0x1008 AGC_Ctrl3
 			pltmp[3] = 0xFFFb203a; // 0x100c AGC_Ctrl4
-			pWb35Reg->BB0c = 0xFFFb203a;
+			reg->BB0c = 0xFFFb203a;
 			pltmp[4] = 0x0FBFDCB7; // 0x1010 AGC_Ctrl5
 			pltmp[5] = 0x00caa333; // 0x1014 AGC_Ctrl6
 			pltmp[6] = 0xf6632112; // 0x1018 AGC_Ctrl7
@@ -1741,25 +1742,25 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[9] = 0x00002A79; // 0x1024 AGC_Ctrl10
 			pltmp[10] = 0x40000228;
 			pltmp[11] = 0x232A9F30;// 0x102c A_ACQ_Ctrl
-			pWb35Reg->BB2c = 0x232A9F30;
+			reg->BB2c = 0x232A9F30;
 			Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 			pltmp[0] = 0x00002C54; // 0x1030 B_ACQ_Ctrl
-			pWb35Reg->BB30 = 0x00002C54;
+			reg->BB30 = 0x00002C54;
 			pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 			pltmp[2] = 0x5B2C8769; // 0x1038 B_TXRX_Ctrl
 			pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-			pWb35Reg->BB3c = 0x00000000;
+			reg->BB3c = 0x00000000;
 			pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 			pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 			pltmp[6] = 0x00453B24; // 0x1048 11b TX RC filter
 			pltmp[7] = 0x0E00FEFF; // 0x104c 11b TX RC filter
 			pltmp[8] = 0x27106200; // 0x1050 MODE_Ctrl
-			pWb35Reg->BB50 = 0x27106200;
+			reg->BB50 = 0x27106200;
 			pltmp[9] = 0; // 0x1054
-			pWb35Reg->BB54 = 0x00000000;
+			reg->BB54 = 0x00000000;
 			pltmp[10] = 0x64645252; // 0x1058 IQ_Alpha
-			pWb35Reg->BB58 = 0x64645252;
+			reg->BB58 = 0x64645252;
 			pltmp[11] = 0xAA0AC000; // 0x105c DC_Cancel
 			Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 */
@@ -1775,7 +1776,7 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[1] = 0x9AFF9ABA; // 0x1004 AGC_Ctrl2
 			pltmp[2] = 0x55D00A04; // 0x1008 AGC_Ctrl3
 			pltmp[3] = 0xEEE91C32; // 0x100c AGC_Ctrl4
-			pWb35Reg->BB0C = 0xEEE91C32;
+			reg->BB0C = 0xEEE91C32;
 			pltmp[4] = 0x0FACDCC5; // 0x1010 AGC_Ctrl5
 			pltmp[5] = 0x000AA344; // 0x1014 AGC_Ctrl6
 			pltmp[6] = 0x22222221; // 0x1018 AGC_Ctrl7
@@ -1784,27 +1785,27 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[9] = 0xA9002A79; // 0x1024 AGC_Ctrl10
 			pltmp[10] = 0x40000528; // 0x1028
 			pltmp[11] = 0x23457F30; // 0x102c A_ACQ_Ctrl
-			pWb35Reg->BB2C = 0x23457F30;
+			reg->BB2C = 0x23457F30;
 			Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 			pltmp[0] = 0x00002C54; // 0x1030 B_ACQ_Ctrl
-			pWb35Reg->BB30 = 0x00002C54;
+			reg->BB30 = 0x00002C54;
 			pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 			pltmp[2] = 0x5B2C8769; // 0x1038 B_TXRX_Ctrl
 			pltmp[3] = pHwData->BB3c_cal; // 0x103c 11a TX LS filter
-			pWb35Reg->BB3C = pHwData->BB3c_cal;
+			reg->BB3C = pHwData->BB3c_cal;
 			pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 			pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 			pltmp[6] = BB48_DEFAULT_WB242_11G; // 0x1048 11b TX RC filter 20060613.2
-			pWb35Reg->BB48 = BB48_DEFAULT_WB242_11G; // 20060613.1 20060613.2
+			reg->BB48 = BB48_DEFAULT_WB242_11G; // 20060613.1 20060613.2
 			pltmp[7] = BB4C_DEFAULT_WB242_11G; // 0x104c 11b TX RC filter 20060613.2
-			pWb35Reg->BB4C = BB4C_DEFAULT_WB242_11G; // 20060613.1 20060613.2
+			reg->BB4C = BB4C_DEFAULT_WB242_11G; // 20060613.1 20060613.2
 			pltmp[8] = 0x27106208; // 0x1050 MODE_Ctrl
-			pWb35Reg->BB50 = 0x27106208;
+			reg->BB50 = 0x27106208;
 			pltmp[9] = pHwData->BB54_cal; // 0x1054
-			pWb35Reg->BB54 = pHwData->BB54_cal;
+			reg->BB54 = pHwData->BB54_cal;
 			pltmp[10] = 0x52523131; // 0x1058 IQ_Alpha
-			pWb35Reg->BB58 = 0x52523131;
+			reg->BB58 = 0x52523131;
 			pltmp[11] = 0xAA0AC000; // 20060825 0xAA2AC000; // 0x105c DC_Cancel
 			Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1813,14 +1814,14 @@ BBProcessor_initial(  phw_data_t pHwData )
     }
 
 	// Fill the LNA table
-	pWb35Reg->LNAValue[0] = (u8)(pWb35Reg->BB0C & 0xff);
-	pWb35Reg->LNAValue[1] = 0;
-	pWb35Reg->LNAValue[2] = (u8)((pWb35Reg->BB0C & 0xff00)>>8);
-	pWb35Reg->LNAValue[3] = 0;
+	reg->LNAValue[0] = (u8)(reg->BB0C & 0xff);
+	reg->LNAValue[1] = 0;
+	reg->LNAValue[2] = (u8)((reg->BB0C & 0xff00)>>8);
+	reg->LNAValue[3] = 0;
 
 	// Fill SQ3 table
 	for( i=0; i<MAX_SQ3_FILTER_SIZE; i++ )
-		pWb35Reg->SQ3_filter[i] = 0x2f; // half of Bit 0 ~ 6
+		reg->SQ3_filter[i] = 0x2f; // half of Bit 0 ~ 6
 }
 
 void set_tx_power_per_channel_max2829(  phw_data_t pHwData,  ChanInfo Channel)
@@ -1903,7 +1904,7 @@ void set_tx_power_per_channel_wb242(  phw_data_t pHwData,  ChanInfo Channel)
 void
 RFSynthesizer_SwitchingChannel(  phw_data_t pHwData,  ChanInfo Channel )
 {
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	u32	pltmp[16]; // The 16 is the maximum capability of hardware
 	u32	count, ltmp;
 	u8	i, j, number;
@@ -2090,40 +2091,40 @@ RFSynthesizer_SwitchingChannel(  phw_data_t pHwData,  ChanInfo Channel )
 	if( Channel.band <= BAND_TYPE_OFDM_24 )
 	{
         // BB: select 2.4 GHz, bit[12-11]=00
-		pWb35Reg->BB50 &= ~(BIT(11)|BIT(12));
-		Wb35Reg_Write( pHwData, 0x1050, pWb35Reg->BB50 ); // MODE_Ctrl
+		reg->BB50 &= ~(BIT(11)|BIT(12));
+		Wb35Reg_Write( pHwData, 0x1050, reg->BB50 ); // MODE_Ctrl
         // MAC: select 2.4 GHz, bit[5]=0
-		pWb35Reg->M78_ERPInformation &= ~BIT(5);
-		Wb35Reg_Write( pHwData, 0x0878, pWb35Reg->M78_ERPInformation );
+		reg->M78_ERPInformation &= ~BIT(5);
+		Wb35Reg_Write( pHwData, 0x0878, reg->M78_ERPInformation );
         // enable 11b Baseband
-		pWb35Reg->BB30 &= ~BIT(31);
-		Wb35Reg_Write( pHwData, 0x1030, pWb35Reg->BB30 );
+		reg->BB30 &= ~BIT(31);
+		Wb35Reg_Write( pHwData, 0x1030, reg->BB30 );
 	}
 	else if( (Channel.band == BAND_TYPE_OFDM_5) )
 	{
         // BB: select 5 GHz
-		pWb35Reg->BB50 &= ~(BIT(11)|BIT(12));
+		reg->BB50 &= ~(BIT(11)|BIT(12));
 		if (Channel.ChanNo <=64 )
-			pWb35Reg->BB50 |= BIT(12);				// 10-5.25GHz
+			reg->BB50 |= BIT(12);				// 10-5.25GHz
 		else if ((Channel.ChanNo >= 100) && (Channel.ChanNo <= 124))
-			pWb35Reg->BB50 |= BIT(11);				// 01-5.48GHz
+			reg->BB50 |= BIT(11);				// 01-5.48GHz
 		else if ((Channel.ChanNo >=128) && (Channel.ChanNo <= 161))
-			pWb35Reg->BB50 |= (BIT(12)|BIT(11));	// 11-5.775GHz
+			reg->BB50 |= (BIT(12)|BIT(11));	// 11-5.775GHz
 		else	//Chan 184 ~ 196 will use bit[12-11] = 10 in version sh-src-1.2.25
-			pWb35Reg->BB50 |= BIT(12);
-		Wb35Reg_Write( pHwData, 0x1050, pWb35Reg->BB50 ); // MODE_Ctrl
+			reg->BB50 |= BIT(12);
+		Wb35Reg_Write( pHwData, 0x1050, reg->BB50 ); // MODE_Ctrl
 
 		//(1) M78 should alway use 2.4G setting when using RF_AIROHA_7230
 		//(2) BB30 has been updated previously.
 		if (pHwData->phy_type != RF_AIROHA_7230)
 		{
     	    // MAC: select 5 GHz, bit[5]=1
-			pWb35Reg->M78_ERPInformation |= BIT(5);
-			Wb35Reg_Write( pHwData, 0x0878, pWb35Reg->M78_ERPInformation );
+			reg->M78_ERPInformation |= BIT(5);
+			Wb35Reg_Write( pHwData, 0x0878, reg->M78_ERPInformation );
 
     	    // disable 11b Baseband
-			pWb35Reg->BB30 |= BIT(31);
-			Wb35Reg_Write( pHwData, 0x1030, pWb35Reg->BB30 );
+			reg->BB30 |= BIT(31);
+			Wb35Reg_Write( pHwData, 0x1030, reg->BB30 );
 		}
 	}
 }
@@ -2313,21 +2314,21 @@ u8 RFSynthesizer_SetWinbond242Power(  phw_data_t pHwData, u8 index )
 //===========================================================================================================
 void Dxx_initial(  phw_data_t pHwData )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 
 	// Old IC:Single mode only.
 	// New IC: operation decide by Software set bit[4]. 1:multiple 0: single
-	pWb35Reg->D00_DmaControl = 0xc0000004;	//Txon, Rxon, multiple Rx for new 4k DMA
+	reg->D00_DmaControl = 0xc0000004;	//Txon, Rxon, multiple Rx for new 4k DMA
 											//Txon, Rxon, single Rx for old 8k ASIC
 	if( !HAL_USB_MODE_BURST( pHwData ) )
-		pWb35Reg->D00_DmaControl = 0xc0000000;//Txon, Rxon, single Rx for new 4k DMA
+		reg->D00_DmaControl = 0xc0000000;//Txon, Rxon, single Rx for new 4k DMA
 
-	Wb35Reg_WriteSync( pHwData, 0x0400, pWb35Reg->D00_DmaControl );
+	Wb35Reg_WriteSync( pHwData, 0x0400, reg->D00_DmaControl );
 }
 
 void Mxx_initial(  phw_data_t pHwData )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	u32		tmp;
 	u32		pltmp[11];
 	u16	i;
@@ -2339,23 +2340,23 @@ void Mxx_initial(  phw_data_t pHwData )
 
 	// M00 bit set
 #ifdef _IBSS_BEACON_SEQ_STICK_
-	pWb35Reg->M00_MacControl = 0; // Solve beacon sequence number stop by software
+	reg->M00_MacControl = 0; // Solve beacon sequence number stop by software
 #else
-	pWb35Reg->M00_MacControl = 0x80000000; // Solve beacon sequence number stop by hardware
+	reg->M00_MacControl = 0x80000000; // Solve beacon sequence number stop by hardware
 #endif
 
 	// M24 disable enter power save, BB RxOn and enable NAV attack
-	pWb35Reg->M24_MacControl = 0x08040042;
-	pltmp[0] = pWb35Reg->M24_MacControl;
+	reg->M24_MacControl = 0x08040042;
+	pltmp[0] = reg->M24_MacControl;
 
 	pltmp[1] = 0; // Skip M28, because no initialize value is required.
 
 	// M2C CWmin and CWmax setting
 	pHwData->cwmin = DEFAULT_CWMIN;
 	pHwData->cwmax = DEFAULT_CWMAX;
-	pWb35Reg->M2C_MacControl = DEFAULT_CWMIN << 10;
-	pWb35Reg->M2C_MacControl |= DEFAULT_CWMAX;
-	pltmp[2] = pWb35Reg->M2C_MacControl;
+	reg->M2C_MacControl = DEFAULT_CWMIN << 10;
+	reg->M2C_MacControl |= DEFAULT_CWMAX;
+	pltmp[2] = reg->M2C_MacControl;
 
 	// M30 BSSID
 	pltmp[3] = *(u32 *)pHwData->bssid;
@@ -2367,35 +2368,35 @@ void Mxx_initial(  phw_data_t pHwData )
 	pltmp[4] = tmp;
 
 	// M38
-	pWb35Reg->M38_MacControl = (DEFAULT_RATE_RETRY_LIMIT<<8) | (DEFAULT_LONG_RETRY_LIMIT << 4) | DEFAULT_SHORT_RETRY_LIMIT;
-	pltmp[5] = pWb35Reg->M38_MacControl;
+	reg->M38_MacControl = (DEFAULT_RATE_RETRY_LIMIT<<8) | (DEFAULT_LONG_RETRY_LIMIT << 4) | DEFAULT_SHORT_RETRY_LIMIT;
+	pltmp[5] = reg->M38_MacControl;
 
 	// M3C
 	tmp = (DEFAULT_PIFST << 26) | (DEFAULT_EIFST << 16) | (DEFAULT_DIFST << 8) | (DEFAULT_SIFST << 4) | DEFAULT_OSIFST ;
-	pWb35Reg->M3C_MacControl = tmp;
+	reg->M3C_MacControl = tmp;
 	pltmp[6] = tmp;
 
 	// M40
 	pHwData->slot_time_select = DEFAULT_SLOT_TIME;
 	tmp = (DEFAULT_ATIMWD << 16) | DEFAULT_SLOT_TIME;
-	pWb35Reg->M40_MacControl = tmp;
+	reg->M40_MacControl = tmp;
 	pltmp[7] = tmp;
 
 	// M44
 	tmp = DEFAULT_MAX_TX_MSDU_LIFE_TIME << 10; // *1024
-	pWb35Reg->M44_MacControl = tmp;
+	reg->M44_MacControl = tmp;
 	pltmp[8] = tmp;
 
 	// M48
 	pHwData->BeaconPeriod = DEFAULT_BEACON_INTERVAL;
 	pHwData->ProbeDelay = DEFAULT_PROBE_DELAY_TIME;
 	tmp = (DEFAULT_BEACON_INTERVAL << 16) | DEFAULT_PROBE_DELAY_TIME;
-	pWb35Reg->M48_MacControl = tmp;
+	reg->M48_MacControl = tmp;
 	pltmp[9] = tmp;
 
 	//M4C
-	pWb35Reg->M4C_MacStatus = (DEFAULT_PROTOCOL_VERSION << 30) | (DEFAULT_MAC_POWER_STATE << 28) | (DEFAULT_DTIM_ALERT_TIME << 24);
-	pltmp[10] = pWb35Reg->M4C_MacStatus;
+	reg->M4C_MacStatus = (DEFAULT_PROTOCOL_VERSION << 30) | (DEFAULT_MAC_POWER_STATE << 28) | (DEFAULT_DTIM_ALERT_TIME << 24);
+	pltmp[10] = reg->M4C_MacStatus;
 
 	// Burst write
 	//Wb35Reg_BurstWrite( pHwData, 0x0824, pltmp, 11, AUTO_INCREMENT );
@@ -2404,15 +2405,15 @@ void Mxx_initial(  phw_data_t pHwData )
 
 	// M60
 	Wb35Reg_WriteSync( pHwData, 0x0860, 0x12481248 );
-	pWb35Reg->M60_MacControl = 0x12481248;
+	reg->M60_MacControl = 0x12481248;
 
 	// M68
 	Wb35Reg_WriteSync( pHwData, 0x0868, 0x00050900 ); // 20051018 0x000F0F00 ); // 940930 0x00131300
-	pWb35Reg->M68_MacControl = 0x00050900;
+	reg->M68_MacControl = 0x00050900;
 
 	// M98
 	Wb35Reg_WriteSync( pHwData, 0x0898, 0xffff8888 );
-	pWb35Reg->M98_MacControl = 0xffff8888;
+	reg->M98_MacControl = 0xffff8888;
 }
 
 
@@ -2620,7 +2621,7 @@ void EEPROMTxVgaAdjust(  phw_data_t pHwData ) // 20060619.5 Add
 
 void BBProcessor_RateChanging(  phw_data_t pHwData,  u8 rate ) // 20060613.1
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	unsigned char		Is11bRate;
 
 	Is11bRate = (rate % 6) ? 1 : 0;
@@ -2630,8 +2631,8 @@ void BBProcessor_RateChanging(  phw_data_t pHwData,  u8 rate ) // 20060613.1
 		case RF_AIROHA_2230S: // 20060420 Add this
 			if( Is11bRate )
 			{
-				if( (pWb35Reg->BB48 != BB48_DEFAULT_AL2230_11B) &&
-					(pWb35Reg->BB4C != BB4C_DEFAULT_AL2230_11B) )
+				if( (reg->BB48 != BB48_DEFAULT_AL2230_11B) &&
+					(reg->BB4C != BB4C_DEFAULT_AL2230_11B) )
 				{
 					Wb35Reg_Write( pHwData, 0x1048, BB48_DEFAULT_AL2230_11B );
 					Wb35Reg_Write( pHwData, 0x104c, BB4C_DEFAULT_AL2230_11B );
@@ -2639,8 +2640,8 @@ void BBProcessor_RateChanging(  phw_data_t pHwData,  u8 rate ) // 20060613.1
 			}
 			else
 			{
-				if( (pWb35Reg->BB48 != BB48_DEFAULT_AL2230_11G) &&
-					(pWb35Reg->BB4C != BB4C_DEFAULT_AL2230_11G) )
+				if( (reg->BB48 != BB48_DEFAULT_AL2230_11G) &&
+					(reg->BB4C != BB4C_DEFAULT_AL2230_11G) )
 				{
 					Wb35Reg_Write( pHwData, 0x1048, BB48_DEFAULT_AL2230_11G );
 					Wb35Reg_Write( pHwData, 0x104c, BB4C_DEFAULT_AL2230_11G );
@@ -2651,22 +2652,22 @@ void BBProcessor_RateChanging(  phw_data_t pHwData,  u8 rate ) // 20060613.1
 		case RF_WB_242: // 20060623 The fix only for old TxVGA setting
 			if( Is11bRate )
 			{
-				if( (pWb35Reg->BB48 != BB48_DEFAULT_WB242_11B) &&
-					(pWb35Reg->BB4C != BB4C_DEFAULT_WB242_11B) )
+				if( (reg->BB48 != BB48_DEFAULT_WB242_11B) &&
+					(reg->BB4C != BB4C_DEFAULT_WB242_11B) )
 				{
-					pWb35Reg->BB48 = BB48_DEFAULT_WB242_11B;
-					pWb35Reg->BB4C = BB4C_DEFAULT_WB242_11B;
+					reg->BB48 = BB48_DEFAULT_WB242_11B;
+					reg->BB4C = BB4C_DEFAULT_WB242_11B;
 					Wb35Reg_Write( pHwData, 0x1048, BB48_DEFAULT_WB242_11B );
 					Wb35Reg_Write( pHwData, 0x104c, BB4C_DEFAULT_WB242_11B );
 				}
 			}
 			else
 			{
-				if( (pWb35Reg->BB48 != BB48_DEFAULT_WB242_11G) &&
-					(pWb35Reg->BB4C != BB4C_DEFAULT_WB242_11G) )
+				if( (reg->BB48 != BB48_DEFAULT_WB242_11G) &&
+					(reg->BB4C != BB4C_DEFAULT_WB242_11G) )
 				{
-					pWb35Reg->BB48 = BB48_DEFAULT_WB242_11G;
-					pWb35Reg->BB4C = BB4C_DEFAULT_WB242_11G;
+					reg->BB48 = BB48_DEFAULT_WB242_11G;
+					reg->BB4C = BB4C_DEFAULT_WB242_11G;
 					Wb35Reg_Write( pHwData, 0x1048, BB48_DEFAULT_WB242_11G );
 					Wb35Reg_Write( pHwData, 0x104c, BB4C_DEFAULT_WB242_11G );
 				}
diff --git a/drivers/staging/winbond/rxisr.c b/drivers/staging/winbond/rxisr.c
deleted file mode 100644
index 18e942c9b821..000000000000
--- a/drivers/staging/winbond/rxisr.c
+++ /dev/null
@@ -1,30 +0,0 @@
-#include "os_common.h"
-
-void vRxTimerInit(PWB32_ADAPTER Adapter)
-{
-	OS_TIMER_INITIAL(&(Adapter->Mds.nTimer), (void*) RxTimerHandler, (void*) Adapter);
-}
-
-void vRxTimerStart(PWB32_ADAPTER Adapter, int timeout_value)
-{
-	if (timeout_value<MIN_TIMEOUT_VAL)
-		timeout_value=MIN_TIMEOUT_VAL;
-
-	OS_TIMER_SET( &(Adapter->Mds.nTimer), timeout_value );
-}
-
-void vRxTimerStop(PWB32_ADAPTER Adapter)
-{
-	OS_TIMER_CANCEL( &(Adapter->Mds.nTimer), 0 );
-}
-
-void RxTimerHandler_1a( PADAPTER Adapter)
-{
-	RxTimerHandler(NULL, Adapter, NULL, NULL);
-}
-
-void RxTimerHandler(void* SystemSpecific1, PWB32_ADAPTER Adapter,
-		    void* SystemSpecific2, void* SystemSpecific3)
-{
-	WARN_ON(1);
-}
diff --git a/drivers/staging/winbond/scan_s.h b/drivers/staging/winbond/scan_s.h
index 1d1b0c4fec17..775bb81f23cc 100644
--- a/drivers/staging/winbond/scan_s.h
+++ b/drivers/staging/winbond/scan_s.h
@@ -1,3 +1,9 @@
+#ifndef __WINBOND_SCAN_S_H
+#define __WINBOND_SCAN_S_H
+
+#include <linux/types.h>
+#include "localpara.h"
+
 //
 // SCAN task global CONSTANTS, STRUCTURES, variables
 //
@@ -62,8 +68,7 @@ typedef struct _SCAN_PARAMETERS
 	u8				boCCAbusy;					// Wb: HWMAC CCA busy status
 	u8				reserved_2;
 
-	//NDIS_MINIPORT_TIMER	nTimer;
-	OS_TIMER			nTimer;
+	struct timer_list timer;
 
 	u32				ScanTimeStamp;			//Increase 1 per background scan(1 minute)
 	u32				BssTimeStamp;			//Increase 1 per connect status check
@@ -78,9 +83,9 @@ typedef struct _SCAN_PARAMETERS
 
 } SCAN_PARAMETERS, *psSCAN_PARAMETERS;
 
-// Encapsulate 'Adapter' data structure
-#define psSCAN			(&(Adapter->sScanPara))
-#define psSCANREQ			(&(Adapter->sScanPara.sScanReq))
+// Encapsulate 'adapter' data structure
+#define psSCAN			(&(adapter->sScanPara))
+#define psSCANREQ			(&(adapter->sScanPara.sScanReq))
 
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //	scan.h
@@ -109,7 +114,8 @@ typedef struct _SCAN_PARAMETERS
 
 // static functions
 
-//static void ScanTimerHandler(PWB32_ADAPTER Adapter);
-//static void vScanTimerStart(PWB32_ADAPTER	Adapter, int timeout_value);
-//static void vScanTimerStop(PWB32_ADAPTER Adapter);
+//static void ScanTimerHandler(struct wbsoft_priv * adapter);
+//static void vScanTimerStart(struct wbsoft_priv *	adapter, int timeout_value);
+//static void vScanTimerStop(struct wbsoft_priv * adapter);
 
+#endif
diff --git a/drivers/staging/winbond/sme_api.c b/drivers/staging/winbond/sme_api.c
deleted file mode 100644
index 31c9673ea865..000000000000
--- a/drivers/staging/winbond/sme_api.c
+++ /dev/null
@@ -1,14 +0,0 @@
-//------------------------------------------------------------------------------------
-// sme_api.c
-//
-// Copyright(C) 2002 Winbond Electronics Corp.
-//
-//
-//------------------------------------------------------------------------------------
-#include "os_common.h"
-
-s8 sme_get_rssi(void *pcore_data, s32 *prssi)
-{
-       BUG();
-       return 0;
-}
diff --git a/drivers/staging/winbond/sme_api.h b/drivers/staging/winbond/sme_api.h
index 745eb376bc70..188a2532bbf4 100644
--- a/drivers/staging/winbond/sme_api.h
+++ b/drivers/staging/winbond/sme_api.h
@@ -13,6 +13,10 @@
 #ifndef __SME_API_H__
 #define __SME_API_H__
 
+#include <linux/types.h>
+
+#include "localpara.h"
+
 /****************** INCLUDE FILES SECTION ***********************************/
 //#include "GL\gl_core.h"
 
@@ -52,9 +56,6 @@ s8 sme_set_fragment_threshold(void *pcore_data, u32 threshold);
 s8 sme_get_rts_threshold(void *pcore_data, u32 *pthreshold);
 s8 sme_set_rts_threshold(void *pcore_data, u32 threshold);
 
-// OID_802_11_RSSI
-s8 sme_get_rssi(void *pcore_data, s32 *prssi);
-
 // OID_802_11_CONFIGURATION
 s8 sme_get_beacon_period(void *pcore_data, u16 *pbeacon_period);
 s8 sme_set_beacon_period(void *pcore_data, u16 beacon_period);
diff --git a/drivers/staging/winbond/sme_s.h b/drivers/staging/winbond/sme_s.h
index dfd2fbc4edef..1bd118f83d20 100644
--- a/drivers/staging/winbond/sme_s.h
+++ b/drivers/staging/winbond/sme_s.h
@@ -1,3 +1,11 @@
+#ifndef __WINBOND_SME_S_H
+#define __WINBOND_SME_S_H
+
+#include <linux/types.h>
+
+#include "mac_structures.h"
+#include "localpara.h"
+
 //
 // SME_S.H -
 // SME task global CONSTANTS, STRUCTURES, variables
@@ -106,8 +114,7 @@ typedef struct _SME_PARAMETERS
 	u8				bDesiredPowerSave;
 
 	// SME timer and timeout value
-	//NDIS_MINIPORT_TIMER	nTimer;
-	OS_TIMER			nTimer;
+	struct timer_list timer;
 
 	u8				boInTimerHandler;
 	u8 				boAuthRetryActive;
@@ -196,9 +203,9 @@ typedef struct _SME_PARAMETERS
 
 } SME_PARAMETERS, *PSME_PARAMETERS;
 
-#define psSME			(&(Adapter->sSmePara))
+#define psSME			(&(adapter->sSmePara))
 
-#define wSMEGetCurrentSTAState(Adapter)		((u16)(Adapter)->sSmePara.wState)
+#define wSMEGetCurrentSTAState(adapter)		((u16)(adapter)->sSmePara.wState)
 
 
 
@@ -226,3 +233,4 @@ typedef struct _SME_PARAMETERS
 
 // Static function
 
+#endif
diff --git a/drivers/staging/winbond/linux/sysdef.h b/drivers/staging/winbond/sysdef.h
index d46d63e5c673..251b9c553b6c 100644
--- a/drivers/staging/winbond/linux/sysdef.h
+++ b/drivers/staging/winbond/sysdef.h
@@ -37,37 +37,4 @@
 #define _PE_USB_INI_DUMP_
 #endif
 
-
-
-#include "common.h"	// Individual file depends on OS
-
-#include "../wb35_ver.h"
-#include "../mac_structures.h"
-#include "../ds_tkip.h"
-#include "../localpara.h"
-#include "../sme_s.h"
-#include "../scan_s.h"
-#include "../mds_s.h"
-#include "../mlme_s.h"
-#include "../bssdscpt.h"
-#include "../sme_api.h"
-#include "../gl_80211.h"
-#include "../mto.h"
-#include "../wblinux_s.h"
-#include "../wbhal_s.h"
-
-
-#include "../adapter.h"
-
-#include "../mlme_mib.h"
-#include "../mds_f.h"
-#include "../bss_f.h"
-#include "../mlmetxrx_f.h"
-#include "../mto_f.h"
-#include "../wbhal_f.h"
-#include "../wblinux_f.h"
-// Kernel Timer resolution, NDIS is 10ms, 10000us
-#define MIN_TIMEOUT_VAL	(10) //ms
-
-
 #endif
diff --git a/drivers/staging/winbond/linux/wb35reg.c b/drivers/staging/winbond/wb35reg.c
index ebb6db5438a4..c74b3fdcbda0 100644
--- a/drivers/staging/winbond/linux/wb35reg.c
+++ b/drivers/staging/winbond/wb35reg.c
@@ -1,9 +1,12 @@
 #include "sysdef.h"
+#include "wb35reg_f.h"
+
+#include <linux/usb.h>
 
 extern void phy_calibration_winbond(hw_data_t *phw_data, u32 frequency);
 
-// TRUE  : read command process successfully
-// FALSE : register not support
+// true  : read command process successfully
+// false : register not support
 // RegisterNo : start base
 // pRegisterData : data point
 // NumberOfData : number of register data
@@ -12,135 +15,135 @@ extern void phy_calibration_winbond(hw_data_t *phw_data, u32 frequency);
 unsigned char
 Wb35Reg_BurstWrite(phw_data_t pHwData, u16 RegisterNo, u32 * pRegisterData, u8 NumberOfData, u8 Flag)
 {
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
-	PURB		pUrb = NULL;
-	PREG_QUEUE	pRegQueue = NULL;
+	struct wb35_reg *reg = &pHwData->reg;
+	struct urb	*urb = NULL;
+	struct wb35_reg_queue *reg_queue = NULL;
 	u16		UrbSize;
 	struct      usb_ctrlrequest *dr;
 	u16		i, DataSize = NumberOfData*4;
 
 	// Module shutdown
 	if (pHwData->SurpriseRemove)
-		return FALSE;
+		return false;
 
 	// Trying to use burst write function if use new hardware
-	UrbSize = sizeof(REG_QUEUE) + DataSize + sizeof(struct usb_ctrlrequest);
-	OS_MEMORY_ALLOC( (void* *)&pRegQueue, UrbSize );
-	pUrb = wb_usb_alloc_urb(0);
-	if( pUrb && pRegQueue ) {
-		pRegQueue->DIRECT = 2;// burst write register
-		pRegQueue->INDEX = RegisterNo;
-		pRegQueue->pBuffer = (u32 *)((u8 *)pRegQueue + sizeof(REG_QUEUE));
-		memcpy( pRegQueue->pBuffer, pRegisterData, DataSize );
+	UrbSize = sizeof(struct wb35_reg_queue) + DataSize + sizeof(struct usb_ctrlrequest);
+	reg_queue = kzalloc(UrbSize, GFP_ATOMIC);
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if( urb && reg_queue ) {
+		reg_queue->DIRECT = 2;// burst write register
+		reg_queue->INDEX = RegisterNo;
+		reg_queue->pBuffer = (u32 *)((u8 *)reg_queue + sizeof(struct wb35_reg_queue));
+		memcpy( reg_queue->pBuffer, pRegisterData, DataSize );
 		//the function for reversing register data from little endian to big endian
 		for( i=0; i<NumberOfData ; i++ )
-			pRegQueue->pBuffer[i] = cpu_to_le32( pRegQueue->pBuffer[i] );
+			reg_queue->pBuffer[i] = cpu_to_le32( reg_queue->pBuffer[i] );
 
-		dr = (struct usb_ctrlrequest *)((u8 *)pRegQueue + sizeof(REG_QUEUE) + DataSize);
+		dr = (struct usb_ctrlrequest *)((u8 *)reg_queue + sizeof(struct wb35_reg_queue) + DataSize);
 		dr->bRequestType = USB_TYPE_VENDOR | USB_DIR_OUT | USB_RECIP_DEVICE;
 		dr->bRequest = 0x04; // USB or vendor-defined request code, burst mode
 		dr->wValue = cpu_to_le16( Flag ); // 0: Register number auto-increment, 1: No auto increment
 		dr->wIndex = cpu_to_le16( RegisterNo );
 		dr->wLength = cpu_to_le16( DataSize );
-		pRegQueue->Next = NULL;
-		pRegQueue->pUsbReq = dr;
-		pRegQueue->pUrb = pUrb;
+		reg_queue->Next = NULL;
+		reg_queue->pUsbReq = dr;
+		reg_queue->urb = urb;
 
-		spin_lock_irq( &pWb35Reg->EP0VM_spin_lock );
-		if (pWb35Reg->pRegFirst == NULL)
-			pWb35Reg->pRegFirst = pRegQueue;
+		spin_lock_irq( &reg->EP0VM_spin_lock );
+		if (reg->reg_first == NULL)
+			reg->reg_first = reg_queue;
 		else
-			pWb35Reg->pRegLast->Next = pRegQueue;
-		pWb35Reg->pRegLast = pRegQueue;
+			reg->reg_last->Next = reg_queue;
+		reg->reg_last = reg_queue;
 
-		spin_unlock_irq( &pWb35Reg->EP0VM_spin_lock );
+		spin_unlock_irq( &reg->EP0VM_spin_lock );
 
 		// Start EP0VM
 		Wb35Reg_EP0VM_start(pHwData);
 
-		return TRUE;
+		return true;
 	} else {
-		if (pUrb)
-			usb_free_urb(pUrb);
-		if (pRegQueue)
-			kfree(pRegQueue);
-		return FALSE;
+		if (urb)
+			usb_free_urb(urb);
+		if (reg_queue)
+			kfree(reg_queue);
+		return false;
 	}
-   return FALSE;
+   return false;
 }
 
 void
 Wb35Reg_Update(phw_data_t pHwData,  u16 RegisterNo,  u32 RegisterValue)
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	switch (RegisterNo) {
-	case 0x3b0: pWb35Reg->U1B0 = RegisterValue; break;
-	case 0x3bc: pWb35Reg->U1BC_LEDConfigure = RegisterValue; break;
-	case 0x400: pWb35Reg->D00_DmaControl = RegisterValue; break;
-	case 0x800: pWb35Reg->M00_MacControl = RegisterValue; break;
-	case 0x804: pWb35Reg->M04_MulticastAddress1 = RegisterValue; break;
-	case 0x808: pWb35Reg->M08_MulticastAddress2 = RegisterValue; break;
-	case 0x824: pWb35Reg->M24_MacControl = RegisterValue; break;
-	case 0x828: pWb35Reg->M28_MacControl = RegisterValue; break;
-	case 0x82c: pWb35Reg->M2C_MacControl = RegisterValue; break;
-	case 0x838: pWb35Reg->M38_MacControl = RegisterValue; break;
-	case 0x840: pWb35Reg->M40_MacControl = RegisterValue; break;
-	case 0x844: pWb35Reg->M44_MacControl = RegisterValue; break;
-	case 0x848: pWb35Reg->M48_MacControl = RegisterValue; break;
-	case 0x84c: pWb35Reg->M4C_MacStatus = RegisterValue; break;
-	case 0x860: pWb35Reg->M60_MacControl = RegisterValue; break;
-	case 0x868: pWb35Reg->M68_MacControl = RegisterValue; break;
-	case 0x870: pWb35Reg->M70_MacControl = RegisterValue; break;
-	case 0x874: pWb35Reg->M74_MacControl = RegisterValue; break;
-	case 0x878: pWb35Reg->M78_ERPInformation = RegisterValue; break;
-	case 0x87C: pWb35Reg->M7C_MacControl = RegisterValue; break;
-	case 0x880: pWb35Reg->M80_MacControl = RegisterValue; break;
-	case 0x884: pWb35Reg->M84_MacControl = RegisterValue; break;
-	case 0x888: pWb35Reg->M88_MacControl = RegisterValue; break;
-	case 0x898: pWb35Reg->M98_MacControl = RegisterValue; break;
-	case 0x100c: pWb35Reg->BB0C = RegisterValue; break;
-	case 0x102c: pWb35Reg->BB2C = RegisterValue; break;
-	case 0x1030: pWb35Reg->BB30 = RegisterValue; break;
-	case 0x103c: pWb35Reg->BB3C = RegisterValue; break;
-	case 0x1048: pWb35Reg->BB48 = RegisterValue; break;
-	case 0x104c: pWb35Reg->BB4C = RegisterValue; break;
-	case 0x1050: pWb35Reg->BB50 = RegisterValue; break;
-	case 0x1054: pWb35Reg->BB54 = RegisterValue; break;
-	case 0x1058: pWb35Reg->BB58 = RegisterValue; break;
-	case 0x105c: pWb35Reg->BB5C = RegisterValue; break;
-	case 0x1060: pWb35Reg->BB60 = RegisterValue; break;
+	case 0x3b0: reg->U1B0 = RegisterValue; break;
+	case 0x3bc: reg->U1BC_LEDConfigure = RegisterValue; break;
+	case 0x400: reg->D00_DmaControl = RegisterValue; break;
+	case 0x800: reg->M00_MacControl = RegisterValue; break;
+	case 0x804: reg->M04_MulticastAddress1 = RegisterValue; break;
+	case 0x808: reg->M08_MulticastAddress2 = RegisterValue; break;
+	case 0x824: reg->M24_MacControl = RegisterValue; break;
+	case 0x828: reg->M28_MacControl = RegisterValue; break;
+	case 0x82c: reg->M2C_MacControl = RegisterValue; break;
+	case 0x838: reg->M38_MacControl = RegisterValue; break;
+	case 0x840: reg->M40_MacControl = RegisterValue; break;
+	case 0x844: reg->M44_MacControl = RegisterValue; break;
+	case 0x848: reg->M48_MacControl = RegisterValue; break;
+	case 0x84c: reg->M4C_MacStatus = RegisterValue; break;
+	case 0x860: reg->M60_MacControl = RegisterValue; break;
+	case 0x868: reg->M68_MacControl = RegisterValue; break;
+	case 0x870: reg->M70_MacControl = RegisterValue; break;
+	case 0x874: reg->M74_MacControl = RegisterValue; break;
+	case 0x878: reg->M78_ERPInformation = RegisterValue; break;
+	case 0x87C: reg->M7C_MacControl = RegisterValue; break;
+	case 0x880: reg->M80_MacControl = RegisterValue; break;
+	case 0x884: reg->M84_MacControl = RegisterValue; break;
+	case 0x888: reg->M88_MacControl = RegisterValue; break;
+	case 0x898: reg->M98_MacControl = RegisterValue; break;
+	case 0x100c: reg->BB0C = RegisterValue; break;
+	case 0x102c: reg->BB2C = RegisterValue; break;
+	case 0x1030: reg->BB30 = RegisterValue; break;
+	case 0x103c: reg->BB3C = RegisterValue; break;
+	case 0x1048: reg->BB48 = RegisterValue; break;
+	case 0x104c: reg->BB4C = RegisterValue; break;
+	case 0x1050: reg->BB50 = RegisterValue; break;
+	case 0x1054: reg->BB54 = RegisterValue; break;
+	case 0x1058: reg->BB58 = RegisterValue; break;
+	case 0x105c: reg->BB5C = RegisterValue; break;
+	case 0x1060: reg->BB60 = RegisterValue; break;
 	}
 }
 
-// TRUE  : read command process successfully
-// FALSE : register not support
+// true  : read command process successfully
+// false : register not support
 unsigned char
 Wb35Reg_WriteSync(  phw_data_t pHwData,  u16 RegisterNo,  u32 RegisterValue )
 {
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	int ret = -1;
 
 	// Module shutdown
 	if (pHwData->SurpriseRemove)
-		return FALSE;
+		return false;
 
 	RegisterValue = cpu_to_le32(RegisterValue);
 
 	// update the register by send usb message------------------------------------
-	pWb35Reg->SyncIoPause = 1;
+	reg->SyncIoPause = 1;
 
 	// 20060717.5 Wait until EP0VM stop
-	while (pWb35Reg->EP0vm_state != VM_STOP)
-		OS_SLEEP(10000);
+	while (reg->EP0vm_state != VM_STOP)
+		msleep(10);
 
 	// Sync IoCallDriver
-	pWb35Reg->EP0vm_state = VM_RUNNING;
+	reg->EP0vm_state = VM_RUNNING;
 	ret = usb_control_msg( pHwData->WbUsb.udev,
 			       usb_sndctrlpipe( pHwData->WbUsb.udev, 0 ),
 			       0x03, USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
 			       0x0,RegisterNo, &RegisterValue, 4, HZ*100 );
-	pWb35Reg->EP0vm_state = VM_STOP;
-	pWb35Reg->SyncIoPause = 0;
+	reg->EP0vm_state = VM_STOP;
+	reg->SyncIoPause = 0;
 
 	Wb35Reg_EP0VM_start(pHwData);
 
@@ -150,38 +153,38 @@ Wb35Reg_WriteSync(  phw_data_t pHwData,  u16 RegisterNo,  u32 RegisterValue )
 		#endif
 
 		pHwData->SurpriseRemove = 1; // 20060704.2
-		return FALSE;
+		return false;
 	}
 
-	return TRUE;
+	return true;
 }
 
-// TRUE  : read command process successfully
-// FALSE : register not support
+// true  : read command process successfully
+// false : register not support
 unsigned char
 Wb35Reg_Write(  phw_data_t pHwData,  u16 RegisterNo,  u32 RegisterValue )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	struct usb_ctrlrequest *dr;
-	PURB		pUrb = NULL;
-	PREG_QUEUE	pRegQueue = NULL;
+	struct urb	*urb = NULL;
+	struct wb35_reg_queue *reg_queue = NULL;
 	u16		UrbSize;
 
 
 	// Module shutdown
 	if (pHwData->SurpriseRemove)
-		return FALSE;
+		return false;
 
 	// update the register by send urb request------------------------------------
-	UrbSize = sizeof(REG_QUEUE) + sizeof(struct usb_ctrlrequest);
-	OS_MEMORY_ALLOC( (void* *)&pRegQueue, UrbSize );
-	pUrb = wb_usb_alloc_urb(0);
-	if (pUrb && pRegQueue) {
-		pRegQueue->DIRECT = 1;// burst write register
-		pRegQueue->INDEX = RegisterNo;
-		pRegQueue->VALUE = cpu_to_le32(RegisterValue);
-		pRegQueue->RESERVED_VALID = FALSE;
-		dr = (struct usb_ctrlrequest *)((u8 *)pRegQueue + sizeof(REG_QUEUE));
+	UrbSize = sizeof(struct wb35_reg_queue) + sizeof(struct usb_ctrlrequest);
+	reg_queue = kzalloc(UrbSize, GFP_ATOMIC);
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (urb && reg_queue) {
+		reg_queue->DIRECT = 1;// burst write register
+		reg_queue->INDEX = RegisterNo;
+		reg_queue->VALUE = cpu_to_le32(RegisterValue);
+		reg_queue->RESERVED_VALID = false;
+		dr = (struct usb_ctrlrequest *)((u8 *)reg_queue + sizeof(struct wb35_reg_queue));
 		dr->bRequestType = USB_TYPE_VENDOR|USB_DIR_OUT |USB_RECIP_DEVICE;
 		dr->bRequest = 0x03; // USB or vendor-defined request code, burst mode
 		dr->wValue = cpu_to_le16(0x0);
@@ -189,61 +192,61 @@ Wb35Reg_Write(  phw_data_t pHwData,  u16 RegisterNo,  u32 RegisterValue )
 		dr->wLength = cpu_to_le16(4);
 
 		// Enter the sending queue
-		pRegQueue->Next = NULL;
-		pRegQueue->pUsbReq = dr;
-		pRegQueue->pUrb = pUrb;
+		reg_queue->Next = NULL;
+		reg_queue->pUsbReq = dr;
+		reg_queue->urb = urb;
 
-		spin_lock_irq(&pWb35Reg->EP0VM_spin_lock );
-		if (pWb35Reg->pRegFirst == NULL)
-			pWb35Reg->pRegFirst = pRegQueue;
+		spin_lock_irq(&reg->EP0VM_spin_lock );
+		if (reg->reg_first == NULL)
+			reg->reg_first = reg_queue;
 		else
-			pWb35Reg->pRegLast->Next = pRegQueue;
-		pWb35Reg->pRegLast = pRegQueue;
+			reg->reg_last->Next = reg_queue;
+		reg->reg_last = reg_queue;
 
-		spin_unlock_irq( &pWb35Reg->EP0VM_spin_lock );
+		spin_unlock_irq( &reg->EP0VM_spin_lock );
 
 		// Start EP0VM
 		Wb35Reg_EP0VM_start(pHwData);
 
-		return TRUE;
+		return true;
 	} else {
-		if (pUrb)
-			usb_free_urb(pUrb);
-		kfree(pRegQueue);
-		return FALSE;
+		if (urb)
+			usb_free_urb(urb);
+		kfree(reg_queue);
+		return false;
 	}
 }
 
 //This command will be executed with a user defined value. When it completes,
 //this value is useful. For example, hal_set_current_channel will use it.
-// TRUE  : read command process successfully
-// FALSE : register not support
+// true  : read command process successfully
+// false : register not support
 unsigned char
 Wb35Reg_WriteWithCallbackValue( phw_data_t pHwData, u16 RegisterNo, u32 RegisterValue,
 				s8 *pValue, s8 Len)
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	struct usb_ctrlrequest *dr;
-	PURB		pUrb = NULL;
-	PREG_QUEUE	pRegQueue = NULL;
+	struct urb	*urb = NULL;
+	struct wb35_reg_queue *reg_queue = NULL;
 	u16		UrbSize;
 
 	// Module shutdown
 	if (pHwData->SurpriseRemove)
-		return FALSE;
+		return false;
 
 	// update the register by send urb request------------------------------------
-	UrbSize = sizeof(REG_QUEUE) + sizeof(struct usb_ctrlrequest);
-	OS_MEMORY_ALLOC((void* *) &pRegQueue, UrbSize );
-	pUrb = wb_usb_alloc_urb(0);
-	if (pUrb && pRegQueue) {
-		pRegQueue->DIRECT = 1;// burst write register
-		pRegQueue->INDEX = RegisterNo;
-		pRegQueue->VALUE = cpu_to_le32(RegisterValue);
+	UrbSize = sizeof(struct wb35_reg_queue) + sizeof(struct usb_ctrlrequest);
+	reg_queue = kzalloc(UrbSize, GFP_ATOMIC);
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (urb && reg_queue) {
+		reg_queue->DIRECT = 1;// burst write register
+		reg_queue->INDEX = RegisterNo;
+		reg_queue->VALUE = cpu_to_le32(RegisterValue);
 		//NOTE : Users must guarantee the size of value will not exceed the buffer size.
-		memcpy(pRegQueue->RESERVED, pValue, Len);
-		pRegQueue->RESERVED_VALID = TRUE;
-		dr = (struct usb_ctrlrequest *)((u8 *)pRegQueue + sizeof(REG_QUEUE));
+		memcpy(reg_queue->RESERVED, pValue, Len);
+		reg_queue->RESERVED_VALID = true;
+		dr = (struct usb_ctrlrequest *)((u8 *)reg_queue + sizeof(struct wb35_reg_queue));
 		dr->bRequestType = USB_TYPE_VENDOR|USB_DIR_OUT |USB_RECIP_DEVICE;
 		dr->bRequest = 0x03; // USB or vendor-defined request code, burst mode
 		dr->wValue = cpu_to_le16(0x0);
@@ -251,52 +254,52 @@ Wb35Reg_WriteWithCallbackValue( phw_data_t pHwData, u16 RegisterNo, u32 Register
 		dr->wLength = cpu_to_le16(4);
 
 		// Enter the sending queue
-		pRegQueue->Next = NULL;
-		pRegQueue->pUsbReq = dr;
-		pRegQueue->pUrb = pUrb;
-		spin_lock_irq (&pWb35Reg->EP0VM_spin_lock );
-		if( pWb35Reg->pRegFirst == NULL )
-			pWb35Reg->pRegFirst = pRegQueue;
+		reg_queue->Next = NULL;
+		reg_queue->pUsbReq = dr;
+		reg_queue->urb = urb;
+		spin_lock_irq (&reg->EP0VM_spin_lock );
+		if( reg->reg_first == NULL )
+			reg->reg_first = reg_queue;
 		else
-			pWb35Reg->pRegLast->Next = pRegQueue;
-		pWb35Reg->pRegLast = pRegQueue;
+			reg->reg_last->Next = reg_queue;
+		reg->reg_last = reg_queue;
 
-		spin_unlock_irq ( &pWb35Reg->EP0VM_spin_lock );
+		spin_unlock_irq ( &reg->EP0VM_spin_lock );
 
 		// Start EP0VM
 		Wb35Reg_EP0VM_start(pHwData);
-		return TRUE;
+		return true;
 	} else {
-		if (pUrb)
-			usb_free_urb(pUrb);
-		kfree(pRegQueue);
-		return FALSE;
+		if (urb)
+			usb_free_urb(urb);
+		kfree(reg_queue);
+		return false;
 	}
 }
 
-// TRUE  : read command process successfully
-// FALSE : register not support
+// true  : read command process successfully
+// false : register not support
 // pRegisterValue : It must be a resident buffer due to asynchronous read register.
 unsigned char
 Wb35Reg_ReadSync(  phw_data_t pHwData,  u16 RegisterNo,   u32 * pRegisterValue )
 {
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	u32 *	pltmp = pRegisterValue;
 	int ret = -1;
 
 	// Module shutdown
 	if (pHwData->SurpriseRemove)
-		return FALSE;
+		return false;
 
 	// Read the register by send usb message------------------------------------
 
-	pWb35Reg->SyncIoPause = 1;
+	reg->SyncIoPause = 1;
 
 	// 20060717.5 Wait until EP0VM stop
-	while (pWb35Reg->EP0vm_state != VM_STOP)
-		OS_SLEEP(10000);
+	while (reg->EP0vm_state != VM_STOP)
+		msleep(10);
 
-	pWb35Reg->EP0vm_state = VM_RUNNING;
+	reg->EP0vm_state = VM_RUNNING;
 	ret = usb_control_msg( pHwData->WbUsb.udev,
 			       usb_rcvctrlpipe(pHwData->WbUsb.udev, 0),
 			       0x01, USB_TYPE_VENDOR|USB_RECIP_DEVICE|USB_DIR_IN,
@@ -304,10 +307,10 @@ Wb35Reg_ReadSync(  phw_data_t pHwData,  u16 RegisterNo,   u32 * pRegisterValue )
 
 	*pRegisterValue = cpu_to_le32(*pltmp);
 
-	pWb35Reg->EP0vm_state = VM_STOP;
+	reg->EP0vm_state = VM_STOP;
 
 	Wb35Reg_Update( pHwData, RegisterNo, *pRegisterValue );
-	pWb35Reg->SyncIoPause = 0;
+	reg->SyncIoPause = 0;
 
 	Wb35Reg_EP0VM_start( pHwData );
 
@@ -317,38 +320,38 @@ Wb35Reg_ReadSync(  phw_data_t pHwData,  u16 RegisterNo,   u32 * pRegisterValue )
 		#endif
 
 		pHwData->SurpriseRemove = 1; // 20060704.2
-		return FALSE;
+		return false;
 	}
 
-	return TRUE;
+	return true;
 }
 
-// TRUE  : read command process successfully
-// FALSE : register not support
+// true  : read command process successfully
+// false : register not support
 // pRegisterValue : It must be a resident buffer due to asynchronous read register.
 unsigned char
 Wb35Reg_Read(phw_data_t pHwData, u16 RegisterNo,  u32 * pRegisterValue )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	struct usb_ctrlrequest * dr;
-	PURB		pUrb;
-	PREG_QUEUE	pRegQueue;
+	struct urb	*urb;
+	struct wb35_reg_queue *reg_queue;
 	u16		UrbSize;
 
 	// Module shutdown
 	if (pHwData->SurpriseRemove)
-		return FALSE;
+		return false;
 
 	// update the variable by send Urb to read register ------------------------------------
-	UrbSize = sizeof(REG_QUEUE) + sizeof(struct usb_ctrlrequest);
-	OS_MEMORY_ALLOC( (void* *)&pRegQueue, UrbSize );
-	pUrb = wb_usb_alloc_urb(0);
-	if( pUrb && pRegQueue )
+	UrbSize = sizeof(struct wb35_reg_queue) + sizeof(struct usb_ctrlrequest);
+	reg_queue = kzalloc(UrbSize, GFP_ATOMIC);
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if( urb && reg_queue )
 	{
-		pRegQueue->DIRECT = 0;// read register
-		pRegQueue->INDEX = RegisterNo;
-		pRegQueue->pBuffer = pRegisterValue;
-		dr = (struct usb_ctrlrequest *)((u8 *)pRegQueue + sizeof(REG_QUEUE));
+		reg_queue->DIRECT = 0;// read register
+		reg_queue->INDEX = RegisterNo;
+		reg_queue->pBuffer = pRegisterValue;
+		dr = (struct usb_ctrlrequest *)((u8 *)reg_queue + sizeof(struct wb35_reg_queue));
 		dr->bRequestType = USB_TYPE_VENDOR|USB_RECIP_DEVICE|USB_DIR_IN;
 		dr->bRequest = 0x01; // USB or vendor-defined request code, burst mode
 		dr->wValue = cpu_to_le16(0x0);
@@ -356,27 +359,27 @@ Wb35Reg_Read(phw_data_t pHwData, u16 RegisterNo,  u32 * pRegisterValue )
 		dr->wLength = cpu_to_le16 (4);
 
 		// Enter the sending queue
-		pRegQueue->Next = NULL;
-		pRegQueue->pUsbReq = dr;
-		pRegQueue->pUrb = pUrb;
-		spin_lock_irq ( &pWb35Reg->EP0VM_spin_lock );
-		if( pWb35Reg->pRegFirst == NULL )
-			pWb35Reg->pRegFirst = pRegQueue;
+		reg_queue->Next = NULL;
+		reg_queue->pUsbReq = dr;
+		reg_queue->urb = urb;
+		spin_lock_irq ( &reg->EP0VM_spin_lock );
+		if( reg->reg_first == NULL )
+			reg->reg_first = reg_queue;
 		else
-			pWb35Reg->pRegLast->Next = pRegQueue;
-		pWb35Reg->pRegLast = pRegQueue;
+			reg->reg_last->Next = reg_queue;
+		reg->reg_last = reg_queue;
 
-		spin_unlock_irq( &pWb35Reg->EP0VM_spin_lock );
+		spin_unlock_irq( &reg->EP0VM_spin_lock );
 
 		// Start EP0VM
 		Wb35Reg_EP0VM_start( pHwData );
 
-		return TRUE;
+		return true;
 	} else {
-		if (pUrb)
-			usb_free_urb( pUrb );
-		kfree(pRegQueue);
-		return FALSE;
+		if (urb)
+			usb_free_urb( urb );
+		kfree(reg_queue);
+		return false;
 	}
 }
 
@@ -384,57 +387,57 @@ Wb35Reg_Read(phw_data_t pHwData, u16 RegisterNo,  u32 * pRegisterValue )
 void
 Wb35Reg_EP0VM_start(  phw_data_t pHwData )
 {
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 
-	if (OS_ATOMIC_INC( pHwData->Adapter, &pWb35Reg->RegFireCount) == 1) {
-		pWb35Reg->EP0vm_state = VM_RUNNING;
+	if (atomic_inc_return(&reg->RegFireCount) == 1) {
+		reg->EP0vm_state = VM_RUNNING;
 		Wb35Reg_EP0VM(pHwData);
 	} else
-		OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Reg->RegFireCount );
+		atomic_dec(&reg->RegFireCount);
 }
 
 void
 Wb35Reg_EP0VM(phw_data_t pHwData )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-	PURB		pUrb;
+	struct wb35_reg *reg = &pHwData->reg;
+	struct urb	*urb;
 	struct usb_ctrlrequest *dr;
 	u32 *		pBuffer;
 	int			ret = -1;
-	PREG_QUEUE	pRegQueue;
+	struct wb35_reg_queue *reg_queue;
 
 
-	if (pWb35Reg->SyncIoPause)
+	if (reg->SyncIoPause)
 		goto cleanup;
 
 	if (pHwData->SurpriseRemove)
 		goto cleanup;
 
 	// Get the register data and send to USB through Irp
-	spin_lock_irq( &pWb35Reg->EP0VM_spin_lock );
-	pRegQueue = pWb35Reg->pRegFirst;
-	spin_unlock_irq( &pWb35Reg->EP0VM_spin_lock );
+	spin_lock_irq( &reg->EP0VM_spin_lock );
+	reg_queue = reg->reg_first;
+	spin_unlock_irq( &reg->EP0VM_spin_lock );
 
-	if (!pRegQueue)
+	if (!reg_queue)
 		goto cleanup;
 
 	// Get an Urb, send it
-	pUrb = (PURB)pRegQueue->pUrb;
+	urb = (struct urb *)reg_queue->urb;
 
-	dr = pRegQueue->pUsbReq;
-	pUrb = pRegQueue->pUrb;
-	pBuffer = pRegQueue->pBuffer;
-	if (pRegQueue->DIRECT == 1) // output
-		pBuffer = &pRegQueue->VALUE;
+	dr = reg_queue->pUsbReq;
+	urb = reg_queue->urb;
+	pBuffer = reg_queue->pBuffer;
+	if (reg_queue->DIRECT == 1) // output
+		pBuffer = &reg_queue->VALUE;
 
-	usb_fill_control_urb( pUrb, pHwData->WbUsb.udev,
-			      REG_DIRECTION(pHwData->WbUsb.udev,pRegQueue),
+	usb_fill_control_urb( urb, pHwData->WbUsb.udev,
+			      REG_DIRECTION(pHwData->WbUsb.udev,reg_queue),
 			      (u8 *)dr,pBuffer,cpu_to_le16(dr->wLength),
 			      Wb35Reg_EP0VM_complete, (void*)pHwData);
 
-	pWb35Reg->EP0vm_state = VM_RUNNING;
+	reg->EP0vm_state = VM_RUNNING;
 
-	ret = wb_usb_submit_urb( pUrb );
+	ret = usb_submit_urb(urb, GFP_ATOMIC);
 
 	if (ret < 0) {
 #ifdef _PE_REG_DUMP_
@@ -446,41 +449,41 @@ Wb35Reg_EP0VM(phw_data_t pHwData )
 	return;
 
  cleanup:
-	pWb35Reg->EP0vm_state = VM_STOP;
-	OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Reg->RegFireCount );
+	reg->EP0vm_state = VM_STOP;
+	atomic_dec(&reg->RegFireCount);
 }
 
 
 void
-Wb35Reg_EP0VM_complete(PURB pUrb)
+Wb35Reg_EP0VM_complete(struct urb *urb)
 {
-	phw_data_t  pHwData = (phw_data_t)pUrb->context;
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-	PREG_QUEUE	pRegQueue;
+	phw_data_t  pHwData = (phw_data_t)urb->context;
+	struct wb35_reg *reg = &pHwData->reg;
+	struct wb35_reg_queue *reg_queue;
 
 
 	// Variable setting
-	pWb35Reg->EP0vm_state = VM_COMPLETED;
-	pWb35Reg->EP0VM_status = pUrb->status;
+	reg->EP0vm_state = VM_COMPLETED;
+	reg->EP0VM_status = urb->status;
 
 	if (pHwData->SurpriseRemove) { // Let WbWlanHalt to handle surprise remove
-		pWb35Reg->EP0vm_state = VM_STOP;
-		OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Reg->RegFireCount );
+		reg->EP0vm_state = VM_STOP;
+		atomic_dec(&reg->RegFireCount);
 	} else {
 		// Complete to send, remove the URB from the first
-		spin_lock_irq( &pWb35Reg->EP0VM_spin_lock );
-		pRegQueue = pWb35Reg->pRegFirst;
-		if (pRegQueue == pWb35Reg->pRegLast)
-			pWb35Reg->pRegLast = NULL;
-		pWb35Reg->pRegFirst = pWb35Reg->pRegFirst->Next;
-		spin_unlock_irq( &pWb35Reg->EP0VM_spin_lock );
-
-		if (pWb35Reg->EP0VM_status) {
+		spin_lock_irq( &reg->EP0VM_spin_lock );
+		reg_queue = reg->reg_first;
+		if (reg_queue == reg->reg_last)
+			reg->reg_last = NULL;
+		reg->reg_first = reg->reg_first->Next;
+		spin_unlock_irq( &reg->EP0VM_spin_lock );
+
+		if (reg->EP0VM_status) {
 #ifdef _PE_REG_DUMP_
 			WBDEBUG(("EP0 IoCompleteRoutine return error\n"));
-			DebugUsbdStatusInformation( pWb35Reg->EP0VM_status );
+			DebugUsbdStatusInformation( reg->EP0VM_status );
 #endif
-			pWb35Reg->EP0vm_state = VM_STOP;
+			reg->EP0vm_state = VM_STOP;
 			pHwData->SurpriseRemove = 1;
 		} else {
 			// Success. Update the result
@@ -489,52 +492,52 @@ Wb35Reg_EP0VM_complete(PURB pUrb)
 			Wb35Reg_EP0VM(pHwData);
 		}
 
-   		kfree(pRegQueue);
+   		kfree(reg_queue);
 	}
 
-	usb_free_urb(pUrb);
+	usb_free_urb(urb);
 }
 
 
 void
 Wb35Reg_destroy(phw_data_t pHwData)
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-	PURB		pUrb;
-	PREG_QUEUE	pRegQueue;
+	struct wb35_reg *reg = &pHwData->reg;
+	struct urb	*urb;
+	struct wb35_reg_queue *reg_queue;
 
 
 	Uxx_power_off_procedure(pHwData);
 
 	// Wait for Reg operation completed
 	do {
-		OS_SLEEP(10000); // Delay for waiting function enter 940623.1.a
-	} while (pWb35Reg->EP0vm_state != VM_STOP);
-	OS_SLEEP(10000);  // Delay for waiting function enter 940623.1.b
+		msleep(10); // Delay for waiting function enter 940623.1.a
+	} while (reg->EP0vm_state != VM_STOP);
+	msleep(10);  // Delay for waiting function enter 940623.1.b
 
 	// Release all the data in RegQueue
-	spin_lock_irq( &pWb35Reg->EP0VM_spin_lock );
-	pRegQueue = pWb35Reg->pRegFirst;
-	while (pRegQueue) {
-		if (pRegQueue == pWb35Reg->pRegLast)
-			pWb35Reg->pRegLast = NULL;
-		pWb35Reg->pRegFirst = pWb35Reg->pRegFirst->Next;
-
-		pUrb = pRegQueue->pUrb;
-		spin_unlock_irq( &pWb35Reg->EP0VM_spin_lock );
-		if (pUrb) {
-			usb_free_urb(pUrb);
-			kfree(pRegQueue);
+	spin_lock_irq( &reg->EP0VM_spin_lock );
+	reg_queue = reg->reg_first;
+	while (reg_queue) {
+		if (reg_queue == reg->reg_last)
+			reg->reg_last = NULL;
+		reg->reg_first = reg->reg_first->Next;
+
+		urb = reg_queue->urb;
+		spin_unlock_irq( &reg->EP0VM_spin_lock );
+		if (urb) {
+			usb_free_urb(urb);
+			kfree(reg_queue);
 		} else {
 			#ifdef _PE_REG_DUMP_
 			WBDEBUG(("EP0 queue release error\n"));
 			#endif
 		}
-		spin_lock_irq( &pWb35Reg->EP0VM_spin_lock );
+		spin_lock_irq( &reg->EP0VM_spin_lock );
 
-		pRegQueue = pWb35Reg->pRegFirst;
+		reg_queue = reg->reg_first;
 	}
-	spin_unlock_irq( &pWb35Reg->EP0VM_spin_lock );
+	spin_unlock_irq( &reg->EP0VM_spin_lock );
 }
 
 //====================================================================================
@@ -542,35 +545,35 @@ Wb35Reg_destroy(phw_data_t pHwData)
 //====================================================================================
 unsigned char Wb35Reg_initial(phw_data_t pHwData)
 {
-	PWB35REG pWb35Reg=&pHwData->Wb35Reg;
+	struct wb35_reg *reg=&pHwData->reg;
 	u32 ltmp;
 	u32 SoftwareSet, VCO_trim, TxVga, Region_ScanInterval;
 
 	// Spin lock is acquired for read and write IRP command
-	spin_lock_init( &pWb35Reg->EP0VM_spin_lock );
+	spin_lock_init( &reg->EP0VM_spin_lock );
 
 	// Getting RF module type from EEPROM ------------------------------------
 	Wb35Reg_WriteSync( pHwData, 0x03b4, 0x080d0000 ); // Start EEPROM access + Read + address(0x0d)
 	Wb35Reg_ReadSync( pHwData, 0x03b4, &ltmp );
 
 	//Update RF module type and determine the PHY type by inf or EEPROM
-	pWb35Reg->EEPROMPhyType = (u8)( ltmp & 0xff );
+	reg->EEPROMPhyType = (u8)( ltmp & 0xff );
 	// 0 V MAX2825, 1 V MAX2827, 2 V MAX2828, 3 V MAX2829
 	// 16V AL2230, 17 - AL7230, 18 - AL2230S
 	// 32 Reserved
 	// 33 - W89RF242(TxVGA 0~19), 34 - W89RF242(TxVGA 0~34)
-	if (pWb35Reg->EEPROMPhyType != RF_DECIDE_BY_INF) {
-		if( (pWb35Reg->EEPROMPhyType == RF_MAXIM_2825)	||
-			(pWb35Reg->EEPROMPhyType == RF_MAXIM_2827)	||
-			(pWb35Reg->EEPROMPhyType == RF_MAXIM_2828)	||
-			(pWb35Reg->EEPROMPhyType == RF_MAXIM_2829)	||
-			(pWb35Reg->EEPROMPhyType == RF_MAXIM_V1)	||
-			(pWb35Reg->EEPROMPhyType == RF_AIROHA_2230)	||
-			(pWb35Reg->EEPROMPhyType == RF_AIROHA_2230S)    ||
-			(pWb35Reg->EEPROMPhyType == RF_AIROHA_7230)	||
-			(pWb35Reg->EEPROMPhyType == RF_WB_242)		||
-			(pWb35Reg->EEPROMPhyType == RF_WB_242_1))
-			pHwData->phy_type = pWb35Reg->EEPROMPhyType;
+	if (reg->EEPROMPhyType != RF_DECIDE_BY_INF) {
+		if( (reg->EEPROMPhyType == RF_MAXIM_2825)	||
+			(reg->EEPROMPhyType == RF_MAXIM_2827)	||
+			(reg->EEPROMPhyType == RF_MAXIM_2828)	||
+			(reg->EEPROMPhyType == RF_MAXIM_2829)	||
+			(reg->EEPROMPhyType == RF_MAXIM_V1)	||
+			(reg->EEPROMPhyType == RF_AIROHA_2230)	||
+			(reg->EEPROMPhyType == RF_AIROHA_2230S)    ||
+			(reg->EEPROMPhyType == RF_AIROHA_7230)	||
+			(reg->EEPROMPhyType == RF_WB_242)		||
+			(reg->EEPROMPhyType == RF_WB_242_1))
+			pHwData->phy_type = reg->EEPROMPhyType;
 	}
 
 	// Power On procedure running. The relative parameter will be set according to phy_type
@@ -606,9 +609,9 @@ unsigned char Wb35Reg_initial(phw_data_t pHwData)
 	if (pHwData->VCO_trim == 0xff)
 		pHwData->VCO_trim = 0x28;
 
-	pWb35Reg->EEPROMRegion = (u8)(Region_ScanInterval>>8); // 20060720
-	if( pWb35Reg->EEPROMRegion<1 || pWb35Reg->EEPROMRegion>6 )
-		pWb35Reg->EEPROMRegion = REGION_AUTO;
+	reg->EEPROMRegion = (u8)(Region_ScanInterval>>8); // 20060720
+	if( reg->EEPROMRegion<1 || reg->EEPROMRegion>6 )
+		reg->EEPROMRegion = REGION_AUTO;
 
 	//For Get Tx VGA from EEPROM 20060315.5 move here
 	GetTxVgaFromEEPROM( pHwData );
@@ -629,9 +632,9 @@ unsigned char Wb35Reg_initial(phw_data_t pHwData)
 	Dxx_initial(pHwData);
 
 	if (pHwData->SurpriseRemove)
-		return FALSE;
+		return false;
 	else
-		return TRUE; // Initial fail
+		return true; // Initial fail
 }
 
 //===================================================================================
diff --git a/drivers/staging/winbond/linux/wb35reg_f.h b/drivers/staging/winbond/wb35reg_f.h
index 3006cfe99ccd..d97215a1eba8 100644
--- a/drivers/staging/winbond/linux/wb35reg_f.h
+++ b/drivers/staging/winbond/wb35reg_f.h
@@ -1,3 +1,8 @@
+#ifndef __WINBOND_WB35REG_F_H
+#define __WINBOND_WB35REG_F_H
+
+#include "wbhal_s.h"
+
 //====================================
 // Interface function declare
 //====================================
@@ -42,7 +47,7 @@ unsigned char Wb35Reg_BurstWrite(  phw_data_t pHwData,  u16 RegisterNo,  u32 * p
 
 void Wb35Reg_EP0VM(  phw_data_t pHwData );
 void Wb35Reg_EP0VM_start(  phw_data_t pHwData );
-void Wb35Reg_EP0VM_complete(  PURB pUrb );
+void Wb35Reg_EP0VM_complete(struct urb *urb);
 
 u32 BitReverse( u32 dwData, u32 DataLength);
 
@@ -53,4 +58,4 @@ void Wb35Reg_phy_calibration(  phw_data_t pHwData );
 void Wb35Reg_Update(  phw_data_t pHwData,  u16 RegisterNo,  u32 RegisterValue );
 unsigned char adjust_TXVGA_for_iq_mag(  phw_data_t pHwData );
 
-
+#endif
diff --git a/drivers/staging/winbond/linux/wb35reg_s.h b/drivers/staging/winbond/wb35reg_s.h
index 8b35b93f7f02..32ef4b8a2d2a 100644
--- a/drivers/staging/winbond/linux/wb35reg_s.h
+++ b/drivers/staging/winbond/wb35reg_s.h
@@ -1,3 +1,10 @@
+#ifndef __WINBOND_WB35REG_S_H
+#define __WINBOND_WB35REG_S_H
+
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <asm/atomic.h>
+
 //=======================================================================================
 /*
 				HAL setting function
@@ -67,30 +74,25 @@
 #define DEFAULT_DTIM_ALERT_TIME			0
 
 
-typedef struct _REG_QUEUE
-{
-    struct  urb *pUrb;
-	void*	pUsbReq;
-	void*	Next;
-	union
-	{
+struct wb35_reg_queue {
+	struct urb 	*urb;
+	void		*pUsbReq;
+	void		*Next;
+	union {
 		u32	VALUE;
-		u32 *	pBuffer;
+		u32	*pBuffer;
 	};
-	u8	RESERVED[4];// space reserved for communication
-
-    u16	INDEX; // For storing the register index
-    u8	RESERVED_VALID;	//Indicate whether the RESERVED space is valid at this command.
-	u8	DIRECT; // 0:In   1:Out
-
-} REG_QUEUE, *PREG_QUEUE;
+	u8		RESERVED[4]; // space reserved for communication
+	u16		INDEX; // For storing the register index
+	u8		RESERVED_VALID;	// Indicate whether the RESERVED space is valid at this command.
+	u8		DIRECT; // 0:In   1:Out
+};
 
 //====================================
 // Internal variable for module
 //====================================
 #define MAX_SQ3_FILTER_SIZE		5
-typedef struct _WB35REG
-{
+struct wb35_reg {
 	//============================
 	// Register Bank backup
 	//============================
@@ -145,9 +147,9 @@ typedef struct _WB35REG
 	//-------------------
 	spinlock_t	EP0VM_spin_lock; // 4B
 	u32	        EP0VM_status;//$$
-	PREG_QUEUE	    pRegFirst;
-	PREG_QUEUE	    pRegLast;
-	OS_ATOMIC       RegFireCount;
+	struct wb35_reg_queue *reg_first;
+	struct wb35_reg_queue *reg_last;
+	atomic_t       RegFireCount;
 
 	// Hardware status
 	u8	EP0vm_state;
@@ -165,6 +167,6 @@ typedef struct _WB35REG
 	u32	SQ3_filter[MAX_SQ3_FILTER_SIZE];
 	u32	SQ3_index;
 
-} WB35REG, *PWB35REG;
-
+};
 
+#endif
diff --git a/drivers/staging/winbond/linux/wb35rx.c b/drivers/staging/winbond/wb35rx.c
index b4b9f5f371d9..7f8b6d749a47 100644
--- a/drivers/staging/winbond/linux/wb35rx.c
+++ b/drivers/staging/winbond/wb35rx.c
@@ -8,85 +8,160 @@
 //    Processing the Rx message from down layer
 //
 //============================================================================
+#include <linux/usb.h>
+
+#include "core.h"
 #include "sysdef.h"
+#include "wb35rx_f.h"
+
+static void packet_came(struct ieee80211_hw *hw, char *pRxBufferAddress, int PacketSize)
+{
+	struct wbsoft_priv *priv = hw->priv;
+	struct sk_buff *skb;
+	struct ieee80211_rx_status rx_status = {0};
+
+	if (!priv->enabled)
+		return;
+
+	skb = dev_alloc_skb(PacketSize);
+	if (!skb) {
+		printk("Not enough memory for packet, FIXME\n");
+		return;
+	}
 
+	memcpy(skb_put(skb, PacketSize),
+	       pRxBufferAddress,
+	       PacketSize);
 
-void Wb35Rx_start(phw_data_t pHwData)
+/*
+	rx_status.rate = 10;
+	rx_status.channel = 1;
+	rx_status.freq = 12345;
+	rx_status.phymode = MODE_IEEE80211B;
+*/
+
+	ieee80211_rx_irqsafe(hw, skb, &rx_status);
+}
+
+static void Wb35Rx_adjust(PDESCRIPTOR pRxDes)
 {
-	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
+	u32 *	pRxBufferAddress;
+	u32	DecryptionMethod;
+	u32	i;
+	u16	BufferSize;
 
-	// Allow only one thread to run into the Wb35Rx() function
-	if (OS_ATOMIC_INC(pHwData->Adapter, &pWb35Rx->RxFireCounter) == 1) {
-		pWb35Rx->EP3vm_state = VM_RUNNING;
-		Wb35Rx(pHwData);
-	} else
-		OS_ATOMIC_DEC(pHwData->Adapter, &pWb35Rx->RxFireCounter);
+	DecryptionMethod = pRxDes->R01.R01_decryption_method;
+	pRxBufferAddress = pRxDes->buffer_address[0];
+	BufferSize = pRxDes->buffer_size[0];
+
+	// Adjust the last part of data. Only data left
+	BufferSize -= 4; // For CRC-32
+	if (DecryptionMethod)
+		BufferSize -= 4;
+	if (DecryptionMethod == 3) // For CCMP
+		BufferSize -= 4;
+
+	// Adjust the IV field which after 802.11 header and ICV field.
+	if (DecryptionMethod == 1) // For WEP
+	{
+		for( i=6; i>0; i-- )
+			pRxBufferAddress[i] = pRxBufferAddress[i-1];
+		pRxDes->buffer_address[0] = pRxBufferAddress + 1;
+		BufferSize -= 4; // 4 byte for IV
+	}
+	else if( DecryptionMethod ) // For TKIP and CCMP
+	{
+		for (i=7; i>1; i--)
+			pRxBufferAddress[i] = pRxBufferAddress[i-2];
+		pRxDes->buffer_address[0] = pRxBufferAddress + 2;//Update the descriptor, shift 8 byte
+		BufferSize -= 8; // 8 byte for IV + ICV
+	}
+	pRxDes->buffer_size[0] = BufferSize;
 }
 
-// This function cannot reentrain
-void Wb35Rx(  phw_data_t pHwData )
+static u16 Wb35Rx_indicate(struct ieee80211_hw *hw)
 {
+	struct wbsoft_priv *priv = hw->priv;
+	phw_data_t pHwData = &priv->sHwData;
+	DESCRIPTOR	RxDes;
 	PWB35RX	pWb35Rx = &pHwData->Wb35Rx;
-	u8 *	pRxBufferAddress;
-	PURB	pUrb = (PURB)pWb35Rx->RxUrb;
-	int	retv;
-	u32	RxBufferId;
+	u8 *		pRxBufferAddress;
+	u16		PacketSize;
+	u16		stmp, BufferSize, stmp2 = 0;
+	u32		RxBufferId;
 
-	//
-	// Issuing URB
-	//
-	if (pHwData->SurpriseRemove || pHwData->HwStop)
-		goto error;
+	// Only one thread be allowed to run into the following
+	do {
+		RxBufferId = pWb35Rx->RxProcessIndex;
+		if (pWb35Rx->RxOwner[ RxBufferId ]) //Owner by VM
+			break;
 
-	if (pWb35Rx->rx_halt)
-		goto error;
+		pWb35Rx->RxProcessIndex++;
+		pWb35Rx->RxProcessIndex %= MAX_USB_RX_BUFFER_NUMBER;
 
-	// Get RxBuffer's ID
-	RxBufferId = pWb35Rx->RxBufferId;
-	if (!pWb35Rx->RxOwner[RxBufferId]) {
-		// It's impossible to run here.
-		#ifdef _PE_RX_DUMP_
-		WBDEBUG(("Rx driver fifo unavailable\n"));
-		#endif
-		goto error;
-	}
+		pRxBufferAddress = pWb35Rx->pDRx;
+		BufferSize = pWb35Rx->RxBufferSize[ RxBufferId ];
 
-	// Update buffer point, then start to bulkin the data from USB
-	pWb35Rx->RxBufferId++;
-	pWb35Rx->RxBufferId %= MAX_USB_RX_BUFFER_NUMBER;
+		// Parse the bulkin buffer
+		while (BufferSize >= 4) {
+			if ((cpu_to_le32(*(u32 *)pRxBufferAddress) & 0x0fffffff) == RX_END_TAG) //Is ending? 921002.9.a
+				break;
 
-	pWb35Rx->CurrentRxBufferId = RxBufferId;
+			// Get the R00 R01 first
+			RxDes.R00.value = le32_to_cpu(*(u32 *)pRxBufferAddress);
+			PacketSize = (u16)RxDes.R00.R00_receive_byte_count;
+			RxDes.R01.value = le32_to_cpu(*((u32 *)(pRxBufferAddress+4)));
+			// For new DMA 4k
+			if ((PacketSize & 0x03) > 0)
+				PacketSize -= 4;
 
-	if (1 != OS_MEMORY_ALLOC((void* *)&pWb35Rx->pDRx, MAX_USB_RX_BUFFER)) {
-		printk("w35und: Rx memory alloc failed\n");
-		goto error;
-	}
-	pRxBufferAddress = pWb35Rx->pDRx;
+			// Basic check for Rx length. Is length valid?
+			if (PacketSize > MAX_PACKET_SIZE) {
+				#ifdef _PE_RX_DUMP_
+				WBDEBUG(("Serious ERROR : Rx data size too long, size =%d\n", PacketSize));
+				#endif
 
-	usb_fill_bulk_urb(pUrb, pHwData->WbUsb.udev,
-			  usb_rcvbulkpipe(pHwData->WbUsb.udev, 3),
-			  pRxBufferAddress, MAX_USB_RX_BUFFER,
-			  Wb35Rx_Complete, pHwData);
+				pWb35Rx->EP3vm_state = VM_STOP;
+				pWb35Rx->Ep3ErrorCount2++;
+				break;
+			}
 
-	pWb35Rx->EP3vm_state = VM_RUNNING;
+			// Start to process Rx buffer
+//			RxDes.Descriptor_ID = RxBufferId; // Due to synchronous indicate, the field doesn't necessary to use.
+			BufferSize -= 8; //subtract 8 byte for 35's USB header length
+			pRxBufferAddress += 8;
 
-	retv = wb_usb_submit_urb(pUrb);
+			RxDes.buffer_address[0] = pRxBufferAddress;
+			RxDes.buffer_size[0] = PacketSize;
+			RxDes.buffer_number = 1;
+			RxDes.buffer_start_index = 0;
+			RxDes.buffer_total_size = RxDes.buffer_size[0];
+			Wb35Rx_adjust(&RxDes);
 
-	if (retv != 0) {
-		printk("Rx URB sending error\n");
-		goto error;
-	}
-	return;
+			packet_came(hw, pRxBufferAddress, PacketSize);
 
-error:
-	// VM stop
-	pWb35Rx->EP3vm_state = VM_STOP;
-	OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Rx->RxFireCounter );
+			// Move RxBuffer point to the next
+			stmp = PacketSize + 3;
+			stmp &= ~0x03; // 4n alignment
+			pRxBufferAddress += stmp;
+			BufferSize -= stmp;
+			stmp2 += stmp;
+		}
+
+		// Reclaim resource
+		pWb35Rx->RxOwner[ RxBufferId ] = 1;
+	} while (true);
+
+	return stmp2;
 }
 
-void Wb35Rx_Complete(PURB pUrb)
+static void Wb35Rx(struct ieee80211_hw *hw);
+
+static void Wb35Rx_Complete(struct urb *urb)
 {
-	phw_data_t	pHwData = pUrb->context;
+	struct ieee80211_hw *hw = urb->context;
+	struct wbsoft_priv *priv = hw->priv;
+	phw_data_t pHwData = &priv->sHwData;
 	PWB35RX		pWb35Rx = &pHwData->Wb35Rx;
 	u8 *		pRxBufferAddress;
 	u32		SizeCheck;
@@ -96,12 +171,12 @@ void Wb35Rx_Complete(PURB pUrb)
 
 	// Variable setting
 	pWb35Rx->EP3vm_state = VM_COMPLETED;
-	pWb35Rx->EP3VM_status = pUrb->status;//Store the last result of Irp
+	pWb35Rx->EP3VM_status = urb->status;//Store the last result of Irp
 
 	RxBufferId = pWb35Rx->CurrentRxBufferId;
 
 	pRxBufferAddress = pWb35Rx->pDRx;
-	BulkLength = (u16)pUrb->actual_length;
+	BulkLength = (u16)urb->actual_length;
 
 	// The IRP is completed
 	pWb35Rx->EP3vm_state = VM_COMPLETED;
@@ -147,62 +222,99 @@ void Wb35Rx_Complete(PURB pUrb)
 	pWb35Rx->RxBufferSize[ RxBufferId ] = BulkLength;
 
 	if (!pWb35Rx->RxOwner[ RxBufferId ])
-		Wb35Rx_indicate(pHwData);
+		Wb35Rx_indicate(hw);
 
 	kfree(pWb35Rx->pDRx);
 	// Do the next receive
-	Wb35Rx(pHwData);
+	Wb35Rx(hw);
 	return;
 
 error:
 	pWb35Rx->RxOwner[ RxBufferId ] = 1; // Set the owner to hardware
-	OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Rx->RxFireCounter );
+	atomic_dec(&pWb35Rx->RxFireCounter);
 	pWb35Rx->EP3vm_state = VM_STOP;
 }
 
-//=====================================================================================
-unsigned char Wb35Rx_initial(phw_data_t pHwData)
+// This function cannot reentrain
+static void Wb35Rx(struct ieee80211_hw *hw)
 {
-	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
-
-	// Initial the Buffer Queue
-	Wb35Rx_reset_descriptor( pHwData );
+	struct wbsoft_priv *priv = hw->priv;
+	phw_data_t pHwData = &priv->sHwData;
+	PWB35RX	pWb35Rx = &pHwData->Wb35Rx;
+	u8 *	pRxBufferAddress;
+	struct urb *urb = pWb35Rx->RxUrb;
+	int	retv;
+	u32	RxBufferId;
 
-	pWb35Rx->RxUrb = wb_usb_alloc_urb(0);
-	return (!!pWb35Rx->RxUrb);
-}
+	//
+	// Issuing URB
+	//
+	if (pHwData->SurpriseRemove || pHwData->HwStop)
+		goto error;
 
-void Wb35Rx_stop(phw_data_t pHwData)
-{
-	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
+	if (pWb35Rx->rx_halt)
+		goto error;
 
-	// Canceling the Irp if already sends it out.
-	if (pWb35Rx->EP3vm_state == VM_RUNNING) {
-		usb_unlink_urb( pWb35Rx->RxUrb ); // Only use unlink, let Wb35Rx_destroy to free them
+	// Get RxBuffer's ID
+	RxBufferId = pWb35Rx->RxBufferId;
+	if (!pWb35Rx->RxOwner[RxBufferId]) {
+		// It's impossible to run here.
 		#ifdef _PE_RX_DUMP_
-		WBDEBUG(("EP3 Rx stop\n"));
+		WBDEBUG(("Rx driver fifo unavailable\n"));
 		#endif
+		goto error;
+	}
+
+	// Update buffer point, then start to bulkin the data from USB
+	pWb35Rx->RxBufferId++;
+	pWb35Rx->RxBufferId %= MAX_USB_RX_BUFFER_NUMBER;
+
+	pWb35Rx->CurrentRxBufferId = RxBufferId;
+
+	pWb35Rx->pDRx = kzalloc(MAX_USB_RX_BUFFER, GFP_ATOMIC);
+	if (!pWb35Rx->pDRx) {
+		printk("w35und: Rx memory alloc failed\n");
+		goto error;
+	}
+	pRxBufferAddress = pWb35Rx->pDRx;
+
+	usb_fill_bulk_urb(urb, pHwData->WbUsb.udev,
+			  usb_rcvbulkpipe(pHwData->WbUsb.udev, 3),
+			  pRxBufferAddress, MAX_USB_RX_BUFFER,
+			  Wb35Rx_Complete, hw);
+
+	pWb35Rx->EP3vm_state = VM_RUNNING;
+
+	retv = usb_submit_urb(urb, GFP_ATOMIC);
+
+	if (retv != 0) {
+		printk("Rx URB sending error\n");
+		goto error;
 	}
+	return;
+
+error:
+	// VM stop
+	pWb35Rx->EP3vm_state = VM_STOP;
+	atomic_dec(&pWb35Rx->RxFireCounter);
 }
 
-// Needs process context
-void Wb35Rx_destroy(phw_data_t pHwData)
+void Wb35Rx_start(struct ieee80211_hw *hw)
 {
+	struct wbsoft_priv *priv = hw->priv;
+	phw_data_t pHwData = &priv->sHwData;
 	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
 
-	do {
-		OS_SLEEP(10000); // Delay for waiting function enter 940623.1.a
-	} while (pWb35Rx->EP3vm_state != VM_STOP);
-	OS_SLEEP(10000); // Delay for waiting function exit 940623.1.b
-
-	if (pWb35Rx->RxUrb)
-		usb_free_urb( pWb35Rx->RxUrb );
-	#ifdef _PE_RX_DUMP_
-	WBDEBUG(("Wb35Rx_destroy OK\n"));
-	#endif
+	// Allow only one thread to run into the Wb35Rx() function
+	if (atomic_inc_return(&pWb35Rx->RxFireCounter) == 1) {
+		pWb35Rx->EP3vm_state = VM_RUNNING;
+		Wb35Rx(hw);
+	} else
+		atomic_dec(&pWb35Rx->RxFireCounter);
 }
 
-void Wb35Rx_reset_descriptor(  phw_data_t pHwData )
+//=====================================================================================
+static void Wb35Rx_reset_descriptor(  phw_data_t pHwData )
 {
 	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
 	u32	i;
@@ -218,117 +330,44 @@ void Wb35Rx_reset_descriptor(  phw_data_t pHwData )
 		pWb35Rx->RxOwner[i] = 1;
 }
 
-void Wb35Rx_adjust(PDESCRIPTOR pRxDes)
+unsigned char Wb35Rx_initial(phw_data_t pHwData)
 {
-	u32 *	pRxBufferAddress;
-	u32	DecryptionMethod;
-	u32	i;
-	u16	BufferSize;
-
-	DecryptionMethod = pRxDes->R01.R01_decryption_method;
-	pRxBufferAddress = pRxDes->buffer_address[0];
-	BufferSize = pRxDes->buffer_size[0];
+	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
 
-	// Adjust the last part of data. Only data left
-	BufferSize -= 4; // For CRC-32
-	if (DecryptionMethod)
-		BufferSize -= 4;
-	if (DecryptionMethod == 3) // For CCMP
-		BufferSize -= 4;
+	// Initial the Buffer Queue
+	Wb35Rx_reset_descriptor( pHwData );
 
-	// Adjust the IV field which after 802.11 header and ICV field.
-	if (DecryptionMethod == 1) // For WEP
-	{
-		for( i=6; i>0; i-- )
-			pRxBufferAddress[i] = pRxBufferAddress[i-1];
-		pRxDes->buffer_address[0] = pRxBufferAddress + 1;
-		BufferSize -= 4; // 4 byte for IV
-	}
-	else if( DecryptionMethod ) // For TKIP and CCMP
-	{
-		for (i=7; i>1; i--)
-			pRxBufferAddress[i] = pRxBufferAddress[i-2];
-		pRxDes->buffer_address[0] = pRxBufferAddress + 2;//Update the descriptor, shift 8 byte
-		BufferSize -= 8; // 8 byte for IV + ICV
-	}
-	pRxDes->buffer_size[0] = BufferSize;
+	pWb35Rx->RxUrb = usb_alloc_urb(0, GFP_ATOMIC);
+	return (!!pWb35Rx->RxUrb);
 }
 
-extern void packet_came(char *pRxBufferAddress, int PacketSize);
+void Wb35Rx_stop(phw_data_t pHwData)
+{
+	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
 
+	// Canceling the Irp if already sends it out.
+	if (pWb35Rx->EP3vm_state == VM_RUNNING) {
+		usb_unlink_urb( pWb35Rx->RxUrb ); // Only use unlink, let Wb35Rx_destroy to free them
+		#ifdef _PE_RX_DUMP_
+		WBDEBUG(("EP3 Rx stop\n"));
+		#endif
+	}
+}
 
-u16 Wb35Rx_indicate(phw_data_t pHwData)
+// Needs process context
+void Wb35Rx_destroy(phw_data_t pHwData)
 {
-	DESCRIPTOR	RxDes;
-	PWB35RX	pWb35Rx = &pHwData->Wb35Rx;
-	u8 *		pRxBufferAddress;
-	u16		PacketSize;
-	u16		stmp, BufferSize, stmp2 = 0;
-	u32		RxBufferId;
+	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
 
-	// Only one thread be allowed to run into the following
 	do {
-		RxBufferId = pWb35Rx->RxProcessIndex;
-		if (pWb35Rx->RxOwner[ RxBufferId ]) //Owner by VM
-			break;
-
-		pWb35Rx->RxProcessIndex++;
-		pWb35Rx->RxProcessIndex %= MAX_USB_RX_BUFFER_NUMBER;
-
-		pRxBufferAddress = pWb35Rx->pDRx;
-		BufferSize = pWb35Rx->RxBufferSize[ RxBufferId ];
-
-		// Parse the bulkin buffer
-		while (BufferSize >= 4) {
-			if ((cpu_to_le32(*(u32 *)pRxBufferAddress) & 0x0fffffff) == RX_END_TAG) //Is ending? 921002.9.a
-				break;
-
-			// Get the R00 R01 first
-			RxDes.R00.value = le32_to_cpu(*(u32 *)pRxBufferAddress);
-			PacketSize = (u16)RxDes.R00.R00_receive_byte_count;
-			RxDes.R01.value = le32_to_cpu(*((u32 *)(pRxBufferAddress+4)));
-			// For new DMA 4k
-			if ((PacketSize & 0x03) > 0)
-				PacketSize -= 4;
-
-			// Basic check for Rx length. Is length valid?
-			if (PacketSize > MAX_PACKET_SIZE) {
-				#ifdef _PE_RX_DUMP_
-				WBDEBUG(("Serious ERROR : Rx data size too long, size =%d\n", PacketSize));
-				#endif
-
-				pWb35Rx->EP3vm_state = VM_STOP;
-				pWb35Rx->Ep3ErrorCount2++;
-				break;
-			}
-
-			// Start to process Rx buffer
-//			RxDes.Descriptor_ID = RxBufferId; // Due to synchronous indicate, the field doesn't necessary to use.
-			BufferSize -= 8; //subtract 8 byte for 35's USB header length
-			pRxBufferAddress += 8;
-
-			RxDes.buffer_address[0] = pRxBufferAddress;
-			RxDes.buffer_size[0] = PacketSize;
-			RxDes.buffer_number = 1;
-			RxDes.buffer_start_index = 0;
-			RxDes.buffer_total_size = RxDes.buffer_size[0];
-			Wb35Rx_adjust(&RxDes);
-
-			packet_came(pRxBufferAddress, PacketSize);
-
-			// Move RxBuffer point to the next
-			stmp = PacketSize + 3;
-			stmp &= ~0x03; // 4n alignment
-			pRxBufferAddress += stmp;
-			BufferSize -= stmp;
-			stmp2 += stmp;
-		}
-
-		// Reclaim resource
-		pWb35Rx->RxOwner[ RxBufferId ] = 1;
-	} while(TRUE);
+		msleep(10); // Delay for waiting function enter 940623.1.a
+	} while (pWb35Rx->EP3vm_state != VM_STOP);
+	msleep(10); // Delay for waiting function exit 940623.1.b
 
-	return stmp2;
+	if (pWb35Rx->RxUrb)
+		usb_free_urb( pWb35Rx->RxUrb );
+	#ifdef _PE_RX_DUMP_
+	WBDEBUG(("Wb35Rx_destroy OK\n"));
+	#endif
 }
 
-
diff --git a/drivers/staging/winbond/wb35rx_f.h b/drivers/staging/winbond/wb35rx_f.h
new file mode 100644
index 000000000000..d993041e0cdd
--- /dev/null
+++ b/drivers/staging/winbond/wb35rx_f.h
@@ -0,0 +1,15 @@
+#ifndef __WINBOND_WB35RX_F_H
+#define __WINBOND_WB35RX_F_H
+
+#include <net/mac80211.h>
+#include "wbhal_s.h"
+
+//====================================
+// Interface function declare
+//====================================
+unsigned char		Wb35Rx_initial(  phw_data_t pHwData );
+void		Wb35Rx_destroy(  phw_data_t pHwData );
+void		Wb35Rx_stop(  phw_data_t pHwData );
+void		Wb35Rx_start(struct ieee80211_hw *hw);
+
+#endif
diff --git a/drivers/staging/winbond/linux/wb35rx_s.h b/drivers/staging/winbond/wb35rx_s.h
index b90c269e6adb..f18350b41c44 100644
--- a/drivers/staging/winbond/linux/wb35rx_s.h
+++ b/drivers/staging/winbond/wb35rx_s.h
@@ -18,7 +18,7 @@
 typedef struct _WB35RX
 {
 	u32			ByteReceived;// For calculating throughput of BulkIn
-	OS_ATOMIC		RxFireCounter;// Does Wb35Rx module fire?
+	atomic_t		RxFireCounter;// Does Wb35Rx module fire?
 
 	u8	RxBuffer[ MAX_USB_RX_BUFFER_NUMBER ][ ((MAX_USB_RX_BUFFER+3) & ~0x03 ) ];
 	u16	RxBufferSize[ ((MAX_USB_RX_BUFFER_NUMBER+1) & ~0x01) ];
diff --git a/drivers/staging/winbond/linux/wb35tx.c b/drivers/staging/winbond/wb35tx.c
index ba9d51244e29..b9b4456c8082 100644
--- a/drivers/staging/winbond/linux/wb35tx.c
+++ b/drivers/staging/winbond/wb35tx.c
@@ -8,8 +8,11 @@
 //    Processing the Tx message and put into down layer
 //
 //============================================================================
-#include "sysdef.h"
+#include <linux/usb.h>
 
+#include "wb35tx_f.h"
+#include "mds_f.h"
+#include "sysdef.h"
 
 unsigned char
 Wb35Tx_get_tx_buffer(phw_data_t pHwData, u8 **pBuffer)
@@ -17,28 +20,54 @@ Wb35Tx_get_tx_buffer(phw_data_t pHwData, u8 **pBuffer)
 	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
 
 	*pBuffer = pWb35Tx->TxBuffer[0];
-	return TRUE;
+	return true;
 }
 
-void Wb35Tx_start(phw_data_t pHwData)
+static void Wb35Tx(struct wbsoft_priv *adapter);
+
+static void Wb35Tx_complete(struct urb * pUrb)
 {
-	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
+	struct wbsoft_priv *adapter = pUrb->context;
+	phw_data_t	pHwData = &adapter->sHwData;
+	PWB35TX		pWb35Tx = &pHwData->Wb35Tx;
+	PMDS		pMds = &adapter->Mds;
 
-	// Allow only one thread to run into function
-	if (OS_ATOMIC_INC(pHwData->Adapter, &pWb35Tx->TxFireCounter) == 1) {
-		pWb35Tx->EP4vm_state = VM_RUNNING;
-		Wb35Tx(pHwData);
-	} else
-		OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Tx->TxFireCounter );
-}
+	printk("wb35: tx complete\n");
+	// Variable setting
+	pWb35Tx->EP4vm_state = VM_COMPLETED;
+	pWb35Tx->EP4VM_status = pUrb->status; //Store the last result of Irp
+	pMds->TxOwner[ pWb35Tx->TxSendIndex ] = 0;// Set the owner. Free the owner bit always.
+	pWb35Tx->TxSendIndex++;
+	pWb35Tx->TxSendIndex %= MAX_USB_TX_BUFFER_NUMBER;
 
+	if (pHwData->SurpriseRemove || pHwData->HwStop) // Let WbWlanHalt to handle surprise remove
+		goto error;
 
-void Wb35Tx(phw_data_t pHwData)
+	if (pWb35Tx->tx_halt)
+		goto error;
+
+	// The URB is completed, check the result
+	if (pWb35Tx->EP4VM_status != 0) {
+		printk("URB submission failed\n");
+		pWb35Tx->EP4vm_state = VM_STOP;
+		goto error;
+	}
+
+	Mds_Tx(adapter);
+	Wb35Tx(adapter);
+	return;
+
+error:
+	atomic_dec(&pWb35Tx->TxFireCounter);
+	pWb35Tx->EP4vm_state = VM_STOP;
+}
+
+static void Wb35Tx(struct wbsoft_priv *adapter)
 {
+	phw_data_t	pHwData = &adapter->sHwData;
 	PWB35TX		pWb35Tx = &pHwData->Wb35Tx;
-	PADAPTER	Adapter = pHwData->Adapter;
 	u8		*pTxBufferAddress;
-	PMDS		pMds = &Adapter->Mds;
+	PMDS		pMds = &adapter->Mds;
 	struct urb *	pUrb = (struct urb *)pWb35Tx->Tx4Urb;
 	int         	retv;
 	u32		SendIndex;
@@ -62,10 +91,10 @@ void Wb35Tx(phw_data_t pHwData)
 	usb_fill_bulk_urb(pUrb, pHwData->WbUsb.udev,
 			  usb_sndbulkpipe(pHwData->WbUsb.udev, 4),
 			  pTxBufferAddress, pMds->TxBufferSize[ SendIndex ],
-			  Wb35Tx_complete, pHwData);
+			  Wb35Tx_complete, adapter);
 
 	pWb35Tx->EP4vm_state = VM_RUNNING;
-	retv = wb_usb_submit_urb( pUrb );
+	retv = usb_submit_urb(pUrb, GFP_ATOMIC);
 	if (retv<0) {
 		printk("EP4 Tx Irp sending error\n");
 		goto cleanup;
@@ -74,78 +103,45 @@ void Wb35Tx(phw_data_t pHwData)
 	// Check if driver needs issue Irp for EP2
 	pWb35Tx->TxFillCount += pMds->TxCountInBuffer[SendIndex];
 	if (pWb35Tx->TxFillCount > 12)
-		Wb35Tx_EP2VM_start( pHwData );
+		Wb35Tx_EP2VM_start(adapter);
 
 	pWb35Tx->ByteTransfer += pMds->TxBufferSize[SendIndex];
 	return;
 
  cleanup:
 	pWb35Tx->EP4vm_state = VM_STOP;
-	OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Tx->TxFireCounter );
-}
-
-
-void Wb35Tx_complete(struct urb * pUrb)
-{
-	phw_data_t	pHwData = pUrb->context;
-	PADAPTER	Adapter = (PADAPTER)pHwData->Adapter;
-	PWB35TX		pWb35Tx = &pHwData->Wb35Tx;
-	PMDS		pMds = &Adapter->Mds;
-
-	printk("wb35: tx complete\n");
-	// Variable setting
-	pWb35Tx->EP4vm_state = VM_COMPLETED;
-	pWb35Tx->EP4VM_status = pUrb->status; //Store the last result of Irp
-	pMds->TxOwner[ pWb35Tx->TxSendIndex ] = 0;// Set the owner. Free the owner bit always.
-	pWb35Tx->TxSendIndex++;
-	pWb35Tx->TxSendIndex %= MAX_USB_TX_BUFFER_NUMBER;
-
-	if (pHwData->SurpriseRemove || pHwData->HwStop) // Let WbWlanHalt to handle surprise remove
-		goto error;
-
-	if (pWb35Tx->tx_halt)
-		goto error;
-
-	// The URB is completed, check the result
-	if (pWb35Tx->EP4VM_status != 0) {
-		printk("URB submission failed\n");
-		pWb35Tx->EP4vm_state = VM_STOP;
-		goto error;
-	}
-
-	Mds_Tx(Adapter);
-	Wb35Tx(pHwData);
-	return;
-
-error:
-	OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Tx->TxFireCounter );
-	pWb35Tx->EP4vm_state = VM_STOP;
+	atomic_dec(&pWb35Tx->TxFireCounter);
 }
 
-void Wb35Tx_reset_descriptor(  phw_data_t pHwData )
+void Wb35Tx_start(struct wbsoft_priv *adapter)
 {
+	phw_data_t pHwData = &adapter->sHwData;
 	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
 
-	pWb35Tx->TxSendIndex = 0;
-	pWb35Tx->tx_halt = 0;
+	// Allow only one thread to run into function
+	if (atomic_inc_return(&pWb35Tx->TxFireCounter) == 1) {
+		pWb35Tx->EP4vm_state = VM_RUNNING;
+		Wb35Tx(adapter);
+	} else
+		atomic_dec(&pWb35Tx->TxFireCounter);
 }
 
 unsigned char Wb35Tx_initial(phw_data_t pHwData)
 {
 	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
 
-	pWb35Tx->Tx4Urb = wb_usb_alloc_urb(0);
+	pWb35Tx->Tx4Urb = usb_alloc_urb(0, GFP_ATOMIC);
 	if (!pWb35Tx->Tx4Urb)
-		return FALSE;
+		return false;
 
-	pWb35Tx->Tx2Urb = wb_usb_alloc_urb(0);
+	pWb35Tx->Tx2Urb = usb_alloc_urb(0, GFP_ATOMIC);
 	if (!pWb35Tx->Tx2Urb)
 	{
 		usb_free_urb( pWb35Tx->Tx4Urb );
-		return FALSE;
+		return false;
 	}
 
-	return TRUE;
+	return true;
 }
 
 //======================================================
@@ -175,9 +171,9 @@ void Wb35Tx_destroy(phw_data_t pHwData)
 
 	// Wait for VM stop
 	do {
-		OS_SLEEP(10000);  // Delay for waiting function enter 940623.1.a
+		msleep(10);  // Delay for waiting function enter 940623.1.a
 	} while( (pWb35Tx->EP2vm_state != VM_STOP) && (pWb35Tx->EP4vm_state != VM_STOP) );
-	OS_SLEEP(10000);  // Delay for waiting function enter 940623.1.b
+	msleep(10);  // Delay for waiting function enter 940623.1.b
 
 	if (pWb35Tx->Tx4Urb)
 		usb_free_urb( pWb35Tx->Tx4Urb );
@@ -190,77 +186,30 @@ void Wb35Tx_destroy(phw_data_t pHwData)
 	#endif
 }
 
-void Wb35Tx_CurrentTime(phw_data_t pHwData, u32 TimeCount)
+void Wb35Tx_CurrentTime(struct wbsoft_priv *adapter, u32 TimeCount)
 {
+	phw_data_t pHwData = &adapter->sHwData;
 	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
-	unsigned char Trigger = FALSE;
+	unsigned char Trigger = false;
 
 	if (pWb35Tx->TxTimer > TimeCount)
-		Trigger = TRUE;
+		Trigger = true;
 	else if (TimeCount > (pWb35Tx->TxTimer+500))
-		Trigger = TRUE;
+		Trigger = true;
 
 	if (Trigger) {
 		pWb35Tx->TxTimer = TimeCount;
-		Wb35Tx_EP2VM_start( pHwData );
-	}
-}
-
-void Wb35Tx_EP2VM_start(phw_data_t pHwData)
-{
-	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
-
-	// Allow only one thread to run into function
-	if (OS_ATOMIC_INC( pHwData->Adapter, &pWb35Tx->TxResultCount ) == 1) {
-		pWb35Tx->EP2vm_state = VM_RUNNING;
-		Wb35Tx_EP2VM( pHwData );
+		Wb35Tx_EP2VM_start(adapter);
 	}
-	else
-		OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Tx->TxResultCount );
 }
 
+static void Wb35Tx_EP2VM(struct wbsoft_priv *adapter);
 
-void Wb35Tx_EP2VM(phw_data_t pHwData)
+static void Wb35Tx_EP2VM_complete(struct urb * pUrb)
 {
-	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
-	struct urb *	pUrb = (struct urb *)pWb35Tx->Tx2Urb;
-	u32 *	pltmp = (u32 *)pWb35Tx->EP2_buf;
-	int		retv;
-
-	if (pHwData->SurpriseRemove || pHwData->HwStop)
-		goto error;
-
-	if (pWb35Tx->tx_halt)
-		goto error;
-
-	//
-	// Issuing URB
-	//
-	usb_fill_int_urb( pUrb, pHwData->WbUsb.udev, usb_rcvintpipe(pHwData->WbUsb.udev,2),
-			  pltmp, MAX_INTERRUPT_LENGTH, Wb35Tx_EP2VM_complete, pHwData, 32);
-
-	pWb35Tx->EP2vm_state = VM_RUNNING;
-	retv = wb_usb_submit_urb( pUrb );
-
-	if (retv < 0) {
-		#ifdef _PE_TX_DUMP_
-		WBDEBUG(("EP2 Tx Irp sending error\n"));
-		#endif
-		goto error;
-	}
-
-	return;
-error:
-	pWb35Tx->EP2vm_state = VM_STOP;
-	OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Tx->TxResultCount );
-}
-
-
-void Wb35Tx_EP2VM_complete(struct urb * pUrb)
-{
-	phw_data_t	pHwData = pUrb->context;
+	struct wbsoft_priv *adapter = pUrb->context;
+	phw_data_t	pHwData = &adapter->sHwData;
 	T02_DESCRIPTOR	T02, TSTATUS;
-	PADAPTER	Adapter = (PADAPTER)pHwData->Adapter;
 	PWB35TX		pWb35Tx = &pHwData->Wb35Tx;
 	u32 *		pltmp = (u32 *)pWb35Tx->EP2_buf;
 	u32		i;
@@ -295,13 +244,62 @@ void Wb35Tx_EP2VM_complete(struct urb * pUrb)
 		T02.value |= ((cpu_to_le32(pltmp[i]) & 0xff) << 24);
 
 		TSTATUS.value = T02.value;  //20061009 anson's endian
-		Mds_SendComplete( Adapter, &TSTATUS );
+		Mds_SendComplete( adapter, &TSTATUS );
 		T02.value = cpu_to_le32(pltmp[i]) >> 8;
 	}
 
 	return;
 error:
-	OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Tx->TxResultCount );
+	atomic_dec(&pWb35Tx->TxResultCount);
+	pWb35Tx->EP2vm_state = VM_STOP;
+}
+
+static void Wb35Tx_EP2VM(struct wbsoft_priv *adapter)
+{
+	phw_data_t	pHwData = &adapter->sHwData;
+	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
+	struct urb *	pUrb = (struct urb *)pWb35Tx->Tx2Urb;
+	u32 *	pltmp = (u32 *)pWb35Tx->EP2_buf;
+	int		retv;
+
+	if (pHwData->SurpriseRemove || pHwData->HwStop)
+		goto error;
+
+	if (pWb35Tx->tx_halt)
+		goto error;
+
+	//
+	// Issuing URB
+	//
+	usb_fill_int_urb( pUrb, pHwData->WbUsb.udev, usb_rcvintpipe(pHwData->WbUsb.udev,2),
+			  pltmp, MAX_INTERRUPT_LENGTH, Wb35Tx_EP2VM_complete, adapter, 32);
+
+	pWb35Tx->EP2vm_state = VM_RUNNING;
+	retv = usb_submit_urb(pUrb, GFP_ATOMIC);
+
+	if (retv < 0) {
+		#ifdef _PE_TX_DUMP_
+		WBDEBUG(("EP2 Tx Irp sending error\n"));
+		#endif
+		goto error;
+	}
+
+	return;
+error:
 	pWb35Tx->EP2vm_state = VM_STOP;
+	atomic_dec(&pWb35Tx->TxResultCount);
 }
 
+void Wb35Tx_EP2VM_start(struct wbsoft_priv *adapter)
+{
+	phw_data_t pHwData = &adapter->sHwData;
+	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
+
+	// Allow only one thread to run into function
+	if (atomic_inc_return(&pWb35Tx->TxResultCount) == 1) {
+		pWb35Tx->EP2vm_state = VM_RUNNING;
+		Wb35Tx_EP2VM(adapter);
+	}
+	else
+		atomic_dec(&pWb35Tx->TxResultCount);
+}
diff --git a/drivers/staging/winbond/wb35tx_f.h b/drivers/staging/winbond/wb35tx_f.h
new file mode 100644
index 000000000000..4222fa80c7bd
--- /dev/null
+++ b/drivers/staging/winbond/wb35tx_f.h
@@ -0,0 +1,21 @@
+#ifndef __WINBOND_WB35TX_F_H
+#define __WINBOND_WB35TX_F_H
+
+#include "core.h"
+#include "wbhal_f.h"
+
+//====================================
+// Interface function declare
+//====================================
+unsigned char Wb35Tx_initial(	 phw_data_t pHwData );
+void Wb35Tx_destroy(  phw_data_t pHwData );
+unsigned char Wb35Tx_get_tx_buffer(  phw_data_t pHwData,  u8 **pBuffer );
+
+void Wb35Tx_EP2VM_start(struct wbsoft_priv *adapter);
+
+void Wb35Tx_start(struct wbsoft_priv *adapter);
+void Wb35Tx_stop(  phw_data_t pHwData );
+
+void Wb35Tx_CurrentTime(struct wbsoft_priv *adapter,  u32 TimeCount);
+
+#endif
diff --git a/drivers/staging/winbond/linux/wb35tx_s.h b/drivers/staging/winbond/wb35tx_s.h
index ac4325736760..3960276cae68 100644
--- a/drivers/staging/winbond/linux/wb35tx_s.h
+++ b/drivers/staging/winbond/wb35tx_s.h
@@ -1,3 +1,8 @@
+#ifndef __WINBOND_WB35_TX_S_H
+#define __WINBOND_WB35_TX_S_H
+
+#include "mds_s.h"
+
 //====================================
 // IS89C35 Tx related definition
 //====================================
@@ -21,8 +26,8 @@ typedef struct _WB35TX
 	// For Interrupt pipe
 	u8	EP2_buf[MAX_INTERRUPT_LENGTH];
 
-	OS_ATOMIC	TxResultCount;// For thread control of EP2 931130.4.m
-	OS_ATOMIC	TxFireCounter;// For thread control of EP4 931130.4.n
+	atomic_t	TxResultCount;// For thread control of EP2 931130.4.m
+	atomic_t	TxFireCounter;// For thread control of EP4 931130.4.n
 	u32			ByteTransfer;
 
 	u32	    TxSendIndex;// The next index of Mds array to be sent
@@ -41,7 +46,4 @@ typedef struct _WB35TX
 
 } WB35TX, *PWB35TX;
 
-
-
-
-
+#endif
diff --git a/drivers/staging/winbond/wbhal.c b/drivers/staging/winbond/wbhal.c
index 5d68ecec34c7..8a9d21cbb0c0 100644
--- a/drivers/staging/winbond/wbhal.c
+++ b/drivers/staging/winbond/wbhal.c
@@ -1,11 +1,6 @@
 #include "os_common.h"
-
-void hal_get_ethernet_address( phw_data_t pHwData, u8 *current_address )
-{
-	if( pHwData->SurpriseRemove ) return;
-
-	memcpy( current_address, pHwData->CurrentMacAddress, ETH_LENGTH_OF_ADDRESS );
-}
+#include "wbhal_f.h"
+#include "wblinux_f.h"
 
 void hal_set_ethernet_address( phw_data_t pHwData, u8 *current_address )
 {
@@ -28,423 +23,11 @@ void hal_get_permanent_address( phw_data_t pHwData, u8 *pethernet_address )
 	memcpy( pethernet_address, pHwData->PermanentMacAddress, 6 );
 }
 
-u8 hal_init_hardware(phw_data_t pHwData, PWB32_ADAPTER Adapter)
-{
-	u16 SoftwareSet;
-	pHwData->Adapter = Adapter;
-
-	// Initial the variable
-	pHwData->MaxReceiveLifeTime = DEFAULT_MSDU_LIFE_TIME; // Setting Rx maximum MSDU life time
-	pHwData->FragmentThreshold = DEFAULT_FRAGMENT_THRESHOLD; // Setting default fragment threshold
-
-	if (WbUsb_initial(pHwData)) {
-		pHwData->InitialResource = 1;
-		if( Wb35Reg_initial(pHwData)) {
-			pHwData->InitialResource = 2;
-			if (Wb35Tx_initial(pHwData)) {
-				pHwData->InitialResource = 3;
-				if (Wb35Rx_initial(pHwData)) {
-					pHwData->InitialResource = 4;
-					OS_TIMER_INITIAL( &pHwData->LEDTimer, hal_led_control, pHwData );
-					OS_TIMER_SET( &pHwData->LEDTimer, 1000 ); // 20060623
-
-					//
-					// For restrict to vendor's hardware
-					//
-					SoftwareSet = hal_software_set( pHwData );
-
-					#ifdef Vendor2
-					// Try to make sure the EEPROM contain
-					SoftwareSet >>= 8;
-					if( SoftwareSet != 0x82 )
-						return FALSE;
-					#endif
-
-					Wb35Rx_start( pHwData );
-					Wb35Tx_EP2VM_start( pHwData );
-
-					return TRUE;
-				}
-			}
-		}
-	}
-
-	pHwData->SurpriseRemove = 1;
-	return FALSE;
-}
-
-
-void hal_halt(phw_data_t pHwData, void *ppa_data)
-{
-	switch( pHwData->InitialResource )
-	{
-		case 4:
-		case 3: OS_TIMER_CANCEL( &pHwData->LEDTimer, &cancel );
-			OS_SLEEP(100000); // Wait for Timer DPC exit 940623.2
-			Wb35Rx_destroy( pHwData ); // Release the Rx
-		case 2: Wb35Tx_destroy( pHwData ); // Release the Tx
-		case 1: Wb35Reg_destroy( pHwData ); // Release the Wb35 Regisster resources
-				WbUsb_destroy( pHwData );// Release the WbUsb
-	}
-}
-
-//---------------------------------------------------------------------------------------------------
-void hal_set_rates(phw_data_t pHwData, u8 *pbss_rates,
-		   u8 length, unsigned char basic_rate_set)
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-	u32		tmp, tmp1;
-	u8		Rate[12]={ 2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108 };
-	u8		SupportedRate[16];
-	u8		i, j, k, Count1, Count2, Byte;
-
-	if( pHwData->SurpriseRemove ) return;
-
-	if (basic_rate_set) {
-		pWb35Reg->M28_MacControl &= ~0x000fff00;
-		tmp1 = 0x00000100;
-	} else {
-		pWb35Reg->M28_MacControl &= ~0xfff00000;
-		tmp1 = 0x00100000;
-	}
-
-	tmp = 0;
-	for (i=0; i<length; i++) {
-		Byte = pbss_rates[i] & 0x7f;
-		for (j=0; j<12; j++) {
-			if( Byte == Rate[j] )
-				break;
-		}
-
-		if (j < 12)
-			tmp |= (tmp1<<j);
-	}
-
-	pWb35Reg->M28_MacControl |= tmp;
-	Wb35Reg_Write( pHwData, 0x0828, pWb35Reg->M28_MacControl );
-
-	// 930206.2.c M78 setting
-	j = k = Count1 = Count2 = 0;
-	memset( SupportedRate, 0, 16 );
-	tmp = 0x00100000;
-	tmp1 = 0x00000100;
-	for (i=0; i<12; i++) { // Get the supported rate
-		if (tmp & pWb35Reg->M28_MacControl) {
-			SupportedRate[j] = Rate[i];
-
-			if (tmp1 & pWb35Reg->M28_MacControl)
-				SupportedRate[j] |= 0x80;
-
-			if (k)
-				Count2++;
-			else
-				Count1++;
-
-			j++;
-		}
-
-		if (i==4 && k==0) {
-			if( !(pWb35Reg->M28_MacControl & 0x000ff000) ) // if basic rate in 11g domain)
-			{
-				k = 1;
-				j = 8;
-			}
-		}
-
-		tmp <<= 1;
-		tmp1 <<= 1;
-	}
-
-	// Fill data into support rate until buffer full
-	//---20060926 add by anson's endian
-	for (i=0; i<4; i++)
-		*(u32 *)(SupportedRate+(i<<2)) = cpu_to_le32( *(u32 *)(SupportedRate+(i<<2)) );
-	//--- end 20060926 add by anson's endian
-	Wb35Reg_BurstWrite( pHwData,0x087c, (u32 *)SupportedRate, 4, AUTO_INCREMENT );
-	pWb35Reg->M7C_MacControl = ((u32 *)SupportedRate)[0];
-	pWb35Reg->M80_MacControl = ((u32 *)SupportedRate)[1];
-	pWb35Reg->M84_MacControl = ((u32 *)SupportedRate)[2];
-	pWb35Reg->M88_MacControl = ((u32 *)SupportedRate)[3];
-
-	// Fill length
-	tmp = Count1<<28 | Count2<<24;
-	pWb35Reg->M78_ERPInformation &= ~0xff000000;
-	pWb35Reg->M78_ERPInformation |= tmp;
-	Wb35Reg_Write( pHwData, 0x0878, pWb35Reg->M78_ERPInformation );
-}
-
-
-//---------------------------------------------------------------------------------------------------
-void hal_set_beacon_period(  phw_data_t pHwData,  u16 beacon_period )
-{
-	u32	tmp;
-
-	if( pHwData->SurpriseRemove ) return;
-
-	pHwData->BeaconPeriod = beacon_period;
-	tmp = pHwData->BeaconPeriod << 16;
-	tmp |= pHwData->ProbeDelay;
-	Wb35Reg_Write( pHwData, 0x0848, tmp );
-}
-
-
-void hal_set_current_channel_ex(  phw_data_t pHwData,  ChanInfo channel )
-{
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
-
-	if( pHwData->SurpriseRemove )
-		return;
-
-	printk("Going to channel: %d/%d\n", channel.band, channel.ChanNo);
-
-	RFSynthesizer_SwitchingChannel( pHwData, channel );// Switch channel
-	pHwData->Channel = channel.ChanNo;
-	pHwData->band = channel.band;
-	#ifdef _PE_STATE_DUMP_
-	WBDEBUG(("Set channel is %d, band =%d\n", pHwData->Channel, pHwData->band));
-	#endif
-	pWb35Reg->M28_MacControl &= ~0xff; // Clean channel information field
-	pWb35Reg->M28_MacControl |= channel.ChanNo;
-	Wb35Reg_WriteWithCallbackValue( pHwData, 0x0828, pWb35Reg->M28_MacControl,
-					(s8 *)&channel, sizeof(ChanInfo));
-}
-//---------------------------------------------------------------------------------------------------
-void hal_set_current_channel(  phw_data_t pHwData,  ChanInfo channel )
-{
-	hal_set_current_channel_ex( pHwData, channel );
-}
-//---------------------------------------------------------------------------------------------------
-void hal_get_current_channel(  phw_data_t pHwData,  ChanInfo *channel )
-{
-	channel->ChanNo = pHwData->Channel;
-	channel->band = pHwData->band;
-}
-//---------------------------------------------------------------------------------------------------
-void hal_set_accept_broadcast(  phw_data_t pHwData,  u8 enable )
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-
-	if( pHwData->SurpriseRemove ) return;
-
-	pWb35Reg->M00_MacControl &= ~0x02000000;//The HW value
-
-	if (enable)
-		pWb35Reg->M00_MacControl |= 0x02000000;//The HW value
-
-	Wb35Reg_Write( pHwData, 0x0800, pWb35Reg->M00_MacControl );
-}
-
-//for wep key error detection, we need to accept broadcast packets to be received temporary.
-void hal_set_accept_promiscuous( phw_data_t pHwData,  u8 enable)
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-
-	if (pHwData->SurpriseRemove) return;
-	if (enable) {
-		pWb35Reg->M00_MacControl |= 0x00400000;
-		Wb35Reg_Write( pHwData, 0x0800, pWb35Reg->M00_MacControl );
-	} else {
-		pWb35Reg->M00_MacControl&=~0x00400000;
-		Wb35Reg_Write( pHwData, 0x0800, pWb35Reg->M00_MacControl );
-	}
-}
-
-void hal_set_accept_multicast(  phw_data_t pHwData,  u8 enable )
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-
-	if( pHwData->SurpriseRemove ) return;
-
-	pWb35Reg->M00_MacControl &= ~0x01000000;//The HW value
-	if (enable)  pWb35Reg->M00_MacControl |= 0x01000000;//The HW value
-	Wb35Reg_Write( pHwData, 0x0800, pWb35Reg->M00_MacControl );
-}
-
-void hal_set_accept_beacon(  phw_data_t pHwData,  u8 enable )
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-
-	if( pHwData->SurpriseRemove ) return;
-
-	// 20040108 debug
-	if( !enable )//Due to SME and MLME are not suitable for 35
-		return;
-
-	pWb35Reg->M00_MacControl &= ~0x04000000;//The HW value
-	if( enable )
-		pWb35Reg->M00_MacControl |= 0x04000000;//The HW value
-
-	Wb35Reg_Write( pHwData, 0x0800, pWb35Reg->M00_MacControl );
-}
-//---------------------------------------------------------------------------------------------------
-void hal_set_multicast_address( phw_data_t pHwData, u8 *address, u8 number )
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-	u8		Byte, Bit;
-
-	if( pHwData->SurpriseRemove ) return;
-
-	//Erases and refills the card multicast registers. Used when an address
-	//    has been deleted and all bits must be recomputed.
-	pWb35Reg->M04_MulticastAddress1 = 0;
-	pWb35Reg->M08_MulticastAddress2 = 0;
-
-	while( number )
-	{
-		number--;
-		CardGetMulticastBit( (address+(number*ETH_LENGTH_OF_ADDRESS)), &Byte, &Bit);
-		pWb35Reg->Multicast[Byte] |= Bit;
-	}
-
-	// Updating register
-	Wb35Reg_BurstWrite( pHwData, 0x0804, (u32 *)pWb35Reg->Multicast, 2, AUTO_INCREMENT );
-}
-//---------------------------------------------------------------------------------------------------
-u8 hal_get_accept_beacon(  phw_data_t pHwData )
-{
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
-
-	if( pHwData->SurpriseRemove ) return 0;
-
-	if( pWb35Reg->M00_MacControl & 0x04000000 )
-		return 1;
-	else
-		return 0;
-}
-
-unsigned char hal_reset_hardware( phw_data_t pHwData, void* ppa )
-{
-	// Not implement yet
-	return TRUE;
-}
-
-void hal_stop(  phw_data_t pHwData )
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-
-	pHwData->Wb35Rx.rx_halt = 1;
-	Wb35Rx_stop( pHwData );
-
-	pHwData->Wb35Tx.tx_halt = 1;
-	Wb35Tx_stop( pHwData );
-
-	pWb35Reg->D00_DmaControl &= ~0xc0000000;//Tx Off, Rx Off
-	Wb35Reg_Write( pHwData, 0x0400, pWb35Reg->D00_DmaControl );
-
-	WbUsb_Stop( pHwData ); // 20051230 Add.4
-}
-
-unsigned char hal_idle(phw_data_t pHwData)
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-	PWBUSB	pWbUsb = &pHwData->WbUsb;
-
-	if( !pHwData->SurpriseRemove && ( pWbUsb->DetectCount || pWb35Reg->EP0vm_state!=VM_STOP ) )
-		return FALSE;
-
-	return TRUE;
-}
-//---------------------------------------------------------------------------------------------------
-void hal_set_cwmin(  phw_data_t pHwData,  u8	cwin_min )
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-
-	if( pHwData->SurpriseRemove ) return;
-
-	pHwData->cwmin = cwin_min;
-	pWb35Reg->M2C_MacControl &= ~0x7c00;	//bit 10 ~ 14
-	pWb35Reg->M2C_MacControl |= (pHwData->cwmin<<10);
-	Wb35Reg_Write( pHwData, 0x082c, pWb35Reg->M2C_MacControl );
-}
-
-s32 hal_get_rssi(  phw_data_t pHwData,  u32 *HalRssiArry,  u8 Count )
+static void hal_led_control(unsigned long data)
 {
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
-	R01_DESCRIPTOR	r01;
-	s32 ltmp = 0, tmp;
-	u8	i;
-
-	if( pHwData->SurpriseRemove ) return -200;
-	if( Count > MAX_ACC_RSSI_COUNT ) // Because the TS may use this funtion
-		Count = MAX_ACC_RSSI_COUNT;
-
-	// RSSI = C1 + C2 * (agc_state[7:0] + offset_map(lna_state[1:0]))
-	// C1 = -195, C2 = 0.66 = 85/128
-	for (i=0; i<Count; i++)
-	{
-		r01.value = HalRssiArry[i];
-		tmp = ((( r01.R01_AGC_state + pWb35Reg->LNAValue[r01.R01_LNA_state]) * 85 ) >>7 ) - 195;
-		ltmp += tmp;
-	}
-	ltmp /= Count;
-	if( pHwData->phy_type == RF_AIROHA_2230 ) ltmp -= 5; // 10;
-	if( pHwData->phy_type == RF_AIROHA_2230S ) ltmp -= 5; // 10; 20060420 Add this
-
-	//if( ltmp < -200 ) ltmp = -200;
-	if( ltmp < -110 ) ltmp = -110;// 1.0.24.0 For NJRC
-
-	return ltmp;
-}
-//----------------------------------------------------------------------------------------------------
-s32 hal_get_rssi_bss(  phw_data_t pHwData,  u16 idx,  u8 Count )
-{
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
-	R01_DESCRIPTOR	r01;
-	s32 ltmp = 0, tmp;
-	u8	i, j;
-	PADAPTER	Adapter = pHwData->Adapter;
-//	u32 *HalRssiArry = psBSS(idx)->HalRssi;
-
-	if( pHwData->SurpriseRemove ) return -200;
-	if( Count > MAX_ACC_RSSI_COUNT ) // Because the TS may use this funtion
-		Count = MAX_ACC_RSSI_COUNT;
-
-	// RSSI = C1 + C2 * (agc_state[7:0] + offset_map(lna_state[1:0]))
-	// C1 = -195, C2 = 0.66 = 85/128
-#if 0
-	for (i=0; i<Count; i++)
-	{
-		r01.value = HalRssiArry[i];
-		tmp = ((( r01.R01_AGC_state + pWb35Reg->LNAValue[r01.R01_LNA_state]) * 85 ) >>7 ) - 195;
-		ltmp += tmp;
-	}
-#else
-	if (psBSS(idx)->HalRssiIndex == 0)
-		psBSS(idx)->HalRssiIndex = MAX_ACC_RSSI_COUNT;
-	j = (u8)psBSS(idx)->HalRssiIndex-1;
-
-	for (i=0; i<Count; i++)
-	{
-		r01.value = psBSS(idx)->HalRssi[j];
-		tmp = ((( r01.R01_AGC_state + pWb35Reg->LNAValue[r01.R01_LNA_state]) * 85 ) >>7 ) - 195;
-		ltmp += tmp;
-		if (j == 0)
-		{
-			j = MAX_ACC_RSSI_COUNT;
-		}
-		j--;
-	}
-#endif
-	ltmp /= Count;
-	if( pHwData->phy_type == RF_AIROHA_2230 ) ltmp -= 5; // 10;
-	if( pHwData->phy_type == RF_AIROHA_2230S ) ltmp -= 5; // 10; 20060420 Add this
-
-	//if( ltmp < -200 ) ltmp = -200;
-	if( ltmp < -110 ) ltmp = -110;// 1.0.24.0 For NJRC
-
-	return ltmp;
-}
-
-//---------------------------------------------------------------------------
-void hal_led_control_1a(  phw_data_t pHwData )
-{
-	hal_led_control( NULL, pHwData, NULL, NULL );
-}
-
-void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
-{
-	PADAPTER	Adapter = pHwData->Adapter;
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wbsoft_priv *adapter = (struct wbsoft_priv *) data;
+	phw_data_t pHwData = &adapter->sHwData;
+	struct wb35_reg *reg = &pHwData->reg;
 	u32	LEDSet = (pHwData->SoftwareSet & HAL_LED_SET_MASK) >> HAL_LED_SET_SHIFT;
 	u8	LEDgray[20] = { 0,3,4,6,8,10,11,12,13,14,15,14,13,12,11,10,8,6,4,2 };
 	u8	LEDgray2[30] = { 7,8,9,10,11,12,13,14,15,0,0,0,0,0,0,0,0,0,0,0,0,0,15,14,13,12,11,10,9,8 };
@@ -487,21 +70,21 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 			}
 			pHwData->LED_Blinking++;
 
-			pWb35Reg->U1BC_LEDConfigure = ltmp;
+			reg->U1BC_LEDConfigure = ltmp;
 			if( LEDSet != 7 ) // Only 111 mode has 2 LEDs on PCB.
 			{
-				pWb35Reg->U1BC_LEDConfigure |= (ltmp &0xff)<<8; // Copy LED result to each LED control register
-				pWb35Reg->U1BC_LEDConfigure |= (ltmp &0xff00)>>8;
+				reg->U1BC_LEDConfigure |= (ltmp &0xff)<<8; // Copy LED result to each LED control register
+				reg->U1BC_LEDConfigure |= (ltmp &0xff00)>>8;
 			}
-			Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure );
+			Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure );
 		}
 	}
 	else if( pHwData->CurrentRadioSw || pHwData->CurrentRadioHw ) // If radio off
 	{
-		if( pWb35Reg->U1BC_LEDConfigure & 0x1010 )
+		if( reg->U1BC_LEDConfigure & 0x1010 )
 		{
-			pWb35Reg->U1BC_LEDConfigure &= ~0x1010;
-			Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure );
+			reg->U1BC_LEDConfigure &= ~0x1010;
+			Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure );
 		}
 	}
 	else
@@ -516,15 +99,15 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 					{
 						if( pHwData->LED_Blinking == 0 )
 						{
-							pWb35Reg->U1BC_LEDConfigure |= 0x10;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 On
+							reg->U1BC_LEDConfigure |= 0x10;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 On
 							pHwData->LED_Blinking = 1;
 							TimeInterval = 300;
 						}
 						else
 						{
-							pWb35Reg->U1BC_LEDConfigure &= ~0x10;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 Off
+							reg->U1BC_LEDConfigure &= ~0x10;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 Off
 							pHwData->LED_Blinking = 0;
 							TimeInterval = 300;
 						}
@@ -532,20 +115,20 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 					else
 					{
 						//Turn Off LED_0
-						if( pWb35Reg->U1BC_LEDConfigure & 0x10 )
+						if( reg->U1BC_LEDConfigure & 0x10 )
 						{
-							pWb35Reg->U1BC_LEDConfigure &= ~0x10;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 Off
+							reg->U1BC_LEDConfigure &= ~0x10;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 Off
 						}
 					}
 				}
 				else
 				{
 					// Turn On LED_0
-					if( (pWb35Reg->U1BC_LEDConfigure & 0x10) == 0 )
+					if( (reg->U1BC_LEDConfigure & 0x10) == 0 )
 					{
-						pWb35Reg->U1BC_LEDConfigure |= 0x10;
-						Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 Off
+						reg->U1BC_LEDConfigure |= 0x10;
+						Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 Off
 					}
 				}
 				break;
@@ -558,16 +141,16 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 					{
 						if( pHwData->LED_Blinking == 0 )
 						{
-							pWb35Reg->U1BC_LEDConfigure &= ~0xf;
-							pWb35Reg->U1BC_LEDConfigure |= 0x10;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 On
+							reg->U1BC_LEDConfigure &= ~0xf;
+							reg->U1BC_LEDConfigure |= 0x10;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 On
 							pHwData->LED_Blinking = 1;
 							TimeInterval = 300;
 						}
 						else
 						{
-							pWb35Reg->U1BC_LEDConfigure &= ~0x1f;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 Off
+							reg->U1BC_LEDConfigure &= ~0x1f;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 Off
 							pHwData->LED_Blinking = 0;
 							TimeInterval = 300;
 						}
@@ -575,26 +158,26 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 					else
 					{
 						// 20060901 Gray blinking if in disconnect state and not scanning
-						ltmp = pWb35Reg->U1BC_LEDConfigure;
-						pWb35Reg->U1BC_LEDConfigure &= ~0x1f;
+						ltmp = reg->U1BC_LEDConfigure;
+						reg->U1BC_LEDConfigure &= ~0x1f;
 						if( LEDgray2[(pHwData->LED_Blinking%30)] )
 						{
-							pWb35Reg->U1BC_LEDConfigure |= 0x10;
-							pWb35Reg->U1BC_LEDConfigure |= LEDgray2[ (pHwData->LED_Blinking%30) ];
+							reg->U1BC_LEDConfigure |= 0x10;
+							reg->U1BC_LEDConfigure |= LEDgray2[ (pHwData->LED_Blinking%30) ];
 						}
 						pHwData->LED_Blinking++;
-						if( pWb35Reg->U1BC_LEDConfigure != ltmp )
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 Off
+						if( reg->U1BC_LEDConfigure != ltmp )
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 Off
 						TimeInterval = 100;
 					}
 				}
 				else
 				{
 					// Turn On LED_0
-					if( (pWb35Reg->U1BC_LEDConfigure & 0x10) == 0 )
+					if( (reg->U1BC_LEDConfigure & 0x10) == 0 )
 					{
-						pWb35Reg->U1BC_LEDConfigure |= 0x10;
-						Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 Off
+						reg->U1BC_LEDConfigure |= 0x10;
+						Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 Off
 					}
 				}
 				break;
@@ -607,15 +190,15 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 					{
 						if( pHwData->LED_Blinking == 0 )
 						{
-							pWb35Reg->U1BC_LEDConfigure |= 0x1000;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_1 On
+							reg->U1BC_LEDConfigure |= 0x1000;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_1 On
 							pHwData->LED_Blinking = 1;
 							TimeInterval = 300;
 						}
 						else
 						{
-							pWb35Reg->U1BC_LEDConfigure &= ~0x1000;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_1 Off
+							reg->U1BC_LEDConfigure &= ~0x1000;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_1 Off
 							pHwData->LED_Blinking = 0;
 							TimeInterval = 300;
 						}
@@ -623,57 +206,57 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 					else
 					{
 						//Turn Off LED_1
-						if( pWb35Reg->U1BC_LEDConfigure & 0x1000 )
+						if( reg->U1BC_LEDConfigure & 0x1000 )
 						{
-							pWb35Reg->U1BC_LEDConfigure &= ~0x1000;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_1 Off
+							reg->U1BC_LEDConfigure &= ~0x1000;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_1 Off
 						}
 					}
 				}
 				else
 				{
 					// Is transmitting/receiving ??
-					if( (OS_CURRENT_RX_BYTE( Adapter ) != pHwData->RxByteCountLast ) ||
-						(OS_CURRENT_TX_BYTE( Adapter ) != pHwData->TxByteCountLast ) )
+					if( (adapter->RxByteCount != pHwData->RxByteCountLast ) ||
+						(adapter->TxByteCount != pHwData->TxByteCountLast ) )
 					{
-						if( (pWb35Reg->U1BC_LEDConfigure & 0x3000) != 0x3000 )
+						if( (reg->U1BC_LEDConfigure & 0x3000) != 0x3000 )
 						{
-							pWb35Reg->U1BC_LEDConfigure |= 0x3000;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_1 On
+							reg->U1BC_LEDConfigure |= 0x3000;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_1 On
 						}
 
 						// Update variable
-						pHwData->RxByteCountLast = OS_CURRENT_RX_BYTE( Adapter );
-						pHwData->TxByteCountLast = OS_CURRENT_TX_BYTE( Adapter );
+						pHwData->RxByteCountLast = adapter->RxByteCount;
+						pHwData->TxByteCountLast = adapter->TxByteCount;
 						TimeInterval = 200;
 					}
 					else
 					{
 						// Turn On LED_1 and blinking if transmitting/receiving
-						 if( (pWb35Reg->U1BC_LEDConfigure & 0x3000) != 0x1000 )
+						 if( (reg->U1BC_LEDConfigure & 0x3000) != 0x1000 )
 						 {
-							 pWb35Reg->U1BC_LEDConfigure &= ~0x3000;
-							 pWb35Reg->U1BC_LEDConfigure |= 0x1000;
-							 Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_1 On
+							 reg->U1BC_LEDConfigure &= ~0x3000;
+							 reg->U1BC_LEDConfigure |= 0x1000;
+							 Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_1 On
 						 }
 					}
 				}
 				break;
 
 			default: // Default setting. 2 LED be placed on PCB. LED_0: Link On LED_1 Active
-				if( (pWb35Reg->U1BC_LEDConfigure & 0x3000) != 0x3000 )
+				if( (reg->U1BC_LEDConfigure & 0x3000) != 0x3000 )
 				{
-					pWb35Reg->U1BC_LEDConfigure |= 0x3000;// LED_1 is always on and event enable
-					Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure );
+					reg->U1BC_LEDConfigure |= 0x3000;// LED_1 is always on and event enable
+					Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure );
 				}
 
 				if( pHwData->LED_Blinking )
 				{
 					// Gray blinking
-					pWb35Reg->U1BC_LEDConfigure &= ~0x0f;
-					pWb35Reg->U1BC_LEDConfigure |= 0x10;
-					pWb35Reg->U1BC_LEDConfigure |= LEDgray[ (pHwData->LED_Blinking-1)%20 ];
-					Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure );
+					reg->U1BC_LEDConfigure &= ~0x0f;
+					reg->U1BC_LEDConfigure |= 0x10;
+					reg->U1BC_LEDConfigure |= LEDgray[ (pHwData->LED_Blinking-1)%20 ];
+					Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure );
 
 					pHwData->LED_Blinking += 2;
 					if( pHwData->LED_Blinking < 40 )
@@ -681,28 +264,28 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 					else
 					{
 						pHwData->LED_Blinking = 0; // Stop blinking
-						pWb35Reg->U1BC_LEDConfigure &= ~0x0f;
-						Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure );
+						reg->U1BC_LEDConfigure &= ~0x0f;
+						Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure );
 					}
 					break;
 				}
 
 				if( pHwData->LED_LinkOn )
 				{
-					if( !(pWb35Reg->U1BC_LEDConfigure & 0x10) ) // Check the LED_0
+					if( !(reg->U1BC_LEDConfigure & 0x10) ) // Check the LED_0
 					{
 						//Try to turn ON LED_0 after gray blinking
-						pWb35Reg->U1BC_LEDConfigure |= 0x10;
+						reg->U1BC_LEDConfigure |= 0x10;
 						pHwData->LED_Blinking = 1; //Start blinking
 						TimeInterval = 50;
 					}
 				}
 				else
 				{
-					if( pWb35Reg->U1BC_LEDConfigure & 0x10 ) // Check the LED_0
+					if( reg->U1BC_LEDConfigure & 0x10 ) // Check the LED_0
 					{
-						pWb35Reg->U1BC_LEDConfigure &= ~0x10;
-						Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure );
+						reg->U1BC_LEDConfigure &= ~0x10;
+						Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure );
 					}
 				}
 				break;
@@ -720,84 +303,240 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 	}
 
 	pHwData->time_count += TimeInterval;
-	Wb35Tx_CurrentTime( pHwData, pHwData->time_count ); // 20060928 add
-	OS_TIMER_SET( &pHwData->LEDTimer, TimeInterval ); // 20060623.1
+	Wb35Tx_CurrentTime(adapter, pHwData->time_count); // 20060928 add
+	pHwData->LEDTimer.expires = jiffies + msecs_to_jiffies(TimeInterval);
+	add_timer(&pHwData->LEDTimer);
 }
 
+u8 hal_init_hardware(struct ieee80211_hw *hw)
+{
+	struct wbsoft_priv *priv = hw->priv;
+	phw_data_t pHwData = &priv->sHwData;
+	u16 SoftwareSet;
 
-void hal_set_phy_type(  phw_data_t pHwData,  u8 PhyType )
+	// Initial the variable
+	pHwData->MaxReceiveLifeTime = DEFAULT_MSDU_LIFE_TIME; // Setting Rx maximum MSDU life time
+	pHwData->FragmentThreshold = DEFAULT_FRAGMENT_THRESHOLD; // Setting default fragment threshold
+
+	pHwData->InitialResource = 1;
+	if( Wb35Reg_initial(pHwData)) {
+		pHwData->InitialResource = 2;
+		if (Wb35Tx_initial(pHwData)) {
+			pHwData->InitialResource = 3;
+			if (Wb35Rx_initial(pHwData)) {
+				pHwData->InitialResource = 4;
+				init_timer(&pHwData->LEDTimer);
+				pHwData->LEDTimer.function = hal_led_control;
+				pHwData->LEDTimer.data = (unsigned long) priv;
+				pHwData->LEDTimer.expires = jiffies + msecs_to_jiffies(1000);
+				add_timer(&pHwData->LEDTimer);
+
+				//
+				// For restrict to vendor's hardware
+				//
+				SoftwareSet = hal_software_set( pHwData );
+
+				#ifdef Vendor2
+				// Try to make sure the EEPROM contain
+				SoftwareSet >>= 8;
+				if( SoftwareSet != 0x82 )
+					return false;
+				#endif
+
+				Wb35Rx_start(hw);
+				Wb35Tx_EP2VM_start(priv);
+
+				return true;
+			}
+		}
+	}
+
+	pHwData->SurpriseRemove = 1;
+	return false;
+}
+
+
+void hal_halt(phw_data_t pHwData, void *ppa_data)
 {
-	pHwData->phy_type = PhyType;
+	switch( pHwData->InitialResource )
+	{
+		case 4:
+		case 3: del_timer_sync(&pHwData->LEDTimer);
+			msleep(100); // Wait for Timer DPC exit 940623.2
+			Wb35Rx_destroy( pHwData ); // Release the Rx
+		case 2: Wb35Tx_destroy( pHwData ); // Release the Tx
+		case 1: Wb35Reg_destroy( pHwData ); // Release the Wb35 Regisster resources
+	}
 }
 
-void hal_get_phy_type(  phw_data_t pHwData,  u8 *PhyType )
+//---------------------------------------------------------------------------------------------------
+void hal_set_beacon_period(  phw_data_t pHwData,  u16 beacon_period )
 {
-	*PhyType = pHwData->phy_type;
+	u32	tmp;
+
+	if( pHwData->SurpriseRemove ) return;
+
+	pHwData->BeaconPeriod = beacon_period;
+	tmp = pHwData->BeaconPeriod << 16;
+	tmp |= pHwData->ProbeDelay;
+	Wb35Reg_Write( pHwData, 0x0848, tmp );
 }
 
-void hal_reset_counter(  phw_data_t pHwData )
+
+static void hal_set_current_channel_ex(  phw_data_t pHwData,  ChanInfo channel )
+{
+	struct wb35_reg *reg = &pHwData->reg;
+
+	if( pHwData->SurpriseRemove )
+		return;
+
+	printk("Going to channel: %d/%d\n", channel.band, channel.ChanNo);
+
+	RFSynthesizer_SwitchingChannel( pHwData, channel );// Switch channel
+	pHwData->Channel = channel.ChanNo;
+	pHwData->band = channel.band;
+	#ifdef _PE_STATE_DUMP_
+	WBDEBUG(("Set channel is %d, band =%d\n", pHwData->Channel, pHwData->band));
+	#endif
+	reg->M28_MacControl &= ~0xff; // Clean channel information field
+	reg->M28_MacControl |= channel.ChanNo;
+	Wb35Reg_WriteWithCallbackValue( pHwData, 0x0828, reg->M28_MacControl,
+					(s8 *)&channel, sizeof(ChanInfo));
+}
+//---------------------------------------------------------------------------------------------------
+void hal_set_current_channel(  phw_data_t pHwData,  ChanInfo channel )
 {
-	pHwData->dto_tx_retry_count = 0;
-	pHwData->dto_tx_frag_count = 0;
-	memset( pHwData->tx_retry_count, 0, 8);
+	hal_set_current_channel_ex( pHwData, channel );
+}
+//---------------------------------------------------------------------------------------------------
+void hal_set_accept_broadcast(  phw_data_t pHwData,  u8 enable )
+{
+	struct wb35_reg *reg = &pHwData->reg;
+
+	if( pHwData->SurpriseRemove ) return;
+
+	reg->M00_MacControl &= ~0x02000000;//The HW value
+
+	if (enable)
+		reg->M00_MacControl |= 0x02000000;//The HW value
+
+	Wb35Reg_Write( pHwData, 0x0800, reg->M00_MacControl );
+}
+
+//for wep key error detection, we need to accept broadcast packets to be received temporary.
+void hal_set_accept_promiscuous( phw_data_t pHwData,  u8 enable)
+{
+	struct wb35_reg *reg = &pHwData->reg;
+
+	if (pHwData->SurpriseRemove) return;
+	if (enable) {
+		reg->M00_MacControl |= 0x00400000;
+		Wb35Reg_Write( pHwData, 0x0800, reg->M00_MacControl );
+	} else {
+		reg->M00_MacControl&=~0x00400000;
+		Wb35Reg_Write( pHwData, 0x0800, reg->M00_MacControl );
+	}
+}
+
+void hal_set_accept_multicast(  phw_data_t pHwData,  u8 enable )
+{
+	struct wb35_reg *reg = &pHwData->reg;
+
+	if( pHwData->SurpriseRemove ) return;
+
+	reg->M00_MacControl &= ~0x01000000;//The HW value
+	if (enable)  reg->M00_MacControl |= 0x01000000;//The HW value
+	Wb35Reg_Write( pHwData, 0x0800, reg->M00_MacControl );
+}
+
+void hal_set_accept_beacon(  phw_data_t pHwData,  u8 enable )
+{
+	struct wb35_reg *reg = &pHwData->reg;
+
+	if( pHwData->SurpriseRemove ) return;
+
+	// 20040108 debug
+	if( !enable )//Due to SME and MLME are not suitable for 35
+		return;
+
+	reg->M00_MacControl &= ~0x04000000;//The HW value
+	if( enable )
+		reg->M00_MacControl |= 0x04000000;//The HW value
+
+	Wb35Reg_Write( pHwData, 0x0800, reg->M00_MacControl );
+}
+//---------------------------------------------------------------------------------------------------
+
+void hal_stop(  phw_data_t pHwData )
+{
+	struct wb35_reg *reg = &pHwData->reg;
+
+	pHwData->Wb35Rx.rx_halt = 1;
+	Wb35Rx_stop( pHwData );
+
+	pHwData->Wb35Tx.tx_halt = 1;
+	Wb35Tx_stop( pHwData );
+
+	reg->D00_DmaControl &= ~0xc0000000;//Tx Off, Rx Off
+	Wb35Reg_Write( pHwData, 0x0400, reg->D00_DmaControl );
+}
+
+unsigned char hal_idle(phw_data_t pHwData)
+{
+	struct wb35_reg *reg = &pHwData->reg;
+	PWBUSB	pWbUsb = &pHwData->WbUsb;
+
+	if( !pHwData->SurpriseRemove && ( pWbUsb->DetectCount || reg->EP0vm_state!=VM_STOP ) )
+		return false;
+
+	return true;
+}
+//---------------------------------------------------------------------------------------------------
+void hal_set_phy_type(  phw_data_t pHwData,  u8 PhyType )
+{
+	pHwData->phy_type = PhyType;
 }
 
 void hal_set_radio_mode( phw_data_t pHwData,  unsigned char radio_off)
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 
 	if( pHwData->SurpriseRemove ) return;
 
 	if (radio_off)	//disable Baseband receive off
 	{
 		pHwData->CurrentRadioSw = 1; // off
-		pWb35Reg->M24_MacControl &= 0xffffffbf;
+		reg->M24_MacControl &= 0xffffffbf;
 	}
 	else
 	{
 		pHwData->CurrentRadioSw = 0; // on
-		pWb35Reg->M24_MacControl |= 0x00000040;
+		reg->M24_MacControl |= 0x00000040;
 	}
-	Wb35Reg_Write( pHwData, 0x0824, pWb35Reg->M24_MacControl );
+	Wb35Reg_Write( pHwData, 0x0824, reg->M24_MacControl );
 }
 
 u8 hal_get_antenna_number(  phw_data_t pHwData )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 
-	if ((pWb35Reg->BB2C & BIT(11)) == 0)
+	if ((reg->BB2C & BIT(11)) == 0)
 		return 0;
 	else
 		return 1;
 }
 
-void hal_set_antenna_number(  phw_data_t pHwData, u8 number )
-{
-
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-
-	if (number == 1) {
-		pWb35Reg->BB2C |= BIT(11);
-	} else {
-		pWb35Reg->BB2C &= ~BIT(11);
-	}
-	Wb35Reg_Write( pHwData, 0x102c, pWb35Reg->BB2C );
-#ifdef _PE_STATE_DUMP_
-	WBDEBUG(("Current antenna number : %d\n", number));
-#endif
-}
-
 //----------------------------------------------------------------------------------------------------
 //0 : radio on; 1: radio off
 u8 hal_get_hw_radio_off(  phw_data_t pHwData )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 
 	if( pHwData->SurpriseRemove ) return 1;
 
 	//read the bit16 of register U1B0
-	Wb35Reg_Read( pHwData, 0x3b0, &pWb35Reg->U1B0 );
-	if ((pWb35Reg->U1B0 & 0x00010000)) {
+	Wb35Reg_Read( pHwData, 0x3b0, &reg->U1B0 );
+	if ((reg->U1B0 & 0x00010000)) {
 		pHwData->CurrentRadioHw = 1;
 		return 1;
 	} else {
@@ -823,56 +562,7 @@ unsigned char hal_set_dxx_reg(  phw_data_t pHwData,  u16 number,  u32 value )
 	return ret;
 }
 
-void hal_scan_status_indicate(phw_data_t pHwData, unsigned char IsOnProgress)
-{
-	if( pHwData->SurpriseRemove ) return;
-	pHwData->LED_Scanning = IsOnProgress ? 1 : 0;
-}
-
-void hal_system_power_change(phw_data_t pHwData, u32 PowerState)
-{
-	if( PowerState != 0 )
-	{
-		pHwData->SurpriseRemove = 1;
-		if( pHwData->WbUsb.IsUsb20 )
-			hal_stop( pHwData );
-	}
-	else
-	{
-		if( !pHwData->WbUsb.IsUsb20 )
-			hal_stop( pHwData );
-	}
-}
-
-void hal_surprise_remove(  phw_data_t pHwData )
-{
-	PADAPTER Adapter = pHwData->Adapter;
-	if (OS_ATOMIC_INC( Adapter, &pHwData->SurpriseRemoveCount ) == 1) {
-		#ifdef _PE_STATE_DUMP_
-		WBDEBUG(("Calling hal_surprise_remove\n"));
-		#endif
-		OS_STOP( Adapter );
-	}
-}
-
-void hal_rate_change(  phw_data_t pHwData ) // Notify the HAL rate is changing 20060613.1
-{
-	PADAPTER	Adapter = pHwData->Adapter;
-	u8		rate = CURRENT_TX_RATE;
-
-	BBProcessor_RateChanging( pHwData, rate );
-}
-
 void hal_set_rf_power(phw_data_t pHwData, u8 PowerIndex)
 {
 	RFSynthesizer_SetPowerIndex( pHwData, PowerIndex );
 }
-
-unsigned char hal_set_LED(phw_data_t pHwData, u32 Mode) // 20061108 for WPS led control
-{
-	pHwData->LED_Blinking = 0;
-	pHwData->LED_control = Mode;
-	OS_TIMER_SET( &pHwData->LEDTimer, 10 ); // 20060623
-	return TRUE;
-}
-
diff --git a/drivers/staging/winbond/wbhal_f.h b/drivers/staging/winbond/wbhal_f.h
index ea9531ac8474..e805f40f6354 100644
--- a/drivers/staging/winbond/wbhal_f.h
+++ b/drivers/staging/winbond/wbhal_f.h
@@ -1,25 +1,19 @@
 //=====================================================================
 // Device related include
 //=====================================================================
-#ifdef WB_LINUX
-	#include "linux/wbusb_f.h"
-	#include "linux/wb35reg_f.h"
-	#include "linux/wb35tx_f.h"
-	#include "linux/wb35rx_f.h"
-#else
-	#include "wbusb_f.h"
-	#include "wb35reg_f.h"
-	#include "wb35tx_f.h"
-	#include "wb35rx_f.h"
-#endif
+#include "wb35reg_f.h"
+#include "wb35tx_f.h"
+#include "wb35rx_f.h"
+
+#include "core.h"
 
 //====================================================================================
 // Function declaration
 //====================================================================================
 void hal_remove_mapping_key(  phw_data_t pHwData,  u8 *pmac_addr );
 void hal_remove_default_key(  phw_data_t pHwData,  u32 index );
-unsigned char hal_set_mapping_key(  phw_data_t Adapter,  u8 *pmac_addr,  u8 null_key,  u8 wep_on,  u8 *ptx_tsc,  u8 *prx_tsc,  u8 key_type,  u8 key_len,  u8 *pkey_data );
-unsigned char hal_set_default_key(  phw_data_t Adapter,  u8 index,  u8 null_key,  u8 wep_on,  u8 *ptx_tsc,  u8 *prx_tsc,  u8 key_type,  u8 key_len,  u8 *pkey_data );
+unsigned char hal_set_mapping_key(  phw_data_t adapter,  u8 *pmac_addr,  u8 null_key,  u8 wep_on,  u8 *ptx_tsc,  u8 *prx_tsc,  u8 key_type,  u8 key_len,  u8 *pkey_data );
+unsigned char hal_set_default_key(  phw_data_t adapter,  u8 index,  u8 null_key,  u8 wep_on,  u8 *ptx_tsc,  u8 *prx_tsc,  u8 key_type,  u8 key_len,  u8 *pkey_data );
 void hal_clear_all_default_key(  phw_data_t pHwData );
 void hal_clear_all_group_key(  phw_data_t pHwData );
 void hal_clear_all_mapping_key(  phw_data_t pHwData );
@@ -27,14 +21,11 @@ void hal_clear_all_key(  phw_data_t pHwData );
 void hal_get_ethernet_address(  phw_data_t pHwData,  u8 *current_address );
 void hal_set_ethernet_address(  phw_data_t pHwData,  u8 *current_address );
 void hal_get_permanent_address(  phw_data_t pHwData,  u8 *pethernet_address );
-unsigned char hal_init_hardware(  phw_data_t pHwData,  PADAPTER Adapter );
+u8 hal_init_hardware(struct ieee80211_hw *hw);
 void hal_set_power_save_mode(  phw_data_t pHwData,  unsigned char power_save,  unsigned char wakeup,  unsigned char dtim );
 void hal_get_power_save_mode(  phw_data_t pHwData,   u8 *pin_pwr_save );
 void hal_set_slot_time(  phw_data_t pHwData,  u8 type );
 #define hal_set_atim_window( _A, _ATM )
-void hal_set_rates(  phw_data_t pHwData,  u8 *pbss_rates,  u8 length,  unsigned char basic_rate_set );
-#define hal_set_basic_rates( _A, _R, _L ) hal_set_rates( _A, _R, _L, TRUE )
-#define hal_set_op_rates( _A, _R, _L ) hal_set_rates( _A, _R, _L, FALSE )
 void hal_start_bss(  phw_data_t pHwData,  u8 mac_op_mode );
 void hal_join_request(  phw_data_t pHwData,  u8 bss_type ); // 0:BSS STA 1:IBSS STA//
 void hal_stop_sync_bss(  phw_data_t pHwData );
@@ -47,39 +38,25 @@ void hal_set_listen_interval(  phw_data_t pHwData,  u16 listen_interval );
 void hal_set_cap_info(  phw_data_t pHwData,  u16 capability_info );
 void hal_set_ssid(  phw_data_t pHwData,  u8 *pssid,  u8 ssid_len );
 void hal_set_current_channel(  phw_data_t pHwData,  ChanInfo channel );
-void hal_set_current_channel_ex(  phw_data_t pHwData,  ChanInfo channel );
-void hal_get_current_channel(  phw_data_t pHwData,  ChanInfo *channel );
 void hal_set_accept_broadcast(  phw_data_t pHwData,  u8 enable );
 void hal_set_accept_multicast(  phw_data_t pHwData,  u8 enable );
 void hal_set_accept_beacon(  phw_data_t pHwData,  u8 enable );
-void hal_set_multicast_address(  phw_data_t pHwData,  u8 *address,  u8 number );
-u8 hal_get_accept_beacon(  phw_data_t pHwData );
 void hal_stop(  phw_data_t pHwData );
 void hal_halt(  phw_data_t pHwData, void *ppa_data );
 void hal_start_tx0(  phw_data_t pHwData );
 void hal_set_phy_type(  phw_data_t pHwData,  u8 PhyType );
-void hal_get_phy_type(  phw_data_t pHwData,  u8 *PhyType );
-unsigned char hal_reset_hardware(  phw_data_t pHwData,  void* ppa );
-void hal_set_cwmin(  phw_data_t pHwData,  u8	cwin_min );
 #define hal_get_cwmin( _A ) ( (_A)->cwmin )
 void hal_set_cwmax(  phw_data_t pHwData,  u16 cwin_max );
 #define hal_get_cwmax( _A ) ( (_A)->cwmax )
 void hal_set_rsn_wpa(  phw_data_t pHwData,  u32 * RSN_IE_Bitmap , u32 * RSN_OUI_type , unsigned char bDesiredAuthMode);
-//s32 hal_get_rssi(  phw_data_t pHwData,  u32 HalRssi );
-s32 hal_get_rssi(  phw_data_t pHwData,  u32 *HalRssiArry,  u8 Count );
-s32 hal_get_rssi_bss(  phw_data_t pHwData,  u16 idx,  u8 Count );
 void hal_set_connect_info(  phw_data_t pHwData,  unsigned char boConnect );
 u8 hal_get_est_sq3(  phw_data_t pHwData,  u8 Count );
-void hal_led_control_1a(  phw_data_t pHwData );
-void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 );
 void hal_set_rf_power(  phw_data_t pHwData,  u8 PowerIndex ); // 20060621 Modify
-void hal_reset_counter(  phw_data_t pHwData );
 void hal_set_radio_mode(  phw_data_t pHwData,  unsigned char boValue);
 void hal_descriptor_indicate(  phw_data_t pHwData,  PDESCRIPTOR pDes );
 u8 hal_get_antenna_number(  phw_data_t pHwData );
-void hal_set_antenna_number(  phw_data_t pHwData, u8 number );
 u32 hal_get_bss_pk_cnt(  phw_data_t pHwData );
-#define hal_get_region_from_EEPROM( _A ) ( (_A)->Wb35Reg.EEPROMRegion )
+#define hal_get_region_from_EEPROM( _A ) ( (_A)->reg.EEPROMRegion )
 void hal_set_accept_promiscuous		(  phw_data_t pHwData,  u8 enable);
 #define hal_get_tx_buffer( _A, _B ) Wb35Tx_get_tx_buffer( _A, _B )
 u8 hal_get_hw_radio_off			(  phw_data_t pHwData );
@@ -88,20 +65,13 @@ u8 hal_get_hw_radio_off			(  phw_data_t pHwData );
 #define hal_rssi_boundary_high( _A ) (_A->RSSI_high)
 #define hal_rssi_boundary_low( _A ) (_A->RSSI_low)
 #define hal_scan_interval( _A )		(_A->Scan_Interval)
-void hal_scan_status_indicate(  phw_data_t pHwData, u8 status);	// 0: complete, 1: in progress
-void hal_system_power_change(  phw_data_t pHwData, u32 PowerState ); // 20051230 -=D0 1=D1 ..
-void hal_surprise_remove(  phw_data_t pHwData );
 
 #define PHY_DEBUG( msg, args... )
 
-
-
-void hal_rate_change(  phw_data_t pHwData ); // Notify the HAL rate is changing 20060613.1
 unsigned char hal_get_dxx_reg(  phw_data_t pHwData,  u16 number,  u32 * pValue );
 unsigned char hal_set_dxx_reg(  phw_data_t pHwData,  u16 number,  u32 value );
 #define hal_get_time_count( _P )	(_P->time_count/10)	// return 100ms count
 #define hal_detect_error( _P )		(_P->WbUsb.DetectCount)
-unsigned char hal_set_LED(  phw_data_t pHwData,  u32 Mode ); // 20061108 for WPS led control
 
 //-------------------------------------------------------------------------
 // The follow function is unused for IS89C35
@@ -113,7 +83,6 @@ unsigned char hal_set_LED(  phw_data_t pHwData,  u32 Mode ); // 20061108 for WPS
 #define hal_ibss_disconnect(_A) hal_stop_sync_bss(_A)
 #define hal_join_request_stop(_A)
 unsigned char	hal_idle(  phw_data_t pHwData );
-#define pa_stall_execution( _A )	//OS_SLEEP( 1 )
 #define hw_get_cxx_reg( _A, _B, _C )
 #define hw_set_cxx_reg( _A, _B, _C )
 #define hw_get_dxx_reg( _A, _B, _C )	hal_get_dxx_reg( _A, _B, (u32 *)_C )
diff --git a/drivers/staging/winbond/wbhal_s.h b/drivers/staging/winbond/wbhal_s.h
index 2ee3f0fc1ad8..276d2b12632f 100644
--- a/drivers/staging/winbond/wbhal_s.h
+++ b/drivers/staging/winbond/wbhal_s.h
@@ -1,3 +1,10 @@
+#ifndef __WINBOND_WBHAL_S_H
+#define __WINBOND_WBHAL_S_H
+
+#include <linux/types.h>
+
+#include "common.h"
+
 //[20040722 WK]
 #define HAL_LED_SET_MASK		0x001c	//20060901 Extend
 #define HAL_LED_SET_SHIFT		2
@@ -415,10 +422,10 @@ typedef struct _TXVGA_FOR_50 {
 // Device related include
 //=====================================================================
 
-#include "linux/wbusb_s.h"
-#include "linux/wb35reg_s.h"
-#include "linux/wb35tx_s.h"
-#include "linux/wb35rx_s.h"
+#include "wbusb_s.h"
+#include "wb35reg_s.h"
+#include "wb35tx_s.h"
+#include "wb35rx_s.h"
 
 
 // For Hal using ==================================================================
@@ -442,16 +449,6 @@ typedef struct _HW_DATA_T
 	u32	FragCount;
 	u32	DMAFix; //V1_DMA_FIX The variable can be removed if driver want to save mem space for V2.
 
-	//=======================================================================================
-	// For USB driver, hal need more variables. Due to
-	//	1. NDIS-WDM operation
-	//	2. The SME, MLME and OLD MDS need Adapter structure, but the driver under HAL doesn't
-	//		have that parameter when receiving and indicating packet.
-	//		The MDS must input the Adapter pointer as the second parameter of hal_init_hardware.
-	//		The function usage is different than PCI driver.
-	//=======================================================================================
-	void* Adapter;
-
 	//===============================================
 	// Definition for MAC address
 	//===============================================
@@ -506,11 +503,11 @@ typedef struct _HW_DATA_T
 	// Variable for each module
 	//========================================================================
 	WBUSB		WbUsb; // Need WbUsb.h
-	WB35REG		Wb35Reg; // Need Wb35Reg.h
+	struct wb35_reg	reg; // Need Wb35Reg.h
 	WB35TX		Wb35Tx; // Need Wb35Tx.h
 	WB35RX		Wb35Rx; // Need Wb35Rx.h
 
-	OS_TIMER	LEDTimer;// For LED
+	struct timer_list	LEDTimer;// For LED
 
 	u32		LEDpoint;// For LED
 
@@ -570,7 +567,7 @@ typedef struct _HW_DATA_T
 	u32		RxByteCountLast;
 	u32		TxByteCountLast;
 
-	s32		SurpriseRemoveCount;
+	atomic_t	SurpriseRemoveCount;
 
 	// For global timer
 	u32		time_count;//TICK_TIME_100ms 1 = 100ms
@@ -612,4 +609,4 @@ typedef struct _HAL_RATE
 	u32   NumRate54M;
 } HAL_RATE, *PHAL_RATE;
 
-
+#endif
diff --git a/drivers/staging/winbond/wblinux.c b/drivers/staging/winbond/wblinux.c
deleted file mode 100644
index 4ed45e488318..000000000000
--- a/drivers/staging/winbond/wblinux.c
+++ /dev/null
@@ -1,275 +0,0 @@
-//============================================================================
-//  Copyright (c) 1996-2005 Winbond Electronic Corporation
-//
-//  Module Name:
-//    wblinux.c
-//
-//  Abstract:
-//    Linux releated routines
-//
-//============================================================================
-#include "os_common.h"
-
-u32
-WBLINUX_MemoryAlloc(void* *VirtualAddress, u32 Length)
-{
-	*VirtualAddress = kzalloc( Length, GFP_ATOMIC ); //GFP_KERNEL is not suitable
-
-	if (*VirtualAddress == NULL)
-		return 0;
-	return 1;
-}
-
-s32
-EncapAtomicInc(PADAPTER Adapter, void* pAtomic)
-{
-	PWBLINUX pWbLinux = &Adapter->WbLinux;
-	u32	ltmp;
-	u32 *	pltmp = (u32 *)pAtomic;
-	spin_lock_irq( &pWbLinux->AtomicSpinLock );
-	(*pltmp)++;
-	ltmp = (*pltmp);
-	spin_unlock_irq( &pWbLinux->AtomicSpinLock );
-	return ltmp;
-}
-
-s32
-EncapAtomicDec(PADAPTER Adapter, void* pAtomic)
-{
-	PWBLINUX pWbLinux = &Adapter->WbLinux;
-	u32	ltmp;
-	u32 *	pltmp = (u32 *)pAtomic;
-	spin_lock_irq( &pWbLinux->AtomicSpinLock );
-	(*pltmp)--;
-	ltmp = (*pltmp);
-	spin_unlock_irq( &pWbLinux->AtomicSpinLock );
-	return ltmp;
-}
-
-unsigned char
-WBLINUX_Initial(PADAPTER Adapter)
-{
-	PWBLINUX pWbLinux = &Adapter->WbLinux;
-
-	spin_lock_init( &pWbLinux->SpinLock );
-	spin_lock_init( &pWbLinux->AtomicSpinLock );
-	return TRUE;
-}
-
-void
-WBLinux_ReceivePacket(PADAPTER Adapter, PRXLAYER1 pRxLayer1)
-{
-	BUG();
-}
-
-
-void
-WBLINUX_GetNextPacket(PADAPTER Adapter,  PDESCRIPTOR pDes)
-{
-	BUG();
-}
-
-void
-WBLINUX_GetNextPacketCompleted(PADAPTER Adapter, PDESCRIPTOR pDes)
-{
-	BUG();
-}
-
-void
-WBLINUX_Destroy(PADAPTER Adapter)
-{
-	WBLINUX_stop( Adapter );
-#ifdef _PE_USB_INI_DUMP_
-	WBDEBUG(("[w35und] unregister_netdev!\n"));
-#endif
-}
-
-void
-WBLINUX_stop(  PADAPTER Adapter )
-{
-	PWBLINUX	pWbLinux = &Adapter->WbLinux;
-	struct sk_buff *pSkb;
-
-	if (OS_ATOMIC_INC( Adapter, &pWbLinux->ThreadCount ) == 1) {
-		// Shutdown module immediately
-		pWbLinux->shutdown = 1;
-
-		while (pWbLinux->skb_array[ pWbLinux->skb_GetIndex ]) {
-			// Trying to free the un-sending packet
-			pSkb = pWbLinux->skb_array[ pWbLinux->skb_GetIndex ];
-			pWbLinux->skb_array[ pWbLinux->skb_GetIndex ] = NULL;
-			if( in_irq() )
-				dev_kfree_skb_irq( pSkb );
-			else
-				dev_kfree_skb( pSkb );
-
-			pWbLinux->skb_GetIndex++;
-			pWbLinux->skb_GetIndex %= WBLINUX_PACKET_ARRAY_SIZE;
-		}
-
-#ifdef _PE_STATE_DUMP_
-		WBDEBUG(( "[w35und] SKB_RELEASE OK\n" ));
-#endif
-	}
-
-	OS_ATOMIC_DEC( Adapter, &pWbLinux->ThreadCount );
-}
-
-void
-WbWlanHalt(  PADAPTER Adapter )
-{
-	//---------------------
-	Adapter->sLocalPara.ShutDowned = TRUE;
-
-	Mds_Destroy( Adapter );
-
-	// Turn off Rx and Tx hardware ability
-	hal_stop( &Adapter->sHwData );
-#ifdef _PE_USB_INI_DUMP_
-	WBDEBUG(("[w35und] Hal_stop O.K.\n"));
-#endif
-	OS_SLEEP(100000);// Waiting Irp completed
-
-	// Destroy the NDIS module
-	WBLINUX_Destroy( Adapter );
-
-	// Halt the HAL
-	hal_halt(&Adapter->sHwData, NULL);
-}
-
-unsigned char
-WbWLanInitialize(PADAPTER Adapter)
-{
-	phw_data_t	pHwData;
-	u8		*pMacAddr;
-	u8		*pMacAddr2;
-	u32		InitStep = 0;
-	u8		EEPROM_region;
-	u8		HwRadioOff;
-
-	//
-	// Setting default value for Linux
-	//
-	Adapter->sLocalPara.region_INF = REGION_AUTO;
-	Adapter->sLocalPara.TxRateMode = RATE_AUTO;
-	psLOCAL->bMacOperationMode = MODE_802_11_BG;	// B/G mode
-	Adapter->Mds.TxRTSThreshold = DEFAULT_RTSThreshold;
-	Adapter->Mds.TxFragmentThreshold = DEFAULT_FRAGMENT_THRESHOLD;
-	hal_set_phy_type( &Adapter->sHwData, RF_WB_242_1 );
-	Adapter->sLocalPara.MTUsize = MAX_ETHERNET_PACKET_SIZE;
-	psLOCAL->bPreambleMode = AUTO_MODE;
-	Adapter->sLocalPara.RadioOffStatus.boSwRadioOff = FALSE;
-	pHwData = &Adapter->sHwData;
-	hal_set_phy_type( pHwData, RF_DECIDE_BY_INF );
-
-	//
-	// Initial each module and variable
-	//
-	if (!WBLINUX_Initial(Adapter)) {
-#ifdef _PE_USB_INI_DUMP_
-		WBDEBUG(("[w35und]WBNDIS initialization failed\n"));
-#endif
-		goto error;
-	}
-
-	// Initial Software variable
-	Adapter->sLocalPara.ShutDowned = FALSE;
-
-	//added by ws for wep key error detection
-	Adapter->sLocalPara.bWepKeyError= FALSE;
-	Adapter->sLocalPara.bToSelfPacketReceived = FALSE;
-	Adapter->sLocalPara.WepKeyDetectTimerCount= 2 * 100; /// 2 seconds
-
-	// Initial USB hal
-	InitStep = 1;
-	pHwData = &Adapter->sHwData;
-	if (!hal_init_hardware(pHwData, Adapter))
-		goto error;
-
-	EEPROM_region = hal_get_region_from_EEPROM( pHwData );
-	if (EEPROM_region != REGION_AUTO)
-		psLOCAL->region = EEPROM_region;
-	else {
-		if (psLOCAL->region_INF != REGION_AUTO)
-			psLOCAL->region = psLOCAL->region_INF;
-		else
-			psLOCAL->region = REGION_USA;	//default setting
-	}
-
-	// Get Software setting flag from hal
-	Adapter->sLocalPara.boAntennaDiversity = FALSE;
-	if (hal_software_set(pHwData) & 0x00000001)
-		Adapter->sLocalPara.boAntennaDiversity = TRUE;
-
-	//
-	// For TS module
-	//
-	InitStep = 2;
-
-	// For MDS module
-	InitStep = 3;
-	Mds_initial(Adapter);
-
-	//=======================================
-	// Initialize the SME, SCAN, MLME, ROAM
-	//=======================================
-	InitStep = 4;
-	InitStep = 5;
-	InitStep = 6;
-
-	// If no user-defined address in the registry, use the addresss "burned" on the NIC instead.
-	pMacAddr = Adapter->sLocalPara.ThisMacAddress;
-	pMacAddr2 = Adapter->sLocalPara.PermanentAddress;
-	hal_get_permanent_address( pHwData, Adapter->sLocalPara.PermanentAddress );// Reading ethernet address from EEPROM
-	if (OS_MEMORY_COMPARE(pMacAddr, "\x00\x00\x00\x00\x00\x00", MAC_ADDR_LENGTH )) // Is equal
-	{
-		memcpy( pMacAddr, pMacAddr2, MAC_ADDR_LENGTH );
-	} else {
-		// Set the user define MAC address
-		hal_set_ethernet_address( pHwData, Adapter->sLocalPara.ThisMacAddress );
-	}
-
-	//get current antenna
-	psLOCAL->bAntennaNo = hal_get_antenna_number(pHwData);
-#ifdef _PE_STATE_DUMP_
-	WBDEBUG(("Driver init, antenna no = %d\n", psLOCAL->bAntennaNo));
-#endif
-	hal_get_hw_radio_off( pHwData );
-
-	// Waiting for HAL setting OK
-	while (!hal_idle(pHwData))
-		OS_SLEEP(10000);
-
-	MTO_Init(Adapter);
-
-	HwRadioOff = hal_get_hw_radio_off( pHwData );
-	psLOCAL->RadioOffStatus.boHwRadioOff = !!HwRadioOff;
-
-	hal_set_radio_mode( pHwData, (unsigned char)(psLOCAL->RadioOffStatus.boSwRadioOff || psLOCAL->RadioOffStatus.boHwRadioOff) );
-
-	hal_driver_init_OK(pHwData) = 1; // Notify hal that the driver is ready now.
-	//set a tx power for reference.....
-//	sme_set_tx_power_level(Adapter, 12);	FIXME?
-	return TRUE;
-
-error:
-	switch (InitStep) {
-	case 5:
-	case 4:
-	case 3: Mds_Destroy( Adapter );
-	case 2:
-	case 1: WBLINUX_Destroy( Adapter );
-		hal_halt( pHwData, NULL );
-	case 0: break;
-	}
-
-	return FALSE;
-}
-
-void WBLINUX_ConnectStatus(PADAPTER Adapter, u32 flag)
-{
-	PWBLINUX	pWbLinux = &Adapter->WbLinux;
-
-	pWbLinux->LinkStatus = flag; // OS_DISCONNECTED	or  OS_CONNECTED
-}
-
diff --git a/drivers/staging/winbond/wblinux_f.h b/drivers/staging/winbond/wblinux_f.h
index 68240c5fc80d..868e87727240 100644
--- a/drivers/staging/winbond/wblinux_f.h
+++ b/drivers/staging/winbond/wblinux_f.h
@@ -1,23 +1,16 @@
+#ifndef __WBLINUX_F_H
+#define __WBLINUX_F_H
+
+#include "core.h"
+#include "mds_s.h"
+
 //=========================================================================
 // Copyright (c) 1996-2004 Winbond Electronic Corporation
 //
 // wblinux_f.h
 //
-u32 WBLINUX_MemoryAlloc(  void* *VirtualAddress,  u32 Length );
-s32 EncapAtomicInc(  PADAPTER Adapter,  void* pAtomic );
-s32 EncapAtomicDec(  PADAPTER Adapter,  void* pAtomic );
-void WBLinux_ReceivePacket(  PADAPTER Adapter,  PRXLAYER1 pRxLayer1 );
-unsigned char WBLINUX_Initial(  PADAPTER Adapter );
 int wb35_start_xmit(struct sk_buff *skb, struct net_device *netdev );
-void WBLINUX_GetNextPacket(  PADAPTER Adapter,  PDESCRIPTOR pDes );
-void WBLINUX_GetNextPacketCompleted(  PADAPTER Adapter,  PDESCRIPTOR pDes );
-void WBLINUX_stop(  PADAPTER Adapter );
-void WBLINUX_Destroy(  PADAPTER Adapter );
 void wb35_set_multicast( struct net_device *netdev );
 struct net_device_stats * wb35_netdev_stats( struct net_device *netdev );
-void WBLINUX_stop(  PADAPTER Adapter );
-void WbWlanHalt(  PADAPTER Adapter );
-void WBLINUX_ConnectStatus(  PADAPTER Adapter,  u32 flag );
-
-
 
+#endif
diff --git a/drivers/staging/winbond/wblinux_s.h b/drivers/staging/winbond/wblinux_s.h
deleted file mode 100644
index fd2bb43bf3cf..000000000000
--- a/drivers/staging/winbond/wblinux_s.h
+++ /dev/null
@@ -1,45 +0,0 @@
-//============================================================
-// wblinux_s.h
-//
-#define OS_MEMORY_ALLOC( _V, _S )	WBLINUX_MemoryAlloc( _V, _S )
-#define OS_LINK_STATUS			(Adapter->WbLinux.LinkStatus == OS_CONNECTED)
-#define OS_SET_SHUTDOWN( _A )		_A->WbLinux.shutdown=1
-#define OS_SET_RESUME( _A )		_A->WbLinux.shutdown=0
-#define OS_CONNECT_STATUS_INDICATE( _A, _F )		WBLINUX_ConnectStatus( _A, _F )
-#define OS_DISCONNECTED	0
-#define OS_CONNECTED	1
-#define OS_STOP( _A )	WBLINUX_stop( _A )
-
-#define OS_CURRENT_RX_BYTE( _A )		_A->WbLinux.RxByteCount
-#define OS_CURRENT_TX_BYTE( _A )		_A->WbLinux.TxByteCount
-#define OS_EVENT_INDICATE( _A, _B, _F )
-#define OS_PMKID_STATUS_EVENT( _A )
-#define OS_RECEIVE_PACKET_INDICATE( _A, _D )		WBLinux_ReceivePacket( _A, _D )
-#define OS_RECEIVE_802_1X_PACKET_INDICATE( _A, _D )	EAP_ReceivePacket( _A, _D )
-#define OS_GET_PACKET( _A, _D )				WBLINUX_GetNextPacket( _A, _D )
-#define OS_GET_PACKET_COMPLETE( _A, _D )	WBLINUX_GetNextPacketCompleted( _A, _D )
-#define OS_SEND_RESULT( _A, _ID, _R )
-
-#define WBLINUX_PACKET_ARRAY_SIZE	(ETHERNET_TX_DESCRIPTORS*4)
-
-typedef struct _WBLINUX
-{
-	spinlock_t	AtomicSpinLock;
-	spinlock_t	SpinLock;
-	u32	shutdown;
-
-	OS_ATOMIC	ThreadCount;
-
-	u32	LinkStatus;		// OS_DISCONNECTED or OS_CONNECTED
-
-	u32	RxByteCount;
-	u32	TxByteCount;
-
-	struct sk_buff *skb_array[ WBLINUX_PACKET_ARRAY_SIZE ];
-	struct sk_buff *packet_return;
-	s32	skb_SetIndex;
-	s32	skb_GetIndex;
-	s32	netif_state_stop; // 1: stop  0: normal
-} WBLINUX, *PWBLINUX;
-
-
diff --git a/drivers/staging/winbond/wbusb.c b/drivers/staging/winbond/wbusb.c
new file mode 100644
index 000000000000..b003f9a7e151
--- /dev/null
+++ b/drivers/staging/winbond/wbusb.c
@@ -0,0 +1,438 @@
+/*
+ * Copyright 2008 Pavel Machek <pavel@suse.cz>
+ *
+ * Distribute under GPLv2.
+ */
+#include <net/mac80211.h>
+#include <linux/usb.h>
+
+#include "core.h"
+#include "mds_f.h"
+#include "mlmetxrx_f.h"
+#include "mto_f.h"
+#include "wbhal_f.h"
+#include "wblinux_f.h"
+
+MODULE_AUTHOR("Original by: Jeff Lee<YY_Lee@issc.com.tw> Adapted to 2.6.x by Costantino Leandro (Rxart Desktop) <le_costantino@pixartargentina.com.ar>");
+MODULE_DESCRIPTION("IS89C35 802.11bg WLAN USB Driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.1");
+
+static struct usb_device_id wb35_table[] __devinitdata = {
+	{USB_DEVICE(0x0416, 0x0035)},
+	{USB_DEVICE(0x18E8, 0x6201)},
+	{USB_DEVICE(0x18E8, 0x6206)},
+	{USB_DEVICE(0x18E8, 0x6217)},
+	{USB_DEVICE(0x18E8, 0x6230)},
+	{USB_DEVICE(0x18E8, 0x6233)},
+	{USB_DEVICE(0x1131, 0x2035)},
+	{ 0, }
+};
+
+MODULE_DEVICE_TABLE(usb, wb35_table);
+
+static struct ieee80211_rate wbsoft_rates[] = {
+	{ .bitrate = 10, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
+};
+
+static struct ieee80211_channel wbsoft_channels[] = {
+	{ .center_freq = 2412},
+};
+
+static struct ieee80211_supported_band wbsoft_band_2GHz = {
+	.channels	= wbsoft_channels,
+	.n_channels	= ARRAY_SIZE(wbsoft_channels),
+	.bitrates	= wbsoft_rates,
+	.n_bitrates	= ARRAY_SIZE(wbsoft_rates),
+};
+
+static int wbsoft_add_interface(struct ieee80211_hw *dev,
+				 struct ieee80211_if_init_conf *conf)
+{
+	printk("wbsoft_add interface called\n");
+	return 0;
+}
+
+static void wbsoft_remove_interface(struct ieee80211_hw *dev,
+				     struct ieee80211_if_init_conf *conf)
+{
+	printk("wbsoft_remove interface called\n");
+}
+
+static void wbsoft_stop(struct ieee80211_hw *hw)
+{
+	printk(KERN_INFO "%s called\n", __func__);
+}
+
+static int wbsoft_get_stats(struct ieee80211_hw *hw,
+			    struct ieee80211_low_level_stats *stats)
+{
+	printk(KERN_INFO "%s called\n", __func__);
+	return 0;
+}
+
+static int wbsoft_get_tx_stats(struct ieee80211_hw *hw,
+			       struct ieee80211_tx_queue_stats *stats)
+{
+	printk(KERN_INFO "%s called\n", __func__);
+	return 0;
+}
+
+static void wbsoft_configure_filter(struct ieee80211_hw *dev,
+				     unsigned int changed_flags,
+				     unsigned int *total_flags,
+				     int mc_count, struct dev_mc_list *mclist)
+{
+	unsigned int bit_nr, new_flags;
+	u32 mc_filter[2];
+	int i;
+
+	new_flags = 0;
+
+	if (*total_flags & FIF_PROMISC_IN_BSS) {
+		new_flags |= FIF_PROMISC_IN_BSS;
+		mc_filter[1] = mc_filter[0] = ~0;
+	} else if ((*total_flags & FIF_ALLMULTI) || (mc_count > 32)) {
+		new_flags |= FIF_ALLMULTI;
+		mc_filter[1] = mc_filter[0] = ~0;
+	} else {
+		mc_filter[1] = mc_filter[0] = 0;
+		for (i = 0; i < mc_count; i++) {
+			if (!mclist)
+				break;
+			printk("Should call ether_crc here\n");
+			//bit_nr = ether_crc(ETH_ALEN, mclist->dmi_addr) >> 26;
+			bit_nr = 0;
+
+			bit_nr &= 0x3F;
+			mc_filter[bit_nr >> 5] |= 1 << (bit_nr & 31);
+			mclist = mclist->next;
+		}
+	}
+
+	dev->flags &= ~IEEE80211_HW_RX_INCLUDES_FCS;
+
+	*total_flags = new_flags;
+}
+
+static int wbsoft_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
+{
+	struct wbsoft_priv *priv = dev->priv;
+
+	MLMESendFrame(priv, skb->data, skb->len, FRAME_TYPE_802_11_MANAGEMENT);
+
+	return NETDEV_TX_OK;
+}
+
+
+static int wbsoft_start(struct ieee80211_hw *dev)
+{
+	struct wbsoft_priv *priv = dev->priv;
+
+	priv->enabled = true;
+
+	return 0;
+}
+
+static int wbsoft_config(struct ieee80211_hw *dev, u32 changed)
+{
+	struct wbsoft_priv *priv = dev->priv;
+	struct ieee80211_conf *conf = &dev->conf;
+	ChanInfo ch;
+
+	printk("wbsoft_config called\n");
+
+	ch.band = 1;
+	ch.ChanNo = 1;	/* Should use channel_num, or something, as that is already pre-translated */
+
+
+	hal_set_current_channel(&priv->sHwData, ch);
+	hal_set_beacon_period(&priv->sHwData, conf->beacon_int);
+//	hal_set_cap_info(&priv->sHwData, ?? );
+// hal_set_ssid(phw_data_t pHwData,  u8 * pssid,  u8 ssid_len); ??
+	hal_set_accept_broadcast(&priv->sHwData, 1);
+	hal_set_accept_promiscuous(&priv->sHwData,  1);
+	hal_set_accept_multicast(&priv->sHwData,  1);
+	hal_set_accept_beacon(&priv->sHwData,  1);
+	hal_set_radio_mode(&priv->sHwData,  0);
+	//hal_set_antenna_number(  phw_data_t pHwData, u8 number )
+	//hal_set_rf_power(phw_data_t pHwData, u8 PowerIndex)
+
+
+//	hal_start_bss(&priv->sHwData, WLAN_BSSTYPE_INFRASTRUCTURE);	??
+
+//void hal_set_rates(phw_data_t pHwData, u8 * pbss_rates,
+//		   u8 length, unsigned char basic_rate_set)
+
+	return 0;
+}
+
+static int wbsoft_config_interface(struct ieee80211_hw *dev,
+				    struct ieee80211_vif *vif,
+				    struct ieee80211_if_conf *conf)
+{
+	printk("wbsoft_config_interface called\n");
+	return 0;
+}
+
+static u64 wbsoft_get_tsf(struct ieee80211_hw *dev)
+{
+	printk("wbsoft_get_tsf called\n");
+	return 0;
+}
+
+static const struct ieee80211_ops wbsoft_ops = {
+	.tx			= wbsoft_tx,
+	.start			= wbsoft_start,		/* Start can be pretty much empty as we do wb35_hw_init() during probe? */
+	.stop			= wbsoft_stop,
+	.add_interface		= wbsoft_add_interface,
+	.remove_interface	= wbsoft_remove_interface,
+	.config			= wbsoft_config,
+	.config_interface	= wbsoft_config_interface,
+	.configure_filter	= wbsoft_configure_filter,
+	.get_stats		= wbsoft_get_stats,
+	.get_tx_stats		= wbsoft_get_tx_stats,
+	.get_tsf		= wbsoft_get_tsf,
+// conf_tx: hal_set_cwmin()/hal_set_cwmax;
+};
+
+static unsigned char wb35_hw_init(struct ieee80211_hw *hw)
+{
+	struct wbsoft_priv *priv = hw->priv;
+	phw_data_t	pHwData;
+	u8		*pMacAddr;
+	u8		*pMacAddr2;
+	u32		InitStep = 0;
+	u8		EEPROM_region;
+	u8		HwRadioOff;
+
+	//
+	// Setting default value for Linux
+	//
+	priv->sLocalPara.region_INF = REGION_AUTO;
+	priv->sLocalPara.TxRateMode = RATE_AUTO;
+	priv->sLocalPara.bMacOperationMode = MODE_802_11_BG;	// B/G mode
+	priv->Mds.TxRTSThreshold = DEFAULT_RTSThreshold;
+	priv->Mds.TxFragmentThreshold = DEFAULT_FRAGMENT_THRESHOLD;
+	hal_set_phy_type( &priv->sHwData, RF_WB_242_1 );
+	priv->sLocalPara.MTUsize = MAX_ETHERNET_PACKET_SIZE;
+	priv->sLocalPara.bPreambleMode = AUTO_MODE;
+	priv->sLocalPara.RadioOffStatus.boSwRadioOff = false;
+	pHwData = &priv->sHwData;
+	hal_set_phy_type( pHwData, RF_DECIDE_BY_INF );
+
+	//added by ws for wep key error detection
+	priv->sLocalPara.bWepKeyError= false;
+	priv->sLocalPara.bToSelfPacketReceived = false;
+	priv->sLocalPara.WepKeyDetectTimerCount= 2 * 100; /// 2 seconds
+
+	// Initial USB hal
+	InitStep = 1;
+	pHwData = &priv->sHwData;
+	if (!hal_init_hardware(hw))
+		goto error;
+
+	EEPROM_region = hal_get_region_from_EEPROM( pHwData );
+	if (EEPROM_region != REGION_AUTO)
+		priv->sLocalPara.region = EEPROM_region;
+	else {
+		if (priv->sLocalPara.region_INF != REGION_AUTO)
+			priv->sLocalPara.region = priv->sLocalPara.region_INF;
+		else
+			priv->sLocalPara.region = REGION_USA;	//default setting
+	}
+
+	// Get Software setting flag from hal
+	priv->sLocalPara.boAntennaDiversity = false;
+	if (hal_software_set(pHwData) & 0x00000001)
+		priv->sLocalPara.boAntennaDiversity = true;
+
+	//
+	// For TS module
+	//
+	InitStep = 2;
+
+	// For MDS module
+	InitStep = 3;
+	Mds_initial(priv);
+
+	//=======================================
+	// Initialize the SME, SCAN, MLME, ROAM
+	//=======================================
+	InitStep = 4;
+	InitStep = 5;
+	InitStep = 6;
+
+	// If no user-defined address in the registry, use the addresss "burned" on the NIC instead.
+	pMacAddr = priv->sLocalPara.ThisMacAddress;
+	pMacAddr2 = priv->sLocalPara.PermanentAddress;
+	hal_get_permanent_address( pHwData, priv->sLocalPara.PermanentAddress );// Reading ethernet address from EEPROM
+	if (memcmp(pMacAddr, "\x00\x00\x00\x00\x00\x00", MAC_ADDR_LENGTH) == 0)
+		memcpy(pMacAddr, pMacAddr2, MAC_ADDR_LENGTH);
+	else {
+		// Set the user define MAC address
+		hal_set_ethernet_address(pHwData, priv->sLocalPara.ThisMacAddress);
+	}
+
+	//get current antenna
+	priv->sLocalPara.bAntennaNo = hal_get_antenna_number(pHwData);
+#ifdef _PE_STATE_DUMP_
+	WBDEBUG(("Driver init, antenna no = %d\n", psLOCAL->bAntennaNo));
+#endif
+	hal_get_hw_radio_off( pHwData );
+
+	// Waiting for HAL setting OK
+	while (!hal_idle(pHwData))
+		msleep(10);
+
+	MTO_Init(priv);
+
+	HwRadioOff = hal_get_hw_radio_off( pHwData );
+	priv->sLocalPara.RadioOffStatus.boHwRadioOff = !!HwRadioOff;
+
+	hal_set_radio_mode( pHwData, (unsigned char)(priv->sLocalPara.RadioOffStatus.boSwRadioOff || priv->sLocalPara.RadioOffStatus.boHwRadioOff) );
+
+	hal_driver_init_OK(pHwData) = 1; // Notify hal that the driver is ready now.
+	//set a tx power for reference.....
+//	sme_set_tx_power_level(priv, 12);	FIXME?
+	return true;
+
+error:
+	switch (InitStep) {
+	case 5:
+	case 4:
+	case 3: Mds_Destroy( priv );
+	case 2:
+	case 1: hal_halt( pHwData, NULL );
+	case 0: break;
+	}
+
+	return false;
+}
+
+static int wb35_probe(struct usb_interface *intf, const struct usb_device_id *id_table)
+{
+	PWBUSB		pWbUsb;
+        struct usb_host_interface *interface;
+	struct usb_endpoint_descriptor *endpoint;
+	u32	ltmp;
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct wbsoft_priv *priv;
+	struct ieee80211_hw *dev;
+	int err;
+
+	usb_get_dev(udev);
+
+	// 20060630.2 Check the device if it already be opened
+	err = usb_control_msg(udev, usb_rcvctrlpipe( udev, 0 ),
+			      0x01, USB_TYPE_VENDOR|USB_RECIP_DEVICE|USB_DIR_IN,
+			      0x0, 0x400, &ltmp, 4, HZ*100 );
+	if (err)
+		goto error;
+
+	ltmp = cpu_to_le32(ltmp);
+	if (ltmp) {  // Is already initialized?
+		err = -EBUSY;
+		goto error;
+	}
+
+	dev = ieee80211_alloc_hw(sizeof(*priv), &wbsoft_ops);
+	if (!dev)
+		goto error;
+
+	priv = dev->priv;
+
+	spin_lock_init(&priv->SpinLock);
+
+	pWbUsb = &priv->sHwData.WbUsb;
+	pWbUsb->udev = udev;
+
+        interface = intf->cur_altsetting;
+        endpoint = &interface->endpoint[0].desc;
+
+	if (endpoint[2].wMaxPacketSize == 512) {
+		printk("[w35und] Working on USB 2.0\n");
+		pWbUsb->IsUsb20 = 1;
+	}
+
+	if (!wb35_hw_init(dev)) {
+		err = -EINVAL;
+		goto error_free_hw;
+	}
+
+	SET_IEEE80211_DEV(dev, &udev->dev);
+	{
+		phw_data_t pHwData = &priv->sHwData;
+		unsigned char		dev_addr[MAX_ADDR_LEN];
+		hal_get_permanent_address(pHwData, dev_addr);
+		SET_IEEE80211_PERM_ADDR(dev, dev_addr);
+	}
+
+	dev->extra_tx_headroom = 12;	/* FIXME */
+	dev->flags = 0;
+
+	dev->channel_change_time = 1000;
+	dev->queues = 1;
+
+	dev->wiphy->bands[IEEE80211_BAND_2GHZ] = &wbsoft_band_2GHz;
+
+	err = ieee80211_register_hw(dev);
+	if (err)
+		goto error_free_hw;
+
+	usb_set_intfdata(intf, priv);
+
+	return 0;
+
+error_free_hw:
+	ieee80211_free_hw(dev);
+error:
+	usb_put_dev(udev);
+	return err;
+}
+
+static void wb35_hw_halt(struct wbsoft_priv *adapter)
+{
+	Mds_Destroy( adapter );
+
+	// Turn off Rx and Tx hardware ability
+	hal_stop( &adapter->sHwData );
+#ifdef _PE_USB_INI_DUMP_
+	WBDEBUG(("[w35und] Hal_stop O.K.\n"));
+#endif
+	msleep(100);// Waiting Irp completed
+
+	// Halt the HAL
+	hal_halt(&adapter->sHwData, NULL);
+}
+
+
+static void wb35_disconnect(struct usb_interface *intf)
+{
+	struct wbsoft_priv *priv = usb_get_intfdata(intf);
+
+	wb35_hw_halt(priv);
+
+	usb_set_intfdata(intf, NULL);
+	usb_put_dev(interface_to_usbdev(intf));
+}
+
+static struct usb_driver wb35_driver = {
+	.name		= "w35und",
+	.id_table	= wb35_table,
+	.probe		= wb35_probe,
+	.disconnect	= wb35_disconnect,
+};
+
+static int __init wb35_init(void)
+{
+	return usb_register(&wb35_driver);
+}
+
+static void __exit wb35_exit(void)
+{
+	usb_deregister(&wb35_driver);
+}
+
+module_init(wb35_init);
+module_exit(wb35_exit);
diff --git a/drivers/staging/winbond/linux/wbusb_s.h b/drivers/staging/winbond/wbusb_s.h
index d5c1d53de70b..1de93600d848 100644
--- a/drivers/staging/winbond/linux/wbusb_s.h
+++ b/drivers/staging/winbond/wbusb_s.h
@@ -11,9 +11,10 @@
 //
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
-#define OS_SLEEP( _MT )	{ set_current_state(TASK_INTERRUPTIBLE); \
-			  schedule_timeout( _MT*HZ/1000000 ); }
+#ifndef __WINBOND_WBUSB_S_H
+#define __WINBOND_WBUSB_S_H
 
+#include <linux/types.h>
 
 //---------------------------------------------------------------------------
 //  RW_CONTEXT --
@@ -23,20 +24,14 @@
 typedef struct _RW_CONTEXT
 {
 	void*			pHwData;
-	PURB			pUrb;
+	struct urb		*urb;
 	void*			pCallBackFunctionParameter;
 } RW_CONTEXT, *PRW_CONTEXT;
 
-
-
-
-#define DRIVER_AUTHOR "Original by: Jeff Lee<YY_Lee@issc.com.tw> Adapted to 2.6.x by Costantino Leandro (Rxart Desktop) <le_costantino@pixartargentina.com.ar>"
-#define DRIVER_DESC   "IS89C35 802.11bg WLAN USB Driver"
-
-
-
 typedef struct _WBUSB {
 	u32	IsUsb20;
 	struct usb_device *udev;
 	u32	DetectCount;
 } WBUSB, *PWBUSB;
+
+#endif
diff --git a/drivers/staging/wlan-ng/Kconfig b/drivers/staging/wlan-ng/Kconfig
index 2425d860dcaf..9959b658c8cf 100644
--- a/drivers/staging/wlan-ng/Kconfig
+++ b/drivers/staging/wlan-ng/Kconfig
@@ -1,9 +1,9 @@
 config PRISM2_USB
-	tristate "Prism2.5 USB driver"
-	depends on WLAN_80211 && USB
+	tristate "Prism2.5/3 USB driver"
+	depends on WLAN_80211 && USB && WIRELESS_EXT
 	default n
 	---help---
-	  This is the wlan-ng prism 2.5 USB driver for a wide range of
+	  This is the wlan-ng prism 2.5/3 USB driver for a wide range of
 	  old USB wireless devices.
 
 	  To compile this driver as a module, choose M here: the module
diff --git a/drivers/staging/wlan-ng/Makefile b/drivers/staging/wlan-ng/Makefile
index 777b5111b3d0..5edac5c8d4ee 100644
--- a/drivers/staging/wlan-ng/Makefile
+++ b/drivers/staging/wlan-ng/Makefile
@@ -1,7 +1,6 @@
 obj-$(CONFIG_PRISM2_USB) += prism2_usb.o
-obj-$(CONFIG_PRISM2_USB) += p80211.o
 
-p80211-objs := p80211mod.o \
+prism2_usb-objs := prism2usb.o \
 		p80211conv.o \
 		p80211req.o \
 		p80211wep.o \
diff --git a/drivers/staging/wlan-ng/README b/drivers/staging/wlan-ng/README
index f50e4eb6c272..9c10dbb000d5 100644
--- a/drivers/staging/wlan-ng/README
+++ b/drivers/staging/wlan-ng/README
@@ -3,6 +3,5 @@ TODO:
 	- sparse warnings
 	- Lindent cleanups
 	- move to use the in-kernel wireless stack
-	- possible enable the pcmcia and pci portions of the driver
 
 Please send all patches to Greg Kroah-Hartman <greg@kroah.com>
diff --git a/drivers/staging/wlan-ng/hfa384x.c b/drivers/staging/wlan-ng/hfa384x.c
deleted file mode 100644
index 04df3fd9c520..000000000000
--- a/drivers/staging/wlan-ng/hfa384x.c
+++ /dev/null
@@ -1,4018 +0,0 @@
-/* src/prism2/driver/hfa384x.c
-*
-* Implements the functions of the Intersil hfa384x MAC
-*
-* Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
-* --------------------------------------------------------------------
-*
-* linux-wlan
-*
-*   The contents of this file are subject to the Mozilla Public
-*   License Version 1.1 (the "License"); you may not use this file
-*   except in compliance with the License. You may obtain a copy of
-*   the License at http://www.mozilla.org/MPL/
-*
-*   Software distributed under the License is distributed on an "AS
-*   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
-*   implied. See the License for the specific language governing
-*   rights and limitations under the License.
-*
-*   Alternatively, the contents of this file may be used under the
-*   terms of the GNU Public License version 2 (the "GPL"), in which
-*   case the provisions of the GPL are applicable instead of the
-*   above.  If you wish to allow the use of your version of this file
-*   only under the terms of the GPL and not to allow others to use
-*   your version of this file under the MPL, indicate your decision
-*   by deleting the provisions above and replace them with the notice
-*   and other provisions required by the GPL.  If you do not delete
-*   the provisions above, a recipient may use your version of this
-*   file under either the MPL or the GPL.
-*
-* --------------------------------------------------------------------
-*
-* Inquiries regarding the linux-wlan Open Source project can be
-* made directly to:
-*
-* AbsoluteValue Systems Inc.
-* info@linux-wlan.com
-* http://www.linux-wlan.com
-*
-* --------------------------------------------------------------------
-*
-* Portions of the development of this software were funded by
-* Intersil Corporation as part of PRISM(R) chipset product development.
-*
-* --------------------------------------------------------------------
-*
-* This file implements functions that correspond to the prism2/hfa384x
-* 802.11 MAC hardware and firmware host interface.
-*
-* The functions can be considered to represent several levels of
-* abstraction.  The lowest level functions are simply C-callable wrappers
-* around the register accesses.  The next higher level represents C-callable
-* prism2 API functions that match the Intersil documentation as closely
-* as is reasonable.  The next higher layer implements common sequences
-* of invokations of the API layer (e.g. write to bap, followed by cmd).
-*
-* Common sequences:
-* hfa384x_drvr_xxx	Highest level abstractions provided by the
-*			hfa384x code.  They are driver defined wrappers
-*			for common sequences.  These functions generally
-*			use the services of the lower levels.
-*
-* hfa384x_drvr_xxxconfig  An example of the drvr level abstraction. These
-*			functions are wrappers for the RID get/set
-*			sequence. They 	call copy_[to|from]_bap() and
-*			cmd_access().	These functions operate on the
-*			RIDs and buffers without validation.  The caller
-*			is responsible for that.
-*
-* API wrapper functions:
-* hfa384x_cmd_xxx	functions that provide access to the f/w commands.
-*			The function arguments correspond to each command
-*			argument, even command arguments that get packed
-*			into single registers.  These functions _just_
-*			issue the command by setting the cmd/parm regs
-*			& reading the status/resp regs.  Additional
-*			activities required to fully use a command
-*			(read/write from/to bap, get/set int status etc.)
-*			are implemented separately.  Think of these as
-*			C-callable prism2 commands.
-*
-* Lowest Layer Functions:
-* hfa384x_docmd_xxx	These functions implement the sequence required
-*			to issue any prism2 command.  Primarily used by the
-*			hfa384x_cmd_xxx functions.
-*
-* hfa384x_bap_xxx	BAP read/write access functions.
-*			Note: we usually use BAP0 for non-interrupt context
-*			 and BAP1 for interrupt context.
-*
-* hfa384x_dl_xxx	download related functions.
-*
-* Driver State Issues:
-* Note that there are two pairs of functions that manage the
-* 'initialized' and 'running' states of the hw/MAC combo.  The four
-* functions are create(), destroy(), start(), and stop().  create()
-* sets up the data structures required to support the hfa384x_*
-* functions and destroy() cleans them up.  The start() function gets
-* the actual hardware running and enables the interrupts.  The stop()
-* function shuts the hardware down.  The sequence should be:
-* create()
-*  .
-*  .  Self contained test routines can run here, particularly
-*  .  corereset() and test_hostif().
-*  .
-* start()
-*  .
-*  .  Do interesting things w/ the hardware
-*  .
-* stop()
-* destroy()
-*
-* Note that destroy() can be called without calling stop() first.
-* --------------------------------------------------------------------
-*/
-
-/*================================================================*/
-
-/* System Includes */
-#define WLAN_DBVAR	prism2_debug
-#include "version.h"
-
-
-#include <linux/version.h>
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/types.h>
-#include <linux/slab.h>
-#include <linux/wireless.h>
-#include <linux/netdevice.h>
-#include <linux/timer.h>
-#include <asm/semaphore.h>
-#include <asm/io.h>
-#include <linux/delay.h>
-#include <asm/byteorder.h>
-#include <linux/list.h>
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-#include <linux/tqueue.h>
-#else
-#include <linux/workqueue.h>
-#endif
-
-#if (WLAN_HOSTIF == WLAN_PCMCIA)
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,13) )
-#include <pcmcia/version.h>
-#endif
-#include <pcmcia/cs_types.h>
-#include <pcmcia/cs.h>
-#include <pcmcia/cistpl.h>
-#include <pcmcia/ds.h>
-#include <pcmcia/cisreg.h>
-#endif
-
-#if ((WLAN_HOSTIF == WLAN_PLX) || (WLAN_HOSTIF == WLAN_PCI))
-#include <linux/ioport.h>
-#include <linux/pci.h>
-#endif
-
-#include "wlan_compat.h"
-
-// XXXX #define CMD_IRQ
-
-/*================================================================*/
-/* Project Includes */
-
-#include "p80211types.h"
-#include "p80211hdr.h"
-#include "p80211mgmt.h"
-#include "p80211conv.h"
-#include "p80211msg.h"
-#include "p80211netdev.h"
-#include "p80211req.h"
-#include "p80211metadef.h"
-#include "p80211metastruct.h"
-#include "hfa384x.h"
-#include "prism2mgmt.h"
-
-/*================================================================*/
-/* Local Constants */
-
-static const UINT16 crc16tab[256] =
-{
-	0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241,
-	0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440,
-	0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40,
-	0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841,
-	0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40,
-	0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41,
-	0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641,
-	0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040,
-	0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240,
-	0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441,
-	0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41,
-	0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840,
-	0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41,
-	0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40,
-	0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640,
-	0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041,
-	0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240,
-	0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441,
-	0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41,
-	0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840,
-	0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41,
-	0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40,
-	0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640,
-	0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041,
-	0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241,
-	0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440,
-	0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40,
-	0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841,
-	0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40,
-	0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41,
-	0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641,
-	0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040
-};
-
-/*================================================================*/
-/* Local Macros */
-
-/*================================================================*/
-/* Local Types */
-
-/*================================================================*/
-/* Local Static Definitions */
-extern int prism2_debug;
-
-/*================================================================*/
-/* Local Function Declarations */
-
-static void	hfa384x_int_dtim(wlandevice_t *wlandev);
-static void	hfa384x_int_infdrop(wlandevice_t *wlandev);
-
-static void     hfa384x_bap_tasklet(unsigned long data);
-
-static void	hfa384x_int_info(wlandevice_t *wlandev);
-static void	hfa384x_int_txexc(wlandevice_t *wlandev);
-static void	hfa384x_int_tx(wlandevice_t *wlandev);
-static void	hfa384x_int_rx(wlandevice_t *wlandev);
-
-#ifdef CMD_IRQ
-static void	hfa384x_int_cmd(wlandevice_t *wlandev);
-#endif
-static void	hfa384x_int_rxmonitor( wlandevice_t *wlandev,
-			UINT16 rxfid, hfa384x_rx_frame_t *rxdesc);
-static void	hfa384x_int_alloc(wlandevice_t *wlandev);
-
-static int hfa384x_docmd_wait( hfa384x_t *hw, hfa384x_metacmd_t *cmd);
-
-static int hfa384x_dl_docmd_wait( hfa384x_t *hw, hfa384x_metacmd_t *cmd);
-
-static UINT16
-hfa384x_mkcrc16(UINT8 *p, int len);
-
-int hfa384x_copy_to_bap4(hfa384x_t *hw, UINT16 bap, UINT16 id, UINT16 offset,
-			 void *buf, UINT len, void* buf2, UINT len2,
-			 void *buf3, UINT len3, void* buf4, UINT len4);
-
-/*================================================================*/
-/* Function Definitions */
-
-static UINT16
-txfid_queue_empty(hfa384x_t *hw)
-{
-	return (hw->txfid_head == hw->txfid_tail) ? 1 : 0;
-}
-
-static UINT16
-txfid_queue_remove(hfa384x_t *hw)
-{
-	UINT16 result= 0;
-
-	if (txfid_queue_empty(hw)) {
-		WLAN_LOG_DEBUG(3,"queue empty.\n");
-	} else {
-		result = hw->txfid_queue[hw->txfid_head];
-		hw->txfid_head = (hw->txfid_head + 1) % hw->txfid_N;
-	}
-
-	return (UINT16)result;
-}
-
-static INT16
-txfid_queue_add(hfa384x_t *hw, UINT16 val)
-{
-	INT16 result = 0;
-
-	if (hw->txfid_head == ((hw->txfid_tail + 1) % hw->txfid_N)) {
-		result = -1;
-		WLAN_LOG_DEBUG(3,"queue full.\n");
-	} else {
-		hw->txfid_queue[hw->txfid_tail] = val;
-		result = hw->txfid_tail;
-		hw->txfid_tail = (hw->txfid_tail + 1) % hw->txfid_N;
-	}
-
-	return result;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_create
-*
-* Initializes the hfa384x_t data structure for use.  Note this
-* does _not_ intialize the actual hardware, just the data structures
-* we use to keep track of its state.
-*
-* Arguments:
-*	hw		device structure
-*	irq		device irq number
-*	iobase		[pcmcia] i/o base address for register access
-*			[pci] zero
-*			[plx] i/o base address for register access
-*	membase		[pcmcia] pcmcia_cs "link" pointer
-*			[pci] memory base address for register access
-*			[plx] memory base address for card attribute memory
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-void hfa384x_create(hfa384x_t *hw, UINT irq, UINT32 iobase,
-		    UINT8 __iomem *membase)
-{
-	DBFENTER;
-	memset(hw, 0, sizeof(hfa384x_t));
-	hw->irq = irq;
-	hw->iobase = iobase;
-	hw->membase = membase;
-	spin_lock_init(&(hw->cmdlock));
-
-	/* BAP setup */
- 	spin_lock_init(&(hw->baplock));
-	tasklet_init(&hw->bap_tasklet,
-		     hfa384x_bap_tasklet,
-		     (unsigned long) hw);
-
-	init_waitqueue_head(&hw->cmdq);
-	sema_init(&hw->infofid_sem, 1);
-
-        hw->txfid_head = 0;
-        hw->txfid_tail = 0;
-        hw->txfid_N = HFA384x_DRVR_FIDSTACKLEN_MAX;
-        memset(hw->txfid_queue, 0, sizeof(hw->txfid_queue));
-
-	hw->isram16 = 1;
-
-	/* Init the auth queue head */
-	skb_queue_head_init(&hw->authq);
-
-	INIT_WORK2(&hw->link_bh, prism2sta_processing_defer);
-
-        INIT_WORK2(&hw->commsqual_bh, prism2sta_commsqual_defer);
-
-	init_timer(&hw->commsqual_timer);
-	hw->commsqual_timer.data = (unsigned long) hw;
-	hw->commsqual_timer.function = prism2sta_commsqual_timer;
-
-	hw->link_status = HFA384x_LINK_NOTCONNECTED;
-	hw->state = HFA384x_STATE_INIT;
-
-	DBFEXIT;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_destroy
-*
-* Partner to hfa384x_create().  This function cleans up the hw
-* structure so that it can be freed by the caller using a simple
-* kfree.  Currently, this function is just a placeholder.  If, at some
-* point in the future, an hw in the 'shutdown' state requires a 'deep'
-* kfree, this is where it should be done.  Note that if this function
-* is called on a _running_ hw structure, the drvr_stop() function is
-* called.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	nothing, this function is not allowed to fail.
-*
-* Side effects:
-*
-* Call context:
-*	process
-----------------------------------------------------------------*/
-void
-hfa384x_destroy( hfa384x_t *hw)
-{
-	struct sk_buff *skb;
-
-	DBFENTER;
-
-	if ( hw->state == HFA384x_STATE_RUNNING ) {
-		hfa384x_drvr_stop(hw);
-	}
-	hw->state = HFA384x_STATE_PREINIT;
-
-	if (hw->scanresults) {
-		kfree(hw->scanresults);
-		hw->scanresults = NULL;
-	}
-
-        /* Now to clean out the auth queue */
-        while ( (skb = skb_dequeue(&hw->authq)) ) {
-                dev_kfree_skb(skb);
-        }
-
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_getconfig
-*
-* Performs the sequence necessary to read a config/info item.
-*
-* Arguments:
-*	hw		device structure
-*	rid		config/info record id (host order)
-*	buf		host side record buffer.  Upon return it will
-*			contain the body portion of the record (minus the
-*			RID and len).
-*	len		buffer length (in bytes, should match record length)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*	-ENODATA 	length mismatch between argument and retrieved
-*			record.
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_getconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len)
-{
-	int 		result = 0;
-	DBFENTER;
-
-	result = hfa384x_cmd_access( hw, 0, rid, buf, len);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_setconfig
-*
-* Performs the sequence necessary to write a config/info item.
-*
-* Arguments:
-*	hw		device structure
-*	rid		config/info record id (in host order)
-*	buf		host side record buffer
-*	len		buffer length (in bytes)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_setconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len)
-{
-	int		result = 0;
-	DBFENTER;
-
-	result = hfa384x_cmd_access( hw, 1, rid, buf, len);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_readpda
-*
-* Performs the sequence to read the PDA space.  Note there is no
-* drvr_writepda() function.  Writing a PDA is
-* generally implemented by a calling component via calls to
-* cmd_download and writing to the flash download buffer via the
-* aux regs.
-*
-* Arguments:
-*	hw		device structure
-*	buf		buffer to store PDA in
-*	len		buffer length
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*	-ETIMEOUT	timout waiting for the cmd regs to become
-*			available, or waiting for the control reg
-*			to indicate the Aux port is enabled.
-*	-ENODATA	the buffer does NOT contain a valid PDA.
-*			Either the card PDA is bad, or the auxdata
-*			reads are giving us garbage.
-
-*
-* Side effects:
-*
-* Call context:
-*	process thread or non-card interrupt.
-----------------------------------------------------------------*/
-int hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, UINT len)
-{
-	int		result = 0;
-	UINT16		*pda = buf;
-	int		pdaok = 0;
-	int		morepdrs = 1;
-	int		currpdr = 0;	/* word offset of the current pdr */
-	int		i;
-	UINT16		pdrlen;		/* pdr length in bytes, host order */
-	UINT16		pdrcode;	/* pdr code, host order */
-	UINT16		crc;
-	UINT16		pdacrc;
-	struct pdaloc {
-		UINT32	cardaddr;
-		UINT16	auxctl;
-	} pdaloc[] =
-	{
-		{ HFA3842_PDA_BASE,		HFA384x_AUX_CTL_NV},
-		{ HFA3842_PDA_BASE,		HFA384x_AUX_CTL_EXTDS},
-		{ HFA3841_PDA_BASE,		HFA384x_AUX_CTL_NV},
-		{ HFA3841_PDA_BASE,		HFA384x_AUX_CTL_EXTDS},
-		{ HFA3841_PDA_BOGUS_BASE,	HFA384x_AUX_CTL_NV}
-	};
-
-	DBFENTER;
-	/* Check for aux available */
-	result = hfa384x_cmd_aux_enable(hw, 0);
-	if ( result ) {
-		WLAN_LOG_DEBUG(1,"aux_enable() failed. result=%d\n", result);
-		goto failed;
-	}
-
-	/* Read the pda from each known address.  */
-	for ( i = 0; i < (sizeof(pdaloc)/sizeof(pdaloc[0])); i++) {
-		WLAN_LOG_DEBUG( 3, "Checking PDA@(0x%08x,%s)\n",
-			pdaloc[i].cardaddr,
-			pdaloc[i].auxctl == HFA384x_AUX_CTL_NV ?
-			"CTL_NV" : "CTL_EXTDS");
-
-		/* Copy bufsize bytes from our current pdaloc */
-		hfa384x_copy_from_aux(hw,
-			pdaloc[i].cardaddr,
-			pdaloc[i].auxctl,
-			buf,
-			len);
-
-		/* Test for garbage */
-		/* Traverse the PDR list Looking for PDA-END */
-		pdaok = 1;	/* intially assume good */
-		morepdrs = 1;
-		currpdr = 0;
-		while ( pdaok && morepdrs ) {
-			pdrlen = hfa384x2host_16(pda[currpdr]) * 2;
-			pdrcode = hfa384x2host_16(pda[currpdr+1]);
-
-			/* Test for completion at END record */
-			if ( pdrcode == HFA384x_PDR_END_OF_PDA ) {
-				if ( pdrlen == 4 ) {
-					morepdrs = 0;
-					/* Calculate CRC-16 and compare to PDA
-					 * value.  Note the addition of 2 words
-					 * for ENDREC.len and ENDREC.code
-					 * fields.
-					 */
-					crc = hfa384x_mkcrc16( (UINT8*)pda,
-						(currpdr + 2) * sizeof(UINT16));
-					pdacrc =hfa384x2host_16(pda[currpdr+2]);
-					if ( crc != pdacrc ) {
-						WLAN_LOG_DEBUG(3,
-						"PDA crc failed:"
-						"calc_crc=0x%04x,"
-						"pdr_crc=0x%04x.\n",
-						crc, pdacrc);
-						pdaok = 0;
-					}
-				} else {
-					WLAN_LOG_DEBUG(3,
-					"END record detected w/ "
-					"len(%d) != 2, assuming bad PDA\n",
-					pdrlen);
-					pdaok = 0;
-
-				}
-				break;
-			}
-
-			/* Test the record length */
-			if ( pdrlen > HFA384x_PDR_LEN_MAX || pdrlen == 0) {
-				WLAN_LOG_DEBUG(3,
-					"pdrlen for address #%d "
-					"at %#x:%#x:%d\n",
-					i, pdaloc[i].cardaddr,
-					pdaloc[i].auxctl, pdrlen);
-				WLAN_LOG_DEBUG(3,"pdrlen invalid=%d\n",
-					pdrlen);
-				pdaok = 0;
-				break;
-			}
-
-			/* Move to the next pdr */
-			if ( morepdrs ) {
-				/* note the access to pda[], we need words */
-				currpdr += hfa384x2host_16(pda[currpdr]) + 1;
-				if (currpdr*sizeof(UINT16) > len) {
-					WLAN_LOG_DEBUG(3,
-					"Didn't find PDA_END in buffer, "
-					"trying next location.\n");
-					pdaok = 0;
-					break;
-				}
-			}
-		}
-		if ( pdaok ) {
-			WLAN_LOG_INFO(
-				"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;
-		}
-	}
-	result = pdaok ? 0 : -ENODATA;
-
-	if ( result ) {
-		WLAN_LOG_DEBUG(3,"Failure: pda is not okay\n");
-	}
-
-	hfa384x_cmd_aux_disable(hw);
-failed:
-	DBFEXIT;
-	return result;
-}
-
-
-
-/*----------------------------------------------------------------
-* mkpda_crc
-*
-* Calculates the CRC16 for the given PDA and inserts the value
-* into the end record.
-*
-* Arguments:
-*	pda	ptr to the PDA data structure.
-*
-* Returns:
-*	0	- success
-*	~0	- failure (probably an errno)
-----------------------------------------------------------------*/
-static UINT16
-hfa384x_mkcrc16(UINT8 *p, int len)
-{
-	UINT16	crc = 0;
-	UINT8	*lim = p + len;
-
-	while (p < lim) {
-		crc = (crc >> 8 ) ^ crc16tab[(crc & 0xff) ^ *p++];
-	}
-
-	return crc;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_ramdl_enable
-*
-* Begins the ram download state.  Checks to see that we're not
-* already in a download state and that a port isn't enabled.
-* Sets the download state and calls cmd_download with the
-* ENABLE_VOLATILE subcommand and the exeaddr argument.
-*
-* Arguments:
-*	hw		device structure
-*	exeaddr		the card execution address that will be
-*                       jumped to when ramdl_disable() is called
-*			(host order).
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_ramdl_enable(hfa384x_t *hw, UINT32 exeaddr)
-{
-	int		result = 0;
-	UINT16		lowaddr;
-	UINT16		hiaddr;
-	int		i;
-	DBFENTER;
-	/* Check that a port isn't active */
-	for ( i = 0; i < HFA384x_PORTID_MAX; i++) {
-		if ( hw->port_enabled[i] ) {
-			WLAN_LOG_DEBUG(1,"Can't download with a port enabled.\n");
-			result = -EINVAL;
-			goto done;
-		}
-	}
-
-	/* Check that we're not already in a download state */
-	if ( hw->dlstate != HFA384x_DLSTATE_DISABLED ) {
-		WLAN_LOG_DEBUG(1,"Download state not disabled.\n");
-		result = -EINVAL;
-		goto done;
-	}
-
-	/* Are we supposed to go into genesis mode? */
-	if (exeaddr == 0x3f0000) {
-		UINT16 initseq[2] = { 0xe100, 0xffa1 };
-		UINT16 readbuf[2];
-		UINT8 hcr = 0x0f; /* Default to x16 SRAM */
-		hw->isram16 = 1;
-
-		WLAN_LOG_DEBUG(1, "Dropping into Genesis mode\n");
-
-		/* Issue card reset and enable aux port */
-		hfa384x_corereset(hw, prism2_reset_holdtime,
-				  prism2_reset_settletime, 0);
-		hfa384x_cmd_aux_enable(hw, 1);
-
-		/* Genesis set */
-		hfa384x_copy_to_aux(hw, 0x7E0038, HFA384x_AUX_CTL_EXTDS,
-				    initseq, sizeof(initseq));
-
-		hfa384x_corereset(hw, prism2_reset_holdtime,
-				  prism2_reset_settletime, hcr);
-
-		/* Validate memory config */
-		hfa384x_copy_to_aux(hw, 0x7E0038, HFA384x_AUX_CTL_EXTDS,
-				    initseq, sizeof(initseq));
-		hfa384x_copy_from_aux(hw, 0x7E0038, HFA384x_AUX_CTL_EXTDS,
-				    readbuf, sizeof(initseq));
-		WLAN_HEX_DUMP(3, "readback", readbuf, sizeof(readbuf));
-
-		if (memcmp(initseq, readbuf, sizeof(readbuf))) {
-			hcr = 0x1f; /* x8 SRAM */
-			hw->isram16 = 0;
-
-			hfa384x_copy_to_aux(hw, 0x7E0038, HFA384x_AUX_CTL_EXTDS,
-					    initseq, sizeof(initseq));
-			hfa384x_corereset(hw, prism2_reset_holdtime,
-					  prism2_reset_settletime, hcr);
-
-			hfa384x_copy_to_aux(hw, 0x7E0038, HFA384x_AUX_CTL_EXTDS,
-					    initseq, sizeof(initseq));
-			hfa384x_copy_from_aux(hw, 0x7E0038, HFA384x_AUX_CTL_EXTDS,
-					    readbuf, sizeof(initseq));
-			WLAN_HEX_DUMP(2, "readback", readbuf, sizeof(readbuf));
-
-			if (memcmp(initseq, readbuf, sizeof(readbuf))) {
-				WLAN_LOG_ERROR("Genesis mode failed\n");
-				result = -1;
-				goto done;
-			}
-		}
-
-		/* Now we're in genesis mode */
-		hw->dlstate = HFA384x_DLSTATE_GENESIS;
-		goto done;
-	}
-
-	/* Retrieve the buffer loc&size and timeout */
-	if ( (result = hfa384x_drvr_getconfig(hw, HFA384x_RID_DOWNLOADBUFFER,
-				&(hw->bufinfo), sizeof(hw->bufinfo))) ) {
-		goto done;
-	}
-	hw->bufinfo.page = hfa384x2host_16(hw->bufinfo.page);
-	hw->bufinfo.offset = hfa384x2host_16(hw->bufinfo.offset);
-	hw->bufinfo.len = hfa384x2host_16(hw->bufinfo.len);
-	if ( (result = hfa384x_drvr_getconfig16(hw, HFA384x_RID_MAXLOADTIME,
-				&(hw->dltimeout))) ) {
-		goto done;
-	}
-	hw->dltimeout = hfa384x2host_16(hw->dltimeout);
-
-	/* Enable the aux port */
-	if ( (result = hfa384x_cmd_aux_enable(hw, 0)) ) {
-		WLAN_LOG_DEBUG(1,"Aux enable failed, result=%d.\n", result);
-		goto done;
-	}
-
-	/* Call the download(1,addr) function */
-	lowaddr = HFA384x_ADDR_CMD_MKOFF(exeaddr);
-	hiaddr =  HFA384x_ADDR_CMD_MKPAGE(exeaddr);
-
-	result = hfa384x_cmd_download(hw, HFA384x_PROGMODE_RAM,
-			lowaddr, hiaddr, 0);
-	if ( result == 0) {
-		/* Set the download state */
-		hw->dlstate = HFA384x_DLSTATE_RAMENABLED;
-	} else {
-		WLAN_LOG_DEBUG(1,"cmd_download(0x%04x, 0x%04x) failed, result=%d.\n",
-				lowaddr,hiaddr, result);
-		/* Disable  the aux port */
-		hfa384x_cmd_aux_disable(hw);
-	}
-
- done:
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_ramdl_disable
-*
-* Ends the ram download state.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_ramdl_disable(hfa384x_t *hw)
-{
-	DBFENTER;
-	/* Check that we're already in the download state */
-	if ( ( hw->dlstate != HFA384x_DLSTATE_RAMENABLED ) &&
-	     ( hw->dlstate != HFA384x_DLSTATE_GENESIS ) ) {
-		return -EINVAL;
-	}
-
-	if (hw->dlstate == HFA384x_DLSTATE_GENESIS) {
-		hfa384x_corereset(hw, prism2_reset_holdtime,
-				  prism2_reset_settletime,
-				  hw->isram16 ? 0x07: 0x17);
-		goto done;
-	}
-
-	/* Disable the aux port */
-	hfa384x_cmd_download(hw, HFA384x_PROGMODE_DISABLE, 0, 0 , 0);
-
- done:
-	hw->dlstate = HFA384x_DLSTATE_DISABLED;
-	hfa384x_cmd_aux_disable(hw);
-
-	DBFEXIT;
-	return 0;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_ramdl_write
-*
-* Performs a RAM download of a chunk of data. First checks to see
-* that we're in the RAM download state, then uses the aux functions
-* to 1) copy the data, 2) readback and compare.  The download
-* state is unaffected.  When all data has been written using
-* this function, call drvr_ramdl_disable() to end the download state
-* and restart the MAC.
-*
-* Arguments:
-*	hw		device structure
-*	daddr		Card address to write to. (host order)
-*	buf		Ptr to data to write.
-*	len		Length of data (host order).
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_ramdl_write(hfa384x_t *hw, UINT32 daddr, void* buf, UINT32 len)
-{
-	int		result = 0;
-	UINT8		*verbuf;
-	DBFENTER;
-	/* Check that we're in the ram download state */
-	if ( ( hw->dlstate != HFA384x_DLSTATE_RAMENABLED ) &&
-	     ( hw->dlstate != HFA384x_DLSTATE_GENESIS ) ) {
-		return -EINVAL;
-	}
-
-	WLAN_LOG_INFO("Writing %d bytes to ram @0x%06x\n", len, daddr);
-#if 0
-WLAN_HEX_DUMP(1, "dldata", buf, len);
-#endif
-	/* Copy the data via the aux port */
-	hfa384x_copy_to_aux(hw, daddr, HFA384x_AUX_CTL_EXTDS, buf, len);
-
-	/* Create a buffer for the verify */
-	verbuf = kmalloc(len, GFP_KERNEL);
-	if (verbuf == NULL ) return 1;
-
-	/* Read back and compare */
-	hfa384x_copy_from_aux(hw, daddr, HFA384x_AUX_CTL_EXTDS, verbuf, len);
-
-	if ( memcmp(buf, verbuf, len) ) {
-		WLAN_LOG_DEBUG(1,"ramdl verify failed!\n");
-		result = -EINVAL;
-	}
-
-	kfree_s(verbuf, len);
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_flashdl_enable
-*
-* Begins the flash download state.  Checks to see that we're not
-* already in a download state and that a port isn't enabled.
-* Sets the download state and retrieves the flash download
-* buffer location, buffer size, and timeout length.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_flashdl_enable(hfa384x_t *hw)
-{
-	int		result = 0;
-	int		i;
-
-	DBFENTER;
-	/* Check that a port isn't active */
-	for ( i = 0; i < HFA384x_PORTID_MAX; i++) {
-		if ( hw->port_enabled[i] ) {
-			WLAN_LOG_DEBUG(1,"called when port enabled.\n");
-			return -EINVAL;
-		}
-	}
-
-	/* Check that we're not already in a download state */
-	if ( hw->dlstate != HFA384x_DLSTATE_DISABLED ) {
-		return -EINVAL;
-	}
-
-	/* Retrieve the buffer loc&size and timeout */
-	if ( (result = hfa384x_drvr_getconfig(hw, HFA384x_RID_DOWNLOADBUFFER,
-				&(hw->bufinfo), sizeof(hw->bufinfo))) ) {
-		return result;
-	}
-	hw->bufinfo.page = hfa384x2host_16(hw->bufinfo.page);
-	hw->bufinfo.offset = hfa384x2host_16(hw->bufinfo.offset);
-	hw->bufinfo.len = hfa384x2host_16(hw->bufinfo.len);
-	if ( (result = hfa384x_drvr_getconfig16(hw, HFA384x_RID_MAXLOADTIME,
-				&(hw->dltimeout))) ) {
-		return result;
-	}
-	hw->dltimeout = hfa384x2host_16(hw->dltimeout);
-
-	/* Enable the aux port */
-	if ( (result = hfa384x_cmd_aux_enable(hw, 0)) ) {
-		return result;
-	}
-
-	hw->dlstate = HFA384x_DLSTATE_FLASHENABLED;
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_flashdl_disable
-*
-* Ends the flash download state.  Note that this will cause the MAC
-* firmware to restart.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_flashdl_disable(hfa384x_t *hw)
-{
-	DBFENTER;
-	/* Check that we're already in the download state */
-	if ( hw->dlstate != HFA384x_DLSTATE_FLASHENABLED ) {
-		return -EINVAL;
-	}
-
-	/* There isn't much we can do at this point, so I don't */
-	/*  bother  w/ the return value */
-	hfa384x_cmd_download(hw, HFA384x_PROGMODE_DISABLE, 0, 0 , 0);
-	hw->dlstate = HFA384x_DLSTATE_DISABLED;
-
-	/* Disable the aux port */
-	hfa384x_cmd_aux_disable(hw);
-
-	DBFEXIT;
-	return 0;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_flashdl_write
-*
-* Performs a FLASH download of a chunk of data. First checks to see
-* that we're in the FLASH download state, then sets the download
-* mode, uses the aux functions to 1) copy the data to the flash
-* buffer, 2) sets the download 'write flash' mode, 3) readback and
-* compare.  Lather rinse, repeat as many times an necessary to get
-* all the given data into flash.
-* When all data has been written using this function (possibly
-* repeatedly), call drvr_flashdl_disable() to end the download state
-* and restart the MAC.
-*
-* Arguments:
-*	hw		device structure
-*	daddr		Card address to write to. (host order)
-*	buf		Ptr to data to write.
-*	len		Length of data (host order).
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_flashdl_write(hfa384x_t *hw, UINT32 daddr, void* buf, UINT32 len)
-{
-	int		result = 0;
-	UINT8		*verbuf;
-	UINT32		dlbufaddr;
-	UINT32		currlen;
-	UINT32		currdaddr;
-	UINT16		destlo;
-	UINT16		desthi;
-	int		nwrites;
-	int		i;
-
-	DBFENTER;
-	/* Check that we're in the flash download state */
-	if ( hw->dlstate != HFA384x_DLSTATE_FLASHENABLED ) {
-		return -EINVAL;
-	}
-
-	WLAN_LOG_INFO("Download %d bytes to flash @0x%06x\n", len, daddr);
-
-	/* Need a flat address for arithmetic */
-	dlbufaddr = HFA384x_ADDR_AUX_MKFLAT(
-			hw->bufinfo.page,
-			hw->bufinfo.offset);
-	verbuf = kmalloc(hw->bufinfo.len, GFP_KERNEL);
-
-#if 0
-WLAN_LOG_WARNING("dlbuf@0x%06lx len=%d to=%d\n", dlbufaddr, hw->bufinfo.len, hw->dltimeout);
-#endif
-	/* Figure out how many times to to the flash prog */
-	nwrites = len / hw->bufinfo.len;
-	nwrites += (len % hw->bufinfo.len) ? 1 : 0;
-
-	if ( verbuf == NULL ) {
-		WLAN_LOG_ERROR("Failed to allocate flash verify buffer\n");
-		return 1;
-	}
-	/* For each */
-	for ( i = 0; i < nwrites; i++) {
-		/* Get the dest address and len */
-		currlen = (len - (hw->bufinfo.len * i)) > hw->bufinfo.len ?
-				hw->bufinfo.len :
-				(len - (hw->bufinfo.len * i));
-		currdaddr = daddr + (hw->bufinfo.len * i);
-		destlo = HFA384x_ADDR_CMD_MKOFF(currdaddr);
-		desthi = HFA384x_ADDR_CMD_MKPAGE(currdaddr);
-		WLAN_LOG_INFO("Writing %d bytes to flash @0x%06x\n", currlen, currdaddr);
-#if 0
-WLAN_HEX_DUMP(1, "dldata", buf+(hw->bufinfo.len*i), currlen);
-#endif
-		/* Set the download mode */
-		result = hfa384x_cmd_download(hw, HFA384x_PROGMODE_NV,
-				destlo, desthi, currlen);
-		if ( result ) {
-			WLAN_LOG_ERROR("download(NV,lo=%x,hi=%x,len=%x) "
-				"cmd failed, result=%d. Aborting d/l\n",
-				destlo, desthi, currlen, result);
-			goto exit_proc;
-		}
-		/* copy the data to the flash buffer */
-		hfa384x_copy_to_aux(hw, dlbufaddr, HFA384x_AUX_CTL_EXTDS,
-					buf+(hw->bufinfo.len*i), currlen);
-		/* set the download 'write flash' mode */
-		result = hfa384x_cmd_download(hw, HFA384x_PROGMODE_NVWRITE, 0,0,0);
-		if ( result ) {
-			WLAN_LOG_ERROR(
-				"download(NVWRITE,lo=%x,hi=%x,len=%x) "
-				"cmd failed, result=%d. Aborting d/l\n",
-				destlo, desthi, currlen, result);
-			goto exit_proc;
-		}
-		/* readback and compare, if fail...bail */
-		hfa384x_copy_from_aux(hw,
-				currdaddr, HFA384x_AUX_CTL_NV,
-				verbuf, currlen);
-
-		if ( memcmp(buf+(hw->bufinfo.len*i), verbuf, currlen) ) {
-			return -EINVAL;
-		}
-	}
-
-exit_proc:
-         /* DOH! This kfree's for you Mark :-) My forehead hurts... */
-         kfree(verbuf);
-
-	/* Leave the firmware in the 'post-prog' mode.  flashdl_disable will */
-	/*  actually disable programming mode.  Remember, that will cause the */
-	/*  the firmware to effectively reset itself. */
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_initialize
-*
-* Issues the initialize command and sets the hw->state based
-* on the result.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_initialize(hfa384x_t *hw)
-{
-	int result = 0;
-	int i;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	/* we don't want to be interrupted during the reset */
-	hfa384x_setreg(hw, 0, HFA384x_INTEN);
-	hfa384x_setreg(hw, 0xffff, HFA384x_EVACK);
-
-	cmd.cmd = HFA384x_CMDCODE_INIT;
-	cmd.parm0 = 0;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	if ( result == 0 ) {
-		for ( i = 0; i < HFA384x_NUMPORTS_MAX; i++) {
-			hw->port_enabled[i] = 0;
-		}
-	}
-
-        hw->link_status = HFA384x_LINK_NOTCONNECTED;
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_commtallies
-*
-* Send a commtallies inquiry to the MAC.  Note that this is an async
-* call that will result in an info frame arriving sometime later.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	zero		success.
-*
-* Side effects:
-*
-* Call context:
-*	process
-----------------------------------------------------------------*/
-int hfa384x_drvr_commtallies( hfa384x_t *hw )
-{
-	hfa384x_metacmd_t cmd;
-	int result;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMDCODE_INQ;
-	cmd.parm0 = HFA384x_IT_COMMTALLIES;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_enable
-*
-* Issues the enable command to enable communications on one of
-* the MACs 'ports'.  Only macport 0 is valid  for stations.
-* APs may also enable macports 1-6.  Only ports that are currently
-* disabled may be enabled.
-*
-* Arguments:
-*	hw		device structure
-*	macport		MAC port number
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_enable(hfa384x_t *hw, UINT16 macport)
-{
-	int	result = 0;
-
-	DBFENTER;
-	if ((!hw->isap && macport != 0) ||
-	    (hw->isap && !(macport <= HFA384x_PORTID_MAX)) ||
-	    (hw->port_enabled[macport]) ){
-		result = -EINVAL;
-	} else {
-		result = hfa384x_cmd_enable(hw, macport);
-		if ( result == 0 ) {
-			hw->port_enabled[macport] = 1;
-		}
-	}
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_enable
-*
-* Issues the the enable command to enable communications on one of the
-* MACs 'ports'.
-*
-* Arguments:
-*	hw		device structure
-*	macport		MAC port number
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_enable(hfa384x_t *hw, UINT16 macport)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_ENABLE) |
-		  HFA384x_CMD_MACPORT_SET(macport);
-	cmd.parm0 = 0;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_disable
-*
-* Issues the disable command to stop communications on one of
-* the MACs 'ports'.  Only macport 0 is valid  for stations.
-* APs may also disable macports 1-6.  Only ports that have been
-* previously enabled may be disabled.
-*
-* Arguments:
-*	hw		device structure
-*	macport		MAC port number (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_disable(hfa384x_t *hw, UINT16 macport)
-{
-	int	result = 0;
-
-	DBFENTER;
-	if ((!hw->isap && macport != 0) ||
-	    (hw->isap && !(macport <= HFA384x_PORTID_MAX)) ||
-	    !(hw->port_enabled[macport]) ){
-		result = -EINVAL;
-	} else {
-		result = hfa384x_cmd_disable(hw, macport);
-		if ( result == 0 ) {
-			hw->port_enabled[macport] = 0;
-		}
-	}
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_disable
-*
-* Issues the command to disable a port.
-*
-* Arguments:
-*	hw		device structure
-*	macport		MAC port number (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_disable(hfa384x_t *hw, UINT16 macport)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_DISABLE) |
-		  HFA384x_CMD_MACPORT_SET(macport);
-	cmd.parm0 = 0;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_diagnose
-*
-* Issues the diagnose command to test the: register interface,
-* MAC controller (including loopback), External RAM, Non-volatile
-* memory integrity, and synthesizers.  Following execution of this
-* command, MAC/firmware are in the 'initial state'.  Therefore,
-* the Initialize command should be issued after successful
-* completion of this command.  This function may only be called
-* when the MAC is in the 'communication disabled' state.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-#define DIAG_PATTERNA ((UINT16)0xaaaa)
-#define DIAG_PATTERNB ((UINT16)0x5555)
-
-int hfa384x_cmd_diagnose(hfa384x_t *hw)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_DIAG);
-	cmd.parm0 = DIAG_PATTERNA;
-	cmd.parm1 = DIAG_PATTERNB;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_allocate
-*
-* Issues the allocate command instructing the firmware to allocate
-* a 'frame structure buffer' in MAC controller RAM.  This command
-* does not provide the result, it only initiates one of the f/w's
-* asynchronous processes to construct the buffer.  When the
-* allocation is complete, it will be indicated via the Alloc
-* bit in the EvStat register and the FID identifying the allocated
-* space will be available from the AllocFID register.  Some care
-* should be taken when waiting for the Alloc event.  If a Tx or
-* Notify command w/ Reclaim has been previously executed, it's
-* possible the first Alloc event after execution of this command
-* will be for the reclaimed buffer and not the one you asked for.
-* This case must be handled in the Alloc event handler.
-*
-* Arguments:
-*	hw		device structure
-*	len		allocation length, must be an even value
-*			in the range [4-2400]. (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_allocate(hfa384x_t *hw, UINT16 len)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	if ( (len % 2) ||
-	     len < HFA384x_CMD_ALLOC_LEN_MIN ||
-	     len > HFA384x_CMD_ALLOC_LEN_MAX ) {
-		result = -EINVAL;
-	} else {
-		cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_ALLOC);
-		cmd.parm0 = len;
-		cmd.parm1 = 0;
-		cmd.parm2 = 0;
-
-		spin_lock_bh(&hw->cmdlock);
-		result = hfa384x_docmd_wait(hw, &cmd);
-		spin_unlock_bh(&hw->cmdlock);
-	}
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_transmit
-*
-* Instructs the firmware to transmit a frame previously copied
-* to a given buffer.  This function returns immediately, the Tx
-* results are available via the Tx or TxExc events (if the frame
-* control bits are set).  The reclaim argument specifies if the
-* FID passed will be used by the f/w tx process or returned for
-* use w/ another transmit command.  If reclaim is set, expect an
-* Alloc event signalling the availibility of the FID for reuse.
-*
-* NOTE: hw->cmdlock MUST BE HELD before calling this function!
-*
-* Arguments:
-*	hw		device structure
-*	reclaim		[0|1] indicates whether the given FID will
-*			be handed back (via Alloc event) for reuse.
-*			(host order)
-*	qos		[0-3] Value to put in the QoS field of the
-*			tx command, identifies a queue to place the
-*			outgoing frame in.
-*			(host order)
-*	fid		FID of buffer containing the frame that was
-*			previously copied to MAC memory via the bap.
-*			(host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*	hw->resp0 will contain the FID being used by async tx
-*	process.  If reclaim==0, resp0 will be the same as the fid
-*	argument.  If reclaim==1, resp0 will be the different and
-*	is the value to watch for in the Tx|TxExc to indicate completion
-*	of the frame passed in fid.
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_transmit(hfa384x_t *hw, UINT16 reclaim, UINT16 qos, UINT16 fid)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_TX) |
-		HFA384x_CMD_RECL_SET(reclaim) |
-		HFA384x_CMD_QOS_SET(qos);
-	cmd.parm0 = fid;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	result = hfa384x_docmd_wait(hw, &cmd);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_clearpersist
-*
-* Instructs the firmware to clear the persistence bit in a given
-* FID.  This has the effect of telling the firmware to drop the
-* persistent frame.  The FID must be one that was previously used
-* to transmit a PRST frame.
-*
-* Arguments:
-*	hw		device structure
-*	fid		FID of the persistent frame (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_clearpersist(hfa384x_t *hw, UINT16 fid)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_CLRPRST);
-	cmd.parm0 = fid;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_notify
-*
-* Sends an info frame to the firmware to alter the behavior
-* of the f/w asynch processes.  Can only be called when the MAC
-* is in the enabled state.
-*
-* Arguments:
-*	hw		device structure
-*	reclaim		[0|1] indicates whether the given FID will
-*			be handed back (via Alloc event) for reuse.
-*			(host order)
-*	fid		FID of buffer containing the frame that was
-*			previously copied to MAC memory via the bap.
-*			(host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*	hw->resp0 will contain the FID being used by async notify
-*	process.  If reclaim==0, resp0 will be the same as the fid
-*	argument.  If reclaim==1, resp0 will be the different.
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_notify(hfa384x_t *hw, UINT16 reclaim, UINT16 fid,
-		       void *buf, UINT16 len)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_NOTIFY) |
-		HFA384x_CMD_RECL_SET(reclaim);
-	cmd.parm0 = fid;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-
-        /* Copy the record to FID */
-        result = hfa384x_copy_to_bap(hw, HFA384x_BAP_PROC, hw->infofid, 0, buf, len);
-        if ( result ) {
-                WLAN_LOG_DEBUG(1,
-			"copy_to_bap(%04x, 0, %d) failed, result=0x%x\n",
-                        hw->infofid, len, result);
-		result = -EIO;
-                goto failed;
-        }
-
-	result = hfa384x_docmd_wait(hw, &cmd);
-
- failed:
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-#if 0
-/*----------------------------------------------------------------
-* hfa384x_cmd_inquiry
-*
-* Requests an info frame from the firmware.  The info frame will
-* be delivered asynchronously via the Info event.
-*
-* Arguments:
-*	hw		device structure
-*	fid		FID of the info frame requested. (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-static int hfa384x_cmd_inquiry(hfa384x_t *hw, UINT16 fid)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_INQ);
-	cmd.parm0 = fid;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-#endif
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_access
-*
-* Requests that a given record be copied to/from the record
-* buffer.  If we're writing from the record buffer, the contents
-* must previously have been written to the record buffer via the
-* bap.  If we're reading into the record buffer, the record can
-* be read out of the record buffer after this call.
-*
-* Arguments:
-*	hw		device structure
-*	write		[0|1] copy the record buffer to the given
-*			configuration record. (host order)
-*	rid		RID of the record to read/write. (host order)
-*	buf		host side record buffer.  Upon return it will
-*			contain the body portion of the record (minus the
-*			RID and len).
-*	len		buffer length (in bytes, should match record length)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_access(hfa384x_t *hw, UINT16 write, UINT16 rid,
-		       void* buf, UINT16 len)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-	hfa384x_rec_t	rec;
-
-	DBFENTER;
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,0))
-	/* This should NOT be called in interrupt context! */
-	if (in_irq()) {
-		WLAN_LOG_ERROR("Krap, in Interrupt context!");
-#ifdef WLAN_INCLUDE_DEBUG
-		BUG();
-#endif
-	}
-#endif
-	spin_lock_bh(&hw->cmdlock);
-
-	if (write) {
-		rec.rid = host2hfa384x_16(rid);
-		rec.reclen = host2hfa384x_16((len/2) + 1); /* note conversion to words, +1 for rid field */
-		/* write the record */
-		result = hfa384x_copy_to_bap4( hw, HFA384x_BAP_PROC, rid, 0,
-					       &rec, sizeof(rec),
-					       buf, len,
-					       NULL, 0, NULL, 0);
-		if ( result ) {
-			WLAN_LOG_DEBUG(3,"Failure writing record header+data\n");
-			goto fail;
-		}
-
-	}
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_ACCESS) |
-		HFA384x_CMD_WRITE_SET(write);
-	cmd.parm0 = rid;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	result = hfa384x_docmd_wait(hw, &cmd);
-	if ( result ) {
-		WLAN_LOG_ERROR("Call to hfa384x_docmd_wait failed (%d %d)\n",
-				result, cmd.result.resp0);
-		goto fail;
-	}
-
-	if (!write) {
-		result = hfa384x_copy_from_bap( hw, HFA384x_BAP_PROC, rid, 0, &rec, sizeof(rec));
-		if ( result ) {
-			WLAN_LOG_DEBUG(3,"Call to hfa384x_copy_from_bap failed\n");
-			goto fail;
-		}
-
-		/* Validate the record length */
-		if ( ((hfa384x2host_16(rec.reclen)-1)*2) != len ) {  /* note body len calculation in bytes */
-			WLAN_LOG_DEBUG(1, "RID len mismatch, rid=0x%04x hlen=%d fwlen=%d\n",
-					rid, len, (hfa384x2host_16(rec.reclen)-1)*2);
-			result = -ENODATA;
-			goto fail;
-		}
-
-		result = hfa384x_copy_from_bap( hw, HFA384x_BAP_PROC, rid, sizeof(rec), buf, len);
-
-	}
-
- fail:
-	spin_unlock_bh(&hw->cmdlock);
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_monitor
-*
-* Enables the 'monitor mode' of the MAC.  Here's the description of
-* monitor mode that I've received thus far:
-*
-*  "The "monitor mode" of operation is that the MAC passes all
-*  frames for which the PLCP checks are correct. All received
-*  MPDUs are passed to the host with MAC Port = 7, with a
-*  receive status of good, FCS error, or undecryptable. Passing
-*  certain MPDUs is a violation of the 802.11 standard, but useful
-*  for a debugging tool."  Normal communication is not possible
-*  while monitor mode is enabled.
-*
-* Arguments:
-*	hw		device structure
-*	enable		a code (0x0b|0x0f) that enables/disables
-*			monitor mode. (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_monitor(hfa384x_t *hw, UINT16 enable)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_MONITOR) |
-		HFA384x_CMD_AINFO_SET(enable);
-	cmd.parm0 = 0;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_download
-*
-* Sets the controls for the MAC controller code/data download
-* process.  The arguments set the mode and address associated
-* with a download.  Note that the aux registers should be enabled
-* prior to setting one of the download enable modes.
-*
-* Arguments:
-*	hw		device structure
-*	mode		0 - Disable programming and begin code exec
-*			1 - Enable volatile mem programming
-*			2 - Enable non-volatile mem programming
-*			3 - Program non-volatile section from NV download
-*			    buffer.
-*			(host order)
-*	lowaddr
-*	highaddr	For mode 1, sets the high & low order bits of
-*			the "destination address".  This address will be
-*			the execution start address when download is
-*			subsequently disabled.
-*			For mode 2, sets the high & low order bits of
-*			the destination in NV ram.
-*			For modes 0 & 3, should be zero. (host order)
-*			NOTE: these address args are in CMD format
-*	codelen		Length of the data to write in mode 2,
-*			zero otherwise. (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_download(hfa384x_t *hw, UINT16 mode, UINT16 lowaddr,
-				UINT16 highaddr, UINT16 codelen)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_DOWNLD) |
-		HFA384x_CMD_PROGMODE_SET(mode);
-	cmd.parm0 = lowaddr;
-	cmd.parm1 = highaddr;
-	cmd.parm2 = codelen;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_dl_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_aux_enable
-*
-* Goes through the process of enabling the auxilary port.  This
-* is necessary prior to raw reads/writes to card data space.
-* Direct access to the card data space is only used for downloading
-* code and debugging.
-* Note that a call to this function is required before attempting
-* a download.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_aux_enable(hfa384x_t *hw, int force)
-{
-	int		result = -ETIMEDOUT;
-	unsigned long	flags;
-	UINT32		retries_remaining;
-	UINT16		reg;
-	UINT		auxen_mirror = hw->auxen;
-
-	DBFENTER;
-
-	/* Check for existing enable */
-	if ( hw->auxen ) {
-		hw->auxen++;
-		return 0;
-	}
-
-	/* acquire the lock */
-	spin_lock_irqsave( &(hw->cmdlock), flags);
-	/* wait for cmd register busy bit to clear */
-	retries_remaining = 100000;
-	do {
-		reg = hfa384x_getreg(hw, HFA384x_CMD);
-		udelay(10);
-	}
-	while (HFA384x_CMD_ISBUSY(reg) && --retries_remaining);
-	if (retries_remaining != 0) {
-		/* busy bit clear, it's OK to write to ParamX regs */
-		hfa384x_setreg(hw, HFA384x_AUXPW0,
-			HFA384x_PARAM0);
-		hfa384x_setreg(hw, HFA384x_AUXPW1,
-			HFA384x_PARAM1);
-		hfa384x_setreg(hw, HFA384x_AUXPW2,
-			HFA384x_PARAM2);
-
-		/* Set the aux enable in the Control register */
-		hfa384x_setreg(hw, HFA384x_CONTROL_AUX_DOENABLE,
-			HFA384x_CONTROL);
-
-		/* Now wait for completion */
-		retries_remaining = 100000;
-		do {
-			reg = hfa384x_getreg(hw, HFA384x_CONTROL);
-			udelay(10);
-		}
-		while ( ((reg & (BIT14|BIT15)) != HFA384x_CONTROL_AUX_ISENABLED) &&
-			--retries_remaining );
-		if (retries_remaining != 0) {
-			result = 0;
-			hw->auxen++;
-		}
-	}
-
-	/* Force it enabled even if the command failed, if told.. */
-	if ((hw->auxen == auxen_mirror) && force)
-		hw->auxen++;
-
-	spin_unlock_irqrestore( &(hw->cmdlock), flags);
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_aux_disable
-*
-* Goes through the process of disabling the auxilary port
-* enabled with aux_enable().
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_aux_disable(hfa384x_t *hw)
-{
-	int		result = -ETIMEDOUT;
-	unsigned long   timeout;
-	UINT16		reg = 0;
-
-	DBFENTER;
-
-	/* See if there's more than one enable */
-	if (hw->auxen) hw->auxen--;
-	if (hw->auxen) return 0;
-
-	/* Clear the aux enable in the Control register */
-	hfa384x_setreg(hw, 0, HFA384x_PARAM0);
-	hfa384x_setreg(hw, 0, HFA384x_PARAM1);
-	hfa384x_setreg(hw, 0, HFA384x_PARAM2);
-	hfa384x_setreg(hw, HFA384x_CONTROL_AUX_DODISABLE,
-		HFA384x_CONTROL);
-
-	/* Now wait for completion */
-	timeout = jiffies + 1*HZ;
-	reg = hfa384x_getreg(hw, HFA384x_CONTROL);
-	while ( ((reg & (BIT14|BIT15)) != HFA384x_CONTROL_AUX_ISDISABLED) &&
-		time_before(jiffies,timeout) ){
-		udelay(10);
-		reg = hfa384x_getreg(hw, HFA384x_CONTROL);
-	}
-	if ((reg & (BIT14|BIT15)) == HFA384x_CONTROL_AUX_ISDISABLED ) {
-		result = 0;
-	}
-	DBFEXIT;
-	return result;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_low_level
-*
-* Write test commands to the card.  Some test commands don't make
-* sense without prior set-up.  For example, continous TX isn't very
-* useful until you set the channel.  That functionality should be
-*
-* Side effects:
-*
-* Call context:
-*      process thread
-* -----------------------------------------------------------------*/
-int hfa384x_drvr_low_level(hfa384x_t *hw, hfa384x_metacmd_t *cmd)
-{
-	int             result = 0;
-	DBFENTER;
-
-	/* Do i need a host2hfa... conversion ? */
-#if 0
-	printk(KERN_INFO "%#x %#x %#x %#x\n", cmd->cmd, cmd->parm0, cmd->parm1, cmd->parm2);
-#endif
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/* TODO: determine if these will ever be needed */
-#if 0
-int hfa384x_cmd_readmif(hfa384x_t *hw)
-{
-	DBFENTER;
-	DBFEXIT;
-	return 0;
-}
-
-
-int hfa384x_cmd_writemif(hfa384x_t *hw)
-{
-	DBFENTER;
-	DBFEXIT;
-	return 0;
-}
-#endif
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_mmi_read
-*
-* Read mmi registers.  mmi is intersil-speak for the baseband
-* processor registers.
-*
-* Arguments:
-*       hw              device structure
-*       register        The test register to be accessed (must be even #).
-*
-* Returns:
-*       0               success
-*       >0              f/w reported error - f/w status code
-*       <0              driver reported error
-*
-* Side effects:
-*
-* Call context:
-*       process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_mmi_read(hfa384x_t *hw, UINT32 addr, UINT32 *resp)
-{
-        int             result = 0;
-	hfa384x_metacmd_t cmd;
-
-        DBFENTER;
-	cmd.cmd = (UINT16) 0x30;
-	cmd.parm0 = (UINT16) addr;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-        /* Do i need a host2hfa... conversion ? */
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	*resp = (UINT32) cmd.result.resp0;
-
-        DBFEXIT;
-        return result;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_mmi_write
-*
-* Read mmi registers.  mmi is intersil-speak for the baseband
-* processor registers.
-*
-* Arguments:
-*       hw              device structure
-*       addr            The test register to be accessed (must be even #).
-*       data            The data value to write to the register.
-*
-* Returns:
-*       0               success
-*       >0              f/w reported error - f/w status code
-*       <0              driver reported error
-*
-* Side effects:
-*
-* Call context:
-*       process thread
-----------------------------------------------------------------*/
-
-int
-hfa384x_drvr_mmi_write(hfa384x_t *hw, UINT32 addr, UINT32 data)
-{
-        int             result = 0;
-	hfa384x_metacmd_t cmd;
-
-        DBFENTER;
-	cmd.cmd = (UINT16) 0x31;
-	cmd.parm0 = (UINT16) addr;
-	cmd.parm1 = (UINT16) data;
-	cmd.parm2 = 0;
-
-        WLAN_LOG_DEBUG(1,"mmi write : addr = 0x%08x\n", addr);
-        WLAN_LOG_DEBUG(1,"mmi write : data = 0x%08x\n", data);
-
-        /* Do i need a host2hfa... conversion ? */
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-        DBFEXIT;
-        return result;
-}
-
-
-/* TODO: determine if these will ever be needed */
-#if 0
-int hfa384x_cmd_readmif(hfa384x_t *hw)
-{
-        DBFENTER;
-        DBFEXIT;
-        return 0;
-}
-
-
-int hfa384x_cmd_writemif(hfa384x_t *hw)
-{
-        DBFENTER;
-        DBFEXIT;
-        return 0;
-}
-#endif
-
-
-
-/*----------------------------------------------------------------
-* hfa384x_copy_from_bap
-*
-* Copies a collection of bytes from the MAC controller memory via
-* one set of BAP registers.
-*
-* Arguments:
-*	hw		device structure
-*	bap		[0|1] which BAP to use
-*	id		FID or RID, destined for the select register (host order)
-*	offset		An _even_ offset into the buffer for the given
-*			FID/RID.  We haven't the means to validate this,
-*			so be careful. (host order)
-*	buf		ptr to array of bytes
-*	len		length of data to transfer in bytes
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - value of offset reg.
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-*	interrupt
-----------------------------------------------------------------*/
-int hfa384x_copy_from_bap(hfa384x_t *hw, UINT16 bap, UINT16 id, UINT16 offset,
-				void *buf, UINT len)
-{
-	int		result = 0;
-	unsigned long	flags = 0;
-	UINT8		*d = (UINT8*)buf;
-	UINT		selectreg;
-	UINT		offsetreg;
-	UINT		datareg;
-	UINT		i;
-	UINT16		reg = 0;
-
-	DBFENTER;
-
-	/* Validate bap, offset, buf, and len */
-	if ( (bap > 1) ||
-	     (offset > HFA384x_BAP_OFFSET_MAX) ||
-	     (offset % 2) ||
-	     (buf == NULL) ||
-	     (len > HFA384x_BAP_DATALEN_MAX) ){
-	     	result = -EINVAL;
-	} else {
-		selectreg = (bap == 1) ?  HFA384x_SELECT1 : HFA384x_SELECT0 ;
-		offsetreg = (bap == 1) ?  HFA384x_OFFSET1 : HFA384x_OFFSET0 ;
-		datareg =   (bap == 1) ?  HFA384x_DATA1 : HFA384x_DATA0 ;
-
-		/* Obtain lock */
-		spin_lock_irqsave( &(hw->baplock), flags);
-
-		/* Write id to select reg */
-		hfa384x_setreg(hw, id, selectreg);
-		/* Write offset to offset reg */
-		hfa384x_setreg(hw, offset, offsetreg);
-		/* Wait for offset[busy] to clear (see BAP_TIMEOUT) */
-		i = 0;
-		do {
-			reg = hfa384x_getreg(hw, offsetreg);
-			if ( i > 0 ) udelay(10);
-			i++;
-		} while ( i < prism2_bap_timeout && HFA384x_OFFSET_ISBUSY(reg));
-#if (WLAN_HOSTIF != WLAN_PCI)
-		/* Release lock */
-		spin_unlock_irqrestore( &(hw->baplock), flags);
-#endif
-
-		if ( HFA384x_OFFSET_ISBUSY(reg) ){
-			/* If timeout, return -ETIMEDOUT */
-			result = reg;
-		} else if ( HFA384x_OFFSET_ISERR(reg) ){
-			/* If offset[err] == 1, return -EINVAL */
-			result = reg;
-		} else {
-			/* Read even(len) buf contents from data reg */
-			for ( i = 0; i < (len & 0xfffe); i+=2 ) {
-				*(UINT16*)(&(d[i])) =
-					hfa384x_getreg_noswap(hw, datareg);
-			}
-			/* If len odd, handle last byte */
-			if ( len % 2 ){
-				reg = hfa384x_getreg_noswap(hw, datareg);
-				d[len-1] = ((UINT8*)(&reg))[0];
-			}
-		}
-
-		/* According to Intersil errata dated 9/16/02:
-
-		"In PRISM PCI MAC host interface, if both BAPs are concurrently
-		 requesing memory access, both will accept the Ack.   There is no
-		 firmware workaround possible.  To prevent BAP access failures or
-		 hang conditions the host MUST NOT access both BAPs in sucession
-		 unless at least 5us elapses between accesses.  The safest choice
-		 is to USE ONLY ONE BAP for all data movement operations."
-
-		 What this means:
-
-		 We have to serialize ALL BAP accesses, and furthermore, add a 5us
-		 delay after access if we're using a PCI platform.
-
-		 Unfortunately, this means we have to lock out interrupts througout
-		 the entire BAP copy.
-
-		 It remains to be seen if "BAP access" means "BAP setup" or the more
-		 literal definition of "copying data back and forth"  I'm erring for
-		 the latter, safer definition.  -- SLP.
-
-		*/
-
-#if (WLAN_HOSTIF == WLAN_PCI)
-		udelay(5);
-		/* Release lock */
-		spin_unlock_irqrestore( &(hw->baplock), flags);
-#endif
-
-	}
-
-	if (result) {
-	  WLAN_LOG_DEBUG(1,
-			  "copy_from_bap(0x%04x, 0, %d) failed, result=0x%x\n",
-			  reg, len, result);
-	}
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_copy_to_bap
-*
-* Copies a collection of bytes to the MAC controller memory via
-* one set of BAP registers.
-*
-* Arguments:
-*	hw		device structure
-*	bap		[0|1] which BAP to use
-*	id		FID or RID, destined for the select register (host order)
-*	offset		An _even_ offset into the buffer for the given
-*			FID/RID.  We haven't the means to validate this,
-*			so be careful. (host order)
-*	buf		ptr to array of bytes
-*	len		length of data to transfer (in bytes)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - value of offset reg.
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-*	interrupt
-----------------------------------------------------------------*/
-int hfa384x_copy_to_bap(hfa384x_t *hw, UINT16 bap, UINT16 id, UINT16 offset,
-				void *buf, UINT len)
-{
-	return hfa384x_copy_to_bap4(hw, bap, id, offset, buf, len, NULL, 0, NULL, 0, NULL, 0);
-}
-
-int hfa384x_copy_to_bap4(hfa384x_t *hw, UINT16 bap, UINT16 id, UINT16 offset,
-			 void *buf, UINT len1, void* buf2, UINT len2,
-			 void *buf3, UINT len3, void *buf4, UINT len4)
-{
-	int		result = 0;
-	unsigned long	flags = 0;
-	UINT8		*d;
-	UINT		selectreg;
-	UINT		offsetreg;
-	UINT		datareg;
-	UINT		i;
-	UINT16		reg;
-
-	DBFENTER;
-
-//	printk(KERN_DEBUG "ctb1 %d id %04x o %d %d %d %d %d\n", bap, id, offset, len1, len2, len3, len4);
-
-	/* Validate bap, offset, buf, and len */
-	if ( (bap > 1) ||
-	     (offset > HFA384x_BAP_OFFSET_MAX) ||
-	     (offset % 2) ||
-	     (buf == NULL) ||
-	     (len1+len2+len3+len4 > HFA384x_BAP_DATALEN_MAX) ){
-	     	result = -EINVAL;
-	} else {
-		selectreg = (bap == 1) ? HFA384x_SELECT1 : HFA384x_SELECT0;
-		offsetreg = (bap == 1) ? HFA384x_OFFSET1 : HFA384x_OFFSET0;
-		datareg =   (bap == 1) ? HFA384x_DATA1   : HFA384x_DATA0;
-		/* Obtain lock */
-		spin_lock_irqsave( &(hw->baplock), flags);
-
-		/* Write id to select reg */
-		hfa384x_setreg(hw, id, selectreg);
-		udelay(10);
-		/* Write offset to offset reg */
-		hfa384x_setreg(hw, offset, offsetreg);
-		/* Wait for offset[busy] to clear (see BAP_TIMEOUT) */
-		i = 0;
-		do {
-			reg = hfa384x_getreg(hw, offsetreg);
-			if ( i > 0 ) udelay(10);
-			i++;
-		} while ( i < prism2_bap_timeout && HFA384x_OFFSET_ISBUSY(reg));
-
-#if (WLAN_HOSTIF != WLAN_PCI)
-		/* Release lock */
-		spin_unlock_irqrestore( &(hw->baplock), flags);
-#endif
-
-		if ( HFA384x_OFFSET_ISBUSY(reg) ){
-			/* If timeout, return reg */
-			result = reg;
-		} else if ( HFA384x_OFFSET_ISERR(reg) ){
-			/* If offset[err] == 1, return reg */
-			result = reg;
-		} else {
-			d = (UINT8*)buf;
-			/* Write even(len1) buf contents to data reg */
-			for ( i = 0; i < (len1 & 0xfffe); i+=2 ) {
-				hfa384x_setreg_noswap(hw,
-					*(UINT16*)(&(d[i])), datareg);
-			}
-			if (len1 & 1) {
-				UINT16 data;
-				UINT8 *b = (UINT8 *) &data;
-				b[0] = d[len1-1];
-				if (buf2 != NULL) {
-					d = (UINT8*)buf2;
-					b[1] = d[0];
-					len2--;
-					buf2++;
-				}
-				hfa384x_setreg_noswap(hw, data, datareg);
-			}
-			if ((buf2 != NULL) && (len2 > 0)) {
-				/* Write even(len2) buf contents to data reg */
-				d = (UINT8*)buf2;
-				for ( i = 0; i < (len2 & 0xfffe); i+=2 ) {
-					hfa384x_setreg_noswap(hw, *(UINT16*)(&(d[i])), datareg);
-				}
-				if (len2 & 1) {
-					UINT16 data;
-					UINT8 *b = (UINT8 *) &data;
-					b[0] = d[len2-1];
-					if (buf3 != NULL) {
-						d = (UINT8*)buf3;
-						b[1] = d[0];
-						len3--;
-						buf3++;
-					}
-					hfa384x_setreg_noswap(hw, data, datareg);
-				}
-			}
-
-			if ((buf3 != NULL) && (len3 > 0)) {
-				/* Write even(len3) buf contents to data reg */
-				d = (UINT8*)buf3;
-				for ( i = 0; i < (len3 & 0xfffe); i+=2 ) {
-					hfa384x_setreg_noswap(hw, *(UINT16*)(&(d[i])), datareg);
-				}
-				if (len3 & 1) {
-					UINT16 data;
-					UINT8 *b = (UINT8 *) &data;
-					b[0] = d[len3-1];
-					if (buf4 != NULL) {
-						d = (UINT8*)buf4;
-						b[1] = d[0];
-						len4--;
-						buf4++;
-					}
-					hfa384x_setreg_noswap(hw, data, datareg);
-				}
-			}
-			if ((buf4 != NULL) && (len4 > 0)) {
-				/* Write even(len4) buf contents to data reg */
-				d = (UINT8*)buf4;
-				for ( i = 0; i < (len4 & 0xfffe); i+=2 ) {
-					hfa384x_setreg_noswap(hw, *(UINT16*)(&(d[i])), datareg);
-				}
-				if (len4 & 1) {
-					UINT16 data;
-					UINT8 *b = (UINT8 *) &data;
-					b[0] = d[len4-1];
-					b[1] = 0;
-
-					hfa384x_setreg_noswap(hw, data, datareg);
-				}
-			}
-//			printk(KERN_DEBUG "ctb2 %d id %04x o %d %d %d %d %d\n", bap, id, offset, len1, len2, len3, len4);
-
-		}
-
-#if (WLAN_HOSTIF == WLAN_PCI)
-		udelay(5);
-		/* Release lock */
-		spin_unlock_irqrestore( &(hw->baplock), flags);
-#endif
-
-	}
-
-	if (result)
-		WLAN_LOG_ERROR("copy_to_bap() failed.\n");
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_copy_from_aux
-*
-* Copies a collection of bytes from the controller memory.  The
-* Auxiliary port MUST be enabled prior to calling this function.
-* We _might_ be in a download state.
-*
-* Arguments:
-*	hw		device structure
-*	cardaddr	address in hfa384x data space to read
-*	auxctl		address space select
-*	buf		ptr to destination host buffer
-*	len		length of data to transfer (in bytes)
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	buf contains the data copied
-*
-* Call context:
-*	process thread
-*	interrupt
-----------------------------------------------------------------*/
-void
-hfa384x_copy_from_aux(
-	hfa384x_t *hw, UINT32 cardaddr, UINT32 auxctl, void *buf, UINT len)
-{
-	UINT16		currpage;
-	UINT16		curroffset;
-	UINT		i = 0;
-
-	DBFENTER;
-
-	if ( !(hw->auxen) ) {
-		WLAN_LOG_DEBUG(1,
-			"Attempt to read 0x%04x when aux not enabled\n",
-			cardaddr);
-		return;
-
-	}
-	/* Build appropriate aux page and offset */
-	currpage = HFA384x_AUX_MKPAGE(cardaddr);
-	curroffset = HFA384x_AUX_MKOFF(cardaddr, auxctl);
-	hfa384x_setreg(hw, currpage, HFA384x_AUXPAGE);
-	hfa384x_setreg(hw, curroffset, HFA384x_AUXOFFSET);
-	udelay(5);	/* beat */
-
-	/* read the data */
-	while ( i < len) {
-		*((UINT16*)(buf+i)) = hfa384x_getreg_noswap(hw, HFA384x_AUXDATA);
-		i+=2;
-		curroffset+=2;
-		if ( (curroffset&HFA384x_ADDR_AUX_OFF_MASK) >
-			HFA384x_ADDR_AUX_OFF_MAX ) {
-			currpage++;
-			curroffset = 0;
-			curroffset = HFA384x_AUX_MKOFF(curroffset, auxctl);
-			hfa384x_setreg(hw, currpage, HFA384x_AUXPAGE);
-			hfa384x_setreg(hw, curroffset, HFA384x_AUXOFFSET);
-			udelay(5);	/* beat */
-		}
-	}
-	/* Make sure the auxctl bits are clear */
-	hfa384x_setreg(hw, 0, HFA384x_AUXOFFSET);
-	DBFEXIT;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_copy_to_aux
-*
-* Copies a collection of bytes to the controller memory.  The
-* Auxiliary port MUST be enabled prior to calling this function.
-* We _might_ be in a download state.
-*
-* Arguments:
-*	hw		device structure
-*	cardaddr	address in hfa384x data space to read
-*	auxctl		address space select
-*	buf		ptr to destination host buffer
-*	len		length of data to transfer (in bytes)
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	Controller memory now contains a copy of buf
-*
-* Call context:
-*	process thread
-*	interrupt
-----------------------------------------------------------------*/
-void
-hfa384x_copy_to_aux(
-	hfa384x_t *hw, UINT32 cardaddr, UINT32 auxctl, void *buf, UINT len)
-{
-	UINT16		currpage;
-	UINT16		curroffset;
-	UINT		i = 0;
-
-	DBFENTER;
-
-	if ( !(hw->auxen) ) {
-		WLAN_LOG_DEBUG(1,
-			"Attempt to read 0x%04x when aux not enabled\n",
-			cardaddr);
-		return;
-
-	}
-	/* Build appropriate aux page and offset */
-	currpage = HFA384x_AUX_MKPAGE(cardaddr);
-	curroffset = HFA384x_AUX_MKOFF(cardaddr, auxctl);
-	hfa384x_setreg(hw, currpage, HFA384x_AUXPAGE);
-	hfa384x_setreg(hw, curroffset, HFA384x_AUXOFFSET);
-	udelay(5);	/* beat */
-
-	/* write the data */
-	while ( i < len) {
-		hfa384x_setreg_noswap(hw,
-			*((UINT16*)(buf+i)), HFA384x_AUXDATA);
-		i+=2;
-		curroffset+=2;
-		if ( curroffset > HFA384x_ADDR_AUX_OFF_MAX ) {
-			currpage++;
-			curroffset = 0;
-			hfa384x_setreg(hw, currpage, HFA384x_AUXPAGE);
-			hfa384x_setreg(hw, curroffset, HFA384x_AUXOFFSET);
-			udelay(5);	/* beat */
-		}
-	}
-	DBFEXIT;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_wait
-*
-* Waits for availability of the Command register, then
-* issues the given command.  Then polls the Evstat register
-* waiting for command completion.  Timeouts shouldn't be
-* possible since we're preventing overlapping commands and all
-* commands should be cleared and acknowledged.
-*
-* Arguments:
-*	wlandev		device structure
-*       cmd             cmd structure.  Includes all arguments and result
-*                       data points.  All in host order.
-*
-* Returns:
-*	0		success
-*	-ETIMEDOUT	timed out waiting for register ready or
-*			command completion
-*	>0		command indicated error, Status and Resp0-2 are
-*			in hw structure.
-*
-* Side effects:
-*
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-static int hfa384x_docmd_wait( hfa384x_t *hw, hfa384x_metacmd_t *cmd)
-{
-	int		result = -ETIMEDOUT;
-	UINT16		reg = 0;
-	UINT16          counter;
-
-	DBFENTER;
-
-	hw->cmdflag = 0;
-	hw->cmddata = cmd;
-
-	/* wait for the busy bit to clear */
-	counter = 0;
-	reg = hfa384x_getreg(hw, HFA384x_CMD);
-	while ( HFA384x_CMD_ISBUSY(reg) &&
-		(counter < 10)) {
-		reg = hfa384x_getreg(hw, HFA384x_CMD);
-		counter++;
-		udelay(10);
-	}
-
-	if (HFA384x_CMD_ISBUSY(reg)) {
-		WLAN_LOG_ERROR("hfa384x_cmd timeout(1), reg=0x%0hx.\n", reg);
-		goto failed;
-	}
-	if (!HFA384x_CMD_ISBUSY(reg)) {
-		/* busy bit clear, write command */
-		hfa384x_setreg(hw, cmd->parm0, HFA384x_PARAM0);
-		hfa384x_setreg(hw, cmd->parm1, HFA384x_PARAM1);
-		hfa384x_setreg(hw, cmd->parm2, HFA384x_PARAM2);
-		hfa384x_setreg(hw, cmd->cmd, HFA384x_CMD);
-
-#ifdef CMD_IRQ
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,2,0))
-		while (! hw->cmdflag)
-			interruptible_sleep_on(&hw->cmdq);
-#else
-		wait_event_interruptible(hw->cmdq, hw->cmdflag);
-#endif
-		result = HFA384x_STATUS_RESULT_GET(cmd->status);
-#else // CMD_IRQ
-		/* Now wait for completion */
-		counter = 0;
-		reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-		/* Initialization is the problem.  It takes about
-                   100ms. "normal" commands are typically is about
-                   200-400 us (I've never seen less than 200).  Longer
-                   is better so that we're not hammering the bus. */
-		while ( !HFA384x_EVSTAT_ISCMD(reg) &&
-			(counter < 5000)) {
-			reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-			counter++;
-			udelay(200);
-		}
-
-		if ( HFA384x_EVSTAT_ISCMD(reg) ) {
-			result = 0;
-			cmd->result.status = hfa384x_getreg(hw, HFA384x_STATUS);
-			cmd->result.resp0 = hfa384x_getreg(hw, HFA384x_RESP0);
-			cmd->result.resp1 = hfa384x_getreg(hw, HFA384x_RESP1);
-			cmd->result.resp2 = hfa384x_getreg(hw, HFA384x_RESP2);
-			hfa384x_setreg(hw, HFA384x_EVACK_CMD,
-				HFA384x_EVACK);
-			result = HFA384x_STATUS_RESULT_GET(cmd->result.status);
-		} else {
-			WLAN_LOG_ERROR("hfa384x_cmd timeout(2), reg=0x%0hx.\n", reg);
-		}
-#endif  /* CMD_IRQ */
-	}
-
- failed:
-	hw->cmdflag = 0;
-	hw->cmddata = NULL;
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_dl_docmd_wait
-*
-* Waits for availability of the Command register, then
-* issues the given command.  Then polls the Evstat register
-* waiting for command completion.  Timeouts shouldn't be
-* possible since we're preventing overlapping commands and all
-* commands should be cleared and acknowledged.
-*
-* This routine is only used for downloads.  Since it doesn't lock out
-* interrupts the system response is much better.
-*
-* Arguments:
-*	wlandev		device structure
-*       cmd             cmd structure.  Includes all arguments and result
-*                       data points.  All in host order.
-*
-* Returns:
-*	0		success
-*	-ETIMEDOUT	timed out waiting for register ready or
-*			command completion
-*	>0		command indicated error, Status and Resp0-2 are
-*			in hw structure.
-*
-* Side effects:
-*
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-static int hfa384x_dl_docmd_wait( hfa384x_t *hw, hfa384x_metacmd_t *cmd)
-{
-	int		result = -ETIMEDOUT;
-	unsigned long	timeout;
-	UINT16		reg = 0;
-
-	DBFENTER;
-	/* wait for the busy bit to clear */
-	timeout = jiffies + 1*HZ;
-	reg = hfa384x_getreg(hw, HFA384x_CMD);
-	while ( HFA384x_CMD_ISBUSY(reg) && time_before( jiffies, timeout) ) {
-		reg = hfa384x_getreg(hw, HFA384x_CMD);
-		udelay(10);
-	}
-	if (HFA384x_CMD_ISBUSY(reg)) {
-		WLAN_LOG_WARNING("Timed out waiting for cmd register.\n");
-		goto failed;
-	}
-
-	if (!HFA384x_CMD_ISBUSY(reg)) {
-		/* busy bit clear, write command */
-		hfa384x_setreg(hw, cmd->parm0, HFA384x_PARAM0);
-		hfa384x_setreg(hw, cmd->parm1, HFA384x_PARAM1);
-		hfa384x_setreg(hw, cmd->parm2, HFA384x_PARAM2);
-		hfa384x_setreg(hw, cmd->cmd, HFA384x_CMD);
-
-		/* Now wait for completion */
-		if ( (HFA384x_CMD_CMDCODE_GET(cmd->cmd) == HFA384x_CMDCODE_DOWNLD) ) {
-			/* dltimeout is in ms */
-			timeout = (((UINT32)hw->dltimeout) / 1000UL) * HZ;
-			if ( timeout > 0 ) {
-				timeout += jiffies;
-			} else {
-				timeout = jiffies + 1*HZ;
-			}
-		} else {
-			timeout = jiffies + 1*HZ;
-		}
-		reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-		while ( !HFA384x_EVSTAT_ISCMD(reg) && time_before(jiffies,timeout) ) {
-			udelay(100);
-			reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-		}
-		if ( HFA384x_EVSTAT_ISCMD(reg) ) {
-			result = 0;
-			cmd->result.status = hfa384x_getreg(hw, HFA384x_STATUS);
-			cmd->result.resp0 = hfa384x_getreg(hw, HFA384x_RESP0);
-			cmd->result.resp1 = hfa384x_getreg(hw, HFA384x_RESP1);
-			cmd->result.resp2 = hfa384x_getreg(hw, HFA384x_RESP2);
-			hfa384x_setreg(hw, HFA384x_EVACK_CMD, HFA384x_EVACK);
-			result = HFA384x_STATUS_RESULT_GET(cmd->result.status);
-		}
-	}
-
-failed:
-	DBFEXIT;
-	return result;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_start
-*
-* Issues the MAC initialize command, sets up some data structures,
-* and enables the interrupts.  After this function completes, the
-* low-level stuff should be ready for any/all commands.
-*
-* Arguments:
-*	hw		device structure
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_start(hfa384x_t *hw)
-{
-	int	result = 0;
-	UINT16			reg;
-	int			i;
-	int			j;
-	DBFENTER;
-
-	/* call initialize */
-	result = hfa384x_cmd_initialize(hw);
-	if (result != 0) {
-		WLAN_LOG_ERROR("Initialize command failed.\n");
-		goto failed;
-	}
-
-	/* make sure interrupts are disabled and any layabout events cleared */
-	hfa384x_setreg(hw, 0, HFA384x_INTEN);
-	hfa384x_setreg(hw, 0xffff, HFA384x_EVACK);
-
-        hw->txfid_head = 0;
-        hw->txfid_tail = 0;
-        hw->txfid_N = HFA384x_DRVR_FIDSTACKLEN_MAX;
-        memset(hw->txfid_queue, 0, sizeof(hw->txfid_queue));
-
-	/* Allocate tx and notify FIDs */
-	/* First, tx */
-	for ( i = 0; i < HFA384x_DRVR_FIDSTACKLEN_MAX-1; i++) {
-		result = hfa384x_cmd_allocate(hw, HFA384x_DRVR_TXBUF_MAX);
-		if (result != 0) {
-			WLAN_LOG_ERROR("Allocate(tx) command failed.\n");
-			goto failed;
-		}
-		j = 0;
-		do {
-			reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-			udelay(10);
-			j++;
-		} while ( !HFA384x_EVSTAT_ISALLOC(reg) && j < 50); /* 50 is timeout */
-		if ( j >= 50 ) {
-			WLAN_LOG_ERROR("Timed out waiting for evalloc(tx).\n");
-			result = -ETIMEDOUT;
-			goto failed;
-		}
-		reg = hfa384x_getreg(hw, HFA384x_ALLOCFID);
-
-		txfid_queue_add(hw, reg);
-
-		WLAN_LOG_DEBUG(4,"hw->txfid_queue[%d]=0x%04x\n",i,reg);
-
-		reg = HFA384x_EVACK_ALLOC_SET(1);
-		hfa384x_setreg(hw, reg, HFA384x_EVACK);
-
-	}
-
-	/* Now, the info frame fid */
-	result = hfa384x_cmd_allocate(hw, HFA384x_INFOFRM_MAXLEN);
-	if (result != 0) {
-		WLAN_LOG_ERROR("Allocate(tx) command failed.\n");
-		goto failed;
-	}
-	i = 0;
-	do {
-		reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-		udelay(10);
-		i++;
-	} while ( !HFA384x_EVSTAT_ISALLOC(reg) && i < 50); /* 50 is timeout */
-	if ( i >= 50 ) {
-		WLAN_LOG_ERROR("Timed out waiting for evalloc(info).\n");
-		result = -ETIMEDOUT;
-		goto failed;
-	}
-	hw->infofid = hfa384x_getreg(hw, HFA384x_ALLOCFID);
-	reg = HFA384x_EVACK_ALLOC_SET(1);
-	hfa384x_setreg(hw, reg, HFA384x_EVACK);
-	WLAN_LOG_DEBUG(4,"hw->infofid=0x%04x\n", hw->infofid);
-
-	/* Set swsupport regs to magic # for card presence detection */
-	hfa384x_setreg(hw, HFA384x_DRVR_MAGIC, HFA384x_SWSUPPORT0);
-
-	/* Now enable the interrupts and set the running state */
-	hfa384x_setreg(hw, 0xffff, HFA384x_EVSTAT);
-	hfa384x_events_all(hw);
-
-	hw->state = HFA384x_STATE_RUNNING;
-
-	goto done;
-failed:
-	WLAN_LOG_ERROR("Failed, result=%d\n", result);
-done:
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_stop
-*
-* Issues the initialize command to leave us in the 'reset' state.
-*
-* Arguments:
-*	hw		device structure
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_stop(hfa384x_t *hw)
-{
-	int	result = 0;
-	int i;
-	DBFENTER;
-
-	del_timer_sync(&hw->commsqual_timer);
-
-	if ( hw->wlandev->hwremoved ) {
-		/* only flush when we're shutting down for good */
-		flush_scheduled_work();
-	}
-
-	if (hw->state == HFA384x_STATE_RUNNING) {
-		/*
-		 * Send the MAC initialize cmd.
-		 */
-		hfa384x_cmd_initialize(hw);
-
-		/*
-		 * Make absolutely sure interrupts are disabled and any
-		 * layabout events cleared
-		 */
-		hfa384x_setreg(hw, 0, HFA384x_INTEN);
-		hfa384x_setreg(hw, 0xffff, HFA384x_EVACK);
-	}
-
-	tasklet_kill(&hw->bap_tasklet);
-
-	hw->link_status = HFA384x_LINK_NOTCONNECTED;
-	hw->state = HFA384x_STATE_INIT;
-
-	/* Clear all the port status */
-	for ( i = 0; i < HFA384x_NUMPORTS_MAX; i++) {
-		hw->port_enabled[i] = 0;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_txframe
-*
-* Takes a frame from prism2sta and queues it for transmission.
-*
-* Arguments:
-*	hw		device structure
-*	skb		packet buffer struct.  Contains an 802.11
-*			data frame.
-*       p80211_hdr      points to the 802.11 header for the packet.
-* Returns:
-*	0		Success and more buffs available
-*	1		Success but no more buffs
-*	2		Allocation failure
-*	3		MAC Tx command failed
-*	4		Buffer full or queue busy
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_txframe(hfa384x_t *hw, struct sk_buff *skb, p80211_hdr_t *p80211_hdr, p80211_metawep_t *p80211_wep)
-{
-	hfa384x_tx_frame_t	txdesc;
-	UINT16			macq = 0;
-	UINT16			fid;
-	int			result;
-
-	DBFENTER;
-
-	/* Build Tx frame structure */
-	/* Set up the control field */
-	memset(&txdesc, 0, sizeof(txdesc));
-
-/* Tx complete and Tx exception disable per dleach.  Might be causing
- * buf depletion
- */
-#define DOBOTH 1
-#if DOBOTH
-	txdesc.tx_control =
-		HFA384x_TX_MACPORT_SET(0) | HFA384x_TX_STRUCTYPE_SET(1) |
-		HFA384x_TX_TXEX_SET(1) | HFA384x_TX_TXOK_SET(1);
-#elif DOEXC
-	txdesc.tx_control =
-		HFA384x_TX_MACPORT_SET(0) | HFA384x_TX_STRUCTYPE_SET(1) |
-		HFA384x_TX_TXEX_SET(1) | HFA384x_TX_TXOK_SET(0);
-#else
-	txdesc.tx_control =
-		HFA384x_TX_MACPORT_SET(0) | HFA384x_TX_STRUCTYPE_SET(1) |
-		HFA384x_TX_TXEX_SET(0) | HFA384x_TX_TXOK_SET(0);
-#endif
-
-	/* if we're using host WEP, increase size by IV+ICV */
-	if (p80211_wep->data) {
-		txdesc.data_len = host2hfa384x_16(skb->len+8);
-		//		txdesc.tx_control |= HFA384x_TX_NOENCRYPT_SET(1);
-	} else {
-		txdesc.data_len =  host2hfa384x_16(skb->len);
-	}
-
-	txdesc.tx_control = host2hfa384x_16(txdesc.tx_control);
-	/* copy the header over to the txdesc */
-	memcpy(&(txdesc.frame_control), p80211_hdr, sizeof(p80211_hdr_t));
-
-	/* Since tbusy is set whenever the stack is empty, there should
-	 * always be something on the stack if we get to this point.
-	 * [MSM]: NOT TRUE!!!!! so I added the test of fid below.
-	 */
-
-	/* Allocate FID */
-
-	fid = txfid_queue_remove(hw);
-
-	if ( fid == 0 ) { /* stack or queue was empty */
-		return 4;
-	}
-
-	/* now let's get the cmdlock */
-	spin_lock(&hw->cmdlock);
-
-	/* Copy descriptor+payload to FID */
-        if (p80211_wep->data) {
-		result = hfa384x_copy_to_bap4(hw, HFA384x_BAP_PROC, fid, 0,
-					      &txdesc, sizeof(txdesc),
-					      p80211_wep->iv, sizeof(p80211_wep->iv),
-					      p80211_wep->data, skb->len,
-					      p80211_wep->icv, sizeof(p80211_wep->icv));
-	} else {
-		result = hfa384x_copy_to_bap4(hw, HFA384x_BAP_PROC, fid, 0,
-					      &txdesc, sizeof(txdesc),
-					      skb->data, skb->len,
-					      NULL, 0, NULL, 0);
-	}
-
-	if ( result ) {
-		WLAN_LOG_DEBUG(1,
-			"copy_to_bap(%04x, %d, %d) failed, result=0x%x\n",
-			fid,
-		 	sizeof(txdesc),
-	 		skb->len,
-			result);
-
-		/* put the fid back in the queue */
-		txfid_queue_add(hw, fid);
-
-		result = 3;
-		goto failed;
-	}
-
-	/* Issue Tx command */
-	result = hfa384x_cmd_transmit(hw, HFA384x_TXCMD_RECL, macq, fid);
-
-	if ( result != 0 ) {
-		txfid_queue_add(hw, fid);
-
-		WLAN_LOG_DEBUG(1,"cmd_tx(%04x) failed, result=%d\n",
-			fid, result);
-		result = 3;
-		goto failed;
-	}
-
-	/* indicate we haven't any buffers, int_alloc will clear */
-	result = txfid_queue_empty(hw);
-failed:
-
-	spin_unlock(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_interrupt
-*
-* Driver interrupt handler.
-*
-* Arguments:
-*	irq		irq number
-*	dev_id		pointer to the device
-*	regs		registers
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	May result in a frame being passed up the stack or an info
-*	frame being handled.
-*
-* Call context:
-*	Ummm, could it be interrupt?
-----------------------------------------------------------------*/
-irqreturn_t hfa384x_interrupt(int irq, void *dev_id PT_REGS)
-{
-	int			reg;
-	wlandevice_t		*wlandev = (wlandevice_t*)dev_id;
-	hfa384x_t		*hw = wlandev->priv;
-	int			ev_read = 0;
-	DBFENTER;
-
-	if (!wlandev || wlandev->hwremoved)
-		return IRQ_NONE;  /* Not much we can do w/o hardware */
-#if (WLAN_HOSTIF == WLAN_PCMCIA)
-	if (hw->iobase == 0)  /* XXX FIXME Properly */
-		return IRQ_NONE;
-#endif
-
-	for (;;ev_read++) {
-		if (ev_read >= prism2_irq_evread_max)
-			break;
-
-		/* Check swsupport reg magic # for card presence */
-		reg = hfa384x_getreg(hw, HFA384x_SWSUPPORT0);
-		if ( reg != HFA384x_DRVR_MAGIC) {
-			WLAN_LOG_DEBUG(2, "irq=%d, no magic.  Card removed?.\n", irq);
-			break;
-		}
-
-		/* read the EvStat register for interrupt enabled events */
-		reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-
-		/* AND with the enabled interrupts */
-		reg &= hfa384x_getreg(hw, HFA384x_INTEN);
-
-		/* Handle the events */
-		if ( HFA384x_EVSTAT_ISWTERR(reg) ){
-			WLAN_LOG_ERROR(
-			"Error: WTERR interrupt received (unhandled).\n");
-			hfa384x_setreg(hw, HFA384x_EVACK_WTERR_SET(1),
-				HFA384x_EVACK);
-		}
-
-		if ( HFA384x_EVSTAT_ISINFDROP(reg) ){
-			hfa384x_int_infdrop(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_INFDROP_SET(1),
-				HFA384x_EVACK);
-		}
-
-		if (HFA384x_EVSTAT_ISBAP_OP(reg)) {
-			/* Disable the BAP interrupts */
-			hfa384x_events_nobap(hw);
-			tasklet_schedule(&hw->bap_tasklet);
-		}
-
-		if ( HFA384x_EVSTAT_ISALLOC(reg) ){
-			hfa384x_int_alloc(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_ALLOC_SET(1),
-				HFA384x_EVACK);
-		}
-
-		if ( HFA384x_EVSTAT_ISDTIM(reg) ){
-			hfa384x_int_dtim(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_DTIM_SET(1),
-				HFA384x_EVACK);
-		}
-#ifdef CMD_IRQ
-		if ( HFA384x_EVSTAT_ISCMD(reg) ){
-			hfa384x_int_cmd(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_CMD_SET(1),
-				       HFA384x_EVACK);
-		}
-#endif
-
-		/* allow the evstat to be updated after the evack */
-		udelay(20);
-	}
-
-	DBFEXIT;
-	return IRQ_HANDLED;
-}
-
-#ifdef CMD_IRQ
-/*----------------------------------------------------------------
-* hfa384x_int_cmd
-*
-* Handles command completion event.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	interrupt
-----------------------------------------------------------------*/
-void hfa384x_int_cmd(wlandevice_t *wlandev)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	DBFENTER;
-
-	// check to make sure it's the right command?
-	if (hw->cmddata) {
-		hw->cmddata->status = hfa384x_getreg(hw, HFA384x_STATUS);
-		hw->cmddata->resp0 = hfa384x_getreg(hw, HFA384x_RESP0);
-		hw->cmddata->resp1 = hfa384x_getreg(hw, HFA384x_RESP1);
-		hw->cmddata->resp2 = hfa384x_getreg(hw, HFA384x_RESP2);
-	}
-	hw->cmdflag = 1;
-
-	printk(KERN_INFO "um. int_cmd\n");
-
-	wake_up_interruptible(&hw->cmdq);
-
-	// XXXX perform a bap copy too?
-
-	DBFEXIT;
-	return;
-}
-#endif
-
-/*----------------------------------------------------------------
-* hfa384x_int_dtim
-*
-* Handles the DTIM early warning event.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	interrupt
-----------------------------------------------------------------*/
-static void hfa384x_int_dtim(wlandevice_t *wlandev)
-{
-#if 0
-	hfa384x_t		*hw = wlandev->priv;
-#endif
-	DBFENTER;
-	prism2sta_ev_dtim(wlandev);
-	DBFEXIT;
-	return;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_int_infdrop
-*
-* Handles the InfDrop event.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	interrupt
-----------------------------------------------------------------*/
-static void hfa384x_int_infdrop(wlandevice_t *wlandev)
-{
-#if 0
-	hfa384x_t		*hw = wlandev->priv;
-#endif
-	DBFENTER;
-	prism2sta_ev_infdrop(wlandev);
-	DBFEXIT;
-	return;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_int_info
-*
-* Handles the Info event.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	tasklet
-----------------------------------------------------------------*/
-static void hfa384x_int_info(wlandevice_t *wlandev)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	UINT16			reg;
-	hfa384x_InfFrame_t	inf;
-	int			result;
-	DBFENTER;
-	/* Retrieve the FID */
-	reg = hfa384x_getreg(hw, HFA384x_INFOFID);
-
-	/* Retrieve the length */
-	result = hfa384x_copy_from_bap( hw,
-		HFA384x_BAP_INT, reg, 0, &inf.framelen, sizeof(UINT16));
-	if ( result ) {
-		WLAN_LOG_DEBUG(1,
-			"copy_from_bap(0x%04x, 0, %d) failed, result=0x%x\n",
-			reg, sizeof(inf), result);
-		goto failed;
-	}
-	inf.framelen = hfa384x2host_16(inf.framelen);
-
-	/* Retrieve the rest */
-	result = hfa384x_copy_from_bap( hw,
-		HFA384x_BAP_INT, reg, sizeof(UINT16),
-		&(inf.infotype), inf.framelen * sizeof(UINT16));
-	if ( result ) {
-		WLAN_LOG_DEBUG(1,
-			"copy_from_bap(0x%04x, 0, %d) failed, result=0x%x\n",
-			reg, sizeof(inf), result);
-		goto failed;
-	}
-
-	prism2sta_ev_info(wlandev, &inf);
-failed:
-	DBFEXIT;
-	return;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_int_txexc
-*
-* Handles the TxExc event.  A Transmit Exception event indicates
-* that the MAC's TX process was unsuccessful - so the packet did
-* not get transmitted.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	tasklet
-----------------------------------------------------------------*/
-static void hfa384x_int_txexc(wlandevice_t *wlandev)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	UINT16			status;
-	UINT16			fid;
-	int			result = 0;
-	DBFENTER;
-	/* Collect the status and display */
-	fid = hfa384x_getreg(hw, HFA384x_TXCOMPLFID);
-	result = hfa384x_copy_from_bap(hw, HFA384x_BAP_INT, fid, 0, &status, sizeof(status));
-	if ( result ) {
-		WLAN_LOG_DEBUG(1,
-			"copy_from_bap(0x%04x, 0, %d) failed, result=0x%x\n",
-			fid, sizeof(status), result);
-		goto failed;
-	}
-	status = hfa384x2host_16(status);
-	prism2sta_ev_txexc(wlandev, status);
-failed:
-	DBFEXIT;
-	return;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_int_tx
-*
-* Handles the Tx event.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	tasklet
-----------------------------------------------------------------*/
-static void hfa384x_int_tx(wlandevice_t *wlandev)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	UINT16			fid;
-	UINT16			status;
-	int			result = 0;
-	DBFENTER;
-	fid = hfa384x_getreg(hw, HFA384x_TXCOMPLFID);
-	result = hfa384x_copy_from_bap(hw, HFA384x_BAP_INT, fid, 0, &status, sizeof(status));
-	if ( result ) {
-		WLAN_LOG_DEBUG(1,
-			"copy_from_bap(0x%04x, 0, %d) failed, result=0x%x\n",
-			fid, sizeof(status), result);
-		goto failed;
-	}
-	status = hfa384x2host_16(status);
-	prism2sta_ev_tx(wlandev, status);
-failed:
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_int_rx
-*
-* Handles the Rx event.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	tasklet
-----------------------------------------------------------------*/
-static void hfa384x_int_rx(wlandevice_t *wlandev)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	UINT16			rxfid;
-	hfa384x_rx_frame_t	rxdesc;
-	int			result;
-	int                     hdrlen;
-	UINT16                  fc;
-	p80211_rxmeta_t	*rxmeta;
-	struct sk_buff          *skb = NULL;
-	UINT8 *datap;
-
-	DBFENTER;
-
-	/* Get the FID */
-	rxfid = hfa384x_getreg(hw, HFA384x_RXFID);
-	/* Get the descriptor (including headers) */
-	result = hfa384x_copy_from_bap(hw,
-			HFA384x_BAP_INT,
-			rxfid,
-			0,
-			&rxdesc,
-			sizeof(rxdesc));
-	if ( result ) {
-		WLAN_LOG_DEBUG(1,
-			"copy_from_bap(0x%04x, %d, %d) failed, result=0x%x\n",
-			rxfid,
-			0,
-			sizeof(rxdesc),
-			result);
-		goto done;
-	}
-
-	/* Byte order convert once up front. */
-	rxdesc.status =	hfa384x2host_16(rxdesc.status);
-	rxdesc.time =	hfa384x2host_32(rxdesc.time);
-
-	/* drop errors and whatnot in promisc mode */
-	if (( wlandev->netdev->flags & IFF_PROMISC ) &&
-	    (HFA384x_RXSTATUS_ISFCSERR(rxdesc.status) ||
-	     HFA384x_RXSTATUS_ISUNDECR(rxdesc.status)))
-	  goto done;
-
-	/* Now handle frame based on port# */
-	switch( HFA384x_RXSTATUS_MACPORT_GET(rxdesc.status) )
-	{
-	case 0:
-
-		fc = ieee2host16(rxdesc.frame_control);
-
-		/* If exclude and we receive an unencrypted, drop it */
-		if ( (wlandev->hostwep & HOSTWEP_EXCLUDEUNENCRYPTED) &&
-		     !WLAN_GET_FC_ISWEP(fc)) {
-			goto done;
-		}
-
-		hdrlen = p80211_headerlen(fc);
-
-		/* Allocate the buffer, note CRC (aka FCS). pballoc */
-		/* assumes there needs to be space for one */
-		skb = dev_alloc_skb(hfa384x2host_16(rxdesc.data_len) + hdrlen + WLAN_CRC_LEN + 2); /* a little extra */
-
-		if ( ! skb ) {
-			WLAN_LOG_ERROR("alloc_skb failed.\n");
-			goto done;
-                }
-
-		skb->dev = wlandev->netdev;
-
-		/* theoretically align the IP header on a 32-bit word. */
-		if ( hdrlen == WLAN_HDR_A4_LEN )
-			skb_reserve(skb, 2);
-
-		/* Copy the 802.11 hdr to the buffer */
-		datap = skb_put(skb, WLAN_HDR_A3_LEN);
-		memcpy(datap, &rxdesc.frame_control, WLAN_HDR_A3_LEN);
-
-		/* Snag the A4 address if present */
-		if (hdrlen == WLAN_HDR_A4_LEN) {
-			datap = skb_put(skb, WLAN_ADDR_LEN);
-			memcpy(datap, &rxdesc.address4, WLAN_HDR_A3_LEN);
-		}
-
-		/* we can convert the data_len as we passed the original on */
-		rxdesc.data_len = hfa384x2host_16(rxdesc.data_len);
-
-		/* Copy the payload data to the buffer */
-		if ( rxdesc.data_len > 0 ) {
-			datap = skb_put(skb, rxdesc.data_len);
-			result = hfa384x_copy_from_bap(hw,
-				HFA384x_BAP_INT, rxfid, HFA384x_RX_DATA_OFF,
-				datap, rxdesc.data_len);
-			if ( result ) {
-				WLAN_LOG_DEBUG(1,
-					"copy_from_bap(0x%04x, %d, %d) failed, result=0x%x\n",
-					rxfid,
-					HFA384x_RX_DATA_OFF,
-					rxdesc.data_len,
-					result);
-				goto failed;
-			}
-		}
-		/* the prism2 cards don't return the FCS */
-		datap = skb_put(skb, WLAN_CRC_LEN);
-		memset (datap, 0xff, WLAN_CRC_LEN);
-		skb_reset_mac_header(skb);
-
-		/* Attach the rxmeta, set some stuff */
-		p80211skb_rxmeta_attach(wlandev, skb);
-		rxmeta = P80211SKB_RXMETA(skb);
-		rxmeta->mactime = rxdesc.time;
-		rxmeta->rxrate = rxdesc.rate;
-		rxmeta->signal = rxdesc.signal - hw->dbmadjust;
-		rxmeta->noise = rxdesc.silence - hw->dbmadjust;
-
-		prism2sta_ev_rx(wlandev, skb);
-		goto done;
-	case 7:
-
-        	if ( ! HFA384x_RXSTATUS_ISFCSERR(rxdesc.status) ) {
-                        hfa384x_int_rxmonitor( wlandev, rxfid, &rxdesc);
-                } else {
-                        WLAN_LOG_DEBUG(3,"Received monitor frame: FCSerr set\n");
-                }
-		goto done;
-
-	default:
-
-		WLAN_LOG_WARNING("Received frame on unsupported port=%d\n",
-			HFA384x_RXSTATUS_MACPORT_GET(rxdesc.status) );
-		goto done;
-	}
-
- failed:
-	dev_kfree_skb(skb);
-
- done:
-	DBFEXIT;
-	return;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_int_rxmonitor
-*
-* Helper function for int_rx.  Handles monitor frames.
-* Note that this function allocates space for the FCS and sets it
-* to 0xffffffff.  The hfa384x doesn't give us the FCS value but the
-* higher layers expect it.  0xffffffff is used as a flag to indicate
-* the FCS is bogus.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	rxfid		received FID
-*	rxdesc		rx descriptor read from card in int_rx
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	Allocates an skb and passes it up via the PF_PACKET interface.
-* Call context:
-*	interrupt
-----------------------------------------------------------------*/
-static void hfa384x_int_rxmonitor( wlandevice_t *wlandev, UINT16 rxfid,
-				   hfa384x_rx_frame_t *rxdesc)
-{
-	hfa384x_t			*hw = wlandev->priv;
-	UINT				hdrlen = 0;
-	UINT				datalen = 0;
-	UINT				skblen = 0;
-	UINT				truncated = 0;
-	UINT8				*datap;
-	UINT16				fc;
-	struct sk_buff			*skb;
-
-	DBFENTER;
-	/* Don't forget the status, time, and data_len fields are in host order */
-	/* Figure out how big the frame is */
-	fc = ieee2host16(rxdesc->frame_control);
-	hdrlen = p80211_headerlen(fc);
-	datalen = hfa384x2host_16(rxdesc->data_len);
-
-	/* Allocate an ind message+framesize skb */
-	skblen = sizeof(p80211msg_lnxind_wlansniffrm_t) +
-		hdrlen + datalen + WLAN_CRC_LEN;
-
-	/* sanity check the length */
-	if ( skblen >
-		(sizeof(p80211msg_lnxind_wlansniffrm_t) +
-		WLAN_HDR_A4_LEN + WLAN_DATA_MAXLEN + WLAN_CRC_LEN) ) {
-		WLAN_LOG_DEBUG(1, "overlen frm: len=%d\n",
-			skblen - sizeof(p80211msg_lnxind_wlansniffrm_t));
-	}
-
-	if ( (skb = dev_alloc_skb(skblen)) == NULL ) {
-		WLAN_LOG_ERROR("alloc_skb failed trying to allocate %d bytes\n", skblen);
-		return;
-	}
-
-	/* only prepend the prism header if in the right mode */
-	if ((wlandev->netdev->type == ARPHRD_IEEE80211_PRISM) &&
-	    (hw->sniffhdr == 0)) {
-		p80211msg_lnxind_wlansniffrm_t	*msg;
-		datap = skb_put(skb, sizeof(p80211msg_lnxind_wlansniffrm_t));
-		msg = (p80211msg_lnxind_wlansniffrm_t*) datap;
-
-		/* Initialize the message members */
-		msg->msgcode = DIDmsg_lnxind_wlansniffrm;
-		msg->msglen = sizeof(p80211msg_lnxind_wlansniffrm_t);
-		strcpy(msg->devname, wlandev->name);
-
-		msg->hosttime.did = DIDmsg_lnxind_wlansniffrm_hosttime;
-		msg->hosttime.status = 0;
-		msg->hosttime.len = 4;
-		msg->hosttime.data = jiffies;
-
-		msg->mactime.did = DIDmsg_lnxind_wlansniffrm_mactime;
-		msg->mactime.status = 0;
-		msg->mactime.len = 4;
-		msg->mactime.data = rxdesc->time * 1000;
-
-		msg->channel.did = DIDmsg_lnxind_wlansniffrm_channel;
-		msg->channel.status = 0;
-		msg->channel.len = 4;
-		msg->channel.data = hw->sniff_channel;
-
-		msg->rssi.did = DIDmsg_lnxind_wlansniffrm_rssi;
-		msg->rssi.status = P80211ENUM_msgitem_status_no_value;
-		msg->rssi.len = 4;
-		msg->rssi.data = 0;
-
-		msg->sq.did = DIDmsg_lnxind_wlansniffrm_sq;
-		msg->sq.status = P80211ENUM_msgitem_status_no_value;
-		msg->sq.len = 4;
-		msg->sq.data = 0;
-
-		msg->signal.did = DIDmsg_lnxind_wlansniffrm_signal;
-		msg->signal.status = 0;
-		msg->signal.len = 4;
-		msg->signal.data = rxdesc->signal;
-
-		msg->noise.did = DIDmsg_lnxind_wlansniffrm_noise;
-		msg->noise.status = 0;
-		msg->noise.len = 4;
-		msg->noise.data = rxdesc->silence;
-
-		msg->rate.did = DIDmsg_lnxind_wlansniffrm_rate;
-		msg->rate.status = 0;
-		msg->rate.len = 4;
-		msg->rate.data = rxdesc->rate / 5; /* set to 802.11 units */
-
-		msg->istx.did = DIDmsg_lnxind_wlansniffrm_istx;
-		msg->istx.status = 0;
-		msg->istx.len = 4;
-		msg->istx.data = P80211ENUM_truth_false;
-
-		msg->frmlen.did = DIDmsg_lnxind_wlansniffrm_frmlen;
-		msg->frmlen.status = 0;
-		msg->frmlen.len = 4;
-		msg->frmlen.data = hdrlen + datalen + WLAN_CRC_LEN;
-	} else if ((wlandev->netdev->type == ARPHRD_IEEE80211_PRISM) &&
-		   (hw->sniffhdr != 0)) {
-		p80211_caphdr_t		*caphdr;
-		/* The NEW header format! */
-		datap = skb_put(skb, sizeof(p80211_caphdr_t));
-		caphdr = (p80211_caphdr_t*) datap;
-
-		caphdr->version =	htonl(P80211CAPTURE_VERSION);
-		caphdr->length =	htonl(sizeof(p80211_caphdr_t));
-		caphdr->mactime =	__cpu_to_be64(rxdesc->time);
-		caphdr->hosttime =	__cpu_to_be64(jiffies);
-		caphdr->phytype =	htonl(4); /* dss_dot11_b */
-		caphdr->channel =	htonl(hw->sniff_channel);
-		caphdr->datarate =	htonl(rxdesc->rate);
-		caphdr->antenna =	htonl(0); /* unknown */
-		caphdr->priority =	htonl(0); /* unknown */
-		caphdr->ssi_type =	htonl(3); /* rssi_raw */
-		caphdr->ssi_signal =	htonl(rxdesc->signal);
-		caphdr->ssi_noise =	htonl(rxdesc->silence);
-		caphdr->preamble =	htonl(0); /* unknown */
-		caphdr->encoding =	htonl(1); /* cck */
-	}
-	/* Copy the 802.11 header to the skb (ctl frames may be less than a full header) */
-	datap = skb_put(skb, hdrlen);
-	memcpy( datap, &(rxdesc->frame_control), hdrlen);
-
-	/* If any, copy the data from the card to the skb */
-	if ( datalen > 0 )
-	{
-		/* Truncate the packet if the user wants us to */
-		UINT	dataread = datalen;
-		if(hw->sniff_truncate > 0 && dataread > hw->sniff_truncate) {
-			dataread = hw->sniff_truncate;
-			truncated = 1;
-		}
-
-		datap = skb_put(skb, dataread);
-		hfa384x_copy_from_bap(hw,
-			HFA384x_BAP_INT, rxfid, HFA384x_RX_DATA_OFF,
-			datap, dataread);
-
-		/* check for unencrypted stuff if WEP bit set. */
-		if (*(datap - hdrlen + 1) & 0x40) // wep set
-		  if ((*(datap) == 0xaa) && (*(datap+1) == 0xaa))
-		    *(datap - hdrlen + 1) &= 0xbf; // clear wep; it's the 802.2 header!
-	}
-
-	if (!truncated && hw->sniff_fcs) {
-		/* Set the FCS */
-		datap = skb_put(skb, WLAN_CRC_LEN);
-		memset( datap, 0xff, WLAN_CRC_LEN);
-	}
-
-	/* pass it back up */
-	prism2sta_ev_rx(wlandev, skb);
-
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_int_alloc
-*
-* Handles the Alloc event.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	interrupt
-----------------------------------------------------------------*/
-static void hfa384x_int_alloc(wlandevice_t *wlandev)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	UINT16			fid;
-	INT16			result;
-
-	DBFENTER;
-
-	/* Handle the reclaimed FID */
-	/*   collect the FID and push it onto the stack */
-	fid = hfa384x_getreg(hw, HFA384x_ALLOCFID);
-
-	if ( fid != hw->infofid ) { /* It's a transmit fid */
-		WLAN_LOG_DEBUG(5, "int_alloc(%#x)\n", fid);
-		result = txfid_queue_add(hw, fid);
-		if (result != -1) {
-			prism2sta_ev_alloc(wlandev);
-			WLAN_LOG_DEBUG(5, "q_add.\n");
-		} else {
-			WLAN_LOG_DEBUG(5, "q_full.\n");
-		}
-	} else {
-		/* unlock the info fid */
-		up(&hw->infofid_sem);
-	}
-
-	DBFEXIT;
-	return;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_handover
-*
-* Sends a handover notification to the MAC.
-*
-* Arguments:
-*	hw		device structure
-*	addr		address of station that's left
-*
-* Returns:
-*	zero		success.
-*	-ERESTARTSYS	received signal while waiting for semaphore.
-*	-EIO		failed to write to bap, or failed in cmd.
-*
-* Side effects:
-*
-* Call context:
-*	process thread, NOTE: this call may block on a semaphore!
-----------------------------------------------------------------*/
-int hfa384x_drvr_handover( hfa384x_t *hw, UINT8 *addr)
-{
-	int			result = 0;
-        hfa384x_HandoverAddr_t  rec;
-        UINT                    len;
-        DBFENTER;
-
-	/* Acquire the infofid */
-	if ( down_interruptible(&hw->infofid_sem) ) {
-		result = -ERESTARTSYS;
-		goto failed;
-	}
-
-        /* Set up the record */
-        len = sizeof(hfa384x_HandoverAddr_t);
-        rec.framelen = host2hfa384x_16(len/2 - 1);
-        rec.infotype = host2hfa384x_16(HFA384x_IT_HANDOVERADDR);
-        memcpy(rec.handover_addr, addr, sizeof(rec.handover_addr));
-
-        /* Issue the command */
-        result = hfa384x_cmd_notify(hw, 1, hw->infofid, &rec, len);
-
-        if ( result != 0 ) {
-                WLAN_LOG_DEBUG(1,"cmd_notify(%04x) failed, result=%d",
-                        hw->infofid, result);
-		result = -EIO;
-                goto failed;
-        }
-
-failed:
-	DBFEXIT;
-	return result;
-}
-
-void hfa384x_tx_timeout(wlandevice_t *wlandev)
-{
-	DBFENTER;
-
-	WLAN_LOG_WARNING("Implement me.\n");
-
-	DBFEXIT;
-}
-
-/* Handles all "rx" BAP operations */
-static void     hfa384x_bap_tasklet(unsigned long data)
-{
-	hfa384x_t *hw = (hfa384x_t *) data;
-	wlandevice_t *wlandev = hw->wlandev;
-	int counter = prism2_irq_evread_max;
-	int			reg;
-
-	DBFENTER;
-
-	while (counter-- > 0) {
-		/* Get interrupt register */
-		reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-
-		if ((reg == 0xffff) ||
-		    !(reg & HFA384x_INT_BAP_OP)) {
-			break;
-		}
-
-		if ( HFA384x_EVSTAT_ISINFO(reg) ){
-			hfa384x_int_info(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_INFO_SET(1),
-				HFA384x_EVACK);
-		}
-		if ( HFA384x_EVSTAT_ISTXEXC(reg) ){
-			hfa384x_int_txexc(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_TXEXC_SET(1),
-				HFA384x_EVACK);
-		}
-		if ( HFA384x_EVSTAT_ISTX(reg) ){
-			hfa384x_int_tx(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_TX_SET(1),
-				HFA384x_EVACK);
-		}
-		if ( HFA384x_EVSTAT_ISRX(reg) ){
-			hfa384x_int_rx(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_RX_SET(1),
-				HFA384x_EVACK);
-		}
-	}
-
-	/* re-enable interrupts */
-	hfa384x_events_all(hw);
-
-	DBFEXIT;
-}
diff --git a/drivers/staging/wlan-ng/hfa384x.h b/drivers/staging/wlan-ng/hfa384x.h
index 0dfb8ce9aae7..9b746654a39c 100644
--- a/drivers/staging/wlan-ng/hfa384x.h
+++ b/drivers/staging/wlan-ng/hfa384x.h
@@ -63,18 +63,18 @@
 
 /*------ Constants --------------------------------------------*/
 /*--- Mins & Maxs -----------------------------------*/
-#define		HFA384x_CMD_ALLOC_LEN_MIN	((UINT16)4)
-#define		HFA384x_CMD_ALLOC_LEN_MAX	((UINT16)2400)
-#define		HFA384x_BAP_DATALEN_MAX		((UINT16)4096)
-#define		HFA384x_BAP_OFFSET_MAX		((UINT16)4096)
-#define		HFA384x_PORTID_MAX		((UINT16)7)
-#define		HFA384x_NUMPORTS_MAX		((UINT16)(HFA384x_PORTID_MAX+1))
-#define		HFA384x_PDR_LEN_MAX		((UINT16)512)	/* in bytes, from EK */
-#define		HFA384x_PDA_RECS_MAX		((UINT16)200)	/* a guess */
-#define		HFA384x_PDA_LEN_MAX		((UINT16)1024)	/* in bytes, from EK */
-#define		HFA384x_SCANRESULT_MAX		((UINT16)31)
-#define		HFA384x_HSCANRESULT_MAX		((UINT16)31)
-#define		HFA384x_CHINFORESULT_MAX	((UINT16)16)
+#define		HFA384x_CMD_ALLOC_LEN_MIN	((u16)4)
+#define		HFA384x_CMD_ALLOC_LEN_MAX	((u16)2400)
+#define		HFA384x_BAP_DATALEN_MAX		((u16)4096)
+#define		HFA384x_BAP_OFFSET_MAX		((u16)4096)
+#define		HFA384x_PORTID_MAX		((u16)7)
+#define		HFA384x_NUMPORTS_MAX		((u16)(HFA384x_PORTID_MAX+1))
+#define		HFA384x_PDR_LEN_MAX		((u16)512)	/* in bytes, from EK */
+#define		HFA384x_PDA_RECS_MAX		((u16)200)	/* a guess */
+#define		HFA384x_PDA_LEN_MAX		((u16)1024)	/* in bytes, from EK */
+#define		HFA384x_SCANRESULT_MAX		((u16)31)
+#define		HFA384x_HSCANRESULT_MAX		((u16)31)
+#define		HFA384x_CHINFORESULT_MAX	((u16)16)
 #define		HFA384x_DRVR_FIDSTACKLEN_MAX	(10)
 #define		HFA384x_DRVR_TXBUF_MAX		(sizeof(hfa384x_tx_frame_t) + \
 						WLAN_DATA_MAXLEN - \
@@ -88,42 +88,42 @@
 #define		HFA384x_USB_RWMEM_MAXLEN	2048
 
 /*--- Support Constants -----------------------------*/
-#define		HFA384x_BAP_PROC			((UINT16)0)
-#define		HFA384x_BAP_INT				((UINT16)1)
-#define		HFA384x_PORTTYPE_IBSS			((UINT16)0)
-#define		HFA384x_PORTTYPE_BSS			((UINT16)1)
-#define		HFA384x_PORTTYPE_WDS			((UINT16)2)
-#define		HFA384x_PORTTYPE_PSUEDOIBSS		((UINT16)3)
-#define		HFA384x_PORTTYPE_HOSTAP    		((UINT16)6)
-#define		HFA384x_WEPFLAGS_PRIVINVOKED		((UINT16)BIT0)
-#define		HFA384x_WEPFLAGS_EXCLUDE		((UINT16)BIT1)
-#define		HFA384x_WEPFLAGS_DISABLE_TXCRYPT	((UINT16)BIT4)
-#define		HFA384x_WEPFLAGS_DISABLE_RXCRYPT	((UINT16)BIT7)
-#define		HFA384x_WEPFLAGS_DISALLOW_MIXED 	((UINT16)BIT11)
-#define		HFA384x_WEPFLAGS_IV_INTERVAL1		((UINT16)0)
-#define		HFA384x_WEPFLAGS_IV_INTERVAL10		((UINT16)BIT5)
-#define		HFA384x_WEPFLAGS_IV_INTERVAL50		((UINT16)BIT6)
-#define		HFA384x_WEPFLAGS_IV_INTERVAL100		((UINT16)(BIT5 | BIT6))
-#define		HFA384x_WEPFLAGS_FIRMWARE_WPA  		((UINT16)BIT8)
-#define		HFA384x_WEPFLAGS_HOST_MIC      		((UINT16)BIT9)
-#define 	HFA384x_ROAMMODE_FWSCAN_FWROAM		((UINT16)1)
-#define 	HFA384x_ROAMMODE_FWSCAN_HOSTROAM	((UINT16)2)
-#define 	HFA384x_ROAMMODE_HOSTSCAN_HOSTROAM	((UINT16)3)
-#define 	HFA384x_PORTSTATUS_DISABLED		((UINT16)1)
-#define 	HFA384x_PORTSTATUS_INITSRCH		((UINT16)2)
-#define 	HFA384x_PORTSTATUS_CONN_IBSS		((UINT16)3)
-#define 	HFA384x_PORTSTATUS_CONN_ESS		((UINT16)4)
-#define 	HFA384x_PORTSTATUS_OOR_ESS		((UINT16)5)
-#define 	HFA384x_PORTSTATUS_CONN_WDS		((UINT16)6)
-#define 	HFA384x_PORTSTATUS_HOSTAP		((UINT16)8)
-#define		HFA384x_RATEBIT_1			((UINT16)1)
-#define		HFA384x_RATEBIT_2			((UINT16)2)
-#define		HFA384x_RATEBIT_5dot5			((UINT16)4)
-#define		HFA384x_RATEBIT_11			((UINT16)8)
+#define		HFA384x_BAP_PROC			((u16)0)
+#define		HFA384x_BAP_int				((u16)1)
+#define		HFA384x_PORTTYPE_IBSS			((u16)0)
+#define		HFA384x_PORTTYPE_BSS			((u16)1)
+#define		HFA384x_PORTTYPE_WDS			((u16)2)
+#define		HFA384x_PORTTYPE_PSUEDOIBSS		((u16)3)
+#define		HFA384x_PORTTYPE_HOSTAP    		((u16)6)
+#define		HFA384x_WEPFLAGS_PRIVINVOKED		((u16)BIT0)
+#define		HFA384x_WEPFLAGS_EXCLUDE		((u16)BIT1)
+#define		HFA384x_WEPFLAGS_DISABLE_TXCRYPT	((u16)BIT4)
+#define		HFA384x_WEPFLAGS_DISABLE_RXCRYPT	((u16)BIT7)
+#define		HFA384x_WEPFLAGS_DISALLOW_MIXED 	((u16)BIT11)
+#define		HFA384x_WEPFLAGS_IV_intERVAL1		((u16)0)
+#define		HFA384x_WEPFLAGS_IV_intERVAL10		((u16)BIT5)
+#define		HFA384x_WEPFLAGS_IV_intERVAL50		((u16)BIT6)
+#define		HFA384x_WEPFLAGS_IV_intERVAL100		((u16)(BIT5 | BIT6))
+#define		HFA384x_WEPFLAGS_FIRMWARE_WPA  		((u16)BIT8)
+#define		HFA384x_WEPFLAGS_HOST_MIC      		((u16)BIT9)
+#define 	HFA384x_ROAMMODE_FWSCAN_FWROAM		((u16)1)
+#define 	HFA384x_ROAMMODE_FWSCAN_HOSTROAM	((u16)2)
+#define 	HFA384x_ROAMMODE_HOSTSCAN_HOSTROAM	((u16)3)
+#define 	HFA384x_PORTSTATUS_DISABLED		((u16)1)
+#define 	HFA384x_PORTSTATUS_INITSRCH		((u16)2)
+#define 	HFA384x_PORTSTATUS_CONN_IBSS		((u16)3)
+#define 	HFA384x_PORTSTATUS_CONN_ESS		((u16)4)
+#define 	HFA384x_PORTSTATUS_OOR_ESS		((u16)5)
+#define 	HFA384x_PORTSTATUS_CONN_WDS		((u16)6)
+#define 	HFA384x_PORTSTATUS_HOSTAP		((u16)8)
+#define		HFA384x_RATEBIT_1			((u16)1)
+#define		HFA384x_RATEBIT_2			((u16)2)
+#define		HFA384x_RATEBIT_5dot5			((u16)4)
+#define		HFA384x_RATEBIT_11			((u16)8)
 
 /*--- Just some symbolic names for legibility -------*/
-#define		HFA384x_TXCMD_NORECL		((UINT16)0)
-#define		HFA384x_TXCMD_RECL		((UINT16)1)
+#define		HFA384x_TXCMD_NORECL		((u16)0)
+#define		HFA384x_TXCMD_RECL		((u16)1)
 
 /*--- MAC Internal memory constants and macros ------*/
 /* masks and macros used to manipulate MAC internal memory addresses. */
@@ -140,7 +140,7 @@
  */
 
 /* Handy constant */
-#define		HFA384x_ADDR_AUX_OFF_MAX	((UINT16)0x007f)
+#define		HFA384x_ADDR_AUX_OFF_MAX	((u16)0x007f)
 
 /* Mask bits for discarding unwanted pieces in a flat address */
 #define		HFA384x_ADDR_FLAT_AUX_PAGE_MASK	(0x007fff80)
@@ -158,25 +158,25 @@
 
 /* Make a 32-bit flat address from AUX format 16-bit page and offset */
 #define		HFA384x_ADDR_AUX_MKFLAT(p,o)	\
-		(((UINT32)(((UINT16)(p))&HFA384x_ADDR_AUX_PAGE_MASK)) <<7) | \
-		((UINT32)(((UINT16)(o))&HFA384x_ADDR_AUX_OFF_MASK))
+		(((u32)(((u16)(p))&HFA384x_ADDR_AUX_PAGE_MASK)) <<7) | \
+		((u32)(((u16)(o))&HFA384x_ADDR_AUX_OFF_MASK))
 
 /* Make a 32-bit flat address from CMD format 16-bit page and offset */
 #define		HFA384x_ADDR_CMD_MKFLAT(p,o)	\
-		(((UINT32)(((UINT16)(p))&HFA384x_ADDR_CMD_PAGE_MASK)) <<16) | \
-		((UINT32)(((UINT16)(o))&HFA384x_ADDR_CMD_OFF_MASK))
+		(((u32)(((u16)(p))&HFA384x_ADDR_CMD_PAGE_MASK)) <<16) | \
+		((u32)(((u16)(o))&HFA384x_ADDR_CMD_OFF_MASK))
 
 /* Make AUX format offset and page from a 32-bit flat address */
 #define		HFA384x_ADDR_AUX_MKPAGE(f) \
-		((UINT16)((((UINT32)(f))&HFA384x_ADDR_FLAT_AUX_PAGE_MASK)>>7))
+		((u16)((((u32)(f))&HFA384x_ADDR_FLAT_AUX_PAGE_MASK)>>7))
 #define		HFA384x_ADDR_AUX_MKOFF(f) \
-		((UINT16)(((UINT32)(f))&HFA384x_ADDR_FLAT_AUX_OFF_MASK))
+		((u16)(((u32)(f))&HFA384x_ADDR_FLAT_AUX_OFF_MASK))
 
 /* Make CMD format offset and page from a 32-bit flat address */
 #define		HFA384x_ADDR_CMD_MKPAGE(f) \
-		((UINT16)((((UINT32)(f))&HFA384x_ADDR_FLAT_CMD_PAGE_MASK)>>16))
+		((u16)((((u32)(f))&HFA384x_ADDR_FLAT_CMD_PAGE_MASK)>>16))
 #define		HFA384x_ADDR_CMD_MKOFF(f) \
-		((UINT16)(((UINT32)(f))&HFA384x_ADDR_FLAT_CMD_OFF_MASK))
+		((u16)(((u32)(f))&HFA384x_ADDR_FLAT_CMD_OFF_MASK))
 
 /*--- Aux register masks/tests ----------------------*/
 /* Some of the upper bits of the AUX offset register are used to */
@@ -188,7 +188,7 @@
 
 /* Make AUX register offset and page values from a flat address */
 #define		HFA384x_AUX_MKOFF(f, c) \
-	(HFA384x_ADDR_AUX_MKOFF(f) | (((UINT16)(c))<<12))
+	(HFA384x_ADDR_AUX_MKOFF(f) | (((u16)(c))<<12))
 #define		HFA384x_AUX_MKPAGE(f)	HFA384x_ADDR_AUX_MKPAGE(f)
 
 
@@ -205,40 +205,6 @@
 #define		HFA384x_DLSTATE_FLASHWRITEPENDING	4
 #define		HFA384x_DLSTATE_GENESIS 		5
 
-/*--- Register I/O offsets --------------------------*/
-#if ((WLAN_HOSTIF == WLAN_PCMCIA) || (WLAN_HOSTIF == WLAN_PLX))
-
-#define		HFA384x_CMD_OFF			(0x00)
-#define		HFA384x_PARAM0_OFF		(0x02)
-#define		HFA384x_PARAM1_OFF		(0x04)
-#define		HFA384x_PARAM2_OFF		(0x06)
-#define		HFA384x_STATUS_OFF		(0x08)
-#define		HFA384x_RESP0_OFF		(0x0A)
-#define		HFA384x_RESP1_OFF		(0x0C)
-#define		HFA384x_RESP2_OFF		(0x0E)
-#define		HFA384x_INFOFID_OFF		(0x10)
-#define		HFA384x_RXFID_OFF		(0x20)
-#define		HFA384x_ALLOCFID_OFF		(0x22)
-#define		HFA384x_TXCOMPLFID_OFF		(0x24)
-#define		HFA384x_SELECT0_OFF		(0x18)
-#define		HFA384x_OFFSET0_OFF		(0x1C)
-#define		HFA384x_DATA0_OFF		(0x36)
-#define		HFA384x_SELECT1_OFF		(0x1A)
-#define		HFA384x_OFFSET1_OFF		(0x1E)
-#define		HFA384x_DATA1_OFF		(0x38)
-#define		HFA384x_EVSTAT_OFF		(0x30)
-#define		HFA384x_INTEN_OFF		(0x32)
-#define		HFA384x_EVACK_OFF		(0x34)
-#define		HFA384x_CONTROL_OFF		(0x14)
-#define		HFA384x_SWSUPPORT0_OFF		(0x28)
-#define		HFA384x_SWSUPPORT1_OFF		(0x2A)
-#define		HFA384x_SWSUPPORT2_OFF		(0x2C)
-#define		HFA384x_AUXPAGE_OFF		(0x3A)
-#define		HFA384x_AUXOFFSET_OFF		(0x3C)
-#define		HFA384x_AUXDATA_OFF		(0x3E)
-
-#elif (WLAN_HOSTIF == WLAN_PCI || WLAN_HOSTIF == WLAN_USB)
-
 #define		HFA384x_CMD_OFF			(0x00)
 #define		HFA384x_PARAM0_OFF		(0x04)
 #define		HFA384x_PARAM1_OFF		(0x08)
@@ -258,7 +224,7 @@
 #define		HFA384x_OFFSET1_OFF		(0x3c)
 #define		HFA384x_DATA1_OFF		(0x70)
 #define		HFA384x_EVSTAT_OFF		(0x60)
-#define		HFA384x_INTEN_OFF		(0x64)
+#define		HFA384x_intEN_OFF		(0x64)
 #define		HFA384x_EVACK_OFF		(0x68)
 #define		HFA384x_CONTROL_OFF		(0x28)
 #define		HFA384x_SWSUPPORT0_OFF		(0x50)
@@ -279,94 +245,92 @@
 #define		HFA384x_PCI_M1_LEN_OFF		(0xa8)
 #define		HFA384x_PCI_M1_CTL_OFF		(0xac)
 
-#endif
-
 /*--- Register Field Masks --------------------------*/
-#define		HFA384x_CMD_BUSY		((UINT16)BIT15)
-#define		HFA384x_CMD_AINFO		((UINT16)(BIT14 | BIT13 | BIT12 | BIT11 | BIT10 | BIT9 | BIT8))
-#define		HFA384x_CMD_MACPORT		((UINT16)(BIT10 | BIT9 | BIT8))
-#define		HFA384x_CMD_RECL		((UINT16)BIT8)
-#define		HFA384x_CMD_WRITE		((UINT16)BIT8)
-#define		HFA384x_CMD_PROGMODE		((UINT16)(BIT9 | BIT8))
-#define		HFA384x_CMD_CMDCODE		((UINT16)(BIT5 | BIT4 | BIT3 | BIT2 | BIT1 | BIT0))
-
-#define		HFA384x_STATUS_RESULT		((UINT16)(BIT14 | BIT13 | BIT12 | BIT11 | BIT10 | BIT9 | BIT8))
-#define		HFA384x_STATUS_CMDCODE		((UINT16)(BIT5 | BIT4 | BIT3 | BIT2 | BIT1 | BIT0))
-
-#define		HFA384x_OFFSET_BUSY		((UINT16)BIT15)
-#define		HFA384x_OFFSET_ERR		((UINT16)BIT14)
-#define		HFA384x_OFFSET_DATAOFF		((UINT16)(BIT11 | BIT10 | BIT9 | BIT8 | BIT7 | BIT6 | BIT5 | BIT4 | BIT3 | BIT2 | BIT1))
-
-#define		HFA384x_EVSTAT_TICK		((UINT16)BIT15)
-#define		HFA384x_EVSTAT_WTERR		((UINT16)BIT14)
-#define		HFA384x_EVSTAT_INFDROP		((UINT16)BIT13)
-#define		HFA384x_EVSTAT_INFO		((UINT16)BIT7)
-#define		HFA384x_EVSTAT_DTIM		((UINT16)BIT5)
-#define		HFA384x_EVSTAT_CMD		((UINT16)BIT4)
-#define		HFA384x_EVSTAT_ALLOC		((UINT16)BIT3)
-#define		HFA384x_EVSTAT_TXEXC		((UINT16)BIT2)
-#define		HFA384x_EVSTAT_TX		((UINT16)BIT1)
-#define		HFA384x_EVSTAT_RX		((UINT16)BIT0)
-
-#define         HFA384x_INT_BAP_OP           (HFA384x_EVSTAT_INFO|HFA384x_EVSTAT_RX|HFA384x_EVSTAT_TX|HFA384x_EVSTAT_TXEXC)
-
-#define         HFA384x_INT_NORMAL           (HFA384x_EVSTAT_INFO|HFA384x_EVSTAT_RX|HFA384x_EVSTAT_TX|HFA384x_EVSTAT_TXEXC|HFA384x_EVSTAT_INFDROP|HFA384x_EVSTAT_ALLOC|HFA384x_EVSTAT_DTIM)
-
-#define		HFA384x_INTEN_TICK		((UINT16)BIT15)
-#define		HFA384x_INTEN_WTERR		((UINT16)BIT14)
-#define		HFA384x_INTEN_INFDROP		((UINT16)BIT13)
-#define		HFA384x_INTEN_INFO		((UINT16)BIT7)
-#define		HFA384x_INTEN_DTIM		((UINT16)BIT5)
-#define		HFA384x_INTEN_CMD		((UINT16)BIT4)
-#define		HFA384x_INTEN_ALLOC		((UINT16)BIT3)
-#define		HFA384x_INTEN_TXEXC		((UINT16)BIT2)
-#define		HFA384x_INTEN_TX		((UINT16)BIT1)
-#define		HFA384x_INTEN_RX		((UINT16)BIT0)
-
-#define		HFA384x_EVACK_TICK		((UINT16)BIT15)
-#define		HFA384x_EVACK_WTERR		((UINT16)BIT14)
-#define		HFA384x_EVACK_INFDROP		((UINT16)BIT13)
-#define		HFA384x_EVACK_INFO		((UINT16)BIT7)
-#define		HFA384x_EVACK_DTIM		((UINT16)BIT5)
-#define		HFA384x_EVACK_CMD		((UINT16)BIT4)
-#define		HFA384x_EVACK_ALLOC		((UINT16)BIT3)
-#define		HFA384x_EVACK_TXEXC		((UINT16)BIT2)
-#define		HFA384x_EVACK_TX		((UINT16)BIT1)
-#define		HFA384x_EVACK_RX		((UINT16)BIT0)
-
-#define		HFA384x_CONTROL_AUXEN		((UINT16)(BIT15 | BIT14))
+#define		HFA384x_CMD_BUSY		((u16)BIT15)
+#define		HFA384x_CMD_AINFO		((u16)(BIT14 | BIT13 | BIT12 | BIT11 | BIT10 | BIT9 | BIT8))
+#define		HFA384x_CMD_MACPORT		((u16)(BIT10 | BIT9 | BIT8))
+#define		HFA384x_CMD_RECL		((u16)BIT8)
+#define		HFA384x_CMD_WRITE		((u16)BIT8)
+#define		HFA384x_CMD_PROGMODE		((u16)(BIT9 | BIT8))
+#define		HFA384x_CMD_CMDCODE		((u16)(BIT5 | BIT4 | BIT3 | BIT2 | BIT1 | BIT0))
+
+#define		HFA384x_STATUS_RESULT		((u16)(BIT14 | BIT13 | BIT12 | BIT11 | BIT10 | BIT9 | BIT8))
+#define		HFA384x_STATUS_CMDCODE		((u16)(BIT5 | BIT4 | BIT3 | BIT2 | BIT1 | BIT0))
+
+#define		HFA384x_OFFSET_BUSY		((u16)BIT15)
+#define		HFA384x_OFFSET_ERR		((u16)BIT14)
+#define		HFA384x_OFFSET_DATAOFF		((u16)(BIT11 | BIT10 | BIT9 | BIT8 | BIT7 | BIT6 | BIT5 | BIT4 | BIT3 | BIT2 | BIT1))
+
+#define		HFA384x_EVSTAT_TICK		((u16)BIT15)
+#define		HFA384x_EVSTAT_WTERR		((u16)BIT14)
+#define		HFA384x_EVSTAT_INFDROP		((u16)BIT13)
+#define		HFA384x_EVSTAT_INFO		((u16)BIT7)
+#define		HFA384x_EVSTAT_DTIM		((u16)BIT5)
+#define		HFA384x_EVSTAT_CMD		((u16)BIT4)
+#define		HFA384x_EVSTAT_ALLOC		((u16)BIT3)
+#define		HFA384x_EVSTAT_TXEXC		((u16)BIT2)
+#define		HFA384x_EVSTAT_TX		((u16)BIT1)
+#define		HFA384x_EVSTAT_RX		((u16)BIT0)
+
+#define         HFA384x_int_BAP_OP           (HFA384x_EVSTAT_INFO|HFA384x_EVSTAT_RX|HFA384x_EVSTAT_TX|HFA384x_EVSTAT_TXEXC)
+
+#define         HFA384x_int_NORMAL           (HFA384x_EVSTAT_INFO|HFA384x_EVSTAT_RX|HFA384x_EVSTAT_TX|HFA384x_EVSTAT_TXEXC|HFA384x_EVSTAT_INFDROP|HFA384x_EVSTAT_ALLOC|HFA384x_EVSTAT_DTIM)
+
+#define		HFA384x_intEN_TICK		((u16)BIT15)
+#define		HFA384x_intEN_WTERR		((u16)BIT14)
+#define		HFA384x_intEN_INFDROP		((u16)BIT13)
+#define		HFA384x_intEN_INFO		((u16)BIT7)
+#define		HFA384x_intEN_DTIM		((u16)BIT5)
+#define		HFA384x_intEN_CMD		((u16)BIT4)
+#define		HFA384x_intEN_ALLOC		((u16)BIT3)
+#define		HFA384x_intEN_TXEXC		((u16)BIT2)
+#define		HFA384x_intEN_TX		((u16)BIT1)
+#define		HFA384x_intEN_RX		((u16)BIT0)
+
+#define		HFA384x_EVACK_TICK		((u16)BIT15)
+#define		HFA384x_EVACK_WTERR		((u16)BIT14)
+#define		HFA384x_EVACK_INFDROP		((u16)BIT13)
+#define		HFA384x_EVACK_INFO		((u16)BIT7)
+#define		HFA384x_EVACK_DTIM		((u16)BIT5)
+#define		HFA384x_EVACK_CMD		((u16)BIT4)
+#define		HFA384x_EVACK_ALLOC		((u16)BIT3)
+#define		HFA384x_EVACK_TXEXC		((u16)BIT2)
+#define		HFA384x_EVACK_TX		((u16)BIT1)
+#define		HFA384x_EVACK_RX		((u16)BIT0)
+
+#define		HFA384x_CONTROL_AUXEN		((u16)(BIT15 | BIT14))
 
 
 /*--- Command Code Constants --------------------------*/
 /*--- Controller Commands --------------------------*/
-#define		HFA384x_CMDCODE_INIT		((UINT16)0x00)
-#define		HFA384x_CMDCODE_ENABLE		((UINT16)0x01)
-#define		HFA384x_CMDCODE_DISABLE		((UINT16)0x02)
-#define		HFA384x_CMDCODE_DIAG		((UINT16)0x03)
+#define		HFA384x_CMDCODE_INIT		((u16)0x00)
+#define		HFA384x_CMDCODE_ENABLE		((u16)0x01)
+#define		HFA384x_CMDCODE_DISABLE		((u16)0x02)
+#define		HFA384x_CMDCODE_DIAG		((u16)0x03)
 
 /*--- Buffer Mgmt Commands --------------------------*/
-#define		HFA384x_CMDCODE_ALLOC		((UINT16)0x0A)
-#define		HFA384x_CMDCODE_TX		((UINT16)0x0B)
-#define		HFA384x_CMDCODE_CLRPRST		((UINT16)0x12)
+#define		HFA384x_CMDCODE_ALLOC		((u16)0x0A)
+#define		HFA384x_CMDCODE_TX		((u16)0x0B)
+#define		HFA384x_CMDCODE_CLRPRST		((u16)0x12)
 
 /*--- Regulate Commands --------------------------*/
-#define		HFA384x_CMDCODE_NOTIFY		((UINT16)0x10)
-#define		HFA384x_CMDCODE_INQ		((UINT16)0x11)
+#define		HFA384x_CMDCODE_NOTIFY		((u16)0x10)
+#define		HFA384x_CMDCODE_INQ		((u16)0x11)
 
 /*--- Configure Commands --------------------------*/
-#define		HFA384x_CMDCODE_ACCESS		((UINT16)0x21)
-#define		HFA384x_CMDCODE_DOWNLD		((UINT16)0x22)
+#define		HFA384x_CMDCODE_ACCESS		((u16)0x21)
+#define		HFA384x_CMDCODE_DOWNLD		((u16)0x22)
 
 /*--- Debugging Commands -----------------------------*/
-#define 	HFA384x_CMDCODE_MONITOR		((UINT16)(0x38))
-#define		HFA384x_MONITOR_ENABLE		((UINT16)(0x0b))
-#define		HFA384x_MONITOR_DISABLE		((UINT16)(0x0f))
+#define 	HFA384x_CMDCODE_MONITOR		((u16)(0x38))
+#define		HFA384x_MONITOR_ENABLE		((u16)(0x0b))
+#define		HFA384x_MONITOR_DISABLE		((u16)(0x0f))
 
 /*--- Result Codes --------------------------*/
-#define		HFA384x_SUCCESS			((UINT16)(0x00))
-#define		HFA384x_CARD_FAIL		((UINT16)(0x01))
-#define		HFA384x_NO_BUFF			((UINT16)(0x05))
-#define		HFA384x_CMD_ERR			((UINT16)(0x7F))
+#define		HFA384x_SUCCESS			((u16)(0x00))
+#define		HFA384x_CARD_FAIL		((u16)(0x01))
+#define		HFA384x_NO_BUFF			((u16)(0x05))
+#define		HFA384x_CMD_ERR			((u16)(0x7F))
 
 /*--- Programming Modes --------------------------
 	MODE 0: Disable programming
@@ -374,48 +338,48 @@
 	MODE 2: Enable non-volatile memory programming
 	MODE 3: Program non-volatile memory section
 --------------------------------------------------*/
-#define		HFA384x_PROGMODE_DISABLE	((UINT16)0x00)
-#define		HFA384x_PROGMODE_RAM		((UINT16)0x01)
-#define		HFA384x_PROGMODE_NV		((UINT16)0x02)
-#define		HFA384x_PROGMODE_NVWRITE	((UINT16)0x03)
+#define		HFA384x_PROGMODE_DISABLE	((u16)0x00)
+#define		HFA384x_PROGMODE_RAM		((u16)0x01)
+#define		HFA384x_PROGMODE_NV		((u16)0x02)
+#define		HFA384x_PROGMODE_NVWRITE	((u16)0x03)
 
 /*--- AUX register enable --------------------------*/
-#define		HFA384x_AUXPW0			((UINT16)0xfe01)
-#define		HFA384x_AUXPW1			((UINT16)0xdc23)
-#define		HFA384x_AUXPW2			((UINT16)0xba45)
+#define		HFA384x_AUXPW0			((u16)0xfe01)
+#define		HFA384x_AUXPW1			((u16)0xdc23)
+#define		HFA384x_AUXPW2			((u16)0xba45)
 
-#define		HFA384x_CONTROL_AUX_ISDISABLED	((UINT16)0x0000)
-#define		HFA384x_CONTROL_AUX_ISENABLED	((UINT16)0xc000)
-#define		HFA384x_CONTROL_AUX_DOENABLE	((UINT16)0x8000)
-#define		HFA384x_CONTROL_AUX_DODISABLE	((UINT16)0x4000)
+#define		HFA384x_CONTROL_AUX_ISDISABLED	((u16)0x0000)
+#define		HFA384x_CONTROL_AUX_ISENABLED	((u16)0xc000)
+#define		HFA384x_CONTROL_AUX_DOENABLE	((u16)0x8000)
+#define		HFA384x_CONTROL_AUX_DODISABLE	((u16)0x4000)
 
 /*--- Record ID Constants --------------------------*/
 /*--------------------------------------------------------------------
 Configuration RIDs: Network Parameters, Static Configuration Entities
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_CNFPORTTYPE		((UINT16)0xFC00)
-#define		HFA384x_RID_CNFOWNMACADDR	((UINT16)0xFC01)
-#define		HFA384x_RID_CNFDESIREDSSID	((UINT16)0xFC02)
-#define		HFA384x_RID_CNFOWNCHANNEL	((UINT16)0xFC03)
-#define		HFA384x_RID_CNFOWNSSID		((UINT16)0xFC04)
-#define		HFA384x_RID_CNFOWNATIMWIN	((UINT16)0xFC05)
-#define		HFA384x_RID_CNFSYSSCALE		((UINT16)0xFC06)
-#define		HFA384x_RID_CNFMAXDATALEN	((UINT16)0xFC07)
-#define		HFA384x_RID_CNFWDSADDR		((UINT16)0xFC08)
-#define		HFA384x_RID_CNFPMENABLED	((UINT16)0xFC09)
-#define		HFA384x_RID_CNFPMEPS		((UINT16)0xFC0A)
-#define		HFA384x_RID_CNFMULTICASTRX	((UINT16)0xFC0B)
-#define		HFA384x_RID_CNFMAXSLEEPDUR	((UINT16)0xFC0C)
-#define		HFA384x_RID_CNFPMHOLDDUR	((UINT16)0xFC0D)
-#define		HFA384x_RID_CNFOWNNAME		((UINT16)0xFC0E)
-#define		HFA384x_RID_CNFOWNDTIMPER	((UINT16)0xFC10)
-#define		HFA384x_RID_CNFWDSADDR1		((UINT16)0xFC11)
-#define		HFA384x_RID_CNFWDSADDR2		((UINT16)0xFC12)
-#define		HFA384x_RID_CNFWDSADDR3		((UINT16)0xFC13)
-#define		HFA384x_RID_CNFWDSADDR4		((UINT16)0xFC14)
-#define		HFA384x_RID_CNFWDSADDR5		((UINT16)0xFC15)
-#define		HFA384x_RID_CNFWDSADDR6		((UINT16)0xFC16)
-#define		HFA384x_RID_CNFMCASTPMBUFF	((UINT16)0xFC17)
+#define		HFA384x_RID_CNFPORTTYPE		((u16)0xFC00)
+#define		HFA384x_RID_CNFOWNMACADDR	((u16)0xFC01)
+#define		HFA384x_RID_CNFDESIREDSSID	((u16)0xFC02)
+#define		HFA384x_RID_CNFOWNCHANNEL	((u16)0xFC03)
+#define		HFA384x_RID_CNFOWNSSID		((u16)0xFC04)
+#define		HFA384x_RID_CNFOWNATIMWIN	((u16)0xFC05)
+#define		HFA384x_RID_CNFSYSSCALE		((u16)0xFC06)
+#define		HFA384x_RID_CNFMAXDATALEN	((u16)0xFC07)
+#define		HFA384x_RID_CNFWDSADDR		((u16)0xFC08)
+#define		HFA384x_RID_CNFPMENABLED	((u16)0xFC09)
+#define		HFA384x_RID_CNFPMEPS		((u16)0xFC0A)
+#define		HFA384x_RID_CNFMULTICASTRX	((u16)0xFC0B)
+#define		HFA384x_RID_CNFMAXSLEEPDUR	((u16)0xFC0C)
+#define		HFA384x_RID_CNFPMHOLDDUR	((u16)0xFC0D)
+#define		HFA384x_RID_CNFOWNNAME		((u16)0xFC0E)
+#define		HFA384x_RID_CNFOWNDTIMPER	((u16)0xFC10)
+#define		HFA384x_RID_CNFWDSADDR1		((u16)0xFC11)
+#define		HFA384x_RID_CNFWDSADDR2		((u16)0xFC12)
+#define		HFA384x_RID_CNFWDSADDR3		((u16)0xFC13)
+#define		HFA384x_RID_CNFWDSADDR4		((u16)0xFC14)
+#define		HFA384x_RID_CNFWDSADDR5		((u16)0xFC15)
+#define		HFA384x_RID_CNFWDSADDR6		((u16)0xFC16)
+#define		HFA384x_RID_CNFMCASTPMBUFF	((u16)0xFC17)
 
 /*--------------------------------------------------------------------
 Configuration RID lengths: Network Params, Static Config Entities
@@ -423,62 +387,62 @@ Configuration RID lengths: Network Params, Static Config Entities
   include the len or code fields)
 --------------------------------------------------------------------*/
 /* TODO: fill in the rest of these */
-#define		HFA384x_RID_CNFPORTTYPE_LEN	((UINT16)2)
-#define		HFA384x_RID_CNFOWNMACADDR_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFDESIREDSSID_LEN	((UINT16)34)
-#define		HFA384x_RID_CNFOWNCHANNEL_LEN	((UINT16)2)
-#define		HFA384x_RID_CNFOWNSSID_LEN	((UINT16)34)
-#define		HFA384x_RID_CNFOWNATIMWIN_LEN	((UINT16)2)
-#define		HFA384x_RID_CNFSYSSCALE_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFMAXDATALEN_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFWDSADDR_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFPMENABLED_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFPMEPS_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFMULTICASTRX_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFMAXSLEEPDUR_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFPMHOLDDUR_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFOWNNAME_LEN	((UINT16)34)
-#define		HFA384x_RID_CNFOWNDTIMPER_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFWDSADDR1_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFWDSADDR2_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFWDSADDR3_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFWDSADDR4_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFWDSADDR5_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFWDSADDR6_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFMCASTPMBUFF_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFAUTHENTICATION_LEN ((UINT16)sizeof(UINT16))
-#define		HFA384x_RID_CNFMAXSLEEPDUR_LEN	((UINT16)0)
+#define		HFA384x_RID_CNFPORTTYPE_LEN	((u16)2)
+#define		HFA384x_RID_CNFOWNMACADDR_LEN	((u16)6)
+#define		HFA384x_RID_CNFDESIREDSSID_LEN	((u16)34)
+#define		HFA384x_RID_CNFOWNCHANNEL_LEN	((u16)2)
+#define		HFA384x_RID_CNFOWNSSID_LEN	((u16)34)
+#define		HFA384x_RID_CNFOWNATIMWIN_LEN	((u16)2)
+#define		HFA384x_RID_CNFSYSSCALE_LEN	((u16)0)
+#define		HFA384x_RID_CNFMAXDATALEN_LEN	((u16)0)
+#define		HFA384x_RID_CNFWDSADDR_LEN	((u16)6)
+#define		HFA384x_RID_CNFPMENABLED_LEN	((u16)0)
+#define		HFA384x_RID_CNFPMEPS_LEN	((u16)0)
+#define		HFA384x_RID_CNFMULTICASTRX_LEN	((u16)0)
+#define		HFA384x_RID_CNFMAXSLEEPDUR_LEN	((u16)0)
+#define		HFA384x_RID_CNFPMHOLDDUR_LEN	((u16)0)
+#define		HFA384x_RID_CNFOWNNAME_LEN	((u16)34)
+#define		HFA384x_RID_CNFOWNDTIMPER_LEN	((u16)0)
+#define		HFA384x_RID_CNFWDSADDR1_LEN	((u16)6)
+#define		HFA384x_RID_CNFWDSADDR2_LEN	((u16)6)
+#define		HFA384x_RID_CNFWDSADDR3_LEN	((u16)6)
+#define		HFA384x_RID_CNFWDSADDR4_LEN	((u16)6)
+#define		HFA384x_RID_CNFWDSADDR5_LEN	((u16)6)
+#define		HFA384x_RID_CNFWDSADDR6_LEN	((u16)6)
+#define		HFA384x_RID_CNFMCASTPMBUFF_LEN	((u16)0)
+#define		HFA384x_RID_CNFAUTHENTICATION_LEN ((u16)sizeof(u16))
+#define		HFA384x_RID_CNFMAXSLEEPDUR_LEN	((u16)0)
 
 /*--------------------------------------------------------------------
 Configuration RIDs: Network Parameters, Dynamic Configuration Entities
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_GROUPADDR		((UINT16)0xFC80)
-#define		HFA384x_RID_CREATEIBSS		((UINT16)0xFC81)
-#define		HFA384x_RID_FRAGTHRESH		((UINT16)0xFC82)
-#define		HFA384x_RID_RTSTHRESH		((UINT16)0xFC83)
-#define		HFA384x_RID_TXRATECNTL		((UINT16)0xFC84)
-#define		HFA384x_RID_PROMISCMODE		((UINT16)0xFC85)
-#define		HFA384x_RID_FRAGTHRESH0		((UINT16)0xFC90)
-#define		HFA384x_RID_FRAGTHRESH1		((UINT16)0xFC91)
-#define		HFA384x_RID_FRAGTHRESH2		((UINT16)0xFC92)
-#define		HFA384x_RID_FRAGTHRESH3		((UINT16)0xFC93)
-#define		HFA384x_RID_FRAGTHRESH4		((UINT16)0xFC94)
-#define		HFA384x_RID_FRAGTHRESH5		((UINT16)0xFC95)
-#define		HFA384x_RID_FRAGTHRESH6		((UINT16)0xFC96)
-#define		HFA384x_RID_RTSTHRESH0		((UINT16)0xFC97)
-#define		HFA384x_RID_RTSTHRESH1		((UINT16)0xFC98)
-#define		HFA384x_RID_RTSTHRESH2		((UINT16)0xFC99)
-#define		HFA384x_RID_RTSTHRESH3		((UINT16)0xFC9A)
-#define		HFA384x_RID_RTSTHRESH4		((UINT16)0xFC9B)
-#define		HFA384x_RID_RTSTHRESH5		((UINT16)0xFC9C)
-#define		HFA384x_RID_RTSTHRESH6		((UINT16)0xFC9D)
-#define		HFA384x_RID_TXRATECNTL0		((UINT16)0xFC9E)
-#define		HFA384x_RID_TXRATECNTL1		((UINT16)0xFC9F)
-#define		HFA384x_RID_TXRATECNTL2		((UINT16)0xFCA0)
-#define		HFA384x_RID_TXRATECNTL3		((UINT16)0xFCA1)
-#define		HFA384x_RID_TXRATECNTL4		((UINT16)0xFCA2)
-#define		HFA384x_RID_TXRATECNTL5		((UINT16)0xFCA3)
-#define		HFA384x_RID_TXRATECNTL6		((UINT16)0xFCA4)
+#define		HFA384x_RID_GROUPADDR		((u16)0xFC80)
+#define		HFA384x_RID_CREATEIBSS		((u16)0xFC81)
+#define		HFA384x_RID_FRAGTHRESH		((u16)0xFC82)
+#define		HFA384x_RID_RTSTHRESH		((u16)0xFC83)
+#define		HFA384x_RID_TXRATECNTL		((u16)0xFC84)
+#define		HFA384x_RID_PROMISCMODE		((u16)0xFC85)
+#define		HFA384x_RID_FRAGTHRESH0		((u16)0xFC90)
+#define		HFA384x_RID_FRAGTHRESH1		((u16)0xFC91)
+#define		HFA384x_RID_FRAGTHRESH2		((u16)0xFC92)
+#define		HFA384x_RID_FRAGTHRESH3		((u16)0xFC93)
+#define		HFA384x_RID_FRAGTHRESH4		((u16)0xFC94)
+#define		HFA384x_RID_FRAGTHRESH5		((u16)0xFC95)
+#define		HFA384x_RID_FRAGTHRESH6		((u16)0xFC96)
+#define		HFA384x_RID_RTSTHRESH0		((u16)0xFC97)
+#define		HFA384x_RID_RTSTHRESH1		((u16)0xFC98)
+#define		HFA384x_RID_RTSTHRESH2		((u16)0xFC99)
+#define		HFA384x_RID_RTSTHRESH3		((u16)0xFC9A)
+#define		HFA384x_RID_RTSTHRESH4		((u16)0xFC9B)
+#define		HFA384x_RID_RTSTHRESH5		((u16)0xFC9C)
+#define		HFA384x_RID_RTSTHRESH6		((u16)0xFC9D)
+#define		HFA384x_RID_TXRATECNTL0		((u16)0xFC9E)
+#define		HFA384x_RID_TXRATECNTL1		((u16)0xFC9F)
+#define		HFA384x_RID_TXRATECNTL2		((u16)0xFCA0)
+#define		HFA384x_RID_TXRATECNTL3		((u16)0xFCA1)
+#define		HFA384x_RID_TXRATECNTL4		((u16)0xFCA2)
+#define		HFA384x_RID_TXRATECNTL5		((u16)0xFCA3)
+#define		HFA384x_RID_TXRATECNTL6		((u16)0xFCA4)
 
 /*--------------------------------------------------------------------
 Configuration RID Lengths: Network Param, Dynamic Config Entities
@@ -486,296 +450,296 @@ Configuration RID Lengths: Network Param, Dynamic Config Entities
   include the len or code fields)
 --------------------------------------------------------------------*/
 /* TODO: fill in the rest of these */
-#define		HFA384x_RID_GROUPADDR_LEN	((UINT16)16 * WLAN_ADDR_LEN)
-#define		HFA384x_RID_CREATEIBSS_LEN	((UINT16)0)
-#define		HFA384x_RID_FRAGTHRESH_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL_LEN	((UINT16)4)
-#define		HFA384x_RID_PROMISCMODE_LEN	((UINT16)2)
-#define		HFA384x_RID_FRAGTHRESH0_LEN	((UINT16)0)
-#define		HFA384x_RID_FRAGTHRESH1_LEN	((UINT16)0)
-#define		HFA384x_RID_FRAGTHRESH2_LEN	((UINT16)0)
-#define		HFA384x_RID_FRAGTHRESH3_LEN	((UINT16)0)
-#define		HFA384x_RID_FRAGTHRESH4_LEN	((UINT16)0)
-#define		HFA384x_RID_FRAGTHRESH5_LEN	((UINT16)0)
-#define		HFA384x_RID_FRAGTHRESH6_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH0_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH1_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH2_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH3_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH4_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH5_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH6_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL0_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL1_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL2_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL3_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL4_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL5_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL6_LEN	((UINT16)0)
+#define		HFA384x_RID_GROUPADDR_LEN	((u16)16 * WLAN_ADDR_LEN)
+#define		HFA384x_RID_CREATEIBSS_LEN	((u16)0)
+#define		HFA384x_RID_FRAGTHRESH_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL_LEN	((u16)4)
+#define		HFA384x_RID_PROMISCMODE_LEN	((u16)2)
+#define		HFA384x_RID_FRAGTHRESH0_LEN	((u16)0)
+#define		HFA384x_RID_FRAGTHRESH1_LEN	((u16)0)
+#define		HFA384x_RID_FRAGTHRESH2_LEN	((u16)0)
+#define		HFA384x_RID_FRAGTHRESH3_LEN	((u16)0)
+#define		HFA384x_RID_FRAGTHRESH4_LEN	((u16)0)
+#define		HFA384x_RID_FRAGTHRESH5_LEN	((u16)0)
+#define		HFA384x_RID_FRAGTHRESH6_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH0_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH1_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH2_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH3_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH4_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH5_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH6_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL0_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL1_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL2_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL3_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL4_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL5_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL6_LEN	((u16)0)
 
 /*--------------------------------------------------------------------
 Configuration RIDs: Behavior Parameters
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_ITICKTIME		((UINT16)0xFCE0)
+#define		HFA384x_RID_ITICKTIME		((u16)0xFCE0)
 
 /*--------------------------------------------------------------------
 Configuration RID Lengths: Behavior Parameters
   This is the length of JUST the DATA part of the RID (does not
   include the len or code fields)
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_ITICKTIME_LEN	((UINT16)2)
+#define		HFA384x_RID_ITICKTIME_LEN	((u16)2)
 
 /*----------------------------------------------------------------------
 Information RIDs: NIC Information
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_MAXLOADTIME		((UINT16)0xFD00)
-#define		HFA384x_RID_DOWNLOADBUFFER	((UINT16)0xFD01)
-#define		HFA384x_RID_PRIIDENTITY		((UINT16)0xFD02)
-#define		HFA384x_RID_PRISUPRANGE		((UINT16)0xFD03)
-#define		HFA384x_RID_PRI_CFIACTRANGES	((UINT16)0xFD04)
-#define		HFA384x_RID_NICSERIALNUMBER	((UINT16)0xFD0A)
-#define		HFA384x_RID_NICIDENTITY		((UINT16)0xFD0B)
-#define		HFA384x_RID_MFISUPRANGE		((UINT16)0xFD0C)
-#define		HFA384x_RID_CFISUPRANGE		((UINT16)0xFD0D)
-#define		HFA384x_RID_CHANNELLIST		((UINT16)0xFD10)
-#define		HFA384x_RID_REGULATORYDOMAINS	((UINT16)0xFD11)
-#define		HFA384x_RID_TEMPTYPE		((UINT16)0xFD12)
-#define		HFA384x_RID_CIS			((UINT16)0xFD13)
-#define		HFA384x_RID_STAIDENTITY		((UINT16)0xFD20)
-#define		HFA384x_RID_STASUPRANGE		((UINT16)0xFD21)
-#define		HFA384x_RID_STA_MFIACTRANGES	((UINT16)0xFD22)
-#define		HFA384x_RID_STA_CFIACTRANGES	((UINT16)0xFD23)
-#define		HFA384x_RID_BUILDSEQ		((UINT16)0xFFFE)
-#define		HFA384x_RID_FWID		((UINT16)0xFFFF)
+#define		HFA384x_RID_MAXLOADTIME		((u16)0xFD00)
+#define		HFA384x_RID_DOWNLOADBUFFER	((u16)0xFD01)
+#define		HFA384x_RID_PRIIDENTITY		((u16)0xFD02)
+#define		HFA384x_RID_PRISUPRANGE		((u16)0xFD03)
+#define		HFA384x_RID_PRI_CFIACTRANGES	((u16)0xFD04)
+#define		HFA384x_RID_NICSERIALNUMBER	((u16)0xFD0A)
+#define		HFA384x_RID_NICIDENTITY		((u16)0xFD0B)
+#define		HFA384x_RID_MFISUPRANGE		((u16)0xFD0C)
+#define		HFA384x_RID_CFISUPRANGE		((u16)0xFD0D)
+#define		HFA384x_RID_CHANNELLIST		((u16)0xFD10)
+#define		HFA384x_RID_REGULATORYDOMAINS	((u16)0xFD11)
+#define		HFA384x_RID_TEMPTYPE		((u16)0xFD12)
+#define		HFA384x_RID_CIS			((u16)0xFD13)
+#define		HFA384x_RID_STAIDENTITY		((u16)0xFD20)
+#define		HFA384x_RID_STASUPRANGE		((u16)0xFD21)
+#define		HFA384x_RID_STA_MFIACTRANGES	((u16)0xFD22)
+#define		HFA384x_RID_STA_CFIACTRANGES	((u16)0xFD23)
+#define		HFA384x_RID_BUILDSEQ		((u16)0xFFFE)
+#define		HFA384x_RID_FWID		((u16)0xFFFF)
 
 /*----------------------------------------------------------------------
 Information RID Lengths: NIC Information
   This is the length of JUST the DATA part of the RID (does not
   include the len or code fields)
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_MAXLOADTIME_LEN		((UINT16)0)
-#define		HFA384x_RID_DOWNLOADBUFFER_LEN		((UINT16)sizeof(hfa384x_downloadbuffer_t))
-#define		HFA384x_RID_PRIIDENTITY_LEN		((UINT16)8)
-#define		HFA384x_RID_PRISUPRANGE_LEN		((UINT16)10)
-#define		HFA384x_RID_CFIACTRANGES_LEN		((UINT16)10)
-#define		HFA384x_RID_NICSERIALNUMBER_LEN		((UINT16)12)
-#define		HFA384x_RID_NICIDENTITY_LEN		((UINT16)8)
-#define		HFA384x_RID_MFISUPRANGE_LEN		((UINT16)10)
-#define		HFA384x_RID_CFISUPRANGE_LEN		((UINT16)10)
-#define		HFA384x_RID_CHANNELLIST_LEN		((UINT16)0)
-#define		HFA384x_RID_REGULATORYDOMAINS_LEN	((UINT16)12)
-#define		HFA384x_RID_TEMPTYPE_LEN		((UINT16)0)
-#define		HFA384x_RID_CIS_LEN			((UINT16)480)
-#define		HFA384x_RID_STAIDENTITY_LEN		((UINT16)8)
-#define		HFA384x_RID_STASUPRANGE_LEN		((UINT16)10)
-#define		HFA384x_RID_MFIACTRANGES_LEN		((UINT16)10)
-#define		HFA384x_RID_CFIACTRANGES2_LEN		((UINT16)10)
-#define		HFA384x_RID_BUILDSEQ_LEN		((UINT16)sizeof(hfa384x_BuildSeq_t))
-#define		HFA384x_RID_FWID_LEN			((UINT16)sizeof(hfa384x_FWID_t))
+#define		HFA384x_RID_MAXLOADTIME_LEN		((u16)0)
+#define		HFA384x_RID_DOWNLOADBUFFER_LEN		((u16)sizeof(hfa384x_downloadbuffer_t))
+#define		HFA384x_RID_PRIIDENTITY_LEN		((u16)8)
+#define		HFA384x_RID_PRISUPRANGE_LEN		((u16)10)
+#define		HFA384x_RID_CFIACTRANGES_LEN		((u16)10)
+#define		HFA384x_RID_NICSERIALNUMBER_LEN		((u16)12)
+#define		HFA384x_RID_NICIDENTITY_LEN		((u16)8)
+#define		HFA384x_RID_MFISUPRANGE_LEN		((u16)10)
+#define		HFA384x_RID_CFISUPRANGE_LEN		((u16)10)
+#define		HFA384x_RID_CHANNELLIST_LEN		((u16)0)
+#define		HFA384x_RID_REGULATORYDOMAINS_LEN	((u16)12)
+#define		HFA384x_RID_TEMPTYPE_LEN		((u16)0)
+#define		HFA384x_RID_CIS_LEN			((u16)480)
+#define		HFA384x_RID_STAIDENTITY_LEN		((u16)8)
+#define		HFA384x_RID_STASUPRANGE_LEN		((u16)10)
+#define		HFA384x_RID_MFIACTRANGES_LEN		((u16)10)
+#define		HFA384x_RID_CFIACTRANGES2_LEN		((u16)10)
+#define		HFA384x_RID_BUILDSEQ_LEN		((u16)sizeof(hfa384x_BuildSeq_t))
+#define		HFA384x_RID_FWID_LEN			((u16)sizeof(hfa384x_FWID_t))
 
 /*--------------------------------------------------------------------
 Information RIDs:  MAC Information
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_PORTSTATUS		((UINT16)0xFD40)
-#define		HFA384x_RID_CURRENTSSID		((UINT16)0xFD41)
-#define		HFA384x_RID_CURRENTBSSID	((UINT16)0xFD42)
-#define		HFA384x_RID_COMMSQUALITY	((UINT16)0xFD43)
-#define		HFA384x_RID_CURRENTTXRATE	((UINT16)0xFD44)
-#define		HFA384x_RID_CURRENTBCNINT	((UINT16)0xFD45)
-#define		HFA384x_RID_CURRENTSCALETHRESH	((UINT16)0xFD46)
-#define		HFA384x_RID_PROTOCOLRSPTIME	((UINT16)0xFD47)
-#define		HFA384x_RID_SHORTRETRYLIMIT	((UINT16)0xFD48)
-#define		HFA384x_RID_LONGRETRYLIMIT	((UINT16)0xFD49)
-#define		HFA384x_RID_MAXTXLIFETIME	((UINT16)0xFD4A)
-#define		HFA384x_RID_MAXRXLIFETIME	((UINT16)0xFD4B)
-#define		HFA384x_RID_CFPOLLABLE		((UINT16)0xFD4C)
-#define		HFA384x_RID_AUTHALGORITHMS	((UINT16)0xFD4D)
-#define		HFA384x_RID_PRIVACYOPTIMP	((UINT16)0xFD4F)
-#define		HFA384x_RID_DBMCOMMSQUALITY	((UINT16)0xFD51)
-#define		HFA384x_RID_CURRENTTXRATE1	((UINT16)0xFD80)
-#define		HFA384x_RID_CURRENTTXRATE2	((UINT16)0xFD81)
-#define		HFA384x_RID_CURRENTTXRATE3	((UINT16)0xFD82)
-#define		HFA384x_RID_CURRENTTXRATE4	((UINT16)0xFD83)
-#define		HFA384x_RID_CURRENTTXRATE5	((UINT16)0xFD84)
-#define		HFA384x_RID_CURRENTTXRATE6	((UINT16)0xFD85)
-#define		HFA384x_RID_OWNMACADDRESS	((UINT16)0xFD86)
-// #define	HFA384x_RID_PCFINFO		((UINT16)0xFD87)
-#define		HFA384x_RID_SCANRESULTS       	((UINT16)0xFD88) // NEW
-#define		HFA384x_RID_HOSTSCANRESULTS   	((UINT16)0xFD89) // NEW
-#define		HFA384x_RID_AUTHENTICATIONUSED	((UINT16)0xFD8A) // NEW
-#define		HFA384x_RID_ASSOCIATEFAILURE  	((UINT16)0xFD8D) // 1.8.0
+#define		HFA384x_RID_PORTSTATUS		((u16)0xFD40)
+#define		HFA384x_RID_CURRENTSSID		((u16)0xFD41)
+#define		HFA384x_RID_CURRENTBSSID	((u16)0xFD42)
+#define		HFA384x_RID_COMMSQUALITY	((u16)0xFD43)
+#define		HFA384x_RID_CURRENTTXRATE	((u16)0xFD44)
+#define		HFA384x_RID_CURRENTBCNint	((u16)0xFD45)
+#define		HFA384x_RID_CURRENTSCALETHRESH	((u16)0xFD46)
+#define		HFA384x_RID_PROTOCOLRSPTIME	((u16)0xFD47)
+#define		HFA384x_RID_SHORTRETRYLIMIT	((u16)0xFD48)
+#define		HFA384x_RID_LONGRETRYLIMIT	((u16)0xFD49)
+#define		HFA384x_RID_MAXTXLIFETIME	((u16)0xFD4A)
+#define		HFA384x_RID_MAXRXLIFETIME	((u16)0xFD4B)
+#define		HFA384x_RID_CFPOLLABLE		((u16)0xFD4C)
+#define		HFA384x_RID_AUTHALGORITHMS	((u16)0xFD4D)
+#define		HFA384x_RID_PRIVACYOPTIMP	((u16)0xFD4F)
+#define		HFA384x_RID_DBMCOMMSQUALITY	((u16)0xFD51)
+#define		HFA384x_RID_CURRENTTXRATE1	((u16)0xFD80)
+#define		HFA384x_RID_CURRENTTXRATE2	((u16)0xFD81)
+#define		HFA384x_RID_CURRENTTXRATE3	((u16)0xFD82)
+#define		HFA384x_RID_CURRENTTXRATE4	((u16)0xFD83)
+#define		HFA384x_RID_CURRENTTXRATE5	((u16)0xFD84)
+#define		HFA384x_RID_CURRENTTXRATE6	((u16)0xFD85)
+#define		HFA384x_RID_OWNMACADDRESS	((u16)0xFD86)
+// #define	HFA384x_RID_PCFINFO		((u16)0xFD87)
+#define		HFA384x_RID_SCANRESULTS       	((u16)0xFD88) // NEW
+#define		HFA384x_RID_HOSTSCANRESULTS   	((u16)0xFD89) // NEW
+#define		HFA384x_RID_AUTHENTICATIONUSED	((u16)0xFD8A) // NEW
+#define		HFA384x_RID_ASSOCIATEFAILURE  	((u16)0xFD8D) // 1.8.0
 
 /*--------------------------------------------------------------------
 Information RID Lengths:  MAC Information
   This is the length of JUST the DATA part of the RID (does not
   include the len or code fields)
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_PORTSTATUS_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTSSID_LEN		((UINT16)34)
-#define		HFA384x_RID_CURRENTBSSID_LEN		((UINT16)WLAN_BSSID_LEN)
-#define		HFA384x_RID_COMMSQUALITY_LEN		((UINT16)sizeof(hfa384x_commsquality_t))
-#define		HFA384x_RID_DBMCOMMSQUALITY_LEN		((UINT16)sizeof(hfa384x_dbmcommsquality_t))
-#define		HFA384x_RID_CURRENTTXRATE_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTBCNINT_LEN		((UINT16)0)
-#define		HFA384x_RID_STACURSCALETHRESH_LEN	((UINT16)12)
-#define		HFA384x_RID_APCURSCALETHRESH_LEN	((UINT16)6)
-#define		HFA384x_RID_PROTOCOLRSPTIME_LEN		((UINT16)0)
-#define		HFA384x_RID_SHORTRETRYLIMIT_LEN		((UINT16)0)
-#define		HFA384x_RID_LONGRETRYLIMIT_LEN		((UINT16)0)
-#define		HFA384x_RID_MAXTXLIFETIME_LEN		((UINT16)0)
-#define		HFA384x_RID_MAXRXLIFETIME_LEN		((UINT16)0)
-#define		HFA384x_RID_CFPOLLABLE_LEN		((UINT16)0)
-#define		HFA384x_RID_AUTHALGORITHMS_LEN		((UINT16)4)
-#define		HFA384x_RID_PRIVACYOPTIMP_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTTXRATE1_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTTXRATE2_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTTXRATE3_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTTXRATE4_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTTXRATE5_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTTXRATE6_LEN		((UINT16)0)
-#define		HFA384x_RID_OWNMACADDRESS_LEN		((UINT16)6)
-#define		HFA384x_RID_PCFINFO_LEN			((UINT16)6)
-#define		HFA384x_RID_CNFAPPCFINFO_LEN		((UINT16)sizeof(hfa384x_PCFInfo_data_t))
-#define		HFA384x_RID_SCANREQUEST_LEN		((UINT16)sizeof(hfa384x_ScanRequest_data_t))
-#define		HFA384x_RID_JOINREQUEST_LEN		((UINT16)sizeof(hfa384x_JoinRequest_data_t))
-#define		HFA384x_RID_AUTHENTICATESTA_LEN		((UINT16)sizeof(hfa384x_authenticateStation_data_t))
-#define		HFA384x_RID_CHANNELINFOREQUEST_LEN	((UINT16)sizeof(hfa384x_ChannelInfoRequest_data_t))
+#define		HFA384x_RID_PORTSTATUS_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTSSID_LEN		((u16)34)
+#define		HFA384x_RID_CURRENTBSSID_LEN		((u16)WLAN_BSSID_LEN)
+#define		HFA384x_RID_COMMSQUALITY_LEN		((u16)sizeof(hfa384x_commsquality_t))
+#define		HFA384x_RID_DBMCOMMSQUALITY_LEN		((u16)sizeof(hfa384x_dbmcommsquality_t))
+#define		HFA384x_RID_CURRENTTXRATE_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTBCNint_LEN		((u16)0)
+#define		HFA384x_RID_STACURSCALETHRESH_LEN	((u16)12)
+#define		HFA384x_RID_APCURSCALETHRESH_LEN	((u16)6)
+#define		HFA384x_RID_PROTOCOLRSPTIME_LEN		((u16)0)
+#define		HFA384x_RID_SHORTRETRYLIMIT_LEN		((u16)0)
+#define		HFA384x_RID_LONGRETRYLIMIT_LEN		((u16)0)
+#define		HFA384x_RID_MAXTXLIFETIME_LEN		((u16)0)
+#define		HFA384x_RID_MAXRXLIFETIME_LEN		((u16)0)
+#define		HFA384x_RID_CFPOLLABLE_LEN		((u16)0)
+#define		HFA384x_RID_AUTHALGORITHMS_LEN		((u16)4)
+#define		HFA384x_RID_PRIVACYOPTIMP_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTTXRATE1_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTTXRATE2_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTTXRATE3_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTTXRATE4_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTTXRATE5_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTTXRATE6_LEN		((u16)0)
+#define		HFA384x_RID_OWNMACADDRESS_LEN		((u16)6)
+#define		HFA384x_RID_PCFINFO_LEN			((u16)6)
+#define		HFA384x_RID_CNFAPPCFINFO_LEN		((u16)sizeof(hfa384x_PCFInfo_data_t))
+#define		HFA384x_RID_SCANREQUEST_LEN		((u16)sizeof(hfa384x_ScanRequest_data_t))
+#define		HFA384x_RID_JOINREQUEST_LEN		((u16)sizeof(hfa384x_JoinRequest_data_t))
+#define		HFA384x_RID_AUTHENTICATESTA_LEN		((u16)sizeof(hfa384x_authenticateStation_data_t))
+#define		HFA384x_RID_CHANNELINFOREQUEST_LEN	((u16)sizeof(hfa384x_ChannelInfoRequest_data_t))
 /*--------------------------------------------------------------------
 Information RIDs:  Modem Information
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_PHYTYPE		((UINT16)0xFDC0)
-#define		HFA384x_RID_CURRENTCHANNEL	((UINT16)0xFDC1)
-#define		HFA384x_RID_CURRENTPOWERSTATE	((UINT16)0xFDC2)
-#define		HFA384x_RID_CCAMODE		((UINT16)0xFDC3)
-#define		HFA384x_RID_SUPPORTEDDATARATES	((UINT16)0xFDC6)
-#define		HFA384x_RID_LFOSTATUS           ((UINT16)0xFDC7) // 1.7.1
+#define		HFA384x_RID_PHYTYPE		((u16)0xFDC0)
+#define		HFA384x_RID_CURRENTCHANNEL	((u16)0xFDC1)
+#define		HFA384x_RID_CURRENTPOWERSTATE	((u16)0xFDC2)
+#define		HFA384x_RID_CCAMODE		((u16)0xFDC3)
+#define		HFA384x_RID_SUPPORTEDDATARATES	((u16)0xFDC6)
+#define		HFA384x_RID_LFOSTATUS           ((u16)0xFDC7) // 1.7.1
 
 /*--------------------------------------------------------------------
 Information RID Lengths:  Modem Information
   This is the length of JUST the DATA part of the RID (does not
   include the len or code fields)
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_PHYTYPE_LEN			((UINT16)0)
-#define		HFA384x_RID_CURRENTCHANNEL_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTPOWERSTATE_LEN	((UINT16)0)
-#define		HFA384x_RID_CCAMODE_LEN			((UINT16)0)
-#define		HFA384x_RID_SUPPORTEDDATARATES_LEN	((UINT16)10)
+#define		HFA384x_RID_PHYTYPE_LEN			((u16)0)
+#define		HFA384x_RID_CURRENTCHANNEL_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTPOWERSTATE_LEN	((u16)0)
+#define		HFA384x_RID_CCAMODE_LEN			((u16)0)
+#define		HFA384x_RID_SUPPORTEDDATARATES_LEN	((u16)10)
 
 /*--------------------------------------------------------------------
 API ENHANCEMENTS (NOT ALREADY IMPLEMENTED)
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_CNFWEPDEFAULTKEYID	((UINT16)0xFC23)
-#define		HFA384x_RID_CNFWEPDEFAULTKEY0	((UINT16)0xFC24)
-#define		HFA384x_RID_CNFWEPDEFAULTKEY1	((UINT16)0xFC25)
-#define		HFA384x_RID_CNFWEPDEFAULTKEY2	((UINT16)0xFC26)
-#define		HFA384x_RID_CNFWEPDEFAULTKEY3	((UINT16)0xFC27)
-#define		HFA384x_RID_CNFWEPFLAGS		((UINT16)0xFC28)
-#define		HFA384x_RID_CNFWEPKEYMAPTABLE	((UINT16)0xFC29)
-#define		HFA384x_RID_CNFAUTHENTICATION	((UINT16)0xFC2A)
-#define		HFA384x_RID_CNFMAXASSOCSTATIONS	((UINT16)0xFC2B)
-#define		HFA384x_RID_CNFTXCONTROL	((UINT16)0xFC2C)
-#define		HFA384x_RID_CNFROAMINGMODE	((UINT16)0xFC2D)
-#define		HFA384x_RID_CNFHOSTAUTHASSOC	((UINT16)0xFC2E)
-#define		HFA384x_RID_CNFRCVCRCERROR	((UINT16)0xFC30)
-// #define		HFA384x_RID_CNFMMLIFE		((UINT16)0xFC31)
-#define		HFA384x_RID_CNFALTRETRYCNT	((UINT16)0xFC32)
-#define		HFA384x_RID_CNFAPBCNINT		((UINT16)0xFC33)
-#define		HFA384x_RID_CNFAPPCFINFO	((UINT16)0xFC34)
-#define		HFA384x_RID_CNFSTAPCFINFO	((UINT16)0xFC35)
-#define		HFA384x_RID_CNFPRIORITYQUSAGE	((UINT16)0xFC37)
-#define		HFA384x_RID_CNFTIMCTRL		((UINT16)0xFC40)
-#define		HFA384x_RID_CNFTHIRTY2TALLY	((UINT16)0xFC42)
-#define		HFA384x_RID_CNFENHSECURITY	((UINT16)0xFC43)
-#define		HFA384x_RID_CNFDBMADJUST  	((UINT16)0xFC46) // NEW
-#define		HFA384x_RID_CNFWPADATA       	((UINT16)0xFC48) // 1.7.0
-#define		HFA384x_RID_CNFPROPOGATIONDELAY	((UINT16)0xFC49) // 1.7.6
-#define		HFA384x_RID_CNFSHORTPREAMBLE	((UINT16)0xFCB0)
-#define		HFA384x_RID_CNFEXCLONGPREAMBLE	((UINT16)0xFCB1)
-#define		HFA384x_RID_CNFAUTHRSPTIMEOUT	((UINT16)0xFCB2)
-#define		HFA384x_RID_CNFBASICRATES	((UINT16)0xFCB3)
-#define		HFA384x_RID_CNFSUPPRATES	((UINT16)0xFCB4)
-#define		HFA384x_RID_CNFFALLBACKCTRL	((UINT16)0xFCB5) // NEW
-#define		HFA384x_RID_WEPKEYSTATUS   	((UINT16)0xFCB6) // NEW
-#define		HFA384x_RID_WEPKEYMAPINDEX 	((UINT16)0xFCB7) // NEW
-#define		HFA384x_RID_BROADCASTKEYID 	((UINT16)0xFCB8) // NEW
-#define		HFA384x_RID_ENTSECFLAGEYID 	((UINT16)0xFCB9) // NEW
-#define		HFA384x_RID_CNFPASSIVESCANCTRL	((UINT16)0xFCBA) // NEW STA
-#define		HFA384x_RID_CNFWPAHANDLING	((UINT16)0xFCBB) // 1.7.0
-#define		HFA384x_RID_MDCCONTROL        	((UINT16)0xFCBC) // 1.7.0/1.4.0
-#define		HFA384x_RID_MDCCOUNTRY        	((UINT16)0xFCBD) // 1.7.0/1.4.0
-#define		HFA384x_RID_TXPOWERMAX        	((UINT16)0xFCBE) // 1.7.0/1.4.0
-#define		HFA384x_RID_CNFLFOENBLED      	((UINT16)0xFCBF) // 1.6.3
-#define         HFA384x_RID_CAPINFO             ((UINT16)0xFCC0) // 1.7.0/1.3.7
-#define         HFA384x_RID_LISTENINTERVAL      ((UINT16)0xFCC1) // 1.7.0/1.3.7
-#define         HFA384x_RID_DIVERSITYENABLED    ((UINT16)0xFCC2) // 1.7.0/1.3.7
-#define         HFA384x_RID_LED_CONTROL         ((UINT16)0xFCC4) // 1.7.6
-#define         HFA384x_RID_HFO_DELAY           ((UINT16)0xFCC5) // 1.7.6
-#define         HFA384x_RID_DISSALOWEDBSSID     ((UINT16)0xFCC6) // 1.8.0
-#define		HFA384x_RID_SCANREQUEST		((UINT16)0xFCE1)
-#define		HFA384x_RID_JOINREQUEST		((UINT16)0xFCE2)
-#define		HFA384x_RID_AUTHENTICATESTA	((UINT16)0xFCE3)
-#define		HFA384x_RID_CHANNELINFOREQUEST	((UINT16)0xFCE4)
-#define		HFA384x_RID_HOSTSCAN          	((UINT16)0xFCE5) // NEW STA
-#define		HFA384x_RID_ASSOCIATESTA	((UINT16)0xFCE6)
-
-#define		HFA384x_RID_CNFWEPDEFAULTKEY_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFWEP128DEFAULTKEY_LEN	((UINT16)14)
-#define		HFA384x_RID_CNFPRIOQUSAGE_LEN		((UINT16)4)
+#define		HFA384x_RID_CNFWEPDEFAULTKEYID	((u16)0xFC23)
+#define		HFA384x_RID_CNFWEPDEFAULTKEY0	((u16)0xFC24)
+#define		HFA384x_RID_CNFWEPDEFAULTKEY1	((u16)0xFC25)
+#define		HFA384x_RID_CNFWEPDEFAULTKEY2	((u16)0xFC26)
+#define		HFA384x_RID_CNFWEPDEFAULTKEY3	((u16)0xFC27)
+#define		HFA384x_RID_CNFWEPFLAGS		((u16)0xFC28)
+#define		HFA384x_RID_CNFWEPKEYMAPTABLE	((u16)0xFC29)
+#define		HFA384x_RID_CNFAUTHENTICATION	((u16)0xFC2A)
+#define		HFA384x_RID_CNFMAXASSOCSTATIONS	((u16)0xFC2B)
+#define		HFA384x_RID_CNFTXCONTROL	((u16)0xFC2C)
+#define		HFA384x_RID_CNFROAMINGMODE	((u16)0xFC2D)
+#define		HFA384x_RID_CNFHOSTAUTHASSOC	((u16)0xFC2E)
+#define		HFA384x_RID_CNFRCVCRCERROR	((u16)0xFC30)
+// #define		HFA384x_RID_CNFMMLIFE		((u16)0xFC31)
+#define		HFA384x_RID_CNFALTRETRYCNT	((u16)0xFC32)
+#define		HFA384x_RID_CNFAPBCNint		((u16)0xFC33)
+#define		HFA384x_RID_CNFAPPCFINFO	((u16)0xFC34)
+#define		HFA384x_RID_CNFSTAPCFINFO	((u16)0xFC35)
+#define		HFA384x_RID_CNFPRIORITYQUSAGE	((u16)0xFC37)
+#define		HFA384x_RID_CNFTIMCTRL		((u16)0xFC40)
+#define		HFA384x_RID_CNFTHIRTY2TALLY	((u16)0xFC42)
+#define		HFA384x_RID_CNFENHSECURITY	((u16)0xFC43)
+#define		HFA384x_RID_CNFDBMADJUST  	((u16)0xFC46) // NEW
+#define		HFA384x_RID_CNFWPADATA       	((u16)0xFC48) // 1.7.0
+#define		HFA384x_RID_CNFPROPOGATIONDELAY	((u16)0xFC49) // 1.7.6
+#define		HFA384x_RID_CNFSHORTPREAMBLE	((u16)0xFCB0)
+#define		HFA384x_RID_CNFEXCLONGPREAMBLE	((u16)0xFCB1)
+#define		HFA384x_RID_CNFAUTHRSPTIMEOUT	((u16)0xFCB2)
+#define		HFA384x_RID_CNFBASICRATES	((u16)0xFCB3)
+#define		HFA384x_RID_CNFSUPPRATES	((u16)0xFCB4)
+#define		HFA384x_RID_CNFFALLBACKCTRL	((u16)0xFCB5) // NEW
+#define		HFA384x_RID_WEPKEYSTATUS   	((u16)0xFCB6) // NEW
+#define		HFA384x_RID_WEPKEYMAPINDEX 	((u16)0xFCB7) // NEW
+#define		HFA384x_RID_BROADCASTKEYID 	((u16)0xFCB8) // NEW
+#define		HFA384x_RID_ENTSECFLAGEYID 	((u16)0xFCB9) // NEW
+#define		HFA384x_RID_CNFPASSIVESCANCTRL	((u16)0xFCBA) // NEW STA
+#define		HFA384x_RID_CNFWPAHANDLING	((u16)0xFCBB) // 1.7.0
+#define		HFA384x_RID_MDCCONTROL        	((u16)0xFCBC) // 1.7.0/1.4.0
+#define		HFA384x_RID_MDCCOUNTRY        	((u16)0xFCBD) // 1.7.0/1.4.0
+#define		HFA384x_RID_TXPOWERMAX        	((u16)0xFCBE) // 1.7.0/1.4.0
+#define		HFA384x_RID_CNFLFOENBLED      	((u16)0xFCBF) // 1.6.3
+#define         HFA384x_RID_CAPINFO             ((u16)0xFCC0) // 1.7.0/1.3.7
+#define         HFA384x_RID_LISTENintERVAL      ((u16)0xFCC1) // 1.7.0/1.3.7
+#define         HFA384x_RID_DIVERSITYENABLED    ((u16)0xFCC2) // 1.7.0/1.3.7
+#define         HFA384x_RID_LED_CONTROL         ((u16)0xFCC4) // 1.7.6
+#define         HFA384x_RID_HFO_DELAY           ((u16)0xFCC5) // 1.7.6
+#define         HFA384x_RID_DISSALOWEDBSSID     ((u16)0xFCC6) // 1.8.0
+#define		HFA384x_RID_SCANREQUEST		((u16)0xFCE1)
+#define		HFA384x_RID_JOINREQUEST		((u16)0xFCE2)
+#define		HFA384x_RID_AUTHENTICATESTA	((u16)0xFCE3)
+#define		HFA384x_RID_CHANNELINFOREQUEST	((u16)0xFCE4)
+#define		HFA384x_RID_HOSTSCAN          	((u16)0xFCE5) // NEW STA
+#define		HFA384x_RID_ASSOCIATESTA	((u16)0xFCE6)
+
+#define		HFA384x_RID_CNFWEPDEFAULTKEY_LEN	((u16)6)
+#define		HFA384x_RID_CNFWEP128DEFAULTKEY_LEN	((u16)14)
+#define		HFA384x_RID_CNFPRIOQUSAGE_LEN		((u16)4)
 /*--------------------------------------------------------------------
 PD Record codes
 --------------------------------------------------------------------*/
-#define HFA384x_PDR_PCB_PARTNUM		((UINT16)0x0001)
-#define HFA384x_PDR_PDAVER		((UINT16)0x0002)
-#define HFA384x_PDR_NIC_SERIAL		((UINT16)0x0003)
-#define HFA384x_PDR_MKK_MEASUREMENTS	((UINT16)0x0004)
-#define HFA384x_PDR_NIC_RAMSIZE		((UINT16)0x0005)
-#define HFA384x_PDR_MFISUPRANGE		((UINT16)0x0006)
-#define HFA384x_PDR_CFISUPRANGE		((UINT16)0x0007)
-#define HFA384x_PDR_NICID		((UINT16)0x0008)
-//#define HFA384x_PDR_REFDAC_MEASUREMENTS	((UINT16)0x0010)
-//#define HFA384x_PDR_VGDAC_MEASUREMENTS	((UINT16)0x0020)
-//#define HFA384x_PDR_LEVEL_COMP_MEASUREMENTS	((UINT16)0x0030)
-//#define HFA384x_PDR_MODEM_TRIMDAC_MEASUREMENTS	((UINT16)0x0040)
-//#define HFA384x_PDR_COREGA_HACK		((UINT16)0x00ff)
-#define HFA384x_PDR_MAC_ADDRESS		((UINT16)0x0101)
-//#define HFA384x_PDR_MKK_CALLNAME	((UINT16)0x0102)
-#define HFA384x_PDR_REGDOMAIN		((UINT16)0x0103)
-#define HFA384x_PDR_ALLOWED_CHANNEL	((UINT16)0x0104)
-#define HFA384x_PDR_DEFAULT_CHANNEL	((UINT16)0x0105)
-//#define HFA384x_PDR_PRIVACY_OPTION	((UINT16)0x0106)
-#define HFA384x_PDR_TEMPTYPE		((UINT16)0x0107)
-//#define HFA384x_PDR_REFDAC_SETUP	((UINT16)0x0110)
-//#define HFA384x_PDR_VGDAC_SETUP		((UINT16)0x0120)
-//#define HFA384x_PDR_LEVEL_COMP_SETUP	((UINT16)0x0130)
-//#define HFA384x_PDR_TRIMDAC_SETUP	((UINT16)0x0140)
-#define HFA384x_PDR_IFR_SETTING		((UINT16)0x0200)
-#define HFA384x_PDR_RFR_SETTING		((UINT16)0x0201)
-#define HFA384x_PDR_HFA3861_BASELINE	((UINT16)0x0202)
-#define HFA384x_PDR_HFA3861_SHADOW	((UINT16)0x0203)
-#define HFA384x_PDR_HFA3861_IFRF	((UINT16)0x0204)
-#define HFA384x_PDR_HFA3861_CHCALSP	((UINT16)0x0300)
-#define HFA384x_PDR_HFA3861_CHCALI	((UINT16)0x0301)
-#define HFA384x_PDR_MAX_TX_POWER  	((UINT16)0x0302)
-#define HFA384x_PDR_MASTER_CHAN_LIST	((UINT16)0x0303)
-#define HFA384x_PDR_3842_NIC_CONFIG	((UINT16)0x0400)
-#define HFA384x_PDR_USB_ID		((UINT16)0x0401)
-#define HFA384x_PDR_PCI_ID		((UINT16)0x0402)
-#define HFA384x_PDR_PCI_IFCONF		((UINT16)0x0403)
-#define HFA384x_PDR_PCI_PMCONF		((UINT16)0x0404)
-#define HFA384x_PDR_RFENRGY		((UINT16)0x0406)
-#define HFA384x_PDR_USB_POWER_TYPE      ((UINT16)0x0407)
-//#define HFA384x_PDR_UNKNOWN408		((UINT16)0x0408)
-#define HFA384x_PDR_USB_MAX_POWER	((UINT16)0x0409)
-#define HFA384x_PDR_USB_MANUFACTURER	((UINT16)0x0410)
-#define HFA384x_PDR_USB_PRODUCT  	((UINT16)0x0411)
-#define HFA384x_PDR_ANT_DIVERSITY   	((UINT16)0x0412)
-#define HFA384x_PDR_HFO_DELAY       	((UINT16)0x0413)
-#define HFA384x_PDR_SCALE_THRESH 	((UINT16)0x0414)
-
-#define HFA384x_PDR_HFA3861_MANF_TESTSP	((UINT16)0x0900)
-#define HFA384x_PDR_HFA3861_MANF_TESTI	((UINT16)0x0901)
-#define HFA384x_PDR_END_OF_PDA		((UINT16)0x0000)
+#define HFA384x_PDR_PCB_PARTNUM		((u16)0x0001)
+#define HFA384x_PDR_PDAVER		((u16)0x0002)
+#define HFA384x_PDR_NIC_SERIAL		((u16)0x0003)
+#define HFA384x_PDR_MKK_MEASUREMENTS	((u16)0x0004)
+#define HFA384x_PDR_NIC_RAMSIZE		((u16)0x0005)
+#define HFA384x_PDR_MFISUPRANGE		((u16)0x0006)
+#define HFA384x_PDR_CFISUPRANGE		((u16)0x0007)
+#define HFA384x_PDR_NICID		((u16)0x0008)
+//#define HFA384x_PDR_REFDAC_MEASUREMENTS	((u16)0x0010)
+//#define HFA384x_PDR_VGDAC_MEASUREMENTS	((u16)0x0020)
+//#define HFA384x_PDR_LEVEL_COMP_MEASUREMENTS	((u16)0x0030)
+//#define HFA384x_PDR_MODEM_TRIMDAC_MEASUREMENTS	((u16)0x0040)
+//#define HFA384x_PDR_COREGA_HACK		((u16)0x00ff)
+#define HFA384x_PDR_MAC_ADDRESS		((u16)0x0101)
+//#define HFA384x_PDR_MKK_CALLNAME	((u16)0x0102)
+#define HFA384x_PDR_REGDOMAIN		((u16)0x0103)
+#define HFA384x_PDR_ALLOWED_CHANNEL	((u16)0x0104)
+#define HFA384x_PDR_DEFAULT_CHANNEL	((u16)0x0105)
+//#define HFA384x_PDR_PRIVACY_OPTION	((u16)0x0106)
+#define HFA384x_PDR_TEMPTYPE		((u16)0x0107)
+//#define HFA384x_PDR_REFDAC_SETUP	((u16)0x0110)
+//#define HFA384x_PDR_VGDAC_SETUP		((u16)0x0120)
+//#define HFA384x_PDR_LEVEL_COMP_SETUP	((u16)0x0130)
+//#define HFA384x_PDR_TRIMDAC_SETUP	((u16)0x0140)
+#define HFA384x_PDR_IFR_SETTING		((u16)0x0200)
+#define HFA384x_PDR_RFR_SETTING		((u16)0x0201)
+#define HFA384x_PDR_HFA3861_BASELINE	((u16)0x0202)
+#define HFA384x_PDR_HFA3861_SHADOW	((u16)0x0203)
+#define HFA384x_PDR_HFA3861_IFRF	((u16)0x0204)
+#define HFA384x_PDR_HFA3861_CHCALSP	((u16)0x0300)
+#define HFA384x_PDR_HFA3861_CHCALI	((u16)0x0301)
+#define HFA384x_PDR_MAX_TX_POWER  	((u16)0x0302)
+#define HFA384x_PDR_MASTER_CHAN_LIST	((u16)0x0303)
+#define HFA384x_PDR_3842_NIC_CONFIG	((u16)0x0400)
+#define HFA384x_PDR_USB_ID		((u16)0x0401)
+#define HFA384x_PDR_PCI_ID		((u16)0x0402)
+#define HFA384x_PDR_PCI_IFCONF		((u16)0x0403)
+#define HFA384x_PDR_PCI_PMCONF		((u16)0x0404)
+#define HFA384x_PDR_RFENRGY		((u16)0x0406)
+#define HFA384x_PDR_USB_POWER_TYPE      ((u16)0x0407)
+//#define HFA384x_PDR_UNKNOWN408		((u16)0x0408)
+#define HFA384x_PDR_USB_MAX_POWER	((u16)0x0409)
+#define HFA384x_PDR_USB_MANUFACTURER	((u16)0x0410)
+#define HFA384x_PDR_USB_PRODUCT  	((u16)0x0411)
+#define HFA384x_PDR_ANT_DIVERSITY   	((u16)0x0412)
+#define HFA384x_PDR_HFO_DELAY       	((u16)0x0413)
+#define HFA384x_PDR_SCALE_THRESH 	((u16)0x0414)
+
+#define HFA384x_PDR_HFA3861_MANF_TESTSP	((u16)0x0900)
+#define HFA384x_PDR_HFA3861_MANF_TESTI	((u16)0x0901)
+#define HFA384x_PDR_END_OF_PDA		((u16)0x0000)
 
 
 /*=============================================================*/
@@ -802,7 +766,7 @@ PD Record codes
 #define		HFA384x_OFFSET1		HFA384x_OFFSET1_OFF
 #define		HFA384x_DATA1		HFA384x_DATA1_OFF
 #define		HFA384x_EVSTAT		HFA384x_EVSTAT_OFF
-#define		HFA384x_INTEN		HFA384x_INTEN_OFF
+#define		HFA384x_intEN		HFA384x_INTEN_OFF
 #define		HFA384x_EVACK		HFA384x_EVACK_OFF
 #define		HFA384x_CONTROL		HFA384x_CONTROL_OFF
 #define		HFA384x_SWSUPPORT0	HFA384x_SWSUPPORT0_OFF
@@ -817,96 +781,96 @@ PD Record codes
 
 /*--- Register Test/Get/Set Field macros ------------------------*/
 
-#define		HFA384x_CMD_ISBUSY(value)		((UINT16)(((UINT16)value) & HFA384x_CMD_BUSY))
-#define		HFA384x_CMD_AINFO_GET(value)		((UINT16)(((UINT16)(value) & HFA384x_CMD_AINFO) >> 8))
-#define		HFA384x_CMD_AINFO_SET(value)		((UINT16)((UINT16)(value) << 8))
-#define		HFA384x_CMD_MACPORT_GET(value)		((UINT16)(HFA384x_CMD_AINFO_GET((UINT16)(value) & HFA384x_CMD_MACPORT)))
-#define		HFA384x_CMD_MACPORT_SET(value)		((UINT16)HFA384x_CMD_AINFO_SET(value))
-#define		HFA384x_CMD_ISRECL(value)		((UINT16)(HFA384x_CMD_AINFO_GET((UINT16)(value) & HFA384x_CMD_RECL)))
-#define		HFA384x_CMD_RECL_SET(value)		((UINT16)HFA384x_CMD_AINFO_SET(value))
-#define		HFA384x_CMD_QOS_GET(value)		((UINT16)((((UINT16)(value))&((UINT16)0x3000)) >> 12))
-#define		HFA384x_CMD_QOS_SET(value)		((UINT16)((((UINT16)(value)) << 12) & 0x3000))
-#define		HFA384x_CMD_ISWRITE(value)		((UINT16)(HFA384x_CMD_AINFO_GET((UINT16)(value) & HFA384x_CMD_WRITE)))
-#define		HFA384x_CMD_WRITE_SET(value)		((UINT16)HFA384x_CMD_AINFO_SET((UINT16)value))
-#define		HFA384x_CMD_PROGMODE_GET(value)		((UINT16)(HFA384x_CMD_AINFO_GET((UINT16)(value) & HFA384x_CMD_PROGMODE)))
-#define		HFA384x_CMD_PROGMODE_SET(value)		((UINT16)HFA384x_CMD_AINFO_SET((UINT16)value))
-#define		HFA384x_CMD_CMDCODE_GET(value)		((UINT16)(((UINT16)(value)) & HFA384x_CMD_CMDCODE))
-#define		HFA384x_CMD_CMDCODE_SET(value)		((UINT16)(value))
-
-#define		HFA384x_STATUS_RESULT_GET(value)	((UINT16)((((UINT16)(value)) & HFA384x_STATUS_RESULT) >> 8))
-#define		HFA384x_STATUS_RESULT_SET(value)	(((UINT16)(value)) << 8)
-#define		HFA384x_STATUS_CMDCODE_GET(value)	(((UINT16)(value)) & HFA384x_STATUS_CMDCODE)
-#define		HFA384x_STATUS_CMDCODE_SET(value)	((UINT16)(value))
-
-#define		HFA384x_OFFSET_ISBUSY(value)		((UINT16)(((UINT16)(value)) & HFA384x_OFFSET_BUSY))
-#define		HFA384x_OFFSET_ISERR(value)		((UINT16)(((UINT16)(value)) & HFA384x_OFFSET_ERR))
-#define		HFA384x_OFFSET_DATAOFF_GET(value)	((UINT16)(((UINT16)(value)) & HFA384x_OFFSET_DATAOFF))
-#define		HFA384x_OFFSET_DATAOFF_SET(value)	((UINT16)(value))
-
-#define		HFA384x_EVSTAT_ISTICK(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_TICK))
-#define		HFA384x_EVSTAT_ISWTERR(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_WTERR))
-#define		HFA384x_EVSTAT_ISINFDROP(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_INFDROP))
-#define		HFA384x_EVSTAT_ISINFO(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_INFO))
-#define		HFA384x_EVSTAT_ISDTIM(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_DTIM))
-#define		HFA384x_EVSTAT_ISCMD(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_CMD))
-#define		HFA384x_EVSTAT_ISALLOC(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_ALLOC))
-#define		HFA384x_EVSTAT_ISTXEXC(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_TXEXC))
-#define		HFA384x_EVSTAT_ISTX(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_TX))
-#define		HFA384x_EVSTAT_ISRX(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_RX))
-
-#define		HFA384x_EVSTAT_ISBAP_OP(value)		((UINT16)(((UINT16)(value)) & HFA384x_INT_BAP_OP))
-
-#define		HFA384x_INTEN_ISTICK(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_TICK))
-#define		HFA384x_INTEN_TICK_SET(value)		((UINT16)(((UINT16)(value)) << 15))
-#define		HFA384x_INTEN_ISWTERR(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_WTERR))
-#define		HFA384x_INTEN_WTERR_SET(value)		((UINT16)(((UINT16)(value)) << 14))
-#define		HFA384x_INTEN_ISINFDROP(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_INFDROP))
-#define		HFA384x_INTEN_INFDROP_SET(value)	((UINT16)(((UINT16)(value)) << 13))
-#define		HFA384x_INTEN_ISINFO(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_INFO))
-#define		HFA384x_INTEN_INFO_SET(value)		((UINT16)(((UINT16)(value)) << 7))
-#define		HFA384x_INTEN_ISDTIM(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_DTIM))
-#define		HFA384x_INTEN_DTIM_SET(value)		((UINT16)(((UINT16)(value)) << 5))
-#define		HFA384x_INTEN_ISCMD(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_CMD))
-#define		HFA384x_INTEN_CMD_SET(value)		((UINT16)(((UINT16)(value)) << 4))
-#define		HFA384x_INTEN_ISALLOC(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_ALLOC))
-#define		HFA384x_INTEN_ALLOC_SET(value)		((UINT16)(((UINT16)(value)) << 3))
-#define		HFA384x_INTEN_ISTXEXC(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_TXEXC))
-#define		HFA384x_INTEN_TXEXC_SET(value)		((UINT16)(((UINT16)(value)) << 2))
-#define		HFA384x_INTEN_ISTX(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_TX))
-#define		HFA384x_INTEN_TX_SET(value)		((UINT16)(((UINT16)(value)) << 1))
-#define		HFA384x_INTEN_ISRX(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_RX))
-#define		HFA384x_INTEN_RX_SET(value)		((UINT16)(((UINT16)(value)) << 0))
-
-#define		HFA384x_EVACK_ISTICK(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_TICK))
-#define		HFA384x_EVACK_TICK_SET(value)		((UINT16)(((UINT16)(value)) << 15))
-#define		HFA384x_EVACK_ISWTERR(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_WTERR))
-#define		HFA384x_EVACK_WTERR_SET(value)		((UINT16)(((UINT16)(value)) << 14))
-#define		HFA384x_EVACK_ISINFDROP(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_INFDROP))
-#define		HFA384x_EVACK_INFDROP_SET(value)	((UINT16)(((UINT16)(value)) << 13))
-#define		HFA384x_EVACK_ISINFO(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_INFO))
-#define		HFA384x_EVACK_INFO_SET(value)		((UINT16)(((UINT16)(value)) << 7))
-#define		HFA384x_EVACK_ISDTIM(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_DTIM))
-#define		HFA384x_EVACK_DTIM_SET(value)		((UINT16)(((UINT16)(value)) << 5))
-#define		HFA384x_EVACK_ISCMD(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_CMD))
-#define		HFA384x_EVACK_CMD_SET(value)		((UINT16)(((UINT16)(value)) << 4))
-#define		HFA384x_EVACK_ISALLOC(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_ALLOC))
-#define		HFA384x_EVACK_ALLOC_SET(value)		((UINT16)(((UINT16)(value)) << 3))
-#define		HFA384x_EVACK_ISTXEXC(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_TXEXC))
-#define		HFA384x_EVACK_TXEXC_SET(value)		((UINT16)(((UINT16)(value)) << 2))
-#define		HFA384x_EVACK_ISTX(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_TX))
-#define		HFA384x_EVACK_TX_SET(value)		((UINT16)(((UINT16)(value)) << 1))
-#define		HFA384x_EVACK_ISRX(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_RX))
-#define		HFA384x_EVACK_RX_SET(value)		((UINT16)(((UINT16)(value)) << 0))
-
-#define		HFA384x_CONTROL_AUXEN_SET(value)	((UINT16)(((UINT16)(value)) << 14))
-#define		HFA384x_CONTROL_AUXEN_GET(value)	((UINT16)(((UINT16)(value)) >> 14))
+#define		HFA384x_CMD_ISBUSY(value)		((u16)(((u16)value) & HFA384x_CMD_BUSY))
+#define		HFA384x_CMD_AINFO_GET(value)		((u16)(((u16)(value) & HFA384x_CMD_AINFO) >> 8))
+#define		HFA384x_CMD_AINFO_SET(value)		((u16)((u16)(value) << 8))
+#define		HFA384x_CMD_MACPORT_GET(value)		((u16)(HFA384x_CMD_AINFO_GET((u16)(value) & HFA384x_CMD_MACPORT)))
+#define		HFA384x_CMD_MACPORT_SET(value)		((u16)HFA384x_CMD_AINFO_SET(value))
+#define		HFA384x_CMD_ISRECL(value)		((u16)(HFA384x_CMD_AINFO_GET((u16)(value) & HFA384x_CMD_RECL)))
+#define		HFA384x_CMD_RECL_SET(value)		((u16)HFA384x_CMD_AINFO_SET(value))
+#define		HFA384x_CMD_QOS_GET(value)		((u16)((((u16)(value))&((u16)0x3000)) >> 12))
+#define		HFA384x_CMD_QOS_SET(value)		((u16)((((u16)(value)) << 12) & 0x3000))
+#define		HFA384x_CMD_ISWRITE(value)		((u16)(HFA384x_CMD_AINFO_GET((u16)(value) & HFA384x_CMD_WRITE)))
+#define		HFA384x_CMD_WRITE_SET(value)		((u16)HFA384x_CMD_AINFO_SET((u16)value))
+#define		HFA384x_CMD_PROGMODE_GET(value)		((u16)(HFA384x_CMD_AINFO_GET((u16)(value) & HFA384x_CMD_PROGMODE)))
+#define		HFA384x_CMD_PROGMODE_SET(value)		((u16)HFA384x_CMD_AINFO_SET((u16)value))
+#define		HFA384x_CMD_CMDCODE_GET(value)		((u16)(((u16)(value)) & HFA384x_CMD_CMDCODE))
+#define		HFA384x_CMD_CMDCODE_SET(value)		((u16)(value))
+
+#define		HFA384x_STATUS_RESULT_GET(value)	((u16)((((u16)(value)) & HFA384x_STATUS_RESULT) >> 8))
+#define		HFA384x_STATUS_RESULT_SET(value)	(((u16)(value)) << 8)
+#define		HFA384x_STATUS_CMDCODE_GET(value)	(((u16)(value)) & HFA384x_STATUS_CMDCODE)
+#define		HFA384x_STATUS_CMDCODE_SET(value)	((u16)(value))
+
+#define		HFA384x_OFFSET_ISBUSY(value)		((u16)(((u16)(value)) & HFA384x_OFFSET_BUSY))
+#define		HFA384x_OFFSET_ISERR(value)		((u16)(((u16)(value)) & HFA384x_OFFSET_ERR))
+#define		HFA384x_OFFSET_DATAOFF_GET(value)	((u16)(((u16)(value)) & HFA384x_OFFSET_DATAOFF))
+#define		HFA384x_OFFSET_DATAOFF_SET(value)	((u16)(value))
+
+#define		HFA384x_EVSTAT_ISTICK(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_TICK))
+#define		HFA384x_EVSTAT_ISWTERR(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_WTERR))
+#define		HFA384x_EVSTAT_ISINFDROP(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_INFDROP))
+#define		HFA384x_EVSTAT_ISINFO(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_INFO))
+#define		HFA384x_EVSTAT_ISDTIM(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_DTIM))
+#define		HFA384x_EVSTAT_ISCMD(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_CMD))
+#define		HFA384x_EVSTAT_ISALLOC(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_ALLOC))
+#define		HFA384x_EVSTAT_ISTXEXC(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_TXEXC))
+#define		HFA384x_EVSTAT_ISTX(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_TX))
+#define		HFA384x_EVSTAT_ISRX(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_RX))
+
+#define		HFA384x_EVSTAT_ISBAP_OP(value)		((u16)(((u16)(value)) & HFA384x_int_BAP_OP))
+
+#define		HFA384x_intEN_ISTICK(value)		((u16)(((u16)(value)) & HFA384x_INTEN_TICK))
+#define		HFA384x_intEN_TICK_SET(value)		((u16)(((u16)(value)) << 15))
+#define		HFA384x_intEN_ISWTERR(value)		((u16)(((u16)(value)) & HFA384x_INTEN_WTERR))
+#define		HFA384x_intEN_WTERR_SET(value)		((u16)(((u16)(value)) << 14))
+#define		HFA384x_intEN_ISINFDROP(value)		((u16)(((u16)(value)) & HFA384x_INTEN_INFDROP))
+#define		HFA384x_intEN_INFDROP_SET(value)	((u16)(((u16)(value)) << 13))
+#define		HFA384x_intEN_ISINFO(value)		((u16)(((u16)(value)) & HFA384x_INTEN_INFO))
+#define		HFA384x_intEN_INFO_SET(value)		((u16)(((u16)(value)) << 7))
+#define		HFA384x_intEN_ISDTIM(value)		((u16)(((u16)(value)) & HFA384x_INTEN_DTIM))
+#define		HFA384x_intEN_DTIM_SET(value)		((u16)(((u16)(value)) << 5))
+#define		HFA384x_intEN_ISCMD(value)		((u16)(((u16)(value)) & HFA384x_INTEN_CMD))
+#define		HFA384x_intEN_CMD_SET(value)		((u16)(((u16)(value)) << 4))
+#define		HFA384x_intEN_ISALLOC(value)		((u16)(((u16)(value)) & HFA384x_INTEN_ALLOC))
+#define		HFA384x_intEN_ALLOC_SET(value)		((u16)(((u16)(value)) << 3))
+#define		HFA384x_intEN_ISTXEXC(value)		((u16)(((u16)(value)) & HFA384x_INTEN_TXEXC))
+#define		HFA384x_intEN_TXEXC_SET(value)		((u16)(((u16)(value)) << 2))
+#define		HFA384x_intEN_ISTX(value)		((u16)(((u16)(value)) & HFA384x_INTEN_TX))
+#define		HFA384x_intEN_TX_SET(value)		((u16)(((u16)(value)) << 1))
+#define		HFA384x_intEN_ISRX(value)		((u16)(((u16)(value)) & HFA384x_INTEN_RX))
+#define		HFA384x_intEN_RX_SET(value)		((u16)(((u16)(value)) << 0))
+
+#define		HFA384x_EVACK_ISTICK(value)		((u16)(((u16)(value)) & HFA384x_EVACK_TICK))
+#define		HFA384x_EVACK_TICK_SET(value)		((u16)(((u16)(value)) << 15))
+#define		HFA384x_EVACK_ISWTERR(value)		((u16)(((u16)(value)) & HFA384x_EVACK_WTERR))
+#define		HFA384x_EVACK_WTERR_SET(value)		((u16)(((u16)(value)) << 14))
+#define		HFA384x_EVACK_ISINFDROP(value)		((u16)(((u16)(value)) & HFA384x_EVACK_INFDROP))
+#define		HFA384x_EVACK_INFDROP_SET(value)	((u16)(((u16)(value)) << 13))
+#define		HFA384x_EVACK_ISINFO(value)		((u16)(((u16)(value)) & HFA384x_EVACK_INFO))
+#define		HFA384x_EVACK_INFO_SET(value)		((u16)(((u16)(value)) << 7))
+#define		HFA384x_EVACK_ISDTIM(value)		((u16)(((u16)(value)) & HFA384x_EVACK_DTIM))
+#define		HFA384x_EVACK_DTIM_SET(value)		((u16)(((u16)(value)) << 5))
+#define		HFA384x_EVACK_ISCMD(value)		((u16)(((u16)(value)) & HFA384x_EVACK_CMD))
+#define		HFA384x_EVACK_CMD_SET(value)		((u16)(((u16)(value)) << 4))
+#define		HFA384x_EVACK_ISALLOC(value)		((u16)(((u16)(value)) & HFA384x_EVACK_ALLOC))
+#define		HFA384x_EVACK_ALLOC_SET(value)		((u16)(((u16)(value)) << 3))
+#define		HFA384x_EVACK_ISTXEXC(value)		((u16)(((u16)(value)) & HFA384x_EVACK_TXEXC))
+#define		HFA384x_EVACK_TXEXC_SET(value)		((u16)(((u16)(value)) << 2))
+#define		HFA384x_EVACK_ISTX(value)		((u16)(((u16)(value)) & HFA384x_EVACK_TX))
+#define		HFA384x_EVACK_TX_SET(value)		((u16)(((u16)(value)) << 1))
+#define		HFA384x_EVACK_ISRX(value)		((u16)(((u16)(value)) & HFA384x_EVACK_RX))
+#define		HFA384x_EVACK_RX_SET(value)		((u16)(((u16)(value)) << 0))
+
+#define		HFA384x_CONTROL_AUXEN_SET(value)	((u16)(((u16)(value)) << 14))
+#define		HFA384x_CONTROL_AUXEN_GET(value)	((u16)(((u16)(value)) >> 14))
 
 /* Byte Order */
 #ifdef __KERNEL__
-#define hfa384x2host_16(n)	(__le16_to_cpu((UINT16)(n)))
-#define hfa384x2host_32(n)	(__le32_to_cpu((UINT32)(n)))
-#define host2hfa384x_16(n)	(__cpu_to_le16((UINT16)(n)))
-#define host2hfa384x_32(n)	(__cpu_to_le32((UINT32)(n)))
+#define hfa384x2host_16(n)	(__le16_to_cpu((u16)(n)))
+#define hfa384x2host_32(n)	(__le32_to_cpu((u32)(n)))
+#define host2hfa384x_16(n)	(__cpu_to_le16((u16)(n)))
+#define host2hfa384x_32(n)	(__cpu_to_le32((u32)(n)))
 #endif
 
 /* Host Maintained State Info */
@@ -927,14 +891,14 @@ PD Record codes
 /* Commonly used basic types */
 typedef struct hfa384x_bytestr
 {
-	UINT16	len;
-	UINT8	data[0];
+	u16	len;
+	u8	data[0];
 } __WLAN_ATTRIB_PACK__ hfa384x_bytestr_t;
 
 typedef struct hfa384x_bytestr32
 {
-	UINT16	len;
-	UINT8	data[32];
+	u16	len;
+	u8	data[32];
 } __WLAN_ATTRIB_PACK__ hfa384x_bytestr32_t;
 
 /*--------------------------------------------------------------------
@@ -946,112 +910,112 @@ these members */
 
 typedef struct hfa384x_record
 {
-	UINT16	reclen;
-	UINT16	rid;
+	u16	reclen;
+	u16	rid;
 } __WLAN_ATTRIB_PACK__ hfa384x_rec_t;
 
 typedef struct hfa384x_record16
 {
-	UINT16	reclen;
-	UINT16	rid;
-	UINT16	val;
+	u16	reclen;
+	u16	rid;
+	u16	val;
 } __WLAN_ATTRIB_PACK__ hfa384x_rec16_t;
 
 typedef struct hfa384x_record32
 {
-	UINT16	reclen;
-	UINT16	rid;
-	UINT32	val;
+	u16	reclen;
+	u16	rid;
+	u32	val;
 } __WLAN_ATTRIB_PACK__ hfa384x_rec32;
 
 /*-- Hardware/Firmware Component Information ----------*/
 typedef struct hfa384x_compident
 {
-	UINT16	id;
-	UINT16	variant;
-	UINT16	major;
-	UINT16	minor;
+	u16	id;
+	u16	variant;
+	u16	major;
+	u16	minor;
 } __WLAN_ATTRIB_PACK__ hfa384x_compident_t;
 
 typedef struct hfa384x_caplevel
 {
-	UINT16	role;
-	UINT16	id;
-	UINT16	variant;
-	UINT16	bottom;
-	UINT16	top;
+	u16	role;
+	u16	id;
+	u16	variant;
+	u16	bottom;
+	u16	top;
 } __WLAN_ATTRIB_PACK__ hfa384x_caplevel_t;
 
 /*-- Configuration Record: cnfPortType --*/
 typedef struct hfa384x_cnfPortType
 {
-	UINT16	cnfPortType;
+	u16	cnfPortType;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfPortType_t;
 
 /*-- Configuration Record: cnfOwnMACAddress --*/
 typedef struct hfa384x_cnfOwnMACAddress
 {
-	UINT8	cnfOwnMACAddress[6];
+	u8	cnfOwnMACAddress[6];
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfOwnMACAddress_t;
 
 /*-- Configuration Record: cnfDesiredSSID --*/
 typedef struct hfa384x_cnfDesiredSSID
 {
-	UINT8	cnfDesiredSSID[34];
+	u8	cnfDesiredSSID[34];
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfDesiredSSID_t;
 
 /*-- Configuration Record: cnfOwnChannel --*/
 typedef struct hfa384x_cnfOwnChannel
 {
-	UINT16	cnfOwnChannel;
+	u16	cnfOwnChannel;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfOwnChannel_t;
 
 /*-- Configuration Record: cnfOwnSSID --*/
 typedef struct hfa384x_cnfOwnSSID
 {
-	UINT8	cnfOwnSSID[34];
+	u8	cnfOwnSSID[34];
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfOwnSSID_t;
 
 /*-- Configuration Record: cnfOwnATIMWindow --*/
 typedef struct hfa384x_cnfOwnATIMWindow
 {
-	UINT16	cnfOwnATIMWindow;
+	u16	cnfOwnATIMWindow;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfOwnATIMWindow_t;
 
 /*-- Configuration Record: cnfSystemScale --*/
 typedef struct hfa384x_cnfSystemScale
 {
-	UINT16	cnfSystemScale;
+	u16	cnfSystemScale;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfSystemScale_t;
 
 /*-- Configuration Record: cnfMaxDataLength --*/
 typedef struct hfa384x_cnfMaxDataLength
 {
-	UINT16	cnfMaxDataLength;
+	u16	cnfMaxDataLength;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfMaxDataLength_t;
 
 /*-- Configuration Record: cnfWDSAddress --*/
 typedef struct hfa384x_cnfWDSAddress
 {
-	UINT8	cnfWDSAddress[6];
+	u8	cnfWDSAddress[6];
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfWDSAddress_t;
 
 /*-- Configuration Record: cnfPMEnabled --*/
 typedef struct hfa384x_cnfPMEnabled
 {
-	UINT16	cnfPMEnabled;
+	u16	cnfPMEnabled;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfPMEnabled_t;
 
 /*-- Configuration Record: cnfPMEPS --*/
 typedef struct hfa384x_cnfPMEPS
 {
-	UINT16	cnfPMEPS;
+	u16	cnfPMEPS;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfPMEPS_t;
 
 /*-- Configuration Record: cnfMulticastReceive --*/
 typedef struct hfa384x_cnfMulticastReceive
 {
-	UINT16	cnfMulticastReceive;
+	u16	cnfMulticastReceive;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfMulticastReceive_t;
 
 /*-- Configuration Record: cnfAuthentication --*/
@@ -1062,37 +1026,37 @@ typedef struct hfa384x_cnfMulticastReceive
 /*-- Configuration Record: cnfMaxSleepDuration --*/
 typedef struct hfa384x_cnfMaxSleepDuration
 {
-	UINT16	cnfMaxSleepDuration;
+	u16	cnfMaxSleepDuration;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfMaxSleepDuration_t;
 
 /*-- Configuration Record: cnfPMHoldoverDuration --*/
 typedef struct hfa384x_cnfPMHoldoverDuration
 {
-	UINT16	cnfPMHoldoverDuration;
+	u16	cnfPMHoldoverDuration;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfPMHoldoverDuration_t;
 
 /*-- Configuration Record: cnfOwnName --*/
 typedef struct hfa384x_cnfOwnName
 {
-	UINT8	cnfOwnName[34];
+	u8	cnfOwnName[34];
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfOwnName_t;
 
 /*-- Configuration Record: cnfOwnDTIMPeriod --*/
 typedef struct hfa384x_cnfOwnDTIMPeriod
 {
-	UINT16	cnfOwnDTIMPeriod;
+	u16	cnfOwnDTIMPeriod;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfOwnDTIMPeriod_t;
 
 /*-- Configuration Record: cnfWDSAddress --*/
 typedef struct hfa384x_cnfWDSAddressN
 {
-	UINT8	cnfWDSAddress[6];
+	u8	cnfWDSAddress[6];
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfWDSAddressN_t;
 
 /*-- Configuration Record: cnfMulticastPMBuffering --*/
 typedef struct hfa384x_cnfMulticastPMBuffering
 {
-	UINT16	cnfMulticastPMBuffering;
+	u16	cnfMulticastPMBuffering;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfMulticastPMBuffering_t;
 
 /*--------------------------------------------------------------------
@@ -1103,13 +1067,13 @@ Configuration Record Structures:
 /*-- Configuration Record: GroupAddresses --*/
 typedef struct hfa384x_GroupAddresses
 {
-	UINT8	MACAddress[16][6];
+	u8	MACAddress[16][6];
 } __WLAN_ATTRIB_PACK__ hfa384x_GroupAddresses_t;
 
 /*-- Configuration Record: CreateIBSS --*/
 typedef struct hfa384x_CreateIBSS
 {
-	UINT16	CreateIBSS;
+	u16	CreateIBSS;
 } __WLAN_ATTRIB_PACK__ hfa384x_CreateIBSS_t;
 
 #define HFA384x_CREATEIBSS_JOINCREATEIBSS          0
@@ -1120,87 +1084,87 @@ typedef struct hfa384x_CreateIBSS
 /*-- Configuration Record: FragmentationThreshold --*/
 typedef struct hfa384x_FragmentationThreshold
 {
-	UINT16	FragmentationThreshold;
+	u16	FragmentationThreshold;
 } __WLAN_ATTRIB_PACK__ hfa384x_FragmentationThreshold_t;
 
 /*-- Configuration Record: RTSThreshold --*/
 typedef struct hfa384x_RTSThreshold
 {
-	UINT16	RTSThreshold;
+	u16	RTSThreshold;
 } __WLAN_ATTRIB_PACK__ hfa384x_RTSThreshold_t;
 
 /*-- Configuration Record: TxRateControl --*/
 typedef struct hfa384x_TxRateControl
 {
-	UINT16	TxRateControl;
+	u16	TxRateControl;
 } __WLAN_ATTRIB_PACK__ hfa384x_TxRateControl_t;
 
 /*-- Configuration Record: PromiscuousMode --*/
 typedef struct hfa384x_PromiscuousMode
 {
-	UINT16	PromiscuousMode;
+	u16	PromiscuousMode;
 } __WLAN_ATTRIB_PACK__ hfa384x_PromiscuousMode_t;
 
 /*-- Configuration Record: ScanRequest (data portion only) --*/
 typedef struct hfa384x_ScanRequest_data
 {
-	UINT16	channelList;
-	UINT16	txRate;
+	u16	channelList;
+	u16	txRate;
 } __WLAN_ATTRIB_PACK__ hfa384x_ScanRequest_data_t;
 
 /*-- Configuration Record: HostScanRequest (data portion only) --*/
 typedef struct hfa384x_HostScanRequest_data
 {
-	UINT16	channelList;
-	UINT16	txRate;
+	u16	channelList;
+	u16	txRate;
 	hfa384x_bytestr32_t ssid;
 } __WLAN_ATTRIB_PACK__ hfa384x_HostScanRequest_data_t;
 
 /*-- Configuration Record: JoinRequest (data portion only) --*/
 typedef struct hfa384x_JoinRequest_data
 {
-	UINT8	bssid[WLAN_BSSID_LEN];
-	UINT16	channel;
+	u8	bssid[WLAN_BSSID_LEN];
+	u16	channel;
 } __WLAN_ATTRIB_PACK__ hfa384x_JoinRequest_data_t;
 
 /*-- Configuration Record: authenticateStation (data portion only) --*/
 typedef struct hfa384x_authenticateStation_data
 {
-	UINT8	address[WLAN_ADDR_LEN];
-	UINT16	status;
-	UINT16	algorithm;
+	u8	address[WLAN_ADDR_LEN];
+	u16	status;
+	u16	algorithm;
 } __WLAN_ATTRIB_PACK__ hfa384x_authenticateStation_data_t;
 
 /*-- Configuration Record: associateStation (data portion only) --*/
 typedef struct hfa384x_associateStation_data
 {
-	UINT8	address[WLAN_ADDR_LEN];
-	UINT16	status;
-	UINT16	type;
+	u8	address[WLAN_ADDR_LEN];
+	u16	status;
+	u16	type;
 } __WLAN_ATTRIB_PACK__ hfa384x_associateStation_data_t;
 
 /*-- Configuration Record: ChannelInfoRequest (data portion only) --*/
 typedef struct hfa384x_ChannelInfoRequest_data
 {
-	UINT16	channelList;
-	UINT16	channelDwellTime;
+	u16	channelList;
+	u16	channelDwellTime;
 } __WLAN_ATTRIB_PACK__ hfa384x_ChannelInfoRequest_data_t;
 
 /*-- Configuration Record: WEPKeyMapping (data portion only) --*/
 typedef struct hfa384x_WEPKeyMapping
 {
-	UINT8	address[WLAN_ADDR_LEN];
-	UINT16	key_index;
-	UINT8 	key[16];
-	UINT8 	mic_transmit_key[4];
-	UINT8 	mic_receive_key[4];
+	u8	address[WLAN_ADDR_LEN];
+	u16	key_index;
+	u8 	key[16];
+	u8 	mic_transmit_key[4];
+	u8 	mic_receive_key[4];
 } __WLAN_ATTRIB_PACK__ hfa384x_WEPKeyMapping_t;
 
 /*-- Configuration Record: WPAData       (data portion only) --*/
 typedef struct hfa384x_WPAData
 {
-	UINT16	datalen;
-        UINT8 	data[0]; // max 80
+	u16	datalen;
+        u8 	data[0]; // max 80
 } __WLAN_ATTRIB_PACK__ hfa384x_WPAData_t;
 
 /*--------------------------------------------------------------------
@@ -1210,7 +1174,7 @@ Configuration Record Structures: Behavior Parameters
 /*-- Configuration Record: TickTime --*/
 typedef struct hfa384x_TickTime
 {
-	UINT16	TickTime;
+	u16	TickTime;
 } __WLAN_ATTRIB_PACK__ hfa384x_TickTime_t;
 
 /*--------------------------------------------------------------------
@@ -1220,146 +1184,146 @@ Information Record Structures: NIC Information
 /*-- Information Record: MaxLoadTime --*/
 typedef struct hfa384x_MaxLoadTime
 {
-	UINT16	MaxLoadTime;
+	u16	MaxLoadTime;
 } __WLAN_ATTRIB_PACK__ hfa384x_MaxLoadTime_t;
 
 /*-- Information Record: DownLoadBuffer --*/
 /* NOTE: The page and offset are in AUX format */
 typedef struct hfa384x_downloadbuffer
 {
-	UINT16	page;
-	UINT16	offset;
-	UINT16	len;
+	u16	page;
+	u16	offset;
+	u16	len;
 } __WLAN_ATTRIB_PACK__ hfa384x_downloadbuffer_t;
 
 /*-- Information Record: PRIIdentity --*/
 typedef struct hfa384x_PRIIdentity
 {
-	UINT16	PRICompID;
-	UINT16	PRIVariant;
-	UINT16	PRIMajorVersion;
-	UINT16	PRIMinorVersion;
+	u16	PRICompID;
+	u16	PRIVariant;
+	u16	PRIMajorVersion;
+	u16	PRIMinorVersion;
 } __WLAN_ATTRIB_PACK__ hfa384x_PRIIdentity_t;
 
 /*-- Information Record: PRISupRange --*/
 typedef struct hfa384x_PRISupRange
 {
-	UINT16	PRIRole;
-	UINT16	PRIID;
-	UINT16	PRIVariant;
-	UINT16	PRIBottom;
-	UINT16	PRITop;
+	u16	PRIRole;
+	u16	PRIID;
+	u16	PRIVariant;
+	u16	PRIBottom;
+	u16	PRITop;
 } __WLAN_ATTRIB_PACK__ hfa384x_PRISupRange_t;
 
 /*-- Information Record: CFIActRanges --*/
 typedef struct hfa384x_CFIActRanges
 {
-	UINT16	CFIRole;
-	UINT16	CFIID;
-	UINT16	CFIVariant;
-	UINT16	CFIBottom;
-	UINT16	CFITop;
+	u16	CFIRole;
+	u16	CFIID;
+	u16	CFIVariant;
+	u16	CFIBottom;
+	u16	CFITop;
 } __WLAN_ATTRIB_PACK__ hfa384x_CFIActRanges_t;
 
 /*-- Information Record: NICSerialNumber --*/
 typedef struct hfa384x_NICSerialNumber
 {
-	UINT8	NICSerialNumber[12];
+	u8	NICSerialNumber[12];
 } __WLAN_ATTRIB_PACK__ hfa384x_NICSerialNumber_t;
 
 /*-- Information Record: NICIdentity --*/
 typedef struct hfa384x_NICIdentity
 {
-	UINT16	NICCompID;
-	UINT16	NICVariant;
-	UINT16	NICMajorVersion;
-	UINT16	NICMinorVersion;
+	u16	NICCompID;
+	u16	NICVariant;
+	u16	NICMajorVersion;
+	u16	NICMinorVersion;
 } __WLAN_ATTRIB_PACK__ hfa384x_NICIdentity_t;
 
 /*-- Information Record: MFISupRange --*/
 typedef struct hfa384x_MFISupRange
 {
-	UINT16	MFIRole;
-	UINT16	MFIID;
-	UINT16	MFIVariant;
-	UINT16	MFIBottom;
-	UINT16	MFITop;
+	u16	MFIRole;
+	u16	MFIID;
+	u16	MFIVariant;
+	u16	MFIBottom;
+	u16	MFITop;
 } __WLAN_ATTRIB_PACK__ hfa384x_MFISupRange_t;
 
 /*-- Information Record: CFISupRange --*/
 typedef struct hfa384x_CFISupRange
 {
-	UINT16	CFIRole;
-	UINT16	CFIID;
-	UINT16	CFIVariant;
-	UINT16	CFIBottom;
-	UINT16	CFITop;
+	u16	CFIRole;
+	u16	CFIID;
+	u16	CFIVariant;
+	u16	CFIBottom;
+	u16	CFITop;
 } __WLAN_ATTRIB_PACK__ hfa384x_CFISupRange_t;
 
 /*-- Information Record: BUILDSEQ:BuildSeq --*/
 typedef struct hfa384x_BuildSeq {
-	UINT16	primary;
-	UINT16	secondary;
+	u16	primary;
+	u16	secondary;
 } __WLAN_ATTRIB_PACK__ hfa384x_BuildSeq_t;
 
 /*-- Information Record: FWID --*/
 #define HFA384x_FWID_LEN	14
 typedef struct hfa384x_FWID {
-	UINT8	primary[HFA384x_FWID_LEN];
-	UINT8	secondary[HFA384x_FWID_LEN];
+	u8	primary[HFA384x_FWID_LEN];
+	u8	secondary[HFA384x_FWID_LEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_FWID_t;
 
 /*-- Information Record: ChannelList --*/
 typedef struct hfa384x_ChannelList
 {
-	UINT16	ChannelList;
+	u16	ChannelList;
 } __WLAN_ATTRIB_PACK__ hfa384x_ChannelList_t;
 
 /*-- Information Record: RegulatoryDomains --*/
 typedef struct hfa384x_RegulatoryDomains
 {
-	UINT8	RegulatoryDomains[12];
+	u8	RegulatoryDomains[12];
 } __WLAN_ATTRIB_PACK__ hfa384x_RegulatoryDomains_t;
 
 /*-- Information Record: TempType --*/
 typedef struct hfa384x_TempType
 {
-	UINT16	TempType;
+	u16	TempType;
 } __WLAN_ATTRIB_PACK__ hfa384x_TempType_t;
 
 /*-- Information Record: CIS --*/
 typedef struct hfa384x_CIS
 {
-	UINT8	CIS[480];
+	u8	CIS[480];
 } __WLAN_ATTRIB_PACK__ hfa384x_CIS_t;
 
 /*-- Information Record: STAIdentity --*/
 typedef struct hfa384x_STAIdentity
 {
-	UINT16	STACompID;
-	UINT16	STAVariant;
-	UINT16	STAMajorVersion;
-	UINT16	STAMinorVersion;
+	u16	STACompID;
+	u16	STAVariant;
+	u16	STAMajorVersion;
+	u16	STAMinorVersion;
 } __WLAN_ATTRIB_PACK__ hfa384x_STAIdentity_t;
 
 /*-- Information Record: STASupRange --*/
 typedef struct hfa384x_STASupRange
 {
-	UINT16	STARole;
-	UINT16	STAID;
-	UINT16	STAVariant;
-	UINT16	STABottom;
-	UINT16	STATop;
+	u16	STARole;
+	u16	STAID;
+	u16	STAVariant;
+	u16	STABottom;
+	u16	STATop;
 } __WLAN_ATTRIB_PACK__ hfa384x_STASupRange_t;
 
 /*-- Information Record: MFIActRanges --*/
 typedef struct hfa384x_MFIActRanges
 {
-	UINT16	MFIRole;
-	UINT16	MFIID;
-	UINT16	MFIVariant;
-	UINT16	MFIBottom;
-	UINT16	MFITop;
+	u16	MFIRole;
+	u16	MFIID;
+	u16	MFIVariant;
+	u16	MFIBottom;
+	u16	MFITop;
 } __WLAN_ATTRIB_PACK__ hfa384x_MFIActRanges_t;
 
 /*--------------------------------------------------------------------
@@ -1369,145 +1333,145 @@ Information Record Structures: NIC Information
 /*-- Information Record: PortStatus --*/
 typedef struct hfa384x_PortStatus
 {
-	UINT16	PortStatus;
+	u16	PortStatus;
 } __WLAN_ATTRIB_PACK__ hfa384x_PortStatus_t;
 
-#define HFA384x_PSTATUS_DISABLED	((UINT16)1)
-#define HFA384x_PSTATUS_SEARCHING	((UINT16)2)
-#define HFA384x_PSTATUS_CONN_IBSS	((UINT16)3)
-#define HFA384x_PSTATUS_CONN_ESS	((UINT16)4)
-#define HFA384x_PSTATUS_OUTOFRANGE	((UINT16)5)
-#define HFA384x_PSTATUS_CONN_WDS	((UINT16)6)
+#define HFA384x_PSTATUS_DISABLED	((u16)1)
+#define HFA384x_PSTATUS_SEARCHING	((u16)2)
+#define HFA384x_PSTATUS_CONN_IBSS	((u16)3)
+#define HFA384x_PSTATUS_CONN_ESS	((u16)4)
+#define HFA384x_PSTATUS_OUTOFRANGE	((u16)5)
+#define HFA384x_PSTATUS_CONN_WDS	((u16)6)
 
 /*-- Information Record: CurrentSSID --*/
 typedef struct hfa384x_CurrentSSID
 {
-	UINT8	CurrentSSID[34];
+	u8	CurrentSSID[34];
 } __WLAN_ATTRIB_PACK__ hfa384x_CurrentSSID_t;
 
 /*-- Information Record: CurrentBSSID --*/
 typedef struct hfa384x_CurrentBSSID
 {
-	UINT8	CurrentBSSID[6];
+	u8	CurrentBSSID[6];
 } __WLAN_ATTRIB_PACK__ hfa384x_CurrentBSSID_t;
 
 /*-- Information Record: commsquality --*/
 typedef struct hfa384x_commsquality
 {
-	UINT16	CQ_currBSS;
-	UINT16	ASL_currBSS;
-	UINT16	ANL_currFC;
+	u16	CQ_currBSS;
+	u16	ASL_currBSS;
+	u16	ANL_currFC;
 } __WLAN_ATTRIB_PACK__ hfa384x_commsquality_t;
 
 /*-- Information Record: dmbcommsquality --*/
 typedef struct hfa384x_dbmcommsquality
 {
-	UINT16	CQdbm_currBSS;
-	UINT16	ASLdbm_currBSS;
-	UINT16	ANLdbm_currFC;
+	u16	CQdbm_currBSS;
+	u16	ASLdbm_currBSS;
+	u16	ANLdbm_currFC;
 } __WLAN_ATTRIB_PACK__ hfa384x_dbmcommsquality_t;
 
 /*-- Information Record: CurrentTxRate --*/
 typedef struct hfa384x_CurrentTxRate
 {
-	UINT16	CurrentTxRate;
+	u16	CurrentTxRate;
 } __WLAN_ATTRIB_PACK__ hfa384x_CurrentTxRate_t;
 
 /*-- Information Record: CurrentBeaconInterval --*/
 typedef struct hfa384x_CurrentBeaconInterval
 {
-	UINT16	CurrentBeaconInterval;
+	u16	CurrentBeaconInterval;
 } __WLAN_ATTRIB_PACK__ hfa384x_CurrentBeaconInterval_t;
 
 /*-- Information Record: CurrentScaleThresholds --*/
 typedef struct hfa384x_CurrentScaleThresholds
 {
-	UINT16	EnergyDetectThreshold;
-	UINT16	CarrierDetectThreshold;
-	UINT16	DeferDetectThreshold;
-	UINT16	CellSearchThreshold; /* Stations only */
-	UINT16	DeadSpotThreshold; /* Stations only */
+	u16	EnergyDetectThreshold;
+	u16	CarrierDetectThreshold;
+	u16	DeferDetectThreshold;
+	u16	CellSearchThreshold; /* Stations only */
+	u16	DeadSpotThreshold; /* Stations only */
 } __WLAN_ATTRIB_PACK__ hfa384x_CurrentScaleThresholds_t;
 
 /*-- Information Record: ProtocolRspTime --*/
 typedef struct hfa384x_ProtocolRspTime
 {
-	UINT16	ProtocolRspTime;
+	u16	ProtocolRspTime;
 } __WLAN_ATTRIB_PACK__ hfa384x_ProtocolRspTime_t;
 
 /*-- Information Record: ShortRetryLimit --*/
 typedef struct hfa384x_ShortRetryLimit
 {
-	UINT16	ShortRetryLimit;
+	u16	ShortRetryLimit;
 } __WLAN_ATTRIB_PACK__ hfa384x_ShortRetryLimit_t;
 
 /*-- Information Record: LongRetryLimit --*/
 typedef struct hfa384x_LongRetryLimit
 {
-	UINT16	LongRetryLimit;
+	u16	LongRetryLimit;
 } __WLAN_ATTRIB_PACK__ hfa384x_LongRetryLimit_t;
 
 /*-- Information Record: MaxTransmitLifetime --*/
 typedef struct hfa384x_MaxTransmitLifetime
 {
-	UINT16	MaxTransmitLifetime;
+	u16	MaxTransmitLifetime;
 } __WLAN_ATTRIB_PACK__ hfa384x_MaxTransmitLifetime_t;
 
 /*-- Information Record: MaxReceiveLifetime --*/
 typedef struct hfa384x_MaxReceiveLifetime
 {
-	UINT16	MaxReceiveLifetime;
+	u16	MaxReceiveLifetime;
 } __WLAN_ATTRIB_PACK__ hfa384x_MaxReceiveLifetime_t;
 
 /*-- Information Record: CFPollable --*/
 typedef struct hfa384x_CFPollable
 {
-	UINT16	CFPollable;
+	u16	CFPollable;
 } __WLAN_ATTRIB_PACK__ hfa384x_CFPollable_t;
 
 /*-- Information Record: AuthenticationAlgorithms --*/
 typedef struct hfa384x_AuthenticationAlgorithms
 {
-	UINT16	AuthenticationType;
-	UINT16	TypeEnabled;
+	u16	AuthenticationType;
+	u16	TypeEnabled;
 } __WLAN_ATTRIB_PACK__ hfa384x_AuthenticationAlgorithms_t;
 
 /*-- Information Record: AuthenticationAlgorithms
 (data only --*/
 typedef struct hfa384x_AuthenticationAlgorithms_data
 {
-	UINT16	AuthenticationType;
-	UINT16	TypeEnabled;
+	u16	AuthenticationType;
+	u16	TypeEnabled;
 } __WLAN_ATTRIB_PACK__ hfa384x_AuthenticationAlgorithms_data_t;
 
 /*-- Information Record: PrivacyOptionImplemented --*/
 typedef struct hfa384x_PrivacyOptionImplemented
 {
-	UINT16	PrivacyOptionImplemented;
+	u16	PrivacyOptionImplemented;
 } __WLAN_ATTRIB_PACK__ hfa384x_PrivacyOptionImplemented_t;
 
 /*-- Information Record: OwnMACAddress --*/
 typedef struct hfa384x_OwnMACAddress
 {
-	UINT8	OwnMACAddress[6];
+	u8	OwnMACAddress[6];
 } __WLAN_ATTRIB_PACK__ hfa384x_OwnMACAddress_t;
 
 /*-- Information Record: PCFInfo --*/
 typedef struct hfa384x_PCFInfo
 {
-	UINT16	MediumOccupancyLimit;
-	UINT16	CFPPeriod;
-	UINT16	CFPMaxDuration;
-	UINT16	CFPFlags;
+	u16	MediumOccupancyLimit;
+	u16	CFPPeriod;
+	u16	CFPMaxDuration;
+	u16	CFPFlags;
 } __WLAN_ATTRIB_PACK__ hfa384x_PCFInfo_t;
 
 /*-- Information Record: PCFInfo (data portion only) --*/
 typedef struct hfa384x_PCFInfo_data
 {
-	UINT16	MediumOccupancyLimit;
-	UINT16	CFPPeriod;
-	UINT16	CFPMaxDuration;
-	UINT16	CFPFlags;
+	u16	MediumOccupancyLimit;
+	u16	CFPPeriod;
+	u16	CFPMaxDuration;
+	u16	CFPFlags;
 } __WLAN_ATTRIB_PACK__ hfa384x_PCFInfo_data_t;
 
 /*--------------------------------------------------------------------
@@ -1517,39 +1481,39 @@ Information Record Structures: Modem Information Records
 /*-- Information Record: PHYType --*/
 typedef struct hfa384x_PHYType
 {
-	UINT16	PHYType;
+	u16	PHYType;
 } __WLAN_ATTRIB_PACK__ hfa384x_PHYType_t;
 
 /*-- Information Record: CurrentChannel --*/
 typedef struct hfa384x_CurrentChannel
 {
-	UINT16	CurrentChannel;
+	u16	CurrentChannel;
 } __WLAN_ATTRIB_PACK__ hfa384x_CurrentChannel_t;
 
 /*-- Information Record: CurrentPowerState --*/
 typedef struct hfa384x_CurrentPowerState
 {
-	UINT16	CurrentPowerState;
+	u16	CurrentPowerState;
 } __WLAN_ATTRIB_PACK__ hfa384x_CurrentPowerState_t;
 
 /*-- Information Record: CCAMode --*/
 typedef struct hfa384x_CCAMode
 {
-	UINT16	CCAMode;
+	u16	CCAMode;
 } __WLAN_ATTRIB_PACK__ hfa384x_CCAMode_t;
 
 /*-- Information Record: SupportedDataRates --*/
 typedef struct hfa384x_SupportedDataRates
 {
-	UINT8	SupportedDataRates[10];
+	u8	SupportedDataRates[10];
 } __WLAN_ATTRIB_PACK__ hfa384x_SupportedDataRates_t;
 
 /*-- Information Record: LFOStatus --*/
 typedef struct hfa384x_LFOStatus
 {
-	UINT16  TestResults;
-	UINT16  LFOResult;
-	UINT16  VRHFOResult;
+	u16  TestResults;
+	u16  LFOResult;
+	u16  VRHFOResult;
 } __WLAN_ATTRIB_PACK__ hfa384x_LFOStatus_t;
 
 #define HFA384x_TESTRESULT_ALLPASSED    BIT0
@@ -1561,11 +1525,11 @@ typedef struct hfa384x_LFOStatus
 /*-- Information Record: LEDControl --*/
 typedef struct hfa384x_LEDControl
 {
-	UINT16  searching_on;
-	UINT16  searching_off;
-	UINT16  assoc_on;
-	UINT16  assoc_off;
-	UINT16  activity;
+	u16  searching_on;
+	u16  searching_off;
+	u16  assoc_on;
+	u16  assoc_off;
+	u16  activity;
 } __WLAN_ATTRIB_PACK__ hfa384x_LEDControl_t;
 
 /*--------------------------------------------------------------------
@@ -1576,32 +1540,32 @@ FRAME DESCRIPTORS: Offsets
 ----------------------------------------------------------------------
 Control Info (offset 44-51)
 --------------------------------------------------------------------*/
-#define		HFA384x_FD_STATUS_OFF			((UINT16)0x44)
-#define		HFA384x_FD_TIME_OFF			((UINT16)0x46)
-#define		HFA384x_FD_SWSUPPORT_OFF		((UINT16)0x4A)
-#define		HFA384x_FD_SILENCE_OFF			((UINT16)0x4A)
-#define		HFA384x_FD_SIGNAL_OFF			((UINT16)0x4B)
-#define		HFA384x_FD_RATE_OFF			((UINT16)0x4C)
-#define		HFA384x_FD_RXFLOW_OFF			((UINT16)0x4D)
-#define		HFA384x_FD_RESERVED_OFF			((UINT16)0x4E)
-#define		HFA384x_FD_TXCONTROL_OFF		((UINT16)0x50)
+#define		HFA384x_FD_STATUS_OFF			((u16)0x44)
+#define		HFA384x_FD_TIME_OFF			((u16)0x46)
+#define		HFA384x_FD_SWSUPPORT_OFF		((u16)0x4A)
+#define		HFA384x_FD_SILENCE_OFF			((u16)0x4A)
+#define		HFA384x_FD_SIGNAL_OFF			((u16)0x4B)
+#define		HFA384x_FD_RATE_OFF			((u16)0x4C)
+#define		HFA384x_FD_RXFLOW_OFF			((u16)0x4D)
+#define		HFA384x_FD_RESERVED_OFF			((u16)0x4E)
+#define		HFA384x_FD_TXCONTROL_OFF		((u16)0x50)
 /*--------------------------------------------------------------------
 802.11 Header (offset 52-6B)
 --------------------------------------------------------------------*/
-#define		HFA384x_FD_FRAMECONTROL_OFF		((UINT16)0x52)
-#define		HFA384x_FD_DURATIONID_OFF		((UINT16)0x54)
-#define		HFA384x_FD_ADDRESS1_OFF			((UINT16)0x56)
-#define		HFA384x_FD_ADDRESS2_OFF			((UINT16)0x5C)
-#define		HFA384x_FD_ADDRESS3_OFF			((UINT16)0x62)
-#define		HFA384x_FD_SEQCONTROL_OFF		((UINT16)0x68)
-#define		HFA384x_FD_ADDRESS4_OFF			((UINT16)0x6A)
-#define		HFA384x_FD_DATALEN_OFF			((UINT16)0x70)
+#define		HFA384x_FD_FRAMECONTROL_OFF		((u16)0x52)
+#define		HFA384x_FD_DURATIONID_OFF		((u16)0x54)
+#define		HFA384x_FD_ADDRESS1_OFF			((u16)0x56)
+#define		HFA384x_FD_ADDRESS2_OFF			((u16)0x5C)
+#define		HFA384x_FD_ADDRESS3_OFF			((u16)0x62)
+#define		HFA384x_FD_SEQCONTROL_OFF		((u16)0x68)
+#define		HFA384x_FD_ADDRESS4_OFF			((u16)0x6A)
+#define		HFA384x_FD_DATALEN_OFF			((u16)0x70)
 /*--------------------------------------------------------------------
 802.3 Header (offset 72-7F)
 --------------------------------------------------------------------*/
-#define		HFA384x_FD_DESTADDRESS_OFF		((UINT16)0x72)
-#define		HFA384x_FD_SRCADDRESS_OFF		((UINT16)0x78)
-#define		HFA384x_FD_DATALENGTH_OFF		((UINT16)0x7E)
+#define		HFA384x_FD_DESTADDRESS_OFF		((u16)0x72)
+#define		HFA384x_FD_SRCADDRESS_OFF		((u16)0x78)
+#define		HFA384x_FD_DATALENGTH_OFF		((u16)0x7E)
 
 /*--------------------------------------------------------------------
 FRAME STRUCTURES: Communication Frames
@@ -1611,67 +1575,67 @@ Communication Frames: Transmit Frames
 /*-- Communication Frame: Transmit Frame Structure --*/
 typedef struct hfa384x_tx_frame
 {
-	UINT16	status;
-	UINT16	reserved1;
-	UINT16	reserved2;
-	UINT32	sw_support;
-	UINT8	tx_retrycount;
-	UINT8   tx_rate;
-	UINT16	tx_control;
+	u16	status;
+	u16	reserved1;
+	u16	reserved2;
+	u32	sw_support;
+	u8	tx_retrycount;
+	u8   tx_rate;
+	u16	tx_control;
 
 	/*-- 802.11 Header Information --*/
 
-	UINT16	frame_control;
-	UINT16	duration_id;
-	UINT8	address1[6];
-	UINT8	address2[6];
-	UINT8	address3[6];
-	UINT16	sequence_control;
-	UINT8	address4[6];
-	UINT16	data_len; /* little endian format */
+	u16	frame_control;
+	u16	duration_id;
+	u8	address1[6];
+	u8	address2[6];
+	u8	address3[6];
+	u16	sequence_control;
+	u8	address4[6];
+	u16	data_len; /* little endian format */
 
 	/*-- 802.3 Header Information --*/
 
-	UINT8	dest_addr[6];
-	UINT8	src_addr[6];
-	UINT16	data_length; /* big endian format */
+	u8	dest_addr[6];
+	u8	src_addr[6];
+	u16	data_length; /* big endian format */
 } __WLAN_ATTRIB_PACK__ hfa384x_tx_frame_t;
 /*--------------------------------------------------------------------
 Communication Frames: Field Masks for Transmit Frames
 --------------------------------------------------------------------*/
 /*-- Status Field --*/
-#define		HFA384x_TXSTATUS_ACKERR			((UINT16)BIT5)
-#define		HFA384x_TXSTATUS_FORMERR		((UINT16)BIT3)
-#define		HFA384x_TXSTATUS_DISCON			((UINT16)BIT2)
-#define		HFA384x_TXSTATUS_AGEDERR		((UINT16)BIT1)
-#define		HFA384x_TXSTATUS_RETRYERR		((UINT16)BIT0)
+#define		HFA384x_TXSTATUS_ACKERR			((u16)BIT5)
+#define		HFA384x_TXSTATUS_FORMERR		((u16)BIT3)
+#define		HFA384x_TXSTATUS_DISCON			((u16)BIT2)
+#define		HFA384x_TXSTATUS_AGEDERR		((u16)BIT1)
+#define		HFA384x_TXSTATUS_RETRYERR		((u16)BIT0)
 /*-- Transmit Control Field --*/
-#define		HFA384x_TX_CFPOLL			((UINT16)BIT12)
-#define		HFA384x_TX_PRST				((UINT16)BIT11)
-#define		HFA384x_TX_MACPORT			((UINT16)(BIT10 | BIT9 | BIT8))
-#define		HFA384x_TX_NOENCRYPT			((UINT16)BIT7)
-#define		HFA384x_TX_RETRYSTRAT			((UINT16)(BIT6 | BIT5))
-#define		HFA384x_TX_STRUCTYPE			((UINT16)(BIT4 | BIT3))
-#define		HFA384x_TX_TXEX				((UINT16)BIT2)
-#define		HFA384x_TX_TXOK				((UINT16)BIT1)
+#define		HFA384x_TX_CFPOLL			((u16)BIT12)
+#define		HFA384x_TX_PRST				((u16)BIT11)
+#define		HFA384x_TX_MACPORT			((u16)(BIT10 | BIT9 | BIT8))
+#define		HFA384x_TX_NOENCRYPT			((u16)BIT7)
+#define		HFA384x_TX_RETRYSTRAT			((u16)(BIT6 | BIT5))
+#define		HFA384x_TX_STRUCTYPE			((u16)(BIT4 | BIT3))
+#define		HFA384x_TX_TXEX				((u16)BIT2)
+#define		HFA384x_TX_TXOK				((u16)BIT1)
 /*--------------------------------------------------------------------
 Communication Frames: Test/Get/Set Field Values for Transmit Frames
 --------------------------------------------------------------------*/
 /*-- Status Field --*/
 #define HFA384x_TXSTATUS_ISERROR(v)	\
-	(((UINT16)(v))&\
+	(((u16)(v))&\
 	(HFA384x_TXSTATUS_ACKERR|HFA384x_TXSTATUS_FORMERR|\
 	HFA384x_TXSTATUS_DISCON|HFA384x_TXSTATUS_AGEDERR|\
 	HFA384x_TXSTATUS_RETRYERR))
 
-#define	HFA384x_TXSTATUS_ISACKERR(v)	((UINT16)(((UINT16)(v)) & HFA384x_TXSTATUS_ACKERR))
-#define	HFA384x_TXSTATUS_ISFORMERR(v)	((UINT16)(((UINT16)(v)) & HFA384x_TXSTATUS_FORMERR))
-#define	HFA384x_TXSTATUS_ISDISCON(v)	((UINT16)(((UINT16)(v)) & HFA384x_TXSTATUS_DISCON))
-#define	HFA384x_TXSTATUS_ISAGEDERR(v)	((UINT16)(((UINT16)(v)) & HFA384x_TXSTATUS_AGEDERR))
-#define	HFA384x_TXSTATUS_ISRETRYERR(v)	((UINT16)(((UINT16)(v)) & HFA384x_TXSTATUS_RETRYERR))
+#define	HFA384x_TXSTATUS_ISACKERR(v)	((u16)(((u16)(v)) & HFA384x_TXSTATUS_ACKERR))
+#define	HFA384x_TXSTATUS_ISFORMERR(v)	((u16)(((u16)(v)) & HFA384x_TXSTATUS_FORMERR))
+#define	HFA384x_TXSTATUS_ISDISCON(v)	((u16)(((u16)(v)) & HFA384x_TXSTATUS_DISCON))
+#define	HFA384x_TXSTATUS_ISAGEDERR(v)	((u16)(((u16)(v)) & HFA384x_TXSTATUS_AGEDERR))
+#define	HFA384x_TXSTATUS_ISRETRYERR(v)	((u16)(((u16)(v)) & HFA384x_TXSTATUS_RETRYERR))
 
-#define	HFA384x_TX_GET(v,m,s)		((((UINT16)(v))&((UINT16)(m)))>>((UINT16)(s)))
-#define	HFA384x_TX_SET(v,m,s)		((((UINT16)(v))<<((UINT16)(s)))&((UINT16)(m)))
+#define	HFA384x_TX_GET(v,m,s)		((((u16)(v))&((u16)(m)))>>((u16)(s)))
+#define	HFA384x_TX_SET(v,m,s)		((((u16)(v))<<((u16)(s)))&((u16)(m)))
 
 #define	HFA384x_TX_CFPOLL_GET(v)	HFA384x_TX_GET(v, HFA384x_TX_CFPOLL,12)
 #define	HFA384x_TX_CFPOLL_SET(v)	HFA384x_TX_SET(v, HFA384x_TX_CFPOLL,12)
@@ -1696,70 +1660,70 @@ Communication Frames: Receive Frames
 typedef struct hfa384x_rx_frame
 {
 	/*-- MAC rx descriptor (hfa384x byte order) --*/
-	UINT16	status;
-	UINT32	time;
-	UINT8	silence;
-	UINT8	signal;
-	UINT8	rate;
-	UINT8	rx_flow;
-	UINT16	reserved1;
-	UINT16	reserved2;
+	u16	status;
+	u32	time;
+	u8	silence;
+	u8	signal;
+	u8	rate;
+	u8	rx_flow;
+	u16	reserved1;
+	u16	reserved2;
 
 	/*-- 802.11 Header Information (802.11 byte order) --*/
-	UINT16	frame_control;
-	UINT16	duration_id;
-	UINT8	address1[6];
-	UINT8	address2[6];
-	UINT8	address3[6];
-	UINT16	sequence_control;
-	UINT8	address4[6];
-	UINT16	data_len; /* hfa384x (little endian) format */
+	u16	frame_control;
+	u16	duration_id;
+	u8	address1[6];
+	u8	address2[6];
+	u8	address3[6];
+	u16	sequence_control;
+	u8	address4[6];
+	u16	data_len; /* hfa384x (little endian) format */
 
 	/*-- 802.3 Header Information --*/
-	UINT8	dest_addr[6];
-	UINT8	src_addr[6];
-	UINT16	data_length; /* IEEE? (big endian) format */
+	u8	dest_addr[6];
+	u8	src_addr[6];
+	u16	data_length; /* IEEE? (big endian) format */
 } __WLAN_ATTRIB_PACK__ hfa384x_rx_frame_t;
 /*--------------------------------------------------------------------
 Communication Frames: Field Masks for Receive Frames
 --------------------------------------------------------------------*/
 /*-- Offsets --------*/
-#define		HFA384x_RX_DATA_LEN_OFF			((UINT16)44)
-#define		HFA384x_RX_80211HDR_OFF			((UINT16)14)
-#define		HFA384x_RX_DATA_OFF			((UINT16)60)
+#define		HFA384x_RX_DATA_LEN_OFF			((u16)44)
+#define		HFA384x_RX_80211HDR_OFF			((u16)14)
+#define		HFA384x_RX_DATA_OFF			((u16)60)
 
 /*-- Status Fields --*/
-#define		HFA384x_RXSTATUS_MSGTYPE		((UINT16)(BIT15 | BIT14 | BIT13))
-#define		HFA384x_RXSTATUS_MACPORT		((UINT16)(BIT10 | BIT9 | BIT8))
-#define		HFA384x_RXSTATUS_UNDECR			((UINT16)BIT1)
-#define		HFA384x_RXSTATUS_FCSERR			((UINT16)BIT0)
+#define		HFA384x_RXSTATUS_MSGTYPE		((u16)(BIT15 | BIT14 | BIT13))
+#define		HFA384x_RXSTATUS_MACPORT		((u16)(BIT10 | BIT9 | BIT8))
+#define		HFA384x_RXSTATUS_UNDECR			((u16)BIT1)
+#define		HFA384x_RXSTATUS_FCSERR			((u16)BIT0)
 /*--------------------------------------------------------------------
 Communication Frames: Test/Get/Set Field Values for Receive Frames
 --------------------------------------------------------------------*/
-#define		HFA384x_RXSTATUS_MSGTYPE_GET(value)	((UINT16)((((UINT16)(value)) & HFA384x_RXSTATUS_MSGTYPE) >> 13))
-#define		HFA384x_RXSTATUS_MSGTYPE_SET(value)	((UINT16)(((UINT16)(value)) << 13))
-#define		HFA384x_RXSTATUS_MACPORT_GET(value)	((UINT16)((((UINT16)(value)) & HFA384x_RXSTATUS_MACPORT) >> 8))
-#define		HFA384x_RXSTATUS_MACPORT_SET(value)	((UINT16)(((UINT16)(value)) << 8))
-#define		HFA384x_RXSTATUS_ISUNDECR(value)	((UINT16)(((UINT16)(value)) & HFA384x_RXSTATUS_UNDECR))
-#define		HFA384x_RXSTATUS_ISFCSERR(value)	((UINT16)(((UINT16)(value)) & HFA384x_RXSTATUS_FCSERR))
+#define		HFA384x_RXSTATUS_MSGTYPE_GET(value)	((u16)((((u16)(value)) & HFA384x_RXSTATUS_MSGTYPE) >> 13))
+#define		HFA384x_RXSTATUS_MSGTYPE_SET(value)	((u16)(((u16)(value)) << 13))
+#define		HFA384x_RXSTATUS_MACPORT_GET(value)	((u16)((((u16)(value)) & HFA384x_RXSTATUS_MACPORT) >> 8))
+#define		HFA384x_RXSTATUS_MACPORT_SET(value)	((u16)(((u16)(value)) << 8))
+#define		HFA384x_RXSTATUS_ISUNDECR(value)	((u16)(((u16)(value)) & HFA384x_RXSTATUS_UNDECR))
+#define		HFA384x_RXSTATUS_ISFCSERR(value)	((u16)(((u16)(value)) & HFA384x_RXSTATUS_FCSERR))
 /*--------------------------------------------------------------------
  FRAME STRUCTURES: Information Types and Information Frame Structures
 ----------------------------------------------------------------------
 Information Types
 --------------------------------------------------------------------*/
-#define		HFA384x_IT_HANDOVERADDR			((UINT16)0xF000UL)
-#define		HFA384x_IT_HANDOVERDEAUTHADDRESS	((UINT16)0xF001UL)//AP 1.3.7
-#define		HFA384x_IT_COMMTALLIES			((UINT16)0xF100UL)
-#define		HFA384x_IT_SCANRESULTS			((UINT16)0xF101UL)
-#define		HFA384x_IT_CHINFORESULTS		((UINT16)0xF102UL)
-#define		HFA384x_IT_HOSTSCANRESULTS		((UINT16)0xF103UL)
-#define		HFA384x_IT_LINKSTATUS			((UINT16)0xF200UL)
-#define		HFA384x_IT_ASSOCSTATUS			((UINT16)0xF201UL)
-#define		HFA384x_IT_AUTHREQ			((UINT16)0xF202UL)
-#define		HFA384x_IT_PSUSERCNT			((UINT16)0xF203UL)
-#define		HFA384x_IT_KEYIDCHANGED			((UINT16)0xF204UL)
-#define		HFA384x_IT_ASSOCREQ    			((UINT16)0xF205UL)
-#define		HFA384x_IT_MICFAILURE  			((UINT16)0xF206UL)
+#define		HFA384x_IT_HANDOVERADDR			((u16)0xF000UL)
+#define		HFA384x_IT_HANDOVERDEAUTHADDRESS	((u16)0xF001UL)//AP 1.3.7
+#define		HFA384x_IT_COMMTALLIES			((u16)0xF100UL)
+#define		HFA384x_IT_SCANRESULTS			((u16)0xF101UL)
+#define		HFA384x_IT_CHINFORESULTS		((u16)0xF102UL)
+#define		HFA384x_IT_HOSTSCANRESULTS		((u16)0xF103UL)
+#define		HFA384x_IT_LINKSTATUS			((u16)0xF200UL)
+#define		HFA384x_IT_ASSOCSTATUS			((u16)0xF201UL)
+#define		HFA384x_IT_AUTHREQ			((u16)0xF202UL)
+#define		HFA384x_IT_PSUSERCNT			((u16)0xF203UL)
+#define		HFA384x_IT_KEYIDCHANGED			((u16)0xF204UL)
+#define		HFA384x_IT_ASSOCREQ    			((u16)0xF205UL)
+#define		HFA384x_IT_MICFAILURE  			((u16)0xF206UL)
 
 /*--------------------------------------------------------------------
 Information Frames Structures
@@ -1769,80 +1733,80 @@ Information Frames: Notification Frame Structures
 /*--  Notification Frame,MAC Mgmt: Handover Address --*/
 typedef struct hfa384x_HandoverAddr
 {
-	UINT16	framelen;
-	UINT16	infotype;
-	UINT8	handover_addr[WLAN_BSSID_LEN];
+	u16	framelen;
+	u16	infotype;
+	u8	handover_addr[WLAN_BSSID_LEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_HandoverAddr_t;
 
 /*--  Inquiry Frame, Diagnose: Communication Tallies --*/
 typedef struct hfa384x_CommTallies16
 {
-	UINT16	txunicastframes;
-	UINT16	txmulticastframes;
-	UINT16	txfragments;
-	UINT16	txunicastoctets;
-	UINT16	txmulticastoctets;
-	UINT16	txdeferredtrans;
-	UINT16	txsingleretryframes;
-	UINT16	txmultipleretryframes;
-	UINT16	txretrylimitexceeded;
-	UINT16	txdiscards;
-	UINT16	rxunicastframes;
-	UINT16	rxmulticastframes;
-	UINT16	rxfragments;
-	UINT16	rxunicastoctets;
-	UINT16	rxmulticastoctets;
-	UINT16	rxfcserrors;
-	UINT16	rxdiscardsnobuffer;
-	UINT16	txdiscardswrongsa;
-	UINT16	rxdiscardswepundecr;
-	UINT16	rxmsginmsgfrag;
-	UINT16	rxmsginbadmsgfrag;
+	u16	txunicastframes;
+	u16	txmulticastframes;
+	u16	txfragments;
+	u16	txunicastoctets;
+	u16	txmulticastoctets;
+	u16	txdeferredtrans;
+	u16	txsingleretryframes;
+	u16	txmultipleretryframes;
+	u16	txretrylimitexceeded;
+	u16	txdiscards;
+	u16	rxunicastframes;
+	u16	rxmulticastframes;
+	u16	rxfragments;
+	u16	rxunicastoctets;
+	u16	rxmulticastoctets;
+	u16	rxfcserrors;
+	u16	rxdiscardsnobuffer;
+	u16	txdiscardswrongsa;
+	u16	rxdiscardswepundecr;
+	u16	rxmsginmsgfrag;
+	u16	rxmsginbadmsgfrag;
 } __WLAN_ATTRIB_PACK__ hfa384x_CommTallies16_t;
 
 typedef struct hfa384x_CommTallies32
 {
-	UINT32	txunicastframes;
-	UINT32	txmulticastframes;
-	UINT32	txfragments;
-	UINT32	txunicastoctets;
-	UINT32	txmulticastoctets;
-	UINT32	txdeferredtrans;
-	UINT32	txsingleretryframes;
-	UINT32	txmultipleretryframes;
-	UINT32	txretrylimitexceeded;
-	UINT32	txdiscards;
-	UINT32	rxunicastframes;
-	UINT32	rxmulticastframes;
-	UINT32	rxfragments;
-	UINT32	rxunicastoctets;
-	UINT32	rxmulticastoctets;
-	UINT32	rxfcserrors;
-	UINT32	rxdiscardsnobuffer;
-	UINT32	txdiscardswrongsa;
-	UINT32	rxdiscardswepundecr;
-	UINT32	rxmsginmsgfrag;
-	UINT32	rxmsginbadmsgfrag;
+	u32	txunicastframes;
+	u32	txmulticastframes;
+	u32	txfragments;
+	u32	txunicastoctets;
+	u32	txmulticastoctets;
+	u32	txdeferredtrans;
+	u32	txsingleretryframes;
+	u32	txmultipleretryframes;
+	u32	txretrylimitexceeded;
+	u32	txdiscards;
+	u32	rxunicastframes;
+	u32	rxmulticastframes;
+	u32	rxfragments;
+	u32	rxunicastoctets;
+	u32	rxmulticastoctets;
+	u32	rxfcserrors;
+	u32	rxdiscardsnobuffer;
+	u32	txdiscardswrongsa;
+	u32	rxdiscardswepundecr;
+	u32	rxmsginmsgfrag;
+	u32	rxmsginbadmsgfrag;
 } __WLAN_ATTRIB_PACK__ hfa384x_CommTallies32_t;
 
 /*--  Inquiry Frame, Diagnose: Scan Results & Subfields--*/
 typedef struct hfa384x_ScanResultSub
 {
-	UINT16	chid;
-	UINT16	anl;
-	UINT16	sl;
-	UINT8	bssid[WLAN_BSSID_LEN];
-	UINT16	bcnint;
-	UINT16	capinfo;
+	u16	chid;
+	u16	anl;
+	u16	sl;
+	u8	bssid[WLAN_BSSID_LEN];
+	u16	bcnint;
+	u16	capinfo;
 	hfa384x_bytestr32_t	ssid;
-	UINT8	supprates[10]; /* 802.11 info element */
-	UINT16	proberesp_rate;
+	u8	supprates[10]; /* 802.11 info element */
+	u16	proberesp_rate;
 } __WLAN_ATTRIB_PACK__ hfa384x_ScanResultSub_t;
 
 typedef struct hfa384x_ScanResult
 {
-	UINT16	rsvd;
-	UINT16	scanreason;
+	u16	rsvd;
+	u16	scanreason;
 	hfa384x_ScanResultSub_t
 		result[HFA384x_SCANRESULT_MAX];
 } __WLAN_ATTRIB_PACK__ hfa384x_ScanResult_t;
@@ -1850,10 +1814,10 @@ typedef struct hfa384x_ScanResult
 /*--  Inquiry Frame, Diagnose: ChInfo Results & Subfields--*/
 typedef struct hfa384x_ChInfoResultSub
 {
-	UINT16	chid;
-	UINT16	anl;
-	UINT16	pnl;
-	UINT16	active;
+	u16	chid;
+	u16	anl;
+	u16	pnl;
+	u16	active;
 } __WLAN_ATTRIB_PACK__ hfa384x_ChInfoResultSub_t;
 
 #define HFA384x_CHINFORESULT_BSSACTIVE	BIT0
@@ -1861,7 +1825,7 @@ typedef struct hfa384x_ChInfoResultSub
 
 typedef struct hfa384x_ChInfoResult
 {
-	UINT16	scanchannels;
+	u16	scanchannels;
 	hfa384x_ChInfoResultSub_t
 		result[HFA384x_CHINFORESULT_MAX];
 } __WLAN_ATTRIB_PACK__ hfa384x_ChInfoResult_t;
@@ -1869,75 +1833,75 @@ typedef struct hfa384x_ChInfoResult
 /*--  Inquiry Frame, Diagnose: Host Scan Results & Subfields--*/
 typedef struct hfa384x_HScanResultSub
 {
-	UINT16	chid;
-	UINT16	anl;
-	UINT16	sl;
-	UINT8	bssid[WLAN_BSSID_LEN];
-	UINT16	bcnint;
-	UINT16	capinfo;
+	u16	chid;
+	u16	anl;
+	u16	sl;
+	u8	bssid[WLAN_BSSID_LEN];
+	u16	bcnint;
+	u16	capinfo;
 	hfa384x_bytestr32_t	ssid;
-	UINT8	supprates[10]; /* 802.11 info element */
-	UINT16	proberesp_rate;
-	UINT16	atim;
+	u8	supprates[10]; /* 802.11 info element */
+	u16	proberesp_rate;
+	u16	atim;
 } __WLAN_ATTRIB_PACK__ hfa384x_HScanResultSub_t;
 
 typedef struct hfa384x_HScanResult
 {
-	UINT16	nresult;
-	UINT16	rsvd;
+	u16	nresult;
+	u16	rsvd;
 	hfa384x_HScanResultSub_t
 		result[HFA384x_HSCANRESULT_MAX];
 } __WLAN_ATTRIB_PACK__ hfa384x_HScanResult_t;
 
 /*--  Unsolicited Frame, MAC Mgmt: LinkStatus --*/
 
-#define HFA384x_LINK_NOTCONNECTED	((UINT16)0)
-#define HFA384x_LINK_CONNECTED		((UINT16)1)
-#define HFA384x_LINK_DISCONNECTED	((UINT16)2)
-#define HFA384x_LINK_AP_CHANGE		((UINT16)3)
-#define HFA384x_LINK_AP_OUTOFRANGE	((UINT16)4)
-#define HFA384x_LINK_AP_INRANGE		((UINT16)5)
-#define HFA384x_LINK_ASSOCFAIL		((UINT16)6)
+#define HFA384x_LINK_NOTCONNECTED	((u16)0)
+#define HFA384x_LINK_CONNECTED		((u16)1)
+#define HFA384x_LINK_DISCONNECTED	((u16)2)
+#define HFA384x_LINK_AP_CHANGE		((u16)3)
+#define HFA384x_LINK_AP_OUTOFRANGE	((u16)4)
+#define HFA384x_LINK_AP_INRANGE		((u16)5)
+#define HFA384x_LINK_ASSOCFAIL		((u16)6)
 
 typedef struct hfa384x_LinkStatus
 {
-	UINT16	linkstatus;
+	u16	linkstatus;
 } __WLAN_ATTRIB_PACK__ hfa384x_LinkStatus_t;
 
 
 /*--  Unsolicited Frame, MAC Mgmt: AssociationStatus (--*/
 
-#define HFA384x_ASSOCSTATUS_STAASSOC	((UINT16)1)
-#define HFA384x_ASSOCSTATUS_REASSOC	((UINT16)2)
-#define HFA384x_ASSOCSTATUS_DISASSOC	((UINT16)3)
-#define HFA384x_ASSOCSTATUS_ASSOCFAIL	((UINT16)4)
-#define HFA384x_ASSOCSTATUS_AUTHFAIL	((UINT16)5)
+#define HFA384x_ASSOCSTATUS_STAASSOC	((u16)1)
+#define HFA384x_ASSOCSTATUS_REASSOC	((u16)2)
+#define HFA384x_ASSOCSTATUS_DISASSOC	((u16)3)
+#define HFA384x_ASSOCSTATUS_ASSOCFAIL	((u16)4)
+#define HFA384x_ASSOCSTATUS_AUTHFAIL	((u16)5)
 
 typedef struct hfa384x_AssocStatus
 {
-	UINT16	assocstatus;
-	UINT8	sta_addr[WLAN_ADDR_LEN];
+	u16	assocstatus;
+	u8	sta_addr[WLAN_ADDR_LEN];
 	/* old_ap_addr is only valid if assocstatus == 2 */
-	UINT8	old_ap_addr[WLAN_ADDR_LEN];
-	UINT16	reason;
-	UINT16	reserved;
+	u8	old_ap_addr[WLAN_ADDR_LEN];
+	u16	reason;
+	u16	reserved;
 } __WLAN_ATTRIB_PACK__ hfa384x_AssocStatus_t;
 
 /*--  Unsolicited Frame, MAC Mgmt: AuthRequest (AP Only) --*/
 
 typedef struct hfa384x_AuthRequest
 {
-	UINT8	sta_addr[WLAN_ADDR_LEN];
-	UINT16	algorithm;
+	u8	sta_addr[WLAN_ADDR_LEN];
+	u16	algorithm;
 } __WLAN_ATTRIB_PACK__ hfa384x_AuthReq_t;
 
 /*--  Unsolicited Frame, MAC Mgmt: AssocRequest (AP Only) --*/
 
 typedef struct hfa384x_AssocRequest
 {
-	UINT8	sta_addr[WLAN_ADDR_LEN];
-	UINT16	type;
-	UINT8   wpa_data[80];
+	u8	sta_addr[WLAN_ADDR_LEN];
+	u16	type;
+	u8   wpa_data[80];
 } __WLAN_ATTRIB_PACK__ hfa384x_AssocReq_t;
 
 
@@ -1948,21 +1912,21 @@ typedef struct hfa384x_AssocRequest
 
 typedef struct hfa384x_MicFailure
 {
-	UINT8	sender[WLAN_ADDR_LEN];
-	UINT8	dest[WLAN_ADDR_LEN];
+	u8	sender[WLAN_ADDR_LEN];
+	u8	dest[WLAN_ADDR_LEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_MicFailure_t;
 
 /*--  Unsolicited Frame, MAC Mgmt: PSUserCount (AP Only) --*/
 
 typedef struct hfa384x_PSUserCount
 {
-	UINT16	usercnt;
+	u16	usercnt;
 } __WLAN_ATTRIB_PACK__ hfa384x_PSUserCount_t;
 
 typedef struct hfa384x_KeyIDChanged
 {
-	UINT8	sta_addr[WLAN_ADDR_LEN];
-	UINT16	keyid;
+	u8	sta_addr[WLAN_ADDR_LEN];
+	u16	keyid;
 } __WLAN_ATTRIB_PACK__ hfa384x_KeyIDChanged_t;
 
 /*--  Collection of all Inf frames ---------------*/
@@ -1981,12 +1945,11 @@ typedef union hfa384x_infodata {
 
 typedef struct hfa384x_InfFrame
 {
-	UINT16			framelen;
-	UINT16			infotype;
+	u16			framelen;
+	u16			infotype;
 	hfa384x_infodata_t	info;
 } __WLAN_ATTRIB_PACK__ hfa384x_InfFrame_t;
 
-#if (WLAN_HOSTIF == WLAN_USB)
 /*--------------------------------------------------------------------
 USB Packet structures and constants.
 --------------------------------------------------------------------*/
@@ -2020,46 +1983,46 @@ USB Packet structures and constants.
 
 typedef struct hfa384x_usb_txfrm {
 	hfa384x_tx_frame_t	desc;
-	UINT8			data[WLAN_DATA_MAXLEN];
+	u8			data[WLAN_DATA_MAXLEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_txfrm_t;
 
 typedef struct hfa384x_usb_cmdreq {
-	UINT16		type;
-	UINT16		cmd;
-	UINT16		parm0;
-	UINT16		parm1;
-	UINT16		parm2;
-	UINT8		pad[54];
+	u16		type;
+	u16		cmd;
+	u16		parm0;
+	u16		parm1;
+	u16		parm2;
+	u8		pad[54];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_cmdreq_t;
 
 typedef struct hfa384x_usb_wridreq {
-	UINT16		type;
-	UINT16		frmlen;
-	UINT16		rid;
-	UINT8		data[HFA384x_RIDDATA_MAXLEN];
+	u16		type;
+	u16		frmlen;
+	u16		rid;
+	u8		data[HFA384x_RIDDATA_MAXLEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_wridreq_t;
 
 typedef struct hfa384x_usb_rridreq {
-	UINT16		type;
-	UINT16		frmlen;
-	UINT16		rid;
-	UINT8		pad[58];
+	u16		type;
+	u16		frmlen;
+	u16		rid;
+	u8		pad[58];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_rridreq_t;
 
 typedef struct hfa384x_usb_wmemreq {
-	UINT16		type;
-	UINT16		frmlen;
-	UINT16		offset;
-	UINT16		page;
-	UINT8		data[HFA384x_USB_RWMEM_MAXLEN];
+	u16		type;
+	u16		frmlen;
+	u16		offset;
+	u16		page;
+	u8		data[HFA384x_USB_RWMEM_MAXLEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_wmemreq_t;
 
 typedef struct hfa384x_usb_rmemreq {
-	UINT16		type;
-	UINT16		frmlen;
-	UINT16		offset;
-	UINT16		page;
-	UINT8		pad[56];
+	u16		type;
+	u16		frmlen;
+	u16		offset;
+	u16		page;
+	u8		pad[56];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_rmemreq_t;
 
 /*------------------------------------*/
@@ -2067,54 +2030,54 @@ typedef struct hfa384x_usb_rmemreq {
 
 typedef struct hfa384x_usb_rxfrm {
 	hfa384x_rx_frame_t	desc;
-	UINT8			data[WLAN_DATA_MAXLEN];
+	u8			data[WLAN_DATA_MAXLEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_rxfrm_t;
 
 typedef struct hfa384x_usb_infofrm {
-	UINT16			type;
+	u16			type;
 	hfa384x_InfFrame_t	info;
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_infofrm_t;
 
 typedef struct hfa384x_usb_statusresp {
-	UINT16		type;
-	UINT16		status;
-	UINT16		resp0;
-	UINT16		resp1;
-	UINT16		resp2;
+	u16		type;
+	u16		status;
+	u16		resp0;
+	u16		resp1;
+	u16		resp2;
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_cmdresp_t;
 
 typedef hfa384x_usb_cmdresp_t hfa384x_usb_wridresp_t;
 
 typedef struct hfa384x_usb_rridresp {
-	UINT16		type;
-	UINT16		frmlen;
-	UINT16		rid;
-	UINT8		data[HFA384x_RIDDATA_MAXLEN];
+	u16		type;
+	u16		frmlen;
+	u16		rid;
+	u8		data[HFA384x_RIDDATA_MAXLEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_rridresp_t;
 
 typedef hfa384x_usb_cmdresp_t hfa384x_usb_wmemresp_t;
 
 typedef struct hfa384x_usb_rmemresp {
-	UINT16		type;
-	UINT16		frmlen;
-	UINT8		data[HFA384x_USB_RWMEM_MAXLEN];
+	u16		type;
+	u16		frmlen;
+	u8		data[HFA384x_USB_RWMEM_MAXLEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_rmemresp_t;
 
 typedef struct hfa384x_usb_bufavail {
-	UINT16		type;
-	UINT16		frmlen;
+	u16		type;
+	u16		frmlen;
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_bufavail_t;
 
 typedef struct hfa384x_usb_error {
-	UINT16		type;
-	UINT16		errortype;
+	u16		type;
+	u16		errortype;
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_error_t;
 
 /*----------------------------------------------------------*/
 /* Unions for packaging all the known packet types together */
 
 typedef union hfa384x_usbout {
-	UINT16			type;
+	u16			type;
 	hfa384x_usb_txfrm_t	txfrm;
 	hfa384x_usb_cmdreq_t	cmdreq;
 	hfa384x_usb_wridreq_t	wridreq;
@@ -2124,7 +2087,7 @@ typedef union hfa384x_usbout {
 } __WLAN_ATTRIB_PACK__ hfa384x_usbout_t;
 
 typedef union hfa384x_usbin {
-	UINT16			type;
+	u16			type;
 	hfa384x_usb_rxfrm_t	rxfrm;
 	hfa384x_usb_txfrm_t	txfrm;
 	hfa384x_usb_infofrm_t	infofrm;
@@ -2135,28 +2098,26 @@ typedef union hfa384x_usbin {
 	hfa384x_usb_rmemresp_t	rmemresp;
 	hfa384x_usb_bufavail_t	bufavail;
 	hfa384x_usb_error_t	usberror;
-	UINT8			boguspad[3000];
+	u8			boguspad[3000];
 } __WLAN_ATTRIB_PACK__ hfa384x_usbin_t;
 
-#endif /* WLAN_USB */
-
 /*--------------------------------------------------------------------
 PD record structures.
 --------------------------------------------------------------------*/
 
 typedef struct hfa384x_pdr_pcb_partnum
 {
-	UINT8	num[8];
+	u8	num[8];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_pcb_partnum_t;
 
 typedef struct hfa384x_pdr_pcb_tracenum
 {
-	UINT8	num[8];
+	u8	num[8];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_pcb_tracenum_t;
 
 typedef struct hfa384x_pdr_nic_serial
 {
-	UINT8	num[12];
+	u8	num[12];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_nic_serial_t;
 
 typedef struct hfa384x_pdr_mkk_measurements
@@ -2180,170 +2141,170 @@ typedef struct hfa384x_pdr_mkk_measurements
 
 typedef struct hfa384x_pdr_nic_ramsize
 {
-	UINT8	size[12]; /* units of KB */
+	u8	size[12]; /* units of KB */
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_nic_ramsize_t;
 
 typedef struct hfa384x_pdr_mfisuprange
 {
-	UINT16	id;
-	UINT16	variant;
-	UINT16	bottom;
-	UINT16	top;
+	u16	id;
+	u16	variant;
+	u16	bottom;
+	u16	top;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_mfisuprange_t;
 
 typedef struct hfa384x_pdr_cfisuprange
 {
-	UINT16	id;
-	UINT16	variant;
-	UINT16	bottom;
-	UINT16	top;
+	u16	id;
+	u16	variant;
+	u16	bottom;
+	u16	top;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_cfisuprange_t;
 
 typedef struct hfa384x_pdr_nicid
 {
-	UINT16	id;
-	UINT16	variant;
-	UINT16	major;
-	UINT16	minor;
+	u16	id;
+	u16	variant;
+	u16	major;
+	u16	minor;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_nicid_t;
 
 
 typedef struct hfa384x_pdr_refdac_measurements
 {
-	UINT16	value[0];
+	u16	value[0];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_refdac_measurements_t;
 
 typedef struct hfa384x_pdr_vgdac_measurements
 {
-	UINT16	value[0];
+	u16	value[0];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_vgdac_measurements_t;
 
 typedef struct hfa384x_pdr_level_comp_measurements
 {
-	UINT16	value[0];
+	u16	value[0];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_level_compc_measurements_t;
 
 typedef struct hfa384x_pdr_mac_address
 {
-	UINT8	addr[6];
+	u8	addr[6];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_mac_address_t;
 
 typedef struct hfa384x_pdr_mkk_callname
 {
-	UINT8	callname[8];
+	u8	callname[8];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_mkk_callname_t;
 
 typedef struct hfa384x_pdr_regdomain
 {
-	UINT16	numdomains;
-	UINT16	domain[5];
+	u16	numdomains;
+	u16	domain[5];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_regdomain_t;
 
 typedef struct hfa384x_pdr_allowed_channel
 {
-	UINT16	ch_bitmap;
+	u16	ch_bitmap;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_allowed_channel_t;
 
 typedef struct hfa384x_pdr_default_channel
 {
-	UINT16	channel;
+	u16	channel;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_default_channel_t;
 
 typedef struct hfa384x_pdr_privacy_option
 {
-	UINT16	available;
+	u16	available;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_privacy_option_t;
 
 typedef struct hfa384x_pdr_temptype
 {
-	UINT16	type;
+	u16	type;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_temptype_t;
 
 typedef struct hfa384x_pdr_refdac_setup
 {
-	UINT16	ch_value[14];
+	u16	ch_value[14];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_refdac_setup_t;
 
 typedef struct hfa384x_pdr_vgdac_setup
 {
-	UINT16	ch_value[14];
+	u16	ch_value[14];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_vgdac_setup_t;
 
 typedef struct hfa384x_pdr_level_comp_setup
 {
-	UINT16	ch_value[14];
+	u16	ch_value[14];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_level_comp_setup_t;
 
 typedef struct hfa384x_pdr_trimdac_setup
 {
-	UINT16	trimidac;
-	UINT16	trimqdac;
+	u16	trimidac;
+	u16	trimqdac;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_trimdac_setup_t;
 
 typedef struct hfa384x_pdr_ifr_setting
 {
-	UINT16	value[3];
+	u16	value[3];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_ifr_setting_t;
 
 typedef struct hfa384x_pdr_rfr_setting
 {
-	UINT16	value[3];
+	u16	value[3];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_rfr_setting_t;
 
 typedef struct hfa384x_pdr_hfa3861_baseline
 {
-	UINT16	value[50];
+	u16	value[50];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_hfa3861_baseline_t;
 
 typedef struct hfa384x_pdr_hfa3861_shadow
 {
-	UINT32	value[32];
+	u32	value[32];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_hfa3861_shadow_t;
 
 typedef struct hfa384x_pdr_hfa3861_ifrf
 {
-	UINT32	value[20];
+	u32	value[20];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_hfa3861_ifrf_t;
 
 typedef struct hfa384x_pdr_hfa3861_chcalsp
 {
-	UINT16	value[14];
+	u16	value[14];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_hfa3861_chcalsp_t;
 
 typedef struct hfa384x_pdr_hfa3861_chcali
 {
-	UINT16	value[17];
+	u16	value[17];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_hfa3861_chcali_t;
 
 typedef struct hfa384x_pdr_hfa3861_nic_config
 {
-	UINT16	config_bitmap;
+	u16	config_bitmap;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_nic_config_t;
 
 typedef struct hfa384x_pdr_hfo_delay
 {
-	UINT8   hfo_delay;
+	u8   hfo_delay;
 } __WLAN_ATTRIB_PACK__ hfa384x_hfo_delay_t;
 
 typedef struct hfa384x_pdr_hfa3861_manf_testsp
 {
-	UINT16	value[30];
+	u16	value[30];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_hfa3861_manf_testsp_t;
 
 typedef struct hfa384x_pdr_hfa3861_manf_testi
 {
-	UINT16	value[30];
+	u16	value[30];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_hfa3861_manf_testi_t;
 
 typedef struct hfa384x_end_of_pda
 {
-	UINT16	crc;
+	u16	crc;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_end_of_pda_t;
 
 typedef struct hfa384x_pdrec
 {
-	UINT16	len; /* in words */
-	UINT16	code;
+	u16	len; /* in words */
+	u16	code;
 	union pdr {
 	hfa384x_pdr_pcb_partnum_t	pcb_partnum;
 	hfa384x_pdr_pcb_tracenum_t	pcb_tracenum;
@@ -2391,14 +2352,12 @@ typedef struct hfa384x_pdrec
 --------------------------------------------------------------------*/
 typedef struct hfa384x_statusresult
 {
-	UINT16	status;
-	UINT16	resp0;
-	UINT16	resp1;
-	UINT16	resp2;
+	u16	status;
+	u16	resp0;
+	u16	resp1;
+	u16	resp2;
 } hfa384x_cmdresult_t;
 
-#if (WLAN_HOSTIF == WLAN_USB)
-
 /* USB Control Exchange (CTLX):
  *  A queue of the structure below is maintained for all of the
  *  Request/Response type USB packets supported by Prism2.
@@ -2411,9 +2370,9 @@ typedef hfa384x_cmdresult_t hfa384x_wmemresult_t;
 
 typedef struct hfa384x_rridresult
 {
-	UINT16		rid;
+	u16		rid;
 	const void	*riddata;
-	UINT		riddata_len;
+	unsigned int		riddata_len;
 } hfa384x_rridresult_t;
 
 enum ctlx_state {
@@ -2467,21 +2426,14 @@ typedef struct hfa384x_usbctlxq
 	struct list_head	completing;
 	struct list_head	reapable;
 } hfa384x_usbctlxq_t;
-#endif
 
 typedef struct hfa484x_metacmd
 {
-	UINT16		cmd;
-
-	UINT16          parm0;
-	UINT16          parm1;
-	UINT16          parm2;
+	u16		cmd;
 
-#if 0 //XXX cmd irq stuff
-	UINT16          bulkid;         /* what RID/FID to copy down. */
-	int             bulklen;        /* how much to copy from BAP */
-        char            *bulkdata;      /* And to where? */
-#endif
+	u16          parm0;
+	u16          parm1;
+	u16          parm2;
 
 	hfa384x_cmdresult_t result;
 } hfa384x_metacmd_t;
@@ -2507,28 +2459,22 @@ typedef struct hfa484x_metacmd
 /* XXX These are going away ASAP */
 typedef struct prism2sta_authlist
 {
-	UINT	cnt;
-	UINT8	addr[WLAN_AUTH_MAX][WLAN_ADDR_LEN];
-	UINT8	assoc[WLAN_AUTH_MAX];
+	unsigned int	cnt;
+	u8	addr[WLAN_AUTH_MAX][WLAN_ADDR_LEN];
+	u8	assoc[WLAN_AUTH_MAX];
 } prism2sta_authlist_t;
 
 typedef struct prism2sta_accesslist
 {
-	UINT	modify;
-	UINT	cnt;
-	UINT8	addr[WLAN_ACCESS_MAX][WLAN_ADDR_LEN];
-	UINT	cnt1;
-	UINT8	addr1[WLAN_ACCESS_MAX][WLAN_ADDR_LEN];
+	unsigned int	modify;
+	unsigned int	cnt;
+	u8	addr[WLAN_ACCESS_MAX][WLAN_ADDR_LEN];
+	unsigned int	cnt1;
+	u8	addr1[WLAN_ACCESS_MAX][WLAN_ADDR_LEN];
 } prism2sta_accesslist_t;
 
 typedef struct hfa384x
 {
-#if (WLAN_HOSTIF != WLAN_USB)
-	/* Resource config */
-	UINT32			iobase;
-	char			__iomem *membase;
-	UINT32			irq;
-#else
 	/* USB support data */
 	struct usb_device	*usb;
 	struct urb		rx_urb;
@@ -2560,16 +2506,6 @@ typedef struct hfa384x
 
 	int                     endp_in;
 	int                     endp_out;
-#endif /* !USB */
-
-#if (WLAN_HOSTIF == WLAN_PCMCIA)
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	struct pcmcia_device *pdev;
-#else
-	dev_link_t	*link;
-#endif
-	dev_node_t	node;
-#endif
 
 	int                     sniff_fcs;
 	int                     sniff_channel;
@@ -2579,36 +2515,14 @@ typedef struct hfa384x
 	wait_queue_head_t cmdq;	        /* wait queue itself */
 
 	/* Controller state */
-	UINT32		state;
-	UINT32		isap;
-	UINT8		port_enabled[HFA384x_NUMPORTS_MAX];
-#if (WLAN_HOSTIF != WLAN_USB)
-	UINT		auxen;
-	UINT            isram16;
-#endif /* !USB */
+	u32		state;
+	u32		isap;
+	u8		port_enabled[HFA384x_NUMPORTS_MAX];
 
 	/* Download support */
-	UINT				dlstate;
+	unsigned int				dlstate;
 	hfa384x_downloadbuffer_t	bufinfo;
-	UINT16				dltimeout;
-
-#if (WLAN_HOSTIF != WLAN_USB)
-	spinlock_t	cmdlock;
-	volatile int    cmdflag;        /* wait queue flag */
-	hfa384x_metacmd_t *cmddata;      /* for our async callback */
-
-	/* BAP support */
-	spinlock_t	baplock;
-	struct tasklet_struct   bap_tasklet;
-
-	/* MAC buffer ids */
-        UINT16          txfid_head;
-        UINT16          txfid_tail;
-        UINT            txfid_N;
-        UINT16          txfid_queue[HFA384x_DRVR_FIDSTACKLEN_MAX];
-	UINT16			infofid;
-	struct semaphore	infofid_sem;
-#endif /* !USB */
+	u16				dltimeout;
 
 	int                          scanflag;    /* to signal scan comlete */
 	int                          join_ap;        /* are we joined to a specific ap */
@@ -2623,31 +2537,30 @@ typedef struct hfa384x
 	hfa384x_commsquality_t  qual;
 	struct timer_list	commsqual_timer;
 
-	UINT16 link_status;
-	UINT16 link_status_new;
+	u16 link_status;
+	u16 link_status_new;
 	struct sk_buff_head        authq;
 
 	/* And here we have stuff that used to be in priv */
 
 	/* State variables */
-	UINT		presniff_port_type;
-	UINT16		presniff_wepflags;
-	UINT32		dot11_desired_bss_type;
-	int		ap;	/* AP flag: 0 - Station, 1 - Access Point. */
+	unsigned int		presniff_port_type;
+	u16		presniff_wepflags;
+	u32		dot11_desired_bss_type;
 
 	int             dbmadjust;
 
 	/* Group Addresses - right now, there are up to a total
 	of MAX_GRP_ADDR group addresses */
-	UINT8		dot11_grp_addr[MAX_GRP_ADDR][WLAN_ADDR_LEN];
-	UINT		dot11_grpcnt;
+	u8		dot11_grp_addr[MAX_GRP_ADDR][WLAN_ADDR_LEN];
+	unsigned int		dot11_grpcnt;
 
 	/* Component Identities */
 	hfa384x_compident_t	ident_nic;
 	hfa384x_compident_t	ident_pri_fw;
 	hfa384x_compident_t	ident_sta_fw;
 	hfa384x_compident_t	ident_ap_fw;
-	UINT16			mm_mods;
+	u16			mm_mods;
 
 	/* Supplier compatibility ranges */
 	hfa384x_caplevel_t	cap_sup_mfi;
@@ -2663,14 +2576,14 @@ typedef struct hfa384x
 	hfa384x_caplevel_t	cap_act_ap_cfi;  /* ap f/w to controller interface */
 	hfa384x_caplevel_t	cap_act_ap_mfi;  /* ap f/w to modem interface */
 
-	UINT32			psusercount;  /* Power save user count. */
+	u32			psusercount;  /* Power save user count. */
 	hfa384x_CommTallies32_t	tallies;      /* Communication tallies. */
-	UINT8			comment[WLAN_COMMENT_MAX+1]; /* User comment */
+	u8			comment[WLAN_COMMENT_MAX+1]; /* User comment */
 
 	/* Channel Info request results (AP only) */
 	struct {
 		atomic_t		done;
-		UINT8			count;
+		u8			count;
 		hfa384x_ChInfoResult_t	results;
 	} channel_info;
 
@@ -2678,7 +2591,7 @@ typedef struct hfa384x
 
 
         prism2sta_authlist_t	authlist;     /* Authenticated station list. */
-	UINT			accessmode;   /* Access mode. */
+	unsigned int			accessmode;   /* Access mode. */
         prism2sta_accesslist_t	allow;        /* Allowed station list. */
         prism2sta_accesslist_t	deny;         /* Denied station list. */
 
@@ -2687,24 +2600,13 @@ typedef struct hfa384x
 /*=============================================================*/
 /*--- Function Declarations -----------------------------------*/
 /*=============================================================*/
-#if (WLAN_HOSTIF == WLAN_USB)
 void
 hfa384x_create(
 	hfa384x_t *hw,
 	struct usb_device *usb);
-#else
-void
-hfa384x_create(
-	hfa384x_t *hw,
-	UINT irq,
-	UINT32 iobase,
-	UINT8 __iomem *membase);
-#endif
 
 void hfa384x_destroy(hfa384x_t *hw);
 
-irqreturn_t
-hfa384x_interrupt(int irq, void *dev_id PT_REGS);
 int
 hfa384x_corereset( hfa384x_t *hw, int holdtime, int settletime, int genesis);
 int
@@ -2712,116 +2614,105 @@ hfa384x_drvr_chinforesults( hfa384x_t *hw);
 int
 hfa384x_drvr_commtallies( hfa384x_t *hw);
 int
-hfa384x_drvr_disable(hfa384x_t *hw, UINT16 macport);
+hfa384x_drvr_disable(hfa384x_t *hw, u16 macport);
 int
-hfa384x_drvr_enable(hfa384x_t *hw, UINT16 macport);
+hfa384x_drvr_enable(hfa384x_t *hw, u16 macport);
 int
 hfa384x_drvr_flashdl_enable(hfa384x_t *hw);
 int
 hfa384x_drvr_flashdl_disable(hfa384x_t *hw);
 int
-hfa384x_drvr_flashdl_write(hfa384x_t *hw, UINT32 daddr, void* buf, UINT32 len);
+hfa384x_drvr_flashdl_write(hfa384x_t *hw, u32 daddr, void* buf, u32 len);
 int
-hfa384x_drvr_getconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len);
+hfa384x_drvr_getconfig(hfa384x_t *hw, u16 rid, void *buf, u16 len);
 int
-hfa384x_drvr_handover( hfa384x_t *hw, UINT8 *addr);
+hfa384x_drvr_handover( hfa384x_t *hw, u8 *addr);
 int
 hfa384x_drvr_hostscanresults( hfa384x_t *hw);
 int
 hfa384x_drvr_low_level(hfa384x_t *hw, hfa384x_metacmd_t *cmd);
 int
-hfa384x_drvr_mmi_read(hfa384x_t *hw, UINT32 address, UINT32 *result);
+hfa384x_drvr_mmi_read(hfa384x_t *hw, u32 address, u32 *result);
 int
-hfa384x_drvr_mmi_write(hfa384x_t *hw, UINT32 address, UINT32 data);
+hfa384x_drvr_mmi_write(hfa384x_t *hw, u32 address, u32 data);
 int
-hfa384x_drvr_ramdl_enable(hfa384x_t *hw, UINT32 exeaddr);
+hfa384x_drvr_ramdl_enable(hfa384x_t *hw, u32 exeaddr);
 int
 hfa384x_drvr_ramdl_disable(hfa384x_t *hw);
 int
-hfa384x_drvr_ramdl_write(hfa384x_t *hw, UINT32 daddr, void* buf, UINT32 len);
+hfa384x_drvr_ramdl_write(hfa384x_t *hw, u32 daddr, void* buf, u32 len);
 int
-hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, UINT len);
+hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, unsigned int len);
 int
 hfa384x_drvr_scanresults( hfa384x_t *hw);
 
 int
-hfa384x_drvr_setconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len);
+hfa384x_drvr_setconfig(hfa384x_t *hw, u16 rid, void *buf, u16 len);
 
 static inline int
-hfa384x_drvr_getconfig16(hfa384x_t *hw, UINT16 rid, void *val)
+hfa384x_drvr_getconfig16(hfa384x_t *hw, u16 rid, void *val)
 {
 	int		result = 0;
-	result = hfa384x_drvr_getconfig(hw, rid, val, sizeof(UINT16));
+	result = hfa384x_drvr_getconfig(hw, rid, val, sizeof(u16));
 	if ( result == 0 ) {
-		*((UINT16*)val) = hfa384x2host_16(*((UINT16*)val));
+		*((u16*)val) = hfa384x2host_16(*((u16*)val));
 	}
 	return result;
 }
 
 static inline int
-hfa384x_drvr_getconfig32(hfa384x_t *hw, UINT16 rid, void *val)
+hfa384x_drvr_getconfig32(hfa384x_t *hw, u16 rid, void *val)
 {
 	int		result = 0;
 
-	result = hfa384x_drvr_getconfig(hw, rid, val, sizeof(UINT32));
+	result = hfa384x_drvr_getconfig(hw, rid, val, sizeof(u32));
 	if ( result == 0 ) {
-		*((UINT32*)val) = hfa384x2host_32(*((UINT32*)val));
+		*((u32*)val) = hfa384x2host_32(*((u32*)val));
 	}
 
 	return result;
 }
 
 static inline int
-hfa384x_drvr_setconfig16(hfa384x_t *hw, UINT16 rid, UINT16 val)
+hfa384x_drvr_setconfig16(hfa384x_t *hw, u16 rid, u16 val)
 {
-	UINT16 value = host2hfa384x_16(val);
+	u16 value = host2hfa384x_16(val);
 	return hfa384x_drvr_setconfig(hw, rid, &value, sizeof(value));
 }
 
 static inline int
-hfa384x_drvr_setconfig32(hfa384x_t *hw, UINT16 rid, UINT32 val)
+hfa384x_drvr_setconfig32(hfa384x_t *hw, u16 rid, u32 val)
 {
-	UINT32 value = host2hfa384x_32(val);
+	u32 value = host2hfa384x_32(val);
 	return hfa384x_drvr_setconfig(hw, rid, &value, sizeof(value));
 }
 
-#if (WLAN_HOSTIF == WLAN_USB)
 int
 hfa384x_drvr_getconfig_async(hfa384x_t     *hw,
-                              UINT16        rid,
+                              u16        rid,
                               ctlx_usercb_t usercb,
                               void          *usercb_data);
 
 int
 hfa384x_drvr_setconfig_async(hfa384x_t *hw,
-                              UINT16 rid,
+                              u16 rid,
                               void *buf,
-                              UINT16 len,
+                              u16 len,
                               ctlx_usercb_t usercb,
                               void *usercb_data);
-#else
-static inline int
-hfa384x_drvr_setconfig_async(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len,
-			     void *ptr1, void *ptr2)
-{
-         (void)ptr1;
-         (void)ptr2;
-         return hfa384x_drvr_setconfig(hw, rid, buf, len);
-}
-#endif
 
 static inline int
-hfa384x_drvr_setconfig16_async(hfa384x_t *hw, UINT16 rid, UINT16 val)
+hfa384x_drvr_setconfig16_async(hfa384x_t *hw, u16 rid, u16 val)
 {
-	UINT16 value = host2hfa384x_16(val);
+	u16 value = host2hfa384x_16(val);
 	return hfa384x_drvr_setconfig_async(hw, rid, &value, sizeof(value),
 					    NULL , NULL);
 }
 
 static inline int
-hfa384x_drvr_setconfig32_async(hfa384x_t *hw, UINT16 rid, UINT32 val)
+hfa384x_drvr_setconfig32_async(hfa384x_t *hw, u16 rid, u32 val)
 {
-	UINT32 value = host2hfa384x_32(val);
+	u32 value = host2hfa384x_32(val);
 	return hfa384x_drvr_setconfig_async(hw, rid, &value, sizeof(value),
 					    NULL , NULL);
 }
@@ -2839,32 +2730,28 @@ hfa384x_tx_timeout(wlandevice_t *wlandev);
 int
 hfa384x_cmd_initialize(hfa384x_t *hw);
 int
-hfa384x_cmd_enable(hfa384x_t *hw, UINT16 macport);
+hfa384x_cmd_enable(hfa384x_t *hw, u16 macport);
 int
-hfa384x_cmd_disable(hfa384x_t *hw, UINT16 macport);
+hfa384x_cmd_disable(hfa384x_t *hw, u16 macport);
 int
 hfa384x_cmd_diagnose(hfa384x_t *hw);
 int
-hfa384x_cmd_allocate(hfa384x_t *hw, UINT16 len);
-int
-hfa384x_cmd_transmit(hfa384x_t *hw, UINT16 reclaim, UINT16 qos, UINT16 fid);
-int
-hfa384x_cmd_clearpersist(hfa384x_t *hw, UINT16 fid);
+hfa384x_cmd_allocate(hfa384x_t *hw, u16 len);
 int
-hfa384x_cmd_notify(hfa384x_t *hw, UINT16 reclaim, UINT16 fid, void *buf, UINT16 len);
+hfa384x_cmd_transmit(hfa384x_t *hw, u16 reclaim, u16 qos, u16 fid);
 int
-hfa384x_cmd_inquire(hfa384x_t *hw, UINT16 fid);
+hfa384x_cmd_clearpersist(hfa384x_t *hw, u16 fid);
 int
-hfa384x_cmd_access(hfa384x_t *hw, UINT16 write, UINT16 rid, void *buf, UINT16 len);
+hfa384x_cmd_access(hfa384x_t *hw, u16 write, u16 rid, void *buf, u16 len);
 int
-hfa384x_cmd_monitor(hfa384x_t *hw, UINT16 enable);
+hfa384x_cmd_monitor(hfa384x_t *hw, u16 enable);
 int
 hfa384x_cmd_download(
 	hfa384x_t *hw,
-	UINT16 mode,
-	UINT16 lowaddr,
-	UINT16 highaddr,
-	UINT16 codelen);
+	u16 mode,
+	u16 lowaddr,
+	u16 highaddr,
+	u16 codelen);
 int
 hfa384x_cmd_aux_enable(hfa384x_t *hw, int force);
 int
@@ -2872,196 +2759,34 @@ hfa384x_cmd_aux_disable(hfa384x_t *hw);
 int
 hfa384x_copy_from_bap(
 	hfa384x_t *hw,
-	UINT16	bap,
-	UINT16	id,
-	UINT16	offset,
+	u16	bap,
+	u16	id,
+	u16	offset,
 	void	*buf,
-	UINT	len);
+	unsigned int	len);
 int
 hfa384x_copy_to_bap(
 	hfa384x_t *hw,
-	UINT16	bap,
-	UINT16	id,
-	UINT16	offset,
+	u16	bap,
+	u16	id,
+	u16	offset,
 	void	*buf,
-	UINT	len);
+	unsigned int	len);
 void
 hfa384x_copy_from_aux(
 	hfa384x_t *hw,
-	UINT32	cardaddr,
-	UINT32	auxctl,
+	u32	cardaddr,
+	u32	auxctl,
 	void	*buf,
-	UINT	len);
+	unsigned int	len);
 void
 hfa384x_copy_to_aux(
 	hfa384x_t *hw,
-	UINT32	cardaddr,
-	UINT32	auxctl,
+	u32	cardaddr,
+	u32	auxctl,
 	void	*buf,
-	UINT	len);
-
-#if (WLAN_HOSTIF != WLAN_USB)
-
-/*
-   HFA384x is a LITTLE ENDIAN part.
-
-   the get/setreg functions implicitly byte-swap the data to LE.
-   the _noswap variants do not perform a byte-swap on the data.
-*/
-
-static inline UINT16
-__hfa384x_getreg(hfa384x_t *hw, UINT reg);
-
-static inline void
-__hfa384x_setreg(hfa384x_t *hw, UINT16 val, UINT reg);
-
-static inline UINT16
-__hfa384x_getreg_noswap(hfa384x_t *hw, UINT reg);
-
-static inline void
-__hfa384x_setreg_noswap(hfa384x_t *hw, UINT16 val, UINT reg);
-
-#ifdef REVERSE_ENDIAN
-#define hfa384x_getreg __hfa384x_getreg_noswap
-#define hfa384x_setreg __hfa384x_setreg_noswap
-#define hfa384x_getreg_noswap __hfa384x_getreg
-#define hfa384x_setreg_noswap __hfa384x_setreg
-#else
-#define hfa384x_getreg __hfa384x_getreg
-#define hfa384x_setreg __hfa384x_setreg
-#define hfa384x_getreg_noswap __hfa384x_getreg_noswap
-#define hfa384x_setreg_noswap __hfa384x_setreg_noswap
-#endif
-
-/*----------------------------------------------------------------
-* hfa384x_getreg
-*
-* Retrieve the value of one of the MAC registers.  Done here
-* because different PRISM2 MAC parts use different buses and such.
-* NOTE: This function returns the value in HOST ORDER!!!!!!
-*
-* Arguments:
-*       hw         MAC part structure
-*       reg        Register identifier (offset for I/O based i/f)
-*
-* Returns:
-*       Value from the register in HOST ORDER!!!!
-----------------------------------------------------------------*/
-static inline UINT16
-__hfa384x_getreg(hfa384x_t *hw, UINT reg)
-{
-/*	printk(KERN_DEBUG "Reading from 0x%0x\n", hw->membase + reg); */
-#if ((WLAN_HOSTIF == WLAN_PCMCIA) || (WLAN_HOSTIF == WLAN_PLX))
-	return wlan_inw_le16_to_cpu(hw->iobase+reg);
-#elif (WLAN_HOSTIF == WLAN_PCI)
-	return __le16_to_cpu(readw(hw->membase + reg));
-#endif
-}
-
-/*----------------------------------------------------------------
-* hfa384x_setreg
-*
-* Set the value of one of the MAC registers.  Done here
-* because different PRISM2 MAC parts use different buses and such.
-* NOTE: This function assumes the value is in HOST ORDER!!!!!!
-*
-* Arguments:
-*       hw	MAC part structure
-*	val	Value, in HOST ORDER!!, to put in the register
-*       reg	Register identifier (offset for I/O based i/f)
-*
-* Returns:
-*       Nothing
-----------------------------------------------------------------*/
-static inline void
-__hfa384x_setreg(hfa384x_t *hw, UINT16 val, UINT reg)
-{
-#if ((WLAN_HOSTIF == WLAN_PCMCIA) || (WLAN_HOSTIF == WLAN_PLX))
-	wlan_outw_cpu_to_le16( val, hw->iobase + reg);
-	return;
-#elif (WLAN_HOSTIF == WLAN_PCI)
-	writew(__cpu_to_le16(val), hw->membase + reg);
-	return;
-#endif
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_getreg_noswap
-*
-* Retrieve the value of one of the MAC registers.  Done here
-* because different PRISM2 MAC parts use different buses and such.
-*
-* Arguments:
-*       hw         MAC part structure
-*       reg        Register identifier (offset for I/O based i/f)
-*
-* Returns:
-*       Value from the register.
-----------------------------------------------------------------*/
-static inline UINT16
-__hfa384x_getreg_noswap(hfa384x_t *hw, UINT reg)
-{
-#if ((WLAN_HOSTIF == WLAN_PCMCIA) || (WLAN_HOSTIF == WLAN_PLX))
-	return wlan_inw(hw->iobase+reg);
-#elif (WLAN_HOSTIF == WLAN_PCI)
-	return readw(hw->membase + reg);
-#endif
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_setreg_noswap
-*
-* Set the value of one of the MAC registers.  Done here
-* because different PRISM2 MAC parts use different buses and such.
-*
-* Arguments:
-*       hw	MAC part structure
-*	val	Value to put in the register
-*       reg	Register identifier (offset for I/O based i/f)
-*
-* Returns:
-*       Nothing
-----------------------------------------------------------------*/
-static inline void
-__hfa384x_setreg_noswap(hfa384x_t *hw, UINT16 val, UINT reg)
-{
-#if ((WLAN_HOSTIF == WLAN_PCMCIA) || (WLAN_HOSTIF == WLAN_PLX))
-	wlan_outw( val, hw->iobase + reg);
-	return;
-#elif (WLAN_HOSTIF == WLAN_PCI)
-	writew(val, hw->membase + reg);
-	return;
-#endif
-}
-
-
-static inline void hfa384x_events_all(hfa384x_t *hw)
-{
-	hfa384x_setreg(hw,
-		       HFA384x_INT_NORMAL
-#ifdef CMD_IRQ
-		       | HFA384x_INTEN_CMD_SET(1)
-#endif
-		       ,
-		       HFA384x_INTEN);
-
-}
-
-static inline void hfa384x_events_nobap(hfa384x_t *hw)
-{
-	hfa384x_setreg(hw,
-		        (HFA384x_INT_NORMAL & ~HFA384x_INT_BAP_OP)
-#ifdef CMD_IRQ
-		       | HFA384x_INTEN_CMD_SET(1)
-#endif
-		       ,
-		       HFA384x_INTEN);
-
-}
+	unsigned int	len);
 
-#endif /* WLAN_HOSTIF != WLAN_USB */
 #endif /* __KERNEL__ */
 
 #endif  /* _HFA384x_H */
diff --git a/drivers/staging/wlan-ng/hfa384x_usb.c b/drivers/staging/wlan-ng/hfa384x_usb.c
index db0c502f5d90..8a75b50f8635 100644
--- a/drivers/staging/wlan-ng/hfa384x_usb.c
+++ b/drivers/staging/wlan-ng/hfa384x_usb.c
@@ -114,9 +114,6 @@
 /* System Includes */
 #define WLAN_DBVAR	prism2_debug
 
-#include "version.h"
-
-
 #include <linux/version.h>
 
 #include <linux/module.h>
@@ -136,63 +133,7 @@
 
 #include "wlan_compat.h"
 
-#if (WLAN_HOSTIF != WLAN_USB)
-#error "This file is specific to USB"
-#endif
-
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,10)
-static int
-wait_for_completion_interruptible(struct completion *x)
-{
-  int ret = 0;
-
-  might_sleep();
-
-  spin_lock_irq(&x->wait.lock);
-  if (!x->done) {
-    DECLARE_WAITQUEUE(wait, current);
-
-    wait.flags |= WQ_FLAG_EXCLUSIVE;
-    __add_wait_queue_tail(&x->wait, &wait);
-    do {
-      if (signal_pending(current)) {
-        ret = -ERESTARTSYS;
-        __remove_wait_queue(&x->wait, &wait);
-        goto out;
-      }
-      __set_current_state(TASK_INTERRUPTIBLE);
-      spin_unlock_irq(&x->wait.lock);
-      schedule();
-      spin_lock_irq(&x->wait.lock);
-    } while (!x->done);
-    __remove_wait_queue(&x->wait, &wait);
-  }
-  x->done--;
-out:
-  spin_unlock_irq(&x->wait.lock);
-
-  return ret;
-}
-#endif
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,69)
-static void
-usb_init_urb(struct urb *urb)
-{
-	memset(urb, 0, sizeof(*urb));
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0) /* tune me! */
-	urb->count = (atomic_t)ATOMIC_INIT(1);
-#endif
-	spin_lock_init(&urb->lock);
-}
-#endif
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0) /* tune me! */
-#  define SUBMIT_URB(u,f)  usb_submit_urb(u,f)
-#else
-#  define SUBMIT_URB(u,f)  usb_submit_urb(u)
-#endif
+#define SUBMIT_URB(u,f)  usb_submit_urb(u,f)
 
 /*================================================================*/
 /* Project Includes */
@@ -257,21 +198,12 @@ submit_tx_urb(hfa384x_t *hw, struct urb *tx_urb, gfp_t flags);
 
 /*---------------------------------------------------*/
 /* Callbacks */
-#ifdef URB_ONLY_CALLBACK
 static void
 hfa384x_usbout_callback(struct urb *urb);
 static void
 hfa384x_ctlxout_callback(struct urb *urb);
 static void
 hfa384x_usbin_callback(struct urb *urb);
-#else
-static void
-hfa384x_usbout_callback(struct urb *urb, struct pt_regs *regs);
-static void
-hfa384x_ctlxout_callback(struct urb *urb, struct pt_regs *regs);
-static void
-hfa384x_usbin_callback(struct urb *urb, struct pt_regs *regs);
-#endif
 
 static void
 hfa384x_usbin_txcompl(wlandevice_t *wlandev, hfa384x_usbin_t *usbin);
@@ -358,9 +290,9 @@ static int
 hfa384x_dorrid(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	rid,
+	u16	rid,
 	void	*riddata,
-	UINT	riddatalen,
+	unsigned int	riddatalen,
 	ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data);
@@ -369,9 +301,9 @@ static int
 hfa384x_dowrid(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	rid,
+	u16	rid,
 	void	*riddata,
-	UINT	riddatalen,
+	unsigned int	riddatalen,
 	ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data);
@@ -380,10 +312,10 @@ static int
 hfa384x_dormem(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	page,
-	UINT16	offset,
+	u16	page,
+	u16	offset,
 	void	*data,
-	UINT	len,
+	unsigned int	len,
 	ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data);
@@ -392,16 +324,16 @@ static int
 hfa384x_dowmem(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	page,
-	UINT16	offset,
+	u16	page,
+	u16	offset,
 	void	*data,
-	UINT	len,
+	unsigned int	len,
 	ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data);
 
 static int
-hfa384x_isgood_pdrcode(UINT16 pdrcode);
+hfa384x_isgood_pdrcode(u16 pdrcode);
 
 /*================================================================*/
 /* Function Definitions */
@@ -435,17 +367,17 @@ dbprint_urb(struct urb* urb)
 	WLAN_LOG_DEBUG(3,"urb->pipe=0x%08x\n", urb->pipe);
 	WLAN_LOG_DEBUG(3,"urb->status=0x%08x\n", urb->status);
 	WLAN_LOG_DEBUG(3,"urb->transfer_flags=0x%08x\n", urb->transfer_flags);
-	WLAN_LOG_DEBUG(3,"urb->transfer_buffer=0x%08x\n", (UINT)urb->transfer_buffer);
+	WLAN_LOG_DEBUG(3,"urb->transfer_buffer=0x%08x\n", (unsigned int)urb->transfer_buffer);
 	WLAN_LOG_DEBUG(3,"urb->transfer_buffer_length=0x%08x\n", urb->transfer_buffer_length);
 	WLAN_LOG_DEBUG(3,"urb->actual_length=0x%08x\n", urb->actual_length);
 	WLAN_LOG_DEBUG(3,"urb->bandwidth=0x%08x\n", urb->bandwidth);
-	WLAN_LOG_DEBUG(3,"urb->setup_packet(ctl)=0x%08x\n", (UINT)urb->setup_packet);
+	WLAN_LOG_DEBUG(3,"urb->setup_packet(ctl)=0x%08x\n", (unsigned int)urb->setup_packet);
 	WLAN_LOG_DEBUG(3,"urb->start_frame(iso/irq)=0x%08x\n", urb->start_frame);
 	WLAN_LOG_DEBUG(3,"urb->interval(irq)=0x%08x\n", urb->interval);
 	WLAN_LOG_DEBUG(3,"urb->error_count(iso)=0x%08x\n", urb->error_count);
 	WLAN_LOG_DEBUG(3,"urb->timeout=0x%08x\n", urb->timeout);
-	WLAN_LOG_DEBUG(3,"urb->context=0x%08x\n", (UINT)urb->context);
-	WLAN_LOG_DEBUG(3,"urb->complete=0x%08x\n", (UINT)urb->complete);
+	WLAN_LOG_DEBUG(3,"urb->context=0x%08x\n", (unsigned int)urb->context);
+	WLAN_LOG_DEBUG(3,"urb->complete=0x%08x\n", (unsigned int)urb->complete);
 }
 #endif
 
@@ -652,7 +584,7 @@ hfa384x_usb_defer(struct work_struct *data)
 
 	/* Resume transmitting. */
 	if ( test_and_clear_bit(WORK_TX_RESUME, &hw->usb_flags) ) {
-		p80211netdev_wake_queue(hw->wlandev);
+		netif_wake_queue(hw->wlandev->netdev);
 	}
 
 	DBFEXIT;
@@ -711,8 +643,8 @@ hfa384x_create( hfa384x_t *hw, struct usb_device *usb)
 	tasklet_init(&hw->completion_bh,
 	             hfa384x_usbctlx_completion_task,
 	             (unsigned long)hw);
-	INIT_WORK2(&hw->link_bh, prism2sta_processing_defer);
-	INIT_WORK2(&hw->usb_work, hfa384x_usb_defer);
+	INIT_WORK(&hw->link_bh, prism2sta_processing_defer);
+	INIT_WORK(&hw->usb_work, hfa384x_usb_defer);
 
 	init_timer(&hw->throttle);
 	hw->throttle.function = hfa384x_usb_throttlefn;
@@ -733,7 +665,7 @@ hfa384x_create( hfa384x_t *hw, struct usb_device *usb)
 	hw->link_status = HFA384x_LINK_NOTCONNECTED;
 	hw->state = HFA384x_STATE_INIT;
 
-        INIT_WORK2(&hw->commsqual_bh, prism2sta_commsqual_defer);
+        INIT_WORK(&hw->commsqual_bh, prism2sta_commsqual_defer);
 	init_timer(&hw->commsqual_timer);
 	hw->commsqual_timer.data = (unsigned long) hw;
 	hw->commsqual_timer.function = prism2sta_commsqual_timer;
@@ -888,7 +820,7 @@ struct usbctlx_rrid_completor
 
 	const hfa384x_usb_rridresp_t	*rridresp;
 	void			*riddata;
-	UINT			riddatalen;
+	unsigned int			riddatalen;
 };
 typedef struct usbctlx_rrid_completor usbctlx_rrid_completor_t;
 
@@ -919,7 +851,7 @@ static inline usbctlx_completor_t*
 init_rrid_completor(usbctlx_rrid_completor_t *completor,
                     const hfa384x_usb_rridresp_t *rridresp,
                     void *riddata,
-                    UINT riddatalen)
+                    unsigned int riddatalen)
 {
 	completor->head.complete = usbctlx_rrid_completor_fn;
 	completor->rridresp = rridresp;
@@ -952,7 +884,7 @@ struct usbctlx_rmem_completor
 
         const hfa384x_usb_rmemresp_t  *rmemresp;
         void                          *data;
-        UINT                          len;
+        unsigned int                          len;
 };
 typedef struct usbctlx_rmem_completor usbctlx_rmem_completor_t;
 
@@ -969,7 +901,7 @@ static inline usbctlx_completor_t*
 init_rmem_completor(usbctlx_rmem_completor_t *completor,
                     hfa384x_usb_rmemresp_t *rmemresp,
                     void *data,
-                    UINT len)
+                    unsigned int len)
 {
 	completor->head.complete = usbctlx_rmem_completor_fn;
 	completor->rmemresp = rmemresp;
@@ -1080,7 +1012,7 @@ hfa384x_docmd_async(hfa384x_t *hw,
 }
 
 static inline int
-hfa384x_dorrid_wait(hfa384x_t *hw, UINT16 rid, void *riddata, UINT riddatalen)
+hfa384x_dorrid_wait(hfa384x_t *hw, u16 rid, void *riddata, unsigned int riddatalen)
 {
 	return hfa384x_dorrid(hw, DOWAIT,
 	                      rid, riddata, riddatalen,
@@ -1089,7 +1021,7 @@ hfa384x_dorrid_wait(hfa384x_t *hw, UINT16 rid, void *riddata, UINT riddatalen)
 
 static inline int
 hfa384x_dorrid_async(hfa384x_t *hw,
-                     UINT16 rid, void *riddata, UINT riddatalen,
+                     u16 rid, void *riddata, unsigned int riddatalen,
                      ctlx_cmdcb_t cmdcb,
                      ctlx_usercb_t usercb,
                      void *usercb_data)
@@ -1100,7 +1032,7 @@ hfa384x_dorrid_async(hfa384x_t *hw,
 }
 
 static inline int
-hfa384x_dowrid_wait(hfa384x_t *hw, UINT16 rid, void *riddata, UINT riddatalen)
+hfa384x_dowrid_wait(hfa384x_t *hw, u16 rid, void *riddata, unsigned int riddatalen)
 {
 	return hfa384x_dowrid(hw, DOWAIT,
 	                      rid, riddata, riddatalen,
@@ -1109,7 +1041,7 @@ hfa384x_dowrid_wait(hfa384x_t *hw, UINT16 rid, void *riddata, UINT riddatalen)
 
 static inline int
 hfa384x_dowrid_async(hfa384x_t *hw,
-                     UINT16 rid, void *riddata, UINT riddatalen,
+                     u16 rid, void *riddata, unsigned int riddatalen,
                      ctlx_cmdcb_t cmdcb,
                      ctlx_usercb_t usercb,
                      void *usercb_data)
@@ -1121,7 +1053,7 @@ hfa384x_dowrid_async(hfa384x_t *hw,
 
 static inline int
 hfa384x_dormem_wait(hfa384x_t *hw,
-                    UINT16 page, UINT16 offset, void *data, UINT len)
+                    u16 page, u16 offset, void *data, unsigned int len)
 {
 	return hfa384x_dormem(hw, DOWAIT,
 	                      page, offset, data, len,
@@ -1130,7 +1062,7 @@ hfa384x_dormem_wait(hfa384x_t *hw,
 
 static inline int
 hfa384x_dormem_async(hfa384x_t *hw,
-                     UINT16 page, UINT16 offset, void *data, UINT len,
+                     u16 page, u16 offset, void *data, unsigned int len,
                      ctlx_cmdcb_t cmdcb,
                      ctlx_usercb_t usercb,
                      void *usercb_data)
@@ -1143,10 +1075,10 @@ hfa384x_dormem_async(hfa384x_t *hw,
 static inline int
 hfa384x_dowmem_wait(
         hfa384x_t *hw,
-        UINT16  page,
-        UINT16  offset,
+        u16  page,
+        u16  offset,
         void    *data,
-        UINT    len)
+        unsigned int    len)
 {
 	return hfa384x_dowmem(hw, DOWAIT,
                                   page, offset, data, len,
@@ -1156,10 +1088,10 @@ hfa384x_dowmem_wait(
 static inline int
 hfa384x_dowmem_async(
         hfa384x_t *hw,
-        UINT16  page,
-        UINT16  offset,
+        u16  page,
+        u16  offset,
         void    *data,
-        UINT    len,
+        unsigned int    len,
         ctlx_cmdcb_t cmdcb,
         ctlx_usercb_t usercb,
         void    *usercb_data)
@@ -1246,7 +1178,7 @@ hfa384x_cmd_initialize(hfa384x_t *hw)
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_cmd_disable(hfa384x_t *hw, UINT16 macport)
+int hfa384x_cmd_disable(hfa384x_t *hw, u16 macport)
 {
 	int	result = 0;
 	hfa384x_metacmd_t cmd;
@@ -1286,7 +1218,7 @@ int hfa384x_cmd_disable(hfa384x_t *hw, UINT16 macport)
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_cmd_enable(hfa384x_t *hw, UINT16 macport)
+int hfa384x_cmd_enable(hfa384x_t *hw, u16 macport)
 {
 	int	result = 0;
 	hfa384x_metacmd_t cmd;
@@ -1305,95 +1237,6 @@ int hfa384x_cmd_enable(hfa384x_t *hw, UINT16 macport)
 	return result;
 }
 
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_notify
-*
-* Sends an info frame to the firmware to alter the behavior
-* of the f/w asynch processes.  Can only be called when the MAC
-* is in the enabled state.
-*
-* Arguments:
-*	hw		device structure
-*	reclaim		[0|1] indicates whether the given FID will
-*			be handed back (via Alloc event) for reuse.
-*			(host order)
-*	fid		FID of buffer containing the frame that was
-*			previously copied to MAC memory via the bap.
-*			(host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*	hw->resp0 will contain the FID being used by async notify
-*	process.  If reclaim==0, resp0 will be the same as the fid
-*	argument.  If reclaim==1, resp0 will be the different.
-*
-* Call context:
-*	process
-----------------------------------------------------------------*/
-int hfa384x_cmd_notify(hfa384x_t *hw, UINT16 reclaim, UINT16 fid,
-		       void *buf, UINT16 len)
-{
-#if 0
-	int	result = 0;
-	UINT16	cmd;
-	DBFENTER;
-	cmd =	HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_NOTIFY) |
-		HFA384x_CMD_RECL_SET(reclaim);
-	result = hfa384x_docmd_wait(hw, cmd);
-
-	DBFEXIT;
-	return result;
-#endif
-return 0;
-}
-
-
-#if 0
-/*----------------------------------------------------------------
-* hfa384x_cmd_inquiry
-*
-* Requests an info frame from the firmware.  The info frame will
-* be delivered asynchronously via the Info event.
-*
-* Arguments:
-*	hw		device structure
-*	fid		FID of the info frame requested. (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process
-----------------------------------------------------------------*/
-int hfa384x_cmd_inquiry(hfa384x_t *hw, UINT16 fid)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_INQ);
-	cmd.parm0 = 0;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	result = hfa384x_docmd_wait(hw, &cmd);
-
-	DBFEXIT;
-	return result;
-}
-#endif
-
-
 /*----------------------------------------------------------------
 * hfa384x_cmd_monitor
 *
@@ -1423,7 +1266,7 @@ int hfa384x_cmd_inquiry(hfa384x_t *hw, UINT16 fid)
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_cmd_monitor(hfa384x_t *hw, UINT16 enable)
+int hfa384x_cmd_monitor(hfa384x_t *hw, u16 enable)
 {
 	int	result = 0;
 	hfa384x_metacmd_t cmd;
@@ -1481,8 +1324,8 @@ int hfa384x_cmd_monitor(hfa384x_t *hw, UINT16 enable)
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_cmd_download(hfa384x_t *hw, UINT16 mode, UINT16 lowaddr,
-				UINT16 highaddr, UINT16 codelen)
+int hfa384x_cmd_download(hfa384x_t *hw, u16 mode, u16 lowaddr,
+				u16 highaddr, u16 codelen)
 {
 	int	result = 0;
 	hfa384x_metacmd_t cmd;
@@ -1532,7 +1375,7 @@ int hfa384x_cmd_download(hfa384x_t *hw, UINT16 mode, UINT16 lowaddr,
 ----------------------------------------------------------------*/
 void
 hfa384x_copy_from_aux(
-	hfa384x_t *hw, UINT32 cardaddr, UINT32 auxctl, void *buf, UINT len)
+	hfa384x_t *hw, u32 cardaddr, u32 auxctl, void *buf, unsigned int len)
 {
 	DBFENTER;
 	WLAN_LOG_ERROR("not used in USB.\n");
@@ -1566,7 +1409,7 @@ hfa384x_copy_from_aux(
 ----------------------------------------------------------------*/
 void
 hfa384x_copy_to_aux(
-	hfa384x_t *hw, UINT32 cardaddr, UINT32 auxctl, void *buf, UINT len)
+	hfa384x_t *hw, u32 cardaddr, u32 auxctl, void *buf, unsigned int len)
 {
 	DBFENTER;
 	WLAN_LOG_ERROR("not used in USB.\n");
@@ -1599,78 +1442,10 @@ hfa384x_copy_to_aux(
 ----------------------------------------------------------------*/
 int hfa384x_corereset(hfa384x_t *hw, int holdtime, int settletime, int genesis)
 {
-#if 0
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-	struct usb_device	*parent = hw->usb->parent;
-	int			i;
-	int			port = -1;
-#endif
-#endif
 	int 			result = 0;
 
-
-#define P2_USB_RT_PORT		(USB_TYPE_CLASS | USB_RECIP_OTHER)
-#define P2_USB_FEAT_RESET	4
-#define P2_USB_FEAT_C_RESET	20
-
 	DBFENTER;
 
-#if 0
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-	/* Find the hub port */
-	for ( i = 0; i < parent->maxchild; i++) {
-		if (parent->children[i] == hw->usb) {
-			port = i;
-			break;
-		}
-	}
-	if (port < 0) return -ENOENT;
-
-	/* Set and clear the reset */
-	usb_control_msg(parent, usb_sndctrlpipe(parent, 0),
-		USB_REQ_SET_FEATURE, P2_USB_RT_PORT, P2_USB_FEAT_RESET,
-		port+1, NULL, 0, 1*HZ);
-	wait_ms(holdtime);
-	usb_control_msg(parent, usb_sndctrlpipe(parent, 0),
-		USB_REQ_CLEAR_FEATURE, P2_USB_RT_PORT, P2_USB_FEAT_C_RESET,
-		port+1, NULL, 0, 1*HZ);
-	wait_ms(settletime);
-
-	/* Set the device address */
-	result=usb_set_address(hw->usb);
-	if (result < 0) {
-		WLAN_LOG_ERROR("reset_usbdev: Dev not accepting address, "
-			"result=%d\n", result);
-		clear_bit(hw->usb->devnum, &hw->usb->bus->devmap.devicemap);
-		hw->usb->devnum = -1;
-		goto done;
-	}
-	/* Let the address settle */
-	wait_ms(20);
-
-	/* Assume we're reusing the original descriptor data */
-
-	/* Set the configuration. */
-	WLAN_LOG_DEBUG(3, "Setting Configuration %d\n",
-		hw->usb->config[0].bConfigurationValue);
-	result=usb_set_configuration(hw->usb, hw->usb->config[0].bConfigurationValue);
-	if ( result ) {
-		WLAN_LOG_ERROR("usb_set_configuration() failed, result=%d.\n",
-				result);
-		goto done;
-	}
-	/* Let the configuration settle */
-	wait_ms(20);
-
- done:
-#else
-	result=usb_reset_device(hw->usb);
-	if(result<0) {
-		WLAN_LOG_ERROR("usb_reset_device() failed, result=%d.\n",result);
-	}
-#endif
-#endif
-
 	result=usb_reset_device(hw->usb);
 	if(result<0) {
 		WLAN_LOG_ERROR("usb_reset_device() failed, result=%d.\n",result);
@@ -1925,9 +1700,9 @@ static int
 hfa384x_dorrid(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	rid,
+	u16	rid,
 	void	*riddata,
-	UINT	riddatalen,
+	unsigned int	riddatalen,
         ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data)
@@ -2011,9 +1786,9 @@ static int
 hfa384x_dowrid(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	rid,
+	u16	rid,
 	void	*riddata,
-	UINT	riddatalen,
+	unsigned int	riddatalen,
 	ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data)
@@ -2104,10 +1879,10 @@ static int
 hfa384x_dormem(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	page,
-	UINT16	offset,
+	u16	page,
+	u16	offset,
 	void	*data,
-	UINT	len,
+	unsigned int	len,
 	ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data)
@@ -2205,10 +1980,10 @@ static int
 hfa384x_dowmem(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	page,
-	UINT16	offset,
+	u16	page,
+	u16	offset,
 	void	*data,
-	UINT	len,
+	unsigned int	len,
 	ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data)
@@ -2325,7 +2100,7 @@ int hfa384x_drvr_commtallies( hfa384x_t *hw )
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_drvr_disable(hfa384x_t *hw, UINT16 macport)
+int hfa384x_drvr_disable(hfa384x_t *hw, u16 macport)
 {
 	int	result = 0;
 
@@ -2367,7 +2142,7 @@ int hfa384x_drvr_disable(hfa384x_t *hw, UINT16 macport)
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_drvr_enable(hfa384x_t *hw, UINT16 macport)
+int hfa384x_drvr_enable(hfa384x_t *hw, u16 macport)
 {
 	int	result = 0;
 
@@ -2520,22 +2295,22 @@ int hfa384x_drvr_flashdl_disable(hfa384x_t *hw)
 int
 hfa384x_drvr_flashdl_write(
 	hfa384x_t	*hw,
-	UINT32		daddr,
+	u32		daddr,
 	void		*buf,
-	UINT32		len)
+	u32		len)
 {
 	int		result = 0;
-	UINT32		dlbufaddr;
+	u32		dlbufaddr;
 	int		nburns;
-	UINT32		burnlen;
-	UINT32		burndaddr;
-	UINT16		burnlo;
-	UINT16		burnhi;
+	u32		burnlen;
+	u32		burndaddr;
+	u16		burnlo;
+	u16		burnhi;
 	int		nwrites;
-	UINT8		*writebuf;
-	UINT16		writepage;
-	UINT16		writeoffset;
-	UINT32		writelen;
+	u8		*writebuf;
+	u16		writepage;
+	u16		writeoffset;
+	u32		writelen;
 	int		i;
 	int		j;
 
@@ -2686,7 +2461,7 @@ exit_proc:
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_drvr_getconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len)
+int hfa384x_drvr_getconfig(hfa384x_t *hw, u16 rid, void *buf, u16 len)
 {
 	int 			result;
 	DBFENTER;
@@ -2727,7 +2502,7 @@ int hfa384x_drvr_getconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len)
 int
 hfa384x_drvr_getconfig_async(
          hfa384x_t               *hw,
-         UINT16                  rid,
+         u16                  rid,
          ctlx_usercb_t           usercb,
          void                    *usercb_data)
 {
@@ -2761,9 +2536,9 @@ hfa384x_drvr_getconfig_async(
 int
 hfa384x_drvr_setconfig_async(
          hfa384x_t       *hw,
-         UINT16          rid,
+         u16          rid,
          void            *buf,
-         UINT16          len,
+         u16          len,
          ctlx_usercb_t   usercb,
          void            *usercb_data)
 {
@@ -2790,7 +2565,7 @@ hfa384x_drvr_setconfig_async(
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_drvr_handover( hfa384x_t *hw, UINT8 *addr)
+int hfa384x_drvr_handover( hfa384x_t *hw, u8 *addr)
 {
         DBFENTER;
 	WLAN_LOG_ERROR("Not currently supported in USB!\n");
@@ -2824,88 +2599,6 @@ int hfa384x_drvr_low_level(hfa384x_t *hw, hfa384x_metacmd_t *cmd)
 }
 
 /*----------------------------------------------------------------
-* hfa384x_drvr_mmi_read
-*
-* Read mmi registers.  mmi is intersil-speak for the baseband
-* processor registers.
-*
-* Arguments:
-*       hw              device structure
-*       register        The test register to be accessed (must be even #).
-*
-* Returns:
-*       0               success
-*       >0              f/w reported error - f/w status code
-*       <0              driver reported error
-*
-* Side effects:
-*
-* Call context:
-*       process
-----------------------------------------------------------------*/
-int hfa384x_drvr_mmi_read(hfa384x_t *hw, UINT32 addr, UINT32 *resp)
-{
-#if 0
-        int             result = 0;
-        UINT16  cmd_code = (UINT16) 0x30;
-        UINT16 param = (UINT16) addr;
-        DBFENTER;
-
-        /* Do i need a host2hfa... conversion ? */
-        result = hfa384x_docmd_wait(hw, cmd_code);
-
-        DBFEXIT;
-        return result;
-#endif
-return 0;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_mmi_write
-*
-* Read mmi registers.  mmi is intersil-speak for the baseband
-* processor registers.
-*
-* Arguments:
-*       hw              device structure
-*       addr            The test register to be accessed (must be even #).
-*       data            The data value to write to the register.
-*
-* Returns:
-*       0               success
-*       >0              f/w reported error - f/w status code
-*       <0              driver reported error
-*
-* Side effects:
-*
-* Call context:
-*       process
-----------------------------------------------------------------*/
-
-int
-hfa384x_drvr_mmi_write(hfa384x_t *hw, UINT32 addr, UINT32 data)
-{
-#if 0
-        int             result = 0;
-        UINT16  cmd_code = (UINT16) 0x31;
-        UINT16 param0 = (UINT16) addr;
-        UINT16 param1 = (UINT16) data;
-        DBFENTER;
-
-        WLAN_LOG_DEBUG(1,"mmi write : addr = 0x%08lx\n", addr);
-        WLAN_LOG_DEBUG(1,"mmi write : data = 0x%08lx\n", data);
-
-        /* Do i need a host2hfa... conversion ? */
-        result = hfa384x_docmd_wait(hw, cmd_code);
-
-        DBFEXIT;
-        return result;
-#endif
-return 0;
-}
-
-
-/*----------------------------------------------------------------
 * hfa384x_drvr_ramdl_disable
 *
 * Ends the ram download state.
@@ -2969,11 +2662,11 @@ hfa384x_drvr_ramdl_disable(hfa384x_t *hw)
 *	process
 ----------------------------------------------------------------*/
 int
-hfa384x_drvr_ramdl_enable(hfa384x_t *hw, UINT32 exeaddr)
+hfa384x_drvr_ramdl_enable(hfa384x_t *hw, u32 exeaddr)
 {
 	int		result = 0;
-	UINT16		lowaddr;
-	UINT16		hiaddr;
+	u16		lowaddr;
+	u16		hiaddr;
 	int		i;
 	DBFENTER;
 	/* Check that a port isn't active */
@@ -3044,16 +2737,16 @@ hfa384x_drvr_ramdl_enable(hfa384x_t *hw, UINT32 exeaddr)
 *	process
 ----------------------------------------------------------------*/
 int
-hfa384x_drvr_ramdl_write(hfa384x_t *hw, UINT32 daddr, void* buf, UINT32 len)
+hfa384x_drvr_ramdl_write(hfa384x_t *hw, u32 daddr, void* buf, u32 len)
 {
 	int		result = 0;
 	int		nwrites;
-	UINT8		*data = buf;
+	u8		*data = buf;
 	int		i;
-	UINT32		curraddr;
-	UINT16		currpage;
-	UINT16		curroffset;
-	UINT16		currlen;
+	u32		curraddr;
+	u16		currpage;
+	u16		curroffset;
+	u16		currlen;
 	DBFENTER;
 	/* Check that we're in the ram download state */
 	if ( hw->dlstate != HFA384x_DLSTATE_RAMENABLED ) {
@@ -3125,21 +2818,21 @@ hfa384x_drvr_ramdl_write(hfa384x_t *hw, UINT32 daddr, void* buf, UINT32 len)
 * Call context:
 *	process or non-card interrupt.
 ----------------------------------------------------------------*/
-int hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, UINT len)
+int hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, unsigned int len)
 {
 	int		result = 0;
-	UINT16		*pda = buf;
+	u16		*pda = buf;
 	int		pdaok = 0;
 	int		morepdrs = 1;
 	int		currpdr = 0;	/* word offset of the current pdr */
 	size_t		i;
-	UINT16		pdrlen;		/* pdr length in bytes, host order */
-	UINT16		pdrcode;	/* pdr code, host order */
-	UINT16		currpage;
-	UINT16		curroffset;
+	u16		pdrlen;		/* pdr length in bytes, host order */
+	u16		pdrcode;	/* pdr code, host order */
+	u16		currpage;
+	u16		curroffset;
 	struct pdaloc {
-		UINT32	cardaddr;
-		UINT16	auxctl;
+		u32	cardaddr;
+		u16	auxctl;
 	} pdaloc[] =
 	{
 		{ HFA3842_PDA_BASE,		0},
@@ -3243,7 +2936,7 @@ int hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, UINT len)
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_drvr_setconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len)
+int hfa384x_drvr_setconfig(hfa384x_t *hw, u16 rid, void *buf, u16 len)
 {
 	return hfa384x_dowrid_wait(hw, rid, buf, len);
 }
@@ -3267,19 +2960,38 @@ int hfa384x_drvr_setconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len)
 * Call context:
 *	process
 ----------------------------------------------------------------*/
+
 int hfa384x_drvr_start(hfa384x_t *hw)
 {
-	int		result;
+	int		result, result1, result2;
+	u16		status;
 	DBFENTER;
 
 	might_sleep();
 
-	if (usb_clear_halt(hw->usb, hw->endp_in)) {
+	/* Clear endpoint stalls - but only do this if the endpoint
+	 * is showing a stall status. Some prism2 cards seem to behave
+	 * badly if a clear_halt is called when the endpoint is already
+	 * ok
+	 */
+	result = usb_get_status(hw->usb, USB_RECIP_ENDPOINT, hw->endp_in, &status);
+	if (result < 0) {
+		WLAN_LOG_ERROR(
+			"Cannot get bulk in endpoint status.\n");
+		goto done;
+	}
+	if ((status == 1) && usb_clear_halt(hw->usb, hw->endp_in)) {
 		WLAN_LOG_ERROR(
 			"Failed to reset bulk in endpoint.\n");
 	}
 
-	if (usb_clear_halt(hw->usb, hw->endp_out)) {
+	result = usb_get_status(hw->usb, USB_RECIP_ENDPOINT, hw->endp_out, &status);
+	if (result < 0) {
+		WLAN_LOG_ERROR(
+			"Cannot get bulk out endpoint status.\n");
+		goto done;
+	}
+	if ((status == 1) && usb_clear_halt(hw->usb, hw->endp_out)) {
 		WLAN_LOG_ERROR(
 			"Failed to reset bulk out endpoint.\n");
 	}
@@ -3296,14 +3008,37 @@ int hfa384x_drvr_start(hfa384x_t *hw)
 		goto done;
 	}
 
-	/* call initialize */
-	result = hfa384x_cmd_initialize(hw);
-	if (result != 0) {
-		usb_kill_urb(&hw->rx_urb);
-		WLAN_LOG_ERROR(
-			"cmd_initialize() failed, result=%d\n",
-			result);
-		goto done;
+	/* Call initialize twice, with a 1 second sleep in between.
+	 * This is a nasty work-around since many prism2 cards seem to
+	 * need time to settle after an init from cold. The second
+	 * call to initialize in theory is not necessary - but we call
+	 * it anyway as a double insurance policy:
+	 * 1) If the first init should fail, the second may well succeed
+	 *    and the card can still be used
+	 * 2) It helps ensures all is well with the card after the first
+	 *    init and settle time.
+	 */
+	result1 = hfa384x_cmd_initialize(hw);
+	msleep(1000);
+	result = result2 = hfa384x_cmd_initialize(hw);
+	if (result1 != 0) {
+		if (result2 != 0) {
+			WLAN_LOG_ERROR(
+				"cmd_initialize() failed on two attempts, results %d and %d\n",
+				result1, result2);
+			usb_kill_urb(&hw->rx_urb);
+			goto done;
+		} else {
+			WLAN_LOG_DEBUG(0, "First cmd_initialize() failed (result %d),\n",
+				result1);
+			WLAN_LOG_DEBUG(0, "but second attempt succeeded. All should be ok\n");
+		}
+	} else if (result2 != 0) {
+		WLAN_LOG_WARNING(
+			"First cmd_initialize() succeeded, but second attempt failed (result=%d)\n",
+			result2);
+		WLAN_LOG_WARNING("Most likely the card will be functional\n");
+			goto done;
 	}
 
 	hw->state = HFA384x_STATE_RUNNING;
@@ -3849,11 +3584,7 @@ hfa384x_usbctlxq_run(hfa384x_t	*hw)
 * Call context:
 *	interrupt
 ----------------------------------------------------------------*/
-#ifdef URB_ONLY_CALLBACK
 static void hfa384x_usbin_callback(struct urb *urb)
-#else
-static void hfa384x_usbin_callback(struct urb *urb, struct pt_regs *regs)
-#endif
 {
 	wlandevice_t		*wlandev = urb->context;
 	hfa384x_t		*hw;
@@ -3861,7 +3592,7 @@ static void hfa384x_usbin_callback(struct urb *urb, struct pt_regs *regs)
 	struct sk_buff          *skb = NULL;
 	int			result;
 	int                     urb_status;
-	UINT16			type;
+	u16			type;
 
 	enum USBIN_ACTION {
 		HANDLE,
@@ -3873,7 +3604,7 @@ static void hfa384x_usbin_callback(struct urb *urb, struct pt_regs *regs)
 
 	if ( !wlandev ||
 	     !wlandev->netdev ||
-	     !netif_device_present(wlandev->netdev) )
+	     wlandev->hwremoved )
 		goto exit;
 
 	hw = wlandev->priv;
@@ -4088,7 +3819,7 @@ retry:
 		if (unlocked_usbctlx_cancel_async(hw, ctlx) == 0)
 			run_queue = 1;
 	} else {
-		const UINT16 intype = (usbin->type&~host2hfa384x_16(0x8000));
+		const u16 intype = (usbin->type&~host2hfa384x_16(0x8000));
 
 		/*
 		 * Check that our message is what we're expecting ...
@@ -4168,7 +3899,7 @@ unlock:
 ----------------------------------------------------------------*/
 static void hfa384x_usbin_txcompl(wlandevice_t *wlandev, hfa384x_usbin_t *usbin)
 {
-	UINT16			status;
+	u16			status;
 	DBFENTER;
 
 	status = hfa384x2host_16(usbin->type); /* yeah I know it says type...*/
@@ -4208,8 +3939,8 @@ static void hfa384x_usbin_rx(wlandevice_t *wlandev, struct sk_buff *skb)
 	hfa384x_t               *hw = wlandev->priv;
 	int                     hdrlen;
 	p80211_rxmeta_t         *rxmeta;
-	UINT16                  data_len;
-	UINT16                  fc;
+	u16                  data_len;
+	u16                  fc;
 
 	DBFENTER;
 
@@ -4315,12 +4046,11 @@ done:
 static void hfa384x_int_rxmonitor( wlandevice_t *wlandev, hfa384x_usb_rxfrm_t *rxfrm)
 {
 	hfa384x_rx_frame_t              *rxdesc = &(rxfrm->desc);
-	UINT				hdrlen = 0;
-	UINT				datalen = 0;
-	UINT				skblen = 0;
-	p80211msg_lnxind_wlansniffrm_t	*msg;
-	UINT8				*datap;
-	UINT16				fc;
+	unsigned int				hdrlen = 0;
+	unsigned int				datalen = 0;
+	unsigned int				skblen = 0;
+	u8				*datap;
+	u16				fc;
 	struct sk_buff			*skb;
 	hfa384x_t		        *hw = wlandev->priv;
 
@@ -4333,15 +4063,15 @@ static void hfa384x_int_rxmonitor( wlandevice_t *wlandev, hfa384x_usb_rxfrm_t *r
 	datalen = hfa384x2host_16(rxdesc->data_len);
 
 	/* Allocate an ind message+framesize skb */
-	skblen = sizeof(p80211msg_lnxind_wlansniffrm_t) +
+	skblen = sizeof(p80211_caphdr_t) +
 		hdrlen + datalen + WLAN_CRC_LEN;
 
 	/* sanity check the length */
 	if ( skblen >
-		(sizeof(p80211msg_lnxind_wlansniffrm_t) +
-		WLAN_HDR_A4_LEN + WLAN_DATA_MAXLEN + WLAN_CRC_LEN) ) {
+	     (sizeof(p80211_caphdr_t) +
+	      WLAN_HDR_A4_LEN + WLAN_DATA_MAXLEN + WLAN_CRC_LEN) ) {
 		WLAN_LOG_DEBUG(1, "overlen frm: len=%zd\n",
-			skblen - sizeof(p80211msg_lnxind_wlansniffrm_t));
+			       skblen - sizeof(p80211_caphdr_t));
 	}
 
 	if ( (skb = dev_alloc_skb(skblen)) == NULL ) {
@@ -4351,66 +4081,7 @@ static void hfa384x_int_rxmonitor( wlandevice_t *wlandev, hfa384x_usb_rxfrm_t *r
 
 	/* only prepend the prism header if in the right mode */
 	if ((wlandev->netdev->type == ARPHRD_IEEE80211_PRISM) &&
-	    (hw->sniffhdr == 0)) {
-		datap = skb_put(skb, sizeof(p80211msg_lnxind_wlansniffrm_t));
-		msg = (p80211msg_lnxind_wlansniffrm_t*) datap;
-
-		/* Initialize the message members */
-		msg->msgcode = DIDmsg_lnxind_wlansniffrm;
-		msg->msglen = sizeof(p80211msg_lnxind_wlansniffrm_t);
-		strcpy(msg->devname, wlandev->name);
-
-		msg->hosttime.did = DIDmsg_lnxind_wlansniffrm_hosttime;
-		msg->hosttime.status = 0;
-		msg->hosttime.len = 4;
-		msg->hosttime.data = jiffies;
-
-		msg->mactime.did = DIDmsg_lnxind_wlansniffrm_mactime;
-		msg->mactime.status = 0;
-		msg->mactime.len = 4;
-		msg->mactime.data = rxdesc->time;
-
-		msg->channel.did = DIDmsg_lnxind_wlansniffrm_channel;
-		msg->channel.status = 0;
-		msg->channel.len = 4;
-		msg->channel.data = hw->sniff_channel;
-
-		msg->rssi.did = DIDmsg_lnxind_wlansniffrm_rssi;
-		msg->rssi.status = P80211ENUM_msgitem_status_no_value;
-		msg->rssi.len = 4;
-		msg->rssi.data = 0;
-
-		msg->sq.did = DIDmsg_lnxind_wlansniffrm_sq;
-		msg->sq.status = P80211ENUM_msgitem_status_no_value;
-		msg->sq.len = 4;
-		msg->sq.data = 0;
-
-		msg->signal.did = DIDmsg_lnxind_wlansniffrm_signal;
-		msg->signal.status = 0;
-		msg->signal.len = 4;
-		msg->signal.data = rxdesc->signal;
-
-		msg->noise.did = DIDmsg_lnxind_wlansniffrm_noise;
-		msg->noise.status = 0;
-		msg->noise.len = 4;
-		msg->noise.data = rxdesc->silence;
-
-		msg->rate.did = DIDmsg_lnxind_wlansniffrm_rate;
-		msg->rate.status = 0;
-		msg->rate.len = 4;
-		msg->rate.data = rxdesc->rate / 5; /* set to 802.11 units */
-
-		msg->istx.did = DIDmsg_lnxind_wlansniffrm_istx;
-		msg->istx.status = 0;
-		msg->istx.len = 4;
-		msg->istx.data = P80211ENUM_truth_false;
-
-		msg->frmlen.did = DIDmsg_lnxind_wlansniffrm_frmlen;
-		msg->frmlen.status = 0;
-		msg->frmlen.len = 4;
-		msg->frmlen.data = hdrlen + datalen + WLAN_CRC_LEN;
-	} else if ((wlandev->netdev->type == ARPHRD_IEEE80211_PRISM) &&
-		   (hw->sniffhdr != 0)) {
+	    (hw->sniffhdr != 0)) {
 		p80211_caphdr_t		*caphdr;
 		/* The NEW header format! */
 		datap = skb_put(skb, sizeof(p80211_caphdr_t));
@@ -4508,11 +4179,7 @@ static void hfa384x_usbin_info(wlandevice_t *wlandev, hfa384x_usbin_t *usbin)
 * Call context:
 *	interrupt
 ----------------------------------------------------------------*/
-#ifdef URB_ONLY_CALLBACK
 static void hfa384x_usbout_callback(struct urb *urb)
-#else
-static void hfa384x_usbout_callback(struct urb *urb, struct pt_regs *regs)
-#endif
 {
 	wlandevice_t		*wlandev = urb->context;
 	hfa384x_usbout_t	*usbout = urb->transfer_buffer;
@@ -4589,11 +4256,7 @@ static void hfa384x_usbout_callback(struct urb *urb, struct pt_regs *regs)
 * Call context:
 * interrupt
 ----------------------------------------------------------------*/
-#ifdef URB_ONLY_CALLBACK
 static void hfa384x_ctlxout_callback(struct urb *urb)
-#else
-static void hfa384x_ctlxout_callback(struct urb *urb, struct pt_regs *regs)
-#endif
 {
 	hfa384x_t	*hw = urb->context;
 	int             delete_resptimer = 0;
@@ -4969,7 +4632,7 @@ static void hfa384x_usbout_tx(wlandevice_t *wlandev, hfa384x_usbout_t *usbout)
 * Call context:
 ----------------------------------------------------------------*/
 static int
-hfa384x_isgood_pdrcode(UINT16 pdrcode)
+hfa384x_isgood_pdrcode(u16 pdrcode)
 {
 	switch(pdrcode) {
 	case HFA384x_PDR_END_OF_PDA:
diff --git a/drivers/staging/wlan-ng/p80211conv.c b/drivers/staging/wlan-ng/p80211conv.c
index 68121b9b34fa..dfc7b3a1e9c9 100644
--- a/drivers/staging/wlan-ng/p80211conv.c
+++ b/drivers/staging/wlan-ng/p80211conv.c
@@ -52,10 +52,6 @@
 /*================================================================*/
 /* System Includes */
 
-#define __NO_VERSION__		/* prevent the static definition */
-
-
-#include <linux/version.h>
 
 #include <linux/module.h>
 #include <linux/kernel.h>
@@ -70,7 +66,6 @@
 
 #include <asm/byteorder.h>
 
-#include "version.h"
 #include "wlan_compat.h"
 
 /*================================================================*/
@@ -100,8 +95,8 @@
 /*================================================================*/
 /* Local Static Definitions */
 
-static UINT8	oui_rfc1042[] = {0x00, 0x00, 0x00};
-static UINT8	oui_8021h[] = {0x00, 0x00, 0xf8};
+static u8	oui_rfc1042[] = {0x00, 0x00, 0x00};
+static u8	oui_8021h[] = {0x00, 0x00, 0xf8};
 
 /*================================================================*/
 /* Local Function Declarations */
@@ -135,11 +130,11 @@ static UINT8	oui_8021h[] = {0x00, 0x00, 0xf8};
 * Call context:
 *	May be called in interrupt or non-interrupt context
 ----------------------------------------------------------------*/
-int skb_ether_to_p80211( wlandevice_t *wlandev, UINT32 ethconv, struct sk_buff *skb, p80211_hdr_t *p80211_hdr, p80211_metawep_t *p80211_wep)
+int skb_ether_to_p80211( wlandevice_t *wlandev, u32 ethconv, struct sk_buff *skb, p80211_hdr_t *p80211_hdr, p80211_metawep_t *p80211_wep)
 {
 
-	UINT16          fc;
-	UINT16          proto;
+	u16          fc;
+	u16          proto;
 	wlan_ethhdr_t   e_hdr;
 	wlan_llc_t      *e_llc;
 	wlan_snap_t     *e_snap;
@@ -298,14 +293,14 @@ static void orinoco_spy_gather(wlandevice_t *wlandev, char *mac,
 * Call context:
 *	May be called in interrupt or non-interrupt context
 ----------------------------------------------------------------*/
-int skb_p80211_to_ether( wlandevice_t *wlandev, UINT32 ethconv, struct sk_buff *skb)
+int skb_p80211_to_ether( wlandevice_t *wlandev, u32 ethconv, struct sk_buff *skb)
 {
 	netdevice_t     *netdev = wlandev->netdev;
-	UINT16          fc;
-	UINT            payload_length;
-	UINT            payload_offset;
-	UINT8		daddr[WLAN_ETHADDR_LEN];
-	UINT8		saddr[WLAN_ETHADDR_LEN];
+	u16          fc;
+	unsigned int            payload_length;
+	unsigned int            payload_offset;
+	u8		daddr[WLAN_ETHADDR_LEN];
+	u8		saddr[WLAN_ETHADDR_LEN];
 	p80211_hdr_t    *w_hdr;
 	wlan_ethhdr_t   *e_hdr;
 	wlan_llc_t      *e_llc;
@@ -333,11 +328,11 @@ int skb_p80211_to_ether( wlandevice_t *wlandev, UINT32 ethconv, struct sk_buff *
 		memcpy(saddr, w_hdr->a3.a2, WLAN_ETHADDR_LEN);
 	} else {
 		payload_offset = WLAN_HDR_A4_LEN;
-		payload_length -= ( WLAN_HDR_A4_LEN - WLAN_HDR_A3_LEN );
-		if (payload_length < 0 ) {
+		if (payload_length < WLAN_HDR_A4_LEN - WLAN_HDR_A3_LEN) {
 			WLAN_LOG_ERROR("A4 frame too short!\n");
 			return 1;
 		}
+		payload_length -= (WLAN_HDR_A4_LEN - WLAN_HDR_A3_LEN);
 		memcpy(daddr, w_hdr->a4.a3, WLAN_ETHADDR_LEN);
 		memcpy(saddr, w_hdr->a4.a4, WLAN_ETHADDR_LEN);
 	}
@@ -497,8 +492,16 @@ int skb_p80211_to_ether( wlandevice_t *wlandev, UINT32 ethconv, struct sk_buff *
 
 	}
 
+        /*
+         * Note that eth_type_trans() expects an skb w/ skb->data pointing
+         * at the MAC header, it then sets the following skb members:
+         * skb->mac_header,
+         * skb->data, and
+         * skb->pkt_type.
+         * It then _returns_ the value that _we're_ supposed to stuff in
+         * skb->protocol.  This is nuts.
+         */
 	skb->protocol = eth_type_trans(skb, netdev);
-	skb_reset_mac_header(skb);
 
         /* jkriegl: process signal and noise as set in hfa384x_int_rx() */
 	/* jkriegl: only process signal/noise if requested by iwspy */
@@ -528,7 +531,7 @@ int skb_p80211_to_ether( wlandevice_t *wlandev, UINT32 ethconv, struct sk_buff *
 * Call context:
 *	May be called in interrupt or non-interrupt context
 ----------------------------------------------------------------*/
-int p80211_stt_findproto(UINT16 proto)
+int p80211_stt_findproto(u16 proto)
 {
 	/* Always return found for now.  This is the behavior used by the */
 	/*  Zoom Win95 driver when 802.1h mode is selected */
diff --git a/drivers/staging/wlan-ng/p80211conv.h b/drivers/staging/wlan-ng/p80211conv.h
index 3f5ab57cd9a8..538e9bd14902 100644
--- a/drivers/staging/wlan-ng/p80211conv.h
+++ b/drivers/staging/wlan-ng/p80211conv.h
@@ -86,22 +86,22 @@ typedef struct p80211_rxmeta
 {
 	struct wlandevice	*wlandev;
 
-	UINT64	mactime;	/* Hi-rez MAC-supplied time value */
-	UINT64	hosttime;	/* Best-rez host supplied time value */
+	u64	mactime;	/* Hi-rez MAC-supplied time value */
+	u64	hosttime;	/* Best-rez host supplied time value */
 
-	UINT	rxrate;		/* Receive data rate in 100kbps */
-	UINT	priority;	/* 0-15, 0=contention, 6=CF */
-	INT	signal;		/* An SSI, see p80211netdev.h */
-	INT	noise;		/* An SSI, see p80211netdev.h */
-	UINT	channel;	/* Receive channel (mostly for snifs) */
-	UINT	preamble;	/* P80211ENUM_preambletype_* */
-	UINT	encoding;	/* P80211ENUM_encoding_* */
+	unsigned int	rxrate;		/* Receive data rate in 100kbps */
+	unsigned int	priority;	/* 0-15, 0=contention, 6=CF */
+	int	signal;		/* An SSI, see p80211netdev.h */
+	int	noise;		/* An SSI, see p80211netdev.h */
+	unsigned int	channel;	/* Receive channel (mostly for snifs) */
+	unsigned int	preamble;	/* P80211ENUM_preambletype_* */
+	unsigned int	encoding;	/* P80211ENUM_encoding_* */
 
 } p80211_rxmeta_t;
 
 typedef struct p80211_frmmeta
 {
-	UINT			magic;
+	unsigned int			magic;
 	p80211_rxmeta_t		*rx;
 } p80211_frmmeta_t;
 
@@ -117,20 +117,20 @@ void p80211skb_rxmeta_detach(struct sk_buff *skb);
  */
 typedef struct p80211_caphdr
 {
-	UINT32		version;
-	UINT32		length;
-	UINT64		mactime;
-	UINT64		hosttime;
-	UINT32		phytype;
-	UINT32		channel;
-	UINT32		datarate;
-	UINT32		antenna;
-	UINT32		priority;
-	UINT32		ssi_type;
-	INT32		ssi_signal;
-	INT32		ssi_noise;
-	UINT32		preamble;
-	UINT32		encoding;
+	u32		version;
+	u32		length;
+	u64		mactime;
+	u64		hosttime;
+	u32		phytype;
+	u32		channel;
+	u32		datarate;
+	u32		antenna;
+	u32		priority;
+	u32		ssi_type;
+	s32		ssi_signal;
+	s32		ssi_noise;
+	u32		preamble;
+	u32		encoding;
 } p80211_caphdr_t;
 
 /* buffer free method pointer type */
@@ -138,31 +138,31 @@ typedef void (* freebuf_method_t)(void *buf, int size);
 
 typedef struct p80211_metawep {
 	void  *data;
-	UINT8 iv[4];
-	UINT8 icv[4];
+	u8 iv[4];
+	u8 icv[4];
 } p80211_metawep_t;
 
 /* local ether header type */
 typedef struct wlan_ethhdr
 {
-	UINT8	daddr[WLAN_ETHADDR_LEN];
-	UINT8	saddr[WLAN_ETHADDR_LEN];
-	UINT16	type;
+	u8	daddr[WLAN_ETHADDR_LEN];
+	u8	saddr[WLAN_ETHADDR_LEN];
+	u16	type;
 } __WLAN_ATTRIB_PACK__ wlan_ethhdr_t;
 
 /* local llc header type */
 typedef struct wlan_llc
 {
-	UINT8	dsap;
-	UINT8	ssap;
-	UINT8	ctl;
+	u8	dsap;
+	u8	ssap;
+	u8	ctl;
 } __WLAN_ATTRIB_PACK__ wlan_llc_t;
 
 /* local snap header type */
 typedef struct wlan_snap
 {
-	UINT8	oui[WLAN_IEEE_OUI_LEN];
-	UINT16	type;
+	u8	oui[WLAN_IEEE_OUI_LEN];
+	u16	type;
 } __WLAN_ATTRIB_PACK__ wlan_snap_t;
 
 /* Circular include trick */
@@ -174,13 +174,13 @@ struct wlandevice;
 /*================================================================*/
 /*Function Declarations */
 
-int skb_p80211_to_ether( struct wlandevice *wlandev, UINT32 ethconv,
+int skb_p80211_to_ether( struct wlandevice *wlandev, u32 ethconv,
 			 struct sk_buff *skb);
-int skb_ether_to_p80211( struct wlandevice *wlandev, UINT32 ethconv,
+int skb_ether_to_p80211( struct wlandevice *wlandev, u32 ethconv,
 			 struct sk_buff *skb, p80211_hdr_t *p80211_hdr,
 			 p80211_metawep_t *p80211_wep );
 
-int p80211_stt_findproto(UINT16 proto);
-int p80211_stt_addproto(UINT16 proto);
+int p80211_stt_findproto(u16 proto);
+int p80211_stt_addproto(u16 proto);
 
 #endif
diff --git a/drivers/staging/wlan-ng/p80211hdr.h b/drivers/staging/wlan-ng/p80211hdr.h
index b7b0872fd861..72f12aff3900 100644
--- a/drivers/staging/wlan-ng/p80211hdr.h
+++ b/drivers/staging/wlan-ng/p80211hdr.h
@@ -166,29 +166,29 @@
 /*                        SET_FC_FSTYPE(WLAN_FSTYPE_RTS) );   */
 /*------------------------------------------------------------*/
 
-#define WLAN_GET_FC_PVER(n)	 (((UINT16)(n)) & (BIT0 | BIT1))
-#define WLAN_GET_FC_FTYPE(n)	((((UINT16)(n)) & (BIT2 | BIT3)) >> 2)
-#define WLAN_GET_FC_FSTYPE(n)	((((UINT16)(n)) & (BIT4|BIT5|BIT6|BIT7)) >> 4)
-#define WLAN_GET_FC_TODS(n) 	((((UINT16)(n)) & (BIT8)) >> 8)
-#define WLAN_GET_FC_FROMDS(n)	((((UINT16)(n)) & (BIT9)) >> 9)
-#define WLAN_GET_FC_MOREFRAG(n) ((((UINT16)(n)) & (BIT10)) >> 10)
-#define WLAN_GET_FC_RETRY(n)	((((UINT16)(n)) & (BIT11)) >> 11)
-#define WLAN_GET_FC_PWRMGT(n)	((((UINT16)(n)) & (BIT12)) >> 12)
-#define WLAN_GET_FC_MOREDATA(n) ((((UINT16)(n)) & (BIT13)) >> 13)
-#define WLAN_GET_FC_ISWEP(n)	((((UINT16)(n)) & (BIT14)) >> 14)
-#define WLAN_GET_FC_ORDER(n)	((((UINT16)(n)) & (BIT15)) >> 15)
-
-#define WLAN_SET_FC_PVER(n)	((UINT16)(n))
-#define WLAN_SET_FC_FTYPE(n)	(((UINT16)(n)) << 2)
-#define WLAN_SET_FC_FSTYPE(n)	(((UINT16)(n)) << 4)
-#define WLAN_SET_FC_TODS(n) 	(((UINT16)(n)) << 8)
-#define WLAN_SET_FC_FROMDS(n)	(((UINT16)(n)) << 9)
-#define WLAN_SET_FC_MOREFRAG(n) (((UINT16)(n)) << 10)
-#define WLAN_SET_FC_RETRY(n)	(((UINT16)(n)) << 11)
-#define WLAN_SET_FC_PWRMGT(n)	(((UINT16)(n)) << 12)
-#define WLAN_SET_FC_MOREDATA(n) (((UINT16)(n)) << 13)
-#define WLAN_SET_FC_ISWEP(n)	(((UINT16)(n)) << 14)
-#define WLAN_SET_FC_ORDER(n)	(((UINT16)(n)) << 15)
+#define WLAN_GET_FC_PVER(n)	 (((u16)(n)) & (BIT0 | BIT1))
+#define WLAN_GET_FC_FTYPE(n)	((((u16)(n)) & (BIT2 | BIT3)) >> 2)
+#define WLAN_GET_FC_FSTYPE(n)	((((u16)(n)) & (BIT4|BIT5|BIT6|BIT7)) >> 4)
+#define WLAN_GET_FC_TODS(n) 	((((u16)(n)) & (BIT8)) >> 8)
+#define WLAN_GET_FC_FROMDS(n)	((((u16)(n)) & (BIT9)) >> 9)
+#define WLAN_GET_FC_MOREFRAG(n) ((((u16)(n)) & (BIT10)) >> 10)
+#define WLAN_GET_FC_RETRY(n)	((((u16)(n)) & (BIT11)) >> 11)
+#define WLAN_GET_FC_PWRMGT(n)	((((u16)(n)) & (BIT12)) >> 12)
+#define WLAN_GET_FC_MOREDATA(n) ((((u16)(n)) & (BIT13)) >> 13)
+#define WLAN_GET_FC_ISWEP(n)	((((u16)(n)) & (BIT14)) >> 14)
+#define WLAN_GET_FC_ORDER(n)	((((u16)(n)) & (BIT15)) >> 15)
+
+#define WLAN_SET_FC_PVER(n)	((u16)(n))
+#define WLAN_SET_FC_FTYPE(n)	(((u16)(n)) << 2)
+#define WLAN_SET_FC_FSTYPE(n)	(((u16)(n)) << 4)
+#define WLAN_SET_FC_TODS(n) 	(((u16)(n)) << 8)
+#define WLAN_SET_FC_FROMDS(n)	(((u16)(n)) << 9)
+#define WLAN_SET_FC_MOREFRAG(n) (((u16)(n)) << 10)
+#define WLAN_SET_FC_RETRY(n)	(((u16)(n)) << 11)
+#define WLAN_SET_FC_PWRMGT(n)	(((u16)(n)) << 12)
+#define WLAN_SET_FC_MOREDATA(n) (((u16)(n)) << 13)
+#define WLAN_SET_FC_ISWEP(n)	(((u16)(n)) << 14)
+#define WLAN_SET_FC_ORDER(n)	(((u16)(n)) << 15)
 
 /*--- Duration Macros ----------------------------------------*/
 /* Macros to get/set the bitfields of the Duration Field      */
@@ -201,45 +201,45 @@
 /* Macros to get/set the bitfields of the Sequence Control    */
 /* Field.                                                     */
 /*------------------------------------------------------------*/
-#define WLAN_GET_SEQ_FRGNUM(n) (((UINT16)(n)) & (BIT0|BIT1|BIT2|BIT3))
-#define WLAN_GET_SEQ_SEQNUM(n) ((((UINT16)(n)) & (~(BIT0|BIT1|BIT2|BIT3))) >> 4)
+#define WLAN_GET_SEQ_FRGNUM(n) (((u16)(n)) & (BIT0|BIT1|BIT2|BIT3))
+#define WLAN_GET_SEQ_SEQNUM(n) ((((u16)(n)) & (~(BIT0|BIT1|BIT2|BIT3))) >> 4)
 
 /*--- Data ptr macro -----------------------------------------*/
-/* Creates a UINT8* to the data portion of a frame            */
+/* Creates a u8* to the data portion of a frame            */
 /* Assumes you're passing in a ptr to the beginning of the hdr*/
 /*------------------------------------------------------------*/
-#define WLAN_HDR_A3_DATAP(p) (((UINT8*)(p)) + WLAN_HDR_A3_LEN)
-#define WLAN_HDR_A4_DATAP(p) (((UINT8*)(p)) + WLAN_HDR_A4_LEN)
+#define WLAN_HDR_A3_DATAP(p) (((u8*)(p)) + WLAN_HDR_A3_LEN)
+#define WLAN_HDR_A4_DATAP(p) (((u8*)(p)) + WLAN_HDR_A4_LEN)
 
-#define DOT11_RATE5_ISBASIC_GET(r)     (((UINT8)(r)) & BIT7)
+#define DOT11_RATE5_ISBASIC_GET(r)     (((u8)(r)) & BIT7)
 
 /*================================================================*/
 /* Types */
 
 /* BSS Timestamp */
-typedef UINT8 wlan_bss_ts_t[WLAN_BSS_TS_LEN];
+typedef u8 wlan_bss_ts_t[WLAN_BSS_TS_LEN];
 
 /* Generic 802.11 Header types */
 
 typedef struct p80211_hdr_a3
 {
-	UINT16	fc;
-	UINT16	dur;
-	UINT8	a1[WLAN_ADDR_LEN];
-	UINT8	a2[WLAN_ADDR_LEN];
-	UINT8	a3[WLAN_ADDR_LEN];
-	UINT16	seq;
+	u16	fc;
+	u16	dur;
+	u8	a1[WLAN_ADDR_LEN];
+	u8	a2[WLAN_ADDR_LEN];
+	u8	a3[WLAN_ADDR_LEN];
+	u16	seq;
 } __WLAN_ATTRIB_PACK__ p80211_hdr_a3_t;
 
 typedef struct p80211_hdr_a4
 {
-	UINT16	fc;
-	UINT16	dur;
-	UINT8	a1[WLAN_ADDR_LEN];
-	UINT8	a2[WLAN_ADDR_LEN];
-	UINT8	a3[WLAN_ADDR_LEN];
-	UINT16	seq;
-	UINT8	a4[WLAN_ADDR_LEN];
+	u16	fc;
+	u16	dur;
+	u8	a1[WLAN_ADDR_LEN];
+	u8	a2[WLAN_ADDR_LEN];
+	u8	a3[WLAN_ADDR_LEN];
+	u16	seq;
+	u8	a4[WLAN_ADDR_LEN];
 } __WLAN_ATTRIB_PACK__ p80211_hdr_a4_t;
 
 typedef union p80211_hdr
@@ -271,9 +271,9 @@ typedef union p80211_hdr
 #define WLAN_FCS_LEN			4
 
 /* ftcl in HOST order */
-inline static UINT16 p80211_headerlen(UINT16 fctl)
+inline static u16 p80211_headerlen(u16 fctl)
 {
-	UINT16 hdrlen = 0;
+	u16 hdrlen = 0;
 
 	switch ( WLAN_GET_FC_FTYPE(fctl) ) {
 	case WLAN_FTYPE_MGMT:
diff --git a/drivers/staging/wlan-ng/p80211ioctl.h b/drivers/staging/wlan-ng/p80211ioctl.h
index 25b2ea836227..ad67b698fa43 100644
--- a/drivers/staging/wlan-ng/p80211ioctl.h
+++ b/drivers/staging/wlan-ng/p80211ioctl.h
@@ -106,9 +106,9 @@ typedef struct p80211ioctl_req
 {
 	char 	name[WLAN_DEVNAMELEN_MAX];
 	caddr_t data;
-	UINT32	magic;
-	UINT16	len;
-	UINT32	result;
+	u32	magic;
+	u16	len;
+	u32	result;
 } __WLAN_ATTRIB_PACK__ p80211ioctl_req_t;
 
 
diff --git a/drivers/staging/wlan-ng/p80211meta.h b/drivers/staging/wlan-ng/p80211meta.h
index 5cb3f5ada4f5..8b61e5fc292b 100644
--- a/drivers/staging/wlan-ng/p80211meta.h
+++ b/drivers/staging/wlan-ng/p80211meta.h
@@ -90,7 +90,7 @@
 #define MKMIBMETASIZE(name)		p80211meta_ ## mib ## _ ## name ## _ ## size
 #define MKGRPMETASIZE(name)		p80211meta_ ## grp ## _ ## name ## _ ## size
 
-#define GETMETASIZE(aptr)		(**((UINT32**)(aptr)))
+#define GETMETASIZE(aptr)		(**((u32**)(aptr)))
 
 /*----------------------------------------------------------------*/
 /* The following ifdef depends on the following defines: */
@@ -114,14 +114,14 @@
 typedef struct p80211meta
 {
 	char			*name;		/* data item name */
-	UINT32			did;		/* partial did */
-	UINT32			flags;		/* set of various flag bits */
-	UINT32			min;		/* min value of a BOUNDEDINT */
-	UINT32			max;		/* max value of a BOUNDEDINT */
-
-	UINT32			maxlen;		/* maxlen of a OCTETSTR or DISPLAYSTR */
-	UINT32			minlen;		/* minlen of a OCTETSTR or DISPLAYSTR */
-	p80211enum_t		*enumptr;	/* ptr to the enum type for ENUMINT */
+	u32			did;		/* partial did */
+	u32			flags;		/* set of various flag bits */
+	u32			min;		/* min value of a BOUNDEDint */
+	u32			max;		/* max value of a BOUNDEDint */
+
+	u32			maxlen;		/* maxlen of a OCTETSTR or DISPLAYSTR */
+	u32			minlen;		/* minlen of a OCTETSTR or DISPLAYSTR */
+	p80211enum_t		*enumptr;	/* ptr to the enum type for ENUMint */
 	p80211_totext_t		totextptr;	/* ptr to totext conversion function */
 	p80211_fromtext_t	fromtextptr;	/* ptr to totext conversion function */
 	p80211_valid_t		validfunptr;	/* ptr to totext conversion function */
@@ -150,20 +150,20 @@ typedef struct catlistitem
 
 /*----------------------------------------------------------------*/
 /* */
-UINT32 p80211_text2did(catlistitem_t *catlist, char *catname, char *grpname, char *itemname);
-UINT32 p80211_text2catdid(catlistitem_t *list, char *name );
-UINT32 p80211_text2grpdid(grplistitem_t *list, char *name );
-UINT32 p80211_text2itemdid(p80211meta_t *list, char *name );
-UINT32 p80211_isvalid_did( catlistitem_t *catlist, UINT32 did );
-UINT32 p80211_isvalid_catdid( catlistitem_t *catlist, UINT32 did );
-UINT32 p80211_isvalid_grpdid( catlistitem_t *catlist, UINT32 did );
-UINT32 p80211_isvalid_itemdid( catlistitem_t *catlist, UINT32 did );
-catlistitem_t *p80211_did2cat( catlistitem_t *catlist, UINT32 did );
-grplistitem_t *p80211_did2grp( catlistitem_t *catlist, UINT32 did );
-p80211meta_t *p80211_did2item( catlistitem_t *catlist, UINT32 did );
-UINT32 p80211item_maxdatalen( struct catlistitem *metalist, UINT32 did );
-UINT32 p80211_metaname2did(struct catlistitem *metalist, char *itemname);
-UINT32 p80211item_getoffset( struct catlistitem *metalist, UINT32 did );
+u32 p80211_text2did(catlistitem_t *catlist, char *catname, char *grpname, char *itemname);
+u32 p80211_text2catdid(catlistitem_t *list, char *name );
+u32 p80211_text2grpdid(grplistitem_t *list, char *name );
+u32 p80211_text2itemdid(p80211meta_t *list, char *name );
+u32 p80211_isvalid_did( catlistitem_t *catlist, u32 did );
+u32 p80211_isvalid_catdid( catlistitem_t *catlist, u32 did );
+u32 p80211_isvalid_grpdid( catlistitem_t *catlist, u32 did );
+u32 p80211_isvalid_itemdid( catlistitem_t *catlist, u32 did );
+catlistitem_t *p80211_did2cat( catlistitem_t *catlist, u32 did );
+grplistitem_t *p80211_did2grp( catlistitem_t *catlist, u32 did );
+p80211meta_t *p80211_did2item( catlistitem_t *catlist, u32 did );
+u32 p80211item_maxdatalen( struct catlistitem *metalist, u32 did );
+u32 p80211_metaname2did(struct catlistitem *metalist, char *itemname);
+u32 p80211item_getoffset( struct catlistitem *metalist, u32 did );
 int p80211item_gettype(p80211meta_t *meta);
 
 #endif /* _P80211META_H */
diff --git a/drivers/staging/wlan-ng/p80211metadef.h b/drivers/staging/wlan-ng/p80211metadef.h
index 2c7f435a97e0..ce4fdd547fc2 100644
--- a/drivers/staging/wlan-ng/p80211metadef.h
+++ b/drivers/staging/wlan-ng/p80211metadef.h
@@ -72,25 +72,6 @@
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(2) | \
 			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_powermgmt \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3))
-#define DIDmsg_dot11req_powermgmt_powermgmtmode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_powermgmt_wakeup \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_powermgmt_receivedtims \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_dot11req_powermgmt_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(4) | 0x00000000)
 #define DIDmsg_dot11req_scan \
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(4))
@@ -301,211 +282,6 @@
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(5) | \
 			P80211DID_MKITEM(40) | 0x00000000)
-#define DIDmsg_dot11req_join \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6))
-#define DIDmsg_dot11req_join_bssid \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_join_joinfailuretimeout \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate1 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate2 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate3 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate4 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate5 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(7) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate6 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(8) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate7 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(9) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate8 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(10) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate1 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(11) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate2 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(12) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate3 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(13) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate4 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(14) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate5 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(15) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate6 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(16) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate7 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(17) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate8 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(18) | 0x00000000)
-#define DIDmsg_dot11req_join_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(19) | 0x00000000)
-#define DIDmsg_dot11req_authenticate \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(7))
-#define DIDmsg_dot11req_authenticate_peerstaaddress \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_authenticate_authenticationtype \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_authenticate_authenticationfailuretimeout \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_dot11req_authenticate_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_dot11req_deauthenticate \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(8))
-#define DIDmsg_dot11req_deauthenticate_peerstaaddress \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_deauthenticate_reasoncode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_deauthenticate_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_dot11req_associate \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9))
-#define DIDmsg_dot11req_associate_peerstaaddress \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_associate_associatefailuretimeout \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_associate_cfpollable \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_dot11req_associate_cfpollreq \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_dot11req_associate_privacy \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_dot11req_associate_listeninterval \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_dot11req_associate_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(7) | 0x00000000)
-#define DIDmsg_dot11req_reassociate \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10))
-#define DIDmsg_dot11req_reassociate_newapaddress \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_reassociate_reassociatefailuretimeout \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_reassociate_cfpollable \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_dot11req_reassociate_cfpollreq \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_dot11req_reassociate_privacy \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_dot11req_reassociate_listeninterval \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_dot11req_reassociate_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(7) | 0x00000000)
-#define DIDmsg_dot11req_disassociate \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(11))
-#define DIDmsg_dot11req_disassociate_peerstaaddress \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(11) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_disassociate_reasoncode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(11) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_disassociate_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(11) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_dot11req_reset \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(12))
-#define DIDmsg_dot11req_reset_setdefaultmib \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(12) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_reset_macaddress \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(12) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_reset_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(12) | \
-			P80211DID_MKITEM(3) | 0x00000000)
 #define DIDmsg_dot11req_start \
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(13))
@@ -795,147 +571,8 @@
 			(P80211DID_MKSECTION(3) | \
 			P80211DID_MKGROUP(5) | \
 			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_cat_lnxind \
-			P80211DID_MKSECTION(4)
-#define DIDmsg_lnxind_wlansniffrm \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1))
-#define DIDmsg_lnxind_wlansniffrm_hosttime \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_mactime \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_channel \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_rssi \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_sq \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_signal \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_noise \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(7) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_rate \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(8) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_istx \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(9) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_frmlen \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(10) | 0x00000000)
-#define DIDmsg_lnxind_roam \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(2))
-#define DIDmsg_lnxind_roam_reason \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(1) | 0x00000000)
 #define DIDmsg_cat_p2req \
 			P80211DID_MKSECTION(5)
-#define DIDmsg_p2req_join \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1))
-#define DIDmsg_p2req_join_bssid \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate4 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate5 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate6 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(7) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate7 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(8) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate8 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(9) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(10) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(11) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(12) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate4 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(13) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate5 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(14) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate6 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(15) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate7 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(16) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate8 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(17) | 0x00000000)
-#define DIDmsg_p2req_join_ssid \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(18) | 0x00000000)
-#define DIDmsg_p2req_join_channel \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(19) | 0x00000000)
-#define DIDmsg_p2req_join_authtype \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(20) | 0x00000000)
-#define DIDmsg_p2req_join_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(21) | 0x00000000)
 #define DIDmsg_p2req_readpda \
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(2))
@@ -947,162 +584,6 @@
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(2) | \
 			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_readcis \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3))
-#define DIDmsg_p2req_readcis_cis \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_readcis_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_auxport_state \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(4))
-#define DIDmsg_p2req_auxport_state_enable \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_auxport_state_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_auxport_read \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5))
-#define DIDmsg_p2req_auxport_read_addr \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_auxport_read_len \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_auxport_read_data \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_auxport_read_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_auxport_write \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6))
-#define DIDmsg_p2req_auxport_write_addr \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_auxport_write_len \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_auxport_write_data \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_auxport_write_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_low_level \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7))
-#define DIDmsg_p2req_low_level_command \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_low_level_param0 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_low_level_param1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_low_level_param2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_low_level_resp0 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_p2req_low_level_resp1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_p2req_low_level_resp2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(7) | 0x00000000)
-#define DIDmsg_p2req_low_level_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(8) | 0x00000000)
-#define DIDmsg_p2req_test_command \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8))
-#define DIDmsg_p2req_test_command_testcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_test_command_testparam \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_test_command_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_test_command_status \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_test_command_resp0 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_p2req_test_command_resp1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_p2req_test_command_resp2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(7) | 0x00000000)
-#define DIDmsg_p2req_mmi_read \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(9))
-#define DIDmsg_p2req_mmi_read_addr \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_mmi_read_value \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_mmi_read_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_mmi_write \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(10))
-#define DIDmsg_p2req_mmi_write_addr \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_mmi_write_data \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_mmi_write_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(3) | 0x00000000)
 #define DIDmsg_p2req_ramdl_state \
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(11))
@@ -1167,224 +648,8 @@
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(14) | \
 			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_mm_state \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(15))
-#define DIDmsg_p2req_mm_state_enable \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(15) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_mm_state_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(15) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_dump_state \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(16))
-#define DIDmsg_p2req_dump_state_level \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(16) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_dump_state_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(16) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_channel_info \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(17))
-#define DIDmsg_p2req_channel_info_channellist \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(17) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_channel_info_channeldwelltime \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(17) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_channel_info_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(17) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_channel_info_numchinfo \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(17) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_channel_info_results \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(18))
-#define DIDmsg_p2req_channel_info_results_channel \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(18) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_channel_info_results_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(18) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_channel_info_results_avgnoiselevel \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(18) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_channel_info_results_peaknoiselevel \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(18) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_channel_info_results_bssactive \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(18) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_p2req_channel_info_results_pcfactive \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(18) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_p2req_enable \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(19))
-#define DIDmsg_p2req_enable_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(19) | \
-			P80211DID_MKITEM(1) | 0x00000000)
 #define DIDmib_cat_dot11smt \
 			P80211DID_MKSECTION(1)
-#define DIDmib_dot11smt_p80211Table \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(1))
-#define DIDmib_dot11smt_p80211Table_p80211_ifstate \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2))
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11StationID \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(1) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11MediumOccupancyLimit \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11CFPollable \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11CFPPeriod \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(4) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11CFPMaxDuration \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(5) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11AuthenticationResponseTimeOut \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(6) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11PrivacyOptionImplemented \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(7) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11PowerManagementMode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(8) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11DesiredSSID \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(9) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11DesiredBSSType \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(10) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11OperationalRateSet \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(11) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11BeaconPeriod \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(12) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11DTIMPeriod \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(13) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11AssociationResponseTimeOut \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(14) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11DisassociateReason \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(15) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11DisassociateStation \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(16) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11DeauthenticateReason \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(17) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11DeauthenticateStation \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(18) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11AuthenticateFailStatus \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(19) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11AuthenticateFailStation \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(20) | 0x10000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3))
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm1 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(1) | 0x14000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable1 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(2) | 0x1c000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm2 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(3) | 0x14000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable2 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(4) | 0x1c000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm3 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(5) | 0x14000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable3 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(6) | 0x1c000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm4 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(7) | 0x14000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable4 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(8) | 0x1c000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm5 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(9) | 0x14000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable5 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(10) | 0x1c000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm6 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(11) | 0x14000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable6 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(12) | 0x1c000000)
 #define DIDmib_dot11smt_dot11WEPDefaultKeysTable \
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(4))
@@ -1404,25 +669,6 @@
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(4) | \
 			P80211DID_MKITEM(4) | 0x0c000000)
-#define DIDmib_dot11smt_dot11WEPKeyMappingsTable \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(5))
-#define DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingIndex \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(1) | 0x1c000000)
-#define DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingAddress \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(2) | 0x1c000000)
-#define DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingWEPOn \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(3) | 0x1c000000)
-#define DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingValue \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(4) | 0x1c000000)
 #define DIDmib_dot11smt_dot11PrivacyTable \
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(6))
@@ -1434,31 +680,19 @@
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(6) | \
 			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_dot11smt_dot11PrivacyTable_dot11WEPKeyMappingLength \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(3) | 0x18000000)
 #define DIDmib_dot11smt_dot11PrivacyTable_dot11ExcludeUnencrypted \
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(6) | \
 			P80211DID_MKITEM(4) | 0x18000000)
-#define DIDmib_dot11smt_dot11PrivacyTable_dot11WEPICVErrorCount \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_dot11smt_dot11PrivacyTable_dot11WEPExcludedCount \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(6) | 0x10000000)
 #define DIDmib_cat_dot11mac \
 			P80211DID_MKSECTION(2)
 #define DIDmib_dot11mac_dot11OperationTable \
 			(P80211DID_MKSECTION(2) | \
 			P80211DID_MKGROUP(1))
 #define DIDmib_dot11mac_dot11OperationTable_dot11MACAddress \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(1) | 0x18000000)
+                        (P80211DID_MKSECTION(2) | \
+                        P80211DID_MKGROUP(1) | \
+                        P80211DID_MKITEM(1) | 0x18000000)
 #define DIDmib_dot11mac_dot11OperationTable_dot11RTSThreshold \
 			(P80211DID_MKSECTION(2) | \
 			P80211DID_MKGROUP(1) | \
@@ -1476,329 +710,18 @@
 			P80211DID_MKGROUP(1) | \
 			P80211DID_MKITEM(5) | 0x18000000)
 #define DIDmib_dot11mac_dot11OperationTable_dot11MaxTransmitMSDULifetime \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_dot11mac_dot11OperationTable_dot11MaxReceiveLifetime \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(7) | 0x10000000)
-#define DIDmib_dot11mac_dot11OperationTable_dot11ManufacturerID \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(8) | 0x10000000)
-#define DIDmib_dot11mac_dot11OperationTable_dot11ProductID \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(9) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2))
-#define DIDmib_dot11mac_dot11CountersTable_dot11TransmittedFragmentCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11MulticastTransmittedFrameCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(2) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11FailedCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11RetryCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(4) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11MultipleRetryCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11FrameDuplicateCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11RTSSuccessCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(7) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11RTSFailureCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(8) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11ACKFailureCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(9) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11ReceivedFragmentCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(10) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11MulticastReceivedFrameCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(11) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11FCSErrorCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(12) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11TransmittedFrameCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(13) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11WEPUndecryptableCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(14) | 0x10000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3))
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address1 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(1) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address2 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(2) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address3 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(3) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address4 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(4) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address5 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(5) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address6 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(6) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address7 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(7) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address8 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(8) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address9 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(9) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address10 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(10) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address11 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(11) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address12 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(12) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address13 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(13) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address14 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(14) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address15 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(15) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address16 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(16) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address17 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(17) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address18 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(18) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address19 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(19) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address20 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(20) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address21 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(21) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address22 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(22) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address23 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(23) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address24 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(24) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address25 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(25) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address26 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(26) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address27 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(27) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address28 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(28) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address29 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(29) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address30 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(30) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address31 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(31) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address32 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(32) | 0x1c000000)
+                       (P80211DID_MKSECTION(2) | \
+                       P80211DID_MKGROUP(1) | \
+                       P80211DID_MKITEM(6) | 0x10000000)
 #define DIDmib_cat_dot11phy \
 			P80211DID_MKSECTION(3)
 #define DIDmib_dot11phy_dot11PhyOperationTable \
 			(P80211DID_MKSECTION(3) | \
 			P80211DID_MKGROUP(1))
-#define DIDmib_dot11phy_dot11PhyOperationTable_dot11PHYType \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyOperationTable_dot11CurrentRegDomain \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyOperationTable_dot11TempType \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyOperationTable_dot11ChannelAgilityPresent \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(4) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyOperationTable_dot11ChannelAgilityEnabled \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyOperationTable_dot11ShortPreambleEnabled \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyAntennaTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(2))
-#define DIDmib_dot11phy_dot11PhyAntennaTable_dot11CurrentTxAntenna \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(1) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyAntennaTable_dot11DiversitySupport \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(2) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyAntennaTable_dot11CurrentRxAntenna \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(3) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3))
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11NumberSupportedPowerLevels \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel1 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(2) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel2 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel3 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(4) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel4 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel5 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel6 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(7) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel7 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(8) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel8 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(9) | 0x10000000)
 #define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11CurrentTxPowerLevel \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(10) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyFHSSTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4))
-#define DIDmib_dot11phy_dot11PhyFHSSTable_dot11HopTime \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentChannelNumber \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyFHSSTable_dot11MaxDwellTime \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentDwellTime \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(4) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentSet \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(5) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentPattern \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(6) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentIndex \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(7) | 0x18000000)
+                       (P80211DID_MKSECTION(3) | \
+                       P80211DID_MKGROUP(3) | \
+                       P80211DID_MKITEM(10) | 0x18000000)
 #define DIDmib_dot11phy_dot11PhyDSSSTable \
 			(P80211DID_MKSECTION(3) | \
 			P80211DID_MKGROUP(5))
@@ -1806,97 +729,6 @@
 			(P80211DID_MKSECTION(3) | \
 			P80211DID_MKGROUP(5) | \
 			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyDSSSTable_dot11CCAModeSupported \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(2) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyDSSSTable_dot11CurrentCCAMode \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyDSSSTable_dot11EDThreshold \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(4) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyDSSSTable_dot11ShortPreambleOptionImplemented \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyDSSSTable_dot11PBCCOptionImplemented \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyIRTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(6))
-#define DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogTimerMax \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(1) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogCountMax \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogTimerMin \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(3) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogCountMin \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(4) | 0x18000000)
-#define DIDmib_dot11phy_dot11RegDomainsSupportedTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(7))
-#define DIDmib_dot11phy_dot11RegDomainsSupportedTable_dot11RegDomainsSupportIndex \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(1) | 0x1c000000)
-#define DIDmib_dot11phy_dot11RegDomainsSupportedTable_dot11RegDomainsSupportValue \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(2) | 0x14000000)
-#define DIDmib_dot11phy_dot11AntennasListTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(8))
-#define DIDmib_dot11phy_dot11AntennasListTable_dot11AntennaListIndex \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(1) | 0x1c000000)
-#define DIDmib_dot11phy_dot11AntennasListTable_dot11SupportedTxAntenna \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(2) | 0x1c000000)
-#define DIDmib_dot11phy_dot11AntennasListTable_dot11SupportedRxAntenna \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(3) | 0x1c000000)
-#define DIDmib_dot11phy_dot11AntennasListTable_dot11DiversitySelectionRx \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(4) | 0x1c000000)
-#define DIDmib_dot11phy_dot11SupportedDataRatesTxTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(9))
-#define DIDmib_dot11phy_dot11SupportedDataRatesTxTable_dot11SupportedDataRatesTxIndex \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(1) | 0x1c000000)
-#define DIDmib_dot11phy_dot11SupportedDataRatesTxTable_dot11SupportedDataRatesTxValue \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(2) | 0x14000000)
-#define DIDmib_dot11phy_dot11SupportedDataRatesRxTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(10))
-#define DIDmib_dot11phy_dot11SupportedDataRatesRxTable_dot11SupportedDataRatesRxIndex \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(1) | 0x1c000000)
-#define DIDmib_dot11phy_dot11SupportedDataRatesRxTable_dot11SupportedDataRatesRxValue \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(2) | 0x14000000)
 #define DIDmib_cat_lnx \
 			P80211DID_MKSECTION(4)
 #define DIDmib_lnx_lnxConfigTable \
@@ -1908,57 +740,6 @@
 			P80211DID_MKITEM(1) | 0x18000000)
 #define DIDmib_cat_p2 \
 			P80211DID_MKSECTION(5)
-#define DIDmib_p2_p2Table \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1))
-#define DIDmib_p2_p2Table_p2MMTx \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(1) | 0x18000000)
-#define DIDmib_p2_p2Table_p2EarlyBeacon \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_p2_p2Table_p2ReceivedFrameStatistics \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_p2_p2Table_p2CommunicationTallies \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(4) | 0x10000000)
-#define DIDmib_p2_p2Table_p2Authenticated \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_p2_p2Table_p2Associated \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_p2_p2Table_p2PowerSaveUserCount \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(7) | 0x10000000)
-#define DIDmib_p2_p2Table_p2Comment \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(8) | 0x18000000)
-#define DIDmib_p2_p2Table_p2AccessMode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(9) | 0x18000000)
-#define DIDmib_p2_p2Table_p2AccessAllow \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(10) | 0x18000000)
-#define DIDmib_p2_p2Table_p2AccessDeny \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(11) | 0x18000000)
-#define DIDmib_p2_p2Table_p2ChannelInfoResults \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(12) | 0x10000000)
 #define DIDmib_p2_p2Static \
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(2))
@@ -1966,559 +747,11 @@
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(2) | \
 			P80211DID_MKITEM(1) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfOwnMACAddress \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfDesiredSSID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(3) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfOwnChannel \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(4) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfOwnSSID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(5) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfOwnATIMWindow \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(6) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfSystemScale \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(7) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfMaxDataLength \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(8) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWDSAddress \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(9) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfPMEnabled \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(10) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfPMEPS \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(11) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfMulticastReceive \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(12) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfMaxSleepDuration \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(13) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfPMHoldoverDuration \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(14) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfOwnName \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(15) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfOwnDTIMPeriod \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(16) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWDSAddress1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(17) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWDSAddress2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(18) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWDSAddress3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(19) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWDSAddress4 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(20) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWDSAddress5 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(21) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWDSAddress6 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(22) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfMulticastPMBuffering \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(23) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWEPDefaultKeyID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(24) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWEPDefaultKey0 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(25) | 0x08000000)
-#define DIDmib_p2_p2Static_p2CnfWEPDefaultKey1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(26) | 0x08000000)
-#define DIDmib_p2_p2Static_p2CnfWEPDefaultKey2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(27) | 0x08000000)
-#define DIDmib_p2_p2Static_p2CnfWEPDefaultKey3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(28) | 0x08000000)
-#define DIDmib_p2_p2Static_p2CnfWEPFlags \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(29) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfAuthentication \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(30) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfMaxAssociatedStations \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(31) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfTxControl \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(32) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfRoamingMode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(33) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfHostAuthentication \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(34) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfRcvCrcError \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(35) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfAltRetryCount \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(36) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfBeaconInterval \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(37) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfMediumOccupancyLimit \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(38) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfCFPPeriod \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(39) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfCFPMaxDuration \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(40) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfCFPFlags \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(41) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfSTAPCFInfo \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(42) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfPriorityQUsage \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(43) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfTIMCtrl \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(44) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfThirty2Tally \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(45) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfEnhSecurity \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(46) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfShortPreamble \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(47) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfExcludeLongPreamble \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(48) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfAuthenticationRspTO \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(49) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfBasicRates \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(50) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfSupportedRates \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(51) | 0x18000000)
-#define DIDmib_p2_p2Dynamic \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3))
-#define DIDmib_p2_p2Dynamic_p2CreateIBSS \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(1) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(3) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(4) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2PromiscuousMode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(5) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold0 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(6) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(7) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(8) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(9) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold4 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(10) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold5 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(11) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold6 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(12) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold0 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(13) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(14) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(15) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(16) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold4 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(17) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold5 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(18) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold6 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(19) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl0 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(20) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(21) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(22) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(23) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl4 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(24) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl5 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(25) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl6 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(26) | 0x18000000)
-#define DIDmib_p2_p2Behavior \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(4))
-#define DIDmib_p2_p2Behavior_p2TickTime \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(1) | 0x18000000)
-#define DIDmib_p2_p2NIC \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5))
-#define DIDmib_p2_p2NIC_p2MaxLoadTime \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2DLBufferPage \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(2) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2DLBufferOffset \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2DLBufferLength \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(4) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2PRIIdentity \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2PRISupRange \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2CFIActRanges \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(7) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2NICSerialNumber \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(8) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2NICIdentity \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(9) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2MFISupRange \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(10) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2CFISupRange \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(11) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2ChannelList \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(12) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2RegulatoryDomains \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(13) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2TempType \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(14) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2STAIdentity \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(15) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2STASupRange \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(16) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2MFIActRanges \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(17) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2STACFIActRanges \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(18) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2BuildSequence \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(19) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2PrimaryFWID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(20) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2SecondaryFWID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(21) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2TertiaryFWID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(22) | 0x10000000)
 #define DIDmib_p2_p2MAC \
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(6))
-#define DIDmib_p2_p2MAC_p2PortStatus \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentSSID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(2) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentBSSID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CommsQuality \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(4) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CommsQualityCQ \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CommsQualityASL \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CommsQualityANL \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(7) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2dbmCommsQuality \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(8) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2dbmCommsQualityCQ \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(9) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2dbmCommsQualityASL \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(10) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2dbmCommsQualityANL \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(11) | 0x10000000)
 #define DIDmib_p2_p2MAC_p2CurrentTxRate \
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(6) | \
 			P80211DID_MKITEM(12) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentBeaconInterval \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(13) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2StaCurrentScaleThresholds \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(14) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2APCurrentScaleThresholds \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(15) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2ProtocolRspTime \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(16) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2ShortRetryLimit \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(17) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2LongRetryLimit \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(18) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2MaxTransmitLifetime \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(19) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2MaxReceiveLifetime \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(20) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CFPollable \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(21) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2AuthenticationAlgorithms \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(22) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2PrivacyOptionImplemented \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(23) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentTxRate1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(24) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentTxRate2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(25) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentTxRate3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(26) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentTxRate4 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(27) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentTxRate5 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(28) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentTxRate6 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(29) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2OwnMACAddress \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(30) | 0x10000000)
-#define DIDmib_p2_p2Modem \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7))
-#define DIDmib_p2_p2Modem_p2PHYType \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_p2_p2Modem_p2CurrentChannel \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(2) | 0x10000000)
-#define DIDmib_p2_p2Modem_p2CurrentPowerState \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_p2_p2Modem_p2CCAMode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(4) | 0x10000000)
-#define DIDmib_p2_p2Modem_p2SupportedDataRates \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_p2_p2Modem_p2TxPowerMax \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(6) | 0x18000000)
 #endif
diff --git a/drivers/staging/wlan-ng/p80211metamib.h b/drivers/staging/wlan-ng/p80211metamib.h
index 19867fd314eb..9cd72ede441c 100644
--- a/drivers/staging/wlan-ng/p80211metamib.h
+++ b/drivers/staging/wlan-ng/p80211metamib.h
@@ -93,7 +93,7 @@
 /* category metadata list */
 
 extern catlistitem_t mib_catlist[];
-extern UINT32 mib_catlist_size;
+extern u32 mib_catlist_size;
 
 
 /*================================================================*/
diff --git a/drivers/staging/wlan-ng/p80211metamsg.h b/drivers/staging/wlan-ng/p80211metamsg.h
index 4d6dfcc79b86..6e659eae8afc 100644
--- a/drivers/staging/wlan-ng/p80211metamsg.h
+++ b/drivers/staging/wlan-ng/p80211metamsg.h
@@ -93,7 +93,7 @@
 /* category metadata list */
 
 extern catlistitem_t msg_catlist[];
-extern UINT32 msg_catlist_size;
+extern u32 msg_catlist_size;
 
 
 /*================================================================*/
diff --git a/drivers/staging/wlan-ng/p80211metastruct.h b/drivers/staging/wlan-ng/p80211metastruct.h
index 715f4b2adc65..d2258b0e89c2 100644
--- a/drivers/staging/wlan-ng/p80211metastruct.h
+++ b/drivers/staging/wlan-ng/p80211metastruct.h
@@ -50,47 +50,36 @@
 
 typedef struct p80211msg_dot11req_mibget
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_unk392_t	mibattribute	;
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_dot11req_mibget_t;
 
 typedef struct p80211msg_dot11req_mibset
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_unk392_t	mibattribute	;
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_dot11req_mibset_t;
 
-typedef struct p80211msg_dot11req_powermgmt
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	powermgmtmode	;
-	p80211item_uint32_t	wakeup	;
-	p80211item_uint32_t	receivedtims	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_powermgmt_t;
-
 typedef struct p80211msg_dot11req_scan
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	bsstype	;
 	p80211item_pstr6_t	bssid	;
-	UINT8	pad_0C[1]	;
+	u8	pad_0C[1]	;
 	p80211item_pstr32_t	ssid	;
-	UINT8	pad_1D[3]	;
+	u8	pad_1D[3]	;
 	p80211item_uint32_t	scantype	;
 	p80211item_uint32_t	probedelay	;
 	p80211item_pstr14_t	channellist	;
-	UINT8	pad_2C[1]	;
+	u8	pad_2C[1]	;
 	p80211item_uint32_t	minchanneltime	;
 	p80211item_uint32_t	maxchanneltime	;
 	p80211item_uint32_t	resultcode	;
@@ -100,17 +89,17 @@ typedef struct p80211msg_dot11req_scan
 
 typedef struct p80211msg_dot11req_scan_results
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	bssindex	;
 	p80211item_uint32_t	resultcode	;
 	p80211item_uint32_t	signal	;
 	p80211item_uint32_t	noise	;
 	p80211item_pstr6_t	bssid	;
-	UINT8	pad_3C[1]	;
+	u8	pad_3C[1]	;
 	p80211item_pstr32_t	ssid	;
-	UINT8	pad_4D[3]	;
+	u8	pad_4D[3]	;
 	p80211item_uint32_t	bsstype	;
 	p80211item_uint32_t	beaconperiod	;
 	p80211item_uint32_t	dtimperiod	;
@@ -147,115 +136,13 @@ typedef struct p80211msg_dot11req_scan_results
 	p80211item_uint32_t	supprate8	;
 } __WLAN_ATTRIB_PACK__ p80211msg_dot11req_scan_results_t;
 
-typedef struct p80211msg_dot11req_join
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	bssid	;
-	UINT8	pad_5C[1]	;
-	p80211item_uint32_t	joinfailuretimeout	;
-	p80211item_uint32_t	basicrate1	;
-	p80211item_uint32_t	basicrate2	;
-	p80211item_uint32_t	basicrate3	;
-	p80211item_uint32_t	basicrate4	;
-	p80211item_uint32_t	basicrate5	;
-	p80211item_uint32_t	basicrate6	;
-	p80211item_uint32_t	basicrate7	;
-	p80211item_uint32_t	basicrate8	;
-	p80211item_uint32_t	operationalrate1	;
-	p80211item_uint32_t	operationalrate2	;
-	p80211item_uint32_t	operationalrate3	;
-	p80211item_uint32_t	operationalrate4	;
-	p80211item_uint32_t	operationalrate5	;
-	p80211item_uint32_t	operationalrate6	;
-	p80211item_uint32_t	operationalrate7	;
-	p80211item_uint32_t	operationalrate8	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_join_t;
-
-typedef struct p80211msg_dot11req_authenticate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_6C[1]	;
-	p80211item_uint32_t	authenticationtype	;
-	p80211item_uint32_t	authenticationfailuretimeout	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_authenticate_t;
-
-typedef struct p80211msg_dot11req_deauthenticate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_7C[1]	;
-	p80211item_uint32_t	reasoncode	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_deauthenticate_t;
-
-typedef struct p80211msg_dot11req_associate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_8C[1]	;
-	p80211item_uint32_t	associatefailuretimeout	;
-	p80211item_uint32_t	cfpollable	;
-	p80211item_uint32_t	cfpollreq	;
-	p80211item_uint32_t	privacy	;
-	p80211item_uint32_t	listeninterval	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_associate_t;
-
-typedef struct p80211msg_dot11req_reassociate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	newapaddress	;
-	UINT8	pad_9C[1]	;
-	p80211item_uint32_t	reassociatefailuretimeout	;
-	p80211item_uint32_t	cfpollable	;
-	p80211item_uint32_t	cfpollreq	;
-	p80211item_uint32_t	privacy	;
-	p80211item_uint32_t	listeninterval	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_reassociate_t;
-
-typedef struct p80211msg_dot11req_disassociate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_10C[1]	;
-	p80211item_uint32_t	reasoncode	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_disassociate_t;
-
-typedef struct p80211msg_dot11req_reset
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	setdefaultmib	;
-	p80211item_pstr6_t	macaddress	;
-	UINT8	pad_11C[1]	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_reset_t;
-
 typedef struct p80211msg_dot11req_start
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_pstr32_t	ssid	;
-	UINT8	pad_12D[3]	;
+	u8	pad_12D[3]	;
 	p80211item_uint32_t	bsstype	;
 	p80211item_uint32_t	beaconperiod	;
 	p80211item_uint32_t	dtimperiod	;
@@ -288,72 +175,20 @@ typedef struct p80211msg_dot11req_start
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_dot11req_start_t;
 
-typedef struct p80211msg_dot11ind_authenticate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_13C[1]	;
-	p80211item_uint32_t	authenticationtype	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11ind_authenticate_t;
-
-typedef struct p80211msg_dot11ind_deauthenticate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_14C[1]	;
-	p80211item_uint32_t	reasoncode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11ind_deauthenticate_t;
-
-typedef struct p80211msg_dot11ind_associate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_15C[1]	;
-	p80211item_uint32_t	aid	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11ind_associate_t;
-
-typedef struct p80211msg_dot11ind_reassociate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_16C[1]	;
-	p80211item_uint32_t	aid	;
-	p80211item_pstr6_t	oldapaddress	;
-	UINT8	pad_17C[1]	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11ind_reassociate_t;
-
-typedef struct p80211msg_dot11ind_disassociate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_18C[1]	;
-	p80211item_uint32_t	reasoncode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11ind_disassociate_t;
-
 typedef struct p80211msg_lnxreq_ifstate
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	ifstate	;
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_lnxreq_ifstate_t;
 
 typedef struct p80211msg_lnxreq_wlansniff
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	enable	;
 	p80211item_uint32_t	channel	;
 	p80211item_uint32_t	prismheader	;
@@ -366,9 +201,9 @@ typedef struct p80211msg_lnxreq_wlansniff
 
 typedef struct p80211msg_lnxreq_hostwep
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	resultcode	;
 	p80211item_uint32_t	decrypt	;
 	p80211item_uint32_t	encrypt	;
@@ -376,9 +211,9 @@ typedef struct p80211msg_lnxreq_hostwep
 
 typedef struct p80211msg_lnxreq_commsquality
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	resultcode	;
 	p80211item_uint32_t	dbm	;
 	p80211item_uint32_t	link	;
@@ -388,173 +223,29 @@ typedef struct p80211msg_lnxreq_commsquality
 
 typedef struct p80211msg_lnxreq_autojoin
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_pstr32_t	ssid	;
-	UINT8	pad_19D[3]	;
+	u8	pad_19D[3]	;
 	p80211item_uint32_t	authtype	;
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_lnxreq_autojoin_t;
 
-typedef struct p80211msg_lnxind_wlansniffrm
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	hosttime	;
-	p80211item_uint32_t	mactime	;
-	p80211item_uint32_t	channel	;
-	p80211item_uint32_t	rssi	;
-	p80211item_uint32_t	sq	;
-	p80211item_uint32_t	signal	;
-	p80211item_uint32_t	noise	;
-	p80211item_uint32_t	rate	;
-	p80211item_uint32_t	istx	;
-	p80211item_uint32_t	frmlen	;
-} __WLAN_ATTRIB_PACK__ p80211msg_lnxind_wlansniffrm_t;
-
-typedef struct p80211msg_lnxind_roam
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	reason	;
-} __WLAN_ATTRIB_PACK__ p80211msg_lnxind_roam_t;
-
-typedef struct p80211msg_p2req_join
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	bssid	;
-	UINT8	pad_20C[1]	;
-	p80211item_uint32_t	basicrate1	;
-	p80211item_uint32_t	basicrate2	;
-	p80211item_uint32_t	basicrate3	;
-	p80211item_uint32_t	basicrate4	;
-	p80211item_uint32_t	basicrate5	;
-	p80211item_uint32_t	basicrate6	;
-	p80211item_uint32_t	basicrate7	;
-	p80211item_uint32_t	basicrate8	;
-	p80211item_uint32_t	operationalrate1	;
-	p80211item_uint32_t	operationalrate2	;
-	p80211item_uint32_t	operationalrate3	;
-	p80211item_uint32_t	operationalrate4	;
-	p80211item_uint32_t	operationalrate5	;
-	p80211item_uint32_t	operationalrate6	;
-	p80211item_uint32_t	operationalrate7	;
-	p80211item_uint32_t	operationalrate8	;
-	p80211item_pstr32_t	ssid	;
-	UINT8	pad_21D[3]	;
-	p80211item_uint32_t	channel	;
-	p80211item_uint32_t	authtype	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_join_t;
-
 typedef struct p80211msg_p2req_readpda
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_unk1024_t	pda	;
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_p2req_readpda_t;
 
-typedef struct p80211msg_p2req_readcis
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_unk1024_t	cis	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_readcis_t;
-
-typedef struct p80211msg_p2req_auxport_state
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	enable	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_auxport_state_t;
-
-typedef struct p80211msg_p2req_auxport_read
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	addr	;
-	p80211item_uint32_t	len	;
-	p80211item_unk1024_t	data	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_auxport_read_t;
-
-typedef struct p80211msg_p2req_auxport_write
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	addr	;
-	p80211item_uint32_t	len	;
-	p80211item_unk1024_t	data	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_auxport_write_t;
-
-typedef struct p80211msg_p2req_low_level
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	command	;
-	p80211item_uint32_t	param0	;
-	p80211item_uint32_t	param1	;
-	p80211item_uint32_t	param2	;
-	p80211item_uint32_t	resp0	;
-	p80211item_uint32_t	resp1	;
-	p80211item_uint32_t	resp2	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_low_level_t;
-
-typedef struct p80211msg_p2req_test_command
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	testcode	;
-	p80211item_uint32_t	testparam	;
-	p80211item_uint32_t	resultcode	;
-	p80211item_uint32_t	status	;
-	p80211item_uint32_t	resp0	;
-	p80211item_uint32_t	resp1	;
-	p80211item_uint32_t	resp2	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_test_command_t;
-
-typedef struct p80211msg_p2req_mmi_read
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	addr	;
-	p80211item_uint32_t	value	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_mmi_read_t;
-
-typedef struct p80211msg_p2req_mmi_write
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	addr	;
-	p80211item_uint32_t	data	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_mmi_write_t;
-
 typedef struct p80211msg_p2req_ramdl_state
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	enable	;
 	p80211item_uint32_t	exeaddr	;
 	p80211item_uint32_t	resultcode	;
@@ -562,9 +253,9 @@ typedef struct p80211msg_p2req_ramdl_state
 
 typedef struct p80211msg_p2req_ramdl_write
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	addr	;
 	p80211item_uint32_t	len	;
 	p80211item_unk4096_t	data	;
@@ -573,72 +264,22 @@ typedef struct p80211msg_p2req_ramdl_write
 
 typedef struct p80211msg_p2req_flashdl_state
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	enable	;
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_p2req_flashdl_state_t;
 
 typedef struct p80211msg_p2req_flashdl_write
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	addr	;
 	p80211item_uint32_t	len	;
 	p80211item_unk4096_t	data	;
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_p2req_flashdl_write_t;
 
-typedef struct p80211msg_p2req_mm_state
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	enable	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_mm_state_t;
-
-typedef struct p80211msg_p2req_dump_state
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	level	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_dump_state_t;
-
-typedef struct p80211msg_p2req_channel_info
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	channellist	;
-	p80211item_uint32_t	channeldwelltime	;
-	p80211item_uint32_t	resultcode	;
-	p80211item_uint32_t	numchinfo	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_channel_info_t;
-
-typedef struct p80211msg_p2req_channel_info_results
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	channel	;
-	p80211item_uint32_t	resultcode	;
-	p80211item_uint32_t	avgnoiselevel	;
-	p80211item_uint32_t	peaknoiselevel	;
-	p80211item_uint32_t	bssactive	;
-	p80211item_uint32_t	pcfactive	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_channel_info_results_t;
-
-typedef struct p80211msg_p2req_enable
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_enable_t;
-
 #endif
diff --git a/drivers/staging/wlan-ng/p80211mgmt.h b/drivers/staging/wlan-ng/p80211mgmt.h
index bd4c1629eabf..6cc16c3f8b15 100644
--- a/drivers/staging/wlan-ng/p80211mgmt.h
+++ b/drivers/staging/wlan-ng/p80211mgmt.h
@@ -172,14 +172,14 @@
 /* Note: These offsets are from the start of the frame data       */
 
 #define WLAN_BEACON_OFF_TS			0
-#define WLAN_BEACON_OFF_BCN_INT			8
+#define WLAN_BEACON_OFF_BCN_int			8
 #define WLAN_BEACON_OFF_CAPINFO			10
 #define WLAN_BEACON_OFF_SSID			12
 
 #define WLAN_DISASSOC_OFF_REASON		0
 
 #define WLAN_ASSOCREQ_OFF_CAP_INFO		0
-#define WLAN_ASSOCREQ_OFF_LISTEN_INT		2
+#define WLAN_ASSOCREQ_OFF_LISTEN_int		2
 #define WLAN_ASSOCREQ_OFF_SSID			4
 
 #define WLAN_ASSOCRESP_OFF_CAP_INFO		0
@@ -188,7 +188,7 @@
 #define WLAN_ASSOCRESP_OFF_SUPP_RATES		6
 
 #define WLAN_REASSOCREQ_OFF_CAP_INFO		0
-#define WLAN_REASSOCREQ_OFF_LISTEN_INT		2
+#define WLAN_REASSOCREQ_OFF_LISTEN_int		2
 #define WLAN_REASSOCREQ_OFF_CURR_AP		4
 #define WLAN_REASSOCREQ_OFF_SSID		10
 
@@ -200,7 +200,7 @@
 #define WLAN_PROBEREQ_OFF_SSID			0
 
 #define WLAN_PROBERESP_OFF_TS			0
-#define WLAN_PROBERESP_OFF_BCN_INT		8
+#define WLAN_PROBERESP_OFF_BCN_int		8
 #define WLAN_PROBERESP_OFF_CAP_INFO		10
 #define WLAN_PROBERESP_OFF_SSID			12
 
@@ -245,82 +245,82 @@
 
 typedef struct wlan_ie
 {
-	UINT8	eid;
-	UINT8	len;
+	u8	eid;
+	u8	len;
 } __WLAN_ATTRIB_PACK__ wlan_ie_t;
 
 /*-- Service Set Identity (SSID)  -----------------*/
 typedef struct wlan_ie_ssid
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT8	ssid[1];  /* may be zero, ptrs may overlap */
+	u8	eid;
+	u8	len;
+	u8	ssid[1];  /* may be zero, ptrs may overlap */
 } __WLAN_ATTRIB_PACK__ wlan_ie_ssid_t;
 
 /*-- Supported Rates  -----------------------------*/
 typedef struct wlan_ie_supp_rates
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT8	rates[1];  /* had better be at LEAST one! */
+	u8	eid;
+	u8	len;
+	u8	rates[1];  /* had better be at LEAST one! */
 } __WLAN_ATTRIB_PACK__ wlan_ie_supp_rates_t;
 
 /*-- FH Parameter Set  ----------------------------*/
 typedef struct wlan_ie_fh_parms
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT16	dwell;
-	UINT8	hopset;
-	UINT8	hoppattern;
-	UINT8	hopindex;
+	u8	eid;
+	u8	len;
+	u16	dwell;
+	u8	hopset;
+	u8	hoppattern;
+	u8	hopindex;
 } __WLAN_ATTRIB_PACK__ wlan_ie_fh_parms_t;
 
 /*-- DS Parameter Set  ----------------------------*/
 typedef struct wlan_ie_ds_parms
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT8	curr_ch;
+	u8	eid;
+	u8	len;
+	u8	curr_ch;
 } __WLAN_ATTRIB_PACK__ wlan_ie_ds_parms_t;
 
 /*-- CF Parameter Set  ----------------------------*/
 
 typedef struct wlan_ie_cf_parms
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT8	cfp_cnt;
-	UINT8	cfp_period;
-	UINT16	cfp_maxdur;
-	UINT16	cfp_durremaining;
+	u8	eid;
+	u8	len;
+	u8	cfp_cnt;
+	u8	cfp_period;
+	u16	cfp_maxdur;
+	u16	cfp_durremaining;
 } __WLAN_ATTRIB_PACK__ wlan_ie_cf_parms_t;
 
 /*-- TIM ------------------------------------------*/
 typedef struct wlan_ie_tim
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT8	dtim_cnt;
-	UINT8	dtim_period;
-	UINT8	bitmap_ctl;
-	UINT8	virt_bm[1];
+	u8	eid;
+	u8	len;
+	u8	dtim_cnt;
+	u8	dtim_period;
+	u8	bitmap_ctl;
+	u8	virt_bm[1];
 } __WLAN_ATTRIB_PACK__ wlan_ie_tim_t;
 
 /*-- IBSS Parameter Set ---------------------------*/
 typedef struct wlan_ie_ibss_parms
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT16	atim_win;
+	u8	eid;
+	u8	len;
+	u16	atim_win;
 } __WLAN_ATTRIB_PACK__ wlan_ie_ibss_parms_t;
 
 /*-- Challenge Text  ------------------------------*/
 typedef struct wlan_ie_challenge
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT8	challenge[1];
+	u8	eid;
+	u8	len;
+	u8	challenge[1];
 } __WLAN_ATTRIB_PACK__ wlan_ie_challenge_t;
 
 /*-------------------------------------------------*/
@@ -329,9 +329,9 @@ typedef struct wlan_ie_challenge
 /* prototype structure, all mgmt frame types will start with these members */
 typedef struct wlan_fr_mgmt
 {
-	UINT16			type;
-	UINT16			len;	/* DOES NOT include CRC !!!!*/
-	UINT8			*buf;
+	u16			type;
+	u16			len;	/* DOES NOT include CRC !!!!*/
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
@@ -342,16 +342,16 @@ typedef struct wlan_fr_mgmt
 /*-- Beacon ---------------------------------------*/
 typedef struct wlan_fr_beacon
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT64			*ts;
-	UINT16			*bcn_int;
-	UINT16			*cap_info;
+	u64			*ts;
+	u16			*bcn_int;
+	u16			*cap_info;
 	/*-- info elements ----------*/
 	wlan_ie_ssid_t		*ssid;
 	wlan_ie_supp_rates_t	*supp_rates;
@@ -367,9 +367,9 @@ typedef struct wlan_fr_beacon
 /*-- IBSS ATIM ------------------------------------*/
 typedef struct wlan_fr_ibssatim
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8*			buf;
+	u16			type;
+	u16			len;
+	u8*			buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
@@ -384,14 +384,14 @@ typedef struct wlan_fr_ibssatim
 /*-- Disassociation -------------------------------*/
 typedef struct wlan_fr_disassoc
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT16			*reason;
+	u16			*reason;
 
 	/*-- info elements ----------*/
 
@@ -400,15 +400,15 @@ typedef struct wlan_fr_disassoc
 /*-- Association Request --------------------------*/
 typedef struct wlan_fr_assocreq
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8*			buf;
+	u16			type;
+	u16			len;
+	u8*			buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT16			*cap_info;
-	UINT16			*listen_int;
+	u16			*cap_info;
+	u16			*listen_int;
 	/*-- info elements ----------*/
 	wlan_ie_ssid_t		*ssid;
 	wlan_ie_supp_rates_t	*supp_rates;
@@ -418,16 +418,16 @@ typedef struct wlan_fr_assocreq
 /*-- Association Response -------------------------*/
 typedef struct wlan_fr_assocresp
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT16			*cap_info;
-	UINT16			*status;
-	UINT16			*aid;
+	u16			*cap_info;
+	u16			*status;
+	u16			*aid;
 	/*-- info elements ----------*/
 	wlan_ie_supp_rates_t	*supp_rates;
 
@@ -436,16 +436,16 @@ typedef struct wlan_fr_assocresp
 /*-- Reassociation Request ------------------------*/
 typedef struct wlan_fr_reassocreq
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT16			*cap_info;
-	UINT16			*listen_int;
-	UINT8			*curr_ap;
+	u16			*cap_info;
+	u16			*listen_int;
+	u8			*curr_ap;
 	/*-- info elements ----------*/
 	wlan_ie_ssid_t		*ssid;
 	wlan_ie_supp_rates_t	*supp_rates;
@@ -455,16 +455,16 @@ typedef struct wlan_fr_reassocreq
 /*-- Reassociation Response -----------------------*/
 typedef struct wlan_fr_reassocresp
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT16			*cap_info;
-	UINT16			*status;
-	UINT16			*aid;
+	u16			*cap_info;
+	u16			*status;
+	u16			*aid;
 	/*-- info elements ----------*/
 	wlan_ie_supp_rates_t	*supp_rates;
 
@@ -473,9 +473,9 @@ typedef struct wlan_fr_reassocresp
 /*-- Probe Request --------------------------------*/
 typedef struct wlan_fr_probereq
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
@@ -489,16 +489,16 @@ typedef struct wlan_fr_probereq
 /*-- Probe Response -------------------------------*/
 typedef struct wlan_fr_proberesp
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT64			*ts;
-	UINT16			*bcn_int;
-	UINT16			*cap_info;
+	u64			*ts;
+	u16			*bcn_int;
+	u16			*cap_info;
 	/*-- info elements ----------*/
 	wlan_ie_ssid_t		*ssid;
 	wlan_ie_supp_rates_t	*supp_rates;
@@ -511,16 +511,16 @@ typedef struct wlan_fr_proberesp
 /*-- Authentication -------------------------------*/
 typedef struct wlan_fr_authen
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT16			*auth_alg;
-	UINT16			*auth_seq;
-	UINT16			*status;
+	u16			*auth_alg;
+	u16			*auth_seq;
+	u16			*status;
 	/*-- info elements ----------*/
 	wlan_ie_challenge_t	*challenge;
 
@@ -529,14 +529,14 @@ typedef struct wlan_fr_authen
 /*-- Deauthenication -----------------------------*/
 typedef struct wlan_fr_deauthen
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT16			*reason;
+	u16			*reason;
 
 	/*-- info elements ----------*/
 
diff --git a/drivers/staging/wlan-ng/p80211mod.c b/drivers/staging/wlan-ng/p80211mod.c
deleted file mode 100644
index e2c3f63be8be..000000000000
--- a/drivers/staging/wlan-ng/p80211mod.c
+++ /dev/null
@@ -1,216 +0,0 @@
-/* src/p80211/p80211mod.c
-*
-* Module entry and exit for p80211
-*
-* Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
-* --------------------------------------------------------------------
-*
-* linux-wlan
-*
-*   The contents of this file are subject to the Mozilla Public
-*   License Version 1.1 (the "License"); you may not use this file
-*   except in compliance with the License. You may obtain a copy of
-*   the License at http://www.mozilla.org/MPL/
-*
-*   Software distributed under the License is distributed on an "AS
-*   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
-*   implied. See the License for the specific language governing
-*   rights and limitations under the License.
-*
-*   Alternatively, the contents of this file may be used under the
-*   terms of the GNU Public License version 2 (the "GPL"), in which
-*   case the provisions of the GPL are applicable instead of the
-*   above.  If you wish to allow the use of your version of this file
-*   only under the terms of the GPL and not to allow others to use
-*   your version of this file under the MPL, indicate your decision
-*   by deleting the provisions above and replace them with the notice
-*   and other provisions required by the GPL.  If you do not delete
-*   the provisions above, a recipient may use your version of this
-*   file under either the MPL or the GPL.
-*
-* --------------------------------------------------------------------
-*
-* Inquiries regarding the linux-wlan Open Source project can be
-* made directly to:
-*
-* AbsoluteValue Systems Inc.
-* info@linux-wlan.com
-* http://www.linux-wlan.com
-*
-* --------------------------------------------------------------------
-*
-* Portions of the development of this software were funded by
-* Intersil Corporation as part of PRISM(R) chipset product development.
-*
-* --------------------------------------------------------------------
-*
-* This file contains the p80211.o entry and exit points defined for linux
-* kernel modules.
-*
-* Notes:
-* - all module parameters for  p80211.o should be defined here.
-*
-* --------------------------------------------------------------------
-*/
-
-/*================================================================*/
-/* System Includes */
-
-
-#include <linux/version.h>
-
-#include <linux/module.h>
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,25))
-#include <linux/moduleparam.h>
-#endif
-
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/types.h>
-#include <linux/wireless.h>
-#include <linux/netdevice.h>
-
-#include "version.h"
-#include "wlan_compat.h"
-
-/*================================================================*/
-/* Project Includes */
-
-#include "p80211types.h"
-#include "p80211hdr.h"
-#include "p80211mgmt.h"
-#include "p80211conv.h"
-#include "p80211msg.h"
-#include "p80211netdev.h"
-#include "p80211req.h"
-
-/*================================================================*/
-/* Local Constants */
-
-
-/*================================================================*/
-/* Local Macros */
-
-
-/*================================================================*/
-/* Local Types */
-
-
-/*================================================================*/
-/* Local Static Definitions */
-
-static char *version = "p80211.o: " WLAN_RELEASE;
-
-
-/*----------------------------------------------------------------*/
-/* --Module Parameters */
-
-int wlan_watchdog = 5000;
-module_param(wlan_watchdog, int, 0644);
-MODULE_PARM_DESC(wlan_watchdog, "transmit timeout in milliseconds");
-
-int wlan_wext_write = 0;
-#if WIRELESS_EXT > 12
-module_param(wlan_wext_write, int, 0644);
-MODULE_PARM_DESC(wlan_wext_write, "enable write wireless extensions");
-#endif
-
-#ifdef WLAN_INCLUDE_DEBUG
-int wlan_debug=0;
-module_param(wlan_debug, int, 0644);
-MODULE_PARM_DESC(wlan_debug, "p80211 debug level");
-#endif
-
-MODULE_LICENSE("Dual MPL/GPL");
-
-/*================================================================*/
-/* Local Function Declarations */
-
-int	init_module(void);
-void	cleanup_module(void);
-
-/*================================================================*/
-/* Function Definitions */
-
-/*----------------------------------------------------------------
-* init_module
-*
-* Module initialization routine, called once at module load time.
-*
-* Arguments:
-*	none
-*
-* Returns:
-*	0	- success
-*	~0	- failure, module is unloaded.
-*
-* Side effects:
-*	TODO: define
-*
-* Call context:
-*	process thread (insmod or modprobe)
-----------------------------------------------------------------*/
-int init_module(void)
-{
-        DBFENTER;
-
-#if 0
-        printk(KERN_NOTICE "%s (%s) Loaded\n", version, WLAN_BUILD_DATE);
-#endif
-
-	p80211netdev_startup();
-#ifdef CONFIG_HOTPLUG
-	p80211_run_sbin_hotplug(NULL, WLAN_HOTPLUG_STARTUP);
-#endif
-
-        DBFEXIT;
-        return 0;
-}
-
-
-/*----------------------------------------------------------------
-* cleanup_module
-*
-* Called at module unload time.  This is our last chance to
-* clean up after ourselves.
-*
-* Arguments:
-*	none
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	TODO: define
-*
-* Call context:
-*	process thread
-*
-----------------------------------------------------------------*/
-void cleanup_module(void)
-{
-        DBFENTER;
-
-#ifdef CONFIG_HOTPLUG
-	p80211_run_sbin_hotplug(NULL, WLAN_HOTPLUG_SHUTDOWN);
-#endif
-	p80211netdev_shutdown();
-        printk(KERN_NOTICE "%s Unloaded\n", version);
-
-        DBFEXIT;
-        return;
-}
-
-EXPORT_SYMBOL(p80211netdev_hwremoved);
-EXPORT_SYMBOL(register_wlandev);
-EXPORT_SYMBOL(p80211netdev_rx);
-EXPORT_SYMBOL(unregister_wlandev);
-EXPORT_SYMBOL(wlan_setup);
-EXPORT_SYMBOL(wlan_unsetup);
-EXPORT_SYMBOL(p80211_suspend);
-EXPORT_SYMBOL(p80211_resume);
-
-EXPORT_SYMBOL(p80211skb_free);
-EXPORT_SYMBOL(p80211skb_rxmeta_attach);
-
-EXPORT_SYMBOL(p80211wext_event_associated);
diff --git a/drivers/staging/wlan-ng/p80211msg.h b/drivers/staging/wlan-ng/p80211msg.h
index c14e9fbbd687..3a575d8cc99e 100644
--- a/drivers/staging/wlan-ng/p80211msg.h
+++ b/drivers/staging/wlan-ng/p80211msg.h
@@ -78,17 +78,17 @@
 
 typedef struct p80211msg
 {
-	UINT32	msgcode;
-	UINT32	msglen;
-	UINT8	devname[WLAN_DEVNAMELEN_MAX];
+	u32	msgcode;
+	u32	msglen;
+	u8	devname[WLAN_DEVNAMELEN_MAX];
 } __WLAN_ATTRIB_PACK__ p80211msg_t;
 
 typedef struct p80211msgd
 {
-	UINT32	msgcode;
-	UINT32	msglen;
-	UINT8	devname[WLAN_DEVNAMELEN_MAX];
-	UINT8	args[0];
+	u32	msgcode;
+	u32	msglen;
+	u8	devname[WLAN_DEVNAMELEN_MAX];
+	u8	args[0];
 } __WLAN_ATTRIB_PACK__ p80211msgd_t;
 
 /*================================================================*/
diff --git a/drivers/staging/wlan-ng/p80211netdev.c b/drivers/staging/wlan-ng/p80211netdev.c
index 2b705eaa50e8..59e5ad10dbda 100644
--- a/drivers/staging/wlan-ng/p80211netdev.c
+++ b/drivers/staging/wlan-ng/p80211netdev.c
@@ -79,15 +79,12 @@
 #include <linux/ethtool.h>
 #endif
 
-#if WIRELESS_EXT > 12
 #include <net/iw_handler.h>
-#endif
 #include <net/net_namespace.h>
 
 /*================================================================*/
 /* Project Includes */
 
-#include "version.h"
 #include "wlan_compat.h"
 #include "p80211types.h"
 #include "p80211hdr.h"
@@ -111,15 +108,6 @@
 /* Local Types */
 
 /*================================================================*/
-/* Local Static Definitions */
-
-#define __NO_VERSION__		/* prevent the static definition */
-
-#ifdef CONFIG_PROC_FS
-static struct proc_dir_entry	*proc_p80211;
-#endif
-
-/*================================================================*/
 /* Local Function Declarations */
 
 /* Support functions */
@@ -135,73 +123,24 @@ static void p80211knetdev_set_multicast_list(netdevice_t *dev);
 static int p80211knetdev_do_ioctl(netdevice_t *dev, struct ifreq *ifr, int cmd);
 static int p80211knetdev_set_mac_address(netdevice_t *dev, void *addr);
 static void p80211knetdev_tx_timeout(netdevice_t *netdev);
-static int p80211_rx_typedrop( wlandevice_t *wlandev, UINT16 fc);
-
-#ifdef CONFIG_PROC_FS
-static int
-p80211netdev_proc_read(
-	char	*page,
-	char	**start,
-	off_t	offset,
-	int	count,
-	int	*eof,
-	void	*data);
-#endif
+static int p80211_rx_typedrop( wlandevice_t *wlandev, u16 fc);
 
-/*================================================================*/
-/* Function Definitions */
+int wlan_watchdog = 5000;
+module_param(wlan_watchdog, int, 0644);
+MODULE_PARM_DESC(wlan_watchdog, "transmit timeout in milliseconds");
 
-/*----------------------------------------------------------------
-* p80211knetdev_startup
-*
-* Initialize the wlandevice/netdevice part of 802.11 services at
-* load time.
-*
-* Arguments:
-*	none
-*
-* Returns:
-*	nothing
-----------------------------------------------------------------*/
-void p80211netdev_startup(void)
-{
-	DBFENTER;
+int wlan_wext_write = 1;
+module_param(wlan_wext_write, int, 0644);
+MODULE_PARM_DESC(wlan_wext_write, "enable write wireless extensions");
 
-#ifdef CONFIG_PROC_FS
-	if (init_net.proc_net != NULL) {
-		proc_p80211 = create_proc_entry(
-				"p80211",
-				(S_IFDIR|S_IRUGO|S_IXUGO),
-				init_net.proc_net);
-	}
+#ifdef WLAN_INCLUDE_DEBUG
+int wlan_debug=0;
+module_param(wlan_debug, int, 0644);
+MODULE_PARM_DESC(wlan_debug, "p80211 debug level");
 #endif
-	DBFEXIT;
-	return;
-}
 
-/*----------------------------------------------------------------
-* p80211knetdev_shutdown
-*
-* Shutdown the wlandevice/netdevice part of 802.11 services at
-* unload time.
-*
-* Arguments:
-*	none
-*
-* Returns:
-*	nothing
-----------------------------------------------------------------*/
-void
-p80211netdev_shutdown(void)
-{
-	DBFENTER;
-#ifdef CONFIG_PROC_FS
-	if (proc_p80211 != NULL) {
-		remove_proc_entry("p80211", init_net.proc_net);
-	}
-#endif
-	DBFEXIT;
-}
+/*================================================================*/
+/* Function Definitions */
 
 /*----------------------------------------------------------------
 * p80211knetdev_init
@@ -285,10 +224,7 @@ static int p80211knetdev_open( netdevice_t *netdev )
 	if ( wlandev->open != NULL) {
 		result = wlandev->open(wlandev);
 		if ( result == 0 ) {
-#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,3,43) )
-			netdev->interrupt = 0;
-#endif
-			p80211netdev_start_queue(wlandev);
+			netif_start_queue(wlandev->netdev);
 			wlandev->state = WLAN_DEVICE_OPEN;
 		}
 	} else {
@@ -323,7 +259,7 @@ static int p80211knetdev_stop( netdevice_t *netdev )
 		result = wlandev->close(wlandev);
 	}
 
-	p80211netdev_stop_queue(wlandev);
+	netif_stop_queue(wlandev->netdev);
 	wlandev->state = WLAN_DEVICE_CLOSED;
 
 	DBFEXIT;
@@ -376,7 +312,7 @@ static void p80211netdev_rx_bh(unsigned long arg)
 	struct sk_buff *skb = NULL;
 	netdevice_t     *dev = wlandev->netdev;
 	p80211_hdr_a3_t *hdr;
-	UINT16 fc;
+	u16 fc;
 
         DBFENTER;
 
@@ -478,15 +414,6 @@ static int p80211knetdev_hard_start_xmit( struct sk_buff *skb, netdevice_t *netd
 	memset(&p80211_hdr, 0, sizeof(p80211_hdr_t));
 	memset(&p80211_wep, 0, sizeof(p80211_metawep_t));
 
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,38) )
-	if ( test_and_set_bit(0, (void*)&(netdev->tbusy)) != 0 ) {
-		/* We've been called w/ tbusy set, has the tx */
-		/* path stalled?   */
-		WLAN_LOG_DEBUG(1, "called when tbusy set\n");
-		result = 1;
-		goto failed;
-	}
-#else
 	if ( netif_queue_stopped(netdev) ) {
 		WLAN_LOG_DEBUG(1, "called when queue stopped.\n");
 		result = 1;
@@ -495,12 +422,6 @@ static int p80211knetdev_hard_start_xmit( struct sk_buff *skb, netdevice_t *netd
 
 	netif_stop_queue(netdev);
 
-	/* No timeout handling here, 2.3.38+ kernels call the
-	 * timeout function directly.
-	 * TODO: Add timeout handling.
-	*/
-#endif
-
 	/* Check to see that a valid mode is set */
 	switch( wlandev->macmode ) {
 	case WLAN_MACMODE_IBSS_STA:
@@ -513,7 +434,7 @@ static int p80211knetdev_hard_start_xmit( struct sk_buff *skb, netdevice_t *netd
 		 * TODO: we need a saner way to handle this
 		 */
 		if(skb->protocol != ETH_P_80211_RAW) {
-			p80211netdev_start_queue(wlandev);
+			netif_start_queue(wlandev->netdev);
 			WLAN_LOG_NOTICE(
 				"Tx attempt prior to association, frame dropped.\n");
 			wlandev->linux_stats.tx_dropped++;
@@ -557,7 +478,7 @@ static int p80211knetdev_hard_start_xmit( struct sk_buff *skb, netdevice_t *netd
 	if ( txresult == 0) {
 		/* success and more buf */
 		/* avail, re: hw_txdata */
-		p80211netdev_wake_queue(wlandev);
+		netif_wake_queue(wlandev->netdev);
 		result = 0;
 	} else if ( txresult == 1 ) {
 		/* success, no more avail */
@@ -619,7 +540,7 @@ static void p80211knetdev_set_multicast_list(netdevice_t *dev)
 
 static int p80211netdev_ethtool(wlandevice_t *wlandev, void __user *useraddr)
 {
-	UINT32 ethcmd;
+	u32 ethcmd;
 	struct ethtool_drvinfo info;
 	struct ethtool_value edata;
 
@@ -686,7 +607,7 @@ static int p80211netdev_ethtool(wlandevice_t *wlandev, void __user *useraddr)
 *		-EFAULT memory fault copying msg from user buffer
 *		-ENOMEM unable to allocate kernel msg buffer
 *		-ENOSYS	bad magic, it the cmd really for us?
-*		-EINTR	sleeping on cmd, awakened by signal, cmd cancelled.
+*		-EintR	sleeping on cmd, awakened by signal, cmd cancelled.
 *
 * Call Context:
 *	Process thread (ioctl caller).  TODO: SMP support may require
@@ -697,21 +618,11 @@ static int p80211knetdev_do_ioctl(netdevice_t *dev, struct ifreq *ifr, int cmd)
 	int			result = 0;
 	p80211ioctl_req_t	*req = (p80211ioctl_req_t*)ifr;
 	wlandevice_t		*wlandev = dev->ml_priv;
-	UINT8			*msgbuf;
+	u8			*msgbuf;
 	DBFENTER;
 
 	WLAN_LOG_DEBUG(2, "rx'd ioctl, cmd=%d, len=%d\n", cmd, req->len);
 
-#if WIRELESS_EXT < 13
-	/* Is this a wireless extensions ioctl? */
-	if ((cmd >= SIOCIWFIRST) && (cmd <= SIOCIWLAST)) {
-		if ((result = p80211wext_support_ioctl(dev, ifr, cmd))
-		    != (-EOPNOTSUPP)) {
-			goto bail;
-		}
-	}
-#endif
-
 #ifdef SIOCETHTOOL
 	if (cmd == SIOCETHTOOL) {
 		result = p80211netdev_ethtool(wlandev, (void __user *) ifr->ifr_data);
@@ -792,15 +703,9 @@ static int p80211knetdev_set_mac_address(netdevice_t *dev, void *addr)
 
 	DBFENTER;
 	/* If we're running, we don't allow MAC address changes */
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,38) )
-	if ( dev->start) {
-		return -EBUSY;
-	}
-#else
 	if (netif_running(dev)) {
 		return -EBUSY;
 	}
-#endif
 
 	/* Set up some convenience pointers. */
 	mibattr = &dot11req.mibattribute;
@@ -833,7 +738,7 @@ static int p80211knetdev_set_mac_address(netdevice_t *dev, void *addr)
 	resultcode->data = 0;
 
 	/* now fire the request */
-	result = p80211req_dorequest(dev->ml_priv, (UINT8 *)&dot11req);
+	result = p80211req_dorequest(dev->ml_priv, (u8 *)&dot11req);
 
 	/* If the request wasn't successful, report an error and don't
 	 * change the netdev address
@@ -909,13 +814,11 @@ int wlan_setup(wlandevice_t *wlandev)
 		     (unsigned long)wlandev);
 
 	/* Allocate and initialize the struct device */
-	dev = kmalloc(sizeof(netdevice_t), GFP_ATOMIC);
+	dev = alloc_netdev(0,"wlan%d",ether_setup);
 	if ( dev == NULL ) {
 		WLAN_LOG_ERROR("Failed to alloc netdev.\n");
 		result = 1;
 	} else {
-		memset( dev, 0, sizeof(netdevice_t));
-		ether_setup(dev);
 		wlandev->netdev = dev;
 		dev->ml_priv = wlandev;
 		dev->hard_start_xmit =	p80211knetdev_hard_start_xmit;
@@ -930,21 +833,12 @@ int wlan_setup(wlandevice_t *wlandev)
 		dev->open =		p80211knetdev_open;
 		dev->stop =		p80211knetdev_stop;
 
-#ifdef CONFIG_NET_WIRELESS
-#if ((WIRELESS_EXT < 17) && (WIRELESS_EXT < 21))
+#if (WIRELESS_EXT < 21)
 		dev->get_wireless_stats = p80211wext_get_wireless_stats;
 #endif
-#if WIRELESS_EXT > 12
 		dev->wireless_handlers = &p80211wext_handler_def;
-#endif
-#endif
 
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,38) )
-		dev->tbusy = 1;
-		dev->start = 0;
-#else
 		netif_stop_queue(dev);
-#endif
 #ifdef HAVE_CHANGE_MTU
 		dev->change_mtu = wlan_change_mtu;
 #endif
@@ -1027,44 +921,12 @@ int wlan_unsetup(wlandevice_t *wlandev)
 int register_wlandev(wlandevice_t *wlandev)
 {
 	int		i = 0;
-	netdevice_t	*dev = wlandev->netdev;
 
 	DBFENTER;
 
-	i = dev_alloc_name(wlandev->netdev, "wlan%d");
-	if (i >= 0) {
-		i = register_netdev(wlandev->netdev);
-	}
-	if (i != 0) {
-		return -EIO;
-	}
-
-#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0) )
-	dev->name = wlandev->name;
-#else
-	strcpy(wlandev->name, dev->name);
-#endif
-
-#ifdef CONFIG_PROC_FS
-	if (proc_p80211) {
-		wlandev->procdir = proc_mkdir(wlandev->name, proc_p80211);
-		if ( wlandev->procdir )
-			wlandev->procwlandev =
-				create_proc_read_entry("wlandev", 0,
-						       wlandev->procdir,
-						       p80211netdev_proc_read,
-						       wlandev);
-		if (wlandev->nsd_proc_read)
-			create_proc_read_entry("nsd", 0,
-					       wlandev->procdir,
-					       wlandev->nsd_proc_read,
-					       wlandev);
-	}
-#endif
-
-#ifdef CONFIG_HOTPLUG
-	p80211_run_sbin_hotplug(wlandev, WLAN_HOTPLUG_REGISTER);
-#endif
+	i = register_netdev(wlandev->netdev);
+	if (i)
+		return i;
 
 	DBFEXIT;
 	return 0;
@@ -1094,22 +956,6 @@ int unregister_wlandev(wlandevice_t *wlandev)
 
 	DBFENTER;
 
-#ifdef CONFIG_HOTPLUG
-	p80211_run_sbin_hotplug(wlandev, WLAN_HOTPLUG_REMOVE);
-#endif
-
-#ifdef CONFIG_PROC_FS
-	if ( wlandev->procwlandev ) {
-		remove_proc_entry("wlandev", wlandev->procdir);
-	}
-	if ( wlandev->nsd_proc_read ) {
-		remove_proc_entry("nsd", wlandev->procdir);
-	}
-	if (wlandev->procdir) {
-		remove_proc_entry(wlandev->name, proc_p80211);
-	}
-#endif
-
 	unregister_netdev(wlandev->netdev);
 
 	/* Now to clean out the rx queue */
@@ -1121,76 +967,6 @@ int unregister_wlandev(wlandevice_t *wlandev)
 	return 0;
 }
 
-#ifdef CONFIG_PROC_FS
-/*----------------------------------------------------------------
-* proc_read
-*
-* Read function for /proc/net/p80211/<device>/wlandev
-*
-* Arguments:
-*	buf
-*	start
-*	offset
-*	count
-*	eof
-*	data
-* Returns:
-*	zero on success, non-zero otherwise.
-* Call Context:
-*	Can be either interrupt or not.
-----------------------------------------------------------------*/
-static int
-p80211netdev_proc_read(
-	char	*page,
-	char	**start,
-	off_t	offset,
-	int	count,
-	int	*eof,
-	void	*data)
-{
-	char	 *p = page;
-	wlandevice_t *wlandev = (wlandevice_t *) data;
-
-	DBFENTER;
-	if (offset != 0) {
-		*eof = 1;
-		goto exit;
-	}
-
-	p += sprintf(p, "p80211 version: %s (%s)\n\n",
-		     WLAN_RELEASE, WLAN_BUILD_DATE);
-	p += sprintf(p, "name       : %s\n", wlandev->name);
-	p += sprintf(p, "nsd name   : %s\n", wlandev->nsdname);
-	p += sprintf(p, "address    : %02x:%02x:%02x:%02x:%02x:%02x\n",
-		     wlandev->netdev->dev_addr[0], wlandev->netdev->dev_addr[1], wlandev->netdev->dev_addr[2],
-		     wlandev->netdev->dev_addr[3], wlandev->netdev->dev_addr[4], wlandev->netdev->dev_addr[5]);
-	p += sprintf(p, "nsd caps   : %s%s%s%s%s%s%s%s%s%s\n",
-		     (wlandev->nsdcaps & P80211_NSDCAP_HARDWAREWEP) ? "wep_hw " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_TIEDWEP) ? "wep_tied " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_NOHOSTWEP) ? "wep_hw_only " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_PBCC) ? "pbcc " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_SHORT_PREAMBLE) ? "short_preamble " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_AGILITY) ? "agility " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_AP_RETRANSMIT) ? "ap_retransmit " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_HWFRAGMENT) ? "hw_frag " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_AUTOJOIN) ? "autojoin " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_NOSCAN) ? "" : "scan ");
-
-
-	p += sprintf(p, "bssid      : %02x:%02x:%02x:%02x:%02x:%02x\n",
-		     wlandev->bssid[0], wlandev->bssid[1], wlandev->bssid[2],
-		     wlandev->bssid[3], wlandev->bssid[4], wlandev->bssid[5]);
-
-	p += sprintf(p, "Enabled    : %s%s\n",
-		     (wlandev->shortpreamble) ? "short_preamble " : "",
-		     (wlandev->hostwep & HOSTWEP_PRIVACYINVOKED) ? "privacy" : "");
-
-
- exit:
-	DBFEXIT;
-	return (p - page);
-}
-#endif
 
 /*----------------------------------------------------------------
 * p80211netdev_hwremoved
@@ -1227,7 +1003,7 @@ void p80211netdev_hwremoved(wlandevice_t *wlandev)
 	DBFENTER;
 	wlandev->hwremoved = 1;
 	if ( wlandev->state == WLAN_DEVICE_OPEN) {
-		p80211netdev_stop_queue(wlandev);
+		netif_stop_queue(wlandev->netdev);
 	}
 
 	netif_device_detach(wlandev->netdev);
@@ -1257,10 +1033,10 @@ void p80211netdev_hwremoved(wlandevice_t *wlandev)
 * Call context:
 *	interrupt
 ----------------------------------------------------------------*/
-static int p80211_rx_typedrop( wlandevice_t *wlandev, UINT16 fc)
+static int p80211_rx_typedrop( wlandevice_t *wlandev, u16 fc)
 {
-	UINT16	ftype;
-	UINT16	fstype;
+	u16	ftype;
+	u16	fstype;
 	int	drop = 0;
 	/* Classify frame, increment counter */
 	ftype = WLAN_GET_FC_FTYPE(fc);
@@ -1416,75 +1192,6 @@ static int p80211_rx_typedrop( wlandevice_t *wlandev, UINT16 fc)
 	return drop;
 }
 
-#ifdef CONFIG_HOTPLUG
-/* Notify userspace when a netdevice event occurs,
- * by running '/sbin/hotplug net' with certain
- * environment variables set.
- */
-int p80211_run_sbin_hotplug(wlandevice_t *wlandev, char *action)
-{
-        char *argv[3], *envp[7], ifname[12 + IFNAMSIZ], action_str[32];
-	char nsdname[32], wlan_wext[32];
-        int i;
-
-	if (wlandev) {
-		sprintf(ifname, "INTERFACE=%s", wlandev->name);
-		sprintf(nsdname, "NSDNAME=%s", wlandev->nsdname);
-	} else {
-		sprintf(ifname, "INTERFACE=null");
-		sprintf(nsdname, "NSDNAME=null");
-	}
-
-	sprintf(wlan_wext, "WLAN_WEXT=%s", wlan_wext_write ? "y" : "");
-        sprintf(action_str, "ACTION=%s", action);
-
-        i = 0;
-        argv[i++] = hotplug_path;
-        argv[i++] = "wlan";
-        argv[i] = NULL;
-
-        i = 0;
-        /* minimal command environment */
-        envp [i++] = "HOME=/";
-        envp [i++] = "PATH=/sbin:/bin:/usr/sbin:/usr/bin";
-        envp [i++] = ifname;
-        envp [i++] = action_str;
-        envp [i++] = nsdname;
-        envp [i++] = wlan_wext;
-        envp [i] = NULL;
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,62))
-        return call_usermodehelper(argv [0], argv, envp);
-#else
-        return call_usermodehelper(argv [0], argv, envp, 0);
-#endif
-}
-
-#endif
-
-
-void    p80211_suspend(wlandevice_t *wlandev)
-{
-	DBFENTER;
-
-#ifdef CONFIG_HOTPLUG
-	p80211_run_sbin_hotplug(wlandev, WLAN_HOTPLUG_SUSPEND);
-#endif
-
-	DBFEXIT;
-}
-
-void    p80211_resume(wlandevice_t *wlandev)
-{
-	DBFENTER;
-
-#ifdef CONFIG_HOTPLUG
-	p80211_run_sbin_hotplug(wlandev, WLAN_HOTPLUG_RESUME);
-#endif
-
-	DBFEXIT;
-}
-
 static void p80211knetdev_tx_timeout( netdevice_t *netdev)
 {
 	wlandevice_t	*wlandev = netdev->ml_priv;
@@ -1495,7 +1202,7 @@ static void p80211knetdev_tx_timeout( netdevice_t *netdev)
 	} else {
 		WLAN_LOG_WARNING("Implement tx_timeout for %s\n",
 				 wlandev->nsdname);
-		p80211netdev_wake_queue(wlandev);
+		netif_wake_queue(wlandev->netdev);
 	}
 
 	DBFEXIT;
diff --git a/drivers/staging/wlan-ng/p80211netdev.h b/drivers/staging/wlan-ng/p80211netdev.h
index 9b2e0cdcb449..940146fba9c1 100644
--- a/drivers/staging/wlan-ng/p80211netdev.h
+++ b/drivers/staging/wlan-ng/p80211netdev.h
@@ -113,54 +113,48 @@
 /* Received frame statistics */
 typedef struct p80211_frmrx_t
 {
-	UINT32	mgmt;
-	UINT32	assocreq;
-	UINT32	assocresp;
-	UINT32	reassocreq;
-	UINT32	reassocresp;
-	UINT32	probereq;
-	UINT32	proberesp;
-	UINT32	beacon;
-	UINT32	atim;
-	UINT32	disassoc;
-	UINT32	authen;
-	UINT32	deauthen;
-	UINT32	mgmt_unknown;
-	UINT32	ctl;
-	UINT32	pspoll;
-	UINT32	rts;
-	UINT32	cts;
-	UINT32	ack;
-	UINT32	cfend;
-	UINT32	cfendcfack;
-	UINT32	ctl_unknown;
-	UINT32	data;
-	UINT32	dataonly;
-	UINT32	data_cfack;
-	UINT32	data_cfpoll;
-	UINT32	data__cfack_cfpoll;
-	UINT32	null;
-	UINT32	cfack;
-	UINT32	cfpoll;
-	UINT32	cfack_cfpoll;
-	UINT32	data_unknown;
-	UINT32  decrypt;
-	UINT32  decrypt_err;
+	u32	mgmt;
+	u32	assocreq;
+	u32	assocresp;
+	u32	reassocreq;
+	u32	reassocresp;
+	u32	probereq;
+	u32	proberesp;
+	u32	beacon;
+	u32	atim;
+	u32	disassoc;
+	u32	authen;
+	u32	deauthen;
+	u32	mgmt_unknown;
+	u32	ctl;
+	u32	pspoll;
+	u32	rts;
+	u32	cts;
+	u32	ack;
+	u32	cfend;
+	u32	cfendcfack;
+	u32	ctl_unknown;
+	u32	data;
+	u32	dataonly;
+	u32	data_cfack;
+	u32	data_cfpoll;
+	u32	data__cfack_cfpoll;
+	u32	null;
+	u32	cfack;
+	u32	cfpoll;
+	u32	cfack_cfpoll;
+	u32	data_unknown;
+	u32  decrypt;
+	u32  decrypt_err;
 } p80211_frmrx_t;
 
-#ifdef WIRELESS_EXT
 /* called by /proc/net/wireless */
 struct iw_statistics* p80211wext_get_wireless_stats(netdevice_t *dev);
 /* wireless extensions' ioctls */
 int p80211wext_support_ioctl(netdevice_t *dev, struct ifreq *ifr, int cmd);
-#if WIRELESS_EXT > 12
 extern struct iw_handler_def p80211wext_handler_def;
-#endif
-
 int p80211wext_event_associated(struct wlandevice *wlandev, int assoc);
 
-#endif /* wireless extensions */
-
 /* WEP stuff */
 #define NUM_WEPKEYS 4
 #define MAX_KEYLEN 32
@@ -184,18 +178,18 @@ typedef struct wlandevice
 	char		name[WLAN_DEVNAMELEN_MAX]; /* Dev name, from register_wlandev()*/
 	char		*nsdname;
 
-	UINT32          state;          /* Device I/F state (open/closed) */
-	UINT32		msdstate;	/* state of underlying driver */
-	UINT32		hwremoved;	/* Has the hw been yanked out? */
+	u32          state;          /* Device I/F state (open/closed) */
+	u32		msdstate;	/* state of underlying driver */
+	u32		hwremoved;	/* Has the hw been yanked out? */
 
 	/* Hardware config */
-	UINT		irq;
-	UINT		iobase;
-	UINT		membase;
-	UINT32          nsdcaps;  /* NSD Capabilities flags */
+	unsigned int		irq;
+	unsigned int		iobase;
+	unsigned int		membase;
+	u32          nsdcaps;  /* NSD Capabilities flags */
 
 	/* Config vars */
-	UINT		ethconv;
+	unsigned int		ethconv;
 
 	/* device methods (init by MSD, used by p80211 */
 	int		(*open)(struct wlandevice *wlandev);
@@ -207,20 +201,15 @@ typedef struct wlandevice
 					      netdevice_t *dev);
 	void		(*tx_timeout)(struct wlandevice *wlandev);
 
-#ifdef CONFIG_PROC_FS
-	int             (*nsd_proc_read)(char *page, char **start, off_t offset, int count, int	*eof, void *data);
-#endif
-
 	/* 802.11 State */
-	UINT8		bssid[WLAN_BSSID_LEN];
+	u8		bssid[WLAN_BSSID_LEN];
 	p80211pstr32_t	ssid;
-	UINT32		macmode;
+	u32		macmode;
 	int             linkstatus;
-	int             shortpreamble;  /* C bool */
 
 	/* WEP State */
-	UINT8 wep_keys[NUM_WEPKEYS][MAX_KEYLEN];
-	UINT8 wep_keylens[NUM_WEPKEYS];
+	u8 wep_keys[NUM_WEPKEYS][MAX_KEYLEN];
+	u8 wep_keylens[NUM_WEPKEYS];
 	int   hostwep;
 
 	/* Request/Confirm i/f state (used by p80211) */
@@ -232,12 +221,6 @@ typedef struct wlandevice
 	netdevice_t		*netdev;	/* ptr to linux netdevice */
 	struct net_device_stats linux_stats;
 
-#ifdef CONFIG_PROC_FS
-	/* Procfs support */
-	struct proc_dir_entry	*procdir;
-	struct proc_dir_entry	*procwlandev;
-#endif
-
 	/* Rx bottom half */
 	struct tasklet_struct	rx_bh;
 
@@ -246,29 +229,18 @@ typedef struct wlandevice
 	/* 802.11 device statistics */
 	struct p80211_frmrx_t	rx;
 
-/* compatibility to wireless extensions */
-#ifdef WIRELESS_EXT
 	struct iw_statistics	wstats;
 
 	/* jkriegl: iwspy fields */
-        UINT8			spy_number;
+        u8			spy_number;
         char			spy_address[IW_MAX_SPY][ETH_ALEN];
         struct iw_quality       spy_stat[IW_MAX_SPY];
-
-#endif
-
 } wlandevice_t;
 
 /* WEP stuff */
-int wep_change_key(wlandevice_t *wlandev, int keynum, UINT8* key, int keylen);
-int wep_decrypt(wlandevice_t *wlandev, UINT8 *buf, UINT32 len, int key_override, UINT8 *iv, UINT8 *icv);
-int wep_encrypt(wlandevice_t *wlandev, UINT8 *buf, UINT8 *dst, UINT32 len, int keynum, UINT8 *iv, UINT8 *icv);
-
-/*================================================================*/
-/* Externs */
-
-/*================================================================*/
-/* Function Declarations */
+int wep_change_key(wlandevice_t *wlandev, int keynum, u8* key, int keylen);
+int wep_decrypt(wlandevice_t *wlandev, u8 *buf, u32 len, int key_override, u8 *iv, u8 *icv);
+int wep_encrypt(wlandevice_t *wlandev, u8 *buf, u8 *dst, u32 len, int keynum, u8 *iv, u8 *icv);
 
 void	p80211netdev_startup(void);
 void	p80211netdev_shutdown(void);
@@ -278,59 +250,5 @@ int	register_wlandev(wlandevice_t *wlandev);
 int	unregister_wlandev(wlandevice_t *wlandev);
 void	p80211netdev_rx(wlandevice_t *wlandev, struct sk_buff *skb);
 void	p80211netdev_hwremoved(wlandevice_t *wlandev);
-void    p80211_suspend(wlandevice_t *wlandev);
-void    p80211_resume(wlandevice_t *wlandev);
-
-/*================================================================*/
-/* Function Definitions */
-
-static inline void
-p80211netdev_stop_queue(wlandevice_t *wlandev)
-{
-	if ( !wlandev ) return;
-	if ( !wlandev->netdev ) return;
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,38) )
-	wlandev->netdev->tbusy = 1;
-	wlandev->netdev->start = 0;
-#else
-	netif_stop_queue(wlandev->netdev);
-#endif
-}
-
-static inline void
-p80211netdev_start_queue(wlandevice_t *wlandev)
-{
-	if ( !wlandev ) return;
-	if ( !wlandev->netdev ) return;
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,38) )
-	wlandev->netdev->tbusy = 0;
-	wlandev->netdev->start = 1;
-#else
-	netif_start_queue(wlandev->netdev);
-#endif
-}
-
-static inline void
-p80211netdev_wake_queue(wlandevice_t *wlandev)
-{
-	if ( !wlandev ) return;
-	if ( !wlandev->netdev ) return;
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,38) )
-	wlandev->netdev->tbusy = 0;
-	mark_bh(NET_BH);
-#else
-	netif_wake_queue(wlandev->netdev);
-#endif
-}
-
-#ifdef CONFIG_HOTPLUG
-#define WLAN_HOTPLUG_REGISTER "register"
-#define WLAN_HOTPLUG_REMOVE   "remove"
-#define WLAN_HOTPLUG_STARTUP  "startup"
-#define WLAN_HOTPLUG_SHUTDOWN "shutdown"
-#define WLAN_HOTPLUG_SUSPEND "suspend"
-#define WLAN_HOTPLUG_RESUME "resume"
-int p80211_run_sbin_hotplug(wlandevice_t *wlandev, char *action);
-#endif
 
 #endif
diff --git a/drivers/staging/wlan-ng/p80211req.c b/drivers/staging/wlan-ng/p80211req.c
index 0233abeccc4b..6e20bff0e67e 100644
--- a/drivers/staging/wlan-ng/p80211req.c
+++ b/drivers/staging/wlan-ng/p80211req.c
@@ -54,7 +54,6 @@
 /* System Includes */
 
 
-#include <linux/version.h>
 
 #include <linux/module.h>
 #include <linux/kernel.h>
@@ -68,7 +67,6 @@
 #include <net/sock.h>
 #include <linux/netlink.h>
 
-#include "version.h"
 #include "wlan_compat.h"
 
 /*================================================================*/
@@ -126,7 +124,7 @@ static int p80211req_mibset_mibget(wlandevice_t *wlandev, p80211msg_dot11req_mib
 *	Potentially blocks the caller, so it's a good idea to
 *	not call this function from an interrupt context.
 ----------------------------------------------------------------*/
-int p80211req_dorequest( wlandevice_t *wlandev, UINT8 *msgbuf)
+int p80211req_dorequest( wlandevice_t *wlandev, u8 *msgbuf)
 {
 	int		result = 0;
 	p80211msg_t	*msg = (p80211msg_t*)msgbuf;
@@ -224,38 +222,11 @@ static int p80211req_mibset_mibget(wlandevice_t *wlandev,
 {
 	p80211itemd_t   *mibitem = (p80211itemd_t *) mib_msg->mibattribute.data;
 	p80211pstrd_t  *pstr = (p80211pstrd_t*) mibitem->data;
-	UINT8 *key = mibitem->data + sizeof(p80211pstrd_t);
+	u8 *key = mibitem->data + sizeof(p80211pstrd_t);
 
 	DBFENTER;
 
 	switch (mibitem->did) {
-	case DIDmib_dot11smt_p80211Table_p80211_ifstate: {
-		UINT32 *data = (UINT32 *) mibitem->data;
-		if (isget)
-			switch (wlandev->msdstate) {
-			case WLAN_MSD_HWPRESENT:
-				*data = P80211ENUM_ifstate_disable;
-				break;
-			case WLAN_MSD_FWLOAD:
-				*data = P80211ENUM_ifstate_fwload;
-				break;
-			case WLAN_MSD_RUNNING:
-				*data = P80211ENUM_ifstate_enable;
-				break;
-			default:
-				*data = P80211ENUM_ifstate_enable;
-			}
-		break;
-	}
-	case DIDmib_dot11phy_dot11PhyOperationTable_dot11ShortPreambleEnabled: {
-		UINT32 *data = (UINT32 *) mibitem->data;
-
-		if (isget)
-			*data = wlandev->shortpreamble;
-		else
-			wlandev->shortpreamble = *data;
-		break;
-	}
 	case DIDmib_dot11smt_dot11WEPDefaultKeysTable_dot11WEPDefaultKey0: {
 		if (!isget)
 			wep_change_key(wlandev, 0, key, pstr->len);
@@ -277,7 +248,7 @@ static int p80211req_mibset_mibget(wlandevice_t *wlandev,
 		break;
 	}
 	case DIDmib_dot11smt_dot11PrivacyTable_dot11WEPDefaultKeyID: {
-		UINT32 *data = (UINT32 *) mibitem->data;
+		u32 *data = (u32 *) mibitem->data;
 
 		if (isget) {
 			*data = wlandev->hostwep & HOSTWEP_DEFAULTKEY_MASK;
@@ -289,7 +260,7 @@ static int p80211req_mibset_mibget(wlandevice_t *wlandev,
 		break;
 	}
 	case DIDmib_dot11smt_dot11PrivacyTable_dot11PrivacyInvoked: {
-		UINT32 *data = (UINT32 *) mibitem->data;
+		u32 *data = (u32 *) mibitem->data;
 
 		if (isget) {
 			if (wlandev->hostwep & HOSTWEP_PRIVACYINVOKED)
@@ -304,7 +275,7 @@ static int p80211req_mibset_mibget(wlandevice_t *wlandev,
 		break;
 	}
 	case DIDmib_dot11smt_dot11PrivacyTable_dot11ExcludeUnencrypted: {
-		UINT32 *data = (UINT32 *) mibitem->data;
+		u32 *data = (u32 *) mibitem->data;
 
 		if (isget) {
 			if (wlandev->hostwep & HOSTWEP_EXCLUDEUNENCRYPTED)
diff --git a/drivers/staging/wlan-ng/p80211req.h b/drivers/staging/wlan-ng/p80211req.h
index 54abdceedc5d..497a4d6eb59a 100644
--- a/drivers/staging/wlan-ng/p80211req.h
+++ b/drivers/staging/wlan-ng/p80211req.h
@@ -63,6 +63,6 @@
 /*================================================================*/
 /* Function Declarations */
 
-int	p80211req_dorequest(wlandevice_t *wlandev, UINT8 *msgbuf);
+int	p80211req_dorequest(wlandevice_t *wlandev, u8 *msgbuf);
 
 #endif
diff --git a/drivers/staging/wlan-ng/p80211types.h b/drivers/staging/wlan-ng/p80211types.h
index 811b0ce39bf6..5be6737e3e5d 100644
--- a/drivers/staging/wlan-ng/p80211types.h
+++ b/drivers/staging/wlan-ng/p80211types.h
@@ -80,13 +80,13 @@
 
 #define P80211_TYPE_OCTETSTR		1	/* pascal array of bytes */
 #define P80211_TYPE_DISPLAYSTR		2	/* pascal array of bytes containing ascii */
-#define P80211_TYPE_INT			4	/* UINT32 min and max limited by 32 bits */
-#define P80211_TYPE_ENUMINT		5	/* UINT32 holding a numeric
+#define P80211_TYPE_int			4	/* u32 min and max limited by 32 bits */
+#define P80211_TYPE_ENUMint		5	/* u32 holding a numeric
 						   code that can be mapped
 						   to a textual name */
 #define P80211_TYPE_UNKDATA		6	/* Data item containing an
 						   unknown data type */
-#define P80211_TYPE_INTARRAY		7	/* Array of 32-bit integers. */
+#define P80211_TYPE_intARRAY		7	/* Array of 32-bit integers. */
 #define P80211_TYPE_BITARRAY		8	/* Array of bits. */
 #define P80211_TYPE_MACARRAY		9	/* Array of MAC addresses. */
 
@@ -243,7 +243,7 @@
 					/* is a DID-LEN-DATA triple */
 					/* with a max size of 4+4+384 */
 
-#define P80211_SET_INT(item, value) do { \
+#define P80211_SET_int(item, value) do { \
 	(item).data   = (value); \
 	(item).status = P80211ENUM_msgitem_status_data_ok; \
 	} while(0)
@@ -279,9 +279,9 @@
 
 #define P80211ITEM_SETFLAGS(q, r, c)	( q | r | c )
 
-#define P80211ITEM_ISREQUIRED(flags)	(((UINT32)(flags & ISREQUIRED)) >> 31 )
-#define P80211ITEM_ISREQUEST(flags)	(((UINT32)(flags & ISREQUEST)) >> 30 )
-#define P80211ITEM_ISCONFIRM(flags)	(((UINT32)(flags & ISCONFIRM)) >> 29 )
+#define P80211ITEM_ISREQUIRED(flags)	(((u32)(flags & ISREQUIRED)) >> 31 )
+#define P80211ITEM_ISREQUEST(flags)	(((u32)(flags & ISREQUEST)) >> 30 )
+#define P80211ITEM_ISCONFIRM(flags)	(((u32)(flags & ISCONFIRM)) >> 29 )
 
 /*----------------------------------------------------------------*/
 /* The following macro creates a name for an enum */
@@ -320,7 +320,7 @@
 #define P80211DID_MASK_ACCESS 		(0x00000003UL)
 
 
-#define P80211DID_MK(a,m,l)	((((UINT32)(a)) & (m)) << (l))
+#define P80211DID_MK(a,m,l)	((((u32)(a)) & (m)) << (l))
 
 #define P80211DID_MKSECTION(a)	P80211DID_MK(a, \
 					P80211DID_MASK_SECTION, \
@@ -347,7 +347,7 @@
 						(a) )
 
 
-#define P80211DID_GET(a,m,l)	((((UINT32)(a)) >> (l)) & (m))
+#define P80211DID_GET(a,m,l)	((((u32)(a)) >> (l)) & (m))
 
 #define P80211DID_SECTION(a)	P80211DID_GET(a, \
 					P80211DID_MASK_SECTION, \
@@ -373,17 +373,17 @@
 
 /*----------------------------------------------------------------*/
 /* The following structure types are used for the represenation */
-/*  of ENUMINT type metadata. */
+/*  of ENUMint type metadata. */
 
 typedef struct p80211enumpair
 {
-	UINT32			val;
+	u32			val;
 	char			*name;
 } p80211enumpair_t;
 
 typedef struct p80211enum
 {
-	INT			nitems;
+	int			nitems;
 	p80211enumpair_t	*list;
 } p80211enum_t;
 
@@ -394,137 +394,137 @@ typedef struct p80211enum
 /* Template pascal string */
 typedef struct p80211pstr
 {
-	UINT8		len;
+	u8		len;
 } __WLAN_ATTRIB_PACK__ p80211pstr_t;
 
 typedef struct p80211pstrd
 {
-	UINT8		len;
-	UINT8		data[0];
+	u8		len;
+	u8		data[0];
 } __WLAN_ATTRIB_PACK__ p80211pstrd_t;
 
 /* Maximum pascal string */
 typedef struct p80211pstr255
 {
-	UINT8		len;
-	UINT8		data[MAXLEN_PSTR255];
+	u8		len;
+	u8		data[MAXLEN_PSTR255];
 } __WLAN_ATTRIB_PACK__ p80211pstr255_t;
 
 /* pascal string for macaddress and bssid */
 typedef struct p80211pstr6
 {
-	UINT8		len;
-	UINT8		data[MAXLEN_PSTR6];
+	u8		len;
+	u8		data[MAXLEN_PSTR6];
 } __WLAN_ATTRIB_PACK__ p80211pstr6_t;
 
 /* pascal string for channel list */
 typedef struct p80211pstr14
 {
-	UINT8		len;
-	UINT8		data[MAXLEN_PSTR14];
+	u8		len;
+	u8		data[MAXLEN_PSTR14];
 } __WLAN_ATTRIB_PACK__ p80211pstr14_t;
 
 /* pascal string for ssid */
 typedef struct p80211pstr32
 {
-	UINT8		len;
-	UINT8		data[MAXLEN_PSTR32];
+	u8		len;
+	u8		data[MAXLEN_PSTR32];
 } __WLAN_ATTRIB_PACK__ p80211pstr32_t;
 
 /* MAC address array */
 typedef struct p80211macarray
 {
-	UINT32		cnt;
-	UINT8		data[1][MAXLEN_PSTR6];
+	u32		cnt;
+	u8		data[1][MAXLEN_PSTR6];
 } __WLAN_ATTRIB_PACK__ p80211macarray_t;
 
 /* prototype template */
 typedef struct p80211item
 {
-	UINT32		did;
-	UINT16		status;
-	UINT16		len;
+	u32		did;
+	u16		status;
+	u16		len;
 } __WLAN_ATTRIB_PACK__ p80211item_t;
 
 /* prototype template w/ data item */
 typedef struct p80211itemd
 {
-	UINT32		did;
-	UINT16		status;
-	UINT16		len;
-	UINT8		data[0];
+	u32		did;
+	u16		status;
+	u16		len;
+	u8		data[0];
 } __WLAN_ATTRIB_PACK__ p80211itemd_t;
 
-/* message data item for INT, BOUNDEDINT, ENUMINT */
+/* message data item for int, BOUNDEDINT, ENUMINT */
 typedef struct p80211item_uint32
 {
-	UINT32		did;
-	UINT16		status;
-	UINT16		len;
-	UINT32		data;
+	u32		did;
+	u16		status;
+	u16		len;
+	u32		data;
 } __WLAN_ATTRIB_PACK__ p80211item_uint32_t;
 
 /* message data item for OCTETSTR, DISPLAYSTR */
 typedef struct p80211item_pstr6
 {
-	UINT32		did;
-	UINT16		status;
-	UINT16		len;
+	u32		did;
+	u16		status;
+	u16		len;
 	p80211pstr6_t	data;
 } __WLAN_ATTRIB_PACK__ p80211item_pstr6_t;
 
 /* message data item for OCTETSTR, DISPLAYSTR */
 typedef struct p80211item_pstr14
 {
-	UINT32			did;
-	UINT16			status;
-	UINT16			len;
+	u32			did;
+	u16			status;
+	u16			len;
 	p80211pstr14_t		data;
 } __WLAN_ATTRIB_PACK__ p80211item_pstr14_t;
 
 /* message data item for OCTETSTR, DISPLAYSTR */
 typedef struct p80211item_pstr32
 {
-	UINT32			did;
-	UINT16			status;
-	UINT16			len;
+	u32			did;
+	u16			status;
+	u16			len;
 	p80211pstr32_t		data;
 } __WLAN_ATTRIB_PACK__ p80211item_pstr32_t;
 
 /* message data item for OCTETSTR, DISPLAYSTR */
 typedef struct p80211item_pstr255
 {
-	UINT32			did;
-	UINT16			status;
-	UINT16			len;
+	u32			did;
+	u16			status;
+	u16			len;
 	p80211pstr255_t		data;
 } __WLAN_ATTRIB_PACK__ p80211item_pstr255_t;
 
 /* message data item for UNK 392, namely mib items */
 typedef struct  p80211item_unk392
 {
-	UINT32		did;
-	UINT16		status;
-	UINT16		len;
-	UINT8		data[MAXLEN_MIBATTRIBUTE];
+	u32		did;
+	u16		status;
+	u16		len;
+	u8		data[MAXLEN_MIBATTRIBUTE];
 } __WLAN_ATTRIB_PACK__ p80211item_unk392_t;
 
 /* message data item for UNK 1025, namely p2 pdas */
 typedef struct  p80211item_unk1024
 {
-	UINT32		did;
-	UINT16		status;
-	UINT16		len;
-	UINT8		data[1024];
+	u32		did;
+	u16		status;
+	u16		len;
+	u8		data[1024];
 }  __WLAN_ATTRIB_PACK__ p80211item_unk1024_t;
 
 /* message data item for UNK 4096, namely p2 download chunks */
 typedef struct  p80211item_unk4096
 {
-	UINT32		did;
-	UINT16		status;
-	UINT16		len;
-	UINT8		data[4096];
+	u32		did;
+	u16		status;
+	u16		len;
+	u8		data[4096];
 }  __WLAN_ATTRIB_PACK__ p80211item_unk4096_t;
 
 struct catlistitem;
@@ -534,9 +534,9 @@ struct catlistitem;
 /*  metadata items.  Some components may choose to use more, */
 /*  less or different metadata items. */
 
-typedef void (*p80211_totext_t)( struct catlistitem *, UINT32 did, UINT8* itembuf, char *textbuf);
-typedef void (*p80211_fromtext_t)( struct catlistitem *, UINT32 did, UINT8* itembuf, char *textbuf);
-typedef UINT32 (*p80211_valid_t)( struct catlistitem *, UINT32 did, UINT8* itembuf);
+typedef void (*p80211_totext_t)( struct catlistitem *, u32 did, u8* itembuf, char *textbuf);
+typedef void (*p80211_fromtext_t)( struct catlistitem *, u32 did, u8* itembuf, char *textbuf);
+typedef u32 (*p80211_valid_t)( struct catlistitem *, u32 did, u8* itembuf);
 
 
 /*================================================================*/
@@ -575,8 +575,8 @@ extern p80211enum_t MKENUMNAME(p2preamble);
 /* The following declare some utility functions for use with the */
 /*  p80211enum_t type. */
 
-UINT32 p80211enum_text2int(p80211enum_t *ep, char *text);
-UINT32 p80211enum_int2text(p80211enum_t *ep, UINT32 val, char *text);
+u32 p80211enum_text2int(p80211enum_t *ep, char *text);
+u32 p80211enum_int2text(p80211enum_t *ep, u32 val, char *text);
 void p80211_error2text(int err_code, char *err_str);
 
 /*----------------------------------------------------------------*/
@@ -591,85 +591,85 @@ void p80211_error2text(int err_code, char *err_str);
 
 /*-- DISPLAYSTR ------------------------------------------------------*/
 /* pstr ==> cstr */
-void p80211_totext_displaystr( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_totext_displaystr( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* cstr ==> pstr */
-void p80211_fromtext_displaystr( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_fromtext_displaystr( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of a displaystr binary value */
-UINT32 p80211_isvalid_displaystr( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_displaystr( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
 /*-- OCTETSTR --------------------------------------------------------*/
 /* pstr ==> "xx:xx:...." */
-void p80211_totext_octetstr( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_totext_octetstr( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* "xx:xx:...." ==> pstr */
-void p80211_fromtext_octetstr( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_fromtext_octetstr( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of an octetstr binary value */
-UINT32 p80211_isvalid_octetstr( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_octetstr( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
-/*-- INT -------------------------------------------------------------*/
-/* UINT32 ==> %d */
-void p80211_totext_int( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/*-- int -------------------------------------------------------------*/
+/* u32 ==> %d */
+void p80211_totext_int( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
-/* %d ==> UINT32 */
-void p80211_fromtext_int( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/* %d ==> u32 */
+void p80211_fromtext_int( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of an int's binary value (always successful) */
-UINT32 p80211_isvalid_int( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_int( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
-/*-- ENUMINT ---------------------------------------------------------*/
-/* UINT32 ==> <valuename> */
-void p80211_totext_enumint( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/*-- ENUMint ---------------------------------------------------------*/
+/* u32 ==> <valuename> */
+void p80211_totext_enumint( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
-/* <valuename> ==> UINT32 */
-void p80211_fromtext_enumint( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/* <valuename> ==> u32 */
+void p80211_fromtext_enumint( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of an enum's binary value */
-UINT32 p80211_isvalid_enumint( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_enumint( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
-/*-- INTARRAY --------------------------------------------------------*/
-/* UINT32[] => %d,%d,%d,... */
-void p80211_totext_intarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/*-- intARRAY --------------------------------------------------------*/
+/* u32[] => %d,%d,%d,... */
+void p80211_totext_intarray( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
-/* %d,%d,%d,... ==> UINT32[] */
-void p80211_fromtext_intarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/* %d,%d,%d,... ==> u32[] */
+void p80211_fromtext_intarray( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of an integer array's value */
-UINT32 p80211_isvalid_intarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_intarray( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
 /*-- BITARRAY --------------------------------------------------------*/
-/* UINT32 ==> %d,%d,%d,... */
-void p80211_totext_bitarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/* u32 ==> %d,%d,%d,... */
+void p80211_totext_bitarray( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
-/* %d,%d,%d,... ==> UINT32 */
-void p80211_fromtext_bitarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/* %d,%d,%d,... ==> u32 */
+void p80211_fromtext_bitarray( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of a bit array's value */
-UINT32 p80211_isvalid_bitarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_bitarray( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
 /*-- MACARRAY --------------------------------------------------------*/
-void p80211_totext_macarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_totext_macarray( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
-void p80211_fromtext_macarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_fromtext_macarray( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of a MAC address array's value */
-UINT32 p80211_isvalid_macarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_macarray( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
 /*-- MIBATTRIUBTE ------------------------------------------------------*/
 /* <mibvalue> ==> <textual representation identified in MIB metadata> */
-void p80211_totext_getmibattribute( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
-void p80211_totext_setmibattribute( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_totext_getmibattribute( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
+void p80211_totext_setmibattribute( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 
 /* <textual representation identified in MIB metadata> ==> <mibvalue> */
-void p80211_fromtext_getmibattribute( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
-void p80211_fromtext_setmibattribute( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_fromtext_getmibattribute( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
+void p80211_fromtext_setmibattribute( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of a mibitem's binary value */
-UINT32 p80211_isvalid_getmibattribute( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
-UINT32 p80211_isvalid_setmibattribute( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_getmibattribute( struct catlistitem *metalist, u32 did, u8 *itembuf );
+u32 p80211_isvalid_setmibattribute( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
 #endif /* _P80211TYPES_H */
 
diff --git a/drivers/staging/wlan-ng/p80211wep.c b/drivers/staging/wlan-ng/p80211wep.c
index 11a50c7fbfc8..46a2a6b3bdca 100644
--- a/drivers/staging/wlan-ng/p80211wep.c
+++ b/drivers/staging/wlan-ng/p80211wep.c
@@ -56,7 +56,6 @@
 #include <linux/slab.h>
 #include <linux/random.h>
 
-#include "version.h"
 #include "wlan_compat.h"
 
 // #define WEP_DEBUG
@@ -73,7 +72,7 @@
 /*================================================================*/
 /* Local Constants */
 
-#define SSWAP(a,b) {UINT8 tmp = s[a]; s[a] = s[b]; s[b] = tmp;}
+#define SSWAP(a,b) {u8 tmp = s[a]; s[a] = s[b]; s[b] = tmp;}
 #define WEP_KEY(x)       (((x) & 0xC0) >> 6)
 
 /*================================================================*/
@@ -87,7 +86,7 @@
 /*================================================================*/
 /* Local Static Definitions */
 
-static const UINT32 wep_crc32_table[256] = {
+static const u32 wep_crc32_table[256] = {
         0x00000000L, 0x77073096L, 0xee0e612cL, 0x990951baL, 0x076dc419L,
         0x706af48fL, 0xe963a535L, 0x9e6495a3L, 0x0edb8832L, 0x79dcb8a4L,
         0xe0d5e91eL, 0x97d2d988L, 0x09b64c2bL, 0x7eb17cbdL, 0xe7b82d07L,
@@ -150,7 +149,7 @@ static const UINT32 wep_crc32_table[256] = {
 
 /* keylen in bytes! */
 
-int wep_change_key(wlandevice_t *wlandev, int keynum, UINT8* key, int keylen)
+int wep_change_key(wlandevice_t *wlandev, int keynum, u8* key, int keylen)
 {
 	if (keylen < 0)  return -1;
 	if (keylen >= MAX_KEYLEN) return -1;
@@ -173,11 +172,11 @@ int wep_change_key(wlandevice_t *wlandev, int keynum, UINT8* key, int keylen)
   4-byte IV at start of buffer, 4-byte ICV at end of buffer.
   if successful, buf start is payload begin, length -= 8;
  */
-int wep_decrypt(wlandevice_t *wlandev, UINT8 *buf, UINT32 len, int key_override, UINT8 *iv, UINT8 *icv)
+int wep_decrypt(wlandevice_t *wlandev, u8 *buf, u32 len, int key_override, u8 *iv, u8 *icv)
 {
-	UINT32 i, j, k, crc, keylen;
-	UINT8 s[256], key[64], c_crc[4];
-	UINT8 keyidx;
+	u32 i, j, k, crc, keylen;
+	u8 s[256], key[64], c_crc[4];
+	u8 keyidx;
 
 	/* Needs to be at least 8 bytes of payload */
 	if (len <= 0) return -1;
@@ -245,10 +244,10 @@ int wep_decrypt(wlandevice_t *wlandev, UINT8 *buf, UINT32 len, int key_override,
 }
 
 /* encrypts in-place. */
-int wep_encrypt(wlandevice_t *wlandev, UINT8 *buf, UINT8 *dst, UINT32 len, int keynum, UINT8 *iv, UINT8 *icv)
+int wep_encrypt(wlandevice_t *wlandev, u8 *buf, u8 *dst, u32 len, int keynum, u8 *iv, u8 *icv)
 {
-	UINT32 i, j, k, crc, keylen;
-	UINT8 s[256], key[64];
+	u32 i, j, k, crc, keylen;
+	u8 s[256], key[64];
 
 	/* no point in WEPping an empty frame */
 	if (len <= 0) return -1;
diff --git a/drivers/staging/wlan-ng/p80211wext.c b/drivers/staging/wlan-ng/p80211wext.c
index b2c9ea25fa42..0d570f1f378c 100644
--- a/drivers/staging/wlan-ng/p80211wext.c
+++ b/drivers/staging/wlan-ng/p80211wext.c
@@ -38,7 +38,6 @@
 /* System Includes */
 
 
-#include <linux/version.h>
 
 #include <linux/kernel.h>
 #include <linux/sched.h>
@@ -47,9 +46,7 @@
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/wireless.h>
-#if WIRELESS_EXT > 12
 #include <net/iw_handler.h>
-#endif
 #include <linux/if_arp.h>
 #include <asm/bitops.h>
 #include <asm/uaccess.h>
@@ -58,7 +55,6 @@
 /*================================================================*/
 /* Project Includes */
 
-#include "version.h"
 #include "wlan_compat.h"
 
 #include "p80211types.h"
@@ -78,10 +74,8 @@ static int p80211wext_giwrate(netdevice_t *dev,
 static int p80211wext_giwessid(netdevice_t *dev,
 			       struct iw_request_info *info,
 			       struct iw_point *data, char *essid);
-/* compatibility to wireless extensions */
-#ifdef WIRELESS_EXT
 
-static UINT8 p80211_mhz_to_channel(UINT16 mhz)
+static u8 p80211_mhz_to_channel(u16 mhz)
 {
 	if (mhz >= 5000) {
 		return ((mhz - 5000) / 5);
@@ -97,7 +91,7 @@ static UINT8 p80211_mhz_to_channel(UINT16 mhz)
 	return 0;
 }
 
-static UINT16 p80211_channel_to_mhz(UINT8 ch, int dot11a)
+static u16 p80211_channel_to_mhz(u8 ch, int dot11a)
 {
 
 	if (ch == 0)
@@ -128,7 +122,7 @@ static const long p80211wext_channel_freq[] = {
 	2412, 2417, 2422, 2427, 2432, 2437, 2442,
 	2447, 2452, 2457, 2462, 2467, 2472, 2484
 };
-#define NUM_CHANNELS (sizeof(p80211wext_channel_freq) / sizeof(p80211wext_channel_freq[0]))
+#define NUM_CHANNELS ARRAY_SIZE(p80211wext_channel_freq)
 
 /* steal a spare bit to store the shared/opensystems state. should default to open if not set */
 #define HOSTWEP_SHAREDKEY BIT3
@@ -147,7 +141,7 @@ static int qual_as_percent(int snr ) {
 
 
 
-static int p80211wext_dorequest(wlandevice_t *wlandev, UINT32 did, UINT32 data)
+static int p80211wext_dorequest(wlandevice_t *wlandev, u32 did, u32 data)
 {
 	p80211msg_dot11req_mibset_t	msg;
 	p80211item_uint32_t		mibitem;
@@ -159,7 +153,7 @@ static int p80211wext_dorequest(wlandevice_t *wlandev, UINT32 did, UINT32 data)
 	mibitem.did = did;
 	mibitem.data = data;
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	DBFEXIT;
 	return result;
@@ -200,7 +194,7 @@ static int p80211wext_autojoin(wlandevice_t *wlandev)
 	memcpy(msg.ssid.data.data, ssid, data.length);
 	msg.ssid.data.len = data.length;
 
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -245,20 +239,14 @@ struct iw_statistics* p80211wext_get_wireless_stats (netdevice_t *dev)
 	wstats->qual.level = quality.level.data;  /* instant signal level */
 	wstats->qual.noise = quality.noise.data;  /* instant noise level */
 
-#if WIRELESS_EXT > 18
 	wstats->qual.updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
-#else
-	wstats->qual.updated = 7;
-#endif
 	wstats->discard.code = wlandev->rx.decrypt_err;
 	wstats->discard.nwid = 0;
 	wstats->discard.misc = 0;
 
-#if WIRELESS_EXT > 11
 	wstats->discard.fragment = 0;  // incomplete fragments
 	wstats->discard.retries = 0;   // tx retries.
 	wstats->miss.beacon = 0;
-#endif
 
 	DBFEXIT;
 
@@ -312,7 +300,7 @@ static int p80211wext_giwfreq(netdevice_t *dev,
 	msg.msgcode = DIDmsg_dot11req_mibget;
 	mibitem.did = DIDmib_dot11phy_dot11PhyDSSSTable_dot11CurrentChannel;
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -362,7 +350,7 @@ static int p80211wext_siwfreq(netdevice_t *dev,
 		mibitem.data = p80211_mhz_to_channel(freq->m);
 
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -374,8 +362,6 @@ static int p80211wext_siwfreq(netdevice_t *dev,
 	return err;
 }
 
-#if WIRELESS_EXT > 8
-
 static int p80211wext_giwmode(netdevice_t *dev,
 			      struct iw_request_info *info,
 			      __u32 *mode, char *extra)
@@ -447,14 +433,11 @@ static int p80211wext_siwmode(netdevice_t *dev,
 	}
 
 	/* Set Operation mode to the PORT TYPE RID */
-
-#warning "get rid of p2mib here"
-
 	msg.msgcode = DIDmsg_dot11req_mibset;
 	mibitem.did = DIDmib_p2_p2Static_p2CnfPortType;
 	mibitem.data = (*mode == IW_MODE_ADHOC) ? 0 : 1;
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result)
 		err = -EFAULT;
@@ -479,12 +462,9 @@ static int p80211wext_giwrange(netdevice_t *dev,
 	data->length = sizeof(*range);
 	memset(range,0,sizeof(*range));
 
-#if WIRELESS_EXT > 9
 	range->txpower_capa = IW_TXPOW_DBM;
 	// XXX what about min/max_pmp, min/max_pmt, etc.
-#endif
 
-#if WIRELESS_EXT > 10
 	range->we_version_compiled = WIRELESS_EXT;
 	range->we_version_source = 13;
 
@@ -492,16 +472,13 @@ static int p80211wext_giwrange(netdevice_t *dev,
 	range->retry_flags = IW_RETRY_LIMIT;
 	range->min_retry = 0;
 	range->max_retry = 255;
-#endif /* WIRELESS_EXT > 10 */
 
-#if WIRELESS_EXT > 16
         range->event_capa[0] = (IW_EVENT_CAPA_K_0 |  //mode/freq/ssid
                                 IW_EVENT_CAPA_MASK(SIOCGIWAP) |
                                 IW_EVENT_CAPA_MASK(SIOCGIWSCAN));
         range->event_capa[1] = IW_EVENT_CAPA_K_1;  //encode
         range->event_capa[4] = (IW_EVENT_CAPA_MASK(IWEVQUAL) |
                                 IW_EVENT_CAPA_MASK(IWEVCUSTOM) );
-#endif
 
 	range->num_channels = NUM_CHANNELS;
 
@@ -543,7 +520,6 @@ static int p80211wext_giwrange(netdevice_t *dev,
 	DBFEXIT;
 	return 0;
 }
-#endif
 
 static int p80211wext_giwap(netdevice_t *dev,
 			    struct iw_request_info *info,
@@ -561,7 +537,6 @@ static int p80211wext_giwap(netdevice_t *dev,
 	return 0;
 }
 
-#if WIRELESS_EXT > 8
 static int p80211wext_giwencode(netdevice_t *dev,
 				struct iw_request_info *info,
 				struct iw_point *erq, char *key)
@@ -572,10 +547,13 @@ static int p80211wext_giwencode(netdevice_t *dev,
 
 	DBFENTER;
 
+	i = (erq->flags & IW_ENCODE_INDEX) - 1;
+	erq->flags = 0;
+
 	if (wlandev->hostwep & HOSTWEP_PRIVACYINVOKED)
-		erq->flags = IW_ENCODE_ENABLED;
+		erq->flags |= IW_ENCODE_ENABLED;
 	else
-		erq->flags = IW_ENCODE_DISABLED;
+		erq->flags |= IW_ENCODE_DISABLED;
 
 	if (wlandev->hostwep & HOSTWEP_EXCLUDEUNENCRYPTED)
 		erq->flags |= IW_ENCODE_RESTRICTED;
@@ -613,7 +591,6 @@ static int p80211wext_siwencode(netdevice_t *dev,
 
 	int err = 0;
 	int result = 0;
-	int enable = 0;
 	int i;
 
 	DBFENTER;
@@ -632,23 +609,23 @@ static int p80211wext_siwencode(netdevice_t *dev,
 		else
 			i--;
 
-		result = p80211wext_dorequest(wlandev, DIDmib_dot11smt_dot11PrivacyTable_dot11WEPDefaultKeyID, i);
+		/* Set current key number only if no keys are given */
+		if (erq->flags & IW_ENCODE_NOKEY) {
+			result = p80211wext_dorequest(wlandev, DIDmib_dot11smt_dot11PrivacyTable_dot11WEPDefaultKeyID, i);
 
-		if (result) {
-			err = -EFAULT;
-			goto exit;
-		}
-		else {
-			enable = 1;
+			if (result) {
+				err = -EFAULT;
+				goto exit;
+			}
 		}
 
-	}
-	else {
-		// Do not thing when no Key Index
+	} else {
+		// Use defaultkey if no Key Index
+		i = wlandev->hostwep & HOSTWEP_DEFAULTKEY_MASK;
 	}
 
 	/* Check if there is no key information in the iwconfig request */
-	if((erq->flags & IW_ENCODE_NOKEY) == 0 && enable == 1) {
+	if((erq->flags & IW_ENCODE_NOKEY) == 0 ) {
 
 		/*------------------------------------------------------------
 		 * If there is WEP Key for setting, check the Key Information
@@ -690,7 +667,7 @@ static int p80211wext_siwencode(netdevice_t *dev,
 
 			msg.msgcode = DIDmsg_dot11req_mibset;
 			memcpy(&msg.mibattribute.data, &pstr, sizeof(pstr));
-			result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+			result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 			if (result) {
 				err = -EFAULT;
@@ -703,8 +680,7 @@ static int p80211wext_siwencode(netdevice_t *dev,
 	/* Check the PrivacyInvoked flag */
 	if (erq->flags & IW_ENCODE_DISABLED) {
 		result = p80211wext_dorequest(wlandev, DIDmib_dot11smt_dot11PrivacyTable_dot11PrivacyInvoked, P80211ENUM_truth_false);
-	}
-	else if((erq->flags & IW_ENCODE_ENABLED) || enable == 1) {
+	} else {
 		result = p80211wext_dorequest(wlandev, DIDmib_dot11smt_dot11PrivacyTable_dot11PrivacyInvoked, P80211ENUM_truth_true);
 	}
 
@@ -713,7 +689,13 @@ static int p80211wext_siwencode(netdevice_t *dev,
 		goto exit;
 	}
 
-	/* Check the ExcludeUnencrypted flag */
+	/*  The  security  mode  may  be open or restricted, and its meaning
+	    depends on the card used. With  most  cards,  in  open  mode  no
+	    authentication  is  used  and  the  card  may  also  accept non-
+	    encrypted sessions, whereas in restricted  mode  only  encrypted
+	    sessions  are  accepted  and the card will use authentication if
+	    available.
+	*/
 	if (erq->flags & IW_ENCODE_RESTRICTED) {
 		result = p80211wext_dorequest(wlandev, DIDmib_dot11smt_dot11PrivacyTable_dot11ExcludeUnencrypted, P80211ENUM_truth_true);
 	}
@@ -798,7 +780,7 @@ static int p80211wext_siwessid(netdevice_t *dev,
 	msg.ssid.data.len = length;
 
 	WLAN_LOG_DEBUG(1,"autojoin_ssid for %s \n",essid);
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
         WLAN_LOG_DEBUG(1,"autojoin_ssid %d\n",result);
 
 	if (result) {
@@ -850,7 +832,7 @@ static int p80211wext_giwrate(netdevice_t *dev,
 	msg.msgcode = DIDmsg_dot11req_mibget;
 	mibitem.did = DIDmib_p2_p2MAC_p2CurrentTxRate;
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -863,10 +845,10 @@ static int p80211wext_giwrate(netdevice_t *dev,
 	rrq->disabled = 0;
 	rrq->value = 0;
 
-#define		HFA384x_RATEBIT_1			((UINT16)1)
-#define		HFA384x_RATEBIT_2			((UINT16)2)
-#define		HFA384x_RATEBIT_5dot5			((UINT16)4)
-#define		HFA384x_RATEBIT_11			((UINT16)8)
+#define		HFA384x_RATEBIT_1			((u16)1)
+#define		HFA384x_RATEBIT_2			((u16)2)
+#define		HFA384x_RATEBIT_5dot5			((u16)4)
+#define		HFA384x_RATEBIT_11			((u16)8)
 
 	switch (mibitem.data) {
 	case HFA384x_RATEBIT_1:
@@ -904,7 +886,7 @@ static int p80211wext_giwrts(netdevice_t *dev,
 	msg.msgcode = DIDmsg_dot11req_mibget;
 	mibitem.did = DIDmib_dot11mac_dot11OperationTable_dot11RTSThreshold;
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -948,7 +930,7 @@ static int p80211wext_siwrts(netdevice_t *dev,
 		mibitem.data = rts->value;
 
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -975,7 +957,7 @@ static int p80211wext_giwfrag(netdevice_t *dev,
 	msg.msgcode = DIDmsg_dot11req_mibget;
 	mibitem.did = DIDmib_dot11mac_dot11OperationTable_dot11FragmentationThreshold;
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -1019,7 +1001,7 @@ static int p80211wext_siwfrag(netdevice_t *dev,
 		mibitem.data = frag->value;
 
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -1031,10 +1013,6 @@ static int p80211wext_siwfrag(netdevice_t *dev,
 	return err;
 }
 
-#endif  /* WIRELESS_EXT > 8 */
-
-#if WIRELESS_EXT > 10
-
 #ifndef IW_RETRY_LONG
 #define IW_RETRY_LONG IW_RETRY_MAX
 #endif
@@ -1052,7 +1030,7 @@ static int p80211wext_giwretry(netdevice_t *dev,
 	p80211msg_dot11req_mibset_t     msg;
 	int result;
 	int err = 0;
-	UINT16 shortretry, longretry, lifetime;
+	u16 shortretry, longretry, lifetime;
 
 	DBFENTER;
 
@@ -1060,7 +1038,7 @@ static int p80211wext_giwretry(netdevice_t *dev,
 	mibitem.did = DIDmib_dot11mac_dot11OperationTable_dot11ShortRetryLimit;
 
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -1074,7 +1052,7 @@ static int p80211wext_giwretry(netdevice_t *dev,
 	mibitem.did = DIDmib_dot11mac_dot11OperationTable_dot11LongRetryLimit;
 
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -1088,7 +1066,7 @@ static int p80211wext_giwretry(netdevice_t *dev,
 	mibitem.did = DIDmib_dot11mac_dot11OperationTable_dot11MaxTransmitMSDULifetime;
 
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -1151,7 +1129,7 @@ static int p80211wext_siwretry(netdevice_t *dev,
 		mibitem.data =  rrq->value /= 1024;
 
 		memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-		result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+		result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 		if (result) {
 			err = -EFAULT;
@@ -1163,7 +1141,7 @@ static int p80211wext_siwretry(netdevice_t *dev,
 			mibitem.data = rrq->value;
 
 			memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-			result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+			result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 			if (result) {
 				err = -EFAULT;
@@ -1176,7 +1154,7 @@ static int p80211wext_siwretry(netdevice_t *dev,
 			mibitem.data = rrq->value;
 
 			memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-			result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+			result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 			if (result) {
 				err = -EFAULT;
@@ -1191,9 +1169,6 @@ static int p80211wext_siwretry(netdevice_t *dev,
 
 }
 
-#endif /* WIRELESS_EXT > 10 */
-
-#if WIRELESS_EXT > 9
 static int p80211wext_siwtxpow(netdevice_t *dev,
                                struct iw_request_info *info,
                                struct iw_param *rrq, char *extra)
@@ -1212,22 +1187,13 @@ static int p80211wext_siwtxpow(netdevice_t *dev,
         }
 
         msg.msgcode = DIDmsg_dot11req_mibset;
-
-        switch (rrq->value) {
-
-          case 1 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel1; break;
-          case 2 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel2; break;
-          case 3 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel3; break;
-          case 4 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel4; break;
-          case 5 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel5; break;
-          case 6 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel6; break;
-          case 7 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel7; break;
-          case 8 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel8; break;
-          default: mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel8; break;
-	}
-
+	mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11CurrentTxPowerLevel;
+	if (rrq->fixed == 0)
+	  mibitem.data = 30;
+	else
+	  mibitem.data = rrq->value;
         memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-        result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+        result = p80211req_dorequest(wlandev, (u8*)&msg);
 
         if (result) {
                 err = -EFAULT;
@@ -1255,7 +1221,7 @@ static int p80211wext_giwtxpow(netdevice_t *dev,
 	mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11CurrentTxPowerLevel;
 
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -1275,7 +1241,6 @@ static int p80211wext_giwtxpow(netdevice_t *dev,
 	DBFEXIT;
 	return err;
 }
-#endif /* WIRELESS_EXT > 9 */
 
 static int p80211wext_siwspy(netdevice_t *dev,
 			     struct iw_request_info *info,
@@ -1373,7 +1338,6 @@ static int prism2_result2err (int prism2_result)
 	return err;
 }
 
-#if WIRELESS_EXT > 13
 static int p80211wext_siwscan(netdevice_t *dev,
 			     struct iw_request_info *info,
 			     struct iw_point *srq, char *extra)
@@ -1409,7 +1373,7 @@ static int p80211wext_siwscan(netdevice_t *dev,
 	msg.maxchanneltime.data = 250;
 	msg.minchanneltime.data = 200;
 
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 	if (result)
 		err = prism2_result2err (msg.resultcode.data);
 
@@ -1520,7 +1484,7 @@ static int p80211wext_giwscan(netdevice_t *dev,
 		msg.msgcode = DIDmsg_dot11req_scan_results;
 		msg.bssindex.data = i;
 
-		result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+		result = p80211req_dorequest(wlandev, (u8*)&msg);
 		if ((result != 0) ||
 		    (msg.resultcode.data != P80211ENUM_resultcode_success)) {
 			break;
@@ -1540,12 +1504,10 @@ static int p80211wext_giwscan(netdevice_t *dev,
 	DBFEXIT;
 	return err;
 }
-#endif
 
 /*****************************************************/
 //extra wireless extensions stuff to support NetworkManager (I hope)
 
-#if WIRELESS_EXT > 17
 /* SIOCSIWENCODEEXT */
 static int p80211wext_set_encodeext(struct net_device *dev,
 				struct iw_request_info *info,
@@ -1580,7 +1542,7 @@ static int p80211wext_set_encodeext(struct net_device *dev,
 
 
   if ( ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY ) {
-    if ( ! ext->alg & IW_ENCODE_ALG_WEP) {
+    if (!(ext->alg & IW_ENCODE_ALG_WEP)) {
       WLAN_LOG_DEBUG(1,"asked to set a non wep key :(");
       return -EINVAL;
     }
@@ -1615,7 +1577,7 @@ static int p80211wext_set_encodeext(struct net_device *dev,
       break;
     }
     msg.msgcode = DIDmsg_dot11req_mibset;
-    result = p80211req_dorequest(wlandev,(UINT8*)&msg);
+    result = p80211req_dorequest(wlandev,(u8*)&msg);
     WLAN_LOG_DEBUG(1,"result (%d)\n",result);
   }
   return result;
@@ -1763,26 +1725,6 @@ static int p80211_wext_get_iwauth (struct net_device *dev,
   return result;
 }
 
-
-#endif
-
-
-
-
-
-
-/*****************************************************/
-
-
-
-
-
-/*
-typedef int (*iw_handler)(netdevice_t *dev, struct iw_request_info *info,
-                          union iwreq_data *wrqu, char *extra);
-*/
-
-#if WIRELESS_EXT > 12
 static iw_handler p80211wext_handlers[] =  {
 	(iw_handler) p80211wext_siwcommit,		/* SIOCSIWCOMMIT */
 	(iw_handler) p80211wext_giwname,		/* SIOCGIWNAME */
@@ -1808,13 +1750,8 @@ static iw_handler p80211wext_handlers[] =  {
 	(iw_handler) p80211wext_giwap,         		/* SIOCGIWAP */
 	(iw_handler) NULL,				/* -- hole -- */
 	(iw_handler) NULL,                  		/* SIOCGIWAPLIST */
-#if WIRELESS_EXT > 13
-	(iw_handler) p80211wext_siwscan,			/* SIOCSIWSCAN */
-	(iw_handler) p80211wext_giwscan,			/* SIOCGIWSCAN */
-#else /* WIRELESS_EXT > 13 */
-	(iw_handler) NULL,	/* null */		/* SIOCSIWSCAN */
-	(iw_handler) NULL,	/* null */		/* SIOCGIWSCAN */
-#endif /* WIRELESS_EXT > 13 */
+	(iw_handler) p80211wext_siwscan,		/* SIOCSIWSCAN */
+	(iw_handler) p80211wext_giwscan,		/* SIOCGIWSCAN */
 	(iw_handler) p80211wext_siwessid,  		/* SIOCSIWESSID */
 	(iw_handler) p80211wext_giwessid,      		/* SIOCGIWESSID */
 	(iw_handler) NULL,                 		/* SIOCSIWNICKN */
@@ -1835,9 +1772,7 @@ static iw_handler p80211wext_handlers[] =  {
 	(iw_handler) p80211wext_giwencode,  		/* SIOCGIWENCODE */
 	(iw_handler) NULL,                 		/* SIOCSIWPOWER */
 	(iw_handler) NULL,                  		/* SIOCGIWPOWER */
-#if WIRELESS_EXT > 17
 /* WPA operations */
-
 	(iw_handler) NULL,				/* -- hole -- */
 	(iw_handler) NULL,				/* -- hole -- */
 	(iw_handler) NULL, /* SIOCSIWGENIE	set generic IE */
@@ -1848,170 +1783,18 @@ static iw_handler p80211wext_handlers[] =  {
 	(iw_handler) p80211wext_set_encodeext, /* SIOCSIWENCODEEXT  set encoding token & mode */
 	(iw_handler) p80211wext_get_encodeext, /* SIOCGIWENCODEEXT  get encoding token & mode */
 	(iw_handler) NULL, /* SIOCSIWPMKSA	PMKSA cache operation */
-#endif
 };
 
 struct iw_handler_def p80211wext_handler_def = {
-	.num_standard = sizeof(p80211wext_handlers) / sizeof(iw_handler),
+	.num_standard = ARRAY_SIZE(p80211wext_handlers),
 	.num_private = 0,
 	.num_private_args = 0,
         .standard = p80211wext_handlers,
 	.private = NULL,
 	.private_args = NULL,
-#if WIRELESS_EXT > 16
 	.get_wireless_stats = p80211wext_get_wireless_stats
-#endif
 };
 
-#endif
-
-/* wireless extensions' ioctls */
-int p80211wext_support_ioctl(netdevice_t *dev, struct ifreq *ifr, int cmd)
-{
-	wlandevice_t *wlandev = dev->ml_priv;
-
-#if WIRELESS_EXT < 13
-	struct iwreq *iwr = (struct iwreq*)ifr;
-#endif
-
-	p80211item_uint32_t             mibitem;
-	int err = 0;
-
-	DBFENTER;
-
-	mibitem.status = P80211ENUM_msgitem_status_data_ok;
-
-	if ( wlandev->msdstate != WLAN_MSD_RUNNING ) {
-		err = -ENODEV;
-		goto exit;
-	}
-
-	WLAN_LOG_DEBUG(1, "Received wireless extension ioctl #%d.\n", cmd);
-
-	switch (cmd) {
-#if WIRELESS_EXT < 13
-	case SIOCSIWNAME:  /* unused  */
-		err = (-EOPNOTSUPP);
-		break;
-	case SIOCGIWNAME: /* get name == wireless protocol */
-                err = p80211wext_giwname(dev, NULL, (char *) &iwr->u, NULL);
-		break;
-	case SIOCSIWNWID:
-	case SIOCGIWNWID:
-		err = (-EOPNOTSUPP);
-		break;
-	case SIOCSIWFREQ: /* set channel */
-                err = p80211wext_siwfreq(dev, NULL, &(iwr->u.freq), NULL);
-		break;
-	case SIOCGIWFREQ: /* get channel */
-                err = p80211wext_giwfreq(dev, NULL, &(iwr->u.freq), NULL);
-		break;
-	case SIOCSIWRANGE:
-	case SIOCSIWPRIV:
-	case SIOCSIWAP: /* set access point MAC addresses (BSSID) */
-		err = (-EOPNOTSUPP);
-		break;
-
-	case SIOCGIWAP:	/* get access point MAC addresses (BSSID) */
-                err = p80211wext_giwap(dev, NULL, &(iwr->u.ap_addr), NULL);
-		break;
-
-#if WIRELESS_EXT > 8
-	case SIOCSIWMODE: /* set operation mode */
-	case SIOCSIWESSID: /* set SSID (network name) */
-	case SIOCSIWRATE: /* set default bit rate (bps) */
-		err = (-EOPNOTSUPP);
-		break;
-
-	case SIOCGIWMODE: /* get operation mode */
-		err = p80211wext_giwmode(dev, NULL, &iwr->u.mode, NULL);
-
-		break;
-	case SIOCGIWNICKN: /* get node name/nickname */
-	case SIOCGIWESSID: /* get SSID */
-		if(iwr->u.essid.pointer) {
-                        char ssid[IW_ESSID_MAX_SIZE+1];
-			memset(ssid, 0, sizeof(ssid));
-
-			err = p80211wext_giwessid(dev, NULL, &iwr->u.essid, ssid);
-			if(copy_to_user(iwr->u.essid.pointer, ssid, sizeof(ssid)))
-				err = (-EFAULT);
-		}
-		break;
-	case SIOCGIWRATE:
-                err = p80211wext_giwrate(dev, NULL, &iwr->u.bitrate, NULL);
-		break;
-	case SIOCGIWRTS:
-		err = p80211wext_giwrts(dev, NULL, &iwr->u.rts, NULL);
-		break;
-	case SIOCGIWFRAG:
-		err = p80211wext_giwfrag(dev, NULL, &iwr->u.rts, NULL);
-		break;
-	case SIOCGIWENCODE:
-		if (!capable(CAP_NET_ADMIN))
-			err = -EPERM;
-		else if (iwr->u.encoding.pointer) {
-			char keybuf[MAX_KEYLEN];
-			err = p80211wext_giwencode(dev, NULL,
-						     &iwr->u.encoding, keybuf);
-			if (copy_to_user(iwr->u.encoding.pointer, keybuf,
-					 iwr->u.encoding.length))
-				err = -EFAULT;
-		}
-		break;
-	case SIOCGIWAPLIST:
-	case SIOCSIWRTS:
-	case SIOCSIWFRAG:
-	case SIOCSIWSENS:
-	case SIOCGIWSENS:
-	case SIOCSIWNICKN: /* set node name/nickname */
-	case SIOCSIWENCODE: /* set encoding token & mode */
-	case SIOCSIWSPY:
-	case SIOCGIWSPY:
-	case SIOCSIWPOWER:
-	case SIOCGIWPOWER:
-	case SIOCGIWPRIV:
-		err = (-EOPNOTSUPP);
-		break;
-	case SIOCGIWRANGE:
-		if(iwr->u.data.pointer != NULL) {
-                        struct iw_range range;
-                        err = p80211wext_giwrange(dev, NULL, &iwr->u.data,
-						  (char *) &range);
-			/* Push that up to the caller */
-			if (copy_to_user(iwr->u.data.pointer, &range, sizeof(range)))
-				err = -EFAULT;
-		}
-		break;
-#endif /* WIRELESS_EXT > 8 */
-#if WIRELESS_EXT > 9
-	case SIOCSIWTXPOW:
-		err = (-EOPNOTSUPP);
-		break;
-	case SIOCGIWTXPOW:
-		err = p80211wext_giwtxpow(dev, NULL, &iwr->u.txpower, NULL);
-		break;
-#endif /* WIRELESS_EXT > 9 */
-#if WIRELESS_EXT > 10
-	case SIOCSIWRETRY:
-		err = (-EOPNOTSUPP);
-		break;
-	case SIOCGIWRETRY:
-		err = p80211wext_giwretry(dev, NULL, &iwr->u.retry, NULL);
-		break;
-#endif /* WIRELESS_EXT > 10 */
-
-#endif /* WIRELESS_EXT <= 12 */
-
-	default:
-		err = (-EOPNOTSUPP);
-		break;
-	}
-
- exit:
-	DBFEXIT;
-	return (err);
-}
 
 int p80211wext_event_associated(wlandevice_t *wlandev, int assoc)
 {
@@ -2019,7 +1802,6 @@ int p80211wext_event_associated(wlandevice_t *wlandev, int assoc)
 
         DBFENTER;
 
-#if WIRELESS_EXT > 13
         /* Send the association state first */
         data.ap_addr.sa_family = ARPHRD_ETHER;
         if (assoc) {
@@ -2034,15 +1816,12 @@ int p80211wext_event_associated(wlandevice_t *wlandev, int assoc)
         if (!assoc) goto done;
 
         // XXX send association data, like IEs, etc etc.
-#endif
+
  done:
         DBFEXIT;
         return 0;
 }
 
 
-#endif /* compatibility to wireless extensions */
-
-
 
 
diff --git a/drivers/staging/wlan-ng/prism2_cs.c b/drivers/staging/wlan-ng/prism2_cs.c
deleted file mode 100644
index 63ce5659f21a..000000000000
--- a/drivers/staging/wlan-ng/prism2_cs.c
+++ /dev/null
@@ -1,1487 +0,0 @@
-#define WLAN_HOSTIF WLAN_PCMCIA
-#include "hfa384x.c"
-#include "prism2mgmt.c"
-#include "prism2mib.c"
-#include "prism2sta.c"
-
-#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2,4,21) )
-#if (WLAN_CPU_FAMILY == WLAN_Ix86)
-#ifndef CONFIG_ISA
-#warning "You may need to enable ISA support in your kernel."
-#endif
-#endif
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11) )
-static u_int	irq_mask = 0xdeb8;		/* Interrupt mask */
-static int	irq_list[4] = { -1 };		/* Interrupt list */
-#endif
-static u_int	prism2_ignorevcc=1;		/* Boolean, if set, we
-						 * ignore what the Vcc
-						 * is set to and what the CIS
-						 * says.
-						 */
-module_param( prism2_ignorevcc, int, 0644);
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11) )
-#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,9))
-static int numlist = 4;
-module_param_array(irq_list, int, numlist, 0444);
-#else
-module_param_array(irq_list, int, NULL, 0444);
-#endif
-module_param( irq_mask, int, 0644);
-#endif
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,15)
-static int prism2_cs_suspend(struct pcmcia_device *pdev);
-static int prism2_cs_resume(struct pcmcia_device *pdev);
-static void prism2_cs_remove(struct pcmcia_device *pdev);
-static int prism2_cs_probe(struct pcmcia_device *pdev);
-#else
-dev_link_t	*prism2sta_attach(void);
-static void	prism2sta_detach(dev_link_t *link);
-static void	prism2sta_config(dev_link_t *link);
-static void	prism2sta_release(u_long arg);
-static int 	prism2sta_event (event_t event, int priority, event_callback_args_t *args);
-
-static dev_link_t	*dev_list = NULL;	/* head of instance list */
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,68))
-/*----------------------------------------------------------------
-* cs_error
-*
-* Utility function to print card services error messages.
-*
-* Arguments:
-*	handle	client handle identifying this CS client
-*	func	CS function number that generated the error
-*	ret	CS function return code
-*
-* Returns:
-*	nothing
-* Side effects:
-*
-* Call context:
-*	process thread
-*	interrupt
-----------------------------------------------------------------*/
-static void cs_error(client_handle_t handle, int func, int ret)
-{
-#if (defined(CS_RELEASE_CODE) && (CS_RELEASE_CODE < 0x2911))
-	CardServices(ReportError, dev_info, (void *)func, (void *)ret);
-#else
-	error_info_t err = { func, ret };
-	pcmcia_report_error(handle, &err);
-#endif
-}
-#else // kernel_version
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,12)
-static struct pcmcia_device_id prism2_cs_ids[] = {
-	PCMCIA_DEVICE_PROD_ID12("INTERSIL",  "HFA384x/IEEE", 0x74c5e40d, 0xdb472a18), // Intersil PRISM2 Reference Design 11Mb/s 802.11b WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0x0138, 0x0002), // Compaq WL100/200 11Mb/s 802.11b WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0x028a, 0x0002), // Compaq iPaq HNW-100 11Mb/s 802.11b WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0x0250, 0x0002), // Samsung SWL2000-N 11Mb/s 802.11b WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0xd601, 0x0002), // Z-Com XI300 11Mb/s 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID12("ZoomAir 11Mbps High",  "Rate wireless Networking", 0x273fe3db, 0x32a1eaee), // ZoomAir 4100 11Mb/s 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID123("Instant Wireless ",  " Network PC CARD",  "Version 01.02", 0x11d901af, 0x6e9bd926, 0x4b74baa0), // Linksys WPC11 11Mbps 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID123("Addtron",  "AWP-100 Wireless PCMCIA",  "Version 01.02", 0xe6ec52ce, 0x8649af2, 0x4b74baa0), // Addtron AWP-100 11Mbps 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID123("D",  "Link DWL-650 11Mbps WLAN Card",  "Version 01.02", 0x71b18589, 0xb6f1b0ab, 0x4b74baa0), // D-Link DWL-650 11Mbps 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID123("SMC",  "SMC2632W",  "Version 01.02", 0xc4f8b18b, 0x474a1f2a, 0x4b74baa0), // SMC 2632W 11Mbps 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID1234("Intersil",  "PRISM 2_5 PCMCIA ADAPTER",  "ISL37300P",  "Eval-RevA", 0x4b801a17, 0x6345a0bf, 0xc9049a39, 0xc23adc0e), // BroMax Freeport 11Mbps 802.11b WLAN Card (Prism 2.5)
-	PCMCIA_DEVICE_PROD_ID123("U.S. Robotics",  "IEEE 802.11b PC-CARD",  "Version 01.02", 0xc7b8df9d, 0x1700d087, 0x4b74baa0), // U.S. Robotics IEEE 802.11b PC-CARD
-	PCMCIA_DEVICE_PROD_ID12("Digital Data Communications",  "WPC-0100", 0xfdd73470, 0xe0b6f146), // Level-One WPC-0100
-	PCMCIA_DEVICE_MANF_CARD(0x0274, 0x1612), // Bromax OEM 11Mbps 802.11b WLAN Card (Prism 2.5)
-	PCMCIA_DEVICE_MANF_CARD(0x0274, 0x1613), // Bromax OEM 11Mbps 802.11b WLAN Card (Prism 3)
-	PCMCIA_DEVICE_PROD_ID12("corega K.K.",  "Wireless LAN PCC-11", 0x5261440f, 0xa6405584), // corega K.K. Wireless LAN PCC-11
-	PCMCIA_DEVICE_PROD_ID12("corega K.K.",  "Wireless LAN PCCA-11", 0x5261440f, 0xdf6115f9), // corega K.K. Wireless LAN PCCA-11
-	PCMCIA_DEVICE_MANF_CARD(0xc001, 0x0008), // CONTEC FLEXSCAN/FX-DDS110-PCC
-	PCMCIA_DEVICE_PROD_ID12("PLANEX",  "GeoWave/GW-NS110", 0x209f40ab, 0x46263178), // PLANEX GeoWave/GW-NS110
-	PCMCIA_DEVICE_PROD_ID123("OEM",  "PRISM2 IEEE 802.11 PC-Card",  "Version 01.02", 0xfea54c90, 0x48f2bdd6, 0x4b74baa0), // Ambicom WL1100 11Mbps 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID123("LeArtery",  "SYNCBYAIR 11Mbps Wireless LAN PC Card",  "Version 01.02", 0x7e3b326a, 0x49893e92, 0x4b74baa0), // LeArtery SYNCBYAIR 11Mbps 802.11b WLAN Card
-PCMCIA_DEVICE_MANF_CARD(0x01ff, 0x0008), // Intermec MobileLAN 11Mbps 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID123("NETGEAR MA401 Wireless PC",  "Card",  "Version 01.00", 0xa37434e9, 0x9762e8f1, 0xa57adb8c), // NETGEAR MA401 11Mbps 802.11 WLAN Card
-	PCMCIA_DEVICE_PROD_ID1234("Intersil",  "PRISM Freedom PCMCIA Adapter",  "ISL37100P",  "Eval-RevA", 0x4b801a17, 0xf222ec2d, 0x630d52b2, 0xc23adc0e), // Intersil PRISM Freedom 11mbps 802.11 WLAN Card
-	PCMCIA_DEVICE_PROD_ID123("OTC",  "Wireless AirEZY 2411-PCC WLAN Card",  "Version 01.02", 0x4ac44287, 0x235a6bed, 0x4b74baa0), // OTC Wireless AirEZY 2411-PCC 11Mbps 802.11 WLAN Card
-	PCMCIA_DEVICE_PROD_ID1234("802.11",  "11Mbps Wireless LAN Card",  "v08C1",  ""   , 0xb67a610e, 0x655aa7b7, 0x264b451a, 0x0), // Dynalink L11HDT 11Mbps 802.11 WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0xc250, 0x0002), // Dynalink L11HDT 11Mbps 802.11 WLAN Card
-	PCMCIA_DEVICE_PROD_ID12("PROXIM",  "RangeLAN-DS/LAN PC CARD", 0xc6536a5e, 0x3f35797d), // PROXIM RangeLAN-DS/LAN PC CARD
-	PCMCIA_DEVICE_PROD_ID1234("ACTIONTEC",  "PRISM Wireless LAN PC Card",  "0381",  "RevA", 0x393089da, 0xa71e69d5, 0x90471fa9, 0x57a66194), // ACTIONTEC PRISM Wireless LAN PC Card
-	PCMCIA_DEVICE_MANF_CARD(0x1668, 0x0101), // ACTIONTEC PRISM Wireless LAN PC Card
-	PCMCIA_DEVICE_PROD_ID12("3Com",  "3CRWE737A AirConnect Wireless LAN PC Card", 0x41240e5b, 0x56010af3), // 3Com AirConnect 3CRWE737A
-	PCMCIA_DEVICE_PROD_ID12("3Com",  "3CRWE777A AirConnect Wireless LAN PCI Card"  , 0x41240e5b, 0xafc7c33e), // 3Com AirConnect 3CRWE777A
-	PCMCIA_DEVICE_PROD_ID12("ASUS",  "802_11b_PC_CARD_25", 0x78fc06ee, 0xdb9aa842), // ASUS WL-100 802.11b WLAN  PC Card
-	PCMCIA_DEVICE_PROD_ID12("ASUS",  "802_11B_CF_CARD_25", 0x78fc06ee, 0x45a50c1e), // ASUS WL-110 802.11b WLAN CF Card
-	PCMCIA_DEVICE_PROD_ID12("BUFFALO",  "WLI-CF-S11G", 0x2decece3, 0x82067c18), // BUFFALO WLI-CF-S11G 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID1234("The Linksys Group, Inc.", "Wireless Network CF Card", "ISL37300P", "RevA", 0xa5f472c2, 0x9c05598d, 0xc9049a39, 0x57a66194), // Linksys WCF11 11Mbps 802.11b WLAN Card (Prism 2.5)
-	PCMCIA_DEVICE_PROD_ID1234("Linksys",  "Wireless CompactFlash Card",  "",  "", 0x733cc81, 0xc52f395, 0x0, 0x0), // Linksys WCF12 11Mbps 802.11b WLAN Card (Prism 3)
-	PCMCIA_DEVICE_MANF_CARD(0x028a, 0x0673), // Linksys WCF12 11Mbps 802.11b WLAN Card (Prism 3)
-	PCMCIA_DEVICE_PROD_ID1234("NETGEAR MA401RA Wireless PC",  "Card",  "ISL37300P",  "Eval-RevA", 0x306467f, 0x9762e8f1, 0xc9049a39, 0xc23adc0e), // NETGEAR MA401RA 11Mbps 802.11 WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0xd601, 0x0005), // D-Link DCF-660W  11Mbps 802.11b WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0x02d2, 0x0001), // Microsoft Wireless Notebook Adapter MN-520
-	PCMCIA_DEVICE_MANF_CARD(0x0089, 0x0002), // AnyPoint(TM) Wireless II PC Card
-	PCMCIA_DEVICE_PROD_ID1234("D",  "Link DRC-650 11Mbps WLAN Card",  "Version 01.02",  "" , 0x71b18589, 0xf144e3ac, 0x4b74baa0, 0x0), // D-Link DRC-650 802.11b WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0x9005, 0x0021), // Adaptec AWN-8030
-	PCMCIA_DEVICE_MANF_CARD(0x000b, 0x7110), // D-Link DWL-650 rev P 802.11b WLAN card
-	// PCMCIA_DEVICE_PROD_ID1234("D-Link",  "DWL-650 Wireless PC Card RevP",  "ISL37101P-10",  "A3", 0x1a424a1c, 0x6ea57632, 0xdd97a26b, 0x56b21f52), // D-Link DWL-650 rev P 802.11b WLAN card
-	PCMCIA_DEVICE_PROD_ID123("INTERSIL",   "I-GATE 11M PC Card / PC Card plus",  "Version 01.02", 0x74c5e40d, 0x8304ff77, 0x4b74baa0), // I-Gate 11M PC Card
-	PCMCIA_DEVICE_PROD_ID1234("BENQ",  "AWL100 PCMCIA ADAPTER",  "ISL37300P",  "Eval-RevA", 0x35dadc74, 0x1f7fedb, 0xc9049a39, 0xc23adc0e), // benQ AWL100 802.11b WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0x000b, 0x7300), // benQ AWL100 802.11b WLAN Card
-	// PCMCIA_DEVICE_PROD_ID1("INTERSIL", 0x74c5e40d), // Intersil Prism 2 card
-	// PCMCIA_DEVICE_MANF_CARD(0x0156, 0x0002), // Intersil Prism 2 card
-
-	PCMCIA_DEVICE_NULL
-};
-
-MODULE_DEVICE_TABLE(pcmcia, prism2_cs_ids);
-#endif
-
-static struct pcmcia_driver prism2_cs_driver = {
-	.drv = {
-		.name = "prism2_cs",
-	},
-	.owner = THIS_MODULE,
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,15)
-	.suspend = prism2_cs_suspend,
-	.resume = prism2_cs_resume,
-	.remove = prism2_cs_remove,
-	.probe = prism2_cs_probe,
-	.id_table = prism2_cs_ids,
-#else
-	.attach = prism2sta_attach,
-	.detach = prism2sta_detach,
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,12)
-	.id_table = prism2_cs_ids,
-	.event =  prism2sta_event,
-#endif // > 2.6.12
-#endif // <= 2.6.15
-};
-#endif /* kernel_version */
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,15)
-#define CS_CHECK(fn, ret) \
-do { last_fn = (fn); if ((last_ret = (ret)) != 0) goto cs_failed; } while (0)
-
-#define CFG_CHECK(fn, retf) \
-do { int ret = (retf); \
-if (ret != 0) { \
-        WLAN_LOG_DEBUG(1, "CardServices(" #fn ") returned %d\n", ret); \
-        cs_error(pdev, fn, ret); \
-        goto next_entry; \
-} \
-} while (0)
-
-static void prism2_cs_remove(struct pcmcia_device *pdev)
-{
-	struct wlandevice  *wlandev;
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,17)
-        dev_link_t *link = dev_to_instance(pdev);
-#endif
-
-	DBFENTER;
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	wlandev = pdev->priv;
-#else
-	wlandev = link->priv;
-#endif
-
-	if (wlandev) {
-		p80211netdev_hwremoved(wlandev);
-		unregister_wlandev(wlandev);
-		wlan_unsetup(wlandev);
-		if (wlandev->priv) {
-			hfa384x_t *hw = wlandev->priv;
-			wlandev->priv = NULL;
-			if (hw) {
-				hfa384x_destroy(hw);
-				kfree(hw);
-			}
-		}
-		kfree(wlandev);
-	}
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	pdev->priv = NULL;
-        pcmcia_disable_device(pdev);
-#else
-        if (link->state & DEV_CONFIG) {
-	        if (link->win)
-			pcmcia_release_window(link->win);
-		pcmcia_release_configuration(link->handle);
-		if (link->io.NumPorts1)
-			pcmcia_release_io(link->handle, &link->io);
-		if (link->irq.AssignedIRQ)
-			pcmcia_release_irq(link->handle, &link->irq);
-
-		link->state &= ~DEV_CONFIG;
-	}
-
-	link->priv = NULL;
-	kfree(link);
-#endif
-
-	DBFEXIT;
-	return;
-}
-
-static int prism2_cs_suspend(struct pcmcia_device *pdev)
-{
-	struct wlandevice  *wlandev;
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,17)
-        dev_link_t *link = dev_to_instance(pdev);
-#endif
-
-	DBFENTER;
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	wlandev = pdev->priv;
-	prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
-#else
-	wlandev = link->priv;
-
-        link->state |= DEV_SUSPEND;
-        if (link->state & DEV_CONFIG) {
-		prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
-		pcmcia_release_configuration(link->handle);
-	}
-#endif
-
-	DBFEXIT;
-
-	return 0;
-}
-
-static int prism2_cs_resume(struct pcmcia_device *pdev)
-{
-	struct wlandevice  *wlandev;
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,17)
-        dev_link_t *link = dev_to_instance(pdev);
-#endif
-
-	DBFENTER;
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	wlandev = pdev->priv;
-	// XXX do something here?
-#else
-	wlandev = link->priv;
-        link->state &= ~DEV_SUSPEND;
-        if (link->state & DEV_CONFIG) {
-                pcmcia_request_configuration(link->handle, &link->conf);
-		// XXX do something here?
-	}
-#endif
-
-
-	DBFEXIT;
-
-	return 0;
-}
-
-static int prism2_cs_probe(struct pcmcia_device *pdev)
-{
-	int rval = 0;
-	struct wlandevice *wlandev = NULL;
-	hfa384x_t *hw = NULL;
-
-        config_info_t socketconf;
-        cisparse_t *parse = NULL;
-	tuple_t tuple;
-	uint8_t	buf[64];
-        int last_fn, last_ret;
-        cistpl_cftable_entry_t dflt = { 0 };
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,17)
-	dev_link_t *link;
-#endif
-
-	DBFENTER;
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	/* Set up interrupt type */
-        pdev->conf.IntType = INT_MEMORY_AND_IO;
-#else
-        link = kmalloc(sizeof(dev_link_t), GFP_KERNEL);
-        if (link == NULL)
-                return -ENOMEM;
-        memset(link, 0, sizeof(dev_link_t));
-
-        link->conf.Vcc = 33;
-        link->conf.IntType = INT_MEMORY_AND_IO;
-
-        link->handle = pdev;
-        pdev->instance = link;
-        link->state |= DEV_PRESENT | DEV_CONFIG_PENDING;
-
-#endif
-
-	// VCC crap?
-        parse = kmalloc(sizeof(cisparse_t), GFP_KERNEL);
-
-	wlandev = create_wlan();
-	if (!wlandev || !parse) {
-		WLAN_LOG_ERROR("%s: Memory allocation failure.\n", dev_info);
-		rval = -EIO;
-		goto failed;
-	}
-	hw = wlandev->priv;
-
-	if ( wlan_setup(wlandev) != 0 ) {
-		WLAN_LOG_ERROR("%s: wlan_setup() failed.\n", dev_info);
-		rval = -EIO;
-		goto failed;
-	}
-
-	/* Initialize the hw struct for now */
-	hfa384x_create(hw, 0, 0, NULL);
-	hw->wlandev = wlandev;
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	hw->pdev = pdev;
-	pdev->priv = wlandev;
-#else
-	hw->link = link;
-	link->priv = wlandev;
-#endif
-
-        tuple.DesiredTuple = CISTPL_CONFIG;
-        tuple.Attributes = 0;
-        tuple.TupleData = buf;
-        tuple.TupleDataMax = sizeof(buf);
-        tuple.TupleOffset = 0;
-        CS_CHECK(GetFirstTuple, pcmcia_get_first_tuple(pdev, &tuple));
-        CS_CHECK(GetTupleData, pcmcia_get_tuple_data(pdev, &tuple));
-        CS_CHECK(ParseTuple, pcmcia_parse_tuple(pdev, &tuple, parse));
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-        pdev->conf.ConfigBase = parse->config.base;
-        pdev->conf.Present = parse->config.rmask[0];
-#else
-        link->conf.ConfigBase = parse->config.base;
-        link->conf.Present = parse->config.rmask[0];
-
-	link->conf.Vcc = socketconf.Vcc;
-#endif
-        CS_CHECK(GetConfigurationInfo,
-                 pcmcia_get_configuration_info(pdev, &socketconf));
-
-	tuple.DesiredTuple = CISTPL_CFTABLE_ENTRY;
-	CS_CHECK(GetFirstTuple, pcmcia_get_first_tuple(pdev, &tuple));
-        for (;;) {
-		cistpl_cftable_entry_t *cfg = &(parse->cftable_entry);
-                CFG_CHECK(GetTupleData,
-                           pcmcia_get_tuple_data(pdev, &tuple));
-                CFG_CHECK(ParseTuple,
-                           pcmcia_parse_tuple(pdev, &tuple, parse));
-
-                if (cfg->flags & CISTPL_CFTABLE_DEFAULT)
-                        dflt = *cfg;
-                if (cfg->index == 0)
-                        goto next_entry;
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-                pdev->conf.ConfigIndex = cfg->index;
-#else
-                link->conf.ConfigIndex = cfg->index;
-#endif
-
-                /* Does this card need audio output? */
-                if (cfg->flags & CISTPL_CFTABLE_AUDIO) {
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-                        pdev->conf.Attributes |= CONF_ENABLE_SPKR;
-                        pdev->conf.Status = CCSR_AUDIO_ENA;
-#else
-                        link->conf.Attributes |= CONF_ENABLE_SPKR;
-                        link->conf.Status = CCSR_AUDIO_ENA;
-#endif
-                }
-
-                /* Use power settings for Vcc and Vpp if present */
-                /*  Note that the CIS values need to be rescaled */
-                if (cfg->vcc.present & (1 << CISTPL_POWER_VNOM)) {
-                        if (socketconf.Vcc != cfg->vcc.param[CISTPL_POWER_VNOM] /
-                            10000 && !prism2_ignorevcc) {
-                                WLAN_LOG_DEBUG(1, "  Vcc mismatch - skipping"
-                                       " this entry\n");
-                                goto next_entry;
-                        }
-                } else if (dflt.vcc.present & (1 << CISTPL_POWER_VNOM)) {
-                        if (socketconf.Vcc != dflt.vcc.param[CISTPL_POWER_VNOM] /
-                            10000 && !prism2_ignorevcc) {
-                                WLAN_LOG_DEBUG(1, "  Vcc (default) mismatch "
-                                       "- skipping this entry\n");
-                                goto next_entry;
-                        }
-                }
-
-                if (cfg->vpp1.present & (1 << CISTPL_POWER_VNOM)) {
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-                        pdev->conf.Vpp =
-                                cfg->vpp1.param[CISTPL_POWER_VNOM] / 10000;
-#else
-                        link->conf.Vpp1 = link->conf.Vpp2 =
-                                cfg->vpp1.param[CISTPL_POWER_VNOM] / 10000;
-#endif
-                } else if (dflt.vpp1.present & (1 << CISTPL_POWER_VNOM)) {
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-                        pdev->conf.Vpp =
-                                dflt.vpp1.param[CISTPL_POWER_VNOM] / 10000;
-#else
-                        link->conf.Vpp1 = link->conf.Vpp2 =
-                                dflt.vpp1.param[CISTPL_POWER_VNOM] / 10000;
-#endif
-		}
-
-		/* Do we need to allocate an interrupt? */
-		/* HACK: due to a bad CIS....we ALWAYS need an interrupt */
-		/* if (cfg->irq.IRQInfo1 || dflt.irq.IRQInfo1) */
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-		pdev->conf.Attributes |= CONF_ENABLE_IRQ;
-#else
-		link->conf.Attributes |= CONF_ENABLE_IRQ;
-#endif
-
-		/* IO window settings */
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-		pdev->io.NumPorts1 = pdev->io.NumPorts2 = 0;
-		if ((cfg->io.nwin > 0) || (dflt.io.nwin > 0)) {
-			cistpl_io_t *io = (cfg->io.nwin) ? &cfg->io : &dflt.io;
-			pdev->io.Attributes1 = IO_DATA_PATH_WIDTH_AUTO;
-			if (!(io->flags & CISTPL_IO_8BIT))
-				pdev->io.Attributes1 = IO_DATA_PATH_WIDTH_16;
-			if (!(io->flags & CISTPL_IO_16BIT))
-				pdev->io.Attributes1 = IO_DATA_PATH_WIDTH_8;
-			pdev->io.BasePort1 = io->win[0].base;
-			if  ( pdev->io.BasePort1 != 0 ) {
-				WLAN_LOG_WARNING(
-				"Brain damaged CIS: hard coded iobase="
-				"0x%x, try letting pcmcia_cs decide...\n",
-				pdev->io.BasePort1 );
-				pdev->io.BasePort1 = 0;
-			}
-			pdev->io.NumPorts1 = io->win[0].len;
-			if (io->nwin > 1) {
-				pdev->io.Attributes2 = pdev->io.Attributes1;
-				pdev->io.BasePort2 = io->win[1].base;
-				pdev->io.NumPorts2 = io->win[1].len;
-			}
-		}
-		/* This reserves IO space but doesn't actually enable it */
-		CFG_CHECK(RequestIO, pcmcia_request_io(pdev, &pdev->io));
-#else
-		link->io.NumPorts1 = link->io.NumPorts2 = 0;
-		if ((cfg->io.nwin > 0) || (dflt.io.nwin > 0)) {
-			cistpl_io_t *io = (cfg->io.nwin) ? &cfg->io : &dflt.io;
-			link->io.Attributes1 = IO_DATA_PATH_WIDTH_AUTO;
-			if (!(io->flags & CISTPL_IO_8BIT))
-				link->io.Attributes1 = IO_DATA_PATH_WIDTH_16;
-			if (!(io->flags & CISTPL_IO_16BIT))
-				link->io.Attributes1 = IO_DATA_PATH_WIDTH_8;
-			link->io.BasePort1 = io->win[0].base;
-			if  ( link->io.BasePort1 != 0 ) {
-				WLAN_LOG_WARNING(
-				"Brain damaged CIS: hard coded iobase="
-				"0x%x, try letting pcmcia_cs decide...\n",
-				link->io.BasePort1 );
-				link->io.BasePort1 = 0;
-			}
-			link->io.NumPorts1 = io->win[0].len;
-			if (io->nwin > 1) {
-				link->io.Attributes2 = link->io.Attributes1;
-				link->io.BasePort2 = io->win[1].base;
-				link->io.NumPorts2 = io->win[1].len;
-			}
-		}
-		/* This reserves IO space but doesn't actually enable it */
-		CFG_CHECK(RequestIO, pcmcia_request_io(pdev, &link->io));
-#endif
-		/* If we got this far, we're cool! */
-		break;
-
-	next_entry:
-		if (cfg->flags & CISTPL_CFTABLE_DEFAULT)
-			dflt = *cfg;
-		CS_CHECK(GetNextTuple, pcmcia_get_next_tuple(pdev, &tuple));
-
-	}
-
-	/* Let pcmcia know the device name */
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	pdev->dev_node = &hw->node;
-#else
-	link->dev = &hw->node;
-#endif
-
-	/* Register the network device and get assigned a name */
-	SET_MODULE_OWNER(wlandev->netdev);
-	SET_NETDEV_DEV(wlandev->netdev,  &handle_to_dev(pdev));
-	if (register_wlandev(wlandev) != 0) {
-		WLAN_LOG_NOTICE("prism2sta_cs: register_wlandev() failed.\n");
-		goto failed;
-	}
-
-	strcpy(hw->node.dev_name, wlandev->name);
-
-	/* Allocate an interrupt line.  Note that this does not assign a */
-	/* handler to the interrupt, unless the 'Handler' member of the */
-	/* irq structure is initialized. */
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	if (pdev->conf.Attributes & CONF_ENABLE_IRQ) {
-		pdev->irq.IRQInfo1 = IRQ_LEVEL_ID;
-		pdev->irq.Attributes = IRQ_TYPE_EXCLUSIVE | IRQ_HANDLE_PRESENT;
-		pdev->irq.Handler = hfa384x_interrupt;
-		pdev->irq.Instance = wlandev;
-		CS_CHECK(RequestIRQ, pcmcia_request_irq(pdev, &pdev->irq));
-	}
-#else
-	if (link->conf.Attributes & CONF_ENABLE_IRQ) {
-		link->irq.IRQInfo1 = IRQ_LEVEL_ID;
-		link->irq.Attributes = IRQ_TYPE_EXCLUSIVE | IRQ_HANDLE_PRESENT;
-		link->irq.Handler = hfa384x_interrupt;
-		link->irq.Instance = wlandev;
-		CS_CHECK(RequestIRQ, pcmcia_request_irq(pdev, &link->irq));
-	}
-#endif
-
-	/* This actually configures the PCMCIA socket -- setting up */
-	/* the I/O windows and the interrupt mapping, and putting the */
-	/* card and host interface into "Memory and IO" mode. */
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	CS_CHECK(RequestConfiguration, pcmcia_request_configuration(pdev, &pdev->conf));
-#else
-	CS_CHECK(RequestConfiguration, pcmcia_request_configuration(pdev, &link->conf));
-#endif
-
-	/* Fill the netdevice with this info */
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	wlandev->netdev->irq = pdev->irq.AssignedIRQ;
-	wlandev->netdev->base_addr = pdev->io.BasePort1;
-#else
-	wlandev->netdev->irq = link->irq.AssignedIRQ;
-	wlandev->netdev->base_addr = link->io.BasePort1;
-#endif
-
-	/* And the rest of the hw structure */
-	hw->irq = wlandev->netdev->irq;
-	hw->iobase = wlandev->netdev->base_addr;
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,17)
-	link->state |= DEV_CONFIG;
-	link->state &= ~DEV_CONFIG_PENDING;
-#endif
-
-	/* And now we're done! */
-	wlandev->msdstate = WLAN_MSD_HWPRESENT;
-
-	goto done;
-
- cs_failed:
-        cs_error(pdev, last_fn, last_ret);
-
-failed:
-	// wlandev, hw, etc etc..
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	pdev->priv = NULL;
-#else
-	pdev->instance = NULL;
-	if (link) {
-		link->priv = NULL;
-		kfree(link);
-	}
-#endif
-	if (wlandev) {
-		wlan_unsetup(wlandev);
-		if (wlandev->priv) {
-			hw = wlandev->priv;
-			wlandev->priv = NULL;
-			if (hw) {
-				hfa384x_destroy(hw);
-				kfree(hw);
-			}
-		}
-		kfree(wlandev);
-	}
-
-done:
-	if (parse) kfree(parse);
-
-	DBFEXIT;
-	return rval;
-}
-#else  // <= 2.6.15
-#define CS_CHECK(fn, ret) \
-do { last_fn = (fn); if ((last_ret = (ret)) != 0) goto cs_failed; } while (0)
-
-#define CFG_CHECK(fn, retf) \
-do { int ret = (retf); \
-if (ret != 0) { \
-        WLAN_LOG_DEBUG(1, "CardServices(" #fn ") returned %d\n", ret); \
-        cs_error(link->handle, fn, ret); \
-        goto next_entry; \
-} \
-} while (0)
-
-/*----------------------------------------------------------------
-* prism2sta_attach
-*
-* Half of the attach/detach pair.  Creates and registers a device
-* instance with Card Services.  In this case, it also creates the
-* wlandev structure and device private structure.  These are
-* linked to the device instance via its priv member.
-*
-* Arguments:
-*	none
-*
-* Returns:
-*	A valid ptr to dev_link_t on success, NULL otherwise
-*
-* Side effects:
-*
-*
-* Call context:
-*	process thread (insmod/init_module/register_pccard_driver)
-----------------------------------------------------------------*/
-dev_link_t *prism2sta_attach(void)
-{
-	client_reg_t		client_reg;
-	int			result;
-	dev_link_t		*link = NULL;
-	wlandevice_t		*wlandev = NULL;
-	hfa384x_t		*hw = NULL;
-
-	DBFENTER;
-
-	/* Alloc our structures */
-	link =		kmalloc(sizeof(struct dev_link_t), GFP_KERNEL);
-
-	if (!link || ((wlandev = create_wlan()) == NULL)) {
-		WLAN_LOG_ERROR("%s: Memory allocation failure.\n", dev_info);
-		result = -EIO;
-		goto failed;
-	}
-	hw = wlandev->priv;
-
-	/* Clear all the structs */
-	memset(link, 0, sizeof(struct dev_link_t));
-
-	if ( wlan_setup(wlandev) != 0 ) {
-		WLAN_LOG_ERROR("%s: wlan_setup() failed.\n", dev_info);
-		result = -EIO;
-		goto failed;
-	}
-
-	/* Initialize the hw struct for now */
-	hfa384x_create(hw, 0, 0, NULL);
-	hw->wlandev = wlandev;
-
-	/* Initialize the PC card device object. */
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
-	init_timer(&link->release);
-	link->release.function = &prism2sta_release;
-	link->release.data = (u_long)link;
-#endif
-	link->conf.IntType = INT_MEMORY_AND_IO;
-	link->priv = wlandev;
-#if (defined(CS_RELEASE_CODE) && (CS_RELEASE_CODE < 0x2911))
-	link->irq.Instance = wlandev;
-#endif
-
-	/* Link in to the list of devices managed by this driver */
-	link->next = dev_list;
-	dev_list = link;
-
-	/* Register with Card Services */
-	client_reg.dev_info = &dev_info;
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11) )
-	client_reg.Attributes = INFO_IO_CLIENT | INFO_CARD_SHARE;
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,13) )
-	client_reg.EventMask =
-		CS_EVENT_CARD_INSERTION | CS_EVENT_CARD_REMOVAL |
-		CS_EVENT_RESET_REQUEST |
-		CS_EVENT_RESET_PHYSICAL | CS_EVENT_CARD_RESET |
-		CS_EVENT_PM_SUSPEND | CS_EVENT_PM_RESUME;
-	client_reg.event_handler = &prism2sta_event;
-#endif
-
-	client_reg.Version = 0x0210;
-	client_reg.event_callback_args.client_data = link;
-
-	result = pcmcia_register_client(&link->handle, &client_reg);
-	if (result != 0) {
-		cs_error(link->handle, RegisterClient, result);
-		prism2sta_detach(link);
-		return NULL;
-	}
-
-	goto done;
-
- failed:
-	if (link)	kfree(link);
-	if (wlandev)	kfree(wlandev);
-	if (hw)		kfree(hw);
-	link = NULL;
-
- done:
-	DBFEXIT;
-	return link;
-}
-
-
-/*----------------------------------------------------------------
-* prism2sta_detach
-*
-* Remove one of the device instances managed by this driver.
-*   Search the list for the given instance,
-*   check our flags for a waiting timer'd release call
-*   call release
-*   Deregister the instance with Card Services
-*   (netdevice) unregister the network device.
-*   unlink the instance from the list
-*   free the link, priv, and priv->priv memory
-* Note: the dev_list variable is a driver scoped static used to
-*	maintain a list of device instances managed by this
-*	driver.
-*
-* Arguments:
-*	link	ptr to the instance to detach
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	the link structure is gone, the netdevice is gone
-*
-* Call context:
-*	Might be interrupt, don't block.
-----------------------------------------------------------------*/
-void prism2sta_detach(dev_link_t *link)
-{
-	dev_link_t		**linkp;
-	wlandevice_t		*wlandev;
-	hfa384x_t		*hw;
-
-	DBFENTER;
-
-	/* Locate prev device structure */
-	for (linkp = &dev_list; *linkp; linkp = &(*linkp)->next) {
-		if (*linkp == link) break;
-	}
-
-	if (*linkp != NULL) {
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0))
-		unsigned long	flags;
-		/* Get rid of any timer'd release call */
-		save_flags(flags);
-		cli();
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
-		if (link->state & DEV_RELEASE_PENDING) {
-			del_timer_sync(&link->release);
-			link->state &= ~DEV_RELEASE_PENDING;
-		}
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0))
-		restore_flags(flags);
-#endif
-
-		/* If link says we're still config'd, call release */
-		if (link->state & DEV_CONFIG) {
-			prism2sta_release((u_long)link);
-			if (link->state & DEV_STALE_CONFIG) {
-				link->state |= DEV_STALE_LINK;
-				return;
-			}
-		}
-
-		/* Tell Card Services we're not around any more */
-		if (link->handle) {
-			pcmcia_deregister_client(link->handle);
-		}
-
-		/* Unlink device structure, free bits */
-		*linkp = link->next;
-		if ( link->priv != NULL ) {
-			wlandev = (wlandevice_t*)link->priv;
-			p80211netdev_hwremoved(wlandev);
-			if (link->dev != NULL) {
-				unregister_wlandev(wlandev);
-			}
-			wlan_unsetup(wlandev);
-			if (wlandev->priv) {
-				hw = wlandev->priv;
-				wlandev->priv = NULL;
-				if (hw) {
-					hfa384x_destroy(hw);
-					kfree(hw);
-				}
-			}
-			link->priv = NULL;
-			kfree(wlandev);
-		}
-		kfree(link);
-	}
-
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* prism2sta_config
-*
-* Half of the config/release pair.  Usually called in response to
-* a card insertion event.  At this point, we _know_ there's some
-* physical device present.  That means we can start poking around
-* at the CIS and at any device specific config data we want.
-*
-* Note the gotos and the macros.  I recoded this once without
-* them, and it got incredibly ugly.  It's actually simpler with
-* them.
-*
-* Arguments:
-*	link	the dev_link_t structure created in attach that
-*		represents this device instance.
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	Resources (irq, io, mem) are allocated
-*	The pcmcia dev_link->node->name is set
-*	(For netcards) The device structure is finished and,
-*	  most importantly, registered.  This means that there
-*	  is now a _named_ device that can be configured from
-*	  userland.
-*
-* Call context:
-*	May be called from a timer.  Don't block!
-----------------------------------------------------------------*/
-#define CS_CHECK(fn, ret) \
-do { last_fn = (fn); if ((last_ret = (ret)) != 0) goto cs_failed; } while (0)
-
-#define CFG_CHECK(fn, retf) \
-do { int ret = (retf); \
-if (ret != 0) { \
-        WLAN_LOG_DEBUG(1, "CardServices(" #fn ") returned %d\n", ret); \
-        cs_error(link->handle, fn, ret); \
-        goto next_entry; \
-} \
-} while (0)
-
-void prism2sta_config(dev_link_t *link)
-{
-	client_handle_t		handle;
-	wlandevice_t		*wlandev;
-	hfa384x_t               *hw;
-	int			last_fn;
-	int			last_ret;
-	tuple_t			tuple;
-	cisparse_t		parse;
-	config_info_t		socketconf;
-	UINT8			buf[64];
-	int			minVcc = 0;
-	int			maxVcc = 0;
-	cistpl_cftable_entry_t	dflt = { 0 };
-
-	DBFENTER;
-
-	handle = link->handle;
-	wlandev = (wlandevice_t*)link->priv;
-	hw = wlandev->priv;
-
-	/* Collect the config register info */
-	tuple.DesiredTuple = CISTPL_CONFIG;
-	tuple.Attributes = 0;
-	tuple.TupleData = buf;
-	tuple.TupleDataMax = sizeof(buf);
-	tuple.TupleOffset = 0;
-	CS_CHECK(GetFirstTuple, pcmcia_get_first_tuple(handle, &tuple));
-	CS_CHECK(GetTupleData, pcmcia_get_tuple_data(handle, &tuple));
-	CS_CHECK(ParseTuple, pcmcia_parse_tuple(handle, &tuple, &parse));
-
-	link->conf.ConfigBase = parse.config.base;
-	link->conf.Present = parse.config.rmask[0];
-
-	/* Configure card */
-	link->state |= DEV_CONFIG;
-
-	/* Acquire the current socket config (need Vcc setting) */
-	CS_CHECK(GetConfigurationInfo, pcmcia_get_configuration_info(handle, &socketconf));
-
-	/* Loop through the config table entries until we find one that works */
-	/* Assumes a complete and valid CIS */
-	tuple.DesiredTuple = CISTPL_CFTABLE_ENTRY;
-	CS_CHECK(GetFirstTuple, pcmcia_get_first_tuple(handle, &tuple));
-	while (1) {
-		cistpl_cftable_entry_t *cfg = &(parse.cftable_entry);
-		CFG_CHECK(GetTupleData, pcmcia_get_tuple_data(handle, &tuple));
-		CFG_CHECK(ParseTuple, pcmcia_parse_tuple(handle, &tuple, &parse));
-
-		if (cfg->index == 0) goto next_entry;
-		link->conf.ConfigIndex = cfg->index;
-
-		/* Lets print out the Vcc that the controller+pcmcia-cs set
-		 * for us, cause that's what we're going to use.
-		 */
-		WLAN_LOG_DEBUG(1,"Initial Vcc=%d/10v\n", socketconf.Vcc);
-		if (prism2_ignorevcc) {
-			link->conf.Vcc = socketconf.Vcc;
-			goto skipvcc;
-		}
-
-		/* Use power settings for Vcc and Vpp if present */
-		/* Note that the CIS values need to be rescaled */
-		if (cfg->vcc.present & (1<<CISTPL_POWER_VNOM)) {
-			WLAN_LOG_DEBUG(1, "Vcc obtained from curtupl.VNOM\n");
-			minVcc = maxVcc =
-				cfg->vcc.param[CISTPL_POWER_VNOM]/10000;
-		} else if (dflt.vcc.present & (1<<CISTPL_POWER_VNOM)) {
-			WLAN_LOG_DEBUG(1, "Vcc set from dflt.VNOM\n");
-			minVcc = maxVcc =
-				dflt.vcc.param[CISTPL_POWER_VNOM]/10000;
-		} else if ((cfg->vcc.present & (1<<CISTPL_POWER_VMAX)) &&
-			   (cfg->vcc.present & (1<<CISTPL_POWER_VMIN)) ) {
-			WLAN_LOG_DEBUG(1, "Vcc set from curtupl(VMIN,VMAX)\n");			minVcc = cfg->vcc.param[CISTPL_POWER_VMIN]/10000;
-			maxVcc = cfg->vcc.param[CISTPL_POWER_VMAX]/10000;
-		} else if ((dflt.vcc.present & (1<<CISTPL_POWER_VMAX)) &&
-			   (dflt.vcc.present & (1<<CISTPL_POWER_VMIN)) ) {
-			WLAN_LOG_DEBUG(1, "Vcc set from dflt(VMIN,VMAX)\n");
-			minVcc = dflt.vcc.param[CISTPL_POWER_VMIN]/10000;
-			maxVcc = dflt.vcc.param[CISTPL_POWER_VMAX]/10000;
-		}
-
-		if ( socketconf.Vcc >= minVcc && socketconf.Vcc <= maxVcc) {
-			link->conf.Vcc = socketconf.Vcc;
-		} else {
-			/* [MSM]: Note that I've given up trying to change
-			 * the Vcc if a change is indicated.  It seems the
-			 * system&socketcontroller&card vendors can't seem
-			 * to get it right, so I'm tired of trying to hack
-			 * my way around it.  pcmcia-cs does its best using
-			 * the voltage sense pins but sometimes the controller
-			 * lies.  Then, even if we have a good read on the VS
-			 * pins, some system designs will silently ignore our
-			 * requests to set the voltage.  Additionally, some
-			 * vendors have 3.3v indicated on their sense pins,
-			 * but 5v specified in the CIS or vice-versa.  I've
-			 * had it.  My only recommendation is "let the buyer
-			 * beware".  Your system might supply 5v to a 3v card
-			 * (possibly causing damage) or a 3v capable system
-			 * might supply 5v to a 3v capable card (wasting
-			 * precious battery life).
-			 * My only recommendation (if you care) is to get
-			 * yourself an extender card (I don't know where, I
-			 * have only one myself) and a meter and test it for
-			 * yourself.
-			 */
-			goto next_entry;
-		}
-skipvcc:
-		WLAN_LOG_DEBUG(1, "link->conf.Vcc=%d\n", link->conf.Vcc);
-
-		/* Do we need to allocate an interrupt? */
-		/* HACK: due to a bad CIS....we ALWAYS need an interrupt */
-		/* if (cfg->irq.IRQInfo1 || dflt.irq.IRQInfo1) */
-			link->conf.Attributes |= CONF_ENABLE_IRQ;
-
-		/* IO window settings */
-		link->io.NumPorts1 = link->io.NumPorts2 = 0;
-		if ((cfg->io.nwin > 0) || (dflt.io.nwin > 0)) {
-			cistpl_io_t *io = (cfg->io.nwin) ? &cfg->io : &dflt.io;
-			link->io.Attributes1 = IO_DATA_PATH_WIDTH_AUTO;
-			if (!(io->flags & CISTPL_IO_8BIT))
-				link->io.Attributes1 = IO_DATA_PATH_WIDTH_16;
-			if (!(io->flags & CISTPL_IO_16BIT))
-				link->io.Attributes1 = IO_DATA_PATH_WIDTH_8;
-			link->io.BasePort1 = io->win[0].base;
-			if  ( link->io.BasePort1 != 0 ) {
-				WLAN_LOG_WARNING(
-				"Brain damaged CIS: hard coded iobase="
-				"0x%x, try letting pcmcia_cs decide...\n",
-				link->io.BasePort1 );
-				link->io.BasePort1 = 0;
-			}
-			link->io.NumPorts1 = io->win[0].len;
-			if (io->nwin > 1) {
-				link->io.Attributes2 = link->io.Attributes1;
-				link->io.BasePort2 = io->win[1].base;
-				link->io.NumPorts2 = io->win[1].len;
-			}
-		}
-
-		/* This reserves IO space but doesn't actually enable it */
-		CFG_CHECK(RequestIO, pcmcia_request_io(link->handle, &link->io));
-
-		/* If we got this far, we're cool! */
-		break;
-
-next_entry:
-		if (cfg->flags & CISTPL_CFTABLE_DEFAULT)
-			dflt = *cfg;
-		CS_CHECK(GetNextTuple,
-                         pcmcia_get_next_tuple(handle, &tuple));
-	}
-
-	/* Allocate an interrupt line.  Note that this does not assign a */
-	/* handler to the interrupt, unless the 'Handler' member of the */
-	/* irq structure is initialized. */
-	if (link->conf.Attributes & CONF_ENABLE_IRQ)
-	{
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11) )
-		int			i;
-		link->irq.IRQInfo1 = IRQ_INFO2_VALID | IRQ_LEVEL_ID;
-		if (irq_list[0] == -1)
-			link->irq.IRQInfo2 = irq_mask;
-		else
-			for (i=0; i<4; i++)
-				link->irq.IRQInfo2 |= 1 << irq_list[i];
-#else
-		link->irq.IRQInfo1 = IRQ_LEVEL_ID;
-#endif
-		link->irq.Attributes = IRQ_TYPE_EXCLUSIVE | IRQ_HANDLE_PRESENT;
-		link->irq.Handler = hfa384x_interrupt;
-		link->irq.Instance = wlandev;
-		CS_CHECK(RequestIRQ, pcmcia_request_irq(link->handle, &link->irq));
-	}
-
-	/* This actually configures the PCMCIA socket -- setting up */
-	/* the I/O windows and the interrupt mapping, and putting the */
-	/* card and host interface into "Memory and IO" mode. */
-	CS_CHECK(RequestConfiguration, pcmcia_request_configuration(link->handle, &link->conf));
-
-	/* Fill the netdevice with this info */
-	wlandev->netdev->irq = link->irq.AssignedIRQ;
-	wlandev->netdev->base_addr = link->io.BasePort1;
-
-	/* Report what we've done */
-	WLAN_LOG_INFO("%s: index 0x%02x: Vcc %d.%d",
-		dev_info, link->conf.ConfigIndex,
-		link->conf.Vcc/10, link->conf.Vcc%10);
-	if (link->conf.Vpp1)
-		printk(", Vpp %d.%d", link->conf.Vpp1/10, link->conf.Vpp1%10);
-	if (link->conf.Attributes & CONF_ENABLE_IRQ)
-		printk(", irq %d", link->irq.AssignedIRQ);
-	if (link->io.NumPorts1)
-		printk(", io 0x%04x-0x%04x", link->io.BasePort1, link->io.BasePort1+link->io.NumPorts1-1);
-	if (link->io.NumPorts2)
-		printk(" & 0x%04x-0x%04x", link->io.BasePort2, link->io.BasePort2+link->io.NumPorts2-1);
-	printk("\n");
-
-	link->state &= ~DEV_CONFIG_PENDING;
-
-	/* Let pcmcia know the device name */
-	link->dev = &hw->node;
-
-	/* Register the network device and get assigned a name */
-	SET_MODULE_OWNER(wlandev->netdev);
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,11) )
-	SET_NETDEV_DEV(wlandev->netdev,  &handle_to_dev(link->handle));
-#endif
-	if (register_wlandev(wlandev) != 0) {
-		WLAN_LOG_NOTICE("prism2sta_cs: register_wlandev() failed.\n");
-		goto failed;
-	}
-
-	strcpy(hw->node.dev_name, wlandev->name);
-
-	/* Any device custom config/query stuff should be done here */
-	/* For a netdevice, we should at least grab the mac address */
-
-	return;
-cs_failed:
-	cs_error(link->handle, last_fn, last_ret);
-	WLAN_LOG_ERROR("NextTuple failure? It's probably a Vcc mismatch.\n");
-
-failed:
-	prism2sta_release((u_long)link);
-	return;
-}
-
-/*----------------------------------------------------------------
-* prism2sta_release
-*
-* Half of the config/release pair.  Usually called in response to
-* a card ejection event.  Checks to make sure no higher layers
-* are still (or think they are) using the card via the link->open
-* field.
-*
-* NOTE: Don't forget to increment the link->open variable in the
-*  device_open method, and decrement it in the device_close
-*  method.
-*
-* Arguments:
-*	arg	a generic 32 bit variable.  It's the value that
-*		we assigned to link->release.data in sta_attach().
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	All resources should be released after this function
-*	executes and finds the device !open.
-*
-* Call context:
-*	Possibly in a timer context.  Don't do anything that'll
-*	block.
-----------------------------------------------------------------*/
-void prism2sta_release(u_long arg)
-{
-        dev_link_t	*link = (dev_link_t *)arg;
-
-	DBFENTER;
-
-	/* First thing we should do is get the MSD back to the
-	 * HWPRESENT state.  I.e. everything quiescent.
-	 */
-	prism2sta_ifstate(link->priv, P80211ENUM_ifstate_disable);
-
-        if (link->open) {
-		/* TODO: I don't think we're even using this bit of code
-		 * and I don't think it's hurting us at the moment.
-		 */
-                WLAN_LOG_DEBUG(1,
-			"prism2sta_cs: release postponed, '%s' still open\n",
-			link->dev->dev_name);
-                link->state |= DEV_STALE_CONFIG;
-                return;
-        }
-
-        pcmcia_release_configuration(link->handle);
-        pcmcia_release_io(link->handle, &link->io);
-        pcmcia_release_irq(link->handle, &link->irq);
-
-        link->state &= ~(DEV_CONFIG | DEV_RELEASE_PENDING);
-
-	DBFEXIT;
-}
-
-/*----------------------------------------------------------------
-* prism2sta_event
-*
-* Handler for card services events.
-*
-* Arguments:
-*	event		The event code
-*	priority	hi/low - REMOVAL is the only hi
-*	args		ptr to card services struct containing info about
-*			pcmcia status
-*
-* Returns:
-*	Zero on success, non-zero otherwise
-*
-* Side effects:
-*
-*
-* Call context:
-*	Both interrupt and process thread, depends on the event.
-----------------------------------------------------------------*/
-static int
-prism2sta_event (
-	event_t event,
-	int priority,
-	event_callback_args_t *args)
-{
-	int			result = 0;
-	dev_link_t		*link = (dev_link_t *) args->client_data;
-	wlandevice_t		*wlandev = (wlandevice_t*)link->priv;
-	hfa384x_t		*hw = NULL;
-
-	DBFENTER;
-
-	if (wlandev) hw = wlandev->priv;
-
-	switch (event)
-	{
-	case CS_EVENT_CARD_INSERTION:
-		WLAN_LOG_DEBUG(5,"event is INSERTION\n");
-		link->state |= DEV_PRESENT | DEV_CONFIG_PENDING;
-		prism2sta_config(link);
-		if (!(link->state & DEV_CONFIG)) {
-			wlandev->netdev->irq = 0;
-			WLAN_LOG_ERROR(
-				"%s: Initialization failed!\n", dev_info);
-			wlandev->msdstate = WLAN_MSD_HWFAIL;
-			break;
-		}
-
-		/* Fill in the rest of the hw struct */
-		hw->irq = wlandev->netdev->irq;
-		hw->iobase = wlandev->netdev->base_addr;
-		hw->link = link;
-
-		if (prism2_doreset) {
-			result = hfa384x_corereset(hw,
-					prism2_reset_holdtime,
-					prism2_reset_settletime, 0);
-			if ( result ) {
-				WLAN_LOG_ERROR(
-					"corereset() failed, result=%d.\n",
-					result);
-				wlandev->msdstate = WLAN_MSD_HWFAIL;
-				break;
-			}
-		}
-
-#if 0
-		/*
-		 * TODO: test_hostif() not implemented yet.
-		 */
-		result = hfa384x_test_hostif(hw);
-		if (result) {
-			WLAN_LOG_ERROR(
-			"test_hostif() failed, result=%d.\n", result);
-			wlandev->msdstate = WLAN_MSD_HWFAIL;
-			break;
-		}
-#endif
-		wlandev->msdstate = WLAN_MSD_HWPRESENT;
-		break;
-
-	case CS_EVENT_CARD_REMOVAL:
-		WLAN_LOG_DEBUG(5,"event is REMOVAL\n");
-		link->state &= ~DEV_PRESENT;
-
-		if (wlandev) {
-			p80211netdev_hwremoved(wlandev);
-		}
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
-		if (link->state & DEV_CONFIG)
-		{
-			link->release.expires = jiffies + (HZ/20);
-			add_timer(&link->release);
-		}
-#endif
-		break;
-	case CS_EVENT_RESET_REQUEST:
-		WLAN_LOG_DEBUG(5,"event is RESET_REQUEST\n");
-		WLAN_LOG_NOTICE(
-			"prism2 card reset not supported "
-			"due to post-reset user mode configuration "
-			"requirements.\n");
-		WLAN_LOG_NOTICE(
-			"  From user mode, use "
-			"'cardctl suspend;cardctl resume' "
-			"instead.\n");
-		break;
-	case CS_EVENT_RESET_PHYSICAL:
-	case CS_EVENT_CARD_RESET:
-		WLAN_LOG_WARNING("Rx'd CS_EVENT_RESET_xxx, should not "
-			"be possible since RESET_REQUEST was denied.\n");
-		break;
-
-	case CS_EVENT_PM_SUSPEND:
-		WLAN_LOG_DEBUG(5,"event is SUSPEND\n");
-		link->state |= DEV_SUSPEND;
-		if (link->state & DEV_CONFIG)
-		{
-			prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
-			pcmcia_release_configuration(link->handle);
-		}
-		break;
-
-	case CS_EVENT_PM_RESUME:
-		WLAN_LOG_DEBUG(5,"event is RESUME\n");
-		link->state &= ~DEV_SUSPEND;
-		if (link->state & DEV_CONFIG) {
-			pcmcia_request_configuration(link->handle, &link->conf);
-		}
-		break;
-	}
-
-	DBFEXIT;
-	return 0;  /* noone else does anthing with the return value */
-}
-#endif // <= 2.6.15
-
-
-
-int hfa384x_corereset(hfa384x_t *hw, int holdtime, int settletime, int genesis)
-{
-	int		result = 0;
-	conf_reg_t	reg;
-	UINT8		corsave;
-	DBFENTER;
-
-	WLAN_LOG_DEBUG(3, "Doing reset via CardServices().\n");
-
-	/* Collect COR */
-	reg.Function = 0;
-	reg.Action = CS_READ;
-	reg.Offset = CISREG_COR;
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	result = pcmcia_access_configuration_register(hw->pdev, &reg);
-#else
-	result = pcmcia_access_configuration_register(
-			hw->link->handle,
-			&reg);
-#endif
-	if (result != CS_SUCCESS ) {
-		WLAN_LOG_ERROR(
-			":0: AccessConfigurationRegister(CS_READ) failed,"
-			"result=%d.\n", result);
-		result = -EIO;
-	}
-	corsave = reg.Value;
-
-	/* Write reset bit (BIT7) */
-	reg.Value |= BIT7;
-	reg.Action = CS_WRITE;
-	reg.Offset = CISREG_COR;
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	result = pcmcia_access_configuration_register(hw->pdev, &reg);
-#else
-	result = pcmcia_access_configuration_register(
-			hw->link->handle,
-			&reg);
-#endif
-	if (result != CS_SUCCESS ) {
-		WLAN_LOG_ERROR(
-			":1: AccessConfigurationRegister(CS_WRITE) failed,"
-			"result=%d.\n", result);
-		result = -EIO;
-	}
-
-	/* Hold for holdtime */
-	mdelay(holdtime);
-
-	if (genesis) {
-		reg.Value = genesis;
-		reg.Action = CS_WRITE;
-		reg.Offset = CISREG_CCSR;
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-		result = pcmcia_access_configuration_register(hw->pdev, &reg);
-#else
-		result = pcmcia_access_configuration_register(
-				      hw->link->handle,
-				      &reg);
-#endif
-		if (result != CS_SUCCESS ) {
-			WLAN_LOG_ERROR(
-				":1: AccessConfigurationRegister(CS_WRITE) failed,"
-				"result=%d.\n", result);
-			result = -EIO;
-		}
-	}
-
-	/* Hold for holdtime */
-	mdelay(holdtime);
-
-	/* Clear reset bit */
-	reg.Value &= ~BIT7;
-	reg.Action = CS_WRITE;
-	reg.Offset = CISREG_COR;
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	result = pcmcia_access_configuration_register(hw->pdev, &reg);
-#else
-	result = pcmcia_access_configuration_register(
-				      hw->link->handle,
-				      &reg);
-#endif
-	if (result != CS_SUCCESS ) {
-		WLAN_LOG_ERROR(
-			":2: AccessConfigurationRegister(CS_WRITE) failed,"
-			"result=%d.\n", result);
-		result = -EIO;
-		goto done;
-	}
-
-	/* Wait for settletime */
-	mdelay(settletime);
-
-	/* Set non-reset bits back what they were */
-	reg.Value = corsave;
-	reg.Action = CS_WRITE;
-	reg.Offset = CISREG_COR;
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	result = pcmcia_access_configuration_register(hw->pdev, &reg);
-#else
-	result = pcmcia_access_configuration_register(
-				      hw->link->handle,
-				      &reg);
-#endif
-	if (result != CS_SUCCESS ) {
-		WLAN_LOG_ERROR(
-			":2: AccessConfigurationRegister(CS_WRITE) failed,"
-			"result=%d.\n", result);
-		result = -EIO;
-		goto done;
-	}
-
-done:
-	DBFEXIT;
-	return result;
-}
-
-#ifdef MODULE
-
-static int __init prism2cs_init(void)
-{
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,68))
-	servinfo_t	serv;
-#endif
-
-	DBFENTER;
-
-        WLAN_LOG_NOTICE("%s Loaded\n", version);
-        WLAN_LOG_NOTICE("dev_info is: %s\n", dev_info);
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,68))
-	pcmcia_get_card_services_info(&serv);
-	if ( serv.Revision != CS_RELEASE_CODE )
-	{
-		printk(KERN_NOTICE"%s: CardServices release does not match!\n", dev_info);
-		return -1;
-	}
-
-	/* This call will result in a call to prism2sta_attach */
-	/*   and eventually prism2sta_detach */
-	register_pccard_driver( &dev_info, &prism2sta_attach, &prism2sta_detach);
-#else
-	pcmcia_register_driver(&prism2_cs_driver);
-#endif
-
-	DBFEXIT;
-	return 0;
-}
-
-static void __exit prism2cs_cleanup(void)
-{
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,68))
-        dev_link_t *link = dev_list;
-        dev_link_t *nlink;
-        DBFENTER;
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,10) )
-	for (link=dev_list; link != NULL; link = nlink) {
-		nlink = link->next;
-		if ( link->state & DEV_CONFIG ) {
-			prism2sta_release((u_long)link);
-		}
-		prism2sta_detach(link); /* remember detach() frees link */
-	}
-#endif
-
-	unregister_pccard_driver( &dev_info);
-#else
-	pcmcia_unregister_driver(&prism2_cs_driver);
-#endif
-
-        printk(KERN_NOTICE "%s Unloaded\n", version);
-
-        DBFEXIT;
-        return;
-}
-
-module_init(prism2cs_init);
-module_exit(prism2cs_cleanup);
-
-#endif // MODULE
-
diff --git a/drivers/staging/wlan-ng/prism2_pci.c b/drivers/staging/wlan-ng/prism2_pci.c
deleted file mode 100644
index afe32dfbf6b1..000000000000
--- a/drivers/staging/wlan-ng/prism2_pci.c
+++ /dev/null
@@ -1,332 +0,0 @@
-#define WLAN_HOSTIF WLAN_PCI
-#include "hfa384x.c"
-#include "prism2mgmt.c"
-#include "prism2mib.c"
-#include "prism2sta.c"
-
-#define PCI_SIZE		0x1000		/* Memory size - 4K bytes */
-
-/* ISL3874A 11Mb/s WLAN controller */
-#define PCIVENDOR_INTERSIL	0x1260UL
-#define PCIDEVICE_ISL3874	0x3873UL /* [MSM] yeah I know...the ID says
-					    3873. Trust me, it's a 3874. */
-
-/* Samsung SWL-2210P 11Mb/s WLAN controller (uses ISL3874A) */
-#define PCIVENDOR_SAMSUNG      0x167dUL
-#define PCIDEVICE_SWL_2210P    0xa000UL
-
-#define PCIVENDOR_NETGEAR      0x1385UL /* for MA311 */
-
-/* PCI Class & Sub-Class code, Network-'Other controller' */
-#define PCI_CLASS_NETWORK_OTHERS 0x280
-
-
-/*----------------------------------------------------------------
-* prism2sta_probe_pci
-*
-* Probe routine called when a PCI device w/ matching ID is found.
-* The ISL3874 implementation uses the following map:
-*   BAR0: Prism2.x registers memory mapped, size=4k
-* Here's the sequence:
-*   - Allocate the PCI resources.
-*   - Read the PCMCIA attribute memory to make sure we have a WLAN card
-*   - Reset the MAC
-*   - Initialize the netdev and wlan data
-*   - Initialize the MAC
-*
-* Arguments:
-*	pdev		ptr to pci device structure containing info about
-*			pci configuration.
-*	id		ptr to the device id entry that matched this device.
-*
-* Returns:
-*	zero		- success
-*	negative	- failed
-*
-* Side effects:
-*
-*
-* Call context:
-*	process thread
-*
-----------------------------------------------------------------*/
-static int __devinit
-prism2sta_probe_pci(
-	struct pci_dev *pdev,
-	const struct pci_device_id *id)
-{
-	int		result;
-	phys_t		phymem = 0;
-	void		__iomem *mem = NULL;
-        wlandevice_t    *wlandev = NULL;
-	hfa384x_t	*hw = NULL;
-
-	DBFENTER;
-
-	/* Enable the pci device */
-	if (pci_enable_device(pdev)) {
-		WLAN_LOG_ERROR("%s: pci_enable_device() failed.\n", dev_info);
-		result = -EIO;
-		goto fail;
-	}
-
-	/* Figure out our resources */
-	phymem = pci_resource_start(pdev, 0);
-
-        if (!request_mem_region(phymem, pci_resource_len(pdev, 0), "Prism2")) {
-		printk(KERN_ERR "prism2: Cannot reserve PCI memory region\n");
-		result = -EIO;
-		goto fail;
-        }
-
-	mem = ioremap(phymem, PCI_SIZE);
-	if ( mem == 0 ) {
-		WLAN_LOG_ERROR("%s: ioremap() failed.\n", dev_info);
-		result = -EIO;
-		goto fail;
-	}
-
-	/* Log the device */
-        WLAN_LOG_INFO("A Prism2.5 PCI device found, "
-		"phymem:0x%llx, irq:%d, mem:0x%p\n",
-		(unsigned long long)phymem, pdev->irq, mem);
-
-	if ((wlandev = create_wlan()) == NULL) {
-		WLAN_LOG_ERROR("%s: Memory allocation failure.\n", dev_info);
-		result = -EIO;
-		goto fail;
-	}
-	hw = wlandev->priv;
-
-	if ( wlan_setup(wlandev) != 0 ) {
-		WLAN_LOG_ERROR("%s: wlan_setup() failed.\n", dev_info);
-		result = -EIO;
-		goto fail;
-	}
-
-	/* Setup netdevice's ability to report resources
-	 * Note: the netdevice was allocated by wlan_setup()
-	 */
-        wlandev->netdev->irq = pdev->irq;
-        wlandev->netdev->mem_start = (unsigned long) mem;
-        wlandev->netdev->mem_end = wlandev->netdev->mem_start +
-		pci_resource_len(pdev, 0);
-
-	/* Initialize the hw data */
-        hfa384x_create(hw, wlandev->netdev->irq, 0, mem);
-	hw->wlandev = wlandev;
-
-	/* Register the wlandev, this gets us a name and registers the
-	 * linux netdevice.
-	 */
-	SET_MODULE_OWNER(wlandev->netdev);
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0))
-       SET_NETDEV_DEV(wlandev->netdev, &(pdev->dev));
-#endif
-        if ( register_wlandev(wlandev) != 0 ) {
-		WLAN_LOG_ERROR("%s: register_wlandev() failed.\n", dev_info);
-		result = -EIO;
-		goto fail;
-        }
-
-#if 0
-	/* TODO: Move this and an irq test into an hfa384x_testif() routine.
-	 */
-	outw(PRISM2STA_MAGIC, HFA384x_SWSUPPORT(wlandev->netdev->base_addr));
-	reg=inw( HFA384x_SWSUPPORT(wlandev->netdev->base_addr));
-	if ( reg != PRISM2STA_MAGIC ) {
-		WLAN_LOG_ERROR("MAC register access test failed!\n");
-		result = -EIO;
-		goto fail;
-	}
-#endif
-
-	/* Do a chip-level reset on the MAC */
-	if (prism2_doreset) {
-		result = hfa384x_corereset(hw,
-				prism2_reset_holdtime,
-				prism2_reset_settletime, 0);
-		if (result != 0) {
-			WLAN_LOG_ERROR(
-				"%s: hfa384x_corereset() failed.\n",
-				dev_info);
-			unregister_wlandev(wlandev);
-			hfa384x_destroy(hw);
-			result = -EIO;
-			goto fail;
-		}
-	}
-
-        pci_set_drvdata(pdev, wlandev);
-
-	/* Shouldn't actually hook up the IRQ until we
-	 * _know_ things are alright.  A test routine would help.
-	 */
-       	request_irq(wlandev->netdev->irq, hfa384x_interrupt,
-		SA_SHIRQ, wlandev->name, wlandev);
-
-	wlandev->msdstate = WLAN_MSD_HWPRESENT;
-
-	result = 0;
-	goto done;
-
- fail:
-	pci_set_drvdata(pdev, NULL);
-	if (wlandev)	kfree(wlandev);
-	if (hw)		kfree(hw);
-        if (mem)        iounmap(mem);
-	pci_release_regions(pdev);
-        pci_disable_device(pdev);
-
- done:
-	DBFEXIT;
-	return result;
-}
-
-static void __devexit prism2sta_remove_pci(struct pci_dev *pdev)
-{
-       	wlandevice_t		*wlandev;
-	hfa384x_t	*hw;
-
-	wlandev = (wlandevice_t *) pci_get_drvdata(pdev);
-	hw = wlandev->priv;
-
-	p80211netdev_hwremoved(wlandev);
-
-	/* reset hardware */
-	prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
-
-        if (pdev->irq)
-		free_irq(pdev->irq, wlandev);
-
-	unregister_wlandev(wlandev);
-
-	/* free local stuff */
-	if (hw) {
-		hfa384x_destroy(hw);
-		kfree(hw);
-	}
-
-	iounmap((void __iomem *)wlandev->netdev->mem_start);
-	wlan_unsetup(wlandev);
-
-	pci_release_regions(pdev);
-        pci_disable_device(pdev);
-	pci_set_drvdata(pdev, NULL);
-
-	kfree(wlandev);
-}
-
-
-static struct pci_device_id pci_id_tbl[] = {
-	{
-		PCIVENDOR_INTERSIL, PCIDEVICE_ISL3874,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Intersil Prism2.5 ISL3874 11Mb/s WLAN Controller"
-	},
-	{
-		PCIVENDOR_INTERSIL, 0x3872,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Intersil Prism2.5 ISL3872 11Mb/s WLAN Controller"
-	},
-        {
-               PCIVENDOR_SAMSUNG, PCIDEVICE_SWL_2210P,
-               PCI_ANY_ID, PCI_ANY_ID,
-               0, 0,
-               /* Driver data, we just put the name here */
-               (unsigned long)"Samsung MagicLAN SWL-2210P 11Mb/s WLAN Controller"
-	},
-	{ /* for NetGear MA311 */
-		PCIVENDOR_NETGEAR, 0x3872,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Netgear MA311 WLAN Controller"
-	},
-	{
-		0, 0, 0, 0, 0, 0, 0
-	}
-};
-
-MODULE_DEVICE_TABLE(pci, pci_id_tbl);
-
-/* Function declared here because of ptr reference below */
-static int  __devinit prism2sta_probe_pci(struct pci_dev *pdev,
-				const struct pci_device_id *id);
-static void  __devexit prism2sta_remove_pci(struct pci_dev *pdev);
-
-static struct pci_driver prism2_pci_drv_id = {
-        .name = "prism2_pci",
-        .id_table = pci_id_tbl,
-        .probe = prism2sta_probe_pci,
-        .remove = prism2sta_remove_pci,
-#ifdef CONFIG_PM
-        .suspend = prism2sta_suspend_pci,
-        .resume = prism2sta_resume_pci,
-#endif
-};
-
-#ifdef MODULE
-
-static int __init prism2pci_init(void)
-{
-        WLAN_LOG_NOTICE("%s Loaded\n", version);
-	return pci_module_init(&prism2_pci_drv_id);
-};
-
-static void __exit prism2pci_cleanup(void)
-{
-	pci_unregister_driver(&prism2_pci_drv_id);
-};
-
-module_init(prism2pci_init);
-module_exit(prism2pci_cleanup);
-
-#endif
-
-int hfa384x_corereset(hfa384x_t *hw, int holdtime, int settletime, int genesis)
-{
-	int		result = 0;
-	unsigned long	timeout;
-	UINT16	reg;
-	DBFENTER;
-
-	/* Assert reset and wait awhile
-	 * (note: these delays are _really_ long, but they appear to be
-	 *        necessary.)
-	 */
-	hfa384x_setreg(hw, 0xc5, HFA384x_PCICOR);
-	timeout = jiffies + HZ/4;
-	while(time_before(jiffies, timeout)) udelay(5);
-
-	if (genesis) {
-		hfa384x_setreg(hw, genesis, HFA384x_PCIHCR);
-		timeout = jiffies + HZ/4;
-		while(time_before(jiffies, timeout)) udelay(5);
-	}
-
-	/* Clear the reset and wait some more
-	 */
-	hfa384x_setreg(hw, 0x45, HFA384x_PCICOR);
-	timeout = jiffies + HZ/2;
-	while(time_before(jiffies, timeout)) udelay(5);
-
-	/* Wait for f/w to complete initialization (CMD:BUSY == 0)
-	 */
-	timeout = jiffies + 2*HZ;
-	reg = hfa384x_getreg(hw, HFA384x_CMD);
-	while ( HFA384x_CMD_ISBUSY(reg) && time_before( jiffies, timeout) ) {
-		reg = hfa384x_getreg(hw, HFA384x_CMD);
-		udelay(10);
-	}
-	if (HFA384x_CMD_ISBUSY(reg)) {
-		WLAN_LOG_WARNING("corereset: Timed out waiting for cmd register.\n");
-		result=1;
-	}
-	DBFEXIT;
-	return result;
-}
diff --git a/drivers/staging/wlan-ng/prism2_plx.c b/drivers/staging/wlan-ng/prism2_plx.c
deleted file mode 100644
index 320443f37a8f..000000000000
--- a/drivers/staging/wlan-ng/prism2_plx.c
+++ /dev/null
@@ -1,472 +0,0 @@
-#define WLAN_HOSTIF WLAN_PLX
-#include "hfa384x.c"
-#include "prism2mgmt.c"
-#include "prism2mib.c"
-#include "prism2sta.c"
-
-#define PLX_ATTR_SIZE	0x1000	/* Attribute memory size - 4K bytes */
-#define COR_OFFSET	0x3e0	/* COR attribute offset of Prism2 PC card */
-#define COR_VALUE	0x41	/* Enable PC card with irq in level trigger */
-#define PLX_INTCSR	0x4c	/* Interrupt Control and Status Register */
-#define PLX_INTCSR_INTEN (1<<6) /* Interrupt Enable bit */
-#define PLX_MIN_ATTR_LEN 512    /* at least 2 x 256 is needed for CIS */
-
-/* 3Com 3CRW777A (PLX) board ID */
-#define PCIVENDOR_3COM       0x10B7
-#define PCIDEVICE_AIRCONNECT 0x7770
-
-/* Eumitcom PCI WL11000 PCI Adapter (PLX) board device+vendor ID */
-#define PCIVENDOR_EUMITCOM	0x1638UL
-#define PCIDEVICE_WL11000	0x1100UL
-
-/* Global Sun Tech GL24110P PCI Adapter (PLX) board device+vendor ID */
-#define PCIVENDOR_GLOBALSUN	0x16abUL
-#define PCIDEVICE_GL24110P	0x1101UL
-#define PCIDEVICE_GL24110P_ALT	0x1102UL
-
-/* Netgear MA301 PCI Adapter (PLX) board device+vendor ID */
-#define PCIVENDOR_NETGEAR	0x1385UL
-#define PCIDEVICE_MA301		0x4100UL
-
-/* US Robotics USR2410 PCI Adapter (PLX) board device+vendor ID */
-#define	PCIVENDOR_USROBOTICS    0x16ecUL
-#define PCIDEVICE_USR2410       0x3685UL
-
-/* Linksys WPC11 card with the WDT11 adapter (PLX) board device+vendor ID */
-#define	PCIVENDOR_Linksys       0x16abUL
-#define PCIDEVICE_Wpc11Wdt11    0x1102UL
-
-/* National Datacomm Corp SOHOware Netblaster II PCI */
-#define PCIVENDOR_NDC	        0x15e8UL
-#define PCIDEVICE_NCP130_PLX 	0x0130UL
-#define PCIDEVICE_NCP130_ASIC	0x0131UL
-
-/* NDC NCP130_PLX is also sold by Corega. Their name is CGWLPCIA11 */
-#define PCIVENDOR_COREGA       PCIVENDOR_NDC
-#define PCIDEVICE_CGWLPCIA11   PCIDEVICE_NCP130_PLX
-
-/* PCI Class & Sub-Class code, Network-'Other controller' */
-#define PCI_CLASS_NETWORK_OTHERS 0x280
-
-/*----------------------------------------------------------------
-* prism2sta_probe_plx
-*
-* Probe routine called when a PCI device w/ matching ID is found.
-* This PLX implementation uses the following map:
-*   BAR0: Unused
-*   BAR1: ????
-*   BAR2: PCMCIA attribute memory
-*   BAR3: PCMCIA i/o space
-* Here's the sequence:
-*   - Allocate the PCI resources.
-*   - Read the PCMCIA attribute memory to make sure we have a WLAN card
-*   - Reset the MAC using the PCMCIA COR
-*   - Initialize the netdev and wlan data
-*   - Initialize the MAC
-*
-* Arguments:
-*	pdev		ptr to pci device structure containing info about
-*			pci configuration.
-*	id		ptr to the device id entry that matched this device.
-*
-* Returns:
-*	zero		- success
-*	negative	- failed
-*
-* Side effects:
-*
-*
-* Call context:
-*	process thread
-*
-----------------------------------------------------------------*/
-static int __devinit
-prism2sta_probe_plx(
-	struct pci_dev			*pdev,
-	const struct pci_device_id	*id)
-{
-	int		result;
-        phys_t	pccard_ioaddr;
-	phys_t  pccard_attr_mem;
-        unsigned int    pccard_attr_len;
-	void __iomem *attr_mem = NULL;
-	UINT32		plx_addr;
-        wlandevice_t    *wlandev = NULL;
-	hfa384x_t	*hw = NULL;
-	int		reg;
-        u32		regic;
-
-	if (pci_enable_device(pdev))
-		return -EIO;
-
-	/* TMC7160 boards are special */
-	if ((pdev->vendor == PCIVENDOR_NDC) &&
-	    (pdev->device == PCIDEVICE_NCP130_ASIC)) {
-		unsigned long delay;
-
-		pccard_attr_mem = 0;
-		pccard_ioaddr = pci_resource_start(pdev, 1);
-
-		outb(0x45, pccard_ioaddr);
-		delay = jiffies + 1*HZ;
-		while (time_before(jiffies, delay));
-
-		if (inb(pccard_ioaddr) != 0x45) {
-			WLAN_LOG_ERROR("Initialize the TMC7160 failed. (0x%x)\n", inb(pccard_ioaddr));
-			return -EIO;
-		}
-
-		pccard_ioaddr = pci_resource_start(pdev, 2);
-		prism2_doreset = 0;
-
-		WLAN_LOG_INFO("NDC NCP130 with TMC716(ASIC) PCI interface device found at io:0x%x, irq:%d\n", pccard_ioaddr, pdev->irq);
-		goto init;
-	}
-
-	/* Collect the resource requirements */
-	pccard_attr_mem = pci_resource_start(pdev, 2);
-	pccard_attr_len = pci_resource_len(pdev, 2);
-        if (pccard_attr_len < PLX_MIN_ATTR_LEN)
-		return -EIO;
-
-	pccard_ioaddr = pci_resource_start(pdev, 3);
-
-	/* bjoern: We need to tell the card to enable interrupts, in
-	 * case the serial eprom didn't do this already. See the
-	 * PLX9052 data book, p8-1 and 8-24 for reference.
-	 * [MSM]: This bit of code came from the orinoco_cs driver.
-	 */
-	plx_addr = pci_resource_start(pdev, 1);
-
-	regic = 0;
-	regic = inl(plx_addr+PLX_INTCSR);
-	if(regic & PLX_INTCSR_INTEN) {
-		WLAN_LOG_DEBUG(1,
-			"%s: Local Interrupt already enabled\n", dev_info);
-	} else {
-		regic |= PLX_INTCSR_INTEN;
-		outl(regic, plx_addr+PLX_INTCSR);
-		regic = inl(plx_addr+PLX_INTCSR);
-		if(!(regic & PLX_INTCSR_INTEN)) {
-			WLAN_LOG_ERROR(
-				"%s: Couldn't enable Local Interrupts\n",
-				dev_info);
-			return -EIO;
-		}
-	}
-
-	/* These assignments are here in case of future mappings for
-	 * io space and irq that might be similar to ioremap
-	 */
-        if (!request_mem_region(pccard_attr_mem, pci_resource_len(pdev, 2), "Prism2")) {
-		WLAN_LOG_ERROR("%s: Couldn't reserve PCI memory region\n", dev_info);
-		return -EIO;
-        }
-
-	attr_mem = ioremap(pccard_attr_mem, pccard_attr_len);
-
-	WLAN_LOG_INFO("A PLX PCI/PCMCIA interface device found, "
-		"phymem:0x%llx, phyio=0x%x, irq:%d, "
-		"mem: 0x%lx\n",
-		(unsigned long long)pccard_attr_mem, pccard_ioaddr, pdev->irq,
-		(unsigned long)attr_mem);
-
-	/* Verify whether PC card is present.
-	 * [MSM] This needs improvement, the right thing to do is
-	 * probably to walk the CIS looking for the vendor and product
-	 * IDs.  It would be nice if this could be tied in with the
-	 * etc/pcmcia/wlan-ng.conf file.  Any volunteers?  ;-)
-	 */
-	if (
-	readb(attr_mem + 0) != 0x01 || readb(attr_mem + 2) != 0x03 ||
-	readb(attr_mem + 4) != 0x00 || readb(attr_mem + 6) != 0x00 ||
-	readb(attr_mem + 8) != 0xFF || readb(attr_mem + 10) != 0x17 ||
-	readb(attr_mem + 12) != 0x04 || readb(attr_mem + 14) != 0x67) {
-		WLAN_LOG_ERROR("Prism2 PC card CIS is invalid.\n");
-		return -EIO;
-        }
-        WLAN_LOG_INFO("A PCMCIA WLAN adapter was found.\n");
-
-        /* Write COR to enable PC card */
-	writeb(COR_VALUE, attr_mem + COR_OFFSET);
-	reg = readb(attr_mem + COR_OFFSET);
-
- init:
-
-	/*
-	 * Now do everything the same as a PCI device
-	 * [MSM] TODO: We could probably factor this out of pcmcia/pci/plx
-	 * and perhaps usb.  Perhaps a task for another day.......
-	 */
-
-	if ((wlandev = create_wlan()) == NULL) {
-		WLAN_LOG_ERROR("%s: Memory allocation failure.\n", dev_info);
-		result = -EIO;
-		goto failed;
-	}
-
-	hw = wlandev->priv;
-
-	if ( wlan_setup(wlandev) != 0 ) {
-		WLAN_LOG_ERROR("%s: wlan_setup() failed.\n", dev_info);
-		result = -EIO;
-		goto failed;
-	}
-
-	/* Setup netdevice's ability to report resources
-	 * Note: the netdevice was allocated by wlan_setup()
-	 */
-        wlandev->netdev->irq = pdev->irq;
-        wlandev->netdev->base_addr = pccard_ioaddr;
-        wlandev->netdev->mem_start = (unsigned long)attr_mem;
-        wlandev->netdev->mem_end = (unsigned long)attr_mem + pci_resource_len(pdev, 0);
-
-	/* Initialize the hw data */
-        hfa384x_create(hw, wlandev->netdev->irq, pccard_ioaddr, attr_mem);
-	hw->wlandev = wlandev;
-
-	/* Register the wlandev, this gets us a name and registers the
-	 * linux netdevice.
-	 */
-	SET_MODULE_OWNER(wlandev->netdev);
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0))
-       SET_NETDEV_DEV(wlandev->netdev, &(pdev->dev));
-#endif
-        if ( register_wlandev(wlandev) != 0 ) {
-		WLAN_LOG_ERROR("%s: register_wlandev() failed.\n", dev_info);
-		result = -EIO;
-		goto failed;
-        }
-
-#if 0
-	/* TODO: Move this and an irq test into an hfa384x_testif() routine.
-	 */
-	outw(PRISM2STA_MAGIC, HFA384x_SWSUPPORT(wlandev->netdev->base_addr));
-	reg=inw( HFA384x_SWSUPPORT(wlandev->netdev->base_addr));
-	if ( reg != PRISM2STA_MAGIC ) {
-		WLAN_LOG_ERROR("MAC register access test failed!\n");
- 		result = -EIO;
-		goto failed;
-	}
-#endif
-
-	/* Do a chip-level reset on the MAC */
-	if (prism2_doreset) {
-		result = hfa384x_corereset(hw,
-				prism2_reset_holdtime,
-				prism2_reset_settletime, 0);
-		if (result != 0) {
-			unregister_wlandev(wlandev);
-			hfa384x_destroy(hw);
-			WLAN_LOG_ERROR(
-				"%s: hfa384x_corereset() failed.\n",
-				dev_info);
-			result = -EIO;
-			goto failed;
-		}
-	}
-
-	pci_set_drvdata(pdev, wlandev);
-
-	/* Shouldn't actually hook up the IRQ until we
-	 * _know_ things are alright.  A test routine would help.
-	 */
-       	request_irq(wlandev->netdev->irq, hfa384x_interrupt,
-		SA_SHIRQ, wlandev->name, wlandev);
-
-	wlandev->msdstate = WLAN_MSD_HWPRESENT;
-
-	result = 0;
-
-	goto done;
-
- failed:
-
-	pci_set_drvdata(pdev, NULL);
-	if (wlandev)	kfree(wlandev);
-	if (hw)		kfree(hw);
-        if (attr_mem)        iounmap(attr_mem);
-	pci_release_regions(pdev);
-        pci_disable_device(pdev);
-
- done:
-	DBFEXIT;
-        return result;
-}
-
-static void __devexit prism2sta_remove_plx(struct pci_dev *pdev)
-{
-       	wlandevice_t		*wlandev;
-	hfa384x_t               *hw;
-
-	wlandev = (wlandevice_t *) pci_get_drvdata(pdev);
-	hw = wlandev->priv;
-
-	p80211netdev_hwremoved(wlandev);
-
-	/* reset hardware */
-	prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
-
-        if (pdev->irq)
-		free_irq(pdev->irq, wlandev);
-
-	unregister_wlandev(wlandev);
-
-	/* free local stuff */
-	if (hw) {
-		hfa384x_destroy(hw);
-		kfree(hw);
-	}
-
-	iounmap((void __iomem *)wlandev->netdev->mem_start);
-	wlan_unsetup(wlandev);
-
-	pci_release_regions(pdev);
-        pci_disable_device(pdev);
-	pci_set_drvdata(pdev, NULL);
-
-	kfree(wlandev);
-}
-
-static struct pci_device_id plx_id_tbl[] = {
-	{
-		PCIVENDOR_EUMITCOM, PCIDEVICE_WL11000,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Eumitcom WL11000 PCI(PLX) card"
-	},
-	{
-		PCIVENDOR_GLOBALSUN, PCIDEVICE_GL24110P,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Global Sun Tech GL24110P PCI(PLX) card"
-	},
-	{
-		PCIVENDOR_GLOBALSUN, PCIDEVICE_GL24110P_ALT,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Global Sun Tech GL24110P PCI(PLX) card"
-	},
-	{
-		PCIVENDOR_NETGEAR, PCIDEVICE_MA301,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Global Sun Tech GL24110P PCI(PLX) card"
-	},
-	{
-		PCIVENDOR_USROBOTICS, PCIDEVICE_USR2410,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"US Robotics USR2410 PCI(PLX) card"
-	},
-	{
-		PCIVENDOR_Linksys, PCIDEVICE_Wpc11Wdt11,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Linksys WPC11 with WDT11 PCI(PLX) adapter"
-	},
-	{
-	        PCIVENDOR_NDC, PCIDEVICE_NCP130_PLX,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"NDC Netblaster II PCI(PLX)"
-	},
-	{
-	        PCIVENDOR_NDC, PCIDEVICE_NCP130_ASIC,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"NDC Netblaster II PCI(TMC7160)"
-	},
-	{
-		PCIVENDOR_3COM, PCIDEVICE_AIRCONNECT,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"3Com AirConnect PCI 802.11b 11Mb/s WLAN Controller"
-	},
-	{
-		0, 0, 0, 0, 0, 0, 0
-	}
-};
-
-MODULE_DEVICE_TABLE(pci, plx_id_tbl);
-
-/* Function declared here because of ptr reference below */
-static int __devinit prism2sta_probe_plx(struct pci_dev *pdev,
-					 const struct pci_device_id *);
-static void __devexit prism2sta_remove_plx(struct pci_dev *pdev);
-
-static struct pci_driver prism2_plx_drv_id = {
-        .name = "prism2_plx",
-        .id_table = plx_id_tbl,
-        .probe = prism2sta_probe_plx,
-        .remove = prism2sta_remove_plx,
-#ifdef CONFIG_PM
-        .suspend = prism2sta_suspend_pci,
-        .resume = prism2sta_resume_pci,
-#endif
-};
-
-#ifdef MODULE
-
-static int __init prism2plx_init(void)
-{
-        WLAN_LOG_NOTICE("%s Loaded\n", version);
-	return pci_module_init(&prism2_plx_drv_id);
-};
-
-static void __exit prism2plx_cleanup(void)
-{
-	pci_unregister_driver(&prism2_plx_drv_id);
-};
-
-module_init(prism2plx_init);
-module_exit(prism2plx_cleanup);
-
-#endif // MODULE
-
-
-int hfa384x_corereset(hfa384x_t *hw, int holdtime, int settletime, int genesis)
-{
-	int		result = 0;
-
-#define COR_OFFSET	0x3e0	/* COR attribute offset of Prism2 PC card */
-#define COR_VALUE	0x41	/* Enable PC card with irq in level trigger */
-
-#define HCR_OFFSET	0x3e2	/* HCR attribute offset of Prism2 PC card */
-
-	UINT8		corsave;
-	DBFENTER;
-
-	WLAN_LOG_DEBUG(3, "Doing reset via direct COR access.\n");
-
-	/* Collect COR */
-	corsave = readb(hw->membase + COR_OFFSET);
-	/* Write reset bit (BIT7) */
-	writeb(corsave | BIT7, hw->membase + COR_OFFSET);
-	/* Hold for holdtime */
-	mdelay(holdtime);
-
-	if (genesis) {
-		writeb(genesis, hw->membase + HCR_OFFSET);
-		/* Hold for holdtime */
-		mdelay(holdtime);
-	}
-
-	/* Clear reset bit */
-	writeb(corsave & ~BIT7, hw->membase + COR_OFFSET);
-	/* Wait for settletime */
-	mdelay(settletime);
-	/* Set non-reset bits back what they were */
-	writeb(corsave, hw->membase + COR_OFFSET);
-	DBFEXIT;
-	return result;
-}
diff --git a/drivers/staging/wlan-ng/prism2mgmt.c b/drivers/staging/wlan-ng/prism2mgmt.c
index c975025b6ae0..f1727ba6ec6f 100644
--- a/drivers/staging/wlan-ng/prism2mgmt.c
+++ b/drivers/staging/wlan-ng/prism2mgmt.c
@@ -61,10 +61,6 @@
 /* System Includes */
 #define WLAN_DBVAR	prism2_debug
 
-#include "version.h"
-
-
-#include <linux/version.h>
 
 #include <linux/if_arp.h>
 #include <linux/module.h>
@@ -79,19 +75,7 @@
 #include <asm/io.h>
 #include <asm/byteorder.h>
 #include <linux/random.h>
-
-#if (WLAN_HOSTIF == WLAN_USB)
 #include <linux/usb.h>
-#endif
-
-#if (WLAN_HOSTIF == WLAN_PCMCIA)
-#include <pcmcia/version.h>
-#include <pcmcia/cs_types.h>
-#include <pcmcia/cs.h>
-#include <pcmcia/cistpl.h>
-#include <pcmcia/ds.h>
-#include <pcmcia/cisreg.h>
-#endif
 
 #include "wlan_compat.h"
 
@@ -109,89 +93,12 @@
 #include "hfa384x.h"
 #include "prism2mgmt.h"
 
-/*================================================================*/
-/* Local Constants */
-
-
-/*================================================================*/
-/* Local Macros */
-
 /* Converts 802.11 format rate specifications to prism2 */
 #define p80211rate_to_p2bit(n)	((((n)&~BIT7) == 2) ? BIT0 : \
 				 (((n)&~BIT7) == 4) ? BIT1 : \
 				 (((n)&~BIT7) == 11) ? BIT2 : \
 				 (((n)&~BIT7) == 22) ? BIT3 : 0)
 
-/*================================================================*/
-/* Local Types */
-
-
-/*================================================================*/
-/* Local Static Definitions */
-
-
-/*================================================================*/
-/* Local Function Declarations */
-
-
-/*================================================================*/
-/* Function Definitions */
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_powermgmt
-*
-* Set the power management state of this station's MAC.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_powermgmt(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_dot11req_powermgmt_t	*msg = msgp;
-
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/*
-		 * Set CNFPMENABLED (on or off)
-		 * Set CNFMULTICASTRX (if PM on, otherwise clear)
-		 * Spout a notice stating that SleepDuration and
-		 * HoldoverDuration and PMEPS also have an impact.
-		 */
-		/* Powermgmt is currently unsupported for STA */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	} else {
-
-		/*** ACCESS POINT ***/
-
-		/* Powermgmt is never supported for AP */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
 /*----------------------------------------------------------------
 * prism2mgmt_scan
 *
@@ -221,7 +128,7 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 	int 			result = 0;
 	hfa384x_t		*hw = wlandev->priv;
 	p80211msg_dot11req_scan_t	*msg = msgp;
-        UINT16                  roamingmode, word;
+        u16                  roamingmode, word;
 	int                     i, timeout;
 	int                     istmpenable = 0;
 
@@ -229,13 +136,6 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 
 	DBFENTER;
 
-        if (hw->ap) {
-                WLAN_LOG_ERROR("Prism2 in AP mode cannot perform scans.\n");
-                result = 1;
-                msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-                goto exit;
-        }
-
         /* gatekeeper check */
         if (HFA384x_FIRMWARE_VERSION(hw->ident_sta_fw.major,
                                      hw->ident_sta_fw.minor,
@@ -296,7 +196,7 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
         /* set up the channel list */
         word = 0;
         for (i = 0; i < msg->channellist.data.len; i++) {
-                UINT8 channel = msg->channellist.data.data[i];
+                u8 channel = msg->channellist.data.data[i];
                 if (channel > 14) continue;
                 /* channel 1 is BIT0 ... channel 14 is BIT13 */
                 word |= (1 << (channel-1));
@@ -317,7 +217,7 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 		goto exit;
 	}
 	if (word == HFA384x_PORTSTATUS_DISABLED) {
-		UINT16 wordbuf[17];
+		u16 wordbuf[17];
 
 		result = hfa384x_drvr_setconfig16(hw,
 			HFA384x_RID_CNFROAMINGMODE,
@@ -480,12 +380,6 @@ int prism2mgmt_scan_results(wlandevice_t *wlandev, void *msgp)
 
 	req->resultcode.status = P80211ENUM_msgitem_status_data_ok;
 
-	if (hw->ap) {
-		result = 1;
-		req->resultcode.data = P80211ENUM_resultcode_not_supported;
-		goto exit;
-	}
-
 	if (! hw->scanresults) {
 		WLAN_LOG_ERROR("dot11req_scan_results can only be used after a successful dot11req_scan.\n");
 		result = 2;
@@ -612,567 +506,6 @@ int prism2mgmt_scan_results(wlandevice_t *wlandev, void *msgp)
 	return result;
 }
 
-
-/*----------------------------------------------------------------
-* prism2mgmt_join
-*
-* Join a BSS whose BSS description was previously obtained with
-* a scan.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_join(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_dot11req_join_t	*msg = msgp;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* TODO: Implement after scan */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	} else {
-
-		/*** ACCESS POINT ***/
-
-		/* Never supported by APs */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_p2_join
-*
-* Join a specific BSS
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_p2_join(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_join_t	*msg = msgp;
-	UINT16			reg;
-	p80211pstrd_t		*pstr;
-	UINT8			bytebuf[256];
-	hfa384x_bytestr_t	*p2bytestr = (hfa384x_bytestr_t*)bytebuf;
-        hfa384x_JoinRequest_data_t	joinreq;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		wlandev->macmode = WLAN_MACMODE_NONE;
-
-		/*** STATION ***/
-		/* Set the PortType */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-
-		/* ess port */
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFPORTTYPE, 1);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set Port Type\n");
-			goto failed;
-		}
-
-		/* Set the auth type */
-		if ( msg->authtype.data == P80211ENUM_authalg_sharedkey ) {
-			reg = HFA384x_CNFAUTHENTICATION_SHAREDKEY;
-		} else {
-			reg = HFA384x_CNFAUTHENTICATION_OPENSYSTEM;
-		}
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFAUTHENTICATION, reg);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set Authentication\n");
-			goto failed;
-		}
-
-		/* Turn off all roaming */
-		hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFROAMINGMODE, 3);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to Turn off Roaming\n");
-			goto failed;
-		}
-
-		/* Basic rates */
-                reg = 0;
-		if ( msg->basicrate1.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg = p80211rate_to_p2bit(msg->basicrate1.data);
-		}
-		if ( msg->basicrate2.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->basicrate2.data);
-		}
-		if ( msg->basicrate3.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->basicrate3.data);
-		}
-		if ( msg->basicrate4.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->basicrate4.data);
-		}
-		if ( msg->basicrate5.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->basicrate5.data);
-		}
-		if ( msg->basicrate6.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->basicrate6.data);
-		}
-		if ( msg->basicrate7.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->basicrate7.data);
-		}
-		if ( msg->basicrate8.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->basicrate8.data);
-		}
-		if( reg == 0)
-			 reg = 0x03;
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFBASICRATES, reg);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set basicrates=%d.\n", reg);
-			goto failed;
-		}
-
-		/* Operational rates (supprates and txratecontrol) */
-		reg = 0;
-		if ( msg->operationalrate1.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg = p80211rate_to_p2bit(msg->operationalrate1.data);
-		}
-		if ( msg->operationalrate2.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->operationalrate2.data);
-		}
-		if ( msg->operationalrate3.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->operationalrate3.data);
-		}
-		if ( msg->operationalrate4.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->operationalrate4.data);
-		}
-		if ( msg->operationalrate5.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->operationalrate5.data);
-		}
-		if ( msg->operationalrate6.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->operationalrate6.data);
-		}
-		if ( msg->operationalrate7.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->operationalrate7.data);
-		}
-		if ( msg->operationalrate8.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->operationalrate8.data);
-		}
-		if( reg == 0)
-			 reg = 0x0f;
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFSUPPRATES, reg);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set supprates=%d.\n", reg);
-			goto failed;
-		}
-
- 		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_TXRATECNTL, reg);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set txrates=%d.\n", reg);
-			goto failed;
-		}
-
-		/* Set the ssid */
-		memset(bytebuf, 0, 256);
-		pstr = (p80211pstrd_t*)&(msg->ssid.data);
-		prism2mgmt_pstr2bytestr(p2bytestr, pstr);
-		result = hfa384x_drvr_setconfig(
-			hw, HFA384x_RID_CNFDESIREDSSID,
-			bytebuf, HFA384x_RID_CNFDESIREDSSID_LEN);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set SSID\n");
-			goto failed;
-		}
-
-		/* Enable the Port */
-		result = hfa384x_cmd_enable(hw, 0);
-		if ( result ) {
-			WLAN_LOG_ERROR("Enable macport failed, result=%d.\n", result);
-			goto failed;
-		}
-
-		/* Fill in the join request */
-		joinreq.channel = msg->channel.data;
-		memcpy( joinreq.bssid, ((unsigned char *) &msg->bssid.data) + 1, WLAN_BSSID_LEN);
-		hw->joinreq = joinreq;
-		hw->join_ap = 1;
-
-		/* Send the join request */
-		result = hfa384x_drvr_setconfig( hw,
-			HFA384x_RID_JOINREQUEST,
-			&joinreq, HFA384x_RID_JOINREQUEST_LEN);
-                if(result != 0) {
-			WLAN_LOG_ERROR("Join request failed, result=%d.\n", result);
-			goto failed;
-		}
-
-	} else {
-
-		/*** ACCESS POINT ***/
-
-		/* Never supported by APs */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	}
-
-        goto done;
-failed:
-	WLAN_LOG_DEBUG(1, "Failed to set a config option, result=%d\n", result);
-	msg->resultcode.data = P80211ENUM_resultcode_invalid_parameters;
-
-done:
-        result = 0;
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_authenticate
-*
-* Station should be begin an authentication exchange.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_authenticate(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_dot11req_authenticate_t	*msg = msgp;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* TODO: Decide how we're going to handle this one w/ Prism2 */
-		/*       It could be entertaining since Prism2 doesn't have  */
-		/*       an explicit way to control this */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	} else {
-
-		/*** ACCESS POINT ***/
-
-		/* Never supported by APs */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_deauthenticate
-*
-* Send a deauthenticate notification.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_deauthenticate(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_dot11req_deauthenticate_t	*msg = msgp;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* TODO: Decide how we're going to handle this one w/ Prism2 */
-		/*       It could be entertaining since Prism2 doesn't have  */
-		/*       an explicit way to control this */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	} else {
-
-		/*** ACCESS POINT ***/
-		hfa384x_drvr_handover(hw, msg->peerstaaddress.data.data);
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_associate
-*
-* Associate with an ESS.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_associate(wlandevice_t *wlandev, void *msgp)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	int 			result = 0;
-	p80211msg_dot11req_associate_t	*msg = msgp;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-#if 0
-		/* Set the TxRates */
-		reg = 0x000f;
-		hfa384x_drvr_setconfig16(hw, HFA384x_RID_TXRATECNTL, reg);
-#endif
-
-		/* Set the PortType */
-		/* ess port */
-		hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFPORTTYPE, 1);
-
-		/* Enable the Port */
-		hfa384x_drvr_enable(hw, 0);
-
-		/* Set the resultcode */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-
-	} else {
-
-		/*** ACCESS POINT ***/
-
-		/* Never supported on AP */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_reassociate
-*
-* Renew association because of a BSS change.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_reassociate(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_dot11req_reassociate_t	*msg = msgp;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* TODO: Not supported yet...not sure how we're going to do it */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	} else {
-
-		/*** ACCESS POINT ***/
-
-		/* Never supported on AP */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_disassociate
-*
-* Send a disassociation notification.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_disassociate(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_dot11req_disassociate_t	*msg = msgp;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* TODO: Not supported yet...not sure how to do it */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	} else {
-
-		/*** ACCESS POINT ***/
-		hfa384x_drvr_handover(hw, msg->peerstaaddress.data.data);
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_reset
-*
-* Reset the MAC and MSD.  The p80211 layer has it's own handling
-* that should be done before and after this function.
-* Procedure:
-*   - disable system interrupts ??
-*   - disable MAC interrupts
-*   - restore system interrupts
-*   - issue the MAC initialize command
-*   - clear any MSD level state (including timers, queued events,
-*     etc.).  Note that if we're removing timer'd/queue events, we may
-*     need to have remained in the system interrupt disabled state.
-*     We should be left in the same state that we're in following
-*     driver initialization.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer, MAY BE NULL! for a driver local
-*			call.
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread, commonly wlanctl, but might be rmmod/pci_close.
-----------------------------------------------------------------*/
-int prism2mgmt_reset(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_dot11req_reset_t	*msg = msgp;
-	DBFENTER;
-
-	/*
-	 * This is supported on both AP and STA and it's not allowed
-	 * to fail.
-	 */
-	if ( msgp ) {
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-		WLAN_LOG_INFO("dot11req_reset: the macaddress and "
-			"setdefaultmib arguments are currently unsupported.\n");
-	}
-
-	/*
-	 * If we got this far, the MSD must be in the MSDRUNNING state
-	 * therefore, we must stop and then restart the hw/MAC combo.
-	 */
-	hfa384x_drvr_stop(hw);
-	result = hfa384x_drvr_start(hw);
-	if (result != 0) {
-		WLAN_LOG_ERROR("dot11req_reset: Initialize command failed,"
-				" bad things will happen from here.\n");
-		return 0;
-	}
-
-	DBFEXIT;
-	return 0;
-}
-
-
 /*----------------------------------------------------------------
 * prism2mgmt_start
 *
@@ -1199,10 +532,9 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 	p80211msg_dot11req_start_t	*msg = msgp;
 
 	p80211pstrd_t		*pstr;
-	UINT8			bytebuf[80];
+	u8			bytebuf[80];
 	hfa384x_bytestr_t	*p2bytestr = (hfa384x_bytestr_t*)bytebuf;
-	hfa384x_PCFInfo_data_t	*pcfinfo = (hfa384x_PCFInfo_data_t*)bytebuf;
-	UINT16			word;
+	u16			word;
 	DBFENTER;
 
 	wlandev->macmode = WLAN_MACMODE_NONE;
@@ -1210,170 +542,45 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 	/* Set the SSID */
 	memcpy(&wlandev->ssid, &msg->ssid.data, sizeof(msg->ssid.data));
 
-	if (!hw->ap) {
-		/*** ADHOC IBSS ***/
-		/* see if current f/w is less than 8c3 */
-		if (HFA384x_FIRMWARE_VERSION(hw->ident_sta_fw.major,
-					     hw->ident_sta_fw.minor,
-					     hw->ident_sta_fw.variant) <
-		    HFA384x_FIRMWARE_VERSION(0,8,3)) {
-			/* Ad-Hoc not quite supported on Prism2 */
-			msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-			msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-			goto done;
-		}
-
-  		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-		/*** STATION ***/
-		/* Set the REQUIRED config items */
-		/* SSID */
-		pstr = (p80211pstrd_t*)&(msg->ssid.data);
-		prism2mgmt_pstr2bytestr(p2bytestr, pstr);
-		result = hfa384x_drvr_setconfig( hw, HFA384x_RID_CNFOWNSSID,
-				bytebuf, HFA384x_RID_CNFOWNSSID_LEN);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set CnfOwnSSID\n");
-			goto failed;
-		}
-		result = hfa384x_drvr_setconfig( hw, HFA384x_RID_CNFDESIREDSSID,
-				bytebuf, HFA384x_RID_CNFDESIREDSSID_LEN);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set CnfDesiredSSID\n");
-			goto failed;
-		}
-
-		/* bsstype - we use the default in the ap firmware */
-		/* IBSS port */
-		hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFPORTTYPE, 0);
-
-		/* beacon period */
-		word = msg->beaconperiod.data;
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFAPBCNINT, word);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set beacon period=%d.\n", word);
-			goto failed;
-		}
-
-		/* dschannel */
-		word = msg->dschannel.data;
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFOWNCHANNEL, word);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set channel=%d.\n", word);
-			goto failed;
-		}
-		/* Basic rates */
-		word = p80211rate_to_p2bit(msg->basicrate1.data);
-		if ( msg->basicrate2.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->basicrate2.data);
-		}
-		if ( msg->basicrate3.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->basicrate3.data);
-		}
-		if ( msg->basicrate4.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->basicrate4.data);
-		}
-		if ( msg->basicrate5.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->basicrate5.data);
-		}
-		if ( msg->basicrate6.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->basicrate6.data);
-		}
-		if ( msg->basicrate7.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->basicrate7.data);
-		}
-		if ( msg->basicrate8.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->basicrate8.data);
-		}
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFBASICRATES, word);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set basicrates=%d.\n", word);
-			goto failed;
-		}
-
-		/* Operational rates (supprates and txratecontrol) */
-		word = p80211rate_to_p2bit(msg->operationalrate1.data);
-		if ( msg->operationalrate2.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->operationalrate2.data);
-		}
-		if ( msg->operationalrate3.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->operationalrate3.data);
-		}
-		if ( msg->operationalrate4.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->operationalrate4.data);
-		}
-		if ( msg->operationalrate5.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->operationalrate5.data);
-		}
-		if ( msg->operationalrate6.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->operationalrate6.data);
-		}
-		if ( msg->operationalrate7.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->operationalrate7.data);
-		}
-		if ( msg->operationalrate8.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->operationalrate8.data);
-		}
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFSUPPRATES, word);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set supprates=%d.\n", word);
-			goto failed;
-		}
-
- 		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_TXRATECNTL, word);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set txrates=%d.\n", word);
-			goto failed;
-		}
-
-		/* Set the macmode so the frame setup code knows what to do */
-		if ( msg->bsstype.data == P80211ENUM_bsstype_independent ) {
-			wlandev->macmode = WLAN_MACMODE_IBSS_STA;
-			/* lets extend the data length a bit */
-			hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFMAXDATALEN, 2304);
-		}
-
-		/* Enable the Port */
-		result = hfa384x_drvr_enable(hw, 0);
-		if ( result ) {
-			WLAN_LOG_ERROR("Enable macport failed, result=%d.\n", result);
-			goto failed;
-		}
-
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-
+	/*** ADHOC IBSS ***/
+	/* see if current f/w is less than 8c3 */
+	if (HFA384x_FIRMWARE_VERSION(hw->ident_sta_fw.major,
+				     hw->ident_sta_fw.minor,
+				     hw->ident_sta_fw.variant) <
+	    HFA384x_FIRMWARE_VERSION(0,8,3)) {
+		/* Ad-Hoc not quite supported on Prism2 */
+		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
 		goto done;
 	}
 
-	/*** ACCESS POINT ***/
-
 	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
 
-	/* Validate the command, if BSStype=infra is the tertiary loaded? */
-	if ( msg->bsstype.data == P80211ENUM_bsstype_independent ) {
-		WLAN_LOG_ERROR("AP driver cannot create IBSS.\n");
-		goto failed;
-	} else if ( hw->cap_sup_sta.id != 5) {
-		WLAN_LOG_ERROR("AP driver failed to detect AP firmware.\n");
-		goto failed;
-	}
-
+	/*** STATION ***/
 	/* Set the REQUIRED config items */
 	/* SSID */
 	pstr = (p80211pstrd_t*)&(msg->ssid.data);
 	prism2mgmt_pstr2bytestr(p2bytestr, pstr);
 	result = hfa384x_drvr_setconfig( hw, HFA384x_RID_CNFOWNSSID,
-				bytebuf, HFA384x_RID_CNFOWNSSID_LEN);
+					 bytebuf, HFA384x_RID_CNFOWNSSID_LEN);
+	if ( result ) {
+		WLAN_LOG_ERROR("Failed to set CnfOwnSSID\n");
+		goto failed;
+	}
+	result = hfa384x_drvr_setconfig( hw, HFA384x_RID_CNFDESIREDSSID,
+					 bytebuf, HFA384x_RID_CNFDESIREDSSID_LEN);
 	if ( result ) {
-		WLAN_LOG_ERROR("Failed to set SSID, result=0x%04x\n", result);
+		WLAN_LOG_ERROR("Failed to set CnfDesiredSSID\n");
 		goto failed;
 	}
 
 	/* bsstype - we use the default in the ap firmware */
+	/* IBSS port */
+	hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFPORTTYPE, 0);
 
 	/* beacon period */
 	word = msg->beaconperiod.data;
-	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFAPBCNINT, word);
+	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFAPBCNint, word);
 	if ( result ) {
 		WLAN_LOG_ERROR("Failed to set beacon period=%d.\n", word);
 		goto failed;
@@ -1443,98 +650,20 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 		WLAN_LOG_ERROR("Failed to set supprates=%d.\n", word);
 		goto failed;
 	}
-	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_TXRATECNTL0, word);
-	if ( result ) {
-		WLAN_LOG_ERROR("Failed to set txrates=%d.\n", word);
-		goto failed;
-	}
-
-	/* ibssatimwindow */
-	if (msg->ibssatimwindow.status == P80211ENUM_msgitem_status_data_ok) {
-		WLAN_LOG_INFO("prism2mgmt_start: atimwindow not used in "
-			       "Infrastructure mode, ignored.\n");
-	}
-
-	/* DTIM period */
-	word = msg->dtimperiod.data;
-	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFOWNDTIMPER, word);
-	if ( result ) {
-		WLAN_LOG_ERROR("Failed to set dtim period=%d.\n", word);
-		goto failed;
-	}
-
-	/* probedelay */
-	if (msg->probedelay.status == P80211ENUM_msgitem_status_data_ok) {
-		WLAN_LOG_INFO("prism2mgmt_start: probedelay not "
-			       "supported in prism2, ignored.\n");
-	}
-
-	/* cfpollable, cfpollreq, cfpperiod, cfpmaxduration */
-	if (msg->cfpollable.data == P80211ENUM_truth_true &&
-	    msg->cfpollreq.data == P80211ENUM_truth_true ) {
-		WLAN_LOG_ERROR("cfpollable=cfpollreq=true is illegal.\n");
-		result = -1;
-		goto failed;
-	}
-
-	/* read the PCFInfo and update */
-	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_CNFAPPCFINFO,
-					pcfinfo, HFA384x_RID_CNFAPPCFINFO_LEN);
-	if ( result ) {
-		WLAN_LOG_INFO("prism2mgmt_start: read(pcfinfo) failed, "
-				"assume it's "
-				"not supported, pcf settings ignored.\n");
-		goto pcf_skip;
-	}
-	if ((msg->cfpollable.data == P80211ENUM_truth_false &&
-	     msg->cfpollreq.data == P80211ENUM_truth_false) ) {
-	    	pcfinfo->MediumOccupancyLimit = 0;
-		pcfinfo->CFPPeriod = 0;
-		pcfinfo->CFPMaxDuration = 0;
-		pcfinfo->CFPFlags &= host2hfa384x_16((UINT16)~BIT0);
-
-		if ( msg->cfpperiod.status == P80211ENUM_msgitem_status_data_ok ||
-		     msg->cfpmaxduration.status == P80211ENUM_msgitem_status_data_ok ) {
-			WLAN_LOG_WARNING(
-				"Setting cfpperiod or cfpmaxduration when "
-				"cfpollable and cfreq are false is pointless.\n");
-		}
-	}
-	if ((msg->cfpollable.data == P80211ENUM_truth_true ||
-	     msg->cfpollreq.data == P80211ENUM_truth_true) ) {
-		if ( msg->cfpollable.data == P80211ENUM_truth_true) {
-			pcfinfo->CFPFlags |= host2hfa384x_16((UINT16)BIT0);
-		}
-
-		if ( msg->cfpperiod.status == P80211ENUM_msgitem_status_data_ok) {
-			pcfinfo->CFPPeriod = msg->cfpperiod.data;
-			pcfinfo->CFPPeriod = host2hfa384x_16(pcfinfo->CFPPeriod);
-		}
 
-		if ( msg->cfpmaxduration.status == P80211ENUM_msgitem_status_data_ok) {
-			pcfinfo->CFPMaxDuration = msg->cfpmaxduration.data;
-			pcfinfo->CFPMaxDuration = host2hfa384x_16(pcfinfo->CFPMaxDuration);
-			pcfinfo->MediumOccupancyLimit = pcfinfo->CFPMaxDuration;
-		}
-	}
-	result = hfa384x_drvr_setconfig(hw, HFA384x_RID_CNFAPPCFINFO,
-					pcfinfo, HFA384x_RID_CNFAPPCFINFO_LEN);
+	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_TXRATECNTL, word);
 	if ( result ) {
-		WLAN_LOG_ERROR("write(pcfinfo) failed.\n");
+		WLAN_LOG_ERROR("Failed to set txrates=%d.\n", word);
 		goto failed;
 	}
 
-pcf_skip:
 	/* Set the macmode so the frame setup code knows what to do */
-	if ( msg->bsstype.data == P80211ENUM_bsstype_infrastructure ) {
-		wlandev->macmode = WLAN_MACMODE_ESS_AP;
+	if ( msg->bsstype.data == P80211ENUM_bsstype_independent ) {
+		wlandev->macmode = WLAN_MACMODE_IBSS_STA;
 		/* lets extend the data length a bit */
 		hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFMAXDATALEN, 2304);
 	}
 
-	/* Set the BSSID to the same as our MAC */
-	memcpy( wlandev->bssid, wlandev->netdev->dev_addr, WLAN_BSSID_LEN);
-
 	/* Enable the Port */
 	result = hfa384x_drvr_enable(hw, 0);
 	if ( result ) {
@@ -1556,80 +685,6 @@ done:
 	return result;
 }
 
-
-/*----------------------------------------------------------------
-* prism2mgmt_enable
-*
-* Start a BSS.  Any station can do this for IBSS, only AP for ESS.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_enable(wlandevice_t *wlandev, void *msgp)
-{
-	int			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_enable_t	*msg = msgp;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* Ad-Hoc not quite supported on Prism2 */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-		goto done;
-	}
-
-	/*** ACCESS POINT ***/
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-	/* Is the tertiary loaded? */
-	if ( hw->cap_sup_sta.id != 5) {
-		WLAN_LOG_ERROR("AP driver failed to detect AP firmware.\n");
-		goto failed;
-	}
-
-	/* Set the macmode so the frame setup code knows what to do */
-	wlandev->macmode = WLAN_MACMODE_ESS_AP;
-
-	/* Set the BSSID to the same as our MAC */
-	memcpy( wlandev->bssid, wlandev->netdev->dev_addr, WLAN_BSSID_LEN);
-
-	/* Enable the Port */
-	result = hfa384x_drvr_enable(hw, 0);
-	if ( result ) {
-		WLAN_LOG_ERROR("Enable macport failed, result=%d.\n", result);
-		goto failed;
-	}
-
-	msg->resultcode.data = P80211ENUM_resultcode_success;
-
-	goto done;
-failed:
-	msg->resultcode.data = P80211ENUM_resultcode_invalid_parameters;
-
-done:
-	result = 0;
-
-	DBFEXIT;
-	return result;
-}
-
-
 /*----------------------------------------------------------------
 * prism2mgmt_readpda
 *
@@ -1696,402 +751,6 @@ int prism2mgmt_readpda(wlandevice_t *wlandev, void *msgp)
 }
 
 /*----------------------------------------------------------------
-* prism2mgmt_readcis
-*
-* Collect the CIS data and put it in the message.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_readcis(wlandevice_t *wlandev, void *msgp)
-{
-	int			result;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_readcis_t	*msg = msgp;
-
-	DBFENTER;
-
-        memset(msg->cis.data, 0, sizeof(msg->cis.data));
-
-	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_CIS,
-					msg->cis.data, HFA384x_RID_CIS_LEN);
-	if ( result ) {
-		WLAN_LOG_INFO("prism2mgmt_readcis: read(cis) failed.\n");
-		msg->cis.status = P80211ENUM_msgitem_status_no_value;
-		msg->resultcode.data = P80211ENUM_resultcode_implementation_failure;
-
-		}
-	else {
-		msg->cis.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-		}
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-	DBFEXIT;
-	return 0;
-}
-
-/*----------------------------------------------------------------
-* prism2mgmt_auxport_state
-*
-* Enables/Disables the card's auxiliary port.  Should be called
-* before and after a sequence of auxport_read()/auxport_write()
-* calls.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_auxport_state(wlandevice_t *wlandev, void *msgp)
-{
-	p80211msg_p2req_auxport_state_t	*msg = msgp;
-
-#if (WLAN_HOSTIF != WLAN_USB)
-	hfa384x_t		*hw = wlandev->priv;
-	DBFENTER;
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-	if (msg->enable.data == P80211ENUM_truth_true) {
-		if ( hfa384x_cmd_aux_enable(hw, 0) ) {
-			msg->resultcode.data = P80211ENUM_resultcode_implementation_failure;
-		} else {
-			msg->resultcode.data = P80211ENUM_resultcode_success;
-		}
-	} else {
-		hfa384x_cmd_aux_disable(hw);
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-	}
-
-#else /* !USB */
-	DBFENTER;
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-	msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-
-#endif /* WLAN_HOSTIF != WLAN_USB */
-
-	DBFEXIT;
-	return 0;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_auxport_read
-*
-* Copies data from the card using the auxport.  The auxport must
-* have previously been enabled.  Note: this is not the way to
-* do downloads, see the [ram|flash]dl functions.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_auxport_read(wlandevice_t *wlandev, void *msgp)
-{
-#if (WLAN_HOSTIF != WLAN_USB)
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_auxport_read_t	*msg = msgp;
-	UINT32			addr;
-	UINT32			len;
-	UINT8*			buf;
-	UINT32			maxlen = sizeof(msg->data.data);
-	DBFENTER;
-
-	if ( hw->auxen ) {
-		addr = msg->addr.data;
-		len = msg->len.data;
-		buf = msg->data.data;
-		if ( len <= maxlen ) {  /* max read/write size */
-			hfa384x_copy_from_aux(hw, addr, HFA384x_AUX_CTL_EXTDS, buf, len);
-			msg->resultcode.data = P80211ENUM_resultcode_success;
-		} else {
-			WLAN_LOG_DEBUG(1,"Attempt to read > maxlen from auxport.\n");
-			msg->resultcode.data = P80211ENUM_resultcode_refused;
-		}
-
-	} else {
-		msg->resultcode.data = P80211ENUM_resultcode_refused;
-	}
-	msg->data.status = P80211ENUM_msgitem_status_data_ok;
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-	DBFEXIT;
-	return 0;
-#else
-	DBFENTER;
-
-	WLAN_LOG_ERROR("prism2mgmt_auxport_read: Not supported on USB.\n");
-
-	DBFEXIT;
-	return 0;
-#endif
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_auxport_write
-*
-* Copies data to the card using the auxport.  The auxport must
-* have previously been enabled.  Note: this is not the way to
-* do downloads, see the [ram|flash]dl functions.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_auxport_write(wlandevice_t *wlandev, void *msgp)
-{
-#if (WLAN_HOSTIF != WLAN_USB)
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_auxport_write_t	*msg = msgp;
-	UINT32			addr;
-	UINT32			len;
-	UINT8*			buf;
-	UINT32			maxlen = sizeof(msg->data.data);
-	DBFENTER;
-
-	if ( hw->auxen ) {
-		addr = msg->addr.data;
-		len = msg->len.data;
-		buf = msg->data.data;
-		if ( len <= maxlen ) {  /* max read/write size */
-			hfa384x_copy_to_aux(hw, addr, HFA384x_AUX_CTL_EXTDS, buf, len);
-		} else {
-			WLAN_LOG_DEBUG(1,"Attempt to write > maxlen from auxport.\n");
-			msg->resultcode.data = P80211ENUM_resultcode_refused;
-		}
-
-	} else {
-		msg->resultcode.data = P80211ENUM_resultcode_refused;
-	}
-	msg->data.status = P80211ENUM_msgitem_status_data_ok;
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-	DBFEXIT;
-	return 0;
-#else
-	DBFENTER;
-	WLAN_LOG_ERROR("prism2mgmt_auxport_read: Not supported on USB.\n");
-	DBFEXIT;
-	return 0;
-#endif
-}
-
-/*----------------------------------------------------------------
-* prism2mgmt_low_level
-*
-* Puts the card into the desired test mode.
-*
-* Arguments:
-*       wlandev         wlan device structure
-*       msgp            ptr to msg buffer
-*
-* Returns:
-*       0       success and done
-*       <0      success, but we're waiting for something to finish.
-*       >0      an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*       process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_low_level(wlandevice_t *wlandev, void *msgp)
-{
-	hfa384x_t		*hw = wlandev->priv;
-        p80211msg_p2req_low_level_t     *msg = msgp;
-	hfa384x_metacmd_t cmd;
-        DBFENTER;
-
-        msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-        /* call some routine to execute the test command */
-	cmd.cmd = (UINT16) msg->command.data;
-	cmd.parm0 = (UINT16) msg->param0.data;
-	cmd.parm1 = (UINT16) msg->param1.data;
-	cmd.parm2 = (UINT16) msg->param2.data;
-
-        hfa384x_drvr_low_level(hw,&cmd);
-
-        msg->resp0.data = (UINT32) cmd.result.resp0;
-        msg->resp1.data = (UINT32) cmd.result.resp1;
-        msg->resp2.data = (UINT32) cmd.result.resp2;
-
-        msg->resultcode.data = P80211ENUM_resultcode_success;
-
-        DBFEXIT;
-        return 0;
-}
-
-/*----------------------------------------------------------------
-* prism2mgmt_test_command
-*
-* Puts the card into the desired test mode.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_test_command(wlandevice_t *wlandev, void *msgp)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_test_command_t	*msg = msgp;
-	hfa384x_metacmd_t cmd;
-
-        DBFENTER;
-
-	cmd.cmd = ((UINT16) msg->testcode.data) << 8 | 0x38;
-	cmd.parm0 = (UINT16) msg->testparam.data;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-        /* call some routine to execute the test command */
-
-        hfa384x_drvr_low_level(hw,&cmd);
-
-        msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-        msg->resultcode.data = P80211ENUM_resultcode_success;
-
-        msg->status.status = P80211ENUM_msgitem_status_data_ok;
-        msg->status.data = cmd.result.status;
-        msg->resp0.status = P80211ENUM_msgitem_status_data_ok;
-        msg->resp0.data = cmd.result.resp0;
-        msg->resp1.status = P80211ENUM_msgitem_status_data_ok;
-        msg->resp1.data = cmd.result.resp1;
-        msg->resp2.status = P80211ENUM_msgitem_status_data_ok;
-        msg->resp2.data = cmd.result.resp2;
-
-	DBFEXIT;
-	return 0;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_mmi_read
-*
-* Read from one of the MMI registers.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_mmi_read(wlandevice_t *wlandev, void *msgp)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_mmi_read_t	*msg = msgp;
-	UINT32 resp = 0;
-
-	DBFENTER;
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-	/* call some routine to execute the test command */
-
-	hfa384x_drvr_mmi_read(hw, msg->addr.data, &resp);
-
-	/* I'm not sure if this is "architecturally" correct, but it
-           is expedient. */
-
-	msg->value.status = P80211ENUM_msgitem_status_data_ok;
-	msg->value.data = resp;
-	msg->resultcode.data = P80211ENUM_resultcode_success;
-
-	DBFEXIT;
-	return 0;
-}
-
-/*----------------------------------------------------------------
-* prism2mgmt_mmi_write
-*
-* Write a data value to one of the MMI registers.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_mmi_write(wlandevice_t *wlandev, void *msgp)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_mmi_write_t	*msg = msgp;
-	DBFENTER;
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-	/* call some routine to execute the test command */
-
-	hfa384x_drvr_mmi_write(hw, msg->addr.data, msg->data.data);
-
-	msg->resultcode.data = P80211ENUM_resultcode_success;
-
-	DBFEXIT;
-	return 0;
-}
-
-/*----------------------------------------------------------------
 * prism2mgmt_ramdl_state
 *
 * Establishes the beginning/end of a card RAM download session.
@@ -2179,9 +838,9 @@ int prism2mgmt_ramdl_write(wlandevice_t *wlandev, void *msgp)
 {
 	hfa384x_t		*hw = wlandev->priv;
 	p80211msg_p2req_ramdl_write_t	*msg = msgp;
-	UINT32			addr;
-	UINT32			len;
-	UINT8			*buf;
+	u32			addr;
+	u32			len;
+	u8			*buf;
 	DBFENTER;
 
 	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
@@ -2319,9 +978,9 @@ int prism2mgmt_flashdl_write(wlandevice_t *wlandev, void *msgp)
 {
 	hfa384x_t		*hw = wlandev->priv;
 	p80211msg_p2req_flashdl_write_t	*msg = msgp;
-	UINT32			addr;
-	UINT32			len;
-	UINT8			*buf;
+	u32			addr;
+	u32			len;
+	u8			*buf;
 	DBFENTER;
 
 	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
@@ -2361,247 +1020,6 @@ int prism2mgmt_flashdl_write(wlandevice_t *wlandev, void *msgp)
 	return 0;
 }
 
-
-/*----------------------------------------------------------------
-* prism2mgmt_dump_state
-*
-* Dumps the driver's and hardware's current state via the kernel
-* log at KERN_NOTICE level.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_dump_state(wlandevice_t *wlandev, void *msgp)
-{
-	p80211msg_p2req_dump_state_t	*msg = msgp;
-	int				result = 0;
-
-#if (WLAN_HOSTIF != WLAN_USB)
-	hfa384x_t		*hw = wlandev->priv;
-	UINT16				auxbuf[15];
-	DBFENTER;
-
-	WLAN_LOG_NOTICE("prism2 driver and hardware state:\n");
-	if  ( (result = hfa384x_cmd_aux_enable(hw, 0)) ) {
-		WLAN_LOG_ERROR("aux_enable failed, result=%d\n", result);
-		goto failed;
-	}
-	hfa384x_copy_from_aux(hw,
-		0x01e2,
-		HFA384x_AUX_CTL_EXTDS,
-		auxbuf,
-		sizeof(auxbuf));
-	hfa384x_cmd_aux_disable(hw);
-	WLAN_LOG_NOTICE("  cmac: FreeBlocks=%d\n", auxbuf[5]);
-	WLAN_LOG_NOTICE("  cmac: IntEn=0x%02x EvStat=0x%02x\n",
-		hfa384x_getreg(hw, HFA384x_INTEN),
-		hfa384x_getreg(hw, HFA384x_EVSTAT));
-
-	#ifdef USE_FID_STACK
-	WLAN_LOG_NOTICE("  drvr: txfid_top=%d stacksize=%d\n",
-		hw->txfid_top,HFA384x_DRVR_FIDSTACKLEN_MAX);
-	#else
-	WLAN_LOG_NOTICE("  drvr: txfid_head=%d txfid_tail=%d txfid_N=%d\n",
-		hw->txfid_head, hw->txfid_tail, hw->txfid_N);
-	#endif
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-	msg->resultcode.data = P80211ENUM_resultcode_success;
-
-#else /* (WLAN_HOSTIF == WLAN_USB) */
-
-	DBFENTER;
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-	msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	goto failed;
-
-#endif /* (WLAN_HOSTIF != WLAN_USB) */
-
-failed:
-	DBFEXIT;
-	return result;
-}
-
-/*----------------------------------------------------------------
-* prism2mgmt_channel_info
-*
-* Issues a ChannelInfoRequest.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_channel_info(wlandevice_t *wlandev, void *msgp)
-{
-	p80211msg_p2req_channel_info_t	*msg=msgp;
-	hfa384x_t			*hw = wlandev->priv;
-	int				result, i, n=0;
-	UINT16				channel_mask=0;
-	hfa384x_ChannelInfoRequest_data_t	chinforeq;
-	// unsigned long 			now;
-
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* Not supported in STA f/w */
-		P80211_SET_INT(msg->resultcode, P80211ENUM_resultcode_not_supported);
-		goto done;
-	}
-
-	/*** ACCESS POINT ***/
-
-#define CHINFO_TIMEOUT 2
-
-	P80211_SET_INT(msg->resultcode, P80211ENUM_resultcode_success);
-
-	/* setting default value for channellist = all channels */
-	if (!msg->channellist.data) {
-		P80211_SET_INT(msg->channellist, 0x00007FFE);
-	}
-	/* setting default value for channeldwelltime = 100 ms */
-	if (!msg->channeldwelltime.data) {
-		P80211_SET_INT(msg->channeldwelltime, 100);
-	}
-	channel_mask = (UINT16) (msg->channellist.data >> 1);
-	for (i=0, n=0; i < 14; i++) {
-		if (channel_mask & (1<<i)) {
-			n++;
-		}
-	}
-	P80211_SET_INT(msg->numchinfo, n);
-	chinforeq.channelList = host2hfa384x_16(channel_mask);
-	chinforeq.channelDwellTime = host2hfa384x_16(msg->channeldwelltime.data);
-
-	atomic_set(&hw->channel_info.done, 1);
-
-	result = hfa384x_drvr_setconfig( hw, HFA384x_RID_CHANNELINFOREQUEST,
-					 &chinforeq, HFA384x_RID_CHANNELINFOREQUEST_LEN);
-	if ( result ) {
-		WLAN_LOG_ERROR("setconfig(CHANNELINFOREQUEST) failed. result=%d\n",
-				result);
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-		goto done;
-	}
-	/*
-	now = jiffies;
-	while (atomic_read(&hw->channel_info.done) != 1) {
-		if ((jiffies - now) > CHINFO_TIMEOUT*HZ) {
-			WLAN_LOG_NOTICE("ChannelInfo results not received in %d seconds, aborting.\n",
-					CHINFO_TIMEOUT);
-			msg->resultcode.data = P80211ENUM_resultcode_timeout;
-			goto done;
-		}
-		current->state = TASK_INTERRUPTIBLE;
-		schedule_timeout(HZ/4);
-		current->state = TASK_RUNNING;
-	}
-	*/
-
-done:
-
-	DBFEXIT;
-	return 0;
-}
-
-/*----------------------------------------------------------------
-* prism2mgmt_channel_info_results
-*
-* Returns required ChannelInfo result.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_channel_info_results(wlandevice_t *wlandev, void *msgp)
-{
-	hfa384x_t			*hw = wlandev->priv;
-
-	p80211msg_p2req_channel_info_results_t	*msg=msgp;
-	int				result=0;
-	int		channel;
-
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* Not supported in STA f/w */
-		P80211_SET_INT(msg->resultcode, P80211ENUM_resultcode_not_supported);
-		goto done;
-	}
-
-	/*** ACCESS POINT ***/
-
-	switch (atomic_read(&hw->channel_info.done)) {
-	case 0: msg->resultcode.status = P80211ENUM_msgitem_status_no_value;
-		goto done;
-	case 1: msg->resultcode.status = P80211ENUM_msgitem_status_incomplete_itemdata;
-		goto done;
-	}
-
-	P80211_SET_INT(msg->resultcode, P80211ENUM_resultcode_success);
-	channel=msg->channel.data-1;
-
-	if (channel < 0 || ! (hw->channel_info.results.scanchannels & 1<<channel) ) {
-		msg->resultcode.data = P80211ENUM_resultcode_invalid_parameters;
-		goto done;
-	}
-	WLAN_LOG_DEBUG(2, "chinfo_results: channel %d, avg/peak level=%d/%d dB, active=%d\n",
-			channel+1,
-			hw->channel_info.results.result[channel].anl,
-			hw->channel_info.results.result[channel].pnl,
-			hw->channel_info.results.result[channel].active
-		);
-	P80211_SET_INT(msg->avgnoiselevel, hw->channel_info.results.result[channel].anl);
-	P80211_SET_INT(msg->peaknoiselevel, hw->channel_info.results.result[channel].pnl);
-	P80211_SET_INT(msg->bssactive, hw->channel_info.results.result[channel].active &
-		HFA384x_CHINFORESULT_BSSACTIVE
-                ? P80211ENUM_truth_true
-                : P80211ENUM_truth_false) ;
-	P80211_SET_INT(msg->pcfactive, hw->channel_info.results.result[channel].active &
-		HFA384x_CHINFORESULT_PCFACTIVE
-                ? P80211ENUM_truth_true
-                : P80211ENUM_truth_false) ;
-
-done:
-	DBFEXIT;
-	return result;
-}
-
-
 /*----------------------------------------------------------------
 * prism2mgmt_autojoin
 *
@@ -2625,11 +1043,11 @@ int prism2mgmt_autojoin(wlandevice_t *wlandev, void *msgp)
 {
 	hfa384x_t			*hw = wlandev->priv;
 	int 			result = 0;
-	UINT16			reg;
-	UINT16			port_type;
+	u16			reg;
+	u16			port_type;
 	p80211msg_lnxreq_autojoin_t	*msg = msgp;
 	p80211pstrd_t		*pstr;
-	UINT8			bytebuf[256];
+	u8			bytebuf[256];
 	hfa384x_bytestr_t	*p2bytestr = (hfa384x_bytestr_t*)bytebuf;
 	DBFENTER;
 
@@ -2638,16 +1056,6 @@ int prism2mgmt_autojoin(wlandevice_t *wlandev, void *msgp)
 	/* Set the SSID */
 	memcpy(&wlandev->ssid, &msg->ssid.data, sizeof(msg->ssid.data));
 
-	if (hw->ap) {
-
-		/*** ACCESS POINT ***/
-
-		/* Never supported on AP */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-		goto done;
-	}
-
 	/* Disable the Port */
 	hfa384x_drvr_disable(hw, 0);
 
@@ -2699,7 +1107,6 @@ int prism2mgmt_autojoin(wlandevice_t *wlandev, void *msgp)
 	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
 	msg->resultcode.data = P80211ENUM_resultcode_success;
 
-done:
 	DBFEXIT;
 	return result;
 }
@@ -2730,7 +1137,7 @@ int prism2mgmt_wlansniff(wlandevice_t *wlandev, void *msgp)
 	p80211msg_lnxreq_wlansniff_t	*msg = msgp;
 
 	hfa384x_t			*hw = wlandev->priv;
-	UINT16			word;
+	u16			word;
 
 	DBFENTER;
 
diff --git a/drivers/staging/wlan-ng/prism2mgmt.h b/drivers/staging/wlan-ng/prism2mgmt.h
index 733fd999c928..caf808d57966 100644
--- a/drivers/staging/wlan-ng/prism2mgmt.h
+++ b/drivers/staging/wlan-ng/prism2mgmt.h
@@ -73,10 +73,6 @@
 /*=============================================================*/
 /*------ Static variable externs ------------------------------*/
 
-#if (WLAN_HOSTIF != WLAN_USB)
-extern int      prism2_bap_timeout;
-extern int	prism2_irq_evread_max;
-#endif
 extern int	prism2_debug;
 extern int      prism2_reset_holdtime;
 extern int      prism2_reset_settletime;
@@ -84,8 +80,8 @@ extern int      prism2_reset_settletime;
 /*--- Function Declarations -----------------------------------*/
 /*=============================================================*/
 
-UINT32
-prism2sta_ifstate(wlandevice_t *wlandev, UINT32 ifstate);
+u32
+prism2sta_ifstate(wlandevice_t *wlandev, u32 ifstate);
 
 void
 prism2sta_ev_dtim(wlandevice_t *wlandev);
@@ -94,47 +90,24 @@ prism2sta_ev_infdrop(wlandevice_t *wlandev);
 void
 prism2sta_ev_info(wlandevice_t *wlandev, hfa384x_InfFrame_t *inf);
 void
-prism2sta_ev_txexc(wlandevice_t *wlandev, UINT16 status);
+prism2sta_ev_txexc(wlandevice_t *wlandev, u16 status);
 void
-prism2sta_ev_tx(wlandevice_t *wlandev, UINT16 status);
+prism2sta_ev_tx(wlandevice_t *wlandev, u16 status);
 void
 prism2sta_ev_rx(wlandevice_t *wlandev, struct sk_buff *skb);
 void
 prism2sta_ev_alloc(wlandevice_t *wlandev);
 
-
 int prism2mgmt_mibset_mibget(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_powermgmt(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_scan_results(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_join(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_p2_join(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_authenticate(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_deauthenticate(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_associate(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_reassociate(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_disassociate(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_reset(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_start(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_wlansniff(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_readpda(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_readcis(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_auxport_state(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_auxport_read(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_auxport_write(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_low_level(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_test_command(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_mmi_read(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_mmi_write(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_ramdl_state(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_ramdl_write(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_flashdl_state(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_flashdl_write(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_mm_state(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_dump_state(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_enable(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_channel_info(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_channel_info_results(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_autojoin(wlandevice_t *wlandev, void *msgp);
 
 /*---------------------------------------------------------------
@@ -142,31 +115,31 @@ int prism2mgmt_autojoin(wlandevice_t *wlandev, void *msgp);
 * Prism2 data types
 ---------------------------------------------------------------*/
 /* byte area conversion functions*/
-void prism2mgmt_pstr2bytearea(UINT8 *bytearea, p80211pstrd_t *pstr);
-void prism2mgmt_bytearea2pstr(UINT8 *bytearea, p80211pstrd_t *pstr, int len);
+void prism2mgmt_pstr2bytearea(u8 *bytearea, p80211pstrd_t *pstr);
+void prism2mgmt_bytearea2pstr(u8 *bytearea, p80211pstrd_t *pstr, int len);
 
 /* byte string conversion functions*/
 void prism2mgmt_pstr2bytestr(hfa384x_bytestr_t *bytestr, p80211pstrd_t *pstr);
 void prism2mgmt_bytestr2pstr(hfa384x_bytestr_t *bytestr, p80211pstrd_t *pstr);
 
 /* integer conversion functions */
-void prism2mgmt_prism2int2p80211int(UINT16 *prism2int, UINT32 *wlanint);
-void prism2mgmt_p80211int2prism2int(UINT16 *prism2int, UINT32 *wlanint);
+void prism2mgmt_prism2int2p80211int(u16 *prism2int, u32 *wlanint);
+void prism2mgmt_p80211int2prism2int(u16 *prism2int, u32 *wlanint);
 
 /* enumerated integer conversion functions */
-void prism2mgmt_prism2enum2p80211enum(UINT16 *prism2enum, UINT32 *wlanenum, UINT16 rid);
-void prism2mgmt_p80211enum2prism2enum(UINT16 *prism2enum, UINT32 *wlanenum, UINT16 rid);
+void prism2mgmt_prism2enum2p80211enum(u16 *prism2enum, u32 *wlanenum, u16 rid);
+void prism2mgmt_p80211enum2prism2enum(u16 *prism2enum, u32 *wlanenum, u16 rid);
 
 /* functions to convert a bit area to/from an Operational Rate Set */
-void prism2mgmt_get_oprateset(UINT16 *rate, p80211pstrd_t *pstr);
-void prism2mgmt_set_oprateset(UINT16 *rate, p80211pstrd_t *pstr);
+void prism2mgmt_get_oprateset(u16 *rate, p80211pstrd_t *pstr);
+void prism2mgmt_set_oprateset(u16 *rate, p80211pstrd_t *pstr);
 
 /* functions to convert Group Addresses */
-void prism2mgmt_get_grpaddr(UINT32 did,
+void prism2mgmt_get_grpaddr(u32 did,
 	p80211pstrd_t *pstr, hfa384x_t *priv );
-int prism2mgmt_set_grpaddr(UINT32 did,
-	UINT8 *prism2buf, p80211pstrd_t *pstr, hfa384x_t *priv );
-int prism2mgmt_get_grpaddr_index( UINT32 did );
+int prism2mgmt_set_grpaddr(u32 did,
+	u8 *prism2buf, p80211pstrd_t *pstr, hfa384x_t *priv );
+int prism2mgmt_get_grpaddr_index( u32 did );
 
 void prism2sta_processing_defer(struct work_struct *data);
 
diff --git a/drivers/staging/wlan-ng/prism2mib.c b/drivers/staging/wlan-ng/prism2mib.c
index eac06f793d81..539c4479d381 100644
--- a/drivers/staging/wlan-ng/prism2mib.c
+++ b/drivers/staging/wlan-ng/prism2mib.c
@@ -54,9 +54,6 @@
 /* System Includes */
 #define WLAN_DBVAR	prism2_debug
 
-#include "version.h"
-
-
 #include <linux/version.h>
 
 #include <linux/module.h>
@@ -69,26 +66,7 @@
 #include <asm/io.h>
 #include <linux/delay.h>
 #include <asm/byteorder.h>
-
-#include "wlan_compat.h"
-
-//#if (WLAN_HOSTIF == WLAN_PCMCIA)
-//#include <pcmcia/version.h>
-//#include <pcmcia/cs_types.h>
-//#include <pcmcia/cs.h>
-//#include <pcmcia/cistpl.h>
-//#include <pcmcia/ds.h>
-//#include <pcmcia/cisreg.h>
-//#endif
-//
-//#if ((WLAN_HOSTIF == WLAN_PLX) || (WLAN_HOSTIF == WLAN_PCI))
-//#include <linux/ioport.h>
-//#include <linux/pci.h>
-//endif
-
-//#if (WLAN_HOSTIF == WLAN_USB)
 #include <linux/usb.h>
-//#endif
 
 /*================================================================*/
 /* Project Includes */
@@ -112,18 +90,17 @@
 /*================================================================*/
 /* Local Types */
 
-#define  F_AP         0x1        /* MIB is supported on Access Points. */
-#define  F_STA        0x2        /* MIB is supported on stations. */
-#define  F_READ       0x4        /* MIB may be read. */
-#define  F_WRITE      0x8        /* MIB may be written. */
+#define  F_STA        0x1        /* MIB is supported on stations. */
+#define  F_READ       0x2        /* MIB may be read. */
+#define  F_WRITE      0x4        /* MIB may be written. */
 
 typedef struct mibrec
 {
-    UINT32   did;
-    UINT16   flag;
-    UINT16   parm1;
-    UINT16   parm2;
-    UINT16   parm3;
+    u32   did;
+    u16   flag;
+    u16   parm1;
+    u16   parm2;
+    u16   parm3;
     int      (*func)(struct mibrec                *mib,
                      int                          isget,
                      wlandevice_t                 *wlandev,
@@ -135,14 +112,6 @@ typedef struct mibrec
 /*================================================================*/
 /* Local Function Declarations */
 
-static int prism2mib_bytestr2pstr(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
 static int prism2mib_bytearea2pstr(
 mibrec_t                     *mib,
 int                          isget,
@@ -159,38 +128,6 @@ hfa384x_t                    *hw,
 p80211msg_dot11req_mibset_t  *msg,
 void                         *data);
 
-static int prism2mib_uint32array(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_uint32offset(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_truth(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_preamble(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
 static int prism2mib_flag(
 mibrec_t                     *mib,
 int                          isget,
@@ -199,22 +136,6 @@ hfa384x_t                    *hw,
 p80211msg_dot11req_mibset_t  *msg,
 void                         *data);
 
-static int prism2mib_appcfinfoflag(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_regulatorydomains(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
 static int prism2mib_wepdefaultkey(
 mibrec_t                     *mib,
 int                          isget,
@@ -223,14 +144,6 @@ hfa384x_t                    *hw,
 p80211msg_dot11req_mibset_t  *msg,
 void                         *data);
 
-static int prism2mib_powermanagement(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
 static int prism2mib_privacyinvoked(
 mibrec_t                     *mib,
 int                          isget,
@@ -255,46 +168,6 @@ hfa384x_t                    *hw,
 p80211msg_dot11req_mibset_t  *msg,
 void                         *data);
 
-static int prism2mib_operationalrateset(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_groupaddress(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_fwid(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_authalg(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_authalgenable(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
 static int prism2mib_priv(
 mibrec_t                     *mib,
 int                          isget,
@@ -303,980 +176,92 @@ hfa384x_t                    *hw,
 p80211msg_dot11req_mibset_t  *msg,
 void                         *data);
 
-static void prism2mib_priv_authlist(
-hfa384x_t      *hw,
-prism2sta_authlist_t  *list);
-
-static void prism2mib_priv_accessmode(
-hfa384x_t         *hw,
-UINT32            mode);
-
-static void prism2mib_priv_accessallow(
-hfa384x_t         *hw,
-p80211macarray_t  *macarray);
-
-static void prism2mib_priv_accessdeny(
-hfa384x_t         *hw,
-p80211macarray_t  *macarray);
-
-static void prism2mib_priv_deauthenticate(
-hfa384x_t         *hw,
-UINT8             *addr);
-
 /*================================================================*/
 /* Local Static Definitions */
 
 static mibrec_t mibtab[] = {
 
     /* dot11smt MIB's */
-
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11StationID,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNMACADDR, HFA384x_RID_CNFOWNMACADDR_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11MediumOccupancyLimit,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPPCFINFO, HFA384x_RID_CNFAPPCFINFO_LEN, 0,
-          prism2mib_uint32offset },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11CFPollable,
-          F_STA | F_READ,
-          HFA384x_RID_CFPOLLABLE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11CFPPeriod,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPPCFINFO, HFA384x_RID_CNFAPPCFINFO_LEN, 1,
-          prism2mib_uint32offset },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11CFPMaxDuration,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPPCFINFO, HFA384x_RID_CNFAPPCFINFO_LEN, 2,
-          prism2mib_uint32offset },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11AuthenticationResponseTimeOut,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFAUTHRSPTIMEOUT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11PrivacyOptionImplemented,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PRIVACYOPTIMP, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11PowerManagementMode,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFPMENABLED, 0, 0,
-          prism2mib_powermanagement },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11DesiredSSID,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFDESIREDSSID, HFA384x_RID_CNFDESIREDSSID_LEN, 0,
-          prism2mib_bytestr2pstr },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11DesiredBSSType,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11OperationalRateSet,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL, 0, 0,
-          prism2mib_operationalrateset },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11OperationalRateSet,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL0, 0, 0,
-          prism2mib_operationalrateset },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11BeaconPeriod,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPBCNINT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11DTIMPeriod,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNDTIMPER, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11AssociationResponseTimeOut,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PROTOCOLRSPTIME, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm1,
-          F_AP | F_STA | F_READ,
-          1, 0, 0,
-          prism2mib_authalg },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm2,
-          F_AP | F_STA | F_READ,
-          2, 0, 0,
-          prism2mib_authalg },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm3,
-          F_AP | F_STA | F_READ,
-          3, 0, 0,
-          prism2mib_authalg },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm4,
-          F_AP | F_STA | F_READ,
-          4, 0, 0,
-          prism2mib_authalg },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm5,
-          F_AP | F_STA | F_READ,
-          5, 0, 0,
-          prism2mib_authalg },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm6,
-          F_AP | F_STA | F_READ,
-          6, 0, 0,
-          prism2mib_authalg },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable1,
-          F_AP | F_STA | F_READ | F_WRITE,
-          1, 0, 0,
-          prism2mib_authalgenable },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable2,
-          F_AP | F_STA | F_READ | F_WRITE,
-          2, 0, 0,
-          prism2mib_authalgenable },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable3,
-          F_AP | F_STA | F_READ | F_WRITE,
-          3, 0, 0,
-          prism2mib_authalgenable },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable4,
-          F_AP | F_STA | F_READ | F_WRITE,
-          4, 0, 0,
-          prism2mib_authalgenable },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable5,
-          F_AP | F_STA | F_READ | F_WRITE,
-          5, 0, 0,
-          prism2mib_authalgenable },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable6,
-          F_AP | F_STA | F_READ | F_WRITE,
-          6, 0, 0,
-          prism2mib_authalgenable },
     { DIDmib_dot11smt_dot11WEPDefaultKeysTable_dot11WEPDefaultKey0,
-          F_AP | F_STA | F_WRITE,
+          F_STA | F_WRITE,
           HFA384x_RID_CNFWEPDEFAULTKEY0, 0, 0,
           prism2mib_wepdefaultkey },
     { DIDmib_dot11smt_dot11WEPDefaultKeysTable_dot11WEPDefaultKey1,
-          F_AP | F_STA | F_WRITE,
+          F_STA | F_WRITE,
           HFA384x_RID_CNFWEPDEFAULTKEY1, 0, 0,
           prism2mib_wepdefaultkey },
     { DIDmib_dot11smt_dot11WEPDefaultKeysTable_dot11WEPDefaultKey2,
-          F_AP | F_STA | F_WRITE,
+          F_STA | F_WRITE,
           HFA384x_RID_CNFWEPDEFAULTKEY2, 0, 0,
           prism2mib_wepdefaultkey },
     { DIDmib_dot11smt_dot11WEPDefaultKeysTable_dot11WEPDefaultKey3,
-          F_AP | F_STA | F_WRITE,
+          F_STA | F_WRITE,
           HFA384x_RID_CNFWEPDEFAULTKEY3, 0, 0,
           prism2mib_wepdefaultkey },
     { DIDmib_dot11smt_dot11PrivacyTable_dot11PrivacyInvoked,
-          F_AP | F_STA | F_READ | F_WRITE,
+          F_STA | F_READ | F_WRITE,
           HFA384x_RID_CNFWEPFLAGS, HFA384x_WEPFLAGS_PRIVINVOKED, 0,
           prism2mib_privacyinvoked },
     { DIDmib_dot11smt_dot11PrivacyTable_dot11WEPDefaultKeyID,
-          F_AP | F_STA | F_READ | F_WRITE,
+          F_STA | F_READ | F_WRITE,
           HFA384x_RID_CNFWEPDEFAULTKEYID, 0, 0,
           prism2mib_uint32 },
     { DIDmib_dot11smt_dot11PrivacyTable_dot11ExcludeUnencrypted,
-          F_AP | F_STA | F_READ | F_WRITE,
+          F_STA | F_READ | F_WRITE,
           HFA384x_RID_CNFWEPFLAGS, HFA384x_WEPFLAGS_EXCLUDE, 0,
           prism2mib_excludeunencrypted },
-    { DIDmib_dot11phy_dot11PhyOperationTable_dot11ShortPreambleEnabled,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFSHORTPREAMBLE, 0, 0,
-          prism2mib_preamble },
 
     /* dot11mac MIB's */
 
     { DIDmib_dot11mac_dot11OperationTable_dot11MACAddress,
-          F_AP | F_STA | F_READ | F_WRITE,
+          F_STA | F_READ | F_WRITE,
           HFA384x_RID_CNFOWNMACADDR, HFA384x_RID_CNFOWNMACADDR_LEN, 0,
           prism2mib_bytearea2pstr },
     { DIDmib_dot11mac_dot11OperationTable_dot11RTSThreshold,
           F_STA | F_READ | F_WRITE,
           HFA384x_RID_RTSTHRESH, 0, 0,
           prism2mib_uint32 },
-    { DIDmib_dot11mac_dot11OperationTable_dot11RTSThreshold,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH0, 0, 0,
-          prism2mib_uint32 },
     { DIDmib_dot11mac_dot11OperationTable_dot11ShortRetryLimit,
-          F_AP | F_STA | F_READ,
+          F_STA | F_READ,
           HFA384x_RID_SHORTRETRYLIMIT, 0, 0,
           prism2mib_uint32 },
     { DIDmib_dot11mac_dot11OperationTable_dot11LongRetryLimit,
-          F_AP | F_STA | F_READ,
+          F_STA | F_READ,
           HFA384x_RID_LONGRETRYLIMIT, 0, 0,
           prism2mib_uint32 },
     { DIDmib_dot11mac_dot11OperationTable_dot11FragmentationThreshold,
           F_STA | F_READ | F_WRITE,
           HFA384x_RID_FRAGTHRESH, 0, 0,
           prism2mib_fragmentationthreshold },
-    { DIDmib_dot11mac_dot11OperationTable_dot11FragmentationThreshold,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH0, 0, 0,
-          prism2mib_fragmentationthreshold },
     { DIDmib_dot11mac_dot11OperationTable_dot11MaxTransmitMSDULifetime,
-          F_AP | F_STA | F_READ,
+          F_STA | F_READ,
           HFA384x_RID_MAXTXLIFETIME, 0, 0,
           prism2mib_uint32 },
-    { DIDmib_dot11mac_dot11OperationTable_dot11MaxReceiveLifetime,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_MAXRXLIFETIME, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address1,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address2,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address3,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address4,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address5,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address6,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address7,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address8,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address9,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address10,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address11,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address12,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address13,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address14,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address15,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address16,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address17,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address18,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address19,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address20,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address21,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address22,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address23,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address24,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address25,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address26,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address27,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address28,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address29,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address30,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address31,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address32,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
 
     /* dot11phy MIB's */
 
-    { DIDmib_dot11phy_dot11PhyOperationTable_dot11PHYType,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PHYTYPE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11phy_dot11PhyOperationTable_dot11TempType,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_TEMPTYPE, 0, 0,
-          prism2mib_uint32 },
     { DIDmib_dot11phy_dot11PhyDSSSTable_dot11CurrentChannel,
           F_STA | F_READ,
           HFA384x_RID_CURRENTCHANNEL, 0, 0,
           prism2mib_uint32 },
-    { DIDmib_dot11phy_dot11PhyDSSSTable_dot11CurrentChannel,
-          F_AP | F_READ,
-          HFA384x_RID_CNFOWNCHANNEL, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11phy_dot11PhyDSSSTable_dot11CurrentCCAMode,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_CCAMODE, 0, 0,
+    { DIDmib_dot11phy_dot11PhyTxPowerTable_dot11CurrentTxPowerLevel,
+          F_STA | F_READ | F_WRITE,
+          HFA384x_RID_TXPOWERMAX, 0, 0,
           prism2mib_uint32 },
 
-    /* p2Table MIB's */
-
-    { DIDmib_p2_p2Table_p2MMTx,
-          F_AP | F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2EarlyBeacon,
-          F_AP | F_READ | F_WRITE,
-          BIT7, 0, 0,
-          prism2mib_appcfinfoflag },
-    { DIDmib_p2_p2Table_p2ReceivedFrameStatistics,
-          F_AP | F_STA | F_READ,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2CommunicationTallies,
-          F_AP | F_STA | F_READ,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2Authenticated,
-          F_AP | F_READ,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2Associated,
-          F_AP | F_READ,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2PowerSaveUserCount,
-          F_AP | F_READ,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2Comment,
-          F_AP | F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2AccessMode,
-          F_AP | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2AccessAllow,
-          F_AP | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2AccessDeny,
-          F_AP | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2ChannelInfoResults,
-          F_AP | F_READ,
-          0, 0, 0,
-          prism2mib_priv },
-
     /* p2Static MIB's */
 
     { DIDmib_p2_p2Static_p2CnfPortType,
           F_STA | F_READ | F_WRITE,
           HFA384x_RID_CNFPORTTYPE, 0, 0,
           prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfOwnMACAddress,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNMACADDR, HFA384x_RID_CNFOWNMACADDR_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfDesiredSSID,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFDESIREDSSID, HFA384x_RID_CNFDESIREDSSID_LEN, 0,
-          prism2mib_bytestr2pstr },
-    { DIDmib_p2_p2Static_p2CnfOwnChannel,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNCHANNEL, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfOwnSSID,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNSSID, HFA384x_RID_CNFOWNSSID_LEN, 0,
-          prism2mib_bytestr2pstr },
-    { DIDmib_p2_p2Static_p2CnfOwnATIMWindow,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNATIMWIN, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfSystemScale,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFSYSSCALE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfMaxDataLength,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFMAXDATALEN, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfWDSAddress,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFWDSADDR, HFA384x_RID_CNFWDSADDR_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfPMEnabled,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFPMENABLED, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Static_p2CnfPMEPS,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFPMEPS, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Static_p2CnfMulticastReceive,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFMULTICASTRX, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Static_p2CnfMaxSleepDuration,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFMAXSLEEPDUR, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfPMHoldoverDuration,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFPMHOLDDUR, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfOwnName,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNNAME, HFA384x_RID_CNFOWNNAME_LEN, 0,
-          prism2mib_bytestr2pstr },
-    { DIDmib_p2_p2Static_p2CnfOwnDTIMPeriod,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNDTIMPER, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfWDSAddress1,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFWDSADDR1, HFA384x_RID_CNFWDSADDR1_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfWDSAddress2,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFWDSADDR2, HFA384x_RID_CNFWDSADDR2_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfWDSAddress3,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFWDSADDR3, HFA384x_RID_CNFWDSADDR3_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfWDSAddress4,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFWDSADDR4, HFA384x_RID_CNFWDSADDR4_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfWDSAddress5,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFWDSADDR5, HFA384x_RID_CNFWDSADDR5_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfWDSAddress6,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFWDSADDR6, HFA384x_RID_CNFWDSADDR6_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfMulticastPMBuffering,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFMCASTPMBUFF, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Static_p2CnfWEPDefaultKeyID,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFWEPDEFAULTKEYID, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfWEPDefaultKey0,
-          F_AP | F_STA | F_WRITE,
-          HFA384x_RID_CNFWEPDEFAULTKEY0, 0, 0,
-          prism2mib_wepdefaultkey },
-    { DIDmib_p2_p2Static_p2CnfWEPDefaultKey1,
-          F_AP | F_STA | F_WRITE,
-          HFA384x_RID_CNFWEPDEFAULTKEY1, 0, 0,
-          prism2mib_wepdefaultkey },
-    { DIDmib_p2_p2Static_p2CnfWEPDefaultKey2,
-          F_AP | F_STA | F_WRITE,
-          HFA384x_RID_CNFWEPDEFAULTKEY2, 0, 0,
-          prism2mib_wepdefaultkey },
-    { DIDmib_p2_p2Static_p2CnfWEPDefaultKey3,
-          F_AP | F_STA | F_WRITE,
-          HFA384x_RID_CNFWEPDEFAULTKEY3, 0, 0,
-          prism2mib_wepdefaultkey },
-    { DIDmib_p2_p2Static_p2CnfWEPFlags,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFWEPFLAGS, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfAuthentication,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFAUTHENTICATION, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfMaxAssociatedStations,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFMAXASSOCSTATIONS, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfTxControl,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFTXCONTROL, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfRoamingMode,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFROAMINGMODE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfHostAuthentication,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFHOSTAUTHASSOC, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Static_p2CnfRcvCrcError,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFRCVCRCERROR, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfAltRetryCount,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFALTRETRYCNT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfBeaconInterval,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPBCNINT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfMediumOccupancyLimit,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPPCFINFO, HFA384x_RID_CNFAPPCFINFO_LEN, 0,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2Static_p2CnfCFPPeriod,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPPCFINFO, HFA384x_RID_CNFAPPCFINFO_LEN, 1,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2Static_p2CnfCFPMaxDuration,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPPCFINFO, HFA384x_RID_CNFAPPCFINFO_LEN, 2,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2Static_p2CnfCFPFlags,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPPCFINFO, HFA384x_RID_CNFAPPCFINFO_LEN, 3,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2Static_p2CnfSTAPCFInfo,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFSTAPCFINFO, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfPriorityQUsage,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFPRIORITYQUSAGE, HFA384x_RID_CNFPRIOQUSAGE_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2Static_p2CnfTIMCtrl,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFTIMCTRL, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfThirty2Tally,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFTHIRTY2TALLY, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Static_p2CnfEnhSecurity,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFENHSECURITY, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfShortPreamble,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFSHORTPREAMBLE, 0, 0,
-          prism2mib_preamble },
-    { DIDmib_p2_p2Static_p2CnfExcludeLongPreamble,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFEXCLONGPREAMBLE, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Static_p2CnfAuthenticationRspTO,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFAUTHRSPTIMEOUT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfBasicRates,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFBASICRATES, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfSupportedRates,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFSUPPRATES, 0, 0,
-          prism2mib_uint32 },
-
-    /* p2Dynamic MIB's */
-
-    { DIDmib_p2_p2Dynamic_p2CreateIBSS,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CREATEIBSS, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2PromiscuousMode,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_PROMISCMODE, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold0,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH0, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold1,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH1, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold2,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH2, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold3,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH3, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold4,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH4, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold5,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH5, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold6,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH6, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold0,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH0, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold1,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH1, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold2,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH2, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold3,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH3, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold4,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH4, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold5,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH5, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold6,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH6, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl0,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL0, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl1,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL1, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl2,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL2, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl3,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL3, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl4,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL4, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl5,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL5, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl6,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL6, 0, 0,
-          prism2mib_uint32 },
-
-    /* p2Behavior MIB's */
-
-    { DIDmib_p2_p2Behavior_p2TickTime,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_ITICKTIME, 0, 0,
-          prism2mib_uint32 },
-
-    /* p2NIC MIB's */
-
-    { DIDmib_p2_p2NIC_p2MaxLoadTime,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_MAXLOADTIME, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2NIC_p2DLBufferPage,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_DOWNLOADBUFFER, HFA384x_RID_DOWNLOADBUFFER_LEN, 0,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2NIC_p2DLBufferOffset,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_DOWNLOADBUFFER, HFA384x_RID_DOWNLOADBUFFER_LEN, 1,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2NIC_p2DLBufferLength,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_DOWNLOADBUFFER, HFA384x_RID_DOWNLOADBUFFER_LEN, 2,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2NIC_p2PRIIdentity,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PRIIDENTITY, HFA384x_RID_PRIIDENTITY_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2PRISupRange,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PRISUPRANGE, HFA384x_RID_PRISUPRANGE_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2CFIActRanges,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PRI_CFIACTRANGES, HFA384x_RID_CFIACTRANGES_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2BuildSequence,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_BUILDSEQ, HFA384x_RID_BUILDSEQ_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2PrimaryFWID,
-          F_AP | F_STA | F_READ,
-          0, 0, 0,
-          prism2mib_fwid },
-    { DIDmib_p2_p2NIC_p2SecondaryFWID,
-          F_AP | F_STA | F_READ,
-          0, 0, 0,
-          prism2mib_fwid },
-    { DIDmib_p2_p2NIC_p2TertiaryFWID,
-          F_AP | F_READ,
-          0, 0, 0,
-          prism2mib_fwid },
-    { DIDmib_p2_p2NIC_p2NICSerialNumber,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_NICSERIALNUMBER, HFA384x_RID_NICSERIALNUMBER_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2NIC_p2NICIdentity,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_NICIDENTITY, HFA384x_RID_NICIDENTITY_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2MFISupRange,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_MFISUPRANGE, HFA384x_RID_MFISUPRANGE_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2CFISupRange,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_CFISUPRANGE, HFA384x_RID_CFISUPRANGE_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2ChannelList,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_CHANNELLIST, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2NIC_p2RegulatoryDomains,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_REGULATORYDOMAINS, HFA384x_RID_REGULATORYDOMAINS_LEN, 0,
-          prism2mib_regulatorydomains },
-    { DIDmib_p2_p2NIC_p2TempType,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_TEMPTYPE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2NIC_p2STAIdentity,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_STAIDENTITY, HFA384x_RID_STAIDENTITY_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2STASupRange,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_STASUPRANGE, HFA384x_RID_STASUPRANGE_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2MFIActRanges,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_STA_MFIACTRANGES, HFA384x_RID_MFIACTRANGES_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2STACFIActRanges,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_STA_CFIACTRANGES, HFA384x_RID_CFIACTRANGES2_LEN, 0,
-          prism2mib_uint32array },
 
     /* p2MAC MIB's */
 
-    { DIDmib_p2_p2MAC_p2PortStatus,
-          F_STA | F_READ,
-          HFA384x_RID_PORTSTATUS, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentSSID,
-          F_STA | F_READ,
-          HFA384x_RID_CURRENTSSID, HFA384x_RID_CURRENTSSID_LEN, 0,
-          prism2mib_bytestr2pstr },
-    { DIDmib_p2_p2MAC_p2CurrentBSSID,
-          F_STA | F_READ,
-          HFA384x_RID_CURRENTBSSID, HFA384x_RID_CURRENTBSSID_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2MAC_p2CommsQuality,
-          F_STA | F_READ,
-          HFA384x_RID_COMMSQUALITY, HFA384x_RID_COMMSQUALITY_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2MAC_p2CommsQualityCQ,
-          F_STA | F_READ,
-          HFA384x_RID_COMMSQUALITY, HFA384x_RID_COMMSQUALITY_LEN, 0,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2MAC_p2CommsQualityASL,
-          F_STA | F_READ,
-          HFA384x_RID_COMMSQUALITY, HFA384x_RID_COMMSQUALITY_LEN, 1,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2MAC_p2CommsQualityANL,
-          F_STA | F_READ,
-          HFA384x_RID_COMMSQUALITY, HFA384x_RID_COMMSQUALITY_LEN, 2,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2MAC_p2dbmCommsQuality,
-          F_STA | F_READ,
-          HFA384x_RID_DBMCOMMSQUALITY, HFA384x_RID_DBMCOMMSQUALITY_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2MAC_p2dbmCommsQualityCQ,
-          F_STA | F_READ,
-          HFA384x_RID_COMMSQUALITY, HFA384x_RID_COMMSQUALITY_LEN, 0,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2MAC_p2dbmCommsQualityASL,
-          F_STA | F_READ,
-          HFA384x_RID_COMMSQUALITY, HFA384x_RID_COMMSQUALITY_LEN, 1,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2MAC_p2dbmCommsQualityANL,
-          F_STA | F_READ,
-          HFA384x_RID_COMMSQUALITY, HFA384x_RID_COMMSQUALITY_LEN, 2,
-          prism2mib_uint32offset },
     { DIDmib_p2_p2MAC_p2CurrentTxRate,
           F_STA | F_READ,
           HFA384x_RID_CURRENTTXRATE, 0, 0,
           prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentBeaconInterval,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_CURRENTBCNINT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2StaCurrentScaleThresholds,
-          F_STA | F_READ,
-          HFA384x_RID_CURRENTSCALETHRESH, HFA384x_RID_STACURSCALETHRESH_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2MAC_p2APCurrentScaleThresholds,
-          F_AP | F_READ,
-          HFA384x_RID_CURRENTSCALETHRESH, HFA384x_RID_APCURSCALETHRESH_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2MAC_p2ProtocolRspTime,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PROTOCOLRSPTIME, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2ShortRetryLimit,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_SHORTRETRYLIMIT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2LongRetryLimit,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_LONGRETRYLIMIT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2MaxTransmitLifetime,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_MAXTXLIFETIME, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2MaxReceiveLifetime,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_MAXRXLIFETIME, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CFPollable,
-          F_STA | F_READ,
-          HFA384x_RID_CFPOLLABLE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2AuthenticationAlgorithms,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_AUTHALGORITHMS, HFA384x_RID_AUTHALGORITHMS_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2MAC_p2PrivacyOptionImplemented,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PRIVACYOPTIMP, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentTxRate1,
-          F_AP | F_READ,
-          HFA384x_RID_CURRENTTXRATE1, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentTxRate2,
-          F_AP | F_READ,
-          HFA384x_RID_CURRENTTXRATE2, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentTxRate3,
-          F_AP | F_READ,
-          HFA384x_RID_CURRENTTXRATE3, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentTxRate4,
-          F_AP | F_READ,
-          HFA384x_RID_CURRENTTXRATE4, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentTxRate5,
-          F_AP | F_READ,
-          HFA384x_RID_CURRENTTXRATE5, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentTxRate6,
-          F_AP | F_READ,
-          HFA384x_RID_CURRENTTXRATE6, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2OwnMACAddress,
-          F_AP | F_READ,
-          HFA384x_RID_OWNMACADDRESS, HFA384x_RID_OWNMACADDRESS_LEN, 0,
-          prism2mib_bytearea2pstr },
-
-    /* p2Modem MIB's */
-
-    { DIDmib_p2_p2Modem_p2PHYType,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PHYTYPE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Modem_p2CurrentChannel,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_CURRENTCHANNEL, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Modem_p2CurrentPowerState,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_CURRENTPOWERSTATE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Modem_p2CCAMode,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_CCAMODE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Modem_p2TxPowerMax,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_TXPOWERMAX, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11phy_dot11PhyTxPowerTable_dot11CurrentTxPowerLevel,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_TXPOWERMAX, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Modem_p2SupportedDataRates,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_SUPPORTEDDATARATES, HFA384x_RID_SUPPORTEDDATARATES_LEN, 0,
-          prism2mib_bytestr2pstr },
 
     /* And finally, lnx mibs */
     { DIDmib_lnx_lnxConfigTable_lnxRSNAIE,
@@ -1285,94 +270,6 @@ static mibrec_t mibtab[] = {
           prism2mib_priv },
     { 0, 0, 0, 0, 0, NULL}};
 
-/*----------------------------------------------------------------
-These MIB's are not supported at this time:
-
-DIDmib_dot11phy_dot11PhyOperationTable_dot11ChannelAgilityPresent
-DIDmib_dot11phy_dot11PhyOperationTable_dot11ChannelAgilityEnabled
-DIDmib_dot11phy_dot11PhyDSSSTable_dot11PBCCOptionImplemented
-DIDmib_dot11phy_dot11RegDomainsSupportedTable_dot11RegDomainsSupportIndex
-DIDmib_dot11phy_dot11SupportedDataRatesTxTable_dot11SupportedDataRatesTxIndex
-DIDmib_dot11phy_dot11SupportedDataRatesTxTable_dot11SupportedDataRatesTxValue
-DIDmib_dot11phy_dot11SupportedDataRatesRxTable_dot11SupportedDataRatesRxIndex
-DIDmib_dot11phy_dot11SupportedDataRatesRxTable_dot11SupportedDataRatesRxValue
-
-DIDmib_dot11phy_dot11RegDomainsSupportedTable_dot11RegDomainsSupportValue
-TODO: need to investigate why wlan has this as enumerated and Prism2 has this
-      as btye str.
-
-DIDmib_dot11phy_dot11PhyDSSSTable_dot11ShortPreambleOptionImplemented
-TODO: Find out the firmware version number(s) for identifying
-      whether the firmware is capable of short preamble. TRUE or FALSE
-      will be returned based on the version of the firmware.
-
-WEP Key mappings aren't supported in the f/w.
-DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingIndex
-DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingAddress
-DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingWEPOn
-DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingValue
-DIDmib_dot11smt_dot11PrivacyTable_dot11WEPKeyMappingLength
-
-TODO: implement counters.
-DIDmib_dot11smt_dot11PrivacyTable_dot11WEPICVErrorCount
-DIDmib_dot11smt_dot11PrivacyTable_dot11WEPExcludedCount
-DIDmib_dot11mac_dot11CountersTable_dot11TransmittedFragmentCount
-DIDmib_dot11mac_dot11CountersTable_dot11MulticastTransmittedFrameCount
-DIDmib_dot11mac_dot11CountersTable_dot11FailedCount
-DIDmib_dot11mac_dot11CountersTable_dot11RetryCount
-DIDmib_dot11mac_dot11CountersTable_dot11MultipleRetryCount
-DIDmib_dot11mac_dot11CountersTable_dot11FrameDuplicateCount
-DIDmib_dot11mac_dot11CountersTable_dot11RTSSuccessCount
-DIDmib_dot11mac_dot11CountersTable_dot11RTSFailureCount
-DIDmib_dot11mac_dot11CountersTable_dot11ACKFailureCount
-DIDmib_dot11mac_dot11CountersTable_dot11ReceivedFragmentCount
-DIDmib_dot11mac_dot11CountersTable_dot11MulticastReceivedFrameCount
-DIDmib_dot11mac_dot11CountersTable_dot11FCSErrorCount
-DIDmib_dot11mac_dot11CountersTable_dot11TransmittedFrameCount
-DIDmib_dot11mac_dot11CountersTable_dot11WEPUndecryptableCount
-
-TODO: implement sane values for these.
-DIDmib_dot11mac_dot11OperationTable_dot11ManufacturerID
-DIDmib_dot11mac_dot11OperationTable_dot11ProductID
-
-Not too worried about these at the moment.
-DIDmib_dot11phy_dot11PhyAntennaTable_dot11CurrentTxAntenna
-DIDmib_dot11phy_dot11PhyAntennaTable_dot11DiversitySupport
-DIDmib_dot11phy_dot11PhyAntennaTable_dot11CurrentRxAntenna
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11NumberSupportedPowerLevels
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel1
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel2
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel3
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel4
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel5
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel6
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel7
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel8
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11CurrentTxPowerLevel
-
-Ummm, FH and IR don't apply
-DIDmib_dot11phy_dot11PhyFHSSTable_dot11HopTime
-DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentChannelNumber
-DIDmib_dot11phy_dot11PhyFHSSTable_dot11MaxDwellTime
-DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentDwellTime
-DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentSet
-DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentPattern
-DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentIndex
-DIDmib_dot11phy_dot11PhyDSSSTable_dot11CCAModeSupported
-DIDmib_dot11phy_dot11PhyDSSSTable_dot11EDThreshold
-DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogTimerMax
-DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogCountMax
-DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogTimerMin
-DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogCountMin
-
-We just don't have enough antennas right now to worry about this.
-DIDmib_dot11phy_dot11AntennasListTable_dot11AntennaListIndex
-DIDmib_dot11phy_dot11AntennasListTable_dot11SupportedTxAntenna
-DIDmib_dot11phy_dot11AntennasListTable_dot11SupportedRxAntenna
-DIDmib_dot11phy_dot11AntennasListTable_dot11DiversitySelectionRx
-
-------------------------------------------------------------------*/
-
 /*================================================================*/
 /* Function Definitions */
 
@@ -1401,7 +298,8 @@ int prism2mgmt_mibset_mibget(wlandevice_t *wlandev, void *msgp)
 	hfa384x_t		*hw = wlandev->priv;
 	int			result, isget;
 	mibrec_t		*mib;
-	UINT16			which;
+
+	u16			which;
 
 	p80211msg_dot11req_mibset_t	*msg = msgp;
 	p80211itemd_t			*mibitem;
@@ -1415,7 +313,7 @@ int prism2mgmt_mibset_mibget(wlandevice_t *wlandev, void *msgp)
 	** Determine if this is an Access Point or a station.
 	*/
 
-	which = hw->ap ? F_AP : F_STA;
+	which = F_STA;
 
 	/*
 	** Find the MIB in the MIB table.  Note that a MIB may be in the
@@ -1491,59 +389,6 @@ done:
 }
 
 /*----------------------------------------------------------------
-* prism2mib_bytestr2pstr
-*
-* Get/set pstr data to/from a byte string.
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Number of bytes of RID data.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_bytestr2pstr(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int                result;
-	p80211pstrd_t      *pstr = (p80211pstrd_t*) data;
-	UINT8              bytebuf[MIB_TMP_MAXLEN];
-	hfa384x_bytestr_t  *p2bytestr = (hfa384x_bytestr_t*) bytebuf;
-
-	DBFENTER;
-
-	if (isget) {
-		result = hfa384x_drvr_getconfig(hw, mib->parm1, bytebuf, mib->parm2);
-		prism2mgmt_bytestr2pstr(p2bytestr, pstr);
-	} else {
-		memset(bytebuf, 0, mib->parm2);
-		prism2mgmt_pstr2bytestr(p2bytestr, pstr);
-		result = hfa384x_drvr_setconfig(hw, mib->parm1, bytebuf, mib->parm2);
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
 * prism2mib_bytearea2pstr
 *
 * Get/set pstr data to/from a byte area.
@@ -1578,13 +423,13 @@ void                         *data)
 {
 	int            result;
 	p80211pstrd_t  *pstr = (p80211pstrd_t*) data;
-	UINT8          bytebuf[MIB_TMP_MAXLEN];
+	u8          bytebuf[MIB_TMP_MAXLEN];
 
 	DBFENTER;
 
 	if (isget) {
 		result = hfa384x_drvr_getconfig(hw, mib->parm1, bytebuf, mib->parm2);
-		prism2mgmt_bytearea2pstr(bytebuf, pstr,	mib->parm2);
+		prism2mgmt_bytearea2pstr(bytebuf, pstr, mib->parm2);
 	} else {
 		memset(bytebuf, 0, mib->parm2);
 		prism2mgmt_pstr2bytearea(bytebuf, pstr);
@@ -1629,9 +474,9 @@ p80211msg_dot11req_mibset_t  *msg,
 void                         *data)
 {
 	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-	UINT8   bytebuf[MIB_TMP_MAXLEN];
-	UINT16  *wordbuf = (UINT16*) bytebuf;
+	u32  *uint32 = (u32*) data;
+	u8   bytebuf[MIB_TMP_MAXLEN];
+	u16  *wordbuf = (u16*) bytebuf;
 
 	DBFENTER;
 
@@ -1654,178 +499,6 @@ void                         *data)
 }
 
 /*----------------------------------------------------------------
-* prism2mib_uint32array
-*
-* Get/set an array of uint32 data.
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Number of bytes of RID data.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_uint32array(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int     result;
-	UINT32  *uint32 = (UINT32 *) data;
-	UINT8   bytebuf[MIB_TMP_MAXLEN];
-	UINT16  *wordbuf = (UINT16*) bytebuf;
-	int     i, cnt;
-
-	DBFENTER;
-
-	cnt = mib->parm2 / sizeof(UINT16);
-
-	if (isget) {
-		result = hfa384x_drvr_getconfig(hw, mib->parm1, wordbuf, mib->parm2);
-		for (i = 0; i < cnt; i++)
-			prism2mgmt_prism2int2p80211int(wordbuf+i, uint32+i);
-	} else {
-		for (i = 0; i < cnt; i++)
-			prism2mgmt_p80211int2prism2int(wordbuf+i, uint32+i);
-		result = hfa384x_drvr_setconfig(hw, mib->parm1, wordbuf, mib->parm2);
-		}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_uint32offset
-*
-* Get/set a single element in an array of uint32 data.
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Number of bytes of RID data.
-*       parm3    Element index.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_uint32offset(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-	UINT8   bytebuf[MIB_TMP_MAXLEN];
-	UINT16  *wordbuf = (UINT16*) bytebuf;
-	UINT16  cnt;
-
-	DBFENTER;
-
-	cnt = mib->parm2 / sizeof(UINT16);
-
-	result = hfa384x_drvr_getconfig(hw, mib->parm1, wordbuf, mib->parm2);
-	if (result == 0) {
-		if (isget) {
-			if (mib->parm3 < cnt)
-				prism2mgmt_prism2int2p80211int(wordbuf+mib->parm3, uint32);
-			else
-				*uint32 = 0;
-		} else {
-			if (mib->parm3 < cnt) {
-				prism2mgmt_p80211int2prism2int(wordbuf+mib->parm3, uint32);
-				result = hfa384x_drvr_setconfig(hw, mib->parm1, wordbuf, mib->parm2);
-			}
-		}
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_truth
-*
-* Get/set truth data.
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_truth(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-	UINT8   bytebuf[MIB_TMP_MAXLEN];
-	UINT16  *wordbuf = (UINT16*) bytebuf;
-
-	DBFENTER;
-
-	if (isget) {
-		result = hfa384x_drvr_getconfig16(hw, mib->parm1, wordbuf);
-		*uint32 = (*wordbuf) ?
-				P80211ENUM_truth_true : P80211ENUM_truth_false;
-	} else {
-		*wordbuf = ((*uint32) == P80211ENUM_truth_true) ? 1 : 0;
-		result = hfa384x_drvr_setconfig16(hw, mib->parm1, *wordbuf);
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
 * prism2mib_flag
 *
 * Get/set a flag.
@@ -1859,10 +532,10 @@ p80211msg_dot11req_mibset_t  *msg,
 void                         *data)
 {
 	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-	UINT8   bytebuf[MIB_TMP_MAXLEN];
-	UINT16  *wordbuf = (UINT16*) bytebuf;
-	UINT32  flags;
+	u32  *uint32 = (u32*) data;
+	u8   bytebuf[MIB_TMP_MAXLEN];
+	u16  *wordbuf = (u16*) bytebuf;
+	u32  flags;
 
 	DBFENTER;
 
@@ -1893,121 +566,6 @@ void                         *data)
 }
 
 /*----------------------------------------------------------------
-* prism2mib_appcfinfoflag
-*
-* Get/set a single flag in the APPCFINFO record.
-*
-* MIB record parameters:
-*       parm1    Bit to get/set.
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_appcfinfoflag(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-	UINT8   bytebuf[MIB_TMP_MAXLEN];
-	UINT16  *wordbuf = (UINT16*) bytebuf;
-	UINT16  word;
-
-	DBFENTER;
-
-	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_CNFAPPCFINFO,
-					bytebuf, HFA384x_RID_CNFAPPCFINFO_LEN);
-	if (result == 0) {
-		if (isget) {
-			*uint32 = (hfa384x2host_16(wordbuf[3]) & mib->parm1) ?
-				P80211ENUM_truth_true : P80211ENUM_truth_false;
-		} else {
-			word = hfa384x2host_16(wordbuf[3]);
-			word = ((*uint32) == P80211ENUM_truth_true) ?
-				(word | mib->parm1) : (word & ~mib->parm1);
-			wordbuf[3] = host2hfa384x_16(word);
-			result = hfa384x_drvr_setconfig(hw, HFA384x_RID_CNFAPPCFINFO,
-					bytebuf, HFA384x_RID_CNFAPPCFINFO_LEN);
-		}
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_regulatorydomains
-*
-* Get regulatory domain data.
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Number of bytes of RID data.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_regulatorydomains(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int            result;
-	UINT32         cnt;
-	p80211pstrd_t  *pstr = (p80211pstrd_t*) data;
-	UINT8          bytebuf[MIB_TMP_MAXLEN];
-	UINT16         *wordbuf = (UINT16*) bytebuf;
-
-	DBFENTER;
-
-	result = 0;
-
-	if (isget) {
-		result = hfa384x_drvr_getconfig(hw, mib->parm1, wordbuf, mib->parm2);
-		prism2mgmt_prism2int2p80211int(wordbuf, &cnt);
-		pstr->len = (UINT8) cnt;
-		memcpy(pstr->data, &wordbuf[1], pstr->len);
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
 * prism2mib_wepdefaultkey
 *
 * Get/set WEP default keys.
@@ -2042,8 +600,8 @@ void                         *data)
 {
 	int            result;
 	p80211pstrd_t  *pstr = (p80211pstrd_t*) data;
-	UINT8          bytebuf[MIB_TMP_MAXLEN];
-	UINT16         len;
+	u8          bytebuf[MIB_TMP_MAXLEN];
+	u16         len;
 
 	DBFENTER;
 
@@ -2062,114 +620,6 @@ void                         *data)
 }
 
 /*----------------------------------------------------------------
-* prism2mib_powermanagement
-*
-* Get/set 802.11 power management value.  Note that this is defined differently
-* by 802.11 and Prism2:
-*
-*       Meaning     802.11       Prism2
-*        active       1           false
-*      powersave      2           true
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_powermanagement(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-	UINT32  value;
-
-	DBFENTER;
-
-	if (isget) {
-		result = prism2mib_uint32(mib, isget, wlandev, hw, msg, &value);
-		*uint32 = (value == 0) ? 1 : 2;
-	} else {
-		value = ((*uint32) == 1) ? 0 : 1;
-		result = prism2mib_uint32(mib, isget, wlandev, hw, msg, &value);
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_preamble
-*
-* Get/set Prism2 short preamble
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_preamble(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-	UINT8   bytebuf[MIB_TMP_MAXLEN];
-	UINT16  *wordbuf = (UINT16*) bytebuf;
-
-	DBFENTER;
-
-	if (isget) {
-		result = hfa384x_drvr_getconfig16(hw, mib->parm1, wordbuf);
-		*uint32 = *wordbuf;
-	} else {
-		*wordbuf = *uint32;
-		result = hfa384x_drvr_setconfig16(hw, mib->parm1, *wordbuf);
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
 * prism2mib_privacyinvoked
 *
 * Get/set the dot11PrivacyInvoked value.
@@ -2296,7 +746,7 @@ p80211msg_dot11req_mibset_t  *msg,
 void                         *data)
 {
 	int     result;
-	UINT32  *uint32 = (UINT32*) data;
+	u32  *uint32 = (u32*) data;
 
 	DBFENTER;
 
@@ -2315,349 +765,6 @@ void                         *data)
 }
 
 /*----------------------------------------------------------------
-* prism2mib_operationalrateset
-*
-* Get/set the operational rate set.
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_operationalrateset(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int            result;
-	p80211pstrd_t  *pstr = (p80211pstrd_t *) data;
-	UINT8          bytebuf[MIB_TMP_MAXLEN];
-	UINT16         *wordbuf = (UINT16*) bytebuf;
-
-	DBFENTER;
-
-	if (isget) {
-		result = hfa384x_drvr_getconfig16(hw, mib->parm1, wordbuf);
-		prism2mgmt_get_oprateset(wordbuf, pstr);
-	} else {
-		prism2mgmt_set_oprateset(wordbuf, pstr);
-		result = hfa384x_drvr_setconfig16(hw, mib->parm1, *wordbuf);
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFSUPPRATES, *wordbuf);
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_groupaddress
-*
-* Get/set the dot11GroupAddressesTable.
-*
-* MIB record parameters:
-*       parm1    Not used.
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_groupaddress(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int            result;
-	p80211pstrd_t  *pstr = (p80211pstrd_t *) data;
-	UINT8          bytebuf[MIB_TMP_MAXLEN];
-	UINT16         len;
-
-	DBFENTER;
-
-	/* TODO: fix this.  f/w doesn't support mcast filters */
-
-	if (isget) {
-		prism2mgmt_get_grpaddr(mib->did, pstr, hw);
-		return(0);
-	}
-
-	result = prism2mgmt_set_grpaddr(mib->did, bytebuf, pstr, hw);
-	if (result != 0) {
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-		return(result);
-	}
-
-	if (hw->dot11_grpcnt <= MAX_PRISM2_GRP_ADDR) {
-		len = hw->dot11_grpcnt * WLAN_ADDR_LEN;
-		memcpy(bytebuf, hw->dot11_grp_addr[0], len);
-		result = hfa384x_drvr_setconfig(hw, HFA384x_RID_GROUPADDR, bytebuf, len);
-
-		/*
-		** Turn off promiscuous mode if count is equal to MAX.  We may
-		** have been at a higher count in promiscuous mode and need to
-		** turn it off.
-		*/
-
-		/* but only if we're not already in promisc mode. :) */
-		if ((hw->dot11_grpcnt == MAX_PRISM2_GRP_ADDR) &&
-		    !( wlandev->netdev->flags & IFF_PROMISC)) {
-			result = hfa384x_drvr_setconfig16(hw,
-					     HFA384x_RID_PROMISCMODE, 0);
-		}
-	} else {
-
-		/*
-		** Clear group addresses in card and set to promiscuous mode.
-		*/
-
-		memset(bytebuf, 0, sizeof(bytebuf));
-		result = hfa384x_drvr_setconfig(hw, HFA384x_RID_GROUPADDR,
-						bytebuf, 0);
-		if (result == 0) {
-			result = hfa384x_drvr_setconfig16(hw,
-					HFA384x_RID_PROMISCMODE, 1);
-		}
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_fwid
-*
-* Get the firmware ID.
-*
-* MIB record parameters:
-*       parm1    Not used.
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_fwid(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int             result;
-	p80211pstrd_t   *pstr = (p80211pstrd_t *) data;
-	hfa384x_FWID_t  fwid;
-
-	DBFENTER;
-
-	if (isget) {
-		result = hfa384x_drvr_getconfig(hw, HFA384x_RID_FWID,
-						&fwid, HFA384x_RID_FWID_LEN);
-		if (mib->did == DIDmib_p2_p2NIC_p2PrimaryFWID) {
-			fwid.primary[HFA384x_FWID_LEN - 1] = '\0';
-			pstr->len = strlen(fwid.primary);
-			memcpy(pstr->data, fwid.primary, pstr->len);
-		} else {
-			fwid.secondary[HFA384x_FWID_LEN - 1] = '\0';
-			pstr->len = strlen(fwid.secondary);
-			memcpy(pstr->data, fwid.secondary, pstr->len);
-		}
-	} else
-		result = 0;     /* Should never happen. */
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_authalg
-*
-* Get values from the AuhtenticationAlgorithmsTable.
-*
-* MIB record parameters:
-*       parm1    Table index (1-6).
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_authalg(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	UINT32  *uint32 = (UINT32*) data;
-
-	DBFENTER;
-
-	/* MSM: pkx supplied code that  code queries RID FD4D....but the f/w's
-         *  results are bogus. Therefore, we have to simulate the appropriate
-         *  results here in the driver based on our knowledge of existing MAC
-         *  features.  That's the whole point behind this ugly function.
-         */
-
-	if (isget) {
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-		switch (mib->parm1) {
-			case 1: /* Open System */
-				*uint32 = P80211ENUM_authalg_opensystem;
-				break;
-			case 2: /* SharedKey */
-				*uint32 = P80211ENUM_authalg_sharedkey;
-				break;
-			default:
-				*uint32 = 0;
-				msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-				break;
-		}
-	}
-
-	DBFEXIT;
-	return(0);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_authalgenable
-*
-* Get/set the enable values from the AuhtenticationAlgorithmsTable.
-*
-* MIB record parameters:
-*       parm1    Table index (1-6).
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_authalgenable(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-
-	int     index;
-	UINT16  cnf_auth;
-	UINT16	mask;
-
-	DBFENTER;
-
-	index = mib->parm1 - 1;
-
-	result = hfa384x_drvr_getconfig16( hw,
-			HFA384x_RID_CNFAUTHENTICATION, &cnf_auth);
-	WLAN_LOG_DEBUG(2,"cnfAuthentication0=%d, index=%d\n", cnf_auth, index);
-
-	if (isget) {
-		if ( index == 0 || index == 1 ) {
-			*uint32 = (cnf_auth & (1<<index)) ?
-				P80211ENUM_truth_true: P80211ENUM_truth_false;
-		} else {
-			*uint32 = P80211ENUM_truth_false;
-			msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-		}
-	} else {
-		if ( index == 0 || index == 1 ) {
-			mask = 1 << index;
-			if (*uint32==P80211ENUM_truth_true ) {
-				cnf_auth |= mask;
-			} else {
-				cnf_auth &= ~mask;
-			}
-			result = hfa384x_drvr_setconfig16( hw,
-					HFA384x_RID_CNFAUTHENTICATION, cnf_auth);
-			WLAN_LOG_DEBUG(2,"cnfAuthentication:=%d\n", cnf_auth);
-			if ( result ) {
-				WLAN_LOG_DEBUG(1,"Unable to set p2cnfAuthentication to %d\n", cnf_auth);
-				msg->resultcode.data = P80211ENUM_resultcode_implementation_failure;
-			}
-		} else {
-			msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-		}
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
 * prism2mib_priv
 *
 * Get/set values in the "priv" data structure.
@@ -2690,218 +797,18 @@ hfa384x_t                    *hw,
 p80211msg_dot11req_mibset_t  *msg,
 void                         *data)
 {
-	UINT32            *uint32 = (UINT32*) data;
 	p80211pstrd_t     *pstr = (p80211pstrd_t*) data;
-	p80211macarray_t  *macarray = (p80211macarray_t *) data;
-
-	int  i, cnt, result, done;
-
-	prism2sta_authlist_t  old;
-
-	/*
-	** "test" is a lot longer than necessary but who cares?  ...as long as
-	** it is long enough!
-	*/
 
-	UINT8  test[sizeof(wlandev->rx) + sizeof(hw->tallies)];
+	int  result;
 
 	DBFENTER;
 
 	switch (mib->did) {
-	case DIDmib_p2_p2Table_p2ReceivedFrameStatistics:
-
-		/*
-		** Note: The values in this record are changed by the
-		** interrupt handler and therefore cannot be guaranteed
-		** to be stable while they are being copied.  However,
-		** the interrupt handler will take priority over this
-		** code.  Hence, if the same values are copied twice,
-		** then we are ensured that the values have not been
-		** changed.  If they have, then just try again.  Don't
-		** try more than 10 times...if we still haven't got it,
-		** then the values we do have are probably good enough.
-		** This scheme for copying values is used in order to
-		** prevent having to block the interrupt handler while
-		** we copy the values.
-		*/
-
-		if (isget)
-			for (i = 0; i < 10; i++) {
-				memcpy(data, &wlandev->rx, sizeof(wlandev->rx));
-				memcpy(test, &wlandev->rx, sizeof(wlandev->rx));
-				if (memcmp(data, test, sizeof(wlandev->rx)) == 0) break;
-			}
-
-		break;
-
-	case DIDmib_p2_p2Table_p2CommunicationTallies:
-
-		/*
-		** Note: The values in this record are changed by the
-		** interrupt handler and therefore cannot be guaranteed
-		** to be stable while they are being copied.  See the
-		** note above about copying values.
-		*/
-
-		if (isget) {
-			result = hfa384x_drvr_commtallies(hw);
-
-			/* ?????? We need to wait a bit here for the */
-			/*   tallies to get updated. ?????? */
-			/* MSM: TODO: The right way to do this is to
-			 *      add a "commtallie" wait queue to the
-			 *      priv structure that gets run every time
-			 *      we receive a commtally info frame.
-			 *      This process would sleep on that
-			 *      queue and get awakened when the
-			 *      the requested info frame arrives.
-			 *      Don't have time to do and test this
-			 *      right now.
-			 */
-
-			/* Ugh, this is nasty. */
-			for (i = 0; i < 10; i++) {
-				memcpy(data,
-				       &hw->tallies,
-				       sizeof(hw->tallies));
-				memcpy(test,
-				       &hw->tallies,
-				       sizeof(hw->tallies));
-				if ( memcmp(data,
-					    test,
-					    sizeof(hw->tallies)) == 0)
-					break;
-			}
-		}
-
-		break;
-
-	case DIDmib_p2_p2Table_p2Authenticated:
-
-		if (isget) {
-			prism2mib_priv_authlist(hw, &old);
-
-			macarray->cnt = 0;
-			for (i = 0; i < old.cnt; i++) {
-				if (!old.assoc[i]) {
-					memcpy(macarray->data[macarray->cnt], old.addr[i], WLAN_ADDR_LEN);
-					macarray->cnt++;
-				}
-			}
-		}
-
-		break;
-
-	case DIDmib_p2_p2Table_p2Associated:
-
-		if (isget) {
-			prism2mib_priv_authlist(hw, &old);
-
-			macarray->cnt = 0;
-			for (i = 0; i < old.cnt; i++) {
-				if (old.assoc[i]) {
-					memcpy(macarray->data[macarray->cnt], old.addr[i], WLAN_ADDR_LEN);
-					macarray->cnt++;
-				}
-			}
-		}
-
-		break;
-
-	case DIDmib_p2_p2Table_p2PowerSaveUserCount:
-
-		if (isget)
-			*uint32 = hw->psusercount;
-
-		break;
-
-	case DIDmib_p2_p2Table_p2Comment:
-
-		if (isget) {
-			pstr->len = strlen(hw->comment);
-			memcpy(pstr->data, hw->comment, pstr->len);
-		} else {
-			cnt = pstr->len;
-			if (cnt < 0) cnt = 0;
-			if (cnt >= sizeof(hw->comment))
-				cnt = sizeof(hw->comment)-1;
-			memcpy(hw->comment, pstr->data, cnt);
-			pstr->data[cnt] = '\0';
-		}
-
-		break;
-
-	case DIDmib_p2_p2Table_p2AccessMode:
-
-		if (isget)
-			*uint32 = hw->accessmode;
-		else
-			prism2mib_priv_accessmode(hw, *uint32);
-
-		break;
-
-	case DIDmib_p2_p2Table_p2AccessAllow:
-
-		if (isget) {
-			macarray->cnt = hw->allow.cnt;
-			memcpy(macarray->data, hw->allow.addr,
-			       macarray->cnt*WLAN_ADDR_LEN);
-		} else {
-			prism2mib_priv_accessallow(hw, macarray);
-		}
-
-		break;
-
-	case DIDmib_p2_p2Table_p2AccessDeny:
-
-		if (isget) {
-			macarray->cnt = hw->deny.cnt;
-			memcpy(macarray->data, hw->deny.addr,
-			       macarray->cnt*WLAN_ADDR_LEN);
-		} else {
-			prism2mib_priv_accessdeny(hw, macarray);
-		}
-
-		break;
-
-	case DIDmib_p2_p2Table_p2ChannelInfoResults:
-
-		if (isget) {
-			done = atomic_read(&hw->channel_info.done);
-			if (done == 0) {
-				msg->resultcode.status = P80211ENUM_msgitem_status_no_value;
-				break;
-			}
-			if (done == 1) {
-				msg->resultcode.status = P80211ENUM_msgitem_status_incomplete_itemdata;
-				break;
-			}
-
-			for (i = 0; i < 14; i++, uint32 += 5) {
-				uint32[0] = i+1;
-				uint32[1] = hw->channel_info.results.result[i].anl;
-				uint32[2] = hw->channel_info.results.result[i].pnl;
-				uint32[3] = (hw->channel_info.results.result[i].active & HFA384x_CHINFORESULT_BSSACTIVE) ? 1 : 0;
-				uint32[4] = (hw->channel_info.results.result[i].active & HFA384x_CHINFORESULT_PCFACTIVE) ? 1 : 0;
-			}
-		}
-
-		break;
-
-	case DIDmib_dot11smt_dot11StationConfigTable_dot11DesiredBSSType:
-
-		if (isget)
-			*uint32 = hw->dot11_desired_bss_type;
-		else
-			hw->dot11_desired_bss_type = *uint32;
-
-		break;
-
 	case DIDmib_lnx_lnxConfigTable_lnxRSNAIE: {
 		hfa384x_WPAData_t wpa;
 		if (isget) {
 			hfa384x_drvr_getconfig( hw, HFA384x_RID_CNFWPADATA,
-						(UINT8 *) &wpa, sizeof(wpa));
+						(u8 *) &wpa, sizeof(wpa));
 			pstr->len = hfa384x2host_16(wpa.datalen);
 			memcpy(pstr->data, wpa.data, pstr->len);
 		} else {
@@ -2909,7 +816,7 @@ void                         *data)
 			memcpy(wpa.data, pstr->data, pstr->len);
 
 			result = hfa384x_drvr_setconfig(hw, HFA384x_RID_CNFWPADATA,
-				(UINT8 *) &wpa, sizeof(wpa));
+				(u8 *) &wpa, sizeof(wpa));
 		}
 		break;
 	}
@@ -2922,345 +829,6 @@ void                         *data)
 }
 
 /*----------------------------------------------------------------
-* prism2mib_priv_authlist
-*
-* Get a copy of the list of authenticated stations.
-*
-* Arguments:
-*       priv     "priv" structure.
-*       list     List of authenticated stations.
-*
-* Returns:
-*	Nothing
-*
-----------------------------------------------------------------*/
-
-static void prism2mib_priv_authlist(
-hfa384x_t             *hw,
-prism2sta_authlist_t  *list)
-{
-	prism2sta_authlist_t  test;
-	int                   i;
-
-	DBFENTER;
-
-	/*
-	** Note: The values in this record are changed by the interrupt
-	** handler and therefore cannot be guaranteed to be stable while
-	** they are being copied.  However, the interrupt handler will
-	** take priority over this code.  Hence, if the same values are
-	** copied twice, then we are ensured that the values have not
-	** been changed.  If they have, then just try again.  Don't try
-	** more than 10 times...the list of authenticated stations is
-	** unlikely to be changing frequently enough that we can't get
-	** a snapshot in 10 tries.  Don't try more than this so that we
-	** don't risk locking-up for long periods of time.  If we still
-	** haven't got the snapshot, then generate an error message and
-	** return an empty list (since this is the only valid list that
-	** we can guarentee).  This scheme for copying values is used in
-	** order to prevent having to block the interrupt handler while
-	** we copy the values.
-	*/
-
-	for (i = 0; i < 10; i++) {
-		memcpy(list, &hw->authlist, sizeof(prism2sta_authlist_t));
-		memcpy(&test, &hw->authlist, sizeof(prism2sta_authlist_t));
-		if (memcmp(list, &test, sizeof(prism2sta_authlist_t)) == 0)
-			break;
-	}
-
-	if (i >= 10) {
-		list->cnt = 0;
-		WLAN_LOG_ERROR("Could not obtain snapshot of authenticated stations.\n");
-		}
-
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* prism2mib_priv_accessmode
-*
-* Set the Access Mode.
-*
-* Arguments:
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       mode     New access mode.
-*
-* Returns:
-*	Nothing
-*
-----------------------------------------------------------------*/
-
-static void prism2mib_priv_accessmode(
-hfa384x_t         *hw,
-UINT32            mode)
-{
-	prism2sta_authlist_t  old;
-	int                   i, j, deauth;
-	UINT8                 *addr;
-
-	DBFENTER;
-
-	/*
-	** If the mode is not changing or it is changing to "All", then it's
-	** okay to go ahead without a lot of messing around.  Otherwise, the
-	** access mode is changing in a way that may leave some stations
-	** authenticated which should not be authenticated.  It will be
-	** necessary to de-authenticate these stations.
-	*/
-
-	if (mode == WLAN_ACCESS_ALL || mode == hw->accessmode) {
-		hw->accessmode = mode;
-		return;
-	}
-
-	/*
-	** Switch to the new access mode.  Once this is done, then the interrupt
-	** handler (which uses this value) will be prevented from authenticating
-	** ADDITIONAL stations which should not be authenticated.  Then get a
-	** copy of the current list of authenticated stations.
-	*/
-
-	hw->accessmode = mode;
-
-	prism2mib_priv_authlist(hw, &old);
-
-	/*
-	** Now go through the list of previously authenticated stations (some
-	** of which might de-authenticate themselves while we are processing it
-	** but that is okay).  Any station which no longer matches the access
-	** mode, must be de-authenticated.
-	*/
-
-	for (i = 0; i < old.cnt; i++) {
-		addr = old.addr[i];
-
-		if (mode == WLAN_ACCESS_NONE)
-			deauth = 1;
-		else {
-			if (mode == WLAN_ACCESS_ALLOW) {
-				for (j = 0; j < hw->allow.cnt; j++)
-					if (memcmp(addr, hw->allow.addr[j],
-							WLAN_ADDR_LEN) == 0)
-						break;
-				deauth = (j >= hw->allow.cnt);
-			} else {
-				for (j = 0; j < hw->deny.cnt; j++)
-					if (memcmp(addr, hw->deny.addr[j],
-							WLAN_ADDR_LEN) == 0)
-						break;
-				deauth = (j < hw->deny.cnt);
-			}
-		}
-
-		if (deauth) prism2mib_priv_deauthenticate(hw, addr);
-	}
-
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* prism2mib_priv_accessallow
-*
-* Change the list of allowed MAC addresses.
-*
-* Arguments:
-*       priv      "priv" structure.
-*       hw        "hw" structure.
-*       macarray  New array of MAC addresses.
-*
-* Returns:
-*	Nothing
-*
-----------------------------------------------------------------*/
-
-static void prism2mib_priv_accessallow(
-hfa384x_t         *hw,
-p80211macarray_t  *macarray)
-{
-	prism2sta_authlist_t  old;
-	int                   i, j;
-
-	DBFENTER;
-
-	/*
-	** Change the access list.  Note that the interrupt handler may be in
-	** the middle of using the access list!!!  Since the interrupt handler
-	** will	always have priority over this process and this is the only
-	** process that will modify the list, this problem can be handled as
-	** follows:
-	**
-	**    1. Set the "modify" flag.
-	**    2. Change the first copy of the list.
-	**    3. Clear the "modify" flag.
-	**    4. Change the backup copy of the list.
-	**
-	** The interrupt handler will check the "modify" flag.  If NOT set, then
-	** the first copy of the list is valid and may be used.  Otherwise, the
-	** first copy is being changed but the backup copy is valid and may be
-	** used.  Doing things this way prevents having to have the interrupt
-	** handler block while the list is being updated.
-	*/
-
-	hw->allow.modify = 1;
-
-	hw->allow.cnt = macarray->cnt;
-	memcpy(hw->allow.addr, macarray->data, macarray->cnt*WLAN_ADDR_LEN);
-
-	hw->allow.modify = 0;
-
-	hw->allow.cnt1 = macarray->cnt;
-	memcpy(hw->allow.addr1, macarray->data, macarray->cnt*WLAN_ADDR_LEN);
-
-	/*
-	** If the current access mode is "Allow", then changing the access
-	** list may leave some stations authenticated which should not be
-	** authenticated.  It will be necessary to de-authenticate these
-	** stations.  Otherwise, the list can be changed without a lot of fuss.
-	*/
-
-	if (hw->accessmode == WLAN_ACCESS_ALLOW) {
-
-		/*
-		** Go through the list of authenticated stations (some of
-		** which might de-authenticate themselves while we are
-		** processing it but that is okay).  Any station which is
-		** no longer in the list of allowed stations, must be
-		** de-authenticated.
-		*/
-
-		prism2mib_priv_authlist(hw, &old);
-
-		for (i = 0; i < old.cnt; i++) {
-			for (j = 0; j < hw->allow.cnt; j++)
-				if (memcmp(old.addr[i], hw->allow.addr[j],
-							WLAN_ADDR_LEN) == 0)
-					break;
-			if (j >= hw->allow.cnt)
-				prism2mib_priv_deauthenticate(hw, old.addr[i]);
-		}
-	}
-
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* prism2mib_priv_accessdeny
-*
-* Change the list of denied MAC addresses.
-*
-* Arguments:
-*       priv      "priv" structure.
-*       hw        "hw" structure.
-*       macarray  New array of MAC addresses.
-*
-* Returns:
-*	Nothing
-*
-----------------------------------------------------------------*/
-
-static void prism2mib_priv_accessdeny(
-hfa384x_t         *hw,
-p80211macarray_t  *macarray)
-{
-	prism2sta_authlist_t  old;
-	int                   i, j;
-
-	DBFENTER;
-
-	/*
-	** Change the access list.  Note that the interrupt handler may be in
-	** the middle of using the access list!!!  Since the interrupt handler
-	** will always have priority over this process and this is the only
-	** process that will modify the list, this problem can be handled as
-	** follows:
-	**
-	**    1. Set the "modify" flag.
-	**    2. Change the first copy of the list.
-	**    3. Clear the "modify" flag.
-	**    4. Change the backup copy of the list.
-	**
-	** The interrupt handler will check the "modify" flag.  If NOT set, then
-	** the first copy of the list is valid and may be used.  Otherwise, the
-	** first copy is being changed but the backup copy is valid and may be
-	** used.  Doing things this way prevents having to have the interrupt
-	** handler block while the list is being updated.
-	*/
-
-	hw->deny.modify = 1;
-
-	hw->deny.cnt = macarray->cnt;
-	memcpy(hw->deny.addr, macarray->data, macarray->cnt*WLAN_ADDR_LEN);
-
-	hw->deny.modify = 0;
-
-	hw->deny.cnt1 = macarray->cnt;
-	memcpy(hw->deny.addr1, macarray->data, macarray->cnt*WLAN_ADDR_LEN);
-
-	/*
-	** If the current access mode is "Deny", then changing the access
-	** list may leave some stations authenticated which should not be
-	** authenticated.  It will be necessary to de-authenticate these
-	** stations.  Otherwise, the list can be changed without a lot of fuss.
-	*/
-
-	if (hw->accessmode == WLAN_ACCESS_DENY) {
-
-		/*
-		** Go through the list of authenticated stations (some of
-		** which might de-authenticate themselves while we are
-		** processing it but that is okay).  Any station which is
-		** now in the list of denied stations, must be de-authenticated.
-		*/
-
-		prism2mib_priv_authlist(hw, &old);
-
-		for (i = 0; i < old.cnt; i++)
-			for (j = 0; j < hw->deny.cnt; j++)
-				if (memcmp(old.addr[i], hw->deny.addr[j],
-							 WLAN_ADDR_LEN) == 0) {
-					prism2mib_priv_deauthenticate(hw, old.addr[i]);
-					break;
-				}
-	}
-
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* prism2mib_priv_deauthenticate
-*
-* De-authenticate a station.  This is done by sending a HandoverAddress
-* information frame to the firmware.  This should work, according to
-* Intersil.
-*
-* Arguments:
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       addr     MAC address of station to be de-authenticated.
-*
-* Returns:
-*	Nothing
-*
-----------------------------------------------------------------*/
-
-static void prism2mib_priv_deauthenticate(
-hfa384x_t         *hw,
-UINT8             *addr)
-{
-	DBFENTER;
-	hfa384x_drvr_handover(hw, addr);
-	DBFEXIT;
-	return;
-}
-
-
-/*----------------------------------------------------------------
 * prism2mgmt_pstr2bytestr
 *
 * Convert the pstr data in the WLAN message structure into an hfa384x
@@ -3279,7 +847,7 @@ void prism2mgmt_pstr2bytestr(hfa384x_bytestr_t *bytestr, p80211pstrd_t *pstr)
 {
 	DBFENTER;
 
-	bytestr->len = host2hfa384x_16((UINT16)(pstr->len));
+	bytestr->len = host2hfa384x_16((u16)(pstr->len));
 	memcpy(bytestr->data, pstr->data, pstr->len);
 	DBFEXIT;
 }
@@ -3300,7 +868,7 @@ void prism2mgmt_pstr2bytestr(hfa384x_bytestr_t *bytestr, p80211pstrd_t *pstr)
 *
 ----------------------------------------------------------------*/
 
-void prism2mgmt_pstr2bytearea(UINT8 *bytearea, p80211pstrd_t *pstr)
+void prism2mgmt_pstr2bytearea(u8 *bytearea, p80211pstrd_t *pstr)
 {
 	DBFENTER;
 
@@ -3328,7 +896,7 @@ void prism2mgmt_bytestr2pstr(hfa384x_bytestr_t *bytestr, p80211pstrd_t *pstr)
 {
 	DBFENTER;
 
-	pstr->len = (UINT8)(hfa384x2host_16((UINT16)(bytestr->len)));
+	pstr->len = (u8)(hfa384x2host_16((u16)(bytestr->len)));
 	memcpy(pstr->data, bytestr->data, pstr->len);
 	DBFEXIT;
 }
@@ -3349,11 +917,11 @@ void prism2mgmt_bytestr2pstr(hfa384x_bytestr_t *bytestr, p80211pstrd_t *pstr)
 *
 ----------------------------------------------------------------*/
 
-void prism2mgmt_bytearea2pstr(UINT8 *bytearea, p80211pstrd_t *pstr, int len)
+void prism2mgmt_bytearea2pstr(u8 *bytearea, p80211pstrd_t *pstr, int len)
 {
 	DBFENTER;
 
-	pstr->len = (UINT8)len;
+	pstr->len = (u8)len;
 	memcpy(pstr->data, bytearea, len);
 	DBFEXIT;
 }
@@ -3373,11 +941,11 @@ void prism2mgmt_bytearea2pstr(UINT8 *bytearea, p80211pstrd_t *pstr, int len)
 *
 ----------------------------------------------------------------*/
 
-void prism2mgmt_prism2int2p80211int(UINT16 *prism2int, UINT32 *wlanint)
+void prism2mgmt_prism2int2p80211int(u16 *prism2int, u32 *wlanint)
 {
 	DBFENTER;
 
-	*wlanint = (UINT32)hfa384x2host_16(*prism2int);
+	*wlanint = (u32)hfa384x2host_16(*prism2int);
 	DBFEXIT;
 }
 
@@ -3396,11 +964,11 @@ void prism2mgmt_prism2int2p80211int(UINT16 *prism2int, UINT32 *wlanint)
 *
 ----------------------------------------------------------------*/
 
-void prism2mgmt_p80211int2prism2int(UINT16 *prism2int, UINT32 *wlanint)
+void prism2mgmt_p80211int2prism2int(u16 *prism2int, u32 *wlanint)
 {
 	DBFENTER;
 
-	*prism2int = host2hfa384x_16((UINT16)(*wlanint));
+	*prism2int = host2hfa384x_16((u16)(*wlanint));
 	DBFEXIT;
 }
 
@@ -3419,7 +987,7 @@ void prism2mgmt_p80211int2prism2int(UINT16 *prism2int, UINT32 *wlanint)
 *	Nothing
 *
 ----------------------------------------------------------------*/
-void prism2mgmt_prism2enum2p80211enum(UINT16 *prism2enum, UINT32 *wlanenum, UINT16 rid)
+void prism2mgmt_prism2enum2p80211enum(u16 *prism2enum, u32 *wlanenum, u16 rid)
 {
 	DBFENTER;
 
@@ -3445,7 +1013,7 @@ void prism2mgmt_prism2enum2p80211enum(UINT16 *prism2enum, UINT32 *wlanenum, UINT
 *	Nothing
 *
 ----------------------------------------------------------------*/
-void prism2mgmt_p80211enum2prism2enum(UINT16 *prism2enum, UINT32 *wlanenum, UINT16 rid)
+void prism2mgmt_p80211enum2prism2enum(u16 *prism2enum, u32 *wlanenum, u16 rid)
 {
 	DBFENTER;
 
@@ -3471,10 +1039,10 @@ void prism2mgmt_p80211enum2prism2enum(UINT16 *prism2enum, UINT32 *wlanenum, UINT
 *	Nothing
 *
 ----------------------------------------------------------------*/
-void prism2mgmt_get_oprateset(UINT16 *rate, p80211pstrd_t *pstr)
+void prism2mgmt_get_oprateset(u16 *rate, p80211pstrd_t *pstr)
 {
-	UINT8	len;
-	UINT8	*datarate;
+	u8	len;
+	u8	*datarate;
 
 	DBFENTER;
 
@@ -3483,29 +1051,29 @@ void prism2mgmt_get_oprateset(UINT16 *rate, p80211pstrd_t *pstr)
 
  	/* 1 Mbps */
 	if ( BIT0 & (*rate) ) {
-		len += (UINT8)1;
-		*datarate = (UINT8)2;
+		len += (u8)1;
+		*datarate = (u8)2;
 		datarate++;
 	}
 
  	/* 2 Mbps */
 	if ( BIT1 & (*rate) ) {
-		len += (UINT8)1;
-		*datarate = (UINT8)4;
+		len += (u8)1;
+		*datarate = (u8)4;
 		datarate++;
 	}
 
  	/* 5.5 Mbps */
 	if ( BIT2 & (*rate) ) {
-		len += (UINT8)1;
-		*datarate = (UINT8)11;
+		len += (u8)1;
+		*datarate = (u8)11;
 		datarate++;
 	}
 
  	/* 11 Mbps */
 	if ( BIT3 & (*rate) ) {
-		len += (UINT8)1;
-		*datarate = (UINT8)22;
+		len += (u8)1;
+		*datarate = (u8)22;
 		datarate++;
 	}
 
@@ -3530,9 +1098,9 @@ void prism2mgmt_get_oprateset(UINT16 *rate, p80211pstrd_t *pstr)
 *	Nothing
 *
 ----------------------------------------------------------------*/
-void prism2mgmt_set_oprateset(UINT16 *rate, p80211pstrd_t *pstr)
+void prism2mgmt_set_oprateset(u16 *rate, p80211pstrd_t *pstr)
 {
-	UINT8	*datarate;
+	u8	*datarate;
 	int	i;
 
 	DBFENTER;
@@ -3565,233 +1133,3 @@ void prism2mgmt_set_oprateset(UINT16 *rate, p80211pstrd_t *pstr)
 	DBFEXIT;
 	return;
 }
-
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_get_grpaddr
-*
-* Retrieves a particular group address from the list of
-* group addresses.
-*
-* Arguments:
-*	did		mibitem did
-*	pstr		wlan octet string
-*	priv		prism2 driver private data structure
-*
-* Returns:
-*	Nothing
-*
-----------------------------------------------------------------*/
-void prism2mgmt_get_grpaddr(UINT32 did, p80211pstrd_t *pstr,
-	hfa384x_t *hw )
-{
-	int	index;
-
-	DBFENTER;
-
-	index = prism2mgmt_get_grpaddr_index(did);
-
-	if ( index >= 0 ) {
-		pstr->len = WLAN_ADDR_LEN;
-		memcpy(pstr->data, hw->dot11_grp_addr[index],
-			WLAN_ADDR_LEN);
-	}
-
-	DBFEXIT;
-	return;
-}
-
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_set_grpaddr
-*
-* Convert the wlan octet string into an hfa384x bit area.
-*
-* Arguments:
-*	did		mibitem did
-*	buf
-*	groups
-*
-* Returns:
-*	0	Success
-*	!0	Error
-*
-----------------------------------------------------------------*/
-int prism2mgmt_set_grpaddr(UINT32 did, UINT8 *prism2buf,
-	p80211pstrd_t *pstr, hfa384x_t *hw )
-{
-	UINT8	no_addr[WLAN_ADDR_LEN];
-	int	index;
-
-	DBFENTER;
-
-	memset(no_addr, 0, WLAN_ADDR_LEN);
-	if (memcmp(no_addr, pstr->data, WLAN_ADDR_LEN) != 0) {
-
-		/*
-		** The address is NOT 0 so we are "adding" an address to the
-		** group address list.  Check to make sure we aren't trying
-		** to add more than the maximum allowed number of group
-		** addresses in the list.  The new address is added to the
-		** end of the list regardless of the DID used to add the
-		** address.
-		*/
-
-		if (hw->dot11_grpcnt >= MAX_GRP_ADDR) return(-1);
-
-		memcpy(hw->dot11_grp_addr[hw->dot11_grpcnt], pstr->data,
-								 WLAN_ADDR_LEN);
-		hw->dot11_grpcnt += 1;
-	} else {
-
-		/*
-		** The address is 0.  Interpret this as "deleting" an address
-		** from the group address list.  Get the address index from
-		** the DID.  If this is within the range of used addresses,
-		** then delete the specified address by shifting all following
-		** addresses down.  Then clear the last address (which should
-		** now be unused).  If the address index is NOT within the
-		** range of used addresses, then just ignore the address.
-		*/
-
-		index = prism2mgmt_get_grpaddr_index(did);
-		if (index >= 0 && index < hw->dot11_grpcnt) {
-			hw->dot11_grpcnt -= 1;
-			memmove(hw->dot11_grp_addr[index],
-				hw->dot11_grp_addr[index + 1],
-				((hw->dot11_grpcnt)-index) * WLAN_ADDR_LEN);
-			memset(hw->dot11_grp_addr[hw->dot11_grpcnt], 0,
-								 WLAN_ADDR_LEN);
-		}
-	}
-
-	DBFEXIT;
-	return(0);
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_get_grpaddr_index
-*
-* Gets the index in the group address list based on the did.
-*
-* Arguments:
-*	did		mibitem did
-*
-* Returns:
-*	>= 0	If valid did
-*	< 0	If not valid did
-*
-----------------------------------------------------------------*/
-int prism2mgmt_get_grpaddr_index( UINT32 did )
-{
-	int	index;
-
-	DBFENTER;
-
-	index = -1;
-
-	switch (did) {
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address1:
-		index = 0;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address2:
-		index = 1;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address3:
-		index = 2;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address4:
-		index = 3;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address5:
-		index = 4;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address6:
-		index = 5;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address7:
-		index = 6;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address8:
-		index = 7;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address9:
-		index = 8;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address10:
-		index = 9;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address11:
-		index = 10;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address12:
-		index = 11;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address13:
-		index = 12;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address14:
-		index = 13;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address15:
-		index = 14;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address16:
-		index = 15;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address17:
-		index = 16;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address18:
-		index = 17;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address19:
-		index = 18;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address20:
-		index = 19;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address21:
-		index = 20;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address22:
-		index = 21;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address23:
-		index = 22;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address24:
-		index = 23;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address25:
-		index = 24;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address26:
-		index = 25;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address27:
-		index = 26;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address28:
-		index = 27;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address29:
-		index = 28;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address30:
-		index = 29;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address31:
-		index = 30;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address32:
-		index = 31;
-		break;
-	}
-
-	DBFEXIT;
-	return index;
-}
diff --git a/drivers/staging/wlan-ng/prism2sta.c b/drivers/staging/wlan-ng/prism2sta.c
index 18aa15f9e417..b279c97cbc02 100644
--- a/drivers/staging/wlan-ng/prism2sta.c
+++ b/drivers/staging/wlan-ng/prism2sta.c
@@ -54,16 +54,9 @@
 /* System Includes */
 #define WLAN_DBVAR	prism2_debug
 
-#include "version.h"
-
-
 #include <linux/version.h>
-
 #include <linux/module.h>
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,25))
 #include <linux/moduleparam.h>
-#endif
-
 #include <linux/kernel.h>
 #include <linux/sched.h>
 #include <linux/types.h>
@@ -71,34 +64,15 @@
 #include <linux/slab.h>
 #include <linux/wireless.h>
 #include <linux/netdevice.h>
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-#include <linux/tqueue.h>
-#else
 #include <linux/workqueue.h>
-#endif
 
 #include <asm/io.h>
 #include <linux/delay.h>
 #include <asm/byteorder.h>
 #include <linux/if_arp.h>
 
-#if (WLAN_HOSTIF == WLAN_PCMCIA)
-#include <pcmcia/version.h>
-#include <pcmcia/cs_types.h>
-#include <pcmcia/cs.h>
-#include <pcmcia/cistpl.h>
-#include <pcmcia/ds.h>
-#include <pcmcia/cisreg.h>
-#endif
-
 #include "wlan_compat.h"
 
-#if ((WLAN_HOSTIF == WLAN_PLX) || (WLAN_HOSTIF == WLAN_PCI))
-#include <linux/ioport.h>
-#include <linux/pci.h>
-#endif
-
 /*================================================================*/
 /* Project Includes */
 
@@ -126,34 +100,7 @@
 /*================================================================*/
 /* Local Static Definitions */
 
-#if (WLAN_HOSTIF == WLAN_PCMCIA)
-#define DRIVER_SUFFIX	"_cs"
-#elif (WLAN_HOSTIF == WLAN_PLX)
-#define DRIVER_SUFFIX	"_plx"
-typedef char* dev_info_t;
-#elif (WLAN_HOSTIF == WLAN_PCI)
-#define DRIVER_SUFFIX	"_pci"
-typedef char* dev_info_t;
-#elif (WLAN_HOSTIF == WLAN_USB)
-#define DRIVER_SUFFIX	"_usb"
-typedef char* dev_info_t;
-#else
-#error "HOSTIF unsupported or undefined!"
-#endif
-
-static char		*version = "prism2" DRIVER_SUFFIX ".o: " WLAN_RELEASE;
-static dev_info_t	dev_info = "prism2" DRIVER_SUFFIX;
-
-#if (WLAN_HOSTIF == WLAN_PLX || WLAN_HOSTIF == WLAN_PCI)
-#ifdef CONFIG_PM
-static int prism2sta_suspend_pci(struct pci_dev *pdev, pm_message_t state);
-static int prism2sta_resume_pci(struct pci_dev *pdev);
-#endif
-#endif
-
-#if (WLAN_HOSTIF == WLAN_PCI)
-
-#endif /* WLAN_PCI */
+static char *dev_info = "prism2_usb";
 
 static wlandevice_t *create_wlan(void);
 
@@ -163,16 +110,7 @@ static wlandevice_t *create_wlan(void);
 int      prism2_reset_holdtime=30;	/* Reset hold time in ms */
 int	 prism2_reset_settletime=100;	/* Reset settle time in ms */
 
-#if (WLAN_HOSTIF == WLAN_USB)
 static int	prism2_doreset=0;		/* Do a reset at init? */
-#else
-static int      prism2_doreset=1;		/* Do a reset at init? */
-int             prism2_bap_timeout=1000;        /* BAP timeout */
-int		prism2_irq_evread_max=20;	/* Maximum number of
-						 * ev_reads (loops)
-						 * in irq handler
-						 */
-#endif
 
 #ifdef WLAN_INCLUDE_DEBUG
 int prism2_debug=0;
@@ -188,13 +126,6 @@ MODULE_PARM_DESC( prism2_reset_holdtime, "reset hold time in ms");
 module_param( prism2_reset_settletime, int, 0644);
 MODULE_PARM_DESC( prism2_reset_settletime, "reset settle time in ms");
 
-#if (WLAN_HOSTIF != WLAN_USB)
-module_param( prism2_bap_timeout, int, 0644);
-MODULE_PARM_DESC(prism2_bap_timeout, "BufferAccessPath Timeout in 10*n us");
-module_param( prism2_irq_evread_max, int, 0644);
-MODULE_PARM_DESC( prism2_irq_evread_max, "Maximim number of event reads in interrupt handler");
-#endif
-
 MODULE_LICENSE("Dual MPL/GPL");
 
 /*================================================================*/
@@ -231,17 +162,6 @@ static void	prism2sta_inf_authreq_defer(
 static void	prism2sta_inf_psusercnt(
 			wlandevice_t *wlandev, hfa384x_InfFrame_t *inf);
 
-#ifdef CONFIG_PROC_FS
-static int
-prism2sta_proc_read(
-	char	*page,
-	char	**start,
-	off_t	offset,
-	int	count,
-	int	*eof,
-	void	*data);
-#endif
-
 /*================================================================*/
 /* Function Definitions */
 
@@ -267,7 +187,7 @@ inline void dmpmem(void *buf, int n)
 	int c;
 	for ( c= 0; c < n; c++) {
 		if ( (c % 16) == 0 ) printk(KERN_DEBUG"dmp[%d]: ", c);
-		printk("%02x ", ((UINT8*)buf)[c]);
+		printk("%02x ", ((u8*)buf)[c]);
 		if ( (c % 16) == 15 ) printk("\n");
 	}
 	if ( (c % 16) != 0 ) printk("\n");
@@ -299,10 +219,6 @@ static int prism2sta_open(wlandevice_t *wlandev)
 {
 	DBFENTER;
 
-#ifdef ANCIENT_MODULE_CODE
-	MOD_INC_USE_COUNT;
-#endif
-
 	/* We don't currently have to do anything else.
 	 * The setup of the MAC should be subsequently completed via
 	 * the mlme commands.
@@ -341,10 +257,6 @@ static int prism2sta_close(wlandevice_t *wlandev)
 {
 	DBFENTER;
 
-#ifdef ANCIENT_MODULE_CODE
-	MOD_DEC_USE_COUNT;
-#endif
-
 	/* We don't currently have to do anything else.
 	 * Higher layers know we're not ready from dev->start==0 and
 	 * dev->tbusy==1.  Our rx path knows to not pass up received
@@ -463,10 +375,6 @@ static int prism2sta_mlmerequest(wlandevice_t *wlandev, p80211msg_t *msg)
 		WLAN_LOG_DEBUG(2,"Received mibset request\n");
 		result = prism2mgmt_mibset_mibget(wlandev, msg);
 		break;
-	case DIDmsg_dot11req_powermgmt :
-		WLAN_LOG_DEBUG(2,"Received powermgmt request\n");
-		result = prism2mgmt_powermgmt(wlandev, msg);
-		break;
 	case DIDmsg_dot11req_scan :
 		WLAN_LOG_DEBUG(2,"Received scan request\n");
 		result = prism2mgmt_scan(wlandev, msg);
@@ -475,34 +383,6 @@ static int prism2sta_mlmerequest(wlandevice_t *wlandev, p80211msg_t *msg)
 		WLAN_LOG_DEBUG(2,"Received scan_results request\n");
 		result = prism2mgmt_scan_results(wlandev, msg);
 		break;
-	case DIDmsg_dot11req_join :
-		WLAN_LOG_DEBUG(2,"Received join request\n");
-		result = prism2mgmt_join(wlandev, msg);
-		break;
-	case DIDmsg_dot11req_authenticate :
-		WLAN_LOG_DEBUG(2,"Received authenticate request\n");
-		result = prism2mgmt_authenticate(wlandev, msg);
-		break;
-	case DIDmsg_dot11req_deauthenticate :
-		WLAN_LOG_DEBUG(2,"Received mlme deauthenticate request\n");
-		result = prism2mgmt_deauthenticate(wlandev, msg);
-		break;
-	case DIDmsg_dot11req_associate :
-		WLAN_LOG_DEBUG(2,"Received mlme associate request\n");
-		result = prism2mgmt_associate(wlandev, msg);
-		break;
-	case DIDmsg_dot11req_reassociate :
-		WLAN_LOG_DEBUG(2,"Received mlme reassociate request\n");
-		result = prism2mgmt_reassociate(wlandev, msg);
-		break;
-	case DIDmsg_dot11req_disassociate :
-		WLAN_LOG_DEBUG(2,"Received mlme disassociate request\n");
-		result = prism2mgmt_disassociate(wlandev, msg);
-		break;
-	case DIDmsg_dot11req_reset :
-		WLAN_LOG_DEBUG(2,"Received mlme reset request\n");
-		result = prism2mgmt_reset(wlandev, msg);
-		break;
 	case DIDmsg_dot11req_start :
 		WLAN_LOG_DEBUG(2,"Received mlme start request\n");
 		result = prism2mgmt_start(wlandev, msg);
@@ -510,46 +390,10 @@ static int prism2sta_mlmerequest(wlandevice_t *wlandev, p80211msg_t *msg)
 	/*
 	 * Prism2 specific messages
 	 */
-	case DIDmsg_p2req_join :
-		WLAN_LOG_DEBUG(2,"Received p2 join request\n");
-		result = prism2mgmt_p2_join(wlandev, msg);
-		break;
        	case DIDmsg_p2req_readpda :
 		WLAN_LOG_DEBUG(2,"Received mlme readpda request\n");
 		result = prism2mgmt_readpda(wlandev, msg);
 		break;
-	case DIDmsg_p2req_readcis :
-		WLAN_LOG_DEBUG(2,"Received mlme readcis request\n");
-		result = prism2mgmt_readcis(wlandev, msg);
-		break;
-	case DIDmsg_p2req_auxport_state :
-		WLAN_LOG_DEBUG(2,"Received mlme auxport_state request\n");
-		result = prism2mgmt_auxport_state(wlandev, msg);
-		break;
-	case DIDmsg_p2req_auxport_read :
-		WLAN_LOG_DEBUG(2,"Received mlme auxport_read request\n");
-		result = prism2mgmt_auxport_read(wlandev, msg);
-		break;
-	case DIDmsg_p2req_auxport_write :
-		WLAN_LOG_DEBUG(2,"Received mlme auxport_write request\n");
-		result = prism2mgmt_auxport_write(wlandev, msg);
-		break;
-	case DIDmsg_p2req_low_level :
-		WLAN_LOG_DEBUG(2,"Received mlme low_level request\n");
-		result = prism2mgmt_low_level(wlandev, msg);
-		break;
-        case DIDmsg_p2req_test_command :
-                WLAN_LOG_DEBUG(2,"Received mlme test_command request\n");
-                result = prism2mgmt_test_command(wlandev, msg);
-                break;
-        case DIDmsg_p2req_mmi_read :
-                WLAN_LOG_DEBUG(2,"Received mlme mmi_read request\n");
-                result = prism2mgmt_mmi_read(wlandev, msg);
-                break;
-        case DIDmsg_p2req_mmi_write :
-                WLAN_LOG_DEBUG(2,"Received mlme mmi_write request\n");
-                result = prism2mgmt_mmi_write(wlandev, msg);
-                break;
 	case DIDmsg_p2req_ramdl_state :
 		WLAN_LOG_DEBUG(2,"Received mlme ramdl_state request\n");
 		result = prism2mgmt_ramdl_state(wlandev, msg);
@@ -566,18 +410,6 @@ static int prism2sta_mlmerequest(wlandevice_t *wlandev, p80211msg_t *msg)
 		WLAN_LOG_DEBUG(2,"Received mlme flashdl_write request\n");
 		result = prism2mgmt_flashdl_write(wlandev, msg);
 		break;
-	case DIDmsg_p2req_dump_state :
-		WLAN_LOG_DEBUG(2,"Received mlme dump_state request\n");
-		result = prism2mgmt_dump_state(wlandev, msg);
-		break;
-	case DIDmsg_p2req_channel_info :
-		WLAN_LOG_DEBUG(2,"Received mlme channel_info request\n");
-		result = prism2mgmt_channel_info(wlandev, msg);
-		break;
-	case DIDmsg_p2req_channel_info_results :
-		WLAN_LOG_DEBUG(2,"Received mlme channel_info_results request\n");
-		result = prism2mgmt_channel_info_results(wlandev, msg);
-		break;
 	/*
 	 * Linux specific messages
 	 */
@@ -603,18 +435,11 @@ static int prism2sta_mlmerequest(wlandevice_t *wlandev, p80211msg_t *msg)
 		WLAN_LOG_DEBUG(2,"Received mlme autojoin request\n");
 		result = prism2mgmt_autojoin(wlandev, msg);
 		break;
-	case DIDmsg_p2req_enable :
-		WLAN_LOG_DEBUG(2,"Received mlme enable request\n");
-		result = prism2mgmt_enable(wlandev, msg);
-		break;
 	case DIDmsg_lnxreq_commsquality: {
 		p80211msg_lnxreq_commsquality_t *qualmsg;
 
 		WLAN_LOG_DEBUG(2,"Received commsquality request\n");
 
-		if (hw->ap)
-			break;
-
 		qualmsg = (p80211msg_lnxreq_commsquality_t*) msg;
 
 		qualmsg->link.status = P80211ENUM_msgitem_status_data_ok;
@@ -659,10 +484,10 @@ static int prism2sta_mlmerequest(wlandevice_t *wlandev, p80211msg_t *msg)
 *	process thread  (usually)
 *	interrupt
 ----------------------------------------------------------------*/
-UINT32 prism2sta_ifstate(wlandevice_t *wlandev, UINT32 ifstate)
+u32 prism2sta_ifstate(wlandevice_t *wlandev, u32 ifstate)
 {
         hfa384x_t               *hw = (hfa384x_t *)wlandev->priv;
-	UINT32 			result;
+	u32 			result;
 	DBFENTER;
 
 	result = P80211ENUM_resultcode_implementation_failure;
@@ -679,9 +504,6 @@ UINT32 prism2sta_ifstate(wlandevice_t *wlandev, UINT32 ifstate)
 			 * Initialize the device+driver sufficiently
 			 * for firmware loading.
 			 */
-#if (WLAN_HOSTIF != WLAN_USB)
-			result=hfa384x_cmd_initialize(hw);
-#else
 			if ((result=hfa384x_drvr_start(hw))) {
 				WLAN_LOG_ERROR(
 					"hfa384x_drvr_start() failed,"
@@ -691,7 +513,6 @@ UINT32 prism2sta_ifstate(wlandevice_t *wlandev, UINT32 ifstate)
 				wlandev->msdstate = WLAN_MSD_HWPRESENT;
 				break;
 			}
-#endif
 			wlandev->msdstate = WLAN_MSD_FWLOAD;
 			result = P80211ENUM_resultcode_success;
 			break;
@@ -841,8 +662,8 @@ static int prism2sta_getcardinfo(wlandevice_t *wlandev)
 {
 	int 			result = 0;
         hfa384x_t               *hw = (hfa384x_t *)wlandev->priv;
-	UINT16                  temp;
-	UINT8			snum[HFA384x_RID_NICSERIALNUMBER_LEN];
+	u16                  temp;
+	u8			snum[HFA384x_RID_NICSERIALNUMBER_LEN];
 	char			pstr[(HFA384x_RID_NICSERIALNUMBER_LEN * 4) + 1];
 
 	DBFENTER;
@@ -907,20 +728,20 @@ static int prism2sta_getcardinfo(wlandevice_t *wlandev)
 
 	/* strip out the 'special' variant bits */
 	hw->mm_mods = hw->ident_sta_fw.variant & (BIT14 | BIT15);
-	hw->ident_sta_fw.variant &= ~((UINT16)(BIT14 | BIT15));
+	hw->ident_sta_fw.variant &= ~((u16)(BIT14 | BIT15));
 
 	if  ( hw->ident_sta_fw.id == 0x1f ) {
-		hw->ap = 0;
 		WLAN_LOG_INFO(
 			"ident: sta f/w: id=0x%02x %d.%d.%d\n",
 			hw->ident_sta_fw.id, hw->ident_sta_fw.major,
 			hw->ident_sta_fw.minor, hw->ident_sta_fw.variant);
 	} else {
-		hw->ap = 1;
 		WLAN_LOG_INFO(
 			"ident:  ap f/w: id=0x%02x %d.%d.%d\n",
 			hw->ident_sta_fw.id, hw->ident_sta_fw.major,
 			hw->ident_sta_fw.minor, hw->ident_sta_fw.variant);
+		WLAN_LOG_ERROR("Unsupported Tertiary AP firmeare loaded!\n");
+		goto failed;
 	}
 
 	/* Compatibility range, Modem supplier */
@@ -1168,7 +989,7 @@ static int prism2sta_setmulticast(wlandevice_t *wlandev, netdevice_t *dev)
 	int result = 0;
 	hfa384x_t               *hw = (hfa384x_t *)wlandev->priv;
 
-	UINT16  promisc;
+	u16  promisc;
 
 	DBFENTER;
 
@@ -1176,10 +997,6 @@ static int prism2sta_setmulticast(wlandevice_t *wlandev, netdevice_t *dev)
 	if ( hw->state != HFA384x_STATE_RUNNING )
 		goto exit;
 
-	/* If we're an AP, do nothing here */
-	if (hw->ap)
-		goto exit;
-
 	if ( (dev->flags & (IFF_PROMISC | IFF_ALLMULTI)) != 0 )
 		promisc = P80211ENUM_truth_true;
 	else
@@ -1247,9 +1064,9 @@ static void prism2sta_inf_handover(wlandevice_t *wlandev, hfa384x_InfFrame_t *in
 static void prism2sta_inf_tallies(wlandevice_t *wlandev, hfa384x_InfFrame_t *inf)
 {
         hfa384x_t               *hw = (hfa384x_t *)wlandev->priv;
-	UINT16			*src16;
-	UINT32			*dst;
-	UINT32			*src32;
+	u16			*src16;
+	u32			*dst;
+	u32			*src32;
 	int			i;
 	int			cnt;
 
@@ -1260,15 +1077,15 @@ static void prism2sta_inf_tallies(wlandevice_t *wlandev, hfa384x_InfFrame_t *inf
 	** record length of the info record.
 	*/
 
-	cnt = sizeof(hfa384x_CommTallies32_t) / sizeof(UINT32);
+	cnt = sizeof(hfa384x_CommTallies32_t) / sizeof(u32);
 	if (inf->framelen > 22) {
-		dst   = (UINT32 *) &hw->tallies;
-		src32 = (UINT32 *) &inf->info.commtallies32;
+		dst   = (u32 *) &hw->tallies;
+		src32 = (u32 *) &inf->info.commtallies32;
 		for (i = 0; i < cnt; i++, dst++, src32++)
 			*dst += hfa384x2host_32(*src32);
 	} else {
-		dst   = (UINT32 *) &hw->tallies;
-		src16 = (UINT16 *) &inf->info.commtallies16;
+		dst   = (u32 *) &hw->tallies;
+		src16 = (u16 *) &inf->info.commtallies16;
 		for (i = 0; i < cnt; i++, dst++, src16++)
 			*dst += hfa384x2host_16(*src16);
 	}
@@ -1308,7 +1125,7 @@ static void prism2sta_inf_scanresults(wlandevice_t *wlandev,
 	DBFENTER;
 
 	/* Get the number of results, first in bytes, then in results */
-	nbss = (inf->framelen * sizeof(UINT16)) -
+	nbss = (inf->framelen * sizeof(u16)) -
 		sizeof(inf->infotype) -
 		sizeof(inf->info.scanresult.scanreason);
 	nbss /= sizeof(hfa384x_ScanResultSub_t);
@@ -1500,7 +1317,7 @@ void prism2sta_processing_defer(struct work_struct *data)
 
 		/* Don't call this in monitor mode */
 		if ( wlandev->netdev->type == ARPHRD_ETHER ) {
-			UINT16			portstatus;
+			u16			portstatus;
 
 			WLAN_LOG_INFO("linkstatus=CONNECTED\n");
 
@@ -1836,7 +1653,7 @@ static void prism2sta_inf_authreq_defer(wlandevice_t *wlandev,
 	hfa384x_authenticateStation_data_t  rec;
 
 	int    i, added, result, cnt;
-	UINT8  *addr;
+	u8  *addr;
 
 	DBFENTER;
 
@@ -2163,7 +1980,7 @@ void prism2sta_ev_info(wlandevice_t *wlandev, hfa384x_InfFrame_t *inf)
 * Call context:
 *	interrupt
 ----------------------------------------------------------------*/
-void prism2sta_ev_txexc(wlandevice_t *wlandev, UINT16 status)
+void prism2sta_ev_txexc(wlandevice_t *wlandev, u16 status)
 {
 	DBFENTER;
 
@@ -2190,7 +2007,7 @@ void prism2sta_ev_txexc(wlandevice_t *wlandev, UINT16 status)
 * Call context:
 *	interrupt
 ----------------------------------------------------------------*/
-void prism2sta_ev_tx(wlandevice_t *wlandev, UINT16 status)
+void prism2sta_ev_tx(wlandevice_t *wlandev, u16 status)
 {
 	DBFENTER;
 	WLAN_LOG_DEBUG(4, "Tx Complete, status=0x%04x\n", status);
@@ -2247,47 +2064,12 @@ void prism2sta_ev_alloc(wlandevice_t *wlandev)
 {
 	DBFENTER;
 
-	p80211netdev_wake_queue(wlandev);
+	netif_wake_queue(wlandev->netdev);
 
 	DBFEXIT;
 	return;
 }
 
-#if (WLAN_HOSTIF == WLAN_PLX || WLAN_HOSTIF == WLAN_PCI)
-#ifdef CONFIG_PM
-static int prism2sta_suspend_pci(struct pci_dev *pdev, pm_message_t state)
-{
-       	wlandevice_t		*wlandev;
-
-	wlandev = (wlandevice_t *) pci_get_drvdata(pdev);
-
-	/* reset hardware */
-	if (wlandev) {
-		prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
-		p80211_suspend(wlandev);
-	}
-
-	// call a netif_device_detach(wlandev->netdev) ?
-
-	return 0;
-}
-
-static int prism2sta_resume_pci (struct pci_dev *pdev)
-{
-       	wlandevice_t		*wlandev;
-
-	wlandev = (wlandevice_t *) pci_get_drvdata(pdev);
-
-	if (wlandev) {
-		prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
-		p80211_resume(wlandev);
-	}
-
-        return 0;
-}
-#endif
-#endif
-
 /*----------------------------------------------------------------
 * create_wlan
 *
@@ -2334,9 +2116,6 @@ static wlandevice_t *create_wlan(void)
 	wlandev->open = prism2sta_open;
 	wlandev->close = prism2sta_close;
 	wlandev->reset = prism2sta_reset;
-#ifdef CONFIG_PROC_FS
-	wlandev->nsd_proc_read = prism2sta_proc_read;
-#endif
 	wlandev->txframe = prism2sta_txframe;
 	wlandev->mlmerequest = prism2sta_mlmerequest;
 	wlandev->set_multicast_list = prism2sta_setmulticast;
@@ -2351,75 +2130,6 @@ static wlandevice_t *create_wlan(void)
 	return wlandev;
 }
 
-#ifdef CONFIG_PROC_FS
-static int
-prism2sta_proc_read(
-	char	*page,
-	char	**start,
-	off_t	offset,
-	int	count,
-	int	*eof,
-	void	*data)
-{
-	char	 *p = page;
-	wlandevice_t *wlandev = (wlandevice_t *) data;
-	hfa384x_t *hw = (hfa384x_t *) wlandev->priv;
-
-	UINT16 hwtype = 0;
-
-	DBFENTER;
-	if (offset != 0) {
-		*eof = 1;
-		goto exit;
-	}
-
-	// XXX 0x0001 for prism2.5/3, 0x0000 for prism2.
-	hwtype = BIT0;
-
-#if (WLAN_HOSTIF != WLAN_USB)
-	if (hw->isram16)
-		hwtype |= BIT1;
-#endif
-
-#if (WLAN_HOSTIF == WLAN_PCI)
-	hwtype |= BIT2;
-#endif
-
-#define PRISM2_CVS_ID "$Id: prism2sta.c 1826 2007-03-19 15:37:00Z pizza $"
-
-	p += sprintf(p, "# %s version %s (%s) '%s'\n\n",
-		     dev_info,
-		     WLAN_RELEASE, WLAN_BUILD_DATE, PRISM2_CVS_ID);
-
-	p += sprintf(p, "# nic h/w: id=0x%02x %d.%d.%d\n",
-		     hw->ident_nic.id, hw->ident_nic.major,
-		     hw->ident_nic.minor, hw->ident_nic.variant);
-
-	p += sprintf(p, "# pri f/w: id=0x%02x %d.%d.%d\n",
-		     hw->ident_pri_fw.id, hw->ident_pri_fw.major,
-		     hw->ident_pri_fw.minor, hw->ident_pri_fw.variant);
-
-	if (hw->ident_sta_fw.id == 0x1f) {
-		p += sprintf(p, "# sta f/w: id=0x%02x %d.%d.%d\n",
-			     hw->ident_sta_fw.id, hw->ident_sta_fw.major,
-			     hw->ident_sta_fw.minor, hw->ident_sta_fw.variant);
-	} else {
-		p += sprintf(p, "# ap f/w: id=0x%02x %d.%d.%d\n",
-			     hw->ident_sta_fw.id, hw->ident_sta_fw.major,
-			     hw->ident_sta_fw.minor, hw->ident_sta_fw.variant);
-	}
-
-#if (WLAN_HOSTIF != WLAN_USB)
-	p += sprintf(p, "# initial nic hw type, needed for SSF ramdl\n");
-	p += sprintf(p, "initnichw=%04x\n", hwtype);
-#endif
-
- exit:
-	DBFEXIT;
-	return (p - page);
-}
-#endif
-
 void prism2sta_commsqual_defer(struct work_struct *data)
 {
 	hfa384x_t		*hw = container_of(data, struct hfa384x, commsqual_bh);
diff --git a/drivers/staging/wlan-ng/prism2_usb.c b/drivers/staging/wlan-ng/prism2usb.c
index e45be2374503..8f7b1f281f0a 100644
--- a/drivers/staging/wlan-ng/prism2_usb.c
+++ b/drivers/staging/wlan-ng/prism2usb.c
@@ -1,13 +1,8 @@
-#define WLAN_HOSTIF WLAN_USB
 #include "hfa384x_usb.c"
 #include "prism2mgmt.c"
 #include "prism2mib.c"
 #include "prism2sta.c"
 
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0))
-#error "prism2_usb requires at least a 2.4.x kernel!"
-#endif
-
 #define PRISM_USB_DEVICE(vid, pid, name) \
            USB_DEVICE(vid, pid),  \
            .driver_info = (unsigned long) name
@@ -80,23 +75,11 @@ MODULE_DEVICE_TABLE(usb, usb_prism_tbl);
 *	I'm not sure, assume it's interrupt.
 *
 ----------------------------------------------------------------*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-static void __devinit *prism2sta_probe_usb(
-	struct usb_device *dev,
-	unsigned int ifnum,
-	const struct usb_device_id *id)
-#else
 static int prism2sta_probe_usb(
 	struct usb_interface *interface,
 	const struct usb_device_id *id)
-#endif
 {
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-	struct usb_interface *interface;
-#else
 	struct usb_device *dev;
-#endif
 
 	wlandevice_t	*wlandev = NULL;
 	hfa384x_t	*hw = NULL;
@@ -104,12 +87,7 @@ static int prism2sta_probe_usb(
 
 	DBFENTER;
 
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-	interface = &dev->actconfig->interface[ifnum];
-#else
 	dev = interface_to_usbdev(interface);
-#endif
-
 
 	if ((wlandev = create_wlan()) == NULL) {
 		WLAN_LOG_ERROR("%s: Memory allocation failure.\n", dev_info);
@@ -131,14 +109,7 @@ static int prism2sta_probe_usb(
 	/* Register the wlandev, this gets us a name and registers the
 	 * linux netdevice.
 	 */
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0))
 	SET_NETDEV_DEV(wlandev->netdev, &(interface->dev));
-#endif
-        if ( register_wlandev(wlandev) != 0 ) {
-		WLAN_LOG_ERROR("%s: register_wlandev() failed.\n", dev_info);
-		result = -EIO;
-		goto failed;
-        }
 
 	/* Do a chip-level reset on the MAC */
 	if (prism2_doreset) {
@@ -156,15 +127,19 @@ static int prism2sta_probe_usb(
 		}
 	}
 
-#ifndef NEW_MODULE_CODE
-	usb_inc_dev_use(dev);
-#endif
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,15))
 	usb_get_dev(dev);
-#endif
 
 	wlandev->msdstate = WLAN_MSD_HWPRESENT;
 
+        if ( register_wlandev(wlandev) != 0 ) {
+		WLAN_LOG_ERROR("%s: register_wlandev() failed.\n", dev_info);
+		result = -EIO;
+		goto failed;
+        }
+
+/* enable the card */
+	prism2sta_ifstate(wlandev, P80211ENUM_ifstate_enable);
+
 	goto done;
 
  failed:
@@ -175,12 +150,8 @@ static int prism2sta_probe_usb(
  done:
 	DBFEXIT;
 
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-	return wlandev;
-#else
 	usb_set_intfdata(interface, wlandev);
 	return result;
-#endif
 }
 
 
@@ -203,25 +174,14 @@ static int prism2sta_probe_usb(
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-static void __devexit
-prism2sta_disconnect_usb(struct usb_device *dev, void *ptr)
-#else
 static void
 prism2sta_disconnect_usb(struct usb_interface *interface)
-#endif
 {
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0))
 	wlandevice_t		*wlandev;
-#else
-	wlandevice_t		*wlandev = (wlandevice_t*)ptr;
-#endif
 
         DBFENTER;
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0))
 	wlandev = (wlandevice_t *) usb_get_intfdata(interface);
-#endif
 
 	if ( wlandev != NULL ) {
 		LIST_HEAD(cleanlist);
@@ -296,12 +256,7 @@ prism2sta_disconnect_usb(struct usb_interface *interface)
 		unregister_wlandev(wlandev);
 		wlan_unsetup(wlandev);
 
-#ifndef NEW_MODULE_CODE
-		usb_dec_dev_use(hw->usb);
-#endif
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,15))
 		usb_put_dev(hw->usb);
-#endif
 
 		hfa384x_destroy(hw);
 		kfree(hw);
@@ -311,17 +266,12 @@ prism2sta_disconnect_usb(struct usb_interface *interface)
 
  exit:
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0))
 	usb_set_intfdata(interface, NULL);
-#endif
 	DBFEXIT;
 }
 
 
 static struct usb_driver prism2_usb_driver = {
-#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,19)) && (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,16))
-	.owner = THIS_MODULE,
-#endif
 	.name = "prism2_usb",
 	.probe = prism2sta_probe_usb,
 	.disconnect = prism2sta_disconnect_usb,
@@ -329,15 +279,10 @@ static struct usb_driver prism2_usb_driver = {
 	/* fops, minor? */
 };
 
-#ifdef MODULE
-
 static int __init prism2usb_init(void)
 {
         DBFENTER;
 
-        WLAN_LOG_NOTICE("%s Loaded\n", version);
-        WLAN_LOG_NOTICE("dev_info is: %s\n", dev_info);
-
 	/* This call will result in calls to prism2sta_probe_usb. */
 	return usb_register(&prism2_usb_driver);
 
@@ -350,12 +295,8 @@ static void __exit prism2usb_cleanup(void)
 
 	usb_deregister(&prism2_usb_driver);
 
-        printk(KERN_NOTICE "%s Unloaded\n", version);
-
 	DBFEXIT;
 };
 
 module_init(prism2usb_init);
 module_exit(prism2usb_cleanup);
-
-#endif // module
diff --git a/drivers/staging/wlan-ng/version.h b/drivers/staging/wlan-ng/version.h
deleted file mode 100644
index 305f88239446..000000000000
--- a/drivers/staging/wlan-ng/version.h
+++ /dev/null
@@ -1,64 +0,0 @@
-/* src/include/wlan/version.h
-*
-*
-* Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
-* --------------------------------------------------------------------
-*
-* linux-wlan
-*
-*   The contents of this file are subject to the Mozilla Public
-*   License Version 1.1 (the "License"); you may not use this file
-*   except in compliance with the License. You may obtain a copy of
-*   the License at http://www.mozilla.org/MPL/
-*
-*   Software distributed under the License is distributed on an "AS
-*   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
-*   implied. See the License for the specific language governing
-*   rights and limitations under the License.
-*
-*   Alternatively, the contents of this file may be used under the
-*   terms of the GNU Public License version 2 (the "GPL"), in which
-*   case the provisions of the GPL are applicable instead of the
-*   above.  If you wish to allow the use of your version of this file
-*   only under the terms of the GPL and not to allow others to use
-*   your version of this file under the MPL, indicate your decision
-*   by deleting the provisions above and replace them with the notice
-*   and other provisions required by the GPL.  If you do not delete
-*   the provisions above, a recipient may use your version of this
-*   file under either the MPL or the GPL.
-*
-* --------------------------------------------------------------------
-*
-* Inquiries regarding the linux-wlan Open Source project can be
-* made directly to:
-*
-* AbsoluteValue Systems Inc.
-* info@linux-wlan.com
-* http://www.linux-wlan.com
-*
-* --------------------------------------------------------------------
-*
-* Portions of the development of this software were funded by
-* Intersil Corporation as part of PRISM(R) chipset product development.
-*
-* --------------------------------------------------------------------
-*/
-#ifndef _WLAN_VERSION_H
-#define _WLAN_VERSION_H
-#ifndef KERNEL_VERSION
-#define KERNEL_VERSION(a,b,c) (((a) << 16) + ((b) << 8) + (c))
-#endif
-
-/* WLAN_HOSTIF (generally set on the command line, not detected) */
-#define WLAN_NONE                       0
-#define WLAN_PCMCIA                     1
-#define WLAN_ISA                        2
-#define WLAN_PCI                        3
-#define WLAN_USB                        4
-#define WLAN_PLX                        5
-#define WLAN_SLAVE                      6
-#define WLAN_RELEASE	"0.2.8"
-#define WLAN_RELEASE_CODE 0x000208
-#define WLAN_BUILD_DATE "Thu Oct  2 11:04:42 PDT 2008"
-
-#endif
diff --git a/drivers/staging/wlan-ng/wlan_compat.h b/drivers/staging/wlan-ng/wlan_compat.h
index 59dfa8f84cbe..8b8a510685c9 100644
--- a/drivers/staging/wlan-ng/wlan_compat.h
+++ b/drivers/staging/wlan-ng/wlan_compat.h
@@ -49,114 +49,6 @@
 #define _WLAN_COMPAT_H
 
 /*=============================================================*/
-/*------ Establish Platform Identity --------------------------*/
-/*=============================================================*/
-/* Key macros: */
-/* WLAN_CPU_FAMILY */
-	#define WLAN_Ix86			1
-	#define WLAN_PPC			2
-	#define WLAN_Ix96			3
-	#define WLAN_ARM			4
-	#define WLAN_ALPHA			5
-	#define WLAN_MIPS			6
-	#define WLAN_HPPA			7
-	#define WLAN_SPARC			8
-	#define WLAN_SH    			9
-	#define WLAN_x86_64                     10
-/* WLAN_SYSARCH */
-	#define WLAN_PCAT			1
-	#define WLAN_MBX			2
-	#define WLAN_RPX			3
-	#define WLAN_LWARCH			4
-	#define WLAN_PMAC			5
-	#define WLAN_SKIFF			6
-	#define WLAN_BITSY			7
-	#define WLAN_ALPHAARCH			7
-	#define WLAN_MIPSARCH			9
-	#define WLAN_HPPAARCH			10
-	#define WLAN_SPARCARCH			11
-	#define WLAN_SHARCH   			12
-
-/* Note: the PLX HOSTIF above refers to some vendors implementations for */
-/*       PCI.  It's a PLX chip that is a PCI to PCMCIA adapter, but it   */
-/*       isn't a real PCMCIA host interface adapter providing all the    */
-/*       card&socket services.                                           */
-
-#if (defined(CONFIG_PPC) || defined(CONFIG_8xx) || defined(__powerpc__))
-#ifndef __ppc__
-#define __ppc__
-#endif
-#endif
-
-#if defined(__KERNEL__)
-
-#ifndef AUTOCONF_INCLUDED
-#include <linux/config.h>
-#endif
-
-#if defined(__x86_64__)
-	#define WLAN_CPU_FAMILY		WLAN_x86_64
-	#define WLAN_SYSARCH		WLAN_PCAT
-#elif defined(__i386__) || defined(__i486__) || defined(__i586__) || defined(__i686__)
-	#define WLAN_CPU_FAMILY		WLAN_Ix86
-	#define WLAN_SYSARCH		WLAN_PCAT
-#elif defined(__ppc__)
-	#define WLAN_CPU_FAMILY		WLAN_PPC
-	#if defined(CONFIG_MBX)
-		#define WLAN_SYSARCH	WLAN_MBX
-	#elif defined(CONFIG_RPXLITE)
-		#define WLAN_SYSARCH	WLAN_RPX
-	#elif defined(CONFIG_RPXCLASSIC)
-		#define WLAN_SYSARCH	WLAN_RPX
-	#else
-		#define WLAN_SYSARCH	WLAN_PMAC
-	#endif
-#elif defined(__arm__)
-	#define WLAN_CPU_FAMILY		WLAN_ARM
-	#define WLAN_SYSARCH		WLAN_SKIFF
-#elif defined(__alpha__)
-	#define WLAN_CPU_FAMILY		WLAN_ALPHA
-	#define WLAN_SYSARCH		WLAN_ALPHAARCH
-#elif defined(__mips__)
-	#define WLAN_CPU_FAMILY		WLAN_MIPS
-	#define WLAN_SYSARCH		WLAN_MIPSARCH
-#elif defined(__hppa__)
-	#define WLAN_CPU_FAMILY		WLAN_HPPA
-	#define WLAN_SYSARCH		WLAN_HPPAARCH
-#elif defined(__sparc__)
-        #define WLAN_CPU_FAMILY         WLAN_SPARC
-        #define WLAN_SYSARCH            WLAN_SPARC
-#elif defined(__sh__)
-        #define WLAN_CPU_FAMILY         WLAN_SH
-        #define WLAN_SYSARCH            WLAN_SHARCH
-        #ifndef __LITTLE_ENDIAN__
-        #define __LITTLE_ENDIAN__
-        #endif
-#else
-	#error "No CPU identified!"
-#endif
-#endif /* __KERNEL__ */
-
-/*
-   Some big endian machines implicitly do all I/O in little endian mode.
-
-   In particular:
-          Linux/PPC on PowerMacs (PCI)
-	  Arm/Intel Xscale (PCI)
-
-   This may also affect PLX boards and other BE &| PPC platforms;
-   as new ones are discovered, add them below.
-*/
-
-#if defined(WLAN_HOSTIF)
-#if ((WLAN_HOSTIF == WLAN_PCI) || (WLAN_HOSTIF == WLAN_PLX))
-#if ((WLAN_SYSARCH == WLAN_SKIFF) || (WLAN_SYSARCH == WLAN_PMAC) || (WLAN_SYSARCH == WLAN_SPARC))
-#define REVERSE_ENDIAN
-#endif
-#endif
-#endif
-
-/*=============================================================*/
 /*------ Bit settings -----------------------------------------*/
 /*=============================================================*/
 
@@ -193,30 +85,6 @@
 #define BIT30	0x40000000
 #define BIT31	0x80000000
 
-#include <linux/types.h>
-
-typedef u_int8_t	UINT8;
-typedef u_int16_t	UINT16;
-typedef u_int32_t	UINT32;
-
-typedef int8_t		INT8;
-typedef int16_t		INT16;
-typedef int32_t		INT32;
-
-typedef unsigned int    UINT;
-typedef signed int      INT;
-
-typedef u_int64_t	UINT64;
-typedef int64_t		INT64;
-
-#define UINT8_MAX	(0xffUL)
-#define UINT16_MAX	(0xffffUL)
-#define UINT32_MAX	(0xffffffffUL)
-
-#define INT8_MAX	(0x7fL)
-#define INT16_MAX	(0x7fffL)
-#define INT32_MAX	(0x7fffffffL)
-
 /*=============================================================*/
 /*------ Compiler Portability Macros --------------------------*/
 /*=============================================================*/
@@ -230,20 +98,9 @@ typedef int64_t		INT64;
 #define WLAN_DBVAR	wlan_debug
 #endif
 
-#ifndef KERNEL_VERSION
-#define KERNEL_VERSION(a,b,c) (((a) << 16) + ((b) << 8) + (c))
-#endif
+#define WLAN_RELEASE	"0.3.0-lkml"
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0))
-#  if (LINUX_VERSION_CODE > KERNEL_VERSION(2,6,8))
-#    include <linux/hardirq.h>
-#  else
-#    include <asm/hardirq.h>
-#  endif
-#elif defined(__KERNEL__)
-#  define PREEMPT_MASK  (0x000000FFUL)
-#  define preempt_count() (0UL)
-#endif
+#include <linux/hardirq.h>
 
 #define WLAN_LOG_ERROR(x,args...) printk(KERN_ERR "%s: " x , __func__ , ##args);
 
@@ -254,20 +111,17 @@ typedef int64_t		INT64;
 #define WLAN_LOG_INFO(args... ) printk(KERN_INFO args)
 
 #if defined(WLAN_INCLUDE_DEBUG)
-	#define WLAN_ASSERT(c) if ((!(c)) && WLAN_DBVAR >= 1) { \
-		WLAN_LOG_DEBUG(1, "Assertion failure!\n"); }
 	#define WLAN_HEX_DUMP( l, x, p, n)	if( WLAN_DBVAR >= (l) ){ \
 		int __i__; \
 		printk(KERN_DEBUG x ":"); \
 		for( __i__=0; __i__ < (n); __i__++) \
-			printk( " %02x", ((UINT8*)(p))[__i__]); \
+			printk( " %02x", ((u8*)(p))[__i__]); \
 		printk("\n"); }
 	#define DBFENTER { if ( WLAN_DBVAR >= 5 ){ WLAN_LOG_DEBUG(3,"---->\n"); } }
 	#define DBFEXIT  { if ( WLAN_DBVAR >= 5 ){ WLAN_LOG_DEBUG(3,"<----\n"); } }
 
 	#define WLAN_LOG_DEBUG(l,x,args...) if ( WLAN_DBVAR >= (l)) printk(KERN_DEBUG "%s(%lu): " x ,  __func__, (preempt_count() & PREEMPT_MASK), ##args );
 #else
-	#define WLAN_ASSERT(c)
 	#define WLAN_HEX_DUMP( l, s, p, n)
 	#define DBFENTER
 	#define DBFEXIT
@@ -275,413 +129,11 @@ typedef int64_t		INT64;
 	#define WLAN_LOG_DEBUG(l, s, args...)
 #endif
 
-#ifdef CONFIG_SMP
-#define __SMP__			1
-#endif
-
-#if defined(__KERNEL__)
-
-#if ((LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)) || (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,19)))
-#define URB_ONLY_CALLBACK
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,19))
-#define PT_REGS    , struct pt_regs *regs
-#else
-#define PT_REGS
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,7))
-#  define del_singleshot_timer_sync(a)  del_timer_sync(a)
-#endif
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,17))
-#define CONFIG_NETLINK		1
-#endif
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,0))
-#define kfree_s(a, b)	kfree((a))
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,2,18))
-#ifndef init_waitqueue_head
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,0,16))
-#define init_waitqueue_head(p)  (*(p) = NULL)
-#else
-#define init_waitqueue_head(p)  init_waitqueue(p)
-#endif
-typedef struct wait_queue *wait_queue_head_t;
-typedef struct wait_queue wait_queue_t;
-#define set_current_state(b)  { current->state = (b); mb(); }
-#define init_waitqueue_entry(a, b) { (a)->task = current; }
-#endif
-#endif
-
-#ifndef wait_event_interruptible_timeout
-// retval == 0; signal met; we're good.
-// retval < 0; interrupted by signal.
-// retval > 0; timed out.
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,0))  // fixme?
-
-#define __wait_event_interruptible_timeout(wq, condition, ret)            \
-do {                                                                      \
-          wait_queue_t __wait;                                            \
-          init_waitqueue_entry(&__wait, current);                         \
-	                                                                  \
-          add_wait_queue(&wq, &__wait);                                   \
-          for (;;) {                                                      \
-                  set_current_state(TASK_INTERRUPTIBLE);                  \
-                  if (condition)                                          \
-                          break;                                          \
-                  if (!signal_pending(current)) {                         \
-                          ret = schedule_timeout(ret)    ;                \
-                          if (!ret)                                       \
-                                 break;                                   \
-                          continue;                                       \
-                  }                                                       \
-                  ret = -ERESTARTSYS;                                     \
-                  break;                                                  \
-          }                                                               \
-          set_current_state(TASK_RUNNING);                                \
-          remove_wait_queue(&wq, &__wait);                                \
-} while (0)
-
-#else // 2.2
-
-
-#define __wait_event_interruptible_timeout(wq, condition, ret)          \
-do {                                                                    \
-        struct wait_queue __wait;                                       \
-                                                                        \
-        __wait.task = current;                                          \
-        add_wait_queue(&wq, &__wait);                                   \
-        for (;;) {                                                      \
-                current->state = TASK_INTERRUPTIBLE;                    \
-                if (condition)                                          \
-                        break;                                          \
-                if (!signal_pending(current)) {                         \
-                        ret = schedule_timeout(ret);                    \
-                        if (!ret)                                       \
-                               break;                                   \
-                        continue;                                       \
-                }                                                       \
-                ret = -ERESTARTSYS;                                     \
-                break;                                                  \
-        }                                                               \
-        current->state = TASK_RUNNING;                                  \
-        remove_wait_queue(&wq, &__wait);                                \
-} while (0)
-
-#endif  // version >= 2.4
-
-#define wait_event_interruptible_timeout(wq, condition, timeout)	  \
-({									  \
-	long __ret = timeout;						  \
-	if (!(condition))						  \
-		__wait_event_interruptible_timeout(wq, condition, __ret); \
-	__ret;								  \
-})
-
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,20))
-#ifdef _LINUX_LIST_H
-
-static inline void list_move_tail(struct list_head *list,
-          struct list_head *head)
-{
-        __list_del(list->prev, list->next);
-        list_add_tail(list, head);
-}
-
-static inline void __list_splice(struct list_head *list,
-				 struct list_head *head)
-{
-      struct list_head *first = list->next;
-      struct list_head *last = list->prev;
-      struct list_head *at = head->next;
-
-      first->prev = head;
-      head->next = first;
-
-      last->next = at;
-      at->prev = last;
-}
-
-static inline void list_move(struct list_head *list, struct list_head *head)
-{
-      __list_del(list->prev, list->next);
-      list_add(list, head);
-}
-
-static inline void list_splice_init(struct list_head *list,
-            struct list_head *head)
-{
-	if (!list_empty(list)) {
-		__list_splice(list, head);
-		INIT_LIST_HEAD(list);
-	}
-}
-
-
-#endif  // LIST_H
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,1,90))
-#define spin_lock(l)            do { } while (0)
-#define spin_unlock(l)          do { } while (0)
-#define spin_lock_irqsave(l,f)  do { save_flags(f); cli(); } while (0)
-#define spin_unlock_irqrestore(l,f) do { restore_flags(f); } while (0)
-#define spin_lock_init(s)       do { } while (0)
-#define spin_trylock(l)         (1)
-typedef int spinlock_t;
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0)) // XXX ???
-#define spin_lock_bh         spin_lock
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0))
-#ifdef CONFIG_SMP
-#define spin_is_locked(x)       (*(volatile char *)(&(x)->lock) <= 0)
-#else
-#define spin_is_locked(l)       (0)
-#endif
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,28))
-#define __user
-#define __iomem
-#endif
-
-#ifdef _LINUX_PROC_FS_H
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,25))
-
-extern inline struct proc_dir_entry *
-create_proc_read_entry(const char *name, mode_t mode,
-                       struct proc_dir_entry *base,
-                       read_proc_t *read_proc, void *data)
-{
-    struct proc_dir_entry *res = create_proc_entry(name, mode, base);
-    if (res) {
-        res->read_proc = read_proc;
-        res->data = data;
-    }
-    return res;
-}
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,29))
-#ifndef proc_mkdir
-#define proc_mkdir(name, root) create_proc_entry(name, S_IFDIR, root)
-#endif
-#endif
-#endif /* _LINUX_PROC_FS_H */
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-#ifndef INIT_TQUEUE
-#define PREPARE_TQUEUE(_tq, _routine, _data)                    \
-        do {                                                    \
-                (_tq)->routine = _routine;                      \
-                (_tq)->data = _data;                            \
-        } while (0)
-#define INIT_TQUEUE(_tq, _routine, _data)                       \
-        do {                                                    \
-                INIT_LIST_HEAD(&(_tq)->list);                   \
-                (_tq)->sync = 0;                                \
-                PREPARE_TQUEUE((_tq), (_routine), (_data));     \
-        } while (0)
-#endif
-
-#ifndef container_of
-#define container_of(ptr, type, member) ({			\
-        const typeof( ((type *)0)->member ) *__mptr = (ptr);	\
-        (type *)( (char *)__mptr - offsetof(type,member) );})
-#endif
-
-#ifndef INIT_WORK
-#define work_struct tq_struct
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0))
-#define schedule_work(a)   queue_task(a, &tq_scheduler)
-#else
-#define schedule_work(a)  schedule_task(a)
-#endif
-
-#define flush_scheduled_work  flush_scheduled_tasks
-#define INIT_WORK2(_wq, _routine)  INIT_TQUEUE(_wq, (void (*)(void *))_routine, _wq)
-#endif
-
-#else // >= 2.5 kernel
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
-#define INIT_WORK2(_wq, _routine)	INIT_WORK(_wq, (void (*)(void *))_routine, _wq)
-#else
-#define INIT_WORK2(_wq, _routine)	INIT_WORK(_wq, _routine)
-#endif
-
-#endif // >= 2.5 kernel
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,38))
-typedef struct device netdevice_t;
-#elif (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,4))
-typedef struct net_device netdevice_t;
-#else
 #undef netdevice_t
 typedef struct net_device netdevice_t;
-#endif
 
-#ifdef WIRELESS_EXT
-#if (WIRELESS_EXT < 13)
-struct iw_request_info
-{
-        __u16           cmd;            /* Wireless Extension command */
-        __u16           flags;          /* More to come ;-) */
-};
-#endif
-#endif
-
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,1,18))
-#define MODULE_PARM(a,b)        extern int __bogus_decl
-#define MODULE_AUTHOR(a)        extern int __bogus_decl
-#define MODULE_DESCRIPTION(a)   extern int __bogus_decl
-#define MODULE_SUPPORTED_DEVICE(a) extern int __bogus_decl
-#undef  GET_USE_COUNT
-#define GET_USE_COUNT(m)        mod_use_count_
-#endif
-
-#ifndef MODULE_OWNER
-#define MODULE_OWNER(a)         extern int __bogus_decl
-#define ANCIENT_MODULE_CODE
-#endif
-
-#ifndef MODULE_LICENSE
-#define MODULE_LICENSE(m)       extern int __bogus_decl
-#endif
-
-/* TODO:  Do we care about this? */
-#ifndef MODULE_DEVICE_TABLE
-#define MODULE_DEVICE_TABLE(foo,bar)
-#endif
-
-#define wlan_minutes2ticks(a) ((a)*(wlan_ticks_per_sec *  60))
-#define wlan_seconds2ticks(a) ((a)*(wlan_ticks_per_sec))
-
-#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,47))
-#define NEW_MODULE_CODE
-#ifdef ANCIENT_MODULE_CODE
-#undef ANCIENT_MODULE_CODE
-#endif
-#elif (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,25))
-#define module_param(name, type, perm)                                       \
-        static inline void *__check_existence_##name(void) { return &name; } \
-        MODULE_PARM(name, _MODULE_PARM_STRING_ ## type)
-
-#define _MODULE_PARM_STRING_byte "b"
-#define _MODULE_PARM_STRING_short "h"
-#define _MODULE_PARM_STRING_ushort "h"
-#define _MODULE_PARM_STRING_int "i"
-#define _MODULE_PARM_STRING_uint "i"
-#define _MODULE_PARM_STRING_long "l"
-#define _MODULE_PARM_STRING_ulong "l"
-#define _MODULE_PARM_STRING_bool "i"
-#endif
-
-/* linux < 2.5.69 */
-#ifndef IRQ_NONE
-typedef void irqreturn_t;
-#define IRQ_NONE
-#define IRQ_HANDLED
-#define IRQ_RETVAL(x)
-#endif
-
-#ifndef in_atomic
-#define in_atomic()  0
-#endif
-
-#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,6,13))
 #define URB_ASYNC_UNLINK 0
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,7))
-#define URB_ASYNC_UNLINK  USB_ASYNC_UNLINK
-#define usb_fill_bulk_urb  FILL_BULK_URB
-#define usb_kill_urb  usb_unlink_urb
-#else
 #define USB_QUEUE_BULK 0
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11))
-typedef u32 pm_message_t;
-#endif
-
-#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,6,9))
-#define hotplug_path  "/etc/hotplug/wlan.agent"
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
-#define free_netdev(x)       kfree(x)
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,9))
-#define eth_hdr(x)           (x)->mac.ethernet
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0))
-#define del_timer_sync(a)       del_timer(a)
-#endif
-
-#ifndef might_sleep
-#define might_sleep(a)   do { } while (0)
-#endif
-
-/* Apparently 2.4.2 ethtool is quite different, maybe newer too? */
-#if (defined(SIOETHTOOL) && !defined(ETHTOOL_GDRVINFO))
-#undef SIOETHTOOL
-#endif
-
-// pcmcia-cs stuff
-#if ((LINUX_VERSION_CODE < KERNEL_VERSION(2,5,68)) && \
-     !defined(pcmcia_access_configuration_register))
-#define pcmcia_access_configuration_register(handle, reg) \
-        CardServices(AccessConfigurationRegister, handle, reg)
-#define pcmcia_register_client(handle, reg) \
-        CardServices(RegisterClient, handle, reg)
-#define pcmcia_deregister_client(handle) \
-        CardServices(DeregisterClient, handle)
-#define pcmcia_get_first_tuple(handle, tuple) \
-        CardServices(GetFirstTuple, handle, tuple)
-#define pcmcia_get_next_tuple(handle, tuple) \
-        CardServices(GetNextTuple, handle, tuple)
-#define pcmcia_get_tuple_data(handle, tuple) \
-        CardServices(GetTupleData, handle, tuple)
-#define pcmcia_parse_tuple(handle, tuple, parse) \
-        CardServices(ParseTuple, handle, tuple, parse)
-#define pcmcia_get_configuration_info(handle, config) \
-        CardServices(GetConfigurationInfo, handle, config)
-#define pcmcia_request_io(handle, req) \
-        CardServices(RequestIO, handle, req)
-#define pcmcia_request_irq(handle, req) \
-        CardServices(RequestIRQ, handle, req)
-#define pcmcia_request_configuration(handle, req) \
-        CardServices(RequestConfiguration, handle, req)
-#define pcmcia_release_configuration(handle) \
-        CardServices(ReleaseConfiguration, handle)
-#define pcmcia_release_io(handle, req) \
-        CardServices(ReleaseIO, handle, req)
-#define pcmcia_release_irq(handle, req) \
-        CardServices(ReleaseIRQ, handle, req)
-#define pcmcia_release_window(win) \
-        CardServices(ReleaseWindow, win)
-#define pcmcia_get_card_services_info(info) \
-        CardServices(GetCardServicesInfo, info)
-#define pcmcia_report_error(handle, err) \
-        CardServices(ReportError, handle, err)
-#endif
-
-#endif /* __KERNEL__ */
 
 /*=============================================================*/
 /*------ Hardware Portability Macros --------------------------*/
@@ -692,22 +144,6 @@ typedef u32 pm_message_t;
 #define host2ieee16(n)	__cpu_to_le16(n)
 #define host2ieee32(n)	__cpu_to_le32(n)
 
-#if (WLAN_CPU_FAMILY != WLAN_MIPS)
-typedef UINT32 phys_t;
-#endif
-
-#if (WLAN_CPU_FAMILY == WLAN_PPC)
-       #define wlan_inw(a)                     in_be16((unsigned short *)((a)+_IO_BASE))
-       #define wlan_inw_le16_to_cpu(a)         inw((a))
-       #define wlan_outw(v,a)                  out_be16((unsigned short *)((a)+_IO_BASE), (v))
-       #define wlan_outw_cpu_to_le16(v,a)      outw((v),(a))
-#else
-       #define wlan_inw(a)                     inw((a))
-       #define wlan_inw_le16_to_cpu(a)         __cpu_to_le16(inw((a)))
-       #define wlan_outw(v,a)                  outw((v),(a))
-       #define wlan_outw_cpu_to_le16(v,a)      outw(__cpu_to_le16((v)),(a))
-#endif
-
 /*=============================================================*/
 /*--- General Macros ------------------------------------------*/
 /*=============================================================*/
diff --git a/drivers/thermal/thermal_sys.c b/drivers/thermal/thermal_sys.c
index fe07462d5947..8171ca17b936 100644
--- a/drivers/thermal/thermal_sys.c
+++ b/drivers/thermal/thermal_sys.c
@@ -579,7 +579,7 @@ static void thermal_release(struct device *dev)
 	struct thermal_zone_device *tz;
 	struct thermal_cooling_device *cdev;
 
-	if (!strncmp(dev->bus_id, "thermal_zone", sizeof "thermal_zone" - 1)) {
+	if (!strncmp(dev_name(dev), "thermal_zone", sizeof "thermal_zone" - 1)) {
 		tz = to_thermal_zone(dev);
 		kfree(tz);
 	} else {
@@ -630,7 +630,7 @@ struct thermal_cooling_device *thermal_cooling_device_register(char *type,
 	cdev->ops = ops;
 	cdev->device.class = &thermal_class;
 	cdev->devdata = devdata;
-	sprintf(cdev->device.bus_id, "cooling_device%d", cdev->id);
+	dev_set_name(&cdev->device, "cooling_device%d", cdev->id);
 	result = device_register(&cdev->device);
 	if (result) {
 		release_idr(&thermal_cdev_idr, &thermal_idr_lock, cdev->id);
@@ -769,7 +769,7 @@ struct thermal_zone_device *thermal_zone_device_register(char *type,
 	tz->device.class = &thermal_class;
 	tz->devdata = devdata;
 	tz->trips = trips;
-	sprintf(tz->device.bus_id, "thermal_zone%d", tz->id);
+	dev_set_name(&tz->device, "thermal_zone%d", tz->id);
 	result = device_register(&tz->device);
 	if (result) {
 		release_idr(&thermal_tz_idr, &thermal_idr_lock, tz->id);
diff --git a/drivers/uio/uio.c b/drivers/uio/uio.c
index 2d2440cd57a9..4ca85a113aa2 100644
--- a/drivers/uio/uio.c
+++ b/drivers/uio/uio.c
@@ -35,6 +35,7 @@ struct uio_device {
 	int			vma_count;
 	struct uio_info		*info;
 	struct kobject		*map_dir;
+	struct kobject		*portio_dir;
 };
 
 static int uio_major;
@@ -75,17 +76,17 @@ static ssize_t map_offset_show(struct uio_mem *mem, char *buf)
 	return sprintf(buf, "0x%lx\n", mem->addr & ~PAGE_MASK);
 }
 
-struct uio_sysfs_entry {
+struct map_sysfs_entry {
 	struct attribute attr;
 	ssize_t (*show)(struct uio_mem *, char *);
 	ssize_t (*store)(struct uio_mem *, const char *, size_t);
 };
 
-static struct uio_sysfs_entry addr_attribute =
+static struct map_sysfs_entry addr_attribute =
 	__ATTR(addr, S_IRUGO, map_addr_show, NULL);
-static struct uio_sysfs_entry size_attribute =
+static struct map_sysfs_entry size_attribute =
 	__ATTR(size, S_IRUGO, map_size_show, NULL);
-static struct uio_sysfs_entry offset_attribute =
+static struct map_sysfs_entry offset_attribute =
 	__ATTR(offset, S_IRUGO, map_offset_show, NULL);
 
 static struct attribute *attrs[] = {
@@ -106,9 +107,9 @@ static ssize_t map_type_show(struct kobject *kobj, struct attribute *attr,
 {
 	struct uio_map *map = to_map(kobj);
 	struct uio_mem *mem = map->mem;
-	struct uio_sysfs_entry *entry;
+	struct map_sysfs_entry *entry;
 
-	entry = container_of(attr, struct uio_sysfs_entry, attr);
+	entry = container_of(attr, struct map_sysfs_entry, attr);
 
 	if (!entry->show)
 		return -EIO;
@@ -116,16 +117,93 @@ static ssize_t map_type_show(struct kobject *kobj, struct attribute *attr,
 	return entry->show(mem, buf);
 }
 
-static struct sysfs_ops uio_sysfs_ops = {
+static struct sysfs_ops map_sysfs_ops = {
 	.show = map_type_show,
 };
 
 static struct kobj_type map_attr_type = {
 	.release	= map_release,
-	.sysfs_ops	= &uio_sysfs_ops,
+	.sysfs_ops	= &map_sysfs_ops,
 	.default_attrs	= attrs,
 };
 
+struct uio_portio {
+	struct kobject kobj;
+	struct uio_port *port;
+};
+#define to_portio(portio) container_of(portio, struct uio_portio, kobj)
+
+static ssize_t portio_start_show(struct uio_port *port, char *buf)
+{
+	return sprintf(buf, "0x%lx\n", port->start);
+}
+
+static ssize_t portio_size_show(struct uio_port *port, char *buf)
+{
+	return sprintf(buf, "0x%lx\n", port->size);
+}
+
+static ssize_t portio_porttype_show(struct uio_port *port, char *buf)
+{
+	const char *porttypes[] = {"none", "x86", "gpio", "other"};
+
+	if ((port->porttype < 0) || (port->porttype > UIO_PORT_OTHER))
+		return -EINVAL;
+
+	return sprintf(buf, "port_%s\n", porttypes[port->porttype]);
+}
+
+struct portio_sysfs_entry {
+	struct attribute attr;
+	ssize_t (*show)(struct uio_port *, char *);
+	ssize_t (*store)(struct uio_port *, const char *, size_t);
+};
+
+static struct portio_sysfs_entry portio_start_attribute =
+	__ATTR(start, S_IRUGO, portio_start_show, NULL);
+static struct portio_sysfs_entry portio_size_attribute =
+	__ATTR(size, S_IRUGO, portio_size_show, NULL);
+static struct portio_sysfs_entry portio_porttype_attribute =
+	__ATTR(porttype, S_IRUGO, portio_porttype_show, NULL);
+
+static struct attribute *portio_attrs[] = {
+	&portio_start_attribute.attr,
+	&portio_size_attribute.attr,
+	&portio_porttype_attribute.attr,
+	NULL,
+};
+
+static void portio_release(struct kobject *kobj)
+{
+	struct uio_portio *portio = to_portio(kobj);
+	kfree(portio);
+}
+
+static ssize_t portio_type_show(struct kobject *kobj, struct attribute *attr,
+			     char *buf)
+{
+	struct uio_portio *portio = to_portio(kobj);
+	struct uio_port *port = portio->port;
+	struct portio_sysfs_entry *entry;
+
+	entry = container_of(attr, struct portio_sysfs_entry, attr);
+
+	if (!entry->show)
+		return -EIO;
+
+	return entry->show(port, buf);
+}
+
+static struct sysfs_ops portio_sysfs_ops = {
+	.show = portio_type_show,
+};
+
+static struct kobj_type portio_attr_type = {
+	.release	= portio_release,
+	.sysfs_ops	= &portio_sysfs_ops,
+	.default_attrs	= portio_attrs,
+};
+
 static ssize_t show_name(struct device *dev,
 			 struct device_attribute *attr, char *buf)
 {
@@ -177,10 +255,13 @@ static struct attribute_group uio_attr_grp = {
 static int uio_dev_add_attributes(struct uio_device *idev)
 {
 	int ret;
-	int mi;
+	int mi, pi;
 	int map_found = 0;
+	int portio_found = 0;
 	struct uio_mem *mem;
 	struct uio_map *map;
+	struct uio_port *port;
+	struct uio_portio *portio;
 
 	ret = sysfs_create_group(&idev->dev->kobj, &uio_attr_grp);
 	if (ret)
@@ -195,25 +276,58 @@ static int uio_dev_add_attributes(struct uio_device *idev)
 			idev->map_dir = kobject_create_and_add("maps",
 							&idev->dev->kobj);
 			if (!idev->map_dir)
-				goto err;
+				goto err_map;
 		}
 		map = kzalloc(sizeof(*map), GFP_KERNEL);
 		if (!map)
-			goto err;
+			goto err_map;
 		kobject_init(&map->kobj, &map_attr_type);
 		map->mem = mem;
 		mem->map = map;
 		ret = kobject_add(&map->kobj, idev->map_dir, "map%d", mi);
 		if (ret)
-			goto err;
+			goto err_map;
 		ret = kobject_uevent(&map->kobj, KOBJ_ADD);
 		if (ret)
-			goto err;
+			goto err_map;
+	}
+
+	for (pi = 0; pi < MAX_UIO_PORT_REGIONS; pi++) {
+		port = &idev->info->port[pi];
+		if (port->size == 0)
+			break;
+		if (!portio_found) {
+			portio_found = 1;
+			idev->portio_dir = kobject_create_and_add("portio",
+							&idev->dev->kobj);
+			if (!idev->portio_dir)
+				goto err_portio;
+		}
+		portio = kzalloc(sizeof(*portio), GFP_KERNEL);
+		if (!portio)
+			goto err_portio;
+		kobject_init(&portio->kobj, &portio_attr_type);
+		portio->port = port;
+		port->portio = portio;
+		ret = kobject_add(&portio->kobj, idev->portio_dir,
+							"port%d", pi);
+		if (ret)
+			goto err_portio;
+		ret = kobject_uevent(&portio->kobj, KOBJ_ADD);
+		if (ret)
+			goto err_portio;
 	}
 
 	return 0;
 
-err:
+err_portio:
+	for (pi--; pi >= 0; pi--) {
+		port = &idev->info->port[pi];
+		portio = port->portio;
+		kobject_put(&portio->kobj);
+	}
+	kobject_put(idev->portio_dir);
+err_map:
 	for (mi--; mi>=0; mi--) {
 		mem = &idev->info->mem[mi];
 		map = mem->map;
@@ -228,15 +342,26 @@ err_group:
 
 static void uio_dev_del_attributes(struct uio_device *idev)
 {
-	int mi;
+	int i;
 	struct uio_mem *mem;
-	for (mi = 0; mi < MAX_UIO_MAPS; mi++) {
-		mem = &idev->info->mem[mi];
+	struct uio_port *port;
+
+	for (i = 0; i < MAX_UIO_MAPS; i++) {
+		mem = &idev->info->mem[i];
 		if (mem->size == 0)
 			break;
 		kobject_put(&mem->map->kobj);
 	}
 	kobject_put(idev->map_dir);
+
+	for (i = 0; i < MAX_UIO_PORT_REGIONS; i++) {
+		port = &idev->info->port[i];
+		if (port->size == 0)
+			break;
+		kobject_put(&port->portio->kobj);
+	}
+	kobject_put(idev->portio_dir);
+
 	sysfs_remove_group(&idev->dev->kobj, &uio_attr_grp);
 }
 
diff --git a/drivers/uio/uio_cif.c b/drivers/uio/uio_cif.c
index 57376060b978..c60b8fcf0e3e 100644
--- a/drivers/uio/uio_cif.c
+++ b/drivers/uio/uio_cif.c
@@ -57,8 +57,7 @@ static int __devinit hilscher_pci_probe(struct pci_dev *dev,
 	info->mem[0].addr = pci_resource_start(dev, 0);
 	if (!info->mem[0].addr)
 		goto out_release;
-	info->mem[0].internal_addr = ioremap(pci_resource_start(dev, 0),
-					     pci_resource_len(dev, 0));
+	info->mem[0].internal_addr = pci_ioremap_bar(dev, 0);
 	if (!info->mem[0].internal_addr)
 		goto out_release;
 
diff --git a/drivers/uio/uio_pdrv_genirq.c b/drivers/uio/uio_pdrv_genirq.c
index 1f82c83a92ae..3f06818cf9fa 100644
--- a/drivers/uio/uio_pdrv_genirq.c
+++ b/drivers/uio/uio_pdrv_genirq.c
@@ -81,7 +81,8 @@ static int uio_pdrv_genirq_probe(struct platform_device *pdev)
 		goto bad0;
 	}
 
-	if (uioinfo->handler || uioinfo->irqcontrol || uioinfo->irq_flags) {
+	if (uioinfo->handler || uioinfo->irqcontrol ||
+	    uioinfo->irq_flags & IRQF_SHARED) {
 		dev_err(&pdev->dev, "interrupt configuration error\n");
 		goto bad0;
 	}
@@ -132,7 +133,7 @@ static int uio_pdrv_genirq_probe(struct platform_device *pdev)
 	 * Interrupt sharing is not supported.
 	 */
 
-	uioinfo->irq_flags = IRQF_DISABLED;
+	uioinfo->irq_flags |= IRQF_DISABLED;
 	uioinfo->handler = uio_pdrv_genirq_handler;
 	uioinfo->irqcontrol = uio_pdrv_genirq_irqcontrol;
 	uioinfo->priv = priv;
diff --git a/drivers/usb/Kconfig b/drivers/usb/Kconfig
index 289d81adfb9c..83babb0a1df7 100644
--- a/drivers/usb/Kconfig
+++ b/drivers/usb/Kconfig
@@ -150,4 +150,6 @@ source "drivers/usb/atm/Kconfig"
 
 source "drivers/usb/gadget/Kconfig"
 
+source "drivers/usb/otg/Kconfig"
+
 endif # USB_SUPPORT
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index d50a99f70aee..00b47ea24f86 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -1275,7 +1275,7 @@ static int acm_suspend(struct usb_interface *intf, pm_message_t message)
 	struct acm *acm = usb_get_intfdata(intf);
 	int cnt;
 
-	if (acm->dev->auto_pm) {
+	if (message.event & PM_EVENT_AUTO) {
 		int b;
 
 		spin_lock_irq(&acm->read_lock);
diff --git a/drivers/usb/class/cdc-wdm.c b/drivers/usb/class/cdc-wdm.c
index 5a8ecc045e3f..3771d6e6d0cc 100644
--- a/drivers/usb/class/cdc-wdm.c
+++ b/drivers/usb/class/cdc-wdm.c
@@ -764,7 +764,8 @@ static int wdm_suspend(struct usb_interface *intf, pm_message_t message)
 
 	mutex_lock(&desc->plock);
 #ifdef CONFIG_PM
-	if (interface_to_usbdev(desc->intf)->auto_pm && test_bit(WDM_IN_USE, &desc->flags)) {
+	if ((message.event & PM_EVENT_AUTO) &&
+			test_bit(WDM_IN_USE, &desc->flags)) {
 		rv = -EBUSY;
 	} else {
 #endif
diff --git a/drivers/usb/class/usbtmc.c b/drivers/usb/class/usbtmc.c
index 43a863c5cc43..0f5c05f6f9df 100644
--- a/drivers/usb/class/usbtmc.c
+++ b/drivers/usb/class/usbtmc.c
@@ -21,6 +21,7 @@
 
 #include <linux/init.h>
 #include <linux/module.h>
+#include <linux/kernel.h>
 #include <linux/fs.h>
 #include <linux/uaccess.h>
 #include <linux/kref.h>
@@ -482,7 +483,6 @@ static ssize_t usbtmc_write(struct file *filp, const char __user *buf,
 	int retval;
 	int actual;
 	unsigned long int n_bytes;
-	int n;
 	int remaining;
 	int done;
 	int this_part;
@@ -526,11 +526,8 @@ static ssize_t usbtmc_write(struct file *filp, const char __user *buf,
 			goto exit;
 		}
 
-		n_bytes = 12 + this_part;
-		if (this_part % 4)
-			n_bytes += 4 - this_part % 4;
-			for (n = 12 + this_part; n < n_bytes; n++)
-				buffer[n] = 0;
+		n_bytes = roundup(12 + this_part, 4);
+		memset(buffer + 12 + this_part, 0, n_bytes - (12 + this_part));
 
 		retval = usb_bulk_msg(data->usb_dev,
 				      usb_sndbulkpipe(data->usb_dev,
diff --git a/drivers/usb/core/devio.c b/drivers/usb/core/devio.c
index aa79280df15d..26fece124e0e 100644
--- a/drivers/usb/core/devio.c
+++ b/drivers/usb/core/devio.c
@@ -981,9 +981,6 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 		return -EINVAL;
 	if (!uurb->buffer)
 		return -EINVAL;
-	if (uurb->signr != 0 && (uurb->signr < SIGRTMIN ||
-				 uurb->signr > SIGRTMAX))
-		return -EINVAL;
 	if (!(uurb->type == USBDEVFS_URB_TYPE_CONTROL &&
 	    (uurb->endpoint & ~USB_ENDPOINT_DIR_MASK) == 0)) {
 		ifnum = findintfep(ps->dev, uurb->endpoint);
@@ -1320,7 +1317,7 @@ static int get_urb32(struct usbdevfs_urb *kurb,
 	if (__get_user(uptr, &uurb->buffer))
 		return -EFAULT;
 	kurb->buffer = compat_ptr(uptr);
-	if (__get_user(uptr, &uurb->buffer))
+	if (__get_user(uptr, &uurb->usercontext))
 		return -EFAULT;
 	kurb->usercontext = compat_ptr(uptr);
 
@@ -1401,8 +1398,6 @@ static int proc_disconnectsignal(struct dev_state *ps, void __user *arg)
 
 	if (copy_from_user(&ds, arg, sizeof(ds)))
 		return -EFAULT;
-	if (ds.signr != 0 && (ds.signr < SIGRTMIN || ds.signr > SIGRTMAX))
-		return -EINVAL;
 	ps->discsignr = ds.signr;
 	ps->disccontext = ds.context;
 	return 0;
diff --git a/drivers/usb/core/driver.c b/drivers/usb/core/driver.c
index 8c081308b0e2..98760553bc95 100644
--- a/drivers/usb/core/driver.c
+++ b/drivers/usb/core/driver.c
@@ -184,6 +184,20 @@ static int usb_unbind_device(struct device *dev)
 	return 0;
 }
 
+/*
+ * Cancel any pending scheduled resets
+ *
+ * [see usb_queue_reset_device()]
+ *
+ * Called after unconfiguring / when releasing interfaces. See
+ * comments in __usb_queue_reset_device() regarding
+ * udev->reset_running.
+ */
+static void usb_cancel_queued_reset(struct usb_interface *iface)
+{
+	if (iface->reset_running == 0)
+		cancel_work_sync(&iface->reset_ws);
+}
 
 /* called from driver core with dev locked */
 static int usb_probe_interface(struct device *dev)
@@ -242,6 +256,7 @@ static int usb_probe_interface(struct device *dev)
 			mark_quiesced(intf);
 			intf->needs_remote_wakeup = 0;
 			intf->condition = USB_INTERFACE_UNBOUND;
+			usb_cancel_queued_reset(intf);
 		} else
 			intf->condition = USB_INTERFACE_BOUND;
 
@@ -272,6 +287,7 @@ static int usb_unbind_interface(struct device *dev)
 		usb_disable_interface(udev, intf);
 
 	driver->disconnect(intf);
+	usb_cancel_queued_reset(intf);
 
 	/* Reset other interface state.
 	 * We cannot do a Set-Interface if the device is suspended or
@@ -279,9 +295,12 @@ static int usb_unbind_interface(struct device *dev)
 	 * altsetting means creating new endpoint device entries).
 	 * When either of these happens, defer the Set-Interface.
 	 */
-	if (intf->cur_altsetting->desc.bAlternateSetting == 0)
-		;	/* Already in altsetting 0 so skip Set-Interface */
-	else if (!error && intf->dev.power.status == DPM_ON)
+	if (intf->cur_altsetting->desc.bAlternateSetting == 0) {
+		/* Already in altsetting 0 so skip Set-Interface.
+		 * Just re-enable it without affecting the endpoint toggles.
+		 */
+		usb_enable_interface(udev, intf, false);
+	} else if (!error && intf->dev.power.status == DPM_ON)
 		usb_set_interface(udev, intf->altsetting[0].
 				desc.bInterfaceNumber, 0);
 	else
@@ -380,8 +399,10 @@ void usb_driver_release_interface(struct usb_driver *driver,
 	if (device_is_registered(dev)) {
 		iface->condition = USB_INTERFACE_UNBINDING;
 		device_release_driver(dev);
+	} else {
+		iface->condition = USB_INTERFACE_UNBOUND;
+		usb_cancel_queued_reset(iface);
 	}
-
 	dev->driver = NULL;
 	usb_set_intfdata(iface, NULL);
 
@@ -904,7 +925,7 @@ static int usb_suspend_device(struct usb_device *udev, pm_message_t msg)
 }
 
 /* Caller has locked udev's pm_mutex */
-static int usb_resume_device(struct usb_device *udev)
+static int usb_resume_device(struct usb_device *udev, pm_message_t msg)
 {
 	struct usb_device_driver	*udriver;
 	int				status = 0;
@@ -922,7 +943,7 @@ static int usb_resume_device(struct usb_device *udev)
 		udev->reset_resume = 1;
 
 	udriver = to_usb_device_driver(udev->dev.driver);
-	status = udriver->resume(udev);
+	status = udriver->resume(udev, msg);
 
  done:
 	dev_vdbg(&udev->dev, "%s: status %d\n", __func__, status);
@@ -942,7 +963,8 @@ static int usb_suspend_interface(struct usb_device *udev,
 	if (udev->state == USB_STATE_NOTATTACHED || !is_active(intf))
 		goto done;
 
-	if (intf->condition == USB_INTERFACE_UNBOUND)	/* This can't happen */
+	/* This can happen; see usb_driver_release_interface() */
+	if (intf->condition == USB_INTERFACE_UNBOUND)
 		goto done;
 	driver = to_usb_driver(intf->dev.driver);
 
@@ -950,7 +972,7 @@ static int usb_suspend_interface(struct usb_device *udev,
 		status = driver->suspend(intf, msg);
 		if (status == 0)
 			mark_quiesced(intf);
-		else if (!udev->auto_pm)
+		else if (!(msg.event & PM_EVENT_AUTO))
 			dev_err(&intf->dev, "%s error %d\n",
 					"suspend", status);
 	} else {
@@ -968,7 +990,7 @@ static int usb_suspend_interface(struct usb_device *udev,
 
 /* Caller has locked intf's usb_device's pm_mutex */
 static int usb_resume_interface(struct usb_device *udev,
-		struct usb_interface *intf, int reset_resume)
+		struct usb_interface *intf, pm_message_t msg, int reset_resume)
 {
 	struct usb_driver	*driver;
 	int			status = 0;
@@ -1092,7 +1114,7 @@ static int autosuspend_check(struct usb_device *udev, int reschedule)
 	if (reschedule) {
 		if (!timer_pending(&udev->autosuspend.timer)) {
 			queue_delayed_work(ksuspend_usb_wq, &udev->autosuspend,
-				round_jiffies_relative(suspend_time - j));
+				round_jiffies_up_relative(suspend_time - j));
 		}
 		return -EAGAIN;
 	}
@@ -1119,10 +1141,9 @@ static inline int autosuspend_check(struct usb_device *udev, int reschedule)
  * all the interfaces which were suspended are resumed so that they remain
  * in the same state as the device.
  *
- * If an autosuspend is in progress (@udev->auto_pm is set), the routine
- * checks first to make sure that neither the device itself or any of its
- * active interfaces is in use (pm_usage_cnt is greater than 0).  If they
- * are, the autosuspend fails.
+ * If an autosuspend is in progress the routine checks first to make sure
+ * that neither the device itself or any of its active interfaces is in use
+ * (pm_usage_cnt is greater than 0).  If they are, the autosuspend fails.
  *
  * If the suspend succeeds, the routine recursively queues an autosuspend
  * request for @udev's parent device, thereby propagating the change up
@@ -1157,7 +1178,7 @@ static int usb_suspend_both(struct usb_device *udev, pm_message_t msg)
 
 	udev->do_remote_wakeup = device_may_wakeup(&udev->dev);
 
-	if (udev->auto_pm) {
+	if (msg.event & PM_EVENT_AUTO) {
 		status = autosuspend_check(udev, 0);
 		if (status < 0)
 			goto done;
@@ -1177,13 +1198,16 @@ static int usb_suspend_both(struct usb_device *udev, pm_message_t msg)
 
 	/* If the suspend failed, resume interfaces that did get suspended */
 	if (status != 0) {
+		pm_message_t msg2;
+
+		msg2.event = msg.event ^ (PM_EVENT_SUSPEND | PM_EVENT_RESUME);
 		while (--i >= 0) {
 			intf = udev->actconfig->interface[i];
-			usb_resume_interface(udev, intf, 0);
+			usb_resume_interface(udev, intf, msg2, 0);
 		}
 
 		/* Try another autosuspend when the interfaces aren't busy */
-		if (udev->auto_pm)
+		if (msg.event & PM_EVENT_AUTO)
 			autosuspend_check(udev, status == -EBUSY);
 
 	/* If the suspend succeeded then prevent any more URB submissions,
@@ -1213,6 +1237,7 @@ static int usb_suspend_both(struct usb_device *udev, pm_message_t msg)
 /**
  * usb_resume_both - resume a USB device and its interfaces
  * @udev: the usb_device to resume
+ * @msg: Power Management message describing this state transition
  *
  * This is the central routine for resuming USB devices.  It calls the
  * the resume method for @udev and then calls the resume methods for all
@@ -1238,7 +1263,7 @@ static int usb_suspend_both(struct usb_device *udev, pm_message_t msg)
  *
  * This routine can run only in process context.
  */
-static int usb_resume_both(struct usb_device *udev)
+static int usb_resume_both(struct usb_device *udev, pm_message_t msg)
 {
 	int			status = 0;
 	int			i;
@@ -1254,14 +1279,15 @@ static int usb_resume_both(struct usb_device *udev)
 
 	/* Propagate the resume up the tree, if necessary */
 	if (udev->state == USB_STATE_SUSPENDED) {
-		if (udev->auto_pm && udev->autoresume_disabled) {
+		if ((msg.event & PM_EVENT_AUTO) &&
+				udev->autoresume_disabled) {
 			status = -EPERM;
 			goto done;
 		}
 		if (parent) {
 			status = usb_autoresume_device(parent);
 			if (status == 0) {
-				status = usb_resume_device(udev);
+				status = usb_resume_device(udev, msg);
 				if (status || udev->state ==
 						USB_STATE_NOTATTACHED) {
 					usb_autosuspend_device(parent);
@@ -1284,15 +1310,16 @@ static int usb_resume_both(struct usb_device *udev)
 			/* We can't progagate beyond the USB subsystem,
 			 * so if a root hub's controller is suspended
 			 * then we're stuck. */
-			status = usb_resume_device(udev);
+			status = usb_resume_device(udev, msg);
 		}
 	} else if (udev->reset_resume)
-		status = usb_resume_device(udev);
+		status = usb_resume_device(udev, msg);
 
 	if (status == 0 && udev->actconfig) {
 		for (i = 0; i < udev->actconfig->desc.bNumInterfaces; i++) {
 			intf = udev->actconfig->interface[i];
-			usb_resume_interface(udev, intf, udev->reset_resume);
+			usb_resume_interface(udev, intf, msg,
+					udev->reset_resume);
 		}
 	}
 
@@ -1320,13 +1347,13 @@ static int usb_autopm_do_device(struct usb_device *udev, int inc_usage_cnt)
 		udev->last_busy = jiffies;
 	if (inc_usage_cnt >= 0 && udev->pm_usage_cnt > 0) {
 		if (udev->state == USB_STATE_SUSPENDED)
-			status = usb_resume_both(udev);
+			status = usb_resume_both(udev, PMSG_AUTO_RESUME);
 		if (status != 0)
 			udev->pm_usage_cnt -= inc_usage_cnt;
 		else if (inc_usage_cnt)
 			udev->last_busy = jiffies;
 	} else if (inc_usage_cnt <= 0 && udev->pm_usage_cnt <= 0) {
-		status = usb_suspend_both(udev, PMSG_SUSPEND);
+		status = usb_suspend_both(udev, PMSG_AUTO_SUSPEND);
 	}
 	usb_pm_unlock(udev);
 	return status;
@@ -1341,6 +1368,19 @@ void usb_autosuspend_work(struct work_struct *work)
 	usb_autopm_do_device(udev, 0);
 }
 
+/* usb_autoresume_work - callback routine to autoresume a USB device */
+void usb_autoresume_work(struct work_struct *work)
+{
+	struct usb_device *udev =
+		container_of(work, struct usb_device, autoresume);
+
+	/* Wake it up, let the drivers do their thing, and then put it
+	 * back to sleep.
+	 */
+	if (usb_autopm_do_device(udev, 1) == 0)
+		usb_autopm_do_device(udev, -1);
+}
+
 /**
  * usb_autosuspend_device - delayed autosuspend of a USB device and its interfaces
  * @udev: the usb_device to autosuspend
@@ -1437,13 +1477,14 @@ static int usb_autopm_do_interface(struct usb_interface *intf,
 		udev->last_busy = jiffies;
 		if (inc_usage_cnt >= 0 && intf->pm_usage_cnt > 0) {
 			if (udev->state == USB_STATE_SUSPENDED)
-				status = usb_resume_both(udev);
+				status = usb_resume_both(udev,
+						PMSG_AUTO_RESUME);
 			if (status != 0)
 				intf->pm_usage_cnt -= inc_usage_cnt;
 			else
 				udev->last_busy = jiffies;
 		} else if (inc_usage_cnt <= 0 && intf->pm_usage_cnt <= 0) {
-			status = usb_suspend_both(udev, PMSG_SUSPEND);
+			status = usb_suspend_both(udev, PMSG_AUTO_SUSPEND);
 		}
 	}
 	usb_pm_unlock(udev);
@@ -1492,6 +1533,45 @@ void usb_autopm_put_interface(struct usb_interface *intf)
 EXPORT_SYMBOL_GPL(usb_autopm_put_interface);
 
 /**
+ * usb_autopm_put_interface_async - decrement a USB interface's PM-usage counter
+ * @intf: the usb_interface whose counter should be decremented
+ *
+ * This routine does essentially the same thing as
+ * usb_autopm_put_interface(): it decrements @intf's usage counter and
+ * queues a delayed autosuspend request if the counter is <= 0.  The
+ * difference is that it does not acquire the device's pm_mutex;
+ * callers must handle all synchronization issues themselves.
+ *
+ * Typically a driver would call this routine during an URB's completion
+ * handler, if no more URBs were pending.
+ *
+ * This routine can run in atomic context.
+ */
+void usb_autopm_put_interface_async(struct usb_interface *intf)
+{
+	struct usb_device	*udev = interface_to_usbdev(intf);
+	int			status = 0;
+
+	if (intf->condition == USB_INTERFACE_UNBOUND) {
+		status = -ENODEV;
+	} else {
+		udev->last_busy = jiffies;
+		--intf->pm_usage_cnt;
+		if (udev->autosuspend_disabled || udev->autosuspend_delay < 0)
+			status = -EPERM;
+		else if (intf->pm_usage_cnt <= 0 &&
+				!timer_pending(&udev->autosuspend.timer)) {
+			queue_delayed_work(ksuspend_usb_wq, &udev->autosuspend,
+					round_jiffies_up_relative(
+						udev->autosuspend_delay));
+		}
+	}
+	dev_vdbg(&intf->dev, "%s: status %d cnt %d\n",
+			__func__, status, intf->pm_usage_cnt);
+}
+EXPORT_SYMBOL_GPL(usb_autopm_put_interface_async);
+
+/**
  * usb_autopm_get_interface - increment a USB interface's PM-usage counter
  * @intf: the usb_interface whose counter should be incremented
  *
@@ -1537,6 +1617,37 @@ int usb_autopm_get_interface(struct usb_interface *intf)
 EXPORT_SYMBOL_GPL(usb_autopm_get_interface);
 
 /**
+ * usb_autopm_get_interface_async - increment a USB interface's PM-usage counter
+ * @intf: the usb_interface whose counter should be incremented
+ *
+ * This routine does much the same thing as
+ * usb_autopm_get_interface(): it increments @intf's usage counter and
+ * queues an autoresume request if the result is > 0.  The differences
+ * are that it does not acquire the device's pm_mutex (callers must
+ * handle all synchronization issues themselves), and it does not
+ * autoresume the device directly (it only queues a request).  After a
+ * successful call, the device will generally not yet be resumed.
+ *
+ * This routine can run in atomic context.
+ */
+int usb_autopm_get_interface_async(struct usb_interface *intf)
+{
+	struct usb_device	*udev = interface_to_usbdev(intf);
+	int			status = 0;
+
+	if (intf->condition == USB_INTERFACE_UNBOUND)
+		status = -ENODEV;
+	else if (udev->autoresume_disabled)
+		status = -EPERM;
+	else if (++intf->pm_usage_cnt > 0 && udev->state == USB_STATE_SUSPENDED)
+		queue_work(ksuspend_usb_wq, &udev->autoresume);
+	dev_vdbg(&intf->dev, "%s: status %d cnt %d\n",
+			__func__, status, intf->pm_usage_cnt);
+	return status;
+}
+EXPORT_SYMBOL_GPL(usb_autopm_get_interface_async);
+
+/**
  * usb_autopm_set_interface - set a USB interface's autosuspend state
  * @intf: the usb_interface whose state should be set
  *
@@ -1563,6 +1674,9 @@ EXPORT_SYMBOL_GPL(usb_autopm_set_interface);
 void usb_autosuspend_work(struct work_struct *work)
 {}
 
+void usb_autoresume_work(struct work_struct *work)
+{}
+
 #endif /* CONFIG_USB_SUSPEND */
 
 /**
@@ -1595,6 +1709,7 @@ int usb_external_suspend_device(struct usb_device *udev, pm_message_t msg)
 /**
  * usb_external_resume_device - external resume of a USB device and its interfaces
  * @udev: the usb_device to resume
+ * @msg: Power Management message describing this state transition
  *
  * This routine handles external resume requests: ones not generated
  * internally by a USB driver (autoresume) but rather coming from the user
@@ -1603,13 +1718,13 @@ int usb_external_suspend_device(struct usb_device *udev, pm_message_t msg)
  *
  * The caller must hold @udev's device lock.
  */
-int usb_external_resume_device(struct usb_device *udev)
+int usb_external_resume_device(struct usb_device *udev, pm_message_t msg)
 {
 	int	status;
 
 	usb_pm_lock(udev);
 	udev->auto_pm = 0;
-	status = usb_resume_both(udev);
+	status = usb_resume_both(udev, msg);
 	udev->last_busy = jiffies;
 	usb_pm_unlock(udev);
 	if (status == 0)
@@ -1622,7 +1737,7 @@ int usb_external_resume_device(struct usb_device *udev)
 	return status;
 }
 
-int usb_suspend(struct device *dev, pm_message_t message)
+int usb_suspend(struct device *dev, pm_message_t msg)
 {
 	struct usb_device	*udev;
 
@@ -1641,10 +1756,10 @@ int usb_suspend(struct device *dev, pm_message_t message)
 	}
 
 	udev->skip_sys_resume = 0;
-	return usb_external_suspend_device(udev, message);
+	return usb_external_suspend_device(udev, msg);
 }
 
-int usb_resume(struct device *dev)
+int usb_resume(struct device *dev, pm_message_t msg)
 {
 	struct usb_device	*udev;
 
@@ -1656,7 +1771,7 @@ int usb_resume(struct device *dev)
 	 */
 	if (udev->skip_sys_resume)
 		return 0;
-	return usb_external_resume_device(udev);
+	return usb_external_resume_device(udev, msg);
 }
 
 #endif /* CONFIG_PM */
diff --git a/drivers/usb/core/endpoint.c b/drivers/usb/core/endpoint.c
index 946fae43d622..e1710f260b4f 100644
--- a/drivers/usb/core/endpoint.c
+++ b/drivers/usb/core/endpoint.c
@@ -276,7 +276,7 @@ static void ep_device_release(struct device *dev)
 	kfree(ep_dev);
 }
 
-int usb_create_ep_files(struct device *parent,
+int usb_create_ep_devs(struct device *parent,
 			struct usb_host_endpoint *endpoint,
 			struct usb_device *udev)
 {
@@ -340,7 +340,7 @@ exit:
 	return retval;
 }
 
-void usb_remove_ep_files(struct usb_host_endpoint *endpoint)
+void usb_remove_ep_devs(struct usb_host_endpoint *endpoint)
 {
 	struct ep_device *ep_dev = endpoint->ep_dev;
 
diff --git a/drivers/usb/core/generic.c b/drivers/usb/core/generic.c
index 7e912f21fd36..30ecac3af15a 100644
--- a/drivers/usb/core/generic.c
+++ b/drivers/usb/core/generic.c
@@ -200,18 +200,18 @@ static int generic_suspend(struct usb_device *udev, pm_message_t msg)
 	 * interfaces manually by doing a bus (or "global") suspend.
 	 */
 	if (!udev->parent)
-		rc = hcd_bus_suspend(udev);
+		rc = hcd_bus_suspend(udev, msg);
 
 	/* Non-root devices don't need to do anything for FREEZE or PRETHAW */
 	else if (msg.event == PM_EVENT_FREEZE || msg.event == PM_EVENT_PRETHAW)
 		rc = 0;
 	else
-		rc = usb_port_suspend(udev);
+		rc = usb_port_suspend(udev, msg);
 
 	return rc;
 }
 
-static int generic_resume(struct usb_device *udev)
+static int generic_resume(struct usb_device *udev, pm_message_t msg)
 {
 	int rc;
 
@@ -221,9 +221,9 @@ static int generic_resume(struct usb_device *udev)
 	 * interfaces manually by doing a bus (or "global") resume.
 	 */
 	if (!udev->parent)
-		rc = hcd_bus_resume(udev);
+		rc = hcd_bus_resume(udev, msg);
 	else
-		rc = usb_port_resume(udev);
+		rc = usb_port_resume(udev, msg);
 	return rc;
 }
 
diff --git a/drivers/usb/core/hcd-pci.c b/drivers/usb/core/hcd-pci.c
index 5b87ae7f0a6a..507741ed4482 100644
--- a/drivers/usb/core/hcd-pci.c
+++ b/drivers/usb/core/hcd-pci.c
@@ -128,6 +128,7 @@ int usb_hcd_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	}
 
 	pci_set_master(dev);
+	device_set_wakeup_enable(&dev->dev, 1);
 
 	retval = usb_add_hcd(hcd, dev->irq, IRQF_DISABLED | IRQF_SHARED);
 	if (retval != 0)
@@ -191,17 +192,15 @@ EXPORT_SYMBOL_GPL(usb_hcd_pci_remove);
 /**
  * usb_hcd_pci_suspend - power management suspend of a PCI-based HCD
  * @dev: USB Host Controller being suspended
- * @message: semantics in flux
+ * @message: Power Management message describing this state transition
  *
- * Store this function in the HCD's struct pci_driver as suspend().
+ * Store this function in the HCD's struct pci_driver as .suspend.
  */
 int usb_hcd_pci_suspend(struct pci_dev *dev, pm_message_t message)
 {
-	struct usb_hcd		*hcd;
+	struct usb_hcd		*hcd = pci_get_drvdata(dev);
 	int			retval = 0;
-	int			has_pci_pm;
-
-	hcd = pci_get_drvdata(dev);
+	int			wake, w;
 
 	/* Root hub suspend should have stopped all downstream traffic,
 	 * and all bus master traffic.  And done so for both the interface
@@ -212,8 +211,15 @@ int usb_hcd_pci_suspend(struct pci_dev *dev, pm_message_t message)
 	 * otherwise the swsusp will save (and restore) garbage state.
 	 */
 	if (!(hcd->state == HC_STATE_SUSPENDED ||
-			hcd->state == HC_STATE_HALT))
-		return -EBUSY;
+			hcd->state == HC_STATE_HALT)) {
+		dev_warn(&dev->dev, "Root hub is not suspended\n");
+		retval = -EBUSY;
+		goto done;
+	}
+
+	/* We might already be suspended (runtime PM -- not yet written) */
+	if (dev->current_state != PCI_D0)
+		goto done;
 
 	if (hcd->driver->pci_suspend) {
 		retval = hcd->driver->pci_suspend(hcd, message);
@@ -221,49 +227,60 @@ int usb_hcd_pci_suspend(struct pci_dev *dev, pm_message_t message)
 		if (retval)
 			goto done;
 	}
-	synchronize_irq(dev->irq);
 
-	/* FIXME until the generic PM interfaces change a lot more, this
-	 * can't use PCI D1 and D2 states.  For example, the confusion
-	 * between messages and states will need to vanish, and messages
-	 * will need to provide a target system state again.
-	 *
-	 * It'll be important to learn characteristics of the target state,
-	 * especially on embedded hardware where the HCD will often be in
-	 * charge of an external VBUS power supply and one or more clocks.
-	 * Some target system states will leave them active; others won't.
-	 * (With PCI, that's often handled by platform BIOS code.)
-	 */
+	synchronize_irq(dev->irq);
 
-	/* even when the PCI layer rejects some of the PCI calls
-	 * below, HCs can try global suspend and reduce DMA traffic.
-	 * PM-sensitive HCDs may already have done this.
+	/* Don't fail on error to enable wakeup.  We rely on pci code
+	 * to reject requests the hardware can't implement, rather
+	 * than coding the same thing.
 	 */
-	has_pci_pm = pci_find_capability(dev, PCI_CAP_ID_PM);
+	wake = (hcd->state == HC_STATE_SUSPENDED &&
+			device_may_wakeup(&dev->dev));
+	w = pci_wake_from_d3(dev, wake);
+	if (w < 0)
+		wake = w;
+	dev_dbg(&dev->dev, "wakeup: %d\n", wake);
 
 	/* Downstream ports from this root hub should already be quiesced, so
 	 * there will be no DMA activity.  Now we can shut down the upstream
 	 * link (except maybe for PME# resume signaling) and enter some PCI
 	 * low power state, if the hardware allows.
 	 */
-	if (hcd->state == HC_STATE_SUSPENDED) {
+	pci_disable_device(dev);
+ done:
+	return retval;
+}
+EXPORT_SYMBOL_GPL(usb_hcd_pci_suspend);
 
-		/* no DMA or IRQs except when HC is active */
-		if (dev->current_state == PCI_D0) {
-			pci_save_state(dev);
-			pci_disable_device(dev);
-		}
+/**
+ * usb_hcd_pci_suspend_late - suspend a PCI-based HCD after IRQs are disabled
+ * @dev: USB Host Controller being suspended
+ * @message: Power Management message describing this state transition
+ *
+ * Store this function in the HCD's struct pci_driver as .suspend_late.
+ */
+int usb_hcd_pci_suspend_late(struct pci_dev *dev, pm_message_t message)
+{
+	int			retval = 0;
+	int			has_pci_pm;
 
-		if (message.event == PM_EVENT_FREEZE ||
-				message.event == PM_EVENT_PRETHAW) {
-			dev_dbg(hcd->self.controller, "--> no state change\n");
-			goto done;
-		}
+	/* We might already be suspended (runtime PM -- not yet written) */
+	if (dev->current_state != PCI_D0)
+		goto done;
 
-		if (!has_pci_pm) {
-			dev_dbg(hcd->self.controller, "--> PCI D0/legacy\n");
-			goto done;
-		}
+	pci_save_state(dev);
+
+	/* Don't change state if we don't need to */
+	if (message.event == PM_EVENT_FREEZE ||
+			message.event == PM_EVENT_PRETHAW) {
+		dev_dbg(&dev->dev, "--> no state change\n");
+		goto done;
+	}
+
+	has_pci_pm = pci_find_capability(dev, PCI_CAP_ID_PM);
+	if (!has_pci_pm) {
+		dev_dbg(&dev->dev, "--> PCI D0 legacy\n");
+	} else {
 
 		/* NOTE:  dev->current_state becomes nonzero only here, and
 		 * only for devices that support PCI PM.  Also, exiting
@@ -273,35 +290,16 @@ int usb_hcd_pci_suspend(struct pci_dev *dev, pm_message_t message)
 		retval = pci_set_power_state(dev, PCI_D3hot);
 		suspend_report_result(pci_set_power_state, retval);
 		if (retval == 0) {
-			int wake = device_can_wakeup(&hcd->self.root_hub->dev);
-
-			wake = wake && device_may_wakeup(hcd->self.controller);
-
-			dev_dbg(hcd->self.controller, "--> PCI D3%s\n",
-					wake ? "/wakeup" : "");
-
-			/* Ignore these return values.  We rely on pci code to
-			 * reject requests the hardware can't implement, rather
-			 * than coding the same thing.
-			 */
-			(void) pci_enable_wake(dev, PCI_D3hot, wake);
-			(void) pci_enable_wake(dev, PCI_D3cold, wake);
+			dev_dbg(&dev->dev, "--> PCI D3\n");
 		} else {
 			dev_dbg(&dev->dev, "PCI D3 suspend fail, %d\n",
 					retval);
-			(void) usb_hcd_pci_resume(dev);
+			pci_restore_state(dev);
 		}
-
-	} else if (hcd->state != HC_STATE_HALT) {
-		dev_dbg(hcd->self.controller, "hcd state %d; not suspended\n",
-			hcd->state);
-		WARN_ON(1);
-		retval = -EINVAL;
 	}
 
-done:
-	if (retval == 0) {
 #ifdef CONFIG_PPC_PMAC
+	if (retval == 0) {
 		/* Disable ASIC clocks for USB */
 		if (machine_is(powermac)) {
 			struct device_node	*of_node;
@@ -311,30 +309,24 @@ done:
 				pmac_call_feature(PMAC_FTR_USB_ENABLE,
 							of_node, 0, 0);
 		}
-#endif
 	}
+#endif
 
+ done:
 	return retval;
 }
-EXPORT_SYMBOL_GPL(usb_hcd_pci_suspend);
+EXPORT_SYMBOL_GPL(usb_hcd_pci_suspend_late);
 
 /**
- * usb_hcd_pci_resume - power management resume of a PCI-based HCD
+ * usb_hcd_pci_resume_early - resume a PCI-based HCD before IRQs are enabled
  * @dev: USB Host Controller being resumed
  *
- * Store this function in the HCD's struct pci_driver as resume().
+ * Store this function in the HCD's struct pci_driver as .resume_early.
  */
-int usb_hcd_pci_resume(struct pci_dev *dev)
+int usb_hcd_pci_resume_early(struct pci_dev *dev)
 {
-	struct usb_hcd		*hcd;
-	int			retval;
-
-	hcd = pci_get_drvdata(dev);
-	if (hcd->state != HC_STATE_SUSPENDED) {
-		dev_dbg(hcd->self.controller,
-				"can't resume, not suspended!\n");
-		return 0;
-	}
+	int		retval = 0;
+	pci_power_t	state = dev->current_state;
 
 #ifdef CONFIG_PPC_PMAC
 	/* Reenable ASIC clocks for USB */
@@ -352,7 +344,7 @@ int usb_hcd_pci_resume(struct pci_dev *dev)
 	 * calls "standby", "suspend to RAM", and so on).  There are also
 	 * dirty cases when swsusp fakes a suspend in "shutdown" mode.
 	 */
-	if (dev->current_state != PCI_D0) {
+	if (state != PCI_D0) {
 #ifdef	DEBUG
 		int	pci_pm;
 		u16	pmcr;
@@ -364,8 +356,7 @@ int usb_hcd_pci_resume(struct pci_dev *dev)
 			/* Clean case:  power to USB and to HC registers was
 			 * maintained; remote wakeup is easy.
 			 */
-			dev_dbg(hcd->self.controller, "resume from PCI D%d\n",
-					pmcr);
+			dev_dbg(&dev->dev, "resume from PCI D%d\n", pmcr);
 		} else {
 			/* Clean:  HC lost Vcc power, D0 uninitialized
 			 *   + Vaux may have preserved port and transceiver
@@ -376,32 +367,55 @@ int usb_hcd_pci_resume(struct pci_dev *dev)
 			 *   + after BIOS init
 			 *   + after Linux init (HCD statically linked)
 			 */
-			dev_dbg(hcd->self.controller,
-				"PCI D0, from previous PCI D%d\n",
-				dev->current_state);
+			dev_dbg(&dev->dev, "resume from previous PCI D%d\n",
+					state);
 		}
 #endif
-		/* yes, ignore these results too... */
-		(void) pci_enable_wake(dev, dev->current_state, 0);
-		(void) pci_enable_wake(dev, PCI_D3cold, 0);
+
+		retval = pci_set_power_state(dev, PCI_D0);
 	} else {
 		/* Same basic cases: clean (powered/not), dirty */
-		dev_dbg(hcd->self.controller, "PCI legacy resume\n");
+		dev_dbg(&dev->dev, "PCI legacy resume\n");
+	}
+
+	if (retval < 0)
+		dev_err(&dev->dev, "can't resume: %d\n", retval);
+	else
+		pci_restore_state(dev);
+
+	return retval;
+}
+EXPORT_SYMBOL_GPL(usb_hcd_pci_resume_early);
+
+/**
+ * usb_hcd_pci_resume - power management resume of a PCI-based HCD
+ * @dev: USB Host Controller being resumed
+ *
+ * Store this function in the HCD's struct pci_driver as .resume.
+ */
+int usb_hcd_pci_resume(struct pci_dev *dev)
+{
+	struct usb_hcd		*hcd;
+	int			retval;
+
+	hcd = pci_get_drvdata(dev);
+	if (hcd->state != HC_STATE_SUSPENDED) {
+		dev_dbg(hcd->self.controller,
+				"can't resume, not suspended!\n");
+		return 0;
 	}
 
-	/* NOTE:  the PCI API itself is asymmetric here.  We don't need to
-	 * pci_set_power_state(PCI_D0) since that's part of re-enabling;
-	 * but that won't re-enable bus mastering.  Yet pci_disable_device()
-	 * explicitly disables bus mastering...
-	 */
 	retval = pci_enable_device(dev);
 	if (retval < 0) {
-		dev_err(hcd->self.controller,
-			"can't re-enable after resume, %d!\n", retval);
+		dev_err(&dev->dev, "can't re-enable after resume, %d!\n",
+				retval);
 		return retval;
 	}
+
 	pci_set_master(dev);
-	pci_restore_state(dev);
+
+	/* yes, ignore this result too... */
+	(void) pci_wake_from_d3(dev, 0);
 
 	clear_bit(HCD_FLAG_SAW_IRQ, &hcd->flags);
 
@@ -413,7 +427,6 @@ int usb_hcd_pci_resume(struct pci_dev *dev)
 			usb_hc_died(hcd);
 		}
 	}
-
 	return retval;
 }
 EXPORT_SYMBOL_GPL(usb_hcd_pci_resume);
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index e1b42626d04d..3c711db55d86 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -1010,7 +1010,7 @@ int usb_hcd_link_urb_to_ep(struct usb_hcd *hcd, struct urb *urb)
 	spin_lock(&hcd_urb_list_lock);
 
 	/* Check that the URB isn't being killed */
-	if (unlikely(urb->reject)) {
+	if (unlikely(atomic_read(&urb->reject))) {
 		rc = -EPERM;
 		goto done;
 	}
@@ -1340,7 +1340,7 @@ int usb_hcd_submit_urb (struct urb *urb, gfp_t mem_flags)
 		INIT_LIST_HEAD(&urb->urb_list);
 		atomic_dec(&urb->use_count);
 		atomic_dec(&urb->dev->urbnum);
-		if (urb->reject)
+		if (atomic_read(&urb->reject))
 			wake_up(&usb_kill_urb_queue);
 		usb_put_urb(urb);
 	}
@@ -1444,7 +1444,7 @@ void usb_hcd_giveback_urb(struct usb_hcd *hcd, struct urb *urb, int status)
 	urb->status = status;
 	urb->complete (urb);
 	atomic_dec (&urb->use_count);
-	if (unlikely (urb->reject))
+	if (unlikely(atomic_read(&urb->reject)))
 		wake_up (&usb_kill_urb_queue);
 	usb_put_urb (urb);
 }
@@ -1573,14 +1573,14 @@ int usb_hcd_get_frame_number (struct usb_device *udev)
 
 #ifdef	CONFIG_PM
 
-int hcd_bus_suspend(struct usb_device *rhdev)
+int hcd_bus_suspend(struct usb_device *rhdev, pm_message_t msg)
 {
 	struct usb_hcd	*hcd = container_of(rhdev->bus, struct usb_hcd, self);
 	int		status;
 	int		old_state = hcd->state;
 
 	dev_dbg(&rhdev->dev, "bus %s%s\n",
-			rhdev->auto_pm ? "auto-" : "", "suspend");
+			(msg.event & PM_EVENT_AUTO ? "auto-" : ""), "suspend");
 	if (!hcd->driver->bus_suspend) {
 		status = -ENOENT;
 	} else {
@@ -1598,14 +1598,14 @@ int hcd_bus_suspend(struct usb_device *rhdev)
 	return status;
 }
 
-int hcd_bus_resume(struct usb_device *rhdev)
+int hcd_bus_resume(struct usb_device *rhdev, pm_message_t msg)
 {
 	struct usb_hcd	*hcd = container_of(rhdev->bus, struct usb_hcd, self);
 	int		status;
 	int		old_state = hcd->state;
 
 	dev_dbg(&rhdev->dev, "usb %s%s\n",
-			rhdev->auto_pm ? "auto-" : "", "resume");
+			(msg.event & PM_EVENT_AUTO ? "auto-" : ""), "resume");
 	if (!hcd->driver->bus_resume)
 		return -ENOENT;
 	if (hcd->state == HC_STATE_RUNNING)
@@ -1638,7 +1638,7 @@ static void hcd_resume_work(struct work_struct *work)
 
 	usb_lock_device(udev);
 	usb_mark_last_busy(udev);
-	usb_external_resume_device(udev);
+	usb_external_resume_device(udev, PMSG_REMOTE_RESUME);
 	usb_unlock_device(udev);
 }
 
@@ -2028,7 +2028,7 @@ EXPORT_SYMBOL_GPL(usb_hcd_platform_shutdown);
 
 /*-------------------------------------------------------------------------*/
 
-#if defined(CONFIG_USB_MON)
+#if defined(CONFIG_USB_MON) || defined(CONFIG_USB_MON_MODULE)
 
 struct usb_mon_operations *mon_ops;
 
@@ -2064,4 +2064,4 @@ void usb_mon_deregister (void)
 }
 EXPORT_SYMBOL_GPL (usb_mon_deregister);
 
-#endif /* CONFIG_USB_MON */
+#endif /* CONFIG_USB_MON || CONFIG_USB_MON_MODULE */
diff --git a/drivers/usb/core/hcd.h b/drivers/usb/core/hcd.h
index 9465e70f4dd0..572d2cf46e8d 100644
--- a/drivers/usb/core/hcd.h
+++ b/drivers/usb/core/hcd.h
@@ -16,6 +16,8 @@
  * Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#ifndef __USB_CORE_HCD_H
+#define __USB_CORE_HCD_H
 
 #ifdef __KERNEL__
 
@@ -254,7 +256,9 @@ extern int usb_hcd_pci_probe(struct pci_dev *dev,
 extern void usb_hcd_pci_remove(struct pci_dev *dev);
 
 #ifdef CONFIG_PM
-extern int usb_hcd_pci_suspend(struct pci_dev *dev, pm_message_t state);
+extern int usb_hcd_pci_suspend(struct pci_dev *dev, pm_message_t msg);
+extern int usb_hcd_pci_suspend_late(struct pci_dev *dev, pm_message_t msg);
+extern int usb_hcd_pci_resume_early(struct pci_dev *dev);
 extern int usb_hcd_pci_resume(struct pci_dev *dev);
 #endif /* CONFIG_PM */
 
@@ -386,8 +390,8 @@ extern int usb_find_interface_driver(struct usb_device *dev,
 #ifdef CONFIG_PM
 extern void usb_hcd_resume_root_hub(struct usb_hcd *hcd);
 extern void usb_root_hub_lost_power(struct usb_device *rhdev);
-extern int hcd_bus_suspend(struct usb_device *rhdev);
-extern int hcd_bus_resume(struct usb_device *rhdev);
+extern int hcd_bus_suspend(struct usb_device *rhdev, pm_message_t msg);
+extern int hcd_bus_resume(struct usb_device *rhdev, pm_message_t msg);
 #else
 static inline void usb_hcd_resume_root_hub(struct usb_hcd *hcd)
 {
@@ -419,7 +423,7 @@ static inline void usbfs_cleanup(void) { }
 
 /*-------------------------------------------------------------------------*/
 
-#if defined(CONFIG_USB_MON)
+#if defined(CONFIG_USB_MON) || defined(CONFIG_USB_MON_MODULE)
 
 struct usb_mon_operations {
 	void (*urb_submit)(struct usb_bus *bus, struct urb *urb);
@@ -461,7 +465,7 @@ static inline void usbmon_urb_submit_error(struct usb_bus *bus, struct urb *urb,
 static inline void usbmon_urb_complete(struct usb_bus *bus, struct urb *urb,
 		int status) {}
 
-#endif /* CONFIG_USB_MON */
+#endif /* CONFIG_USB_MON || CONFIG_USB_MON_MODULE */
 
 /*-------------------------------------------------------------------------*/
 
@@ -490,3 +494,5 @@ extern struct rw_semaphore ehci_cf_port_reset_rwsem;
 extern unsigned long usb_hcds_loaded;
 
 #endif /* __KERNEL__ */
+
+#endif /* __USB_CORE_HCD_H */
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index b19cbfcd51da..d5d0e40b1e2d 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -107,7 +107,9 @@ MODULE_PARM_DESC (blinkenlights, "true to cycle leds on hubs");
 /* define initial 64-byte descriptor request timeout in milliseconds */
 static int initial_descriptor_timeout = USB_CTRL_GET_TIMEOUT;
 module_param(initial_descriptor_timeout, int, S_IRUGO|S_IWUSR);
-MODULE_PARM_DESC(initial_descriptor_timeout, "initial 64-byte descriptor request timeout in milliseconds (default 5000 - 5.0 seconds)");
+MODULE_PARM_DESC(initial_descriptor_timeout,
+		"initial 64-byte descriptor request timeout in milliseconds "
+		"(default 5000 - 5.0 seconds)");
 
 /*
  * As of 2.6.10 we introduce a new USB device initialization scheme which
@@ -1136,8 +1138,8 @@ static int hub_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	hdev = interface_to_usbdev(intf);
 
 	if (hdev->level == MAX_TOPO_LEVEL) {
-		dev_err(&intf->dev, "Unsupported bus topology: "
-				"hub nested too deep\n");
+		dev_err(&intf->dev,
+			"Unsupported bus topology: hub nested too deep\n");
 		return -E2BIG;
 	}
 
@@ -1374,8 +1376,9 @@ static void usb_stop_pm(struct usb_device *udev)
 		usb_autosuspend_device(udev->parent);
 	usb_pm_unlock(udev);
 
-	/* Stop any autosuspend requests already submitted */
-	cancel_rearming_delayed_work(&udev->autosuspend);
+	/* Stop any autosuspend or autoresume requests already submitted */
+	cancel_delayed_work_sync(&udev->autosuspend);
+	cancel_work_sync(&udev->autoresume);
 }
 
 #else
@@ -1434,17 +1437,12 @@ void usb_disconnect(struct usb_device **pdev)
 	usb_disable_device(udev, 0);
 	usb_hcd_synchronize_unlinks(udev);
 
+	usb_remove_ep_devs(&udev->ep0);
 	usb_unlock_device(udev);
 
-	/* Remove the device-specific files from sysfs.  This must be
-	 * done with udev unlocked, because some of the attribute
-	 * routines try to acquire the device lock.
-	 */
-	usb_remove_sysfs_dev_files(udev);
-
 	/* Unregister the device.  The device driver is responsible
-	 * for removing the device files from usbfs and sysfs and for
-	 * de-configuring the device.
+	 * for de-configuring the device and invoking the remove-device
+	 * notifier chain (used by usbfs and possibly others).
 	 */
 	device_del(&udev->dev);
 
@@ -1476,8 +1474,8 @@ static void announce_device(struct usb_device *udev)
 	dev_info(&udev->dev, "New USB device found, idVendor=%04x, idProduct=%04x\n",
 		le16_to_cpu(udev->descriptor.idVendor),
 		le16_to_cpu(udev->descriptor.idProduct));
-	dev_info(&udev->dev, "New USB device strings: Mfr=%d, Product=%d, "
-		"SerialNumber=%d\n",
+	dev_info(&udev->dev,
+		"New USB device strings: Mfr=%d, Product=%d, SerialNumber=%d\n",
 		udev->descriptor.iManufacturer,
 		udev->descriptor.iProduct,
 		udev->descriptor.iSerialNumber);
@@ -1542,7 +1540,7 @@ static int usb_configure_device_otg(struct usb_device *udev)
 					 * customize to match your product.
 					 */
 					dev_info(&udev->dev,
-						"can't set HNP mode; %d\n",
+						"can't set HNP mode: %d\n",
 						err);
 					bus->b_hnp_enable = 0;
 				}
@@ -1635,6 +1633,10 @@ int usb_new_device(struct usb_device *udev)
 {
 	int err;
 
+	/* Increment the parent's count of unsuspended children */
+	if (udev->parent)
+		usb_autoresume_device(udev->parent);
+
 	usb_detect_quirks(udev);		/* Determine quirks */
 	err = usb_configure_device(udev);	/* detect & probe dev/intfs */
 	if (err < 0)
@@ -1643,13 +1645,12 @@ int usb_new_device(struct usb_device *udev)
 	udev->dev.devt = MKDEV(USB_DEVICE_MAJOR,
 			(((udev->bus->busnum-1) * 128) + (udev->devnum-1)));
 
-	/* Increment the parent's count of unsuspended children */
-	if (udev->parent)
-		usb_autoresume_device(udev->parent);
+	/* Tell the world! */
+	announce_device(udev);
 
 	/* Register the device.  The device driver is responsible
-	 * for adding the device files to sysfs and for configuring
-	 * the device.
+	 * for configuring the device and invoking the add-device
+	 * notifier chain (used by usbfs and possibly others).
 	 */
 	err = device_add(&udev->dev);
 	if (err) {
@@ -1657,15 +1658,12 @@ int usb_new_device(struct usb_device *udev)
 		goto fail;
 	}
 
-	/* put device-specific files into sysfs */
-	usb_create_sysfs_dev_files(udev);
-
-	/* Tell the world! */
-	announce_device(udev);
+	(void) usb_create_ep_devs(&udev->dev, &udev->ep0, udev);
 	return err;
 
 fail:
 	usb_set_device_state(udev, USB_STATE_NOTATTACHED);
+	usb_stop_pm(udev);
 	return err;
 }
 
@@ -1982,7 +1980,7 @@ static int check_port_resume_type(struct usb_device *udev,
  *
  * Returns 0 on success, else negative errno.
  */
-int usb_port_suspend(struct usb_device *udev)
+int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 {
 	struct usb_hub	*hub = hdev_to_hub(udev->parent);
 	int		port1 = udev->portnum;
@@ -2021,7 +2019,7 @@ int usb_port_suspend(struct usb_device *udev)
 	} else {
 		/* device has up to 10 msec to fully suspend */
 		dev_dbg(&udev->dev, "usb %ssuspend\n",
-				udev->auto_pm ? "auto-" : "");
+				(msg.event & PM_EVENT_AUTO ? "auto-" : ""));
 		usb_set_device_state(udev, USB_STATE_SUSPENDED);
 		msleep(10);
 	}
@@ -2045,8 +2043,8 @@ static int finish_port_resume(struct usb_device *udev)
 	u16	devstatus;
 
 	/* caller owns the udev device lock */
-	dev_dbg(&udev->dev, "finish %sresume\n",
-			udev->reset_resume ? "reset-" : "");
+	dev_dbg(&udev->dev, "%s\n",
+		udev->reset_resume ? "finish reset-resume" : "finish resume");
 
 	/* usb ch9 identifies four variants of SUSPENDED, based on what
 	 * state the device resumes to.  Linux currently won't see the
@@ -2098,8 +2096,9 @@ static int finish_port_resume(struct usb_device *udev)
 					NULL, 0,
 					USB_CTRL_SET_TIMEOUT);
 			if (status)
-				dev_dbg(&udev->dev, "disable remote "
-					"wakeup, status %d\n", status);
+				dev_dbg(&udev->dev,
+					"disable remote wakeup, status %d\n",
+					status);
 		}
 		status = 0;
 	}
@@ -2140,7 +2139,7 @@ static int finish_port_resume(struct usb_device *udev)
  *
  * Returns 0 on success, else negative errno.
  */
-int usb_port_resume(struct usb_device *udev)
+int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 {
 	struct usb_hub	*hub = hdev_to_hub(udev->parent);
 	int		port1 = udev->portnum;
@@ -2165,7 +2164,7 @@ int usb_port_resume(struct usb_device *udev)
 	} else {
 		/* drive resume for at least 20 msec */
 		dev_dbg(&udev->dev, "usb %sresume\n",
-				udev->auto_pm ? "auto-" : "");
+				(msg.event & PM_EVENT_AUTO ? "auto-" : ""));
 		msleep(25);
 
 		/* Virtual root hubs can trigger on GET_PORT_STATUS to
@@ -2206,7 +2205,7 @@ static int remote_wakeup(struct usb_device *udev)
 	if (udev->state == USB_STATE_SUSPENDED) {
 		dev_dbg(&udev->dev, "usb %sresume\n", "wakeup-");
 		usb_mark_last_busy(udev);
-		status = usb_external_resume_device(udev);
+		status = usb_external_resume_device(udev, PMSG_REMOTE_RESUME);
 	}
 	return status;
 }
@@ -2215,14 +2214,14 @@ static int remote_wakeup(struct usb_device *udev)
 
 /* When CONFIG_USB_SUSPEND isn't set, we never suspend or resume any ports. */
 
-int usb_port_suspend(struct usb_device *udev)
+int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 {
 	return 0;
 }
 
 /* However we may need to do a reset-resume */
 
-int usb_port_resume(struct usb_device *udev)
+int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 {
 	struct usb_hub	*hub = hdev_to_hub(udev->parent);
 	int		port1 = udev->portnum;
@@ -2262,7 +2261,7 @@ static int hub_suspend(struct usb_interface *intf, pm_message_t msg)
 
 		udev = hdev->children [port1-1];
 		if (udev && udev->can_submit) {
-			if (!hdev->auto_pm)
+			if (!(msg.event & PM_EVENT_AUTO))
 				dev_dbg(&intf->dev, "port %d nyet suspended\n",
 						port1);
 			return -EBUSY;
@@ -2385,7 +2384,7 @@ void usb_ep0_reinit(struct usb_device *udev)
 {
 	usb_disable_endpoint(udev, 0 + USB_DIR_IN);
 	usb_disable_endpoint(udev, 0 + USB_DIR_OUT);
-	usb_enable_endpoint(udev, &udev->ep0);
+	usb_enable_endpoint(udev, &udev->ep0, true);
 }
 EXPORT_SYMBOL_GPL(usb_ep0_reinit);
 
@@ -2582,9 +2581,9 @@ hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
 				goto fail;
 			}
 			if (r) {
-				dev_err(&udev->dev, "device descriptor "
-						"read/%s, error %d\n",
-						"64", r);
+				dev_err(&udev->dev,
+					"device descriptor read/64, error %d\n",
+					r);
 				retval = -EMSGSIZE;
 				continue;
 			}
@@ -2621,9 +2620,9 @@ hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
 
 		retval = usb_get_device_descriptor(udev, 8);
 		if (retval < 8) {
-			dev_err(&udev->dev, "device descriptor "
-					"read/%s, error %d\n",
-					"8", retval);
+			dev_err(&udev->dev,
+					"device descriptor read/8, error %d\n",
+					retval);
 			if (retval >= 0)
 				retval = -EMSGSIZE;
 		} else {
@@ -2650,8 +2649,8 @@ hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
   
 	retval = usb_get_device_descriptor(udev, USB_DT_DEVICE_SIZE);
 	if (retval < (signed)sizeof(udev->descriptor)) {
-		dev_err(&udev->dev, "device descriptor read/%s, error %d\n",
-			"all", retval);
+		dev_err(&udev->dev, "device descriptor read/all, error %d\n",
+			retval);
 		if (retval >= 0)
 			retval = -ENOMSG;
 		goto fail;
@@ -2719,9 +2718,9 @@ 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(&udev->dev,
+				 "%dmA is over %umA budget for port %d!\n",
+				 delta, hub->mA_per_port, port1);
 		remaining -= delta;
 	}
 	if (remaining < 0) {
@@ -3517,3 +3516,46 @@ int usb_reset_device(struct usb_device *udev)
 	return ret;
 }
 EXPORT_SYMBOL_GPL(usb_reset_device);
+
+
+/**
+ * usb_queue_reset_device - Reset a USB device from an atomic context
+ * @iface: USB interface belonging to the device to reset
+ *
+ * This function can be used to reset a USB device from an atomic
+ * context, where usb_reset_device() won't work (as it blocks).
+ *
+ * Doing a reset via this method is functionally equivalent to calling
+ * usb_reset_device(), except for the fact that it is delayed to a
+ * workqueue. This means that any drivers bound to other interfaces
+ * might be unbound, as well as users from usbfs in user space.
+ *
+ * Corner cases:
+ *
+ * - Scheduling two resets at the same time from two different drivers
+ *   attached to two different interfaces of the same device is
+ *   possible; depending on how the driver attached to each interface
+ *   handles ->pre_reset(), the second reset might happen or not.
+ *
+ * - If a driver is unbound and it had a pending reset, the reset will
+ *   be cancelled.
+ *
+ * - This function can be called during .probe() or .disconnect()
+ *   times. On return from .disconnect(), any pending resets will be
+ *   cancelled.
+ *
+ * There is no no need to lock/unlock the @reset_ws as schedule_work()
+ * does its own.
+ *
+ * NOTE: We don't do any reference count tracking because it is not
+ *     needed. The lifecycle of the work_struct is tied to the
+ *     usb_interface. Before destroying the interface we cancel the
+ *     work_struct, so the fact that work_struct is queued and or
+ *     running means the interface (and thus, the device) exist and
+ *     are referenced.
+ */
+void usb_queue_reset_device(struct usb_interface *iface)
+{
+	schedule_work(&iface->reset_ws);
+}
+EXPORT_SYMBOL_GPL(usb_queue_reset_device);
diff --git a/drivers/usb/core/message.c b/drivers/usb/core/message.c
index 6d1048faf08e..de51667dd64d 100644
--- a/drivers/usb/core/message.c
+++ b/drivers/usb/core/message.c
@@ -18,6 +18,8 @@
 #include "hcd.h"	/* for usbcore internals */
 #include "usb.h"
 
+static void cancel_async_set_config(struct usb_device *udev);
+
 struct api_context {
 	struct completion	done;
 	int			status;
@@ -139,9 +141,9 @@ int usb_control_msg(struct usb_device *dev, unsigned int pipe, __u8 request,
 
 	dr->bRequestType = requesttype;
 	dr->bRequest = request;
-	dr->wValue = cpu_to_le16p(&value);
-	dr->wIndex = cpu_to_le16p(&index);
-	dr->wLength = cpu_to_le16p(&size);
+	dr->wValue = cpu_to_le16(value);
+	dr->wIndex = cpu_to_le16(index);
+	dr->wLength = cpu_to_le16(size);
 
 	/* dbg("usb_control_msg"); */
 
@@ -1004,6 +1006,34 @@ int usb_clear_halt(struct usb_device *dev, int pipe)
 }
 EXPORT_SYMBOL_GPL(usb_clear_halt);
 
+static int create_intf_ep_devs(struct usb_interface *intf)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct usb_host_interface *alt = intf->cur_altsetting;
+	int i;
+
+	if (intf->ep_devs_created || intf->unregistering)
+		return 0;
+
+	for (i = 0; i < alt->desc.bNumEndpoints; ++i)
+		(void) usb_create_ep_devs(&intf->dev, &alt->endpoint[i], udev);
+	intf->ep_devs_created = 1;
+	return 0;
+}
+
+static void remove_intf_ep_devs(struct usb_interface *intf)
+{
+	struct usb_host_interface *alt = intf->cur_altsetting;
+	int i;
+
+	if (!intf->ep_devs_created)
+		return;
+
+	for (i = 0; i < alt->desc.bNumEndpoints; ++i)
+		usb_remove_ep_devs(&alt->endpoint[i]);
+	intf->ep_devs_created = 0;
+}
+
 /**
  * usb_disable_endpoint -- Disable an endpoint by address
  * @dev: the device whose endpoint is being disabled
@@ -1092,7 +1122,7 @@ void usb_disable_device(struct usb_device *dev, int skip_ep0)
 			dev_dbg(&dev->dev, "unregistering interface %s\n",
 				dev_name(&interface->dev));
 			interface->unregistering = 1;
-			usb_remove_sysfs_intf_files(interface);
+			remove_intf_ep_devs(interface);
 			device_del(&interface->dev);
 		}
 
@@ -1113,22 +1143,26 @@ void usb_disable_device(struct usb_device *dev, int skip_ep0)
  * usb_enable_endpoint - Enable an endpoint for USB communications
  * @dev: the device whose interface is being enabled
  * @ep: the endpoint
+ * @reset_toggle: flag to set the endpoint's toggle back to 0
  *
- * Resets the endpoint toggle, and sets dev->ep_{in,out} pointers.
+ * Resets the endpoint toggle if asked, and sets dev->ep_{in,out} pointers.
  * For control endpoints, both the input and output sides are handled.
  */
-void usb_enable_endpoint(struct usb_device *dev, struct usb_host_endpoint *ep)
+void usb_enable_endpoint(struct usb_device *dev, struct usb_host_endpoint *ep,
+		bool reset_toggle)
 {
 	int epnum = usb_endpoint_num(&ep->desc);
 	int is_out = usb_endpoint_dir_out(&ep->desc);
 	int is_control = usb_endpoint_xfer_control(&ep->desc);
 
 	if (is_out || is_control) {
-		usb_settoggle(dev, epnum, 1, 0);
+		if (reset_toggle)
+			usb_settoggle(dev, epnum, 1, 0);
 		dev->ep_out[epnum] = ep;
 	}
 	if (!is_out || is_control) {
-		usb_settoggle(dev, epnum, 0, 0);
+		if (reset_toggle)
+			usb_settoggle(dev, epnum, 0, 0);
 		dev->ep_in[epnum] = ep;
 	}
 	ep->enabled = 1;
@@ -1138,17 +1172,18 @@ void usb_enable_endpoint(struct usb_device *dev, struct usb_host_endpoint *ep)
  * usb_enable_interface - Enable all the endpoints for an interface
  * @dev: the device whose interface is being enabled
  * @intf: pointer to the interface descriptor
+ * @reset_toggles: flag to set the endpoints' toggles back to 0
  *
  * Enables all the endpoints for the interface's current altsetting.
  */
-static void usb_enable_interface(struct usb_device *dev,
-				 struct usb_interface *intf)
+void usb_enable_interface(struct usb_device *dev,
+		struct usb_interface *intf, bool reset_toggles)
 {
 	struct usb_host_interface *alt = intf->cur_altsetting;
 	int i;
 
 	for (i = 0; i < alt->desc.bNumEndpoints; ++i)
-		usb_enable_endpoint(dev, &alt->endpoint[i]);
+		usb_enable_endpoint(dev, &alt->endpoint[i], reset_toggles);
 }
 
 /**
@@ -1235,8 +1270,10 @@ int usb_set_interface(struct usb_device *dev, int interface, int alternate)
 	 */
 
 	/* prevent submissions using previous endpoint settings */
-	if (iface->cur_altsetting != alt)
+	if (iface->cur_altsetting != alt) {
+		remove_intf_ep_devs(iface);
 		usb_remove_sysfs_intf_files(iface);
+	}
 	usb_disable_interface(dev, iface);
 
 	iface->cur_altsetting = alt;
@@ -1271,10 +1308,11 @@ int usb_set_interface(struct usb_device *dev, int interface, int alternate)
 	 * during the SETUP stage - hence EP0 toggles are "don't care" here.
 	 * (Likewise, EP0 never "halts" on well designed devices.)
 	 */
-	usb_enable_interface(dev, iface);
-	if (device_is_registered(&iface->dev))
+	usb_enable_interface(dev, iface, true);
+	if (device_is_registered(&iface->dev)) {
 		usb_create_sysfs_intf_files(iface);
-
+		create_intf_ep_devs(iface);
+	}
 	return 0;
 }
 EXPORT_SYMBOL_GPL(usb_set_interface);
@@ -1334,7 +1372,6 @@ int usb_reset_configuration(struct usb_device *dev)
 		struct usb_interface *intf = config->interface[i];
 		struct usb_host_interface *alt;
 
-		usb_remove_sysfs_intf_files(intf);
 		alt = usb_altnum_to_altsetting(intf, 0);
 
 		/* No altsetting 0?  We'll assume the first altsetting.
@@ -1345,10 +1382,16 @@ int usb_reset_configuration(struct usb_device *dev)
 		if (!alt)
 			alt = &intf->altsetting[0];
 
+		if (alt != intf->cur_altsetting) {
+			remove_intf_ep_devs(intf);
+			usb_remove_sysfs_intf_files(intf);
+		}
 		intf->cur_altsetting = alt;
-		usb_enable_interface(dev, intf);
-		if (device_is_registered(&intf->dev))
+		usb_enable_interface(dev, intf, true);
+		if (device_is_registered(&intf->dev)) {
 			usb_create_sysfs_intf_files(intf);
+			create_intf_ep_devs(intf);
+		}
 	}
 	return 0;
 }
@@ -1441,6 +1484,46 @@ static struct usb_interface_assoc_descriptor *find_iad(struct usb_device *dev,
 	return retval;
 }
 
+
+/*
+ * Internal function to queue a device reset
+ *
+ * This is initialized into the workstruct in 'struct
+ * usb_device->reset_ws' that is launched by
+ * message.c:usb_set_configuration() when initializing each 'struct
+ * usb_interface'.
+ *
+ * It is safe to get the USB device without reference counts because
+ * the life cycle of @iface is bound to the life cycle of @udev. Then,
+ * this function will be ran only if @iface is alive (and before
+ * freeing it any scheduled instances of it will have been cancelled).
+ *
+ * We need to set a flag (usb_dev->reset_running) because when we call
+ * the reset, the interfaces might be unbound. The current interface
+ * cannot try to remove the queued work as it would cause a deadlock
+ * (you cannot remove your work from within your executing
+ * workqueue). This flag lets it know, so that
+ * usb_cancel_queued_reset() doesn't try to do it.
+ *
+ * See usb_queue_reset_device() for more details
+ */
+void __usb_queue_reset_device(struct work_struct *ws)
+{
+	int rc;
+	struct usb_interface *iface =
+		container_of(ws, struct usb_interface, reset_ws);
+	struct usb_device *udev = interface_to_usbdev(iface);
+
+	rc = usb_lock_device_for_reset(udev, iface);
+	if (rc >= 0) {
+		iface->reset_running = 1;
+		usb_reset_device(udev);
+		iface->reset_running = 0;
+		usb_unlock_device(udev);
+	}
+}
+
+
 /*
  * usb_set_configuration - Makes a particular device setting be current
  * @dev: the device whose configuration is being updated
@@ -1560,6 +1643,9 @@ free_interfaces:
 	if (dev->state != USB_STATE_ADDRESS)
 		usb_disable_device(dev, 1);	/* Skip ep0 */
 
+	/* Get rid of pending async Set-Config requests for this device */
+	cancel_async_set_config(dev);
+
 	ret = usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
 			      USB_REQ_SET_CONFIGURATION, 0, configuration, 0,
 			      NULL, 0, USB_CTRL_SET_TIMEOUT);
@@ -1604,13 +1690,14 @@ free_interfaces:
 			alt = &intf->altsetting[0];
 
 		intf->cur_altsetting = alt;
-		usb_enable_interface(dev, intf);
+		usb_enable_interface(dev, intf, true);
 		intf->dev.parent = &dev->dev;
 		intf->dev.driver = NULL;
 		intf->dev.bus = &usb_bus_type;
 		intf->dev.type = &usb_if_device_type;
 		intf->dev.groups = usb_interface_groups;
 		intf->dev.dma_mask = dev->dev.dma_mask;
+		INIT_WORK(&intf->reset_ws, __usb_queue_reset_device);
 		device_initialize(&intf->dev);
 		mark_quiesced(intf);
 		dev_set_name(&intf->dev, "%d-%s:%d.%d",
@@ -1641,17 +1728,21 @@ free_interfaces:
 				dev_name(&intf->dev), ret);
 			continue;
 		}
-		usb_create_sysfs_intf_files(intf);
+		create_intf_ep_devs(intf);
 	}
 
 	usb_autosuspend_device(dev);
 	return 0;
 }
 
+static LIST_HEAD(set_config_list);
+static DEFINE_SPINLOCK(set_config_lock);
+
 struct set_config_request {
 	struct usb_device	*udev;
 	int			config;
 	struct work_struct	work;
+	struct list_head	node;
 };
 
 /* Worker routine for usb_driver_set_configuration() */
@@ -1659,14 +1750,35 @@ static void driver_set_config_work(struct work_struct *work)
 {
 	struct set_config_request *req =
 		container_of(work, struct set_config_request, work);
+	struct usb_device *udev = req->udev;
+
+	usb_lock_device(udev);
+	spin_lock(&set_config_lock);
+	list_del(&req->node);
+	spin_unlock(&set_config_lock);
 
-	usb_lock_device(req->udev);
-	usb_set_configuration(req->udev, req->config);
-	usb_unlock_device(req->udev);
-	usb_put_dev(req->udev);
+	if (req->config >= -1)		/* Is req still valid? */
+		usb_set_configuration(udev, req->config);
+	usb_unlock_device(udev);
+	usb_put_dev(udev);
 	kfree(req);
 }
 
+/* Cancel pending Set-Config requests for a device whose configuration
+ * was just changed
+ */
+static void cancel_async_set_config(struct usb_device *udev)
+{
+	struct set_config_request *req;
+
+	spin_lock(&set_config_lock);
+	list_for_each_entry(req, &set_config_list, node) {
+		if (req->udev == udev)
+			req->config = -999;	/* Mark as cancelled */
+	}
+	spin_unlock(&set_config_lock);
+}
+
 /**
  * usb_driver_set_configuration - Provide a way for drivers to change device configurations
  * @udev: the device whose configuration is being updated
@@ -1698,6 +1810,10 @@ int usb_driver_set_configuration(struct usb_device *udev, int config)
 	req->config = config;
 	INIT_WORK(&req->work, driver_set_config_work);
 
+	spin_lock(&set_config_lock);
+	list_add(&req->node, &set_config_list);
+	spin_unlock(&set_config_lock);
+
 	usb_get_dev(udev);
 	schedule_work(&req->work);
 	return 0;
diff --git a/drivers/usb/core/sysfs.c b/drivers/usb/core/sysfs.c
index 4fb65fdc9dc3..4cc2456ef3be 100644
--- a/drivers/usb/core/sysfs.c
+++ b/drivers/usb/core/sysfs.c
@@ -359,19 +359,19 @@ set_level(struct device *dev, struct device_attribute *attr,
 			strncmp(buf, on_string, len) == 0) {
 		udev->autosuspend_disabled = 1;
 		udev->autoresume_disabled = 0;
-		rc = usb_external_resume_device(udev);
+		rc = usb_external_resume_device(udev, PMSG_USER_RESUME);
 
 	} else if (len == sizeof auto_string - 1 &&
 			strncmp(buf, auto_string, len) == 0) {
 		udev->autosuspend_disabled = 0;
 		udev->autoresume_disabled = 0;
-		rc = usb_external_resume_device(udev);
+		rc = usb_external_resume_device(udev, PMSG_USER_RESUME);
 
 	} else if (len == sizeof suspend_string - 1 &&
 			strncmp(buf, suspend_string, len) == 0) {
 		udev->autosuspend_disabled = 0;
 		udev->autoresume_disabled = 1;
-		rc = usb_external_suspend_device(udev, PMSG_SUSPEND);
+		rc = usb_external_suspend_device(udev, PMSG_USER_SUSPEND);
 
 	} else
 		rc = -EINVAL;
@@ -629,9 +629,6 @@ int usb_create_sysfs_dev_files(struct usb_device *udev)
 	struct device *dev = &udev->dev;
 	int retval;
 
-	/* Unforunately these attributes cannot be created before
-	 * the uevent is broadcast.
-	 */
 	retval = device_create_bin_file(dev, &dev_bin_attr_descriptors);
 	if (retval)
 		goto error;
@@ -643,11 +640,7 @@ int usb_create_sysfs_dev_files(struct usb_device *udev)
 	retval = add_power_attributes(dev);
 	if (retval)
 		goto error;
-
-	retval = usb_create_ep_files(dev, &udev->ep0, udev);
-	if (retval)
-		goto error;
-	return 0;
+	return retval;
 error:
 	usb_remove_sysfs_dev_files(udev);
 	return retval;
@@ -657,7 +650,6 @@ void usb_remove_sysfs_dev_files(struct usb_device *udev)
 {
 	struct device *dev = &udev->dev;
 
-	usb_remove_ep_files(&udev->ep0);
 	remove_power_attributes(dev);
 	remove_persist_attributes(dev);
 	device_remove_bin_file(dev, &dev_bin_attr_descriptors);
@@ -812,28 +804,6 @@ struct attribute_group *usb_interface_groups[] = {
 	NULL
 };
 
-static inline void usb_create_intf_ep_files(struct usb_interface *intf,
-		struct usb_device *udev)
-{
-	struct usb_host_interface *iface_desc;
-	int i;
-
-	iface_desc = intf->cur_altsetting;
-	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i)
-		usb_create_ep_files(&intf->dev, &iface_desc->endpoint[i],
-				udev);
-}
-
-static inline void usb_remove_intf_ep_files(struct usb_interface *intf)
-{
-	struct usb_host_interface *iface_desc;
-	int i;
-
-	iface_desc = intf->cur_altsetting;
-	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i)
-		usb_remove_ep_files(&iface_desc->endpoint[i]);
-}
-
 int usb_create_sysfs_intf_files(struct usb_interface *intf)
 {
 	struct usb_device *udev = interface_to_usbdev(intf);
@@ -843,26 +813,19 @@ int usb_create_sysfs_intf_files(struct usb_interface *intf)
 	if (intf->sysfs_files_created || intf->unregistering)
 		return 0;
 
-	/* The interface string may be present in some altsettings
-	 * and missing in others.  Hence its attribute cannot be created
-	 * before the uevent is broadcast.
-	 */
 	if (alt->string == NULL)
 		alt->string = usb_cache_string(udev, alt->desc.iInterface);
 	if (alt->string)
 		retval = device_create_file(&intf->dev, &dev_attr_interface);
-	usb_create_intf_ep_files(intf, udev);
 	intf->sysfs_files_created = 1;
 	return 0;
 }
 
 void usb_remove_sysfs_intf_files(struct usb_interface *intf)
 {
-	struct device *dev = &intf->dev;
-
 	if (!intf->sysfs_files_created)
 		return;
-	usb_remove_intf_ep_files(intf);
-	device_remove_file(dev, &dev_attr_interface);
+
+	device_remove_file(&intf->dev, &dev_attr_interface);
 	intf->sysfs_files_created = 0;
 }
diff --git a/drivers/usb/core/urb.c b/drivers/usb/core/urb.c
index 1f68af9db3f7..58bc5e3c2560 100644
--- a/drivers/usb/core/urb.c
+++ b/drivers/usb/core/urb.c
@@ -10,7 +10,6 @@
 
 #define to_urb(d) container_of(d, struct urb, kref)
 
-static DEFINE_SPINLOCK(usb_reject_lock);
 
 static void urb_destroy(struct kref *kref)
 {
@@ -131,9 +130,7 @@ void usb_anchor_urb(struct urb *urb, struct usb_anchor *anchor)
 	urb->anchor = anchor;
 
 	if (unlikely(anchor->poisoned)) {
-		spin_lock(&usb_reject_lock);
-		urb->reject++;
-		spin_unlock(&usb_reject_lock);
+		atomic_inc(&urb->reject);
 	}
 
 	spin_unlock_irqrestore(&anchor->lock, flags);
@@ -565,16 +562,12 @@ void usb_kill_urb(struct urb *urb)
 	might_sleep();
 	if (!(urb && urb->dev && urb->ep))
 		return;
-	spin_lock_irq(&usb_reject_lock);
-	++urb->reject;
-	spin_unlock_irq(&usb_reject_lock);
+	atomic_inc(&urb->reject);
 
 	usb_hcd_unlink_urb(urb, -ENOENT);
 	wait_event(usb_kill_urb_queue, atomic_read(&urb->use_count) == 0);
 
-	spin_lock_irq(&usb_reject_lock);
-	--urb->reject;
-	spin_unlock_irq(&usb_reject_lock);
+	atomic_dec(&urb->reject);
 }
 EXPORT_SYMBOL_GPL(usb_kill_urb);
 
@@ -606,9 +599,7 @@ void usb_poison_urb(struct urb *urb)
 	might_sleep();
 	if (!(urb && urb->dev && urb->ep))
 		return;
-	spin_lock_irq(&usb_reject_lock);
-	++urb->reject;
-	spin_unlock_irq(&usb_reject_lock);
+	atomic_inc(&urb->reject);
 
 	usb_hcd_unlink_urb(urb, -ENOENT);
 	wait_event(usb_kill_urb_queue, atomic_read(&urb->use_count) == 0);
@@ -617,14 +608,10 @@ EXPORT_SYMBOL_GPL(usb_poison_urb);
 
 void usb_unpoison_urb(struct urb *urb)
 {
-	unsigned long flags;
-
 	if (!urb)
 		return;
 
-	spin_lock_irqsave(&usb_reject_lock, flags);
-	--urb->reject;
-	spin_unlock_irqrestore(&usb_reject_lock, flags);
+	atomic_dec(&urb->reject);
 }
 EXPORT_SYMBOL_GPL(usb_unpoison_urb);
 
@@ -692,6 +679,26 @@ void usb_poison_anchored_urbs(struct usb_anchor *anchor)
 EXPORT_SYMBOL_GPL(usb_poison_anchored_urbs);
 
 /**
+ * usb_unpoison_anchored_urbs - let an anchor be used successfully again
+ * @anchor: anchor the requests are bound to
+ *
+ * Reverses the effect of usb_poison_anchored_urbs
+ * the anchor can be used normally after it returns
+ */
+void usb_unpoison_anchored_urbs(struct usb_anchor *anchor)
+{
+	unsigned long flags;
+	struct urb *lazarus;
+
+	spin_lock_irqsave(&anchor->lock, flags);
+	list_for_each_entry(lazarus, &anchor->urb_list, anchor_list) {
+		usb_unpoison_urb(lazarus);
+	}
+	anchor->poisoned = 0;
+	spin_unlock_irqrestore(&anchor->lock, flags);
+}
+EXPORT_SYMBOL_GPL(usb_unpoison_anchored_urbs);
+/**
  * usb_unlink_anchored_urbs - asynchronously cancel transfer requests en masse
  * @anchor: anchor the requests are bound to
  *
diff --git a/drivers/usb/core/usb.c b/drivers/usb/core/usb.c
index be1fa0723f2c..dcfc072630c1 100644
--- a/drivers/usb/core/usb.c
+++ b/drivers/usb/core/usb.c
@@ -253,7 +253,7 @@ static int usb_dev_prepare(struct device *dev)
 static void usb_dev_complete(struct device *dev)
 {
 	/* Currently used only for rebinding interfaces */
-	usb_resume(dev);	/* Implement eventually? */
+	usb_resume(dev, PMSG_RESUME);	/* Message event is meaningless */
 }
 
 static int usb_dev_suspend(struct device *dev)
@@ -263,7 +263,7 @@ static int usb_dev_suspend(struct device *dev)
 
 static int usb_dev_resume(struct device *dev)
 {
-	return usb_resume(dev);
+	return usb_resume(dev, PMSG_RESUME);
 }
 
 static int usb_dev_freeze(struct device *dev)
@@ -273,7 +273,7 @@ static int usb_dev_freeze(struct device *dev)
 
 static int usb_dev_thaw(struct device *dev)
 {
-	return usb_resume(dev);
+	return usb_resume(dev, PMSG_THAW);
 }
 
 static int usb_dev_poweroff(struct device *dev)
@@ -283,10 +283,10 @@ static int usb_dev_poweroff(struct device *dev)
 
 static int usb_dev_restore(struct device *dev)
 {
-	return usb_resume(dev);
+	return usb_resume(dev, PMSG_RESTORE);
 }
 
-static struct pm_ops usb_device_pm_ops = {
+static struct dev_pm_ops usb_device_pm_ops = {
 	.prepare =	usb_dev_prepare,
 	.complete =	usb_dev_complete,
 	.suspend =	usb_dev_suspend,
@@ -301,7 +301,7 @@ static struct pm_ops usb_device_pm_ops = {
 
 #define ksuspend_usb_init()	0
 #define ksuspend_usb_cleanup()	do {} while (0)
-#define usb_device_pm_ops	(*(struct pm_ops *)0)
+#define usb_device_pm_ops	(*(struct dev_pm_ops *)0)
 
 #endif	/* CONFIG_PM */
 
@@ -362,7 +362,7 @@ struct usb_device *usb_alloc_dev(struct usb_device *parent,
 	dev->ep0.desc.bLength = USB_DT_ENDPOINT_SIZE;
 	dev->ep0.desc.bDescriptorType = USB_DT_ENDPOINT;
 	/* ep0 maxpacket comes later, from device descriptor */
-	usb_enable_endpoint(dev, &dev->ep0);
+	usb_enable_endpoint(dev, &dev->ep0, true);
 	dev->can_submit = 1;
 
 	/* Save readable and stable topology id, distinguishing devices
@@ -402,6 +402,7 @@ struct usb_device *usb_alloc_dev(struct usb_device *parent,
 #ifdef	CONFIG_PM
 	mutex_init(&dev->pm_mutex);
 	INIT_DELAYED_WORK(&dev->autosuspend, usb_autosuspend_work);
+	INIT_WORK(&dev->autoresume, usb_autoresume_work);
 	dev->autosuspend_delay = usb_autosuspend_delay * HZ;
 	dev->connect_time = jiffies;
 	dev->active_duration = -jiffies;
@@ -513,10 +514,7 @@ EXPORT_SYMBOL_GPL(usb_put_intf);
  * disconnect; in some drivers (such as usb-storage) the disconnect()
  * or suspend() method will block waiting for a device reset to complete.
  *
- * Returns a negative error code for failure, otherwise 1 or 0 to indicate
- * that the device will or will not have to be unlocked.  (0 can be
- * returned when an interface is given and is BINDING, because in that
- * case the driver already owns the device lock.)
+ * Returns a negative error code for failure, otherwise 0.
  */
 int usb_lock_device_for_reset(struct usb_device *udev,
 			      const struct usb_interface *iface)
@@ -527,16 +525,9 @@ int usb_lock_device_for_reset(struct usb_device *udev,
 		return -ENODEV;
 	if (udev->state == USB_STATE_SUSPENDED)
 		return -EHOSTUNREACH;
-	if (iface) {
-		switch (iface->condition) {
-		case USB_INTERFACE_BINDING:
-			return 0;
-		case USB_INTERFACE_BOUND:
-			break;
-		default:
-			return -EINTR;
-		}
-	}
+	if (iface && (iface->condition == USB_INTERFACE_UNBINDING ||
+			iface->condition == USB_INTERFACE_UNBOUND))
+		return -EINTR;
 
 	while (usb_trylock_device(udev) != 0) {
 
@@ -550,10 +541,11 @@ int usb_lock_device_for_reset(struct usb_device *udev,
 			return -ENODEV;
 		if (udev->state == USB_STATE_SUSPENDED)
 			return -EHOSTUNREACH;
-		if (iface && iface->condition != USB_INTERFACE_BOUND)
+		if (iface && (iface->condition == USB_INTERFACE_UNBINDING ||
+				iface->condition == USB_INTERFACE_UNBOUND))
 			return -EINTR;
 	}
-	return 1;
+	return 0;
 }
 EXPORT_SYMBOL_GPL(usb_lock_device_for_reset);
 
@@ -962,8 +954,12 @@ void usb_buffer_unmap_sg(const struct usb_device *dev, int is_in,
 }
 EXPORT_SYMBOL_GPL(usb_buffer_unmap_sg);
 
-/* format to disable USB on kernel command line is: nousb */
-__module_param_call("", nousb, param_set_bool, param_get_bool, &nousb, 0444);
+/* To disable USB, kernel command line is 'nousb' not 'usbcore.nousb' */
+#ifdef MODULE
+module_param(nousb, bool, 0444);
+#else
+core_param(nousb, nousb, bool, 0444);
+#endif
 
 /*
  * for external read access to <nousb>
@@ -975,6 +971,37 @@ int usb_disabled(void)
 EXPORT_SYMBOL_GPL(usb_disabled);
 
 /*
+ * Notifications of device and interface registration
+ */
+static int usb_bus_notify(struct notifier_block *nb, unsigned long action,
+		void *data)
+{
+	struct device *dev = data;
+
+	switch (action) {
+	case BUS_NOTIFY_ADD_DEVICE:
+		if (dev->type == &usb_device_type)
+			(void) usb_create_sysfs_dev_files(to_usb_device(dev));
+		else if (dev->type == &usb_if_device_type)
+			(void) usb_create_sysfs_intf_files(
+					to_usb_interface(dev));
+		break;
+
+	case BUS_NOTIFY_DEL_DEVICE:
+		if (dev->type == &usb_device_type)
+			usb_remove_sysfs_dev_files(to_usb_device(dev));
+		else if (dev->type == &usb_if_device_type)
+			usb_remove_sysfs_intf_files(to_usb_interface(dev));
+		break;
+	}
+	return 0;
+}
+
+static struct notifier_block usb_bus_nb = {
+	.notifier_call = usb_bus_notify,
+};
+
+/*
  * Init
  */
 static int __init usb_init(void)
@@ -991,6 +1018,9 @@ static int __init usb_init(void)
 	retval = bus_register(&usb_bus_type);
 	if (retval)
 		goto bus_register_failed;
+	retval = bus_register_notifier(&usb_bus_type, &usb_bus_nb);
+	if (retval)
+		goto bus_notifier_failed;
 	retval = usb_host_init();
 	if (retval)
 		goto host_init_failed;
@@ -1025,6 +1055,8 @@ driver_register_failed:
 major_init_failed:
 	usb_host_cleanup();
 host_init_failed:
+	bus_unregister_notifier(&usb_bus_type, &usb_bus_nb);
+bus_notifier_failed:
 	bus_unregister(&usb_bus_type);
 bus_register_failed:
 	ksuspend_usb_cleanup();
@@ -1048,6 +1080,7 @@ static void __exit usb_exit(void)
 	usb_devio_cleanup();
 	usb_hub_cleanup();
 	usb_host_cleanup();
+	bus_unregister_notifier(&usb_bus_type, &usb_bus_nb);
 	bus_unregister(&usb_bus_type);
 	ksuspend_usb_cleanup();
 }
diff --git a/drivers/usb/core/usb.h b/drivers/usb/core/usb.h
index 9a1a45ac3add..386177867a8a 100644
--- a/drivers/usb/core/usb.h
+++ b/drivers/usb/core/usb.h
@@ -1,16 +1,20 @@
+#include <linux/pm.h>
+
 /* Functions local to drivers/usb/core/ */
 
 extern int usb_create_sysfs_dev_files(struct usb_device *dev);
 extern void usb_remove_sysfs_dev_files(struct usb_device *dev);
 extern int usb_create_sysfs_intf_files(struct usb_interface *intf);
 extern void usb_remove_sysfs_intf_files(struct usb_interface *intf);
-extern int usb_create_ep_files(struct device *parent,
+extern int usb_create_ep_devs(struct device *parent,
 				struct usb_host_endpoint *endpoint,
 				struct usb_device *udev);
-extern void usb_remove_ep_files(struct usb_host_endpoint *endpoint);
+extern void usb_remove_ep_devs(struct usb_host_endpoint *endpoint);
 
 extern void usb_enable_endpoint(struct usb_device *dev,
-		struct usb_host_endpoint *ep);
+		struct usb_host_endpoint *ep, bool reset_toggle);
+extern void usb_enable_interface(struct usb_device *dev,
+		struct usb_interface *intf, bool reset_toggles);
 extern void usb_disable_endpoint(struct usb_device *dev, unsigned int epaddr);
 extern void usb_disable_interface(struct usb_device *dev,
 		struct usb_interface *intf);
@@ -42,14 +46,16 @@ extern void usb_host_cleanup(void);
 #ifdef	CONFIG_PM
 
 extern int usb_suspend(struct device *dev, pm_message_t msg);
-extern int usb_resume(struct device *dev);
+extern int usb_resume(struct device *dev, pm_message_t msg);
 
 extern void usb_autosuspend_work(struct work_struct *work);
-extern int usb_port_suspend(struct usb_device *dev);
-extern int usb_port_resume(struct usb_device *dev);
+extern void usb_autoresume_work(struct work_struct *work);
+extern int usb_port_suspend(struct usb_device *dev, pm_message_t msg);
+extern int usb_port_resume(struct usb_device *dev, pm_message_t msg);
 extern int usb_external_suspend_device(struct usb_device *udev,
 		pm_message_t msg);
-extern int usb_external_resume_device(struct usb_device *udev);
+extern int usb_external_resume_device(struct usb_device *udev,
+		pm_message_t msg);
 
 static inline void usb_pm_lock(struct usb_device *udev)
 {
@@ -63,12 +69,12 @@ static inline void usb_pm_unlock(struct usb_device *udev)
 
 #else
 
-static inline int usb_port_suspend(struct usb_device *udev)
+static inline int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 {
 	return 0;
 }
 
-static inline int usb_port_resume(struct usb_device *udev)
+static inline int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 {
 	return 0;
 }
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index dd4cd5a51370..3219d137340a 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -297,13 +297,34 @@ config USB_S3C2410_DEBUG
 
 # musb builds in ../musb along with host support
 config USB_GADGET_MUSB_HDRC
-	boolean "Inventra HDRC USB Peripheral (TI, ...)"
+	boolean "Inventra HDRC USB Peripheral (TI, ADI, ...)"
 	depends on USB_MUSB_HDRC && (USB_MUSB_PERIPHERAL || USB_MUSB_OTG)
 	select USB_GADGET_DUALSPEED
 	select USB_GADGET_SELECTED
 	help
 	  This OTG-capable silicon IP is used in dual designs including
-	  the TI DaVinci, OMAP 243x, OMAP 343x, and TUSB 6010.
+	  the TI DaVinci, OMAP 243x, OMAP 343x, TUSB 6010, and ADI Blackfin
+
+config USB_GADGET_IMX
+	boolean "Freescale IMX USB Peripheral Controller"
+	depends on ARCH_MX1
+	help
+	   Freescale's IMX series include an integrated full speed
+	   USB 1.1 device controller.  The controller in the IMX series
+	   is register-compatible.
+
+	   It has Six fixed-function endpoints, as well as endpoint
+	   zero (for control transfers).
+
+	   Say "y" to link the driver statically, or "m" to build a
+	   dynamically linked module called "imx_udc" and force all
+	   gadget drivers to also be dynamically linked.
+
+config USB_IMX
+	tristate
+	depends on USB_GADGET_IMX
+	default USB_GADGET
+	select USB_GADGET_SELECTED
 
 config USB_GADGET_M66592
 	boolean "Renesas M66592 USB Peripheral Controller"
@@ -377,6 +398,24 @@ config USB_FSL_QE
 	default USB_GADGET
 	select USB_GADGET_SELECTED
 
+config USB_GADGET_CI13XXX
+	boolean "MIPS USB CI13xxx"
+	depends on PCI
+	select USB_GADGET_DUALSPEED
+	help
+	  MIPS USB IP core family device controller
+	  Currently it only supports IP part number CI13412
+
+	  Say "y" to link the driver statically, or "m" to build a
+	  dynamically linked module called "ci13xxx_udc" and force all
+	  gadget drivers to also be dynamically linked.
+
+config USB_CI13XXX
+	tristate
+	depends on USB_GADGET_CI13XXX
+	default USB_GADGET
+	select USB_GADGET_SELECTED
+
 config USB_GADGET_NET2280
 	boolean "NetChip 228x"
 	depends on PCI
diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index bd4041b47dce..39a51d746cb7 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -10,6 +10,7 @@ obj-$(CONFIG_USB_NET2280)	+= net2280.o
 obj-$(CONFIG_USB_AMD5536UDC)	+= amd5536udc.o
 obj-$(CONFIG_USB_PXA25X)	+= pxa25x_udc.o
 obj-$(CONFIG_USB_PXA27X)	+= pxa27x_udc.o
+obj-$(CONFIG_USB_IMX)		+= imx_udc.o
 obj-$(CONFIG_USB_GOKU)		+= goku_udc.o
 obj-$(CONFIG_USB_OMAP)		+= omap_udc.o
 obj-$(CONFIG_USB_LH7A40X)	+= lh7a40x_udc.o
@@ -19,6 +20,7 @@ obj-$(CONFIG_USB_ATMEL_USBA)	+= atmel_usba_udc.o
 obj-$(CONFIG_USB_FSL_USB2)	+= fsl_usb2_udc.o
 obj-$(CONFIG_USB_M66592)	+= m66592-udc.o
 obj-$(CONFIG_USB_FSL_QE)	+= fsl_qe_udc.o
+obj-$(CONFIG_USB_CI13XXX)	+= ci13xxx_udc.o
 
 #
 # USB gadget drivers
diff --git a/drivers/usb/gadget/at91_udc.c b/drivers/usb/gadget/at91_udc.c
index a8a1de413321..0b2bb8f0706d 100644
--- a/drivers/usb/gadget/at91_udc.c
+++ b/drivers/usb/gadget/at91_udc.c
@@ -1474,7 +1474,7 @@ static struct at91_udc controller = {
 		.ep0	= &controller.ep[0].ep,
 		.name	= driver_name,
 		.dev	= {
-			.bus_id = "gadget",
+			.init_name = "gadget",
 			.release = nop_release,
 		}
 	},
diff --git a/drivers/usb/gadget/atmel_usba_udc.c b/drivers/usb/gadget/atmel_usba_udc.c
index ae30ab1d264f..65b03e3445a1 100644
--- a/drivers/usb/gadget/atmel_usba_udc.c
+++ b/drivers/usb/gadget/atmel_usba_udc.c
@@ -1034,7 +1034,7 @@ static struct usba_udc the_udc = {
 		.is_dualspeed	= 1,
 		.name		= "atmel_usba_udc",
 		.dev	= {
-			.bus_id		= "gadget",
+			.init_name	= "gadget",
 			.release	= nop_release,
 		},
 	},
diff --git a/drivers/usb/gadget/ci13xxx_udc.c b/drivers/usb/gadget/ci13xxx_udc.c
new file mode 100644
index 000000000000..bebf911c7e5f
--- /dev/null
+++ b/drivers/usb/gadget/ci13xxx_udc.c
@@ -0,0 +1,2830 @@
+/*
+ * ci13xxx_udc.c - MIPS USB IP core family device controller
+ *
+ * Copyright (C) 2008 Chipidea - MIPS Technologies, Inc. All rights reserved.
+ *
+ * Author: David Lopo
+ *
+ * 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.
+ */
+
+/*
+ * Description: MIPS USB IP core family device controller
+ *              Currently it only supports IP part number CI13412
+ *
+ * This driver is composed of several blocks:
+ * - HW:     hardware interface
+ * - DBG:    debug facilities (optional)
+ * - UTIL:   utilities
+ * - ISR:    interrupts handling
+ * - ENDPT:  endpoint operations (Gadget API)
+ * - GADGET: gadget operations (Gadget API)
+ * - BUS:    bus glue code, bus abstraction layer
+ * - PCI:    PCI core interface and PCI resources (interrupts, memory...)
+ *
+ * Compile Options
+ * - CONFIG_USB_GADGET_DEBUG_FILES: 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
+ *              => case 5: Hi >  Di
+ *              => case 8: Hi <> Do
+ *              if undefined usbtest 13 fails
+ * - TRACE:     enable function tracing (depends on DEBUG)
+ *
+ * Main Features
+ * - Chapter 9 & Mass Storage Compliance with Gadget File Storage
+ * - Chapter 9 Compliance with Gadget Zero (STALL_IN undefined)
+ * - Normal & LPM support
+ *
+ * USBTEST Report
+ * - OK: 0-12, 13 (STALL_IN defined) & 14
+ * - Not Supported: 15 & 16 (ISO)
+ *
+ * TODO List
+ * - OTG
+ * - Isochronous & Interrupt Traffic
+ * - Handle requests which spawns into several TDs
+ * - GET_STATUS(device) - always reports 0
+ * - Gadget API (majority of optional features)
+ * - Suspend & Remote Wakeup
+ */
+#include <linux/device.h>
+#include <linux/dmapool.h>
+#include <linux/dma-mapping.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+
+#include "ci13xxx_udc.h"
+
+
+/******************************************************************************
+ * DEFINE
+ *****************************************************************************/
+/* ctrl register bank access */
+static DEFINE_SPINLOCK(udc_lock);
+
+/* driver name */
+#define UDC_DRIVER_NAME   "ci13xxx_udc"
+
+/* control endpoint description */
+static const struct usb_endpoint_descriptor
+ctrl_endpt_desc = {
+	.bLength         = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType = USB_DT_ENDPOINT,
+
+	.bmAttributes    = USB_ENDPOINT_XFER_CONTROL,
+	.wMaxPacketSize  = cpu_to_le16(CTRL_PAYLOAD_MAX),
+};
+
+/* UDC descriptor */
+static struct ci13xxx *_udc;
+
+/* Interrupt statistics */
+#define ISR_MASK   0x1F
+static struct {
+	u32 test;
+	u32 ui;
+	u32 uei;
+	u32 pci;
+	u32 uri;
+	u32 sli;
+	u32 none;
+	struct {
+		u32 cnt;
+		u32 buf[ISR_MASK+1];
+		u32 idx;
+	} hndl;
+} isr_statistics;
+
+/**
+ * ffs_nr: find first (least significant) bit set
+ * @x: the word to search
+ *
+ * This function returns bit number (instead of position)
+ */
+static int ffs_nr(u32 x)
+{
+	int n = ffs(x);
+
+	return n ? n-1 : 32;
+}
+
+/******************************************************************************
+ * HW block
+ *****************************************************************************/
+/* register bank descriptor */
+static struct {
+	unsigned      lpm;    /* is LPM? */
+	void __iomem *abs;    /* bus map offset */
+	void __iomem *cap;    /* bus map offset + CAP offset + CAP data */
+	size_t        size;   /* bank size */
+} hw_bank;
+
+/* UDC register map */
+#define ABS_CAPLENGTH       (0x100UL)
+#define ABS_HCCPARAMS       (0x108UL)
+#define ABS_DCCPARAMS       (0x124UL)
+#define ABS_TESTMODE        (hw_bank.lpm ? 0x0FCUL : 0x138UL)
+/* offset to CAPLENTGH (addr + data) */
+#define CAP_USBCMD          (0x000UL)
+#define CAP_USBSTS          (0x004UL)
+#define CAP_USBINTR         (0x008UL)
+#define CAP_DEVICEADDR      (0x014UL)
+#define CAP_ENDPTLISTADDR   (0x018UL)
+#define CAP_PORTSC          (0x044UL)
+#define CAP_DEVLC           (0x0B4UL)
+#define CAP_USBMODE         (hw_bank.lpm ? 0x0C8UL : 0x068UL)
+#define CAP_ENDPTSETUPSTAT  (hw_bank.lpm ? 0x0D8UL : 0x06CUL)
+#define CAP_ENDPTPRIME      (hw_bank.lpm ? 0x0DCUL : 0x070UL)
+#define CAP_ENDPTFLUSH      (hw_bank.lpm ? 0x0E0UL : 0x074UL)
+#define CAP_ENDPTSTAT       (hw_bank.lpm ? 0x0E4UL : 0x078UL)
+#define CAP_ENDPTCOMPLETE   (hw_bank.lpm ? 0x0E8UL : 0x07CUL)
+#define CAP_ENDPTCTRL       (hw_bank.lpm ? 0x0ECUL : 0x080UL)
+#define CAP_LAST            (hw_bank.lpm ? 0x12CUL : 0x0C0UL)
+
+/* maximum number of enpoints: valid only after hw_device_reset() */
+static unsigned hw_ep_max;
+
+/**
+ * hw_ep_bit: calculates the bit number
+ * @num: endpoint number
+ * @dir: endpoint direction
+ *
+ * This function returns bit number
+ */
+static inline int hw_ep_bit(int num, int dir)
+{
+	return num + (dir ? 16 : 0);
+}
+
+/**
+ * hw_aread: reads from register bitfield
+ * @addr: address relative to bus map
+ * @mask: bitfield mask
+ *
+ * This function returns register bitfield data
+ */
+static u32 hw_aread(u32 addr, u32 mask)
+{
+	return ioread32(addr + hw_bank.abs) & mask;
+}
+
+/**
+ * hw_awrite: writes to register bitfield
+ * @addr: address relative to bus map
+ * @mask: bitfield mask
+ * @data: new data
+ */
+static void hw_awrite(u32 addr, u32 mask, u32 data)
+{
+	iowrite32(hw_aread(addr, ~mask) | (data & mask),
+		  addr + hw_bank.abs);
+}
+
+/**
+ * hw_cread: reads from register bitfield
+ * @addr: address relative to CAP offset plus content
+ * @mask: bitfield mask
+ *
+ * This function returns register bitfield data
+ */
+static u32 hw_cread(u32 addr, u32 mask)
+{
+	return ioread32(addr + hw_bank.cap) & mask;
+}
+
+/**
+ * hw_cwrite: writes to register bitfield
+ * @addr: address relative to CAP offset plus content
+ * @mask: bitfield mask
+ * @data: new data
+ */
+static void hw_cwrite(u32 addr, u32 mask, u32 data)
+{
+	iowrite32(hw_cread(addr, ~mask) | (data & mask),
+		  addr + hw_bank.cap);
+}
+
+/**
+ * hw_ctest_and_clear: tests & clears register bitfield
+ * @addr: address relative to CAP offset plus content
+ * @mask: bitfield mask
+ *
+ * This function returns register bitfield data
+ */
+static u32 hw_ctest_and_clear(u32 addr, u32 mask)
+{
+	u32 reg = hw_cread(addr, mask);
+
+	iowrite32(reg, addr + hw_bank.cap);
+	return reg;
+}
+
+/**
+ * hw_ctest_and_write: tests & writes register bitfield
+ * @addr: address relative to CAP offset plus content
+ * @mask: bitfield mask
+ * @data: new data
+ *
+ * This function returns register bitfield data
+ */
+static u32 hw_ctest_and_write(u32 addr, u32 mask, u32 data)
+{
+	u32 reg = hw_cread(addr, ~0);
+
+	iowrite32((reg & ~mask) | (data & mask), addr + hw_bank.cap);
+	return (reg & mask) >> ffs_nr(mask);
+}
+
+/**
+ * hw_device_reset: resets chip (execute without interruption)
+ * @base: register base address
+ *
+ * This function returns an error code
+ */
+static int hw_device_reset(void __iomem *base)
+{
+	u32 reg;
+
+	/* bank is a module variable */
+	hw_bank.abs = base;
+
+	hw_bank.cap = hw_bank.abs;
+	hw_bank.cap += ABS_CAPLENGTH;
+	hw_bank.cap += ioread8(hw_bank.cap);
+
+	reg = hw_aread(ABS_HCCPARAMS, HCCPARAMS_LEN) >> ffs_nr(HCCPARAMS_LEN);
+	hw_bank.lpm  = reg;
+	hw_bank.size = hw_bank.cap - hw_bank.abs;
+	hw_bank.size += CAP_LAST;
+	hw_bank.size /= sizeof(u32);
+
+	/* should flush & stop before reset */
+	hw_cwrite(CAP_ENDPTFLUSH, ~0, ~0);
+	hw_cwrite(CAP_USBCMD, USBCMD_RS, 0);
+
+	hw_cwrite(CAP_USBCMD, USBCMD_RST, USBCMD_RST);
+	while (hw_cread(CAP_USBCMD, USBCMD_RST))
+		udelay(10);             /* not RTOS friendly */
+
+	/* USBMODE should be configured step by step */
+	hw_cwrite(CAP_USBMODE, USBMODE_CM, USBMODE_CM_IDLE);
+	hw_cwrite(CAP_USBMODE, USBMODE_CM, USBMODE_CM_DEVICE);
+	hw_cwrite(CAP_USBMODE, USBMODE_SLOM, USBMODE_SLOM);  /* HW >= 2.3 */
+
+	if (hw_cread(CAP_USBMODE, USBMODE_CM) != USBMODE_CM_DEVICE) {
+		pr_err("cannot enter in device mode");
+		pr_err("lpm = %i", hw_bank.lpm);
+		return -ENODEV;
+	}
+
+	reg = hw_aread(ABS_DCCPARAMS, DCCPARAMS_DEN) >> ffs_nr(DCCPARAMS_DEN);
+	if (reg == 0 || reg > ENDPT_MAX)
+		return -ENODEV;
+
+	hw_ep_max = reg;   /* cache hw ENDPT_MAX */
+
+	/* setup lock mode ? */
+
+	/* ENDPTSETUPSTAT is '0' by default */
+
+	/* HCSPARAMS.bf.ppc SHOULD BE zero for device */
+
+	return 0;
+}
+
+/**
+ * hw_device_state: enables/disables interrupts & starts/stops device (execute
+ *                  without interruption)
+ * @dma: 0 => disable, !0 => enable and set dma engine
+ *
+ * This function returns an error code
+ */
+static int hw_device_state(u32 dma)
+{
+	if (dma) {
+		hw_cwrite(CAP_ENDPTLISTADDR, ~0, dma);
+		/* interrupt, error, port change, reset, sleep/suspend */
+		hw_cwrite(CAP_USBINTR, ~0,
+			     USBi_UI|USBi_UEI|USBi_PCI|USBi_URI|USBi_SLI);
+		hw_cwrite(CAP_USBCMD, USBCMD_RS, USBCMD_RS);
+	} else {
+		hw_cwrite(CAP_USBCMD, USBCMD_RS, 0);
+		hw_cwrite(CAP_USBINTR, ~0, 0);
+	}
+	return 0;
+}
+
+/**
+ * hw_ep_flush: flush endpoint fifo (execute without interruption)
+ * @num: endpoint number
+ * @dir: endpoint direction
+ *
+ * This function returns an error code
+ */
+static int hw_ep_flush(int num, int dir)
+{
+	int n = hw_ep_bit(num, dir);
+
+	do {
+		/* flush any pending transfer */
+		hw_cwrite(CAP_ENDPTFLUSH, BIT(n), BIT(n));
+		while (hw_cread(CAP_ENDPTFLUSH, BIT(n)))
+			cpu_relax();
+	} while (hw_cread(CAP_ENDPTSTAT, BIT(n)));
+
+	return 0;
+}
+
+/**
+ * hw_ep_disable: disables endpoint (execute without interruption)
+ * @num: endpoint number
+ * @dir: endpoint direction
+ *
+ * This function returns an error code
+ */
+static int hw_ep_disable(int num, int dir)
+{
+	hw_ep_flush(num, dir);
+	hw_cwrite(CAP_ENDPTCTRL + num * sizeof(u32),
+		  dir ? ENDPTCTRL_TXE : ENDPTCTRL_RXE, 0);
+	return 0;
+}
+
+/**
+ * hw_ep_enable: enables endpoint (execute without interruption)
+ * @num:  endpoint number
+ * @dir:  endpoint direction
+ * @type: endpoint type
+ *
+ * This function returns an error code
+ */
+static int hw_ep_enable(int num, int dir, int type)
+{
+	u32 mask, data;
+
+	if (dir) {
+		mask  = ENDPTCTRL_TXT;  /* type    */
+		data  = type << ffs_nr(mask);
+
+		mask |= ENDPTCTRL_TXS;  /* unstall */
+		mask |= ENDPTCTRL_TXR;  /* reset data toggle */
+		data |= ENDPTCTRL_TXR;
+		mask |= ENDPTCTRL_TXE;  /* enable  */
+		data |= ENDPTCTRL_TXE;
+	} else {
+		mask  = ENDPTCTRL_RXT;  /* type    */
+		data  = type << ffs_nr(mask);
+
+		mask |= ENDPTCTRL_RXS;  /* unstall */
+		mask |= ENDPTCTRL_RXR;  /* reset data toggle */
+		data |= ENDPTCTRL_RXR;
+		mask |= ENDPTCTRL_RXE;  /* enable  */
+		data |= ENDPTCTRL_RXE;
+	}
+	hw_cwrite(CAP_ENDPTCTRL + num * sizeof(u32), mask, data);
+	return 0;
+}
+
+/**
+ * hw_ep_get_halt: return endpoint halt status
+ * @num: endpoint number
+ * @dir: endpoint direction
+ *
+ * This function returns 1 if endpoint halted
+ */
+static int hw_ep_get_halt(int num, int dir)
+{
+	u32 mask = dir ? ENDPTCTRL_TXS : ENDPTCTRL_RXS;
+
+	return hw_cread(CAP_ENDPTCTRL + num * sizeof(u32), mask) ? 1 : 0;
+}
+
+/**
+ * hw_ep_is_primed: test if endpoint is primed (execute without interruption)
+ * @num:   endpoint number
+ * @dir:   endpoint direction
+ *
+ * This function returns true if endpoint primed
+ */
+static int hw_ep_is_primed(int num, int dir)
+{
+	u32 reg = hw_cread(CAP_ENDPTPRIME, ~0) | hw_cread(CAP_ENDPTSTAT, ~0);
+
+	return test_bit(hw_ep_bit(num, dir), (void *)&reg);
+}
+
+/**
+ * hw_test_and_clear_setup_status: test & clear setup status (execute without
+ *                                 interruption)
+ * @n: bit number (endpoint)
+ *
+ * This function returns setup status
+ */
+static int hw_test_and_clear_setup_status(int n)
+{
+	return hw_ctest_and_clear(CAP_ENDPTSETUPSTAT, BIT(n));
+}
+
+/**
+ * hw_ep_prime: primes endpoint (execute without interruption)
+ * @num:     endpoint number
+ * @dir:     endpoint direction
+ * @is_ctrl: true if control endpoint
+ *
+ * This function returns an error code
+ */
+static int hw_ep_prime(int num, int dir, int is_ctrl)
+{
+	int n = hw_ep_bit(num, dir);
+
+	/* the caller should flush first */
+	if (hw_ep_is_primed(num, dir))
+		return -EBUSY;
+
+	if (is_ctrl && dir == RX && hw_cread(CAP_ENDPTSETUPSTAT, BIT(num)))
+		return -EAGAIN;
+
+	hw_cwrite(CAP_ENDPTPRIME, BIT(n), BIT(n));
+
+	while (hw_cread(CAP_ENDPTPRIME, BIT(n)))
+		cpu_relax();
+	if (is_ctrl && dir == RX  && hw_cread(CAP_ENDPTSETUPSTAT, BIT(num)))
+		return -EAGAIN;
+
+	/* status shoult be tested according with manual but it doesn't work */
+	return 0;
+}
+
+/**
+ * hw_ep_set_halt: configures ep halt & resets data toggle after clear (execute
+ *                 without interruption)
+ * @num:   endpoint number
+ * @dir:   endpoint direction
+ * @value: true => stall, false => unstall
+ *
+ * This function returns an error code
+ */
+static int hw_ep_set_halt(int num, int dir, int value)
+{
+	if (value != 0 && value != 1)
+		return -EINVAL;
+
+	do {
+		u32 addr = CAP_ENDPTCTRL + num * sizeof(u32);
+		u32 mask_xs = dir ? ENDPTCTRL_TXS : ENDPTCTRL_RXS;
+		u32 mask_xr = dir ? ENDPTCTRL_TXR : ENDPTCTRL_RXR;
+
+		/* data toggle - reserved for EP0 but it's in ESS */
+		hw_cwrite(addr, mask_xs|mask_xr, value ? mask_xs : mask_xr);
+
+	} while (value != hw_ep_get_halt(num, dir));
+
+	return 0;
+}
+
+/**
+ * hw_intr_clear: disables interrupt & clears interrupt status (execute without
+ *                interruption)
+ * @n: interrupt bit
+ *
+ * This function returns an error code
+ */
+static int hw_intr_clear(int n)
+{
+	if (n >= REG_BITS)
+		return -EINVAL;
+
+	hw_cwrite(CAP_USBINTR, BIT(n), 0);
+	hw_cwrite(CAP_USBSTS,  BIT(n), BIT(n));
+	return 0;
+}
+
+/**
+ * hw_intr_force: enables interrupt & forces interrupt status (execute without
+ *                interruption)
+ * @n: interrupt bit
+ *
+ * This function returns an error code
+ */
+static int hw_intr_force(int n)
+{
+	if (n >= REG_BITS)
+		return -EINVAL;
+
+	hw_awrite(ABS_TESTMODE, TESTMODE_FORCE, TESTMODE_FORCE);
+	hw_cwrite(CAP_USBINTR,  BIT(n), BIT(n));
+	hw_cwrite(CAP_USBSTS,   BIT(n), BIT(n));
+	hw_awrite(ABS_TESTMODE, TESTMODE_FORCE, 0);
+	return 0;
+}
+
+/**
+ * hw_is_port_high_speed: test if port is high speed
+ *
+ * This function returns true if high speed port
+ */
+static int hw_port_is_high_speed(void)
+{
+	return hw_bank.lpm ? hw_cread(CAP_DEVLC, DEVLC_PSPD) :
+		hw_cread(CAP_PORTSC, PORTSC_HSP);
+}
+
+/**
+ * hw_port_test_get: reads port test mode value
+ *
+ * This function returns port test mode value
+ */
+static u8 hw_port_test_get(void)
+{
+	return hw_cread(CAP_PORTSC, PORTSC_PTC) >> ffs_nr(PORTSC_PTC);
+}
+
+/**
+ * hw_port_test_set: writes port test mode (execute without interruption)
+ * @mode: new value
+ *
+ * This function returns an error code
+ */
+static int hw_port_test_set(u8 mode)
+{
+	const u8 TEST_MODE_MAX = 7;
+
+	if (mode > TEST_MODE_MAX)
+		return -EINVAL;
+
+	hw_cwrite(CAP_PORTSC, PORTSC_PTC, mode << ffs_nr(PORTSC_PTC));
+	return 0;
+}
+
+/**
+ * hw_read_intr_enable: returns interrupt enable register
+ *
+ * This function returns register data
+ */
+static u32 hw_read_intr_enable(void)
+{
+	return hw_cread(CAP_USBINTR, ~0);
+}
+
+/**
+ * hw_read_intr_status: returns interrupt status register
+ *
+ * This function returns register data
+ */
+static u32 hw_read_intr_status(void)
+{
+	return hw_cread(CAP_USBSTS, ~0);
+}
+
+/**
+ * hw_register_read: reads all device registers (execute without interruption)
+ * @buf:  destination buffer
+ * @size: buffer size
+ *
+ * This function returns number of registers read
+ */
+static size_t hw_register_read(u32 *buf, size_t size)
+{
+	unsigned i;
+
+	if (size > hw_bank.size)
+		size = hw_bank.size;
+
+	for (i = 0; i < size; i++)
+		buf[i] = hw_aread(i * sizeof(u32), ~0);
+
+	return size;
+}
+
+/**
+ * hw_register_write: writes to register
+ * @addr: register address
+ * @data: register value
+ *
+ * This function returns an error code
+ */
+static int hw_register_write(u16 addr, u32 data)
+{
+	/* align */
+	addr /= sizeof(u32);
+
+	if (addr >= hw_bank.size)
+		return -EINVAL;
+
+	/* align */
+	addr *= sizeof(u32);
+
+	hw_awrite(addr, ~0, data);
+	return 0;
+}
+
+/**
+ * hw_test_and_clear_complete: test & clear complete status (execute without
+ *                             interruption)
+ * @n: bit number (endpoint)
+ *
+ * This function returns complete status
+ */
+static int hw_test_and_clear_complete(int n)
+{
+	return hw_ctest_and_clear(CAP_ENDPTCOMPLETE, BIT(n));
+}
+
+/**
+ * hw_test_and_clear_intr_active: test & clear active interrupts (execute
+ *                                without interruption)
+ *
+ * This function returns active interrutps
+ */
+static u32 hw_test_and_clear_intr_active(void)
+{
+	u32 reg = hw_read_intr_status() & hw_read_intr_enable();
+
+	hw_cwrite(CAP_USBSTS, ~0, reg);
+	return reg;
+}
+
+/**
+ * hw_test_and_clear_setup_guard: test & clear setup guard (execute without
+ *                                interruption)
+ *
+ * This function returns guard value
+ */
+static int hw_test_and_clear_setup_guard(void)
+{
+	return hw_ctest_and_write(CAP_USBCMD, USBCMD_SUTW, 0);
+}
+
+/**
+ * hw_test_and_set_setup_guard: test & set setup guard (execute without
+ *                              interruption)
+ *
+ * This function returns guard value
+ */
+static int hw_test_and_set_setup_guard(void)
+{
+	return hw_ctest_and_write(CAP_USBCMD, USBCMD_SUTW, USBCMD_SUTW);
+}
+
+/**
+ * hw_usb_set_address: configures USB address (execute without interruption)
+ * @value: new USB address
+ *
+ * This function returns an error code
+ */
+static int hw_usb_set_address(u8 value)
+{
+	/* advance */
+	hw_cwrite(CAP_DEVICEADDR, DEVICEADDR_USBADR | DEVICEADDR_USBADRA,
+		  value << ffs_nr(DEVICEADDR_USBADR) | DEVICEADDR_USBADRA);
+	return 0;
+}
+
+/**
+ * hw_usb_reset: restart device after a bus reset (execute without
+ *               interruption)
+ *
+ * This function returns an error code
+ */
+static int hw_usb_reset(void)
+{
+	hw_usb_set_address(0);
+
+	/* ESS flushes only at end?!? */
+	hw_cwrite(CAP_ENDPTFLUSH,    ~0, ~0);   /* flush all EPs */
+
+	/* clear setup token semaphores */
+	hw_cwrite(CAP_ENDPTSETUPSTAT, 0,  0);   /* writes its content */
+
+	/* clear complete status */
+	hw_cwrite(CAP_ENDPTCOMPLETE,  0,  0);   /* writes its content */
+
+	/* wait until all bits cleared */
+	while (hw_cread(CAP_ENDPTPRIME, ~0))
+		udelay(10);             /* not RTOS friendly */
+
+	/* reset all endpoints ? */
+
+	/* reset internal status and wait for further instructions
+	   no need to verify the port reset status (ESS does it) */
+
+	return 0;
+}
+
+/******************************************************************************
+ * DBG block
+ *****************************************************************************/
+/**
+ * show_device: prints information about device capabilities and status
+ *
+ * Check "device.h" for details
+ */
+static ssize_t show_device(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	struct usb_gadget *gadget = &udc->gadget;
+	int n = 0;
+
+	dbg_trace("[%s] %p\n", __func__, buf);
+	if (attr == NULL || buf == NULL) {
+		dev_err(dev, "[%s] EINVAL\n", __func__);
+		return 0;
+	}
+
+	n += scnprintf(buf + n, PAGE_SIZE - n, "speed             = %d\n",
+		       gadget->speed);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "is_dualspeed      = %d\n",
+		       gadget->is_dualspeed);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "is_otg            = %d\n",
+		       gadget->is_otg);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "is_a_peripheral   = %d\n",
+		       gadget->is_a_peripheral);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "b_hnp_enable      = %d\n",
+		       gadget->b_hnp_enable);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "a_hnp_support     = %d\n",
+		       gadget->a_hnp_support);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "a_alt_hnp_support = %d\n",
+		       gadget->a_alt_hnp_support);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "name              = %s\n",
+		       (gadget->name ? gadget->name : ""));
+
+	return n;
+}
+static DEVICE_ATTR(device, S_IRUSR, show_device, NULL);
+
+/**
+ * show_driver: prints information about attached gadget (if any)
+ *
+ * Check "device.h" for details
+ */
+static ssize_t show_driver(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	struct usb_gadget_driver *driver = udc->driver;
+	int n = 0;
+
+	dbg_trace("[%s] %p\n", __func__, buf);
+	if (attr == NULL || buf == NULL) {
+		dev_err(dev, "[%s] EINVAL\n", __func__);
+		return 0;
+	}
+
+	if (driver == NULL)
+		return scnprintf(buf, PAGE_SIZE,
+				 "There is no gadget attached!\n");
+
+	n += scnprintf(buf + n, PAGE_SIZE - n, "function  = %s\n",
+		       (driver->function ? driver->function : ""));
+	n += scnprintf(buf + n, PAGE_SIZE - n, "max speed = %d\n",
+		       driver->speed);
+
+	return n;
+}
+static DEVICE_ATTR(driver, S_IRUSR, show_driver, NULL);
+
+/* Maximum event message length */
+#define DBG_DATA_MSG   64UL
+
+/* Maximum event messages */
+#define DBG_DATA_MAX   128UL
+
+/* Event buffer descriptor */
+static struct {
+	char     (buf[DBG_DATA_MAX])[DBG_DATA_MSG];   /* buffer */
+	unsigned idx;   /* index */
+	unsigned tty;   /* print to console? */
+	rwlock_t lck;   /* lock */
+} dbg_data = {
+	.idx = 0,
+	.tty = 0,
+	.lck = __RW_LOCK_UNLOCKED(lck)
+};
+
+/**
+ * dbg_dec: decrements debug event index
+ * @idx: buffer index
+ */
+static void dbg_dec(unsigned *idx)
+{
+	*idx = (*idx - 1) & (DBG_DATA_MAX-1);
+}
+
+/**
+ * dbg_inc: increments debug event index
+ * @idx: buffer index
+ */
+static void dbg_inc(unsigned *idx)
+{
+	*idx = (*idx + 1) & (DBG_DATA_MAX-1);
+}
+
+/**
+ * dbg_print:  prints the common part of the event
+ * @addr:   endpoint address
+ * @name:   event name
+ * @status: status
+ * @extra:  extra information
+ */
+static void dbg_print(u8 addr, const char *name, int status, const char *extra)
+{
+	struct timeval tval;
+	unsigned int stamp;
+	unsigned long flags;
+
+	write_lock_irqsave(&dbg_data.lck, flags);
+
+	do_gettimeofday(&tval);
+	stamp = tval.tv_sec & 0xFFFF;	/* 2^32 = 4294967296. Limit to 4096s */
+	stamp = stamp * 1000000 + tval.tv_usec;
+
+	scnprintf(dbg_data.buf[dbg_data.idx], DBG_DATA_MSG,
+		  "%04X\t» %02X %-7.7s %4i «\t%s\n",
+		  stamp, addr, name, status, extra);
+
+	dbg_inc(&dbg_data.idx);
+
+	write_unlock_irqrestore(&dbg_data.lck, flags);
+
+	if (dbg_data.tty != 0)
+		pr_notice("%04X\t» %02X %-7.7s %4i «\t%s\n",
+			  stamp, addr, name, status, extra);
+}
+
+/**
+ * dbg_done: prints a DONE event
+ * @addr:   endpoint address
+ * @td:     transfer descriptor
+ * @status: status
+ */
+static void dbg_done(u8 addr, const u32 token, int status)
+{
+	char msg[DBG_DATA_MSG];
+
+	scnprintf(msg, sizeof(msg), "%d %02X",
+		  (int)(token & TD_TOTAL_BYTES) >> ffs_nr(TD_TOTAL_BYTES),
+		  (int)(token & TD_STATUS)      >> ffs_nr(TD_STATUS));
+	dbg_print(addr, "DONE", status, msg);
+}
+
+/**
+ * dbg_event: prints a generic event
+ * @addr:   endpoint address
+ * @name:   event name
+ * @status: status
+ */
+static void dbg_event(u8 addr, const char *name, int status)
+{
+	if (name != NULL)
+		dbg_print(addr, name, status, "");
+}
+
+/*
+ * dbg_queue: prints a QUEUE event
+ * @addr:   endpoint address
+ * @req:    USB request
+ * @status: status
+ */
+static void dbg_queue(u8 addr, const struct usb_request *req, int status)
+{
+	char msg[DBG_DATA_MSG];
+
+	if (req != NULL) {
+		scnprintf(msg, sizeof(msg),
+			  "%d %d", !req->no_interrupt, req->length);
+		dbg_print(addr, "QUEUE", status, msg);
+	}
+}
+
+/**
+ * dbg_setup: prints a SETUP event
+ * @addr: endpoint address
+ * @req:  setup request
+ */
+static void dbg_setup(u8 addr, const struct usb_ctrlrequest *req)
+{
+	char msg[DBG_DATA_MSG];
+
+	if (req != NULL) {
+		scnprintf(msg, sizeof(msg),
+			  "%02X %02X %04X %04X %d", req->bRequestType,
+			  req->bRequest, le16_to_cpu(req->wValue),
+			  le16_to_cpu(req->wIndex), le16_to_cpu(req->wLength));
+		dbg_print(addr, "SETUP", 0, msg);
+	}
+}
+
+/**
+ * show_events: displays the event buffer
+ *
+ * Check "device.h" for details
+ */
+static ssize_t show_events(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	unsigned long flags;
+	unsigned i, j, n = 0;
+
+	dbg_trace("[%s] %p\n", __func__, buf);
+	if (attr == NULL || buf == NULL) {
+		dev_err(dev, "[%s] EINVAL\n", __func__);
+		return 0;
+	}
+
+	read_lock_irqsave(&dbg_data.lck, flags);
+
+	i = dbg_data.idx;
+	for (dbg_dec(&i); i != dbg_data.idx; dbg_dec(&i)) {
+		n += strlen(dbg_data.buf[i]);
+		if (n >= PAGE_SIZE) {
+			n -= strlen(dbg_data.buf[i]);
+			break;
+		}
+	}
+	for (j = 0, dbg_inc(&i); j < n; dbg_inc(&i))
+		j += scnprintf(buf + j, PAGE_SIZE - j,
+			       "%s", dbg_data.buf[i]);
+
+	read_unlock_irqrestore(&dbg_data.lck, flags);
+
+	return n;
+}
+
+/**
+ * store_events: configure if events are going to be also printed to console
+ *
+ * Check "device.h" for details
+ */
+static ssize_t store_events(struct device *dev, struct device_attribute *attr,
+			    const char *buf, size_t count)
+{
+	unsigned tty;
+
+	dbg_trace("[%s] %p, %d\n", __func__, buf, count);
+	if (attr == NULL || buf == NULL) {
+		dev_err(dev, "[%s] EINVAL\n", __func__);
+		goto done;
+	}
+
+	if (sscanf(buf, "%u", &tty) != 1 || tty > 1) {
+		dev_err(dev, "<1|0>: enable|disable console log\n");
+		goto done;
+	}
+
+	dbg_data.tty = tty;
+	dev_info(dev, "tty = %u", dbg_data.tty);
+
+ done:
+	return count;
+}
+static DEVICE_ATTR(events, S_IRUSR | S_IWUSR, show_events, store_events);
+
+/**
+ * show_inters: interrupt status, enable status and historic
+ *
+ * Check "device.h" for details
+ */
+static ssize_t show_inters(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long flags;
+	u32 intr;
+	unsigned i, j, n = 0;
+
+	dbg_trace("[%s] %p\n", __func__, buf);
+	if (attr == NULL || buf == NULL) {
+		dev_err(dev, "[%s] EINVAL\n", __func__);
+		return 0;
+	}
+
+	spin_lock_irqsave(udc->lock, flags);
+
+	n += scnprintf(buf + n, PAGE_SIZE - n,
+		       "status = %08x\n", hw_read_intr_status());
+	n += scnprintf(buf + n, PAGE_SIZE - n,
+		       "enable = %08x\n", hw_read_intr_enable());
+
+	n += scnprintf(buf + n, PAGE_SIZE - n, "*test = %d\n",
+		       isr_statistics.test);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "» ui  = %d\n",
+		       isr_statistics.ui);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "» uei = %d\n",
+		       isr_statistics.uei);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "» pci = %d\n",
+		       isr_statistics.pci);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "» uri = %d\n",
+		       isr_statistics.uri);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "» sli = %d\n",
+		       isr_statistics.sli);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "*none = %d\n",
+		       isr_statistics.none);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "*hndl = %d\n",
+		       isr_statistics.hndl.cnt);
+
+	for (i = isr_statistics.hndl.idx, j = 0; j <= ISR_MASK; j++, i++) {
+		i   &= ISR_MASK;
+		intr = isr_statistics.hndl.buf[i];
+
+		if (USBi_UI  & intr)
+			n += scnprintf(buf + n, PAGE_SIZE - n, "ui  ");
+		intr &= ~USBi_UI;
+		if (USBi_UEI & intr)
+			n += scnprintf(buf + n, PAGE_SIZE - n, "uei ");
+		intr &= ~USBi_UEI;
+		if (USBi_PCI & intr)
+			n += scnprintf(buf + n, PAGE_SIZE - n, "pci ");
+		intr &= ~USBi_PCI;
+		if (USBi_URI & intr)
+			n += scnprintf(buf + n, PAGE_SIZE - n, "uri ");
+		intr &= ~USBi_URI;
+		if (USBi_SLI & intr)
+			n += scnprintf(buf + n, PAGE_SIZE - n, "sli ");
+		intr &= ~USBi_SLI;
+		if (intr)
+			n += scnprintf(buf + n, PAGE_SIZE - n, "??? ");
+		if (isr_statistics.hndl.buf[i])
+			n += scnprintf(buf + n, PAGE_SIZE - n, "\n");
+	}
+
+	spin_unlock_irqrestore(udc->lock, flags);
+
+	return n;
+}
+
+/**
+ * store_inters: enable & force or disable an individual interrutps
+ *                   (to be used for test purposes only)
+ *
+ * Check "device.h" for details
+ */
+static ssize_t store_inters(struct device *dev, struct device_attribute *attr,
+			    const char *buf, size_t count)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long flags;
+	unsigned en, bit;
+
+	dbg_trace("[%s] %p, %d\n", __func__, buf, count);
+	if (attr == NULL || buf == NULL) {
+		dev_err(dev, "[%s] EINVAL\n", __func__);
+		goto done;
+	}
+
+	if (sscanf(buf, "%u %u", &en, &bit) != 2 || en > 1) {
+		dev_err(dev, "<1|0> <bit>: enable|disable interrupt");
+		goto done;
+	}
+
+	spin_lock_irqsave(udc->lock, flags);
+	if (en) {
+		if (hw_intr_force(bit))
+			dev_err(dev, "invalid bit number\n");
+		else
+			isr_statistics.test++;
+	} else {
+		if (hw_intr_clear(bit))
+			dev_err(dev, "invalid bit number\n");
+	}
+	spin_unlock_irqrestore(udc->lock, flags);
+
+ done:
+	return count;
+}
+static DEVICE_ATTR(inters, S_IRUSR | S_IWUSR, show_inters, store_inters);
+
+/**
+ * show_port_test: reads port test mode
+ *
+ * Check "device.h" for details
+ */
+static ssize_t show_port_test(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long flags;
+	unsigned mode;
+
+	dbg_trace("[%s] %p\n", __func__, buf);
+	if (attr == NULL || buf == NULL) {
+		dev_err(dev, "[%s] EINVAL\n", __func__);
+		return 0;
+	}
+
+	spin_lock_irqsave(udc->lock, flags);
+	mode = hw_port_test_get();
+	spin_unlock_irqrestore(udc->lock, flags);
+
+	return scnprintf(buf, PAGE_SIZE, "mode = %u\n", mode);
+}
+
+/**
+ * store_port_test: writes port test mode
+ *
+ * Check "device.h" for details
+ */
+static ssize_t store_port_test(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long flags;
+	unsigned mode;
+
+	dbg_trace("[%s] %p, %d\n", __func__, buf, count);
+	if (attr == NULL || buf == NULL) {
+		dev_err(dev, "[%s] EINVAL\n", __func__);
+		goto done;
+	}
+
+	if (sscanf(buf, "%u", &mode) != 1) {
+		dev_err(dev, "<mode>: set port test mode");
+		goto done;
+	}
+
+	spin_lock_irqsave(udc->lock, flags);
+	if (hw_port_test_set(mode))
+		dev_err(dev, "invalid mode\n");
+	spin_unlock_irqrestore(udc->lock, flags);
+
+ done:
+	return count;
+}
+static DEVICE_ATTR(port_test, S_IRUSR | S_IWUSR,
+		   show_port_test, store_port_test);
+
+/**
+ * show_qheads: DMA contents of all queue heads
+ *
+ * Check "device.h" for details
+ */
+static ssize_t show_qheads(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long flags;
+	unsigned i, j, n = 0;
+
+	dbg_trace("[%s] %p\n", __func__, buf);
+	if (attr == NULL || buf == NULL) {
+		dev_err(dev, "[%s] EINVAL\n", __func__);
+		return 0;
+	}
+
+	spin_lock_irqsave(udc->lock, flags);
+	for (i = 0; i < hw_ep_max; i++) {
+		struct ci13xxx_ep *mEp = &udc->ci13xxx_ep[i];
+		n += scnprintf(buf + n, PAGE_SIZE - n,
+			       "EP=%02i: RX=%08X TX=%08X\n",
+			       i, (u32)mEp->qh[RX].dma, (u32)mEp->qh[TX].dma);
+		for (j = 0; j < (sizeof(struct ci13xxx_qh)/sizeof(u32)); j++) {
+			n += scnprintf(buf + n, PAGE_SIZE - n,
+				       " %04X:    %08X    %08X\n", j,
+				       *((u32 *)mEp->qh[RX].ptr + j),
+				       *((u32 *)mEp->qh[TX].ptr + j));
+		}
+	}
+	spin_unlock_irqrestore(udc->lock, flags);
+
+	return n;
+}
+static DEVICE_ATTR(qheads, S_IRUSR, show_qheads, NULL);
+
+/**
+ * show_registers: dumps all registers
+ *
+ * Check "device.h" for details
+ */
+static ssize_t show_registers(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long flags;
+	u32 dump[512];
+	unsigned i, k, n = 0;
+
+	dbg_trace("[%s] %p\n", __func__, buf);
+	if (attr == NULL || buf == NULL) {
+		dev_err(dev, "[%s] EINVAL\n", __func__);
+		return 0;
+	}
+
+	spin_lock_irqsave(udc->lock, flags);
+	k = hw_register_read(dump, sizeof(dump)/sizeof(u32));
+	spin_unlock_irqrestore(udc->lock, flags);
+
+	for (i = 0; i < k; i++) {
+		n += scnprintf(buf + n, PAGE_SIZE - n,
+			       "reg[0x%04X] = 0x%08X\n",
+			       i * (unsigned)sizeof(u32), dump[i]);
+	}
+
+	return n;
+}
+
+/**
+ * store_registers: writes value to register address
+ *
+ * Check "device.h" for details
+ */
+static ssize_t store_registers(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long addr, data, flags;
+
+	dbg_trace("[%s] %p, %d\n", __func__, buf, count);
+	if (attr == NULL || buf == NULL) {
+		dev_err(dev, "[%s] EINVAL\n", __func__);
+		goto done;
+	}
+
+	if (sscanf(buf, "%li %li", &addr, &data) != 2) {
+		dev_err(dev, "<addr> <data>: write data to register address");
+		goto done;
+	}
+
+	spin_lock_irqsave(udc->lock, flags);
+	if (hw_register_write(addr, data))
+		dev_err(dev, "invalid address range\n");
+	spin_unlock_irqrestore(udc->lock, flags);
+
+ done:
+	return count;
+}
+static DEVICE_ATTR(registers, S_IRUSR | S_IWUSR,
+		   show_registers, store_registers);
+
+/**
+ * show_requests: DMA contents of all requests currently queued (all endpts)
+ *
+ * Check "device.h" for details
+ */
+static ssize_t show_requests(struct device *dev, struct device_attribute *attr,
+			     char *buf)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long flags;
+	struct list_head   *ptr = NULL;
+	struct ci13xxx_req *req = NULL;
+	unsigned i, j, k, n = 0, qSize = sizeof(struct ci13xxx_td)/sizeof(u32);
+
+	dbg_trace("[%s] %p\n", __func__, buf);
+	if (attr == NULL || buf == NULL) {
+		dev_err(dev, "[%s] EINVAL\n", __func__);
+		return 0;
+	}
+
+	spin_lock_irqsave(udc->lock, flags);
+	for (i = 0; i < hw_ep_max; i++)
+		for (k = RX; k <= TX; k++)
+			list_for_each(ptr, &udc->ci13xxx_ep[i].qh[k].queue)
+			{
+				req = list_entry(ptr,
+						 struct ci13xxx_req, queue);
+
+				n += scnprintf(buf + n, PAGE_SIZE - n,
+					       "EP=%02i: TD=%08X %s\n",
+					       i, (u32)req->dma,
+					       ((k == RX) ? "RX" : "TX"));
+
+				for (j = 0; j < qSize; j++)
+					n += scnprintf(buf + n, PAGE_SIZE - n,
+						       " %04X:    %08X\n", j,
+						       *((u32 *)req->ptr + j));
+			}
+	spin_unlock_irqrestore(udc->lock, flags);
+
+	return n;
+}
+static DEVICE_ATTR(requests, S_IRUSR, show_requests, NULL);
+
+/**
+ * dbg_create_files: initializes the attribute interface
+ * @dev: device
+ *
+ * This function returns an error code
+ */
+__maybe_unused static int dbg_create_files(struct device *dev)
+{
+	int retval = 0;
+
+	if (dev == NULL)
+		return -EINVAL;
+	retval = device_create_file(dev, &dev_attr_device);
+	if (retval)
+		goto done;
+	retval = device_create_file(dev, &dev_attr_driver);
+	if (retval)
+		goto rm_device;
+	retval = device_create_file(dev, &dev_attr_events);
+	if (retval)
+		goto rm_driver;
+	retval = device_create_file(dev, &dev_attr_inters);
+	if (retval)
+		goto rm_events;
+	retval = device_create_file(dev, &dev_attr_port_test);
+	if (retval)
+		goto rm_inters;
+	retval = device_create_file(dev, &dev_attr_qheads);
+	if (retval)
+		goto rm_port_test;
+	retval = device_create_file(dev, &dev_attr_registers);
+	if (retval)
+		goto rm_qheads;
+	retval = device_create_file(dev, &dev_attr_requests);
+	if (retval)
+		goto rm_registers;
+	return 0;
+
+ rm_registers:
+	device_remove_file(dev, &dev_attr_registers);
+ rm_qheads:
+	device_remove_file(dev, &dev_attr_qheads);
+ rm_port_test:
+	device_remove_file(dev, &dev_attr_port_test);
+ rm_inters:
+	device_remove_file(dev, &dev_attr_inters);
+ rm_events:
+	device_remove_file(dev, &dev_attr_events);
+ rm_driver:
+	device_remove_file(dev, &dev_attr_driver);
+ rm_device:
+	device_remove_file(dev, &dev_attr_device);
+ done:
+	return retval;
+}
+
+/**
+ * dbg_remove_files: destroys the attribute interface
+ * @dev: device
+ *
+ * This function returns an error code
+ */
+__maybe_unused static int dbg_remove_files(struct device *dev)
+{
+	if (dev == NULL)
+		return -EINVAL;
+	device_remove_file(dev, &dev_attr_requests);
+	device_remove_file(dev, &dev_attr_registers);
+	device_remove_file(dev, &dev_attr_qheads);
+	device_remove_file(dev, &dev_attr_port_test);
+	device_remove_file(dev, &dev_attr_inters);
+	device_remove_file(dev, &dev_attr_events);
+	device_remove_file(dev, &dev_attr_driver);
+	device_remove_file(dev, &dev_attr_device);
+	return 0;
+}
+
+/******************************************************************************
+ * UTIL block
+ *****************************************************************************/
+/**
+ * _usb_addr: calculates endpoint address from direction & number
+ * @ep:  endpoint
+ */
+static inline u8 _usb_addr(struct ci13xxx_ep *ep)
+{
+	return ((ep->dir == TX) ? USB_ENDPOINT_DIR_MASK : 0) | ep->num;
+}
+
+/**
+ * _hardware_queue: configures a request at hardware level
+ * @gadget: gadget
+ * @mEp:    endpoint
+ *
+ * This function returns an error code
+ */
+static int _hardware_enqueue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
+{
+	unsigned i;
+
+	trace("%p, %p", mEp, mReq);
+
+	/* don't queue twice */
+	if (mReq->req.status == -EALREADY)
+		return -EALREADY;
+
+	if (hw_ep_is_primed(mEp->num, mEp->dir))
+		return -EBUSY;
+
+	mReq->req.status = -EALREADY;
+
+	if (mReq->req.length && !mReq->req.dma) {
+		mReq->req.dma = \
+			dma_map_single(mEp->device, mReq->req.buf,
+				       mReq->req.length, mEp->dir ?
+				       DMA_TO_DEVICE : DMA_FROM_DEVICE);
+		if (mReq->req.dma == 0)
+			return -ENOMEM;
+
+		mReq->map = 1;
+	}
+
+	/*
+	 * TD configuration
+	 * TODO - handle requests which spawns into several TDs
+	 */
+	memset(mReq->ptr, 0, sizeof(*mReq->ptr));
+	mReq->ptr->next    |= TD_TERMINATE;
+	mReq->ptr->token    = mReq->req.length << ffs_nr(TD_TOTAL_BYTES);
+	mReq->ptr->token   &= TD_TOTAL_BYTES;
+	mReq->ptr->token   |= TD_IOC;
+	mReq->ptr->token   |= TD_STATUS_ACTIVE;
+	mReq->ptr->page[0]  = mReq->req.dma;
+	for (i = 1; i < 5; i++)
+		mReq->ptr->page[i] =
+			(mReq->req.dma + i * PAGE_SIZE) & ~TD_RESERVED_MASK;
+
+	/*
+	 *  QH configuration
+	 *  At this point it's guaranteed exclusive access to qhead
+	 *  (endpt is not primed) so it's no need to use tripwire
+	 */
+	mEp->qh[mEp->dir].ptr->td.next   = mReq->dma;    /* TERMINATE = 0 */
+	mEp->qh[mEp->dir].ptr->td.token &= ~TD_STATUS;   /* clear status */
+	if (mReq->req.zero == 0)
+		mEp->qh[mEp->dir].ptr->cap |=  QH_ZLT;
+	else
+		mEp->qh[mEp->dir].ptr->cap &= ~QH_ZLT;
+
+	wmb();   /* synchronize before ep prime */
+
+	return hw_ep_prime(mEp->num, mEp->dir,
+			   mEp->type == USB_ENDPOINT_XFER_CONTROL);
+}
+
+/**
+ * _hardware_dequeue: handles a request at hardware level
+ * @gadget: gadget
+ * @mEp:    endpoint
+ *
+ * This function returns an error code
+ */
+static int _hardware_dequeue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
+{
+	trace("%p, %p", mEp, mReq);
+
+	if (mReq->req.status != -EALREADY)
+		return -EINVAL;
+
+	if (hw_ep_is_primed(mEp->num, mEp->dir))
+		hw_ep_flush(mEp->num, mEp->dir);
+
+	mReq->req.status = 0;
+
+	if (mReq->map) {
+		dma_unmap_single(mEp->device, mReq->req.dma, mReq->req.length,
+				 mEp->dir ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
+		mReq->req.dma = 0;
+		mReq->map     = 0;
+	}
+
+	mReq->req.status = mReq->ptr->token & TD_STATUS;
+	if      ((TD_STATUS_ACTIVE & mReq->req.status) != 0)
+		mReq->req.status = -ECONNRESET;
+	else if ((TD_STATUS_HALTED & mReq->req.status) != 0)
+		mReq->req.status = -1;
+	else if ((TD_STATUS_DT_ERR & mReq->req.status) != 0)
+		mReq->req.status = -1;
+	else if ((TD_STATUS_TR_ERR & mReq->req.status) != 0)
+		mReq->req.status = -1;
+
+	mReq->req.actual   = mReq->ptr->token & TD_TOTAL_BYTES;
+	mReq->req.actual >>= ffs_nr(TD_TOTAL_BYTES);
+	mReq->req.actual   = mReq->req.length - mReq->req.actual;
+	mReq->req.actual   = mReq->req.status ? 0 : mReq->req.actual;
+
+	return mReq->req.actual;
+}
+
+/**
+ * _ep_nuke: dequeues all endpoint requests
+ * @mEp: endpoint
+ *
+ * This function returns an error code
+ * Caller must hold lock
+ */
+static int _ep_nuke(struct ci13xxx_ep *mEp)
+__releases(mEp->lock)
+__acquires(mEp->lock)
+{
+	trace("%p", mEp);
+
+	if (mEp == NULL)
+		return -EINVAL;
+
+	hw_ep_flush(mEp->num, mEp->dir);
+
+	while (!list_empty(&mEp->qh[mEp->dir].queue)) {
+
+		/* pop oldest request */
+		struct ci13xxx_req *mReq = \
+			list_entry(mEp->qh[mEp->dir].queue.next,
+				   struct ci13xxx_req, queue);
+		list_del_init(&mReq->queue);
+		mReq->req.status = -ESHUTDOWN;
+
+		if (!mReq->req.no_interrupt && mReq->req.complete != NULL) {
+			spin_unlock(mEp->lock);
+			mReq->req.complete(&mEp->ep, &mReq->req);
+			spin_lock(mEp->lock);
+		}
+	}
+	return 0;
+}
+
+/**
+ * _gadget_stop_activity: stops all USB activity, flushes & disables all endpts
+ * @gadget: gadget
+ *
+ * This function returns an error code
+ * Caller must hold lock
+ */
+static int _gadget_stop_activity(struct usb_gadget *gadget)
+__releases(udc->lock)
+__acquires(udc->lock)
+{
+	struct usb_ep *ep;
+	struct ci13xxx    *udc = container_of(gadget, struct ci13xxx, gadget);
+	struct ci13xxx_ep *mEp = container_of(gadget->ep0,
+					      struct ci13xxx_ep, ep);
+
+	trace("%p", gadget);
+
+	if (gadget == NULL)
+		return -EINVAL;
+
+	spin_unlock(udc->lock);
+
+	/* flush all endpoints */
+	gadget_for_each_ep(ep, gadget) {
+		usb_ep_fifo_flush(ep);
+	}
+	usb_ep_fifo_flush(gadget->ep0);
+
+	udc->driver->disconnect(gadget);
+
+	/* make sure to disable all endpoints */
+	gadget_for_each_ep(ep, gadget) {
+		usb_ep_disable(ep);
+	}
+	usb_ep_disable(gadget->ep0);
+
+	if (mEp->status != NULL) {
+		usb_ep_free_request(gadget->ep0, mEp->status);
+		mEp->status = NULL;
+	}
+
+	spin_lock(udc->lock);
+
+	return 0;
+}
+
+/******************************************************************************
+ * ISR block
+ *****************************************************************************/
+/**
+ * isr_reset_handler: USB reset interrupt handler
+ * @udc: UDC device
+ *
+ * This function resets USB engine after a bus reset occurred
+ */
+static void isr_reset_handler(struct ci13xxx *udc)
+__releases(udc->lock)
+__acquires(udc->lock)
+{
+	struct ci13xxx_ep *mEp = &udc->ci13xxx_ep[0];
+	int retval;
+
+	trace("%p", udc);
+
+	if (udc == NULL) {
+		err("EINVAL");
+		return;
+	}
+
+	dbg_event(0xFF, "BUS RST", 0);
+
+	retval = _gadget_stop_activity(&udc->gadget);
+	if (retval)
+		goto done;
+
+	retval = hw_usb_reset();
+	if (retval)
+		goto done;
+
+	spin_unlock(udc->lock);
+	retval = usb_ep_enable(&mEp->ep, &ctrl_endpt_desc);
+	if (!retval) {
+		mEp->status = usb_ep_alloc_request(&mEp->ep, GFP_KERNEL);
+		if (mEp->status == NULL) {
+			usb_ep_disable(&mEp->ep);
+			retval = -ENOMEM;
+		}
+	}
+	spin_lock(udc->lock);
+
+ done:
+	if (retval)
+		err("error: %i", retval);
+}
+
+/**
+ * isr_get_status_complete: get_status request complete function
+ * @ep:  endpoint
+ * @req: request handled
+ *
+ * Caller must release lock
+ */
+static void isr_get_status_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	trace("%p, %p", ep, req);
+
+	if (ep == NULL || req == NULL) {
+		err("EINVAL");
+		return;
+	}
+
+	kfree(req->buf);
+	usb_ep_free_request(ep, req);
+}
+
+/**
+ * isr_get_status_response: get_status request response
+ * @ep:    endpoint
+ * @setup: setup request packet
+ *
+ * This function returns an error code
+ */
+static int isr_get_status_response(struct ci13xxx_ep *mEp,
+				   struct usb_ctrlrequest *setup)
+__releases(mEp->lock)
+__acquires(mEp->lock)
+{
+	struct usb_request *req = NULL;
+	gfp_t gfp_flags = GFP_ATOMIC;
+	int dir, num, retval;
+
+	trace("%p, %p", mEp, setup);
+
+	if (mEp == NULL || setup == NULL)
+		return -EINVAL;
+
+	spin_unlock(mEp->lock);
+	req = usb_ep_alloc_request(&mEp->ep, gfp_flags);
+	spin_lock(mEp->lock);
+	if (req == NULL)
+		return -ENOMEM;
+
+	req->complete = isr_get_status_complete;
+	req->length   = 2;
+	req->buf      = kzalloc(req->length, gfp_flags);
+	if (req->buf == NULL) {
+		retval = -ENOMEM;
+		goto err_free_req;
+	}
+
+	if ((setup->bRequestType & USB_RECIP_MASK) == USB_RECIP_DEVICE) {
+		/* TODO: D1 - Remote Wakeup; D0 - Self Powered */
+		retval = 0;
+	} else if ((setup->bRequestType & USB_RECIP_MASK) \
+		   == USB_RECIP_ENDPOINT) {
+		dir = (le16_to_cpu(setup->wIndex) & USB_ENDPOINT_DIR_MASK) ?
+			TX : RX;
+		num =  le16_to_cpu(setup->wIndex) & USB_ENDPOINT_NUMBER_MASK;
+		*((u16 *)req->buf) = hw_ep_get_halt(num, dir);
+	}
+	/* else do nothing; reserved for future use */
+
+	spin_unlock(mEp->lock);
+	retval = usb_ep_queue(&mEp->ep, req, gfp_flags);
+	spin_lock(mEp->lock);
+	if (retval)
+		goto err_free_buf;
+
+	return 0;
+
+ err_free_buf:
+	kfree(req->buf);
+ err_free_req:
+	spin_unlock(mEp->lock);
+	usb_ep_free_request(&mEp->ep, req);
+	spin_lock(mEp->lock);
+	return retval;
+}
+
+/**
+ * isr_setup_status_phase: queues the status phase of a setup transation
+ * @mEp: endpoint
+ *
+ * This function returns an error code
+ */
+static int isr_setup_status_phase(struct ci13xxx_ep *mEp)
+__releases(mEp->lock)
+__acquires(mEp->lock)
+{
+	int retval;
+
+	trace("%p", mEp);
+
+	/* mEp is always valid & configured */
+
+	if (mEp->type == USB_ENDPOINT_XFER_CONTROL)
+		mEp->dir = (mEp->dir == TX) ? RX : TX;
+
+	mEp->status->no_interrupt = 1;
+
+	spin_unlock(mEp->lock);
+	retval = usb_ep_queue(&mEp->ep, mEp->status, GFP_ATOMIC);
+	spin_lock(mEp->lock);
+
+	return retval;
+}
+
+/**
+ * isr_tr_complete_low: transaction complete low level handler
+ * @mEp: endpoint
+ *
+ * This function returns an error code
+ * Caller must hold lock
+ */
+static int isr_tr_complete_low(struct ci13xxx_ep *mEp)
+__releases(mEp->lock)
+__acquires(mEp->lock)
+{
+	struct ci13xxx_req *mReq;
+	int retval;
+
+	trace("%p", mEp);
+
+	if (list_empty(&mEp->qh[mEp->dir].queue))
+		return -EINVAL;
+
+	/* pop oldest request */
+	mReq = list_entry(mEp->qh[mEp->dir].queue.next,
+			  struct ci13xxx_req, queue);
+	list_del_init(&mReq->queue);
+
+	retval = _hardware_dequeue(mEp, mReq);
+	if (retval < 0) {
+		dbg_event(_usb_addr(mEp), "DONE", retval);
+		goto done;
+	}
+
+	dbg_done(_usb_addr(mEp), mReq->ptr->token, retval);
+
+	if (!mReq->req.no_interrupt && mReq->req.complete != NULL) {
+		spin_unlock(mEp->lock);
+		mReq->req.complete(&mEp->ep, &mReq->req);
+		spin_lock(mEp->lock);
+	}
+
+	if (!list_empty(&mEp->qh[mEp->dir].queue)) {
+		mReq = list_entry(mEp->qh[mEp->dir].queue.next,
+				  struct ci13xxx_req, queue);
+		_hardware_enqueue(mEp, mReq);
+	}
+
+ done:
+	return retval;
+}
+
+/**
+ * isr_tr_complete_handler: transaction complete interrupt handler
+ * @udc: UDC descriptor
+ *
+ * This function handles traffic events
+ */
+static void isr_tr_complete_handler(struct ci13xxx *udc)
+__releases(udc->lock)
+__acquires(udc->lock)
+{
+	unsigned i;
+
+	trace("%p", udc);
+
+	if (udc == NULL) {
+		err("EINVAL");
+		return;
+	}
+
+	for (i = 0; i < hw_ep_max; i++) {
+		struct ci13xxx_ep *mEp  = &udc->ci13xxx_ep[i];
+		int type, num, err = -EINVAL;
+		struct usb_ctrlrequest req;
+
+
+		if (mEp->desc == NULL)
+			continue;   /* not configured */
+
+		if ((mEp->dir == RX && hw_test_and_clear_complete(i)) ||
+		    (mEp->dir == TX && hw_test_and_clear_complete(i + 16))) {
+			err = isr_tr_complete_low(mEp);
+			if (mEp->type == USB_ENDPOINT_XFER_CONTROL) {
+				if (err > 0)   /* needs status phase */
+					err = isr_setup_status_phase(mEp);
+				if (err < 0) {
+					dbg_event(_usb_addr(mEp),
+						  "ERROR", err);
+					spin_unlock(udc->lock);
+					if (usb_ep_set_halt(&mEp->ep))
+						err("error: ep_set_halt");
+					spin_lock(udc->lock);
+				}
+			}
+		}
+
+		if (mEp->type != USB_ENDPOINT_XFER_CONTROL ||
+		    !hw_test_and_clear_setup_status(i))
+			continue;
+
+		if (i != 0) {
+			warn("ctrl traffic received at endpoint");
+			continue;
+		}
+
+		/* read_setup_packet */
+		do {
+			hw_test_and_set_setup_guard();
+			memcpy(&req, &mEp->qh[RX].ptr->setup, sizeof(req));
+		} while (!hw_test_and_clear_setup_guard());
+
+		type = req.bRequestType;
+
+		mEp->dir = (type & USB_DIR_IN) ? TX : RX;
+
+		dbg_setup(_usb_addr(mEp), &req);
+
+		switch (req.bRequest) {
+		case USB_REQ_CLEAR_FEATURE:
+			if (type != (USB_DIR_OUT|USB_RECIP_ENDPOINT) &&
+			    le16_to_cpu(req.wValue) != USB_ENDPOINT_HALT)
+				goto delegate;
+			if (req.wLength != 0)
+				break;
+			num  = le16_to_cpu(req.wIndex);
+			num &= USB_ENDPOINT_NUMBER_MASK;
+			if (!udc->ci13xxx_ep[num].wedge) {
+				spin_unlock(udc->lock);
+				err = usb_ep_clear_halt(
+					&udc->ci13xxx_ep[num].ep);
+				spin_lock(udc->lock);
+				if (err)
+					break;
+			}
+			err = isr_setup_status_phase(mEp);
+			break;
+		case USB_REQ_GET_STATUS:
+			if (type != (USB_DIR_IN|USB_RECIP_DEVICE)   &&
+			    type != (USB_DIR_IN|USB_RECIP_ENDPOINT) &&
+			    type != (USB_DIR_IN|USB_RECIP_INTERFACE))
+				goto delegate;
+			if (le16_to_cpu(req.wLength) != 2 ||
+			    le16_to_cpu(req.wValue)  != 0)
+				break;
+			err = isr_get_status_response(mEp, &req);
+			break;
+		case USB_REQ_SET_ADDRESS:
+			if (type != (USB_DIR_OUT|USB_RECIP_DEVICE))
+				goto delegate;
+			if (le16_to_cpu(req.wLength) != 0 ||
+			    le16_to_cpu(req.wIndex)  != 0)
+				break;
+			err = hw_usb_set_address((u8)le16_to_cpu(req.wValue));
+			if (err)
+				break;
+			err = isr_setup_status_phase(mEp);
+			break;
+		case USB_REQ_SET_FEATURE:
+			if (type != (USB_DIR_OUT|USB_RECIP_ENDPOINT) &&
+			    le16_to_cpu(req.wValue) != USB_ENDPOINT_HALT)
+				goto delegate;
+			if (req.wLength != 0)
+				break;
+			num  = le16_to_cpu(req.wIndex);
+			num &= USB_ENDPOINT_NUMBER_MASK;
+
+			spin_unlock(udc->lock);
+			err = usb_ep_set_halt(&udc->ci13xxx_ep[num].ep);
+			spin_lock(udc->lock);
+			if (err)
+				break;
+			err = isr_setup_status_phase(mEp);
+			break;
+		default:
+delegate:
+			if (req.wLength == 0)   /* no data phase */
+				mEp->dir = TX;
+
+			spin_unlock(udc->lock);
+			err = udc->driver->setup(&udc->gadget, &req);
+			spin_lock(udc->lock);
+			break;
+		}
+
+		if (err < 0) {
+			dbg_event(_usb_addr(mEp), "ERROR", err);
+
+			spin_unlock(udc->lock);
+			if (usb_ep_set_halt(&mEp->ep))
+				err("error: ep_set_halt");
+			spin_lock(udc->lock);
+		}
+	}
+}
+
+/******************************************************************************
+ * ENDPT block
+ *****************************************************************************/
+/**
+ * ep_enable: configure endpoint, making it usable
+ *
+ * Check usb_ep_enable() at "usb_gadget.h" for details
+ */
+static int ep_enable(struct usb_ep *ep,
+		     const struct usb_endpoint_descriptor *desc)
+{
+	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
+	int direction, retval = 0;
+	unsigned long flags;
+
+	trace("%p, %p", ep, desc);
+
+	if (ep == NULL || desc == NULL)
+		return -EINVAL;
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+	/* only internal SW should enable ctrl endpts */
+
+	mEp->desc = desc;
+
+	if (!list_empty(&mEp->qh[mEp->dir].queue))
+		warn("enabling a non-empty endpoint!");
+
+	mEp->dir  = (desc->bEndpointAddress & USB_ENDPOINT_DIR_MASK) ? TX : RX;
+	mEp->num  =  desc->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK;
+	mEp->type =  desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK;
+
+	mEp->ep.maxpacket = __constant_le16_to_cpu(desc->wMaxPacketSize);
+
+	direction = mEp->dir;
+	do {
+		dbg_event(_usb_addr(mEp), "ENABLE", 0);
+
+		if (mEp->type == USB_ENDPOINT_XFER_CONTROL)
+			mEp->qh[mEp->dir].ptr->cap |=  QH_IOS;
+		else if (mEp->type == USB_ENDPOINT_XFER_ISOC)
+			mEp->qh[mEp->dir].ptr->cap &= ~QH_MULT;
+		else
+			mEp->qh[mEp->dir].ptr->cap &= ~QH_ZLT;
+
+		mEp->qh[mEp->dir].ptr->cap |=
+			(mEp->ep.maxpacket << ffs_nr(QH_MAX_PKT)) & QH_MAX_PKT;
+		mEp->qh[mEp->dir].ptr->td.next |= TD_TERMINATE;   /* needed? */
+
+		retval |= hw_ep_enable(mEp->num, mEp->dir, mEp->type);
+
+		if (mEp->type == USB_ENDPOINT_XFER_CONTROL)
+			mEp->dir = (mEp->dir == TX) ? RX : TX;
+
+	} while (mEp->dir != direction);
+
+	spin_unlock_irqrestore(mEp->lock, flags);
+	return retval;
+}
+
+/**
+ * ep_disable: endpoint is no longer usable
+ *
+ * Check usb_ep_disable() at "usb_gadget.h" for details
+ */
+static int ep_disable(struct usb_ep *ep)
+{
+	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
+	int direction, retval = 0;
+	unsigned long flags;
+
+	trace("%p", ep);
+
+	if (ep == NULL)
+		return -EINVAL;
+	else if (mEp->desc == NULL)
+		return -EBUSY;
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+	/* only internal SW should disable ctrl endpts */
+
+	direction = mEp->dir;
+	do {
+		dbg_event(_usb_addr(mEp), "DISABLE", 0);
+
+		retval |= _ep_nuke(mEp);
+		retval |= hw_ep_disable(mEp->num, mEp->dir);
+
+		if (mEp->type == USB_ENDPOINT_XFER_CONTROL)
+			mEp->dir = (mEp->dir == TX) ? RX : TX;
+
+	} while (mEp->dir != direction);
+
+	mEp->desc = NULL;
+
+	spin_unlock_irqrestore(mEp->lock, flags);
+	return retval;
+}
+
+/**
+ * ep_alloc_request: allocate a request object to use with this endpoint
+ *
+ * Check usb_ep_alloc_request() at "usb_gadget.h" for details
+ */
+static struct usb_request *ep_alloc_request(struct usb_ep *ep, gfp_t gfp_flags)
+{
+	struct ci13xxx_ep  *mEp  = container_of(ep, struct ci13xxx_ep, ep);
+	struct ci13xxx_req *mReq = NULL;
+	unsigned long flags;
+
+	trace("%p, %i", ep, gfp_flags);
+
+	if (ep == NULL) {
+		err("EINVAL");
+		return NULL;
+	}
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+	mReq = kzalloc(sizeof(struct ci13xxx_req), gfp_flags);
+	if (mReq != NULL) {
+		INIT_LIST_HEAD(&mReq->queue);
+
+		mReq->ptr = dma_pool_alloc(mEp->td_pool, gfp_flags,
+					   &mReq->dma);
+		if (mReq->ptr == NULL) {
+			kfree(mReq);
+			mReq = NULL;
+		}
+	}
+
+	dbg_event(_usb_addr(mEp), "ALLOC", mReq == NULL);
+
+	spin_unlock_irqrestore(mEp->lock, flags);
+
+	return (mReq == NULL) ? NULL : &mReq->req;
+}
+
+/**
+ * ep_free_request: frees a request object
+ *
+ * Check usb_ep_free_request() at "usb_gadget.h" for details
+ */
+static void ep_free_request(struct usb_ep *ep, struct usb_request *req)
+{
+	struct ci13xxx_ep  *mEp  = container_of(ep,  struct ci13xxx_ep, ep);
+	struct ci13xxx_req *mReq = container_of(req, struct ci13xxx_req, req);
+	unsigned long flags;
+
+	trace("%p, %p", ep, req);
+
+	if (ep == NULL || req == NULL) {
+		err("EINVAL");
+		return;
+	} else if (!list_empty(&mReq->queue)) {
+		err("EBUSY");
+		return;
+	}
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+	if (mReq->ptr)
+		dma_pool_free(mEp->td_pool, mReq->ptr, mReq->dma);
+	kfree(mReq);
+
+	dbg_event(_usb_addr(mEp), "FREE", 0);
+
+	spin_unlock_irqrestore(mEp->lock, flags);
+}
+
+/**
+ * ep_queue: queues (submits) an I/O request to an endpoint
+ *
+ * Check usb_ep_queue()* at usb_gadget.h" for details
+ */
+static int ep_queue(struct usb_ep *ep, struct usb_request *req,
+		    gfp_t __maybe_unused gfp_flags)
+{
+	struct ci13xxx_ep  *mEp  = container_of(ep,  struct ci13xxx_ep, ep);
+	struct ci13xxx_req *mReq = container_of(req, struct ci13xxx_req, req);
+	int retval = 0;
+	unsigned long flags;
+
+	trace("%p, %p, %X", ep, req, gfp_flags);
+
+	if (ep == NULL || req == NULL || mEp->desc == NULL)
+		return -EINVAL;
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+	if (mEp->type == USB_ENDPOINT_XFER_CONTROL &&
+	    !list_empty(&mEp->qh[mEp->dir].queue)) {
+		_ep_nuke(mEp);
+		retval = -EOVERFLOW;
+		warn("endpoint ctrl %X nuked", _usb_addr(mEp));
+	}
+
+	/* first nuke then test link, e.g. previous status has not sent */
+	if (!list_empty(&mReq->queue)) {
+		retval = -EBUSY;
+		err("request already in queue");
+		goto done;
+	}
+
+	if (req->length > (4 * PAGE_SIZE)) {
+		req->length = (4 * PAGE_SIZE);
+		retval = -EMSGSIZE;
+		warn("request length truncated");
+	}
+
+	dbg_queue(_usb_addr(mEp), req, retval);
+
+	/* push request */
+	mReq->req.status = -EINPROGRESS;
+	mReq->req.actual = 0;
+	list_add_tail(&mReq->queue, &mEp->qh[mEp->dir].queue);
+
+	retval = _hardware_enqueue(mEp, mReq);
+	if (retval == -EALREADY || retval == -EBUSY) {
+		dbg_event(_usb_addr(mEp), "QUEUE", retval);
+		retval = 0;
+	}
+
+ done:
+	spin_unlock_irqrestore(mEp->lock, flags);
+	return retval;
+}
+
+/**
+ * ep_dequeue: dequeues (cancels, unlinks) an I/O request from an endpoint
+ *
+ * Check usb_ep_dequeue() at "usb_gadget.h" for details
+ */
+static int ep_dequeue(struct usb_ep *ep, struct usb_request *req)
+{
+	struct ci13xxx_ep  *mEp  = container_of(ep,  struct ci13xxx_ep, ep);
+	struct ci13xxx_req *mReq = container_of(req, struct ci13xxx_req, req);
+	unsigned long flags;
+
+	trace("%p, %p", ep, req);
+
+	if (ep == NULL || req == NULL || mEp->desc == NULL ||
+	    list_empty(&mReq->queue)  || list_empty(&mEp->qh[mEp->dir].queue))
+		return -EINVAL;
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+	dbg_event(_usb_addr(mEp), "DEQUEUE", 0);
+
+	if (mReq->req.status == -EALREADY)
+		_hardware_dequeue(mEp, mReq);
+
+	/* pop request */
+	list_del_init(&mReq->queue);
+	req->status = -ECONNRESET;
+
+	if (!mReq->req.no_interrupt && mReq->req.complete != NULL) {
+		spin_unlock(mEp->lock);
+		mReq->req.complete(&mEp->ep, &mReq->req);
+		spin_lock(mEp->lock);
+	}
+
+	spin_unlock_irqrestore(mEp->lock, flags);
+	return 0;
+}
+
+/**
+ * ep_set_halt: sets the endpoint halt feature
+ *
+ * Check usb_ep_set_halt() at "usb_gadget.h" for details
+ */
+static int ep_set_halt(struct usb_ep *ep, int value)
+{
+	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
+	int direction, retval = 0;
+	unsigned long flags;
+
+	trace("%p, %i", ep, value);
+
+	if (ep == NULL || mEp->desc == NULL)
+		return -EINVAL;
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+#ifndef STALL_IN
+	/* g_file_storage MS compliant but g_zero fails chapter 9 compliance */
+	if (value && mEp->type == USB_ENDPOINT_XFER_BULK && mEp->dir == TX &&
+	    !list_empty(&mEp->qh[mEp->dir].queue)) {
+		spin_unlock_irqrestore(mEp->lock, flags);
+		return -EAGAIN;
+	}
+#endif
+
+	direction = mEp->dir;
+	do {
+		dbg_event(_usb_addr(mEp), "HALT", value);
+		retval |= hw_ep_set_halt(mEp->num, mEp->dir, value);
+
+		if (!value)
+			mEp->wedge = 0;
+
+		if (mEp->type == USB_ENDPOINT_XFER_CONTROL)
+			mEp->dir = (mEp->dir == TX) ? RX : TX;
+
+	} while (mEp->dir != direction);
+
+	spin_unlock_irqrestore(mEp->lock, flags);
+	return retval;
+}
+
+/**
+ * ep_set_wedge: sets the halt feature and ignores clear requests
+ *
+ * Check usb_ep_set_wedge() at "usb_gadget.h" for details
+ */
+static int ep_set_wedge(struct usb_ep *ep)
+{
+	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
+	unsigned long flags;
+
+	trace("%p", ep);
+
+	if (ep == NULL || mEp->desc == NULL)
+		return -EINVAL;
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+	dbg_event(_usb_addr(mEp), "WEDGE", 0);
+	mEp->wedge = 1;
+
+	spin_unlock_irqrestore(mEp->lock, flags);
+
+	return usb_ep_set_halt(ep);
+}
+
+/**
+ * ep_fifo_flush: flushes contents of a fifo
+ *
+ * Check usb_ep_fifo_flush() at "usb_gadget.h" for details
+ */
+static void ep_fifo_flush(struct usb_ep *ep)
+{
+	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
+	unsigned long flags;
+
+	trace("%p", ep);
+
+	if (ep == NULL) {
+		err("%02X: -EINVAL", _usb_addr(mEp));
+		return;
+	}
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+	dbg_event(_usb_addr(mEp), "FFLUSH", 0);
+	hw_ep_flush(mEp->num, mEp->dir);
+
+	spin_unlock_irqrestore(mEp->lock, flags);
+}
+
+/**
+ * Endpoint-specific part of the API to the USB controller hardware
+ * Check "usb_gadget.h" for details
+ */
+static const struct usb_ep_ops usb_ep_ops = {
+	.enable	       = ep_enable,
+	.disable       = ep_disable,
+	.alloc_request = ep_alloc_request,
+	.free_request  = ep_free_request,
+	.queue	       = ep_queue,
+	.dequeue       = ep_dequeue,
+	.set_halt      = ep_set_halt,
+	.set_wedge     = ep_set_wedge,
+	.fifo_flush    = ep_fifo_flush,
+};
+
+/******************************************************************************
+ * GADGET block
+ *****************************************************************************/
+/**
+ * Device operations part of the API to the USB controller hardware,
+ * which don't involve endpoints (or i/o)
+ * Check  "usb_gadget.h" for details
+ */
+static const struct usb_gadget_ops usb_gadget_ops;
+
+/**
+ * usb_gadget_register_driver: register a gadget driver
+ *
+ * Check usb_gadget_register_driver() at "usb_gadget.h" for details
+ * Interrupts are enabled here
+ */
+int usb_gadget_register_driver(struct usb_gadget_driver *driver)
+{
+	struct ci13xxx *udc = _udc;
+	unsigned long i, k, flags;
+	int retval = -ENOMEM;
+
+	trace("%p", driver);
+
+	if (driver             == NULL ||
+	    driver->bind       == NULL ||
+	    driver->unbind     == NULL ||
+	    driver->setup      == NULL ||
+	    driver->disconnect == NULL ||
+	    driver->suspend    == NULL ||
+	    driver->resume     == NULL)
+		return -EINVAL;
+	else if (udc         == NULL)
+		return -ENODEV;
+	else if (udc->driver != NULL)
+		return -EBUSY;
+
+	/* alloc resources */
+	udc->qh_pool = dma_pool_create("ci13xxx_qh", &udc->gadget.dev,
+				       sizeof(struct ci13xxx_qh),
+				       64, PAGE_SIZE);
+	if (udc->qh_pool == NULL)
+		return -ENOMEM;
+
+	udc->td_pool = dma_pool_create("ci13xxx_td", &udc->gadget.dev,
+				       sizeof(struct ci13xxx_td),
+				       64, PAGE_SIZE);
+	if (udc->td_pool == NULL) {
+		dma_pool_destroy(udc->qh_pool);
+		udc->qh_pool = NULL;
+		return -ENOMEM;
+	}
+
+	spin_lock_irqsave(udc->lock, flags);
+
+	info("hw_ep_max = %d", hw_ep_max);
+
+	udc->driver = driver;
+	udc->gadget.ops        = NULL;
+	udc->gadget.dev.driver = NULL;
+
+	retval = 0;
+	for (i = 0; i < hw_ep_max; i++) {
+		struct ci13xxx_ep *mEp = &udc->ci13xxx_ep[i];
+
+		scnprintf(mEp->name, sizeof(mEp->name), "ep%i", (int)i);
+
+		mEp->lock         = udc->lock;
+		mEp->device       = &udc->gadget.dev;
+		mEp->td_pool      = udc->td_pool;
+
+		mEp->ep.name      = mEp->name;
+		mEp->ep.ops       = &usb_ep_ops;
+		mEp->ep.maxpacket = CTRL_PAYLOAD_MAX;
+
+		/* this allocation cannot be random */
+		for (k = RX; k <= TX; k++) {
+			INIT_LIST_HEAD(&mEp->qh[k].queue);
+			mEp->qh[k].ptr = dma_pool_alloc(udc->qh_pool,
+							GFP_KERNEL,
+							&mEp->qh[k].dma);
+			if (mEp->qh[k].ptr == NULL)
+				retval = -ENOMEM;
+			else
+				memset(mEp->qh[k].ptr, 0,
+				       sizeof(*mEp->qh[k].ptr));
+		}
+		if (i == 0)
+			udc->gadget.ep0 = &mEp->ep;
+		else
+			list_add_tail(&mEp->ep.ep_list, &udc->gadget.ep_list);
+	}
+	if (retval)
+		goto done;
+
+	/* bind gadget */
+	driver->driver.bus     = NULL;
+	udc->gadget.ops        = &usb_gadget_ops;
+	udc->gadget.dev.driver = &driver->driver;
+
+	spin_unlock_irqrestore(udc->lock, flags);
+	retval = driver->bind(&udc->gadget);                /* MAY SLEEP */
+	spin_lock_irqsave(udc->lock, flags);
+
+	if (retval) {
+		udc->gadget.ops        = NULL;
+		udc->gadget.dev.driver = NULL;
+		goto done;
+	}
+
+	retval = hw_device_state(udc->ci13xxx_ep[0].qh[RX].dma);
+
+ done:
+	spin_unlock_irqrestore(udc->lock, flags);
+	if (retval)
+		usb_gadget_unregister_driver(driver);
+	return retval;
+}
+EXPORT_SYMBOL(usb_gadget_register_driver);
+
+/**
+ * usb_gadget_unregister_driver: unregister a gadget driver
+ *
+ * Check usb_gadget_unregister_driver() at "usb_gadget.h" for details
+ */
+int usb_gadget_unregister_driver(struct usb_gadget_driver *driver)
+{
+	struct ci13xxx *udc = _udc;
+	unsigned long i, k, flags;
+
+	trace("%p", driver);
+
+	if (driver             == NULL ||
+	    driver->bind       == NULL ||
+	    driver->unbind     == NULL ||
+	    driver->setup      == NULL ||
+	    driver->disconnect == NULL ||
+	    driver->suspend    == NULL ||
+	    driver->resume     == NULL ||
+	    driver             != udc->driver)
+		return -EINVAL;
+
+	spin_lock_irqsave(udc->lock, flags);
+
+	hw_device_state(0);
+
+	/* unbind gadget */
+	if (udc->gadget.ops != NULL) {
+		_gadget_stop_activity(&udc->gadget);
+
+		spin_unlock_irqrestore(udc->lock, flags);
+		driver->unbind(&udc->gadget);               /* MAY SLEEP */
+		spin_lock_irqsave(udc->lock, flags);
+
+		udc->gadget.ops        = NULL;
+		udc->gadget.dev.driver = NULL;
+	}
+
+	/* free resources */
+	for (i = 0; i < hw_ep_max; i++) {
+		struct ci13xxx_ep *mEp = &udc->ci13xxx_ep[i];
+
+		if (i == 0)
+			udc->gadget.ep0 = NULL;
+		else if (!list_empty(&mEp->ep.ep_list))
+			list_del_init(&mEp->ep.ep_list);
+
+		for (k = RX; k <= TX; k++)
+			if (mEp->qh[k].ptr != NULL)
+				dma_pool_free(udc->qh_pool,
+					      mEp->qh[k].ptr, mEp->qh[k].dma);
+	}
+
+	udc->driver = NULL;
+
+	spin_unlock_irqrestore(udc->lock, flags);
+
+	if (udc->td_pool != NULL) {
+		dma_pool_destroy(udc->td_pool);
+		udc->td_pool = NULL;
+	}
+	if (udc->qh_pool != NULL) {
+		dma_pool_destroy(udc->qh_pool);
+		udc->qh_pool = NULL;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(usb_gadget_unregister_driver);
+
+/******************************************************************************
+ * BUS block
+ *****************************************************************************/
+/**
+ * udc_irq: global interrupt handler
+ *
+ * This function returns IRQ_HANDLED if the IRQ has been handled
+ * It locks access to registers
+ */
+static irqreturn_t udc_irq(void)
+{
+	struct ci13xxx *udc = _udc;
+	irqreturn_t retval;
+	u32 intr;
+
+	trace();
+
+	if (udc == NULL) {
+		err("ENODEV");
+		return IRQ_HANDLED;
+	}
+
+	spin_lock(udc->lock);
+	intr = hw_test_and_clear_intr_active();
+	if (intr) {
+		isr_statistics.hndl.buf[isr_statistics.hndl.idx++] = intr;
+		isr_statistics.hndl.idx &= ISR_MASK;
+		isr_statistics.hndl.cnt++;
+
+		/* order defines priority - do NOT change it */
+		if (USBi_URI & intr) {
+			isr_statistics.uri++;
+			isr_reset_handler(udc);
+		}
+		if (USBi_PCI & intr) {
+			isr_statistics.pci++;
+			udc->gadget.speed = hw_port_is_high_speed() ?
+				USB_SPEED_HIGH : USB_SPEED_FULL;
+		}
+		if (USBi_UEI & intr)
+			isr_statistics.uei++;
+		if (USBi_UI  & intr) {
+			isr_statistics.ui++;
+			isr_tr_complete_handler(udc);
+		}
+		if (USBi_SLI & intr)
+			isr_statistics.sli++;
+		retval = IRQ_HANDLED;
+	} else {
+		isr_statistics.none++;
+		retval = IRQ_NONE;
+	}
+	spin_unlock(udc->lock);
+
+	return retval;
+}
+
+/**
+ * udc_release: driver release function
+ * @dev: device
+ *
+ * Currently does nothing
+ */
+static void udc_release(struct device *dev)
+{
+	trace("%p", dev);
+
+	if (dev == NULL)
+		err("EINVAL");
+}
+
+/**
+ * udc_probe: parent probe must call this to initialize UDC
+ * @dev:  parent device
+ * @regs: registers base address
+ * @name: driver name
+ *
+ * This function returns an error code
+ * No interrupts active, the IRQ has not been requested yet
+ * Kernel assumes 32-bit DMA operations by default, no need to dma_set_mask
+ */
+static int udc_probe(struct device *dev, void __iomem *regs, const char *name)
+{
+	struct ci13xxx *udc;
+	int retval = 0;
+
+	trace("%p, %p, %p", dev, regs, name);
+
+	if (dev == NULL || regs == NULL || name == NULL)
+		return -EINVAL;
+
+	udc = kzalloc(sizeof(struct ci13xxx), GFP_KERNEL);
+	if (udc == NULL)
+		return -ENOMEM;
+
+	udc->lock = &udc_lock;
+
+	retval = hw_device_reset(regs);
+	if (retval)
+		goto done;
+
+	udc->gadget.ops          = NULL;
+	udc->gadget.speed        = USB_SPEED_UNKNOWN;
+	udc->gadget.is_dualspeed = 1;
+	udc->gadget.is_otg       = 0;
+	udc->gadget.name         = name;
+
+	INIT_LIST_HEAD(&udc->gadget.ep_list);
+	udc->gadget.ep0 = NULL;
+
+	strcpy(udc->gadget.dev.bus_id, "gadget");
+	udc->gadget.dev.dma_mask = dev->dma_mask;
+	udc->gadget.dev.parent   = dev;
+	udc->gadget.dev.release  = udc_release;
+
+	retval = device_register(&udc->gadget.dev);
+	if (retval)
+		goto done;
+
+#ifdef CONFIG_USB_GADGET_DEBUG_FILES
+	retval = dbg_create_files(&udc->gadget.dev);
+#endif
+	if (retval) {
+		device_unregister(&udc->gadget.dev);
+		goto done;
+	}
+
+	_udc = udc;
+	return retval;
+
+ done:
+	err("error = %i", retval);
+	kfree(udc);
+	_udc = NULL;
+	return retval;
+}
+
+/**
+ * udc_remove: parent remove must call this to remove UDC
+ *
+ * No interrupts active, the IRQ has been released
+ */
+static void udc_remove(void)
+{
+	struct ci13xxx *udc = _udc;
+
+	if (udc == NULL) {
+		err("EINVAL");
+		return;
+	}
+
+#ifdef CONFIG_USB_GADGET_DEBUG_FILES
+	dbg_remove_files(&udc->gadget.dev);
+#endif
+	device_unregister(&udc->gadget.dev);
+
+	kfree(udc);
+	_udc = NULL;
+}
+
+/******************************************************************************
+ * PCI block
+ *****************************************************************************/
+/**
+ * ci13xxx_pci_irq: interrut handler
+ * @irq:  irq number
+ * @pdev: USB Device Controller interrupt source
+ *
+ * This function returns IRQ_HANDLED if the IRQ has been handled
+ * This is an ISR don't trace, use attribute interface instead
+ */
+static irqreturn_t ci13xxx_pci_irq(int irq, void *pdev)
+{
+	if (irq == 0) {
+		dev_err(&((struct pci_dev *)pdev)->dev, "Invalid IRQ0 usage!");
+		return IRQ_HANDLED;
+	}
+	return udc_irq();
+}
+
+/**
+ * ci13xxx_pci_probe: PCI probe
+ * @pdev: USB device controller being probed
+ * @id:   PCI hotplug ID connecting controller to UDC framework
+ *
+ * This function returns an error code
+ * Allocates basic PCI resources for this USB device controller, and then
+ * invokes the udc_probe() method to start the UDC associated with it
+ */
+static int __devinit ci13xxx_pci_probe(struct pci_dev *pdev,
+				       const struct pci_device_id *id)
+{
+	void __iomem *regs = NULL;
+	int retval = 0;
+
+	if (id == NULL)
+		return -EINVAL;
+
+	retval = pci_enable_device(pdev);
+	if (retval)
+		goto done;
+
+	if (!pdev->irq) {
+		dev_err(&pdev->dev, "No IRQ, check BIOS/PCI setup!");
+		retval = -ENODEV;
+		goto disable_device;
+	}
+
+	retval = pci_request_regions(pdev, UDC_DRIVER_NAME);
+	if (retval)
+		goto disable_device;
+
+	/* BAR 0 holds all the registers */
+	regs = pci_iomap(pdev, 0, 0);
+	if (!regs) {
+		dev_err(&pdev->dev, "Error mapping memory!");
+		retval = -EFAULT;
+		goto release_regions;
+	}
+	pci_set_drvdata(pdev, (__force void *)regs);
+
+	pci_set_master(pdev);
+	pci_try_set_mwi(pdev);
+
+	retval = udc_probe(&pdev->dev, regs, UDC_DRIVER_NAME);
+	if (retval)
+		goto iounmap;
+
+	/* our device does not have MSI capability */
+
+	retval = request_irq(pdev->irq, ci13xxx_pci_irq, IRQF_SHARED,
+			     UDC_DRIVER_NAME, pdev);
+	if (retval)
+		goto gadget_remove;
+
+	return 0;
+
+ gadget_remove:
+	udc_remove();
+ iounmap:
+	pci_iounmap(pdev, regs);
+ release_regions:
+	pci_release_regions(pdev);
+ disable_device:
+	pci_disable_device(pdev);
+ done:
+	return retval;
+}
+
+/**
+ * ci13xxx_pci_remove: PCI remove
+ * @pdev: USB Device Controller being removed
+ *
+ * Reverses the effect of ci13xxx_pci_probe(),
+ * first invoking the udc_remove() and then releases
+ * all PCI resources allocated for this USB device controller
+ */
+static void __devexit ci13xxx_pci_remove(struct pci_dev *pdev)
+{
+	free_irq(pdev->irq, pdev);
+	udc_remove();
+	pci_iounmap(pdev, (__force void __iomem *)pci_get_drvdata(pdev));
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+}
+
+/**
+ * PCI device table
+ * PCI device structure
+ *
+ * Check "pci.h" for details
+ */
+static DEFINE_PCI_DEVICE_TABLE(ci13xxx_pci_id_table) = {
+	{ PCI_DEVICE(0x153F, 0x1004) },
+	{ PCI_DEVICE(0x153F, 0x1006) },
+	{ 0, 0, 0, 0, 0, 0, 0 /* end: all zeroes */ }
+};
+MODULE_DEVICE_TABLE(pci, ci13xxx_pci_id_table);
+
+static struct pci_driver ci13xxx_pci_driver = {
+	.name         =	UDC_DRIVER_NAME,
+	.id_table     =	ci13xxx_pci_id_table,
+	.probe        =	ci13xxx_pci_probe,
+	.remove       =	__devexit_p(ci13xxx_pci_remove),
+};
+
+/**
+ * ci13xxx_pci_init: module init
+ *
+ * Driver load
+ */
+static int __init ci13xxx_pci_init(void)
+{
+	return pci_register_driver(&ci13xxx_pci_driver);
+}
+module_init(ci13xxx_pci_init);
+
+/**
+ * ci13xxx_pci_exit: module exit
+ *
+ * Driver unload
+ */
+static void __exit ci13xxx_pci_exit(void)
+{
+	pci_unregister_driver(&ci13xxx_pci_driver);
+}
+module_exit(ci13xxx_pci_exit);
+
+MODULE_AUTHOR("MIPS - David Lopo <dlopo@chipidea.mips.com>");
+MODULE_DESCRIPTION("MIPS CI13XXX USB Peripheral Controller");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("June 2008");
diff --git a/drivers/usb/gadget/ci13xxx_udc.h b/drivers/usb/gadget/ci13xxx_udc.h
new file mode 100644
index 000000000000..4026e9cede34
--- /dev/null
+++ b/drivers/usb/gadget/ci13xxx_udc.h
@@ -0,0 +1,195 @@
+/*
+ * ci13xxx_udc.h - structures, registers, and macros MIPS USB IP core
+ *
+ * Copyright (C) 2008 Chipidea - MIPS Technologies, Inc. All rights reserved.
+ *
+ * Author: David Lopo
+ *
+ * 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.
+ *
+ * Description: MIPS USB IP core family device controller
+ *              Structures, registers and logging macros
+ */
+
+#ifndef _CI13XXX_h_
+#define _CI13XXX_h_
+
+/******************************************************************************
+ * DEFINE
+ *****************************************************************************/
+#define ENDPT_MAX          (16)
+#define CTRL_PAYLOAD_MAX   (64)
+#define RX        (0)  /* similar to USB_DIR_OUT but can be used as an index */
+#define TX        (1)  /* similar to USB_DIR_IN  but can be used as an index */
+
+/******************************************************************************
+ * STRUCTURES
+ *****************************************************************************/
+/* DMA layout of transfer descriptors */
+struct ci13xxx_td {
+	/* 0 */
+	u32 next;
+#define TD_TERMINATE          BIT(0)
+	/* 1 */
+	u32 token;
+#define TD_STATUS             (0x00FFUL <<  0)
+#define TD_STATUS_TR_ERR      BIT(3)
+#define TD_STATUS_DT_ERR      BIT(5)
+#define TD_STATUS_HALTED      BIT(6)
+#define TD_STATUS_ACTIVE      BIT(7)
+#define TD_MULTO              (0x0003UL << 10)
+#define TD_IOC                BIT(15)
+#define TD_TOTAL_BYTES        (0x7FFFUL << 16)
+	/* 2 */
+	u32 page[5];
+#define TD_CURR_OFFSET        (0x0FFFUL <<  0)
+#define TD_FRAME_NUM          (0x07FFUL <<  0)
+#define TD_RESERVED_MASK      (0x0FFFUL <<  0)
+} __attribute__ ((packed));
+
+/* DMA layout of queue heads */
+struct ci13xxx_qh {
+	/* 0 */
+	u32 cap;
+#define QH_IOS                BIT(15)
+#define QH_MAX_PKT            (0x07FFUL << 16)
+#define QH_ZLT                BIT(29)
+#define QH_MULT               (0x0003UL << 30)
+	/* 1 */
+	u32 curr;
+	/* 2 - 8 */
+	struct ci13xxx_td        td;
+	/* 9 */
+	u32 RESERVED;
+	struct usb_ctrlrequest   setup;
+} __attribute__ ((packed));
+
+/* Extension of usb_request */
+struct ci13xxx_req {
+	struct usb_request   req;
+	unsigned             map;
+	struct list_head     queue;
+	struct ci13xxx_td   *ptr;
+	dma_addr_t           dma;
+};
+
+/* Extension of usb_ep */
+struct ci13xxx_ep {
+	struct usb_ep                          ep;
+	const struct usb_endpoint_descriptor  *desc;
+	u8                                     dir;
+	u8                                     num;
+	u8                                     type;
+	char                                   name[16];
+	struct {
+		struct list_head   queue;
+		struct ci13xxx_qh *ptr;
+		dma_addr_t         dma;
+	}                                      qh[2];
+	struct usb_request                    *status;
+	int                                    wedge;
+
+	/* global resources */
+	spinlock_t                            *lock;
+	struct device                         *device;
+	struct dma_pool                       *td_pool;
+};
+
+/* CI13XXX UDC descriptor & global resources */
+struct ci13xxx {
+	spinlock_t		  *lock;      /* ctrl register bank access */
+
+	struct dma_pool           *qh_pool;   /* DMA pool for queue heads */
+	struct dma_pool           *td_pool;   /* DMA pool for transfer descs */
+
+	struct usb_gadget          gadget;     /* USB slave device */
+	struct ci13xxx_ep          ci13xxx_ep[ENDPT_MAX]; /* extended endpts */
+
+	struct usb_gadget_driver  *driver;     /* 3rd party gadget driver */
+};
+
+/******************************************************************************
+ * REGISTERS
+ *****************************************************************************/
+/* register size */
+#define REG_BITS   (32)
+
+/* HCCPARAMS */
+#define HCCPARAMS_LEN         BIT(17)
+
+/* DCCPARAMS */
+#define DCCPARAMS_DEN         (0x1F << 0)
+#define DCCPARAMS_DC          BIT(7)
+
+/* TESTMODE */
+#define TESTMODE_FORCE        BIT(0)
+
+/* USBCMD */
+#define USBCMD_RS             BIT(0)
+#define USBCMD_RST            BIT(1)
+#define USBCMD_SUTW           BIT(13)
+
+/* USBSTS & USBINTR */
+#define USBi_UI               BIT(0)
+#define USBi_UEI              BIT(1)
+#define USBi_PCI              BIT(2)
+#define USBi_URI              BIT(6)
+#define USBi_SLI              BIT(8)
+
+/* DEVICEADDR */
+#define DEVICEADDR_USBADRA    BIT(24)
+#define DEVICEADDR_USBADR     (0x7FUL << 25)
+
+/* PORTSC */
+#define PORTSC_SUSP           BIT(7)
+#define PORTSC_HSP            BIT(9)
+#define PORTSC_PTC            (0x0FUL << 16)
+
+/* DEVLC */
+#define DEVLC_PSPD            (0x03UL << 25)
+#define    DEVLC_PSPD_HS      (0x02UL << 25)
+
+/* USBMODE */
+#define USBMODE_CM            (0x03UL <<  0)
+#define    USBMODE_CM_IDLE    (0x00UL <<  0)
+#define    USBMODE_CM_DEVICE  (0x02UL <<  0)
+#define    USBMODE_CM_HOST    (0x03UL <<  0)
+#define USBMODE_SLOM          BIT(3)
+
+/* ENDPTCTRL */
+#define ENDPTCTRL_RXS         BIT(0)
+#define ENDPTCTRL_RXT         (0x03UL <<  2)
+#define ENDPTCTRL_RXR         BIT(6)         /* reserved for port 0 */
+#define ENDPTCTRL_RXE         BIT(7)
+#define ENDPTCTRL_TXS         BIT(16)
+#define ENDPTCTRL_TXT         (0x03UL << 18)
+#define ENDPTCTRL_TXR         BIT(22)        /* reserved for port 0 */
+#define ENDPTCTRL_TXE         BIT(23)
+
+/******************************************************************************
+ * LOGGING
+ *****************************************************************************/
+#define ci13xxx_printk(level, format, args...) \
+do { \
+	if (_udc == NULL) \
+		printk(level "[%s] " format "\n", __func__, ## args); \
+	else \
+		dev_printk(level, _udc->gadget.dev.parent, \
+			   "[%s] " format "\n", __func__, ## args); \
+} while (0)
+
+#define err(format, args...)    ci13xxx_printk(KERN_ERR, format, ## args)
+#define warn(format, args...)   ci13xxx_printk(KERN_WARNING, format, ## args)
+#define info(format, args...)   ci13xxx_printk(KERN_INFO, format, ## args)
+
+#ifdef TRACE
+#define trace(format, args...)      ci13xxx_printk(KERN_DEBUG, format, ## args)
+#define dbg_trace(format, args...)  dev_dbg(dev, format, ##args)
+#else
+#define trace(format, args...)      do {} while (0)
+#define dbg_trace(format, args...)  do {} while (0)
+#endif
+
+#endif	/* _CI13XXX_h_ */
diff --git a/drivers/usb/gadget/epautoconf.c b/drivers/usb/gadget/epautoconf.c
index 9462e30192d8..a36b1175b18d 100644
--- a/drivers/usb/gadget/epautoconf.c
+++ b/drivers/usb/gadget/epautoconf.c
@@ -161,7 +161,7 @@ ep_matches (
 	/* report address */
 	desc->bEndpointAddress &= USB_DIR_IN;
 	if (isdigit (ep->name [2])) {
-		u8	num = simple_strtol (&ep->name [2], NULL, 10);
+		u8	num = simple_strtoul (&ep->name [2], NULL, 10);
 		desc->bEndpointAddress |= num;
 #ifdef	MANY_ENDPOINTS
 	} else if (desc->bEndpointAddress & USB_DIR_IN) {
diff --git a/drivers/usb/gadget/file_storage.c b/drivers/usb/gadget/file_storage.c
index 2e71368f45b4..b10fa31cc915 100644
--- a/drivers/usb/gadget/file_storage.c
+++ b/drivers/usb/gadget/file_storage.c
@@ -1,7 +1,7 @@
 /*
  * file_storage.c -- File-backed USB Storage Gadget, for USB development
  *
- * Copyright (C) 2003-2007 Alan Stern
+ * Copyright (C) 2003-2008 Alan Stern
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -38,16 +38,17 @@
 
 /*
  * The File-backed Storage Gadget acts as a USB Mass Storage device,
- * appearing to the host as a disk drive.  In addition to providing an
- * example of a genuinely useful gadget driver for a USB device, it also
- * illustrates a technique of double-buffering for increased throughput.
- * Last but not least, it gives an easy way to probe the behavior of the
- * Mass Storage drivers in a USB host.
+ * appearing to the host as a disk drive or as a CD-ROM drive.  In addition
+ * to providing an example of a genuinely useful gadget driver for a USB
+ * device, it also illustrates a technique of double-buffering for increased
+ * throughput.  Last but not least, it gives an easy way to probe the
+ * behavior of the Mass Storage drivers in a USB host.
  *
  * Backing storage is provided by a regular file or a block device, specified
  * by the "file" module parameter.  Access can be limited to read-only by
- * setting the optional "ro" module parameter.  The gadget will indicate that
- * it has removable media if the optional "removable" module parameter is set.
+ * setting the optional "ro" module parameter.  (For CD-ROM emulation,
+ * access is always read-only.)  The gadget will indicate that it has
+ * removable media if the optional "removable" module parameter is set.
  *
  * The gadget supports the Control-Bulk (CB), Control-Bulk-Interrupt (CBI),
  * and Bulk-Only (also known as Bulk-Bulk-Bulk or BBB) transports, selected
@@ -64,7 +65,12 @@
  * The default number of LUNs is taken from the number of "file" elements;
  * it is 1 if "file" is not given.  If "removable" is not set then a backing
  * file must be specified for each LUN.  If it is set, then an unspecified
- * or empty backing filename means the LUN's medium is not loaded.
+ * or empty backing filename means the LUN's medium is not loaded.  Ideally
+ * each LUN would be settable independently as a disk drive or a CD-ROM
+ * drive, but currently all LUNs have to be the same type.  The CD-ROM
+ * emulation includes a single data track and no audio tracks; hence there
+ * need be only one backing file per LUN.  Note also that the CD-ROM block
+ * length is set to 512 rather than the more common value 2048.
  *
  * Requirements are modest; only a bulk-in and a bulk-out endpoint are
  * needed (an interrupt-out endpoint is also needed for CBI).  The memory
@@ -91,6 +97,8 @@
  *					USB device controller (usually true),
  *					boolean to permit the driver to halt
  *					bulk endpoints
+ *	cdrom			Default false, boolean for whether to emulate
+ *					a CD-ROM drive
  *	transport=XXX		Default BBB, transport name (CB, CBI, or BBB)
  *	protocol=YYY		Default SCSI, protocol name (RBC, 8020 or
  *					ATAPI, QIC, UFI, 8070, or SCSI;
@@ -103,15 +111,16 @@
  *					PAGE_CACHE_SIZE)
  *
  * If CONFIG_USB_FILE_STORAGE_TEST is not set, only the "file", "ro",
- * "removable", "luns", and "stall" options are available; default values
- * are used for everything else.
+ * "removable", "luns", "stall", and "cdrom" options are available; default
+ * values are used for everything else.
  *
  * The pathnames of the backing files and the ro settings are available in
  * the attribute files "file" and "ro" in the lun<n> subdirectory of the
  * gadget's sysfs directory.  If the "removable" option is set, writing to
  * these files will simulate ejecting/loading the medium (writing an empty
  * line means eject) and adjusting a write-enable tab.  Changes to the ro
- * setting are not allowed when the medium is loaded.
+ * setting are not allowed when the medium is loaded or if CD-ROM emulation
+ * is being used.
  *
  * This gadget driver is heavily based on "Gadget Zero" by David Brownell.
  * The driver's SCSI command interface was based on the "Information
@@ -261,7 +270,7 @@
 
 #define DRIVER_DESC		"File-backed Storage Gadget"
 #define DRIVER_NAME		"g_file_storage"
-#define DRIVER_VERSION		"7 August 2007"
+#define DRIVER_VERSION		"20 November 2008"
 
 static const char longname[] = DRIVER_DESC;
 static const char shortname[] = DRIVER_NAME;
@@ -341,6 +350,7 @@ static struct {
 
 	int		removable;
 	int		can_stall;
+	int		cdrom;
 
 	char		*transport_parm;
 	char		*protocol_parm;
@@ -359,6 +369,7 @@ static struct {
 	.protocol_parm		= "SCSI",
 	.removable		= 0,
 	.can_stall		= 1,
+	.cdrom			= 0,
 	.vendor			= DRIVER_VENDOR_ID,
 	.product		= DRIVER_PRODUCT_ID,
 	.release		= 0xffff,	// Use controller chip type
@@ -382,6 +393,9 @@ MODULE_PARM_DESC(removable, "true to simulate removable media");
 module_param_named(stall, mod_data.can_stall, bool, S_IRUGO);
 MODULE_PARM_DESC(stall, "false to prevent bulk stalls");
 
+module_param_named(cdrom, mod_data.cdrom, bool, S_IRUGO);
+MODULE_PARM_DESC(cdrom, "true to emulate cdrom instead of disk");
+
 
 /* In the non-TEST version, only the module parameters listed above
  * are available. */
@@ -411,6 +425,10 @@ MODULE_PARM_DESC(buflen, "I/O buffer size");
 
 /*-------------------------------------------------------------------------*/
 
+/* SCSI device types */
+#define TYPE_DISK	0x00
+#define TYPE_CDROM	0x05
+
 /* USB protocol value = the transport method */
 #define USB_PR_CBI	0x00		// Control/Bulk/Interrupt
 #define USB_PR_CB	0x01		// Control/Bulk w/o interrupt
@@ -487,6 +505,8 @@ struct interrupt_data {
 #define SC_READ_12			0xa8
 #define SC_READ_CAPACITY		0x25
 #define SC_READ_FORMAT_CAPACITIES	0x23
+#define SC_READ_HEADER			0x44
+#define SC_READ_TOC			0x43
 #define SC_RELEASE			0x17
 #define SC_REQUEST_SENSE		0x03
 #define SC_RESERVE			0x16
@@ -2006,23 +2026,28 @@ static int do_inquiry(struct fsg_dev *fsg, struct fsg_buffhd *bh)
 	u8	*buf = (u8 *) bh->buf;
 
 	static char vendor_id[] = "Linux   ";
-	static char product_id[] = "File-Stor Gadget";
+	static char product_disk_id[] = "File-Stor Gadget";
+	static char product_cdrom_id[] = "File-CD Gadget  ";
 
 	if (!fsg->curlun) {		// Unsupported LUNs are okay
 		fsg->bad_lun_okay = 1;
 		memset(buf, 0, 36);
 		buf[0] = 0x7f;		// Unsupported, no device-type
+		buf[4] = 31;		// Additional length
 		return 36;
 	}
 
-	memset(buf, 0, 8);	// Non-removable, direct-access device
+	memset(buf, 0, 8);
+	buf[0] = (mod_data.cdrom ? TYPE_CDROM : TYPE_DISK);
 	if (mod_data.removable)
 		buf[1] = 0x80;
 	buf[2] = 2;		// ANSI SCSI level 2
 	buf[3] = 2;		// SCSI-2 INQUIRY data format
 	buf[4] = 31;		// Additional length
 				// No special options
-	sprintf(buf + 8, "%-8s%-16s%04x", vendor_id, product_id,
+	sprintf(buf + 8, "%-8s%-16s%04x", vendor_id,
+			(mod_data.cdrom ? product_cdrom_id :
+				product_disk_id),
 			mod_data.release);
 	return 36;
 }
@@ -2101,6 +2126,75 @@ static int do_read_capacity(struct fsg_dev *fsg, struct fsg_buffhd *bh)
 }
 
 
+static void store_cdrom_address(u8 *dest, int msf, u32 addr)
+{
+	if (msf) {
+		/* Convert to Minutes-Seconds-Frames */
+		addr >>= 2;		/* Convert to 2048-byte frames */
+		addr += 2*75;		/* Lead-in occupies 2 seconds */
+		dest[3] = addr % 75;	/* Frames */
+		addr /= 75;
+		dest[2] = addr % 60;	/* Seconds */
+		addr /= 60;
+		dest[1] = addr;		/* Minutes */
+		dest[0] = 0;		/* Reserved */
+	} else {
+		/* Absolute sector */
+		put_be32(dest, addr);
+	}
+}
+
+static int do_read_header(struct fsg_dev *fsg, struct fsg_buffhd *bh)
+{
+	struct lun	*curlun = fsg->curlun;
+	int		msf = fsg->cmnd[1] & 0x02;
+	u32		lba = get_be32(&fsg->cmnd[2]);
+	u8		*buf = (u8 *) bh->buf;
+
+	if ((fsg->cmnd[1] & ~0x02) != 0) {		/* Mask away MSF */
+		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
+		return -EINVAL;
+	}
+	if (lba >= curlun->num_sectors) {
+		curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
+		return -EINVAL;
+	}
+
+	memset(buf, 0, 8);
+	buf[0] = 0x01;		/* 2048 bytes of user data, rest is EC */
+	store_cdrom_address(&buf[4], msf, lba);
+	return 8;
+}
+
+
+static int do_read_toc(struct fsg_dev *fsg, struct fsg_buffhd *bh)
+{
+	struct lun	*curlun = fsg->curlun;
+	int		msf = fsg->cmnd[1] & 0x02;
+	int		start_track = fsg->cmnd[6];
+	u8		*buf = (u8 *) bh->buf;
+
+	if ((fsg->cmnd[1] & ~0x02) != 0 ||		/* Mask away MSF */
+			start_track > 1) {
+		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
+		return -EINVAL;
+	}
+
+	memset(buf, 0, 20);
+	buf[1] = (20-2);		/* TOC data length */
+	buf[2] = 1;			/* First track number */
+	buf[3] = 1;			/* Last track number */
+	buf[5] = 0x16;			/* Data track, copying allowed */
+	buf[6] = 0x01;			/* Only track is number 1 */
+	store_cdrom_address(&buf[8], msf, 0);
+
+	buf[13] = 0x16;			/* Lead-out track is data */
+	buf[14] = 0xAA;			/* Lead-out track number */
+	store_cdrom_address(&buf[16], msf, curlun->num_sectors);
+	return 20;
+}
+
+
 static int do_mode_sense(struct fsg_dev *fsg, struct fsg_buffhd *bh)
 {
 	struct lun	*curlun = fsg->curlun;
@@ -2848,6 +2942,26 @@ static int do_scsi_command(struct fsg_dev *fsg)
 			reply = do_read_capacity(fsg, bh);
 		break;
 
+	case SC_READ_HEADER:
+		if (!mod_data.cdrom)
+			goto unknown_cmnd;
+		fsg->data_size_from_cmnd = get_be16(&fsg->cmnd[7]);
+		if ((reply = check_command(fsg, 10, DATA_DIR_TO_HOST,
+				(3<<7) | (0x1f<<1), 1,
+				"READ HEADER")) == 0)
+			reply = do_read_header(fsg, bh);
+		break;
+
+	case SC_READ_TOC:
+		if (!mod_data.cdrom)
+			goto unknown_cmnd;
+		fsg->data_size_from_cmnd = get_be16(&fsg->cmnd[7]);
+		if ((reply = check_command(fsg, 10, DATA_DIR_TO_HOST,
+				(7<<6) | (1<<1), 1,
+				"READ TOC")) == 0)
+			reply = do_read_toc(fsg, bh);
+		break;
+
 	case SC_READ_FORMAT_CAPACITIES:
 		fsg->data_size_from_cmnd = get_be16(&fsg->cmnd[7]);
 		if ((reply = check_command(fsg, 10, DATA_DIR_TO_HOST,
@@ -2933,6 +3047,7 @@ static int do_scsi_command(struct fsg_dev *fsg)
 		// Fall through
 
 	default:
+ unknown_cmnd:
 		fsg->data_size_from_cmnd = 0;
 		sprintf(unknown, "Unknown x%02x", fsg->cmnd[0]);
 		if ((reply = check_command(fsg, fsg->cmnd_size,
@@ -3498,6 +3613,7 @@ static int open_backing_file(struct lun *curlun, const char *filename)
 	struct inode			*inode = NULL;
 	loff_t				size;
 	loff_t				num_sectors;
+	loff_t				min_sectors;
 
 	/* R/W if we can, R/O if we must */
 	ro = curlun->ro;
@@ -3541,8 +3657,19 @@ static int open_backing_file(struct lun *curlun, const char *filename)
 		rc = (int) size;
 		goto out;
 	}
-	num_sectors = size >> 9;	// File size in 512-byte sectors
-	if (num_sectors == 0) {
+	num_sectors = size >> 9;	// File size in 512-byte blocks
+	min_sectors = 1;
+	if (mod_data.cdrom) {
+		num_sectors &= ~3;	// Reduce to a multiple of 2048
+		min_sectors = 300*4;	// Smallest track is 300 frames
+		if (num_sectors >= 256*60*75*4) {
+			num_sectors = (256*60*75 - 1) * 4;
+			LINFO(curlun, "file too big: %s\n", filename);
+			LINFO(curlun, "using only first %d blocks\n",
+					(int) num_sectors);
+		}
+	}
+	if (num_sectors < min_sectors) {
 		LINFO(curlun, "file too small: %s\n", filename);
 		rc = -ETOOSMALL;
 		goto out;
@@ -3845,9 +3972,12 @@ static int __init fsg_bind(struct usb_gadget *gadget)
 		goto out;
 
 	if (mod_data.removable) {	// Enable the store_xxx attributes
-		dev_attr_ro.attr.mode = dev_attr_file.attr.mode = 0644;
-		dev_attr_ro.store = store_ro;
+		dev_attr_file.attr.mode = 0644;
 		dev_attr_file.store = store_file;
+		if (!mod_data.cdrom) {
+			dev_attr_ro.attr.mode = 0644;
+			dev_attr_ro.store = store_ro;
+		}
 	}
 
 	/* Find out how many LUNs there should be */
@@ -3872,6 +4002,8 @@ static int __init fsg_bind(struct usb_gadget *gadget)
 	for (i = 0; i < fsg->nluns; ++i) {
 		curlun = &fsg->luns[i];
 		curlun->ro = mod_data.ro[i];
+		if (mod_data.cdrom)
+			curlun->ro = 1;
 		curlun->dev.release = lun_release;
 		curlun->dev.parent = &gadget->dev;
 		curlun->dev.driver = &fsg_driver.driver;
@@ -4031,9 +4163,9 @@ static int __init fsg_bind(struct usb_gadget *gadget)
 			mod_data.protocol_name, mod_data.protocol_type);
 	DBG(fsg, "VendorID=x%04x, ProductID=x%04x, Release=x%04x\n",
 			mod_data.vendor, mod_data.product, mod_data.release);
-	DBG(fsg, "removable=%d, stall=%d, buflen=%u\n",
+	DBG(fsg, "removable=%d, stall=%d, cdrom=%d, buflen=%u\n",
 			mod_data.removable, mod_data.can_stall,
-			mod_data.buflen);
+			mod_data.cdrom, mod_data.buflen);
 	DBG(fsg, "I/O thread pid: %d\n", task_pid_nr(fsg->thread_task));
 
 	set_bit(REGISTERED, &fsg->atomic_bitflags);
@@ -4050,6 +4182,7 @@ out:
 	fsg->state = FSG_STATE_TERMINATED;	// The thread is dead
 	fsg_unbind(gadget);
 	close_all_backing_files(fsg);
+	complete(&fsg->thread_notifier);
 	return rc;
 }
 
diff --git a/drivers/usb/gadget/fsl_qe_udc.c b/drivers/usb/gadget/fsl_qe_udc.c
index b3408ff39fba..d6c5bcd40064 100644
--- a/drivers/usb/gadget/fsl_qe_udc.c
+++ b/drivers/usb/gadget/fsl_qe_udc.c
@@ -26,6 +26,7 @@
 #include <linux/ioport.h>
 #include <linux/types.h>
 #include <linux/errno.h>
+#include <linux/err.h>
 #include <linux/slab.h>
 #include <linux/list.h>
 #include <linux/interrupt.h>
@@ -370,6 +371,9 @@ static int qe_ep_bd_init(struct qe_udc *udc, unsigned char pipe_num)
 	/* alloc multi-ram for BD rings and set the ep parameters */
 	tmp_addr = cpm_muram_alloc(sizeof(struct qe_bd) * (bdring_len +
 				USB_BDRING_LEN_TX), QE_ALIGNMENT_OF_BD);
+	if (IS_ERR_VALUE(tmp_addr))
+		return -ENOMEM;
+
 	out_be16(&epparam->rbase, (u16)tmp_addr);
 	out_be16(&epparam->tbase, (u16)(tmp_addr +
 				(sizeof(struct qe_bd) * bdring_len)));
@@ -689,7 +693,7 @@ en_done2:
 en_done1:
 	spin_unlock_irqrestore(&udc->lock, flags);
 en_done:
-	dev_dbg(udc->dev, "failed to initialize %s\n", ep->ep.name);
+	dev_err(udc->dev, "failed to initialize %s\n", ep->ep.name);
 	return -ENODEV;
 }
 
@@ -2408,6 +2412,8 @@ static struct qe_udc __devinit *qe_udc_config(struct of_device *ofdev)
 	tmp_addr = cpm_muram_alloc((USB_MAX_ENDPOINTS *
 					sizeof(struct usb_ep_para)),
 					   USB_EP_PARA_ALIGNMENT);
+	if (IS_ERR_VALUE(tmp_addr))
+		goto cleanup;
 
 	for (i = 0; i < USB_MAX_ENDPOINTS; i++) {
 		out_be16(&usbpram->epptr[i], (u16)tmp_addr);
@@ -2513,7 +2519,7 @@ static int __devinit qe_udc_probe(struct of_device *ofdev,
 	/* Initialize the udc structure including QH member and other member */
 	udc_controller = qe_udc_config(ofdev);
 	if (!udc_controller) {
-		dev_dbg(&ofdev->dev, "udc_controll is NULL\n");
+		dev_err(&ofdev->dev, "failed to initialize\n");
 		return -ENOMEM;
 	}
 
@@ -2545,7 +2551,7 @@ static int __devinit qe_udc_probe(struct of_device *ofdev,
 
 	device_initialize(&udc_controller->gadget.dev);
 
-	strcpy(udc_controller->gadget.dev.bus_id, "gadget");
+	dev_set_name(&udc_controller->gadget.dev, "gadget");
 
 	udc_controller->gadget.dev.release = qe_udc_release;
 	udc_controller->gadget.dev.parent = &ofdev->dev;
@@ -2568,7 +2574,7 @@ static int __devinit qe_udc_probe(struct of_device *ofdev,
 	/* create a buf for ZLP send, need to remain zeroed */
 	udc_controller->nullbuf = kzalloc(256, GFP_KERNEL);
 	if (udc_controller->nullbuf == NULL) {
-		dev_dbg(udc_controller->dev, "cannot alloc nullbuf\n");
+		dev_err(udc_controller->dev, "cannot alloc nullbuf\n");
 		ret = -ENOMEM;
 		goto err3;
 	}
diff --git a/drivers/usb/gadget/gadget_chips.h b/drivers/usb/gadget/gadget_chips.h
index 4e3107dd2f34..ec6d439a2aa5 100644
--- a/drivers/usb/gadget/gadget_chips.h
+++ b/drivers/usb/gadget/gadget_chips.h
@@ -110,7 +110,6 @@
 #define gadget_is_at91(g)	0
 #endif
 
-/* status unclear */
 #ifdef CONFIG_USB_GADGET_IMX
 #define gadget_is_imx(g)	!strcmp("imx_udc", (g)->name)
 #else
@@ -158,6 +157,11 @@
 #define gadget_is_fsl_qe(g)	0
 #endif
 
+#ifdef CONFIG_USB_GADGET_CI13XXX
+#define gadget_is_ci13xxx(g)	(!strcmp("ci13xxx_udc", (g)->name))
+#else
+#define gadget_is_ci13xxx(g)	0
+#endif
 
 // CONFIG_USB_GADGET_SX2
 // CONFIG_USB_GADGET_AU1X00
@@ -225,6 +229,8 @@ static inline int usb_gadget_controller_number(struct usb_gadget *gadget)
 		return 0x21;
 	else if (gadget_is_fsl_qe(gadget))
 		return 0x22;
+	else if (gadget_is_ci13xxx(gadget))
+		return 0x23;
 	return -ENOENT;
 }
 
diff --git a/drivers/usb/gadget/goku_udc.c b/drivers/usb/gadget/goku_udc.c
index 60aa04847b18..63419c4d503c 100644
--- a/drivers/usb/gadget/goku_udc.c
+++ b/drivers/usb/gadget/goku_udc.c
@@ -1349,7 +1349,7 @@ int usb_gadget_register_driver(struct usb_gadget_driver *driver)
 	int			retval;
 
 	if (!driver
-			|| driver->speed != USB_SPEED_FULL
+			|| driver->speed < USB_SPEED_FULL
 			|| !driver->bind
 			|| !driver->disconnect
 			|| !driver->setup)
diff --git a/drivers/usb/gadget/imx_udc.c b/drivers/usb/gadget/imx_udc.c
new file mode 100644
index 000000000000..cde8fdf15d5b
--- /dev/null
+++ b/drivers/usb/gadget/imx_udc.c
@@ -0,0 +1,1516 @@
+/*
+ *	driver/usb/gadget/imx_udc.c
+ *
+ *	Copyright (C) 2005 Mike Lee(eemike@gmail.com)
+ *	Copyright (C) 2008 Darius Augulis <augulis.darius@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/init.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+
+#include <mach/usb.h>
+#include <mach/hardware.h>
+
+#include "imx_udc.h"
+
+static const char driver_name[] = "imx_udc";
+static const char ep0name[] = "ep0";
+
+void ep0_chg_stat(const char *label, struct imx_udc_struct *imx_usb,
+							enum ep0_state stat);
+
+/*******************************************************************************
+ * IMX UDC hardware related functions
+ *******************************************************************************
+ */
+
+void imx_udc_enable(struct imx_udc_struct *imx_usb)
+{
+	int temp = __raw_readl(imx_usb->base + USB_CTRL);
+	__raw_writel(temp | CTRL_FE_ENA | CTRL_AFE_ENA, imx_usb->base + USB_CTRL);
+	imx_usb->gadget.speed = USB_SPEED_FULL;
+}
+
+void imx_udc_disable(struct imx_udc_struct *imx_usb)
+{
+	int temp = __raw_readl(imx_usb->base + USB_CTRL);
+
+	__raw_writel(temp & ~(CTRL_FE_ENA | CTRL_AFE_ENA),
+		 imx_usb->base + USB_CTRL);
+
+	ep0_chg_stat(__func__, imx_usb, EP0_IDLE);
+	imx_usb->gadget.speed = USB_SPEED_UNKNOWN;
+}
+
+void imx_udc_reset(struct imx_udc_struct *imx_usb)
+{
+	int temp = __raw_readl(imx_usb->base + USB_ENAB);
+
+	/* set RST bit */
+	__raw_writel(temp | ENAB_RST, imx_usb->base + USB_ENAB);
+
+	/* wait RST bit to clear */
+	do {} while (__raw_readl(imx_usb->base + USB_ENAB) & ENAB_RST);
+
+	/* wait CFG bit to assert */
+	do {} while (!(__raw_readl(imx_usb->base + USB_DADR) & DADR_CFG));
+
+	/* udc module is now ready */
+}
+
+void imx_udc_config(struct imx_udc_struct *imx_usb)
+{
+	u8 ep_conf[5];
+	u8 i, j, cfg;
+	struct imx_ep_struct *imx_ep;
+
+	/* wait CFG bit to assert */
+	do {} while (!(__raw_readl(imx_usb->base + USB_DADR) & DADR_CFG));
+
+	/* Download the endpoint buffer for endpoint 0. */
+	for (j = 0; j < 5; j++) {
+		i = (j == 2 ? imx_usb->imx_ep[0].fifosize : 0x00);
+		__raw_writeb(i, imx_usb->base + USB_DDAT);
+		do {} while (__raw_readl(imx_usb->base + USB_DADR) & DADR_BSY);
+	}
+
+	/* Download the endpoint buffers for endpoints 1-5.
+	 * We specify two configurations, one interface
+	 */
+	for (cfg = 1; cfg < 3; cfg++) {
+		for (i = 1; i < IMX_USB_NB_EP; i++) {
+			imx_ep = &imx_usb->imx_ep[i];
+			/* EP no | Config no */
+			ep_conf[0] = (i << 4) | (cfg << 2);
+			/* Type | Direction */
+			ep_conf[1] = (imx_ep->bmAttributes << 3) |
+					(EP_DIR(imx_ep) << 2);
+			/* Max packet size */
+			ep_conf[2] = imx_ep->fifosize;
+			/* TRXTYP */
+			ep_conf[3] = 0xC0;
+			/* FIFO no */
+			ep_conf[4] = i;
+
+			D_INI(imx_usb->dev,
+				"<%s> ep%d_conf[%d]:"
+				"[%02x-%02x-%02x-%02x-%02x]\n",
+				__func__, i, cfg,
+				ep_conf[0], ep_conf[1], ep_conf[2],
+				ep_conf[3], ep_conf[4]);
+
+			for (j = 0; j < 5; j++) {
+				__raw_writeb(ep_conf[j],
+					imx_usb->base + USB_DDAT);
+				do {} while (__raw_readl(imx_usb->base + USB_DADR)
+					& DADR_BSY);
+			}
+		}
+	}
+
+	/* wait CFG bit to clear */
+	do {} while (__raw_readl(imx_usb->base + USB_DADR) & DADR_CFG);
+}
+
+void imx_udc_init_irq(struct imx_udc_struct *imx_usb)
+{
+	int i;
+
+	/* Mask and clear all irqs */
+	__raw_writel(0xFFFFFFFF, imx_usb->base + USB_MASK);
+	__raw_writel(0xFFFFFFFF, imx_usb->base + USB_INTR);
+	for (i = 0; i < IMX_USB_NB_EP; i++) {
+		__raw_writel(0x1FF, imx_usb->base + USB_EP_MASK(i));
+		__raw_writel(0x1FF, imx_usb->base + USB_EP_INTR(i));
+	}
+
+	/* Enable USB irqs */
+	__raw_writel(INTR_MSOF | INTR_FRAME_MATCH, imx_usb->base + USB_MASK);
+
+	/* Enable EP0 irqs */
+	__raw_writel(0x1FF & ~(EPINTR_DEVREQ | EPINTR_MDEVREQ | EPINTR_EOT
+		| EPINTR_EOF | EPINTR_FIFO_EMPTY | EPINTR_FIFO_FULL),
+		imx_usb->base + USB_EP_MASK(0));
+}
+
+void imx_udc_init_ep(struct imx_udc_struct *imx_usb)
+{
+	int i, max, temp;
+	struct imx_ep_struct *imx_ep;
+	for (i = 0; i < IMX_USB_NB_EP; i++) {
+		imx_ep = &imx_usb->imx_ep[i];
+		switch (imx_ep->fifosize) {
+		case 8:
+			max = 0;
+			break;
+		case 16:
+			max = 1;
+			break;
+		case 32:
+			max = 2;
+			break;
+		case 64:
+			max = 3;
+			break;
+		default:
+			max = 1;
+			break;
+		}
+		temp = (EP_DIR(imx_ep) << 7) | (max << 5)
+			| (imx_ep->bmAttributes << 3);
+		__raw_writel(temp, imx_usb->base + USB_EP_STAT(i));
+		__raw_writel(temp | EPSTAT_FLUSH, imx_usb->base + USB_EP_STAT(i));
+		D_INI(imx_usb->dev, "<%s> ep%d_stat %08x\n", __func__, i,
+			__raw_readl(imx_usb->base + USB_EP_STAT(i)));
+	}
+}
+
+void imx_udc_init_fifo(struct imx_udc_struct *imx_usb)
+{
+	int i, temp;
+	struct imx_ep_struct *imx_ep;
+	for (i = 0; i < IMX_USB_NB_EP; i++) {
+		imx_ep = &imx_usb->imx_ep[i];
+
+		/* Fifo control */
+		temp = EP_DIR(imx_ep) ? 0x0B000000 : 0x0F000000;
+		__raw_writel(temp, imx_usb->base + USB_EP_FCTRL(i));
+		D_INI(imx_usb->dev, "<%s> ep%d_fctrl %08x\n", __func__, i,
+			__raw_readl(imx_usb->base + USB_EP_FCTRL(i)));
+
+		/* Fifo alarm */
+		temp = (i ? imx_ep->fifosize / 2 : 0);
+		__raw_writel(temp, imx_usb->base + USB_EP_FALRM(i));
+		D_INI(imx_usb->dev, "<%s> ep%d_falrm %08x\n", __func__, i,
+			__raw_readl(imx_usb->base + USB_EP_FALRM(i)));
+	}
+}
+
+static void imx_udc_init(struct imx_udc_struct *imx_usb)
+{
+	/* Reset UDC */
+	imx_udc_reset(imx_usb);
+
+	/* Download config to enpoint buffer */
+	imx_udc_config(imx_usb);
+
+	/* Setup interrups */
+	imx_udc_init_irq(imx_usb);
+
+	/* Setup endpoints */
+	imx_udc_init_ep(imx_usb);
+
+	/* Setup fifos */
+	imx_udc_init_fifo(imx_usb);
+}
+
+void imx_ep_irq_enable(struct imx_ep_struct *imx_ep)
+{
+
+	int i = EP_NO(imx_ep);
+
+	__raw_writel(0x1FF, imx_ep->imx_usb->base + USB_EP_MASK(i));
+	__raw_writel(0x1FF, imx_ep->imx_usb->base + USB_EP_INTR(i));
+	__raw_writel(0x1FF & ~(EPINTR_EOT | EPINTR_EOF),
+		imx_ep->imx_usb->base + USB_EP_MASK(i));
+}
+
+void imx_ep_irq_disable(struct imx_ep_struct *imx_ep)
+{
+
+	int i = EP_NO(imx_ep);
+
+	__raw_writel(0x1FF, imx_ep->imx_usb->base + USB_EP_MASK(i));
+	__raw_writel(0x1FF, imx_ep->imx_usb->base + USB_EP_INTR(i));
+}
+
+int imx_ep_empty(struct imx_ep_struct *imx_ep)
+{
+	struct imx_udc_struct *imx_usb = imx_ep->imx_usb;
+
+	return __raw_readl(imx_usb->base + USB_EP_FSTAT(EP_NO(imx_ep)))
+			& FSTAT_EMPTY;
+}
+
+unsigned imx_fifo_bcount(struct imx_ep_struct *imx_ep)
+{
+	struct imx_udc_struct *imx_usb = imx_ep->imx_usb;
+
+	return (__raw_readl(imx_usb->base + USB_EP_STAT(EP_NO(imx_ep)))
+			& EPSTAT_BCOUNT) >> 16;
+}
+
+void imx_flush(struct imx_ep_struct *imx_ep)
+{
+	struct imx_udc_struct *imx_usb = imx_ep->imx_usb;
+
+	int temp = __raw_readl(imx_usb->base + USB_EP_STAT(EP_NO(imx_ep)));
+	__raw_writel(temp | EPSTAT_FLUSH,
+		imx_usb->base + USB_EP_STAT(EP_NO(imx_ep)));
+}
+
+void imx_ep_stall(struct imx_ep_struct *imx_ep)
+{
+	struct imx_udc_struct *imx_usb = imx_ep->imx_usb;
+	int temp, i;
+
+	D_ERR(imx_usb->dev, "<%s> Forced stall on %s\n", __func__, imx_ep->ep.name);
+
+	imx_flush(imx_ep);
+
+	/* Special care for ep0 */
+	if (EP_NO(imx_ep)) {
+		temp = __raw_readl(imx_usb->base + USB_CTRL);
+		__raw_writel(temp | CTRL_CMDOVER | CTRL_CMDERROR, imx_usb->base + USB_CTRL);
+		do { } while (__raw_readl(imx_usb->base + USB_CTRL) & CTRL_CMDOVER);
+		temp = __raw_readl(imx_usb->base + USB_CTRL);
+		__raw_writel(temp & ~CTRL_CMDERROR, imx_usb->base + USB_CTRL);
+	}
+	else {
+		temp = __raw_readl(imx_usb->base + USB_EP_STAT(EP_NO(imx_ep)));
+		__raw_writel(temp | EPSTAT_STALL,
+			imx_usb->base + USB_EP_STAT(EP_NO(imx_ep)));
+
+		for (i = 0; i < 100; i ++) {
+			temp = __raw_readl(imx_usb->base + USB_EP_STAT(EP_NO(imx_ep)));
+	 		if (!temp & EPSTAT_STALL)
+	 			break;
+	 		udelay(20);
+	 	}
+		if (i == 50)
+			D_ERR(imx_usb->dev, "<%s> Non finished stall on %s\n",
+				__func__, imx_ep->ep.name);
+	}
+}
+
+static int imx_udc_get_frame(struct usb_gadget *_gadget)
+{
+	struct imx_udc_struct *imx_usb = container_of(_gadget,
+			struct imx_udc_struct, gadget);
+
+	return __raw_readl(imx_usb->base + USB_FRAME) & 0x7FF;
+}
+
+static int imx_udc_wakeup(struct usb_gadget *_gadget)
+{
+	return 0;
+}
+
+/*******************************************************************************
+ * USB request control functions
+ *******************************************************************************
+ */
+
+static void ep_add_request(struct imx_ep_struct *imx_ep, struct imx_request *req)
+{
+	if (unlikely(!req))
+		return;
+
+	req->in_use = 1;
+	list_add_tail(&req->queue, &imx_ep->queue);
+}
+
+static void ep_del_request(struct imx_ep_struct *imx_ep, struct imx_request *req)
+{
+	if (unlikely(!req))
+		return;
+
+	list_del_init(&req->queue);
+	req->in_use = 0;
+}
+
+static void done(struct imx_ep_struct *imx_ep, struct imx_request *req, int status)
+{
+	ep_del_request(imx_ep, req);
+
+	if (likely(req->req.status == -EINPROGRESS))
+		req->req.status = status;
+	else
+		status = req->req.status;
+
+	if (status && status != -ESHUTDOWN)
+		D_ERR(imx_ep->imx_usb->dev,
+			"<%s> complete %s req %p stat %d len %u/%u\n", __func__,
+			imx_ep->ep.name, &req->req, status,
+			req->req.actual, req->req.length);
+
+	req->req.complete(&imx_ep->ep, &req->req);
+}
+
+static void nuke(struct imx_ep_struct *imx_ep, int status)
+{
+	struct imx_request *req;
+
+	while (!list_empty(&imx_ep->queue)) {
+		req = list_entry(imx_ep->queue.next, struct imx_request, queue);
+		done(imx_ep, req, status);
+	}
+}
+
+/*******************************************************************************
+ * Data tansfer over USB functions
+ *******************************************************************************
+ */
+static int read_packet(struct imx_ep_struct *imx_ep, struct imx_request *req)
+{
+	u8	*buf;
+	int	bytes_ep, bufferspace, count, i;
+
+	bytes_ep = imx_fifo_bcount(imx_ep);
+	bufferspace = req->req.length - req->req.actual;
+
+	buf = req->req.buf + req->req.actual;
+	prefetchw(buf);
+
+	if (unlikely(imx_ep_empty(imx_ep)))
+		count = 0;	/* zlp */
+	else
+		count = min(bytes_ep, bufferspace);
+
+	for (i = count; i > 0; i--)
+		*buf++ = __raw_readb(imx_ep->imx_usb->base
+						+ USB_EP_FDAT0(EP_NO(imx_ep)));
+	req->req.actual += count;
+
+	return count;
+}
+
+static int write_packet(struct imx_ep_struct *imx_ep, struct imx_request *req)
+{
+	u8	*buf;
+	int	length, count, temp;
+
+	buf = req->req.buf + req->req.actual;
+	prefetch(buf);
+
+	length = min(req->req.length - req->req.actual, (u32)imx_ep->fifosize);
+
+	if (imx_fifo_bcount(imx_ep) + length > imx_ep->fifosize) {
+		D_TRX(imx_ep->imx_usb->dev, "<%s> packet overfill %s fifo\n",
+			__func__, imx_ep->ep.name);
+		return -1;
+	}
+
+	req->req.actual += length;
+	count = length;
+
+	if (!count && req->req.zero) {	/* zlp */
+		temp = __raw_readl(imx_ep->imx_usb->base
+			+ USB_EP_STAT(EP_NO(imx_ep)));
+		__raw_writel(temp | EPSTAT_ZLPS, imx_ep->imx_usb->base
+			+ USB_EP_STAT(EP_NO(imx_ep)));
+		D_TRX(imx_ep->imx_usb->dev, "<%s> zero packet\n", __func__);
+		return 0;
+	}
+
+	while (count--) {
+		if (count == 0) {	/* last byte */
+			temp = __raw_readl(imx_ep->imx_usb->base
+				+ USB_EP_FCTRL(EP_NO(imx_ep)));
+			__raw_writel(temp | FCTRL_WFR, imx_ep->imx_usb->base
+				+ USB_EP_FCTRL(EP_NO(imx_ep)));
+		}
+		__raw_writeb(*buf++,
+			imx_ep->imx_usb->base + USB_EP_FDAT0(EP_NO(imx_ep)));
+	}
+
+	return length;
+}
+
+static int read_fifo(struct imx_ep_struct *imx_ep, struct imx_request *req)
+{
+	int 	bytes = 0,
+		count,
+		completed = 0;
+
+	while (__raw_readl(imx_ep->imx_usb->base + USB_EP_FSTAT(EP_NO(imx_ep)))
+		& FSTAT_FR) {
+			count = read_packet(imx_ep, req);
+			bytes += count;
+
+			completed = (count != imx_ep->fifosize);
+			if (completed || req->req.actual == req->req.length) {
+				completed = 1;
+				break;
+			}
+	}
+
+	if (completed || !req->req.length) {
+		done(imx_ep, req, 0);
+		D_REQ(imx_ep->imx_usb->dev, "<%s> %s req<%p> %s\n",
+			__func__, imx_ep->ep.name, req,
+			completed ? "completed" : "not completed");
+		if (!EP_NO(imx_ep))
+			ep0_chg_stat(__func__, imx_ep->imx_usb, EP0_IDLE);
+	}
+
+	D_TRX(imx_ep->imx_usb->dev, "<%s> bytes read: %d\n", __func__, bytes);
+
+	return completed;
+}
+
+static int write_fifo(struct imx_ep_struct *imx_ep, struct imx_request *req)
+{
+	int	bytes = 0,
+		count,
+		completed = 0;
+
+	while (!completed) {
+		count = write_packet(imx_ep, req);
+		if (count < 0)
+			break; /* busy */
+		bytes += count;
+
+		/* last packet "must be" short (or a zlp) */
+		completed = (count != imx_ep->fifosize);
+
+		if (unlikely(completed)) {
+			done(imx_ep, req, 0);
+			D_REQ(imx_ep->imx_usb->dev, "<%s> %s req<%p> %s\n",
+				__func__, imx_ep->ep.name, req,
+				completed ? "completed" : "not completed");
+			if (!EP_NO(imx_ep))
+				ep0_chg_stat(__func__, imx_ep->imx_usb, EP0_IDLE);
+		}
+	}
+
+	D_TRX(imx_ep->imx_usb->dev, "<%s> bytes sent: %d\n", __func__, bytes);
+
+	return completed;
+}
+
+/*******************************************************************************
+ * Endpoint handlers
+ *******************************************************************************
+ */
+static int handle_ep(struct imx_ep_struct *imx_ep)
+{
+	struct imx_request *req;
+	int completed = 0;
+
+	do {
+		if (!list_empty(&imx_ep->queue))
+			req = list_entry(imx_ep->queue.next,
+				struct imx_request, queue);
+		else {
+			D_REQ(imx_ep->imx_usb->dev, "<%s> no request on %s\n",
+				__func__, imx_ep->ep.name);
+			return 0;
+		}
+
+		if (EP_DIR(imx_ep))	/* to host */
+			completed = write_fifo(imx_ep, req);
+		else			/* to device */
+			completed = read_fifo(imx_ep, req);
+
+		dump_ep_stat(__func__, imx_ep);
+
+	} while (completed);
+
+	return 0;
+}
+
+static int handle_ep0(struct imx_ep_struct *imx_ep)
+{
+	struct imx_request *req = NULL;
+	int ret = 0;
+
+	if (!list_empty(&imx_ep->queue))
+		req = list_entry(imx_ep->queue.next, struct imx_request, queue);
+
+	if (req) {
+		switch (imx_ep->imx_usb->ep0state) {
+
+		case EP0_IN_DATA_PHASE:			/* GET_DESCRIPTOR */
+			write_fifo(imx_ep, req);
+			break;
+		case EP0_OUT_DATA_PHASE:		/* SET_DESCRIPTOR */
+			read_fifo(imx_ep, req);
+			break;
+		default:
+			D_EP0(imx_ep->imx_usb->dev,
+				"<%s> ep0 i/o, odd state %d\n",
+				__func__, imx_ep->imx_usb->ep0state);
+			ep_del_request(imx_ep, req);
+			ret = -EL2HLT;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static void handle_ep0_devreq(struct imx_udc_struct *imx_usb)
+{
+	struct imx_ep_struct *imx_ep = &imx_usb->imx_ep[0];
+	union {
+		struct usb_ctrlrequest	r;
+		u8			raw[8];
+		u32			word[2];
+	} u;
+	int temp, i;
+
+	nuke(imx_ep, -EPROTO);
+
+	/* read SETUP packet */
+	for (i = 0; i < 2; i++) {
+		if (imx_ep_empty(imx_ep)) {
+			D_ERR(imx_usb->dev,
+				"<%s> no setup packet received\n", __func__);
+			goto stall;
+		}
+		u.word[i] = __raw_readl(imx_usb->base +	USB_EP_FDAT(EP_NO(imx_ep)));
+	}
+
+	temp = imx_ep_empty(imx_ep);
+	while (!imx_ep_empty(imx_ep)) {
+		i = __raw_readl(imx_usb->base +	USB_EP_FDAT(EP_NO(imx_ep)));
+		D_ERR(imx_usb->dev,
+			"<%s> wrong to have extra bytes for setup : 0x%08x\n",
+			__func__, i);
+	}
+	if (!temp)
+		goto stall;
+
+	le16_to_cpus(&u.r.wValue);
+	le16_to_cpus(&u.r.wIndex);
+	le16_to_cpus(&u.r.wLength);
+
+	D_REQ(imx_usb->dev, "<%s> SETUP %02x.%02x v%04x i%04x l%04x\n",
+		__func__, u.r.bRequestType, u.r.bRequest,
+		u.r.wValue, u.r.wIndex, u.r.wLength);
+
+	if (imx_usb->set_config) {
+		/* NACK the host by using CMDOVER */
+		temp = __raw_readl(imx_usb->base + USB_CTRL);
+		__raw_writel(temp | CTRL_CMDOVER, imx_usb->base + USB_CTRL);
+
+		D_ERR(imx_usb->dev,
+			"<%s> set config req is pending, NACK the host\n",
+			__func__);
+		return;
+	}
+
+	if (u.r.bRequestType & USB_DIR_IN)
+		ep0_chg_stat(__func__, imx_usb, EP0_IN_DATA_PHASE);
+	else
+		ep0_chg_stat(__func__, imx_usb, EP0_OUT_DATA_PHASE);
+
+	i = imx_usb->driver->setup(&imx_usb->gadget, &u.r);
+	if (i < 0) {
+		D_ERR(imx_usb->dev, "<%s> device setup error %d\n",
+			__func__, i);
+		goto stall;
+	}
+
+	return;
+stall:
+	D_ERR(imx_usb->dev, "<%s> protocol STALL\n", __func__);
+	imx_ep_stall(imx_ep);
+	ep0_chg_stat(__func__, imx_usb, EP0_STALL);
+	return;
+}
+
+/*******************************************************************************
+ * USB gadget callback functions
+ *******************************************************************************
+ */
+
+static int imx_ep_enable(struct usb_ep *usb_ep,
+				const struct usb_endpoint_descriptor *desc)
+{
+	struct imx_ep_struct *imx_ep = container_of(usb_ep,
+						struct imx_ep_struct, ep);
+	struct imx_udc_struct *imx_usb = imx_ep->imx_usb;
+	unsigned long flags;
+
+	if (!usb_ep
+		|| !desc
+		|| !EP_NO(imx_ep)
+		|| desc->bDescriptorType != USB_DT_ENDPOINT
+		|| imx_ep->bEndpointAddress != desc->bEndpointAddress) {
+			D_ERR(imx_usb->dev,
+				"<%s> bad ep or descriptor\n", __func__);
+			return -EINVAL;
+	}
+
+	if (imx_ep->bmAttributes != desc->bmAttributes) {
+		D_ERR(imx_usb->dev,
+			"<%s> %s type mismatch\n", __func__, usb_ep->name);
+		return -EINVAL;
+	}
+
+	if (imx_ep->fifosize < le16_to_cpu(desc->wMaxPacketSize)) {
+		D_ERR(imx_usb->dev,
+			"<%s> bad %s maxpacket\n", __func__, usb_ep->name);
+		return -ERANGE;
+	}
+
+	if (!imx_usb->driver || imx_usb->gadget.speed == USB_SPEED_UNKNOWN) {
+		D_ERR(imx_usb->dev, "<%s> bogus device state\n", __func__);
+		return -ESHUTDOWN;
+	}
+
+	local_irq_save(flags);
+
+	imx_ep->stopped = 0;
+	imx_flush(imx_ep);
+	imx_ep_irq_enable(imx_ep);
+
+	local_irq_restore(flags);
+
+	D_EPX(imx_usb->dev, "<%s> ENABLED %s\n", __func__, usb_ep->name);
+	return 0;
+}
+
+static int imx_ep_disable(struct usb_ep *usb_ep)
+{
+	struct imx_ep_struct *imx_ep = container_of(usb_ep,
+						struct imx_ep_struct, ep);
+	unsigned long flags;
+
+	if (!usb_ep || !EP_NO(imx_ep) || !list_empty(&imx_ep->queue)) {
+		D_ERR(imx_ep->imx_usb->dev, "<%s> %s can not be disabled\n",
+			__func__, usb_ep ? imx_ep->ep.name : NULL);
+		return -EINVAL;
+	}
+
+	local_irq_save(flags);
+
+	imx_ep->stopped = 1;
+	nuke(imx_ep, -ESHUTDOWN);
+	imx_flush(imx_ep);
+	imx_ep_irq_disable(imx_ep);
+
+	local_irq_restore(flags);
+
+	D_EPX(imx_ep->imx_usb->dev,
+		"<%s> DISABLED %s\n", __func__, usb_ep->name);
+	return 0;
+}
+
+static struct usb_request *imx_ep_alloc_request
+					(struct usb_ep *usb_ep, gfp_t gfp_flags)
+{
+	struct imx_request *req;
+
+	req = kzalloc(sizeof *req, gfp_flags);
+	if (!req || !usb_ep)
+		return 0;
+
+	INIT_LIST_HEAD(&req->queue);
+	req->in_use = 0;
+
+	return &req->req;
+}
+
+static void imx_ep_free_request
+			(struct usb_ep *usb_ep, struct usb_request *usb_req)
+{
+	struct imx_request *req;
+
+	req = container_of(usb_req, struct imx_request, req);
+	WARN_ON(!list_empty(&req->queue));
+	kfree(req);
+}
+
+static int imx_ep_queue
+	(struct usb_ep *usb_ep, struct usb_request *usb_req, gfp_t gfp_flags)
+{
+	struct imx_ep_struct	*imx_ep;
+	struct imx_udc_struct	*imx_usb;
+	struct imx_request	*req;
+	unsigned long		flags;
+	int			ret = 0;
+
+	imx_ep = container_of(usb_ep, struct imx_ep_struct, ep);
+	imx_usb = imx_ep->imx_usb;
+	req = container_of(usb_req, struct imx_request, req);
+
+	/*
+	  Special care on IMX udc.
+	  Ignore enqueue when after set configuration from the
+	  host. This assume all gadget drivers reply set
+	  configuration with the next ep0 req enqueue.
+	*/
+	if (imx_usb->set_config && !EP_NO(imx_ep)) {
+		imx_usb->set_config = 0;
+		D_EPX(imx_usb->dev,
+			"<%s> gadget reply set config\n", __func__);
+		return 0;
+	}
+
+	if (unlikely(!usb_req || !req || !usb_req->complete || !usb_req->buf)) {
+		D_ERR(imx_usb->dev, "<%s> bad params\n", __func__);
+		return -EINVAL;
+	}
+
+	if (unlikely(!usb_ep || !imx_ep)) {
+		D_ERR(imx_usb->dev, "<%s> bad ep\n", __func__);
+		return -EINVAL;
+	}
+
+	if (!imx_usb->driver || imx_usb->gadget.speed == USB_SPEED_UNKNOWN) {
+		D_ERR(imx_usb->dev, "<%s> bogus device state\n", __func__);
+		return -ESHUTDOWN;
+	}
+
+	local_irq_save(flags);
+
+	/* Debug */
+	D_REQ(imx_usb->dev, "<%s> ep%d %s request for [%d] bytes\n",
+		__func__, EP_NO(imx_ep),
+		((!EP_NO(imx_ep) && imx_ep->imx_usb->ep0state == EP0_IN_DATA_PHASE)
+		|| (EP_NO(imx_ep) && EP_DIR(imx_ep))) ? "IN" : "OUT", usb_req->length);
+	dump_req(__func__, imx_ep, usb_req);
+
+	if (imx_ep->stopped) {
+		usb_req->status = -ESHUTDOWN;
+		ret = -ESHUTDOWN;
+		goto out;
+	}
+
+	if (req->in_use) {
+		D_ERR(imx_usb->dev,
+			"<%s> refusing to queue req %p (already queued)\n",
+			__func__, req);
+		goto out;
+	}
+
+	usb_req->status = -EINPROGRESS;
+	usb_req->actual = 0;
+
+	ep_add_request(imx_ep, req);
+
+	if (!EP_NO(imx_ep))
+		ret = handle_ep0(imx_ep);
+	else
+		ret = handle_ep(imx_ep);
+out:
+	local_irq_restore(flags);
+	return ret;
+}
+
+static int imx_ep_dequeue(struct usb_ep *usb_ep, struct usb_request *usb_req)
+{
+
+	struct imx_ep_struct *imx_ep = container_of
+					(usb_ep, struct imx_ep_struct, ep);
+	struct imx_request *req;
+	unsigned long flags;
+
+	if (unlikely(!usb_ep || !EP_NO(imx_ep))) {
+		D_ERR(imx_ep->imx_usb->dev, "<%s> bad ep\n", __func__);
+		return -EINVAL;
+	}
+
+	local_irq_save(flags);
+
+	/* make sure it's actually queued on this endpoint */
+	list_for_each_entry(req, &imx_ep->queue, queue) {
+		if (&req->req == usb_req)
+			break;
+	}
+	if (&req->req != usb_req) {
+		local_irq_restore(flags);
+		return -EINVAL;
+	}
+
+	done(imx_ep, req, -ECONNRESET);
+
+	local_irq_restore(flags);
+	return 0;
+}
+
+static int imx_ep_set_halt(struct usb_ep *usb_ep, int value)
+{
+	struct imx_ep_struct *imx_ep = container_of
+					(usb_ep, struct imx_ep_struct, ep);
+	unsigned long flags;
+
+	if (unlikely(!usb_ep || !EP_NO(imx_ep))) {
+		D_ERR(imx_ep->imx_usb->dev, "<%s> bad ep\n", __func__);
+		return -EINVAL;
+	}
+
+	local_irq_save(flags);
+
+	if ((imx_ep->bEndpointAddress & USB_DIR_IN)
+		&& !list_empty(&imx_ep->queue)) {
+			local_irq_restore(flags);
+			return -EAGAIN;
+	}
+
+	imx_ep_stall(imx_ep);
+
+	local_irq_restore(flags);
+
+	D_EPX(imx_ep->imx_usb->dev, "<%s> %s halt\n", __func__, usb_ep->name);
+	return 0;
+}
+
+static int imx_ep_fifo_status(struct usb_ep *usb_ep)
+{
+	struct imx_ep_struct *imx_ep = container_of
+					(usb_ep, struct imx_ep_struct, ep);
+
+	if (!usb_ep) {
+		D_ERR(imx_ep->imx_usb->dev, "<%s> bad ep\n", __func__);
+		return -ENODEV;
+	}
+
+	if (imx_ep->imx_usb->gadget.speed == USB_SPEED_UNKNOWN)
+		return 0;
+	else
+		return imx_fifo_bcount(imx_ep);
+}
+
+static void imx_ep_fifo_flush(struct usb_ep *usb_ep)
+{
+	struct imx_ep_struct *imx_ep = container_of
+					(usb_ep, struct imx_ep_struct, ep);
+	unsigned long flags;
+
+	local_irq_save(flags);
+
+	if (!usb_ep || !EP_NO(imx_ep) || !list_empty(&imx_ep->queue)) {
+		D_ERR(imx_ep->imx_usb->dev, "<%s> bad ep\n", __func__);
+		local_irq_restore(flags);
+		return;
+	}
+
+	/* toggle and halt bits stay unchanged */
+	imx_flush(imx_ep);
+
+	local_irq_restore(flags);
+}
+
+static struct usb_ep_ops imx_ep_ops = {
+	.enable		= imx_ep_enable,
+	.disable	= imx_ep_disable,
+
+	.alloc_request	= imx_ep_alloc_request,
+	.free_request	= imx_ep_free_request,
+
+	.queue		= imx_ep_queue,
+	.dequeue	= imx_ep_dequeue,
+
+	.set_halt	= imx_ep_set_halt,
+	.fifo_status	= imx_ep_fifo_status,
+	.fifo_flush	= imx_ep_fifo_flush,
+};
+
+/*******************************************************************************
+ * USB endpoint control functions
+ *******************************************************************************
+ */
+
+void ep0_chg_stat(const char *label,
+			struct imx_udc_struct *imx_usb, enum ep0_state stat)
+{
+	D_EP0(imx_usb->dev, "<%s> from %15s to %15s\n",
+		label, state_name[imx_usb->ep0state], state_name[stat]);
+
+	if (imx_usb->ep0state == stat)
+		return;
+
+	imx_usb->ep0state = stat;
+}
+
+static void usb_init_data(struct imx_udc_struct *imx_usb)
+{
+	struct imx_ep_struct *imx_ep;
+	u8 i;
+
+	/* device/ep0 records init */
+	INIT_LIST_HEAD(&imx_usb->gadget.ep_list);
+	INIT_LIST_HEAD(&imx_usb->gadget.ep0->ep_list);
+	ep0_chg_stat(__func__, imx_usb, EP0_IDLE);
+
+	/* basic endpoint records init */
+	for (i = 0; i < IMX_USB_NB_EP; i++) {
+		imx_ep = &imx_usb->imx_ep[i];
+
+		if (i) {
+			list_add_tail(&imx_ep->ep.ep_list,
+				&imx_usb->gadget.ep_list);
+			imx_ep->stopped = 1;
+		} else
+			imx_ep->stopped = 0;
+
+		INIT_LIST_HEAD(&imx_ep->queue);
+	}
+}
+
+static void udc_stop_activity(struct imx_udc_struct *imx_usb,
+					struct usb_gadget_driver *driver)
+{
+	struct imx_ep_struct *imx_ep;
+	int i;
+
+	if (imx_usb->gadget.speed == USB_SPEED_UNKNOWN)
+		driver = NULL;
+
+	/* prevent new request submissions, kill any outstanding requests  */
+	for (i = 1; i < IMX_USB_NB_EP; i++) {
+		imx_ep = &imx_usb->imx_ep[i];
+		imx_flush(imx_ep);
+		imx_ep->stopped = 1;
+		imx_ep_irq_disable(imx_ep);
+		nuke(imx_ep, -ESHUTDOWN);
+	}
+
+	imx_usb->cfg = 0;
+	imx_usb->intf = 0;
+	imx_usb->alt = 0;
+
+	if (driver)
+		driver->disconnect(&imx_usb->gadget);
+}
+
+/*******************************************************************************
+ * Interrupt handlers
+ *******************************************************************************
+ */
+
+static irqreturn_t imx_udc_irq(int irq, void *dev)
+{
+	struct imx_udc_struct *imx_usb = dev;
+	struct usb_ctrlrequest u;
+	int temp, cfg, intf, alt;
+	int intr = __raw_readl(imx_usb->base + USB_INTR);
+
+	if (intr & (INTR_WAKEUP | INTR_SUSPEND | INTR_RESUME | INTR_RESET_START
+			| INTR_RESET_STOP | INTR_CFG_CHG)) {
+				dump_intr(__func__, intr, imx_usb->dev);
+				dump_usb_stat(__func__, imx_usb);
+	}
+
+	if (!imx_usb->driver) {
+		/*imx_udc_disable(imx_usb);*/
+		goto end_irq;
+	}
+
+	if (intr & INTR_WAKEUP) {
+		if (imx_usb->gadget.speed == USB_SPEED_UNKNOWN
+			&& imx_usb->driver && imx_usb->driver->resume)
+				imx_usb->driver->resume(&imx_usb->gadget);
+		imx_usb->set_config = 0;
+		imx_usb->gadget.speed = USB_SPEED_FULL;
+	}
+
+	if (intr & INTR_SUSPEND) {
+		if (imx_usb->gadget.speed != USB_SPEED_UNKNOWN
+			&& imx_usb->driver && imx_usb->driver->suspend)
+				imx_usb->driver->suspend(&imx_usb->gadget);
+		imx_usb->set_config = 0;
+		imx_usb->gadget.speed = USB_SPEED_UNKNOWN;
+	}
+
+	if (intr & INTR_RESET_START) {
+		__raw_writel(intr, imx_usb->base + USB_INTR);
+		udc_stop_activity(imx_usb, imx_usb->driver);
+		imx_usb->set_config = 0;
+		imx_usb->gadget.speed = USB_SPEED_UNKNOWN;
+	}
+
+	if (intr & INTR_RESET_STOP)
+		imx_usb->gadget.speed = USB_SPEED_FULL;
+
+	if (intr & INTR_CFG_CHG) {
+		__raw_writel(INTR_CFG_CHG, imx_usb->base + USB_INTR);
+		temp = __raw_readl(imx_usb->base + USB_STAT);
+		cfg  = (temp & STAT_CFG) >> 5;
+		intf = (temp & STAT_INTF) >> 3;
+		alt  =  temp & STAT_ALTSET;
+
+		D_REQ(imx_usb->dev,
+			"<%s> orig config C=%d, I=%d, A=%d / "
+			"req config C=%d, I=%d, A=%d\n",
+			__func__, imx_usb->cfg, imx_usb->intf, imx_usb->alt,
+			cfg, intf, alt);
+
+		if (cfg != 1 && cfg != 2)
+			goto end_irq;
+
+		imx_usb->set_config = 0;
+
+		/* Config setup */
+		if (imx_usb->cfg != cfg) {
+			D_REQ(imx_usb->dev, "<%s> Change config start\n",__func__);
+			u.bRequest = USB_REQ_SET_CONFIGURATION;
+			u.bRequestType = USB_DIR_OUT |
+					USB_TYPE_STANDARD |
+					USB_RECIP_DEVICE;
+			u.wValue = cfg;
+			u.wIndex = 0;
+			u.wLength = 0;
+			imx_usb->cfg = cfg;
+			imx_usb->set_config = 1;
+			imx_usb->driver->setup(&imx_usb->gadget, &u);
+			imx_usb->set_config = 0;
+			D_REQ(imx_usb->dev, "<%s> Change config done\n",__func__);
+
+		}
+		if (imx_usb->intf != intf || imx_usb->alt != alt) {
+			D_REQ(imx_usb->dev, "<%s> Change interface start\n",__func__);
+			u.bRequest = USB_REQ_SET_INTERFACE;
+			u.bRequestType = USB_DIR_OUT |
+					  USB_TYPE_STANDARD |
+					  USB_RECIP_INTERFACE;
+			u.wValue = alt;
+			u.wIndex = intf;
+			u.wLength = 0;
+			imx_usb->intf = intf;
+			imx_usb->alt = alt;
+			imx_usb->set_config = 1;
+			imx_usb->driver->setup(&imx_usb->gadget, &u);
+			imx_usb->set_config = 0;
+			D_REQ(imx_usb->dev, "<%s> Change interface done\n",__func__);
+		}
+	}
+
+	if (intr & INTR_SOF) {
+		if (imx_usb->ep0state == EP0_IDLE) {
+			temp = __raw_readl(imx_usb->base + USB_CTRL);
+			__raw_writel(temp | CTRL_CMDOVER, imx_usb->base + USB_CTRL);
+		}
+	}
+
+end_irq:
+	__raw_writel(intr, imx_usb->base + USB_INTR);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t imx_udc_ctrl_irq(int irq, void *dev)
+{
+	struct imx_udc_struct *imx_usb = dev;
+	int intr = __raw_readl(imx_usb->base + USB_EP_INTR(0));
+
+	dump_ep_intr(__func__, 0, intr, imx_usb->dev);
+
+	if (!imx_usb->driver) {
+		__raw_writel(intr, imx_usb->base + USB_EP_INTR(0));
+		return IRQ_HANDLED;
+	}
+
+	/* DEVREQ IRQ has highest priority */
+	if (intr & (EPINTR_DEVREQ | EPINTR_MDEVREQ))
+		handle_ep0_devreq(imx_usb);
+	/* Seem i.MX is missing EOF interrupt sometimes.
+	 * Therefore we monitor both EOF and FIFO_EMPTY interrups
+	 * when transmiting, and both EOF and FIFO_FULL when
+	 * receiving data.
+	 */
+	else if (intr & (EPINTR_EOF | EPINTR_FIFO_EMPTY | EPINTR_FIFO_FULL))
+		handle_ep0(&imx_usb->imx_ep[0]);
+
+	__raw_writel(intr, imx_usb->base + USB_EP_INTR(0));
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t imx_udc_bulk_irq(int irq, void *dev)
+{
+	struct imx_udc_struct *imx_usb = dev;
+	struct imx_ep_struct *imx_ep = &imx_usb->imx_ep[irq - USBD_INT0];
+	int intr = __raw_readl(imx_usb->base + USB_EP_INTR(EP_NO(imx_ep)));
+
+	dump_ep_intr(__func__, irq - USBD_INT0, intr, imx_usb->dev);
+
+	if (!imx_usb->driver) {
+		__raw_writel(intr, imx_usb->base + USB_EP_INTR(EP_NO(imx_ep)));
+		return IRQ_HANDLED;
+	}
+
+	handle_ep(imx_ep);
+
+	__raw_writel(intr, imx_usb->base + USB_EP_INTR(EP_NO(imx_ep)));
+
+	return IRQ_HANDLED;
+}
+
+irq_handler_t intr_handler(int i)
+{
+	switch (i) {
+	case 0:
+		return imx_udc_ctrl_irq;
+	case 1:
+	case 2:
+	case 3:
+	case 4:
+	case 5:
+		return imx_udc_bulk_irq;
+	default:
+		return imx_udc_irq;
+	}
+}
+
+/*******************************************************************************
+ * Static defined IMX UDC structure
+ *******************************************************************************
+ */
+
+static const struct usb_gadget_ops imx_udc_ops = {
+	.get_frame	 = imx_udc_get_frame,
+	.wakeup		 = imx_udc_wakeup,
+};
+
+static struct imx_udc_struct controller = {
+	.gadget = {
+		.ops		= &imx_udc_ops,
+		.ep0		= &controller.imx_ep[0].ep,
+		.name		= driver_name,
+		.dev = {
+			 .bus_id	= "gadget",
+		 },
+	},
+
+	.imx_ep[0] = {
+		.ep = {
+			.name		= ep0name,
+			.ops		= &imx_ep_ops,
+			.maxpacket	= 32,
+		},
+		.imx_usb		= &controller,
+		.fifosize		= 32,
+		.bEndpointAddress	= 0,
+		.bmAttributes		= USB_ENDPOINT_XFER_CONTROL,
+	 },
+	.imx_ep[1] = {
+		.ep = {
+			.name		= "ep1in-bulk",
+			.ops		= &imx_ep_ops,
+			.maxpacket	= 64,
+		},
+		.imx_usb		= &controller,
+		.fifosize		= 64,
+		.bEndpointAddress	= USB_DIR_IN | 1,
+		.bmAttributes		= USB_ENDPOINT_XFER_BULK,
+	 },
+	.imx_ep[2] = {
+		.ep = {
+			.name		= "ep2out-bulk",
+			.ops		= &imx_ep_ops,
+			.maxpacket	= 64,
+		},
+		.imx_usb		= &controller,
+		.fifosize		= 64,
+		.bEndpointAddress	= USB_DIR_OUT | 2,
+		.bmAttributes		= USB_ENDPOINT_XFER_BULK,
+	 },
+	.imx_ep[3] = {
+		.ep = {
+			.name		= "ep3out-bulk",
+			.ops		= &imx_ep_ops,
+			.maxpacket	= 32,
+		},
+		.imx_usb		= &controller,
+		.fifosize		= 32,
+		.bEndpointAddress 	= USB_DIR_OUT | 3,
+		.bmAttributes		= USB_ENDPOINT_XFER_BULK,
+	 },
+	.imx_ep[4] = {
+		.ep = {
+			.name		= "ep4in-int",
+			.ops		= &imx_ep_ops,
+			.maxpacket	= 32,
+		 },
+		.imx_usb		= &controller,
+		.fifosize		= 32,
+		.bEndpointAddress 	= USB_DIR_IN | 4,
+		.bmAttributes		= USB_ENDPOINT_XFER_INT,
+	 },
+	.imx_ep[5] = {
+		.ep = {
+			.name		= "ep5out-int",
+			.ops		= &imx_ep_ops,
+			.maxpacket	= 32,
+		},
+		.imx_usb		= &controller,
+		.fifosize		= 32,
+		.bEndpointAddress 	= USB_DIR_OUT | 5,
+		.bmAttributes		= USB_ENDPOINT_XFER_INT,
+	 },
+};
+
+/*******************************************************************************
+ * USB gadged driver functions
+ *******************************************************************************
+ */
+int usb_gadget_register_driver(struct usb_gadget_driver *driver)
+{
+	struct imx_udc_struct *imx_usb = &controller;
+	int retval;
+
+	if (!driver
+		|| driver->speed < USB_SPEED_FULL
+		|| !driver->bind
+		|| !driver->disconnect
+		|| !driver->setup)
+			return -EINVAL;
+	if (!imx_usb)
+		return -ENODEV;
+	if (imx_usb->driver)
+		return -EBUSY;
+
+	/* first hook up the driver ... */
+	imx_usb->driver = driver;
+	imx_usb->gadget.dev.driver = &driver->driver;
+
+	retval = device_add(&imx_usb->gadget.dev);
+	if (retval)
+		goto fail;
+	retval = driver->bind(&imx_usb->gadget);
+	if (retval) {
+		D_ERR(imx_usb->dev, "<%s> bind to driver %s --> error %d\n",
+			__func__, driver->driver.name, retval);
+		device_del(&imx_usb->gadget.dev);
+
+		goto fail;
+	}
+
+	D_INI(imx_usb->dev, "<%s> registered gadget driver '%s'\n",
+		__func__, driver->driver.name);
+
+	imx_udc_enable(imx_usb);
+
+	return 0;
+fail:
+	imx_usb->driver = NULL;
+	imx_usb->gadget.dev.driver = NULL;
+	return retval;
+}
+EXPORT_SYMBOL(usb_gadget_register_driver);
+
+int usb_gadget_unregister_driver(struct usb_gadget_driver *driver)
+{
+	struct imx_udc_struct *imx_usb = &controller;
+
+	if (!imx_usb)
+		return -ENODEV;
+	if (!driver || driver != imx_usb->driver || !driver->unbind)
+		return -EINVAL;
+
+	udc_stop_activity(imx_usb, driver);
+	imx_udc_disable(imx_usb);
+
+	driver->unbind(&imx_usb->gadget);
+	imx_usb->gadget.dev.driver = NULL;
+	imx_usb->driver = NULL;
+
+	device_del(&imx_usb->gadget.dev);
+
+	D_INI(imx_usb->dev, "<%s> unregistered gadget driver '%s'\n",
+		__func__, driver->driver.name);
+
+	return 0;
+}
+EXPORT_SYMBOL(usb_gadget_unregister_driver);
+
+/*******************************************************************************
+ * Module functions
+ *******************************************************************************
+ */
+
+static int __init imx_udc_probe(struct platform_device *pdev)
+{
+	struct imx_udc_struct *imx_usb = &controller;
+	struct resource *res;
+	struct imxusb_platform_data *pdata;
+	struct clk *clk;
+	void __iomem *base;
+	int ret = 0;
+	int i, res_size;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "can't get device resources\n");
+		return -ENODEV;
+	}
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata) {
+		dev_err(&pdev->dev, "driver needs platform data\n");
+		return -ENODEV;
+	}
+
+	res_size = res->end - res->start + 1;
+	if (!request_mem_region(res->start, res_size, res->name)) {
+		dev_err(&pdev->dev, "can't allocate %d bytes at %d address\n",
+			res_size, res->start);
+		return -ENOMEM;
+	}
+
+	if (pdata->init) {
+		ret = pdata->init(&pdev->dev);
+		if (ret)
+			goto fail0;
+	}
+
+	base = ioremap(res->start, res_size);
+	if (!base) {
+		dev_err(&pdev->dev, "ioremap failed\n");
+		ret = -EIO;
+		goto fail1;
+	}
+
+	clk = clk_get(NULL, "usbd_clk");
+	if (IS_ERR(clk)) {
+		ret = PTR_ERR(clk);
+		dev_err(&pdev->dev, "can't get USB clock\n");
+		goto fail2;
+	}
+	clk_enable(clk);
+
+	if (clk_get_rate(clk) != 48000000) {
+		D_INI(&pdev->dev,
+			"Bad USB clock (%d Hz), changing to 48000000 Hz\n",
+			(int)clk_get_rate(clk));
+		if (clk_set_rate(clk, 48000000)) {
+			dev_err(&pdev->dev,
+				"Unable to set correct USB clock (48MHz)\n");
+			ret = -EIO;
+			goto fail3;
+		}
+	}
+
+	for (i = 0; i < IMX_USB_NB_EP + 1; i++) {
+		imx_usb->usbd_int[i] = platform_get_irq(pdev, i);
+		if (imx_usb->usbd_int[i] < 0) {
+			dev_err(&pdev->dev, "can't get irq number\n");
+			ret = -ENODEV;
+			goto fail3;
+		}
+	}
+
+	for (i = 0; i < IMX_USB_NB_EP + 1; i++) {
+		ret = request_irq(imx_usb->usbd_int[i], intr_handler(i),
+				     IRQF_DISABLED, driver_name, imx_usb);
+		if (ret) {
+			dev_err(&pdev->dev, "can't get irq %i, err %d\n",
+				imx_usb->usbd_int[i], ret);
+			for (--i; i >= 0; i--)
+				free_irq(imx_usb->usbd_int[i], imx_usb);
+			goto fail3;
+		}
+	}
+
+	imx_usb->res = res;
+	imx_usb->base = base;
+	imx_usb->clk = clk;
+	imx_usb->dev = &pdev->dev;
+
+	device_initialize(&imx_usb->gadget.dev);
+
+	imx_usb->gadget.dev.parent = &pdev->dev;
+	imx_usb->gadget.dev.dma_mask = pdev->dev.dma_mask;
+
+	platform_set_drvdata(pdev, imx_usb);
+
+	usb_init_data(imx_usb);
+	imx_udc_init(imx_usb);
+
+	return 0;
+
+fail3:
+	clk_put(clk);
+	clk_disable(clk);
+fail2:
+	iounmap(base);
+fail1:
+	if (pdata->exit)
+		pdata->exit(&pdev->dev);
+fail0:
+	release_mem_region(res->start, res_size);
+	return ret;
+}
+
+static int __exit imx_udc_remove(struct platform_device *pdev)
+{
+	struct imx_udc_struct *imx_usb = platform_get_drvdata(pdev);
+	struct imxusb_platform_data *pdata = pdev->dev.platform_data;
+	int i;
+
+	imx_udc_disable(imx_usb);
+
+	for (i = 0; i < IMX_USB_NB_EP + 1; i++)
+		free_irq(imx_usb->usbd_int[i], imx_usb);
+
+	clk_put(imx_usb->clk);
+	clk_disable(imx_usb->clk);
+	iounmap(imx_usb->base);
+
+	release_mem_region(imx_usb->res->start,
+		imx_usb->res->end - imx_usb->res->start + 1);
+
+	if (pdata->exit)
+		pdata->exit(&pdev->dev);
+
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+
+#ifdef	CONFIG_PM
+#define	imx_udc_suspend	NULL
+#define	imx_udc_resume	NULL
+#else
+#define	imx_udc_suspend	NULL
+#define	imx_udc_resume	NULL
+#endif
+
+/*----------------------------------------------------------------------------*/
+
+static struct platform_driver udc_driver = {
+	.driver		= {
+		.name	= driver_name,
+		.owner	= THIS_MODULE,
+	},
+	.remove		= __exit_p(imx_udc_remove),
+	.suspend	= imx_udc_suspend,
+	.resume		= imx_udc_resume,
+};
+
+static int __init udc_init(void)
+{
+	return platform_driver_probe(&udc_driver, imx_udc_probe);
+}
+module_init(udc_init);
+
+static void __exit udc_exit(void)
+{
+	platform_driver_unregister(&udc_driver);
+}
+module_exit(udc_exit);
+
+MODULE_DESCRIPTION("IMX USB Device Controller driver");
+MODULE_AUTHOR("Darius Augulis <augulis.darius@gmail.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:imx_udc");
diff --git a/drivers/usb/gadget/imx_udc.h b/drivers/usb/gadget/imx_udc.h
new file mode 100644
index 000000000000..850076937d8d
--- /dev/null
+++ b/drivers/usb/gadget/imx_udc.h
@@ -0,0 +1,344 @@
+/*
+ *	Copyright (C) 2005 Mike Lee(eemike@gmail.com)
+ *
+ *	This udc driver is now under testing and code is based on pxa2xx_udc.h
+ *	Please use it with your own risk!
+ *
+ *	This program is free software; you can redistribute it and/or modify
+ *	it under the terms of the GNU General Public License as published by
+ *	the Free Software Foundation; either version 2 of the License, or
+ *	(at your option) any later version.
+ *
+ *	This program is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *	GNU General Public License for more details.
+ */
+
+#ifndef __LINUX_USB_GADGET_IMX_H
+#define __LINUX_USB_GADGET_IMX_H
+
+#include <linux/types.h>
+
+/* Helper macros */
+#define EP_NO(ep)	((ep->bEndpointAddress) & ~USB_DIR_IN) /* IN:1, OUT:0 */
+#define EP_DIR(ep)	((ep->bEndpointAddress) & USB_DIR_IN ? 1 : 0)
+#define irq_to_ep(irq)	(((irq) >= USBD_INT0) || ((irq) <= USBD_INT6) ? ((irq) - USBD_INT0) : (USBD_INT6)) /*should not happen*/
+#define ep_to_irq(ep)	(EP_NO((ep)) + USBD_INT0)
+#define IMX_USB_NB_EP	6
+
+/* Driver structures */
+struct imx_request {
+	struct usb_request			req;
+	struct list_head			queue;
+	unsigned int				in_use;
+};
+
+enum ep0_state {
+	EP0_IDLE,
+	EP0_IN_DATA_PHASE,
+	EP0_OUT_DATA_PHASE,
+	EP0_CONFIG,
+	EP0_STALL,
+};
+
+struct imx_ep_struct {
+	struct usb_ep				ep;
+	struct imx_udc_struct			*imx_usb;
+	struct list_head			queue;
+	unsigned char				stopped;
+	unsigned char				fifosize;
+	unsigned char				bEndpointAddress;
+	unsigned char				bmAttributes;
+};
+
+struct imx_udc_struct {
+	struct usb_gadget			gadget;
+	struct usb_gadget_driver		*driver;
+	struct device				*dev;
+	struct imx_ep_struct			imx_ep[IMX_USB_NB_EP];
+	struct clk				*clk;
+	enum ep0_state				ep0state;
+	struct resource				*res;
+	void __iomem				*base;
+	unsigned char				set_config;
+	int					cfg,
+						intf,
+						alt,
+						usbd_int[7];
+};
+
+/* USB registers */
+#define  USB_FRAME		(0x00)	/* USB frame */
+#define  USB_SPEC		(0x04)	/* USB Spec */
+#define  USB_STAT		(0x08)	/* USB Status */
+#define  USB_CTRL		(0x0C)	/* USB Control */
+#define  USB_DADR		(0x10)	/* USB Desc RAM addr */
+#define  USB_DDAT		(0x14)	/* USB Desc RAM/EP buffer data */
+#define  USB_INTR		(0x18)	/* USB interrupt */
+#define  USB_MASK		(0x1C)	/* USB Mask */
+#define  USB_ENAB		(0x24)	/* USB Enable */
+#define  USB_EP_STAT(x)		(0x30 + (x*0x30)) /* USB status/control */
+#define  USB_EP_INTR(x)		(0x34 + (x*0x30)) /* USB interrupt */
+#define  USB_EP_MASK(x)		(0x38 + (x*0x30)) /* USB mask */
+#define  USB_EP_FDAT(x)		(0x3C + (x*0x30)) /* USB FIFO data */
+#define  USB_EP_FDAT0(x)	(0x3C + (x*0x30)) /* USB FIFO data */
+#define  USB_EP_FDAT1(x)	(0x3D + (x*0x30)) /* USB FIFO data */
+#define  USB_EP_FDAT2(x)	(0x3E + (x*0x30)) /* USB FIFO data */
+#define  USB_EP_FDAT3(x)	(0x3F + (x*0x30)) /* USB FIFO data */
+#define  USB_EP_FSTAT(x)	(0x40 + (x*0x30)) /* USB FIFO status */
+#define  USB_EP_FCTRL(x)	(0x44 + (x*0x30)) /* USB FIFO control */
+#define  USB_EP_LRFP(x)		(0x48 + (x*0x30)) /* USB last read frame pointer */
+#define  USB_EP_LWFP(x)		(0x4C + (x*0x30)) /* USB last write frame pointer */
+#define  USB_EP_FALRM(x)	(0x50 + (x*0x30)) /* USB FIFO alarm */
+#define  USB_EP_FRDP(x)		(0x54 + (x*0x30)) /* USB FIFO read pointer */
+#define  USB_EP_FWRP(x)		(0x58 + (x*0x30)) /* USB FIFO write pointer */
+/* USB Control Register Bit Fields.*/
+#define CTRL_CMDOVER		(1<<6)	/* UDC status */
+#define CTRL_CMDERROR		(1<<5)	/* UDC status */
+#define CTRL_FE_ENA		(1<<3)	/* Enable Font End logic */
+#define CTRL_UDC_RST		(1<<2)	/* UDC reset */
+#define CTRL_AFE_ENA		(1<<1)	/* Analog Font end enable */
+#define CTRL_RESUME		(1<<0)	/* UDC resume */
+/* USB Status Register Bit Fields.*/
+#define STAT_RST		(1<<8)
+#define STAT_SUSP		(1<<7)
+#define STAT_CFG		(3<<5)
+#define STAT_INTF		(3<<3)
+#define STAT_ALTSET		(7<<0)
+/* USB Interrupt Status/Mask Registers Bit fields */
+#define INTR_WAKEUP		(1<<31)	/* Wake up Interrupt */
+#define INTR_MSOF		(1<<7)	/* Missed Start of Frame */
+#define INTR_SOF		(1<<6)	/* Start of Frame */
+#define INTR_RESET_STOP		(1<<5)	/* Reset Signaling stop */
+#define INTR_RESET_START	(1<<4)	/* Reset Signaling start */
+#define INTR_RESUME		(1<<3)	/* Suspend to resume */
+#define INTR_SUSPEND		(1<<2)	/* Active to suspend */
+#define INTR_FRAME_MATCH	(1<<1)	/* Frame matched */
+#define INTR_CFG_CHG		(1<<0)	/* Configuration change occurred */
+/* USB Enable Register Bit Fields.*/
+#define ENAB_RST		(1<<31)	/* Reset USB modules */
+#define ENAB_ENAB		(1<<30)	/* Enable USB modules*/
+#define ENAB_SUSPEND		(1<<29)	/* Suspend USB modules */
+#define ENAB_ENDIAN		(1<<28)	/* Endian of USB modules */
+#define ENAB_PWRMD		(1<<0)	/* Power mode of USB modules */
+/* USB Descriptor Ram Address Register bit fields */
+#define DADR_CFG		(1<<31)	/* Configuration */
+#define DADR_BSY		(1<<30)	/* Busy status */
+#define DADR_DADR		(0x1FF)	/* Descriptor Ram Address */
+/* USB Descriptor RAM/Endpoint Buffer Data Register bit fields */
+#define DDAT_DDAT		(0xFF)	/* Descriptor Endpoint Buffer */
+/* USB Endpoint Status Register bit fields */
+#define EPSTAT_BCOUNT		(0x7F<<16)	/* Endpoint FIFO byte count */
+#define EPSTAT_SIP		(1<<8)	/* Endpoint setup in progress */
+#define EPSTAT_DIR		(1<<7)	/* Endpoint transfer direction */
+#define EPSTAT_MAX		(3<<5)	/* Endpoint Max packet size */
+#define EPSTAT_TYP		(3<<3)	/* Endpoint type */
+#define EPSTAT_ZLPS		(1<<2)	/* Send zero length packet */
+#define EPSTAT_FLUSH		(1<<1)	/* Endpoint FIFO Flush */
+#define EPSTAT_STALL		(1<<0)	/* Force stall */
+/* USB Endpoint FIFO Status Register bit fields */
+#define FSTAT_FRAME_STAT	(0xF<<24)	/* Frame status bit [0-3] */
+#define FSTAT_ERR		(1<<22)	/* FIFO error */
+#define FSTAT_UF		(1<<21)	/* FIFO underflow */
+#define FSTAT_OF		(1<<20)	/* FIFO overflow */
+#define FSTAT_FR		(1<<19)	/* FIFO frame ready */
+#define FSTAT_FULL		(1<<18)	/* FIFO full */
+#define FSTAT_ALRM		(1<<17)	/* FIFO alarm */
+#define FSTAT_EMPTY		(1<<16)	/* FIFO empty */
+/* USB Endpoint FIFO Control Register bit fields */
+#define FCTRL_WFR		(1<<29)	/* Write frame end */
+/* USB Endpoint Interrupt Status Regsiter bit fields */
+#define EPINTR_FIFO_FULL	(1<<8)	/* fifo full */
+#define EPINTR_FIFO_EMPTY	(1<<7)	/* fifo empty */
+#define EPINTR_FIFO_ERROR	(1<<6)	/* fifo error */
+#define EPINTR_FIFO_HIGH	(1<<5)	/* fifo high */
+#define EPINTR_FIFO_LOW		(1<<4)	/* fifo low */
+#define EPINTR_MDEVREQ		(1<<3)	/* multi Device request */
+#define EPINTR_EOT		(1<<2)	/* fifo end of transfer */
+#define EPINTR_DEVREQ		(1<<1)	/* Device request */
+#define EPINTR_EOF		(1<<0)	/* fifo end of frame */
+
+/* Debug macros */
+#ifdef DEBUG
+
+/* #define DEBUG_REQ */
+/* #define DEBUG_TRX */
+/* #define DEBUG_INIT */
+/* #define DEBUG_EP0 */
+/* #define DEBUG_EPX */
+/* #define DEBUG_IRQ */
+/* #define DEBUG_EPIRQ */
+/* #define DEBUG_DUMP */
+#define DEBUG_ERR
+
+#ifdef DEBUG_REQ
+	#define D_REQ(dev, args...)	dev_dbg(dev, ## args)
+#else
+	#define D_REQ(dev, args...)	do {} while (0)
+#endif /* DEBUG_REQ */
+
+#ifdef DEBUG_TRX
+	#define D_TRX(dev, args...)	dev_dbg(dev, ## args)
+#else
+	#define D_TRX(dev, args...)	do {} while (0)
+#endif /* DEBUG_TRX */
+
+#ifdef DEBUG_INIT
+	#define D_INI(dev, args...)	dev_dbg(dev, ## args)
+#else
+	#define D_INI(dev, args...)	do {} while (0)
+#endif /* DEBUG_INIT */
+
+#ifdef DEBUG_EP0
+	static const char *state_name[] = {
+		"EP0_IDLE",
+		"EP0_IN_DATA_PHASE",
+		"EP0_OUT_DATA_PHASE",
+		"EP0_CONFIG",
+		"EP0_STALL"
+	};
+	#define D_EP0(dev, args...)	dev_dbg(dev, ## args)
+#else
+	#define D_EP0(dev, args...)	do {} while (0)
+#endif /* DEBUG_EP0 */
+
+#ifdef DEBUG_EPX
+	#define D_EPX(dev, args...)	dev_dbg(dev, ## args)
+#else
+	#define D_EPX(dev, args...)	do {} while (0)
+#endif /* DEBUG_EP0 */
+
+#ifdef DEBUG_IRQ
+	static void dump_intr(const char *label, int irqreg, struct device *dev)
+	{
+		dev_dbg(dev, "<%s> USB_INTR=[%s%s%s%s%s%s%s%s%s]\n", label,
+			(irqreg & INTR_WAKEUP) ? " wake" : "",
+			(irqreg & INTR_MSOF) ? " msof" : "",
+			(irqreg & INTR_SOF) ? " sof" : "",
+			(irqreg & INTR_RESUME) ? " resume" : "",
+			(irqreg & INTR_SUSPEND) ? " suspend" : "",
+			(irqreg & INTR_RESET_STOP) ? " noreset" : "",
+			(irqreg & INTR_RESET_START) ? " reset" : "",
+			(irqreg & INTR_FRAME_MATCH) ? " fmatch" : "",
+			(irqreg & INTR_CFG_CHG) ? " config" : "");
+	}
+#else
+	#define dump_intr(x, y, z)		do {} while (0)
+#endif /* DEBUG_IRQ */
+
+#ifdef DEBUG_EPIRQ
+	static void dump_ep_intr(const char *label, int nr, int irqreg, struct device *dev)
+	{
+		dev_dbg(dev, "<%s> EP%d_INTR=[%s%s%s%s%s%s%s%s%s]\n", label, nr,
+			(irqreg & EPINTR_FIFO_FULL) ? " full" : "",
+			(irqreg & EPINTR_FIFO_EMPTY) ? " fempty" : "",
+			(irqreg & EPINTR_FIFO_ERROR) ? " ferr" : "",
+			(irqreg & EPINTR_FIFO_HIGH) ? " fhigh" : "",
+			(irqreg & EPINTR_FIFO_LOW) ? " flow" : "",
+			(irqreg & EPINTR_MDEVREQ) ? " mreq" : "",
+			(irqreg & EPINTR_EOF) ? " eof" : "",
+			(irqreg & EPINTR_DEVREQ) ? " devreq" : "",
+			(irqreg & EPINTR_EOT) ? " eot" : "");
+	}
+#else
+	#define dump_ep_intr(x, y, z, i)	do {} while (0)
+#endif /* DEBUG_IRQ */
+
+#ifdef DEBUG_DUMP
+	static void dump_usb_stat(const char *label, struct imx_udc_struct *imx_usb)
+	{
+		int temp = __raw_readl(imx_usb->base + USB_STAT);
+
+		dev_dbg(imx_usb->dev,
+			"<%s> USB_STAT=[%s%s CFG=%d, INTF=%d, ALTR=%d]\n", label,
+			(temp & STAT_RST) ? " reset" : "",
+			(temp & STAT_SUSP) ? " suspend" : "",
+			(temp & STAT_CFG) >> 5,
+			(temp & STAT_INTF) >> 3,
+			(temp & STAT_ALTSET));
+	}
+
+	static void dump_ep_stat(const char *label, struct imx_ep_struct *imx_ep)
+	{
+		int temp = __raw_readl(imx_ep->imx_usb->base + USB_EP_INTR(EP_NO(imx_ep)));
+
+		dev_dbg(imx_ep->imx_usb->dev,
+			"<%s> EP%d_INTR=[%s%s%s%s%s%s%s%s%s]\n", label, EP_NO(imx_ep),
+			(temp & EPINTR_FIFO_FULL) ? " full" : "",
+			(temp & EPINTR_FIFO_EMPTY) ? " fempty" : "",
+			(temp & EPINTR_FIFO_ERROR) ? " ferr" : "",
+			(temp & EPINTR_FIFO_HIGH) ? " fhigh" : "",
+			(temp & EPINTR_FIFO_LOW) ? " flow" : "",
+			(temp & EPINTR_MDEVREQ) ? " mreq" : "",
+			(temp & EPINTR_EOF) ? " eof" : "",
+			(temp & EPINTR_DEVREQ) ? " devreq" : "",
+			(temp & EPINTR_EOT) ? " eot" : "");
+
+		temp = __raw_readl(imx_ep->imx_usb->base + USB_EP_STAT(EP_NO(imx_ep)));
+
+		dev_dbg(imx_ep->imx_usb->dev,
+			"<%s> EP%d_STAT=[%s%s bcount=%d]\n", label, EP_NO(imx_ep),
+			(temp & EPSTAT_SIP) ? " sip" : "",
+			(temp & EPSTAT_STALL) ? " stall" : "",
+			(temp & EPSTAT_BCOUNT) >> 16);
+
+		temp = __raw_readl(imx_ep->imx_usb->base + USB_EP_FSTAT(EP_NO(imx_ep)));
+
+		dev_dbg(imx_ep->imx_usb->dev,
+			"<%s> EP%d_FSTAT=[%s%s%s%s%s%s%s]\n", label, EP_NO(imx_ep),
+			(temp & FSTAT_ERR) ? " ferr" : "",
+			(temp & FSTAT_UF) ? " funder" : "",
+			(temp & FSTAT_OF) ? " fover" : "",
+			(temp & FSTAT_FR) ? " fready" : "",
+			(temp & FSTAT_FULL) ? " ffull" : "",
+			(temp & FSTAT_ALRM) ? " falarm" : "",
+			(temp & FSTAT_EMPTY) ? " fempty" : "");
+	}
+
+	static void dump_req(const char *label, struct imx_ep_struct *imx_ep, struct usb_request *req)
+	{
+		int i;
+
+		if (!req || !req->buf) {
+			dev_dbg(imx_ep->imx_usb->dev, "<%s> req or req buf is free\n", label);
+			return;
+		}
+
+		if ((!EP_NO(imx_ep) && imx_ep->imx_usb->ep0state == EP0_IN_DATA_PHASE)
+			|| (EP_NO(imx_ep) && EP_DIR(imx_ep))) {
+
+			dev_dbg(imx_ep->imx_usb->dev, "<%s> request dump <", label);
+			for (i = 0; i < req->length; i++)
+				printk("%02x-", *((u8 *)req->buf + i));
+			printk(">\n");
+		}
+	}
+
+#else
+	#define dump_ep_stat(x, y)		do {} while (0)
+	#define dump_usb_stat(x, y)		do {} while (0)
+	#define dump_req(x, y, z)		do {} while (0)
+#endif /* DEBUG_DUMP */
+
+#ifdef DEBUG_ERR
+	#define D_ERR(dev, args...)	dev_dbg(dev, ## args)
+#else
+	#define D_ERR(dev, args...)	do {} while (0)
+#endif
+
+#else
+	#define D_REQ(dev, args...)		do {} while (0)
+	#define D_TRX(dev, args...)		do {} while (0)
+	#define D_INI(dev, args...)		do {} while (0)
+	#define D_EP0(dev, args...)		do {} while (0)
+	#define D_EPX(dev, args...)		do {} while (0)
+	#define dump_ep_intr(x, y, z, i)	do {} while (0)
+	#define dump_intr(x, y, z)		do {} while (0)
+	#define dump_ep_stat(x, y)		do {} while (0)
+	#define dump_usb_stat(x, y)		do {} while (0)
+	#define dump_req(x, y, z)		do {} while (0)
+	#define D_ERR(dev, args...)		do {} while (0)
+#endif /* DEBUG */
+
+#endif /* __LINUX_USB_GADGET_IMX_H */
diff --git a/drivers/usb/gadget/lh7a40x_udc.c b/drivers/usb/gadget/lh7a40x_udc.c
index c6e7df04c69a..d554b0895603 100644
--- a/drivers/usb/gadget/lh7a40x_udc.c
+++ b/drivers/usb/gadget/lh7a40x_udc.c
@@ -1981,7 +1981,7 @@ static struct lh7a40x_udc memory = {
 		   .ep0 = &memory.ep[0].ep,
 		   .name = driver_name,
 		   .dev = {
-			   .bus_id = "gadget",
+			   .init_name = "gadget",
 			   .release = nop_release,
 			   },
 		   },
diff --git a/drivers/usb/gadget/m66592-udc.c b/drivers/usb/gadget/m66592-udc.c
index 3a8879ec2061..43dcf9e1af6b 100644
--- a/drivers/usb/gadget/m66592-udc.c
+++ b/drivers/usb/gadget/m66592-udc.c
@@ -1546,8 +1546,6 @@ static void nop_completion(struct usb_ep *ep, struct usb_request *r)
 {
 }
 
-#define resource_len(r) (((r)->end - (r)->start) + 1)
-
 static int __init m66592_probe(struct platform_device *pdev)
 {
 	struct resource *res;
@@ -1560,11 +1558,10 @@ static int __init m66592_probe(struct platform_device *pdev)
 	int ret = 0;
 	int i;
 
-	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
-			(char *)udc_name);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		ret = -ENODEV;
-		pr_err("platform_get_resource_byname error.\n");
+		pr_err("platform_get_resource error.\n");
 		goto clean_up;
 	}
 
@@ -1575,7 +1572,7 @@ static int __init m66592_probe(struct platform_device *pdev)
 		goto clean_up;
 	}
 
-	reg = ioremap(res->start, resource_len(res));
+	reg = ioremap(res->start, resource_size(res));
 	if (reg == NULL) {
 		ret = -ENOMEM;
 		pr_err("ioremap error.\n");
diff --git a/drivers/usb/gadget/net2280.c b/drivers/usb/gadget/net2280.c
index 8ae70de2c37d..12c6d83b218c 100644
--- a/drivers/usb/gadget/net2280.c
+++ b/drivers/usb/gadget/net2280.c
@@ -669,7 +669,7 @@ fill_dma_desc (struct net2280_ep *ep, struct net2280_request *req, int valid)
 
 	/* 2280 may be polling VALID_BIT through ep->dma->dmadesc */
 	wmb ();
-	td->dmacount = cpu_to_le32p (&dmacount);
+	td->dmacount = cpu_to_le32(dmacount);
 }
 
 static const u32 dmactl_default =
diff --git a/drivers/usb/gadget/omap_udc.c b/drivers/usb/gadget/omap_udc.c
index 34e9e393f929..57d9641c6bf8 100644
--- a/drivers/usb/gadget/omap_udc.c
+++ b/drivers/usb/gadget/omap_udc.c
@@ -3006,7 +3006,7 @@ cleanup1:
 
 cleanup0:
 	if (xceiv)
-		put_device(xceiv->dev);
+		otg_put_transceiver(xceiv);
 
 	if (cpu_is_omap16xx() || cpu_is_omap24xx()) {
 		clk_disable(hhc_clk);
@@ -3034,7 +3034,7 @@ static int __exit omap_udc_remove(struct platform_device *pdev)
 
 	pullup_disable(udc);
 	if (udc->transceiver) {
-		put_device(udc->transceiver->dev);
+		otg_put_transceiver(udc->transceiver);
 		udc->transceiver = NULL;
 	}
 	omap_writew(0, UDC_SYSCON1);
diff --git a/drivers/usb/gadget/pxa25x_udc.c b/drivers/usb/gadget/pxa25x_udc.c
index 8c5026be79d4..9b36205c5759 100644
--- a/drivers/usb/gadget/pxa25x_udc.c
+++ b/drivers/usb/gadget/pxa25x_udc.c
@@ -1833,7 +1833,7 @@ static struct pxa25x_udc memory = {
 		.ep0		= &memory.ep[0].ep,
 		.name		= driver_name,
 		.dev = {
-			.bus_id		= "gadget",
+			.init_name	= "gadget",
 			.release	= nop_release,
 		},
 	},
@@ -2198,7 +2198,7 @@ static int __init pxa25x_udc_probe(struct platform_device *pdev)
 	udc_disable(dev);
 	udc_reinit(dev);
 
-	dev->vbus = is_vbus_present();
+	dev->vbus = !!is_vbus_present();
 
 	/* irq setup after old hardware state is cleaned up */
 	retval = request_irq(irq, pxa25x_udc_irq,
diff --git a/drivers/usb/gadget/pxa27x_udc.c b/drivers/usb/gadget/pxa27x_udc.c
index 944e4ff641df..990f40f988d4 100644
--- a/drivers/usb/gadget/pxa27x_udc.c
+++ b/drivers/usb/gadget/pxa27x_udc.c
@@ -430,7 +430,6 @@ static void pio_irq_enable(struct pxa_ep *ep)
 /**
  * pio_irq_disable - Disables irq generation for one endpoint
  * @ep: udc endpoint
- * @index: endpoint number
  */
 static void pio_irq_disable(struct pxa_ep *ep)
 {
@@ -586,7 +585,6 @@ static void inc_ep_stats_reqs(struct pxa_ep *ep, int is_in)
  * inc_ep_stats_bytes - Update ep stats counts
  * @ep: physical endpoint
  * @count: bytes transfered on endpoint
- * @req: usb request
  * @is_in: ep direction (USB_DIR_IN or 0)
  */
 static void inc_ep_stats_bytes(struct pxa_ep *ep, int count, int is_in)
@@ -2162,7 +2160,7 @@ static struct pxa_udc memory = {
 		.ep0		= &memory.udc_usb_ep[0].usb_ep,
 		.name		= driver_name,
 		.dev = {
-			.bus_id		= "gadget",
+			.init_name	= "gadget",
 		},
 	},
 
diff --git a/drivers/usb/gadget/s3c2410_udc.c b/drivers/usb/gadget/s3c2410_udc.c
index 8d8d65165983..9a2b8920532d 100644
--- a/drivers/usb/gadget/s3c2410_udc.c
+++ b/drivers/usb/gadget/s3c2410_udc.c
@@ -36,6 +36,7 @@
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
+#include <linux/gpio.h>
 
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
@@ -51,7 +52,6 @@
 #include <mach/irqs.h>
 
 #include <mach/hardware.h>
-#include <mach/regs-gpio.h>
 
 #include <plat/regs-udc.h>
 #include <plat/udc.h>
@@ -1510,11 +1510,7 @@ static irqreturn_t s3c2410_udc_vbus_irq(int irq, void *_dev)
 
 	dprintk(DEBUG_NORMAL, "%s()\n", __func__);
 
-	/* some cpus cannot read from an line configured to IRQ! */
-	s3c2410_gpio_cfgpin(udc_info->vbus_pin, S3C2410_GPIO_INPUT);
-	value = s3c2410_gpio_getpin(udc_info->vbus_pin);
-	s3c2410_gpio_cfgpin(udc_info->vbus_pin, S3C2410_GPIO_SFN2);
-
+	value = gpio_get_value(udc_info->vbus_pin) ? 1 : 0;
 	if (udc_info->vbus_pin_inverted)
 		value = !value;
 
@@ -1727,7 +1723,7 @@ static struct s3c2410_udc memory = {
 		.ep0		= &memory.ep[0].ep,
 		.name		= gadget_name,
 		.dev = {
-			.bus_id		= "gadget",
+			.init_name	= "gadget",
 		},
 	},
 
@@ -1802,7 +1798,7 @@ static int s3c2410_udc_probe(struct platform_device *pdev)
 	struct s3c2410_udc *udc = &memory;
 	struct device *dev = &pdev->dev;
 	int retval;
-	unsigned int irq;
+	int irq;
 
 	dev_dbg(dev, "%s()\n", __func__);
 
@@ -1861,7 +1857,7 @@ static int s3c2410_udc_probe(struct platform_device *pdev)
 
 	/* irq setup after old hardware state is cleaned up */
 	retval = request_irq(IRQ_USBD, s3c2410_udc_irq,
-			IRQF_DISABLED, gadget_name, udc);
+			     IRQF_DISABLED, gadget_name, udc);
 
 	if (retval != 0) {
 		dev_err(dev, "cannot get irq %i, err %d\n", IRQ_USBD, retval);
@@ -1872,17 +1868,28 @@ static int s3c2410_udc_probe(struct platform_device *pdev)
 	dev_dbg(dev, "got irq %i\n", IRQ_USBD);
 
 	if (udc_info && udc_info->vbus_pin > 0) {
-		irq = s3c2410_gpio_getirq(udc_info->vbus_pin);
+		retval = gpio_request(udc_info->vbus_pin, "udc vbus");
+		if (retval < 0) {
+			dev_err(dev, "cannot claim vbus pin\n");
+			goto err_int;
+		}
+
+		irq = gpio_to_irq(udc_info->vbus_pin);
+		if (irq < 0) {
+			dev_err(dev, "no irq for gpio vbus pin\n");
+			goto err_gpio_claim;
+		}
+
 		retval = request_irq(irq, s3c2410_udc_vbus_irq,
 				     IRQF_DISABLED | IRQF_TRIGGER_RISING
 				     | IRQF_TRIGGER_FALLING | IRQF_SHARED,
 				     gadget_name, udc);
 
 		if (retval != 0) {
-			dev_err(dev, "can't get vbus irq %i, err %d\n",
+			dev_err(dev, "can't get vbus irq %d, err %d\n",
 				irq, retval);
 			retval = -EBUSY;
-			goto err_int;
+			goto err_gpio_claim;
 		}
 
 		dev_dbg(dev, "got irq %i\n", irq);
@@ -1902,6 +1909,9 @@ static int s3c2410_udc_probe(struct platform_device *pdev)
 
 	return 0;
 
+err_gpio_claim:
+	if (udc_info && udc_info->vbus_pin > 0)
+		gpio_free(udc_info->vbus_pin);
 err_int:
 	free_irq(IRQ_USBD, udc);
 err_map:
@@ -1927,7 +1937,7 @@ static int s3c2410_udc_remove(struct platform_device *pdev)
 	debugfs_remove(udc->regs_info);
 
 	if (udc_info && udc_info->vbus_pin > 0) {
-		irq = s3c2410_gpio_getirq(udc_info->vbus_pin);
+		irq = gpio_to_irq(udc_info->vbus_pin);
 		free_irq(irq, udc);
 	}
 
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index f3a75a929e0a..2b476b6b3d4d 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -96,6 +96,19 @@ config USB_EHCI_HCD_PPC_OF
 	  Enables support for the USB controller present on the PowerPC
 	  OpenFirmware platform bus.
 
+config USB_OXU210HP_HCD
+	tristate "OXU210HP HCD support"
+	depends on USB
+	---help---
+	  The OXU210HP is an USB host/OTG/device controller. Enable this
+	  option if your board has this chip. If unsure, say N.
+
+	  This driver does not support isochronous transfers and doesn't
+	  implement OTG nor USB device controllers.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called oxu210hp-hcd.
+
 config USB_ISP116X_HCD
 	tristate "ISP116X HCD support"
 	depends on USB
diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index 23be22224044..e5f3f20787e4 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -13,6 +13,7 @@ obj-$(CONFIG_USB_WHCI_HCD)	+= whci/
 obj-$(CONFIG_PCI)		+= pci-quirks.o
 
 obj-$(CONFIG_USB_EHCI_HCD)	+= ehci-hcd.o
+obj-$(CONFIG_USB_OXU210HP_HCD)	+= oxu210hp-hcd.o
 obj-$(CONFIG_USB_ISP116X_HCD)	+= isp116x-hcd.o
 obj-$(CONFIG_USB_OHCI_HCD)	+= ohci-hcd.o
 obj-$(CONFIG_USB_UHCI_HCD)	+= uhci-hcd.o
diff --git a/drivers/usb/host/ehci-dbg.c b/drivers/usb/host/ehci-dbg.c
index 0cb53ca8d343..7f4ace73d44a 100644
--- a/drivers/usb/host/ehci-dbg.c
+++ b/drivers/usb/host/ehci-dbg.c
@@ -455,9 +455,7 @@ static void qh_lines (
 				(scratch >> 16) & 0x7fff,
 				scratch,
 				td->urb);
-		if (temp < 0)
-			temp = 0;
-		else if (size < temp)
+		if (size < temp)
 			temp = size;
 		size -= temp;
 		next += temp;
@@ -466,9 +464,7 @@ static void qh_lines (
 	}
 
 	temp = snprintf (next, size, "\n");
-	if (temp < 0)
-		temp = 0;
-	else if (size < temp)
+	if (size < temp)
 		temp = size;
 	size -= temp;
 	next += temp;
diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index 218f9660d7ee..97a53a48a3d8 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -194,6 +194,7 @@ static int ehci_bus_resume (struct usb_hcd *hcd)
 	u32			temp;
 	u32			power_okay;
 	int			i;
+	u8			resume_needed = 0;
 
 	if (time_before (jiffies, ehci->next_statechange))
 		msleep(5);
@@ -228,7 +229,9 @@ static int ehci_bus_resume (struct usb_hcd *hcd)
 
 	/* Some controller/firmware combinations need a delay during which
 	 * they set up the port statuses.  See Bugzilla #8190. */
-	mdelay(8);
+	spin_unlock_irq(&ehci->lock);
+	msleep(8);
+	spin_lock_irq(&ehci->lock);
 
 	/* manually resume the ports we suspended during bus_suspend() */
 	i = HCS_N_PORTS (ehci->hcs_params);
@@ -236,12 +239,21 @@ static int ehci_bus_resume (struct usb_hcd *hcd)
 		temp = ehci_readl(ehci, &ehci->regs->port_status [i]);
 		temp &= ~(PORT_RWC_BITS | PORT_WAKE_BITS);
 		if (test_bit(i, &ehci->bus_suspended) &&
-				(temp & PORT_SUSPEND))
+				(temp & PORT_SUSPEND)) {
 			temp |= PORT_RESUME;
+			resume_needed = 1;
+		}
 		ehci_writel(ehci, temp, &ehci->regs->port_status [i]);
 	}
+
+	/* msleep for 20ms only if code is trying to resume port */
+	if (resume_needed) {
+		spin_unlock_irq(&ehci->lock);
+		msleep(20);
+		spin_lock_irq(&ehci->lock);
+	}
+
 	i = HCS_N_PORTS (ehci->hcs_params);
-	mdelay (20);
 	while (i--) {
 		temp = ehci_readl(ehci, &ehci->regs->port_status [i]);
 		if (test_bit(i, &ehci->bus_suspended) &&
@@ -422,8 +434,15 @@ static int check_reset_complete (
 		port_status &= ~PORT_RWC_BITS;
 		ehci_writel(ehci, port_status, status_reg);
 
-	} else
+		/* ensure 440EPX ohci controller state is operational */
+		if (ehci->has_amcc_usb23)
+			set_ohci_hcfs(ehci, 1);
+	} else {
 		ehci_dbg (ehci, "port %d high speed\n", index + 1);
+		/* ensure 440EPx ohci controller state is suspended */
+		if (ehci->has_amcc_usb23)
+			set_ohci_hcfs(ehci, 0);
+	}
 
 	return port_status;
 }
diff --git a/drivers/usb/host/ehci-pci.c b/drivers/usb/host/ehci-pci.c
index 36864f958444..bdc6e86e1f8b 100644
--- a/drivers/usb/host/ehci-pci.c
+++ b/drivers/usb/host/ehci-pci.c
@@ -219,15 +219,19 @@ static int ehci_pci_setup(struct usb_hcd *hcd)
 	/* Serial Bus Release Number is at PCI 0x60 offset */
 	pci_read_config_byte(pdev, 0x60, &ehci->sbrn);
 
-	/* Workaround current PCI init glitch:  wakeup bits aren't
-	 * being set from PCI PM capability.
+	/* Keep this around for a while just in case some EHCI
+	 * implementation uses legacy PCI PM support.  This test
+	 * can be removed on 17 Dec 2009 if the dev_warn() hasn't
+	 * been triggered by then.
 	 */
 	if (!device_can_wakeup(&pdev->dev)) {
 		u16	port_wake;
 
 		pci_read_config_word(pdev, 0x62, &port_wake);
-		if (port_wake & 0x0001)
+		if (port_wake & 0x0001) {
+			dev_warn(&pdev->dev, "Enabling legacy PCI PM\n");
 			device_init_wakeup(&pdev->dev, 1);
+		}
 	}
 
 #ifdef	CONFIG_USB_SUSPEND
@@ -428,6 +432,8 @@ static struct pci_driver ehci_pci_driver = {
 
 #ifdef	CONFIG_PM
 	.suspend =	usb_hcd_pci_suspend,
+	.suspend_late =	usb_hcd_pci_suspend_late,
+	.resume_early =	usb_hcd_pci_resume_early,
 	.resume =	usb_hcd_pci_resume,
 #endif
 	.shutdown = 	usb_hcd_pci_shutdown,
diff --git a/drivers/usb/host/ehci-ppc-of.c b/drivers/usb/host/ehci-ppc-of.c
index b018deed2e8f..ef732b704f53 100644
--- a/drivers/usb/host/ehci-ppc-of.c
+++ b/drivers/usb/host/ehci-ppc-of.c
@@ -107,11 +107,13 @@ ehci_hcd_ppc_of_probe(struct of_device *op, const struct of_device_id *match)
 {
 	struct device_node *dn = op->node;
 	struct usb_hcd *hcd;
-	struct ehci_hcd	*ehci;
+	struct ehci_hcd	*ehci = NULL;
 	struct resource res;
 	int irq;
 	int rv;
 
+	struct device_node *np;
+
 	if (usb_disabled())
 		return -ENODEV;
 
@@ -149,6 +151,20 @@ ehci_hcd_ppc_of_probe(struct of_device *op, const struct of_device_id *match)
 	}
 
 	ehci = hcd_to_ehci(hcd);
+	np = of_find_compatible_node(NULL, NULL, "ibm,usb-ohci-440epx");
+	if (np != NULL) {
+		/* claim we really affected by usb23 erratum */
+		if (!of_address_to_resource(np, 0, &res))
+			ehci->ohci_hcctrl_reg = ioremap(res.start +
+					OHCI_HCCTRL_OFFSET, OHCI_HCCTRL_LEN);
+		else
+			pr_debug(__FILE__ ": no ohci offset in fdt\n");
+		if (!ehci->ohci_hcctrl_reg) {
+			pr_debug(__FILE__ ": ioremap for ohci hcctrl failed\n");
+		} else {
+			ehci->has_amcc_usb23 = 1;
+		}
+	}
 
 	if (of_get_property(dn, "big-endian", NULL)) {
 		ehci->big_endian_mmio = 1;
@@ -181,6 +197,9 @@ err_ioremap:
 	irq_dispose_mapping(irq);
 err_irq:
 	release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
+
+	if (ehci->has_amcc_usb23)
+		iounmap(ehci->ohci_hcctrl_reg);
 err_rmr:
 	usb_put_hcd(hcd);
 
@@ -191,6 +210,11 @@ err_rmr:
 static int ehci_hcd_ppc_of_remove(struct of_device *op)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(&op->dev);
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+
+	struct device_node *np;
+	struct resource res;
+
 	dev_set_drvdata(&op->dev, NULL);
 
 	dev_dbg(&op->dev, "stopping PPC-OF USB Controller\n");
@@ -201,6 +225,25 @@ static int ehci_hcd_ppc_of_remove(struct of_device *op)
 	irq_dispose_mapping(hcd->irq);
 	release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
 
+	/* use request_mem_region to test if the ohci driver is loaded.  if so
+	 * ensure the ohci core is operational.
+	 */
+	if (ehci->has_amcc_usb23) {
+		np = of_find_compatible_node(NULL, NULL, "ibm,usb-ohci-440epx");
+		if (np != NULL) {
+			if (!of_address_to_resource(np, 0, &res))
+				if (!request_mem_region(res.start,
+							    0x4, hcd_name))
+					set_ohci_hcfs(ehci, 1);
+				else
+					release_mem_region(res.start, 0x4);
+			else
+				pr_debug(__FILE__ ": no ohci offset in fdt\n");
+			of_node_put(np);
+		}
+
+		iounmap(ehci->ohci_hcctrl_reg);
+	}
 	usb_put_hcd(hcd);
 
 	return 0;
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index c7d4b5a06bdb..fb7054ccf4fc 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -120,6 +120,16 @@ struct ehci_hcd {			/* one per controller */
 	unsigned		has_fsl_port_bug:1; /* FreeScale */
 	unsigned		big_endian_mmio:1;
 	unsigned		big_endian_desc:1;
+	unsigned		has_amcc_usb23:1;
+
+	/* required for usb32 quirk */
+	#define OHCI_CTRL_HCFS          (3 << 6)
+	#define OHCI_USB_OPER           (2 << 6)
+	#define OHCI_USB_SUSPEND        (3 << 6)
+
+	#define OHCI_HCCTRL_OFFSET      0x4
+	#define OHCI_HCCTRL_LEN         0x4
+	__hc32			*ohci_hcctrl_reg;
 
 	u8			sbrn;		/* packed release number */
 
@@ -636,6 +646,30 @@ static inline void ehci_writel(const struct ehci_hcd *ehci,
 #endif
 }
 
+/*
+ * On certain ppc-44x SoC there is a HW issue, that could only worked around with
+ * explicit suspend/operate of OHCI. This function hereby makes sense only on that arch.
+ * Other common bits are dependant on has_amcc_usb23 quirk flag.
+ */
+#ifdef CONFIG_44x
+static inline void set_ohci_hcfs(struct ehci_hcd *ehci, int operational)
+{
+	u32 hc_control;
+
+	hc_control = (readl_be(ehci->ohci_hcctrl_reg) & ~OHCI_CTRL_HCFS);
+	if (operational)
+		hc_control |= OHCI_USB_OPER;
+	else
+		hc_control |= OHCI_USB_SUSPEND;
+
+	writel_be(hc_control, ehci->ohci_hcctrl_reg);
+	(void) readl_be(ehci->ohci_hcctrl_reg);
+}
+#else
+static inline void set_ohci_hcfs(struct ehci_hcd *ehci, int operational)
+{ }
+#endif
+
 /*-------------------------------------------------------------------------*/
 
 /*
diff --git a/drivers/usb/host/isp1760-hcd.c b/drivers/usb/host/isp1760-hcd.c
index 8017f1cf78e2..b899f1a59c26 100644
--- a/drivers/usb/host/isp1760-hcd.c
+++ b/drivers/usb/host/isp1760-hcd.c
@@ -435,14 +435,13 @@ static int isp1760_hc_setup(struct usb_hcd *hcd)
 
 	/*
 	 * PORT 1 Control register of the ISP1760 is the OTG control
-	 * register on ISP1761.
+	 * register on ISP1761. Since there is no OTG or device controller
+	 * support in this driver, we use port 1 as a "normal" USB host port on
+	 * both chips.
 	 */
-	if (!(priv->devflags & ISP1760_FLAG_ISP1761) &&
-	    !(priv->devflags & ISP1760_FLAG_PORT1_DIS)) {
-		isp1760_writel(PORT1_POWER | PORT1_INIT2,
-			       hcd->regs + HC_PORT1_CTRL);
-		mdelay(10);
-	}
+	isp1760_writel(PORT1_POWER | PORT1_INIT2,
+		       hcd->regs + HC_PORT1_CTRL);
+	mdelay(10);
 
 	priv->hcs_params = isp1760_readl(hcd->regs + HC_HCSPARAMS);
 
diff --git a/drivers/usb/host/isp1760-hcd.h b/drivers/usb/host/isp1760-hcd.h
index 4377277667d9..a9daea587962 100644
--- a/drivers/usb/host/isp1760-hcd.h
+++ b/drivers/usb/host/isp1760-hcd.h
@@ -135,7 +135,6 @@ typedef void (packet_enqueue)(struct usb_hcd *hcd, struct isp1760_qh *qh,
  * indicate the most "atypical" case, so that a devflags of 0 is
  * a sane default configuration.
  */
-#define ISP1760_FLAG_PORT1_DIS		0x00000001 /* Port 1 disabled */
 #define ISP1760_FLAG_BUS_WIDTH_16	0x00000002 /* 16-bit data bus width */
 #define ISP1760_FLAG_OTG_EN		0x00000004 /* Port 1 supports OTG */
 #define ISP1760_FLAG_ANALOG_OC		0x00000008 /* Analog overcurrent */
diff --git a/drivers/usb/host/isp1760-if.c b/drivers/usb/host/isp1760-if.c
index b87ca7cf4b37..4cf7ca428b33 100644
--- a/drivers/usb/host/isp1760-if.c
+++ b/drivers/usb/host/isp1760-if.c
@@ -60,9 +60,6 @@ static int of_isp1760_probe(struct of_device *dev,
 	if (of_device_is_compatible(dp, "nxp,usb-isp1761"))
 		devflags |= ISP1760_FLAG_ISP1761;
 
-	if (of_get_property(dp, "port1-disable", NULL) != NULL)
-		devflags |= ISP1760_FLAG_PORT1_DIS;
-
 	/* Some systems wire up only 16 of the 32 data lines */
 	prop = of_get_property(dp, "bus-width", NULL);
 	if (prop && *prop == 16)
@@ -129,23 +126,23 @@ static struct of_platform_driver isp1760_of_driver = {
 #endif
 
 #ifdef CONFIG_PCI
-static u32 nxp_pci_io_base;
-static u32 iolength;
-static u32 pci_mem_phy0;
-static u32 length;
-static u8 __iomem *chip_addr;
-static u8 __iomem *iobase;
-
 static int __devinit isp1761_pci_probe(struct pci_dev *dev,
 		const struct pci_device_id *id)
 {
 	u8 latency, limit;
 	__u32 reg_data;
 	int retry_count;
-	int length;
-	int status = 1;
 	struct usb_hcd *hcd;
 	unsigned int devflags = 0;
+	int ret_status = 0;
+
+	resource_size_t pci_mem_phy0;
+	resource_size_t memlength;
+
+	u8 __iomem *chip_addr;
+	u8 __iomem *iobase;
+	resource_size_t nxp_pci_io_base;
+	resource_size_t iolength;
 
 	if (usb_disabled())
 		return -ENODEV;
@@ -168,26 +165,30 @@ static int __devinit isp1761_pci_probe(struct pci_dev *dev,
 	iobase = ioremap_nocache(nxp_pci_io_base, iolength);
 	if (!iobase) {
 		printk(KERN_ERR "ioremap #1\n");
-		release_mem_region(nxp_pci_io_base, iolength);
-		return -ENOMEM;
+		ret_status = -ENOMEM;
+		goto cleanup1;
 	}
 	/* Grab the PLX PCI shared memory of the ISP 1761 we need  */
 	pci_mem_phy0 = pci_resource_start(dev, 3);
-	length = pci_resource_len(dev, 3);
-
-	if (length < 0xffff) {
-		printk(KERN_ERR "memory length for this resource is less than "
-				"required\n");
-		release_mem_region(nxp_pci_io_base, iolength);
-		iounmap(iobase);
-		return  -ENOMEM;
+	memlength = pci_resource_len(dev, 3);
+	if (memlength < 0xffff) {
+		printk(KERN_ERR "memory length for this resource is wrong\n");
+		ret_status = -ENOMEM;
+		goto cleanup2;
 	}
 
-	if (!request_mem_region(pci_mem_phy0, length, "ISP-PCI")) {
+	if (!request_mem_region(pci_mem_phy0, memlength, "ISP-PCI")) {
 		printk(KERN_ERR "host controller already in use\n");
-		release_mem_region(nxp_pci_io_base, iolength);
-		iounmap(iobase);
-		return -EBUSY;
+		ret_status = -EBUSY;
+		goto cleanup2;
+	}
+
+	/* map available memory */
+	chip_addr = ioremap_nocache(pci_mem_phy0,memlength);
+	if (!chip_addr) {
+		printk(KERN_ERR "Error ioremap failed\n");
+		ret_status = -ENOMEM;
+		goto cleanup3;
 	}
 
 	/* bad pci latencies can contribute to overruns */
@@ -210,39 +211,54 @@ static int __devinit isp1761_pci_probe(struct pci_dev *dev,
 		 * */
 		writel(0xface, chip_addr + HC_SCRATCH_REG);
 		udelay(100);
-		reg_data = readl(chip_addr + HC_SCRATCH_REG);
+		reg_data = readl(chip_addr + HC_SCRATCH_REG) & 0x0000ffff;
 		retry_count--;
 	}
 
+	iounmap(chip_addr);
+
 	/* Host Controller presence is detected by writing to scratch register
 	 * and reading back and checking the contents are same or not
 	 */
 	if (reg_data != 0xFACE) {
 		dev_err(&dev->dev, "scratch register mismatch %x\n", reg_data);
-		goto clean;
+		ret_status = -ENOMEM;
+		goto cleanup3;
 	}
 
 	pci_set_master(dev);
 
-	status = readl(iobase + 0x68);
-	status |= 0x900;
-	writel(status, iobase + 0x68);
+	/* configure PLX PCI chip to pass interrupts */
+#define PLX_INT_CSR_REG 0x68
+	reg_data = readl(iobase + PLX_INT_CSR_REG);
+	reg_data |= 0x900;
+	writel(reg_data, iobase + PLX_INT_CSR_REG);
 
 	dev->dev.dma_mask = NULL;
-	hcd = isp1760_register(pci_mem_phy0, length, dev->irq,
+	hcd = isp1760_register(pci_mem_phy0, memlength, dev->irq,
 		IRQF_SHARED | IRQF_DISABLED, &dev->dev, dev_name(&dev->dev),
 		devflags);
-	if (!IS_ERR(hcd)) {
-		pci_set_drvdata(dev, hcd);
-		return 0;
+	if (IS_ERR(hcd)) {
+		ret_status = -ENODEV;
+		goto cleanup3;
 	}
-clean:
-	status = -ENODEV;
+
+	/* done with PLX IO access */
+	iounmap(iobase);
+	release_mem_region(nxp_pci_io_base, iolength);
+
+	pci_set_drvdata(dev, hcd);
+	return 0;
+
+cleanup3:
+	release_mem_region(pci_mem_phy0, memlength);
+cleanup2:
 	iounmap(iobase);
-	release_mem_region(pci_mem_phy0, length);
+cleanup1:
 	release_mem_region(nxp_pci_io_base, iolength);
-	return status;
+	return ret_status;
 }
+
 static void isp1761_pci_remove(struct pci_dev *dev)
 {
 	struct usb_hcd *hcd;
@@ -255,12 +271,6 @@ static void isp1761_pci_remove(struct pci_dev *dev)
 	usb_put_hcd(hcd);
 
 	pci_disable_device(dev);
-
-	iounmap(iobase);
-	iounmap(chip_addr);
-
-	release_mem_region(nxp_pci_io_base, iolength);
-	release_mem_region(pci_mem_phy0, length);
 }
 
 static void isp1761_pci_shutdown(struct pci_dev *dev)
@@ -268,12 +278,16 @@ static void isp1761_pci_shutdown(struct pci_dev *dev)
 	printk(KERN_ERR "ips1761_pci_shutdown\n");
 }
 
-static const struct pci_device_id isp1760_plx [] = { {
-	/* handle any USB 2.0 EHCI controller */
-	PCI_DEVICE_CLASS(((PCI_CLASS_BRIDGE_OTHER << 8) | (0x06 << 16)), ~0),
-		.driver_data = 0,
-},
-{ /* end: all zeroes */ }
+static const struct pci_device_id isp1760_plx [] = {
+	{
+		.class          = PCI_CLASS_BRIDGE_OTHER << 8,
+		.class_mask     = ~0,
+		.vendor		= PCI_VENDOR_ID_PLX,
+		.device		= 0x5406,
+		.subvendor	= PCI_VENDOR_ID_PLX,
+		.subdevice	= 0x9054,
+	},
+	{ }
 };
 MODULE_DEVICE_TABLE(pci, isp1760_plx);
 
diff --git a/drivers/usb/host/ohci-hcd.c b/drivers/usb/host/ohci-hcd.c
index 8aa3f4556a32..65a9609f4ad6 100644
--- a/drivers/usb/host/ohci-hcd.c
+++ b/drivers/usb/host/ohci-hcd.c
@@ -589,13 +589,15 @@ static int ohci_run (struct ohci_hcd *ohci)
 		/* also: power/overcurrent flags in roothub.a */
 	}
 
-	/* Reset USB nearly "by the book".  RemoteWakeupConnected was
-	 * saved if boot firmware (BIOS/SMM/...) told us it's connected,
-	 * or if bus glue did the same (e.g. for PCI add-in cards with
-	 * PCI PM support).
+	/* Reset USB nearly "by the book".  RemoteWakeupConnected has
+	 * to be checked in case boot firmware (BIOS/SMM/...) has set up
+	 * wakeup in a way the bus isn't aware of (e.g., legacy PCI PM).
+	 * If the bus glue detected wakeup capability then it should
+	 * already be enabled.  Either way, if wakeup should be enabled
+	 * but isn't, we'll enable it now.
 	 */
 	if ((ohci->hc_control & OHCI_CTRL_RWC) != 0
-			&& !device_may_wakeup(hcd->self.controller))
+			&& !device_can_wakeup(hcd->self.controller))
 		device_init_wakeup(hcd->self.controller, 1);
 
 	switch (ohci->hc_control & OHCI_CTRL_HCFS) {
diff --git a/drivers/usb/host/ohci-pci.c b/drivers/usb/host/ohci-pci.c
index a9c2ae36c7ad..8b28ae7865ba 100644
--- a/drivers/usb/host/ohci-pci.c
+++ b/drivers/usb/host/ohci-pci.c
@@ -355,9 +355,9 @@ static int __devinit ohci_pci_start (struct usb_hcd *hcd)
 
 		/* RWC may not be set for add-in PCI cards, since boot
 		 * firmware probably ignored them.  This transfers PCI
-		 * PM wakeup capabilities (once the PCI layer is fixed).
+		 * PM wakeup capabilities.
 		 */
-		if (device_may_wakeup(&pdev->dev))
+		if (device_can_wakeup(&pdev->dev))
 			ohci->hc_control |= OHCI_CTRL_RWC;
 	}
 #endif /* CONFIG_PM */
@@ -487,6 +487,8 @@ static struct pci_driver ohci_pci_driver = {
 
 #ifdef	CONFIG_PM
 	.suspend =	usb_hcd_pci_suspend,
+	.suspend_late =	usb_hcd_pci_suspend_late,
+	.resume_early =	usb_hcd_pci_resume_early,
 	.resume =	usb_hcd_pci_resume,
 #endif
 
diff --git a/drivers/usb/host/ohci-pnx4008.c b/drivers/usb/host/ohci-pnx4008.c
index e306ca6aef3d..100bf3d8437c 100644
--- a/drivers/usb/host/ohci-pnx4008.c
+++ b/drivers/usb/host/ohci-pnx4008.c
@@ -106,65 +106,34 @@ extern int ocpi_enable(void);
 
 static struct clk *usb_clk;
 
-static int isp1301_probe(struct i2c_adapter *adap);
-static int isp1301_detach(struct i2c_client *client);
-
 static const unsigned short normal_i2c[] =
     { ISP1301_I2C_ADDR, ISP1301_I2C_ADDR + 1, I2C_CLIENT_END };
-static const unsigned short dummy_i2c_addrlist[] = { I2C_CLIENT_END };
-
-static struct i2c_client_address_data addr_data = {
-	.normal_i2c = normal_i2c,
-	.probe = dummy_i2c_addrlist,
-	.ignore = dummy_i2c_addrlist,
-};
-
-struct i2c_driver isp1301_driver = {
-	.driver = {
-		.name = "isp1301_pnx",
-	},
-	.attach_adapter = isp1301_probe,
-	.detach_client = isp1301_detach,
-};
 
-static int isp1301_attach(struct i2c_adapter *adap, int addr, int kind)
+static int isp1301_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
 {
-	struct i2c_client *c;
-	int err;
-
-	c = kzalloc(sizeof(*c), GFP_KERNEL);
-	if (!c)
-		return -ENOMEM;
-
-	strlcpy(c->name, "isp1301_pnx", I2C_NAME_SIZE);
-	c->flags = 0;
-	c->addr = addr;
-	c->adapter = adap;
-	c->driver = &isp1301_driver;
-
-	err = i2c_attach_client(c);
-	if (err) {
-		kfree(c);
-		return err;
-	}
-
-	isp1301_i2c_client = c;
-
 	return 0;
 }
 
-static int isp1301_probe(struct i2c_adapter *adap)
+static int isp1301_remove(struct i2c_client *client)
 {
-	return i2c_probe(adap, &addr_data, isp1301_attach);
-}
-
-static int isp1301_detach(struct i2c_client *client)
-{
-	i2c_detach_client(client);
-	kfree(isp1301_i2c_client);
 	return 0;
 }
 
+const struct i2c_device_id isp1301_id[] = {
+	{ "isp1301_pnx", 0 },
+	{ }
+};
+
+struct i2c_driver isp1301_driver = {
+	.driver = {
+		.name = "isp1301_pnx",
+	},
+	.probe = isp1301_probe,
+	.remove = isp1301_remove,
+	.id_table = isp1301_id,
+};
+
 static void i2c_write(u8 buf, u8 subaddr)
 {
 	char tmpbuf[2];
@@ -328,6 +297,8 @@ static int __devinit usb_hcd_pnx4008_probe(struct platform_device *pdev)
 	struct usb_hcd *hcd = 0;
 	struct ohci_hcd *ohci;
 	const struct hc_driver *driver = &ohci_pnx4008_hc_driver;
+	struct i2c_adapter *i2c_adap;
+	struct i2c_board_info i2c_info;
 
 	int ret = 0, irq;
 
@@ -351,9 +322,20 @@ static int __devinit usb_hcd_pnx4008_probe(struct platform_device *pdev)
 
 	ret = i2c_add_driver(&isp1301_driver);
 	if (ret < 0) {
-		err("failed to connect I2C to ISP1301 USB Transceiver");
+		err("failed to add ISP1301 driver");
 		goto out;
 	}
+	i2c_adap = i2c_get_adapter(2);
+	memset(&i2c_info, 0, sizeof(struct i2c_board_info));
+	strlcpy(i2c_info.name, "isp1301_pnx", I2C_NAME_SIZE);
+	isp1301_i2c_client = i2c_new_probed_device(i2c_adap, &i2c_info,
+						   normal_i2c);
+	i2c_put_adapter(i2c_adap);
+	if (!isp1301_i2c_client) {
+		err("failed to connect I2C to ISP1301 USB Transceiver");
+		ret = -ENODEV;
+		goto out_i2c_driver;
+	}
 
 	isp1301_configure();
 
@@ -429,6 +411,9 @@ out3:
 out2:
 	clk_put(usb_clk);
 out1:
+	i2c_unregister_client(isp1301_i2c_client);
+	isp1301_i2c_client = NULL;
+out_i2c_driver:
 	i2c_del_driver(&isp1301_driver);
 out:
 	return ret;
@@ -445,6 +430,8 @@ static int usb_hcd_pnx4008_remove(struct platform_device *pdev)
 	pnx4008_unset_usb_bits();
 	clk_disable(usb_clk);
 	clk_put(usb_clk);
+	i2c_unregister_client(isp1301_i2c_client);
+	isp1301_i2c_client = NULL;
 	i2c_del_driver(&isp1301_driver);
 
 	platform_set_drvdata(pdev, NULL);
diff --git a/drivers/usb/host/ohci-ppc-of.c b/drivers/usb/host/ohci-ppc-of.c
index 7ac53264ead3..68a301710297 100644
--- a/drivers/usb/host/ohci-ppc-of.c
+++ b/drivers/usb/host/ohci-ppc-of.c
@@ -91,6 +91,7 @@ ohci_hcd_ppc_of_probe(struct of_device *op, const struct of_device_id *match)
 
 	int rv;
 	int is_bigendian;
+	struct device_node *np;
 
 	if (usb_disabled())
 		return -ENODEV;
@@ -147,6 +148,30 @@ ohci_hcd_ppc_of_probe(struct of_device *op, const struct of_device_id *match)
 	if (rv == 0)
 		return 0;
 
+	/* by now, 440epx is known to show usb_23 erratum */
+	np = of_find_compatible_node(NULL, NULL, "ibm,usb-ehci-440epx");
+
+	/* Work around - At this point ohci_run has executed, the
+	* controller is running, everything, the root ports, etc., is
+	* set up.  If the ehci driver is loaded, put the ohci core in
+	* the suspended state.  The ehci driver will bring it out of
+	* suspended state when / if a non-high speed USB device is
+	* attached to the USB Host port.  If the ehci driver is not
+	* loaded, do nothing. request_mem_region is used to test if
+	* the ehci driver is loaded.
+	*/
+	if (np !=  NULL) {
+		if (!of_address_to_resource(np, 0, &res)) {
+			if (!request_mem_region(res.start, 0x4, hcd_name)) {
+				writel_be((readl_be(&ohci->regs->control) |
+					OHCI_USB_SUSPEND), &ohci->regs->control);
+					(void) readl_be(&ohci->regs->control);
+			} else
+				release_mem_region(res.start, 0x4);
+		} else
+		    pr_debug(__FILE__ ": cannot get ehci offset from fdt\n");
+	}
+
 	iounmap(hcd->regs);
 err_ioremap:
 	irq_dispose_mapping(irq);
diff --git a/drivers/usb/host/ohci-tmio.c b/drivers/usb/host/ohci-tmio.c
index f9f134af0bd1..8dabe8e31d8c 100644
--- a/drivers/usb/host/ohci-tmio.c
+++ b/drivers/usb/host/ohci-tmio.c
@@ -201,7 +201,7 @@ static int __devinit ohci_hcd_tmio_drv_probe(struct platform_device *dev)
 	if (!cell)
 		return -EINVAL;
 
-	hcd = usb_create_hcd(&ohci_tmio_hc_driver, &dev->dev, dev->dev.bus_id);
+	hcd = usb_create_hcd(&ohci_tmio_hc_driver, &dev->dev, dev_name(&dev->dev));
 	if (!hcd) {
 		ret = -ENOMEM;
 		goto err_usb_create_hcd;
diff --git a/drivers/usb/host/oxu210hp-hcd.c b/drivers/usb/host/oxu210hp-hcd.c
new file mode 100644
index 000000000000..75548f7c716b
--- /dev/null
+++ b/drivers/usb/host/oxu210hp-hcd.c
@@ -0,0 +1,3985 @@
+/*
+ * Copyright (c) 2008 Rodolfo Giometti <giometti@linux.it>
+ * Copyright (c) 2008 Eurotech S.p.A. <info@eurtech.it>
+ *
+ * This code is *strongly* based on EHCI-HCD code by David Brownell since
+ * the chip is a quasi-EHCI compatible.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/dmapool.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/timer.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/reboot.h>
+#include <linux/usb.h>
+#include <linux/moduleparam.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+
+#include "../core/hcd.h"
+
+#include <asm/irq.h>
+#include <asm/system.h>
+#include <asm/unaligned.h>
+
+#include <linux/irq.h>
+#include <linux/platform_device.h>
+
+#include "oxu210hp.h"
+
+#define DRIVER_VERSION "0.0.50"
+
+/*
+ * Main defines
+ */
+
+#define oxu_dbg(oxu, fmt, args...) \
+		dev_dbg(oxu_to_hcd(oxu)->self.controller , fmt , ## args)
+#define oxu_err(oxu, fmt, args...) \
+		dev_err(oxu_to_hcd(oxu)->self.controller , fmt , ## args)
+#define oxu_info(oxu, fmt, args...) \
+		dev_info(oxu_to_hcd(oxu)->self.controller , fmt , ## args)
+
+static inline struct usb_hcd *oxu_to_hcd(struct oxu_hcd *oxu)
+{
+	return container_of((void *) oxu, struct usb_hcd, hcd_priv);
+}
+
+static inline struct oxu_hcd *hcd_to_oxu(struct usb_hcd *hcd)
+{
+	return (struct oxu_hcd *) (hcd->hcd_priv);
+}
+
+/*
+ * Debug stuff
+ */
+
+#undef OXU_URB_TRACE
+#undef OXU_VERBOSE_DEBUG
+
+#ifdef OXU_VERBOSE_DEBUG
+#define oxu_vdbg			oxu_dbg
+#else
+#define oxu_vdbg(oxu, fmt, args...)	/* Nop */
+#endif
+
+#ifdef DEBUG
+
+static int __attribute__((__unused__))
+dbg_status_buf(char *buf, unsigned len, const char *label, u32 status)
+{
+	return scnprintf(buf, len, "%s%sstatus %04x%s%s%s%s%s%s%s%s%s%s",
+		label, label[0] ? " " : "", status,
+		(status & STS_ASS) ? " Async" : "",
+		(status & STS_PSS) ? " Periodic" : "",
+		(status & STS_RECL) ? " Recl" : "",
+		(status & STS_HALT) ? " Halt" : "",
+		(status & STS_IAA) ? " IAA" : "",
+		(status & STS_FATAL) ? " FATAL" : "",
+		(status & STS_FLR) ? " FLR" : "",
+		(status & STS_PCD) ? " PCD" : "",
+		(status & STS_ERR) ? " ERR" : "",
+		(status & STS_INT) ? " INT" : ""
+		);
+}
+
+static int __attribute__((__unused__))
+dbg_intr_buf(char *buf, unsigned len, const char *label, u32 enable)
+{
+	return scnprintf(buf, len, "%s%sintrenable %02x%s%s%s%s%s%s",
+		label, label[0] ? " " : "", enable,
+		(enable & STS_IAA) ? " IAA" : "",
+		(enable & STS_FATAL) ? " FATAL" : "",
+		(enable & STS_FLR) ? " FLR" : "",
+		(enable & STS_PCD) ? " PCD" : "",
+		(enable & STS_ERR) ? " ERR" : "",
+		(enable & STS_INT) ? " INT" : ""
+		);
+}
+
+static const char *const fls_strings[] =
+    { "1024", "512", "256", "??" };
+
+static int dbg_command_buf(char *buf, unsigned len,
+				const char *label, u32 command)
+{
+	return scnprintf(buf, len,
+		"%s%scommand %06x %s=%d ithresh=%d%s%s%s%s period=%s%s %s",
+		label, label[0] ? " " : "", command,
+		(command & CMD_PARK) ? "park" : "(park)",
+		CMD_PARK_CNT(command),
+		(command >> 16) & 0x3f,
+		(command & CMD_LRESET) ? " LReset" : "",
+		(command & CMD_IAAD) ? " IAAD" : "",
+		(command & CMD_ASE) ? " Async" : "",
+		(command & CMD_PSE) ? " Periodic" : "",
+		fls_strings[(command >> 2) & 0x3],
+		(command & CMD_RESET) ? " Reset" : "",
+		(command & CMD_RUN) ? "RUN" : "HALT"
+		);
+}
+
+static int dbg_port_buf(char *buf, unsigned len, const char *label,
+				int port, u32 status)
+{
+	char	*sig;
+
+	/* signaling state */
+	switch (status & (3 << 10)) {
+	case 0 << 10:
+		sig = "se0";
+		break;
+	case 1 << 10:
+		sig = "k";	/* low speed */
+		break;
+	case 2 << 10:
+		sig = "j";
+		break;
+	default:
+		sig = "?";
+		break;
+	}
+
+	return scnprintf(buf, len,
+		"%s%sport %d status %06x%s%s sig=%s%s%s%s%s%s%s%s%s%s",
+		label, label[0] ? " " : "", port, status,
+		(status & PORT_POWER) ? " POWER" : "",
+		(status & PORT_OWNER) ? " OWNER" : "",
+		sig,
+		(status & PORT_RESET) ? " RESET" : "",
+		(status & PORT_SUSPEND) ? " SUSPEND" : "",
+		(status & PORT_RESUME) ? " RESUME" : "",
+		(status & PORT_OCC) ? " OCC" : "",
+		(status & PORT_OC) ? " OC" : "",
+		(status & PORT_PEC) ? " PEC" : "",
+		(status & PORT_PE) ? " PE" : "",
+		(status & PORT_CSC) ? " CSC" : "",
+		(status & PORT_CONNECT) ? " CONNECT" : ""
+	    );
+}
+
+#else
+
+static inline int __attribute__((__unused__))
+dbg_status_buf(char *buf, unsigned len, const char *label, u32 status)
+{ return 0; }
+
+static inline int __attribute__((__unused__))
+dbg_command_buf(char *buf, unsigned len, const char *label, u32 command)
+{ return 0; }
+
+static inline int __attribute__((__unused__))
+dbg_intr_buf(char *buf, unsigned len, const char *label, u32 enable)
+{ return 0; }
+
+static inline int __attribute__((__unused__))
+dbg_port_buf(char *buf, unsigned len, const char *label, int port, u32 status)
+{ return 0; }
+
+#endif /* DEBUG */
+
+/* functions have the "wrong" filename when they're output... */
+#define dbg_status(oxu, label, status) { \
+	char _buf[80]; \
+	dbg_status_buf(_buf, sizeof _buf, label, status); \
+	oxu_dbg(oxu, "%s\n", _buf); \
+}
+
+#define dbg_cmd(oxu, label, command) { \
+	char _buf[80]; \
+	dbg_command_buf(_buf, sizeof _buf, label, command); \
+	oxu_dbg(oxu, "%s\n", _buf); \
+}
+
+#define dbg_port(oxu, label, port, status) { \
+	char _buf[80]; \
+	dbg_port_buf(_buf, sizeof _buf, label, port, status); \
+	oxu_dbg(oxu, "%s\n", _buf); \
+}
+
+/*
+ * Module parameters
+ */
+
+/* Initial IRQ latency: faster than hw default */
+static int log2_irq_thresh;			/* 0 to 6 */
+module_param(log2_irq_thresh, int, S_IRUGO);
+MODULE_PARM_DESC(log2_irq_thresh, "log2 IRQ latency, 1-64 microframes");
+
+/* Initial park setting: slower than hw default */
+static unsigned park;
+module_param(park, uint, S_IRUGO);
+MODULE_PARM_DESC(park, "park setting; 1-3 back-to-back async packets");
+
+/* For flakey hardware, ignore overcurrent indicators */
+static int ignore_oc;
+module_param(ignore_oc, bool, S_IRUGO);
+MODULE_PARM_DESC(ignore_oc, "ignore bogus hardware overcurrent indications");
+
+
+static void ehci_work(struct oxu_hcd *oxu);
+static int oxu_hub_control(struct usb_hcd *hcd,
+				u16 typeReq, u16 wValue, u16 wIndex,
+				char *buf, u16 wLength);
+
+/*
+ * Local functions
+ */
+
+/* Low level read/write registers functions */
+static inline u32 oxu_readl(void *base, u32 reg)
+{
+	return readl(base + reg);
+}
+
+static inline void oxu_writel(void *base, u32 reg, u32 val)
+{
+	writel(val, base + reg);
+}
+
+static inline void timer_action_done(struct oxu_hcd *oxu,
+					enum ehci_timer_action action)
+{
+	clear_bit(action, &oxu->actions);
+}
+
+static inline void timer_action(struct oxu_hcd *oxu,
+					enum ehci_timer_action action)
+{
+	if (!test_and_set_bit(action, &oxu->actions)) {
+		unsigned long t;
+
+		switch (action) {
+		case TIMER_IAA_WATCHDOG:
+			t = EHCI_IAA_JIFFIES;
+			break;
+		case TIMER_IO_WATCHDOG:
+			t = EHCI_IO_JIFFIES;
+			break;
+		case TIMER_ASYNC_OFF:
+			t = EHCI_ASYNC_JIFFIES;
+			break;
+		case TIMER_ASYNC_SHRINK:
+		default:
+			t = EHCI_SHRINK_JIFFIES;
+			break;
+		}
+		t += jiffies;
+		/* all timings except IAA watchdog can be overridden.
+		 * async queue SHRINK often precedes IAA.  while it's ready
+		 * to go OFF neither can matter, and afterwards the IO
+		 * watchdog stops unless there's still periodic traffic.
+		 */
+		if (action != TIMER_IAA_WATCHDOG
+				&& t > oxu->watchdog.expires
+				&& timer_pending(&oxu->watchdog))
+			return;
+		mod_timer(&oxu->watchdog, t);
+	}
+}
+
+/*
+ * handshake - spin reading hc until handshake completes or fails
+ * @ptr: address of hc register to be read
+ * @mask: bits to look at in result of read
+ * @done: value of those bits when handshake succeeds
+ * @usec: timeout in microseconds
+ *
+ * Returns negative errno, or zero on success
+ *
+ * Success happens when the "mask" bits have the specified value (hardware
+ * handshake done).  There are two failure modes:  "usec" have passed (major
+ * hardware flakeout), or the register reads as all-ones (hardware removed).
+ *
+ * That last failure should_only happen in cases like physical cardbus eject
+ * before driver shutdown. But it also seems to be caused by bugs in cardbus
+ * bridge shutdown:  shutting down the bridge before the devices using it.
+ */
+static int handshake(struct oxu_hcd *oxu, void __iomem *ptr,
+					u32 mask, u32 done, int usec)
+{
+	u32 result;
+
+	do {
+		result = readl(ptr);
+		if (result == ~(u32)0)		/* card removed */
+			return -ENODEV;
+		result &= mask;
+		if (result == done)
+			return 0;
+		udelay(1);
+		usec--;
+	} while (usec > 0);
+	return -ETIMEDOUT;
+}
+
+/* Force HC to halt state from unknown (EHCI spec section 2.3) */
+static int ehci_halt(struct oxu_hcd *oxu)
+{
+	u32	temp = readl(&oxu->regs->status);
+
+	/* disable any irqs left enabled by previous code */
+	writel(0, &oxu->regs->intr_enable);
+
+	if ((temp & STS_HALT) != 0)
+		return 0;
+
+	temp = readl(&oxu->regs->command);
+	temp &= ~CMD_RUN;
+	writel(temp, &oxu->regs->command);
+	return handshake(oxu, &oxu->regs->status,
+			  STS_HALT, STS_HALT, 16 * 125);
+}
+
+/* Put TDI/ARC silicon into EHCI mode */
+static void tdi_reset(struct oxu_hcd *oxu)
+{
+	u32 __iomem *reg_ptr;
+	u32 tmp;
+
+	reg_ptr = (u32 __iomem *)(((u8 __iomem *)oxu->regs) + 0x68);
+	tmp = readl(reg_ptr);
+	tmp |= 0x3;
+	writel(tmp, reg_ptr);
+}
+
+/* Reset a non-running (STS_HALT == 1) controller */
+static int ehci_reset(struct oxu_hcd *oxu)
+{
+	int	retval;
+	u32	command = readl(&oxu->regs->command);
+
+	command |= CMD_RESET;
+	dbg_cmd(oxu, "reset", command);
+	writel(command, &oxu->regs->command);
+	oxu_to_hcd(oxu)->state = HC_STATE_HALT;
+	oxu->next_statechange = jiffies;
+	retval = handshake(oxu, &oxu->regs->command,
+			    CMD_RESET, 0, 250 * 1000);
+
+	if (retval)
+		return retval;
+
+	tdi_reset(oxu);
+
+	return retval;
+}
+
+/* Idle the controller (from running) */
+static void ehci_quiesce(struct oxu_hcd *oxu)
+{
+	u32	temp;
+
+#ifdef DEBUG
+	if (!HC_IS_RUNNING(oxu_to_hcd(oxu)->state))
+		BUG();
+#endif
+
+	/* wait for any schedule enables/disables to take effect */
+	temp = readl(&oxu->regs->command) << 10;
+	temp &= STS_ASS | STS_PSS;
+	if (handshake(oxu, &oxu->regs->status, STS_ASS | STS_PSS,
+				temp, 16 * 125) != 0) {
+		oxu_to_hcd(oxu)->state = HC_STATE_HALT;
+		return;
+	}
+
+	/* then disable anything that's still active */
+	temp = readl(&oxu->regs->command);
+	temp &= ~(CMD_ASE | CMD_IAAD | CMD_PSE);
+	writel(temp, &oxu->regs->command);
+
+	/* hardware can take 16 microframes to turn off ... */
+	if (handshake(oxu, &oxu->regs->status, STS_ASS | STS_PSS,
+				0, 16 * 125) != 0) {
+		oxu_to_hcd(oxu)->state = HC_STATE_HALT;
+		return;
+	}
+}
+
+static int check_reset_complete(struct oxu_hcd *oxu, int index,
+				u32 __iomem *status_reg, int port_status)
+{
+	if (!(port_status & PORT_CONNECT)) {
+		oxu->reset_done[index] = 0;
+		return port_status;
+	}
+
+	/* if reset finished and it's still not enabled -- handoff */
+	if (!(port_status & PORT_PE)) {
+		oxu_dbg(oxu, "Failed to enable port %d on root hub TT\n",
+				index+1);
+		return port_status;
+	} else
+		oxu_dbg(oxu, "port %d high speed\n", index + 1);
+
+	return port_status;
+}
+
+static void ehci_hub_descriptor(struct oxu_hcd *oxu,
+				struct usb_hub_descriptor *desc)
+{
+	int ports = HCS_N_PORTS(oxu->hcs_params);
+	u16 temp;
+
+	desc->bDescriptorType = 0x29;
+	desc->bPwrOn2PwrGood = 10;	/* oxu 1.0, 2.3.9 says 20ms max */
+	desc->bHubContrCurrent = 0;
+
+	desc->bNbrPorts = ports;
+	temp = 1 + (ports / 8);
+	desc->bDescLength = 7 + 2 * temp;
+
+	/* two bitmaps:  ports removable, and usb 1.0 legacy PortPwrCtrlMask */
+	memset(&desc->bitmap[0], 0, temp);
+	memset(&desc->bitmap[temp], 0xff, temp);
+
+	temp = 0x0008;			/* per-port overcurrent reporting */
+	if (HCS_PPC(oxu->hcs_params))
+		temp |= 0x0001;		/* per-port power control */
+	else
+		temp |= 0x0002;		/* no power switching */
+	desc->wHubCharacteristics = (__force __u16)cpu_to_le16(temp);
+}
+
+
+/* Allocate an OXU210HP on-chip memory data buffer
+ *
+ * An on-chip memory data buffer is required for each OXU210HP USB transfer.
+ * Each transfer descriptor has one or more on-chip memory data buffers.
+ *
+ * Data buffers are allocated from a fix sized pool of data blocks.
+ * To minimise fragmentation and give reasonable memory utlisation,
+ * data buffers are allocated with sizes the power of 2 multiples of
+ * the block size, starting on an address a multiple of the allocated size.
+ *
+ * FIXME: callers of this function require a buffer to be allocated for
+ * len=0. This is a waste of on-chip memory and should be fix. Then this
+ * function should be changed to not allocate a buffer for len=0.
+ */
+static int oxu_buf_alloc(struct oxu_hcd *oxu, struct ehci_qtd *qtd, int len)
+{
+	int n_blocks;	/* minium blocks needed to hold len */
+	int a_blocks;	/* blocks allocated */
+	int i, j;
+
+	/* Don't allocte bigger than supported */
+	if (len > BUFFER_SIZE * BUFFER_NUM) {
+		oxu_err(oxu, "buffer too big (%d)\n", len);
+		return -ENOMEM;
+	}
+
+	spin_lock(&oxu->mem_lock);
+
+	/* Number of blocks needed to hold len */
+	n_blocks = (len + BUFFER_SIZE - 1) / BUFFER_SIZE;
+
+	/* Round the number of blocks up to the power of 2 */
+	for (a_blocks = 1; a_blocks < n_blocks; a_blocks <<= 1)
+		;
+
+	/* Find a suitable available data buffer */
+	for (i = 0; i < BUFFER_NUM;
+			i += max(a_blocks, (int)oxu->db_used[i])) {
+
+		/* Check all the required blocks are available */
+		for (j = 0; j < a_blocks; j++)
+			if (oxu->db_used[i + j])
+				break;
+
+		if (j != a_blocks)
+			continue;
+
+		/* Allocate blocks found! */
+		qtd->buffer = (void *) &oxu->mem->db_pool[i];
+		qtd->buffer_dma = virt_to_phys(qtd->buffer);
+
+		qtd->qtd_buffer_len = BUFFER_SIZE * a_blocks;
+		oxu->db_used[i] = a_blocks;
+
+		spin_unlock(&oxu->mem_lock);
+
+		return 0;
+	}
+
+	/* Failed */
+
+	spin_unlock(&oxu->mem_lock);
+
+	return -ENOMEM;
+}
+
+static void oxu_buf_free(struct oxu_hcd *oxu, struct ehci_qtd *qtd)
+{
+	int index;
+
+	spin_lock(&oxu->mem_lock);
+
+	index = (qtd->buffer - (void *) &oxu->mem->db_pool[0])
+							 / BUFFER_SIZE;
+	oxu->db_used[index] = 0;
+	qtd->qtd_buffer_len = 0;
+	qtd->buffer_dma = 0;
+	qtd->buffer = NULL;
+
+	spin_unlock(&oxu->mem_lock);
+
+	return;
+}
+
+static inline void ehci_qtd_init(struct ehci_qtd *qtd, dma_addr_t dma)
+{
+	memset(qtd, 0, sizeof *qtd);
+	qtd->qtd_dma = dma;
+	qtd->hw_token = cpu_to_le32(QTD_STS_HALT);
+	qtd->hw_next = EHCI_LIST_END;
+	qtd->hw_alt_next = EHCI_LIST_END;
+	INIT_LIST_HEAD(&qtd->qtd_list);
+}
+
+static inline void oxu_qtd_free(struct oxu_hcd *oxu, struct ehci_qtd *qtd)
+{
+	int index;
+
+	if (qtd->buffer)
+		oxu_buf_free(oxu, qtd);
+
+	spin_lock(&oxu->mem_lock);
+
+	index = qtd - &oxu->mem->qtd_pool[0];
+	oxu->qtd_used[index] = 0;
+
+	spin_unlock(&oxu->mem_lock);
+
+	return;
+}
+
+static struct ehci_qtd *ehci_qtd_alloc(struct oxu_hcd *oxu)
+{
+	int i;
+	struct ehci_qtd *qtd = NULL;
+
+	spin_lock(&oxu->mem_lock);
+
+	for (i = 0; i < QTD_NUM; i++)
+		if (!oxu->qtd_used[i])
+			break;
+
+	if (i < QTD_NUM) {
+		qtd = (struct ehci_qtd *) &oxu->mem->qtd_pool[i];
+		memset(qtd, 0, sizeof *qtd);
+
+		qtd->hw_token = cpu_to_le32(QTD_STS_HALT);
+		qtd->hw_next = EHCI_LIST_END;
+		qtd->hw_alt_next = EHCI_LIST_END;
+		INIT_LIST_HEAD(&qtd->qtd_list);
+
+		qtd->qtd_dma = virt_to_phys(qtd);
+
+		oxu->qtd_used[i] = 1;
+	}
+
+	spin_unlock(&oxu->mem_lock);
+
+	return qtd;
+}
+
+static void oxu_qh_free(struct oxu_hcd *oxu, struct ehci_qh *qh)
+{
+	int index;
+
+	spin_lock(&oxu->mem_lock);
+
+	index = qh - &oxu->mem->qh_pool[0];
+	oxu->qh_used[index] = 0;
+
+	spin_unlock(&oxu->mem_lock);
+
+	return;
+}
+
+static void qh_destroy(struct kref *kref)
+{
+	struct ehci_qh *qh = container_of(kref, struct ehci_qh, kref);
+	struct oxu_hcd *oxu = qh->oxu;
+
+	/* clean qtds first, and know this is not linked */
+	if (!list_empty(&qh->qtd_list) || qh->qh_next.ptr) {
+		oxu_dbg(oxu, "unused qh not empty!\n");
+		BUG();
+	}
+	if (qh->dummy)
+		oxu_qtd_free(oxu, qh->dummy);
+	oxu_qh_free(oxu, qh);
+}
+
+static struct ehci_qh *oxu_qh_alloc(struct oxu_hcd *oxu)
+{
+	int i;
+	struct ehci_qh *qh = NULL;
+
+	spin_lock(&oxu->mem_lock);
+
+	for (i = 0; i < QHEAD_NUM; i++)
+		if (!oxu->qh_used[i])
+			break;
+
+	if (i < QHEAD_NUM) {
+		qh = (struct ehci_qh *) &oxu->mem->qh_pool[i];
+		memset(qh, 0, sizeof *qh);
+
+		kref_init(&qh->kref);
+		qh->oxu = oxu;
+		qh->qh_dma = virt_to_phys(qh);
+		INIT_LIST_HEAD(&qh->qtd_list);
+
+		/* dummy td enables safe urb queuing */
+		qh->dummy = ehci_qtd_alloc(oxu);
+		if (qh->dummy == NULL) {
+			oxu_dbg(oxu, "no dummy td\n");
+			oxu->qh_used[i] = 0;
+
+			return NULL;
+		}
+
+		oxu->qh_used[i] = 1;
+	}
+
+	spin_unlock(&oxu->mem_lock);
+
+	return qh;
+}
+
+/* to share a qh (cpu threads, or hc) */
+static inline struct ehci_qh *qh_get(struct ehci_qh *qh)
+{
+	kref_get(&qh->kref);
+	return qh;
+}
+
+static inline void qh_put(struct ehci_qh *qh)
+{
+	kref_put(&qh->kref, qh_destroy);
+}
+
+static void oxu_murb_free(struct oxu_hcd *oxu, struct oxu_murb *murb)
+{
+	int index;
+
+	spin_lock(&oxu->mem_lock);
+
+	index = murb - &oxu->murb_pool[0];
+	oxu->murb_used[index] = 0;
+
+	spin_unlock(&oxu->mem_lock);
+
+	return;
+}
+
+static struct oxu_murb *oxu_murb_alloc(struct oxu_hcd *oxu)
+
+{
+	int i;
+	struct oxu_murb *murb = NULL;
+
+	spin_lock(&oxu->mem_lock);
+
+	for (i = 0; i < MURB_NUM; i++)
+		if (!oxu->murb_used[i])
+			break;
+
+	if (i < MURB_NUM) {
+		murb = &(oxu->murb_pool)[i];
+
+		oxu->murb_used[i] = 1;
+	}
+
+	spin_unlock(&oxu->mem_lock);
+
+	return murb;
+}
+
+/* The queue heads and transfer descriptors are managed from pools tied
+ * to each of the "per device" structures.
+ * This is the initialisation and cleanup code.
+ */
+static void ehci_mem_cleanup(struct oxu_hcd *oxu)
+{
+	kfree(oxu->murb_pool);
+	oxu->murb_pool = NULL;
+
+	if (oxu->async)
+		qh_put(oxu->async);
+	oxu->async = NULL;
+
+	del_timer(&oxu->urb_timer);
+
+	oxu->periodic = NULL;
+
+	/* shadow periodic table */
+	kfree(oxu->pshadow);
+	oxu->pshadow = NULL;
+}
+
+/* Remember to add cleanup code (above) if you add anything here.
+ */
+static int ehci_mem_init(struct oxu_hcd *oxu, gfp_t flags)
+{
+	int i;
+
+	for (i = 0; i < oxu->periodic_size; i++)
+		oxu->mem->frame_list[i] = EHCI_LIST_END;
+	for (i = 0; i < QHEAD_NUM; i++)
+		oxu->qh_used[i] = 0;
+	for (i = 0; i < QTD_NUM; i++)
+		oxu->qtd_used[i] = 0;
+
+	oxu->murb_pool = kcalloc(MURB_NUM, sizeof(struct oxu_murb), flags);
+	if (!oxu->murb_pool)
+		goto fail;
+
+	for (i = 0; i < MURB_NUM; i++)
+		oxu->murb_used[i] = 0;
+
+	oxu->async = oxu_qh_alloc(oxu);
+	if (!oxu->async)
+		goto fail;
+
+	oxu->periodic = (__le32 *) &oxu->mem->frame_list;
+	oxu->periodic_dma = virt_to_phys(oxu->periodic);
+
+	for (i = 0; i < oxu->periodic_size; i++)
+		oxu->periodic[i] = EHCI_LIST_END;
+
+	/* software shadow of hardware table */
+	oxu->pshadow = kcalloc(oxu->periodic_size, sizeof(void *), flags);
+	if (oxu->pshadow != NULL)
+		return 0;
+
+fail:
+	oxu_dbg(oxu, "couldn't init memory\n");
+	ehci_mem_cleanup(oxu);
+	return -ENOMEM;
+}
+
+/* Fill a qtd, returning how much of the buffer we were able to queue up.
+ */
+static int qtd_fill(struct ehci_qtd *qtd, dma_addr_t buf, size_t len,
+				int token, int maxpacket)
+{
+	int i, count;
+	u64 addr = buf;
+
+	/* one buffer entry per 4K ... first might be short or unaligned */
+	qtd->hw_buf[0] = cpu_to_le32((u32)addr);
+	qtd->hw_buf_hi[0] = cpu_to_le32((u32)(addr >> 32));
+	count = 0x1000 - (buf & 0x0fff);	/* rest of that page */
+	if (likely(len < count))		/* ... iff needed */
+		count = len;
+	else {
+		buf +=  0x1000;
+		buf &= ~0x0fff;
+
+		/* per-qtd limit: from 16K to 20K (best alignment) */
+		for (i = 1; count < len && i < 5; i++) {
+			addr = buf;
+			qtd->hw_buf[i] = cpu_to_le32((u32)addr);
+			qtd->hw_buf_hi[i] = cpu_to_le32((u32)(addr >> 32));
+			buf += 0x1000;
+			if ((count + 0x1000) < len)
+				count += 0x1000;
+			else
+				count = len;
+		}
+
+		/* short packets may only terminate transfers */
+		if (count != len)
+			count -= (count % maxpacket);
+	}
+	qtd->hw_token = cpu_to_le32((count << 16) | token);
+	qtd->length = count;
+
+	return count;
+}
+
+static inline void qh_update(struct oxu_hcd *oxu,
+				struct ehci_qh *qh, struct ehci_qtd *qtd)
+{
+	/* writes to an active overlay are unsafe */
+	BUG_ON(qh->qh_state != QH_STATE_IDLE);
+
+	qh->hw_qtd_next = QTD_NEXT(qtd->qtd_dma);
+	qh->hw_alt_next = EHCI_LIST_END;
+
+	/* Except for control endpoints, we make hardware maintain data
+	 * toggle (like OHCI) ... here (re)initialize the toggle in the QH,
+	 * and set the pseudo-toggle in udev. Only usb_clear_halt() will
+	 * ever clear it.
+	 */
+	if (!(qh->hw_info1 & cpu_to_le32(1 << 14))) {
+		unsigned	is_out, epnum;
+
+		is_out = !(qtd->hw_token & cpu_to_le32(1 << 8));
+		epnum = (le32_to_cpup(&qh->hw_info1) >> 8) & 0x0f;
+		if (unlikely(!usb_gettoggle(qh->dev, epnum, is_out))) {
+			qh->hw_token &= ~__constant_cpu_to_le32(QTD_TOGGLE);
+			usb_settoggle(qh->dev, epnum, is_out, 1);
+		}
+	}
+
+	/* HC must see latest qtd and qh data before we clear ACTIVE+HALT */
+	wmb();
+	qh->hw_token &= __constant_cpu_to_le32(QTD_TOGGLE | QTD_STS_PING);
+}
+
+/* If it weren't for a common silicon quirk (writing the dummy into the qh
+ * overlay, so qh->hw_token wrongly becomes inactive/halted), only fault
+ * recovery (including urb dequeue) would need software changes to a QH...
+ */
+static void qh_refresh(struct oxu_hcd *oxu, struct ehci_qh *qh)
+{
+	struct ehci_qtd *qtd;
+
+	if (list_empty(&qh->qtd_list))
+		qtd = qh->dummy;
+	else {
+		qtd = list_entry(qh->qtd_list.next,
+				struct ehci_qtd, qtd_list);
+		/* first qtd may already be partially processed */
+		if (cpu_to_le32(qtd->qtd_dma) == qh->hw_current)
+			qtd = NULL;
+	}
+
+	if (qtd)
+		qh_update(oxu, qh, qtd);
+}
+
+static void qtd_copy_status(struct oxu_hcd *oxu, struct urb *urb,
+				size_t length, u32 token)
+{
+	/* count IN/OUT bytes, not SETUP (even short packets) */
+	if (likely(QTD_PID(token) != 2))
+		urb->actual_length += length - QTD_LENGTH(token);
+
+	/* don't modify error codes */
+	if (unlikely(urb->status != -EINPROGRESS))
+		return;
+
+	/* force cleanup after short read; not always an error */
+	if (unlikely(IS_SHORT_READ(token)))
+		urb->status = -EREMOTEIO;
+
+	/* serious "can't proceed" faults reported by the hardware */
+	if (token & QTD_STS_HALT) {
+		if (token & QTD_STS_BABBLE) {
+			/* FIXME "must" disable babbling device's port too */
+			urb->status = -EOVERFLOW;
+		} else if (token & QTD_STS_MMF) {
+			/* fs/ls interrupt xfer missed the complete-split */
+			urb->status = -EPROTO;
+		} else if (token & QTD_STS_DBE) {
+			urb->status = (QTD_PID(token) == 1) /* IN ? */
+				? -ENOSR  /* hc couldn't read data */
+				: -ECOMM; /* hc couldn't write data */
+		} else if (token & QTD_STS_XACT) {
+			/* timeout, bad crc, wrong PID, etc; retried */
+			if (QTD_CERR(token))
+				urb->status = -EPIPE;
+			else {
+				oxu_dbg(oxu, "devpath %s ep%d%s 3strikes\n",
+					urb->dev->devpath,
+					usb_pipeendpoint(urb->pipe),
+					usb_pipein(urb->pipe) ? "in" : "out");
+				urb->status = -EPROTO;
+			}
+		/* CERR nonzero + no errors + halt --> stall */
+		} else if (QTD_CERR(token))
+			urb->status = -EPIPE;
+		else	/* unknown */
+			urb->status = -EPROTO;
+
+		oxu_vdbg(oxu, "dev%d ep%d%s qtd token %08x --> status %d\n",
+			usb_pipedevice(urb->pipe),
+			usb_pipeendpoint(urb->pipe),
+			usb_pipein(urb->pipe) ? "in" : "out",
+			token, urb->status);
+	}
+}
+
+static void ehci_urb_done(struct oxu_hcd *oxu, struct urb *urb)
+__releases(oxu->lock)
+__acquires(oxu->lock)
+{
+	if (likely(urb->hcpriv != NULL)) {
+		struct ehci_qh	*qh = (struct ehci_qh *) urb->hcpriv;
+
+		/* S-mask in a QH means it's an interrupt urb */
+		if ((qh->hw_info2 & __constant_cpu_to_le32(QH_SMASK)) != 0) {
+
+			/* ... update hc-wide periodic stats (for usbfs) */
+			oxu_to_hcd(oxu)->self.bandwidth_int_reqs--;
+		}
+		qh_put(qh);
+	}
+
+	urb->hcpriv = NULL;
+	switch (urb->status) {
+	case -EINPROGRESS:		/* success */
+		urb->status = 0;
+	default:			/* fault */
+		break;
+	case -EREMOTEIO:		/* fault or normal */
+		if (!(urb->transfer_flags & URB_SHORT_NOT_OK))
+			urb->status = 0;
+		break;
+	case -ECONNRESET:		/* canceled */
+	case -ENOENT:
+		break;
+	}
+
+#ifdef OXU_URB_TRACE
+	oxu_dbg(oxu, "%s %s urb %p ep%d%s status %d len %d/%d\n",
+		__func__, urb->dev->devpath, urb,
+		usb_pipeendpoint(urb->pipe),
+		usb_pipein(urb->pipe) ? "in" : "out",
+		urb->status,
+		urb->actual_length, urb->transfer_buffer_length);
+#endif
+
+	/* complete() can reenter this HCD */
+	spin_unlock(&oxu->lock);
+	usb_hcd_giveback_urb(oxu_to_hcd(oxu), urb, urb->status);
+	spin_lock(&oxu->lock);
+}
+
+static void start_unlink_async(struct oxu_hcd *oxu, struct ehci_qh *qh);
+static void unlink_async(struct oxu_hcd *oxu, struct ehci_qh *qh);
+
+static void intr_deschedule(struct oxu_hcd *oxu, struct ehci_qh *qh);
+static int qh_schedule(struct oxu_hcd *oxu, struct ehci_qh *qh);
+
+#define HALT_BIT __constant_cpu_to_le32(QTD_STS_HALT)
+
+/* Process and free completed qtds for a qh, returning URBs to drivers.
+ * Chases up to qh->hw_current.  Returns number of completions called,
+ * indicating how much "real" work we did.
+ */
+static unsigned qh_completions(struct oxu_hcd *oxu, struct ehci_qh *qh)
+{
+	struct ehci_qtd *last = NULL, *end = qh->dummy;
+	struct list_head *entry, *tmp;
+	int stopped;
+	unsigned count = 0;
+	int do_status = 0;
+	u8 state;
+	struct oxu_murb *murb = NULL;
+
+	if (unlikely(list_empty(&qh->qtd_list)))
+		return count;
+
+	/* completions (or tasks on other cpus) must never clobber HALT
+	 * till we've gone through and cleaned everything up, even when
+	 * they add urbs to this qh's queue or mark them for unlinking.
+	 *
+	 * NOTE:  unlinking expects to be done in queue order.
+	 */
+	state = qh->qh_state;
+	qh->qh_state = QH_STATE_COMPLETING;
+	stopped = (state == QH_STATE_IDLE);
+
+	/* remove de-activated QTDs from front of queue.
+	 * after faults (including short reads), cleanup this urb
+	 * then let the queue advance.
+	 * if queue is stopped, handles unlinks.
+	 */
+	list_for_each_safe(entry, tmp, &qh->qtd_list) {
+		struct ehci_qtd	*qtd;
+		struct urb *urb;
+		u32 token = 0;
+
+		qtd = list_entry(entry, struct ehci_qtd, qtd_list);
+		urb = qtd->urb;
+
+		/* Clean up any state from previous QTD ...*/
+		if (last) {
+			if (likely(last->urb != urb)) {
+				if (last->urb->complete == NULL) {
+					murb = (struct oxu_murb *) last->urb;
+					last->urb = murb->main;
+					if (murb->last) {
+						ehci_urb_done(oxu, last->urb);
+						count++;
+					}
+					oxu_murb_free(oxu, murb);
+				} else {
+					ehci_urb_done(oxu, last->urb);
+					count++;
+				}
+			}
+			oxu_qtd_free(oxu, last);
+			last = NULL;
+		}
+
+		/* ignore urbs submitted during completions we reported */
+		if (qtd == end)
+			break;
+
+		/* hardware copies qtd out of qh overlay */
+		rmb();
+		token = le32_to_cpu(qtd->hw_token);
+
+		/* always clean up qtds the hc de-activated */
+		if ((token & QTD_STS_ACTIVE) == 0) {
+
+			if ((token & QTD_STS_HALT) != 0) {
+				stopped = 1;
+
+			/* magic dummy for some short reads; qh won't advance.
+			 * that silicon quirk can kick in with this dummy too.
+			 */
+			} else if (IS_SHORT_READ(token) &&
+					!(qtd->hw_alt_next & EHCI_LIST_END)) {
+				stopped = 1;
+				goto halt;
+			}
+
+		/* stop scanning when we reach qtds the hc is using */
+		} else if (likely(!stopped &&
+				HC_IS_RUNNING(oxu_to_hcd(oxu)->state))) {
+			break;
+
+		} else {
+			stopped = 1;
+
+			if (unlikely(!HC_IS_RUNNING(oxu_to_hcd(oxu)->state)))
+				urb->status = -ESHUTDOWN;
+
+			/* ignore active urbs unless some previous qtd
+			 * for the urb faulted (including short read) or
+			 * its urb was canceled.  we may patch qh or qtds.
+			 */
+			if (likely(urb->status == -EINPROGRESS))
+				continue;
+
+			/* issue status after short control reads */
+			if (unlikely(do_status != 0)
+					&& QTD_PID(token) == 0 /* OUT */) {
+				do_status = 0;
+				continue;
+			}
+
+			/* token in overlay may be most current */
+			if (state == QH_STATE_IDLE
+					&& cpu_to_le32(qtd->qtd_dma)
+						== qh->hw_current)
+				token = le32_to_cpu(qh->hw_token);
+
+			/* force halt for unlinked or blocked qh, so we'll
+			 * patch the qh later and so that completions can't
+			 * activate it while we "know" it's stopped.
+			 */
+			if ((HALT_BIT & qh->hw_token) == 0) {
+halt:
+				qh->hw_token |= HALT_BIT;
+				wmb();
+			}
+		}
+
+		/* Remove it from the queue */
+		qtd_copy_status(oxu, urb->complete ?
+					urb : ((struct oxu_murb *) urb)->main,
+				qtd->length, token);
+		if ((usb_pipein(qtd->urb->pipe)) &&
+				(NULL != qtd->transfer_buffer))
+			memcpy(qtd->transfer_buffer, qtd->buffer, qtd->length);
+		do_status = (urb->status == -EREMOTEIO)
+				&& usb_pipecontrol(urb->pipe);
+
+		if (stopped && qtd->qtd_list.prev != &qh->qtd_list) {
+			last = list_entry(qtd->qtd_list.prev,
+					struct ehci_qtd, qtd_list);
+			last->hw_next = qtd->hw_next;
+		}
+		list_del(&qtd->qtd_list);
+		last = qtd;
+	}
+
+	/* last urb's completion might still need calling */
+	if (likely(last != NULL)) {
+		if (last->urb->complete == NULL) {
+			murb = (struct oxu_murb *) last->urb;
+			last->urb = murb->main;
+			if (murb->last) {
+				ehci_urb_done(oxu, last->urb);
+				count++;
+			}
+			oxu_murb_free(oxu, murb);
+		} else {
+			ehci_urb_done(oxu, last->urb);
+			count++;
+		}
+		oxu_qtd_free(oxu, last);
+	}
+
+	/* restore original state; caller must unlink or relink */
+	qh->qh_state = state;
+
+	/* be sure the hardware's done with the qh before refreshing
+	 * it after fault cleanup, or recovering from silicon wrongly
+	 * overlaying the dummy qtd (which reduces DMA chatter).
+	 */
+	if (stopped != 0 || qh->hw_qtd_next == EHCI_LIST_END) {
+		switch (state) {
+		case QH_STATE_IDLE:
+			qh_refresh(oxu, qh);
+			break;
+		case QH_STATE_LINKED:
+			/* should be rare for periodic transfers,
+			 * except maybe high bandwidth ...
+			 */
+			if ((__constant_cpu_to_le32(QH_SMASK)
+					& qh->hw_info2) != 0) {
+				intr_deschedule(oxu, qh);
+				(void) qh_schedule(oxu, qh);
+			} else
+				unlink_async(oxu, qh);
+			break;
+		/* otherwise, unlink already started */
+		}
+	}
+
+	return count;
+}
+
+/* High bandwidth multiplier, as encoded in highspeed endpoint descriptors */
+#define hb_mult(wMaxPacketSize)		(1 + (((wMaxPacketSize) >> 11) & 0x03))
+/* ... and packet size, for any kind of endpoint descriptor */
+#define max_packet(wMaxPacketSize)	((wMaxPacketSize) & 0x07ff)
+
+/* Reverse of qh_urb_transaction: free a list of TDs.
+ * used for cleanup after errors, before HC sees an URB's TDs.
+ */
+static void qtd_list_free(struct oxu_hcd *oxu,
+				struct urb *urb, struct list_head *qtd_list)
+{
+	struct list_head *entry, *temp;
+
+	list_for_each_safe(entry, temp, qtd_list) {
+		struct ehci_qtd	*qtd;
+
+		qtd = list_entry(entry, struct ehci_qtd, qtd_list);
+		list_del(&qtd->qtd_list);
+		oxu_qtd_free(oxu, qtd);
+	}
+}
+
+/* Create a list of filled qtds for this URB; won't link into qh.
+ */
+static struct list_head *qh_urb_transaction(struct oxu_hcd *oxu,
+						struct urb *urb,
+						struct list_head *head,
+						gfp_t flags)
+{
+	struct ehci_qtd	*qtd, *qtd_prev;
+	dma_addr_t buf;
+	int len, maxpacket;
+	int is_input;
+	u32 token;
+	void *transfer_buf = NULL;
+	int ret;
+
+	/*
+	 * URBs map to sequences of QTDs: one logical transaction
+	 */
+	qtd = ehci_qtd_alloc(oxu);
+	if (unlikely(!qtd))
+		return NULL;
+	list_add_tail(&qtd->qtd_list, head);
+	qtd->urb = urb;
+
+	token = QTD_STS_ACTIVE;
+	token |= (EHCI_TUNE_CERR << 10);
+	/* for split transactions, SplitXState initialized to zero */
+
+	len = urb->transfer_buffer_length;
+	is_input = usb_pipein(urb->pipe);
+	if (!urb->transfer_buffer && urb->transfer_buffer_length && is_input)
+		urb->transfer_buffer = phys_to_virt(urb->transfer_dma);
+
+	if (usb_pipecontrol(urb->pipe)) {
+		/* SETUP pid */
+		ret = oxu_buf_alloc(oxu, qtd, sizeof(struct usb_ctrlrequest));
+		if (ret)
+			goto cleanup;
+
+		qtd_fill(qtd, qtd->buffer_dma, sizeof(struct usb_ctrlrequest),
+				token | (2 /* "setup" */ << 8), 8);
+		memcpy(qtd->buffer, qtd->urb->setup_packet,
+				sizeof(struct usb_ctrlrequest));
+
+		/* ... and always at least one more pid */
+		token ^= QTD_TOGGLE;
+		qtd_prev = qtd;
+		qtd = ehci_qtd_alloc(oxu);
+		if (unlikely(!qtd))
+			goto cleanup;
+		qtd->urb = urb;
+		qtd_prev->hw_next = QTD_NEXT(qtd->qtd_dma);
+		list_add_tail(&qtd->qtd_list, head);
+
+		/* for zero length DATA stages, STATUS is always IN */
+		if (len == 0)
+			token |= (1 /* "in" */ << 8);
+	}
+
+	/*
+	 * Data transfer stage: buffer setup
+	 */
+
+	ret = oxu_buf_alloc(oxu, qtd, len);
+	if (ret)
+		goto cleanup;
+
+	buf = qtd->buffer_dma;
+	transfer_buf = urb->transfer_buffer;
+
+	if (!is_input)
+		memcpy(qtd->buffer, qtd->urb->transfer_buffer, len);
+
+	if (is_input)
+		token |= (1 /* "in" */ << 8);
+	/* else it's already initted to "out" pid (0 << 8) */
+
+	maxpacket = max_packet(usb_maxpacket(urb->dev, urb->pipe, !is_input));
+
+	/*
+	 * buffer gets wrapped in one or more qtds;
+	 * last one may be "short" (including zero len)
+	 * and may serve as a control status ack
+	 */
+	for (;;) {
+		int this_qtd_len;
+
+		this_qtd_len = qtd_fill(qtd, buf, len, token, maxpacket);
+		qtd->transfer_buffer = transfer_buf;
+		len -= this_qtd_len;
+		buf += this_qtd_len;
+		transfer_buf += this_qtd_len;
+		if (is_input)
+			qtd->hw_alt_next = oxu->async->hw_alt_next;
+
+		/* qh makes control packets use qtd toggle; maybe switch it */
+		if ((maxpacket & (this_qtd_len + (maxpacket - 1))) == 0)
+			token ^= QTD_TOGGLE;
+
+		if (likely(len <= 0))
+			break;
+
+		qtd_prev = qtd;
+		qtd = ehci_qtd_alloc(oxu);
+		if (unlikely(!qtd))
+			goto cleanup;
+		if (likely(len > 0)) {
+			ret = oxu_buf_alloc(oxu, qtd, len);
+			if (ret)
+				goto cleanup;
+		}
+		qtd->urb = urb;
+		qtd_prev->hw_next = QTD_NEXT(qtd->qtd_dma);
+		list_add_tail(&qtd->qtd_list, head);
+	}
+
+	/* unless the bulk/interrupt caller wants a chance to clean
+	 * up after short reads, hc should advance qh past this urb
+	 */
+	if (likely((urb->transfer_flags & URB_SHORT_NOT_OK) == 0
+				|| usb_pipecontrol(urb->pipe)))
+		qtd->hw_alt_next = EHCI_LIST_END;
+
+	/*
+	 * control requests may need a terminating data "status" ack;
+	 * bulk ones may need a terminating short packet (zero length).
+	 */
+	if (likely(urb->transfer_buffer_length != 0)) {
+		int	one_more = 0;
+
+		if (usb_pipecontrol(urb->pipe)) {
+			one_more = 1;
+			token ^= 0x0100;	/* "in" <--> "out"  */
+			token |= QTD_TOGGLE;	/* force DATA1 */
+		} else if (usb_pipebulk(urb->pipe)
+				&& (urb->transfer_flags & URB_ZERO_PACKET)
+				&& !(urb->transfer_buffer_length % maxpacket)) {
+			one_more = 1;
+		}
+		if (one_more) {
+			qtd_prev = qtd;
+			qtd = ehci_qtd_alloc(oxu);
+			if (unlikely(!qtd))
+				goto cleanup;
+			qtd->urb = urb;
+			qtd_prev->hw_next = QTD_NEXT(qtd->qtd_dma);
+			list_add_tail(&qtd->qtd_list, head);
+
+			/* never any data in such packets */
+			qtd_fill(qtd, 0, 0, token, 0);
+		}
+	}
+
+	/* by default, enable interrupt on urb completion */
+		qtd->hw_token |= __constant_cpu_to_le32(QTD_IOC);
+	return head;
+
+cleanup:
+	qtd_list_free(oxu, urb, head);
+	return NULL;
+}
+
+/* Each QH holds a qtd list; a QH is used for everything except iso.
+ *
+ * For interrupt urbs, the scheduler must set the microframe scheduling
+ * mask(s) each time the QH gets scheduled.  For highspeed, that's
+ * just one microframe in the s-mask.  For split interrupt transactions
+ * there are additional complications: c-mask, maybe FSTNs.
+ */
+static struct ehci_qh *qh_make(struct oxu_hcd *oxu,
+				struct urb *urb, gfp_t flags)
+{
+	struct ehci_qh *qh = oxu_qh_alloc(oxu);
+	u32 info1 = 0, info2 = 0;
+	int is_input, type;
+	int maxp = 0;
+
+	if (!qh)
+		return qh;
+
+	/*
+	 * init endpoint/device data for this QH
+	 */
+	info1 |= usb_pipeendpoint(urb->pipe) << 8;
+	info1 |= usb_pipedevice(urb->pipe) << 0;
+
+	is_input = usb_pipein(urb->pipe);
+	type = usb_pipetype(urb->pipe);
+	maxp = usb_maxpacket(urb->dev, urb->pipe, !is_input);
+
+	/* Compute interrupt scheduling parameters just once, and save.
+	 * - allowing for high bandwidth, how many nsec/uframe are used?
+	 * - split transactions need a second CSPLIT uframe; same question
+	 * - splits also need a schedule gap (for full/low speed I/O)
+	 * - qh has a polling interval
+	 *
+	 * For control/bulk requests, the HC or TT handles these.
+	 */
+	if (type == PIPE_INTERRUPT) {
+		qh->usecs = NS_TO_US(usb_calc_bus_time(USB_SPEED_HIGH,
+								is_input, 0,
+				hb_mult(maxp) * max_packet(maxp)));
+		qh->start = NO_FRAME;
+
+		if (urb->dev->speed == USB_SPEED_HIGH) {
+			qh->c_usecs = 0;
+			qh->gap_uf = 0;
+
+			qh->period = urb->interval >> 3;
+			if (qh->period == 0 && urb->interval != 1) {
+				/* NOTE interval 2 or 4 uframes could work.
+				 * But interval 1 scheduling is simpler, and
+				 * includes high bandwidth.
+				 */
+				dbg("intr period %d uframes, NYET!",
+						urb->interval);
+				goto done;
+			}
+		} else {
+			struct usb_tt	*tt = urb->dev->tt;
+			int		think_time;
+
+			/* gap is f(FS/LS transfer times) */
+			qh->gap_uf = 1 + usb_calc_bus_time(urb->dev->speed,
+					is_input, 0, maxp) / (125 * 1000);
+
+			/* FIXME this just approximates SPLIT/CSPLIT times */
+			if (is_input) {		/* SPLIT, gap, CSPLIT+DATA */
+				qh->c_usecs = qh->usecs + HS_USECS(0);
+				qh->usecs = HS_USECS(1);
+			} else {		/* SPLIT+DATA, gap, CSPLIT */
+				qh->usecs += HS_USECS(1);
+				qh->c_usecs = HS_USECS(0);
+			}
+
+			think_time = tt ? tt->think_time : 0;
+			qh->tt_usecs = NS_TO_US(think_time +
+					usb_calc_bus_time(urb->dev->speed,
+					is_input, 0, max_packet(maxp)));
+			qh->period = urb->interval;
+		}
+	}
+
+	/* support for tt scheduling, and access to toggles */
+	qh->dev = urb->dev;
+
+	/* using TT? */
+	switch (urb->dev->speed) {
+	case USB_SPEED_LOW:
+		info1 |= (1 << 12);	/* EPS "low" */
+		/* FALL THROUGH */
+
+	case USB_SPEED_FULL:
+		/* EPS 0 means "full" */
+		if (type != PIPE_INTERRUPT)
+			info1 |= (EHCI_TUNE_RL_TT << 28);
+		if (type == PIPE_CONTROL) {
+			info1 |= (1 << 27);	/* for TT */
+			info1 |= 1 << 14;	/* toggle from qtd */
+		}
+		info1 |= maxp << 16;
+
+		info2 |= (EHCI_TUNE_MULT_TT << 30);
+		info2 |= urb->dev->ttport << 23;
+
+		/* NOTE:  if (PIPE_INTERRUPT) { scheduler sets c-mask } */
+
+		break;
+
+	case USB_SPEED_HIGH:		/* no TT involved */
+		info1 |= (2 << 12);	/* EPS "high" */
+		if (type == PIPE_CONTROL) {
+			info1 |= (EHCI_TUNE_RL_HS << 28);
+			info1 |= 64 << 16;	/* usb2 fixed maxpacket */
+			info1 |= 1 << 14;	/* toggle from qtd */
+			info2 |= (EHCI_TUNE_MULT_HS << 30);
+		} else if (type == PIPE_BULK) {
+			info1 |= (EHCI_TUNE_RL_HS << 28);
+			info1 |= 512 << 16;	/* usb2 fixed maxpacket */
+			info2 |= (EHCI_TUNE_MULT_HS << 30);
+		} else {		/* PIPE_INTERRUPT */
+			info1 |= max_packet(maxp) << 16;
+			info2 |= hb_mult(maxp) << 30;
+		}
+		break;
+	default:
+		dbg("bogus dev %p speed %d", urb->dev, urb->dev->speed);
+done:
+		qh_put(qh);
+		return NULL;
+	}
+
+	/* NOTE:  if (PIPE_INTERRUPT) { scheduler sets s-mask } */
+
+	/* init as live, toggle clear, advance to dummy */
+	qh->qh_state = QH_STATE_IDLE;
+	qh->hw_info1 = cpu_to_le32(info1);
+	qh->hw_info2 = cpu_to_le32(info2);
+	usb_settoggle(urb->dev, usb_pipeendpoint(urb->pipe), !is_input, 1);
+	qh_refresh(oxu, qh);
+	return qh;
+}
+
+/* Move qh (and its qtds) onto async queue; maybe enable queue.
+ */
+static void qh_link_async(struct oxu_hcd *oxu, struct ehci_qh *qh)
+{
+	__le32 dma = QH_NEXT(qh->qh_dma);
+	struct ehci_qh *head;
+
+	/* (re)start the async schedule? */
+	head = oxu->async;
+	timer_action_done(oxu, TIMER_ASYNC_OFF);
+	if (!head->qh_next.qh) {
+		u32	cmd = readl(&oxu->regs->command);
+
+		if (!(cmd & CMD_ASE)) {
+			/* in case a clear of CMD_ASE didn't take yet */
+			(void)handshake(oxu, &oxu->regs->status,
+					STS_ASS, 0, 150);
+			cmd |= CMD_ASE | CMD_RUN;
+			writel(cmd, &oxu->regs->command);
+			oxu_to_hcd(oxu)->state = HC_STATE_RUNNING;
+			/* posted write need not be known to HC yet ... */
+		}
+	}
+
+	/* clear halt and/or toggle; and maybe recover from silicon quirk */
+	if (qh->qh_state == QH_STATE_IDLE)
+		qh_refresh(oxu, qh);
+
+	/* splice right after start */
+	qh->qh_next = head->qh_next;
+	qh->hw_next = head->hw_next;
+	wmb();
+
+	head->qh_next.qh = qh;
+	head->hw_next = dma;
+
+	qh->qh_state = QH_STATE_LINKED;
+	/* qtd completions reported later by interrupt */
+}
+
+#define	QH_ADDR_MASK	__constant_cpu_to_le32(0x7f)
+
+/*
+ * For control/bulk/interrupt, return QH with these TDs appended.
+ * Allocates and initializes the QH if necessary.
+ * Returns null if it can't allocate a QH it needs to.
+ * If the QH has TDs (urbs) already, that's great.
+ */
+static struct ehci_qh *qh_append_tds(struct oxu_hcd *oxu,
+				struct urb *urb, struct list_head *qtd_list,
+				int epnum, void	**ptr)
+{
+	struct ehci_qh *qh = NULL;
+
+	qh = (struct ehci_qh *) *ptr;
+	if (unlikely(qh == NULL)) {
+		/* can't sleep here, we have oxu->lock... */
+		qh = qh_make(oxu, urb, GFP_ATOMIC);
+		*ptr = qh;
+	}
+	if (likely(qh != NULL)) {
+		struct ehci_qtd	*qtd;
+
+		if (unlikely(list_empty(qtd_list)))
+			qtd = NULL;
+		else
+			qtd = list_entry(qtd_list->next, struct ehci_qtd,
+					qtd_list);
+
+		/* control qh may need patching ... */
+		if (unlikely(epnum == 0)) {
+
+			/* usb_reset_device() briefly reverts to address 0 */
+			if (usb_pipedevice(urb->pipe) == 0)
+				qh->hw_info1 &= ~QH_ADDR_MASK;
+		}
+
+		/* just one way to queue requests: swap with the dummy qtd.
+		 * only hc or qh_refresh() ever modify the overlay.
+		 */
+		if (likely(qtd != NULL)) {
+			struct ehci_qtd	*dummy;
+			dma_addr_t dma;
+			__le32 token;
+
+			/* to avoid racing the HC, use the dummy td instead of
+			 * the first td of our list (becomes new dummy).  both
+			 * tds stay deactivated until we're done, when the
+			 * HC is allowed to fetch the old dummy (4.10.2).
+			 */
+			token = qtd->hw_token;
+			qtd->hw_token = HALT_BIT;
+			wmb();
+			dummy = qh->dummy;
+
+			dma = dummy->qtd_dma;
+			*dummy = *qtd;
+			dummy->qtd_dma = dma;
+
+			list_del(&qtd->qtd_list);
+			list_add(&dummy->qtd_list, qtd_list);
+			list_splice(qtd_list, qh->qtd_list.prev);
+
+			ehci_qtd_init(qtd, qtd->qtd_dma);
+			qh->dummy = qtd;
+
+			/* hc must see the new dummy at list end */
+			dma = qtd->qtd_dma;
+			qtd = list_entry(qh->qtd_list.prev,
+					struct ehci_qtd, qtd_list);
+			qtd->hw_next = QTD_NEXT(dma);
+
+			/* let the hc process these next qtds */
+			dummy->hw_token = (token & ~(0x80));
+			wmb();
+			dummy->hw_token = token;
+
+			urb->hcpriv = qh_get(qh);
+		}
+	}
+	return qh;
+}
+
+static int submit_async(struct oxu_hcd	*oxu, struct urb *urb,
+			struct list_head *qtd_list, gfp_t mem_flags)
+{
+	struct ehci_qtd	*qtd;
+	int epnum;
+	unsigned long flags;
+	struct ehci_qh *qh = NULL;
+	int rc = 0;
+
+	qtd = list_entry(qtd_list->next, struct ehci_qtd, qtd_list);
+	epnum = urb->ep->desc.bEndpointAddress;
+
+#ifdef OXU_URB_TRACE
+	oxu_dbg(oxu, "%s %s urb %p ep%d%s len %d, qtd %p [qh %p]\n",
+		__func__, urb->dev->devpath, urb,
+		epnum & 0x0f, (epnum & USB_DIR_IN) ? "in" : "out",
+		urb->transfer_buffer_length,
+		qtd, urb->ep->hcpriv);
+#endif
+
+	spin_lock_irqsave(&oxu->lock, flags);
+	if (unlikely(!test_bit(HCD_FLAG_HW_ACCESSIBLE,
+			       &oxu_to_hcd(oxu)->flags))) {
+		rc = -ESHUTDOWN;
+		goto done;
+	}
+
+	qh = qh_append_tds(oxu, urb, qtd_list, epnum, &urb->ep->hcpriv);
+	if (unlikely(qh == NULL)) {
+		rc = -ENOMEM;
+		goto done;
+	}
+
+	/* Control/bulk operations through TTs don't need scheduling,
+	 * the HC and TT handle it when the TT has a buffer ready.
+	 */
+	if (likely(qh->qh_state == QH_STATE_IDLE))
+		qh_link_async(oxu, qh_get(qh));
+done:
+	spin_unlock_irqrestore(&oxu->lock, flags);
+	if (unlikely(qh == NULL))
+		qtd_list_free(oxu, urb, qtd_list);
+	return rc;
+}
+
+/* The async qh for the qtds being reclaimed are now unlinked from the HC */
+
+static void end_unlink_async(struct oxu_hcd *oxu)
+{
+	struct ehci_qh *qh = oxu->reclaim;
+	struct ehci_qh *next;
+
+	timer_action_done(oxu, TIMER_IAA_WATCHDOG);
+
+	qh->qh_state = QH_STATE_IDLE;
+	qh->qh_next.qh = NULL;
+	qh_put(qh);			/* refcount from reclaim */
+
+	/* other unlink(s) may be pending (in QH_STATE_UNLINK_WAIT) */
+	next = qh->reclaim;
+	oxu->reclaim = next;
+	oxu->reclaim_ready = 0;
+	qh->reclaim = NULL;
+
+	qh_completions(oxu, qh);
+
+	if (!list_empty(&qh->qtd_list)
+			&& HC_IS_RUNNING(oxu_to_hcd(oxu)->state))
+		qh_link_async(oxu, qh);
+	else {
+		qh_put(qh);		/* refcount from async list */
+
+		/* it's not free to turn the async schedule on/off; leave it
+		 * active but idle for a while once it empties.
+		 */
+		if (HC_IS_RUNNING(oxu_to_hcd(oxu)->state)
+				&& oxu->async->qh_next.qh == NULL)
+			timer_action(oxu, TIMER_ASYNC_OFF);
+	}
+
+	if (next) {
+		oxu->reclaim = NULL;
+		start_unlink_async(oxu, next);
+	}
+}
+
+/* makes sure the async qh will become idle */
+/* caller must own oxu->lock */
+
+static void start_unlink_async(struct oxu_hcd *oxu, struct ehci_qh *qh)
+{
+	int cmd = readl(&oxu->regs->command);
+	struct ehci_qh *prev;
+
+#ifdef DEBUG
+	assert_spin_locked(&oxu->lock);
+	if (oxu->reclaim || (qh->qh_state != QH_STATE_LINKED
+				&& qh->qh_state != QH_STATE_UNLINK_WAIT))
+		BUG();
+#endif
+
+	/* stop async schedule right now? */
+	if (unlikely(qh == oxu->async)) {
+		/* can't get here without STS_ASS set */
+		if (oxu_to_hcd(oxu)->state != HC_STATE_HALT
+				&& !oxu->reclaim) {
+			/* ... and CMD_IAAD clear */
+			writel(cmd & ~CMD_ASE, &oxu->regs->command);
+			wmb();
+			/* handshake later, if we need to */
+			timer_action_done(oxu, TIMER_ASYNC_OFF);
+		}
+		return;
+	}
+
+	qh->qh_state = QH_STATE_UNLINK;
+	oxu->reclaim = qh = qh_get(qh);
+
+	prev = oxu->async;
+	while (prev->qh_next.qh != qh)
+		prev = prev->qh_next.qh;
+
+	prev->hw_next = qh->hw_next;
+	prev->qh_next = qh->qh_next;
+	wmb();
+
+	if (unlikely(oxu_to_hcd(oxu)->state == HC_STATE_HALT)) {
+		/* if (unlikely(qh->reclaim != 0))
+		 *	this will recurse, probably not much
+		 */
+		end_unlink_async(oxu);
+		return;
+	}
+
+	oxu->reclaim_ready = 0;
+	cmd |= CMD_IAAD;
+	writel(cmd, &oxu->regs->command);
+	(void) readl(&oxu->regs->command);
+	timer_action(oxu, TIMER_IAA_WATCHDOG);
+}
+
+static void scan_async(struct oxu_hcd *oxu)
+{
+	struct ehci_qh *qh;
+	enum ehci_timer_action action = TIMER_IO_WATCHDOG;
+
+	if (!++(oxu->stamp))
+		oxu->stamp++;
+	timer_action_done(oxu, TIMER_ASYNC_SHRINK);
+rescan:
+	qh = oxu->async->qh_next.qh;
+	if (likely(qh != NULL)) {
+		do {
+			/* clean any finished work for this qh */
+			if (!list_empty(&qh->qtd_list)
+					&& qh->stamp != oxu->stamp) {
+				int temp;
+
+				/* unlinks could happen here; completion
+				 * reporting drops the lock.  rescan using
+				 * the latest schedule, but don't rescan
+				 * qhs we already finished (no looping).
+				 */
+				qh = qh_get(qh);
+				qh->stamp = oxu->stamp;
+				temp = qh_completions(oxu, qh);
+				qh_put(qh);
+				if (temp != 0)
+					goto rescan;
+			}
+
+			/* unlink idle entries, reducing HC PCI usage as well
+			 * as HCD schedule-scanning costs.  delay for any qh
+			 * we just scanned, there's a not-unusual case that it
+			 * doesn't stay idle for long.
+			 * (plus, avoids some kind of re-activation race.)
+			 */
+			if (list_empty(&qh->qtd_list)) {
+				if (qh->stamp == oxu->stamp)
+					action = TIMER_ASYNC_SHRINK;
+				else if (!oxu->reclaim
+					    && qh->qh_state == QH_STATE_LINKED)
+					start_unlink_async(oxu, qh);
+			}
+
+			qh = qh->qh_next.qh;
+		} while (qh);
+	}
+	if (action == TIMER_ASYNC_SHRINK)
+		timer_action(oxu, TIMER_ASYNC_SHRINK);
+}
+
+/*
+ * periodic_next_shadow - return "next" pointer on shadow list
+ * @periodic: host pointer to qh/itd/sitd
+ * @tag: hardware tag for type of this record
+ */
+static union ehci_shadow *periodic_next_shadow(union ehci_shadow *periodic,
+						__le32 tag)
+{
+	switch (tag) {
+	default:
+	case Q_TYPE_QH:
+		return &periodic->qh->qh_next;
+	}
+}
+
+/* caller must hold oxu->lock */
+static void periodic_unlink(struct oxu_hcd *oxu, unsigned frame, void *ptr)
+{
+	union ehci_shadow *prev_p = &oxu->pshadow[frame];
+	__le32 *hw_p = &oxu->periodic[frame];
+	union ehci_shadow here = *prev_p;
+
+	/* find predecessor of "ptr"; hw and shadow lists are in sync */
+	while (here.ptr && here.ptr != ptr) {
+		prev_p = periodic_next_shadow(prev_p, Q_NEXT_TYPE(*hw_p));
+		hw_p = here.hw_next;
+		here = *prev_p;
+	}
+	/* an interrupt entry (at list end) could have been shared */
+	if (!here.ptr)
+		return;
+
+	/* update shadow and hardware lists ... the old "next" pointers
+	 * from ptr may still be in use, the caller updates them.
+	 */
+	*prev_p = *periodic_next_shadow(&here, Q_NEXT_TYPE(*hw_p));
+	*hw_p = *here.hw_next;
+}
+
+/* how many of the uframe's 125 usecs are allocated? */
+static unsigned short periodic_usecs(struct oxu_hcd *oxu,
+					unsigned frame, unsigned uframe)
+{
+	__le32 *hw_p = &oxu->periodic[frame];
+	union ehci_shadow *q = &oxu->pshadow[frame];
+	unsigned usecs = 0;
+
+	while (q->ptr) {
+		switch (Q_NEXT_TYPE(*hw_p)) {
+		case Q_TYPE_QH:
+		default:
+			/* is it in the S-mask? */
+			if (q->qh->hw_info2 & cpu_to_le32(1 << uframe))
+				usecs += q->qh->usecs;
+			/* ... or C-mask? */
+			if (q->qh->hw_info2 & cpu_to_le32(1 << (8 + uframe)))
+				usecs += q->qh->c_usecs;
+			hw_p = &q->qh->hw_next;
+			q = &q->qh->qh_next;
+			break;
+		}
+	}
+#ifdef DEBUG
+	if (usecs > 100)
+		oxu_err(oxu, "uframe %d sched overrun: %d usecs\n",
+						frame * 8 + uframe, usecs);
+#endif
+	return usecs;
+}
+
+static int enable_periodic(struct oxu_hcd *oxu)
+{
+	u32 cmd;
+	int status;
+
+	/* did clearing PSE did take effect yet?
+	 * takes effect only at frame boundaries...
+	 */
+	status = handshake(oxu, &oxu->regs->status, STS_PSS, 0, 9 * 125);
+	if (status != 0) {
+		oxu_to_hcd(oxu)->state = HC_STATE_HALT;
+		return status;
+	}
+
+	cmd = readl(&oxu->regs->command) | CMD_PSE;
+	writel(cmd, &oxu->regs->command);
+	/* posted write ... PSS happens later */
+	oxu_to_hcd(oxu)->state = HC_STATE_RUNNING;
+
+	/* make sure ehci_work scans these */
+	oxu->next_uframe = readl(&oxu->regs->frame_index)
+		% (oxu->periodic_size << 3);
+	return 0;
+}
+
+static int disable_periodic(struct oxu_hcd *oxu)
+{
+	u32 cmd;
+	int status;
+
+	/* did setting PSE not take effect yet?
+	 * takes effect only at frame boundaries...
+	 */
+	status = handshake(oxu, &oxu->regs->status, STS_PSS, STS_PSS, 9 * 125);
+	if (status != 0) {
+		oxu_to_hcd(oxu)->state = HC_STATE_HALT;
+		return status;
+	}
+
+	cmd = readl(&oxu->regs->command) & ~CMD_PSE;
+	writel(cmd, &oxu->regs->command);
+	/* posted write ... */
+
+	oxu->next_uframe = -1;
+	return 0;
+}
+
+/* periodic schedule slots have iso tds (normal or split) first, then a
+ * sparse tree for active interrupt transfers.
+ *
+ * this just links in a qh; caller guarantees uframe masks are set right.
+ * no FSTN support (yet; oxu 0.96+)
+ */
+static int qh_link_periodic(struct oxu_hcd *oxu, struct ehci_qh *qh)
+{
+	unsigned i;
+	unsigned period = qh->period;
+
+	dev_dbg(&qh->dev->dev,
+		"link qh%d-%04x/%p start %d [%d/%d us]\n",
+		period, le32_to_cpup(&qh->hw_info2) & (QH_CMASK | QH_SMASK),
+		qh, qh->start, qh->usecs, qh->c_usecs);
+
+	/* high bandwidth, or otherwise every microframe */
+	if (period == 0)
+		period = 1;
+
+	for (i = qh->start; i < oxu->periodic_size; i += period) {
+		union ehci_shadow	*prev = &oxu->pshadow[i];
+		__le32			*hw_p = &oxu->periodic[i];
+		union ehci_shadow	here = *prev;
+		__le32			type = 0;
+
+		/* skip the iso nodes at list head */
+		while (here.ptr) {
+			type = Q_NEXT_TYPE(*hw_p);
+			if (type == Q_TYPE_QH)
+				break;
+			prev = periodic_next_shadow(prev, type);
+			hw_p = &here.qh->hw_next;
+			here = *prev;
+		}
+
+		/* sorting each branch by period (slow-->fast)
+		 * enables sharing interior tree nodes
+		 */
+		while (here.ptr && qh != here.qh) {
+			if (qh->period > here.qh->period)
+				break;
+			prev = &here.qh->qh_next;
+			hw_p = &here.qh->hw_next;
+			here = *prev;
+		}
+		/* link in this qh, unless some earlier pass did that */
+		if (qh != here.qh) {
+			qh->qh_next = here;
+			if (here.qh)
+				qh->hw_next = *hw_p;
+			wmb();
+			prev->qh = qh;
+			*hw_p = QH_NEXT(qh->qh_dma);
+		}
+	}
+	qh->qh_state = QH_STATE_LINKED;
+	qh_get(qh);
+
+	/* update per-qh bandwidth for usbfs */
+	oxu_to_hcd(oxu)->self.bandwidth_allocated += qh->period
+		? ((qh->usecs + qh->c_usecs) / qh->period)
+		: (qh->usecs * 8);
+
+	/* maybe enable periodic schedule processing */
+	if (!oxu->periodic_sched++)
+		return enable_periodic(oxu);
+
+	return 0;
+}
+
+static void qh_unlink_periodic(struct oxu_hcd *oxu, struct ehci_qh *qh)
+{
+	unsigned i;
+	unsigned period;
+
+	/* FIXME:
+	 *   IF this isn't high speed
+	 *   and this qh is active in the current uframe
+	 *   (and overlay token SplitXstate is false?)
+	 * THEN
+	 *   qh->hw_info1 |= __constant_cpu_to_le32(1 << 7 "ignore");
+	 */
+
+	/* high bandwidth, or otherwise part of every microframe */
+	period = qh->period;
+	if (period == 0)
+		period = 1;
+
+	for (i = qh->start; i < oxu->periodic_size; i += period)
+		periodic_unlink(oxu, i, qh);
+
+	/* update per-qh bandwidth for usbfs */
+	oxu_to_hcd(oxu)->self.bandwidth_allocated -= qh->period
+		? ((qh->usecs + qh->c_usecs) / qh->period)
+		: (qh->usecs * 8);
+
+	dev_dbg(&qh->dev->dev,
+		"unlink qh%d-%04x/%p start %d [%d/%d us]\n",
+		qh->period,
+		le32_to_cpup(&qh->hw_info2) & (QH_CMASK | QH_SMASK),
+		qh, qh->start, qh->usecs, qh->c_usecs);
+
+	/* qh->qh_next still "live" to HC */
+	qh->qh_state = QH_STATE_UNLINK;
+	qh->qh_next.ptr = NULL;
+	qh_put(qh);
+
+	/* maybe turn off periodic schedule */
+	oxu->periodic_sched--;
+	if (!oxu->periodic_sched)
+		(void) disable_periodic(oxu);
+}
+
+static void intr_deschedule(struct oxu_hcd *oxu, struct ehci_qh *qh)
+{
+	unsigned wait;
+
+	qh_unlink_periodic(oxu, qh);
+
+	/* simple/paranoid:  always delay, expecting the HC needs to read
+	 * qh->hw_next or finish a writeback after SPLIT/CSPLIT ... and
+	 * expect khubd to clean up after any CSPLITs we won't issue.
+	 * active high speed queues may need bigger delays...
+	 */
+	if (list_empty(&qh->qtd_list)
+		|| (__constant_cpu_to_le32(QH_CMASK) & qh->hw_info2) != 0)
+		wait = 2;
+	else
+		wait = 55;	/* worst case: 3 * 1024 */
+
+	udelay(wait);
+	qh->qh_state = QH_STATE_IDLE;
+	qh->hw_next = EHCI_LIST_END;
+	wmb();
+}
+
+static int check_period(struct oxu_hcd *oxu,
+			unsigned frame, unsigned uframe,
+			unsigned period, unsigned usecs)
+{
+	int claimed;
+
+	/* complete split running into next frame?
+	 * given FSTN support, we could sometimes check...
+	 */
+	if (uframe >= 8)
+		return 0;
+
+	/*
+	 * 80% periodic == 100 usec/uframe available
+	 * convert "usecs we need" to "max already claimed"
+	 */
+	usecs = 100 - usecs;
+
+	/* we "know" 2 and 4 uframe intervals were rejected; so
+	 * for period 0, check _every_ microframe in the schedule.
+	 */
+	if (unlikely(period == 0)) {
+		do {
+			for (uframe = 0; uframe < 7; uframe++) {
+				claimed = periodic_usecs(oxu, frame, uframe);
+				if (claimed > usecs)
+					return 0;
+			}
+		} while ((frame += 1) < oxu->periodic_size);
+
+	/* just check the specified uframe, at that period */
+	} else {
+		do {
+			claimed = periodic_usecs(oxu, frame, uframe);
+			if (claimed > usecs)
+				return 0;
+		} while ((frame += period) < oxu->periodic_size);
+	}
+
+	return 1;
+}
+
+static int check_intr_schedule(struct oxu_hcd	*oxu,
+				unsigned frame, unsigned uframe,
+				const struct ehci_qh *qh, __le32 *c_maskp)
+{
+	int retval = -ENOSPC;
+
+	if (qh->c_usecs && uframe >= 6)		/* FSTN territory? */
+		goto done;
+
+	if (!check_period(oxu, frame, uframe, qh->period, qh->usecs))
+		goto done;
+	if (!qh->c_usecs) {
+		retval = 0;
+		*c_maskp = 0;
+		goto done;
+	}
+
+done:
+	return retval;
+}
+
+/* "first fit" scheduling policy used the first time through,
+ * or when the previous schedule slot can't be re-used.
+ */
+static int qh_schedule(struct oxu_hcd *oxu, struct ehci_qh *qh)
+{
+	int		status;
+	unsigned	uframe;
+	__le32		c_mask;
+	unsigned	frame;		/* 0..(qh->period - 1), or NO_FRAME */
+
+	qh_refresh(oxu, qh);
+	qh->hw_next = EHCI_LIST_END;
+	frame = qh->start;
+
+	/* reuse the previous schedule slots, if we can */
+	if (frame < qh->period) {
+		uframe = ffs(le32_to_cpup(&qh->hw_info2) & QH_SMASK);
+		status = check_intr_schedule(oxu, frame, --uframe,
+				qh, &c_mask);
+	} else {
+		uframe = 0;
+		c_mask = 0;
+		status = -ENOSPC;
+	}
+
+	/* else scan the schedule to find a group of slots such that all
+	 * uframes have enough periodic bandwidth available.
+	 */
+	if (status) {
+		/* "normal" case, uframing flexible except with splits */
+		if (qh->period) {
+			frame = qh->period - 1;
+			do {
+				for (uframe = 0; uframe < 8; uframe++) {
+					status = check_intr_schedule(oxu,
+							frame, uframe, qh,
+							&c_mask);
+					if (status == 0)
+						break;
+				}
+			} while (status && frame--);
+
+		/* qh->period == 0 means every uframe */
+		} else {
+			frame = 0;
+			status = check_intr_schedule(oxu, 0, 0, qh, &c_mask);
+		}
+		if (status)
+			goto done;
+		qh->start = frame;
+
+		/* reset S-frame and (maybe) C-frame masks */
+		qh->hw_info2 &= __constant_cpu_to_le32(~(QH_CMASK | QH_SMASK));
+		qh->hw_info2 |= qh->period
+			? cpu_to_le32(1 << uframe)
+			: __constant_cpu_to_le32(QH_SMASK);
+		qh->hw_info2 |= c_mask;
+	} else
+		oxu_dbg(oxu, "reused qh %p schedule\n", qh);
+
+	/* stuff into the periodic schedule */
+	status = qh_link_periodic(oxu, qh);
+done:
+	return status;
+}
+
+static int intr_submit(struct oxu_hcd *oxu, struct urb *urb,
+			struct list_head *qtd_list, gfp_t mem_flags)
+{
+	unsigned epnum;
+	unsigned long flags;
+	struct ehci_qh *qh;
+	int status = 0;
+	struct list_head	empty;
+
+	/* get endpoint and transfer/schedule data */
+	epnum = urb->ep->desc.bEndpointAddress;
+
+	spin_lock_irqsave(&oxu->lock, flags);
+
+	if (unlikely(!test_bit(HCD_FLAG_HW_ACCESSIBLE,
+			       &oxu_to_hcd(oxu)->flags))) {
+		status = -ESHUTDOWN;
+		goto done;
+	}
+
+	/* get qh and force any scheduling errors */
+	INIT_LIST_HEAD(&empty);
+	qh = qh_append_tds(oxu, urb, &empty, epnum, &urb->ep->hcpriv);
+	if (qh == NULL) {
+		status = -ENOMEM;
+		goto done;
+	}
+	if (qh->qh_state == QH_STATE_IDLE) {
+		status = qh_schedule(oxu, qh);
+		if (status != 0)
+			goto done;
+	}
+
+	/* then queue the urb's tds to the qh */
+	qh = qh_append_tds(oxu, urb, qtd_list, epnum, &urb->ep->hcpriv);
+	BUG_ON(qh == NULL);
+
+	/* ... update usbfs periodic stats */
+	oxu_to_hcd(oxu)->self.bandwidth_int_reqs++;
+
+done:
+	spin_unlock_irqrestore(&oxu->lock, flags);
+	if (status)
+		qtd_list_free(oxu, urb, qtd_list);
+
+	return status;
+}
+
+static inline int itd_submit(struct oxu_hcd *oxu, struct urb *urb,
+						gfp_t mem_flags)
+{
+	oxu_dbg(oxu, "iso support is missing!\n");
+	return -ENOSYS;
+}
+
+static inline int sitd_submit(struct oxu_hcd *oxu, struct urb *urb,
+						gfp_t mem_flags)
+{
+	oxu_dbg(oxu, "split iso support is missing!\n");
+	return -ENOSYS;
+}
+
+static void scan_periodic(struct oxu_hcd *oxu)
+{
+	unsigned frame, clock, now_uframe, mod;
+	unsigned modified;
+
+	mod = oxu->periodic_size << 3;
+
+	/*
+	 * When running, scan from last scan point up to "now"
+	 * else clean up by scanning everything that's left.
+	 * Touches as few pages as possible:  cache-friendly.
+	 */
+	now_uframe = oxu->next_uframe;
+	if (HC_IS_RUNNING(oxu_to_hcd(oxu)->state))
+		clock = readl(&oxu->regs->frame_index);
+	else
+		clock = now_uframe + mod - 1;
+	clock %= mod;
+
+	for (;;) {
+		union ehci_shadow	q, *q_p;
+		__le32			type, *hw_p;
+		unsigned		uframes;
+
+		/* don't scan past the live uframe */
+		frame = now_uframe >> 3;
+		if (frame == (clock >> 3))
+			uframes = now_uframe & 0x07;
+		else {
+			/* safe to scan the whole frame at once */
+			now_uframe |= 0x07;
+			uframes = 8;
+		}
+
+restart:
+		/* scan each element in frame's queue for completions */
+		q_p = &oxu->pshadow[frame];
+		hw_p = &oxu->periodic[frame];
+		q.ptr = q_p->ptr;
+		type = Q_NEXT_TYPE(*hw_p);
+		modified = 0;
+
+		while (q.ptr != NULL) {
+			union ehci_shadow temp;
+			int live;
+
+			live = HC_IS_RUNNING(oxu_to_hcd(oxu)->state);
+			switch (type) {
+			case Q_TYPE_QH:
+				/* handle any completions */
+				temp.qh = qh_get(q.qh);
+				type = Q_NEXT_TYPE(q.qh->hw_next);
+				q = q.qh->qh_next;
+				modified = qh_completions(oxu, temp.qh);
+				if (unlikely(list_empty(&temp.qh->qtd_list)))
+					intr_deschedule(oxu, temp.qh);
+				qh_put(temp.qh);
+				break;
+			default:
+				dbg("corrupt type %d frame %d shadow %p",
+					type, frame, q.ptr);
+				q.ptr = NULL;
+			}
+
+			/* assume completion callbacks modify the queue */
+			if (unlikely(modified))
+				goto restart;
+		}
+
+		/* Stop when we catch up to the HC */
+
+		/* FIXME:  this assumes we won't get lapped when
+		 * latencies climb; that should be rare, but...
+		 * detect it, and just go all the way around.
+		 * FLR might help detect this case, so long as latencies
+		 * don't exceed periodic_size msec (default 1.024 sec).
+		 */
+
+		/* FIXME: likewise assumes HC doesn't halt mid-scan */
+
+		if (now_uframe == clock) {
+			unsigned	now;
+
+			if (!HC_IS_RUNNING(oxu_to_hcd(oxu)->state))
+				break;
+			oxu->next_uframe = now_uframe;
+			now = readl(&oxu->regs->frame_index) % mod;
+			if (now_uframe == now)
+				break;
+
+			/* rescan the rest of this frame, then ... */
+			clock = now;
+		} else {
+			now_uframe++;
+			now_uframe %= mod;
+		}
+	}
+}
+
+/* On some systems, leaving remote wakeup enabled prevents system shutdown.
+ * The firmware seems to think that powering off is a wakeup event!
+ * This routine turns off remote wakeup and everything else, on all ports.
+ */
+static void ehci_turn_off_all_ports(struct oxu_hcd *oxu)
+{
+	int port = HCS_N_PORTS(oxu->hcs_params);
+
+	while (port--)
+		writel(PORT_RWC_BITS, &oxu->regs->port_status[port]);
+}
+
+static void ehci_port_power(struct oxu_hcd *oxu, int is_on)
+{
+	unsigned port;
+
+	if (!HCS_PPC(oxu->hcs_params))
+		return;
+
+	oxu_dbg(oxu, "...power%s ports...\n", is_on ? "up" : "down");
+	for (port = HCS_N_PORTS(oxu->hcs_params); port > 0; )
+		(void) oxu_hub_control(oxu_to_hcd(oxu),
+				is_on ? SetPortFeature : ClearPortFeature,
+				USB_PORT_FEAT_POWER,
+				port--, NULL, 0);
+	msleep(20);
+}
+
+/* Called from some interrupts, timers, and so on.
+ * It calls driver completion functions, after dropping oxu->lock.
+ */
+static void ehci_work(struct oxu_hcd *oxu)
+{
+	timer_action_done(oxu, TIMER_IO_WATCHDOG);
+	if (oxu->reclaim_ready)
+		end_unlink_async(oxu);
+
+	/* another CPU may drop oxu->lock during a schedule scan while
+	 * it reports urb completions.  this flag guards against bogus
+	 * attempts at re-entrant schedule scanning.
+	 */
+	if (oxu->scanning)
+		return;
+	oxu->scanning = 1;
+	scan_async(oxu);
+	if (oxu->next_uframe != -1)
+		scan_periodic(oxu);
+	oxu->scanning = 0;
+
+	/* the IO watchdog guards against hardware or driver bugs that
+	 * misplace IRQs, and should let us run completely without IRQs.
+	 * such lossage has been observed on both VT6202 and VT8235.
+	 */
+	if (HC_IS_RUNNING(oxu_to_hcd(oxu)->state) &&
+			(oxu->async->qh_next.ptr != NULL ||
+			 oxu->periodic_sched != 0))
+		timer_action(oxu, TIMER_IO_WATCHDOG);
+}
+
+static void unlink_async(struct oxu_hcd *oxu, struct ehci_qh *qh)
+{
+	/* if we need to use IAA and it's busy, defer */
+	if (qh->qh_state == QH_STATE_LINKED
+			&& oxu->reclaim
+			&& HC_IS_RUNNING(oxu_to_hcd(oxu)->state)) {
+		struct ehci_qh		*last;
+
+		for (last = oxu->reclaim;
+				last->reclaim;
+				last = last->reclaim)
+			continue;
+		qh->qh_state = QH_STATE_UNLINK_WAIT;
+		last->reclaim = qh;
+
+	/* bypass IAA if the hc can't care */
+	} else if (!HC_IS_RUNNING(oxu_to_hcd(oxu)->state) && oxu->reclaim)
+		end_unlink_async(oxu);
+
+	/* something else might have unlinked the qh by now */
+	if (qh->qh_state == QH_STATE_LINKED)
+		start_unlink_async(oxu, qh);
+}
+
+/*
+ * USB host controller methods
+ */
+
+static irqreturn_t oxu210_hcd_irq(struct usb_hcd *hcd)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+	u32 status, pcd_status = 0;
+	int bh;
+
+	spin_lock(&oxu->lock);
+
+	status = readl(&oxu->regs->status);
+
+	/* e.g. cardbus physical eject */
+	if (status == ~(u32) 0) {
+		oxu_dbg(oxu, "device removed\n");
+		goto dead;
+	}
+
+	status &= INTR_MASK;
+	if (!status) {			/* irq sharing? */
+		spin_unlock(&oxu->lock);
+		return IRQ_NONE;
+	}
+
+	/* clear (just) interrupts */
+	writel(status, &oxu->regs->status);
+	readl(&oxu->regs->command);	/* unblock posted write */
+	bh = 0;
+
+#ifdef OXU_VERBOSE_DEBUG
+	/* unrequested/ignored: Frame List Rollover */
+	dbg_status(oxu, "irq", status);
+#endif
+
+	/* INT, ERR, and IAA interrupt rates can be throttled */
+
+	/* normal [4.15.1.2] or error [4.15.1.1] completion */
+	if (likely((status & (STS_INT|STS_ERR)) != 0))
+		bh = 1;
+
+	/* complete the unlinking of some qh [4.15.2.3] */
+	if (status & STS_IAA) {
+		oxu->reclaim_ready = 1;
+		bh = 1;
+	}
+
+	/* remote wakeup [4.3.1] */
+	if (status & STS_PCD) {
+		unsigned i = HCS_N_PORTS(oxu->hcs_params);
+		pcd_status = status;
+
+		/* resume root hub? */
+		if (!(readl(&oxu->regs->command) & CMD_RUN))
+			usb_hcd_resume_root_hub(hcd);
+
+		while (i--) {
+			int pstatus = readl(&oxu->regs->port_status[i]);
+
+			if (pstatus & PORT_OWNER)
+				continue;
+			if (!(pstatus & PORT_RESUME)
+					|| oxu->reset_done[i] != 0)
+				continue;
+
+			/* start 20 msec resume signaling from this port,
+			 * and make khubd collect PORT_STAT_C_SUSPEND to
+			 * stop that signaling.
+			 */
+			oxu->reset_done[i] = jiffies + msecs_to_jiffies(20);
+			oxu_dbg(oxu, "port %d remote wakeup\n", i + 1);
+			mod_timer(&hcd->rh_timer, oxu->reset_done[i]);
+		}
+	}
+
+	/* PCI errors [4.15.2.4] */
+	if (unlikely((status & STS_FATAL) != 0)) {
+		/* bogus "fatal" IRQs appear on some chips... why?  */
+		status = readl(&oxu->regs->status);
+		dbg_cmd(oxu, "fatal", readl(&oxu->regs->command));
+		dbg_status(oxu, "fatal", status);
+		if (status & STS_HALT) {
+			oxu_err(oxu, "fatal error\n");
+dead:
+			ehci_reset(oxu);
+			writel(0, &oxu->regs->configured_flag);
+			/* generic layer kills/unlinks all urbs, then
+			 * uses oxu_stop to clean up the rest
+			 */
+			bh = 1;
+		}
+	}
+
+	if (bh)
+		ehci_work(oxu);
+	spin_unlock(&oxu->lock);
+	if (pcd_status & STS_PCD)
+		usb_hcd_poll_rh_status(hcd);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t oxu_irq(struct usb_hcd *hcd)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+	int ret = IRQ_HANDLED;
+
+	u32 status = oxu_readl(hcd->regs, OXU_CHIPIRQSTATUS);
+	u32 enable = oxu_readl(hcd->regs, OXU_CHIPIRQEN_SET);
+
+	/* Disable all interrupt */
+	oxu_writel(hcd->regs, OXU_CHIPIRQEN_CLR, enable);
+
+	if ((oxu->is_otg && (status & OXU_USBOTGI)) ||
+		(!oxu->is_otg && (status & OXU_USBSPHI)))
+		oxu210_hcd_irq(hcd);
+	else
+		ret = IRQ_NONE;
+
+	/* Enable all interrupt back */
+	oxu_writel(hcd->regs, OXU_CHIPIRQEN_SET, enable);
+
+	return ret;
+}
+
+static void oxu_watchdog(unsigned long param)
+{
+	struct oxu_hcd	*oxu = (struct oxu_hcd *) param;
+	unsigned long flags;
+
+	spin_lock_irqsave(&oxu->lock, flags);
+
+	/* lost IAA irqs wedge things badly; seen with a vt8235 */
+	if (oxu->reclaim) {
+		u32 status = readl(&oxu->regs->status);
+		if (status & STS_IAA) {
+			oxu_vdbg(oxu, "lost IAA\n");
+			writel(STS_IAA, &oxu->regs->status);
+			oxu->reclaim_ready = 1;
+		}
+	}
+
+	/* stop async processing after it's idled a bit */
+	if (test_bit(TIMER_ASYNC_OFF, &oxu->actions))
+		start_unlink_async(oxu, oxu->async);
+
+	/* oxu could run by timer, without IRQs ... */
+	ehci_work(oxu);
+
+	spin_unlock_irqrestore(&oxu->lock, flags);
+}
+
+/* One-time init, only for memory state.
+ */
+static int oxu_hcd_init(struct usb_hcd *hcd)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+	u32 temp;
+	int retval;
+	u32 hcc_params;
+
+	spin_lock_init(&oxu->lock);
+
+	init_timer(&oxu->watchdog);
+	oxu->watchdog.function = oxu_watchdog;
+	oxu->watchdog.data = (unsigned long) oxu;
+
+	/*
+	 * hw default: 1K periodic list heads, one per frame.
+	 * periodic_size can shrink by USBCMD update if hcc_params allows.
+	 */
+	oxu->periodic_size = DEFAULT_I_TDPS;
+	retval = ehci_mem_init(oxu, GFP_KERNEL);
+	if (retval < 0)
+		return retval;
+
+	/* controllers may cache some of the periodic schedule ... */
+	hcc_params = readl(&oxu->caps->hcc_params);
+	if (HCC_ISOC_CACHE(hcc_params))		/* full frame cache */
+		oxu->i_thresh = 8;
+	else					/* N microframes cached */
+		oxu->i_thresh = 2 + HCC_ISOC_THRES(hcc_params);
+
+	oxu->reclaim = NULL;
+	oxu->reclaim_ready = 0;
+	oxu->next_uframe = -1;
+
+	/*
+	 * dedicate a qh for the async ring head, since we couldn't unlink
+	 * a 'real' qh without stopping the async schedule [4.8].  use it
+	 * as the 'reclamation list head' too.
+	 * its dummy is used in hw_alt_next of many tds, to prevent the qh
+	 * from automatically advancing to the next td after short reads.
+	 */
+	oxu->async->qh_next.qh = NULL;
+	oxu->async->hw_next = QH_NEXT(oxu->async->qh_dma);
+	oxu->async->hw_info1 = cpu_to_le32(QH_HEAD);
+	oxu->async->hw_token = cpu_to_le32(QTD_STS_HALT);
+	oxu->async->hw_qtd_next = EHCI_LIST_END;
+	oxu->async->qh_state = QH_STATE_LINKED;
+	oxu->async->hw_alt_next = QTD_NEXT(oxu->async->dummy->qtd_dma);
+
+	/* clear interrupt enables, set irq latency */
+	if (log2_irq_thresh < 0 || log2_irq_thresh > 6)
+		log2_irq_thresh = 0;
+	temp = 1 << (16 + log2_irq_thresh);
+	if (HCC_CANPARK(hcc_params)) {
+		/* HW default park == 3, on hardware that supports it (like
+		 * NVidia and ALI silicon), maximizes throughput on the async
+		 * schedule by avoiding QH fetches between transfers.
+		 *
+		 * With fast usb storage devices and NForce2, "park" seems to
+		 * make problems:  throughput reduction (!), data errors...
+		 */
+		if (park) {
+			park = min(park, (unsigned) 3);
+			temp |= CMD_PARK;
+			temp |= park << 8;
+		}
+		oxu_dbg(oxu, "park %d\n", park);
+	}
+	if (HCC_PGM_FRAMELISTLEN(hcc_params)) {
+		/* periodic schedule size can be smaller than default */
+		temp &= ~(3 << 2);
+		temp |= (EHCI_TUNE_FLS << 2);
+	}
+	oxu->command = temp;
+
+	return 0;
+}
+
+/* Called during probe() after chip reset completes.
+ */
+static int oxu_reset(struct usb_hcd *hcd)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+	int ret;
+
+	spin_lock_init(&oxu->mem_lock);
+	INIT_LIST_HEAD(&oxu->urb_list);
+	oxu->urb_len = 0;
+
+	/* FIMXE */
+	hcd->self.controller->dma_mask = 0UL;
+
+	if (oxu->is_otg) {
+		oxu->caps = hcd->regs + OXU_OTG_CAP_OFFSET;
+		oxu->regs = hcd->regs + OXU_OTG_CAP_OFFSET + \
+			HC_LENGTH(readl(&oxu->caps->hc_capbase));
+
+		oxu->mem = hcd->regs + OXU_SPH_MEM;
+	} else {
+		oxu->caps = hcd->regs + OXU_SPH_CAP_OFFSET;
+		oxu->regs = hcd->regs + OXU_SPH_CAP_OFFSET + \
+			HC_LENGTH(readl(&oxu->caps->hc_capbase));
+
+		oxu->mem = hcd->regs + OXU_OTG_MEM;
+	}
+
+	oxu->hcs_params = readl(&oxu->caps->hcs_params);
+	oxu->sbrn = 0x20;
+
+	ret = oxu_hcd_init(hcd);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int oxu_run(struct usb_hcd *hcd)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+	int retval;
+	u32 temp, hcc_params;
+
+	hcd->uses_new_polling = 1;
+	hcd->poll_rh = 0;
+
+	/* EHCI spec section 4.1 */
+	retval = ehci_reset(oxu);
+	if (retval != 0) {
+		ehci_mem_cleanup(oxu);
+		return retval;
+	}
+	writel(oxu->periodic_dma, &oxu->regs->frame_list);
+	writel((u32) oxu->async->qh_dma, &oxu->regs->async_next);
+
+	/* hcc_params controls whether oxu->regs->segment must (!!!)
+	 * be used; it constrains QH/ITD/SITD and QTD locations.
+	 * pci_pool consistent memory always uses segment zero.
+	 * streaming mappings for I/O buffers, like pci_map_single(),
+	 * can return segments above 4GB, if the device allows.
+	 *
+	 * NOTE:  the dma mask is visible through dma_supported(), so
+	 * drivers can pass this info along ... like NETIF_F_HIGHDMA,
+	 * Scsi_Host.highmem_io, and so forth.  It's readonly to all
+	 * host side drivers though.
+	 */
+	hcc_params = readl(&oxu->caps->hcc_params);
+	if (HCC_64BIT_ADDR(hcc_params))
+		writel(0, &oxu->regs->segment);
+
+	oxu->command &= ~(CMD_LRESET | CMD_IAAD | CMD_PSE |
+				CMD_ASE | CMD_RESET);
+	oxu->command |= CMD_RUN;
+	writel(oxu->command, &oxu->regs->command);
+	dbg_cmd(oxu, "init", oxu->command);
+
+	/*
+	 * Start, enabling full USB 2.0 functionality ... usb 1.1 devices
+	 * are explicitly handed to companion controller(s), so no TT is
+	 * involved with the root hub.  (Except where one is integrated,
+	 * and there's no companion controller unless maybe for USB OTG.)
+	 */
+	hcd->state = HC_STATE_RUNNING;
+	writel(FLAG_CF, &oxu->regs->configured_flag);
+	readl(&oxu->regs->command);	/* unblock posted writes */
+
+	temp = HC_VERSION(readl(&oxu->caps->hc_capbase));
+	oxu_info(oxu, "USB %x.%x started, quasi-EHCI %x.%02x, driver %s%s\n",
+		((oxu->sbrn & 0xf0)>>4), (oxu->sbrn & 0x0f),
+		temp >> 8, temp & 0xff, DRIVER_VERSION,
+		ignore_oc ? ", overcurrent ignored" : "");
+
+	writel(INTR_MASK, &oxu->regs->intr_enable); /* Turn On Interrupts */
+
+	return 0;
+}
+
+static void oxu_stop(struct usb_hcd *hcd)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+
+	/* Turn off port power on all root hub ports. */
+	ehci_port_power(oxu, 0);
+
+	/* no more interrupts ... */
+	del_timer_sync(&oxu->watchdog);
+
+	spin_lock_irq(&oxu->lock);
+	if (HC_IS_RUNNING(hcd->state))
+		ehci_quiesce(oxu);
+
+	ehci_reset(oxu);
+	writel(0, &oxu->regs->intr_enable);
+	spin_unlock_irq(&oxu->lock);
+
+	/* let companion controllers work when we aren't */
+	writel(0, &oxu->regs->configured_flag);
+
+	/* root hub is shut down separately (first, when possible) */
+	spin_lock_irq(&oxu->lock);
+	if (oxu->async)
+		ehci_work(oxu);
+	spin_unlock_irq(&oxu->lock);
+	ehci_mem_cleanup(oxu);
+
+	dbg_status(oxu, "oxu_stop completed", readl(&oxu->regs->status));
+}
+
+/* Kick in for silicon on any bus (not just pci, etc).
+ * This forcibly disables dma and IRQs, helping kexec and other cases
+ * where the next system software may expect clean state.
+ */
+static void oxu_shutdown(struct usb_hcd *hcd)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+
+	(void) ehci_halt(oxu);
+	ehci_turn_off_all_ports(oxu);
+
+	/* make BIOS/etc use companion controller during reboot */
+	writel(0, &oxu->regs->configured_flag);
+
+	/* unblock posted writes */
+	readl(&oxu->regs->configured_flag);
+}
+
+/* Non-error returns are a promise to giveback() the urb later
+ * we drop ownership so next owner (or urb unlink) can get it
+ *
+ * urb + dev is in hcd.self.controller.urb_list
+ * we're queueing TDs onto software and hardware lists
+ *
+ * hcd-specific init for hcpriv hasn't been done yet
+ *
+ * NOTE:  control, bulk, and interrupt share the same code to append TDs
+ * to a (possibly active) QH, and the same QH scanning code.
+ */
+static int __oxu_urb_enqueue(struct usb_hcd *hcd, struct urb *urb,
+				gfp_t mem_flags)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+	struct list_head qtd_list;
+
+	INIT_LIST_HEAD(&qtd_list);
+
+	switch (usb_pipetype(urb->pipe)) {
+	case PIPE_CONTROL:
+	case PIPE_BULK:
+	default:
+		if (!qh_urb_transaction(oxu, urb, &qtd_list, mem_flags))
+			return -ENOMEM;
+		return submit_async(oxu, urb, &qtd_list, mem_flags);
+
+	case PIPE_INTERRUPT:
+		if (!qh_urb_transaction(oxu, urb, &qtd_list, mem_flags))
+			return -ENOMEM;
+		return intr_submit(oxu, urb, &qtd_list, mem_flags);
+
+	case PIPE_ISOCHRONOUS:
+		if (urb->dev->speed == USB_SPEED_HIGH)
+			return itd_submit(oxu, urb, mem_flags);
+		else
+			return sitd_submit(oxu, urb, mem_flags);
+	}
+}
+
+/* This function is responsible for breaking URBs with big data size
+ * into smaller size and processing small urbs in sequence.
+ */
+static int oxu_urb_enqueue(struct usb_hcd *hcd, struct urb *urb,
+				gfp_t mem_flags)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+	int num, rem;
+	int transfer_buffer_length;
+	void *transfer_buffer;
+	struct urb *murb;
+	int i, ret;
+
+	/* If not bulk pipe just enqueue the URB */
+	if (!usb_pipebulk(urb->pipe))
+		return __oxu_urb_enqueue(hcd, urb, mem_flags);
+
+	/* Otherwise we should verify the USB transfer buffer size! */
+	transfer_buffer = urb->transfer_buffer;
+	transfer_buffer_length = urb->transfer_buffer_length;
+
+	num = urb->transfer_buffer_length / 4096;
+	rem = urb->transfer_buffer_length % 4096;
+	if (rem != 0)
+		num++;
+
+	/* If URB is smaller than 4096 bytes just enqueue it! */
+	if (num == 1)
+		return __oxu_urb_enqueue(hcd, urb, mem_flags);
+
+	/* Ok, we have more job to do! :) */
+
+	for (i = 0; i < num - 1; i++) {
+		/* Get free micro URB poll till a free urb is recieved */
+
+		do {
+			murb = (struct urb *) oxu_murb_alloc(oxu);
+			if (!murb)
+				schedule();
+		} while (!murb);
+
+		/* Coping the urb */
+		memcpy(murb, urb, sizeof(struct urb));
+
+		murb->transfer_buffer_length = 4096;
+		murb->transfer_buffer = transfer_buffer + i * 4096;
+
+		/* Null pointer for the encodes that this is a micro urb */
+		murb->complete = NULL;
+
+		((struct oxu_murb *) murb)->main = urb;
+		((struct oxu_murb *) murb)->last = 0;
+
+		/* This loop is to guarantee urb to be processed when there's
+		 * not enough resources at a particular time by retrying.
+		 */
+		do {
+			ret  = __oxu_urb_enqueue(hcd, murb, mem_flags);
+			if (ret)
+				schedule();
+		} while (ret);
+	}
+
+	/* Last urb requires special handling  */
+
+	/* Get free micro URB poll till a free urb is recieved */
+	do {
+		murb = (struct urb *) oxu_murb_alloc(oxu);
+		if (!murb)
+			schedule();
+	} while (!murb);
+
+	/* Coping the urb */
+	memcpy(murb, urb, sizeof(struct urb));
+
+	murb->transfer_buffer_length = rem > 0 ? rem : 4096;
+	murb->transfer_buffer = transfer_buffer + (num - 1) * 4096;
+
+	/* Null pointer for the encodes that this is a micro urb */
+	murb->complete = NULL;
+
+	((struct oxu_murb *) murb)->main = urb;
+	((struct oxu_murb *) murb)->last = 1;
+
+	do {
+		ret = __oxu_urb_enqueue(hcd, murb, mem_flags);
+		if (ret)
+			schedule();
+	} while (ret);
+
+	return ret;
+}
+
+/* Remove from hardware lists.
+ * Completions normally happen asynchronously
+ */
+static int oxu_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+	struct ehci_qh *qh;
+	unsigned long flags;
+
+	spin_lock_irqsave(&oxu->lock, flags);
+	switch (usb_pipetype(urb->pipe)) {
+	case PIPE_CONTROL:
+	case PIPE_BULK:
+	default:
+		qh = (struct ehci_qh *) urb->hcpriv;
+		if (!qh)
+			break;
+		unlink_async(oxu, qh);
+		break;
+
+	case PIPE_INTERRUPT:
+		qh = (struct ehci_qh *) urb->hcpriv;
+		if (!qh)
+			break;
+		switch (qh->qh_state) {
+		case QH_STATE_LINKED:
+			intr_deschedule(oxu, qh);
+			/* FALL THROUGH */
+		case QH_STATE_IDLE:
+			qh_completions(oxu, qh);
+			break;
+		default:
+			oxu_dbg(oxu, "bogus qh %p state %d\n",
+					qh, qh->qh_state);
+			goto done;
+		}
+
+		/* reschedule QH iff another request is queued */
+		if (!list_empty(&qh->qtd_list)
+				&& HC_IS_RUNNING(hcd->state)) {
+			int status;
+
+			status = qh_schedule(oxu, qh);
+			spin_unlock_irqrestore(&oxu->lock, flags);
+
+			if (status != 0) {
+				/* shouldn't happen often, but ...
+				 * FIXME kill those tds' urbs
+				 */
+				err("can't reschedule qh %p, err %d",
+					qh, status);
+			}
+			return status;
+		}
+		break;
+	}
+done:
+	spin_unlock_irqrestore(&oxu->lock, flags);
+	return 0;
+}
+
+/* Bulk qh holds the data toggle */
+static void oxu_endpoint_disable(struct usb_hcd *hcd,
+					struct usb_host_endpoint *ep)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+	unsigned long		flags;
+	struct ehci_qh		*qh, *tmp;
+
+	/* ASSERT:  any requests/urbs are being unlinked */
+	/* ASSERT:  nobody can be submitting urbs for this any more */
+
+rescan:
+	spin_lock_irqsave(&oxu->lock, flags);
+	qh = ep->hcpriv;
+	if (!qh)
+		goto done;
+
+	/* endpoints can be iso streams.  for now, we don't
+	 * accelerate iso completions ... so spin a while.
+	 */
+	if (qh->hw_info1 == 0) {
+		oxu_vdbg(oxu, "iso delay\n");
+		goto idle_timeout;
+	}
+
+	if (!HC_IS_RUNNING(hcd->state))
+		qh->qh_state = QH_STATE_IDLE;
+	switch (qh->qh_state) {
+	case QH_STATE_LINKED:
+		for (tmp = oxu->async->qh_next.qh;
+				tmp && tmp != qh;
+				tmp = tmp->qh_next.qh)
+			continue;
+		/* periodic qh self-unlinks on empty */
+		if (!tmp)
+			goto nogood;
+		unlink_async(oxu, qh);
+		/* FALL THROUGH */
+	case QH_STATE_UNLINK:		/* wait for hw to finish? */
+idle_timeout:
+		spin_unlock_irqrestore(&oxu->lock, flags);
+		schedule_timeout_uninterruptible(1);
+		goto rescan;
+	case QH_STATE_IDLE:		/* fully unlinked */
+		if (list_empty(&qh->qtd_list)) {
+			qh_put(qh);
+			break;
+		}
+		/* else FALL THROUGH */
+	default:
+nogood:
+		/* caller was supposed to have unlinked any requests;
+		 * that's not our job.  just leak this memory.
+		 */
+		oxu_err(oxu, "qh %p (#%02x) state %d%s\n",
+			qh, ep->desc.bEndpointAddress, qh->qh_state,
+			list_empty(&qh->qtd_list) ? "" : "(has tds)");
+		break;
+	}
+	ep->hcpriv = NULL;
+done:
+	spin_unlock_irqrestore(&oxu->lock, flags);
+	return;
+}
+
+static int oxu_get_frame(struct usb_hcd *hcd)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+
+	return (readl(&oxu->regs->frame_index) >> 3) %
+		oxu->periodic_size;
+}
+
+/* Build "status change" packet (one or two bytes) from HC registers */
+static int oxu_hub_status_data(struct usb_hcd *hcd, char *buf)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+	u32 temp, mask, status = 0;
+	int ports, i, retval = 1;
+	unsigned long flags;
+
+	/* if !USB_SUSPEND, root hub timers won't get shut down ... */
+	if (!HC_IS_RUNNING(hcd->state))
+		return 0;
+
+	/* init status to no-changes */
+	buf[0] = 0;
+	ports = HCS_N_PORTS(oxu->hcs_params);
+	if (ports > 7) {
+		buf[1] = 0;
+		retval++;
+	}
+
+	/* Some boards (mostly VIA?) report bogus overcurrent indications,
+	 * causing massive log spam unless we completely ignore them.  It
+	 * may be relevant that VIA VT8235 controlers, where PORT_POWER is
+	 * always set, seem to clear PORT_OCC and PORT_CSC when writing to
+	 * PORT_POWER; that's surprising, but maybe within-spec.
+	 */
+	if (!ignore_oc)
+		mask = PORT_CSC | PORT_PEC | PORT_OCC;
+	else
+		mask = PORT_CSC | PORT_PEC;
+
+	/* no hub change reports (bit 0) for now (power, ...) */
+
+	/* port N changes (bit N)? */
+	spin_lock_irqsave(&oxu->lock, flags);
+	for (i = 0; i < ports; i++) {
+		temp = readl(&oxu->regs->port_status[i]);
+
+		/*
+		 * Return status information even for ports with OWNER set.
+		 * Otherwise khubd wouldn't see the disconnect event when a
+		 * high-speed device is switched over to the companion
+		 * controller by the user.
+		 */
+
+		if (!(temp & PORT_CONNECT))
+			oxu->reset_done[i] = 0;
+		if ((temp & mask) != 0 || ((temp & PORT_RESUME) != 0 &&
+				time_after_eq(jiffies, oxu->reset_done[i]))) {
+			if (i < 7)
+				buf[0] |= 1 << (i + 1);
+			else
+				buf[1] |= 1 << (i - 7);
+			status = STS_PCD;
+		}
+	}
+	/* FIXME autosuspend idle root hubs */
+	spin_unlock_irqrestore(&oxu->lock, flags);
+	return status ? retval : 0;
+}
+
+/* Returns the speed of a device attached to a port on the root hub. */
+static inline unsigned int oxu_port_speed(struct oxu_hcd *oxu,
+						unsigned int portsc)
+{
+	switch ((portsc >> 26) & 3) {
+	case 0:
+		return 0;
+	case 1:
+		return 1 << USB_PORT_FEAT_LOWSPEED;
+	case 2:
+	default:
+		return 1 << USB_PORT_FEAT_HIGHSPEED;
+	}
+}
+
+#define	PORT_WAKE_BITS	(PORT_WKOC_E|PORT_WKDISC_E|PORT_WKCONN_E)
+static int oxu_hub_control(struct usb_hcd *hcd, u16 typeReq,
+				u16 wValue, u16 wIndex, char *buf, u16 wLength)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+	int ports = HCS_N_PORTS(oxu->hcs_params);
+	u32 __iomem *status_reg = &oxu->regs->port_status[wIndex - 1];
+	u32 temp, status;
+	unsigned long	flags;
+	int retval = 0;
+	unsigned selector;
+
+	/*
+	 * FIXME:  support SetPortFeatures USB_PORT_FEAT_INDICATOR.
+	 * HCS_INDICATOR may say we can change LEDs to off/amber/green.
+	 * (track current state ourselves) ... blink for diagnostics,
+	 * power, "this is the one", etc.  EHCI spec supports this.
+	 */
+
+	spin_lock_irqsave(&oxu->lock, flags);
+	switch (typeReq) {
+	case ClearHubFeature:
+		switch (wValue) {
+		case C_HUB_LOCAL_POWER:
+		case C_HUB_OVER_CURRENT:
+			/* no hub-wide feature/status flags */
+			break;
+		default:
+			goto error;
+		}
+		break;
+	case ClearPortFeature:
+		if (!wIndex || wIndex > ports)
+			goto error;
+		wIndex--;
+		temp = readl(status_reg);
+
+		/*
+		 * Even if OWNER is set, so the port is owned by the
+		 * companion controller, khubd needs to be able to clear
+		 * the port-change status bits (especially
+		 * USB_PORT_FEAT_C_CONNECTION).
+		 */
+
+		switch (wValue) {
+		case USB_PORT_FEAT_ENABLE:
+			writel(temp & ~PORT_PE, status_reg);
+			break;
+		case USB_PORT_FEAT_C_ENABLE:
+			writel((temp & ~PORT_RWC_BITS) | PORT_PEC, status_reg);
+			break;
+		case USB_PORT_FEAT_SUSPEND:
+			if (temp & PORT_RESET)
+				goto error;
+			if (temp & PORT_SUSPEND) {
+				if ((temp & PORT_PE) == 0)
+					goto error;
+				/* resume signaling for 20 msec */
+				temp &= ~(PORT_RWC_BITS | PORT_WAKE_BITS);
+				writel(temp | PORT_RESUME, status_reg);
+				oxu->reset_done[wIndex] = jiffies
+						+ msecs_to_jiffies(20);
+			}
+			break;
+		case USB_PORT_FEAT_C_SUSPEND:
+			/* we auto-clear this feature */
+			break;
+		case USB_PORT_FEAT_POWER:
+			if (HCS_PPC(oxu->hcs_params))
+				writel(temp & ~(PORT_RWC_BITS | PORT_POWER),
+					  status_reg);
+			break;
+		case USB_PORT_FEAT_C_CONNECTION:
+			writel((temp & ~PORT_RWC_BITS) | PORT_CSC, status_reg);
+			break;
+		case USB_PORT_FEAT_C_OVER_CURRENT:
+			writel((temp & ~PORT_RWC_BITS) | PORT_OCC, status_reg);
+			break;
+		case USB_PORT_FEAT_C_RESET:
+			/* GetPortStatus clears reset */
+			break;
+		default:
+			goto error;
+		}
+		readl(&oxu->regs->command);	/* unblock posted write */
+		break;
+	case GetHubDescriptor:
+		ehci_hub_descriptor(oxu, (struct usb_hub_descriptor *)
+			buf);
+		break;
+	case GetHubStatus:
+		/* no hub-wide feature/status flags */
+		memset(buf, 0, 4);
+		break;
+	case GetPortStatus:
+		if (!wIndex || wIndex > ports)
+			goto error;
+		wIndex--;
+		status = 0;
+		temp = readl(status_reg);
+
+		/* wPortChange bits */
+		if (temp & PORT_CSC)
+			status |= 1 << USB_PORT_FEAT_C_CONNECTION;
+		if (temp & PORT_PEC)
+			status |= 1 << USB_PORT_FEAT_C_ENABLE;
+		if ((temp & PORT_OCC) && !ignore_oc)
+			status |= 1 << USB_PORT_FEAT_C_OVER_CURRENT;
+
+		/* whoever resumes must GetPortStatus to complete it!! */
+		if (temp & PORT_RESUME) {
+
+			/* Remote Wakeup received? */
+			if (!oxu->reset_done[wIndex]) {
+				/* resume signaling for 20 msec */
+				oxu->reset_done[wIndex] = jiffies
+						+ msecs_to_jiffies(20);
+				/* check the port again */
+				mod_timer(&oxu_to_hcd(oxu)->rh_timer,
+						oxu->reset_done[wIndex]);
+			}
+
+			/* resume completed? */
+			else if (time_after_eq(jiffies,
+					oxu->reset_done[wIndex])) {
+				status |= 1 << USB_PORT_FEAT_C_SUSPEND;
+				oxu->reset_done[wIndex] = 0;
+
+				/* stop resume signaling */
+				temp = readl(status_reg);
+				writel(temp & ~(PORT_RWC_BITS | PORT_RESUME),
+					status_reg);
+				retval = handshake(oxu, status_reg,
+					   PORT_RESUME, 0, 2000 /* 2msec */);
+				if (retval != 0) {
+					oxu_err(oxu,
+						"port %d resume error %d\n",
+						wIndex + 1, retval);
+					goto error;
+				}
+				temp &= ~(PORT_SUSPEND|PORT_RESUME|(3<<10));
+			}
+		}
+
+		/* whoever resets must GetPortStatus to complete it!! */
+		if ((temp & PORT_RESET)
+				&& time_after_eq(jiffies,
+					oxu->reset_done[wIndex])) {
+			status |= 1 << USB_PORT_FEAT_C_RESET;
+			oxu->reset_done[wIndex] = 0;
+
+			/* force reset to complete */
+			writel(temp & ~(PORT_RWC_BITS | PORT_RESET),
+					status_reg);
+			/* REVISIT:  some hardware needs 550+ usec to clear
+			 * this bit; seems too long to spin routinely...
+			 */
+			retval = handshake(oxu, status_reg,
+					PORT_RESET, 0, 750);
+			if (retval != 0) {
+				oxu_err(oxu, "port %d reset error %d\n",
+					wIndex + 1, retval);
+				goto error;
+			}
+
+			/* see what we found out */
+			temp = check_reset_complete(oxu, wIndex, status_reg,
+					readl(status_reg));
+		}
+
+		/* transfer dedicated ports to the companion hc */
+		if ((temp & PORT_CONNECT) &&
+				test_bit(wIndex, &oxu->companion_ports)) {
+			temp &= ~PORT_RWC_BITS;
+			temp |= PORT_OWNER;
+			writel(temp, status_reg);
+			oxu_dbg(oxu, "port %d --> companion\n", wIndex + 1);
+			temp = readl(status_reg);
+		}
+
+		/*
+		 * Even if OWNER is set, there's no harm letting khubd
+		 * see the wPortStatus values (they should all be 0 except
+		 * for PORT_POWER anyway).
+		 */
+
+		if (temp & PORT_CONNECT) {
+			status |= 1 << USB_PORT_FEAT_CONNECTION;
+			/* status may be from integrated TT */
+			status |= oxu_port_speed(oxu, temp);
+		}
+		if (temp & PORT_PE)
+			status |= 1 << USB_PORT_FEAT_ENABLE;
+		if (temp & (PORT_SUSPEND|PORT_RESUME))
+			status |= 1 << USB_PORT_FEAT_SUSPEND;
+		if (temp & PORT_OC)
+			status |= 1 << USB_PORT_FEAT_OVER_CURRENT;
+		if (temp & PORT_RESET)
+			status |= 1 << USB_PORT_FEAT_RESET;
+		if (temp & PORT_POWER)
+			status |= 1 << USB_PORT_FEAT_POWER;
+
+#ifndef	OXU_VERBOSE_DEBUG
+	if (status & ~0xffff)	/* only if wPortChange is interesting */
+#endif
+		dbg_port(oxu, "GetStatus", wIndex + 1, temp);
+		put_unaligned(cpu_to_le32(status), (__le32 *) buf);
+		break;
+	case SetHubFeature:
+		switch (wValue) {
+		case C_HUB_LOCAL_POWER:
+		case C_HUB_OVER_CURRENT:
+			/* no hub-wide feature/status flags */
+			break;
+		default:
+			goto error;
+		}
+		break;
+	case SetPortFeature:
+		selector = wIndex >> 8;
+		wIndex &= 0xff;
+		if (!wIndex || wIndex > ports)
+			goto error;
+		wIndex--;
+		temp = readl(status_reg);
+		if (temp & PORT_OWNER)
+			break;
+
+		temp &= ~PORT_RWC_BITS;
+		switch (wValue) {
+		case USB_PORT_FEAT_SUSPEND:
+			if ((temp & PORT_PE) == 0
+					|| (temp & PORT_RESET) != 0)
+				goto error;
+			if (device_may_wakeup(&hcd->self.root_hub->dev))
+				temp |= PORT_WAKE_BITS;
+			writel(temp | PORT_SUSPEND, status_reg);
+			break;
+		case USB_PORT_FEAT_POWER:
+			if (HCS_PPC(oxu->hcs_params))
+				writel(temp | PORT_POWER, status_reg);
+			break;
+		case USB_PORT_FEAT_RESET:
+			if (temp & PORT_RESUME)
+				goto error;
+			/* line status bits may report this as low speed,
+			 * which can be fine if this root hub has a
+			 * transaction translator built in.
+			 */
+			oxu_vdbg(oxu, "port %d reset\n", wIndex + 1);
+			temp |= PORT_RESET;
+			temp &= ~PORT_PE;
+
+			/*
+			 * caller must wait, then call GetPortStatus
+			 * usb 2.0 spec says 50 ms resets on root
+			 */
+			oxu->reset_done[wIndex] = jiffies
+					+ msecs_to_jiffies(50);
+			writel(temp, status_reg);
+			break;
+
+		/* For downstream facing ports (these):  one hub port is put
+		 * into test mode according to USB2 11.24.2.13, then the hub
+		 * must be reset (which for root hub now means rmmod+modprobe,
+		 * or else system reboot).  See EHCI 2.3.9 and 4.14 for info
+		 * about the EHCI-specific stuff.
+		 */
+		case USB_PORT_FEAT_TEST:
+			if (!selector || selector > 5)
+				goto error;
+			ehci_quiesce(oxu);
+			ehci_halt(oxu);
+			temp |= selector << 16;
+			writel(temp, status_reg);
+			break;
+
+		default:
+			goto error;
+		}
+		readl(&oxu->regs->command);	/* unblock posted writes */
+		break;
+
+	default:
+error:
+		/* "stall" on error */
+		retval = -EPIPE;
+	}
+	spin_unlock_irqrestore(&oxu->lock, flags);
+	return retval;
+}
+
+#ifdef CONFIG_PM
+
+static int oxu_bus_suspend(struct usb_hcd *hcd)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+	int port;
+	int mask;
+
+	oxu_dbg(oxu, "suspend root hub\n");
+
+	if (time_before(jiffies, oxu->next_statechange))
+		msleep(5);
+
+	port = HCS_N_PORTS(oxu->hcs_params);
+	spin_lock_irq(&oxu->lock);
+
+	/* stop schedules, clean any completed work */
+	if (HC_IS_RUNNING(hcd->state)) {
+		ehci_quiesce(oxu);
+		hcd->state = HC_STATE_QUIESCING;
+	}
+	oxu->command = readl(&oxu->regs->command);
+	if (oxu->reclaim)
+		oxu->reclaim_ready = 1;
+	ehci_work(oxu);
+
+	/* Unlike other USB host controller types, EHCI doesn't have
+	 * any notion of "global" or bus-wide suspend.  The driver has
+	 * to manually suspend all the active unsuspended ports, and
+	 * then manually resume them in the bus_resume() routine.
+	 */
+	oxu->bus_suspended = 0;
+	while (port--) {
+		u32 __iomem *reg = &oxu->regs->port_status[port];
+		u32 t1 = readl(reg) & ~PORT_RWC_BITS;
+		u32 t2 = t1;
+
+		/* keep track of which ports we suspend */
+		if ((t1 & PORT_PE) && !(t1 & PORT_OWNER) &&
+				!(t1 & PORT_SUSPEND)) {
+			t2 |= PORT_SUSPEND;
+			set_bit(port, &oxu->bus_suspended);
+		}
+
+		/* enable remote wakeup on all ports */
+		if (device_may_wakeup(&hcd->self.root_hub->dev))
+			t2 |= PORT_WKOC_E|PORT_WKDISC_E|PORT_WKCONN_E;
+		else
+			t2 &= ~(PORT_WKOC_E|PORT_WKDISC_E|PORT_WKCONN_E);
+
+		if (t1 != t2) {
+			oxu_vdbg(oxu, "port %d, %08x -> %08x\n",
+				port + 1, t1, t2);
+			writel(t2, reg);
+		}
+	}
+
+	/* turn off now-idle HC */
+	del_timer_sync(&oxu->watchdog);
+	ehci_halt(oxu);
+	hcd->state = HC_STATE_SUSPENDED;
+
+	/* allow remote wakeup */
+	mask = INTR_MASK;
+	if (!device_may_wakeup(&hcd->self.root_hub->dev))
+		mask &= ~STS_PCD;
+	writel(mask, &oxu->regs->intr_enable);
+	readl(&oxu->regs->intr_enable);
+
+	oxu->next_statechange = jiffies + msecs_to_jiffies(10);
+	spin_unlock_irq(&oxu->lock);
+	return 0;
+}
+
+/* Caller has locked the root hub, and should reset/reinit on error */
+static int oxu_bus_resume(struct usb_hcd *hcd)
+{
+	struct oxu_hcd *oxu = hcd_to_oxu(hcd);
+	u32 temp;
+	int i;
+
+	if (time_before(jiffies, oxu->next_statechange))
+		msleep(5);
+	spin_lock_irq(&oxu->lock);
+
+	/* Ideally and we've got a real resume here, and no port's power
+	 * was lost.  (For PCI, that means Vaux was maintained.)  But we
+	 * could instead be restoring a swsusp snapshot -- so that BIOS was
+	 * the last user of the controller, not reset/pm hardware keeping
+	 * state we gave to it.
+	 */
+	temp = readl(&oxu->regs->intr_enable);
+	oxu_dbg(oxu, "resume root hub%s\n", temp ? "" : " after power loss");
+
+	/* at least some APM implementations will try to deliver
+	 * IRQs right away, so delay them until we're ready.
+	 */
+	writel(0, &oxu->regs->intr_enable);
+
+	/* re-init operational registers */
+	writel(0, &oxu->regs->segment);
+	writel(oxu->periodic_dma, &oxu->regs->frame_list);
+	writel((u32) oxu->async->qh_dma, &oxu->regs->async_next);
+
+	/* restore CMD_RUN, framelist size, and irq threshold */
+	writel(oxu->command, &oxu->regs->command);
+
+	/* Some controller/firmware combinations need a delay during which
+	 * they set up the port statuses.  See Bugzilla #8190. */
+	mdelay(8);
+
+	/* manually resume the ports we suspended during bus_suspend() */
+	i = HCS_N_PORTS(oxu->hcs_params);
+	while (i--) {
+		temp = readl(&oxu->regs->port_status[i]);
+		temp &= ~(PORT_RWC_BITS
+			| PORT_WKOC_E | PORT_WKDISC_E | PORT_WKCONN_E);
+		if (test_bit(i, &oxu->bus_suspended) && (temp & PORT_SUSPEND)) {
+			oxu->reset_done[i] = jiffies + msecs_to_jiffies(20);
+			temp |= PORT_RESUME;
+		}
+		writel(temp, &oxu->regs->port_status[i]);
+	}
+	i = HCS_N_PORTS(oxu->hcs_params);
+	mdelay(20);
+	while (i--) {
+		temp = readl(&oxu->regs->port_status[i]);
+		if (test_bit(i, &oxu->bus_suspended) && (temp & PORT_SUSPEND)) {
+			temp &= ~(PORT_RWC_BITS | PORT_RESUME);
+			writel(temp, &oxu->regs->port_status[i]);
+			oxu_vdbg(oxu, "resumed port %d\n", i + 1);
+		}
+	}
+	(void) readl(&oxu->regs->command);
+
+	/* maybe re-activate the schedule(s) */
+	temp = 0;
+	if (oxu->async->qh_next.qh)
+		temp |= CMD_ASE;
+	if (oxu->periodic_sched)
+		temp |= CMD_PSE;
+	if (temp) {
+		oxu->command |= temp;
+		writel(oxu->command, &oxu->regs->command);
+	}
+
+	oxu->next_statechange = jiffies + msecs_to_jiffies(5);
+	hcd->state = HC_STATE_RUNNING;
+
+	/* Now we can safely re-enable irqs */
+	writel(INTR_MASK, &oxu->regs->intr_enable);
+
+	spin_unlock_irq(&oxu->lock);
+	return 0;
+}
+
+#else
+
+static int oxu_bus_suspend(struct usb_hcd *hcd)
+{
+	return 0;
+}
+
+static int oxu_bus_resume(struct usb_hcd *hcd)
+{
+	return 0;
+}
+
+#endif	/* CONFIG_PM */
+
+static const struct hc_driver oxu_hc_driver = {
+	.description =		"oxu210hp_hcd",
+	.product_desc =		"oxu210hp HCD",
+	.hcd_priv_size =	sizeof(struct oxu_hcd),
+
+	/*
+	 * Generic hardware linkage
+	 */
+	.irq =			oxu_irq,
+	.flags =		HCD_MEMORY | HCD_USB2,
+
+	/*
+	 * Basic lifecycle operations
+	 */
+	.reset =		oxu_reset,
+	.start =		oxu_run,
+	.stop =			oxu_stop,
+	.shutdown =		oxu_shutdown,
+
+	/*
+	 * Managing i/o requests and associated device resources
+	 */
+	.urb_enqueue =		oxu_urb_enqueue,
+	.urb_dequeue =		oxu_urb_dequeue,
+	.endpoint_disable =	oxu_endpoint_disable,
+
+	/*
+	 * Scheduling support
+	 */
+	.get_frame_number =	oxu_get_frame,
+
+	/*
+	 * Root hub support
+	 */
+	.hub_status_data =	oxu_hub_status_data,
+	.hub_control =		oxu_hub_control,
+	.bus_suspend =		oxu_bus_suspend,
+	.bus_resume =		oxu_bus_resume,
+};
+
+/*
+ * Module stuff
+ */
+
+static void oxu_configuration(struct platform_device *pdev, void *base)
+{
+	u32 tmp;
+
+	/* Initialize top level registers.
+	 * First write ever
+	 */
+	oxu_writel(base, OXU_HOSTIFCONFIG, 0x0000037D);
+	oxu_writel(base, OXU_SOFTRESET, OXU_SRESET);
+	oxu_writel(base, OXU_HOSTIFCONFIG, 0x0000037D);
+
+	tmp = oxu_readl(base, OXU_PIOBURSTREADCTRL);
+	oxu_writel(base, OXU_PIOBURSTREADCTRL, tmp | 0x0040);
+
+	oxu_writel(base, OXU_ASO, OXU_SPHPOEN | OXU_OVRCCURPUPDEN |
+					OXU_COMPARATOR | OXU_ASO_OP);
+
+	tmp = oxu_readl(base, OXU_CLKCTRL_SET);
+	oxu_writel(base, OXU_CLKCTRL_SET, tmp | OXU_SYSCLKEN | OXU_USBOTGCLKEN);
+
+	/* Clear all top interrupt enable */
+	oxu_writel(base, OXU_CHIPIRQEN_CLR, 0xff);
+
+	/* Clear all top interrupt status */
+	oxu_writel(base, OXU_CHIPIRQSTATUS, 0xff);
+
+	/* Enable all needed top interrupt except OTG SPH core */
+	oxu_writel(base, OXU_CHIPIRQEN_SET, OXU_USBSPHLPWUI | OXU_USBOTGLPWUI);
+}
+
+static int oxu_verify_id(struct platform_device *pdev, void *base)
+{
+	u32 id;
+	char *bo[] = {
+		"reserved",
+		"128-pin LQFP",
+		"84-pin TFBGA",
+		"reserved",
+	};
+
+	/* Read controller signature register to find a match */
+	id = oxu_readl(base, OXU_DEVICEID);
+	dev_info(&pdev->dev, "device ID %x\n", id);
+	if ((id & OXU_REV_MASK) != (OXU_REV_2100 << OXU_REV_SHIFT))
+		return -1;
+
+	dev_info(&pdev->dev, "found device %x %s (%04x:%04x)\n",
+		id >> OXU_REV_SHIFT,
+		bo[(id & OXU_BO_MASK) >> OXU_BO_SHIFT],
+		(id & OXU_MAJ_REV_MASK) >> OXU_MAJ_REV_SHIFT,
+		(id & OXU_MIN_REV_MASK) >> OXU_MIN_REV_SHIFT);
+
+	return 0;
+}
+
+static const struct hc_driver oxu_hc_driver;
+static struct usb_hcd *oxu_create(struct platform_device *pdev,
+				unsigned long memstart, unsigned long memlen,
+				void *base, int irq, int otg)
+{
+	struct device *dev = &pdev->dev;
+
+	struct usb_hcd *hcd;
+	struct oxu_hcd *oxu;
+	int ret;
+
+	/* Set endian mode and host mode */
+	oxu_writel(base + (otg ? OXU_OTG_CORE_OFFSET : OXU_SPH_CORE_OFFSET),
+				OXU_USBMODE,
+				OXU_CM_HOST_ONLY | OXU_ES_LITTLE | OXU_VBPS);
+
+	hcd = usb_create_hcd(&oxu_hc_driver, dev,
+				otg ? "oxu210hp_otg" : "oxu210hp_sph");
+	if (!hcd)
+		return ERR_PTR(-ENOMEM);
+
+	hcd->rsrc_start = memstart;
+	hcd->rsrc_len = memlen;
+	hcd->regs = base;
+	hcd->irq = irq;
+	hcd->state = HC_STATE_HALT;
+
+	oxu = hcd_to_oxu(hcd);
+	oxu->is_otg = otg;
+
+	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	return hcd;
+}
+
+static int oxu_init(struct platform_device *pdev,
+				unsigned long memstart, unsigned long memlen,
+				void *base, int irq)
+{
+	struct oxu_info *info = platform_get_drvdata(pdev);
+	struct usb_hcd *hcd;
+	int ret;
+
+	/* First time configuration at start up */
+	oxu_configuration(pdev, base);
+
+	ret = oxu_verify_id(pdev, base);
+	if (ret) {
+		dev_err(&pdev->dev, "no devices found!\n");
+		return -ENODEV;
+	}
+
+	/* Create the OTG controller */
+	hcd = oxu_create(pdev, memstart, memlen, base, irq, 1);
+	if (IS_ERR(hcd)) {
+		dev_err(&pdev->dev, "cannot create OTG controller!\n");
+		ret = PTR_ERR(hcd);
+		goto error_create_otg;
+	}
+	info->hcd[0] = hcd;
+
+	/* Create the SPH host controller */
+	hcd = oxu_create(pdev, memstart, memlen, base, irq, 0);
+	if (IS_ERR(hcd)) {
+		dev_err(&pdev->dev, "cannot create SPH controller!\n");
+		ret = PTR_ERR(hcd);
+		goto error_create_sph;
+	}
+	info->hcd[1] = hcd;
+
+	oxu_writel(base, OXU_CHIPIRQEN_SET,
+		oxu_readl(base, OXU_CHIPIRQEN_SET) | 3);
+
+	return 0;
+
+error_create_sph:
+	usb_remove_hcd(info->hcd[0]);
+	usb_put_hcd(info->hcd[0]);
+
+error_create_otg:
+	return ret;
+}
+
+static int oxu_drv_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	void *base;
+	unsigned long memstart, memlen;
+	int irq, ret;
+	struct oxu_info *info;
+
+	if (usb_disabled())
+		return -ENODEV;
+
+	/*
+	 * Get the platform resources
+	 */
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(&pdev->dev,
+			"no IRQ! Check %s setup!\n", dev_name(&pdev->dev));
+		return -ENODEV;
+	}
+	irq = res->start;
+	dev_dbg(&pdev->dev, "IRQ resource %d\n", irq);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "no registers address! Check %s setup!\n",
+			dev_name(&pdev->dev));
+		return -ENODEV;
+	}
+	memstart = res->start;
+	memlen = res->end - res->start + 1;
+	dev_dbg(&pdev->dev, "MEM resource %lx-%lx\n", memstart, memlen);
+	if (!request_mem_region(memstart, memlen,
+				oxu_hc_driver.description)) {
+		dev_dbg(&pdev->dev, "memory area already in use\n");
+		return -EBUSY;
+	}
+
+	ret = set_irq_type(irq, IRQF_TRIGGER_FALLING);
+	if (ret) {
+		dev_err(&pdev->dev, "error setting irq type\n");
+		ret = -EFAULT;
+		goto error_set_irq_type;
+	}
+
+	base = ioremap(memstart, memlen);
+	if (!base) {
+		dev_dbg(&pdev->dev, "error mapping memory\n");
+		ret = -EFAULT;
+		goto error_ioremap;
+	}
+
+	/* Allocate a driver data struct to hold useful info for both
+	 * SPH & OTG devices
+	 */
+	info = kzalloc(sizeof(struct oxu_info), GFP_KERNEL);
+	if (!info) {
+		dev_dbg(&pdev->dev, "error allocating memory\n");
+		ret = -EFAULT;
+		goto error_alloc;
+	}
+	platform_set_drvdata(pdev, info);
+
+	ret = oxu_init(pdev, memstart, memlen, base, irq);
+	if (ret < 0) {
+		dev_dbg(&pdev->dev, "cannot init USB devices\n");
+		goto error_init;
+	}
+
+	dev_info(&pdev->dev, "devices enabled and running\n");
+	platform_set_drvdata(pdev, info);
+
+	return 0;
+
+error_init:
+	kfree(info);
+	platform_set_drvdata(pdev, NULL);
+
+error_alloc:
+	iounmap(base);
+
+error_set_irq_type:
+error_ioremap:
+	release_mem_region(memstart, memlen);
+
+	dev_err(&pdev->dev, "init %s fail, %d\n", dev_name(&pdev->dev), ret);
+	return ret;
+}
+
+static void oxu_remove(struct platform_device *pdev, struct usb_hcd *hcd)
+{
+	usb_remove_hcd(hcd);
+	usb_put_hcd(hcd);
+}
+
+static int oxu_drv_remove(struct platform_device *pdev)
+{
+	struct oxu_info *info = platform_get_drvdata(pdev);
+	unsigned long memstart = info->hcd[0]->rsrc_start,
+			memlen = info->hcd[0]->rsrc_len;
+	void *base = info->hcd[0]->regs;
+
+	oxu_remove(pdev, info->hcd[0]);
+	oxu_remove(pdev, info->hcd[1]);
+
+	iounmap(base);
+	release_mem_region(memstart, memlen);
+
+	kfree(info);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static void oxu_drv_shutdown(struct platform_device *pdev)
+{
+	oxu_drv_remove(pdev);
+}
+
+#if 0
+/* FIXME: TODO */
+static int oxu_drv_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+
+	return 0;
+}
+
+static int oxu_drv_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+
+	return 0;
+}
+#else
+#define oxu_drv_suspend	NULL
+#define oxu_drv_resume	NULL
+#endif
+
+static struct platform_driver oxu_driver = {
+	.probe		= oxu_drv_probe,
+	.remove		= oxu_drv_remove,
+	.shutdown	= oxu_drv_shutdown,
+	.suspend	= oxu_drv_suspend,
+	.resume		= oxu_drv_resume,
+	.driver = {
+		.name = "oxu210hp-hcd",
+		.bus = &platform_bus_type
+	}
+};
+
+static int __init oxu_module_init(void)
+{
+	int retval = 0;
+
+	retval = platform_driver_register(&oxu_driver);
+	if (retval < 0)
+		return retval;
+
+	return retval;
+}
+
+static void __exit oxu_module_cleanup(void)
+{
+	platform_driver_unregister(&oxu_driver);
+}
+
+module_init(oxu_module_init);
+module_exit(oxu_module_cleanup);
+
+MODULE_DESCRIPTION("Oxford OXU210HP HCD driver - ver. " DRIVER_VERSION);
+MODULE_AUTHOR("Rodolfo Giometti <giometti@linux.it>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/host/oxu210hp.h b/drivers/usb/host/oxu210hp.h
new file mode 100644
index 000000000000..8910e271cc7d
--- /dev/null
+++ b/drivers/usb/host/oxu210hp.h
@@ -0,0 +1,447 @@
+/*
+ * Host interface registers
+ */
+
+#define OXU_DEVICEID			0x00
+	#define OXU_REV_MASK		0xffff0000
+	#define OXU_REV_SHIFT		16
+	#define OXU_REV_2100		0x2100
+	#define OXU_BO_SHIFT		8
+	#define OXU_BO_MASK		(0x3 << OXU_BO_SHIFT)
+	#define OXU_MAJ_REV_SHIFT	4
+	#define OXU_MAJ_REV_MASK	(0xf << OXU_MAJ_REV_SHIFT)
+	#define OXU_MIN_REV_SHIFT	0
+	#define OXU_MIN_REV_MASK	(0xf << OXU_MIN_REV_SHIFT)
+#define OXU_HOSTIFCONFIG		0x04
+#define OXU_SOFTRESET			0x08
+	#define OXU_SRESET		(1 << 0)
+
+#define OXU_PIOBURSTREADCTRL		0x0C
+
+#define OXU_CHIPIRQSTATUS		0x10
+#define OXU_CHIPIRQEN_SET		0x14
+#define OXU_CHIPIRQEN_CLR		0x18
+	#define OXU_USBSPHLPWUI		0x00000080
+	#define OXU_USBOTGLPWUI		0x00000040
+	#define OXU_USBSPHI		0x00000002
+	#define OXU_USBOTGI		0x00000001
+
+#define OXU_CLKCTRL_SET			0x1C
+	#define OXU_SYSCLKEN		0x00000008
+	#define OXU_USBSPHCLKEN		0x00000002
+	#define OXU_USBOTGCLKEN		0x00000001
+
+#define OXU_ASO				0x68
+	#define OXU_SPHPOEN		0x00000100
+	#define OXU_OVRCCURPUPDEN	0x00000800
+	#define OXU_ASO_OP		(1 << 10)
+	#define OXU_COMPARATOR		0x000004000
+
+#define OXU_USBMODE			0x1A8
+	#define OXU_VBPS		0x00000020
+	#define OXU_ES_LITTLE		0x00000000
+	#define OXU_CM_HOST_ONLY	0x00000003
+
+/*
+ * Proper EHCI structs & defines
+ */
+
+/* Magic numbers that can affect system performance */
+#define EHCI_TUNE_CERR		3	/* 0-3 qtd retries; 0 == don't stop */
+#define EHCI_TUNE_RL_HS		4	/* nak throttle; see 4.9 */
+#define EHCI_TUNE_RL_TT		0
+#define EHCI_TUNE_MULT_HS	1	/* 1-3 transactions/uframe; 4.10.3 */
+#define EHCI_TUNE_MULT_TT	1
+#define EHCI_TUNE_FLS		2	/* (small) 256 frame schedule */
+
+struct oxu_hcd;
+
+/* EHCI register interface, corresponds to EHCI Revision 0.95 specification */
+
+/* Section 2.2 Host Controller Capability Registers */
+struct ehci_caps {
+	/* these fields are specified as 8 and 16 bit registers,
+	 * but some hosts can't perform 8 or 16 bit PCI accesses.
+	 */
+	u32		hc_capbase;
+#define HC_LENGTH(p)		(((p)>>00)&0x00ff)	/* bits 7:0 */
+#define HC_VERSION(p)		(((p)>>16)&0xffff)	/* bits 31:16 */
+	u32		hcs_params;     /* HCSPARAMS - offset 0x4 */
+#define HCS_DEBUG_PORT(p)	(((p)>>20)&0xf)	/* bits 23:20, debug port? */
+#define HCS_INDICATOR(p)	((p)&(1 << 16))	/* true: has port indicators */
+#define HCS_N_CC(p)		(((p)>>12)&0xf)	/* bits 15:12, #companion HCs */
+#define HCS_N_PCC(p)		(((p)>>8)&0xf)	/* bits 11:8, ports per CC */
+#define HCS_PORTROUTED(p)	((p)&(1 << 7))	/* true: port routing */
+#define HCS_PPC(p)		((p)&(1 << 4))	/* true: port power control */
+#define HCS_N_PORTS(p)		(((p)>>0)&0xf)	/* bits 3:0, ports on HC */
+
+	u32		hcc_params;      /* HCCPARAMS - offset 0x8 */
+#define HCC_EXT_CAPS(p)		(((p)>>8)&0xff)	/* for pci extended caps */
+#define HCC_ISOC_CACHE(p)       ((p)&(1 << 7))  /* true: can cache isoc frame */
+#define HCC_ISOC_THRES(p)       (((p)>>4)&0x7)  /* bits 6:4, uframes cached */
+#define HCC_CANPARK(p)		((p)&(1 << 2))  /* true: can park on async qh */
+#define HCC_PGM_FRAMELISTLEN(p) ((p)&(1 << 1))  /* true: periodic_size changes*/
+#define HCC_64BIT_ADDR(p)       ((p)&(1))       /* true: can use 64-bit addr */
+	u8		portroute[8];	 /* nibbles for routing - offset 0xC */
+} __attribute__ ((packed));
+
+
+/* Section 2.3 Host Controller Operational Registers */
+struct ehci_regs {
+	/* USBCMD: offset 0x00 */
+	u32		command;
+/* 23:16 is r/w intr rate, in microframes; default "8" == 1/msec */
+#define CMD_PARK	(1<<11)		/* enable "park" on async qh */
+#define CMD_PARK_CNT(c)	(((c)>>8)&3)	/* how many transfers to park for */
+#define CMD_LRESET	(1<<7)		/* partial reset (no ports, etc) */
+#define CMD_IAAD	(1<<6)		/* "doorbell" interrupt async advance */
+#define CMD_ASE		(1<<5)		/* async schedule enable */
+#define CMD_PSE		(1<<4)		/* periodic schedule enable */
+/* 3:2 is periodic frame list size */
+#define CMD_RESET	(1<<1)		/* reset HC not bus */
+#define CMD_RUN		(1<<0)		/* start/stop HC */
+
+	/* USBSTS: offset 0x04 */
+	u32		status;
+#define STS_ASS		(1<<15)		/* Async Schedule Status */
+#define STS_PSS		(1<<14)		/* Periodic Schedule Status */
+#define STS_RECL	(1<<13)		/* Reclamation */
+#define STS_HALT	(1<<12)		/* Not running (any reason) */
+/* some bits reserved */
+	/* these STS_* flags are also intr_enable bits (USBINTR) */
+#define STS_IAA		(1<<5)		/* Interrupted on async advance */
+#define STS_FATAL	(1<<4)		/* such as some PCI access errors */
+#define STS_FLR		(1<<3)		/* frame list rolled over */
+#define STS_PCD		(1<<2)		/* port change detect */
+#define STS_ERR		(1<<1)		/* "error" completion (overflow, ...) */
+#define STS_INT		(1<<0)		/* "normal" completion (short, ...) */
+
+#define INTR_MASK (STS_IAA | STS_FATAL | STS_PCD | STS_ERR | STS_INT)
+
+	/* USBINTR: offset 0x08 */
+	u32		intr_enable;
+
+	/* FRINDEX: offset 0x0C */
+	u32		frame_index;	/* current microframe number */
+	/* CTRLDSSEGMENT: offset 0x10 */
+	u32		segment;	/* address bits 63:32 if needed */
+	/* PERIODICLISTBASE: offset 0x14 */
+	u32		frame_list;	/* points to periodic list */
+	/* ASYNCLISTADDR: offset 0x18 */
+	u32		async_next;	/* address of next async queue head */
+
+	u32		reserved[9];
+
+	/* CONFIGFLAG: offset 0x40 */
+	u32		configured_flag;
+#define FLAG_CF		(1<<0)		/* true: we'll support "high speed" */
+
+	/* PORTSC: offset 0x44 */
+	u32		port_status[0];	/* up to N_PORTS */
+/* 31:23 reserved */
+#define PORT_WKOC_E	(1<<22)		/* wake on overcurrent (enable) */
+#define PORT_WKDISC_E	(1<<21)		/* wake on disconnect (enable) */
+#define PORT_WKCONN_E	(1<<20)		/* wake on connect (enable) */
+/* 19:16 for port testing */
+#define PORT_LED_OFF	(0<<14)
+#define PORT_LED_AMBER	(1<<14)
+#define PORT_LED_GREEN	(2<<14)
+#define PORT_LED_MASK	(3<<14)
+#define PORT_OWNER	(1<<13)		/* true: companion hc owns this port */
+#define PORT_POWER	(1<<12)		/* true: has power (see PPC) */
+#define PORT_USB11(x) (((x)&(3<<10)) == (1<<10))	/* USB 1.1 device */
+/* 11:10 for detecting lowspeed devices (reset vs release ownership) */
+/* 9 reserved */
+#define PORT_RESET	(1<<8)		/* reset port */
+#define PORT_SUSPEND	(1<<7)		/* suspend port */
+#define PORT_RESUME	(1<<6)		/* resume it */
+#define PORT_OCC	(1<<5)		/* over current change */
+#define PORT_OC		(1<<4)		/* over current active */
+#define PORT_PEC	(1<<3)		/* port enable change */
+#define PORT_PE		(1<<2)		/* port enable */
+#define PORT_CSC	(1<<1)		/* connect status change */
+#define PORT_CONNECT	(1<<0)		/* device connected */
+#define PORT_RWC_BITS   (PORT_CSC | PORT_PEC | PORT_OCC)
+} __attribute__ ((packed));
+
+/* Appendix C, Debug port ... intended for use with special "debug devices"
+ * that can help if there's no serial console.  (nonstandard enumeration.)
+ */
+struct ehci_dbg_port {
+	u32	control;
+#define DBGP_OWNER	(1<<30)
+#define DBGP_ENABLED	(1<<28)
+#define DBGP_DONE	(1<<16)
+#define DBGP_INUSE	(1<<10)
+#define DBGP_ERRCODE(x)	(((x)>>7)&0x07)
+#	define DBGP_ERR_BAD	1
+#	define DBGP_ERR_SIGNAL	2
+#define DBGP_ERROR	(1<<6)
+#define DBGP_GO		(1<<5)
+#define DBGP_OUT	(1<<4)
+#define DBGP_LEN(x)	(((x)>>0)&0x0f)
+	u32	pids;
+#define DBGP_PID_GET(x)		(((x)>>16)&0xff)
+#define DBGP_PID_SET(data, tok)	(((data)<<8)|(tok))
+	u32	data03;
+	u32	data47;
+	u32	address;
+#define DBGP_EPADDR(dev, ep)	(((dev)<<8)|(ep))
+} __attribute__ ((packed));
+
+
+#define	QTD_NEXT(dma)	cpu_to_le32((u32)dma)
+
+/*
+ * EHCI Specification 0.95 Section 3.5
+ * QTD: describe data transfer components (buffer, direction, ...)
+ * See Fig 3-6 "Queue Element Transfer Descriptor Block Diagram".
+ *
+ * These are associated only with "QH" (Queue Head) structures,
+ * used with control, bulk, and interrupt transfers.
+ */
+struct ehci_qtd {
+	/* first part defined by EHCI spec */
+	__le32			hw_next;		/* see EHCI 3.5.1 */
+	__le32			hw_alt_next;		/* see EHCI 3.5.2 */
+	__le32			hw_token;		/* see EHCI 3.5.3 */
+#define	QTD_TOGGLE	(1 << 31)	/* data toggle */
+#define	QTD_LENGTH(tok)	(((tok)>>16) & 0x7fff)
+#define	QTD_IOC		(1 << 15)	/* interrupt on complete */
+#define	QTD_CERR(tok)	(((tok)>>10) & 0x3)
+#define	QTD_PID(tok)	(((tok)>>8) & 0x3)
+#define	QTD_STS_ACTIVE	(1 << 7)	/* HC may execute this */
+#define	QTD_STS_HALT	(1 << 6)	/* halted on error */
+#define	QTD_STS_DBE	(1 << 5)	/* data buffer error (in HC) */
+#define	QTD_STS_BABBLE	(1 << 4)	/* device was babbling (qtd halted) */
+#define	QTD_STS_XACT	(1 << 3)	/* device gave illegal response */
+#define	QTD_STS_MMF	(1 << 2)	/* incomplete split transaction */
+#define	QTD_STS_STS	(1 << 1)	/* split transaction state */
+#define	QTD_STS_PING	(1 << 0)	/* issue PING? */
+	__le32			hw_buf[5];		/* see EHCI 3.5.4 */
+	__le32			hw_buf_hi[5];		/* Appendix B */
+
+	/* the rest is HCD-private */
+	dma_addr_t		qtd_dma;		/* qtd address */
+	struct list_head	qtd_list;		/* sw qtd list */
+	struct urb		*urb;			/* qtd's urb */
+	size_t			length;			/* length of buffer */
+
+	u32			qtd_buffer_len;
+	void			*buffer;
+	dma_addr_t		buffer_dma;
+	void			*transfer_buffer;
+	void			*transfer_dma;
+} __attribute__ ((aligned(32)));
+
+/* mask NakCnt+T in qh->hw_alt_next */
+#define QTD_MASK __constant_cpu_to_le32 (~0x1f)
+
+#define IS_SHORT_READ(token) (QTD_LENGTH(token) != 0 && QTD_PID(token) == 1)
+
+/* Type tag from {qh, itd, sitd, fstn}->hw_next */
+#define Q_NEXT_TYPE(dma) ((dma) & __constant_cpu_to_le32 (3 << 1))
+
+/* values for that type tag */
+#define Q_TYPE_QH	__constant_cpu_to_le32 (1 << 1)
+
+/* next async queue entry, or pointer to interrupt/periodic QH */
+#define	QH_NEXT(dma)	(cpu_to_le32(((u32)dma)&~0x01f)|Q_TYPE_QH)
+
+/* for periodic/async schedules and qtd lists, mark end of list */
+#define	EHCI_LIST_END	__constant_cpu_to_le32(1) /* "null pointer" to hw */
+
+/*
+ * Entries in periodic shadow table are pointers to one of four kinds
+ * of data structure.  That's dictated by the hardware; a type tag is
+ * encoded in the low bits of the hardware's periodic schedule.  Use
+ * Q_NEXT_TYPE to get the tag.
+ *
+ * For entries in the async schedule, the type tag always says "qh".
+ */
+union ehci_shadow {
+	struct ehci_qh		*qh;		/* Q_TYPE_QH */
+	__le32			*hw_next;	/* (all types) */
+	void			*ptr;
+};
+
+/*
+ * EHCI Specification 0.95 Section 3.6
+ * QH: describes control/bulk/interrupt endpoints
+ * See Fig 3-7 "Queue Head Structure Layout".
+ *
+ * These appear in both the async and (for interrupt) periodic schedules.
+ */
+
+struct ehci_qh {
+	/* first part defined by EHCI spec */
+	__le32			hw_next;	 /* see EHCI 3.6.1 */
+	__le32			hw_info1;	/* see EHCI 3.6.2 */
+#define	QH_HEAD		0x00008000
+	__le32			hw_info2;	/* see EHCI 3.6.2 */
+#define	QH_SMASK	0x000000ff
+#define	QH_CMASK	0x0000ff00
+#define	QH_HUBADDR	0x007f0000
+#define	QH_HUBPORT	0x3f800000
+#define	QH_MULT		0xc0000000
+	__le32			hw_current;	 /* qtd list - see EHCI 3.6.4 */
+
+	/* qtd overlay (hardware parts of a struct ehci_qtd) */
+	__le32			hw_qtd_next;
+	__le32			hw_alt_next;
+	__le32			hw_token;
+	__le32			hw_buf[5];
+	__le32			hw_buf_hi[5];
+
+	/* the rest is HCD-private */
+	dma_addr_t		qh_dma;		/* address of qh */
+	union ehci_shadow	qh_next;	/* ptr to qh; or periodic */
+	struct list_head	qtd_list;	/* sw qtd list */
+	struct ehci_qtd		*dummy;
+	struct ehci_qh		*reclaim;	/* next to reclaim */
+
+	struct oxu_hcd		*oxu;
+	struct kref		kref;
+	unsigned		stamp;
+
+	u8			qh_state;
+#define	QH_STATE_LINKED		1		/* HC sees this */
+#define	QH_STATE_UNLINK		2		/* HC may still see this */
+#define	QH_STATE_IDLE		3		/* HC doesn't see this */
+#define	QH_STATE_UNLINK_WAIT	4		/* LINKED and on reclaim q */
+#define	QH_STATE_COMPLETING	5		/* don't touch token.HALT */
+
+	/* periodic schedule info */
+	u8			usecs;		/* intr bandwidth */
+	u8			gap_uf;		/* uframes split/csplit gap */
+	u8			c_usecs;	/* ... split completion bw */
+	u16			tt_usecs;	/* tt downstream bandwidth */
+	unsigned short		period;		/* polling interval */
+	unsigned short		start;		/* where polling starts */
+#define NO_FRAME ((unsigned short)~0)			/* pick new start */
+	struct usb_device	*dev;		/* access to TT */
+} __attribute__ ((aligned(32)));
+
+/*
+ * Proper OXU210HP structs
+ */
+
+#define OXU_OTG_CORE_OFFSET	0x00400
+#define OXU_OTG_CAP_OFFSET	(OXU_OTG_CORE_OFFSET + 0x100)
+#define OXU_SPH_CORE_OFFSET	0x00800
+#define OXU_SPH_CAP_OFFSET	(OXU_SPH_CORE_OFFSET + 0x100)
+
+#define OXU_OTG_MEM		0xE000
+#define OXU_SPH_MEM		0x16000
+
+/* Only how many elements & element structure are specifies here. */
+/* 2 host controllers are enabled - total size <= 28 kbytes */
+#define	DEFAULT_I_TDPS		1024
+#define QHEAD_NUM		16
+#define QTD_NUM			32
+#define SITD_NUM		8
+#define MURB_NUM		8
+
+#define BUFFER_NUM		8
+#define BUFFER_SIZE		512
+
+struct oxu_info {
+	struct usb_hcd *hcd[2];
+};
+
+struct oxu_buf {
+	u8			buffer[BUFFER_SIZE];
+} __attribute__ ((aligned(BUFFER_SIZE)));
+
+struct oxu_onchip_mem {
+	struct oxu_buf		db_pool[BUFFER_NUM];
+
+	u32			frame_list[DEFAULT_I_TDPS];
+	struct ehci_qh		qh_pool[QHEAD_NUM];
+	struct ehci_qtd		qtd_pool[QTD_NUM];
+} __attribute__ ((aligned(4 << 10)));
+
+#define	EHCI_MAX_ROOT_PORTS	15		/* see HCS_N_PORTS */
+
+struct oxu_murb {
+	struct urb		urb;
+	struct urb		*main;
+	u8			last;
+};
+
+struct oxu_hcd {				/* one per controller */
+	unsigned int		is_otg:1;
+
+	u8			qh_used[QHEAD_NUM];
+	u8			qtd_used[QTD_NUM];
+	u8			db_used[BUFFER_NUM];
+	u8			murb_used[MURB_NUM];
+
+	struct oxu_onchip_mem	__iomem *mem;
+	spinlock_t		mem_lock;
+
+	struct timer_list	urb_timer;
+
+	struct ehci_caps __iomem *caps;
+	struct ehci_regs __iomem *regs;
+
+	__u32			hcs_params;	/* cached register copy */
+	spinlock_t		lock;
+
+	/* async schedule support */
+	struct ehci_qh		*async;
+	struct ehci_qh		*reclaim;
+	unsigned		reclaim_ready:1;
+	unsigned		scanning:1;
+
+	/* periodic schedule support */
+	unsigned		periodic_size;
+	__le32			*periodic;	/* hw periodic table */
+	dma_addr_t		periodic_dma;
+	unsigned		i_thresh;	/* uframes HC might cache */
+
+	union ehci_shadow	*pshadow;	/* mirror hw periodic table */
+	int			next_uframe;	/* scan periodic, start here */
+	unsigned		periodic_sched;	/* periodic activity count */
+
+	/* per root hub port */
+	unsigned long		reset_done[EHCI_MAX_ROOT_PORTS];
+	/* bit vectors (one bit per port) */
+	unsigned long		bus_suspended;	/* which ports were
+						 * already suspended at the
+						 * start of a bus suspend
+						 */
+	unsigned long		companion_ports;/* which ports are dedicated
+						 * to the companion controller
+						 */
+
+	struct timer_list	watchdog;
+	unsigned long		actions;
+	unsigned		stamp;
+	unsigned long		next_statechange;
+	u32			command;
+
+	/* SILICON QUIRKS */
+	struct list_head	urb_list;	/* this is the head to urb
+						 * queue that didn't get enough
+						 * resources
+						 */
+	struct oxu_murb		*murb_pool;	/* murb per split big urb */
+	unsigned urb_len;
+
+	u8			sbrn;		/* packed release number */
+};
+
+#define EHCI_IAA_JIFFIES	(HZ/100)	/* arbitrary; ~10 msec */
+#define EHCI_IO_JIFFIES	 	(HZ/10)		/* io watchdog > irq_thresh */
+#define EHCI_ASYNC_JIFFIES      (HZ/20)		/* async idle timeout */
+#define EHCI_SHRINK_JIFFIES     (HZ/200)	/* async qh unlink delay */
+
+enum ehci_timer_action {
+	TIMER_IO_WATCHDOG,
+	TIMER_IAA_WATCHDOG,
+	TIMER_ASYNC_SHRINK,
+	TIMER_ASYNC_OFF,
+};
+
+#include <linux/oxu210hp.h>
diff --git a/drivers/usb/host/pci-quirks.c b/drivers/usb/host/pci-quirks.c
index ae6e70edd745..75b69847918e 100644
--- a/drivers/usb/host/pci-quirks.c
+++ b/drivers/usb/host/pci-quirks.c
@@ -172,9 +172,9 @@ static void __devinit quirk_usb_handoff_ohci(struct pci_dev *pdev)
 	if (!mmio_resource_enabled(pdev, 0))
 		return;
 
-	base = ioremap_nocache(pci_resource_start(pdev, 0),
-				     pci_resource_len(pdev, 0));
-	if (base == NULL) return;
+	base = pci_ioremap_bar(pdev, 0);
+	if (base == NULL)
+		return;
 
 /* On PA-RISC, PDC can leave IR set incorrectly; ignore it there. */
 #ifndef __hppa__
@@ -221,9 +221,9 @@ static void __devinit quirk_usb_disable_ehci(struct pci_dev *pdev)
 	if (!mmio_resource_enabled(pdev, 0))
 		return;
 
-	base = ioremap_nocache(pci_resource_start(pdev, 0),
-				pci_resource_len(pdev, 0));
-	if (base == NULL) return;
+	base = pci_ioremap_bar(pdev, 0);
+	if (base == NULL)
+		return;
 
 	cap_length = readb(base);
 	op_reg_base = base + cap_length;
@@ -271,7 +271,7 @@ static void __devinit quirk_usb_disable_ehci(struct pci_dev *pdev)
 			/* if boot firmware now owns EHCI, spin till
 			 * it hands it over.
 			 */
-			msec = 5000;
+			msec = 1000;
 			while ((cap & EHCI_USBLEGSUP_BIOS) && (msec > 0)) {
 				tried_handoff = 1;
 				msleep(10);
diff --git a/drivers/usb/host/r8a66597-hcd.c b/drivers/usb/host/r8a66597-hcd.c
index c21f14e0666a..319041205b57 100644
--- a/drivers/usb/host/r8a66597-hcd.c
+++ b/drivers/usb/host/r8a66597-hcd.c
@@ -2275,7 +2275,6 @@ static int __init_or_module r8a66597_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#define resource_len(r) (((r)->end - (r)->start) + 1)
 static int __init r8a66597_probe(struct platform_device *pdev)
 {
 #if defined(CONFIG_SUPERH_ON_CHIP_R8A66597) && defined(CONFIG_HAVE_CLK)
@@ -2296,11 +2295,10 @@ static int __init r8a66597_probe(struct platform_device *pdev)
 		goto clean_up;
 	}
 
-	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
-					   (char *)hcd_name);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		ret = -ENODEV;
-		dev_err(&pdev->dev, "platform_get_resource_byname error.\n");
+		dev_err(&pdev->dev, "platform_get_resource error.\n");
 		goto clean_up;
 	}
 
@@ -2315,7 +2313,7 @@ static int __init r8a66597_probe(struct platform_device *pdev)
 	irq = ires->start;
 	irq_trigger = ires->flags & IRQF_TRIGGER_MASK;
 
-	reg = ioremap(res->start, resource_len(res));
+	reg = ioremap(res->start, resource_size(res));
 	if (reg == NULL) {
 		ret = -ENOMEM;
 		dev_err(&pdev->dev, "ioremap error.\n");
diff --git a/drivers/usb/host/uhci-hcd.c b/drivers/usb/host/uhci-hcd.c
index cf5e4cf7ea42..4e221060f58c 100644
--- a/drivers/usb/host/uhci-hcd.c
+++ b/drivers/usb/host/uhci-hcd.c
@@ -942,6 +942,8 @@ static struct pci_driver uhci_pci_driver = {
 
 #ifdef	CONFIG_PM
 	.suspend =	usb_hcd_pci_suspend,
+	.suspend_late =	usb_hcd_pci_suspend_late,
+	.resume_early =	usb_hcd_pci_resume_early,
 	.resume =	usb_hcd_pci_resume,
 #endif	/* PM */
 };
diff --git a/drivers/usb/image/microtek.c b/drivers/usb/image/microtek.c
index 885867a86de8..4541dfcea88f 100644
--- a/drivers/usb/image/microtek.c
+++ b/drivers/usb/image/microtek.c
@@ -350,17 +350,16 @@ static int mts_scsi_abort(struct scsi_cmnd *srb)
 static int mts_scsi_host_reset(struct scsi_cmnd *srb)
 {
 	struct mts_desc* desc = (struct mts_desc*)(srb->device->host->hostdata[0]);
-	int result, rc;
+	int result;
 
 	MTS_DEBUG_GOT_HERE();
 	mts_debug_dump(desc);
 
-	rc = usb_lock_device_for_reset(desc->usb_dev, desc->usb_intf);
-	if (rc < 0)
-		return FAILED;
-	result = usb_reset_device(desc->usb_dev);
-	if (rc)
+	result = usb_lock_device_for_reset(desc->usb_dev, desc->usb_intf);
+	if (result == 0) {
+		result = usb_reset_device(desc->usb_dev);
 		usb_unlock_device(desc->usb_dev);
+	}
 	return result ? FAILED : SUCCESS;
 }
 
diff --git a/drivers/usb/misc/berry_charge.c b/drivers/usb/misc/berry_charge.c
index 24e2dc3148a4..c05a85bc5925 100644
--- a/drivers/usb/misc/berry_charge.c
+++ b/drivers/usb/misc/berry_charge.c
@@ -123,6 +123,11 @@ static int berry_probe(struct usb_interface *intf,
 {
 	struct usb_device *udev = interface_to_usbdev(intf);
 
+	if (udev->bus_mA < 500) {
+		dbg(&udev->dev, "Not enough power to charge available\n");
+		return -ENODEV;
+	}
+
 	dbg(&udev->dev, "Power is set to %dmA\n",
 	    udev->actconfig->desc.bMaxPower * 2);
 
diff --git a/drivers/usb/misc/emi26.c b/drivers/usb/misc/emi26.c
index e762beb5f3c6..879a980ca8c4 100644
--- a/drivers/usb/misc/emi26.c
+++ b/drivers/usb/misc/emi26.c
@@ -160,7 +160,7 @@ static int emi26_load_firmware (struct usb_device *dev)
 			err("%s - error loading firmware: error = %d", __func__, err);
 			goto wraperr;
 		}
-	} while (i > 0);
+	} while (rec);
 
 	/* Assert reset (stop the CPU in the EMI) */
 	err = emi26_set_reset(dev,1);
diff --git a/drivers/usb/misc/usbtest.c b/drivers/usb/misc/usbtest.c
index 444c69c447be..5f1a19d1497d 100644
--- a/drivers/usb/misc/usbtest.c
+++ b/drivers/usb/misc/usbtest.c
@@ -192,8 +192,6 @@ static struct urb *simple_alloc_urb (
 {
 	struct urb		*urb;
 
-	if (bytes < 0)
-		return NULL;
 	urb = usb_alloc_urb (0, GFP_KERNEL);
 	if (!urb)
 		return urb;
diff --git a/drivers/usb/mon/Kconfig b/drivers/usb/mon/Kconfig
index deb9ddffa402..f28f350cd96a 100644
--- a/drivers/usb/mon/Kconfig
+++ b/drivers/usb/mon/Kconfig
@@ -3,14 +3,13 @@
 #
 
 config USB_MON
-	bool "USB Monitor"
-	depends on USB!=n
-	default y
+	tristate "USB Monitor"
+	depends on USB
+	default y if USB=y
+	default m if USB=m
 	help
-	  If you say Y here, a component which captures the USB traffic
+	  If you select this option, a component which captures the USB traffic
 	  between peripheral-specific drivers and HC drivers will be built.
 	  For more information, see <file:Documentation/usb/usbmon.txt>.
 
-	  This is somewhat experimental at this time, but it should be safe.
-
-	  If unsure, say Y.
+	  If unsure, say Y (if allowed), otherwise M.
diff --git a/drivers/usb/mon/Makefile b/drivers/usb/mon/Makefile
index 0f76ed5e1617..c6516b566731 100644
--- a/drivers/usb/mon/Makefile
+++ b/drivers/usb/mon/Makefile
@@ -4,5 +4,4 @@
 
 usbmon-objs	:= mon_main.o mon_stat.o mon_text.o mon_bin.o mon_dma.o
 
-# This does not use CONFIG_USB_MON because we want this to use a tristate.
-obj-$(CONFIG_USB)	+= usbmon.o
+obj-$(CONFIG_USB_MON)	+= usbmon.o
diff --git a/drivers/usb/musb/Kconfig b/drivers/usb/musb/Kconfig
index 4b9542bbb35c..5af7379cd9a3 100644
--- a/drivers/usb/musb/Kconfig
+++ b/drivers/usb/musb/Kconfig
@@ -11,7 +11,7 @@ config USB_MUSB_HDRC
 	depends on (USB || USB_GADGET) && HAVE_CLK
 	depends on !SUPERH
 	select TWL4030_USB if MACH_OMAP_3430SDP
-	tristate 'Inventra Highspeed Dual Role Controller (TI, ...)'
+	tristate 'Inventra Highspeed Dual Role Controller (TI, ADI, ...)'
 	help
 	  Say Y here if your system has a dual role high speed USB
 	  controller based on the Mentor Graphics silicon IP.  Then
@@ -22,6 +22,9 @@ config USB_MUSB_HDRC
 	  Texas Instruments parts using this IP include DaVinci 644x,
 	  OMAP 243x, OMAP 343x, and TUSB 6010.
 
+	  Analog Devices parts using this IP include Blackfin BF54x,
+	  BF525 and BF527.
+
 	  If you do not know what this is, please say N.
 
 	  To compile this driver as a module, choose M here; the
@@ -33,6 +36,8 @@ config USB_MUSB_SOC
 	default y if ARCH_DAVINCI
 	default y if ARCH_OMAP2430
 	default y if ARCH_OMAP34XX
+	default y if (BF54x && !BF544)
+	default y if (BF52x && !BF522 && !BF523)
 
 comment "DaVinci 644x USB support"
 	depends on USB_MUSB_HDRC && ARCH_DAVINCI
@@ -43,6 +48,9 @@ comment "OMAP 243x high speed USB support"
 comment "OMAP 343x high speed USB support"
 	depends on USB_MUSB_HDRC && ARCH_OMAP34XX
 
+comment "Blackfin high speed USB Support"
+	depends on USB_MUSB_HDRC && (BF54x && !BF544) || (BF52x && !BF522 && !BF523)
+
 config USB_TUSB6010
 	boolean "TUSB 6010 support"
 	depends on USB_MUSB_HDRC && !USB_MUSB_SOC
@@ -142,7 +150,7 @@ config MUSB_PIO_ONLY
 config USB_INVENTRA_DMA
 	bool
 	depends on USB_MUSB_HDRC && !MUSB_PIO_ONLY
-	default ARCH_OMAP2430 || ARCH_OMAP34XX
+	default ARCH_OMAP2430 || ARCH_OMAP34XX || BLACKFIN
 	help
 	  Enable DMA transfers using Mentor's engine.
 
diff --git a/drivers/usb/musb/Makefile b/drivers/usb/musb/Makefile
index b6af0d687a73..85710ccc1887 100644
--- a/drivers/usb/musb/Makefile
+++ b/drivers/usb/musb/Makefile
@@ -22,6 +22,14 @@ ifeq ($(CONFIG_ARCH_OMAP3430),y)
 	musb_hdrc-objs	+= omap2430.o
 endif
 
+ifeq ($(CONFIG_BF54x),y)
+	musb_hdrc-objs	+= blackfin.o
+endif
+
+ifeq ($(CONFIG_BF52x),y)
+	musb_hdrc-objs	+= blackfin.o
+endif
+
 ifeq ($(CONFIG_USB_GADGET_MUSB_HDRC),y)
 	musb_hdrc-objs		+= musb_gadget_ep0.o musb_gadget.o
 endif
diff --git a/drivers/usb/musb/blackfin.c b/drivers/usb/musb/blackfin.c
new file mode 100644
index 000000000000..786134852092
--- /dev/null
+++ b/drivers/usb/musb/blackfin.c
@@ -0,0 +1,320 @@
+/*
+ * MUSB OTG controller driver for Blackfin Processors
+ *
+ * Copyright 2006-2008 Analog Devices Inc.
+ *
+ * Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/clk.h>
+#include <linux/gpio.h>
+#include <linux/io.h>
+
+#include <asm/cacheflush.h>
+
+#include "musb_core.h"
+#include "blackfin.h"
+
+/*
+ * Load an endpoint's FIFO
+ */
+void musb_write_fifo(struct musb_hw_ep *hw_ep, u16 len, const u8 *src)
+{
+	void __iomem *fifo = hw_ep->fifo;
+	void __iomem *epio = hw_ep->regs;
+
+	prefetch((u8 *)src);
+
+	musb_writew(epio, MUSB_TXCOUNT, len);
+
+	DBG(4, "TX ep%d fifo %p count %d buf %p, epio %p\n",
+			hw_ep->epnum, fifo, len, src, epio);
+
+	dump_fifo_data(src, len);
+
+	if (unlikely((unsigned long)src & 0x01))
+		outsw_8((unsigned long)fifo, src,
+			len & 0x01 ? (len >> 1) + 1 : len >> 1);
+	else
+		outsw((unsigned long)fifo, src,
+			len & 0x01 ? (len >> 1) + 1 : len >> 1);
+}
+
+/*
+ * Unload an endpoint's FIFO
+ */
+void musb_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *dst)
+{
+	void __iomem *fifo = hw_ep->fifo;
+	u8 epnum = hw_ep->epnum;
+	u16 dma_reg = 0;
+
+	DBG(4, "%cX ep%d fifo %p count %d buf %p\n",
+			'R', hw_ep->epnum, fifo, len, dst);
+
+#ifdef CONFIG_BF52x
+	invalidate_dcache_range((unsigned int)dst,
+		(unsigned int)(dst + len));
+
+	/* Setup DMA address register */
+	dma_reg = (u16) ((u32) dst & 0xFFFF);
+	bfin_write16(USB_DMA_REG(epnum, USB_DMAx_ADDR_LOW), dma_reg);
+	SSYNC();
+
+	dma_reg = (u16) (((u32) dst >> 16) & 0xFFFF);
+	bfin_write16(USB_DMA_REG(epnum, USB_DMAx_ADDR_HIGH), dma_reg);
+	SSYNC();
+
+	/* Setup DMA count register */
+	bfin_write16(USB_DMA_REG(epnum, USB_DMAx_COUNT_LOW), len);
+	bfin_write16(USB_DMA_REG(epnum, USB_DMAx_COUNT_HIGH), 0);
+	SSYNC();
+
+	/* Enable the DMA */
+	dma_reg = (epnum << 4) | DMA_ENA | INT_ENA;
+	bfin_write16(USB_DMA_REG(epnum, USB_DMAx_CTRL), dma_reg);
+	SSYNC();
+
+	/* Wait for compelete */
+	while (!(bfin_read_USB_DMA_INTERRUPT() & (1 << epnum)))
+		cpu_relax();
+
+	/* acknowledge dma interrupt */
+	bfin_write_USB_DMA_INTERRUPT(1 << epnum);
+	SSYNC();
+
+	/* Reset DMA */
+	bfin_write16(USB_DMA_REG(epnum, USB_DMAx_CTRL), 0);
+	SSYNC();
+#else
+	if (unlikely((unsigned long)dst & 0x01))
+		insw_8((unsigned long)fifo, dst,
+			len & 0x01 ? (len >> 1) + 1 : len >> 1);
+	else
+		insw((unsigned long)fifo, dst,
+			len & 0x01 ? (len >> 1) + 1 : len >> 1);
+#endif
+
+	dump_fifo_data(dst, len);
+}
+
+static irqreturn_t blackfin_interrupt(int irq, void *__hci)
+{
+	unsigned long	flags;
+	irqreturn_t	retval = IRQ_NONE;
+	struct musb	*musb = __hci;
+
+	spin_lock_irqsave(&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);
+
+	if (musb->int_usb || musb->int_tx || musb->int_rx) {
+		musb_writeb(musb->mregs, MUSB_INTRUSB, musb->int_usb);
+		musb_writew(musb->mregs, MUSB_INTRTX, musb->int_tx);
+		musb_writew(musb->mregs, MUSB_INTRRX, musb->int_rx);
+		retval = musb_interrupt(musb);
+	}
+
+	spin_unlock_irqrestore(&musb->lock, flags);
+
+	/* REVISIT we sometimes get spurious IRQs on g_ep0
+	 * not clear why... fall in BF54x too.
+	 */
+	if (retval != IRQ_HANDLED)
+		DBG(5, "spurious?\n");
+
+	return IRQ_HANDLED;
+}
+
+static void musb_conn_timer_handler(unsigned long _musb)
+{
+	struct musb *musb = (void *)_musb;
+	unsigned long flags;
+	u16 val;
+
+	spin_lock_irqsave(&musb->lock, flags);
+	switch (musb->xceiv.state) {
+	case OTG_STATE_A_IDLE:
+	case OTG_STATE_A_WAIT_BCON:
+		/* Start a new session */
+		val = musb_readw(musb->mregs, MUSB_DEVCTL);
+		val |= MUSB_DEVCTL_SESSION;
+		musb_writew(musb->mregs, MUSB_DEVCTL, val);
+
+		val = musb_readw(musb->mregs, MUSB_DEVCTL);
+		if (!(val & MUSB_DEVCTL_BDEVICE)) {
+			gpio_set_value(musb->config->gpio_vrsel, 1);
+			musb->xceiv.state = OTG_STATE_A_WAIT_BCON;
+		} else {
+			gpio_set_value(musb->config->gpio_vrsel, 0);
+
+			/* Ignore VBUSERROR and SUSPEND IRQ */
+			val = musb_readb(musb->mregs, MUSB_INTRUSBE);
+			val &= ~MUSB_INTR_VBUSERROR;
+			musb_writeb(musb->mregs, MUSB_INTRUSBE, val);
+
+			val = MUSB_INTR_SUSPEND | MUSB_INTR_VBUSERROR;
+			musb_writeb(musb->mregs, MUSB_INTRUSB, val);
+
+			val = MUSB_POWER_HSENAB;
+			musb_writeb(musb->mregs, MUSB_POWER, val);
+		}
+		mod_timer(&musb_conn_timer, jiffies + TIMER_DELAY);
+		break;
+
+	default:
+		DBG(1, "%s state not handled\n", otg_state_string(musb));
+		break;
+	}
+	spin_unlock_irqrestore(&musb->lock, flags);
+
+	DBG(4, "state is %s\n", otg_state_string(musb));
+}
+
+void musb_platform_enable(struct musb *musb)
+{
+	if (is_host_enabled(musb)) {
+		mod_timer(&musb_conn_timer, jiffies + TIMER_DELAY);
+		musb->a_wait_bcon = TIMER_DELAY;
+	}
+}
+
+void musb_platform_disable(struct musb *musb)
+{
+}
+
+static void bfin_vbus_power(struct musb *musb, int is_on, int sleeping)
+{
+}
+
+static void bfin_set_vbus(struct musb *musb, int is_on)
+{
+	if (is_on)
+		gpio_set_value(musb->config->gpio_vrsel, 1);
+	else
+		gpio_set_value(musb->config->gpio_vrsel, 0);
+
+	DBG(1, "VBUS %s, devctl %02x "
+		/* otg %3x conf %08x prcm %08x */ "\n",
+		otg_state_string(musb),
+		musb_readb(musb->mregs, MUSB_DEVCTL));
+}
+
+static int bfin_set_power(struct otg_transceiver *x, unsigned mA)
+{
+	return 0;
+}
+
+void musb_platform_try_idle(struct musb *musb, unsigned long timeout)
+{
+	if (is_host_enabled(musb))
+		mod_timer(&musb_conn_timer, jiffies + TIMER_DELAY);
+}
+
+int musb_platform_get_vbus_status(struct musb *musb)
+{
+	return 0;
+}
+
+void musb_platform_set_mode(struct musb *musb, u8 musb_mode)
+{
+}
+
+int __init musb_platform_init(struct musb *musb)
+{
+
+	/*
+	 * Rev 1.0 BF549 EZ-KITs require PE7 to be high for both DEVICE
+	 * and OTG HOST modes, while rev 1.1 and greater require PE7 to
+	 * be low for DEVICE mode and high for HOST mode. We set it high
+	 * here because we are in host mode
+	 */
+
+	if (gpio_request(musb->config->gpio_vrsel, "USB_VRSEL")) {
+		printk(KERN_ERR "Failed ro request USB_VRSEL GPIO_%d \n",
+			musb->config->gpio_vrsel);
+		return -ENODEV;
+	}
+	gpio_direction_output(musb->config->gpio_vrsel, 0);
+
+	if (ANOMALY_05000346) {
+		bfin_write_USB_APHY_CALIB(ANOMALY_05000346_value);
+		SSYNC();
+	}
+
+	if (ANOMALY_05000347) {
+		bfin_write_USB_APHY_CNTRL(0x0);
+		SSYNC();
+	}
+
+	/* TODO
+	 * Set SIC-IVG register
+	 */
+
+	/* Configure PLL oscillator register */
+	bfin_write_USB_PLLOSC_CTRL(0x30a8);
+	SSYNC();
+
+	bfin_write_USB_SRP_CLKDIV((get_sclk()/1000) / 32 - 1);
+	SSYNC();
+
+	bfin_write_USB_EP_NI0_RXMAXP(64);
+	SSYNC();
+
+	bfin_write_USB_EP_NI0_TXMAXP(64);
+	SSYNC();
+
+	/* Route INTRUSB/INTR_RX/INTR_TX to USB_INT0*/
+	bfin_write_USB_GLOBINTR(0x7);
+	SSYNC();
+
+	bfin_write_USB_GLOBAL_CTL(GLOBAL_ENA | EP1_TX_ENA | EP2_TX_ENA |
+				EP3_TX_ENA | EP4_TX_ENA | EP5_TX_ENA |
+				EP6_TX_ENA | EP7_TX_ENA | EP1_RX_ENA |
+				EP2_RX_ENA | EP3_RX_ENA | EP4_RX_ENA |
+				EP5_RX_ENA | EP6_RX_ENA | EP7_RX_ENA);
+	SSYNC();
+
+	if (is_host_enabled(musb)) {
+		musb->board_set_vbus = bfin_set_vbus;
+		setup_timer(&musb_conn_timer,
+			musb_conn_timer_handler, (unsigned long) musb);
+	}
+	if (is_peripheral_enabled(musb))
+		musb->xceiv.set_power = bfin_set_power;
+
+	musb->isr = blackfin_interrupt;
+
+	return 0;
+}
+
+int musb_platform_suspend(struct musb *musb)
+{
+	return 0;
+}
+
+int musb_platform_resume(struct musb *musb)
+{
+	return 0;
+}
+
+
+int musb_platform_exit(struct musb *musb)
+{
+
+	bfin_vbus_power(musb, 0 /*off*/, 1);
+	gpio_free(musb->config->gpio_vrsel);
+	musb_platform_suspend(musb);
+
+	return 0;
+}
diff --git a/drivers/usb/musb/blackfin.h b/drivers/usb/musb/blackfin.h
new file mode 100644
index 000000000000..a240c1e53d16
--- /dev/null
+++ b/drivers/usb/musb/blackfin.h
@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2007 by Analog Devices, Inc.
+ *
+ * The Inventra Controller Driver for Linux 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 __MUSB_BLACKFIN_H__
+#define __MUSB_BLACKFIN_H__
+
+/*
+ * Blackfin specific definitions
+ */
+
+#undef DUMP_FIFO_DATA
+#ifdef DUMP_FIFO_DATA
+static void dump_fifo_data(u8 *buf, u16 len)
+{
+	u8 *tmp = buf;
+	int i;
+
+	for (i = 0; i < len; i++) {
+		if (!(i % 16) && i)
+			pr_debug("\n");
+		pr_debug("%02x ", *tmp++);
+	}
+	pr_debug("\n");
+}
+#else
+#define dump_fifo_data(buf, len)	do {} while (0)
+#endif
+
+#ifdef CONFIG_BF52x
+
+#define USB_DMA_BASE		USB_DMA_INTERRUPT
+#define USB_DMAx_CTRL		0x04
+#define USB_DMAx_ADDR_LOW	0x08
+#define USB_DMAx_ADDR_HIGH	0x0C
+#define USB_DMAx_COUNT_LOW	0x10
+#define USB_DMAx_COUNT_HIGH	0x14
+
+#define USB_DMA_REG(ep, reg)	(USB_DMA_BASE + 0x20 * ep + reg)
+#endif
+
+/* Almost 1 second */
+#define TIMER_DELAY	(1 * HZ)
+
+static struct timer_list musb_conn_timer;
+
+#endif	/* __MUSB_BLACKFIN_H__ */
diff --git a/drivers/usb/musb/davinci.c b/drivers/usb/musb/davinci.c
index dfb3bcbe00fc..0d566dc5ce06 100644
--- a/drivers/usb/musb/davinci.c
+++ b/drivers/usb/musb/davinci.c
@@ -32,9 +32,9 @@
 #include <linux/io.h>
 #include <linux/gpio.h>
 
-#include <asm/arch/hardware.h>
-#include <asm/arch/memory.h>
-#include <asm/arch/gpio.h>
+#include <mach/arch/hardware.h>
+#include <mach/arch/memory.h>
+#include <mach/arch/gpio.h>
 #include <asm/mach-types.h>
 
 #include "musb_core.h"
@@ -364,6 +364,18 @@ static irqreturn_t davinci_interrupt(int irq, void *__hci)
 	return IRQ_HANDLED;
 }
 
+int musb_platform_set_mode(struct musb *musb, u8 mode)
+{
+	/* EVM can't do this (right?) */
+	return -EIO;
+}
+
+int musb_platform_set_mode(struct musb *musb, u8 mode)
+{
+       /* EVM can't do this (right?) */
+       return -EIO;
+}
+
 int __init musb_platform_init(struct musb *musb)
 {
 	void __iomem	*tibase = musb->ctrl_base;
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index 5280dba9b1fb..6c7faacfb535 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -148,7 +148,8 @@ static inline struct musb *dev_to_musb(struct device *dev)
 
 /*-------------------------------------------------------------------------*/
 
-#ifndef CONFIG_USB_TUSB6010
+#if !defined(CONFIG_USB_TUSB6010) && !defined(CONFIG_BLACKFIN)
+
 /*
  * Load an endpoint's FIFO
  */
@@ -1124,25 +1125,25 @@ fifo_setup(struct musb *musb, struct musb_hw_ep  *hw_ep,
 #endif
 	switch (cfg->style) {
 	case FIFO_TX:
-		musb_writeb(mbase, MUSB_TXFIFOSZ, c_size);
-		musb_writew(mbase, MUSB_TXFIFOADD, c_off);
+		musb_write_txfifosz(mbase, c_size);
+		musb_write_txfifoadd(mbase, c_off);
 		hw_ep->tx_double_buffered = !!(c_size & MUSB_FIFOSZ_DPB);
 		hw_ep->max_packet_sz_tx = maxpacket;
 		break;
 	case FIFO_RX:
-		musb_writeb(mbase, MUSB_RXFIFOSZ, c_size);
-		musb_writew(mbase, MUSB_RXFIFOADD, c_off);
+		musb_write_rxfifosz(mbase, c_size);
+		musb_write_rxfifoadd(mbase, c_off);
 		hw_ep->rx_double_buffered = !!(c_size & MUSB_FIFOSZ_DPB);
 		hw_ep->max_packet_sz_rx = maxpacket;
 		break;
 	case FIFO_RXTX:
-		musb_writeb(mbase, MUSB_TXFIFOSZ, c_size);
-		musb_writew(mbase, MUSB_TXFIFOADD, c_off);
+		musb_write_txfifosz(mbase, c_size);
+		musb_write_txfifoadd(mbase, c_off);
 		hw_ep->rx_double_buffered = !!(c_size & MUSB_FIFOSZ_DPB);
 		hw_ep->max_packet_sz_rx = maxpacket;
 
-		musb_writeb(mbase, MUSB_RXFIFOSZ, c_size);
-		musb_writew(mbase, MUSB_RXFIFOADD, c_off);
+		musb_write_rxfifosz(mbase, c_size);
+		musb_write_rxfifoadd(mbase, c_off);
 		hw_ep->tx_double_buffered = hw_ep->rx_double_buffered;
 		hw_ep->max_packet_sz_tx = maxpacket;
 
@@ -1212,7 +1213,7 @@ static int __init ep_config_from_table(struct musb *musb)
 		if (epn >= musb->config->num_eps) {
 			pr_debug("%s: invalid ep %d\n",
 					musb_driver_name, epn);
-			continue;
+			return -EINVAL;
 		}
 		offset = fifo_setup(musb, hw_ep + epn, cfg++, offset);
 		if (offset < 0) {
@@ -1246,9 +1247,10 @@ static int __init ep_config_from_table(struct musb *musb)
  */
 static int __init ep_config_from_hw(struct musb *musb)
 {
-	u8 epnum = 0, reg;
+	u8 epnum = 0;
 	struct musb_hw_ep *hw_ep;
 	void *mbase = musb->mregs;
+	int ret = 0;
 
 	DBG(2, "<== static silicon ep config\n");
 
@@ -1258,26 +1260,9 @@ static int __init ep_config_from_hw(struct musb *musb)
 		musb_ep_select(mbase, epnum);
 		hw_ep = musb->endpoints + epnum;
 
-		/* read from core using indexed model */
-		reg = musb_readb(hw_ep->regs, 0x10 + MUSB_FIFOSIZE);
-		if (!reg) {
-			/* 0's returned when no more endpoints */
+		ret = musb_read_fifosize(musb, hw_ep, epnum);
+		if (ret < 0)
 			break;
-		}
-		musb->nr_endpoints++;
-		musb->epmask |= (1 << epnum);
-
-		hw_ep->max_packet_sz_tx = 1 << (reg & 0x0f);
-
-		/* shared TX/RX FIFO? */
-		if ((reg & 0xf0) == 0xf0) {
-			hw_ep->max_packet_sz_rx = hw_ep->max_packet_sz_tx;
-			hw_ep->is_shared_fifo = true;
-			continue;
-		} else {
-			hw_ep->max_packet_sz_rx = 1 << ((reg & 0xf0) >> 4);
-			hw_ep->is_shared_fifo = false;
-		}
 
 		/* FIXME set up hw_ep->{rx,tx}_double_buffered */
 
@@ -1326,7 +1311,7 @@ static int __init musb_core_init(u16 musb_type, struct musb *musb)
 
 	/* log core options (read using indexed model) */
 	musb_ep_select(mbase, 0);
-	reg = musb_readb(mbase, 0x10 + MUSB_CONFIGDATA);
+	reg = musb_read_configdata(mbase);
 
 	strcpy(aInfo, (reg & MUSB_CONFIGDATA_UTMIDW) ? "UTMI-16" : "UTMI-8");
 	if (reg & MUSB_CONFIGDATA_DYNFIFO)
@@ -1391,7 +1376,7 @@ static int __init musb_core_init(u16 musb_type, struct musb *musb)
 	}
 
 	/* log release info */
-	hwvers = musb_readw(mbase, MUSB_HWVERS);
+	hwvers = musb_read_hwvers(mbase);
 	rev_major = (hwvers >> 10) & 0x1f;
 	rev_minor = hwvers & 0x3ff;
 	snprintf(aRevision, 32, "%d.%d%s", rev_major,
@@ -1400,8 +1385,7 @@ static int __init musb_core_init(u16 musb_type, struct musb *musb)
 			musb_driver_name, type, aRevision, aDate);
 
 	/* configure ep0 */
-	musb->endpoints[0].max_packet_sz_tx = MUSB_EP0_FIFOSIZE;
-	musb->endpoints[0].max_packet_sz_rx = MUSB_EP0_FIFOSIZE;
+	musb_configure_ep0(musb);
 
 	/* discover endpoint configuration */
 	musb->nr_endpoints = 1;
@@ -1445,7 +1429,7 @@ static int __init musb_core_init(u16 musb_type, struct musb *musb)
 
 		hw_ep->regs = MUSB_EP_OFFSET(i, 0) + mbase;
 #ifdef CONFIG_USB_MUSB_HDRC_HCD
-		hw_ep->target_regs = MUSB_BUSCTL_OFFSET(i, 0) + mbase;
+		hw_ep->target_regs = musb_read_target_reg_base(i, mbase);
 		hw_ep->rx_reinit = 1;
 		hw_ep->tx_reinit = 1;
 #endif
@@ -1671,17 +1655,20 @@ musb_mode_store(struct device *dev, struct device_attribute *attr,
 {
 	struct musb	*musb = dev_to_musb(dev);
 	unsigned long	flags;
+	int		status;
 
 	spin_lock_irqsave(&musb->lock, flags);
-	if (!strncmp(buf, "host", 4))
-		musb_platform_set_mode(musb, MUSB_HOST);
-	if (!strncmp(buf, "peripheral", 10))
-		musb_platform_set_mode(musb, MUSB_PERIPHERAL);
-	if (!strncmp(buf, "otg", 3))
-		musb_platform_set_mode(musb, MUSB_OTG);
+	if (sysfs_streq(buf, "host"))
+		status = musb_platform_set_mode(musb, MUSB_HOST);
+	else if (sysfs_streq(buf, "peripheral"))
+		status = musb_platform_set_mode(musb, MUSB_PERIPHERAL);
+	else if (sysfs_streq(buf, "otg"))
+		status = musb_platform_set_mode(musb, MUSB_OTG);
+	else
+		status = -EINVAL;
 	spin_unlock_irqrestore(&musb->lock, flags);
 
-	return n;
+	return (status == 0) ? n : status;
 }
 static DEVICE_ATTR(mode, 0644, musb_mode_show, musb_mode_store);
 
@@ -1781,7 +1768,7 @@ allocate_instance(struct device *dev,
 #ifdef CONFIG_USB_MUSB_HDRC_HCD
 	struct usb_hcd	*hcd;
 
-	hcd = usb_create_hcd(&musb_hc_driver, dev, dev->bus_id);
+	hcd = usb_create_hcd(&musb_hc_driver, dev, dev_name(dev));
 	if (!hcd)
 		return NULL;
 	/* usbcore sets dev->driver_data to hcd, and sometimes uses that... */
@@ -1810,7 +1797,6 @@ allocate_instance(struct device *dev,
 	for (epnum = 0, ep = musb->endpoints;
 			epnum < musb->config->num_eps;
 			epnum++, ep++) {
-
 		ep->musb = musb;
 		ep->epnum = epnum;
 	}
@@ -1838,7 +1824,7 @@ static void musb_free(struct musb *musb)
 	musb_gadget_cleanup(musb);
 #endif
 
-	if (musb->nIrq >= 0) {
+	if (musb->nIrq >= 0 && musb->irq_wake) {
 		disable_irq_wake(musb->nIrq);
 		free_irq(musb->nIrq, musb);
 	}
@@ -1984,15 +1970,19 @@ bad_config:
 	INIT_WORK(&musb->irq_work, musb_irq_work);
 
 	/* attach to the IRQ */
-	if (request_irq(nIrq, musb->isr, 0, dev->bus_id, musb)) {
+	if (request_irq(nIrq, musb->isr, 0, dev_name(dev), musb)) {
 		dev_err(dev, "request_irq %d failed!\n", nIrq);
 		status = -ENODEV;
 		goto fail2;
 	}
 	musb->nIrq = nIrq;
 /* FIXME this handles wakeup irqs wrong */
-	if (enable_irq_wake(nIrq) == 0)
+	if (enable_irq_wake(nIrq) == 0) {
+		musb->irq_wake = 1;
 		device_init_wakeup(dev, 1);
+	} else {
+		musb->irq_wake = 0;
+	}
 
 	pr_info("%s: USB %s mode controller at %p using %s, IRQ %d\n",
 			musb_driver_name,
diff --git a/drivers/usb/musb/musb_core.h b/drivers/usb/musb/musb_core.h
index 82227251931b..630946a2d9fc 100644
--- a/drivers/usb/musb/musb_core.h
+++ b/drivers/usb/musb/musb_core.h
@@ -191,7 +191,7 @@ enum musb_g_ep0_state {
  */
 
 #if defined(CONFIG_ARCH_DAVINCI) || defined(CONFIG_ARCH_OMAP2430) \
-		|| defined(CONFIG_ARCH_OMAP3430)
+		|| defined(CONFIG_ARCH_OMAP3430) || defined(CONFIG_BLACKFIN)
 /* REVISIT indexed access seemed to
  * misbehave (on DaVinci) for at least peripheral IN ...
  */
@@ -359,6 +359,7 @@ struct musb {
 	struct otg_transceiver	xceiv;
 
 	int nIrq;
+	unsigned		irq_wake:1;
 
 	struct musb_hw_ep	 endpoints[MUSB_C_NUM_EPS];
 #define control_ep		endpoints
@@ -447,6 +448,70 @@ static inline struct musb *gadget_to_musb(struct usb_gadget *g)
 }
 #endif
 
+#ifdef CONFIG_BLACKFIN
+static inline int musb_read_fifosize(struct musb *musb,
+		struct musb_hw_ep *hw_ep, u8 epnum)
+{
+	musb->nr_endpoints++;
+	musb->epmask |= (1 << epnum);
+
+	if (epnum < 5) {
+		hw_ep->max_packet_sz_tx = 128;
+		hw_ep->max_packet_sz_rx = 128;
+	} else {
+		hw_ep->max_packet_sz_tx = 1024;
+		hw_ep->max_packet_sz_rx = 1024;
+	}
+	hw_ep->is_shared_fifo = false;
+
+	return 0;
+}
+
+static inline void musb_configure_ep0(struct musb *musb)
+{
+	musb->endpoints[0].max_packet_sz_tx = MUSB_EP0_FIFOSIZE;
+	musb->endpoints[0].max_packet_sz_rx = MUSB_EP0_FIFOSIZE;
+	musb->endpoints[0].is_shared_fifo = true;
+}
+
+#else
+
+static inline int musb_read_fifosize(struct musb *musb,
+		struct musb_hw_ep *hw_ep, u8 epnum)
+{
+	u8 reg = 0;
+
+	/* read from core using indexed model */
+	reg = musb_readb(hw_ep->regs, 0x10 + MUSB_FIFOSIZE);
+	/* 0's returned when no more endpoints */
+	if (!reg)
+		return -ENODEV;
+
+	musb->nr_endpoints++;
+	musb->epmask |= (1 << epnum);
+
+	hw_ep->max_packet_sz_tx = 1 << (reg & 0x0f);
+
+	/* shared TX/RX FIFO? */
+	if ((reg & 0xf0) == 0xf0) {
+		hw_ep->max_packet_sz_rx = hw_ep->max_packet_sz_tx;
+		hw_ep->is_shared_fifo = true;
+		return 0;
+	} else {
+		hw_ep->max_packet_sz_rx = 1 << ((reg & 0xf0) >> 4);
+		hw_ep->is_shared_fifo = false;
+	}
+
+	return 0;
+}
+
+static inline void musb_configure_ep0(struct musb *musb)
+{
+	musb->endpoints[0].max_packet_sz_tx = MUSB_EP0_FIFOSIZE;
+	musb->endpoints[0].max_packet_sz_rx = MUSB_EP0_FIFOSIZE;
+}
+#endif /* CONFIG_BLACKFIN */
+
 
 /***************************** Glue it together *****************************/
 
@@ -467,16 +532,16 @@ extern void musb_platform_disable(struct musb *musb);
 
 extern void musb_hnp_stop(struct musb *musb);
 
-extern void musb_platform_set_mode(struct musb *musb, u8 musb_mode);
+extern int musb_platform_set_mode(struct musb *musb, u8 musb_mode);
 
-#if defined(CONFIG_USB_TUSB6010) || \
+#if defined(CONFIG_USB_TUSB6010) || defined(CONFIG_BLACKFIN) || \
 	defined(CONFIG_ARCH_OMAP2430) || defined(CONFIG_ARCH_OMAP34XX)
 extern void musb_platform_try_idle(struct musb *musb, unsigned long timeout);
 #else
 #define musb_platform_try_idle(x, y)		do {} while (0)
 #endif
 
-#ifdef CONFIG_USB_TUSB6010
+#if defined(CONFIG_USB_TUSB6010) || defined(CONFIG_BLACKFIN)
 extern int musb_platform_get_vbus_status(struct musb *musb);
 #else
 #define musb_platform_get_vbus_status(x)	0
diff --git a/drivers/usb/musb/musb_gadget.c b/drivers/usb/musb/musb_gadget.c
index d6a802c224fa..6197daeab8f9 100644
--- a/drivers/usb/musb/musb_gadget.c
+++ b/drivers/usb/musb/musb_gadget.c
@@ -1633,7 +1633,7 @@ int __init musb_gadget_setup(struct musb *musb)
 	musb->g.speed = USB_SPEED_UNKNOWN;
 
 	/* this "gadget" abstracts/virtualizes the controller */
-	strcpy(musb->g.dev.bus_id, "gadget");
+	dev_set_name(&musb->g.dev, "gadget");
 	musb->g.dev.parent = musb->controller;
 	musb->g.dev.dma_mask = musb->controller->dma_mask;
 	musb->g.dev.release = musb_gadget_release;
diff --git a/drivers/usb/musb/musb_host.c b/drivers/usb/musb/musb_host.c
index cc64462d4c4e..99fa61234876 100644
--- a/drivers/usb/musb/musb_host.c
+++ b/drivers/usb/musb/musb_host.c
@@ -112,18 +112,21 @@ static void musb_h_tx_flush_fifo(struct musb_hw_ep *ep)
 {
 	void __iomem	*epio = ep->regs;
 	u16		csr;
+	u16		lastcsr = 0;
 	int		retries = 1000;
 
 	csr = musb_readw(epio, MUSB_TXCSR);
 	while (csr & MUSB_TXCSR_FIFONOTEMPTY) {
-		DBG(5, "Host TX FIFONOTEMPTY csr: %02x\n", csr);
+		if (csr != lastcsr)
+			DBG(3, "Host TX FIFONOTEMPTY csr: %02x\n", csr);
+		lastcsr = csr;
 		csr |= MUSB_TXCSR_FLUSHFIFO;
 		musb_writew(epio, MUSB_TXCSR, csr);
 		csr = musb_readw(epio, MUSB_TXCSR);
-		if (retries-- < 1) {
-			ERR("Could not flush host TX fifo: csr: %04x\n", csr);
+		if (WARN(retries-- < 1,
+				"Could not flush host TX%d fifo: csr: %04x\n",
+				ep->epnum, csr))
 			return;
-		}
 		mdelay(1);
 	}
 }
@@ -268,7 +271,7 @@ __musb_giveback(struct musb *musb, struct urb *urb, int status)
 __releases(musb->lock)
 __acquires(musb->lock)
 {
-	DBG(({ int level; switch (urb->status) {
+	DBG(({ int level; switch (status) {
 				case 0:
 					level = 4;
 					break;
@@ -283,8 +286,8 @@ __acquires(musb->lock)
 					level = 2;
 					break;
 				}; level; }),
-			"complete %p (%d), dev%d ep%d%s, %d/%d\n",
-			urb, urb->status,
+			"complete %p %pF (%d), dev%d ep%d%s, %d/%d\n",
+			urb, urb->complete, status,
 			usb_pipedevice(urb->pipe),
 			usb_pipeendpoint(urb->pipe),
 			usb_pipein(urb->pipe) ? "in" : "out",
@@ -593,12 +596,10 @@ musb_rx_reinit(struct musb *musb, struct musb_qh *qh, struct musb_hw_ep *ep)
 
 	/* target addr and (for multipoint) hub addr/port */
 	if (musb->is_multipoint) {
-		musb_writeb(ep->target_regs, MUSB_RXFUNCADDR,
-			qh->addr_reg);
-		musb_writeb(ep->target_regs, MUSB_RXHUBADDR,
-			qh->h_addr_reg);
-		musb_writeb(ep->target_regs, MUSB_RXHUBPORT,
-			qh->h_port_reg);
+		musb_write_rxfunaddr(ep->target_regs, qh->addr_reg);
+		musb_write_rxhubaddr(ep->target_regs, qh->h_addr_reg);
+		musb_write_rxhubport(ep->target_regs, qh->h_port_reg);
+
 	} else
 		musb_writeb(musb->mregs, MUSB_FADDR, qh->addr_reg);
 
@@ -712,15 +713,9 @@ static void musb_ep_program(struct musb *musb, u8 epnum,
 
 		/* target addr and (for multipoint) hub addr/port */
 		if (musb->is_multipoint) {
-			musb_writeb(mbase,
-				MUSB_BUSCTL_OFFSET(epnum, MUSB_TXFUNCADDR),
-				qh->addr_reg);
-			musb_writeb(mbase,
-				MUSB_BUSCTL_OFFSET(epnum, MUSB_TXHUBADDR),
-				qh->h_addr_reg);
-			musb_writeb(mbase,
-				MUSB_BUSCTL_OFFSET(epnum, MUSB_TXHUBPORT),
-				qh->h_port_reg);
+			musb_write_txfunaddr(mbase, epnum, qh->addr_reg);
+			musb_write_txhubaddr(mbase, epnum, qh->h_addr_reg);
+			musb_write_txhubport(mbase, epnum, qh->h_port_reg);
 /* FIXME if !epnum, do the same for RX ... */
 		} else
 			musb_writeb(mbase, MUSB_FADDR, qh->addr_reg);
@@ -988,8 +983,10 @@ static bool musb_h_ep0_continue(struct musb *musb, u16 len, struct urb *urb)
 		if (fifo_count) {
 			fifo_dest = (u8 *) (urb->transfer_buffer
 					+ urb->actual_length);
-			DBG(3, "Sending %d bytes to %p\n",
-					fifo_count, fifo_dest);
+			DBG(3, "Sending %d byte%s to ep0 fifo %p\n",
+					fifo_count,
+					(fifo_count == 1) ? "" : "s",
+					fifo_dest);
 			musb_write_fifo(hw_ep, fifo_count, fifo_dest);
 
 			urb->actual_length += fifo_count;
diff --git a/drivers/usb/musb/musb_io.h b/drivers/usb/musb/musb_io.h
index 223f0a514094..b06e9ef00cfc 100644
--- a/drivers/usb/musb/musb_io.h
+++ b/drivers/usb/musb/musb_io.h
@@ -39,7 +39,7 @@
 
 #if !defined(CONFIG_ARM) && !defined(CONFIG_SUPERH) \
 	&& !defined(CONFIG_AVR32) && !defined(CONFIG_PPC32) \
-	&& !defined(CONFIG_PPC64)
+	&& !defined(CONFIG_PPC64) && !defined(CONFIG_BLACKFIN)
 static inline void readsl(const void __iomem *addr, void *buf, int len)
 	{ insl((unsigned long)addr, buf, len); }
 static inline void readsw(const void __iomem *addr, void *buf, int len)
@@ -56,6 +56,8 @@ static inline void writesb(const void __iomem *addr, const void *buf, int len)
 
 #endif
 
+#ifndef CONFIG_BLACKFIN
+
 /* NOTE:  these offsets are all in bytes */
 
 static inline u16 musb_readw(const void __iomem *addr, unsigned offset)
@@ -114,4 +116,26 @@ static inline void musb_writeb(void __iomem *addr, unsigned offset, u8 data)
 
 #endif	/* CONFIG_USB_TUSB6010 */
 
+#else
+
+static inline u8 musb_readb(const void __iomem *addr, unsigned offset)
+	{ return (u8) (bfin_read16(addr + offset)); }
+
+static inline u16 musb_readw(const void __iomem *addr, unsigned offset)
+	{ return bfin_read16(addr + offset); }
+
+static inline u32 musb_readl(const void __iomem *addr, unsigned offset)
+	{ return (u32) (bfin_read16(addr + offset)); }
+
+static inline void musb_writeb(void __iomem *addr, unsigned offset, u8 data)
+	{ bfin_write16(addr + offset, (u16) data); }
+
+static inline void musb_writew(void __iomem *addr, unsigned offset, u16 data)
+	{ bfin_write16(addr + offset, data); }
+
+static inline void musb_writel(void __iomem *addr, unsigned offset, u32 data)
+	{ bfin_write16(addr + offset, (u16) data); }
+
+#endif /* CONFIG_BLACKFIN */
+
 #endif
diff --git a/drivers/usb/musb/musb_regs.h b/drivers/usb/musb/musb_regs.h
index 9c228661aa5a..de3b2f18db44 100644
--- a/drivers/usb/musb/musb_regs.h
+++ b/drivers/usb/musb/musb_regs.h
@@ -38,97 +38,6 @@
 #define MUSB_EP0_FIFOSIZE	64	/* This is non-configurable */
 
 /*
- * Common USB registers
- */
-
-#define MUSB_FADDR		0x00	/* 8-bit */
-#define MUSB_POWER		0x01	/* 8-bit */
-
-#define MUSB_INTRTX		0x02	/* 16-bit */
-#define MUSB_INTRRX		0x04
-#define MUSB_INTRTXE		0x06
-#define MUSB_INTRRXE		0x08
-#define MUSB_INTRUSB		0x0A	/* 8 bit */
-#define MUSB_INTRUSBE		0x0B	/* 8 bit */
-#define MUSB_FRAME		0x0C
-#define MUSB_INDEX		0x0E	/* 8 bit */
-#define MUSB_TESTMODE		0x0F	/* 8 bit */
-
-/* Get offset for a given FIFO from musb->mregs */
-#ifdef	CONFIG_USB_TUSB6010
-#define MUSB_FIFO_OFFSET(epnum)	(0x200 + ((epnum) * 0x20))
-#else
-#define MUSB_FIFO_OFFSET(epnum)	(0x20 + ((epnum) * 4))
-#endif
-
-/*
- * Additional Control Registers
- */
-
-#define MUSB_DEVCTL		0x60	/* 8 bit */
-
-/* These are always controlled through the INDEX register */
-#define MUSB_TXFIFOSZ		0x62	/* 8-bit (see masks) */
-#define MUSB_RXFIFOSZ		0x63	/* 8-bit (see masks) */
-#define MUSB_TXFIFOADD		0x64	/* 16-bit offset shifted right 3 */
-#define MUSB_RXFIFOADD		0x66	/* 16-bit offset shifted right 3 */
-
-/* REVISIT: vctrl/vstatus: optional vendor utmi+phy register at 0x68 */
-#define MUSB_HWVERS		0x6C	/* 8 bit */
-
-#define MUSB_EPINFO		0x78	/* 8 bit */
-#define MUSB_RAMINFO		0x79	/* 8 bit */
-#define MUSB_LINKINFO		0x7a	/* 8 bit */
-#define MUSB_VPLEN		0x7b	/* 8 bit */
-#define MUSB_HS_EOF1		0x7c	/* 8 bit */
-#define MUSB_FS_EOF1		0x7d	/* 8 bit */
-#define MUSB_LS_EOF1		0x7e	/* 8 bit */
-
-/* Offsets to endpoint registers */
-#define MUSB_TXMAXP		0x00
-#define MUSB_TXCSR		0x02
-#define MUSB_CSR0		MUSB_TXCSR	/* Re-used for EP0 */
-#define MUSB_RXMAXP		0x04
-#define MUSB_RXCSR		0x06
-#define MUSB_RXCOUNT		0x08
-#define MUSB_COUNT0		MUSB_RXCOUNT	/* Re-used for EP0 */
-#define MUSB_TXTYPE		0x0A
-#define MUSB_TYPE0		MUSB_TXTYPE	/* Re-used for EP0 */
-#define MUSB_TXINTERVAL		0x0B
-#define MUSB_NAKLIMIT0		MUSB_TXINTERVAL	/* Re-used for EP0 */
-#define MUSB_RXTYPE		0x0C
-#define MUSB_RXINTERVAL		0x0D
-#define MUSB_FIFOSIZE		0x0F
-#define MUSB_CONFIGDATA		MUSB_FIFOSIZE	/* Re-used for EP0 */
-
-/* Offsets to endpoint registers in indexed model (using INDEX register) */
-#define MUSB_INDEXED_OFFSET(_epnum, _offset)	\
-	(0x10 + (_offset))
-
-/* Offsets to endpoint registers in flat models */
-#define MUSB_FLAT_OFFSET(_epnum, _offset)	\
-	(0x100 + (0x10*(_epnum)) + (_offset))
-
-#ifdef CONFIG_USB_TUSB6010
-/* TUSB6010 EP0 configuration register is special */
-#define MUSB_TUSB_OFFSET(_epnum, _offset)	\
-	(0x10 + _offset)
-#include "tusb6010.h"		/* Needed "only" for TUSB_EP0_CONF */
-#endif
-
-/* "bus control"/target registers, for host side multipoint (external hubs) */
-#define MUSB_TXFUNCADDR		0x00
-#define MUSB_TXHUBADDR		0x02
-#define MUSB_TXHUBPORT		0x03
-
-#define MUSB_RXFUNCADDR		0x04
-#define MUSB_RXHUBADDR		0x06
-#define MUSB_RXHUBPORT		0x07
-
-#define MUSB_BUSCTL_OFFSET(_epnum, _offset) \
-	(0x80 + (8*(_epnum)) + (_offset))
-
-/*
  * MUSB Register bits
  */
 
@@ -228,7 +137,6 @@
 
 /* TXCSR in Peripheral and Host mode */
 #define MUSB_TXCSR_AUTOSET		0x8000
-#define MUSB_TXCSR_MODE			0x2000
 #define MUSB_TXCSR_DMAENAB		0x1000
 #define MUSB_TXCSR_FRCDATATOG		0x0800
 #define MUSB_TXCSR_DMAMODE		0x0400
@@ -297,4 +205,309 @@
 /* HUBADDR */
 #define MUSB_HUBADDR_MULTI_TT		0x80
 
+
+#ifndef CONFIG_BLACKFIN
+
+/*
+ * Common USB registers
+ */
+
+#define MUSB_FADDR		0x00	/* 8-bit */
+#define MUSB_POWER		0x01	/* 8-bit */
+
+#define MUSB_INTRTX		0x02	/* 16-bit */
+#define MUSB_INTRRX		0x04
+#define MUSB_INTRTXE		0x06
+#define MUSB_INTRRXE		0x08
+#define MUSB_INTRUSB		0x0A	/* 8 bit */
+#define MUSB_INTRUSBE		0x0B	/* 8 bit */
+#define MUSB_FRAME		0x0C
+#define MUSB_INDEX		0x0E	/* 8 bit */
+#define MUSB_TESTMODE		0x0F	/* 8 bit */
+
+/* Get offset for a given FIFO from musb->mregs */
+#ifdef	CONFIG_USB_TUSB6010
+#define MUSB_FIFO_OFFSET(epnum)	(0x200 + ((epnum) * 0x20))
+#else
+#define MUSB_FIFO_OFFSET(epnum)	(0x20 + ((epnum) * 4))
+#endif
+
+/*
+ * Additional Control Registers
+ */
+
+#define MUSB_DEVCTL		0x60	/* 8 bit */
+
+/* These are always controlled through the INDEX register */
+#define MUSB_TXFIFOSZ		0x62	/* 8-bit (see masks) */
+#define MUSB_RXFIFOSZ		0x63	/* 8-bit (see masks) */
+#define MUSB_TXFIFOADD		0x64	/* 16-bit offset shifted right 3 */
+#define MUSB_RXFIFOADD		0x66	/* 16-bit offset shifted right 3 */
+
+/* REVISIT: vctrl/vstatus: optional vendor utmi+phy register at 0x68 */
+#define MUSB_HWVERS		0x6C	/* 8 bit */
+
+#define MUSB_EPINFO		0x78	/* 8 bit */
+#define MUSB_RAMINFO		0x79	/* 8 bit */
+#define MUSB_LINKINFO		0x7a	/* 8 bit */
+#define MUSB_VPLEN		0x7b	/* 8 bit */
+#define MUSB_HS_EOF1		0x7c	/* 8 bit */
+#define MUSB_FS_EOF1		0x7d	/* 8 bit */
+#define MUSB_LS_EOF1		0x7e	/* 8 bit */
+
+/* Offsets to endpoint registers */
+#define MUSB_TXMAXP		0x00
+#define MUSB_TXCSR		0x02
+#define MUSB_CSR0		MUSB_TXCSR	/* Re-used for EP0 */
+#define MUSB_RXMAXP		0x04
+#define MUSB_RXCSR		0x06
+#define MUSB_RXCOUNT		0x08
+#define MUSB_COUNT0		MUSB_RXCOUNT	/* Re-used for EP0 */
+#define MUSB_TXTYPE		0x0A
+#define MUSB_TYPE0		MUSB_TXTYPE	/* Re-used for EP0 */
+#define MUSB_TXINTERVAL		0x0B
+#define MUSB_NAKLIMIT0		MUSB_TXINTERVAL	/* Re-used for EP0 */
+#define MUSB_RXTYPE		0x0C
+#define MUSB_RXINTERVAL		0x0D
+#define MUSB_FIFOSIZE		0x0F
+#define MUSB_CONFIGDATA		MUSB_FIFOSIZE	/* Re-used for EP0 */
+
+/* Offsets to endpoint registers in indexed model (using INDEX register) */
+#define MUSB_INDEXED_OFFSET(_epnum, _offset)	\
+	(0x10 + (_offset))
+
+/* Offsets to endpoint registers in flat models */
+#define MUSB_FLAT_OFFSET(_epnum, _offset)	\
+	(0x100 + (0x10*(_epnum)) + (_offset))
+
+#ifdef CONFIG_USB_TUSB6010
+/* TUSB6010 EP0 configuration register is special */
+#define MUSB_TUSB_OFFSET(_epnum, _offset)	\
+	(0x10 + _offset)
+#include "tusb6010.h"		/* Needed "only" for TUSB_EP0_CONF */
+#endif
+
+#define MUSB_TXCSR_MODE			0x2000
+
+/* "bus control"/target registers, for host side multipoint (external hubs) */
+#define MUSB_TXFUNCADDR		0x00
+#define MUSB_TXHUBADDR		0x02
+#define MUSB_TXHUBPORT		0x03
+
+#define MUSB_RXFUNCADDR		0x04
+#define MUSB_RXHUBADDR		0x06
+#define MUSB_RXHUBPORT		0x07
+
+#define MUSB_BUSCTL_OFFSET(_epnum, _offset) \
+	(0x80 + (8*(_epnum)) + (_offset))
+
+static inline void musb_write_txfifosz(void __iomem *mbase, u8 c_size)
+{
+	musb_writeb(mbase, MUSB_TXFIFOSZ, c_size);
+}
+
+static inline void musb_write_txfifoadd(void __iomem *mbase, u16 c_off)
+{
+	musb_writew(mbase, MUSB_TXFIFOADD, c_off);
+}
+
+static inline void musb_write_rxfifosz(void __iomem *mbase, u8 c_size)
+{
+	musb_writeb(mbase, MUSB_RXFIFOSZ, c_size);
+}
+
+static inline void  musb_write_rxfifoadd(void __iomem *mbase, u16 c_off)
+{
+	musb_writew(mbase, MUSB_RXFIFOADD, c_off);
+}
+
+static inline u8 musb_read_configdata(void __iomem *mbase)
+{
+	return musb_readb(mbase, 0x10 + MUSB_CONFIGDATA);
+}
+
+static inline u16 musb_read_hwvers(void __iomem *mbase)
+{
+	return musb_readw(mbase, MUSB_HWVERS);
+}
+
+static inline void __iomem *musb_read_target_reg_base(u8 i, void __iomem *mbase)
+{
+	return (MUSB_BUSCTL_OFFSET(i, 0) + mbase);
+}
+
+static inline void musb_write_rxfunaddr(void __iomem *ep_target_regs,
+		u8 qh_addr_reg)
+{
+	musb_writeb(ep_target_regs, MUSB_RXFUNCADDR, qh_addr_reg);
+}
+
+static inline void musb_write_rxhubaddr(void __iomem *ep_target_regs,
+		u8 qh_h_addr_reg)
+{
+	musb_writeb(ep_target_regs, MUSB_RXHUBADDR, qh_h_addr_reg);
+}
+
+static inline void musb_write_rxhubport(void __iomem *ep_target_regs,
+		u8 qh_h_port_reg)
+{
+	musb_writeb(ep_target_regs, MUSB_RXHUBPORT, qh_h_port_reg);
+}
+
+static inline void  musb_write_txfunaddr(void __iomem *mbase, u8 epnum,
+		u8 qh_addr_reg)
+{
+	musb_writeb(mbase, MUSB_BUSCTL_OFFSET(epnum, MUSB_TXFUNCADDR),
+			qh_addr_reg);
+}
+
+static inline void  musb_write_txhubaddr(void __iomem *mbase, u8 epnum,
+		u8 qh_addr_reg)
+{
+	musb_writeb(mbase, MUSB_BUSCTL_OFFSET(epnum, MUSB_TXHUBADDR),
+			qh_addr_reg);
+}
+
+static inline void  musb_write_txhubport(void __iomem *mbase, u8 epnum,
+		u8 qh_h_port_reg)
+{
+	musb_writeb(mbase, MUSB_BUSCTL_OFFSET(epnum, MUSB_TXHUBPORT),
+			qh_h_port_reg);
+}
+
+#else /* CONFIG_BLACKFIN */
+
+#define USB_BASE		USB_FADDR
+#define USB_OFFSET(reg)		(reg - USB_BASE)
+
+/*
+ * Common USB registers
+ */
+#define MUSB_FADDR		USB_OFFSET(USB_FADDR)	/* 8-bit */
+#define MUSB_POWER		USB_OFFSET(USB_POWER)	/* 8-bit */
+#define MUSB_INTRTX		USB_OFFSET(USB_INTRTX)	/* 16-bit */
+#define MUSB_INTRRX		USB_OFFSET(USB_INTRRX)
+#define MUSB_INTRTXE		USB_OFFSET(USB_INTRTXE)
+#define MUSB_INTRRXE		USB_OFFSET(USB_INTRRXE)
+#define MUSB_INTRUSB		USB_OFFSET(USB_INTRUSB)	/* 8 bit */
+#define MUSB_INTRUSBE		USB_OFFSET(USB_INTRUSBE)/* 8 bit */
+#define MUSB_FRAME		USB_OFFSET(USB_FRAME)
+#define MUSB_INDEX		USB_OFFSET(USB_INDEX)	/* 8 bit */
+#define MUSB_TESTMODE		USB_OFFSET(USB_TESTMODE)/* 8 bit */
+
+/* Get offset for a given FIFO from musb->mregs */
+#define MUSB_FIFO_OFFSET(epnum)	\
+	(USB_OFFSET(USB_EP0_FIFO) + ((epnum) * 8))
+
+/*
+ * Additional Control Registers
+ */
+
+#define MUSB_DEVCTL		USB_OFFSET(USB_OTG_DEV_CTL)	/* 8 bit */
+
+#define MUSB_LINKINFO		USB_OFFSET(USB_LINKINFO)/* 8 bit */
+#define MUSB_VPLEN		USB_OFFSET(USB_VPLEN)	/* 8 bit */
+#define MUSB_HS_EOF1		USB_OFFSET(USB_HS_EOF1)	/* 8 bit */
+#define MUSB_FS_EOF1		USB_OFFSET(USB_FS_EOF1)	/* 8 bit */
+#define MUSB_LS_EOF1		USB_OFFSET(USB_LS_EOF1)	/* 8 bit */
+
+/* Offsets to endpoint registers */
+#define MUSB_TXMAXP		0x00
+#define MUSB_TXCSR		0x04
+#define MUSB_CSR0		MUSB_TXCSR	/* Re-used for EP0 */
+#define MUSB_RXMAXP		0x08
+#define MUSB_RXCSR		0x0C
+#define MUSB_RXCOUNT		0x10
+#define MUSB_COUNT0		MUSB_RXCOUNT	/* Re-used for EP0 */
+#define MUSB_TXTYPE		0x14
+#define MUSB_TYPE0		MUSB_TXTYPE	/* Re-used for EP0 */
+#define MUSB_TXINTERVAL		0x18
+#define MUSB_NAKLIMIT0		MUSB_TXINTERVAL	/* Re-used for EP0 */
+#define MUSB_RXTYPE		0x1C
+#define MUSB_RXINTERVAL		0x20
+#define MUSB_TXCOUNT		0x28
+
+/* Offsets to endpoint registers in indexed model (using INDEX register) */
+#define MUSB_INDEXED_OFFSET(_epnum, _offset)	\
+	(0x40 + (_offset))
+
+/* Offsets to endpoint registers in flat models */
+#define MUSB_FLAT_OFFSET(_epnum, _offset)	\
+	(USB_OFFSET(USB_EP_NI0_TXMAXP) + (0x40 * (_epnum)) + (_offset))
+
+/* Not implemented - HW has seperate Tx/Rx FIFO */
+#define MUSB_TXCSR_MODE			0x0000
+
+/*
+ * Dummy stub for clk framework, it will be removed
+ * until Blackfin supports clk framework
+ */
+#define clk_get(dev, id)	NULL
+#define clk_put(clock)		do {} while (0)
+#define clk_enable(clock)	do {} while (0)
+#define clk_disable(clock)	do {} while (0)
+
+static inline void musb_write_txfifosz(void __iomem *mbase, u8 c_size)
+{
+}
+
+static inline void musb_write_txfifoadd(void __iomem *mbase, u16 c_off)
+{
+}
+
+static inline void musb_write_rxfifosz(void __iomem *mbase, u8 c_size)
+{
+}
+
+static inline void  musb_write_rxfifoadd(void __iomem *mbase, u16 c_off)
+{
+}
+
+static inline u8 musb_read_configdata(void __iomem *mbase)
+{
+	return 0;
+}
+
+static inline u16 musb_read_hwvers(void __iomem *mbase)
+{
+	return 0;
+}
+
+static inline u16 musb_read_target_reg_base(u8 i, void __iomem *mbase)
+{
+	return 0;
+}
+
+static inline void musb_write_rxfunaddr(void __iomem *ep_target_regs,
+		u8 qh_addr_req)
+{
+}
+
+static inline void musb_write_rxhubaddr(void __iomem *ep_target_regs,
+		u8 qh_h_addr_reg)
+{
+}
+
+static inline void musb_write_rxhubport(void __iomem *ep_target_regs,
+		u8 qh_h_port_reg)
+{
+}
+
+static inline void  musb_write_txfunaddr(void __iomem *mbase, u8 epnum,
+		u8 qh_addr_reg)
+{
+}
+
+static inline void  musb_write_txhubaddr(void __iomem *mbase, u8 epnum,
+		u8 qh_addr_reg)
+{
+}
+
+static inline void  musb_write_txhubport(void __iomem *mbase, u8 epnum,
+		u8 qh_h_port_reg)
+{
+}
+
+#endif /* CONFIG_BLACKFIN */
+
 #endif	/* __MUSB_REGS_H__ */
diff --git a/drivers/usb/musb/musbhsdma.c b/drivers/usb/musb/musbhsdma.c
index 8c734ef2c1ed..8662e9e159c3 100644
--- a/drivers/usb/musb/musbhsdma.c
+++ b/drivers/usb/musb/musbhsdma.c
@@ -34,58 +34,7 @@
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
 #include "musb_core.h"
-
-#if defined(CONFIG_ARCH_OMAP2430) || defined(CONFIG_ARCH_OMAP3430)
-#include "omap2430.h"
-#endif
-
-#define MUSB_HSDMA_BASE		0x200
-#define MUSB_HSDMA_INTR		(MUSB_HSDMA_BASE + 0)
-#define MUSB_HSDMA_CONTROL		0x4
-#define MUSB_HSDMA_ADDRESS		0x8
-#define MUSB_HSDMA_COUNT		0xc
-
-#define MUSB_HSDMA_CHANNEL_OFFSET(_bchannel, _offset)		\
-		(MUSB_HSDMA_BASE + (_bchannel << 4) + _offset)
-
-/* control register (16-bit): */
-#define MUSB_HSDMA_ENABLE_SHIFT		0
-#define MUSB_HSDMA_TRANSMIT_SHIFT		1
-#define MUSB_HSDMA_MODE1_SHIFT		2
-#define MUSB_HSDMA_IRQENABLE_SHIFT		3
-#define MUSB_HSDMA_ENDPOINT_SHIFT		4
-#define MUSB_HSDMA_BUSERROR_SHIFT		8
-#define MUSB_HSDMA_BURSTMODE_SHIFT		9
-#define MUSB_HSDMA_BURSTMODE		(3 << MUSB_HSDMA_BURSTMODE_SHIFT)
-#define MUSB_HSDMA_BURSTMODE_UNSPEC	0
-#define MUSB_HSDMA_BURSTMODE_INCR4	1
-#define MUSB_HSDMA_BURSTMODE_INCR8	2
-#define MUSB_HSDMA_BURSTMODE_INCR16	3
-
-#define MUSB_HSDMA_CHANNELS		8
-
-struct musb_dma_controller;
-
-struct musb_dma_channel {
-	struct dma_channel		channel;
-	struct musb_dma_controller	*controller;
-	u32				start_addr;
-	u32				len;
-	u16				max_packet_sz;
-	u8				idx;
-	u8				epnum;
-	u8				transmit;
-};
-
-struct musb_dma_controller {
-	struct dma_controller		controller;
-	struct musb_dma_channel		channel[MUSB_HSDMA_CHANNELS];
-	void				*private_data;
-	void __iomem			*base;
-	u8				channel_count;
-	u8				used_channels;
-	u8				irq;
-};
+#include "musbhsdma.h"
 
 static int dma_controller_start(struct dma_controller *c)
 {
@@ -203,12 +152,8 @@ static void configure_channel(struct dma_channel *channel,
 				: 0);
 
 	/* address/count */
-	musb_writel(mbase,
-		MUSB_HSDMA_CHANNEL_OFFSET(bchannel, MUSB_HSDMA_ADDRESS),
-		dma_addr);
-	musb_writel(mbase,
-		MUSB_HSDMA_CHANNEL_OFFSET(bchannel, MUSB_HSDMA_COUNT),
-		len);
+	musb_write_hsdma_addr(mbase, bchannel, dma_addr);
+	musb_write_hsdma_count(mbase, bchannel, len);
 
 	/* control (this should start things) */
 	musb_writew(mbase,
@@ -279,13 +224,8 @@ static int dma_channel_abort(struct dma_channel *channel)
 		musb_writew(mbase,
 			MUSB_HSDMA_CHANNEL_OFFSET(bchannel, MUSB_HSDMA_CONTROL),
 			0);
-		musb_writel(mbase,
-			MUSB_HSDMA_CHANNEL_OFFSET(bchannel, MUSB_HSDMA_ADDRESS),
-			0);
-		musb_writel(mbase,
-			MUSB_HSDMA_CHANNEL_OFFSET(bchannel, MUSB_HSDMA_COUNT),
-			0);
-
+		musb_write_hsdma_addr(mbase, bchannel, 0);
+		musb_write_hsdma_count(mbase, bchannel, 0);
 		channel->status = MUSB_DMA_STATUS_FREE;
 	}
 
@@ -333,10 +273,8 @@ static irqreturn_t dma_controller_irq(int irq, void *private_data)
 			} else {
 				u8 devctl;
 
-				addr = musb_readl(mbase,
-						MUSB_HSDMA_CHANNEL_OFFSET(
-							bchannel,
-							MUSB_HSDMA_ADDRESS));
+				addr = musb_read_hsdma_addr(mbase,
+						bchannel);
 				channel->actual_len = addr
 					- musb_channel->start_addr;
 
@@ -375,6 +313,12 @@ static irqreturn_t dma_controller_irq(int irq, void *private_data)
 			}
 		}
 	}
+
+#ifdef CONFIG_BLACKFIN
+	/* Clear DMA interrup flags */
+	musb_writeb(mbase, MUSB_HSDMA_INTR, int_hsdma);
+#endif
+
 	retval = IRQ_HANDLED;
 done:
 	spin_unlock_irqrestore(&musb->lock, flags);
@@ -424,7 +368,7 @@ dma_controller_create(struct musb *musb, void __iomem *base)
 	controller->controller.channel_abort = dma_channel_abort;
 
 	if (request_irq(irq, dma_controller_irq, IRQF_DISABLED,
-			musb->controller->bus_id, &controller->controller)) {
+			dev_name(musb->controller), &controller->controller)) {
 		dev_err(dev, "request_irq %d failed!\n", irq);
 		dma_controller_destroy(&controller->controller);
 
diff --git a/drivers/usb/musb/musbhsdma.h b/drivers/usb/musb/musbhsdma.h
new file mode 100644
index 000000000000..1299d92dc83f
--- /dev/null
+++ b/drivers/usb/musb/musbhsdma.h
@@ -0,0 +1,149 @@
+/*
+ * MUSB OTG driver - support for Mentor's DMA controller
+ *
+ * Copyright 2005 Mentor Graphics Corporation
+ * Copyright (C) 2005-2007 by Texas Instruments
+ *
+ * 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
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN
+ * NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#if defined(CONFIG_ARCH_OMAP2430) || defined(CONFIG_ARCH_OMAP3430)
+#include "omap2430.h"
+#endif
+
+#ifndef CONFIG_BLACKFIN
+
+#define MUSB_HSDMA_BASE		0x200
+#define MUSB_HSDMA_INTR		(MUSB_HSDMA_BASE + 0)
+#define MUSB_HSDMA_CONTROL		0x4
+#define MUSB_HSDMA_ADDRESS		0x8
+#define MUSB_HSDMA_COUNT		0xc
+
+#define MUSB_HSDMA_CHANNEL_OFFSET(_bchannel, _offset)		\
+		(MUSB_HSDMA_BASE + (_bchannel << 4) + _offset)
+
+#define musb_read_hsdma_addr(mbase, bchannel)	\
+	musb_readl(mbase,	\
+		   MUSB_HSDMA_CHANNEL_OFFSET(bchannel, MUSB_HSDMA_ADDRESS))
+
+#define musb_write_hsdma_addr(mbase, bchannel, addr) \
+	musb_writel(mbase, \
+		    MUSB_HSDMA_CHANNEL_OFFSET(bchannel, MUSB_HSDMA_ADDRESS), \
+		    addr)
+
+#define musb_write_hsdma_count(mbase, bchannel, len) \
+	musb_writel(mbase, \
+		    MUSB_HSDMA_CHANNEL_OFFSET(bchannel, MUSB_HSDMA_COUNT), \
+		    len)
+#else
+
+#define MUSB_HSDMA_BASE		0x400
+#define MUSB_HSDMA_INTR		(MUSB_HSDMA_BASE + 0)
+#define MUSB_HSDMA_CONTROL		0x04
+#define MUSB_HSDMA_ADDR_LOW		0x08
+#define MUSB_HSDMA_ADDR_HIGH		0x0C
+#define MUSB_HSDMA_COUNT_LOW		0x10
+#define MUSB_HSDMA_COUNT_HIGH		0x14
+
+#define MUSB_HSDMA_CHANNEL_OFFSET(_bchannel, _offset)		\
+		(MUSB_HSDMA_BASE + (_bchannel * 0x20) + _offset)
+
+static inline u32 musb_read_hsdma_addr(void __iomem *mbase, u8 bchannel)
+{
+	u32 addr = musb_readw(mbase,
+		MUSB_HSDMA_CHANNEL_OFFSET(bchannel, MUSB_HSDMA_ADDR_HIGH));
+
+	addr = addr << 16;
+
+	addr |= musb_readw(mbase,
+		MUSB_HSDMA_CHANNEL_OFFSET(bchannel, MUSB_HSDMA_ADDR_LOW));
+
+	return addr;
+}
+
+static inline void musb_write_hsdma_addr(void __iomem *mbase,
+				u8 bchannel, dma_addr_t dma_addr)
+{
+	musb_writew(mbase,
+		MUSB_HSDMA_CHANNEL_OFFSET(bchannel, MUSB_HSDMA_ADDR_LOW),
+		((u16)((u32) dma_addr & 0xFFFF)));
+	musb_writew(mbase,
+		MUSB_HSDMA_CHANNEL_OFFSET(bchannel, MUSB_HSDMA_ADDR_HIGH),
+		((u16)(((u32) dma_addr >> 16) & 0xFFFF)));
+}
+
+static inline void musb_write_hsdma_count(void __iomem *mbase,
+				u8 bchannel, u32 len)
+{
+	musb_writew(mbase,
+		MUSB_HSDMA_CHANNEL_OFFSET(bchannel, MUSB_HSDMA_COUNT_LOW),
+		((u16)((u32) len & 0xFFFF)));
+	musb_writew(mbase,
+		MUSB_HSDMA_CHANNEL_OFFSET(bchannel, MUSB_HSDMA_COUNT_HIGH),
+		((u16)(((u32) len >> 16) & 0xFFFF)));
+}
+
+#endif /* CONFIG_BLACKFIN */
+
+/* control register (16-bit): */
+#define MUSB_HSDMA_ENABLE_SHIFT		0
+#define MUSB_HSDMA_TRANSMIT_SHIFT	1
+#define MUSB_HSDMA_MODE1_SHIFT		2
+#define MUSB_HSDMA_IRQENABLE_SHIFT	3
+#define MUSB_HSDMA_ENDPOINT_SHIFT	4
+#define MUSB_HSDMA_BUSERROR_SHIFT	8
+#define MUSB_HSDMA_BURSTMODE_SHIFT	9
+#define MUSB_HSDMA_BURSTMODE		(3 << MUSB_HSDMA_BURSTMODE_SHIFT)
+#define MUSB_HSDMA_BURSTMODE_UNSPEC	0
+#define MUSB_HSDMA_BURSTMODE_INCR4	1
+#define MUSB_HSDMA_BURSTMODE_INCR8	2
+#define MUSB_HSDMA_BURSTMODE_INCR16	3
+
+#define MUSB_HSDMA_CHANNELS		8
+
+struct musb_dma_controller;
+
+struct musb_dma_channel {
+	struct dma_channel		channel;
+	struct musb_dma_controller	*controller;
+	u32				start_addr;
+	u32				len;
+	u16				max_packet_sz;
+	u8				idx;
+	u8				epnum;
+	u8				transmit;
+};
+
+struct musb_dma_controller {
+	struct dma_controller		controller;
+	struct musb_dma_channel		channel[MUSB_HSDMA_CHANNELS];
+	void				*private_data;
+	void __iomem			*base;
+	u8				channel_count;
+	u8				used_channels;
+	u8				irq;
+};
diff --git a/drivers/usb/musb/omap2430.c b/drivers/usb/musb/omap2430.c
index ce6c162920f7..901dffdf23b1 100644
--- a/drivers/usb/musb/omap2430.c
+++ b/drivers/usb/musb/omap2430.c
@@ -58,10 +58,10 @@ static void musb_do_idle(unsigned long _musb)
 #endif
 	u8	devctl;
 
-	devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
-
 	spin_lock_irqsave(&musb->lock, flags);
 
+	devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
+
 	switch (musb->xceiv.state) {
 	case OTG_STATE_A_WAIT_BCON:
 		devctl &= ~MUSB_DEVCTL_SESSION;
@@ -196,7 +196,7 @@ static int omap_set_power(struct otg_transceiver *x, unsigned mA)
 
 static int musb_platform_resume(struct musb *musb);
 
-void musb_platform_set_mode(struct musb *musb, u8 musb_mode)
+int musb_platform_set_mode(struct musb *musb, u8 musb_mode)
 {
 	u8	devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
 
@@ -204,15 +204,24 @@ void musb_platform_set_mode(struct musb *musb, u8 musb_mode)
 	musb_writeb(musb->mregs, MUSB_DEVCTL, devctl);
 
 	switch (musb_mode) {
+#ifdef CONFIG_USB_MUSB_HDRC_HCD
 	case MUSB_HOST:
 		otg_set_host(&musb->xceiv, musb->xceiv.host);
 		break;
+#endif
+#ifdef CONFIG_USB_GADGET_MUSB_HDRC
 	case MUSB_PERIPHERAL:
 		otg_set_peripheral(&musb->xceiv, musb->xceiv.gadget);
 		break;
+#endif
+#ifdef CONFIG_USB_MUSB_OTG
 	case MUSB_OTG:
 		break;
+#endif
+	default:
+		return -EINVAL;
 	}
+	return 0;
 }
 
 int __init musb_platform_init(struct musb *musb)
diff --git a/drivers/usb/musb/tusb6010.c b/drivers/usb/musb/tusb6010.c
index ee8fca92a4ac..9e20fd070d71 100644
--- a/drivers/usb/musb/tusb6010.c
+++ b/drivers/usb/musb/tusb6010.c
@@ -598,7 +598,7 @@ static void tusb_source_power(struct musb *musb, int is_on)
  * and peripheral modes in non-OTG configurations by reconfiguring hardware
  * and then setting musb->board_mode. For now, only support OTG mode.
  */
-void musb_platform_set_mode(struct musb *musb, u8 musb_mode)
+int musb_platform_set_mode(struct musb *musb, u8 musb_mode)
 {
 	void __iomem	*tbase = musb->ctrl_base;
 	u32		otg_stat, phy_otg_ctrl, phy_otg_ena, dev_conf;
@@ -641,7 +641,8 @@ void musb_platform_set_mode(struct musb *musb, u8 musb_mode)
 #endif
 
 	default:
-		DBG(2, "Trying to set unknown mode %i\n", musb_mode);
+		DBG(2, "Trying to set mode %i\n", musb_mode);
+		return -EINVAL;
 	}
 
 	musb_writel(tbase, TUSB_PHY_OTG_CTRL,
@@ -655,6 +656,8 @@ void musb_platform_set_mode(struct musb *musb, u8 musb_mode)
 		!(otg_stat & TUSB_DEV_OTG_STAT_ID_STATUS))
 			INFO("Cannot be peripheral with mini-A cable "
 			"otg_stat: %08x\n", otg_stat);
+
+	return 0;
 }
 
 static inline unsigned long
diff --git a/drivers/usb/otg/Kconfig b/drivers/usb/otg/Kconfig
new file mode 100644
index 000000000000..8e8dbdb9b39b
--- /dev/null
+++ b/drivers/usb/otg/Kconfig
@@ -0,0 +1,54 @@
+#
+# USB OTG infrastructure may be needed for peripheral-only, host-only,
+# or OTG-capable configurations when OTG transceivers or controllers
+# are used.
+#
+
+comment "OTG and related infrastructure"
+
+if USB || USB_GADGET
+
+config USB_OTG_UTILS
+	bool
+	help
+	  Select this to make sure the build includes objects from
+	  the OTG infrastructure directory.
+
+#
+# USB Transceiver Drivers
+#
+config USB_GPIO_VBUS
+	tristate "GPIO based peripheral-only VBUS sensing 'transceiver'"
+	depends on GENERIC_GPIO
+	select USB_OTG_UTILS
+	help
+	  Provides simple GPIO VBUS sensing for controllers with an
+	  internal transceiver via the otg_transceiver interface, and
+	  optionally control of a D+ pullup GPIO as well as a VBUS
+	  current limit regulator.
+
+config ISP1301_OMAP
+	tristate "Philips ISP1301 with OMAP OTG"
+	depends on I2C && ARCH_OMAP_OTG
+	select USB_OTG_UTILS
+	help
+	  If you say yes here you get support for the Philips ISP1301
+	  USB-On-The-Go transceiver working with the OMAP OTG controller.
+	  The ISP1301 is a full speed USB  transceiver which is used in
+	  products including H2, H3, and H4 development boards for Texas
+	  Instruments OMAP processors.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called isp1301_omap.
+
+config TWL4030_USB
+	tristate "TWL4030 USB Transceiver Driver"
+	depends on TWL4030_CORE
+	select USB_OTG_UTILS
+	help
+	  Enable this to support the USB OTG transceiver on TWL4030
+	  family chips (including the TWL5030 and TPS659x0 devices).
+	  This transceiver supports high and full speed devices plus,
+	  in host mode, low speed.
+
+endif # USB || OTG
diff --git a/drivers/usb/otg/Makefile b/drivers/usb/otg/Makefile
new file mode 100644
index 000000000000..d73c7cf5e2f7
--- /dev/null
+++ b/drivers/usb/otg/Makefile
@@ -0,0 +1,15 @@
+#
+# OTG infrastructure and transceiver drivers
+#
+
+# infrastructure
+obj-$(CONFIG_USB_OTG_UTILS)	+= otg.o
+
+# transceiver drivers
+obj-$(CONFIG_USB_GPIO_VBUS)	+= gpio_vbus.o
+obj-$(CONFIG_ISP1301_OMAP)	+= isp1301_omap.o
+obj-$(CONFIG_TWL4030_USB)	+= twl4030-usb.o
+
+ccflags-$(CONFIG_USB_DEBUG)	+= -DDEBUG
+ccflags-$(CONFIG_USB_GADGET_DEBUG) += -DDEBUG
+
diff --git a/drivers/usb/otg/gpio_vbus.c b/drivers/usb/otg/gpio_vbus.c
new file mode 100644
index 000000000000..63a6036f04be
--- /dev/null
+++ b/drivers/usb/otg/gpio_vbus.c
@@ -0,0 +1,335 @@
+/*
+ * gpio-vbus.c - simple GPIO VBUS sensing driver for B peripheral devices
+ *
+ * Copyright (c) 2008 Philipp Zabel <philipp.zabel@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 <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/usb.h>
+
+#include <linux/regulator/consumer.h>
+
+#include <linux/usb/gadget.h>
+#include <linux/usb/gpio_vbus.h>
+#include <linux/usb/otg.h>
+
+
+/*
+ * A simple GPIO VBUS sensing driver for B peripheral only devices
+ * with internal transceivers. It can control a D+ pullup GPIO and
+ * a regulator to limit the current drawn from VBUS.
+ *
+ * Needs to be loaded before the UDC driver that will use it.
+ */
+struct gpio_vbus_data {
+	struct otg_transceiver otg;
+	struct device          *dev;
+	struct regulator       *vbus_draw;
+	int			vbus_draw_enabled;
+	unsigned		mA;
+};
+
+
+/*
+ * This driver relies on "both edges" triggering.  VBUS has 100 msec to
+ * stabilize, so the peripheral controller driver may need to cope with
+ * some bouncing due to current surges (e.g. charging local capacitance)
+ * and contact chatter.
+ *
+ * REVISIT in desperate straits, toggling between rising and falling
+ * edges might be workable.
+ */
+#define VBUS_IRQ_FLAGS \
+	( IRQF_SAMPLE_RANDOM | IRQF_SHARED \
+	| IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING )
+
+
+/* interface to regulator framework */
+static void set_vbus_draw(struct gpio_vbus_data *gpio_vbus, unsigned mA)
+{
+	struct regulator *vbus_draw = gpio_vbus->vbus_draw;
+	int enabled;
+
+	if (!vbus_draw)
+		return;
+
+	enabled = gpio_vbus->vbus_draw_enabled;
+	if (mA) {
+		regulator_set_current_limit(vbus_draw, 0, 1000 * mA);
+		if (!enabled) {
+			regulator_enable(vbus_draw);
+			gpio_vbus->vbus_draw_enabled = 1;
+		}
+	} else {
+		if (enabled) {
+			regulator_disable(vbus_draw);
+			gpio_vbus->vbus_draw_enabled = 0;
+		}
+	}
+	gpio_vbus->mA = mA;
+}
+
+/* VBUS change IRQ handler */
+static irqreturn_t gpio_vbus_irq(int irq, void *data)
+{
+	struct platform_device *pdev = data;
+	struct gpio_vbus_mach_info *pdata = pdev->dev.platform_data;
+	struct gpio_vbus_data *gpio_vbus = platform_get_drvdata(pdev);
+	int gpio, vbus;
+
+	vbus = gpio_get_value(pdata->gpio_vbus);
+	if (pdata->gpio_vbus_inverted)
+		vbus = !vbus;
+
+	dev_dbg(&pdev->dev, "VBUS %s (gadget: %s)\n",
+		vbus ? "supplied" : "inactive",
+		gpio_vbus->otg.gadget ? gpio_vbus->otg.gadget->name : "none");
+
+	if (!gpio_vbus->otg.gadget)
+		return IRQ_HANDLED;
+
+	/* Peripheral controllers which manage the pullup themselves won't have
+	 * gpio_pullup configured here.  If it's configured here, we'll do what
+	 * isp1301_omap::b_peripheral() does and enable the pullup here... although
+	 * that may complicate usb_gadget_{,dis}connect() support.
+	 */
+	gpio = pdata->gpio_pullup;
+	if (vbus) {
+		gpio_vbus->otg.state = OTG_STATE_B_PERIPHERAL;
+		usb_gadget_vbus_connect(gpio_vbus->otg.gadget);
+
+		/* drawing a "unit load" is *always* OK, except for OTG */
+		set_vbus_draw(gpio_vbus, 100);
+
+		/* optionally enable D+ pullup */
+		if (gpio_is_valid(gpio))
+			gpio_set_value(gpio, !pdata->gpio_pullup_inverted);
+	} else {
+		/* optionally disable D+ pullup */
+		if (gpio_is_valid(gpio))
+			gpio_set_value(gpio, pdata->gpio_pullup_inverted);
+
+		set_vbus_draw(gpio_vbus, 0);
+
+		usb_gadget_vbus_disconnect(gpio_vbus->otg.gadget);
+		gpio_vbus->otg.state = OTG_STATE_B_IDLE;
+	}
+
+	return IRQ_HANDLED;
+}
+
+/* OTG transceiver interface */
+
+/* bind/unbind the peripheral controller */
+static int gpio_vbus_set_peripheral(struct otg_transceiver *otg,
+				struct usb_gadget *gadget)
+{
+	struct gpio_vbus_data *gpio_vbus;
+	struct gpio_vbus_mach_info *pdata;
+	struct platform_device *pdev;
+	int gpio, irq;
+
+	gpio_vbus = container_of(otg, struct gpio_vbus_data, otg);
+	pdev = to_platform_device(gpio_vbus->dev);
+	pdata = gpio_vbus->dev->platform_data;
+	irq = gpio_to_irq(pdata->gpio_vbus);
+	gpio = pdata->gpio_pullup;
+
+	if (!gadget) {
+		dev_dbg(&pdev->dev, "unregistering gadget '%s'\n",
+			otg->gadget->name);
+
+		/* optionally disable D+ pullup */
+		if (gpio_is_valid(gpio))
+			gpio_set_value(gpio, pdata->gpio_pullup_inverted);
+
+		set_vbus_draw(gpio_vbus, 0);
+
+		usb_gadget_vbus_disconnect(otg->gadget);
+		otg->state = OTG_STATE_UNDEFINED;
+
+		otg->gadget = NULL;
+		return 0;
+	}
+
+	otg->gadget = gadget;
+	dev_dbg(&pdev->dev, "registered gadget '%s'\n", gadget->name);
+
+	/* initialize connection state */
+	gpio_vbus_irq(irq, pdev);
+	return 0;
+}
+
+/* effective for B devices, ignored for A-peripheral */
+static int gpio_vbus_set_power(struct otg_transceiver *otg, unsigned mA)
+{
+	struct gpio_vbus_data *gpio_vbus;
+
+	gpio_vbus = container_of(otg, struct gpio_vbus_data, otg);
+
+	if (otg->state == OTG_STATE_B_PERIPHERAL)
+		set_vbus_draw(gpio_vbus, mA);
+	return 0;
+}
+
+/* for non-OTG B devices: set/clear transceiver suspend mode */
+static int gpio_vbus_set_suspend(struct otg_transceiver *otg, int suspend)
+{
+	struct gpio_vbus_data *gpio_vbus;
+
+	gpio_vbus = container_of(otg, struct gpio_vbus_data, otg);
+
+	/* draw max 0 mA from vbus in suspend mode; or the previously
+	 * recorded amount of current if not suspended
+	 *
+	 * NOTE: high powered configs (mA > 100) may draw up to 2.5 mA
+	 * if they're wake-enabled ... we don't handle that yet.
+	 */
+	return gpio_vbus_set_power(otg, suspend ? 0 : gpio_vbus->mA);
+}
+
+/* platform driver interface */
+
+static int __init gpio_vbus_probe(struct platform_device *pdev)
+{
+	struct gpio_vbus_mach_info *pdata = pdev->dev.platform_data;
+	struct gpio_vbus_data *gpio_vbus;
+	struct resource *res;
+	int err, gpio, irq;
+
+	if (!pdata || !gpio_is_valid(pdata->gpio_vbus))
+		return -EINVAL;
+	gpio = pdata->gpio_vbus;
+
+	gpio_vbus = kzalloc(sizeof(struct gpio_vbus_data), GFP_KERNEL);
+	if (!gpio_vbus)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, gpio_vbus);
+	gpio_vbus->dev = &pdev->dev;
+	gpio_vbus->otg.label = "gpio-vbus";
+	gpio_vbus->otg.state = OTG_STATE_UNDEFINED;
+	gpio_vbus->otg.set_peripheral = gpio_vbus_set_peripheral;
+	gpio_vbus->otg.set_power = gpio_vbus_set_power;
+	gpio_vbus->otg.set_suspend = gpio_vbus_set_suspend;
+
+	err = gpio_request(gpio, "vbus_detect");
+	if (err) {
+		dev_err(&pdev->dev, "can't request vbus gpio %d, err: %d\n",
+			gpio, err);
+		goto err_gpio;
+	}
+	gpio_direction_input(gpio);
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res) {
+		irq = res->start;
+		res->flags &= IRQF_TRIGGER_MASK;
+		res->flags |= IRQF_SAMPLE_RANDOM | IRQF_SHARED;
+	} else
+		irq = gpio_to_irq(gpio);
+
+	/* if data line pullup is in use, initialize it to "not pulling up" */
+	gpio = pdata->gpio_pullup;
+	if (gpio_is_valid(gpio)) {
+		err = gpio_request(gpio, "udc_pullup");
+		if (err) {
+			dev_err(&pdev->dev,
+				"can't request pullup gpio %d, err: %d\n",
+				gpio, err);
+			gpio_free(pdata->gpio_vbus);
+			goto err_gpio;
+		}
+		gpio_direction_output(gpio, pdata->gpio_pullup_inverted);
+	}
+
+	err = request_irq(irq, gpio_vbus_irq, VBUS_IRQ_FLAGS,
+		"vbus_detect", pdev);
+	if (err) {
+		dev_err(&pdev->dev, "can't request irq %i, err: %d\n",
+			irq, err);
+		goto err_irq;
+	}
+
+	/* only active when a gadget is registered */
+	err = otg_set_transceiver(&gpio_vbus->otg);
+	if (err) {
+		dev_err(&pdev->dev, "can't register transceiver, err: %d\n",
+			err);
+		goto err_otg;
+	}
+
+	gpio_vbus->vbus_draw = regulator_get(&pdev->dev, "vbus_draw");
+	if (IS_ERR(gpio_vbus->vbus_draw)) {
+		dev_dbg(&pdev->dev, "can't get vbus_draw regulator, err: %ld\n",
+			PTR_ERR(gpio_vbus->vbus_draw));
+		gpio_vbus->vbus_draw = NULL;
+	}
+
+	return 0;
+err_otg:
+	free_irq(irq, &pdev->dev);
+err_irq:
+	if (gpio_is_valid(pdata->gpio_pullup))
+		gpio_free(pdata->gpio_pullup);
+	gpio_free(pdata->gpio_vbus);
+err_gpio:
+	platform_set_drvdata(pdev, NULL);
+	kfree(gpio_vbus);
+	return err;
+}
+
+static int __exit gpio_vbus_remove(struct platform_device *pdev)
+{
+	struct gpio_vbus_data *gpio_vbus = platform_get_drvdata(pdev);
+	struct gpio_vbus_mach_info *pdata = pdev->dev.platform_data;
+	int gpio = pdata->gpio_vbus;
+
+	regulator_put(gpio_vbus->vbus_draw);
+
+	otg_set_transceiver(NULL);
+
+	free_irq(gpio_to_irq(gpio), &pdev->dev);
+	if (gpio_is_valid(pdata->gpio_pullup))
+		gpio_free(pdata->gpio_pullup);
+	gpio_free(gpio);
+	platform_set_drvdata(pdev, NULL);
+	kfree(gpio_vbus);
+
+	return 0;
+}
+
+/* NOTE:  the gpio-vbus device may *NOT* be hotplugged */
+
+MODULE_ALIAS("platform:gpio-vbus");
+
+static struct platform_driver gpio_vbus_driver = {
+	.driver = {
+		.name  = "gpio-vbus",
+		.owner = THIS_MODULE,
+	},
+	.remove  = __exit_p(gpio_vbus_remove),
+};
+
+static int __init gpio_vbus_init(void)
+{
+	return platform_driver_probe(&gpio_vbus_driver, gpio_vbus_probe);
+}
+module_init(gpio_vbus_init);
+
+static void __exit gpio_vbus_exit(void)
+{
+	platform_driver_unregister(&gpio_vbus_driver);
+}
+module_exit(gpio_vbus_exit);
+
+MODULE_DESCRIPTION("simple GPIO controlled OTG transceiver driver");
+MODULE_AUTHOR("Philipp Zabel");
+MODULE_LICENSE("GPL");
diff --git a/drivers/i2c/chips/isp1301_omap.c b/drivers/usb/otg/isp1301_omap.c
index e0d56ef2bcb0..e0d56ef2bcb0 100644
--- a/drivers/i2c/chips/isp1301_omap.c
+++ b/drivers/usb/otg/isp1301_omap.c
diff --git a/drivers/usb/otg/otg.c b/drivers/usb/otg/otg.c
new file mode 100644
index 000000000000..ff318fae7d4d
--- /dev/null
+++ b/drivers/usb/otg/otg.c
@@ -0,0 +1,65 @@
+/*
+ * otg.c -- USB OTG utility code
+ *
+ * Copyright (C) 2004 Texas Instruments
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+
+#include <linux/usb/otg.h>
+
+static struct otg_transceiver *xceiv;
+
+/**
+ * otg_get_transceiver - find the (single) OTG transceiver
+ *
+ * Returns the transceiver driver, after getting a refcount to it; or
+ * null if there is no such transceiver.  The caller is responsible for
+ * calling otg_put_transceiver() to release that count.
+ *
+ * For use by USB host and peripheral drivers.
+ */
+struct otg_transceiver *otg_get_transceiver(void)
+{
+	if (xceiv)
+		get_device(xceiv->dev);
+	return xceiv;
+}
+EXPORT_SYMBOL(otg_get_transceiver);
+
+/**
+ * otg_put_transceiver - release the (single) OTG transceiver
+ * @x: the transceiver returned by otg_get_transceiver()
+ *
+ * Releases a refcount the caller received from otg_get_transceiver().
+ *
+ * For use by USB host and peripheral drivers.
+ */
+void otg_put_transceiver(struct otg_transceiver *x)
+{
+	put_device(x->dev);
+}
+EXPORT_SYMBOL(otg_put_transceiver);
+
+/**
+ * otg_set_transceiver - declare the (single) OTG transceiver
+ * @x: the USB OTG transceiver to be used; or NULL
+ *
+ * This call is exclusively for use by transceiver drivers, which
+ * coordinate the activities of drivers for host and peripheral
+ * controllers, and in some cases for VBUS current regulation.
+ */
+int otg_set_transceiver(struct otg_transceiver *x)
+{
+	if (xceiv && x)
+		return -EBUSY;
+	xceiv = x;
+	return 0;
+}
+EXPORT_SYMBOL(otg_set_transceiver);
diff --git a/drivers/usb/otg/twl4030-usb.c b/drivers/usb/otg/twl4030-usb.c
new file mode 100644
index 000000000000..416e4410be02
--- /dev/null
+++ b/drivers/usb/otg/twl4030-usb.c
@@ -0,0 +1,721 @@
+/*
+ * twl4030_usb - TWL4030 USB transceiver, talking to OMAP OTG controller
+ *
+ * Copyright (C) 2004-2007 Texas Instruments
+ * Copyright (C) 2008 Nokia Corporation
+ * Contact: Felipe Balbi <felipe.balbi@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ * Current status:
+ *	- HS USB ULPI mode works.
+ *	- 3-pin mode support may be added in future.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/workqueue.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/usb/otg.h>
+#include <linux/i2c/twl4030.h>
+
+
+/* Register defines */
+
+#define VENDOR_ID_LO			0x00
+#define VENDOR_ID_HI			0x01
+#define PRODUCT_ID_LO			0x02
+#define PRODUCT_ID_HI			0x03
+
+#define FUNC_CTRL			0x04
+#define FUNC_CTRL_SET			0x05
+#define FUNC_CTRL_CLR			0x06
+#define FUNC_CTRL_SUSPENDM		(1 << 6)
+#define FUNC_CTRL_RESET			(1 << 5)
+#define FUNC_CTRL_OPMODE_MASK		(3 << 3) /* bits 3 and 4 */
+#define FUNC_CTRL_OPMODE_NORMAL		(0 << 3)
+#define FUNC_CTRL_OPMODE_NONDRIVING	(1 << 3)
+#define FUNC_CTRL_OPMODE_DISABLE_BIT_NRZI	(2 << 3)
+#define FUNC_CTRL_TERMSELECT		(1 << 2)
+#define FUNC_CTRL_XCVRSELECT_MASK	(3 << 0) /* bits 0 and 1 */
+#define FUNC_CTRL_XCVRSELECT_HS		(0 << 0)
+#define FUNC_CTRL_XCVRSELECT_FS		(1 << 0)
+#define FUNC_CTRL_XCVRSELECT_LS		(2 << 0)
+#define FUNC_CTRL_XCVRSELECT_FS4LS	(3 << 0)
+
+#define IFC_CTRL			0x07
+#define IFC_CTRL_SET			0x08
+#define IFC_CTRL_CLR			0x09
+#define IFC_CTRL_INTERFACE_PROTECT_DISABLE	(1 << 7)
+#define IFC_CTRL_AUTORESUME		(1 << 4)
+#define IFC_CTRL_CLOCKSUSPENDM		(1 << 3)
+#define IFC_CTRL_CARKITMODE		(1 << 2)
+#define IFC_CTRL_FSLSSERIALMODE_3PIN	(1 << 1)
+
+#define TWL4030_OTG_CTRL		0x0A
+#define TWL4030_OTG_CTRL_SET		0x0B
+#define TWL4030_OTG_CTRL_CLR		0x0C
+#define TWL4030_OTG_CTRL_DRVVBUS	(1 << 5)
+#define TWL4030_OTG_CTRL_CHRGVBUS	(1 << 4)
+#define TWL4030_OTG_CTRL_DISCHRGVBUS	(1 << 3)
+#define TWL4030_OTG_CTRL_DMPULLDOWN	(1 << 2)
+#define TWL4030_OTG_CTRL_DPPULLDOWN	(1 << 1)
+#define TWL4030_OTG_CTRL_IDPULLUP	(1 << 0)
+
+#define USB_INT_EN_RISE			0x0D
+#define USB_INT_EN_RISE_SET		0x0E
+#define USB_INT_EN_RISE_CLR		0x0F
+#define USB_INT_EN_FALL			0x10
+#define USB_INT_EN_FALL_SET		0x11
+#define USB_INT_EN_FALL_CLR		0x12
+#define USB_INT_STS			0x13
+#define USB_INT_LATCH			0x14
+#define USB_INT_IDGND			(1 << 4)
+#define USB_INT_SESSEND			(1 << 3)
+#define USB_INT_SESSVALID		(1 << 2)
+#define USB_INT_VBUSVALID		(1 << 1)
+#define USB_INT_HOSTDISCONNECT		(1 << 0)
+
+#define CARKIT_CTRL			0x19
+#define CARKIT_CTRL_SET			0x1A
+#define CARKIT_CTRL_CLR			0x1B
+#define CARKIT_CTRL_MICEN		(1 << 6)
+#define CARKIT_CTRL_SPKRIGHTEN		(1 << 5)
+#define CARKIT_CTRL_SPKLEFTEN		(1 << 4)
+#define CARKIT_CTRL_RXDEN		(1 << 3)
+#define CARKIT_CTRL_TXDEN		(1 << 2)
+#define CARKIT_CTRL_IDGNDDRV		(1 << 1)
+#define CARKIT_CTRL_CARKITPWR		(1 << 0)
+#define CARKIT_PLS_CTRL			0x22
+#define CARKIT_PLS_CTRL_SET		0x23
+#define CARKIT_PLS_CTRL_CLR		0x24
+#define CARKIT_PLS_CTRL_SPKRRIGHT_BIASEN	(1 << 3)
+#define CARKIT_PLS_CTRL_SPKRLEFT_BIASEN	(1 << 2)
+#define CARKIT_PLS_CTRL_RXPLSEN		(1 << 1)
+#define CARKIT_PLS_CTRL_TXPLSEN		(1 << 0)
+
+#define MCPC_CTRL			0x30
+#define MCPC_CTRL_SET			0x31
+#define MCPC_CTRL_CLR			0x32
+#define MCPC_CTRL_RTSOL			(1 << 7)
+#define MCPC_CTRL_EXTSWR		(1 << 6)
+#define MCPC_CTRL_EXTSWC		(1 << 5)
+#define MCPC_CTRL_VOICESW		(1 << 4)
+#define MCPC_CTRL_OUT64K		(1 << 3)
+#define MCPC_CTRL_RTSCTSSW		(1 << 2)
+#define MCPC_CTRL_HS_UART		(1 << 0)
+
+#define MCPC_IO_CTRL			0x33
+#define MCPC_IO_CTRL_SET		0x34
+#define MCPC_IO_CTRL_CLR		0x35
+#define MCPC_IO_CTRL_MICBIASEN		(1 << 5)
+#define MCPC_IO_CTRL_CTS_NPU		(1 << 4)
+#define MCPC_IO_CTRL_RXD_PU		(1 << 3)
+#define MCPC_IO_CTRL_TXDTYP		(1 << 2)
+#define MCPC_IO_CTRL_CTSTYP		(1 << 1)
+#define MCPC_IO_CTRL_RTSTYP		(1 << 0)
+
+#define MCPC_CTRL2			0x36
+#define MCPC_CTRL2_SET			0x37
+#define MCPC_CTRL2_CLR			0x38
+#define MCPC_CTRL2_MCPC_CK_EN		(1 << 0)
+
+#define OTHER_FUNC_CTRL			0x80
+#define OTHER_FUNC_CTRL_SET		0x81
+#define OTHER_FUNC_CTRL_CLR		0x82
+#define OTHER_FUNC_CTRL_BDIS_ACON_EN	(1 << 4)
+#define OTHER_FUNC_CTRL_FIVEWIRE_MODE	(1 << 2)
+
+#define OTHER_IFC_CTRL			0x83
+#define OTHER_IFC_CTRL_SET		0x84
+#define OTHER_IFC_CTRL_CLR		0x85
+#define OTHER_IFC_CTRL_OE_INT_EN	(1 << 6)
+#define OTHER_IFC_CTRL_CEA2011_MODE	(1 << 5)
+#define OTHER_IFC_CTRL_FSLSSERIALMODE_4PIN	(1 << 4)
+#define OTHER_IFC_CTRL_HIZ_ULPI_60MHZ_OUT	(1 << 3)
+#define OTHER_IFC_CTRL_HIZ_ULPI		(1 << 2)
+#define OTHER_IFC_CTRL_ALT_INT_REROUTE	(1 << 0)
+
+#define OTHER_INT_EN_RISE		0x86
+#define OTHER_INT_EN_RISE_SET		0x87
+#define OTHER_INT_EN_RISE_CLR		0x88
+#define OTHER_INT_EN_FALL		0x89
+#define OTHER_INT_EN_FALL_SET		0x8A
+#define OTHER_INT_EN_FALL_CLR		0x8B
+#define OTHER_INT_STS			0x8C
+#define OTHER_INT_LATCH			0x8D
+#define OTHER_INT_VB_SESS_VLD		(1 << 7)
+#define OTHER_INT_DM_HI			(1 << 6) /* not valid for "latch" reg */
+#define OTHER_INT_DP_HI			(1 << 5) /* not valid for "latch" reg */
+#define OTHER_INT_BDIS_ACON		(1 << 3) /* not valid for "fall" regs */
+#define OTHER_INT_MANU			(1 << 1)
+#define OTHER_INT_ABNORMAL_STRESS	(1 << 0)
+
+#define ID_STATUS			0x96
+#define ID_RES_FLOAT			(1 << 4)
+#define ID_RES_440K			(1 << 3)
+#define ID_RES_200K			(1 << 2)
+#define ID_RES_102K			(1 << 1)
+#define ID_RES_GND			(1 << 0)
+
+#define POWER_CTRL			0xAC
+#define POWER_CTRL_SET			0xAD
+#define POWER_CTRL_CLR			0xAE
+#define POWER_CTRL_OTG_ENAB		(1 << 5)
+
+#define OTHER_IFC_CTRL2			0xAF
+#define OTHER_IFC_CTRL2_SET		0xB0
+#define OTHER_IFC_CTRL2_CLR		0xB1
+#define OTHER_IFC_CTRL2_ULPI_STP_LOW	(1 << 4)
+#define OTHER_IFC_CTRL2_ULPI_TXEN_POL	(1 << 3)
+#define OTHER_IFC_CTRL2_ULPI_4PIN_2430	(1 << 2)
+#define OTHER_IFC_CTRL2_USB_INT_OUTSEL_MASK	(3 << 0) /* bits 0 and 1 */
+#define OTHER_IFC_CTRL2_USB_INT_OUTSEL_INT1N	(0 << 0)
+#define OTHER_IFC_CTRL2_USB_INT_OUTSEL_INT2N	(1 << 0)
+
+#define REG_CTRL_EN			0xB2
+#define REG_CTRL_EN_SET			0xB3
+#define REG_CTRL_EN_CLR			0xB4
+#define REG_CTRL_ERROR			0xB5
+#define ULPI_I2C_CONFLICT_INTEN		(1 << 0)
+
+#define OTHER_FUNC_CTRL2		0xB8
+#define OTHER_FUNC_CTRL2_SET		0xB9
+#define OTHER_FUNC_CTRL2_CLR		0xBA
+#define OTHER_FUNC_CTRL2_VBAT_TIMER_EN	(1 << 0)
+
+/* following registers do not have separate _clr and _set registers */
+#define VBUS_DEBOUNCE			0xC0
+#define ID_DEBOUNCE			0xC1
+#define VBAT_TIMER			0xD3
+#define PHY_PWR_CTRL			0xFD
+#define PHY_PWR_PHYPWD			(1 << 0)
+#define PHY_CLK_CTRL			0xFE
+#define PHY_CLK_CTRL_CLOCKGATING_EN	(1 << 2)
+#define PHY_CLK_CTRL_CLK32K_EN		(1 << 1)
+#define REQ_PHY_DPLL_CLK		(1 << 0)
+#define PHY_CLK_CTRL_STS		0xFF
+#define PHY_DPLL_CLK			(1 << 0)
+
+/* In module TWL4030_MODULE_PM_MASTER */
+#define PROTECT_KEY			0x0E
+
+/* In module TWL4030_MODULE_PM_RECEIVER */
+#define VUSB_DEDICATED1			0x7D
+#define VUSB_DEDICATED2			0x7E
+#define VUSB1V5_DEV_GRP			0x71
+#define VUSB1V5_TYPE			0x72
+#define VUSB1V5_REMAP			0x73
+#define VUSB1V8_DEV_GRP			0x74
+#define VUSB1V8_TYPE			0x75
+#define VUSB1V8_REMAP			0x76
+#define VUSB3V1_DEV_GRP			0x77
+#define VUSB3V1_TYPE			0x78
+#define VUSB3V1_REMAP			0x79
+
+/* In module TWL4030_MODULE_INTBR */
+#define PMBR1				0x0D
+#define GPIO_USB_4PIN_ULPI_2430C	(3 << 0)
+
+
+
+enum linkstat {
+	USB_LINK_UNKNOWN = 0,
+	USB_LINK_NONE,
+	USB_LINK_VBUS,
+	USB_LINK_ID,
+};
+
+struct twl4030_usb {
+	struct otg_transceiver	otg;
+	struct device		*dev;
+
+	/* for vbus reporting with irqs disabled */
+	spinlock_t		lock;
+
+	/* pin configuration */
+	enum twl4030_usb_mode	usb_mode;
+
+	int			irq;
+	u8			linkstat;
+	u8			asleep;
+	bool			irq_enabled;
+};
+
+/* internal define on top of container_of */
+#define xceiv_to_twl(x)		container_of((x), struct twl4030_usb, otg);
+
+/*-------------------------------------------------------------------------*/
+
+static int twl4030_i2c_write_u8_verify(struct twl4030_usb *twl,
+		u8 module, u8 data, u8 address)
+{
+	u8 check;
+
+	if ((twl4030_i2c_write_u8(module, data, address) >= 0) &&
+	    (twl4030_i2c_read_u8(module, &check, address) >= 0) &&
+						(check == data))
+		return 0;
+	dev_dbg(twl->dev, "Write%d[%d,0x%x] wrote %02x but read %02x\n",
+			1, module, address, check, data);
+
+	/* Failed once: Try again */
+	if ((twl4030_i2c_write_u8(module, data, address) >= 0) &&
+	    (twl4030_i2c_read_u8(module, &check, address) >= 0) &&
+						(check == data))
+		return 0;
+	dev_dbg(twl->dev, "Write%d[%d,0x%x] wrote %02x but read %02x\n",
+			2, module, address, check, data);
+
+	/* Failed again: Return error */
+	return -EBUSY;
+}
+
+#define twl4030_usb_write_verify(twl, address, data)	\
+	twl4030_i2c_write_u8_verify(twl, TWL4030_MODULE_USB, (data), (address))
+
+static inline int twl4030_usb_write(struct twl4030_usb *twl,
+		u8 address, u8 data)
+{
+	int ret = 0;
+
+	ret = twl4030_i2c_write_u8(TWL4030_MODULE_USB, data, address);
+	if (ret < 0)
+		dev_dbg(twl->dev,
+			"TWL4030:USB:Write[0x%x] Error %d\n", address, ret);
+	return ret;
+}
+
+static inline int twl4030_readb(struct twl4030_usb *twl, u8 module, u8 address)
+{
+	u8 data;
+	int ret = 0;
+
+	ret = twl4030_i2c_read_u8(module, &data, address);
+	if (ret >= 0)
+		ret = data;
+	else
+		dev_dbg(twl->dev,
+			"TWL4030:readb[0x%x,0x%x] Error %d\n",
+					module, address, ret);
+
+	return ret;
+}
+
+static inline int twl4030_usb_read(struct twl4030_usb *twl, u8 address)
+{
+	return twl4030_readb(twl, TWL4030_MODULE_USB, address);
+}
+
+/*-------------------------------------------------------------------------*/
+
+static inline int
+twl4030_usb_set_bits(struct twl4030_usb *twl, u8 reg, u8 bits)
+{
+	return twl4030_usb_write(twl, reg + 1, bits);
+}
+
+static inline int
+twl4030_usb_clear_bits(struct twl4030_usb *twl, u8 reg, u8 bits)
+{
+	return twl4030_usb_write(twl, reg + 2, bits);
+}
+
+/*-------------------------------------------------------------------------*/
+
+static enum linkstat twl4030_usb_linkstat(struct twl4030_usb *twl)
+{
+	int	status;
+	int	linkstat = USB_LINK_UNKNOWN;
+
+	/* STS_HW_CONDITIONS */
+	status = twl4030_readb(twl, TWL4030_MODULE_PM_MASTER, 0x0f);
+	if (status < 0)
+		dev_err(twl->dev, "USB link status err %d\n", status);
+	else if (status & BIT(7))
+		linkstat = USB_LINK_VBUS;
+	else if (status & BIT(2))
+		linkstat = USB_LINK_ID;
+	else
+		linkstat = USB_LINK_NONE;
+
+	dev_dbg(twl->dev, "HW_CONDITIONS 0x%02x/%d; link %d\n",
+			status, status, linkstat);
+
+	/* REVISIT this assumes host and peripheral controllers
+	 * are registered, and that both are active...
+	 */
+
+	spin_lock_irq(&twl->lock);
+	twl->linkstat = linkstat;
+	if (linkstat == USB_LINK_ID) {
+		twl->otg.default_a = true;
+		twl->otg.state = OTG_STATE_A_IDLE;
+	} else {
+		twl->otg.default_a = false;
+		twl->otg.state = OTG_STATE_B_IDLE;
+	}
+	spin_unlock_irq(&twl->lock);
+
+	return linkstat;
+}
+
+static void twl4030_usb_set_mode(struct twl4030_usb *twl, int mode)
+{
+	twl->usb_mode = mode;
+
+	switch (mode) {
+	case T2_USB_MODE_ULPI:
+		twl4030_usb_clear_bits(twl, IFC_CTRL, IFC_CTRL_CARKITMODE);
+		twl4030_usb_set_bits(twl, POWER_CTRL, POWER_CTRL_OTG_ENAB);
+		twl4030_usb_clear_bits(twl, FUNC_CTRL,
+					FUNC_CTRL_XCVRSELECT_MASK |
+					FUNC_CTRL_OPMODE_MASK);
+		break;
+	case -1:
+		/* FIXME: power on defaults */
+		break;
+	default:
+		dev_err(twl->dev, "unsupported T2 transceiver mode %d\n",
+				mode);
+		break;
+	};
+}
+
+static void twl4030_i2c_access(struct twl4030_usb *twl, int on)
+{
+	unsigned long timeout;
+	int val = twl4030_usb_read(twl, PHY_CLK_CTRL);
+
+	if (val >= 0) {
+		if (on) {
+			/* enable DPLL to access PHY registers over I2C */
+			val |= REQ_PHY_DPLL_CLK;
+			WARN_ON(twl4030_usb_write_verify(twl, PHY_CLK_CTRL,
+						(u8)val) < 0);
+
+			timeout = jiffies + HZ;
+			while (!(twl4030_usb_read(twl, PHY_CLK_CTRL_STS) &
+							PHY_DPLL_CLK)
+				&& time_before(jiffies, timeout))
+					udelay(10);
+			if (!(twl4030_usb_read(twl, PHY_CLK_CTRL_STS) &
+							PHY_DPLL_CLK))
+				dev_err(twl->dev, "Timeout setting T2 HSUSB "
+						"PHY DPLL clock\n");
+		} else {
+			/* let ULPI control the DPLL clock */
+			val &= ~REQ_PHY_DPLL_CLK;
+			WARN_ON(twl4030_usb_write_verify(twl, PHY_CLK_CTRL,
+						(u8)val) < 0);
+		}
+	}
+}
+
+static void twl4030_phy_power(struct twl4030_usb *twl, int on)
+{
+	u8 pwr;
+
+	pwr = twl4030_usb_read(twl, PHY_PWR_CTRL);
+	if (on) {
+		pwr &= ~PHY_PWR_PHYPWD;
+		WARN_ON(twl4030_usb_write_verify(twl, PHY_PWR_CTRL, pwr) < 0);
+		twl4030_usb_write(twl, PHY_CLK_CTRL,
+				  twl4030_usb_read(twl, PHY_CLK_CTRL) |
+					(PHY_CLK_CTRL_CLOCKGATING_EN |
+						PHY_CLK_CTRL_CLK32K_EN));
+	} else  {
+		pwr |= PHY_PWR_PHYPWD;
+		WARN_ON(twl4030_usb_write_verify(twl, PHY_PWR_CTRL, pwr) < 0);
+	}
+}
+
+static void twl4030_phy_suspend(struct twl4030_usb *twl, int controller_off)
+{
+	if (twl->asleep)
+		return;
+
+	twl4030_phy_power(twl, 0);
+	twl->asleep = 1;
+}
+
+static void twl4030_phy_resume(struct twl4030_usb *twl)
+{
+	if (!twl->asleep)
+		return;
+
+	twl4030_phy_power(twl, 1);
+	twl4030_i2c_access(twl, 1);
+	twl4030_usb_set_mode(twl, twl->usb_mode);
+	if (twl->usb_mode == T2_USB_MODE_ULPI)
+		twl4030_i2c_access(twl, 0);
+	twl->asleep = 0;
+}
+
+static void twl4030_usb_ldo_init(struct twl4030_usb *twl)
+{
+	/* Enable writing to power configuration registers */
+	twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, 0xC0, PROTECT_KEY);
+	twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, 0x0C, PROTECT_KEY);
+
+	/* put VUSB3V1 LDO in active state */
+	twl4030_i2c_write_u8(TWL4030_MODULE_PM_RECEIVER, 0, VUSB_DEDICATED2);
+
+	/* input to VUSB3V1 LDO is from VBAT, not VBUS */
+	twl4030_i2c_write_u8(TWL4030_MODULE_PM_RECEIVER, 0x14, VUSB_DEDICATED1);
+
+	/* turn on 3.1V regulator */
+	twl4030_i2c_write_u8(TWL4030_MODULE_PM_RECEIVER, 0x20, VUSB3V1_DEV_GRP);
+	twl4030_i2c_write_u8(TWL4030_MODULE_PM_RECEIVER, 0, VUSB3V1_TYPE);
+
+	/* turn on 1.5V regulator */
+	twl4030_i2c_write_u8(TWL4030_MODULE_PM_RECEIVER, 0x20, VUSB1V5_DEV_GRP);
+	twl4030_i2c_write_u8(TWL4030_MODULE_PM_RECEIVER, 0, VUSB1V5_TYPE);
+
+	/* turn on 1.8V regulator */
+	twl4030_i2c_write_u8(TWL4030_MODULE_PM_RECEIVER, 0x20, VUSB1V8_DEV_GRP);
+	twl4030_i2c_write_u8(TWL4030_MODULE_PM_RECEIVER, 0, VUSB1V8_TYPE);
+
+	/* disable access to power configuration registers */
+	twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, 0, PROTECT_KEY);
+}
+
+static ssize_t twl4030_usb_vbus_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct twl4030_usb *twl = dev_get_drvdata(dev);
+	unsigned long flags;
+	int ret = -EINVAL;
+
+	spin_lock_irqsave(&twl->lock, flags);
+	ret = sprintf(buf, "%s\n",
+			(twl->linkstat == USB_LINK_VBUS) ? "on" : "off");
+	spin_unlock_irqrestore(&twl->lock, flags);
+
+	return ret;
+}
+static DEVICE_ATTR(vbus, 0444, twl4030_usb_vbus_show, NULL);
+
+static irqreturn_t twl4030_usb_irq(int irq, void *_twl)
+{
+	struct twl4030_usb *twl = _twl;
+	int status;
+
+#ifdef CONFIG_LOCKDEP
+	/* WORKAROUND for lockdep forcing IRQF_DISABLED on us, which
+	 * we don't want and can't tolerate.  Although it might be
+	 * friendlier not to borrow this thread context...
+	 */
+	local_irq_enable();
+#endif
+
+	status = twl4030_usb_linkstat(twl);
+	if (status != USB_LINK_UNKNOWN) {
+
+		/* FIXME add a set_power() method so that B-devices can
+		 * configure the charger appropriately.  It's not always
+		 * correct to consume VBUS power, and how much current to
+		 * consume is a function of the USB configuration chosen
+		 * by the host.
+		 *
+		 * REVISIT usb_gadget_vbus_connect(...) as needed, ditto
+		 * its disconnect() sibling, when changing to/from the
+		 * USB_LINK_VBUS state.  musb_hdrc won't care until it
+		 * starts to handle softconnect right.
+		 */
+		twl4030charger_usb_en(status == USB_LINK_VBUS);
+
+		if (status == USB_LINK_NONE)
+			twl4030_phy_suspend(twl, 0);
+		else
+			twl4030_phy_resume(twl);
+	}
+	sysfs_notify(&twl->dev->kobj, NULL, "vbus");
+
+	return IRQ_HANDLED;
+}
+
+static int twl4030_set_suspend(struct otg_transceiver *x, int suspend)
+{
+	struct twl4030_usb *twl = xceiv_to_twl(x);
+
+	if (suspend)
+		twl4030_phy_suspend(twl, 1);
+	else
+		twl4030_phy_resume(twl);
+
+	return 0;
+}
+
+static int twl4030_set_peripheral(struct otg_transceiver *x,
+		struct usb_gadget *gadget)
+{
+	struct twl4030_usb *twl;
+
+	if (!x)
+		return -ENODEV;
+
+	twl = xceiv_to_twl(x);
+	twl->otg.gadget = gadget;
+	if (!gadget)
+		twl->otg.state = OTG_STATE_UNDEFINED;
+
+	return 0;
+}
+
+static int twl4030_set_host(struct otg_transceiver *x, struct usb_bus *host)
+{
+	struct twl4030_usb *twl;
+
+	if (!x)
+		return -ENODEV;
+
+	twl = xceiv_to_twl(x);
+	twl->otg.host = host;
+	if (!host)
+		twl->otg.state = OTG_STATE_UNDEFINED;
+
+	return 0;
+}
+
+static int __init twl4030_usb_probe(struct platform_device *pdev)
+{
+	struct twl4030_usb_data *pdata = pdev->dev.platform_data;
+	struct twl4030_usb	*twl;
+	int			status;
+
+	if (!pdata) {
+		dev_dbg(&pdev->dev, "platform_data not available\n");
+		return -EINVAL;
+	}
+
+	twl = kzalloc(sizeof *twl, GFP_KERNEL);
+	if (!twl)
+		return -ENOMEM;
+
+	twl->dev		= &pdev->dev;
+	twl->irq		= platform_get_irq(pdev, 0);
+	twl->otg.dev		= twl->dev;
+	twl->otg.label		= "twl4030";
+	twl->otg.set_host	= twl4030_set_host;
+	twl->otg.set_peripheral	= twl4030_set_peripheral;
+	twl->otg.set_suspend	= twl4030_set_suspend;
+	twl->usb_mode		= pdata->usb_mode;
+	twl->asleep		= 1;
+
+	/* init spinlock for workqueue */
+	spin_lock_init(&twl->lock);
+
+	twl4030_usb_ldo_init(twl);
+	otg_set_transceiver(&twl->otg);
+
+	platform_set_drvdata(pdev, twl);
+	if (device_create_file(&pdev->dev, &dev_attr_vbus))
+		dev_warn(&pdev->dev, "could not create sysfs file\n");
+
+	/* Our job is to use irqs and status from the power module
+	 * to keep the transceiver disabled when nothing's connected.
+	 *
+	 * FIXME we actually shouldn't start enabling it until the
+	 * USB controller drivers have said they're ready, by calling
+	 * set_host() and/or set_peripheral() ... OTG_capable boards
+	 * need both handles, otherwise just one suffices.
+	 */
+	twl->irq_enabled = true;
+	status = request_irq(twl->irq, twl4030_usb_irq,
+			IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+			"twl4030_usb", twl);
+	if (status < 0) {
+		dev_dbg(&pdev->dev, "can't get IRQ %d, err %d\n",
+			twl->irq, status);
+		kfree(twl);
+		return status;
+	}
+
+	/* The IRQ handler just handles changes from the previous states
+	 * of the ID and VBUS pins ... in probe() we must initialize that
+	 * previous state.  The easy way:  fake an IRQ.
+	 *
+	 * REVISIT:  a real IRQ might have happened already, if PREEMPT is
+	 * enabled.  Else the IRQ may not yet be configured or enabled,
+	 * because of scheduling delays.
+	 */
+	twl4030_usb_irq(twl->irq, twl);
+
+	dev_info(&pdev->dev, "Initialized TWL4030 USB module\n");
+	return 0;
+}
+
+static int __exit twl4030_usb_remove(struct platform_device *pdev)
+{
+	struct twl4030_usb *twl = platform_get_drvdata(pdev);
+	int val;
+
+	free_irq(twl->irq, twl);
+	device_remove_file(twl->dev, &dev_attr_vbus);
+
+	/* set transceiver mode to power on defaults */
+	twl4030_usb_set_mode(twl, -1);
+
+	/* autogate 60MHz ULPI clock,
+	 * clear dpll clock request for i2c access,
+	 * disable 32KHz
+	 */
+	val = twl4030_usb_read(twl, PHY_CLK_CTRL);
+	if (val >= 0) {
+		val |= PHY_CLK_CTRL_CLOCKGATING_EN;
+		val &= ~(PHY_CLK_CTRL_CLK32K_EN | REQ_PHY_DPLL_CLK);
+		twl4030_usb_write(twl, PHY_CLK_CTRL, (u8)val);
+	}
+
+	/* disable complete OTG block */
+	twl4030_usb_clear_bits(twl, POWER_CTRL, POWER_CTRL_OTG_ENAB);
+
+	twl4030_phy_power(twl, 0);
+
+	kfree(twl);
+
+	return 0;
+}
+
+static struct platform_driver twl4030_usb_driver = {
+	.probe		= twl4030_usb_probe,
+	.remove		= __exit_p(twl4030_usb_remove),
+	.driver		= {
+		.name	= "twl4030_usb",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init twl4030_usb_init(void)
+{
+	return platform_driver_register(&twl4030_usb_driver);
+}
+subsys_initcall(twl4030_usb_init);
+
+static void __exit twl4030_usb_exit(void)
+{
+	platform_driver_unregister(&twl4030_usb_driver);
+}
+module_exit(twl4030_usb_exit);
+
+MODULE_ALIAS("platform:twl4030_usb");
+MODULE_AUTHOR("Texas Instruments, Inc, Nokia Corporation");
+MODULE_DESCRIPTION("TWL4030 USB transceiver driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/serial/Kconfig b/drivers/usb/serial/Kconfig
index 70338f4ec918..b361f05cafac 100644
--- a/drivers/usb/serial/Kconfig
+++ b/drivers/usb/serial/Kconfig
@@ -496,6 +496,14 @@ config USB_SERIAL_SAFE_PADDED
 	bool "USB Secure Encapsulated Driver - Padded"
 	depends on USB_SERIAL_SAFE
 
+config USB_SERIAL_SIEMENS_MPI
+	tristate "USB Siemens MPI driver"
+	help
+	  Say M here if you want to use a Siemens USB/MPI adapter.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called siemens_mpi.
+
 config USB_SERIAL_SIERRAWIRELESS
 	tristate "USB Sierra Wireless Driver"
 	help
@@ -565,6 +573,15 @@ config USB_SERIAL_OMNINET
 	  To compile this driver as a module, choose M here: the
 	  module will be called omninet.
 
+config USB_SERIAL_OPTICON
+	tristate "USB Opticon Barcode driver (serial mode)"
+	help
+	  Say Y here if you want to use a Opticon USB Barcode device
+	  in serial emulation mode.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called opticon.
+
 config USB_SERIAL_DEBUG
 	tristate "USB Debugging Device"
 	help
diff --git a/drivers/usb/serial/Makefile b/drivers/usb/serial/Makefile
index 6047f818adfe..b75be91eb8f1 100644
--- a/drivers/usb/serial/Makefile
+++ b/drivers/usb/serial/Makefile
@@ -41,10 +41,12 @@ obj-$(CONFIG_USB_SERIAL_MOS7840)		+= mos7840.o
 obj-$(CONFIG_USB_SERIAL_MOTOROLA)		+= moto_modem.o
 obj-$(CONFIG_USB_SERIAL_NAVMAN)			+= navman.o
 obj-$(CONFIG_USB_SERIAL_OMNINET)		+= omninet.o
+obj-$(CONFIG_USB_SERIAL_OPTICON)		+= opticon.o
 obj-$(CONFIG_USB_SERIAL_OPTION)			+= option.o
 obj-$(CONFIG_USB_SERIAL_OTI6858)		+= oti6858.o
 obj-$(CONFIG_USB_SERIAL_PL2303)			+= pl2303.o
 obj-$(CONFIG_USB_SERIAL_SAFE)			+= safe_serial.o
+obj-$(CONFIG_USB_SERIAL_SIEMENS_MPI)		+= siemens_mpi.o
 obj-$(CONFIG_USB_SERIAL_SIERRAWIRELESS)		+= sierra.o
 obj-$(CONFIG_USB_SERIAL_SPCP8X5)		+= spcp8x5.o
 obj-$(CONFIG_USB_SERIAL_TI)			+= ti_usb_3410_5052.o
diff --git a/drivers/usb/serial/digi_acceleport.c b/drivers/usb/serial/digi_acceleport.c
index 69f84f0ea6fe..38ba4ea8b6bf 100644
--- a/drivers/usb/serial/digi_acceleport.c
+++ b/drivers/usb/serial/digi_acceleport.c
@@ -635,8 +635,7 @@ static int digi_write_oob_command(struct usb_serial_port *port,
 
 	spin_lock_irqsave(&oob_priv->dp_port_lock, flags);
 	while (count > 0) {
-		while (oob_port->write_urb->status == -EINPROGRESS
-			|| oob_priv->dp_write_urb_in_use) {
+		while (oob_priv->dp_write_urb_in_use) {
 			cond_wait_interruptible_timeout_irqrestore(
 				&oob_port->write_wait, DIGI_RETRY_TIMEOUT,
 				&oob_priv->dp_port_lock, flags);
@@ -699,9 +698,8 @@ static int digi_write_inb_command(struct usb_serial_port *port,
 
 	spin_lock_irqsave(&priv->dp_port_lock, flags);
 	while (count > 0 && ret == 0) {
-		while ((port->write_urb->status == -EINPROGRESS
-				|| priv->dp_write_urb_in_use)
-					&& time_before(jiffies, timeout)) {
+		while (priv->dp_write_urb_in_use &&
+		       time_before(jiffies, timeout)) {
 			cond_wait_interruptible_timeout_irqrestore(
 				&port->write_wait, DIGI_RETRY_TIMEOUT,
 				&priv->dp_port_lock, flags);
@@ -779,8 +777,7 @@ static int digi_set_modem_signals(struct usb_serial_port *port,
 	spin_lock_irqsave(&oob_priv->dp_port_lock, flags);
 	spin_lock(&port_priv->dp_port_lock);
 
-	while (oob_port->write_urb->status == -EINPROGRESS ||
-					oob_priv->dp_write_urb_in_use) {
+	while (oob_priv->dp_write_urb_in_use) {
 		spin_unlock(&port_priv->dp_port_lock);
 		cond_wait_interruptible_timeout_irqrestore(
 			&oob_port->write_wait, DIGI_RETRY_TIMEOUT,
@@ -1168,12 +1165,10 @@ static int digi_write(struct tty_struct *tty, struct usb_serial_port *port,
 
 	/* be sure only one write proceeds at a time */
 	/* there are races on the port private buffer */
-	/* and races to check write_urb->status */
 	spin_lock_irqsave(&priv->dp_port_lock, flags);
 
 	/* wait for urb status clear to submit another urb */
-	if (port->write_urb->status == -EINPROGRESS ||
-					priv->dp_write_urb_in_use) {
+	if (priv->dp_write_urb_in_use) {
 		/* buffer data if count is 1 (probably put_char) if possible */
 		if (count == 1 && priv->dp_out_buf_len < DIGI_OUT_BUF_SIZE) {
 			priv->dp_out_buf[priv->dp_out_buf_len++] = *buf;
@@ -1236,7 +1231,7 @@ static void digi_write_bulk_callback(struct urb *urb)
 	int ret = 0;
 	int status = urb->status;
 
-	dbg("digi_write_bulk_callback: TOP, urb->status=%d", status);
+	dbg("digi_write_bulk_callback: TOP, status=%d", status);
 
 	/* port and serial sanity check */
 	if (port == NULL || (priv = usb_get_serial_port_data(port)) == NULL) {
@@ -1266,8 +1261,7 @@ static void digi_write_bulk_callback(struct urb *urb)
 	/* try to send any buffered data on this port, if it is open */
 	spin_lock(&priv->dp_port_lock);
 	priv->dp_write_urb_in_use = 0;
-	if (port->port.count && port->write_urb->status != -EINPROGRESS
-	    && priv->dp_out_buf_len > 0) {
+	if (port->port.count && priv->dp_out_buf_len > 0) {
 		*((unsigned char *)(port->write_urb->transfer_buffer))
 			= (unsigned char)DIGI_CMD_SEND_DATA;
 		*((unsigned char *)(port->write_urb->transfer_buffer) + 1)
@@ -1305,8 +1299,7 @@ static int digi_write_room(struct tty_struct *tty)
 
 	spin_lock_irqsave(&priv->dp_port_lock, flags);
 
-	if (port->write_urb->status == -EINPROGRESS ||
-					priv->dp_write_urb_in_use)
+	if (priv->dp_write_urb_in_use)
 		room = 0;
 	else
 		room = port->bulk_out_size - 2 - priv->dp_out_buf_len;
@@ -1322,8 +1315,7 @@ static int digi_chars_in_buffer(struct tty_struct *tty)
 	struct usb_serial_port *port = tty->driver_data;
 	struct digi_port *priv = usb_get_serial_port_data(port);
 
-	if (port->write_urb->status == -EINPROGRESS
-	    || priv->dp_write_urb_in_use) {
+	if (priv->dp_write_urb_in_use) {
 		dbg("digi_chars_in_buffer: port=%d, chars=%d",
 			priv->dp_port_num, port->bulk_out_size - 2);
 		/* return(port->bulk_out_size - 2); */
@@ -1702,7 +1694,7 @@ static int digi_read_inb_callback(struct urb *urb)
 	/* short/multiple packet check */
 	if (urb->actual_length != len + 2) {
 		dev_err(&port->dev, "%s: INCOMPLETE OR MULTIPLE PACKET, "
-			"urb->status=%d, port=%d, opcode=%d, len=%d, "
+			"status=%d, port=%d, opcode=%d, len=%d, "
 			"actual_length=%d, status=%d\n", __func__, status,
 			priv->dp_port_num, opcode, len, urb->actual_length,
 			port_status);
diff --git a/drivers/usb/serial/garmin_gps.c b/drivers/usb/serial/garmin_gps.c
index 8e6a66e38db2..a26a0e2cdb4a 100644
--- a/drivers/usb/serial/garmin_gps.c
+++ b/drivers/usb/serial/garmin_gps.c
@@ -1056,7 +1056,7 @@ static void garmin_write_bulk_callback(struct urb *urb)
 
 		if (status) {
 			dbg("%s - nonzero write bulk status received: %d",
-			    __func__, urb->status);
+			    __func__, status);
 			spin_lock_irqsave(&garmin_data_p->lock, flags);
 			garmin_data_p->flags |= CLEAR_HALT_REQUIRED;
 			spin_unlock_irqrestore(&garmin_data_p->lock, flags);
diff --git a/drivers/usb/serial/ipw.c b/drivers/usb/serial/ipw.c
index 3ac59a8a980f..f530032ed93d 100644
--- a/drivers/usb/serial/ipw.c
+++ b/drivers/usb/serial/ipw.c
@@ -473,7 +473,7 @@ static struct usb_serial_driver ipw_device = {
 
 
 
-static int usb_ipw_init(void)
+static int __init usb_ipw_init(void)
 {
 	int retval;
 
@@ -490,7 +490,7 @@ static int usb_ipw_init(void)
 	return 0;
 }
 
-static void usb_ipw_exit(void)
+static void __exit usb_ipw_exit(void)
 {
 	usb_deregister(&usb_ipw_driver);
 	usb_serial_deregister(&ipw_device);
diff --git a/drivers/usb/serial/iuu_phoenix.c b/drivers/usb/serial/iuu_phoenix.c
index e320972cb227..2314c6ae4fc2 100644
--- a/drivers/usb/serial/iuu_phoenix.c
+++ b/drivers/usb/serial/iuu_phoenix.c
@@ -190,10 +190,12 @@ static void iuu_rxcmd(struct urb *urb)
 {
 	struct usb_serial_port *port = urb->context;
 	int result;
+	int status = urb->status;
+
 	dbg("%s - enter", __func__);
 
-	if (urb->status) {
-		dbg("%s - urb->status = %d", __func__, urb->status);
+	if (status) {
+		dbg("%s - status = %d", __func__, status);
 		/* error stop all */
 		return;
 	}
@@ -245,10 +247,12 @@ static void iuu_update_status_callback(struct urb *urb)
 	struct usb_serial_port *port = urb->context;
 	struct iuu_private *priv = usb_get_serial_port_data(port);
 	u8 *st;
+	int status = urb->status;
+
 	dbg("%s - enter", __func__);
 
-	if (urb->status) {
-		dbg("%s - urb->status = %d", __func__, urb->status);
+	if (status) {
+		dbg("%s - status = %d", __func__, status);
 		/* error stop all */
 		return;
 	}
@@ -274,9 +278,9 @@ static void iuu_status_callback(struct urb *urb)
 {
 	struct usb_serial_port *port = urb->context;
 	int result;
-	dbg("%s - enter", __func__);
+	int status = urb->status;
 
-	dbg("%s - urb->status = %d", __func__, urb->status);
+	dbg("%s - status = %d", __func__, status);
 	usb_fill_bulk_urb(port->read_urb, port->serial->dev,
 			  usb_rcvbulkpipe(port->serial->dev,
 					  port->bulk_in_endpointAddress),
@@ -618,11 +622,12 @@ static void read_buf_callback(struct urb *urb)
 	struct usb_serial_port *port = urb->context;
 	unsigned char *data = urb->transfer_buffer;
 	struct tty_struct *tty;
-	dbg("%s - urb->status = %d", __func__, urb->status);
+	int status = urb->status;
 
-	if (urb->status) {
-		dbg("%s - urb->status = %d", __func__, urb->status);
-		if (urb->status == -EPROTO) {
+	dbg("%s - status = %d", __func__, status);
+
+	if (status) {
+		if (status == -EPROTO) {
 			/* reschedule needed */
 		}
 		return;
@@ -695,7 +700,7 @@ static void iuu_uart_read_callback(struct urb *urb)
 	struct usb_serial_port *port = urb->context;
 	struct iuu_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
-	int status;
+	int status = urb->status;
 	int error = 0;
 	int len = 0;
 	unsigned char *data = urb->transfer_buffer;
@@ -703,8 +708,8 @@ static void iuu_uart_read_callback(struct urb *urb)
 
 	dbg("%s - enter", __func__);
 
-	if (urb->status) {
-		dbg("%s - urb->status = %d", __func__, urb->status);
+	if (status) {
+		dbg("%s - status = %d", __func__, status);
 		/* error stop all */
 		return;
 	}
@@ -782,12 +787,11 @@ static void read_rxcmd_callback(struct urb *urb)
 {
 	struct usb_serial_port *port = urb->context;
 	int result;
-	dbg("%s - enter", __func__);
+	int status = urb->status;
 
-	dbg("%s - urb->status = %d", __func__, urb->status);
+	dbg("%s - status = %d", __func__, status);
 
-	if (urb->status) {
-		dbg("%s - urb->status = %d", __func__, urb->status);
+	if (status) {
 		/* error stop all */
 		return;
 	}
diff --git a/drivers/usb/serial/mos7840.c b/drivers/usb/serial/mos7840.c
index 96a8c7713212..2c20e88a91b3 100644
--- a/drivers/usb/serial/mos7840.c
+++ b/drivers/usb/serial/mos7840.c
@@ -214,6 +214,7 @@ struct moschip_port {
 	spinlock_t pool_lock;
 	struct urb *write_urb_pool[NUM_URBS];
 	char busy[NUM_URBS];
+	bool read_urb_busy;
 };
 
 
@@ -679,26 +680,30 @@ static void mos7840_bulk_in_callback(struct urb *urb)
 	struct tty_struct *tty;
 	int status = urb->status;
 
-	if (status) {
-		dbg("nonzero read bulk status received: %d", status);
-		return;
-	}
-
 	mos7840_port = urb->context;
 	if (!mos7840_port) {
 		dbg("%s", "NULL mos7840_port pointer \n");
+		mos7840_port->read_urb_busy = false;
+		return;
+	}
+
+	if (status) {
+		dbg("nonzero read bulk status received: %d", status);
+		mos7840_port->read_urb_busy = false;
 		return;
 	}
 
 	port = (struct usb_serial_port *)mos7840_port->port;
 	if (mos7840_port_paranoia_check(port, __func__)) {
 		dbg("%s", "Port Paranoia failed \n");
+		mos7840_port->read_urb_busy = false;
 		return;
 	}
 
 	serial = mos7840_get_usb_serial(port, __func__);
 	if (!serial) {
 		dbg("%s\n", "Bad serial pointer ");
+		mos7840_port->read_urb_busy = false;
 		return;
 	}
 
@@ -725,17 +730,19 @@ static void mos7840_bulk_in_callback(struct urb *urb)
 
 	if (!mos7840_port->read_urb) {
 		dbg("%s", "URB KILLED !!!\n");
+		mos7840_port->read_urb_busy = false;
 		return;
 	}
 
 
 	mos7840_port->read_urb->dev = serial->dev;
 
+	mos7840_port->read_urb_busy = true;
 	retval = usb_submit_urb(mos7840_port->read_urb, GFP_ATOMIC);
 
 	if (retval) {
-		dbg(" usb_submit_urb(read bulk) failed, retval = %d",
-		 retval);
+		dbg("usb_submit_urb(read bulk) failed, retval = %d", retval);
+		mos7840_port->read_urb_busy = false;
 	}
 }
 
@@ -1055,10 +1062,12 @@ static int mos7840_open(struct tty_struct *tty,
 
 	dbg("mos7840_open: bulkin endpoint is %d\n",
 	    port->bulk_in_endpointAddress);
+	mos7840_port->read_urb_busy = true;
 	response = usb_submit_urb(mos7840_port->read_urb, GFP_KERNEL);
 	if (response) {
 		dev_err(&port->dev, "%s - Error %d submitting control urb\n",
 			__func__, response);
+		mos7840_port->read_urb_busy = false;
 	}
 
 	/* initialize our wait queues */
@@ -1227,6 +1236,7 @@ static void mos7840_close(struct tty_struct *tty,
 		if (mos7840_port->read_urb) {
 			dbg("%s", "Shutdown bulk read\n");
 			usb_kill_urb(mos7840_port->read_urb);
+			mos7840_port->read_urb_busy = false;
 		}
 		if ((&mos7840_port->control_urb)) {
 			dbg("%s", "Shutdown control read\n");
@@ -2043,14 +2053,14 @@ static void mos7840_change_port_settings(struct tty_struct *tty,
 	Data = 0x0c;
 	mos7840_set_uart_reg(port, INTERRUPT_ENABLE_REGISTER, Data);
 
-	if (mos7840_port->read_urb->status != -EINPROGRESS) {
+	if (mos7840_port->read_urb_busy == false) {
 		mos7840_port->read_urb->dev = serial->dev;
-
+		mos7840_port->read_urb_busy = true;
 		status = usb_submit_urb(mos7840_port->read_urb, GFP_ATOMIC);
-
 		if (status) {
-			dbg(" usb_submit_urb(read bulk) failed, status = %d",
+			dbg("usb_submit_urb(read bulk) failed, status = %d",
 			    status);
+			mos7840_port->read_urb_busy = false;
 		}
 	}
 	wake_up(&mos7840_port->delta_msr_wait);
@@ -2117,12 +2127,14 @@ static void mos7840_set_termios(struct tty_struct *tty,
 		return;
 	}
 
-	if (mos7840_port->read_urb->status != -EINPROGRESS) {
+	if (mos7840_port->read_urb_busy == false) {
 		mos7840_port->read_urb->dev = serial->dev;
+		mos7840_port->read_urb_busy = true;
 		status = usb_submit_urb(mos7840_port->read_urb, GFP_ATOMIC);
 		if (status) {
-			dbg(" usb_submit_urb(read bulk) failed, status = %d",
+			dbg("usb_submit_urb(read bulk) failed, status = %d",
 			    status);
+			mos7840_port->read_urb_busy = false;
 		}
 	}
 	return;
diff --git a/drivers/usb/serial/opticon.c b/drivers/usb/serial/opticon.c
new file mode 100644
index 000000000000..cea326f1f105
--- /dev/null
+++ b/drivers/usb/serial/opticon.c
@@ -0,0 +1,358 @@
+/*
+ * Opticon USB barcode to serial driver
+ *
+ * Copyright (C) 2008 Greg Kroah-Hartman <gregkh@suse.de>
+ * Copyright (C) 2008 Novell 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/kernel.h>
+#include <linux/init.h>
+#include <linux/tty.h>
+#include <linux/tty_driver.h>
+#include <linux/tty_flip.h>
+#include <linux/module.h>
+#include <linux/usb.h>
+#include <linux/usb/serial.h>
+#include <linux/uaccess.h>
+
+static int debug;
+
+static struct usb_device_id id_table[] = {
+	{ USB_DEVICE(0x065a, 0x0009) },
+	{ },
+};
+MODULE_DEVICE_TABLE(usb, id_table);
+
+/* This structure holds all of the individual device information */
+struct opticon_private {
+	struct usb_device *udev;
+	struct usb_serial *serial;
+	struct usb_serial_port *port;
+	unsigned char *bulk_in_buffer;
+	struct urb *bulk_read_urb;
+	int buffer_size;
+	u8 bulk_address;
+	spinlock_t lock;	/* protects the following flags */
+	bool throttled;
+	bool actually_throttled;
+	bool rts;
+};
+
+static void opticon_bulk_callback(struct urb *urb)
+{
+	struct opticon_private *priv = urb->context;
+	unsigned char *data = urb->transfer_buffer;
+	struct usb_serial_port *port = priv->port;
+	int status = urb->status;
+	struct tty_struct *tty;
+	int result;
+	int available_room = 0;
+	int data_length;
+
+	dbg("%s - port %d", __func__, port->number);
+
+	switch (status) {
+	case 0:
+		/* success */
+		break;
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+		/* this urb is terminated, clean up */
+		dbg("%s - urb shutting down with status: %d",
+		    __func__, status);
+		return;
+	default:
+		dbg("%s - nonzero urb status received: %d",
+		    __func__, status);
+		goto exit;
+	}
+
+	usb_serial_debug_data(debug, &port->dev, __func__, urb->actual_length,
+			      data);
+
+	if (urb->actual_length > 2) {
+		data_length = urb->actual_length - 2;
+
+		/*
+		 * Data from the device comes with a 2 byte header:
+		 *
+		 * <0x00><0x00>data...
+		 * 	This is real data to be sent to the tty layer
+		 * <0x00><0x01)level
+		 * 	This is a RTS level change, the third byte is the RTS
+		 * 	value (0 for low, 1 for high).
+		 */
+		if ((data[0] == 0x00) && (data[1] == 0x00)) {
+			/* real data, send it to the tty layer */
+			tty = tty_port_tty_get(&port->port);
+			if (tty) {
+				available_room = tty_buffer_request_room(tty,
+								data_length);
+				if (available_room) {
+					tty_insert_flip_string(tty, data,
+							       available_room);
+					tty_flip_buffer_push(tty);
+				}
+				tty_kref_put(tty);
+			}
+		} else {
+			if ((data[0] == 0x00) && (data[1] == 0x01)) {
+				if (data[2] == 0x00)
+					priv->rts = false;
+				else
+					priv->rts = true;
+				/* FIXME change the RTS level */
+			} else {
+			dev_dbg(&priv->udev->dev,
+				"Unknown data packet received from the device:"
+				" %2x %2x\n",
+				data[0], data[1]);
+			}
+		}
+	} else {
+		dev_dbg(&priv->udev->dev,
+			"Improper ammount of data received from the device, "
+			"%d bytes", urb->actual_length);
+	}
+
+exit:
+	spin_lock(&priv->lock);
+
+	/* Continue trying to always read if we should */
+	if (!priv->throttled) {
+		usb_fill_bulk_urb(priv->bulk_read_urb, priv->udev,
+				  usb_rcvbulkpipe(priv->udev,
+						  priv->bulk_address),
+				  priv->bulk_in_buffer, priv->buffer_size,
+				  opticon_bulk_callback, priv);
+		result = usb_submit_urb(port->read_urb, GFP_ATOMIC);
+		if (result)
+			dev_err(&port->dev,
+			    "%s - failed resubmitting read urb, error %d\n",
+							__func__, result);
+	} else
+		priv->actually_throttled = true;
+	spin_unlock(&priv->lock);
+}
+
+static int opticon_open(struct tty_struct *tty, struct usb_serial_port *port,
+			struct file *filp)
+{
+	struct opticon_private *priv = usb_get_serial_data(port->serial);
+	unsigned long flags;
+	int result = 0;
+
+	dbg("%s - port %d", __func__, port->number);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->throttled = false;
+	priv->actually_throttled = false;
+	priv->port = port;
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/*
+	 * Force low_latency on so that our tty_push actually forces the data
+	 * through, otherwise it is scheduled, and with high data rates (like
+	 * with OHCI) data can get lost.
+	 */
+	if (tty)
+		tty->low_latency = 1;
+
+	/* Start reading from the device */
+	usb_fill_bulk_urb(priv->bulk_read_urb, priv->udev,
+			  usb_rcvbulkpipe(priv->udev,
+					  priv->bulk_address),
+			  priv->bulk_in_buffer, priv->buffer_size,
+			  opticon_bulk_callback, priv);
+	result = usb_submit_urb(priv->bulk_read_urb, GFP_KERNEL);
+	if (result)
+		dev_err(&port->dev,
+			"%s - failed resubmitting read urb, error %d\n",
+			__func__, result);
+	return result;
+}
+
+static void opticon_close(struct tty_struct *tty, struct usb_serial_port *port,
+			  struct file *filp)
+{
+	struct opticon_private *priv = usb_get_serial_data(port->serial);
+
+	dbg("%s - port %d", __func__, port->number);
+
+	/* shutdown our urbs */
+	usb_kill_urb(priv->bulk_read_urb);
+}
+
+static void opticon_throttle(struct tty_struct *tty)
+{
+	struct usb_serial_port *port = tty->driver_data;
+	struct opticon_private *priv = usb_get_serial_data(port->serial);
+	unsigned long flags;
+
+	dbg("%s - port %d", __func__, port->number);
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->throttled = true;
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+
+static void opticon_unthrottle(struct tty_struct *tty)
+{
+	struct usb_serial_port *port = tty->driver_data;
+	struct opticon_private *priv = usb_get_serial_data(port->serial);
+	unsigned long flags;
+	int result;
+
+	dbg("%s - port %d", __func__, port->number);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->throttled = false;
+	priv->actually_throttled = false;
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	priv->bulk_read_urb->dev = port->serial->dev;
+	result = usb_submit_urb(priv->bulk_read_urb, GFP_ATOMIC);
+	if (result)
+		dev_err(&port->dev,
+			"%s - failed submitting read urb, error %d\n",
+							__func__, result);
+}
+
+static int opticon_startup(struct usb_serial *serial)
+{
+	struct opticon_private *priv;
+	struct usb_host_interface *intf;
+	int i;
+	int retval = -ENOMEM;
+	bool bulk_in_found = false;
+
+	/* create our private serial structure */
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (priv == NULL) {
+		dev_err(&serial->dev->dev, "%s - Out of memory\n", __func__);
+		return -ENOMEM;
+	}
+	spin_lock_init(&priv->lock);
+	priv->serial = serial;
+	priv->port = serial->port[0];
+	priv->udev = serial->dev;
+
+	/* find our bulk endpoint */
+	intf = serial->interface->altsetting;
+	for (i = 0; i < intf->desc.bNumEndpoints; ++i) {
+		struct usb_endpoint_descriptor *endpoint;
+
+		endpoint = &intf->endpoint[i].desc;
+		if (!usb_endpoint_is_bulk_in(endpoint))
+			continue;
+
+		priv->bulk_read_urb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!priv->bulk_read_urb) {
+			dev_err(&priv->udev->dev, "out of memory\n");
+			goto error;
+		}
+
+		priv->buffer_size = le16_to_cpu(endpoint->wMaxPacketSize) * 2;
+		priv->bulk_in_buffer = kmalloc(priv->buffer_size, GFP_KERNEL);
+		if (!priv->bulk_in_buffer) {
+			dev_err(&priv->udev->dev, "out of memory\n");
+			goto error;
+		}
+
+		priv->bulk_address = endpoint->bEndpointAddress;
+
+		/* set up our bulk urb */
+		usb_fill_bulk_urb(priv->bulk_read_urb, priv->udev,
+				  usb_rcvbulkpipe(priv->udev,
+						  endpoint->bEndpointAddress),
+				  priv->bulk_in_buffer, priv->buffer_size,
+				  opticon_bulk_callback, priv);
+
+		bulk_in_found = true;
+		break;
+		}
+
+	if (!bulk_in_found) {
+		dev_err(&priv->udev->dev,
+			"Error - the proper endpoints were not found!\n");
+		goto error;
+	}
+
+	usb_set_serial_data(serial, priv);
+	return 0;
+
+error:
+	usb_free_urb(priv->bulk_read_urb);
+	kfree(priv->bulk_in_buffer);
+	kfree(priv);
+	return retval;
+}
+
+static void opticon_shutdown(struct usb_serial *serial)
+{
+	struct opticon_private *priv = usb_get_serial_data(serial);
+
+	dbg("%s", __func__);
+
+	usb_kill_urb(priv->bulk_read_urb);
+	usb_free_urb(priv->bulk_read_urb);
+	kfree(priv->bulk_in_buffer);
+	kfree(priv);
+	usb_set_serial_data(serial, NULL);
+}
+
+
+static struct usb_driver opticon_driver = {
+	.name =		"opticon",
+	.probe =	usb_serial_probe,
+	.disconnect =	usb_serial_disconnect,
+	.id_table =	id_table,
+	.no_dynamic_id = 	1,
+};
+
+static struct usb_serial_driver opticon_device = {
+	.driver = {
+		.owner =	THIS_MODULE,
+		.name =		"opticon",
+	},
+	.id_table =		id_table,
+	.usb_driver = 		&opticon_driver,
+	.num_ports =		1,
+	.attach =		opticon_startup,
+	.open =			opticon_open,
+	.close =		opticon_close,
+	.shutdown =		opticon_shutdown,
+	.throttle = 		opticon_throttle,
+	.unthrottle =		opticon_unthrottle,
+};
+
+static int __init opticon_init(void)
+{
+	int retval;
+
+	retval = usb_serial_register(&opticon_device);
+	if (retval)
+		return retval;
+	retval = usb_register(&opticon_driver);
+	if (retval)
+		usb_serial_deregister(&opticon_device);
+	return retval;
+}
+
+static void __exit opticon_exit(void)
+{
+	usb_deregister(&opticon_driver);
+	usb_serial_deregister(&opticon_device);
+}
+
+module_init(opticon_init);
+module_exit(opticon_exit);
+MODULE_LICENSE("GPL");
+
+module_param(debug, bool, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Debug enabled or not");
diff --git a/drivers/usb/serial/option.c b/drivers/usb/serial/option.c
index 809697b3c7fc..5ed183477aaf 100644
--- a/drivers/usb/serial/option.c
+++ b/drivers/usb/serial/option.c
@@ -522,9 +522,9 @@ static int debug;
 /* per port private data */
 
 #define N_IN_URB 4
-#define N_OUT_URB 1
+#define N_OUT_URB 4
 #define IN_BUFLEN 4096
-#define OUT_BUFLEN 128
+#define OUT_BUFLEN 4096
 
 struct option_port_private {
 	/* Input endpoints and buffer for this port */
@@ -654,10 +654,6 @@ static int option_write(struct tty_struct *tty, struct usb_serial_port *port,
 			usb_unlink_urb(this_urb);
 			continue;
 		}
-		if (this_urb->status != 0)
-			dbg("usb_write %p failed (err=%d)",
-				this_urb, this_urb->status);
-
 		dbg("%s: endpoint %d buf %d", __func__,
 			usb_pipeendpoint(this_urb->pipe), i);
 
@@ -669,8 +665,7 @@ static int option_write(struct tty_struct *tty, struct usb_serial_port *port,
 		err = usb_submit_urb(this_urb, GFP_ATOMIC);
 		if (err) {
 			dbg("usb_submit_urb %p (write bulk) failed "
-				"(%d, has %d)", this_urb,
-				err, this_urb->status);
+				"(%d)", this_urb, err);
 			clear_bit(i, &portdata->out_busy);
 			continue;
 		}
diff --git a/drivers/usb/serial/siemens_mpi.c b/drivers/usb/serial/siemens_mpi.c
new file mode 100644
index 000000000000..951ea0c6ba77
--- /dev/null
+++ b/drivers/usb/serial/siemens_mpi.c
@@ -0,0 +1,77 @@
+/*
+ * Siemens USB-MPI Serial USB driver
+ *
+ * Copyright (C) 2005 Thomas Hergenhahn <thomas.hergenhahn@suse.de>
+ * Copyright (C) 2005,2008 Greg Kroah-Hartman <gregkh@suse.de>
+ *
+ *	This program is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU General Public License version
+ *	2 as published by the Free Software Foundation.
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/tty.h>
+#include <linux/module.h>
+#include <linux/usb.h>
+#include <linux/usb/serial.h>
+
+/* Version Information */
+#define DRIVER_VERSION "Version 0.1 09/26/2005"
+#define DRIVER_AUTHOR "Thomas Hergenhahn@web.de http://libnodave.sf.net"
+#define DRIVER_DESC "Driver for Siemens USB/MPI adapter"
+
+
+static struct usb_device_id id_table[] = {
+	/* Vendor and product id for 6ES7-972-0CB20-0XA0 */
+	{ USB_DEVICE(0x908, 0x0004) },
+	{ },
+};
+MODULE_DEVICE_TABLE(usb, id_table);
+
+static struct usb_driver siemens_usb_mpi_driver = {
+	.name =		"siemens_mpi",
+	.probe =	usb_serial_probe,
+	.disconnect =	usb_serial_disconnect,
+	.id_table =	id_table,
+};
+
+static struct usb_serial_driver siemens_usb_mpi_device = {
+	.driver = {
+		.owner =	THIS_MODULE,
+		.name =		"siemens_mpi",
+	},
+	.id_table =		id_table,
+	.num_ports =		1,
+};
+
+static int __init siemens_usb_mpi_init(void)
+{
+	int retval;
+
+	retval = usb_serial_register(&siemens_usb_mpi_device);
+	if (retval)
+		goto failed_usb_serial_register;
+	retval = usb_register(&siemens_usb_mpi_driver);
+	if (retval)
+		goto failed_usb_register;
+	printk(KERN_INFO DRIVER_DESC "\n");
+	printk(KERN_INFO DRIVER_VERSION " " DRIVER_AUTHOR "\n");
+	return retval;
+failed_usb_register:
+	usb_serial_deregister(&siemens_usb_mpi_device);
+failed_usb_serial_register:
+	return retval;
+}
+
+static void __exit siemens_usb_mpi_exit(void)
+{
+	usb_deregister(&siemens_usb_mpi_driver);
+	usb_serial_deregister(&siemens_usb_mpi_device);
+}
+
+module_init(siemens_usb_mpi_init);
+module_exit(siemens_usb_mpi_exit);
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/serial/spcp8x5.c b/drivers/usb/serial/spcp8x5.c
index a65bc2bd8e71..5e7528cc81a8 100644
--- a/drivers/usb/serial/spcp8x5.c
+++ b/drivers/usb/serial/spcp8x5.c
@@ -709,21 +709,20 @@ static void spcp8x5_read_bulk_callback(struct urb *urb)
 	unsigned char *data = urb->transfer_buffer;
 	unsigned long flags;
 	int i;
-	int result;
-	u8 status = 0;
+	int result = urb->status;
+	u8 status;
 	char tty_flag;
 
-	dev_dbg(&port->dev, "start, urb->status = %d, "
-		"urb->actual_length = %d\n,", urb->status, urb->actual_length);
+	dev_dbg(&port->dev, "start, result = %d, urb->actual_length = %d\n,",
+		result, urb->actual_length);
 
 	/* check the urb status */
-	if (urb->status) {
+	if (result) {
 		if (!port->port.count)
 			return;
-		if (urb->status == -EPROTO) {
+		if (result == -EPROTO) {
 			/* spcp8x5 mysteriously fails with -EPROTO */
 			/* reschedule the read */
-			urb->status = 0;
 			urb->dev = port->serial->dev;
 			result = usb_submit_urb(urb , GFP_ATOMIC);
 			if (result)
@@ -833,8 +832,9 @@ static void spcp8x5_write_bulk_callback(struct urb *urb)
 	struct usb_serial_port *port = urb->context;
 	struct spcp8x5_private *priv = usb_get_serial_port_data(port);
 	int result;
+	int status = urb->status;
 
-	switch (urb->status) {
+	switch (status) {
 	case 0:
 		/* success */
 		break;
@@ -843,14 +843,14 @@ static void spcp8x5_write_bulk_callback(struct urb *urb)
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
 		dev_dbg(&port->dev, "urb shutting down with status: %d\n",
-			urb->status);
+			status);
 		priv->write_urb_in_use = 0;
 		return;
 	default:
 		/* error in the urb, so we have to resubmit it */
 		dbg("%s - Overflow in write", __func__);
 		dbg("%s - nonzero write bulk status received: %d",
-			__func__, urb->status);
+			__func__, status);
 		port->write_urb->transfer_buffer_length = 1;
 		port->write_urb->dev = port->serial->dev;
 		result = usb_submit_urb(port->write_urb, GFP_ATOMIC);
diff --git a/drivers/usb/serial/usb_debug.c b/drivers/usb/serial/usb_debug.c
index fc5d9952b03b..6c9cbb59552a 100644
--- a/drivers/usb/serial/usb_debug.c
+++ b/drivers/usb/serial/usb_debug.c
@@ -31,7 +31,7 @@ static struct usb_driver debug_driver = {
 	.no_dynamic_id = 	1,
 };
 
-int usb_debug_open(struct tty_struct *tty, struct usb_serial_port *port,
+static int usb_debug_open(struct tty_struct *tty, struct usb_serial_port *port,
 							struct file *filp)
 {
 	port->bulk_out_size = USB_DEBUG_MAX_PACKET_SIZE;
diff --git a/drivers/usb/storage/Kconfig b/drivers/usb/storage/Kconfig
index c68b738900bd..9df6887b91f6 100644
--- a/drivers/usb/storage/Kconfig
+++ b/drivers/usb/storage/Kconfig
@@ -61,13 +61,6 @@ config USB_STORAGE_ISD200
 	  - CyQ've CQ8060A CDRW drive
 	  - Planex eXtreme Drive RX-25HU USB-IDE cable (not model RX-25U)
 
-config USB_STORAGE_DPCM
-	bool "Microtech/ZiO! CompactFlash/SmartMedia support"
-	depends on USB_STORAGE
-	help
-	  Say Y here to support the Microtech/ZiO! CompactFlash reader.
-	  There is a web page at <http://www.ziocorp.com/products/>.
-
 config USB_STORAGE_USBAT
 	bool "USBAT/USBAT02-based storage support"
 	depends on USB_STORAGE
@@ -90,12 +83,12 @@ config USB_STORAGE_USBAT
 	  - Sandisk ImageMate SDDR-05b
 
 config USB_STORAGE_SDDR09
-	bool "SanDisk SDDR-09 (and other SmartMedia) support"
+	bool "SanDisk SDDR-09 (and other SmartMedia, including DPCM) support"
 	depends on USB_STORAGE
 	help
 	  Say Y here to include additional code to support the Sandisk SDDR-09
 	  SmartMedia reader in the USB Mass Storage driver.
-	  Also works for the Microtech Zio! SmartMedia reader.
+	  Also works for the Microtech Zio! CompactFlash/SmartMedia reader.
 
 config USB_STORAGE_SDDR55
 	bool "SanDisk SDDR-55 SmartMedia support"
diff --git a/drivers/usb/storage/Makefile b/drivers/usb/storage/Makefile
index 7f8beb5366ae..b32069313390 100644
--- a/drivers/usb/storage/Makefile
+++ b/drivers/usb/storage/Makefile
@@ -14,7 +14,6 @@ usb-storage-obj-$(CONFIG_USB_STORAGE_USBAT)	+= shuttle_usbat.o
 usb-storage-obj-$(CONFIG_USB_STORAGE_SDDR09)	+= sddr09.o
 usb-storage-obj-$(CONFIG_USB_STORAGE_SDDR55)	+= sddr55.o
 usb-storage-obj-$(CONFIG_USB_STORAGE_FREECOM)	+= freecom.o
-usb-storage-obj-$(CONFIG_USB_STORAGE_DPCM)	+= dpcm.o
 usb-storage-obj-$(CONFIG_USB_STORAGE_ISD200)	+= isd200.o
 usb-storage-obj-$(CONFIG_USB_STORAGE_DATAFAB)	+= datafab.o
 usb-storage-obj-$(CONFIG_USB_STORAGE_JUMPSHOT)	+= jumpshot.o
@@ -24,7 +23,7 @@ usb-storage-obj-$(CONFIG_USB_STORAGE_KARMA)	+= karma.o
 usb-storage-obj-$(CONFIG_USB_STORAGE_CYPRESS_ATACB) += cypress_atacb.o
 
 usb-storage-objs :=	scsiglue.o protocol.o transport.o usb.o \
-			initializers.o sierra_ms.o $(usb-storage-obj-y)
+			initializers.o sierra_ms.o option_ms.o $(usb-storage-obj-y)
 
 ifneq ($(CONFIG_USB_LIBUSUAL),)
 	obj-$(CONFIG_USB)	+= libusual.o
diff --git a/drivers/usb/storage/dpcm.c b/drivers/usb/storage/dpcm.c
deleted file mode 100644
index 939923471af4..000000000000
--- a/drivers/usb/storage/dpcm.c
+++ /dev/null
@@ -1,86 +0,0 @@
-/* Driver for Microtech DPCM-USB CompactFlash/SmartMedia reader
- *
- * DPCM driver v0.1:
- *
- * First release
- *
- * Current development and maintenance by:
- *   (c) 2000 Brian Webb (webbb@earthlink.net)
- *
- * This device contains both a CompactFlash card reader, which
- * uses the Control/Bulk w/o Interrupt protocol and
- * a SmartMedia card reader that uses the same protocol
- * as the SDDR09.
- *
- * 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, write to the Free Software Foundation, Inc.,
- * 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-
-#include <scsi/scsi.h>
-#include <scsi/scsi_cmnd.h>
-#include <scsi/scsi_device.h>
-
-#include "usb.h"
-#include "transport.h"
-#include "protocol.h"
-#include "debug.h"
-#include "dpcm.h"
-#include "sddr09.h"
-
-/*
- * Transport for the Microtech DPCM-USB
- *
- */
-int dpcm_transport(struct scsi_cmnd *srb, struct us_data *us)
-{
-	int ret;
-
-	if (srb == NULL)
-		return USB_STOR_TRANSPORT_ERROR;
-
-	US_DEBUGP("dpcm_transport: LUN=%d\n", srb->device->lun);
-
-	switch (srb->device->lun) {
-		case 0:
-
-			/*
-			 * LUN 0 corresponds to the CompactFlash card reader.
-			 */
-			ret = usb_stor_CB_transport(srb, us);
-			break;
-
-#ifdef CONFIG_USB_STORAGE_SDDR09
-		case 1:
-
-			/*
-			 * LUN 1 corresponds to the SmartMedia card reader.
-			 */
-
-			/*
-			 * Set the LUN to 0 (just in case).
-			 */
-			srb->device->lun = 0; us->srb->device->lun = 0;
-			ret = sddr09_transport(srb, us);
-			srb->device->lun = 1; us->srb->device->lun = 1;
-			break;
-
-#endif
-
-		default:
-			US_DEBUGP("dpcm_transport: Invalid LUN %d\n", srb->device->lun);
-			ret = USB_STOR_TRANSPORT_ERROR;
-			break;
-	}
-	return ret;
-}
diff --git a/drivers/usb/storage/dpcm.h b/drivers/usb/storage/dpcm.h
deleted file mode 100644
index e7b7b0f120d7..000000000000
--- a/drivers/usb/storage/dpcm.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/* Driver for Microtech DPCM-USB CompactFlash/SmartMedia reader
- *
- * DPCM driver v0.1:
- *
- * First release
- *
- * Current development and maintenance by:
- *   (c) 2000 Brian Webb (webbb@earthlink.net)
- *
- * See dpcm.c for more explanation
- *
- * 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, write to the Free Software Foundation, Inc.,
- * 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-
-#ifndef _MICROTECH_DPCM_USB_H
-#define _MICROTECH_DPCM_USB_H
-
-extern int dpcm_transport(struct scsi_cmnd *srb, struct us_data *us);
-
-#endif
diff --git a/drivers/usb/storage/libusual.c b/drivers/usb/storage/libusual.c
index d617e8ae6b00..f970b27ba308 100644
--- a/drivers/usb/storage/libusual.c
+++ b/drivers/usb/storage/libusual.c
@@ -46,6 +46,12 @@ static int usu_probe_thread(void *arg);
 { USB_DEVICE_VER(id_vendor, id_product, bcdDeviceMin,bcdDeviceMax), \
   .driver_info = (flags)|(USB_US_TYPE_STOR<<24) }
 
+#define COMPLIANT_DEV(id_vendor, id_product, bcdDeviceMin, bcdDeviceMax, \
+		    vendorName, productName, useProtocol, useTransport, \
+		    initFunction, flags) \
+{ USB_DEVICE_VER(id_vendor, id_product, bcdDeviceMin, bcdDeviceMax), \
+  .driver_info = (flags) }
+
 #define USUAL_DEV(useProto, useTrans, useType) \
 { USB_INTERFACE_INFO(USB_CLASS_MASS_STORAGE, useProto, useTrans), \
   .driver_info = ((useType)<<24) }
@@ -57,6 +63,7 @@ struct usb_device_id storage_usb_ids [] = {
 
 #undef USUAL_DEV
 #undef UNUSUAL_DEV
+#undef COMPLIANT_DEV
 
 MODULE_DEVICE_TABLE(usb, storage_usb_ids);
 EXPORT_SYMBOL_GPL(storage_usb_ids);
diff --git a/drivers/usb/storage/option_ms.c b/drivers/usb/storage/option_ms.c
new file mode 100644
index 000000000000..353f922939a4
--- /dev/null
+++ b/drivers/usb/storage/option_ms.c
@@ -0,0 +1,147 @@
+/*
+ * Driver for Option High Speed Mobile Devices.
+ *
+ *   (c) 2008 Dan Williams <dcbw@redhat.com>
+ *
+ * Inspiration taken from sierra_ms.c by Kevin Lloyd <klloyd@sierrawireless.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/usb.h>
+
+#include "usb.h"
+#include "transport.h"
+#include "option_ms.h"
+#include "debug.h"
+
+#define ZCD_FORCE_MODEM			0x01
+#define ZCD_ALLOW_MS 			0x02
+
+static unsigned int option_zero_cd = ZCD_FORCE_MODEM;
+module_param(option_zero_cd, uint, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(option_zero_cd, "ZeroCD mode (1=Force Modem (default),"
+		 " 2=Allow CD-Rom");
+
+#define RESPONSE_LEN 1024
+
+static int option_rezero(struct us_data *us, int ep_in, int ep_out)
+{
+	const unsigned char rezero_msg[] = {
+	  0x55, 0x53, 0x42, 0x43, 0x78, 0x56, 0x34, 0x12,
+	  0x01, 0x00, 0x00, 0x00, 0x80, 0x00, 0x06, 0x01,
+	  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+	};
+	char *buffer;
+	int result;
+
+	US_DEBUGP("Option MS: %s", "DEVICE MODE SWITCH\n");
+
+	buffer = kzalloc(RESPONSE_LEN, GFP_KERNEL);
+	if (buffer == NULL)
+		return USB_STOR_TRANSPORT_ERROR;
+
+	memcpy(buffer, rezero_msg, sizeof (rezero_msg));
+	result = usb_stor_bulk_transfer_buf(us,
+			usb_sndbulkpipe(us->pusb_dev, ep_out),
+			buffer, sizeof (rezero_msg), NULL);
+	if (result != USB_STOR_XFER_GOOD) {
+		result = USB_STOR_XFER_ERROR;
+		goto out;
+	}
+
+	/* Some of the devices need to be asked for a response, but we don't
+	 * care what that response is.
+	 */
+	result = usb_stor_bulk_transfer_buf(us,
+			usb_sndbulkpipe(us->pusb_dev, ep_out),
+			buffer, RESPONSE_LEN, NULL);
+	result = USB_STOR_XFER_GOOD;
+
+out:
+	kfree(buffer);
+	return result;
+}
+
+int option_ms_init(struct us_data *us)
+{
+	struct usb_device *udev;
+	struct usb_interface *intf;
+	struct usb_host_interface *iface_desc;
+	struct usb_endpoint_descriptor *endpoint = NULL;
+	u8 ep_in = 0, ep_out = 0;
+	int ep_in_size = 0, ep_out_size = 0;
+	int i, result;
+
+	udev = us->pusb_dev;
+	intf = us->pusb_intf;
+
+	/* Ensure it's really a ZeroCD device; devices that are already
+	 * in modem mode return 0xFF for class, subclass, and protocol.
+	 */
+	if (udev->descriptor.bDeviceClass != 0 ||
+	    udev->descriptor.bDeviceSubClass != 0 ||
+	    udev->descriptor.bDeviceProtocol != 0)
+		return USB_STOR_TRANSPORT_GOOD;
+
+	US_DEBUGP("Option MS: option_ms_init called\n");
+
+	/* Find the right mass storage interface */
+	iface_desc = intf->cur_altsetting;
+	if (iface_desc->desc.bInterfaceClass != 0x8 ||
+	    iface_desc->desc.bInterfaceSubClass != 0x6 ||
+	    iface_desc->desc.bInterfaceProtocol != 0x50) {
+		US_DEBUGP("Option MS: mass storage interface not found, no action "
+		          "required\n");
+		return USB_STOR_TRANSPORT_GOOD;
+	}
+
+	/* Find the mass storage bulk endpoints */
+	for (i = 0; i < iface_desc->desc.bNumEndpoints && (!ep_in_size || !ep_out_size); ++i) {
+		endpoint = &iface_desc->endpoint[i].desc;
+
+		if (usb_endpoint_is_bulk_in(endpoint)) {
+			ep_in = usb_endpoint_num(endpoint);
+			ep_in_size = le16_to_cpu(endpoint->wMaxPacketSize);
+		} else if (usb_endpoint_is_bulk_out(endpoint)) {
+			ep_out = usb_endpoint_num(endpoint);
+			ep_out_size = le16_to_cpu(endpoint->wMaxPacketSize);
+		}
+	}
+
+	/* Can't find the mass storage endpoints */
+	if (!ep_in_size || !ep_out_size) {
+		US_DEBUGP("Option MS: mass storage endpoints not found, no action "
+		          "required\n");
+		return USB_STOR_TRANSPORT_GOOD;
+	}
+
+	/* Force Modem mode */
+	if (option_zero_cd == ZCD_FORCE_MODEM) {
+		US_DEBUGP("Option MS: %s", "Forcing Modem Mode\n");
+		result = option_rezero(us, ep_in, ep_out);
+		if (result != USB_STOR_XFER_GOOD)
+			US_DEBUGP("Option MS: Failed to switch to modem mode.\n");
+		return -EIO;
+	} else if (option_zero_cd == ZCD_ALLOW_MS) {
+		/* Allow Mass Storage mode (keep CD-Rom) */
+		US_DEBUGP("Option MS: %s", "Allowing Mass Storage Mode if device"
+		          " requests it\n");
+	}
+
+	return USB_STOR_TRANSPORT_GOOD;
+}
+
diff --git a/drivers/usb/storage/option_ms.h b/drivers/usb/storage/option_ms.h
new file mode 100644
index 000000000000..b6e448cab039
--- /dev/null
+++ b/drivers/usb/storage/option_ms.h
@@ -0,0 +1,4 @@
+#ifndef _OPTION_MS_H_
+#define _OPTION_MS_H_
+extern int option_ms_init(struct us_data *us);
+#endif
diff --git a/drivers/usb/storage/protocol.c b/drivers/usb/storage/protocol.c
index 3b3357e20ea7..be441d84bc64 100644
--- a/drivers/usb/storage/protocol.c
+++ b/drivers/usb/storage/protocol.c
@@ -56,9 +56,9 @@
  * Protocol routines
  ***********************************************************************/
 
-void usb_stor_qic157_command(struct scsi_cmnd *srb, struct us_data *us)
+void usb_stor_pad12_command(struct scsi_cmnd *srb, struct us_data *us)
 {
-	/* Pad the ATAPI command with zeros 
+	/* Pad the SCSI command with zeros out to 12 bytes
 	 *
 	 * NOTE: This only works because a scsi_cmnd struct field contains
 	 * a unsigned char cmnd[16], so we know we have storage available
@@ -73,26 +73,6 @@ void usb_stor_qic157_command(struct scsi_cmnd *srb, struct us_data *us)
 	usb_stor_invoke_transport(srb, us);
 }
 
-void usb_stor_ATAPI_command(struct scsi_cmnd *srb, struct us_data *us)
-{
-	/* Pad the ATAPI command with zeros 
-	 *
-	 * NOTE: This only works because a scsi_cmnd struct field contains
-	 * a unsigned char cmnd[16], so we know we have storage available
-	 */
-
-	/* Pad the ATAPI command with zeros */
-	for (; srb->cmd_len<12; srb->cmd_len++)
-		srb->cmnd[srb->cmd_len] = 0;
-
-	/* set command length to 12 bytes */
-	srb->cmd_len = 12;
-
-	/* send the command to the transport layer */
-	usb_stor_invoke_transport(srb, us);
-}
-
-
 void usb_stor_ufi_command(struct scsi_cmnd *srb, struct us_data *us)
 {
 	/* fix some commands -- this is a form of mode translation
diff --git a/drivers/usb/storage/protocol.h b/drivers/usb/storage/protocol.h
index 487056ffb516..ffc3e2af0156 100644
--- a/drivers/usb/storage/protocol.h
+++ b/drivers/usb/storage/protocol.h
@@ -40,8 +40,7 @@
 #define _PROTOCOL_H_
 
 /* Protocol handling routines */
-extern void usb_stor_ATAPI_command(struct scsi_cmnd*, struct us_data*);
-extern void usb_stor_qic157_command(struct scsi_cmnd*, struct us_data*);
+extern void usb_stor_pad12_command(struct scsi_cmnd*, struct us_data*);
 extern void usb_stor_ufi_command(struct scsi_cmnd*, struct us_data*);
 extern void usb_stor_transparent_scsi_command(struct scsi_cmnd*,
 		struct us_data*);
diff --git a/drivers/usb/storage/scsiglue.c b/drivers/usb/storage/scsiglue.c
index 09779f6a8179..2a42b862aa9f 100644
--- a/drivers/usb/storage/scsiglue.c
+++ b/drivers/usb/storage/scsiglue.c
@@ -59,6 +59,13 @@
 #include "transport.h"
 #include "protocol.h"
 
+/* Vendor IDs for companies that seem to include the READ CAPACITY bug
+ * in all their devices
+ */
+#define VENDOR_ID_NOKIA		0x0421
+#define VENDOR_ID_NIKON		0x04b0
+#define VENDOR_ID_MOTOROLA	0x22b8
+
 /***********************************************************************
  * Host functions 
  ***********************************************************************/
@@ -129,11 +136,35 @@ static int slave_configure(struct scsi_device *sdev)
 					      max_sectors);
 	}
 
+	/* Some USB host controllers can't do DMA; they have to use PIO.
+	 * They indicate this by setting their dma_mask to NULL.  For
+	 * such controllers we need to make sure the block layer sets
+	 * up bounce buffers in addressable memory.
+	 */
+	if (!us->pusb_dev->bus->controller->dma_mask)
+		blk_queue_bounce_limit(sdev->request_queue, BLK_BOUNCE_HIGH);
+
 	/* We can't put these settings in slave_alloc() because that gets
 	 * called before the device type is known.  Consequently these
 	 * settings can't be overridden via the scsi devinfo mechanism. */
 	if (sdev->type == TYPE_DISK) {
 
+		/* Some vendors seem to put the READ CAPACITY bug into
+		 * all their devices -- primarily makers of cell phones
+		 * and digital cameras.  Since these devices always use
+		 * flash media and can be expected to have an even number
+		 * of sectors, we will always enable the CAPACITY_HEURISTICS
+		 * flag unless told otherwise. */
+		switch (le16_to_cpu(us->pusb_dev->descriptor.idVendor)) {
+		case VENDOR_ID_NOKIA:
+		case VENDOR_ID_NIKON:
+		case VENDOR_ID_MOTOROLA:
+			if (!(us->fflags & (US_FL_FIX_CAPACITY |
+					US_FL_CAPACITY_OK)))
+				us->fflags |= US_FL_CAPACITY_HEURISTICS;
+			break;
+		}
+
 		/* Disk-type devices use MODE SENSE(6) if the protocol
 		 * (SubClass) is Transparent SCSI, otherwise they use
 		 * MODE SENSE(10). */
@@ -170,6 +201,10 @@ static int slave_configure(struct scsi_device *sdev)
 		if (us->fflags & US_FL_CAPACITY_HEURISTICS)
 			sdev->guess_capacity = 1;
 
+		/* assume SPC3 or latter devices support sense size > 18 */
+		if (sdev->scsi_level > SCSI_SPC_2)
+			us->fflags |= US_FL_SANE_SENSE;
+
 		/* Some devices report a SCSI revision level above 2 but are
 		 * unable to handle the REPORT LUNS command (for which
 		 * support is mandatory at level 3).  Since we already have
@@ -196,6 +231,14 @@ static int slave_configure(struct scsi_device *sdev)
 		 * sector in a larger then 1 sector read, since the performance
 		 * impact is negible we set this flag for all USB disks */
 		sdev->last_sector_bug = 1;
+
+		/* Enable last-sector hacks for single-target devices using
+		 * the Bulk-only transport, unless we already know the
+		 * capacity will be decremented or is correct. */
+		if (!(us->fflags & (US_FL_FIX_CAPACITY | US_FL_CAPACITY_OK |
+					US_FL_SCM_MULT_TARG)) &&
+				us->protocol == US_PR_BULK)
+			us->use_last_sector_hacks = 1;
 	} else {
 
 		/* Non-disk-type devices don't need to blacklist any pages
diff --git a/drivers/usb/storage/sddr09.c b/drivers/usb/storage/sddr09.c
index c5a54b872c24..531ae5c5abf3 100644
--- a/drivers/usb/storage/sddr09.c
+++ b/drivers/usb/storage/sddr09.c
@@ -45,6 +45,7 @@
 
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_device.h>
 
 #include "usb.h"
 #include "transport.h"
@@ -1446,6 +1447,48 @@ usb_stor_sddr09_dpcm_init(struct us_data *us) {
 }
 
 /*
+ * Transport for the Microtech DPCM-USB
+ */
+int dpcm_transport(struct scsi_cmnd *srb, struct us_data *us)
+{
+	int ret;
+
+	US_DEBUGP("dpcm_transport: LUN=%d\n", srb->device->lun);
+
+	switch (srb->device->lun) {
+	case 0:
+
+		/*
+		 * LUN 0 corresponds to the CompactFlash card reader.
+		 */
+		ret = usb_stor_CB_transport(srb, us);
+		break;
+
+	case 1:
+
+		/*
+		 * LUN 1 corresponds to the SmartMedia card reader.
+		 */
+
+		/*
+		 * Set the LUN to 0 (just in case).
+		 */
+		srb->device->lun = 0;
+		ret = sddr09_transport(srb, us);
+		srb->device->lun = 1;
+		break;
+
+	default:
+		US_DEBUGP("dpcm_transport: Invalid LUN %d\n",
+				srb->device->lun);
+		ret = USB_STOR_TRANSPORT_ERROR;
+		break;
+	}
+	return ret;
+}
+
+
+/*
  * Transport for the Sandisk SDDR-09
  */
 int sddr09_transport(struct scsi_cmnd *srb, struct us_data *us)
diff --git a/drivers/usb/storage/sddr09.h b/drivers/usb/storage/sddr09.h
index e50033ad7b19..b701172e12e3 100644
--- a/drivers/usb/storage/sddr09.h
+++ b/drivers/usb/storage/sddr09.h
@@ -28,8 +28,11 @@
 /* Sandisk SDDR-09 stuff */
 
 extern int sddr09_transport(struct scsi_cmnd *srb, struct us_data *us);
+extern int usb_stor_sddr09_init(struct us_data *us);
+
+/* Microtech DPCM-USB stuff */
 
+extern int dpcm_transport(struct scsi_cmnd *srb, struct us_data *us);
 extern int usb_stor_sddr09_dpcm_init(struct us_data *us);
-extern int usb_stor_sddr09_init(struct us_data *us);
 
 #endif
diff --git a/drivers/usb/storage/transport.c b/drivers/usb/storage/transport.c
index 79108d5d3171..1d5438e6363b 100644
--- a/drivers/usb/storage/transport.c
+++ b/drivers/usb/storage/transport.c
@@ -57,6 +57,9 @@
 #include "scsiglue.h"
 #include "debug.h"
 
+#include <linux/blkdev.h>
+#include "../../scsi/sd.h"
+
 
 /***********************************************************************
  * Data transfer routines
@@ -511,6 +514,110 @@ int usb_stor_bulk_transfer_sg(struct us_data* us, unsigned int pipe,
  * Transport routines
  ***********************************************************************/
 
+/* There are so many devices that report the capacity incorrectly,
+ * this routine was written to counteract some of the resulting
+ * problems.
+ */
+static void last_sector_hacks(struct us_data *us, struct scsi_cmnd *srb)
+{
+	struct gendisk *disk;
+	struct scsi_disk *sdkp;
+	u32 sector;
+
+	/* To Report "Medium Error: Record Not Found */
+	static unsigned char record_not_found[18] = {
+		[0]	= 0x70,			/* current error */
+		[2]	= MEDIUM_ERROR,		/* = 0x03 */
+		[7]	= 0x0a,			/* additional length */
+		[12]	= 0x14			/* Record Not Found */
+	};
+
+	/* If last-sector problems can't occur, whether because the
+	 * capacity was already decremented or because the device is
+	 * known to report the correct capacity, then we don't need
+	 * to do anything.
+	 */
+	if (!us->use_last_sector_hacks)
+		return;
+
+	/* Was this command a READ(10) or a WRITE(10)? */
+	if (srb->cmnd[0] != READ_10 && srb->cmnd[0] != WRITE_10)
+		goto done;
+
+	/* Did this command access the last sector? */
+	sector = (srb->cmnd[2] << 24) | (srb->cmnd[3] << 16) |
+			(srb->cmnd[4] << 8) | (srb->cmnd[5]);
+	disk = srb->request->rq_disk;
+	if (!disk)
+		goto done;
+	sdkp = scsi_disk(disk);
+	if (!sdkp)
+		goto done;
+	if (sector + 1 != sdkp->capacity)
+		goto done;
+
+	if (srb->result == SAM_STAT_GOOD && scsi_get_resid(srb) == 0) {
+
+		/* The command succeeded.  If the capacity is odd
+		 * (i.e., if the sector number is even) then the
+		 * "always-even" heuristic would be wrong for this
+		 * device.  Issue a WARN() so that the kerneloops.org
+		 * project will be notified and we will then know to
+		 * mark the device with a CAPACITY_OK flag.  Hopefully
+		 * this will occur for only a few devices.
+		 *
+		 * Use the sign of us->last_sector_hacks to tell whether
+		 * the warning has already been issued; we don't need
+		 * more than one warning per device.
+		 */
+		if (!(sector & 1) && us->use_last_sector_hacks > 0) {
+			unsigned vid = le16_to_cpu(
+					us->pusb_dev->descriptor.idVendor);
+			unsigned pid = le16_to_cpu(
+					us->pusb_dev->descriptor.idProduct);
+			unsigned rev = le16_to_cpu(
+					us->pusb_dev->descriptor.bcdDevice);
+
+			WARN(1, "%s: Successful last sector success at %u, "
+					"device %04x:%04x:%04x\n",
+					sdkp->disk->disk_name, sector,
+					vid, pid, rev);
+			us->use_last_sector_hacks = -1;
+		}
+
+	} else {
+		/* The command failed.  Allow up to 3 retries in case this
+		 * is some normal sort of failure.  After that, assume the
+		 * capacity is wrong and we're trying to access the sector
+		 * beyond the end.  Replace the result code and sense data
+		 * with values that will cause the SCSI core to fail the
+		 * command immediately, instead of going into an infinite
+		 * (or even just a very long) retry loop.
+		 */
+		if (++us->last_sector_retries < 3)
+			return;
+		srb->result = SAM_STAT_CHECK_CONDITION;
+		memcpy(srb->sense_buffer, record_not_found,
+				sizeof(record_not_found));
+
+		/* In theory we might want to issue a WARN() here if the
+		 * capacity is even, since it could indicate the device
+		 * has the READ CAPACITY bug _and_ the real capacity is
+		 * odd.  But it could also indicate that the device
+		 * simply can't access its last sector, a failure mode
+		 * which is surprisingly common.  So no warning.
+		 */
+	}
+
+ done:
+	/* Don't reset the retry counter for TEST UNIT READY commands,
+	 * because they get issued after device resets which might be
+	 * caused by a failed last-sector access.
+	 */
+	if (srb->cmnd[0] != TEST_UNIT_READY)
+		us->last_sector_retries = 0;
+}
+
 /* Invoke the transport and basic error-handling/recovery methods
  *
  * This is used by the protocol layers to actually send the message to
@@ -544,6 +651,7 @@ void usb_stor_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 	/* if the transport provided its own sense data, don't auto-sense */
 	if (result == USB_STOR_TRANSPORT_NO_SENSE) {
 		srb->result = SAM_STAT_CHECK_CONDITION;
+		last_sector_hacks(us, srb);
 		return;
 	}
 
@@ -579,6 +687,20 @@ void usb_stor_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 	}
 
 	/*
+	 * Determine if this device is SAT by seeing if the
+	 * command executed successfully.  Otherwise we'll have
+	 * to wait for at least one CHECK_CONDITION to determine
+	 * SANE_SENSE support
+	 */
+	if ((srb->cmnd[0] == ATA_16 || srb->cmnd[0] == ATA_12) &&
+	    result == USB_STOR_TRANSPORT_GOOD &&
+	    !(us->fflags & US_FL_SANE_SENSE) &&
+	    !(srb->cmnd[2] & 0x20)) {
+		US_DEBUGP("-- SAT supported, increasing auto-sense\n");
+		us->fflags |= US_FL_SANE_SENSE;
+	}
+
+	/*
 	 * A short transfer on a command where we don't expect it
 	 * is unusual, but it doesn't mean we need to auto-sense.
 	 */
@@ -595,10 +717,15 @@ void usb_stor_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 	if (need_auto_sense) {
 		int temp_result;
 		struct scsi_eh_save ses;
+		int sense_size = US_SENSE_SIZE;
+
+		/* device supports and needs bigger sense buffer */
+		if (us->fflags & US_FL_SANE_SENSE)
+			sense_size = ~0;
 
 		US_DEBUGP("Issuing auto-REQUEST_SENSE\n");
 
-		scsi_eh_prep_cmnd(srb, &ses, NULL, 0, US_SENSE_SIZE);
+		scsi_eh_prep_cmnd(srb, &ses, NULL, 0, sense_size);
 
 		/* FIXME: we must do the protocol translation here */
 		if (us->subclass == US_SC_RBC || us->subclass == US_SC_SCSI ||
@@ -632,6 +759,25 @@ void usb_stor_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 			return;
 		}
 
+		/* If the sense data returned is larger than 18-bytes then we
+		 * assume this device supports requesting more in the future.
+		 * The response code must be 70h through 73h inclusive.
+		 */
+		if (srb->sense_buffer[7] > (US_SENSE_SIZE - 8) &&
+		    !(us->fflags & US_FL_SANE_SENSE) &&
+		    (srb->sense_buffer[0] & 0x7C) == 0x70) {
+			US_DEBUGP("-- SANE_SENSE support enabled\n");
+			us->fflags |= US_FL_SANE_SENSE;
+
+			/* Indicate to the user that we truncated their sense
+			 * because we didn't know it supported larger sense.
+			 */
+			US_DEBUGP("-- Sense data truncated to %i from %i\n",
+			          US_SENSE_SIZE,
+			          srb->sense_buffer[7] + 8);
+			srb->sense_buffer[7] = (US_SENSE_SIZE - 8);
+		}
+
 		US_DEBUGP("-- Result from auto-sense is %d\n", temp_result);
 		US_DEBUGP("-- code: 0x%x, key: 0x%x, ASC: 0x%x, ASCQ: 0x%x\n",
 			  srb->sense_buffer[0],
@@ -667,6 +813,7 @@ void usb_stor_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 			scsi_bufflen(srb) - scsi_get_resid(srb) < srb->underflow)
 		srb->result = (DID_ERROR << 16) | (SUGGEST_RETRY << 24);
 
+	last_sector_hacks(us, srb);
 	return;
 
 	/* Error and abort processing: try to resynchronize with the device
@@ -694,6 +841,7 @@ void usb_stor_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 		us->transport_reset(us);
 	}
 	clear_bit(US_FLIDX_RESETTING, &us->dflags);
+	last_sector_hacks(us, srb);
 }
 
 /* Stop the current URB transfer */
@@ -718,10 +866,10 @@ void usb_stor_stop_transport(struct us_data *us)
 }
 
 /*
- * Control/Bulk/Interrupt transport
+ * Control/Bulk and Control/Bulk/Interrupt transport
  */
 
-int usb_stor_CBI_transport(struct scsi_cmnd *srb, struct us_data *us)
+int usb_stor_CB_transport(struct scsi_cmnd *srb, struct us_data *us)
 {
 	unsigned int transfer_length = scsi_bufflen(srb);
 	unsigned int pipe = 0;
@@ -763,6 +911,13 @@ int usb_stor_CBI_transport(struct scsi_cmnd *srb, struct us_data *us)
 	}
 
 	/* STATUS STAGE */
+
+	/* NOTE: CB does not have a status stage.  Silly, I know.  So
+	 * we have to catch this at a higher level.
+	 */
+	if (us->protocol != US_PR_CBI)
+		return USB_STOR_TRANSPORT_GOOD;
+
 	result = usb_stor_intr_transfer(us, us->iobuf, 2);
 	US_DEBUGP("Got interrupt data (0x%x, 0x%x)\n", 
 			us->iobuf[0], us->iobuf[1]);
@@ -817,56 +972,6 @@ int usb_stor_CBI_transport(struct scsi_cmnd *srb, struct us_data *us)
 }
 
 /*
- * Control/Bulk transport
- */
-int usb_stor_CB_transport(struct scsi_cmnd *srb, struct us_data *us)
-{
-	unsigned int transfer_length = scsi_bufflen(srb);
-	int result;
-
-	/* COMMAND STAGE */
-	/* let's send the command via the control pipe */
-	result = usb_stor_ctrl_transfer(us, us->send_ctrl_pipe,
-				      US_CBI_ADSC, 
-				      USB_TYPE_CLASS | USB_RECIP_INTERFACE, 0, 
-				      us->ifnum, srb->cmnd, srb->cmd_len);
-
-	/* check the return code for the command */
-	US_DEBUGP("Call to usb_stor_ctrl_transfer() returned %d\n", result);
-
-	/* if we stalled the command, it means command failed */
-	if (result == USB_STOR_XFER_STALLED) {
-		return USB_STOR_TRANSPORT_FAILED;
-	}
-
-	/* Uh oh... serious problem here */
-	if (result != USB_STOR_XFER_GOOD) {
-		return USB_STOR_TRANSPORT_ERROR;
-	}
-
-	/* DATA STAGE */
-	/* transfer the data payload for this command, if one exists*/
-	if (transfer_length) {
-		unsigned int pipe = srb->sc_data_direction == DMA_FROM_DEVICE ? 
-				us->recv_bulk_pipe : us->send_bulk_pipe;
-		result = usb_stor_bulk_srb(us, pipe, srb);
-		US_DEBUGP("CB data stage result is 0x%x\n", result);
-
-		/* if we stalled the data transfer it means command failed */
-		if (result == USB_STOR_XFER_STALLED)
-			return USB_STOR_TRANSPORT_FAILED;
-		if (result > USB_STOR_XFER_STALLED)
-			return USB_STOR_TRANSPORT_ERROR;
-	}
-
-	/* STATUS STAGE */
-	/* NOTE: CB does not have a status stage.  Silly, I know.  So
-	 * we have to catch this at a higher level.
-	 */
-	return USB_STOR_TRANSPORT_GOOD;
-}
-
-/*
  * Bulk only transport
  */
 
@@ -1173,10 +1278,9 @@ int usb_stor_Bulk_reset(struct us_data *us)
  */
 int usb_stor_port_reset(struct us_data *us)
 {
-	int result, rc_lock;
+	int result;
 
-	result = rc_lock =
-		usb_lock_device_for_reset(us->pusb_dev, us->pusb_intf);
+	result = usb_lock_device_for_reset(us->pusb_dev, us->pusb_intf);
 	if (result < 0)
 		US_DEBUGP("unable to lock device for reset: %d\n", result);
 	else {
@@ -1189,8 +1293,7 @@ int usb_stor_port_reset(struct us_data *us)
 			US_DEBUGP("usb_reset_device returns %d\n",
 					result);
 		}
-		if (rc_lock)
-			usb_unlock_device(us->pusb_dev);
+		usb_unlock_device(us->pusb_dev);
 	}
 	return result;
 }
diff --git a/drivers/usb/storage/transport.h b/drivers/usb/storage/transport.h
index e70b88182f0e..242ff5e791a5 100644
--- a/drivers/usb/storage/transport.h
+++ b/drivers/usb/storage/transport.h
@@ -113,8 +113,6 @@ struct bulk_cs_wrap {
 
 #define US_CBI_ADSC		0
 
-extern int usb_stor_CBI_transport(struct scsi_cmnd *, struct us_data*);
-
 extern int usb_stor_CB_transport(struct scsi_cmnd *, struct us_data*);
 extern int usb_stor_CB_reset(struct us_data*);
 
diff --git a/drivers/usb/storage/unusual_devs.h b/drivers/usb/storage/unusual_devs.h
index bfcc1fe82518..a7f9513fa19d 100644
--- a/drivers/usb/storage/unusual_devs.h
+++ b/drivers/usb/storage/unusual_devs.h
@@ -27,7 +27,8 @@
 
 /* IMPORTANT NOTE: This file must be included in another file which does
  * the following thing for it to work:
- * The macro UNUSUAL_DEV() must be defined before this file is included
+ * The UNUSUAL_DEV, COMPLIANT_DEV, and USUAL_DEV macros must be defined
+ * before this file is included.
  */
 
 /* If you edit this file, please try to keep it sorted first by VendorID,
@@ -46,6 +47,12 @@
  * <usb-storage@lists.one-eyed-alien.net>
  */
 
+/* Note: If you add an entry only in order to set the CAPACITY_OK flag,
+ * use the COMPLIANT_DEV macro instead of UNUSUAL_DEV.  This is
+ * because such entries mark devices which actually work correctly,
+ * as opposed to devices that do something strangely or wrongly.
+ */
+
 /* patch submitted by Vivian Bregier <Vivian.Bregier@imag.fr>
  */
 UNUSUAL_DEV(  0x03eb, 0x2002, 0x0100, 0x0100,
@@ -85,6 +92,13 @@ UNUSUAL_DEV(  0x03f0, 0x0307, 0x0001, 0x0001,
 		US_SC_8070, US_PR_USBAT, init_usbat_cd, 0),
 #endif
 
+/* Reported by Ben Efros <ben@pc-doctor.com> */
+UNUSUAL_DEV(  0x03f0, 0x070c, 0x0000, 0x0000,
+		"HP",
+		"Personal Media Drive",
+		US_SC_DEVICE, US_PR_DEVICE, NULL,
+		US_FL_SANE_SENSE ),
+
 /* Reported by Grant Grundler <grundler@parisc-linux.org>
  * HP r707 camera in "Disk" mode with 2.00.23 or 2.00.24 firmware.
  */
@@ -160,34 +174,6 @@ UNUSUAL_DEV(  0x0421, 0x0019, 0x0592, 0x0592,
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
 		US_FL_MAX_SECTORS_64 ),
 
-/* Reported by Filip Joelsson <filip@blueturtle.nu> */
-UNUSUAL_DEV(  0x0421, 0x005d, 0x0001, 0x0600,
-		"Nokia",
-		"Nokia 3110c",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY ),
-
-/* Reported by Ozan Sener <themgzzy@gmail.com> */
-UNUSUAL_DEV(  0x0421, 0x0060, 0x0551, 0x0551,
-		"Nokia",
-		"3500c",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY ),
-
-/* Reported by CSECSY Laszlo <boobaa@frugalware.org> */
-UNUSUAL_DEV(  0x0421, 0x0063, 0x0001, 0x0601,
-		"Nokia",
-		"Nokia 3109c",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY ),
-
-/* Patch for Nokia 5310 capacity */
-UNUSUAL_DEV(  0x0421, 0x006a, 0x0000, 0x0701,
-		"Nokia",
-		"5310",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY ),
-
 /* Reported by Mario Rettig <mariorettig@web.de> */
 UNUSUAL_DEV(  0x0421, 0x042e, 0x0100, 0x0100,
 		"Nokia",
@@ -253,35 +239,6 @@ UNUSUAL_DEV(  0x0421, 0x0495, 0x0370, 0x0370,
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
 		US_FL_MAX_SECTORS_64 ),
 
-/* Reported by Cedric Godin <cedric@belbone.be> */
-UNUSUAL_DEV(  0x0421, 0x04b9, 0x0500, 0x0551,
-		"Nokia",
-		"5300",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY ),
-
-/* Reported by Richard Nauber <RichardNauber@web.de> */
-UNUSUAL_DEV(  0x0421, 0x04fa, 0x0550, 0x0660,
-		"Nokia",
-		"6300",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY ),
-
-/* Patch for Nokia 5310 capacity */
-UNUSUAL_DEV(  0x0421, 0x006a, 0x0000, 0x0591,
-	"Nokia",
-	"5310",
-	US_SC_DEVICE, US_PR_DEVICE, NULL,
-	US_FL_FIX_CAPACITY ),
-
-/* Submitted by Ricky Wong Yung Fei <evilbladewarrior@gmail.com> */
-/* Nokia 7610 Supernova - Too many sectors reported in usb storage mode */
-UNUSUAL_DEV(  0x0421, 0x00f5, 0x0000, 0x0470,
-	"Nokia",
-	"7610 Supernova",
-	US_SC_DEVICE, US_PR_DEVICE, NULL,
-	US_FL_FIX_CAPACITY ),
-
 /* Reported by Olaf Hering <olh@suse.de> from novell bug #105878 */
 UNUSUAL_DEV(  0x0424, 0x0fdc, 0x0210, 0x0210,
 		"SMSC",
@@ -289,11 +246,17 @@ UNUSUAL_DEV(  0x0424, 0x0fdc, 0x0210, 0x0210,
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
 		US_FL_SINGLE_LUN ),
 
-#ifdef CONFIG_USB_STORAGE_DPCM
+#ifdef CONFIG_USB_STORAGE_SDDR09
 UNUSUAL_DEV(  0x0436, 0x0005, 0x0100, 0x0100,
 		"Microtech",
 		"CameraMate (DPCM_USB)",
  		US_SC_SCSI, US_PR_DPCM_USB, NULL, 0 ),
+#else
+UNUSUAL_DEV(  0x0436, 0x0005, 0x0100, 0x0100,
+		"Microtech",
+		"CameraMate",
+		US_SC_SCSI, US_PR_CB, NULL,
+		US_FL_SINGLE_LUN ),
 #endif
 
 /* Patch submitted by Daniel Drake <dsd@gentoo.org>
@@ -388,6 +351,15 @@ UNUSUAL_DEV(  0x04a4, 0x0004, 0x0001, 0x0001,
 		"DVD-CAM DZ-MV100A Camcorder",
 		US_SC_SCSI, US_PR_CB, NULL, US_FL_SINGLE_LUN),
 
+/* BENQ DC5330
+ * Reported by Manuel Fombuena <mfombuena@ya.com> and
+ * Frank Copeland <fjc@thingy.apana.org.au> */
+UNUSUAL_DEV(  0x04a5, 0x3010, 0x0100, 0x0100,
+		"Tekom Technologies, Inc",
+		"300_CAMERA",
+		US_SC_DEVICE, US_PR_DEVICE, NULL,
+		US_FL_IGNORE_RESIDUE ),
+
 /* Patch for Nikon coolpix 2000
  * Submitted by Fabien Cosse <fabien.cosse@wanadoo.fr>*/
 UNUSUAL_DEV(  0x04b0, 0x0301, 0x0010, 0x0010,
@@ -396,83 +368,6 @@ UNUSUAL_DEV(  0x04b0, 0x0301, 0x0010, 0x0010,
 		US_SC_DEVICE, US_PR_DEVICE,NULL,
 		US_FL_NOT_LOCKABLE ),
 
-/* Reported by Stefan de Konink <skinkie@xs4all.nl> */
-UNUSUAL_DEV(  0x04b0, 0x0401, 0x0200, 0x0200,
-		"NIKON",
-		"NIKON DSC D100",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY),
-
-/* Reported by Tobias Kunze Briseno <t-linux@fictive.com> */
-UNUSUAL_DEV(  0x04b0, 0x0403, 0x0200, 0x0200,
-		"NIKON",
-		"NIKON DSC D2H",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY),
-
-/* Reported by Milinevsky Dmitry <niam.niam@gmail.com> */
-UNUSUAL_DEV(  0x04b0, 0x0409, 0x0100, 0x0100,
-		"NIKON",
-		"NIKON DSC D50",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY),
-
-/* Reported by Andreas Bockhold <andreas@bockionline.de> */
-UNUSUAL_DEV(  0x04b0, 0x0405, 0x0100, 0x0100,
-		"NIKON",
-		"NIKON DSC D70",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY),
-
-/* Reported by Jamie Kitson <jamie@staberinde.fsnet.co.uk> */
-UNUSUAL_DEV(  0x04b0, 0x040d, 0x0100, 0x0100,
-		"NIKON",
-		"NIKON DSC D70s",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY),
-
-/* Reported by Graber and Mike Pagano <mpagano-kernel@mpagano.com> */
-UNUSUAL_DEV(  0x04b0, 0x040f, 0x0100, 0x0200,
-		"NIKON",
-		"NIKON DSC D200",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY),
-
-/* Reported by Emil Larsson <emil@swip.net> */
-UNUSUAL_DEV(  0x04b0, 0x0411, 0x0100, 0x0111,
-		"NIKON",
-		"NIKON DSC D80",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY),
-
-/* Reported by Ortwin Glueck <odi@odi.ch> */
-UNUSUAL_DEV(  0x04b0, 0x0413, 0x0110, 0x0111,
-		"NIKON",
-		"NIKON DSC D40",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY),
-
-/* Reported by Paul Check <paul@openstreet.com> */
-UNUSUAL_DEV(  0x04b0, 0x0415, 0x0100, 0x0100,
-		"NIKON",
-		"NIKON DSC D2Xs",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY),
-
-/* Reported by Shan Destromp (shansan@gmail.com) */
-UNUSUAL_DEV(  0x04b0, 0x0417, 0x0100, 0x0100,
-		"NIKON",
-		"NIKON DSC D40X",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY),
-
-/* Reported by paul ready <lxtwin@homecall.co.uk> */
-UNUSUAL_DEV(  0x04b0, 0x0419, 0x0100, 0x0200,
-		"NIKON",
-		"NIKON DSC D300",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY),
-
 /* Reported by Doug Maxey (dwm@austin.ibm.com) */
 UNUSUAL_DEV(  0x04b3, 0x4001, 0x0110, 0x0110,
 		"IBM",
@@ -480,15 +375,6 @@ UNUSUAL_DEV(  0x04b3, 0x4001, 0x0110, 0x0110,
 		US_SC_DEVICE, US_PR_CB, NULL,
 		US_FL_MAX_SECTORS_MIN),
 
-/* BENQ DC5330
- * Reported by Manuel Fombuena <mfombuena@ya.com> and
- * Frank Copeland <fjc@thingy.apana.org.au> */
-UNUSUAL_DEV(  0x04a5, 0x3010, 0x0100, 0x0100,
-		"Tekom Technologies, Inc",
-		"300_CAMERA",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_IGNORE_RESIDUE ),
-
 #ifdef CONFIG_USB_STORAGE_CYPRESS_ATACB
 /* CY7C68300 : support atacb */
 UNUSUAL_DEV(  0x04b4, 0x6830, 0x0000, 0x9999,
@@ -594,6 +480,12 @@ UNUSUAL_DEV(  0x04e6, 0x0005, 0x0100, 0x0208,
 		"eUSB SmartMedia / CompactFlash Adapter",
 		US_SC_SCSI, US_PR_DPCM_USB, usb_stor_sddr09_dpcm_init,
 		0),
+#else
+UNUSUAL_DEV(  0x04e6, 0x0005, 0x0100, 0x0208,
+		"SCM Microsystems",
+		"eUSB CompactFlash Adapter",
+		US_SC_SCSI, US_PR_CB, NULL,
+		US_FL_SINGLE_LUN),
 #endif
 
 /* Reported by Markus Demleitner <msdemlei@cl.uni-heidelberg.de> */
@@ -685,6 +577,13 @@ UNUSUAL_DEV(  0x0525, 0xa140, 0x0100, 0x0100,
 		US_SC_8070, US_PR_DEVICE, NULL,
 		US_FL_FIX_INQUIRY ),
 
+/* Added by Alan Stern <stern@rowland.harvard.edu> */
+COMPLIANT_DEV(0x0525, 0xa4a5, 0x0000, 0x9999,
+		"Linux",
+		"File-backed Storage Gadget",
+		US_SC_DEVICE, US_PR_DEVICE, NULL,
+		US_FL_CAPACITY_OK ),
+
 /* Yakumo Mega Image 37
  * Submitted by Stephan Fuhrmann <atomenergie@t-online.de> */
 UNUSUAL_DEV(  0x052b, 0x1801, 0x0100, 0x0100,
@@ -807,15 +706,15 @@ UNUSUAL_DEV(  0x054c, 0x006d, 0x0000, 0x9999,
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
 		US_FL_FIX_INQUIRY ),
 
-/* Submitted by Mike Alborn <malborn@deandra.homeip.net> */
-UNUSUAL_DEV(  0x054c, 0x016a, 0x0000, 0x9999,
+/* Submitted by Frank Engel <frankie@cse.unsw.edu.au> */
+UNUSUAL_DEV(  0x054c, 0x0099, 0x0000, 0x9999,
 		"Sony",
 		"PEG Mass Storage",
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
 		US_FL_FIX_INQUIRY ),
-		
-/* Submitted by Frank Engel <frankie@cse.unsw.edu.au> */
-UNUSUAL_DEV(  0x054c, 0x0099, 0x0000, 0x9999,
+
+/* Submitted by Mike Alborn <malborn@deandra.homeip.net> */
+UNUSUAL_DEV(  0x054c, 0x016a, 0x0000, 0x9999,
 		"Sony",
 		"PEG Mass Storage",
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
@@ -966,6 +865,18 @@ UNUSUAL_DEV( 0x05ac, 0x120a, 0x0000, 0x9999,
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
 		US_FL_FIX_CAPACITY ),
 
+/* Reported by Dan Williams <dcbw@redhat.com>
+ * Option N.V. mobile broadband modems
+ * Ignore driver CD mode and force into modem mode by default.
+ */
+
+/* Globetrotter HSDPA; mass storage shows up as Qualcomm for vendor */
+UNUSUAL_DEV(  0x05c6, 0x1000, 0x0000, 0x9999,
+		"Option N.V.",
+		"Mass Storage",
+		US_SC_DEVICE, US_PR_DEVICE, option_ms_init,
+		0),
+
 #ifdef CONFIG_USB_STORAGE_JUMPSHOT
 UNUSUAL_DEV(  0x05dc, 0x0001, 0x0000, 0x0001,
 		"Lexar",
@@ -1004,6 +915,13 @@ UNUSUAL_DEV(  0x05e3, 0x0702, 0x0000, 0xffff,
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
 		US_FL_GO_SLOW | US_FL_MAX_SECTORS_64 ),
 
+/* Reported by Ben Efros <ben@pc-doctor.com> */
+UNUSUAL_DEV(  0x05e3, 0x0723, 0x9451, 0x9451,
+		"Genesys Logic",
+		"USB to SATA",
+		US_SC_DEVICE, US_PR_DEVICE, NULL,
+		US_FL_SANE_SENSE ),
+
 /* Reported by Hanno Boeck <hanno@gmx.de>
  * Taken from the Lycoris Kernel */
 UNUSUAL_DEV(  0x0636, 0x0003, 0x0000, 0x9999,
@@ -1040,7 +958,7 @@ UNUSUAL_DEV( 0x067b, 0x2507, 0x0100, 0x0100,
 		US_FL_FIX_CAPACITY | US_FL_GO_SLOW ),
 
 /* Reported by Alex Butcher <alex.butcher@assursys.co.uk> */
-UNUSUAL_DEV( 0x067b, 0x3507, 0x0001, 0x0001,
+UNUSUAL_DEV( 0x067b, 0x3507, 0x0001, 0x0101,
 		"Prolific Technology Inc.",
 		"ATAPI-6 Bridge Controller",
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
@@ -1161,11 +1079,17 @@ UNUSUAL_DEV(  0x07af, 0x0005, 0x0100, 0x0100,
 		US_SC_DEVICE, US_PR_DEVICE, usb_stor_euscsi_init,
 		US_FL_SCM_MULT_TARG ),
 
-#ifdef CONFIG_USB_STORAGE_DPCM
+#ifdef CONFIG_USB_STORAGE_SDDR09
 UNUSUAL_DEV(  0x07af, 0x0006, 0x0100, 0x0100,
 		"Microtech",
 		"CameraMate (DPCM_USB)",
  		US_SC_SCSI, US_PR_DPCM_USB, NULL, 0 ),
+#else
+UNUSUAL_DEV(  0x07af, 0x0006, 0x0100, 0x0100,
+		"Microtech",
+		"CameraMate",
+		US_SC_SCSI, US_PR_CB, NULL,
+		US_FL_SINGLE_LUN ),
 #endif
 
 #ifdef CONFIG_USB_STORAGE_ALAUDA
@@ -1320,6 +1244,13 @@ UNUSUAL_DEV( 0x0840, 0x0082, 0x0001, 0x0001,
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
 		US_FL_FIX_CAPACITY),
 
+/* Reported and patched by Nguyen Anh Quynh <aquynh@gmail.com> */
+UNUSUAL_DEV( 0x0840, 0x0084, 0x0001, 0x0001,
+		"Argosy",
+		"Storage",
+		US_SC_DEVICE, US_PR_DEVICE, NULL,
+		US_FL_FIX_CAPACITY),
+
 /* Entry and supporting patch by Theodore Kilgore <kilgota@auburn.edu>.
  * Flag will support Bulk devices which use a standards-violating 32-byte
  * Command Block Wrapper. Here, the "DC2MEGA" cameras (several brands) with
@@ -1343,17 +1274,6 @@ UNUSUAL_DEV(  0x0851, 0x1543, 0x0200, 0x0200,
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
 		US_FL_NOT_LOCKABLE),
 
-/* Andrew Lunn <andrew@lunn.ch>
- * PanDigital Digital Picture Frame. Does not like ALLOW_MEDIUM_REMOVAL
- * on LUN 4.
- * Note: Vend:Prod clash with "Ltd Maxell WS30 Slim Digital Camera"
-*/
-UNUSUAL_DEV(  0x0851, 0x1543, 0x0200, 0x0200,
-		"PanDigital",
-		"Photo Frame",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_NOT_LOCKABLE),
-
 /* Submitted by Jan De Luyck <lkml@kcore.org> */
 UNUSUAL_DEV(  0x08bd, 0x1100, 0x0000, 0x0000,
 		"CITIZEN",
@@ -1425,6 +1345,13 @@ UNUSUAL_DEV( 0x0a17, 0x006, 0x0000, 0xffff,
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
 		US_FL_FIX_INQUIRY ),
 
+/* Reported by Jaak Ristioja <Ristioja@gmail.com> */
+UNUSUAL_DEV( 0x0a17, 0x006e, 0x0100, 0x0100,
+		"Pentax",
+		"K10D",
+		US_SC_DEVICE, US_PR_DEVICE, NULL,
+		US_FL_FIX_CAPACITY ),
+
 /* These are virtual windows driver CDs, which the zd1211rw driver
  * automatically converts into WLAN devices. */
 UNUSUAL_DEV( 0x0ace, 0x2011, 0x0101, 0x0101,
@@ -1439,6 +1366,18 @@ UNUSUAL_DEV( 0x0ace, 0x20ff, 0x0101, 0x0101,
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
 		US_FL_IGNORE_DEVICE ),
 
+/* Reported by Dan Williams <dcbw@redhat.com>
+ * Option N.V. mobile broadband modems
+ * Ignore driver CD mode and force into modem mode by default.
+ */
+
+/* iCON 225 */
+UNUSUAL_DEV(  0x0af0, 0x6971, 0x0000, 0x9999,
+		"Option N.V.",
+		"Mass Storage",
+		US_SC_DEVICE, US_PR_DEVICE, option_ms_init,
+		0),
+
 /* Reported by F. Aben <f.aben@option.com>
  * This device (wrongly) has a vendor-specific device descriptor.
  * The entry is needed so usb-storage can bind to it's mass-storage
@@ -1449,6 +1388,13 @@ UNUSUAL_DEV( 0x0af0, 0x7401, 0x0000, 0x0000,
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
 		0 ),
 
+/* Reported by Ben Efros <ben@pc-doctor.com> */
+UNUSUAL_DEV( 0x0bc2, 0x3010, 0x0000, 0x0000,
+		"Seagate",
+		"FreeAgent Pro",
+		US_SC_DEVICE, US_PR_DEVICE, NULL,
+		US_FL_SANE_SENSE ),
+
 #ifdef CONFIG_USB_STORAGE_ISD200
 UNUSUAL_DEV(  0x0bf6, 0xa001, 0x0100, 0x0110,
 		"ATI",
@@ -1472,6 +1418,22 @@ UNUSUAL_DEV( 0x0c0b, 0xa109, 0x0000, 0xffff,
 		US_FL_SINGLE_LUN ),
 #endif
 
+UNUSUAL_DEV(  0x0d49, 0x7310, 0x0000, 0x9999,
+		"Maxtor",
+		"USB to SATA",
+		US_SC_DEVICE, US_PR_DEVICE, NULL,
+		US_FL_SANE_SENSE),
+
+/*
+ * Pete Zaitcev <zaitcev@yahoo.com>, bz#164688.
+ * The device blatantly ignores LUN and returns 1 in GetMaxLUN.
+ */
+UNUSUAL_DEV( 0x0c45, 0x1060, 0x0100, 0x0100,
+		"Unknown",
+		"Unknown",
+		US_SC_DEVICE, US_PR_DEVICE, NULL,
+		US_FL_SINGLE_LUN ),
+
 /* Submitted by: Nick Sillik <n.sillik@temple.edu>
  * Needed for OneTouch extension to usb-storage
  *
@@ -1489,16 +1451,6 @@ UNUSUAL_DEV( 0x0c0b, 0xa109, 0x0000, 0xffff,
 			0),
 #endif
 
-/*
- * Pete Zaitcev <zaitcev@yahoo.com>, bz#164688.
- * The device blatantly ignores LUN and returns 1 in GetMaxLUN.
- */
-UNUSUAL_DEV( 0x0c45, 0x1060, 0x0100, 0x0100,
-		"Unknown",
-		"Unknown",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_SINGLE_LUN ),
-
 /* Submitted by Joris Struyve <joris@struyve.be> */
 UNUSUAL_DEV( 0x0d96, 0x410a, 0x0001, 0xffff,
 		"Medion",
@@ -1516,6 +1468,13 @@ UNUSUAL_DEV(  0x0d96, 0x5200, 0x0001, 0x0200,
 		"JD 5200 z3",
 		US_SC_DEVICE, US_PR_DEVICE, NULL, US_FL_FIX_INQUIRY),
 
+/* Reported by  Jason Johnston <killean@shaw.ca> */
+UNUSUAL_DEV(  0x0dc4, 0x0073, 0x0000, 0x0000,
+		"Macpower Technology Co.LTD.",
+		"USB 2.0 3.5\" DEVICE",
+		US_SC_DEVICE, US_PR_DEVICE, NULL,
+		US_FL_FIX_CAPACITY),
+
 /* Reported by Lubomir Blaha <tritol@trilogic.cz>
  * I _REALLY_ don't know what 3rd, 4th number and all defines mean, but this
  * works for me. Can anybody correct these values? (I able to test corrected
@@ -1638,13 +1597,6 @@ UNUSUAL_DEV(  0x0fce, 0xe030, 0x0000, 0x0000,
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
 		US_FL_FIX_CAPACITY | US_FL_IGNORE_RESIDUE ),
 
-/* Reported by Ricardo Barberis <ricardo@dattatec.com> */
-UNUSUAL_DEV(  0x0fce, 0xe092, 0x0000, 0x0000,
-		"Sony Ericsson",
-		"P1i",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_IGNORE_RESIDUE ),
-
 /* Reported by Emmanuel Vasilakis <evas@forthnet.gr> */
 UNUSUAL_DEV(  0x0fce, 0xe031, 0x0000, 0x0000,
 		"Sony Ericsson",
@@ -1652,6 +1604,13 @@ UNUSUAL_DEV(  0x0fce, 0xe031, 0x0000, 0x0000,
 		US_SC_DEVICE, US_PR_DEVICE, NULL,
 		US_FL_IGNORE_RESIDUE | US_FL_FIX_CAPACITY ),
 
+/* Reported by Ricardo Barberis <ricardo@dattatec.com> */
+UNUSUAL_DEV(  0x0fce, 0xe092, 0x0000, 0x0000,
+		"Sony Ericsson",
+		"P1i",
+		US_SC_DEVICE, US_PR_DEVICE, NULL,
+		US_FL_IGNORE_RESIDUE ),
+
 /* Reported by Kevin Cernekee <kpc-usbdev@gelato.uiuc.edu>
  * Tested on hardware version 1.10.
  * Entry is needed only for the initializer function override.
@@ -1664,6 +1623,12 @@ UNUSUAL_DEV(  0x1019, 0x0c55, 0x0000, 0x0110,
 		US_SC_DEVICE, US_PR_DEVICE, usb_stor_ucr61s2b_init,
 		0 ),
 
+UNUSUAL_DEV(  0x1058, 0x0704, 0x0000, 0x9999,
+		"Western Digital",
+		"External HDD",
+		US_SC_DEVICE, US_PR_DEVICE, NULL,
+		US_FL_SANE_SENSE),
+
 /* Reported by Fabio Venturi <f.venturi@tdnet.it>
  * The device reports a vendor-specific bDeviceClass.
  */
@@ -2053,10 +2018,10 @@ UNUSUAL_DEV(  0x14cd, 0x6600, 0x0201, 0x0201,
  * JMicron responds to USN and several other SCSI ioctls with a
  * residue that causes subsequent I/O requests to fail.  */
 UNUSUAL_DEV(  0x152d, 0x2329, 0x0100, 0x0100,
-	        "JMicron",
-	        "USB to ATA/ATAPI Bridge",
-	        US_SC_DEVICE, US_PR_DEVICE, NULL,
-	        US_FL_IGNORE_RESIDUE ),
+		"JMicron",
+		"USB to ATA/ATAPI Bridge",
+		US_SC_DEVICE, US_PR_DEVICE, NULL,
+		US_FL_IGNORE_RESIDUE | US_FL_SANE_SENSE ),
 
 /* Reported by Robert Schedel <r.schedel@yahoo.de>
  * Note: this is a 'super top' device like the above 14cd/6600 device */
@@ -2086,27 +2051,6 @@ UNUSUAL_DEV(  0x22b8, 0x3010, 0x0001, 0x0001,
 		US_FL_FIX_CAPACITY | US_FL_IGNORE_RESIDUE ),
 
 /*
- * Patch by Pete Zaitcev <zaitcev@redhat.com>
- * Report by Mark Patton. Red Hat bz#208928.
- * Added support for rev 0x0002 (Motorola ROKR W5)
- * by Javier Smaldone <javier@smaldone.com.ar>
- */
-UNUSUAL_DEV(  0x22b8, 0x4810, 0x0001, 0x0002,
-		"Motorola",
-		"RAZR V3i/ROKR W5",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY),
-
-/*
- * Patch by Jost Diederichs <jost@qdusa.com>
- */
-UNUSUAL_DEV(0x22b8, 0x6410, 0x0001, 0x9999,
-		"Motorola Inc.",
-		"Motorola Phone (RAZRV3xx)",
-		US_SC_DEVICE, US_PR_DEVICE, NULL,
-		US_FL_FIX_CAPACITY),
-
-/*
  * Patch by Constantin Baranov <const@tltsu.ru>
  * Report by Andreas Koenecke.
  * Motorola ROKR Z6.
diff --git a/drivers/usb/storage/usb.c b/drivers/usb/storage/usb.c
index 27016fd2cad1..4becf495ca2d 100644
--- a/drivers/usb/storage/usb.c
+++ b/drivers/usb/storage/usb.c
@@ -75,9 +75,6 @@
 #ifdef CONFIG_USB_STORAGE_SDDR55
 #include "sddr55.h"
 #endif
-#ifdef CONFIG_USB_STORAGE_DPCM
-#include "dpcm.h"
-#endif
 #ifdef CONFIG_USB_STORAGE_FREECOM
 #include "freecom.h"
 #endif
@@ -103,6 +100,7 @@
 #include "cypress_atacb.h"
 #endif
 #include "sierra_ms.h"
+#include "option_ms.h"
 
 /* Some informational data */
 MODULE_AUTHOR("Matthew Dharm <mdharm-usb@one-eyed-alien.net>");
@@ -113,6 +111,10 @@ static unsigned int delay_use = 5;
 module_param(delay_use, uint, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(delay_use, "seconds to delay before using a new device");
 
+static char quirks[128];
+module_param_string(quirks, quirks, sizeof(quirks), S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(quirks, "supplemental list of device IDs and their quirks");
+
 
 /*
  * The entries in this table correspond, line for line,
@@ -126,6 +128,8 @@ MODULE_PARM_DESC(delay_use, "seconds to delay before using a new device");
 { USB_DEVICE_VER(id_vendor, id_product, bcdDeviceMin,bcdDeviceMax), \
   .driver_info = (flags)|(USB_US_TYPE_STOR<<24) }
 
+#define COMPLIANT_DEV	UNUSUAL_DEV
+
 #define USUAL_DEV(useProto, useTrans, useType) \
 { USB_INTERFACE_INFO(USB_CLASS_MASS_STORAGE, useProto, useTrans), \
   .driver_info = (USB_US_TYPE_STOR<<24) }
@@ -134,6 +138,7 @@ static struct usb_device_id storage_usb_ids [] = {
 
 #	include "unusual_devs.h"
 #undef UNUSUAL_DEV
+#undef COMPLIANT_DEV
 #undef USUAL_DEV
 	/* Terminating entry */
 	{ }
@@ -164,6 +169,8 @@ MODULE_DEVICE_TABLE (usb, storage_usb_ids);
 	.initFunction = init_function,	\
 }
 
+#define COMPLIANT_DEV	UNUSUAL_DEV
+
 #define USUAL_DEV(use_protocol, use_transport, use_type) \
 { \
 	.useProtocol = use_protocol,	\
@@ -173,6 +180,7 @@ MODULE_DEVICE_TABLE (usb, storage_usb_ids);
 static struct us_unusual_dev us_unusual_dev_list[] = {
 #	include "unusual_devs.h" 
 #	undef UNUSUAL_DEV
+#	undef COMPLIANT_DEV
 #	undef USUAL_DEV
 
 	/* Terminating entry */
@@ -464,13 +472,83 @@ static int associate_dev(struct us_data *us, struct usb_interface *intf)
 		US_DEBUGP("I/O buffer allocation failed\n");
 		return -ENOMEM;
 	}
+	return 0;
+}
 
-	us->sensebuf = kmalloc(US_SENSE_SIZE, GFP_KERNEL);
-	if (!us->sensebuf) {
-		US_DEBUGP("Sense buffer allocation failed\n");
-		return -ENOMEM;
+/* Works only for digits and letters, but small and fast */
+#define TOLOWER(x) ((x) | 0x20)
+
+/* Adjust device flags based on the "quirks=" module parameter */
+static void adjust_quirks(struct us_data *us)
+{
+	char *p;
+	u16 vid = le16_to_cpu(us->pusb_dev->descriptor.idVendor);
+	u16 pid = le16_to_cpu(us->pusb_dev->descriptor.idProduct);
+	unsigned f = 0;
+	unsigned int mask = (US_FL_SANE_SENSE | US_FL_FIX_CAPACITY |
+			US_FL_CAPACITY_HEURISTICS | US_FL_IGNORE_DEVICE |
+			US_FL_NOT_LOCKABLE | US_FL_MAX_SECTORS_64 |
+			US_FL_CAPACITY_OK | US_FL_IGNORE_RESIDUE |
+			US_FL_SINGLE_LUN | US_FL_NO_WP_DETECT);
+
+	p = quirks;
+	while (*p) {
+		/* Each entry consists of VID:PID:flags */
+		if (vid == simple_strtoul(p, &p, 16) &&
+				*p == ':' &&
+				pid == simple_strtoul(p+1, &p, 16) &&
+				*p == ':')
+			break;
+
+		/* Move forward to the next entry */
+		while (*p) {
+			if (*p++ == ',')
+				break;
+		}
 	}
-	return 0;
+	if (!*p)	/* No match */
+		return;
+
+	/* Collect the flags */
+	while (*++p && *p != ',') {
+		switch (TOLOWER(*p)) {
+		case 'a':
+			f |= US_FL_SANE_SENSE;
+			break;
+		case 'c':
+			f |= US_FL_FIX_CAPACITY;
+			break;
+		case 'h':
+			f |= US_FL_CAPACITY_HEURISTICS;
+			break;
+		case 'i':
+			f |= US_FL_IGNORE_DEVICE;
+			break;
+		case 'l':
+			f |= US_FL_NOT_LOCKABLE;
+			break;
+		case 'm':
+			f |= US_FL_MAX_SECTORS_64;
+			break;
+		case 'o':
+			f |= US_FL_CAPACITY_OK;
+			break;
+		case 'r':
+			f |= US_FL_IGNORE_RESIDUE;
+			break;
+		case 's':
+			f |= US_FL_SINGLE_LUN;
+			break;
+		case 'w':
+			f |= US_FL_NO_WP_DETECT;
+			break;
+		/* Ignore unrecognized flag characters */
+		}
+	}
+	us->fflags = (us->fflags & ~mask) | f;
+	dev_info(&us->pusb_intf->dev, "Quirks match for "
+			"vid %04x pid %04x: %x\n",
+			vid, pid, f);
 }
 
 /* Find an unusual_dev descriptor (always succeeds in the current code) */
@@ -497,6 +575,7 @@ static int get_device_info(struct us_data *us, const struct usb_device_id *id)
 			idesc->bInterfaceProtocol :
 			unusual_dev->useTransport;
 	us->fflags = USB_US_ORIG_FLAGS(id->driver_info);
+	adjust_quirks(us);
 
 	if (us->fflags & US_FL_IGNORE_DEVICE) {
 		printk(KERN_INFO USB_STORAGE "device ignored\n");
@@ -562,7 +641,7 @@ static int get_transport(struct us_data *us)
 
 	case US_PR_CBI:
 		us->transport_name = "Control/Bulk/Interrupt";
-		us->transport = usb_stor_CBI_transport;
+		us->transport = usb_stor_CB_transport;
 		us->transport_reset = usb_stor_CB_reset;
 		us->max_lun = 7;
 		break;
@@ -675,19 +754,19 @@ static int get_protocol(struct us_data *us)
 
 	case US_SC_8020:
 		us->protocol_name = "8020i";
-		us->proto_handler = usb_stor_ATAPI_command;
+		us->proto_handler = usb_stor_pad12_command;
 		us->max_lun = 0;
 		break;
 
 	case US_SC_QIC:
 		us->protocol_name = "QIC-157";
-		us->proto_handler = usb_stor_qic157_command;
+		us->proto_handler = usb_stor_pad12_command;
 		us->max_lun = 0;
 		break;
 
 	case US_SC_8070:
 		us->protocol_name = "8070i";
-		us->proto_handler = usb_stor_ATAPI_command;
+		us->proto_handler = usb_stor_pad12_command;
 		us->max_lun = 0;
 		break;
 
@@ -840,8 +919,6 @@ static void dissociate_dev(struct us_data *us)
 {
 	US_DEBUGP("-- %s\n", __func__);
 
-	kfree(us->sensebuf);
-
 	/* Free the device-related DMA-mapped buffers */
 	if (us->cr)
 		usb_buffer_free(us->pusb_dev, sizeof(*us->cr), us->cr,
@@ -1064,6 +1141,7 @@ static struct usb_driver usb_storage_driver = {
 static int __init usb_stor_init(void)
 {
 	int retval;
+
 	printk(KERN_INFO "Initializing USB Mass Storage driver...\n");
 
 	/* register the driver, return usb_register return code if error */
diff --git a/drivers/usb/storage/usb.h b/drivers/usb/storage/usb.h
index a4ad73bd832d..65e674e4be99 100644
--- a/drivers/usb/storage/usb.h
+++ b/drivers/usb/storage/usb.h
@@ -138,7 +138,6 @@ struct us_data {
 	struct usb_ctrlrequest	*cr;		 /* control requests	 */
 	struct usb_sg_request	current_sg;	 /* scatter-gather req.  */
 	unsigned char		*iobuf;		 /* I/O buffer		 */
-	unsigned char		*sensebuf;	 /* sense data buffer	 */
 	dma_addr_t		cr_dma;		 /* buffer DMA addresses */
 	dma_addr_t		iobuf_dma;
 	struct task_struct	*ctl_thread;	 /* the control thread   */
@@ -155,6 +154,10 @@ struct us_data {
 #ifdef CONFIG_PM
 	pm_hook			suspend_resume_hook;
 #endif
+
+	/* hacks for READ CAPACITY bug handling */
+	int			use_last_sector_hacks;
+	int			last_sector_retries;
 };
 
 /* Convert between us_data and the corresponding Scsi_Host */
diff --git a/drivers/usb/wusbcore/rh.c b/drivers/usb/wusbcore/rh.c
index 95c6fa3bf6b2..3937bf6f8cef 100644
--- a/drivers/usb/wusbcore/rh.c
+++ b/drivers/usb/wusbcore/rh.c
@@ -326,7 +326,7 @@ static int wusbhc_rh_clear_port_feat(struct wusbhc *wusbhc, u16 feature,
 static int wusbhc_rh_get_port_status(struct wusbhc *wusbhc, u16 port_idx,
 				     u32 *_buf, u16 wLength)
 {
-	u16 *buf = (u16 *) _buf;
+	__le16 *buf = (__le16 *)_buf;
 
 	if (port_idx > wusbhc->ports_max)
 		return -EINVAL;
diff --git a/drivers/uwb/i1480/dfu/usb.c b/drivers/uwb/i1480/dfu/usb.c
index 686795e97195..c7080d497311 100644
--- a/drivers/uwb/i1480/dfu/usb.c
+++ b/drivers/uwb/i1480/dfu/usb.c
@@ -387,7 +387,7 @@ int i1480_usb_probe(struct usb_interface *iface, const struct usb_device_id *id)
 		goto error_create;
 	}
 
-	/* setup the fops and upload the firmare */
+	/* setup the fops and upload the firmware */
 	i1480->pre_fw_name = "i1480-pre-phy-0.0.bin";
 	i1480->mac_fw_name = "i1480-usb-0.0.bin";
 	i1480->mac_fw_name_deprecate = "ptc-0.0.bin";
diff --git a/drivers/video/aty/aty128fb.c b/drivers/video/aty/aty128fb.c
index 243ea4ab20c8..db16112cf197 100644
--- a/drivers/video/aty/aty128fb.c
+++ b/drivers/video/aty/aty128fb.c
@@ -2051,7 +2051,7 @@ static int __devinit aty128_probe(struct pci_dev *pdev, const struct pci_device_
 
 	/* Virtualize mmio region */
 	info->fix.mmio_start = reg_addr;
-	par->regbase = ioremap(reg_addr, pci_resource_len(pdev, 2));
+	par->regbase = pci_ioremap_bar(pdev, 2);
 	if (!par->regbase)
 		goto err_free_info;
 
diff --git a/drivers/video/backlight/backlight.c b/drivers/video/backlight/backlight.c
index fab0bc874b58..0664fc032235 100644
--- a/drivers/video/backlight/backlight.c
+++ b/drivers/video/backlight/backlight.c
@@ -217,7 +217,7 @@ struct backlight_device *backlight_device_register(const char *name,
 	new_bd->dev.class = backlight_class;
 	new_bd->dev.parent = parent;
 	new_bd->dev.release = bl_device_release;
-	strlcpy(new_bd->dev.bus_id, name, BUS_ID_SIZE);
+	dev_set_name(&new_bd->dev, name);
 	dev_set_drvdata(&new_bd->dev, devdata);
 
 	rc = device_register(&new_bd->dev);
diff --git a/drivers/video/backlight/lcd.c b/drivers/video/backlight/lcd.c
index 680e57b616cd..b6449470106c 100644
--- a/drivers/video/backlight/lcd.c
+++ b/drivers/video/backlight/lcd.c
@@ -208,7 +208,7 @@ struct lcd_device *lcd_device_register(const char *name, struct device *parent,
 	new_ld->dev.class = lcd_class;
 	new_ld->dev.parent = parent;
 	new_ld->dev.release = lcd_device_release;
-	strlcpy(new_ld->dev.bus_id, name, BUS_ID_SIZE);
+	dev_set_name(&new_ld->dev, name);
 	dev_set_drvdata(&new_ld->dev, devdata);
 
 	rc = device_register(&new_ld->dev);
diff --git a/drivers/video/bfin-t350mcqb-fb.c b/drivers/video/bfin-t350mcqb-fb.c
index 7d1b819e501c..a9b3ada05d99 100644
--- a/drivers/video/bfin-t350mcqb-fb.c
+++ b/drivers/video/bfin-t350mcqb-fb.c
@@ -255,7 +255,7 @@ static int bfin_t350mcqb_fb_check_var(struct fb_var_screeninfo *var,
 {
 
 	if (var->bits_per_pixel != LCD_BPP) {
-		pr_debug("%s: depth not supported: %u BPP\n", __FUNCTION__,
+		pr_debug("%s: depth not supported: %u BPP\n", __func__,
 			 var->bits_per_pixel);
 		return -EINVAL;
 	}
@@ -264,7 +264,7 @@ static int bfin_t350mcqb_fb_check_var(struct fb_var_screeninfo *var,
 	    info->var.xres_virtual != var->xres_virtual ||
 	    info->var.yres_virtual != var->yres_virtual) {
 		pr_debug("%s: Resolution not supported: X%u x Y%u \n",
-			 __FUNCTION__, var->xres, var->yres);
+			 __func__, var->xres, var->yres);
 		return -EINVAL;
 	}
 
@@ -274,7 +274,7 @@ static int bfin_t350mcqb_fb_check_var(struct fb_var_screeninfo *var,
 
 	if ((info->fix.line_length * var->yres_virtual) > info->fix.smem_len) {
 		pr_debug("%s: Memory Limit requested yres_virtual = %u\n",
-			 __FUNCTION__, var->yres_virtual);
+			 __func__, var->yres_virtual);
 		return -ENOMEM;
 	}
 
diff --git a/drivers/video/carminefb.c b/drivers/video/carminefb.c
index c9b191319a9a..c7ff3c1a266a 100644
--- a/drivers/video/carminefb.c
+++ b/drivers/video/carminefb.c
@@ -168,7 +168,7 @@ static int carmine_setcolreg(unsigned regno, unsigned red, unsigned green,
 	blue >>= 8;
 	transp >>= 8;
 
-	((u32 *)info->pseudo_palette)[regno] = be32_to_cpu(transp << 24 |
+	((__be32 *)info->pseudo_palette)[regno] = cpu_to_be32(transp << 24 |
 		red << 0 | green << 8 | blue << 16);
 	return 0;
 }
diff --git a/drivers/video/console/vgacon.c b/drivers/video/console/vgacon.c
index e6210725b9ab..d012edda6d11 100644
--- a/drivers/video/console/vgacon.c
+++ b/drivers/video/console/vgacon.c
@@ -1332,7 +1332,7 @@ static void vgacon_save_screen(struct vc_data *c)
 		c->vc_y = screen_info.orig_y;
 	}
 
-	/* We can't copy in more then the size of the video buffer,
+	/* We can't copy in more than the size of the video buffer,
 	 * or we'll be copying in VGA BIOS */
 
 	if (!vga_is_gfx)
diff --git a/drivers/video/cyber2000fb.c b/drivers/video/cyber2000fb.c
index 39d5d643a50b..7a9e42e3a9a9 100644
--- a/drivers/video/cyber2000fb.c
+++ b/drivers/video/cyber2000fb.c
@@ -1583,8 +1583,7 @@ cyberpro_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 		goto failed_release;
 
 	cfb->dev = dev;
-	cfb->region = ioremap(pci_resource_start(dev, 0),
-			      pci_resource_len(dev, 0));
+	cfb->region = pci_ioremap_bar(dev, 0);
 	if (!cfb->region)
 		goto failed_ioremap;
 
diff --git a/drivers/video/fbmem.c b/drivers/video/fbmem.c
index 3c65b0d67617..756efeb91abc 100644
--- a/drivers/video/fbmem.c
+++ b/drivers/video/fbmem.c
@@ -510,6 +510,10 @@ static int fb_prepare_extra_logos(struct fb_info *info, unsigned int height,
 		fb_logo_ex_num = 0;
 
 	for (i = 0; i < fb_logo_ex_num; i++) {
+		if (fb_logo_ex[i].logo->type != fb_logo.logo->type) {
+			fb_logo_ex[i].logo = NULL;
+			continue;
+		}
 		height += fb_logo_ex[i].logo->height;
 		if (height > yres) {
 			height -= fb_logo_ex[i].logo->height;
diff --git a/drivers/video/gbefb.c b/drivers/video/gbefb.c
index f89c3cce1e0c..fe5b519860b1 100644
--- a/drivers/video/gbefb.c
+++ b/drivers/video/gbefb.c
@@ -912,6 +912,7 @@ static int gbefb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
 	unsigned int line_length;
 	struct gbe_timing_info timing;
+	int ret;
 
 	/* Limit bpp to 8, 16, and 32 */
 	if (var->bits_per_pixel <= 8)
@@ -930,8 +931,10 @@ static int gbefb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 
 	var->grayscale = 0;	/* No grayscale for now */
 
-	if ((var->pixclock = compute_gbe_timing(var, &timing)) < 0)
-		return(-EINVAL);
+	ret = compute_gbe_timing(var, &timing);
+	var->pixclock = ret;
+	if (ret < 0)
+		return -EINVAL;
 
 	/* Adjust virtual resolution, if necessary */
 	if (var->xres > var->xres_virtual || (!ywrap && !ypan))
diff --git a/drivers/video/geode/gx1fb_core.c b/drivers/video/geode/gx1fb_core.c
index bb20a2289760..751e491ca8c8 100644
--- a/drivers/video/geode/gx1fb_core.c
+++ b/drivers/video/geode/gx1fb_core.c
@@ -217,8 +217,7 @@ static int __init gx1fb_map_video_memory(struct fb_info *info, struct pci_dev *d
 	ret = pci_request_region(dev, 0, "gx1fb (video)");
 	if (ret < 0)
 		return ret;
-	par->vid_regs = ioremap(pci_resource_start(dev, 0),
-				pci_resource_len(dev, 0));
+	par->vid_regs = pci_ioremap_bar(dev, 0);
 	if (!par->vid_regs)
 		return -ENOMEM;
 
diff --git a/drivers/video/geode/gxfb_core.c b/drivers/video/geode/gxfb_core.c
index de2b8f9876a5..484118926318 100644
--- a/drivers/video/geode/gxfb_core.c
+++ b/drivers/video/geode/gxfb_core.c
@@ -242,23 +242,21 @@ static int __init gxfb_map_video_memory(struct fb_info *info, struct pci_dev *de
 	ret = pci_request_region(dev, 3, "gxfb (video processor)");
 	if (ret < 0)
 		return ret;
-	par->vid_regs = ioremap(pci_resource_start(dev, 3),
-				pci_resource_len(dev, 3));
+	par->vid_regs = pci_ioremap_bar(dev, 3);
 	if (!par->vid_regs)
 		return -ENOMEM;
 
 	ret = pci_request_region(dev, 2, "gxfb (display controller)");
 	if (ret < 0)
 		return ret;
-	par->dc_regs = ioremap(pci_resource_start(dev, 2), pci_resource_len(dev, 2));
+	par->dc_regs = pci_ioremap_bar(dev, 2);
 	if (!par->dc_regs)
 		return -ENOMEM;
 
 	ret = pci_request_region(dev, 1, "gxfb (graphics processor)");
 	if (ret < 0)
 		return ret;
-	par->gp_regs = ioremap(pci_resource_start(dev, 1),
-	pci_resource_len(dev, 1));
+	par->gp_regs = pci_ioremap_bar(dev, 1);
 
 	if (!par->gp_regs)
 		return -ENOMEM;
diff --git a/drivers/video/geode/lxfb_core.c b/drivers/video/geode/lxfb_core.c
index 2cd9b74d2225..b965ecdbc604 100644
--- a/drivers/video/geode/lxfb_core.c
+++ b/drivers/video/geode/lxfb_core.c
@@ -379,20 +379,17 @@ static int __init lxfb_map_video_memory(struct fb_info *info,
 	if (info->screen_base == NULL)
 		return ret;
 
-	par->gp_regs = ioremap(pci_resource_start(dev, 1),
-				pci_resource_len(dev, 1));
+	par->gp_regs = pci_ioremap_bar(dev, 1);
 
 	if (par->gp_regs == NULL)
 		return ret;
 
-	par->dc_regs = ioremap(pci_resource_start(dev, 2),
-			       pci_resource_len(dev, 2));
+	par->dc_regs = pci_ioremap_bar(dev, 2);
 
 	if (par->dc_regs == NULL)
 		return ret;
 
-	par->vp_regs = ioremap(pci_resource_start(dev, 3),
-			       pci_resource_len(dev, 3));
+	par->vp_regs = pci_ioremap_bar(dev, 3);
 
 	if (par->vp_regs == NULL)
 		return ret;
diff --git a/drivers/video/gxt4500.c b/drivers/video/gxt4500.c
index 564557792bed..896e53dea906 100644
--- a/drivers/video/gxt4500.c
+++ b/drivers/video/gxt4500.c
@@ -648,7 +648,7 @@ static int __devinit gxt4500_probe(struct pci_dev *pdev,
 	info->pseudo_palette = par->pseudo_palette;
 
 	info->fix.mmio_start = reg_phys;
-	par->regs = ioremap(reg_phys, pci_resource_len(pdev, 0));
+	par->regs = pci_ioremap_bar(pdev, 0);
 	if (!par->regs) {
 		dev_err(&pdev->dev, "gxt4500: cannot map registers\n");
 		goto err_free_all;
@@ -656,7 +656,7 @@ static int __devinit gxt4500_probe(struct pci_dev *pdev,
 
 	info->fix.smem_start = fb_phys;
 	info->fix.smem_len = pci_resource_len(pdev, 1);
-	info->screen_base = ioremap(fb_phys, pci_resource_len(pdev, 1));
+	info->screen_base = pci_ioremap_bar(pdev, 1);
 	if (!info->screen_base) {
 		dev_err(&pdev->dev, "gxt4500: cannot map framebuffer\n");
 		goto err_unmap_regs;
diff --git a/drivers/video/i810/i810_accel.c b/drivers/video/i810/i810_accel.c
index 76764ea3486a..f5bedee4310a 100644
--- a/drivers/video/i810/i810_accel.c
+++ b/drivers/video/i810/i810_accel.c
@@ -301,8 +301,10 @@ void i810fb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 	u32 dx, dy, width, height, dest, rop = 0, color = 0;
 
 	if (!info->var.accel_flags || par->dev_flags & LOCKUP ||
-	    par->depth == 4) 
-		return cfb_fillrect(info, rect);
+	    par->depth == 4) {
+		cfb_fillrect(info, rect);
+		return;
+	}
 
 	if (par->depth == 1) 
 		color = rect->color;
@@ -327,8 +329,10 @@ void i810fb_copyarea(struct fb_info *info, const struct fb_copyarea *region)
 	u32 sx, sy, dx, dy, pitch, width, height, src, dest, xdir;
 
 	if (!info->var.accel_flags || par->dev_flags & LOCKUP ||
-	    par->depth == 4)
-		return cfb_copyarea(info, region);
+	    par->depth == 4) {
+		cfb_copyarea(info, region);
+		return;
+	}
 
 	dx = region->dx * par->depth;
 	sx = region->sx * par->depth;
@@ -366,8 +370,10 @@ void i810fb_imageblit(struct fb_info *info, const struct fb_image *image)
 	u32 fg = 0, bg = 0, size, dst;
 	
 	if (!info->var.accel_flags || par->dev_flags & LOCKUP ||
-	    par->depth == 4 || image->depth != 1) 
-		return cfb_imageblit(info, image);
+	    par->depth == 4 || image->depth != 1) {
+		cfb_imageblit(info, image);
+		return;
+	}
 
 	switch (info->var.bits_per_pixel) {
 	case 8:
diff --git a/drivers/video/intelfb/intelfbdrv.c b/drivers/video/intelfb/intelfbdrv.c
index a09e23649357..6d8e5415c809 100644
--- a/drivers/video/intelfb/intelfbdrv.c
+++ b/drivers/video/intelfb/intelfbdrv.c
@@ -1493,8 +1493,10 @@ static void intelfb_fillrect (struct fb_info *info,
 	DBG_MSG("intelfb_fillrect\n");
 #endif
 
-	if (!ACCEL(dinfo, info) || dinfo->depth == 4)
-		return cfb_fillrect(info, rect);
+	if (!ACCEL(dinfo, info) || dinfo->depth == 4) {
+		cfb_fillrect(info, rect);
+		return;
+	}
 
 	if (rect->rop == ROP_COPY)
 		rop = PAT_ROP_GXCOPY;
@@ -1521,8 +1523,10 @@ static void intelfb_copyarea(struct fb_info *info,
 	DBG_MSG("intelfb_copyarea\n");
 #endif
 
-	if (!ACCEL(dinfo, info) || dinfo->depth == 4)
-		return cfb_copyarea(info, region);
+	if (!ACCEL(dinfo, info) || dinfo->depth == 4) {
+		cfb_copyarea(info, region);
+		return;
+	}
 
 	intelfbhw_do_bitblt(dinfo, region->sx, region->sy, region->dx,
 			    region->dy, region->width, region->height,
@@ -1540,8 +1544,10 @@ static void intelfb_imageblit(struct fb_info *info,
 #endif
 
 	if (!ACCEL(dinfo, info) || dinfo->depth == 4
-	    || image->depth != 1)
-		return cfb_imageblit(info, image);
+	    || image->depth != 1) {
+		cfb_imageblit(info, image);
+		return;
+	}
 
 	if (dinfo->depth != 8) {
 		fgcolor = dinfo->pseudo_palette[image->fg_color];
@@ -1554,8 +1560,10 @@ static void intelfb_imageblit(struct fb_info *info,
 	if (!intelfbhw_do_drawglyph(dinfo, fgcolor, bgcolor, image->width,
 				    image->height, image->data,
 				    image->dx, image->dy,
-				    dinfo->pitch, info->var.bits_per_pixel))
-		return cfb_imageblit(info, image);
+				    dinfo->pitch, info->var.bits_per_pixel)) {
+		cfb_imageblit(info, image);
+		return;
+	}
 }
 
 static int intelfb_cursor(struct fb_info *info, struct fb_cursor *cursor)
diff --git a/drivers/video/modedb.c b/drivers/video/modedb.c
index d3c3af53a290..16186240c5f2 100644
--- a/drivers/video/modedb.c
+++ b/drivers/video/modedb.c
@@ -329,7 +329,7 @@ const struct fb_videomode vesa_modes[] = {
 	  FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
 	  FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
 	/* 17 1152x864-75 VESA */
-	{ NULL, 75, 1153, 864, 9259, 256, 64, 32, 1, 128, 3,
+	{ NULL, 75, 1152, 864, 9259, 256, 64, 32, 1, 128, 3,
 	  FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
 	  FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
 	/* 18 1280x960-60 VESA */
diff --git a/drivers/video/neofb.c b/drivers/video/neofb.c
index bfb802d26d5a..588527a254c2 100644
--- a/drivers/video/neofb.c
+++ b/drivers/video/neofb.c
@@ -1453,7 +1453,8 @@ neo2200_imageblit(struct fb_info *info, const struct fb_image *image)
 			 * is less than 16 bits wide. This is due to insufficient
 			 * padding when writing the image. We need to adjust
 			 * struct fb_pixmap. Not yet done. */
-			return cfb_imageblit(info, image);
+			cfb_imageblit(info, image);
+			return;
 		}
 		bltCntl_flags = NEO_BC0_SRC_MONO;
 	} else if (image->depth == info->var.bits_per_pixel) {
@@ -1461,7 +1462,8 @@ neo2200_imageblit(struct fb_info *info, const struct fb_image *image)
 	} else {
 		/* We don't currently support hardware acceleration if image
 		 * depth is different from display */
-		return cfb_imageblit(info, image);
+		cfb_imageblit(info, image);
+		return;
 	}
 
 	switch (info->var.bits_per_pixel) {
diff --git a/drivers/video/nvidia/nv_accel.c b/drivers/video/nvidia/nv_accel.c
index fa4821c5572b..ad6472a894ea 100644
--- a/drivers/video/nvidia/nv_accel.c
+++ b/drivers/video/nvidia/nv_accel.c
@@ -300,8 +300,10 @@ void nvidiafb_copyarea(struct fb_info *info, const struct fb_copyarea *region)
 	if (info->state != FBINFO_STATE_RUNNING)
 		return;
 
-	if (par->lockup)
-		return cfb_copyarea(info, region);
+	if (par->lockup) {
+		cfb_copyarea(info, region);
+		return;
+	}
 
 	NVDmaStart(info, par, BLIT_POINT_SRC, 3);
 	NVDmaNext(par, (region->sy << 16) | region->sx);
@@ -319,8 +321,10 @@ void nvidiafb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 	if (info->state != FBINFO_STATE_RUNNING)
 		return;
 
-	if (par->lockup)
-		return cfb_fillrect(info, rect);
+	if (par->lockup) {
+		cfb_fillrect(info, rect);
+		return;
+	}
 
 	if (info->var.bits_per_pixel == 8)
 		color = rect->color;
diff --git a/drivers/video/output.c b/drivers/video/output.c
index f2df5519c9c4..5e6439ae7394 100644
--- a/drivers/video/output.c
+++ b/drivers/video/output.c
@@ -96,7 +96,7 @@ struct output_device *video_output_register(const char *name,
 	new_dev->props = op;
 	new_dev->dev.class = &video_output_class;
 	new_dev->dev.parent = dev;
-	strlcpy(new_dev->dev.bus_id,name, BUS_ID_SIZE);
+	dev_set_name(&new_dev->dev, name);
 	dev_set_drvdata(&new_dev->dev, devdata);
 	ret_code = device_register(&new_dev->dev);
 	if (ret_code) {
diff --git a/drivers/video/pm3fb.c b/drivers/video/pm3fb.c
index 68089d1456c2..6666f45a2f8c 100644
--- a/drivers/video/pm3fb.c
+++ b/drivers/video/pm3fb.c
@@ -539,8 +539,10 @@ static void pm3fb_imageblit(struct fb_info *info, const struct fb_image *image)
 		bgx = par->palette[image->bg_color];
 		break;
 	}
-	if (image->depth != 1)
-		return cfb_imageblit(info, image);
+	if (image->depth != 1) {
+		cfb_imageblit(info, image);
+		return;
+	}
 
 	if (info->var.bits_per_pixel == 8) {
 		fgx |= fgx << 8;
diff --git a/drivers/video/sm501fb.c b/drivers/video/sm501fb.c
index f94ae84a58cd..dcd98793d568 100644
--- a/drivers/video/sm501fb.c
+++ b/drivers/video/sm501fb.c
@@ -159,6 +159,9 @@ static int sm501_alloc_mem(struct sm501fb_info *inf, struct sm501_mem *mem,
 		break;
 
 	case SM501_MEMF_PANEL:
+		if (size > inf->fbmem_len)
+			return -ENOMEM;
+
 		ptr = inf->fbmem_len - size;
 		fbi = inf->fb[HEAD_CRT];
 
@@ -172,9 +175,6 @@ static int sm501_alloc_mem(struct sm501fb_info *inf, struct sm501_mem *mem,
 		if (fbi && ptr < fbi->fix.smem_len)
 			return -ENOMEM;
 
-		if (ptr < 0)
-			return -ENOMEM;
-
 		break;
 
 	case SM501_MEMF_CRT:
diff --git a/drivers/video/via/viafbdev.c b/drivers/video/via/viafbdev.c
index e21fe5b6f9ff..37b433a08ce8 100644
--- a/drivers/video/via/viafbdev.c
+++ b/drivers/video/via/viafbdev.c
@@ -870,8 +870,10 @@ static void viafb_fillrect(struct fb_info *info,
 	u32 col = 0, rop = 0;
 	int pitch;
 
-	if (!viafb_accel)
-		return cfb_fillrect(info, rect);
+	if (!viafb_accel) {
+		cfb_fillrect(info, rect);
+		return;
+	}
 
 	if (!rect->width || !rect->height)
 		return;
@@ -937,8 +939,10 @@ static void viafb_copyarea(struct fb_info *info,
 
 	DEBUG_MSG(KERN_INFO "viafb_copyarea!!\n");
 
-	if (!viafb_accel)
-		return cfb_copyarea(info, area);
+	if (!viafb_accel) {
+		cfb_copyarea(info, area);
+		return;
+	}
 
 	if (!area->width || !area->height)
 		return;
@@ -994,8 +998,10 @@ static void viafb_imageblit(struct fb_info *info,
 	int i;
 	int pitch;
 
-	if (!viafb_accel)
-		return cfb_imageblit(info, image);
+	if (!viafb_accel) {
+		cfb_imageblit(info, image);
+		return;
+	}
 
 	udata = (u32 *) image->data;
 
diff --git a/drivers/virtio/virtio_pci.c b/drivers/virtio/virtio_pci.c
index 265fdf2d1276..bef6b45e8a5c 100644
--- a/drivers/virtio/virtio_pci.c
+++ b/drivers/virtio/virtio_pci.c
@@ -73,10 +73,7 @@ MODULE_DEVICE_TABLE(pci, virtio_pci_id_table);
 /* A PCI device has it's own struct device and so does a virtio device so
  * we create a place for the virtio devices to show up in sysfs.  I think it
  * would make more sense for virtio to not insist on having it's own device. */
-static struct device virtio_pci_root = {
-	.parent		= NULL,
-	.init_name	= "virtio-pci",
-};
+static struct device *virtio_pci_root;
 
 /* Convert a generic virtio device to our structure */
 static struct virtio_pci_device *to_vp_device(struct virtio_device *vdev)
@@ -343,7 +340,7 @@ static int __devinit virtio_pci_probe(struct pci_dev *pci_dev,
 	if (vp_dev == NULL)
 		return -ENOMEM;
 
-	vp_dev->vdev.dev.parent = &virtio_pci_root;
+	vp_dev->vdev.dev.parent = virtio_pci_root;
 	vp_dev->vdev.dev.release = virtio_pci_release_dev;
 	vp_dev->vdev.config = &virtio_pci_config_ops;
 	vp_dev->pci_dev = pci_dev;
@@ -437,13 +434,13 @@ static int __init virtio_pci_init(void)
 {
 	int err;
 
-	err = device_register(&virtio_pci_root);
-	if (err)
-		return err;
+	virtio_pci_root = root_device_register("virtio-pci");
+	if (IS_ERR(virtio_pci_root))
+		return PTR_ERR(virtio_pci_root);
 
 	err = pci_register_driver(&virtio_pci_driver);
 	if (err)
-		device_unregister(&virtio_pci_root);
+		device_unregister(virtio_pci_root);
 
 	return err;
 }
@@ -452,8 +449,8 @@ module_init(virtio_pci_init);
 
 static void __exit virtio_pci_exit(void)
 {
-	device_unregister(&virtio_pci_root);
 	pci_unregister_driver(&virtio_pci_driver);
+	root_device_unregister(virtio_pci_root);
 }
 
 module_exit(virtio_pci_exit);
diff --git a/drivers/w1/w1.c b/drivers/w1/w1.c
index 3b615d4022ee..acc7e3b7fe17 100644
--- a/drivers/w1/w1.c
+++ b/drivers/w1/w1.c
@@ -197,7 +197,7 @@ struct device_driver w1_master_driver = {
 struct device w1_master_device = {
 	.parent = NULL,
 	.bus = &w1_bus_type,
-	.bus_id = "w1 bus master",
+	.init_name = "w1 bus master",
 	.driver = &w1_master_driver,
 	.release = &w1_master_release
 };
@@ -211,7 +211,7 @@ static struct device_driver w1_slave_driver = {
 struct device w1_slave_device = {
 	.parent = NULL,
 	.bus = &w1_bus_type,
-	.bus_id = "w1 bus slave",
+	.init_name = "w1 bus slave",
 	.driver = &w1_slave_driver,
 	.release = &w1_slave_release
 };
@@ -573,7 +573,7 @@ static int w1_uevent(struct device *dev, struct kobj_uevent_env *env)
 	}
 
 	dev_dbg(dev, "Hotplug event for %s %s, bus_id=%s.\n",
-			event_owner, name, dev->bus_id);
+			event_owner, name, dev_name(dev));
 
 	if (dev->driver != &w1_slave_driver || !sl)
 		return 0;
@@ -605,8 +605,7 @@ static int __w1_attach_slave_device(struct w1_slave *sl)
 	sl->dev.bus = &w1_bus_type;
 	sl->dev.release = &w1_slave_release;
 
-	snprintf(&sl->dev.bus_id[0], sizeof(sl->dev.bus_id),
-		 "%02x-%012llx",
+	dev_set_name(&sl->dev, "%02x-%012llx",
 		 (unsigned int) sl->reg_num.family,
 		 (unsigned long long) sl->reg_num.id);
 	snprintf(&sl->name[0], sizeof(sl->name),
@@ -615,13 +614,13 @@ static int __w1_attach_slave_device(struct w1_slave *sl)
 		 (unsigned long long) sl->reg_num.id);
 
 	dev_dbg(&sl->dev, "%s: registering %s as %p.\n", __func__,
-		&sl->dev.bus_id[0], sl);
+		dev_name(&sl->dev), sl);
 
 	err = device_register(&sl->dev);
 	if (err < 0) {
 		dev_err(&sl->dev,
 			"Device registration [%s] failed. err=%d\n",
-			sl->dev.bus_id, err);
+			dev_name(&sl->dev), err);
 		return err;
 	}
 
@@ -630,7 +629,7 @@ static int __w1_attach_slave_device(struct w1_slave *sl)
 	if (err < 0) {
 		dev_err(&sl->dev,
 			"sysfs file creation for [%s] failed. err=%d\n",
-			sl->dev.bus_id, err);
+			dev_name(&sl->dev), err);
 		goto out_unreg;
 	}
 
@@ -639,7 +638,7 @@ static int __w1_attach_slave_device(struct w1_slave *sl)
 	if (err < 0) {
 		dev_err(&sl->dev,
 			"sysfs file creation for [%s] failed. err=%d\n",
-			sl->dev.bus_id, err);
+			dev_name(&sl->dev), err);
 		goto out_rem1;
 	}
 
@@ -648,7 +647,7 @@ static int __w1_attach_slave_device(struct w1_slave *sl)
 	    ((err = sl->family->fops->add_slave(sl)) < 0)) {
 		dev_err(&sl->dev,
 			"sysfs file creation for [%s] failed. err=%d\n",
-			sl->dev.bus_id, err);
+			dev_name(&sl->dev), err);
 		goto out_rem2;
 	}
 
diff --git a/drivers/w1/w1_int.c b/drivers/w1/w1_int.c
index a3a54567bfba..4a46ed58ece9 100644
--- a/drivers/w1/w1_int.c
+++ b/drivers/w1/w1_int.c
@@ -75,8 +75,7 @@ static struct w1_master * w1_alloc_dev(u32 id, int slave_count, int slave_ttl,
 	mutex_init(&dev->mutex);
 
 	memcpy(&dev->dev, device, sizeof(struct device));
-	snprintf(dev->dev.bus_id, sizeof(dev->dev.bus_id),
-		  "w1_bus_master%u", dev->id);
+	dev_set_name(&dev->dev, "w1_bus_master%u", dev->id);
 	snprintf(dev->name, sizeof(dev->name), "w1_bus_master%u", dev->id);
 
 	dev->driver = driver;
diff --git a/drivers/xen/xenbus/xenbus_probe.c b/drivers/xen/xenbus/xenbus_probe.c
index 7f24a98a446f..b2a03184a246 100644
--- a/drivers/xen/xenbus/xenbus_probe.c
+++ b/drivers/xen/xenbus/xenbus_probe.c
@@ -99,15 +99,15 @@ static int xenbus_uevent(struct device *_dev, struct kobj_uevent_env *env)
 }
 
 /* device/<type>/<id> => <type>-<id> */
-static int frontend_bus_id(char bus_id[BUS_ID_SIZE], const char *nodename)
+static int frontend_bus_id(char bus_id[XEN_BUS_ID_SIZE], const char *nodename)
 {
 	nodename = strchr(nodename, '/');
-	if (!nodename || strlen(nodename + 1) >= BUS_ID_SIZE) {
+	if (!nodename || strlen(nodename + 1) >= XEN_BUS_ID_SIZE) {
 		printk(KERN_WARNING "XENBUS: bad frontend %s\n", nodename);
 		return -EINVAL;
 	}
 
-	strlcpy(bus_id, nodename + 1, BUS_ID_SIZE);
+	strlcpy(bus_id, nodename + 1, XEN_BUS_ID_SIZE);
 	if (!strchr(bus_id, '/')) {
 		printk(KERN_WARNING "XENBUS: bus_id %s no slash\n", bus_id);
 		return -EINVAL;
@@ -460,6 +460,7 @@ int xenbus_probe_node(struct xen_bus_type *bus,
 		      const char *type,
 		      const char *nodename)
 {
+	char devname[XEN_BUS_ID_SIZE];
 	int err;
 	struct xenbus_device *xendev;
 	size_t stringlen;
@@ -494,10 +495,12 @@ int xenbus_probe_node(struct xen_bus_type *bus,
 	xendev->dev.bus = &bus->bus;
 	xendev->dev.release = xenbus_dev_release;
 
-	err = bus->get_bus_id(xendev->dev.bus_id, xendev->nodename);
+	err = bus->get_bus_id(devname, xendev->nodename);
 	if (err)
 		goto fail;
 
+	dev_set_name(&xendev->dev, devname);
+
 	/* Register with generic device framework. */
 	err = device_register(&xendev->dev);
 	if (err)
@@ -611,7 +614,7 @@ void xenbus_dev_changed(const char *node, struct xen_bus_type *bus)
 {
 	int exists, rootlen;
 	struct xenbus_device *dev;
-	char type[BUS_ID_SIZE];
+	char type[XEN_BUS_ID_SIZE];
 	const char *p, *root;
 
 	if (char_count(node, '/') < 2)
@@ -625,8 +628,8 @@ void xenbus_dev_changed(const char *node, struct xen_bus_type *bus)
 
 	/* backend/<type>/... or device/<type>/... */
 	p = strchr(node, '/') + 1;
-	snprintf(type, BUS_ID_SIZE, "%.*s", (int)strcspn(p, "/"), p);
-	type[BUS_ID_SIZE-1] = '\0';
+	snprintf(type, XEN_BUS_ID_SIZE, "%.*s", (int)strcspn(p, "/"), p);
+	type[XEN_BUS_ID_SIZE-1] = '\0';
 
 	rootlen = strsep_len(node, '/', bus->levels);
 	if (rootlen < 0)
@@ -674,7 +677,7 @@ static int suspend_dev(struct device *dev, void *data)
 		err = drv->suspend(xdev);
 	if (err)
 		printk(KERN_WARNING
-		       "xenbus: suspend %s failed: %i\n", dev->bus_id, err);
+		       "xenbus: suspend %s failed: %i\n", dev_name(dev), err);
 	return 0;
 }
 
@@ -695,7 +698,7 @@ static int suspend_cancel_dev(struct device *dev, void *data)
 	if (err)
 		printk(KERN_WARNING
 		       "xenbus: suspend_cancel %s failed: %i\n",
-		       dev->bus_id, err);
+		       dev_name(dev), err);
 	return 0;
 }
 
@@ -717,7 +720,7 @@ static int resume_dev(struct device *dev, void *data)
 	if (err) {
 		printk(KERN_WARNING
 		       "xenbus: resume (talk_to_otherend) %s failed: %i\n",
-		       dev->bus_id, err);
+		       dev_name(dev), err);
 		return err;
 	}
 
@@ -728,7 +731,7 @@ static int resume_dev(struct device *dev, void *data)
 		if (err) {
 			printk(KERN_WARNING
 			       "xenbus: resume %s failed: %i\n",
-			       dev->bus_id, err);
+			       dev_name(dev), err);
 			return err;
 		}
 	}
@@ -737,7 +740,7 @@ static int resume_dev(struct device *dev, void *data)
 	if (err) {
 		printk(KERN_WARNING
 		       "xenbus_probe: resume (watch_otherend) %s failed: "
-		       "%d.\n", dev->bus_id, err);
+		       "%d.\n", dev_name(dev), err);
 		return err;
 	}
 
diff --git a/drivers/xen/xenbus/xenbus_probe.h b/drivers/xen/xenbus/xenbus_probe.h
index e09b19415a40..6c5e3185a6a2 100644
--- a/drivers/xen/xenbus/xenbus_probe.h
+++ b/drivers/xen/xenbus/xenbus_probe.h
@@ -34,6 +34,8 @@
 #ifndef _XENBUS_PROBE_H
 #define _XENBUS_PROBE_H
 
+#define XEN_BUS_ID_SIZE			20
+
 #ifdef CONFIG_XEN_BACKEND
 extern void xenbus_backend_suspend(int (*fn)(struct device *, void *));
 extern void xenbus_backend_resume(int (*fn)(struct device *, void *));
@@ -52,7 +54,7 @@ struct xen_bus_type
 {
 	char *root;
 	unsigned int levels;
-	int (*get_bus_id)(char bus_id[BUS_ID_SIZE], const char *nodename);
+	int (*get_bus_id)(char bus_id[XEN_BUS_ID_SIZE], const char *nodename);
 	int (*probe)(const char *type, const char *dir);
 	struct bus_type bus;
 };
diff --git a/firmware/.gitignore b/firmware/.gitignore
index d9c69017bc9a..f89a21fffbf1 100644
--- a/firmware/.gitignore
+++ b/firmware/.gitignore
@@ -3,4 +3,3 @@
 *.bin
 *.csp
 *.dsp
-ihex2fw
diff --git a/firmware/Makefile b/firmware/Makefile
index d872b7942a30..55d3082ea13e 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -99,10 +99,10 @@ quiet_cmd_ihex  = IHEX    $@
       cmd_ihex  = $(OBJCOPY) -Iihex -Obinary $< $@
 
 quiet_cmd_ihex2fw  = IHEX2FW $@
-      cmd_ihex2fw  = $(objtree)/$(obj)/ihex2fw $< $@
+      cmd_ihex2fw  = $(objtree)/scripts/ihex2fw $< $@
 
 quiet_cmd_h16tofw  = H16TOFW $@
-      cmd_h16tofw  = $(objtree)/$(obj)/ihex2fw -w $< $@
+      cmd_h16tofw  = $(objtree)/scripts/ihex2fw -w $< $@
 
 quiet_cmd_fwbin = MK_FW   $@
       cmd_fwbin = FWNAME="$(patsubst firmware/%.gen.S,%,$@)";		     \
@@ -165,11 +165,11 @@ $(obj)/%: $(obj)/%.ihex | $(objtree)/$(obj)/$$(dir %)
 # is actually meaningful, because the firmware has to be loaded in a certain
 # order rather than as a single binary blob. Thus, we convert them into our
 # more compact binary representation of ihex records (<linux/ihex.h>)
-$(obj)/%.fw: $(obj)/%.HEX $(obj)/ihex2fw | $(objtree)/$(obj)/$$(dir %)
+$(obj)/%.fw: $(obj)/%.HEX  | $(objtree)/$(obj)/$$(dir %)
 	$(call cmd,ihex2fw)
 
 # .H16 is our own modified form of Intel HEX, with 16-bit length for records.
-$(obj)/%.fw: $(obj)/%.H16 $(obj)/ihex2fw | $(objtree)/$(obj)/$$(dir %)
+$(obj)/%.fw: $(obj)/%.H16 | $(objtree)/$(obj)/$$(dir %)
 	$(call cmd,h16tofw)
 
 $(firmware-dirs):
@@ -186,5 +186,3 @@ targets := $(fw-shipped-) $(patsubst $(obj)/%,%, \
 # Without this, built-in.o won't be created when it's empty, and the
 # final vmlinux link will fail.
 obj-n := dummy
-
-hostprogs-y := ihex2fw
diff --git a/firmware/dsp56k/bootstrap.asm b/firmware/dsp56k/bootstrap.asm
index 10d891929cd3..a411047e6db3 100644
--- a/firmware/dsp56k/bootstrap.asm
+++ b/firmware/dsp56k/bootstrap.asm
@@ -51,19 +51,19 @@ start   jmp     <$40
         ; Copy DSP program control
         move    #real,r0
         move    #upload,r1
-        do      #upload_end-upload,<_copy
-        move    P:(r0)+,x0
-        move    x0,P:(r1)+
-_copy   movep   #>4,X:<<M_HCR
-        movep   #>$c00,X:<<M_IPR
+        do      #upload_end-upload,_copy
+        movem    P:(r0)+,x0
+        movem    x0,P:(r1)+
+_copy   movep   #4,X:<<M_HCR
+        movep   #$c00,X:<<M_IPR
         and     #<$fe,mr
         jmp     upload
 
 real
         org     P:$7ea9
 upload
-        movep   #>1,X:<<M_PBC
-        movep   #>0,X:<<M_BCR
+        movep   #1,X:<<M_PBC
+        movep   #0,X:<<M_BCR
 
 next    jclr    #0,X:<<M_HSR,*
         movep   X:<<M_HRX,A
@@ -81,18 +81,18 @@ _get_length
         cmp     x0,A
         jeq     load_Y
 
-load_P  do      y0,_load
+load_P  do      y0,_load_P
         jclr    #0,X:<<M_HSR,*
         movep   X:<<M_HRX,P:(r0)+
-_load   jmp     next
-load_X  do      y0,_load
+_load_P jmp     next
+load_X  do      y0,_load_X
         jclr    #0,X:<<M_HSR,*
         movep   X:<<M_HRX,X:(r0)+
-_load   jmp     next
-load_Y  do      y0,_load
+_load_X jmp     next
+load_Y  do      y0,_load_Y
         jclr    #0,X:<<M_HSR,*
         movep   X:<<M_HRX,Y:(r0)+
-_load   jmp     next
+_load_Y jmp     next
 
 upload_end
         end
diff --git a/fs/Kconfig b/fs/Kconfig
index f9b6e2979aaa..32883589ee54 100644
--- a/fs/Kconfig
+++ b/fs/Kconfig
@@ -721,7 +721,20 @@ config CONFIGFS_FS
 
 endmenu
 
-menu "Miscellaneous filesystems"
+menuconfig MISC_FILESYSTEMS
+	bool "Miscellaneous filesystems"
+	default y
+	---help---
+	  Say Y here to get to see options for various miscellaneous
+	  filesystems, such as filesystems that came from other
+	  operating systems.
+
+	  This option alone does not add any kernel code.
+
+	  If you say N, all options in this submenu will be skipped and
+	  disabled; if unsure, say Y here.
+
+if MISC_FILESYSTEMS
 
 config ADFS_FS
 	tristate "ADFS file system support (EXPERIMENTAL)"
@@ -1091,7 +1104,7 @@ config UFS_DEBUG
 	  Y here.  This will result in _many_ additional debugging messages to be
 	  written to the system log.
 
-endmenu
+endif # MISC_FILESYSTEMS
 
 menuconfig NETWORK_FILESYSTEMS
 	bool "Network File Systems"
diff --git a/fs/autofs4/autofs_i.h b/fs/autofs4/autofs_i.h
index e0f16da00e54..a76803108d06 100644
--- a/fs/autofs4/autofs_i.h
+++ b/fs/autofs4/autofs_i.h
@@ -25,8 +25,6 @@
 #define AUTOFS_DEV_IOCTL_IOC_FIRST	(AUTOFS_DEV_IOCTL_VERSION)
 #define AUTOFS_DEV_IOCTL_IOC_COUNT	(AUTOFS_IOC_COUNT - 11)
 
-#define AUTOFS_TYPE_TRIGGER	(AUTOFS_TYPE_DIRECT|AUTOFS_TYPE_OFFSET)
-
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/time.h>
diff --git a/fs/autofs4/dev-ioctl.c b/fs/autofs4/dev-ioctl.c
index 63b7c7afe8df..025e105bffea 100644
--- a/fs/autofs4/dev-ioctl.c
+++ b/fs/autofs4/dev-ioctl.c
@@ -124,7 +124,7 @@ static inline void free_dev_ioctl(struct autofs_dev_ioctl *param)
 
 /*
  * Check sanity of parameter control fields and if a path is present
- * check that it has a "/" and is terminated.
+ * check that it is terminated and contains at least one "/".
  */
 static int validate_dev_ioctl(int cmd, struct autofs_dev_ioctl *param)
 {
@@ -138,15 +138,16 @@ static int validate_dev_ioctl(int cmd, struct autofs_dev_ioctl *param)
 	}
 
 	if (param->size > sizeof(*param)) {
-		err = check_name(param->path);
+		err = invalid_str(param->path,
+				 (void *) ((size_t) param + param->size));
 		if (err) {
-			AUTOFS_WARN("invalid path supplied for cmd(0x%08x)",
-				    cmd);
+			AUTOFS_WARN(
+			  "path string terminator missing for cmd(0x%08x)",
+			  cmd);
 			goto out;
 		}
 
-		err = invalid_str(param->path,
-				 (void *) ((size_t) param + param->size));
+		err = check_name(param->path);
 		if (err) {
 			AUTOFS_WARN("invalid path supplied for cmd(0x%08x)",
 				    cmd);
@@ -180,7 +181,7 @@ static int autofs_dev_ioctl_protover(struct file *fp,
 				     struct autofs_sb_info *sbi,
 				     struct autofs_dev_ioctl *param)
 {
-	param->arg1 = sbi->version;
+	param->protover.version = sbi->version;
 	return 0;
 }
 
@@ -189,7 +190,7 @@ static int autofs_dev_ioctl_protosubver(struct file *fp,
 					struct autofs_sb_info *sbi,
 					struct autofs_dev_ioctl *param)
 {
-	param->arg1 = sbi->sub_version;
+	param->protosubver.sub_version = sbi->sub_version;
 	return 0;
 }
 
@@ -335,13 +336,13 @@ static int autofs_dev_ioctl_openmount(struct file *fp,
 	int err, fd;
 
 	/* param->path has already been checked */
-	if (!param->arg1)
+	if (!param->openmount.devid)
 		return -EINVAL;
 
 	param->ioctlfd = -1;
 
 	path = param->path;
-	devid = param->arg1;
+	devid = param->openmount.devid;
 
 	err = 0;
 	fd = autofs_dev_ioctl_open_mountpoint(path, devid);
@@ -373,7 +374,7 @@ static int autofs_dev_ioctl_ready(struct file *fp,
 {
 	autofs_wqt_t token;
 
-	token = (autofs_wqt_t) param->arg1;
+	token = (autofs_wqt_t) param->ready.token;
 	return autofs4_wait_release(sbi, token, 0);
 }
 
@@ -388,8 +389,8 @@ static int autofs_dev_ioctl_fail(struct file *fp,
 	autofs_wqt_t token;
 	int status;
 
-	token = (autofs_wqt_t) param->arg1;
-	status = param->arg2 ? param->arg2 : -ENOENT;
+	token = (autofs_wqt_t) param->fail.token;
+	status = param->fail.status ? param->fail.status : -ENOENT;
 	return autofs4_wait_release(sbi, token, status);
 }
 
@@ -412,10 +413,10 @@ static int autofs_dev_ioctl_setpipefd(struct file *fp,
 	int pipefd;
 	int err = 0;
 
-	if (param->arg1 == -1)
+	if (param->setpipefd.pipefd == -1)
 		return -EINVAL;
 
-	pipefd = param->arg1;
+	pipefd = param->setpipefd.pipefd;
 
 	mutex_lock(&sbi->wq_mutex);
 	if (!sbi->catatonic) {
@@ -457,8 +458,8 @@ static int autofs_dev_ioctl_timeout(struct file *fp,
 {
 	unsigned long timeout;
 
-	timeout = param->arg1;
-	param->arg1 = sbi->exp_timeout / HZ;
+	timeout = param->timeout.timeout;
+	param->timeout.timeout = sbi->exp_timeout / HZ;
 	sbi->exp_timeout = timeout * HZ;
 	return 0;
 }
@@ -489,7 +490,7 @@ static int autofs_dev_ioctl_requester(struct file *fp,
 	path = param->path;
 	devid = sbi->sb->s_dev;
 
-	param->arg1 = param->arg2 = -1;
+	param->requester.uid = param->requester.gid = -1;
 
 	/* Get nameidata of the parent directory */
 	err = path_lookup(path, LOOKUP_PARENT, &nd);
@@ -505,8 +506,8 @@ static int autofs_dev_ioctl_requester(struct file *fp,
 		err = 0;
 		autofs4_expire_wait(nd.path.dentry);
 		spin_lock(&sbi->fs_lock);
-		param->arg1 = ino->uid;
-		param->arg2 = ino->gid;
+		param->requester.uid = ino->uid;
+		param->requester.gid = ino->gid;
 		spin_unlock(&sbi->fs_lock);
 	}
 
@@ -529,10 +530,10 @@ static int autofs_dev_ioctl_expire(struct file *fp,
 	int err = -EAGAIN;
 	int how;
 
-	how = param->arg1;
+	how = param->expire.how;
 	mnt = fp->f_path.mnt;
 
-	if (sbi->type & AUTOFS_TYPE_TRIGGER)
+	if (autofs_type_trigger(sbi->type))
 		dentry = autofs4_expire_direct(sbi->sb, mnt, sbi, how);
 	else
 		dentry = autofs4_expire_indirect(sbi->sb, mnt, sbi, how);
@@ -565,9 +566,9 @@ static int autofs_dev_ioctl_askumount(struct file *fp,
 				      struct autofs_sb_info *sbi,
 				      struct autofs_dev_ioctl *param)
 {
-	param->arg1 = 0;
+	param->askumount.may_umount = 0;
 	if (may_umount(fp->f_path.mnt))
-		param->arg1 = 1;
+		param->askumount.may_umount = 1;
 	return 0;
 }
 
@@ -600,6 +601,7 @@ static int autofs_dev_ioctl_ismountpoint(struct file *fp,
 	struct nameidata nd;
 	const char *path;
 	unsigned int type;
+	unsigned int devid, magic;
 	int err = -ENOENT;
 
 	if (param->size <= sizeof(*param)) {
@@ -608,13 +610,13 @@ static int autofs_dev_ioctl_ismountpoint(struct file *fp,
 	}
 
 	path = param->path;
-	type = param->arg1;
+	type = param->ismountpoint.in.type;
 
-	param->arg1 = 0;
-	param->arg2 = 0;
+	param->ismountpoint.out.devid = devid = 0;
+	param->ismountpoint.out.magic = magic = 0;
 
 	if (!fp || param->ioctlfd == -1) {
-		if (type == AUTOFS_TYPE_ANY) {
+		if (autofs_type_any(type)) {
 			struct super_block *sb;
 
 			err = path_lookup(path, LOOKUP_FOLLOW, &nd);
@@ -622,7 +624,7 @@ static int autofs_dev_ioctl_ismountpoint(struct file *fp,
 				goto out;
 
 			sb = nd.path.dentry->d_sb;
-			param->arg1 = new_encode_dev(sb->s_dev);
+			devid = new_encode_dev(sb->s_dev);
 		} else {
 			struct autofs_info *ino;
 
@@ -635,38 +637,41 @@ static int autofs_dev_ioctl_ismountpoint(struct file *fp,
 				goto out_release;
 
 			ino = autofs4_dentry_ino(nd.path.dentry);
-			param->arg1 = autofs4_get_dev(ino->sbi);
+			devid = autofs4_get_dev(ino->sbi);
 		}
 
 		err = 0;
 		if (nd.path.dentry->d_inode &&
 		    nd.path.mnt->mnt_root == nd.path.dentry) {
 			err = 1;
-			param->arg2 = nd.path.dentry->d_inode->i_sb->s_magic;
+			magic = nd.path.dentry->d_inode->i_sb->s_magic;
 		}
 	} else {
-		dev_t devid = new_encode_dev(sbi->sb->s_dev);
+		dev_t dev = autofs4_get_dev(sbi);
 
 		err = path_lookup(path, LOOKUP_PARENT, &nd);
 		if (err)
 			goto out;
 
-		err = autofs_dev_ioctl_find_super(&nd, devid);
+		err = autofs_dev_ioctl_find_super(&nd, dev);
 		if (err)
 			goto out_release;
 
-		param->arg1 = autofs4_get_dev(sbi);
+		devid = dev;
 
 		err = have_submounts(nd.path.dentry);
 
 		if (nd.path.mnt->mnt_mountpoint != nd.path.mnt->mnt_root) {
 			if (follow_down(&nd.path.mnt, &nd.path.dentry)) {
 				struct inode *inode = nd.path.dentry->d_inode;
-				param->arg2 = inode->i_sb->s_magic;
+				magic = inode->i_sb->s_magic;
 			}
 		}
 	}
 
+	param->ismountpoint.out.devid = devid;
+	param->ismountpoint.out.magic = magic;
+
 out_release:
 	path_put(&nd.path);
 out:
diff --git a/fs/autofs4/expire.c b/fs/autofs4/expire.c
index 4b6fb3f628c0..e3bd50776f9e 100644
--- a/fs/autofs4/expire.c
+++ b/fs/autofs4/expire.c
@@ -63,7 +63,7 @@ static int autofs4_mount_busy(struct vfsmount *mnt, struct dentry *dentry)
 		struct autofs_sb_info *sbi = autofs4_sbi(dentry->d_sb);
 
 		/* This is an autofs submount, we can't expire it */
-		if (sbi->type == AUTOFS_TYPE_INDIRECT)
+		if (autofs_type_indirect(sbi->type))
 			goto done;
 
 		/*
@@ -490,7 +490,7 @@ int autofs4_expire_multi(struct super_block *sb, struct vfsmount *mnt,
 	if (arg && get_user(do_now, arg))
 		return -EFAULT;
 
-	if (sbi->type & AUTOFS_TYPE_TRIGGER)
+	if (autofs_type_trigger(sbi->type))
 		dentry = autofs4_expire_direct(sb, mnt, sbi, do_now);
 	else
 		dentry = autofs4_expire_indirect(sb, mnt, sbi, do_now);
diff --git a/fs/autofs4/inode.c b/fs/autofs4/inode.c
index cfc23e53b6f4..716e12b627b2 100644
--- a/fs/autofs4/inode.c
+++ b/fs/autofs4/inode.c
@@ -197,9 +197,9 @@ static int autofs4_show_options(struct seq_file *m, struct vfsmount *mnt)
 	seq_printf(m, ",minproto=%d", sbi->min_proto);
 	seq_printf(m, ",maxproto=%d", sbi->max_proto);
 
-	if (sbi->type & AUTOFS_TYPE_OFFSET)
+	if (autofs_type_offset(sbi->type))
 		seq_printf(m, ",offset");
-	else if (sbi->type & AUTOFS_TYPE_DIRECT)
+	else if (autofs_type_direct(sbi->type))
 		seq_printf(m, ",direct");
 	else
 		seq_printf(m, ",indirect");
@@ -284,13 +284,13 @@ static int parse_options(char *options, int *pipefd, uid_t *uid, gid_t *gid,
 			*maxproto = option;
 			break;
 		case Opt_indirect:
-			*type = AUTOFS_TYPE_INDIRECT;
+			set_autofs_type_indirect(type);
 			break;
 		case Opt_direct:
-			*type = AUTOFS_TYPE_DIRECT;
+			set_autofs_type_direct(type);
 			break;
 		case Opt_offset:
-			*type = AUTOFS_TYPE_OFFSET;
+			set_autofs_type_offset(type);
 			break;
 		default:
 			return 1;
@@ -338,7 +338,7 @@ int autofs4_fill_super(struct super_block *s, void *data, int silent)
 	sbi->sb = s;
 	sbi->version = 0;
 	sbi->sub_version = 0;
-	sbi->type = AUTOFS_TYPE_INDIRECT;
+	set_autofs_type_indirect(&sbi->type);
 	sbi->min_proto = 0;
 	sbi->max_proto = 0;
 	mutex_init(&sbi->wq_mutex);
@@ -380,7 +380,7 @@ int autofs4_fill_super(struct super_block *s, void *data, int silent)
 	}
 
 	root_inode->i_fop = &autofs4_root_operations;
-	root_inode->i_op = sbi->type & AUTOFS_TYPE_TRIGGER ?
+	root_inode->i_op = autofs_type_trigger(sbi->type) ?
 			&autofs4_direct_root_inode_operations :
 			&autofs4_indirect_root_inode_operations;
 
diff --git a/fs/autofs4/waitq.c b/fs/autofs4/waitq.c
index e02cc8ae5eb3..eeb246845909 100644
--- a/fs/autofs4/waitq.c
+++ b/fs/autofs4/waitq.c
@@ -337,7 +337,7 @@ int autofs4_wait(struct autofs_sb_info *sbi, struct dentry *dentry,
 		 * is very similar for indirect mounts except only dentrys
 		 * in the root of the autofs file system may be negative.
 		 */
-		if (sbi->type & AUTOFS_TYPE_TRIGGER)
+		if (autofs_type_trigger(sbi->type))
 			return -ENOENT;
 		else if (!IS_ROOT(dentry->d_parent))
 			return -ENOENT;
@@ -348,7 +348,7 @@ int autofs4_wait(struct autofs_sb_info *sbi, struct dentry *dentry,
 		return -ENOMEM;
 
 	/* If this is a direct mount request create a dummy name */
-	if (IS_ROOT(dentry) && sbi->type & AUTOFS_TYPE_TRIGGER)
+	if (IS_ROOT(dentry) && autofs_type_trigger(sbi->type))
 		qstr.len = sprintf(name, "%p", dentry);
 	else {
 		qstr.len = autofs4_getpath(sbi, dentry, &name);
@@ -406,11 +406,11 @@ int autofs4_wait(struct autofs_sb_info *sbi, struct dentry *dentry,
 				type = autofs_ptype_expire_multi;
 		} else {
 			if (notify == NFY_MOUNT)
-				type = (sbi->type & AUTOFS_TYPE_TRIGGER) ?
+				type = autofs_type_trigger(sbi->type) ?
 					autofs_ptype_missing_direct :
 					 autofs_ptype_missing_indirect;
 			else
-				type = (sbi->type & AUTOFS_TYPE_TRIGGER) ?
+				type = autofs_type_trigger(sbi->type) ?
 					autofs_ptype_expire_direct :
 					autofs_ptype_expire_indirect;
 		}
diff --git a/fs/bfs/inode.c b/fs/bfs/inode.c
index 0ed57b5ee012..cc4062d12ca2 100644
--- a/fs/bfs/inode.c
+++ b/fs/bfs/inode.c
@@ -213,6 +213,9 @@ static void bfs_put_super(struct super_block *s)
 {
 	struct bfs_sb_info *info = BFS_SB(s);
 
+	if (!info)
+		return;
+
 	brelse(info->si_sbh);
 	mutex_destroy(&info->bfs_lock);
 	kfree(info->si_imap);
@@ -327,6 +330,7 @@ static int bfs_fill_super(struct super_block *s, void *data, int silent)
 	unsigned i, imap_len;
 	struct bfs_sb_info *info;
 	long ret = -EINVAL;
+	unsigned long i_sblock, i_eblock, i_eoff, s_size;
 
 	info = kzalloc(sizeof(*info), GFP_KERNEL);
 	if (!info)
@@ -350,6 +354,12 @@ static int bfs_fill_super(struct super_block *s, void *data, int silent)
 
 	s->s_magic = BFS_MAGIC;
 	info->si_sbh = bh;
+
+	if (le32_to_cpu(bfs_sb->s_start) > le32_to_cpu(bfs_sb->s_end)) {
+		printf("Superblock is corrupted\n");
+		goto out;
+	}
+
 	info->si_lasti = (le32_to_cpu(bfs_sb->s_start) - BFS_BSIZE) /
 					sizeof(struct bfs_inode)
 					+ BFS_ROOT_INO - 1;
@@ -380,6 +390,18 @@ static int bfs_fill_super(struct super_block *s, void *data, int silent)
 			- le32_to_cpu(bfs_sb->s_start)) >> BFS_BSIZE_BITS;
 	info->si_freei = 0;
 	info->si_lf_eblk = 0;
+
+	/* can we read the last block? */
+	bh = sb_bread(s, info->si_blocks - 1);
+	if (!bh) {
+		printf("Last block not available: %lu\n", info->si_blocks - 1);
+		iput(inode);
+		ret = -EIO;
+		kfree(info->si_imap);
+		goto out;
+	}
+	brelse(bh);
+
 	bh = NULL;
 	for (i = BFS_ROOT_INO; i <= info->si_lasti; i++) {
 		struct bfs_inode *di;
@@ -397,6 +419,29 @@ static int bfs_fill_super(struct super_block *s, void *data, int silent)
 
 		di = (struct bfs_inode *)bh->b_data + off;
 
+		/* test if filesystem is not corrupted */
+
+		i_eoff = le32_to_cpu(di->i_eoffset);
+		i_sblock = le32_to_cpu(di->i_sblock);
+		i_eblock = le32_to_cpu(di->i_eblock);
+		s_size = le32_to_cpu(bfs_sb->s_end);
+
+		if (i_sblock > info->si_blocks ||
+			i_eblock > info->si_blocks ||
+			i_sblock > i_eblock ||
+			i_eoff > s_size ||
+			i_sblock * BFS_BSIZE > i_eoff) {
+
+			printf("Inode 0x%08x corrupted\n", i);
+
+			brelse(bh);
+			s->s_root = NULL;
+			kfree(info->si_imap);
+			kfree(info);
+			s->s_fs_info = NULL;
+			return -EIO;
+		}
+
 		if (!di->i_ino) {
 			info->si_freei++;
 			continue;
diff --git a/fs/binfmt_misc.c b/fs/binfmt_misc.c
index e1158cb4fbd6..c4e83537ead7 100644
--- a/fs/binfmt_misc.c
+++ b/fs/binfmt_misc.c
@@ -649,7 +649,7 @@ static const struct file_operations bm_register_operations = {
 static ssize_t
 bm_status_read(struct file *file, char __user *buf, size_t nbytes, loff_t *ppos)
 {
-	char *s = enabled ? "enabled" : "disabled";
+	char *s = enabled ? "enabled\n" : "disabled\n";
 
 	return simple_read_from_buffer(buf, nbytes, ppos, s, strlen(s));
 }
diff --git a/fs/block_dev.c b/fs/block_dev.c
index 349a26c10001..b957717e25ab 100644
--- a/fs/block_dev.c
+++ b/fs/block_dev.c
@@ -1262,7 +1262,7 @@ EXPORT_SYMBOL(ioctl_by_bdev);
 
 /**
  * lookup_bdev  - lookup a struct block_device by name
- * @path:	special file representing the block device
+ * @pathname:	special file representing the block device
  *
  * Get a reference to the blockdevice at @pathname in the current
  * namespace if possible and return it.  Return ERR_PTR(error)
diff --git a/fs/buffer.c b/fs/buffer.c
index a13f09b696f7..c26da785938a 100644
--- a/fs/buffer.c
+++ b/fs/buffer.c
@@ -2022,7 +2022,6 @@ int block_write_begin(struct file *file, struct address_space *mapping,
 			if (pos + len > inode->i_size)
 				vmtruncate(inode, inode->i_size);
 		}
-		goto out;
 	}
 
 out:
diff --git a/fs/char_dev.c b/fs/char_dev.c
index 700697a72618..38f71222a552 100644
--- a/fs/char_dev.c
+++ b/fs/char_dev.c
@@ -120,7 +120,7 @@ __register_chrdev_region(unsigned int major, unsigned int baseminor,
 	cd->major = major;
 	cd->baseminor = baseminor;
 	cd->minorct = minorct;
-	strncpy(cd->name,name, 64);
+	strlcpy(cd->name, name, sizeof(cd->name));
 
 	i = major_to_index(major);
 
diff --git a/fs/compat.c b/fs/compat.c
index d1ece79b6411..30f2faa22f5c 100644
--- a/fs/compat.c
+++ b/fs/compat.c
@@ -1187,6 +1187,9 @@ compat_sys_readv(unsigned long fd, const struct compat_iovec __user *vec, unsign
 	ret = compat_do_readv_writev(READ, file, vec, vlen, &file->f_pos);
 
 out:
+	if (ret > 0)
+		add_rchar(current, ret);
+	inc_syscr(current);
 	fput(file);
 	return ret;
 }
@@ -1210,6 +1213,9 @@ compat_sys_writev(unsigned long fd, const struct compat_iovec __user *vec, unsig
 	ret = compat_do_readv_writev(WRITE, file, vec, vlen, &file->f_pos);
 
 out:
+	if (ret > 0)
+		add_wchar(current, ret);
+	inc_syscw(current);
 	fput(file);
 	return ret;
 }
diff --git a/fs/debugfs/file.c b/fs/debugfs/file.c
index 159a5efd6a8a..33a90120f6ad 100644
--- a/fs/debugfs/file.c
+++ b/fs/debugfs/file.c
@@ -294,6 +294,38 @@ struct dentry *debugfs_create_x32(const char *name, mode_t mode,
 }
 EXPORT_SYMBOL_GPL(debugfs_create_x32);
 
+
+static int debugfs_size_t_set(void *data, u64 val)
+{
+	*(size_t *)data = val;
+	return 0;
+}
+static int debugfs_size_t_get(void *data, u64 *val)
+{
+	*val = *(size_t *)data;
+	return 0;
+}
+DEFINE_SIMPLE_ATTRIBUTE(fops_size_t, debugfs_size_t_get, debugfs_size_t_set,
+			"%llu\n");	/* %llu and %zu are more or less the same */
+
+/**
+ * debugfs_create_size_t - create a debugfs file that is used to read and write an size_t value
+ * @name: a pointer to a string containing the name of the file to create.
+ * @mode: the permission that the file should have
+ * @parent: a pointer to the parent dentry for this file.  This should be a
+ *          directory dentry if set.  If this parameter is %NULL, then the
+ *          file will be created in the root of the debugfs filesystem.
+ * @value: a pointer to the variable that the file should read to and write
+ *         from.
+ */
+struct dentry *debugfs_create_size_t(const char *name, mode_t mode,
+				     struct dentry *parent, size_t *value)
+{
+	return debugfs_create_file(name, mode, parent, value, &fops_size_t);
+}
+EXPORT_SYMBOL_GPL(debugfs_create_size_t);
+
+
 static ssize_t read_file_bool(struct file *file, char __user *user_buf,
 			      size_t count, loff_t *ppos)
 {
diff --git a/fs/direct-io.c b/fs/direct-io.c
index af0558dbe8b7..b6d43908ff7a 100644
--- a/fs/direct-io.c
+++ b/fs/direct-io.c
@@ -1209,6 +1209,19 @@ __blockdev_direct_IO(int rw, struct kiocb *iocb, struct inode *inode,
 	retval = direct_io_worker(rw, iocb, inode, iov, offset,
 				nr_segs, blkbits, get_block, end_io, dio);
 
+	/*
+	 * In case of error extending write may have instantiated a few
+	 * blocks outside i_size. Trim these off again for DIO_LOCKING.
+	 * NOTE: DIO_NO_LOCK/DIO_OWN_LOCK callers have to handle this by
+	 * it's own meaner.
+	 */
+	if (unlikely(retval < 0 && (rw & WRITE))) {
+		loff_t isize = i_size_read(inode);
+
+		if (end > isize && dio_lock_type == DIO_LOCKING)
+			vmtruncate(inode, isize);
+	}
+
 	if (rw == READ && dio_lock_type == DIO_LOCKING)
 		release_i_mutex = 0;
 
diff --git a/fs/ecryptfs/crypto.c b/fs/ecryptfs/crypto.c
index 6046239465a1..c01e043670e2 100644
--- a/fs/ecryptfs/crypto.c
+++ b/fs/ecryptfs/crypto.c
@@ -175,8 +175,8 @@ out:
  *
  * Returns zero on success; non-zero on error.
  */
-static int ecryptfs_derive_iv(char *iv, struct ecryptfs_crypt_stat *crypt_stat,
-			      loff_t offset)
+int ecryptfs_derive_iv(char *iv, struct ecryptfs_crypt_stat *crypt_stat,
+		       loff_t offset)
 {
 	int rc = 0;
 	char dst[MD5_DIGEST_SIZE];
@@ -924,6 +924,15 @@ static void ecryptfs_copy_mount_wide_flags_to_inode_flags(
 		crypt_stat->flags |= ECRYPTFS_METADATA_IN_XATTR;
 	if (mount_crypt_stat->flags & ECRYPTFS_ENCRYPTED_VIEW_ENABLED)
 		crypt_stat->flags |= ECRYPTFS_VIEW_AS_ENCRYPTED;
+	if (mount_crypt_stat->flags & ECRYPTFS_GLOBAL_ENCRYPT_FILENAMES) {
+		crypt_stat->flags |= ECRYPTFS_ENCRYPT_FILENAMES;
+		if (mount_crypt_stat->flags
+		    & ECRYPTFS_GLOBAL_ENCFN_USE_MOUNT_FNEK)
+			crypt_stat->flags |= ECRYPTFS_ENCFN_USE_MOUNT_FNEK;
+		else if (mount_crypt_stat->flags
+			 & ECRYPTFS_GLOBAL_ENCFN_USE_FEK)
+			crypt_stat->flags |= ECRYPTFS_ENCFN_USE_FEK;
+	}
 }
 
 static int ecryptfs_copy_mount_wide_sigs_to_inode_sigs(
@@ -1060,7 +1069,8 @@ struct ecryptfs_flag_map_elem {
 static struct ecryptfs_flag_map_elem ecryptfs_flag_map[] = {
 	{0x00000001, ECRYPTFS_ENABLE_HMAC},
 	{0x00000002, ECRYPTFS_ENCRYPTED},
-	{0x00000004, ECRYPTFS_METADATA_IN_XATTR}
+	{0x00000004, ECRYPTFS_METADATA_IN_XATTR},
+	{0x00000008, ECRYPTFS_ENCRYPT_FILENAMES}
 };
 
 /**
@@ -1149,19 +1159,20 @@ ecryptfs_cipher_code_str_map[] = {
 
 /**
  * ecryptfs_code_for_cipher_string
- * @crypt_stat: The cryptographic context
+ * @cipher_name: The string alias for the cipher
+ * @key_bytes: Length of key in bytes; used for AES code selection
  *
  * Returns zero on no match, or the cipher code on match
  */
-u8 ecryptfs_code_for_cipher_string(struct ecryptfs_crypt_stat *crypt_stat)
+u8 ecryptfs_code_for_cipher_string(char *cipher_name, size_t key_bytes)
 {
 	int i;
 	u8 code = 0;
 	struct ecryptfs_cipher_code_str_map_elem *map =
 		ecryptfs_cipher_code_str_map;
 
-	if (strcmp(crypt_stat->cipher, "aes") == 0) {
-		switch (crypt_stat->key_size) {
+	if (strcmp(cipher_name, "aes") == 0) {
+		switch (key_bytes) {
 		case 16:
 			code = RFC2440_CIPHER_AES_128;
 			break;
@@ -1173,7 +1184,7 @@ u8 ecryptfs_code_for_cipher_string(struct ecryptfs_crypt_stat *crypt_stat)
 		}
 	} else {
 		for (i = 0; i < ARRAY_SIZE(ecryptfs_cipher_code_str_map); i++)
-			if (strcmp(crypt_stat->cipher, map[i].cipher_str) == 0){
+			if (strcmp(cipher_name, map[i].cipher_str) == 0) {
 				code = map[i].cipher_code;
 				break;
 			}
@@ -1212,6 +1223,8 @@ int ecryptfs_read_and_validate_header_region(char *data,
 		&(ecryptfs_inode_to_private(ecryptfs_inode)->crypt_stat);
 	int rc;
 
+	if (crypt_stat->extent_size == 0)
+		crypt_stat->extent_size = ECRYPTFS_DEFAULT_EXTENT_SIZE;
 	rc = ecryptfs_read_lower(data, 0, crypt_stat->extent_size,
 				 ecryptfs_inode);
 	if (rc) {
@@ -1221,7 +1234,6 @@ int ecryptfs_read_and_validate_header_region(char *data,
 	}
 	if (!contains_ecryptfs_marker(data + ECRYPTFS_FILE_SIZE_BYTES)) {
 		rc = -EINVAL;
-		ecryptfs_printk(KERN_DEBUG, "Valid marker not found\n");
 	}
 out:
 	return rc;
@@ -1628,95 +1640,95 @@ out:
 }
 
 /**
- * ecryptfs_encode_filename - converts a plaintext file name to cipher text
- * @crypt_stat: The crypt_stat struct associated with the file anem to encode
- * @name: The plaintext name
- * @length: The length of the plaintext
- * @encoded_name: The encypted name
+ * ecryptfs_encrypt_filename - encrypt filename
  *
- * Encrypts and encodes a filename into something that constitutes a
- * valid filename for a filesystem, with printable characters.
+ * CBC-encrypts the filename. We do not want to encrypt the same
+ * filename with the same key and IV, which may happen with hard
+ * links, so we prepend random bits to each filename.
  *
- * We assume that we have a properly initialized crypto context,
- * pointed to by crypt_stat->tfm.
- *
- * TODO: Implement filename decoding and decryption here, in place of
- * memcpy. We are keeping the framework around for now to (1)
- * facilitate testing of the components needed to implement filename
- * encryption and (2) to provide a code base from which other
- * developers in the community can easily implement this feature.
- *
- * Returns the length of encoded filename; negative if error
+ * Returns zero on success; non-zero otherwise
  */
-int
-ecryptfs_encode_filename(struct ecryptfs_crypt_stat *crypt_stat,
-			 const char *name, int length, char **encoded_name)
+static int
+ecryptfs_encrypt_filename(struct ecryptfs_filename *filename,
+			  struct ecryptfs_crypt_stat *crypt_stat,
+			  struct ecryptfs_mount_crypt_stat *mount_crypt_stat)
 {
-	int error = 0;
+	int rc = 0;
 
-	(*encoded_name) = kmalloc(length + 2, GFP_KERNEL);
-	if (!(*encoded_name)) {
-		error = -ENOMEM;
+	filename->encrypted_filename = NULL;
+	filename->encrypted_filename_size = 0;
+	if ((crypt_stat && (crypt_stat->flags & ECRYPTFS_ENCFN_USE_MOUNT_FNEK))
+	    || (mount_crypt_stat && (mount_crypt_stat->flags
+				     & ECRYPTFS_GLOBAL_ENCFN_USE_MOUNT_FNEK))) {
+		size_t packet_size;
+		size_t remaining_bytes;
+
+		rc = ecryptfs_write_tag_70_packet(
+			NULL, NULL,
+			&filename->encrypted_filename_size,
+			mount_crypt_stat, NULL,
+			filename->filename_size);
+		if (rc) {
+			printk(KERN_ERR "%s: Error attempting to get packet "
+			       "size for tag 72; rc = [%d]\n", __func__,
+			       rc);
+			filename->encrypted_filename_size = 0;
+			goto out;
+		}
+		filename->encrypted_filename =
+			kmalloc(filename->encrypted_filename_size, GFP_KERNEL);
+		if (!filename->encrypted_filename) {
+			printk(KERN_ERR "%s: Out of memory whilst attempting "
+			       "to kmalloc [%zd] bytes\n", __func__,
+			       filename->encrypted_filename_size);
+			rc = -ENOMEM;
+			goto out;
+		}
+		remaining_bytes = filename->encrypted_filename_size;
+		rc = ecryptfs_write_tag_70_packet(filename->encrypted_filename,
+						  &remaining_bytes,
+						  &packet_size,
+						  mount_crypt_stat,
+						  filename->filename,
+						  filename->filename_size);
+		if (rc) {
+			printk(KERN_ERR "%s: Error attempting to generate "
+			       "tag 70 packet; rc = [%d]\n", __func__,
+			       rc);
+			kfree(filename->encrypted_filename);
+			filename->encrypted_filename = NULL;
+			filename->encrypted_filename_size = 0;
+			goto out;
+		}
+		filename->encrypted_filename_size = packet_size;
+	} else {
+		printk(KERN_ERR "%s: No support for requested filename "
+		       "encryption method in this release\n", __func__);
+		rc = -ENOTSUPP;
 		goto out;
 	}
-	/* TODO: Filename encryption is a scheduled feature for a
-	 * future version of eCryptfs. This function is here only for
-	 * the purpose of providing a framework for other developers
-	 * to easily implement filename encryption. Hint: Replace this
-	 * memcpy() with a call to encrypt and encode the
-	 * filename, the set the length accordingly. */
-	memcpy((void *)(*encoded_name), (void *)name, length);
-	(*encoded_name)[length] = '\0';
-	error = length + 1;
 out:
-	return error;
+	return rc;
 }
 
-/**
- * ecryptfs_decode_filename - converts the cipher text name to plaintext
- * @crypt_stat: The crypt_stat struct associated with the file
- * @name: The filename in cipher text
- * @length: The length of the cipher text name
- * @decrypted_name: The plaintext name
- *
- * Decodes and decrypts the filename.
- *
- * We assume that we have a properly initialized crypto context,
- * pointed to by crypt_stat->tfm.
- *
- * TODO: Implement filename decoding and decryption here, in place of
- * memcpy. We are keeping the framework around for now to (1)
- * facilitate testing of the components needed to implement filename
- * encryption and (2) to provide a code base from which other
- * developers in the community can easily implement this feature.
- *
- * Returns the length of decoded filename; negative if error
- */
-int
-ecryptfs_decode_filename(struct ecryptfs_crypt_stat *crypt_stat,
-			 const char *name, int length, char **decrypted_name)
+static int ecryptfs_copy_filename(char **copied_name, size_t *copied_name_size,
+				  const char *name, size_t name_size)
 {
-	int error = 0;
+	int rc = 0;
 
-	(*decrypted_name) = kmalloc(length + 2, GFP_KERNEL);
-	if (!(*decrypted_name)) {
-		error = -ENOMEM;
+	(*copied_name) = kmalloc((name_size + 2), GFP_KERNEL);
+	if (!(*copied_name)) {
+		rc = -ENOMEM;
 		goto out;
 	}
-	/* TODO: Filename encryption is a scheduled feature for a
-	 * future version of eCryptfs. This function is here only for
-	 * the purpose of providing a framework for other developers
-	 * to easily implement filename encryption. Hint: Replace this
-	 * memcpy() with a call to decode and decrypt the
-	 * filename, the set the length accordingly. */
-	memcpy((void *)(*decrypted_name), (void *)name, length);
-	(*decrypted_name)[length + 1] = '\0';	/* Only for convenience
+	memcpy((void *)(*copied_name), (void *)name, name_size);
+	(*copied_name)[(name_size)] = '\0';	/* Only for convenience
 						 * in printing out the
 						 * string in debug
 						 * messages */
-	error = length;
+	(*copied_name_size) = (name_size + 1);
 out:
-	return error;
+	return rc;
 }
 
 /**
@@ -1740,7 +1752,7 @@ ecryptfs_process_key_cipher(struct crypto_blkcipher **key_tfm,
 	*key_tfm = NULL;
 	if (*key_size > ECRYPTFS_MAX_KEY_BYTES) {
 		rc = -EINVAL;
-		printk(KERN_ERR "Requested key size is [%Zd] bytes; maximum "
+		printk(KERN_ERR "Requested key size is [%zd] bytes; maximum "
 		      "allowable is [%d]\n", *key_size, ECRYPTFS_MAX_KEY_BYTES);
 		goto out;
 	}
@@ -1765,7 +1777,7 @@ ecryptfs_process_key_cipher(struct crypto_blkcipher **key_tfm,
 	get_random_bytes(dummy_key, *key_size);
 	rc = crypto_blkcipher_setkey(*key_tfm, dummy_key, *key_size);
 	if (rc) {
-		printk(KERN_ERR "Error attempting to set key of size [%Zd] for "
+		printk(KERN_ERR "Error attempting to set key of size [%zd] for "
 		       "cipher [%s]; rc = [%d]\n", *key_size, cipher_name, rc);
 		rc = -EINVAL;
 		goto out;
@@ -1910,3 +1922,341 @@ out:
 	mutex_unlock(&key_tfm_list_mutex);
 	return rc;
 }
+
+/* 64 characters forming a 6-bit target field */
+static unsigned char *portable_filename_chars = ("-.0123456789ABCD"
+						 "EFGHIJKLMNOPQRST"
+						 "UVWXYZabcdefghij"
+						 "klmnopqrstuvwxyz");
+
+/* We could either offset on every reverse map or just pad some 0x00's
+ * at the front here */
+static const unsigned char filename_rev_map[] = {
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 7 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 15 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 23 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 31 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 39 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, /* 47 */
+	0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, /* 55 */
+	0x0A, 0x0B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 63 */
+	0x00, 0x0C, 0x0D, 0x0E, 0x0F, 0x10, 0x11, 0x12, /* 71 */
+	0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1A, /* 79 */
+	0x1B, 0x1C, 0x1D, 0x1E, 0x1F, 0x20, 0x21, 0x22, /* 87 */
+	0x23, 0x24, 0x25, 0x00, 0x00, 0x00, 0x00, 0x00, /* 95 */
+	0x00, 0x26, 0x27, 0x28, 0x29, 0x2A, 0x2B, 0x2C, /* 103 */
+	0x2D, 0x2E, 0x2F, 0x30, 0x31, 0x32, 0x33, 0x34, /* 111 */
+	0x35, 0x36, 0x37, 0x38, 0x39, 0x3A, 0x3B, 0x3C, /* 119 */
+	0x3D, 0x3E, 0x3F
+};
+
+/**
+ * ecryptfs_encode_for_filename
+ * @dst: Destination location for encoded filename
+ * @dst_size: Size of the encoded filename in bytes
+ * @src: Source location for the filename to encode
+ * @src_size: Size of the source in bytes
+ */
+void ecryptfs_encode_for_filename(unsigned char *dst, size_t *dst_size,
+				  unsigned char *src, size_t src_size)
+{
+	size_t num_blocks;
+	size_t block_num = 0;
+	size_t dst_offset = 0;
+	unsigned char last_block[3];
+
+	if (src_size == 0) {
+		(*dst_size) = 0;
+		goto out;
+	}
+	num_blocks = (src_size / 3);
+	if ((src_size % 3) == 0) {
+		memcpy(last_block, (&src[src_size - 3]), 3);
+	} else {
+		num_blocks++;
+		last_block[2] = 0x00;
+		switch (src_size % 3) {
+		case 1:
+			last_block[0] = src[src_size - 1];
+			last_block[1] = 0x00;
+			break;
+		case 2:
+			last_block[0] = src[src_size - 2];
+			last_block[1] = src[src_size - 1];
+		}
+	}
+	(*dst_size) = (num_blocks * 4);
+	if (!dst)
+		goto out;
+	while (block_num < num_blocks) {
+		unsigned char *src_block;
+		unsigned char dst_block[4];
+
+		if (block_num == (num_blocks - 1))
+			src_block = last_block;
+		else
+			src_block = &src[block_num * 3];
+		dst_block[0] = ((src_block[0] >> 2) & 0x3F);
+		dst_block[1] = (((src_block[0] << 4) & 0x30)
+				| ((src_block[1] >> 4) & 0x0F));
+		dst_block[2] = (((src_block[1] << 2) & 0x3C)
+				| ((src_block[2] >> 6) & 0x03));
+		dst_block[3] = (src_block[2] & 0x3F);
+		dst[dst_offset++] = portable_filename_chars[dst_block[0]];
+		dst[dst_offset++] = portable_filename_chars[dst_block[1]];
+		dst[dst_offset++] = portable_filename_chars[dst_block[2]];
+		dst[dst_offset++] = portable_filename_chars[dst_block[3]];
+		block_num++;
+	}
+out:
+	return;
+}
+
+/**
+ * ecryptfs_decode_from_filename
+ * @dst: If NULL, this function only sets @dst_size and returns. If
+ *       non-NULL, this function decodes the encoded octets in @src
+ *       into the memory that @dst points to.
+ * @dst_size: Set to the size of the decoded string.
+ * @src: The encoded set of octets to decode.
+ * @src_size: The size of the encoded set of octets to decode.
+ */
+static void
+ecryptfs_decode_from_filename(unsigned char *dst, size_t *dst_size,
+			      const unsigned char *src, size_t src_size)
+{
+	u8 current_bit_offset = 0;
+	size_t src_byte_offset = 0;
+	size_t dst_byte_offset = 0;
+
+	if (dst == NULL) {
+		/* Not exact; conservatively long. Every block of 4
+		 * encoded characters decodes into a block of 3
+		 * decoded characters. This segment of code provides
+		 * the caller with the maximum amount of allocated
+		 * space that @dst will need to point to in a
+		 * subsequent call. */
+		(*dst_size) = (((src_size + 1) * 3) / 4);
+		goto out;
+	}
+	while (src_byte_offset < src_size) {
+		unsigned char src_byte =
+				filename_rev_map[(int)src[src_byte_offset]];
+
+		switch (current_bit_offset) {
+		case 0:
+			dst[dst_byte_offset] = (src_byte << 2);
+			current_bit_offset = 6;
+			break;
+		case 6:
+			dst[dst_byte_offset++] |= (src_byte >> 4);
+			dst[dst_byte_offset] = ((src_byte & 0xF)
+						 << 4);
+			current_bit_offset = 4;
+			break;
+		case 4:
+			dst[dst_byte_offset++] |= (src_byte >> 2);
+			dst[dst_byte_offset] = (src_byte << 6);
+			current_bit_offset = 2;
+			break;
+		case 2:
+			dst[dst_byte_offset++] |= (src_byte);
+			dst[dst_byte_offset] = 0;
+			current_bit_offset = 0;
+			break;
+		}
+		src_byte_offset++;
+	}
+	(*dst_size) = dst_byte_offset;
+out:
+	return;
+}
+
+/**
+ * ecryptfs_encrypt_and_encode_filename - converts a plaintext file name to cipher text
+ * @crypt_stat: The crypt_stat struct associated with the file anem to encode
+ * @name: The plaintext name
+ * @length: The length of the plaintext
+ * @encoded_name: The encypted name
+ *
+ * Encrypts and encodes a filename into something that constitutes a
+ * valid filename for a filesystem, with printable characters.
+ *
+ * We assume that we have a properly initialized crypto context,
+ * pointed to by crypt_stat->tfm.
+ *
+ * Returns zero on success; non-zero on otherwise
+ */
+int ecryptfs_encrypt_and_encode_filename(
+	char **encoded_name,
+	size_t *encoded_name_size,
+	struct ecryptfs_crypt_stat *crypt_stat,
+	struct ecryptfs_mount_crypt_stat *mount_crypt_stat,
+	const char *name, size_t name_size)
+{
+	size_t encoded_name_no_prefix_size;
+	int rc = 0;
+
+	(*encoded_name) = NULL;
+	(*encoded_name_size) = 0;
+	if ((crypt_stat && (crypt_stat->flags & ECRYPTFS_ENCRYPT_FILENAMES))
+	    || (mount_crypt_stat && (mount_crypt_stat->flags
+				     & ECRYPTFS_GLOBAL_ENCRYPT_FILENAMES))) {
+		struct ecryptfs_filename *filename;
+
+		filename = kzalloc(sizeof(*filename), GFP_KERNEL);
+		if (!filename) {
+			printk(KERN_ERR "%s: Out of memory whilst attempting "
+			       "to kzalloc [%zd] bytes\n", __func__,
+			       sizeof(*filename));
+			rc = -ENOMEM;
+			goto out;
+		}
+		filename->filename = (char *)name;
+		filename->filename_size = name_size;
+		rc = ecryptfs_encrypt_filename(filename, crypt_stat,
+					       mount_crypt_stat);
+		if (rc) {
+			printk(KERN_ERR "%s: Error attempting to encrypt "
+			       "filename; rc = [%d]\n", __func__, rc);
+			kfree(filename);
+			goto out;
+		}
+		ecryptfs_encode_for_filename(
+			NULL, &encoded_name_no_prefix_size,
+			filename->encrypted_filename,
+			filename->encrypted_filename_size);
+		if ((crypt_stat && (crypt_stat->flags
+				    & ECRYPTFS_ENCFN_USE_MOUNT_FNEK))
+		    || (mount_crypt_stat
+			&& (mount_crypt_stat->flags
+			    & ECRYPTFS_GLOBAL_ENCFN_USE_MOUNT_FNEK)))
+			(*encoded_name_size) =
+				(ECRYPTFS_FNEK_ENCRYPTED_FILENAME_PREFIX_SIZE
+				 + encoded_name_no_prefix_size);
+		else
+			(*encoded_name_size) =
+				(ECRYPTFS_FEK_ENCRYPTED_FILENAME_PREFIX_SIZE
+				 + encoded_name_no_prefix_size);
+		(*encoded_name) = kmalloc((*encoded_name_size) + 1, GFP_KERNEL);
+		if (!(*encoded_name)) {
+			printk(KERN_ERR "%s: Out of memory whilst attempting "
+			       "to kzalloc [%zd] bytes\n", __func__,
+			       (*encoded_name_size));
+			rc = -ENOMEM;
+			kfree(filename->encrypted_filename);
+			kfree(filename);
+			goto out;
+		}
+		if ((crypt_stat && (crypt_stat->flags
+				    & ECRYPTFS_ENCFN_USE_MOUNT_FNEK))
+		    || (mount_crypt_stat
+			&& (mount_crypt_stat->flags
+			    & ECRYPTFS_GLOBAL_ENCFN_USE_MOUNT_FNEK))) {
+			memcpy((*encoded_name),
+			       ECRYPTFS_FNEK_ENCRYPTED_FILENAME_PREFIX,
+			       ECRYPTFS_FNEK_ENCRYPTED_FILENAME_PREFIX_SIZE);
+			ecryptfs_encode_for_filename(
+			    ((*encoded_name)
+			     + ECRYPTFS_FNEK_ENCRYPTED_FILENAME_PREFIX_SIZE),
+			    &encoded_name_no_prefix_size,
+			    filename->encrypted_filename,
+			    filename->encrypted_filename_size);
+			(*encoded_name_size) =
+				(ECRYPTFS_FNEK_ENCRYPTED_FILENAME_PREFIX_SIZE
+				 + encoded_name_no_prefix_size);
+			(*encoded_name)[(*encoded_name_size)] = '\0';
+			(*encoded_name_size)++;
+		} else {
+			rc = -ENOTSUPP;
+		}
+		if (rc) {
+			printk(KERN_ERR "%s: Error attempting to encode "
+			       "encrypted filename; rc = [%d]\n", __func__,
+			       rc);
+			kfree((*encoded_name));
+			(*encoded_name) = NULL;
+			(*encoded_name_size) = 0;
+		}
+		kfree(filename->encrypted_filename);
+		kfree(filename);
+	} else {
+		rc = ecryptfs_copy_filename(encoded_name,
+					    encoded_name_size,
+					    name, name_size);
+	}
+out:
+	return rc;
+}
+
+/**
+ * ecryptfs_decode_and_decrypt_filename - converts the encoded cipher text name to decoded plaintext
+ * @plaintext_name: The plaintext name
+ * @plaintext_name_size: The plaintext name size
+ * @ecryptfs_dir_dentry: eCryptfs directory dentry
+ * @name: The filename in cipher text
+ * @name_size: The cipher text name size
+ *
+ * Decrypts and decodes the filename.
+ *
+ * Returns zero on error; non-zero otherwise
+ */
+int ecryptfs_decode_and_decrypt_filename(char **plaintext_name,
+					 size_t *plaintext_name_size,
+					 struct dentry *ecryptfs_dir_dentry,
+					 const char *name, size_t name_size)
+{
+	char *decoded_name;
+	size_t decoded_name_size;
+	size_t packet_size;
+	int rc = 0;
+
+	if ((name_size > ECRYPTFS_FNEK_ENCRYPTED_FILENAME_PREFIX_SIZE)
+	    && (strncmp(name, ECRYPTFS_FNEK_ENCRYPTED_FILENAME_PREFIX,
+			ECRYPTFS_FNEK_ENCRYPTED_FILENAME_PREFIX_SIZE) == 0)) {
+		struct ecryptfs_mount_crypt_stat *mount_crypt_stat =
+			&ecryptfs_superblock_to_private(
+				ecryptfs_dir_dentry->d_sb)->mount_crypt_stat;
+		const char *orig_name = name;
+		size_t orig_name_size = name_size;
+
+		name += ECRYPTFS_FNEK_ENCRYPTED_FILENAME_PREFIX_SIZE;
+		name_size -= ECRYPTFS_FNEK_ENCRYPTED_FILENAME_PREFIX_SIZE;
+		ecryptfs_decode_from_filename(NULL, &decoded_name_size,
+					      name, name_size);
+		decoded_name = kmalloc(decoded_name_size, GFP_KERNEL);
+		if (!decoded_name) {
+			printk(KERN_ERR "%s: Out of memory whilst attempting "
+			       "to kmalloc [%zd] bytes\n", __func__,
+			       decoded_name_size);
+			rc = -ENOMEM;
+			goto out;
+		}
+		ecryptfs_decode_from_filename(decoded_name, &decoded_name_size,
+					      name, name_size);
+		rc = ecryptfs_parse_tag_70_packet(plaintext_name,
+						  plaintext_name_size,
+						  &packet_size,
+						  mount_crypt_stat,
+						  decoded_name,
+						  decoded_name_size);
+		if (rc) {
+			printk(KERN_INFO "%s: Could not parse tag 70 packet "
+			       "from filename; copying through filename "
+			       "as-is\n", __func__);
+			rc = ecryptfs_copy_filename(plaintext_name,
+						    plaintext_name_size,
+						    orig_name, orig_name_size);
+			goto out_free;
+		}
+	} else {
+		rc = ecryptfs_copy_filename(plaintext_name,
+					    plaintext_name_size,
+					    name, name_size);
+		goto out;
+	}
+out_free:
+	kfree(decoded_name);
+out:
+	return rc;
+}
diff --git a/fs/ecryptfs/ecryptfs_kernel.h b/fs/ecryptfs/ecryptfs_kernel.h
index a75026d35d16..c11fc95714ab 100644
--- a/fs/ecryptfs/ecryptfs_kernel.h
+++ b/fs/ecryptfs/ecryptfs_kernel.h
@@ -51,12 +51,16 @@
 #define ECRYPTFS_VERSIONING_XATTR                 0x00000010
 #define ECRYPTFS_VERSIONING_MULTKEY               0x00000020
 #define ECRYPTFS_VERSIONING_DEVMISC               0x00000040
+#define ECRYPTFS_VERSIONING_HMAC                  0x00000080
+#define ECRYPTFS_VERSIONING_FILENAME_ENCRYPTION   0x00000100
+#define ECRYPTFS_VERSIONING_GCM                   0x00000200
 #define ECRYPTFS_VERSIONING_MASK (ECRYPTFS_VERSIONING_PASSPHRASE \
 				  | ECRYPTFS_VERSIONING_PLAINTEXT_PASSTHROUGH \
 				  | ECRYPTFS_VERSIONING_PUBKEY \
 				  | ECRYPTFS_VERSIONING_XATTR \
 				  | ECRYPTFS_VERSIONING_MULTKEY \
-				  | ECRYPTFS_VERSIONING_DEVMISC)
+				  | ECRYPTFS_VERSIONING_DEVMISC \
+				  | ECRYPTFS_VERSIONING_FILENAME_ENCRYPTION)
 #define ECRYPTFS_MAX_PASSWORD_LENGTH 64
 #define ECRYPTFS_MAX_PASSPHRASE_BYTES ECRYPTFS_MAX_PASSWORD_LENGTH
 #define ECRYPTFS_SALT_SIZE 8
@@ -199,6 +203,7 @@ ecryptfs_get_key_payload_data(struct key *key)
 #define ECRYPTFS_DEFAULT_CIPHER "aes"
 #define ECRYPTFS_DEFAULT_KEY_BYTES 16
 #define ECRYPTFS_DEFAULT_HASH "md5"
+#define ECRYPTFS_TAG_70_DIGEST ECRYPTFS_DEFAULT_HASH
 #define ECRYPTFS_TAG_1_PACKET_TYPE 0x01
 #define ECRYPTFS_TAG_3_PACKET_TYPE 0x8C
 #define ECRYPTFS_TAG_11_PACKET_TYPE 0xED
@@ -206,30 +211,64 @@ ecryptfs_get_key_payload_data(struct key *key)
 #define ECRYPTFS_TAG_65_PACKET_TYPE 0x41
 #define ECRYPTFS_TAG_66_PACKET_TYPE 0x42
 #define ECRYPTFS_TAG_67_PACKET_TYPE 0x43
+#define ECRYPTFS_TAG_70_PACKET_TYPE 0x46 /* FNEK-encrypted filename
+					  * as dentry name */
+#define ECRYPTFS_TAG_71_PACKET_TYPE 0x47 /* FNEK-encrypted filename in
+					  * metadata */
+#define ECRYPTFS_TAG_72_PACKET_TYPE 0x48 /* FEK-encrypted filename as
+					  * dentry name */
+#define ECRYPTFS_TAG_73_PACKET_TYPE 0x49 /* FEK-encrypted filename as
+					  * metadata */
+/* Constraint: ECRYPTFS_FILENAME_MIN_RANDOM_PREPEND_BYTES >=
+ * ECRYPTFS_MAX_IV_BYTES */
+#define ECRYPTFS_FILENAME_MIN_RANDOM_PREPEND_BYTES 16
+#define ECRYPTFS_NON_NULL 0x42 /* A reasonable substitute for NULL */
 #define MD5_DIGEST_SIZE 16
+#define ECRYPTFS_TAG_70_DIGEST_SIZE MD5_DIGEST_SIZE
+#define ECRYPTFS_FEK_ENCRYPTED_FILENAME_PREFIX "ECRYPTFS_FEK_ENCRYPTED."
+#define ECRYPTFS_FEK_ENCRYPTED_FILENAME_PREFIX_SIZE 23
+#define ECRYPTFS_FNEK_ENCRYPTED_FILENAME_PREFIX "ECRYPTFS_FNEK_ENCRYPTED."
+#define ECRYPTFS_FNEK_ENCRYPTED_FILENAME_PREFIX_SIZE 24
+#define ECRYPTFS_ENCRYPTED_DENTRY_NAME_LEN (18 + 1 + 4 + 1 + 32)
 
 struct ecryptfs_key_sig {
 	struct list_head crypt_stat_list;
 	char keysig[ECRYPTFS_SIG_SIZE_HEX];
 };
 
+struct ecryptfs_filename {
+	struct list_head crypt_stat_list;
+#define ECRYPTFS_FILENAME_CONTAINS_DECRYPTED 0x00000001
+	u32 flags;
+	u32 seq_no;
+	char *filename;
+	char *encrypted_filename;
+	size_t filename_size;
+	size_t encrypted_filename_size;
+	char fnek_sig[ECRYPTFS_SIG_SIZE_HEX];
+	char dentry_name[ECRYPTFS_ENCRYPTED_DENTRY_NAME_LEN + 1];
+};
+
 /**
  * This is the primary struct associated with each encrypted file.
  *
  * TODO: cache align/pack?
  */
 struct ecryptfs_crypt_stat {
-#define ECRYPTFS_STRUCT_INITIALIZED 0x00000001
-#define ECRYPTFS_POLICY_APPLIED     0x00000002
-#define ECRYPTFS_NEW_FILE           0x00000004
-#define ECRYPTFS_ENCRYPTED          0x00000008
-#define ECRYPTFS_SECURITY_WARNING   0x00000010
-#define ECRYPTFS_ENABLE_HMAC        0x00000020
-#define ECRYPTFS_ENCRYPT_IV_PAGES   0x00000040
-#define ECRYPTFS_KEY_VALID          0x00000080
-#define ECRYPTFS_METADATA_IN_XATTR  0x00000100
-#define ECRYPTFS_VIEW_AS_ENCRYPTED  0x00000200
-#define ECRYPTFS_KEY_SET            0x00000400
+#define ECRYPTFS_STRUCT_INITIALIZED   0x00000001
+#define ECRYPTFS_POLICY_APPLIED       0x00000002
+#define ECRYPTFS_NEW_FILE             0x00000004
+#define ECRYPTFS_ENCRYPTED            0x00000008
+#define ECRYPTFS_SECURITY_WARNING     0x00000010
+#define ECRYPTFS_ENABLE_HMAC          0x00000020
+#define ECRYPTFS_ENCRYPT_IV_PAGES     0x00000040
+#define ECRYPTFS_KEY_VALID            0x00000080
+#define ECRYPTFS_METADATA_IN_XATTR    0x00000100
+#define ECRYPTFS_VIEW_AS_ENCRYPTED    0x00000200
+#define ECRYPTFS_KEY_SET              0x00000400
+#define ECRYPTFS_ENCRYPT_FILENAMES    0x00000800
+#define ECRYPTFS_ENCFN_USE_MOUNT_FNEK 0x00001000
+#define ECRYPTFS_ENCFN_USE_FEK        0x00002000
 	u32 flags;
 	unsigned int file_version;
 	size_t iv_bytes;
@@ -332,13 +371,20 @@ struct ecryptfs_mount_crypt_stat {
 #define ECRYPTFS_XATTR_METADATA_ENABLED        0x00000002
 #define ECRYPTFS_ENCRYPTED_VIEW_ENABLED        0x00000004
 #define ECRYPTFS_MOUNT_CRYPT_STAT_INITIALIZED  0x00000008
+#define ECRYPTFS_GLOBAL_ENCRYPT_FILENAMES      0x00000010
+#define ECRYPTFS_GLOBAL_ENCFN_USE_MOUNT_FNEK   0x00000020
+#define ECRYPTFS_GLOBAL_ENCFN_USE_FEK          0x00000040
 	u32 flags;
 	struct list_head global_auth_tok_list;
 	struct mutex global_auth_tok_list_mutex;
 	size_t num_global_auth_toks;
 	size_t global_default_cipher_key_size;
+	size_t global_default_fn_cipher_key_bytes;
 	unsigned char global_default_cipher_name[ECRYPTFS_MAX_CIPHER_NAME_SIZE
 						 + 1];
+	unsigned char global_default_fn_cipher_name[
+		ECRYPTFS_MAX_CIPHER_NAME_SIZE + 1];
+	char global_default_fnek_sig[ECRYPTFS_SIG_SIZE_HEX + 1];
 };
 
 /* superblock private data. */
@@ -571,13 +617,22 @@ struct ecryptfs_open_req {
 int ecryptfs_interpose(struct dentry *hidden_dentry,
 		       struct dentry *this_dentry, struct super_block *sb,
 		       u32 flags);
+int ecryptfs_lookup_and_interpose_lower(struct dentry *ecryptfs_dentry,
+					struct dentry *lower_dentry,
+					struct ecryptfs_crypt_stat *crypt_stat,
+					struct inode *ecryptfs_dir_inode,
+					struct nameidata *ecryptfs_nd);
+int ecryptfs_decode_and_decrypt_filename(char **decrypted_name,
+					 size_t *decrypted_name_size,
+					 struct dentry *ecryptfs_dentry,
+					 const char *name, size_t name_size);
 int ecryptfs_fill_zeros(struct file *file, loff_t new_length);
-int ecryptfs_decode_filename(struct ecryptfs_crypt_stat *crypt_stat,
-			     const char *name, int length,
-			     char **decrypted_name);
-int ecryptfs_encode_filename(struct ecryptfs_crypt_stat *crypt_stat,
-			     const char *name, int length,
-			     char **encoded_name);
+int ecryptfs_encrypt_and_encode_filename(
+	char **encoded_name,
+	size_t *encoded_name_size,
+	struct ecryptfs_crypt_stat *crypt_stat,
+	struct ecryptfs_mount_crypt_stat *mount_crypt_stat,
+	const char *name, size_t name_size);
 struct dentry *ecryptfs_lower_dentry(struct dentry *this_dentry);
 void ecryptfs_dump_hex(char *data, int bytes);
 int virt_to_scatterlist(const void *addr, int size, struct scatterlist *sg,
@@ -599,7 +654,7 @@ int ecryptfs_read_and_validate_header_region(char *data,
 					     struct inode *ecryptfs_inode);
 int ecryptfs_read_and_validate_xattr_region(char *page_virt,
 					    struct dentry *ecryptfs_dentry);
-u8 ecryptfs_code_for_cipher_string(struct ecryptfs_crypt_stat *crypt_stat);
+u8 ecryptfs_code_for_cipher_string(char *cipher_name, size_t key_bytes);
 int ecryptfs_cipher_code_to_string(char *str, u8 cipher_code);
 void ecryptfs_set_default_sizes(struct ecryptfs_crypt_stat *crypt_stat);
 int ecryptfs_generate_key_packet_set(char *dest_base,
@@ -694,5 +749,17 @@ int ecryptfs_privileged_open(struct file **lower_file,
 			     struct vfsmount *lower_mnt,
 			     const struct cred *cred);
 int ecryptfs_init_persistent_file(struct dentry *ecryptfs_dentry);
+int
+ecryptfs_write_tag_70_packet(char *dest, size_t *remaining_bytes,
+			     size_t *packet_size,
+			     struct ecryptfs_mount_crypt_stat *mount_crypt_stat,
+			     char *filename, size_t filename_size);
+int
+ecryptfs_parse_tag_70_packet(char **filename, size_t *filename_size,
+			     size_t *packet_size,
+			     struct ecryptfs_mount_crypt_stat *mount_crypt_stat,
+			     char *data, size_t max_packet_size);
+int ecryptfs_derive_iv(char *iv, struct ecryptfs_crypt_stat *crypt_stat,
+		       loff_t offset);
 
 #endif /* #ifndef ECRYPTFS_KERNEL_H */
diff --git a/fs/ecryptfs/file.c b/fs/ecryptfs/file.c
index 713834371229..9e944057001b 100644
--- a/fs/ecryptfs/file.c
+++ b/fs/ecryptfs/file.c
@@ -77,27 +77,27 @@ struct ecryptfs_getdents_callback {
 
 /* Inspired by generic filldir in fs/readdir.c */
 static int
-ecryptfs_filldir(void *dirent, const char *name, int namelen, loff_t offset,
-		 u64 ino, unsigned int d_type)
+ecryptfs_filldir(void *dirent, const char *lower_name, int lower_namelen,
+		 loff_t offset, u64 ino, unsigned int d_type)
 {
-	struct ecryptfs_crypt_stat *crypt_stat;
 	struct ecryptfs_getdents_callback *buf =
 	    (struct ecryptfs_getdents_callback *)dirent;
+	size_t name_size;
+	char *name;
 	int rc;
-	int decoded_length;
-	char *decoded_name;
 
-	crypt_stat = ecryptfs_dentry_to_private(buf->dentry)->crypt_stat;
 	buf->filldir_called++;
-	decoded_length = ecryptfs_decode_filename(crypt_stat, name, namelen,
-						  &decoded_name);
-	if (decoded_length < 0) {
-		rc = decoded_length;
+	rc = ecryptfs_decode_and_decrypt_filename(&name, &name_size,
+						  buf->dentry, lower_name,
+						  lower_namelen);
+	if (rc) {
+		printk(KERN_ERR "%s: Error attempting to decode and decrypt "
+		       "filename [%s]; rc = [%d]\n", __func__, lower_name,
+		       rc);
 		goto out;
 	}
-	rc = buf->filldir(buf->dirent, decoded_name, decoded_length, offset,
-			  ino, d_type);
-	kfree(decoded_name);
+	rc = buf->filldir(buf->dirent, name, name_size, offset, ino, d_type);
+	kfree(name);
 	if (rc >= 0)
 		buf->entries_written++;
 out:
@@ -106,8 +106,8 @@ out:
 
 /**
  * ecryptfs_readdir
- * @file: The ecryptfs file struct
- * @dirent: Directory entry
+ * @file: The eCryptfs directory file
+ * @dirent: Directory entry handle
  * @filldir: The filldir callback function
  */
 static int ecryptfs_readdir(struct file *file, void *dirent, filldir_t filldir)
diff --git a/fs/ecryptfs/inode.c b/fs/ecryptfs/inode.c
index 0111906a8877..5697899a168d 100644
--- a/fs/ecryptfs/inode.c
+++ b/fs/ecryptfs/inode.c
@@ -52,8 +52,7 @@ static void unlock_dir(struct dentry *dir)
 /**
  * ecryptfs_create_underlying_file
  * @lower_dir_inode: inode of the parent in the lower fs of the new file
- * @lower_dentry: New file's dentry in the lower fs
- * @ecryptfs_dentry: New file's dentry in ecryptfs
+ * @dentry: New file's dentry
  * @mode: The mode of the new file
  * @nd: nameidata of ecryptfs' parent's dentry & vfsmount
  *
@@ -228,8 +227,7 @@ ecryptfs_create(struct inode *directory_inode, struct dentry *ecryptfs_dentry,
 {
 	int rc;
 
-	/* ecryptfs_do_create() calls ecryptfs_interpose(), which opens
-	 * the crypt_stat->lower_file (persistent file) */
+	/* ecryptfs_do_create() calls ecryptfs_interpose() */
 	rc = ecryptfs_do_create(directory_inode, ecryptfs_dentry, mode, nd);
 	if (unlikely(rc)) {
 		ecryptfs_printk(KERN_WARNING, "Failed to create file in"
@@ -244,141 +242,91 @@ out:
 }
 
 /**
- * ecryptfs_lookup
- * @dir: inode
- * @dentry: The dentry
- * @nd: nameidata, may be NULL
- *
- * Find a file on disk. If the file does not exist, then we'll add it to the
- * dentry cache and continue on to read it from the disk.
+ * ecryptfs_lookup_and_interpose_lower - Perform a lookup
  */
-static struct dentry *ecryptfs_lookup(struct inode *dir, struct dentry *dentry,
-				      struct nameidata *nd)
+int ecryptfs_lookup_and_interpose_lower(struct dentry *ecryptfs_dentry,
+					struct dentry *lower_dentry,
+					struct ecryptfs_crypt_stat *crypt_stat,
+					struct inode *ecryptfs_dir_inode,
+					struct nameidata *ecryptfs_nd)
 {
-	int rc = 0;
 	struct dentry *lower_dir_dentry;
-	struct dentry *lower_dentry;
 	struct vfsmount *lower_mnt;
-	char *encoded_name;
-	int encoded_namelen;
-	struct ecryptfs_crypt_stat *crypt_stat = NULL;
+	struct inode *lower_inode;
 	struct ecryptfs_mount_crypt_stat *mount_crypt_stat;
 	char *page_virt = NULL;
-	struct inode *lower_inode;
 	u64 file_size;
+	int rc = 0;
 
-	lower_dir_dentry = ecryptfs_dentry_to_lower(dentry->d_parent);
-	dentry->d_op = &ecryptfs_dops;
-	if ((dentry->d_name.len == 1 && !strcmp(dentry->d_name.name, "."))
-	    || (dentry->d_name.len == 2
-		&& !strcmp(dentry->d_name.name, ".."))) {
-		d_drop(dentry);
-		goto out;
-	}
-	encoded_namelen = ecryptfs_encode_filename(crypt_stat,
-						   dentry->d_name.name,
-						   dentry->d_name.len,
-						   &encoded_name);
-	if (encoded_namelen < 0) {
-		rc = encoded_namelen;
-		d_drop(dentry);
-		goto out;
-	}
-	ecryptfs_printk(KERN_DEBUG, "encoded_name = [%s]; encoded_namelen "
-			"= [%d]\n", encoded_name, encoded_namelen);
-	lower_dentry = lookup_one_len(encoded_name, lower_dir_dentry,
-				      encoded_namelen - 1);
-	kfree(encoded_name);
-	if (IS_ERR(lower_dentry)) {
-		ecryptfs_printk(KERN_ERR, "ERR from lower_dentry\n");
-		rc = PTR_ERR(lower_dentry);
-		d_drop(dentry);
-		goto out;
-	}
-	lower_mnt = mntget(ecryptfs_dentry_to_lower_mnt(dentry->d_parent));
-	ecryptfs_printk(KERN_DEBUG, "lower_dentry = [%p]; lower_dentry->"
-       		"d_name.name = [%s]\n", lower_dentry,
-		lower_dentry->d_name.name);
+	lower_dir_dentry = lower_dentry->d_parent;
+	lower_mnt = mntget(ecryptfs_dentry_to_lower_mnt(
+				   ecryptfs_dentry->d_parent));
 	lower_inode = lower_dentry->d_inode;
-	fsstack_copy_attr_atime(dir, lower_dir_dentry->d_inode);
+	fsstack_copy_attr_atime(ecryptfs_dir_inode, lower_dir_dentry->d_inode);
 	BUG_ON(!atomic_read(&lower_dentry->d_count));
-	ecryptfs_set_dentry_private(dentry,
+	ecryptfs_set_dentry_private(ecryptfs_dentry,
 				    kmem_cache_alloc(ecryptfs_dentry_info_cache,
 						     GFP_KERNEL));
-	if (!ecryptfs_dentry_to_private(dentry)) {
+	if (!ecryptfs_dentry_to_private(ecryptfs_dentry)) {
 		rc = -ENOMEM;
-		ecryptfs_printk(KERN_ERR, "Out of memory whilst attempting "
-				"to allocate ecryptfs_dentry_info struct\n");
+		printk(KERN_ERR "%s: Out of memory whilst attempting "
+		       "to allocate ecryptfs_dentry_info struct\n",
+			__func__);
 		goto out_dput;
 	}
-	ecryptfs_set_dentry_lower(dentry, lower_dentry);
-	ecryptfs_set_dentry_lower_mnt(dentry, lower_mnt);
+	ecryptfs_set_dentry_lower(ecryptfs_dentry, lower_dentry);
+	ecryptfs_set_dentry_lower_mnt(ecryptfs_dentry, lower_mnt);
 	if (!lower_dentry->d_inode) {
 		/* We want to add because we couldn't find in lower */
-		d_add(dentry, NULL);
+		d_add(ecryptfs_dentry, NULL);
 		goto out;
 	}
-	rc = ecryptfs_interpose(lower_dentry, dentry, dir->i_sb,
-				ECRYPTFS_INTERPOSE_FLAG_D_ADD);
+	rc = ecryptfs_interpose(lower_dentry, ecryptfs_dentry,
+				ecryptfs_dir_inode->i_sb, 1);
 	if (rc) {
-		ecryptfs_printk(KERN_ERR, "Error interposing\n");
+		printk(KERN_ERR "%s: Error interposing; rc = [%d]\n",
+		       __func__, rc);
 		goto out;
 	}
-	if (S_ISDIR(lower_inode->i_mode)) {
-		ecryptfs_printk(KERN_DEBUG, "Is a directory; returning\n");
+	if (S_ISDIR(lower_inode->i_mode))
 		goto out;
-	}
-	if (S_ISLNK(lower_inode->i_mode)) {
-		ecryptfs_printk(KERN_DEBUG, "Is a symlink; returning\n");
+	if (S_ISLNK(lower_inode->i_mode))
 		goto out;
-	}
-	if (special_file(lower_inode->i_mode)) {
-		ecryptfs_printk(KERN_DEBUG, "Is a special file; returning\n");
+	if (special_file(lower_inode->i_mode))
 		goto out;
-	}
-	if (!nd) {
-		ecryptfs_printk(KERN_DEBUG, "We have a NULL nd, just leave"
-				"as we *think* we are about to unlink\n");
+	if (!ecryptfs_nd)
 		goto out;
-	}
 	/* Released in this function */
-	page_virt = kmem_cache_zalloc(ecryptfs_header_cache_2,
-				      GFP_USER);
+	page_virt = kmem_cache_zalloc(ecryptfs_header_cache_2, GFP_USER);
 	if (!page_virt) {
+		printk(KERN_ERR "%s: Cannot kmem_cache_zalloc() a page\n",
+		       __func__);
 		rc = -ENOMEM;
-		ecryptfs_printk(KERN_ERR,
-				"Cannot ecryptfs_kmalloc a page\n");
 		goto out;
 	}
-	crypt_stat = &ecryptfs_inode_to_private(dentry->d_inode)->crypt_stat;
-	if (!(crypt_stat->flags & ECRYPTFS_POLICY_APPLIED))
-		ecryptfs_set_default_sizes(crypt_stat);
-	if (!ecryptfs_inode_to_private(dentry->d_inode)->lower_file) {
-		rc = ecryptfs_init_persistent_file(dentry);
+	if (!ecryptfs_inode_to_private(ecryptfs_dentry->d_inode)->lower_file) {
+		rc = ecryptfs_init_persistent_file(ecryptfs_dentry);
 		if (rc) {
 			printk(KERN_ERR "%s: Error attempting to initialize "
 			       "the persistent file for the dentry with name "
 			       "[%s]; rc = [%d]\n", __func__,
-			       dentry->d_name.name, rc);
-			goto out;
+			       ecryptfs_dentry->d_name.name, rc);
+			goto out_free_kmem;
 		}
 	}
 	rc = ecryptfs_read_and_validate_header_region(page_virt,
-						      dentry->d_inode);
+						      ecryptfs_dentry->d_inode);
 	if (rc) {
-		rc = ecryptfs_read_and_validate_xattr_region(page_virt, dentry);
+		rc = ecryptfs_read_and_validate_xattr_region(page_virt,
+							     ecryptfs_dentry);
 		if (rc) {
-			printk(KERN_DEBUG "Valid metadata not found in header "
-			       "region or xattr region; treating file as "
-			       "unencrypted\n");
 			rc = 0;
-			kmem_cache_free(ecryptfs_header_cache_2, page_virt);
-			goto out;
+			goto out_free_kmem;
 		}
 		crypt_stat->flags |= ECRYPTFS_METADATA_IN_XATTR;
 	}
 	mount_crypt_stat = &ecryptfs_superblock_to_private(
-		dentry->d_sb)->mount_crypt_stat;
+		ecryptfs_dentry->d_sb)->mount_crypt_stat;
 	if (mount_crypt_stat->flags & ECRYPTFS_ENCRYPTED_VIEW_ENABLED) {
 		if (crypt_stat->flags & ECRYPTFS_METADATA_IN_XATTR)
 			file_size = (crypt_stat->num_header_bytes_at_front
@@ -388,14 +336,103 @@ static struct dentry *ecryptfs_lookup(struct inode *dir, struct dentry *dentry,
 	} else {
 		file_size = get_unaligned_be64(page_virt);
 	}
-	i_size_write(dentry->d_inode, (loff_t)file_size);
+	i_size_write(ecryptfs_dentry->d_inode, (loff_t)file_size);
+out_free_kmem:
 	kmem_cache_free(ecryptfs_header_cache_2, page_virt);
 	goto out;
-
 out_dput:
 	dput(lower_dentry);
-	d_drop(dentry);
+	d_drop(ecryptfs_dentry);
+out:
+	return rc;
+}
+
+/**
+ * ecryptfs_lookup
+ * @ecryptfs_dir_inode: The eCryptfs directory inode
+ * @ecryptfs_dentry: The eCryptfs dentry that we are looking up
+ * @ecryptfs_nd: nameidata; may be NULL
+ *
+ * Find a file on disk. If the file does not exist, then we'll add it to the
+ * dentry cache and continue on to read it from the disk.
+ */
+static struct dentry *ecryptfs_lookup(struct inode *ecryptfs_dir_inode,
+				      struct dentry *ecryptfs_dentry,
+				      struct nameidata *ecryptfs_nd)
+{
+	char *encrypted_and_encoded_name = NULL;
+	size_t encrypted_and_encoded_name_size;
+	struct ecryptfs_crypt_stat *crypt_stat = NULL;
+	struct ecryptfs_mount_crypt_stat *mount_crypt_stat = NULL;
+	struct ecryptfs_inode_info *inode_info;
+	struct dentry *lower_dir_dentry, *lower_dentry;
+	int rc = 0;
+
+	ecryptfs_dentry->d_op = &ecryptfs_dops;
+	if ((ecryptfs_dentry->d_name.len == 1
+	     && !strcmp(ecryptfs_dentry->d_name.name, "."))
+	    || (ecryptfs_dentry->d_name.len == 2
+		&& !strcmp(ecryptfs_dentry->d_name.name, ".."))) {
+		goto out_d_drop;
+	}
+	lower_dir_dentry = ecryptfs_dentry_to_lower(ecryptfs_dentry->d_parent);
+	lower_dentry = lookup_one_len(ecryptfs_dentry->d_name.name,
+				      lower_dir_dentry,
+				      ecryptfs_dentry->d_name.len);
+	if (IS_ERR(lower_dentry)) {
+		rc = PTR_ERR(lower_dentry);
+		printk(KERN_ERR "%s: lookup_one_len() returned [%d] on "
+		       "lower_dentry = [%s]\n", __func__, rc,
+		       ecryptfs_dentry->d_name.name);
+		goto out_d_drop;
+	}
+	if (lower_dentry->d_inode)
+		goto lookup_and_interpose;
+	inode_info =  ecryptfs_inode_to_private(ecryptfs_dentry->d_inode);
+	if (inode_info) {
+		crypt_stat = &inode_info->crypt_stat;
+		/* TODO: lock for crypt_stat comparison */
+		if (!(crypt_stat->flags & ECRYPTFS_POLICY_APPLIED))
+			ecryptfs_set_default_sizes(crypt_stat);
+	}
+	if (crypt_stat)
+		mount_crypt_stat = crypt_stat->mount_crypt_stat;
+	else
+		mount_crypt_stat = &ecryptfs_superblock_to_private(
+			ecryptfs_dentry->d_sb)->mount_crypt_stat;
+	if (!(crypt_stat && (crypt_stat->flags & ECRYPTFS_ENCRYPT_FILENAMES))
+	    && !(mount_crypt_stat && (mount_crypt_stat->flags
+				     & ECRYPTFS_GLOBAL_ENCRYPT_FILENAMES)))
+		goto lookup_and_interpose;
+	dput(lower_dentry);
+	rc = ecryptfs_encrypt_and_encode_filename(
+		&encrypted_and_encoded_name, &encrypted_and_encoded_name_size,
+		crypt_stat, mount_crypt_stat, ecryptfs_dentry->d_name.name,
+		ecryptfs_dentry->d_name.len);
+	if (rc) {
+		printk(KERN_ERR "%s: Error attempting to encrypt and encode "
+		       "filename; rc = [%d]\n", __func__, rc);
+		goto out_d_drop;
+	}
+	lower_dentry = lookup_one_len(encrypted_and_encoded_name,
+				      lower_dir_dentry,
+				      encrypted_and_encoded_name_size - 1);
+	if (IS_ERR(lower_dentry)) {
+		rc = PTR_ERR(lower_dentry);
+		printk(KERN_ERR "%s: lookup_one_len() returned [%d] on "
+		       "lower_dentry = [%s]\n", __func__, rc,
+		       encrypted_and_encoded_name);
+		goto out_d_drop;
+	}
+lookup_and_interpose:
+	rc = ecryptfs_lookup_and_interpose_lower(ecryptfs_dentry, lower_dentry,
+						 crypt_stat, ecryptfs_dir_inode,
+						 ecryptfs_nd);
+	goto out;
+out_d_drop:
+	d_drop(ecryptfs_dentry);
 out:
+	kfree(encrypted_and_encoded_name);
 	return ERR_PTR(rc);
 }
 
@@ -466,19 +503,21 @@ static int ecryptfs_symlink(struct inode *dir, struct dentry *dentry,
 	struct dentry *lower_dentry;
 	struct dentry *lower_dir_dentry;
 	char *encoded_symname;
-	int encoded_symlen;
-	struct ecryptfs_crypt_stat *crypt_stat = NULL;
+	size_t encoded_symlen;
+	struct ecryptfs_mount_crypt_stat *mount_crypt_stat = NULL;
 
 	lower_dentry = ecryptfs_dentry_to_lower(dentry);
 	dget(lower_dentry);
 	lower_dir_dentry = lock_parent(lower_dentry);
-	encoded_symlen = ecryptfs_encode_filename(crypt_stat, symname,
-						  strlen(symname),
-						  &encoded_symname);
-	if (encoded_symlen < 0) {
-		rc = encoded_symlen;
+	mount_crypt_stat = &ecryptfs_superblock_to_private(
+		dir->i_sb)->mount_crypt_stat;
+	rc = ecryptfs_encrypt_and_encode_filename(&encoded_symname,
+						  &encoded_symlen,
+						  NULL,
+						  mount_crypt_stat, symname,
+						  strlen(symname));
+	if (rc)
 		goto out_lock;
-	}
 	rc = vfs_symlink(lower_dir_dentry->d_inode, lower_dentry,
 			 encoded_symname);
 	kfree(encoded_symname);
@@ -602,52 +641,54 @@ out_lock:
 }
 
 static int
-ecryptfs_readlink(struct dentry *dentry, char __user * buf, int bufsiz)
+ecryptfs_readlink(struct dentry *dentry, char __user *buf, int bufsiz)
 {
-	int rc;
-	struct dentry *lower_dentry;
-	char *decoded_name;
 	char *lower_buf;
-	mm_segment_t old_fs;
+	struct dentry *lower_dentry;
 	struct ecryptfs_crypt_stat *crypt_stat;
+	char *plaintext_name;
+	size_t plaintext_name_size;
+	mm_segment_t old_fs;
+	int rc;
 
 	lower_dentry = ecryptfs_dentry_to_lower(dentry);
 	if (!lower_dentry->d_inode->i_op->readlink) {
 		rc = -EINVAL;
 		goto out;
 	}
+	crypt_stat = &ecryptfs_inode_to_private(dentry->d_inode)->crypt_stat;
 	/* Released in this function */
 	lower_buf = kmalloc(bufsiz, GFP_KERNEL);
 	if (lower_buf == NULL) {
-		ecryptfs_printk(KERN_ERR, "Out of memory\n");
+		printk(KERN_ERR "%s: Out of memory whilst attempting to "
+		       "kmalloc [%d] bytes\n", __func__, bufsiz);
 		rc = -ENOMEM;
 		goto out;
 	}
 	old_fs = get_fs();
 	set_fs(get_ds());
-	ecryptfs_printk(KERN_DEBUG, "Calling readlink w/ "
-			"lower_dentry->d_name.name = [%s]\n",
-			lower_dentry->d_name.name);
 	rc = lower_dentry->d_inode->i_op->readlink(lower_dentry,
 						   (char __user *)lower_buf,
 						   bufsiz);
 	set_fs(old_fs);
 	if (rc >= 0) {
-		crypt_stat = NULL;
-		rc = ecryptfs_decode_filename(crypt_stat, lower_buf, rc,
-					      &decoded_name);
-		if (rc == -ENOMEM)
+		rc = ecryptfs_decode_and_decrypt_filename(&plaintext_name,
+							  &plaintext_name_size,
+							  dentry, lower_buf,
+							  rc);
+		if (rc) {
+			printk(KERN_ERR "%s: Error attempting to decode and "
+			       "decrypt filename; rc = [%d]\n", __func__,
+				rc);
 			goto out_free_lower_buf;
-		if (rc > 0) {
-			ecryptfs_printk(KERN_DEBUG, "Copying [%d] bytes "
-					"to userspace: [%*s]\n", rc,
-					decoded_name);
-			if (copy_to_user(buf, decoded_name, rc))
-				rc = -EFAULT;
 		}
-		kfree(decoded_name);
-		fsstack_copy_attr_atime(dentry->d_inode,
-					lower_dentry->d_inode);
+		rc = copy_to_user(buf, plaintext_name, plaintext_name_size);
+		if (rc)
+			rc = -EFAULT;
+		else
+			rc = plaintext_name_size;
+		kfree(plaintext_name);
+		fsstack_copy_attr_atime(dentry->d_inode, lower_dentry->d_inode);
 	}
 out_free_lower_buf:
 	kfree(lower_buf);
@@ -669,8 +710,6 @@ static void *ecryptfs_follow_link(struct dentry *dentry, struct nameidata *nd)
 	}
 	old_fs = get_fs();
 	set_fs(get_ds());
-	ecryptfs_printk(KERN_DEBUG, "Calling readlink w/ "
-			"dentry->d_name.name = [%s]\n", dentry->d_name.name);
 	rc = dentry->d_inode->i_op->readlink(dentry, (char __user *)buf, len);
 	set_fs(old_fs);
 	if (rc < 0)
diff --git a/fs/ecryptfs/keystore.c b/fs/ecryptfs/keystore.c
index 0d713b691941..ff539420cc6f 100644
--- a/fs/ecryptfs/keystore.c
+++ b/fs/ecryptfs/keystore.c
@@ -358,7 +358,7 @@ parse_tag_67_packet(struct ecryptfs_key_record *key_rec,
 	/* verify that everything through the encrypted FEK size is present */
 	if (message_len < 4) {
 		rc = -EIO;
-		printk(KERN_ERR "%s: message_len is [%Zd]; minimum acceptable "
+		printk(KERN_ERR "%s: message_len is [%zd]; minimum acceptable "
 		       "message length is [%d]\n", __func__, message_len, 4);
 		goto out;
 	}
@@ -385,13 +385,13 @@ parse_tag_67_packet(struct ecryptfs_key_record *key_rec,
 	i += data_len;
 	if (message_len < (i + key_rec->enc_key_size)) {
 		rc = -EIO;
-		printk(KERN_ERR "%s: message_len [%Zd]; max len is [%Zd]\n",
+		printk(KERN_ERR "%s: message_len [%zd]; max len is [%zd]\n",
 		       __func__, message_len, (i + key_rec->enc_key_size));
 		goto out;
 	}
 	if (key_rec->enc_key_size > ECRYPTFS_MAX_ENCRYPTED_KEY_BYTES) {
 		rc = -EIO;
-		printk(KERN_ERR "%s: Encrypted key_size [%Zd] larger than "
+		printk(KERN_ERR "%s: Encrypted key_size [%zd] larger than "
 		       "the maximum key size [%d]\n", __func__,
 		       key_rec->enc_key_size,
 		       ECRYPTFS_MAX_ENCRYPTED_KEY_BYTES);
@@ -403,6 +403,580 @@ out:
 }
 
 static int
+ecryptfs_find_global_auth_tok_for_sig(
+	struct ecryptfs_global_auth_tok **global_auth_tok,
+	struct ecryptfs_mount_crypt_stat *mount_crypt_stat, char *sig)
+{
+	struct ecryptfs_global_auth_tok *walker;
+	int rc = 0;
+
+	(*global_auth_tok) = NULL;
+	mutex_lock(&mount_crypt_stat->global_auth_tok_list_mutex);
+	list_for_each_entry(walker,
+			    &mount_crypt_stat->global_auth_tok_list,
+			    mount_crypt_stat_list) {
+		if (memcmp(walker->sig, sig, ECRYPTFS_SIG_SIZE_HEX) == 0) {
+			(*global_auth_tok) = walker;
+			goto out;
+		}
+	}
+	rc = -EINVAL;
+out:
+	mutex_unlock(&mount_crypt_stat->global_auth_tok_list_mutex);
+	return rc;
+}
+
+/**
+ * ecryptfs_find_auth_tok_for_sig
+ * @auth_tok: Set to the matching auth_tok; NULL if not found
+ * @crypt_stat: inode crypt_stat crypto context
+ * @sig: Sig of auth_tok to find
+ *
+ * For now, this function simply looks at the registered auth_tok's
+ * linked off the mount_crypt_stat, so all the auth_toks that can be
+ * used must be registered at mount time. This function could
+ * potentially try a lot harder to find auth_tok's (e.g., by calling
+ * out to ecryptfsd to dynamically retrieve an auth_tok object) so
+ * that static registration of auth_tok's will no longer be necessary.
+ *
+ * Returns zero on no error; non-zero on error
+ */
+static int
+ecryptfs_find_auth_tok_for_sig(
+	struct ecryptfs_auth_tok **auth_tok,
+	struct ecryptfs_mount_crypt_stat *mount_crypt_stat,
+	char *sig)
+{
+	struct ecryptfs_global_auth_tok *global_auth_tok;
+	int rc = 0;
+
+	(*auth_tok) = NULL;
+	if (ecryptfs_find_global_auth_tok_for_sig(&global_auth_tok,
+						  mount_crypt_stat, sig)) {
+		struct key *auth_tok_key;
+
+		rc = ecryptfs_keyring_auth_tok_for_sig(&auth_tok_key, auth_tok,
+						       sig);
+	} else
+		(*auth_tok) = global_auth_tok->global_auth_tok;
+	return rc;
+}
+
+/**
+ * write_tag_70_packet can gobble a lot of stack space. We stuff most
+ * of the function's parameters in a kmalloc'd struct to help reduce
+ * eCryptfs' overall stack usage.
+ */
+struct ecryptfs_write_tag_70_packet_silly_stack {
+	u8 cipher_code;
+	size_t max_packet_size;
+	size_t packet_size_len;
+	size_t block_aligned_filename_size;
+	size_t block_size;
+	size_t i;
+	size_t j;
+	size_t num_rand_bytes;
+	struct mutex *tfm_mutex;
+	char *block_aligned_filename;
+	struct ecryptfs_auth_tok *auth_tok;
+	struct scatterlist src_sg;
+	struct scatterlist dst_sg;
+	struct blkcipher_desc desc;
+	char iv[ECRYPTFS_MAX_IV_BYTES];
+	char hash[ECRYPTFS_TAG_70_DIGEST_SIZE];
+	char tmp_hash[ECRYPTFS_TAG_70_DIGEST_SIZE];
+	struct hash_desc hash_desc;
+	struct scatterlist hash_sg;
+};
+
+/**
+ * write_tag_70_packet - Write encrypted filename (EFN) packet against FNEK
+ * @filename: NULL-terminated filename string
+ *
+ * This is the simplest mechanism for achieving filename encryption in
+ * eCryptfs. It encrypts the given filename with the mount-wide
+ * filename encryption key (FNEK) and stores it in a packet to @dest,
+ * which the callee will encode and write directly into the dentry
+ * name.
+ */
+int
+ecryptfs_write_tag_70_packet(char *dest, size_t *remaining_bytes,
+			     size_t *packet_size,
+			     struct ecryptfs_mount_crypt_stat *mount_crypt_stat,
+			     char *filename, size_t filename_size)
+{
+	struct ecryptfs_write_tag_70_packet_silly_stack *s;
+	int rc = 0;
+
+	s = kmalloc(sizeof(*s), GFP_KERNEL);
+	if (!s) {
+		printk(KERN_ERR "%s: Out of memory whilst trying to kmalloc "
+		       "[%zd] bytes of kernel memory\n", __func__, sizeof(*s));
+		goto out;
+	}
+	s->desc.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+	(*packet_size) = 0;
+	rc = ecryptfs_get_tfm_and_mutex_for_cipher_name(
+		&s->desc.tfm,
+		&s->tfm_mutex, mount_crypt_stat->global_default_fn_cipher_name);
+	if (unlikely(rc)) {
+		printk(KERN_ERR "Internal error whilst attempting to get "
+		       "tfm and mutex for cipher name [%s]; rc = [%d]\n",
+		       mount_crypt_stat->global_default_fn_cipher_name, rc);
+		goto out;
+	}
+	mutex_lock(s->tfm_mutex);
+	s->block_size = crypto_blkcipher_blocksize(s->desc.tfm);
+	/* Plus one for the \0 separator between the random prefix
+	 * and the plaintext filename */
+	s->num_rand_bytes = (ECRYPTFS_FILENAME_MIN_RANDOM_PREPEND_BYTES + 1);
+	s->block_aligned_filename_size = (s->num_rand_bytes + filename_size);
+	if ((s->block_aligned_filename_size % s->block_size) != 0) {
+		s->num_rand_bytes += (s->block_size
+				      - (s->block_aligned_filename_size
+					 % s->block_size));
+		s->block_aligned_filename_size = (s->num_rand_bytes
+						  + filename_size);
+	}
+	/* Octet 0: Tag 70 identifier
+	 * Octets 1-N1: Tag 70 packet size (includes cipher identifier
+	 *              and block-aligned encrypted filename size)
+	 * Octets N1-N2: FNEK sig (ECRYPTFS_SIG_SIZE)
+	 * Octet N2-N3: Cipher identifier (1 octet)
+	 * Octets N3-N4: Block-aligned encrypted filename
+	 *  - Consists of a minimum number of random characters, a \0
+	 *    separator, and then the filename */
+	s->max_packet_size = (1                   /* Tag 70 identifier */
+			      + 3                 /* Max Tag 70 packet size */
+			      + ECRYPTFS_SIG_SIZE /* FNEK sig */
+			      + 1                 /* Cipher identifier */
+			      + s->block_aligned_filename_size);
+	if (dest == NULL) {
+		(*packet_size) = s->max_packet_size;
+		goto out_unlock;
+	}
+	if (s->max_packet_size > (*remaining_bytes)) {
+		printk(KERN_WARNING "%s: Require [%zd] bytes to write; only "
+		       "[%zd] available\n", __func__, s->max_packet_size,
+		       (*remaining_bytes));
+		rc = -EINVAL;
+		goto out_unlock;
+	}
+	s->block_aligned_filename = kzalloc(s->block_aligned_filename_size,
+					    GFP_KERNEL);
+	if (!s->block_aligned_filename) {
+		printk(KERN_ERR "%s: Out of kernel memory whilst attempting to "
+		       "kzalloc [%zd] bytes\n", __func__,
+		       s->block_aligned_filename_size);
+		rc = -ENOMEM;
+		goto out_unlock;
+	}
+	s->i = 0;
+	dest[s->i++] = ECRYPTFS_TAG_70_PACKET_TYPE;
+	rc = ecryptfs_write_packet_length(&dest[s->i],
+					  (ECRYPTFS_SIG_SIZE
+					   + 1 /* Cipher code */
+					   + s->block_aligned_filename_size),
+					  &s->packet_size_len);
+	if (rc) {
+		printk(KERN_ERR "%s: Error generating tag 70 packet "
+		       "header; cannot generate packet length; rc = [%d]\n",
+		       __func__, rc);
+		goto out_free_unlock;
+	}
+	s->i += s->packet_size_len;
+	ecryptfs_from_hex(&dest[s->i],
+			  mount_crypt_stat->global_default_fnek_sig,
+			  ECRYPTFS_SIG_SIZE);
+	s->i += ECRYPTFS_SIG_SIZE;
+	s->cipher_code = ecryptfs_code_for_cipher_string(
+		mount_crypt_stat->global_default_fn_cipher_name,
+		mount_crypt_stat->global_default_fn_cipher_key_bytes);
+	if (s->cipher_code == 0) {
+		printk(KERN_WARNING "%s: Unable to generate code for "
+		       "cipher [%s] with key bytes [%zd]\n", __func__,
+		       mount_crypt_stat->global_default_fn_cipher_name,
+		       mount_crypt_stat->global_default_fn_cipher_key_bytes);
+		rc = -EINVAL;
+		goto out_free_unlock;
+	}
+	dest[s->i++] = s->cipher_code;
+	rc = ecryptfs_find_auth_tok_for_sig(
+		&s->auth_tok, mount_crypt_stat,
+		mount_crypt_stat->global_default_fnek_sig);
+	if (rc) {
+		printk(KERN_ERR "%s: Error attempting to find auth tok for "
+		       "fnek sig [%s]; rc = [%d]\n", __func__,
+		       mount_crypt_stat->global_default_fnek_sig, rc);
+		goto out_free_unlock;
+	}
+	/* TODO: Support other key modules than passphrase for
+	 * filename encryption */
+	BUG_ON(s->auth_tok->token_type != ECRYPTFS_PASSWORD);
+	sg_init_one(
+		&s->hash_sg,
+		(u8 *)s->auth_tok->token.password.session_key_encryption_key,
+		s->auth_tok->token.password.session_key_encryption_key_bytes);
+	s->hash_desc.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+	s->hash_desc.tfm = crypto_alloc_hash(ECRYPTFS_TAG_70_DIGEST, 0,
+					     CRYPTO_ALG_ASYNC);
+	if (IS_ERR(s->hash_desc.tfm)) {
+			rc = PTR_ERR(s->hash_desc.tfm);
+			printk(KERN_ERR "%s: Error attempting to "
+			       "allocate hash crypto context; rc = [%d]\n",
+			       __func__, rc);
+			goto out_free_unlock;
+	}
+	rc = crypto_hash_init(&s->hash_desc);
+	if (rc) {
+		printk(KERN_ERR
+		       "%s: Error initializing crypto hash; rc = [%d]\n",
+		       __func__, rc);
+		goto out_release_free_unlock;
+	}
+	rc = crypto_hash_update(
+		&s->hash_desc, &s->hash_sg,
+		s->auth_tok->token.password.session_key_encryption_key_bytes);
+	if (rc) {
+		printk(KERN_ERR
+		       "%s: Error updating crypto hash; rc = [%d]\n",
+		       __func__, rc);
+		goto out_release_free_unlock;
+	}
+	rc = crypto_hash_final(&s->hash_desc, s->hash);
+	if (rc) {
+		printk(KERN_ERR
+		       "%s: Error finalizing crypto hash; rc = [%d]\n",
+		       __func__, rc);
+		goto out_release_free_unlock;
+	}
+	for (s->j = 0; s->j < (s->num_rand_bytes - 1); s->j++) {
+		s->block_aligned_filename[s->j] =
+			s->hash[(s->j % ECRYPTFS_TAG_70_DIGEST_SIZE)];
+		if ((s->j % ECRYPTFS_TAG_70_DIGEST_SIZE)
+		    == (ECRYPTFS_TAG_70_DIGEST_SIZE - 1)) {
+			sg_init_one(&s->hash_sg, (u8 *)s->hash,
+				    ECRYPTFS_TAG_70_DIGEST_SIZE);
+			rc = crypto_hash_init(&s->hash_desc);
+			if (rc) {
+				printk(KERN_ERR
+				       "%s: Error initializing crypto hash; "
+				       "rc = [%d]\n", __func__, rc);
+				goto out_release_free_unlock;
+			}
+			rc = crypto_hash_update(&s->hash_desc, &s->hash_sg,
+						ECRYPTFS_TAG_70_DIGEST_SIZE);
+			if (rc) {
+				printk(KERN_ERR
+				       "%s: Error updating crypto hash; "
+				       "rc = [%d]\n", __func__, rc);
+				goto out_release_free_unlock;
+			}
+			rc = crypto_hash_final(&s->hash_desc, s->tmp_hash);
+			if (rc) {
+				printk(KERN_ERR
+				       "%s: Error finalizing crypto hash; "
+				       "rc = [%d]\n", __func__, rc);
+				goto out_release_free_unlock;
+			}
+			memcpy(s->hash, s->tmp_hash,
+			       ECRYPTFS_TAG_70_DIGEST_SIZE);
+		}
+		if (s->block_aligned_filename[s->j] == '\0')
+			s->block_aligned_filename[s->j] = ECRYPTFS_NON_NULL;
+	}
+	memcpy(&s->block_aligned_filename[s->num_rand_bytes], filename,
+	       filename_size);
+	rc = virt_to_scatterlist(s->block_aligned_filename,
+				 s->block_aligned_filename_size, &s->src_sg, 1);
+	if (rc != 1) {
+		printk(KERN_ERR "%s: Internal error whilst attempting to "
+		       "convert filename memory to scatterlist; "
+		       "expected rc = 1; got rc = [%d]. "
+		       "block_aligned_filename_size = [%zd]\n", __func__, rc,
+		       s->block_aligned_filename_size);
+		goto out_release_free_unlock;
+	}
+	rc = virt_to_scatterlist(&dest[s->i], s->block_aligned_filename_size,
+				 &s->dst_sg, 1);
+	if (rc != 1) {
+		printk(KERN_ERR "%s: Internal error whilst attempting to "
+		       "convert encrypted filename memory to scatterlist; "
+		       "expected rc = 1; got rc = [%d]. "
+		       "block_aligned_filename_size = [%zd]\n", __func__, rc,
+		       s->block_aligned_filename_size);
+		goto out_release_free_unlock;
+	}
+	/* The characters in the first block effectively do the job
+	 * of the IV here, so we just use 0's for the IV. Note the
+	 * constraint that ECRYPTFS_FILENAME_MIN_RANDOM_PREPEND_BYTES
+	 * >= ECRYPTFS_MAX_IV_BYTES. */
+	memset(s->iv, 0, ECRYPTFS_MAX_IV_BYTES);
+	s->desc.info = s->iv;
+	rc = crypto_blkcipher_setkey(
+		s->desc.tfm,
+		s->auth_tok->token.password.session_key_encryption_key,
+		mount_crypt_stat->global_default_fn_cipher_key_bytes);
+	if (rc < 0) {
+		printk(KERN_ERR "%s: Error setting key for crypto context; "
+		       "rc = [%d]. s->auth_tok->token.password.session_key_"
+		       "encryption_key = [0x%p]; mount_crypt_stat->"
+		       "global_default_fn_cipher_key_bytes = [%zd]\n", __func__,
+		       rc,
+		       s->auth_tok->token.password.session_key_encryption_key,
+		       mount_crypt_stat->global_default_fn_cipher_key_bytes);
+		goto out_release_free_unlock;
+	}
+	rc = crypto_blkcipher_encrypt_iv(&s->desc, &s->dst_sg, &s->src_sg,
+					 s->block_aligned_filename_size);
+	if (rc) {
+		printk(KERN_ERR "%s: Error attempting to encrypt filename; "
+		       "rc = [%d]\n", __func__, rc);
+		goto out_release_free_unlock;
+	}
+	s->i += s->block_aligned_filename_size;
+	(*packet_size) = s->i;
+	(*remaining_bytes) -= (*packet_size);
+out_release_free_unlock:
+	crypto_free_hash(s->hash_desc.tfm);
+out_free_unlock:
+	memset(s->block_aligned_filename, 0, s->block_aligned_filename_size);
+	kfree(s->block_aligned_filename);
+out_unlock:
+	mutex_unlock(s->tfm_mutex);
+out:
+	kfree(s);
+	return rc;
+}
+
+struct ecryptfs_parse_tag_70_packet_silly_stack {
+	u8 cipher_code;
+	size_t max_packet_size;
+	size_t packet_size_len;
+	size_t parsed_tag_70_packet_size;
+	size_t block_aligned_filename_size;
+	size_t block_size;
+	size_t i;
+	struct mutex *tfm_mutex;
+	char *decrypted_filename;
+	struct ecryptfs_auth_tok *auth_tok;
+	struct scatterlist src_sg;
+	struct scatterlist dst_sg;
+	struct blkcipher_desc desc;
+	char fnek_sig_hex[ECRYPTFS_SIG_SIZE_HEX + 1];
+	char iv[ECRYPTFS_MAX_IV_BYTES];
+	char cipher_string[ECRYPTFS_MAX_CIPHER_NAME_SIZE];
+};
+
+/**
+ * parse_tag_70_packet - Parse and process FNEK-encrypted passphrase packet
+ * @filename: This function kmalloc's the memory for the filename
+ * @filename_size: This function sets this to the amount of memory
+ *                 kmalloc'd for the filename
+ * @packet_size: This function sets this to the the number of octets
+ *               in the packet parsed
+ * @mount_crypt_stat: The mount-wide cryptographic context
+ * @data: The memory location containing the start of the tag 70
+ *        packet
+ * @max_packet_size: The maximum legal size of the packet to be parsed
+ *                   from @data
+ *
+ * Returns zero on success; non-zero otherwise
+ */
+int
+ecryptfs_parse_tag_70_packet(char **filename, size_t *filename_size,
+			     size_t *packet_size,
+			     struct ecryptfs_mount_crypt_stat *mount_crypt_stat,
+			     char *data, size_t max_packet_size)
+{
+	struct ecryptfs_parse_tag_70_packet_silly_stack *s;
+	int rc = 0;
+
+	(*packet_size) = 0;
+	(*filename_size) = 0;
+	(*filename) = NULL;
+	s = kmalloc(sizeof(*s), GFP_KERNEL);
+	if (!s) {
+		printk(KERN_ERR "%s: Out of memory whilst trying to kmalloc "
+		       "[%zd] bytes of kernel memory\n", __func__, sizeof(*s));
+		goto out;
+	}
+	s->desc.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+	if (max_packet_size < (1 + 1 + ECRYPTFS_SIG_SIZE + 1 + 1)) {
+		printk(KERN_WARNING "%s: max_packet_size is [%zd]; it must be "
+		       "at least [%d]\n", __func__, max_packet_size,
+			(1 + 1 + ECRYPTFS_SIG_SIZE + 1 + 1));
+		rc = -EINVAL;
+		goto out;
+	}
+	/* Octet 0: Tag 70 identifier
+	 * Octets 1-N1: Tag 70 packet size (includes cipher identifier
+	 *              and block-aligned encrypted filename size)
+	 * Octets N1-N2: FNEK sig (ECRYPTFS_SIG_SIZE)
+	 * Octet N2-N3: Cipher identifier (1 octet)
+	 * Octets N3-N4: Block-aligned encrypted filename
+	 *  - Consists of a minimum number of random numbers, a \0
+	 *    separator, and then the filename */
+	if (data[(*packet_size)++] != ECRYPTFS_TAG_70_PACKET_TYPE) {
+		printk(KERN_WARNING "%s: Invalid packet tag [0x%.2x]; must be "
+		       "tag [0x%.2x]\n", __func__,
+		       data[((*packet_size) - 1)], ECRYPTFS_TAG_70_PACKET_TYPE);
+		rc = -EINVAL;
+		goto out;
+	}
+	rc = ecryptfs_parse_packet_length(&data[(*packet_size)],
+					  &s->parsed_tag_70_packet_size,
+					  &s->packet_size_len);
+	if (rc) {
+		printk(KERN_WARNING "%s: Error parsing packet length; "
+		       "rc = [%d]\n", __func__, rc);
+		goto out;
+	}
+	s->block_aligned_filename_size = (s->parsed_tag_70_packet_size
+					  - ECRYPTFS_SIG_SIZE - 1);
+	if ((1 + s->packet_size_len + s->parsed_tag_70_packet_size)
+	    > max_packet_size) {
+		printk(KERN_WARNING "%s: max_packet_size is [%zd]; real packet "
+		       "size is [%zd]\n", __func__, max_packet_size,
+		       (1 + s->packet_size_len + 1
+			+ s->block_aligned_filename_size));
+		rc = -EINVAL;
+		goto out;
+	}
+	(*packet_size) += s->packet_size_len;
+	ecryptfs_to_hex(s->fnek_sig_hex, &data[(*packet_size)],
+			ECRYPTFS_SIG_SIZE);
+	s->fnek_sig_hex[ECRYPTFS_SIG_SIZE_HEX] = '\0';
+	(*packet_size) += ECRYPTFS_SIG_SIZE;
+	s->cipher_code = data[(*packet_size)++];
+	rc = ecryptfs_cipher_code_to_string(s->cipher_string, s->cipher_code);
+	if (rc) {
+		printk(KERN_WARNING "%s: Cipher code [%d] is invalid\n",
+		       __func__, s->cipher_code);
+		goto out;
+	}
+	rc = ecryptfs_get_tfm_and_mutex_for_cipher_name(&s->desc.tfm,
+							&s->tfm_mutex,
+							s->cipher_string);
+	if (unlikely(rc)) {
+		printk(KERN_ERR "Internal error whilst attempting to get "
+		       "tfm and mutex for cipher name [%s]; rc = [%d]\n",
+		       s->cipher_string, rc);
+		goto out;
+	}
+	mutex_lock(s->tfm_mutex);
+	rc = virt_to_scatterlist(&data[(*packet_size)],
+				 s->block_aligned_filename_size, &s->src_sg, 1);
+	if (rc != 1) {
+		printk(KERN_ERR "%s: Internal error whilst attempting to "
+		       "convert encrypted filename memory to scatterlist; "
+		       "expected rc = 1; got rc = [%d]. "
+		       "block_aligned_filename_size = [%zd]\n", __func__, rc,
+		       s->block_aligned_filename_size);
+		goto out_unlock;
+	}
+	(*packet_size) += s->block_aligned_filename_size;
+	s->decrypted_filename = kmalloc(s->block_aligned_filename_size,
+					GFP_KERNEL);
+	if (!s->decrypted_filename) {
+		printk(KERN_ERR "%s: Out of memory whilst attempting to "
+		       "kmalloc [%zd] bytes\n", __func__,
+		       s->block_aligned_filename_size);
+		rc = -ENOMEM;
+		goto out_unlock;
+	}
+	rc = virt_to_scatterlist(s->decrypted_filename,
+				 s->block_aligned_filename_size, &s->dst_sg, 1);
+	if (rc != 1) {
+		printk(KERN_ERR "%s: Internal error whilst attempting to "
+		       "convert decrypted filename memory to scatterlist; "
+		       "expected rc = 1; got rc = [%d]. "
+		       "block_aligned_filename_size = [%zd]\n", __func__, rc,
+		       s->block_aligned_filename_size);
+		goto out_free_unlock;
+	}
+	/* The characters in the first block effectively do the job of
+	 * the IV here, so we just use 0's for the IV. Note the
+	 * constraint that ECRYPTFS_FILENAME_MIN_RANDOM_PREPEND_BYTES
+	 * >= ECRYPTFS_MAX_IV_BYTES. */
+	memset(s->iv, 0, ECRYPTFS_MAX_IV_BYTES);
+	s->desc.info = s->iv;
+	rc = ecryptfs_find_auth_tok_for_sig(&s->auth_tok, mount_crypt_stat,
+					    s->fnek_sig_hex);
+	if (rc) {
+		printk(KERN_ERR "%s: Error attempting to find auth tok for "
+		       "fnek sig [%s]; rc = [%d]\n", __func__, s->fnek_sig_hex,
+		       rc);
+		goto out_free_unlock;
+	}
+	/* TODO: Support other key modules than passphrase for
+	 * filename encryption */
+	BUG_ON(s->auth_tok->token_type != ECRYPTFS_PASSWORD);
+	rc = crypto_blkcipher_setkey(
+		s->desc.tfm,
+		s->auth_tok->token.password.session_key_encryption_key,
+		mount_crypt_stat->global_default_fn_cipher_key_bytes);
+	if (rc < 0) {
+		printk(KERN_ERR "%s: Error setting key for crypto context; "
+		       "rc = [%d]. s->auth_tok->token.password.session_key_"
+		       "encryption_key = [0x%p]; mount_crypt_stat->"
+		       "global_default_fn_cipher_key_bytes = [%zd]\n", __func__,
+		       rc,
+		       s->auth_tok->token.password.session_key_encryption_key,
+		       mount_crypt_stat->global_default_fn_cipher_key_bytes);
+		goto out_free_unlock;
+	}
+	rc = crypto_blkcipher_decrypt_iv(&s->desc, &s->dst_sg, &s->src_sg,
+					 s->block_aligned_filename_size);
+	if (rc) {
+		printk(KERN_ERR "%s: Error attempting to decrypt filename; "
+		       "rc = [%d]\n", __func__, rc);
+		goto out_free_unlock;
+	}
+	s->i = 0;
+	while (s->decrypted_filename[s->i] != '\0'
+	       && s->i < s->block_aligned_filename_size)
+		s->i++;
+	if (s->i == s->block_aligned_filename_size) {
+		printk(KERN_WARNING "%s: Invalid tag 70 packet; could not "
+		       "find valid separator between random characters and "
+		       "the filename\n", __func__);
+		rc = -EINVAL;
+		goto out_free_unlock;
+	}
+	s->i++;
+	(*filename_size) = (s->block_aligned_filename_size - s->i);
+	if (!((*filename_size) > 0 && (*filename_size < PATH_MAX))) {
+		printk(KERN_WARNING "%s: Filename size is [%zd], which is "
+		       "invalid\n", __func__, (*filename_size));
+		rc = -EINVAL;
+		goto out_free_unlock;
+	}
+	(*filename) = kmalloc(((*filename_size) + 1), GFP_KERNEL);
+	if (!(*filename)) {
+		printk(KERN_ERR "%s: Out of memory whilst attempting to "
+		       "kmalloc [%zd] bytes\n", __func__,
+		       ((*filename_size) + 1));
+		rc = -ENOMEM;
+		goto out_free_unlock;
+	}
+	memcpy((*filename), &s->decrypted_filename[s->i], (*filename_size));
+	(*filename)[(*filename_size)] = '\0';
+out_free_unlock:
+	kfree(s->decrypted_filename);
+out_unlock:
+	mutex_unlock(s->tfm_mutex);
+out:
+	if (rc) {
+		(*packet_size) = 0;
+		(*filename_size) = 0;
+		(*filename) = NULL;
+	}
+	kfree(s);
+	return rc;
+}
+
+static int
 ecryptfs_get_auth_tok_sig(char **sig, struct ecryptfs_auth_tok *auth_tok)
 {
 	int rc = 0;
@@ -897,30 +1471,6 @@ out:
 	return rc;
 }
 
-static int
-ecryptfs_find_global_auth_tok_for_sig(
-	struct ecryptfs_global_auth_tok **global_auth_tok,
-	struct ecryptfs_mount_crypt_stat *mount_crypt_stat, char *sig)
-{
-	struct ecryptfs_global_auth_tok *walker;
-	int rc = 0;
-
-	(*global_auth_tok) = NULL;
-	mutex_lock(&mount_crypt_stat->global_auth_tok_list_mutex);
-	list_for_each_entry(walker,
-			    &mount_crypt_stat->global_auth_tok_list,
-			    mount_crypt_stat_list) {
-		if (memcmp(walker->sig, sig, ECRYPTFS_SIG_SIZE_HEX) == 0) {
-			(*global_auth_tok) = walker;
-			goto out;
-		}
-	}
-	rc = -EINVAL;
-out:
-	mutex_unlock(&mount_crypt_stat->global_auth_tok_list_mutex);
-	return rc;
-}
-
 /**
  * ecryptfs_verify_version
  * @version: The version number to confirm
@@ -990,43 +1540,6 @@ out:
 }
 
 /**
- * ecryptfs_find_auth_tok_for_sig
- * @auth_tok: Set to the matching auth_tok; NULL if not found
- * @crypt_stat: inode crypt_stat crypto context
- * @sig: Sig of auth_tok to find
- *
- * For now, this function simply looks at the registered auth_tok's
- * linked off the mount_crypt_stat, so all the auth_toks that can be
- * used must be registered at mount time. This function could
- * potentially try a lot harder to find auth_tok's (e.g., by calling
- * out to ecryptfsd to dynamically retrieve an auth_tok object) so
- * that static registration of auth_tok's will no longer be necessary.
- *
- * Returns zero on no error; non-zero on error
- */
-static int
-ecryptfs_find_auth_tok_for_sig(
-	struct ecryptfs_auth_tok **auth_tok,
-	struct ecryptfs_crypt_stat *crypt_stat, char *sig)
-{
-	struct ecryptfs_mount_crypt_stat *mount_crypt_stat =
-		crypt_stat->mount_crypt_stat;
-	struct ecryptfs_global_auth_tok *global_auth_tok;
-	int rc = 0;
-
-	(*auth_tok) = NULL;
-	if (ecryptfs_find_global_auth_tok_for_sig(&global_auth_tok,
-						  mount_crypt_stat, sig)) {
-		struct key *auth_tok_key;
-
-		rc = ecryptfs_keyring_auth_tok_for_sig(&auth_tok_key, auth_tok,
-						       sig);
-	} else
-		(*auth_tok) = global_auth_tok->global_auth_tok;
-	return rc;
-}
-
-/**
  * decrypt_passphrase_encrypted_session_key - Decrypt the session key with the given auth_tok.
  * @auth_tok: The passphrase authentication token to use to encrypt the FEK
  * @crypt_stat: The cryptographic context
@@ -1256,7 +1769,8 @@ find_next_matching_auth_tok:
 			rc = -EINVAL;
 			goto out_wipe_list;
 		}
-		ecryptfs_find_auth_tok_for_sig(&matching_auth_tok, crypt_stat,
+		ecryptfs_find_auth_tok_for_sig(&matching_auth_tok,
+					       crypt_stat->mount_crypt_stat,
 					       candidate_auth_tok_sig);
 		if (matching_auth_tok) {
 			found_auth_tok = 1;
@@ -1336,7 +1850,9 @@ pki_encrypt_session_key(struct ecryptfs_auth_tok *auth_tok,
 	int rc;
 
 	rc = write_tag_66_packet(auth_tok->token.private_key.signature,
-				 ecryptfs_code_for_cipher_string(crypt_stat),
+				 ecryptfs_code_for_cipher_string(
+					 crypt_stat->cipher,
+					 crypt_stat->key_size),
 				 crypt_stat, &payload, &payload_len);
 	if (rc) {
 		ecryptfs_printk(KERN_ERR, "Error generating tag 66 packet\n");
@@ -1696,7 +2212,8 @@ encrypted_session_key_set:
 	dest[(*packet_size)++] = 0x04; /* version 4 */
 	/* TODO: Break from RFC2440 so that arbitrary ciphers can be
 	 * specified with strings */
-	cipher_code = ecryptfs_code_for_cipher_string(crypt_stat);
+	cipher_code = ecryptfs_code_for_cipher_string(crypt_stat->cipher,
+						      crypt_stat->key_size);
 	if (cipher_code == 0) {
 		ecryptfs_printk(KERN_WARNING, "Unable to generate code for "
 				"cipher [%s]\n", crypt_stat->cipher);
diff --git a/fs/ecryptfs/main.c b/fs/ecryptfs/main.c
index fd630713c5c7..789cf2e1be1e 100644
--- a/fs/ecryptfs/main.c
+++ b/fs/ecryptfs/main.c
@@ -206,7 +206,9 @@ enum { ecryptfs_opt_sig, ecryptfs_opt_ecryptfs_sig,
        ecryptfs_opt_cipher, ecryptfs_opt_ecryptfs_cipher,
        ecryptfs_opt_ecryptfs_key_bytes,
        ecryptfs_opt_passthrough, ecryptfs_opt_xattr_metadata,
-       ecryptfs_opt_encrypted_view, ecryptfs_opt_err };
+       ecryptfs_opt_encrypted_view, ecryptfs_opt_fnek_sig,
+       ecryptfs_opt_fn_cipher, ecryptfs_opt_fn_cipher_key_bytes,
+       ecryptfs_opt_err };
 
 static const match_table_t tokens = {
 	{ecryptfs_opt_sig, "sig=%s"},
@@ -217,6 +219,9 @@ static const match_table_t tokens = {
 	{ecryptfs_opt_passthrough, "ecryptfs_passthrough"},
 	{ecryptfs_opt_xattr_metadata, "ecryptfs_xattr_metadata"},
 	{ecryptfs_opt_encrypted_view, "ecryptfs_encrypted_view"},
+	{ecryptfs_opt_fnek_sig, "ecryptfs_fnek_sig=%s"},
+	{ecryptfs_opt_fn_cipher, "ecryptfs_fn_cipher=%s"},
+	{ecryptfs_opt_fn_cipher_key_bytes, "ecryptfs_fn_key_bytes=%u"},
 	{ecryptfs_opt_err, NULL}
 };
 
@@ -281,8 +286,11 @@ static int ecryptfs_parse_options(struct super_block *sb, char *options)
 	int rc = 0;
 	int sig_set = 0;
 	int cipher_name_set = 0;
+	int fn_cipher_name_set = 0;
 	int cipher_key_bytes;
 	int cipher_key_bytes_set = 0;
+	int fn_cipher_key_bytes;
+	int fn_cipher_key_bytes_set = 0;
 	struct ecryptfs_mount_crypt_stat *mount_crypt_stat =
 		&ecryptfs_superblock_to_private(sb)->mount_crypt_stat;
 	substring_t args[MAX_OPT_ARGS];
@@ -290,7 +298,12 @@ static int ecryptfs_parse_options(struct super_block *sb, char *options)
 	char *sig_src;
 	char *cipher_name_dst;
 	char *cipher_name_src;
+	char *fn_cipher_name_dst;
+	char *fn_cipher_name_src;
+	char *fnek_dst;
+	char *fnek_src;
 	char *cipher_key_bytes_src;
+	char *fn_cipher_key_bytes_src;
 
 	if (!options) {
 		rc = -EINVAL;
@@ -322,10 +335,7 @@ static int ecryptfs_parse_options(struct super_block *sb, char *options)
 				global_default_cipher_name;
 			strncpy(cipher_name_dst, cipher_name_src,
 				ECRYPTFS_MAX_CIPHER_NAME_SIZE);
-			ecryptfs_printk(KERN_DEBUG,
-					"The mount_crypt_stat "
-					"global_default_cipher_name set to: "
-					"[%s]\n", cipher_name_dst);
+			cipher_name_dst[ECRYPTFS_MAX_CIPHER_NAME_SIZE] = '\0';
 			cipher_name_set = 1;
 			break;
 		case ecryptfs_opt_ecryptfs_key_bytes:
@@ -335,11 +345,6 @@ static int ecryptfs_parse_options(struct super_block *sb, char *options)
 						   &cipher_key_bytes_src, 0);
 			mount_crypt_stat->global_default_cipher_key_size =
 				cipher_key_bytes;
-			ecryptfs_printk(KERN_DEBUG,
-					"The mount_crypt_stat "
-					"global_default_cipher_key_size "
-					"set to: [%d]\n", mount_crypt_stat->
-					global_default_cipher_key_size);
 			cipher_key_bytes_set = 1;
 			break;
 		case ecryptfs_opt_passthrough:
@@ -356,11 +361,51 @@ static int ecryptfs_parse_options(struct super_block *sb, char *options)
 			mount_crypt_stat->flags |=
 				ECRYPTFS_ENCRYPTED_VIEW_ENABLED;
 			break;
+		case ecryptfs_opt_fnek_sig:
+			fnek_src = args[0].from;
+			fnek_dst =
+				mount_crypt_stat->global_default_fnek_sig;
+			strncpy(fnek_dst, fnek_src, ECRYPTFS_SIG_SIZE_HEX);
+			mount_crypt_stat->global_default_fnek_sig[
+				ECRYPTFS_SIG_SIZE_HEX] = '\0';
+			rc = ecryptfs_add_global_auth_tok(
+				mount_crypt_stat,
+				mount_crypt_stat->global_default_fnek_sig);
+			if (rc) {
+				printk(KERN_ERR "Error attempting to register "
+				       "global fnek sig [%s]; rc = [%d]\n",
+				       mount_crypt_stat->global_default_fnek_sig,
+				       rc);
+				goto out;
+			}
+			mount_crypt_stat->flags |=
+				(ECRYPTFS_GLOBAL_ENCRYPT_FILENAMES
+				 | ECRYPTFS_GLOBAL_ENCFN_USE_MOUNT_FNEK);
+			break;
+		case ecryptfs_opt_fn_cipher:
+			fn_cipher_name_src = args[0].from;
+			fn_cipher_name_dst =
+				mount_crypt_stat->global_default_fn_cipher_name;
+			strncpy(fn_cipher_name_dst, fn_cipher_name_src,
+				ECRYPTFS_MAX_CIPHER_NAME_SIZE);
+			mount_crypt_stat->global_default_fn_cipher_name[
+				ECRYPTFS_MAX_CIPHER_NAME_SIZE] = '\0';
+			fn_cipher_name_set = 1;
+			break;
+		case ecryptfs_opt_fn_cipher_key_bytes:
+			fn_cipher_key_bytes_src = args[0].from;
+			fn_cipher_key_bytes =
+				(int)simple_strtol(fn_cipher_key_bytes_src,
+						   &fn_cipher_key_bytes_src, 0);
+			mount_crypt_stat->global_default_fn_cipher_key_bytes =
+				fn_cipher_key_bytes;
+			fn_cipher_key_bytes_set = 1;
+			break;
 		case ecryptfs_opt_err:
 		default:
-			ecryptfs_printk(KERN_WARNING,
-					"eCryptfs: unrecognized option '%s'\n",
-					p);
+			printk(KERN_WARNING
+			       "%s: eCryptfs: unrecognized option [%s]\n",
+			       __func__, p);
 		}
 	}
 	if (!sig_set) {
@@ -374,33 +419,60 @@ static int ecryptfs_parse_options(struct super_block *sb, char *options)
 		int cipher_name_len = strlen(ECRYPTFS_DEFAULT_CIPHER);
 
 		BUG_ON(cipher_name_len >= ECRYPTFS_MAX_CIPHER_NAME_SIZE);
-
 		strcpy(mount_crypt_stat->global_default_cipher_name,
 		       ECRYPTFS_DEFAULT_CIPHER);
 	}
-	if (!cipher_key_bytes_set) {
+	if ((mount_crypt_stat->flags & ECRYPTFS_GLOBAL_ENCRYPT_FILENAMES)
+	    && !fn_cipher_name_set)
+		strcpy(mount_crypt_stat->global_default_fn_cipher_name,
+		       mount_crypt_stat->global_default_cipher_name);
+	if (!cipher_key_bytes_set)
 		mount_crypt_stat->global_default_cipher_key_size = 0;
-	}
+	if ((mount_crypt_stat->flags & ECRYPTFS_GLOBAL_ENCRYPT_FILENAMES)
+	    && !fn_cipher_key_bytes_set)
+		mount_crypt_stat->global_default_fn_cipher_key_bytes =
+			mount_crypt_stat->global_default_cipher_key_size;
 	mutex_lock(&key_tfm_list_mutex);
 	if (!ecryptfs_tfm_exists(mount_crypt_stat->global_default_cipher_name,
-				 NULL))
+				 NULL)) {
 		rc = ecryptfs_add_new_key_tfm(
 			NULL, mount_crypt_stat->global_default_cipher_name,
 			mount_crypt_stat->global_default_cipher_key_size);
-	mutex_unlock(&key_tfm_list_mutex);
-	if (rc) {
-		printk(KERN_ERR "Error attempting to initialize cipher with "
-		       "name = [%s] and key size = [%td]; rc = [%d]\n",
-		       mount_crypt_stat->global_default_cipher_name,
-		       mount_crypt_stat->global_default_cipher_key_size, rc);
-		rc = -EINVAL;
-		goto out;
+		if (rc) {
+			printk(KERN_ERR "Error attempting to initialize "
+			       "cipher with name = [%s] and key size = [%td]; "
+			       "rc = [%d]\n",
+			       mount_crypt_stat->global_default_cipher_name,
+			       mount_crypt_stat->global_default_cipher_key_size,
+			       rc);
+			rc = -EINVAL;
+			mutex_unlock(&key_tfm_list_mutex);
+			goto out;
+		}
 	}
+	if ((mount_crypt_stat->flags & ECRYPTFS_GLOBAL_ENCRYPT_FILENAMES)
+	    && !ecryptfs_tfm_exists(
+		    mount_crypt_stat->global_default_fn_cipher_name, NULL)) {
+		rc = ecryptfs_add_new_key_tfm(
+			NULL, mount_crypt_stat->global_default_fn_cipher_name,
+			mount_crypt_stat->global_default_fn_cipher_key_bytes);
+		if (rc) {
+			printk(KERN_ERR "Error attempting to initialize "
+			       "cipher with name = [%s] and key size = [%td]; "
+			       "rc = [%d]\n",
+			       mount_crypt_stat->global_default_fn_cipher_name,
+			       mount_crypt_stat->global_default_fn_cipher_key_bytes,
+			       rc);
+			rc = -EINVAL;
+			mutex_unlock(&key_tfm_list_mutex);
+			goto out;
+		}
+	}
+	mutex_unlock(&key_tfm_list_mutex);
 	rc = ecryptfs_init_global_auth_toks(mount_crypt_stat);
-	if (rc) {
+	if (rc)
 		printk(KERN_WARNING "One or more global auth toks could not "
 		       "properly register; rc = [%d]\n", rc);
-	}
 out:
 	return rc;
 }
diff --git a/fs/ecryptfs/messaging.c b/fs/ecryptfs/messaging.c
index 6913f727624d..96ef51489e01 100644
--- a/fs/ecryptfs/messaging.c
+++ b/fs/ecryptfs/messaging.c
@@ -193,7 +193,7 @@ ecryptfs_spawn_daemon(struct ecryptfs_daemon **daemon, uid_t euid,
 	(*daemon) = kzalloc(sizeof(**daemon), GFP_KERNEL);
 	if (!(*daemon)) {
 		rc = -ENOMEM;
-		printk(KERN_ERR "%s: Failed to allocate [%Zd] bytes of "
+		printk(KERN_ERR "%s: Failed to allocate [%zd] bytes of "
 		       "GFP_KERNEL memory\n", __func__, sizeof(**daemon));
 		goto out;
 	}
@@ -435,7 +435,7 @@ int ecryptfs_process_response(struct ecryptfs_message *msg, uid_t euid,
 	msg_ctx->msg = kmalloc(msg_size, GFP_KERNEL);
 	if (!msg_ctx->msg) {
 		rc = -ENOMEM;
-		printk(KERN_ERR "%s: Failed to allocate [%Zd] bytes of "
+		printk(KERN_ERR "%s: Failed to allocate [%zd] bytes of "
 		       "GFP_KERNEL memory\n", __func__, msg_size);
 		goto unlock;
 	}
diff --git a/fs/ecryptfs/miscdev.c b/fs/ecryptfs/miscdev.c
index efd95a0ed1ea..a67fea655f49 100644
--- a/fs/ecryptfs/miscdev.c
+++ b/fs/ecryptfs/miscdev.c
@@ -199,7 +199,7 @@ int ecryptfs_send_miscdev(char *data, size_t data_size,
 		if (!msg_ctx->msg) {
 			rc = -ENOMEM;
 			printk(KERN_ERR "%s: Out of memory whilst attempting "
-			       "to kmalloc(%Zd, GFP_KERNEL)\n", __func__,
+			       "to kmalloc(%zd, GFP_KERNEL)\n", __func__,
 			       (sizeof(*msg_ctx->msg) + data_size));
 			goto out_unlock;
 		}
@@ -322,7 +322,7 @@ check_list:
 	if (count < total_length) {
 		rc = 0;
 		printk(KERN_WARNING "%s: Only given user buffer of "
-		       "size [%Zd], but we need [%Zd] to read the "
+		       "size [%zd], but we need [%zd] to read the "
 		       "pending message\n", __func__, count, total_length);
 		goto out_unlock_msg_ctx;
 	}
@@ -376,7 +376,7 @@ static int ecryptfs_miscdev_response(char *data, size_t data_size,
 
 	if ((sizeof(*msg) + msg->data_len) != data_size) {
 		printk(KERN_WARNING "%s: (sizeof(*msg) + msg->data_len) = "
-		       "[%Zd]; data_size = [%Zd]. Invalid packet.\n", __func__,
+		       "[%zd]; data_size = [%zd]. Invalid packet.\n", __func__,
 		       (sizeof(*msg) + msg->data_len), data_size);
 		rc = -EINVAL;
 		goto out;
@@ -421,7 +421,7 @@ ecryptfs_miscdev_write(struct file *file, const char __user *buf,
 	data = kmalloc(count, GFP_KERNEL);
 	if (!data) {
 		printk(KERN_ERR "%s: Out of memory whilst attempting to "
-		       "kmalloc([%Zd], GFP_KERNEL)\n", __func__, count);
+		       "kmalloc([%zd], GFP_KERNEL)\n", __func__, count);
 		goto out;
 	}
 	rc = copy_from_user(data, buf, count);
@@ -436,8 +436,8 @@ ecryptfs_miscdev_write(struct file *file, const char __user *buf,
 	case ECRYPTFS_MSG_RESPONSE:
 		if (count < (1 + 4 + 1 + sizeof(struct ecryptfs_message))) {
 			printk(KERN_WARNING "%s: Minimum acceptable packet "
-			       "size is [%Zd], but amount of data written is "
-			       "only [%Zd]. Discarding response packet.\n",
+			       "size is [%zd], but amount of data written is "
+			       "only [%zd]. Discarding response packet.\n",
 			       __func__,
 			       (1 + 4 + 1 + sizeof(struct ecryptfs_message)),
 			       count);
@@ -455,9 +455,9 @@ ecryptfs_miscdev_write(struct file *file, const char __user *buf,
 		}
 		i += packet_size_length;
 		if ((1 + 4 + packet_size_length + packet_size) != count) {
-			printk(KERN_WARNING "%s: (1 + packet_size_length([%Zd])"
-			       " + packet_size([%Zd]))([%Zd]) != "
-			       "count([%Zd]). Invalid packet format.\n",
+			printk(KERN_WARNING "%s: (1 + packet_size_length([%zd])"
+			       " + packet_size([%zd]))([%zd]) != "
+			       "count([%zd]). Invalid packet format.\n",
 			       __func__, packet_size_length, packet_size,
 			       (1 + packet_size_length + packet_size), count);
 			goto out_free;
diff --git a/fs/exec.c b/fs/exec.c
index 9c33f542dc77..71a6efe5d8bd 100644
--- a/fs/exec.c
+++ b/fs/exec.c
@@ -232,13 +232,13 @@ static void flush_arg_page(struct linux_binprm *bprm, unsigned long pos,
 
 static int __bprm_mm_init(struct linux_binprm *bprm)
 {
-	int err = -ENOMEM;
+	int err;
 	struct vm_area_struct *vma = NULL;
 	struct mm_struct *mm = bprm->mm;
 
 	bprm->vma = vma = kmem_cache_zalloc(vm_area_cachep, GFP_KERNEL);
 	if (!vma)
-		goto err;
+		return -ENOMEM;
 
 	down_write(&mm->mmap_sem);
 	vma->vm_mm = mm;
@@ -251,28 +251,20 @@ static int __bprm_mm_init(struct linux_binprm *bprm)
 	 */
 	vma->vm_end = STACK_TOP_MAX;
 	vma->vm_start = vma->vm_end - PAGE_SIZE;
-
 	vma->vm_flags = VM_STACK_FLAGS;
 	vma->vm_page_prot = vm_get_page_prot(vma->vm_flags);
 	err = insert_vm_struct(mm, vma);
-	if (err) {
-		up_write(&mm->mmap_sem);
+	if (err)
 		goto err;
-	}
 
 	mm->stack_vm = mm->total_vm = 1;
 	up_write(&mm->mmap_sem);
-
 	bprm->p = vma->vm_end - sizeof(void *);
-
 	return 0;
-
 err:
-	if (vma) {
-		bprm->vma = NULL;
-		kmem_cache_free(vm_area_cachep, vma);
-	}
-
+	up_write(&mm->mmap_sem);
+	bprm->vma = NULL;
+	kmem_cache_free(vm_area_cachep, vma);
 	return err;
 }
 
@@ -1694,7 +1686,7 @@ int get_dumpable(struct mm_struct *mm)
 	return (ret >= 2) ? 2 : ret;
 }
 
-int do_coredump(long signr, int exit_code, struct pt_regs * regs)
+void do_coredump(long signr, int exit_code, struct pt_regs *regs)
 {
 	struct core_state core_state;
 	char corename[CORENAME_MAX_SIZE + 1];
@@ -1778,6 +1770,11 @@ int do_coredump(long signr, int exit_code, struct pt_regs * regs)
 
  	if (ispipe) {
 		helper_argv = argv_split(GFP_KERNEL, corename+1, &helper_argc);
+		if (!helper_argv) {
+			printk(KERN_WARNING "%s failed to allocate memory\n",
+			       __func__);
+			goto fail_unlock;
+		}
 		/* Terminate the string before the first option */
 		delimit = strchr(corename, ' ');
 		if (delimit)
@@ -1845,5 +1842,5 @@ fail_unlock:
 	put_cred(cred);
 	coredump_finish(mm);
 fail:
-	return retval;
+	return;
 }
diff --git a/fs/ext4/ext4.h b/fs/ext4/ext4.h
index b0537c827024..6c46c648430d 100644
--- a/fs/ext4/ext4.h
+++ b/fs/ext4/ext4.h
@@ -1225,11 +1225,11 @@ do {								\
 } while (0)
 
 #ifdef CONFIG_SMP
-/* Each CPU can accumulate FBC_BATCH blocks in their local
+/* Each CPU can accumulate percpu_counter_batch blocks in their local
  * counters. So we need to make sure we have free blocks more
- * than FBC_BATCH  * nr_cpu_ids. Also add a window of 4 times.
+ * than percpu_counter_batch  * nr_cpu_ids. Also add a window of 4 times.
  */
-#define EXT4_FREEBLOCKS_WATERMARK (4 * (FBC_BATCH * nr_cpu_ids))
+#define EXT4_FREEBLOCKS_WATERMARK (4 * (percpu_counter_batch * nr_cpu_ids))
 #else
 #define EXT4_FREEBLOCKS_WATERMARK 0
 #endif
diff --git a/fs/ext4/inode.c b/fs/ext4/inode.c
index 6702a49992a6..98d3fe7057ef 100644
--- a/fs/ext4/inode.c
+++ b/fs/ext4/inode.c
@@ -2498,7 +2498,7 @@ static int ext4_nonda_switch(struct super_block *sb)
 	/*
 	 * switch to non delalloc mode if we are running low
 	 * on free block. The free block accounting via percpu
-	 * counters can get slightly wrong with FBC_BATCH getting
+	 * counters can get slightly wrong with percpu_counter_batch getting
 	 * accumulated on each CPU without updating global counters
 	 * Delalloc need an accurate free block accounting. So switch
 	 * to non delalloc when we are near to error range.
diff --git a/fs/fs-writeback.c b/fs/fs-writeback.c
index d0ff0b8cf309..e5eaa62fd17f 100644
--- a/fs/fs-writeback.c
+++ b/fs/fs-writeback.c
@@ -421,9 +421,6 @@ __writeback_single_inode(struct inode *inode, struct writeback_control *wbc)
  * If we're a pdlfush thread, then implement pdflush collision avoidance
  * against the entire list.
  *
- * WB_SYNC_HOLD is a hack for sys_sync(): reattach the inode to sb->s_dirty so
- * that it can be located for waiting on in __writeback_single_inode().
- *
  * If `bdi' is non-zero then we're being asked to writeback a specific queue.
  * This function assumes that the blockdev superblock's inodes are backed by
  * a variety of queues, so all inodes are searched.  For other superblocks,
@@ -443,6 +440,7 @@ void generic_sync_sb_inodes(struct super_block *sb,
 				struct writeback_control *wbc)
 {
 	const unsigned long start = jiffies;	/* livelock avoidance */
+	int sync = wbc->sync_mode == WB_SYNC_ALL;
 
 	spin_lock(&inode_lock);
 	if (!wbc->for_kupdate || list_empty(&sb->s_io))
@@ -499,10 +497,6 @@ void generic_sync_sb_inodes(struct super_block *sb,
 		__iget(inode);
 		pages_skipped = wbc->pages_skipped;
 		__writeback_single_inode(inode, wbc);
-		if (wbc->sync_mode == WB_SYNC_HOLD) {
-			inode->dirtied_when = jiffies;
-			list_move(&inode->i_list, &sb->s_dirty);
-		}
 		if (current_is_pdflush())
 			writeback_release(bdi);
 		if (wbc->pages_skipped != pages_skipped) {
@@ -523,7 +517,49 @@ void generic_sync_sb_inodes(struct super_block *sb,
 		if (!list_empty(&sb->s_more_io))
 			wbc->more_io = 1;
 	}
-	spin_unlock(&inode_lock);
+
+	if (sync) {
+		struct inode *inode, *old_inode = NULL;
+
+		/*
+		 * Data integrity sync. Must wait for all pages under writeback,
+		 * because there may have been pages dirtied before our sync
+		 * call, but which had writeout started before we write it out.
+		 * In which case, the inode may not be on the dirty list, but
+		 * we still have to wait for that writeout.
+		 */
+		list_for_each_entry(inode, &sb->s_inodes, i_sb_list) {
+			struct address_space *mapping;
+
+			if (inode->i_state & (I_FREEING|I_WILL_FREE))
+				continue;
+			mapping = inode->i_mapping;
+			if (mapping->nrpages == 0)
+				continue;
+			__iget(inode);
+			spin_unlock(&inode_lock);
+			/*
+			 * We hold a reference to 'inode' so it couldn't have
+			 * been removed from s_inodes list while we dropped the
+			 * inode_lock.  We cannot iput the inode now as we can
+			 * be holding the last reference and we cannot iput it
+			 * under inode_lock. So we keep the reference and iput
+			 * it later.
+			 */
+			iput(old_inode);
+			old_inode = inode;
+
+			filemap_fdatawait(mapping);
+
+			cond_resched();
+
+			spin_lock(&inode_lock);
+		}
+		spin_unlock(&inode_lock);
+		iput(old_inode);
+	} else
+		spin_unlock(&inode_lock);
+
 	return;		/* Leave any unwritten inodes on s_io */
 }
 EXPORT_SYMBOL_GPL(generic_sync_sb_inodes);
@@ -588,8 +624,7 @@ restart:
 
 /*
  * writeback and wait upon the filesystem's dirty inodes.  The caller will
- * do this in two passes - one to write, and one to wait.  WB_SYNC_HOLD is
- * used to park the written inodes on sb->s_dirty for the wait pass.
+ * do this in two passes - one to write, and one to wait.
  *
  * A finite limit is set on the number of pages which will be written.
  * To prevent infinite livelock of sys_sync().
@@ -600,30 +635,21 @@ restart:
 void sync_inodes_sb(struct super_block *sb, int wait)
 {
 	struct writeback_control wbc = {
-		.sync_mode	= wait ? WB_SYNC_ALL : WB_SYNC_HOLD,
+		.sync_mode	= wait ? WB_SYNC_ALL : WB_SYNC_NONE,
 		.range_start	= 0,
 		.range_end	= LLONG_MAX,
 	};
-	unsigned long nr_dirty = global_page_state(NR_FILE_DIRTY);
-	unsigned long nr_unstable = global_page_state(NR_UNSTABLE_NFS);
 
-	wbc.nr_to_write = nr_dirty + nr_unstable +
-			(inodes_stat.nr_inodes - inodes_stat.nr_unused) +
-			nr_dirty + nr_unstable;
-	wbc.nr_to_write += wbc.nr_to_write / 2;		/* Bit more for luck */
-	sync_sb_inodes(sb, &wbc);
-}
+	if (!wait) {
+		unsigned long nr_dirty = global_page_state(NR_FILE_DIRTY);
+		unsigned long nr_unstable = global_page_state(NR_UNSTABLE_NFS);
 
-/*
- * Rather lame livelock avoidance.
- */
-static void set_sb_syncing(int val)
-{
-	struct super_block *sb;
-	spin_lock(&sb_lock);
-	list_for_each_entry_reverse(sb, &super_blocks, s_list)
-		sb->s_syncing = val;
-	spin_unlock(&sb_lock);
+		wbc.nr_to_write = nr_dirty + nr_unstable +
+			(inodes_stat.nr_inodes - inodes_stat.nr_unused);
+	} else
+		wbc.nr_to_write = LONG_MAX; /* doesn't actually matter */
+
+	sync_sb_inodes(sb, &wbc);
 }
 
 /**
@@ -652,9 +678,6 @@ static void __sync_inodes(int wait)
 	spin_lock(&sb_lock);
 restart:
 	list_for_each_entry(sb, &super_blocks, s_list) {
-		if (sb->s_syncing)
-			continue;
-		sb->s_syncing = 1;
 		sb->s_count++;
 		spin_unlock(&sb_lock);
 		down_read(&sb->s_umount);
@@ -672,13 +695,10 @@ restart:
 
 void sync_inodes(int wait)
 {
-	set_sb_syncing(0);
 	__sync_inodes(0);
 
-	if (wait) {
-		set_sb_syncing(0);
+	if (wait)
 		__sync_inodes(1);
-	}
 }
 
 /**
diff --git a/fs/fuse/control.c b/fs/fuse/control.c
index 4f3cab321415..99c99dfb0373 100644
--- a/fs/fuse/control.c
+++ b/fs/fuse/control.c
@@ -1,6 +1,6 @@
 /*
   FUSE: Filesystem in Userspace
-  Copyright (C) 2001-2006  Miklos Szeredi <miklos@szeredi.hu>
+  Copyright (C) 2001-2008  Miklos Szeredi <miklos@szeredi.hu>
 
   This program can be distributed under the terms of the GNU GPL.
   See the file COPYING.
@@ -48,11 +48,13 @@ static ssize_t fuse_conn_waiting_read(struct file *file, char __user *buf,
 	size_t size;
 
 	if (!*ppos) {
+		long value;
 		struct fuse_conn *fc = fuse_ctl_file_conn_get(file);
 		if (!fc)
 			return 0;
 
-		file->private_data=(void *)(long)atomic_read(&fc->num_waiting);
+		value = atomic_read(&fc->num_waiting);
+		file->private_data = (void *)value;
 		fuse_conn_put(fc);
 	}
 	size = sprintf(tmp, "%ld\n", (long)file->private_data);
diff --git a/fs/fuse/dev.c b/fs/fuse/dev.c
index fba571648a8e..e0c7ada08a1f 100644
--- a/fs/fuse/dev.c
+++ b/fs/fuse/dev.c
@@ -1,6 +1,6 @@
 /*
   FUSE: Filesystem in Userspace
-  Copyright (C) 2001-2006  Miklos Szeredi <miklos@szeredi.hu>
+  Copyright (C) 2001-2008  Miklos Szeredi <miklos@szeredi.hu>
 
   This program can be distributed under the terms of the GNU GPL.
   See the file COPYING.
@@ -269,7 +269,7 @@ static void flush_bg_queue(struct fuse_conn *fc)
  * Called with fc->lock, unlocks it
  */
 static void request_end(struct fuse_conn *fc, struct fuse_req *req)
-	__releases(fc->lock)
+__releases(&fc->lock)
 {
 	void (*end) (struct fuse_conn *, struct fuse_req *) = req->end;
 	req->end = NULL;
@@ -293,13 +293,13 @@ static void request_end(struct fuse_conn *fc, struct fuse_req *req)
 	wake_up(&req->waitq);
 	if (end)
 		end(fc, req);
-	else
-		fuse_put_request(fc, req);
+	fuse_put_request(fc, req);
 }
 
 static void wait_answer_interruptible(struct fuse_conn *fc,
 				      struct fuse_req *req)
-	__releases(fc->lock) __acquires(fc->lock)
+__releases(&fc->lock)
+__acquires(&fc->lock)
 {
 	if (signal_pending(current))
 		return;
@@ -317,7 +317,8 @@ static void queue_interrupt(struct fuse_conn *fc, struct fuse_req *req)
 }
 
 static void request_wait_answer(struct fuse_conn *fc, struct fuse_req *req)
-	__releases(fc->lock) __acquires(fc->lock)
+__releases(&fc->lock)
+__acquires(&fc->lock)
 {
 	if (!fc->no_interrupt) {
 		/* Any signal may interrupt this */
@@ -380,7 +381,7 @@ static void request_wait_answer(struct fuse_conn *fc, struct fuse_req *req)
 	}
 }
 
-void request_send(struct fuse_conn *fc, struct fuse_req *req)
+void fuse_request_send(struct fuse_conn *fc, struct fuse_req *req)
 {
 	req->isreply = 1;
 	spin_lock(&fc->lock);
@@ -399,8 +400,8 @@ void request_send(struct fuse_conn *fc, struct fuse_req *req)
 	spin_unlock(&fc->lock);
 }
 
-static void request_send_nowait_locked(struct fuse_conn *fc,
-				       struct fuse_req *req)
+static void fuse_request_send_nowait_locked(struct fuse_conn *fc,
+					    struct fuse_req *req)
 {
 	req->background = 1;
 	fc->num_background++;
@@ -414,11 +415,11 @@ static void request_send_nowait_locked(struct fuse_conn *fc,
 	flush_bg_queue(fc);
 }
 
-static void request_send_nowait(struct fuse_conn *fc, struct fuse_req *req)
+static void fuse_request_send_nowait(struct fuse_conn *fc, struct fuse_req *req)
 {
 	spin_lock(&fc->lock);
 	if (fc->connected) {
-		request_send_nowait_locked(fc, req);
+		fuse_request_send_nowait_locked(fc, req);
 		spin_unlock(&fc->lock);
 	} else {
 		req->out.h.error = -ENOTCONN;
@@ -426,16 +427,16 @@ static void request_send_nowait(struct fuse_conn *fc, struct fuse_req *req)
 	}
 }
 
-void request_send_noreply(struct fuse_conn *fc, struct fuse_req *req)
+void fuse_request_send_noreply(struct fuse_conn *fc, struct fuse_req *req)
 {
 	req->isreply = 0;
-	request_send_nowait(fc, req);
+	fuse_request_send_nowait(fc, req);
 }
 
-void request_send_background(struct fuse_conn *fc, struct fuse_req *req)
+void fuse_request_send_background(struct fuse_conn *fc, struct fuse_req *req)
 {
 	req->isreply = 1;
-	request_send_nowait(fc, req);
+	fuse_request_send_nowait(fc, req);
 }
 
 /*
@@ -443,10 +444,11 @@ void request_send_background(struct fuse_conn *fc, struct fuse_req *req)
  *
  * fc->connected must have been checked previously
  */
-void request_send_background_locked(struct fuse_conn *fc, struct fuse_req *req)
+void fuse_request_send_background_locked(struct fuse_conn *fc,
+					 struct fuse_req *req)
 {
 	req->isreply = 1;
-	request_send_nowait_locked(fc, req);
+	fuse_request_send_nowait_locked(fc, req);
 }
 
 /*
@@ -539,8 +541,8 @@ static int fuse_copy_fill(struct fuse_copy_state *cs)
 		BUG_ON(!cs->nr_segs);
 		cs->seglen = cs->iov[0].iov_len;
 		cs->addr = (unsigned long) cs->iov[0].iov_base;
-		cs->iov ++;
-		cs->nr_segs --;
+		cs->iov++;
+		cs->nr_segs--;
 	}
 	down_read(&current->mm->mmap_sem);
 	err = get_user_pages(current, current->mm, cs->addr, 1, cs->write, 0,
@@ -589,9 +591,11 @@ static int fuse_copy_page(struct fuse_copy_state *cs, struct page *page,
 		kunmap_atomic(mapaddr, KM_USER1);
 	}
 	while (count) {
-		int err;
-		if (!cs->len && (err = fuse_copy_fill(cs)))
-			return err;
+		if (!cs->len) {
+			int err = fuse_copy_fill(cs);
+			if (err)
+				return err;
+		}
 		if (page) {
 			void *mapaddr = kmap_atomic(page, KM_USER1);
 			void *buf = mapaddr + offset;
@@ -631,9 +635,11 @@ static int fuse_copy_pages(struct fuse_copy_state *cs, unsigned nbytes,
 static int fuse_copy_one(struct fuse_copy_state *cs, void *val, unsigned size)
 {
 	while (size) {
-		int err;
-		if (!cs->len && (err = fuse_copy_fill(cs)))
-			return err;
+		if (!cs->len) {
+			int err = fuse_copy_fill(cs);
+			if (err)
+				return err;
+		}
 		fuse_copy_do(cs, &val, &size);
 	}
 	return 0;
@@ -664,6 +670,8 @@ static int request_pending(struct fuse_conn *fc)
 
 /* Wait until a request is available on the pending list */
 static void request_wait(struct fuse_conn *fc)
+__releases(&fc->lock)
+__acquires(&fc->lock)
 {
 	DECLARE_WAITQUEUE(wait, current);
 
@@ -691,7 +699,7 @@ static void request_wait(struct fuse_conn *fc)
  */
 static int fuse_read_interrupt(struct fuse_conn *fc, struct fuse_req *req,
 			       const struct iovec *iov, unsigned long nr_segs)
-	__releases(fc->lock)
+__releases(&fc->lock)
 {
 	struct fuse_copy_state cs;
 	struct fuse_in_header ih;
@@ -813,6 +821,34 @@ static ssize_t fuse_dev_read(struct kiocb *iocb, const struct iovec *iov,
 	return err;
 }
 
+static int fuse_notify_poll(struct fuse_conn *fc, unsigned int size,
+			    struct fuse_copy_state *cs)
+{
+	struct fuse_notify_poll_wakeup_out outarg;
+	int err;
+
+	if (size != sizeof(outarg))
+		return -EINVAL;
+
+	err = fuse_copy_one(cs, &outarg, sizeof(outarg));
+	if (err)
+		return err;
+
+	return fuse_notify_poll_wakeup(fc, &outarg);
+}
+
+static int fuse_notify(struct fuse_conn *fc, enum fuse_notify_code code,
+		       unsigned int size, struct fuse_copy_state *cs)
+{
+	switch (code) {
+	case FUSE_NOTIFY_POLL:
+		return fuse_notify_poll(fc, size, cs);
+
+	default:
+		return -EINVAL;
+	}
+}
+
 /* Look up request on processing list by unique ID */
 static struct fuse_req *request_find(struct fuse_conn *fc, u64 unique)
 {
@@ -876,9 +912,23 @@ static ssize_t fuse_dev_write(struct kiocb *iocb, const struct iovec *iov,
 	err = fuse_copy_one(&cs, &oh, sizeof(oh));
 	if (err)
 		goto err_finish;
+
+	err = -EINVAL;
+	if (oh.len != nbytes)
+		goto err_finish;
+
+	/*
+	 * Zero oh.unique indicates unsolicited notification message
+	 * and error contains notification code.
+	 */
+	if (!oh.unique) {
+		err = fuse_notify(fc, oh.error, nbytes - sizeof(oh), &cs);
+		fuse_copy_finish(&cs);
+		return err ? err : nbytes;
+	}
+
 	err = -EINVAL;
-	if (!oh.unique || oh.error <= -1000 || oh.error > 0 ||
-	    oh.len != nbytes)
+	if (oh.error <= -1000 || oh.error > 0)
 		goto err_finish;
 
 	spin_lock(&fc->lock);
@@ -966,6 +1016,8 @@ static unsigned fuse_dev_poll(struct file *file, poll_table *wait)
  * This function releases and reacquires fc->lock
  */
 static void end_requests(struct fuse_conn *fc, struct list_head *head)
+__releases(&fc->lock)
+__acquires(&fc->lock)
 {
 	while (!list_empty(head)) {
 		struct fuse_req *req;
@@ -988,7 +1040,8 @@ static void end_requests(struct fuse_conn *fc, struct list_head *head)
  * locked).
  */
 static void end_io_requests(struct fuse_conn *fc)
-	__releases(fc->lock) __acquires(fc->lock)
+__releases(&fc->lock)
+__acquires(&fc->lock)
 {
 	while (!list_empty(&fc->io)) {
 		struct fuse_req *req =
@@ -1002,11 +1055,11 @@ static void end_io_requests(struct fuse_conn *fc)
 		wake_up(&req->waitq);
 		if (end) {
 			req->end = NULL;
-			/* The end function will consume this reference */
 			__fuse_get_request(req);
 			spin_unlock(&fc->lock);
 			wait_event(req->waitq, !req->locked);
 			end(fc, req);
+			fuse_put_request(fc, req);
 			spin_lock(&fc->lock);
 		}
 	}
diff --git a/fs/fuse/dir.c b/fs/fuse/dir.c
index 95bc22bdd060..fdff346e96fd 100644
--- a/fs/fuse/dir.c
+++ b/fs/fuse/dir.c
@@ -1,6 +1,6 @@
 /*
   FUSE: Filesystem in Userspace
-  Copyright (C) 2001-2006  Miklos Szeredi <miklos@szeredi.hu>
+  Copyright (C) 2001-2008  Miklos Szeredi <miklos@szeredi.hu>
 
   This program can be distributed under the terms of the GNU GPL.
   See the file COPYING.
@@ -189,7 +189,7 @@ static int fuse_dentry_revalidate(struct dentry *entry, struct nameidata *nd)
 		parent = dget_parent(entry);
 		fuse_lookup_init(fc, req, get_node_id(parent->d_inode),
 				 &entry->d_name, &outarg);
-		request_send(fc, req);
+		fuse_request_send(fc, req);
 		dput(parent);
 		err = req->out.h.error;
 		fuse_put_request(fc, req);
@@ -204,7 +204,7 @@ static int fuse_dentry_revalidate(struct dentry *entry, struct nameidata *nd)
 				return 0;
 			}
 			spin_lock(&fc->lock);
-			fi->nlookup ++;
+			fi->nlookup++;
 			spin_unlock(&fc->lock);
 		}
 		fuse_put_request(fc, forget_req);
@@ -283,7 +283,7 @@ int fuse_lookup_name(struct super_block *sb, u64 nodeid, struct qstr *name,
 	attr_version = fuse_get_attr_version(fc);
 
 	fuse_lookup_init(fc, req, nodeid, name, outarg);
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 	/* Zero nodeid is same as -ENOENT, but with valid timeout */
@@ -369,7 +369,7 @@ static void fuse_sync_release(struct fuse_conn *fc, struct fuse_file *ff,
 {
 	fuse_release_fill(ff, nodeid, flags, FUSE_RELEASE);
 	ff->reserved_req->force = 1;
-	request_send(fc, ff->reserved_req);
+	fuse_request_send(fc, ff->reserved_req);
 	fuse_put_request(fc, ff->reserved_req);
 	kfree(ff);
 }
@@ -408,7 +408,7 @@ static int fuse_create_open(struct inode *dir, struct dentry *entry, int mode,
 		goto out_put_forget_req;
 
 	err = -ENOMEM;
-	ff = fuse_file_alloc();
+	ff = fuse_file_alloc(fc);
 	if (!ff)
 		goto out_put_request;
 
@@ -432,7 +432,7 @@ static int fuse_create_open(struct inode *dir, struct dentry *entry, int mode,
 	req->out.args[0].value = &outentry;
 	req->out.args[1].size = sizeof(outopen);
 	req->out.args[1].value = &outopen;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	if (err) {
 		if (err == -ENOSYS)
@@ -502,7 +502,7 @@ static int create_new_entry(struct fuse_conn *fc, struct fuse_req *req,
 	else
 		req->out.args[0].size = sizeof(outarg);
 	req->out.args[0].value = &outarg;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 	if (err)
@@ -631,15 +631,17 @@ static int fuse_unlink(struct inode *dir, struct dentry *entry)
 	req->in.numargs = 1;
 	req->in.args[0].size = entry->d_name.len + 1;
 	req->in.args[0].value = entry->d_name.name;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 	if (!err) {
 		struct inode *inode = entry->d_inode;
 
-		/* Set nlink to zero so the inode can be cleared, if
-                   the inode does have more links this will be
-                   discovered at the next lookup/getattr */
+		/*
+		 * Set nlink to zero so the inode can be cleared, if the inode
+		 * does have more links this will be discovered at the next
+		 * lookup/getattr.
+		 */
 		clear_nlink(inode);
 		fuse_invalidate_attr(inode);
 		fuse_invalidate_attr(dir);
@@ -662,7 +664,7 @@ static int fuse_rmdir(struct inode *dir, struct dentry *entry)
 	req->in.numargs = 1;
 	req->in.args[0].size = entry->d_name.len + 1;
 	req->in.args[0].value = entry->d_name.name;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 	if (!err) {
@@ -695,7 +697,7 @@ static int fuse_rename(struct inode *olddir, struct dentry *oldent,
 	req->in.args[1].value = oldent->d_name.name;
 	req->in.args[2].size = newent->d_name.len + 1;
 	req->in.args[2].value = newent->d_name.name;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 	if (!err) {
@@ -811,7 +813,7 @@ static int fuse_do_getattr(struct inode *inode, struct kstat *stat,
 	else
 		req->out.args[0].size = sizeof(outarg);
 	req->out.args[0].value = &outarg;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 	if (!err) {
@@ -911,7 +913,7 @@ static int fuse_access(struct inode *inode, int mask)
 	req->in.numargs = 1;
 	req->in.args[0].size = sizeof(inarg);
 	req->in.args[0].value = &inarg;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 	if (err == -ENOSYS) {
@@ -1033,7 +1035,7 @@ static int fuse_readdir(struct file *file, void *dstbuf, filldir_t filldir)
 	req->num_pages = 1;
 	req->pages[0] = page;
 	fuse_read_fill(req, file, inode, file->f_pos, PAGE_SIZE, FUSE_READDIR);
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	nbytes = req->out.args[0].size;
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
@@ -1067,7 +1069,7 @@ static char *read_link(struct dentry *dentry)
 	req->out.numargs = 1;
 	req->out.args[0].size = PAGE_SIZE - 1;
 	req->out.args[0].value = link;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	if (req->out.h.error) {
 		free_page((unsigned long) link);
 		link = ERR_PTR(req->out.h.error);
@@ -1273,7 +1275,7 @@ static int fuse_do_setattr(struct dentry *entry, struct iattr *attr,
 	else
 		req->out.args[0].size = sizeof(outarg);
 	req->out.args[0].value = &outarg;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 	if (err) {
@@ -1367,7 +1369,7 @@ static int fuse_setxattr(struct dentry *entry, const char *name,
 	req->in.args[1].value = name;
 	req->in.args[2].size = size;
 	req->in.args[2].value = value;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 	if (err == -ENOSYS) {
@@ -1413,7 +1415,7 @@ static ssize_t fuse_getxattr(struct dentry *entry, const char *name,
 		req->out.args[0].size = sizeof(outarg);
 		req->out.args[0].value = &outarg;
 	}
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	ret = req->out.h.error;
 	if (!ret)
 		ret = size ? req->out.args[0].size : outarg.size;
@@ -1463,7 +1465,7 @@ static ssize_t fuse_listxattr(struct dentry *entry, char *list, size_t size)
 		req->out.args[0].size = sizeof(outarg);
 		req->out.args[0].value = &outarg;
 	}
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	ret = req->out.h.error;
 	if (!ret)
 		ret = size ? req->out.args[0].size : outarg.size;
@@ -1496,7 +1498,7 @@ static int fuse_removexattr(struct dentry *entry, const char *name)
 	req->in.numargs = 1;
 	req->in.args[0].size = strlen(name) + 1;
 	req->in.args[0].value = name;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 	if (err == -ENOSYS) {
diff --git a/fs/fuse/file.c b/fs/fuse/file.c
index 4c9ee7011265..e8162646a9b5 100644
--- a/fs/fuse/file.c
+++ b/fs/fuse/file.c
@@ -1,6 +1,6 @@
 /*
   FUSE: Filesystem in Userspace
-  Copyright (C) 2001-2006  Miklos Szeredi <miklos@szeredi.hu>
+  Copyright (C) 2001-2008  Miklos Szeredi <miklos@szeredi.hu>
 
   This program can be distributed under the terms of the GNU GPL.
   See the file COPYING.
@@ -39,14 +39,14 @@ static int fuse_send_open(struct inode *inode, struct file *file, int isdir,
 	req->out.numargs = 1;
 	req->out.args[0].size = sizeof(*outargp);
 	req->out.args[0].value = outargp;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 
 	return err;
 }
 
-struct fuse_file *fuse_file_alloc(void)
+struct fuse_file *fuse_file_alloc(struct fuse_conn *fc)
 {
 	struct fuse_file *ff;
 	ff = kmalloc(sizeof(struct fuse_file), GFP_KERNEL);
@@ -58,7 +58,12 @@ struct fuse_file *fuse_file_alloc(void)
 		} else {
 			INIT_LIST_HEAD(&ff->write_entry);
 			atomic_set(&ff->count, 0);
+			spin_lock(&fc->lock);
+			ff->kh = ++fc->khctr;
+			spin_unlock(&fc->lock);
 		}
+		RB_CLEAR_NODE(&ff->polled_node);
+		init_waitqueue_head(&ff->poll_wait);
 	}
 	return ff;
 }
@@ -79,7 +84,6 @@ static void fuse_release_end(struct fuse_conn *fc, struct fuse_req *req)
 {
 	dput(req->misc.release.dentry);
 	mntput(req->misc.release.vfsmount);
-	fuse_put_request(fc, req);
 }
 
 static void fuse_file_put(struct fuse_file *ff)
@@ -89,7 +93,7 @@ static void fuse_file_put(struct fuse_file *ff)
 		struct inode *inode = req->misc.release.dentry->d_inode;
 		struct fuse_conn *fc = get_fuse_conn(inode);
 		req->end = fuse_release_end;
-		request_send_background(fc, req);
+		fuse_request_send_background(fc, req);
 		kfree(ff);
 	}
 }
@@ -109,6 +113,7 @@ void fuse_finish_open(struct inode *inode, struct file *file,
 
 int fuse_open_common(struct inode *inode, struct file *file, int isdir)
 {
+	struct fuse_conn *fc = get_fuse_conn(inode);
 	struct fuse_open_out outarg;
 	struct fuse_file *ff;
 	int err;
@@ -121,7 +126,7 @@ int fuse_open_common(struct inode *inode, struct file *file, int isdir)
 	if (err)
 		return err;
 
-	ff = fuse_file_alloc();
+	ff = fuse_file_alloc(fc);
 	if (!ff)
 		return -ENOMEM;
 
@@ -167,7 +172,11 @@ int fuse_release_common(struct inode *inode, struct file *file, int isdir)
 
 		spin_lock(&fc->lock);
 		list_del(&ff->write_entry);
+		if (!RB_EMPTY_NODE(&ff->polled_node))
+			rb_erase(&ff->polled_node, &fc->polled_files);
 		spin_unlock(&fc->lock);
+
+		wake_up_interruptible_sync(&ff->poll_wait);
 		/*
 		 * Normally this will send the RELEASE request,
 		 * however if some asynchronous READ or WRITE requests
@@ -280,7 +289,7 @@ static int fuse_flush(struct file *file, fl_owner_t id)
 	req->in.args[0].size = sizeof(inarg);
 	req->in.args[0].value = &inarg;
 	req->force = 1;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 	if (err == -ENOSYS) {
@@ -344,7 +353,7 @@ int fuse_fsync_common(struct file *file, struct dentry *de, int datasync,
 	req->in.numargs = 1;
 	req->in.args[0].size = sizeof(inarg);
 	req->in.args[0].value = &inarg;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 	if (err == -ENOSYS) {
@@ -396,7 +405,7 @@ static size_t fuse_send_read(struct fuse_req *req, struct file *file,
 		inarg->read_flags |= FUSE_READ_LOCKOWNER;
 		inarg->lock_owner = fuse_lock_owner_id(fc, owner);
 	}
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	return req->out.args[0].size;
 }
 
@@ -493,7 +502,6 @@ static void fuse_readpages_end(struct fuse_conn *fc, struct fuse_req *req)
 	}
 	if (req->ff)
 		fuse_file_put(req->ff);
-	fuse_put_request(fc, req);
 }
 
 static void fuse_send_readpages(struct fuse_req *req, struct file *file,
@@ -509,10 +517,11 @@ static void fuse_send_readpages(struct fuse_req *req, struct file *file,
 		struct fuse_file *ff = file->private_data;
 		req->ff = fuse_file_get(ff);
 		req->end = fuse_readpages_end;
-		request_send_background(fc, req);
+		fuse_request_send_background(fc, req);
 	} else {
-		request_send(fc, req);
+		fuse_request_send(fc, req);
 		fuse_readpages_end(fc, req);
+		fuse_put_request(fc, req);
 	}
 }
 
@@ -543,7 +552,7 @@ static int fuse_readpages_fill(void *_data, struct page *page)
 		}
 	}
 	req->pages[req->num_pages] = page;
-	req->num_pages ++;
+	req->num_pages++;
 	return 0;
 }
 
@@ -636,7 +645,7 @@ static size_t fuse_send_write(struct fuse_req *req, struct file *file,
 		inarg->write_flags |= FUSE_WRITE_LOCKOWNER;
 		inarg->lock_owner = fuse_lock_owner_id(fc, owner);
 	}
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	return req->misc.write.out.size;
 }
 
@@ -1042,7 +1051,6 @@ static void fuse_writepage_free(struct fuse_conn *fc, struct fuse_req *req)
 {
 	__free_page(req->pages[0]);
 	fuse_file_put(req->ff);
-	fuse_put_request(fc, req);
 }
 
 static void fuse_writepage_finish(struct fuse_conn *fc, struct fuse_req *req)
@@ -1060,6 +1068,8 @@ static void fuse_writepage_finish(struct fuse_conn *fc, struct fuse_req *req)
 
 /* Called under fc->lock, may release and reacquire it */
 static void fuse_send_writepage(struct fuse_conn *fc, struct fuse_req *req)
+__releases(&fc->lock)
+__acquires(&fc->lock)
 {
 	struct fuse_inode *fi = get_fuse_inode(req->inode);
 	loff_t size = i_size_read(req->inode);
@@ -1079,13 +1089,14 @@ static void fuse_send_writepage(struct fuse_conn *fc, struct fuse_req *req)
 
 	req->in.args[1].size = inarg->size;
 	fi->writectr++;
-	request_send_background_locked(fc, req);
+	fuse_request_send_background_locked(fc, req);
 	return;
 
  out_free:
 	fuse_writepage_finish(fc, req);
 	spin_unlock(&fc->lock);
 	fuse_writepage_free(fc, req);
+	fuse_put_request(fc, req);
 	spin_lock(&fc->lock);
 }
 
@@ -1096,6 +1107,8 @@ static void fuse_send_writepage(struct fuse_conn *fc, struct fuse_req *req)
  * Called with fc->lock
  */
 void fuse_flush_writepages(struct inode *inode)
+__releases(&fc->lock)
+__acquires(&fc->lock)
 {
 	struct fuse_conn *fc = get_fuse_conn(inode);
 	struct fuse_inode *fi = get_fuse_inode(inode);
@@ -1325,7 +1338,7 @@ static int fuse_getlk(struct file *file, struct file_lock *fl)
 	req->out.numargs = 1;
 	req->out.args[0].size = sizeof(outarg);
 	req->out.args[0].value = &outarg;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 	if (!err)
@@ -1357,7 +1370,7 @@ static int fuse_setlk(struct file *file, struct file_lock *fl, int flock)
 		return PTR_ERR(req);
 
 	fuse_lk_fill(req, file, fl, opcode, pid, flock);
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	/* locking is restartable */
 	if (err == -EINTR)
@@ -1433,7 +1446,7 @@ static sector_t fuse_bmap(struct address_space *mapping, sector_t block)
 	req->out.numargs = 1;
 	req->out.args[0].size = sizeof(outarg);
 	req->out.args[0].value = &outarg;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 	if (err == -ENOSYS)
@@ -1470,6 +1483,406 @@ static loff_t fuse_file_llseek(struct file *file, loff_t offset, int origin)
 	return retval;
 }
 
+static int fuse_ioctl_copy_user(struct page **pages, struct iovec *iov,
+			unsigned int nr_segs, size_t bytes, bool to_user)
+{
+	struct iov_iter ii;
+	int page_idx = 0;
+
+	if (!bytes)
+		return 0;
+
+	iov_iter_init(&ii, iov, nr_segs, bytes, 0);
+
+	while (iov_iter_count(&ii)) {
+		struct page *page = pages[page_idx++];
+		size_t todo = min_t(size_t, PAGE_SIZE, iov_iter_count(&ii));
+		void *kaddr, *map;
+
+		kaddr = map = kmap(page);
+
+		while (todo) {
+			char __user *uaddr = ii.iov->iov_base + ii.iov_offset;
+			size_t iov_len = ii.iov->iov_len - ii.iov_offset;
+			size_t copy = min(todo, iov_len);
+			size_t left;
+
+			if (!to_user)
+				left = copy_from_user(kaddr, uaddr, copy);
+			else
+				left = copy_to_user(uaddr, kaddr, copy);
+
+			if (unlikely(left))
+				return -EFAULT;
+
+			iov_iter_advance(&ii, copy);
+			todo -= copy;
+			kaddr += copy;
+		}
+
+		kunmap(map);
+	}
+
+	return 0;
+}
+
+/*
+ * For ioctls, there is no generic way to determine how much memory
+ * needs to be read and/or written.  Furthermore, ioctls are allowed
+ * to dereference the passed pointer, so the parameter requires deep
+ * copying but FUSE has no idea whatsoever about what to copy in or
+ * out.
+ *
+ * This is solved by allowing FUSE server to retry ioctl with
+ * necessary in/out iovecs.  Let's assume the ioctl implementation
+ * needs to read in the following structure.
+ *
+ * struct a {
+ *	char	*buf;
+ *	size_t	buflen;
+ * }
+ *
+ * On the first callout to FUSE server, inarg->in_size and
+ * inarg->out_size will be NULL; then, the server completes the ioctl
+ * with FUSE_IOCTL_RETRY set in out->flags, out->in_iovs set to 1 and
+ * the actual iov array to
+ *
+ * { { .iov_base = inarg.arg,	.iov_len = sizeof(struct a) } }
+ *
+ * which tells FUSE to copy in the requested area and retry the ioctl.
+ * On the second round, the server has access to the structure and
+ * from that it can tell what to look for next, so on the invocation,
+ * it sets FUSE_IOCTL_RETRY, out->in_iovs to 2 and iov array to
+ *
+ * { { .iov_base = inarg.arg,	.iov_len = sizeof(struct a)	},
+ *   { .iov_base = a.buf,	.iov_len = a.buflen		} }
+ *
+ * FUSE will copy both struct a and the pointed buffer from the
+ * process doing the ioctl and retry ioctl with both struct a and the
+ * buffer.
+ *
+ * This time, FUSE server has everything it needs and completes ioctl
+ * without FUSE_IOCTL_RETRY which finishes the ioctl call.
+ *
+ * Copying data out works the same way.
+ *
+ * Note that if FUSE_IOCTL_UNRESTRICTED is clear, the kernel
+ * automatically initializes in and out iovs by decoding @cmd with
+ * _IOC_* macros and the server is not allowed to request RETRY.  This
+ * limits ioctl data transfers to well-formed ioctls and is the forced
+ * behavior for all FUSE servers.
+ */
+static long fuse_file_do_ioctl(struct file *file, unsigned int cmd,
+			       unsigned long arg, unsigned int flags)
+{
+	struct inode *inode = file->f_dentry->d_inode;
+	struct fuse_file *ff = file->private_data;
+	struct fuse_conn *fc = get_fuse_conn(inode);
+	struct fuse_ioctl_in inarg = {
+		.fh = ff->fh,
+		.cmd = cmd,
+		.arg = arg,
+		.flags = flags
+	};
+	struct fuse_ioctl_out outarg;
+	struct fuse_req *req = NULL;
+	struct page **pages = NULL;
+	struct page *iov_page = NULL;
+	struct iovec *in_iov = NULL, *out_iov = NULL;
+	unsigned int in_iovs = 0, out_iovs = 0, num_pages = 0, max_pages;
+	size_t in_size, out_size, transferred;
+	int err;
+
+	/* assume all the iovs returned by client always fits in a page */
+	BUILD_BUG_ON(sizeof(struct iovec) * FUSE_IOCTL_MAX_IOV > PAGE_SIZE);
+
+	if (!fuse_allow_task(fc, current))
+		return -EACCES;
+
+	err = -EIO;
+	if (is_bad_inode(inode))
+		goto out;
+
+	err = -ENOMEM;
+	pages = kzalloc(sizeof(pages[0]) * FUSE_MAX_PAGES_PER_REQ, GFP_KERNEL);
+	iov_page = alloc_page(GFP_KERNEL);
+	if (!pages || !iov_page)
+		goto out;
+
+	/*
+	 * If restricted, initialize IO parameters as encoded in @cmd.
+	 * RETRY from server is not allowed.
+	 */
+	if (!(flags & FUSE_IOCTL_UNRESTRICTED)) {
+		struct iovec *iov = page_address(iov_page);
+
+		iov->iov_base = (void __user *)arg;
+		iov->iov_len = _IOC_SIZE(cmd);
+
+		if (_IOC_DIR(cmd) & _IOC_WRITE) {
+			in_iov = iov;
+			in_iovs = 1;
+		}
+
+		if (_IOC_DIR(cmd) & _IOC_READ) {
+			out_iov = iov;
+			out_iovs = 1;
+		}
+	}
+
+ retry:
+	inarg.in_size = in_size = iov_length(in_iov, in_iovs);
+	inarg.out_size = out_size = iov_length(out_iov, out_iovs);
+
+	/*
+	 * Out data can be used either for actual out data or iovs,
+	 * make sure there always is at least one page.
+	 */
+	out_size = max_t(size_t, out_size, PAGE_SIZE);
+	max_pages = DIV_ROUND_UP(max(in_size, out_size), PAGE_SIZE);
+
+	/* make sure there are enough buffer pages and init request with them */
+	err = -ENOMEM;
+	if (max_pages > FUSE_MAX_PAGES_PER_REQ)
+		goto out;
+	while (num_pages < max_pages) {
+		pages[num_pages] = alloc_page(GFP_KERNEL | __GFP_HIGHMEM);
+		if (!pages[num_pages])
+			goto out;
+		num_pages++;
+	}
+
+	req = fuse_get_req(fc);
+	if (IS_ERR(req)) {
+		err = PTR_ERR(req);
+		req = NULL;
+		goto out;
+	}
+	memcpy(req->pages, pages, sizeof(req->pages[0]) * num_pages);
+	req->num_pages = num_pages;
+
+	/* okay, let's send it to the client */
+	req->in.h.opcode = FUSE_IOCTL;
+	req->in.h.nodeid = get_node_id(inode);
+	req->in.numargs = 1;
+	req->in.args[0].size = sizeof(inarg);
+	req->in.args[0].value = &inarg;
+	if (in_size) {
+		req->in.numargs++;
+		req->in.args[1].size = in_size;
+		req->in.argpages = 1;
+
+		err = fuse_ioctl_copy_user(pages, in_iov, in_iovs, in_size,
+					   false);
+		if (err)
+			goto out;
+	}
+
+	req->out.numargs = 2;
+	req->out.args[0].size = sizeof(outarg);
+	req->out.args[0].value = &outarg;
+	req->out.args[1].size = out_size;
+	req->out.argpages = 1;
+	req->out.argvar = 1;
+
+	fuse_request_send(fc, req);
+	err = req->out.h.error;
+	transferred = req->out.args[1].size;
+	fuse_put_request(fc, req);
+	req = NULL;
+	if (err)
+		goto out;
+
+	/* did it ask for retry? */
+	if (outarg.flags & FUSE_IOCTL_RETRY) {
+		char *vaddr;
+
+		/* no retry if in restricted mode */
+		err = -EIO;
+		if (!(flags & FUSE_IOCTL_UNRESTRICTED))
+			goto out;
+
+		in_iovs = outarg.in_iovs;
+		out_iovs = outarg.out_iovs;
+
+		/*
+		 * Make sure things are in boundary, separate checks
+		 * are to protect against overflow.
+		 */
+		err = -ENOMEM;
+		if (in_iovs > FUSE_IOCTL_MAX_IOV ||
+		    out_iovs > FUSE_IOCTL_MAX_IOV ||
+		    in_iovs + out_iovs > FUSE_IOCTL_MAX_IOV)
+			goto out;
+
+		err = -EIO;
+		if ((in_iovs + out_iovs) * sizeof(struct iovec) != transferred)
+			goto out;
+
+		/* okay, copy in iovs and retry */
+		vaddr = kmap_atomic(pages[0], KM_USER0);
+		memcpy(page_address(iov_page), vaddr, transferred);
+		kunmap_atomic(vaddr, KM_USER0);
+
+		in_iov = page_address(iov_page);
+		out_iov = in_iov + in_iovs;
+
+		goto retry;
+	}
+
+	err = -EIO;
+	if (transferred > inarg.out_size)
+		goto out;
+
+	err = fuse_ioctl_copy_user(pages, out_iov, out_iovs, transferred, true);
+ out:
+	if (req)
+		fuse_put_request(fc, req);
+	if (iov_page)
+		__free_page(iov_page);
+	while (num_pages)
+		__free_page(pages[--num_pages]);
+	kfree(pages);
+
+	return err ? err : outarg.result;
+}
+
+static long fuse_file_ioctl(struct file *file, unsigned int cmd,
+			    unsigned long arg)
+{
+	return fuse_file_do_ioctl(file, cmd, arg, 0);
+}
+
+static long fuse_file_compat_ioctl(struct file *file, unsigned int cmd,
+				   unsigned long arg)
+{
+	return fuse_file_do_ioctl(file, cmd, arg, FUSE_IOCTL_COMPAT);
+}
+
+/*
+ * All files which have been polled are linked to RB tree
+ * fuse_conn->polled_files which is indexed by kh.  Walk the tree and
+ * find the matching one.
+ */
+static struct rb_node **fuse_find_polled_node(struct fuse_conn *fc, u64 kh,
+					      struct rb_node **parent_out)
+{
+	struct rb_node **link = &fc->polled_files.rb_node;
+	struct rb_node *last = NULL;
+
+	while (*link) {
+		struct fuse_file *ff;
+
+		last = *link;
+		ff = rb_entry(last, struct fuse_file, polled_node);
+
+		if (kh < ff->kh)
+			link = &last->rb_left;
+		else if (kh > ff->kh)
+			link = &last->rb_right;
+		else
+			return link;
+	}
+
+	if (parent_out)
+		*parent_out = last;
+	return link;
+}
+
+/*
+ * The file is about to be polled.  Make sure it's on the polled_files
+ * RB tree.  Note that files once added to the polled_files tree are
+ * not removed before the file is released.  This is because a file
+ * polled once is likely to be polled again.
+ */
+static void fuse_register_polled_file(struct fuse_conn *fc,
+				      struct fuse_file *ff)
+{
+	spin_lock(&fc->lock);
+	if (RB_EMPTY_NODE(&ff->polled_node)) {
+		struct rb_node **link, *parent;
+
+		link = fuse_find_polled_node(fc, ff->kh, &parent);
+		BUG_ON(*link);
+		rb_link_node(&ff->polled_node, parent, link);
+		rb_insert_color(&ff->polled_node, &fc->polled_files);
+	}
+	spin_unlock(&fc->lock);
+}
+
+static unsigned fuse_file_poll(struct file *file, poll_table *wait)
+{
+	struct inode *inode = file->f_dentry->d_inode;
+	struct fuse_file *ff = file->private_data;
+	struct fuse_conn *fc = get_fuse_conn(inode);
+	struct fuse_poll_in inarg = { .fh = ff->fh, .kh = ff->kh };
+	struct fuse_poll_out outarg;
+	struct fuse_req *req;
+	int err;
+
+	if (fc->no_poll)
+		return DEFAULT_POLLMASK;
+
+	poll_wait(file, &ff->poll_wait, wait);
+
+	/*
+	 * Ask for notification iff there's someone waiting for it.
+	 * The client may ignore the flag and always notify.
+	 */
+	if (waitqueue_active(&ff->poll_wait)) {
+		inarg.flags |= FUSE_POLL_SCHEDULE_NOTIFY;
+		fuse_register_polled_file(fc, ff);
+	}
+
+	req = fuse_get_req(fc);
+	if (IS_ERR(req))
+		return PTR_ERR(req);
+
+	req->in.h.opcode = FUSE_POLL;
+	req->in.h.nodeid = get_node_id(inode);
+	req->in.numargs = 1;
+	req->in.args[0].size = sizeof(inarg);
+	req->in.args[0].value = &inarg;
+	req->out.numargs = 1;
+	req->out.args[0].size = sizeof(outarg);
+	req->out.args[0].value = &outarg;
+	fuse_request_send(fc, req);
+	err = req->out.h.error;
+	fuse_put_request(fc, req);
+
+	if (!err)
+		return outarg.revents;
+	if (err == -ENOSYS) {
+		fc->no_poll = 1;
+		return DEFAULT_POLLMASK;
+	}
+	return POLLERR;
+}
+
+/*
+ * This is called from fuse_handle_notify() on FUSE_NOTIFY_POLL and
+ * wakes up the poll waiters.
+ */
+int fuse_notify_poll_wakeup(struct fuse_conn *fc,
+			    struct fuse_notify_poll_wakeup_out *outarg)
+{
+	u64 kh = outarg->kh;
+	struct rb_node **link;
+
+	spin_lock(&fc->lock);
+
+	link = fuse_find_polled_node(fc, kh, NULL);
+	if (*link) {
+		struct fuse_file *ff;
+
+		ff = rb_entry(*link, struct fuse_file, polled_node);
+		wake_up_interruptible_sync(&ff->poll_wait);
+	}
+
+	spin_unlock(&fc->lock);
+	return 0;
+}
+
 static const struct file_operations fuse_file_operations = {
 	.llseek		= fuse_file_llseek,
 	.read		= do_sync_read,
@@ -1484,6 +1897,9 @@ static const struct file_operations fuse_file_operations = {
 	.lock		= fuse_file_lock,
 	.flock		= fuse_file_flock,
 	.splice_read	= generic_file_splice_read,
+	.unlocked_ioctl	= fuse_file_ioctl,
+	.compat_ioctl	= fuse_file_compat_ioctl,
+	.poll		= fuse_file_poll,
 };
 
 static const struct file_operations fuse_direct_io_file_operations = {
@@ -1496,6 +1912,9 @@ static const struct file_operations fuse_direct_io_file_operations = {
 	.fsync		= fuse_fsync,
 	.lock		= fuse_file_lock,
 	.flock		= fuse_file_flock,
+	.unlocked_ioctl	= fuse_file_ioctl,
+	.compat_ioctl	= fuse_file_compat_ioctl,
+	.poll		= fuse_file_poll,
 	/* no mmap and splice_read */
 };
 
diff --git a/fs/fuse/fuse_i.h b/fs/fuse/fuse_i.h
index 35accfdd747f..5e64b815a5a1 100644
--- a/fs/fuse/fuse_i.h
+++ b/fs/fuse/fuse_i.h
@@ -1,6 +1,6 @@
 /*
   FUSE: Filesystem in Userspace
-  Copyright (C) 2001-2006  Miklos Szeredi <miklos@szeredi.hu>
+  Copyright (C) 2001-2008  Miklos Szeredi <miklos@szeredi.hu>
 
   This program can be distributed under the terms of the GNU GPL.
   See the file COPYING.
@@ -19,6 +19,8 @@
 #include <linux/backing-dev.h>
 #include <linux/mutex.h>
 #include <linux/rwsem.h>
+#include <linux/rbtree.h>
+#include <linux/poll.h>
 
 /** Max number of pages that can be used in a single read request */
 #define FUSE_MAX_PAGES_PER_REQ 32
@@ -100,6 +102,9 @@ struct fuse_file {
 	/** Request reserved for flush and release */
 	struct fuse_req *reserved_req;
 
+	/** Kernel file handle guaranteed to be unique */
+	u64 kh;
+
 	/** File handle used by userspace */
 	u64 fh;
 
@@ -108,6 +113,12 @@ struct fuse_file {
 
 	/** Entry on inode's write_files list */
 	struct list_head write_entry;
+
+	/** RB node to be linked on fuse_conn->polled_files */
+	struct rb_node polled_node;
+
+	/** Wait queue head for poll */
+	wait_queue_head_t poll_wait;
 };
 
 /** One input argument of a request */
@@ -322,6 +333,12 @@ struct fuse_conn {
 	/** The list of requests under I/O */
 	struct list_head io;
 
+	/** The next unique kernel file handle */
+	u64 khctr;
+
+	/** rbtree of fuse_files waiting for poll events indexed by ph */
+	struct rb_root polled_files;
+
 	/** Number of requests currently in the background */
 	unsigned num_background;
 
@@ -355,19 +372,19 @@ struct fuse_conn {
 	/** Connection failed (version mismatch).  Cannot race with
 	    setting other bitfields since it is only set once in INIT
 	    reply, before any other request, and never cleared */
-	unsigned conn_error : 1;
+	unsigned conn_error:1;
 
 	/** Connection successful.  Only set in INIT */
-	unsigned conn_init : 1;
+	unsigned conn_init:1;
 
 	/** Do readpages asynchronously?  Only set in INIT */
-	unsigned async_read : 1;
+	unsigned async_read:1;
 
 	/** Do not send separate SETATTR request before open(O_TRUNC)  */
-	unsigned atomic_o_trunc : 1;
+	unsigned atomic_o_trunc:1;
 
 	/** Filesystem supports NFS exporting.  Only set in INIT */
-	unsigned export_support : 1;
+	unsigned export_support:1;
 
 	/*
 	 * The following bitfields are only for optimization purposes
@@ -375,43 +392,46 @@ struct fuse_conn {
 	 */
 
 	/** Is fsync not implemented by fs? */
-	unsigned no_fsync : 1;
+	unsigned no_fsync:1;
 
 	/** Is fsyncdir not implemented by fs? */
-	unsigned no_fsyncdir : 1;
+	unsigned no_fsyncdir:1;
 
 	/** Is flush not implemented by fs? */
-	unsigned no_flush : 1;
+	unsigned no_flush:1;
 
 	/** Is setxattr not implemented by fs? */
-	unsigned no_setxattr : 1;
+	unsigned no_setxattr:1;
 
 	/** Is getxattr not implemented by fs? */
-	unsigned no_getxattr : 1;
+	unsigned no_getxattr:1;
 
 	/** Is listxattr not implemented by fs? */
-	unsigned no_listxattr : 1;
+	unsigned no_listxattr:1;
 
 	/** Is removexattr not implemented by fs? */
-	unsigned no_removexattr : 1;
+	unsigned no_removexattr:1;
 
 	/** Are file locking primitives not implemented by fs? */
-	unsigned no_lock : 1;
+	unsigned no_lock:1;
 
 	/** Is access not implemented by fs? */
-	unsigned no_access : 1;
+	unsigned no_access:1;
 
 	/** Is create not implemented by fs? */
-	unsigned no_create : 1;
+	unsigned no_create:1;
 
 	/** Is interrupt not implemented by fs? */
-	unsigned no_interrupt : 1;
+	unsigned no_interrupt:1;
 
 	/** Is bmap not implemented by fs? */
-	unsigned no_bmap : 1;
+	unsigned no_bmap:1;
+
+	/** Is poll not implemented by fs? */
+	unsigned no_poll:1;
 
 	/** Do multi-page cached writes */
-	unsigned big_writes : 1;
+	unsigned big_writes:1;
 
 	/** The number of requests waiting for completion */
 	atomic_t num_waiting;
@@ -445,6 +465,9 @@ struct fuse_conn {
 
 	/** Version counter for attribute changes */
 	u64 attr_version;
+
+	/** Called on final put */
+	void (*release)(struct fuse_conn *);
 };
 
 static inline struct fuse_conn *get_fuse_conn_super(struct super_block *sb)
@@ -499,7 +522,7 @@ void fuse_read_fill(struct fuse_req *req, struct file *file,
  */
 int fuse_open_common(struct inode *inode, struct file *file, int isdir);
 
-struct fuse_file *fuse_file_alloc(void);
+struct fuse_file *fuse_file_alloc(struct fuse_conn *fc);
 void fuse_file_free(struct fuse_file *ff);
 void fuse_finish_open(struct inode *inode, struct file *file,
 		      struct fuse_file *ff, struct fuse_open_out *outarg);
@@ -519,6 +542,12 @@ int fuse_fsync_common(struct file *file, struct dentry *de, int datasync,
 		      int isdir);
 
 /**
+ * Notify poll wakeup
+ */
+int fuse_notify_poll_wakeup(struct fuse_conn *fc,
+			    struct fuse_notify_poll_wakeup_out *outarg);
+
+/**
  * Initialize file operations on a regular file
  */
 void fuse_init_file_inode(struct inode *inode);
@@ -593,19 +622,20 @@ void fuse_put_request(struct fuse_conn *fc, struct fuse_req *req);
 /**
  * Send a request (synchronous)
  */
-void request_send(struct fuse_conn *fc, struct fuse_req *req);
+void fuse_request_send(struct fuse_conn *fc, struct fuse_req *req);
 
 /**
  * Send a request with no reply
  */
-void request_send_noreply(struct fuse_conn *fc, struct fuse_req *req);
+void fuse_request_send_noreply(struct fuse_conn *fc, struct fuse_req *req);
 
 /**
  * Send a request in the background
  */
-void request_send_background(struct fuse_conn *fc, struct fuse_req *req);
+void fuse_request_send_background(struct fuse_conn *fc, struct fuse_req *req);
 
-void request_send_background_locked(struct fuse_conn *fc, struct fuse_req *req);
+void fuse_request_send_background_locked(struct fuse_conn *fc,
+					 struct fuse_req *req);
 
 /* Abort all requests */
 void fuse_abort_conn(struct fuse_conn *fc);
@@ -623,6 +653,11 @@ void fuse_invalidate_entry_cache(struct dentry *entry);
 struct fuse_conn *fuse_conn_get(struct fuse_conn *fc);
 
 /**
+ * Initialize fuse_conn
+ */
+int fuse_conn_init(struct fuse_conn *fc, struct super_block *sb);
+
+/**
  * Release reference to fuse_conn
  */
 void fuse_conn_put(struct fuse_conn *fc);
diff --git a/fs/fuse/inode.c b/fs/fuse/inode.c
index 2e99f34b4435..47c96fdca1ac 100644
--- a/fs/fuse/inode.c
+++ b/fs/fuse/inode.c
@@ -1,6 +1,6 @@
 /*
   FUSE: Filesystem in Userspace
-  Copyright (C) 2001-2006  Miklos Szeredi <miklos@szeredi.hu>
+  Copyright (C) 2001-2008  Miklos Szeredi <miklos@szeredi.hu>
 
   This program can be distributed under the terms of the GNU GPL.
   See the file COPYING.
@@ -37,10 +37,10 @@ struct fuse_mount_data {
 	unsigned rootmode;
 	unsigned user_id;
 	unsigned group_id;
-	unsigned fd_present : 1;
-	unsigned rootmode_present : 1;
-	unsigned user_id_present : 1;
-	unsigned group_id_present : 1;
+	unsigned fd_present:1;
+	unsigned rootmode_present:1;
+	unsigned user_id_present:1;
+	unsigned group_id_present:1;
 	unsigned flags;
 	unsigned max_read;
 	unsigned blksize;
@@ -94,7 +94,7 @@ void fuse_send_forget(struct fuse_conn *fc, struct fuse_req *req,
 	req->in.numargs = 1;
 	req->in.args[0].size = sizeof(struct fuse_forget_in);
 	req->in.args[0].value = inarg;
-	request_send_noreply(fc, req);
+	fuse_request_send_noreply(fc, req);
 }
 
 static void fuse_clear_inode(struct inode *inode)
@@ -250,7 +250,7 @@ struct inode *fuse_iget(struct super_block *sb, u64 nodeid,
 
 	fi = get_fuse_inode(inode);
 	spin_lock(&fc->lock);
-	fi->nlookup ++;
+	fi->nlookup++;
 	spin_unlock(&fc->lock);
 	fuse_change_attributes(inode, attr, attr_valid, attr_version);
 
@@ -269,7 +269,7 @@ static void fuse_send_destroy(struct fuse_conn *fc)
 		fc->destroy_req = NULL;
 		req->in.h.opcode = FUSE_DESTROY;
 		req->force = 1;
-		request_send(fc, req);
+		fuse_request_send(fc, req);
 		fuse_put_request(fc, req);
 	}
 }
@@ -334,7 +334,7 @@ static int fuse_statfs(struct dentry *dentry, struct kstatfs *buf)
 	req->out.args[0].size =
 		fc->minor < 4 ? FUSE_COMPAT_STATFS_SIZE : sizeof(outarg);
 	req->out.args[0].value = &outarg;
-	request_send(fc, req);
+	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	if (!err)
 		convert_fuse_statfs(buf, &outarg.st);
@@ -462,68 +462,69 @@ static int fuse_show_options(struct seq_file *m, struct vfsmount *mnt)
 	return 0;
 }
 
-static struct fuse_conn *new_conn(struct super_block *sb)
+int fuse_conn_init(struct fuse_conn *fc, struct super_block *sb)
 {
-	struct fuse_conn *fc;
 	int err;
 
-	fc = kzalloc(sizeof(*fc), GFP_KERNEL);
-	if (fc) {
-		spin_lock_init(&fc->lock);
-		mutex_init(&fc->inst_mutex);
-		atomic_set(&fc->count, 1);
-		init_waitqueue_head(&fc->waitq);
-		init_waitqueue_head(&fc->blocked_waitq);
-		init_waitqueue_head(&fc->reserved_req_waitq);
-		INIT_LIST_HEAD(&fc->pending);
-		INIT_LIST_HEAD(&fc->processing);
-		INIT_LIST_HEAD(&fc->io);
-		INIT_LIST_HEAD(&fc->interrupts);
-		INIT_LIST_HEAD(&fc->bg_queue);
-		atomic_set(&fc->num_waiting, 0);
-		fc->bdi.ra_pages = (VM_MAX_READAHEAD * 1024) / PAGE_CACHE_SIZE;
-		fc->bdi.unplug_io_fn = default_unplug_io_fn;
-		/* fuse does it's own writeback accounting */
-		fc->bdi.capabilities = BDI_CAP_NO_ACCT_WB;
-		fc->dev = sb->s_dev;
-		err = bdi_init(&fc->bdi);
-		if (err)
-			goto error_kfree;
-		if (sb->s_bdev) {
-			err = bdi_register(&fc->bdi, NULL, "%u:%u-fuseblk",
-					   MAJOR(fc->dev), MINOR(fc->dev));
-		} else {
-			err = bdi_register_dev(&fc->bdi, fc->dev);
-		}
-		if (err)
-			goto error_bdi_destroy;
-		/*
-		 * For a single fuse filesystem use max 1% of dirty +
-		 * writeback threshold.
-		 *
-		 * This gives about 1M of write buffer for memory maps on a
-		 * machine with 1G and 10% dirty_ratio, which should be more
-		 * than enough.
-		 *
-		 * Privileged users can raise it by writing to
-		 *
-		 *    /sys/class/bdi/<bdi>/max_ratio
-		 */
-		bdi_set_max_ratio(&fc->bdi, 1);
-		fc->reqctr = 0;
-		fc->blocked = 1;
-		fc->attr_version = 1;
-		get_random_bytes(&fc->scramble_key, sizeof(fc->scramble_key));
+	memset(fc, 0, sizeof(*fc));
+	spin_lock_init(&fc->lock);
+	mutex_init(&fc->inst_mutex);
+	atomic_set(&fc->count, 1);
+	init_waitqueue_head(&fc->waitq);
+	init_waitqueue_head(&fc->blocked_waitq);
+	init_waitqueue_head(&fc->reserved_req_waitq);
+	INIT_LIST_HEAD(&fc->pending);
+	INIT_LIST_HEAD(&fc->processing);
+	INIT_LIST_HEAD(&fc->io);
+	INIT_LIST_HEAD(&fc->interrupts);
+	INIT_LIST_HEAD(&fc->bg_queue);
+	INIT_LIST_HEAD(&fc->entry);
+	atomic_set(&fc->num_waiting, 0);
+	fc->bdi.ra_pages = (VM_MAX_READAHEAD * 1024) / PAGE_CACHE_SIZE;
+	fc->bdi.unplug_io_fn = default_unplug_io_fn;
+	/* fuse does it's own writeback accounting */
+	fc->bdi.capabilities = BDI_CAP_NO_ACCT_WB;
+	fc->khctr = 0;
+	fc->polled_files = RB_ROOT;
+	fc->dev = sb->s_dev;
+	err = bdi_init(&fc->bdi);
+	if (err)
+		goto error_mutex_destroy;
+	if (sb->s_bdev) {
+		err = bdi_register(&fc->bdi, NULL, "%u:%u-fuseblk",
+				   MAJOR(fc->dev), MINOR(fc->dev));
+	} else {
+		err = bdi_register_dev(&fc->bdi, fc->dev);
 	}
-	return fc;
+	if (err)
+		goto error_bdi_destroy;
+	/*
+	 * For a single fuse filesystem use max 1% of dirty +
+	 * writeback threshold.
+	 *
+	 * This gives about 1M of write buffer for memory maps on a
+	 * machine with 1G and 10% dirty_ratio, which should be more
+	 * than enough.
+	 *
+	 * Privileged users can raise it by writing to
+	 *
+	 *    /sys/class/bdi/<bdi>/max_ratio
+	 */
+	bdi_set_max_ratio(&fc->bdi, 1);
+	fc->reqctr = 0;
+	fc->blocked = 1;
+	fc->attr_version = 1;
+	get_random_bytes(&fc->scramble_key, sizeof(fc->scramble_key));
 
-error_bdi_destroy:
+	return 0;
+
+ error_bdi_destroy:
 	bdi_destroy(&fc->bdi);
-error_kfree:
+ error_mutex_destroy:
 	mutex_destroy(&fc->inst_mutex);
-	kfree(fc);
-	return NULL;
+	return err;
 }
+EXPORT_SYMBOL_GPL(fuse_conn_init);
 
 void fuse_conn_put(struct fuse_conn *fc)
 {
@@ -532,7 +533,7 @@ void fuse_conn_put(struct fuse_conn *fc)
 			fuse_request_free(fc->destroy_req);
 		mutex_destroy(&fc->inst_mutex);
 		bdi_destroy(&fc->bdi);
-		kfree(fc);
+		fc->release(fc);
 	}
 }
 
@@ -542,7 +543,7 @@ struct fuse_conn *fuse_conn_get(struct fuse_conn *fc)
 	return fc;
 }
 
-static struct inode *get_root_inode(struct super_block *sb, unsigned mode)
+static struct inode *fuse_get_root_inode(struct super_block *sb, unsigned mode)
 {
 	struct fuse_attr attr;
 	memset(&attr, 0, sizeof(attr));
@@ -553,8 +554,7 @@ static struct inode *get_root_inode(struct super_block *sb, unsigned mode)
 	return fuse_iget(sb, 1, 0, &attr, 0, 0);
 }
 
-struct fuse_inode_handle
-{
+struct fuse_inode_handle {
 	u64 nodeid;
 	u32 generation;
 };
@@ -761,7 +761,6 @@ static void process_init_reply(struct fuse_conn *fc, struct fuse_req *req)
 		fc->max_write = max_t(unsigned, 4096, fc->max_write);
 		fc->conn_init = 1;
 	}
-	fuse_put_request(fc, req);
 	fc->blocked = 0;
 	wake_up_all(&fc->blocked_waitq);
 }
@@ -787,7 +786,12 @@ static void fuse_send_init(struct fuse_conn *fc, struct fuse_req *req)
 	req->out.args[0].size = sizeof(struct fuse_init_out);
 	req->out.args[0].value = &req->misc.init_out;
 	req->end = process_init_reply;
-	request_send_background(fc, req);
+	fuse_request_send_background(fc, req);
+}
+
+static void fuse_free_conn(struct fuse_conn *fc)
+{
+	kfree(fc);
 }
 
 static int fuse_fill_super(struct super_block *sb, void *data, int silent)
@@ -828,10 +832,17 @@ static int fuse_fill_super(struct super_block *sb, void *data, int silent)
 	if (file->f_op != &fuse_dev_operations)
 		return -EINVAL;
 
-	fc = new_conn(sb);
+	fc = kmalloc(sizeof(*fc), GFP_KERNEL);
 	if (!fc)
 		return -ENOMEM;
 
+	err = fuse_conn_init(fc, sb);
+	if (err) {
+		kfree(fc);
+		return err;
+	}
+
+	fc->release = fuse_free_conn;
 	fc->flags = d.flags;
 	fc->user_id = d.user_id;
 	fc->group_id = d.group_id;
@@ -841,7 +852,7 @@ static int fuse_fill_super(struct super_block *sb, void *data, int silent)
 	sb->s_fs_info = fc;
 
 	err = -ENOMEM;
-	root = get_root_inode(sb, d.rootmode);
+	root = fuse_get_root_inode(sb, d.rootmode);
 	if (!root)
 		goto err;
 
@@ -952,7 +963,7 @@ static inline void unregister_fuseblk(void)
 
 static void fuse_inode_init_once(void *foo)
 {
-	struct inode * inode = foo;
+	struct inode *inode = foo;
 
 	inode_init_once(inode);
 }
@@ -1031,7 +1042,7 @@ static int __init fuse_init(void)
 {
 	int res;
 
-	printk("fuse init (API version %i.%i)\n",
+	printk(KERN_INFO "fuse init (API version %i.%i)\n",
 	       FUSE_KERNEL_VERSION, FUSE_KERNEL_MINOR_VERSION);
 
 	INIT_LIST_HEAD(&fuse_conn_list);
diff --git a/fs/gfs2/Kconfig b/fs/gfs2/Kconfig
index ab2f57e3fb87..e563a6449811 100644
--- a/fs/gfs2/Kconfig
+++ b/fs/gfs2/Kconfig
@@ -1,6 +1,6 @@
 config GFS2_FS
 	tristate "GFS2 file system support"
-	depends on EXPERIMENTAL && (64BIT || (LSF && LBD))
+	depends on EXPERIMENTAL && (64BIT || LBD)
 	select FS_POSIX_ACL
 	select CRC32
 	help
diff --git a/fs/gfs2/ops_address.c b/fs/gfs2/ops_address.c
index 6e4ea36c6605..4ddab67867eb 100644
--- a/fs/gfs2/ops_address.c
+++ b/fs/gfs2/ops_address.c
@@ -675,6 +675,7 @@ static int gfs2_write_begin(struct file *file, struct address_space *mapping,
 		goto out_trans_fail;
 
 	error = -ENOMEM;
+	flags |= AOP_FLAG_NOFS;
 	page = grab_cache_page_write_begin(mapping, index, flags);
 	*pagep = page;
 	if (unlikely(!page))
diff --git a/fs/gfs2/ops_file.c b/fs/gfs2/ops_file.c
index 289c5f54ba53..93fe41b67f97 100644
--- a/fs/gfs2/ops_file.c
+++ b/fs/gfs2/ops_file.c
@@ -342,7 +342,7 @@ static int gfs2_page_mkwrite(struct vm_area_struct *vma, struct page *page)
 	struct gfs2_inode *ip = GFS2_I(inode);
 	struct gfs2_sbd *sdp = GFS2_SB(inode);
 	unsigned long last_index;
-	u64 pos = page->index << (PAGE_CACHE_SIZE - inode->i_blkbits);
+	u64 pos = page->index << PAGE_CACHE_SHIFT;
 	unsigned int data_blocks, ind_blocks, rblocks;
 	int alloc_required = 0;
 	struct gfs2_holder gh;
diff --git a/fs/hugetlbfs/inode.c b/fs/hugetlbfs/inode.c
index 0ab0c6f5f438..6903d37af037 100644
--- a/fs/hugetlbfs/inode.c
+++ b/fs/hugetlbfs/inode.c
@@ -252,6 +252,7 @@ static ssize_t hugetlbfs_read(struct file *filp, char __user *buf,
 	for (;;) {
 		struct page *page;
 		unsigned long nr, ret;
+		int ra;
 
 		/* nr is the maximum number of bytes to copy from this page */
 		nr = huge_page_size(h);
@@ -274,16 +275,19 @@ static ssize_t hugetlbfs_read(struct file *filp, char __user *buf,
 			 */
 			ret = len < nr ? len : nr;
 			if (clear_user(buf, ret))
-				ret = -EFAULT;
+				ra = -EFAULT;
+			else
+				ra = 0;
 		} else {
 			/*
 			 * We have the page, copy it to user space buffer.
 			 */
-			ret = hugetlbfs_read_actor(page, offset, buf, len, nr);
+			ra = hugetlbfs_read_actor(page, offset, buf, len, nr);
+			ret = ra;
 		}
-		if (ret < 0) {
+		if (ra < 0) {
 			if (retval == 0)
-				retval = ret;
+				retval = ra;
 			if (page)
 				page_cache_release(page);
 			goto out;
diff --git a/fs/inode.c b/fs/inode.c
index bd48e5e6d3e8..0013ac1af8e7 100644
--- a/fs/inode.c
+++ b/fs/inode.c
@@ -22,6 +22,7 @@
 #include <linux/bootmem.h>
 #include <linux/inotify.h>
 #include <linux/mount.h>
+#include <linux/async.h>
 
 /*
  * This is needed for the following functions:
@@ -110,8 +111,8 @@ static void wake_up_inode(struct inode *inode)
 
 /**
  * inode_init_always - perform inode structure intialisation
- * @sb		- superblock inode belongs to.
- * @inode	- inode to initialise
+ * @sb: superblock inode belongs to
+ * @inode: inode to initialise
  *
  * These are initializations that need to be done on every inode
  * allocation as the fields are not initialised by slab allocation.
@@ -166,7 +167,7 @@ struct inode *inode_init_always(struct super_block *sb, struct inode *inode)
 	mapping->a_ops = &empty_aops;
 	mapping->host = inode;
 	mapping->flags = 0;
-	mapping_set_gfp_mask(mapping, GFP_HIGHUSER_PAGECACHE);
+	mapping_set_gfp_mask(mapping, GFP_HIGHUSER_MOVABLE);
 	mapping->assoc_mapping = NULL;
 	mapping->backing_dev_info = &default_backing_dev_info;
 	mapping->writeback_index = 0;
@@ -576,8 +577,8 @@ __inode_add_to_lists(struct super_block *sb, struct hlist_head *head,
 
 /**
  * inode_add_to_lists - add a new inode to relevant lists
- * @sb		- superblock inode belongs to.
- * @inode	- inode to mark in use
+ * @sb: superblock inode belongs to
+ * @inode: inode to mark in use
  *
  * When an inode is allocated it needs to be accounted for, added to the in use
  * list, the owning superblock and the inode hash. This needs to be done under
@@ -601,7 +602,7 @@ EXPORT_SYMBOL_GPL(inode_add_to_lists);
  *	@sb: superblock
  *
  *	Allocates a new inode for given superblock. The default gfp_mask
- *	for allocations related to inode->i_mapping is GFP_HIGHUSER_PAGECACHE.
+ *	for allocations related to inode->i_mapping is GFP_HIGHUSER_MOVABLE.
  *	If HIGHMEM pages are unsuitable or it is known that pages allocated
  *	for the page cache are not reclaimable or migratable,
  *	mapping_set_gfp_mask() must be called with suitable flags on the
@@ -1138,16 +1139,11 @@ EXPORT_SYMBOL(remove_inode_hash);
  * I_FREEING is set so that no-one will take a new reference to the inode while
  * it is being deleted.
  */
-void generic_delete_inode(struct inode *inode)
+static void generic_delete_inode_async(void *data, async_cookie_t cookie)
 {
+	struct inode *inode = data;
 	const struct super_operations *op = inode->i_sb->s_op;
 
-	list_del_init(&inode->i_list);
-	list_del_init(&inode->i_sb_list);
-	inode->i_state |= I_FREEING;
-	inodes_stat.nr_inodes--;
-	spin_unlock(&inode_lock);
-
 	security_inode_delete(inode);
 
 	if (op->delete_inode) {
@@ -1171,6 +1167,16 @@ void generic_delete_inode(struct inode *inode)
 	destroy_inode(inode);
 }
 
+void generic_delete_inode(struct inode *inode)
+{
+	list_del_init(&inode->i_list);
+	list_del_init(&inode->i_sb_list);
+	inode->i_state |= I_FREEING;
+	inodes_stat.nr_inodes--;
+	spin_unlock(&inode_lock);
+	async_schedule_special(generic_delete_inode_async, inode, &inode->i_sb->s_async_list);
+}
+
 EXPORT_SYMBOL(generic_delete_inode);
 
 static void generic_forget_inode(struct inode *inode)
diff --git a/fs/lockd/clntproc.c b/fs/lockd/clntproc.c
index 31668b690e03..dd7957064a8c 100644
--- a/fs/lockd/clntproc.c
+++ b/fs/lockd/clntproc.c
@@ -16,7 +16,6 @@
 #include <linux/sunrpc/clnt.h>
 #include <linux/sunrpc/svc.h>
 #include <linux/lockd/lockd.h>
-#include <linux/lockd/sm_inter.h>
 
 #define NLMDBG_FACILITY		NLMDBG_CLIENT
 #define NLMCLNT_GRACE_WAIT	(5*HZ)
@@ -518,11 +517,9 @@ nlmclnt_lock(struct nlm_rqst *req, struct file_lock *fl)
 	unsigned char fl_type;
 	int status = -ENOLCK;
 
-	if (nsm_monitor(host) < 0) {
-		printk(KERN_NOTICE "lockd: failed to monitor %s\n",
-					host->h_name);
+	if (nsm_monitor(host) < 0)
 		goto out;
-	}
+
 	fl->fl_flags |= FL_ACCESS;
 	status = do_vfs_lock(fl);
 	fl->fl_flags = fl_flags;
diff --git a/fs/lockd/host.c b/fs/lockd/host.c
index abdebf76b820..99d737bd4325 100644
--- a/fs/lockd/host.c
+++ b/fs/lockd/host.c
@@ -15,7 +15,6 @@
 #include <linux/sunrpc/clnt.h>
 #include <linux/sunrpc/svc.h>
 #include <linux/lockd/lockd.h>
-#include <linux/lockd/sm_inter.h>
 #include <linux/mutex.h>
 
 #include <net/ipv6.h>
@@ -32,11 +31,6 @@ static int			nrhosts;
 static DEFINE_MUTEX(nlm_host_mutex);
 
 static void			nlm_gc_hosts(void);
-static struct nsm_handle	*nsm_find(const struct sockaddr *sap,
-						const size_t salen,
-						const char *hostname,
-						const size_t hostname_len,
-						const int create);
 
 struct nlm_lookup_host_info {
 	const int		server;		/* search for server|client */
@@ -105,32 +99,6 @@ static void nlm_clear_port(struct sockaddr *sap)
 	}
 }
 
-static void nlm_display_address(const struct sockaddr *sap,
-				char *buf, const size_t len)
-{
-	const struct sockaddr_in *sin = (struct sockaddr_in *)sap;
-	const struct sockaddr_in6 *sin6 = (struct sockaddr_in6 *)sap;
-
-	switch (sap->sa_family) {
-	case AF_UNSPEC:
-		snprintf(buf, len, "unspecified");
-		break;
-	case AF_INET:
-		snprintf(buf, len, "%pI4", &sin->sin_addr.s_addr);
-		break;
-	case AF_INET6:
-		if (ipv6_addr_v4mapped(&sin6->sin6_addr))
-			snprintf(buf, len, "%pI4",
-				 &sin6->sin6_addr.s6_addr32[3]);
-		else
-			snprintf(buf, len, "%pI6", &sin6->sin6_addr);
-		break;
-	default:
-		snprintf(buf, len, "unsupported address family");
-		break;
-	}
-}
-
 /*
  * Common host lookup routine for server & client
  */
@@ -190,8 +158,8 @@ static struct nlm_host *nlm_lookup_host(struct nlm_lookup_host_info *ni)
 		atomic_inc(&nsm->sm_count);
 	else {
 		host = NULL;
-		nsm = nsm_find(ni->sap, ni->salen,
-				ni->hostname, ni->hostname_len, 1);
+		nsm = nsm_get_handle(ni->sap, ni->salen,
+					ni->hostname, ni->hostname_len);
 		if (!nsm) {
 			dprintk("lockd: nlm_lookup_host failed; "
 				"no nsm handle\n");
@@ -206,6 +174,7 @@ static struct nlm_host *nlm_lookup_host(struct nlm_lookup_host_info *ni)
 		goto out;
 	}
 	host->h_name	   = nsm->sm_name;
+	host->h_addrbuf    = nsm->sm_addrbuf;
 	memcpy(nlm_addr(host), ni->sap, ni->salen);
 	host->h_addrlen = ni->salen;
 	nlm_clear_port(nlm_addr(host));
@@ -232,11 +201,6 @@ static struct nlm_host *nlm_lookup_host(struct nlm_lookup_host_info *ni)
 
 	nrhosts++;
 
-	nlm_display_address((struct sockaddr *)&host->h_addr,
-				host->h_addrbuf, sizeof(host->h_addrbuf));
-	nlm_display_address((struct sockaddr *)&host->h_srcaddr,
-				host->h_srcaddrbuf, sizeof(host->h_srcaddrbuf));
-
 	dprintk("lockd: nlm_lookup_host created host %s\n",
 			host->h_name);
 
@@ -256,10 +220,8 @@ nlm_destroy_host(struct nlm_host *host)
 	BUG_ON(!list_empty(&host->h_lockowners));
 	BUG_ON(atomic_read(&host->h_count));
 
-	/*
-	 * Release NSM handle and unmonitor host.
-	 */
 	nsm_unmonitor(host);
+	nsm_release(host->h_nsmhandle);
 
 	clnt = host->h_rpcclnt;
 	if (clnt != NULL)
@@ -378,8 +340,8 @@ nlm_bind_host(struct nlm_host *host)
 {
 	struct rpc_clnt	*clnt;
 
-	dprintk("lockd: nlm_bind_host %s (%s), my addr=%s\n",
-			host->h_name, host->h_addrbuf, host->h_srcaddrbuf);
+	dprintk("lockd: nlm_bind_host %s (%s)\n",
+			host->h_name, host->h_addrbuf);
 
 	/* Lock host handle */
 	mutex_lock(&host->h_mutex);
@@ -481,35 +443,23 @@ void nlm_release_host(struct nlm_host *host)
 	}
 }
 
-/*
- * We were notified that the host indicated by address &sin
- * has rebooted.
- * Release all resources held by that peer.
+/**
+ * nlm_host_rebooted - Release all resources held by rebooted host
+ * @info: pointer to decoded results of NLM_SM_NOTIFY call
+ *
+ * We were notified that the specified host has rebooted.  Release
+ * all resources held by that peer.
  */
-void nlm_host_rebooted(const struct sockaddr_in *sin,
-				const char *hostname,
-				unsigned int hostname_len,
-				u32 new_state)
+void nlm_host_rebooted(const struct nlm_reboot *info)
 {
 	struct hlist_head *chain;
 	struct hlist_node *pos;
 	struct nsm_handle *nsm;
 	struct nlm_host	*host;
 
-	nsm = nsm_find((struct sockaddr *)sin, sizeof(*sin),
-			hostname, hostname_len, 0);
-	if (nsm == NULL) {
-		dprintk("lockd: never saw rebooted peer '%.*s' before\n",
-				hostname_len, hostname);
+	nsm = nsm_reboot_lookup(info);
+	if (unlikely(nsm == NULL))
 		return;
-	}
-
-	dprintk("lockd: nlm_host_rebooted(%.*s, %s)\n",
-			hostname_len, hostname, nsm->sm_addrbuf);
-
-	/* When reclaiming locks on this peer, make sure that
-	 * we set up a new notification */
-	nsm->sm_monitored = 0;
 
 	/* Mark all hosts tied to this NSM state as having rebooted.
 	 * We run the loop repeatedly, because we drop the host table
@@ -520,8 +470,8 @@ again:	mutex_lock(&nlm_host_mutex);
 	for (chain = nlm_hosts; chain < nlm_hosts + NLM_HOST_NRHASH; ++chain) {
 		hlist_for_each_entry(host, pos, chain, h_hash) {
 			if (host->h_nsmhandle == nsm
-			 && host->h_nsmstate != new_state) {
-				host->h_nsmstate = new_state;
+			 && host->h_nsmstate != info->state) {
+				host->h_nsmstate = info->state;
 				host->h_state++;
 
 				nlm_get_host(host);
@@ -629,89 +579,3 @@ nlm_gc_hosts(void)
 
 	next_gc = jiffies + NLM_HOST_COLLECT;
 }
-
-
-/*
- * Manage NSM handles
- */
-static LIST_HEAD(nsm_handles);
-static DEFINE_SPINLOCK(nsm_lock);
-
-static struct nsm_handle *nsm_find(const struct sockaddr *sap,
-				   const size_t salen,
-				   const char *hostname,
-				   const size_t hostname_len,
-				   const int create)
-{
-	struct nsm_handle *nsm = NULL;
-	struct nsm_handle *pos;
-
-	if (!sap)
-		return NULL;
-
-	if (hostname && memchr(hostname, '/', hostname_len) != NULL) {
-		if (printk_ratelimit()) {
-			printk(KERN_WARNING "Invalid hostname \"%.*s\" "
-					    "in NFS lock request\n",
-				(int)hostname_len, hostname);
-		}
-		return NULL;
-	}
-
-retry:
-	spin_lock(&nsm_lock);
-	list_for_each_entry(pos, &nsm_handles, sm_link) {
-
-		if (hostname && nsm_use_hostnames) {
-			if (strlen(pos->sm_name) != hostname_len
-			 || memcmp(pos->sm_name, hostname, hostname_len))
-				continue;
-		} else if (!nlm_cmp_addr(nsm_addr(pos), sap))
-			continue;
-		atomic_inc(&pos->sm_count);
-		kfree(nsm);
-		nsm = pos;
-		goto found;
-	}
-	if (nsm) {
-		list_add(&nsm->sm_link, &nsm_handles);
-		goto found;
-	}
-	spin_unlock(&nsm_lock);
-
-	if (!create)
-		return NULL;
-
-	nsm = kzalloc(sizeof(*nsm) + hostname_len + 1, GFP_KERNEL);
-	if (nsm == NULL)
-		return NULL;
-
-	memcpy(nsm_addr(nsm), sap, salen);
-	nsm->sm_addrlen = salen;
-	nsm->sm_name = (char *) (nsm + 1);
-	memcpy(nsm->sm_name, hostname, hostname_len);
-	nsm->sm_name[hostname_len] = '\0';
-	nlm_display_address((struct sockaddr *)&nsm->sm_addr,
-				nsm->sm_addrbuf, sizeof(nsm->sm_addrbuf));
-	atomic_set(&nsm->sm_count, 1);
-	goto retry;
-
-found:
-	spin_unlock(&nsm_lock);
-	return nsm;
-}
-
-/*
- * Release an NSM handle
- */
-void
-nsm_release(struct nsm_handle *nsm)
-{
-	if (!nsm)
-		return;
-	if (atomic_dec_and_lock(&nsm->sm_count, &nsm_lock)) {
-		list_del(&nsm->sm_link);
-		spin_unlock(&nsm_lock);
-		kfree(nsm);
-	}
-}
diff --git a/fs/lockd/mon.c b/fs/lockd/mon.c
index ffd3461f75ef..5e2c4d5ac827 100644
--- a/fs/lockd/mon.c
+++ b/fs/lockd/mon.c
@@ -9,35 +9,123 @@
 #include <linux/types.h>
 #include <linux/utsname.h>
 #include <linux/kernel.h>
+#include <linux/ktime.h>
+
 #include <linux/sunrpc/clnt.h>
 #include <linux/sunrpc/xprtsock.h>
 #include <linux/sunrpc/svc.h>
 #include <linux/lockd/lockd.h>
-#include <linux/lockd/sm_inter.h>
-
 
 #define NLMDBG_FACILITY		NLMDBG_MONITOR
+#define NSM_PROGRAM		100024
+#define NSM_VERSION		1
+
+enum {
+	NSMPROC_NULL,
+	NSMPROC_STAT,
+	NSMPROC_MON,
+	NSMPROC_UNMON,
+	NSMPROC_UNMON_ALL,
+	NSMPROC_SIMU_CRASH,
+	NSMPROC_NOTIFY,
+};
+
+struct nsm_args {
+	struct nsm_private	*priv;
+	u32			prog;		/* RPC callback info */
+	u32			vers;
+	u32			proc;
 
-#define XDR_ADDRBUF_LEN		(20)
+	char			*mon_name;
+};
 
-static struct rpc_clnt *	nsm_create(void);
+struct nsm_res {
+	u32			status;
+	u32			state;
+};
 
 static struct rpc_program	nsm_program;
+static				LIST_HEAD(nsm_handles);
+static				DEFINE_SPINLOCK(nsm_lock);
 
 /*
  * Local NSM state
  */
-int				nsm_local_state;
+int	__read_mostly		nsm_local_state;
+int	__read_mostly		nsm_use_hostnames;
 
-/*
- * Common procedure for SM_MON/SM_UNMON calls
- */
-static int
-nsm_mon_unmon(struct nsm_handle *nsm, u32 proc, struct nsm_res *res)
+static inline struct sockaddr *nsm_addr(const struct nsm_handle *nsm)
+{
+	return (struct sockaddr *)&nsm->sm_addr;
+}
+
+static void nsm_display_ipv4_address(const struct sockaddr *sap, char *buf,
+				     const size_t len)
+{
+	const struct sockaddr_in *sin = (struct sockaddr_in *)sap;
+	snprintf(buf, len, "%pI4", &sin->sin_addr.s_addr);
+}
+
+static void nsm_display_ipv6_address(const struct sockaddr *sap, char *buf,
+				     const size_t len)
+{
+	const struct sockaddr_in6 *sin6 = (struct sockaddr_in6 *)sap;
+
+	if (ipv6_addr_v4mapped(&sin6->sin6_addr))
+		snprintf(buf, len, "%pI4", &sin6->sin6_addr.s6_addr32[3]);
+	else if (sin6->sin6_scope_id != 0)
+		snprintf(buf, len, "%pI6%%%u", &sin6->sin6_addr,
+				sin6->sin6_scope_id);
+	else
+		snprintf(buf, len, "%pI6", &sin6->sin6_addr);
+}
+
+static void nsm_display_address(const struct sockaddr *sap,
+				char *buf, const size_t len)
+{
+	switch (sap->sa_family) {
+	case AF_INET:
+		nsm_display_ipv4_address(sap, buf, len);
+		break;
+	case AF_INET6:
+		nsm_display_ipv6_address(sap, buf, len);
+		break;
+	default:
+		snprintf(buf, len, "unsupported address family");
+		break;
+	}
+}
+
+static struct rpc_clnt *nsm_create(void)
+{
+	struct sockaddr_in sin = {
+		.sin_family		= AF_INET,
+		.sin_addr.s_addr	= htonl(INADDR_LOOPBACK),
+	};
+	struct rpc_create_args args = {
+		.protocol		= XPRT_TRANSPORT_UDP,
+		.address		= (struct sockaddr *)&sin,
+		.addrsize		= sizeof(sin),
+		.servername		= "rpc.statd",
+		.program		= &nsm_program,
+		.version		= NSM_VERSION,
+		.authflavor		= RPC_AUTH_NULL,
+	};
+
+	return rpc_create(&args);
+}
+
+static int nsm_mon_unmon(struct nsm_handle *nsm, u32 proc, struct nsm_res *res)
 {
 	struct rpc_clnt	*clnt;
 	int		status;
-	struct nsm_args	args;
+	struct nsm_args args = {
+		.priv		= &nsm->sm_priv,
+		.prog		= NLM_PROGRAM,
+		.vers		= 3,
+		.proc		= NLMPROC_NSM_NOTIFY,
+		.mon_name	= nsm->sm_mon_name,
+	};
 	struct rpc_message msg = {
 		.rpc_argp	= &args,
 		.rpc_resp	= res,
@@ -46,22 +134,18 @@ nsm_mon_unmon(struct nsm_handle *nsm, u32 proc, struct nsm_res *res)
 	clnt = nsm_create();
 	if (IS_ERR(clnt)) {
 		status = PTR_ERR(clnt);
+		dprintk("lockd: failed to create NSM upcall transport, "
+				"status=%d\n", status);
 		goto out;
 	}
 
-	memset(&args, 0, sizeof(args));
-	args.mon_name = nsm->sm_name;
-	args.addr = nsm_addr_in(nsm)->sin_addr.s_addr;
-	args.prog = NLM_PROGRAM;
-	args.vers = 3;
-	args.proc = NLMPROC_NSM_NOTIFY;
 	memset(res, 0, sizeof(*res));
 
 	msg.rpc_proc = &clnt->cl_procinfo[proc];
 	status = rpc_call_sync(clnt, &msg, 0);
 	if (status < 0)
-		printk(KERN_DEBUG "nsm_mon_unmon: rpc failed, status=%d\n",
-			status);
+		dprintk("lockd: NSM upcall RPC failed, status=%d\n",
+				status);
 	else
 		status = 0;
 	rpc_shutdown_client(clnt);
@@ -69,82 +153,272 @@ nsm_mon_unmon(struct nsm_handle *nsm, u32 proc, struct nsm_res *res)
 	return status;
 }
 
-/*
- * Set up monitoring of a remote host
+/**
+ * nsm_monitor - Notify a peer in case we reboot
+ * @host: pointer to nlm_host of peer to notify
+ *
+ * If this peer is not already monitored, this function sends an
+ * upcall to the local rpc.statd to record the name/address of
+ * the peer to notify in case we reboot.
+ *
+ * Returns zero if the peer is monitored by the local rpc.statd;
+ * otherwise a negative errno value is returned.
  */
-int
-nsm_monitor(struct nlm_host *host)
+int nsm_monitor(const struct nlm_host *host)
 {
 	struct nsm_handle *nsm = host->h_nsmhandle;
 	struct nsm_res	res;
 	int		status;
 
-	dprintk("lockd: nsm_monitor(%s)\n", host->h_name);
-	BUG_ON(nsm == NULL);
+	dprintk("lockd: nsm_monitor(%s)\n", nsm->sm_name);
 
 	if (nsm->sm_monitored)
 		return 0;
 
-	status = nsm_mon_unmon(nsm, SM_MON, &res);
+	/*
+	 * Choose whether to record the caller_name or IP address of
+	 * this peer in the local rpc.statd's database.
+	 */
+	nsm->sm_mon_name = nsm_use_hostnames ? nsm->sm_name : nsm->sm_addrbuf;
 
-	if (status < 0 || res.status != 0)
-		printk(KERN_NOTICE "lockd: cannot monitor %s\n", host->h_name);
+	status = nsm_mon_unmon(nsm, NSMPROC_MON, &res);
+	if (res.status != 0)
+		status = -EIO;
+	if (status < 0)
+		printk(KERN_NOTICE "lockd: cannot monitor %s\n", nsm->sm_name);
 	else
 		nsm->sm_monitored = 1;
 	return status;
 }
 
-/*
- * Cease to monitor remote host
+/**
+ * nsm_unmonitor - Unregister peer notification
+ * @host: pointer to nlm_host of peer to stop monitoring
+ *
+ * If this peer is monitored, this function sends an upcall to
+ * tell the local rpc.statd not to send this peer a notification
+ * when we reboot.
  */
-int
-nsm_unmonitor(struct nlm_host *host)
+void nsm_unmonitor(const struct nlm_host *host)
 {
 	struct nsm_handle *nsm = host->h_nsmhandle;
 	struct nsm_res	res;
-	int		status = 0;
-
-	if (nsm == NULL)
-		return 0;
-	host->h_nsmhandle = NULL;
+	int status;
 
 	if (atomic_read(&nsm->sm_count) == 1
 	 && nsm->sm_monitored && !nsm->sm_sticky) {
-		dprintk("lockd: nsm_unmonitor(%s)\n", host->h_name);
+		dprintk("lockd: nsm_unmonitor(%s)\n", nsm->sm_name);
 
-		status = nsm_mon_unmon(nsm, SM_UNMON, &res);
+		status = nsm_mon_unmon(nsm, NSMPROC_UNMON, &res);
+		if (res.status != 0)
+			status = -EIO;
 		if (status < 0)
 			printk(KERN_NOTICE "lockd: cannot unmonitor %s\n",
-					host->h_name);
+					nsm->sm_name);
 		else
 			nsm->sm_monitored = 0;
 	}
-	nsm_release(nsm);
-	return status;
+}
+
+static struct nsm_handle *nsm_lookup_hostname(const char *hostname,
+					      const size_t len)
+{
+	struct nsm_handle *nsm;
+
+	list_for_each_entry(nsm, &nsm_handles, sm_link)
+		if (strlen(nsm->sm_name) == len &&
+		    memcmp(nsm->sm_name, hostname, len) == 0)
+			return nsm;
+	return NULL;
+}
+
+static struct nsm_handle *nsm_lookup_addr(const struct sockaddr *sap)
+{
+	struct nsm_handle *nsm;
+
+	list_for_each_entry(nsm, &nsm_handles, sm_link)
+		if (nlm_cmp_addr(nsm_addr(nsm), sap))
+			return nsm;
+	return NULL;
+}
+
+static struct nsm_handle *nsm_lookup_priv(const struct nsm_private *priv)
+{
+	struct nsm_handle *nsm;
+
+	list_for_each_entry(nsm, &nsm_handles, sm_link)
+		if (memcmp(nsm->sm_priv.data, priv->data,
+					sizeof(priv->data)) == 0)
+			return nsm;
+	return NULL;
 }
 
 /*
- * Create NSM client for the local host
+ * Construct a unique cookie to match this nsm_handle to this monitored
+ * host.  It is passed to the local rpc.statd via NSMPROC_MON, and
+ * returned via NLMPROC_SM_NOTIFY, in the "priv" field of these
+ * requests.
+ *
+ * The NSM protocol requires that these cookies be unique while the
+ * system is running.  We prefer a stronger requirement of making them
+ * unique across reboots.  If user space bugs cause a stale cookie to
+ * be sent to the kernel, it could cause the wrong host to lose its
+ * lock state if cookies were not unique across reboots.
+ *
+ * The cookies are exposed only to local user space via loopback.  They
+ * do not appear on the physical network.  If we want greater security
+ * for some reason, nsm_init_private() could perform a one-way hash to
+ * obscure the contents of the cookie.
  */
-static struct rpc_clnt *
-nsm_create(void)
+static void nsm_init_private(struct nsm_handle *nsm)
 {
-	struct sockaddr_in	sin = {
-		.sin_family	= AF_INET,
-		.sin_addr.s_addr = htonl(INADDR_LOOPBACK),
-		.sin_port	= 0,
-	};
-	struct rpc_create_args args = {
-		.protocol	= XPRT_TRANSPORT_UDP,
-		.address	= (struct sockaddr *)&sin,
-		.addrsize	= sizeof(sin),
-		.servername	= "localhost",
-		.program	= &nsm_program,
-		.version	= SM_VERSION,
-		.authflavor	= RPC_AUTH_NULL,
-	};
+	u64 *p = (u64 *)&nsm->sm_priv.data;
+	struct timespec ts;
 
-	return rpc_create(&args);
+	ktime_get_ts(&ts);
+	*p++ = timespec_to_ns(&ts);
+	*p = (unsigned long)nsm;
+}
+
+static struct nsm_handle *nsm_create_handle(const struct sockaddr *sap,
+					    const size_t salen,
+					    const char *hostname,
+					    const size_t hostname_len)
+{
+	struct nsm_handle *new;
+
+	new = kzalloc(sizeof(*new) + hostname_len + 1, GFP_KERNEL);
+	if (unlikely(new == NULL))
+		return NULL;
+
+	atomic_set(&new->sm_count, 1);
+	new->sm_name = (char *)(new + 1);
+	memcpy(nsm_addr(new), sap, salen);
+	new->sm_addrlen = salen;
+	nsm_init_private(new);
+	nsm_display_address((const struct sockaddr *)&new->sm_addr,
+				new->sm_addrbuf, sizeof(new->sm_addrbuf));
+	memcpy(new->sm_name, hostname, hostname_len);
+	new->sm_name[hostname_len] = '\0';
+
+	return new;
+}
+
+/**
+ * nsm_get_handle - Find or create a cached nsm_handle
+ * @sap: pointer to socket address of handle to find
+ * @salen: length of socket address
+ * @hostname: pointer to C string containing hostname to find
+ * @hostname_len: length of C string
+ *
+ * Behavior is modulated by the global nsm_use_hostnames variable.
+ *
+ * Returns a cached nsm_handle after bumping its ref count, or
+ * returns a fresh nsm_handle if a handle that matches @sap and/or
+ * @hostname cannot be found in the handle cache.  Returns NULL if
+ * an error occurs.
+ */
+struct nsm_handle *nsm_get_handle(const struct sockaddr *sap,
+				  const size_t salen, const char *hostname,
+				  const size_t hostname_len)
+{
+	struct nsm_handle *cached, *new = NULL;
+
+	if (hostname && memchr(hostname, '/', hostname_len) != NULL) {
+		if (printk_ratelimit()) {
+			printk(KERN_WARNING "Invalid hostname \"%.*s\" "
+					    "in NFS lock request\n",
+				(int)hostname_len, hostname);
+		}
+		return NULL;
+	}
+
+retry:
+	spin_lock(&nsm_lock);
+
+	if (nsm_use_hostnames && hostname != NULL)
+		cached = nsm_lookup_hostname(hostname, hostname_len);
+	else
+		cached = nsm_lookup_addr(sap);
+
+	if (cached != NULL) {
+		atomic_inc(&cached->sm_count);
+		spin_unlock(&nsm_lock);
+		kfree(new);
+		dprintk("lockd: found nsm_handle for %s (%s), "
+				"cnt %d\n", cached->sm_name,
+				cached->sm_addrbuf,
+				atomic_read(&cached->sm_count));
+		return cached;
+	}
+
+	if (new != NULL) {
+		list_add(&new->sm_link, &nsm_handles);
+		spin_unlock(&nsm_lock);
+		dprintk("lockd: created nsm_handle for %s (%s)\n",
+				new->sm_name, new->sm_addrbuf);
+		return new;
+	}
+
+	spin_unlock(&nsm_lock);
+
+	new = nsm_create_handle(sap, salen, hostname, hostname_len);
+	if (unlikely(new == NULL))
+		return NULL;
+	goto retry;
+}
+
+/**
+ * nsm_reboot_lookup - match NLMPROC_SM_NOTIFY arguments to an nsm_handle
+ * @info: pointer to NLMPROC_SM_NOTIFY arguments
+ *
+ * Returns a matching nsm_handle if found in the nsm cache; the returned
+ * nsm_handle's reference count is bumped and sm_monitored is cleared.
+ * Otherwise returns NULL if some error occurred.
+ */
+struct nsm_handle *nsm_reboot_lookup(const struct nlm_reboot *info)
+{
+	struct nsm_handle *cached;
+
+	spin_lock(&nsm_lock);
+
+	cached = nsm_lookup_priv(&info->priv);
+	if (unlikely(cached == NULL)) {
+		spin_unlock(&nsm_lock);
+		dprintk("lockd: never saw rebooted peer '%.*s' before\n",
+				info->len, info->mon);
+		return cached;
+	}
+
+	atomic_inc(&cached->sm_count);
+	spin_unlock(&nsm_lock);
+
+	/*
+	 * During subsequent lock activity, force a fresh
+	 * notification to be set up for this host.
+	 */
+	cached->sm_monitored = 0;
+
+	dprintk("lockd: host %s (%s) rebooted, cnt %d\n",
+			cached->sm_name, cached->sm_addrbuf,
+			atomic_read(&cached->sm_count));
+	return cached;
+}
+
+/**
+ * nsm_release - Release an NSM handle
+ * @nsm: pointer to handle to be released
+ *
+ */
+void nsm_release(struct nsm_handle *nsm)
+{
+	if (atomic_dec_and_lock(&nsm->sm_count, &nsm_lock)) {
+		list_del(&nsm->sm_link);
+		spin_unlock(&nsm_lock);
+		dprintk("lockd: destroyed nsm_handle for %s (%s)\n",
+				nsm->sm_name, nsm->sm_addrbuf);
+		kfree(nsm);
+	}
 }
 
 /*
@@ -154,127 +428,132 @@ nsm_create(void)
  * Status Monitor wire protocol.
  */
 
-static __be32 *xdr_encode_nsm_string(__be32 *p, char *string)
+static int encode_nsm_string(struct xdr_stream *xdr, const char *string)
 {
-	size_t len = strlen(string);
-
-	if (len > SM_MAXSTRLEN)
-		len = SM_MAXSTRLEN;
-	return xdr_encode_opaque(p, string, len);
+	const u32 len = strlen(string);
+	__be32 *p;
+
+	if (unlikely(len > SM_MAXSTRLEN))
+		return -EIO;
+	p = xdr_reserve_space(xdr, sizeof(u32) + len);
+	if (unlikely(p == NULL))
+		return -EIO;
+	xdr_encode_opaque(p, string, len);
+	return 0;
 }
 
 /*
  * "mon_name" specifies the host to be monitored.
- *
- * Linux uses a text version of the IP address of the remote
- * host as the host identifier (the "mon_name" argument).
- *
- * Linux statd always looks up the canonical hostname first for
- * whatever remote hostname it receives, so this works alright.
  */
-static __be32 *xdr_encode_mon_name(__be32 *p, struct nsm_args *argp)
+static int encode_mon_name(struct xdr_stream *xdr, const struct nsm_args *argp)
 {
-	char	buffer[XDR_ADDRBUF_LEN + 1];
-	char	*name = argp->mon_name;
-
-	if (!nsm_use_hostnames) {
-		snprintf(buffer, XDR_ADDRBUF_LEN,
-			 "%pI4", &argp->addr);
-		name = buffer;
-	}
-
-	return xdr_encode_nsm_string(p, name);
+	return encode_nsm_string(xdr, argp->mon_name);
 }
 
 /*
  * The "my_id" argument specifies the hostname and RPC procedure
  * to be called when the status manager receives notification
- * (via the SM_NOTIFY call) that the state of host "mon_name"
+ * (via the NLMPROC_SM_NOTIFY call) that the state of host "mon_name"
  * has changed.
  */
-static __be32 *xdr_encode_my_id(__be32 *p, struct nsm_args *argp)
+static int encode_my_id(struct xdr_stream *xdr, const struct nsm_args *argp)
 {
-	p = xdr_encode_nsm_string(p, utsname()->nodename);
-	if (!p)
-		return ERR_PTR(-EIO);
-
+	int status;
+	__be32 *p;
+
+	status = encode_nsm_string(xdr, utsname()->nodename);
+	if (unlikely(status != 0))
+		return status;
+	p = xdr_reserve_space(xdr, 3 * sizeof(u32));
+	if (unlikely(p == NULL))
+		return -EIO;
 	*p++ = htonl(argp->prog);
 	*p++ = htonl(argp->vers);
 	*p++ = htonl(argp->proc);
-
-	return p;
+	return 0;
 }
 
 /*
  * The "mon_id" argument specifies the non-private arguments
- * of an SM_MON or SM_UNMON call.
+ * of an NSMPROC_MON or NSMPROC_UNMON call.
  */
-static __be32 *xdr_encode_mon_id(__be32 *p, struct nsm_args *argp)
+static int encode_mon_id(struct xdr_stream *xdr, const struct nsm_args *argp)
 {
-	p = xdr_encode_mon_name(p, argp);
-	if (!p)
-		return ERR_PTR(-EIO);
+	int status;
 
-	return xdr_encode_my_id(p, argp);
+	status = encode_mon_name(xdr, argp);
+	if (unlikely(status != 0))
+		return status;
+	return encode_my_id(xdr, argp);
 }
 
 /*
  * The "priv" argument may contain private information required
- * by the SM_MON call. This information will be supplied in the
- * SM_NOTIFY call.
- *
- * Linux provides the raw IP address of the monitored host,
- * left in network byte order.
+ * by the NSMPROC_MON call. This information will be supplied in the
+ * NLMPROC_SM_NOTIFY call.
  */
-static __be32 *xdr_encode_priv(__be32 *p, struct nsm_args *argp)
+static int encode_priv(struct xdr_stream *xdr, const struct nsm_args *argp)
 {
-	*p++ = argp->addr;
-	*p++ = 0;
-	*p++ = 0;
-	*p++ = 0;
+	__be32 *p;
 
-	return p;
+	p = xdr_reserve_space(xdr, SM_PRIV_SIZE);
+	if (unlikely(p == NULL))
+		return -EIO;
+	xdr_encode_opaque_fixed(p, argp->priv->data, SM_PRIV_SIZE);
+	return 0;
 }
 
-static int
-xdr_encode_mon(struct rpc_rqst *rqstp, __be32 *p, struct nsm_args *argp)
+static int xdr_enc_mon(struct rpc_rqst *req, __be32 *p,
+		       const struct nsm_args *argp)
 {
-	p = xdr_encode_mon_id(p, argp);
-	if (IS_ERR(p))
-		return PTR_ERR(p);
-
-	p = xdr_encode_priv(p, argp);
-	if (IS_ERR(p))
-		return PTR_ERR(p);
-
-	rqstp->rq_slen = xdr_adjust_iovec(rqstp->rq_svec, p);
-	return 0;
+	struct xdr_stream xdr;
+	int status;
+
+	xdr_init_encode(&xdr, &req->rq_snd_buf, p);
+	status = encode_mon_id(&xdr, argp);
+	if (unlikely(status))
+		return status;
+	return encode_priv(&xdr, argp);
 }
 
-static int
-xdr_encode_unmon(struct rpc_rqst *rqstp, __be32 *p, struct nsm_args *argp)
+static int xdr_enc_unmon(struct rpc_rqst *req, __be32 *p,
+			 const struct nsm_args *argp)
 {
-	p = xdr_encode_mon_id(p, argp);
-	if (IS_ERR(p))
-		return PTR_ERR(p);
-	rqstp->rq_slen = xdr_adjust_iovec(rqstp->rq_svec, p);
-	return 0;
+	struct xdr_stream xdr;
+
+	xdr_init_encode(&xdr, &req->rq_snd_buf, p);
+	return encode_mon_id(&xdr, argp);
 }
 
-static int
-xdr_decode_stat_res(struct rpc_rqst *rqstp, __be32 *p, struct nsm_res *resp)
+static int xdr_dec_stat_res(struct rpc_rqst *rqstp, __be32 *p,
+			    struct nsm_res *resp)
 {
+	struct xdr_stream xdr;
+
+	xdr_init_decode(&xdr, &rqstp->rq_rcv_buf, p);
+	p = xdr_inline_decode(&xdr, 2 * sizeof(u32));
+	if (unlikely(p == NULL))
+		return -EIO;
 	resp->status = ntohl(*p++);
-	resp->state = ntohl(*p++);
-	dprintk("nsm: xdr_decode_stat_res status %d state %d\n",
+	resp->state = ntohl(*p);
+
+	dprintk("lockd: xdr_dec_stat_res status %d state %d\n",
 			resp->status, resp->state);
 	return 0;
 }
 
-static int
-xdr_decode_stat(struct rpc_rqst *rqstp, __be32 *p, struct nsm_res *resp)
+static int xdr_dec_stat(struct rpc_rqst *rqstp, __be32 *p,
+			struct nsm_res *resp)
 {
-	resp->state = ntohl(*p++);
+	struct xdr_stream xdr;
+
+	xdr_init_decode(&xdr, &rqstp->rq_rcv_buf, p);
+	p = xdr_inline_decode(&xdr, sizeof(u32));
+	if (unlikely(p == NULL))
+		return -EIO;
+	resp->state = ntohl(*p);
+
+	dprintk("lockd: xdr_dec_stat state %d\n", resp->state);
 	return 0;
 }
 
@@ -288,22 +567,22 @@ xdr_decode_stat(struct rpc_rqst *rqstp, __be32 *p, struct nsm_res *resp)
 #define SM_unmonres_sz	1
 
 static struct rpc_procinfo	nsm_procedures[] = {
-[SM_MON] = {
-		.p_proc		= SM_MON,
-		.p_encode	= (kxdrproc_t) xdr_encode_mon,
-		.p_decode	= (kxdrproc_t) xdr_decode_stat_res,
+[NSMPROC_MON] = {
+		.p_proc		= NSMPROC_MON,
+		.p_encode	= (kxdrproc_t)xdr_enc_mon,
+		.p_decode	= (kxdrproc_t)xdr_dec_stat_res,
 		.p_arglen	= SM_mon_sz,
 		.p_replen	= SM_monres_sz,
-		.p_statidx	= SM_MON,
+		.p_statidx	= NSMPROC_MON,
 		.p_name		= "MONITOR",
 	},
-[SM_UNMON] = {
-		.p_proc		= SM_UNMON,
-		.p_encode	= (kxdrproc_t) xdr_encode_unmon,
-		.p_decode	= (kxdrproc_t) xdr_decode_stat,
+[NSMPROC_UNMON] = {
+		.p_proc		= NSMPROC_UNMON,
+		.p_encode	= (kxdrproc_t)xdr_enc_unmon,
+		.p_decode	= (kxdrproc_t)xdr_dec_stat,
 		.p_arglen	= SM_mon_id_sz,
 		.p_replen	= SM_unmonres_sz,
-		.p_statidx	= SM_UNMON,
+		.p_statidx	= NSMPROC_UNMON,
 		.p_name		= "UNMONITOR",
 	},
 };
@@ -322,7 +601,7 @@ static struct rpc_stat		nsm_stats;
 
 static struct rpc_program	nsm_program = {
 		.name		= "statd",
-		.number		= SM_PROGRAM,
+		.number		= NSM_PROGRAM,
 		.nrvers		= ARRAY_SIZE(nsm_version),
 		.version	= nsm_version,
 		.stats		= &nsm_stats
diff --git a/fs/lockd/svc.c b/fs/lockd/svc.c
index 252d80163d02..64f1c31b5853 100644
--- a/fs/lockd/svc.c
+++ b/fs/lockd/svc.c
@@ -35,7 +35,6 @@
 #include <linux/sunrpc/svcsock.h>
 #include <net/ip.h>
 #include <linux/lockd/lockd.h>
-#include <linux/lockd/sm_inter.h>
 #include <linux/nfs.h>
 
 #define NLMDBG_FACILITY		NLMDBG_SVC
@@ -54,13 +53,26 @@ static struct svc_rqst		*nlmsvc_rqst;
 unsigned long			nlmsvc_timeout;
 
 /*
+ * If the kernel has IPv6 support available, always listen for
+ * both AF_INET and AF_INET6 requests.
+ */
+#if (defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)) && \
+	defined(CONFIG_SUNRPC_REGISTER_V4)
+static const sa_family_t	nlmsvc_family = AF_INET6;
+#else	/* (CONFIG_IPV6 || CONFIG_IPV6_MODULE) && CONFIG_SUNRPC_REGISTER_V4 */
+static const sa_family_t	nlmsvc_family = AF_INET;
+#endif	/* (CONFIG_IPV6 || CONFIG_IPV6_MODULE) && CONFIG_SUNRPC_REGISTER_V4 */
+
+/*
  * These can be set at insmod time (useful for NFS as root filesystem),
  * and also changed through the sysctl interface.  -- Jamie Lokier, Aug 2003
  */
 static unsigned long		nlm_grace_period;
 static unsigned long		nlm_timeout = LOCKD_DFLT_TIMEO;
 static int			nlm_udpport, nlm_tcpport;
-int				nsm_use_hostnames = 0;
+
+/* RLIM_NOFILE defaults to 1024. That seems like a reasonable default here. */
+static unsigned int		nlm_max_connections = 1024;
 
 /*
  * Constants needed for the sysctl interface.
@@ -143,6 +155,9 @@ lockd(void *vrqstp)
 		long timeout = MAX_SCHEDULE_TIMEOUT;
 		RPC_IFDEBUG(char buf[RPC_MAX_ADDRBUFLEN]);
 
+		/* update sv_maxconn if it has changed */
+		rqstp->rq_server->sv_maxconn = nlm_max_connections;
+
 		if (signalled()) {
 			flush_signals(current);
 			if (nlmsvc_ops) {
@@ -189,6 +204,19 @@ lockd(void *vrqstp)
 	return 0;
 }
 
+static int create_lockd_listener(struct svc_serv *serv, char *name,
+				 unsigned short port)
+{
+	struct svc_xprt *xprt;
+
+	xprt = svc_find_xprt(serv, name, 0, 0);
+	if (xprt == NULL)
+		return svc_create_xprt(serv, name, port, SVC_SOCK_DEFAULTS);
+
+	svc_xprt_put(xprt);
+	return 0;
+}
+
 /*
  * Ensure there are active UDP and TCP listeners for lockd.
  *
@@ -202,29 +230,23 @@ lockd(void *vrqstp)
 static int make_socks(struct svc_serv *serv)
 {
 	static int warned;
-	struct svc_xprt *xprt;
-	int err = 0;
+	int err;
 
-	xprt = svc_find_xprt(serv, "udp", 0, 0);
-	if (!xprt)
-		err = svc_create_xprt(serv, "udp", nlm_udpport,
-				      SVC_SOCK_DEFAULTS);
-	else
-		svc_xprt_put(xprt);
-	if (err >= 0) {
-		xprt = svc_find_xprt(serv, "tcp", 0, 0);
-		if (!xprt)
-			err = svc_create_xprt(serv, "tcp", nlm_tcpport,
-					      SVC_SOCK_DEFAULTS);
-		else
-			svc_xprt_put(xprt);
-	}
-	if (err >= 0) {
-		warned = 0;
-		err = 0;
-	} else if (warned++ == 0)
+	err = create_lockd_listener(serv, "udp", nlm_udpport);
+	if (err < 0)
+		goto out_err;
+
+	err = create_lockd_listener(serv, "tcp", nlm_tcpport);
+	if (err < 0)
+		goto out_err;
+
+	warned = 0;
+	return 0;
+
+out_err:
+	if (warned++ == 0)
 		printk(KERN_WARNING
-		       "lockd_up: makesock failed, error=%d\n", err);
+			"lockd_up: makesock failed, error=%d\n", err);
 	return err;
 }
 
@@ -252,7 +274,7 @@ int lockd_up(void)
 			"lockd_up: no pid, %d users??\n", nlmsvc_users);
 
 	error = -ENOMEM;
-	serv = svc_create(&nlmsvc_program, LOCKD_BUFSIZE, AF_INET, NULL);
+	serv = svc_create(&nlmsvc_program, LOCKD_BUFSIZE, nlmsvc_family, NULL);
 	if (!serv) {
 		printk(KERN_WARNING "lockd_up: create service failed\n");
 		goto out;
@@ -276,6 +298,7 @@ int lockd_up(void)
 	}
 
 	svc_sock_update_bufs(serv);
+	serv->sv_maxconn = nlm_max_connections;
 
 	nlmsvc_task = kthread_run(lockd, nlmsvc_rqst, serv->sv_name);
 	if (IS_ERR(nlmsvc_task)) {
@@ -485,6 +508,7 @@ module_param_call(nlm_udpport, param_set_port, param_get_int,
 module_param_call(nlm_tcpport, param_set_port, param_get_int,
 		  &nlm_tcpport, 0644);
 module_param(nsm_use_hostnames, bool, 0644);
+module_param(nlm_max_connections, uint, 0644);
 
 /*
  * Initialising and terminating the module.
diff --git a/fs/lockd/svc4proc.c b/fs/lockd/svc4proc.c
index 4dfdcbc6bf68..1725037374c5 100644
--- a/fs/lockd/svc4proc.c
+++ b/fs/lockd/svc4proc.c
@@ -16,8 +16,6 @@
 #include <linux/nfsd/nfsd.h>
 #include <linux/lockd/lockd.h>
 #include <linux/lockd/share.h>
-#include <linux/lockd/sm_inter.h>
-
 
 #define NLMDBG_FACILITY		NLMDBG_CLIENT
 
@@ -419,8 +417,6 @@ static __be32
 nlm4svc_proc_sm_notify(struct svc_rqst *rqstp, struct nlm_reboot *argp,
 					      void	        *resp)
 {
-	struct sockaddr_in	saddr;
-
 	dprintk("lockd: SM_NOTIFY     called\n");
 
 	if (!nlm_privileged_requester(rqstp)) {
@@ -430,14 +426,7 @@ nlm4svc_proc_sm_notify(struct svc_rqst *rqstp, struct nlm_reboot *argp,
 		return rpc_system_err;
 	}
 
-	/* Obtain the host pointer for this NFS server and try to
-	 * reclaim all locks we hold on this server.
-	 */
-	memset(&saddr, 0, sizeof(saddr));
-	saddr.sin_family = AF_INET;
-	saddr.sin_addr.s_addr = argp->addr;
-	nlm_host_rebooted(&saddr, argp->mon, argp->len, argp->state);
-
+	nlm_host_rebooted(argp);
 	return rpc_success;
 }
 
diff --git a/fs/lockd/svcproc.c b/fs/lockd/svcproc.c
index 3ca89e2a9381..3688e55901fc 100644
--- a/fs/lockd/svcproc.c
+++ b/fs/lockd/svcproc.c
@@ -16,8 +16,6 @@
 #include <linux/nfsd/nfsd.h>
 #include <linux/lockd/lockd.h>
 #include <linux/lockd/share.h>
-#include <linux/lockd/sm_inter.h>
-
 
 #define NLMDBG_FACILITY		NLMDBG_CLIENT
 
@@ -451,8 +449,6 @@ static __be32
 nlmsvc_proc_sm_notify(struct svc_rqst *rqstp, struct nlm_reboot *argp,
 					      void	        *resp)
 {
-	struct sockaddr_in	saddr;
-
 	dprintk("lockd: SM_NOTIFY     called\n");
 
 	if (!nlm_privileged_requester(rqstp)) {
@@ -462,14 +458,7 @@ nlmsvc_proc_sm_notify(struct svc_rqst *rqstp, struct nlm_reboot *argp,
 		return rpc_system_err;
 	}
 
-	/* Obtain the host pointer for this NFS server and try to
-	 * reclaim all locks we hold on this server.
-	 */
-	memset(&saddr, 0, sizeof(saddr));
-	saddr.sin_family = AF_INET;
-	saddr.sin_addr.s_addr = argp->addr;
-	nlm_host_rebooted(&saddr, argp->mon, argp->len, argp->state);
-
+	nlm_host_rebooted(argp);
 	return rpc_success;
 }
 
diff --git a/fs/lockd/svcsubs.c b/fs/lockd/svcsubs.c
index 34c2766e27c7..9e4d6aab611b 100644
--- a/fs/lockd/svcsubs.c
+++ b/fs/lockd/svcsubs.c
@@ -17,7 +17,6 @@
 #include <linux/nfsd/export.h>
 #include <linux/lockd/lockd.h>
 #include <linux/lockd/share.h>
-#include <linux/lockd/sm_inter.h>
 #include <linux/module.h>
 #include <linux/mount.h>
 
diff --git a/fs/lockd/xdr.c b/fs/lockd/xdr.c
index 1f226290c67c..0336f2beacde 100644
--- a/fs/lockd/xdr.c
+++ b/fs/lockd/xdr.c
@@ -16,7 +16,6 @@
 #include <linux/sunrpc/svc.h>
 #include <linux/sunrpc/stats.h>
 #include <linux/lockd/lockd.h>
-#include <linux/lockd/sm_inter.h>
 
 #define NLMDBG_FACILITY		NLMDBG_XDR
 
@@ -349,8 +348,8 @@ nlmsvc_decode_reboot(struct svc_rqst *rqstp, __be32 *p, struct nlm_reboot *argp)
 	if (!(p = xdr_decode_string_inplace(p, &argp->mon, &argp->len, SM_MAXSTRLEN)))
 		return 0;
 	argp->state = ntohl(*p++);
-	/* Preserve the address in network byte order */
-	argp->addr = *p++;
+	memcpy(&argp->priv.data, p, sizeof(argp->priv.data));
+	p += XDR_QUADLEN(SM_PRIV_SIZE);
 	return xdr_argsize_check(rqstp, p);
 }
 
diff --git a/fs/lockd/xdr4.c b/fs/lockd/xdr4.c
index 50c493a8ad8e..e1d528653192 100644
--- a/fs/lockd/xdr4.c
+++ b/fs/lockd/xdr4.c
@@ -17,7 +17,6 @@
 #include <linux/sunrpc/svc.h>
 #include <linux/sunrpc/stats.h>
 #include <linux/lockd/lockd.h>
-#include <linux/lockd/sm_inter.h>
 
 #define NLMDBG_FACILITY		NLMDBG_XDR
 
@@ -356,8 +355,8 @@ nlm4svc_decode_reboot(struct svc_rqst *rqstp, __be32 *p, struct nlm_reboot *argp
 	if (!(p = xdr_decode_string_inplace(p, &argp->mon, &argp->len, SM_MAXSTRLEN)))
 		return 0;
 	argp->state = ntohl(*p++);
-	/* Preserve the address in network byte order */
-	argp->addr  = *p++;
+	memcpy(&argp->priv.data, p, sizeof(argp->priv.data));
+	p += XDR_QUADLEN(SM_PRIV_SIZE);
 	return xdr_argsize_check(rqstp, p);
 }
 
diff --git a/fs/minix/dir.c b/fs/minix/dir.c
index f70433816a38..d4946c4c90e2 100644
--- a/fs/minix/dir.c
+++ b/fs/minix/dir.c
@@ -280,7 +280,7 @@ int minix_add_link(struct dentry *dentry, struct inode *inode)
 	return -EINVAL;
 
 got_it:
-	pos = (page->index >> PAGE_CACHE_SHIFT) + p - (char*)page_address(page);
+	pos = page_offset(page) + p - (char *)page_address(page);
 	err = __minix_write_begin(NULL, page->mapping, pos, sbi->s_dirsize,
 					AOP_FLAG_UNINTERRUPTIBLE, &page, NULL);
 	if (err)
diff --git a/fs/mpage.c b/fs/mpage.c
index 552b80b3facc..16c3ef37eae3 100644
--- a/fs/mpage.c
+++ b/fs/mpage.c
@@ -241,7 +241,6 @@ do_mpage_readpage(struct bio *bio, struct page *page, unsigned nr_pages,
 				first_hole = page_block;
 			page_block++;
 			block_in_file++;
-			clear_buffer_mapped(map_bh);
 			continue;
 		}
 
@@ -308,7 +307,10 @@ alloc_new:
 		goto alloc_new;
 	}
 
-	if (buffer_boundary(map_bh) || (first_hole != blocks_per_page))
+	relative_block = block_in_file - *first_logical_block;
+	nblocks = map_bh->b_size >> blkbits;
+	if ((buffer_boundary(map_bh) && relative_block == nblocks) ||
+	    (first_hole != blocks_per_page))
 		bio = mpage_bio_submit(READ, bio);
 	else
 		*last_block_in_bio = blocks[blocks_per_page - 1];
diff --git a/fs/ncpfs/getopt.c b/fs/ncpfs/getopt.c
index 335b003dddf9..0af3349de851 100644
--- a/fs/ncpfs/getopt.c
+++ b/fs/ncpfs/getopt.c
@@ -16,7 +16,6 @@
  *	@opts: an array of &struct option entries controlling parser operations
  *	@optopt: output; will contain the current option
  *	@optarg: output; will contain the value (if one exists)
- *	@flag: output; may be NULL; should point to a long for or'ing flags
  *	@value: output; may be NULL; will be overwritten with the integer value
  *		of the current argument.
  *
diff --git a/fs/ncpfs/ioctl.c b/fs/ncpfs/ioctl.c
index 6d04e050c74e..f54360f50a9c 100644
--- a/fs/ncpfs/ioctl.c
+++ b/fs/ncpfs/ioctl.c
@@ -98,7 +98,7 @@ struct compat_ncp_objectname_ioctl
 {
 	s32		auth_type;
 	u32		object_name_len;
-	compat_caddr_t	object_name;	/* an userspace data, in most cases user name */
+	compat_caddr_t	object_name;	/* a userspace data, in most cases user name */
 };
 
 struct compat_ncp_fs_info_v2 {
diff --git a/fs/nfsd/auth.c b/fs/nfsd/auth.c
index 0184fe9b514c..c903e04aa217 100644
--- a/fs/nfsd/auth.c
+++ b/fs/nfsd/auth.c
@@ -76,10 +76,10 @@ int nfsd_setuser(struct svc_rqst *rqstp, struct svc_export *exp)
 
 	ret = set_groups(new, gi);
 	put_group_info(gi);
-	if (!ret)
+	if (ret < 0)
 		goto error;
 
-	if (new->uid)
+	if (new->fsuid)
 		new->cap_effective = cap_drop_nfsd_set(new->cap_effective);
 	else
 		new->cap_effective = cap_raise_nfsd_set(new->cap_effective,
diff --git a/fs/nfsd/nfs4callback.c b/fs/nfsd/nfs4callback.c
index 6d7d8c02c197..c464181b5994 100644
--- a/fs/nfsd/nfs4callback.c
+++ b/fs/nfsd/nfs4callback.c
@@ -53,9 +53,6 @@
 #define NFSPROC4_CB_NULL 0
 #define NFSPROC4_CB_COMPOUND 1
 
-/* declarations */
-static const struct rpc_call_ops nfs4_cb_null_ops;
-
 /* Index of predefined Linux callback client operations */
 
 enum {
diff --git a/fs/nfsd/nfs4proc.c b/fs/nfsd/nfs4proc.c
index 669461e291ae..9fa60a3ad48c 100644
--- a/fs/nfsd/nfs4proc.c
+++ b/fs/nfsd/nfs4proc.c
@@ -946,6 +946,11 @@ encode_op:
 			nfsd4_encode_operation(resp, op);
 			status = op->status;
 		}
+
+		dprintk("nfsv4 compound op %p opcnt %d #%d: %d: status %d\n",
+			args->ops, args->opcnt, resp->opcnt, op->opnum,
+			be32_to_cpu(status));
+
 		if (cstate->replay_owner) {
 			nfs4_put_stateowner(cstate->replay_owner);
 			cstate->replay_owner = NULL;
diff --git a/fs/nfsd/nfs4recover.c b/fs/nfsd/nfs4recover.c
index 0f9d6efaa62b..74f7b67567fd 100644
--- a/fs/nfsd/nfs4recover.c
+++ b/fs/nfsd/nfs4recover.c
@@ -116,9 +116,9 @@ nfs4_make_rec_clidname(char *dname, struct xdr_netobj *clname)
 
 	md5_to_hex(dname, cksum.data);
 
-	kfree(cksum.data);
 	status = nfs_ok;
 out:
+	kfree(cksum.data);
 	crypto_free_hash(desc.tfm);
 out_no_tfm:
 	return status;
diff --git a/fs/nfsd/nfs4state.c b/fs/nfsd/nfs4state.c
index 13e0e074dbb8..88db7d3ec120 100644
--- a/fs/nfsd/nfs4state.c
+++ b/fs/nfsd/nfs4state.c
@@ -2416,6 +2416,26 @@ out:
 #define LOCK_HASH_SIZE             (1 << LOCK_HASH_BITS)
 #define LOCK_HASH_MASK             (LOCK_HASH_SIZE - 1)
 
+static inline u64
+end_offset(u64 start, u64 len)
+{
+	u64 end;
+
+	end = start + len;
+	return end >= start ? end: NFS4_MAX_UINT64;
+}
+
+/* last octet in a range */
+static inline u64
+last_byte_offset(u64 start, u64 len)
+{
+	u64 end;
+
+	BUG_ON(!len);
+	end = start + len;
+	return end > start ? end - 1: NFS4_MAX_UINT64;
+}
+
 #define lockownerid_hashval(id) \
         ((id) & LOCK_HASH_MASK)
 
@@ -2435,13 +2455,13 @@ static struct list_head lockstateid_hashtbl[STATEID_HASH_SIZE];
 static struct nfs4_stateid *
 find_stateid(stateid_t *stid, int flags)
 {
-	struct nfs4_stateid *local = NULL;
+	struct nfs4_stateid *local;
 	u32 st_id = stid->si_stateownerid;
 	u32 f_id = stid->si_fileid;
 	unsigned int hashval;
 
 	dprintk("NFSD: find_stateid flags 0x%x\n",flags);
-	if ((flags & LOCK_STATE) || (flags & RD_STATE) || (flags & WR_STATE)) {
+	if (flags & (LOCK_STATE | RD_STATE | WR_STATE)) {
 		hashval = stateid_hashval(st_id, f_id);
 		list_for_each_entry(local, &lockstateid_hashtbl[hashval], st_hash) {
 			if ((local->st_stateid.si_stateownerid == st_id) &&
@@ -2449,7 +2469,8 @@ find_stateid(stateid_t *stid, int flags)
 				return local;
 		}
 	} 
-	if ((flags & OPEN_STATE) || (flags & RD_STATE) || (flags & WR_STATE)) {
+
+	if (flags & (OPEN_STATE | RD_STATE | WR_STATE)) {
 		hashval = stateid_hashval(st_id, f_id);
 		list_for_each_entry(local, &stateid_hashtbl[hashval], st_hash) {
 			if ((local->st_stateid.si_stateownerid == st_id) &&
@@ -2518,8 +2539,8 @@ nfs4_set_lock_denied(struct file_lock *fl, struct nfsd4_lock_denied *deny)
 		deny->ld_clientid.cl_id = 0;
 	}
 	deny->ld_start = fl->fl_start;
-	deny->ld_length = ~(u64)0;
-	if (fl->fl_end != ~(u64)0)
+	deny->ld_length = NFS4_MAX_UINT64;
+	if (fl->fl_end != NFS4_MAX_UINT64)
 		deny->ld_length = fl->fl_end - fl->fl_start + 1;        
 	deny->ld_type = NFS4_READ_LT;
 	if (fl->fl_type != F_RDLCK)
@@ -2616,7 +2637,7 @@ out:
 static int
 check_lock_length(u64 offset, u64 length)
 {
-	return ((length == 0)  || ((length != ~(u64)0) &&
+	return ((length == 0)  || ((length != NFS4_MAX_UINT64) &&
 	     LOFF_OVERFLOW(offset, length)));
 }
 
@@ -2736,11 +2757,7 @@ nfsd4_lock(struct svc_rqst *rqstp, struct nfsd4_compound_state *cstate,
 	file_lock.fl_lmops = &nfsd_posix_mng_ops;
 
 	file_lock.fl_start = lock->lk_offset;
-	if ((lock->lk_length == ~(u64)0) || 
-			LOFF_OVERFLOW(lock->lk_offset, lock->lk_length))
-		file_lock.fl_end = ~(u64)0;
-	else
-		file_lock.fl_end = lock->lk_offset + lock->lk_length - 1;
+	file_lock.fl_end = last_byte_offset(lock->lk_offset, lock->lk_length);
 	nfs4_transform_lock_offset(&file_lock);
 
 	/*
@@ -2781,6 +2798,25 @@ out:
 }
 
 /*
+ * The NFSv4 spec allows a client to do a LOCKT without holding an OPEN,
+ * so we do a temporary open here just to get an open file to pass to
+ * vfs_test_lock.  (Arguably perhaps test_lock should be done with an
+ * inode operation.)
+ */
+static int nfsd_test_lock(struct svc_rqst *rqstp, struct svc_fh *fhp, struct file_lock *lock)
+{
+	struct file *file;
+	int err;
+
+	err = nfsd_open(rqstp, fhp, S_IFREG, NFSD_MAY_READ, &file);
+	if (err)
+		return err;
+	err = vfs_test_lock(file, lock);
+	nfsd_close(file);
+	return err;
+}
+
+/*
  * LOCKT operation
  */
 __be32
@@ -2788,7 +2824,6 @@ nfsd4_lockt(struct svc_rqst *rqstp, struct nfsd4_compound_state *cstate,
 	    struct nfsd4_lockt *lockt)
 {
 	struct inode *inode;
-	struct file file;
 	struct file_lock file_lock;
 	int error;
 	__be32 status;
@@ -2839,23 +2874,12 @@ nfsd4_lockt(struct svc_rqst *rqstp, struct nfsd4_compound_state *cstate,
 	file_lock.fl_lmops = &nfsd_posix_mng_ops;
 
 	file_lock.fl_start = lockt->lt_offset;
-	if ((lockt->lt_length == ~(u64)0) || LOFF_OVERFLOW(lockt->lt_offset, lockt->lt_length))
-		file_lock.fl_end = ~(u64)0;
-	else
-		file_lock.fl_end = lockt->lt_offset + lockt->lt_length - 1;
+	file_lock.fl_end = last_byte_offset(lockt->lt_offset, lockt->lt_length);
 
 	nfs4_transform_lock_offset(&file_lock);
 
-	/* vfs_test_lock uses the struct file _only_ to resolve the inode.
-	 * since LOCKT doesn't require an OPEN, and therefore a struct
-	 * file may not exist, pass vfs_test_lock a struct file with
-	 * only the dentry:inode set.
-	 */
-	memset(&file, 0, sizeof (struct file));
-	file.f_path.dentry = cstate->current_fh.fh_dentry;
-
 	status = nfs_ok;
-	error = vfs_test_lock(&file, &file_lock);
+	error = nfsd_test_lock(rqstp, &cstate->current_fh, &file_lock);
 	if (error) {
 		status = nfserrno(error);
 		goto out;
@@ -2906,10 +2930,7 @@ nfsd4_locku(struct svc_rqst *rqstp, struct nfsd4_compound_state *cstate,
 	file_lock.fl_lmops = &nfsd_posix_mng_ops;
 	file_lock.fl_start = locku->lu_offset;
 
-	if ((locku->lu_length == ~(u64)0) || LOFF_OVERFLOW(locku->lu_offset, locku->lu_length))
-		file_lock.fl_end = ~(u64)0;
-	else
-		file_lock.fl_end = locku->lu_offset + locku->lu_length - 1;
+	file_lock.fl_end = last_byte_offset(locku->lu_offset, locku->lu_length);
 	nfs4_transform_lock_offset(&file_lock);
 
 	/*
diff --git a/fs/nfsd/nfs4xdr.c b/fs/nfsd/nfs4xdr.c
index afcdf4b76843..f65953be39c0 100644
--- a/fs/nfsd/nfs4xdr.c
+++ b/fs/nfsd/nfs4xdr.c
@@ -1,6 +1,4 @@
 /*
- *  fs/nfs/nfs4xdr.c
- *
  *  Server-side XDR for NFSv4
  *
  *  Copyright (c) 2002 The Regents of the University of Michigan.
diff --git a/fs/nfsd/nfsctl.c b/fs/nfsd/nfsctl.c
index 77d7b8c531a6..3d93b2064ce5 100644
--- a/fs/nfsd/nfsctl.c
+++ b/fs/nfsd/nfsctl.c
@@ -84,6 +84,8 @@ static ssize_t write_unexport(struct file *file, char *buf, size_t size);
 static ssize_t write_getfd(struct file *file, char *buf, size_t size);
 static ssize_t write_getfs(struct file *file, char *buf, size_t size);
 static ssize_t write_filehandle(struct file *file, char *buf, size_t size);
+static ssize_t write_unlock_ip(struct file *file, char *buf, size_t size);
+static ssize_t write_unlock_fs(struct file *file, char *buf, size_t size);
 static ssize_t write_threads(struct file *file, char *buf, size_t size);
 static ssize_t write_pool_threads(struct file *file, char *buf, size_t size);
 static ssize_t write_versions(struct file *file, char *buf, size_t size);
@@ -94,9 +96,6 @@ static ssize_t write_leasetime(struct file *file, char *buf, size_t size);
 static ssize_t write_recoverydir(struct file *file, char *buf, size_t size);
 #endif
 
-static ssize_t failover_unlock_ip(struct file *file, char *buf, size_t size);
-static ssize_t failover_unlock_fs(struct file *file, char *buf, size_t size);
-
 static ssize_t (*write_op[])(struct file *, char *, size_t) = {
 	[NFSD_Svc] = write_svc,
 	[NFSD_Add] = write_add,
@@ -106,8 +105,8 @@ static ssize_t (*write_op[])(struct file *, char *, size_t) = {
 	[NFSD_Getfd] = write_getfd,
 	[NFSD_Getfs] = write_getfs,
 	[NFSD_Fh] = write_filehandle,
-	[NFSD_FO_UnlockIP] = failover_unlock_ip,
-	[NFSD_FO_UnlockFS] = failover_unlock_fs,
+	[NFSD_FO_UnlockIP] = write_unlock_ip,
+	[NFSD_FO_UnlockFS] = write_unlock_fs,
 	[NFSD_Threads] = write_threads,
 	[NFSD_Pool_Threads] = write_pool_threads,
 	[NFSD_Versions] = write_versions,
@@ -176,10 +175,24 @@ static const struct file_operations exports_operations = {
 /*----------------------------------------------------------------------------*/
 /*
  * payload - write methods
- * If the method has a response, the response should be put in buf,
- * and the length returned.  Otherwise return 0 or and -error.
  */
 
+/**
+ * write_svc - Start kernel's NFSD server
+ *
+ * Deprecated.  /proc/fs/nfsd/threads is preferred.
+ * Function remains to support old versions of nfs-utils.
+ *
+ * Input:
+ *			buf:	struct nfsctl_svc
+ *				svc_port:	port number of this
+ *						server's listener
+ *				svc_nthreads:	number of threads to start
+ *			size:	size in bytes of passed in nfsctl_svc
+ * Output:
+ *	On success:	returns zero
+ *	On error:	return code is negative errno value
+ */
 static ssize_t write_svc(struct file *file, char *buf, size_t size)
 {
 	struct nfsctl_svc *data;
@@ -189,6 +202,30 @@ static ssize_t write_svc(struct file *file, char *buf, size_t size)
 	return nfsd_svc(data->svc_port, data->svc_nthreads);
 }
 
+/**
+ * write_add - Add or modify client entry in auth unix cache
+ *
+ * Deprecated.  /proc/net/rpc/auth.unix.ip is preferred.
+ * Function remains to support old versions of nfs-utils.
+ *
+ * Input:
+ *			buf:	struct nfsctl_client
+ *				cl_ident:	'\0'-terminated C string
+ *						containing domain name
+ *						of client
+ *				cl_naddr:	no. of items in cl_addrlist
+ *				cl_addrlist:	array of client addresses
+ *				cl_fhkeytype:	ignored
+ *				cl_fhkeylen:	ignored
+ *				cl_fhkey:	ignored
+ *			size:	size in bytes of passed in nfsctl_client
+ * Output:
+ *	On success:	returns zero
+ *	On error:	return code is negative errno value
+ *
+ * Note: Only AF_INET client addresses are passed in, since
+ * nfsctl_client.cl_addrlist contains only in_addr fields for addresses.
+ */
 static ssize_t write_add(struct file *file, char *buf, size_t size)
 {
 	struct nfsctl_client *data;
@@ -198,6 +235,30 @@ static ssize_t write_add(struct file *file, char *buf, size_t size)
 	return exp_addclient(data);
 }
 
+/**
+ * write_del - Remove client from auth unix cache
+ *
+ * Deprecated.  /proc/net/rpc/auth.unix.ip is preferred.
+ * Function remains to support old versions of nfs-utils.
+ *
+ * Input:
+ *			buf:	struct nfsctl_client
+ *				cl_ident:	'\0'-terminated C string
+ *						containing domain name
+ *						of client
+ *				cl_naddr:	ignored
+ *				cl_addrlist:	ignored
+ *				cl_fhkeytype:	ignored
+ *				cl_fhkeylen:	ignored
+ *				cl_fhkey:	ignored
+ *			size:	size in bytes of passed in nfsctl_client
+ * Output:
+ *	On success:	returns zero
+ *	On error:	return code is negative errno value
+ *
+ * Note: Only AF_INET client addresses are passed in, since
+ * nfsctl_client.cl_addrlist contains only in_addr fields for addresses.
+ */
 static ssize_t write_del(struct file *file, char *buf, size_t size)
 {
 	struct nfsctl_client *data;
@@ -207,6 +268,33 @@ static ssize_t write_del(struct file *file, char *buf, size_t size)
 	return exp_delclient(data);
 }
 
+/**
+ * write_export - Export part or all of a local file system
+ *
+ * Deprecated.  /proc/net/rpc/{nfsd.export,nfsd.fh} are preferred.
+ * Function remains to support old versions of nfs-utils.
+ *
+ * Input:
+ *			buf:	struct nfsctl_export
+ *				ex_client:	'\0'-terminated C string
+ *						containing domain name
+ *						of client allowed to access
+ *						this export
+ *				ex_path:	'\0'-terminated C string
+ *						containing pathname of
+ *						directory in local file system
+ *				ex_dev:		fsid to use for this export
+ *				ex_ino:		ignored
+ *				ex_flags:	export flags for this export
+ *				ex_anon_uid:	UID to use for anonymous
+ *						requests
+ *				ex_anon_gid:	GID to use for anonymous
+ *						requests
+ *			size:	size in bytes of passed in nfsctl_export
+ * Output:
+ *	On success:	returns zero
+ *	On error:	return code is negative errno value
+ */
 static ssize_t write_export(struct file *file, char *buf, size_t size)
 {
 	struct nfsctl_export *data;
@@ -216,6 +304,31 @@ static ssize_t write_export(struct file *file, char *buf, size_t size)
 	return exp_export(data);
 }
 
+/**
+ * write_unexport - Unexport a previously exported file system
+ *
+ * Deprecated.  /proc/net/rpc/{nfsd.export,nfsd.fh} are preferred.
+ * Function remains to support old versions of nfs-utils.
+ *
+ * Input:
+ *			buf:	struct nfsctl_export
+ *				ex_client:	'\0'-terminated C string
+ *						containing domain name
+ *						of client no longer allowed
+ *						to access this export
+ *				ex_path:	'\0'-terminated C string
+ *						containing pathname of
+ *						directory in local file system
+ *				ex_dev:		ignored
+ *				ex_ino:		ignored
+ *				ex_flags:	ignored
+ *				ex_anon_uid:	ignored
+ *				ex_anon_gid:	ignored
+ *			size:	size in bytes of passed in nfsctl_export
+ * Output:
+ *	On success:	returns zero
+ *	On error:	return code is negative errno value
+ */
 static ssize_t write_unexport(struct file *file, char *buf, size_t size)
 {
 	struct nfsctl_export *data;
@@ -226,6 +339,30 @@ static ssize_t write_unexport(struct file *file, char *buf, size_t size)
 	return exp_unexport(data);
 }
 
+/**
+ * write_getfs - Get a variable-length NFS file handle by path
+ *
+ * Deprecated.  /proc/fs/nfsd/filehandle is preferred.
+ * Function remains to support old versions of nfs-utils.
+ *
+ * Input:
+ *			buf:	struct nfsctl_fsparm
+ *				gd_addr:	socket address of client
+ *				gd_path:	'\0'-terminated C string
+ *						containing pathname of
+ *						directory in local file system
+ *				gd_maxlen:	maximum size of returned file
+ *						handle
+ *			size:	size in bytes of passed in nfsctl_fsparm
+ * Output:
+ *	On success:	passed-in buffer filled with a knfsd_fh structure
+ *			(a variable-length raw NFS file handle);
+ *			return code is the size in bytes of the file handle
+ *	On error:	return code is negative errno value
+ *
+ * Note: Only AF_INET client addresses are passed in, since gd_addr
+ * is the same size as a struct sockaddr_in.
+ */
 static ssize_t write_getfs(struct file *file, char *buf, size_t size)
 {
 	struct nfsctl_fsparm *data;
@@ -265,6 +402,29 @@ static ssize_t write_getfs(struct file *file, char *buf, size_t size)
 	return err;
 }
 
+/**
+ * write_getfd - Get a fixed-length NFS file handle by path (used by mountd)
+ *
+ * Deprecated.  /proc/fs/nfsd/filehandle is preferred.
+ * Function remains to support old versions of nfs-utils.
+ *
+ * Input:
+ *			buf:	struct nfsctl_fdparm
+ *				gd_addr:	socket address of client
+ *				gd_path:	'\0'-terminated C string
+ *						containing pathname of
+ *						directory in local file system
+ *				gd_version:	fdparm structure version
+ *			size:	size in bytes of passed in nfsctl_fdparm
+ * Output:
+ *	On success:	passed-in buffer filled with nfsctl_res
+ *			(a fixed-length raw NFS file handle);
+ *			return code is the size in bytes of the file handle
+ *	On error:	return code is negative errno value
+ *
+ * Note: Only AF_INET client addresses are passed in, since gd_addr
+ * is the same size as a struct sockaddr_in.
+ */
 static ssize_t write_getfd(struct file *file, char *buf, size_t size)
 {
 	struct nfsctl_fdparm *data;
@@ -309,7 +469,23 @@ static ssize_t write_getfd(struct file *file, char *buf, size_t size)
 	return err;
 }
 
-static ssize_t failover_unlock_ip(struct file *file, char *buf, size_t size)
+/**
+ * write_unlock_ip - Release all locks used by a client
+ *
+ * Experimental.
+ *
+ * Input:
+ *			buf:	'\n'-terminated C string containing a
+ *				presentation format IPv4 address
+ *			size:	length of C string in @buf
+ * Output:
+ *	On success:	returns zero if all specified locks were released;
+ *			returns one if one or more locks were not released
+ *	On error:	return code is negative errno value
+ *
+ * Note: Only AF_INET client addresses are passed in
+ */
+static ssize_t write_unlock_ip(struct file *file, char *buf, size_t size)
 {
 	struct sockaddr_in sin = {
 		.sin_family	= AF_INET,
@@ -339,7 +515,21 @@ static ssize_t failover_unlock_ip(struct file *file, char *buf, size_t size)
 	return nlmsvc_unlock_all_by_ip((struct sockaddr *)&sin);
 }
 
-static ssize_t failover_unlock_fs(struct file *file, char *buf, size_t size)
+/**
+ * write_unlock_fs - Release all locks on a local file system
+ *
+ * Experimental.
+ *
+ * Input:
+ *			buf:	'\n'-terminated C string containing the
+ *				absolute pathname of a local file system
+ *			size:	length of C string in @buf
+ * Output:
+ *	On success:	returns zero if all specified locks were released;
+ *			returns one if one or more locks were not released
+ *	On error:	return code is negative errno value
+ */
+static ssize_t write_unlock_fs(struct file *file, char *buf, size_t size)
 {
 	struct path path;
 	char *fo_path;
@@ -360,21 +550,44 @@ static ssize_t failover_unlock_fs(struct file *file, char *buf, size_t size)
 	if (error)
 		return error;
 
+	/*
+	 * XXX: Needs better sanity checking.  Otherwise we could end up
+	 * releasing locks on the wrong file system.
+	 *
+	 * For example:
+	 * 1.  Does the path refer to a directory?
+	 * 2.  Is that directory a mount point, or
+	 * 3.  Is that directory the root of an exported file system?
+	 */
 	error = nlmsvc_unlock_all_by_sb(path.mnt->mnt_sb);
 
 	path_put(&path);
 	return error;
 }
 
+/**
+ * write_filehandle - Get a variable-length NFS file handle by path
+ *
+ * On input, the buffer contains a '\n'-terminated C string comprised of
+ * three alphanumeric words separated by whitespace.  The string may
+ * contain escape sequences.
+ *
+ * Input:
+ *			buf:
+ *				domain:		client domain name
+ *				path:		export pathname
+ *				maxsize:	numeric maximum size of
+ *						@buf
+ *			size:	length of C string in @buf
+ * Output:
+ *	On success:	passed-in buffer filled with '\n'-terminated C
+ *			string containing a ASCII hex text version
+ *			of the NFS file handle;
+ *			return code is the size in bytes of the string
+ *	On error:	return code is negative errno value
+ */
 static ssize_t write_filehandle(struct file *file, char *buf, size_t size)
 {
-	/* request is:
-	 *   domain path maxsize
-	 * response is
-	 *   filehandle
-	 *
-	 * qword quoting is used, so filehandle will be \x....
-	 */
 	char *dname, *path;
 	int uninitialized_var(maxsize);
 	char *mesg = buf;
@@ -391,11 +604,13 @@ static ssize_t write_filehandle(struct file *file, char *buf, size_t size)
 
 	dname = mesg;
 	len = qword_get(&mesg, dname, size);
-	if (len <= 0) return -EINVAL;
+	if (len <= 0)
+		return -EINVAL;
 	
 	path = dname+len+1;
 	len = qword_get(&mesg, path, size);
-	if (len <= 0) return -EINVAL;
+	if (len <= 0)
+		return -EINVAL;
 
 	len = get_int(&mesg, &maxsize);
 	if (len)
@@ -419,17 +634,43 @@ static ssize_t write_filehandle(struct file *file, char *buf, size_t size)
 	if (len)
 		return len;
 	
-	mesg = buf; len = SIMPLE_TRANSACTION_LIMIT;
+	mesg = buf;
+	len = SIMPLE_TRANSACTION_LIMIT;
 	qword_addhex(&mesg, &len, (char*)&fh.fh_base, fh.fh_size);
 	mesg[-1] = '\n';
 	return mesg - buf;	
 }
 
+/**
+ * write_threads - Start NFSD, or report the current number of running threads
+ *
+ * Input:
+ *			buf:		ignored
+ *			size:		zero
+ * Output:
+ *	On success:	passed-in buffer filled with '\n'-terminated C
+ *			string numeric value representing the number of
+ *			running NFSD threads;
+ *			return code is the size in bytes of the string
+ *	On error:	return code is zero
+ *
+ * OR
+ *
+ * Input:
+ *			buf:		C string containing an unsigned
+ *					integer value representing the
+ *					number of NFSD threads to start
+ *			size:		non-zero length of C string in @buf
+ * Output:
+ *	On success:	NFS service is started;
+ *			passed-in buffer filled with '\n'-terminated C
+ *			string numeric value representing the number of
+ *			running NFSD threads;
+ *			return code is the size in bytes of the string
+ *	On error:	return code is zero or a negative errno value
+ */
 static ssize_t write_threads(struct file *file, char *buf, size_t size)
 {
-	/* if size > 0, look for a number of threads and call nfsd_svc
-	 * then write out number of threads as reply
-	 */
 	char *mesg = buf;
 	int rv;
 	if (size > 0) {
@@ -437,9 +678,9 @@ static ssize_t write_threads(struct file *file, char *buf, size_t size)
 		rv = get_int(&mesg, &newthreads);
 		if (rv)
 			return rv;
-		if (newthreads <0)
+		if (newthreads < 0)
 			return -EINVAL;
-		rv = nfsd_svc(2049, newthreads);
+		rv = nfsd_svc(NFS_PORT, newthreads);
 		if (rv)
 			return rv;
 	}
@@ -447,6 +688,28 @@ static ssize_t write_threads(struct file *file, char *buf, size_t size)
 	return strlen(buf);
 }
 
+/**
+ * write_pool_threads - Set or report the current number of threads per pool
+ *
+ * Input:
+ *			buf:		ignored
+ *			size:		zero
+ *
+ * OR
+ *
+ * Input:
+ * 			buf:		C string containing whitespace-
+ * 					separated unsigned integer values
+ *					representing the number of NFSD
+ *					threads to start in each pool
+ *			size:		non-zero length of C string in @buf
+ * Output:
+ *	On success:	passed-in buffer filled with '\n'-terminated C
+ *			string containing integer values representing the
+ *			number of NFSD threads in each pool;
+ *			return code is the size in bytes of the string
+ *	On error:	return code is zero or a negative errno value
+ */
 static ssize_t write_pool_threads(struct file *file, char *buf, size_t size)
 {
 	/* if size > 0, look for an array of number of threads per node
@@ -517,10 +780,6 @@ out_free:
 
 static ssize_t __write_versions(struct file *file, char *buf, size_t size)
 {
-	/*
-	 * Format:
-	 *   [-/+]vers [-/+]vers ...
-	 */
 	char *mesg = buf;
 	char *vers, sign;
 	int len, num;
@@ -578,6 +837,38 @@ static ssize_t __write_versions(struct file *file, char *buf, size_t size)
 	return len;
 }
 
+/**
+ * write_versions - Set or report the available NFS protocol versions
+ *
+ * Input:
+ *			buf:		ignored
+ *			size:		zero
+ * Output:
+ *	On success:	passed-in buffer filled with '\n'-terminated C
+ *			string containing positive or negative integer
+ *			values representing the current status of each
+ *			protocol version;
+ *			return code is the size in bytes of the string
+ *	On error:	return code is zero or a negative errno value
+ *
+ * OR
+ *
+ * Input:
+ * 			buf:		C string containing whitespace-
+ * 					separated positive or negative
+ * 					integer values representing NFS
+ * 					protocol versions to enable ("+n")
+ * 					or disable ("-n")
+ *			size:		non-zero length of C string in @buf
+ * Output:
+ *	On success:	status of zero or more protocol versions has
+ *			been updated; passed-in buffer filled with
+ *			'\n'-terminated C string containing positive
+ *			or negative integer values representing the
+ *			current status of each protocol version;
+ *			return code is the size in bytes of the string
+ *	On error:	return code is zero or a negative errno value
+ */
 static ssize_t write_versions(struct file *file, char *buf, size_t size)
 {
 	ssize_t rv;
@@ -687,6 +978,75 @@ static ssize_t __write_ports(struct file *file, char *buf, size_t size)
 	return -EINVAL;
 }
 
+/**
+ * write_ports - Pass a socket file descriptor or transport name to listen on
+ *
+ * Input:
+ *			buf:		ignored
+ *			size:		zero
+ * Output:
+ *	On success:	passed-in buffer filled with a '\n'-terminated C
+ *			string containing a whitespace-separated list of
+ *			named NFSD listeners;
+ *			return code is the size in bytes of the string
+ *	On error:	return code is zero or a negative errno value
+ *
+ * OR
+ *
+ * Input:
+ *			buf:		C string containing an unsigned
+ *					integer value representing a bound
+ *					but unconnected socket that is to be
+ *					used as an NFSD listener
+ *			size:		non-zero length of C string in @buf
+ * Output:
+ *	On success:	NFS service is started;
+ *			passed-in buffer filled with a '\n'-terminated C
+ *			string containing a unique alphanumeric name of
+ *			the listener;
+ *			return code is the size in bytes of the string
+ *	On error:	return code is a negative errno value
+ *
+ * OR
+ *
+ * Input:
+ *			buf:		C string containing a "-" followed
+ *					by an integer value representing a
+ *					previously passed in socket file
+ *					descriptor
+ *			size:		non-zero length of C string in @buf
+ * Output:
+ *	On success:	NFS service no longer listens on that socket;
+ *			passed-in buffer filled with a '\n'-terminated C
+ *			string containing a unique name of the listener;
+ *			return code is the size in bytes of the string
+ *	On error:	return code is a negative errno value
+ *
+ * OR
+ *
+ * Input:
+ *			buf:		C string containing a transport
+ *					name and an unsigned integer value
+ *					representing the port to listen on,
+ *					separated by whitespace
+ *			size:		non-zero length of C string in @buf
+ * Output:
+ *	On success:	returns zero; NFS service is started
+ *	On error:	return code is a negative errno value
+ *
+ * OR
+ *
+ * Input:
+ *			buf:		C string containing a "-" followed
+ *					by a transport name and an unsigned
+ *					integer value representing the port
+ *					to listen on, separated by whitespace
+ *			size:		non-zero length of C string in @buf
+ * Output:
+ *	On success:	returns zero; NFS service no longer listens
+ *			on that transport
+ *	On error:	return code is a negative errno value
+ */
 static ssize_t write_ports(struct file *file, char *buf, size_t size)
 {
 	ssize_t rv;
@@ -700,6 +1060,27 @@ static ssize_t write_ports(struct file *file, char *buf, size_t size)
 
 int nfsd_max_blksize;
 
+/**
+ * write_maxblksize - Set or report the current NFS blksize
+ *
+ * Input:
+ *			buf:		ignored
+ *			size:		zero
+ *
+ * OR
+ *
+ * Input:
+ * 			buf:		C string containing an unsigned
+ * 					integer value representing the new
+ * 					NFS blksize
+ *			size:		non-zero length of C string in @buf
+ * Output:
+ *	On success:	passed-in buffer filled with '\n'-terminated C string
+ *			containing numeric value of the current NFS blksize
+ *			setting;
+ *			return code is the size in bytes of the string
+ *	On error:	return code is zero or a negative errno value
+ */
 static ssize_t write_maxblksize(struct file *file, char *buf, size_t size)
 {
 	char *mesg = buf;
@@ -752,6 +1133,27 @@ static ssize_t __write_leasetime(struct file *file, char *buf, size_t size)
 	return strlen(buf);
 }
 
+/**
+ * write_leasetime - Set or report the current NFSv4 lease time
+ *
+ * Input:
+ *			buf:		ignored
+ *			size:		zero
+ *
+ * OR
+ *
+ * Input:
+ *			buf:		C string containing an unsigned
+ *					integer value representing the new
+ *					NFSv4 lease expiry time
+ *			size:		non-zero length of C string in @buf
+ * Output:
+ *	On success:	passed-in buffer filled with '\n'-terminated C
+ *			string containing unsigned integer value of the
+ *			current lease expiry time;
+ *			return code is the size in bytes of the string
+ *	On error:	return code is zero or a negative errno value
+ */
 static ssize_t write_leasetime(struct file *file, char *buf, size_t size)
 {
 	ssize_t rv;
@@ -788,6 +1190,27 @@ static ssize_t __write_recoverydir(struct file *file, char *buf, size_t size)
 	return strlen(buf);
 }
 
+/**
+ * write_recoverydir - Set or report the pathname of the recovery directory
+ *
+ * Input:
+ *			buf:		ignored
+ *			size:		zero
+ *
+ * OR
+ *
+ * Input:
+ *			buf:		C string containing the pathname
+ *					of the directory on a local file
+ *					system containing permanent NFSv4
+ *					recovery data
+ *			size:		non-zero length of C string in @buf
+ * Output:
+ *	On success:	passed-in buffer filled with '\n'-terminated C string
+ *			containing the current recovery pathname setting;
+ *			return code is the size in bytes of the string
+ *	On error:	return code is zero or a negative errno value
+ */
 static ssize_t write_recoverydir(struct file *file, char *buf, size_t size)
 {
 	ssize_t rv;
diff --git a/fs/nfsd/nfsfh.c b/fs/nfsd/nfsfh.c
index f0da7d9c3a92..9f1ca17293d3 100644
--- a/fs/nfsd/nfsfh.c
+++ b/fs/nfsd/nfsfh.c
@@ -258,14 +258,32 @@ out:
 	return error;
 }
 
-/*
- * Perform sanity checks on the dentry in a client's file handle.
+/**
+ * fh_verify - filehandle lookup and access checking
+ * @rqstp: pointer to current rpc request
+ * @fhp: filehandle to be verified
+ * @type: expected type of object pointed to by filehandle
+ * @access: type of access needed to object
+ *
+ * Look up a dentry from the on-the-wire filehandle, check the client's
+ * access to the export, and set the current task's credentials.
+ *
+ * Regardless of success or failure of fh_verify(), fh_put() should be
+ * called on @fhp when the caller is finished with the filehandle.
  *
- * Note that the file handle dentry may need to be freed even after
- * an error return.
+ * fh_verify() may be called multiple times on a given filehandle, for
+ * example, when processing an NFSv4 compound.  The first call will look
+ * up a dentry using the on-the-wire filehandle.  Subsequent calls will
+ * skip the lookup and just perform the other checks and possibly change
+ * the current task's credentials.
  *
- * This is only called at the start of an nfsproc call, so fhp points to
- * a svc_fh which is all 0 except for the over-the-wire file handle.
+ * @type specifies the type of object expected using one of the S_IF*
+ * constants defined in include/linux/stat.h.  The caller may use zero
+ * to indicate that it doesn't care, or a negative integer to indicate
+ * that it expects something not of the given type.
+ *
+ * @access is formed from the NFSD_MAY_* constants defined in
+ * include/linux/nfsd/nfsd.h.
  */
 __be32
 fh_verify(struct svc_rqst *rqstp, struct svc_fh *fhp, int type, int access)
@@ -466,6 +484,8 @@ fh_compose(struct svc_fh *fhp, struct svc_export *exp, struct dentry *dentry,
 				goto retry;
 			break;
 		}
+	} else if (exp->ex_flags & NFSEXP_FSID) {
+		fsid_type = FSID_NUM;
 	} else if (exp->ex_uuid) {
 		if (fhp->fh_maxsize >= 64) {
 			if (root_export)
@@ -478,9 +498,7 @@ fh_compose(struct svc_fh *fhp, struct svc_export *exp, struct dentry *dentry,
 			else
 				fsid_type = FSID_UUID4_INUM;
 		}
-	} else if (exp->ex_flags & NFSEXP_FSID)
-		fsid_type = FSID_NUM;
-	else if (!old_valid_dev(ex_dev))
+	} else if (!old_valid_dev(ex_dev))
 		/* for newer device numbers, we must use a newer fsid format */
 		fsid_type = FSID_ENCODE_DEV;
 	else
diff --git a/fs/nfsd/nfsproc.c b/fs/nfsd/nfsproc.c
index 5cffeca7acef..6f7f26351227 100644
--- a/fs/nfsd/nfsproc.c
+++ b/fs/nfsd/nfsproc.c
@@ -622,6 +622,7 @@ nfserrno (int errno)
 		{ nfserr_badname, -ESRCH },
 		{ nfserr_io, -ETXTBSY },
 		{ nfserr_notsupp, -EOPNOTSUPP },
+		{ nfserr_toosmall, -ETOOSMALL },
 	};
 	int	i;
 
diff --git a/fs/nfsd/vfs.c b/fs/nfsd/vfs.c
index 44aa92aba891..6e50aaa56ca2 100644
--- a/fs/nfsd/vfs.c
+++ b/fs/nfsd/vfs.c
@@ -744,16 +744,44 @@ nfsd_close(struct file *filp)
 	fput(filp);
 }
 
+/*
+ * Sync a file
+ * As this calls fsync (not fdatasync) there is no need for a write_inode
+ * after it.
+ */
+static inline int nfsd_dosync(struct file *filp, struct dentry *dp,
+			      const struct file_operations *fop)
+{
+	struct inode *inode = dp->d_inode;
+	int (*fsync) (struct file *, struct dentry *, int);
+	int err;
+
+	err = filemap_fdatawrite(inode->i_mapping);
+	if (err == 0 && fop && (fsync = fop->fsync))
+		err = fsync(filp, dp, 0);
+	if (err == 0)
+		err = filemap_fdatawait(inode->i_mapping);
+
+	return err;
+}
+
 static int
 nfsd_sync(struct file *filp)
 {
-	return vfs_fsync(filp, filp->f_path.dentry, 0);
+        int err;
+	struct inode *inode = filp->f_path.dentry->d_inode;
+	dprintk("nfsd: sync file %s\n", filp->f_path.dentry->d_name.name);
+	mutex_lock(&inode->i_mutex);
+	err=nfsd_dosync(filp, filp->f_path.dentry, filp->f_op);
+	mutex_unlock(&inode->i_mutex);
+
+	return err;
 }
 
 int
-nfsd_sync_dir(struct dentry *dentry)
+nfsd_sync_dir(struct dentry *dp)
 {
-	return vfs_fsync(NULL, dentry, 0);
+	return nfsd_dosync(NULL, dp, dp->d_inode->i_fop);
 }
 
 /*
diff --git a/fs/ocfs2/cluster/heartbeat.c b/fs/ocfs2/cluster/heartbeat.c
index 6ebaa58e2c03..04697ba7f73e 100644
--- a/fs/ocfs2/cluster/heartbeat.c
+++ b/fs/ocfs2/cluster/heartbeat.c
@@ -854,7 +854,7 @@ static int o2hb_thread(void *data)
 
 	while (!kthread_should_stop() && !reg->hr_unclean_stop) {
 		/* We track the time spent inside
-		 * o2hb_do_disk_heartbeat so that we avoid more then
+		 * o2hb_do_disk_heartbeat so that we avoid more than
 		 * hr_timeout_ms between disk writes. On busy systems
 		 * this should result in a heartbeat which is less
 		 * likely to time itself out. */
diff --git a/fs/partitions/check.c b/fs/partitions/check.c
index 6d5b213b8a9b..5198ada67398 100644
--- a/fs/partitions/check.c
+++ b/fs/partitions/check.c
@@ -384,9 +384,9 @@ struct hd_struct *add_partition(struct gendisk *disk, int partno,
 
 	dname = dev_name(ddev);
 	if (isdigit(dname[strlen(dname) - 1]))
-		snprintf(pdev->bus_id, BUS_ID_SIZE, "%sp%d", dname, partno);
+		dev_set_name(pdev, "%sp%d", dname, partno);
 	else
-		snprintf(pdev->bus_id, BUS_ID_SIZE, "%s%d", dname, partno);
+		dev_set_name(pdev, "%s%d", dname, partno);
 
 	device_initialize(pdev);
 	pdev->class = &block_class;
@@ -447,16 +447,11 @@ void register_disk(struct gendisk *disk)
 	struct block_device *bdev;
 	struct disk_part_iter piter;
 	struct hd_struct *part;
-	char *s;
 	int err;
 
 	ddev->parent = disk->driverfs_dev;
 
-	strlcpy(ddev->bus_id, disk->disk_name, BUS_ID_SIZE);
-	/* ewww... some of these buggers have / in the name... */
-	s = strchr(ddev->bus_id, '/');
-	if (s)
-		*s = '!';
+	dev_set_name(ddev, disk->disk_name);
 
 	/* delay uevents, until we scanned partition table */
 	ddev->uevent_suppress = 1;
diff --git a/fs/proc/base.c b/fs/proc/base.c
index 10fd5223d600..0c9de19a1633 100644
--- a/fs/proc/base.c
+++ b/fs/proc/base.c
@@ -65,6 +65,7 @@
 #include <linux/mm.h>
 #include <linux/rcupdate.h>
 #include <linux/kallsyms.h>
+#include <linux/stacktrace.h>
 #include <linux/resource.h>
 #include <linux/module.h>
 #include <linux/mount.h>
@@ -109,25 +110,22 @@ struct pid_entry {
 	.op   = OP,					\
 }
 
-#define DIR(NAME, MODE, OTYPE)							\
-	NOD(NAME, (S_IFDIR|(MODE)),						\
-		&proc_##OTYPE##_inode_operations, &proc_##OTYPE##_operations,	\
-		{} )
-#define LNK(NAME, OTYPE)					\
+#define DIR(NAME, MODE, iops, fops)	\
+	NOD(NAME, (S_IFDIR|(MODE)), &iops, &fops, {} )
+#define LNK(NAME, get_link)					\
 	NOD(NAME, (S_IFLNK|S_IRWXUGO),				\
 		&proc_pid_link_inode_operations, NULL,		\
-		{ .proc_get_link = &proc_##OTYPE##_link } )
-#define REG(NAME, MODE, OTYPE)				\
-	NOD(NAME, (S_IFREG|(MODE)), NULL,		\
-		&proc_##OTYPE##_operations, {})
-#define INF(NAME, MODE, OTYPE)				\
+		{ .proc_get_link = get_link } )
+#define REG(NAME, MODE, fops)				\
+	NOD(NAME, (S_IFREG|(MODE)), NULL, &fops, {})
+#define INF(NAME, MODE, read)				\
 	NOD(NAME, (S_IFREG|(MODE)), 			\
 		NULL, &proc_info_file_operations,	\
-		{ .proc_read = &proc_##OTYPE } )
-#define ONE(NAME, MODE, OTYPE)				\
+		{ .proc_read = read } )
+#define ONE(NAME, MODE, show)				\
 	NOD(NAME, (S_IFREG|(MODE)), 			\
 		NULL, &proc_single_file_operations,	\
-		{ .proc_show = &proc_##OTYPE } )
+		{ .proc_show = show } )
 
 /*
  * Count the number of hardlinks for the pid_entry table, excluding the .
@@ -308,9 +306,9 @@ static int proc_pid_auxv(struct task_struct *task, char *buffer)
 	struct mm_struct *mm = get_task_mm(task);
 	if (mm) {
 		unsigned int nwords = 0;
-		do
+		do {
 			nwords += 2;
-		while (mm->saved_auxv[nwords - 2] != 0); /* AT_NULL */
+		} while (mm->saved_auxv[nwords - 2] != 0); /* AT_NULL */
 		res = nwords * sizeof(mm->saved_auxv[0]);
 		if (res > PAGE_SIZE)
 			res = PAGE_SIZE;
@@ -340,6 +338,37 @@ static int proc_pid_wchan(struct task_struct *task, char *buffer)
 }
 #endif /* CONFIG_KALLSYMS */
 
+#ifdef CONFIG_STACKTRACE
+
+#define MAX_STACK_TRACE_DEPTH	64
+
+static int proc_pid_stack(struct seq_file *m, struct pid_namespace *ns,
+			  struct pid *pid, struct task_struct *task)
+{
+	struct stack_trace trace;
+	unsigned long *entries;
+	int i;
+
+	entries = kmalloc(MAX_STACK_TRACE_DEPTH * sizeof(*entries), GFP_KERNEL);
+	if (!entries)
+		return -ENOMEM;
+
+	trace.nr_entries	= 0;
+	trace.max_entries	= MAX_STACK_TRACE_DEPTH;
+	trace.entries		= entries;
+	trace.skip		= 0;
+	save_stack_trace_tsk(task, &trace);
+
+	for (i = 0; i < trace.nr_entries; i++) {
+		seq_printf(m, "[<%p>] %pS\n",
+			   (void *)entries[i], (void *)entries[i]);
+	}
+	kfree(entries);
+
+	return 0;
+}
+#endif
+
 #ifdef CONFIG_SCHEDSTATS
 /*
  * Provides /proc/PID/schedstat
@@ -1186,8 +1215,6 @@ static int sched_show(struct seq_file *m, void *v)
 	struct inode *inode = m->private;
 	struct task_struct *p;
 
-	WARN_ON(!inode);
-
 	p = get_proc_task(inode);
 	if (!p)
 		return -ESRCH;
@@ -1205,8 +1232,6 @@ sched_write(struct file *file, const char __user *buf,
 	struct inode *inode = file->f_path.dentry->d_inode;
 	struct task_struct *p;
 
-	WARN_ON(!inode);
-
 	p = get_proc_task(inode);
 	if (!p)
 		return -ESRCH;
@@ -1974,13 +1999,11 @@ static struct dentry *proc_pident_lookup(struct inode *dir,
 					 const struct pid_entry *ents,
 					 unsigned int nents)
 {
-	struct inode *inode;
 	struct dentry *error;
 	struct task_struct *task = get_proc_task(dir);
 	const struct pid_entry *p, *last;
 
 	error = ERR_PTR(-ENOENT);
-	inode = NULL;
 
 	if (!task)
 		goto out_no_task;
@@ -2136,12 +2159,12 @@ static const struct file_operations proc_pid_attr_operations = {
 };
 
 static const struct pid_entry attr_dir_stuff[] = {
-	REG("current",    S_IRUGO|S_IWUGO, pid_attr),
-	REG("prev",       S_IRUGO,	   pid_attr),
-	REG("exec",       S_IRUGO|S_IWUGO, pid_attr),
-	REG("fscreate",   S_IRUGO|S_IWUGO, pid_attr),
-	REG("keycreate",  S_IRUGO|S_IWUGO, pid_attr),
-	REG("sockcreate", S_IRUGO|S_IWUGO, pid_attr),
+	REG("current",    S_IRUGO|S_IWUGO, proc_pid_attr_operations),
+	REG("prev",       S_IRUGO,	   proc_pid_attr_operations),
+	REG("exec",       S_IRUGO|S_IWUGO, proc_pid_attr_operations),
+	REG("fscreate",   S_IRUGO|S_IWUGO, proc_pid_attr_operations),
+	REG("keycreate",  S_IRUGO|S_IWUGO, proc_pid_attr_operations),
+	REG("sockcreate", S_IRUGO|S_IWUGO, proc_pid_attr_operations),
 };
 
 static int proc_attr_dir_readdir(struct file * filp,
@@ -2461,74 +2484,77 @@ static const struct file_operations proc_task_operations;
 static const struct inode_operations proc_task_inode_operations;
 
 static const struct pid_entry tgid_base_stuff[] = {
-	DIR("task",       S_IRUGO|S_IXUGO, task),
-	DIR("fd",         S_IRUSR|S_IXUSR, fd),
-	DIR("fdinfo",     S_IRUSR|S_IXUSR, fdinfo),
+	DIR("task",       S_IRUGO|S_IXUGO, proc_task_inode_operations, proc_task_operations),
+	DIR("fd",         S_IRUSR|S_IXUSR, proc_fd_inode_operations, proc_fd_operations),
+	DIR("fdinfo",     S_IRUSR|S_IXUSR, proc_fdinfo_inode_operations, proc_fdinfo_operations),
 #ifdef CONFIG_NET
-	DIR("net",        S_IRUGO|S_IXUGO, net),
+	DIR("net",        S_IRUGO|S_IXUGO, proc_net_inode_operations, proc_net_operations),
 #endif
-	REG("environ",    S_IRUSR, environ),
-	INF("auxv",       S_IRUSR, pid_auxv),
-	ONE("status",     S_IRUGO, pid_status),
-	ONE("personality", S_IRUSR, pid_personality),
-	INF("limits",	  S_IRUSR, pid_limits),
+	REG("environ",    S_IRUSR, proc_environ_operations),
+	INF("auxv",       S_IRUSR, proc_pid_auxv),
+	ONE("status",     S_IRUGO, proc_pid_status),
+	ONE("personality", S_IRUSR, proc_pid_personality),
+	INF("limits",	  S_IRUSR, proc_pid_limits),
 #ifdef CONFIG_SCHED_DEBUG
-	REG("sched",      S_IRUGO|S_IWUSR, pid_sched),
+	REG("sched",      S_IRUGO|S_IWUSR, proc_pid_sched_operations),
 #endif
 #ifdef CONFIG_HAVE_ARCH_TRACEHOOK
-	INF("syscall",    S_IRUSR, pid_syscall),
+	INF("syscall",    S_IRUSR, proc_pid_syscall),
 #endif
-	INF("cmdline",    S_IRUGO, pid_cmdline),
-	ONE("stat",       S_IRUGO, tgid_stat),
-	ONE("statm",      S_IRUGO, pid_statm),
-	REG("maps",       S_IRUGO, maps),
+	INF("cmdline",    S_IRUGO, proc_pid_cmdline),
+	ONE("stat",       S_IRUGO, proc_tgid_stat),
+	ONE("statm",      S_IRUGO, proc_pid_statm),
+	REG("maps",       S_IRUGO, proc_maps_operations),
 #ifdef CONFIG_NUMA
-	REG("numa_maps",  S_IRUGO, numa_maps),
+	REG("numa_maps",  S_IRUGO, proc_numa_maps_operations),
 #endif
-	REG("mem",        S_IRUSR|S_IWUSR, mem),
-	LNK("cwd",        cwd),
-	LNK("root",       root),
-	LNK("exe",        exe),
-	REG("mounts",     S_IRUGO, mounts),
-	REG("mountinfo",  S_IRUGO, mountinfo),
-	REG("mountstats", S_IRUSR, mountstats),
+	REG("mem",        S_IRUSR|S_IWUSR, proc_mem_operations),
+	LNK("cwd",        proc_cwd_link),
+	LNK("root",       proc_root_link),
+	LNK("exe",        proc_exe_link),
+	REG("mounts",     S_IRUGO, proc_mounts_operations),
+	REG("mountinfo",  S_IRUGO, proc_mountinfo_operations),
+	REG("mountstats", S_IRUSR, proc_mountstats_operations),
 #ifdef CONFIG_PROC_PAGE_MONITOR
-	REG("clear_refs", S_IWUSR, clear_refs),
-	REG("smaps",      S_IRUGO, smaps),
-	REG("pagemap",    S_IRUSR, pagemap),
+	REG("clear_refs", S_IWUSR, proc_clear_refs_operations),
+	REG("smaps",      S_IRUGO, proc_smaps_operations),
+	REG("pagemap",    S_IRUSR, proc_pagemap_operations),
 #endif
 #ifdef CONFIG_SECURITY
-	DIR("attr",       S_IRUGO|S_IXUGO, attr_dir),
+	DIR("attr",       S_IRUGO|S_IXUGO, proc_attr_dir_inode_operations, proc_attr_dir_operations),
 #endif
 #ifdef CONFIG_KALLSYMS
-	INF("wchan",      S_IRUGO, pid_wchan),
+	INF("wchan",      S_IRUGO, proc_pid_wchan),
+#endif
+#ifdef CONFIG_STACKTRACE
+	ONE("stack",      S_IRUSR, proc_pid_stack),
 #endif
 #ifdef CONFIG_SCHEDSTATS
-	INF("schedstat",  S_IRUGO, pid_schedstat),
+	INF("schedstat",  S_IRUGO, proc_pid_schedstat),
 #endif
 #ifdef CONFIG_LATENCYTOP
-	REG("latency",  S_IRUGO, lstats),
+	REG("latency",  S_IRUGO, proc_lstats_operations),
 #endif
 #ifdef CONFIG_PROC_PID_CPUSET
-	REG("cpuset",     S_IRUGO, cpuset),
+	REG("cpuset",     S_IRUGO, proc_cpuset_operations),
 #endif
 #ifdef CONFIG_CGROUPS
-	REG("cgroup",  S_IRUGO, cgroup),
+	REG("cgroup",  S_IRUGO, proc_cgroup_operations),
 #endif
-	INF("oom_score",  S_IRUGO, oom_score),
-	REG("oom_adj",    S_IRUGO|S_IWUSR, oom_adjust),
+	INF("oom_score",  S_IRUGO, proc_oom_score),
+	REG("oom_adj",    S_IRUGO|S_IWUSR, proc_oom_adjust_operations),
 #ifdef CONFIG_AUDITSYSCALL
-	REG("loginuid",   S_IWUSR|S_IRUGO, loginuid),
-	REG("sessionid",  S_IRUGO, sessionid),
+	REG("loginuid",   S_IWUSR|S_IRUGO, proc_loginuid_operations),
+	REG("sessionid",  S_IRUGO, proc_sessionid_operations),
 #endif
 #ifdef CONFIG_FAULT_INJECTION
-	REG("make-it-fail", S_IRUGO|S_IWUSR, fault_inject),
+	REG("make-it-fail", S_IRUGO|S_IWUSR, proc_fault_inject_operations),
 #endif
 #if defined(USE_ELF_CORE_DUMP) && defined(CONFIG_ELF_CORE)
-	REG("coredump_filter", S_IRUGO|S_IWUSR, coredump_filter),
+	REG("coredump_filter", S_IRUGO|S_IWUSR, proc_coredump_filter_operations),
 #endif
 #ifdef CONFIG_TASK_IO_ACCOUNTING
-	INF("io",	S_IRUGO, tgid_io_accounting),
+	INF("io",	S_IRUGO, proc_tgid_io_accounting),
 #endif
 };
 
@@ -2801,66 +2827,69 @@ out_no_task:
  * Tasks
  */
 static const struct pid_entry tid_base_stuff[] = {
-	DIR("fd",        S_IRUSR|S_IXUSR, fd),
-	DIR("fdinfo",    S_IRUSR|S_IXUSR, fdinfo),
-	REG("environ",   S_IRUSR, environ),
-	INF("auxv",      S_IRUSR, pid_auxv),
-	ONE("status",    S_IRUGO, pid_status),
-	ONE("personality", S_IRUSR, pid_personality),
-	INF("limits",	 S_IRUSR, pid_limits),
+	DIR("fd",        S_IRUSR|S_IXUSR, proc_fd_inode_operations, proc_fd_operations),
+	DIR("fdinfo",    S_IRUSR|S_IXUSR, proc_fdinfo_inode_operations, proc_fd_operations),
+	REG("environ",   S_IRUSR, proc_environ_operations),
+	INF("auxv",      S_IRUSR, proc_pid_auxv),
+	ONE("status",    S_IRUGO, proc_pid_status),
+	ONE("personality", S_IRUSR, proc_pid_personality),
+	INF("limits",	 S_IRUSR, proc_pid_limits),
 #ifdef CONFIG_SCHED_DEBUG
-	REG("sched",     S_IRUGO|S_IWUSR, pid_sched),
+	REG("sched",     S_IRUGO|S_IWUSR, proc_pid_sched_operations),
 #endif
 #ifdef CONFIG_HAVE_ARCH_TRACEHOOK
-	INF("syscall",   S_IRUSR, pid_syscall),
+	INF("syscall",   S_IRUSR, proc_pid_syscall),
 #endif
-	INF("cmdline",   S_IRUGO, pid_cmdline),
-	ONE("stat",      S_IRUGO, tid_stat),
-	ONE("statm",     S_IRUGO, pid_statm),
-	REG("maps",      S_IRUGO, maps),
+	INF("cmdline",   S_IRUGO, proc_pid_cmdline),
+	ONE("stat",      S_IRUGO, proc_tid_stat),
+	ONE("statm",     S_IRUGO, proc_pid_statm),
+	REG("maps",      S_IRUGO, proc_maps_operations),
 #ifdef CONFIG_NUMA
-	REG("numa_maps", S_IRUGO, numa_maps),
+	REG("numa_maps", S_IRUGO, proc_numa_maps_operations),
 #endif
-	REG("mem",       S_IRUSR|S_IWUSR, mem),
-	LNK("cwd",       cwd),
-	LNK("root",      root),
-	LNK("exe",       exe),
-	REG("mounts",    S_IRUGO, mounts),
-	REG("mountinfo",  S_IRUGO, mountinfo),
+	REG("mem",       S_IRUSR|S_IWUSR, proc_mem_operations),
+	LNK("cwd",       proc_cwd_link),
+	LNK("root",      proc_root_link),
+	LNK("exe",       proc_exe_link),
+	REG("mounts",    S_IRUGO, proc_mounts_operations),
+	REG("mountinfo",  S_IRUGO, proc_mountinfo_operations),
 #ifdef CONFIG_PROC_PAGE_MONITOR
-	REG("clear_refs", S_IWUSR, clear_refs),
-	REG("smaps",     S_IRUGO, smaps),
-	REG("pagemap",    S_IRUSR, pagemap),
+	REG("clear_refs", S_IWUSR, proc_clear_refs_operations),
+	REG("smaps",     S_IRUGO, proc_smaps_operations),
+	REG("pagemap",    S_IRUSR, proc_pagemap_operations),
 #endif
 #ifdef CONFIG_SECURITY
-	DIR("attr",      S_IRUGO|S_IXUGO, attr_dir),
+	DIR("attr",      S_IRUGO|S_IXUGO, proc_attr_dir_inode_operations, proc_attr_dir_operations),
 #endif
 #ifdef CONFIG_KALLSYMS
-	INF("wchan",     S_IRUGO, pid_wchan),
+	INF("wchan",     S_IRUGO, proc_pid_wchan),
+#endif
+#ifdef CONFIG_STACKTRACE
+	ONE("stack",      S_IRUSR, proc_pid_stack),
 #endif
 #ifdef CONFIG_SCHEDSTATS
-	INF("schedstat", S_IRUGO, pid_schedstat),
+	INF("schedstat", S_IRUGO, proc_pid_schedstat),
 #endif
 #ifdef CONFIG_LATENCYTOP
-	REG("latency",  S_IRUGO, lstats),
+	REG("latency",  S_IRUGO, proc_lstats_operations),
 #endif
 #ifdef CONFIG_PROC_PID_CPUSET
-	REG("cpuset",    S_IRUGO, cpuset),
+	REG("cpuset",    S_IRUGO, proc_cpuset_operations),
 #endif
 #ifdef CONFIG_CGROUPS
-	REG("cgroup",  S_IRUGO, cgroup),
+	REG("cgroup",  S_IRUGO, proc_cgroup_operations),
 #endif
-	INF("oom_score", S_IRUGO, oom_score),
-	REG("oom_adj",   S_IRUGO|S_IWUSR, oom_adjust),
+	INF("oom_score", S_IRUGO, proc_oom_score),
+	REG("oom_adj",   S_IRUGO|S_IWUSR, proc_oom_adjust_operations),
 #ifdef CONFIG_AUDITSYSCALL
-	REG("loginuid",  S_IWUSR|S_IRUGO, loginuid),
-	REG("sessionid",  S_IRUSR, sessionid),
+	REG("loginuid",  S_IWUSR|S_IRUGO, proc_loginuid_operations),
+	REG("sessionid",  S_IRUSR, proc_sessionid_operations),
 #endif
 #ifdef CONFIG_FAULT_INJECTION
-	REG("make-it-fail", S_IRUGO|S_IWUSR, fault_inject),
+	REG("make-it-fail", S_IRUGO|S_IWUSR, proc_fault_inject_operations),
 #endif
 #ifdef CONFIG_TASK_IO_ACCOUNTING
-	INF("io",	S_IRUGO, tid_io_accounting),
+	INF("io",	S_IRUGO, proc_tid_io_accounting),
 #endif
 };
 
diff --git a/fs/proc/generic.c b/fs/proc/generic.c
index 60a359b35582..db7fa5cab988 100644
--- a/fs/proc/generic.c
+++ b/fs/proc/generic.c
@@ -14,7 +14,6 @@
 #include <linux/stat.h>
 #include <linux/module.h>
 #include <linux/mount.h>
-#include <linux/smp_lock.h>
 #include <linux/init.h>
 #include <linux/idr.h>
 #include <linux/namei.h>
@@ -379,7 +378,6 @@ struct dentry *proc_lookup_de(struct proc_dir_entry *de, struct inode *dir,
 	struct inode *inode = NULL;
 	int error = -ENOENT;
 
-	lock_kernel();
 	spin_lock(&proc_subdir_lock);
 	for (de = de->subdir; de ; de = de->next) {
 		if (de->namelen != dentry->d_name.len)
@@ -397,7 +395,6 @@ struct dentry *proc_lookup_de(struct proc_dir_entry *de, struct inode *dir,
 	}
 	spin_unlock(&proc_subdir_lock);
 out_unlock:
-	unlock_kernel();
 
 	if (inode) {
 		dentry->d_op = &proc_dentry_operations;
@@ -432,8 +429,6 @@ int proc_readdir_de(struct proc_dir_entry *de, struct file *filp, void *dirent,
 	struct inode *inode = filp->f_path.dentry->d_inode;
 	int ret = 0;
 
-	lock_kernel();
-
 	ino = inode->i_ino;
 	i = filp->f_pos;
 	switch (i) {
@@ -487,7 +482,7 @@ int proc_readdir_de(struct proc_dir_entry *de, struct file *filp, void *dirent,
 			spin_unlock(&proc_subdir_lock);
 	}
 	ret = 1;
-out:	unlock_kernel();
+out:
 	return ret;	
 }
 
@@ -504,6 +499,7 @@ int proc_readdir(struct file *filp, void *dirent, filldir_t filldir)
  * the /proc directory.
  */
 static const struct file_operations proc_dir_operations = {
+	.llseek			= generic_file_llseek,
 	.read			= generic_read_dir,
 	.readdir		= proc_readdir,
 };
diff --git a/fs/proc/inode.c b/fs/proc/inode.c
index 2543fd00c658..3e76bb9b3ad6 100644
--- a/fs/proc/inode.c
+++ b/fs/proc/inode.c
@@ -35,16 +35,13 @@ struct proc_dir_entry *de_get(struct proc_dir_entry *de)
  */
 void de_put(struct proc_dir_entry *de)
 {
-	lock_kernel();
 	if (!atomic_read(&de->count)) {
 		printk("de_put: entry %s already free!\n", de->name);
-		unlock_kernel();
 		return;
 	}
 
 	if (atomic_dec_and_test(&de->count))
 		free_proc_entry(de);
-	unlock_kernel();
 }
 
 /*
diff --git a/fs/proc/proc_net.c b/fs/proc/proc_net.c
index 7bc296f424ae..04d1270f1c38 100644
--- a/fs/proc/proc_net.c
+++ b/fs/proc/proc_net.c
@@ -18,7 +18,6 @@
 #include <linux/sched.h>
 #include <linux/module.h>
 #include <linux/bitops.h>
-#include <linux/smp_lock.h>
 #include <linux/mount.h>
 #include <linux/nsproxy.h>
 #include <net/net_namespace.h>
@@ -172,6 +171,7 @@ static int proc_tgid_net_readdir(struct file *filp, void *dirent,
 }
 
 const struct file_operations proc_net_operations = {
+	.llseek		= generic_file_llseek,
 	.read		= generic_read_dir,
 	.readdir	= proc_tgid_net_readdir,
 };
diff --git a/fs/proc/root.c b/fs/proc/root.c
index 7761602af9de..f6299a25594e 100644
--- a/fs/proc/root.c
+++ b/fs/proc/root.c
@@ -16,7 +16,6 @@
 #include <linux/sched.h>
 #include <linux/module.h>
 #include <linux/bitops.h>
-#include <linux/smp_lock.h>
 #include <linux/mount.h>
 #include <linux/pid_namespace.h>
 
@@ -162,17 +161,12 @@ static int proc_root_readdir(struct file * filp,
 	unsigned int nr = filp->f_pos;
 	int ret;
 
-	lock_kernel();
-
 	if (nr < FIRST_PROCESS_ENTRY) {
 		int error = proc_readdir(filp, dirent, filldir);
-		if (error <= 0) {
-			unlock_kernel();
+		if (error <= 0)
 			return error;
-		}
 		filp->f_pos = FIRST_PROCESS_ENTRY;
 	}
-	unlock_kernel();
 
 	ret = proc_pid_readdir(filp, dirent, filldir);
 	return ret;
diff --git a/fs/proc/task_mmu.c b/fs/proc/task_mmu.c
index 3a8bdd7f5756..94063840832a 100644
--- a/fs/proc/task_mmu.c
+++ b/fs/proc/task_mmu.c
@@ -396,7 +396,9 @@ static int show_smap(struct seq_file *m, void *v)
 		   "Private_Clean:  %8lu kB\n"
 		   "Private_Dirty:  %8lu kB\n"
 		   "Referenced:     %8lu kB\n"
-		   "Swap:           %8lu kB\n",
+		   "Swap:           %8lu kB\n"
+		   "KernelPageSize: %8lu kB\n"
+		   "MMUPageSize:    %8lu kB\n",
 		   (vma->vm_end - vma->vm_start) >> 10,
 		   mss.resident >> 10,
 		   (unsigned long)(mss.pss >> (10 + PSS_SHIFT)),
@@ -405,7 +407,9 @@ static int show_smap(struct seq_file *m, void *v)
 		   mss.private_clean >> 10,
 		   mss.private_dirty >> 10,
 		   mss.referenced >> 10,
-		   mss.swap >> 10);
+		   mss.swap >> 10,
+		   vma_kernel_pagesize(vma) >> 10,
+		   vma_mmu_pagesize(vma) >> 10);
 
 	if (m->count < m->size)  /* vma is copied successfully */
 		m->version = (vma != get_gate_vma(task)) ? vma->vm_start : 0;
diff --git a/fs/proc/task_nommu.c b/fs/proc/task_nommu.c
index 219bd79ea894..d4a8be32b902 100644
--- a/fs/proc/task_nommu.c
+++ b/fs/proc/task_nommu.c
@@ -9,7 +9,7 @@
 
 /*
  * Logic: we've got two memory sums for each process, "shared", and
- * "non-shared". Shared memory may get counted more then once, for
+ * "non-shared". Shared memory may get counted more than once, for
  * each process that owns it. Non-shared memory is counted
  * accurately.
  */
diff --git a/fs/select.c b/fs/select.c
index 87df51eadcf2..08b91beed806 100644
--- a/fs/select.c
+++ b/fs/select.c
@@ -109,11 +109,11 @@ static void __pollwait(struct file *filp, wait_queue_head_t *wait_address,
 void poll_initwait(struct poll_wqueues *pwq)
 {
 	init_poll_funcptr(&pwq->pt, __pollwait);
+	pwq->polling_task = current;
 	pwq->error = 0;
 	pwq->table = NULL;
 	pwq->inline_index = 0;
 }
-
 EXPORT_SYMBOL(poll_initwait);
 
 static void free_poll_entry(struct poll_table_entry *entry)
@@ -142,12 +142,10 @@ void poll_freewait(struct poll_wqueues *pwq)
 		free_page((unsigned long) old);
 	}
 }
-
 EXPORT_SYMBOL(poll_freewait);
 
-static struct poll_table_entry *poll_get_entry(poll_table *_p)
+static struct poll_table_entry *poll_get_entry(struct poll_wqueues *p)
 {
-	struct poll_wqueues *p = container_of(_p, struct poll_wqueues, pt);
 	struct poll_table_page *table = p->table;
 
 	if (p->inline_index < N_INLINE_POLL_ENTRIES)
@@ -159,7 +157,6 @@ static struct poll_table_entry *poll_get_entry(poll_table *_p)
 		new_table = (struct poll_table_page *) __get_free_page(GFP_KERNEL);
 		if (!new_table) {
 			p->error = -ENOMEM;
-			__set_current_state(TASK_RUNNING);
 			return NULL;
 		}
 		new_table->entry = new_table->entries;
@@ -171,20 +168,75 @@ static struct poll_table_entry *poll_get_entry(poll_table *_p)
 	return table->entry++;
 }
 
+static int pollwake(wait_queue_t *wait, unsigned mode, int sync, void *key)
+{
+	struct poll_wqueues *pwq = wait->private;
+	DECLARE_WAITQUEUE(dummy_wait, pwq->polling_task);
+
+	/*
+	 * Although this function is called under waitqueue lock, LOCK
+	 * doesn't imply write barrier and the users expect write
+	 * barrier semantics on wakeup functions.  The following
+	 * smp_wmb() is equivalent to smp_wmb() in try_to_wake_up()
+	 * and is paired with set_mb() in poll_schedule_timeout.
+	 */
+	smp_wmb();
+	pwq->triggered = 1;
+
+	/*
+	 * Perform the default wake up operation using a dummy
+	 * waitqueue.
+	 *
+	 * TODO: This is hacky but there currently is no interface to
+	 * pass in @sync.  @sync is scheduled to be removed and once
+	 * that happens, wake_up_process() can be used directly.
+	 */
+	return default_wake_function(&dummy_wait, mode, sync, key);
+}
+
 /* Add a new entry */
 static void __pollwait(struct file *filp, wait_queue_head_t *wait_address,
 				poll_table *p)
 {
-	struct poll_table_entry *entry = poll_get_entry(p);
+	struct poll_wqueues *pwq = container_of(p, struct poll_wqueues, pt);
+	struct poll_table_entry *entry = poll_get_entry(pwq);
 	if (!entry)
 		return;
 	get_file(filp);
 	entry->filp = filp;
 	entry->wait_address = wait_address;
-	init_waitqueue_entry(&entry->wait, current);
+	init_waitqueue_func_entry(&entry->wait, pollwake);
+	entry->wait.private = pwq;
 	add_wait_queue(wait_address, &entry->wait);
 }
 
+int poll_schedule_timeout(struct poll_wqueues *pwq, int state,
+			  ktime_t *expires, unsigned long slack)
+{
+	int rc = -EINTR;
+
+	set_current_state(state);
+	if (!pwq->triggered)
+		rc = schedule_hrtimeout_range(expires, slack, HRTIMER_MODE_ABS);
+	__set_current_state(TASK_RUNNING);
+
+	/*
+	 * Prepare for the next iteration.
+	 *
+	 * The following set_mb() serves two purposes.  First, it's
+	 * the counterpart rmb of the wmb in pollwake() such that data
+	 * written before wake up is always visible after wake up.
+	 * Second, the full barrier guarantees that triggered clearing
+	 * doesn't pass event check of the next iteration.  Note that
+	 * this problem doesn't exist for the first iteration as
+	 * add_wait_queue() has full barrier semantics.
+	 */
+	set_mb(pwq->triggered, 0);
+
+	return rc;
+}
+EXPORT_SYMBOL(poll_schedule_timeout);
+
 /**
  * poll_select_set_timeout - helper function to setup the timeout value
  * @to:		pointer to timespec variable for the final timeout
@@ -340,8 +392,6 @@ int do_select(int n, fd_set_bits *fds, struct timespec *end_time)
 	for (;;) {
 		unsigned long *rinp, *routp, *rexp, *inp, *outp, *exp;
 
-		set_current_state(TASK_INTERRUPTIBLE);
-
 		inp = fds->in; outp = fds->out; exp = fds->ex;
 		rinp = fds->res_in; routp = fds->res_out; rexp = fds->res_ex;
 
@@ -411,10 +461,10 @@ int do_select(int n, fd_set_bits *fds, struct timespec *end_time)
 			to = &expire;
 		}
 
-		if (!schedule_hrtimeout_range(to, slack, HRTIMER_MODE_ABS))
+		if (!poll_schedule_timeout(&table, TASK_INTERRUPTIBLE,
+					   to, slack))
 			timed_out = 1;
 	}
-	__set_current_state(TASK_RUNNING);
 
 	poll_freewait(&table);
 
@@ -666,7 +716,6 @@ static int do_poll(unsigned int nfds,  struct poll_list *list,
 	for (;;) {
 		struct poll_list *walk;
 
-		set_current_state(TASK_INTERRUPTIBLE);
 		for (walk = list; walk != NULL; walk = walk->next) {
 			struct pollfd * pfd, * pfd_end;
 
@@ -709,10 +758,9 @@ static int do_poll(unsigned int nfds,  struct poll_list *list,
 			to = &expire;
 		}
 
-		if (!schedule_hrtimeout_range(to, slack, HRTIMER_MODE_ABS))
+		if (!poll_schedule_timeout(wait, TASK_INTERRUPTIBLE, to, slack))
 			timed_out = 1;
 	}
-	__set_current_state(TASK_RUNNING);
 	return count;
 }
 
diff --git a/fs/super.c b/fs/super.c
index ddba069d7a99..cb20744ec789 100644
--- a/fs/super.c
+++ b/fs/super.c
@@ -38,6 +38,7 @@
 #include <linux/kobject.h>
 #include <linux/mutex.h>
 #include <linux/file.h>
+#include <linux/async.h>
 #include <asm/uaccess.h>
 #include "internal.h"
 
@@ -71,6 +72,7 @@ static struct super_block *alloc_super(struct file_system_type *type)
 		INIT_HLIST_HEAD(&s->s_anon);
 		INIT_LIST_HEAD(&s->s_inodes);
 		INIT_LIST_HEAD(&s->s_dentry_lru);
+		INIT_LIST_HEAD(&s->s_async_list);
 		init_rwsem(&s->s_umount);
 		mutex_init(&s->s_lock);
 		lockdep_set_class(&s->s_umount, &type->s_umount_key);
@@ -289,11 +291,18 @@ void generic_shutdown_super(struct super_block *sb)
 {
 	const struct super_operations *sop = sb->s_op;
 
+
 	if (sb->s_root) {
 		shrink_dcache_for_umount(sb);
 		fsync_super(sb);
 		lock_super(sb);
 		sb->s_flags &= ~MS_ACTIVE;
+
+		/*
+		 * wait for asynchronous fs operations to finish before going further
+		 */
+		async_synchronize_full_special(&sb->s_async_list);
+
 		/* bad name - it should be evict_inodes() */
 		invalidate_inodes(sb);
 		lock_kernel();
@@ -449,6 +458,7 @@ void sync_filesystems(int wait)
 		if (sb->s_flags & MS_RDONLY)
 			continue;
 		sb->s_need_sync_fs = 1;
+		async_synchronize_full_special(&sb->s_async_list);
 	}
 
 restart:
diff --git a/fs/sync.c b/fs/sync.c
index 0921d6d4b5e6..ac02b56548bc 100644
--- a/fs/sync.c
+++ b/fs/sync.c
@@ -295,7 +295,7 @@ int do_sync_mapping_range(struct address_space *mapping, loff_t offset,
 
 	if (flags & SYNC_FILE_RANGE_WRITE) {
 		ret = __filemap_fdatawrite_range(mapping, offset, endbyte,
-						WB_SYNC_NONE);
+						WB_SYNC_ALL);
 		if (ret < 0)
 			goto out;
 	}
diff --git a/fs/ubifs/Kconfig b/fs/ubifs/Kconfig
index 91ceeda7e5bf..e35b54d5059d 100644
--- a/fs/ubifs/Kconfig
+++ b/fs/ubifs/Kconfig
@@ -40,7 +40,7 @@ config UBIFS_FS_ZLIB
 	depends on UBIFS_FS
 	default y
 	help
-	  Zlib copresses better then LZO but it is slower. Say 'Y' if unsure.
+	  Zlib compresses better than LZO but it is slower. Say 'Y' if unsure.
 
 # Debugging-related stuff
 config UBIFS_FS_DEBUG
diff --git a/fs/ubifs/budget.c b/fs/ubifs/budget.c
index 0e5e54d82924..175f9c590b77 100644
--- a/fs/ubifs/budget.c
+++ b/fs/ubifs/budget.c
@@ -142,7 +142,7 @@ static long long get_liability(struct ubifs_info *c)
  *
  * This function is called when an operation cannot be budgeted because there
  * is supposedly no free space. But in most cases there is some free space:
- *   o budgeting is pessimistic, so it always budgets more then it is actually
+ *   o budgeting is pessimistic, so it always budgets more than it is actually
  *     needed, so shrinking the liability is one way to make free space - the
  *     cached data will take less space then it was budgeted for;
  *   o GC may turn some dark space into free space (budgeting treats dark space
@@ -606,7 +606,7 @@ void ubifs_release_budget(struct ubifs_info *c, struct ubifs_budget_req *req)
  * @c: UBIFS file-system description object
  *
  * This function converts budget which was allocated for a new page of data to
- * the budget of changing an existing page of data. The latter is smaller then
+ * the budget of changing an existing page of data. The latter is smaller than
  * the former, so this function only does simple re-calculation and does not
  * involve any write-back.
  */
diff --git a/fs/ubifs/gc.c b/fs/ubifs/gc.c
index 0bef6501d58a..9832f9abe28e 100644
--- a/fs/ubifs/gc.c
+++ b/fs/ubifs/gc.c
@@ -45,7 +45,7 @@
 #define SMALL_NODE_WM  UBIFS_MAX_DENT_NODE_SZ
 
 /*
- * GC may need to move more then one LEB to make progress. The below constants
+ * GC may need to move more than one LEB to make progress. The below constants
  * define "soft" and "hard" limits on the number of LEBs the garbage collector
  * may move.
  */
diff --git a/fs/ubifs/journal.c b/fs/ubifs/journal.c
index 10ae25b7d1db..9b7c54e0cd2a 100644
--- a/fs/ubifs/journal.c
+++ b/fs/ubifs/journal.c
@@ -191,7 +191,7 @@ again:
 	if (wbuf->lnum != -1 && avail >= len) {
 		/*
 		 * Someone else has switched the journal head and we have
-		 * enough space now. This happens when more then one process is
+		 * enough space now. This happens when more than one process is
 		 * trying to write to the same journal head at the same time.
 		 */
 		dbg_jnl("return LEB %d back, already have LEB %d:%d",
diff --git a/fs/ubifs/shrinker.c b/fs/ubifs/shrinker.c
index f248533841a2..e7bab52a1410 100644
--- a/fs/ubifs/shrinker.c
+++ b/fs/ubifs/shrinker.c
@@ -151,7 +151,7 @@ static int shrink_tnc(struct ubifs_info *c, int nr, int age, int *contention)
  * @contention: if any contention, this is set to %1
  *
  * This function walks the list of mounted UBIFS file-systems and frees clean
- * znodes which are older then @age, until at least @nr znodes are freed.
+ * znodes which are older than @age, until at least @nr znodes are freed.
  * Returns the number of freed znodes.
  */
 static int shrink_tnc_trees(int nr, int age, int *contention)
diff --git a/fs/ubifs/super.c b/fs/ubifs/super.c
index 0d7564b95f8e..89556ee72518 100644
--- a/fs/ubifs/super.c
+++ b/fs/ubifs/super.c
@@ -432,12 +432,19 @@ static int ubifs_sync_fs(struct super_block *sb, int wait)
 	int i, err;
 	struct ubifs_info *c = sb->s_fs_info;
 	struct writeback_control wbc = {
-		.sync_mode   = wait ? WB_SYNC_ALL : WB_SYNC_HOLD,
+		.sync_mode   = wait ? WB_SYNC_ALL : WB_SYNC_NONE,
 		.range_start = 0,
 		.range_end   = LLONG_MAX,
 		.nr_to_write = LONG_MAX,
 	};
 
+	/*
+	 * Note by akpm about WB_SYNC_NONE used above: zero @wait is just an
+	 * advisory thing to help the file system shove lots of data into the
+	 * queues. If some gets missed then it'll be picked up on the second
+	 * '->sync_fs()' call, with non-zero @wait.
+	 */
+
 	if (sb->s_flags & MS_RDONLY)
 		return 0;
 
diff --git a/fs/xfs/linux-2.6/xfs_super.c b/fs/xfs/linux-2.6/xfs_super.c
index 36f6cc703ef2..be846d606ae8 100644
--- a/fs/xfs/linux-2.6/xfs_super.c
+++ b/fs/xfs/linux-2.6/xfs_super.c
@@ -1348,7 +1348,7 @@ xfs_finish_flags(
 {
 	int			ronly = (mp->m_flags & XFS_MOUNT_RDONLY);
 
-	/* Fail a mount where the logbuf is smaller then the log stripe */
+	/* Fail a mount where the logbuf is smaller than the log stripe */
 	if (xfs_sb_version_haslogv2(&mp->m_sb)) {
 		if (mp->m_logbsize <= 0 &&
 		    mp->m_sb.sb_logsunit > XLOG_BIG_RECORD_BSIZE) {
diff --git a/include/acpi/acmacros.h b/include/acpi/acmacros.h
index a597207e2835..1954c9d1d012 100644
--- a/include/acpi/acmacros.h
+++ b/include/acpi/acmacros.h
@@ -333,8 +333,8 @@ struct acpi_integer_overlay {
 #define ACPI_INSERT_BITS(target, mask, source)          target = ((target & (~(mask))) | (source & mask))
 
 /*
- * An struct acpi_namespace_node can appear in some contexts
- * where a pointer to an union acpi_operand_object can also
+ * A struct acpi_namespace_node can appear in some contexts
+ * where a pointer to a union acpi_operand_object can also
  * appear. This macro is used to distinguish them.
  *
  * The "Descriptor" field is the first field in both structures.
diff --git a/include/acpi/actypes.h b/include/acpi/actypes.h
index 7220361790b3..8222e8de0d1c 100644
--- a/include/acpi/actypes.h
+++ b/include/acpi/actypes.h
@@ -467,7 +467,7 @@ typedef u32 acpi_object_type;
 
 /*
  * These are special object types that never appear in
- * a Namespace node, only in an union acpi_operand_object
+ * a Namespace node, only in a union acpi_operand_object
  */
 #define ACPI_TYPE_LOCAL_EXTRA           0x1C
 #define ACPI_TYPE_LOCAL_DATA            0x1D
diff --git a/include/asm-frv/Kbuild b/include/asm-frv/Kbuild
index 0f8956def738..1f44e7c76995 100644
--- a/include/asm-frv/Kbuild
+++ b/include/asm-frv/Kbuild
@@ -3,3 +3,4 @@ include include/asm-generic/Kbuild.asm
 header-y += registers.h
 
 unifdef-y += termios.h
+unifdef-y += swab.h
diff --git a/include/asm-frv/atomic.h b/include/asm-frv/atomic.h
index 46d696b331e7..296c35cfb207 100644
--- a/include/asm-frv/atomic.h
+++ b/include/asm-frv/atomic.h
@@ -35,10 +35,6 @@
 #define smp_mb__before_atomic_inc()	barrier()
 #define smp_mb__after_atomic_inc()	barrier()
 
-typedef struct {
-	int counter;
-} atomic_t;
-
 #define ATOMIC_INIT(i)		{ (i) }
 #define atomic_read(v)		((v)->counter)
 #define atomic_set(v, i)	(((v)->counter) = (i))
diff --git a/include/asm-frv/byteorder.h b/include/asm-frv/byteorder.h
index 411bec3cc1fc..1187e51ecd13 100644
--- a/include/asm-frv/byteorder.h
+++ b/include/asm-frv/byteorder.h
@@ -1,13 +1,7 @@
 #ifndef _ASM_BYTEORDER_H
 #define _ASM_BYTEORDER_H
 
-#include <asm/types.h>
-
-#if defined(__GNUC__) && !defined(__STRICT_ANSI__) || defined(__KERNEL__)
-#  define __BYTEORDER_HAS_U64__
-#  define __SWAB_64_THRU_32__
-#endif
-
+#include <asm/swab.h>
 #include <linux/byteorder/big_endian.h>
 
 #endif /* _ASM_BYTEORDER_H */
diff --git a/include/asm-frv/swab.h b/include/asm-frv/swab.h
new file mode 100644
index 000000000000..afb3396ba5ed
--- /dev/null
+++ b/include/asm-frv/swab.h
@@ -0,0 +1,10 @@
+#ifndef _ASM_SWAB_H
+#define _ASM_SWAB_H
+
+#include <asm/types.h>
+
+#if defined(__GNUC__) && !defined(__STRICT_ANSI__) || defined(__KERNEL__)
+#  define __SWAB_64_THRU_32__
+#endif
+
+#endif /* _ASM_SWAB_H */
diff --git a/include/asm-generic/bug.h b/include/asm-generic/bug.h
index 8af276361bf2..37b82cb96c89 100644
--- a/include/asm-generic/bug.h
+++ b/include/asm-generic/bug.h
@@ -28,6 +28,17 @@ struct bug_entry {
 #define BUGFLAG_WARNING	(1<<0)
 #endif	/* CONFIG_GENERIC_BUG */
 
+/*
+ * Don't use BUG() or BUG_ON() unless there's really no way out; one
+ * example might be detecting data structure corruption in the middle
+ * of an operation that can't be backed out of.  If the (sub)system
+ * can somehow continue operating, perhaps with reduced functionality,
+ * it's probably not BUG-worthy.
+ *
+ * If you're tempted to BUG(), think again:  is completely giving up
+ * really the *only* solution?  There are usually better options, where
+ * users don't need to reboot ASAP and can mostly shut down cleanly.
+ */
 #ifndef HAVE_ARCH_BUG
 #define BUG() do { \
 	printk("BUG: failure at %s:%d/%s()!\n", __FILE__, __LINE__, __func__); \
@@ -39,6 +50,12 @@ struct bug_entry {
 #define BUG_ON(condition) do { if (unlikely(condition)) BUG(); } while(0)
 #endif
 
+/*
+ * WARN(), WARN_ON(), WARN_ON_ONCE, and so on can be used to report
+ * significant issues that need prompt attention if they should ever
+ * appear at runtime.  Use the versions with printk format strings
+ * to provide better diagnostics.
+ */
 #ifndef __WARN
 #ifndef __ASSEMBLY__
 extern void warn_slowpath(const char *file, const int line,
diff --git a/include/asm-generic/local.h b/include/asm-generic/local.h
index 33d7d04e4119..dbd6150763e9 100644
--- a/include/asm-generic/local.h
+++ b/include/asm-generic/local.h
@@ -2,7 +2,6 @@
 #define _ASM_GENERIC_LOCAL_H
 
 #include <linux/percpu.h>
-#include <linux/hardirq.h>
 #include <asm/atomic.h>
 #include <asm/types.h>
 
diff --git a/include/asm-generic/memory_model.h b/include/asm-generic/memory_model.h
index 36fa286adad5..4c8d0afae711 100644
--- a/include/asm-generic/memory_model.h
+++ b/include/asm-generic/memory_model.h
@@ -69,15 +69,8 @@
 })
 #endif /* CONFIG_FLATMEM/DISCONTIGMEM/SPARSEMEM */
 
-#ifdef CONFIG_OUT_OF_LINE_PFN_TO_PAGE
-struct page;
-/* this is useful when inlined pfn_to_page is too big */
-extern struct page *pfn_to_page(unsigned long pfn);
-extern unsigned long page_to_pfn(struct page *page);
-#else
 #define page_to_pfn __page_to_pfn
 #define pfn_to_page __pfn_to_page
-#endif /* CONFIG_OUT_OF_LINE_PFN_TO_PAGE */
 
 #endif /* __ASSEMBLY__ */
 
diff --git a/include/asm-m32r/Kbuild b/include/asm-m32r/Kbuild
index c68e1680da01..27b108a86b39 100644
--- a/include/asm-m32r/Kbuild
+++ b/include/asm-m32r/Kbuild
@@ -1 +1,2 @@
 include include/asm-generic/Kbuild.asm
+unifdef-y += swab.h
diff --git a/include/asm-m32r/atomic.h b/include/asm-m32r/atomic.h
index 3a38ffe4a4f4..2eed30f84080 100644
--- a/include/asm-m32r/atomic.h
+++ b/include/asm-m32r/atomic.h
@@ -9,6 +9,7 @@
  *    Copyright (C) 2004  Hirokazu Takata <takata at linux-m32r.org>
  */
 
+#include <linux/types.h>
 #include <asm/assembler.h>
 #include <asm/system.h>
 
@@ -17,13 +18,6 @@
  * resource counting etc..
  */
 
-/*
- * Make sure gcc doesn't try to be clever and move things around
- * on us. We need to use _exactly_ the address the user gave us,
- * not some alias that contains the same information.
- */
-typedef struct { volatile int counter; } atomic_t;
-
 #define ATOMIC_INIT(i)	{ (i) }
 
 /**
diff --git a/include/asm-m32r/byteorder.h b/include/asm-m32r/byteorder.h
index 10b2c1d11614..61ff9cfd8451 100644
--- a/include/asm-m32r/byteorder.h
+++ b/include/asm-m32r/byteorder.h
@@ -1,12 +1,7 @@
 #ifndef _ASM_M32R_BYTEORDER_H
 #define _ASM_M32R_BYTEORDER_H
 
-#include <asm/types.h>
-
-#if !defined(__STRICT_ANSI__) || defined(__KERNEL__)
-#  define __BYTEORDER_HAS_U64__
-#  define __SWAB_64_THRU_32__
-#endif
+#include <asm/swab.h>
 
 #if defined(__LITTLE_ENDIAN__)
 #  include <linux/byteorder/little_endian.h>
diff --git a/include/asm-m32r/swab.h b/include/asm-m32r/swab.h
new file mode 100644
index 000000000000..97973e101825
--- /dev/null
+++ b/include/asm-m32r/swab.h
@@ -0,0 +1,10 @@
+#ifndef _ASM_M32R_SWAB_H
+#define _ASM_M32R_SWAB_H
+
+#include <asm/types.h>
+
+#if !defined(__STRICT_ANSI__) || defined(__KERNEL__)
+#  define __SWAB_64_THRU_32__
+#endif
+
+#endif /* _ASM_M32R_SWAB_H */
diff --git a/include/asm-m68k/Kbuild b/include/asm-m68k/Kbuild
index 1a922fad76f7..52fd96b4142a 100644
--- a/include/asm-m68k/Kbuild
+++ b/include/asm-m68k/Kbuild
@@ -1,2 +1,3 @@
 include include/asm-generic/Kbuild.asm
 header-y += cachectl.h
+unifdef-y += swab.h
diff --git a/include/asm-m68k/atomic.h b/include/asm-m68k/atomic.h
index 4915294fea63..eb0ab9d4ee77 100644
--- a/include/asm-m68k/atomic.h
+++ b/include/asm-m68k/atomic.h
@@ -1,7 +1,7 @@
 #ifndef __ARCH_M68K_ATOMIC__
 #define __ARCH_M68K_ATOMIC__
 
-
+#include <linux/types.h>
 #include <asm/system.h>
 
 /*
@@ -13,7 +13,6 @@
  * We do not have SMP m68k systems, so we don't have to deal with that.
  */
 
-typedef struct { int counter; } atomic_t;
 #define ATOMIC_INIT(i)	{ (i) }
 
 #define atomic_read(v)		((v)->counter)
diff --git a/include/asm-m68k/byteorder.h b/include/asm-m68k/byteorder.h
index b354acdafec8..300866523b86 100644
--- a/include/asm-m68k/byteorder.h
+++ b/include/asm-m68k/byteorder.h
@@ -1,19 +1,7 @@
 #ifndef _M68K_BYTEORDER_H
 #define _M68K_BYTEORDER_H
 
-#include <asm/types.h>
-#include <linux/compiler.h>
-
-#define __BIG_ENDIAN
-#define __SWAB_64_THRU_32__
-
-static inline __attribute_const__ __u32 __arch_swab32(__u32 val)
-{
-	__asm__("rolw #8,%0; swap %0; rolw #8,%0" : "=d" (val) : "0" (val));
-	return val;
-}
-#define __arch_swab32 __arch_swab32
-
-#include <linux/byteorder.h>
+#include <asm/swab.h>
+#include <linux/byteorder/big_endian.h>
 
 #endif /* _M68K_BYTEORDER_H */
diff --git a/include/asm-m68k/swab.h b/include/asm-m68k/swab.h
new file mode 100644
index 000000000000..7221e3066825
--- /dev/null
+++ b/include/asm-m68k/swab.h
@@ -0,0 +1,16 @@
+#ifndef _M68K_SWAB_H
+#define _M68K_SWAB_H
+
+#include <asm/types.h>
+#include <linux/compiler.h>
+
+#define __SWAB_64_THRU_32__
+
+static inline __attribute_const__ __u32 __arch_swab32(__u32 val)
+{
+	__asm__("rolw #8,%0; swap %0; rolw #8,%0" : "=d" (val) : "0" (val));
+	return val;
+}
+#define __arch_swab32 __arch_swab32
+
+#endif /* _M68K_SWAB_H */
diff --git a/include/asm-mn10300/Kbuild b/include/asm-mn10300/Kbuild
index c68e1680da01..27b108a86b39 100644
--- a/include/asm-mn10300/Kbuild
+++ b/include/asm-mn10300/Kbuild
@@ -1 +1,2 @@
 include include/asm-generic/Kbuild.asm
+unifdef-y += swab.h
diff --git a/include/asm-mn10300/atomic.h b/include/asm-mn10300/atomic.h
index 27c9690b9574..bc064825f9b1 100644
--- a/include/asm-mn10300/atomic.h
+++ b/include/asm-mn10300/atomic.h
@@ -20,15 +20,6 @@
  * resource counting etc..
  */
 
-/*
- * Make sure gcc doesn't try to be clever and move things around
- * on us. We need to use _exactly_ the address the user gave us,
- * not some alias that contains the same information.
- */
-typedef struct {
-	int	counter;
-} atomic_t;
-
 #define ATOMIC_INIT(i)	{ (i) }
 
 #ifdef __KERNEL__
diff --git a/include/asm-mn10300/byteorder.h b/include/asm-mn10300/byteorder.h
index 3c993cc625f8..45b18ded19e6 100644
--- a/include/asm-mn10300/byteorder.h
+++ b/include/asm-mn10300/byteorder.h
@@ -1,46 +1,7 @@
-/* MN10300 Byte-order primitive construction
- *
- * Copyright (C) 2007 Red Hat, Inc. All Rights Reserved.
- * Written by David Howells (dhowells@redhat.com)
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public Licence
- * as published by the Free Software Foundation; either version
- * 2 of the Licence, or (at your option) any later version.
- */
 #ifndef _ASM_BYTEORDER_H
 #define _ASM_BYTEORDER_H
 
-#include <asm/types.h>
-
-#ifdef __GNUC__
-
-static inline __attribute__((const))
-__u32 ___arch__swab32(__u32 x)
-{
-	__u32 ret;
-	asm("swap %1,%0" : "=r" (ret) : "r" (x));
-	return ret;
-}
-
-static inline __attribute__((const))
-__u16 ___arch__swab16(__u16 x)
-{
-	__u16 ret;
-	asm("swaph %1,%0" : "=r" (ret) : "r" (x));
-	return ret;
-}
-
-#define __arch__swab32(x) ___arch__swab32(x)
-#define __arch__swab16(x) ___arch__swab16(x)
-
-#if !defined(__STRICT_ANSI__) || defined(__KERNEL__)
-#  define __BYTEORDER_HAS_U64__
-#  define __SWAB_64_THRU_32__
-#endif
-
-#endif /* __GNUC__ */
-
+#include <asm/swab.h>
 #include <linux/byteorder/little_endian.h>
 
 #endif /* _ASM_BYTEORDER_H */
diff --git a/include/asm-mn10300/swab.h b/include/asm-mn10300/swab.h
new file mode 100644
index 000000000000..4504d1b4b477
--- /dev/null
+++ b/include/asm-mn10300/swab.h
@@ -0,0 +1,42 @@
+/* MN10300 Byte-order primitive construction
+ *
+ * Copyright (C) 2007 Red Hat, Inc. All Rights Reserved.
+ * Written by David Howells (dhowells@redhat.com)
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public Licence
+ * as published by the Free Software Foundation; either version
+ * 2 of the Licence, or (at your option) any later version.
+ */
+#ifndef _ASM_SWAB_H
+#define _ASM_SWAB_H
+
+#include <asm/types.h>
+
+#ifdef __GNUC__
+
+static inline __attribute__((const))
+__u32 __arch_swab32(__u32 x)
+{
+	__u32 ret;
+	asm("swap %1,%0" : "=r" (ret) : "r" (x));
+	return ret;
+}
+#define __arch_swab32 __arch_swab32
+
+static inline __attribute__((const))
+__u16 __arch_swab16(__u16 x)
+{
+	__u16 ret;
+	asm("swaph %1,%0" : "=r" (ret) : "r" (x));
+	return ret;
+}
+#define __arch_swab32 __arch_swab32
+
+#if !defined(__STRICT_ANSI__) || defined(__KERNEL__)
+#  define __SWAB_64_THRU_32__
+#endif
+
+#endif /* __GNUC__ */
+
+#endif /* _ASM_SWAB_H */
diff --git a/include/linux/Kbuild b/include/linux/Kbuild
index 39da666067b9..12e9a2957caf 100644
--- a/include/linux/Kbuild
+++ b/include/linux/Kbuild
@@ -179,7 +179,6 @@ unifdef-y += auto_fs.h
 unifdef-y += auxvec.h
 unifdef-y += binfmts.h
 unifdef-y += blktrace_api.h
-unifdef-y += byteorder.h
 unifdef-y += capability.h
 unifdef-y += capi.h
 unifdef-y += cciss_ioctl.h
@@ -372,3 +371,5 @@ unifdef-y += xattr.h
 unifdef-y += xfrm.h
 
 objhdr-y += version.h
+header-y += wimax.h
+header-y += wimax/
diff --git a/include/linux/async.h b/include/linux/async.h
new file mode 100644
index 000000000000..c4ecacd0b327
--- /dev/null
+++ b/include/linux/async.h
@@ -0,0 +1,25 @@
+/*
+ * async.h: Asynchronous function calls for boot performance
+ *
+ * (C) Copyright 2009 Intel Corporation
+ * Author: Arjan van de Ven <arjan@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
+ * as published by the Free Software Foundation; version 2
+ * of the License.
+ */
+
+#include <linux/types.h>
+#include <linux/list.h>
+
+typedef u64 async_cookie_t;
+typedef void (async_func_ptr) (void *data, async_cookie_t cookie);
+
+extern async_cookie_t async_schedule(async_func_ptr *ptr, void *data);
+extern async_cookie_t async_schedule_special(async_func_ptr *ptr, void *data, struct list_head *list);
+extern void async_synchronize_full(void);
+extern void async_synchronize_full_special(struct list_head *list);
+extern void async_synchronize_cookie(async_cookie_t cookie);
+extern void async_synchronize_cookie_special(async_cookie_t cookie, struct list_head *list);
+
diff --git a/arch/avr32/include/asm/atmel-mci.h b/include/linux/atmel-mci.h
index 59f3fadd0b68..2a2213eefd85 100644
--- a/arch/avr32/include/asm/atmel-mci.h
+++ b/include/linux/atmel-mci.h
@@ -1,5 +1,5 @@
-#ifndef __ASM_AVR32_ATMEL_MCI_H
-#define __ASM_AVR32_ATMEL_MCI_H
+#ifndef __LINUX_ATMEL_MCI_H
+#define __LINUX_ATMEL_MCI_H
 
 #define ATMEL_MCI_MAX_NR_SLOTS	2
 
@@ -36,4 +36,4 @@ struct mci_platform_data {
 	struct mci_slot_pdata	slot[ATMEL_MCI_MAX_NR_SLOTS];
 };
 
-#endif /* __ASM_AVR32_ATMEL_MCI_H */
+#endif /* __LINUX_ATMEL_MCI_H */
diff --git a/include/linux/auto_dev-ioctl.h b/include/linux/auto_dev-ioctl.h
index f4d05ccd731f..91a773993a5c 100644
--- a/include/linux/auto_dev-ioctl.h
+++ b/include/linux/auto_dev-ioctl.h
@@ -10,6 +10,7 @@
 #ifndef _LINUX_AUTO_DEV_IOCTL_H
 #define _LINUX_AUTO_DEV_IOCTL_H
 
+#include <linux/string.h>
 #include <linux/types.h>
 
 #define AUTOFS_DEVICE_NAME		"autofs"
@@ -25,6 +26,60 @@
  * An ioctl interface for autofs mount point control.
  */
 
+struct args_protover {
+	__u32	version;
+};
+
+struct args_protosubver {
+	__u32	sub_version;
+};
+
+struct args_openmount {
+	__u32	devid;
+};
+
+struct args_ready {
+	__u32	token;
+};
+
+struct args_fail {
+	__u32	token;
+	__s32	status;
+};
+
+struct args_setpipefd {
+	__s32	pipefd;
+};
+
+struct args_timeout {
+	__u64	timeout;
+};
+
+struct args_requester {
+	__u32	uid;
+	__u32	gid;
+};
+
+struct args_expire {
+	__u32	how;
+};
+
+struct args_askumount {
+	__u32	may_umount;
+};
+
+struct args_ismountpoint {
+	union {
+		struct args_in {
+			__u32	type;
+		} in;
+		struct args_out {
+			__u32	devid;
+			__u32	magic;
+		} out;
+	};
+};
+
 /*
  * All the ioctls use this structure.
  * When sending a path size must account for the total length
@@ -39,20 +94,32 @@ struct autofs_dev_ioctl {
 				 * including this struct */
 	__s32 ioctlfd;		/* automount command fd */
 
-	__u32 arg1;		/* Command parameters */
-	__u32 arg2;
+	/* Command parameters */
+
+	union {
+		struct args_protover		protover;
+		struct args_protosubver		protosubver;
+		struct args_openmount		openmount;
+		struct args_ready		ready;
+		struct args_fail		fail;
+		struct args_setpipefd		setpipefd;
+		struct args_timeout		timeout;
+		struct args_requester		requester;
+		struct args_expire		expire;
+		struct args_askumount		askumount;
+		struct args_ismountpoint	ismountpoint;
+	};
 
 	char path[0];
 };
 
 static inline void init_autofs_dev_ioctl(struct autofs_dev_ioctl *in)
 {
+	memset(in, 0, sizeof(struct autofs_dev_ioctl));
 	in->ver_major = AUTOFS_DEV_IOCTL_VERSION_MAJOR;
 	in->ver_minor = AUTOFS_DEV_IOCTL_VERSION_MINOR;
 	in->size = sizeof(struct autofs_dev_ioctl);
 	in->ioctlfd = -1;
-	in->arg1 = 0;
-	in->arg2 = 0;
 	return;
 }
 
diff --git a/include/linux/auto_fs4.h b/include/linux/auto_fs4.h
index 2253716d4b92..55fa478bd639 100644
--- a/include/linux/auto_fs4.h
+++ b/include/linux/auto_fs4.h
@@ -29,10 +29,64 @@
 #define AUTOFS_EXP_IMMEDIATE		1
 #define AUTOFS_EXP_LEAVES		2
 
-#define AUTOFS_TYPE_ANY			0x0000
-#define AUTOFS_TYPE_INDIRECT		0x0001
-#define AUTOFS_TYPE_DIRECT		0x0002
-#define AUTOFS_TYPE_OFFSET		0x0004
+#define AUTOFS_TYPE_ANY			0U
+#define AUTOFS_TYPE_INDIRECT		1U
+#define AUTOFS_TYPE_DIRECT		2U
+#define AUTOFS_TYPE_OFFSET		4U
+
+static inline void set_autofs_type_indirect(unsigned int *type)
+{
+	*type = AUTOFS_TYPE_INDIRECT;
+	return;
+}
+
+static inline unsigned int autofs_type_indirect(unsigned int type)
+{
+	return (type == AUTOFS_TYPE_INDIRECT);
+}
+
+static inline void set_autofs_type_direct(unsigned int *type)
+{
+	*type = AUTOFS_TYPE_DIRECT;
+	return;
+}
+
+static inline unsigned int autofs_type_direct(unsigned int type)
+{
+	return (type == AUTOFS_TYPE_DIRECT);
+}
+
+static inline void set_autofs_type_offset(unsigned int *type)
+{
+	*type = AUTOFS_TYPE_OFFSET;
+	return;
+}
+
+static inline unsigned int autofs_type_offset(unsigned int type)
+{
+	return (type == AUTOFS_TYPE_OFFSET);
+}
+
+static inline unsigned int autofs_type_trigger(unsigned int type)
+{
+	return (type == AUTOFS_TYPE_DIRECT || type == AUTOFS_TYPE_OFFSET);
+}
+
+/*
+ * This isn't really a type as we use it to say "no type set" to
+ * indicate we want to search for "any" mount in the
+ * autofs_dev_ioctl_ismountpoint() device ioctl function.
+ */
+static inline void set_autofs_type_any(unsigned int *type)
+{
+	*type = AUTOFS_TYPE_ANY;
+	return;
+}
+
+static inline unsigned int autofs_type_any(unsigned int type)
+{
+	return (type == AUTOFS_TYPE_ANY);
+}
 
 /* Daemon notification packet types */
 enum autofs_notify {
diff --git a/include/linux/binfmts.h b/include/linux/binfmts.h
index 6cbfbe297180..77b4a9e46004 100644
--- a/include/linux/binfmts.h
+++ b/include/linux/binfmts.h
@@ -18,6 +18,7 @@ struct pt_regs;
 #define BINPRM_BUF_SIZE 128
 
 #ifdef __KERNEL__
+#include <linux/list.h>
 
 #define CORENAME_MAX_SIZE 128
 
@@ -106,7 +107,7 @@ extern int setup_arg_pages(struct linux_binprm * bprm,
 extern int bprm_mm_init(struct linux_binprm *bprm);
 extern int copy_strings_kernel(int argc,char ** argv,struct linux_binprm *bprm);
 extern void install_exec_creds(struct linux_binprm *bprm);
-extern int do_coredump(long signr, int exit_code, struct pt_regs * regs);
+extern void do_coredump(long signr, int exit_code, struct pt_regs *regs);
 extern int set_binfmt(struct linux_binfmt *new);
 extern void free_bprm(struct linux_binprm *);
 
diff --git a/include/linux/byteorder.h b/include/linux/byteorder.h
deleted file mode 100644
index 29f002d73d98..000000000000
--- a/include/linux/byteorder.h
+++ /dev/null
@@ -1,372 +0,0 @@
-#ifndef _LINUX_BYTEORDER_H
-#define _LINUX_BYTEORDER_H
-
-#include <linux/types.h>
-#include <linux/swab.h>
-
-#if defined(__LITTLE_ENDIAN) && defined(__BIG_ENDIAN)
-# error Fix asm/byteorder.h to define one endianness
-#endif
-
-#if !defined(__LITTLE_ENDIAN) && !defined(__BIG_ENDIAN)
-# error Fix asm/byteorder.h to define arch endianness
-#endif
-
-#ifdef __LITTLE_ENDIAN
-# undef __LITTLE_ENDIAN
-# define __LITTLE_ENDIAN 1234
-#endif
-
-#ifdef __BIG_ENDIAN
-# undef __BIG_ENDIAN
-# define __BIG_ENDIAN 4321
-#endif
-
-#if defined(__LITTLE_ENDIAN) && !defined(__LITTLE_ENDIAN_BITFIELD)
-# define __LITTLE_ENDIAN_BITFIELD
-#endif
-
-#if defined(__BIG_ENDIAN) && !defined(__BIG_ENDIAN_BITFIELD)
-# define __BIG_ENDIAN_BITFIELD
-#endif
-
-#ifdef __LITTLE_ENDIAN
-# define __le16_to_cpu(x) ((__force __u16)(__le16)(x))
-# define __le32_to_cpu(x) ((__force __u32)(__le32)(x))
-# define __le64_to_cpu(x) ((__force __u64)(__le64)(x))
-# define __cpu_to_le16(x) ((__force __le16)(__u16)(x))
-# define __cpu_to_le32(x) ((__force __le32)(__u32)(x))
-# define __cpu_to_le64(x) ((__force __le64)(__u64)(x))
-
-# define __be16_to_cpu(x) __swab16((__force __u16)(__be16)(x))
-# define __be32_to_cpu(x) __swab32((__force __u32)(__be32)(x))
-# define __be64_to_cpu(x) __swab64((__force __u64)(__be64)(x))
-# define __cpu_to_be16(x) ((__force __be16)__swab16(x))
-# define __cpu_to_be32(x) ((__force __be32)__swab32(x))
-# define __cpu_to_be64(x) ((__force __be64)__swab64(x))
-#endif
-
-#ifdef __BIG_ENDIAN
-# define __be16_to_cpu(x) ((__force __u16)(__be16)(x))
-# define __be32_to_cpu(x) ((__force __u32)(__be32)(x))
-# define __be64_to_cpu(x) ((__force __u64)(__be64)(x))
-# define __cpu_to_be16(x) ((__force __be16)(__u16)(x))
-# define __cpu_to_be32(x) ((__force __be32)(__u32)(x))
-# define __cpu_to_be64(x) ((__force __be64)(__u64)(x))
-
-# define __le16_to_cpu(x) __swab16((__force __u16)(__le16)(x))
-# define __le32_to_cpu(x) __swab32((__force __u32)(__le32)(x))
-# define __le64_to_cpu(x) __swab64((__force __u64)(__le64)(x))
-# define __cpu_to_le16(x) ((__force __le16)__swab16(x))
-# define __cpu_to_le32(x) ((__force __le32)__swab32(x))
-# define __cpu_to_le64(x) ((__force __le64)__swab64(x))
-#endif
-
-/*
- * These helpers could be phased out over time as the base version
- * handles constant folding.
- */
-#define __constant_htonl(x) __cpu_to_be32(x)
-#define __constant_ntohl(x) __be32_to_cpu(x)
-#define __constant_htons(x) __cpu_to_be16(x)
-#define __constant_ntohs(x) __be16_to_cpu(x)
-
-#define __constant_le16_to_cpu(x) __le16_to_cpu(x)
-#define __constant_le32_to_cpu(x) __le32_to_cpu(x)
-#define __constant_le64_to_cpu(x) __le64_to_cpu(x)
-#define __constant_be16_to_cpu(x) __be16_to_cpu(x)
-#define __constant_be32_to_cpu(x) __be32_to_cpu(x)
-#define __constant_be64_to_cpu(x) __be64_to_cpu(x)
-
-#define __constant_cpu_to_le16(x) __cpu_to_le16(x)
-#define __constant_cpu_to_le32(x) __cpu_to_le32(x)
-#define __constant_cpu_to_le64(x) __cpu_to_le64(x)
-#define __constant_cpu_to_be16(x) __cpu_to_be16(x)
-#define __constant_cpu_to_be32(x) __cpu_to_be32(x)
-#define __constant_cpu_to_be64(x) __cpu_to_be64(x)
-
-static inline void __le16_to_cpus(__u16 *p)
-{
-#ifdef __BIG_ENDIAN
-	__swab16s(p);
-#endif
-}
-
-static inline void __cpu_to_le16s(__u16 *p)
-{
-#ifdef __BIG_ENDIAN
-	__swab16s(p);
-#endif
-}
-
-static inline void __le32_to_cpus(__u32 *p)
-{
-#ifdef __BIG_ENDIAN
-	__swab32s(p);
-#endif
-}
-
-static inline void __cpu_to_le32s(__u32 *p)
-{
-#ifdef __BIG_ENDIAN
-	__swab32s(p);
-#endif
-}
-
-static inline void __le64_to_cpus(__u64 *p)
-{
-#ifdef __BIG_ENDIAN
-	__swab64s(p);
-#endif
-}
-
-static inline void __cpu_to_le64s(__u64 *p)
-{
-#ifdef __BIG_ENDIAN
-	__swab64s(p);
-#endif
-}
-
-static inline void __be16_to_cpus(__u16 *p)
-{
-#ifdef __LITTLE_ENDIAN
-	__swab16s(p);
-#endif
-}
-
-static inline void __cpu_to_be16s(__u16 *p)
-{
-#ifdef __LITTLE_ENDIAN
-	__swab16s(p);
-#endif
-}
-
-static inline void __be32_to_cpus(__u32 *p)
-{
-#ifdef __LITTLE_ENDIAN
-	__swab32s(p);
-#endif
-}
-
-static inline void __cpu_to_be32s(__u32 *p)
-{
-#ifdef __LITTLE_ENDIAN
-	__swab32s(p);
-#endif
-}
-
-static inline void __be64_to_cpus(__u64 *p)
-{
-#ifdef __LITTLE_ENDIAN
-	__swab64s(p);
-#endif
-}
-
-static inline void __cpu_to_be64s(__u64 *p)
-{
-#ifdef __LITTLE_ENDIAN
-	__swab64s(p);
-#endif
-}
-
-static inline __u16 __le16_to_cpup(const __le16 *p)
-{
-#ifdef __LITTLE_ENDIAN
-	return (__force __u16)*p;
-#else
-	return __swab16p((__force __u16 *)p);
-#endif
-}
-
-static inline __u32 __le32_to_cpup(const __le32 *p)
-{
-#ifdef __LITTLE_ENDIAN
-	return (__force __u32)*p;
-#else
-	return __swab32p((__force __u32 *)p);
-#endif
-}
-
-static inline __u64 __le64_to_cpup(const __le64 *p)
-{
-#ifdef __LITTLE_ENDIAN
-	return (__force __u64)*p;
-#else
-	return __swab64p((__force __u64 *)p);
-#endif
-}
-
-static inline __le16 __cpu_to_le16p(const __u16 *p)
-{
-#ifdef __LITTLE_ENDIAN
-	return (__force __le16)*p;
-#else
-	return (__force __le16)__swab16p(p);
-#endif
-}
-
-static inline __le32 __cpu_to_le32p(const __u32 *p)
-{
-#ifdef __LITTLE_ENDIAN
-	return (__force __le32)*p;
-#else
-	return (__force __le32)__swab32p(p);
-#endif
-}
-
-static inline __le64 __cpu_to_le64p(const __u64 *p)
-{
-#ifdef __LITTLE_ENDIAN
-	return (__force __le64)*p;
-#else
-	return (__force __le64)__swab64p(p);
-#endif
-}
-
-static inline __u16 __be16_to_cpup(const __be16 *p)
-{
-#ifdef __BIG_ENDIAN
-	return (__force __u16)*p;
-#else
-	return __swab16p((__force __u16 *)p);
-#endif
-}
-
-static inline __u32 __be32_to_cpup(const __be32 *p)
-{
-#ifdef __BIG_ENDIAN
-	return (__force __u32)*p;
-#else
-	return __swab32p((__force __u32 *)p);
-#endif
-}
-
-static inline __u64 __be64_to_cpup(const __be64 *p)
-{
-#ifdef __BIG_ENDIAN
-	return (__force __u64)*p;
-#else
-	return __swab64p((__force __u64 *)p);
-#endif
-}
-
-static inline __be16 __cpu_to_be16p(const __u16 *p)
-{
-#ifdef __BIG_ENDIAN
-	return (__force __be16)*p;
-#else
-	return (__force __be16)__swab16p(p);
-#endif
-}
-
-static inline __be32 __cpu_to_be32p(const __u32 *p)
-{
-#ifdef __BIG_ENDIAN
-	return (__force __be32)*p;
-#else
-	return (__force __be32)__swab32p(p);
-#endif
-}
-
-static inline __be64 __cpu_to_be64p(const __u64 *p)
-{
-#ifdef __BIG_ENDIAN
-	return (__force __be64)*p;
-#else
-	return (__force __be64)__swab64p(p);
-#endif
-}
-
-#ifdef __KERNEL__
-
-# define le16_to_cpu __le16_to_cpu
-# define le32_to_cpu __le32_to_cpu
-# define le64_to_cpu __le64_to_cpu
-# define be16_to_cpu __be16_to_cpu
-# define be32_to_cpu __be32_to_cpu
-# define be64_to_cpu __be64_to_cpu
-# define cpu_to_le16 __cpu_to_le16
-# define cpu_to_le32 __cpu_to_le32
-# define cpu_to_le64 __cpu_to_le64
-# define cpu_to_be16 __cpu_to_be16
-# define cpu_to_be32 __cpu_to_be32
-# define cpu_to_be64 __cpu_to_be64
-
-# define le16_to_cpup __le16_to_cpup
-# define le32_to_cpup __le32_to_cpup
-# define le64_to_cpup __le64_to_cpup
-# define be16_to_cpup __be16_to_cpup
-# define be32_to_cpup __be32_to_cpup
-# define be64_to_cpup __be64_to_cpup
-# define cpu_to_le16p __cpu_to_le16p
-# define cpu_to_le32p __cpu_to_le32p
-# define cpu_to_le64p __cpu_to_le64p
-# define cpu_to_be16p __cpu_to_be16p
-# define cpu_to_be32p __cpu_to_be32p
-# define cpu_to_be64p __cpu_to_be64p
-
-# define le16_to_cpus __le16_to_cpus
-# define le32_to_cpus __le32_to_cpus
-# define le64_to_cpus __le64_to_cpus
-# define be16_to_cpus __be16_to_cpus
-# define be32_to_cpus __be32_to_cpus
-# define be64_to_cpus __be64_to_cpus
-# define cpu_to_le16s __cpu_to_le16s
-# define cpu_to_le32s __cpu_to_le32s
-# define cpu_to_le64s __cpu_to_le64s
-# define cpu_to_be16s __cpu_to_be16s
-# define cpu_to_be32s __cpu_to_be32s
-# define cpu_to_be64s __cpu_to_be64s
-
-/*
- * They have to be macros in order to do the constant folding
- * correctly - if the argument passed into a inline function
- * it is no longer constant according to gcc..
- */
-# undef ntohl
-# undef ntohs
-# undef htonl
-# undef htons
-
-# define ___htonl(x) __cpu_to_be32(x)
-# define ___htons(x) __cpu_to_be16(x)
-# define ___ntohl(x) __be32_to_cpu(x)
-# define ___ntohs(x) __be16_to_cpu(x)
-
-# define htonl(x) ___htonl(x)
-# define ntohl(x) ___ntohl(x)
-# define htons(x) ___htons(x)
-# define ntohs(x) ___ntohs(x)
-
-static inline void le16_add_cpu(__le16 *var, u16 val)
-{
-	*var = cpu_to_le16(le16_to_cpup(var) + val);
-}
-
-static inline void le32_add_cpu(__le32 *var, u32 val)
-{
-	*var = cpu_to_le32(le32_to_cpup(var) + val);
-}
-
-static inline void le64_add_cpu(__le64 *var, u64 val)
-{
-	*var = cpu_to_le64(le64_to_cpup(var) + val);
-}
-
-static inline void be16_add_cpu(__be16 *var, u16 val)
-{
-	*var = cpu_to_be16(be16_to_cpup(var) + val);
-}
-
-static inline void be32_add_cpu(__be32 *var, u32 val)
-{
-	*var = cpu_to_be32(be32_to_cpup(var) + val);
-}
-
-static inline void be64_add_cpu(__be64 *var, u64 val)
-{
-	*var = cpu_to_be64(be64_to_cpup(var) + val);
-}
-
-#endif /* __KERNEL__ */
-#endif /* _LINUX_BYTEORDER_H */
diff --git a/include/linux/byteorder/Kbuild b/include/linux/byteorder/Kbuild
index fbaa7f9cee32..38437225b092 100644
--- a/include/linux/byteorder/Kbuild
+++ b/include/linux/byteorder/Kbuild
@@ -1,4 +1,2 @@
 unifdef-y += big_endian.h
 unifdef-y += little_endian.h
-unifdef-y += swab.h
-unifdef-y += swabb.h
diff --git a/include/linux/byteorder/big_endian.h b/include/linux/byteorder/big_endian.h
index 1cba3f3efe5f..3c80fd7e8b56 100644
--- a/include/linux/byteorder/big_endian.h
+++ b/include/linux/byteorder/big_endian.h
@@ -9,8 +9,7 @@
 #endif
 
 #include <linux/types.h>
-#include <linux/byteorder/swab.h>
-#include <linux/byteorder/swabb.h>
+#include <linux/swab.h>
 
 #define __constant_htonl(x) ((__force __be32)(__u32)(x))
 #define __constant_ntohl(x) ((__force __u32)(__be32)(x))
diff --git a/include/linux/byteorder/little_endian.h b/include/linux/byteorder/little_endian.h
index cedc1b5a289c..83195fb82962 100644
--- a/include/linux/byteorder/little_endian.h
+++ b/include/linux/byteorder/little_endian.h
@@ -9,8 +9,7 @@
 #endif
 
 #include <linux/types.h>
-#include <linux/byteorder/swab.h>
-#include <linux/byteorder/swabb.h>
+#include <linux/swab.h>
 
 #define __constant_htonl(x) ((__force __be32)___constant_swab32((x)))
 #define __constant_ntohl(x) ___constant_swab32((__force __be32)(x))
diff --git a/include/linux/byteorder/swab.h b/include/linux/byteorder/swab.h
deleted file mode 100644
index 142134ff1645..000000000000
--- a/include/linux/byteorder/swab.h
+++ /dev/null
@@ -1,222 +0,0 @@
-#ifndef _LINUX_BYTEORDER_SWAB_H
-#define _LINUX_BYTEORDER_SWAB_H
-
-/*
- * linux/byteorder/swab.h
- * Byte-swapping, independently from CPU endianness
- *	swabXX[ps]?(foo)
- *
- * Francois-Rene Rideau <fare@tunes.org> 19971205
- *    separated swab functions from cpu_to_XX,
- *    to clean up support for bizarre-endian architectures.
- *
- * Trent Piepho <xyzzy@speakeasy.org> 2007114
- *    make constant-folding work, provide C versions that
- *    gcc can optimize better, explain different versions
- *
- * See asm-i386/byteorder.h and suches for examples of how to provide
- * architecture-dependent optimized versions
- *
- */
-
-#include <linux/compiler.h>
-
-/* Functions/macros defined, there are a lot:
- *
- * ___swabXX
- *    Generic C versions of the swab functions.
- *
- * ___constant_swabXX
- *    C versions that gcc can fold into a compile-time constant when
- *    the argument is a compile-time constant.
- *
- * __arch__swabXX[sp]?
- *    Architecture optimized versions of all the swab functions
- *    (including the s and p versions).  These can be defined in
- *    asm-arch/byteorder.h.  Any which are not, are defined here.
- *    __arch__swabXXs() is defined in terms of __arch__swabXXp(), which
- *    is defined in terms of __arch__swabXX(), which is in turn defined
- *    in terms of ___swabXX(x).
- *    These must be macros.  They may be unsafe for arguments with
- *    side-effects.
- *
- * __fswabXX
- *    Inline function versions of the __arch__ macros.  These _are_ safe
- *    if the arguments have side-effects.  Note there are no s and p
- *    versions of these.
- *
- * __swabXX[sb]
- *    There are the ones you should actually use.  The __swabXX versions
- *    will be a constant given a constant argument and use the arch
- *    specific code (if any) for non-constant arguments.  The s and p
- *    versions always use the arch specific code (constant folding
- *    doesn't apply).  They are safe to use with arguments with
- *    side-effects.
- *
- * swabXX[sb]
- *    Nicknames for __swabXX[sb] to use in the kernel.
- */
-
-/* casts are necessary for constants, because we never know how for sure
- * how U/UL/ULL map to __u16, __u32, __u64. At least not in a portable way.
- */
-
-static __inline__ __attribute_const__ __u16 ___swab16(__u16 x)
-{
-	return x<<8 | x>>8;
-}
-static __inline__ __attribute_const__ __u32 ___swab32(__u32 x)
-{
-	return x<<24 | x>>24 |
-		(x & (__u32)0x0000ff00UL)<<8 |
-		(x & (__u32)0x00ff0000UL)>>8;
-}
-static __inline__ __attribute_const__ __u64 ___swab64(__u64 x)
-{
-	return x<<56 | x>>56 |
-		(x & (__u64)0x000000000000ff00ULL)<<40 |
-		(x & (__u64)0x0000000000ff0000ULL)<<24 |
-		(x & (__u64)0x00000000ff000000ULL)<< 8 |
-	        (x & (__u64)0x000000ff00000000ULL)>> 8 |
-		(x & (__u64)0x0000ff0000000000ULL)>>24 |
-		(x & (__u64)0x00ff000000000000ULL)>>40;
-}
-
-#define ___constant_swab16(x) \
-	((__u16)( \
-		(((__u16)(x) & (__u16)0x00ffU) << 8) | \
-		(((__u16)(x) & (__u16)0xff00U) >> 8) ))
-#define ___constant_swab32(x) \
-	((__u32)( \
-		(((__u32)(x) & (__u32)0x000000ffUL) << 24) | \
-		(((__u32)(x) & (__u32)0x0000ff00UL) <<  8) | \
-		(((__u32)(x) & (__u32)0x00ff0000UL) >>  8) | \
-		(((__u32)(x) & (__u32)0xff000000UL) >> 24) ))
-#define ___constant_swab64(x) \
-	((__u64)( \
-		(__u64)(((__u64)(x) & (__u64)0x00000000000000ffULL) << 56) | \
-		(__u64)(((__u64)(x) & (__u64)0x000000000000ff00ULL) << 40) | \
-		(__u64)(((__u64)(x) & (__u64)0x0000000000ff0000ULL) << 24) | \
-		(__u64)(((__u64)(x) & (__u64)0x00000000ff000000ULL) <<  8) | \
-	        (__u64)(((__u64)(x) & (__u64)0x000000ff00000000ULL) >>  8) | \
-		(__u64)(((__u64)(x) & (__u64)0x0000ff0000000000ULL) >> 24) | \
-		(__u64)(((__u64)(x) & (__u64)0x00ff000000000000ULL) >> 40) | \
-		(__u64)(((__u64)(x) & (__u64)0xff00000000000000ULL) >> 56) ))
-
-/*
- * provide defaults when no architecture-specific optimization is detected
- */
-#ifndef __arch__swab16
-#  define __arch__swab16(x) ___swab16(x)
-#endif
-#ifndef __arch__swab32
-#  define __arch__swab32(x) ___swab32(x)
-#endif
-#ifndef __arch__swab64
-#  define __arch__swab64(x) ___swab64(x)
-#endif
-
-#ifndef __arch__swab16p
-#  define __arch__swab16p(x) __arch__swab16(*(x))
-#endif
-#ifndef __arch__swab32p
-#  define __arch__swab32p(x) __arch__swab32(*(x))
-#endif
-#ifndef __arch__swab64p
-#  define __arch__swab64p(x) __arch__swab64(*(x))
-#endif
-
-#ifndef __arch__swab16s
-#  define __arch__swab16s(x) ((void)(*(x) = __arch__swab16p(x)))
-#endif
-#ifndef __arch__swab32s
-#  define __arch__swab32s(x) ((void)(*(x) = __arch__swab32p(x)))
-#endif
-#ifndef __arch__swab64s
-#  define __arch__swab64s(x) ((void)(*(x) = __arch__swab64p(x)))
-#endif
-
-
-/*
- * Allow constant folding
- */
-#if defined(__GNUC__) && defined(__OPTIMIZE__)
-#  define __swab16(x) \
-(__builtin_constant_p((__u16)(x)) ? \
- ___constant_swab16((x)) : \
- __fswab16((x)))
-#  define __swab32(x) \
-(__builtin_constant_p((__u32)(x)) ? \
- ___constant_swab32((x)) : \
- __fswab32((x)))
-#  define __swab64(x) \
-(__builtin_constant_p((__u64)(x)) ? \
- ___constant_swab64((x)) : \
- __fswab64((x)))
-#else
-#  define __swab16(x) __fswab16(x)
-#  define __swab32(x) __fswab32(x)
-#  define __swab64(x) __fswab64(x)
-#endif /* OPTIMIZE */
-
-
-static __inline__ __attribute_const__ __u16 __fswab16(__u16 x)
-{
-	return __arch__swab16(x);
-}
-static __inline__ __u16 __swab16p(const __u16 *x)
-{
-	return __arch__swab16p(x);
-}
-static __inline__ void __swab16s(__u16 *addr)
-{
-	__arch__swab16s(addr);
-}
-
-static __inline__ __attribute_const__ __u32 __fswab32(__u32 x)
-{
-	return __arch__swab32(x);
-}
-static __inline__ __u32 __swab32p(const __u32 *x)
-{
-	return __arch__swab32p(x);
-}
-static __inline__ void __swab32s(__u32 *addr)
-{
-	__arch__swab32s(addr);
-}
-
-#ifdef __BYTEORDER_HAS_U64__
-static __inline__ __attribute_const__ __u64 __fswab64(__u64 x)
-{
-#  ifdef __SWAB_64_THRU_32__
-	__u32 h = x >> 32;
-        __u32 l = x & ((1ULL<<32)-1);
-        return (((__u64)__swab32(l)) << 32) | ((__u64)(__swab32(h)));
-#  else
-	return __arch__swab64(x);
-#  endif
-}
-static __inline__ __u64 __swab64p(const __u64 *x)
-{
-	return __arch__swab64p(x);
-}
-static __inline__ void __swab64s(__u64 *addr)
-{
-	__arch__swab64s(addr);
-}
-#endif /* __BYTEORDER_HAS_U64__ */
-
-#if defined(__KERNEL__)
-#define swab16 __swab16
-#define swab32 __swab32
-#define swab64 __swab64
-#define swab16p __swab16p
-#define swab32p __swab32p
-#define swab64p __swab64p
-#define swab16s __swab16s
-#define swab32s __swab32s
-#define swab64s __swab64s
-#endif
-
-#endif /* _LINUX_BYTEORDER_SWAB_H */
diff --git a/include/linux/byteorder/swabb.h b/include/linux/byteorder/swabb.h
deleted file mode 100644
index 8c780c7d779e..000000000000
--- a/include/linux/byteorder/swabb.h
+++ /dev/null
@@ -1,135 +0,0 @@
-#ifndef _LINUX_BYTEORDER_SWABB_H
-#define _LINUX_BYTEORDER_SWABB_H
-
-/*
- * linux/byteorder/swabb.h
- * SWAp Bytes Bizarrely
- *	swaHHXX[ps]?(foo)
- *
- * Support for obNUXIous pdp-endian and other bizarre architectures.
- * Will Linux ever run on such ancient beasts? if not, this file
- * will be but a programming pearl. Still, it's a reminder that we
- * shouldn't be making too many assumptions when trying to be portable.
- *
- */
-
-/*
- * Meaning of the names I chose (vaxlinux people feel free to correct them):
- * swahw32	swap 16-bit half-words in a 32-bit word
- * swahb32	swap 8-bit halves of each 16-bit half-word in a 32-bit word
- *
- * No 64-bit support yet. I don't know NUXI conventions for long longs.
- * I guarantee it will be a mess when it's there, though :->
- * It will be even worse if there are conflicting 64-bit conventions.
- * Hopefully, no one ever used 64-bit objects on NUXI machines.
- *
- */
-
-#include <linux/types.h>
-
-#define ___swahw32(x) \
-({ \
-	__u32 __x = (x); \
-	((__u32)( \
-		(((__u32)(__x) & (__u32)0x0000ffffUL) << 16) | \
-		(((__u32)(__x) & (__u32)0xffff0000UL) >> 16) )); \
-})
-#define ___swahb32(x) \
-({ \
-	__u32 __x = (x); \
-	((__u32)( \
-		(((__u32)(__x) & (__u32)0x00ff00ffUL) << 8) | \
-		(((__u32)(__x) & (__u32)0xff00ff00UL) >> 8) )); \
-})
-
-#define ___constant_swahw32(x) \
-	((__u32)( \
-		(((__u32)(x) & (__u32)0x0000ffffUL) << 16) | \
-		(((__u32)(x) & (__u32)0xffff0000UL) >> 16) ))
-#define ___constant_swahb32(x) \
-	((__u32)( \
-		(((__u32)(x) & (__u32)0x00ff00ffUL) << 8) | \
-		(((__u32)(x) & (__u32)0xff00ff00UL) >> 8) ))
-
-/*
- * provide defaults when no architecture-specific optimization is detected
- */
-#ifndef __arch__swahw32
-#  define __arch__swahw32(x) ___swahw32(x)
-#endif
-#ifndef __arch__swahb32
-#  define __arch__swahb32(x) ___swahb32(x)
-#endif
-
-#ifndef __arch__swahw32p
-#  define __arch__swahw32p(x) __swahw32(*(x))
-#endif
-#ifndef __arch__swahb32p
-#  define __arch__swahb32p(x) __swahb32(*(x))
-#endif
-
-#ifndef __arch__swahw32s
-#  define __arch__swahw32s(x) do { *(x) = __swahw32p((x)); } while (0)
-#endif
-#ifndef __arch__swahb32s
-#  define __arch__swahb32s(x) do { *(x) = __swahb32p((x)); } while (0)
-#endif
-
-
-/*
- * Allow constant folding
- */
-#define __swahw32(x) \
-(__builtin_constant_p((__u32)(x)) ? \
- ___swahw32((x)) : \
- __fswahw32((x)))
-#define __swahb32(x) \
-(__builtin_constant_p((__u32)(x)) ? \
- ___swahb32((x)) : \
- __fswahb32((x)))
-
-
-static inline __u32 __fswahw32(__u32 x)
-{
-	return __arch__swahw32(x);
-}
-
-static inline __u32 __swahw32p(__u32 *x)
-{
-	return __arch__swahw32p(x);
-}
-
-static inline void __swahw32s(__u32 *addr)
-{
-	__arch__swahw32s(addr);
-}
-
-static inline __u32 __fswahb32(__u32 x)
-{
-	return __arch__swahb32(x);
-}
-
-static inline __u32 __swahb32p(__u32 *x)
-{
-	return __arch__swahb32p(x);
-}
-
-static inline void __swahb32s(__u32 *addr)
-{
-	__arch__swahb32s(addr);
-}
-
-#ifdef __BYTEORDER_HAS_U64__
-/*
- * Not supported yet
- */
-#endif /* __BYTEORDER_HAS_U64__ */
-
-#define swahw32 __swahw32
-#define swahb32 __swahb32
-#define swahw32p __swahw32p
-#define swahb32p __swahb32p
-#define swahw32s __swahw32s
-#define swahb32s __swahb32s
-
-#endif /* _LINUX_BYTEORDER_SWABB_H */
diff --git a/include/linux/capability.h b/include/linux/capability.h
index e22f48c2a46f..02bdb768d43b 100644
--- a/include/linux/capability.h
+++ b/include/linux/capability.h
@@ -529,8 +529,21 @@ extern const kernel_cap_t __cap_init_eff_set;
  *
  * Note that this does not set PF_SUPERPRIV on the task.
  */
-#define has_capability(t, cap) (security_capable((t), (cap)) == 0)
-#define has_capability_noaudit(t, cap) (security_capable_noaudit((t), (cap)) == 0)
+#define has_capability(t, cap) (security_real_capable((t), (cap)) == 0)
+
+/**
+ * has_capability_noaudit - Determine if a task has a superior capability available (unaudited)
+ * @t: The task in question
+ * @cap: The capability to be tested for
+ *
+ * Return true if the specified task has the given superior capability
+ * currently in effect, false if not, but don't write an audit message for the
+ * check.
+ *
+ * Note that this does not set PF_SUPERPRIV on the task.
+ */
+#define has_capability_noaudit(t, cap) \
+	(security_real_capable_noaudit((t), (cap)) == 0)
 
 extern int capable(int cap);
 
diff --git a/include/linux/cgroup.h b/include/linux/cgroup.h
index 1164963c3a85..08b78c09b09a 100644
--- a/include/linux/cgroup.h
+++ b/include/linux/cgroup.h
@@ -329,13 +329,7 @@ struct cgroup_subsys {
 			struct cgroup *cgrp);
 	void (*post_clone)(struct cgroup_subsys *ss, struct cgroup *cgrp);
 	void (*bind)(struct cgroup_subsys *ss, struct cgroup *root);
-	/*
-	 * This routine is called with the task_lock of mm->owner held
-	 */
-	void (*mm_owner_changed)(struct cgroup_subsys *ss,
-					struct cgroup *old,
-					struct cgroup *new,
-					struct task_struct *p);
+
 	int subsys_id;
 	int active;
 	int disabled;
@@ -400,9 +394,6 @@ void cgroup_iter_end(struct cgroup *cgrp, struct cgroup_iter *it);
 int cgroup_scan_tasks(struct cgroup_scanner *scan);
 int cgroup_attach_task(struct cgroup *, struct task_struct *);
 
-void cgroup_mm_owner_callbacks(struct task_struct *old,
-			       struct task_struct *new);
-
 #else /* !CONFIG_CGROUPS */
 
 static inline int cgroup_init_early(void) { return 0; }
@@ -420,9 +411,6 @@ static inline int cgroupstats_build(struct cgroupstats *stats,
 	return -EINVAL;
 }
 
-static inline void cgroup_mm_owner_callbacks(struct task_struct *old,
-					     struct task_struct *new) {}
-
 #endif /* !CONFIG_CGROUPS */
 
 #endif /* _LINUX_CGROUP_H */
diff --git a/include/linux/cpuset.h b/include/linux/cpuset.h
index 8e540d32c9fe..51ea2bdea0f9 100644
--- a/include/linux/cpuset.h
+++ b/include/linux/cpuset.h
@@ -78,6 +78,8 @@ extern int current_cpuset_is_being_rebound(void);
 
 extern void rebuild_sched_domains(void);
 
+extern void cpuset_print_task_mems_allowed(struct task_struct *p);
+
 #else /* !CONFIG_CPUSETS */
 
 static inline int cpuset_init_early(void) { return 0; }
@@ -159,6 +161,10 @@ static inline void rebuild_sched_domains(void)
 	partition_sched_domains(1, NULL, NULL);
 }
 
+static inline void cpuset_print_task_mems_allowed(struct task_struct *p)
+{
+}
+
 #endif /* !CONFIG_CPUSETS */
 
 #endif /* _LINUX_CPUSET_H */
diff --git a/include/linux/debugfs.h b/include/linux/debugfs.h
index e1a6c046cea3..23936b16426b 100644
--- a/include/linux/debugfs.h
+++ b/include/linux/debugfs.h
@@ -63,6 +63,8 @@ struct dentry *debugfs_create_x16(const char *name, mode_t mode,
 				  struct dentry *parent, u16 *value);
 struct dentry *debugfs_create_x32(const char *name, mode_t mode,
 				  struct dentry *parent, u32 *value);
+struct dentry *debugfs_create_size_t(const char *name, mode_t mode,
+				     struct dentry *parent, size_t *value);
 struct dentry *debugfs_create_bool(const char *name, mode_t mode,
 				  struct dentry *parent, u32 *value);
 
diff --git a/include/linux/device.h b/include/linux/device.h
index 1a3686d15f98..7d9da4b4993f 100644
--- a/include/linux/device.h
+++ b/include/linux/device.h
@@ -28,6 +28,7 @@
 #define BUS_ID_SIZE		20
 
 struct device;
+struct device_private;
 struct device_driver;
 struct driver_private;
 struct class;
@@ -65,7 +66,7 @@ struct bus_type {
 	int (*resume_early)(struct device *dev);
 	int (*resume)(struct device *dev);
 
-	struct pm_ext_ops *pm;
+	struct dev_pm_ops *pm;
 
 	struct bus_type_private *p;
 };
@@ -133,7 +134,7 @@ struct device_driver {
 	int (*resume) (struct device *dev);
 	struct attribute_group **groups;
 
-	struct pm_ops *pm;
+	struct dev_pm_ops *pm;
 
 	struct driver_private *p;
 };
@@ -198,7 +199,7 @@ struct class {
 	int (*suspend)(struct device *dev, pm_message_t state);
 	int (*resume)(struct device *dev);
 
-	struct pm_ops *pm;
+	struct dev_pm_ops *pm;
 	struct class_private *p;
 };
 
@@ -291,7 +292,7 @@ struct device_type {
 	int (*suspend)(struct device *dev, pm_message_t state);
 	int (*resume)(struct device *dev);
 
-	struct pm_ops *pm;
+	struct dev_pm_ops *pm;
 };
 
 /* interface for exporting device attributes */
@@ -365,17 +366,15 @@ struct device_dma_parameters {
 };
 
 struct device {
-	struct klist		klist_children;
-	struct klist_node	knode_parent;	/* node in sibling list */
-	struct klist_node	knode_driver;
-	struct klist_node	knode_bus;
 	struct device		*parent;
 
+	struct device_private	*p;
+
 	struct kobject kobj;
 	char	bus_id[BUS_ID_SIZE];	/* position on parent bus */
+	unsigned		uevent_suppress:1;
 	const char		*init_name; /* initial name of the device */
 	struct device_type	*type;
-	unsigned		uevent_suppress:1;
 
 	struct semaphore	sem;	/* semaphore to synchronize calls to
 					 * its driver.
@@ -408,12 +407,13 @@ struct device {
 	/* arch specific additions */
 	struct dev_archdata	archdata;
 
+	dev_t			devt;	/* dev_t, creates the sysfs "dev" */
+
 	spinlock_t		devres_lock;
 	struct list_head	devres_head;
 
 	struct klist_node	knode_class;
 	struct class		*class;
-	dev_t			devt;	/* dev_t, creates the sysfs "dev" */
 	struct attribute_group	**groups;	/* optional groups */
 
 	void	(*release)(struct device *dev);
@@ -483,6 +483,17 @@ extern int device_rename(struct device *dev, char *new_name);
 extern int device_move(struct device *dev, struct device *new_parent);
 
 /*
+ * Root device objects for grouping under /sys/devices
+ */
+extern struct device *__root_device_register(const char *name,
+					     struct module *owner);
+static inline struct device *root_device_register(const char *name)
+{
+	return __root_device_register(name, THIS_MODULE);
+}
+extern void root_device_unregister(struct device *root);
+
+/*
  * Manual binding of a device to driver. See drivers/base/bus.c
  * for information on use.
  */
@@ -553,13 +564,13 @@ extern const char *dev_driver_string(const struct device *dev);
 #define dev_info(dev, format, arg...)		\
 	dev_printk(KERN_INFO , dev , format , ## arg)
 
-#if defined(CONFIG_DYNAMIC_PRINTK_DEBUG)
+#if defined(DEBUG)
+#define dev_dbg(dev, format, arg...)		\
+	dev_printk(KERN_DEBUG , dev , format , ## arg)
+#elif defined(CONFIG_DYNAMIC_PRINTK_DEBUG)
 #define dev_dbg(dev, format, ...) do { \
 	dynamic_dev_dbg(dev, format, ##__VA_ARGS__); \
 	} while (0)
-#elif defined(DEBUG)
-#define dev_dbg(dev, format, arg...)		\
-	dev_printk(KERN_DEBUG , dev , format , ## arg)
 #else
 #define dev_dbg(dev, format, arg...)		\
 	({ if (0) dev_printk(KERN_DEBUG, dev, format, ##arg); 0; })
diff --git a/include/linux/fs.h b/include/linux/fs.h
index fb59673c60b1..e38a64d71eff 100644
--- a/include/linux/fs.h
+++ b/include/linux/fs.h
@@ -1133,7 +1133,6 @@ struct super_block {
 	struct rw_semaphore	s_umount;
 	struct mutex		s_lock;
 	int			s_count;
-	int			s_syncing;
 	int			s_need_sync_fs;
 	atomic_t		s_active;
 #ifdef CONFIG_SECURITY
@@ -1185,6 +1184,11 @@ struct super_block {
 	 * generic_show_options()
 	 */
 	char *s_options;
+
+	/*
+	 * storage for asynchronous operations
+	 */
+	struct list_head s_async_list;
 };
 
 extern struct timespec current_fs_time(struct super_block *sb);
diff --git a/include/linux/fuse.h b/include/linux/fuse.h
index 350fe9767bbc..162e5defe683 100644
--- a/include/linux/fuse.h
+++ b/include/linux/fuse.h
@@ -1,6 +1,6 @@
 /*
     FUSE: Filesystem in Userspace
-    Copyright (C) 2001-2006  Miklos Szeredi <miklos@szeredi.hu>
+    Copyright (C) 2001-2008  Miklos Szeredi <miklos@szeredi.hu>
 
     This program can be distributed under the terms of the GNU GPL.
     See the file COPYING.
@@ -20,29 +20,27 @@
  *
  * 7.10
  *  - add nonseekable open flag
+ *
+ * 7.11
+ *  - add IOCTL message
+ *  - add unsolicited notification support
+ *  - add POLL message and NOTIFY_POLL notification
  */
 
 #ifndef _LINUX_FUSE_H
 #define _LINUX_FUSE_H
 
-#include <asm/types.h>
-#include <linux/major.h>
+#include <linux/types.h>
 
 /** Version number of this interface */
 #define FUSE_KERNEL_VERSION 7
 
 /** Minor version number of this interface */
-#define FUSE_KERNEL_MINOR_VERSION 10
+#define FUSE_KERNEL_MINOR_VERSION 11
 
 /** The node ID of the root inode */
 #define FUSE_ROOT_ID 1
 
-/** The major number of the fuse character device */
-#define FUSE_MAJOR MISC_MAJOR
-
-/** The minor number of the fuse character device */
-#define FUSE_MINOR 229
-
 /* Make sure all structures are padded to 64bit boundary, so 32bit
    userspace works under 64bit kernels */
 
@@ -151,6 +149,28 @@ struct fuse_file_lock {
  */
 #define FUSE_READ_LOCKOWNER	(1 << 1)
 
+/**
+ * Ioctl flags
+ *
+ * FUSE_IOCTL_COMPAT: 32bit compat ioctl on 64bit machine
+ * FUSE_IOCTL_UNRESTRICTED: not restricted to well-formed ioctls, retry allowed
+ * FUSE_IOCTL_RETRY: retry with new iovecs
+ *
+ * FUSE_IOCTL_MAX_IOV: maximum of in_iovecs + out_iovecs
+ */
+#define FUSE_IOCTL_COMPAT	(1 << 0)
+#define FUSE_IOCTL_UNRESTRICTED	(1 << 1)
+#define FUSE_IOCTL_RETRY	(1 << 2)
+
+#define FUSE_IOCTL_MAX_IOV	256
+
+/**
+ * Poll flags
+ *
+ * FUSE_POLL_SCHEDULE_NOTIFY: request poll notify
+ */
+#define FUSE_POLL_SCHEDULE_NOTIFY (1 << 0)
+
 enum fuse_opcode {
 	FUSE_LOOKUP	   = 1,
 	FUSE_FORGET	   = 2,  /* no reply */
@@ -188,6 +208,13 @@ enum fuse_opcode {
 	FUSE_INTERRUPT     = 36,
 	FUSE_BMAP          = 37,
 	FUSE_DESTROY       = 38,
+	FUSE_IOCTL         = 39,
+	FUSE_POLL          = 40,
+};
+
+enum fuse_notify_code {
+	FUSE_NOTIFY_POLL   = 1,
+	FUSE_NOTIFY_CODE_MAX,
 };
 
 /* The read buffer is required to be at least 8k, but may be much larger */
@@ -388,6 +415,38 @@ struct fuse_bmap_out {
 	__u64	block;
 };
 
+struct fuse_ioctl_in {
+	__u64	fh;
+	__u32	flags;
+	__u32	cmd;
+	__u64	arg;
+	__u32	in_size;
+	__u32	out_size;
+};
+
+struct fuse_ioctl_out {
+	__s32	result;
+	__u32	flags;
+	__u32	in_iovs;
+	__u32	out_iovs;
+};
+
+struct fuse_poll_in {
+	__u64	fh;
+	__u64	kh;
+	__u32	flags;
+	__u32   padding;
+};
+
+struct fuse_poll_out {
+	__u32	revents;
+	__u32	padding;
+};
+
+struct fuse_notify_poll_wakeup_out {
+	__u64	kh;
+};
+
 struct fuse_in_header {
 	__u32	len;
 	__u32	opcode;
diff --git a/include/linux/gfp.h b/include/linux/gfp.h
index e8003afeffba..dd20cd78faa8 100644
--- a/include/linux/gfp.h
+++ b/include/linux/gfp.h
@@ -69,12 +69,6 @@ struct vm_area_struct;
 #define GFP_HIGHUSER_MOVABLE	(__GFP_WAIT | __GFP_IO | __GFP_FS | \
 				 __GFP_HARDWALL | __GFP_HIGHMEM | \
 				 __GFP_MOVABLE)
-#define GFP_NOFS_PAGECACHE	(__GFP_WAIT | __GFP_IO | __GFP_MOVABLE)
-#define GFP_USER_PAGECACHE	(__GFP_WAIT | __GFP_IO | __GFP_FS | \
-				 __GFP_HARDWALL | __GFP_MOVABLE)
-#define GFP_HIGHUSER_PAGECACHE	(__GFP_WAIT | __GFP_IO | __GFP_FS | \
-				 __GFP_HARDWALL | __GFP_HIGHMEM | \
-				 __GFP_MOVABLE)
 
 #ifdef CONFIG_NUMA
 #define GFP_THISNODE	(__GFP_THISNODE | __GFP_NOWARN | __GFP_NORETRY)
diff --git a/include/linux/gpio_keys.h b/include/linux/gpio_keys.h
index ec6ecd74781d..1289fa7623ca 100644
--- a/include/linux/gpio_keys.h
+++ b/include/linux/gpio_keys.h
@@ -15,6 +15,7 @@ struct gpio_keys_button {
 struct gpio_keys_platform_data {
 	struct gpio_keys_button *buttons;
 	int nbuttons;
+	unsigned int rep:1;		/* enable input subsystem auto repeat */
 };
 
 #endif
diff --git a/include/linux/hugetlb.h b/include/linux/hugetlb.h
index e1c8afc002c0..f1d2fba19ea0 100644
--- a/include/linux/hugetlb.h
+++ b/include/linux/hugetlb.h
@@ -233,6 +233,10 @@ static inline unsigned long huge_page_size(struct hstate *h)
 	return (unsigned long)PAGE_SIZE << h->order;
 }
 
+extern unsigned long vma_kernel_pagesize(struct vm_area_struct *vma);
+
+extern unsigned long vma_mmu_pagesize(struct vm_area_struct *vma);
+
 static inline unsigned long huge_page_mask(struct hstate *h)
 {
 	return h->mask;
@@ -273,6 +277,8 @@ struct hstate {};
 #define hstate_inode(i) NULL
 #define huge_page_size(h) PAGE_SIZE
 #define huge_page_mask(h) PAGE_MASK
+#define vma_kernel_pagesize(v) PAGE_SIZE
+#define vma_mmu_pagesize(v) PAGE_SIZE
 #define huge_page_order(h) 0
 #define huge_page_shift(h) PAGE_SHIFT
 static inline unsigned int pages_per_huge_page(struct hstate *h)
diff --git a/include/linux/i2c.h b/include/linux/i2c.h
index 33a5992d4936..20873d402467 100644
--- a/include/linux/i2c.h
+++ b/include/linux/i2c.h
@@ -393,11 +393,7 @@ static inline void i2c_set_adapdata(struct i2c_adapter *dev, void *data)
 #define I2C_CLASS_TV_ANALOG	(1<<1)	/* bttv + friends */
 #define I2C_CLASS_TV_DIGITAL	(1<<2)	/* dvb cards */
 #define I2C_CLASS_DDC		(1<<3)	/* DDC bus on graphics adapters */
-#define I2C_CLASS_CAM_ANALOG	(1<<4)	/* camera with analog CCD */
-#define I2C_CLASS_CAM_DIGITAL	(1<<5)	/* most webcams */
-#define I2C_CLASS_SOUND		(1<<6)	/* sound devices */
 #define I2C_CLASS_SPD		(1<<7)	/* SPD EEPROMs and similar */
-#define I2C_CLASS_ALL		(UINT_MAX) /* all of the above */
 
 /* i2c_client_address_data is the struct for holding default client
  * addresses for a driver and for the parameters supplied on the
diff --git a/include/linux/i2c/tsc2007.h b/include/linux/i2c/tsc2007.h
new file mode 100644
index 000000000000..c6361fbb7bf9
--- /dev/null
+++ b/include/linux/i2c/tsc2007.h
@@ -0,0 +1,17 @@
+#ifndef __LINUX_I2C_TSC2007_H
+#define __LINUX_I2C_TSC2007_H
+
+/* linux/i2c/tsc2007.h */
+
+struct tsc2007_platform_data {
+	u16	model;				/* 2007. */
+	u16	x_plate_ohms;
+
+	int	(*get_pendown_state)(void);
+	void	(*clear_penirq)(void);		/* If needed, clear 2nd level
+						   interrupt source */
+	int	(*init_platform_hw)(void);
+	void	(*exit_platform_hw)(void);
+};
+
+#endif
diff --git a/include/linux/i2c/twl4030.h b/include/linux/i2c/twl4030.h
index a8f84c01f82e..8137f660a5cc 100644
--- a/include/linux/i2c/twl4030.h
+++ b/include/linux/i2c/twl4030.h
@@ -234,6 +234,9 @@ struct twl4030_gpio_platform_data {
 	/* gpio-n should control VMMC(n+1) if BIT(n) in mmc_cd is set */
 	u8		mmc_cd;
 
+	/* if BIT(N) is set, or VMMC(n+1) is linked, debounce GPIO-N */
+	u32		debounce;
+
 	/* For gpio-N, bit (1 << N) in "pullups" is set if that pullup
 	 * should be enabled.  Else, if that bit is set in "pulldowns",
 	 * that pulldown is enabled.  Don't waste power by letting any
@@ -307,12 +310,6 @@ int twl4030_sih_setup(int module);
 #define TWL4030_VAUX3_DEV_GRP		0x1F
 #define TWL4030_VAUX3_DEDICATED		0x22
 
-/*
- * Exported TWL4030 GPIO APIs
- *
- * WARNING -- use standard GPIO and IRQ calls instead; these will vanish.
- */
-int twl4030_set_gpio_debounce(int gpio, int enable);
 
 #if defined(CONFIG_TWL4030_BCI_BATTERY) || \
 	defined(CONFIG_TWL4030_BCI_BATTERY_MODULE)
diff --git a/include/linux/ide.h b/include/linux/ide.h
index db5ef8ae1ab9..3644f6323384 100644
--- a/include/linux/ide.h
+++ b/include/linux/ide.h
@@ -32,18 +32,14 @@
 # define SUPPORT_VLB_SYNC 1
 #endif
 
-typedef unsigned char	byte;	/* used everywhere */
-
 /*
  * Probably not wise to fiddle with these
  */
+#define IDE_DEFAULT_MAX_FAILURES	1
 #define ERROR_MAX	8	/* Max read/write errors per sector */
 #define ERROR_RESET	3	/* Reset controller every 4th retry */
 #define ERROR_RECAL	1	/* Recalibrate every 2nd retry */
 
-#define HWIF(drive)		((ide_hwif_t *)((drive)->hwif))
-#define HWGROUP(drive)		((ide_hwgroup_t *)(HWIF(drive)->hwgroup))
-
 /*
  * Definitions for accessing IDE controller registers
  */
@@ -185,9 +181,6 @@ typedef struct hw_regs_s {
 	unsigned long	config;
 } hw_regs_t;
 
-void ide_init_port_data(struct hwif_s *, unsigned int);
-void ide_init_port_hw(struct hwif_s *, hw_regs_t *);
-
 static inline void ide_std_init_ports(hw_regs_t *hw,
 				      unsigned long io_addr,
 				      unsigned long ctl_addr)
@@ -433,18 +426,14 @@ struct ide_atapi_pc {
 	struct idetape_bh *bh;
 	char *b_data;
 
-	/* idescsi only for now */
 	struct scatterlist *sg;
 	unsigned int sg_cnt;
 
-	struct scsi_cmnd *scsi_cmd;
-	void (*done) (struct scsi_cmnd *);
-
 	unsigned long timeout;
 };
 
 struct ide_devset;
-struct ide_driver_s;
+struct ide_driver;
 
 #ifdef CONFIG_BLK_DEV_IDEACPI
 struct ide_acpi_drive_link;
@@ -588,7 +577,6 @@ struct ide_drive_s {
 	struct request_queue	*queue;	/* request queue */
 
 	struct request		*rq;	/* current request */
-	struct ide_drive_s 	*next;	/* circular list of hwgroup drives */
 	void		*driver_data;	/* extra driver data */
 	u16			*id;	/* identification info */
 #ifdef CONFIG_IDE_PROC_FS
@@ -662,6 +650,8 @@ struct ide_drive_s {
 	int  (*pc_io_buffers)(struct ide_drive_s *, struct ide_atapi_pc *,
 			      unsigned int, int);
 
+	ide_startstop_t (*irq_handler)(struct ide_drive_s *);
+
 	unsigned long atapi_flags;
 
 	struct ide_atapi_pc request_sense_pc;
@@ -684,7 +674,6 @@ struct ide_tp_ops {
 	void	(*exec_command)(struct hwif_s *, u8);
 	u8	(*read_status)(struct hwif_s *);
 	u8	(*read_altstatus)(struct hwif_s *);
-	u8	(*read_sff_dma_status)(struct hwif_s *);
 
 	void	(*set_irq)(struct hwif_s *, int);
 
@@ -745,14 +734,17 @@ struct ide_dma_ops {
 	int	(*dma_test_irq)(struct ide_drive_s *);
 	void	(*dma_lost_irq)(struct ide_drive_s *);
 	void	(*dma_timeout)(struct ide_drive_s *);
+	/*
+	 * The following method is optional and only required to be
+	 * implemented for the SFF-8038i compatible controllers.
+	 */
+	u8	(*dma_sff_read_status)(struct hwif_s *);
 };
 
 struct ide_host;
 
 typedef struct hwif_s {
-	struct hwif_s *next;		/* for linked-list in ide_hwgroup_t */
 	struct hwif_s *mate;		/* other hwif from same PCI chip */
-	struct hwgroup_s *hwgroup;	/* actually (ide_hwgroup_t *) */
 	struct proc_dir_entry *proc;	/* /proc/ide/ directory entry */
 
 	struct ide_host *host;
@@ -763,7 +755,7 @@ typedef struct hwif_s {
 
 	unsigned long	sata_scr[SATA_NR_PORTS];
 
-	ide_drive_t	drives[MAX_DRIVES];	/* drive info */
+	ide_drive_t	*devices[MAX_DRIVES + 1];
 
 	u8 major;	/* our major number */
 	u8 index;	/* 0 for ide0; 1 for ide1; ... */
@@ -829,7 +821,7 @@ typedef struct hwif_s {
 	unsigned	extra_ports;	/* number of extra dma ports */
 
 	unsigned	present    : 1;	/* this interface exists */
-	unsigned	sg_mapped  : 1;	/* sg_table and sg_nents are ready */
+	unsigned	busy	   : 1; /* serializes devices on a port */
 
 	struct device		gendev;
 	struct device		*portdev;
@@ -841,19 +833,49 @@ typedef struct hwif_s {
 #ifdef CONFIG_BLK_DEV_IDEACPI
 	struct ide_acpi_hwif_link *acpidata;
 #endif
+
+	/* IRQ handler, if active */
+	ide_startstop_t	(*handler)(ide_drive_t *);
+
+	/* BOOL: polling active & poll_timeout field valid */
+	unsigned int polling : 1;
+
+	/* current drive */
+	ide_drive_t *cur_dev;
+
+	/* current request */
+	struct request *rq;
+
+	/* failsafe timer */
+	struct timer_list timer;
+	/* timeout value during long polls */
+	unsigned long poll_timeout;
+	/* queried upon timeouts */
+	int (*expiry)(ide_drive_t *);
+
+	int req_gen;
+	int req_gen_timer;
+
+	spinlock_t lock;
 } ____cacheline_internodealigned_in_smp ide_hwif_t;
 
 #define MAX_HOST_PORTS 4
 
 struct ide_host {
-	ide_hwif_t	*ports[MAX_HOST_PORTS];
+	ide_hwif_t	*ports[MAX_HOST_PORTS + 1];
 	unsigned int	n_ports;
 	struct device	*dev[2];
 	unsigned int	(*init_chipset)(struct pci_dev *);
 	unsigned long	host_flags;
 	void		*host_priv;
+	ide_hwif_t	*cur_port;	/* for hosts requiring serialization */
+
+	/* used for hosts requiring serialization */
+	volatile long	host_busy;
 };
 
+#define IDE_HOST_BUSY 0
+
 /*
  *  internal ide interrupt handler type
  */
@@ -863,38 +885,6 @@ typedef int (ide_expiry_t)(ide_drive_t *);
 /* used by ide-cd, ide-floppy, etc. */
 typedef void (xfer_func_t)(ide_drive_t *, struct request *rq, void *, unsigned);
 
-typedef struct hwgroup_s {
-		/* irq handler, if active */
-	ide_startstop_t	(*handler)(ide_drive_t *);
-
-		/* BOOL: protects all fields below */
-	volatile int busy;
-		/* BOOL: polling active & poll_timeout field valid */
-	unsigned int polling	: 1;
-
-		/* current drive */
-	ide_drive_t *drive;
-		/* ptr to current hwif in linked-list */
-	ide_hwif_t *hwif;
-
-		/* current request */
-	struct request *rq;
-
-		/* failsafe timer */
-	struct timer_list timer;
-		/* timeout value during long polls */
-	unsigned long poll_timeout;
-		/* queried upon timeouts */
-	int (*expiry)(ide_drive_t *);
-
-	int req_gen;
-	int req_gen_timer;
-
-	spinlock_t lock;
-} ide_hwgroup_t;
-
-typedef struct ide_driver_s ide_driver_t;
-
 extern struct mutex ide_setting_mtx;
 
 /*
@@ -1020,8 +1010,8 @@ void ide_proc_register_port(ide_hwif_t *);
 void ide_proc_port_register_devices(ide_hwif_t *);
 void ide_proc_unregister_device(ide_drive_t *);
 void ide_proc_unregister_port(ide_hwif_t *);
-void ide_proc_register_driver(ide_drive_t *, ide_driver_t *);
-void ide_proc_unregister_driver(ide_drive_t *, ide_driver_t *);
+void ide_proc_register_driver(ide_drive_t *, struct ide_driver *);
+void ide_proc_unregister_driver(ide_drive_t *, struct ide_driver *);
 
 read_proc_t proc_ide_read_capacity;
 read_proc_t proc_ide_read_geometry;
@@ -1048,8 +1038,10 @@ static inline void ide_proc_register_port(ide_hwif_t *hwif) { ; }
 static inline void ide_proc_port_register_devices(ide_hwif_t *hwif) { ; }
 static inline void ide_proc_unregister_device(ide_drive_t *drive) { ; }
 static inline void ide_proc_unregister_port(ide_hwif_t *hwif) { ; }
-static inline void ide_proc_register_driver(ide_drive_t *drive, ide_driver_t *driver) { ; }
-static inline void ide_proc_unregister_driver(ide_drive_t *drive, ide_driver_t *driver) { ; }
+static inline void ide_proc_register_driver(ide_drive_t *drive,
+					    struct ide_driver *driver) { ; }
+static inline void ide_proc_unregister_driver(ide_drive_t *drive,
+					      struct ide_driver *driver) { ; }
 #define PROC_IDE_READ_RETURN(page,start,off,count,eof,len) return 0;
 #endif
 
@@ -1118,11 +1110,10 @@ void ide_check_pm_state(ide_drive_t *, struct request *);
  * The gendriver.owner field should be set to the module owner of this driver.
  * The gendriver.name field should be set to the name of this driver
  */
-struct ide_driver_s {
+struct ide_driver {
 	const char			*version;
 	ide_startstop_t	(*do_request)(ide_drive_t *, struct request *, sector_t);
 	int		(*end_request)(ide_drive_t *, int, int);
-	ide_startstop_t	(*error)(ide_drive_t *, struct request *rq, u8, u8);
 	struct device_driver	gen_driver;
 	int		(*probe)(ide_drive_t *);
 	void		(*remove)(ide_drive_t *);
@@ -1134,7 +1125,7 @@ struct ide_driver_s {
 #endif
 };
 
-#define to_ide_driver(drv) container_of(drv, ide_driver_t, gen_driver)
+#define to_ide_driver(drv) container_of(drv, struct ide_driver, gen_driver)
 
 int ide_device_get(ide_drive_t *);
 void ide_device_put(ide_drive_t *);
@@ -1166,9 +1157,7 @@ void ide_execute_pkt_cmd(ide_drive_t *);
 
 void ide_pad_transfer(ide_drive_t *, int, int);
 
-ide_startstop_t __ide_error(ide_drive_t *, struct request *, u8, u8);
-
-ide_startstop_t ide_error (ide_drive_t *drive, const char *msg, byte stat);
+ide_startstop_t ide_error(ide_drive_t *, const char *, u8);
 
 void ide_fix_driveid(u16 *);
 
@@ -1192,7 +1181,6 @@ void ide_tf_dump(const char *, struct ide_taskfile *);
 void ide_exec_command(ide_hwif_t *, u8);
 u8 ide_read_status(ide_hwif_t *);
 u8 ide_read_altstatus(ide_hwif_t *);
-u8 ide_read_sff_dma_status(ide_hwif_t *);
 
 void ide_set_irq(ide_hwif_t *, int);
 
@@ -1272,26 +1260,6 @@ extern void ide_stall_queue(ide_drive_t *drive, unsigned long timeout);
 
 extern void ide_timer_expiry(unsigned long);
 extern irqreturn_t ide_intr(int irq, void *dev_id);
-
-static inline int ide_lock_hwgroup(ide_hwgroup_t *hwgroup)
-{
-	if (hwgroup->busy)
-		return 1;
-
-	hwgroup->busy = 1;
-	/* for atari only */
-	ide_get_lock(ide_intr, hwgroup);
-
-	return 0;
-}
-
-static inline void ide_unlock_hwgroup(ide_hwgroup_t *hwgroup)
-{
-	/* for atari only */
-	ide_release_lock();
-	hwgroup->busy = 0;
-}
-
 extern void do_ide_request(struct request_queue *);
 
 void ide_init_disk(struct gendisk *, ide_drive_t *);
@@ -1327,11 +1295,11 @@ static inline int ide_hwif_setup_dma(ide_hwif_t *hwif,
 }
 #endif
 
-typedef struct ide_pci_enablebit_s {
+struct ide_pci_enablebit {
 	u8	reg;	/* byte pci reg holding the enable-bit */
 	u8	mask;	/* mask to isolate the enable-bit */
 	u8	val;	/* value of masked reg when "enabled" */
-} ide_pci_enablebit_t;
+};
 
 enum {
 	/* Uses ISA control ports not PCI ones. */
@@ -1420,7 +1388,8 @@ struct ide_port_info {
 	const struct ide_port_ops	*port_ops;
 	const struct ide_dma_ops	*dma_ops;
 
-	ide_pci_enablebit_t	enablebits[2];
+	struct ide_pci_enablebit	enablebits[2];
+
 	hwif_chipset_t		chipset;
 
 	u16			max_sectors;	/* if < than the default one */
@@ -1492,6 +1461,7 @@ void ide_dma_exec_cmd(ide_drive_t *, u8);
 extern void ide_dma_start(ide_drive_t *);
 int ide_dma_end(ide_drive_t *);
 int ide_dma_test_irq(ide_drive_t *);
+u8 ide_dma_sff_read_status(ide_hwif_t *);
 extern const struct ide_dma_ops sff_dma_ops;
 #else
 static inline int config_drive_for_dma(ide_drive_t *drive) { return 0; }
@@ -1529,9 +1499,6 @@ static inline void ide_acpi_port_init_devices(ide_hwif_t *hwif) { ; }
 static inline void ide_acpi_set_state(ide_hwif_t *hwif, int on) {}
 #endif
 
-void ide_remove_port_from_hwgroup(ide_hwif_t *);
-void ide_unregister(ide_hwif_t *);
-
 void ide_register_region(struct gendisk *);
 void ide_unregister_region(struct gendisk *);
 
@@ -1616,23 +1583,6 @@ static inline void ide_set_max_pio(ide_drive_t *drive)
 	ide_set_pio(drive, 255);
 }
 
-extern spinlock_t ide_lock;
-extern struct mutex ide_cfg_mtx;
-/*
- * Structure locking:
- *
- * ide_cfg_mtx and hwgroup->lock together protect changes to
- * ide_hwif_t->next
- * ide_drive_t->next
- *
- * ide_hwgroup_t->busy: hwgroup->lock
- * ide_hwgroup_t->hwif: hwgroup->lock
- * ide_hwif_t->{hwgroup,mate}: constant, no locking
- * ide_drive_t->hwif: constant, no locking
- */
-
-#define local_irq_set(flags)	do { local_save_flags((flags)); local_irq_enable_in_hardirq(); } while (0)
-
 char *ide_media_string(ide_drive_t *);
 
 extern struct device_attribute ide_dev_attrs[];
@@ -1651,8 +1601,15 @@ static inline int hwif_to_node(ide_hwif_t *hwif)
 
 static inline ide_drive_t *ide_get_pair_dev(ide_drive_t *drive)
 {
-	ide_drive_t *peer = &drive->hwif->drives[(drive->dn ^ 1) & 1];
+	ide_drive_t *peer = drive->hwif->devices[(drive->dn ^ 1) & 1];
 
 	return (peer->dev_flags & IDE_DFLAG_PRESENT) ? peer : NULL;
 }
+
+#define ide_port_for_each_dev(i, dev, port) \
+	for ((i) = 0; ((dev) = (port)->devices[i]) || (i) < MAX_DRIVES; (i)++)
+
+#define ide_host_for_each_port(i, port, host) \
+	for ((i) = 0; ((port) = (host)->ports[i]) || (i) < MAX_HOST_PORTS; (i)++)
+
 #endif /* _IDE_H */
diff --git a/include/linux/interrupt.h b/include/linux/interrupt.h
index 0702c4d7bdf0..9127f6b51a39 100644
--- a/include/linux/interrupt.h
+++ b/include/linux/interrupt.h
@@ -14,7 +14,6 @@
 #include <linux/irqflags.h>
 #include <linux/smp.h>
 #include <linux/percpu.h>
-#include <linux/irqnr.h>
 
 #include <asm/atomic.h>
 #include <asm/ptrace.h>
@@ -253,7 +252,8 @@ enum
 	BLOCK_SOFTIRQ,
 	TASKLET_SOFTIRQ,
 	SCHED_SOFTIRQ,
-	RCU_SOFTIRQ, 	/* Preferable RCU should always be the last softirq */
+	HRTIMER_SOFTIRQ,
+	RCU_SOFTIRQ,	/* Preferable RCU should always be the last softirq */
 
 	NR_SOFTIRQS
 };
diff --git a/include/linux/ioport.h b/include/linux/ioport.h
index 041e95aac2bf..f6bb2ca8e3ba 100644
--- a/include/linux/ioport.h
+++ b/include/linux/ioport.h
@@ -49,6 +49,7 @@ struct resource_list {
 #define IORESOURCE_SIZEALIGN	0x00020000	/* size indicates alignment */
 #define IORESOURCE_STARTALIGN	0x00040000	/* start field is alignment */
 
+#define IORESOURCE_EXCLUSIVE	0x08000000	/* Userland may not map this resource */
 #define IORESOURCE_DISABLED	0x10000000
 #define IORESOURCE_UNSET	0x20000000
 #define IORESOURCE_AUTO		0x40000000
@@ -133,13 +134,16 @@ static inline unsigned long resource_type(struct resource *res)
 }
 
 /* Convenience shorthand with allocation */
-#define request_region(start,n,name)	__request_region(&ioport_resource, (start), (n), (name))
-#define request_mem_region(start,n,name) __request_region(&iomem_resource, (start), (n), (name))
+#define request_region(start,n,name)	__request_region(&ioport_resource, (start), (n), (name), 0)
+#define __request_mem_region(start,n,name, excl) __request_region(&iomem_resource, (start), (n), (name), excl)
+#define request_mem_region(start,n,name) __request_region(&iomem_resource, (start), (n), (name), 0)
+#define request_mem_region_exclusive(start,n,name) \
+	__request_region(&iomem_resource, (start), (n), (name), IORESOURCE_EXCLUSIVE)
 #define rename_region(region, newname) do { (region)->name = (newname); } while (0)
 
 extern struct resource * __request_region(struct resource *,
 					resource_size_t start,
-					resource_size_t n, const char *name);
+					resource_size_t n, const char *name, int relaxed);
 
 /* Compatibility cruft */
 #define release_region(start,n)	__release_region(&ioport_resource, (start), (n))
@@ -175,6 +179,7 @@ extern struct resource * __devm_request_region(struct device *dev,
 extern void __devm_release_region(struct device *dev, struct resource *parent,
 				  resource_size_t start, resource_size_t n);
 extern int iomem_map_sanity_check(resource_size_t addr, unsigned long size);
+extern int iomem_is_exclusive(u64 addr);
 
 #endif /* __ASSEMBLY__ */
 #endif	/* _LINUX_IOPORT_H */
diff --git a/include/linux/irqnr.h b/include/linux/irqnr.h
index 5504a5c97836..86af92e9e84c 100644
--- a/include/linux/irqnr.h
+++ b/include/linux/irqnr.h
@@ -8,7 +8,12 @@
 
 #ifndef CONFIG_GENERIC_HARDIRQS
 #include <asm/irq.h>
-# define nr_irqs		NR_IRQS
+
+/*
+ * Wrappers for non-genirq architectures:
+ */
+#define nr_irqs			NR_IRQS
+#define irq_to_desc(irq)	(&irq_desc[irq])
 
 # define for_each_irq_desc(irq, desc)		\
 	for (irq = 0; irq < nr_irqs; irq++)
diff --git a/include/linux/kernel.h b/include/linux/kernel.h
index ca9ff6411dfa..6b8e2027165e 100644
--- a/include/linux/kernel.h
+++ b/include/linux/kernel.h
@@ -48,6 +48,12 @@ extern const char linux_proc_banner[];
 #define FIELD_SIZEOF(t, f) (sizeof(((t*)0)->f))
 #define DIV_ROUND_UP(n,d) (((n) + (d) - 1) / (d))
 #define roundup(x, y) ((((x) + ((y) - 1)) / (y)) * (y))
+#define DIV_ROUND_CLOSEST(x, divisor)(			\
+{							\
+	typeof(divisor) __divisor = divisor;		\
+	(((x) + ((__divisor) / 2)) / (__divisor));	\
+}							\
+)
 
 #define _RET_IP_		(unsigned long)__builtin_return_address(0)
 #define _THIS_IP_  ({ __label__ __here; __here: (unsigned long)&&__here; })
@@ -349,13 +355,13 @@ static inline char *pack_hex_byte(char *buf, u8 byte)
         printk(KERN_INFO pr_fmt(fmt), ##__VA_ARGS__)
 
 /* If you are writing a driver, please use dev_dbg instead */
-#if defined(CONFIG_DYNAMIC_PRINTK_DEBUG)
+#if defined(DEBUG)
+#define pr_debug(fmt, ...) \
+	printk(KERN_DEBUG pr_fmt(fmt), ##__VA_ARGS__)
+#elif defined(CONFIG_DYNAMIC_PRINTK_DEBUG)
 #define pr_debug(fmt, ...) do { \
 	dynamic_pr_debug(pr_fmt(fmt), ##__VA_ARGS__); \
 	} while (0)
-#elif defined(DEBUG)
-#define pr_debug(fmt, ...) \
-	printk(KERN_DEBUG pr_fmt(fmt), ##__VA_ARGS__)
 #else
 #define pr_debug(fmt, ...) \
 	({ if (0) printk(KERN_DEBUG pr_fmt(fmt), ##__VA_ARGS__); 0; })
diff --git a/include/linux/klist.h b/include/linux/klist.h
index 8ea98db223e5..d5a27af9dba5 100644
--- a/include/linux/klist.h
+++ b/include/linux/klist.h
@@ -13,7 +13,6 @@
 #define _LINUX_KLIST_H
 
 #include <linux/spinlock.h>
-#include <linux/completion.h>
 #include <linux/kref.h>
 #include <linux/list.h>
 
@@ -41,7 +40,6 @@ struct klist_node {
 	void			*n_klist;	/* never access directly */
 	struct list_head	n_node;
 	struct kref		n_ref;
-	struct completion	n_removed;
 };
 
 extern void klist_add_tail(struct klist_node *n, struct klist *k);
diff --git a/include/linux/kprobes.h b/include/linux/kprobes.h
index 497b1d1f7a05..d6ea19e314bb 100644
--- a/include/linux/kprobes.h
+++ b/include/linux/kprobes.h
@@ -69,9 +69,6 @@ struct kprobe {
 	/* list of kprobes for multi-handler support */
 	struct list_head list;
 
-	/* Indicates that the corresponding module has been ref counted */
-	unsigned int mod_refcounted;
-
 	/*count the number of times this probe was temporarily disarmed */
 	unsigned long nmissed;
 
@@ -103,8 +100,19 @@ struct kprobe {
 
 	/* copy of the original instruction */
 	struct arch_specific_insn ainsn;
+
+	/* Indicates various status flags.  Protected by kprobe_mutex. */
+	u32 flags;
 };
 
+/* Kprobe status flags */
+#define KPROBE_FLAG_GONE	1 /* breakpoint has already gone */
+
+static inline int kprobe_gone(struct kprobe *p)
+{
+	return p->flags & KPROBE_FLAG_GONE;
+}
+
 /*
  * Special probe type that uses setjmp-longjmp type tricks to resume
  * execution at a specified entry with a matching prototype corresponding
@@ -201,7 +209,6 @@ static inline int init_test_probes(void)
 }
 #endif /* CONFIG_KPROBES_SANITY_TEST */
 
-extern struct mutex kprobe_mutex;
 extern int arch_prepare_kprobe(struct kprobe *p);
 extern void arch_arm_kprobe(struct kprobe *p);
 extern void arch_disarm_kprobe(struct kprobe *p);
diff --git a/include/linux/libps2.h b/include/linux/libps2.h
index afc413369101..b94534b7e266 100644
--- a/include/linux/libps2.h
+++ b/include/linux/libps2.h
@@ -18,11 +18,13 @@
 #define PS2_RET_ID		0x00
 #define PS2_RET_ACK		0xfa
 #define PS2_RET_NAK		0xfe
+#define PS2_RET_ERR		0xfc
 
 #define PS2_FLAG_ACK		1	/* Waiting for ACK/NAK */
 #define PS2_FLAG_CMD		2	/* Waiting for command to finish */
 #define PS2_FLAG_CMD1		4	/* Waiting for the first byte of command response */
 #define PS2_FLAG_WAITID		8	/* Command execiting is GET ID */
+#define PS2_FLAG_NAK		16	/* Last transmission was NAKed */
 
 struct ps2dev {
 	struct serio *serio;
diff --git a/include/linux/lockd/lockd.h b/include/linux/lockd/lockd.h
index 23da3fa69efa..aa6fe7026de7 100644
--- a/include/linux/lockd/lockd.h
+++ b/include/linux/lockd/lockd.h
@@ -43,8 +43,8 @@ struct nlm_host {
 	struct sockaddr_storage	h_addr;		/* peer address */
 	size_t			h_addrlen;
 	struct sockaddr_storage	h_srcaddr;	/* our address (optional) */
-	struct rpc_clnt	*	h_rpcclnt;	/* RPC client to talk to peer */
-	char *			h_name;		/* remote hostname */
+	struct rpc_clnt		*h_rpcclnt;	/* RPC client to talk to peer */
+	char			*h_name;		/* remote hostname */
 	u32			h_version;	/* interface version */
 	unsigned short		h_proto;	/* transport proto */
 	unsigned short		h_reclaiming : 1,
@@ -64,21 +64,29 @@ struct nlm_host {
 	spinlock_t		h_lock;
 	struct list_head	h_granted;	/* Locks in GRANTED state */
 	struct list_head	h_reclaim;	/* Locks in RECLAIM state */
-	struct nsm_handle *	h_nsmhandle;	/* NSM status handle */
-
-	char			h_addrbuf[48],	/* address eyecatchers */
-				h_srcaddrbuf[48];
+	struct nsm_handle	*h_nsmhandle;	/* NSM status handle */
+	char			*h_addrbuf;	/* address eyecatcher */
 };
 
+/*
+ * The largest string sm_addrbuf should hold is a full-size IPv6 address
+ * (no "::" anywhere) with a scope ID.  The buffer size is computed to
+ * hold eight groups of colon-separated four-hex-digit numbers, a
+ * percent sign, a scope id (at most 32 bits, in decimal), and NUL.
+ */
+#define NSM_ADDRBUF		((8 * 4 + 7) + (1 + 10) + 1)
+
 struct nsm_handle {
 	struct list_head	sm_link;
 	atomic_t		sm_count;
-	char *			sm_name;
+	char			*sm_mon_name;
+	char			*sm_name;
 	struct sockaddr_storage	sm_addr;
 	size_t			sm_addrlen;
 	unsigned int		sm_monitored : 1,
 				sm_sticky : 1;	/* don't unmonitor */
-	char			sm_addrbuf[48];	/* address eyecatcher */
+	struct nsm_private	sm_priv;
+	char			sm_addrbuf[NSM_ADDRBUF];
 };
 
 /*
@@ -104,16 +112,6 @@ static inline struct sockaddr *nlm_srcaddr(const struct nlm_host *host)
 	return (struct sockaddr *)&host->h_srcaddr;
 }
 
-static inline struct sockaddr_in *nsm_addr_in(const struct nsm_handle *handle)
-{
-	return (struct sockaddr_in *)&handle->sm_addr;
-}
-
-static inline struct sockaddr *nsm_addr(const struct nsm_handle *handle)
-{
-	return (struct sockaddr *)&handle->sm_addr;
-}
-
 /*
  * Map an fl_owner_t into a unique 32-bit "pid"
  */
@@ -197,6 +195,7 @@ extern struct svc_procedure	nlmsvc_procedures4[];
 extern int			nlmsvc_grace_period;
 extern unsigned long		nlmsvc_timeout;
 extern int			nsm_use_hostnames;
+extern int			nsm_local_state;
 
 /*
  * Lockd client functions
@@ -231,10 +230,20 @@ void		  nlm_rebind_host(struct nlm_host *);
 struct nlm_host * nlm_get_host(struct nlm_host *);
 void		  nlm_release_host(struct nlm_host *);
 void		  nlm_shutdown_hosts(void);
-extern void	  nlm_host_rebooted(const struct sockaddr_in *, const char *,
-					unsigned int, u32);
-void		  nsm_release(struct nsm_handle *);
+void		  nlm_host_rebooted(const struct nlm_reboot *);
 
+/*
+ * Host monitoring
+ */
+int		  nsm_monitor(const struct nlm_host *host);
+void		  nsm_unmonitor(const struct nlm_host *host);
+
+struct nsm_handle *nsm_get_handle(const struct sockaddr *sap,
+					const size_t salen,
+					const char *hostname,
+					const size_t hostname_len);
+struct nsm_handle *nsm_reboot_lookup(const struct nlm_reboot *info);
+void		  nsm_release(struct nsm_handle *nsm);
 
 /*
  * This is used in garbage collection and resource reclaim
@@ -282,16 +291,25 @@ static inline struct inode *nlmsvc_file_inode(struct nlm_file *file)
 static inline int __nlm_privileged_request4(const struct sockaddr *sap)
 {
 	const struct sockaddr_in *sin = (struct sockaddr_in *)sap;
-	return (sin->sin_addr.s_addr == htonl(INADDR_LOOPBACK)) &&
-			(ntohs(sin->sin_port) < 1024);
+
+	if (ntohs(sin->sin_port) > 1023)
+		return 0;
+
+	return ipv4_is_loopback(sin->sin_addr.s_addr);
 }
 
 #if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
 static inline int __nlm_privileged_request6(const struct sockaddr *sap)
 {
 	const struct sockaddr_in6 *sin6 = (struct sockaddr_in6 *)sap;
-	return (ipv6_addr_type(&sin6->sin6_addr) & IPV6_ADDR_LOOPBACK) &&
-			(ntohs(sin6->sin6_port) < 1024);
+
+	if (ntohs(sin6->sin6_port) > 1023)
+		return 0;
+
+	if (ipv6_addr_type(&sin6->sin6_addr) & IPV6_ADDR_MAPPED)
+		return ipv4_is_loopback(sin6->sin6_addr.s6_addr32[3]);
+
+	return ipv6_addr_type(&sin6->sin6_addr) & IPV6_ADDR_LOOPBACK;
 }
 #else	/* defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE) */
 static inline int __nlm_privileged_request6(const struct sockaddr *sap)
diff --git a/include/linux/lockd/sm_inter.h b/include/linux/lockd/sm_inter.h
deleted file mode 100644
index 5a5448bdb17d..000000000000
--- a/include/linux/lockd/sm_inter.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- * linux/include/linux/lockd/sm_inter.h
- *
- * Declarations for the kernel statd client.
- *
- * Copyright (C) 1996, Olaf Kirch <okir@monad.swb.de>
- */
-
-#ifndef LINUX_LOCKD_SM_INTER_H
-#define LINUX_LOCKD_SM_INTER_H
-
-#define SM_PROGRAM	100024
-#define SM_VERSION	1
-#define SM_STAT		1
-#define SM_MON		2
-#define SM_UNMON	3
-#define SM_UNMON_ALL	4
-#define SM_SIMU_CRASH	5
-#define SM_NOTIFY	6
-
-#define SM_MAXSTRLEN	1024
-#define SM_PRIV_SIZE	16
-
-/*
- * Arguments for all calls to statd
- */
-struct nsm_args {
-	__be32		addr;		/* remote address */
-	u32		prog;		/* RPC callback info */
-	u32		vers;
-	u32		proc;
-
-	char *		mon_name;
-};
-
-/*
- * Result returned by statd
- */
-struct nsm_res {
-	u32		status;
-	u32		state;
-};
-
-int		nsm_monitor(struct nlm_host *);
-int		nsm_unmonitor(struct nlm_host *);
-extern int	nsm_local_state;
-
-#endif /* LINUX_LOCKD_SM_INTER_H */
diff --git a/include/linux/lockd/xdr.h b/include/linux/lockd/xdr.h
index d6b3a802c046..7dc5b6cb44cd 100644
--- a/include/linux/lockd/xdr.h
+++ b/include/linux/lockd/xdr.h
@@ -13,6 +13,13 @@
 #include <linux/nfs.h>
 #include <linux/sunrpc/xdr.h>
 
+#define SM_MAXSTRLEN		1024
+#define SM_PRIV_SIZE		16
+
+struct nsm_private {
+	unsigned char		data[SM_PRIV_SIZE];
+};
+
 struct svc_rqst;
 
 #define NLM_MAXCOOKIELEN    	32
@@ -77,10 +84,10 @@ struct nlm_res {
  * statd callback when client has rebooted
  */
 struct nlm_reboot {
-	char *		mon;
-	unsigned int	len;
-	u32		state;
-	__be32		addr;
+	char			*mon;
+	unsigned int		len;
+	u32			state;
+	struct nsm_private	priv;
 };
 
 /*
diff --git a/include/linux/map_to_7segment.h b/include/linux/map_to_7segment.h
index 7df8432c4402..12d62a54d470 100644
--- a/include/linux/map_to_7segment.h
+++ b/include/linux/map_to_7segment.h
@@ -75,7 +75,7 @@ struct seg7_conversion_map {
 	unsigned char	table[128];
 };
 
-static inline int map_to_seg7(struct seg7_conversion_map *map, int c)
+static __inline__ int map_to_seg7(struct seg7_conversion_map *map, int c)
 {
 	return c >= 0 && c < sizeof(map->table) ? map->table[c] : -EINVAL;
 }
diff --git a/include/linux/memory.h b/include/linux/memory.h
index 36c82c9e6ea7..3fdc10806d31 100644
--- a/include/linux/memory.h
+++ b/include/linux/memory.h
@@ -79,14 +79,14 @@ static inline int memory_notify(unsigned long val, void *v)
 #else
 extern int register_memory_notifier(struct notifier_block *nb);
 extern void unregister_memory_notifier(struct notifier_block *nb);
-extern int register_new_memory(struct mem_section *);
+extern int register_new_memory(int, struct mem_section *);
 extern int unregister_memory_section(struct mem_section *);
 extern int memory_dev_init(void);
 extern int remove_memory_block(unsigned long, struct mem_section *, int);
 extern int memory_notify(unsigned long val, void *v);
+extern struct memory_block *find_memory_block(struct mem_section *);
 #define CONFIG_MEM_BLOCK_SIZE	(PAGES_PER_SECTION<<PAGE_SHIFT)
-
-
+enum mem_add_context { BOOT, HOTPLUG };
 #endif /* CONFIG_MEMORY_HOTPLUG_SPARSE */
 
 #ifdef CONFIG_MEMORY_HOTPLUG
diff --git a/include/linux/memory_hotplug.h b/include/linux/memory_hotplug.h
index 763ba81fc0f0..d95f72e79b82 100644
--- a/include/linux/memory_hotplug.h
+++ b/include/linux/memory_hotplug.h
@@ -72,7 +72,7 @@ extern void __offline_isolated_pages(unsigned long, unsigned long);
 extern int offline_pages(unsigned long, unsigned long, unsigned long);
 
 /* reasonably generic interface to expand the physical pages in a zone  */
-extern int __add_pages(struct zone *zone, unsigned long start_pfn,
+extern int __add_pages(int nid, struct zone *zone, unsigned long start_pfn,
 	unsigned long nr_pages);
 extern int __remove_pages(struct zone *zone, unsigned long start_pfn,
 	unsigned long nr_pages);
diff --git a/include/linux/migrate.h b/include/linux/migrate.h
index 3f34005068d4..527602cdea1c 100644
--- a/include/linux/migrate.h
+++ b/include/linux/migrate.h
@@ -7,6 +7,8 @@
 typedef struct page *new_page_t(struct page *, unsigned long private, int **);
 
 #ifdef CONFIG_MIGRATION
+#define PAGE_MIGRATION 1
+
 extern int putback_lru_pages(struct list_head *l);
 extern int migrate_page(struct address_space *,
 			struct page *, struct page *);
@@ -20,6 +22,8 @@ extern int migrate_vmas(struct mm_struct *mm,
 		const nodemask_t *from, const nodemask_t *to,
 		unsigned long flags);
 #else
+#define PAGE_MIGRATION 0
+
 static inline int putback_lru_pages(struct list_head *l) { return 0; }
 static inline int migrate_pages(struct list_head *l, new_page_t x,
 		unsigned long private) { return -ENOSYS; }
diff --git a/include/linux/miscdevice.h b/include/linux/miscdevice.h
index 26433ec520b3..a820f816a49e 100644
--- a/include/linux/miscdevice.h
+++ b/include/linux/miscdevice.h
@@ -3,33 +3,33 @@
 #include <linux/module.h>
 #include <linux/major.h>
 
-#define PSMOUSE_MINOR  1
-#define MS_BUSMOUSE_MINOR 2
-#define ATIXL_BUSMOUSE_MINOR 3
-/*#define AMIGAMOUSE_MINOR 4	FIXME OBSOLETE */
-#define ATARIMOUSE_MINOR 5
-#define SUN_MOUSE_MINOR 6
-#define APOLLO_MOUSE_MINOR 7
-#define PC110PAD_MINOR 9
-/*#define ADB_MOUSE_MINOR 10	FIXME OBSOLETE */
+#define PSMOUSE_MINOR		1
+#define MS_BUSMOUSE_MINOR	2
+#define ATIXL_BUSMOUSE_MINOR	3
+/*#define AMIGAMOUSE_MINOR	4	FIXME OBSOLETE */
+#define ATARIMOUSE_MINOR	5
+#define SUN_MOUSE_MINOR		6
+#define APOLLO_MOUSE_MINOR	7
+#define PC110PAD_MINOR		9
+/*#define ADB_MOUSE_MINOR	10	FIXME OBSOLETE */
 #define WATCHDOG_MINOR		130	/* Watchdog timer     */
 #define TEMP_MINOR		131	/* Temperature Sensor */
-#define RTC_MINOR 135
+#define RTC_MINOR		135
 #define EFI_RTC_MINOR		136	/* EFI Time services */
-#define SUN_OPENPROM_MINOR 139
+#define SUN_OPENPROM_MINOR	139
 #define DMAPI_MINOR		140	/* DMAPI */
-#define NVRAM_MINOR 144
-#define SGI_MMTIMER        153
+#define NVRAM_MINOR		144
+#define SGI_MMTIMER		153
 #define STORE_QUEUE_MINOR	155
-#define I2O_MINOR 166
+#define I2O_MINOR		166
 #define MICROCODE_MINOR		184
-#define MWAVE_MINOR	219		/* ACP/Mwave Modem */
-#define MPT_MINOR	220
-#define MISC_DYNAMIC_MINOR 255
-
-#define TUN_MINOR	     200
-#define	HPET_MINOR	     228
-#define KVM_MINOR            232
+#define TUN_MINOR		200
+#define MWAVE_MINOR		219	/* ACP/Mwave Modem */
+#define MPT_MINOR		220
+#define HPET_MINOR		228
+#define FUSE_MINOR		229
+#define KVM_MINOR		232
+#define MISC_DYNAMIC_MINOR	255
 
 struct device;
 
diff --git a/include/linux/mm.h b/include/linux/mm.h
index aaa8b843be28..4a3d28c86443 100644
--- a/include/linux/mm.h
+++ b/include/linux/mm.h
@@ -717,6 +717,11 @@ static inline int page_mapped(struct page *page)
 
 #define VM_FAULT_ERROR	(VM_FAULT_OOM | VM_FAULT_SIGBUS)
 
+/*
+ * Can be called by the pagefault handler when it gets a VM_FAULT_OOM.
+ */
+extern void pagefault_out_of_memory(void);
+
 #define offset_in_page(p)	((unsigned long)(p) & ~PAGE_MASK)
 
 extern void show_free_areas(void);
diff --git a/include/linux/module.h b/include/linux/module.h
index 3bfed013350b..4f7ea12463d3 100644
--- a/include/linux/module.h
+++ b/include/linux/module.h
@@ -294,9 +294,6 @@ struct module
 	/* The size of the executable code in each section.  */
 	unsigned int init_text_size, core_text_size;
 
-	/* The handle returned from unwind_add_table. */
-	void *unwind_info;
-
 	/* Arch-specific module values */
 	struct mod_arch_specific arch;
 
@@ -368,6 +365,18 @@ struct module *module_text_address(unsigned long addr);
 struct module *__module_text_address(unsigned long addr);
 int is_module_address(unsigned long addr);
 
+static inline int within_module_core(unsigned long addr, struct module *mod)
+{
+	return (unsigned long)mod->module_core <= addr &&
+	       addr < (unsigned long)mod->module_core + mod->core_size;
+}
+
+static inline int within_module_init(unsigned long addr, struct module *mod)
+{
+	return (unsigned long)mod->module_init <= addr &&
+	       addr < (unsigned long)mod->module_init + mod->init_size;
+}
+
 /* Returns 0 and fills in value, defined and namebuf, or -ERANGE if
    symnum out of range. */
 int module_get_kallsym(unsigned int symnum, unsigned long *value, char *type,
diff --git a/include/linux/mtd/concat.h b/include/linux/mtd/concat.h
index c02f3d264ecf..e80c674daeb3 100644
--- a/include/linux/mtd/concat.h
+++ b/include/linux/mtd/concat.h
@@ -13,7 +13,7 @@
 struct mtd_info *mtd_concat_create(
     struct mtd_info *subdev[],  /* subdevices to concatenate */
     int num_devs,               /* number of subdevices      */
-    char *name);                /* name for the new device   */
+    const char *name);          /* name for the new device   */
 
 void mtd_concat_destroy(struct mtd_info *mtd);
 
diff --git a/include/linux/mtd/mtd.h b/include/linux/mtd/mtd.h
index eae26bb6430a..64433eb411d7 100644
--- a/include/linux/mtd/mtd.h
+++ b/include/linux/mtd/mtd.h
@@ -83,7 +83,7 @@ typedef enum {
  * @datbuf:	data buffer - if NULL only oob data are read/written
  * @oobbuf:	oob data buffer
  *
- * Note, it is allowed to read more then one OOB area at one go, but not write.
+ * Note, it is allowed to read more than one OOB area at one go, but not write.
  * The interface assumes that the OOB write requests program only one page's
  * OOB area.
  */
diff --git a/include/linux/ncp_fs.h b/include/linux/ncp_fs.h
index 9f2d76347f19..f69e66d151cc 100644
--- a/include/linux/ncp_fs.h
+++ b/include/linux/ncp_fs.h
@@ -87,7 +87,7 @@ struct ncp_objectname_ioctl
 #define NCP_AUTH_NDS	0x32
 	int		auth_type;
 	size_t		object_name_len;
-	void __user *	object_name;	/* an userspace data, in most cases user name */
+	void __user *	object_name;	/* a userspace data, in most cases user name */
 };
 
 struct ncp_privatedata_ioctl
diff --git a/include/linux/nfs4.h b/include/linux/nfs4.h
index ea0366769484..b912311a56b1 100644
--- a/include/linux/nfs4.h
+++ b/include/linux/nfs4.h
@@ -88,6 +88,8 @@
 #define NFS4_ACE_GENERIC_EXECUTE              0x001200A0
 #define NFS4_ACE_MASK_ALL                     0x001F01FF
 
+#define NFS4_MAX_UINT64	(~(u64)0)
+
 enum nfs4_acl_whotype {
 	NFS4_ACL_WHO_NAMED = 0,
 	NFS4_ACL_WHO_OWNER,
diff --git a/include/linux/nfsd/nfsd.h b/include/linux/nfsd/nfsd.h
index 21269405ffe2..e19f45991b2e 100644
--- a/include/linux/nfsd/nfsd.h
+++ b/include/linux/nfsd/nfsd.h
@@ -23,7 +23,6 @@
 /*
  * nfsd version
  */
-#define NFSD_VERSION		"0.5"
 #define NFSD_SUPPORTED_MINOR_VERSION	0
 
 /*
diff --git a/include/linux/nfsd/nfsfh.h b/include/linux/nfsd/nfsfh.h
index d1941cb965e9..b2e093870bc6 100644
--- a/include/linux/nfsd/nfsfh.h
+++ b/include/linux/nfsd/nfsfh.h
@@ -68,6 +68,10 @@ struct nfs_fhbase_old {
  *     1  - 4 byte user specified identifier
  *     2  - 4 byte major, 4 byte minor, 4 byte inode number - DEPRECATED
  *     3  - 4 byte device id, encoded for user-space, 4 byte inode number
+ *     4  - 4 byte inode number and 4 byte uuid
+ *     5  - 8 byte uuid
+ *     6  - 16 byte uuid
+ *     7  - 8 byte inode number and 16 byte uuid
  *
  * The fileid_type identified how the file within the filesystem is encoded.
  * This is (will be) passed to, and set by, the underlying filesystem if it supports
diff --git a/include/linux/node.h b/include/linux/node.h
index bc001bc225c3..681a697b9a86 100644
--- a/include/linux/node.h
+++ b/include/linux/node.h
@@ -26,6 +26,7 @@ struct node {
 	struct sys_device	sysdev;
 };
 
+struct memory_block;
 extern struct node node_devices[];
 
 extern int register_node(struct node *, int, struct node *);
@@ -35,6 +36,9 @@ extern int register_one_node(int nid);
 extern void unregister_one_node(int nid);
 extern int register_cpu_under_node(unsigned int cpu, unsigned int nid);
 extern int unregister_cpu_under_node(unsigned int cpu, unsigned int nid);
+extern int register_mem_sect_under_node(struct memory_block *mem_blk,
+						int nid);
+extern int unregister_mem_sect_under_nodes(struct memory_block *mem_blk);
 #else
 static inline int register_one_node(int nid)
 {
@@ -52,6 +56,15 @@ static inline int unregister_cpu_under_node(unsigned int cpu, unsigned int nid)
 {
 	return 0;
 }
+static inline int register_mem_sect_under_node(struct memory_block *mem_blk,
+							int nid)
+{
+	return 0;
+}
+static inline int unregister_mem_sect_under_nodes(struct memory_block *mem_blk)
+{
+	return 0;
+}
 #endif
 
 #define to_node(sys_device) container_of(sys_device, struct node, sysdev)
diff --git a/include/linux/oxu210hp.h b/include/linux/oxu210hp.h
new file mode 100644
index 000000000000..0bf96eae5389
--- /dev/null
+++ b/include/linux/oxu210hp.h
@@ -0,0 +1,7 @@
+/* platform data for the OXU210HP HCD */
+
+struct oxu210hp_platform_data {
+	unsigned int bus16:1;
+	unsigned int use_hcd_otg:1;
+	unsigned int use_hcd_sph:1;
+};
diff --git a/include/linux/page-flags.h b/include/linux/page-flags.h
index b12f93a3c345..219a523ecdb0 100644
--- a/include/linux/page-flags.h
+++ b/include/linux/page-flags.h
@@ -228,6 +228,7 @@ PAGEFLAG_FALSE(HighMem)
 PAGEFLAG(SwapCache, swapcache)
 #else
 PAGEFLAG_FALSE(SwapCache)
+	SETPAGEFLAG_NOOP(SwapCache) CLEARPAGEFLAG_NOOP(SwapCache)
 #endif
 
 #ifdef CONFIG_UNEVICTABLE_LRU
@@ -372,31 +373,22 @@ static inline void __ClearPageTail(struct page *page)
 #define __PG_MLOCKED		0
 #endif
 
-#define PAGE_FLAGS	(1 << PG_lru   | 1 << PG_private   | 1 << PG_locked | \
-			 1 << PG_buddy | 1 << PG_writeback | \
-			 1 << PG_slab  | 1 << PG_swapcache | 1 << PG_active | \
-			 __PG_UNEVICTABLE | __PG_MLOCKED)
-
-/*
- * Flags checked in bad_page().  Pages on the free list should not have
- * these flags set.  It they are, there is a problem.
- */
-#define PAGE_FLAGS_CLEAR_WHEN_BAD (PAGE_FLAGS | \
-		1 << PG_reclaim | 1 << PG_dirty | 1 << PG_swapbacked)
-
 /*
  * Flags checked when a page is freed.  Pages being freed should not have
  * these flags set.  It they are, there is a problem.
  */
-#define PAGE_FLAGS_CHECK_AT_FREE (PAGE_FLAGS | 1 << PG_reserved)
+#define PAGE_FLAGS_CHECK_AT_FREE \
+	(1 << PG_lru   | 1 << PG_private   | 1 << PG_locked | \
+	 1 << PG_buddy | 1 << PG_writeback | 1 << PG_reserved | \
+	 1 << PG_slab  | 1 << PG_swapcache | 1 << PG_active | \
+	 __PG_UNEVICTABLE | __PG_MLOCKED)
 
 /*
  * Flags checked when a page is prepped for return by the page allocator.
- * Pages being prepped should not have these flags set.  It they are, there
- * is a problem.
+ * Pages being prepped should not have any flags set.  It they are set,
+ * there has been a kernel bug or struct page corruption.
  */
-#define PAGE_FLAGS_CHECK_AT_PREP (PAGE_FLAGS | \
-		1 << PG_reserved | 1 << PG_dirty | 1 << PG_swapbacked)
+#define PAGE_FLAGS_CHECK_AT_PREP	((1 << NR_PAGEFLAGS) - 1)
 
 #endif /* !__GENERATING_BOUNDS_H */
 #endif	/* PAGE_FLAGS_H */
diff --git a/include/linux/pagevec.h b/include/linux/pagevec.h
index e90a2cb02915..7b2886fa7fdc 100644
--- a/include/linux/pagevec.h
+++ b/include/linux/pagevec.h
@@ -21,7 +21,6 @@ struct pagevec {
 };
 
 void __pagevec_release(struct pagevec *pvec);
-void __pagevec_release_nonlru(struct pagevec *pvec);
 void __pagevec_free(struct pagevec *pvec);
 void ____pagevec_lru_add(struct pagevec *pvec, enum lru_list lru);
 void pagevec_strip(struct pagevec *pvec);
@@ -69,12 +68,6 @@ static inline void pagevec_release(struct pagevec *pvec)
 		__pagevec_release(pvec);
 }
 
-static inline void pagevec_release_nonlru(struct pagevec *pvec)
-{
-	if (pagevec_count(pvec))
-		__pagevec_release_nonlru(pvec);
-}
-
 static inline void pagevec_free(struct pagevec *pvec)
 {
 	if (pagevec_count(pvec))
diff --git a/include/linux/pci-acpi.h b/include/linux/pci-acpi.h
index 8837928fbf33..042c166f65d5 100644
--- a/include/linux/pci-acpi.h
+++ b/include/linux/pci-acpi.h
@@ -8,6 +8,8 @@
 #ifndef _PCI_ACPI_H_
 #define _PCI_ACPI_H_
 
+#include <linux/acpi.h>
+
 #define OSC_QUERY_TYPE			0
 #define OSC_SUPPORT_TYPE 		1
 #define OSC_CONTROL_TYPE		2
@@ -48,15 +50,7 @@
 
 #ifdef CONFIG_ACPI
 extern acpi_status pci_osc_control_set(acpi_handle handle, u32 flags);
-extern acpi_status __pci_osc_support_set(u32 flags, const char *hid);
-static inline acpi_status pci_osc_support_set(u32 flags)
-{
-	return __pci_osc_support_set(flags, PCI_ROOT_HID_STRING);
-}
-static inline acpi_status pcie_osc_support_set(u32 flags)
-{
-	return __pci_osc_support_set(flags, PCI_EXPRESS_ROOT_HID_STRING);
-}
+int pci_acpi_osc_support(acpi_handle handle, u32 flags);
 static inline acpi_handle acpi_find_root_bridge_handle(struct pci_dev *pdev)
 {
 	/* Find root host bridge */
@@ -66,6 +60,15 @@ static inline acpi_handle acpi_find_root_bridge_handle(struct pci_dev *pdev)
 	return acpi_get_pci_rootbridge_handle(pci_domain_nr(pdev->bus),
 			pdev->bus->number);
 }
+
+static inline acpi_handle acpi_pci_get_bridge_handle(struct pci_bus *pbus)
+{
+	int seg = pci_domain_nr(pbus), busnr = pbus->number;
+	struct pci_dev *bridge = pbus->self;
+	if (bridge)
+		return DEVICE_ACPI_HANDLE(&(bridge->dev));
+	return acpi_get_pci_rootbridge_handle(seg, busnr);
+}
 #else
 #if !defined(AE_ERROR)
 typedef u32 		acpi_status;
@@ -73,8 +76,6 @@ typedef u32 		acpi_status;
 #endif    
 static inline acpi_status pci_osc_control_set(acpi_handle handle, u32 flags)
 {return AE_ERROR;}
-static inline acpi_status pci_osc_support_set(u32 flags) {return AE_ERROR;} 
-static inline acpi_status pcie_osc_support_set(u32 flags) {return AE_ERROR;}
 static inline acpi_handle acpi_find_root_bridge_handle(struct pci_dev *pdev)
 { return NULL; }
 #endif
diff --git a/include/linux/pci.h b/include/linux/pci.h
index 03b0b8c3c81b..80f8b8b65fde 100644
--- a/include/linux/pci.h
+++ b/include/linux/pci.h
@@ -82,7 +82,30 @@ enum pci_mmap_state {
 #define PCI_DMA_FROMDEVICE	2
 #define PCI_DMA_NONE		3
 
-#define DEVICE_COUNT_RESOURCE	12
+/*
+ *  For PCI devices, the region numbers are assigned this way:
+ */
+enum {
+	/* #0-5: standard PCI resources */
+	PCI_STD_RESOURCES,
+	PCI_STD_RESOURCE_END = 5,
+
+	/* #6: expansion ROM resource */
+	PCI_ROM_RESOURCE,
+
+	/* resources assigned to buses behind the bridge */
+#define PCI_BRIDGE_RESOURCE_NUM 4
+
+	PCI_BRIDGE_RESOURCES,
+	PCI_BRIDGE_RESOURCE_END = PCI_BRIDGE_RESOURCES +
+				  PCI_BRIDGE_RESOURCE_NUM - 1,
+
+	/* total resources associated with a PCI device */
+	PCI_NUM_RESOURCES,
+
+	/* preserve this for compatibility */
+	DEVICE_COUNT_RESOURCE
+};
 
 typedef int __bitwise pci_power_t;
 
@@ -274,18 +297,6 @@ static inline void pci_add_saved_cap(struct pci_dev *pci_dev,
 	hlist_add_head(&new_cap->next, &pci_dev->saved_cap_space);
 }
 
-/*
- *  For PCI devices, the region numbers are assigned this way:
- *
- *	0-5	standard PCI regions
- *	6	expansion ROM
- *	7-10	bridges: address space assigned to buses behind the bridge
- */
-
-#define PCI_ROM_RESOURCE	6
-#define PCI_BRIDGE_RESOURCES	7
-#define PCI_NUM_RESOURCES	11
-
 #ifndef PCI_BUS_NUM_RESOURCES
 #define PCI_BUS_NUM_RESOURCES	16
 #endif
@@ -325,6 +336,15 @@ struct pci_bus {
 #define pci_bus_b(n)	list_entry(n, struct pci_bus, node)
 #define to_pci_bus(n)	container_of(n, struct pci_bus, dev)
 
+#ifdef CONFIG_PCI_MSI
+static inline bool pci_dev_msi_enabled(struct pci_dev *pci_dev)
+{
+	return pci_dev->msi_enabled || pci_dev->msix_enabled;
+}
+#else
+static inline bool pci_dev_msi_enabled(struct pci_dev *pci_dev) { return false; }
+#endif
+
 /*
  * Error values that may be returned by PCI functions.
  */
@@ -421,7 +441,6 @@ struct pci_driver {
 	int  (*resume_early) (struct pci_dev *dev);
 	int  (*resume) (struct pci_dev *dev);	                /* Device woken up */
 	void (*shutdown) (struct pci_dev *dev);
-	struct pm_ext_ops *pm;
 	struct pci_error_handlers *err_handler;
 	struct device_driver	driver;
 	struct pci_dynids dynids;
@@ -533,7 +552,9 @@ int __must_check 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);
+u8 pci_swizzle_interrupt_pin(struct pci_dev *dev, u8 pin);
 int pci_get_interrupt_pin(struct pci_dev *dev, struct pci_dev **bridge);
+u8 pci_common_swizzle(struct pci_dev *dev, u8 *pinp);
 extern struct pci_dev *pci_dev_get(struct pci_dev *dev);
 extern void pci_dev_put(struct pci_dev *dev);
 extern void pci_remove_bus(struct pci_bus *b);
@@ -630,6 +651,7 @@ static inline int pci_is_managed(struct pci_dev *pdev)
 
 void pci_disable_device(struct pci_dev *dev);
 void pci_set_master(struct pci_dev *dev);
+void pci_clear_master(struct pci_dev *dev);
 int pci_set_pcie_reset_state(struct pci_dev *dev, enum pcie_reset_state state);
 #define HAVE_PCI_SET_MWI
 int __must_check pci_set_mwi(struct pci_dev *dev);
@@ -648,7 +670,7 @@ int pcie_get_readrq(struct pci_dev *dev);
 int pcie_set_readrq(struct pci_dev *dev, int rq);
 int pci_reset_function(struct pci_dev *dev);
 int pci_execute_reset_function(struct pci_dev *dev);
-void pci_update_resource(struct pci_dev *dev, struct resource *res, int resno);
+void pci_update_resource(struct pci_dev *dev, int resno);
 int __must_check pci_assign_resource(struct pci_dev *dev, int i);
 int pci_select_bars(struct pci_dev *dev, unsigned long flags);
 
@@ -675,6 +697,11 @@ int pci_back_from_sleep(struct pci_dev *dev);
 /* Functions for PCI Hotplug drivers to use */
 int pci_bus_find_capability(struct pci_bus *bus, unsigned int devfn, int cap);
 
+/* Vital product data routines */
+ssize_t pci_read_vpd(struct pci_dev *dev, loff_t pos, size_t count, void *buf);
+ssize_t pci_write_vpd(struct pci_dev *dev, loff_t pos, size_t count, const void *buf);
+int pci_vpd_truncate(struct pci_dev *dev, size_t size);
+
 /* Helper functions for low-level code (drivers/pci/setup-[bus,res].c) */
 void pci_bus_assign_resources(struct pci_bus *bus);
 void pci_bus_size_bridges(struct pci_bus *bus);
@@ -687,10 +714,13 @@ void pci_fixup_irqs(u8 (*)(struct pci_dev *, u8 *),
 		    int (*)(struct pci_dev *, u8, u8));
 #define HAVE_PCI_REQ_REGIONS	2
 int __must_check pci_request_regions(struct pci_dev *, const char *);
+int __must_check pci_request_regions_exclusive(struct pci_dev *, const char *);
 void pci_release_regions(struct pci_dev *);
 int __must_check pci_request_region(struct pci_dev *, int, const char *);
+int __must_check pci_request_region_exclusive(struct pci_dev *, int, const char *);
 void pci_release_region(struct pci_dev *, int);
 int pci_request_selected_regions(struct pci_dev *, int, const char *);
+int pci_request_selected_regions_exclusive(struct pci_dev *, int, const char *);
 void pci_release_selected_regions(struct pci_dev *, int);
 
 /* drivers/pci/bus.c */
@@ -780,6 +810,10 @@ static inline void msi_remove_pci_irq_vectors(struct pci_dev *dev)
 
 static inline void pci_restore_msi_state(struct pci_dev *dev)
 { }
+static inline int pci_msi_enabled(void)
+{
+	return 0;
+}
 #else
 extern int pci_enable_msi(struct pci_dev *dev);
 extern void pci_msi_shutdown(struct pci_dev *dev);
@@ -790,6 +824,16 @@ extern void pci_msix_shutdown(struct pci_dev *dev);
 extern void pci_disable_msix(struct pci_dev *dev);
 extern void msi_remove_pci_irq_vectors(struct pci_dev *dev);
 extern void pci_restore_msi_state(struct pci_dev *dev);
+extern int pci_msi_enabled(void);
+#endif
+
+#ifndef CONFIG_PCIEASPM
+static inline int pcie_aspm_enabled(void)
+{
+	return 0;
+}
+#else
+extern int pcie_aspm_enabled(void);
 #endif
 
 #ifdef CONFIG_HT_IRQ
@@ -1141,20 +1185,9 @@ static inline void pci_mmcfg_early_init(void) { }
 static inline void pci_mmcfg_late_init(void) { }
 #endif
 
-#ifdef CONFIG_HAS_IOMEM
-static inline void __iomem *pci_ioremap_bar(struct pci_dev *pdev, int bar)
-{
-	/*
-	 * Make sure the BAR is actually a memory resource, not an IO resource
-	 */
-	if (!(pci_resource_flags(pdev, bar) & IORESOURCE_MEM)) {
-		WARN_ON(1);
-		return NULL;
-	}
-	return ioremap_nocache(pci_resource_start(pdev, bar),
-				     pci_resource_len(pdev, bar));
-}
-#endif
+int pci_ext_cfg_avail(struct pci_dev *dev);
+
+void __iomem *pci_ioremap_bar(struct pci_dev *pdev, int bar);
 
 #endif /* __KERNEL__ */
 #endif /* LINUX_PCI_H */
diff --git a/include/linux/pci_hotplug.h b/include/linux/pci_hotplug.h
index a00bd1a0f156..f7cc204fab07 100644
--- a/include/linux/pci_hotplug.h
+++ b/include/linux/pci_hotplug.h
@@ -228,6 +228,8 @@ extern acpi_status acpi_get_hp_params_from_firmware(struct pci_bus *bus,
 				struct hotplug_params *hpp);
 int acpi_get_hp_hw_control_from_firmware(struct pci_dev *dev, u32 flags);
 int acpi_root_bridge(acpi_handle handle);
+int acpi_pci_check_ejectable(struct pci_bus *pbus, acpi_handle handle);
+int acpi_pci_detect_ejectable(struct pci_bus *pbus);
 #endif
 #endif
 
diff --git a/include/linux/pci_ids.h b/include/linux/pci_ids.h
index 218c73b1e6d4..d543365518ab 100644
--- a/include/linux/pci_ids.h
+++ b/include/linux/pci_ids.h
@@ -1658,6 +1658,7 @@
 #define PCI_VENDOR_ID_ROCKWELL		0x127A
 
 #define PCI_VENDOR_ID_ITE		0x1283
+#define PCI_DEVICE_ID_ITE_8172		0x8172
 #define PCI_DEVICE_ID_ITE_8211		0x8211
 #define PCI_DEVICE_ID_ITE_8212		0x8212
 #define PCI_DEVICE_ID_ITE_8213		0x8213
diff --git a/include/linux/pci_regs.h b/include/linux/pci_regs.h
index e5effd47ed74..027815b4635e 100644
--- a/include/linux/pci_regs.h
+++ b/include/linux/pci_regs.h
@@ -210,6 +210,7 @@
 #define  PCI_CAP_ID_AGP3	0x0E	/* AGP Target PCI-PCI bridge */
 #define  PCI_CAP_ID_EXP 	0x10	/* PCI Express */
 #define  PCI_CAP_ID_MSIX	0x11	/* MSI-X */
+#define  PCI_CAP_ID_AF		0x13	/* PCI Advanced Features */
 #define PCI_CAP_LIST_NEXT	1	/* Next capability in the list */
 #define PCI_CAP_FLAGS		2	/* Capability defined flags (16 bits) */
 #define PCI_CAP_SIZEOF		4
@@ -316,6 +317,17 @@
 #define  PCI_CHSWP_EXT		0x40	/* ENUM# status - extraction */
 #define  PCI_CHSWP_INS		0x80	/* ENUM# status - insertion */
 
+/* PCI Advanced Feature registers */
+
+#define PCI_AF_LENGTH		2
+#define PCI_AF_CAP		3
+#define  PCI_AF_CAP_TP		0x01
+#define  PCI_AF_CAP_FLR		0x02
+#define PCI_AF_CTRL		4
+#define  PCI_AF_CTRL_FLR	0x01
+#define PCI_AF_STATUS		5
+#define  PCI_AF_STATUS_TP	0x01
+
 /* PCI-X registers */
 
 #define PCI_X_CMD		2	/* Modes & Features */
@@ -399,20 +411,70 @@
 #define  PCI_EXP_DEVSTA_AUXPD	0x10	/* AUX Power Detected */
 #define  PCI_EXP_DEVSTA_TRPND	0x20	/* Transactions Pending */
 #define PCI_EXP_LNKCAP		12	/* Link Capabilities */
-#define  PCI_EXP_LNKCAP_ASPMS	0xc00	/* ASPM Support */
-#define  PCI_EXP_LNKCAP_L0SEL	0x7000	/* L0s Exit Latency */
-#define  PCI_EXP_LNKCAP_L1EL	0x38000	/* L1 Exit Latency */
-#define  PCI_EXP_LNKCAP_CLKPM	0x40000	/* L1 Clock Power Management */
+#define  PCI_EXP_LNKCAP_SLS	0x0000000f /* Supported Link Speeds */
+#define  PCI_EXP_LNKCAP_MLW	0x000003f0 /* Maximum Link Width */
+#define  PCI_EXP_LNKCAP_ASPMS	0x00000c00 /* ASPM Support */
+#define  PCI_EXP_LNKCAP_L0SEL	0x00007000 /* L0s Exit Latency */
+#define  PCI_EXP_LNKCAP_L1EL	0x00038000 /* L1 Exit Latency */
+#define  PCI_EXP_LNKCAP_CLKPM	0x00040000 /* L1 Clock Power Management */
+#define  PCI_EXP_LNKCAP_SDERC	0x00080000 /* Suprise Down Error Reporting Capable */
+#define  PCI_EXP_LNKCAP_DLLLARC	0x00100000 /* Data Link Layer Link Active Reporting Capable */
+#define  PCI_EXP_LNKCAP_LBNC	0x00200000 /* Link Bandwidth Notification Capability */
+#define  PCI_EXP_LNKCAP_PN	0xff000000 /* Port Number */
 #define PCI_EXP_LNKCTL		16	/* Link Control */
-#define  PCI_EXP_LNKCTL_RL	0x20	/* Retrain Link */
-#define  PCI_EXP_LNKCTL_CCC	0x40	/* Common Clock COnfiguration */
+#define  PCI_EXP_LNKCTL_ASPMC	0x0003	/* ASPM Control */
+#define  PCI_EXP_LNKCTL_RCB	0x0008	/* Read Completion Boundary */
+#define  PCI_EXP_LNKCTL_LD	0x0010	/* Link Disable */
+#define  PCI_EXP_LNKCTL_RL	0x0020	/* Retrain Link */
+#define  PCI_EXP_LNKCTL_CCC	0x0040	/* Common Clock Configuration */
+#define  PCI_EXP_LNKCTL_ES	0x0080	/* Extended Synch */
 #define  PCI_EXP_LNKCTL_CLKREQ_EN 0x100	/* Enable clkreq */
+#define  PCI_EXP_LNKCTL_HAWD	0x0200	/* Hardware Autonomous Width Disable */
+#define  PCI_EXP_LNKCTL_LBMIE	0x0400	/* Link Bandwidth Management Interrupt Enable */
+#define  PCI_EXP_LNKCTL_LABIE	0x0800	/* Lnk Autonomous Bandwidth Interrupt Enable */
 #define PCI_EXP_LNKSTA		18	/* Link Status */
-#define  PCI_EXP_LNKSTA_LT	0x800	/* Link Training */
+#define  PCI_EXP_LNKSTA_CLS	0x000f	/* Current Link Speed */
+#define  PCI_EXP_LNKSTA_NLW	0x03f0	/* Nogotiated Link Width */
+#define  PCI_EXP_LNKSTA_LT	0x0800	/* Link Training */
 #define  PCI_EXP_LNKSTA_SLC	0x1000	/* Slot Clock Configuration */
+#define  PCI_EXP_LNKSTA_DLLLA	0x2000	/* Data Link Layer Link Active */
+#define  PCI_EXP_LNKSTA_LBMS	0x4000	/* Link Bandwidth Management Status */
+#define  PCI_EXP_LNKSTA_LABS	0x8000	/* Link Autonomous Bandwidth Status */
 #define PCI_EXP_SLTCAP		20	/* Slot Capabilities */
+#define  PCI_EXP_SLTCAP_ABP	0x00000001 /* Attention Button Present */
+#define  PCI_EXP_SLTCAP_PCP	0x00000002 /* Power Controller Present */
+#define  PCI_EXP_SLTCAP_MRLSP	0x00000004 /* MRL Sensor Present */
+#define  PCI_EXP_SLTCAP_AIP	0x00000008 /* Attention Indicator Present */
+#define  PCI_EXP_SLTCAP_PIP	0x00000010 /* Power Indicator Present */
+#define  PCI_EXP_SLTCAP_HPS	0x00000020 /* Hot-Plug Surprise */
+#define  PCI_EXP_SLTCAP_HPC	0x00000040 /* Hot-Plug Capable */
+#define  PCI_EXP_SLTCAP_SPLV	0x00007f80 /* Slot Power Limit Value */
+#define  PCI_EXP_SLTCAP_SPLS	0x00018000 /* Slot Power Limit Scale */
+#define  PCI_EXP_SLTCAP_EIP	0x00020000 /* Electromechanical Interlock Present */
+#define  PCI_EXP_SLTCAP_NCCS	0x00040000 /* No Command Completed Support */
+#define  PCI_EXP_SLTCAP_PSN	0xfff80000 /* Physical Slot Number */
 #define PCI_EXP_SLTCTL		24	/* Slot Control */
+#define  PCI_EXP_SLTCTL_ABPE	0x0001	/* Attention Button Pressed Enable */
+#define  PCI_EXP_SLTCTL_PFDE	0x0002	/* Power Fault Detected Enable */
+#define  PCI_EXP_SLTCTL_MRLSCE	0x0004	/* MRL Sensor Changed Enable */
+#define  PCI_EXP_SLTCTL_PDCE	0x0008	/* Presence Detect Changed Enable */
+#define  PCI_EXP_SLTCTL_CCIE	0x0010	/* Command Completed Interrupt Enable */
+#define  PCI_EXP_SLTCTL_HPIE	0x0020	/* Hot-Plug Interrupt Enable */
+#define  PCI_EXP_SLTCTL_AIC	0x00c0	/* Attention Indicator Control */
+#define  PCI_EXP_SLTCTL_PIC	0x0300	/* Power Indicator Control */
+#define  PCI_EXP_SLTCTL_PCC	0x0400	/* Power Controller Control */
+#define  PCI_EXP_SLTCTL_EIC	0x0800	/* Electromechanical Interlock Control */
+#define  PCI_EXP_SLTCTL_DLLSCE	0x1000	/* Data Link Layer State Changed Enable */
 #define PCI_EXP_SLTSTA		26	/* Slot Status */
+#define  PCI_EXP_SLTSTA_ABP	0x0001	/* Attention Button Pressed */
+#define  PCI_EXP_SLTSTA_PFD	0x0002	/* Power Fault Detected */
+#define  PCI_EXP_SLTSTA_MRLSC	0x0004	/* MRL Sensor Changed */
+#define  PCI_EXP_SLTSTA_PDC	0x0008	/* Presence Detect Changed */
+#define  PCI_EXP_SLTSTA_CC	0x0010	/* Command Completed */
+#define  PCI_EXP_SLTSTA_MRLSS	0x0020	/* MRL Sensor State */
+#define  PCI_EXP_SLTSTA_PDS	0x0040	/* Presence Detect State */
+#define  PCI_EXP_SLTSTA_EIS	0x0080	/* Electromechanical Interlock Status */
+#define  PCI_EXP_SLTSTA_DLLSC	0x0100	/* Data Link Layer State Changed */
 #define PCI_EXP_RTCTL		28	/* Root Control */
 #define  PCI_EXP_RTCTL_SECEE	0x01	/* System Error on Correctable Error */
 #define  PCI_EXP_RTCTL_SENFEE	0x02	/* System Error on Non-Fatal Error */
diff --git a/include/linux/percpu_counter.h b/include/linux/percpu_counter.h
index 9007ccdfc112..a7684a513994 100644
--- a/include/linux/percpu_counter.h
+++ b/include/linux/percpu_counter.h
@@ -24,14 +24,18 @@ struct percpu_counter {
 	s32 *counters;
 };
 
-#if NR_CPUS >= 16
-#define FBC_BATCH	(NR_CPUS*2)
-#else
-#define FBC_BATCH	(NR_CPUS*4)
-#endif
+extern int percpu_counter_batch;
+
+int __percpu_counter_init(struct percpu_counter *fbc, s64 amount,
+			  struct lock_class_key *key);
+
+#define percpu_counter_init(fbc, value)					\
+	({								\
+		static struct lock_class_key __key;			\
+									\
+		__percpu_counter_init(fbc, value, &__key);		\
+	})
 
-int percpu_counter_init(struct percpu_counter *fbc, s64 amount);
-int percpu_counter_init_irq(struct percpu_counter *fbc, s64 amount);
 void percpu_counter_destroy(struct percpu_counter *fbc);
 void percpu_counter_set(struct percpu_counter *fbc, s64 amount);
 void __percpu_counter_add(struct percpu_counter *fbc, s64 amount, s32 batch);
@@ -39,7 +43,7 @@ s64 __percpu_counter_sum(struct percpu_counter *fbc);
 
 static inline void percpu_counter_add(struct percpu_counter *fbc, s64 amount)
 {
-	__percpu_counter_add(fbc, amount, FBC_BATCH);
+	__percpu_counter_add(fbc, amount, percpu_counter_batch);
 }
 
 static inline s64 percpu_counter_sum_positive(struct percpu_counter *fbc)
@@ -85,8 +89,6 @@ static inline int percpu_counter_init(struct percpu_counter *fbc, s64 amount)
 	return 0;
 }
 
-#define percpu_counter_init_irq percpu_counter_init
-
 static inline void percpu_counter_destroy(struct percpu_counter *fbc)
 {
 }
diff --git a/include/linux/platform_device.h b/include/linux/platform_device.h
index 4b8cc6a32479..9a342699c607 100644
--- a/include/linux/platform_device.h
+++ b/include/linux/platform_device.h
@@ -55,7 +55,6 @@ struct platform_driver {
 	int (*suspend_late)(struct platform_device *, pm_message_t state);
 	int (*resume_early)(struct platform_device *);
 	int (*resume)(struct platform_device *);
-	struct pm_ext_ops *pm;
 	struct device_driver driver;
 };
 
diff --git a/include/linux/pm.h b/include/linux/pm.h
index 42de4003c4ee..de2e0a8f6728 100644
--- a/include/linux/pm.h
+++ b/include/linux/pm.h
@@ -41,7 +41,7 @@ typedef struct pm_message {
 } pm_message_t;
 
 /**
- * struct pm_ops - device PM callbacks
+ * struct dev_pm_ops - device PM callbacks
  *
  * Several driver power state transitions are externally visible, affecting
  * the state of pending I/O queues and (for drivers that touch hardware)
@@ -126,46 +126,6 @@ typedef struct pm_message {
  *	On most platforms, there are no restrictions on availability of
  *	resources like clocks during @restore().
  *
- * All of the above callbacks, except for @complete(), return error codes.
- * However, the error codes returned by the resume operations, @resume(),
- * @thaw(), and @restore(), do not cause the PM core to abort the resume
- * transition during which they are returned.  The error codes returned in
- * that cases are only printed by the PM core to the system logs for debugging
- * purposes.  Still, it is recommended that drivers only return error codes
- * from their resume methods in case of an unrecoverable failure (i.e. when the
- * device being handled refuses to resume and becomes unusable) to allow us to
- * modify the PM core in the future, so that it can avoid attempting to handle
- * devices that failed to resume and their children.
- *
- * It is allowed to unregister devices while the above callbacks are being
- * executed.  However, it is not allowed to unregister a device from within any
- * of its own callbacks.
- */
-
-struct pm_ops {
-	int (*prepare)(struct device *dev);
-	void (*complete)(struct device *dev);
-	int (*suspend)(struct device *dev);
-	int (*resume)(struct device *dev);
-	int (*freeze)(struct device *dev);
-	int (*thaw)(struct device *dev);
-	int (*poweroff)(struct device *dev);
-	int (*restore)(struct device *dev);
-};
-
-/**
- * struct pm_ext_ops - extended device PM callbacks
- *
- * Some devices require certain operations related to suspend and hibernation
- * to be carried out with interrupts disabled.  Thus, 'struct pm_ext_ops' below
- * is defined, adding callbacks to be executed with interrupts disabled to
- * 'struct pm_ops'.
- *
- * The following callbacks included in 'struct pm_ext_ops' are executed with
- * the nonboot CPUs switched off and with interrupts disabled on the only
- * functional CPU.  They also are executed with the PM core list of devices
- * locked, so they must NOT unregister any devices.
- *
  * @suspend_noirq: Complete the operations of ->suspend() by carrying out any
  *	actions required for suspending the device that need interrupts to be
  *	disabled
@@ -190,18 +150,32 @@ struct pm_ops {
  *	actions required for restoring the operations of the device that need
  *	interrupts to be disabled
  *
- * All of the above callbacks return error codes, but the error codes returned
- * by the resume operations, @resume_noirq(), @thaw_noirq(), and
- * @restore_noirq(), do not cause the PM core to abort the resume transition
- * during which they are returned.  The error codes returned in that cases are
- * only printed by the PM core to the system logs for debugging purposes.
- * Still, as stated above, it is recommended that drivers only return error
- * codes from their resume methods if the device being handled fails to resume
- * and is not usable any more.
+ * All of the above callbacks, except for @complete(), return error codes.
+ * However, the error codes returned by the resume operations, @resume(),
+ * @thaw(), @restore(), @resume_noirq(), @thaw_noirq(), and @restore_noirq() do
+ * not cause the PM core to abort the resume transition during which they are
+ * returned.  The error codes returned in that cases are only printed by the PM
+ * core to the system logs for debugging purposes.  Still, it is recommended
+ * that drivers only return error codes from their resume methods in case of an
+ * unrecoverable failure (i.e. when the device being handled refuses to resume
+ * and becomes unusable) to allow us to modify the PM core in the future, so
+ * that it can avoid attempting to handle devices that failed to resume and
+ * their children.
+ *
+ * It is allowed to unregister devices while the above callbacks are being
+ * executed.  However, it is not allowed to unregister a device from within any
+ * of its own callbacks.
  */
 
-struct pm_ext_ops {
-	struct pm_ops base;
+struct dev_pm_ops {
+	int (*prepare)(struct device *dev);
+	void (*complete)(struct device *dev);
+	int (*suspend)(struct device *dev);
+	int (*resume)(struct device *dev);
+	int (*freeze)(struct device *dev);
+	int (*thaw)(struct device *dev);
+	int (*poweroff)(struct device *dev);
+	int (*restore)(struct device *dev);
 	int (*suspend_noirq)(struct device *dev);
 	int (*resume_noirq)(struct device *dev);
 	int (*freeze_noirq)(struct device *dev);
@@ -278,7 +252,7 @@ struct pm_ext_ops {
 #define PM_EVENT_SLEEP		(PM_EVENT_SUSPEND | PM_EVENT_HIBERNATE)
 #define PM_EVENT_USER_SUSPEND	(PM_EVENT_USER | PM_EVENT_SUSPEND)
 #define PM_EVENT_USER_RESUME	(PM_EVENT_USER | PM_EVENT_RESUME)
-#define PM_EVENT_REMOTE_WAKEUP	(PM_EVENT_REMOTE | PM_EVENT_RESUME)
+#define PM_EVENT_REMOTE_RESUME	(PM_EVENT_REMOTE | PM_EVENT_RESUME)
 #define PM_EVENT_AUTO_SUSPEND	(PM_EVENT_AUTO | PM_EVENT_SUSPEND)
 #define PM_EVENT_AUTO_RESUME	(PM_EVENT_AUTO | PM_EVENT_RESUME)
 
@@ -291,15 +265,15 @@ struct pm_ext_ops {
 #define PMSG_THAW	((struct pm_message){ .event = PM_EVENT_THAW, })
 #define PMSG_RESTORE	((struct pm_message){ .event = PM_EVENT_RESTORE, })
 #define PMSG_RECOVER	((struct pm_message){ .event = PM_EVENT_RECOVER, })
-#define PMSG_USER_SUSPEND	((struct pm_messge) \
+#define PMSG_USER_SUSPEND	((struct pm_message) \
 					{ .event = PM_EVENT_USER_SUSPEND, })
-#define PMSG_USER_RESUME	((struct pm_messge) \
+#define PMSG_USER_RESUME	((struct pm_message) \
 					{ .event = PM_EVENT_USER_RESUME, })
-#define PMSG_REMOTE_RESUME	((struct pm_messge) \
+#define PMSG_REMOTE_RESUME	((struct pm_message) \
 					{ .event = PM_EVENT_REMOTE_RESUME, })
-#define PMSG_AUTO_SUSPEND	((struct pm_messge) \
+#define PMSG_AUTO_SUSPEND	((struct pm_message) \
 					{ .event = PM_EVENT_AUTO_SUSPEND, })
-#define PMSG_AUTO_RESUME		((struct pm_messge) \
+#define PMSG_AUTO_RESUME	((struct pm_message) \
 					{ .event = PM_EVENT_AUTO_RESUME, })
 
 /**
diff --git a/include/linux/poll.h b/include/linux/poll.h
index badd98ab06f6..8c24ef8d9976 100644
--- a/include/linux/poll.h
+++ b/include/linux/poll.h
@@ -46,9 +46,9 @@ static inline void init_poll_funcptr(poll_table *pt, poll_queue_proc qproc)
 }
 
 struct poll_table_entry {
-	struct file * filp;
+	struct file *filp;
 	wait_queue_t wait;
-	wait_queue_head_t * wait_address;
+	wait_queue_head_t *wait_address;
 };
 
 /*
@@ -56,7 +56,9 @@ struct poll_table_entry {
  */
 struct poll_wqueues {
 	poll_table pt;
-	struct poll_table_page * table;
+	struct poll_table_page *table;
+	struct task_struct *polling_task;
+	int triggered;
 	int error;
 	int inline_index;
 	struct poll_table_entry inline_entries[N_INLINE_POLL_ENTRIES];
@@ -64,6 +66,13 @@ struct poll_wqueues {
 
 extern void poll_initwait(struct poll_wqueues *pwq);
 extern void poll_freewait(struct poll_wqueues *pwq);
+extern int poll_schedule_timeout(struct poll_wqueues *pwq, int state,
+				 ktime_t *expires, unsigned long slack);
+
+static inline int poll_schedule(struct poll_wqueues *pwq, int state)
+{
+	return poll_schedule_timeout(pwq, state, NULL, 0);
+}
 
 /*
  * Scaleable version of the fd_set.
diff --git a/include/linux/qnx4_fs.h b/include/linux/qnx4_fs.h
index 34a196ee7941..787d19ea9f46 100644
--- a/include/linux/qnx4_fs.h
+++ b/include/linux/qnx4_fs.h
@@ -2,14 +2,12 @@
  *  Name                         : qnx4_fs.h
  *  Author                       : Richard Frowijn
  *  Function                     : qnx4 global filesystem definitions
- *  Version                      : 1.0.2
- *  Last modified                : 2000-01-31
- *
  *  History                      : 23-03-1998 created
  */
 #ifndef _LINUX_QNX4_FS_H
 #define _LINUX_QNX4_FS_H
 
+#include <linux/types.h>
 #include <linux/qnxtypes.h>
 #include <linux/magic.h>
 
diff --git a/include/linux/qnxtypes.h b/include/linux/qnxtypes.h
index a3eb1137857b..bebbe5cc4fb8 100644
--- a/include/linux/qnxtypes.h
+++ b/include/linux/qnxtypes.h
@@ -2,9 +2,6 @@
  *  Name                         : qnxtypes.h
  *  Author                       : Richard Frowijn
  *  Function                     : standard qnx types
- *  Version                      : 1.0.2
- *  Last modified                : 2000-01-06
- *
  *  History                      : 22-03-1998 created
  *
  */
@@ -12,6 +9,8 @@
 #ifndef _QNX4TYPES_H
 #define _QNX4TYPES_H
 
+#include <linux/types.h>
+
 typedef __le16 qnx4_nxtnt_t;
 typedef __u8  qnx4_ftype_t;
 
diff --git a/include/linux/rcupdate.h b/include/linux/rcupdate.h
index 1168fbcea8d4..921340a7b71c 100644
--- a/include/linux/rcupdate.h
+++ b/include/linux/rcupdate.h
@@ -204,18 +204,6 @@ struct rcu_synchronize {
 
 extern void wakeme_after_rcu(struct rcu_head  *head);
 
-#define synchronize_rcu_xxx(name, func) \
-void name(void) \
-{ \
-	struct rcu_synchronize rcu; \
-	\
-	init_completion(&rcu.completion); \
-	/* Will wake me after RCU finished. */ \
-	func(&rcu.head, wakeme_after_rcu); \
-	/* Wait for it. */ \
-	wait_for_completion(&rcu.completion); \
-}
-
 /**
  * synchronize_sched - block until all CPUs have exited any non-preemptive
  * kernel code sequences.
diff --git a/include/linux/rio_drv.h b/include/linux/rio_drv.h
index 32c0547ffafc..c93a58a40033 100644
--- a/include/linux/rio_drv.h
+++ b/include/linux/rio_drv.h
@@ -391,7 +391,6 @@ static inline int rio_add_inb_buffer(struct rio_mport *mport, int mbox,
  * rio_get_inb_message - Get A RIO message from an inbound mailbox queue
  * @mport: Master port containing the inbound mailbox
  * @mbox: The inbound mailbox number
- * @buffer: Pointer to the message buffer
  *
  * Get a RIO message from an inbound mailbox queue. Returns 0 on success.
  */
diff --git a/include/linux/rmap.h b/include/linux/rmap.h
index 89f0564b10c8..b35bc0e19cd9 100644
--- a/include/linux/rmap.h
+++ b/include/linux/rmap.h
@@ -63,16 +63,13 @@ void anon_vma_unlink(struct vm_area_struct *);
 void anon_vma_link(struct vm_area_struct *);
 void __anon_vma_link(struct vm_area_struct *);
 
-extern struct anon_vma *page_lock_anon_vma(struct page *page);
-extern void page_unlock_anon_vma(struct anon_vma *anon_vma);
-
 /*
  * rmap interfaces called when adding or removing pte of page
  */
 void page_add_anon_rmap(struct page *, struct vm_area_struct *, unsigned long);
 void page_add_new_anon_rmap(struct page *, struct vm_area_struct *, unsigned long);
 void page_add_file_rmap(struct page *);
-void page_remove_rmap(struct page *, struct vm_area_struct *);
+void page_remove_rmap(struct page *);
 
 #ifdef CONFIG_DEBUG_VM
 void page_dup_rmap(struct page *page, struct vm_area_struct *vma, unsigned long address);
diff --git a/include/linux/sched.h b/include/linux/sched.h
index 38a3f4b15394..4cae9b81a1f8 100644
--- a/include/linux/sched.h
+++ b/include/linux/sched.h
@@ -386,6 +386,9 @@ extern void arch_unmap_area_topdown(struct mm_struct *, unsigned long);
 		(mm)->hiwater_vm = (mm)->total_vm;	\
 } while (0)
 
+#define get_mm_hiwater_rss(mm)	max((mm)->hiwater_rss, get_mm_rss(mm))
+#define get_mm_hiwater_vm(mm)	max((mm)->hiwater_vm, (mm)->total_vm)
+
 extern void set_dumpable(struct mm_struct *mm, int value);
 extern int get_dumpable(struct mm_struct *mm);
 
@@ -912,7 +915,6 @@ static inline struct cpumask *sched_domain_span(struct sched_domain *sd)
 
 extern void partition_sched_domains(int ndoms_new, struct cpumask *doms_new,
 				    struct sched_domain_attr *dattr_new);
-extern int arch_reinit_sched_domains(void);
 
 /* Test a flag in parent sched domain */
 static inline int test_sd_parent(struct sched_domain *sd, int flag)
@@ -1704,16 +1706,16 @@ extern void wake_up_idle_cpu(int cpu);
 static inline void wake_up_idle_cpu(int cpu) { }
 #endif
 
-#ifdef CONFIG_SCHED_DEBUG
 extern unsigned int sysctl_sched_latency;
 extern unsigned int sysctl_sched_min_granularity;
 extern unsigned int sysctl_sched_wakeup_granularity;
+extern unsigned int sysctl_sched_shares_ratelimit;
+extern unsigned int sysctl_sched_shares_thresh;
+#ifdef CONFIG_SCHED_DEBUG
 extern unsigned int sysctl_sched_child_runs_first;
 extern unsigned int sysctl_sched_features;
 extern unsigned int sysctl_sched_migration_cost;
 extern unsigned int sysctl_sched_nr_migrate;
-extern unsigned int sysctl_sched_shares_ratelimit;
-extern unsigned int sysctl_sched_shares_thresh;
 
 int sched_nr_latency_handler(struct ctl_table *table, int write,
 		struct file *file, void __user *buffer, size_t *length,
diff --git a/include/linux/security.h b/include/linux/security.h
index b92b5e453f64..1f2ab6353c00 100644
--- a/include/linux/security.h
+++ b/include/linux/security.h
@@ -48,7 +48,8 @@ struct audit_krule;
  * These functions are in security/capability.c and are used
  * as the default capabilities functions
  */
-extern int cap_capable(struct task_struct *tsk, int cap, int audit);
+extern int cap_capable(struct task_struct *tsk, const struct cred *cred,
+		       int cap, int audit);
 extern int cap_settime(struct timespec *ts, struct timezone *tz);
 extern int cap_ptrace_may_access(struct task_struct *child, unsigned int mode);
 extern int cap_ptrace_traceme(struct task_struct *parent);
@@ -1251,9 +1252,12 @@ static inline void security_free_mnt_opts(struct security_mnt_opts *opts)
  *	@permitted contains the permitted capability set.
  *	Return 0 and update @new if permission is granted.
  * @capable:
- *	Check whether the @tsk process has the @cap capability.
+ *	Check whether the @tsk process has the @cap capability in the indicated
+ *	credentials.
  *	@tsk contains the task_struct for the process.
+ *	@cred contains the credentials to use.
  *	@cap contains the capability <include/linux/capability.h>.
+ *	@audit: Whether to write an audit message or not
  *	Return 0 if the capability is granted for @tsk.
  * @acct:
  *	Check permission before enabling or disabling process accounting.  If
@@ -1346,7 +1350,8 @@ struct security_operations {
 		       const kernel_cap_t *effective,
 		       const kernel_cap_t *inheritable,
 		       const kernel_cap_t *permitted);
-	int (*capable) (struct task_struct *tsk, int cap, int audit);
+	int (*capable) (struct task_struct *tsk, const struct cred *cred,
+			int cap, int audit);
 	int (*acct) (struct file *file);
 	int (*sysctl) (struct ctl_table *table, int op);
 	int (*quotactl) (int cmds, int type, int id, struct super_block *sb);
@@ -1628,8 +1633,9 @@ int security_capset(struct cred *new, const struct cred *old,
 		    const kernel_cap_t *effective,
 		    const kernel_cap_t *inheritable,
 		    const kernel_cap_t *permitted);
-int security_capable(struct task_struct *tsk, int cap);
-int security_capable_noaudit(struct task_struct *tsk, int cap);
+int security_capable(int cap);
+int security_real_capable(struct task_struct *tsk, int cap);
+int security_real_capable_noaudit(struct task_struct *tsk, int cap);
 int security_acct(struct file *file);
 int security_sysctl(struct ctl_table *table, int op);
 int security_quotactl(int cmds, int type, int id, struct super_block *sb);
@@ -1826,14 +1832,31 @@ static inline int security_capset(struct cred *new,
 	return cap_capset(new, old, effective, inheritable, permitted);
 }
 
-static inline int security_capable(struct task_struct *tsk, int cap)
+static inline int security_capable(int cap)
 {
-	return cap_capable(tsk, cap, SECURITY_CAP_AUDIT);
+	return cap_capable(current, current_cred(), cap, SECURITY_CAP_AUDIT);
 }
 
-static inline int security_capable_noaudit(struct task_struct *tsk, int cap)
+static inline int security_real_capable(struct task_struct *tsk, int cap)
 {
-	return cap_capable(tsk, cap, SECURITY_CAP_NOAUDIT);
+	int ret;
+
+	rcu_read_lock();
+	ret = cap_capable(tsk, __task_cred(tsk), cap, SECURITY_CAP_AUDIT);
+	rcu_read_unlock();
+	return ret;
+}
+
+static inline
+int security_real_capable_noaudit(struct task_struct *tsk, int cap)
+{
+	int ret;
+
+	rcu_read_lock();
+	ret = cap_capable(tsk, __task_cred(tsk), cap,
+			       SECURITY_CAP_NOAUDIT);
+	rcu_read_unlock();
+	return ret;
 }
 
 static inline int security_acct(struct file *file)
diff --git a/include/linux/serio.h b/include/linux/serio.h
index 25641d9e0ea8..1bcb357a01a1 100644
--- a/include/linux/serio.h
+++ b/include/linux/serio.h
@@ -213,5 +213,6 @@ static inline void serio_unpin_driver(struct serio *serio)
 #define SERIO_ZHENHUA	0x36
 #define SERIO_INEXIO	0x37
 #define SERIO_TOUCHIT213	0x37
+#define SERIO_W8001	0x39
 
 #endif
diff --git a/include/linux/spi/spi.h b/include/linux/spi/spi.h
index 82229317753d..68bb1c501d0d 100644
--- a/include/linux/spi/spi.h
+++ b/include/linux/spi/spi.h
@@ -327,9 +327,9 @@ extern struct spi_master *spi_busnum_to_master(u16 busnum);
  * @tx_dma: DMA address of tx_buf, if @spi_message.is_dma_mapped
  * @rx_dma: DMA address of rx_buf, if @spi_message.is_dma_mapped
  * @len: size of rx and tx buffers (in bytes)
- * @speed_hz: Select a speed other then the device default for this
+ * @speed_hz: Select a speed other than the device default for this
  *      transfer. If 0 the default (from @spi_device) is used.
- * @bits_per_word: select a bits_per_word other then the device default
+ * @bits_per_word: select a bits_per_word other than the device default
  *      for this transfer. If 0 the default (from @spi_device) is used.
  * @cs_change: affects chipselect after this transfer completes
  * @delay_usecs: microseconds to delay after this transfer before
diff --git a/include/linux/spi/spi_gpio.h b/include/linux/spi/spi_gpio.h
new file mode 100644
index 000000000000..0f01a0f1f40c
--- /dev/null
+++ b/include/linux/spi/spi_gpio.h
@@ -0,0 +1,60 @@
+#ifndef __LINUX_SPI_GPIO_H
+#define __LINUX_SPI_GPIO_H
+
+/*
+ * For each bitbanged SPI bus, set up a platform_device node with:
+ *   - name "spi_gpio"
+ *   - id the same as the SPI bus number it implements
+ *   - dev.platform data pointing to a struct spi_gpio_platform_data
+ *
+ * Or, see the driver code for information about speedups that are
+ * possible on platforms that support inlined access for GPIOs (no
+ * spi_gpio_platform_data is used).
+ *
+ * Use spi_board_info with these busses in the usual way, being sure
+ * that the controller_data being the GPIO used for each device's
+ * chipselect:
+ *
+ *	static struct spi_board_info ... [] = {
+ *	...
+ *		// this slave uses GPIO 42 for its chipselect
+ *		.controller_data = (void *) 42,
+ *	...
+ *		// this one uses GPIO 86 for its chipselect
+ *		.controller_data = (void *) 86,
+ *	...
+ *	};
+ *
+ * If the bitbanged bus is later switched to a "native" controller,
+ * that platform_device and controller_data should be removed.
+ */
+
+/**
+ * struct spi_gpio_platform_data - parameter for bitbanged SPI master
+ * @sck: number of the GPIO used for clock output
+ * @mosi: number of the GPIO used for Master Output, Slave In (MOSI) data
+ * @miso: number of the GPIO used for Master Input, Slave Output (MISO) data
+ * @num_chipselect: how many slaves to allow
+ *
+ * All GPIO signals used with the SPI bus managed through this driver
+ * (chipselects, MOSI, MISO, SCK) must be configured as GPIOs, instead
+ * of some alternate function.
+ *
+ * It can be convenient to use this driver with pins that have alternate
+ * functions associated with a "native" SPI controller if a driver for that
+ * controller is not available, or is missing important functionality.
+ *
+ * On platforms which can do so, configure MISO with a weak pullup unless
+ * there's an external pullup on that signal.  That saves power by avoiding
+ * floating signals.  (A weak pulldown would save power too, but many
+ * drivers expect to see all-ones data as the no slave "response".)
+ */
+struct spi_gpio_platform_data {
+	unsigned	sck;
+	unsigned	mosi;
+	unsigned	miso;
+
+	u16		num_chipselect;
+};
+
+#endif /* __LINUX_SPI_GPIO_H */
diff --git a/include/linux/sunrpc/svc.h b/include/linux/sunrpc/svc.h
index 3afe7fb403b2..3435d24bfe55 100644
--- a/include/linux/sunrpc/svc.h
+++ b/include/linux/sunrpc/svc.h
@@ -58,10 +58,13 @@ struct svc_serv {
 	struct svc_stat *	sv_stats;	/* RPC statistics */
 	spinlock_t		sv_lock;
 	unsigned int		sv_nrthreads;	/* # of server threads */
+	unsigned int		sv_maxconn;	/* max connections allowed or
+						 * '0' causing max to be based
+						 * on number of threads. */
+
 	unsigned int		sv_max_payload;	/* datagram payload size */
 	unsigned int		sv_max_mesg;	/* max_payload + 1 page for overheads */
 	unsigned int		sv_xdrsize;	/* XDR buffer size */
-
 	struct list_head	sv_permsocks;	/* all permanent sockets */
 	struct list_head	sv_tempsocks;	/* all temporary sockets */
 	int			sv_tmpcnt;	/* count of temporary sockets */
diff --git a/include/linux/swab.h b/include/linux/swab.h
index bbed279f3b32..be5284d4a053 100644
--- a/include/linux/swab.h
+++ b/include/linux/swab.h
@@ -9,17 +9,17 @@
  * casts are necessary for constants, because we never know how for sure
  * how U/UL/ULL map to __u16, __u32, __u64. At least not in a portable way.
  */
-#define __const_swab16(x) ((__u16)(				\
+#define ___constant_swab16(x) ((__u16)(				\
 	(((__u16)(x) & (__u16)0x00ffU) << 8) |			\
 	(((__u16)(x) & (__u16)0xff00U) >> 8)))
 
-#define __const_swab32(x) ((__u32)(				\
+#define ___constant_swab32(x) ((__u32)(				\
 	(((__u32)(x) & (__u32)0x000000ffUL) << 24) |		\
 	(((__u32)(x) & (__u32)0x0000ff00UL) <<  8) |		\
 	(((__u32)(x) & (__u32)0x00ff0000UL) >>  8) |		\
 	(((__u32)(x) & (__u32)0xff000000UL) >> 24)))
 
-#define __const_swab64(x) ((__u64)(				\
+#define ___constant_swab64(x) ((__u64)(				\
 	(((__u64)(x) & (__u64)0x00000000000000ffULL) << 56) |	\
 	(((__u64)(x) & (__u64)0x000000000000ff00ULL) << 40) |	\
 	(((__u64)(x) & (__u64)0x0000000000ff0000ULL) << 24) |	\
@@ -29,11 +29,11 @@
 	(((__u64)(x) & (__u64)0x00ff000000000000ULL) >> 40) |	\
 	(((__u64)(x) & (__u64)0xff00000000000000ULL) >> 56)))
 
-#define __const_swahw32(x) ((__u32)(				\
+#define ___constant_swahw32(x) ((__u32)(			\
 	(((__u32)(x) & (__u32)0x0000ffffUL) << 16) |		\
 	(((__u32)(x) & (__u32)0xffff0000UL) >> 16)))
 
-#define __const_swahb32(x) ((__u32)(				\
+#define ___constant_swahb32(x) ((__u32)(			\
 	(((__u32)(x) & (__u32)0x00ff00ffUL) << 8) |		\
 	(((__u32)(x) & (__u32)0xff00ff00UL) >> 8)))
 
@@ -43,52 +43,52 @@
  * ___swab16, ___swab32, ___swab64, ___swahw32, ___swahb32
  */
 
-static inline __attribute_const__ __u16 ___swab16(__u16 val)
+static inline __attribute_const__ __u16 __fswab16(__u16 val)
 {
 #ifdef __arch_swab16
 	return __arch_swab16(val);
 #else
-	return __const_swab16(val);
+	return ___constant_swab16(val);
 #endif
 }
 
-static inline __attribute_const__ __u32 ___swab32(__u32 val)
+static inline __attribute_const__ __u32 __fswab32(__u32 val)
 {
 #ifdef __arch_swab32
 	return __arch_swab32(val);
 #else
-	return __const_swab32(val);
+	return ___constant_swab32(val);
 #endif
 }
 
-static inline __attribute_const__ __u64 ___swab64(__u64 val)
+static inline __attribute_const__ __u64 __fswab64(__u64 val)
 {
 #ifdef __arch_swab64
 	return __arch_swab64(val);
 #elif defined(__SWAB_64_THRU_32__)
 	__u32 h = val >> 32;
 	__u32 l = val & ((1ULL << 32) - 1);
-	return (((__u64)___swab32(l)) << 32) | ((__u64)(___swab32(h)));
+	return (((__u64)__fswab32(l)) << 32) | ((__u64)(__fswab32(h)));
 #else
-	return __const_swab64(val);
+	return ___constant_swab64(val);
 #endif
 }
 
-static inline __attribute_const__ __u32 ___swahw32(__u32 val)
+static inline __attribute_const__ __u32 __fswahw32(__u32 val)
 {
 #ifdef __arch_swahw32
 	return __arch_swahw32(val);
 #else
-	return __const_swahw32(val);
+	return ___constant_swahw32(val);
 #endif
 }
 
-static inline __attribute_const__ __u32 ___swahb32(__u32 val)
+static inline __attribute_const__ __u32 __fswahb32(__u32 val)
 {
 #ifdef __arch_swahb32
 	return __arch_swahb32(val);
 #else
-	return __const_swahb32(val);
+	return ___constant_swahb32(val);
 #endif
 }
 
@@ -98,8 +98,8 @@ static inline __attribute_const__ __u32 ___swahb32(__u32 val)
  */
 #define __swab16(x)				\
 	(__builtin_constant_p((__u16)(x)) ?	\
-	__const_swab16((x)) :			\
-	___swab16((x)))
+	___constant_swab16(x) :			\
+	__fswab16(x))
 
 /**
  * __swab32 - return a byteswapped 32-bit value
@@ -107,8 +107,8 @@ static inline __attribute_const__ __u32 ___swahb32(__u32 val)
  */
 #define __swab32(x)				\
 	(__builtin_constant_p((__u32)(x)) ?	\
-	__const_swab32((x)) :			\
-	___swab32((x)))
+	___constant_swab32(x) :			\
+	__fswab32(x))
 
 /**
  * __swab64 - return a byteswapped 64-bit value
@@ -116,8 +116,8 @@ static inline __attribute_const__ __u32 ___swahb32(__u32 val)
  */
 #define __swab64(x)				\
 	(__builtin_constant_p((__u64)(x)) ?	\
-	__const_swab64((x)) :			\
-	___swab64((x)))
+	___constant_swab64(x) :			\
+	__fswab64(x))
 
 /**
  * __swahw32 - return a word-swapped 32-bit value
@@ -127,8 +127,8 @@ static inline __attribute_const__ __u32 ___swahb32(__u32 val)
  */
 #define __swahw32(x)				\
 	(__builtin_constant_p((__u32)(x)) ?	\
-	__const_swahw32((x)) :			\
-	___swahw32((x)))
+	___constant_swahw32(x) :		\
+	__fswahw32(x))
 
 /**
  * __swahb32 - return a high and low byte-swapped 32-bit value
@@ -138,8 +138,8 @@ static inline __attribute_const__ __u32 ___swahb32(__u32 val)
  */
 #define __swahb32(x)				\
 	(__builtin_constant_p((__u32)(x)) ?	\
-	__const_swahb32((x)) :			\
-	___swahb32((x)))
+	___constant_swahb32(x) :		\
+	__fswahb32(x))
 
 /**
  * __swab16p - return a byteswapped 16-bit value from a pointer
diff --git a/include/linux/swap.h b/include/linux/swap.h
index a3af95b2cb6d..91dee50fe260 100644
--- a/include/linux/swap.h
+++ b/include/linux/swap.h
@@ -120,7 +120,9 @@ struct swap_extent {
 enum {
 	SWP_USED	= (1 << 0),	/* is slot in swap_info[] used? */
 	SWP_WRITEOK	= (1 << 1),	/* ok to write to this swap?	*/
-	SWP_ACTIVE	= (SWP_USED | SWP_WRITEOK),
+	SWP_DISCARDABLE = (1 << 2),	/* blkdev supports discard */
+	SWP_DISCARDING	= (1 << 3),	/* now discarding a free cluster */
+	SWP_SOLIDSTATE	= (1 << 4),	/* blkdev seeks are cheap */
 					/* add others here before... */
 	SWP_SCANNING	= (1 << 8),	/* refcount in scan_swap_map */
 };
@@ -134,22 +136,24 @@ enum {
  * The in-memory structure used to track swap areas.
  */
 struct swap_info_struct {
-	unsigned int flags;
+	unsigned long flags;
 	int prio;			/* swap priority */
+	int next;			/* next entry on swap list */
 	struct file *swap_file;
 	struct block_device *bdev;
 	struct list_head extent_list;
 	struct swap_extent *curr_swap_extent;
-	unsigned old_block_size;
-	unsigned short * swap_map;
+	unsigned short *swap_map;
 	unsigned int lowest_bit;
 	unsigned int highest_bit;
+	unsigned int lowest_alloc;	/* while preparing discard cluster */
+	unsigned int highest_alloc;	/* while preparing discard cluster */
 	unsigned int cluster_next;
 	unsigned int cluster_nr;
 	unsigned int pages;
 	unsigned int max;
 	unsigned int inuse_pages;
-	int next;			/* next entry on swap list */
+	unsigned int old_block_size;
 };
 
 struct swap_list_t {
@@ -163,7 +167,6 @@ struct swap_list_t {
 /* linux/mm/page_alloc.c */
 extern unsigned long totalram_pages;
 extern unsigned long totalreserve_pages;
-extern long nr_swap_pages;
 extern unsigned int nr_free_buffer_pages(void);
 extern unsigned int nr_free_pagecache_pages(void);
 
@@ -174,8 +177,6 @@ extern unsigned int nr_free_pagecache_pages(void);
 /* linux/mm/swap.c */
 extern void __lru_cache_add(struct page *, enum lru_list lru);
 extern void lru_cache_add_lru(struct page *, enum lru_list lru);
-extern void lru_cache_add_active_or_unevictable(struct page *,
-					struct vm_area_struct *);
 extern void activate_page(struct page *);
 extern void mark_page_accessed(struct page *);
 extern void lru_add_drain(void);
@@ -280,7 +281,7 @@ extern void end_swap_bio_read(struct bio *bio, int err);
 extern struct address_space swapper_space;
 #define total_swapcache_pages  swapper_space.nrpages
 extern void show_swap_cache_info(void);
-extern int add_to_swap(struct page *, gfp_t);
+extern int add_to_swap(struct page *);
 extern int add_to_swap_cache(struct page *, swp_entry_t, gfp_t);
 extern void __delete_from_swap_cache(struct page *);
 extern void delete_from_swap_cache(struct page *);
@@ -293,6 +294,7 @@ extern struct page *swapin_readahead(swp_entry_t, gfp_t,
 			struct vm_area_struct *vma, unsigned long addr);
 
 /* linux/mm/swapfile.c */
+extern long nr_swap_pages;
 extern long total_swap_pages;
 extern void si_swapinfo(struct sysinfo *);
 extern swp_entry_t get_swap_page(void);
@@ -300,15 +302,14 @@ extern swp_entry_t get_swap_page_of_type(int);
 extern int swap_duplicate(swp_entry_t);
 extern int valid_swaphandles(swp_entry_t, unsigned long *);
 extern void swap_free(swp_entry_t);
-extern void free_swap_and_cache(swp_entry_t);
+extern int free_swap_and_cache(swp_entry_t);
 extern int swap_type_of(dev_t, sector_t, struct block_device **);
 extern unsigned int count_swap_pages(int, int);
 extern sector_t map_swap_page(struct swap_info_struct *, pgoff_t);
 extern sector_t swapdev_block(int, pgoff_t);
 extern struct swap_info_struct *get_swap_info_struct(unsigned);
-extern int can_share_swap_page(struct page *);
-extern int remove_exclusive_swap_page(struct page *);
-extern int remove_exclusive_swap_page_ref(struct page *);
+extern int reuse_swap_page(struct page *);
+extern int try_to_free_swap(struct page *);
 struct backing_dev_info;
 
 /* linux/mm/thrash.c */
@@ -334,7 +335,8 @@ static inline void disable_swap_token(void)
 
 #else /* CONFIG_SWAP */
 
-#define total_swap_pages			0
+#define nr_swap_pages				0L
+#define total_swap_pages			0L
 #define total_swapcache_pages			0UL
 
 #define si_swapinfo(val) \
@@ -350,14 +352,8 @@ static inline void show_swap_cache_info(void)
 {
 }
 
-static inline void free_swap_and_cache(swp_entry_t swp)
-{
-}
-
-static inline int swap_duplicate(swp_entry_t swp)
-{
-	return 0;
-}
+#define free_swap_and_cache(swp)	is_migration_entry(swp)
+#define swap_duplicate(swp)		is_migration_entry(swp)
 
 static inline void swap_free(swp_entry_t swp)
 {
@@ -374,7 +370,10 @@ static inline struct page *lookup_swap_cache(swp_entry_t swp)
 	return NULL;
 }
 
-#define can_share_swap_page(p)			(page_mapcount(p) == 1)
+static inline int add_to_swap(struct page *page)
+{
+	return 0;
+}
 
 static inline int add_to_swap_cache(struct page *page, swp_entry_t entry,
 							gfp_t gfp_mask)
@@ -390,14 +389,9 @@ static inline void delete_from_swap_cache(struct page *page)
 {
 }
 
-#define swap_token_default_timeout		0
-
-static inline int remove_exclusive_swap_page(struct page *p)
-{
-	return 0;
-}
+#define reuse_swap_page(page)	(page_mapcount(page) == 1)
 
-static inline int remove_exclusive_swap_page_ref(struct page *page)
+static inline int try_to_free_swap(struct page *page)
 {
 	return 0;
 }
diff --git a/include/linux/swiotlb.h b/include/linux/swiotlb.h
index 325af1de0351..dedd3c0cfe30 100644
--- a/include/linux/swiotlb.h
+++ b/include/linux/swiotlb.h
@@ -27,7 +27,8 @@ swiotlb_init(void);
 extern void *swiotlb_alloc_boot(size_t bytes, unsigned long nslabs);
 extern void *swiotlb_alloc(unsigned order, unsigned long nslabs);
 
-extern dma_addr_t swiotlb_phys_to_bus(phys_addr_t address);
+extern dma_addr_t swiotlb_phys_to_bus(struct device *hwdev,
+				      phys_addr_t address);
 extern phys_addr_t swiotlb_bus_to_phys(dma_addr_t address);
 
 extern int swiotlb_arch_range_needs_mapping(void *ptr, size_t size);
diff --git a/include/linux/time.h b/include/linux/time.h
index ce321ac5c8f8..fbbd2a1c92ba 100644
--- a/include/linux/time.h
+++ b/include/linux/time.h
@@ -105,6 +105,7 @@ extern unsigned long read_persistent_clock(void);
 extern int update_persistent_clock(struct timespec now);
 extern int no_sync_cmos_clock __read_mostly;
 void timekeeping_init(void);
+extern int timekeeping_suspended;
 
 unsigned long get_seconds(void);
 struct timespec current_kernel_time(void);
diff --git a/include/linux/types.h b/include/linux/types.h
index 121f349cb7ec..712ca53bc348 100644
--- a/include/linux/types.h
+++ b/include/linux/types.h
@@ -176,10 +176,9 @@ typedef __u16 __bitwise __le16;
 typedef __u16 __bitwise __be16;
 typedef __u32 __bitwise __le32;
 typedef __u32 __bitwise __be32;
-#if defined(__GNUC__)
 typedef __u64 __bitwise __le64;
 typedef __u64 __bitwise __be64;
-#endif
+
 typedef __u16 __bitwise __sum16;
 typedef __u32 __bitwise __wsum;
 
@@ -195,6 +194,16 @@ typedef u32 phys_addr_t;
 
 typedef phys_addr_t resource_size_t;
 
+typedef struct {
+	volatile int counter;
+} atomic_t;
+
+#ifdef CONFIG_64BIT
+typedef struct {
+	volatile long counter;
+} atomic64_t;
+#endif
+
 struct ustat {
 	__kernel_daddr_t	f_tfree;
 	__kernel_ino_t		f_tinode;
diff --git a/include/linux/uio_driver.h b/include/linux/uio_driver.h
index cdf338d94b7f..a0bb6bd2e5c1 100644
--- a/include/linux/uio_driver.h
+++ b/include/linux/uio_driver.h
@@ -38,6 +38,24 @@ struct uio_mem {
 
 #define MAX_UIO_MAPS	5
 
+struct uio_portio;
+
+/**
+ * struct uio_port - description of a UIO port region
+ * @start:		start of port region
+ * @size:		size of port region
+ * @porttype:		type of port (see UIO_PORT_* below)
+ * @portio:		for use by the UIO core only.
+ */
+struct uio_port {
+	unsigned long		start;
+	unsigned long		size;
+	int			porttype;
+	struct uio_portio	*portio;
+};
+
+#define MAX_UIO_PORT_REGIONS	5
+
 struct uio_device;
 
 /**
@@ -46,6 +64,7 @@ struct uio_device;
  * @name:		device name
  * @version:		device driver version
  * @mem:		list of mappable memory regions, size==0 for end of list
+ * @port:		list of port regions, size==0 for end of list
  * @irq:		interrupt number or UIO_IRQ_CUSTOM
  * @irq_flags:		flags for request_irq()
  * @priv:		optional private data
@@ -57,9 +76,10 @@ struct uio_device;
  */
 struct uio_info {
 	struct uio_device	*uio_dev;
-	char			*name;
-	char			*version;
+	const char		*name;
+	const char		*version;
 	struct uio_mem		mem[MAX_UIO_MAPS];
+	struct uio_port		port[MAX_UIO_PORT_REGIONS];
 	long			irq;
 	unsigned long		irq_flags;
 	void			*priv;
@@ -92,4 +112,10 @@ extern void uio_event_notify(struct uio_info *info);
 #define UIO_MEM_LOGICAL	2
 #define UIO_MEM_VIRTUAL 3
 
+/* defines for uio_port->porttype */
+#define UIO_PORT_NONE	0
+#define UIO_PORT_X86	1
+#define UIO_PORT_GPIO	2
+#define UIO_PORT_OTHER	3
+
 #endif /* _LINUX_UIO_DRIVER_H_ */
diff --git a/include/linux/unwind.h b/include/linux/unwind.h
deleted file mode 100644
index 7760860fa170..000000000000
--- a/include/linux/unwind.h
+++ /dev/null
@@ -1,68 +0,0 @@
-#ifndef _LINUX_UNWIND_H
-#define _LINUX_UNWIND_H
-
-/*
- * Copyright (C) 2002-2006 Novell, Inc.
- *	Jan Beulich <jbeulich@novell.com>
- * This code is released under version 2 of the GNU GPL.
- *
- * A simple API for unwinding kernel stacks.  This is used for
- * debugging and error reporting purposes.  The kernel doesn't need
- * full-blown stack unwinding with all the bells and whistles, so there
- * is not much point in implementing the full Dwarf2 unwind API.
- */
-
-struct module;
-
-struct unwind_frame_info {};
-
-static inline void unwind_init(void) {}
-static inline void unwind_setup(void) {}
-
-#ifdef CONFIG_MODULES
-
-static inline void *unwind_add_table(struct module *mod,
-                                     const void *table_start,
-                                     unsigned long table_size)
-{
-	return NULL;
-}
-
-static inline void unwind_remove_table(void *handle, int init_only)
-{
-}
-
-#endif
-
-static inline int unwind_init_frame_info(struct unwind_frame_info *info,
-                                         struct task_struct *tsk,
-                                         const struct pt_regs *regs)
-{
-	return -ENOSYS;
-}
-
-static inline int unwind_init_blocked(struct unwind_frame_info *info,
-                                      struct task_struct *tsk)
-{
-	return -ENOSYS;
-}
-
-static inline int unwind_init_running(struct unwind_frame_info *info,
-                                      asmlinkage int (*cb)(struct unwind_frame_info *,
-                                                           void *arg),
-                                      void *arg)
-{
-	return -ENOSYS;
-}
-
-static inline int unwind(struct unwind_frame_info *info)
-{
-	return -ENOSYS;
-}
-
-static inline int unwind_to_user(struct unwind_frame_info *info)
-{
-	return -ENOSYS;
-}
-
-#endif /* _LINUX_UNWIND_H */
diff --git a/include/linux/usb.h b/include/linux/usb.h
index f72aa51f7bcd..85ee9be9361e 100644
--- a/include/linux/usb.h
+++ b/include/linux/usb.h
@@ -108,6 +108,7 @@ enum usb_interface_condition {
  *	(in probe()), bound to a driver, or unbinding (in disconnect())
  * @is_active: flag set when the interface is bound and not suspended.
  * @sysfs_files_created: sysfs attributes exist
+ * @ep_devs_created: endpoint child pseudo-devices exist
  * @unregistering: flag set when the interface is being unregistered
  * @needs_remote_wakeup: flag set when the driver requires remote-wakeup
  *	capability during autosuspend.
@@ -120,6 +121,11 @@ enum usb_interface_condition {
  *	to the sysfs representation for that device.
  * @pm_usage_cnt: PM usage counter for this interface; autosuspend is not
  *	allowed unless the counter is 0.
+ * @reset_ws: Used for scheduling resets from atomic context.
+ * @reset_running: set to 1 if the interface is currently running a
+ *      queued reset so that usb_cancel_queued_reset() doesn't try to
+ *      remove from the workqueue when running inside the worker
+ *      thread. See __usb_queue_reset_device().
  *
  * USB device drivers attach to interfaces on a physical device.  Each
  * interface encapsulates a single high level function, such as feeding
@@ -164,14 +170,17 @@ struct usb_interface {
 	enum usb_interface_condition condition;		/* state of binding */
 	unsigned is_active:1;		/* the interface is not suspended */
 	unsigned sysfs_files_created:1;	/* the sysfs attributes exist */
+	unsigned ep_devs_created:1;	/* endpoint "devices" exist */
 	unsigned unregistering:1;	/* unregistration is in progress */
 	unsigned needs_remote_wakeup:1;	/* driver requires remote wakeup */
 	unsigned needs_altsetting0:1;	/* switch to altsetting 0 is pending */
 	unsigned needs_binding:1;	/* needs delayed unbind/rebind */
+	unsigned reset_running:1;
 
 	struct device dev;		/* interface specific device info */
 	struct device *usb_dev;
 	int pm_usage_cnt;		/* usage counter for autosuspend */
+	struct work_struct reset_ws;	/* for resets in atomic context */
 };
 #define	to_usb_interface(d) container_of(d, struct usb_interface, dev)
 #define	interface_to_usbdev(intf) \
@@ -329,7 +338,7 @@ struct usb_bus {
 #endif
 	struct device *dev;		/* device for this bus */
 
-#if defined(CONFIG_USB_MON)
+#if defined(CONFIG_USB_MON) || defined(CONFIG_USB_MON_MODULE)
 	struct mon_bus *mon_bus;	/* non-null when associated */
 	int monitored;			/* non-zero when monitored */
 #endif
@@ -398,6 +407,7 @@ struct usb_tt;
  * @urbnum: number of URBs submitted for the whole device
  * @active_duration: total time device is not suspended
  * @autosuspend: for delayed autosuspends
+ * @autoresume: for autoresumes requested while in_interrupt
  * @pm_mutex: protects PM operations
  * @last_busy: time of last use
  * @autosuspend_delay: in jiffies
@@ -476,6 +486,7 @@ struct usb_device {
 
 #ifdef CONFIG_PM
 	struct delayed_work autosuspend;
+	struct work_struct autoresume;
 	struct mutex pm_mutex;
 
 	unsigned long last_busy;
@@ -505,6 +516,7 @@ extern int usb_lock_device_for_reset(struct usb_device *udev,
 
 /* USB port reset for device reinitialization */
 extern int usb_reset_device(struct usb_device *dev);
+extern void usb_queue_reset_device(struct usb_interface *dev);
 
 extern struct usb_device *usb_find_device(u16 vendor_id, u16 product_id);
 
@@ -513,6 +525,8 @@ extern struct usb_device *usb_find_device(u16 vendor_id, u16 product_id);
 extern int usb_autopm_set_interface(struct usb_interface *intf);
 extern int usb_autopm_get_interface(struct usb_interface *intf);
 extern void usb_autopm_put_interface(struct usb_interface *intf);
+extern int usb_autopm_get_interface_async(struct usb_interface *intf);
+extern void usb_autopm_put_interface_async(struct usb_interface *intf);
 
 static inline void usb_autopm_enable(struct usb_interface *intf)
 {
@@ -539,8 +553,13 @@ static inline int usb_autopm_set_interface(struct usb_interface *intf)
 static inline int usb_autopm_get_interface(struct usb_interface *intf)
 { return 0; }
 
+static inline int usb_autopm_get_interface_async(struct usb_interface *intf)
+{ return 0; }
+
 static inline void usb_autopm_put_interface(struct usb_interface *intf)
 { }
+static inline void usb_autopm_put_interface_async(struct usb_interface *intf)
+{ }
 static inline void usb_autopm_enable(struct usb_interface *intf)
 { }
 static inline void usb_autopm_disable(struct usb_interface *intf)
@@ -1050,7 +1069,7 @@ struct usb_device_driver {
 	void (*disconnect) (struct usb_device *udev);
 
 	int (*suspend) (struct usb_device *udev, pm_message_t message);
-	int (*resume) (struct usb_device *udev);
+	int (*resume) (struct usb_device *udev, pm_message_t message);
 	struct usbdrv_wrap drvwrap;
 	unsigned int supports_autosuspend:1;
 };
@@ -1321,7 +1340,7 @@ struct urb {
 	struct kref kref;		/* reference count of the URB */
 	void *hcpriv;			/* private data for host controller */
 	atomic_t use_count;		/* concurrent submissions counter */
-	u8 reject;			/* submissions will fail */
+	atomic_t reject;		/* submissions will fail */
 	int unlinked;			/* unlink error code */
 
 	/* public: documented fields in the urb that can be used by drivers */
@@ -1466,6 +1485,7 @@ extern void usb_poison_urb(struct urb *urb);
 extern void usb_unpoison_urb(struct urb *urb);
 extern void usb_kill_anchored_urbs(struct usb_anchor *anchor);
 extern void usb_poison_anchored_urbs(struct usb_anchor *anchor);
+extern void usb_unpoison_anchored_urbs(struct usb_anchor *anchor);
 extern void usb_unlink_anchored_urbs(struct usb_anchor *anchor);
 extern void usb_anchor_urb(struct urb *urb, struct usb_anchor *anchor);
 extern void usb_unanchor_urb(struct urb *urb);
@@ -1722,10 +1742,6 @@ extern void usb_unregister_notify(struct notifier_block *nb);
 
 #define err(format, arg...) printk(KERN_ERR KBUILD_MODNAME ": " \
 	format "\n" , ## arg)
-#define info(format, arg...) printk(KERN_INFO KBUILD_MODNAME ": " \
-	format "\n" , ## arg)
-#define warn(format, arg...) printk(KERN_WARNING KBUILD_MODNAME ": " \
-	format "\n" , ## arg)
 
 #endif  /* __KERNEL__ */
 
diff --git a/include/linux/usb/association.h b/include/linux/usb/association.h
index 07c5e3cf5898..0a4a18b3c1bb 100644
--- a/include/linux/usb/association.h
+++ b/include/linux/usb/association.h
@@ -28,17 +28,17 @@ struct wusb_am_attr {
 };
 
 /* Different fields defined by the spec */
-#define WUSB_AR_AssociationTypeId	{ .id = 0x0000, .len =  2 }
-#define WUSB_AR_AssociationSubTypeId	{ .id = 0x0001, .len =  2 }
-#define WUSB_AR_Length			{ .id = 0x0002, .len =  4 }
-#define WUSB_AR_AssociationStatus	{ .id = 0x0004, .len =  4 }
-#define WUSB_AR_LangID			{ .id = 0x0008, .len =  2 }
-#define WUSB_AR_DeviceFriendlyName	{ .id = 0x000b, .len = 64 } /* max */
-#define WUSB_AR_HostFriendlyName	{ .id = 0x000c, .len = 64 } /* max */
-#define WUSB_AR_CHID			{ .id = 0x1000, .len = 16 }
-#define WUSB_AR_CDID			{ .id = 0x1001, .len = 16 }
-#define WUSB_AR_ConnectionContext	{ .id = 0x1002, .len = 48 }
-#define WUSB_AR_BandGroups		{ .id = 0x1004, .len =  2 }
+#define WUSB_AR_AssociationTypeId	{ .id = cpu_to_le16(0x0000), .len = cpu_to_le16(2) }
+#define WUSB_AR_AssociationSubTypeId	{ .id = cpu_to_le16(0x0001), .len = cpu_to_le16(2) }
+#define WUSB_AR_Length			{ .id = cpu_to_le16(0x0002), .len = cpu_to_le16(4) }
+#define WUSB_AR_AssociationStatus	{ .id = cpu_to_le16(0x0004), .len = cpu_to_le16(4) }
+#define WUSB_AR_LangID			{ .id = cpu_to_le16(0x0008), .len = cpu_to_le16(2) }
+#define WUSB_AR_DeviceFriendlyName	{ .id = cpu_to_le16(0x000b), .len = cpu_to_le16(64) } /* max */
+#define WUSB_AR_HostFriendlyName	{ .id = cpu_to_le16(0x000c), .len = cpu_to_le16(64) } /* max */
+#define WUSB_AR_CHID			{ .id = cpu_to_le16(0x1000), .len = cpu_to_le16(16) }
+#define WUSB_AR_CDID			{ .id = cpu_to_le16(0x1001), .len = cpu_to_le16(16) }
+#define WUSB_AR_ConnectionContext	{ .id = cpu_to_le16(0x1002), .len = cpu_to_le16(48) }
+#define WUSB_AR_BandGroups		{ .id = cpu_to_le16(0x1004), .len = cpu_to_le16(2) }
 
 /* CBAF Control Requests (AMS1.0[T4-1] */
 enum {
diff --git a/include/linux/usb/gpio_vbus.h b/include/linux/usb/gpio_vbus.h
new file mode 100644
index 000000000000..d9f03ccc2d60
--- /dev/null
+++ b/include/linux/usb/gpio_vbus.h
@@ -0,0 +1,30 @@
+/*
+ * A simple GPIO VBUS sensing driver for B peripheral only devices
+ * with internal transceivers.
+ * Optionally D+ pullup can be controlled by a second GPIO.
+ *
+ * Copyright (c) 2008 Philipp Zabel <philipp.zabel@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.
+ *
+ */
+
+/**
+ * struct gpio_vbus_mach_info - configuration for gpio_vbus
+ * @gpio_vbus: VBUS sensing GPIO
+ * @gpio_pullup: optional D+ or D- pullup GPIO (else negative/invalid)
+ * @gpio_vbus_inverted: true if gpio_vbus is active low
+ * @gpio_pullup_inverted: true if gpio_pullup is active low
+ *
+ * The VBUS sensing GPIO should have a pulldown, which will normally be
+ * part of a resistor ladder turning a 4.0V-5.25V level on VBUS into a
+ * value the GPIO detects as active.  Some systems will use comparators.
+ */
+struct gpio_vbus_mach_info {
+	int gpio_vbus;
+	int gpio_pullup;
+	bool gpio_vbus_inverted;
+	bool gpio_pullup_inverted;
+};
diff --git a/include/linux/usb/musb.h b/include/linux/usb/musb.h
index 630962c04ca4..d6aad0ea6033 100644
--- a/include/linux/usb/musb.h
+++ b/include/linux/usb/musb.h
@@ -47,6 +47,11 @@ struct musb_hdrc_config {
 	u8		ram_bits;	/* ram address size */
 
 	struct musb_hdrc_eps_bits *eps_bits;
+#ifdef CONFIG_BLACKFIN
+        /* A GPIO controlling VRSEL in Blackfin */
+        unsigned int    gpio_vrsel;
+#endif
+
 };
 
 struct musb_hdrc_platform_data {
diff --git a/include/linux/usb/otg.h b/include/linux/usb/otg.h
index 1db25d152ad8..94df4fe6c6c0 100644
--- a/include/linux/usb/otg.h
+++ b/include/linux/usb/otg.h
@@ -84,6 +84,7 @@ extern int otg_set_transceiver(struct otg_transceiver *);
 
 /* for usb host and peripheral controller drivers */
 extern struct otg_transceiver *otg_get_transceiver(void);
+extern void otg_put_transceiver(struct otg_transceiver *);
 
 static inline int
 otg_start_hnp(struct otg_transceiver *otg)
diff --git a/include/linux/usb_usual.h b/include/linux/usb_usual.h
index d9a3bbe38e6b..1eea1ab68dc4 100644
--- a/include/linux/usb_usual.h
+++ b/include/linux/usb_usual.h
@@ -52,8 +52,11 @@
 	US_FLAG(MAX_SECTORS_MIN,0x00002000)			\
 		/* Sets max_sectors to arch min */		\
 	US_FLAG(BULK_IGNORE_TAG,0x00004000)			\
-		/* Ignore tag mismatch in bulk operations */
-
+		/* Ignore tag mismatch in bulk operations */    \
+	US_FLAG(SANE_SENSE,     0x00008000)			\
+		/* Sane Sense (> 18 bytes) */			\
+	US_FLAG(CAPACITY_OK,	0x00010000)			\
+		/* READ CAPACITY response is correct */
 
 #define US_FLAG(name, value)	US_FL_##name = value ,
 enum { US_DO_ALL_FLAGS };
diff --git a/include/linux/vmalloc.h b/include/linux/vmalloc.h
index 307b88577eaa..506e7620a986 100644
--- a/include/linux/vmalloc.h
+++ b/include/linux/vmalloc.h
@@ -97,6 +97,10 @@ extern void unmap_kernel_range(unsigned long addr, unsigned long size);
 extern struct vm_struct *alloc_vm_area(size_t size);
 extern void free_vm_area(struct vm_struct *area);
 
+/* for /dev/kmem */
+extern long vread(char *buf, char *addr, unsigned long count);
+extern long vwrite(char *buf, char *addr, unsigned long count);
+
 /*
  *	Internals.  Dont't use..
  */
diff --git a/include/linux/wimax.h b/include/linux/wimax.h
new file mode 100644
index 000000000000..c89de7f4e5b9
--- /dev/null
+++ b/include/linux/wimax.h
@@ -0,0 +1,234 @@
+/*
+ * Linux WiMax
+ * API for user space
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ *  - Initial implementation
+ *
+ *
+ * This file declares the user/kernel protocol that is spoken over
+ * Generic Netlink, as well as any type declaration that is to be used
+ * by kernel and user space.
+ *
+ * It is intended for user space to clone it verbatim to use it as a
+ * primary reference for definitions.
+ *
+ * Stuff intended for kernel usage as well as full protocol and stack
+ * documentation is rooted in include/net/wimax.h.
+ */
+
+#ifndef __LINUX__WIMAX_H__
+#define __LINUX__WIMAX_H__
+
+#include <linux/types.h>
+
+enum {
+	/**
+	 * Version of the interface (unsigned decimal, MMm, max 25.5)
+	 * M - Major: change if removing or modifying an existing call.
+	 * m - minor: change when adding a new call
+	 */
+	WIMAX_GNL_VERSION = 00,
+	/* Generic NetLink attributes */
+	WIMAX_GNL_ATTR_INVALID = 0x00,
+	WIMAX_GNL_ATTR_MAX = 10,
+};
+
+
+/*
+ * Generic NetLink operations
+ *
+ * Most of these map to an API call; _OP_ stands for operation, _RP_
+ * for reply and _RE_ for report (aka: signal).
+ */
+enum {
+	WIMAX_GNL_OP_MSG_FROM_USER,	/* User to kernel message */
+	WIMAX_GNL_OP_MSG_TO_USER,	/* Kernel to user message */
+	WIMAX_GNL_OP_RFKILL,	/* Run wimax_rfkill() */
+	WIMAX_GNL_OP_RESET,	/* Run wimax_rfkill() */
+	WIMAX_GNL_RE_STATE_CHANGE,	/* Report: status change */
+};
+
+
+/* Message from user / to user */
+enum {
+	WIMAX_GNL_MSG_IFIDX = 1,
+	WIMAX_GNL_MSG_PIPE_NAME,
+	WIMAX_GNL_MSG_DATA,
+};
+
+
+/*
+ * wimax_rfkill()
+ *
+ * The state of the radio (ON/OFF) is mapped to the rfkill subsystem's
+ * switch state (DISABLED/ENABLED).
+ */
+enum wimax_rf_state {
+	WIMAX_RF_OFF = 0,	/* Radio is off, rfkill on/enabled */
+	WIMAX_RF_ON = 1,	/* Radio is on, rfkill off/disabled */
+	WIMAX_RF_QUERY = 2,
+};
+
+/* Attributes */
+enum {
+	WIMAX_GNL_RFKILL_IFIDX = 1,
+	WIMAX_GNL_RFKILL_STATE,
+};
+
+
+/* Attributes for wimax_reset() */
+enum {
+	WIMAX_GNL_RESET_IFIDX = 1,
+};
+
+
+/*
+ * Attributes for the Report State Change
+ *
+ * For now we just have the old and new states; new attributes might
+ * be added later on.
+ */
+enum {
+	WIMAX_GNL_STCH_IFIDX = 1,
+	WIMAX_GNL_STCH_STATE_OLD,
+	WIMAX_GNL_STCH_STATE_NEW,
+};
+
+
+/**
+ * enum wimax_st - The different states of a WiMAX device
+ * @__WIMAX_ST_NULL: The device structure has been allocated and zeroed,
+ *     but still wimax_dev_add() hasn't been called. There is no state.
+ *
+ * @WIMAX_ST_DOWN: The device has been registered with the WiMAX and
+ *     networking stacks, but it is not initialized (normally that is
+ *     done with 'ifconfig DEV up' [or equivalent], which can upload
+ *     firmware and enable communications with the device).
+ *     In this state, the device is powered down and using as less
+ *     power as possible.
+ *     This state is the default after a call to wimax_dev_add(). It
+ *     is ok to have drivers move directly to %WIMAX_ST_UNINITIALIZED
+ *     or %WIMAX_ST_RADIO_OFF in _probe() after the call to
+ *     wimax_dev_add().
+ *     It is recommended that the driver leaves this state when
+ *     calling 'ifconfig DEV up' and enters it back on 'ifconfig DEV
+ *     down'.
+ *
+ * @__WIMAX_ST_QUIESCING: The device is being torn down, so no API
+ *     operations are allowed to proceed except the ones needed to
+ *     complete the device clean up process.
+ *
+ * @WIMAX_ST_UNINITIALIZED: [optional] Communication with the device
+ *     is setup, but the device still requires some configuration
+ *     before being operational.
+ *     Some WiMAX API calls might work.
+ *
+ * @WIMAX_ST_RADIO_OFF: The device is fully up; radio is off (wether
+ *     by hardware or software switches).
+ *     It is recommended to always leave the device in this state
+ *     after initialization.
+ *
+ * @WIMAX_ST_READY: The device is fully up and radio is on.
+ *
+ * @WIMAX_ST_SCANNING: [optional] The device has been instructed to
+ *     scan. In this state, the device cannot be actively connected to
+ *     a network.
+ *
+ * @WIMAX_ST_CONNECTING: The device is connecting to a network. This
+ *     state exists because in some devices, the connect process can
+ *     include a number of negotiations between user space, kernel
+ *     space and the device. User space needs to know what the device
+ *     is doing. If the connect sequence in a device is atomic and
+ *     fast, the device can transition directly to CONNECTED
+ *
+ * @WIMAX_ST_CONNECTED: The device is connected to a network.
+ *
+ * @__WIMAX_ST_INVALID: This is an invalid state used to mark the
+ *     maximum numeric value of states.
+ *
+ * Description:
+ *
+ * Transitions from one state to another one are atomic and can only
+ * be caused in kernel space with wimax_state_change(). To read the
+ * state, use wimax_state_get().
+ *
+ * States starting with __ are internal and shall not be used or
+ * referred to by drivers or userspace. They look ugly, but that's the
+ * point -- if any use is made non-internal to the stack, it is easier
+ * to catch on review.
+ *
+ * All API operations [with well defined exceptions] will take the
+ * device mutex before starting and then check the state. If the state
+ * is %__WIMAX_ST_NULL, %WIMAX_ST_DOWN, %WIMAX_ST_UNINITIALIZED or
+ * %__WIMAX_ST_QUIESCING, it will drop the lock and quit with
+ * -%EINVAL, -%ENOMEDIUM, -%ENOTCONN or -%ESHUTDOWN.
+ *
+ * The order of the definitions is important, so we can do numerical
+ * comparisons (eg: < %WIMAX_ST_RADIO_OFF means the device is not ready
+ * to operate).
+ */
+/*
+ * The allowed state transitions are described in the table below
+ * (states in rows can go to states in columns where there is an X):
+ *
+ *                                  UNINI   RADIO READY SCAN CONNEC CONNEC
+ *             NULL DOWN QUIESCING TIALIZED  OFF        NING  TING   TED
+ * NULL         -    x
+ * DOWN              -      x        x       x
+ * QUIESCING         x      -
+ * UNINITIALIZED            x        -       x
+ * RADIO_OFF                x                -     x
+ * READY                    x                x     -     x     x      x
+ * SCANNING                 x                x     x     -     x      x
+ * CONNECTING               x                x     x     x     -      x
+ * CONNECTED                x                x     x                  -
+ *
+ * This table not available in kernel-doc because the formatting messes it up.
+ */
+ enum wimax_st {
+	__WIMAX_ST_NULL = 0,
+	WIMAX_ST_DOWN,
+	__WIMAX_ST_QUIESCING,
+	WIMAX_ST_UNINITIALIZED,
+	WIMAX_ST_RADIO_OFF,
+	WIMAX_ST_READY,
+	WIMAX_ST_SCANNING,
+	WIMAX_ST_CONNECTING,
+	WIMAX_ST_CONNECTED,
+	__WIMAX_ST_INVALID			/* Always keep last */
+};
+
+
+#endif /* #ifndef __LINUX__WIMAX_H__ */
diff --git a/include/linux/wimax/Kbuild b/include/linux/wimax/Kbuild
new file mode 100644
index 000000000000..3cb4f269bb09
--- /dev/null
+++ b/include/linux/wimax/Kbuild
@@ -0,0 +1 @@
+header-y += i2400m.h
diff --git a/include/linux/wimax/debug.h b/include/linux/wimax/debug.h
new file mode 100644
index 000000000000..ba0c49399a83
--- /dev/null
+++ b/include/linux/wimax/debug.h
@@ -0,0 +1,453 @@
+/*
+ * Linux WiMAX
+ * Collection of tools to manage debug operations.
+ *
+ *
+ * Copyright (C) 2005-2007 Intel Corporation
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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.
+ *
+ *
+ * Don't #include this file directly, read on!
+ *
+ *
+ * EXECUTING DEBUGGING ACTIONS OR NOT
+ *
+ * The main thing this framework provides is decission power to take a
+ * debug action (like printing a message) if the current debug level
+ * allows it.
+ *
+ * The decission power is at two levels: at compile-time (what does
+ * not make it is compiled out) and at run-time. The run-time
+ * selection is done per-submodule (as they are declared by the user
+ * of the framework).
+ *
+ * A call to d_test(L) (L being the target debug level) returns true
+ * if the action should be taken because the current debug levels
+ * allow it (both compile and run time).
+ *
+ * It follows that a call to d_test() that can be determined to be
+ * always false at compile time will get the code depending on it
+ * compiled out by optimization.
+ *
+ *
+ * DEBUG LEVELS
+ *
+ * It is up to the caller to define how much a debugging level is.
+ *
+ * Convention sets 0 as "no debug" (so an action marked as debug level 0
+ * will always be taken). The increasing debug levels are used for
+ * increased verbosity.
+ *
+ *
+ * USAGE
+ *
+ * Group the code in modules and submodules inside each module [which
+ * in most cases maps to Linux modules and .c files that compose
+ * those].
+ *
+ *
+ * For each module, there is:
+ *
+ *  - a MODULENAME (single word, legal C identifier)
+ *
+ *  - a debug-levels.h header file that declares the list of
+ *    submodules and that is included by all .c files that use
+ *    the debugging tools. The file name can be anything.
+ *
+ *  - some (optional) .c code to manipulate the runtime debug levels
+ *    through debugfs.
+ *
+ * The debug-levels.h file would look like:
+ *
+ *     #ifndef __debug_levels__h__
+ *     #define __debug_levels__h__
+ *
+ *     #define D_MODULENAME modulename
+ *     #define D_MASTER 10
+ *
+ *     #include <linux/wimax/debug.h>
+ *
+ *     enum d_module {
+ *             D_SUBMODULE_DECLARE(submodule_1),
+ *             D_SUBMODULE_DECLARE(submodule_2),
+ *             ...
+ *             D_SUBMODULE_DECLARE(submodule_N)
+ *     };
+ *
+ *     #endif
+ *
+ * D_MASTER is the maximum compile-time debug level; any debug actions
+ * above this will be out. D_MODULENAME is the module name (legal C
+ * identifier), which has to be unique for each module (to avoid
+ * namespace collisions during linkage). Note those #defines need to
+ * be done before #including debug.h
+ *
+ * We declare N different submodules whose debug level can be
+ * independently controlled during runtime.
+ *
+ * In a .c file of the module (and only in one of them), define the
+ * following code:
+ *
+ *     struct d_level D_LEVEL[] = {
+ *             D_SUBMODULE_DEFINE(submodule_1),
+ *             D_SUBMODULE_DEFINE(submodule_2),
+ *             ...
+ *             D_SUBMODULE_DEFINE(submodule_N),
+ *     };
+ *     size_t D_LEVEL_SIZE = ARRAY_SIZE(D_LEVEL);
+ *
+ * Externs for d_level_MODULENAME and d_level_size_MODULENAME are used
+ * and declared in this file using the D_LEVEL and D_LEVEL_SIZE macros
+ * #defined also in this file.
+ *
+ * To manipulate from user space the levels, create a debugfs dentry
+ * and then register each submodule with:
+ *
+ *     result = d_level_register_debugfs("PREFIX_", submodule_X, parent);
+ *     if (result < 0)
+ *            goto error;
+ *
+ * Where PREFIX_ is a name of your chosing. This will create debugfs
+ * file with a single numeric value that can be use to tweak it. To
+ * remove the entires, just use debugfs_remove_recursive() on 'parent'.
+ *
+ * NOTE: remember that even if this will show attached to some
+ *     particular instance of a device, the settings are *global*.
+ *
+ *
+ * On each submodule (for example, .c files), the debug infrastructure
+ * should be included like this:
+ *
+ *     #define D_SUBMODULE submodule_x     // matches one in debug-levels.h
+ *     #include "debug-levels.h"
+ *
+ * after #including all your include files.
+ *
+ *
+ * Now you can use the d_*() macros below [d_test(), d_fnstart(),
+ * d_fnend(), d_printf(), d_dump()].
+ *
+ * If their debug level is greater than D_MASTER, they will be
+ * compiled out.
+ *
+ * If their debug level is lower or equal than D_MASTER but greater
+ * than the current debug level of their submodule, they'll be
+ * ignored.
+ *
+ * Otherwise, the action will be performed.
+ */
+#ifndef __debug__h__
+#define __debug__h__
+
+#include <linux/types.h>
+#include <linux/device.h>
+
+
+/* Backend stuff */
+
+/*
+ * Debug backend: generate a message header from a 'struct device'
+ *
+ * @head: buffer where to place the header
+ * @head_size: length of @head
+ * @dev: pointer to device used to generate a header from. If NULL,
+ *     an empty ("") header is generated.
+ */
+static inline
+void __d_head(char *head, size_t head_size,
+	      struct device *dev)
+{
+	if (dev == NULL)
+		head[0] = 0;
+	else if ((unsigned long)dev < 4096) {
+		printk(KERN_ERR "E: Corrupt dev %p\n", dev);
+		WARN_ON(1);
+	} else
+		snprintf(head, head_size, "%s %s: ",
+			 dev_driver_string(dev), dev->bus_id);
+}
+
+
+/*
+ * Debug backend: log some message if debugging is enabled
+ *
+ * @l: intended debug level
+ * @tag: tag to prefix the message with
+ * @dev: 'struct device' associated to this message
+ * @f: printf-like format and arguments
+ *
+ * Note this is optimized out if it doesn't pass the compile-time
+ * check; however, it is *always* compiled. This is useful to make
+ * sure the printf-like formats and variables are always checked and
+ * they don't get bit rot if you have all the debugging disabled.
+ */
+#define _d_printf(l, tag, dev, f, a...)					\
+do {									\
+	char head[64];							\
+	if (!d_test(l))							\
+		break;							\
+	__d_head(head, sizeof(head), dev);				\
+	printk(KERN_ERR "%s%s%s: " f, head, __func__, tag, ##a);	\
+} while (0)
+
+
+/*
+ * CPP sintatic sugar to generate A_B like symbol names when one of
+ * the arguments is a a preprocessor #define.
+ */
+#define __D_PASTE__(varname, modulename) varname##_##modulename
+#define __D_PASTE(varname, modulename) (__D_PASTE__(varname, modulename))
+#define _D_SUBMODULE_INDEX(_name) (D_SUBMODULE_DECLARE(_name))
+
+
+/*
+ * Store a submodule's runtime debug level and name
+ */
+struct d_level {
+	u8 level;
+	const char *name;
+};
+
+
+/*
+ * List of available submodules and their debug levels
+ *
+ * We call them d_level_MODULENAME and d_level_size_MODULENAME; the
+ * macros D_LEVEL and D_LEVEL_SIZE contain the name already for
+ * convenience.
+ *
+ * This array and the size are defined on some .c file that is part of
+ * the current module.
+ */
+#define D_LEVEL __D_PASTE(d_level, D_MODULENAME)
+#define D_LEVEL_SIZE __D_PASTE(d_level_size, D_MODULENAME)
+
+extern struct d_level D_LEVEL[];
+extern size_t D_LEVEL_SIZE;
+
+
+/*
+ * Frontend stuff
+ *
+ *
+ * Stuff you need to declare prior to using the actual "debug" actions
+ * (defined below).
+ */
+
+#ifndef D_MODULENAME
+#error D_MODULENAME is not defined in your debug-levels.h file
+/**
+ * D_MODULE - Name of the current module
+ *
+ * #define in your module's debug-levels.h, making sure it is
+ * unique. This has to be a legal C identifier.
+ */
+#define D_MODULENAME undefined_modulename
+#endif
+
+
+#ifndef D_MASTER
+#warning D_MASTER not defined, but debug.h included! [see docs]
+/**
+ * D_MASTER - Compile time maximum debug level
+ *
+ * #define in your debug-levels.h file to the maximum debug level the
+ * runtime code will be allowed to have. This allows you to provide a
+ * main knob.
+ *
+ * Anything above that level will be optimized out of the compile.
+ *
+ * Defaults to zero (no debug code compiled in).
+ *
+ * Maximum one definition per module (at the debug-levels.h file).
+ */
+#define D_MASTER 0
+#endif
+
+#ifndef D_SUBMODULE
+#error D_SUBMODULE not defined, but debug.h included! [see docs]
+/**
+ * D_SUBMODULE - Name of the current submodule
+ *
+ * #define in your submodule .c file before #including debug-levels.h
+ * to the name of the current submodule as previously declared and
+ * defined with D_SUBMODULE_DECLARE() (in your module's
+ * debug-levels.h) and D_SUBMODULE_DEFINE().
+ *
+ * This is used to provide runtime-control over the debug levels.
+ *
+ * Maximum one per .c file! Can be shared among different .c files
+ * (meaning they belong to the same submodule categorization).
+ */
+#define D_SUBMODULE undefined_module
+#endif
+
+
+/**
+ * D_SUBMODULE_DECLARE - Declare a submodule for runtime debug level control
+ *
+ * @_name: name of the submodule, restricted to the chars that make up a
+ *     valid C identifier ([a-zA-Z0-9_]).
+ *
+ * Declare in the module's debug-levels.h header file as:
+ *
+ * enum d_module {
+ *         D_SUBMODULE_DECLARE(submodule_1),
+ *         D_SUBMODULE_DECLARE(submodule_2),
+ *         D_SUBMODULE_DECLARE(submodule_3),
+ * };
+ *
+ * Some corresponding .c file needs to have a matching
+ * D_SUBMODULE_DEFINE().
+ */
+#define D_SUBMODULE_DECLARE(_name) __D_SUBMODULE_##_name
+
+
+/**
+ * D_SUBMODULE_DEFINE - Define a submodule for runtime debug level control
+ *
+ * @_name: name of the submodule, restricted to the chars that make up a
+ *     valid C identifier ([a-zA-Z0-9_]).
+ *
+ * Use once per module (in some .c file) as:
+ *
+ * static
+ * struct d_level d_level_SUBMODULENAME[] = {
+ *         D_SUBMODULE_DEFINE(submodule_1),
+ *         D_SUBMODULE_DEFINE(submodule_2),
+ *         D_SUBMODULE_DEFINE(submodule_3),
+ * };
+ * size_t d_level_size_SUBDMODULENAME = ARRAY_SIZE(d_level_SUBDMODULENAME);
+ *
+ * Matching D_SUBMODULE_DECLARE()s have to be present in a
+ * debug-levels.h header file.
+ */
+#define D_SUBMODULE_DEFINE(_name)		\
+[__D_SUBMODULE_##_name] = {			\
+	.level = 0,				\
+	.name = #_name				\
+}
+
+
+
+/* The actual "debug" operations */
+
+
+/**
+ * d_test - Returns true if debugging should be enabled
+ *
+ * @l: intended debug level (unsigned)
+ *
+ * If the master debug switch is enabled and the current settings are
+ * higher or equal to the requested level, then debugging
+ * output/actions should be enabled.
+ *
+ * NOTE:
+ *
+ * This needs to be coded so that it can be evaluated in compile
+ * time; this is why the ugly BUG_ON() is placed in there, so the
+ * D_MASTER evaluation compiles all out if it is compile-time false.
+ */
+#define d_test(l)							\
+({									\
+	unsigned __l = l;	/* type enforcer */			\
+	(D_MASTER) >= __l						\
+	&& ({								\
+		BUG_ON(_D_SUBMODULE_INDEX(D_SUBMODULE) >= D_LEVEL_SIZE);\
+		D_LEVEL[_D_SUBMODULE_INDEX(D_SUBMODULE)].level >= __l;	\
+	});								\
+})
+
+
+/**
+ * d_fnstart - log message at function start if debugging enabled
+ *
+ * @l: intended debug level
+ * @_dev: 'struct device' pointer, NULL if none (for context)
+ * @f: printf-like format and arguments
+ */
+#define d_fnstart(l, _dev, f, a...) _d_printf(l, " FNSTART", _dev, f, ## a)
+
+
+/**
+ * d_fnend - log message at function end if debugging enabled
+ *
+ * @l: intended debug level
+ * @_dev: 'struct device' pointer, NULL if none (for context)
+ * @f: printf-like format and arguments
+ */
+#define d_fnend(l, _dev, f, a...) _d_printf(l, " FNEND", _dev, f, ## a)
+
+
+/**
+ * d_printf - log message if debugging enabled
+ *
+ * @l: intended debug level
+ * @_dev: 'struct device' pointer, NULL if none (for context)
+ * @f: printf-like format and arguments
+ */
+#define d_printf(l, _dev, f, a...) _d_printf(l, "", _dev, f, ## a)
+
+
+/**
+ * d_dump - log buffer hex dump if debugging enabled
+ *
+ * @l: intended debug level
+ * @_dev: 'struct device' pointer, NULL if none (for context)
+ * @f: printf-like format and arguments
+ */
+#define d_dump(l, dev, ptr, size)			\
+do {							\
+	char head[64];					\
+	if (!d_test(l))					\
+		break;					\
+	__d_head(head, sizeof(head), dev);		\
+	print_hex_dump(KERN_ERR, head, 0, 16, 1,	\
+		       ((void *) ptr), (size), 0);	\
+} while (0)
+
+
+/**
+ * Export a submodule's debug level over debugfs as PREFIXSUBMODULE
+ *
+ * @prefix: string to prefix the name with
+ * @submodule: name of submodule (not a string, just the name)
+ * @dentry: debugfs parent dentry
+ *
+ * Returns: 0 if ok, < 0 errno on error.
+ *
+ * For removing, just use debugfs_remove_recursive() on the parent.
+ */
+#define d_level_register_debugfs(prefix, name, parent)			\
+({									\
+	int rc;								\
+	struct dentry *fd;						\
+	struct dentry *verify_parent_type = parent;			\
+	fd = debugfs_create_u8(						\
+		prefix #name, 0600, verify_parent_type,			\
+		&(D_LEVEL[__D_SUBMODULE_ ## name].level));		\
+	rc = PTR_ERR(fd);						\
+	if (IS_ERR(fd) && rc != -ENODEV)				\
+		printk(KERN_ERR "%s: Can't create debugfs entry %s: "	\
+		       "%d\n", __func__, prefix #name, rc);		\
+	else								\
+		rc = 0;							\
+	rc;								\
+})
+
+
+#endif /* #ifndef __debug__h__ */
diff --git a/include/linux/wimax/i2400m.h b/include/linux/wimax/i2400m.h
new file mode 100644
index 000000000000..74198f5bb4dc
--- /dev/null
+++ b/include/linux/wimax/i2400m.h
@@ -0,0 +1,512 @@
+/*
+ * Intel Wireless WiMax Connection 2400m
+ * Host-Device protocol interface definitions
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ * Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
+ *  - Initial implementation
+ *
+ *
+ * This header defines the data structures and constants used to
+ * communicate with the device.
+ *
+ * BOOTMODE/BOOTROM/FIRMWARE UPLOAD PROTOCOL
+ *
+ * The firmware upload protocol is quite simple and only requires a
+ * handful of commands. See drivers/net/wimax/i2400m/fw.c for more
+ * details.
+ *
+ * The BCF data structure is for the firmware file header.
+ *
+ *
+ * THE DATA / CONTROL PROTOCOL
+ *
+ * This is the normal protocol spoken with the device once the
+ * firmware is uploaded. It transports data payloads and control
+ * messages back and forth.
+ *
+ * It consists 'messages' that pack one or more payloads each. The
+ * format is described in detail in drivers/net/wimax/i2400m/rx.c and
+ * tx.c.
+ *
+ *
+ * THE L3L4 PROTOCOL
+ *
+ * The term L3L4 refers to Layer 3 (the device), Layer 4 (the
+ * driver/host software).
+ *
+ * This is the control protocol used by the host to control the i2400m
+ * device (scan, connect, disconnect...). This is sent to / received
+ * as control frames. These frames consist of a header and zero or
+ * more TLVs with information. We call each control frame a "message".
+ *
+ * Each message is composed of:
+ *
+ * HEADER
+ * [TLV0 + PAYLOAD0]
+ * [TLV1 + PAYLOAD1]
+ * [...]
+ * [TLVN + PAYLOADN]
+ *
+ * The HEADER is defined by 'struct i2400m_l3l4_hdr'. The payloads are
+ * defined by a TLV structure (Type Length Value) which is a 'header'
+ * (struct i2400m_tlv_hdr) and then the payload.
+ *
+ * All integers are represented as Little Endian.
+ *
+ * - REQUESTS AND EVENTS
+ *
+ * The requests can be clasified as follows:
+ *
+ *   COMMAND:  implies a request from the host to the device requesting
+ *             an action being performed. The device will reply with a
+ *             message (with the same type as the command), status and
+ *             no (TLV) payload. Execution of a command might cause
+ *             events (of different type) to be sent later on as
+ *             device's state changes.
+ *
+ *   GET/SET:  similar to COMMAND, but will not cause other
+ *             EVENTs. The reply, in the case of GET, will contain
+ *             TLVs with the requested information.
+ *
+ *   EVENT:    asynchronous messages sent from the device, maybe as a
+ *             consequence of previous COMMANDs but disassociated from
+ *             them.
+ *
+ * Only one request might be pending at the same time (ie: don't
+ * parallelize nor post another GET request before the previous
+ * COMMAND has been acknowledged with it's corresponding reply by the
+ * device).
+ *
+ * The different requests and their formats are described below:
+ *
+ *  I2400M_MT_*   Message types
+ *  I2400M_MS_*   Message status (for replies, events)
+ *  i2400m_tlv_*  TLVs
+ *
+ * data types are named 'struct i2400m_msg_OPNAME', OPNAME matching the
+ * operation.
+ */
+
+#ifndef __LINUX__WIMAX__I2400M_H__
+#define __LINUX__WIMAX__I2400M_H__
+
+#include <linux/types.h>
+
+
+/*
+ * Host Device Interface (HDI) common to all busses
+ */
+
+/* Boot-mode (firmware upload mode) commands */
+
+/* Header for the firmware file */
+struct i2400m_bcf_hdr {
+	__le32 module_type;
+	__le32 header_len;
+	__le32 header_version;
+	__le32 module_id;
+	__le32 module_vendor;
+	__le32 date;		/* BCD YYYMMDD */
+	__le32 size;
+	__le32 key_size;	/* in dwords */
+	__le32 modulus_size;	/* in dwords */
+	__le32 exponent_size;	/* in dwords */
+	__u8 reserved[88];
+} __attribute__ ((packed));
+
+/* Boot mode opcodes */
+enum i2400m_brh_opcode {
+	I2400M_BRH_READ = 1,
+	I2400M_BRH_WRITE = 2,
+	I2400M_BRH_JUMP = 3,
+	I2400M_BRH_SIGNED_JUMP = 8,
+	I2400M_BRH_HASH_PAYLOAD_ONLY = 9,
+};
+
+/* Boot mode command masks and stuff */
+enum i2400m_brh {
+	I2400M_BRH_SIGNATURE = 0xcbbc0000,
+	I2400M_BRH_SIGNATURE_MASK = 0xffff0000,
+	I2400M_BRH_SIGNATURE_SHIFT = 16,
+	I2400M_BRH_OPCODE_MASK = 0x0000000f,
+	I2400M_BRH_RESPONSE_MASK = 0x000000f0,
+	I2400M_BRH_RESPONSE_SHIFT = 4,
+	I2400M_BRH_DIRECT_ACCESS = 0x00000400,
+	I2400M_BRH_RESPONSE_REQUIRED = 0x00000200,
+	I2400M_BRH_USE_CHECKSUM = 0x00000100,
+};
+
+
+/* Constants for bcf->module_id */
+enum i2400m_bcf_mod_id {
+	/* Firmware file carries its own pokes -- pokes are a set of
+	 * magical values that have to be written in certain memory
+	 * addresses to get the device up and ready for firmware
+	 * download when it is in non-signed boot mode. */
+	I2400M_BCF_MOD_ID_POKES = 0x000000001,
+};
+
+
+/**
+ * i2400m_bootrom_header - Header for a boot-mode command
+ *
+ * @cmd: the above command descriptor
+ * @target_addr: where on the device memory should the action be performed.
+ * @data_size: for read/write, amount of data to be read/written
+ * @block_checksum: checksum value (if applicable)
+ * @payload: the beginning of data attached to this header
+ */
+struct i2400m_bootrom_header {
+	__le32 command;		/* Compose with enum i2400_brh */
+	__le32 target_addr;
+	__le32 data_size;
+	__le32 block_checksum;
+	char payload[0];
+} __attribute__ ((packed));
+
+
+/*
+ * Data / control protocol
+ */
+
+/* Packet types for the host-device interface */
+enum i2400m_pt {
+	I2400M_PT_DATA = 0,
+	I2400M_PT_CTRL,
+	I2400M_PT_TRACE,	/* For device debug */
+	I2400M_PT_RESET_WARM,	/* device reset */
+	I2400M_PT_RESET_COLD,	/* USB[transport] reset, like reconnect */
+	I2400M_PT_ILLEGAL
+};
+
+
+/*
+ * Payload for a data packet
+ *
+ * This is prefixed to each and every outgoing DATA type.
+ */
+struct i2400m_pl_data_hdr {
+	__le32 reserved;
+} __attribute__((packed));
+
+
+/* Misc constants */
+enum {
+	I2400M_PL_PAD = 16,	/* Payload data size alignment */
+	I2400M_PL_SIZE_MAX = 0x3EFF,
+	I2400M_MAX_PLS_IN_MSG = 60,
+	/* protocol barkers: sync sequences; for notifications they
+	 * are sent in groups of four. */
+	I2400M_H2D_PREVIEW_BARKER = 0xcafe900d,
+	I2400M_COLD_RESET_BARKER = 0xc01dc01d,
+	I2400M_WARM_RESET_BARKER = 0x50f750f7,
+	I2400M_NBOOT_BARKER = 0xdeadbeef,
+	I2400M_SBOOT_BARKER = 0x0ff1c1a1,
+	I2400M_ACK_BARKER = 0xfeedbabe,
+	I2400M_D2H_MSG_BARKER = 0xbeefbabe,
+};
+
+
+/*
+ * Hardware payload descriptor
+ *
+ * Bitfields encoded in a struct to enforce typing semantics.
+ *
+ * Look in rx.c and tx.c for a full description of the format.
+ */
+struct i2400m_pld {
+	__le32 val;
+} __attribute__ ((packed));
+
+#define I2400M_PLD_SIZE_MASK 0x00003fff
+#define I2400M_PLD_TYPE_SHIFT 16
+#define I2400M_PLD_TYPE_MASK 0x000f0000
+
+/*
+ * Header for a TX message or RX message
+ *
+ * @barker: preamble
+ * @size: used for management of the FIFO queue buffer; before
+ *     sending, this is converted to be a real preamble. This
+ *     indicates the real size of the TX message that starts at this
+ *     point. If the highest bit is set, then this message is to be
+ *     skipped.
+ * @sequence: sequence number of this message
+ * @offset: offset where the message itself starts -- see the comments
+ *     in the file header about message header and payload descriptor
+ *     alignment.
+ * @num_pls: number of payloads in this message
+ * @padding: amount of padding bytes at the end of the message to make
+ *           it be of block-size aligned
+ *
+ * Look in rx.c and tx.c for a full description of the format.
+ */
+struct i2400m_msg_hdr {
+	union {
+		__le32 barker;
+		__u32 size;	/* same size type as barker!! */
+	};
+	union {
+		__le32 sequence;
+		__u32 offset;	/* same size type as barker!! */
+	};
+	__le16 num_pls;
+	__le16 rsv1;
+	__le16 padding;
+	__le16 rsv2;
+	struct i2400m_pld pld[0];
+} __attribute__ ((packed));
+
+
+
+/*
+ * L3/L4 control protocol
+ */
+
+enum {
+	/* Interface version */
+	I2400M_L3L4_VERSION             = 0x0100,
+};
+
+/* Message types */
+enum i2400m_mt {
+	I2400M_MT_RESERVED              = 0x0000,
+	I2400M_MT_INVALID               = 0xffff,
+	I2400M_MT_REPORT_MASK		= 0x8000,
+
+	I2400M_MT_GET_SCAN_RESULT  	= 0x4202,
+	I2400M_MT_SET_SCAN_PARAM   	= 0x4402,
+	I2400M_MT_CMD_RF_CONTROL   	= 0x4602,
+	I2400M_MT_CMD_SCAN         	= 0x4603,
+	I2400M_MT_CMD_CONNECT      	= 0x4604,
+	I2400M_MT_CMD_DISCONNECT   	= 0x4605,
+	I2400M_MT_CMD_EXIT_IDLE   	= 0x4606,
+	I2400M_MT_GET_LM_VERSION   	= 0x5201,
+	I2400M_MT_GET_DEVICE_INFO  	= 0x5202,
+	I2400M_MT_GET_LINK_STATUS  	= 0x5203,
+	I2400M_MT_GET_STATISTICS   	= 0x5204,
+	I2400M_MT_GET_STATE        	= 0x5205,
+	I2400M_MT_GET_MEDIA_STATUS	= 0x5206,
+	I2400M_MT_SET_INIT_CONFIG	= 0x5404,
+	I2400M_MT_CMD_INIT	        = 0x5601,
+	I2400M_MT_CMD_TERMINATE		= 0x5602,
+	I2400M_MT_CMD_MODE_OF_OP	= 0x5603,
+	I2400M_MT_CMD_RESET_DEVICE	= 0x5604,
+	I2400M_MT_CMD_MONITOR_CONTROL   = 0x5605,
+	I2400M_MT_CMD_ENTER_POWERSAVE   = 0x5606,
+	I2400M_MT_GET_TLS_OPERATION_RESULT = 0x6201,
+	I2400M_MT_SET_EAP_SUCCESS       = 0x6402,
+	I2400M_MT_SET_EAP_FAIL          = 0x6403,
+	I2400M_MT_SET_EAP_KEY          	= 0x6404,
+	I2400M_MT_CMD_SEND_EAP_RESPONSE = 0x6602,
+	I2400M_MT_REPORT_SCAN_RESULT    = 0xc002,
+	I2400M_MT_REPORT_STATE		= 0xd002,
+	I2400M_MT_REPORT_POWERSAVE_READY = 0xd005,
+	I2400M_MT_REPORT_EAP_REQUEST    = 0xe002,
+	I2400M_MT_REPORT_EAP_RESTART    = 0xe003,
+	I2400M_MT_REPORT_ALT_ACCEPT    	= 0xe004,
+	I2400M_MT_REPORT_KEY_REQUEST 	= 0xe005,
+};
+
+
+/*
+ * Message Ack Status codes
+ *
+ * When a message is replied-to, this status is reported.
+ */
+enum i2400m_ms {
+	I2400M_MS_DONE_OK                  = 0,
+	I2400M_MS_DONE_IN_PROGRESS         = 1,
+	I2400M_MS_INVALID_OP               = 2,
+	I2400M_MS_BAD_STATE                = 3,
+	I2400M_MS_ILLEGAL_VALUE            = 4,
+	I2400M_MS_MISSING_PARAMS           = 5,
+	I2400M_MS_VERSION_ERROR            = 6,
+	I2400M_MS_ACCESSIBILITY_ERROR      = 7,
+	I2400M_MS_BUSY                     = 8,
+	I2400M_MS_CORRUPTED_TLV            = 9,
+	I2400M_MS_UNINITIALIZED            = 10,
+	I2400M_MS_UNKNOWN_ERROR            = 11,
+	I2400M_MS_PRODUCTION_ERROR         = 12,
+	I2400M_MS_NO_RF                    = 13,
+	I2400M_MS_NOT_READY_FOR_POWERSAVE  = 14,
+	I2400M_MS_THERMAL_CRITICAL         = 15,
+	I2400M_MS_MAX
+};
+
+
+/**
+ * i2400m_tlv - enumeration of the different types of TLVs
+ *
+ * TLVs stand for type-length-value and are the header for a payload
+ * composed of almost anything. Each payload has a type assigned
+ * and a length.
+ */
+enum i2400m_tlv {
+	I2400M_TLV_L4_MESSAGE_VERSIONS = 129,
+	I2400M_TLV_SYSTEM_STATE = 141,
+	I2400M_TLV_MEDIA_STATUS = 161,
+	I2400M_TLV_RF_OPERATION = 162,
+	I2400M_TLV_RF_STATUS = 163,
+	I2400M_TLV_DEVICE_RESET_TYPE = 132,
+	I2400M_TLV_CONFIG_IDLE_PARAMETERS = 601,
+};
+
+
+struct i2400m_tlv_hdr {
+	__le16 type;
+	__le16 length;		/* payload's */
+	__u8   pl[0];
+} __attribute__((packed));
+
+
+struct i2400m_l3l4_hdr {
+	__le16 type;
+	__le16 length;		/* payload's */
+	__le16 version;
+	__le16 resv1;
+	__le16 status;
+	__le16 resv2;
+	struct i2400m_tlv_hdr pl[0];
+} __attribute__((packed));
+
+
+/**
+ * i2400m_system_state - different states of the device
+ */
+enum i2400m_system_state {
+	I2400M_SS_UNINITIALIZED = 1,
+	I2400M_SS_INIT,
+	I2400M_SS_READY,
+	I2400M_SS_SCAN,
+	I2400M_SS_STANDBY,
+	I2400M_SS_CONNECTING,
+	I2400M_SS_WIMAX_CONNECTED,
+	I2400M_SS_DATA_PATH_CONNECTED,
+	I2400M_SS_IDLE,
+	I2400M_SS_DISCONNECTING,
+	I2400M_SS_OUT_OF_ZONE,
+	I2400M_SS_SLEEPACTIVE,
+	I2400M_SS_PRODUCTION,
+	I2400M_SS_CONFIG,
+	I2400M_SS_RF_OFF,
+	I2400M_SS_RF_SHUTDOWN,
+	I2400M_SS_DEVICE_DISCONNECT,
+	I2400M_SS_MAX,
+};
+
+
+/**
+ * i2400m_tlv_system_state - report on the state of the system
+ *
+ * @state: see enum i2400m_system_state
+ */
+struct i2400m_tlv_system_state {
+	struct i2400m_tlv_hdr hdr;
+	__le32 state;
+} __attribute__((packed));
+
+
+struct i2400m_tlv_l4_message_versions {
+	struct i2400m_tlv_hdr hdr;
+	__le16 major;
+	__le16 minor;
+	__le16 branch;
+	__le16 reserved;
+} __attribute__((packed));
+
+
+struct i2400m_tlv_detailed_device_info {
+	struct i2400m_tlv_hdr hdr;
+	__u8 reserved1[400];
+	__u8 mac_address[6];
+	__u8 reserved2[2];
+} __attribute__((packed));
+
+
+enum i2400m_rf_switch_status {
+	I2400M_RF_SWITCH_ON = 1,
+	I2400M_RF_SWITCH_OFF = 2,
+};
+
+struct i2400m_tlv_rf_switches_status {
+	struct i2400m_tlv_hdr hdr;
+	__u8 sw_rf_switch;	/* 1 ON, 2 OFF */
+	__u8 hw_rf_switch;	/* 1 ON, 2 OFF */
+	__u8 reserved[2];
+} __attribute__((packed));
+
+
+enum {
+	i2400m_rf_operation_on = 1,
+	i2400m_rf_operation_off = 2
+};
+
+struct i2400m_tlv_rf_operation {
+	struct i2400m_tlv_hdr hdr;
+	__le32 status;	/* 1 ON, 2 OFF */
+} __attribute__((packed));
+
+
+enum i2400m_tlv_reset_type {
+	I2400M_RESET_TYPE_COLD = 1,
+	I2400M_RESET_TYPE_WARM
+};
+
+struct i2400m_tlv_device_reset_type {
+	struct i2400m_tlv_hdr hdr;
+	__le32 reset_type;
+} __attribute__((packed));
+
+
+struct i2400m_tlv_config_idle_parameters {
+	struct i2400m_tlv_hdr hdr;
+	__le32 idle_timeout;	/* 100 to 300000 ms [5min], 100 increments
+				 * 0 disabled */
+	__le32 idle_paging_interval;	/* frames */
+} __attribute__((packed));
+
+
+enum i2400m_media_status {
+	I2400M_MEDIA_STATUS_LINK_UP = 1,
+	I2400M_MEDIA_STATUS_LINK_DOWN,
+	I2400M_MEDIA_STATUS_LINK_RENEW,
+};
+
+struct i2400m_tlv_media_status {
+	struct i2400m_tlv_hdr hdr;
+	__le32 media_status;
+} __attribute__((packed));
+
+#endif /* #ifndef __LINUX__WIMAX__I2400M_H__ */
diff --git a/include/linux/writeback.h b/include/linux/writeback.h
index e585657e9831..7300ecdc480c 100644
--- a/include/linux/writeback.h
+++ b/include/linux/writeback.h
@@ -30,7 +30,6 @@ static inline int task_is_pdflush(struct task_struct *task)
 enum writeback_sync_modes {
 	WB_SYNC_NONE,	/* Don't wait on anything */
 	WB_SYNC_ALL,	/* Wait on every mapping */
-	WB_SYNC_HOLD,	/* Hold the inode on sb_dirty for sys_sync() */
 };
 
 /*
@@ -107,7 +106,9 @@ void throttle_vm_writeout(gfp_t gfp_mask);
 
 /* These are exported to sysctl. */
 extern int dirty_background_ratio;
+extern unsigned long dirty_background_bytes;
 extern int vm_dirty_ratio;
+extern unsigned long vm_dirty_bytes;
 extern int dirty_writeback_interval;
 extern int dirty_expire_interval;
 extern int vm_highmem_is_dirtyable;
@@ -116,17 +117,26 @@ extern int laptop_mode;
 
 extern unsigned long determine_dirtyable_memory(void);
 
+extern int dirty_background_ratio_handler(struct ctl_table *table, int write,
+		struct file *filp, void __user *buffer, size_t *lenp,
+		loff_t *ppos);
+extern int dirty_background_bytes_handler(struct ctl_table *table, int write,
+		struct file *filp, void __user *buffer, size_t *lenp,
+		loff_t *ppos);
 extern int dirty_ratio_handler(struct ctl_table *table, int write,
 		struct file *filp, void __user *buffer, size_t *lenp,
 		loff_t *ppos);
+extern int dirty_bytes_handler(struct ctl_table *table, int write,
+		struct file *filp, void __user *buffer, size_t *lenp,
+		loff_t *ppos);
 
 struct ctl_table;
 struct file;
 int dirty_writeback_centisecs_handler(struct ctl_table *, int, struct file *,
 				      void __user *, size_t *, loff_t *);
 
-void get_dirty_limits(long *pbackground, long *pdirty, long *pbdi_dirty,
-		 struct backing_dev_info *bdi);
+void get_dirty_limits(unsigned long *pbackground, unsigned long *pdirty,
+		      unsigned long *pbdi_dirty, struct backing_dev_info *bdi);
 
 void page_writeback_init(void);
 void balance_dirty_pages_ratelimited_nr(struct address_space *mapping,
diff --git a/include/mtd/ubi-user.h b/include/mtd/ubi-user.h
index ccdc562e444e..2dc2eb2b8e22 100644
--- a/include/mtd/ubi-user.h
+++ b/include/mtd/ubi-user.h
@@ -253,7 +253,7 @@ struct ubi_mkvol_req {
  *
  * Re-sizing is possible for both dynamic and static volumes. But while dynamic
  * volumes may be re-sized arbitrarily, static volumes cannot be made to be
- * smaller then the number of bytes they bear. To arbitrarily shrink a static
+ * smaller than the number of bytes they bear. To arbitrarily shrink a static
  * volume, it must be wiped out first (by means of volume update operation with
  * zero number of bytes).
  */
diff --git a/include/net/cipso_ipv4.h b/include/net/cipso_ipv4.h
index 9909774eb998..bedc7f62e35d 100644
--- a/include/net/cipso_ipv4.h
+++ b/include/net/cipso_ipv4.h
@@ -131,7 +131,8 @@ extern int cipso_v4_rbm_strictvalid;
  */
 
 #ifdef CONFIG_NETLABEL
-int cipso_v4_doi_add(struct cipso_v4_doi *doi_def);
+int cipso_v4_doi_add(struct cipso_v4_doi *doi_def,
+		     struct netlbl_audit *audit_info);
 void cipso_v4_doi_free(struct cipso_v4_doi *doi_def);
 int cipso_v4_doi_remove(u32 doi, struct netlbl_audit *audit_info);
 struct cipso_v4_doi *cipso_v4_doi_getdef(u32 doi);
@@ -140,7 +141,8 @@ int cipso_v4_doi_walk(u32 *skip_cnt,
 		     int (*callback) (struct cipso_v4_doi *doi_def, void *arg),
 	             void *cb_arg);
 #else
-static inline int cipso_v4_doi_add(struct cipso_v4_doi *doi_def)
+static inline int cipso_v4_doi_add(struct cipso_v4_doi *doi_def,
+				   struct netlbl_audit *audit_info)
 {
 	return -ENOSYS;
 }
diff --git a/include/net/netlabel.h b/include/net/netlabel.h
index 17c442a4514e..749011eedc0b 100644
--- a/include/net/netlabel.h
+++ b/include/net/netlabel.h
@@ -33,6 +33,8 @@
 #include <linux/types.h>
 #include <linux/net.h>
 #include <linux/skbuff.h>
+#include <linux/in.h>
+#include <linux/in6.h>
 #include <net/netlink.h>
 #include <asm/atomic.h>
 
@@ -353,13 +355,37 @@ static inline void netlbl_secattr_free(struct netlbl_lsm_secattr *secattr)
 /*
  * LSM configuration operations
  */
-int netlbl_cfg_map_del(const char *domain, struct netlbl_audit *audit_info);
-int netlbl_cfg_unlbl_add_map(const char *domain,
+int netlbl_cfg_map_del(const char *domain,
+		       u16 family,
+		       const void *addr,
+		       const void *mask,
+		       struct netlbl_audit *audit_info);
+int netlbl_cfg_unlbl_map_add(const char *domain,
+			     u16 family,
+			     const void *addr,
+			     const void *mask,
 			     struct netlbl_audit *audit_info);
-int netlbl_cfg_cipsov4_add_map(struct cipso_v4_doi *doi_def,
+int netlbl_cfg_unlbl_static_add(struct net *net,
+				const char *dev_name,
+				const void *addr,
+				const void *mask,
+				u16 family,
+				u32 secid,
+				struct netlbl_audit *audit_info);
+int netlbl_cfg_unlbl_static_del(struct net *net,
+				const char *dev_name,
+				const void *addr,
+				const void *mask,
+				u16 family,
+				struct netlbl_audit *audit_info);
+int netlbl_cfg_cipsov4_add(struct cipso_v4_doi *doi_def,
+			   struct netlbl_audit *audit_info);
+void netlbl_cfg_cipsov4_del(u32 doi, struct netlbl_audit *audit_info);
+int netlbl_cfg_cipsov4_map_add(u32 doi,
 			       const char *domain,
+			       const struct in_addr *addr,
+			       const struct in_addr *mask,
 			       struct netlbl_audit *audit_info);
-
 /*
  * LSM security attribute operations
  */
@@ -401,19 +427,62 @@ void netlbl_skbuff_err(struct sk_buff *skb, int error, int gateway);
 void netlbl_cache_invalidate(void);
 int netlbl_cache_add(const struct sk_buff *skb,
 		     const struct netlbl_lsm_secattr *secattr);
+
+/*
+ * Protocol engine operations
+ */
+struct audit_buffer *netlbl_audit_start(int type,
+					struct netlbl_audit *audit_info);
 #else
 static inline int netlbl_cfg_map_del(const char *domain,
+				     u16 family,
+				     const void *addr,
+				     const void *mask,
 				     struct netlbl_audit *audit_info)
 {
 	return -ENOSYS;
 }
-static inline int netlbl_cfg_unlbl_add_map(const char *domain,
+static inline int netlbl_cfg_unlbl_map_add(const char *domain,
+					   u16 family,
+					   void *addr,
+					   void *mask,
 					   struct netlbl_audit *audit_info)
 {
 	return -ENOSYS;
 }
-static inline int netlbl_cfg_cipsov4_add_map(struct cipso_v4_doi *doi_def,
+static inline int netlbl_cfg_unlbl_static_add(struct net *net,
+					      const char *dev_name,
+					      const void *addr,
+					      const void *mask,
+					      u16 family,
+					      u32 secid,
+					      struct netlbl_audit *audit_info)
+{
+	return -ENOSYS;
+}
+static inline int netlbl_cfg_unlbl_static_del(struct net *net,
+					      const char *dev_name,
+					      const void *addr,
+					      const void *mask,
+					      u16 family,
+					      struct netlbl_audit *audit_info)
+{
+	return -ENOSYS;
+}
+static inline int netlbl_cfg_cipsov4_add(struct cipso_v4_doi *doi_def,
+					 struct netlbl_audit *audit_info)
+{
+	return -ENOSYS;
+}
+static inline void netlbl_cfg_cipsov4_del(u32 doi,
+					  struct netlbl_audit *audit_info)
+{
+	return;
+}
+static inline int netlbl_cfg_cipsov4_map_add(u32 doi,
 					     const char *domain,
+					     const struct in_addr *addr,
+					     const struct in_addr *mask,
 					     struct netlbl_audit *audit_info)
 {
 	return -ENOSYS;
@@ -495,6 +564,11 @@ static inline int netlbl_cache_add(const struct sk_buff *skb,
 {
 	return 0;
 }
+static inline struct audit_buffer *netlbl_audit_start(int type,
+						struct netlbl_audit *audit_info)
+{
+	return NULL;
+}
 #endif /* CONFIG_NETLABEL */
 
 #endif /* _NETLABEL_H */
diff --git a/include/net/wimax.h b/include/net/wimax.h
new file mode 100644
index 000000000000..1602614fdaf9
--- /dev/null
+++ b/include/net/wimax.h
@@ -0,0 +1,520 @@
+/*
+ * Linux WiMAX
+ * Kernel space API for accessing WiMAX devices
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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.
+ *
+ *
+ * The WiMAX stack provides an API for controlling and managing the
+ * system's WiMAX devices. This API affects the control plane; the
+ * data plane is accessed via the network stack (netdev).
+ *
+ * Parts of the WiMAX stack API and notifications are exported to
+ * user space via Generic Netlink. In user space, libwimax (part of
+ * the wimax-tools package) provides a shim layer for accessing those
+ * calls.
+ *
+ * The API is standarized for all WiMAX devices and different drivers
+ * implement the backend support for it. However, device-specific
+ * messaging pipes are provided that can be used to issue commands and
+ * receive notifications in free form.
+ *
+ * Currently the messaging pipes are the only means of control as it
+ * is not known (due to the lack of more devices in the market) what
+ * will be a good abstraction layer. Expect this to change as more
+ * devices show in the market. This API is designed to be growable in
+ * order to address this problem.
+ *
+ * USAGE
+ *
+ * Embed a `struct wimax_dev` at the beginning of the the device's
+ * private structure, initialize and register it. For details, see
+ * `struct wimax_dev`s documentation.
+ *
+ * Once this is done, wimax-tools's libwimaxll can be used to
+ * communicate with the driver from user space. You user space
+ * application does not have to forcibily use libwimaxll and can talk
+ * the generic netlink protocol directly if desired.
+ *
+ * Remember this is a very low level API that will to provide all of
+ * WiMAX features. Other daemons and services running in user space
+ * are the expected clients of it. They offer a higher level API that
+ * applications should use (an example of this is the Intel's WiMAX
+ * Network Service for the i2400m).
+ *
+ * DESIGN
+ *
+ * Although not set on final stone, this very basic interface is
+ * mostly completed. Remember this is meant to grow as new common
+ * operations are decided upon. New operations will be added to the
+ * interface, intent being on keeping backwards compatibility as much
+ * as possible.
+ *
+ * This layer implements a set of calls to control a WiMAX device,
+ * exposing a frontend to the rest of the kernel and user space (via
+ * generic netlink) and a backend implementation in the driver through
+ * function pointers.
+ *
+ * WiMAX devices have a state, and a kernel-only API allows the
+ * drivers to manipulate that state. State transitions are atomic, and
+ * only some of them are allowed (see `enum wimax_st`).
+ *
+ * Most API calls will set the state automatically; in most cases
+ * drivers have to only report state changes due to external
+ * conditions.
+ *
+ * All API operations are 'atomic', serialized thorough a mutex in the
+ * `struct wimax_dev`.
+ *
+ * EXPORTING TO USER SPACE THROUGH GENERIC NETLINK
+ *
+ * The API is exported to user space using generic netlink (other
+ * methods can be added as needed).
+ *
+ * There is a Generic Netlink Family named "WiMAX", where interfaces
+ * supporting the WiMAX interface receive commands and broadcast their
+ * signals over a multicast group named "msg".
+ *
+ * Mapping to the source/destination interface is done by an interface
+ * index attribute.
+ *
+ * For user-to-kernel traffic (commands) we use a function call
+ * marshalling mechanism, where a message X with attributes A, B, C
+ * sent from user space to kernel space means executing the WiMAX API
+ * call wimax_X(A, B, C), sending the results back as a message.
+ *
+ * Kernel-to-user (notifications or signals) communication is sent
+ * over multicast groups. This allows to have multiple applications
+ * monitoring them.
+ *
+ * Each command/signal gets assigned it's own attribute policy. This
+ * way the validator will verify that all the attributes in there are
+ * only the ones that should be for each command/signal. Thing of an
+ * attribute mapping to a type+argumentname for each command/signal.
+ *
+ * If we had a single policy for *all* commands/signals, after running
+ * the validator we'd have to check "does this attribute belong in
+ * here"?  for each one. It can be done manually, but it's just easier
+ * to have the validator do that job with multiple policies. As well,
+ * it makes it easier to later expand each command/signal signature
+ * without affecting others and keeping the namespace more or less
+ * sane. Not that it is too complicated, but it makes it even easier.
+ *
+ * No state information is maintained in the kernel for each user
+ * space connection (the connection is stateless).
+ *
+ * TESTING FOR THE INTERFACE AND VERSIONING
+ *
+ * If network interface X is a WiMAX device, there will be a Generic
+ * Netlink family named "WiMAX X" and the device will present a
+ * "wimax" directory in it's network sysfs directory
+ * (/sys/class/net/DEVICE/wimax) [used by HAL].
+ *
+ * The inexistence of any of these means the device does not support
+ * this WiMAX API.
+ *
+ * By querying the generic netlink controller, versioning information
+ * and the multicast groups available can be found. Applications using
+ * the interface can either rely on that or use the generic netlink
+ * controller to figure out which generic netlink commands/signals are
+ * supported.
+ *
+ * NOTE: this versioning is a last resort to avoid hard
+ *    incompatibilities. It is the intention of the design of this
+ *    stack not to introduce backward incompatible changes.
+ *
+ * The version code has to fit in one byte (restrictions imposed by
+ * generic netlink); we use `version / 10` for the major version and
+ * `version % 10` for the minor. This gives 9 minors for each major
+ * and 25 majors.
+ *
+ * The version change protocol is as follow:
+ *
+ * - Major versions: needs to be increased if an existing message/API
+ *   call is changed or removed. Doesn't need to be changed if a new
+ *   message is added.
+ *
+ * - Minor version: needs to be increased if new messages/API calls are
+ *   being added or some other consideration that doesn't impact the
+ *   user-kernel interface too much (like some kind of bug fix) and
+ *   that is kind of left up in the air to common sense.
+ *
+ * User space code should not try to work if the major version it was
+ * compiled for differs from what the kernel offers. As well, if the
+ * minor version of the kernel interface is lower than the one user
+ * space is expecting (the one it was compiled for), the kernel
+ * might be missing API calls; user space shall be ready to handle
+ * said condition. Use the generic netlink controller operations to
+ * find which ones are supported and which not.
+ *
+ * libwimaxll:wimaxll_open() takes care of checking versions.
+ *
+ * THE OPERATIONS:
+ *
+ * Each operation is defined in its on file (drivers/net/wimax/op-*.c)
+ * for clarity. The parts needed for an operation are:
+ *
+ *  - a function pointer in `struct wimax_dev`: optional, as the
+ *    operation might be implemented by the stack and not by the
+ *    driver.
+ *
+ *    All function pointers are named wimax_dev->op_*(), and drivers
+ *    must implement them except where noted otherwise.
+ *
+ *  - When exported to user space, a `struct nla_policy` to define the
+ *    attributes of the generic netlink command and a `struct genl_ops`
+ *    to define the operation.
+ *
+ * All the declarations for the operation codes (WIMAX_GNL_OP_<NAME>)
+ * and generic netlink attributes (WIMAX_GNL_<NAME>_*) are declared in
+ * include/linux/wimax.h; this file is intended to be cloned by user
+ * space to gain access to those declarations.
+ *
+ * A few caveats to remember:
+ *
+ *  - Need to define attribute numbers starting in 1; otherwise it
+ *    fails.
+ *
+ *  - the `struct genl_family` requires a maximum attribute id; when
+ *    defining the `struct nla_policy` for each message, it has to have
+ *    an array size of WIMAX_GNL_ATTR_MAX+1.
+ *
+ * THE PIPE INTERFACE:
+ *
+ * This interface is kept intentionally simple. The driver can send
+ * and receive free-form messages to/from user space through a
+ * pipe. See drivers/net/wimax/op-msg.c for details.
+ *
+ * The kernel-to-user messages are sent with
+ * wimax_msg(). user-to-kernel messages are delivered via
+ * wimax_dev->op_msg_from_user().
+ *
+ * RFKILL:
+ *
+ * RFKILL support is built into the wimax_dev layer; the driver just
+ * needs to call wimax_report_rfkill_{hw,sw}() to inform of changes in
+ * the hardware or software RF kill switches. When the stack wants to
+ * turn the radio off, it will call wimax_dev->op_rfkill_sw_toggle(),
+ * which the driver implements.
+ *
+ * User space can set the software RF Kill switch by calling
+ * wimax_rfkill().
+ *
+ * The code for now only supports devices that don't require polling;
+ * If the device needs to be polled, create a self-rearming delayed
+ * work struct for polling or look into adding polled support to the
+ * WiMAX stack.
+ *
+ * When initializing the hardware (_probe), after calling
+ * wimax_dev_add(), query the device for it's RF Kill switches status
+ * and feed it back to the WiMAX stack using
+ * wimax_report_rfkill_{hw,sw}(). If any switch is missing, always
+ * report it as ON.
+ *
+ * NOTE: the wimax stack uses an inverted terminology to that of the
+ * RFKILL subsystem:
+ *
+ *  - ON: radio is ON, RFKILL is DISABLED or OFF.
+ *  - OFF: radio is OFF, RFKILL is ENABLED or ON.
+ *
+ * MISCELLANEOUS OPS:
+ *
+ * wimax_reset() can be used to reset the device to power on state; by
+ * default it issues a warm reset that maintains the same device
+ * node. If that is not possible, it falls back to a cold reset
+ * (device reconnect). The driver implements the backend to this
+ * through wimax_dev->op_reset().
+ */
+
+#ifndef __NET__WIMAX_H__
+#define __NET__WIMAX_H__
+#ifdef __KERNEL__
+
+#include <linux/wimax.h>
+#include <net/genetlink.h>
+#include <linux/netdevice.h>
+
+struct net_device;
+struct genl_info;
+struct wimax_dev;
+struct input_dev;
+
+/**
+ * struct wimax_dev - Generic WiMAX device
+ *
+ * @net_dev: [fill] Pointer to the &struct net_device this WiMAX
+ *     device implements.
+ *
+ * @op_msg_from_user: [fill] Driver-specific operation to
+ *     handle a raw message from user space to the driver. The
+ *     driver can send messages to user space using with
+ *     wimax_msg_to_user().
+ *
+ * @op_rfkill_sw_toggle: [fill] Driver-specific operation to act on
+ *     userspace (or any other agent) requesting the WiMAX device to
+ *     change the RF Kill software switch (WIMAX_RF_ON or
+ *     WIMAX_RF_OFF).
+ *     If such hardware support is not present, it is assumed the
+ *     radio cannot be switched off and it is always on (and the stack
+ *     will error out when trying to switch it off). In such case,
+ *     this function pointer can be left as NULL.
+ *
+ * @op_reset: [fill] Driver specific operation to reset the
+ *     device.
+ *     This operation should always attempt first a warm reset that
+ *     does not disconnect the device from the bus and return 0.
+ *     If that fails, it should resort to some sort of cold or bus
+ *     reset (even if it implies a bus disconnection and device
+ *     dissapearance). In that case, -ENODEV should be returned to
+ *     indicate the device is gone.
+ *     This operation has to be synchronous, and return only when the
+ *     reset is complete. In case of having had to resort to bus/cold
+ *     reset implying a device disconnection, the call is allowed to
+ *     return inmediately.
+ *     NOTE: wimax_dev->mutex is NOT locked when this op is being
+ *     called; however, wimax_dev->mutex_reset IS locked to ensure
+ *     serialization of calls to wimax_reset().
+ *     See wimax_reset()'s documentation.
+ *
+ * @name: [fill] A way to identify this device. We need to register a
+ *     name with many subsystems (input for RFKILL, workqueue
+ *     creation, etc). We can't use the network device name as that
+ *     might change and in some instances we don't know it yet (until
+ *     we don't call register_netdev()). So we generate an unique one
+ *     using the driver name and device bus id, place it here and use
+ *     it across the board. Recommended naming:
+ *     DRIVERNAME-BUSNAME:BUSID (dev->bus->name, dev->bus_id).
+ *
+ * @id_table_node: [private] link to the list of wimax devices kept by
+ *     id-table.c. Protected by it's own spinlock.
+ *
+ * @mutex: [private] Serializes all concurrent access and execution of
+ *     operations.
+ *
+ * @mutex_reset: [private] Serializes reset operations. Needs to be a
+ *     different mutex because as part of the reset operation, the
+ *     driver has to call back into the stack to do things such as
+ *     state change, that require wimax_dev->mutex.
+ *
+ * @state: [private] Current state of the WiMAX device.
+ *
+ * @rfkill: [private] integration into the RF-Kill infrastructure.
+ *
+ * @rfkill_input: [private] virtual input device to process the
+ *     hardware RF Kill switches.
+ *
+ * @rf_sw: [private] State of the software radio switch (OFF/ON)
+ *
+ * @rf_hw: [private] State of the hardware radio switch (OFF/ON)
+ *
+ * Description:
+ * This structure defines a common interface to access all WiMAX
+ * devices from different vendors and provides a common API as well as
+ * a free-form device-specific messaging channel.
+ *
+ * Usage:
+ *  1. Embed a &struct wimax_dev at *the beginning* the network
+ *     device structure so that netdev_priv() points to it.
+ *
+ *  2. memset() it to zero
+ *
+ *  3. Initialize with wimax_dev_init(). This will leave the WiMAX
+ *     device in the %__WIMAX_ST_NULL state.
+ *
+ *  4. Fill all the fields marked with [fill]; once called
+ *     wimax_dev_add(), those fields CANNOT be modified.
+ *
+ *  5. Call wimax_dev_add() *after* registering the network
+ *     device. This will leave the WiMAX device in the %WIMAX_ST_DOWN
+ *     state.
+ *     Protect the driver's net_device->open() against succeeding if
+ *     the wimax device state is lower than %WIMAX_ST_DOWN.
+ *
+ *  6. Select when the device is going to be turned on/initialized;
+ *     for example, it could be initialized on 'ifconfig up' (when the
+ *     netdev op 'open()' is called on the driver).
+ *
+ * When the device is initialized (at `ifconfig up` time, or right
+ * after calling wimax_dev_add() from _probe(), make sure the
+ * following steps are taken
+ *
+ *  a. Move the device to %WIMAX_ST_UNINITIALIZED. This is needed so
+ *     some API calls that shouldn't work until the device is ready
+ *     can be blocked.
+ *
+ *  b. Initialize the device. Make sure to turn the SW radio switch
+ *     off and move the device to state %WIMAX_ST_RADIO_OFF when
+ *     done. When just initialized, a device should be left in RADIO
+ *     OFF state until user space devices to turn it on.
+ *
+ *  c. Query the device for the state of the hardware rfkill switch
+ *     and call wimax_rfkill_report_hw() and wimax_rfkill_report_sw()
+ *     as needed. See below.
+ *
+ * wimax_dev_rm() undoes before unregistering the network device. Once
+ * wimax_dev_add() is called, the driver can get called on the
+ * wimax_dev->op_* function pointers
+ *
+ * CONCURRENCY:
+ *
+ * The stack provides a mutex for each device that will disallow API
+ * calls happening concurrently; thus, op calls into the driver
+ * through the wimax_dev->op*() function pointers will always be
+ * serialized and *never* concurrent.
+ *
+ * For locking, take wimax_dev->mutex is taken; (most) operations in
+ * the API have to check for wimax_dev_is_ready() to return 0 before
+ * continuing (this is done internally).
+ *
+ * REFERENCE COUNTING:
+ *
+ * The WiMAX device is reference counted by the associated network
+ * device. The only operation that can be used to reference the device
+ * is wimax_dev_get_by_genl_info(), and the reference it acquires has
+ * to be released with dev_put(wimax_dev->net_dev).
+ *
+ * RFKILL:
+ *
+ * At startup, both HW and SW radio switchess are assumed to be off.
+ *
+ * At initialization time [after calling wimax_dev_add()], have the
+ * driver query the device for the status of the software and hardware
+ * RF kill switches and call wimax_report_rfkill_hw() and
+ * wimax_rfkill_report_sw() to indicate their state. If any is
+ * missing, just call it to indicate it is ON (radio always on).
+ *
+ * Whenever the driver detects a change in the state of the RF kill
+ * switches, it should call wimax_report_rfkill_hw() or
+ * wimax_report_rfkill_sw() to report it to the stack.
+ */
+struct wimax_dev {
+	struct net_device *net_dev;
+	struct list_head id_table_node;
+	struct mutex mutex;		/* Protects all members and API calls */
+	struct mutex mutex_reset;
+	enum wimax_st state;
+
+	int (*op_msg_from_user)(struct wimax_dev *wimax_dev,
+				const char *,
+				const void *, size_t,
+				const struct genl_info *info);
+	int (*op_rfkill_sw_toggle)(struct wimax_dev *wimax_dev,
+				   enum wimax_rf_state);
+	int (*op_reset)(struct wimax_dev *wimax_dev);
+
+	struct rfkill *rfkill;
+	struct input_dev *rfkill_input;
+	unsigned rf_hw;
+	unsigned rf_sw;
+	char name[32];
+
+	struct dentry *debugfs_dentry;
+};
+
+
+
+/*
+ * WiMAX stack public API for device drivers
+ * -----------------------------------------
+ *
+ * These functions are not exported to user space.
+ */
+extern void wimax_dev_init(struct wimax_dev *);
+extern int wimax_dev_add(struct wimax_dev *, struct net_device *);
+extern void wimax_dev_rm(struct wimax_dev *);
+
+static inline
+struct wimax_dev *net_dev_to_wimax(struct net_device *net_dev)
+{
+	return netdev_priv(net_dev);
+}
+
+static inline
+struct device *wimax_dev_to_dev(struct wimax_dev *wimax_dev)
+{
+	return wimax_dev->net_dev->dev.parent;
+}
+
+extern void wimax_state_change(struct wimax_dev *, enum wimax_st);
+extern enum wimax_st wimax_state_get(struct wimax_dev *);
+
+/*
+ * Radio Switch state reporting.
+ *
+ * enum wimax_rf_state is declared in linux/wimax.h so the exports
+ * to user space can use it.
+ */
+extern void wimax_report_rfkill_hw(struct wimax_dev *, enum wimax_rf_state);
+extern void wimax_report_rfkill_sw(struct wimax_dev *, enum wimax_rf_state);
+
+
+/*
+ * Free-form messaging to/from user space
+ *
+ * Sending a message:
+ *
+ *   wimax_msg(wimax_dev, pipe_name, buf, buf_size, GFP_KERNEL);
+ *
+ * Broken up:
+ *
+ *   skb = wimax_msg_alloc(wimax_dev, pipe_name, buf_size, GFP_KERNEL);
+ *   ...fill up skb...
+ *   wimax_msg_send(wimax_dev, pipe_name, skb);
+ *
+ * Be sure not to modify skb->data in the middle (ie: don't use
+ * skb_push()/skb_pull()/skb_reserve() on the skb).
+ *
+ * "pipe_name" is any string, than can be interpreted as the name of
+ * the pipe or destinatary; the interpretation of it is driver
+ * specific, so the recipient can multiplex it as wished. It can be
+ * NULL, it won't be used - an example is using a "diagnostics" tag to
+ * send diagnostics information that a device-specific diagnostics
+ * tool would be interested in.
+ */
+extern struct sk_buff *wimax_msg_alloc(struct wimax_dev *, const char *,
+				       const void *, size_t, gfp_t);
+extern int wimax_msg_send(struct wimax_dev *, struct sk_buff *);
+extern int wimax_msg(struct wimax_dev *, const char *,
+		     const void *, size_t, gfp_t);
+
+extern const void *wimax_msg_data_len(struct sk_buff *, size_t *);
+extern const void *wimax_msg_data(struct sk_buff *);
+extern ssize_t wimax_msg_len(struct sk_buff *);
+
+
+/*
+ * WiMAX stack user space API
+ * --------------------------
+ *
+ * This API is what gets exported to user space for general
+ * operations. As well, they can be called from within the kernel,
+ * (with a properly referenced `struct wimax_dev`).
+ *
+ * Properly referenced means: the 'struct net_device' that embeds the
+ * device's control structure and (as such) the 'struct wimax_dev' is
+ * referenced by the caller.
+ */
+extern int wimax_rfkill(struct wimax_dev *, enum wimax_rf_state);
+extern int wimax_reset(struct wimax_dev *);
+
+#else
+/* You might be looking for linux/wimax.h */
+#error This file should not be included from user space.
+#endif /* #ifdef __KERNEL__ */
+#endif /* #ifndef __NET__WIMAX_H__ */
diff --git a/include/sound/soc-dapm.h b/include/sound/soc-dapm.h
index 7ee2f70ca42e..4af1083e3287 100644
--- a/include/sound/soc-dapm.h
+++ b/include/sound/soc-dapm.h
@@ -85,6 +85,10 @@
 #define SND_SOC_DAPM_MUX(wname, wreg, wshift, winvert, wcontrols) \
 {	.id = snd_soc_dapm_mux, .name = wname, .reg = wreg, .shift = wshift, \
 	.invert = winvert, .kcontrols = wcontrols, .num_kcontrols = 1}
+#define SND_SOC_DAPM_VALUE_MUX(wname, wreg, wshift, winvert, wcontrols) \
+{	.id = snd_soc_dapm_value_mux, .name = wname, .reg = wreg, \
+	.shift = wshift, .invert = winvert, .kcontrols = wcontrols, \
+	.num_kcontrols = 1}
 
 /* path domain with event - event handler must return 0 for success */
 #define SND_SOC_DAPM_PGA_E(wname, wreg, wshift, winvert, wcontrols, \
@@ -172,6 +176,12 @@
  	.get = snd_soc_dapm_get_enum_double, \
  	.put = snd_soc_dapm_put_enum_double, \
   	.private_value = (unsigned long)&xenum }
+#define SOC_DAPM_VALUE_ENUM(xname, xenum) \
+{	.iface = SNDRV_CTL_ELEM_IFACE_MIXER, .name = xname, \
+	.info = snd_soc_info_value_enum_double, \
+	.get = snd_soc_dapm_get_value_enum_double, \
+	.put = snd_soc_dapm_put_value_enum_double, \
+	.private_value = (unsigned long)&xenum }
 
 /* dapm stream operations */
 #define SND_SOC_DAPM_STREAM_NOP			0x0
@@ -214,6 +224,10 @@ int snd_soc_dapm_get_enum_double(struct snd_kcontrol *kcontrol,
 	struct snd_ctl_elem_value *ucontrol);
 int snd_soc_dapm_put_enum_double(struct snd_kcontrol *kcontrol,
 	struct snd_ctl_elem_value *ucontrol);
+int snd_soc_dapm_get_value_enum_double(struct snd_kcontrol *kcontrol,
+	struct snd_ctl_elem_value *ucontrol);
+int snd_soc_dapm_put_value_enum_double(struct snd_kcontrol *kcontrol,
+	struct snd_ctl_elem_value *ucontrol);
 int snd_soc_dapm_new_control(struct snd_soc_codec *codec,
 	const struct snd_soc_dapm_widget *widget);
 int snd_soc_dapm_new_controls(struct snd_soc_codec *codec,
@@ -247,6 +261,7 @@ enum snd_soc_dapm_type {
 	snd_soc_dapm_input = 0,		/* input pin */
 	snd_soc_dapm_output,		/* output pin */
 	snd_soc_dapm_mux,			/* selects 1 analog signal from many inputs */
+	snd_soc_dapm_value_mux,			/* selects 1 analog signal from many inputs */
 	snd_soc_dapm_mixer,			/* mixes several analog signals together */
 	snd_soc_dapm_pga,			/* programmable gain/attenuation (volume) */
 	snd_soc_dapm_adc,			/* analog to digital converter */
diff --git a/include/sound/soc.h b/include/sound/soc.h
index f86e455d3828..9b930d342116 100644
--- a/include/sound/soc.h
+++ b/include/sound/soc.h
@@ -94,11 +94,22 @@
 	SOC_ENUM_DOUBLE(xreg, xshift, xshift, xmax, xtexts)
 #define SOC_ENUM_SINGLE_EXT(xmax, xtexts) \
 {	.max = xmax, .texts = xtexts }
+#define SOC_VALUE_ENUM_DOUBLE(xreg, xshift_l, xshift_r, xmask, xmax, xtexts, xvalues) \
+{	.reg = xreg, .shift_l = xshift_l, .shift_r = xshift_r, \
+	.mask = xmask, .max = xmax, .texts = xtexts, .values = xvalues}
+#define SOC_VALUE_ENUM_SINGLE(xreg, xshift, xmask, xmax, xtexts, xvalues) \
+	SOC_VALUE_ENUM_DOUBLE(xreg, xshift, xshift, xmask, xmax, xtexts, xvalues)
 #define SOC_ENUM(xname, xenum) \
 {	.iface = SNDRV_CTL_ELEM_IFACE_MIXER, .name = xname,\
 	.info = snd_soc_info_enum_double, \
 	.get = snd_soc_get_enum_double, .put = snd_soc_put_enum_double, \
 	.private_value = (unsigned long)&xenum }
+#define SOC_VALUE_ENUM(xname, xenum) \
+{	.iface = SNDRV_CTL_ELEM_IFACE_MIXER, .name = xname,\
+	.info = snd_soc_info_value_enum_double, \
+	.get = snd_soc_get_value_enum_double, \
+	.put = snd_soc_put_value_enum_double, \
+	.private_value = (unsigned long)&xenum }
 #define SOC_SINGLE_EXT(xname, xreg, xshift, xmax, xinvert,\
 	 xhandler_get, xhandler_put) \
 {	.iface = SNDRV_CTL_ELEM_IFACE_MIXER, .name = xname, \
@@ -200,6 +211,12 @@ int snd_soc_get_enum_double(struct snd_kcontrol *kcontrol,
 	struct snd_ctl_elem_value *ucontrol);
 int snd_soc_put_enum_double(struct snd_kcontrol *kcontrol,
 	struct snd_ctl_elem_value *ucontrol);
+int snd_soc_info_value_enum_double(struct snd_kcontrol *kcontrol,
+	struct snd_ctl_elem_info *uinfo);
+int snd_soc_get_value_enum_double(struct snd_kcontrol *kcontrol,
+	struct snd_ctl_elem_value *ucontrol);
+int snd_soc_put_value_enum_double(struct snd_kcontrol *kcontrol,
+	struct snd_ctl_elem_value *ucontrol);
 int snd_soc_info_volsw(struct snd_kcontrol *kcontrol,
 	struct snd_ctl_elem_info *uinfo);
 int snd_soc_info_volsw_ext(struct snd_kcontrol *kcontrol,
@@ -406,6 +423,19 @@ struct soc_enum {
 	void *dapm;
 };
 
+/* semi enumerated kcontrol */
+struct soc_value_enum {
+	unsigned short reg;
+	unsigned short reg2;
+	unsigned char shift_l;
+	unsigned char shift_r;
+	unsigned int max;
+	unsigned int mask;
+	const char **texts;
+	const unsigned int *values;
+	void *dapm;
+};
+
 #include <sound/soc-dai.h>
 
 #endif
diff --git a/init/Kconfig b/init/Kconfig
index 52847eec7398..e7893b1d3e42 100644
--- a/init/Kconfig
+++ b/init/Kconfig
@@ -423,27 +423,37 @@ config SYSFS_DEPRECATED
 	bool
 
 config SYSFS_DEPRECATED_V2
-	bool "Create deprecated sysfs files"
+	bool "Create deprecated sysfs layout for older userspace tools"
 	depends on SYSFS
 	default y
 	select SYSFS_DEPRECATED
 	help
-	  This option creates deprecated symlinks such as the
-	  "device"-link, the <subsystem>:<name>-link, and the
-	  "bus"-link. It may also add deprecated key in the
-	  uevent environment.
-	  None of these features or values should be used today, as
-	  they export driver core implementation details to userspace
-	  or export properties which can't be kept stable across kernel
-	  releases.
-
-	  If enabled, this option will also move any device structures
-	  that belong to a class, back into the /sys/class hierarchy, in
-	  order to support older versions of udev and some userspace
-	  programs.
-
-	  If you are using a distro with the most recent userspace
-	  packages, it should be safe to say N here.
+	  This option switches the layout of sysfs to the deprecated
+	  version.
+
+	  The current sysfs layout features a unified device tree at
+	  /sys/devices/, which is able to express a hierarchy between
+	  class devices. If the deprecated option is set to Y, the
+	  unified device tree is split into a bus device tree at
+	  /sys/devices/ and several individual class device trees at
+	  /sys/class/. The class and bus devices will be connected by
+	  "<subsystem>:<name>" and the "device" links. The "block"
+	  class devices, will not show up in /sys/class/block/. Some
+	  subsystems will suppress the creation of some devices which
+	  depend on the unified device tree.
+
+	  This option is not a pure compatibility option that can
+	  be safely enabled on newer distributions. It will change the
+	  layout of sysfs to the non-extensible deprecated version,
+	  and disable some features, which can not be exported without
+	  confusing older userspace tools. Since 2007/2008 all major
+	  distributions do not enable this option, and ship no tools which
+	  depend on the deprecated layout or this option.
+
+	  If you are using a new kernel on an older distribution, or use
+	  older userspace tools, you might need to say Y here. Do not say Y,
+	  if the original kernel, that came with your distribution, has
+	  this option set to N.
 
 config PROC_PID_CPUSET
 	bool "Include legacy /proc/<pid>/cpuset file"
@@ -838,10 +848,6 @@ config RT_MUTEXES
 	boolean
 	select PLIST
 
-config TINY_SHMEM
-	default !SHMEM
-	bool
-
 config BASE_SMALL
 	int
 	default 0 if BASE_FULL
diff --git a/init/do_mounts.c b/init/do_mounts.c
index d055b1914c3d..708105e163df 100644
--- a/init/do_mounts.c
+++ b/init/do_mounts.c
@@ -13,6 +13,7 @@
 #include <linux/init.h>
 #include <linux/fs.h>
 #include <linux/initrd.h>
+#include <linux/async.h>
 
 #include <linux/nfs_fs.h>
 #include <linux/nfs_fs_sb.h>
@@ -220,10 +221,10 @@ static int __init do_mount_root(char *name, char *fs, int flags, void *data)
 
 	sys_chdir("/root");
 	ROOT_DEV = current->fs->pwd.mnt->mnt_sb->s_dev;
-	printk("VFS: Mounted root (%s filesystem)%s.\n",
+	printk("VFS: Mounted root (%s filesystem)%s on device %u:%u.\n",
 	       current->fs->pwd.mnt->mnt_sb->s_type->name,
 	       current->fs->pwd.mnt->mnt_sb->s_flags & MS_RDONLY ?
-	       " readonly" : "");
+	       " readonly" : "", MAJOR(ROOT_DEV), MINOR(ROOT_DEV));
 	return 0;
 }
 
@@ -372,6 +373,7 @@ void __init prepare_namespace(void)
 	/* wait for the known devices to complete their probing */
 	while (driver_probe_done() != 0)
 		msleep(100);
+	async_synchronize_full();
 
 	md_run_setup();
 
diff --git a/init/do_mounts_md.c b/init/do_mounts_md.c
index d6da5cdd3c38..ff95e3192884 100644
--- a/init/do_mounts_md.c
+++ b/init/do_mounts_md.c
@@ -271,7 +271,7 @@ static int __init raid_setup(char *str)
 __setup("raid=", raid_setup);
 __setup("md=", md_setup);
 
-static void autodetect_raid(void)
+static void __init autodetect_raid(void)
 {
 	int fd;
 
diff --git a/init/main.c b/init/main.c
index cd168ebc5924..844209453c02 100644
--- a/init/main.c
+++ b/init/main.c
@@ -50,7 +50,6 @@
 #include <linux/rmap.h>
 #include <linux/mempolicy.h>
 #include <linux/key.h>
-#include <linux/unwind.h>
 #include <linux/buffer_head.h>
 #include <linux/page_cgroup.h>
 #include <linux/debug_locks.h>
@@ -63,6 +62,7 @@
 #include <linux/signal.h>
 #include <linux/idr.h>
 #include <linux/ftrace.h>
+#include <linux/async.h>
 #include <trace/boot.h>
 
 #include <asm/io.h>
@@ -108,7 +108,7 @@ EXPORT_SYMBOL(system_state);
 
 extern void time_init(void);
 /* Default late time init is NULL. archs can override this later. */
-void (*late_time_init)(void);
+void (*__initdata late_time_init)(void);
 extern void softirq_init(void);
 
 /* Untouched command line saved by arch-specific code. */
@@ -447,7 +447,7 @@ static void __init setup_command_line(char *command_line)
  * gcc-3.4 accidentally inlines this function, so use noinline.
  */
 
-static void noinline __init_refok rest_init(void)
+static noinline void __init_refok rest_init(void)
 	__releases(kernel_lock)
 {
 	int pid;
@@ -537,7 +537,6 @@ asmlinkage void __init start_kernel(void)
 	 * Need to run as early as possible, to initialize the
 	 * lockdep hash:
 	 */
-	unwind_init();
 	lockdep_init();
 	debug_objects_early_init();
 	cgroup_init_early();
@@ -559,7 +558,6 @@ asmlinkage void __init start_kernel(void)
 	setup_arch(&command_line);
 	mm_init_owner(&init_mm, &init_task);
 	setup_command_line(command_line);
-	unwind_setup();
 	setup_per_cpu_areas();
 	setup_nr_cpu_ids();
 	smp_prepare_boot_cpu();	/* arch-specific boot-cpu hooks */
@@ -602,7 +600,8 @@ asmlinkage void __init start_kernel(void)
 	sched_clock_init();
 	profile_init();
 	if (!irqs_disabled())
-		printk("start_kernel(): bug: interrupts were enabled early\n");
+		printk(KERN_CRIT "start_kernel(): bug: interrupts were "
+				 "enabled early\n");
 	early_boot_irqs_on();
 	local_irq_enable();
 
@@ -687,7 +686,7 @@ asmlinkage void __init start_kernel(void)
 	rest_init();
 }
 
-static int initcall_debug;
+int initcall_debug;
 core_param(initcall_debug, initcall_debug, bool, 0644);
 
 int do_one_initcall(initcall_t fn)
@@ -786,8 +785,10 @@ static void run_init_process(char *init_filename)
 /* This is a non __init function. Force it to be noinline otherwise gcc
  * makes it inline to init() and it becomes part of init.text section
  */
-static int noinline init_post(void)
+static noinline int init_post(void)
 {
+	/* need to finish all async __init code before freeing the memory */
+	async_synchronize_full();
 	free_initmem();
 	unlock_kernel();
 	mark_rodata_ro();
diff --git a/ipc/ipc_sysctl.c b/ipc/ipc_sysctl.c
index 0dfebc509426..4a7a12c95abe 100644
--- a/ipc/ipc_sysctl.c
+++ b/ipc/ipc_sysctl.c
@@ -26,29 +26,6 @@ static void *get_ipc(ctl_table *table)
 	return which;
 }
 
-/*
- * Routine that is called when the file "auto_msgmni" has successfully been
- * written.
- * Two values are allowed:
- * 0: unregister msgmni's callback routine from the ipc namespace notifier
- *    chain. This means that msgmni won't be recomputed anymore upon memory
- *    add/remove or ipc namespace creation/removal.
- * 1: register back the callback routine.
- */
-static void ipc_auto_callback(int val)
-{
-	if (!val)
-		unregister_ipcns_notifier(current->nsproxy->ipc_ns);
-	else {
-		/*
-		 * Re-enable automatic recomputing only if not already
-		 * enabled.
-		 */
-		recompute_msgmni(current->nsproxy->ipc_ns);
-		cond_register_ipcns_notifier(current->nsproxy->ipc_ns);
-	}
-}
-
 #ifdef CONFIG_PROC_FS
 static int proc_ipc_dointvec(ctl_table *table, int write, struct file *filp,
 	void __user *buffer, size_t *lenp, loff_t *ppos)
@@ -94,6 +71,29 @@ static int proc_ipc_doulongvec_minmax(ctl_table *table, int write,
 					lenp, ppos);
 }
 
+/*
+ * Routine that is called when the file "auto_msgmni" has successfully been
+ * written.
+ * Two values are allowed:
+ * 0: unregister msgmni's callback routine from the ipc namespace notifier
+ *    chain. This means that msgmni won't be recomputed anymore upon memory
+ *    add/remove or ipc namespace creation/removal.
+ * 1: register back the callback routine.
+ */
+static void ipc_auto_callback(int val)
+{
+	if (!val)
+		unregister_ipcns_notifier(current->nsproxy->ipc_ns);
+	else {
+		/*
+		 * Re-enable automatic recomputing only if not already
+		 * enabled.
+		 */
+		recompute_msgmni(current->nsproxy->ipc_ns);
+		cond_register_ipcns_notifier(current->nsproxy->ipc_ns);
+	}
+}
+
 static int proc_ipcauto_dointvec_minmax(ctl_table *table, int write,
 	struct file *filp, void __user *buffer, size_t *lenp, loff_t *ppos)
 {
diff --git a/ipc/sem.c b/ipc/sem.c
index fea0ad3aed7b..c68cd3f8f0c9 100644
--- a/ipc/sem.c
+++ b/ipc/sem.c
@@ -1216,7 +1216,6 @@ asmlinkage long sys_semtimedop(int semid, struct sembuf __user *tsops,
 	if (timeout && jiffies_left == 0)
 		error = -EAGAIN;
 	list_del(&queue.list);
-	goto out_unlock_free;
 
 out_unlock_free:
 	sem_unlock(sma);
diff --git a/ipc/shm.c b/ipc/shm.c
index 57dd50046cef..b125b560240e 100644
--- a/ipc/shm.c
+++ b/ipc/shm.c
@@ -75,7 +75,7 @@ void shm_init_ns(struct ipc_namespace *ns)
 	ns->shm_ctlall = SHMALL;
 	ns->shm_ctlmni = SHMMNI;
 	ns->shm_tot = 0;
-	ipc_init_ids(&ns->ids[IPC_SHM_IDS]);
+	ipc_init_ids(&shm_ids(ns));
 }
 
 /*
@@ -644,7 +644,7 @@ asmlinkage long sys_shmctl(int shmid, int cmd, struct shmid_ds __user *buf)
 		if (err)
 			return err;
 
-		memset(&shminfo,0,sizeof(shminfo));
+		memset(&shminfo, 0, sizeof(shminfo));
 		shminfo.shmmni = shminfo.shmseg = ns->shm_ctlmni;
 		shminfo.shmmax = ns->shm_ctlmax;
 		shminfo.shmall = ns->shm_ctlall;
@@ -669,7 +669,7 @@ asmlinkage long sys_shmctl(int shmid, int cmd, struct shmid_ds __user *buf)
 		if (err)
 			return err;
 
-		memset(&shm_info,0,sizeof(shm_info));
+		memset(&shm_info, 0, sizeof(shm_info));
 		down_read(&shm_ids(ns).rw_mutex);
 		shm_info.used_ids = shm_ids(ns).in_use;
 		shm_get_stat (ns, &shm_info.shm_rss, &shm_info.shm_swp);
@@ -678,7 +678,7 @@ asmlinkage long sys_shmctl(int shmid, int cmd, struct shmid_ds __user *buf)
 		shm_info.swap_successes = 0;
 		err = ipc_get_maxid(&shm_ids(ns));
 		up_read(&shm_ids(ns).rw_mutex);
-		if(copy_to_user (buf, &shm_info, sizeof(shm_info))) {
+		if (copy_to_user(buf, &shm_info, sizeof(shm_info))) {
 			err = -EFAULT;
 			goto out;
 		}
@@ -692,11 +692,6 @@ asmlinkage long sys_shmctl(int shmid, int cmd, struct shmid_ds __user *buf)
 		struct shmid64_ds tbuf;
 		int result;
 
-		if (!buf) {
-			err = -EFAULT;
-			goto out;
-		}
-
 		if (cmd == SHM_STAT) {
 			shp = shm_lock(ns, shmid);
 			if (IS_ERR(shp)) {
@@ -712,7 +707,7 @@ asmlinkage long sys_shmctl(int shmid, int cmd, struct shmid_ds __user *buf)
 			}
 			result = 0;
 		}
-		err=-EACCES;
+		err = -EACCES;
 		if (ipcperms (&shp->shm_perm, S_IRUGO))
 			goto out_unlock;
 		err = security_shm_shmctl(shp, cmd);
diff --git a/kernel/Makefile b/kernel/Makefile
index e1c5bf3365c0..2921d90ce32f 100644
--- a/kernel/Makefile
+++ b/kernel/Makefile
@@ -9,7 +9,8 @@ obj-y     = sched.o fork.o exec_domain.o panic.o printk.o \
 	    rcupdate.o extable.o params.o posix-timers.o \
 	    kthread.o wait.o kfifo.o sys_ni.o posix-cpu-timers.o mutex.o \
 	    hrtimer.o rwsem.o nsproxy.o srcu.o semaphore.o \
-	    notifier.o ksysfs.o pm_qos_params.o sched_clock.o cred.o
+	    notifier.o ksysfs.o pm_qos_params.o sched_clock.o cred.o \
+	    async.o
 
 ifdef CONFIG_FUNCTION_TRACER
 # Do not trace debug files and internal ftrace files
diff --git a/kernel/async.c b/kernel/async.c
new file mode 100644
index 000000000000..97373380c9e7
--- /dev/null
+++ b/kernel/async.c
@@ -0,0 +1,321 @@
+/*
+ * async.c: Asynchronous function calls for boot performance
+ *
+ * (C) Copyright 2009 Intel Corporation
+ * Author: Arjan van de Ven <arjan@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
+ * as published by the Free Software Foundation; version 2
+ * of the License.
+ */
+
+
+/*
+
+Goals and Theory of Operation
+
+The primary goal of this feature is to reduce the kernel boot time,
+by doing various independent hardware delays and discovery operations
+decoupled and not strictly serialized.
+
+More specifically, the asynchronous function call concept allows
+certain operations (primarily during system boot) to happen
+asynchronously, out of order, while these operations still
+have their externally visible parts happen sequentially and in-order.
+(not unlike how out-of-order CPUs retire their instructions in order)
+
+Key to the asynchronous function call implementation is the concept of
+a "sequence cookie" (which, although it has an abstracted type, can be
+thought of as a monotonically incrementing number).
+
+The async core will assign each scheduled event such a sequence cookie and
+pass this to the called functions.
+
+The asynchronously called function should before doing a globally visible
+operation, such as registering device numbers, call the
+async_synchronize_cookie() function and pass in its own cookie. The
+async_synchronize_cookie() function will make sure that all asynchronous
+operations that were scheduled prior to the operation corresponding with the
+cookie have completed.
+
+Subsystem/driver initialization code that scheduled asynchronous probe
+functions, but which shares global resources with other drivers/subsystems
+that do not use the asynchronous call feature, need to do a full
+synchronization with the async_synchronize_full() function, before returning
+from their init function. This is to maintain strict ordering between the
+asynchronous and synchronous parts of the kernel.
+
+*/
+
+#include <linux/async.h>
+#include <linux/module.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/init.h>
+#include <linux/kthread.h>
+#include <asm/atomic.h>
+
+static async_cookie_t next_cookie = 1;
+
+#define MAX_THREADS	256
+#define MAX_WORK	32768
+
+static LIST_HEAD(async_pending);
+static LIST_HEAD(async_running);
+static DEFINE_SPINLOCK(async_lock);
+
+struct async_entry {
+	struct list_head list;
+	async_cookie_t   cookie;
+	async_func_ptr	 *func;
+	void             *data;
+	struct list_head *running;
+};
+
+static DECLARE_WAIT_QUEUE_HEAD(async_done);
+static DECLARE_WAIT_QUEUE_HEAD(async_new);
+
+static atomic_t entry_count;
+static atomic_t thread_count;
+
+extern int initcall_debug;
+
+
+/*
+ * MUST be called with the lock held!
+ */
+static async_cookie_t  __lowest_in_progress(struct list_head *running)
+{
+	struct async_entry *entry;
+	if (!list_empty(&async_pending)) {
+		entry = list_first_entry(&async_pending,
+			struct async_entry, list);
+		return entry->cookie;
+	} else if (!list_empty(running)) {
+		entry = list_first_entry(running,
+			struct async_entry, list);
+		return entry->cookie;
+	} else {
+		/* nothing in progress... next_cookie is "infinity" */
+		return next_cookie;
+	}
+
+}
+/*
+ * pick the first pending entry and run it
+ */
+static void run_one_entry(void)
+{
+	unsigned long flags;
+	struct async_entry *entry;
+	ktime_t calltime, delta, rettime;
+
+	/* 1) pick one task from the pending queue */
+
+	spin_lock_irqsave(&async_lock, flags);
+	if (list_empty(&async_pending))
+		goto out;
+	entry = list_first_entry(&async_pending, struct async_entry, list);
+
+	/* 2) move it to the running queue */
+	list_del(&entry->list);
+	list_add_tail(&entry->list, &async_running);
+	spin_unlock_irqrestore(&async_lock, flags);
+
+	/* 3) run it (and print duration)*/
+	if (initcall_debug && system_state == SYSTEM_BOOTING) {
+		printk("calling  %lli_%pF @ %i\n", entry->cookie, entry->func, task_pid_nr(current));
+		calltime = ktime_get();
+	}
+	entry->func(entry->data, entry->cookie);
+	if (initcall_debug && system_state == SYSTEM_BOOTING) {
+		rettime = ktime_get();
+		delta = ktime_sub(rettime, calltime);
+		printk("initcall %lli_%pF returned 0 after %lld usecs\n", entry->cookie,
+			entry->func, ktime_to_ns(delta) >> 10);
+	}
+
+	/* 4) remove it from the running queue */
+	spin_lock_irqsave(&async_lock, flags);
+	list_del(&entry->list);
+
+	/* 5) free the entry  */
+	kfree(entry);
+	atomic_dec(&entry_count);
+
+	spin_unlock_irqrestore(&async_lock, flags);
+
+	/* 6) wake up any waiters. */
+	wake_up(&async_done);
+	return;
+
+out:
+	spin_unlock_irqrestore(&async_lock, flags);
+}
+
+
+static async_cookie_t __async_schedule(async_func_ptr *ptr, void *data, struct list_head *running)
+{
+	struct async_entry *entry;
+	unsigned long flags;
+	async_cookie_t newcookie;
+	
+
+	/* allow irq-off callers */
+	entry = kzalloc(sizeof(struct async_entry), GFP_ATOMIC);
+
+	/*
+	 * If we're out of memory or if there's too much work
+	 * pending already, we execute synchronously.
+	 */
+	if (!entry || atomic_read(&entry_count) > MAX_WORK) {
+		kfree(entry);
+		spin_lock_irqsave(&async_lock, flags);
+		newcookie = next_cookie++;
+		spin_unlock_irqrestore(&async_lock, flags);
+
+		/* low on memory.. run synchronously */
+		ptr(data, newcookie);
+		return newcookie;
+	}
+	entry->func = ptr;
+	entry->data = data;
+	entry->running = running;
+
+	spin_lock_irqsave(&async_lock, flags);
+	newcookie = entry->cookie = next_cookie++;
+	list_add_tail(&entry->list, &async_pending);
+	atomic_inc(&entry_count);
+	spin_unlock_irqrestore(&async_lock, flags);
+	wake_up(&async_new);
+	return newcookie;
+}
+
+async_cookie_t async_schedule(async_func_ptr *ptr, void *data)
+{
+	return __async_schedule(ptr, data, &async_pending);
+}
+EXPORT_SYMBOL_GPL(async_schedule);
+
+async_cookie_t async_schedule_special(async_func_ptr *ptr, void *data, struct list_head *running)
+{
+	return __async_schedule(ptr, data, running);
+}
+EXPORT_SYMBOL_GPL(async_schedule_special);
+
+void async_synchronize_full(void)
+{
+	async_synchronize_cookie(next_cookie);
+}
+EXPORT_SYMBOL_GPL(async_synchronize_full);
+
+void async_synchronize_full_special(struct list_head *list)
+{
+	async_synchronize_cookie_special(next_cookie, list);
+}
+EXPORT_SYMBOL_GPL(async_synchronize_full_special);
+
+void async_synchronize_cookie_special(async_cookie_t cookie, struct list_head *running)
+{
+	ktime_t starttime, delta, endtime;
+
+	if (initcall_debug && system_state == SYSTEM_BOOTING) {
+		printk("async_waiting @ %i\n", task_pid_nr(current));
+		starttime = ktime_get();
+	}
+
+	wait_event(async_done, __lowest_in_progress(running) >= cookie);
+
+	if (initcall_debug && system_state == SYSTEM_BOOTING) {
+		endtime = ktime_get();
+		delta = ktime_sub(endtime, starttime);
+
+		printk("async_continuing @ %i after %lli usec\n",
+			task_pid_nr(current), ktime_to_ns(delta) >> 10);
+	}
+}
+EXPORT_SYMBOL_GPL(async_synchronize_cookie_special);
+
+void async_synchronize_cookie(async_cookie_t cookie)
+{
+	async_synchronize_cookie_special(cookie, &async_running);
+}
+EXPORT_SYMBOL_GPL(async_synchronize_cookie);
+
+
+static int async_thread(void *unused)
+{
+	DECLARE_WAITQUEUE(wq, current);
+	add_wait_queue(&async_new, &wq);
+
+	while (!kthread_should_stop()) {
+		int ret = HZ;
+		set_current_state(TASK_INTERRUPTIBLE);
+		/*
+		 * check the list head without lock.. false positives
+		 * are dealt with inside run_one_entry() while holding
+		 * the lock.
+		 */
+		rmb();
+		if (!list_empty(&async_pending))
+			run_one_entry();
+		else
+			ret = schedule_timeout(HZ);
+
+		if (ret == 0) {
+			/*
+			 * we timed out, this means we as thread are redundant.
+			 * we sign off and die, but we to avoid any races there
+			 * is a last-straw check to see if work snuck in.
+			 */
+			atomic_dec(&thread_count);
+			wmb(); /* manager must see our departure first */
+			if (list_empty(&async_pending))
+				break;
+			/*
+			 * woops work came in between us timing out and us
+			 * signing off; we need to stay alive and keep working.
+			 */
+			atomic_inc(&thread_count);
+		}
+	}
+	remove_wait_queue(&async_new, &wq);
+
+	return 0;
+}
+
+static int async_manager_thread(void *unused)
+{
+	DECLARE_WAITQUEUE(wq, current);
+	add_wait_queue(&async_new, &wq);
+
+	while (!kthread_should_stop()) {
+		int tc, ec;
+
+		set_current_state(TASK_INTERRUPTIBLE);
+
+		tc = atomic_read(&thread_count);
+		rmb();
+		ec = atomic_read(&entry_count);
+
+		while (tc < ec && tc < MAX_THREADS) {
+			kthread_run(async_thread, NULL, "async/%i", tc);
+			atomic_inc(&thread_count);
+			tc++;
+		}
+
+		schedule();
+	}
+	remove_wait_queue(&async_new, &wq);
+
+	return 0;
+}
+
+static int __init async_init(void)
+{
+	kthread_run(async_manager_thread, NULL, "async/mgr");
+	return 0;
+}
+
+core_initcall(async_init);
diff --git a/kernel/capability.c b/kernel/capability.c
index c598d9d5be4f..688926e496be 100644
--- a/kernel/capability.c
+++ b/kernel/capability.c
@@ -306,7 +306,7 @@ int capable(int cap)
 		BUG();
 	}
 
-	if (has_capability(current, cap)) {
+	if (security_capable(cap) == 0) {
 		current->flags |= PF_SUPERPRIV;
 		return 1;
 	}
diff --git a/kernel/cgroup.c b/kernel/cgroup.c
index 87bb0258fd27..f221446aa02d 100644
--- a/kernel/cgroup.c
+++ b/kernel/cgroup.c
@@ -116,7 +116,6 @@ static int root_count;
  * be called.
  */
 static int need_forkexit_callback __read_mostly;
-static int need_mm_owner_callback __read_mostly;
 
 /* convenient tests for these bits */
 inline int cgroup_is_removed(const struct cgroup *cgrp)
@@ -2539,7 +2538,6 @@ static void __init cgroup_init_subsys(struct cgroup_subsys *ss)
 	init_css_set.subsys[ss->subsys_id] = dummytop->subsys[ss->subsys_id];
 
 	need_forkexit_callback |= ss->fork || ss->exit;
-	need_mm_owner_callback |= !!ss->mm_owner_changed;
 
 	/* At system boot, before all subsystems have been
 	 * registered, no tasks have been forked, so we don't
@@ -2789,37 +2787,6 @@ void cgroup_fork_callbacks(struct task_struct *child)
 	}
 }
 
-#ifdef CONFIG_MM_OWNER
-/**
- * cgroup_mm_owner_callbacks - run callbacks when the mm->owner changes
- * @p: the new owner
- *
- * Called on every change to mm->owner. mm_init_owner() does not
- * invoke this routine, since it assigns the mm->owner the first time
- * and does not change it.
- *
- * The callbacks are invoked with mmap_sem held in read mode.
- */
-void cgroup_mm_owner_callbacks(struct task_struct *old, struct task_struct *new)
-{
-	struct cgroup *oldcgrp, *newcgrp = NULL;
-
-	if (need_mm_owner_callback) {
-		int i;
-		for (i = 0; i < CGROUP_SUBSYS_COUNT; i++) {
-			struct cgroup_subsys *ss = subsys[i];
-			oldcgrp = task_cgroup(old, ss->subsys_id);
-			if (new)
-				newcgrp = task_cgroup(new, ss->subsys_id);
-			if (oldcgrp == newcgrp)
-				continue;
-			if (ss->mm_owner_changed)
-				ss->mm_owner_changed(ss, oldcgrp, newcgrp, new);
-		}
-	}
-}
-#endif /* CONFIG_MM_OWNER */
-
 /**
  * cgroup_post_fork - called on a new task after adding it to the task list
  * @child: the task in question
diff --git a/kernel/compat.c b/kernel/compat.c
index d52e2ec1deb5..42d56544460f 100644
--- a/kernel/compat.c
+++ b/kernel/compat.c
@@ -24,6 +24,7 @@
 #include <linux/migrate.h>
 #include <linux/posix-timers.h>
 #include <linux/times.h>
+#include <linux/ptrace.h>
 
 #include <asm/uaccess.h>
 
@@ -229,6 +230,7 @@ asmlinkage long compat_sys_times(struct compat_tms __user *tbuf)
 		if (copy_to_user(tbuf, &tmp, sizeof(tmp)))
 			return -EFAULT;
 	}
+	force_successful_syscall_return();
 	return compat_jiffies_to_clock_t(jiffies);
 }
 
@@ -894,8 +896,9 @@ asmlinkage long compat_sys_time(compat_time_t __user * tloc)
 
 	if (tloc) {
 		if (put_user(i,tloc))
-			i = -EFAULT;
+			return -EFAULT;
 	}
+	force_successful_syscall_return();
 	return i;
 }
 
diff --git a/kernel/cpu.c b/kernel/cpu.c
index 30e74dd6d01b..79e40f00dcb8 100644
--- a/kernel/cpu.c
+++ b/kernel/cpu.c
@@ -379,8 +379,11 @@ static cpumask_var_t frozen_cpus;
 
 int disable_nonboot_cpus(void)
 {
-	int cpu, first_cpu, error = 0;
+	int cpu, first_cpu, error;
 
+	error = stop_machine_create();
+	if (error)
+		return error;
 	cpu_maps_update_begin();
 	first_cpu = cpumask_first(cpu_online_mask);
 	/* We take down all of the non-boot CPUs in one shot to avoid races
@@ -409,6 +412,7 @@ int disable_nonboot_cpus(void)
 		printk(KERN_ERR "Non-boot CPUs are not disabled\n");
 	}
 	cpu_maps_update_done();
+	stop_machine_destroy();
 	return error;
 }
 
diff --git a/kernel/cpuset.c b/kernel/cpuset.c
index 39c1a4c1c5a9..345ace5117de 100644
--- a/kernel/cpuset.c
+++ b/kernel/cpuset.c
@@ -240,6 +240,17 @@ static struct cpuset top_cpuset = {
 static DEFINE_MUTEX(callback_mutex);
 
 /*
+ * cpuset_buffer_lock protects both the cpuset_name and cpuset_nodelist
+ * buffers.  They are statically allocated to prevent using excess stack
+ * when calling cpuset_print_task_mems_allowed().
+ */
+#define CPUSET_NAME_LEN		(128)
+#define	CPUSET_NODELIST_LEN	(256)
+static char cpuset_name[CPUSET_NAME_LEN];
+static char cpuset_nodelist[CPUSET_NODELIST_LEN];
+static DEFINE_SPINLOCK(cpuset_buffer_lock);
+
+/*
  * This is ugly, but preserves the userspace API for existing cpuset
  * users. If someone tries to mount the "cpuset" filesystem, we
  * silently switch it to mount "cgroup" instead
@@ -2356,6 +2367,29 @@ int cpuset_mems_allowed_intersects(const struct task_struct *tsk1,
 	return nodes_intersects(tsk1->mems_allowed, tsk2->mems_allowed);
 }
 
+/**
+ * cpuset_print_task_mems_allowed - prints task's cpuset and mems_allowed
+ * @task: pointer to task_struct of some task.
+ *
+ * Description: Prints @task's name, cpuset name, and cached copy of its
+ * mems_allowed to the kernel log.  Must hold task_lock(task) to allow
+ * dereferencing task_cs(task).
+ */
+void cpuset_print_task_mems_allowed(struct task_struct *tsk)
+{
+	struct dentry *dentry;
+
+	dentry = task_cs(tsk)->css.cgroup->dentry;
+	spin_lock(&cpuset_buffer_lock);
+	snprintf(cpuset_name, CPUSET_NAME_LEN,
+		 dentry ? (const char *)dentry->d_name.name : "/");
+	nodelist_scnprintf(cpuset_nodelist, CPUSET_NODELIST_LEN,
+			   tsk->mems_allowed);
+	printk(KERN_INFO "%s cpuset=%s mems_allowed=%s\n",
+	       tsk->comm, cpuset_name, cpuset_nodelist);
+	spin_unlock(&cpuset_buffer_lock);
+}
+
 /*
  * Collection of memory_pressure is suppressed unless
  * this flag is enabled by writing "1" to the special
diff --git a/kernel/dma-coherent.c b/kernel/dma-coherent.c
index f013a0c2e111..038707404b76 100644
--- a/kernel/dma-coherent.c
+++ b/kernel/dma-coherent.c
@@ -109,20 +109,40 @@ EXPORT_SYMBOL(dma_mark_declared_memory_occupied);
 int dma_alloc_from_coherent(struct device *dev, ssize_t size,
 				       dma_addr_t *dma_handle, void **ret)
 {
-	struct dma_coherent_mem *mem = dev ? dev->dma_mem : NULL;
+	struct dma_coherent_mem *mem;
 	int order = get_order(size);
+	int pageno;
 
-	if (mem) {
-		int page = bitmap_find_free_region(mem->bitmap, mem->size,
-						     order);
-		if (page >= 0) {
-			*dma_handle = mem->device_base + (page << PAGE_SHIFT);
-			*ret = mem->virt_base + (page << PAGE_SHIFT);
-			memset(*ret, 0, size);
-		} else if (mem->flags & DMA_MEMORY_EXCLUSIVE)
-			*ret = NULL;
+	if (!dev)
+		return 0;
+	mem = dev->dma_mem;
+	if (!mem)
+		return 0;
+	if (unlikely(size > mem->size))
+ 		return 0;
+
+	pageno = bitmap_find_free_region(mem->bitmap, mem->size, order);
+	if (pageno >= 0) {
+		/*
+		 * Memory was found in the per-device arena.
+		 */
+		*dma_handle = mem->device_base + (pageno << PAGE_SHIFT);
+		*ret = mem->virt_base + (pageno << PAGE_SHIFT);
+		memset(*ret, 0, size);
+	} else if (mem->flags & DMA_MEMORY_EXCLUSIVE) {
+		/*
+		 * The per-device arena is exhausted and we are not
+		 * permitted to fall back to generic memory.
+		 */
+		*ret = NULL;
+	} else {
+		/*
+		 * The per-device arena is exhausted and we are
+		 * permitted to fall back to generic memory.
+		 */
+		 return 0;
 	}
-	return (mem != NULL);
+	return 1;
 }
 EXPORT_SYMBOL(dma_alloc_from_coherent);
 
diff --git a/kernel/exit.c b/kernel/exit.c
index c9e5a1c14e08..c7740fa3252c 100644
--- a/kernel/exit.c
+++ b/kernel/exit.c
@@ -642,35 +642,31 @@ retry:
 	/*
 	 * We found no owner yet mm_users > 1: this implies that we are
 	 * most likely racing with swapoff (try_to_unuse()) or /proc or
-	 * ptrace or page migration (get_task_mm()).  Mark owner as NULL,
-	 * so that subsystems can understand the callback and take action.
+	 * ptrace or page migration (get_task_mm()).  Mark owner as NULL.
 	 */
-	down_write(&mm->mmap_sem);
-	cgroup_mm_owner_callbacks(mm->owner, NULL);
 	mm->owner = NULL;
-	up_write(&mm->mmap_sem);
 	return;
 
 assign_new_owner:
 	BUG_ON(c == p);
 	get_task_struct(c);
-	read_unlock(&tasklist_lock);
-	down_write(&mm->mmap_sem);
 	/*
 	 * The task_lock protects c->mm from changing.
 	 * We always want mm->owner->mm == mm
 	 */
 	task_lock(c);
+	/*
+	 * Delay read_unlock() till we have the task_lock()
+	 * to ensure that c does not slip away underneath us
+	 */
+	read_unlock(&tasklist_lock);
 	if (c->mm != mm) {
 		task_unlock(c);
-		up_write(&mm->mmap_sem);
 		put_task_struct(c);
 		goto retry;
 	}
-	cgroup_mm_owner_callbacks(mm->owner, c);
 	mm->owner = c;
 	task_unlock(c);
-	up_write(&mm->mmap_sem);
 	put_task_struct(c);
 }
 #endif /* CONFIG_MM_OWNER */
@@ -1055,10 +1051,7 @@ NORET_TYPE void do_exit(long code)
 				preempt_count());
 
 	acct_update_integrals(tsk);
-	if (tsk->mm) {
-		update_hiwater_rss(tsk->mm);
-		update_hiwater_vm(tsk->mm);
-	}
+
 	group_dead = atomic_dec_and_test(&tsk->signal->live);
 	if (group_dead) {
 		hrtimer_cancel(&tsk->signal->real_timer);
diff --git a/kernel/fork.c b/kernel/fork.c
index 43cbf30669e6..7b8f2a78be3d 100644
--- a/kernel/fork.c
+++ b/kernel/fork.c
@@ -400,6 +400,18 @@ __cacheline_aligned_in_smp DEFINE_SPINLOCK(mmlist_lock);
 #define allocate_mm()	(kmem_cache_alloc(mm_cachep, GFP_KERNEL))
 #define free_mm(mm)	(kmem_cache_free(mm_cachep, (mm)))
 
+static unsigned long default_dump_filter = MMF_DUMP_FILTER_DEFAULT;
+
+static int __init coredump_filter_setup(char *s)
+{
+	default_dump_filter =
+		(simple_strtoul(s, NULL, 0) << MMF_DUMP_FILTER_SHIFT) &
+		MMF_DUMP_FILTER_MASK;
+	return 1;
+}
+
+__setup("coredump_filter=", coredump_filter_setup);
+
 #include <linux/init_task.h>
 
 static struct mm_struct * mm_init(struct mm_struct * mm, struct task_struct *p)
@@ -408,8 +420,7 @@ static struct mm_struct * mm_init(struct mm_struct * mm, struct task_struct *p)
 	atomic_set(&mm->mm_count, 1);
 	init_rwsem(&mm->mmap_sem);
 	INIT_LIST_HEAD(&mm->mmlist);
-	mm->flags = (current->mm) ? current->mm->flags
-				  : MMF_DUMP_FILTER_DEFAULT;
+	mm->flags = (current->mm) ? current->mm->flags : default_dump_filter;
 	mm->core_state = NULL;
 	mm->nr_ptes = 0;
 	set_mm_counter(mm, file_rss, 0);
@@ -758,7 +769,7 @@ static int copy_sighand(unsigned long clone_flags, struct task_struct *tsk)
 {
 	struct sighand_struct *sig;
 
-	if (clone_flags & (CLONE_SIGHAND | CLONE_THREAD)) {
+	if (clone_flags & CLONE_SIGHAND) {
 		atomic_inc(&current->sighand->count);
 		return 0;
 	}
diff --git a/kernel/futex.c b/kernel/futex.c
index 7c6cbabe52b3..002aa189eb09 100644
--- a/kernel/futex.c
+++ b/kernel/futex.c
@@ -170,8 +170,11 @@ static void get_futex_key_refs(union futex_key *key)
  */
 static void drop_futex_key_refs(union futex_key *key)
 {
-	if (!key->both.ptr)
+	if (!key->both.ptr) {
+		/* If we're here then we tried to put a key we failed to get */
+		WARN_ON_ONCE(1);
 		return;
+	}
 
 	switch (key->both.offset & (FUT_OFF_INODE|FUT_OFF_MMSHARED)) {
 	case FUT_OFF_INODE:
@@ -730,8 +733,8 @@ static int futex_wake(u32 __user *uaddr, int fshared, int nr_wake, u32 bitset)
 	}
 
 	spin_unlock(&hb->lock);
-out:
 	put_futex_key(fshared, &key);
+out:
 	return ret;
 }
 
@@ -755,7 +758,7 @@ retryfull:
 		goto out;
 	ret = get_futex_key(uaddr2, fshared, &key2);
 	if (unlikely(ret != 0))
-		goto out;
+		goto out_put_key1;
 
 	hb1 = hash_futex(&key1);
 	hb2 = hash_futex(&key2);
@@ -777,12 +780,12 @@ retry:
 		 * but we might get them from range checking
 		 */
 		ret = op_ret;
-		goto out;
+		goto out_put_keys;
 #endif
 
 		if (unlikely(op_ret != -EFAULT)) {
 			ret = op_ret;
-			goto out;
+			goto out_put_keys;
 		}
 
 		/*
@@ -796,7 +799,7 @@ retry:
 			ret = futex_handle_fault((unsigned long)uaddr2,
 						 attempt);
 			if (ret)
-				goto out;
+				goto out_put_keys;
 			goto retry;
 		}
 
@@ -834,10 +837,11 @@ retry:
 	spin_unlock(&hb1->lock);
 	if (hb1 != hb2)
 		spin_unlock(&hb2->lock);
-out:
+out_put_keys:
 	put_futex_key(fshared, &key2);
+out_put_key1:
 	put_futex_key(fshared, &key1);
-
+out:
 	return ret;
 }
 
@@ -854,13 +858,13 @@ static int futex_requeue(u32 __user *uaddr1, int fshared, u32 __user *uaddr2,
 	struct futex_q *this, *next;
 	int ret, drop_count = 0;
 
- retry:
+retry:
 	ret = get_futex_key(uaddr1, fshared, &key1);
 	if (unlikely(ret != 0))
 		goto out;
 	ret = get_futex_key(uaddr2, fshared, &key2);
 	if (unlikely(ret != 0))
-		goto out;
+		goto out_put_key1;
 
 	hb1 = hash_futex(&key1);
 	hb2 = hash_futex(&key2);
@@ -882,7 +886,7 @@ static int futex_requeue(u32 __user *uaddr1, int fshared, u32 __user *uaddr2,
 			if (!ret)
 				goto retry;
 
-			return ret;
+			goto out_put_keys;
 		}
 		if (curval != *cmpval) {
 			ret = -EAGAIN;
@@ -927,9 +931,11 @@ out_unlock:
 	while (--drop_count >= 0)
 		drop_futex_key_refs(&key1);
 
-out:
+out_put_keys:
 	put_futex_key(fshared, &key2);
+out_put_key1:
 	put_futex_key(fshared, &key1);
+out:
 	return ret;
 }
 
@@ -990,7 +996,7 @@ static int unqueue_me(struct futex_q *q)
 	int ret = 0;
 
 	/* In the common case we don't take the spinlock, which is nice. */
- retry:
+retry:
 	lock_ptr = q->lock_ptr;
 	barrier();
 	if (lock_ptr != NULL) {
@@ -1172,11 +1178,11 @@ static int futex_wait(u32 __user *uaddr, int fshared,
 
 	q.pi_state = NULL;
 	q.bitset = bitset;
- retry:
+retry:
 	q.key = FUTEX_KEY_INIT;
 	ret = get_futex_key(uaddr, fshared, &q.key);
 	if (unlikely(ret != 0))
-		goto out_release_sem;
+		goto out;
 
 	hb = queue_lock(&q);
 
@@ -1204,6 +1210,7 @@ static int futex_wait(u32 __user *uaddr, int fshared,
 
 	if (unlikely(ret)) {
 		queue_unlock(&q, hb);
+		put_futex_key(fshared, &q.key);
 
 		ret = get_user(uval, uaddr);
 
@@ -1213,7 +1220,7 @@ static int futex_wait(u32 __user *uaddr, int fshared,
 	}
 	ret = -EWOULDBLOCK;
 	if (uval != val)
-		goto out_unlock_release_sem;
+		goto out_unlock_put_key;
 
 	/* Only actually queue if *uaddr contained val.  */
 	queue_me(&q, hb);
@@ -1305,11 +1312,11 @@ static int futex_wait(u32 __user *uaddr, int fshared,
 		return -ERESTART_RESTARTBLOCK;
 	}
 
- out_unlock_release_sem:
+out_unlock_put_key:
 	queue_unlock(&q, hb);
-
- out_release_sem:
 	put_futex_key(fshared, &q.key);
+
+out:
 	return ret;
 }
 
@@ -1358,16 +1365,16 @@ static int futex_lock_pi(u32 __user *uaddr, int fshared,
 	}
 
 	q.pi_state = NULL;
- retry:
+retry:
 	q.key = FUTEX_KEY_INIT;
 	ret = get_futex_key(uaddr, fshared, &q.key);
 	if (unlikely(ret != 0))
-		goto out_release_sem;
+		goto out;
 
- retry_unlocked:
+retry_unlocked:
 	hb = queue_lock(&q);
 
- retry_locked:
+retry_locked:
 	ret = lock_taken = 0;
 
 	/*
@@ -1388,14 +1395,14 @@ static int futex_lock_pi(u32 __user *uaddr, int fshared,
 	 */
 	if (unlikely((curval & FUTEX_TID_MASK) == task_pid_vnr(current))) {
 		ret = -EDEADLK;
-		goto out_unlock_release_sem;
+		goto out_unlock_put_key;
 	}
 
 	/*
 	 * Surprise - we got the lock. Just return to userspace:
 	 */
 	if (unlikely(!curval))
-		goto out_unlock_release_sem;
+		goto out_unlock_put_key;
 
 	uval = curval;
 
@@ -1431,7 +1438,7 @@ static int futex_lock_pi(u32 __user *uaddr, int fshared,
 	 * We took the lock due to owner died take over.
 	 */
 	if (unlikely(lock_taken))
-		goto out_unlock_release_sem;
+		goto out_unlock_put_key;
 
 	/*
 	 * We dont have the lock. Look up the PI state (or create it if
@@ -1470,7 +1477,7 @@ static int futex_lock_pi(u32 __user *uaddr, int fshared,
 				goto retry_locked;
 			}
 		default:
-			goto out_unlock_release_sem;
+			goto out_unlock_put_key;
 		}
 	}
 
@@ -1561,16 +1568,17 @@ static int futex_lock_pi(u32 __user *uaddr, int fshared,
 		destroy_hrtimer_on_stack(&to->timer);
 	return ret != -EINTR ? ret : -ERESTARTNOINTR;
 
- out_unlock_release_sem:
+out_unlock_put_key:
 	queue_unlock(&q, hb);
 
- out_release_sem:
+out_put_key:
 	put_futex_key(fshared, &q.key);
+out:
 	if (to)
 		destroy_hrtimer_on_stack(&to->timer);
 	return ret;
 
- uaddr_faulted:
+uaddr_faulted:
 	/*
 	 * We have to r/w  *(int __user *)uaddr, and we have to modify it
 	 * atomically.  Therefore, if we continue to fault after get_user()
@@ -1583,7 +1591,7 @@ static int futex_lock_pi(u32 __user *uaddr, int fshared,
 	if (attempt++) {
 		ret = futex_handle_fault((unsigned long)uaddr, attempt);
 		if (ret)
-			goto out_release_sem;
+			goto out_put_key;
 		goto retry_unlocked;
 	}
 
@@ -1675,9 +1683,9 @@ retry_unlocked:
 
 out_unlock:
 	spin_unlock(&hb->lock);
-out:
 	put_futex_key(fshared, &key);
 
+out:
 	return ret;
 
 pi_faulted:
diff --git a/kernel/hrtimer.c b/kernel/hrtimer.c
index eb2bfefa6dcc..1455b7651b6b 100644
--- a/kernel/hrtimer.c
+++ b/kernel/hrtimer.c
@@ -634,7 +634,6 @@ static inline void hrtimer_init_timer_hres(struct hrtimer *timer)
 {
 }
 
-static void __run_hrtimer(struct hrtimer *timer);
 
 /*
  * When High resolution timers are active, try to reprogram. Note, that in case
@@ -646,13 +645,9 @@ static inline int hrtimer_enqueue_reprogram(struct hrtimer *timer,
 					    struct hrtimer_clock_base *base)
 {
 	if (base->cpu_base->hres_active && hrtimer_reprogram(timer, base)) {
-		/*
-		 * XXX: recursion check?
-		 * hrtimer_forward() should round up with timer granularity
-		 * so that we never get into inf recursion here,
-		 * it doesn't do that though
-		 */
-		__run_hrtimer(timer);
+		spin_unlock(&base->cpu_base->lock);
+		raise_softirq_irqoff(HRTIMER_SOFTIRQ);
+		spin_lock(&base->cpu_base->lock);
 		return 1;
 	}
 	return 0;
@@ -705,11 +700,6 @@ static inline int hrtimer_enqueue_reprogram(struct hrtimer *timer,
 }
 static inline void hrtimer_init_hres(struct hrtimer_cpu_base *base) { }
 static inline void hrtimer_init_timer_hres(struct hrtimer *timer) { }
-static inline int hrtimer_reprogram(struct hrtimer *timer,
-				    struct hrtimer_clock_base *base)
-{
-	return 0;
-}
 
 #endif /* CONFIG_HIGH_RES_TIMERS */
 
@@ -780,9 +770,11 @@ EXPORT_SYMBOL_GPL(hrtimer_forward);
  *
  * The timer is inserted in expiry order. Insertion into the
  * red black tree is O(log(n)). Must hold the base lock.
+ *
+ * Returns 1 when the new timer is the leftmost timer in the tree.
  */
-static void enqueue_hrtimer(struct hrtimer *timer,
-			    struct hrtimer_clock_base *base, int reprogram)
+static int enqueue_hrtimer(struct hrtimer *timer,
+			   struct hrtimer_clock_base *base)
 {
 	struct rb_node **link = &base->active.rb_node;
 	struct rb_node *parent = NULL;
@@ -814,20 +806,8 @@ static void enqueue_hrtimer(struct hrtimer *timer,
 	 * Insert the timer to the rbtree and check whether it
 	 * replaces the first pending timer
 	 */
-	if (leftmost) {
-		/*
-		 * Reprogram the clock event device. When the timer is already
-		 * expired hrtimer_enqueue_reprogram has either called the
-		 * callback or added it to the pending list and raised the
-		 * softirq.
-		 *
-		 * This is a NOP for !HIGHRES
-		 */
-		if (reprogram && hrtimer_enqueue_reprogram(timer, base))
-			return;
-
+	if (leftmost)
 		base->first = &timer->node;
-	}
 
 	rb_link_node(&timer->node, parent, link);
 	rb_insert_color(&timer->node, &base->active);
@@ -836,6 +816,8 @@ static void enqueue_hrtimer(struct hrtimer *timer,
 	 * state of a possibly running callback.
 	 */
 	timer->state |= HRTIMER_STATE_ENQUEUED;
+
+	return leftmost;
 }
 
 /*
@@ -912,7 +894,7 @@ hrtimer_start_range_ns(struct hrtimer *timer, ktime_t tim, unsigned long delta_n
 {
 	struct hrtimer_clock_base *base, *new_base;
 	unsigned long flags;
-	int ret;
+	int ret, leftmost;
 
 	base = lock_hrtimer_base(timer, &flags);
 
@@ -940,12 +922,16 @@ hrtimer_start_range_ns(struct hrtimer *timer, ktime_t tim, unsigned long delta_n
 
 	timer_stats_hrtimer_set_start_info(timer);
 
+	leftmost = enqueue_hrtimer(timer, new_base);
+
 	/*
 	 * Only allow reprogramming if the new base is on this CPU.
 	 * (it might still be on another CPU if the timer was pending)
+	 *
+	 * XXX send_remote_softirq() ?
 	 */
-	enqueue_hrtimer(timer, new_base,
-			new_base->cpu_base == &__get_cpu_var(hrtimer_bases));
+	if (leftmost && new_base->cpu_base == &__get_cpu_var(hrtimer_bases))
+		hrtimer_enqueue_reprogram(timer, new_base);
 
 	unlock_hrtimer_base(timer, &flags);
 
@@ -1157,13 +1143,13 @@ static void __run_hrtimer(struct hrtimer *timer)
 	spin_lock(&cpu_base->lock);
 
 	/*
-	 * Note: We clear the CALLBACK bit after enqueue_hrtimer to avoid
-	 * reprogramming of the event hardware. This happens at the end of this
-	 * function anyway.
+	 * Note: We clear the CALLBACK bit after enqueue_hrtimer and
+	 * we do not reprogramm the event hardware. Happens either in
+	 * hrtimer_start_range_ns() or in hrtimer_interrupt()
 	 */
 	if (restart != HRTIMER_NORESTART) {
 		BUG_ON(timer->state != HRTIMER_STATE_CALLBACK);
-		enqueue_hrtimer(timer, base, 0);
+		enqueue_hrtimer(timer, base);
 	}
 	timer->state &= ~HRTIMER_STATE_CALLBACK;
 }
@@ -1243,6 +1229,22 @@ void hrtimer_interrupt(struct clock_event_device *dev)
 	}
 }
 
+/*
+ * local version of hrtimer_peek_ahead_timers() called with interrupts
+ * disabled.
+ */
+static void __hrtimer_peek_ahead_timers(void)
+{
+	struct tick_device *td;
+
+	if (!hrtimer_hres_active())
+		return;
+
+	td = &__get_cpu_var(tick_cpu_device);
+	if (td && td->evtdev)
+		hrtimer_interrupt(td->evtdev);
+}
+
 /**
  * hrtimer_peek_ahead_timers -- run soft-expired timers now
  *
@@ -1254,20 +1256,23 @@ void hrtimer_interrupt(struct clock_event_device *dev)
  */
 void hrtimer_peek_ahead_timers(void)
 {
-	struct tick_device *td;
 	unsigned long flags;
 
-	if (!hrtimer_hres_active())
-		return;
-
 	local_irq_save(flags);
-	td = &__get_cpu_var(tick_cpu_device);
-	if (td && td->evtdev)
-		hrtimer_interrupt(td->evtdev);
+	__hrtimer_peek_ahead_timers();
 	local_irq_restore(flags);
 }
 
-#endif	/* CONFIG_HIGH_RES_TIMERS */
+static void run_hrtimer_softirq(struct softirq_action *h)
+{
+	hrtimer_peek_ahead_timers();
+}
+
+#else /* CONFIG_HIGH_RES_TIMERS */
+
+static inline void __hrtimer_peek_ahead_timers(void) { }
+
+#endif	/* !CONFIG_HIGH_RES_TIMERS */
 
 /*
  * Called from timer softirq every jiffy, expire hrtimers:
@@ -1513,39 +1518,36 @@ static void migrate_hrtimer_list(struct hrtimer_clock_base *old_base,
 		__remove_hrtimer(timer, old_base, HRTIMER_STATE_MIGRATE, 0);
 		timer->base = new_base;
 		/*
-		 * Enqueue the timers on the new cpu, but do not reprogram 
-		 * the timer as that would enable a deadlock between
-		 * hrtimer_enqueue_reprogramm() running the timer and us still
-		 * holding a nested base lock.
-		 *
-		 * Instead we tickle the hrtimer interrupt after the migration
-		 * is done, which will run all expired timers and re-programm
-		 * the timer device.
+		 * Enqueue the timers on the new cpu. This does not
+		 * reprogram the event device in case the timer
+		 * expires before the earliest on this CPU, but we run
+		 * hrtimer_interrupt after we migrated everything to
+		 * sort out already expired timers and reprogram the
+		 * event device.
 		 */
-		enqueue_hrtimer(timer, new_base, 0);
+		enqueue_hrtimer(timer, new_base);
 
 		/* Clear the migration state bit */
 		timer->state &= ~HRTIMER_STATE_MIGRATE;
 	}
 }
 
-static int migrate_hrtimers(int scpu)
+static void migrate_hrtimers(int scpu)
 {
 	struct hrtimer_cpu_base *old_base, *new_base;
-	int dcpu, i;
+	int i;
 
 	BUG_ON(cpu_online(scpu));
-	old_base = &per_cpu(hrtimer_bases, scpu);
-	new_base = &get_cpu_var(hrtimer_bases);
-
-	dcpu = smp_processor_id();
-
 	tick_cancel_sched_timer(scpu);
+
+	local_irq_disable();
+	old_base = &per_cpu(hrtimer_bases, scpu);
+	new_base = &__get_cpu_var(hrtimer_bases);
 	/*
 	 * The caller is globally serialized and nobody else
 	 * takes two locks at once, deadlock is not possible.
 	 */
-	spin_lock_irq(&new_base->lock);
+	spin_lock(&new_base->lock);
 	spin_lock_nested(&old_base->lock, SINGLE_DEPTH_NESTING);
 
 	for (i = 0; i < HRTIMER_MAX_CLOCK_BASES; i++) {
@@ -1554,15 +1556,11 @@ static int migrate_hrtimers(int scpu)
 	}
 
 	spin_unlock(&old_base->lock);
-	spin_unlock_irq(&new_base->lock);
-	put_cpu_var(hrtimer_bases);
+	spin_unlock(&new_base->lock);
 
-	return dcpu;
-}
-
-static void tickle_timers(void *arg)
-{
-	hrtimer_peek_ahead_timers();
+	/* Check, if we got expired work to do */
+	__hrtimer_peek_ahead_timers();
+	local_irq_enable();
 }
 
 #endif /* CONFIG_HOTPLUG_CPU */
@@ -1583,11 +1581,8 @@ static int __cpuinit hrtimer_cpu_notify(struct notifier_block *self,
 	case CPU_DEAD:
 	case CPU_DEAD_FROZEN:
 	{
-		int dcpu;
-
 		clockevents_notify(CLOCK_EVT_NOTIFY_CPU_DEAD, &scpu);
-		dcpu = migrate_hrtimers(scpu);
-		smp_call_function_single(dcpu, tickle_timers, NULL, 0);
+		migrate_hrtimers(scpu);
 		break;
 	}
 #endif
@@ -1608,6 +1603,9 @@ void __init hrtimers_init(void)
 	hrtimer_cpu_notify(&hrtimers_nb, (unsigned long)CPU_UP_PREPARE,
 			  (void *)(long)smp_processor_id());
 	register_cpu_notifier(&hrtimers_nb);
+#ifdef CONFIG_HIGH_RES_TIMERS
+	open_softirq(HRTIMER_SOFTIRQ, run_hrtimer_softirq);
+#endif
 }
 
 /**
diff --git a/kernel/irq/autoprobe.c b/kernel/irq/autoprobe.c
index cc0f7321b8ce..1de9700f416e 100644
--- a/kernel/irq/autoprobe.c
+++ b/kernel/irq/autoprobe.c
@@ -10,6 +10,7 @@
 #include <linux/module.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
+#include <linux/async.h>
 
 #include "internals.h"
 
@@ -34,6 +35,10 @@ unsigned long probe_irq_on(void)
 	unsigned int status;
 	int i;
 
+	/*
+	 * quiesce the kernel, or at least the asynchronous portion
+	 */
+	async_synchronize_full();
 	mutex_lock(&probing_active);
 	/*
 	 * something may have generated an irq long ago and we want to
diff --git a/kernel/kmod.c b/kernel/kmod.c
index b46dbb908669..a27a5f64443d 100644
--- a/kernel/kmod.c
+++ b/kernel/kmod.c
@@ -51,8 +51,8 @@ char modprobe_path[KMOD_PATH_LEN] = "/sbin/modprobe";
 
 /**
  * request_module - try to load a kernel module
- * @fmt:     printf style format string for the name of the module
- * @varargs: arguements as specified in the format string
+ * @fmt: printf style format string for the name of the module
+ * @...: arguments as specified in the format string
  *
  * Load a module using the user mode module loader. The function returns
  * zero on success or a negative errno code on failure. Note that a
diff --git a/kernel/kprobes.c b/kernel/kprobes.c
index 9f8a3f25259a..1b9cbdc0127a 100644
--- a/kernel/kprobes.c
+++ b/kernel/kprobes.c
@@ -69,7 +69,7 @@ static struct hlist_head kretprobe_inst_table[KPROBE_TABLE_SIZE];
 /* NOTE: change this value only with kprobe_mutex held */
 static bool kprobe_enabled;
 
-DEFINE_MUTEX(kprobe_mutex);		/* Protects kprobe_table */
+static DEFINE_MUTEX(kprobe_mutex);	/* Protects kprobe_table */
 static DEFINE_PER_CPU(struct kprobe *, kprobe_instance) = NULL;
 static struct {
 	spinlock_t lock ____cacheline_aligned_in_smp;
@@ -115,6 +115,7 @@ enum kprobe_slot_state {
 	SLOT_USED = 2,
 };
 
+static DEFINE_MUTEX(kprobe_insn_mutex);	/* Protects kprobe_insn_pages */
 static struct hlist_head kprobe_insn_pages;
 static int kprobe_garbage_slots;
 static int collect_garbage_slots(void);
@@ -144,10 +145,10 @@ loop_end:
 }
 
 /**
- * get_insn_slot() - Find a slot on an executable page for an instruction.
+ * __get_insn_slot() - Find a slot on an executable page for an instruction.
  * We allocate an executable page if there's no room on existing ones.
  */
-kprobe_opcode_t __kprobes *get_insn_slot(void)
+static kprobe_opcode_t __kprobes *__get_insn_slot(void)
 {
 	struct kprobe_insn_page *kip;
 	struct hlist_node *pos;
@@ -196,6 +197,15 @@ kprobe_opcode_t __kprobes *get_insn_slot(void)
 	return kip->insns;
 }
 
+kprobe_opcode_t __kprobes *get_insn_slot(void)
+{
+	kprobe_opcode_t *ret;
+	mutex_lock(&kprobe_insn_mutex);
+	ret = __get_insn_slot();
+	mutex_unlock(&kprobe_insn_mutex);
+	return ret;
+}
+
 /* Return 1 if all garbages are collected, otherwise 0. */
 static int __kprobes collect_one_slot(struct kprobe_insn_page *kip, int idx)
 {
@@ -226,9 +236,13 @@ static int __kprobes collect_garbage_slots(void)
 {
 	struct kprobe_insn_page *kip;
 	struct hlist_node *pos, *next;
+	int safety;
 
 	/* Ensure no-one is preepmted on the garbages */
-	if (check_safety() != 0)
+	mutex_unlock(&kprobe_insn_mutex);
+	safety = check_safety();
+	mutex_lock(&kprobe_insn_mutex);
+	if (safety != 0)
 		return -EAGAIN;
 
 	hlist_for_each_entry_safe(kip, pos, next, &kprobe_insn_pages, hlist) {
@@ -251,6 +265,7 @@ void __kprobes free_insn_slot(kprobe_opcode_t * slot, int dirty)
 	struct kprobe_insn_page *kip;
 	struct hlist_node *pos;
 
+	mutex_lock(&kprobe_insn_mutex);
 	hlist_for_each_entry(kip, pos, &kprobe_insn_pages, hlist) {
 		if (kip->insns <= slot &&
 		    slot < kip->insns + (INSNS_PER_PAGE * MAX_INSN_SIZE)) {
@@ -267,6 +282,8 @@ void __kprobes free_insn_slot(kprobe_opcode_t * slot, int dirty)
 
 	if (dirty && ++kprobe_garbage_slots > INSNS_PER_PAGE)
 		collect_garbage_slots();
+
+	mutex_unlock(&kprobe_insn_mutex);
 }
 #endif
 
@@ -310,7 +327,7 @@ static int __kprobes aggr_pre_handler(struct kprobe *p, struct pt_regs *regs)
 	struct kprobe *kp;
 
 	list_for_each_entry_rcu(kp, &p->list, list) {
-		if (kp->pre_handler) {
+		if (kp->pre_handler && !kprobe_gone(kp)) {
 			set_kprobe_instance(kp);
 			if (kp->pre_handler(kp, regs))
 				return 1;
@@ -326,7 +343,7 @@ static void __kprobes aggr_post_handler(struct kprobe *p, struct pt_regs *regs,
 	struct kprobe *kp;
 
 	list_for_each_entry_rcu(kp, &p->list, list) {
-		if (kp->post_handler) {
+		if (kp->post_handler && !kprobe_gone(kp)) {
 			set_kprobe_instance(kp);
 			kp->post_handler(kp, regs, flags);
 			reset_kprobe_instance();
@@ -393,7 +410,7 @@ void __kprobes recycle_rp_inst(struct kretprobe_instance *ri,
 		hlist_add_head(&ri->hlist, head);
 }
 
-void kretprobe_hash_lock(struct task_struct *tsk,
+void __kprobes kretprobe_hash_lock(struct task_struct *tsk,
 			 struct hlist_head **head, unsigned long *flags)
 {
 	unsigned long hash = hash_ptr(tsk, KPROBE_HASH_BITS);
@@ -404,13 +421,15 @@ void kretprobe_hash_lock(struct task_struct *tsk,
 	spin_lock_irqsave(hlist_lock, *flags);
 }
 
-static void kretprobe_table_lock(unsigned long hash, unsigned long *flags)
+static void __kprobes kretprobe_table_lock(unsigned long hash,
+	unsigned long *flags)
 {
 	spinlock_t *hlist_lock = kretprobe_table_lock_ptr(hash);
 	spin_lock_irqsave(hlist_lock, *flags);
 }
 
-void kretprobe_hash_unlock(struct task_struct *tsk, unsigned long *flags)
+void __kprobes kretprobe_hash_unlock(struct task_struct *tsk,
+	unsigned long *flags)
 {
 	unsigned long hash = hash_ptr(tsk, KPROBE_HASH_BITS);
 	spinlock_t *hlist_lock;
@@ -419,7 +438,7 @@ void kretprobe_hash_unlock(struct task_struct *tsk, unsigned long *flags)
 	spin_unlock_irqrestore(hlist_lock, *flags);
 }
 
-void kretprobe_table_unlock(unsigned long hash, unsigned long *flags)
+void __kprobes kretprobe_table_unlock(unsigned long hash, unsigned long *flags)
 {
 	spinlock_t *hlist_lock = kretprobe_table_lock_ptr(hash);
 	spin_unlock_irqrestore(hlist_lock, *flags);
@@ -526,9 +545,10 @@ static inline void add_aggr_kprobe(struct kprobe *ap, struct kprobe *p)
 	ap->addr = p->addr;
 	ap->pre_handler = aggr_pre_handler;
 	ap->fault_handler = aggr_fault_handler;
-	if (p->post_handler)
+	/* We don't care the kprobe which has gone. */
+	if (p->post_handler && !kprobe_gone(p))
 		ap->post_handler = aggr_post_handler;
-	if (p->break_handler)
+	if (p->break_handler && !kprobe_gone(p))
 		ap->break_handler = aggr_break_handler;
 
 	INIT_LIST_HEAD(&ap->list);
@@ -547,17 +567,41 @@ static int __kprobes register_aggr_kprobe(struct kprobe *old_p,
 	int ret = 0;
 	struct kprobe *ap;
 
+	if (kprobe_gone(old_p)) {
+		/*
+		 * Attempting to insert new probe at the same location that
+		 * had a probe in the module vaddr area which already
+		 * freed. So, the instruction slot has already been
+		 * released. We need a new slot for the new probe.
+		 */
+		ret = arch_prepare_kprobe(old_p);
+		if (ret)
+			return ret;
+	}
 	if (old_p->pre_handler == aggr_pre_handler) {
 		copy_kprobe(old_p, p);
 		ret = add_new_kprobe(old_p, p);
+		ap = old_p;
 	} else {
 		ap = kzalloc(sizeof(struct kprobe), GFP_KERNEL);
-		if (!ap)
+		if (!ap) {
+			if (kprobe_gone(old_p))
+				arch_remove_kprobe(old_p);
 			return -ENOMEM;
+		}
 		add_aggr_kprobe(ap, old_p);
 		copy_kprobe(ap, p);
 		ret = add_new_kprobe(ap, p);
 	}
+	if (kprobe_gone(old_p)) {
+		/*
+		 * If the old_p has gone, its breakpoint has been disarmed.
+		 * We have to arm it again after preparing real kprobes.
+		 */
+		ap->flags &= ~KPROBE_FLAG_GONE;
+		if (kprobe_enabled)
+			arch_arm_kprobe(ap);
+	}
 	return ret;
 }
 
@@ -600,8 +644,7 @@ static kprobe_opcode_t __kprobes *kprobe_addr(struct kprobe *p)
 	return (kprobe_opcode_t *)(((char *)addr) + p->offset);
 }
 
-static int __kprobes __register_kprobe(struct kprobe *p,
-	unsigned long called_from)
+int __kprobes register_kprobe(struct kprobe *p)
 {
 	int ret = 0;
 	struct kprobe *old_p;
@@ -620,28 +663,30 @@ static int __kprobes __register_kprobe(struct kprobe *p,
 		return -EINVAL;
 	}
 
-	p->mod_refcounted = 0;
-
+	p->flags = 0;
 	/*
 	 * Check if are we probing a module.
 	 */
 	probed_mod = __module_text_address((unsigned long) p->addr);
 	if (probed_mod) {
-		struct module *calling_mod;
-		calling_mod = __module_text_address(called_from);
 		/*
-		 * We must allow modules to probe themself and in this case
-		 * avoid incrementing the module refcount, so as to allow
-		 * unloading of self probing modules.
+		 * We must hold a refcount of the probed module while updating
+		 * its code to prohibit unexpected unloading.
 		 */
-		if (calling_mod && calling_mod != probed_mod) {
-			if (unlikely(!try_module_get(probed_mod))) {
-				preempt_enable();
-				return -EINVAL;
-			}
-			p->mod_refcounted = 1;
-		} else
-			probed_mod = NULL;
+		if (unlikely(!try_module_get(probed_mod))) {
+			preempt_enable();
+			return -EINVAL;
+		}
+		/*
+		 * If the module freed .init.text, we couldn't insert
+		 * kprobes in there.
+		 */
+		if (within_module_init((unsigned long)p->addr, probed_mod) &&
+		    probed_mod->state != MODULE_STATE_COMING) {
+			module_put(probed_mod);
+			preempt_enable();
+			return -EINVAL;
+		}
 	}
 	preempt_enable();
 
@@ -668,8 +713,9 @@ static int __kprobes __register_kprobe(struct kprobe *p,
 out:
 	mutex_unlock(&kprobe_mutex);
 
-	if (ret && probed_mod)
+	if (probed_mod)
 		module_put(probed_mod);
+
 	return ret;
 }
 
@@ -697,16 +743,16 @@ valid_p:
 	     list_is_singular(&old_p->list))) {
 		/*
 		 * Only probe on the hash list. Disarm only if kprobes are
-		 * enabled - otherwise, the breakpoint would already have
-		 * been removed. We save on flushing icache.
+		 * enabled and not gone - otherwise, the breakpoint would
+		 * already have been removed. We save on flushing icache.
 		 */
-		if (kprobe_enabled)
+		if (kprobe_enabled && !kprobe_gone(old_p))
 			arch_disarm_kprobe(p);
 		hlist_del_rcu(&old_p->hlist);
 	} else {
-		if (p->break_handler)
+		if (p->break_handler && !kprobe_gone(p))
 			old_p->break_handler = NULL;
-		if (p->post_handler) {
+		if (p->post_handler && !kprobe_gone(p)) {
 			list_for_each_entry_rcu(list_p, &old_p->list, list) {
 				if ((list_p != p) && (list_p->post_handler))
 					goto noclean;
@@ -721,39 +767,27 @@ noclean:
 
 static void __kprobes __unregister_kprobe_bottom(struct kprobe *p)
 {
-	struct module *mod;
 	struct kprobe *old_p;
 
-	if (p->mod_refcounted) {
-		/*
-		 * Since we've already incremented refcount,
-		 * we don't need to disable preemption.
-		 */
-		mod = module_text_address((unsigned long)p->addr);
-		if (mod)
-			module_put(mod);
-	}
-
-	if (list_empty(&p->list) || list_is_singular(&p->list)) {
-		if (!list_empty(&p->list)) {
-			/* "p" is the last child of an aggr_kprobe */
-			old_p = list_entry(p->list.next, struct kprobe, list);
-			list_del(&p->list);
-			kfree(old_p);
-		}
+	if (list_empty(&p->list))
 		arch_remove_kprobe(p);
+	else if (list_is_singular(&p->list)) {
+		/* "p" is the last child of an aggr_kprobe */
+		old_p = list_entry(p->list.next, struct kprobe, list);
+		list_del(&p->list);
+		arch_remove_kprobe(old_p);
+		kfree(old_p);
 	}
 }
 
-static int __register_kprobes(struct kprobe **kps, int num,
-	unsigned long called_from)
+int __kprobes register_kprobes(struct kprobe **kps, int num)
 {
 	int i, ret = 0;
 
 	if (num <= 0)
 		return -EINVAL;
 	for (i = 0; i < num; i++) {
-		ret = __register_kprobe(kps[i], called_from);
+		ret = register_kprobe(kps[i]);
 		if (ret < 0) {
 			if (i > 0)
 				unregister_kprobes(kps, i);
@@ -763,26 +797,11 @@ static int __register_kprobes(struct kprobe **kps, int num,
 	return ret;
 }
 
-/*
- * Registration and unregistration functions for kprobe.
- */
-int __kprobes register_kprobe(struct kprobe *p)
-{
-	return __register_kprobes(&p, 1,
-				  (unsigned long)__builtin_return_address(0));
-}
-
 void __kprobes unregister_kprobe(struct kprobe *p)
 {
 	unregister_kprobes(&p, 1);
 }
 
-int __kprobes register_kprobes(struct kprobe **kps, int num)
-{
-	return __register_kprobes(kps, num,
-				  (unsigned long)__builtin_return_address(0));
-}
-
 void __kprobes unregister_kprobes(struct kprobe **kps, int num)
 {
 	int i;
@@ -811,8 +830,7 @@ unsigned long __weak arch_deref_entry_point(void *entry)
 	return (unsigned long)entry;
 }
 
-static int __register_jprobes(struct jprobe **jps, int num,
-	unsigned long called_from)
+int __kprobes register_jprobes(struct jprobe **jps, int num)
 {
 	struct jprobe *jp;
 	int ret = 0, i;
@@ -830,7 +848,7 @@ static int __register_jprobes(struct jprobe **jps, int num,
 			/* Todo: Verify probepoint is a function entry point */
 			jp->kp.pre_handler = setjmp_pre_handler;
 			jp->kp.break_handler = longjmp_break_handler;
-			ret = __register_kprobe(&jp->kp, called_from);
+			ret = register_kprobe(&jp->kp);
 		}
 		if (ret < 0) {
 			if (i > 0)
@@ -843,8 +861,7 @@ static int __register_jprobes(struct jprobe **jps, int num,
 
 int __kprobes register_jprobe(struct jprobe *jp)
 {
-	return __register_jprobes(&jp, 1,
-		(unsigned long)__builtin_return_address(0));
+	return register_jprobes(&jp, 1);
 }
 
 void __kprobes unregister_jprobe(struct jprobe *jp)
@@ -852,12 +869,6 @@ void __kprobes unregister_jprobe(struct jprobe *jp)
 	unregister_jprobes(&jp, 1);
 }
 
-int __kprobes register_jprobes(struct jprobe **jps, int num)
-{
-	return __register_jprobes(jps, num,
-		(unsigned long)__builtin_return_address(0));
-}
-
 void __kprobes unregister_jprobes(struct jprobe **jps, int num)
 {
 	int i;
@@ -920,8 +931,7 @@ static int __kprobes pre_handler_kretprobe(struct kprobe *p,
 	return 0;
 }
 
-static int __kprobes __register_kretprobe(struct kretprobe *rp,
-					  unsigned long called_from)
+int __kprobes register_kretprobe(struct kretprobe *rp)
 {
 	int ret = 0;
 	struct kretprobe_instance *inst;
@@ -967,21 +977,20 @@ static int __kprobes __register_kretprobe(struct kretprobe *rp,
 
 	rp->nmissed = 0;
 	/* Establish function entry probe point */
-	ret = __register_kprobe(&rp->kp, called_from);
+	ret = register_kprobe(&rp->kp);
 	if (ret != 0)
 		free_rp_inst(rp);
 	return ret;
 }
 
-static int __register_kretprobes(struct kretprobe **rps, int num,
-	unsigned long called_from)
+int __kprobes register_kretprobes(struct kretprobe **rps, int num)
 {
 	int ret = 0, i;
 
 	if (num <= 0)
 		return -EINVAL;
 	for (i = 0; i < num; i++) {
-		ret = __register_kretprobe(rps[i], called_from);
+		ret = register_kretprobe(rps[i]);
 		if (ret < 0) {
 			if (i > 0)
 				unregister_kretprobes(rps, i);
@@ -991,23 +1000,11 @@ static int __register_kretprobes(struct kretprobe **rps, int num,
 	return ret;
 }
 
-int __kprobes register_kretprobe(struct kretprobe *rp)
-{
-	return __register_kretprobes(&rp, 1,
-			(unsigned long)__builtin_return_address(0));
-}
-
 void __kprobes unregister_kretprobe(struct kretprobe *rp)
 {
 	unregister_kretprobes(&rp, 1);
 }
 
-int __kprobes register_kretprobes(struct kretprobe **rps, int num)
-{
-	return __register_kretprobes(rps, num,
-			(unsigned long)__builtin_return_address(0));
-}
-
 void __kprobes unregister_kretprobes(struct kretprobe **rps, int num)
 {
 	int i;
@@ -1055,6 +1052,72 @@ static int __kprobes pre_handler_kretprobe(struct kprobe *p,
 
 #endif /* CONFIG_KRETPROBES */
 
+/* Set the kprobe gone and remove its instruction buffer. */
+static void __kprobes kill_kprobe(struct kprobe *p)
+{
+	struct kprobe *kp;
+	p->flags |= KPROBE_FLAG_GONE;
+	if (p->pre_handler == aggr_pre_handler) {
+		/*
+		 * If this is an aggr_kprobe, we have to list all the
+		 * chained probes and mark them GONE.
+		 */
+		list_for_each_entry_rcu(kp, &p->list, list)
+			kp->flags |= KPROBE_FLAG_GONE;
+		p->post_handler = NULL;
+		p->break_handler = NULL;
+	}
+	/*
+	 * Here, we can remove insn_slot safely, because no thread calls
+	 * the original probed function (which will be freed soon) any more.
+	 */
+	arch_remove_kprobe(p);
+}
+
+/* Module notifier call back, checking kprobes on the module */
+static int __kprobes kprobes_module_callback(struct notifier_block *nb,
+					     unsigned long val, void *data)
+{
+	struct module *mod = data;
+	struct hlist_head *head;
+	struct hlist_node *node;
+	struct kprobe *p;
+	unsigned int i;
+	int checkcore = (val == MODULE_STATE_GOING);
+
+	if (val != MODULE_STATE_GOING && val != MODULE_STATE_LIVE)
+		return NOTIFY_DONE;
+
+	/*
+	 * When MODULE_STATE_GOING was notified, both of module .text and
+	 * .init.text sections would be freed. When MODULE_STATE_LIVE was
+	 * notified, only .init.text section would be freed. We need to
+	 * disable kprobes which have been inserted in the sections.
+	 */
+	mutex_lock(&kprobe_mutex);
+	for (i = 0; i < KPROBE_TABLE_SIZE; i++) {
+		head = &kprobe_table[i];
+		hlist_for_each_entry_rcu(p, node, head, hlist)
+			if (within_module_init((unsigned long)p->addr, mod) ||
+			    (checkcore &&
+			     within_module_core((unsigned long)p->addr, mod))) {
+				/*
+				 * The vaddr this probe is installed will soon
+				 * be vfreed buy not synced to disk. Hence,
+				 * disarming the breakpoint isn't needed.
+				 */
+				kill_kprobe(p);
+			}
+	}
+	mutex_unlock(&kprobe_mutex);
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block kprobe_module_nb = {
+	.notifier_call = kprobes_module_callback,
+	.priority = 0
+};
+
 static int __init init_kprobes(void)
 {
 	int i, err = 0;
@@ -1111,6 +1174,9 @@ static int __init init_kprobes(void)
 	err = arch_init_kprobes();
 	if (!err)
 		err = register_die_notifier(&kprobe_exceptions_nb);
+	if (!err)
+		err = register_module_notifier(&kprobe_module_nb);
+
 	kprobes_initialized = (err == 0);
 
 	if (!err)
@@ -1131,10 +1197,12 @@ static void __kprobes report_probe(struct seq_file *pi, struct kprobe *p,
 	else
 		kprobe_type = "k";
 	if (sym)
-		seq_printf(pi, "%p  %s  %s+0x%x  %s\n", p->addr, kprobe_type,
-			sym, offset, (modname ? modname : " "));
+		seq_printf(pi, "%p  %s  %s+0x%x  %s %s\n", p->addr, kprobe_type,
+			sym, offset, (modname ? modname : " "),
+			(kprobe_gone(p) ? "[GONE]" : ""));
 	else
-		seq_printf(pi, "%p  %s  %p\n", p->addr, kprobe_type, p->addr);
+		seq_printf(pi, "%p  %s  %p %s\n", p->addr, kprobe_type, p->addr,
+			(kprobe_gone(p) ? "[GONE]" : ""));
 }
 
 static void __kprobes *kprobe_seq_start(struct seq_file *f, loff_t *pos)
@@ -1215,7 +1283,8 @@ static void __kprobes enable_all_kprobes(void)
 	for (i = 0; i < KPROBE_TABLE_SIZE; i++) {
 		head = &kprobe_table[i];
 		hlist_for_each_entry_rcu(p, node, head, hlist)
-			arch_arm_kprobe(p);
+			if (!kprobe_gone(p))
+				arch_arm_kprobe(p);
 	}
 
 	kprobe_enabled = true;
@@ -1244,7 +1313,7 @@ static void __kprobes disable_all_kprobes(void)
 	for (i = 0; i < KPROBE_TABLE_SIZE; i++) {
 		head = &kprobe_table[i];
 		hlist_for_each_entry_rcu(p, node, head, hlist) {
-			if (!arch_trampoline_kprobe(p))
+			if (!arch_trampoline_kprobe(p) && !kprobe_gone(p))
 				arch_disarm_kprobe(p);
 		}
 	}
diff --git a/kernel/ksysfs.c b/kernel/ksysfs.c
index 08dd8ed86c77..528dd78e7e7e 100644
--- a/kernel/ksysfs.c
+++ b/kernel/ksysfs.c
@@ -24,7 +24,7 @@ static struct kobj_attribute _name##_attr = __ATTR_RO(_name)
 static struct kobj_attribute _name##_attr = \
 	__ATTR(_name, 0644, _name##_show, _name##_store)
 
-#if defined(CONFIG_HOTPLUG) && defined(CONFIG_NET)
+#if defined(CONFIG_HOTPLUG)
 /* current uevent sequence number */
 static ssize_t uevent_seqnum_show(struct kobject *kobj,
 				  struct kobj_attribute *attr, char *buf)
@@ -137,7 +137,7 @@ struct kobject *kernel_kobj;
 EXPORT_SYMBOL_GPL(kernel_kobj);
 
 static struct attribute * kernel_attrs[] = {
-#if defined(CONFIG_HOTPLUG) && defined(CONFIG_NET)
+#if defined(CONFIG_HOTPLUG)
 	&uevent_seqnum_attr.attr,
 	&uevent_helper_attr.attr,
 #endif
diff --git a/kernel/module.c b/kernel/module.c
index f47cce910f25..c9332c90d5a0 100644
--- a/kernel/module.c
+++ b/kernel/module.c
@@ -43,7 +43,6 @@
 #include <linux/device.h>
 #include <linux/string.h>
 #include <linux/mutex.h>
-#include <linux/unwind.h>
 #include <linux/rculist.h>
 #include <asm/uaccess.h>
 #include <asm/cacheflush.h>
@@ -51,6 +50,7 @@
 #include <asm/sections.h>
 #include <linux/tracepoint.h>
 #include <linux/ftrace.h>
+#include <linux/async.h>
 
 #if 0
 #define DEBUGP printk
@@ -817,6 +817,7 @@ sys_delete_module(const char __user *name_user, unsigned int flags)
 		mod->exit();
 	blocking_notifier_call_chain(&module_notify_list,
 				     MODULE_STATE_GOING, mod);
+	async_synchronize_full();
 	mutex_lock(&module_mutex);
 	/* Store the name of the last unloaded module for diagnostic purposes */
 	strlcpy(last_unloaded_module, mod->name, sizeof(last_unloaded_module));
@@ -1449,8 +1450,6 @@ static void free_module(struct module *mod)
 	remove_sect_attrs(mod);
 	mod_kobject_remove(mod);
 
-	unwind_remove_table(mod->unwind_info, 0);
-
 	/* Arch-specific cleanup. */
 	module_arch_cleanup(mod);
 
@@ -1867,7 +1866,6 @@ static noinline struct module *load_module(void __user *umod,
 	unsigned int symindex = 0;
 	unsigned int strindex = 0;
 	unsigned int modindex, versindex, infoindex, pcpuindex;
-	unsigned int unwindex = 0;
 	unsigned int num_kp, num_mcount;
 	struct kernel_param *kp;
 	struct module *mod;
@@ -1957,9 +1955,6 @@ static noinline struct module *load_module(void __user *umod,
 	versindex = find_sec(hdr, sechdrs, secstrings, "__versions");
 	infoindex = find_sec(hdr, sechdrs, secstrings, ".modinfo");
 	pcpuindex = find_pcpusec(hdr, sechdrs, secstrings);
-#ifdef ARCH_UNWIND_SECTION_NAME
-	unwindex = find_sec(hdr, sechdrs, secstrings, ARCH_UNWIND_SECTION_NAME);
-#endif
 
 	/* Don't keep modinfo and version sections. */
 	sechdrs[infoindex].sh_flags &= ~(unsigned long)SHF_ALLOC;
@@ -1969,8 +1964,6 @@ static noinline struct module *load_module(void __user *umod,
 	sechdrs[symindex].sh_flags |= SHF_ALLOC;
 	sechdrs[strindex].sh_flags |= SHF_ALLOC;
 #endif
-	if (unwindex)
-		sechdrs[unwindex].sh_flags |= SHF_ALLOC;
 
 	/* Check module struct version now, before we try to use module. */
 	if (!check_modstruct_version(sechdrs, versindex, mod)) {
@@ -2267,11 +2260,6 @@ static noinline struct module *load_module(void __user *umod,
 	add_sect_attrs(mod, hdr->e_shnum, secstrings, sechdrs);
 	add_notes_attrs(mod, hdr->e_shnum, secstrings, sechdrs);
 
-	/* Size of section 0 is 0, so this works well if no unwind info. */
-	mod->unwind_info = unwind_add_table(mod,
-					    (void *)sechdrs[unwindex].sh_addr,
-					    sechdrs[unwindex].sh_size);
-
 	/* Get rid of temporary copy */
 	vfree(hdr);
 
@@ -2366,11 +2354,12 @@ sys_init_module(void __user *umod,
 	/* Now it's a first class citizen!  Wake up anyone waiting for it. */
 	mod->state = MODULE_STATE_LIVE;
 	wake_up(&module_wq);
+	blocking_notifier_call_chain(&module_notify_list,
+				     MODULE_STATE_LIVE, mod);
 
 	mutex_lock(&module_mutex);
 	/* Drop initial reference. */
 	module_put(mod);
-	unwind_remove_table(mod->unwind_info, 1);
 	module_free(mod, mod->module_init);
 	mod->module_init = NULL;
 	mod->init_size = 0;
@@ -2405,7 +2394,7 @@ static const char *get_ksymbol(struct module *mod,
 	unsigned long nextval;
 
 	/* At worse, next value is at end of module */
-	if (within(addr, mod->module_init, mod->init_size))
+	if (within_module_init(addr, mod))
 		nextval = (unsigned long)mod->module_init+mod->init_text_size;
 	else
 		nextval = (unsigned long)mod->module_core+mod->core_text_size;
@@ -2453,8 +2442,8 @@ const char *module_address_lookup(unsigned long addr,
 
 	preempt_disable();
 	list_for_each_entry_rcu(mod, &modules, list) {
-		if (within(addr, mod->module_init, mod->init_size)
-		    || within(addr, mod->module_core, mod->core_size)) {
+		if (within_module_init(addr, mod) ||
+		    within_module_core(addr, mod)) {
 			if (modname)
 				*modname = mod->name;
 			ret = get_ksymbol(mod, addr, size, offset);
@@ -2476,8 +2465,8 @@ int lookup_module_symbol_name(unsigned long addr, char *symname)
 
 	preempt_disable();
 	list_for_each_entry_rcu(mod, &modules, list) {
-		if (within(addr, mod->module_init, mod->init_size) ||
-		    within(addr, mod->module_core, mod->core_size)) {
+		if (within_module_init(addr, mod) ||
+		    within_module_core(addr, mod)) {
 			const char *sym;
 
 			sym = get_ksymbol(mod, addr, NULL, NULL);
@@ -2500,8 +2489,8 @@ int lookup_module_symbol_attrs(unsigned long addr, unsigned long *size,
 
 	preempt_disable();
 	list_for_each_entry_rcu(mod, &modules, list) {
-		if (within(addr, mod->module_init, mod->init_size) ||
-		    within(addr, mod->module_core, mod->core_size)) {
+		if (within_module_init(addr, mod) ||
+		    within_module_core(addr, mod)) {
 			const char *sym;
 
 			sym = get_ksymbol(mod, addr, size, offset);
@@ -2720,7 +2709,7 @@ int is_module_address(unsigned long addr)
 	preempt_disable();
 
 	list_for_each_entry_rcu(mod, &modules, list) {
-		if (within(addr, mod->module_core, mod->core_size)) {
+		if (within_module_core(addr, mod)) {
 			preempt_enable();
 			return 1;
 		}
diff --git a/kernel/panic.c b/kernel/panic.c
index 13f06349a786..2a2ff36ff44d 100644
--- a/kernel/panic.c
+++ b/kernel/panic.c
@@ -299,6 +299,8 @@ static int init_oops_id(void)
 {
 	if (!oops_id)
 		get_random_bytes(&oops_id, sizeof(oops_id));
+	else
+		oops_id++;
 
 	return 0;
 }
diff --git a/kernel/pid.c b/kernel/pid.c
index 064e76afa507..af9224cdd6c0 100644
--- a/kernel/pid.c
+++ b/kernel/pid.c
@@ -475,7 +475,7 @@ pid_t task_session_nr_ns(struct task_struct *tsk, struct pid_namespace *ns)
 EXPORT_SYMBOL(task_session_nr_ns);
 
 /*
- * Used by proc to find the first pid that is greater then or equal to nr.
+ * Used by proc to find the first pid that is greater than or equal to nr.
  *
  * If there is a pid at nr this function is exactly the same as find_pid_ns.
  */
diff --git a/kernel/power/main.c b/kernel/power/main.c
index 613f16941b85..239988873971 100644
--- a/kernel/power/main.c
+++ b/kernel/power/main.c
@@ -615,7 +615,7 @@ static void __init test_wakealarm(struct rtc_device *rtc, suspend_state_t state)
 	/* this may fail if the RTC hasn't been initialized */
 	status = rtc_read_time(rtc, &alm.time);
 	if (status < 0) {
-		printk(err_readtime, rtc->dev.bus_id, status);
+		printk(err_readtime, dev_name(&rtc->dev), status);
 		return;
 	}
 	rtc_tm_to_time(&alm.time, &now);
@@ -626,7 +626,7 @@ static void __init test_wakealarm(struct rtc_device *rtc, suspend_state_t state)
 
 	status = rtc_set_alarm(rtc, &alm);
 	if (status < 0) {
-		printk(err_wakealarm, rtc->dev.bus_id, status);
+		printk(err_wakealarm, dev_name(&rtc->dev), status);
 		return;
 	}
 
@@ -660,7 +660,7 @@ static int __init has_wakealarm(struct device *dev, void *name_ptr)
 	if (!device_may_wakeup(candidate->dev.parent))
 		return 0;
 
-	*(char **)name_ptr = dev->bus_id;
+	*(const char **)name_ptr = dev_name(dev);
 	return 1;
 }
 
diff --git a/kernel/printk.c b/kernel/printk.c
index e651ab05655f..7015733793e8 100644
--- a/kernel/printk.c
+++ b/kernel/printk.c
@@ -619,7 +619,7 @@ static int acquire_console_semaphore_for_printk(unsigned int cpu)
 static const char recursion_bug_msg [] =
 		KERN_CRIT "BUG: recent printk recursion!\n";
 static int recursion_bug;
-	static int new_text_line = 1;
+static int new_text_line = 1;
 static char printk_buf[1024];
 
 asmlinkage int vprintk(const char *fmt, va_list args)
diff --git a/kernel/profile.c b/kernel/profile.c
index d18e2d2654f2..784933acf5b8 100644
--- a/kernel/profile.c
+++ b/kernel/profile.c
@@ -445,7 +445,6 @@ void profile_tick(int type)
 #ifdef CONFIG_PROC_FS
 #include <linux/proc_fs.h>
 #include <asm/uaccess.h>
-#include <asm/ptrace.h>
 
 static int prof_cpu_mask_read_proc(char *page, char **start, off_t off,
 			int count, int *eof, void *data)
diff --git a/kernel/rcupdate.c b/kernel/rcupdate.c
index ad63af8b2521..d92a76a881aa 100644
--- a/kernel/rcupdate.c
+++ b/kernel/rcupdate.c
@@ -77,8 +77,15 @@ void wakeme_after_rcu(struct rcu_head  *head)
  * sections are delimited by rcu_read_lock() and rcu_read_unlock(),
  * and may be nested.
  */
-void synchronize_rcu(void);	/* Makes kernel-doc tools happy */
-synchronize_rcu_xxx(synchronize_rcu, call_rcu)
+void synchronize_rcu(void)
+{
+	struct rcu_synchronize rcu;
+	init_completion(&rcu.completion);
+	/* Will wake me after RCU finished. */
+	call_rcu(&rcu.head, wakeme_after_rcu);
+	/* Wait for it. */
+	wait_for_completion(&rcu.completion);
+}
 EXPORT_SYMBOL_GPL(synchronize_rcu);
 
 static void rcu_barrier_callback(struct rcu_head *notused)
diff --git a/kernel/rcupreempt.c b/kernel/rcupreempt.c
index f9dc8f3720f6..33cfc50781f9 100644
--- a/kernel/rcupreempt.c
+++ b/kernel/rcupreempt.c
@@ -1177,7 +1177,16 @@ EXPORT_SYMBOL_GPL(call_rcu_sched);
  * in -rt this does -not- necessarily result in all currently executing
  * interrupt -handlers- having completed.
  */
-synchronize_rcu_xxx(__synchronize_sched, call_rcu_sched)
+void __synchronize_sched(void)
+{
+	struct rcu_synchronize rcu;
+
+	init_completion(&rcu.completion);
+	/* Will wake me after RCU finished. */
+	call_rcu_sched(&rcu.head, wakeme_after_rcu);
+	/* Wait for it. */
+	wait_for_completion(&rcu.completion);
+}
 EXPORT_SYMBOL_GPL(__synchronize_sched);
 
 /*
diff --git a/kernel/rcutorture.c b/kernel/rcutorture.c
index 3245b40952c6..1cff28db56b6 100644
--- a/kernel/rcutorture.c
+++ b/kernel/rcutorture.c
@@ -136,7 +136,7 @@ static int stutter_pause_test = 0;
 #endif
 int rcutorture_runnable = RCUTORTURE_RUNNABLE_INIT;
 
-#define FULLSTOP_SIGNALED 1	/* Bail due to signal. */
+#define FULLSTOP_SHUTDOWN 1	/* Bail due to system shutdown/panic. */
 #define FULLSTOP_CLEANUP  2	/* Orderly shutdown. */
 static int fullstop;		/* stop generating callbacks at test end. */
 DEFINE_MUTEX(fullstop_mutex);	/* protect fullstop transitions and */
@@ -151,12 +151,10 @@ rcutorture_shutdown_notify(struct notifier_block *unused1,
 {
 	if (fullstop)
 		return NOTIFY_DONE;
-	if (signal_pending(current)) {
-		mutex_lock(&fullstop_mutex);
-		if (!ACCESS_ONCE(fullstop))
-			fullstop = FULLSTOP_SIGNALED;
-		mutex_unlock(&fullstop_mutex);
-	}
+	mutex_lock(&fullstop_mutex);
+	if (!fullstop)
+		fullstop = FULLSTOP_SHUTDOWN;
+	mutex_unlock(&fullstop_mutex);
 	return NOTIFY_DONE;
 }
 
@@ -624,7 +622,7 @@ rcu_torture_writer(void *arg)
 		rcu_stutter_wait();
 	} while (!kthread_should_stop() && !fullstop);
 	VERBOSE_PRINTK_STRING("rcu_torture_writer task stopping");
-	while (!kthread_should_stop() && fullstop != FULLSTOP_SIGNALED)
+	while (!kthread_should_stop() && fullstop != FULLSTOP_SHUTDOWN)
 		schedule_timeout_uninterruptible(1);
 	return 0;
 }
@@ -649,7 +647,7 @@ rcu_torture_fakewriter(void *arg)
 	} while (!kthread_should_stop() && !fullstop);
 
 	VERBOSE_PRINTK_STRING("rcu_torture_fakewriter task stopping");
-	while (!kthread_should_stop() && fullstop != FULLSTOP_SIGNALED)
+	while (!kthread_should_stop() && fullstop != FULLSTOP_SHUTDOWN)
 		schedule_timeout_uninterruptible(1);
 	return 0;
 }
@@ -759,7 +757,7 @@ rcu_torture_reader(void *arg)
 	VERBOSE_PRINTK_STRING("rcu_torture_reader task stopping");
 	if (irqreader && cur_ops->irqcapable)
 		del_timer_sync(&t);
-	while (!kthread_should_stop() && fullstop != FULLSTOP_SIGNALED)
+	while (!kthread_should_stop() && fullstop != FULLSTOP_SHUTDOWN)
 		schedule_timeout_uninterruptible(1);
 	return 0;
 }
diff --git a/kernel/rcutree.c b/kernel/rcutree.c
index a342b032112c..f2d8638e6c60 100644
--- a/kernel/rcutree.c
+++ b/kernel/rcutree.c
@@ -79,7 +79,10 @@ struct rcu_state rcu_bh_state = RCU_STATE_INITIALIZER(rcu_bh_state);
 DEFINE_PER_CPU(struct rcu_data, rcu_bh_data);
 
 #ifdef CONFIG_NO_HZ
-DEFINE_PER_CPU(struct rcu_dynticks, rcu_dynticks);
+DEFINE_PER_CPU(struct rcu_dynticks, rcu_dynticks) = {
+	.dynticks_nesting = 1,
+	.dynticks = 1,
+};
 #endif /* #ifdef CONFIG_NO_HZ */
 
 static int blimit = 10;		/* Maximum callbacks per softirq. */
@@ -572,6 +575,7 @@ rcu_start_gp(struct rcu_state *rsp, unsigned long flags)
 	/* Special-case the common single-level case. */
 	if (NUM_RCU_NODES == 1) {
 		rnp->qsmask = rnp->qsmaskinit;
+		rsp->signaled = RCU_SIGNAL_INIT; /* force_quiescent_state OK. */
 		spin_unlock_irqrestore(&rnp->lock, flags);
 		return;
 	}
@@ -1379,13 +1383,6 @@ rcu_init_percpu_data(int cpu, struct rcu_state *rsp)
 
 static void __cpuinit rcu_online_cpu(int cpu)
 {
-#ifdef CONFIG_NO_HZ
-	struct rcu_dynticks *rdtp = &per_cpu(rcu_dynticks, cpu);
-
-	rdtp->dynticks_nesting = 1;
-	rdtp->dynticks |= 1; 	/* need consecutive #s even for hotplug. */
-	rdtp->dynticks_nmi = (rdtp->dynticks_nmi + 1) & ~0x1;
-#endif /* #ifdef CONFIG_NO_HZ */
 	rcu_init_percpu_data(cpu, &rcu_state);
 	rcu_init_percpu_data(cpu, &rcu_bh_state);
 	open_softirq(RCU_SOFTIRQ, rcu_process_callbacks);
diff --git a/kernel/resource.c b/kernel/resource.c
index e633106b12f6..ca6a1536b205 100644
--- a/kernel/resource.c
+++ b/kernel/resource.c
@@ -623,7 +623,7 @@ resource_size_t resource_alignment(struct resource *res)
  */
 struct resource * __request_region(struct resource *parent,
 				   resource_size_t start, resource_size_t n,
-				   const char *name)
+				   const char *name, int flags)
 {
 	struct resource *res = kzalloc(sizeof(*res), GFP_KERNEL);
 
@@ -634,6 +634,7 @@ struct resource * __request_region(struct resource *parent,
 	res->start = start;
 	res->end = start + n - 1;
 	res->flags = IORESOURCE_BUSY;
+	res->flags |= flags;
 
 	write_lock(&resource_lock);
 
@@ -679,7 +680,7 @@ int __check_region(struct resource *parent, resource_size_t start,
 {
 	struct resource * res;
 
-	res = __request_region(parent, start, n, "check-region");
+	res = __request_region(parent, start, n, "check-region", 0);
 	if (!res)
 		return -EBUSY;
 
@@ -776,7 +777,7 @@ struct resource * __devm_request_region(struct device *dev,
 	dr->start = start;
 	dr->n = n;
 
-	res = __request_region(parent, start, n, name);
+	res = __request_region(parent, start, n, name, 0);
 	if (res)
 		devres_add(dev, dr);
 	else
@@ -876,3 +877,57 @@ int iomem_map_sanity_check(resource_size_t addr, unsigned long size)
 
 	return err;
 }
+
+#ifdef CONFIG_STRICT_DEVMEM
+static int strict_iomem_checks = 1;
+#else
+static int strict_iomem_checks;
+#endif
+
+/*
+ * check if an address is reserved in the iomem resource tree
+ * returns 1 if reserved, 0 if not reserved.
+ */
+int iomem_is_exclusive(u64 addr)
+{
+	struct resource *p = &iomem_resource;
+	int err = 0;
+	loff_t l;
+	int size = PAGE_SIZE;
+
+	if (!strict_iomem_checks)
+		return 0;
+
+	addr = addr & PAGE_MASK;
+
+	read_lock(&resource_lock);
+	for (p = p->child; p ; p = r_next(NULL, p, &l)) {
+		/*
+		 * We can probably skip the resources without
+		 * IORESOURCE_IO attribute?
+		 */
+		if (p->start >= addr + size)
+			break;
+		if (p->end < addr)
+			continue;
+		if (p->flags & IORESOURCE_BUSY &&
+		     p->flags & IORESOURCE_EXCLUSIVE) {
+			err = 1;
+			break;
+		}
+	}
+	read_unlock(&resource_lock);
+
+	return err;
+}
+
+static int __init strict_iomem(char *str)
+{
+	if (strstr(str, "relaxed"))
+		strict_iomem_checks = 0;
+	if (strstr(str, "strict"))
+		strict_iomem_checks = 1;
+	return 1;
+}
+
+__setup("iomem=", strict_iomem);
diff --git a/kernel/sched.c b/kernel/sched.c
index 545c6fccd1dc..deb5ac8c12f3 100644
--- a/kernel/sched.c
+++ b/kernel/sched.c
@@ -3728,8 +3728,13 @@ redo:
 		}
 
 		double_unlock_balance(this_rq, busiest);
+		/*
+		 * Should not call ttwu while holding a rq->lock
+		 */
+		spin_unlock(&this_rq->lock);
 		if (active_balance)
 			wake_up_process(busiest->migration_thread);
+		spin_lock(&this_rq->lock);
 
 	} else
 		sd->nr_balance_failed = 0;
@@ -6957,7 +6962,7 @@ static void rq_attach_root(struct rq *rq, struct root_domain *rd)
 	spin_unlock_irqrestore(&rq->lock, flags);
 }
 
-static int init_rootdomain(struct root_domain *rd, bool bootmem)
+static int __init_refok init_rootdomain(struct root_domain *rd, bool bootmem)
 {
 	memset(rd, 0, sizeof(*rd));
 
@@ -6970,7 +6975,7 @@ static int init_rootdomain(struct root_domain *rd, bool bootmem)
 	}
 
 	if (!alloc_cpumask_var(&rd->span, GFP_KERNEL))
-		goto free_rd;
+		goto out;
 	if (!alloc_cpumask_var(&rd->online, GFP_KERNEL))
 		goto free_span;
 	if (!alloc_cpumask_var(&rd->rto_mask, GFP_KERNEL))
@@ -6986,8 +6991,7 @@ free_online:
 	free_cpumask_var(rd->online);
 free_span:
 	free_cpumask_var(rd->span);
-free_rd:
-	kfree(rd);
+out:
 	return -ENOMEM;
 }
 
@@ -7987,7 +7991,7 @@ match2:
 }
 
 #if defined(CONFIG_SCHED_MC) || defined(CONFIG_SCHED_SMT)
-int arch_reinit_sched_domains(void)
+static void arch_reinit_sched_domains(void)
 {
 	get_online_cpus();
 
@@ -7996,13 +8000,10 @@ int arch_reinit_sched_domains(void)
 
 	rebuild_sched_domains();
 	put_online_cpus();
-
-	return 0;
 }
 
 static ssize_t sched_power_savings_store(const char *buf, size_t count, int smt)
 {
-	int ret;
 	unsigned int level = 0;
 
 	if (sscanf(buf, "%u", &level) != 1)
@@ -8023,9 +8024,9 @@ static ssize_t sched_power_savings_store(const char *buf, size_t count, int smt)
 	else
 		sched_mc_power_savings = level;
 
-	ret = arch_reinit_sched_domains();
+	arch_reinit_sched_domains();
 
-	return ret ? ret : count;
+	return count;
 }
 
 #ifdef CONFIG_SCHED_MC
@@ -8060,7 +8061,7 @@ static SYSDEV_CLASS_ATTR(sched_smt_power_savings, 0644,
 		   sched_smt_power_savings_store);
 #endif
 
-int sched_create_sysfs_power_savings_entries(struct sysdev_class *cls)
+int __init sched_create_sysfs_power_savings_entries(struct sysdev_class *cls)
 {
 	int err = 0;
 
diff --git a/kernel/sched_clock.c b/kernel/sched_clock.c
index e8ab096ddfe3..a0b0852414cc 100644
--- a/kernel/sched_clock.c
+++ b/kernel/sched_clock.c
@@ -124,7 +124,7 @@ static u64 __update_sched_clock(struct sched_clock_data *scd, u64 now)
 
 	clock = scd->tick_gtod + delta;
 	min_clock = wrap_max(scd->tick_gtod, scd->clock);
-	max_clock = scd->tick_gtod + TICK_NSEC;
+	max_clock = wrap_max(scd->clock, scd->tick_gtod + TICK_NSEC);
 
 	clock = wrap_max(clock, min_clock);
 	clock = wrap_min(clock, max_clock);
@@ -227,6 +227,9 @@ EXPORT_SYMBOL_GPL(sched_clock_idle_sleep_event);
  */
 void sched_clock_idle_wakeup_event(u64 delta_ns)
 {
+	if (timekeeping_suspended)
+		return;
+
 	sched_clock_tick();
 	touch_softlockup_watchdog();
 }
diff --git a/kernel/sched_cpupri.c b/kernel/sched_cpupri.c
index 018b7be1db2e..1e00bfacf9b8 100644
--- a/kernel/sched_cpupri.c
+++ b/kernel/sched_cpupri.c
@@ -151,7 +151,7 @@ void cpupri_set(struct cpupri *cp, int cpu, int newpri)
  *
  * Returns: -ENOMEM if memory fails.
  */
-int cpupri_init(struct cpupri *cp, bool bootmem)
+int __init_refok cpupri_init(struct cpupri *cp, bool bootmem)
 {
 	int i;
 
diff --git a/kernel/sched_fair.c b/kernel/sched_fair.c
index 56c0efe902a7..e0c0b4bc3f08 100644
--- a/kernel/sched_fair.c
+++ b/kernel/sched_fair.c
@@ -386,20 +386,6 @@ int sched_nr_latency_handler(struct ctl_table *table, int write,
 #endif
 
 /*
- * delta *= P[w / rw]
- */
-static inline unsigned long
-calc_delta_weight(unsigned long delta, struct sched_entity *se)
-{
-	for_each_sched_entity(se) {
-		delta = calc_delta_mine(delta,
-				se->load.weight, &cfs_rq_of(se)->load);
-	}
-
-	return delta;
-}
-
-/*
  * delta /= w
  */
 static inline unsigned long
@@ -440,12 +426,20 @@ static u64 __sched_period(unsigned long nr_running)
  */
 static u64 sched_slice(struct cfs_rq *cfs_rq, struct sched_entity *se)
 {
-	unsigned long nr_running = cfs_rq->nr_running;
+	u64 slice = __sched_period(cfs_rq->nr_running + !se->on_rq);
 
-	if (unlikely(!se->on_rq))
-		nr_running++;
+	for_each_sched_entity(se) {
+		struct load_weight *load = &cfs_rq->load;
 
-	return calc_delta_weight(__sched_period(nr_running), se);
+		if (unlikely(!se->on_rq)) {
+			struct load_weight lw = cfs_rq->load;
+
+			update_load_add(&lw, se->load.weight);
+			load = &lw;
+		}
+		slice = calc_delta_mine(slice, se->load.weight, load);
+	}
+	return slice;
 }
 
 /*
diff --git a/kernel/signal.c b/kernel/signal.c
index 8e95855ff3cf..3152ac3b62e2 100644
--- a/kernel/signal.c
+++ b/kernel/signal.c
@@ -858,7 +858,8 @@ static int send_signal(int sig, struct siginfo *info, struct task_struct *t,
 			q->info.si_signo = sig;
 			q->info.si_errno = 0;
 			q->info.si_code = SI_USER;
-			q->info.si_pid = task_pid_vnr(current);
+			q->info.si_pid = task_tgid_nr_ns(current,
+							task_active_pid_ns(t));
 			q->info.si_uid = current_uid();
 			break;
 		case (unsigned long) SEND_SIG_PRIV:
diff --git a/kernel/sys.c b/kernel/sys.c
index d356d79e84ac..763c3c17ded3 100644
--- a/kernel/sys.c
+++ b/kernel/sys.c
@@ -33,6 +33,7 @@
 #include <linux/task_io_accounting_ops.h>
 #include <linux/seccomp.h>
 #include <linux/cpu.h>
+#include <linux/ptrace.h>
 
 #include <linux/compat.h>
 #include <linux/syscalls.h>
@@ -927,6 +928,7 @@ asmlinkage long sys_times(struct tms __user * tbuf)
 		if (copy_to_user(tbuf, &tmp, sizeof(struct tms)))
 			return -EFAULT;
 	}
+	force_successful_syscall_return();
 	return (long) jiffies_64_to_clock_t(get_jiffies_64());
 }
 
@@ -1627,6 +1629,8 @@ static void k_getrusage(struct task_struct *p, int who, struct rusage *r)
 	utime = stime = cputime_zero;
 
 	if (who == RUSAGE_THREAD) {
+		utime = task_utime(current);
+		stime = task_stime(current);
 		accumulate_thread_rusage(p, r);
 		goto out;
 	}
diff --git a/kernel/sysctl.c b/kernel/sysctl.c
index ff6d45c7626f..92f6e5bc3c24 100644
--- a/kernel/sysctl.c
+++ b/kernel/sysctl.c
@@ -87,10 +87,6 @@ extern int rcutorture_runnable;
 #endif /* #ifdef CONFIG_RCU_TORTURE_TEST */
 
 /* Constants used for minimum and  maximum */
-#if defined(CONFIG_HIGHMEM) || defined(CONFIG_DETECT_SOFTLOCKUP)
-static int one = 1;
-#endif
-
 #ifdef CONFIG_DETECT_SOFTLOCKUP
 static int sixty = 60;
 static int neg_one = -1;
@@ -101,6 +97,7 @@ static int two = 2;
 #endif
 
 static int zero;
+static int one = 1;
 static int one_hundred = 100;
 
 /* this is needed for the proc_dointvec_minmax for [fs_]overflow UID and GID */
@@ -952,12 +949,22 @@ static struct ctl_table vm_table[] = {
 		.data		= &dirty_background_ratio,
 		.maxlen		= sizeof(dirty_background_ratio),
 		.mode		= 0644,
-		.proc_handler	= &proc_dointvec_minmax,
+		.proc_handler	= &dirty_background_ratio_handler,
 		.strategy	= &sysctl_intvec,
 		.extra1		= &zero,
 		.extra2		= &one_hundred,
 	},
 	{
+		.ctl_name	= CTL_UNNUMBERED,
+		.procname	= "dirty_background_bytes",
+		.data		= &dirty_background_bytes,
+		.maxlen		= sizeof(dirty_background_bytes),
+		.mode		= 0644,
+		.proc_handler	= &dirty_background_bytes_handler,
+		.strategy	= &sysctl_intvec,
+		.extra1		= &one,
+	},
+	{
 		.ctl_name	= VM_DIRTY_RATIO,
 		.procname	= "dirty_ratio",
 		.data		= &vm_dirty_ratio,
@@ -969,6 +976,16 @@ static struct ctl_table vm_table[] = {
 		.extra2		= &one_hundred,
 	},
 	{
+		.ctl_name	= CTL_UNNUMBERED,
+		.procname	= "dirty_bytes",
+		.data		= &vm_dirty_bytes,
+		.maxlen		= sizeof(vm_dirty_bytes),
+		.mode		= 0644,
+		.proc_handler	= &dirty_bytes_handler,
+		.strategy	= &sysctl_intvec,
+		.extra1		= &one,
+	},
+	{
 		.procname	= "dirty_writeback_centisecs",
 		.data		= &dirty_writeback_interval,
 		.maxlen		= sizeof(dirty_writeback_interval),
diff --git a/kernel/test_kprobes.c b/kernel/test_kprobes.c
index 06b6395b45b2..4f104515a19b 100644
--- a/kernel/test_kprobes.c
+++ b/kernel/test_kprobes.c
@@ -22,21 +22,11 @@
 
 static u32 rand1, preh_val, posth_val, jph_val;
 static int errors, handler_errors, num_tests;
+static u32 (*target)(u32 value);
+static u32 (*target2)(u32 value);
 
 static noinline u32 kprobe_target(u32 value)
 {
-	/*
-	 * gcc ignores noinline on some architectures unless we stuff
-	 * sufficient lard into the function. The get_kprobe() here is
-	 * just for that.
-	 *
-	 * NOTE: We aren't concerned about the correctness of get_kprobe()
-	 * here; hence, this call is neither under !preempt nor with the
-	 * kprobe_mutex held. This is fine(tm)
-	 */
-	if (get_kprobe((void *)0xdeadbeef))
-		printk(KERN_INFO "Kprobe smoke test: probe on 0xdeadbeef!\n");
-
 	return (value / div_factor);
 }
 
@@ -74,7 +64,7 @@ static int test_kprobe(void)
 		return ret;
 	}
 
-	ret = kprobe_target(rand1);
+	ret = target(rand1);
 	unregister_kprobe(&kp);
 
 	if (preh_val == 0) {
@@ -92,6 +82,84 @@ static int test_kprobe(void)
 	return 0;
 }
 
+static noinline u32 kprobe_target2(u32 value)
+{
+	return (value / div_factor) + 1;
+}
+
+static int kp_pre_handler2(struct kprobe *p, struct pt_regs *regs)
+{
+	preh_val = (rand1 / div_factor) + 1;
+	return 0;
+}
+
+static void kp_post_handler2(struct kprobe *p, struct pt_regs *regs,
+		unsigned long flags)
+{
+	if (preh_val != (rand1 / div_factor) + 1) {
+		handler_errors++;
+		printk(KERN_ERR "Kprobe smoke test failed: "
+				"incorrect value in post_handler2\n");
+	}
+	posth_val = preh_val + div_factor;
+}
+
+static struct kprobe kp2 = {
+	.symbol_name = "kprobe_target2",
+	.pre_handler = kp_pre_handler2,
+	.post_handler = kp_post_handler2
+};
+
+static int test_kprobes(void)
+{
+	int ret;
+	struct kprobe *kps[2] = {&kp, &kp2};
+
+	kp.addr = 0; /* addr should be cleard for reusing kprobe. */
+	ret = register_kprobes(kps, 2);
+	if (ret < 0) {
+		printk(KERN_ERR "Kprobe smoke test failed: "
+				"register_kprobes returned %d\n", ret);
+		return ret;
+	}
+
+	preh_val = 0;
+	posth_val = 0;
+	ret = target(rand1);
+
+	if (preh_val == 0) {
+		printk(KERN_ERR "Kprobe smoke test failed: "
+				"kprobe pre_handler not called\n");
+		handler_errors++;
+	}
+
+	if (posth_val == 0) {
+		printk(KERN_ERR "Kprobe smoke test failed: "
+				"kprobe post_handler not called\n");
+		handler_errors++;
+	}
+
+	preh_val = 0;
+	posth_val = 0;
+	ret = target2(rand1);
+
+	if (preh_val == 0) {
+		printk(KERN_ERR "Kprobe smoke test failed: "
+				"kprobe pre_handler2 not called\n");
+		handler_errors++;
+	}
+
+	if (posth_val == 0) {
+		printk(KERN_ERR "Kprobe smoke test failed: "
+				"kprobe post_handler2 not called\n");
+		handler_errors++;
+	}
+
+	unregister_kprobes(kps, 2);
+	return 0;
+
+}
+
 static u32 j_kprobe_target(u32 value)
 {
 	if (value != rand1) {
@@ -121,7 +189,7 @@ static int test_jprobe(void)
 		return ret;
 	}
 
-	ret = kprobe_target(rand1);
+	ret = target(rand1);
 	unregister_jprobe(&jp);
 	if (jph_val == 0) {
 		printk(KERN_ERR "Kprobe smoke test failed: "
@@ -132,6 +200,43 @@ static int test_jprobe(void)
 	return 0;
 }
 
+static struct jprobe jp2 = {
+	.entry          = j_kprobe_target,
+	.kp.symbol_name = "kprobe_target2"
+};
+
+static int test_jprobes(void)
+{
+	int ret;
+	struct jprobe *jps[2] = {&jp, &jp2};
+
+	jp.kp.addr = 0; /* addr should be cleard for reusing kprobe. */
+	ret = register_jprobes(jps, 2);
+	if (ret < 0) {
+		printk(KERN_ERR "Kprobe smoke test failed: "
+				"register_jprobes returned %d\n", ret);
+		return ret;
+	}
+
+	jph_val = 0;
+	ret = target(rand1);
+	if (jph_val == 0) {
+		printk(KERN_ERR "Kprobe smoke test failed: "
+				"jprobe handler not called\n");
+		handler_errors++;
+	}
+
+	jph_val = 0;
+	ret = target2(rand1);
+	if (jph_val == 0) {
+		printk(KERN_ERR "Kprobe smoke test failed: "
+				"jprobe handler2 not called\n");
+		handler_errors++;
+	}
+	unregister_jprobes(jps, 2);
+
+	return 0;
+}
 #ifdef CONFIG_KRETPROBES
 static u32 krph_val;
 
@@ -177,7 +282,7 @@ static int test_kretprobe(void)
 		return ret;
 	}
 
-	ret = kprobe_target(rand1);
+	ret = target(rand1);
 	unregister_kretprobe(&rp);
 	if (krph_val != rand1) {
 		printk(KERN_ERR "Kprobe smoke test failed: "
@@ -187,12 +292,72 @@ static int test_kretprobe(void)
 
 	return 0;
 }
+
+static int return_handler2(struct kretprobe_instance *ri, struct pt_regs *regs)
+{
+	unsigned long ret = regs_return_value(regs);
+
+	if (ret != (rand1 / div_factor) + 1) {
+		handler_errors++;
+		printk(KERN_ERR "Kprobe smoke test failed: "
+				"incorrect value in kretprobe handler2\n");
+	}
+	if (krph_val == 0) {
+		handler_errors++;
+		printk(KERN_ERR "Kprobe smoke test failed: "
+				"call to kretprobe entry handler failed\n");
+	}
+
+	krph_val = rand1;
+	return 0;
+}
+
+static struct kretprobe rp2 = {
+	.handler	= return_handler2,
+	.entry_handler  = entry_handler,
+	.kp.symbol_name = "kprobe_target2"
+};
+
+static int test_kretprobes(void)
+{
+	int ret;
+	struct kretprobe *rps[2] = {&rp, &rp2};
+
+	rp.kp.addr = 0; /* addr should be cleard for reusing kprobe. */
+	ret = register_kretprobes(rps, 2);
+	if (ret < 0) {
+		printk(KERN_ERR "Kprobe smoke test failed: "
+				"register_kretprobe returned %d\n", ret);
+		return ret;
+	}
+
+	krph_val = 0;
+	ret = target(rand1);
+	if (krph_val != rand1) {
+		printk(KERN_ERR "Kprobe smoke test failed: "
+				"kretprobe handler not called\n");
+		handler_errors++;
+	}
+
+	krph_val = 0;
+	ret = target2(rand1);
+	if (krph_val != rand1) {
+		printk(KERN_ERR "Kprobe smoke test failed: "
+				"kretprobe handler2 not called\n");
+		handler_errors++;
+	}
+	unregister_kretprobes(rps, 2);
+	return 0;
+}
 #endif /* CONFIG_KRETPROBES */
 
 int init_test_probes(void)
 {
 	int ret;
 
+	target = kprobe_target;
+	target2 = kprobe_target2;
+
 	do {
 		rand1 = random32();
 	} while (rand1 <= div_factor);
@@ -204,15 +369,30 @@ int init_test_probes(void)
 		errors++;
 
 	num_tests++;
+	ret = test_kprobes();
+	if (ret < 0)
+		errors++;
+
+	num_tests++;
 	ret = test_jprobe();
 	if (ret < 0)
 		errors++;
 
+	num_tests++;
+	ret = test_jprobes();
+	if (ret < 0)
+		errors++;
+
 #ifdef CONFIG_KRETPROBES
 	num_tests++;
 	ret = test_kretprobe();
 	if (ret < 0)
 		errors++;
+
+	num_tests++;
+	ret = test_kretprobes();
+	if (ret < 0)
+		errors++;
 #endif /* CONFIG_KRETPROBES */
 
 	if (errors)
diff --git a/kernel/time.c b/kernel/time.c
index d63a4336fad6..4886e3ce83a4 100644
--- a/kernel/time.c
+++ b/kernel/time.c
@@ -37,6 +37,7 @@
 #include <linux/fs.h>
 #include <linux/slab.h>
 #include <linux/math64.h>
+#include <linux/ptrace.h>
 
 #include <asm/uaccess.h>
 #include <asm/unistd.h>
@@ -65,8 +66,9 @@ asmlinkage long sys_time(time_t __user * tloc)
 
 	if (tloc) {
 		if (put_user(i,tloc))
-			i = -EFAULT;
+			return -EFAULT;
 	}
+	force_successful_syscall_return();
 	return i;
 }
 
diff --git a/kernel/time/jiffies.c b/kernel/time/jiffies.c
index 1ca99557e929..06f197560f3b 100644
--- a/kernel/time/jiffies.c
+++ b/kernel/time/jiffies.c
@@ -45,7 +45,7 @@
  *
  * The value 8 is somewhat carefully chosen, as anything
  * larger can result in overflows. NSEC_PER_JIFFY grows as
- * HZ shrinks, so values greater then 8 overflow 32bits when
+ * HZ shrinks, so values greater than 8 overflow 32bits when
  * HZ=100.
  */
 #define JIFFIES_SHIFT	8
diff --git a/kernel/time/timekeeping.c b/kernel/time/timekeeping.c
index fa05e88aa76f..900f1b6598d1 100644
--- a/kernel/time/timekeeping.c
+++ b/kernel/time/timekeeping.c
@@ -46,6 +46,9 @@ struct timespec xtime __attribute__ ((aligned (16)));
 struct timespec wall_to_monotonic __attribute__ ((aligned (16)));
 static unsigned long total_sleep_time;		/* seconds */
 
+/* flag for if timekeeping is suspended */
+int __read_mostly timekeeping_suspended;
+
 static struct timespec xtime_cache __attribute__ ((aligned (16)));
 void update_xtime_cache(u64 nsec)
 {
@@ -92,6 +95,8 @@ void getnstimeofday(struct timespec *ts)
 	unsigned long seq;
 	s64 nsecs;
 
+	WARN_ON(timekeeping_suspended);
+
 	do {
 		seq = read_seqbegin(&xtime_lock);
 
@@ -299,8 +304,6 @@ void __init timekeeping_init(void)
 	write_sequnlock_irqrestore(&xtime_lock, flags);
 }
 
-/* flag for if timekeeping is suspended */
-static int timekeeping_suspended;
 /* time in seconds when suspend began */
 static unsigned long timekeeping_suspend_time;
 
diff --git a/kernel/tsacct.c b/kernel/tsacct.c
index 2dc06ab35716..43f891b05a4b 100644
--- a/kernel/tsacct.c
+++ b/kernel/tsacct.c
@@ -92,8 +92,8 @@ void xacct_add_tsk(struct taskstats *stats, struct task_struct *p)
 	mm = get_task_mm(p);
 	if (mm) {
 		/* adjust to KB unit */
-		stats->hiwater_rss   = mm->hiwater_rss * PAGE_SIZE / KB;
-		stats->hiwater_vm    = mm->hiwater_vm * PAGE_SIZE / KB;
+		stats->hiwater_rss   = get_mm_hiwater_rss(mm) * PAGE_SIZE / KB;
+		stats->hiwater_vm    = get_mm_hiwater_vm(mm)  * PAGE_SIZE / KB;
 		mmput(mm);
 	}
 	stats->read_char	= p->ioac.rchar;
diff --git a/lib/bust_spinlocks.c b/lib/bust_spinlocks.c
index 486da62b2b07..9681d54b95d1 100644
--- a/lib/bust_spinlocks.c
+++ b/lib/bust_spinlocks.c
@@ -12,6 +12,7 @@
 #include <linux/tty.h>
 #include <linux/wait.h>
 #include <linux/vt_kern.h>
+#include <linux/console.h>
 
 
 void __attribute__((weak)) bust_spinlocks(int yes)
@@ -22,6 +23,7 @@ void __attribute__((weak)) bust_spinlocks(int yes)
 #ifdef CONFIG_VT
 		unblank_screen();
 #endif
+		console_unblank();
 		if (--oops_in_progress == 0)
 			wake_up_klogd();
 	}
diff --git a/lib/dynamic_printk.c b/lib/dynamic_printk.c
index 8e30295e8566..165a19763dc9 100644
--- a/lib/dynamic_printk.c
+++ b/lib/dynamic_printk.c
@@ -277,40 +277,34 @@ static ssize_t pr_debug_write(struct file *file, const char __user *buf,
 				dynamic_enabled = DYNAMIC_ENABLED_NONE;
 			}
 			err = 0;
-		} else {
-			if (elem) {
-				if (value && (elem->enable == 0)) {
-					dynamic_printk_enabled |=
-							(1LL << elem->hash1);
-					dynamic_printk_enabled2 |=
-							(1LL << elem->hash2);
-					elem->enable = 1;
-					num_enabled++;
-					dynamic_enabled = DYNAMIC_ENABLED_SOME;
-					err = 0;
-					printk(KERN_DEBUG
-					       "debugging enabled for module %s\n",
-					       elem->name);
-				} else if (!value && (elem->enable == 1)) {
-					elem->enable = 0;
-					num_enabled--;
-					if (disabled_hash(elem->hash1, true))
-						dynamic_printk_enabled &=
+		} else if (elem) {
+			if (value && (elem->enable == 0)) {
+				dynamic_printk_enabled |= (1LL << elem->hash1);
+				dynamic_printk_enabled2 |= (1LL << elem->hash2);
+				elem->enable = 1;
+				num_enabled++;
+				dynamic_enabled = DYNAMIC_ENABLED_SOME;
+				err = 0;
+				printk(KERN_DEBUG
+					"debugging enabled for module %s\n",
+					elem->name);
+			} else if (!value && (elem->enable == 1)) {
+				elem->enable = 0;
+				num_enabled--;
+				if (disabled_hash(elem->hash1, true))
+					dynamic_printk_enabled &=
 							~(1LL << elem->hash1);
-					if (disabled_hash(elem->hash2, false))
-						dynamic_printk_enabled2 &=
+				if (disabled_hash(elem->hash2, false))
+					dynamic_printk_enabled2 &=
 							~(1LL << elem->hash2);
-					if (num_enabled)
-						dynamic_enabled =
-							DYNAMIC_ENABLED_SOME;
-					else
-						dynamic_enabled =
-							DYNAMIC_ENABLED_NONE;
-					err = 0;
-					printk(KERN_DEBUG
-					       "debugging disabled for module "
-					       "%s\n", elem->name);
-				}
+				if (num_enabled)
+					dynamic_enabled = DYNAMIC_ENABLED_SOME;
+				else
+					dynamic_enabled = DYNAMIC_ENABLED_NONE;
+				err = 0;
+				printk(KERN_DEBUG
+					"debugging disabled for module %s\n",
+					elem->name);
 			}
 		}
 	}
diff --git a/lib/fault-inject.c b/lib/fault-inject.c
index a50a311554cc..f97af55bdd96 100644
--- a/lib/fault-inject.c
+++ b/lib/fault-inject.c
@@ -6,7 +6,6 @@
 #include <linux/fs.h>
 #include <linux/module.h>
 #include <linux/interrupt.h>
-#include <linux/unwind.h>
 #include <linux/stacktrace.h>
 #include <linux/kallsyms.h>
 #include <linux/fault-inject.h>
diff --git a/lib/klist.c b/lib/klist.c
index bbdd3015c2c7..573d6068a42e 100644
--- a/lib/klist.c
+++ b/lib/klist.c
@@ -36,6 +36,7 @@
 
 #include <linux/klist.h>
 #include <linux/module.h>
+#include <linux/sched.h>
 
 /*
  * Use the lowest bit of n_klist to mark deleted nodes and exclude
@@ -108,7 +109,6 @@ static void add_tail(struct klist *k, struct klist_node *n)
 static void klist_node_init(struct klist *k, struct klist_node *n)
 {
 	INIT_LIST_HEAD(&n->n_node);
-	init_completion(&n->n_removed);
 	kref_init(&n->n_ref);
 	knode_set_klist(n, k);
 	if (k->get)
@@ -171,13 +171,34 @@ void klist_add_before(struct klist_node *n, struct klist_node *pos)
 }
 EXPORT_SYMBOL_GPL(klist_add_before);
 
+struct klist_waiter {
+	struct list_head list;
+	struct klist_node *node;
+	struct task_struct *process;
+	int woken;
+};
+
+static DEFINE_SPINLOCK(klist_remove_lock);
+static LIST_HEAD(klist_remove_waiters);
+
 static void klist_release(struct kref *kref)
 {
+	struct klist_waiter *waiter, *tmp;
 	struct klist_node *n = container_of(kref, struct klist_node, n_ref);
 
 	WARN_ON(!knode_dead(n));
 	list_del(&n->n_node);
-	complete(&n->n_removed);
+	spin_lock(&klist_remove_lock);
+	list_for_each_entry_safe(waiter, tmp, &klist_remove_waiters, list) {
+		if (waiter->node != n)
+			continue;
+
+		waiter->woken = 1;
+		mb();
+		wake_up_process(waiter->process);
+		list_del(&waiter->list);
+	}
+	spin_unlock(&klist_remove_lock);
 	knode_set_klist(n, NULL);
 }
 
@@ -217,8 +238,24 @@ EXPORT_SYMBOL_GPL(klist_del);
  */
 void klist_remove(struct klist_node *n)
 {
+	struct klist_waiter waiter;
+
+	waiter.node = n;
+	waiter.process = current;
+	waiter.woken = 0;
+	spin_lock(&klist_remove_lock);
+	list_add(&waiter.list, &klist_remove_waiters);
+	spin_unlock(&klist_remove_lock);
+
 	klist_del(n);
-	wait_for_completion(&n->n_removed);
+
+	for (;;) {
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		if (waiter.woken)
+			break;
+		schedule();
+	}
+	__set_current_state(TASK_RUNNING);
 }
 EXPORT_SYMBOL_GPL(klist_remove);
 
diff --git a/lib/kobject_uevent.c b/lib/kobject_uevent.c
index 3f914725bda8..318328ddbd1c 100644
--- a/lib/kobject_uevent.c
+++ b/lib/kobject_uevent.c
@@ -165,7 +165,7 @@ int kobject_uevent_env(struct kobject *kobj, enum kobject_action action,
 	/* keys passed in from the caller */
 	if (envp_ext) {
 		for (i = 0; envp_ext[i]; i++) {
-			retval = add_uevent_var(env, envp_ext[i]);
+			retval = add_uevent_var(env, "%s", envp_ext[i]);
 			if (retval)
 				goto exit;
 		}
@@ -225,8 +225,10 @@ int kobject_uevent_env(struct kobject *kobj, enum kobject_action action,
 			}
 
 			NETLINK_CB(skb).dst_group = 1;
-			netlink_broadcast(uevent_sock, skb, 0, 1, GFP_KERNEL);
-		}
+			retval = netlink_broadcast(uevent_sock, skb, 0, 1,
+						   GFP_KERNEL);
+		} else
+			retval = -ENOMEM;
 	}
 #endif
 
diff --git a/lib/percpu_counter.c b/lib/percpu_counter.c
index b255b939bc1b..aeaa6d734447 100644
--- a/lib/percpu_counter.c
+++ b/lib/percpu_counter.c
@@ -9,10 +9,8 @@
 #include <linux/cpu.h>
 #include <linux/module.h>
 
-#ifdef CONFIG_HOTPLUG_CPU
 static LIST_HEAD(percpu_counters);
 static DEFINE_MUTEX(percpu_counters_lock);
-#endif
 
 void percpu_counter_set(struct percpu_counter *fbc, s64 amount)
 {
@@ -68,11 +66,11 @@ s64 __percpu_counter_sum(struct percpu_counter *fbc)
 }
 EXPORT_SYMBOL(__percpu_counter_sum);
 
-static struct lock_class_key percpu_counter_irqsafe;
-
-int percpu_counter_init(struct percpu_counter *fbc, s64 amount)
+int __percpu_counter_init(struct percpu_counter *fbc, s64 amount,
+			  struct lock_class_key *key)
 {
 	spin_lock_init(&fbc->lock);
+	lockdep_set_class(&fbc->lock, key);
 	fbc->count = amount;
 	fbc->counters = alloc_percpu(s32);
 	if (!fbc->counters)
@@ -84,17 +82,7 @@ int percpu_counter_init(struct percpu_counter *fbc, s64 amount)
 #endif
 	return 0;
 }
-EXPORT_SYMBOL(percpu_counter_init);
-
-int percpu_counter_init_irq(struct percpu_counter *fbc, s64 amount)
-{
-	int err;
-
-	err = percpu_counter_init(fbc, amount);
-	if (!err)
-		lockdep_set_class(&fbc->lock, &percpu_counter_irqsafe);
-	return err;
-}
+EXPORT_SYMBOL(__percpu_counter_init);
 
 void percpu_counter_destroy(struct percpu_counter *fbc)
 {
@@ -111,13 +99,24 @@ void percpu_counter_destroy(struct percpu_counter *fbc)
 }
 EXPORT_SYMBOL(percpu_counter_destroy);
 
-#ifdef CONFIG_HOTPLUG_CPU
+int percpu_counter_batch __read_mostly = 32;
+EXPORT_SYMBOL(percpu_counter_batch);
+
+static void compute_batch_value(void)
+{
+	int nr = num_online_cpus();
+
+	percpu_counter_batch = max(32, nr*2);
+}
+
 static int __cpuinit percpu_counter_hotcpu_callback(struct notifier_block *nb,
 					unsigned long action, void *hcpu)
 {
+#ifdef CONFIG_HOTPLUG_CPU
 	unsigned int cpu;
 	struct percpu_counter *fbc;
 
+	compute_batch_value();
 	if (action != CPU_DEAD)
 		return NOTIFY_OK;
 
@@ -134,13 +133,14 @@ static int __cpuinit percpu_counter_hotcpu_callback(struct notifier_block *nb,
 		spin_unlock_irqrestore(&fbc->lock, flags);
 	}
 	mutex_unlock(&percpu_counters_lock);
+#endif
 	return NOTIFY_OK;
 }
 
 static int __init percpu_counter_startup(void)
 {
+	compute_batch_value();
 	hotcpu_notifier(percpu_counter_hotcpu_callback, 0);
 	return 0;
 }
 module_init(percpu_counter_startup);
-#endif
diff --git a/lib/prio_heap.c b/lib/prio_heap.c
index 471944a54e23..a7af6f85eca8 100644
--- a/lib/prio_heap.c
+++ b/lib/prio_heap.c
@@ -31,7 +31,7 @@ void *heap_insert(struct ptr_heap *heap, void *p)
 
 	if (heap->size < heap->max) {
 		/* Heap insertion */
-		int pos = heap->size++;
+		pos = heap->size++;
 		while (pos > 0 && heap->gt(p, ptrs[(pos-1)/2])) {
 			ptrs[pos] = ptrs[(pos-1)/2];
 			pos = (pos-1)/2;
diff --git a/lib/proportions.c b/lib/proportions.c
index 4f387a643d72..d50746a79de2 100644
--- a/lib/proportions.c
+++ b/lib/proportions.c
@@ -83,11 +83,11 @@ int prop_descriptor_init(struct prop_descriptor *pd, int shift)
 	pd->index = 0;
 	pd->pg[0].shift = shift;
 	mutex_init(&pd->mutex);
-	err = percpu_counter_init_irq(&pd->pg[0].events, 0);
+	err = percpu_counter_init(&pd->pg[0].events, 0);
 	if (err)
 		goto out;
 
-	err = percpu_counter_init_irq(&pd->pg[1].events, 0);
+	err = percpu_counter_init(&pd->pg[1].events, 0);
 	if (err)
 		percpu_counter_destroy(&pd->pg[0].events);
 
@@ -147,6 +147,7 @@ out:
  * this is used to track the active references.
  */
 static struct prop_global *prop_get_global(struct prop_descriptor *pd)
+__acquires(RCU)
 {
 	int index;
 
@@ -160,6 +161,7 @@ static struct prop_global *prop_get_global(struct prop_descriptor *pd)
 }
 
 static void prop_put_global(struct prop_descriptor *pd, struct prop_global *pg)
+__releases(RCU)
 {
 	rcu_read_unlock();
 }
@@ -191,7 +193,7 @@ int prop_local_init_percpu(struct prop_local_percpu *pl)
 	spin_lock_init(&pl->lock);
 	pl->shift = 0;
 	pl->period = 0;
-	return percpu_counter_init_irq(&pl->events, 0);
+	return percpu_counter_init(&pl->events, 0);
 }
 
 void prop_local_destroy_percpu(struct prop_local_percpu *pl)
diff --git a/lib/radix-tree.c b/lib/radix-tree.c
index be86b32bc874..4bb42a0344ec 100644
--- a/lib/radix-tree.c
+++ b/lib/radix-tree.c
@@ -81,7 +81,7 @@ struct radix_tree_preload {
 	int nr;
 	struct radix_tree_node *nodes[RADIX_TREE_MAX_PATH];
 };
-DEFINE_PER_CPU(struct radix_tree_preload, radix_tree_preloads) = { 0, };
+static DEFINE_PER_CPU(struct radix_tree_preload, radix_tree_preloads) = { 0, };
 
 static inline gfp_t root_gfp_mask(struct radix_tree_root *root)
 {
@@ -640,13 +640,14 @@ EXPORT_SYMBOL(radix_tree_tag_get);
  *
  *	Returns: the index of the hole if found, otherwise returns an index
  *	outside of the set specified (in which case 'return - index >= max_scan'
- *	will be true).
+ *	will be true). In rare cases of index wrap-around, 0 will be returned.
  *
  *	radix_tree_next_hole may be called under rcu_read_lock. However, like
- *	radix_tree_gang_lookup, this will not atomically search a snapshot of the
- *	tree at a single point in time. For example, if a hole is created at index
- *	5, then subsequently a hole is created at index 10, radix_tree_next_hole
- *	covering both indexes may return 10 if called under rcu_read_lock.
+ *	radix_tree_gang_lookup, this will not atomically search a snapshot of
+ *	the tree at a single point in time. For example, if a hole is created
+ *	at index 5, then subsequently a hole is created at index 10,
+ *	radix_tree_next_hole covering both indexes may return 10 if called
+ *	under rcu_read_lock.
  */
 unsigned long radix_tree_next_hole(struct radix_tree_root *root,
 				unsigned long index, unsigned long max_scan)
diff --git a/lib/swiotlb.c b/lib/swiotlb.c
index 7f5e21b9c16b..1f991acc2a05 100644
--- a/lib/swiotlb.c
+++ b/lib/swiotlb.c
@@ -14,6 +14,7 @@
  * 04/07/.. ak		Better overflow handling. Assorted fixes.
  * 05/09/10 linville	Add support for syncing ranges, support syncing for
  *			DMA_BIDIRECTIONAL mappings, miscellaneous cleanup.
+ * 08/12/11 beckyb	Add highmem support
  */
 
 #include <linux/cache.h>
@@ -21,8 +22,9 @@
 #include <linux/mm.h>
 #include <linux/module.h>
 #include <linux/spinlock.h>
-#include <linux/swiotlb.h>
 #include <linux/string.h>
+#include <linux/swiotlb.h>
+#include <linux/pfn.h>
 #include <linux/types.h>
 #include <linux/ctype.h>
 #include <linux/highmem.h>
@@ -88,10 +90,7 @@ static unsigned int io_tlb_index;
  * We need to save away the original address corresponding to a mapped entry
  * for the sync operations.
  */
-static struct swiotlb_phys_addr {
-	struct page *page;
-	unsigned int offset;
-} *io_tlb_orig_addr;
+static phys_addr_t *io_tlb_orig_addr;
 
 /*
  * Protect the above data structures in the map and unmap calls
@@ -125,7 +124,7 @@ void * __weak swiotlb_alloc(unsigned order, unsigned long nslabs)
 	return (void *)__get_free_pages(GFP_DMA | __GFP_NOWARN, order);
 }
 
-dma_addr_t __weak swiotlb_phys_to_bus(phys_addr_t paddr)
+dma_addr_t __weak swiotlb_phys_to_bus(struct device *hwdev, phys_addr_t paddr)
 {
 	return paddr;
 }
@@ -135,9 +134,10 @@ phys_addr_t __weak swiotlb_bus_to_phys(dma_addr_t baddr)
 	return baddr;
 }
 
-static dma_addr_t swiotlb_virt_to_bus(volatile void *address)
+static dma_addr_t swiotlb_virt_to_bus(struct device *hwdev,
+				      volatile void *address)
 {
-	return swiotlb_phys_to_bus(virt_to_phys(address));
+	return swiotlb_phys_to_bus(hwdev, virt_to_phys(address));
 }
 
 static void *swiotlb_bus_to_virt(dma_addr_t address)
@@ -150,35 +150,18 @@ int __weak swiotlb_arch_range_needs_mapping(void *ptr, size_t size)
 	return 0;
 }
 
-static dma_addr_t swiotlb_sg_to_bus(struct scatterlist *sg)
-{
-	return swiotlb_phys_to_bus(page_to_phys(sg_page(sg)) + sg->offset);
-}
-
 static void swiotlb_print_info(unsigned long bytes)
 {
 	phys_addr_t pstart, pend;
-	dma_addr_t bstart, bend;
 
 	pstart = virt_to_phys(io_tlb_start);
 	pend = virt_to_phys(io_tlb_end);
 
-	bstart = swiotlb_phys_to_bus(pstart);
-	bend = swiotlb_phys_to_bus(pend);
-
 	printk(KERN_INFO "Placing %luMB software IO TLB between %p - %p\n",
 	       bytes >> 20, io_tlb_start, io_tlb_end);
-	if (pstart != bstart || pend != bend)
-		printk(KERN_INFO "software IO TLB at phys %#llx - %#llx"
-		       " bus %#llx - %#llx\n",
-		       (unsigned long long)pstart,
-		       (unsigned long long)pend,
-		       (unsigned long long)bstart,
-		       (unsigned long long)bend);
-	else
-		printk(KERN_INFO "software IO TLB at phys %#llx - %#llx\n",
-		       (unsigned long long)pstart,
-		       (unsigned long long)pend);
+	printk(KERN_INFO "software IO TLB at phys %#llx - %#llx\n",
+	       (unsigned long long)pstart,
+	       (unsigned long long)pend);
 }
 
 /*
@@ -214,7 +197,7 @@ swiotlb_init_with_default_size(size_t default_size)
 	for (i = 0; i < io_tlb_nslabs; i++)
  		io_tlb_list[i] = IO_TLB_SEGSIZE - OFFSET(i, IO_TLB_SEGSIZE);
 	io_tlb_index = 0;
-	io_tlb_orig_addr = alloc_bootmem(io_tlb_nslabs * sizeof(struct swiotlb_phys_addr));
+	io_tlb_orig_addr = alloc_bootmem(io_tlb_nslabs * sizeof(phys_addr_t));
 
 	/*
 	 * Get the overflow emergency buffer
@@ -288,12 +271,14 @@ swiotlb_late_init_with_default_size(size_t default_size)
  		io_tlb_list[i] = IO_TLB_SEGSIZE - OFFSET(i, IO_TLB_SEGSIZE);
 	io_tlb_index = 0;
 
-	io_tlb_orig_addr = (struct swiotlb_phys_addr *)__get_free_pages(GFP_KERNEL,
-	                           get_order(io_tlb_nslabs * sizeof(struct swiotlb_phys_addr)));
+	io_tlb_orig_addr = (phys_addr_t *)
+		__get_free_pages(GFP_KERNEL,
+				 get_order(io_tlb_nslabs *
+					   sizeof(phys_addr_t)));
 	if (!io_tlb_orig_addr)
 		goto cleanup3;
 
-	memset(io_tlb_orig_addr, 0, io_tlb_nslabs * sizeof(struct swiotlb_phys_addr));
+	memset(io_tlb_orig_addr, 0, io_tlb_nslabs * sizeof(phys_addr_t));
 
 	/*
 	 * Get the overflow emergency buffer
@@ -308,8 +293,8 @@ swiotlb_late_init_with_default_size(size_t default_size)
 	return 0;
 
 cleanup4:
-	free_pages((unsigned long)io_tlb_orig_addr, get_order(io_tlb_nslabs *
-	                                                      sizeof(char *)));
+	free_pages((unsigned long)io_tlb_orig_addr,
+		   get_order(io_tlb_nslabs * sizeof(phys_addr_t)));
 	io_tlb_orig_addr = NULL;
 cleanup3:
 	free_pages((unsigned long)io_tlb_list, get_order(io_tlb_nslabs *
@@ -340,51 +325,44 @@ static int is_swiotlb_buffer(char *addr)
 	return addr >= io_tlb_start && addr < io_tlb_end;
 }
 
-static struct swiotlb_phys_addr swiotlb_bus_to_phys_addr(char *dma_addr)
-{
-	int index = (dma_addr - io_tlb_start) >> IO_TLB_SHIFT;
-	struct swiotlb_phys_addr buffer = io_tlb_orig_addr[index];
-	buffer.offset += (long)dma_addr & ((1 << IO_TLB_SHIFT) - 1);
-	buffer.page += buffer.offset >> PAGE_SHIFT;
-	buffer.offset &= PAGE_SIZE - 1;
-	return buffer;
-}
-
-static void
-__sync_single(struct swiotlb_phys_addr buffer, char *dma_addr, size_t size, int dir)
-{
-	if (PageHighMem(buffer.page)) {
-		size_t len, bytes;
-		char *dev, *host, *kmp;
-
-		len = size;
-		while (len != 0) {
-			unsigned long flags;
-
-			bytes = len;
-			if ((bytes + buffer.offset) > PAGE_SIZE)
-				bytes = PAGE_SIZE - buffer.offset;
-			local_irq_save(flags); /* protects KM_BOUNCE_READ */
-			kmp  = kmap_atomic(buffer.page, KM_BOUNCE_READ);
-			dev  = dma_addr + size - len;
-			host = kmp + buffer.offset;
-			if (dir == DMA_FROM_DEVICE)
-				memcpy(host, dev, bytes);
+/*
+ * Bounce: copy the swiotlb buffer back to the original dma location
+ */
+static void swiotlb_bounce(phys_addr_t phys, char *dma_addr, size_t size,
+			   enum dma_data_direction dir)
+{
+	unsigned long pfn = PFN_DOWN(phys);
+
+	if (PageHighMem(pfn_to_page(pfn))) {
+		/* The buffer does not have a mapping.  Map it in and copy */
+		unsigned int offset = phys & ~PAGE_MASK;
+		char *buffer;
+		unsigned int sz = 0;
+		unsigned long flags;
+
+		while (size) {
+			sz = min(PAGE_SIZE - offset, size);
+
+			local_irq_save(flags);
+			buffer = kmap_atomic(pfn_to_page(pfn),
+					     KM_BOUNCE_READ);
+			if (dir == DMA_TO_DEVICE)
+				memcpy(dma_addr, buffer + offset, sz);
 			else
-				memcpy(dev, host, bytes);
-			kunmap_atomic(kmp, KM_BOUNCE_READ);
+				memcpy(buffer + offset, dma_addr, sz);
+			kunmap_atomic(buffer, KM_BOUNCE_READ);
 			local_irq_restore(flags);
-			len -= bytes;
-			buffer.page++;
-			buffer.offset = 0;
+
+			size -= sz;
+			pfn++;
+			dma_addr += sz;
+			offset = 0;
 		}
 	} else {
-		void *v = page_address(buffer.page) + buffer.offset;
-
 		if (dir == DMA_TO_DEVICE)
-			memcpy(dma_addr, v, size);
+			memcpy(dma_addr, phys_to_virt(phys), size);
 		else
-			memcpy(v, dma_addr, size);
+			memcpy(phys_to_virt(phys), dma_addr, size);
 	}
 }
 
@@ -392,7 +370,7 @@ __sync_single(struct swiotlb_phys_addr buffer, char *dma_addr, size_t size, int
  * Allocates bounce buffer and returns its kernel virtual address.
  */
 static void *
-map_single(struct device *hwdev, struct swiotlb_phys_addr buffer, size_t size, int dir)
+map_single(struct device *hwdev, phys_addr_t phys, size_t size, int dir)
 {
 	unsigned long flags;
 	char *dma_addr;
@@ -402,10 +380,9 @@ map_single(struct device *hwdev, struct swiotlb_phys_addr buffer, size_t size, i
 	unsigned long mask;
 	unsigned long offset_slots;
 	unsigned long max_slots;
-	struct swiotlb_phys_addr slot_buf;
 
 	mask = dma_get_seg_boundary(hwdev);
-	start_dma_addr = swiotlb_virt_to_bus(io_tlb_start) & mask;
+	start_dma_addr = swiotlb_virt_to_bus(hwdev, io_tlb_start) & mask;
 
 	offset_slots = ALIGN(start_dma_addr, 1 << IO_TLB_SHIFT) >> IO_TLB_SHIFT;
 
@@ -487,15 +464,10 @@ found:
 	 * This is needed when we sync the memory.  Then we sync the buffer if
 	 * needed.
 	 */
-	slot_buf = buffer;
-	for (i = 0; i < nslots; i++) {
-		slot_buf.page += slot_buf.offset >> PAGE_SHIFT;
-		slot_buf.offset &= PAGE_SIZE - 1;
-		io_tlb_orig_addr[index+i] = slot_buf;
-		slot_buf.offset += 1 << IO_TLB_SHIFT;
-	}
+	for (i = 0; i < nslots; i++)
+		io_tlb_orig_addr[index+i] = phys + (i << IO_TLB_SHIFT);
 	if (dir == DMA_TO_DEVICE || dir == DMA_BIDIRECTIONAL)
-		__sync_single(buffer, dma_addr, size, DMA_TO_DEVICE);
+		swiotlb_bounce(phys, dma_addr, size, DMA_TO_DEVICE);
 
 	return dma_addr;
 }
@@ -509,17 +481,13 @@ unmap_single(struct device *hwdev, char *dma_addr, size_t size, int dir)
 	unsigned long flags;
 	int i, count, nslots = ALIGN(size, 1 << IO_TLB_SHIFT) >> IO_TLB_SHIFT;
 	int index = (dma_addr - io_tlb_start) >> IO_TLB_SHIFT;
-	struct swiotlb_phys_addr buffer = swiotlb_bus_to_phys_addr(dma_addr);
+	phys_addr_t phys = io_tlb_orig_addr[index];
 
 	/*
 	 * First, sync the memory before unmapping the entry
 	 */
-	if ((dir == DMA_FROM_DEVICE) || (dir == DMA_BIDIRECTIONAL))
-		/*
-		 * bounce... copy the data back into the original buffer * and
-		 * delete the bounce buffer.
-		 */
-		__sync_single(buffer, dma_addr, size, DMA_FROM_DEVICE);
+	if (phys && ((dir == DMA_FROM_DEVICE) || (dir == DMA_BIDIRECTIONAL)))
+		swiotlb_bounce(phys, dma_addr, size, DMA_FROM_DEVICE);
 
 	/*
 	 * Return the buffer to the free list by setting the corresponding
@@ -551,18 +519,21 @@ static void
 sync_single(struct device *hwdev, char *dma_addr, size_t size,
 	    int dir, int target)
 {
-	struct swiotlb_phys_addr buffer = swiotlb_bus_to_phys_addr(dma_addr);
+	int index = (dma_addr - io_tlb_start) >> IO_TLB_SHIFT;
+	phys_addr_t phys = io_tlb_orig_addr[index];
+
+	phys += ((unsigned long)dma_addr & ((1 << IO_TLB_SHIFT) - 1));
 
 	switch (target) {
 	case SYNC_FOR_CPU:
 		if (likely(dir == DMA_FROM_DEVICE || dir == DMA_BIDIRECTIONAL))
-			__sync_single(buffer, dma_addr, size, DMA_FROM_DEVICE);
+			swiotlb_bounce(phys, dma_addr, size, DMA_FROM_DEVICE);
 		else
 			BUG_ON(dir != DMA_TO_DEVICE);
 		break;
 	case SYNC_FOR_DEVICE:
 		if (likely(dir == DMA_TO_DEVICE || dir == DMA_BIDIRECTIONAL))
-			__sync_single(buffer, dma_addr, size, DMA_TO_DEVICE);
+			swiotlb_bounce(phys, dma_addr, size, DMA_TO_DEVICE);
 		else
 			BUG_ON(dir != DMA_FROM_DEVICE);
 		break;
@@ -584,7 +555,9 @@ swiotlb_alloc_coherent(struct device *hwdev, size_t size,
 		dma_mask = hwdev->coherent_dma_mask;
 
 	ret = (void *)__get_free_pages(flags, order);
-	if (ret && !is_buffer_dma_capable(dma_mask, swiotlb_virt_to_bus(ret), size)) {
+	if (ret &&
+	    !is_buffer_dma_capable(dma_mask, swiotlb_virt_to_bus(hwdev, ret),
+				   size)) {
 		/*
 		 * The allocated memory isn't reachable by the device.
 		 * Fall back on swiotlb_map_single().
@@ -599,16 +572,13 @@ swiotlb_alloc_coherent(struct device *hwdev, size_t size,
 		 * swiotlb_map_single(), which will grab memory from
 		 * the lowest available address range.
 		 */
-		struct swiotlb_phys_addr buffer;
-		buffer.page = virt_to_page(NULL);
-		buffer.offset = 0;
-		ret = map_single(hwdev, buffer, size, DMA_FROM_DEVICE);
+		ret = map_single(hwdev, 0, size, DMA_FROM_DEVICE);
 		if (!ret)
 			return NULL;
 	}
 
 	memset(ret, 0, size);
-	dev_addr = swiotlb_virt_to_bus(ret);
+	dev_addr = swiotlb_virt_to_bus(hwdev, ret);
 
 	/* Confirm address can be DMA'd by device */
 	if (!is_buffer_dma_capable(dma_mask, dev_addr, size)) {
@@ -623,6 +593,7 @@ swiotlb_alloc_coherent(struct device *hwdev, size_t size,
 	*dma_handle = dev_addr;
 	return ret;
 }
+EXPORT_SYMBOL(swiotlb_alloc_coherent);
 
 void
 swiotlb_free_coherent(struct device *hwdev, size_t size, void *vaddr,
@@ -635,6 +606,7 @@ swiotlb_free_coherent(struct device *hwdev, size_t size, void *vaddr,
 		/* DMA_TO_DEVICE to avoid memcpy in unmap_single */
 		unmap_single(hwdev, vaddr, size, DMA_TO_DEVICE);
 }
+EXPORT_SYMBOL(swiotlb_free_coherent);
 
 static void
 swiotlb_full(struct device *dev, size_t size, int dir, int do_panic)
@@ -647,7 +619,7 @@ swiotlb_full(struct device *dev, size_t size, int dir, int do_panic)
 	 * the damage, or panic when the transfer is too big.
 	 */
 	printk(KERN_ERR "DMA: Out of SW-IOMMU space for %zu bytes at "
-	       "device %s\n", size, dev ? dev->bus_id : "?");
+	       "device %s\n", size, dev ? dev_name(dev) : "?");
 
 	if (size > io_tlb_overflow && do_panic) {
 		if (dir == DMA_FROM_DEVICE || dir == DMA_BIDIRECTIONAL)
@@ -668,9 +640,8 @@ dma_addr_t
 swiotlb_map_single_attrs(struct device *hwdev, void *ptr, size_t size,
 			 int dir, struct dma_attrs *attrs)
 {
-	dma_addr_t dev_addr = swiotlb_virt_to_bus(ptr);
+	dma_addr_t dev_addr = swiotlb_virt_to_bus(hwdev, ptr);
 	void *map;
-	struct swiotlb_phys_addr buffer;
 
 	BUG_ON(dir == DMA_NONE);
 	/*
@@ -685,15 +656,13 @@ swiotlb_map_single_attrs(struct device *hwdev, void *ptr, size_t size,
 	/*
 	 * Oh well, have to allocate and map a bounce buffer.
 	 */
-	buffer.page   = virt_to_page(ptr);
-	buffer.offset = (unsigned long)ptr & ~PAGE_MASK;
-	map = map_single(hwdev, buffer, size, dir);
+	map = map_single(hwdev, virt_to_phys(ptr), size, dir);
 	if (!map) {
 		swiotlb_full(hwdev, size, dir, 1);
 		map = io_tlb_overflow_buffer;
 	}
 
-	dev_addr = swiotlb_virt_to_bus(map);
+	dev_addr = swiotlb_virt_to_bus(hwdev, map);
 
 	/*
 	 * Ensure that the address returned is DMA'ble
@@ -710,6 +679,7 @@ swiotlb_map_single(struct device *hwdev, void *ptr, size_t size, int dir)
 {
 	return swiotlb_map_single_attrs(hwdev, ptr, size, dir, NULL);
 }
+EXPORT_SYMBOL(swiotlb_map_single);
 
 /*
  * Unmap a single streaming mode DMA translation.  The dma_addr and size must
@@ -739,6 +709,8 @@ swiotlb_unmap_single(struct device *hwdev, dma_addr_t dev_addr, size_t size,
 {
 	return swiotlb_unmap_single_attrs(hwdev, dev_addr, size, dir, NULL);
 }
+EXPORT_SYMBOL(swiotlb_unmap_single);
+
 /*
  * Make physical memory consistent for a single streaming mode DMA translation
  * after a transfer.
@@ -768,6 +740,7 @@ swiotlb_sync_single_for_cpu(struct device *hwdev, dma_addr_t dev_addr,
 {
 	swiotlb_sync_single(hwdev, dev_addr, size, dir, SYNC_FOR_CPU);
 }
+EXPORT_SYMBOL(swiotlb_sync_single_for_cpu);
 
 void
 swiotlb_sync_single_for_device(struct device *hwdev, dma_addr_t dev_addr,
@@ -775,6 +748,7 @@ swiotlb_sync_single_for_device(struct device *hwdev, dma_addr_t dev_addr,
 {
 	swiotlb_sync_single(hwdev, dev_addr, size, dir, SYNC_FOR_DEVICE);
 }
+EXPORT_SYMBOL(swiotlb_sync_single_for_device);
 
 /*
  * Same as above, but for a sub-range of the mapping.
@@ -800,6 +774,7 @@ swiotlb_sync_single_range_for_cpu(struct device *hwdev, dma_addr_t dev_addr,
 	swiotlb_sync_single_range(hwdev, dev_addr, offset, size, dir,
 				  SYNC_FOR_CPU);
 }
+EXPORT_SYMBOL_GPL(swiotlb_sync_single_range_for_cpu);
 
 void
 swiotlb_sync_single_range_for_device(struct device *hwdev, dma_addr_t dev_addr,
@@ -808,9 +783,8 @@ swiotlb_sync_single_range_for_device(struct device *hwdev, dma_addr_t dev_addr,
 	swiotlb_sync_single_range(hwdev, dev_addr, offset, size, dir,
 				  SYNC_FOR_DEVICE);
 }
+EXPORT_SYMBOL_GPL(swiotlb_sync_single_range_for_device);
 
-void swiotlb_unmap_sg_attrs(struct device *, struct scatterlist *, int, int,
-			    struct dma_attrs *);
 /*
  * Map a set of buffers described by scatterlist in streaming mode for DMA.
  * This is the scatter-gather version of the above swiotlb_map_single
@@ -832,20 +806,18 @@ swiotlb_map_sg_attrs(struct device *hwdev, struct scatterlist *sgl, int nelems,
 		     int dir, struct dma_attrs *attrs)
 {
 	struct scatterlist *sg;
-	struct swiotlb_phys_addr buffer;
-	dma_addr_t dev_addr;
 	int i;
 
 	BUG_ON(dir == DMA_NONE);
 
 	for_each_sg(sgl, sg, nelems, i) {
-		dev_addr = swiotlb_sg_to_bus(sg);
-		if (range_needs_mapping(sg_virt(sg), sg->length) ||
+		void *addr = sg_virt(sg);
+		dma_addr_t dev_addr = swiotlb_virt_to_bus(hwdev, addr);
+
+		if (range_needs_mapping(addr, sg->length) ||
 		    address_needs_mapping(hwdev, dev_addr, sg->length)) {
-			void *map;
-			buffer.page   = sg_page(sg);
-			buffer.offset = sg->offset;
-			map = map_single(hwdev, buffer, sg->length, dir);
+			void *map = map_single(hwdev, sg_phys(sg),
+					       sg->length, dir);
 			if (!map) {
 				/* Don't panic here, we expect map_sg users
 				   to do proper error handling. */
@@ -855,7 +827,7 @@ swiotlb_map_sg_attrs(struct device *hwdev, struct scatterlist *sgl, int nelems,
 				sgl[0].dma_length = 0;
 				return 0;
 			}
-			sg->dma_address = swiotlb_virt_to_bus(map);
+			sg->dma_address = swiotlb_virt_to_bus(hwdev, map);
 		} else
 			sg->dma_address = dev_addr;
 		sg->dma_length = sg->length;
@@ -870,6 +842,7 @@ swiotlb_map_sg(struct device *hwdev, struct scatterlist *sgl, int nelems,
 {
 	return swiotlb_map_sg_attrs(hwdev, sgl, nelems, dir, NULL);
 }
+EXPORT_SYMBOL(swiotlb_map_sg);
 
 /*
  * Unmap a set of streaming mode DMA translations.  Again, cpu read rules
@@ -885,11 +858,11 @@ swiotlb_unmap_sg_attrs(struct device *hwdev, struct scatterlist *sgl,
 	BUG_ON(dir == DMA_NONE);
 
 	for_each_sg(sgl, sg, nelems, i) {
-		if (sg->dma_address != swiotlb_sg_to_bus(sg))
+		if (sg->dma_address != swiotlb_virt_to_bus(hwdev, sg_virt(sg)))
 			unmap_single(hwdev, swiotlb_bus_to_virt(sg->dma_address),
 				     sg->dma_length, dir);
 		else if (dir == DMA_FROM_DEVICE)
-			dma_mark_clean(swiotlb_bus_to_virt(sg->dma_address), sg->dma_length);
+			dma_mark_clean(sg_virt(sg), sg->dma_length);
 	}
 }
 EXPORT_SYMBOL(swiotlb_unmap_sg_attrs);
@@ -900,6 +873,7 @@ swiotlb_unmap_sg(struct device *hwdev, struct scatterlist *sgl, int nelems,
 {
 	return swiotlb_unmap_sg_attrs(hwdev, sgl, nelems, dir, NULL);
 }
+EXPORT_SYMBOL(swiotlb_unmap_sg);
 
 /*
  * Make physical memory consistent for a set of streaming mode DMA translations
@@ -918,11 +892,11 @@ swiotlb_sync_sg(struct device *hwdev, struct scatterlist *sgl,
 	BUG_ON(dir == DMA_NONE);
 
 	for_each_sg(sgl, sg, nelems, i) {
-		if (sg->dma_address != swiotlb_sg_to_bus(sg))
+		if (sg->dma_address != swiotlb_virt_to_bus(hwdev, sg_virt(sg)))
 			sync_single(hwdev, swiotlb_bus_to_virt(sg->dma_address),
 				    sg->dma_length, dir, target);
 		else if (dir == DMA_FROM_DEVICE)
-			dma_mark_clean(swiotlb_bus_to_virt(sg->dma_address), sg->dma_length);
+			dma_mark_clean(sg_virt(sg), sg->dma_length);
 	}
 }
 
@@ -932,6 +906,7 @@ swiotlb_sync_sg_for_cpu(struct device *hwdev, struct scatterlist *sg,
 {
 	swiotlb_sync_sg(hwdev, sg, nelems, dir, SYNC_FOR_CPU);
 }
+EXPORT_SYMBOL(swiotlb_sync_sg_for_cpu);
 
 void
 swiotlb_sync_sg_for_device(struct device *hwdev, struct scatterlist *sg,
@@ -939,12 +914,14 @@ swiotlb_sync_sg_for_device(struct device *hwdev, struct scatterlist *sg,
 {
 	swiotlb_sync_sg(hwdev, sg, nelems, dir, SYNC_FOR_DEVICE);
 }
+EXPORT_SYMBOL(swiotlb_sync_sg_for_device);
 
 int
 swiotlb_dma_mapping_error(struct device *hwdev, dma_addr_t dma_addr)
 {
-	return (dma_addr == swiotlb_virt_to_bus(io_tlb_overflow_buffer));
+	return (dma_addr == swiotlb_virt_to_bus(hwdev, io_tlb_overflow_buffer));
 }
+EXPORT_SYMBOL(swiotlb_dma_mapping_error);
 
 /*
  * Return whether the given device DMA address mask can be supported
@@ -955,20 +932,6 @@ swiotlb_dma_mapping_error(struct device *hwdev, dma_addr_t dma_addr)
 int
 swiotlb_dma_supported(struct device *hwdev, u64 mask)
 {
-	return swiotlb_virt_to_bus(io_tlb_end - 1) <= mask;
+	return swiotlb_virt_to_bus(hwdev, io_tlb_end - 1) <= mask;
 }
-
-EXPORT_SYMBOL(swiotlb_map_single);
-EXPORT_SYMBOL(swiotlb_unmap_single);
-EXPORT_SYMBOL(swiotlb_map_sg);
-EXPORT_SYMBOL(swiotlb_unmap_sg);
-EXPORT_SYMBOL(swiotlb_sync_single_for_cpu);
-EXPORT_SYMBOL(swiotlb_sync_single_for_device);
-EXPORT_SYMBOL_GPL(swiotlb_sync_single_range_for_cpu);
-EXPORT_SYMBOL_GPL(swiotlb_sync_single_range_for_device);
-EXPORT_SYMBOL(swiotlb_sync_sg_for_cpu);
-EXPORT_SYMBOL(swiotlb_sync_sg_for_device);
-EXPORT_SYMBOL(swiotlb_dma_mapping_error);
-EXPORT_SYMBOL(swiotlb_alloc_coherent);
-EXPORT_SYMBOL(swiotlb_free_coherent);
 EXPORT_SYMBOL(swiotlb_dma_supported);
diff --git a/lib/vsprintf.c b/lib/vsprintf.c
index 98d632277ca8..0fbd0121d91d 100644
--- a/lib/vsprintf.c
+++ b/lib/vsprintf.c
@@ -170,6 +170,8 @@ int strict_strtoul(const char *cp, unsigned int base, unsigned long *res)
 		return -EINVAL;
 
 	val = simple_strtoul(cp, &tail, base);
+	if (tail == cp)
+		return -EINVAL;
 	if ((*tail == '\0') ||
 		((len == (size_t)(tail - cp) + 1) && (*tail == '\n'))) {
 		*res = val;
@@ -241,6 +243,8 @@ int strict_strtoull(const char *cp, unsigned int base, unsigned long long *res)
 		return -EINVAL;
 
 	val = simple_strtoull(cp, &tail, base);
+	if (tail == cp)
+		return -EINVAL;
 	if ((*tail == '\0') ||
 		((len == (size_t)(tail - cp) + 1) && (*tail == '\n'))) {
 		*res = val;
diff --git a/mm/Kconfig b/mm/Kconfig
index 5b5790f8a816..a5b77811fdf2 100644
--- a/mm/Kconfig
+++ b/mm/Kconfig
@@ -181,12 +181,6 @@ config MIGRATION
 	  example on NUMA systems to put pages nearer to the processors accessing
 	  the page.
 
-config RESOURCES_64BIT
-	bool "64 bit Memory and IO resources (EXPERIMENTAL)" if (!64BIT && EXPERIMENTAL)
-	default 64BIT
-	help
-	  This option allows memory and IO resources to be 64 bit.
-
 config PHYS_ADDR_T_64BIT
 	def_bool 64BIT || ARCH_PHYS_ADDR_T_64BIT
 
diff --git a/mm/Makefile b/mm/Makefile
index 51c27709cc7c..72255be57f89 100644
--- a/mm/Makefile
+++ b/mm/Makefile
@@ -9,7 +9,7 @@ mmu-$(CONFIG_MMU)	:= fremap.o highmem.o madvise.o memory.o mincore.o \
 
 obj-y			:= bootmem.o filemap.o mempool.o oom_kill.o fadvise.o \
 			   maccess.o page_alloc.o page-writeback.o pdflush.o \
-			   readahead.o swap.o truncate.o vmscan.o \
+			   readahead.o swap.o truncate.o vmscan.o shmem.o \
 			   prio_tree.o util.o mmzone.o vmstat.o backing-dev.o \
 			   page_isolation.o mm_init.o $(mmu-y)
 
@@ -21,9 +21,7 @@ obj-$(CONFIG_HUGETLBFS)	+= hugetlb.o
 obj-$(CONFIG_NUMA) 	+= mempolicy.o
 obj-$(CONFIG_SPARSEMEM)	+= sparse.o
 obj-$(CONFIG_SPARSEMEM_VMEMMAP) += sparse-vmemmap.o
-obj-$(CONFIG_SHMEM) += shmem.o
 obj-$(CONFIG_TMPFS_POSIX_ACL) += shmem_acl.o
-obj-$(CONFIG_TINY_SHMEM) += tiny-shmem.o
 obj-$(CONFIG_SLOB) += slob.o
 obj-$(CONFIG_MMU_NOTIFIER) += mmu_notifier.o
 obj-$(CONFIG_SLAB) += slab.o
diff --git a/mm/backing-dev.c b/mm/backing-dev.c
index 801c08b046e6..8e8587444132 100644
--- a/mm/backing-dev.c
+++ b/mm/backing-dev.c
@@ -24,9 +24,9 @@ static void bdi_debug_init(void)
 static int bdi_debug_stats_show(struct seq_file *m, void *v)
 {
 	struct backing_dev_info *bdi = m->private;
-	long background_thresh;
-	long dirty_thresh;
-	long bdi_thresh;
+	unsigned long background_thresh;
+	unsigned long dirty_thresh;
+	unsigned long bdi_thresh;
 
 	get_dirty_limits(&background_thresh, &dirty_thresh, &bdi_thresh, bdi);
 
@@ -223,7 +223,7 @@ int bdi_init(struct backing_dev_info *bdi)
 	bdi->max_prop_frac = PROP_FRAC_BASE;
 
 	for (i = 0; i < NR_BDI_STAT_ITEMS; i++) {
-		err = percpu_counter_init_irq(&bdi->bdi_stat[i], 0);
+		err = percpu_counter_init(&bdi->bdi_stat[i], 0);
 		if (err)
 			goto err;
 	}
diff --git a/mm/bootmem.c b/mm/bootmem.c
index ac5a891f142a..51a0ccf61e0e 100644
--- a/mm/bootmem.c
+++ b/mm/bootmem.c
@@ -435,6 +435,10 @@ static void * __init alloc_bootmem_core(struct bootmem_data *bdata,
 	unsigned long fallback = 0;
 	unsigned long min, max, start, sidx, midx, step;
 
+	bdebug("nid=%td size=%lx [%lu pages] align=%lx goal=%lx limit=%lx\n",
+		bdata - bootmem_node_data, size, PAGE_ALIGN(size) >> PAGE_SHIFT,
+		align, goal, limit);
+
 	BUG_ON(!size);
 	BUG_ON(align & (align - 1));
 	BUG_ON(limit && goal + size > limit);
@@ -442,10 +446,6 @@ static void * __init alloc_bootmem_core(struct bootmem_data *bdata,
 	if (!bdata->node_bootmem_map)
 		return NULL;
 
-	bdebug("nid=%td size=%lx [%lu pages] align=%lx goal=%lx limit=%lx\n",
-		bdata - bootmem_node_data, size, PAGE_ALIGN(size) >> PAGE_SHIFT,
-		align, goal, limit);
-
 	min = bdata->node_min_pfn;
 	max = bdata->node_low_pfn;
 
diff --git a/mm/filemap.c b/mm/filemap.c
index f5769b4dc075..2f55a1e2baf7 100644
--- a/mm/filemap.c
+++ b/mm/filemap.c
@@ -210,7 +210,7 @@ int __filemap_fdatawrite_range(struct address_space *mapping, loff_t start,
 	int ret;
 	struct writeback_control wbc = {
 		.sync_mode = sync_mode,
-		.nr_to_write = mapping->nrpages * 2,
+		.nr_to_write = LONG_MAX,
 		.range_start = start,
 		.range_end = end,
 	};
@@ -741,7 +741,14 @@ repeat:
 		page = __page_cache_alloc(gfp_mask);
 		if (!page)
 			return NULL;
-		err = add_to_page_cache_lru(page, mapping, index, gfp_mask);
+		/*
+		 * We want a regular kernel memory (not highmem or DMA etc)
+		 * allocation for the radix tree nodes, but we need to honour
+		 * the context-specific requirements the caller has asked for.
+		 * GFP_RECLAIM_MASK collects those requirements.
+		 */
+		err = add_to_page_cache_lru(page, mapping, index,
+			(gfp_mask & GFP_RECLAIM_MASK));
 		if (unlikely(err)) {
 			page_cache_release(page);
 			page = NULL;
@@ -950,7 +957,7 @@ grab_cache_page_nowait(struct address_space *mapping, pgoff_t index)
 		return NULL;
 	}
 	page = __page_cache_alloc(mapping_gfp_mask(mapping) & ~__GFP_FS);
-	if (page && add_to_page_cache_lru(page, mapping, index, GFP_KERNEL)) {
+	if (page && add_to_page_cache_lru(page, mapping, index, GFP_NOFS)) {
 		page_cache_release(page);
 		page = NULL;
 	}
@@ -1317,7 +1324,8 @@ generic_file_aio_read(struct kiocb *iocb, const struct iovec *iov,
 			goto out; /* skip atime */
 		size = i_size_read(inode);
 		if (pos < size) {
-			retval = filemap_write_and_wait(mapping);
+			retval = filemap_write_and_wait_range(mapping, pos,
+					pos + iov_length(iov, nr_segs) - 1);
 			if (!retval) {
 				retval = mapping->a_ops->direct_IO(READ, iocb,
 							iov, pos, nr_segs);
@@ -1530,7 +1538,6 @@ retry_find:
 	/*
 	 * Found the page and have a reference on it.
 	 */
-	mark_page_accessed(page);
 	ra->prev_pos = (loff_t)page->index << PAGE_CACHE_SHIFT;
 	vmf->page = page;
 	return ret | VM_FAULT_LOCKED;
@@ -2060,18 +2067,10 @@ generic_file_direct_write(struct kiocb *iocb, const struct iovec *iov,
 	if (count != ocount)
 		*nr_segs = iov_shorten((struct iovec *)iov, *nr_segs, count);
 
-	/*
-	 * Unmap all mmappings of the file up-front.
-	 *
-	 * This will cause any pte dirty bits to be propagated into the
-	 * pageframes for the subsequent filemap_write_and_wait().
-	 */
 	write_len = iov_length(iov, *nr_segs);
 	end = (pos + write_len - 1) >> PAGE_CACHE_SHIFT;
-	if (mapping_mapped(mapping))
-		unmap_mapping_range(mapping, pos, write_len, 0);
 
-	written = filemap_write_and_wait(mapping);
+	written = filemap_write_and_wait_range(mapping, pos, pos + write_len - 1);
 	if (written)
 		goto out;
 
@@ -2291,7 +2290,8 @@ generic_file_buffered_write(struct kiocb *iocb, const struct iovec *iov,
 	 * the file data here, to try to honour O_DIRECT expectations.
 	 */
 	if (unlikely(file->f_flags & O_DIRECT) && written)
-		status = filemap_write_and_wait(mapping);
+		status = filemap_write_and_wait_range(mapping,
+					pos, pos + written - 1);
 
 	return written ? written : status;
 }
diff --git a/mm/filemap_xip.c b/mm/filemap_xip.c
index b5167dfb2f2d..0c04615651b7 100644
--- a/mm/filemap_xip.c
+++ b/mm/filemap_xip.c
@@ -193,7 +193,7 @@ retry:
 			/* Nuke the page table entry. */
 			flush_cache_page(vma, address, pte_pfn(*pte));
 			pteval = ptep_clear_flush_notify(vma, address, pte);
-			page_remove_rmap(page, vma);
+			page_remove_rmap(page);
 			dec_mm_counter(mm, file_rss);
 			BUG_ON(pte_dirty(pteval));
 			pte_unmap_unlock(pte, ptl);
diff --git a/mm/fremap.c b/mm/fremap.c
index 7d12ca70ef7b..62d5bbda921a 100644
--- a/mm/fremap.c
+++ b/mm/fremap.c
@@ -37,7 +37,7 @@ static void zap_pte(struct mm_struct *mm, struct vm_area_struct *vma,
 		if (page) {
 			if (pte_dirty(pte))
 				set_page_dirty(page);
-			page_remove_rmap(page, vma);
+			page_remove_rmap(page);
 			page_cache_release(page);
 			update_hiwater_rss(mm);
 			dec_mm_counter(mm, file_rss);
diff --git a/mm/hugetlb.c b/mm/hugetlb.c
index 6058b53dcb89..618e98304080 100644
--- a/mm/hugetlb.c
+++ b/mm/hugetlb.c
@@ -220,6 +220,35 @@ static pgoff_t vma_hugecache_offset(struct hstate *h,
 }
 
 /*
+ * Return the size of the pages allocated when backing a VMA. In the majority
+ * cases this will be same size as used by the page table entries.
+ */
+unsigned long vma_kernel_pagesize(struct vm_area_struct *vma)
+{
+	struct hstate *hstate;
+
+	if (!is_vm_hugetlb_page(vma))
+		return PAGE_SIZE;
+
+	hstate = hstate_vma(vma);
+
+	return 1UL << (hstate->order + PAGE_SHIFT);
+}
+
+/*
+ * Return the page size being used by the MMU to back a VMA. In the majority
+ * of cases, the page size used by the kernel matches the MMU size. On
+ * architectures where it differs, an architecture-specific version of this
+ * function is required.
+ */
+#ifndef vma_mmu_pagesize
+unsigned long vma_mmu_pagesize(struct vm_area_struct *vma)
+{
+	return vma_kernel_pagesize(vma);
+}
+#endif
+
+/*
  * Flags for MAP_PRIVATE reservations.  These are stored in the bottom
  * bits of the reservation map pointer, which are always clear due to
  * alignment.
@@ -371,8 +400,10 @@ static void clear_huge_page(struct page *page,
 {
 	int i;
 
-	if (unlikely(sz > MAX_ORDER_NR_PAGES))
-		return clear_gigantic_page(page, addr, sz);
+	if (unlikely(sz > MAX_ORDER_NR_PAGES)) {
+		clear_gigantic_page(page, addr, sz);
+		return;
+	}
 
 	might_sleep();
 	for (i = 0; i < sz/PAGE_SIZE; i++) {
@@ -404,8 +435,10 @@ static void copy_huge_page(struct page *dst, struct page *src,
 	int i;
 	struct hstate *h = hstate_vma(vma);
 
-	if (unlikely(pages_per_huge_page(h) > MAX_ORDER_NR_PAGES))
-		return copy_gigantic_page(dst, src, addr, vma);
+	if (unlikely(pages_per_huge_page(h) > MAX_ORDER_NR_PAGES)) {
+		copy_gigantic_page(dst, src, addr, vma);
+		return;
+	}
 
 	might_sleep();
 	for (i = 0; i < pages_per_huge_page(h); i++) {
@@ -972,7 +1005,7 @@ static struct page *alloc_huge_page(struct vm_area_struct *vma,
 	return page;
 }
 
-__attribute__((weak)) int alloc_bootmem_huge_page(struct hstate *h)
+int __weak alloc_bootmem_huge_page(struct hstate *h)
 {
 	struct huge_bootmem_page *m;
 	int nr_nodes = nodes_weight(node_online_map);
@@ -991,8 +1024,7 @@ __attribute__((weak)) int alloc_bootmem_huge_page(struct hstate *h)
 			 * puts them into the mem_map).
 			 */
 			m = addr;
-			if (m)
-				goto found;
+			goto found;
 		}
 		hstate_next_node(h);
 		nr_nodes--;
diff --git a/mm/internal.h b/mm/internal.h
index 13333bc2eb68..478223b73a2a 100644
--- a/mm/internal.h
+++ b/mm/internal.h
@@ -49,6 +49,7 @@ extern void putback_lru_page(struct page *page);
 /*
  * in mm/page_alloc.c
  */
+extern unsigned long highest_memmap_pfn;
 extern void __free_pages_bootmem(struct page *page, unsigned int order);
 
 /*
@@ -275,6 +276,7 @@ static inline void mminit_validate_memmodel_limits(unsigned long *start_pfn,
 #define GUP_FLAGS_WRITE                  0x1
 #define GUP_FLAGS_FORCE                  0x2
 #define GUP_FLAGS_IGNORE_VMA_PERMISSIONS 0x4
+#define GUP_FLAGS_IGNORE_SIGKILL         0x8
 
 int __get_user_pages(struct task_struct *tsk, struct mm_struct *mm,
 		     unsigned long start, int len, int flags,
diff --git a/mm/memcontrol.c b/mm/memcontrol.c
index 866dcc7eeb0c..51ee96545579 100644
--- a/mm/memcontrol.c
+++ b/mm/memcontrol.c
@@ -779,7 +779,8 @@ int mem_cgroup_shrink_usage(struct mm_struct *mm, gfp_t gfp_mask)
 	return 0;
 }
 
-int mem_cgroup_resize_limit(struct mem_cgroup *memcg, unsigned long long val)
+static int mem_cgroup_resize_limit(struct mem_cgroup *memcg,
+				   unsigned long long val)
 {
 
 	int retry_count = MEM_CGROUP_RECLAIM_RETRIES;
diff --git a/mm/memory.c b/mm/memory.c
index 7b9db658aca2..3f8fa06b963b 100644
--- a/mm/memory.c
+++ b/mm/memory.c
@@ -52,6 +52,9 @@
 #include <linux/writeback.h>
 #include <linux/memcontrol.h>
 #include <linux/mmu_notifier.h>
+#include <linux/kallsyms.h>
+#include <linux/swapops.h>
+#include <linux/elf.h>
 
 #include <asm/pgalloc.h>
 #include <asm/uaccess.h>
@@ -59,9 +62,6 @@
 #include <asm/tlbflush.h>
 #include <asm/pgtable.h>
 
-#include <linux/swapops.h>
-#include <linux/elf.h>
-
 #include "internal.h"
 
 #ifndef CONFIG_NEED_MULTIPLE_NODES
@@ -375,15 +375,65 @@ static inline void add_mm_rss(struct mm_struct *mm, int file_rss, int anon_rss)
  *
  * The calling function must still handle the error.
  */
-static void print_bad_pte(struct vm_area_struct *vma, pte_t pte,
-			  unsigned long vaddr)
-{
-	printk(KERN_ERR "Bad pte = %08llx, process = %s, "
-			"vm_flags = %lx, vaddr = %lx\n",
-		(long long)pte_val(pte),
-		(vma->vm_mm == current->mm ? current->comm : "???"),
-		vma->vm_flags, vaddr);
+static void print_bad_pte(struct vm_area_struct *vma, unsigned long addr,
+			  pte_t pte, struct page *page)
+{
+	pgd_t *pgd = pgd_offset(vma->vm_mm, addr);
+	pud_t *pud = pud_offset(pgd, addr);
+	pmd_t *pmd = pmd_offset(pud, addr);
+	struct address_space *mapping;
+	pgoff_t index;
+	static unsigned long resume;
+	static unsigned long nr_shown;
+	static unsigned long nr_unshown;
+
+	/*
+	 * Allow a burst of 60 reports, then keep quiet for that minute;
+	 * or allow a steady drip of one report per second.
+	 */
+	if (nr_shown == 60) {
+		if (time_before(jiffies, resume)) {
+			nr_unshown++;
+			return;
+		}
+		if (nr_unshown) {
+			printk(KERN_ALERT
+				"BUG: Bad page map: %lu messages suppressed\n",
+				nr_unshown);
+			nr_unshown = 0;
+		}
+		nr_shown = 0;
+	}
+	if (nr_shown++ == 0)
+		resume = jiffies + 60 * HZ;
+
+	mapping = vma->vm_file ? vma->vm_file->f_mapping : NULL;
+	index = linear_page_index(vma, addr);
+
+	printk(KERN_ALERT
+		"BUG: Bad page map in process %s  pte:%08llx pmd:%08llx\n",
+		current->comm,
+		(long long)pte_val(pte), (long long)pmd_val(*pmd));
+	if (page) {
+		printk(KERN_ALERT
+		"page:%p flags:%p count:%d mapcount:%d mapping:%p index:%lx\n",
+		page, (void *)page->flags, page_count(page),
+		page_mapcount(page), page->mapping, page->index);
+	}
+	printk(KERN_ALERT
+		"addr:%p vm_flags:%08lx anon_vma:%p mapping:%p index:%lx\n",
+		(void *)addr, vma->vm_flags, vma->anon_vma, mapping, index);
+	/*
+	 * Choose text because data symbols depend on CONFIG_KALLSYMS_ALL=y
+	 */
+	if (vma->vm_ops)
+		print_symbol(KERN_ALERT "vma->vm_ops->fault: %s\n",
+				(unsigned long)vma->vm_ops->fault);
+	if (vma->vm_file && vma->vm_file->f_op)
+		print_symbol(KERN_ALERT "vma->vm_file->f_op->mmap: %s\n",
+				(unsigned long)vma->vm_file->f_op->mmap);
 	dump_stack();
+	add_taint(TAINT_BAD_PAGE);
 }
 
 static inline int is_cow_mapping(unsigned int flags)
@@ -441,21 +491,18 @@ static inline int is_cow_mapping(unsigned int flags)
 struct page *vm_normal_page(struct vm_area_struct *vma, unsigned long addr,
 				pte_t pte)
 {
-	unsigned long pfn;
+	unsigned long pfn = pte_pfn(pte);
 
 	if (HAVE_PTE_SPECIAL) {
-		if (likely(!pte_special(pte))) {
-			VM_BUG_ON(!pfn_valid(pte_pfn(pte)));
-			return pte_page(pte);
-		}
-		VM_BUG_ON(!(vma->vm_flags & (VM_PFNMAP | VM_MIXEDMAP)));
+		if (likely(!pte_special(pte)))
+			goto check_pfn;
+		if (!(vma->vm_flags & (VM_PFNMAP | VM_MIXEDMAP)))
+			print_bad_pte(vma, addr, pte, NULL);
 		return NULL;
 	}
 
 	/* !HAVE_PTE_SPECIAL case follows: */
 
-	pfn = pte_pfn(pte);
-
 	if (unlikely(vma->vm_flags & (VM_PFNMAP|VM_MIXEDMAP))) {
 		if (vma->vm_flags & VM_MIXEDMAP) {
 			if (!pfn_valid(pfn))
@@ -471,11 +518,14 @@ struct page *vm_normal_page(struct vm_area_struct *vma, unsigned long addr,
 		}
 	}
 
-	VM_BUG_ON(!pfn_valid(pfn));
+check_pfn:
+	if (unlikely(pfn > highest_memmap_pfn)) {
+		print_bad_pte(vma, addr, pte, NULL);
+		return NULL;
+	}
 
 	/*
 	 * NOTE! We still have PageReserved() pages in the page tables.
-	 *
 	 * eg. VDSO mappings can cause them to exist.
 	 */
 out:
@@ -767,11 +817,14 @@ static unsigned long zap_pte_range(struct mmu_gather *tlb,
 			else {
 				if (pte_dirty(ptent))
 					set_page_dirty(page);
-				if (pte_young(ptent))
-					SetPageReferenced(page);
+				if (pte_young(ptent) &&
+				    likely(!VM_SequentialReadHint(vma)))
+					mark_page_accessed(page);
 				file_rss--;
 			}
-			page_remove_rmap(page, vma);
+			page_remove_rmap(page);
+			if (unlikely(page_mapcount(page) < 0))
+				print_bad_pte(vma, addr, ptent, page);
 			tlb_remove_page(tlb, page);
 			continue;
 		}
@@ -781,8 +834,12 @@ static unsigned long zap_pte_range(struct mmu_gather *tlb,
 		 */
 		if (unlikely(details))
 			continue;
-		if (!pte_file(ptent))
-			free_swap_and_cache(pte_to_swp_entry(ptent));
+		if (pte_file(ptent)) {
+			if (unlikely(!(vma->vm_flags & VM_NONLINEAR)))
+				print_bad_pte(vma, addr, ptent, NULL);
+		} else if
+		  (unlikely(!free_swap_and_cache(pte_to_swp_entry(ptent))))
+			print_bad_pte(vma, addr, ptent, NULL);
 		pte_clear_not_present_full(mm, addr, pte, tlb->fullmm);
 	} while (pte++, addr += PAGE_SIZE, (addr != end && *zap_work > 0));
 
@@ -1153,6 +1210,7 @@ int __get_user_pages(struct task_struct *tsk, struct mm_struct *mm,
 	int write = !!(flags & GUP_FLAGS_WRITE);
 	int force = !!(flags & GUP_FLAGS_FORCE);
 	int ignore = !!(flags & GUP_FLAGS_IGNORE_VMA_PERMISSIONS);
+	int ignore_sigkill = !!(flags & GUP_FLAGS_IGNORE_SIGKILL);
 
 	if (len <= 0)
 		return 0;
@@ -1231,12 +1289,15 @@ int __get_user_pages(struct task_struct *tsk, struct mm_struct *mm,
 			struct page *page;
 
 			/*
-			 * If tsk is ooming, cut off its access to large memory
-			 * allocations. It has a pending SIGKILL, but it can't
-			 * be processed until returning to user space.
+			 * If we have a pending SIGKILL, don't keep faulting
+			 * pages and potentially allocating memory, unless
+			 * current is handling munlock--e.g., on exit. In
+			 * that case, we are not allocating memory.  Rather,
+			 * we're only unlocking already resident/mapped pages.
 			 */
-			if (unlikely(test_tsk_thread_flag(tsk, TIF_MEMDIE)))
-				return i ? i : -ENOMEM;
+			if (unlikely(!ignore_sigkill &&
+					fatal_signal_pending(current)))
+				return i ? i : -ERESTARTSYS;
 
 			if (write)
 				foll_flags |= FOLL_WRITE;
@@ -1263,9 +1324,15 @@ int __get_user_pages(struct task_struct *tsk, struct mm_struct *mm,
 				 * do_wp_page has broken COW when necessary,
 				 * even if maybe_mkwrite decided not to set
 				 * pte_write. We can thus safely do subsequent
-				 * page lookups as if they were reads.
+				 * page lookups as if they were reads. But only
+				 * do so when looping for pte_write is futile:
+				 * in some cases userspace may also be wanting
+				 * to write to the gotten user page, which a
+				 * read fault here might prevent (a readonly
+				 * page might get reCOWed by userspace write).
 				 */
-				if (ret & VM_FAULT_WRITE)
+				if ((ret & VM_FAULT_WRITE) &&
+				    !(vma->vm_flags & VM_WRITE))
 					foll_flags &= ~FOLL_WRITE;
 
 				cond_resched();
@@ -1644,6 +1711,8 @@ static int apply_to_pte_range(struct mm_struct *mm, pmd_t *pmd,
 
 	BUG_ON(pmd_huge(*pmd));
 
+	arch_enter_lazy_mmu_mode();
+
 	token = pmd_pgtable(*pmd);
 
 	do {
@@ -1652,6 +1721,8 @@ static int apply_to_pte_range(struct mm_struct *mm, pmd_t *pmd,
 			break;
 	} while (pte++, addr += PAGE_SIZE, addr != end);
 
+	arch_leave_lazy_mmu_mode();
+
 	if (mm != &init_mm)
 		pte_unmap_unlock(pte-1, ptl);
 	return err;
@@ -1837,10 +1908,21 @@ static int do_wp_page(struct mm_struct *mm, struct vm_area_struct *vma,
 	 * not dirty accountable.
 	 */
 	if (PageAnon(old_page)) {
-		if (trylock_page(old_page)) {
-			reuse = can_share_swap_page(old_page);
-			unlock_page(old_page);
+		if (!trylock_page(old_page)) {
+			page_cache_get(old_page);
+			pte_unmap_unlock(page_table, ptl);
+			lock_page(old_page);
+			page_table = pte_offset_map_lock(mm, pmd, address,
+							 &ptl);
+			if (!pte_same(*page_table, orig_pte)) {
+				unlock_page(old_page);
+				page_cache_release(old_page);
+				goto unlock;
+			}
+			page_cache_release(old_page);
 		}
+		reuse = reuse_swap_page(old_page);
+		unlock_page(old_page);
 	} else if (unlikely((vma->vm_flags & (VM_WRITE|VM_SHARED)) ==
 					(VM_WRITE|VM_SHARED))) {
 		/*
@@ -1943,11 +2025,7 @@ gotten:
 		 * thread doing COW.
 		 */
 		ptep_clear_flush_notify(vma, address, page_table);
-		SetPageSwapBacked(new_page);
-		lru_cache_add_active_or_unevictable(new_page, vma);
 		page_add_new_anon_rmap(new_page, vma, address);
-
-//TODO:  is this safe?  do_anonymous_page() does it this way.
 		set_pte_at(mm, address, page_table, entry);
 		update_mmu_cache(vma, address, entry);
 		if (old_page) {
@@ -1973,7 +2051,7 @@ gotten:
 			 * mapcount is visible. So transitively, TLBs to
 			 * old page will be flushed before it can be reused.
 			 */
-			page_remove_rmap(old_page, vma);
+			page_remove_rmap(old_page);
 		}
 
 		/* Free the old page.. */
@@ -2374,7 +2452,7 @@ static int do_swap_page(struct mm_struct *mm, struct vm_area_struct *vma,
 
 	inc_mm_counter(mm, anon_rss);
 	pte = mk_pte(page, vma->vm_page_prot);
-	if (write_access && can_share_swap_page(page)) {
+	if (write_access && reuse_swap_page(page)) {
 		pte = maybe_mkwrite(pte_mkdirty(pte), vma);
 		write_access = 0;
 	}
@@ -2385,7 +2463,7 @@ static int do_swap_page(struct mm_struct *mm, struct vm_area_struct *vma,
 
 	swap_free(entry);
 	if (vm_swap_full() || (vma->vm_flags & VM_LOCKED) || PageMlocked(page))
-		remove_exclusive_swap_page(page);
+		try_to_free_swap(page);
 	unlock_page(page);
 
 	if (write_access) {
@@ -2442,8 +2520,6 @@ static int do_anonymous_page(struct mm_struct *mm, struct vm_area_struct *vma,
 	if (!pte_none(*page_table))
 		goto release;
 	inc_mm_counter(mm, anon_rss);
-	SetPageSwapBacked(page);
-	lru_cache_add_active_or_unevictable(page, vma);
 	page_add_new_anon_rmap(page, vma, address);
 	set_pte_at(mm, address, page_table, entry);
 
@@ -2591,8 +2667,6 @@ static int __do_fault(struct mm_struct *mm, struct vm_area_struct *vma,
 			entry = maybe_mkwrite(pte_mkdirty(entry), vma);
 		if (anon) {
 			inc_mm_counter(mm, anon_rss);
-			SetPageSwapBacked(page);
-			lru_cache_add_active_or_unevictable(page, vma);
 			page_add_new_anon_rmap(page, vma, address);
 		} else {
 			inc_mm_counter(mm, file_rss);
@@ -2602,7 +2676,6 @@ static int __do_fault(struct mm_struct *mm, struct vm_area_struct *vma,
 				get_page(dirty_page);
 			}
 		}
-//TODO:  is this safe?  do_anonymous_page() does it this way.
 		set_pte_at(mm, address, page_table, entry);
 
 		/* no need to invalidate: a not-present page won't be cached */
@@ -2666,12 +2739,11 @@ static int do_nonlinear_fault(struct mm_struct *mm, struct vm_area_struct *vma,
 	if (!pte_unmap_same(mm, pmd, page_table, orig_pte))
 		return 0;
 
-	if (unlikely(!(vma->vm_flags & VM_NONLINEAR) ||
-			!(vma->vm_flags & VM_CAN_NONLINEAR))) {
+	if (unlikely(!(vma->vm_flags & VM_NONLINEAR))) {
 		/*
 		 * Page table corrupted: show pte and kill process.
 		 */
-		print_bad_pte(vma, orig_pte, address);
+		print_bad_pte(vma, address, orig_pte, NULL);
 		return VM_FAULT_OOM;
 	}
 
@@ -2953,7 +3025,7 @@ int generic_access_phys(struct vm_area_struct *vma, unsigned long addr,
 {
 	resource_size_t phys_addr;
 	unsigned long prot = 0;
-	void *maddr;
+	void __iomem *maddr;
 	int offset = addr & (PAGE_SIZE-1);
 
 	if (follow_phys(vma, addr, write, &prot, &phys_addr))
diff --git a/mm/memory_hotplug.c b/mm/memory_hotplug.c
index b17371185468..c083cf5fd6df 100644
--- a/mm/memory_hotplug.c
+++ b/mm/memory_hotplug.c
@@ -216,7 +216,8 @@ static int __meminit __add_zone(struct zone *zone, unsigned long phys_start_pfn)
 	return 0;
 }
 
-static int __meminit __add_section(struct zone *zone, unsigned long phys_start_pfn)
+static int __meminit __add_section(int nid, struct zone *zone,
+					unsigned long phys_start_pfn)
 {
 	int nr_pages = PAGES_PER_SECTION;
 	int ret;
@@ -234,7 +235,7 @@ static int __meminit __add_section(struct zone *zone, unsigned long phys_start_p
 	if (ret < 0)
 		return ret;
 
-	return register_new_memory(__pfn_to_section(phys_start_pfn));
+	return register_new_memory(nid, __pfn_to_section(phys_start_pfn));
 }
 
 #ifdef CONFIG_SPARSEMEM_VMEMMAP
@@ -273,8 +274,8 @@ static int __remove_section(struct zone *zone, struct mem_section *ms)
  * call this function after deciding the zone to which to
  * add the new pages.
  */
-int __ref __add_pages(struct zone *zone, unsigned long phys_start_pfn,
-		 unsigned long nr_pages)
+int __ref __add_pages(int nid, struct zone *zone, unsigned long phys_start_pfn,
+			unsigned long nr_pages)
 {
 	unsigned long i;
 	int err = 0;
@@ -284,7 +285,7 @@ int __ref __add_pages(struct zone *zone, unsigned long phys_start_pfn,
 	end_sec = pfn_to_section_nr(phys_start_pfn + nr_pages - 1);
 
 	for (i = start_sec; i <= end_sec; i++) {
-		err = __add_section(zone, i << PFN_SECTION_SHIFT);
+		err = __add_section(nid, zone, i << PFN_SECTION_SHIFT);
 
 		/*
 		 * EEXIST is finally dealt with by ioresource collision
@@ -626,15 +627,12 @@ int scan_lru_pages(unsigned long start, unsigned long end)
 }
 
 static struct page *
-hotremove_migrate_alloc(struct page *page,
-			unsigned long private,
-			int **x)
+hotremove_migrate_alloc(struct page *page, unsigned long private, int **x)
 {
-	/* This should be improoooooved!! */
-	return alloc_page(GFP_HIGHUSER_PAGECACHE);
+	/* This should be improooooved!! */
+	return alloc_page(GFP_HIGHUSER_MOVABLE);
 }
 
-
 #define NR_OFFLINE_AT_ONCE_PAGES	(256)
 static int
 do_migrate_range(unsigned long start_pfn, unsigned long end_pfn)
diff --git a/mm/migrate.c b/mm/migrate.c
index 21631ab8c08b..55373983c9c6 100644
--- a/mm/migrate.c
+++ b/mm/migrate.c
@@ -300,12 +300,10 @@ static int migrate_page_move_mapping(struct address_space *mapping,
 	 * Now we know that no one else is looking at the page.
 	 */
 	get_page(newpage);	/* add cache reference */
-#ifdef CONFIG_SWAP
 	if (PageSwapCache(page)) {
 		SetPageSwapCache(newpage);
 		set_page_private(newpage, page_private(page));
 	}
-#endif
 
 	radix_tree_replace_slot(pslot, newpage);
 
@@ -373,9 +371,7 @@ static void migrate_page_copy(struct page *newpage, struct page *page)
 
 	mlock_migrate_page(newpage, page);
 
-#ifdef CONFIG_SWAP
 	ClearPageSwapCache(page);
-#endif
 	ClearPagePrivate(page);
 	set_page_private(page, 0);
 	/* page->mapping contains a flag for PageAnon() */
@@ -848,12 +844,6 @@ static int do_move_page_to_node_array(struct mm_struct *mm,
 		struct vm_area_struct *vma;
 		struct page *page;
 
-		/*
-		 * A valid page pointer that will not match any of the
-		 * pages that will be moved.
-		 */
-		pp->page = ZERO_PAGE(0);
-
 		err = -EFAULT;
 		vma = find_vma(mm, pp->addr);
 		if (!vma || !vma_migratable(vma))
@@ -919,41 +909,43 @@ static int do_pages_move(struct mm_struct *mm, struct task_struct *task,
 			 const int __user *nodes,
 			 int __user *status, int flags)
 {
-	struct page_to_node *pm = NULL;
+	struct page_to_node *pm;
 	nodemask_t task_nodes;
-	int err = 0;
-	int i;
+	unsigned long chunk_nr_pages;
+	unsigned long chunk_start;
+	int err;
 
 	task_nodes = cpuset_mems_allowed(task);
 
-	/* Limit nr_pages so that the multiplication may not overflow */
-	if (nr_pages >= ULONG_MAX / sizeof(struct page_to_node) - 1) {
-		err = -E2BIG;
-		goto out;
-	}
-
-	pm = vmalloc((nr_pages + 1) * sizeof(struct page_to_node));
-	if (!pm) {
-		err = -ENOMEM;
+	err = -ENOMEM;
+	pm = (struct page_to_node *)__get_free_page(GFP_KERNEL);
+	if (!pm)
 		goto out;
-	}
-
 	/*
-	 * Get parameters from user space and initialize the pm
-	 * array. Return various errors if the user did something wrong.
+	 * Store a chunk of page_to_node array in a page,
+	 * but keep the last one as a marker
 	 */
-	for (i = 0; i < nr_pages; i++) {
-		const void __user *p;
+	chunk_nr_pages = (PAGE_SIZE / sizeof(struct page_to_node)) - 1;
 
-		err = -EFAULT;
-		if (get_user(p, pages + i))
-			goto out_pm;
+	for (chunk_start = 0;
+	     chunk_start < nr_pages;
+	     chunk_start += chunk_nr_pages) {
+		int j;
 
-		pm[i].addr = (unsigned long)p;
-		if (nodes) {
+		if (chunk_start + chunk_nr_pages > nr_pages)
+			chunk_nr_pages = nr_pages - chunk_start;
+
+		/* fill the chunk pm with addrs and nodes from user-space */
+		for (j = 0; j < chunk_nr_pages; j++) {
+			const void __user *p;
 			int node;
 
-			if (get_user(node, nodes + i))
+			err = -EFAULT;
+			if (get_user(p, pages + j + chunk_start))
+				goto out_pm;
+			pm[j].addr = (unsigned long) p;
+
+			if (get_user(node, nodes + j + chunk_start))
 				goto out_pm;
 
 			err = -ENODEV;
@@ -964,22 +956,29 @@ static int do_pages_move(struct mm_struct *mm, struct task_struct *task,
 			if (!node_isset(node, task_nodes))
 				goto out_pm;
 
-			pm[i].node = node;
-		} else
-			pm[i].node = 0;	/* anything to not match MAX_NUMNODES */
-	}
-	/* End marker */
-	pm[nr_pages].node = MAX_NUMNODES;
+			pm[j].node = node;
+		}
+
+		/* End marker for this chunk */
+		pm[chunk_nr_pages].node = MAX_NUMNODES;
+
+		/* Migrate this chunk */
+		err = do_move_page_to_node_array(mm, pm,
+						 flags & MPOL_MF_MOVE_ALL);
+		if (err < 0)
+			goto out_pm;
 
-	err = do_move_page_to_node_array(mm, pm, flags & MPOL_MF_MOVE_ALL);
-	if (err >= 0)
 		/* Return status information */
-		for (i = 0; i < nr_pages; i++)
-			if (put_user(pm[i].status, status + i))
+		for (j = 0; j < chunk_nr_pages; j++)
+			if (put_user(pm[j].status, status + j + chunk_start)) {
 				err = -EFAULT;
+				goto out_pm;
+			}
+	}
+	err = 0;
 
 out_pm:
-	vfree(pm);
+	free_page((unsigned long)pm);
 out:
 	return err;
 }
diff --git a/mm/mlock.c b/mm/mlock.c
index 3035a56e7616..e125156c664e 100644
--- a/mm/mlock.c
+++ b/mm/mlock.c
@@ -173,12 +173,13 @@ static long __mlock_vma_pages_range(struct vm_area_struct *vma,
 		  (atomic_read(&mm->mm_users) != 0));
 
 	/*
-	 * mlock:   don't page populate if page has PROT_NONE permission.
-	 * munlock: the pages always do munlock althrough
-	 *          its has PROT_NONE permission.
+	 * mlock:   don't page populate if vma has PROT_NONE permission.
+	 * munlock: always do munlock although the vma has PROT_NONE
+	 *          permission, or SIGKILL is pending.
 	 */
 	if (!mlock)
-		gup_flags |= GUP_FLAGS_IGNORE_VMA_PERMISSIONS;
+		gup_flags |= GUP_FLAGS_IGNORE_VMA_PERMISSIONS |
+			     GUP_FLAGS_IGNORE_SIGKILL;
 
 	if (vma->vm_flags & VM_WRITE)
 		gup_flags |= GUP_FLAGS_WRITE;
diff --git a/mm/mmap.c b/mm/mmap.c
index 2c778fcfd9bd..a910c045cfd4 100644
--- a/mm/mmap.c
+++ b/mm/mmap.c
@@ -413,7 +413,7 @@ void __vma_link_rb(struct mm_struct *mm, struct vm_area_struct *vma,
 
 static void __vma_link_file(struct vm_area_struct *vma)
 {
-	struct file * file;
+	struct file *file;
 
 	file = vma->vm_file;
 	if (file) {
@@ -474,11 +474,10 @@ static void vma_link(struct mm_struct *mm, struct vm_area_struct *vma,
  * insert vm structure into list and rbtree and anon_vma,
  * but it has already been inserted into prio_tree earlier.
  */
-static void
-__insert_vm_struct(struct mm_struct * mm, struct vm_area_struct * vma)
+static void __insert_vm_struct(struct mm_struct *mm, struct vm_area_struct *vma)
 {
-	struct vm_area_struct * __vma, * prev;
-	struct rb_node ** rb_link, * rb_parent;
+	struct vm_area_struct *__vma, *prev;
+	struct rb_node **rb_link, *rb_parent;
 
 	__vma = find_vma_prepare(mm, vma->vm_start,&prev, &rb_link, &rb_parent);
 	BUG_ON(__vma && __vma->vm_start < vma->vm_end);
@@ -908,7 +907,7 @@ void vm_stat_account(struct mm_struct *mm, unsigned long flags,
  * The caller must hold down_write(current->mm->mmap_sem).
  */
 
-unsigned long do_mmap_pgoff(struct file * file, unsigned long addr,
+unsigned long do_mmap_pgoff(struct file *file, unsigned long addr,
 			unsigned long len, unsigned long prot,
 			unsigned long flags, unsigned long pgoff)
 {
@@ -1464,7 +1463,7 @@ get_unmapped_area(struct file *file, unsigned long addr, unsigned long len,
 EXPORT_SYMBOL(get_unmapped_area);
 
 /* Look up the first VMA which satisfies  addr < vm_end,  NULL if none. */
-struct vm_area_struct * find_vma(struct mm_struct * mm, unsigned long addr)
+struct vm_area_struct *find_vma(struct mm_struct *mm, unsigned long addr)
 {
 	struct vm_area_struct *vma = NULL;
 
@@ -1507,7 +1506,7 @@ find_vma_prev(struct mm_struct *mm, unsigned long addr,
 			struct vm_area_struct **pprev)
 {
 	struct vm_area_struct *vma = NULL, *prev = NULL;
-	struct rb_node * rb_node;
+	struct rb_node *rb_node;
 	if (!mm)
 		goto out;
 
@@ -1541,7 +1540,7 @@ out:
  * update accounting. This is shared with both the
  * grow-up and grow-down cases.
  */
-static int acct_stack_growth(struct vm_area_struct * vma, unsigned long size, unsigned long grow)
+static int acct_stack_growth(struct vm_area_struct *vma, unsigned long size, unsigned long grow)
 {
 	struct mm_struct *mm = vma->vm_mm;
 	struct rlimit *rlim = current->signal->rlim;
@@ -2091,6 +2090,9 @@ void exit_mmap(struct mm_struct *mm)
 	arch_exit_mmap(mm);
 	mmu_notifier_release(mm);
 
+	if (!mm->mmap)	/* Can happen if dup_mmap() received an OOM */
+		return;
+
 	if (mm->locked_vm) {
 		vma = mm->mmap;
 		while (vma) {
@@ -2103,7 +2105,7 @@ void exit_mmap(struct mm_struct *mm)
 	lru_add_drain();
 	flush_cache_mm(mm);
 	tlb = tlb_gather_mmu(mm, 1);
-	/* Don't update_hiwater_rss(mm) here, do_exit already did */
+	/* update_hiwater_rss(mm) here? but nobody should be looking */
 	/* Use -1 here to ensure all VMAs in the mm are unmapped */
 	end = unmap_vmas(&tlb, vma, 0, -1, &nr_accounted, NULL);
 	vm_unacct_memory(nr_accounted);
diff --git a/mm/mprotect.c b/mm/mprotect.c
index cfb4c4852062..d0f6e7ce09f1 100644
--- a/mm/mprotect.c
+++ b/mm/mprotect.c
@@ -22,6 +22,7 @@
 #include <linux/swap.h>
 #include <linux/swapops.h>
 #include <linux/mmu_notifier.h>
+#include <linux/migrate.h>
 #include <asm/uaccess.h>
 #include <asm/pgtable.h>
 #include <asm/cacheflush.h>
@@ -59,8 +60,7 @@ static void change_pte_range(struct mm_struct *mm, pmd_t *pmd,
 				ptent = pte_mkwrite(ptent);
 
 			ptep_modify_prot_commit(mm, addr, pte, ptent);
-#ifdef CONFIG_MIGRATION
-		} else if (!pte_file(oldpte)) {
+		} else if (PAGE_MIGRATION && !pte_file(oldpte)) {
 			swp_entry_t entry = pte_to_swp_entry(oldpte);
 
 			if (is_write_migration_entry(entry)) {
@@ -72,9 +72,7 @@ static void change_pte_range(struct mm_struct *mm, pmd_t *pmd,
 				set_pte_at(mm, addr, pte,
 					swp_entry_to_pte(entry));
 			}
-#endif
 		}
-
 	} while (pte++, addr += PAGE_SIZE, addr != end);
 	arch_leave_lazy_mmu_mode();
 	pte_unmap_unlock(pte - 1, ptl);
diff --git a/mm/oom_kill.c b/mm/oom_kill.c
index 558f9afe6e4e..6b9e758c98a5 100644
--- a/mm/oom_kill.c
+++ b/mm/oom_kill.c
@@ -31,7 +31,7 @@
 int sysctl_panic_on_oom;
 int sysctl_oom_kill_allocating_task;
 int sysctl_oom_dump_tasks;
-static DEFINE_SPINLOCK(zone_scan_mutex);
+static DEFINE_SPINLOCK(zone_scan_lock);
 /* #define DEBUG */
 
 /**
@@ -392,6 +392,9 @@ static int oom_kill_process(struct task_struct *p, gfp_t gfp_mask, int order,
 		printk(KERN_WARNING "%s invoked oom-killer: "
 			"gfp_mask=0x%x, order=%d, oomkilladj=%d\n",
 			current->comm, gfp_mask, order, current->oomkilladj);
+		task_lock(current);
+		cpuset_print_task_mems_allowed(current);
+		task_unlock(current);
 		dump_stack();
 		show_mem();
 		if (sysctl_oom_dump_tasks)
@@ -470,7 +473,7 @@ int try_set_zone_oom(struct zonelist *zonelist, gfp_t gfp_mask)
 	struct zone *zone;
 	int ret = 1;
 
-	spin_lock(&zone_scan_mutex);
+	spin_lock(&zone_scan_lock);
 	for_each_zone_zonelist(zone, z, zonelist, gfp_zone(gfp_mask)) {
 		if (zone_is_oom_locked(zone)) {
 			ret = 0;
@@ -480,7 +483,7 @@ int try_set_zone_oom(struct zonelist *zonelist, gfp_t gfp_mask)
 
 	for_each_zone_zonelist(zone, z, zonelist, gfp_zone(gfp_mask)) {
 		/*
-		 * Lock each zone in the zonelist under zone_scan_mutex so a
+		 * Lock each zone in the zonelist under zone_scan_lock so a
 		 * parallel invocation of try_set_zone_oom() doesn't succeed
 		 * when it shouldn't.
 		 */
@@ -488,7 +491,7 @@ int try_set_zone_oom(struct zonelist *zonelist, gfp_t gfp_mask)
 	}
 
 out:
-	spin_unlock(&zone_scan_mutex);
+	spin_unlock(&zone_scan_lock);
 	return ret;
 }
 
@@ -502,11 +505,74 @@ void clear_zonelist_oom(struct zonelist *zonelist, gfp_t gfp_mask)
 	struct zoneref *z;
 	struct zone *zone;
 
-	spin_lock(&zone_scan_mutex);
+	spin_lock(&zone_scan_lock);
 	for_each_zone_zonelist(zone, z, zonelist, gfp_zone(gfp_mask)) {
 		zone_clear_flag(zone, ZONE_OOM_LOCKED);
 	}
-	spin_unlock(&zone_scan_mutex);
+	spin_unlock(&zone_scan_lock);
+}
+
+/*
+ * Must be called with tasklist_lock held for read.
+ */
+static void __out_of_memory(gfp_t gfp_mask, int order)
+{
+	if (sysctl_oom_kill_allocating_task) {
+		oom_kill_process(current, gfp_mask, order, 0, NULL,
+				"Out of memory (oom_kill_allocating_task)");
+
+	} else {
+		unsigned long points;
+		struct task_struct *p;
+
+retry:
+		/*
+		 * Rambo mode: Shoot down a process and hope it solves whatever
+		 * issues we may have.
+		 */
+		p = select_bad_process(&points, NULL);
+
+		if (PTR_ERR(p) == -1UL)
+			return;
+
+		/* Found nothing?!?! Either we hang forever, or we panic. */
+		if (!p) {
+			read_unlock(&tasklist_lock);
+			panic("Out of memory and no killable processes...\n");
+		}
+
+		if (oom_kill_process(p, gfp_mask, order, points, NULL,
+				     "Out of memory"))
+			goto retry;
+	}
+}
+
+/*
+ * pagefault handler calls into here because it is out of memory but
+ * doesn't know exactly how or why.
+ */
+void pagefault_out_of_memory(void)
+{
+	unsigned long freed = 0;
+
+	blocking_notifier_call_chain(&oom_notify_list, 0, &freed);
+	if (freed > 0)
+		/* Got some memory back in the last second. */
+		return;
+
+	if (sysctl_panic_on_oom)
+		panic("out of memory from page fault. panic_on_oom is selected.\n");
+
+	read_lock(&tasklist_lock);
+	__out_of_memory(0, 0); /* unknown gfp_mask and order */
+	read_unlock(&tasklist_lock);
+
+	/*
+	 * Give "p" a good chance of killing itself before we
+	 * retry to allocate memory.
+	 */
+	if (!test_thread_flag(TIF_MEMDIE))
+		schedule_timeout_uninterruptible(1);
 }
 
 /**
@@ -522,8 +588,6 @@ void clear_zonelist_oom(struct zonelist *zonelist, gfp_t gfp_mask)
  */
 void out_of_memory(struct zonelist *zonelist, gfp_t gfp_mask, int order)
 {
-	struct task_struct *p;
-	unsigned long points = 0;
 	unsigned long freed = 0;
 	enum oom_constraint constraint;
 
@@ -544,7 +608,7 @@ void out_of_memory(struct zonelist *zonelist, gfp_t gfp_mask, int order)
 
 	switch (constraint) {
 	case CONSTRAINT_MEMORY_POLICY:
-		oom_kill_process(current, gfp_mask, order, points, NULL,
+		oom_kill_process(current, gfp_mask, order, 0, NULL,
 				"No available memory (MPOL_BIND)");
 		break;
 
@@ -553,35 +617,10 @@ void out_of_memory(struct zonelist *zonelist, gfp_t gfp_mask, int order)
 			panic("out of memory. panic_on_oom is selected\n");
 		/* Fall-through */
 	case CONSTRAINT_CPUSET:
-		if (sysctl_oom_kill_allocating_task) {
-			oom_kill_process(current, gfp_mask, order, points, NULL,
-					"Out of memory (oom_kill_allocating_task)");
-			break;
-		}
-retry:
-		/*
-		 * Rambo mode: Shoot down a process and hope it solves whatever
-		 * issues we may have.
-		 */
-		p = select_bad_process(&points, NULL);
-
-		if (PTR_ERR(p) == -1UL)
-			goto out;
-
-		/* Found nothing?!?! Either we hang forever, or we panic. */
-		if (!p) {
-			read_unlock(&tasklist_lock);
-			panic("Out of memory and no killable processes...\n");
-		}
-
-		if (oom_kill_process(p, gfp_mask, order, points, NULL,
-				     "Out of memory"))
-			goto retry;
-
+		__out_of_memory(gfp_mask, order);
 		break;
 	}
 
-out:
 	read_unlock(&tasklist_lock);
 
 	/*
diff --git a/mm/page-writeback.c b/mm/page-writeback.c
index 2970e35fd03f..b493db7841dc 100644
--- a/mm/page-writeback.c
+++ b/mm/page-writeback.c
@@ -69,6 +69,12 @@ static inline long sync_writeback_pages(void)
 int dirty_background_ratio = 5;
 
 /*
+ * dirty_background_bytes starts at 0 (disabled) so that it is a function of
+ * dirty_background_ratio * the amount of dirtyable memory
+ */
+unsigned long dirty_background_bytes;
+
+/*
  * free highmem will not be subtracted from the total free memory
  * for calculating free ratios if vm_highmem_is_dirtyable is true
  */
@@ -80,6 +86,12 @@ int vm_highmem_is_dirtyable;
 int vm_dirty_ratio = 10;
 
 /*
+ * vm_dirty_bytes starts at 0 (disabled) so that it is a function of
+ * vm_dirty_ratio * the amount of dirtyable memory
+ */
+unsigned long vm_dirty_bytes;
+
+/*
  * The interval between `kupdate'-style writebacks, in jiffies
  */
 int dirty_writeback_interval = 5 * HZ;
@@ -135,23 +147,75 @@ static int calc_period_shift(void)
 {
 	unsigned long dirty_total;
 
-	dirty_total = (vm_dirty_ratio * determine_dirtyable_memory()) / 100;
+	if (vm_dirty_bytes)
+		dirty_total = vm_dirty_bytes / PAGE_SIZE;
+	else
+		dirty_total = (vm_dirty_ratio * determine_dirtyable_memory()) /
+				100;
 	return 2 + ilog2(dirty_total - 1);
 }
 
 /*
- * update the period when the dirty ratio changes.
+ * update the period when the dirty threshold changes.
  */
+static void update_completion_period(void)
+{
+	int shift = calc_period_shift();
+	prop_change_shift(&vm_completions, shift);
+	prop_change_shift(&vm_dirties, shift);
+}
+
+int dirty_background_ratio_handler(struct ctl_table *table, int write,
+		struct file *filp, void __user *buffer, size_t *lenp,
+		loff_t *ppos)
+{
+	int ret;
+
+	ret = proc_dointvec_minmax(table, write, filp, buffer, lenp, ppos);
+	if (ret == 0 && write)
+		dirty_background_bytes = 0;
+	return ret;
+}
+
+int dirty_background_bytes_handler(struct ctl_table *table, int write,
+		struct file *filp, void __user *buffer, size_t *lenp,
+		loff_t *ppos)
+{
+	int ret;
+
+	ret = proc_doulongvec_minmax(table, write, filp, buffer, lenp, ppos);
+	if (ret == 0 && write)
+		dirty_background_ratio = 0;
+	return ret;
+}
+
 int dirty_ratio_handler(struct ctl_table *table, int write,
 		struct file *filp, void __user *buffer, size_t *lenp,
 		loff_t *ppos)
 {
 	int old_ratio = vm_dirty_ratio;
-	int ret = proc_dointvec_minmax(table, write, filp, buffer, lenp, ppos);
+	int ret;
+
+	ret = proc_dointvec_minmax(table, write, filp, buffer, lenp, ppos);
 	if (ret == 0 && write && vm_dirty_ratio != old_ratio) {
-		int shift = calc_period_shift();
-		prop_change_shift(&vm_completions, shift);
-		prop_change_shift(&vm_dirties, shift);
+		update_completion_period();
+		vm_dirty_bytes = 0;
+	}
+	return ret;
+}
+
+
+int dirty_bytes_handler(struct ctl_table *table, int write,
+		struct file *filp, void __user *buffer, size_t *lenp,
+		loff_t *ppos)
+{
+	int old_bytes = vm_dirty_bytes;
+	int ret;
+
+	ret = proc_doulongvec_minmax(table, write, filp, buffer, lenp, ppos);
+	if (ret == 0 && write && vm_dirty_bytes != old_bytes) {
+		update_completion_period();
+		vm_dirty_ratio = 0;
 	}
 	return ret;
 }
@@ -362,26 +426,32 @@ unsigned long determine_dirtyable_memory(void)
 }
 
 void
-get_dirty_limits(long *pbackground, long *pdirty, long *pbdi_dirty,
-		 struct backing_dev_info *bdi)
+get_dirty_limits(unsigned long *pbackground, unsigned long *pdirty,
+		 unsigned long *pbdi_dirty, struct backing_dev_info *bdi)
 {
-	int background_ratio;		/* Percentages */
-	int dirty_ratio;
-	long background;
-	long dirty;
+	unsigned long background;
+	unsigned long dirty;
 	unsigned long available_memory = determine_dirtyable_memory();
 	struct task_struct *tsk;
 
-	dirty_ratio = vm_dirty_ratio;
-	if (dirty_ratio < 5)
-		dirty_ratio = 5;
+	if (vm_dirty_bytes)
+		dirty = DIV_ROUND_UP(vm_dirty_bytes, PAGE_SIZE);
+	else {
+		int dirty_ratio;
 
-	background_ratio = dirty_background_ratio;
-	if (background_ratio >= dirty_ratio)
-		background_ratio = dirty_ratio / 2;
+		dirty_ratio = vm_dirty_ratio;
+		if (dirty_ratio < 5)
+			dirty_ratio = 5;
+		dirty = (dirty_ratio * available_memory) / 100;
+	}
+
+	if (dirty_background_bytes)
+		background = DIV_ROUND_UP(dirty_background_bytes, PAGE_SIZE);
+	else
+		background = (dirty_background_ratio * available_memory) / 100;
 
-	background = (background_ratio * available_memory) / 100;
-	dirty = (dirty_ratio * available_memory) / 100;
+	if (background >= dirty)
+		background = dirty / 2;
 	tsk = current;
 	if (tsk->flags & PF_LESS_THROTTLE || rt_task(tsk)) {
 		background += background / 4;
@@ -423,9 +493,9 @@ static void balance_dirty_pages(struct address_space *mapping)
 {
 	long nr_reclaimable, bdi_nr_reclaimable;
 	long nr_writeback, bdi_nr_writeback;
-	long background_thresh;
-	long dirty_thresh;
-	long bdi_thresh;
+	unsigned long background_thresh;
+	unsigned long dirty_thresh;
+	unsigned long bdi_thresh;
 	unsigned long pages_written = 0;
 	unsigned long write_chunk = sync_writeback_pages();
 
@@ -580,8 +650,8 @@ EXPORT_SYMBOL(balance_dirty_pages_ratelimited_nr);
 
 void throttle_vm_writeout(gfp_t gfp_mask)
 {
-	long background_thresh;
-	long dirty_thresh;
+	unsigned long background_thresh;
+	unsigned long dirty_thresh;
 
         for ( ; ; ) {
 		get_dirty_limits(&background_thresh, &dirty_thresh, NULL, NULL);
@@ -624,8 +694,8 @@ static void background_writeout(unsigned long _min_pages)
 	};
 
 	for ( ; ; ) {
-		long background_thresh;
-		long dirty_thresh;
+		unsigned long background_thresh;
+		unsigned long dirty_thresh;
 
 		get_dirty_limits(&background_thresh, &dirty_thresh, NULL, NULL);
 		if (global_page_state(NR_FILE_DIRTY) +
@@ -868,9 +938,11 @@ int write_cache_pages(struct address_space *mapping,
 	int done = 0;
 	struct pagevec pvec;
 	int nr_pages;
+	pgoff_t uninitialized_var(writeback_index);
 	pgoff_t index;
 	pgoff_t end;		/* Inclusive */
-	int scanned = 0;
+	pgoff_t done_index;
+	int cycled;
 	int range_whole = 0;
 	long nr_to_write = wbc->nr_to_write;
 
@@ -881,83 +953,134 @@ int write_cache_pages(struct address_space *mapping,
 
 	pagevec_init(&pvec, 0);
 	if (wbc->range_cyclic) {
-		index = mapping->writeback_index; /* Start from prev offset */
+		writeback_index = mapping->writeback_index; /* prev offset */
+		index = writeback_index;
+		if (index == 0)
+			cycled = 1;
+		else
+			cycled = 0;
 		end = -1;
 	} else {
 		index = wbc->range_start >> PAGE_CACHE_SHIFT;
 		end = wbc->range_end >> PAGE_CACHE_SHIFT;
 		if (wbc->range_start == 0 && wbc->range_end == LLONG_MAX)
 			range_whole = 1;
-		scanned = 1;
+		cycled = 1; /* ignore range_cyclic tests */
 	}
 retry:
-	while (!done && (index <= end) &&
-	       (nr_pages = pagevec_lookup_tag(&pvec, mapping, &index,
-					      PAGECACHE_TAG_DIRTY,
-					      min(end - index, (pgoff_t)PAGEVEC_SIZE-1) + 1))) {
-		unsigned i;
+	done_index = index;
+	while (!done && (index <= end)) {
+		int i;
+
+		nr_pages = pagevec_lookup_tag(&pvec, mapping, &index,
+			      PAGECACHE_TAG_DIRTY,
+			      min(end - index, (pgoff_t)PAGEVEC_SIZE-1) + 1);
+		if (nr_pages == 0)
+			break;
 
-		scanned = 1;
 		for (i = 0; i < nr_pages; i++) {
 			struct page *page = pvec.pages[i];
 
 			/*
-			 * At this point we hold neither mapping->tree_lock nor
-			 * lock on the page itself: the page may be truncated or
-			 * invalidated (changing page->mapping to NULL), or even
-			 * swizzled back from swapper_space to tmpfs file
-			 * mapping
+			 * At this point, the page may be truncated or
+			 * invalidated (changing page->mapping to NULL), or
+			 * even swizzled back from swapper_space to tmpfs file
+			 * mapping. However, page->index will not change
+			 * because we have a reference on the page.
 			 */
+			if (page->index > end) {
+				/*
+				 * can't be range_cyclic (1st pass) because
+				 * end == -1 in that case.
+				 */
+				done = 1;
+				break;
+			}
+
+			done_index = page->index + 1;
+
 			lock_page(page);
 
+			/*
+			 * Page truncated or invalidated. We can freely skip it
+			 * then, even for data integrity operations: the page
+			 * has disappeared concurrently, so there could be no
+			 * real expectation of this data interity operation
+			 * even if there is now a new, dirty page at the same
+			 * pagecache address.
+			 */
 			if (unlikely(page->mapping != mapping)) {
+continue_unlock:
 				unlock_page(page);
 				continue;
 			}
 
-			if (!wbc->range_cyclic && page->index > end) {
-				done = 1;
-				unlock_page(page);
-				continue;
+			if (!PageDirty(page)) {
+				/* someone wrote it for us */
+				goto continue_unlock;
 			}
 
-			if (wbc->sync_mode != WB_SYNC_NONE)
-				wait_on_page_writeback(page);
-
-			if (PageWriteback(page) ||
-			    !clear_page_dirty_for_io(page)) {
-				unlock_page(page);
-				continue;
+			if (PageWriteback(page)) {
+				if (wbc->sync_mode != WB_SYNC_NONE)
+					wait_on_page_writeback(page);
+				else
+					goto continue_unlock;
 			}
 
-			ret = (*writepage)(page, wbc, data);
+			BUG_ON(PageWriteback(page));
+			if (!clear_page_dirty_for_io(page))
+				goto continue_unlock;
 
-			if (unlikely(ret == AOP_WRITEPAGE_ACTIVATE)) {
-				unlock_page(page);
-				ret = 0;
+			ret = (*writepage)(page, wbc, data);
+			if (unlikely(ret)) {
+				if (ret == AOP_WRITEPAGE_ACTIVATE) {
+					unlock_page(page);
+					ret = 0;
+				} else {
+					/*
+					 * done_index is set past this page,
+					 * so media errors will not choke
+					 * background writeout for the entire
+					 * file. This has consequences for
+					 * range_cyclic semantics (ie. it may
+					 * not be suitable for data integrity
+					 * writeout).
+					 */
+					done = 1;
+					break;
+				}
+ 			}
+
+			if (wbc->sync_mode == WB_SYNC_NONE) {
+				wbc->nr_to_write--;
+				if (wbc->nr_to_write <= 0) {
+					done = 1;
+					break;
+				}
 			}
-			if (ret || (--nr_to_write <= 0))
-				done = 1;
 			if (wbc->nonblocking && bdi_write_congested(bdi)) {
 				wbc->encountered_congestion = 1;
 				done = 1;
+				break;
 			}
 		}
 		pagevec_release(&pvec);
 		cond_resched();
 	}
-	if (!scanned && !done) {
+	if (!cycled) {
 		/*
+		 * range_cyclic:
 		 * We hit the last page and there is more work to be done: wrap
 		 * back to the start of the file
 		 */
-		scanned = 1;
+		cycled = 1;
 		index = 0;
+		end = writeback_index - 1;
 		goto retry;
 	}
 	if (!wbc->no_nrwrite_index_update) {
 		if (wbc->range_cyclic || (range_whole && nr_to_write > 0))
-			mapping->writeback_index = index;
+			mapping->writeback_index = done_index;
 		wbc->nr_to_write = nr_to_write;
 	}
 
diff --git a/mm/page_alloc.c b/mm/page_alloc.c
index d8ac01474563..7bf22e045318 100644
--- a/mm/page_alloc.c
+++ b/mm/page_alloc.c
@@ -69,7 +69,7 @@ EXPORT_SYMBOL(node_states);
 
 unsigned long totalram_pages __read_mostly;
 unsigned long totalreserve_pages __read_mostly;
-long nr_swap_pages;
+unsigned long highest_memmap_pfn __read_mostly;
 int percpu_pagelist_fraction;
 
 #ifdef CONFIG_HUGETLB_PAGE_SIZE_VARIABLE
@@ -223,19 +223,41 @@ static inline int bad_range(struct zone *zone, struct page *page)
 
 static void bad_page(struct page *page)
 {
-	printk(KERN_EMERG "Bad page state in process '%s'\n" KERN_EMERG
-		"page:%p flags:0x%0*lx mapping:%p mapcount:%d count:%d\n",
-		current->comm, page, (int)(2*sizeof(unsigned long)),
-		(unsigned long)page->flags, page->mapping,
-		page_mapcount(page), page_count(page));
+	static unsigned long resume;
+	static unsigned long nr_shown;
+	static unsigned long nr_unshown;
+
+	/*
+	 * Allow a burst of 60 reports, then keep quiet for that minute;
+	 * or allow a steady drip of one report per second.
+	 */
+	if (nr_shown == 60) {
+		if (time_before(jiffies, resume)) {
+			nr_unshown++;
+			goto out;
+		}
+		if (nr_unshown) {
+			printk(KERN_ALERT
+			      "BUG: Bad page state: %lu messages suppressed\n",
+				nr_unshown);
+			nr_unshown = 0;
+		}
+		nr_shown = 0;
+	}
+	if (nr_shown++ == 0)
+		resume = jiffies + 60 * HZ;
+
+	printk(KERN_ALERT "BUG: Bad page state in process %s  pfn:%05lx\n",
+		current->comm, page_to_pfn(page));
+	printk(KERN_ALERT
+		"page:%p flags:%p count:%d mapcount:%d mapping:%p index:%lx\n",
+		page, (void *)page->flags, page_count(page),
+		page_mapcount(page), page->mapping, page->index);
 
-	printk(KERN_EMERG "Trying to fix it up, but a reboot is needed\n"
-		KERN_EMERG "Backtrace:\n");
 	dump_stack();
-	page->flags &= ~PAGE_FLAGS_CLEAR_WHEN_BAD;
-	set_page_count(page, 0);
-	reset_page_mapcount(page);
-	page->mapping = NULL;
+out:
+	/* Leave bad fields for debug, except PageBuddy could make trouble */
+	__ClearPageBuddy(page);
 	add_taint(TAINT_BAD_PAGE);
 }
 
@@ -292,25 +314,31 @@ void prep_compound_gigantic_page(struct page *page, unsigned long order)
 }
 #endif
 
-static void destroy_compound_page(struct page *page, unsigned long order)
+static int destroy_compound_page(struct page *page, unsigned long order)
 {
 	int i;
 	int nr_pages = 1 << order;
+	int bad = 0;
 
-	if (unlikely(compound_order(page) != order))
+	if (unlikely(compound_order(page) != order) ||
+	    unlikely(!PageHead(page))) {
 		bad_page(page);
+		bad++;
+	}
 
-	if (unlikely(!PageHead(page)))
-			bad_page(page);
 	__ClearPageHead(page);
+
 	for (i = 1; i < nr_pages; i++) {
 		struct page *p = page + i;
 
-		if (unlikely(!PageTail(p) |
-				(p->first_page != page)))
+		if (unlikely(!PageTail(p) | (p->first_page != page))) {
 			bad_page(page);
+			bad++;
+		}
 		__ClearPageTail(p);
 	}
+
+	return bad;
 }
 
 static inline void prep_zero_page(struct page *page, int order, gfp_t gfp_flags)
@@ -430,7 +458,8 @@ static inline void __free_one_page(struct page *page,
 	int migratetype = get_pageblock_migratetype(page);
 
 	if (unlikely(PageCompound(page)))
-		destroy_compound_page(page, order);
+		if (unlikely(destroy_compound_page(page, order)))
+			return;
 
 	page_idx = page_to_pfn(page) & ((1 << MAX_ORDER) - 1);
 
@@ -467,18 +496,13 @@ static inline int free_pages_check(struct page *page)
 	if (unlikely(page_mapcount(page) |
 		(page->mapping != NULL)  |
 		(page_count(page) != 0)  |
-		(page->flags & PAGE_FLAGS_CHECK_AT_FREE)))
+		(page->flags & PAGE_FLAGS_CHECK_AT_FREE))) {
 		bad_page(page);
-	if (PageDirty(page))
-		__ClearPageDirty(page);
-	if (PageSwapBacked(page))
-		__ClearPageSwapBacked(page);
-	/*
-	 * For now, we report if PG_reserved was found set, but do not
-	 * clear it, and do not free the page.  But we shall soon need
-	 * to do more, for when the ZERO_PAGE count wraps negative.
-	 */
-	return PageReserved(page);
+		return 1;
+	}
+	if (page->flags & PAGE_FLAGS_CHECK_AT_PREP)
+		page->flags &= ~PAGE_FLAGS_CHECK_AT_PREP;
+	return 0;
 }
 
 /*
@@ -523,11 +547,11 @@ static void __free_pages_ok(struct page *page, unsigned int order)
 {
 	unsigned long flags;
 	int i;
-	int reserved = 0;
+	int bad = 0;
 
 	for (i = 0 ; i < (1 << order) ; ++i)
-		reserved += free_pages_check(page + i);
-	if (reserved)
+		bad += free_pages_check(page + i);
+	if (bad)
 		return;
 
 	if (!PageHighMem(page)) {
@@ -612,23 +636,11 @@ static int prep_new_page(struct page *page, int order, gfp_t gfp_flags)
 	if (unlikely(page_mapcount(page) |
 		(page->mapping != NULL)  |
 		(page_count(page) != 0)  |
-		(page->flags & PAGE_FLAGS_CHECK_AT_PREP)))
+		(page->flags & PAGE_FLAGS_CHECK_AT_PREP))) {
 		bad_page(page);
-
-	/*
-	 * For now, we report if PG_reserved was found set, but do not
-	 * clear it, and do not allocate the page: as a safety net.
-	 */
-	if (PageReserved(page))
 		return 1;
+	}
 
-	page->flags &= ~(1 << PG_uptodate | 1 << PG_error | 1 << PG_reclaim |
-			1 << PG_referenced | 1 << PG_arch_1 |
-			1 << PG_owner_priv_1 | 1 << PG_mappedtodisk
-#ifdef CONFIG_UNEVICTABLE_LRU
-			| 1 << PG_mlocked
-#endif
-			);
 	set_page_private(page, 0);
 	set_page_refcounted(page);
 
@@ -2609,6 +2621,9 @@ void __meminit memmap_init_zone(unsigned long size, int nid, unsigned long zone,
 	unsigned long pfn;
 	struct zone *z;
 
+	if (highest_memmap_pfn < end_pfn - 1)
+		highest_memmap_pfn = end_pfn - 1;
+
 	z = &NODE_DATA(nid)->node_zones[zone];
 	for (pfn = start_pfn; pfn < end_pfn; pfn++) {
 		/*
@@ -3381,10 +3396,8 @@ static void __init setup_usemap(struct pglist_data *pgdat,
 {
 	unsigned long usemapsize = usemap_size(zonesize);
 	zone->pageblock_flags = NULL;
-	if (usemapsize) {
+	if (usemapsize)
 		zone->pageblock_flags = alloc_bootmem_node(pgdat, usemapsize);
-		memset(zone->pageblock_flags, 0, usemapsize);
-	}
 }
 #else
 static void inline setup_usemap(struct pglist_data *pgdat,
@@ -3469,9 +3482,10 @@ static void __paginginit free_area_init_core(struct pglist_data *pgdat,
 			PAGE_ALIGN(size * sizeof(struct page)) >> PAGE_SHIFT;
 		if (realsize >= memmap_pages) {
 			realsize -= memmap_pages;
-			printk(KERN_DEBUG
-				"  %s zone: %lu pages used for memmap\n",
-				zone_names[j], memmap_pages);
+			if (memmap_pages)
+				printk(KERN_DEBUG
+				       "  %s zone: %lu pages used for memmap\n",
+				       zone_names[j], memmap_pages);
 		} else
 			printk(KERN_WARNING
 				"  %s zone: %lu pages exceeds realsize %lu\n",
@@ -4316,7 +4330,7 @@ void setup_per_zone_pages_min(void)
  *    1TB     101        10GB
  *   10TB     320        32GB
  */
-void setup_per_zone_inactive_ratio(void)
+static void setup_per_zone_inactive_ratio(void)
 {
 	struct zone *zone;
 
@@ -4573,19 +4587,6 @@ void *__init alloc_large_system_hash(const char *tablename,
 	return table;
 }
 
-#ifdef CONFIG_OUT_OF_LINE_PFN_TO_PAGE
-struct page *pfn_to_page(unsigned long pfn)
-{
-	return __pfn_to_page(pfn);
-}
-unsigned long page_to_pfn(struct page *page)
-{
-	return __page_to_pfn(page);
-}
-EXPORT_SYMBOL(pfn_to_page);
-EXPORT_SYMBOL(page_to_pfn);
-#endif /* CONFIG_OUT_OF_LINE_PFN_TO_PAGE */
-
 /* Return a pointer to the bitmap storing bits affecting a block of pages */
 static inline unsigned long *get_pageblock_bitmap(struct zone *zone,
 							unsigned long pfn)
diff --git a/mm/page_cgroup.c b/mm/page_cgroup.c
index ab27ff750519..d6507a660ed6 100644
--- a/mm/page_cgroup.c
+++ b/mm/page_cgroup.c
@@ -101,7 +101,7 @@ struct page_cgroup *lookup_page_cgroup(struct page *page)
 }
 
 /* __alloc_bootmem...() is protected by !slab_available() */
-int __init_refok init_section_page_cgroup(unsigned long pfn)
+static int __init_refok init_section_page_cgroup(unsigned long pfn)
 {
 	struct mem_section *section;
 	struct page_cgroup *base, *pc;
diff --git a/mm/page_io.c b/mm/page_io.c
index 065c4480eaf0..dc6ce0afbded 100644
--- a/mm/page_io.c
+++ b/mm/page_io.c
@@ -98,7 +98,7 @@ int swap_writepage(struct page *page, struct writeback_control *wbc)
 	struct bio *bio;
 	int ret = 0, rw = WRITE;
 
-	if (remove_exclusive_swap_page(page)) {
+	if (try_to_free_swap(page)) {
 		unlock_page(page);
 		goto out;
 	}
@@ -125,8 +125,8 @@ int swap_readpage(struct file *file, struct page *page)
 	struct bio *bio;
 	int ret = 0;
 
-	BUG_ON(!PageLocked(page));
-	BUG_ON(PageUptodate(page));
+	VM_BUG_ON(!PageLocked(page));
+	VM_BUG_ON(PageUptodate(page));
 	bio = get_swap_bio(GFP_KERNEL, page_private(page), page,
 				end_swap_bio_read);
 	if (bio == NULL) {
diff --git a/mm/rmap.c b/mm/rmap.c
index 10993942d6c9..ac4af8cffbf9 100644
--- a/mm/rmap.c
+++ b/mm/rmap.c
@@ -47,9 +47,9 @@
 #include <linux/rmap.h>
 #include <linux/rcupdate.h>
 #include <linux/module.h>
-#include <linux/kallsyms.h>
 #include <linux/memcontrol.h>
 #include <linux/mmu_notifier.h>
+#include <linux/migrate.h>
 
 #include <asm/tlbflush.h>
 
@@ -191,7 +191,7 @@ void __init anon_vma_init(void)
  * Getting a lock on a stable anon_vma from a page off the LRU is
  * tricky: page_lock_anon_vma rely on RCU to guard against the races.
  */
-struct anon_vma *page_lock_anon_vma(struct page *page)
+static struct anon_vma *page_lock_anon_vma(struct page *page)
 {
 	struct anon_vma *anon_vma;
 	unsigned long anon_mapping;
@@ -211,7 +211,7 @@ out:
 	return NULL;
 }
 
-void page_unlock_anon_vma(struct anon_vma *anon_vma)
+static void page_unlock_anon_vma(struct anon_vma *anon_vma)
 {
 	spin_unlock(&anon_vma->lock);
 	rcu_read_unlock();
@@ -359,8 +359,17 @@ static int page_referenced_one(struct page *page,
 		goto out_unmap;
 	}
 
-	if (ptep_clear_flush_young_notify(vma, address, pte))
-		referenced++;
+	if (ptep_clear_flush_young_notify(vma, address, pte)) {
+		/*
+		 * Don't treat a reference through a sequentially read
+		 * mapping as such.  If the page has been used in
+		 * another mapping, we will catch it; if this other
+		 * mapping is already gone, the unmap path will have
+		 * set PG_referenced or activated the page.
+		 */
+		if (likely(!VM_SequentialReadHint(vma)))
+			referenced++;
+	}
 
 	/* Pretend the page is referenced if the task has the
 	   swap token and is in the middle of a page fault. */
@@ -661,9 +670,14 @@ void page_add_anon_rmap(struct page *page,
 void page_add_new_anon_rmap(struct page *page,
 	struct vm_area_struct *vma, unsigned long address)
 {
-	BUG_ON(address < vma->vm_start || address >= vma->vm_end);
-	atomic_set(&page->_mapcount, 0); /* elevate count by 1 (starts at -1) */
+	VM_BUG_ON(address < vma->vm_start || address >= vma->vm_end);
+	SetPageSwapBacked(page);
+	atomic_set(&page->_mapcount, 0); /* increment count (starts at -1) */
 	__page_set_anon_rmap(page, vma, address);
+	if (page_evictable(page, vma))
+		lru_cache_add_lru(page, LRU_ACTIVE_ANON);
+	else
+		add_page_to_unevictable_list(page);
 }
 
 /**
@@ -693,7 +707,6 @@ void page_add_file_rmap(struct page *page)
  */
 void page_dup_rmap(struct page *page, struct vm_area_struct *vma, unsigned long address)
 {
-	BUG_ON(page_mapcount(page) == 0);
 	if (PageAnon(page))
 		__page_check_anon_rmap(page, vma, address);
 	atomic_inc(&page->_mapcount);
@@ -703,28 +716,12 @@ void page_dup_rmap(struct page *page, struct vm_area_struct *vma, unsigned long
 /**
  * page_remove_rmap - take down pte mapping from a page
  * @page: page to remove mapping from
- * @vma: the vm area in which the mapping is removed
  *
  * The caller needs to hold the pte lock.
  */
-void page_remove_rmap(struct page *page, struct vm_area_struct *vma)
+void page_remove_rmap(struct page *page)
 {
 	if (atomic_add_negative(-1, &page->_mapcount)) {
-		if (unlikely(page_mapcount(page) < 0)) {
-			printk (KERN_EMERG "Eeek! page_mapcount(page) went negative! (%d)\n", page_mapcount(page));
-			printk (KERN_EMERG "  page pfn = %lx\n", page_to_pfn(page));
-			printk (KERN_EMERG "  page->flags = %lx\n", page->flags);
-			printk (KERN_EMERG "  page->count = %x\n", page_count(page));
-			printk (KERN_EMERG "  page->mapping = %p\n", page->mapping);
-			print_symbol (KERN_EMERG "  vma->vm_ops = %s\n", (unsigned long)vma->vm_ops);
-			if (vma->vm_ops) {
-				print_symbol (KERN_EMERG "  vma->vm_ops->fault = %s\n", (unsigned long)vma->vm_ops->fault);
-			}
-			if (vma->vm_file && vma->vm_file->f_op)
-				print_symbol (KERN_EMERG "  vma->vm_file->f_op->mmap = %s\n", (unsigned long)vma->vm_file->f_op->mmap);
-			BUG();
-		}
-
 		/*
 		 * Now that the last pte has gone, s390 must transfer dirty
 		 * flag from storage key to struct page.  We can usually skip
@@ -818,8 +815,7 @@ static int try_to_unmap_one(struct page *page, struct vm_area_struct *vma,
 				spin_unlock(&mmlist_lock);
 			}
 			dec_mm_counter(mm, anon_rss);
-#ifdef CONFIG_MIGRATION
-		} else {
+		} else if (PAGE_MIGRATION) {
 			/*
 			 * Store the pfn of the page in a special migration
 			 * pte. do_swap_page() will wait until the migration
@@ -827,23 +823,19 @@ static int try_to_unmap_one(struct page *page, struct vm_area_struct *vma,
 			 */
 			BUG_ON(!migration);
 			entry = make_migration_entry(page, pte_write(pteval));
-#endif
 		}
 		set_pte_at(mm, address, pte, swp_entry_to_pte(entry));
 		BUG_ON(pte_file(*pte));
-	} else
-#ifdef CONFIG_MIGRATION
-	if (migration) {
+	} else if (PAGE_MIGRATION && migration) {
 		/* Establish migration entry for a file page */
 		swp_entry_t entry;
 		entry = make_migration_entry(page, pte_write(pteval));
 		set_pte_at(mm, address, pte, swp_entry_to_pte(entry));
 	} else
-#endif
 		dec_mm_counter(mm, file_rss);
 
 
-	page_remove_rmap(page, vma);
+	page_remove_rmap(page);
 	page_cache_release(page);
 
 out_unmap:
@@ -958,7 +950,7 @@ static int try_to_unmap_cluster(unsigned long cursor, unsigned int *mapcount,
 		if (pte_dirty(pteval))
 			set_page_dirty(page);
 
-		page_remove_rmap(page, vma);
+		page_remove_rmap(page);
 		page_cache_release(page);
 		dec_mm_counter(mm, file_rss);
 		(*mapcount)--;
diff --git a/mm/shmem.c b/mm/shmem.c
index f1b0d4871f3a..5941f9801363 100644
--- a/mm/shmem.c
+++ b/mm/shmem.c
@@ -14,31 +14,39 @@
  * Copyright (c) 2004, Luke Kenneth Casson Leighton <lkcl@lkcl.net>
  * Copyright (c) 2004 Red Hat, Inc., James Morris <jmorris@redhat.com>
  *
+ * tiny-shmem:
+ * Copyright (c) 2004, 2008 Matt Mackall <mpm@selenic.com>
+ *
  * This file is released under the GPL.
  */
 
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/vfs.h>
+#include <linux/mount.h>
+#include <linux/file.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/swap.h>
+
+static struct vfsmount *shm_mnt;
+
+#ifdef CONFIG_SHMEM
 /*
  * This virtual memory filesystem is heavily based on the ramfs. It
  * extends ramfs by the ability to use swap and honor resource limits
  * which makes it a completely usable filesystem.
  */
 
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/fs.h>
 #include <linux/xattr.h>
 #include <linux/exportfs.h>
 #include <linux/generic_acl.h>
-#include <linux/mm.h>
 #include <linux/mman.h>
-#include <linux/file.h>
-#include <linux/swap.h>
 #include <linux/pagemap.h>
 #include <linux/string.h>
 #include <linux/slab.h>
 #include <linux/backing-dev.h>
 #include <linux/shmem_fs.h>
-#include <linux/mount.h>
 #include <linux/writeback.h>
 #include <linux/vfs.h>
 #include <linux/blkdev.h>
@@ -1444,7 +1452,6 @@ static int shmem_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 	if (error)
 		return ((error == -ENOMEM) ? VM_FAULT_OOM : VM_FAULT_SIGBUS);
 
-	mark_page_accessed(vmf->page);
 	return ret | VM_FAULT_LOCKED;
 }
 
@@ -2486,7 +2493,6 @@ static struct file_system_type tmpfs_fs_type = {
 	.get_sb		= shmem_get_sb,
 	.kill_sb	= kill_litter_super,
 };
-static struct vfsmount *shm_mnt;
 
 static int __init init_tmpfs(void)
 {
@@ -2525,7 +2531,51 @@ out4:
 	shm_mnt = ERR_PTR(error);
 	return error;
 }
-module_init(init_tmpfs)
+
+#else /* !CONFIG_SHMEM */
+
+/*
+ * tiny-shmem: simple shmemfs and tmpfs using ramfs code
+ *
+ * This is intended for small system where the benefits of the full
+ * shmem code (swap-backed and resource-limited) are outweighed by
+ * their complexity. On systems without swap this code should be
+ * effectively equivalent, but much lighter weight.
+ */
+
+#include <linux/ramfs.h>
+
+static struct file_system_type tmpfs_fs_type = {
+	.name		= "tmpfs",
+	.get_sb		= ramfs_get_sb,
+	.kill_sb	= kill_litter_super,
+};
+
+static int __init init_tmpfs(void)
+{
+	BUG_ON(register_filesystem(&tmpfs_fs_type) != 0);
+
+	shm_mnt = kern_mount(&tmpfs_fs_type);
+	BUG_ON(IS_ERR(shm_mnt));
+
+	return 0;
+}
+
+int shmem_unuse(swp_entry_t entry, struct page *page)
+{
+	return 0;
+}
+
+#define shmem_file_operations ramfs_file_operations
+#define shmem_vm_ops generic_file_vm_ops
+#define shmem_get_inode ramfs_get_inode
+#define shmem_acct_size(a, b) 0
+#define shmem_unacct_size(a, b) do {} while (0)
+#define SHMEM_MAX_BYTES LLONG_MAX
+
+#endif /* CONFIG_SHMEM */
+
+/* common code */
 
 /**
  * shmem_file_setup - get an unlinked file living in tmpfs
@@ -2569,12 +2619,20 @@ struct file *shmem_file_setup(char *name, loff_t size, unsigned long flags)
 	if (!inode)
 		goto close_file;
 
+#ifdef CONFIG_SHMEM
 	SHMEM_I(inode)->flags = flags & VM_ACCOUNT;
+#endif
 	d_instantiate(dentry, inode);
 	inode->i_size = size;
 	inode->i_nlink = 0;	/* It is unlinked */
 	init_file(file, shm_mnt, dentry, FMODE_WRITE | FMODE_READ,
-			&shmem_file_operations);
+		  &shmem_file_operations);
+
+#ifndef CONFIG_MMU
+	error = ramfs_nommu_expand_for_mapping(inode, size);
+	if (error)
+		goto close_file;
+#endif
 	return file;
 
 close_file:
@@ -2606,3 +2664,5 @@ int shmem_zero_setup(struct vm_area_struct *vma)
 	vma->vm_ops = &shmem_vm_ops;
 	return 0;
 }
+
+module_init(init_tmpfs)
diff --git a/mm/slub.c b/mm/slub.c
index f0e2892fe403..6392ae5cc6b1 100644
--- a/mm/slub.c
+++ b/mm/slub.c
@@ -2254,7 +2254,7 @@ static int calculate_sizes(struct kmem_cache *s, int forced_order)
 		 * Add some empty padding so that we can catch
 		 * overwrites from earlier objects rather than let
 		 * tracking information or the free pointer be
-		 * corrupted if an user writes before the start
+		 * corrupted if a user writes before the start
 		 * of the object.
 		 */
 		size += sizeof(void *);
diff --git a/mm/swap.c b/mm/swap.c
index b135ec90cdeb..ba2c0e8b8b54 100644
--- a/mm/swap.c
+++ b/mm/swap.c
@@ -246,25 +246,6 @@ void add_page_to_unevictable_list(struct page *page)
 	spin_unlock_irq(&zone->lru_lock);
 }
 
-/**
- * lru_cache_add_active_or_unevictable
- * @page:  the page to be added to LRU
- * @vma:   vma in which page is mapped for determining reclaimability
- *
- * place @page on active or unevictable LRU list, depending on
- * page_evictable().  Note that if the page is not evictable,
- * it goes directly back onto it's zone's unevictable list.  It does
- * NOT use a per cpu pagevec.
- */
-void lru_cache_add_active_or_unevictable(struct page *page,
-					struct vm_area_struct *vma)
-{
-	if (page_evictable(page, vma))
-		lru_cache_add_lru(page, LRU_ACTIVE + page_is_file_cache(page));
-	else
-		add_page_to_unevictable_list(page);
-}
-
 /*
  * Drain pages out of the cpu's pagevecs.
  * Either "cpu" is the current CPU, and preemption has already been
@@ -398,28 +379,6 @@ void __pagevec_release(struct pagevec *pvec)
 EXPORT_SYMBOL(__pagevec_release);
 
 /*
- * pagevec_release() for pages which are known to not be on the LRU
- *
- * This function reinitialises the caller's pagevec.
- */
-void __pagevec_release_nonlru(struct pagevec *pvec)
-{
-	int i;
-	struct pagevec pages_to_free;
-
-	pagevec_init(&pages_to_free, pvec->cold);
-	for (i = 0; i < pagevec_count(pvec); i++) {
-		struct page *page = pvec->pages[i];
-
-		VM_BUG_ON(PageLRU(page));
-		if (put_page_testzero(page))
-			pagevec_add(&pages_to_free, page);
-	}
-	pagevec_free(&pages_to_free);
-	pagevec_reinit(pvec);
-}
-
-/*
  * Add the passed pages to the LRU, then drop the caller's refcount
  * on them.  Reinitialises the caller's pagevec.
  */
@@ -495,8 +454,7 @@ void pagevec_swap_free(struct pagevec *pvec)
 		struct page *page = pvec->pages[i];
 
 		if (PageSwapCache(page) && trylock_page(page)) {
-			if (PageSwapCache(page))
-				remove_exclusive_swap_page_ref(page);
+			try_to_free_swap(page);
 			unlock_page(page);
 		}
 	}
diff --git a/mm/swap_state.c b/mm/swap_state.c
index 3353c9029cef..81c825f67a7f 100644
--- a/mm/swap_state.c
+++ b/mm/swap_state.c
@@ -72,10 +72,10 @@ int add_to_swap_cache(struct page *page, swp_entry_t entry, gfp_t gfp_mask)
 {
 	int error;
 
-	BUG_ON(!PageLocked(page));
-	BUG_ON(PageSwapCache(page));
-	BUG_ON(PagePrivate(page));
-	BUG_ON(!PageSwapBacked(page));
+	VM_BUG_ON(!PageLocked(page));
+	VM_BUG_ON(PageSwapCache(page));
+	VM_BUG_ON(!PageSwapBacked(page));
+
 	error = radix_tree_preload(gfp_mask);
 	if (!error) {
 		page_cache_get(page);
@@ -108,10 +108,9 @@ int add_to_swap_cache(struct page *page, swp_entry_t entry, gfp_t gfp_mask)
  */
 void __delete_from_swap_cache(struct page *page)
 {
-	BUG_ON(!PageLocked(page));
-	BUG_ON(!PageSwapCache(page));
-	BUG_ON(PageWriteback(page));
-	BUG_ON(PagePrivate(page));
+	VM_BUG_ON(!PageLocked(page));
+	VM_BUG_ON(!PageSwapCache(page));
+	VM_BUG_ON(PageWriteback(page));
 
 	radix_tree_delete(&swapper_space.page_tree, page_private(page));
 	set_page_private(page, 0);
@@ -129,13 +128,13 @@ void __delete_from_swap_cache(struct page *page)
  * Allocate swap space for the page and add the page to the
  * swap cache.  Caller needs to hold the page lock. 
  */
-int add_to_swap(struct page * page, gfp_t gfp_mask)
+int add_to_swap(struct page *page)
 {
 	swp_entry_t entry;
 	int err;
 
-	BUG_ON(!PageLocked(page));
-	BUG_ON(!PageUptodate(page));
+	VM_BUG_ON(!PageLocked(page));
+	VM_BUG_ON(!PageUptodate(page));
 
 	for (;;) {
 		entry = get_swap_page();
@@ -154,7 +153,7 @@ int add_to_swap(struct page * page, gfp_t gfp_mask)
 		 * Add it to the swap cache and mark it dirty
 		 */
 		err = add_to_swap_cache(page, entry,
-				gfp_mask|__GFP_NOMEMALLOC|__GFP_NOWARN);
+				__GFP_HIGH|__GFP_NOMEMALLOC|__GFP_NOWARN);
 
 		switch (err) {
 		case 0:				/* Success */
@@ -196,14 +195,14 @@ void delete_from_swap_cache(struct page *page)
  * If we are the only user, then try to free up the swap cache. 
  * 
  * Its ok to check for PageSwapCache without the page lock
- * here because we are going to recheck again inside 
- * exclusive_swap_page() _with_ the lock. 
+ * here because we are going to recheck again inside
+ * try_to_free_swap() _with_ the lock.
  * 					- Marcelo
  */
 static inline void free_swap_cache(struct page *page)
 {
-	if (PageSwapCache(page) && trylock_page(page)) {
-		remove_exclusive_swap_page(page);
+	if (PageSwapCache(page) && !page_mapped(page) && trylock_page(page)) {
+		try_to_free_swap(page);
 		unlock_page(page);
 	}
 }
diff --git a/mm/swapfile.c b/mm/swapfile.c
index 54a9f87e5162..eec5ca758a23 100644
--- a/mm/swapfile.c
+++ b/mm/swapfile.c
@@ -16,6 +16,7 @@
 #include <linux/namei.h>
 #include <linux/shm.h>
 #include <linux/blkdev.h>
+#include <linux/random.h>
 #include <linux/writeback.h>
 #include <linux/proc_fs.h>
 #include <linux/seq_file.h>
@@ -35,6 +36,7 @@
 
 static DEFINE_SPINLOCK(swap_lock);
 static unsigned int nr_swapfiles;
+long nr_swap_pages;
 long total_swap_pages;
 static int swap_overflow;
 static int least_priority;
@@ -83,15 +85,96 @@ void swap_unplug_io_fn(struct backing_dev_info *unused_bdi, struct page *page)
 	up_read(&swap_unplug_sem);
 }
 
+/*
+ * swapon tell device that all the old swap contents can be discarded,
+ * to allow the swap device to optimize its wear-levelling.
+ */
+static int discard_swap(struct swap_info_struct *si)
+{
+	struct swap_extent *se;
+	int err = 0;
+
+	list_for_each_entry(se, &si->extent_list, list) {
+		sector_t start_block = se->start_block << (PAGE_SHIFT - 9);
+		sector_t nr_blocks = (sector_t)se->nr_pages << (PAGE_SHIFT - 9);
+
+		if (se->start_page == 0) {
+			/* Do not discard the swap header page! */
+			start_block += 1 << (PAGE_SHIFT - 9);
+			nr_blocks -= 1 << (PAGE_SHIFT - 9);
+			if (!nr_blocks)
+				continue;
+		}
+
+		err = blkdev_issue_discard(si->bdev, start_block,
+						nr_blocks, GFP_KERNEL);
+		if (err)
+			break;
+
+		cond_resched();
+	}
+	return err;		/* That will often be -EOPNOTSUPP */
+}
+
+/*
+ * swap allocation tell device that a cluster of swap can now be discarded,
+ * to allow the swap device to optimize its wear-levelling.
+ */
+static void discard_swap_cluster(struct swap_info_struct *si,
+				 pgoff_t start_page, pgoff_t nr_pages)
+{
+	struct swap_extent *se = si->curr_swap_extent;
+	int found_extent = 0;
+
+	while (nr_pages) {
+		struct list_head *lh;
+
+		if (se->start_page <= start_page &&
+		    start_page < se->start_page + se->nr_pages) {
+			pgoff_t offset = start_page - se->start_page;
+			sector_t start_block = se->start_block + offset;
+			sector_t nr_blocks = se->nr_pages - offset;
+
+			if (nr_blocks > nr_pages)
+				nr_blocks = nr_pages;
+			start_page += nr_blocks;
+			nr_pages -= nr_blocks;
+
+			if (!found_extent++)
+				si->curr_swap_extent = se;
+
+			start_block <<= PAGE_SHIFT - 9;
+			nr_blocks <<= PAGE_SHIFT - 9;
+			if (blkdev_issue_discard(si->bdev, start_block,
+							nr_blocks, GFP_NOIO))
+				break;
+		}
+
+		lh = se->list.next;
+		if (lh == &si->extent_list)
+			lh = lh->next;
+		se = list_entry(lh, struct swap_extent, list);
+	}
+}
+
+static int wait_for_discard(void *word)
+{
+	schedule();
+	return 0;
+}
+
 #define SWAPFILE_CLUSTER	256
 #define LATENCY_LIMIT		256
 
 static inline unsigned long scan_swap_map(struct swap_info_struct *si)
 {
-	unsigned long offset, last_in_cluster;
+	unsigned long offset;
+	unsigned long scan_base;
+	unsigned long last_in_cluster = 0;
 	int latency_ration = LATENCY_LIMIT;
+	int found_free_cluster = 0;
 
-	/* 
+	/*
 	 * We try to cluster swap pages by allocating them sequentially
 	 * in swap.  Once we've allocated SWAPFILE_CLUSTER pages this
 	 * way, however, we resort to first-free allocation, starting
@@ -99,16 +182,42 @@ static inline unsigned long scan_swap_map(struct swap_info_struct *si)
 	 * all over the entire swap partition, so that we reduce
 	 * overall disk seek times between swap pages.  -- sct
 	 * But we do now try to find an empty cluster.  -Andrea
+	 * And we let swap pages go all over an SSD partition.  Hugh
 	 */
 
 	si->flags += SWP_SCANNING;
-	if (unlikely(!si->cluster_nr)) {
-		si->cluster_nr = SWAPFILE_CLUSTER - 1;
-		if (si->pages - si->inuse_pages < SWAPFILE_CLUSTER)
-			goto lowest;
+	scan_base = offset = si->cluster_next;
+
+	if (unlikely(!si->cluster_nr--)) {
+		if (si->pages - si->inuse_pages < SWAPFILE_CLUSTER) {
+			si->cluster_nr = SWAPFILE_CLUSTER - 1;
+			goto checks;
+		}
+		if (si->flags & SWP_DISCARDABLE) {
+			/*
+			 * Start range check on racing allocations, in case
+			 * they overlap the cluster we eventually decide on
+			 * (we scan without swap_lock to allow preemption).
+			 * It's hardly conceivable that cluster_nr could be
+			 * wrapped during our scan, but don't depend on it.
+			 */
+			if (si->lowest_alloc)
+				goto checks;
+			si->lowest_alloc = si->max;
+			si->highest_alloc = 0;
+		}
 		spin_unlock(&swap_lock);
 
-		offset = si->lowest_bit;
+		/*
+		 * If seek is expensive, start searching for new cluster from
+		 * start of partition, to minimize the span of allocated swap.
+		 * But if seek is cheap, search from our current position, so
+		 * that swap is allocated from all over the partition: if the
+		 * Flash Translation Layer only remaps within limited zones,
+		 * we don't want to wear out the first zone too quickly.
+		 */
+		if (!(si->flags & SWP_SOLIDSTATE))
+			scan_base = offset = si->lowest_bit;
 		last_in_cluster = offset + SWAPFILE_CLUSTER - 1;
 
 		/* Locate the first empty (unaligned) cluster */
@@ -117,43 +226,124 @@ static inline unsigned long scan_swap_map(struct swap_info_struct *si)
 				last_in_cluster = offset + SWAPFILE_CLUSTER;
 			else if (offset == last_in_cluster) {
 				spin_lock(&swap_lock);
-				si->cluster_next = offset-SWAPFILE_CLUSTER+1;
-				goto cluster;
+				offset -= SWAPFILE_CLUSTER - 1;
+				si->cluster_next = offset;
+				si->cluster_nr = SWAPFILE_CLUSTER - 1;
+				found_free_cluster = 1;
+				goto checks;
 			}
 			if (unlikely(--latency_ration < 0)) {
 				cond_resched();
 				latency_ration = LATENCY_LIMIT;
 			}
 		}
+
+		offset = si->lowest_bit;
+		last_in_cluster = offset + SWAPFILE_CLUSTER - 1;
+
+		/* Locate the first empty (unaligned) cluster */
+		for (; last_in_cluster < scan_base; offset++) {
+			if (si->swap_map[offset])
+				last_in_cluster = offset + SWAPFILE_CLUSTER;
+			else if (offset == last_in_cluster) {
+				spin_lock(&swap_lock);
+				offset -= SWAPFILE_CLUSTER - 1;
+				si->cluster_next = offset;
+				si->cluster_nr = SWAPFILE_CLUSTER - 1;
+				found_free_cluster = 1;
+				goto checks;
+			}
+			if (unlikely(--latency_ration < 0)) {
+				cond_resched();
+				latency_ration = LATENCY_LIMIT;
+			}
+		}
+
+		offset = scan_base;
 		spin_lock(&swap_lock);
-		goto lowest;
+		si->cluster_nr = SWAPFILE_CLUSTER - 1;
+		si->lowest_alloc = 0;
 	}
 
-	si->cluster_nr--;
-cluster:
-	offset = si->cluster_next;
-	if (offset > si->highest_bit)
-lowest:		offset = si->lowest_bit;
-checks:	if (!(si->flags & SWP_WRITEOK))
+checks:
+	if (!(si->flags & SWP_WRITEOK))
 		goto no_page;
 	if (!si->highest_bit)
 		goto no_page;
-	if (!si->swap_map[offset]) {
-		if (offset == si->lowest_bit)
-			si->lowest_bit++;
-		if (offset == si->highest_bit)
-			si->highest_bit--;
-		si->inuse_pages++;
-		if (si->inuse_pages == si->pages) {
-			si->lowest_bit = si->max;
-			si->highest_bit = 0;
+	if (offset > si->highest_bit)
+		scan_base = offset = si->lowest_bit;
+	if (si->swap_map[offset])
+		goto scan;
+
+	if (offset == si->lowest_bit)
+		si->lowest_bit++;
+	if (offset == si->highest_bit)
+		si->highest_bit--;
+	si->inuse_pages++;
+	if (si->inuse_pages == si->pages) {
+		si->lowest_bit = si->max;
+		si->highest_bit = 0;
+	}
+	si->swap_map[offset] = 1;
+	si->cluster_next = offset + 1;
+	si->flags -= SWP_SCANNING;
+
+	if (si->lowest_alloc) {
+		/*
+		 * Only set when SWP_DISCARDABLE, and there's a scan
+		 * for a free cluster in progress or just completed.
+		 */
+		if (found_free_cluster) {
+			/*
+			 * To optimize wear-levelling, discard the
+			 * old data of the cluster, taking care not to
+			 * discard any of its pages that have already
+			 * been allocated by racing tasks (offset has
+			 * already stepped over any at the beginning).
+			 */
+			if (offset < si->highest_alloc &&
+			    si->lowest_alloc <= last_in_cluster)
+				last_in_cluster = si->lowest_alloc - 1;
+			si->flags |= SWP_DISCARDING;
+			spin_unlock(&swap_lock);
+
+			if (offset < last_in_cluster)
+				discard_swap_cluster(si, offset,
+					last_in_cluster - offset + 1);
+
+			spin_lock(&swap_lock);
+			si->lowest_alloc = 0;
+			si->flags &= ~SWP_DISCARDING;
+
+			smp_mb();	/* wake_up_bit advises this */
+			wake_up_bit(&si->flags, ilog2(SWP_DISCARDING));
+
+		} else if (si->flags & SWP_DISCARDING) {
+			/*
+			 * Delay using pages allocated by racing tasks
+			 * until the whole discard has been issued. We
+			 * could defer that delay until swap_writepage,
+			 * but it's easier to keep this self-contained.
+			 */
+			spin_unlock(&swap_lock);
+			wait_on_bit(&si->flags, ilog2(SWP_DISCARDING),
+				wait_for_discard, TASK_UNINTERRUPTIBLE);
+			spin_lock(&swap_lock);
+		} else {
+			/*
+			 * Note pages allocated by racing tasks while
+			 * scan for a free cluster is in progress, so
+			 * that its final discard can exclude them.
+			 */
+			if (offset < si->lowest_alloc)
+				si->lowest_alloc = offset;
+			if (offset > si->highest_alloc)
+				si->highest_alloc = offset;
 		}
-		si->swap_map[offset] = 1;
-		si->cluster_next = offset + 1;
-		si->flags -= SWP_SCANNING;
-		return offset;
 	}
+	return offset;
 
+scan:
 	spin_unlock(&swap_lock);
 	while (++offset <= si->highest_bit) {
 		if (!si->swap_map[offset]) {
@@ -165,8 +355,18 @@ checks:	if (!(si->flags & SWP_WRITEOK))
 			latency_ration = LATENCY_LIMIT;
 		}
 	}
+	offset = si->lowest_bit;
+	while (++offset < scan_base) {
+		if (!si->swap_map[offset]) {
+			spin_lock(&swap_lock);
+			goto checks;
+		}
+		if (unlikely(--latency_ration < 0)) {
+			cond_resched();
+			latency_ration = LATENCY_LIMIT;
+		}
+	}
 	spin_lock(&swap_lock);
-	goto lowest;
 
 no_page:
 	si->flags -= SWP_SCANNING;
@@ -268,7 +468,7 @@ bad_nofile:
 	printk(KERN_ERR "swap_free: %s%08lx\n", Bad_file, entry.val);
 out:
 	return NULL;
-}	
+}
 
 static int swap_entry_free(struct swap_info_struct *p, unsigned long offset)
 {
@@ -326,97 +526,58 @@ static inline int page_swapcount(struct page *page)
 }
 
 /*
- * We can use this swap cache entry directly
- * if there are no other references to it.
+ * We can write to an anon page without COW if there are no other references
+ * to it.  And as a side-effect, free up its swap: because the old content
+ * on disk will never be read, and seeking back there to write new content
+ * later would only waste time away from clustering.
  */
-int can_share_swap_page(struct page *page)
+int reuse_swap_page(struct page *page)
 {
 	int count;
 
-	BUG_ON(!PageLocked(page));
+	VM_BUG_ON(!PageLocked(page));
 	count = page_mapcount(page);
-	if (count <= 1 && PageSwapCache(page))
+	if (count <= 1 && PageSwapCache(page)) {
 		count += page_swapcount(page);
+		if (count == 1 && !PageWriteback(page)) {
+			delete_from_swap_cache(page);
+			SetPageDirty(page);
+		}
+	}
 	return count == 1;
 }
 
 /*
- * Work out if there are any other processes sharing this
- * swap cache page. Free it if you can. Return success.
+ * If swap is getting full, or if there are no more mappings of this page,
+ * then try_to_free_swap is called to free its swap space.
  */
-static int remove_exclusive_swap_page_count(struct page *page, int count)
+int try_to_free_swap(struct page *page)
 {
-	int retval;
-	struct swap_info_struct * p;
-	swp_entry_t entry;
-
-	BUG_ON(PagePrivate(page));
-	BUG_ON(!PageLocked(page));
+	VM_BUG_ON(!PageLocked(page));
 
 	if (!PageSwapCache(page))
 		return 0;
 	if (PageWriteback(page))
 		return 0;
-	if (page_count(page) != count) /* us + cache + ptes */
-		return 0;
-
-	entry.val = page_private(page);
-	p = swap_info_get(entry);
-	if (!p)
+	if (page_swapcount(page))
 		return 0;
 
-	/* Is the only swap cache user the cache itself? */
-	retval = 0;
-	if (p->swap_map[swp_offset(entry)] == 1) {
-		/* Recheck the page count with the swapcache lock held.. */
-		spin_lock_irq(&swapper_space.tree_lock);
-		if ((page_count(page) == count) && !PageWriteback(page)) {
-			__delete_from_swap_cache(page);
-			SetPageDirty(page);
-			retval = 1;
-		}
-		spin_unlock_irq(&swapper_space.tree_lock);
-	}
-	spin_unlock(&swap_lock);
-
-	if (retval) {
-		swap_free(entry);
-		page_cache_release(page);
-	}
-
-	return retval;
-}
-
-/*
- * Most of the time the page should have two references: one for the
- * process and one for the swap cache.
- */
-int remove_exclusive_swap_page(struct page *page)
-{
-	return remove_exclusive_swap_page_count(page, 2);
-}
-
-/*
- * The pageout code holds an extra reference to the page.  That raises
- * the reference count to test for to 2 for a page that is only in the
- * swap cache plus 1 for each process that maps the page.
- */
-int remove_exclusive_swap_page_ref(struct page *page)
-{
-	return remove_exclusive_swap_page_count(page, 2 + page_mapcount(page));
+	delete_from_swap_cache(page);
+	SetPageDirty(page);
+	return 1;
 }
 
 /*
  * Free the swap entry like above, but also try to
  * free the page cache entry if it is the last user.
  */
-void free_swap_and_cache(swp_entry_t entry)
+int free_swap_and_cache(swp_entry_t entry)
 {
-	struct swap_info_struct * p;
+	struct swap_info_struct *p;
 	struct page *page = NULL;
 
 	if (is_migration_entry(entry))
-		return;
+		return 1;
 
 	p = swap_info_get(entry);
 	if (p) {
@@ -430,20 +591,19 @@ void free_swap_and_cache(swp_entry_t entry)
 		spin_unlock(&swap_lock);
 	}
 	if (page) {
-		int one_user;
-
-		BUG_ON(PagePrivate(page));
-		one_user = (page_count(page) == 2);
-		/* Only cache user (+us), or swap space full? Free it! */
-		/* Also recheck PageSwapCache after page is locked (above) */
+		/*
+		 * Not mapped elsewhere, or swap space full? Free it!
+		 * Also recheck PageSwapCache now page is locked (above).
+		 */
 		if (PageSwapCache(page) && !PageWriteback(page) &&
-					(one_user || vm_swap_full())) {
+				(!page_mapped(page) || vm_swap_full())) {
 			delete_from_swap_cache(page);
 			SetPageDirty(page);
 		}
 		unlock_page(page);
 		page_cache_release(page);
 	}
+	return p != NULL;
 }
 
 #ifdef CONFIG_HIBERNATION
@@ -776,10 +936,10 @@ static int try_to_unuse(unsigned int type)
 			break;
 		}
 
-		/* 
+		/*
 		 * Get a page for the entry, using the existing swap
 		 * cache page if there is one.  Otherwise, get a clean
-		 * page and read the swap into it. 
+		 * page and read the swap into it.
 		 */
 		swap_map = &si->swap_map[i];
 		entry = swp_entry(type, i);
@@ -930,7 +1090,16 @@ static int try_to_unuse(unsigned int type)
 			lock_page(page);
 			wait_on_page_writeback(page);
 		}
-		if (PageSwapCache(page))
+
+		/*
+		 * It is conceivable that a racing task removed this page from
+		 * swap cache just before we acquired the page lock at the top,
+		 * or while we dropped it in unuse_mm().  The page might even
+		 * be back in swap cache on another swap area: that we must not
+		 * delete, since it may not have been written out to swap yet.
+		 */
+		if (PageSwapCache(page) &&
+		    likely(page_private(page) == entry.val))
 			delete_from_swap_cache(page);
 
 		/*
@@ -1203,26 +1372,6 @@ out:
 	return ret;
 }
 
-#if 0	/* We don't need this yet */
-#include <linux/backing-dev.h>
-int page_queue_congested(struct page *page)
-{
-	struct backing_dev_info *bdi;
-
-	BUG_ON(!PageLocked(page));	/* It pins the swap_info_struct */
-
-	if (PageSwapCache(page)) {
-		swp_entry_t entry = { .val = page_private(page) };
-		struct swap_info_struct *sis;
-
-		sis = get_swap_info_struct(swp_type(entry));
-		bdi = sis->bdev->bd_inode->i_mapping->backing_dev_info;
-	} else
-		bdi = page->mapping->backing_dev_info;
-	return bdi_write_congested(bdi);
-}
-#endif
-
 asmlinkage long sys_swapoff(const char __user * specialfile)
 {
 	struct swap_info_struct * p = NULL;
@@ -1233,7 +1382,7 @@ asmlinkage long sys_swapoff(const char __user * specialfile)
 	char * pathname;
 	int i, type, prev;
 	int err;
-	
+
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
 
@@ -1253,7 +1402,7 @@ asmlinkage long sys_swapoff(const char __user * specialfile)
 	spin_lock(&swap_lock);
 	for (type = swap_list.head; type >= 0; type = swap_info[type].next) {
 		p = swap_info + type;
-		if ((p->flags & SWP_ACTIVE) == SWP_ACTIVE) {
+		if (p->flags & SWP_WRITEOK) {
 			if (p->swap_file->f_mapping == mapping)
 				break;
 		}
@@ -1426,12 +1575,12 @@ static int swap_show(struct seq_file *swap, void *v)
 	file = ptr->swap_file;
 	len = seq_path(swap, &file->f_path, " \t\n\\");
 	seq_printf(swap, "%*s%s\t%u\t%u\t%d\n",
-		       len < 40 ? 40 - len : 1, " ",
-		       S_ISBLK(file->f_path.dentry->d_inode->i_mode) ?
+			len < 40 ? 40 - len : 1, " ",
+			S_ISBLK(file->f_path.dentry->d_inode->i_mode) ?
 				"partition" : "file\t",
-		       ptr->pages << (PAGE_SHIFT - 10),
-		       ptr->inuse_pages << (PAGE_SHIFT - 10),
-		       ptr->prio);
+			ptr->pages << (PAGE_SHIFT - 10),
+			ptr->inuse_pages << (PAGE_SHIFT - 10),
+			ptr->prio);
 	return 0;
 }
 
@@ -1487,12 +1636,11 @@ asmlinkage long sys_swapon(const char __user * specialfile, int swap_flags)
 	int i, prev;
 	int error;
 	union swap_header *swap_header = NULL;
-	int swap_header_version;
 	unsigned int nr_good_pages = 0;
 	int nr_extents = 0;
 	sector_t span;
 	unsigned long maxpages = 1;
-	int swapfilesize;
+	unsigned long swapfilepages;
 	unsigned short *swap_map = NULL;
 	struct page *page = NULL;
 	struct inode *inode = NULL;
@@ -1570,7 +1718,7 @@ asmlinkage long sys_swapon(const char __user * specialfile, int swap_flags)
 		goto bad_swap;
 	}
 
-	swapfilesize = i_size_read(inode) >> PAGE_SHIFT;
+	swapfilepages = i_size_read(inode) >> PAGE_SHIFT;
 
 	/*
 	 * Read the swap header.
@@ -1584,101 +1732,86 @@ asmlinkage long sys_swapon(const char __user * specialfile, int swap_flags)
 		error = PTR_ERR(page);
 		goto bad_swap;
 	}
-	kmap(page);
-	swap_header = page_address(page);
+	swap_header = kmap(page);
 
-	if (!memcmp("SWAP-SPACE",swap_header->magic.magic,10))
-		swap_header_version = 1;
-	else if (!memcmp("SWAPSPACE2",swap_header->magic.magic,10))
-		swap_header_version = 2;
-	else {
+	if (memcmp("SWAPSPACE2", swap_header->magic.magic, 10)) {
 		printk(KERN_ERR "Unable to find swap-space signature\n");
 		error = -EINVAL;
 		goto bad_swap;
 	}
-	
-	switch (swap_header_version) {
-	case 1:
-		printk(KERN_ERR "version 0 swap is no longer supported. "
-			"Use mkswap -v1 %s\n", name);
+
+	/* swap partition endianess hack... */
+	if (swab32(swap_header->info.version) == 1) {
+		swab32s(&swap_header->info.version);
+		swab32s(&swap_header->info.last_page);
+		swab32s(&swap_header->info.nr_badpages);
+		for (i = 0; i < swap_header->info.nr_badpages; i++)
+			swab32s(&swap_header->info.badpages[i]);
+	}
+	/* Check the swap header's sub-version */
+	if (swap_header->info.version != 1) {
+		printk(KERN_WARNING
+		       "Unable to handle swap header version %d\n",
+		       swap_header->info.version);
 		error = -EINVAL;
 		goto bad_swap;
-	case 2:
-		/* swap partition endianess hack... */
-		if (swab32(swap_header->info.version) == 1) {
-			swab32s(&swap_header->info.version);
-			swab32s(&swap_header->info.last_page);
-			swab32s(&swap_header->info.nr_badpages);
-			for (i = 0; i < swap_header->info.nr_badpages; i++)
-				swab32s(&swap_header->info.badpages[i]);
-		}
-		/* Check the swap header's sub-version and the size of
-                   the swap file and bad block lists */
-		if (swap_header->info.version != 1) {
-			printk(KERN_WARNING
-			       "Unable to handle swap header version %d\n",
-			       swap_header->info.version);
-			error = -EINVAL;
-			goto bad_swap;
-		}
+	}
 
-		p->lowest_bit  = 1;
-		p->cluster_next = 1;
+	p->lowest_bit  = 1;
+	p->cluster_next = 1;
 
-		/*
-		 * Find out how many pages are allowed for a single swap
-		 * device. There are two limiting factors: 1) the number of
-		 * bits for the swap offset in the swp_entry_t type and
-		 * 2) the number of bits in the a swap pte as defined by
-		 * the different architectures. In order to find the
-		 * largest possible bit mask a swap entry with swap type 0
-		 * and swap offset ~0UL is created, encoded to a swap pte,
-		 * decoded to a swp_entry_t again and finally the swap
-		 * offset is extracted. This will mask all the bits from
-		 * the initial ~0UL mask that can't be encoded in either
-		 * the swp_entry_t or the architecture definition of a
-		 * swap pte.
-		 */
-		maxpages = swp_offset(pte_to_swp_entry(swp_entry_to_pte(swp_entry(0,~0UL)))) - 1;
-		if (maxpages > swap_header->info.last_page)
-			maxpages = swap_header->info.last_page;
-		p->highest_bit = maxpages - 1;
+	/*
+	 * Find out how many pages are allowed for a single swap
+	 * device. There are two limiting factors: 1) the number of
+	 * bits for the swap offset in the swp_entry_t type and
+	 * 2) the number of bits in the a swap pte as defined by
+	 * the different architectures. In order to find the
+	 * largest possible bit mask a swap entry with swap type 0
+	 * and swap offset ~0UL is created, encoded to a swap pte,
+	 * decoded to a swp_entry_t again and finally the swap
+	 * offset is extracted. This will mask all the bits from
+	 * the initial ~0UL mask that can't be encoded in either
+	 * the swp_entry_t or the architecture definition of a
+	 * swap pte.
+	 */
+	maxpages = swp_offset(pte_to_swp_entry(
+			swp_entry_to_pte(swp_entry(0, ~0UL)))) - 1;
+	if (maxpages > swap_header->info.last_page)
+		maxpages = swap_header->info.last_page;
+	p->highest_bit = maxpages - 1;
 
-		error = -EINVAL;
-		if (!maxpages)
-			goto bad_swap;
-		if (swapfilesize && maxpages > swapfilesize) {
-			printk(KERN_WARNING
-			       "Swap area shorter than signature indicates\n");
-			goto bad_swap;
-		}
-		if (swap_header->info.nr_badpages && S_ISREG(inode->i_mode))
-			goto bad_swap;
-		if (swap_header->info.nr_badpages > MAX_SWAP_BADPAGES)
-			goto bad_swap;
+	error = -EINVAL;
+	if (!maxpages)
+		goto bad_swap;
+	if (swapfilepages && maxpages > swapfilepages) {
+		printk(KERN_WARNING
+		       "Swap area shorter than signature indicates\n");
+		goto bad_swap;
+	}
+	if (swap_header->info.nr_badpages && S_ISREG(inode->i_mode))
+		goto bad_swap;
+	if (swap_header->info.nr_badpages > MAX_SWAP_BADPAGES)
+		goto bad_swap;
 
-		/* OK, set up the swap map and apply the bad block list */
-		swap_map = vmalloc(maxpages * sizeof(short));
-		if (!swap_map) {
-			error = -ENOMEM;
-			goto bad_swap;
-		}
+	/* OK, set up the swap map and apply the bad block list */
+	swap_map = vmalloc(maxpages * sizeof(short));
+	if (!swap_map) {
+		error = -ENOMEM;
+		goto bad_swap;
+	}
 
-		error = 0;
-		memset(swap_map, 0, maxpages * sizeof(short));
-		for (i = 0; i < swap_header->info.nr_badpages; i++) {
-			int page_nr = swap_header->info.badpages[i];
-			if (page_nr <= 0 || page_nr >= swap_header->info.last_page)
-				error = -EINVAL;
-			else
-				swap_map[page_nr] = SWAP_MAP_BAD;
-		}
-		nr_good_pages = swap_header->info.last_page -
-				swap_header->info.nr_badpages -
-				1 /* header page */;
-		if (error)
+	memset(swap_map, 0, maxpages * sizeof(short));
+	for (i = 0; i < swap_header->info.nr_badpages; i++) {
+		int page_nr = swap_header->info.badpages[i];
+		if (page_nr <= 0 || page_nr >= swap_header->info.last_page) {
+			error = -EINVAL;
 			goto bad_swap;
+		}
+		swap_map[page_nr] = SWAP_MAP_BAD;
 	}
+	nr_good_pages = swap_header->info.last_page -
+			swap_header->info.nr_badpages -
+			1 /* header page */;
 
 	if (nr_good_pages) {
 		swap_map[0] = SWAP_MAP_BAD;
@@ -1697,6 +1830,13 @@ asmlinkage long sys_swapon(const char __user * specialfile, int swap_flags)
 		goto bad_swap;
 	}
 
+	if (blk_queue_nonrot(bdev_get_queue(p->bdev))) {
+		p->flags |= SWP_SOLIDSTATE;
+		p->cluster_next = 1 + (random32() % p->highest_bit);
+	}
+	if (discard_swap(p) == 0)
+		p->flags |= SWP_DISCARDABLE;
+
 	mutex_lock(&swapon_mutex);
 	spin_lock(&swap_lock);
 	if (swap_flags & SWAP_FLAG_PREFER)
@@ -1705,14 +1845,16 @@ asmlinkage long sys_swapon(const char __user * specialfile, int swap_flags)
 	else
 		p->prio = --least_priority;
 	p->swap_map = swap_map;
-	p->flags = SWP_ACTIVE;
+	p->flags |= SWP_WRITEOK;
 	nr_swap_pages += nr_good_pages;
 	total_swap_pages += nr_good_pages;
 
 	printk(KERN_INFO "Adding %uk swap on %s.  "
-			"Priority:%d extents:%d across:%lluk\n",
+			"Priority:%d extents:%d across:%lluk %s%s\n",
 		nr_good_pages<<(PAGE_SHIFT-10), name, p->prio,
-		nr_extents, (unsigned long long)span<<(PAGE_SHIFT-10));
+		nr_extents, (unsigned long long)span<<(PAGE_SHIFT-10),
+		(p->flags & SWP_SOLIDSTATE) ? "SS" : "",
+		(p->flags & SWP_DISCARDABLE) ? "D" : "");
 
 	/* insert swap space into swap_list: */
 	prev = -1;
diff --git a/mm/tiny-shmem.c b/mm/tiny-shmem.c
deleted file mode 100644
index 3e67d575ee6e..000000000000
--- a/mm/tiny-shmem.c
+++ /dev/null
@@ -1,134 +0,0 @@
-/*
- * tiny-shmem.c: simple shmemfs and tmpfs using ramfs code
- *
- * Matt Mackall <mpm@selenic.com> January, 2004
- * derived from mm/shmem.c and fs/ramfs/inode.c
- *
- * This is intended for small system where the benefits of the full
- * shmem code (swap-backed and resource-limited) are outweighed by
- * their complexity. On systems without swap this code should be
- * effectively equivalent, but much lighter weight.
- */
-
-#include <linux/fs.h>
-#include <linux/init.h>
-#include <linux/vfs.h>
-#include <linux/mount.h>
-#include <linux/file.h>
-#include <linux/mm.h>
-#include <linux/module.h>
-#include <linux/swap.h>
-#include <linux/ramfs.h>
-
-static struct file_system_type tmpfs_fs_type = {
-	.name		= "tmpfs",
-	.get_sb		= ramfs_get_sb,
-	.kill_sb	= kill_litter_super,
-};
-
-static struct vfsmount *shm_mnt;
-
-static int __init init_tmpfs(void)
-{
-	BUG_ON(register_filesystem(&tmpfs_fs_type) != 0);
-
-	shm_mnt = kern_mount(&tmpfs_fs_type);
-	BUG_ON(IS_ERR(shm_mnt));
-
-	return 0;
-}
-module_init(init_tmpfs)
-
-/**
- * shmem_file_setup - get an unlinked file living in tmpfs
- * @name: name for dentry (to be seen in /proc/<pid>/maps
- * @size: size to be set for the file
- * @flags: vm_flags
- */
-struct file *shmem_file_setup(char *name, loff_t size, unsigned long flags)
-{
-	int error;
-	struct file *file;
-	struct inode *inode;
-	struct dentry *dentry, *root;
-	struct qstr this;
-
-	if (IS_ERR(shm_mnt))
-		return (void *)shm_mnt;
-
-	error = -ENOMEM;
-	this.name = name;
-	this.len = strlen(name);
-	this.hash = 0; /* will go */
-	root = shm_mnt->mnt_root;
-	dentry = d_alloc(root, &this);
-	if (!dentry)
-		goto put_memory;
-
-	error = -ENFILE;
-	file = get_empty_filp();
-	if (!file)
-		goto put_dentry;
-
-	error = -ENOSPC;
-	inode = ramfs_get_inode(root->d_sb, S_IFREG | S_IRWXUGO, 0);
-	if (!inode)
-		goto close_file;
-
-	d_instantiate(dentry, inode);
-	inode->i_size = size;
-	inode->i_nlink = 0;	/* It is unlinked */
-	init_file(file, shm_mnt, dentry, FMODE_WRITE | FMODE_READ,
-			&ramfs_file_operations);
-
-#ifndef CONFIG_MMU
-	error = ramfs_nommu_expand_for_mapping(inode, size);
-	if (error)
-		goto close_file;
-#endif
-	return file;
-
-close_file:
-	put_filp(file);
-put_dentry:
-	dput(dentry);
-put_memory:
-	return ERR_PTR(error);
-}
-EXPORT_SYMBOL_GPL(shmem_file_setup);
-
-/**
- * shmem_zero_setup - setup a shared anonymous mapping
- * @vma: the vma to be mmapped is prepared by do_mmap_pgoff
- */
-int shmem_zero_setup(struct vm_area_struct *vma)
-{
-	struct file *file;
-	loff_t size = vma->vm_end - vma->vm_start;
-
-	file = shmem_file_setup("dev/zero", size, vma->vm_flags);
-	if (IS_ERR(file))
-		return PTR_ERR(file);
-
-	if (vma->vm_file)
-		fput(vma->vm_file);
-	vma->vm_file = file;
-	vma->vm_ops = &generic_file_vm_ops;
-	return 0;
-}
-
-int shmem_unuse(swp_entry_t entry, struct page *page)
-{
-	return 0;
-}
-
-#ifndef CONFIG_MMU
-unsigned long shmem_get_unmapped_area(struct file *file,
-				      unsigned long addr,
-				      unsigned long len,
-				      unsigned long pgoff,
-				      unsigned long flags)
-{
-	return ramfs_nommu_get_unmapped_area(file, addr, len, pgoff, flags);
-}
-#endif
diff --git a/mm/vmalloc.c b/mm/vmalloc.c
index 7465f22fec0c..c5db9a7264d9 100644
--- a/mm/vmalloc.c
+++ b/mm/vmalloc.c
@@ -14,6 +14,7 @@
 #include <linux/highmem.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
+#include <linux/mutex.h>
 #include <linux/interrupt.h>
 #include <linux/proc_fs.h>
 #include <linux/seq_file.h>
@@ -381,8 +382,9 @@ found:
 			goto retry;
 		}
 		if (printk_ratelimit())
-			printk(KERN_WARNING "vmap allocation failed: "
-				 "use vmalloc=<size> to increase size.\n");
+			printk(KERN_WARNING
+				"vmap allocation for size %lu failed: "
+				"use vmalloc=<size> to increase size.\n", size);
 		return ERR_PTR(-EBUSY);
 	}
 
@@ -432,6 +434,27 @@ static void unmap_vmap_area(struct vmap_area *va)
 	vunmap_page_range(va->va_start, va->va_end);
 }
 
+static void vmap_debug_free_range(unsigned long start, unsigned long end)
+{
+	/*
+	 * Unmap page tables and force a TLB flush immediately if
+	 * CONFIG_DEBUG_PAGEALLOC is set. This catches use after free
+	 * bugs similarly to those in linear kernel virtual address
+	 * space after a page has been freed.
+	 *
+	 * All the lazy freeing logic is still retained, in order to
+	 * minimise intrusiveness of this debugging feature.
+	 *
+	 * This is going to be *slow* (linear kernel virtual address
+	 * debugging doesn't do a broadcast TLB flush so it is a lot
+	 * faster).
+	 */
+#ifdef CONFIG_DEBUG_PAGEALLOC
+	vunmap_page_range(start, end);
+	flush_tlb_kernel_range(start, end);
+#endif
+}
+
 /*
  * lazy_max_pages is the maximum amount of virtual address space we gather up
  * before attempting to purge with a TLB flush.
@@ -472,7 +495,7 @@ static atomic_t vmap_lazy_nr = ATOMIC_INIT(0);
 static void __purge_vmap_area_lazy(unsigned long *start, unsigned long *end,
 					int sync, int force_flush)
 {
-	static DEFINE_SPINLOCK(purge_lock);
+	static DEFINE_MUTEX(purge_lock);
 	LIST_HEAD(valist);
 	struct vmap_area *va;
 	int nr = 0;
@@ -483,10 +506,10 @@ static void __purge_vmap_area_lazy(unsigned long *start, unsigned long *end,
 	 * the case that isn't actually used at the moment anyway.
 	 */
 	if (!sync && !force_flush) {
-		if (!spin_trylock(&purge_lock))
+		if (!mutex_trylock(&purge_lock))
 			return;
 	} else
-		spin_lock(&purge_lock);
+		mutex_lock(&purge_lock);
 
 	rcu_read_lock();
 	list_for_each_entry_rcu(va, &vmap_area_list, list) {
@@ -518,7 +541,7 @@ static void __purge_vmap_area_lazy(unsigned long *start, unsigned long *end,
 			__free_vmap_area(va);
 		spin_unlock(&vmap_area_lock);
 	}
-	spin_unlock(&purge_lock);
+	mutex_unlock(&purge_lock);
 }
 
 /*
@@ -912,6 +935,7 @@ void vm_unmap_ram(const void *mem, unsigned int count)
 	BUG_ON(addr & (PAGE_SIZE-1));
 
 	debug_check_no_locks_freed(mem, size);
+	vmap_debug_free_range(addr, addr+size);
 
 	if (likely(count <= VMAP_MAX_ALLOC))
 		vb_free(mem, size);
@@ -1128,6 +1152,8 @@ struct vm_struct *remove_vm_area(const void *addr)
 	if (va && va->flags & VM_VM_AREA) {
 		struct vm_struct *vm = va->private;
 		struct vm_struct *tmp, **p;
+
+		vmap_debug_free_range(va->va_start, va->va_end);
 		free_unmap_vmap_area(va);
 		vm->size -= PAGE_SIZE;
 
@@ -1375,7 +1401,8 @@ void *vmalloc_user(unsigned long size)
 	struct vm_struct *area;
 	void *ret;
 
-	ret = __vmalloc(size, GFP_KERNEL | __GFP_HIGHMEM | __GFP_ZERO, PAGE_KERNEL);
+	ret = __vmalloc_node(size, GFP_KERNEL | __GFP_HIGHMEM | __GFP_ZERO,
+			     PAGE_KERNEL, -1, __builtin_return_address(0));
 	if (ret) {
 		area = find_vm_area(ret);
 		area->flags |= VM_USERMAP;
@@ -1420,7 +1447,8 @@ EXPORT_SYMBOL(vmalloc_node);
 
 void *vmalloc_exec(unsigned long size)
 {
-	return __vmalloc(size, GFP_KERNEL | __GFP_HIGHMEM, PAGE_KERNEL_EXEC);
+	return __vmalloc_node(size, GFP_KERNEL | __GFP_HIGHMEM, PAGE_KERNEL_EXEC,
+			      -1, __builtin_return_address(0));
 }
 
 #if defined(CONFIG_64BIT) && defined(CONFIG_ZONE_DMA32)
@@ -1440,7 +1468,8 @@ void *vmalloc_exec(unsigned long size)
  */
 void *vmalloc_32(unsigned long size)
 {
-	return __vmalloc(size, GFP_VMALLOC32, PAGE_KERNEL);
+	return __vmalloc_node(size, GFP_VMALLOC32, PAGE_KERNEL,
+			      -1, __builtin_return_address(0));
 }
 EXPORT_SYMBOL(vmalloc_32);
 
@@ -1456,7 +1485,8 @@ void *vmalloc_32_user(unsigned long size)
 	struct vm_struct *area;
 	void *ret;
 
-	ret = __vmalloc(size, GFP_VMALLOC32 | __GFP_ZERO, PAGE_KERNEL);
+	ret = __vmalloc_node(size, GFP_VMALLOC32 | __GFP_ZERO, PAGE_KERNEL,
+			     -1, __builtin_return_address(0));
 	if (ret) {
 		area = find_vm_area(ret);
 		area->flags |= VM_USERMAP;
diff --git a/mm/vmscan.c b/mm/vmscan.c
index d196f46c8808..b07c48b09a93 100644
--- a/mm/vmscan.c
+++ b/mm/vmscan.c
@@ -52,6 +52,9 @@ struct scan_control {
 	/* Incremented by the number of inactive pages that were scanned */
 	unsigned long nr_scanned;
 
+	/* Number of pages freed so far during a call to shrink_zones() */
+	unsigned long nr_reclaimed;
+
 	/* This context's GFP mask */
 	gfp_t gfp_mask;
 
@@ -617,7 +620,6 @@ static unsigned long shrink_page_list(struct list_head *page_list,
 					referenced && page_mapping_inuse(page))
 			goto activate_locked;
 
-#ifdef CONFIG_SWAP
 		/*
 		 * Anonymous process memory has backing store?
 		 * Try to allocate it some swap space here.
@@ -625,20 +627,10 @@ static unsigned long shrink_page_list(struct list_head *page_list,
 		if (PageAnon(page) && !PageSwapCache(page)) {
 			if (!(sc->gfp_mask & __GFP_IO))
 				goto keep_locked;
-			switch (try_to_munlock(page)) {
-			case SWAP_FAIL:		/* shouldn't happen */
-			case SWAP_AGAIN:
-				goto keep_locked;
-			case SWAP_MLOCK:
-				goto cull_mlocked;
-			case SWAP_SUCCESS:
-				; /* fall thru'; add to swap cache */
-			}
-			if (!add_to_swap(page, GFP_ATOMIC))
+			if (!add_to_swap(page))
 				goto activate_locked;
 			may_enter_fs = 1;
 		}
-#endif /* CONFIG_SWAP */
 
 		mapping = page_mapping(page);
 
@@ -752,6 +744,8 @@ free_it:
 		continue;
 
 cull_mlocked:
+		if (PageSwapCache(page))
+			try_to_free_swap(page);
 		unlock_page(page);
 		putback_lru_page(page);
 		continue;
@@ -759,7 +753,7 @@ cull_mlocked:
 activate_locked:
 		/* Not a candidate for swapping, so reclaim swap space. */
 		if (PageSwapCache(page) && vm_swap_full())
-			remove_exclusive_swap_page_ref(page);
+			try_to_free_swap(page);
 		VM_BUG_ON(PageActive(page));
 		SetPageActive(page);
 		pgactivate++;
@@ -1173,11 +1167,6 @@ static inline void note_zone_scanning_priority(struct zone *zone, int priority)
 		zone->prev_priority = priority;
 }
 
-static inline int zone_is_near_oom(struct zone *zone)
-{
-	return zone->pages_scanned >= (zone_lru_pages(zone) * 3);
-}
-
 /*
  * This moves pages from the active list to the inactive list.
  *
@@ -1248,6 +1237,13 @@ static void shrink_active_list(unsigned long nr_pages, struct zone *zone,
 		list_add(&page->lru, &l_inactive);
 	}
 
+	/*
+	 * Move the pages to the [file or anon] inactive list.
+	 */
+	pagevec_init(&pvec, 1);
+	pgmoved = 0;
+	lru = LRU_BASE + file * LRU_FILE;
+
 	spin_lock_irq(&zone->lru_lock);
 	/*
 	 * Count referenced pages from currently used mappings as
@@ -1255,15 +1251,9 @@ static void shrink_active_list(unsigned long nr_pages, struct zone *zone,
 	 * This helps balance scan pressure between file and anonymous
 	 * pages in get_scan_ratio.
 	 */
-	zone->recent_rotated[!!file] += pgmoved;
-
-	/*
-	 * Move the pages to the [file or anon] inactive list.
-	 */
-	pagevec_init(&pvec, 1);
+	if (scan_global_lru(sc))
+		zone->recent_rotated[!!file] += pgmoved;
 
-	pgmoved = 0;
-	lru = LRU_BASE + file * LRU_FILE;
 	while (!list_empty(&l_inactive)) {
 		page = lru_to_page(&l_inactive);
 		prefetchw_prev_lru_page(page, &l_inactive, flags);
@@ -1336,12 +1326,6 @@ static void get_scan_ratio(struct zone *zone, struct scan_control *sc,
 	unsigned long anon_prio, file_prio;
 	unsigned long ap, fp;
 
-	anon  = zone_page_state(zone, NR_ACTIVE_ANON) +
-		zone_page_state(zone, NR_INACTIVE_ANON);
-	file  = zone_page_state(zone, NR_ACTIVE_FILE) +
-		zone_page_state(zone, NR_INACTIVE_FILE);
-	free  = zone_page_state(zone, NR_FREE_PAGES);
-
 	/* If we have no swap space, do not bother scanning anon pages. */
 	if (nr_swap_pages <= 0) {
 		percent[0] = 0;
@@ -1349,6 +1333,12 @@ static void get_scan_ratio(struct zone *zone, struct scan_control *sc,
 		return;
 	}
 
+	anon  = zone_page_state(zone, NR_ACTIVE_ANON) +
+		zone_page_state(zone, NR_INACTIVE_ANON);
+	file  = zone_page_state(zone, NR_ACTIVE_FILE) +
+		zone_page_state(zone, NR_INACTIVE_FILE);
+	free  = zone_page_state(zone, NR_FREE_PAGES);
+
 	/* If we have very few page cache pages, force-scan anon pages. */
 	if (unlikely(file + free <= zone->pages_high)) {
 		percent[0] = 100;
@@ -1408,14 +1398,15 @@ static void get_scan_ratio(struct zone *zone, struct scan_control *sc,
 /*
  * This is a basic per-zone page freer.  Used by both kswapd and direct reclaim.
  */
-static unsigned long shrink_zone(int priority, struct zone *zone,
+static void shrink_zone(int priority, struct zone *zone,
 				struct scan_control *sc)
 {
 	unsigned long nr[NR_LRU_LISTS];
 	unsigned long nr_to_scan;
-	unsigned long nr_reclaimed = 0;
 	unsigned long percent[2];	/* anon @ 0; file @ 1 */
 	enum lru_list l;
+	unsigned long nr_reclaimed = sc->nr_reclaimed;
+	unsigned long swap_cluster_max = sc->swap_cluster_max;
 
 	get_scan_ratio(zone, sc, percent);
 
@@ -1431,7 +1422,7 @@ static unsigned long shrink_zone(int priority, struct zone *zone,
 			}
 			zone->lru[l].nr_scan += scan;
 			nr[l] = zone->lru[l].nr_scan;
-			if (nr[l] >= sc->swap_cluster_max)
+			if (nr[l] >= swap_cluster_max)
 				zone->lru[l].nr_scan = 0;
 			else
 				nr[l] = 0;
@@ -1450,16 +1441,28 @@ static unsigned long shrink_zone(int priority, struct zone *zone,
 					nr[LRU_INACTIVE_FILE]) {
 		for_each_evictable_lru(l) {
 			if (nr[l]) {
-				nr_to_scan = min(nr[l],
-					(unsigned long)sc->swap_cluster_max);
+				nr_to_scan = min(nr[l], swap_cluster_max);
 				nr[l] -= nr_to_scan;
 
 				nr_reclaimed += shrink_list(l, nr_to_scan,
-							zone, sc, priority);
+							    zone, sc, priority);
 			}
 		}
+		/*
+		 * On large memory systems, scan >> priority can become
+		 * really large. This is fine for the starting priority;
+		 * we want to put equal scanning pressure on each zone.
+		 * However, if the VM has a harder time of freeing pages,
+		 * with multiple processes reclaiming pages, the total
+		 * freeing target can get unreasonably large.
+		 */
+		if (nr_reclaimed > swap_cluster_max &&
+			priority < DEF_PRIORITY && !current_is_kswapd())
+			break;
 	}
 
+	sc->nr_reclaimed = nr_reclaimed;
+
 	/*
 	 * Even if we did not try to evict anon pages at all, we want to
 	 * rebalance the anon lru active/inactive ratio.
@@ -1470,7 +1473,6 @@ static unsigned long shrink_zone(int priority, struct zone *zone,
 		shrink_active_list(SWAP_CLUSTER_MAX, zone, sc, priority, 0);
 
 	throttle_vm_writeout(sc->gfp_mask);
-	return nr_reclaimed;
 }
 
 /*
@@ -1484,16 +1486,13 @@ static unsigned long shrink_zone(int priority, struct zone *zone,
  * b) The zones may be over pages_high but they must go *over* pages_high to
  *    satisfy the `incremental min' zone defense algorithm.
  *
- * Returns the number of reclaimed pages.
- *
  * If a zone is deemed to be full of pinned pages then just give it a light
  * scan then give up on it.
  */
-static unsigned long shrink_zones(int priority, struct zonelist *zonelist,
+static void shrink_zones(int priority, struct zonelist *zonelist,
 					struct scan_control *sc)
 {
 	enum zone_type high_zoneidx = gfp_zone(sc->gfp_mask);
-	unsigned long nr_reclaimed = 0;
 	struct zoneref *z;
 	struct zone *zone;
 
@@ -1524,10 +1523,8 @@ static unsigned long shrink_zones(int priority, struct zonelist *zonelist,
 							priority);
 		}
 
-		nr_reclaimed += shrink_zone(priority, zone, sc);
+		shrink_zone(priority, zone, sc);
 	}
-
-	return nr_reclaimed;
 }
 
 /*
@@ -1552,7 +1549,6 @@ static unsigned long do_try_to_free_pages(struct zonelist *zonelist,
 	int priority;
 	unsigned long ret = 0;
 	unsigned long total_scanned = 0;
-	unsigned long nr_reclaimed = 0;
 	struct reclaim_state *reclaim_state = current->reclaim_state;
 	unsigned long lru_pages = 0;
 	struct zoneref *z;
@@ -1580,7 +1576,7 @@ static unsigned long do_try_to_free_pages(struct zonelist *zonelist,
 		sc->nr_scanned = 0;
 		if (!priority)
 			disable_swap_token();
-		nr_reclaimed += shrink_zones(priority, zonelist, sc);
+		shrink_zones(priority, zonelist, sc);
 		/*
 		 * Don't shrink slabs when reclaiming memory from
 		 * over limit cgroups
@@ -1588,13 +1584,13 @@ static unsigned long do_try_to_free_pages(struct zonelist *zonelist,
 		if (scan_global_lru(sc)) {
 			shrink_slab(sc->nr_scanned, sc->gfp_mask, lru_pages);
 			if (reclaim_state) {
-				nr_reclaimed += reclaim_state->reclaimed_slab;
+				sc->nr_reclaimed += reclaim_state->reclaimed_slab;
 				reclaim_state->reclaimed_slab = 0;
 			}
 		}
 		total_scanned += sc->nr_scanned;
-		if (nr_reclaimed >= sc->swap_cluster_max) {
-			ret = nr_reclaimed;
+		if (sc->nr_reclaimed >= sc->swap_cluster_max) {
+			ret = sc->nr_reclaimed;
 			goto out;
 		}
 
@@ -1617,7 +1613,7 @@ static unsigned long do_try_to_free_pages(struct zonelist *zonelist,
 	}
 	/* top priority shrink_zones still had more to do? don't OOM, then */
 	if (!sc->all_unreclaimable && scan_global_lru(sc))
-		ret = nr_reclaimed;
+		ret = sc->nr_reclaimed;
 out:
 	/*
 	 * Now that we've scanned all the zones at this priority level, note
@@ -1712,7 +1708,6 @@ static unsigned long balance_pgdat(pg_data_t *pgdat, int order)
 	int priority;
 	int i;
 	unsigned long total_scanned;
-	unsigned long nr_reclaimed;
 	struct reclaim_state *reclaim_state = current->reclaim_state;
 	struct scan_control sc = {
 		.gfp_mask = GFP_KERNEL,
@@ -1731,7 +1726,7 @@ static unsigned long balance_pgdat(pg_data_t *pgdat, int order)
 
 loop_again:
 	total_scanned = 0;
-	nr_reclaimed = 0;
+	sc.nr_reclaimed = 0;
 	sc.may_writepage = !laptop_mode;
 	count_vm_event(PAGEOUTRUN);
 
@@ -1817,11 +1812,11 @@ loop_again:
 			 */
 			if (!zone_watermark_ok(zone, order, 8*zone->pages_high,
 						end_zone, 0))
-				nr_reclaimed += shrink_zone(priority, zone, &sc);
+				shrink_zone(priority, zone, &sc);
 			reclaim_state->reclaimed_slab = 0;
 			nr_slab = shrink_slab(sc.nr_scanned, GFP_KERNEL,
 						lru_pages);
-			nr_reclaimed += reclaim_state->reclaimed_slab;
+			sc.nr_reclaimed += reclaim_state->reclaimed_slab;
 			total_scanned += sc.nr_scanned;
 			if (zone_is_all_unreclaimable(zone))
 				continue;
@@ -1835,7 +1830,7 @@ loop_again:
 			 * even in laptop mode
 			 */
 			if (total_scanned > SWAP_CLUSTER_MAX * 2 &&
-			    total_scanned > nr_reclaimed + nr_reclaimed / 2)
+			    total_scanned > sc.nr_reclaimed + sc.nr_reclaimed / 2)
 				sc.may_writepage = 1;
 		}
 		if (all_zones_ok)
@@ -1853,7 +1848,7 @@ loop_again:
 		 * matches the direct reclaim path behaviour in terms of impact
 		 * on zone->*_priority.
 		 */
-		if (nr_reclaimed >= SWAP_CLUSTER_MAX)
+		if (sc.nr_reclaimed >= SWAP_CLUSTER_MAX)
 			break;
 	}
 out:
@@ -1872,10 +1867,27 @@ out:
 
 		try_to_freeze();
 
+		/*
+		 * Fragmentation may mean that the system cannot be
+		 * rebalanced for high-order allocations in all zones.
+		 * At this point, if nr_reclaimed < SWAP_CLUSTER_MAX,
+		 * it means the zones have been fully scanned and are still
+		 * not balanced. For high-order allocations, there is
+		 * little point trying all over again as kswapd may
+		 * infinite loop.
+		 *
+		 * Instead, recheck all watermarks at order-0 as they
+		 * are the most important. If watermarks are ok, kswapd will go
+		 * back to sleep. High-order users can still perform direct
+		 * reclaim if they wish.
+		 */
+		if (sc.nr_reclaimed < SWAP_CLUSTER_MAX)
+			order = sc.order = 0;
+
 		goto loop_again;
 	}
 
-	return nr_reclaimed;
+	return sc.nr_reclaimed;
 }
 
 /*
@@ -2227,7 +2239,6 @@ static int __zone_reclaim(struct zone *zone, gfp_t gfp_mask, unsigned int order)
 	struct task_struct *p = current;
 	struct reclaim_state reclaim_state;
 	int priority;
-	unsigned long nr_reclaimed = 0;
 	struct scan_control sc = {
 		.may_writepage = !!(zone_reclaim_mode & RECLAIM_WRITE),
 		.may_swap = !!(zone_reclaim_mode & RECLAIM_SWAP),
@@ -2260,9 +2271,9 @@ static int __zone_reclaim(struct zone *zone, gfp_t gfp_mask, unsigned int order)
 		priority = ZONE_RECLAIM_PRIORITY;
 		do {
 			note_zone_scanning_priority(zone, priority);
-			nr_reclaimed += shrink_zone(priority, zone, &sc);
+			shrink_zone(priority, zone, &sc);
 			priority--;
-		} while (priority >= 0 && nr_reclaimed < nr_pages);
+		} while (priority >= 0 && sc.nr_reclaimed < nr_pages);
 	}
 
 	slab_reclaimable = zone_page_state(zone, NR_SLAB_RECLAIMABLE);
@@ -2286,13 +2297,13 @@ static int __zone_reclaim(struct zone *zone, gfp_t gfp_mask, unsigned int order)
 		 * Update nr_reclaimed by the number of slab pages we
 		 * reclaimed from this zone.
 		 */
-		nr_reclaimed += slab_reclaimable -
+		sc.nr_reclaimed += slab_reclaimable -
 			zone_page_state(zone, NR_SLAB_RECLAIMABLE);
 	}
 
 	p->reclaim_state = NULL;
 	current->flags &= ~(PF_MEMALLOC | PF_SWAPWRITE);
-	return nr_reclaimed >= nr_pages;
+	return sc.nr_reclaimed >= nr_pages;
 }
 
 int zone_reclaim(struct zone *zone, gfp_t gfp_mask, unsigned int order)
@@ -2472,7 +2483,7 @@ void scan_mapping_unevictable_pages(struct address_space *mapping)
  * back onto @zone's unevictable list.
  */
 #define SCAN_UNEVICTABLE_BATCH_SIZE 16UL /* arbitrary lock hold batch size */
-void scan_zone_unevictable_pages(struct zone *zone)
+static void scan_zone_unevictable_pages(struct zone *zone)
 {
 	struct list_head *l_unevictable = &zone->lru[LRU_UNEVICTABLE].list;
 	unsigned long scan;
@@ -2514,7 +2525,7 @@ void scan_zone_unevictable_pages(struct zone *zone)
  * that has possibly/probably made some previously unevictable pages
  * evictable.
  */
-void scan_all_zones_unevictable_pages(void)
+static void scan_all_zones_unevictable_pages(void)
 {
 	struct zone *zone;
 
diff --git a/net/Kconfig b/net/Kconfig
index 6ec2cce7c167..bf2776018f71 100644
--- a/net/Kconfig
+++ b/net/Kconfig
@@ -254,6 +254,8 @@ source "net/mac80211/Kconfig"
 
 endif # WIRELESS
 
+source "net/wimax/Kconfig"
+
 source "net/rfkill/Kconfig"
 source "net/9p/Kconfig"
 
diff --git a/net/Makefile b/net/Makefile
index ba4460432b7c..0fcce89d7169 100644
--- a/net/Makefile
+++ b/net/Makefile
@@ -63,3 +63,4 @@ endif
 ifeq ($(CONFIG_NET),y)
 obj-$(CONFIG_SYSCTL)		+= sysctl_net.o
 endif
+obj-$(CONFIG_WIMAX)		+= wimax/
diff --git a/net/ipv4/cipso_ipv4.c b/net/ipv4/cipso_ipv4.c
index e52799047a5f..6bb2635b5ded 100644
--- a/net/ipv4/cipso_ipv4.c
+++ b/net/ipv4/cipso_ipv4.c
@@ -38,6 +38,7 @@
 #include <linux/spinlock.h>
 #include <linux/string.h>
 #include <linux/jhash.h>
+#include <linux/audit.h>
 #include <net/ip.h>
 #include <net/icmp.h>
 #include <net/tcp.h>
@@ -449,6 +450,7 @@ static struct cipso_v4_doi *cipso_v4_doi_search(u32 doi)
 /**
  * cipso_v4_doi_add - Add a new DOI to the CIPSO protocol engine
  * @doi_def: the DOI structure
+ * @audit_info: NetLabel audit information
  *
  * Description:
  * The caller defines a new DOI for use by the CIPSO engine and calls this
@@ -458,50 +460,78 @@ static struct cipso_v4_doi *cipso_v4_doi_search(u32 doi)
  * zero on success and non-zero on failure.
  *
  */
-int cipso_v4_doi_add(struct cipso_v4_doi *doi_def)
+int cipso_v4_doi_add(struct cipso_v4_doi *doi_def,
+		     struct netlbl_audit *audit_info)
 {
+	int ret_val = -EINVAL;
 	u32 iter;
+	u32 doi;
+	u32 doi_type;
+	struct audit_buffer *audit_buf;
+
+	doi = doi_def->doi;
+	doi_type = doi_def->type;
 
 	if (doi_def == NULL || doi_def->doi == CIPSO_V4_DOI_UNKNOWN)
-		return -EINVAL;
+		goto doi_add_return;
 	for (iter = 0; iter < CIPSO_V4_TAG_MAXCNT; iter++) {
 		switch (doi_def->tags[iter]) {
 		case CIPSO_V4_TAG_RBITMAP:
 			break;
 		case CIPSO_V4_TAG_RANGE:
-			if (doi_def->type != CIPSO_V4_MAP_PASS)
-				return -EINVAL;
-			break;
-		case CIPSO_V4_TAG_INVALID:
-			if (iter == 0)
-				return -EINVAL;
-			break;
 		case CIPSO_V4_TAG_ENUM:
 			if (doi_def->type != CIPSO_V4_MAP_PASS)
-				return -EINVAL;
+				goto doi_add_return;
 			break;
 		case CIPSO_V4_TAG_LOCAL:
 			if (doi_def->type != CIPSO_V4_MAP_LOCAL)
-				return -EINVAL;
+				goto doi_add_return;
+			break;
+		case CIPSO_V4_TAG_INVALID:
+			if (iter == 0)
+				goto doi_add_return;
 			break;
 		default:
-			return -EINVAL;
+			goto doi_add_return;
 		}
 	}
 
 	atomic_set(&doi_def->refcount, 1);
 
 	spin_lock(&cipso_v4_doi_list_lock);
-	if (cipso_v4_doi_search(doi_def->doi) != NULL)
-		goto doi_add_failure;
+	if (cipso_v4_doi_search(doi_def->doi) != NULL) {
+		spin_unlock(&cipso_v4_doi_list_lock);
+		ret_val = -EEXIST;
+		goto doi_add_return;
+	}
 	list_add_tail_rcu(&doi_def->list, &cipso_v4_doi_list);
 	spin_unlock(&cipso_v4_doi_list_lock);
+	ret_val = 0;
 
-	return 0;
+doi_add_return:
+	audit_buf = netlbl_audit_start(AUDIT_MAC_CIPSOV4_ADD, audit_info);
+	if (audit_buf != NULL) {
+		const char *type_str;
+		switch (doi_type) {
+		case CIPSO_V4_MAP_TRANS:
+			type_str = "trans";
+			break;
+		case CIPSO_V4_MAP_PASS:
+			type_str = "pass";
+			break;
+		case CIPSO_V4_MAP_LOCAL:
+			type_str = "local";
+			break;
+		default:
+			type_str = "(unknown)";
+		}
+		audit_log_format(audit_buf,
+				 " cipso_doi=%u cipso_type=%s res=%u",
+				 doi, type_str, ret_val == 0 ? 1 : 0);
+		audit_log_end(audit_buf);
+	}
 
-doi_add_failure:
-	spin_unlock(&cipso_v4_doi_list_lock);
-	return -EEXIST;
+	return ret_val;
 }
 
 /**
@@ -559,25 +589,39 @@ static void cipso_v4_doi_free_rcu(struct rcu_head *entry)
  */
 int cipso_v4_doi_remove(u32 doi, struct netlbl_audit *audit_info)
 {
+	int ret_val;
 	struct cipso_v4_doi *doi_def;
+	struct audit_buffer *audit_buf;
 
 	spin_lock(&cipso_v4_doi_list_lock);
 	doi_def = cipso_v4_doi_search(doi);
 	if (doi_def == NULL) {
 		spin_unlock(&cipso_v4_doi_list_lock);
-		return -ENOENT;
+		ret_val = -ENOENT;
+		goto doi_remove_return;
 	}
 	if (!atomic_dec_and_test(&doi_def->refcount)) {
 		spin_unlock(&cipso_v4_doi_list_lock);
-		return -EBUSY;
+		ret_val = -EBUSY;
+		goto doi_remove_return;
 	}
 	list_del_rcu(&doi_def->list);
 	spin_unlock(&cipso_v4_doi_list_lock);
 
 	cipso_v4_cache_invalidate();
 	call_rcu(&doi_def->rcu, cipso_v4_doi_free_rcu);
+	ret_val = 0;
+
+doi_remove_return:
+	audit_buf = netlbl_audit_start(AUDIT_MAC_CIPSOV4_DEL, audit_info);
+	if (audit_buf != NULL) {
+		audit_log_format(audit_buf,
+				 " cipso_doi=%u res=%u",
+				 doi, ret_val == 0 ? 1 : 0);
+		audit_log_end(audit_buf);
+	}
 
-	return 0;
+	return ret_val;
 }
 
 /**
diff --git a/net/iucv/iucv.c b/net/iucv/iucv.c
index 032f61e98595..a35240f61ec3 100644
--- a/net/iucv/iucv.c
+++ b/net/iucv/iucv.c
@@ -50,7 +50,6 @@
 #include <asm/ebcdic.h>
 #include <asm/io.h>
 #include <asm/s390_ext.h>
-#include <asm/s390_rdev.h>
 #include <asm/smp.h>
 
 /*
@@ -1696,7 +1695,7 @@ static int __init iucv_init(void)
 	rc = register_external_interrupt(0x4000, iucv_external_interrupt);
 	if (rc)
 		goto out;
-	iucv_root = s390_root_dev_register("iucv");
+	iucv_root = root_device_register("iucv");
 	if (IS_ERR(iucv_root)) {
 		rc = PTR_ERR(iucv_root);
 		goto out_int;
@@ -1740,7 +1739,7 @@ out_free:
 		kfree(iucv_irq_data[cpu]);
 		iucv_irq_data[cpu] = NULL;
 	}
-	s390_root_dev_unregister(iucv_root);
+	root_device_unregister(iucv_root);
 out_int:
 	unregister_external_interrupt(0x4000, iucv_external_interrupt);
 out:
@@ -1770,7 +1769,7 @@ static void __exit iucv_exit(void)
 		kfree(iucv_irq_data[cpu]);
 		iucv_irq_data[cpu] = NULL;
 	}
-	s390_root_dev_unregister(iucv_root);
+	root_device_unregister(iucv_root);
 	bus_unregister(&iucv_bus);
 	unregister_external_interrupt(0x4000, iucv_external_interrupt);
 }
diff --git a/net/netlabel/netlabel_cipso_v4.c b/net/netlabel/netlabel_cipso_v4.c
index fff32b70efa9..bf1ab1a6790d 100644
--- a/net/netlabel/netlabel_cipso_v4.c
+++ b/net/netlabel/netlabel_cipso_v4.c
@@ -130,6 +130,7 @@ static int netlbl_cipsov4_add_common(struct genl_info *info,
 /**
  * netlbl_cipsov4_add_std - Adds a CIPSO V4 DOI definition
  * @info: the Generic NETLINK info block
+ * @audit_info: NetLabel audit information
  *
  * Description:
  * Create a new CIPSO_V4_MAP_TRANS DOI definition based on the given ADD
@@ -137,7 +138,8 @@ static int netlbl_cipsov4_add_common(struct genl_info *info,
  * non-zero on error.
  *
  */
-static int netlbl_cipsov4_add_std(struct genl_info *info)
+static int netlbl_cipsov4_add_std(struct genl_info *info,
+				  struct netlbl_audit *audit_info)
 {
 	int ret_val = -EINVAL;
 	struct cipso_v4_doi *doi_def = NULL;
@@ -316,7 +318,7 @@ static int netlbl_cipsov4_add_std(struct genl_info *info)
 			}
 	}
 
-	ret_val = cipso_v4_doi_add(doi_def);
+	ret_val = cipso_v4_doi_add(doi_def, audit_info);
 	if (ret_val != 0)
 		goto add_std_failure;
 	return 0;
@@ -330,6 +332,7 @@ add_std_failure:
 /**
  * netlbl_cipsov4_add_pass - Adds a CIPSO V4 DOI definition
  * @info: the Generic NETLINK info block
+ * @audit_info: NetLabel audit information
  *
  * Description:
  * Create a new CIPSO_V4_MAP_PASS DOI definition based on the given ADD message
@@ -337,7 +340,8 @@ add_std_failure:
  * error.
  *
  */
-static int netlbl_cipsov4_add_pass(struct genl_info *info)
+static int netlbl_cipsov4_add_pass(struct genl_info *info,
+				   struct netlbl_audit *audit_info)
 {
 	int ret_val;
 	struct cipso_v4_doi *doi_def = NULL;
@@ -354,7 +358,7 @@ static int netlbl_cipsov4_add_pass(struct genl_info *info)
 	if (ret_val != 0)
 		goto add_pass_failure;
 
-	ret_val = cipso_v4_doi_add(doi_def);
+	ret_val = cipso_v4_doi_add(doi_def, audit_info);
 	if (ret_val != 0)
 		goto add_pass_failure;
 	return 0;
@@ -367,6 +371,7 @@ add_pass_failure:
 /**
  * netlbl_cipsov4_add_local - Adds a CIPSO V4 DOI definition
  * @info: the Generic NETLINK info block
+ * @audit_info: NetLabel audit information
  *
  * Description:
  * Create a new CIPSO_V4_MAP_LOCAL DOI definition based on the given ADD
@@ -374,7 +379,8 @@ add_pass_failure:
  * non-zero on error.
  *
  */
-static int netlbl_cipsov4_add_local(struct genl_info *info)
+static int netlbl_cipsov4_add_local(struct genl_info *info,
+				    struct netlbl_audit *audit_info)
 {
 	int ret_val;
 	struct cipso_v4_doi *doi_def = NULL;
@@ -391,7 +397,7 @@ static int netlbl_cipsov4_add_local(struct genl_info *info)
 	if (ret_val != 0)
 		goto add_local_failure;
 
-	ret_val = cipso_v4_doi_add(doi_def);
+	ret_val = cipso_v4_doi_add(doi_def, audit_info);
 	if (ret_val != 0)
 		goto add_local_failure;
 	return 0;
@@ -415,48 +421,31 @@ static int netlbl_cipsov4_add(struct sk_buff *skb, struct genl_info *info)
 
 {
 	int ret_val = -EINVAL;
-	u32 type;
-	u32 doi;
 	const char *type_str = "(unknown)";
-	struct audit_buffer *audit_buf;
 	struct netlbl_audit audit_info;
 
 	if (!info->attrs[NLBL_CIPSOV4_A_DOI] ||
 	    !info->attrs[NLBL_CIPSOV4_A_MTYPE])
 		return -EINVAL;
 
-	doi = nla_get_u32(info->attrs[NLBL_CIPSOV4_A_DOI]);
 	netlbl_netlink_auditinfo(skb, &audit_info);
-
-	type = nla_get_u32(info->attrs[NLBL_CIPSOV4_A_MTYPE]);
-	switch (type) {
+	switch (nla_get_u32(info->attrs[NLBL_CIPSOV4_A_MTYPE])) {
 	case CIPSO_V4_MAP_TRANS:
 		type_str = "trans";
-		ret_val = netlbl_cipsov4_add_std(info);
+		ret_val = netlbl_cipsov4_add_std(info, &audit_info);
 		break;
 	case CIPSO_V4_MAP_PASS:
 		type_str = "pass";
-		ret_val = netlbl_cipsov4_add_pass(info);
+		ret_val = netlbl_cipsov4_add_pass(info, &audit_info);
 		break;
 	case CIPSO_V4_MAP_LOCAL:
 		type_str = "local";
-		ret_val = netlbl_cipsov4_add_local(info);
+		ret_val = netlbl_cipsov4_add_local(info, &audit_info);
 		break;
 	}
 	if (ret_val == 0)
 		atomic_inc(&netlabel_mgmt_protocount);
 
-	audit_buf = netlbl_audit_start_common(AUDIT_MAC_CIPSOV4_ADD,
-					      &audit_info);
-	if (audit_buf != NULL) {
-		audit_log_format(audit_buf,
-				 " cipso_doi=%u cipso_type=%s res=%u",
-				 doi,
-				 type_str,
-				 ret_val == 0 ? 1 : 0);
-		audit_log_end(audit_buf);
-	}
-
 	return ret_val;
 }
 
@@ -725,9 +714,7 @@ static int netlbl_cipsov4_remove_cb(struct netlbl_dom_map *entry, void *arg)
 static int netlbl_cipsov4_remove(struct sk_buff *skb, struct genl_info *info)
 {
 	int ret_val = -EINVAL;
-	u32 doi = 0;
 	struct netlbl_domhsh_walk_arg cb_arg;
-	struct audit_buffer *audit_buf;
 	struct netlbl_audit audit_info;
 	u32 skip_bkt = 0;
 	u32 skip_chain = 0;
@@ -735,29 +722,17 @@ static int netlbl_cipsov4_remove(struct sk_buff *skb, struct genl_info *info)
 	if (!info->attrs[NLBL_CIPSOV4_A_DOI])
 		return -EINVAL;
 
-	doi = nla_get_u32(info->attrs[NLBL_CIPSOV4_A_DOI]);
 	netlbl_netlink_auditinfo(skb, &audit_info);
-
-	cb_arg.doi = doi;
+	cb_arg.doi = nla_get_u32(info->attrs[NLBL_CIPSOV4_A_DOI]);
 	cb_arg.audit_info = &audit_info;
 	ret_val = netlbl_domhsh_walk(&skip_bkt, &skip_chain,
 				     netlbl_cipsov4_remove_cb, &cb_arg);
 	if (ret_val == 0 || ret_val == -ENOENT) {
-		ret_val = cipso_v4_doi_remove(doi, &audit_info);
+		ret_val = cipso_v4_doi_remove(cb_arg.doi, &audit_info);
 		if (ret_val == 0)
 			atomic_dec(&netlabel_mgmt_protocount);
 	}
 
-	audit_buf = netlbl_audit_start_common(AUDIT_MAC_CIPSOV4_DEL,
-					      &audit_info);
-	if (audit_buf != NULL) {
-		audit_log_format(audit_buf,
-				 " cipso_doi=%u res=%u",
-				 doi,
-				 ret_val == 0 ? 1 : 0);
-		audit_log_end(audit_buf);
-	}
-
 	return ret_val;
 }
 
diff --git a/net/netlabel/netlabel_domainhash.c b/net/netlabel/netlabel_domainhash.c
index 5fadf10e5ddf..7a10bbe02c13 100644
--- a/net/netlabel/netlabel_domainhash.c
+++ b/net/netlabel/netlabel_domainhash.c
@@ -483,6 +483,73 @@ int netlbl_domhsh_remove_entry(struct netlbl_dom_map *entry,
 }
 
 /**
+ * netlbl_domhsh_remove_af4 - Removes an address selector entry
+ * @domain: the domain
+ * @addr: IPv4 address
+ * @mask: IPv4 address mask
+ * @audit_info: NetLabel audit information
+ *
+ * Description:
+ * Removes an individual address selector from a domain mapping and potentially
+ * the entire mapping if it is empty.  Returns zero on success, negative values
+ * on failure.
+ *
+ */
+int netlbl_domhsh_remove_af4(const char *domain,
+			     const struct in_addr *addr,
+			     const struct in_addr *mask,
+			     struct netlbl_audit *audit_info)
+{
+	struct netlbl_dom_map *entry_map;
+	struct netlbl_af4list *entry_addr;
+	struct netlbl_af4list *iter4;
+#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
+	struct netlbl_af6list *iter6;
+#endif /* IPv6 */
+	struct netlbl_domaddr4_map *entry;
+
+	rcu_read_lock();
+
+	if (domain)
+		entry_map = netlbl_domhsh_search(domain);
+	else
+		entry_map = netlbl_domhsh_search_def(domain);
+	if (entry_map == NULL || entry_map->type != NETLBL_NLTYPE_ADDRSELECT)
+		goto remove_af4_failure;
+
+	spin_lock(&netlbl_domhsh_lock);
+	entry_addr = netlbl_af4list_remove(addr->s_addr, mask->s_addr,
+					   &entry_map->type_def.addrsel->list4);
+	spin_unlock(&netlbl_domhsh_lock);
+
+	if (entry_addr == NULL)
+		goto remove_af4_failure;
+	netlbl_af4list_foreach_rcu(iter4, &entry_map->type_def.addrsel->list4)
+		goto remove_af4_single_addr;
+#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
+	netlbl_af6list_foreach_rcu(iter6, &entry_map->type_def.addrsel->list6)
+		goto remove_af4_single_addr;
+#endif /* IPv6 */
+	/* the domain mapping is empty so remove it from the mapping table */
+	netlbl_domhsh_remove_entry(entry_map, audit_info);
+
+remove_af4_single_addr:
+	rcu_read_unlock();
+	/* yick, we can't use call_rcu here because we don't have a rcu head
+	 * pointer but hopefully this should be a rare case so the pause
+	 * shouldn't be a problem */
+	synchronize_rcu();
+	entry = netlbl_domhsh_addr4_entry(entry_addr);
+	cipso_v4_doi_putdef(entry->type_def.cipsov4);
+	kfree(entry);
+	return 0;
+
+remove_af4_failure:
+	rcu_read_unlock();
+	return -ENOENT;
+}
+
+/**
  * netlbl_domhsh_remove - Removes an entry from the domain hash table
  * @domain: the domain to remove
  * @audit_info: NetLabel audit information
diff --git a/net/netlabel/netlabel_domainhash.h b/net/netlabel/netlabel_domainhash.h
index bfcb6763a1a1..0261dda3f2d2 100644
--- a/net/netlabel/netlabel_domainhash.h
+++ b/net/netlabel/netlabel_domainhash.h
@@ -90,6 +90,10 @@ int netlbl_domhsh_add_default(struct netlbl_dom_map *entry,
 			      struct netlbl_audit *audit_info);
 int netlbl_domhsh_remove_entry(struct netlbl_dom_map *entry,
 			       struct netlbl_audit *audit_info);
+int netlbl_domhsh_remove_af4(const char *domain,
+			     const struct in_addr *addr,
+			     const struct in_addr *mask,
+			     struct netlbl_audit *audit_info);
 int netlbl_domhsh_remove(const char *domain, struct netlbl_audit *audit_info);
 int netlbl_domhsh_remove_default(struct netlbl_audit *audit_info);
 struct netlbl_dom_map *netlbl_domhsh_getentry(const char *domain);
diff --git a/net/netlabel/netlabel_kapi.c b/net/netlabel/netlabel_kapi.c
index b32eceb3ab0d..fd9229db075c 100644
--- a/net/netlabel/netlabel_kapi.c
+++ b/net/netlabel/netlabel_kapi.c
@@ -31,7 +31,10 @@
 #include <linux/init.h>
 #include <linux/types.h>
 #include <linux/audit.h>
+#include <linux/in.h>
+#include <linux/in6.h>
 #include <net/ip.h>
+#include <net/ipv6.h>
 #include <net/netlabel.h>
 #include <net/cipso_ipv4.h>
 #include <asm/bug.h>
@@ -42,6 +45,7 @@
 #include "netlabel_cipso_v4.h"
 #include "netlabel_user.h"
 #include "netlabel_mgmt.h"
+#include "netlabel_addrlist.h"
 
 /*
  * Configuration Functions
@@ -50,6 +54,9 @@
 /**
  * netlbl_cfg_map_del - Remove a NetLabel/LSM domain mapping
  * @domain: the domain mapping to remove
+ * @family: address family
+ * @addr: IP address
+ * @mask: IP address mask
  * @audit_info: NetLabel audit information
  *
  * Description:
@@ -58,14 +65,32 @@
  * values on failure.
  *
  */
-int netlbl_cfg_map_del(const char *domain, struct netlbl_audit *audit_info)
+int netlbl_cfg_map_del(const char *domain,
+		       u16 family,
+		       const void *addr,
+		       const void *mask,
+		       struct netlbl_audit *audit_info)
 {
-	return netlbl_domhsh_remove(domain, audit_info);
+	if (addr == NULL && mask == NULL) {
+		return netlbl_domhsh_remove(domain, audit_info);
+	} else if (addr != NULL && mask != NULL) {
+		switch (family) {
+		case AF_INET:
+			return netlbl_domhsh_remove_af4(domain, addr, mask,
+							audit_info);
+		default:
+			return -EPFNOSUPPORT;
+		}
+	} else
+		return -EINVAL;
 }
 
 /**
- * netlbl_cfg_unlbl_add_map - Add an unlabeled NetLabel/LSM domain mapping
+ * netlbl_cfg_unlbl_map_add - Add a new unlabeled mapping
  * @domain: the domain mapping to add
+ * @family: address family
+ * @addr: IP address
+ * @mask: IP address mask
  * @audit_info: NetLabel audit information
  *
  * Description:
@@ -74,11 +99,19 @@ int netlbl_cfg_map_del(const char *domain, struct netlbl_audit *audit_info)
  * negative values on failure.
  *
  */
-int netlbl_cfg_unlbl_add_map(const char *domain,
+int netlbl_cfg_unlbl_map_add(const char *domain,
+			     u16 family,
+			     const void *addr,
+			     const void *mask,
 			     struct netlbl_audit *audit_info)
 {
 	int ret_val = -ENOMEM;
 	struct netlbl_dom_map *entry;
+	struct netlbl_domaddr_map *addrmap = NULL;
+	struct netlbl_domaddr4_map *map4 = NULL;
+	struct netlbl_domaddr6_map *map6 = NULL;
+	const struct in_addr *addr4, *mask4;
+	const struct in6_addr *addr6, *mask6;
 
 	entry = kzalloc(sizeof(*entry), GFP_ATOMIC);
 	if (entry == NULL)
@@ -86,49 +119,225 @@ int netlbl_cfg_unlbl_add_map(const char *domain,
 	if (domain != NULL) {
 		entry->domain = kstrdup(domain, GFP_ATOMIC);
 		if (entry->domain == NULL)
-			goto cfg_unlbl_add_map_failure;
+			goto cfg_unlbl_map_add_failure;
+	}
+
+	if (addr == NULL && mask == NULL)
+		entry->type = NETLBL_NLTYPE_UNLABELED;
+	else if (addr != NULL && mask != NULL) {
+		addrmap = kzalloc(sizeof(*addrmap), GFP_ATOMIC);
+		if (addrmap == NULL)
+			goto cfg_unlbl_map_add_failure;
+		INIT_LIST_HEAD(&addrmap->list4);
+		INIT_LIST_HEAD(&addrmap->list6);
+
+		switch (family) {
+		case AF_INET:
+			addr4 = addr;
+			mask4 = mask;
+			map4 = kzalloc(sizeof(*map4), GFP_ATOMIC);
+			if (map4 == NULL)
+				goto cfg_unlbl_map_add_failure;
+			map4->type = NETLBL_NLTYPE_UNLABELED;
+			map4->list.addr = addr4->s_addr & mask4->s_addr;
+			map4->list.mask = mask4->s_addr;
+			map4->list.valid = 1;
+			ret_val = netlbl_af4list_add(&map4->list,
+						     &addrmap->list4);
+			if (ret_val != 0)
+				goto cfg_unlbl_map_add_failure;
+			break;
+		case AF_INET6:
+			addr6 = addr;
+			mask6 = mask;
+			map6 = kzalloc(sizeof(*map6), GFP_ATOMIC);
+			if (map4 == NULL)
+				goto cfg_unlbl_map_add_failure;
+			map6->type = NETLBL_NLTYPE_UNLABELED;
+			ipv6_addr_copy(&map6->list.addr, addr6);
+			map6->list.addr.s6_addr32[0] &= mask6->s6_addr32[0];
+			map6->list.addr.s6_addr32[1] &= mask6->s6_addr32[1];
+			map6->list.addr.s6_addr32[2] &= mask6->s6_addr32[2];
+			map6->list.addr.s6_addr32[3] &= mask6->s6_addr32[3];
+			ipv6_addr_copy(&map6->list.mask, mask6);
+			map6->list.valid = 1;
+			ret_val = netlbl_af4list_add(&map4->list,
+						     &addrmap->list4);
+			if (ret_val != 0)
+				goto cfg_unlbl_map_add_failure;
+			break;
+		default:
+			goto cfg_unlbl_map_add_failure;
+			break;
+		}
+
+		entry->type_def.addrsel = addrmap;
+		entry->type = NETLBL_NLTYPE_ADDRSELECT;
+	} else {
+		ret_val = -EINVAL;
+		goto cfg_unlbl_map_add_failure;
 	}
-	entry->type = NETLBL_NLTYPE_UNLABELED;
 
 	ret_val = netlbl_domhsh_add(entry, audit_info);
 	if (ret_val != 0)
-		goto cfg_unlbl_add_map_failure;
+		goto cfg_unlbl_map_add_failure;
 
 	return 0;
 
-cfg_unlbl_add_map_failure:
+cfg_unlbl_map_add_failure:
 	if (entry != NULL)
 		kfree(entry->domain);
 	kfree(entry);
+	kfree(addrmap);
+	kfree(map4);
+	kfree(map6);
 	return ret_val;
 }
 
+
+/**
+ * netlbl_cfg_unlbl_static_add - Adds a new static label
+ * @net: network namespace
+ * @dev_name: interface name
+ * @addr: IP address in network byte order (struct in[6]_addr)
+ * @mask: address mask in network byte order (struct in[6]_addr)
+ * @family: address family
+ * @secid: LSM secid value for the entry
+ * @audit_info: NetLabel audit information
+ *
+ * Description:
+ * Adds a new NetLabel static label to be used when protocol provided labels
+ * are not present on incoming traffic.  If @dev_name is NULL then the default
+ * interface will be used.  Returns zero on success, negative values on failure.
+ *
+ */
+int netlbl_cfg_unlbl_static_add(struct net *net,
+				const char *dev_name,
+				const void *addr,
+				const void *mask,
+				u16 family,
+				u32 secid,
+				struct netlbl_audit *audit_info)
+{
+	u32 addr_len;
+
+	switch (family) {
+	case AF_INET:
+		addr_len = sizeof(struct in_addr);
+		break;
+	case AF_INET6:
+		addr_len = sizeof(struct in6_addr);
+		break;
+	default:
+		return -EPFNOSUPPORT;
+	}
+
+	return netlbl_unlhsh_add(net,
+				 dev_name, addr, mask, addr_len,
+				 secid, audit_info);
+}
+
+/**
+ * netlbl_cfg_unlbl_static_del - Removes an existing static label
+ * @net: network namespace
+ * @dev_name: interface name
+ * @addr: IP address in network byte order (struct in[6]_addr)
+ * @mask: address mask in network byte order (struct in[6]_addr)
+ * @family: address family
+ * @secid: LSM secid value for the entry
+ * @audit_info: NetLabel audit information
+ *
+ * Description:
+ * Removes an existing NetLabel static label used when protocol provided labels
+ * are not present on incoming traffic.  If @dev_name is NULL then the default
+ * interface will be used.  Returns zero on success, negative values on failure.
+ *
+ */
+int netlbl_cfg_unlbl_static_del(struct net *net,
+				const char *dev_name,
+				const void *addr,
+				const void *mask,
+				u16 family,
+				struct netlbl_audit *audit_info)
+{
+	u32 addr_len;
+
+	switch (family) {
+	case AF_INET:
+		addr_len = sizeof(struct in_addr);
+		break;
+	case AF_INET6:
+		addr_len = sizeof(struct in6_addr);
+		break;
+	default:
+		return -EPFNOSUPPORT;
+	}
+
+	return netlbl_unlhsh_remove(net,
+				    dev_name, addr, mask, addr_len,
+				    audit_info);
+}
+
+/**
+ * netlbl_cfg_cipsov4_add - Add a new CIPSOv4 DOI definition
+ * @doi_def: CIPSO DOI definition
+ * @audit_info: NetLabel audit information
+ *
+ * Description:
+ * Add a new CIPSO DOI definition as defined by @doi_def.  Returns zero on
+ * success and negative values on failure.
+ *
+ */
+int netlbl_cfg_cipsov4_add(struct cipso_v4_doi *doi_def,
+			   struct netlbl_audit *audit_info)
+{
+	return cipso_v4_doi_add(doi_def, audit_info);
+}
+
+/**
+ * netlbl_cfg_cipsov4_del - Remove an existing CIPSOv4 DOI definition
+ * @doi: CIPSO DOI
+ * @audit_info: NetLabel audit information
+ *
+ * Description:
+ * Remove an existing CIPSO DOI definition matching @doi.  Returns zero on
+ * success and negative values on failure.
+ *
+ */
+void netlbl_cfg_cipsov4_del(u32 doi, struct netlbl_audit *audit_info)
+{
+	cipso_v4_doi_remove(doi, audit_info);
+}
+
 /**
- * netlbl_cfg_cipsov4_add_map - Add a new CIPSOv4 DOI definition and mapping
- * @doi_def: the DOI definition
+ * netlbl_cfg_cipsov4_map_add - Add a new CIPSOv4 DOI mapping
+ * @doi: the CIPSO DOI
  * @domain: the domain mapping to add
+ * @addr: IP address
+ * @mask: IP address mask
  * @audit_info: NetLabel audit information
  *
  * Description:
- * Add a new CIPSOv4 DOI definition and NetLabel/LSM domain mapping for this
- * new DOI definition to the NetLabel subsystem.  A @domain value of NULL adds
- * a new default domain mapping.  Returns zero on success, negative values on
- * failure.
+ * Add a new NetLabel/LSM domain mapping for the given CIPSO DOI to the NetLabel
+ * subsystem.  A @domain value of NULL adds a new default domain mapping.
+ * Returns zero on success, negative values on failure.
  *
  */
-int netlbl_cfg_cipsov4_add_map(struct cipso_v4_doi *doi_def,
+int netlbl_cfg_cipsov4_map_add(u32 doi,
 			       const char *domain,
+			       const struct in_addr *addr,
+			       const struct in_addr *mask,
 			       struct netlbl_audit *audit_info)
 {
 	int ret_val = -ENOMEM;
-	u32 doi;
-	u32 doi_type;
+	struct cipso_v4_doi *doi_def;
 	struct netlbl_dom_map *entry;
-	const char *type_str;
-	struct audit_buffer *audit_buf;
+	struct netlbl_domaddr_map *addrmap = NULL;
+	struct netlbl_domaddr4_map *addrinfo = NULL;
 
-	doi = doi_def->doi;
-	doi_type = doi_def->type;
+	doi_def = cipso_v4_doi_getdef(doi);
+	if (doi_def == NULL)
+		return -ENOENT;
 
 	entry = kzalloc(sizeof(*entry), GFP_ATOMIC);
 	if (entry == NULL)
@@ -136,56 +345,52 @@ int netlbl_cfg_cipsov4_add_map(struct cipso_v4_doi *doi_def,
 	if (domain != NULL) {
 		entry->domain = kstrdup(domain, GFP_ATOMIC);
 		if (entry->domain == NULL)
-			goto cfg_cipsov4_add_map_failure;
+			goto cfg_cipsov4_map_add_failure;
 	}
 
-	ret_val = cipso_v4_doi_add(doi_def);
-	if (ret_val != 0)
-		goto cfg_cipsov4_add_map_failure_remove_doi;
-	entry->type = NETLBL_NLTYPE_CIPSOV4;
-	entry->type_def.cipsov4 = cipso_v4_doi_getdef(doi);
-	if (entry->type_def.cipsov4 == NULL) {
-		ret_val = -ENOENT;
-		goto cfg_cipsov4_add_map_failure_remove_doi;
+	if (addr == NULL && mask == NULL) {
+		entry->type_def.cipsov4 = doi_def;
+		entry->type = NETLBL_NLTYPE_CIPSOV4;
+	} else if (addr != NULL && mask != NULL) {
+		addrmap = kzalloc(sizeof(*addrmap), GFP_ATOMIC);
+		if (addrmap == NULL)
+			goto cfg_cipsov4_map_add_failure;
+		INIT_LIST_HEAD(&addrmap->list4);
+		INIT_LIST_HEAD(&addrmap->list6);
+
+		addrinfo = kzalloc(sizeof(*addrinfo), GFP_ATOMIC);
+		if (addrinfo == NULL)
+			goto cfg_cipsov4_map_add_failure;
+		addrinfo->type_def.cipsov4 = doi_def;
+		addrinfo->type = NETLBL_NLTYPE_CIPSOV4;
+		addrinfo->list.addr = addr->s_addr & mask->s_addr;
+		addrinfo->list.mask = mask->s_addr;
+		addrinfo->list.valid = 1;
+		ret_val = netlbl_af4list_add(&addrinfo->list, &addrmap->list4);
+		if (ret_val != 0)
+			goto cfg_cipsov4_map_add_failure;
+
+		entry->type_def.addrsel = addrmap;
+		entry->type = NETLBL_NLTYPE_ADDRSELECT;
+	} else {
+		ret_val = -EINVAL;
+		goto cfg_cipsov4_map_add_failure;
 	}
+
 	ret_val = netlbl_domhsh_add(entry, audit_info);
 	if (ret_val != 0)
-		goto cfg_cipsov4_add_map_failure_release_doi;
-
-cfg_cipsov4_add_map_return:
-	audit_buf = netlbl_audit_start_common(AUDIT_MAC_CIPSOV4_ADD,
-					      audit_info);
-	if (audit_buf != NULL) {
-		switch (doi_type) {
-		case CIPSO_V4_MAP_TRANS:
-			type_str = "trans";
-			break;
-		case CIPSO_V4_MAP_PASS:
-			type_str = "pass";
-			break;
-		case CIPSO_V4_MAP_LOCAL:
-			type_str = "local";
-			break;
-		default:
-			type_str = "(unknown)";
-		}
-		audit_log_format(audit_buf,
-				 " cipso_doi=%u cipso_type=%s res=%u",
-				 doi, type_str, ret_val == 0 ? 1 : 0);
-		audit_log_end(audit_buf);
-	}
+		goto cfg_cipsov4_map_add_failure;
 
-	return ret_val;
+	return 0;
 
-cfg_cipsov4_add_map_failure_release_doi:
+cfg_cipsov4_map_add_failure:
 	cipso_v4_doi_putdef(doi_def);
-cfg_cipsov4_add_map_failure_remove_doi:
-	cipso_v4_doi_remove(doi, audit_info);
-cfg_cipsov4_add_map_failure:
 	if (entry != NULL)
 		kfree(entry->domain);
 	kfree(entry);
-	goto cfg_cipsov4_add_map_return;
+	kfree(addrmap);
+	kfree(addrinfo);
+	return ret_val;
 }
 
 /*
@@ -691,6 +896,28 @@ int netlbl_cache_add(const struct sk_buff *skb,
 }
 
 /*
+ * Protocol Engine Functions
+ */
+
+/**
+ * netlbl_audit_start - Start an audit message
+ * @type: audit message type
+ * @audit_info: NetLabel audit information
+ *
+ * Description:
+ * Start an audit message using the type specified in @type and fill the audit
+ * message with some fields common to all NetLabel audit messages.  This
+ * function should only be used by protocol engines, not LSMs.  Returns a
+ * pointer to the audit buffer on success, NULL on failure.
+ *
+ */
+struct audit_buffer *netlbl_audit_start(int type,
+					struct netlbl_audit *audit_info)
+{
+	return netlbl_audit_start_common(type, audit_info);
+}
+
+/*
  * Setup Functions
  */
 
diff --git a/net/netlabel/netlabel_unlabeled.c b/net/netlabel/netlabel_unlabeled.c
index 8c0308032178..f3c5c68c6848 100644
--- a/net/netlabel/netlabel_unlabeled.c
+++ b/net/netlabel/netlabel_unlabeled.c
@@ -450,13 +450,13 @@ add_iface_failure:
  * success, negative values on failure.
  *
  */
-static int netlbl_unlhsh_add(struct net *net,
-			     const char *dev_name,
-			     const void *addr,
-			     const void *mask,
-			     u32 addr_len,
-			     u32 secid,
-			     struct netlbl_audit *audit_info)
+int netlbl_unlhsh_add(struct net *net,
+		      const char *dev_name,
+		      const void *addr,
+		      const void *mask,
+		      u32 addr_len,
+		      u32 secid,
+		      struct netlbl_audit *audit_info)
 {
 	int ret_val;
 	int ifindex;
@@ -720,12 +720,12 @@ unlhsh_condremove_failure:
  * Returns zero on success, negative values on failure.
  *
  */
-static int netlbl_unlhsh_remove(struct net *net,
-				const char *dev_name,
-				const void *addr,
-				const void *mask,
-				u32 addr_len,
-				struct netlbl_audit *audit_info)
+int netlbl_unlhsh_remove(struct net *net,
+			 const char *dev_name,
+			 const void *addr,
+			 const void *mask,
+			 u32 addr_len,
+			 struct netlbl_audit *audit_info)
 {
 	int ret_val;
 	struct net_device *dev;
diff --git a/net/netlabel/netlabel_unlabeled.h b/net/netlabel/netlabel_unlabeled.h
index 06b1301ac072..7aba63595137 100644
--- a/net/netlabel/netlabel_unlabeled.h
+++ b/net/netlabel/netlabel_unlabeled.h
@@ -221,6 +221,21 @@ int netlbl_unlabel_genl_init(void);
 /* General Unlabeled init function */
 int netlbl_unlabel_init(u32 size);
 
+/* Static/Fallback label management functions */
+int netlbl_unlhsh_add(struct net *net,
+		      const char *dev_name,
+		      const void *addr,
+		      const void *mask,
+		      u32 addr_len,
+		      u32 secid,
+		      struct netlbl_audit *audit_info);
+int netlbl_unlhsh_remove(struct net *net,
+			 const char *dev_name,
+			 const void *addr,
+			 const void *mask,
+			 u32 addr_len,
+			 struct netlbl_audit *audit_info);
+
 /* Process Unlabeled incoming network packets */
 int netlbl_unlabel_getattr(const struct sk_buff *skb,
 			   u16 family,
diff --git a/net/netlink/genetlink.c b/net/netlink/genetlink.c
index 3e1191cecaf0..1d3dd30099df 100644
--- a/net/netlink/genetlink.c
+++ b/net/netlink/genetlink.c
@@ -225,6 +225,7 @@ void genl_unregister_mc_group(struct genl_family *family,
 	__genl_unregister_mc_group(family, grp);
 	genl_unlock();
 }
+EXPORT_SYMBOL(genl_unregister_mc_group);
 
 static void genl_unregister_mc_groups(struct genl_family *family)
 {
diff --git a/net/sctp/auth.c b/net/sctp/auth.c
index 52db5f60daa0..20c576f530fa 100644
--- a/net/sctp/auth.c
+++ b/net/sctp/auth.c
@@ -141,8 +141,8 @@ void sctp_auth_destroy_keys(struct list_head *keys)
 /* Compare two byte vectors as numbers.  Return values
  * are:
  * 	  0 - vectors are equal
- * 	< 0 - vector 1 is smaller then vector2
- * 	> 0 - vector 1 is greater then vector2
+ * 	< 0 - vector 1 is smaller than vector2
+ * 	> 0 - vector 1 is greater than vector2
  *
  * Algorithm is:
  * 	This is performed by selecting the numerically smaller key vector...
diff --git a/net/sctp/sm_statefuns.c b/net/sctp/sm_statefuns.c
index 1c4e5d6c29c0..3a0cd075914f 100644
--- a/net/sctp/sm_statefuns.c
+++ b/net/sctp/sm_statefuns.c
@@ -4268,9 +4268,9 @@ nomem:
 
 /*
  * Handle a protocol violation when the chunk length is invalid.
- * "Invalid" length is identified as smaller then the minimal length a
+ * "Invalid" length is identified as smaller than the minimal length a
  * given chunk can be.  For example, a SACK chunk has invalid length
- * if it's length is set to be smaller then the size of sctp_sack_chunk_t.
+ * if its length is set to be smaller than the size of sctp_sack_chunk_t.
  *
  * We inform the other end by sending an ABORT with a Protocol Violation
  * error code.
@@ -4300,7 +4300,7 @@ static sctp_disposition_t sctp_sf_violation_chunklen(
 
 /*
  * Handle a protocol violation when the parameter length is invalid.
- * "Invalid" length is identified as smaller then the minimal length a
+ * "Invalid" length is identified as smaller than the minimal length a
  * given parameter can be.
  */
 static sctp_disposition_t sctp_sf_violation_paramlen(
diff --git a/net/sctp/socket.c b/net/sctp/socket.c
index b14a8f33e42d..ff0a8f88de04 100644
--- a/net/sctp/socket.c
+++ b/net/sctp/socket.c
@@ -2717,7 +2717,7 @@ static int sctp_setsockopt_associnfo(struct sock *sk, char __user *optval, int o
 				paths++;
 			}
 
-			/* Only validate asocmaxrxt if we have more then
+			/* Only validate asocmaxrxt if we have more than
 			 * one path/transport.  We do this because path
 			 * retransmissions are only counted when we have more
 			 * then one path.
diff --git a/net/sctp/tsnmap.c b/net/sctp/tsnmap.c
index 35c73e82553a..9bd64565021a 100644
--- a/net/sctp/tsnmap.c
+++ b/net/sctp/tsnmap.c
@@ -227,7 +227,7 @@ void sctp_tsnmap_skip(struct sctp_tsnmap *map, __u32 tsn)
 		 */
 		bitmap_zero(map->tsn_map, map->len);
 	} else {
-		/* If the gap is smaller then the map size,
+		/* If the gap is smaller than the map size,
 		 * shift the map by 'gap' bits and update further.
 		 */
 		bitmap_shift_right(map->tsn_map, map->tsn_map, gap, map->len);
diff --git a/net/sunrpc/cache.c b/net/sunrpc/cache.c
index c9966713282a..4735caad26ed 100644
--- a/net/sunrpc/cache.c
+++ b/net/sunrpc/cache.c
@@ -98,7 +98,7 @@ struct cache_head *sunrpc_cache_lookup(struct cache_detail *detail,
 
 	return new;
 }
-EXPORT_SYMBOL(sunrpc_cache_lookup);
+EXPORT_SYMBOL_GPL(sunrpc_cache_lookup);
 
 
 static void queue_loose(struct cache_detail *detail, struct cache_head *ch);
@@ -173,7 +173,7 @@ struct cache_head *sunrpc_cache_update(struct cache_detail *detail,
 	cache_put(old, detail);
 	return tmp;
 }
-EXPORT_SYMBOL(sunrpc_cache_update);
+EXPORT_SYMBOL_GPL(sunrpc_cache_update);
 
 static int cache_make_upcall(struct cache_detail *detail, struct cache_head *h);
 /*
@@ -245,7 +245,7 @@ int cache_check(struct cache_detail *detail,
 		cache_put(h, detail);
 	return rv;
 }
-EXPORT_SYMBOL(cache_check);
+EXPORT_SYMBOL_GPL(cache_check);
 
 /*
  * caches need to be periodically cleaned.
@@ -373,7 +373,7 @@ int cache_register(struct cache_detail *cd)
 	schedule_delayed_work(&cache_cleaner, 0);
 	return 0;
 }
-EXPORT_SYMBOL(cache_register);
+EXPORT_SYMBOL_GPL(cache_register);
 
 void cache_unregister(struct cache_detail *cd)
 {
@@ -399,7 +399,7 @@ void cache_unregister(struct cache_detail *cd)
 out:
 	printk(KERN_ERR "nfsd: failed to unregister %s cache\n", cd->name);
 }
-EXPORT_SYMBOL(cache_unregister);
+EXPORT_SYMBOL_GPL(cache_unregister);
 
 /* clean cache tries to find something to clean
  * and cleans it.
@@ -514,7 +514,7 @@ void cache_flush(void)
 	while (cache_clean() != -1)
 		cond_resched();
 }
-EXPORT_SYMBOL(cache_flush);
+EXPORT_SYMBOL_GPL(cache_flush);
 
 void cache_purge(struct cache_detail *detail)
 {
@@ -523,7 +523,7 @@ void cache_purge(struct cache_detail *detail)
 	cache_flush();
 	detail->flush_time = 1;
 }
-EXPORT_SYMBOL(cache_purge);
+EXPORT_SYMBOL_GPL(cache_purge);
 
 
 /*
@@ -988,7 +988,7 @@ void qword_add(char **bpp, int *lp, char *str)
 	*bpp = bp;
 	*lp = len;
 }
-EXPORT_SYMBOL(qword_add);
+EXPORT_SYMBOL_GPL(qword_add);
 
 void qword_addhex(char **bpp, int *lp, char *buf, int blen)
 {
@@ -1017,7 +1017,7 @@ void qword_addhex(char **bpp, int *lp, char *buf, int blen)
 	*bpp = bp;
 	*lp = len;
 }
-EXPORT_SYMBOL(qword_addhex);
+EXPORT_SYMBOL_GPL(qword_addhex);
 
 static void warn_no_listener(struct cache_detail *detail)
 {
@@ -1140,7 +1140,7 @@ int qword_get(char **bpp, char *dest, int bufsize)
 	*dest = '\0';
 	return len;
 }
-EXPORT_SYMBOL(qword_get);
+EXPORT_SYMBOL_GPL(qword_get);
 
 
 /*
diff --git a/net/sunrpc/stats.c b/net/sunrpc/stats.c
index 50b049c6598a..085372ef4feb 100644
--- a/net/sunrpc/stats.c
+++ b/net/sunrpc/stats.c
@@ -106,7 +106,7 @@ void svc_seq_show(struct seq_file *seq, const struct svc_stat *statp) {
 		seq_putc(seq, '\n');
 	}
 }
-EXPORT_SYMBOL(svc_seq_show);
+EXPORT_SYMBOL_GPL(svc_seq_show);
 
 /**
  * rpc_alloc_iostats - allocate an rpc_iostats structure
@@ -249,14 +249,14 @@ svc_proc_register(struct svc_stat *statp, const struct file_operations *fops)
 {
 	return do_register(statp->program->pg_name, statp, fops);
 }
-EXPORT_SYMBOL(svc_proc_register);
+EXPORT_SYMBOL_GPL(svc_proc_register);
 
 void
 svc_proc_unregister(const char *name)
 {
 	remove_proc_entry(name, proc_net_rpc);
 }
-EXPORT_SYMBOL(svc_proc_unregister);
+EXPORT_SYMBOL_GPL(svc_proc_unregister);
 
 void
 rpc_proc_init(void)
diff --git a/net/sunrpc/svc.c b/net/sunrpc/svc.c
index 54c98d876847..c51fed4d1af1 100644
--- a/net/sunrpc/svc.c
+++ b/net/sunrpc/svc.c
@@ -431,7 +431,7 @@ svc_create(struct svc_program *prog, unsigned int bufsize,
 {
 	return __svc_create(prog, bufsize, /*npools*/1, family, shutdown);
 }
-EXPORT_SYMBOL(svc_create);
+EXPORT_SYMBOL_GPL(svc_create);
 
 struct svc_serv *
 svc_create_pooled(struct svc_program *prog, unsigned int bufsize,
@@ -450,7 +450,7 @@ svc_create_pooled(struct svc_program *prog, unsigned int bufsize,
 
 	return serv;
 }
-EXPORT_SYMBOL(svc_create_pooled);
+EXPORT_SYMBOL_GPL(svc_create_pooled);
 
 /*
  * Destroy an RPC service. Should be called with appropriate locking to
@@ -492,7 +492,7 @@ svc_destroy(struct svc_serv *serv)
 	kfree(serv->sv_pools);
 	kfree(serv);
 }
-EXPORT_SYMBOL(svc_destroy);
+EXPORT_SYMBOL_GPL(svc_destroy);
 
 /*
  * Allocate an RPC server's buffer space.
@@ -567,7 +567,7 @@ out_thread:
 out_enomem:
 	return ERR_PTR(-ENOMEM);
 }
-EXPORT_SYMBOL(svc_prepare_thread);
+EXPORT_SYMBOL_GPL(svc_prepare_thread);
 
 /*
  * Choose a pool in which to create a new thread, for svc_set_num_threads
@@ -689,7 +689,7 @@ svc_set_num_threads(struct svc_serv *serv, struct svc_pool *pool, int nrservs)
 
 	return error;
 }
-EXPORT_SYMBOL(svc_set_num_threads);
+EXPORT_SYMBOL_GPL(svc_set_num_threads);
 
 /*
  * Called from a server thread as it's exiting. Caller must hold the BKL or
@@ -717,7 +717,7 @@ svc_exit_thread(struct svc_rqst *rqstp)
 	if (serv)
 		svc_destroy(serv);
 }
-EXPORT_SYMBOL(svc_exit_thread);
+EXPORT_SYMBOL_GPL(svc_exit_thread);
 
 #ifdef CONFIG_SUNRPC_REGISTER_V4
 
@@ -1231,7 +1231,7 @@ err_bad:
 	svc_putnl(resv, ntohl(rpc_stat));
 	goto sendit;
 }
-EXPORT_SYMBOL(svc_process);
+EXPORT_SYMBOL_GPL(svc_process);
 
 /*
  * Return (transport-specific) limit on the rpc payload.
diff --git a/net/sunrpc/svc_xprt.c b/net/sunrpc/svc_xprt.c
index bf5b5cdafebf..e588df5d6b34 100644
--- a/net/sunrpc/svc_xprt.c
+++ b/net/sunrpc/svc_xprt.c
@@ -440,7 +440,7 @@ void svc_reserve(struct svc_rqst *rqstp, int space)
 		svc_xprt_enqueue(xprt);
 	}
 }
-EXPORT_SYMBOL(svc_reserve);
+EXPORT_SYMBOL_GPL(svc_reserve);
 
 static void svc_xprt_release(struct svc_rqst *rqstp)
 {
@@ -448,6 +448,9 @@ static void svc_xprt_release(struct svc_rqst *rqstp)
 
 	rqstp->rq_xprt->xpt_ops->xpo_release_rqst(rqstp);
 
+	kfree(rqstp->rq_deferred);
+	rqstp->rq_deferred = NULL;
+
 	svc_free_res_pages(rqstp);
 	rqstp->rq_res.page_len = 0;
 	rqstp->rq_res.page_base = 0;
@@ -498,7 +501,7 @@ void svc_wake_up(struct svc_serv *serv)
 		spin_unlock_bh(&pool->sp_lock);
 	}
 }
-EXPORT_SYMBOL(svc_wake_up);
+EXPORT_SYMBOL_GPL(svc_wake_up);
 
 int svc_port_is_privileged(struct sockaddr *sin)
 {
@@ -515,8 +518,10 @@ int svc_port_is_privileged(struct sockaddr *sin)
 }
 
 /*
- * Make sure that we don't have too many active connections.  If we
- * have, something must be dropped.
+ * Make sure that we don't have too many active connections. If we have,
+ * something must be dropped. It's not clear what will happen if we allow
+ * "too many" connections, but when dealing with network-facing software,
+ * we have to code defensively. Here we do that by imposing hard limits.
  *
  * There's no point in trying to do random drop here for DoS
  * prevention. The NFS clients does 1 reconnect in 15 seconds. An
@@ -525,19 +530,27 @@ int svc_port_is_privileged(struct sockaddr *sin)
  * The only somewhat efficient mechanism would be if drop old
  * connections from the same IP first. But right now we don't even
  * record the client IP in svc_sock.
+ *
+ * single-threaded services that expect a lot of clients will probably
+ * need to set sv_maxconn to override the default value which is based
+ * on the number of threads
  */
 static void svc_check_conn_limits(struct svc_serv *serv)
 {
-	if (serv->sv_tmpcnt > (serv->sv_nrthreads+3)*20) {
+	unsigned int limit = serv->sv_maxconn ? serv->sv_maxconn :
+				(serv->sv_nrthreads+3) * 20;
+
+	if (serv->sv_tmpcnt > limit) {
 		struct svc_xprt *xprt = NULL;
 		spin_lock_bh(&serv->sv_lock);
 		if (!list_empty(&serv->sv_tempsocks)) {
 			if (net_ratelimit()) {
 				/* Try to help the admin */
 				printk(KERN_NOTICE "%s: too many open  "
-				       "connections, consider increasing the "
-				       "number of nfsd threads\n",
-				       serv->sv_name);
+				       "connections, consider increasing %s\n",
+				       serv->sv_name, serv->sv_maxconn ?
+				       "the max number of connections." :
+				       "the number of threads.");
 			}
 			/*
 			 * Always select the oldest connection. It's not fair,
@@ -730,7 +743,7 @@ int svc_recv(struct svc_rqst *rqstp, long timeout)
 		serv->sv_stats->netcnt++;
 	return len;
 }
-EXPORT_SYMBOL(svc_recv);
+EXPORT_SYMBOL_GPL(svc_recv);
 
 /*
  * Drop request
@@ -740,7 +753,7 @@ void svc_drop(struct svc_rqst *rqstp)
 	dprintk("svc: xprt %p dropped request\n", rqstp->rq_xprt);
 	svc_xprt_release(rqstp);
 }
-EXPORT_SYMBOL(svc_drop);
+EXPORT_SYMBOL_GPL(svc_drop);
 
 /*
  * Return reply to client.
@@ -837,6 +850,11 @@ static void svc_age_temp_xprts(unsigned long closure)
 void svc_delete_xprt(struct svc_xprt *xprt)
 {
 	struct svc_serv	*serv = xprt->xpt_server;
+	struct svc_deferred_req *dr;
+
+	/* Only do this once */
+	if (test_and_set_bit(XPT_DEAD, &xprt->xpt_flags))
+		return;
 
 	dprintk("svc: svc_delete_xprt(%p)\n", xprt);
 	xprt->xpt_ops->xpo_detach(xprt);
@@ -851,12 +869,16 @@ void svc_delete_xprt(struct svc_xprt *xprt)
 	 * while still attached to a queue, the queue itself
 	 * is about to be destroyed (in svc_destroy).
 	 */
-	if (!test_and_set_bit(XPT_DEAD, &xprt->xpt_flags)) {
-		BUG_ON(atomic_read(&xprt->xpt_ref.refcount) < 2);
-		if (test_bit(XPT_TEMP, &xprt->xpt_flags))
-			serv->sv_tmpcnt--;
+	if (test_bit(XPT_TEMP, &xprt->xpt_flags))
+		serv->sv_tmpcnt--;
+
+	for (dr = svc_deferred_dequeue(xprt); dr;
+	     dr = svc_deferred_dequeue(xprt)) {
 		svc_xprt_put(xprt);
+		kfree(dr);
 	}
+
+	svc_xprt_put(xprt);
 	spin_unlock_bh(&serv->sv_lock);
 }
 
@@ -902,17 +924,19 @@ static void svc_revisit(struct cache_deferred_req *dreq, int too_many)
 		container_of(dreq, struct svc_deferred_req, handle);
 	struct svc_xprt *xprt = dr->xprt;
 
-	if (too_many) {
+	spin_lock(&xprt->xpt_lock);
+	set_bit(XPT_DEFERRED, &xprt->xpt_flags);
+	if (too_many || test_bit(XPT_DEAD, &xprt->xpt_flags)) {
+		spin_unlock(&xprt->xpt_lock);
+		dprintk("revisit canceled\n");
 		svc_xprt_put(xprt);
 		kfree(dr);
 		return;
 	}
 	dprintk("revisit queued\n");
 	dr->xprt = NULL;
-	spin_lock(&xprt->xpt_lock);
 	list_add(&dr->handle.recent, &xprt->xpt_deferred);
 	spin_unlock(&xprt->xpt_lock);
-	set_bit(XPT_DEFERRED, &xprt->xpt_flags);
 	svc_xprt_enqueue(xprt);
 	svc_xprt_put(xprt);
 }
diff --git a/net/sunrpc/svcauth.c b/net/sunrpc/svcauth.c
index 8a73cbb16052..e64109b02aee 100644
--- a/net/sunrpc/svcauth.c
+++ b/net/sunrpc/svcauth.c
@@ -57,13 +57,13 @@ svc_authenticate(struct svc_rqst *rqstp, __be32 *authp)
 	rqstp->rq_authop = aops;
 	return aops->accept(rqstp, authp);
 }
-EXPORT_SYMBOL(svc_authenticate);
+EXPORT_SYMBOL_GPL(svc_authenticate);
 
 int svc_set_client(struct svc_rqst *rqstp)
 {
 	return rqstp->rq_authop->set_client(rqstp);
 }
-EXPORT_SYMBOL(svc_set_client);
+EXPORT_SYMBOL_GPL(svc_set_client);
 
 /* A request, which was authenticated, has now executed.
  * Time to finalise the credentials and verifier
@@ -95,7 +95,7 @@ svc_auth_register(rpc_authflavor_t flavor, struct auth_ops *aops)
 	spin_unlock(&authtab_lock);
 	return rv;
 }
-EXPORT_SYMBOL(svc_auth_register);
+EXPORT_SYMBOL_GPL(svc_auth_register);
 
 void
 svc_auth_unregister(rpc_authflavor_t flavor)
@@ -105,7 +105,7 @@ svc_auth_unregister(rpc_authflavor_t flavor)
 		authtab[flavor] = NULL;
 	spin_unlock(&authtab_lock);
 }
-EXPORT_SYMBOL(svc_auth_unregister);
+EXPORT_SYMBOL_GPL(svc_auth_unregister);
 
 /**************************************************
  * 'auth_domains' are stored in a hash table indexed by name.
@@ -132,7 +132,7 @@ void auth_domain_put(struct auth_domain *dom)
 		spin_unlock(&auth_domain_lock);
 	}
 }
-EXPORT_SYMBOL(auth_domain_put);
+EXPORT_SYMBOL_GPL(auth_domain_put);
 
 struct auth_domain *
 auth_domain_lookup(char *name, struct auth_domain *new)
@@ -157,10 +157,10 @@ auth_domain_lookup(char *name, struct auth_domain *new)
 	spin_unlock(&auth_domain_lock);
 	return new;
 }
-EXPORT_SYMBOL(auth_domain_lookup);
+EXPORT_SYMBOL_GPL(auth_domain_lookup);
 
 struct auth_domain *auth_domain_find(char *name)
 {
 	return auth_domain_lookup(name, NULL);
 }
-EXPORT_SYMBOL(auth_domain_find);
+EXPORT_SYMBOL_GPL(auth_domain_find);
diff --git a/net/sunrpc/svcauth_unix.c b/net/sunrpc/svcauth_unix.c
index 82240e6127b2..5c865e2d299e 100644
--- a/net/sunrpc/svcauth_unix.c
+++ b/net/sunrpc/svcauth_unix.c
@@ -64,7 +64,7 @@ struct auth_domain *unix_domain_find(char *name)
 		rv = auth_domain_lookup(name, &new->h);
 	}
 }
-EXPORT_SYMBOL(unix_domain_find);
+EXPORT_SYMBOL_GPL(unix_domain_find);
 
 static void svcauth_unix_domain_release(struct auth_domain *dom)
 {
@@ -358,7 +358,7 @@ int auth_unix_add_addr(struct in6_addr *addr, struct auth_domain *dom)
 	else
 		return -ENOMEM;
 }
-EXPORT_SYMBOL(auth_unix_add_addr);
+EXPORT_SYMBOL_GPL(auth_unix_add_addr);
 
 int auth_unix_forget_old(struct auth_domain *dom)
 {
@@ -370,7 +370,7 @@ int auth_unix_forget_old(struct auth_domain *dom)
 	udom->addr_changes++;
 	return 0;
 }
-EXPORT_SYMBOL(auth_unix_forget_old);
+EXPORT_SYMBOL_GPL(auth_unix_forget_old);
 
 struct auth_domain *auth_unix_lookup(struct in6_addr *addr)
 {
@@ -395,13 +395,13 @@ struct auth_domain *auth_unix_lookup(struct in6_addr *addr)
 	cache_put(&ipm->h, &ip_map_cache);
 	return rv;
 }
-EXPORT_SYMBOL(auth_unix_lookup);
+EXPORT_SYMBOL_GPL(auth_unix_lookup);
 
 void svcauth_unix_purge(void)
 {
 	cache_purge(&ip_map_cache);
 }
-EXPORT_SYMBOL(svcauth_unix_purge);
+EXPORT_SYMBOL_GPL(svcauth_unix_purge);
 
 static inline struct ip_map *
 ip_map_cached_get(struct svc_rqst *rqstp)
@@ -714,7 +714,7 @@ svcauth_unix_set_client(struct svc_rqst *rqstp)
 	return SVC_OK;
 }
 
-EXPORT_SYMBOL(svcauth_unix_set_client);
+EXPORT_SYMBOL_GPL(svcauth_unix_set_client);
 
 static int
 svcauth_null_accept(struct svc_rqst *rqstp, __be32 *authp)
diff --git a/net/sunrpc/svcsock.c b/net/sunrpc/svcsock.c
index ef3238d665ee..5763e6460fea 100644
--- a/net/sunrpc/svcsock.c
+++ b/net/sunrpc/svcsock.c
@@ -59,6 +59,7 @@ static void		svc_udp_data_ready(struct sock *, int);
 static int		svc_udp_recvfrom(struct svc_rqst *);
 static int		svc_udp_sendto(struct svc_rqst *);
 static void		svc_sock_detach(struct svc_xprt *);
+static void		svc_tcp_sock_detach(struct svc_xprt *);
 static void		svc_sock_free(struct svc_xprt *);
 
 static struct svc_xprt *svc_create_socket(struct svc_serv *, int,
@@ -102,7 +103,6 @@ static void svc_reclassify_socket(struct socket *sock)
 static void svc_release_skb(struct svc_rqst *rqstp)
 {
 	struct sk_buff *skb = rqstp->rq_xprt_ctxt;
-	struct svc_deferred_req *dr = rqstp->rq_deferred;
 
 	if (skb) {
 		struct svc_sock *svsk =
@@ -112,10 +112,6 @@ static void svc_release_skb(struct svc_rqst *rqstp)
 		dprintk("svc: service %p, releasing skb %p\n", rqstp, skb);
 		skb_free_datagram(svsk->sk_sk, skb);
 	}
-	if (dr) {
-		rqstp->rq_deferred = NULL;
-		kfree(dr);
-	}
 }
 
 union svc_pktinfo_u {
@@ -289,7 +285,7 @@ svc_sock_names(char *buf, struct svc_serv *serv, char *toclose)
 		return -ENOENT;
 	return len;
 }
-EXPORT_SYMBOL(svc_sock_names);
+EXPORT_SYMBOL_GPL(svc_sock_names);
 
 /*
  * Check input queue length
@@ -1017,7 +1013,7 @@ static struct svc_xprt_ops svc_tcp_ops = {
 	.xpo_recvfrom = svc_tcp_recvfrom,
 	.xpo_sendto = svc_tcp_sendto,
 	.xpo_release_rqst = svc_release_skb,
-	.xpo_detach = svc_sock_detach,
+	.xpo_detach = svc_tcp_sock_detach,
 	.xpo_free = svc_sock_free,
 	.xpo_prep_reply_hdr = svc_tcp_prep_reply_hdr,
 	.xpo_has_wspace = svc_tcp_has_wspace,
@@ -1101,7 +1097,7 @@ void svc_sock_update_bufs(struct svc_serv *serv)
 	}
 	spin_unlock_bh(&serv->sv_lock);
 }
-EXPORT_SYMBOL(svc_sock_update_bufs);
+EXPORT_SYMBOL_GPL(svc_sock_update_bufs);
 
 /*
  * Initialize socket for RPC use and create svc_sock struct
@@ -1287,6 +1283,24 @@ static void svc_sock_detach(struct svc_xprt *xprt)
 	sk->sk_state_change = svsk->sk_ostate;
 	sk->sk_data_ready = svsk->sk_odata;
 	sk->sk_write_space = svsk->sk_owspace;
+
+	if (sk->sk_sleep && waitqueue_active(sk->sk_sleep))
+		wake_up_interruptible(sk->sk_sleep);
+}
+
+/*
+ * Disconnect the socket, and reset the callbacks
+ */
+static void svc_tcp_sock_detach(struct svc_xprt *xprt)
+{
+	struct svc_sock *svsk = container_of(xprt, struct svc_sock, sk_xprt);
+
+	dprintk("svc: svc_tcp_sock_detach(%p)\n", svsk);
+
+	svc_sock_detach(xprt);
+
+	if (!test_bit(XPT_LISTENER, &xprt->xpt_flags))
+		kernel_sock_shutdown(svsk->sk_sock, SHUT_RDWR);
 }
 
 /*
diff --git a/net/wimax/Kconfig b/net/wimax/Kconfig
new file mode 100644
index 000000000000..0bdbb6928205
--- /dev/null
+++ b/net/wimax/Kconfig
@@ -0,0 +1,38 @@
+#
+# WiMAX LAN device configuration
+#
+
+menuconfig WIMAX
+	tristate "WiMAX Wireless Broadband support"
+	help
+
+	  Select to configure support for devices that provide
+	  wireless broadband connectivity using the WiMAX protocol
+	  (IEEE 802.16).
+
+	  Please note that most of these devices require signing up
+	  for a service plan with a provider.
+
+	  The different WiMAX drivers can be enabled in the menu entry
+
+	  Device Drivers > Network device support > WiMAX Wireless
+	  Broadband devices
+
+	  If unsure, it is safe to select M (module).
+
+config WIMAX_DEBUG_LEVEL
+	int "WiMAX debug level"
+	depends on WIMAX
+	default 8
+	help
+
+	  Select the maximum debug verbosity level to be compiled into
+	  the WiMAX stack code.
+
+	  By default, debug messages are disabled at runtime and can
+	  be selectively enabled for different parts of the code using
+	  the sysfs debug-levels file.
+
+	  If set at zero, this will compile out all the debug code.
+
+	  It is recommended that it is left at 8.
diff --git a/net/wimax/Makefile b/net/wimax/Makefile
new file mode 100644
index 000000000000..5b80b941c2c9
--- /dev/null
+++ b/net/wimax/Makefile
@@ -0,0 +1,13 @@
+
+obj-$(CONFIG_WIMAX)		+= wimax.o
+
+wimax-y :=		\
+	id-table.o	\
+	op-msg.o	\
+	op-reset.o	\
+	op-rfkill.o	\
+	stack.o
+
+wimax-$(CONFIG_DEBUG_FS) += debugfs.o
+
+
diff --git a/net/wimax/debug-levels.h b/net/wimax/debug-levels.h
new file mode 100644
index 000000000000..1c29123a3aa9
--- /dev/null
+++ b/net/wimax/debug-levels.h
@@ -0,0 +1,42 @@
+/*
+ * Linux WiMAX Stack
+ * Debug levels control file for the wimax module
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+#ifndef __debug_levels__h__
+#define __debug_levels__h__
+
+/* Maximum compile and run time debug level for all submodules */
+#define D_MODULENAME wimax
+#define D_MASTER CONFIG_WIMAX_DEBUG_LEVEL
+
+#include <linux/wimax/debug.h>
+
+/* List of all the enabled modules */
+enum d_module {
+	D_SUBMODULE_DECLARE(debugfs),
+	D_SUBMODULE_DECLARE(id_table),
+	D_SUBMODULE_DECLARE(op_msg),
+	D_SUBMODULE_DECLARE(op_reset),
+	D_SUBMODULE_DECLARE(op_rfkill),
+	D_SUBMODULE_DECLARE(stack),
+};
+
+#endif /* #ifndef __debug_levels__h__ */
diff --git a/net/wimax/debugfs.c b/net/wimax/debugfs.c
new file mode 100644
index 000000000000..87cf4430079c
--- /dev/null
+++ b/net/wimax/debugfs.c
@@ -0,0 +1,90 @@
+/*
+ * Linux WiMAX
+ * Debugfs support
+ *
+ *
+ * Copyright (C) 2005-2006 Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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/debugfs.h>
+#include <linux/wimax.h>
+#include "wimax-internal.h"
+
+#define D_SUBMODULE debugfs
+#include "debug-levels.h"
+
+
+/* Debug framework control of debug levels */
+struct d_level D_LEVEL[] = {
+	D_SUBMODULE_DEFINE(debugfs),
+	D_SUBMODULE_DEFINE(id_table),
+	D_SUBMODULE_DEFINE(op_msg),
+	D_SUBMODULE_DEFINE(op_reset),
+	D_SUBMODULE_DEFINE(op_rfkill),
+	D_SUBMODULE_DEFINE(stack),
+};
+size_t D_LEVEL_SIZE = ARRAY_SIZE(D_LEVEL);
+
+#define __debugfs_register(prefix, name, parent)			\
+do {									\
+	result = d_level_register_debugfs(prefix, name, parent);	\
+	if (result < 0)							\
+		goto error;						\
+} while (0)
+
+
+int wimax_debugfs_add(struct wimax_dev *wimax_dev)
+{
+	int result;
+	struct net_device *net_dev = wimax_dev->net_dev;
+	struct device *dev = net_dev->dev.parent;
+	struct dentry *dentry;
+	char buf[128];
+
+	snprintf(buf, sizeof(buf), "wimax:%s", net_dev->name);
+	dentry = debugfs_create_dir(buf, NULL);
+	result = PTR_ERR(dentry);
+	if (IS_ERR(dentry)) {
+		if (result == -ENODEV)
+			result = 0;	/* No debugfs support */
+		else
+			dev_err(dev, "Can't create debugfs dentry: %d\n",
+				result);
+		goto out;
+	}
+	wimax_dev->debugfs_dentry = dentry;
+	__debugfs_register("wimax_dl_", debugfs, dentry);
+	__debugfs_register("wimax_dl_", id_table, dentry);
+	__debugfs_register("wimax_dl_", op_msg, dentry);
+	__debugfs_register("wimax_dl_", op_reset, dentry);
+	__debugfs_register("wimax_dl_", op_rfkill, dentry);
+	__debugfs_register("wimax_dl_", stack, dentry);
+	result = 0;
+out:
+	return result;
+
+error:
+	debugfs_remove_recursive(wimax_dev->debugfs_dentry);
+	return result;
+}
+
+void wimax_debugfs_rm(struct wimax_dev *wimax_dev)
+{
+	debugfs_remove_recursive(wimax_dev->debugfs_dentry);
+}
+
+
diff --git a/net/wimax/id-table.c b/net/wimax/id-table.c
new file mode 100644
index 000000000000..d3b88558682c
--- /dev/null
+++ b/net/wimax/id-table.c
@@ -0,0 +1,142 @@
+/*
+ * Linux WiMAX
+ * Mappping of generic netlink family IDs to net devices
+ *
+ *
+ * Copyright (C) 2005-2006 Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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.
+ *
+ *
+ * We assign a single generic netlink family ID to each device (to
+ * simplify lookup).
+ *
+ * We need a way to map family ID to a wimax_dev pointer.
+ *
+ * The idea is to use a very simple lookup. Using a netlink attribute
+ * with (for example) the interface name implies a heavier search over
+ * all the network devices; seemed kind of a waste given that we know
+ * we are looking for a WiMAX device and that most systems will have
+ * just a single WiMAX adapter.
+ *
+ * We put all the WiMAX devices in the system in a linked list and
+ * match the generic link family ID against the list.
+ *
+ * By using a linked list, the case of a single adapter in the system
+ * becomes (almost) no overhead, while still working for many more. If
+ * it ever goes beyond two, I'll be surprised.
+ */
+#include <linux/device.h>
+#include <net/genetlink.h>
+#include <linux/netdevice.h>
+#include <linux/list.h>
+#include <linux/wimax.h>
+#include "wimax-internal.h"
+
+
+#define D_SUBMODULE id_table
+#include "debug-levels.h"
+
+
+static DEFINE_SPINLOCK(wimax_id_table_lock);
+static struct list_head wimax_id_table = LIST_HEAD_INIT(wimax_id_table);
+
+
+/*
+ * wimax_id_table_add - add a gennetlink familiy ID / wimax_dev mapping
+ *
+ * @wimax_dev: WiMAX device descriptor to associate to the Generic
+ *     Netlink family ID.
+ *
+ * Look for an empty spot in the ID table; if none found, double the
+ * table's size and get the first spot.
+ */
+void wimax_id_table_add(struct wimax_dev *wimax_dev)
+{
+	d_fnstart(3, NULL, "(wimax_dev %p)\n", wimax_dev);
+	spin_lock(&wimax_id_table_lock);
+	list_add(&wimax_dev->id_table_node, &wimax_id_table);
+	spin_unlock(&wimax_id_table_lock);
+	d_fnend(3, NULL, "(wimax_dev %p)\n", wimax_dev);
+}
+
+
+/*
+ * wimax_get_netdev_by_info - lookup a wimax_dev from the gennetlink info
+ *
+ * The generic netlink family ID has been filled out in the
+ * nlmsghdr->nlmsg_type field, so we pull it from there, look it up in
+ * the mapping table and reference the wimax_dev.
+ *
+ * When done, the reference should be dropped with
+ * 'dev_put(wimax_dev->net_dev)'.
+ */
+struct wimax_dev *wimax_dev_get_by_genl_info(
+	struct genl_info *info, int ifindex)
+{
+	struct wimax_dev *wimax_dev = NULL;
+
+	d_fnstart(3, NULL, "(info %p ifindex %d)\n", info, ifindex);
+	spin_lock(&wimax_id_table_lock);
+	list_for_each_entry(wimax_dev, &wimax_id_table, id_table_node) {
+		if (wimax_dev->net_dev->ifindex == ifindex) {
+			dev_hold(wimax_dev->net_dev);
+			break;
+		}
+	}
+	if (wimax_dev == NULL)
+		d_printf(1, NULL, "wimax: no devices found with ifindex %d\n",
+			 ifindex);
+	spin_unlock(&wimax_id_table_lock);
+	d_fnend(3, NULL, "(info %p ifindex %d) = %p\n",
+		info, ifindex, wimax_dev);
+	return wimax_dev;
+}
+
+
+/*
+ * wimax_id_table_rm - Remove a gennetlink familiy ID / wimax_dev mapping
+ *
+ * @id: family ID to remove from the table
+ */
+void wimax_id_table_rm(struct wimax_dev *wimax_dev)
+{
+	spin_lock(&wimax_id_table_lock);
+	list_del_init(&wimax_dev->id_table_node);
+	spin_unlock(&wimax_id_table_lock);
+}
+
+
+/*
+ * Release the gennetlink family id / mapping table
+ *
+ * On debug, verify that the table is empty upon removal.
+ */
+void wimax_id_table_release(void)
+{
+#ifndef CONFIG_BUG
+	return;
+#endif
+	struct wimax_dev *wimax_dev;
+
+	spin_lock(&wimax_id_table_lock);
+	list_for_each_entry(wimax_dev, &wimax_id_table, id_table_node) {
+		printk(KERN_ERR "BUG: %s wimax_dev %p ifindex %d not cleared\n",
+		       __func__, wimax_dev, wimax_dev->net_dev->ifindex);
+		WARN_ON(1);
+	}
+	spin_unlock(&wimax_id_table_lock);
+}
diff --git a/net/wimax/op-msg.c b/net/wimax/op-msg.c
new file mode 100644
index 000000000000..cb3b4ad53683
--- /dev/null
+++ b/net/wimax/op-msg.c
@@ -0,0 +1,421 @@
+/*
+ * Linux WiMAX
+ * Generic messaging interface between userspace and driver/device
+ *
+ *
+ * Copyright (C) 2007-2008 Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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.
+ *
+ *
+ * This implements a direct communication channel between user space and
+ * the driver/device, by which free form messages can be sent back and
+ * forth.
+ *
+ * This is intended for device-specific features, vendor quirks, etc.
+ *
+ * See include/net/wimax.h
+ *
+ * GENERIC NETLINK ENCODING AND CAPACITY
+ *
+ * A destination "pipe name" is added to each message; it is up to the
+ * drivers to assign or use those names (if using them at all).
+ *
+ * Messages are encoded as a binary netlink attribute using nla_put()
+ * using type NLA_UNSPEC (as some versions of libnl still in
+ * deployment don't yet understand NLA_BINARY).
+ *
+ * The maximum capacity of this transport is PAGESIZE per message (so
+ * the actual payload will be bit smaller depending on the
+ * netlink/generic netlink attributes and headers).
+ *
+ * RECEPTION OF MESSAGES
+ *
+ * When a message is received from user space, it is passed verbatim
+ * to the driver calling wimax_dev->op_msg_from_user(). The return
+ * value from this function is passed back to user space as an ack
+ * over the generic netlink protocol.
+ *
+ * The stack doesn't do any processing or interpretation of these
+ * messages.
+ *
+ * SENDING MESSAGES
+ *
+ * Messages can be sent with wimax_msg().
+ *
+ * If the message delivery needs to happen on a different context to
+ * that of its creation, wimax_msg_alloc() can be used to get a
+ * pointer to the message that can be delivered later on with
+ * wimax_msg_send().
+ *
+ * ROADMAP
+ *
+ * wimax_gnl_doit_msg_from_user()    Process a message from user space
+ *   wimax_dev_get_by_genl_info()
+ *   wimax_dev->op_msg_from_user()   Delivery of message to the driver
+ *
+ * wimax_msg()                       Send a message to user space
+ *   wimax_msg_alloc()
+ *   wimax_msg_send()
+ */
+#include <linux/device.h>
+#include <net/genetlink.h>
+#include <linux/netdevice.h>
+#include <linux/wimax.h>
+#include <linux/security.h>
+#include "wimax-internal.h"
+
+
+#define D_SUBMODULE op_msg
+#include "debug-levels.h"
+
+
+/**
+ * wimax_msg_alloc - Create a new skb for sending a message to userspace
+ *
+ * @wimax_dev: WiMAX device descriptor
+ * @pipe_name: "named pipe" the message will be sent to
+ * @msg: pointer to the message data to send
+ * @size: size of the message to send (in bytes), including the header.
+ * @gfp_flags: flags for memory allocation.
+ *
+ * Returns: %0 if ok, negative errno code on error
+ *
+ * Description:
+ *
+ * Allocates an skb that will contain the message to send to user
+ * space over the messaging pipe and initializes it, copying the
+ * payload.
+ *
+ * Once this call is done, you can deliver it with
+ * wimax_msg_send().
+ *
+ * IMPORTANT:
+ *
+ * Don't use skb_push()/skb_pull()/skb_reserve() on the skb, as
+ * wimax_msg_send() depends on skb->data being placed at the
+ * beginning of the user message.
+ */
+struct sk_buff *wimax_msg_alloc(struct wimax_dev *wimax_dev,
+				const char *pipe_name,
+				const void *msg, size_t size,
+				gfp_t gfp_flags)
+{
+	int result;
+	struct device *dev = wimax_dev->net_dev->dev.parent;
+	size_t msg_size;
+	void *genl_msg;
+	struct sk_buff *skb;
+
+	msg_size = nla_total_size(size)
+		+ nla_total_size(sizeof(u32))
+		+ (pipe_name ? nla_total_size(strlen(pipe_name)) : 0);
+	result = -ENOMEM;
+	skb = genlmsg_new(msg_size, gfp_flags);
+	if (skb == NULL)
+		goto error_new;
+	genl_msg = genlmsg_put(skb, 0, 0, &wimax_gnl_family,
+			       0, WIMAX_GNL_OP_MSG_TO_USER);
+	if (genl_msg == NULL) {
+		dev_err(dev, "no memory to create generic netlink message\n");
+		goto error_genlmsg_put;
+	}
+	result = nla_put_u32(skb, WIMAX_GNL_MSG_IFIDX,
+			     wimax_dev->net_dev->ifindex);
+	if (result < 0) {
+		dev_err(dev, "no memory to add ifindex attribute\n");
+		goto error_nla_put;
+	}
+	if (pipe_name) {
+		result = nla_put_string(skb, WIMAX_GNL_MSG_PIPE_NAME,
+					pipe_name);
+		if (result < 0) {
+			dev_err(dev, "no memory to add pipe_name attribute\n");
+			goto error_nla_put;
+		}
+	}
+	result = nla_put(skb, WIMAX_GNL_MSG_DATA, size, msg);
+	if (result < 0) {
+		dev_err(dev, "no memory to add payload in attribute\n");
+		goto error_nla_put;
+	}
+	genlmsg_end(skb, genl_msg);
+	return skb;
+
+error_nla_put:
+error_genlmsg_put:
+error_new:
+	nlmsg_free(skb);
+	return ERR_PTR(result);
+
+}
+EXPORT_SYMBOL_GPL(wimax_msg_alloc);
+
+
+/**
+ * wimax_msg_data_len - Return a pointer and size of a message's payload
+ *
+ * @msg: Pointer to a message created with wimax_msg_alloc()
+ * @size: Pointer to where to store the message's size
+ *
+ * Returns the pointer to the message data.
+ */
+const void *wimax_msg_data_len(struct sk_buff *msg, size_t *size)
+{
+	struct nlmsghdr *nlh = (void *) msg->head;
+	struct nlattr *nla;
+
+	nla = nlmsg_find_attr(nlh, sizeof(struct genlmsghdr),
+			      WIMAX_GNL_MSG_DATA);
+	if (nla == NULL) {
+		printk(KERN_ERR "Cannot find attribute WIMAX_GNL_MSG_DATA\n");
+		return NULL;
+	}
+	*size = nla_len(nla);
+	return nla_data(nla);
+}
+EXPORT_SYMBOL_GPL(wimax_msg_data_len);
+
+
+/**
+ * wimax_msg_data - Return a pointer to a message's payload
+ *
+ * @msg: Pointer to a message created with wimax_msg_alloc()
+ */
+const void *wimax_msg_data(struct sk_buff *msg)
+{
+	struct nlmsghdr *nlh = (void *) msg->head;
+	struct nlattr *nla;
+
+	nla = nlmsg_find_attr(nlh, sizeof(struct genlmsghdr),
+			      WIMAX_GNL_MSG_DATA);
+	if (nla == NULL) {
+		printk(KERN_ERR "Cannot find attribute WIMAX_GNL_MSG_DATA\n");
+		return NULL;
+	}
+	return nla_data(nla);
+}
+EXPORT_SYMBOL_GPL(wimax_msg_data);
+
+
+/**
+ * wimax_msg_len - Return a message's payload length
+ *
+ * @msg: Pointer to a message created with wimax_msg_alloc()
+ */
+ssize_t wimax_msg_len(struct sk_buff *msg)
+{
+	struct nlmsghdr *nlh = (void *) msg->head;
+	struct nlattr *nla;
+
+	nla = nlmsg_find_attr(nlh, sizeof(struct genlmsghdr),
+			      WIMAX_GNL_MSG_DATA);
+	if (nla == NULL) {
+		printk(KERN_ERR "Cannot find attribute WIMAX_GNL_MSG_DATA\n");
+		return -EINVAL;
+	}
+	return nla_len(nla);
+}
+EXPORT_SYMBOL_GPL(wimax_msg_len);
+
+
+/**
+ * wimax_msg_send - Send a pre-allocated message to user space
+ *
+ * @wimax_dev: WiMAX device descriptor
+ *
+ * @skb: &struct sk_buff returned by wimax_msg_alloc(). Note the
+ *     ownership of @skb is transferred to this function.
+ *
+ * Returns: 0 if ok, < 0 errno code on error
+ *
+ * Description:
+ *
+ * Sends a free-form message that was preallocated with
+ * wimax_msg_alloc() and filled up.
+ *
+ * Assumes that once you pass an skb to this function for sending, it
+ * owns it and will release it when done (on success).
+ *
+ * IMPORTANT:
+ *
+ * Don't use skb_push()/skb_pull()/skb_reserve() on the skb, as
+ * wimax_msg_send() depends on skb->data being placed at the
+ * beginning of the user message.
+ */
+int wimax_msg_send(struct wimax_dev *wimax_dev, struct sk_buff *skb)
+{
+	int result;
+	struct device *dev = wimax_dev->net_dev->dev.parent;
+	void *msg = skb->data;
+	size_t size = skb->len;
+	might_sleep();
+
+	d_printf(1, dev, "CTX: wimax msg, %zu bytes\n", size);
+	d_dump(2, dev, msg, size);
+	result = genlmsg_multicast(skb, 0, wimax_gnl_mcg.id, GFP_KERNEL);
+	d_printf(1, dev, "CTX: genl multicast result %d\n", result);
+	if (result == -ESRCH)	/* Nobody connected, ignore it */
+		result = 0;	/* btw, the skb is freed already */
+	return result;
+}
+EXPORT_SYMBOL_GPL(wimax_msg_send);
+
+
+/**
+ * wimax_msg - Send a message to user space
+ *
+ * @wimax_dev: WiMAX device descriptor (properly referenced)
+ * @pipe_name: "named pipe" the message will be sent to
+ * @buf: pointer to the message to send.
+ * @size: size of the buffer pointed to by @buf (in bytes).
+ * @gfp_flags: flags for memory allocation.
+ *
+ * Returns: %0 if ok, negative errno code on error.
+ *
+ * Description:
+ *
+ * Sends a free-form message to user space on the device @wimax_dev.
+ *
+ * NOTES:
+ *
+ * Once the @skb is given to this function, who will own it and will
+ * release it when done (unless it returns error).
+ */
+int wimax_msg(struct wimax_dev *wimax_dev, const char *pipe_name,
+	      const void *buf, size_t size, gfp_t gfp_flags)
+{
+	int result = -ENOMEM;
+	struct sk_buff *skb;
+
+	skb = wimax_msg_alloc(wimax_dev, pipe_name, buf, size, gfp_flags);
+	if (skb == NULL)
+		goto error_msg_new;
+	result = wimax_msg_send(wimax_dev, skb);
+error_msg_new:
+	return result;
+}
+EXPORT_SYMBOL_GPL(wimax_msg);
+
+
+static const
+struct nla_policy wimax_gnl_msg_policy[WIMAX_GNL_ATTR_MAX + 1] = {
+	[WIMAX_GNL_MSG_IFIDX] = {
+		.type = NLA_U32,
+	},
+	[WIMAX_GNL_MSG_DATA] = {
+		.type = NLA_UNSPEC,	/* libnl doesn't grok BINARY yet */
+	},
+};
+
+
+/*
+ * Relays a message from user space to the driver
+ *
+ * The skb is passed to the driver-specific function with the netlink
+ * and generic netlink headers already stripped.
+ *
+ * This call will block while handling/relaying the message.
+ */
+static
+int wimax_gnl_doit_msg_from_user(struct sk_buff *skb, struct genl_info *info)
+{
+	int result, ifindex;
+	struct wimax_dev *wimax_dev;
+	struct device *dev;
+	struct nlmsghdr *nlh = info->nlhdr;
+	char *pipe_name;
+	void *msg_buf;
+	size_t msg_len;
+
+	might_sleep();
+	d_fnstart(3, NULL, "(skb %p info %p)\n", skb, info);
+	result = -ENODEV;
+	if (info->attrs[WIMAX_GNL_MSG_IFIDX] == NULL) {
+		printk(KERN_ERR "WIMAX_GNL_MSG_FROM_USER: can't find IFIDX "
+		       "attribute\n");
+		goto error_no_wimax_dev;
+	}
+	ifindex = nla_get_u32(info->attrs[WIMAX_GNL_MSG_IFIDX]);
+	wimax_dev = wimax_dev_get_by_genl_info(info, ifindex);
+	if (wimax_dev == NULL)
+		goto error_no_wimax_dev;
+	dev = wimax_dev_to_dev(wimax_dev);
+
+	/* Unpack arguments */
+	result = -EINVAL;
+	if (info->attrs[WIMAX_GNL_MSG_DATA] == NULL) {
+		dev_err(dev, "WIMAX_GNL_MSG_FROM_USER: can't find MSG_DATA "
+			"attribute\n");
+		goto error_no_data;
+	}
+	msg_buf = nla_data(info->attrs[WIMAX_GNL_MSG_DATA]);
+	msg_len = nla_len(info->attrs[WIMAX_GNL_MSG_DATA]);
+
+	if (info->attrs[WIMAX_GNL_MSG_PIPE_NAME] == NULL)
+		pipe_name = NULL;
+	else {
+		struct nlattr *attr = info->attrs[WIMAX_GNL_MSG_PIPE_NAME];
+		size_t attr_len = nla_len(attr);
+		/* libnl-1.1 does not yet support NLA_NUL_STRING */
+		result = -ENOMEM;
+		pipe_name = kstrndup(nla_data(attr), attr_len + 1, GFP_KERNEL);
+		if (pipe_name == NULL)
+			goto error_alloc;
+		pipe_name[attr_len] = 0;
+	}
+	mutex_lock(&wimax_dev->mutex);
+	result = wimax_dev_is_ready(wimax_dev);
+	if (result < 0)
+		goto error_not_ready;
+	result = -ENOSYS;
+	if (wimax_dev->op_msg_from_user == NULL)
+		goto error_noop;
+
+	d_printf(1, dev,
+		 "CRX: nlmsghdr len %u type %u flags 0x%04x seq 0x%x pid %u\n",
+		 nlh->nlmsg_len, nlh->nlmsg_type, nlh->nlmsg_flags,
+		 nlh->nlmsg_seq, nlh->nlmsg_pid);
+	d_printf(1, dev, "CRX: wimax message %zu bytes\n", msg_len);
+	d_dump(2, dev, msg_buf, msg_len);
+
+	result = wimax_dev->op_msg_from_user(wimax_dev, pipe_name,
+					     msg_buf, msg_len, info);
+error_noop:
+error_not_ready:
+	mutex_unlock(&wimax_dev->mutex);
+error_alloc:
+	kfree(pipe_name);
+error_no_data:
+	dev_put(wimax_dev->net_dev);
+error_no_wimax_dev:
+	d_fnend(3, NULL, "(skb %p info %p) = %d\n", skb, info, result);
+	return result;
+}
+
+
+/*
+ * Generic Netlink glue
+ */
+
+struct genl_ops wimax_gnl_msg_from_user = {
+	.cmd = WIMAX_GNL_OP_MSG_FROM_USER,
+	.flags = GENL_ADMIN_PERM,
+	.policy = wimax_gnl_msg_policy,
+	.doit = wimax_gnl_doit_msg_from_user,
+	.dumpit = NULL,
+};
+
diff --git a/net/wimax/op-reset.c b/net/wimax/op-reset.c
new file mode 100644
index 000000000000..ca269178c4d4
--- /dev/null
+++ b/net/wimax/op-reset.c
@@ -0,0 +1,143 @@
+/*
+ * Linux WiMAX
+ * Implement and export a method for resetting a WiMAX device
+ *
+ *
+ * Copyright (C) 2008 Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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.
+ *
+ *
+ * This implements a simple synchronous call to reset a WiMAX device.
+ *
+ * Resets aim at being warm, keeping the device handles active;
+ * however, when that fails, it falls back to a cold reset (that will
+ * disconnect and reconnect the device).
+ */
+
+#include <net/wimax.h>
+#include <net/genetlink.h>
+#include <linux/wimax.h>
+#include <linux/security.h>
+#include "wimax-internal.h"
+
+#define D_SUBMODULE op_reset
+#include "debug-levels.h"
+
+
+/**
+ * wimax_reset - Reset a WiMAX device
+ *
+ * @wimax_dev: WiMAX device descriptor
+ *
+ * Returns:
+ *
+ * %0 if ok and a warm reset was done (the device still exists in
+ * the system).
+ *
+ * -%ENODEV if a cold/bus reset had to be done (device has
+ * disconnected and reconnected, so current handle is not valid
+ * any more).
+ *
+ * -%EINVAL if the device is not even registered.
+ *
+ * Any other negative error code shall be considered as
+ * non-recoverable.
+ *
+ * Description:
+ *
+ * Called when wanting to reset the device for any reason. Device is
+ * taken back to power on status.
+ *
+ * This call blocks; on succesful return, the device has completed the
+ * reset process and is ready to operate.
+ */
+int wimax_reset(struct wimax_dev *wimax_dev)
+{
+	int result = -EINVAL;
+	struct device *dev = wimax_dev_to_dev(wimax_dev);
+	enum wimax_st state;
+
+	might_sleep();
+	d_fnstart(3, dev, "(wimax_dev %p)\n", wimax_dev);
+	mutex_lock(&wimax_dev->mutex);
+	dev_hold(wimax_dev->net_dev);
+	state = wimax_dev->state;
+	mutex_unlock(&wimax_dev->mutex);
+
+	if (state >= WIMAX_ST_DOWN) {
+		mutex_lock(&wimax_dev->mutex_reset);
+		result = wimax_dev->op_reset(wimax_dev);
+		mutex_unlock(&wimax_dev->mutex_reset);
+	}
+	dev_put(wimax_dev->net_dev);
+
+	d_fnend(3, dev, "(wimax_dev %p) = %d\n", wimax_dev, result);
+	return result;
+}
+EXPORT_SYMBOL(wimax_reset);
+
+
+static const
+struct nla_policy wimax_gnl_reset_policy[WIMAX_GNL_ATTR_MAX + 1] = {
+	[WIMAX_GNL_RESET_IFIDX] = {
+		.type = NLA_U32,
+	},
+};
+
+
+/*
+ * Exporting to user space over generic netlink
+ *
+ * Parse the reset command from user space, return error code.
+ *
+ * No attributes.
+ */
+static
+int wimax_gnl_doit_reset(struct sk_buff *skb, struct genl_info *info)
+{
+	int result, ifindex;
+	struct wimax_dev *wimax_dev;
+	struct device *dev;
+
+	d_fnstart(3, NULL, "(skb %p info %p)\n", skb, info);
+	result = -ENODEV;
+	if (info->attrs[WIMAX_GNL_RESET_IFIDX] == NULL) {
+		printk(KERN_ERR "WIMAX_GNL_OP_RFKILL: can't find IFIDX "
+			"attribute\n");
+		goto error_no_wimax_dev;
+	}
+	ifindex = nla_get_u32(info->attrs[WIMAX_GNL_RESET_IFIDX]);
+	wimax_dev = wimax_dev_get_by_genl_info(info, ifindex);
+	if (wimax_dev == NULL)
+		goto error_no_wimax_dev;
+	dev = wimax_dev_to_dev(wimax_dev);
+	/* Execute the operation and send the result back to user space */
+	result = wimax_reset(wimax_dev);
+	dev_put(wimax_dev->net_dev);
+error_no_wimax_dev:
+	d_fnend(3, NULL, "(skb %p info %p) = %d\n", skb, info, result);
+	return result;
+}
+
+
+struct genl_ops wimax_gnl_reset = {
+	.cmd = WIMAX_GNL_OP_RESET,
+	.flags = GENL_ADMIN_PERM,
+	.policy = wimax_gnl_reset_policy,
+	.doit = wimax_gnl_doit_reset,
+	.dumpit = NULL,
+};
diff --git a/net/wimax/op-rfkill.c b/net/wimax/op-rfkill.c
new file mode 100644
index 000000000000..8745bac173f1
--- /dev/null
+++ b/net/wimax/op-rfkill.c
@@ -0,0 +1,532 @@
+/*
+ * Linux WiMAX
+ * RF-kill framework integration
+ *
+ *
+ * Copyright (C) 2008 Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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.
+ *
+ *
+ * This integrates into the Linux Kernel rfkill susbystem so that the
+ * drivers just have to do the bare minimal work, which is providing a
+ * method to set the software RF-Kill switch and to report changes in
+ * the software and hardware switch status.
+ *
+ * A non-polled generic rfkill device is embedded into the WiMAX
+ * subsystem's representation of a device.
+ *
+ * FIXME: Need polled support? use a timer or add the implementation
+ *     to the stack.
+ *
+ * All device drivers have to do is after wimax_dev_init(), call
+ * wimax_report_rfkill_hw() and wimax_report_rfkill_sw() to update
+ * initial state and then every time it changes. See wimax.h:struct
+ * wimax_dev for more information.
+ *
+ * ROADMAP
+ *
+ * wimax_gnl_doit_rfkill()      User space calling wimax_rfkill()
+ *   wimax_rfkill()             Kernel calling wimax_rfkill()
+ *     __wimax_rf_toggle_radio()
+ *
+ * wimax_rfkill_toggle_radio()  RF-Kill subsytem calling
+ *   __wimax_rf_toggle_radio()
+ *
+ * __wimax_rf_toggle_radio()
+ *   wimax_dev->op_rfkill_sw_toggle() Driver backend
+ *   __wimax_state_change()
+ *
+ * wimax_report_rfkill_sw()     Driver reports state change
+ *   __wimax_state_change()
+ *
+ * wimax_report_rfkill_hw()     Driver reports state change
+ *   __wimax_state_change()
+ *
+ * wimax_rfkill_add()           Initialize/shutdown rfkill support
+ * wimax_rfkill_rm()            [called by wimax_dev_add/rm()]
+ */
+
+#include <net/wimax.h>
+#include <net/genetlink.h>
+#include <linux/wimax.h>
+#include <linux/security.h>
+#include <linux/rfkill.h>
+#include <linux/input.h>
+#include "wimax-internal.h"
+
+#define D_SUBMODULE op_rfkill
+#include "debug-levels.h"
+
+#ifdef CONFIG_RFKILL
+
+
+/**
+ * wimax_report_rfkill_hw - Reports changes in the hardware RF switch
+ *
+ * @wimax_dev: WiMAX device descriptor
+ *
+ * @state: New state of the RF Kill switch. %WIMAX_RF_ON radio on,
+ *     %WIMAX_RF_OFF radio off.
+ *
+ * When the device detects a change in the state of thehardware RF
+ * switch, it must call this function to let the WiMAX kernel stack
+ * know that the state has changed so it can be properly propagated.
+ *
+ * The WiMAX stack caches the state (the driver doesn't need to). As
+ * well, as the change is propagated it will come back as a request to
+ * change the software state to mirror the hardware state.
+ *
+ * If the device doesn't have a hardware kill switch, just report
+ * it on initialization as always on (%WIMAX_RF_ON, radio on).
+ */
+void wimax_report_rfkill_hw(struct wimax_dev *wimax_dev,
+			    enum wimax_rf_state state)
+{
+	int result;
+	struct device *dev = wimax_dev_to_dev(wimax_dev);
+	enum wimax_st wimax_state;
+	enum rfkill_state rfkill_state;
+
+	d_fnstart(3, dev, "(wimax_dev %p state %u)\n", wimax_dev, state);
+	BUG_ON(state == WIMAX_RF_QUERY);
+	BUG_ON(state != WIMAX_RF_ON && state != WIMAX_RF_OFF);
+
+	mutex_lock(&wimax_dev->mutex);
+	result = wimax_dev_is_ready(wimax_dev);
+	if (result < 0)
+		goto error_not_ready;
+
+	if (state != wimax_dev->rf_hw) {
+		wimax_dev->rf_hw = state;
+		rfkill_state = state == WIMAX_RF_ON ?
+			RFKILL_STATE_OFF : RFKILL_STATE_ON;
+		if (wimax_dev->rf_hw == WIMAX_RF_ON
+		    && wimax_dev->rf_sw == WIMAX_RF_ON)
+			wimax_state = WIMAX_ST_READY;
+		else
+			wimax_state = WIMAX_ST_RADIO_OFF;
+		__wimax_state_change(wimax_dev, wimax_state);
+		input_report_key(wimax_dev->rfkill_input, KEY_WIMAX,
+				 rfkill_state);
+	}
+error_not_ready:
+	mutex_unlock(&wimax_dev->mutex);
+	d_fnend(3, dev, "(wimax_dev %p state %u) = void [%d]\n",
+		wimax_dev, state, result);
+}
+EXPORT_SYMBOL_GPL(wimax_report_rfkill_hw);
+
+
+/**
+ * wimax_report_rfkill_sw - Reports changes in the software RF switch
+ *
+ * @wimax_dev: WiMAX device descriptor
+ *
+ * @state: New state of the RF kill switch. %WIMAX_RF_ON radio on,
+ *     %WIMAX_RF_OFF radio off.
+ *
+ * Reports changes in the software RF switch state to the the WiMAX
+ * stack.
+ *
+ * The main use is during initialization, so the driver can query the
+ * device for its current software radio kill switch state and feed it
+ * to the system.
+ *
+ * On the side, the device does not change the software state by
+ * itself. In practice, this can happen, as the device might decide to
+ * switch (in software) the radio off for different reasons.
+ */
+void wimax_report_rfkill_sw(struct wimax_dev *wimax_dev,
+			    enum wimax_rf_state state)
+{
+	int result;
+	struct device *dev = wimax_dev_to_dev(wimax_dev);
+	enum wimax_st wimax_state;
+
+	d_fnstart(3, dev, "(wimax_dev %p state %u)\n", wimax_dev, state);
+	BUG_ON(state == WIMAX_RF_QUERY);
+	BUG_ON(state != WIMAX_RF_ON && state != WIMAX_RF_OFF);
+
+	mutex_lock(&wimax_dev->mutex);
+	result = wimax_dev_is_ready(wimax_dev);
+	if (result < 0)
+		goto error_not_ready;
+
+	if (state != wimax_dev->rf_sw) {
+		wimax_dev->rf_sw = state;
+		if (wimax_dev->rf_hw == WIMAX_RF_ON
+		    && wimax_dev->rf_sw == WIMAX_RF_ON)
+			wimax_state = WIMAX_ST_READY;
+		else
+			wimax_state = WIMAX_ST_RADIO_OFF;
+		__wimax_state_change(wimax_dev, wimax_state);
+	}
+error_not_ready:
+	mutex_unlock(&wimax_dev->mutex);
+	d_fnend(3, dev, "(wimax_dev %p state %u) = void [%d]\n",
+		wimax_dev, state, result);
+}
+EXPORT_SYMBOL_GPL(wimax_report_rfkill_sw);
+
+
+/*
+ * Callback for the RF Kill toggle operation
+ *
+ * This function is called by:
+ *
+ * - The rfkill subsystem when the RF-Kill key is pressed in the
+ *   hardware and the driver notifies through
+ *   wimax_report_rfkill_hw(). The rfkill subsystem ends up calling back
+ *   here so the software RF Kill switch state is changed to reflect
+ *   the hardware switch state.
+ *
+ * - When the user sets the state through sysfs' rfkill/state file
+ *
+ * - When the user calls wimax_rfkill().
+ *
+ * This call blocks!
+ *
+ * WARNING! When we call rfkill_unregister(), this will be called with
+ * state 0!
+ *
+ * WARNING: wimax_dev must be locked
+ */
+static
+int __wimax_rf_toggle_radio(struct wimax_dev *wimax_dev,
+			    enum wimax_rf_state state)
+{
+	int result = 0;
+	struct device *dev = wimax_dev_to_dev(wimax_dev);
+	enum wimax_st wimax_state;
+
+	might_sleep();
+	d_fnstart(3, dev, "(wimax_dev %p state %u)\n", wimax_dev, state);
+	if (wimax_dev->rf_sw == state)
+		goto out_no_change;
+	if (wimax_dev->op_rfkill_sw_toggle != NULL)
+		result = wimax_dev->op_rfkill_sw_toggle(wimax_dev, state);
+	else if (state == WIMAX_RF_OFF)	/* No op? can't turn off */
+		result = -ENXIO;
+	else				/* No op? can turn on */
+		result = 0;		/* should never happen tho */
+	if (result >= 0) {
+		result = 0;
+		wimax_dev->rf_sw = state;
+		wimax_state = state == WIMAX_RF_ON ?
+			WIMAX_ST_READY : WIMAX_ST_RADIO_OFF;
+		__wimax_state_change(wimax_dev, wimax_state);
+	}
+out_no_change:
+	d_fnend(3, dev, "(wimax_dev %p state %u) = %d\n",
+		wimax_dev, state, result);
+	return result;
+}
+
+
+/*
+ * Translate from rfkill state to wimax state
+ *
+ * NOTE: Special state handling rules here
+ *
+ *     Just pretend the call didn't happen if we are in a state where
+ *     we know for sure it cannot be handled (WIMAX_ST_DOWN or
+ *     __WIMAX_ST_QUIESCING). rfkill() needs it to register and
+ *     unregister, as it will run this path.
+ *
+ * NOTE: This call will block until the operation is completed.
+ */
+static
+int wimax_rfkill_toggle_radio(void *data, enum rfkill_state state)
+{
+	int result;
+	struct wimax_dev *wimax_dev = data;
+	struct device *dev = wimax_dev_to_dev(wimax_dev);
+	enum wimax_rf_state rf_state;
+
+	d_fnstart(3, dev, "(wimax_dev %p state %u)\n", wimax_dev, state);
+	switch (state) {
+	case RFKILL_STATE_ON:
+		rf_state = WIMAX_RF_OFF;
+		break;
+	case RFKILL_STATE_OFF:
+		rf_state = WIMAX_RF_ON;
+		break;
+	default:
+		BUG();
+	}
+	mutex_lock(&wimax_dev->mutex);
+	if (wimax_dev->state <= __WIMAX_ST_QUIESCING)
+		result = 0;	/* just pretend it didn't happen */
+	else
+		result = __wimax_rf_toggle_radio(wimax_dev, rf_state);
+	mutex_unlock(&wimax_dev->mutex);
+	d_fnend(3, dev, "(wimax_dev %p state %u) = %d\n",
+		wimax_dev, state, result);
+	return result;
+}
+
+
+/**
+ * wimax_rfkill - Set the software RF switch state for a WiMAX device
+ *
+ * @wimax_dev: WiMAX device descriptor
+ *
+ * @state: New RF state.
+ *
+ * Returns:
+ *
+ * >= 0 toggle state if ok, < 0 errno code on error. The toggle state
+ * is returned as a bitmap, bit 0 being the hardware RF state, bit 1
+ * the software RF state.
+ *
+ * 0 means disabled (%WIMAX_RF_ON, radio on), 1 means enabled radio
+ * off (%WIMAX_RF_OFF).
+ *
+ * Description:
+ *
+ * Called by the user when he wants to request the WiMAX radio to be
+ * switched on (%WIMAX_RF_ON) or off (%WIMAX_RF_OFF). With
+ * %WIMAX_RF_QUERY, just the current state is returned.
+ *
+ * NOTE:
+ *
+ * This call will block until the operation is complete.
+ */
+int wimax_rfkill(struct wimax_dev *wimax_dev, enum wimax_rf_state state)
+{
+	int result;
+	struct device *dev = wimax_dev_to_dev(wimax_dev);
+
+	d_fnstart(3, dev, "(wimax_dev %p state %u)\n", wimax_dev, state);
+	mutex_lock(&wimax_dev->mutex);
+	result = wimax_dev_is_ready(wimax_dev);
+	if (result < 0)
+		goto error_not_ready;
+	switch (state) {
+	case WIMAX_RF_ON:
+	case WIMAX_RF_OFF:
+		result = __wimax_rf_toggle_radio(wimax_dev, state);
+		if (result < 0)
+			goto error;
+		break;
+	case WIMAX_RF_QUERY:
+		break;
+	default:
+		result = -EINVAL;
+		goto error;
+	}
+	result = wimax_dev->rf_sw << 1 | wimax_dev->rf_hw;
+error:
+error_not_ready:
+	mutex_unlock(&wimax_dev->mutex);
+	d_fnend(3, dev, "(wimax_dev %p state %u) = %d\n",
+		wimax_dev, state, result);
+	return result;
+}
+EXPORT_SYMBOL(wimax_rfkill);
+
+
+/*
+ * Register a new WiMAX device's RF Kill support
+ *
+ * WARNING: wimax_dev->mutex must be unlocked
+ */
+int wimax_rfkill_add(struct wimax_dev *wimax_dev)
+{
+	int result;
+	struct rfkill *rfkill;
+	struct input_dev *input_dev;
+	struct device *dev = wimax_dev_to_dev(wimax_dev);
+
+	d_fnstart(3, dev, "(wimax_dev %p)\n", wimax_dev);
+	/* Initialize RF Kill */
+	result = -ENOMEM;
+	rfkill = rfkill_allocate(dev, RFKILL_TYPE_WIMAX);
+	if (rfkill == NULL)
+		goto error_rfkill_allocate;
+	wimax_dev->rfkill = rfkill;
+
+	rfkill->name = wimax_dev->name;
+	rfkill->state = RFKILL_STATE_OFF;
+	rfkill->data = wimax_dev;
+	rfkill->toggle_radio = wimax_rfkill_toggle_radio;
+	rfkill->user_claim_unsupported = 1;
+
+	/* Initialize the input device for the hw key */
+	input_dev = input_allocate_device();
+	if (input_dev == NULL)
+		goto error_input_allocate;
+	wimax_dev->rfkill_input = input_dev;
+	d_printf(1, dev, "rfkill %p input %p\n", rfkill, input_dev);
+
+	input_dev->name = wimax_dev->name;
+	/* FIXME: get a real device bus ID and stuff? do we care? */
+	input_dev->id.bustype = BUS_HOST;
+	input_dev->id.vendor = 0xffff;
+	input_dev->evbit[0] = BIT(EV_KEY);
+	set_bit(KEY_WIMAX, input_dev->keybit);
+
+	/* Register both */
+	result = input_register_device(wimax_dev->rfkill_input);
+	if (result < 0)
+		goto error_input_register;
+	result = rfkill_register(wimax_dev->rfkill);
+	if (result < 0)
+		goto error_rfkill_register;
+
+	/* If there is no SW toggle op, SW RFKill is always on */
+	if (wimax_dev->op_rfkill_sw_toggle == NULL)
+		wimax_dev->rf_sw = WIMAX_RF_ON;
+
+	d_fnend(3, dev, "(wimax_dev %p) = 0\n", wimax_dev);
+	return 0;
+
+	/* if rfkill_register() suceeds, can't use rfkill_free() any
+	 * more, only rfkill_unregister() [it owns the refcount]; with
+	 * the input device we have the same issue--hence the if. */
+error_rfkill_register:
+	input_unregister_device(wimax_dev->rfkill_input);
+	wimax_dev->rfkill_input = NULL;
+error_input_register:
+	if (wimax_dev->rfkill_input)
+		input_free_device(wimax_dev->rfkill_input);
+error_input_allocate:
+	rfkill_free(wimax_dev->rfkill);
+error_rfkill_allocate:
+	d_fnend(3, dev, "(wimax_dev %p) = %d\n", wimax_dev, result);
+	return result;
+}
+
+
+/*
+ * Deregister a WiMAX device's RF Kill support
+ *
+ * Ick, we can't call rfkill_free() after rfkill_unregister()...oh
+ * well.
+ *
+ * WARNING: wimax_dev->mutex must be unlocked
+ */
+void wimax_rfkill_rm(struct wimax_dev *wimax_dev)
+{
+	struct device *dev = wimax_dev_to_dev(wimax_dev);
+	d_fnstart(3, dev, "(wimax_dev %p)\n", wimax_dev);
+	rfkill_unregister(wimax_dev->rfkill);	/* frees */
+	input_unregister_device(wimax_dev->rfkill_input);
+	d_fnend(3, dev, "(wimax_dev %p)\n", wimax_dev);
+}
+
+
+#else /* #ifdef CONFIG_RFKILL */
+
+void wimax_report_rfkill_hw(struct wimax_dev *wimax_dev,
+			    enum wimax_rf_state state)
+{
+}
+EXPORT_SYMBOL_GPL(wimax_report_rfkill_hw);
+
+void wimax_report_rfkill_sw(struct wimax_dev *wimax_dev,
+			    enum wimax_rf_state state)
+{
+}
+EXPORT_SYMBOL_GPL(wimax_report_rfkill_sw);
+
+int wimax_rfkill(struct wimax_dev *wimax_dev,
+		 enum wimax_rf_state state)
+{
+	return WIMAX_RF_ON << 1 | WIMAX_RF_ON;
+}
+EXPORT_SYMBOL_GPL(wimax_rfkill);
+
+int wimax_rfkill_add(struct wimax_dev *wimax_dev)
+{
+	return 0;
+}
+
+void wimax_rfkill_rm(struct wimax_dev *wimax_dev)
+{
+}
+
+#endif /* #ifdef CONFIG_RFKILL */
+
+
+/*
+ * Exporting to user space over generic netlink
+ *
+ * Parse the rfkill command from user space, return a combination
+ * value that describe the states of the different toggles.
+ *
+ * Only one attribute: the new state requested (on, off or no change,
+ * just query).
+ */
+
+static const
+struct nla_policy wimax_gnl_rfkill_policy[WIMAX_GNL_ATTR_MAX + 1] = {
+	[WIMAX_GNL_RFKILL_IFIDX] = {
+		.type = NLA_U32,
+	},
+	[WIMAX_GNL_RFKILL_STATE] = {
+		.type = NLA_U32		/* enum wimax_rf_state */
+	},
+};
+
+
+static
+int wimax_gnl_doit_rfkill(struct sk_buff *skb, struct genl_info *info)
+{
+	int result, ifindex;
+	struct wimax_dev *wimax_dev;
+	struct device *dev;
+	enum wimax_rf_state new_state;
+
+	d_fnstart(3, NULL, "(skb %p info %p)\n", skb, info);
+	result = -ENODEV;
+	if (info->attrs[WIMAX_GNL_RFKILL_IFIDX] == NULL) {
+		printk(KERN_ERR "WIMAX_GNL_OP_RFKILL: can't find IFIDX "
+			"attribute\n");
+		goto error_no_wimax_dev;
+	}
+	ifindex = nla_get_u32(info->attrs[WIMAX_GNL_RFKILL_IFIDX]);
+	wimax_dev = wimax_dev_get_by_genl_info(info, ifindex);
+	if (wimax_dev == NULL)
+		goto error_no_wimax_dev;
+	dev = wimax_dev_to_dev(wimax_dev);
+	result = -EINVAL;
+	if (info->attrs[WIMAX_GNL_RFKILL_STATE] == NULL) {
+		dev_err(dev, "WIMAX_GNL_RFKILL: can't find RFKILL_STATE "
+			"attribute\n");
+		goto error_no_pid;
+	}
+	new_state = nla_get_u32(info->attrs[WIMAX_GNL_RFKILL_STATE]);
+
+	/* Execute the operation and send the result back to user space */
+	result = wimax_rfkill(wimax_dev, new_state);
+error_no_pid:
+	dev_put(wimax_dev->net_dev);
+error_no_wimax_dev:
+	d_fnend(3, NULL, "(skb %p info %p) = %d\n", skb, info, result);
+	return result;
+}
+
+
+struct genl_ops wimax_gnl_rfkill = {
+	.cmd = WIMAX_GNL_OP_RFKILL,
+	.flags = GENL_ADMIN_PERM,
+	.policy = wimax_gnl_rfkill_policy,
+	.doit = wimax_gnl_doit_rfkill,
+	.dumpit = NULL,
+};
+
diff --git a/net/wimax/stack.c b/net/wimax/stack.c
new file mode 100644
index 000000000000..d4da92f8981a
--- /dev/null
+++ b/net/wimax/stack.c
@@ -0,0 +1,599 @@
+/*
+ * Linux WiMAX
+ * Initialization, addition and removal of wimax devices
+ *
+ *
+ * Copyright (C) 2005-2006 Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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.
+ *
+ *
+ * This implements:
+ *
+ *   - basic life cycle of 'struct wimax_dev' [wimax_dev_*()]; on
+ *     addition/registration initialize all subfields and allocate
+ *     generic netlink resources for user space communication. On
+ *     removal/unregistration, undo all that.
+ *
+ *   - device state machine [wimax_state_change()] and support to send
+ *     reports to user space when the state changes
+ *     [wimax_gnl_re_state_change*()].
+ *
+ * See include/net/wimax.h for rationales and design.
+ *
+ * ROADMAP
+ *
+ * [__]wimax_state_change()     Called by drivers to update device's state
+ *   wimax_gnl_re_state_change_alloc()
+ *   wimax_gnl_re_state_change_send()
+ *
+ * wimax_dev_init()	        Init a device
+ * wimax_dev_add()              Register
+ *   wimax_rfkill_add()
+ *   wimax_gnl_add()            Register all the generic netlink resources.
+ *   wimax_id_table_add()
+ * wimax_dev_rm()               Unregister
+ *   wimax_id_table_rm()
+ *   wimax_gnl_rm()
+ *   wimax_rfkill_rm()
+ */
+#include <linux/device.h>
+#include <net/genetlink.h>
+#include <linux/netdevice.h>
+#include <linux/wimax.h>
+#include "wimax-internal.h"
+
+
+#define D_SUBMODULE stack
+#include "debug-levels.h"
+
+/*
+ * Authoritative source for the RE_STATE_CHANGE attribute policy
+ *
+ * We don't really use it here, but /me likes to keep the definition
+ * close to where the data is generated.
+ */
+/*
+static const
+struct nla_policy wimax_gnl_re_status_change[WIMAX_GNL_ATTR_MAX + 1] = {
+	[WIMAX_GNL_STCH_STATE_OLD] = { .type = NLA_U8 },
+	[WIMAX_GNL_STCH_STATE_NEW] = { .type = NLA_U8 },
+};
+*/
+
+
+/*
+ * Allocate a Report State Change message
+ *
+ * @header: save it, you need it for _send()
+ *
+ * Creates and fills a basic state change message; different code
+ * paths can then add more attributes to the message as needed.
+ *
+ * Use wimax_gnl_re_state_change_send() to send the returned skb.
+ *
+ * Returns: skb with the genl message if ok, IS_ERR() ptr on error
+ *     with an errno code.
+ */
+static
+struct sk_buff *wimax_gnl_re_state_change_alloc(
+	struct wimax_dev *wimax_dev,
+	enum wimax_st new_state, enum wimax_st old_state,
+	void **header)
+{
+	int result;
+	struct device *dev = wimax_dev_to_dev(wimax_dev);
+	void *data;
+	struct sk_buff *report_skb;
+
+	d_fnstart(3, dev, "(wimax_dev %p new_state %u old_state %u)\n",
+		  wimax_dev, new_state, old_state);
+	result = -ENOMEM;
+	report_skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+	if (report_skb == NULL) {
+		dev_err(dev, "RE_STCH: can't create message\n");
+		goto error_new;
+	}
+	data = genlmsg_put(report_skb, 0, wimax_gnl_mcg.id, &wimax_gnl_family,
+			   0, WIMAX_GNL_RE_STATE_CHANGE);
+	if (data == NULL) {
+		dev_err(dev, "RE_STCH: can't put data into message\n");
+		goto error_put;
+	}
+	*header = data;
+
+	result = nla_put_u8(report_skb, WIMAX_GNL_STCH_STATE_OLD, old_state);
+	if (result < 0) {
+		dev_err(dev, "RE_STCH: Error adding OLD attr: %d\n", result);
+		goto error_put;
+	}
+	result = nla_put_u8(report_skb, WIMAX_GNL_STCH_STATE_NEW, new_state);
+	if (result < 0) {
+		dev_err(dev, "RE_STCH: Error adding NEW attr: %d\n", result);
+		goto error_put;
+	}
+	result = nla_put_u32(report_skb, WIMAX_GNL_STCH_IFIDX,
+			     wimax_dev->net_dev->ifindex);
+	if (result < 0) {
+		dev_err(dev, "RE_STCH: Error adding IFINDEX attribute\n");
+		goto error_put;
+	}
+	d_fnend(3, dev, "(wimax_dev %p new_state %u old_state %u) = %p\n",
+		wimax_dev, new_state, old_state, report_skb);
+	return report_skb;
+
+error_put:
+	nlmsg_free(report_skb);
+error_new:
+	d_fnend(3, dev, "(wimax_dev %p new_state %u old_state %u) = %d\n",
+		wimax_dev, new_state, old_state, result);
+	return ERR_PTR(result);
+}
+
+
+/*
+ * Send a Report State Change message (as created with _alloc).
+ *
+ * @report_skb: as returned by wimax_gnl_re_state_change_alloc()
+ * @header: as returned by wimax_gnl_re_state_change_alloc()
+ *
+ * Returns: 0 if ok, < 0 errno code on error.
+ *
+ * If the message is  NULL, pretend it didn't happen.
+ */
+static
+int wimax_gnl_re_state_change_send(
+	struct wimax_dev *wimax_dev, struct sk_buff *report_skb,
+	void *header)
+{
+	int result = 0;
+	struct device *dev = wimax_dev_to_dev(wimax_dev);
+	d_fnstart(3, dev, "(wimax_dev %p report_skb %p)\n",
+		  wimax_dev, report_skb);
+	if (report_skb == NULL)
+		goto out;
+	genlmsg_end(report_skb, header);
+	result = genlmsg_multicast(report_skb, 0, wimax_gnl_mcg.id, GFP_KERNEL);
+	if (result == -ESRCH)	/* Nobody connected, ignore it */
+		result = 0;	/* btw, the skb is freed already */
+	if (result < 0) {
+		dev_err(dev, "RE_STCH: Error sending: %d\n", result);
+		nlmsg_free(report_skb);
+	}
+out:
+	d_fnend(3, dev, "(wimax_dev %p report_skb %p) = %d\n",
+		wimax_dev, report_skb, result);
+	return result;
+}
+
+
+static
+void __check_new_state(enum wimax_st old_state, enum wimax_st new_state,
+		       unsigned allowed_states_bm)
+{
+	if (WARN_ON(((1 << new_state) & allowed_states_bm) == 0)) {
+		printk(KERN_ERR "SW BUG! Forbidden state change %u -> %u\n",
+			old_state, new_state);
+	}
+}
+
+
+/*
+ * Set the current state of a WiMAX device [unlocking version of
+ * wimax_state_change().
+ */
+void __wimax_state_change(struct wimax_dev *wimax_dev, enum wimax_st new_state)
+{
+	struct device *dev = wimax_dev_to_dev(wimax_dev);
+	enum wimax_st old_state = wimax_dev->state;
+	struct sk_buff *stch_skb;
+	void *header;
+
+	d_fnstart(3, dev, "(wimax_dev %p new_state %u [old %u])\n",
+		  wimax_dev, new_state, old_state);
+
+	if (WARN_ON(new_state >= __WIMAX_ST_INVALID)) {
+		dev_err(dev, "SW BUG: requesting invalid state %u\n",
+			new_state);
+		goto out;
+	}
+	if (old_state == new_state)
+		goto out;
+	header = NULL;	/* gcc complains? can't grok why */
+	stch_skb = wimax_gnl_re_state_change_alloc(
+		wimax_dev, new_state, old_state, &header);
+
+	/* Verify the state transition and do exit-from-state actions */
+	switch (old_state) {
+	case __WIMAX_ST_NULL:
+		__check_new_state(old_state, new_state,
+				  1 << WIMAX_ST_DOWN);
+		break;
+	case WIMAX_ST_DOWN:
+		__check_new_state(old_state, new_state,
+				  1 << __WIMAX_ST_QUIESCING
+				  | 1 << WIMAX_ST_UNINITIALIZED
+				  | 1 << WIMAX_ST_RADIO_OFF);
+		break;
+	case __WIMAX_ST_QUIESCING:
+		__check_new_state(old_state, new_state, 1 << WIMAX_ST_DOWN);
+		break;
+	case WIMAX_ST_UNINITIALIZED:
+		__check_new_state(old_state, new_state,
+				  1 << __WIMAX_ST_QUIESCING
+				  | 1 << WIMAX_ST_RADIO_OFF);
+		break;
+	case WIMAX_ST_RADIO_OFF:
+		__check_new_state(old_state, new_state,
+				  1 << __WIMAX_ST_QUIESCING
+				  | 1 << WIMAX_ST_READY);
+		break;
+	case WIMAX_ST_READY:
+		__check_new_state(old_state, new_state,
+				  1 << __WIMAX_ST_QUIESCING
+				  | 1 << WIMAX_ST_RADIO_OFF
+				  | 1 << WIMAX_ST_SCANNING
+				  | 1 << WIMAX_ST_CONNECTING
+				  | 1 << WIMAX_ST_CONNECTED);
+		break;
+	case WIMAX_ST_SCANNING:
+		__check_new_state(old_state, new_state,
+				  1 << __WIMAX_ST_QUIESCING
+				  | 1 << WIMAX_ST_RADIO_OFF
+				  | 1 << WIMAX_ST_READY
+				  | 1 << WIMAX_ST_CONNECTING
+				  | 1 << WIMAX_ST_CONNECTED);
+		break;
+	case WIMAX_ST_CONNECTING:
+		__check_new_state(old_state, new_state,
+				  1 << __WIMAX_ST_QUIESCING
+				  | 1 << WIMAX_ST_RADIO_OFF
+				  | 1 << WIMAX_ST_READY
+				  | 1 << WIMAX_ST_SCANNING
+				  | 1 << WIMAX_ST_CONNECTED);
+		break;
+	case WIMAX_ST_CONNECTED:
+		__check_new_state(old_state, new_state,
+				  1 << __WIMAX_ST_QUIESCING
+				  | 1 << WIMAX_ST_RADIO_OFF
+				  | 1 << WIMAX_ST_READY);
+		netif_tx_disable(wimax_dev->net_dev);
+		netif_carrier_off(wimax_dev->net_dev);
+		break;
+	case __WIMAX_ST_INVALID:
+	default:
+		dev_err(dev, "SW BUG: wimax_dev %p is in unknown state %u\n",
+			wimax_dev, wimax_dev->state);
+		WARN_ON(1);
+		goto out;
+	}
+
+	/* Execute the actions of entry to the new state */
+	switch (new_state) {
+	case __WIMAX_ST_NULL:
+		dev_err(dev, "SW BUG: wimax_dev %p entering NULL state "
+			"from %u\n", wimax_dev, wimax_dev->state);
+		WARN_ON(1);		/* Nobody can enter this state */
+		break;
+	case WIMAX_ST_DOWN:
+		break;
+	case __WIMAX_ST_QUIESCING:
+		break;
+	case WIMAX_ST_UNINITIALIZED:
+		break;
+	case WIMAX_ST_RADIO_OFF:
+		break;
+	case WIMAX_ST_READY:
+		break;
+	case WIMAX_ST_SCANNING:
+		break;
+	case WIMAX_ST_CONNECTING:
+		break;
+	case WIMAX_ST_CONNECTED:
+		netif_carrier_on(wimax_dev->net_dev);
+		netif_wake_queue(wimax_dev->net_dev);
+		break;
+	case __WIMAX_ST_INVALID:
+	default:
+		BUG();
+	}
+	__wimax_state_set(wimax_dev, new_state);
+	if (stch_skb)
+		wimax_gnl_re_state_change_send(wimax_dev, stch_skb, header);
+out:
+	d_fnend(3, dev, "(wimax_dev %p new_state %u [old %u]) = void\n",
+		wimax_dev, new_state, old_state);
+	return;
+}
+
+
+/**
+ * wimax_state_change - Set the current state of a WiMAX device
+ *
+ * @wimax_dev: WiMAX device descriptor (properly referenced)
+ * @new_state: New state to switch to
+ *
+ * This implements the state changes for the wimax devices. It will
+ *
+ * - verify that the state transition is legal (for now it'll just
+ *   print a warning if not) according to the table in
+ *   linux/wimax.h's documentation for 'enum wimax_st'.
+ *
+ * - perform the actions needed for leaving the current state and
+ *   whichever are needed for entering the new state.
+ *
+ * - issue a report to user space indicating the new state (and an
+ *   optional payload with information about the new state).
+ *
+ * NOTE: @wimax_dev must be locked
+ */
+void wimax_state_change(struct wimax_dev *wimax_dev, enum wimax_st new_state)
+{
+	mutex_lock(&wimax_dev->mutex);
+	__wimax_state_change(wimax_dev, new_state);
+	mutex_unlock(&wimax_dev->mutex);
+	return;
+}
+EXPORT_SYMBOL_GPL(wimax_state_change);
+
+
+/**
+ * wimax_state_get() - Return the current state of a WiMAX device
+ *
+ * @wimax_dev: WiMAX device descriptor
+ *
+ * Returns: Current state of the device according to its driver.
+ */
+enum wimax_st wimax_state_get(struct wimax_dev *wimax_dev)
+{
+	enum wimax_st state;
+	mutex_lock(&wimax_dev->mutex);
+	state = wimax_dev->state;
+	mutex_unlock(&wimax_dev->mutex);
+	return state;
+}
+EXPORT_SYMBOL_GPL(wimax_state_get);
+
+
+/**
+ * wimax_dev_init - initialize a newly allocated instance
+ *
+ * @wimax_dev: WiMAX device descriptor to initialize.
+ *
+ * Initializes fields of a freshly allocated @wimax_dev instance. This
+ * function assumes that after allocation, the memory occupied by
+ * @wimax_dev was zeroed.
+ */
+void wimax_dev_init(struct wimax_dev *wimax_dev)
+{
+	INIT_LIST_HEAD(&wimax_dev->id_table_node);
+	__wimax_state_set(wimax_dev, WIMAX_ST_UNINITIALIZED);
+	mutex_init(&wimax_dev->mutex);
+	mutex_init(&wimax_dev->mutex_reset);
+}
+EXPORT_SYMBOL_GPL(wimax_dev_init);
+
+/*
+ * This extern is declared here because it's easier to keep track --
+ * both declarations are a list of the same
+ */
+extern struct genl_ops
+	wimax_gnl_msg_from_user,
+	wimax_gnl_reset,
+	wimax_gnl_rfkill;
+
+static
+struct genl_ops *wimax_gnl_ops[] = {
+	&wimax_gnl_msg_from_user,
+	&wimax_gnl_reset,
+	&wimax_gnl_rfkill,
+};
+
+
+static
+size_t wimax_addr_scnprint(char *addr_str, size_t addr_str_size,
+			   unsigned char *addr, size_t addr_len)
+{
+	unsigned cnt, total;
+	for (total = cnt = 0; cnt < addr_len; cnt++)
+		total += scnprintf(addr_str + total, addr_str_size - total,
+				   "%02x%c", addr[cnt],
+				   cnt == addr_len - 1 ? '\0' : ':');
+	return total;
+}
+
+
+/**
+ * wimax_dev_add - Register a new WiMAX device
+ *
+ * @wimax_dev: WiMAX device descriptor (as embedded in your @net_dev's
+ *     priv data). You must have called wimax_dev_init() on it before.
+ *
+ * @net_dev: net device the @wimax_dev is associated with. The
+ *     function expects SET_NETDEV_DEV() and register_netdev() were
+ *     already called on it.
+ *
+ * Registers the new WiMAX device, sets up the user-kernel control
+ * interface (generic netlink) and common WiMAX infrastructure.
+ *
+ * Note that the parts that will allow interaction with user space are
+ * setup at the very end, when the rest is in place, as once that
+ * happens, the driver might get user space control requests via
+ * netlink or from debugfs that might translate into calls into
+ * wimax_dev->op_*().
+ */
+int wimax_dev_add(struct wimax_dev *wimax_dev, struct net_device *net_dev)
+{
+	int result;
+	struct device *dev = net_dev->dev.parent;
+	char addr_str[32];
+
+	d_fnstart(3, dev, "(wimax_dev %p net_dev %p)\n", wimax_dev, net_dev);
+
+	/* Do the RFKILL setup before locking, as RFKILL will call
+	 * into our functions. */
+	wimax_dev->net_dev = net_dev;
+	result = wimax_rfkill_add(wimax_dev);
+	if (result < 0)
+		goto error_rfkill_add;
+
+	/* Set up user-space interaction */
+	mutex_lock(&wimax_dev->mutex);
+	wimax_id_table_add(wimax_dev);
+	result = wimax_debugfs_add(wimax_dev);
+	if (result < 0) {
+		dev_err(dev, "cannot initialize debugfs: %d\n",
+			result);
+		goto error_debugfs_add;
+	}
+
+	__wimax_state_set(wimax_dev, WIMAX_ST_DOWN);
+	mutex_unlock(&wimax_dev->mutex);
+
+	wimax_addr_scnprint(addr_str, sizeof(addr_str),
+			    net_dev->dev_addr, net_dev->addr_len);
+	dev_err(dev, "WiMAX interface %s (%s) ready\n",
+		net_dev->name, addr_str);
+	d_fnend(3, dev, "(wimax_dev %p net_dev %p) = 0\n", wimax_dev, net_dev);
+	return 0;
+
+error_debugfs_add:
+	wimax_id_table_rm(wimax_dev);
+	mutex_unlock(&wimax_dev->mutex);
+	wimax_rfkill_rm(wimax_dev);
+error_rfkill_add:
+	d_fnend(3, dev, "(wimax_dev %p net_dev %p) = %d\n",
+		wimax_dev, net_dev, result);
+	return result;
+}
+EXPORT_SYMBOL_GPL(wimax_dev_add);
+
+
+/**
+ * wimax_dev_rm - Unregister an existing WiMAX device
+ *
+ * @wimax_dev: WiMAX device descriptor
+ *
+ * Unregisters a WiMAX device previously registered for use with
+ * wimax_add_rm().
+ *
+ * IMPORTANT! Must call before calling unregister_netdev().
+ *
+ * After this function returns, you will not get any more user space
+ * control requests (via netlink or debugfs) and thus to wimax_dev->ops.
+ *
+ * Reentrancy control is ensured by setting the state to
+ * %__WIMAX_ST_QUIESCING. rfkill operations coming through
+ * wimax_*rfkill*() will be stopped by the quiescing state; ops coming
+ * from the rfkill subsystem will be stopped by the support being
+ * removed by wimax_rfkill_rm().
+ */
+void wimax_dev_rm(struct wimax_dev *wimax_dev)
+{
+	d_fnstart(3, NULL, "(wimax_dev %p)\n", wimax_dev);
+
+	mutex_lock(&wimax_dev->mutex);
+	__wimax_state_change(wimax_dev, __WIMAX_ST_QUIESCING);
+	wimax_debugfs_rm(wimax_dev);
+	wimax_id_table_rm(wimax_dev);
+	__wimax_state_change(wimax_dev, WIMAX_ST_DOWN);
+	mutex_unlock(&wimax_dev->mutex);
+	wimax_rfkill_rm(wimax_dev);
+	d_fnend(3, NULL, "(wimax_dev %p) = void\n", wimax_dev);
+}
+EXPORT_SYMBOL_GPL(wimax_dev_rm);
+
+struct genl_family wimax_gnl_family = {
+	.id = GENL_ID_GENERATE,
+	.name = "WiMAX",
+	.version = WIMAX_GNL_VERSION,
+	.hdrsize = 0,
+	.maxattr = WIMAX_GNL_ATTR_MAX,
+};
+
+struct genl_multicast_group wimax_gnl_mcg = {
+	.name = "msg",
+};
+
+
+
+/* Shutdown the wimax stack */
+static
+int __init wimax_subsys_init(void)
+{
+	int result, cnt;
+
+	d_fnstart(4, NULL, "()\n");
+	snprintf(wimax_gnl_family.name, sizeof(wimax_gnl_family.name),
+		 "WiMAX");
+	result = genl_register_family(&wimax_gnl_family);
+	if (unlikely(result < 0)) {
+		printk(KERN_ERR "cannot register generic netlink family: %d\n",
+		       result);
+		goto error_register_family;
+	}
+
+	for (cnt = 0; cnt < ARRAY_SIZE(wimax_gnl_ops); cnt++) {
+		result = genl_register_ops(&wimax_gnl_family,
+					   wimax_gnl_ops[cnt]);
+		d_printf(4, NULL, "registering generic netlink op code "
+			 "%u: %d\n", wimax_gnl_ops[cnt]->cmd, result);
+		if (unlikely(result < 0)) {
+			printk(KERN_ERR "cannot register generic netlink op "
+			       "code %u: %d\n",
+			       wimax_gnl_ops[cnt]->cmd, result);
+			goto error_register_ops;
+		}
+	}
+
+	result = genl_register_mc_group(&wimax_gnl_family, &wimax_gnl_mcg);
+	if (result < 0)
+		goto error_mc_group;
+	d_fnend(4, NULL, "() = 0\n");
+	return 0;
+
+error_mc_group:
+error_register_ops:
+	for (cnt--; cnt >= 0; cnt--)
+		genl_unregister_ops(&wimax_gnl_family,
+				    wimax_gnl_ops[cnt]);
+	genl_unregister_family(&wimax_gnl_family);
+error_register_family:
+	d_fnend(4, NULL, "() = %d\n", result);
+	return result;
+
+}
+module_init(wimax_subsys_init);
+
+
+/* Shutdown the wimax stack */
+static
+void __exit wimax_subsys_exit(void)
+{
+	int cnt;
+	wimax_id_table_release();
+	genl_unregister_mc_group(&wimax_gnl_family, &wimax_gnl_mcg);
+	for (cnt = ARRAY_SIZE(wimax_gnl_ops) - 1; cnt >= 0; cnt--)
+		genl_unregister_ops(&wimax_gnl_family,
+				    wimax_gnl_ops[cnt]);
+	genl_unregister_family(&wimax_gnl_family);
+}
+module_exit(wimax_subsys_exit);
+
+MODULE_AUTHOR("Intel Corporation <linux-wimax@intel.com>");
+MODULE_DESCRIPTION("Linux WiMAX stack");
+MODULE_LICENSE("GPL");
+
diff --git a/net/wimax/wimax-internal.h b/net/wimax/wimax-internal.h
new file mode 100644
index 000000000000..1e743d214856
--- /dev/null
+++ b/net/wimax/wimax-internal.h
@@ -0,0 +1,91 @@
+/*
+ * Linux WiMAX
+ * Internal API for kernel space WiMAX stack
+ *
+ *
+ * Copyright (C) 2007 Intel Corporation <linux-wimax@intel.com>
+ * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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.
+ *
+ *
+ * This header file is for declarations and definitions internal to
+ * the WiMAX stack. For public APIs and documentation, see
+ * include/net/wimax.h and include/linux/wimax.h.
+ */
+
+#ifndef __WIMAX_INTERNAL_H__
+#define __WIMAX_INTERNAL_H__
+#ifdef __KERNEL__
+
+#include <linux/device.h>
+#include <net/wimax.h>
+
+
+/*
+ * Decide if a (locked) device is ready for use
+ *
+ * Before using the device structure, it must be locked
+ * (wimax_dev->mutex). As well, most operations need to call this
+ * function to check if the state is the right one.
+ *
+ * An error value will be returned if the state is not the right
+ * one. In that case, the caller should not attempt to use the device
+ * and just unlock it.
+ */
+static inline __must_check
+int wimax_dev_is_ready(struct wimax_dev *wimax_dev)
+{
+	if (wimax_dev->state == __WIMAX_ST_NULL)
+		return -EINVAL;	/* Device is not even registered! */
+	if (wimax_dev->state == WIMAX_ST_DOWN)
+		return -ENOMEDIUM;
+	if (wimax_dev->state == __WIMAX_ST_QUIESCING)
+		return -ESHUTDOWN;
+	return 0;
+}
+
+
+static inline
+void __wimax_state_set(struct wimax_dev *wimax_dev, enum wimax_st state)
+{
+	wimax_dev->state = state;
+}
+extern void __wimax_state_change(struct wimax_dev *, enum wimax_st);
+
+#ifdef CONFIG_DEBUG_FS
+extern int wimax_debugfs_add(struct wimax_dev *);
+extern void wimax_debugfs_rm(struct wimax_dev *);
+#else
+static inline int wimax_debugfs_add(struct wimax_dev *wimax_dev)
+{
+	return 0;
+}
+static inline void wimax_debugfs_rm(struct wimax_dev *wimax_dev) {}
+#endif
+
+extern void wimax_id_table_add(struct wimax_dev *);
+extern struct wimax_dev *wimax_dev_get_by_genl_info(struct genl_info *, int);
+extern void wimax_id_table_rm(struct wimax_dev *);
+extern void wimax_id_table_release(void);
+
+extern int wimax_rfkill_add(struct wimax_dev *);
+extern void wimax_rfkill_rm(struct wimax_dev *);
+
+extern struct genl_family wimax_gnl_family;
+extern struct genl_multicast_group wimax_gnl_mcg;
+
+#endif /* #ifdef __KERNEL__ */
+#endif /* #ifndef __WIMAX_INTERNAL_H__ */
diff --git a/samples/firmware_class/firmware_sample_driver.c b/samples/firmware_class/firmware_sample_driver.c
index 11114f389c49..219a29896603 100644
--- a/samples/firmware_class/firmware_sample_driver.c
+++ b/samples/firmware_class/firmware_sample_driver.c
@@ -100,7 +100,7 @@ static void sample_probe_async(void)
 		       " request_firmware_nowait failed\n");
 }
 
-static int sample_init(void)
+static int __init sample_init(void)
 {
 	device_initialize(&ghost_device);
 	/* since there is no real hardware insertion I just call the
diff --git a/samples/kobject/kobject-example.c b/samples/kobject/kobject-example.c
index 08d0d3ff3263..8d9b55a12023 100644
--- a/samples/kobject/kobject-example.c
+++ b/samples/kobject/kobject-example.c
@@ -101,7 +101,7 @@ static struct attribute_group attr_group = {
 
 static struct kobject *example_kobj;
 
-static int example_init(void)
+static int __init example_init(void)
 {
 	int retval;
 
@@ -126,7 +126,7 @@ static int example_init(void)
 	return retval;
 }
 
-static void example_exit(void)
+static void __exit example_exit(void)
 {
 	kobject_put(example_kobj);
 }
diff --git a/samples/kobject/kset-example.c b/samples/kobject/kset-example.c
index 7395c0bbae18..45b7d56fb541 100644
--- a/samples/kobject/kset-example.c
+++ b/samples/kobject/kset-example.c
@@ -229,7 +229,7 @@ static void destroy_foo_obj(struct foo_obj *foo)
 	kobject_put(&foo->kobj);
 }
 
-static int example_init(void)
+static int __init example_init(void)
 {
 	/*
 	 * Create a kset with the name of "kset_example",
@@ -264,7 +264,7 @@ foo_error:
 	return -EINVAL;
 }
 
-static void example_exit(void)
+static void __exit example_exit(void)
 {
 	destroy_foo_obj(baz_obj);
 	destroy_foo_obj(bar_obj);
diff --git a/samples/markers/marker-example.c b/samples/markers/marker-example.c
index e90dc5d04392..e9cd9c0bc84f 100644
--- a/samples/markers/marker-example.c
+++ b/samples/markers/marker-example.c
@@ -30,7 +30,7 @@ static struct file_operations mark_ops = {
 	.open = my_open,
 };
 
-static int example_init(void)
+static int __init example_init(void)
 {
 	printk(KERN_ALERT "example init\n");
 	pentry_example = proc_create("marker-example", 0444, NULL, &mark_ops);
@@ -39,7 +39,7 @@ static int example_init(void)
 	return 0;
 }
 
-static void example_exit(void)
+static void __exit example_exit(void)
 {
 	printk(KERN_ALERT "example exit\n");
 	remove_proc_entry("marker-example", NULL);
diff --git a/samples/tracepoints/tracepoint-probe-sample.c b/samples/tracepoints/tracepoint-probe-sample.c
index e3a964889dc7..9e60eb6ca2d8 100644
--- a/samples/tracepoints/tracepoint-probe-sample.c
+++ b/samples/tracepoints/tracepoint-probe-sample.c
@@ -28,7 +28,7 @@ static void probe_subsys_eventb(void)
 	printk(KERN_INFO "Event B is encountered\n");
 }
 
-int __init tp_sample_trace_init(void)
+static int __init tp_sample_trace_init(void)
 {
 	int ret;
 
@@ -42,7 +42,7 @@ int __init tp_sample_trace_init(void)
 
 module_init(tp_sample_trace_init);
 
-void __exit tp_sample_trace_exit(void)
+static void __exit tp_sample_trace_exit(void)
 {
 	unregister_trace_subsys_eventb(probe_subsys_eventb);
 	unregister_trace_subsys_event(probe_subsys_event);
diff --git a/samples/tracepoints/tracepoint-probe-sample2.c b/samples/tracepoints/tracepoint-probe-sample2.c
index 685a5acb4562..be2a960573f1 100644
--- a/samples/tracepoints/tracepoint-probe-sample2.c
+++ b/samples/tracepoints/tracepoint-probe-sample2.c
@@ -18,7 +18,7 @@ static void probe_subsys_event(struct inode *inode, struct file *file)
 		inode->i_ino);
 }
 
-int __init tp_sample_trace_init(void)
+static int __init tp_sample_trace_init(void)
 {
 	int ret;
 
@@ -30,7 +30,7 @@ int __init tp_sample_trace_init(void)
 
 module_init(tp_sample_trace_init);
 
-void __exit tp_sample_trace_exit(void)
+static void __exit tp_sample_trace_exit(void)
 {
 	unregister_trace_subsys_event(probe_subsys_event);
 	tracepoint_synchronize_unregister();
diff --git a/samples/tracepoints/tracepoint-sample.c b/samples/tracepoints/tracepoint-sample.c
index 00d169792a3e..68d5dc0310e4 100644
--- a/samples/tracepoints/tracepoint-sample.c
+++ b/samples/tracepoints/tracepoint-sample.c
@@ -32,7 +32,7 @@ static struct file_operations mark_ops = {
 	.open = my_open,
 };
 
-static int example_init(void)
+static int __init example_init(void)
 {
 	printk(KERN_ALERT "example init\n");
 	pentry_example = proc_create("tracepoint-example", 0444, NULL,
@@ -42,7 +42,7 @@ static int example_init(void)
 	return 0;
 }
 
-static void example_exit(void)
+static void __exit example_exit(void)
 {
 	printk(KERN_ALERT "example exit\n");
 	remove_proc_entry("tracepoint-example", NULL);
diff --git a/scripts/.gitignore b/scripts/.gitignore
index b939fbd01195..09e2406f3b78 100644
--- a/scripts/.gitignore
+++ b/scripts/.gitignore
@@ -1,6 +1,7 @@
 #
 # Generated files
 #
+ihex2fw
 conmakehash
 kallsyms
 pnmtologo
diff --git a/scripts/Makefile b/scripts/Makefile
index aafdf064feef..035182e16afb 100644
--- a/scripts/Makefile
+++ b/scripts/Makefile
@@ -2,11 +2,12 @@
 # scripts contains sources for various helper programs used throughout
 # the kernel for the build process.
 # ---------------------------------------------------------------------------
+# ihex2fw:       Parser/loader for IHEX formatted data
 # kallsyms:      Find all symbols in vmlinux
 # pnmttologo:    Convert pnm files to logo files
-# conmakehash:   Create chartable
 # conmakehash:	 Create arrays for initializing the kernel console tables
 
+hostprogs-y                      := ihex2fw
 hostprogs-$(CONFIG_KALLSYMS)     += kallsyms
 hostprogs-$(CONFIG_LOGO)         += pnmtologo
 hostprogs-$(CONFIG_VT)           += conmakehash
diff --git a/scripts/bootgraph.pl b/scripts/bootgraph.pl
index f0af9aa9b243..0a498e33b30b 100644
--- a/scripts/bootgraph.pl
+++ b/scripts/bootgraph.pl
@@ -88,7 +88,7 @@ END
 }
 
 print "<?xml version=\"1.0\" standalone=\"no\"?> \n";
-print "<svg width=\"1000\" height=\"100%\" version=\"1.1\" xmlns=\"http://www.w3.org/2000/svg\">\n";
+print "<svg width=\"2000\" height=\"100%\" version=\"1.1\" xmlns=\"http://www.w3.org/2000/svg\">\n";
 
 my @styles;
 
@@ -105,8 +105,9 @@ $styles[9] = "fill:rgb(255,255,128);fill-opacity:0.5;stroke-width:1;stroke:rgb(0
 $styles[10] = "fill:rgb(255,128,255);fill-opacity:0.5;stroke-width:1;stroke:rgb(0,0,0)";
 $styles[11] = "fill:rgb(128,255,255);fill-opacity:0.5;stroke-width:1;stroke:rgb(0,0,0)";
 
-my $mult = 950.0 / ($maxtime - $firsttime);
-my $threshold = ($maxtime - $firsttime) / 60.0;
+my $mult = 1950.0 / ($maxtime - $firsttime);
+my $threshold2 = ($maxtime - $firsttime) / 120.0;
+my $threshold = $threshold2/10;
 my $stylecounter = 0;
 my %rows;
 my $rowscount = 1;
@@ -116,7 +117,7 @@ foreach my $key (@initcalls) {
 	my $duration = $end{$key} - $start{$key};
 
 	if ($duration >= $threshold) {
-		my ($s, $s2, $e, $w, $y, $y2, $style);
+		my ($s, $s2, $s3, $e, $w, $y, $y2, $style);
 		my $pid = $pids{$key};
 
 		if (!defined($rows{$pid})) {
@@ -125,6 +126,7 @@ foreach my $key (@initcalls) {
 		}
 		$s = ($start{$key} - $firsttime) * $mult;
 		$s2 = $s + 6;
+		$s3 = $s + 1;
 		$e = ($end{$key} - $firsttime) * $mult;
 		$w = $e - $s;
 
@@ -138,7 +140,11 @@ foreach my $key (@initcalls) {
 		};
 
 		print "<rect x=\"$s\" width=\"$w\" y=\"$y\" height=\"145\" style=\"$style\"/>\n";
-		print "<text transform=\"translate($s2,$y2) rotate(90)\">$key</text>\n";
+		if ($duration >= $threshold2) {
+			print "<text transform=\"translate($s2,$y2) rotate(90)\">$key</text>\n";
+		} else {
+			print "<text transform=\"translate($s3,$y2) rotate(90)\" font-size=\"3pt\">$key</text>\n";
+		}
 	}
 }
 
diff --git a/scripts/checkpatch.pl b/scripts/checkpatch.pl
index f88bb3e21cda..7bed4ed2c519 100755
--- a/scripts/checkpatch.pl
+++ b/scripts/checkpatch.pl
@@ -1,7 +1,8 @@
 #!/usr/bin/perl -w
 # (c) 2001, Dave Jones. <davej@redhat.com> (the file handling bit)
 # (c) 2005, Joel Schopp <jschopp@austin.ibm.com> (the ugly bit)
-# (c) 2007, Andy Whitcroft <apw@uk.ibm.com> (new conditions, test suite, etc)
+# (c) 2007,2008, Andy Whitcroft <apw@uk.ibm.com> (new conditions, test suite)
+# (c) 2008, Andy Whitcroft <apw@canonical.com>
 # Licensed under the terms of the GNU GPL License version 2
 
 use strict;
@@ -9,7 +10,7 @@ use strict;
 my $P = $0;
 $P =~ s@.*/@@g;
 
-my $V = '0.24';
+my $V = '0.26';
 
 use Getopt::Long qw(:config no_auto_abbrev);
 
@@ -68,7 +69,9 @@ my $dbg_possible = 0;
 my $dbg_type = 0;
 my $dbg_attr = 0;
 for my $key (keys %debug) {
-	eval "\${dbg_$key} = '$debug{$key}';"
+	## no critic
+	eval "\${dbg_$key} = '$debug{$key}';";
+	die "$@" if ($@);
 }
 
 if ($terse) {
@@ -116,7 +119,8 @@ our $Attribute	= qr{
 			__(?:mem|cpu|dev|)(?:initdata|init)|
 			____cacheline_aligned|
 			____cacheline_aligned_in_smp|
-			____cacheline_internodealigned_in_smp
+			____cacheline_internodealigned_in_smp|
+			__weak
 		  }x;
 our $Modifier;
 our $Inline	= qr{inline|__always_inline|noinline};
@@ -125,6 +129,7 @@ our $Lval	= qr{$Ident(?:$Member)*};
 
 our $Constant	= qr{(?:[0-9]+|0x[0-9a-fA-F]+)[UL]*};
 our $Assignment	= qr{(?:\*\=|/=|%=|\+=|-=|<<=|>>=|&=|\^=|\|=|=)};
+our $Compare    = qr{<=|>=|==|!=|<|>};
 our $Operators	= qr{
 			<=|>=|==|!=|
 			=>|->|<<|>>|<|>|!|~|
@@ -190,7 +195,7 @@ sub build_types {
 		  }x;
 	$Type	= qr{
 			$NonptrType
-			(?:\s*\*+\s*const|\s*\*+|(?:\s*\[\s*\])+)?
+			(?:[\s\*]+\s*const|[\s\*]+|(?:\s*\[\s*\])+)?
 			(?:\s+$Inline|\s+$Modifier)*
 		  }x;
 	$Declare	= qr{(?:$Storage\s+)?$Type};
@@ -203,9 +208,9 @@ my @dep_includes = ();
 my @dep_functions = ();
 my $removal = "Documentation/feature-removal-schedule.txt";
 if ($tree && -f "$root/$removal") {
-	open(REMOVE, "<$root/$removal") ||
+	open(my $REMOVE, '<', "$root/$removal") ||
 				die "$P: $removal: open failed - $!\n";
-	while (<REMOVE>) {
+	while (<$REMOVE>) {
 		if (/^Check:\s+(.*\S)/) {
 			for my $entry (split(/[, ]+/, $1)) {
 				if ($entry =~ m@include/(.*)@) {
@@ -217,17 +222,21 @@ if ($tree && -f "$root/$removal") {
 			}
 		}
 	}
+	close($REMOVE);
 }
 
 my @rawlines = ();
 my @lines = ();
 my $vname;
 for my $filename (@ARGV) {
+	my $FILE;
 	if ($file) {
-		open(FILE, "diff -u /dev/null $filename|") ||
+		open($FILE, '-|', "diff -u /dev/null $filename") ||
 			die "$P: $filename: diff failed - $!\n";
+	} elsif ($filename eq '-') {
+		open($FILE, '<&STDIN');
 	} else {
-		open(FILE, "<$filename") ||
+		open($FILE, '<', "$filename") ||
 			die "$P: $filename: open failed - $!\n";
 	}
 	if ($filename eq '-') {
@@ -235,11 +244,11 @@ for my $filename (@ARGV) {
 	} else {
 		$vname = $filename;
 	}
-	while (<FILE>) {
+	while (<$FILE>) {
 		chomp;
 		push(@rawlines, $_);
 	}
-	close(FILE);
+	close($FILE);
 	if (!process($filename)) {
 		$exit = 1;
 	}
@@ -366,7 +375,7 @@ sub sanitise_line {
 			}
 		}
 
-		#print "SQ:$sanitise_quote\n";
+		#print "c<$c> SQ<$sanitise_quote>\n";
 		if ($off != 0 && $sanitise_quote eq '*/' && $c ne "\t") {
 			substr($res, $off, 1, $;);
 		} elsif ($off != 0 && $sanitise_quote && $c ne "\t") {
@@ -402,6 +411,7 @@ sub ctx_statement_block {
 
 	my $type = '';
 	my $level = 0;
+	my @stack = ([$type, $level]);
 	my $p;
 	my $c;
 	my $len = 0;
@@ -433,6 +443,16 @@ sub ctx_statement_block {
 		$remainder = substr($blk, $off);
 
 		#warn "CSB: c<$c> type<$type> level<$level> remainder<$remainder> coff_set<$coff_set>\n";
+
+		# Handle nested #if/#else.
+		if ($remainder =~ /^#\s*(?:ifndef|ifdef|if)\s/) {
+			push(@stack, [ $type, $level ]);
+		} elsif ($remainder =~ /^#\s*(?:else|elif)\b/) {
+			($type, $level) = @{$stack[$#stack - 1]};
+		} elsif ($remainder =~ /^#\s*endif\b/) {
+			($type, $level) = @{pop(@stack)};
+		}
+
 		# Statement ends at the ';' or a close '}' at the
 		# outermost level.
 		if ($level == 0 && $c eq ';') {
@@ -579,11 +599,22 @@ sub ctx_block_get {
 	my @res = ();
 
 	my $level = 0;
+	my @stack = ($level);
 	for ($line = $start; $remain > 0; $line++) {
 		next if ($rawlines[$line] =~ /^-/);
 		$remain--;
 
 		$blk .= $rawlines[$line];
+
+		# Handle nested #if/#else.
+		if ($rawlines[$line] =~ /^.\s*#\s*(?:ifndef|ifdef|if)\s/) {
+			push(@stack, $level);
+		} elsif ($rawlines[$line] =~ /^.\s*#\s*(?:else|elif)\b/) {
+			$level = $stack[$#stack - 1];
+		} elsif ($rawlines[$line] =~ /^.\s*#\s*endif\b/) {
+			$level = pop(@stack);
+		}
+
 		foreach my $c (split(//, $rawlines[$line])) {
 			##print "C<$c>L<$level><$open$close>O<$off>\n";
 			if ($off > 0) {
@@ -843,11 +874,11 @@ sub annotate_values {
 			$type = 'V';
 			$av_pending = 'V';
 
-		} elsif ($cur =~ /^($Ident\s*):/) {
-			if ($type eq 'E') {
-				$av_pend_colon = 'L';
-			} elsif ($type eq 'T') {
+		} elsif ($cur =~ /^($Ident\s*):(?:\s*\d+\s*(,|=|;))?/) {
+			if (defined $2 && $type eq 'C' || $type eq 'T') {
 				$av_pend_colon = 'B';
+			} elsif ($type eq 'E') {
+				$av_pend_colon = 'L';
 			}
 			print "IDENT_COLON($1,$type>$av_pend_colon)\n" if ($dbg_values > 1);
 			$type = 'V';
@@ -865,6 +896,10 @@ sub annotate_values {
 			$type = 'E';
 			$av_pend_colon = 'O';
 
+		} elsif ($cur =~/^(,)/) {
+			print "COMMA($1)\n" if ($dbg_values > 1);
+			$type = 'C';
+
 		} elsif ($cur =~ /^(\?)/o) {
 			print "QUESTION($1)\n" if ($dbg_values > 1);
 			$type = 'N';
@@ -880,7 +915,7 @@ sub annotate_values {
 			}
 			$av_pend_colon = 'O';
 
-		} elsif ($cur =~ /^(;|\[)/o) {
+		} elsif ($cur =~ /^(\[)/o) {
 			print "CLOSE($1)\n" if ($dbg_values > 1);
 			$type = 'N';
 
@@ -1051,6 +1086,7 @@ sub process {
 	my $in_comment = 0;
 	my $comment_edge = 0;
 	my $first_line = 0;
+	my $p1_prefix = '';
 
 	my $prev_values = 'E';
 
@@ -1097,9 +1133,12 @@ sub process {
 					 $rawlines[$ln - 1] =~ /^-/);
 				$cnt--;
 				#print "RAW<$rawlines[$ln - 1]>\n";
-				($edge) = (defined $rawlines[$ln - 1] &&
-					$rawlines[$ln - 1] =~ m@(/\*|\*/)@);
-				last if (defined $edge);
+				last if (!defined $rawlines[$ln - 1]);
+				if ($rawlines[$ln - 1] =~ m@(/\*|\*/)@ &&
+				    $rawlines[$ln - 1] !~ m@"[^"]*(?:/\*|\*/)[^"]*"@) {
+					($edge) = $1;
+					last;
+				}
 			}
 			if (defined $edge && $edge eq '*/') {
 				$in_comment = 1;
@@ -1109,7 +1148,7 @@ sub process {
 			# is the start of a diff block and this line starts
 			# ' *' then it is very likely a comment.
 			if (!defined $edge &&
-			    $rawlines[$linenr] =~ m@^.\s* \*(?:\s|$)@)
+			    $rawlines[$linenr] =~ m@^.\s*(?:\*\*+| \*)(?:\s|$)@)
 			{
 				$in_comment = 1;
 			}
@@ -1196,7 +1235,12 @@ sub process {
 		# extract the filename as it passes
 		if ($line=~/^\+\+\+\s+(\S+)/) {
 			$realfile = $1;
-			$realfile =~ s@^[^/]*/@@;
+			$realfile =~ s@^([^/]*)/@@;
+
+			$p1_prefix = $1;
+			if ($tree && $p1_prefix ne '' && -e "$root/$p1_prefix") {
+				WARN("patch prefix '$p1_prefix' exists, appears to be a -p0 patch\n");
+			}
 
 			if ($realfile =~ m@^include/asm/@) {
 				ERROR("do not modify files in include/asm, change architecture specific files in include/asm-<architecture>\n" . "$here$rawline\n");
@@ -1336,7 +1380,7 @@ sub process {
 			}
 
 			# any (foo ... *) is a pointer cast, and foo is a type
-			while ($s =~ /\(($Ident)(?:\s+$Sparse)*\s*\*+\s*\)/sg) {
+			while ($s =~ /\(($Ident)(?:\s+$Sparse)*[\s\*]+\s*\)/sg) {
 				possible($1, "C:" . $s);
 			}
 
@@ -1594,7 +1638,7 @@ sub process {
 				$herecurr);
 		}
 # check for static initialisers.
-		if ($line =~ /\s*static\s.*=\s*(0|NULL|false)\s*;/) {
+		if ($line =~ /\bstatic\s.*=\s*(0|NULL|false)\s*;/) {
 			ERROR("do not initialise statics to 0 or NULL\n" .
 				$herecurr);
 		}
@@ -1602,7 +1646,7 @@ sub process {
 # check for new typedefs, only function parameters and sparse annotations
 # make sense.
 		if ($line =~ /\btypedef\s/ &&
-		    $line !~ /\btypedef\s+$Type\s+\(\s*\*?$Ident\s*\)\s*\(/ &&
+		    $line !~ /\btypedef\s+$Type\s*\(\s*\*?$Ident\s*\)\s*\(/ &&
 		    $line !~ /\btypedef\s+$Type\s+$Ident\s*\(/ &&
 		    $line !~ /\b$typeTypedefs\b/ &&
 		    $line !~ /\b__bitwise(?:__|)\b/) {
@@ -1610,21 +1654,39 @@ sub process {
 		}
 
 # * goes on variable not on type
-		if ($line =~ m{\($NonptrType(\*+)(?:\s+const)?\)}) {
-			ERROR("\"(foo$1)\" should be \"(foo $1)\"\n" .
-				$herecurr);
+		# (char*[ const])
+		if ($line =~ m{\($NonptrType(\s*\*[\s\*]*(?:$Modifier\s*)*)\)}) {
+			my ($from, $to) = ($1, $1);
 
-		} elsif ($line =~ m{\($NonptrType\s+(\*+)(?!\s+const)\s+\)}) {
-			ERROR("\"(foo $1 )\" should be \"(foo $1)\"\n" .
-				$herecurr);
+			# Should start with a space.
+			$to =~ s/^(\S)/ $1/;
+			# Should not end with a space.
+			$to =~ s/\s+$//;
+			# '*'s should not have spaces between.
+			while ($to =~ s/(.)\s\*/$1\*/) {
+			}
 
-		} elsif ($line =~ m{\b$NonptrType(\*+)(?:\s+(?:$Attribute|$Sparse))?\s+[A-Za-z\d_]+}) {
-			ERROR("\"foo$1 bar\" should be \"foo $1bar\"\n" .
-				$herecurr);
+			#print "from<$from> to<$to>\n";
+			if ($from ne $to) {
+				ERROR("\"(foo$from)\" should be \"(foo$to)\"\n" .  $herecurr);
+			}
+		} elsif ($line =~ m{\b$NonptrType(\s*\*[\s\*]*(?:$Modifier\s*)?)($Ident)}) {
+			my ($from, $to, $ident) = ($1, $1, $2);
 
-		} elsif ($line =~ m{\b$NonptrType\s+(\*+)(?!\s+(?:$Attribute|$Sparse))\s+[A-Za-z\d_]+}) {
-			ERROR("\"foo $1 bar\" should be \"foo $1bar\"\n" .
-				$herecurr);
+			# Should start with a space.
+			$to =~ s/^(\S)/ $1/;
+			# Should not end with a space.
+			$to =~ s/\s+$//;
+			# '*'s should not have spaces between.
+			while ($to =~ s/(.)\s\*/$1\*/) {
+			}
+			# Modifiers should have spaces.
+			$to =~ s/(\b$Modifier$)/$1 /;
+
+			#print "from<$from> to<$to>\n";
+			if ($from ne $to) {
+				ERROR("\"foo${from}bar\" should be \"foo${to}bar\"\n" .  $herecurr);
+			}
 		}
 
 # # no BUG() or BUG_ON()
@@ -1759,7 +1821,7 @@ sub process {
 					$c = 'C' if ($elements[$n + 2] =~ /^$;/);
 					$c = 'B' if ($elements[$n + 2] =~ /^(\)|\]|;)/);
 					$c = 'O' if ($elements[$n + 2] eq '');
-					$c = 'E' if ($elements[$n + 2] =~ /\s*\\$/);
+					$c = 'E' if ($elements[$n + 2] =~ /^\s*\\$/);
 				} else {
 					$c = 'E';
 				}
@@ -1950,9 +2012,9 @@ sub process {
 			my $spacing = $1;
 			my $value = $2;
 
-			# Flatten any parentheses and braces
+			# Flatten any parentheses
 			$value =~ s/\)\(/\) \(/g;
-			while ($value =~ s/\([^\(\)]*\)/1/) {
+			while ($value !~ /(?:$Ident|-?$Constant)\s*$Compare\s*(?:$Ident|-?$Constant)/ && $value =~ s/\([^\(\)]*\)/1/) {
 			}
 
 			if ($value =~ /^(?:$Ident|-?$Constant)$/) {
@@ -1992,7 +2054,7 @@ sub process {
 		    $line =~ /\b(?:if|while|for)\s*\(/ && $line !~ /^.\s*#/) {
 			my ($s, $c) = ($stat, $cond);
 
-			if ($c =~ /\bif\s*\(.*[^<>!=]=[^=].*/) {
+			if ($c =~ /\bif\s*\(.*[^<>!=]=[^=].*/s) {
 				ERROR("do not use assignment in if condition\n" . $herecurr);
 			}
 
@@ -2167,9 +2229,10 @@ sub process {
 				MODULE_PARAM_DESC|
 				DECLARE_PER_CPU|
 				DEFINE_PER_CPU|
-				__typeof__\(
+				__typeof__\(|
+				\.$Ident\s*=\s*
 			}x;
-			#print "REST<$rest>\n";
+			#print "REST<$rest> dstat<$dstat>\n";
 			if ($rest ne '') {
 				if ($rest !~ /while\s*\(/ &&
 				    $dstat !~ /$exceptions/)
@@ -2189,6 +2252,15 @@ sub process {
 			}
 		}
 
+# make sure symbols are always wrapped with VMLINUX_SYMBOL() ...
+# all assignments may have only one of the following with an assignment:
+#	.
+#	ALIGN(...)
+#	VMLINUX_SYMBOL(...)
+		if ($realfile eq 'vmlinux.lds.h' && $line =~ /(?:(?:^|\s)$Ident\s*=|=\s*$Ident(?:\s|$))/) {
+			WARN("vmlinux.lds.h needs VMLINUX_SYMBOL() around C-visible symbols\n" . $herecurr);
+		}
+
 # check for redundant bracing round if etc
 		if ($line =~ /(^.*)\bif\b/ && $1 !~ /else\s*$/) {
 			my ($level, $endln, @chunks) =
@@ -2443,6 +2515,11 @@ sub process {
 		if ($line =~ /^.\s*__initcall\s*\(/) {
 			WARN("please use device_initcall() instead of __initcall()\n" . $herecurr);
 		}
+# check for struct file_operations, ensure they are const.
+		if ($line =~ /\bstruct\s+file_operations\b/ &&
+		    $line !~ /\bconst\b/) {
+			WARN("struct file_operations should normally be const\n" . $herecurr);
+		}
 
 # use of NR_CPUS is usually wrong
 # ignore definitions of NR_CPUS and usage to define arrays as likely right
@@ -2466,6 +2543,15 @@ sub process {
 				last;
 			}
 		}
+
+# whine mightly about in_atomic
+		if ($line =~ /\bin_atomic\s*\(/) {
+			if ($realfile =~ m@^drivers/@) {
+				ERROR("do not use in_atomic in drivers\n" . $herecurr);
+			} else {
+				WARN("use of in_atomic() is incorrect outside core kernel code\n" . $herecurr);
+			}
+		}
 	}
 
 	# If we have no input at all, then there is nothing to report on
diff --git a/scripts/config b/scripts/config
new file mode 100755
index 000000000000..68b9761cdc38
--- /dev/null
+++ b/scripts/config
@@ -0,0 +1,150 @@
+#!/bin/bash
+# Manipulate options in a .config file from the command line
+
+usage() {
+	cat >&2 <<EOL
+Manipulate options in a .config file from the command line.
+Usage:
+config options command ...
+commands:
+	--enable|-e option   Enable option
+	--disable|-d option  Disable option
+	--module|-m option      Turn option into a module
+	--state|-s option       Print state of option (n,y,m,undef)
+
+	--enable-after|-E beforeopt option
+                             Enable option directly after other option
+	--disable-after|-D beforeopt option
+                             Disable option directly after other option
+	--module-after|-M beforeopt option
+                             Turn option into module directly after other option
+
+	commands can be repeated multiple times
+
+options:
+	--file .config file to change (default .config)
+
+config doesn't check the validity of the .config file. This is done at next
+ make time.
+The options need to be already in the file before they can be changed,
+but sometimes you can cheat with the --*-after options.
+EOL
+	exit 1
+}
+
+checkarg() {
+	ARG="$1"
+	if [ "$ARG" = "" ] ; then
+		usage
+	fi
+	case "$ARG" in
+	CONFIG_*)
+		ARG="${ARG/CONFIG_/}"
+		;;
+	esac
+	ARG="`echo $ARG | tr a-z A-Z`"
+}
+
+replace() {
+	sed -i -e "$@" $FN
+}
+
+if [ "$1" = "--file" ]; then
+	FN="$2"
+	if [ "$FN" = "" ] ; then
+		usage
+	fi
+	shift
+	shift
+else
+	FN=.config
+fi
+
+while [ "$1" != "" ] ; do
+	CMD="$1"
+	shift
+	case "$CMD" in
+	--enable|-e)
+		checkarg "$1"
+		replace "s/# CONFIG_$ARG is not set/CONFIG_$ARG=y/"
+		shift
+		;;
+
+	--disable|-d)
+		checkarg "$1"
+		replace "s/CONFIG_$ARG=[my]/# CONFIG_$ARG is not set/"
+		shift
+		;;
+
+	--module|-m)
+		checkarg "$1"
+		replace "s/CONFIG_$ARG=y/CONFIG_$ARG=m/" \
+			-e "s/# CONFIG_$ARG is not set/CONFIG_$ARG=m/"
+		shift
+		;;
+
+	--state|-s)
+		checkarg "$1"
+		if grep -q "# CONFIG_$ARG is not set" $FN ; then
+			echo n
+		else
+			V="$(grep "^CONFIG_$ARG=" $FN)"
+			if [ $? != 0 ] ; then
+				echo undef
+			else
+				V="${V/CONFIG_$ARG=/}"
+				V="${V/\"/}"
+				echo "$V"
+			fi
+		fi
+		shift
+		;;
+
+	--enable-after|-E)
+		checkarg "$1"
+		A=$ARG
+		checkarg "$2"
+		B=$ARG
+		replace "/CONFIG_$A=[my]/aCONFIG_$B=y" \
+			-e "/# CONFIG_$ARG is not set/a/CONFIG_$ARG=y" \
+			-e "s/# CONFIG_$ARG is not set/CONFIG_$ARG=y/"
+		shift
+		shift
+		;;
+
+	--disable-after|-D)
+		checkarg "$1"
+		A=$ARG
+		checkarg "$2"
+		B=$ARG
+		replace "/CONFIG_$A=[my]/a# CONFIG_$B is not set" \
+		-e "/# CONFIG_$ARG is not set/a/# CONFIG_$ARG is not set" \
+		-e "s/CONFIG_$ARG=[my]/# CONFIG_$ARG is not set/"
+		shift
+		shift
+		;;
+
+	--module-after|-M)
+		checkarg "$1"
+		A=$ARG
+		checkarg "$2"
+		B=$ARG
+		replace "/CONFIG_$A=[my]/aCONFIG_$B=m" \
+			-e "/# CONFIG_$ARG is not set/a/CONFIG_$ARG=m" \
+			-e "s/CONFIG_$ARG=y/CONFIG_$ARG=m/" \
+			-e "s/# CONFIG_$ARG is not set/CONFIG_$ARG=m/"
+		shift
+		shift
+		;;
+
+	# undocumented because it ignores --file (fixme)
+	--refresh)
+		yes "" | make oldconfig
+		;;
+
+	*)
+		usage
+		;;
+	esac
+done
+
diff --git a/firmware/ihex2fw.c b/scripts/ihex2fw.c
index 8f7fdaa9e010..8f7fdaa9e010 100644
--- a/firmware/ihex2fw.c
+++ b/scripts/ihex2fw.c
diff --git a/scripts/markup_oops.pl b/scripts/markup_oops.pl
new file mode 100644
index 000000000000..700a7a654a3f
--- /dev/null
+++ b/scripts/markup_oops.pl
@@ -0,0 +1,162 @@
+#!/usr/bin/perl -w
+
+# Copyright 2008, Intel Corporation
+#
+# This file is part of the Linux kernel
+#
+# This program file 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.
+#
+# Authors:
+# 	Arjan van de Ven <arjan@linux.intel.com>
+
+
+my $vmlinux_name = $ARGV[0];
+
+#
+# Step 1: Parse the oops to find the EIP value
+#
+
+my $target = "0";
+while (<STDIN>) {
+	if ($_ =~ /EIP: 0060:\[\<([a-z0-9]+)\>\]/) {
+		$target = $1;
+	}
+}
+
+if ($target =~ /^f8/) {
+	print "This script does not work on modules ... \n";
+	exit;
+}
+
+if ($target eq "0") {
+	print "No oops found!\n";
+	print "Usage: \n";
+	print "    dmesg | perl scripts/markup_oops.pl vmlinux\n";
+	exit;
+}
+
+my $counter = 0;
+my $state   = 0;
+my $center  = 0;
+my @lines;
+
+sub InRange {
+	my ($address, $target) = @_;
+	my $ad = "0x".$address;
+	my $ta = "0x".$target;
+	my $delta = hex($ad) - hex($ta);
+
+	if (($delta > -4096) && ($delta < 4096)) {
+		return 1;
+	}
+	return 0;
+}
+
+
+
+# first, parse the input into the lines array, but to keep size down,
+# we only do this for 4Kb around the sweet spot
+
+my $filename;
+
+open(FILE, "objdump -dS $vmlinux_name |") || die "Cannot start objdump";
+
+while (<FILE>) {
+	my $line = $_;
+	chomp($line);
+	if ($state == 0) {
+		if ($line =~ /^([a-f0-9]+)\:/) {
+			if (InRange($1, $target)) {
+				$state = 1;
+			}
+		}
+	} else {
+		if ($line =~ /^([a-f0-9][a-f0-9][a-f0-9][a-f0-9][a-f0-9][a-f0-9]+)\:/) {
+			my $val = $1;
+			if (!InRange($val, $target)) {
+				last;
+			}
+			if ($val eq $target) {
+				$center = $counter;
+			}
+		}
+		$lines[$counter] = $line;
+
+		$counter = $counter + 1;
+	}
+}
+
+close(FILE);
+
+if ($counter == 0) {
+	print "No matching code found \n";
+	exit;
+}
+
+if ($center == 0) {
+	print "No matching code found \n";
+	exit;
+}
+
+my $start;
+my $finish;
+my $codelines = 0;
+my $binarylines = 0;
+# now we go up and down in the array to find how much we want to print
+
+$start = $center;
+
+while ($start > 1) {
+	$start = $start - 1;
+	my $line = $lines[$start];
+	if ($line =~ /^([a-f0-9]+)\:/) {
+		$binarylines = $binarylines + 1;
+	} else {
+		$codelines = $codelines + 1;
+	}
+	if ($codelines > 10) {
+		last;
+	}
+	if ($binarylines > 20) {
+		last;
+	}
+}
+
+
+$finish = $center;
+$codelines = 0;
+$binarylines = 0;
+while ($finish < $counter) {
+	$finish = $finish + 1;
+	my $line = $lines[$finish];
+	if ($line =~ /^([a-f0-9]+)\:/) {
+		$binarylines = $binarylines + 1;
+	} else {
+		$codelines = $codelines + 1;
+	}
+	if ($codelines > 10) {
+		last;
+	}
+	if ($binarylines > 20) {
+		last;
+	}
+}
+
+
+my $i;
+
+my $fulltext = "";
+$i = $start;
+while ($i < $finish) {
+	if ($i == $center) {
+		$fulltext = $fulltext . "*$lines[$i]     <----- faulting instruction\n";
+	} else {
+		$fulltext = $fulltext .  " $lines[$i]\n";
+	}
+	$i = $i +1;
+}
+
+print $fulltext;
+
diff --git a/scripts/tags.sh b/scripts/tags.sh
index 9e3451d2c3a1..fdbe78bb5e2b 100755
--- a/scripts/tags.sh
+++ b/scripts/tags.sh
@@ -24,6 +24,11 @@ else
 	tree=${srctree}/
 fi
 
+# Detect if ALLSOURCE_ARCHS is set. If not, we assume SRCARCH
+if [ "${ALLSOURCE_ARCHS}" = "" ]; then
+	ALLSOURCE_ARCHS=${SRCARCH}
+fi
+
 # find sources in arch/$ARCH
 find_arch_sources()
 {
@@ -54,26 +59,29 @@ find_other_sources()
 find_sources()
 {
 	find_arch_sources $1 "$2"
-	find_include_sources "$2"
-	find_other_sources "$2"
 }
 
 all_sources()
 {
-	find_sources $SRCARCH '*.[chS]'
+	for arch in $ALLSOURCE_ARCHS
+	do
+		find_sources $arch '*.[chS]'
+	done
 	if [ ! -z "$archinclude" ]; then
 		find_arch_include_sources $archinclude '*.[chS]'
 	fi
+	find_include_sources '*.[chS]'
+	find_other_sources '*.[chS]'
 }
 
 all_kconfigs()
 {
-	find_sources $SRCARCH 'Kconfig*'
+	find_sources $ALLSOURCE_ARCHS 'Kconfig*'
 }
 
 all_defconfigs()
 {
-	find_sources $SRCARCH "defconfig"
+	find_sources $ALLSOURCE_ARCHS "defconfig"
 }
 
 docscope()
diff --git a/security/commoncap.c b/security/commoncap.c
index 69fc9952650f..7cd61a5f5205 100644
--- a/security/commoncap.c
+++ b/security/commoncap.c
@@ -45,26 +45,22 @@ EXPORT_SYMBOL(cap_netlink_recv);
 /**
  * cap_capable - Determine whether a task has a particular effective capability
  * @tsk: The task to query
+ * @cred: The credentials to use
  * @cap: The capability to check for
  * @audit: Whether to write an audit message or not
  *
  * Determine whether the nominated task has the specified capability amongst
  * its effective set, returning 0 if it does, -ve if it does not.
  *
- * NOTE WELL: cap_capable() cannot be used like the kernel's capable()
- * function.  That is, it has the reverse semantics: cap_capable() returns 0
- * when a task has a capability, but the kernel's capable() returns 1 for this
- * case.
+ * NOTE WELL: cap_has_capability() cannot be used like the kernel's capable()
+ * and has_capability() functions.  That is, it has the reverse semantics:
+ * cap_has_capability() returns 0 when a task has a capability, but the
+ * kernel's capable() and has_capability() returns 1 for this case.
  */
-int cap_capable(struct task_struct *tsk, int cap, int audit)
+int cap_capable(struct task_struct *tsk, const struct cred *cred, int cap,
+		int audit)
 {
-	__u32 cap_raised;
-
-	/* Derived from include/linux/sched.h:capable. */
-	rcu_read_lock();
-	cap_raised = cap_raised(__task_cred(tsk)->cap_effective, cap);
-	rcu_read_unlock();
-	return cap_raised ? 0 : -EPERM;
+	return cap_raised(cred->cap_effective, cap) ? 0 : -EPERM;
 }
 
 /**
@@ -160,7 +156,8 @@ static inline int cap_inh_is_capped(void)
 	/* they are so limited unless the current task has the CAP_SETPCAP
 	 * capability
 	 */
-	if (cap_capable(current, CAP_SETPCAP, SECURITY_CAP_AUDIT) == 0)
+	if (cap_capable(current, current_cred(), CAP_SETPCAP,
+			SECURITY_CAP_AUDIT) == 0)
 		return 0;
 #endif
 	return 1;
@@ -869,7 +866,8 @@ int cap_task_prctl(int option, unsigned long arg2, unsigned long arg3,
 		     & (new->securebits ^ arg2))			/*[1]*/
 		    || ((new->securebits & SECURE_ALL_LOCKS & ~arg2))	/*[2]*/
 		    || (arg2 & ~(SECURE_ALL_LOCKS | SECURE_ALL_BITS))	/*[3]*/
-		    || (cap_capable(current, CAP_SETPCAP, SECURITY_CAP_AUDIT) != 0) /*[4]*/
+		    || (cap_capable(current, current_cred(), CAP_SETPCAP,
+				    SECURITY_CAP_AUDIT) != 0)		/*[4]*/
 			/*
 			 * [1] no changing of bits that are locked
 			 * [2] no unlocking of locks
@@ -950,7 +948,8 @@ int cap_vm_enough_memory(struct mm_struct *mm, long pages)
 {
 	int cap_sys_admin = 0;
 
-	if (cap_capable(current, CAP_SYS_ADMIN, SECURITY_CAP_NOAUDIT) == 0)
+	if (cap_capable(current, current_cred(), CAP_SYS_ADMIN,
+			SECURITY_CAP_NOAUDIT) == 0)
 		cap_sys_admin = 1;
 	return __vm_enough_memory(mm, pages, cap_sys_admin);
 }
diff --git a/security/keys/keyctl.c b/security/keys/keyctl.c
index 6688765bd8b9..09796797d122 100644
--- a/security/keys/keyctl.c
+++ b/security/keys/keyctl.c
@@ -1294,7 +1294,7 @@ asmlinkage long sys_keyctl(int option, unsigned long arg2, unsigned long arg3,
 
 	case KEYCTL_GET_SECURITY:
 		return keyctl_get_security((key_serial_t) arg2,
-					   (char *) arg3,
+					   (char __user *) arg3,
 					   (size_t) arg4);
 
 	default:
diff --git a/security/security.c b/security/security.c
index 678d4d07b852..c3586c0d97e2 100644
--- a/security/security.c
+++ b/security/security.c
@@ -154,14 +154,32 @@ int security_capset(struct cred *new, const struct cred *old,
 				    effective, inheritable, permitted);
 }
 
-int security_capable(struct task_struct *tsk, int cap)
+int security_capable(int cap)
 {
-	return security_ops->capable(tsk, cap, SECURITY_CAP_AUDIT);
+	return security_ops->capable(current, current_cred(), cap,
+				     SECURITY_CAP_AUDIT);
 }
 
-int security_capable_noaudit(struct task_struct *tsk, int cap)
+int security_real_capable(struct task_struct *tsk, int cap)
 {
-	return security_ops->capable(tsk, cap, SECURITY_CAP_NOAUDIT);
+	const struct cred *cred;
+	int ret;
+
+	cred = get_task_cred(tsk);
+	ret = security_ops->capable(tsk, cred, cap, SECURITY_CAP_AUDIT);
+	put_cred(cred);
+	return ret;
+}
+
+int security_real_capable_noaudit(struct task_struct *tsk, int cap)
+{
+	const struct cred *cred;
+	int ret;
+
+	cred = get_task_cred(tsk);
+	ret = security_ops->capable(tsk, cred, cap, SECURITY_CAP_NOAUDIT);
+	put_cred(cred);
+	return ret;
 }
 
 int security_acct(struct file *file)
diff --git a/security/selinux/Kconfig b/security/selinux/Kconfig
index 26301dd651d3..bca1b74a4a2f 100644
--- a/security/selinux/Kconfig
+++ b/security/selinux/Kconfig
@@ -94,33 +94,6 @@ config SECURITY_SELINUX_CHECKREQPROT_VALUE
 
 	  If you are unsure how to answer this question, answer 1.
 
-config SECURITY_SELINUX_ENABLE_SECMARK_DEFAULT
-	bool "NSA SELinux enable new secmark network controls by default"
-	depends on SECURITY_SELINUX
-	default n
-	help
-	  This option determines whether the new secmark-based network
-	  controls will be enabled by default.  If not, the old internal
-	  per-packet controls will be enabled by default, preserving
-	  old behavior.
-
-	  If you enable the new controls, you will need updated
-	  SELinux userspace libraries, tools and policy.  Typically,
-	  your distribution will provide these and enable the new controls
-	  in the kernel they also distribute.
-
-	  Note that this option can be overridden at boot with the
-	  selinux_compat_net parameter, and after boot via
-	  /selinux/compat_net.  See Documentation/kernel-parameters.txt
-	  for details on this parameter.
-
-	  If you enable the new network controls, you will likely
-	  also require the SECMARK and CONNSECMARK targets, as
-	  well as any conntrack helpers for protocols which you
-	  wish to control.
-
-	  If you are unsure what to do here, select N.
-
 config SECURITY_SELINUX_POLICYDB_VERSION_MAX
 	bool "NSA SELinux maximum supported policy format version"
 	depends on SECURITY_SELINUX
diff --git a/security/selinux/avc.c b/security/selinux/avc.c
index d43bd6baeeaa..eb41f43e2772 100644
--- a/security/selinux/avc.c
+++ b/security/selinux/avc.c
@@ -53,18 +53,20 @@ static const char *class_to_string[] = {
 #undef S_
 
 static const struct av_inherit av_inherit[] = {
-#define S_(c, i, b) { c, common_##i##_perm_to_string, b },
+#define S_(c, i, b) {	.tclass = c,\
+			.common_pts = common_##i##_perm_to_string,\
+			.common_base =  b },
 #include "av_inherit.h"
 #undef S_
 };
 
 const struct selinux_class_perm selinux_class_perm = {
-	av_perm_to_string,
-	ARRAY_SIZE(av_perm_to_string),
-	class_to_string,
-	ARRAY_SIZE(class_to_string),
-	av_inherit,
-	ARRAY_SIZE(av_inherit)
+	.av_perm_to_string = av_perm_to_string,
+	.av_pts_len = ARRAY_SIZE(av_perm_to_string),
+	.class_to_string = class_to_string,
+	.cts_len = ARRAY_SIZE(class_to_string),
+	.av_inherit = av_inherit,
+	.av_inherit_len = ARRAY_SIZE(av_inherit)
 };
 
 #define AVC_CACHE_SLOTS			512
diff --git a/security/selinux/hooks.c b/security/selinux/hooks.c
index dbeaa783b2a9..00815973d412 100644
--- a/security/selinux/hooks.c
+++ b/security/selinux/hooks.c
@@ -1433,12 +1433,13 @@ static int current_has_perm(const struct task_struct *tsk,
 
 /* Check whether a task is allowed to use a capability. */
 static int task_has_capability(struct task_struct *tsk,
+			       const struct cred *cred,
 			       int cap, int audit)
 {
 	struct avc_audit_data ad;
 	struct av_decision avd;
 	u16 sclass;
-	u32 sid = task_sid(tsk);
+	u32 sid = cred_sid(cred);
 	u32 av = CAP_TO_MASK(cap);
 	int rc;
 
@@ -1865,15 +1866,16 @@ static int selinux_capset(struct cred *new, const struct cred *old,
 	return cred_has_perm(old, new, PROCESS__SETCAP);
 }
 
-static int selinux_capable(struct task_struct *tsk, int cap, int audit)
+static int selinux_capable(struct task_struct *tsk, const struct cred *cred,
+			   int cap, int audit)
 {
 	int rc;
 
-	rc = secondary_ops->capable(tsk, cap, audit);
+	rc = secondary_ops->capable(tsk, cred, cap, audit);
 	if (rc)
 		return rc;
 
-	return task_has_capability(tsk, cap, audit);
+	return task_has_capability(tsk, cred, cap, audit);
 }
 
 static int selinux_sysctl_get_sid(ctl_table *table, u16 tclass, u32 *sid)
@@ -2037,7 +2039,8 @@ static int selinux_vm_enough_memory(struct mm_struct *mm, long pages)
 {
 	int rc, cap_sys_admin = 0;
 
-	rc = selinux_capable(current, CAP_SYS_ADMIN, SECURITY_CAP_NOAUDIT);
+	rc = selinux_capable(current, current_cred(), CAP_SYS_ADMIN,
+			     SECURITY_CAP_NOAUDIT);
 	if (rc == 0)
 		cap_sys_admin = 1;
 
@@ -2880,7 +2883,8 @@ static int selinux_inode_getsecurity(const struct inode *inode, const char *name
 	 * and lack of permission just means that we fall back to the
 	 * in-core context value, not a denial.
 	 */
-	error = selinux_capable(current, CAP_MAC_ADMIN, SECURITY_CAP_NOAUDIT);
+	error = selinux_capable(current, current_cred(), CAP_MAC_ADMIN,
+				SECURITY_CAP_NOAUDIT);
 	if (!error)
 		error = security_sid_to_context_force(isec->sid, &context,
 						      &size);
@@ -4185,7 +4189,7 @@ static int selinux_sock_rcv_skb_iptables_compat(struct sock *sk,
 static int selinux_sock_rcv_skb_compat(struct sock *sk, struct sk_buff *skb,
 				       u16 family)
 {
-	int err;
+	int err = 0;
 	struct sk_security_struct *sksec = sk->sk_security;
 	u32 peer_sid;
 	u32 sk_sid = sksec->sid;
@@ -4202,7 +4206,7 @@ static int selinux_sock_rcv_skb_compat(struct sock *sk, struct sk_buff *skb,
 	if (selinux_compat_net)
 		err = selinux_sock_rcv_skb_iptables_compat(sk, skb, &ad,
 							   family, addrp);
-	else
+	else if (selinux_secmark_enabled())
 		err = avc_has_perm(sk_sid, skb->secmark, SECCLASS_PACKET,
 				   PACKET__RECV, &ad);
 	if (err)
@@ -4705,7 +4709,7 @@ static unsigned int selinux_ip_postroute_compat(struct sk_buff *skb,
 		if (selinux_ip_postroute_iptables_compat(skb->sk, ifindex,
 							 &ad, family, addrp))
 			return NF_DROP;
-	} else {
+	} else if (selinux_secmark_enabled()) {
 		if (avc_has_perm(sksec->sid, skb->secmark,
 				 SECCLASS_PACKET, PACKET__SEND, &ad))
 			return NF_DROP;
diff --git a/security/selinux/include/avc_ss.h b/security/selinux/include/avc_ss.h
index c0d314d9f8e1..bb1ec801bdfe 100644
--- a/security/selinux/include/avc_ss.h
+++ b/security/selinux/include/avc_ss.h
@@ -17,16 +17,16 @@ struct av_perm_to_string {
 };
 
 struct av_inherit {
-	u16 tclass;
 	const char **common_pts;
 	u32 common_base;
+	u16 tclass;
 };
 
 struct selinux_class_perm {
 	const struct av_perm_to_string *av_perm_to_string;
 	u32 av_pts_len;
-	const char **class_to_string;
 	u32 cts_len;
+	const char **class_to_string;
 	const struct av_inherit *av_inherit;
 	u32 av_inherit_len;
 };
diff --git a/security/selinux/selinuxfs.c b/security/selinux/selinuxfs.c
index 8f612c8becb5..01ec6d2c6b97 100644
--- a/security/selinux/selinuxfs.c
+++ b/security/selinux/selinuxfs.c
@@ -47,13 +47,7 @@ static char *policycap_names[] = {
 
 unsigned int selinux_checkreqprot = CONFIG_SECURITY_SELINUX_CHECKREQPROT_VALUE;
 
-#ifdef CONFIG_SECURITY_SELINUX_ENABLE_SECMARK_DEFAULT
-#define SELINUX_COMPAT_NET_VALUE 0
-#else
-#define SELINUX_COMPAT_NET_VALUE 1
-#endif
-
-int selinux_compat_net = SELINUX_COMPAT_NET_VALUE;
+int selinux_compat_net = 0;
 
 static int __init checkreqprot_setup(char *str)
 {
@@ -494,7 +488,13 @@ static ssize_t sel_write_compat_net(struct file *file, const char __user *buf,
 	if (sscanf(page, "%d", &new_value) != 1)
 		goto out;
 
-	selinux_compat_net = new_value ? 1 : 0;
+	if (new_value) {
+		printk(KERN_NOTICE
+		       "SELinux: compat_net is deprecated, please use secmark"
+		       " instead\n");
+		selinux_compat_net = 1;
+	} else
+		selinux_compat_net = 0;
 	length = count;
 out:
 	free_page((unsigned long) page);
diff --git a/security/selinux/ss/context.h b/security/selinux/ss/context.h
index 658c2bd17da8..d9dd7a2f6a8a 100644
--- a/security/selinux/ss/context.h
+++ b/security/selinux/ss/context.h
@@ -27,9 +27,9 @@ struct context {
 	u32 user;
 	u32 role;
 	u32 type;
+	u32 len;        /* length of string in bytes */
 	struct mls_range range;
 	char *str;	/* string representation if context cannot be mapped. */
-	u32 len;        /* length of string in bytes */
 };
 
 static inline void mls_context_init(struct context *c)
diff --git a/security/smack/smack.h b/security/smack/smack.h
index 31dce559595a..b79582e4fbfd 100644
--- a/security/smack/smack.h
+++ b/security/smack/smack.h
@@ -16,6 +16,7 @@
 #include <linux/capability.h>
 #include <linux/spinlock.h>
 #include <linux/security.h>
+#include <linux/in.h>
 #include <net/netlabel.h>
 
 /*
@@ -39,6 +40,7 @@ struct superblock_smack {
 struct socket_smack {
 	char		*smk_out;			/* outbound label */
 	char		*smk_in;			/* inbound label */
+	int		smk_labeled;			/* label scheme */
 	char		smk_packet[SMK_LABELLEN];	/* TCP peer label */
 };
 
@@ -80,6 +82,16 @@ struct smack_cipso {
 };
 
 /*
+ * An entry in the table identifying hosts.
+ */
+struct smk_netlbladdr {
+	struct smk_netlbladdr	*smk_next;
+	struct sockaddr_in	smk_host;	/* network address */
+	struct in_addr		smk_mask;	/* network mask */
+	char			*smk_label;	/* label */
+};
+
+/*
  * This is the repository for labels seen so that it is
  * not necessary to keep allocating tiny chuncks of memory
  * and so that they can be shared.
@@ -127,6 +139,20 @@ struct smack_known {
 #define XATTR_NAME_SMACKIPOUT	XATTR_SECURITY_PREFIX XATTR_SMACK_IPOUT
 
 /*
+ * How communications on this socket are treated.
+ * Usually it's determined by the underlying netlabel code
+ * but there are certain cases, including single label hosts
+ * and potentially single label interfaces for which the
+ * treatment can not be known in advance.
+ *
+ * The possibility of additional labeling schemes being
+ * introduced in the future exists as well.
+ */
+#define SMACK_UNLABELED_SOCKET	0
+#define SMACK_CIPSO_SOCKET	1
+
+/*
+ * smackfs magic number
  * smackfs macic number
  */
 #define SMACK_MAGIC	0x43415d53 /* "SMAC" */
@@ -141,6 +167,7 @@ struct smack_known {
  * CIPSO defaults.
  */
 #define SMACK_CIPSO_DOI_DEFAULT		3	/* Historical */
+#define SMACK_CIPSO_DOI_INVALID		-1	/* Not a DOI */
 #define SMACK_CIPSO_DIRECT_DEFAULT	250	/* Arbitrary */
 #define SMACK_CIPSO_MAXCATVAL		63	/* Bigger gets harder */
 #define SMACK_CIPSO_MAXLEVEL            255     /* CIPSO 2.2 standard */
@@ -176,7 +203,6 @@ u32 smack_to_secid(const char *);
  * Shared data.
  */
 extern int smack_cipso_direct;
-extern int smack_net_nltype;
 extern char *smack_net_ambient;
 extern char *smack_onlycap;
 
@@ -186,9 +212,10 @@ extern struct smack_known smack_known_hat;
 extern struct smack_known smack_known_huh;
 extern struct smack_known smack_known_invalid;
 extern struct smack_known smack_known_star;
-extern struct smack_known smack_known_unset;
+extern struct smack_known smack_known_web;
 
 extern struct smk_list_entry *smack_list;
+extern struct smk_netlbladdr *smack_netlbladdrs;
 extern struct security_operations smack_ops;
 
 /*
diff --git a/security/smack/smack_access.c b/security/smack/smack_access.c
index 247cec3b5a43..2e0b83e77ffe 100644
--- a/security/smack/smack_access.c
+++ b/security/smack/smack_access.c
@@ -15,15 +15,8 @@
 #include <linux/sched.h>
 #include "smack.h"
 
-struct smack_known smack_known_unset = {
-	.smk_next	= NULL,
-	.smk_known	= "UNSET",
-	.smk_secid	= 1,
-	.smk_cipso	= NULL,
-};
-
 struct smack_known smack_known_huh = {
-	.smk_next	= &smack_known_unset,
+	.smk_next	= NULL,
 	.smk_known	= "?",
 	.smk_secid	= 2,
 	.smk_cipso	= NULL,
@@ -57,7 +50,14 @@ struct smack_known smack_known_invalid = {
 	.smk_cipso	= NULL,
 };
 
-struct smack_known *smack_known = &smack_known_invalid;
+struct smack_known smack_known_web = {
+	.smk_next	= &smack_known_invalid,
+	.smk_known	= "@",
+	.smk_secid	= 7,
+	.smk_cipso	= NULL,
+};
+
+struct smack_known *smack_known = &smack_known_web;
 
 /*
  * The initial value needs to be bigger than any of the
@@ -99,6 +99,16 @@ int smk_access(char *subject_label, char *object_label, int request)
 	    strcmp(subject_label, smack_known_star.smk_known) == 0)
 		return -EACCES;
 	/*
+	 * An internet object can be accessed by any subject.
+	 * Tasks cannot be assigned the internet label.
+	 * An internet subject can access any object.
+	 */
+	if (object_label == smack_known_web.smk_known ||
+	    subject_label == smack_known_web.smk_known ||
+	    strcmp(object_label, smack_known_web.smk_known) == 0 ||
+	    strcmp(subject_label, smack_known_web.smk_known) == 0)
+		return 0;
+	/*
 	 * A star object can be accessed by any subject.
 	 */
 	if (object_label == smack_known_star.smk_known ||
diff --git a/security/smack/smack_lsm.c b/security/smack/smack_lsm.c
index 848212fd4845..0278bc083044 100644
--- a/security/smack/smack_lsm.c
+++ b/security/smack/smack_lsm.c
@@ -1277,6 +1277,7 @@ static int smack_sk_alloc_security(struct sock *sk, int family, gfp_t gfp_flags)
 
 	ssp->smk_in = csp;
 	ssp->smk_out = csp;
+	ssp->smk_labeled = SMACK_CIPSO_SOCKET;
 	ssp->smk_packet[0] = '\0';
 
 	sk->sk_security = ssp;
@@ -1341,45 +1342,69 @@ static void smack_to_secattr(char *smack, struct netlbl_lsm_secattr *nlsp)
 	struct smack_cipso cipso;
 	int rc;
 
-	switch (smack_net_nltype) {
-	case NETLBL_NLTYPE_CIPSOV4:
-		nlsp->domain = smack;
-		nlsp->flags = NETLBL_SECATTR_DOMAIN | NETLBL_SECATTR_MLS_LVL;
+	nlsp->domain = smack;
+	nlsp->flags = NETLBL_SECATTR_DOMAIN | NETLBL_SECATTR_MLS_LVL;
 
-		rc = smack_to_cipso(smack, &cipso);
-		if (rc == 0) {
-			nlsp->attr.mls.lvl = cipso.smk_level;
-			smack_set_catset(cipso.smk_catset, nlsp);
-		} else {
-			nlsp->attr.mls.lvl = smack_cipso_direct;
-			smack_set_catset(smack, nlsp);
-		}
-		break;
-	default:
-		break;
+	rc = smack_to_cipso(smack, &cipso);
+	if (rc == 0) {
+		nlsp->attr.mls.lvl = cipso.smk_level;
+		smack_set_catset(cipso.smk_catset, nlsp);
+	} else {
+		nlsp->attr.mls.lvl = smack_cipso_direct;
+		smack_set_catset(smack, nlsp);
 	}
 }
 
 /**
  * smack_netlabel - Set the secattr on a socket
  * @sk: the socket
+ * @labeled: socket label scheme
  *
  * Convert the outbound smack value (smk_out) to a
  * secattr and attach it to the socket.
  *
  * Returns 0 on success or an error code
  */
-static int smack_netlabel(struct sock *sk)
+static int smack_netlabel(struct sock *sk, int labeled)
 {
 	struct socket_smack *ssp;
 	struct netlbl_lsm_secattr secattr;
-	int rc;
+	int rc = 0;
 
 	ssp = sk->sk_security;
-	netlbl_secattr_init(&secattr);
-	smack_to_secattr(ssp->smk_out, &secattr);
-	rc = netlbl_sock_setattr(sk, &secattr);
-	netlbl_secattr_destroy(&secattr);
+	/*
+	 * Usually the netlabel code will handle changing the
+	 * packet labeling based on the label.
+	 * The case of a single label host is different, because
+	 * a single label host should never get a labeled packet
+	 * even though the label is usually associated with a packet
+	 * label.
+	 */
+	local_bh_disable();
+	bh_lock_sock_nested(sk);
+
+	if (ssp->smk_out == smack_net_ambient ||
+	    labeled == SMACK_UNLABELED_SOCKET)
+		netlbl_sock_delattr(sk);
+	else {
+		netlbl_secattr_init(&secattr);
+		smack_to_secattr(ssp->smk_out, &secattr);
+		rc = netlbl_sock_setattr(sk, &secattr);
+		netlbl_secattr_destroy(&secattr);
+	}
+
+	bh_unlock_sock(sk);
+	local_bh_enable();
+	/*
+	 * Remember the label scheme used so that it is not
+	 * necessary to do the netlabel setting if it has not
+	 * changed the next time through.
+	 *
+	 * The -EDESTADDRREQ case is an indication that there's
+	 * a single level host involved.
+	 */
+	if (rc == 0)
+		ssp->smk_labeled = labeled;
 
 	return rc;
 }
@@ -1432,7 +1457,7 @@ static int smack_inode_setsecurity(struct inode *inode, const char *name,
 		ssp->smk_in = sp;
 	else if (strcmp(name, XATTR_SMACK_IPOUT) == 0) {
 		ssp->smk_out = sp;
-		rc = smack_netlabel(sock->sk);
+		rc = smack_netlabel(sock->sk, SMACK_CIPSO_SOCKET);
 		if (rc != 0)
 			printk(KERN_WARNING "Smack: \"%s\" netlbl error %d.\n",
 			       __func__, -rc);
@@ -1462,7 +1487,108 @@ static int smack_socket_post_create(struct socket *sock, int family,
 	/*
 	 * Set the outbound netlbl.
 	 */
-	return smack_netlabel(sock->sk);
+	return smack_netlabel(sock->sk, SMACK_CIPSO_SOCKET);
+}
+
+
+/**
+ * smack_host_label - check host based restrictions
+ * @sip: the object end
+ *
+ * looks for host based access restrictions
+ *
+ * This version will only be appropriate for really small
+ * sets of single label hosts. Because of the masking
+ * it cannot shortcut out on the first match. There are
+ * numerious ways to address the problem, but none of them
+ * have been applied here.
+ *
+ * Returns the label of the far end or NULL if it's not special.
+ */
+static char *smack_host_label(struct sockaddr_in *sip)
+{
+	struct smk_netlbladdr *snp;
+	char *bestlabel = NULL;
+	struct in_addr *siap = &sip->sin_addr;
+	struct in_addr *liap;
+	struct in_addr *miap;
+	struct in_addr bestmask;
+
+	if (siap->s_addr == 0)
+		return NULL;
+
+	bestmask.s_addr = 0;
+
+	for (snp = smack_netlbladdrs; snp != NULL; snp = snp->smk_next) {
+		liap = &snp->smk_host.sin_addr;
+		miap = &snp->smk_mask;
+		/*
+		 * If the addresses match after applying the list entry mask
+		 * the entry matches the address. If it doesn't move along to
+		 * the next entry.
+		 */
+		if ((liap->s_addr & miap->s_addr) !=
+		    (siap->s_addr & miap->s_addr))
+			continue;
+		/*
+		 * If the list entry mask identifies a single address
+		 * it can't get any more specific.
+		 */
+		if (miap->s_addr == 0xffffffff)
+			return snp->smk_label;
+		/*
+		 * If the list entry mask is less specific than the best
+		 * already found this entry is uninteresting.
+		 */
+		if ((miap->s_addr | bestmask.s_addr) == bestmask.s_addr)
+			continue;
+		/*
+		 * This is better than any entry found so far.
+		 */
+		bestmask.s_addr = miap->s_addr;
+		bestlabel = snp->smk_label;
+	}
+
+	return bestlabel;
+}
+
+/**
+ * smack_socket_connect - connect access check
+ * @sock: the socket
+ * @sap: the other end
+ * @addrlen: size of sap
+ *
+ * Verifies that a connection may be possible
+ *
+ * Returns 0 on success, and error code otherwise
+ */
+static int smack_socket_connect(struct socket *sock, struct sockaddr *sap,
+				int addrlen)
+{
+	struct socket_smack *ssp = sock->sk->sk_security;
+	char *hostsp;
+	int rc;
+
+	if (sock->sk == NULL || sock->sk->sk_family != PF_INET)
+		return 0;
+
+	if (addrlen < sizeof(struct sockaddr_in))
+		return -EINVAL;
+
+	hostsp = smack_host_label((struct sockaddr_in *)sap);
+	if (hostsp == NULL) {
+		if (ssp->smk_labeled != SMACK_CIPSO_SOCKET)
+			return smack_netlabel(sock->sk, SMACK_CIPSO_SOCKET);
+		return 0;
+	}
+
+	rc = smk_access(ssp->smk_out, hostsp, MAY_WRITE);
+	if (rc != 0)
+		return rc;
+
+	if (ssp->smk_labeled != SMACK_UNLABELED_SOCKET)
+		return smack_netlabel(sock->sk, SMACK_UNLABELED_SOCKET);
+	return 0;
 }
 
 /**
@@ -2101,8 +2227,14 @@ static int smack_setprocattr(struct task_struct *p, char *name,
 	if (newsmack == NULL)
 		return -EINVAL;
 
+	/*
+	 * No process is ever allowed the web ("@") label.
+	 */
+	if (newsmack == smack_known_web.smk_known)
+		return -EPERM;
+
 	new = prepare_creds();
-	if (!new)
+	if (new == NULL)
 		return -ENOMEM;
 	new->security = newsmack;
 	commit_creds(new);
@@ -2144,6 +2276,49 @@ static int smack_unix_may_send(struct socket *sock, struct socket *other)
 }
 
 /**
+ * smack_socket_sendmsg - Smack check based on destination host
+ * @sock: the socket
+ * @msghdr: the message
+ * @size: the size of the message
+ *
+ * Return 0 if the current subject can write to the destination
+ * host. This is only a question if the destination is a single
+ * label host.
+ */
+static int smack_socket_sendmsg(struct socket *sock, struct msghdr *msg,
+				int size)
+{
+	struct sockaddr_in *sip = (struct sockaddr_in *) msg->msg_name;
+	struct socket_smack *ssp = sock->sk->sk_security;
+	char *hostsp;
+	int rc;
+
+	/*
+	 * Perfectly reasonable for this to be NULL
+	 */
+	if (sip == NULL || sip->sin_family != PF_INET)
+		return 0;
+
+	hostsp = smack_host_label(sip);
+	if (hostsp == NULL) {
+		if (ssp->smk_labeled != SMACK_CIPSO_SOCKET)
+			return smack_netlabel(sock->sk, SMACK_CIPSO_SOCKET);
+		return 0;
+	}
+
+	rc = smk_access(ssp->smk_out, hostsp, MAY_WRITE);
+	if (rc != 0)
+		return rc;
+
+	if (ssp->smk_labeled != SMACK_UNLABELED_SOCKET)
+		return smack_netlabel(sock->sk, SMACK_UNLABELED_SOCKET);
+
+	return 0;
+
+}
+
+
+/**
  * smack_from_secattr - Convert a netlabel attr.mls.lvl/attr.mls.cat
  * 	pair to smack
  * @sap: netlabel secattr
@@ -2154,44 +2329,66 @@ static int smack_unix_may_send(struct socket *sock, struct socket *other)
 static void smack_from_secattr(struct netlbl_lsm_secattr *sap, char *sip)
 {
 	char smack[SMK_LABELLEN];
+	char *sp;
 	int pcat;
 
-	if ((sap->flags & NETLBL_SECATTR_MLS_LVL) == 0) {
+	if ((sap->flags & NETLBL_SECATTR_MLS_LVL) != 0) {
 		/*
+		 * Looks like a CIPSO packet.
 		 * If there are flags but no level netlabel isn't
 		 * behaving the way we expect it to.
 		 *
+		 * Get the categories, if any
 		 * Without guidance regarding the smack value
 		 * for the packet fall back on the network
 		 * ambient value.
 		 */
-		strncpy(sip, smack_net_ambient, SMK_MAXLEN);
+		memset(smack, '\0', SMK_LABELLEN);
+		if ((sap->flags & NETLBL_SECATTR_MLS_CAT) != 0)
+			for (pcat = -1;;) {
+				pcat = netlbl_secattr_catmap_walk(
+					sap->attr.mls.cat, pcat + 1);
+				if (pcat < 0)
+					break;
+				smack_catset_bit(pcat, smack);
+			}
+		/*
+		 * If it is CIPSO using smack direct mapping
+		 * we are already done. WeeHee.
+		 */
+		if (sap->attr.mls.lvl == smack_cipso_direct) {
+			memcpy(sip, smack, SMK_MAXLEN);
+			return;
+		}
+		/*
+		 * Look it up in the supplied table if it is not
+		 * a direct mapping.
+		 */
+		smack_from_cipso(sap->attr.mls.lvl, smack, sip);
 		return;
 	}
-	/*
-	 * Get the categories, if any
-	 */
-	memset(smack, '\0', SMK_LABELLEN);
-	if ((sap->flags & NETLBL_SECATTR_MLS_CAT) != 0)
-		for (pcat = -1;;) {
-			pcat = netlbl_secattr_catmap_walk(sap->attr.mls.cat,
-							  pcat + 1);
-			if (pcat < 0)
-				break;
-			smack_catset_bit(pcat, smack);
-		}
-	/*
-	 * If it is CIPSO using smack direct mapping
-	 * we are already done. WeeHee.
-	 */
-	if (sap->attr.mls.lvl == smack_cipso_direct) {
-		memcpy(sip, smack, SMK_MAXLEN);
+	if ((sap->flags & NETLBL_SECATTR_SECID) != 0) {
+		/*
+		 * Looks like a fallback, which gives us a secid.
+		 */
+		sp = smack_from_secid(sap->attr.secid);
+		/*
+		 * This has got to be a bug because it is
+		 * impossible to specify a fallback without
+		 * specifying the label, which will ensure
+		 * it has a secid, and the only way to get a
+		 * secid is from a fallback.
+		 */
+		BUG_ON(sp == NULL);
+		strncpy(sip, sp, SMK_MAXLEN);
 		return;
 	}
 	/*
-	 * Look it up in the supplied table if it is not a direct mapping.
+	 * Without guidance regarding the smack value
+	 * for the packet fall back on the network
+	 * ambient value.
 	 */
-	smack_from_cipso(sap->attr.mls.lvl, smack, sip);
+	strncpy(sip, smack_net_ambient, SMK_MAXLEN);
 	return;
 }
 
@@ -2207,6 +2404,7 @@ static int smack_socket_sock_rcv_skb(struct sock *sk, struct sk_buff *skb)
 	struct netlbl_lsm_secattr secattr;
 	struct socket_smack *ssp = sk->sk_security;
 	char smack[SMK_LABELLEN];
+	char *csp;
 	int rc;
 
 	if (sk->sk_family != PF_INET && sk->sk_family != PF_INET6)
@@ -2215,21 +2413,24 @@ static int smack_socket_sock_rcv_skb(struct sock *sk, struct sk_buff *skb)
 	/*
 	 * Translate what netlabel gave us.
 	 */
-	memset(smack, '\0', SMK_LABELLEN);
 	netlbl_secattr_init(&secattr);
+
 	rc = netlbl_skbuff_getattr(skb, sk->sk_family, &secattr);
-	if (rc == 0)
+	if (rc == 0) {
 		smack_from_secattr(&secattr, smack);
-	else
-		strncpy(smack, smack_net_ambient, SMK_MAXLEN);
+		csp = smack;
+	} else
+		csp = smack_net_ambient;
+
 	netlbl_secattr_destroy(&secattr);
+
 	/*
 	 * Receiving a packet requires that the other end
 	 * be able to write here. Read access is not required.
 	 * This is the simplist possible security model
 	 * for networking.
 	 */
-	rc = smk_access(smack, ssp->smk_in, MAY_WRITE);
+	rc = smk_access(csp, ssp->smk_in, MAY_WRITE);
 	if (rc != 0)
 		netlbl_skbuff_err(skb, rc, 0);
 	return rc;
@@ -2298,7 +2499,6 @@ static int smack_socket_getpeersec_dgram(struct socket *sock,
 	/*
 	 * Translate what netlabel gave us.
 	 */
-	memset(smack, '\0', SMK_LABELLEN);
 	netlbl_secattr_init(&secattr);
 	rc = netlbl_skbuff_getattr(skb, family, &secattr);
 	if (rc == 0)
@@ -2341,7 +2541,7 @@ static void smack_sock_graft(struct sock *sk, struct socket *parent)
 	ssp->smk_in = ssp->smk_out = current_security();
 	ssp->smk_packet[0] = '\0';
 
-	rc = smack_netlabel(sk);
+	rc = smack_netlabel(sk, SMACK_CIPSO_SOCKET);
 	if (rc != 0)
 		printk(KERN_WARNING "Smack: \"%s\" netlbl error %d.\n",
 		       __func__, -rc);
@@ -2367,7 +2567,6 @@ static int smack_inet_conn_request(struct sock *sk, struct sk_buff *skb,
 	if (skb == NULL)
 		return -EACCES;
 
-	memset(smack, '\0', SMK_LABELLEN);
 	netlbl_secattr_init(&skb_secattr);
 	rc = netlbl_skbuff_getattr(skb, sk->sk_family, &skb_secattr);
 	if (rc == 0)
@@ -2732,6 +2931,8 @@ struct security_operations smack_ops = {
 	.unix_may_send = 		smack_unix_may_send,
 
 	.socket_post_create = 		smack_socket_post_create,
+	.socket_connect =		smack_socket_connect,
+	.socket_sendmsg =		smack_socket_sendmsg,
 	.socket_sock_rcv_skb = 		smack_socket_sock_rcv_skb,
 	.socket_getpeersec_stream =	smack_socket_getpeersec_stream,
 	.socket_getpeersec_dgram =	smack_socket_getpeersec_dgram,
@@ -2783,7 +2984,6 @@ static __init int smack_init(void)
 	/*
 	 * Initialize locks
 	 */
-	spin_lock_init(&smack_known_unset.smk_cipsolock);
 	spin_lock_init(&smack_known_huh.smk_cipsolock);
 	spin_lock_init(&smack_known_hat.smk_cipsolock);
 	spin_lock_init(&smack_known_star.smk_cipsolock);
diff --git a/security/smack/smackfs.c b/security/smack/smackfs.c
index 247dc9ebbc71..bf107a389ac1 100644
--- a/security/smack/smackfs.c
+++ b/security/smack/smackfs.c
@@ -20,6 +20,7 @@
 #include <linux/vmalloc.h>
 #include <linux/security.h>
 #include <linux/mutex.h>
+#include <net/net_namespace.h>
 #include <net/netlabel.h>
 #include <net/cipso_ipv4.h>
 #include <linux/seq_file.h>
@@ -38,7 +39,7 @@ enum smk_inos {
 	SMK_DOI		= 5,	/* CIPSO DOI */
 	SMK_DIRECT	= 6,	/* CIPSO level indicating direct label */
 	SMK_AMBIENT	= 7,	/* internet ambient label */
-	SMK_NLTYPE	= 8,	/* label scheme to use by default */
+	SMK_NETLBLADDR	= 8,	/* single label hosts */
 	SMK_ONLYCAP	= 9,	/* the only "capable" label */
 };
 
@@ -48,6 +49,7 @@ enum smk_inos {
 static DEFINE_MUTEX(smack_list_lock);
 static DEFINE_MUTEX(smack_cipso_lock);
 static DEFINE_MUTEX(smack_ambient_lock);
+static DEFINE_MUTEX(smk_netlbladdr_lock);
 
 /*
  * This is the "ambient" label for network traffic.
@@ -57,12 +59,6 @@ static DEFINE_MUTEX(smack_ambient_lock);
 char *smack_net_ambient = smack_known_floor.smk_known;
 
 /*
- * This is the default packet marking scheme for network traffic.
- * It can be reset via smackfs/nltype
- */
-int smack_net_nltype = NETLBL_NLTYPE_CIPSOV4;
-
-/*
  * This is the level in a CIPSO header that indicates a
  * smack label is contained directly in the category set.
  * It can be reset via smackfs/direct
@@ -79,6 +75,13 @@ int smack_cipso_direct = SMACK_CIPSO_DIRECT_DEFAULT;
  */
 char *smack_onlycap;
 
+/*
+ * Certain IP addresses may be designated as single label hosts.
+ * Packets are sent there unlabeled, but only from tasks that
+ * can write to the specified label.
+ */
+struct smk_netlbladdr *smack_netlbladdrs;
+
 static int smk_cipso_doi_value = SMACK_CIPSO_DOI_DEFAULT;
 struct smk_list_entry *smack_list;
 
@@ -104,6 +107,24 @@ struct smk_list_entry *smack_list;
 #define SMK_ACCESSLEN (sizeof(SMK_ACCESS) - 1)
 #define SMK_LOADLEN   (SMK_LABELLEN + SMK_LABELLEN + SMK_ACCESSLEN)
 
+/**
+ * smk_netlabel_audit_set - fill a netlbl_audit struct
+ * @nap: structure to fill
+ */
+static void smk_netlabel_audit_set(struct netlbl_audit *nap)
+{
+	nap->loginuid = audit_get_loginuid(current);
+	nap->sessionid = audit_get_sessionid(current);
+	nap->secid = smack_to_secid(current_security());
+}
+
+/*
+ * Values for parsing single label host rules
+ * "1.2.3.4 X"
+ * "192.168.138.129/32 abcdefghijklmnopqrstuvw"
+ */
+#define SMK_NETLBLADDRMIN	9
+#define SMK_NETLBLADDRMAX	42
 
 /*
  * Seq_file read operations for /smack/load
@@ -344,13 +365,11 @@ static void smk_cipso_doi(void)
 {
 	int rc;
 	struct cipso_v4_doi *doip;
-	struct netlbl_audit audit_info;
+	struct netlbl_audit nai;
 
-	audit_info.loginuid = audit_get_loginuid(current);
-	audit_info.sessionid = audit_get_sessionid(current);
-	audit_info.secid = smack_to_secid(current_security());
+	smk_netlabel_audit_set(&nai);
 
-	rc = netlbl_cfg_map_del(NULL, &audit_info);
+	rc = netlbl_cfg_map_del(NULL, PF_INET, NULL, NULL, &nai);
 	if (rc != 0)
 		printk(KERN_WARNING "%s:%d remove rc = %d\n",
 		       __func__, __LINE__, rc);
@@ -365,11 +384,19 @@ static void smk_cipso_doi(void)
 	for (rc = 1; rc < CIPSO_V4_TAG_MAXCNT; rc++)
 		doip->tags[rc] = CIPSO_V4_TAG_INVALID;
 
-	rc = netlbl_cfg_cipsov4_add_map(doip, NULL, &audit_info);
+	rc = netlbl_cfg_cipsov4_add(doip, &nai);
 	if (rc != 0) {
-		printk(KERN_WARNING "%s:%d add rc = %d\n",
+		printk(KERN_WARNING "%s:%d cipso add rc = %d\n",
 		       __func__, __LINE__, rc);
 		kfree(doip);
+		return;
+	}
+	rc = netlbl_cfg_cipsov4_map_add(doip->doi, NULL, NULL, NULL, &nai);
+	if (rc != 0) {
+		printk(KERN_WARNING "%s:%d map add rc = %d\n",
+		       __func__, __LINE__, rc);
+		kfree(doip);
+		return;
 	}
 }
 
@@ -379,20 +406,19 @@ static void smk_cipso_doi(void)
 static void smk_unlbl_ambient(char *oldambient)
 {
 	int rc;
-	struct netlbl_audit audit_info;
+	struct netlbl_audit nai;
 
-	audit_info.loginuid = audit_get_loginuid(current);
-	audit_info.sessionid = audit_get_sessionid(current);
-	audit_info.secid = smack_to_secid(current_security());
+	smk_netlabel_audit_set(&nai);
 
 	if (oldambient != NULL) {
-		rc = netlbl_cfg_map_del(oldambient, &audit_info);
+		rc = netlbl_cfg_map_del(oldambient, PF_INET, NULL, NULL, &nai);
 		if (rc != 0)
 			printk(KERN_WARNING "%s:%d remove rc = %d\n",
 			       __func__, __LINE__, rc);
 	}
 
-	rc = netlbl_cfg_unlbl_add_map(smack_net_ambient, &audit_info);
+	rc = netlbl_cfg_unlbl_map_add(smack_net_ambient, PF_INET,
+				      NULL, NULL, &nai);
 	if (rc != 0)
 		printk(KERN_WARNING "%s:%d add rc = %d\n",
 		       __func__, __LINE__, rc);
@@ -603,6 +629,201 @@ static const struct file_operations smk_cipso_ops = {
 	.release        = seq_release,
 };
 
+/*
+ * Seq_file read operations for /smack/netlabel
+ */
+
+static void *netlbladdr_seq_start(struct seq_file *s, loff_t *pos)
+{
+	if (*pos == SEQ_READ_FINISHED)
+		return NULL;
+
+	return smack_netlbladdrs;
+}
+
+static void *netlbladdr_seq_next(struct seq_file *s, void *v, loff_t *pos)
+{
+	struct smk_netlbladdr *skp = ((struct smk_netlbladdr *) v)->smk_next;
+
+	if (skp == NULL)
+		*pos = SEQ_READ_FINISHED;
+
+	return skp;
+}
+/*
+#define BEMASK	0x80000000
+*/
+#define BEMASK	0x00000001
+#define BEBITS	(sizeof(__be32) * 8)
+
+/*
+ * Print host/label pairs
+ */
+static int netlbladdr_seq_show(struct seq_file *s, void *v)
+{
+	struct smk_netlbladdr *skp = (struct smk_netlbladdr *) v;
+	unsigned char *hp = (char *) &skp->smk_host.sin_addr.s_addr;
+	__be32 bebits;
+	int maskn = 0;
+
+	for (bebits = BEMASK; bebits != 0; maskn++, bebits <<= 1)
+		if ((skp->smk_mask.s_addr & bebits) == 0)
+			break;
+
+	seq_printf(s, "%u.%u.%u.%u/%d %s\n",
+		hp[0], hp[1], hp[2], hp[3], maskn, skp->smk_label);
+
+	return 0;
+}
+
+static void netlbladdr_seq_stop(struct seq_file *s, void *v)
+{
+	/* No-op */
+}
+
+static struct seq_operations netlbladdr_seq_ops = {
+	.start = netlbladdr_seq_start,
+	.stop  = netlbladdr_seq_stop,
+	.next  = netlbladdr_seq_next,
+	.show  = netlbladdr_seq_show,
+};
+
+/**
+ * smk_open_netlbladdr - open() for /smack/netlabel
+ * @inode: inode structure representing file
+ * @file: "netlabel" file pointer
+ *
+ * Connect our netlbladdr_seq_* operations with /smack/netlabel
+ * file_operations
+ */
+static int smk_open_netlbladdr(struct inode *inode, struct file *file)
+{
+	return seq_open(file, &netlbladdr_seq_ops);
+}
+
+/**
+ * smk_write_netlbladdr - write() for /smack/netlabel
+ * @filp: file pointer, not actually used
+ * @buf: where to get the data from
+ * @count: bytes sent
+ * @ppos: where to start
+ *
+ * Accepts only one netlbladdr per write call.
+ * Returns number of bytes written or error code, as appropriate
+ */
+static ssize_t smk_write_netlbladdr(struct file *file, const char __user *buf,
+				size_t count, loff_t *ppos)
+{
+	struct smk_netlbladdr *skp;
+	struct sockaddr_in newname;
+	char smack[SMK_LABELLEN];
+	char *sp;
+	char data[SMK_NETLBLADDRMAX];
+	char *host = (char *)&newname.sin_addr.s_addr;
+	int rc;
+	struct netlbl_audit audit_info;
+	struct in_addr mask;
+	unsigned int m;
+	__be32 bebits = BEMASK;
+	__be32 nsa;
+
+	/*
+	 * Must have privilege.
+	 * No partial writes.
+	 * Enough data must be present.
+	 * "<addr/mask, as a.b.c.d/e><space><label>"
+	 * "<addr, as a.b.c.d><space><label>"
+	 */
+	if (!capable(CAP_MAC_ADMIN))
+		return -EPERM;
+	if (*ppos != 0)
+		return -EINVAL;
+	if (count < SMK_NETLBLADDRMIN || count > SMK_NETLBLADDRMAX)
+		return -EINVAL;
+	if (copy_from_user(data, buf, count) != 0)
+		return -EFAULT;
+
+	data[count] = '\0';
+
+	rc = sscanf(data, "%hhd.%hhd.%hhd.%hhd/%d %s",
+		&host[0], &host[1], &host[2], &host[3], &m, smack);
+	if (rc != 6) {
+		rc = sscanf(data, "%hhd.%hhd.%hhd.%hhd %s",
+			&host[0], &host[1], &host[2], &host[3], smack);
+		if (rc != 5)
+			return -EINVAL;
+		m = BEBITS;
+	}
+	if (m > BEBITS)
+		return -EINVAL;
+
+	sp = smk_import(smack, 0);
+	if (sp == NULL)
+		return -EINVAL;
+
+	for (mask.s_addr = 0; m > 0; m--) {
+		mask.s_addr |= bebits;
+		bebits <<= 1;
+	}
+	/*
+	 * Only allow one writer at a time. Writes should be
+	 * quite rare and small in any case.
+	 */
+	mutex_lock(&smk_netlbladdr_lock);
+
+	nsa = newname.sin_addr.s_addr;
+	for (skp = smack_netlbladdrs; skp != NULL; skp = skp->smk_next)
+		if (skp->smk_host.sin_addr.s_addr == nsa &&
+		    skp->smk_mask.s_addr == mask.s_addr)
+			break;
+
+	smk_netlabel_audit_set(&audit_info);
+
+	if (skp == NULL) {
+		skp = kzalloc(sizeof(*skp), GFP_KERNEL);
+		if (skp == NULL)
+			rc = -ENOMEM;
+		else {
+			rc = 0;
+			skp->smk_host.sin_addr.s_addr = newname.sin_addr.s_addr;
+			skp->smk_mask.s_addr = mask.s_addr;
+			skp->smk_next = smack_netlbladdrs;
+			skp->smk_label = sp;
+			smack_netlbladdrs = skp;
+		}
+	} else {
+		rc = netlbl_cfg_unlbl_static_del(&init_net, NULL,
+			&skp->smk_host.sin_addr, &skp->smk_mask,
+			PF_INET, &audit_info);
+		skp->smk_label = sp;
+	}
+
+	/*
+	 * Now tell netlabel about the single label nature of
+	 * this host so that incoming packets get labeled.
+	 */
+
+	if (rc == 0)
+		rc = netlbl_cfg_unlbl_static_add(&init_net, NULL,
+			&skp->smk_host.sin_addr, &skp->smk_mask, PF_INET,
+			smack_to_secid(skp->smk_label), &audit_info);
+
+	if (rc == 0)
+		rc = count;
+
+	mutex_unlock(&smk_netlbladdr_lock);
+
+	return rc;
+}
+
+static const struct file_operations smk_netlbladdr_ops = {
+	.open           = smk_open_netlbladdr,
+	.read		= seq_read,
+	.llseek         = seq_lseek,
+	.write		= smk_write_netlbladdr,
+	.release        = seq_release,
+};
+
 /**
  * smk_read_doi - read() for /smack/doi
  * @filp: file pointer, not actually used
@@ -891,110 +1112,6 @@ static const struct file_operations smk_onlycap_ops = {
 	.write		= smk_write_onlycap,
 };
 
-struct option_names {
-	int	o_number;
-	char	*o_name;
-	char	*o_alias;
-};
-
-static struct option_names netlbl_choices[] = {
-	{ NETLBL_NLTYPE_RIPSO,
-		NETLBL_NLTYPE_RIPSO_NAME,	"ripso" },
-	{ NETLBL_NLTYPE_CIPSOV4,
-		NETLBL_NLTYPE_CIPSOV4_NAME,	"cipsov4" },
-	{ NETLBL_NLTYPE_CIPSOV4,
-		NETLBL_NLTYPE_CIPSOV4_NAME,	"cipso" },
-	{ NETLBL_NLTYPE_CIPSOV6,
-		NETLBL_NLTYPE_CIPSOV6_NAME,	"cipsov6" },
-	{ NETLBL_NLTYPE_UNLABELED,
-		NETLBL_NLTYPE_UNLABELED_NAME,	"unlabeled" },
-};
-
-/**
- * smk_read_nltype - read() for /smack/nltype
- * @filp: file pointer, not actually used
- * @buf: where to put the result
- * @count: maximum to send along
- * @ppos: where to start
- *
- * Returns number of bytes read or error code, as appropriate
- */
-static ssize_t smk_read_nltype(struct file *filp, char __user *buf,
-			       size_t count, loff_t *ppos)
-{
-	char bound[40];
-	ssize_t rc;
-	int i;
-
-	if (count < SMK_LABELLEN)
-		return -EINVAL;
-
-	if (*ppos != 0)
-		return 0;
-
-	sprintf(bound, "unknown");
-
-	for (i = 0; i < ARRAY_SIZE(netlbl_choices); i++)
-		if (smack_net_nltype == netlbl_choices[i].o_number) {
-			sprintf(bound, "%s", netlbl_choices[i].o_name);
-			break;
-		}
-
-	rc = simple_read_from_buffer(buf, count, ppos, bound, strlen(bound));
-
-	return rc;
-}
-
-/**
- * smk_write_nltype - write() for /smack/nltype
- * @filp: file pointer, not actually used
- * @buf: where to get the data from
- * @count: bytes sent
- * @ppos: where to start
- *
- * Returns number of bytes written or error code, as appropriate
- */
-static ssize_t smk_write_nltype(struct file *file, const char __user *buf,
-				size_t count, loff_t *ppos)
-{
-	char bound[40];
-	char *cp;
-	int i;
-
-	if (!capable(CAP_MAC_ADMIN))
-		return -EPERM;
-
-	if (count >= 40)
-		return -EINVAL;
-
-	if (copy_from_user(bound, buf, count) != 0)
-		return -EFAULT;
-
-	bound[count] = '\0';
-	cp = strchr(bound, ' ');
-	if (cp != NULL)
-		*cp = '\0';
-	cp = strchr(bound, '\n');
-	if (cp != NULL)
-		*cp = '\0';
-
-	for (i = 0; i < ARRAY_SIZE(netlbl_choices); i++)
-		if (strcmp(bound, netlbl_choices[i].o_name) == 0 ||
-		    strcmp(bound, netlbl_choices[i].o_alias) == 0) {
-			smack_net_nltype = netlbl_choices[i].o_number;
-			return count;
-		}
-	/*
-	 * Not a valid choice.
-	 */
-	return -EINVAL;
-}
-
-static const struct file_operations smk_nltype_ops = {
-	.read		= smk_read_nltype,
-	.write		= smk_write_nltype,
-};
-
 /**
  * smk_fill_super - fill the /smackfs superblock
  * @sb: the empty superblock
@@ -1021,8 +1138,8 @@ static int smk_fill_super(struct super_block *sb, void *data, int silent)
 			{"direct", &smk_direct_ops, S_IRUGO|S_IWUSR},
 		[SMK_AMBIENT]	=
 			{"ambient", &smk_ambient_ops, S_IRUGO|S_IWUSR},
-		[SMK_NLTYPE]	=
-			{"nltype", &smk_nltype_ops, S_IRUGO|S_IWUSR},
+		[SMK_NETLBLADDR] =
+			{"netlabel", &smk_netlbladdr_ops, S_IRUGO|S_IWUSR},
 		[SMK_ONLYCAP]	=
 			{"onlycap", &smk_onlycap_ops, S_IRUGO|S_IWUSR},
 		/* last one */ {""}
diff --git a/sound/arm/pxa2xx-ac97-lib.c b/sound/arm/pxa2xx-ac97-lib.c
index ef6539eea579..35afd0c33be5 100644
--- a/sound/arm/pxa2xx-ac97-lib.c
+++ b/sound/arm/pxa2xx-ac97-lib.c
@@ -321,10 +321,6 @@ int __devinit pxa2xx_ac97_hw_probe(struct platform_device *dev)
 {
 	int ret;
 
-	ret = request_irq(IRQ_AC97, pxa2xx_ac97_irq, 0, "AC97", NULL);
-	if (ret < 0)
-		goto err;
-
 	if (cpu_is_pxa25x() || cpu_is_pxa27x()) {
 		pxa_gpio_mode(GPIO31_SYNC_AC97_MD);
 		pxa_gpio_mode(GPIO30_SDATA_OUT_AC97_MD);
@@ -339,7 +335,7 @@ int __devinit pxa2xx_ac97_hw_probe(struct platform_device *dev)
 		if (IS_ERR(ac97conf_clk)) {
 			ret = PTR_ERR(ac97conf_clk);
 			ac97conf_clk = NULL;
-			goto err_irq;
+			goto err_conf;
 		}
 	}
 
@@ -347,19 +343,30 @@ int __devinit pxa2xx_ac97_hw_probe(struct platform_device *dev)
 	if (IS_ERR(ac97_clk)) {
 		ret = PTR_ERR(ac97_clk);
 		ac97_clk = NULL;
-		goto err_irq;
+		goto err_clk;
 	}
 
-	return clk_enable(ac97_clk);
+	ret = clk_enable(ac97_clk);
+	if (ret)
+		goto err_clk2;
+
+	ret = request_irq(IRQ_AC97, pxa2xx_ac97_irq, IRQF_DISABLED, "AC97", NULL);
+	if (ret < 0)
+		goto err_irq;
+
+	return 0;
 
 err_irq:
 	GCR |= GCR_ACLINK_OFF;
+err_clk2:
+	clk_put(ac97_clk);
+	ac97_clk = NULL;
+err_clk:
 	if (ac97conf_clk) {
 		clk_put(ac97conf_clk);
 		ac97conf_clk = NULL;
 	}
-	free_irq(IRQ_AC97, NULL);
-err:
+err_conf:
 	return ret;
 }
 EXPORT_SYMBOL_GPL(pxa2xx_ac97_hw_probe);
diff --git a/sound/core/sound.c b/sound/core/sound.c
index 44a69bb8d4f0..7872a02f6ca9 100644
--- a/sound/core/sound.c
+++ b/sound/core/sound.c
@@ -152,6 +152,10 @@ static int __snd_open(struct inode *inode, struct file *file)
 	}
 	old_fops = file->f_op;
 	file->f_op = fops_get(mptr->f_ops);
+	if (file->f_op == NULL) {
+		file->f_op = old_fops;
+		return -ENODEV;
+	}
 	if (file->f_op->open)
 		err = file->f_op->open(inode, file);
 	if (err) {
diff --git a/sound/oss/aedsp16.c b/sound/oss/aedsp16.c
index a0274f3dac08..3ee9900ffd7b 100644
--- a/sound/oss/aedsp16.c
+++ b/sound/oss/aedsp16.c
@@ -157,7 +157,7 @@
 
    Started Fri Mar 17 16:13:18 MET 1995
 
-   v0.1 (ALPHA, was an user-level program called AudioExcelDSP16.c)
+   v0.1 (ALPHA, was a user-level program called AudioExcelDSP16.c)
    - Initial code.
    v0.2 (ALPHA)
    - Cleanups.
diff --git a/sound/soc/atmel/sam9g20_wm8731.c b/sound/soc/atmel/sam9g20_wm8731.c
index 1fb59a9d3719..6ea04be911d0 100644
--- a/sound/soc/atmel/sam9g20_wm8731.c
+++ b/sound/soc/atmel/sam9g20_wm8731.c
@@ -221,8 +221,8 @@ static int at91sam9g20ek_wm8731_init(struct snd_soc_codec *codec)
 	snd_soc_dapm_add_routes(codec, intercon, ARRAY_SIZE(intercon));
 
 	/* not connected */
-	snd_soc_dapm_disable_pin(codec, "RLINEIN");
-	snd_soc_dapm_disable_pin(codec, "LLINEIN");
+	snd_soc_dapm_nc_pin(codec, "RLINEIN");
+	snd_soc_dapm_nc_pin(codec, "LLINEIN");
 
 	/* always connected */
 	snd_soc_dapm_enable_pin(codec, "Int Mic");
diff --git a/sound/soc/codecs/Kconfig b/sound/soc/codecs/Kconfig
index c41289b5f586..d0e0d691ae51 100644
--- a/sound/soc/codecs/Kconfig
+++ b/sound/soc/codecs/Kconfig
@@ -1,3 +1,13 @@
+# Helper to resolve issues with configs that have SPI enabled but I2C
+# modular, meaning we can't build the codec driver in with I2C support.
+# We use an ordered list of conditional defaults to pick the appropriate
+# setting - SPI can't be modular so that case doesn't need to be covered.
+config SND_SOC_I2C_AND_SPI
+	tristate
+	default m if I2C=m
+	default y if I2C=y
+	default y if SPI_MASTER=y
+
 config SND_SOC_ALL_CODECS
 	tristate "Build all ASoC CODEC drivers"
 	select SND_SOC_AC97_CODEC if SND_SOC_AC97_BUS
@@ -14,12 +24,12 @@ config SND_SOC_ALL_CODECS
 	select SND_SOC_UDA134X
 	select SND_SOC_UDA1380 if I2C
 	select SND_SOC_WM8350 if MFD_WM8350
-	select SND_SOC_WM8510 if (I2C || SPI_MASTER)
+	select SND_SOC_WM8510 if SND_SOC_I2C_AND_SPI
 	select SND_SOC_WM8580 if I2C
-	select SND_SOC_WM8728 if (I2C || SPI_MASTER)
-	select SND_SOC_WM8731 if (I2C || SPI_MASTER)
-	select SND_SOC_WM8750 if (I2C || SPI_MASTER)
-	select SND_SOC_WM8753 if (I2C || SPI_MASTER)
+	select SND_SOC_WM8728 if SND_SOC_I2C_AND_SPI
+	select SND_SOC_WM8731 if SND_SOC_I2C_AND_SPI
+	select SND_SOC_WM8750 if SND_SOC_I2C_AND_SPI
+	select SND_SOC_WM8753 if SND_SOC_I2C_AND_SPI
 	select SND_SOC_WM8900 if I2C
 	select SND_SOC_WM8903 if I2C
 	select SND_SOC_WM8971 if I2C
diff --git a/sound/soc/codecs/twl4030.c b/sound/soc/codecs/twl4030.c
index 31e44e346dc8..fd0f338374a7 100644
--- a/sound/soc/codecs/twl4030.c
+++ b/sound/soc/codecs/twl4030.c
@@ -192,39 +192,51 @@ static void twl4030_init_chip(struct snd_soc_codec *codec)
 
 /* Earpiece */
 static const char *twl4030_earpiece_texts[] =
-		{"Off", "DACL1", "DACL2", "Invalid", "DACR1"};
+		{"Off", "DACL1", "DACL2", "DACR1"};
 
-static const struct soc_enum twl4030_earpiece_enum =
-	SOC_ENUM_SINGLE(TWL4030_REG_EAR_CTL, 1,
+static const unsigned int twl4030_earpiece_values[] =
+		{0x0, 0x1, 0x2, 0x4};
+
+static const struct soc_value_enum twl4030_earpiece_enum =
+	SOC_VALUE_ENUM_SINGLE(TWL4030_REG_EAR_CTL, 1, 0x7,
 			ARRAY_SIZE(twl4030_earpiece_texts),
-			twl4030_earpiece_texts);
+			twl4030_earpiece_texts,
+			twl4030_earpiece_values);
 
 static const struct snd_kcontrol_new twl4030_dapm_earpiece_control =
-SOC_DAPM_ENUM("Route", twl4030_earpiece_enum);
+SOC_DAPM_VALUE_ENUM("Route", twl4030_earpiece_enum);
 
 /* PreDrive Left */
 static const char *twl4030_predrivel_texts[] =
-		{"Off", "DACL1", "DACL2", "Invalid", "DACR2"};
+		{"Off", "DACL1", "DACL2", "DACR2"};
+
+static const unsigned int twl4030_predrivel_values[] =
+		{0x0, 0x1, 0x2, 0x4};
 
-static const struct soc_enum twl4030_predrivel_enum =
-	SOC_ENUM_SINGLE(TWL4030_REG_PREDL_CTL, 1,
+static const struct soc_value_enum twl4030_predrivel_enum =
+	SOC_VALUE_ENUM_SINGLE(TWL4030_REG_PREDL_CTL, 1, 0x7,
 			ARRAY_SIZE(twl4030_predrivel_texts),
-			twl4030_predrivel_texts);
+			twl4030_predrivel_texts,
+			twl4030_predrivel_values);
 
 static const struct snd_kcontrol_new twl4030_dapm_predrivel_control =
-SOC_DAPM_ENUM("Route", twl4030_predrivel_enum);
+SOC_DAPM_VALUE_ENUM("Route", twl4030_predrivel_enum);
 
 /* PreDrive Right */
 static const char *twl4030_predriver_texts[] =
-		{"Off", "DACR1", "DACR2", "Invalid", "DACL2"};
+		{"Off", "DACR1", "DACR2", "DACL2"};
 
-static const struct soc_enum twl4030_predriver_enum =
-	SOC_ENUM_SINGLE(TWL4030_REG_PREDR_CTL, 1,
+static const unsigned int twl4030_predriver_values[] =
+		{0x0, 0x1, 0x2, 0x4};
+
+static const struct soc_value_enum twl4030_predriver_enum =
+	SOC_VALUE_ENUM_SINGLE(TWL4030_REG_PREDR_CTL, 1, 0x7,
 			ARRAY_SIZE(twl4030_predriver_texts),
-			twl4030_predriver_texts);
+			twl4030_predriver_texts,
+			twl4030_predriver_values);
 
 static const struct snd_kcontrol_new twl4030_dapm_predriver_control =
-SOC_DAPM_ENUM("Route", twl4030_predriver_enum);
+SOC_DAPM_VALUE_ENUM("Route", twl4030_predriver_enum);
 
 /* Headset Left */
 static const char *twl4030_hsol_texts[] =
@@ -300,28 +312,35 @@ SOC_DAPM_ENUM("Route", twl4030_handsfreer_enum);
 
 /* Left analog microphone selection */
 static const char *twl4030_analoglmic_texts[] =
-		{"Off", "Main mic", "Headset mic", "Invalid", "AUXL",
-		 "Invalid", "Invalid", "Invalid", "Carkit mic"};
+		{"Off", "Main mic", "Headset mic", "AUXL", "Carkit mic"};
+
+static const unsigned int twl4030_analoglmic_values[] =
+		{0x0, 0x1, 0x2, 0x4, 0x8};
 
-static const struct soc_enum twl4030_analoglmic_enum =
-	SOC_ENUM_SINGLE(TWL4030_REG_ANAMICL, 0,
+static const struct soc_value_enum twl4030_analoglmic_enum =
+	SOC_VALUE_ENUM_SINGLE(TWL4030_REG_ANAMICL, 0, 0xf,
 			ARRAY_SIZE(twl4030_analoglmic_texts),
-			twl4030_analoglmic_texts);
+			twl4030_analoglmic_texts,
+			twl4030_analoglmic_values);
 
 static const struct snd_kcontrol_new twl4030_dapm_analoglmic_control =
-SOC_DAPM_ENUM("Route", twl4030_analoglmic_enum);
+SOC_DAPM_VALUE_ENUM("Route", twl4030_analoglmic_enum);
 
 /* Right analog microphone selection */
 static const char *twl4030_analogrmic_texts[] =
-		{"Off", "Sub mic", "Invalid", "Invalid", "AUXR"};
+		{"Off", "Sub mic", "AUXR"};
 
-static const struct soc_enum twl4030_analogrmic_enum =
-	SOC_ENUM_SINGLE(TWL4030_REG_ANAMICR, 0,
+static const unsigned int twl4030_analogrmic_values[] =
+		{0x0, 0x1, 0x4};
+
+static const struct soc_value_enum twl4030_analogrmic_enum =
+	SOC_VALUE_ENUM_SINGLE(TWL4030_REG_ANAMICR, 0, 0x5,
 			ARRAY_SIZE(twl4030_analogrmic_texts),
-			twl4030_analogrmic_texts);
+			twl4030_analogrmic_texts,
+			twl4030_analogrmic_values);
 
 static const struct snd_kcontrol_new twl4030_dapm_analogrmic_control =
-SOC_DAPM_ENUM("Route", twl4030_analogrmic_enum);
+SOC_DAPM_VALUE_ENUM("Route", twl4030_analogrmic_enum);
 
 /* TX1 L/R Analog/Digital microphone selection */
 static const char *twl4030_micpathtx1_texts[] =
@@ -347,28 +366,6 @@ static const struct soc_enum twl4030_micpathtx2_enum =
 static const struct snd_kcontrol_new twl4030_dapm_micpathtx2_control =
 SOC_DAPM_ENUM("Route", twl4030_micpathtx2_enum);
 
-/*
- * This function filters out the non valid mux settings, named as "Invalid"
- * in the enum texts.
- * Just refuse to set an invalid mux mode.
- */
-static int twl4030_enum_event(struct snd_soc_dapm_widget *w,
-	struct snd_kcontrol *kcontrol, int event)
-{
-	struct soc_enum *e = (struct soc_enum *)kcontrol->private_value;
-	int ret = 0;
-	int val;
-
-	val = w->value >> e->shift_l;
-	if (!strcmp("Invalid", e->texts[val])) {
-		printk(KERN_WARNING "Invalid MUX setting on 0x%02x (%d)\n",
-			e->reg, val);
-		ret = -1;
-	}
-
-	return ret;
-}
-
 static int micpath_event(struct snd_soc_dapm_widget *w,
 	struct snd_kcontrol *kcontrol, int event)
 {
@@ -737,16 +734,13 @@ static const struct snd_soc_dapm_widget twl4030_dapm_widgets[] = {
 
 	/* Output MUX controls */
 	/* Earpiece */
-	SND_SOC_DAPM_MUX_E("Earpiece Mux", SND_SOC_NOPM, 0, 0,
-		&twl4030_dapm_earpiece_control, twl4030_enum_event,
-		SND_SOC_DAPM_PRE_REG),
+	SND_SOC_DAPM_VALUE_MUX("Earpiece Mux", SND_SOC_NOPM, 0, 0,
+		&twl4030_dapm_earpiece_control),
 	/* PreDrivL/R */
-	SND_SOC_DAPM_MUX_E("PredriveL Mux", SND_SOC_NOPM, 0, 0,
-		&twl4030_dapm_predrivel_control, twl4030_enum_event,
-		SND_SOC_DAPM_PRE_REG),
-	SND_SOC_DAPM_MUX_E("PredriveR Mux", SND_SOC_NOPM, 0, 0,
-		&twl4030_dapm_predriver_control, twl4030_enum_event,
-		SND_SOC_DAPM_PRE_REG),
+	SND_SOC_DAPM_VALUE_MUX("PredriveL Mux", SND_SOC_NOPM, 0, 0,
+		&twl4030_dapm_predrivel_control),
+	SND_SOC_DAPM_VALUE_MUX("PredriveR Mux", SND_SOC_NOPM, 0, 0,
+		&twl4030_dapm_predriver_control),
 	/* HeadsetL/R */
 	SND_SOC_DAPM_MUX("HeadsetL Mux", SND_SOC_NOPM, 0, 0,
 		&twl4030_dapm_hsol_control),
@@ -789,12 +783,10 @@ static const struct snd_soc_dapm_widget twl4030_dapm_widgets[] = {
 		SND_SOC_DAPM_POST_REG),
 
 	/* Analog input muxes with power switch for the physical ADCL/R */
-	SND_SOC_DAPM_MUX_E("Analog Left Capture Route",
-		TWL4030_REG_AVADC_CTL, 3, 0, &twl4030_dapm_analoglmic_control,
-		twl4030_enum_event, SND_SOC_DAPM_PRE_REG),
-	SND_SOC_DAPM_MUX_E("Analog Right Capture Route",
-		TWL4030_REG_AVADC_CTL, 1, 0, &twl4030_dapm_analogrmic_control,
-		twl4030_enum_event, SND_SOC_DAPM_PRE_REG),
+	SND_SOC_DAPM_VALUE_MUX("Analog Left Capture Route",
+		TWL4030_REG_AVADC_CTL, 3, 0, &twl4030_dapm_analoglmic_control),
+	SND_SOC_DAPM_VALUE_MUX("Analog Right Capture Route",
+		TWL4030_REG_AVADC_CTL, 1, 0, &twl4030_dapm_analogrmic_control),
 
 	SND_SOC_DAPM_PGA("Analog Left Amplifier",
 		TWL4030_REG_ANAMICL, 4, 0, NULL, 0),
diff --git a/sound/soc/davinci/davinci-evm.c b/sound/soc/davinci/davinci-evm.c
index 01b948bb55a1..54851f318568 100644
--- a/sound/soc/davinci/davinci-evm.c
+++ b/sound/soc/davinci/davinci-evm.c
@@ -26,7 +26,6 @@
 #include "davinci-pcm.h"
 #include "davinci-i2s.h"
 
-#define EVM_CODEC_CLOCK 22579200
 
 #define AUDIO_FORMAT (SND_SOC_DAIFMT_DSP_B | \
 		SND_SOC_DAIFMT_CBM_CFM | SND_SOC_DAIFMT_IB_NF)
@@ -37,6 +36,21 @@ static int evm_hw_params(struct snd_pcm_substream *substream,
 	struct snd_soc_dai *codec_dai = rtd->dai->codec_dai;
 	struct snd_soc_dai *cpu_dai = rtd->dai->cpu_dai;
 	int ret = 0;
+	unsigned sysclk;
+
+	/* ASP1 on DM355 EVM is clocked by an external oscillator */
+	if (machine_is_davinci_dm355_evm())
+		sysclk = 27000000;
+
+	/* ASP0 in DM6446 EVM is clocked by U55, as configured by
+	 * board-dm644x-evm.c using GPIOs from U18.  There are six
+	 * options; here we "know" we use a 48 KHz sample rate.
+	 */
+	else if (machine_is_davinci_evm())
+		sysclk = 12288000;
+
+	else
+		return -EINVAL;
 
 	/* set codec DAI configuration */
 	ret = snd_soc_dai_set_fmt(codec_dai, AUDIO_FORMAT);
@@ -49,8 +63,7 @@ static int evm_hw_params(struct snd_pcm_substream *substream,
 		return ret;
 
 	/* set the codec system clock */
-	ret = snd_soc_dai_set_sysclk(codec_dai, 0, EVM_CODEC_CLOCK,
-					    SND_SOC_CLOCK_OUT);
+	ret = snd_soc_dai_set_sysclk(codec_dai, 0, sysclk, SND_SOC_CLOCK_OUT);
 	if (ret < 0)
 		return ret;
 
diff --git a/sound/soc/omap/Kconfig b/sound/soc/omap/Kconfig
index a7b1d77b2105..4f7f04014585 100644
--- a/sound/soc/omap/Kconfig
+++ b/sound/soc/omap/Kconfig
@@ -10,6 +10,7 @@ config SND_OMAP_SOC_N810
 	tristate "SoC Audio support for Nokia N810"
 	depends on SND_OMAP_SOC && MACH_NOKIA_N810
 	select SND_OMAP_SOC_MCBSP
+	select OMAP_MUX
 	select SND_SOC_TLV320AIC3X
 	help
 	  Say Y if you want to add support for SoC audio on Nokia N810.
diff --git a/sound/soc/omap/omap3pandora.c b/sound/soc/omap/omap3pandora.c
index bd91594496b1..fcc2f5d9a878 100644
--- a/sound/soc/omap/omap3pandora.c
+++ b/sound/soc/omap/omap3pandora.c
@@ -180,6 +180,19 @@ static int omap3pandora_in_init(struct snd_soc_codec *codec)
 {
 	int ret;
 
+	/* All TWL4030 output pins are floating */
+	snd_soc_dapm_nc_pin(codec, "OUTL"),
+	snd_soc_dapm_nc_pin(codec, "OUTR"),
+	snd_soc_dapm_nc_pin(codec, "EARPIECE"),
+	snd_soc_dapm_nc_pin(codec, "PREDRIVEL"),
+	snd_soc_dapm_nc_pin(codec, "PREDRIVER"),
+	snd_soc_dapm_nc_pin(codec, "HSOL"),
+	snd_soc_dapm_nc_pin(codec, "HSOR"),
+	snd_soc_dapm_nc_pin(codec, "CARKITL"),
+	snd_soc_dapm_nc_pin(codec, "CARKITR"),
+	snd_soc_dapm_nc_pin(codec, "HFL"),
+	snd_soc_dapm_nc_pin(codec, "HFR"),
+
 	ret = snd_soc_dapm_new_controls(codec, omap3pandora_in_dapm_widgets,
 				ARRAY_SIZE(omap3pandora_in_dapm_widgets));
 	if (ret < 0)
diff --git a/sound/soc/soc-core.c b/sound/soc/soc-core.c
index f73c1341437c..6cbe7e82f238 100644
--- a/sound/soc/soc-core.c
+++ b/sound/soc/soc-core.c
@@ -1585,6 +1585,113 @@ int snd_soc_put_enum_double(struct snd_kcontrol *kcontrol,
 EXPORT_SYMBOL_GPL(snd_soc_put_enum_double);
 
 /**
+ * snd_soc_info_value_enum_double - semi enumerated double mixer info callback
+ * @kcontrol: mixer control
+ * @uinfo: control element information
+ *
+ * Callback to provide information about a double semi enumerated
+ * mixer control.
+ *
+ * Semi enumerated mixer: the enumerated items are referred as values. Can be
+ * used for handling bitfield coded enumeration for example.
+ *
+ * Returns 0 for success.
+ */
+int snd_soc_info_value_enum_double(struct snd_kcontrol *kcontrol,
+	struct snd_ctl_elem_info *uinfo)
+{
+	struct soc_value_enum *e = (struct soc_value_enum *)
+			kcontrol->private_value;
+
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->count = e->shift_l == e->shift_r ? 1 : 2;
+	uinfo->value.enumerated.items = e->max;
+
+	if (uinfo->value.enumerated.item > e->max - 1)
+		uinfo->value.enumerated.item = e->max - 1;
+	strcpy(uinfo->value.enumerated.name,
+		e->texts[uinfo->value.enumerated.item]);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(snd_soc_info_value_enum_double);
+
+/**
+ * snd_soc_get_value_enum_double - semi enumerated double mixer get callback
+ * @kcontrol: mixer control
+ * @ucontrol: control element information
+ *
+ * Callback to get the value of a double semi enumerated mixer.
+ *
+ * Semi enumerated mixer: the enumerated items are referred as values. Can be
+ * used for handling bitfield coded enumeration for example.
+ *
+ * Returns 0 for success.
+ */
+int snd_soc_get_value_enum_double(struct snd_kcontrol *kcontrol,
+	struct snd_ctl_elem_value *ucontrol)
+{
+	struct snd_soc_codec *codec = snd_kcontrol_chip(kcontrol);
+	struct soc_value_enum *e = (struct soc_value_enum *)
+			kcontrol->private_value;
+	unsigned short reg_val, val, mux;
+
+	reg_val = snd_soc_read(codec, e->reg);
+	val = (reg_val >> e->shift_l) & e->mask;
+	for (mux = 0; mux < e->max; mux++) {
+		if (val == e->values[mux])
+			break;
+	}
+	ucontrol->value.enumerated.item[0] = mux;
+	if (e->shift_l != e->shift_r) {
+		val = (reg_val >> e->shift_r) & e->mask;
+		for (mux = 0; mux < e->max; mux++) {
+			if (val == e->values[mux])
+				break;
+		}
+		ucontrol->value.enumerated.item[1] = mux;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(snd_soc_get_value_enum_double);
+
+/**
+ * snd_soc_put_value_enum_double - semi enumerated double mixer put callback
+ * @kcontrol: mixer control
+ * @ucontrol: control element information
+ *
+ * Callback to set the value of a double semi enumerated mixer.
+ *
+ * Semi enumerated mixer: the enumerated items are referred as values. Can be
+ * used for handling bitfield coded enumeration for example.
+ *
+ * Returns 0 for success.
+ */
+int snd_soc_put_value_enum_double(struct snd_kcontrol *kcontrol,
+	struct snd_ctl_elem_value *ucontrol)
+{
+	struct snd_soc_codec *codec = snd_kcontrol_chip(kcontrol);
+	struct soc_value_enum *e = (struct soc_value_enum *)
+			kcontrol->private_value;
+	unsigned short val;
+	unsigned short mask;
+
+	if (ucontrol->value.enumerated.item[0] > e->max - 1)
+		return -EINVAL;
+	val = e->values[ucontrol->value.enumerated.item[0]] << e->shift_l;
+	mask = e->mask << e->shift_l;
+	if (e->shift_l != e->shift_r) {
+		if (ucontrol->value.enumerated.item[1] > e->max - 1)
+			return -EINVAL;
+		val |= e->values[ucontrol->value.enumerated.item[1]] << e->shift_r;
+		mask |= e->mask << e->shift_r;
+	}
+
+	return snd_soc_update_bits(codec, e->reg, mask, val);
+}
+EXPORT_SYMBOL_GPL(snd_soc_put_value_enum_double);
+
+/**
  * snd_soc_info_enum_ext - external enumerated single mixer info callback
  * @kcontrol: mixer control
  * @uinfo: control element information
diff --git a/sound/soc/soc-dapm.c b/sound/soc/soc-dapm.c
index 6c79ca6df0bf..ad0d801677c1 100644
--- a/sound/soc/soc-dapm.c
+++ b/sound/soc/soc-dapm.c
@@ -53,13 +53,15 @@
 /* dapm power sequences - make this per codec in the future */
 static int dapm_up_seq[] = {
 	snd_soc_dapm_pre, snd_soc_dapm_micbias, snd_soc_dapm_mic,
-	snd_soc_dapm_mux, snd_soc_dapm_dac, snd_soc_dapm_mixer, snd_soc_dapm_pga,
-	snd_soc_dapm_adc, snd_soc_dapm_hp, snd_soc_dapm_spk, snd_soc_dapm_post
+	snd_soc_dapm_mux, snd_soc_dapm_value_mux, snd_soc_dapm_dac,
+	snd_soc_dapm_mixer, snd_soc_dapm_pga, snd_soc_dapm_adc, snd_soc_dapm_hp,
+	snd_soc_dapm_spk, snd_soc_dapm_post
 };
 static int dapm_down_seq[] = {
 	snd_soc_dapm_pre, snd_soc_dapm_adc, snd_soc_dapm_hp, snd_soc_dapm_spk,
 	snd_soc_dapm_pga, snd_soc_dapm_mixer, snd_soc_dapm_dac, snd_soc_dapm_mic,
-	snd_soc_dapm_micbias, snd_soc_dapm_mux, snd_soc_dapm_post
+	snd_soc_dapm_micbias, snd_soc_dapm_mux, snd_soc_dapm_value_mux,
+	snd_soc_dapm_post
 };
 
 static int dapm_status = 1;
@@ -134,6 +136,25 @@ static void dapm_set_path_status(struct snd_soc_dapm_widget *w,
 		}
 	}
 	break;
+	case snd_soc_dapm_value_mux: {
+		struct soc_value_enum *e = (struct soc_value_enum *)
+			w->kcontrols[i].private_value;
+		int val, item;
+
+		val = snd_soc_read(w->codec, e->reg);
+		val = (val >> e->shift_l) & e->mask;
+		for (item = 0; item < e->max; item++) {
+			if (val == e->values[item])
+				break;
+		}
+
+		p->connect = 0;
+		for (i = 0; i < e->max; i++) {
+			if (!(strcmp(p->name, e->texts[i])) && item == i)
+				p->connect = 1;
+		}
+	}
+	break;
 	/* does not effect routing - always connected */
 	case snd_soc_dapm_pga:
 	case snd_soc_dapm_output:
@@ -179,6 +200,30 @@ static int dapm_connect_mux(struct snd_soc_codec *codec,
 	return -ENODEV;
 }
 
+/* connect value_mux widget to it's interconnecting audio paths */
+static int dapm_connect_value_mux(struct snd_soc_codec *codec,
+	struct snd_soc_dapm_widget *src, struct snd_soc_dapm_widget *dest,
+	struct snd_soc_dapm_path *path, const char *control_name,
+	const struct snd_kcontrol_new *kcontrol)
+{
+	struct soc_value_enum *e = (struct soc_value_enum *)
+			kcontrol->private_value;
+	int i;
+
+	for (i = 0; i < e->max; i++) {
+		if (!(strcmp(control_name, e->texts[i]))) {
+			list_add(&path->list, &codec->dapm_paths);
+			list_add(&path->list_sink, &dest->sources);
+			list_add(&path->list_source, &src->sinks);
+			path->name = (char *)e->texts[i];
+			dapm_set_path_status(dest, path, 0);
+			return 0;
+		}
+	}
+
+	return -ENODEV;
+}
+
 /* connect mixer widget to it's interconnecting audio paths */
 static int dapm_connect_mixer(struct snd_soc_codec *codec,
 	struct snd_soc_dapm_widget *src, struct snd_soc_dapm_widget *dest,
@@ -653,6 +698,7 @@ static void dbg_dump_dapm(struct snd_soc_codec* codec, const char *action)
 		case snd_soc_dapm_vmid:
 			continue;
 		case snd_soc_dapm_mux:
+		case snd_soc_dapm_value_mux:
 		case snd_soc_dapm_output:
 		case snd_soc_dapm_input:
 		case snd_soc_dapm_switch:
@@ -728,6 +774,45 @@ static int dapm_mux_update_power(struct snd_soc_dapm_widget *widget,
 	return 0;
 }
 
+/* test and update the power status of a value_mux widget */
+static int dapm_value_mux_update_power(struct snd_soc_dapm_widget *widget,
+				 struct snd_kcontrol *kcontrol, int mask,
+				 int mux, int val, struct soc_value_enum *e)
+{
+	struct snd_soc_dapm_path *path;
+	int found = 0;
+
+	if (widget->id != snd_soc_dapm_value_mux)
+		return -ENODEV;
+
+	if (!snd_soc_test_bits(widget->codec, e->reg, mask, val))
+		return 0;
+
+	/* find dapm widget path assoc with kcontrol */
+	list_for_each_entry(path, &widget->codec->dapm_paths, list) {
+		if (path->kcontrol != kcontrol)
+			continue;
+
+		if (!path->name || !e->texts[mux])
+			continue;
+
+		found = 1;
+		/* we now need to match the string in the enum to the path */
+		if (!(strcmp(path->name, e->texts[mux])))
+			path->connect = 1; /* new connection */
+		else
+			path->connect = 0; /* old connection must be
+					      powered down */
+	}
+
+	if (found) {
+		dapm_power_widgets(widget->codec, SND_SOC_DAPM_STREAM_NOP);
+		dump_dapm(widget->codec, "mux power update");
+	}
+
+	return 0;
+}
+
 /* test and update the power status of a mixer or switch widget */
 static int dapm_mixer_update_power(struct snd_soc_dapm_widget *widget,
 				   struct snd_kcontrol *kcontrol, int reg,
@@ -965,6 +1050,12 @@ static int snd_soc_dapm_add_route(struct snd_soc_codec *codec,
 		if (ret != 0)
 			goto err;
 		break;
+	case snd_soc_dapm_value_mux:
+		ret = dapm_connect_value_mux(codec, wsource, wsink, path,
+			control, &wsink->kcontrols[0]);
+		if (ret != 0)
+			goto err;
+		break;
 	case snd_soc_dapm_switch:
 	case snd_soc_dapm_mixer:
 		ret = dapm_connect_mixer(codec, wsource, wsink, path, control);
@@ -1047,6 +1138,7 @@ int snd_soc_dapm_new_widgets(struct snd_soc_codec *codec)
 			dapm_new_mixer(codec, w);
 			break;
 		case snd_soc_dapm_mux:
+		case snd_soc_dapm_value_mux:
 			dapm_new_mux(codec, w);
 			break;
 		case snd_soc_dapm_adc:
@@ -1274,6 +1366,105 @@ out:
 EXPORT_SYMBOL_GPL(snd_soc_dapm_put_enum_double);
 
 /**
+ * snd_soc_dapm_get_value_enum_double - dapm semi enumerated double mixer get
+ *					callback
+ * @kcontrol: mixer control
+ * @ucontrol: control element information
+ *
+ * Callback to get the value of a dapm semi enumerated double mixer control.
+ *
+ * Semi enumerated mixer: the enumerated items are referred as values. Can be
+ * used for handling bitfield coded enumeration for example.
+ *
+ * Returns 0 for success.
+ */
+int snd_soc_dapm_get_value_enum_double(struct snd_kcontrol *kcontrol,
+	struct snd_ctl_elem_value *ucontrol)
+{
+	struct snd_soc_dapm_widget *widget = snd_kcontrol_chip(kcontrol);
+	struct soc_value_enum *e = (struct soc_value_enum *)
+			kcontrol->private_value;
+	unsigned short reg_val, val, mux;
+
+	reg_val = snd_soc_read(widget->codec, e->reg);
+	val = (reg_val >> e->shift_l) & e->mask;
+	for (mux = 0; mux < e->max; mux++) {
+		if (val == e->values[mux])
+			break;
+	}
+	ucontrol->value.enumerated.item[0] = mux;
+	if (e->shift_l != e->shift_r) {
+		val = (reg_val >> e->shift_r) & e->mask;
+		for (mux = 0; mux < e->max; mux++) {
+			if (val == e->values[mux])
+				break;
+		}
+		ucontrol->value.enumerated.item[1] = mux;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(snd_soc_dapm_get_value_enum_double);
+
+/**
+ * snd_soc_dapm_put_value_enum_double - dapm semi enumerated double mixer set
+ *					callback
+ * @kcontrol: mixer control
+ * @ucontrol: control element information
+ *
+ * Callback to set the value of a dapm semi enumerated double mixer control.
+ *
+ * Semi enumerated mixer: the enumerated items are referred as values. Can be
+ * used for handling bitfield coded enumeration for example.
+ *
+ * Returns 0 for success.
+ */
+int snd_soc_dapm_put_value_enum_double(struct snd_kcontrol *kcontrol,
+	struct snd_ctl_elem_value *ucontrol)
+{
+	struct snd_soc_dapm_widget *widget = snd_kcontrol_chip(kcontrol);
+	struct soc_value_enum *e = (struct soc_value_enum *)
+			kcontrol->private_value;
+	unsigned short val, mux;
+	unsigned short mask;
+	int ret = 0;
+
+	if (ucontrol->value.enumerated.item[0] > e->max - 1)
+		return -EINVAL;
+	mux = ucontrol->value.enumerated.item[0];
+	val = e->values[ucontrol->value.enumerated.item[0]] << e->shift_l;
+	mask = e->mask << e->shift_l;
+	if (e->shift_l != e->shift_r) {
+		if (ucontrol->value.enumerated.item[1] > e->max - 1)
+			return -EINVAL;
+		val |= e->values[ucontrol->value.enumerated.item[1]] << e->shift_r;
+		mask |= e->mask << e->shift_r;
+	}
+
+	mutex_lock(&widget->codec->mutex);
+	widget->value = val;
+	dapm_value_mux_update_power(widget, kcontrol, mask, mux, val, e);
+	if (widget->event) {
+		if (widget->event_flags & SND_SOC_DAPM_PRE_REG) {
+			ret = widget->event(widget,
+				kcontrol, SND_SOC_DAPM_PRE_REG);
+			if (ret < 0)
+				goto out;
+		}
+		ret = snd_soc_update_bits(widget->codec, e->reg, mask, val);
+		if (widget->event_flags & SND_SOC_DAPM_POST_REG)
+			ret = widget->event(widget,
+				kcontrol, SND_SOC_DAPM_POST_REG);
+	} else
+		ret = snd_soc_update_bits(widget->codec, e->reg, mask, val);
+
+out:
+	mutex_unlock(&widget->codec->mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(snd_soc_dapm_put_value_enum_double);
+
+/**
  * snd_soc_dapm_new_control - create new dapm control
  * @codec: audio codec
  * @widget: widget template
diff --git a/sound/usb/usx2y/usbusx2y.c b/sound/usb/usx2y/usbusx2y.c
index ca26c532e77e..11639bd72a51 100644
--- a/sound/usb/usx2y/usbusx2y.c
+++ b/sound/usb/usx2y/usbusx2y.c
@@ -238,7 +238,7 @@ static void i_usX2Y_In04Int(struct urb *urb)
 					send = 0;
 				for (j = 0; j < URBS_AsyncSeq  &&  !err; ++j)
 					if (0 == usX2Y->AS04.urb[j]->status) {
-						struct us428_p4out *p4out = us428ctls->p4out + send;	// FIXME if more then 1 p4out is new, 1 gets lost.
+						struct us428_p4out *p4out = us428ctls->p4out + send;	// FIXME if more than 1 p4out is new, 1 gets lost.
 						usb_fill_bulk_urb(usX2Y->AS04.urb[j], usX2Y->chip.dev,
 								  usb_sndbulkpipe(usX2Y->chip.dev, 0x04), &p4out->val.vol, 
 								  p4out->type == eLT_Light ? sizeof(struct us428_lights) : 5,